From 80caceb98481f9ab9cd42c766227a94332608178 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 18 May 2013 14:17:26 +0200 Subject: [PATCH 01/61] make uncrustify_all;make uncrustify_all; --- flight/libraries/CoordinateConversions.c | 522 +- flight/libraries/WorldMagModel.c | 1850 +- flight/libraries/aes.c | 636 +- flight/libraries/alarms.c | 15 +- flight/libraries/fifo_buffer.c | 232 +- flight/libraries/inc/CoordinateConversions.h | 30 +- flight/libraries/inc/WMMInternal.h | 155 +- flight/libraries/inc/WorldMagModel.h | 2 +- flight/libraries/inc/aes.h | 7 +- flight/libraries/inc/alarms.h | 6 +- flight/libraries/inc/fifo_buffer.h | 7 +- flight/libraries/inc/insgps.h | 42 +- flight/libraries/inc/op_dfu.h | 26 +- flight/libraries/inc/packet_handler.h | 76 +- flight/libraries/inc/paths.h | 12 +- flight/libraries/inc/sanitycheck.h | 6 +- flight/libraries/inc/stopwatch.h | 6 +- flight/libraries/insgps13state.c | 1137 +- flight/libraries/insgps16state.c | 1290 +- flight/libraries/insgps_helper.c | 522 +- flight/libraries/math/pid.c | 109 +- flight/libraries/math/pid.h | 20 +- flight/libraries/math/sin_lookup.c | 103 +- flight/libraries/math/sin_lookup.h | 2 +- flight/libraries/op_dfu.c | 790 +- flight/libraries/paths.c | 98 +- flight/libraries/printf-stdarg.c | 312 +- flight/libraries/printf2.c | 769 +- flight/libraries/sanitycheck.c | 229 +- flight/libraries/stopwatch.c | 137 +- flight/modules/Actuator/actuator.c | 973 +- flight/modules/Actuator/inc/actuator.h | 10 +- flight/modules/Airspeed/revolution/airspeed.c | 164 +- .../revolution/baro_airspeed_etasv3.c | 79 +- .../Airspeed/revolution/baro_airspeed_mpxv.c | 105 +- .../Airspeed/revolution/gps_airspeed.c | 148 +- .../Airspeed/revolution/inc/airspeed.h | 4 +- .../revolution/inc/baro_airspeed_etasv3.h | 4 +- .../revolution/inc/baro_airspeed_mpxv.h | 4 +- .../Airspeed/revolution/inc/gps_airspeed.h | 4 +- flight/modules/Altitude/altitude.c | 203 +- flight/modules/Altitude/inc/altitude.h | 4 +- flight/modules/Altitude/revolution/altitude.c | 158 +- .../Altitude/revolution/inc/altitude.h | 4 +- flight/modules/AltitudeHold/altitudehold.c | 500 +- flight/modules/Attitude/attitude.c | 913 +- flight/modules/Attitude/inc/attitude.h | 4 +- flight/modules/Attitude/revolution/attitude.c | 1574 +- .../Attitude/revolution/inc/attitude.h | 4 +- flight/modules/Autotune/autotune.c | 348 +- flight/modules/Autotune/inc/autotune.h | 4 +- flight/modules/Battery/battery.c | 231 +- flight/modules/Battery/inc/battery.h | 10 +- flight/modules/CallbackTest/callbacktest.c | 136 +- flight/modules/CameraStab/camerastab.c | 406 +- flight/modules/CameraStab/inc/camerastab.h | 10 +- flight/modules/ComUsbBridge/ComUsbBridge.c | 188 +- flight/modules/Example/example.c | 6 +- flight/modules/Example/examplemodcallback.c | 87 +- flight/modules/Example/examplemodevent.c | 80 +- flight/modules/Example/examplemodperiodic.c | 84 +- flight/modules/Example/examplemodthread.c | 112 +- .../modules/Example/inc/examplemodperiodic.h | 1 - flight/modules/Example/inc/examplemodthread.h | 1 - .../modules/Extensions/MagBaro/inc/magbaro.h | 4 +- flight/modules/Extensions/MagBaro/magbaro.c | 187 +- flight/modules/Fault/Fault.c | 153 +- flight/modules/FirmwareIAP/firmwareiap.c | 282 +- flight/modules/FirmwareIAP/inc/firmwareiap.h | 1 - .../fixedwingpathfollower.c | 902 +- flight/modules/FlightPlan/flightplan.c | 352 +- flight/modules/GPS/GPS.c | 381 +- flight/modules/GPS/NMEA.c | 967 +- flight/modules/GPS/UBX.c | 474 +- flight/modules/GPS/inc/GPS.h | 32 +- flight/modules/GPS/inc/NMEA.h | 2 +- flight/modules/GPS/inc/UBX.h | 238 +- flight/modules/MK/MKSerial/MKSerial.c | 728 +- flight/modules/MK/MKSerial/inc/MkSerial.h | 16 +- .../modules/ManualControl/inc/manualcontrol.h | 160 +- flight/modules/ManualControl/manualcontrol.c | 740 +- flight/modules/OPLink/inc/oplinkmod.h | 4 +- flight/modules/OPLink/oplinkmod.c | 244 +- flight/modules/Osd/OsdEtStd/OsdEtStd.c | 651 +- flight/modules/Osd/WavPlayer/wavplayer.c | 12 +- flight/modules/Osd/osdgen/inc/osdgen.h | 137 +- flight/modules/Osd/osdgen/osdgen.c | 1181 +- flight/modules/Osd/osdinput/osdinput.c | 51 +- flight/modules/Osd/osdoutput/inc/osdoutput.h | 2 +- flight/modules/Osd/osdoutput/osdoutput.c | 212 +- flight/modules/OveroSync/inc/overosync.h | 14 +- flight/modules/OveroSync/overosync.c | 191 +- .../OveroSync/simulated/inc/overosync.h | 14 +- .../modules/OveroSync/simulated/overosync.c | 225 +- flight/modules/PathPlanner/pathplanner.c | 606 +- .../modules/RadioComBridge/RadioComBridge.c | 190 +- .../RadioComBridge/inc/radiocombridge.h | 10 +- flight/modules/Sensors/inc/sensors.h | 4 +- flight/modules/Sensors/sensors.c | 771 +- .../modules/Sensors/simulated/inc/sensors.h | 4 +- flight/modules/Sensors/simulated/sensors.c | 1495 +- .../modules/Stabilization/inc/stabilization.h | 14 +- .../modules/Stabilization/inc/virtualflybar.h | 2 +- flight/modules/Stabilization/relay_tuning.c | 152 +- flight/modules/Stabilization/stabilization.c | 661 +- flight/modules/Stabilization/virtualflybar.c | 98 +- flight/modules/System/inc/systemmod.h | 4 +- flight/modules/System/systemmod.c | 191 +- flight/modules/Telemetry/inc/telemetry.h | 14 +- flight/modules/Telemetry/telemetry.c | 689 +- flight/modules/TxPID/inc/txpid.h | 10 +- flight/modules/TxPID/txpid.c | 412 +- .../VtolPathFollower/vtolpathfollower.c | 454 +- flight/pios/common/pios_adxl345.c | 356 +- flight/pios/common/pios_bma180.c | 589 +- flight/pios/common/pios_bmp085.c | 446 +- flight/pios/common/pios_board_info.c | 22 +- flight/pios/common/pios_com.c | 678 +- flight/pios/common/pios_com_msg.c | 208 +- flight/pios/common/pios_crc.c | 221 +- flight/pios/common/pios_etasv3.c | 84 +- flight/pios/common/pios_flash_jedec.c | 603 +- flight/pios/common/pios_flashfs_logfs.c | 1314 +- flight/pios/common/pios_gcsrcvr.c | 157 +- flight/pios/common/pios_hcsr04.c | 395 +- flight/pios/common/pios_hmc5843.c | 572 +- flight/pios/common/pios_hmc5883.c | 472 +- flight/pios/common/pios_i2c_esc.c | 252 +- flight/pios/common/pios_iap.c | 166 +- flight/pios/common/pios_l3gd20.c | 450 +- flight/pios/common/pios_mpu6000.c | 774 +- flight/pios/common/pios_mpxv.c | 64 +- flight/pios/common/pios_ms5611.c | 323 +- flight/pios/common/pios_rcvr.c | 110 +- flight/pios/common/pios_rfm22b.c | 897 +- flight/pios/common/pios_rfm22b_com.c | 85 +- flight/pios/common/pios_rfm22b_rcvr.c | 36 +- flight/pios/common/pios_sbus.c | 363 +- flight/pios/common/pios_sdcard.c | 1504 +- flight/pios/common/pios_task_monitor.c | 127 +- flight/pios/common/pios_usb_desc_hid_cdc.c | 514 +- flight/pios/common/pios_usb_desc_hid_only.c | 222 +- flight/pios/common/pios_usb_util.c | 14 +- flight/pios/common/pios_video.c | 239 +- flight/pios/common/pios_wavplay.c | 830 +- flight/pios/inc/pios_adc.h | 36 +- flight/pios/inc/pios_adc_priv.h | 13 +- flight/pios/inc/pios_adxl345.h | 60 +- flight/pios/inc/pios_bkp.h | 129 +- flight/pios/inc/pios_bl_helper.h | 2 +- flight/pios/inc/pios_bma180.h | 93 +- flight/pios/inc/pios_bmp085.h | 78 +- flight/pios/inc/pios_board_info.h | 24 +- flight/pios/inc/pios_com.h | 52 +- flight/pios/inc/pios_com_msg.h | 34 +- flight/pios/inc/pios_com_msg_priv.h | 12 +- flight/pios/inc/pios_com_priv.h | 8 +- flight/pios/inc/pios_constants.h | 224 +- flight/pios/inc/pios_crc.h | 30 +- flight/pios/inc/pios_debug.h | 46 +- flight/pios/inc/pios_delay.h | 30 +- flight/pios/inc/pios_dsm.h | 22 +- flight/pios/inc/pios_dsm_priv.h | 40 +- flight/pios/inc/pios_eeprom.h | 34 +- flight/pios/inc/pios_etasv3.h | 52 +- flight/pios/inc/pios_exti.h | 42 +- flight/pios/inc/pios_flash.h | 20 +- flight/pios/inc/pios_flash_internal_priv.h | 34 +- flight/pios/inc/pios_flash_jedec_catalog.h | 58 +- flight/pios/inc/pios_flash_jedec_priv.h | 16 +- flight/pios/inc/pios_flashfs.h | 6 +- flight/pios/inc/pios_flashfs_logfs_priv.h | 20 +- flight/pios/inc/pios_gpio.h | 28 +- flight/pios/inc/pios_hcsr04.h | 30 +- flight/pios/inc/pios_hcsr04_priv.h | 10 +- flight/pios/inc/pios_hmc5843.h | 30 +- flight/pios/inc/pios_hmc5883.h | 131 +- flight/pios/inc/pios_i2c.h | 62 +- flight/pios/inc/pios_i2c_esc.h | 24 +- flight/pios/inc/pios_i2c_priv.h | 104 +- flight/pios/inc/pios_iap.h | 83 +- flight/pios/inc/pios_initcall.h | 84 +- flight/pios/inc/pios_irq.h | 26 +- flight/pios/inc/pios_l3gd20.h | 66 +- flight/pios/inc/pios_led.h | 24 +- flight/pios/inc/pios_led_priv.h | 18 +- flight/pios/inc/pios_math.h | 10 +- flight/pios/inc/pios_mpu6000.h | 118 +- flight/pios/inc/pios_mpu6000_config.h | 68 +- flight/pios/inc/pios_mpxv.h | 94 +- flight/pios/inc/pios_ms5611.h | 70 +- flight/pios/inc/pios_overo_priv.h | 22 +- flight/pios/inc/pios_posix.h | 36 +- flight/pios/inc/pios_ppm_out_priv.h | 6 +- flight/pios/inc/pios_ppm_priv.h | 8 +- flight/pios/inc/pios_pwm_priv.h | 8 +- flight/pios/inc/pios_rcvr.h | 46 +- flight/pios/inc/pios_rcvr_priv.h | 8 +- flight/pios/inc/pios_rfm22b.h | 128 +- flight/pios/inc/pios_rfm22b_com.h | 28 +- flight/pios/inc/pios_rfm22b_priv.h | 922 +- flight/pios/inc/pios_rtc.h | 24 +- flight/pios/inc/pios_rtc_priv.h | 9 +- flight/pios/inc/pios_sbus.h | 24 +- flight/pios/inc/pios_sbus_priv.h | 36 +- flight/pios/inc/pios_sdcard.h | 132 +- flight/pios/inc/pios_servo.h | 32 +- flight/pios/inc/pios_servo_priv.h | 14 +- flight/pios/inc/pios_spi.h | 16 +- flight/pios/inc/pios_spi_priv.h | 40 +- flight/pios/inc/pios_stm32.h | 58 +- flight/pios/inc/pios_struct_helper.h | 8 +- flight/pios/inc/pios_sys.h | 36 +- flight/pios/inc/pios_task_monitor.h | 20 +- flight/pios/inc/pios_tim.h | 2 +- flight/pios/inc/pios_tim_priv.h | 24 +- flight/pios/inc/pios_udp.h | 26 +- flight/pios/inc/pios_udp_priv.h | 38 +- flight/pios/inc/pios_usart.h | 30 +- flight/pios/inc/pios_usart_priv.h | 22 +- flight/pios/inc/pios_usb.h | 6 +- flight/pios/inc/pios_usb_board_data_priv.h | 28 +- flight/pios/inc/pios_usb_cdc_priv.h | 18 +- flight/pios/inc/pios_usb_defs.h | 404 +- flight/pios/inc/pios_usb_desc_hid_cdc_priv.h | 28 +- flight/pios/inc/pios_usb_desc_hid_only_priv.h | 28 +- flight/pios/inc/pios_usb_hid.h | 10 +- flight/pios/inc/pios_usb_hid_istr.h | 2 +- flight/pios/inc/pios_usb_hid_priv.h | 14 +- flight/pios/inc/pios_usb_hid_pwr.h | 34 +- flight/pios/inc/pios_usb_priv.h | 16 +- flight/pios/inc/pios_usb_rctx_priv.h | 12 +- flight/pios/inc/pios_usb_util.h | 6 +- flight/pios/inc/pios_usbhook.h | 31 +- flight/pios/inc/pios_video.h | 56 +- flight/pios/inc/pios_wavplay.h | 18 +- flight/pios/inc/pios_wdg.h | 2 +- flight/pios/inc/stm32f10x_conf.h | 58 +- flight/pios/inc/stm32f2xx_conf.h | 78 +- flight/pios/inc/stm32f4xx_conf.h | 78 +- flight/pios/inc/usb_conf.h | 159 +- flight/pios/osx/inc/FreeRTOSConfig.h | 150 +- flight/pios/osx/inc/pios_bl_helper.h | 2 +- flight/pios/osx/inc/pios_board_info.h | 24 +- flight/pios/osx/inc/pios_com.h | 52 +- flight/pios/osx/inc/pios_com_priv.h | 8 +- flight/pios/osx/inc/pios_crc.h | 26 +- flight/pios/osx/inc/pios_debug.h | 38 +- flight/pios/osx/inc/pios_delay.h | 30 +- flight/pios/osx/inc/pios_iap.h | 40 +- flight/pios/osx/inc/pios_initcall.h | 50 +- flight/pios/osx/inc/pios_irq.h | 26 +- flight/pios/osx/inc/pios_led.h | 24 +- flight/pios/osx/inc/pios_posix.h | 35 +- flight/pios/osx/inc/pios_rcvr.h | 50 +- flight/pios/osx/inc/pios_rcvr_priv.h | 10 +- flight/pios/osx/inc/pios_sdcard.h | 128 +- flight/pios/osx/inc/pios_servo.h | 32 +- flight/pios/osx/inc/pios_sim.h | 3 +- flight/pios/osx/inc/pios_sim_priv.h | 17 +- flight/pios/osx/inc/pios_struct_helper.h | 8 +- flight/pios/osx/inc/pios_sys.h | 36 +- flight/pios/osx/inc/pios_tcp.h | 26 +- flight/pios/osx/inc/pios_tcp_priv.h | 44 +- flight/pios/osx/inc/pios_udp.h | 26 +- flight/pios/osx/inc/pios_udp_priv.h | 37 +- flight/pios/osx/inc/pios_wdg.h | 12 +- flight/pios/osx/inc/sim_model.h | 3 +- flight/pios/osx/osx/pios_bl_helper.c | 16 +- flight/pios/osx/osx/pios_board_info.c | 22 +- flight/pios/osx/osx/pios_com.c | 663 +- flight/pios/osx/osx/pios_crc.c | 56 +- flight/pios/osx/osx/pios_debug.c | 80 +- flight/pios/osx/osx/pios_delay.c | 128 +- flight/pios/osx/osx/pios_gcsrcvr.c | 102 +- flight/pios/osx/osx/pios_iap.c | 44 +- flight/pios/osx/osx/pios_irq.c | 58 +- flight/pios/osx/osx/pios_led.c | 67 +- flight/pios/osx/osx/pios_rcvr.c | 112 +- flight/pios/osx/osx/pios_sdcard.c | 376 +- flight/pios/osx/osx/pios_servo.c | 67 +- flight/pios/osx/osx/pios_sim.c | 77 +- flight/pios/osx/osx/pios_sys.c | 119 +- flight/pios/osx/osx/pios_tcp.c | 381 +- flight/pios/osx/osx/pios_udp.c | 272 +- flight/pios/osx/osx/pios_wdg.c | 100 +- flight/pios/osx/pios.h | 28 +- flight/pios/pios.h | 24 +- flight/pios/pios_sim_posix.h | 26 +- flight/pios/posix/pios_bl_helper.c | 16 +- flight/pios/posix/pios_board_info.c | 22 +- flight/pios/posix/pios_com.c | 663 +- flight/pios/posix/pios_crc.c | 56 +- flight/pios/posix/pios_debug.c | 78 +- flight/pios/posix/pios_delay.c | 139 +- flight/pios/posix/pios_iap.c | 44 +- flight/pios/posix/pios_irq.c | 58 +- flight/pios/posix/pios_led.c | 97 +- flight/pios/posix/pios_rcvr.c | 128 +- flight/pios/posix/pios_sdcard.c | 376 +- flight/pios/posix/pios_servo.c | 67 +- flight/pios/posix/pios_sys.c | 240 +- flight/pios/posix/pios_udp.c | 269 +- flight/pios/posix/pios_wdg.c | 65 +- flight/pios/stm32f10x/pios_adc.c | 448 +- flight/pios/stm32f10x/pios_bkp.c | 181 +- flight/pios/stm32f10x/pios_bl_helper.c | 50 +- flight/pios/stm32f10x/pios_debug.c | 166 +- flight/pios/stm32f10x/pios_delay.c | 122 +- flight/pios/stm32f10x/pios_dsm.c | 465 +- flight/pios/stm32f10x/pios_eeprom.c | 202 +- flight/pios/stm32f10x/pios_exti.c | 364 +- flight/pios/stm32f10x/pios_gpio.c | 81 +- flight/pios/stm32f10x/pios_i2c.c | 1518 +- flight/pios/stm32f10x/pios_irq.c | 96 +- flight/pios/stm32f10x/pios_led.c | 188 +- flight/pios/stm32f10x/pios_ppm.c | 448 +- flight/pios/stm32f10x/pios_ppm_out.c | 384 +- flight/pios/stm32f10x/pios_pwm.c | 322 +- flight/pios/stm32f10x/pios_rtc.c | 96 +- flight/pios/stm32f10x/pios_servo.c | 174 +- flight/pios/stm32f10x/pios_spi.c | 829 +- flight/pios/stm32f10x/pios_sys.c | 259 +- flight/pios/stm32f10x/pios_tim.c | 645 +- flight/pios/stm32f10x/pios_usart.c | 390 +- flight/pios/stm32f10x/pios_usb.c | 194 +- flight/pios/stm32f10x/pios_usb_cdc.c | 492 +- flight/pios/stm32f10x/pios_usb_hid.c | 384 +- flight/pios/stm32f10x/pios_usb_hid_istr.c | 353 +- flight/pios/stm32f10x/pios_usb_hid_pwr.c | 277 +- flight/pios/stm32f10x/pios_usb_rctx.c | 202 +- flight/pios/stm32f10x/pios_usbhook.c | 454 +- flight/pios/stm32f10x/pios_wdg.c | 150 +- flight/pios/stm32f4xx/inc/pios_i2c_config.h | 104 +- flight/pios/stm32f4xx/inc/pios_usart_config.h | 80 +- flight/pios/stm32f4xx/inc/stm32f4xx_conf.h | 60 +- flight/pios/stm32f4xx/pios_adc.c | 438 +- flight/pios/stm32f4xx/pios_bkp.c | 128 +- flight/pios/stm32f4xx/pios_bl_helper.c | 214 +- flight/pios/stm32f4xx/pios_debug.c | 172 +- flight/pios/stm32f4xx/pios_delay.c | 122 +- flight/pios/stm32f4xx/pios_dsm.c | 461 +- flight/pios/stm32f4xx/pios_exti.c | 364 +- flight/pios/stm32f4xx/pios_flash_internal.c | 438 +- flight/pios/stm32f4xx/pios_gpio.c | 83 +- flight/pios/stm32f4xx/pios_i2c.c | 1658 +- flight/pios/stm32f4xx/pios_irq.c | 94 +- flight/pios/stm32f4xx/pios_led.c | 228 +- flight/pios/stm32f4xx/pios_overo.c | 453 +- flight/pios/stm32f4xx/pios_ppm.c | 434 +- flight/pios/stm32f4xx/pios_pwm.c | 322 +- flight/pios/stm32f4xx/pios_rtc.c | 115 +- flight/pios/stm32f4xx/pios_servo.c | 187 +- flight/pios/stm32f4xx/pios_spi.c | 954 +- flight/pios/stm32f4xx/pios_sys.c | 218 +- flight/pios/stm32f4xx/pios_tim.c | 661 +- flight/pios/stm32f4xx/pios_usart.c | 430 +- flight/pios/stm32f4xx/pios_usb.c | 214 +- flight/pios/stm32f4xx/pios_usb_cdc.c | 460 +- flight/pios/stm32f4xx/pios_usb_hid.c | 625 +- flight/pios/stm32f4xx/pios_usbhook.c | 439 +- flight/pios/stm32f4xx/pios_wdg.c | 142 +- flight/pios/stm32f4xx/startup.c | 115 +- flight/pios/stm32f4xx/vectors_stm32f4xx.c | 339 +- flight/pios/win32/inc/FreeRTOSConfig.h | 102 +- flight/pios/win32/inc/pios_com.h | 38 +- flight/pios/win32/inc/pios_com_priv.h | 9 +- flight/pios/win32/inc/pios_crc.h | 26 +- flight/pios/win32/inc/pios_delay.h | 26 +- flight/pios/win32/inc/pios_initcall.h | 39 +- flight/pios/win32/inc/pios_led.h | 28 +- flight/pios/win32/inc/pios_posix.h | 29 +- flight/pios/win32/inc/pios_sdcard.h | 128 +- flight/pios/win32/inc/pios_servo.h | 32 +- flight/pios/win32/inc/pios_sys.h | 26 +- flight/pios/win32/inc/pios_udp.h | 28 +- flight/pios/win32/inc/pios_udp_priv.h | 29 +- flight/pios/win32/inc/pios_wdg.h | 20 +- flight/pios/win32/pios.h | 26 +- flight/pios/win32/win32/pios_com.c | 348 +- flight/pios/win32/win32/pios_crc.c | 56 +- flight/pios/win32/win32/pios_delay.c | 92 +- flight/pios/win32/win32/pios_led.c | 97 +- flight/pios/win32/win32/pios_sdcard.c | 376 +- flight/pios/win32/win32/pios_servo.c | 67 +- flight/pios/win32/win32/pios_sys.c | 227 +- flight/pios/win32/win32/pios_udp.c | 628 +- flight/pios/win32/win32/pios_wdg.c | 66 +- .../boards/coptercontrol/board_hw_defs.c | 2338 +- .../coptercontrol/bootloader/inc/common.h | 90 +- .../bootloader/inc/pios_config.h | 26 +- .../bootloader/inc/pios_usb_board_data.h | 12 +- .../boards/coptercontrol/bootloader/main.c | 275 +- .../coptercontrol/bootloader/pios_board.c | 92 +- .../coptercontrol/firmware/coptercontrol.c | 128 +- .../firmware/inc/FreeRTOSConfig.h | 132 +- .../coptercontrol/firmware/inc/openpilot.h | 22 +- .../firmware/inc/pios_board_posix.h | 83 +- .../coptercontrol/firmware/inc/pios_config.h | 28 +- .../firmware/inc/pios_config_posix.h | 30 +- .../firmware/inc/pios_usb_board_data.h | 12 +- .../coptercontrol/firmware/pios_board.c | 1402 +- .../coptercontrol/firmware/pios_board_posix.c | 137 +- .../targets/boards/coptercontrol/pios_board.h | 298 +- .../coptercontrol/pios_usb_board_data.c | 105 +- .../targets/boards/oplinkmini/board_hw_defs.c | 946 +- .../boards/oplinkmini/bootloader/inc/common.h | 90 +- .../oplinkmini/bootloader/inc/pios_config.h | 26 +- .../bootloader/inc/pios_usb_board_data.h | 12 +- .../boards/oplinkmini/bootloader/main.c | 277 +- .../boards/oplinkmini/bootloader/pios_board.c | 67 +- .../oplinkmini/firmware/inc/FreeRTOSConfig.h | 132 +- .../oplinkmini/firmware/inc/openpilot.h | 22 +- .../firmware/inc/pios_board_posix.h | 83 +- .../oplinkmini/firmware/inc/pios_config.h | 28 +- .../firmware/inc/pios_config_posix.h | 38 +- .../firmware/inc/pios_usb_board_data.h | 12 +- .../boards/oplinkmini/firmware/oplink.c | 90 +- .../boards/oplinkmini/firmware/pios_board.c | 412 +- .../oplinkmini/firmware/pios_board_posix.c | 137 +- flight/targets/boards/oplinkmini/pios_board.h | 362 +- .../boards/oplinkmini/pios_usb_board_data.c | 97 +- flight/targets/boards/osd/board_hw_defs.c | 1497 +- .../boards/osd/bootloader/inc/common.h | 90 +- .../boards/osd/bootloader/inc/pios_config.h | 26 +- .../osd/bootloader/inc/pios_usb_board_data.h | 12 +- flight/targets/boards/osd/bootloader/main.c | 279 +- .../boards/osd/bootloader/pios_board.c | 75 +- .../boards/osd/firmware/cm3_fault_handlers.c | 109 +- .../targets/boards/osd/firmware/dcc_stdio.c | 176 +- .../boards/osd/firmware/font_outlined8x14.c | 502 +- .../boards/osd/firmware/font_outlined8x8.c | 330 +- flight/targets/boards/osd/firmware/fonts.c | 26 +- .../boards/osd/firmware/inc/FreeRTOSConfig.h | 136 +- .../boards/osd/firmware/inc/dcc_stdio.h | 42 +- .../boards/osd/firmware/inc/font12x18.h | 19454 ++++++++-------- .../boards/osd/firmware/inc/font8x10.h | 11262 +++++---- .../targets/boards/osd/firmware/inc/fonts.h | 25 +- .../boards/osd/firmware/inc/openpilot.h | 22 +- .../boards/osd/firmware/inc/pios_config.h | 14 +- .../osd/firmware/inc/pios_usb_board_data.h | 12 +- .../targets/boards/osd/firmware/inc/splash.h | 2844 +-- flight/targets/boards/osd/firmware/osd.c | 184 +- .../targets/boards/osd/firmware/pios_board.c | 632 +- flight/targets/boards/osd/pios_board.h | 282 +- .../targets/boards/osd/pios_usb_board_data.c | 85 +- .../targets/boards/revolution/board_hw_defs.c | 3020 +-- .../boards/revolution/bootloader/inc/common.h | 90 +- .../revolution/bootloader/inc/pios_config.h | 26 +- .../bootloader/inc/pios_usb_board_data.h | 12 +- .../boards/revolution/bootloader/main.c | 310 +- .../boards/revolution/bootloader/pios_board.c | 83 +- .../revolution/firmware/cm3_fault_handlers.c | 109 +- .../boards/revolution/firmware/dcc_stdio.c | 176 +- .../revolution/firmware/inc/FreeRTOSConfig.h | 136 +- .../revolution/firmware/inc/dcc_stdio.h | 42 +- .../revolution/firmware/inc/openpilot.h | 22 +- .../revolution/firmware/inc/pios_board_sim.h | 71 +- .../revolution/firmware/inc/pios_config.h | 16 +- .../revolution/firmware/inc/pios_config_sim.h | 60 +- .../firmware/inc/pios_usb_board_data.h | 12 +- .../boards/revolution/firmware/pios_board.c | 1217 +- .../revolution/firmware/pios_board_sim.c | 241 +- .../boards/revolution/firmware/revolution.c | 121 +- flight/targets/boards/revolution/pios_board.h | 308 +- .../boards/revolution/pios_usb_board_data.c | 99 +- .../targets/boards/revoproto/board_hw_defs.c | 3193 +-- .../boards/revoproto/bootloader/inc/common.h | 90 +- .../revoproto/bootloader/inc/pios_config.h | 26 +- .../bootloader/inc/pios_usb_board_data.h | 12 +- .../boards/revoproto/bootloader/main.c | 304 +- .../boards/revoproto/bootloader/pios_board.c | 75 +- .../revoproto/firmware/cm3_fault_handlers.c | 109 +- .../boards/revoproto/firmware/dcc_stdio.c | 176 +- .../revoproto/firmware/inc/FreeRTOSConfig.h | 136 +- .../boards/revoproto/firmware/inc/dcc_stdio.h | 42 +- .../boards/revoproto/firmware/inc/openpilot.h | 22 +- .../revoproto/firmware/inc/pios_board_sim.h | 71 +- .../revoproto/firmware/inc/pios_config.h | 16 +- .../revoproto/firmware/inc/pios_config_sim.h | 60 +- .../firmware/inc/pios_usb_board_data.h | 12 +- .../boards/revoproto/firmware/pios_board.c | 1411 +- .../revoproto/firmware/pios_board_sim.c | 241 +- .../boards/revoproto/firmware/revolution.c | 121 +- flight/targets/boards/revoproto/pios_board.h | 262 +- .../boards/revoproto/pios_usb_board_data.c | 99 +- .../targets/boards/simposix/board_hw_defs.c | 35 +- .../simposix/firmware/inc/FreeRTOSConfig.h | 148 +- .../simposix/firmware/inc/FreeRTOSConfig_bk.h | 136 +- .../boards/simposix/firmware/inc/dcc_stdio.h | 42 +- .../boards/simposix/firmware/inc/openpilot.h | 22 +- .../simposix/firmware/inc/pios_config.h | 78 +- .../boards/simposix/firmware/pios_board.c | 239 +- .../boards/simposix/firmware/simposix.c | 119 +- flight/targets/boards/simposix/pios_board.h | 280 +- .../bootloader_updater/inc/pios_config.h | 26 +- .../targets/common/bootloader_updater/main.c | 63 +- .../common/bootloader_updater/pios_board.c | 22 +- flight/tests/logfs/openpilot.h | 4 +- flight/tests/logfs/pios_flash_ut.c | 165 +- flight/tests/logfs/pios_flash_ut_priv.h | 6 +- flight/tests/logfs/unittest.cpp | 320 +- flight/tests/logfs/unittest_init.c | 21 +- flight/uavobjects/callbackscheduler.c | 456 +- flight/uavobjects/eventdispatcher.c | 318 +- flight/uavobjects/inc/callbackscheduler.h | 54 +- flight/uavobjects/inc/eventdispatcher.h | 16 +- flight/uavobjects/inc/uavobjectmanager.h | 132 +- flight/uavobjects/inc/utlist.h | 583 +- flight/uavobjects/uavobjectmanager.c | 2214 +- flight/uavtalk/inc/uavtalk.h | 6 +- flight/uavtalk/inc/uavtalk_priv.h | 98 +- flight/uavtalk/uavtalk.c | 1309 +- .../openpilotgcs/src/app/gcssplashscreen.cpp | 12 +- ground/openpilotgcs/src/app/gcssplashscreen.h | 11 +- ground/openpilotgcs/src/app/main.cpp | 611 +- .../src/experimental/HIDTest/main.cpp | 50 +- .../src/experimental/PowerLog6S/main.cpp | 310 +- .../src/experimental/SerialLogger/main.cpp | 56 +- .../experimental/USB_UPLOAD_TOOL/SSP/main.cpp | 91 +- .../experimental/USB_UPLOAD_TOOL/SSP/port.cpp | 30 +- .../experimental/USB_UPLOAD_TOOL/SSP/port.h | 53 +- .../experimental/USB_UPLOAD_TOOL/SSP/qssp.cpp | 515 +- .../experimental/USB_UPLOAD_TOOL/SSP/qssp.h | 104 +- .../USB_UPLOAD_TOOL/SSP/qsspt.cpp | 57 +- .../experimental/USB_UPLOAD_TOOL/SSP/qsspt.h | 11 +- .../experimental/USB_UPLOAD_TOOL/SSP/ssp.cpp | 709 +- .../experimental/USB_UPLOAD_TOOL/SSP/ssp.h | 163 +- .../experimental/USB_UPLOAD_TOOL/delay.cpp | 2 - .../src/experimental/USB_UPLOAD_TOOL/delay.h | 3 +- .../src/experimental/USB_UPLOAD_TOOL/main.cpp | 409 +- .../experimental/USB_UPLOAD_TOOL/op_dfu.cpp | 996 +- .../src/experimental/USB_UPLOAD_TOOL/op_dfu.h | 156 +- .../src/experimental/finaltest/main.cpp | 1 + .../src/experimental/finaltest/mainwindow.cpp | 89 +- .../src/experimental/finaltest/mainwindow.h | 8 +- .../experimental/finaltest/ui_mainwindow.h | 15 +- .../tools/DocumentationHelper/main.cpp | 54 +- .../tools/DocumentationHelper/mainwindow.cpp | 300 +- .../tools/DocumentationHelper/mainwindow.h | 52 +- .../src/libs/aggregation/aggregate.cpp | 81 +- .../src/libs/aggregation/aggregate.h | 65 +- .../src/libs/aggregation/aggregation_global.h | 28 +- .../libs/aggregation/examples/text/main.cpp | 30 +- .../src/libs/aggregation/examples/text/main.h | 31 +- .../aggregation/examples/text/myinterfaces.h | 60 +- .../extensionsystem/extensionsystem_global.h | 28 +- .../src/libs/extensionsystem/iplugin.cpp | 57 +- .../src/libs/extensionsystem/iplugin.h | 39 +- .../src/libs/extensionsystem/iplugin_p.h | 34 +- .../libs/extensionsystem/optionsparser.cpp | 118 +- .../src/libs/extensionsystem/optionsparser.h | 47 +- .../extensionsystem/plugindetailsview.cpp | 42 +- .../libs/extensionsystem/plugindetailsview.h | 39 +- .../libs/extensionsystem/pluginerrorview.cpp | 55 +- .../libs/extensionsystem/pluginerrorview.h | 35 +- .../libs/extensionsystem/pluginmanager.cpp | 219 +- .../src/libs/extensionsystem/pluginmanager.h | 60 +- .../libs/extensionsystem/pluginmanager_p.h | 38 +- .../src/libs/extensionsystem/pluginspec.cpp | 334 +- .../src/libs/extensionsystem/pluginspec.h | 46 +- .../src/libs/extensionsystem/pluginspec_p.h | 34 +- .../src/libs/extensionsystem/pluginview.cpp | 91 +- .../src/libs/extensionsystem/pluginview.h | 37 +- .../src/libs/extensionsystem/pluginview_p.h | 34 +- .../circularplugins/plugin1/plugin1.cpp | 35 +- .../circularplugins/plugin1/plugin1.h | 33 +- .../circularplugins/plugin2/plugin2.cpp | 35 +- .../circularplugins/plugin2/plugin2.h | 33 +- .../circularplugins/plugin3/plugin3.cpp | 35 +- .../circularplugins/plugin3/plugin3.h | 33 +- .../correctplugins1/plugin1/plugin1.cpp | 51 +- .../correctplugins1/plugin1/plugin1.h | 33 +- .../correctplugins1/plugin2/plugin2.cpp | 35 +- .../correctplugins1/plugin2/plugin2.h | 33 +- .../correctplugins1/plugin3/plugin3.cpp | 45 +- .../correctplugins1/plugin3/plugin3.h | 33 +- .../auto/pluginmanager/tst_pluginmanager.cpp | 129 +- .../auto/pluginspec/testplugin/testplugin.cpp | 32 +- .../auto/pluginspec/testplugin/testplugin.h | 43 +- .../pluginspec/testplugin/testplugin_global.h | 28 +- .../test/auto/pluginspec/tst_pluginspec.cpp | 60 +- .../test/manual/pluginview/plugindialog.cpp | 48 +- .../test/manual/pluginview/plugindialog.h | 31 +- .../pluginview/plugins/plugin1/plugin1.cpp | 50 +- .../pluginview/plugins/plugin1/plugin1.h | 33 +- .../pluginview/plugins/plugin2/plugin2.cpp | 34 +- .../pluginview/plugins/plugin2/plugin2.h | 33 +- .../pluginview/plugins/plugin3/plugin3.cpp | 45 +- .../pluginview/plugins/plugin3/plugin3.h | 33 +- .../src/libs/opmapcontrol/opmapcontrol.h | 25 +- .../libs/opmapcontrol/src/core/accessmode.h | 140 +- .../opmapcontrol/src/core/alllayersoftype.cpp | 147 +- .../opmapcontrol/src/core/alllayersoftype.h | 62 +- .../src/libs/opmapcontrol/src/core/cache.cpp | 364 +- .../src/libs/opmapcontrol/src/core/cache.h | 103 +- .../opmapcontrol/src/core/cacheitemqueue.cpp | 146 +- .../opmapcontrol/src/core/cacheitemqueue.h | 111 +- .../libs/opmapcontrol/src/core/debugheader.h | 16 +- .../opmapcontrol/src/core/diagnostics.cpp | 53 +- .../libs/opmapcontrol/src/core/diagnostics.h | 68 +- .../opmapcontrol/src/core/geodecoderstatus.h | 215 +- .../opmapcontrol/src/core/kibertilecache.cpp | 125 +- .../opmapcontrol/src/core/kibertilecache.h | 86 +- .../opmapcontrol/src/core/languagetype.cpp | 185 +- .../libs/opmapcontrol/src/core/languagetype.h | 236 +- .../src/libs/opmapcontrol/src/core/maptype.h | 212 +- .../opmapcontrol/src/core/memorycache.cpp | 103 +- .../libs/opmapcontrol/src/core/memorycache.h | 69 +- .../src/libs/opmapcontrol/src/core/opmaps.cpp | 538 +- .../src/libs/opmapcontrol/src/core/opmaps.h | 150 +- .../libs/opmapcontrol/src/core/placemark.cpp | 51 +- .../libs/opmapcontrol/src/core/placemark.h | 93 +- .../src/libs/opmapcontrol/src/core/point.cpp | 135 +- .../src/libs/opmapcontrol/src/core/point.h | 135 +- .../opmapcontrol/src/core/providerstrings.cpp | 147 +- .../opmapcontrol/src/core/providerstrings.h | 129 +- .../libs/opmapcontrol/src/core/pureimage.cpp | 57 +- .../libs/opmapcontrol/src/core/pureimage.h | 64 +- .../opmapcontrol/src/core/pureimagecache.cpp | 623 +- .../opmapcontrol/src/core/pureimagecache.h | 86 +- .../libs/opmapcontrol/src/core/rawtile.cpp | 75 +- .../src/libs/opmapcontrol/src/core/rawtile.h | 85 +- .../src/libs/opmapcontrol/src/core/size.cpp | 54 +- .../src/libs/opmapcontrol/src/core/size.h | 121 +- .../opmapcontrol/src/core/tilecachequeue.cpp | 142 +- .../opmapcontrol/src/core/tilecachequeue.h | 79 +- .../libs/opmapcontrol/src/core/urlfactory.cpp | 1313 +- .../libs/opmapcontrol/src/core/urlfactory.h | 124 +- .../src/internals/MouseWheelZoomType.cpp | 54 +- .../src/internals/copyrightstrings.h | 63 +- .../libs/opmapcontrol/src/internals/core.cpp | 1113 +- .../libs/opmapcontrol/src/internals/core.h | 462 +- .../opmapcontrol/src/internals/debugheader.h | 6 +- .../opmapcontrol/src/internals/loadtask.cpp | 56 +- .../opmapcontrol/src/internals/loadtask.h | 78 +- .../src/internals/mousewheelzoomtype.h | 84 +- .../src/internals/pointlatlng.cpp | 74 +- .../opmapcontrol/src/internals/pointlatlng.h | 174 +- .../internals/projections/lks94projection.cpp | 897 +- .../internals/projections/lks94projection.h | 101 +- .../projections/mercatorprojection.cpp | 77 +- .../projections/mercatorprojection.h | 71 +- .../projections/mercatorprojectionyandex.cpp | 109 +- .../projections/mercatorprojectionyandex.h | 73 +- .../projections/platecarreeprojection.cpp | 82 +- .../projections/platecarreeprojection.h | 72 +- .../platecarreeprojectionpergo.cpp | 82 +- .../projections/platecarreeprojectionpergo.h | 72 +- .../src/internals/pureprojection.cpp | 525 +- .../src/internals/pureprojection.h | 106 +- .../opmapcontrol/src/internals/rectangle.cpp | 125 +- .../opmapcontrol/src/internals/rectangle.h | 218 +- .../opmapcontrol/src/internals/rectlatlng.cpp | 64 +- .../opmapcontrol/src/internals/rectlatlng.h | 231 +- .../opmapcontrol/src/internals/sizelatlng.cpp | 80 +- .../opmapcontrol/src/internals/sizelatlng.h | 187 +- .../libs/opmapcontrol/src/internals/tile.cpp | 76 +- .../libs/opmapcontrol/src/internals/tile.h | 91 +- .../opmapcontrol/src/internals/tilematrix.cpp | 202 +- .../opmapcontrol/src/internals/tilematrix.h | 67 +- .../src/mapwidget/configuration.cpp | 65 +- .../src/mapwidget/configuration.h | 276 +- .../opmapcontrol/src/mapwidget/gpsitem.cpp | 329 +- .../libs/opmapcontrol/src/mapwidget/gpsitem.h | 443 +- .../opmapcontrol/src/mapwidget/homeitem.cpp | 291 +- .../opmapcontrol/src/mapwidget/homeitem.h | 169 +- .../src/mapwidget/mapgraphicitem.cpp | 922 +- .../src/mapwidget/mapgraphicitem.h | 433 +- .../opmapcontrol/src/mapwidget/mapripform.cpp | 52 +- .../opmapcontrol/src/mapwidget/mapripform.h | 59 +- .../opmapcontrol/src/mapwidget/mapripper.cpp | 217 +- .../opmapcontrol/src/mapwidget/mapripper.h | 102 +- .../src/mapwidget/opmapwidget.cpp | 1070 +- .../opmapcontrol/src/mapwidget/opmapwidget.h | 1057 +- .../opmapcontrol/src/mapwidget/trailitem.cpp | 108 +- .../opmapcontrol/src/mapwidget/trailitem.h | 91 +- .../src/mapwidget/traillineitem.cpp | 82 +- .../src/mapwidget/traillineitem.h | 85 +- .../opmapcontrol/src/mapwidget/uavitem.cpp | 806 +- .../libs/opmapcontrol/src/mapwidget/uavitem.h | 537 +- .../src/mapwidget/uavmapfollowtype.h | 138 +- .../opmapcontrol/src/mapwidget/uavtrailtype.h | 150 +- .../src/mapwidget/waypointcircle.cpp | 131 +- .../src/mapwidget/waypointcircle.h | 72 +- .../src/mapwidget/waypointitem.cpp | 934 +- .../opmapcontrol/src/mapwidget/waypointitem.h | 365 +- .../src/mapwidget/waypointline.cpp | 134 +- .../opmapcontrol/src/mapwidget/waypointline.h | 71 +- .../src/posix_qextserialport.cpp | 1125 +- .../qextserialport/src/qextserialenumerator.h | 243 +- .../src/qextserialenumerator_osx.cpp | 159 +- .../src/qextserialenumerator_unix.cpp | 43 +- .../src/qextserialenumerator_win.cpp | 122 +- .../qextserialport/src/qextserialport.cpp | 201 +- .../libs/qextserialport/src/qextserialport.h | 384 +- .../src/qextserialport_global.h | 3 - .../qextserialport/src/win_qextserialport.cpp | 1047 +- .../src/libs/qscispinbox/QScienceSpinBox.cpp | 394 +- .../src/libs/qscispinbox/QScienceSpinBox.h | 38 +- .../src/libs/qtconcurrent/multitask.h | 75 +- .../libs/qtconcurrent/qtconcurrent_global.h | 28 +- .../src/libs/qtconcurrent/runextensions.h | 142 +- .../src/libs/sdlgamepad/sdlgamepad.cpp | 178 +- .../src/libs/sdlgamepad/sdlgamepad.h | 79 +- .../src/libs/utils/abstractprocess.h | 60 +- .../src/libs/utils/abstractprocess_win.cpp | 57 +- .../src/libs/utils/basevalidatinglineedit.cpp | 54 +- .../src/libs/utils/basevalidatinglineedit.h | 38 +- .../src/libs/utils/cachedsvgitem.cpp | 61 +- .../src/libs/utils/cachedsvgitem.h | 15 +- .../src/libs/utils/checkablemessagebox.cpp | 66 +- .../src/libs/utils/checkablemessagebox.h | 75 +- .../utils/classnamevalidatinglineedit.cpp | 51 +- .../libs/utils/classnamevalidatinglineedit.h | 33 +- .../src/libs/utils/codegeneration.cpp | 58 +- .../src/libs/utils/codegeneration.h | 30 +- .../src/libs/utils/consoleprocess.cpp | 42 +- .../src/libs/utils/consoleprocess.h | 68 +- .../src/libs/utils/consoleprocess_unix.cpp | 96 +- .../src/libs/utils/consoleprocess_win.cpp | 69 +- .../src/libs/utils/coordinateconversions.cpp | 296 +- .../src/libs/utils/coordinateconversions.h | 5 +- .../src/libs/utils/detailsbutton.cpp | 28 +- .../src/libs/utils/detailsbutton.h | 31 +- .../src/libs/utils/detailswidget.cpp | 78 +- .../src/libs/utils/detailswidget.h | 34 +- .../src/libs/utils/fancylineedit.cpp | 71 +- .../src/libs/utils/fancylineedit.h | 40 +- .../src/libs/utils/fancymainwindow.cpp | 70 +- .../src/libs/utils/fancymainwindow.h | 45 +- .../libs/utils/filenamevalidatinglineedit.cpp | 63 +- .../libs/utils/filenamevalidatinglineedit.h | 36 +- .../src/libs/utils/filesearch.cpp | 151 +- .../openpilotgcs/src/libs/utils/filesearch.h | 42 +- .../src/libs/utils/filewizarddialog.cpp | 31 +- .../src/libs/utils/filewizarddialog.h | 35 +- .../src/libs/utils/filewizardpage.cpp | 41 +- .../src/libs/utils/filewizardpage.h | 36 +- .../src/libs/utils/homelocationutil.cpp | 69 +- .../src/libs/utils/homelocationutil.h | 16 +- .../src/libs/utils/iwelcomepage.cpp | 36 +- .../src/libs/utils/iwelcomepage.h | 38 +- .../src/libs/utils/linecolumnlabel.cpp | 41 +- .../src/libs/utils/linecolumnlabel.h | 36 +- .../openpilotgcs/src/libs/utils/listutils.h | 32 +- .../src/libs/utils/mylistwidget.cpp | 3 +- .../src/libs/utils/mylistwidget.h | 8 +- .../src/libs/utils/mytabbedstackwidget.cpp | 30 +- .../src/libs/utils/mytabbedstackwidget.h | 25 +- .../src/libs/utils/mytabwidget.cpp | 4 +- .../openpilotgcs/src/libs/utils/mytabwidget.h | 3 +- .../src/libs/utils/newclasswidget.cpp | 131 +- .../src/libs/utils/newclasswidget.h | 36 +- .../src/libs/utils/parameteraction.cpp | 43 +- .../src/libs/utils/parameteraction.h | 35 +- .../src/libs/utils/pathchooser.cpp | 94 +- .../openpilotgcs/src/libs/utils/pathchooser.h | 37 +- .../src/libs/utils/pathlisteditor.cpp | 98 +- .../src/libs/utils/pathlisteditor.h | 40 +- .../openpilotgcs/src/libs/utils/pathutils.cpp | 71 +- .../openpilotgcs/src/libs/utils/pathutils.h | 6 +- .../src/libs/utils/projectintropage.cpp | 44 +- .../src/libs/utils/projectintropage.h | 38 +- .../utils/projectnamevalidatinglineedit.cpp | 43 +- .../utils/projectnamevalidatinglineedit.h | 36 +- .../openpilotgcs/src/libs/utils/qtcassert.h | 34 +- .../src/libs/utils/qtcolorbutton.cpp | 99 +- .../src/libs/utils/qtcolorbutton.h | 36 +- .../src/libs/utils/qwineventnotifier_p.h | 11 +- .../src/libs/utils/reloadpromptutils.cpp | 47 +- .../src/libs/utils/reloadpromptutils.h | 30 +- .../src/libs/utils/savedaction.cpp | 233 +- .../openpilotgcs/src/libs/utils/savedaction.h | 43 +- .../src/libs/utils/settingsutils.cpp | 34 +- .../src/libs/utils/settingsutils.h | 30 +- .../openpilotgcs/src/libs/utils/styledbar.cpp | 34 +- .../openpilotgcs/src/libs/utils/styledbar.h | 36 +- .../src/libs/utils/stylehelper.cpp | 71 +- .../openpilotgcs/src/libs/utils/stylehelper.h | 47 +- .../src/libs/utils/submiteditorwidget.cpp | 150 +- .../src/libs/utils/submiteditorwidget.h | 40 +- .../src/libs/utils/submitfieldwidget.cpp | 139 +- .../src/libs/utils/submitfieldwidget.h | 38 +- .../src/libs/utils/svgimageprovider.cpp | 89 +- .../src/libs/utils/svgimageprovider.h | 13 +- .../src/libs/utils/synchronousprocess.cpp | 186 +- .../src/libs/utils/synchronousprocess.h | 47 +- .../libs/utils/treewidgetcolumnstretcher.cpp | 46 +- .../libs/utils/treewidgetcolumnstretcher.h | 41 +- .../src/libs/utils/uncommentselection.cpp | 70 +- .../src/libs/utils/uncommentselection.h | 30 +- .../src/libs/utils/utils_global.h | 28 +- .../src/libs/utils/welcomemodetreewidget.cpp | 58 +- .../src/libs/utils/welcomemodetreewidget.h | 38 +- .../openpilotgcs/src/libs/utils/winutils.cpp | 49 +- ground/openpilotgcs/src/libs/utils/winutils.h | 29 +- .../src/libs/utils/worldmagmodel.cpp | 1785 +- .../src/libs/utils/worldmagmodel.h | 212 +- .../openpilotgcs/src/libs/utils/xmlconfig.cpp | 154 +- .../openpilotgcs/src/libs/utils/xmlconfig.h | 8 +- .../antennatrack/antennatrackgadget.cpp | 93 +- .../plugins/antennatrack/antennatrackgadget.h | 12 +- .../antennatrackgadgetconfiguration.cpp | 65 +- .../antennatrackgadgetconfiguration.h | 114 +- .../antennatrackgadgetfactory.cpp | 22 +- .../antennatrack/antennatrackgadgetfactory.h | 5 +- .../antennatrackgadgetoptionspage.cpp | 273 +- .../antennatrackgadgetoptionspage.h | 7 +- .../antennatrack/antennatrackplugin.cpp | 21 +- .../plugins/antennatrack/antennatrackplugin.h | 5 +- .../antennatrack/antennatrackwidget.cpp | 150 +- .../plugins/antennatrack/antennatrackwidget.h | 43 +- .../src/plugins/antennatrack/gpsparser.cpp | 10 +- .../src/plugins/antennatrack/gpsparser.h | 28 +- .../plugins/antennatrack/telemetryparser.cpp | 27 +- .../plugins/antennatrack/telemetryparser.h | 14 +- .../plugins/config/alignment-calibration.cpp | 47 +- .../src/plugins/config/assertions.h | 53 +- .../src/plugins/config/calibration.h | 71 +- .../cfg_vehicletypes/configccpmwidget.cpp | 761 +- .../cfg_vehicletypes/configccpmwidget.h | 12 +- .../cfg_vehicletypes/configcustomwidget.cpp | 108 +- .../cfg_vehicletypes/configcustomwidget.h | 7 +- .../configfixedwingwidget.cpp | 235 +- .../cfg_vehicletypes/configfixedwingwidget.h | 4 +- .../configgroundvehiclewidget.cpp | 168 +- .../configgroundvehiclewidget.h | 4 +- .../configmultirotorwidget.cpp | 361 +- .../cfg_vehicletypes/configmultirotorwidget.h | 3 +- .../config/cfg_vehicletypes/vehicleconfig.cpp | 51 +- .../config/cfg_vehicletypes/vehicleconfig.h | 121 +- .../plugins/config/config_cc_hw_widget.cpp | 74 +- .../src/plugins/config/config_cc_hw_widget.h | 3 +- .../plugins/config/configautotunewidget.cpp | 62 +- .../src/plugins/config/configautotunewidget.h | 3 +- .../configcamerastabilizationwidget.cpp | 58 +- .../config/configcamerastabilizationwidget.h | 3 +- .../plugins/config/configccattitudewidget.cpp | 96 +- .../plugins/config/configccattitudewidget.h | 6 +- .../src/plugins/config/configgadget.cpp | 7 +- .../src/plugins/config/configgadget.h | 12 +- .../plugins/config/configgadgetfactory.cpp | 10 +- .../src/plugins/config/configgadgetfactory.h | 5 +- .../src/plugins/config/configgadgetwidget.cpp | 2 +- .../src/plugins/config/configgadgetwidget.h | 21 +- .../src/plugins/config/configinputwidget.cpp | 1229 +- .../src/plugins/config/configinputwidget.h | 209 +- .../src/plugins/config/configoutputwidget.cpp | 145 +- .../src/plugins/config/configoutputwidget.h | 43 +- .../plugins/config/configpipxtremewidget.cpp | 515 +- .../plugins/config/configpipxtremewidget.h | 61 +- .../src/plugins/config/configplugin.cpp | 116 +- .../src/plugins/config/configplugin.h | 12 +- .../src/plugins/config/configrevohwwidget.cpp | 216 +- .../src/plugins/config/configrevohwwidget.h | 6 +- .../src/plugins/config/configrevowidget.cpp | 23 +- .../config/configstabilizationwidget.h | 7 +- .../src/plugins/config/configtxpidwidget.cpp | 13 +- .../src/plugins/config/configtxpidwidget.h | 3 +- .../config/configvehicletypewidget.cpp | 124 +- .../plugins/config/configvehicletypewidget.h | 4 +- .../src/plugins/config/dblspindelegate.cpp | 36 +- .../src/plugins/config/dblspindelegate.h | 38 +- .../plugins/config/defaultattitudewidget.cpp | 5 +- .../plugins/config/defaultattitudewidget.h | 3 +- .../config/defaulthwsettingswidget.cpp | 5 +- .../plugins/config/defaulthwsettingswidget.h | 3 +- .../src/plugins/config/fancytabwidget.cpp | 113 +- .../src/plugins/config/fancytabwidget.h | 88 +- .../src/plugins/config/gyro-calibration.cpp | 124 +- .../src/plugins/config/inputchannelform.h | 7 +- .../src/plugins/config/legacy-calibration.cpp | 113 +- .../src/plugins/config/mixercurve.h | 25 +- .../src/plugins/config/outputchannelform.cpp | 179 +- .../src/plugins/config/outputchannelform.h | 3 +- .../src/plugins/config/twostep.cpp | 757 +- .../plugins/consolegadget/consolegadget.cpp | 9 +- .../src/plugins/consolegadget/consolegadget.h | 20 +- .../consolegadget/consolegadgetfactory.cpp | 19 +- .../consolegadget/consolegadgetfactory.h | 5 +- .../consolegadget/consolegadgetwidget.cpp | 7 +- .../consolegadget/consolegadgetwidget.h | 7 +- .../plugins/consolegadget/consoleplugin.cpp | 23 +- .../src/plugins/consolegadget/consoleplugin.h | 17 +- .../consolegadget/texteditloggerengine.cpp | 62 +- .../consolegadget/texteditloggerengine.h | 28 +- .../actionmanager/actioncontainer.cpp | 141 +- .../actionmanager/actioncontainer.h | 35 +- .../actionmanager/actioncontainer_p.h | 41 +- .../actionmanager/actionmanager.cpp | 117 +- .../coreplugin/actionmanager/actionmanager.h | 27 +- .../actionmanager/actionmanager_p.h | 45 +- .../coreplugin/actionmanager/command.cpp | 118 +- .../coreplugin/actionmanager/command.h | 41 +- .../coreplugin/actionmanager/command_p.h | 36 +- .../coreplugin/actionmanager/commandsfile.cpp | 51 +- .../coreplugin/actionmanager/commandsfile.h | 29 +- .../src/plugins/coreplugin/authorsdialog.cpp | 136 +- .../src/plugins/coreplugin/authorsdialog.h | 25 +- .../src/plugins/coreplugin/basemode.cpp | 31 +- .../src/plugins/coreplugin/basemode.h | 87 +- .../src/plugins/coreplugin/baseview.cpp | 27 +- .../src/plugins/coreplugin/baseview.h | 25 +- .../plugins/coreplugin/connectionmanager.cpp | 204 +- .../plugins/coreplugin/connectionmanager.h | 66 +- .../src/plugins/coreplugin/core_global.h | 20 +- .../src/plugins/coreplugin/coreconstants.h | 327 +- .../src/plugins/coreplugin/coreimpl.cpp | 39 +- .../src/plugins/coreplugin/coreimpl.h | 33 +- .../src/plugins/coreplugin/coreplugin.cpp | 29 +- .../src/plugins/coreplugin/coreplugin.h | 27 +- .../coreplugin/dialogs/ioptionspage.cpp | 44 +- .../plugins/coreplugin/dialogs/ioptionspage.h | 57 +- .../plugins/coreplugin/dialogs/iwizard.cpp | 67 +- .../src/plugins/coreplugin/dialogs/iwizard.h | 39 +- .../coreplugin/dialogs/settingsdialog.cpp | 124 +- .../coreplugin/dialogs/settingsdialog.h | 30 +- .../coreplugin/dialogs/shortcutsettings.cpp | 150 +- .../coreplugin/dialogs/shortcutsettings.h | 29 +- .../coreplugin/eventfilteringmainwindow.cpp | 23 +- .../coreplugin/eventfilteringmainwindow.h | 26 +- .../src/plugins/coreplugin/fancyactionbar.cpp | 66 +- .../src/plugins/coreplugin/fancyactionbar.h | 32 +- .../src/plugins/coreplugin/fancytabwidget.cpp | 105 +- .../src/plugins/coreplugin/fancytabwidget.h | 91 +- .../plugins/coreplugin/fileiconprovider.cpp | 83 +- .../src/plugins/coreplugin/fileiconprovider.h | 28 +- .../plugins/coreplugin/generalsettings.cpp | 70 +- .../src/plugins/coreplugin/generalsettings.h | 36 +- .../plugins/coreplugin/iconfigurableplugin.h | 9 +- .../src/plugins/coreplugin/iconnection.cpp | 1 - .../src/plugins/coreplugin/iconnection.h | 55 +- .../src/plugins/coreplugin/icontext.h | 46 +- .../src/plugins/coreplugin/icore.cpp | 95 +- .../src/plugins/coreplugin/icore.h | 61 +- .../src/plugins/coreplugin/icorelistener.h | 63 +- .../src/plugins/coreplugin/imode.h | 27 +- .../src/plugins/coreplugin/ioutputpane.h | 39 +- .../src/plugins/coreplugin/iuavgadget.cpp | 1 - .../src/plugins/coreplugin/iuavgadget.h | 55 +- .../coreplugin/iuavgadgetconfiguration.cpp | 23 +- .../coreplugin/iuavgadgetconfiguration.h | 72 +- .../plugins/coreplugin/iuavgadgetfactory.h | 60 +- .../src/plugins/coreplugin/iversioncontrol.h | 31 +- .../src/plugins/coreplugin/iview.h | 29 +- .../src/plugins/coreplugin/mainwindow.cpp | 381 +- .../src/plugins/coreplugin/mainwindow.h | 50 +- .../src/plugins/coreplugin/manhattanstyle.cpp | 896 +- .../src/plugins/coreplugin/manhattanstyle.h | 23 +- .../src/plugins/coreplugin/messagemanager.cpp | 33 +- .../src/plugins/coreplugin/messagemanager.h | 30 +- .../coreplugin/messageoutputwindow.cpp | 31 +- .../plugins/coreplugin/messageoutputwindow.h | 30 +- .../src/plugins/coreplugin/mimedatabase.cpp | 462 +- .../src/plugins/coreplugin/mimedatabase.h | 49 +- .../src/plugins/coreplugin/minisplitter.cpp | 33 +- .../src/plugins/coreplugin/minisplitter.h | 25 +- .../src/plugins/coreplugin/modemanager.cpp | 148 +- .../src/plugins/coreplugin/modemanager.h | 47 +- .../src/plugins/coreplugin/outputpane.h | 38 +- .../src/plugins/coreplugin/plugindialog.cpp | 40 +- .../src/plugins/coreplugin/plugindialog.h | 25 +- .../src/plugins/coreplugin/rightpane.cpp | 52 +- .../src/plugins/coreplugin/rightpane.h | 33 +- .../plugins/coreplugin/settingsdatabase.cpp | 64 +- .../src/plugins/coreplugin/settingsdatabase.h | 25 +- .../src/plugins/coreplugin/sidebar.cpp | 125 +- .../src/plugins/coreplugin/sidebar.h | 54 +- .../src/plugins/coreplugin/styleanimator.cpp | 103 +- .../src/plugins/coreplugin/styleanimator.h | 99 +- .../coreplugin/tabpositionindicator.cpp | 24 +- .../plugins/coreplugin/tabpositionindicator.h | 30 +- .../coreplugin/telemetrymonitorwidget.cpp | 60 +- .../coreplugin/telemetrymonitorwidget.h | 51 +- .../src/plugins/coreplugin/threadmanager.cpp | 26 +- .../src/plugins/coreplugin/threadmanager.h | 32 +- .../src/plugins/coreplugin/uavconfiginfo.cpp | 104 +- .../src/plugins/coreplugin/uavconfiginfo.h | 53 +- .../plugins/coreplugin/uavgadgetdecorator.cpp | 61 +- .../plugins/coreplugin/uavgadgetdecorator.h | 59 +- .../coreplugin/uavgadgetinstancemanager.cpp | 215 +- .../coreplugin/uavgadgetinstancemanager.h | 68 +- .../uavgadgetmanager/splitterorview.cpp | 115 +- .../uavgadgetmanager/splitterorview.h | 57 +- .../uavgadgetmanager/uavgadgetmanager.cpp | 129 +- .../uavgadgetmanager/uavgadgetmanager.h | 52 +- .../uavgadgetmanager/uavgadgetview.cpp | 70 +- .../uavgadgetmanager/uavgadgetview.h | 7 +- .../uavgadgetoptionspagedecorator.cpp | 24 +- .../uavgadgetoptionspagedecorator.h | 47 +- .../plugins/coreplugin/uniqueidmanager.cpp | 25 +- .../src/plugins/coreplugin/uniqueidmanager.h | 30 +- .../plugins/coreplugin/variablemanager.cpp | 23 +- .../src/plugins/coreplugin/variablemanager.h | 30 +- .../src/plugins/coreplugin/versiondialog.cpp | 80 +- .../src/plugins/coreplugin/versiondialog.h | 25 +- .../plugins/coreplugin/workspacesettings.cpp | 74 +- .../plugins/coreplugin/workspacesettings.h | 60 +- .../src/plugins/debuggadget/debugengine.cpp | 7 +- .../src/plugins/debuggadget/debugengine.h | 11 +- .../src/plugins/debuggadget/debuggadget.cpp | 9 +- .../src/plugins/debuggadget/debuggadget.h | 26 +- .../debuggadget/debuggadgetfactory.cpp | 19 +- .../plugins/debuggadget/debuggadgetfactory.h | 5 +- .../plugins/debuggadget/debuggadgetwidget.cpp | 17 +- .../plugins/debuggadget/debuggadgetwidget.h | 11 +- .../src/plugins/debuggadget/debugplugin.cpp | 23 +- .../src/plugins/debuggadget/debugplugin.h | 17 +- .../src/plugins/dial/dialgadget.cpp | 34 +- .../src/plugins/dial/dialgadget.h | 12 +- .../plugins/dial/dialgadgetconfiguration.cpp | 81 +- .../plugins/dial/dialgadgetconfiguration.h | 297 +- .../src/plugins/dial/dialgadgetfactory.cpp | 24 +- .../src/plugins/dial/dialgadgetfactory.h | 7 +- .../plugins/dial/dialgadgetoptionspage.cpp | 151 +- .../src/plugins/dial/dialgadgetoptionspage.h | 9 +- .../src/plugins/dial/dialgadgetwidget.cpp | 683 +- .../src/plugins/dial/dialgadgetwidget.h | 205 +- .../src/plugins/dial/dialplugin.cpp | 23 +- .../src/plugins/dial/dialplugin.h | 17 +- .../src/plugins/donothing/donothingplugin.cpp | 70 +- .../src/plugins/donothing/donothingplugin.h | 47 +- .../src/plugins/emptygadget/emptygadget.cpp | 9 +- .../src/plugins/emptygadget/emptygadget.h | 28 +- .../emptygadget/emptygadgetfactory.cpp | 19 +- .../plugins/emptygadget/emptygadgetfactory.h | 5 +- .../plugins/emptygadget/emptygadgetwidget.cpp | 7 +- .../plugins/emptygadget/emptygadgetwidget.h | 7 +- .../src/plugins/emptygadget/emptyplugin.cpp | 23 +- .../src/plugins/emptygadget/emptyplugin.h | 17 +- .../plugins/gcscontrol/gcscontrolgadget.cpp | 439 +- .../src/plugins/gcscontrol/gcscontrolgadget.h | 28 +- .../gcscontrolgadgetconfiguration.cpp | 83 +- .../gcscontrolgadgetconfiguration.h | 85 +- .../gcscontrol/gcscontrolgadgetfactory.cpp | 23 +- .../gcscontrol/gcscontrolgadgetfactory.h | 6 +- .../gcscontrolgadgetoptionspage.cpp | 357 +- .../gcscontrol/gcscontrolgadgetoptionspage.h | 59 +- .../gcscontrol/gcscontrolgadgetwidget.cpp | 94 +- .../gcscontrol/gcscontrolgadgetwidget.h | 7 +- .../plugins/gcscontrol/gcscontrolplugin.cpp | 36 +- .../src/plugins/gcscontrol/gcscontrolplugin.h | 16 +- .../gcscontrol/gcsonctrolgadgetwidget.h | 7 +- .../plugins/gcscontrol/joystickcontrol.cpp | 57 +- .../src/plugins/gcscontrol/joystickcontrol.h | 19 +- .../src/plugins/gpsdisplay/buffer.cpp | 131 +- .../src/plugins/gpsdisplay/buffer.h | 57 +- .../gpsdisplay/gpsconstellationwidget.cpp | 56 +- .../gpsdisplay/gpsconstellationwidget.h | 27 +- .../plugins/gpsdisplay/gpsdisplaygadget.cpp | 99 +- .../src/plugins/gpsdisplay/gpsdisplaygadget.h | 14 +- .../gpsdisplaygadgetconfiguration.cpp | 67 +- .../gpsdisplaygadgetconfiguration.h | 116 +- .../gpsdisplay/gpsdisplaygadgetfactory.cpp | 24 +- .../gpsdisplay/gpsdisplaygadgetfactory.h | 7 +- .../gpsdisplaygadgetoptionspage.cpp | 275 +- .../gpsdisplay/gpsdisplaygadgetoptionspage.h | 9 +- .../plugins/gpsdisplay/gpsdisplayplugin.cpp | 22 +- .../src/plugins/gpsdisplay/gpsdisplayplugin.h | 17 +- .../plugins/gpsdisplay/gpsdisplaywidget.cpp | 77 +- .../src/plugins/gpsdisplay/gpsdisplaywidget.h | 25 +- .../src/plugins/gpsdisplay/gpsparser.cpp | 10 +- .../src/plugins/gpsdisplay/gpsparser.h | 26 +- .../src/plugins/gpsdisplay/gpssnrwidget.cpp | 70 +- .../src/plugins/gpsdisplay/gpssnrwidget.h | 8 +- .../src/plugins/gpsdisplay/nmeaparser.cpp | 651 +- .../src/plugins/gpsdisplay/nmeaparser.h | 78 +- .../plugins/gpsdisplay/telemetryparser.cpp | 81 +- .../src/plugins/gpsdisplay/telemetryparser.h | 17 +- .../hitl/aerosimrc/src/aerosimrcdatastruct.h | 284 +- .../src/plugins/hitl/aerosimrc/src/enums.h | 27 +- .../src/plugins/hitl/aerosimrc/src/plugin.cpp | 256 +- .../src/plugins/hitl/aerosimrc/src/plugin.h | 9 +- .../hitl/aerosimrc/src/qdebughandler.cpp | 3 +- .../plugins/hitl/aerosimrc/src/settings.cpp | 48 +- .../src/plugins/hitl/aerosimrc/src/settings.h | 48 +- .../plugins/hitl/aerosimrc/src/udpconnect.cpp | 75 +- .../plugins/hitl/aerosimrc/src/udpconnect.h | 30 +- .../hitl/aerosimrc/src/udptestmain.cpp | 1 + .../hitl/aerosimrc/src/udptestwidget.cpp | 154 +- .../hitl/aerosimrc/src/udptestwidget.h | 9 +- .../src/plugins/hitl/aerosimrcsimulator.cpp | 125 +- .../src/plugins/hitl/aerosimrcsimulator.h | 14 +- .../src/plugins/hitl/fgsimulator.cpp | 225 +- .../src/plugins/hitl/fgsimulator.h | 39 +- .../src/plugins/hitl/hitlconfiguration.cpp | 156 +- .../src/plugins/hitl/hitlconfiguration.h | 46 +- .../src/plugins/hitl/hitlfactory.cpp | 27 +- .../src/plugins/hitl/hitlfactory.h | 9 +- .../src/plugins/hitl/hitlgadget.cpp | 18 +- .../src/plugins/hitl/hitlgadget.h | 22 +- .../src/plugins/hitl/hitlnoisegeneration.cpp | 54 +- .../src/plugins/hitl/hitlnoisegeneration.h | 19 +- .../src/plugins/hitl/hitloptionspage.cpp | 123 +- .../src/plugins/hitl/hitloptionspage.h | 43 +- .../src/plugins/hitl/hitlplugin.cpp | 33 +- .../src/plugins/hitl/hitlplugin.h | 45 +- .../src/plugins/hitl/hitlwidget.cpp | 180 +- .../src/plugins/hitl/hitlwidget.h | 32 +- .../src/plugins/hitl/il2simulator.cpp | 230 +- .../src/plugins/hitl/il2simulator.h | 34 +- .../src/plugins/hitl/isimulator.h | 85 +- .../src/plugins/hitl/simulator.cpp | 698 +- .../openpilotgcs/src/plugins/hitl/simulator.h | 258 +- .../src/plugins/hitl/xplanesimulator.cpp | 341 +- .../src/plugins/hitl/xplanesimulator.h | 79 +- .../importexport/importexportdialog.cpp | 3 +- .../plugins/importexport/importexportdialog.h | 5 +- .../importexport/importexportgadget.cpp | 19 +- .../plugins/importexport/importexportgadget.h | 8 +- .../importexport/importexportgadgetwidget.cpp | 96 +- .../importexport/importexportgadgetwidget.h | 18 +- .../importexport/importexportplugin.cpp | 14 +- .../plugins/importexport/importexportplugin.h | 6 +- .../ipconnection/ipconnection_internal.h | 9 +- .../ipconnectionconfiguration.cpp | 50 +- .../ipconnection/ipconnectionconfiguration.h | 54 +- .../ipconnection/ipconnectionoptionspage.cpp | 18 +- .../ipconnection/ipconnectionoptionspage.h | 30 +- .../ipconnection/ipconnectionplugin.cpp | 109 +- .../plugins/ipconnection/ipconnectionplugin.h | 39 +- .../plugins/lineardial/lineardialgadget.cpp | 26 +- .../src/plugins/lineardial/lineardialgadget.h | 12 +- .../lineardialgadgetconfiguration.cpp | 101 +- .../lineardialgadgetconfiguration.h | 149 +- .../lineardial/lineardialgadgetfactory.cpp | 24 +- .../lineardial/lineardialgadgetfactory.h | 7 +- .../lineardialgadgetoptionspage.cpp | 80 +- .../lineardial/lineardialgadgetoptionspage.h | 10 +- .../lineardial/lineardialgadgetwidget.cpp | 483 +- .../lineardial/lineardialgadgetwidget.h | 134 +- .../plugins/lineardial/lineardialplugin.cpp | 23 +- .../src/plugins/lineardial/lineardialplugin.h | 17 +- .../src/plugins/logging/logfile.cpp | 94 +- .../src/plugins/logging/logfile.h | 22 +- .../src/plugins/logging/logginggadget.cpp | 12 +- .../src/plugins/logging/logginggadget.h | 28 +- .../plugins/logging/logginggadgetfactory.cpp | 16 +- .../plugins/logging/logginggadgetfactory.h | 10 +- .../plugins/logging/logginggadgetwidget.cpp | 19 +- .../src/plugins/logging/logginggadgetwidget.h | 13 +- .../src/plugins/logging/loggingplugin.cpp | 227 +- .../src/plugins/logging/loggingplugin.h | 62 +- .../magicwaypoint/magicwaypointgadget.cpp | 9 +- .../magicwaypoint/magicwaypointgadget.h | 28 +- .../magicwaypointgadgetfactory.cpp | 17 +- .../magicwaypointgadgetfactory.h | 3 +- .../magicwaypointgadgetwidget.cpp | 59 +- .../magicwaypoint/magicwaypointgadgetwidget.h | 11 +- .../magicwaypoint/magicwaypointplugin.cpp | 26 +- .../magicwaypoint/magicwaypointplugin.h | 13 +- .../plugins/magicwaypoint/positionfield.cpp | 52 +- .../src/plugins/magicwaypoint/positionfield.h | 11 +- .../src/plugins/modelview/modelviewgadget.cpp | 15 +- .../src/plugins/modelview/modelviewgadget.h | 12 +- .../modelviewgadgetconfiguration.cpp | 44 +- .../modelview/modelviewgadgetconfiguration.h | 63 +- .../modelview/modelviewgadgetfactory.cpp | 24 +- .../modelview/modelviewgadgetfactory.h | 7 +- .../modelview/modelviewgadgetoptionspage.cpp | 26 +- .../modelview/modelviewgadgetoptionspage.h | 51 +- .../modelview/modelviewgadgetwidget.cpp | 312 +- .../plugins/modelview/modelviewgadgetwidget.h | 46 +- .../src/plugins/modelview/modelviewplugin.cpp | 23 +- .../src/plugins/modelview/modelviewplugin.h | 17 +- .../src/plugins/notify/notificationitem.cpp | 202 +- .../src/plugins/notify/notificationitem.h | 243 +- .../src/plugins/notify/notifyitemdelegate.cpp | 72 +- .../src/plugins/notify/notifyitemdelegate.h | 11 +- .../src/plugins/notify/notifylogging.h | 4 +- .../src/plugins/notify/notifyplugin.cpp | 312 +- .../src/plugins/notify/notifyplugin.h | 87 +- .../plugins/notify/notifypluginfactory.cpp | 24 +- .../src/plugins/notify/notifypluginfactory.h | 3 +- .../src/plugins/notify/notifyplugingadget.h | 13 +- .../notify/notifypluginoptionspage.cpp | 234 +- .../plugins/notify/notifypluginoptionspage.h | 104 +- .../src/plugins/notify/notifytablemodel.cpp | 119 +- .../src/plugins/notify/notifytablemodel.h | 24 +- .../src/plugins/opmap/flightdatamodel.cpp | 718 +- .../src/plugins/opmap/flightdatamodel.h | 57 +- .../src/plugins/opmap/homeeditor.cpp | 7 +- .../src/plugins/opmap/homeeditor.h | 11 +- .../src/plugins/opmap/modelmapproxy.cpp | 283 +- .../src/plugins/opmap/modelmapproxy.h | 25 +- .../src/plugins/opmap/modeluavoproxy.cpp | 290 +- .../src/plugins/opmap/modeluavoproxy.h | 11 +- .../opmap/opmap_edit_waypoint_dialog.cpp | 141 +- .../opmap/opmap_edit_waypoint_dialog.h | 23 +- .../plugins/opmap/opmap_statusbar_widget.cpp | 2 +- .../plugins/opmap/opmap_statusbar_widget.h | 7 +- .../opmap/opmap_zoom_slider_widget.cpp | 2 +- .../plugins/opmap/opmap_zoom_slider_widget.h | 7 +- .../src/plugins/opmap/opmapgadget.cpp | 21 +- .../src/plugins/opmap/opmapgadget.h | 12 +- .../opmap/opmapgadgetconfiguration.cpp | 145 +- .../plugins/opmap/opmapgadgetconfiguration.h | 154 +- .../src/plugins/opmap/opmapgadgetfactory.cpp | 20 +- .../src/plugins/opmap/opmapgadgetfactory.h | 13 +- .../plugins/opmap/opmapgadgetoptionspage.cpp | 57 +- .../plugins/opmap/opmapgadgetoptionspage.h | 11 +- .../src/plugins/opmap/opmapgadgetwidget.cpp | 1437 +- .../src/plugins/opmap/opmapgadgetwidget.h | 91 +- .../src/plugins/opmap/opmapplugin.cpp | 22 +- .../src/plugins/opmap/opmapplugin.h | 13 +- .../src/plugins/opmap/pathcompiler.cpp | 157 +- .../src/plugins/opmap/pathcompiler.h | 110 +- .../src/plugins/opmap/pathplanner.cpp | 35 +- .../src/plugins/opmap/pathplanner.h | 31 +- .../src/plugins/opmap/widgetdelegates.cpp | 120 +- .../src/plugins/opmap/widgetdelegates.h | 47 +- .../osgearthview/osgearthviewgadget.cpp | 35 +- .../plugins/osgearthview/osgearthviewgadget.h | 28 +- .../osgearthviewgadgetconfiguration.cpp | 27 +- .../osgearthviewgadgetconfiguration.h | 27 +- .../osgearthviewgadgetfactory.cpp | 38 +- .../osgearthview/osgearthviewgadgetfactory.h | 7 +- .../osgearthviewgadgetoptionspage.cpp | 37 +- .../osgearthviewgadgetoptionspage.h | 9 +- .../osgearthview/osgearthviewplugin.cpp | 39 +- .../plugins/osgearthview/osgearthviewplugin.h | 17 +- .../osgearthview/osgearthviewwidget.cpp | 35 +- .../plugins/osgearthview/osgearthviewwidget.h | 25 +- .../plugins/osgearthview/osgviewerwidget.cpp | 150 +- .../plugins/osgearthview/osgviewerwidget.h | 33 +- .../pathactioneditor/browseritemdelegate.cpp | 39 +- .../pathactioneditor/browseritemdelegate.h | 30 +- .../pathactioneditor/fieldtreeitem.cpp | 21 +- .../plugins/pathactioneditor/fieldtreeitem.h | 258 +- .../pathactioneditorgadget.cpp | 9 +- .../pathactioneditor/pathactioneditorgadget.h | 28 +- .../pathactioneditorgadgetfactory.cpp | 17 +- .../pathactioneditorgadgetfactory.h | 3 +- .../pathactioneditorgadgetwidget.cpp | 23 +- .../pathactioneditorgadgetwidget.h | 5 +- .../pathactioneditorplugin.cpp | 26 +- .../pathactioneditor/pathactioneditorplugin.h | 13 +- .../pathactioneditortreemodel.cpp | 313 +- .../pathactioneditortreemodel.h | 39 +- .../src/plugins/pathactioneditor/treeitem.cpp | 75 +- .../src/plugins/pathactioneditor/treeitem.h | 208 +- .../src/plugins/pfd/pfdgadget.cpp | 22 +- .../openpilotgcs/src/plugins/pfd/pfdgadget.h | 12 +- .../plugins/pfd/pfdgadgetconfiguration.cpp | 19 +- .../src/plugins/pfd/pfdgadgetconfiguration.h | 55 +- .../src/plugins/pfd/pfdgadgetfactory.cpp | 24 +- .../src/plugins/pfd/pfdgadgetfactory.h | 7 +- .../src/plugins/pfd/pfdgadgetoptionspage.cpp | 20 +- .../src/plugins/pfd/pfdgadgetoptionspage.h | 9 +- .../src/plugins/pfd/pfdgadgetwidget.cpp | 1294 +- .../src/plugins/pfd/pfdgadgetwidget.h | 192 +- .../src/plugins/pfd/pfdplugin.cpp | 23 +- .../openpilotgcs/src/plugins/pfd/pfdplugin.h | 17 +- .../src/plugins/pfdqml/osgearth.cpp | 100 +- .../src/plugins/pfdqml/osgearth.h | 58 +- .../src/plugins/pfdqml/pfdqmlgadget.cpp | 24 +- .../src/plugins/pfdqml/pfdqmlgadget.h | 10 +- .../pfdqml/pfdqmlgadgetconfiguration.cpp | 47 +- .../pfdqml/pfdqmlgadgetconfiguration.h | 99 +- .../plugins/pfdqml/pfdqmlgadgetfactory.cpp | 20 +- .../src/plugins/pfdqml/pfdqmlgadgetfactory.h | 3 +- .../pfdqml/pfdqmlgadgetoptionspage.cpp | 16 +- .../plugins/pfdqml/pfdqmlgadgetoptionspage.h | 7 +- .../src/plugins/pfdqml/pfdqmlgadgetwidget.cpp | 58 +- .../src/plugins/pfdqml/pfdqmlgadgetwidget.h | 40 +- .../src/plugins/pfdqml/pfdqmlplugin.cpp | 21 +- .../src/plugins/pfdqml/pfdqmlplugin.h | 15 +- .../src/plugins/powerlog/powerlogplugin.cpp | 245 +- .../src/plugins/powerlog/powerlogplugin.h | 66 +- .../src/plugins/qmlview/qmlviewgadget.cpp | 22 +- .../src/plugins/qmlview/qmlviewgadget.h | 12 +- .../qmlview/qmlviewgadgetconfiguration.cpp | 17 +- .../qmlview/qmlviewgadgetconfiguration.h | 35 +- .../plugins/qmlview/qmlviewgadgetfactory.cpp | 24 +- .../plugins/qmlview/qmlviewgadgetfactory.h | 7 +- .../qmlview/qmlviewgadgetoptionspage.cpp | 19 +- .../qmlview/qmlviewgadgetoptionspage.h | 9 +- .../plugins/qmlview/qmlviewgadgetwidget.cpp | 36 +- .../src/plugins/qmlview/qmlviewgadgetwidget.h | 22 +- .../src/plugins/qmlview/qmlviewplugin.cpp | 23 +- .../src/plugins/qmlview/qmlviewplugin.h | 17 +- .../src/plugins/rawhid/pjrc_rawhid.h | 85 +- .../src/plugins/rawhid/pjrc_rawhid_mac.cpp | 138 +- .../src/plugins/rawhid/pjrc_rawhid_unix.cpp | 453 +- .../src/plugins/rawhid/pjrc_rawhid_win.cpp | 665 +- .../src/plugins/rawhid/rawhid.cpp | 210 +- .../openpilotgcs/src/plugins/rawhid/rawhid.h | 23 +- .../src/plugins/rawhid/rawhid_const.h | 10 +- .../src/plugins/rawhid/rawhid_global.h | 20 +- .../src/plugins/rawhid/rawhidplugin.cpp | 89 +- .../src/plugins/rawhid/rawhidplugin.h | 44 +- .../src/plugins/rawhid/usbmonitor.h | 105 +- .../src/plugins/rawhid/usbmonitor_linux.cpp | 104 +- .../src/plugins/rawhid/usbmonitor_mac.cpp | 137 +- .../src/plugins/rawhid/usbmonitor_win.cpp | 320 +- .../src/plugins/rawhid/usbsignalfilter.cpp | 13 +- .../src/plugins/rawhid/usbsignalfilter.h | 3 +- .../src/plugins/scope/plotdata.cpp | 159 +- .../openpilotgcs/src/plugins/scope/plotdata.h | 95 +- .../src/plugins/scope/scope_global.h | 21 +- .../src/plugins/scope/scopegadget.cpp | 5 +- .../src/plugins/scope/scopegadget.h | 16 +- .../scope/scopegadgetconfiguration.cpp | 36 +- .../src/plugins/scope/scopegadgetfactory.cpp | 25 +- .../src/plugins/scope/scopegadgetfactory.h | 9 +- .../plugins/scope/scopegadgetoptionspage.cpp | 12 +- .../src/plugins/scope/scopegadgetwidget.h | 85 +- .../src/plugins/scope/scopeplugin.cpp | 3 +- .../src/plugins/scope/scopeplugin.h | 7 +- .../plugins/serialconnection/serial_global.h | 22 +- .../plugins/serialconnection/serialplugin.cpp | 173 +- .../plugins/serialconnection/serialplugin.h | 71 +- .../serialpluginconfiguration.cpp | 12 +- .../serialpluginconfiguration.h | 18 +- .../serialpluginoptionspage.cpp | 73 +- .../serialpluginoptionspage.h | 31 +- .../setupwizard/biascalibrationutil.cpp | 50 +- .../plugins/setupwizard/biascalibrationutil.h | 9 +- .../plugins/setupwizard/connectiondiagram.cpp | 149 +- .../plugins/setupwizard/connectiondiagram.h | 9 +- .../setupwizard/outputcalibrationutil.cpp | 32 +- .../setupwizard/outputcalibrationutil.h | 7 +- .../setupwizard/pages/abstractwizardpage.cpp | 4 +- .../setupwizard/pages/abstractwizardpage.h | 9 +- .../setupwizard/pages/autoupdatepage.cpp | 7 +- .../setupwizard/pages/autoupdatepage.h | 11 +- .../setupwizard/pages/biascalibrationpage.cpp | 20 +- .../setupwizard/pages/biascalibrationpage.h | 5 +- .../setupwizard/pages/controllerpage.cpp | 45 +- .../setupwizard/pages/controllerpage.h | 7 +- .../src/plugins/setupwizard/pages/endpage.cpp | 9 +- .../src/plugins/setupwizard/pages/endpage.h | 7 +- .../plugins/setupwizard/pages/fixedwingpage.h | 7 +- .../src/plugins/setupwizard/pages/helipage.h | 7 +- .../plugins/setupwizard/pages/inputpage.cpp | 97 +- .../src/plugins/setupwizard/pages/inputpage.h | 3 +- .../plugins/setupwizard/pages/multipage.cpp | 111 +- .../src/plugins/setupwizard/pages/multipage.h | 7 +- .../setupwizard/pages/notyetimplementedpage.h | 7 +- .../pages/outputcalibrationpage.cpp | 177 +- .../setupwizard/pages/outputcalibrationpage.h | 15 +- .../plugins/setupwizard/pages/outputpage.cpp | 7 +- .../plugins/setupwizard/pages/outputpage.h | 7 +- .../plugins/setupwizard/pages/rebootpage.cpp | 2 +- .../plugins/setupwizard/pages/rebootpage.h | 7 +- .../setupwizard/pages/revocalibrationpage.h | 7 +- .../plugins/setupwizard/pages/savepage.cpp | 16 +- .../src/plugins/setupwizard/pages/savepage.h | 3 +- .../plugins/setupwizard/pages/startpage.cpp | 2 +- .../src/plugins/setupwizard/pages/startpage.h | 7 +- .../plugins/setupwizard/pages/summarypage.cpp | 3 +- .../plugins/setupwizard/pages/summarypage.h | 3 +- .../plugins/setupwizard/pages/surfacepage.h | 7 +- .../plugins/setupwizard/pages/vehiclepage.cpp | 14 +- .../plugins/setupwizard/pages/vehiclepage.h | 5 +- .../src/plugins/setupwizard/setupwizard.cpp | 368 +- .../src/plugins/setupwizard/setupwizard.h | 124 +- .../plugins/setupwizard/setupwizardplugin.cpp | 24 +- .../plugins/setupwizard/setupwizardplugin.h | 19 +- .../vehicleconfigurationhelper.cpp | 1455 +- .../setupwizard/vehicleconfigurationhelper.h | 17 +- .../vehicleconfigurationsource.cpp | 3 +- .../setupwizard/vehicleconfigurationsource.h | 37 +- .../systemhealth/systemhealthgadget.cpp | 20 +- .../plugins/systemhealth/systemhealthgadget.h | 10 +- .../systemhealthgadgetconfiguration.cpp | 15 +- .../systemhealthgadgetconfiguration.h | 26 +- .../systemhealthgadgetfactory.cpp | 22 +- .../systemhealth/systemhealthgadgetfactory.h | 5 +- .../systemhealthgadgetoptionspage.cpp | 14 +- .../systemhealthgadgetoptionspage.h | 7 +- .../systemhealth/systemhealthgadgetwidget.cpp | 158 +- .../systemhealth/systemhealthgadgetwidget.h | 40 +- .../systemhealth/systemhealthplugin.cpp | 20 +- .../plugins/systemhealth/systemhealthplugin.h | 15 +- .../uavobjectbrowser/browseritemdelegate.cpp | 39 +- .../uavobjectbrowser/browseritemdelegate.h | 30 +- .../uavobjectbrowser/browserplugin.cpp | 21 +- .../plugins/uavobjectbrowser/browserplugin.h | 15 +- .../uavobjectbrowser/fieldtreeitem.cpp | 21 +- .../plugins/uavobjectbrowser/fieldtreeitem.h | 174 +- .../src/plugins/uavobjectbrowser/treeitem.cpp | 94 +- .../src/plugins/uavobjectbrowser/treeitem.h | 265 +- .../uavobjectbrowser/uavobjectbrowser.cpp | 21 +- .../uavobjectbrowser/uavobjectbrowser.h | 12 +- .../uavobjectbrowserconfiguration.cpp | 52 +- .../uavobjectbrowserconfiguration.h | 112 +- .../uavobjectbrowserfactory.cpp | 18 +- .../uavobjectbrowserfactory.h | 5 +- .../uavobjectbrowseroptionspage.cpp | 24 +- .../uavobjectbrowseroptionspage.h | 27 +- .../uavobjectbrowserwidget.cpp | 73 +- .../uavobjectbrowser/uavobjectbrowserwidget.h | 27 +- .../uavobjectbrowser/uavobjecttreemodel.cpp | 240 +- .../uavobjectbrowser/uavobjecttreemodel.h | 47 +- .../src/plugins/uavobjects/tests/main.cpp | 2 +- .../uavobjects/tests/uavobjectstest.cpp | 84 +- .../plugins/uavobjects/tests/uavobjectstest.h | 25 +- .../src/plugins/uavobjects/uavdataobject.cpp | 45 +- .../src/plugins/uavobjects/uavdataobject.h | 40 +- .../src/plugins/uavobjects/uavmetaobject.cpp | 48 +- .../src/plugins/uavobjects/uavmetaobject.h | 34 +- .../src/plugins/uavobjects/uavobject.cpp | 244 +- .../src/plugins/uavobjects/uavobject.h | 131 +- .../src/plugins/uavobjects/uavobjectfield.cpp | 663 +- .../src/plugins/uavobjects/uavobjectfield.h | 60 +- .../plugins/uavobjects/uavobjectmanager.cpp | 186 +- .../src/plugins/uavobjects/uavobjectmanager.h | 57 +- .../plugins/uavobjects/uavobjects_global.h | 20 +- .../src/plugins/uavobjects/uavobjectsinit.h | 24 +- .../plugins/uavobjects/uavobjectsplugin.cpp | 41 +- .../src/plugins/uavobjects/uavobjectsplugin.h | 27 +- .../wireshark/op-uavtalk/packet-op-uavtalk.c | 292 +- .../uavobjectutil/devicedescriptorstruct.h | 103 +- .../uavobjectutil/uavobjectutilmanager.cpp | 338 +- .../uavobjectutil/uavobjectutilmanager.h | 67 +- .../uavobjectutil/uavobjectutilplugin.cpp | 44 +- .../uavobjectutil/uavobjectutilplugin.h | 29 +- .../uavobjectwidgetutils/configtaskwidget.cpp | 4 +- .../uavobjectwidgetutils/configtaskwidget.h | 12 +- .../uavobjectwidgetutils/mixercurveline.cpp | 26 +- .../uavobjectwidgetutils/mixercurveline.h | 13 +- .../uavobjectwidgetutils/mixercurvepoint.cpp | 66 +- .../uavobjectwidgetutils/mixercurvewidget.cpp | 94 +- .../uavobjectwidgetutils/popupwidget.cpp | 21 +- .../uavobjectwidgetutils/smartsavebutton.cpp | 190 +- .../uavobjectwidgetutilsplugin.cpp | 38 +- .../uavobjectwidgetutilsplugin.h | 25 +- .../uavsettingsimportexport/importsummary.cpp | 107 +- .../uavsettingsimportexport/importsummary.h | 9 +- .../uavsettingsimportexport.cpp | 53 +- .../uavsettingsimportexport.h | 48 +- .../uavsettingsimportexportfactory.cpp | 105 +- .../uavsettingsimportexportfactory.h | 4 +- .../src/plugins/uavtalk/telemetry.cpp | 345 +- .../src/plugins/uavtalk/telemetry.h | 96 +- .../src/plugins/uavtalk/telemetrymanager.cpp | 15 +- .../src/plugins/uavtalk/telemetrymanager.h | 11 +- .../src/plugins/uavtalk/telemetrymonitor.cpp | 125 +- .../src/plugins/uavtalk/telemetrymonitor.h | 31 +- .../src/plugins/uavtalk/uavtalk.cpp | 784 +- .../src/plugins/uavtalk/uavtalk.h | 85 +- .../src/plugins/uavtalk/uavtalk_global.h | 20 +- .../src/plugins/uavtalk/uavtalkplugin.cpp | 47 +- .../src/plugins/uavtalk/uavtalkplugin.h | 29 +- .../src/plugins/uploader/SSP/port.cpp | 33 +- .../src/plugins/uploader/SSP/port.h | 53 +- .../src/plugins/uploader/SSP/qssp.cpp | 515 +- .../src/plugins/uploader/SSP/qssp.h | 104 +- .../src/plugins/uploader/SSP/qsspt.cpp | 57 +- .../src/plugins/uploader/SSP/qsspt.h | 11 +- .../src/plugins/uploader/delay.cpp | 1 - .../openpilotgcs/src/plugins/uploader/delay.h | 3 +- .../src/plugins/uploader/devicewidget.cpp | 193 +- .../src/plugins/uploader/devicewidget.h | 8 +- .../openpilotgcs/src/plugins/uploader/enums.h | 7 +- .../src/plugins/uploader/op_dfu.cpp | 1063 +- .../src/plugins/uploader/op_dfu.h | 356 +- .../plugins/uploader/runningdevicewidget.cpp | 19 +- .../plugins/uploader/runningdevicewidget.h | 10 +- .../src/plugins/uploader/uploadergadget.cpp | 14 +- .../src/plugins/uploader/uploadergadget.h | 11 +- .../uploader/uploadergadgetconfiguration.cpp | 54 +- .../uploader/uploadergadgetconfiguration.h | 83 +- .../uploader/uploadergadgetfactory.cpp | 21 +- .../plugins/uploader/uploadergadgetfactory.h | 7 +- .../uploader/uploadergadgetoptionspage.cpp | 21 +- .../uploader/uploadergadgetoptionspage.h | 5 +- .../plugins/uploader/uploadergadgetwidget.cpp | 433 +- .../plugins/uploader/uploadergadgetwidget.h | 47 +- .../src/plugins/uploader/uploaderplugin.cpp | 22 +- .../src/plugins/uploader/uploaderplugin.h | 15 +- .../src/plugins/welcome/welcome_global.h | 20 +- .../src/plugins/welcome/welcomemode.cpp | 33 +- .../src/plugins/welcome/welcomemode.h | 35 +- .../src/plugins/welcome/welcomeplugin.cpp | 29 +- .../src/plugins/welcome/welcomeplugin.h | 26 +- .../src/shared/namespace_global.h | 30 +- .../src/shared/qtlockedfile/qtlockedfile.cpp | 56 +- .../src/shared/qtlockedfile/qtlockedfile.h | 39 +- .../shared/qtlockedfile/qtlockedfile_unix.cpp | 76 +- .../shared/qtlockedfile/qtlockedfile_win.cpp | 116 +- .../qtsingleapplication/qtlocalpeer.cpp | 88 +- .../shared/qtsingleapplication/qtlocalpeer.h | 41 +- .../qtsingleapplication.cpp | 57 +- .../qtsingleapplication/qtsingleapplication.h | 53 +- .../qtsinglecoreapplication.cpp | 34 +- .../qtsinglecoreapplication.h | 39 +- .../scriptwrapper/interface_wrap_helpers.h | 49 +- .../src/shared/scriptwrapper/wrap_helpers.h | 221 +- .../flight/uavobjectgeneratorflight.cpp | 381 +- .../flight/uavobjectgeneratorflight.h | 9 +- .../generators/gcs/uavobjectgeneratorgcs.cpp | 331 +- .../generators/gcs/uavobjectgeneratorgcs.h | 11 +- .../generators/generator_common.cpp | 37 +- .../generators/generator_common.h | 4 +- .../generators/generator_io.cpp | 23 +- .../uavobjgenerator/generators/generator_io.h | 4 +- .../java/uavobjectgeneratorjava.cpp | 211 +- .../generators/java/uavobjectgeneratorjava.h | 15 +- .../matlab/uavobjectgeneratormatlab.cpp | 130 +- .../matlab/uavobjectgeneratormatlab.h | 10 +- .../python/uavobjectgeneratorpython.cpp | 39 +- .../python/uavobjectgeneratorpython.h | 7 +- .../wireshark/uavobjectgeneratorwireshark.cpp | 319 +- .../wireshark/uavobjectgeneratorwireshark.h | 9 +- ground/uavobjgenerator/main.cpp | 147 +- ground/uavobjgenerator/uavobjectparser.cpp | 355 +- ground/uavobjgenerator/uavobjectparser.h | 65 +- 1510 files changed, 121831 insertions(+), 118983 deletions(-) diff --git a/flight/libraries/CoordinateConversions.c b/flight/libraries/CoordinateConversions.c index 1bac5f502..5d286aac1 100644 --- a/flight/libraries/CoordinateConversions.c +++ b/flight/libraries/CoordinateConversions.c @@ -37,242 +37,239 @@ // ****** convert Lat,Lon,Alt to ECEF ************ void LLA2ECEF(float LLA[3], float ECEF[3]) { - const float a = 6378137.0f; // Equatorial Radius - const float e = 8.1819190842622e-2f; // Eccentricity - float sinLat, sinLon, cosLat, cosLon; - float N; + const float a = 6378137.0f; // Equatorial Radius + const float e = 8.1819190842622e-2f; // Eccentricity + float sinLat, sinLon, cosLat, cosLon; + float N; - sinLat = sinf(DEG2RAD(LLA[0])); - sinLon = sinf(DEG2RAD(LLA[1])); - cosLat = cosf(DEG2RAD(LLA[0])); - cosLon = cosf(DEG2RAD(LLA[1])); + sinLat = sinf(DEG2RAD(LLA[0])); + sinLon = sinf(DEG2RAD(LLA[1])); + cosLat = cosf(DEG2RAD(LLA[0])); + cosLon = cosf(DEG2RAD(LLA[1])); - N = a / sqrtf(1.0f - e * e * sinLat * sinLat); //prime vertical radius of curvature + N = a / sqrtf(1.0f - e * e * sinLat * sinLat); // prime vertical radius of curvature - ECEF[0] = (N + LLA[2]) * cosLat * cosLon; - ECEF[1] = (N + LLA[2]) * cosLat * sinLon; - ECEF[2] = ((1 - e * e) * N + LLA[2]) * sinLat; + ECEF[0] = (N + LLA[2]) * cosLat * cosLon; + ECEF[1] = (N + LLA[2]) * cosLat * sinLon; + ECEF[2] = ((1 - e * e) * N + LLA[2]) * sinLat; } // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* uint16_t ECEF2LLA(float ECEF[3], float LLA[3]) { - /** - * LLA parameter is used to prime the iteration. - * A position within 1 meter of the specified LLA - * will be calculated within at most 3 iterations. - * If unknown: Call with any valid LLA coordinate - * will compute within at most 5 iterations. - * Suggestion: [0,0,0] - **/ + /** + * LLA parameter is used to prime the iteration. + * A position within 1 meter of the specified LLA + * will be calculated within at most 3 iterations. + * If unknown: Call with any valid LLA coordinate + * will compute within at most 5 iterations. + * Suggestion: [0,0,0] + **/ - const float a = 6378137.0f; // Equatorial Radius - const float e = 8.1819190842622e-2f; // Eccentricity - float x = ECEF[0], y = ECEF[1], z = ECEF[2]; - float Lat, N, NplusH, delta, esLat; - uint16_t iter; -#define MAX_ITER 10 // should not take more than 5 for valid coordinates -#define ACCURACY 1.0e-11f // used to be e-14, but we don't need sub micrometer exact calculations + const float a = 6378137.0f; // Equatorial Radius + const float e = 8.1819190842622e-2f; // Eccentricity + float x = ECEF[0], y = ECEF[1], z = ECEF[2]; + float Lat, N, NplusH, delta, esLat; + uint16_t iter; - LLA[1] = RAD2DEG(atan2f(y, x)); - Lat = DEG2RAD(LLA[0]); - esLat = e * sinf(Lat); - N = a / sqrtf(1 - esLat * esLat); - NplusH = N + LLA[2]; - delta = 1; - iter = 0; +#define MAX_ITER 10 // should not take more than 5 for valid coordinates +#define ACCURACY 1.0e-11f // used to be e-14, but we don't need sub micrometer exact calculations - while (((delta > ACCURACY) || (delta < -ACCURACY)) - && (iter < MAX_ITER)) { - delta = Lat - atanf(z / (sqrtf(x * x + y * y) * (1 - (N * e * e / NplusH)))); - Lat = Lat - delta; - esLat = e * sinf(Lat); - N = a / sqrtf(1 - esLat * esLat); - NplusH = sqrtf(x * x + y * y) / cosf(Lat); - iter += 1; - } + LLA[1] = RAD2DEG(atan2f(y, x)); + Lat = DEG2RAD(LLA[0]); + esLat = e * sinf(Lat); + N = a / sqrtf(1 - esLat * esLat); + NplusH = N + LLA[2]; + delta = 1; + iter = 0; - LLA[0] = RAD2DEG(Lat); - LLA[2] = NplusH - N; + while (((delta > ACCURACY) || (delta < -ACCURACY)) + && (iter < MAX_ITER)) { + delta = Lat - atanf(z / (sqrtf(x * x + y * y) * (1 - (N * e * e / NplusH)))); + Lat = Lat - delta; + esLat = e * sinf(Lat); + N = a / sqrtf(1 - esLat * esLat); + NplusH = sqrtf(x * x + y * y) / cosf(Lat); + iter += 1; + } - return (iter < MAX_ITER); + LLA[0] = RAD2DEG(Lat); + LLA[2] = NplusH - N; + + return iter < MAX_ITER; } // ****** find ECEF to NED rotation matrix ******** void RneFromLLA(float LLA[3], float Rne[3][3]) { - float sinLat, sinLon, cosLat, cosLon; + float sinLat, sinLon, cosLat, cosLon; - sinLat = (float)sinf(DEG2RAD(LLA[0])); - sinLon = (float)sinf(DEG2RAD(LLA[1])); - cosLat = (float)cosf(DEG2RAD(LLA[0])); - cosLon = (float)cosf(DEG2RAD(LLA[1])); + sinLat = (float)sinf(DEG2RAD(LLA[0])); + sinLon = (float)sinf(DEG2RAD(LLA[1])); + cosLat = (float)cosf(DEG2RAD(LLA[0])); + cosLon = (float)cosf(DEG2RAD(LLA[1])); - Rne[0][0] = -sinLat * cosLon; - Rne[0][1] = -sinLat * sinLon; - Rne[0][2] = cosLat; - Rne[1][0] = -sinLon; - Rne[1][1] = cosLon; - Rne[1][2] = 0; - Rne[2][0] = -cosLat * cosLon; - Rne[2][1] = -cosLat * sinLon; - Rne[2][2] = -sinLat; + Rne[0][0] = -sinLat * cosLon; + Rne[0][1] = -sinLat * sinLon; + Rne[0][2] = cosLat; + Rne[1][0] = -sinLon; + Rne[1][1] = cosLon; + Rne[1][2] = 0; + Rne[2][0] = -cosLat * cosLon; + Rne[2][1] = -cosLat * sinLon; + Rne[2][2] = -sinLat; } // ****** find roll, pitch, yaw from quaternion ******** void Quaternion2RPY(const float q[4], float rpy[3]) { - float R13, R11, R12, R23, R33; - float q0s = q[0] * q[0]; - float q1s = q[1] * q[1]; - float q2s = q[2] * q[2]; - float q3s = q[3] * q[3]; + float R13, R11, R12, R23, R33; + float q0s = q[0] * q[0]; + float q1s = q[1] * q[1]; + float q2s = q[2] * q[2]; + float q3s = q[3] * q[3]; - R13 = 2.0f * (q[1] * q[3] - q[0] * q[2]); - R11 = q0s + q1s - q2s - q3s; - R12 = 2.0f * (q[1] * q[2] + q[0] * q[3]); - R23 = 2.0f * (q[2] * q[3] + q[0] * q[1]); - R33 = q0s - q1s - q2s + q3s; + R13 = 2.0f * (q[1] * q[3] - q[0] * q[2]); + R11 = q0s + q1s - q2s - q3s; + R12 = 2.0f * (q[1] * q[2] + q[0] * q[3]); + R23 = 2.0f * (q[2] * q[3] + q[0] * q[1]); + R33 = q0s - q1s - q2s + q3s; - rpy[1] = RAD2DEG(asinf(-R13)); // pitch always between -pi/2 to pi/2 - rpy[2] = RAD2DEG(atan2f(R12, R11)); - rpy[0] = RAD2DEG(atan2f(R23, R33)); + rpy[1] = RAD2DEG(asinf(-R13)); // pitch always between -pi/2 to pi/2 + rpy[2] = RAD2DEG(atan2f(R12, R11)); + rpy[0] = RAD2DEG(atan2f(R23, R33)); - //TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2 + // TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2 } // ****** find quaternion from roll, pitch, yaw ******** void RPY2Quaternion(const float rpy[3], float q[4]) { - float phi, theta, psi; - float cphi, sphi, ctheta, stheta, cpsi, spsi; + float phi, theta, psi; + float cphi, sphi, ctheta, stheta, cpsi, spsi; - phi = DEG2RAD(rpy[0] / 2); - theta = DEG2RAD(rpy[1] / 2); - psi = DEG2RAD(rpy[2] / 2); - cphi = cosf(phi); - sphi = sinf(phi); - ctheta = cosf(theta); - stheta = sinf(theta); - cpsi = cosf(psi); - spsi = sinf(psi); + phi = DEG2RAD(rpy[0] / 2); + theta = DEG2RAD(rpy[1] / 2); + psi = DEG2RAD(rpy[2] / 2); + cphi = cosf(phi); + sphi = sinf(phi); + ctheta = cosf(theta); + stheta = sinf(theta); + cpsi = cosf(psi); + spsi = sinf(psi); - q[0] = cphi * ctheta * cpsi + sphi * stheta * spsi; - q[1] = sphi * ctheta * cpsi - cphi * stheta * spsi; - q[2] = cphi * stheta * cpsi + sphi * ctheta * spsi; - q[3] = cphi * ctheta * spsi - sphi * stheta * cpsi; + q[0] = cphi * ctheta * cpsi + sphi * stheta * spsi; + q[1] = sphi * ctheta * cpsi - cphi * stheta * spsi; + q[2] = cphi * stheta * cpsi + sphi * ctheta * spsi; + q[3] = cphi * ctheta * spsi - sphi * stheta * cpsi; - if (q[0] < 0) { // q0 always positive for uniqueness - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; - } + if (q[0] < 0) { // q0 always positive for uniqueness + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } } -//** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** void Quaternion2R(float q[4], float Rbe[3][3]) { + float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3]; - float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3]; - - Rbe[0][0] = q0s + q1s - q2s - q3s; - Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]); - Rbe[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]); - Rbe[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]); - Rbe[1][1] = q0s - q1s + q2s - q3s; - Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]); - Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]); - Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]); - Rbe[2][2] = q0s - q1s - q2s + q3s; + Rbe[0][0] = q0s + q1s - q2s - q3s; + Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]); + Rbe[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]); + Rbe[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]); + Rbe[1][1] = q0s - q1s + q2s - q3s; + Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]); + Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]); + Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]); + Rbe[2][2] = q0s - q1s - q2s + q3s; } // ****** Express LLA in a local NED Base Frame ******** void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3]) { - float ECEF[3]; - float diff[3]; + float ECEF[3]; + float diff[3]; - LLA2ECEF(LLA, ECEF); + LLA2ECEF(LLA, ECEF); - diff[0] = (float)(ECEF[0] - BaseECEF[0]); - diff[1] = (float)(ECEF[1] - BaseECEF[1]); - diff[2] = (float)(ECEF[2] - BaseECEF[2]); + diff[0] = (float)(ECEF[0] - BaseECEF[0]); + diff[1] = (float)(ECEF[1] - BaseECEF[1]); + diff[2] = (float)(ECEF[2] - BaseECEF[2]); - NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; - NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; - NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; + NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; + NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; + NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; } // ****** Express ECEF in a local NED Base Frame ******** void ECEF2Base(float ECEF[3], float BaseECEF[3], float Rne[3][3], float NED[3]) { - float diff[3]; + float diff[3]; - diff[0] = (float)(ECEF[0] - BaseECEF[0]); - diff[1] = (float)(ECEF[1] - BaseECEF[1]); - diff[2] = (float)(ECEF[2] - BaseECEF[2]); + diff[0] = (float)(ECEF[0] - BaseECEF[0]); + diff[1] = (float)(ECEF[1] - BaseECEF[1]); + diff[2] = (float)(ECEF[2] - BaseECEF[2]); - NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; - NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; - NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; + NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; + NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; + NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; } // ****** convert Rotation Matrix to Quaternion ******** // ****** if R converts from e to b, q is rotation from e to b **** void R2Quaternion(float R[3][3], float q[4]) { - float m[4], mag; - uint8_t index,i; + float m[4], mag; + uint8_t index, i; - m[0] = 1 + R[0][0] + R[1][1] + R[2][2]; - m[1] = 1 + R[0][0] - R[1][1] - R[2][2]; - m[2] = 1 - R[0][0] + R[1][1] - R[2][2]; - m[3] = 1 - R[0][0] - R[1][1] + R[2][2]; + m[0] = 1 + R[0][0] + R[1][1] + R[2][2]; + m[1] = 1 + R[0][0] - R[1][1] - R[2][2]; + m[2] = 1 - R[0][0] + R[1][1] - R[2][2]; + m[3] = 1 - R[0][0] - R[1][1] + R[2][2]; - // find maximum divisor - index = 0; - mag = m[0]; - for (i=1;i<4;i++){ - if (m[i] > mag){ - mag = m[i]; - index = i; - } - } - mag = 2*sqrtf(mag); + // find maximum divisor + index = 0; + mag = m[0]; + for (i = 1; i < 4; i++) { + if (m[i] > mag) { + mag = m[i]; + index = i; + } + } + mag = 2 * sqrtf(mag); - if (index == 0) { - q[0] = mag/4; - q[1] = (R[1][2]-R[2][1])/mag; - q[2] = (R[2][0]-R[0][2])/mag; - q[3] = (R[0][1]-R[1][0])/mag; - } - else if (index == 1) { - q[1] = mag/4; - q[0] = (R[1][2]-R[2][1])/mag; - q[2] = (R[0][1]+R[1][0])/mag; - q[3] = (R[0][2]+R[2][0])/mag; - } - else if (index == 2) { - q[2] = mag/4; - q[0] = (R[2][0]-R[0][2])/mag; - q[1] = (R[0][1]+R[1][0])/mag; - q[3] = (R[1][2]+R[2][1])/mag; - } - else { - q[3] = mag/4; - q[0] = (R[0][1]-R[1][0])/mag; - q[1] = (R[0][2]+R[2][0])/mag; - q[2] = (R[1][2]+R[2][1])/mag; - } + if (index == 0) { + q[0] = mag / 4; + q[1] = (R[1][2] - R[2][1]) / mag; + q[2] = (R[2][0] - R[0][2]) / mag; + q[3] = (R[0][1] - R[1][0]) / mag; + } else if (index == 1) { + q[1] = mag / 4; + q[0] = (R[1][2] - R[2][1]) / mag; + q[2] = (R[0][1] + R[1][0]) / mag; + q[3] = (R[0][2] + R[2][0]) / mag; + } else if (index == 2) { + q[2] = mag / 4; + q[0] = (R[2][0] - R[0][2]) / mag; + q[1] = (R[0][1] + R[1][0]) / mag; + q[3] = (R[1][2] + R[2][1]) / mag; + } else { + q[3] = mag / 4; + q[0] = (R[0][1] - R[1][0]) / mag; + q[1] = (R[0][2] + R[2][0]) / mag; + q[2] = (R[1][2] + R[2][1]) / mag; + } - // q0 positive, i.e. angle between pi and -pi - if (q[0] < 0){ - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; - } + // q0 positive, i.e. angle between pi and -pi + if (q[0] < 0) { + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } } // ****** Rotation Matrix from Two Vector Directions ******** @@ -280,111 +277,122 @@ void R2Quaternion(float R[3][3], float q[4]) // ****** solution is approximate if can't be exact *** uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[3], const float v2e[3], float Rbe[3][3]) { - float Rib[3][3], Rie[3][3]; - float mag; - uint8_t i,j,k; + float Rib[3][3], Rie[3][3]; + float mag; + uint8_t i, j, k; - // identity rotation in case of error - for (i=0;i<3;i++){ - for (j=0;j<3;j++) - Rbe[i][j]=0; - Rbe[i][i]=1; - } + // identity rotation in case of error + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + Rbe[i][j] = 0; + } + Rbe[i][i] = 1; + } - // The first rows of rot matrices chosen in direction of v1 - mag = VectorMagnitude(v1b); - if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) - return (-1); - for (i=0;i<3;i++) - Rib[0][i]=v1b[i]/mag; + // The first rows of rot matrices chosen in direction of v1 + mag = VectorMagnitude(v1b); + if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) { + return -1; + } + for (i = 0; i < 3; i++) { + Rib[0][i] = v1b[i] / mag; + } - mag = VectorMagnitude(v1e); - if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) - return (-1); - for (i=0;i<3;i++) - Rie[0][i]=v1e[i]/mag; + mag = VectorMagnitude(v1e); + if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) { + return -1; + } + for (i = 0; i < 3; i++) { + Rie[0][i] = v1e[i] / mag; + } - // The second rows of rot matrices chosen in direction of v1xv2 - CrossProduct(v1b,v2b,&Rib[1][0]); - mag = VectorMagnitude(&Rib[1][0]); - if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) - return (-1); - for (i=0;i<3;i++) - Rib[1][i]=Rib[1][i]/mag; + // The second rows of rot matrices chosen in direction of v1xv2 + CrossProduct(v1b, v2b, &Rib[1][0]); + mag = VectorMagnitude(&Rib[1][0]); + if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) { + return -1; + } + for (i = 0; i < 3; i++) { + Rib[1][i] = Rib[1][i] / mag; + } - CrossProduct(v1e,v2e,&Rie[1][0]); - mag = VectorMagnitude(&Rie[1][0]); - if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) - return (-1); - for (i=0;i<3;i++) - Rie[1][i]=Rie[1][i]/mag; + CrossProduct(v1e, v2e, &Rie[1][0]); + mag = VectorMagnitude(&Rie[1][0]); + if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) { + return -1; + } + for (i = 0; i < 3; i++) { + Rie[1][i] = Rie[1][i] / mag; + } - // The third rows of rot matrices are XxY (Row1xRow2) - CrossProduct(&Rib[0][0],&Rib[1][0],&Rib[2][0]); - CrossProduct(&Rie[0][0],&Rie[1][0],&Rie[2][0]); + // The third rows of rot matrices are XxY (Row1xRow2) + CrossProduct(&Rib[0][0], &Rib[1][0], &Rib[2][0]); + CrossProduct(&Rie[0][0], &Rie[1][0], &Rie[2][0]); - // Rbe = Rbi*Rie = Rib'*Rie - for (i=0;i<3;i++) - for(j=0;j<3;j++){ - Rbe[i][j]=0; - for(k=0;k<3;k++) - Rbe[i][j] += Rib[k][i]*Rie[k][j]; - } + // Rbe = Rbi*Rie = Rib'*Rie + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + Rbe[i][j] = 0; + for (k = 0; k < 3; k++) { + Rbe[i][j] += Rib[k][i] * Rie[k][j]; + } + } + } - return 1; + return 1; } void Rv2Rot(float Rv[3], float R[3][3]) { - // Compute rotation matrix from a rotation vector - // To save .text space, uses Quaternion2R() - float q[4]; + // Compute rotation matrix from a rotation vector + // To save .text space, uses Quaternion2R() + float q[4]; - float angle = VectorMagnitude(Rv); - if (angle <= 0.00048828125f) { - // angle < sqrt(2*machine_epsilon(float)), so flush cos(x) to 1.0f - q[0] = 1.0f; + float angle = VectorMagnitude(Rv); + + if (angle <= 0.00048828125f) { + // angle < sqrt(2*machine_epsilon(float)), so flush cos(x) to 1.0f + q[0] = 1.0f; // and flush sin(x/2)/x to 0.5 - q[1] = 0.5f*Rv[0]; - q[2] = 0.5f*Rv[1]; - q[3] = 0.5f*Rv[2]; - // This prevents division by zero, while retaining full accuracy - } - else { - q[0] = cosf(angle*0.5f); - float scale = sinf(angle*0.5f) / angle; - q[1] = scale*Rv[0]; - q[2] = scale*Rv[1]; - q[3] = scale*Rv[2]; - } + q[1] = 0.5f * Rv[0]; + q[2] = 0.5f * Rv[1]; + q[3] = 0.5f * Rv[2]; + // This prevents division by zero, while retaining full accuracy + } else { + q[0] = cosf(angle * 0.5f); + float scale = sinf(angle * 0.5f) / angle; + q[1] = scale * Rv[0]; + q[2] = scale * Rv[1]; + q[3] = scale * Rv[2]; + } - Quaternion2R(q, R); + Quaternion2R(q, R); } // ****** Vector Cross Product ******** void CrossProduct(const float v1[3], const float v2[3], float result[3]) { - result[0] = v1[1]*v2[2] - v2[1]*v1[2]; - result[1] = v2[0]*v1[2] - v1[0]*v2[2]; - result[2] = v1[0]*v2[1] - v2[0]*v1[1]; + result[0] = v1[1] * v2[2] - v2[1] * v1[2]; + result[1] = v2[0] * v1[2] - v1[0] * v2[2]; + result[2] = v1[0] * v2[1] - v2[0] * v1[1]; } // ****** Vector Magnitude ******** float VectorMagnitude(const float v[3]) { - return(sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2])); + return sqrtf(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); } /** - * @brief Compute the inverse of a quaternion + * @brief Compute the inverse of a quaternion * @param [in][out] q The matrix to invert */ -void quat_inverse(float q[4]) +void quat_inverse(float q[4]) { - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; } /** @@ -392,12 +400,12 @@ void quat_inverse(float q[4]) * @param[in] q quaternion in * @param[out] qnew quaternion to copy to */ -void quat_copy(const float q[4], float qnew[4]) +void quat_copy(const float q[4], float qnew[4]) { - qnew[0] = q[0]; - qnew[1] = q[1]; - qnew[2] = q[2]; - qnew[3] = q[3]; + qnew[0] = q[0]; + qnew[1] = q[1]; + qnew[2] = q[2]; + qnew[3] = q[3]; } /** @@ -406,12 +414,12 @@ void quat_copy(const float q[4], float qnew[4]) * @param[in] q2 Second quaternion * @param[out] qout Output quaternion */ -void quat_mult(const float q1[4], const float q2[4], float qout[4]) +void quat_mult(const float q1[4], const float q2[4], float qout[4]) { - qout[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3]; - qout[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2]; - qout[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1]; - qout[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0]; + qout[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; + qout[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; + qout[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]; + qout[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]; } /** @@ -420,9 +428,9 @@ void quat_mult(const float q1[4], const float q2[4], float qout[4]) * @param[in] vec the source vector * @param[out] vec_out the output vector */ -void rot_mult(float R[3][3], const float vec[3], float vec_out[3]) +void rot_mult(float R[3][3], const float vec[3], float vec_out[3]) { - vec_out[0] = R[0][0] * vec[0] + R[0][1] * vec[1] + R[0][2] * vec[2]; - vec_out[1] = R[1][0] * vec[0] + R[1][1] * vec[1] + R[1][2] * vec[2]; - vec_out[2] = R[2][0] * vec[0] + R[2][1] * vec[1] + R[2][2] * vec[2]; + vec_out[0] = R[0][0] * vec[0] + R[0][1] * vec[1] + R[0][2] * vec[2]; + vec_out[1] = R[1][0] * vec[0] + R[1][1] * vec[1] + R[1][2] * vec[2]; + vec_out[2] = R[2][0] * vec[0] + R[2][1] * vec[1] + R[2][2] * vec[2]; } diff --git a/flight/libraries/WorldMagModel.c b/flight/libraries/WorldMagModel.c index af50c8a19..fe55c3b27 100644 --- a/flight/libraries/WorldMagModel.c +++ b/flight/libraries/WorldMagModel.c @@ -53,108 +53,109 @@ #include "WMMInternal.h" #define MALLOC(x) pvPortMalloc(x) -#define FREE(x) vPortFree(x) -//#define MALLOC(x) malloc(x) -//#define FREE(x) free(x) +#define FREE(x) vPortFree(x) +// #define MALLOC(x) malloc(x) +// #define FREE(x) free(x) // http://reviews.openpilot.org/cru/OPReview-436#c6476 : // first column not used but it will be optimized out by compiler -static const float CoeffFile[91][6] = { {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, - {1.0f, 0.0f, -29496.6f, 0.0f, 11.6f, 0.0f}, - {1.0f, 1.0f, -1586.3f, 4944.4f, 16.5f, -25.9f}, - {2.0f, 0.0f, -2396.6f, 0.0f, -12.1f, 0.0f}, - {2.0f, 1.0f, 3026.1f, -2707.7f, -4.4f, -22.5f}, - {2.0f, 2.0f, 1668.6f, -576.1f, 1.9f, -11.8f}, - {3.0f, 0.0f, 1340.1f, 0.0f, 0.4f, 0.0f}, - {3.0f, 1.0f, -2326.2f, -160.2f, -4.1f, 7.3f}, - {3.0f, 2.0f, 1231.9f, 251.9f, -2.9f, -3.9f}, - {3.0f, 3.0f, 634.0f, -536.6f, -7.7f, -2.6f}, - {4.0f, 0.0f, 912.6f, 0.0f, -1.8f, 0.0f}, - {4.0f, 1.0f, 808.9f, 286.4f, 2.3f, 1.1f}, - {4.0f, 2.0f, 166.7f, -211.2f, -8.7f, 2.7f}, - {4.0f, 3.0f, -357.1f, 164.3f, 4.6f, 3.9f}, - {4.0f, 4.0f, 89.4f, -309.1f, -2.1f, -0.8f}, - {5.0f, 0.0f, -230.9f, 0.0f, -1.0f, 0.0f}, - {5.0f, 1.0f, 357.2f, 44.6f, 0.6f, 0.4f}, - {5.0f, 2.0f, 200.3f, 188.9f, -1.8f, 1.8f}, - {5.0f, 3.0f, -141.1f, -118.2f, -1.0f, 1.2f}, - {5.0f, 4.0f, -163.0f, 0.0f, 0.9f, 4.0f}, - {5.0f, 5.0f, -7.8f, 100.9f, 1.0f, -0.6f}, - {6.0f, 0.0f, 72.8f, 0.0f, -0.2f, 0.0f}, - {6.0f, 1.0f, 68.6f, -20.8f, -0.2f, -0.2f}, - {6.0f, 2.0f, 76.0f, 44.1f, -0.1f, -2.1f}, - {6.0f, 3.0f, -141.4f, 61.5f, 2.0f, -0.4f}, - {6.0f, 4.0f, -22.8f, -66.3f, -1.7f, -0.6f}, - {6.0f, 5.0f, 13.2f, 3.1f, -0.3f, 0.5f}, - {6.0f, 6.0f, -77.9f, 55.0f, 1.7f, 0.9f}, - {7.0f, 0.0f, 80.5f, 0.0f, 0.1f, 0.0f}, - {7.0f, 1.0f, -75.1f, -57.9f, -0.1f, 0.7f}, - {7.0f, 2.0f, -4.7f, -21.1f, -0.6f, 0.3f}, - {7.0f, 3.0f, 45.3f, 6.5f, 1.3f, -0.1f}, - {7.0f, 4.0f, 13.9f, 24.9f, 0.4f, -0.1f}, - {7.0f, 5.0f, 10.4f, 7.0f, 0.3f, -0.8f}, - {7.0f, 6.0f, 1.7f, -27.7f, -0.7f, -0.3f}, - {7.0f, 7.0f, 4.9f, -3.3f, 0.6f, 0.3f}, - {8.0f, 0.0f, 24.4f, 0.0f, -0.1f, 0.0f}, - {8.0f, 1.0f, 8.1f, 11.0f, 0.1f, -0.1f}, - {8.0f, 2.0f, -14.5f, -20.0f, -0.6f, 0.2f}, - {8.0f, 3.0f, -5.6f, 11.9f, 0.2f, 0.4f}, - {8.0f, 4.0f, -19.3f, -17.4f, -0.2f, 0.4f}, - {8.0f, 5.0f, 11.5f, 16.7f, 0.3f, 0.1f}, - {8.0f, 6.0f, 10.9f, 7.0f, 0.3f, -0.1f}, - {8.0f, 7.0f, -14.1f, -10.8f, -0.6f, 0.4f}, - {8.0f, 8.0f, -3.7f, 1.7f, 0.2f, 0.3f}, - {9.0f, 0.0f, 5.4f, 0.0f, 0.0f, 0.0f}, - {9.0f, 1.0f, 9.4f, -20.5f, -0.1f, 0.0f}, - {9.0f, 2.0f, 3.4f, 11.5f, 0.0f, -0.2f}, - {9.0f, 3.0f, -5.2f, 12.8f, 0.3f, 0.0f}, - {9.0f, 4.0f, 3.1f, -7.2f, -0.4f, -0.1f}, - {9.0f, 5.0f, -12.4f, -7.4f, -0.3f, 0.1f}, - {9.0f, 6.0f, -0.7f, 8.0f, 0.1f, 0.0f}, - {9.0f, 7.0f, 8.4f, 2.1f, -0.1f, -0.2f}, - {9.0f, 8.0f, -8.5f, -6.1f, -0.4f, 0.3f}, - {9.0f, 9.0f, -10.1f, 7.0f, -0.2f, 0.2f}, - {10.0f, 0.0f, -2.0f, 0.0f, 0.0f, 0.0f}, - {10.0f, 1.0f, -6.3f, 2.8f, 0.0f, 0.1f}, - {10.0f, 2.0f, 0.9f, -0.1f, -0.1f, -0.1f}, - {10.0f, 3.0f, -1.1f, 4.7f, 0.2f, 0.0f}, - {10.0f, 4.0f, -0.2f, 4.4f, 0.0f, -0.1f}, - {10.0f, 5.0f, 2.5f, -7.2f, -0.1f, -0.1f}, - {10.0f, 6.0f, -0.3f, -1.0f, -0.2f, 0.0f}, - {10.0f, 7.0f, 2.2f, -3.9f, 0.0f, -0.1f}, - {10.0f, 8.0f, 3.1f, -2.0f, -0.1f, -0.2f}, - {10.0f, 9.0f, -1.0f, -2.0f, -0.2f, 0.0f}, - {10.0f, 10.0f, -2.8f, -8.3f, -0.2f, -0.1f}, - {11.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f}, - {11.0f, 1.0f, -1.5f, 0.2f, 0.0f, 0.0f}, - {11.0f, 2.0f, -2.1f, 1.7f, 0.0f, 0.1f}, - {11.0f, 3.0f, 1.7f, -0.6f, 0.1f, 0.0f}, - {11.0f, 4.0f, -0.5f, -1.8f, 0.0f, 0.1f}, - {11.0f, 5.0f, 0.5f, 0.9f, 0.0f, 0.0f}, - {11.0f, 6.0f, -0.8f, -0.4f, 0.0f, 0.1f}, - {11.0f, 7.0f, 0.4f, -2.5f, 0.0f, 0.0f}, - {11.0f, 8.0f, 1.8f, -1.3f, 0.0f, -0.1f}, - {11.0f, 9.0f, 0.1f, -2.1f, 0.0f, -0.1f}, - {11.0f, 10.0f, 0.7f, -1.9f, -0.1f, 0.0f}, - {11.0f, 11.0f, 3.8f, -1.8f, 0.0f, -0.1f}, - {12.0f, 0.0f, -2.2f, 0.0f, 0.0f, 0.0f}, - {12.0f, 1.0f, -0.2f, -0.9f, 0.0f, 0.0f}, - {12.0f, 2.0f, 0.3f, 0.3f, 0.1f, 0.0f}, - {12.0f, 3.0f, 1.0f, 2.1f, 0.1f, 0.0f}, - {12.0f, 4.0f, -0.6f, -2.5f, -0.1f, 0.0f}, - {12.0f, 5.0f, 0.9f, 0.5f, 0.0f, 0.0f}, - {12.0f, 6.0f, -0.1f, 0.6f, 0.0f, 0.1f}, - {12.0f, 7.0f, 0.5f, 0.0f, 0.0f, 0.0f}, - {12.0f, 8.0f, -0.4f, 0.1f, 0.0f, 0.0f}, - {12.0f, 9.0f, -0.4f, 0.3f, 0.0f, 0.0f}, - {12.0f, 10.0f, 0.2f, -0.9f, 0.0f, 0.0f}, - {12.0f, 11.0f, -0.8f, -0.2f, -0.1f, 0.0f}, - {12.0f, 12.0f, 0.0f, 0.9f, 0.1f, 0.0f} +static const float CoeffFile[91][6] = { + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }, + { 1.0f, 0.0f, -29496.6f, 0.0f, 11.6f, 0.0f }, + { 1.0f, 1.0f, -1586.3f, 4944.4f, 16.5f, -25.9f }, + { 2.0f, 0.0f, -2396.6f, 0.0f, -12.1f, 0.0f }, + { 2.0f, 1.0f, 3026.1f, -2707.7f, -4.4f, -22.5f }, + { 2.0f, 2.0f, 1668.6f, -576.1f, 1.9f, -11.8f }, + { 3.0f, 0.0f, 1340.1f, 0.0f, 0.4f, 0.0f }, + { 3.0f, 1.0f, -2326.2f, -160.2f, -4.1f, 7.3f }, + { 3.0f, 2.0f, 1231.9f, 251.9f, -2.9f, -3.9f }, + { 3.0f, 3.0f, 634.0f, -536.6f, -7.7f, -2.6f }, + { 4.0f, 0.0f, 912.6f, 0.0f, -1.8f, 0.0f }, + { 4.0f, 1.0f, 808.9f, 286.4f, 2.3f, 1.1f }, + { 4.0f, 2.0f, 166.7f, -211.2f, -8.7f, 2.7f }, + { 4.0f, 3.0f, -357.1f, 164.3f, 4.6f, 3.9f }, + { 4.0f, 4.0f, 89.4f, -309.1f, -2.1f, -0.8f }, + { 5.0f, 0.0f, -230.9f, 0.0f, -1.0f, 0.0f }, + { 5.0f, 1.0f, 357.2f, 44.6f, 0.6f, 0.4f }, + { 5.0f, 2.0f, 200.3f, 188.9f, -1.8f, 1.8f }, + { 5.0f, 3.0f, -141.1f, -118.2f, -1.0f, 1.2f }, + { 5.0f, 4.0f, -163.0f, 0.0f, 0.9f, 4.0f }, + { 5.0f, 5.0f, -7.8f, 100.9f, 1.0f, -0.6f }, + { 6.0f, 0.0f, 72.8f, 0.0f, -0.2f, 0.0f }, + { 6.0f, 1.0f, 68.6f, -20.8f, -0.2f, -0.2f }, + { 6.0f, 2.0f, 76.0f, 44.1f, -0.1f, -2.1f }, + { 6.0f, 3.0f, -141.4f, 61.5f, 2.0f, -0.4f }, + { 6.0f, 4.0f, -22.8f, -66.3f, -1.7f, -0.6f }, + { 6.0f, 5.0f, 13.2f, 3.1f, -0.3f, 0.5f }, + { 6.0f, 6.0f, -77.9f, 55.0f, 1.7f, 0.9f }, + { 7.0f, 0.0f, 80.5f, 0.0f, 0.1f, 0.0f }, + { 7.0f, 1.0f, -75.1f, -57.9f, -0.1f, 0.7f }, + { 7.0f, 2.0f, -4.7f, -21.1f, -0.6f, 0.3f }, + { 7.0f, 3.0f, 45.3f, 6.5f, 1.3f, -0.1f }, + { 7.0f, 4.0f, 13.9f, 24.9f, 0.4f, -0.1f }, + { 7.0f, 5.0f, 10.4f, 7.0f, 0.3f, -0.8f }, + { 7.0f, 6.0f, 1.7f, -27.7f, -0.7f, -0.3f }, + { 7.0f, 7.0f, 4.9f, -3.3f, 0.6f, 0.3f }, + { 8.0f, 0.0f, 24.4f, 0.0f, -0.1f, 0.0f }, + { 8.0f, 1.0f, 8.1f, 11.0f, 0.1f, -0.1f }, + { 8.0f, 2.0f, -14.5f, -20.0f, -0.6f, 0.2f }, + { 8.0f, 3.0f, -5.6f, 11.9f, 0.2f, 0.4f }, + { 8.0f, 4.0f, -19.3f, -17.4f, -0.2f, 0.4f }, + { 8.0f, 5.0f, 11.5f, 16.7f, 0.3f, 0.1f }, + { 8.0f, 6.0f, 10.9f, 7.0f, 0.3f, -0.1f }, + { 8.0f, 7.0f, -14.1f, -10.8f, -0.6f, 0.4f }, + { 8.0f, 8.0f, -3.7f, 1.7f, 0.2f, 0.3f }, + { 9.0f, 0.0f, 5.4f, 0.0f, 0.0f, 0.0f }, + { 9.0f, 1.0f, 9.4f, -20.5f, -0.1f, 0.0f }, + { 9.0f, 2.0f, 3.4f, 11.5f, 0.0f, -0.2f }, + { 9.0f, 3.0f, -5.2f, 12.8f, 0.3f, 0.0f }, + { 9.0f, 4.0f, 3.1f, -7.2f, -0.4f, -0.1f }, + { 9.0f, 5.0f, -12.4f, -7.4f, -0.3f, 0.1f }, + { 9.0f, 6.0f, -0.7f, 8.0f, 0.1f, 0.0f }, + { 9.0f, 7.0f, 8.4f, 2.1f, -0.1f, -0.2f }, + { 9.0f, 8.0f, -8.5f, -6.1f, -0.4f, 0.3f }, + { 9.0f, 9.0f, -10.1f, 7.0f, -0.2f, 0.2f }, + { 10.0f, 0.0f, -2.0f, 0.0f, 0.0f, 0.0f }, + { 10.0f, 1.0f, -6.3f, 2.8f, 0.0f, 0.1f }, + { 10.0f, 2.0f, 0.9f, -0.1f, -0.1f, -0.1f }, + { 10.0f, 3.0f, -1.1f, 4.7f, 0.2f, 0.0f }, + { 10.0f, 4.0f, -0.2f, 4.4f, 0.0f, -0.1f }, + { 10.0f, 5.0f, 2.5f, -7.2f, -0.1f, -0.1f }, + { 10.0f, 6.0f, -0.3f, -1.0f, -0.2f, 0.0f }, + { 10.0f, 7.0f, 2.2f, -3.9f, 0.0f, -0.1f }, + { 10.0f, 8.0f, 3.1f, -2.0f, -0.1f, -0.2f }, + { 10.0f, 9.0f, -1.0f, -2.0f, -0.2f, 0.0f }, + { 10.0f, 10.0f, -2.8f, -8.3f, -0.2f, -0.1f }, + { 11.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f }, + { 11.0f, 1.0f, -1.5f, 0.2f, 0.0f, 0.0f }, + { 11.0f, 2.0f, -2.1f, 1.7f, 0.0f, 0.1f }, + { 11.0f, 3.0f, 1.7f, -0.6f, 0.1f, 0.0f }, + { 11.0f, 4.0f, -0.5f, -1.8f, 0.0f, 0.1f }, + { 11.0f, 5.0f, 0.5f, 0.9f, 0.0f, 0.0f }, + { 11.0f, 6.0f, -0.8f, -0.4f, 0.0f, 0.1f }, + { 11.0f, 7.0f, 0.4f, -2.5f, 0.0f, 0.0f }, + { 11.0f, 8.0f, 1.8f, -1.3f, 0.0f, -0.1f }, + { 11.0f, 9.0f, 0.1f, -2.1f, 0.0f, -0.1f }, + { 11.0f, 10.0f, 0.7f, -1.9f, -0.1f, 0.0f }, + { 11.0f, 11.0f, 3.8f, -1.8f, 0.0f, -0.1f }, + { 12.0f, 0.0f, -2.2f, 0.0f, 0.0f, 0.0f }, + { 12.0f, 1.0f, -0.2f, -0.9f, 0.0f, 0.0f }, + { 12.0f, 2.0f, 0.3f, 0.3f, 0.1f, 0.0f }, + { 12.0f, 3.0f, 1.0f, 2.1f, 0.1f, 0.0f }, + { 12.0f, 4.0f, -0.6f, -2.5f, -0.1f, 0.0f }, + { 12.0f, 5.0f, 0.9f, 0.5f, 0.0f, 0.0f }, + { 12.0f, 6.0f, -0.1f, 0.6f, 0.0f, 0.1f }, + { 12.0f, 7.0f, 0.5f, 0.0f, 0.0f, 0.0f }, + { 12.0f, 8.0f, -0.4f, 0.1f, 0.0f, 0.0f }, + { 12.0f, 9.0f, -0.4f, 0.3f, 0.0f, 0.0f }, + { 12.0f, 10.0f, 0.2f, -0.9f, 0.0f, 0.0f }, + { 12.0f, 11.0f, -0.8f, -0.2f, -0.1f, 0.0f }, + { 12.0f, 12.0f, 0.0f, 0.9f, 0.1f, 0.0f } }; -static WMMtype_Ellipsoid *Ellip = NULL; -static WMMtype_MagneticModel *MagneticModel = NULL; -static float decimal_date; +static WMMtype_Ellipsoid *Ellip = NULL; +static WMMtype_MagneticModel *MagneticModel = NULL; +static float decimal_date; /************************************************************************************** * Example use - very simple - only two exposed functions @@ -168,31 +169,34 @@ static float decimal_date; **************************************************************************************/ int WMM_Initialize() -// Sets default values for WMM subroutines. -// UPDATES : Ellip and MagneticModel -{ - if (!Ellip) return -1; // invalid pointer - if (!MagneticModel) return -2; // invalid pointer - - // Sets WGS-84 parameters - Ellip->a = 6378.137f; // semi-major axis of the ellipsoid in km - Ellip->b = 6356.7523142f; // semi-minor axis of the ellipsoid in km - Ellip->fla = 1.0f / 298.257223563f; // flattening - Ellip->eps = sqrt(1 - (Ellip->b * Ellip->b) / (Ellip->a * Ellip->a)); // first eccentricity - Ellip->epssq = (Ellip->eps * Ellip->eps); // first eccentricity squared - Ellip->re = 6371.2f; // Earth's radius in km +// Sets default values for WMM subroutines. +// UPDATES : Ellip and MagneticModel +{ + if (!Ellip) { + return -1; // invalid pointer + } + if (!MagneticModel) { + return -2; // invalid pointer + } + // Sets WGS-84 parameters + Ellip->a = 6378.137f; // semi-major axis of the ellipsoid in km + Ellip->b = 6356.7523142f; // semi-minor axis of the ellipsoid in km + Ellip->fla = 1.0f / 298.257223563f; // flattening + Ellip->eps = sqrt(1 - (Ellip->b * Ellip->b) / (Ellip->a * Ellip->a)); // first eccentricity + Ellip->epssq = (Ellip->eps * Ellip->eps); // first eccentricity squared + Ellip->re = 6371.2f; // Earth's radius in km - // Sets Magnetic Model parameters - MagneticModel->nMax = WMM_MAX_MODEL_DEGREES; - MagneticModel->nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES; - MagneticModel->SecularVariationUsed = 0; + // Sets Magnetic Model parameters + MagneticModel->nMax = WMM_MAX_MODEL_DEGREES; + MagneticModel->nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES; + MagneticModel->SecularVariationUsed = 0; - // Really, Really needs to be read from a file - out of date in 2015 at latest - MagneticModel->EditionDate = 0.0f; /* OP change. Originally 5.7863328170559505e-307, truncates to 0.0f */ - MagneticModel->epoch = 2010.0f; - sprintf(MagneticModel->ModelName, "WMM-2010"); + // Really, Really needs to be read from a file - out of date in 2015 at latest + MagneticModel->EditionDate = 0.0f; /* OP change. Originally 5.7863328170559505e-307, truncates to 0.0f */ + MagneticModel->epoch = 2010.0f; + sprintf(MagneticModel->ModelName, "WMM-2010"); - return 0; // OK + return 0; // OK } int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, uint16_t Day, uint16_t Year, float B[3]) @@ -200,206 +204,213 @@ int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, u // return '0' if all appears to be OK // return < 0 if error - int returned = 0; // default to OK + int returned = 0; // default to OK // *********** // range check supplied params - if (Lat < -90.0f) return -1; // error - if (Lat > 90.0f) return -2; // error - - if (Lon < -180.0f) return -3; // error - if (Lon > 180.0f) return -4; // error - + if (Lat < -90.0f) { + return -1; // error + } + if (Lat > 90.0f) { + return -2; // error + } + if (Lon < -180.0f) { + return -3; // error + } + if (Lon > 180.0f) { + return -4; // error + } // *********** // allocated required memory -// Ellip = NULL; -// MagneticModel = NULL; +// Ellip = NULL; +// MagneticModel = NULL; -// MagneticModel = NULL; -// CoordGeodetic = NULL; -// GeoMagneticElements = NULL; +// MagneticModel = NULL; +// CoordGeodetic = NULL; +// GeoMagneticElements = NULL; - Ellip = (WMMtype_Ellipsoid *) MALLOC(sizeof(WMMtype_Ellipsoid)); - MagneticModel = (WMMtype_MagneticModel *) MALLOC(sizeof(WMMtype_MagneticModel)); + Ellip = (WMMtype_Ellipsoid *)MALLOC(sizeof(WMMtype_Ellipsoid)); + MagneticModel = (WMMtype_MagneticModel *)MALLOC(sizeof(WMMtype_MagneticModel)); - WMMtype_CoordSpherical *CoordSpherical = (WMMtype_CoordSpherical *) MALLOC(sizeof(WMMtype_CoordSpherical)); - WMMtype_CoordGeodetic *CoordGeodetic = (WMMtype_CoordGeodetic *) MALLOC(sizeof(WMMtype_CoordGeodetic)); - WMMtype_GeoMagneticElements *GeoMagneticElements = (WMMtype_GeoMagneticElements *) MALLOC(sizeof(WMMtype_GeoMagneticElements)); - - if (!Ellip || !MagneticModel || !CoordSpherical || !CoordGeodetic || !GeoMagneticElements) - returned = -5; // error + WMMtype_CoordSpherical *CoordSpherical = (WMMtype_CoordSpherical *)MALLOC(sizeof(WMMtype_CoordSpherical)); + WMMtype_CoordGeodetic *CoordGeodetic = (WMMtype_CoordGeodetic *)MALLOC(sizeof(WMMtype_CoordGeodetic)); + WMMtype_GeoMagneticElements *GeoMagneticElements = (WMMtype_GeoMagneticElements *)MALLOC(sizeof(WMMtype_GeoMagneticElements)); + if (!Ellip || !MagneticModel || !CoordSpherical || !CoordGeodetic || !GeoMagneticElements) { + returned = -5; // error + } // *********** - if (returned >= 0) - { - if (WMM_Initialize() < 0) - returned = -6; // error + if (returned >= 0) { + if (WMM_Initialize() < 0) { + returned = -6; // error + } } - if (returned >= 0) - { + if (returned >= 0) { CoordGeodetic->lambda = Lon; - CoordGeodetic->phi = Lat; - CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid/1000.0f; // convert to km + CoordGeodetic->phi = Lat; + CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid / 1000.0f; // convert to km // Convert from geodetic to Spherical Equations: 17-18, WMM Technical report - if (WMM_GeodeticToSpherical(CoordGeodetic, CoordSpherical) < 0) - returned = -7; // error + if (WMM_GeodeticToSpherical(CoordGeodetic, CoordSpherical) < 0) { + returned = -7; // error + } } - if (returned >= 0) - { - if (WMM_DateToYear(Month, Day, Year) < 0) - returned = -8; // error + if (returned >= 0) { + if (WMM_DateToYear(Month, Day, Year) < 0) { + returned = -8; // error + } } - if (returned >= 0) - { + if (returned >= 0) { // Compute the geoMagnetic field elements and their time change - if (WMM_Geomag(CoordSpherical, CoordGeodetic, GeoMagneticElements) < 0) - returned = -9; // error - else - { // set the returned values + if (WMM_Geomag(CoordSpherical, CoordGeodetic, GeoMagneticElements) < 0) { + returned = -9; // error + } else { // set the returned values B[0] = GeoMagneticElements->X; B[1] = GeoMagneticElements->Y; B[2] = GeoMagneticElements->Z; } } - // *********** - // free allocated memory + // *********** + // free allocated memory - if (GeoMagneticElements) + if (GeoMagneticElements) { FREE(GeoMagneticElements); - - if (CoordGeodetic) - FREE(CoordGeodetic); - - if (CoordSpherical) - FREE(CoordSpherical); - - if (MagneticModel) - { - FREE(MagneticModel); - MagneticModel = NULL; } - if (Ellip) - { + if (CoordGeodetic) { + FREE(CoordGeodetic); + } + + if (CoordSpherical) { + FREE(CoordSpherical); + } + + if (MagneticModel) { + FREE(MagneticModel); + MagneticModel = NULL; + } + + if (Ellip) { FREE(Ellip); Ellip = NULL; } - B[0] = GeoMagneticElements->X * 1e-2f; - B[1] = GeoMagneticElements->Y * 1e-2f; - B[2] = GeoMagneticElements->Z * 1e-2f; + B[0] = GeoMagneticElements->X * 1e-2f; + B[1] = GeoMagneticElements->Y * 1e-2f; + B[2] = GeoMagneticElements->Z * 1e-2f; return returned; } -int WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical, WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_GeoMagneticElements * GeoMagneticElements) - /* - The main subroutine that calls a sequence of WMM sub-functions to calculate the magnetic field elements for a single point. - The function expects the model coefficients and point coordinates as input and returns the magnetic field elements and - their rate of change. Though, this subroutine can be called successively to calculate a time series, profile or grid - of magnetic field, these are better achieved by the subroutine WMM_Grid. +int WMM_Geomag(WMMtype_CoordSpherical *CoordSpherical, WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_GeoMagneticElements *GeoMagneticElements) +/* + The main subroutine that calls a sequence of WMM sub-functions to calculate the magnetic field elements for a single point. + The function expects the model coefficients and point coordinates as input and returns the magnetic field elements and + their rate of change. Though, this subroutine can be called successively to calculate a time series, profile or grid + of magnetic field, these are better achieved by the subroutine WMM_Grid. - INPUT: Ellip - CoordSpherical - CoordGeodetic - TimedMagneticModel + INPUT: Ellip + CoordSpherical + CoordGeodetic + TimedMagneticModel - OUTPUT : GeoMagneticElements + OUTPUT : GeoMagneticElements - CALLS: WMM_ComputeSphericalHarmonicVariables( Ellip, CoordSpherical, TimedMagneticModel->nMax, &SphVariables); (Compute Spherical Harmonic variables ) - WMM_AssociatedLegendreFunction(CoordSpherical, TimedMagneticModel->nMax, LegendreFunction); Compute ALF - WMM_Summation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSph); Accumulate the spherical harmonic coefficients - WMM_SecVarSummation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSphVar); Sum the Secular Variation Coefficients - WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSph, &MagneticResultsGeo); Map the computed Magnetic fields to Geodeitic coordinates - WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSphVar, &MagneticResultsGeoVar); Map the secular variation field components to Geodetic coordinates - WMM_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); Calculate the Geomagnetic elements - WMM_CalculateSecularVariation(MagneticResultsGeoVar, GeoMagneticElements); Calculate the secular variation of each of the Geomagnetic elements + CALLS: WMM_ComputeSphericalHarmonicVariables( Ellip, CoordSpherical, TimedMagneticModel->nMax, &SphVariables); (Compute Spherical Harmonic variables ) + WMM_AssociatedLegendreFunction(CoordSpherical, TimedMagneticModel->nMax, LegendreFunction); Compute ALF + WMM_Summation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSph); Accumulate the spherical harmonic coefficients + WMM_SecVarSummation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSphVar); Sum the Secular Variation Coefficients + WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSph, &MagneticResultsGeo); Map the computed Magnetic fields to Geodeitic coordinates + WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSphVar, &MagneticResultsGeoVar); Map the secular variation field components to Geodetic coordinates + WMM_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); Calculate the Geomagnetic elements + WMM_CalculateSecularVariation(MagneticResultsGeoVar, GeoMagneticElements); Calculate the secular variation of each of the Geomagnetic elements - */ + */ { - int returned = 0; // default to OK + int returned = 0; // default to OK - WMMtype_MagneticResults MagneticResultsSph; - WMMtype_MagneticResults MagneticResultsGeo; - WMMtype_MagneticResults MagneticResultsSphVar; - WMMtype_MagneticResults MagneticResultsGeoVar; + WMMtype_MagneticResults MagneticResultsSph; + WMMtype_MagneticResults MagneticResultsGeo; + WMMtype_MagneticResults MagneticResultsSphVar; + WMMtype_MagneticResults MagneticResultsGeoVar; // ******** // allocate required memory - WMMtype_LegendreFunction *LegendreFunction = (WMMtype_LegendreFunction *) MALLOC(sizeof(WMMtype_LegendreFunction)); - WMMtype_SphericalHarmonicVariables *SphVariables = (WMMtype_SphericalHarmonicVariables *) MALLOC(sizeof(WMMtype_SphericalHarmonicVariables)); - - if (!LegendreFunction || !SphVariables) - returned = -1; // memory allocation error + WMMtype_LegendreFunction *LegendreFunction = (WMMtype_LegendreFunction *)MALLOC(sizeof(WMMtype_LegendreFunction)); + WMMtype_SphericalHarmonicVariables *SphVariables = (WMMtype_SphericalHarmonicVariables *)MALLOC(sizeof(WMMtype_SphericalHarmonicVariables)); + if (!LegendreFunction || !SphVariables) { + returned = -1; // memory allocation error + } // ******** - if (returned >= 0) - { // Compute Spherical Harmonic variables - if (WMM_ComputeSphericalHarmonicVariables(CoordSpherical, MagneticModel->nMax, SphVariables) < 0) - returned = -2; // error + if (returned >= 0) { // Compute Spherical Harmonic variables + if (WMM_ComputeSphericalHarmonicVariables(CoordSpherical, MagneticModel->nMax, SphVariables) < 0) { + returned = -2; // error + } } - if (returned >= 0) - { // Compute ALF - if (WMM_AssociatedLegendreFunction(CoordSpherical, MagneticModel->nMax, LegendreFunction) < 0) - returned = -3; // error + if (returned >= 0) { // Compute ALF + if (WMM_AssociatedLegendreFunction(CoordSpherical, MagneticModel->nMax, LegendreFunction) < 0) { + returned = -3; // error + } } - if (returned >= 0) - { // Accumulate the spherical harmonic coefficients - if (WMM_Summation(LegendreFunction, SphVariables, CoordSpherical, &MagneticResultsSph) < 0) - returned = -4; // error + if (returned >= 0) { // Accumulate the spherical harmonic coefficients + if (WMM_Summation(LegendreFunction, SphVariables, CoordSpherical, &MagneticResultsSph) < 0) { + returned = -4; // error + } } - if (returned >= 0) - { // Sum the Secular Variation Coefficients - if (WMM_SecVarSummation(LegendreFunction, SphVariables, CoordSpherical, &MagneticResultsSphVar) < 0) - returned = -5; // error + if (returned >= 0) { // Sum the Secular Variation Coefficients + if (WMM_SecVarSummation(LegendreFunction, SphVariables, CoordSpherical, &MagneticResultsSphVar) < 0) { + returned = -5; // error + } } - if (returned >= 0) - { // Map the computed Magnetic fields to Geodeitic coordinates - if (WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSph, &MagneticResultsGeo) < 0) - returned = -6; // error + if (returned >= 0) { // Map the computed Magnetic fields to Geodeitic coordinates + if (WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSph, &MagneticResultsGeo) < 0) { + returned = -6; // error + } } - if (returned >= 0) - { // Map the secular variation field components to Geodetic coordinates - if (WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSphVar, &MagneticResultsGeoVar) < 0) - returned = -7; // error + if (returned >= 0) { // Map the secular variation field components to Geodetic coordinates + if (WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSphVar, &MagneticResultsGeoVar) < 0) { + returned = -7; // error + } } - if (returned >= 0) - { // Calculate the Geomagnetic elements, Equation 18 , WMM Technical report - if (WMM_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements) < 0) - returned = -8; // error + if (returned >= 0) { // Calculate the Geomagnetic elements, Equation 18 , WMM Technical report + if (WMM_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements) < 0) { + returned = -8; // error + } } - if (returned >= 0) - { // Calculate the secular variation of each of the Geomagnetic elements - if (WMM_CalculateSecularVariation(&MagneticResultsGeoVar, GeoMagneticElements) < 0) - returned = -9; // error + if (returned >= 0) { // Calculate the secular variation of each of the Geomagnetic elements + if (WMM_CalculateSecularVariation(&MagneticResultsGeoVar, GeoMagneticElements) < 0) { + returned = -9; // error + } } // ******** // free allocated memory - if (SphVariables) + if (SphVariables) { FREE(SphVariables); + } - if (LegendreFunction) + if (LegendreFunction) { FREE(LegendreFunction); + } // ******** @@ -407,425 +418,416 @@ int WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical, WMMtype_CoordGeodetic * } int WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical, uint16_t nMax, WMMtype_SphericalHarmonicVariables *SphVariables) +/* Computes Spherical variables + Variables computed are (a/r)^(n+2), cos_m(lamda) and sin_m(lambda) for spherical harmonic + summations. (Equations 10-12 in the WMM Technical Report) + INPUT Ellip data structure with the following elements + float a; semi-major axis of the ellipsoid + float b; semi-minor axis of the ellipsoid + float fla; flattening + float epssq; first eccentricity squared + float eps; first eccentricity + float re; mean radius of ellipsoid + CoordSpherical A data structure with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) + nMax integer ( Maxumum degree of spherical harmonic secular model)\ - /* Computes Spherical variables - Variables computed are (a/r)^(n+2), cos_m(lamda) and sin_m(lambda) for spherical harmonic - summations. (Equations 10-12 in the WMM Technical Report) - INPUT Ellip data structure with the following elements - float a; semi-major axis of the ellipsoid - float b; semi-minor axis of the ellipsoid - float fla; flattening - float epssq; first eccentricity squared - float eps; first eccentricity - float re; mean radius of ellipsoid - CoordSpherical A data structure with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) - nMax integer ( Maxumum degree of spherical harmonic secular model)\ - - OUTPUT SphVariables Pointer to the data structure with the following elements - float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES+1]; [earth_reference_radius_km sph. radius ]^n - float cos_mlambda[WMM_MAX_MODEL_DEGREES+1]; cp(m) - cosine of (mspherical coord. longitude) - float sin_mlambda[WMM_MAX_MODEL_DEGREES+1]; sp(m) - sine of (mspherical coord. longitude) - CALLS : none - */ + OUTPUT SphVariables Pointer to the data structure with the following elements + float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES+1]; [earth_reference_radius_km sph. radius ]^n + float cos_mlambda[WMM_MAX_MODEL_DEGREES+1]; cp(m) - cosine of (mspherical coord. longitude) + float sin_mlambda[WMM_MAX_MODEL_DEGREES+1]; sp(m) - sine of (mspherical coord. longitude) + CALLS : none + */ { - float cos_lambda, sin_lambda; - uint16_t m, n; + float cos_lambda, sin_lambda; + uint16_t m, n; - cos_lambda = cosf(DEG2RAD(CoordSpherical->lambda)); - sin_lambda = sinf(DEG2RAD(CoordSpherical->lambda)); + cos_lambda = cosf(DEG2RAD(CoordSpherical->lambda)); + sin_lambda = sinf(DEG2RAD(CoordSpherical->lambda)); - /* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2) - for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ + /* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2) + for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ - SphVariables->RelativeRadiusPower[0] = (Ellip->re / CoordSpherical->r) * (Ellip->re / CoordSpherical->r); - for (n = 1; n <= nMax; n++) - SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n - 1] * (Ellip->re / CoordSpherical->r); + SphVariables->RelativeRadiusPower[0] = (Ellip->re / CoordSpherical->r) * (Ellip->re / CoordSpherical->r); + for (n = 1; n <= nMax; n++) { + SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n - 1] * (Ellip->re / CoordSpherical->r); + } - /* - Compute cosf(m*lambda), sinf(m*lambda) for m = 0 ... nMax - cosf(a + b) = cosf(a)*cosf(b) - sinf(a)*sinf(b) - sinf(a + b) = cosf(a)*sinf(b) + sinf(a)*cosf(b) - */ - SphVariables->cos_mlambda[0] = 1.0f; - SphVariables->sin_mlambda[0] = 0.0f; + /* + Compute cosf(m*lambda), sinf(m*lambda) for m = 0 ... nMax + cosf(a + b) = cosf(a)*cosf(b) - sinf(a)*sinf(b) + sinf(a + b) = cosf(a)*sinf(b) + sinf(a)*cosf(b) + */ + SphVariables->cos_mlambda[0] = 1.0f; + SphVariables->sin_mlambda[0] = 0.0f; - SphVariables->cos_mlambda[1] = cos_lambda; - SphVariables->sin_mlambda[1] = sin_lambda; - for (m = 2; m <= nMax; m++) - { - SphVariables->cos_mlambda[m] = SphVariables->cos_mlambda[m - 1] * cos_lambda - SphVariables->sin_mlambda[m - 1] * sin_lambda; - SphVariables->sin_mlambda[m] = SphVariables->cos_mlambda[m - 1] * sin_lambda + SphVariables->sin_mlambda[m - 1] * cos_lambda; - } + SphVariables->cos_mlambda[1] = cos_lambda; + SphVariables->sin_mlambda[1] = sin_lambda; + for (m = 2; m <= nMax; m++) { + SphVariables->cos_mlambda[m] = SphVariables->cos_mlambda[m - 1] * cos_lambda - SphVariables->sin_mlambda[m - 1] * sin_lambda; + SphVariables->sin_mlambda[m] = SphVariables->cos_mlambda[m - 1] * sin_lambda + SphVariables->sin_mlambda[m - 1] * cos_lambda; + } - return 0; // OK + return 0; // OK } -int WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical * CoordSpherical, uint16_t nMax, WMMtype_LegendreFunction * LegendreFunction) +int WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical *CoordSpherical, uint16_t nMax, WMMtype_LegendreFunction *LegendreFunction) +/* Computes all of the Schmidt-semi normalized associated Legendre + functions up to degree nMax. If nMax <= 16, function WMM_PcupLow is used. + Otherwise WMM_PcupHigh is called. + INPUT CoordSpherical A data structure with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) + nMax integer ( Maxumum degree of spherical harmonic secular model) + LegendreFunction Pointer to data structure with the following elements + float *Pcup; ( pointer to store Legendre Function ) + float *dPcup; ( pointer to store Derivative of Lagendre function ) - /* Computes all of the Schmidt-semi normalized associated Legendre - functions up to degree nMax. If nMax <= 16, function WMM_PcupLow is used. - Otherwise WMM_PcupHigh is called. - INPUT CoordSpherical A data structure with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) - nMax integer ( Maxumum degree of spherical harmonic secular model) - LegendreFunction Pointer to data structure with the following elements - float *Pcup; ( pointer to store Legendre Function ) - float *dPcup; ( pointer to store Derivative of Lagendre function ) + OUTPUT LegendreFunction Calculated Legendre variables in the data structure - OUTPUT LegendreFunction Calculated Legendre variables in the data structure - - */ + */ { - float sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); /* sinf (geocentric latitude) */ + float sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); /* sinf (geocentric latitude) */ - if (nMax <= 16 || (1 - fabsf(sin_phi)) < 1.0e-10f) /* If nMax is less tha 16 or at the poles */ - { - if (WMM_PcupLow(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0) - return -1; // error - } - else - { - if (WMM_PcupHigh(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0) - return -2; // error - } + if (nMax <= 16 || (1 - fabsf(sin_phi)) < 1.0e-10f) { /* If nMax is less tha 16 or at the poles */ + if (WMM_PcupLow(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0) { + return -1; // error + } + } else { + if (WMM_PcupHigh(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0) { + return -2; // error + } + } - return 0; // OK + return 0; // OK } -int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction, - WMMtype_SphericalHarmonicVariables * SphVariables, - WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults) +int WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables *SphVariables, + WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) { - /* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using - spherical harmonic summation. + /* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using + spherical harmonic summation. - The vector Magnetic field is given by -grad V, where V is Geomagnetic scalar potential - The gradient in spherical coordinates is given by: + The vector Magnetic field is given by -grad V, where V is Geomagnetic scalar potential + The gradient in spherical coordinates is given by: - dV ^ 1 dV ^ 1 dV ^ - grad V = -- r + - -- t + -------- -- p - dr r dt r sinf(t) dp + dV ^ 1 dV ^ 1 dV ^ + grad V = -- r + - -- t + -------- -- p + dr r dt r sinf(t) dp - INPUT : LegendreFunction - MagneticModel - SphVariables - CoordSpherical - OUTPUT : MagneticResults + INPUT : LegendreFunction + MagneticModel + SphVariables + CoordSpherical + OUTPUT : MagneticResults - CALLS : WMM_SummationSpecial + CALLS : WMM_SummationSpecial - Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov - */ + Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov + */ uint16_t m, n, index; - float cos_phi; + float cos_phi; - MagneticResults->Bz = 0.0f; - MagneticResults->By = 0.0f; - MagneticResults->Bx = 0.0f; + MagneticResults->Bz = 0.0f; + MagneticResults->By = 0.0f; + MagneticResults->Bx = 0.0f; - for (n = 1; n <= MagneticModel->nMax; n++) - { - for (m = 0; m <= n; m++) - { - index = (n * (n + 1) / 2 + m); + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); -/* nMax (n+2) n m m m - Bz = -SUM (a/r) (n+1) SUM [g cosf(m p) + h sinf(m p)] P (sinf(phi)) - n=1 m=0 n n n */ +/* nMax (n+2) n m m m + Bz = -SUM (a/r) (n+1) SUM [g cosf(m p) + h sinf(m p)] P (sinf(phi)) + n=1 m=0 n n n */ /* Equation 12 in the WMM Technical report. Derivative with respect to radius.*/ - MagneticResults->Bz -= - SphVariables->RelativeRadiusPower[n] * - (WMM_get_main_field_coeff_g(index) * - SphVariables->cos_mlambda[m] + WMM_get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) - * (float)(n + 1) * LegendreFunction->Pcup[index]; + MagneticResults->Bz -= + SphVariables->RelativeRadiusPower[n] * + (WMM_get_main_field_coeff_g(index) * + SphVariables->cos_mlambda[m] + WMM_get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) + * (float)(n + 1) * LegendreFunction->Pcup[index]; /* 1 nMax (n+2) n m m m - By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) - n=1 m=0 n n n */ + By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) + n=1 m=0 n n n */ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (WMM_get_main_field_coeff_g(index) * - SphVariables->sin_mlambda[m] - WMM_get_main_field_coeff_h(index) * SphVariables->cos_mlambda[m]) - * (float)(m) * LegendreFunction->Pcup[index]; + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (WMM_get_main_field_coeff_g(index) * + SphVariables->sin_mlambda[m] - WMM_get_main_field_coeff_h(index) * SphVariables->cos_mlambda[m]) + * (float)(m) * LegendreFunction->Pcup[index]; /* nMax (n+2) n m m m - Bx = - SUM (a/r) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) - n=1 m=0 n n n */ + Bx = - SUM (a/r) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) + n=1 m=0 n n n */ /* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */ - MagneticResults->Bx -= - SphVariables->RelativeRadiusPower[n] * - (WMM_get_main_field_coeff_g(index) * - SphVariables->cos_mlambda[m] + WMM_get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) - * LegendreFunction->dPcup[index]; + MagneticResults->Bx -= + SphVariables->RelativeRadiusPower[n] * + (WMM_get_main_field_coeff_g(index) * + SphVariables->cos_mlambda[m] + WMM_get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) + * LegendreFunction->dPcup[index]; + } + } - } - } - - cos_phi = cosf(DEG2RAD(CoordSpherical->phig)); - if (fabsf(cos_phi) > 1.0e-10f) - { - MagneticResults->By = MagneticResults->By / cos_phi; - } - else - { + cos_phi = cosf(DEG2RAD(CoordSpherical->phig)); + if (fabsf(cos_phi) > 1.0e-10f) { + MagneticResults->By = MagneticResults->By / cos_phi; + } else { /* Special calculation for component - By - at Geographic poles. * If the user wants to avoid using this function, please make sure that * the latitude is not exactly +/-90. An option is to make use the function * WMM_CheckGeographicPoles. */ - if (WMM_SummationSpecial(SphVariables, CoordSpherical, MagneticResults) < 0) - return -1; // error - } + if (WMM_SummationSpecial(SphVariables, CoordSpherical, MagneticResults) < 0) { + return -1; // error + } + } - return 0; // OK + return 0; // OK } -int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction, - WMMtype_SphericalHarmonicVariables * - SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults) +int WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables * + SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) { - /*This Function sums the secular variation coefficients to get the secular variation of the Magnetic vector. - INPUT : LegendreFunction - MagneticModel - SphVariables - CoordSpherical - OUTPUT : MagneticResults + /*This Function sums the secular variation coefficients to get the secular variation of the Magnetic vector. + INPUT : LegendreFunction + MagneticModel + SphVariables + CoordSpherical + OUTPUT : MagneticResults - CALLS : WMM_SecVarSummationSpecial + CALLS : WMM_SecVarSummationSpecial - */ + */ uint16_t m, n, index; - float cos_phi; + float cos_phi; - MagneticModel->SecularVariationUsed = TRUE; + MagneticModel->SecularVariationUsed = TRUE; - MagneticResults->Bz = 0.0f; - MagneticResults->By = 0.0f; - MagneticResults->Bx = 0.0f; + MagneticResults->Bz = 0.0f; + MagneticResults->By = 0.0f; + MagneticResults->Bx = 0.0f; - for (n = 1; n <= MagneticModel->nMaxSecVar; n++) - { - for (m = 0; m <= n; m++) - { - index = (n * (n + 1) / 2 + m); + for (n = 1; n <= MagneticModel->nMaxSecVar; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); -/* nMax (n+2) n m m m - Bz = -SUM (a/r) (n+1) SUM [g cosf(m p) + h sinf(m p)] P (sinf(phi)) - n=1 m=0 n n n */ +/* nMax (n+2) n m m m + Bz = -SUM (a/r) (n+1) SUM [g cosf(m p) + h sinf(m p)] P (sinf(phi)) + n=1 m=0 n n n */ /* Derivative with respect to radius.*/ - MagneticResults->Bz -= - SphVariables->RelativeRadiusPower[n] * - (WMM_get_secular_var_coeff_g(index) * - SphVariables->cos_mlambda[m] + WMM_get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) - * (float)(n + 1) * LegendreFunction->Pcup[index]; + MagneticResults->Bz -= + SphVariables->RelativeRadiusPower[n] * + (WMM_get_secular_var_coeff_g(index) * + SphVariables->cos_mlambda[m] + WMM_get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) + * (float)(n + 1) * LegendreFunction->Pcup[index]; /* 1 nMax (n+2) n m m m - By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) - n=1 m=0 n n n */ + By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) + n=1 m=0 n n n */ /* Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (WMM_get_secular_var_coeff_g(index) * - SphVariables->sin_mlambda[m] - WMM_get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[m]) - * (float)(m) * LegendreFunction->Pcup[index]; + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (WMM_get_secular_var_coeff_g(index) * + SphVariables->sin_mlambda[m] - WMM_get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[m]) + * (float)(m) * LegendreFunction->Pcup[index]; /* nMax (n+2) n m m m - Bx = - SUM (a/r) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) - n=1 m=0 n n n */ + Bx = - SUM (a/r) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) + n=1 m=0 n n n */ /* Derivative with respect to latitude, divided by radius. */ - MagneticResults->Bx -= - SphVariables->RelativeRadiusPower[n] * - (WMM_get_secular_var_coeff_g(index) * - SphVariables->cos_mlambda[m] + WMM_get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) - * LegendreFunction->dPcup[index]; - } - } - cos_phi = cosf(DEG2RAD(CoordSpherical->phig)); - if (fabsf(cos_phi) > 1.0e-10f) - { - MagneticResults->By = MagneticResults->By / cos_phi; - } - else - /* Special calculation for component By at Geographic poles */ - { - if (WMM_SecVarSummationSpecial(SphVariables, CoordSpherical, MagneticResults) < 0) - return -1; // error - } + MagneticResults->Bx -= + SphVariables->RelativeRadiusPower[n] * + (WMM_get_secular_var_coeff_g(index) * + SphVariables->cos_mlambda[m] + WMM_get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) + * LegendreFunction->dPcup[index]; + } + } + cos_phi = cosf(DEG2RAD(CoordSpherical->phig)); + if (fabsf(cos_phi) > 1.0e-10f) { + MagneticResults->By = MagneticResults->By / cos_phi; + } else { + /* Special calculation for component By at Geographic poles */ + if (WMM_SecVarSummationSpecial(SphVariables, CoordSpherical, MagneticResults) < 0) { + return -1; // error + } + } - return 0; // OK + return 0; // OK } -int WMM_RotateMagneticVector(WMMtype_CoordSpherical * CoordSpherical, - WMMtype_CoordGeodetic * CoordGeodetic, - WMMtype_MagneticResults * MagneticResultsSph, WMMtype_MagneticResults * MagneticResultsGeo) - /* Rotate the Magnetic Vectors to Geodetic Coordinates - Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov - Equation 16, WMM Technical report +int WMM_RotateMagneticVector(WMMtype_CoordSpherical *CoordSpherical, + WMMtype_CoordGeodetic *CoordGeodetic, + WMMtype_MagneticResults *MagneticResultsSph, WMMtype_MagneticResults *MagneticResultsGeo) +/* Rotate the Magnetic Vectors to Geodetic Coordinates + Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov + Equation 16, WMM Technical report - INPUT : CoordSpherical : Data structure WMMtype_CoordSpherical with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) + INPUT : CoordSpherical : Data structure WMMtype_CoordSpherical with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) - CoordGeodetic : Data structure WMMtype_CoordGeodetic with the following elements - float lambda; (longitude) - float phi; ( geodetic latitude) - float HeightAboveEllipsoid; (height above the ellipsoid (HaE) ) - float HeightAboveGeoid;(height above the Geoid ) + CoordGeodetic : Data structure WMMtype_CoordGeodetic with the following elements + float lambda; (longitude) + float phi; ( geodetic latitude) + float HeightAboveEllipsoid; (height above the ellipsoid (HaE) ) + float HeightAboveGeoid;(height above the Geoid ) - MagneticResultsSph : Data structure WMMtype_MagneticResults with the following elements - float Bx; North - float By; East - float Bz; Down + MagneticResultsSph : Data structure WMMtype_MagneticResults with the following elements + float Bx; North + float By; East + float Bz; Down - OUTPUT: MagneticResultsGeo Pointer to the data structure WMMtype_MagneticResults, with the following elements - float Bx; North - float By; East - float Bz; Down + OUTPUT: MagneticResultsGeo Pointer to the data structure WMMtype_MagneticResults, with the following elements + float Bx; North + float By; East + float Bz; Down - CALLS : none + CALLS : none - */ + */ { - /* Difference between the spherical and Geodetic latitudes */ - float Psi = DEG2RAD(CoordSpherical->phig - CoordGeodetic->phi); + /* Difference between the spherical and Geodetic latitudes */ + float Psi = DEG2RAD(CoordSpherical->phig - CoordGeodetic->phi); - /* Rotate spherical field components to the Geodeitic system */ - MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sinf(Psi) + MagneticResultsSph->Bz * cosf(Psi); - MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cosf(Psi) - MagneticResultsSph->Bz * sinf(Psi); - MagneticResultsGeo->By = MagneticResultsSph->By; + /* Rotate spherical field components to the Geodeitic system */ + MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sinf(Psi) + MagneticResultsSph->Bz * cosf(Psi); + MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cosf(Psi) - MagneticResultsSph->Bz * sinf(Psi); + MagneticResultsGeo->By = MagneticResultsSph->By; return 0; } -int WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults * MagneticResultsGeo, WMMtype_GeoMagneticElements * GeoMagneticElements) - - /* Calculate all the Geomagnetic elements from X,Y and Z components - INPUT MagneticResultsGeo Pointer to data structure with the following elements - float Bx; ( North ) - float By; ( East ) - float Bz; ( Down ) - OUTPUT GeoMagneticElements Pointer to data structure with the following elements - float Decl; (Angle between the magnetic field vector and true north, positive east) - float Incl; Angle between the magnetic field vector and the horizontal plane, positive down - float F; Magnetic Field Strength - float H; Horizontal Magnetic Field Strength - float X; Northern component of the magnetic field vector - float Y; Eastern component of the magnetic field vector - float Z; Downward component of the magnetic field vector - CALLS : none - */ +int WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements) +/* Calculate all the Geomagnetic elements from X,Y and Z components + INPUT MagneticResultsGeo Pointer to data structure with the following elements + float Bx; ( North ) + float By; ( East ) + float Bz; ( Down ) + OUTPUT GeoMagneticElements Pointer to data structure with the following elements + float Decl; (Angle between the magnetic field vector and true north, positive east) + float Incl; Angle between the magnetic field vector and the horizontal plane, positive down + float F; Magnetic Field Strength + float H; Horizontal Magnetic Field Strength + float X; Northern component of the magnetic field vector + float Y; Eastern component of the magnetic field vector + float Z; Downward component of the magnetic field vector + CALLS : none + */ { - GeoMagneticElements->X = MagneticResultsGeo->Bx; - GeoMagneticElements->Y = MagneticResultsGeo->By; - GeoMagneticElements->Z = MagneticResultsGeo->Bz; + GeoMagneticElements->X = MagneticResultsGeo->Bx; + GeoMagneticElements->Y = MagneticResultsGeo->By; + GeoMagneticElements->Z = MagneticResultsGeo->Bz; - GeoMagneticElements->H = sqrtf(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By); - GeoMagneticElements->F = sqrtf(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz); - GeoMagneticElements->Decl = RAD2DEG(atan2f(GeoMagneticElements->Y, GeoMagneticElements->X)); - GeoMagneticElements->Incl = RAD2DEG(atan2f(GeoMagneticElements->Z, GeoMagneticElements->H)); + GeoMagneticElements->H = sqrtf(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By); + GeoMagneticElements->F = sqrtf(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz); + GeoMagneticElements->Decl = RAD2DEG(atan2f(GeoMagneticElements->Y, GeoMagneticElements->X)); + GeoMagneticElements->Incl = RAD2DEG(atan2f(GeoMagneticElements->Z, GeoMagneticElements->H)); - return 0; // OK + return 0; // OK } -int WMM_CalculateSecularVariation(WMMtype_MagneticResults * MagneticVariation, WMMtype_GeoMagneticElements * MagneticElements) +int WMM_CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements) /*This takes the Magnetic Variation in x, y, and z and uses it to calculate the secular variation of each of the Geomagnetic elements. - INPUT MagneticVariation Data structure with the following elements - float Bx; ( North ) - float By; ( East ) - float Bz; ( Down ) - OUTPUT MagneticElements Pointer to the data structure with the following elements updated - float Decldot; Yearly Rate of change in declination - float Incldot; Yearly Rate of change in inclination - float Fdot; Yearly rate of change in Magnetic field strength - float Hdot; Yearly rate of change in horizontal field strength - float Xdot; Yearly rate of change in the northern component - float Ydot; Yearly rate of change in the eastern component - float Zdot; Yearly rate of change in the downward component - float GVdot;Yearly rate of chnage in grid variation - CALLS : none + INPUT MagneticVariation Data structure with the following elements + float Bx; ( North ) + float By; ( East ) + float Bz; ( Down ) + OUTPUT MagneticElements Pointer to the data structure with the following elements updated + float Decldot; Yearly Rate of change in declination + float Incldot; Yearly Rate of change in inclination + float Fdot; Yearly rate of change in Magnetic field strength + float Hdot; Yearly rate of change in horizontal field strength + float Xdot; Yearly rate of change in the northern component + float Ydot; Yearly rate of change in the eastern component + float Zdot; Yearly rate of change in the downward component + float GVdot;Yearly rate of chnage in grid variation + CALLS : none -*/ + */ { - MagneticElements->Xdot = MagneticVariation->Bx; - MagneticElements->Ydot = MagneticVariation->By; - MagneticElements->Zdot = MagneticVariation->Bz; - MagneticElements->Hdot = (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot) / MagneticElements->H; //See equation 19 in the WMM technical report - MagneticElements->Fdot = - (MagneticElements->X * MagneticElements->Xdot + - MagneticElements->Y * MagneticElements->Ydot + MagneticElements->Z * MagneticElements->Zdot) / MagneticElements->F; - MagneticElements->Decldot = - 180.0f / M_PI_F * (MagneticElements->X * MagneticElements->Ydot - - MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H); - MagneticElements->Incldot = - 180.0f / M_PI_F * (MagneticElements->H * MagneticElements->Zdot - - MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F); - MagneticElements->GVdot = MagneticElements->Decldot; + MagneticElements->Xdot = MagneticVariation->Bx; + MagneticElements->Ydot = MagneticVariation->By; + MagneticElements->Zdot = MagneticVariation->Bz; + MagneticElements->Hdot = (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot) / MagneticElements->H; // See equation 19 in the WMM technical report + MagneticElements->Fdot = + (MagneticElements->X * MagneticElements->Xdot + + MagneticElements->Y * MagneticElements->Ydot + MagneticElements->Z * MagneticElements->Zdot) / MagneticElements->F; + MagneticElements->Decldot = + 180.0f / M_PI_F * (MagneticElements->X * MagneticElements->Ydot - + MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H); + MagneticElements->Incldot = + 180.0f / M_PI_F * (MagneticElements->H * MagneticElements->Zdot - + MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F); + MagneticElements->GVdot = MagneticElements->Decldot; - return 0; // OK + return 0; // OK } int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax) - /* This function evaluates all of the Schmidt-semi normalized associated Legendre - functions up to degree nMax. The functions are initially scaled by - 10^280 sinf^m in order to minimize the effects of underflow at large m - near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299). - Note that this function performs the same operation as WMM_PcupLow. - However this function also can be used for high degree (large nMax) models. + functions up to degree nMax. The functions are initially scaled by + 10^280 sinf^m in order to minimize the effects of underflow at large m + near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299). + Note that this function performs the same operation as WMM_PcupLow. + However this function also can be used for high degree (large nMax) models. - Calling Parameters: - INPUT - nMax: Maximum spherical harmonic degree to compute. - x: cosf(colatitude) or sinf(latitude). + Calling Parameters: + INPUT + nMax: Maximum spherical harmonic degree to compute. + x: cosf(colatitude) or sinf(latitude). - OUTPUT - Pcup: A vector of all associated Legendgre polynomials evaluated at - x up to nMax. The lenght must by greater or equal to (nMax+1)*(nMax+2)/2. - dPcup: Derivative of Pcup(x) with respect to latitude + OUTPUT + Pcup: A vector of all associated Legendgre polynomials evaluated at + x up to nMax. The lenght must by greater or equal to (nMax+1)*(nMax+2)/2. + dPcup: Derivative of Pcup(x) with respect to latitude - CALLS : none - Notes: + CALLS : none + Notes: - Adopted from the FORTRAN code written by Mark Wieczorek September 25, 2005. + Adopted from the FORTRAN code written by Mark Wieczorek September 25, 2005. - Manoj Nair, Nov, 2009 Manoj.C.Nair@Noaa.Gov + Manoj Nair, Nov, 2009 Manoj.C.Nair@Noaa.Gov - Change from the previous version - The prevous version computes the derivatives as - dP(n,m)(x)/dx, where x = sinf(latitude) (or cosf(colatitude) ). - However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude. - Hence the derivatives are multiplied by sinf(latitude). - Removed the options for CS phase and normalizations. + Change from the previous version + The prevous version computes the derivatives as + dP(n,m)(x)/dx, where x = sinf(latitude) (or cosf(colatitude) ). + However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude. + Hence the derivatives are multiplied by sinf(latitude). + Removed the options for CS phase and normalizations. - Note: In geomagnetism, the derivatives of ALF are usually found with - respect to the colatitudes. Here the derivatives are found with respect - to the latitude. The difference is a sign reversal for the derivative of - the Associated Legendre Functions. + Note: In geomagnetism, the derivatives of ALF are usually found with + respect to the colatitudes. Here the derivatives are found with respect + to the latitude. The difference is a sign reversal for the derivative of + the Associated Legendre Functions. - The derivates can't be computed for latitude = |90| degrees. - */ + The derivates can't be computed for latitude = |90| degrees. + */ { - uint16_t k, kstart, m, n; - float pm2, pm1, pmm, plm, rescalem, z, scalef; + uint16_t k, kstart, m, n; + float pm2, pm1, pmm, plm, rescalem, z, scalef; - float *f1 = (float *) MALLOC(sizeof(float) * NUMPCUP); - float *f2 = (float *) MALLOC(sizeof(float) * NUMPCUP); - float *PreSqr = (float *) MALLOC(sizeof(float) * NUMPCUP); + float *f1 = (float *)MALLOC(sizeof(float) * NUMPCUP); + float *f2 = (float *)MALLOC(sizeof(float) * NUMPCUP); + float *PreSqr = (float *)MALLOC(sizeof(float) * NUMPCUP); - if (!PreSqr || !f2 || !f1) - { // memory allocation error - if (PreSqr) FREE(PreSqr); - if (f2) FREE(f2); - if (f1) FREE(f1); + if (!PreSqr || !f2 || !f1) { // memory allocation error + if (PreSqr) { + FREE(PreSqr); + } + if (f2) { + FREE(f2); + } + if (f1) { + FREE(f1); + } return -1; } @@ -834,499 +836,471 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax) * Note: OP code change to avoid floating point equality test. * Was: if (fabs(x) == 1.0) */ - if (fabsf(x) - 1.0f < 1e-9f) - { - FREE(PreSqr); - FREE(f2); - FREE(f1); + if (fabsf(x) - 1.0f < 1e-9f) { + FREE(PreSqr); + FREE(f2); + FREE(f1); - // printf("Error in PcupHigh: derivative cannot be calculated at poles\n"); - return -2; - } + // printf("Error in PcupHigh: derivative cannot be calculated at poles\n"); + return -2; + } - /* OP Change: 1.0e-280 is too small to store in a float - the compiler truncates - * it to 0.0f, which is bad as the code below divides by scalef. */ - scalef = 1.0e-20f; + /* OP Change: 1.0e-280 is too small to store in a float - the compiler truncates + * it to 0.0f, which is bad as the code below divides by scalef. */ + scalef = 1.0e-20f; - for (n = 0; n <= 2 * nMax + 1; ++n) - PreSqr[n] = sqrtf((float)(n)); + for (n = 0; n <= 2 * nMax + 1; ++n) { + PreSqr[n] = sqrtf((float)(n)); + } - k = 2; + k = 2; - for (n = 2; n <= nMax; n++) - { - k = k + 1; - f1[k] = (float)(2 * n - 1) / (float)(n); - f2[k] = (float)(n - 1) / (float)(n); - for (m = 1; m <= n - 2; m++) - { - k = k + 1; - f1[k] = (float)(2 * n - 1) / PreSqr[n + m] / PreSqr[n - m]; - f2[k] = PreSqr[n - m - 1] * PreSqr[n + m - 1] / PreSqr[n + m] / PreSqr[n - m]; - } - k = k + 2; - } + for (n = 2; n <= nMax; n++) { + k = k + 1; + f1[k] = (float)(2 * n - 1) / (float)(n); + f2[k] = (float)(n - 1) / (float)(n); + for (m = 1; m <= n - 2; m++) { + k = k + 1; + f1[k] = (float)(2 * n - 1) / PreSqr[n + m] / PreSqr[n - m]; + f2[k] = PreSqr[n - m - 1] * PreSqr[n + m - 1] / PreSqr[n + m] / PreSqr[n - m]; + } + k = k + 2; + } - /*z = sinf (geocentric latitude) */ - z = sqrtf((1.0f - x) * (1.0f + x)); - pm2 = 1.0f; - Pcup[0] = 1.0f; - dPcup[0] = 0.0f; - if (nMax == 0) - { + /*z = sinf (geocentric latitude) */ + z = sqrtf((1.0f - x) * (1.0f + x)); + pm2 = 1.0f; + Pcup[0] = 1.0f; + dPcup[0] = 0.0f; + if (nMax == 0) { FREE(PreSqr); FREE(f2); FREE(f1); return -3; } - pm1 = x; - Pcup[1] = pm1; - dPcup[1] = z; - k = 1; + pm1 = x; + Pcup[1] = pm1; + dPcup[1] = z; + k = 1; - for (n = 2; n <= nMax; n++) - { - k = k + n; - plm = f1[k] * x * pm1 - f2[k] * pm2; - Pcup[k] = plm; - dPcup[k] = (float)(n) * (pm1 - x * plm) / z; - pm2 = pm1; - pm1 = plm; - } + for (n = 2; n <= nMax; n++) { + k = k + n; + plm = f1[k] * x * pm1 - f2[k] * pm2; + Pcup[k] = plm; + dPcup[k] = (float)(n) * (pm1 - x * plm) / z; + pm2 = pm1; + pm1 = plm; + } - pmm = PreSqr[2] * scalef; - rescalem = 1.0f / scalef; - kstart = 0; + pmm = PreSqr[2] * scalef; + rescalem = 1.0f / scalef; + kstart = 0; - for (m = 1; m <= nMax - 1; ++m) - { - rescalem = rescalem * z; + for (m = 1; m <= nMax - 1; ++m) { + rescalem = rescalem * z; - /* Calculate Pcup(m,m) */ - kstart = kstart + m + 1; - pmm = pmm * PreSqr[2 * m + 1] / PreSqr[2 * m]; - Pcup[kstart] = pmm * rescalem / PreSqr[2 * m + 1]; - dPcup[kstart] = -((float)(m) * x * Pcup[kstart] / z); - pm2 = pmm / PreSqr[2 * m + 1]; - /* Calculate Pcup(m+1,m) */ - k = kstart + m + 1; - pm1 = x * PreSqr[2 * m + 1] * pm2; - Pcup[k] = pm1 * rescalem; - dPcup[k] = ((pm2 * rescalem) * PreSqr[2 * m + 1] - x * (float)(m + 1) * Pcup[k]) / z; - /* Calculate Pcup(n,m) */ - for (n = m + 2; n <= nMax; ++n) - { - k = k + n; - plm = x * f1[k] * pm1 - f2[k] * pm2; - Pcup[k] = plm * rescalem; - dPcup[k] = (PreSqr[n + m] * PreSqr[n - m] * (pm1 * rescalem) - (float)(n) * x * Pcup[k]) / z; - pm2 = pm1; - pm1 = plm; - } - } + /* Calculate Pcup(m,m) */ + kstart = kstart + m + 1; + pmm = pmm * PreSqr[2 * m + 1] / PreSqr[2 * m]; + Pcup[kstart] = pmm * rescalem / PreSqr[2 * m + 1]; + dPcup[kstart] = -((float)(m) * x * Pcup[kstart] / z); + pm2 = pmm / PreSqr[2 * m + 1]; + /* Calculate Pcup(m+1,m) */ + k = kstart + m + 1; + pm1 = x * PreSqr[2 * m + 1] * pm2; + Pcup[k] = pm1 * rescalem; + dPcup[k] = ((pm2 * rescalem) * PreSqr[2 * m + 1] - x * (float)(m + 1) * Pcup[k]) / z; + /* Calculate Pcup(n,m) */ + for (n = m + 2; n <= nMax; ++n) { + k = k + n; + plm = x * f1[k] * pm1 - f2[k] * pm2; + Pcup[k] = plm * rescalem; + dPcup[k] = (PreSqr[n + m] * PreSqr[n - m] * (pm1 * rescalem) - (float)(n) * x * Pcup[k]) / z; + pm2 = pm1; + pm1 = plm; + } + } - /* Calculate Pcup(nMax,nMax) */ - rescalem = rescalem * z; - kstart = kstart + m + 1; - pmm = pmm / PreSqr[2 * nMax]; - Pcup[kstart] = pmm * rescalem; - dPcup[kstart] = -(float)(nMax) * x * Pcup[kstart] / z; + /* Calculate Pcup(nMax,nMax) */ + rescalem = rescalem * z; + kstart = kstart + m + 1; + pmm = pmm / PreSqr[2 * nMax]; + Pcup[kstart] = pmm * rescalem; + dPcup[kstart] = -(float)(nMax) * x * Pcup[kstart] / z; - // ********* - // free allocated memory + // ********* + // free allocated memory - FREE(PreSqr); + FREE(PreSqr); FREE(f2); - FREE(f1); + FREE(f1); // ********* - return 0; // OK + return 0; // OK } int WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax) - /* This function evaluates all of the Schmidt-semi normalized associated Legendre - functions up to degree nMax. + functions up to degree nMax. - Calling Parameters: - INPUT - nMax: Maximum spherical harmonic degree to compute. - x: cosf(colatitude) or sinf(latitude). + Calling Parameters: + INPUT + nMax: Maximum spherical harmonic degree to compute. + x: cosf(colatitude) or sinf(latitude). - OUTPUT - Pcup: A vector of all associated Legendgre polynomials evaluated at - x up to nMax. - dPcup: Derivative of Pcup(x) with respect to latitude + OUTPUT + Pcup: A vector of all associated Legendgre polynomials evaluated at + x up to nMax. + dPcup: Derivative of Pcup(x) with respect to latitude - Notes: Overflow may occur if nMax > 20 , especially for high-latitudes. - Use WMM_PcupHigh for large nMax. + Notes: Overflow may occur if nMax > 20 , especially for high-latitudes. + Use WMM_PcupHigh for large nMax. Writted by Manoj Nair, June, 2009 . Manoj.C.Nair@Noaa.Gov. - Note: In geomagnetism, the derivatives of ALF are usually found with - respect to the colatitudes. Here the derivatives are found with respect - to the latitude. The difference is a sign reversal for the derivative of - the Associated Legendre Functions. -*/ + Note: In geomagnetism, the derivatives of ALF are usually found with + respect to the colatitudes. Here the derivatives are found with respect + to the latitude. The difference is a sign reversal for the derivative of + the Associated Legendre Functions. + */ { - uint16_t n, m, index, index1, index2; - float k, z; + uint16_t n, m, index, index1, index2; + float k, z; - float *schmidtQuasiNorm = (float *) MALLOC(sizeof(float) * NUMPCUP); - if (!schmidtQuasiNorm) - { // memory allocation error + float *schmidtQuasiNorm = (float *)MALLOC(sizeof(float) * NUMPCUP); + + if (!schmidtQuasiNorm) { // memory allocation error return -1; } - Pcup[0] = 1.0f; - dPcup[0] = 0.0f; + Pcup[0] = 1.0f; + dPcup[0] = 0.0f; - /*sinf (geocentric latitude) - sin_phi */ - z = sqrtf((1.0f - x) * (1.0f + x)); + /*sinf (geocentric latitude) - sin_phi */ + z = sqrtf((1.0f - x) * (1.0f + x)); - /* First, Compute the Gauss-normalized associated Legendre functions */ - for (n = 1; n <= nMax; n++) - { - for (m = 0; m <= n; m++) - { - index = (n * (n + 1) / 2 + m); - if (n == m) - { - index1 = (n - 1) * n / 2 + m - 1; - Pcup[index] = z * Pcup[index1]; - dPcup[index] = z * dPcup[index1] + x * Pcup[index1]; - } - else - if (n == 1 && m == 0) - { - index1 = (n - 1) * n / 2 + m; - Pcup[index] = x * Pcup[index1]; - dPcup[index] = x * dPcup[index1] - z * Pcup[index1]; - } - else - if (n > 1 && n != m) - { - index1 = (n - 2) * (n - 1) / 2 + m; - index2 = (n - 1) * n / 2 + m; - if (m > n - 2) - { - Pcup[index] = x * Pcup[index2]; - dPcup[index] = x * dPcup[index2] - z * Pcup[index2]; - } - else - { - k = (float)(((n - 1) * (n - 1)) - (m * m)) / (float)((2 * n - 1) - * (2 * n - 3)); - Pcup[index] = x * Pcup[index2] - k * Pcup[index1]; - dPcup[index] = x * dPcup[index2] - z * Pcup[index2] - k * dPcup[index1]; - } - } - } - } + /* First, Compute the Gauss-normalized associated Legendre functions */ + for (n = 1; n <= nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + if (n == m) { + index1 = (n - 1) * n / 2 + m - 1; + Pcup[index] = z * Pcup[index1]; + dPcup[index] = z * dPcup[index1] + x * Pcup[index1]; + } else if (n == 1 && m == 0) { + index1 = (n - 1) * n / 2 + m; + Pcup[index] = x * Pcup[index1]; + dPcup[index] = x * dPcup[index1] - z * Pcup[index1]; + } else if (n > 1 && n != m) { + index1 = (n - 2) * (n - 1) / 2 + m; + index2 = (n - 1) * n / 2 + m; + if (m > n - 2) { + Pcup[index] = x * Pcup[index2]; + dPcup[index] = x * dPcup[index2] - z * Pcup[index2]; + } else { + k = (float)(((n - 1) * (n - 1)) - (m * m)) / (float)((2 * n - 1) + * (2 * n - 3)); + Pcup[index] = x * Pcup[index2] - k * Pcup[index1]; + dPcup[index] = x * dPcup[index2] - z * Pcup[index2] - k * dPcup[index1]; + } + } + } + } /*Compute the ration between the Gauss-normalized associated Legendre - functions and the Schmidt quasi-normalized version. This is equivalent to - sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ + functions and the Schmidt quasi-normalized version. This is equivalent to + sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ - schmidtQuasiNorm[0] = 1.0f; - for (n = 1; n <= nMax; n++) - { - index = (n * (n + 1) / 2); - index1 = (n - 1) * n / 2; - /* for m = 0 */ - schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (float)(2 * n - 1) / (float)n; + schmidtQuasiNorm[0] = 1.0f; + for (n = 1; n <= nMax; n++) { + index = (n * (n + 1) / 2); + index1 = (n - 1) * n / 2; + /* for m = 0 */ + schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (float)(2 * n - 1) / (float)n; - for (m = 1; m <= n; m++) - { - index = (n * (n + 1) / 2 + m); - index1 = (n * (n + 1) / 2 + m - 1); - schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * sqrtf((float)((n - m + 1) * (m == 1 ? 2 : 1)) / (float)(n + m)); - } - - } + for (m = 1; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + index1 = (n * (n + 1) / 2 + m - 1); + schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * sqrtf((float)((n - m + 1) * (m == 1 ? 2 : 1)) / (float)(n + m)); + } + } /* Converts the Gauss-normalized associated Legendre - functions to the Schmidt quasi-normalized version using pre-computed - relation stored in the variable schmidtQuasiNorm */ + functions to the Schmidt quasi-normalized version using pre-computed + relation stored in the variable schmidtQuasiNorm */ - for (n = 1; n <= nMax; n++) - { - for (m = 0; m <= n; m++) - { - index = (n * (n + 1) / 2 + m); - Pcup[index] = Pcup[index] * schmidtQuasiNorm[index]; - dPcup[index] = -dPcup[index] * schmidtQuasiNorm[index]; - /* The sign is changed since the new WMM routines use derivative with respect to latitude - insted of co-latitude */ - } - } + for (n = 1; n <= nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + Pcup[index] = Pcup[index] * schmidtQuasiNorm[index]; + dPcup[index] = -dPcup[index] * schmidtQuasiNorm[index]; + /* The sign is changed since the new WMM routines use derivative with respect to latitude + insted of co-latitude */ + } + } - FREE(schmidtQuasiNorm); + FREE(schmidtQuasiNorm); - return 0; // OK + return 0; // OK } int WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables * - SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults) - /* Special calculation for the component By at Geographic poles. - Manoj Nair, June, 2009 manoj.c.nair@noaa.gov - INPUT: MagneticModel - SphVariables - CoordSpherical - OUTPUT: MagneticResults - CALLS : none - See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report + SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) +/* Special calculation for the component By at Geographic poles. + Manoj Nair, June, 2009 manoj.c.nair@noaa.gov + INPUT: MagneticModel + SphVariables + CoordSpherical + OUTPUT: MagneticResults + CALLS : none + See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report - */ + */ { - uint16_t n, index; - float k, sin_phi; - float schmidtQuasiNorm1; - float schmidtQuasiNorm2; - float schmidtQuasiNorm3; + uint16_t n, index; + float k, sin_phi; + float schmidtQuasiNorm1; + float schmidtQuasiNorm2; + float schmidtQuasiNorm3; - float *PcupS = (float *) MALLOC(sizeof(float) * NUMPCUPS); - if (!PcupS) - return -1; // memory allocation error + float *PcupS = (float *)MALLOC(sizeof(float) * NUMPCUPS); - PcupS[0] = 1; - schmidtQuasiNorm1 = 1.0f; + if (!PcupS) { + return -1; // memory allocation error + } + PcupS[0] = 1; + schmidtQuasiNorm1 = 1.0f; - MagneticResults->By = 0.0f; - sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); + MagneticResults->By = 0.0f; + sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); - for (n = 1; n <= MagneticModel->nMax; n++) - { + for (n = 1; n <= MagneticModel->nMax; n++) { + /*Compute the ration between the Gauss-normalized associated Legendre + functions and the Schmidt quasi-normalized version. This is equivalent to + sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ - /*Compute the ration between the Gauss-normalized associated Legendre - functions and the Schmidt quasi-normalized version. This is equivalent to - sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ - - index = (n * (n + 1) / 2 + 1); - schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n; - schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrtf((float)(n * 2) / (float)(n + 1)); - schmidtQuasiNorm1 = schmidtQuasiNorm2; - if (n == 1) - { - PcupS[n] = PcupS[n - 1]; - } - else - { - k = (float)(((n - 1) * (n - 1)) - 1) / (float)((2 * n - 1) * (2 * n - 3)); - PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; - } + index = (n * (n + 1) / 2 + 1); + schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n; + schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrtf((float)(n * 2) / (float)(n + 1)); + schmidtQuasiNorm1 = schmidtQuasiNorm2; + if (n == 1) { + PcupS[n] = PcupS[n - 1]; + } else { + k = (float)(((n - 1) * (n - 1)) - 1) / (float)((2 * n - 1) * (2 * n - 3)); + PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; + } /* 1 nMax (n+2) n m m m - By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) - n=1 m=0 n n n */ + By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) + n=1 m=0 n n n */ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (WMM_get_main_field_coeff_g(index) * - SphVariables->sin_mlambda[1] - WMM_get_main_field_coeff_h(index) * SphVariables->cos_mlambda[1]) - * PcupS[n] * schmidtQuasiNorm3; - } + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (WMM_get_main_field_coeff_g(index) * + SphVariables->sin_mlambda[1] - WMM_get_main_field_coeff_h(index) * SphVariables->cos_mlambda[1]) + * PcupS[n] * schmidtQuasiNorm3; + } - FREE(PcupS); + FREE(PcupS); - return 0; // OK + return 0; // OK } int WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * - SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults) + SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) { - /*Special calculation for the secular variation summation at the poles. + /*Special calculation for the secular variation summation at the poles. - INPUT: MagneticModel - SphVariables - CoordSpherical - OUTPUT: MagneticResults - CALLS : none + INPUT: MagneticModel + SphVariables + CoordSpherical + OUTPUT: MagneticResults + CALLS : none - */ - uint16_t n, index; - float k, sin_phi; - float schmidtQuasiNorm1; - float schmidtQuasiNorm2; - float schmidtQuasiNorm3; + */ + uint16_t n, index; + float k, sin_phi; + float schmidtQuasiNorm1; + float schmidtQuasiNorm2; + float schmidtQuasiNorm3; - float *PcupS = (float *) MALLOC(sizeof(float) * NUMPCUPS); - if (!PcupS) - return -1; // memory allocation error + float *PcupS = (float *)MALLOC(sizeof(float) * NUMPCUPS); - PcupS[0] = 1; - schmidtQuasiNorm1 = 1.0f; + if (!PcupS) { + return -1; // memory allocation error + } + PcupS[0] = 1; + schmidtQuasiNorm1 = 1.0f; - MagneticResults->By = 0.0f; - sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); + MagneticResults->By = 0.0f; + sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); - for (n = 1; n <= MagneticModel->nMaxSecVar; n++) - { - index = (n * (n + 1) / 2 + 1); - schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n; - schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrtf((float)(n * 2) / (float)(n + 1)); - schmidtQuasiNorm1 = schmidtQuasiNorm2; - if (n == 1) - { - PcupS[n] = PcupS[n - 1]; - } - else - { - k = (float)(((n - 1) * (n - 1)) - 1) / (float)((2 * n - 1) * (2 * n - 3)); - PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; - } + for (n = 1; n <= MagneticModel->nMaxSecVar; n++) { + index = (n * (n + 1) / 2 + 1); + schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n; + schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrtf((float)(n * 2) / (float)(n + 1)); + schmidtQuasiNorm1 = schmidtQuasiNorm2; + if (n == 1) { + PcupS[n] = PcupS[n - 1]; + } else { + k = (float)(((n - 1) * (n - 1)) - 1) / (float)((2 * n - 1) * (2 * n - 3)); + PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; + } /* 1 nMax (n+2) n m m m - By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) - n=1 m=0 n n n */ + By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) + n=1 m=0 n n n */ /* Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (WMM_get_secular_var_coeff_g(index) * - SphVariables->sin_mlambda[1] - WMM_get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[1]) - * PcupS[n] * schmidtQuasiNorm3; - } + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (WMM_get_secular_var_coeff_g(index) * + SphVariables->sin_mlambda[1] - WMM_get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[1]) + * PcupS[n] * schmidtQuasiNorm3; + } - FREE(PcupS); + FREE(PcupS); - return 0; // OK + return 0; // OK } /** * @brief Comput the MainFieldCoeffH accounting for the date */ -float WMM_get_main_field_coeff_g(uint16_t index) -{ - if (index >= NUMTERMS) - return 0; +float WMM_get_main_field_coeff_g(uint16_t index) +{ + if (index >= NUMTERMS) { + return 0; + } uint16_t n, m, sum_index, a, b; float coeff = CoeffFile[index][2]; - - a = MagneticModel->nMaxSecVar; - b = (a * (a + 1) / 2 + a); - for (n = 1; n <= MagneticModel->nMax; n++) - { - for (m = 0; m <= n; m++) - { - - sum_index = (n * (n + 1) / 2 + m); - - /* Hacky for now, will solve for which conditions need summing analytically */ - if (sum_index != index) - continue; - - if (index <= b) - coeff += (decimal_date - MagneticModel->epoch) * WMM_get_secular_var_coeff_g(sum_index); - - } - } - - return coeff; + + a = MagneticModel->nMaxSecVar; + b = (a * (a + 1) / 2 + a); + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { + sum_index = (n * (n + 1) / 2 + m); + + /* Hacky for now, will solve for which conditions need summing analytically */ + if (sum_index != index) { + continue; + } + + if (index <= b) { + coeff += (decimal_date - MagneticModel->epoch) * WMM_get_secular_var_coeff_g(sum_index); + } + } + } + + return coeff; } -float WMM_get_main_field_coeff_h(uint16_t index) -{ - if (index >= NUMTERMS) - return 0; - +float WMM_get_main_field_coeff_h(uint16_t index) +{ + if (index >= NUMTERMS) { + return 0; + } + uint16_t n, m, sum_index, a, b; - float coeff = CoeffFile[index][3]; - - a = MagneticModel->nMaxSecVar; - b = (a * (a + 1) / 2 + a); - for (n = 1; n <= MagneticModel->nMax; n++) - { - for (m = 0; m <= n; m++) - { - - sum_index = (n * (n + 1) / 2 + m); - - /* Hacky for now, will solve for which conditions need summing analytically */ - if (sum_index != index) - continue; - - if (index <= b) - coeff += (decimal_date - MagneticModel->epoch) * WMM_get_secular_var_coeff_h(sum_index); - } - } - - return coeff; + float coeff = CoeffFile[index][3]; + + a = MagneticModel->nMaxSecVar; + b = (a * (a + 1) / 2 + a); + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { + sum_index = (n * (n + 1) / 2 + m); + + /* Hacky for now, will solve for which conditions need summing analytically */ + if (sum_index != index) { + continue; + } + + if (index <= b) { + coeff += (decimal_date - MagneticModel->epoch) * WMM_get_secular_var_coeff_h(sum_index); + } + } + } + + return coeff; } -float WMM_get_secular_var_coeff_g(uint16_t index) -{ - if (index >= NUMTERMS) - return 0; - - return CoeffFile[index][4]; +float WMM_get_secular_var_coeff_g(uint16_t index) +{ + if (index >= NUMTERMS) { + return 0; + } + + return CoeffFile[index][4]; } -float WMM_get_secular_var_coeff_h(uint16_t index) -{ - if (index >= NUMTERMS) - return 0; - - return CoeffFile[index][5]; +float WMM_get_secular_var_coeff_h(uint16_t index) +{ + if (index >= NUMTERMS) { + return 0; + } + + return CoeffFile[index][5]; } int WMM_DateToYear(uint16_t month, uint16_t day, uint16_t year) // Converts a given calendar date into a decimal year { - uint16_t temp = 0; // Total number of days - uint16_t MonthDays[13] = { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; - uint16_t ExtraDay = 0; - uint16_t i; + uint16_t temp = 0; // Total number of days + uint16_t MonthDays[13] = { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; + uint16_t ExtraDay = 0; + uint16_t i; - if ((year % 4 == 0 && year % 100 != 0) || (year % 400 == 0)) - ExtraDay = 1; - MonthDays[2] += ExtraDay; + if ((year % 4 == 0 && year % 100 != 0) || (year % 400 == 0)) { + ExtraDay = 1; + } + MonthDays[2] += ExtraDay; - /******************Validation********************************/ + /******************Validation********************************/ - if (month <= 0 || month > 12) - return -1; // error + if (month <= 0 || month > 12) { + return -1; // error + } + if (day <= 0 || day > MonthDays[month]) { + return -2; // error + } + /****************Calculation of t***************************/ + for (i = 1; i <= month; i++) { + temp += MonthDays[i - 1]; + } + temp += day; - if (day <= 0 || day > MonthDays[month]) - return -2; // error + decimal_date = year + (temp - 1) / (365.0f + ExtraDay); - /****************Calculation of t***************************/ - for (i = 1; i <= month; i++) - temp += MonthDays[i - 1]; - temp += day; - - decimal_date = year + (temp - 1) / (365.0f + ExtraDay); - - return 0; // OK + return 0; // OK } -int WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_CoordSpherical * CoordSpherical) +int WMM_GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical) // Converts Geodetic coordinates to Spherical coordinates // Convert geodetic coordinates, (defined by the WGS-84 // reference ellipsoid), to Earth Centered Earth Fixed Cartesian // coordinates, and then to spherical coordinates. { - float CosLat, SinLat, rc, xp, zp; // all local variables + float CosLat, SinLat, rc, xp, zp; // all local variables - CosLat = cosf(DEG2RAD(CoordGeodetic->phi)); - SinLat = sinf(DEG2RAD(CoordGeodetic->phi)); + CosLat = cosf(DEG2RAD(CoordGeodetic->phi)); + SinLat = sinf(DEG2RAD(CoordGeodetic->phi)); - // compute the local radius of curvature on the WGS-84 reference ellipsoid - rc = Ellip->a / sqrtf(1.0f - Ellip->epssq * SinLat * SinLat); + // compute the local radius of curvature on the WGS-84 reference ellipsoid + rc = Ellip->a / sqrtf(1.0f - Ellip->epssq * SinLat * SinLat); - // compute ECEF Cartesian coordinates of specified point (for longitude=0) + // compute ECEF Cartesian coordinates of specified point (for longitude=0) - xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat; - zp = (rc * (1.0f - Ellip->epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat; + xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat; + zp = (rc * (1.0f - Ellip->epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat; - // compute spherical radius and angle lambda and phi of specified point + // compute spherical radius and angle lambda and phi of specified point - CoordSpherical->r = sqrtf(xp * xp + zp * zp); - CoordSpherical->phig = RAD2DEG(asinf(zp / CoordSpherical->r)); // geocentric latitude - CoordSpherical->lambda = CoordGeodetic->lambda; // longitude + CoordSpherical->r = sqrtf(xp * xp + zp * zp); + CoordSpherical->phig = RAD2DEG(asinf(zp / CoordSpherical->r)); // geocentric latitude + CoordSpherical->lambda = CoordGeodetic->lambda; // longitude - return 0; // OK + return 0; // OK } diff --git a/flight/libraries/aes.c b/flight/libraries/aes.c index c89636dfa..001b1d8eb 100644 --- a/flight/libraries/aes.c +++ b/flight/libraries/aes.c @@ -1,288 +1,291 @@ - // http://gladman.plushost.co.uk/oldsite/AES/index.php #include #include "aes.h" -#define BPOLY 0x1B -#define DPOLY 0x8D +#define BPOLY 0x1B +#define DPOLY 0x8D -const uint8_t sbox[256] = -{ - 0x63,0x7c,0x77,0x7b,0xf2,0x6b,0x6f,0xc5,0x30,0x01,0x67,0x2b,0xfe,0xd7,0xab,0x76, - 0xca,0x82,0xc9,0x7d,0xfa,0x59,0x47,0xf0,0xad,0xd4,0xa2,0xaf,0x9c,0xa4,0x72,0xc0, - 0xb7,0xfd,0x93,0x26,0x36,0x3f,0xf7,0xcc,0x34,0xa5,0xe5,0xf1,0x71,0xd8,0x31,0x15, - 0x04,0xc7,0x23,0xc3,0x18,0x96,0x05,0x9a,0x07,0x12,0x80,0xe2,0xeb,0x27,0xb2,0x75, - 0x09,0x83,0x2c,0x1a,0x1b,0x6e,0x5a,0xa0,0x52,0x3b,0xd6,0xb3,0x29,0xe3,0x2f,0x84, - 0x53,0xd1,0x00,0xed,0x20,0xfc,0xb1,0x5b,0x6a,0xcb,0xbe,0x39,0x4a,0x4c,0x58,0xcf, - 0xd0,0xef,0xaa,0xfb,0x43,0x4d,0x33,0x85,0x45,0xf9,0x02,0x7f,0x50,0x3c,0x9f,0xa8, - 0x51,0xa3,0x40,0x8f,0x92,0x9d,0x38,0xf5,0xbc,0xb6,0xda,0x21,0x10,0xff,0xf3,0xd2, - 0xcd,0x0c,0x13,0xec,0x5f,0x97,0x44,0x17,0xc4,0xa7,0x7e,0x3d,0x64,0x5d,0x19,0x73, - 0x60,0x81,0x4f,0xdc,0x22,0x2a,0x90,0x88,0x46,0xee,0xb8,0x14,0xde,0x5e,0x0b,0xdb, - 0xe0,0x32,0x3a,0x0a,0x49,0x06,0x24,0x5c,0xc2,0xd3,0xac,0x62,0x91,0x95,0xe4,0x79, - 0xe7,0xc8,0x37,0x6d,0x8d,0xd5,0x4e,0xa9,0x6c,0x56,0xf4,0xea,0x65,0x7a,0xae,0x08, - 0xba,0x78,0x25,0x2e,0x1c,0xa6,0xb4,0xc6,0xe8,0xdd,0x74,0x1f,0x4b,0xbd,0x8b,0x8a, - 0x70,0x3e,0xb5,0x66,0x48,0x03,0xf6,0x0e,0x61,0x35,0x57,0xb9,0x86,0xc1,0x1d,0x9e, - 0xe1,0xf8,0x98,0x11,0x69,0xd9,0x8e,0x94,0x9b,0x1e,0x87,0xe9,0xce,0x55,0x28,0xdf, - 0x8c,0xa1,0x89,0x0d,0xbf,0xe6,0x42,0x68,0x41,0x99,0x2d,0x0f,0xb0,0x54,0xbb,0x16 +const uint8_t sbox[256] = { + 0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76, + 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0, + 0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15, + 0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75, + 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84, + 0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf, + 0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8, + 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2, + 0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73, + 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb, + 0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79, + 0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08, + 0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a, + 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e, + 0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf, + 0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16 }; -const uint8_t isbox[256] = -{ - 0x52,0x09,0x6a,0xd5,0x30,0x36,0xa5,0x38,0xbf,0x40,0xa3,0x9e,0x81,0xf3,0xd7,0xfb, - 0x7c,0xe3,0x39,0x82,0x9b,0x2f,0xff,0x87,0x34,0x8e,0x43,0x44,0xc4,0xde,0xe9,0xcb, - 0x54,0x7b,0x94,0x32,0xa6,0xc2,0x23,0x3d,0xee,0x4c,0x95,0x0b,0x42,0xfa,0xc3,0x4e, - 0x08,0x2e,0xa1,0x66,0x28,0xd9,0x24,0xb2,0x76,0x5b,0xa2,0x49,0x6d,0x8b,0xd1,0x25, - 0x72,0xf8,0xf6,0x64,0x86,0x68,0x98,0x16,0xd4,0xa4,0x5c,0xcc,0x5d,0x65,0xb6,0x92, - 0x6c,0x70,0x48,0x50,0xfd,0xed,0xb9,0xda,0x5e,0x15,0x46,0x57,0xa7,0x8d,0x9d,0x84, - 0x90,0xd8,0xab,0x00,0x8c,0xbc,0xd3,0x0a,0xf7,0xe4,0x58,0x05,0xb8,0xb3,0x45,0x06, - 0xd0,0x2c,0x1e,0x8f,0xca,0x3f,0x0f,0x02,0xc1,0xaf,0xbd,0x03,0x01,0x13,0x8a,0x6b, - 0x3a,0x91,0x11,0x41,0x4f,0x67,0xdc,0xea,0x97,0xf2,0xcf,0xce,0xf0,0xb4,0xe6,0x73, - 0x96,0xac,0x74,0x22,0xe7,0xad,0x35,0x85,0xe2,0xf9,0x37,0xe8,0x1c,0x75,0xdf,0x6e, - 0x47,0xf1,0x1a,0x71,0x1d,0x29,0xc5,0x89,0x6f,0xb7,0x62,0x0e,0xaa,0x18,0xbe,0x1b, - 0xfc,0x56,0x3e,0x4b,0xc6,0xd2,0x79,0x20,0x9a,0xdb,0xc0,0xfe,0x78,0xcd,0x5a,0xf4, - 0x1f,0xdd,0xa8,0x33,0x88,0x07,0xc7,0x31,0xb1,0x12,0x10,0x59,0x27,0x80,0xec,0x5f, - 0x60,0x51,0x7f,0xa9,0x19,0xb5,0x4a,0x0d,0x2d,0xe5,0x7a,0x9f,0x93,0xc9,0x9c,0xef, - 0xa0,0xe0,0x3b,0x4d,0xae,0x2a,0xf5,0xb0,0xc8,0xeb,0xbb,0x3c,0x83,0x53,0x99,0x61, - 0x17,0x2b,0x04,0x7e,0xba,0x77,0xd6,0x26,0xe1,0x69,0x14,0x63,0x55,0x21,0x0c,0x7d +const uint8_t isbox[256] = { + 0x52, 0x09, 0x6a, 0xd5, 0x30, 0x36, 0xa5, 0x38, 0xbf, 0x40, 0xa3, 0x9e, 0x81, 0xf3, 0xd7, 0xfb, + 0x7c, 0xe3, 0x39, 0x82, 0x9b, 0x2f, 0xff, 0x87, 0x34, 0x8e, 0x43, 0x44, 0xc4, 0xde, 0xe9, 0xcb, + 0x54, 0x7b, 0x94, 0x32, 0xa6, 0xc2, 0x23, 0x3d, 0xee, 0x4c, 0x95, 0x0b, 0x42, 0xfa, 0xc3, 0x4e, + 0x08, 0x2e, 0xa1, 0x66, 0x28, 0xd9, 0x24, 0xb2, 0x76, 0x5b, 0xa2, 0x49, 0x6d, 0x8b, 0xd1, 0x25, + 0x72, 0xf8, 0xf6, 0x64, 0x86, 0x68, 0x98, 0x16, 0xd4, 0xa4, 0x5c, 0xcc, 0x5d, 0x65, 0xb6, 0x92, + 0x6c, 0x70, 0x48, 0x50, 0xfd, 0xed, 0xb9, 0xda, 0x5e, 0x15, 0x46, 0x57, 0xa7, 0x8d, 0x9d, 0x84, + 0x90, 0xd8, 0xab, 0x00, 0x8c, 0xbc, 0xd3, 0x0a, 0xf7, 0xe4, 0x58, 0x05, 0xb8, 0xb3, 0x45, 0x06, + 0xd0, 0x2c, 0x1e, 0x8f, 0xca, 0x3f, 0x0f, 0x02, 0xc1, 0xaf, 0xbd, 0x03, 0x01, 0x13, 0x8a, 0x6b, + 0x3a, 0x91, 0x11, 0x41, 0x4f, 0x67, 0xdc, 0xea, 0x97, 0xf2, 0xcf, 0xce, 0xf0, 0xb4, 0xe6, 0x73, + 0x96, 0xac, 0x74, 0x22, 0xe7, 0xad, 0x35, 0x85, 0xe2, 0xf9, 0x37, 0xe8, 0x1c, 0x75, 0xdf, 0x6e, + 0x47, 0xf1, 0x1a, 0x71, 0x1d, 0x29, 0xc5, 0x89, 0x6f, 0xb7, 0x62, 0x0e, 0xaa, 0x18, 0xbe, 0x1b, + 0xfc, 0x56, 0x3e, 0x4b, 0xc6, 0xd2, 0x79, 0x20, 0x9a, 0xdb, 0xc0, 0xfe, 0x78, 0xcd, 0x5a, 0xf4, + 0x1f, 0xdd, 0xa8, 0x33, 0x88, 0x07, 0xc7, 0x31, 0xb1, 0x12, 0x10, 0x59, 0x27, 0x80, 0xec, 0x5f, + 0x60, 0x51, 0x7f, 0xa9, 0x19, 0xb5, 0x4a, 0x0d, 0x2d, 0xe5, 0x7a, 0x9f, 0x93, 0xc9, 0x9c, 0xef, + 0xa0, 0xe0, 0x3b, 0x4d, 0xae, 0x2a, 0xf5, 0xb0, 0xc8, 0xeb, 0xbb, 0x3c, 0x83, 0x53, 0x99, 0x61, + 0x17, 0x2b, 0x04, 0x7e, 0xba, 0x77, 0xd6, 0x26, 0xe1, 0x69, 0x14, 0x63, 0x55, 0x21, 0x0c, 0x7d }; -//#define xtime(x) ( (x << 1) ^ (((x >> 7) & 1) * BPOLY) ) -//#define xtime(x) ( (x << 1) ^ ((x & 0x80) ? BPOLY : 0) ) +// #define xtime(x) ( (x << 1) ^ (((x >> 7) & 1) * BPOLY) ) +// #define xtime(x) ( (x << 1) ^ ((x & 0x80) ? BPOLY : 0) ) -const uint8_t xtime[256] = -{ - 0x00,0x02,0x04,0x06,0x08,0x0a,0x0c,0x0e,0x10,0x12,0x14,0x16,0x18,0x1a,0x1c,0x1e, - 0x20,0x22,0x24,0x26,0x28,0x2a,0x2c,0x2e,0x30,0x32,0x34,0x36,0x38,0x3a,0x3c,0x3e, - 0x40,0x42,0x44,0x46,0x48,0x4a,0x4c,0x4e,0x50,0x52,0x54,0x56,0x58,0x5a,0x5c,0x5e, - 0x60,0x62,0x64,0x66,0x68,0x6a,0x6c,0x6e,0x70,0x72,0x74,0x76,0x78,0x7a,0x7c,0x7e, - 0x80,0x82,0x84,0x86,0x88,0x8a,0x8c,0x8e,0x90,0x92,0x94,0x96,0x98,0x9a,0x9c,0x9e, - 0xa0,0xa2,0xa4,0xa6,0xa8,0xaa,0xac,0xae,0xb0,0xb2,0xb4,0xb6,0xb8,0xba,0xbc,0xbe, - 0xc0,0xc2,0xc4,0xc6,0xc8,0xca,0xcc,0xce,0xd0,0xd2,0xd4,0xd6,0xd8,0xda,0xdc,0xde, - 0xe0,0xe2,0xe4,0xe6,0xe8,0xea,0xec,0xee,0xf0,0xf2,0xf4,0xf6,0xf8,0xfa,0xfc,0xfe, - 0x1b,0x19,0x1f,0x1d,0x13,0x11,0x17,0x15,0x0b,0x09,0x0f,0x0d,0x03,0x01,0x07,0x05, - 0x3b,0x39,0x3f,0x3d,0x33,0x31,0x37,0x35,0x2b,0x29,0x2f,0x2d,0x23,0x21,0x27,0x25, - 0x5b,0x59,0x5f,0x5d,0x53,0x51,0x57,0x55,0x4b,0x49,0x4f,0x4d,0x43,0x41,0x47,0x45, - 0x7b,0x79,0x7f,0x7d,0x73,0x71,0x77,0x75,0x6b,0x69,0x6f,0x6d,0x63,0x61,0x67,0x65, - 0x9b,0x99,0x9f,0x9d,0x93,0x91,0x97,0x95,0x8b,0x89,0x8f,0x8d,0x83,0x81,0x87,0x85, - 0xbb,0xb9,0xbf,0xbd,0xb3,0xb1,0xb7,0xb5,0xab,0xa9,0xaf,0xad,0xa3,0xa1,0xa7,0xa5, - 0xdb,0xd9,0xdf,0xdd,0xd3,0xd1,0xd7,0xd5,0xcb,0xc9,0xcf,0xcd,0xc3,0xc1,0xc7,0xc5, - 0xfb,0xf9,0xff,0xfd,0xf3,0xf1,0xf7,0xf5,0xeb,0xe9,0xef,0xed,0xe3,0xe1,0xe7,0xe5 +const uint8_t xtime[256] = { + 0x00, 0x02, 0x04, 0x06, 0x08, 0x0a, 0x0c, 0x0e, 0x10, 0x12, 0x14, 0x16, 0x18, 0x1a, 0x1c, 0x1e, + 0x20, 0x22, 0x24, 0x26, 0x28, 0x2a, 0x2c, 0x2e, 0x30, 0x32, 0x34, 0x36, 0x38, 0x3a, 0x3c, 0x3e, + 0x40, 0x42, 0x44, 0x46, 0x48, 0x4a, 0x4c, 0x4e, 0x50, 0x52, 0x54, 0x56, 0x58, 0x5a, 0x5c, 0x5e, + 0x60, 0x62, 0x64, 0x66, 0x68, 0x6a, 0x6c, 0x6e, 0x70, 0x72, 0x74, 0x76, 0x78, 0x7a, 0x7c, 0x7e, + 0x80, 0x82, 0x84, 0x86, 0x88, 0x8a, 0x8c, 0x8e, 0x90, 0x92, 0x94, 0x96, 0x98, 0x9a, 0x9c, 0x9e, + 0xa0, 0xa2, 0xa4, 0xa6, 0xa8, 0xaa, 0xac, 0xae, 0xb0, 0xb2, 0xb4, 0xb6, 0xb8, 0xba, 0xbc, 0xbe, + 0xc0, 0xc2, 0xc4, 0xc6, 0xc8, 0xca, 0xcc, 0xce, 0xd0, 0xd2, 0xd4, 0xd6, 0xd8, 0xda, 0xdc, 0xde, + 0xe0, 0xe2, 0xe4, 0xe6, 0xe8, 0xea, 0xec, 0xee, 0xf0, 0xf2, 0xf4, 0xf6, 0xf8, 0xfa, 0xfc, 0xfe, + 0x1b, 0x19, 0x1f, 0x1d, 0x13, 0x11, 0x17, 0x15, 0x0b, 0x09, 0x0f, 0x0d, 0x03, 0x01, 0x07, 0x05, + 0x3b, 0x39, 0x3f, 0x3d, 0x33, 0x31, 0x37, 0x35, 0x2b, 0x29, 0x2f, 0x2d, 0x23, 0x21, 0x27, 0x25, + 0x5b, 0x59, 0x5f, 0x5d, 0x53, 0x51, 0x57, 0x55, 0x4b, 0x49, 0x4f, 0x4d, 0x43, 0x41, 0x47, 0x45, + 0x7b, 0x79, 0x7f, 0x7d, 0x73, 0x71, 0x77, 0x75, 0x6b, 0x69, 0x6f, 0x6d, 0x63, 0x61, 0x67, 0x65, + 0x9b, 0x99, 0x9f, 0x9d, 0x93, 0x91, 0x97, 0x95, 0x8b, 0x89, 0x8f, 0x8d, 0x83, 0x81, 0x87, 0x85, + 0xbb, 0xb9, 0xbf, 0xbd, 0xb3, 0xb1, 0xb7, 0xb5, 0xab, 0xa9, 0xaf, 0xad, 0xa3, 0xa1, 0xa7, 0xa5, + 0xdb, 0xd9, 0xdf, 0xdd, 0xd3, 0xd1, 0xd7, 0xd5, 0xcb, 0xc9, 0xcf, 0xcd, 0xc3, 0xc1, 0xc7, 0xc5, + 0xfb, 0xf9, 0xff, 0xfd, 0xf3, 0xf1, 0xf7, 0xf5, 0xeb, 0xe9, 0xef, 0xed, 0xe3, 0xe1, 0xe7, 0xe5 }; // *********************************************************************************** /* -void createXTimeTable(void) -{ - uint8_t i = 0; - do { - xtime[i] = (i << 1) ^ ((i & 0x80) ? BPOLY : 0); - } while (++i != 0); -} -*/ + void createXTimeTable(void) + { + uint8_t i = 0; + do { + xtime[i] = (i << 1) ^ ((i & 0x80) ? BPOLY : 0); + } while (++i != 0); + } + */ // *********************************************************************************** /* -uint8_t powTable[256]; -uint8_t logTable[256]; + uint8_t powTable[256]; + uint8_t logTable[256]; -uint8_t sbox[256]; -uint8_t isbox[256]; + uint8_t sbox[256]; + uint8_t isbox[256]; -void createPowLogTables(void) -{ - uint8_t i = 0; - uint8_t t = 1; + void createPowLogTables(void) + { + uint8_t i = 0; + uint8_t t = 1; - do { - powTable[i] = t; - logTable[t] = i; - i++; - t ^= (t << 1) ^ ((t & 0x80) ? BPOLY : 0); - } while (t != 1); + do { + powTable[i] = t; + logTable[t] = i; + i++; + t ^= (t << 1) ^ ((t & 0x80) ? BPOLY : 0); + } while (t != 1); - powTable[255] = powTable[0]; -} + powTable[255] = powTable[0]; + } -void createSubstitueBoxTable(void) -{ - int i = 0; - do { - uint8_t temp; + void createSubstitueBoxTable(void) + { + int i = 0; + do { + uint8_t temp; - if (i > 0) - temp = powTable[255 - logTable[i]]; - else - temp = 0; + if (i > 0) + temp = powTable[255 - logTable[i]]; + else + temp = 0; - uint8_t sb = temp ^ 0x63; - for (int j = 0; j < 4; j++) - { - temp = (temp << 1) | (temp >> 7); - sb ^= temp; - } + uint8_t sb = temp ^ 0x63; + for (int j = 0; j < 4; j++) + { + temp = (temp << 1) | (temp >> 7); + sb ^= temp; + } - sbox[i] = sb; - } while (++i != 0); -} + sbox[i] = sb; + } while (++i != 0); + } -void createInverseSubstitueBoxTable(void) -{ - uint8_t i = 0; - uint8_t j = 0; - do { - do { - if (sbox[j] == i) - { - isbox[i] = j; - j = 255; - } - } while (++j != 0); - } while (++i != 0); -} -*/ + void createInverseSubstitueBoxTable(void) + { + uint8_t i = 0; + uint8_t j = 0; + do { + do { + if (sbox[j] == i) + { + isbox[i] = j; + j = 255; + } + } while (++j != 0); + } while (++i != 0); + } + */ // *********************************************************************************** void copy_block(void *d, void *s) { - if (d == s) return; + if (d == s) { + return; + } - register uint8_t *src = s; - register uint8_t *dest = d; - for (int i = N_BLOCK; i; --i) - *dest++ = *src++; + register uint8_t *src = s; + register uint8_t *dest = d; + for (int i = N_BLOCK; i; --i) { + *dest++ = *src++; + } } void xor_block(void *d, void *s) { - register uint8_t *src = s; - register uint8_t *dest = d; - for (int i = N_BLOCK; i; --i) - *dest++ ^= *src++; + register uint8_t *src = s; + register uint8_t *dest = d; + + for (int i = N_BLOCK; i; --i) { + *dest++ ^= *src++; + } } void xor_word(uint8_t *d, uint8_t *s) { - *d++ ^= *s++; - *d++ ^= *s++; - *d++ ^= *s++; - *d++ ^= *s++; + *d++ ^= *s++; + *d++ ^= *s++; + *d++ ^= *s++; + *d++ ^= *s++; } void xor_sub_word(uint8_t *d, uint8_t *s) { - *d++ ^= sbox[*s++]; - *d++ ^= sbox[*s++]; - *d++ ^= sbox[*s++]; - *d++ ^= sbox[*s++]; + *d++ ^= sbox[*s++]; + *d++ ^= sbox[*s++]; + *d++ ^= sbox[*s++]; + *d++ ^= sbox[*s++]; } void xor_sub_rot_word(uint8_t *d, uint8_t *s, uint8_t rc) { - *d++ ^= sbox[s[1]] ^ rc; - *d++ ^= sbox[s[2]]; - *d++ ^= sbox[s[3]]; - *d++ ^= sbox[s[0]]; + *d++ ^= sbox[s[1]] ^ rc; + *d++ ^= sbox[s[2]]; + *d++ ^= sbox[s[3]]; + *d++ ^= sbox[s[0]]; } void mix_sub_column(uint8_t *a) { - uint8_t a0 = a[0]; - uint8_t a1 = a[1]; - uint8_t a2 = a[2]; - uint8_t a3 = a[3]; - uint8_t tmp = a0 ^ a1 ^ a2 ^ a3; - a[0] = a0 ^ xtime[a0 ^ a1] ^ tmp; - a[1] = a1 ^ xtime[a1 ^ a2] ^ tmp; - a[2] = a2 ^ xtime[a2 ^ a3] ^ tmp; - a[3] = a3 ^ xtime[a3 ^ a0] ^ tmp; + uint8_t a0 = a[0]; + uint8_t a1 = a[1]; + uint8_t a2 = a[2]; + uint8_t a3 = a[3]; + uint8_t tmp = a0 ^ a1 ^ a2 ^ a3; + + a[0] = a0 ^ xtime[a0 ^ a1] ^ tmp; + a[1] = a1 ^ xtime[a1 ^ a2] ^ tmp; + a[2] = a2 ^ xtime[a2 ^ a3] ^ tmp; + a[3] = a3 ^ xtime[a3 ^ a0] ^ tmp; } void mix_sub_columns(void *a) { - mix_sub_column((uint8_t*)a + 0); - mix_sub_column((uint8_t*)a + 4); - mix_sub_column((uint8_t*)a + 8); - mix_sub_column((uint8_t*)a + 12); + mix_sub_column((uint8_t *)a + 0); + mix_sub_column((uint8_t *)a + 4); + mix_sub_column((uint8_t *)a + 8); + mix_sub_column((uint8_t *)a + 12); } void inv_mix_sub_column(uint8_t *a) { - uint8_t tmp; - tmp = xtime[xtime[a[0] ^ a[2]]]; - a[0] ^= tmp; - a[2] ^= tmp; - tmp = xtime[xtime[a[1] ^ a[3]]]; - a[1] ^= tmp; - a[3] ^= tmp; + uint8_t tmp; + + tmp = xtime[xtime[a[0] ^ a[2]]]; + a[0] ^= tmp; + a[2] ^= tmp; + tmp = xtime[xtime[a[1] ^ a[3]]]; + a[1] ^= tmp; + a[3] ^= tmp; } void inv_mix_sub_columns(void *a) { - inv_mix_sub_column((uint8_t*)a + 0); - inv_mix_sub_column((uint8_t*)a + 4); - inv_mix_sub_column((uint8_t*)a + 8); - inv_mix_sub_column((uint8_t*)a + 12); + inv_mix_sub_column((uint8_t *)a + 0); + inv_mix_sub_column((uint8_t *)a + 4); + inv_mix_sub_column((uint8_t *)a + 8); + inv_mix_sub_column((uint8_t *)a + 12); - mix_sub_columns(a); + mix_sub_columns(a); } void shift_sub_rows(uint8_t *a) { - uint8_t tmp; + uint8_t tmp; - a[0] = sbox[a[0]]; - a[4] = sbox[a[4]]; - a[8] = sbox[a[8]]; - a[12] = sbox[a[12]]; + a[0] = sbox[a[0]]; + a[4] = sbox[a[4]]; + a[8] = sbox[a[8]]; + a[12] = sbox[a[12]]; - tmp = a[1]; - a[1] = sbox[a[5]]; - a[5] = sbox[a[9]]; - a[9] = sbox[a[13]]; - a[13] = sbox[tmp]; + tmp = a[1]; + a[1] = sbox[a[5]]; + a[5] = sbox[a[9]]; + a[9] = sbox[a[13]]; + a[13] = sbox[tmp]; - tmp = a[2]; - a[2] = sbox[a[10]]; - a[10] = sbox[tmp]; - tmp = a[6]; - a[6] = sbox[a[14]]; - a[14] = sbox[tmp]; + tmp = a[2]; + a[2] = sbox[a[10]]; + a[10] = sbox[tmp]; + tmp = a[6]; + a[6] = sbox[a[14]]; + a[14] = sbox[tmp]; - tmp = a[15]; - a[15] = sbox[a[11]]; - a[11] = sbox[a[7]]; - a[7] = sbox[a[3]]; - a[3] = sbox[tmp]; + tmp = a[15]; + a[15] = sbox[a[11]]; + a[11] = sbox[a[7]]; + a[7] = sbox[a[3]]; + a[3] = sbox[tmp]; } void inv_shift_sub_rows(uint8_t *a) { - uint8_t tmp; + uint8_t tmp; - a[0] = isbox[a[0]]; - a[4] = isbox[a[4]]; - a[8] = isbox[a[8]]; - a[12] = isbox[a[12]]; + a[0] = isbox[a[0]]; + a[4] = isbox[a[4]]; + a[8] = isbox[a[8]]; + a[12] = isbox[a[12]]; - tmp = a[13]; - a[13] = isbox[a[9]]; - a[9] = isbox[a[5]]; - a[5] = isbox[a[1]]; - a[1] = isbox[tmp]; + tmp = a[13]; + a[13] = isbox[a[9]]; + a[9] = isbox[a[5]]; + a[5] = isbox[a[1]]; + a[1] = isbox[tmp]; - tmp = a[2]; - a[2] = isbox[a[10]]; - a[10] = isbox[tmp]; - tmp = a[6]; - a[6] = isbox[a[14]]; - a[14] = isbox[tmp]; + tmp = a[2]; + a[2] = isbox[a[10]]; + a[10] = isbox[tmp]; + tmp = a[6]; + a[6] = isbox[a[14]]; + a[14] = isbox[tmp]; - tmp = a[3]; - a[3] = isbox[a[7]]; - a[7] = isbox[a[11]]; - a[11] = isbox[a[15]]; - a[15] = isbox[tmp]; + tmp = a[3]; + a[3] = isbox[a[7]]; + a[7] = isbox[a[11]]; + a[11] = isbox[a[15]]; + a[15] = isbox[tmp]; } // *********************************************************************************** @@ -290,80 +293,86 @@ void inv_shift_sub_rows(uint8_t *a) // 'on the fly' encryption key update for 128 bit keys void update_encrypt_key_128(uint8_t *k, uint8_t *rc) { - xor_sub_rot_word(k + 0, k + 12, *rc); + xor_sub_rot_word(k + 0, k + 12, *rc); - *rc = (*rc << 1) ^ ((*rc & 0x80) ? BPOLY : 0); + *rc = (*rc << 1) ^ ((*rc & 0x80) ? BPOLY : 0); - for (int i = 4; i < 16; i += 4) - xor_word(k + i + 0, k + i - 4); + for (int i = 4; i < 16; i += 4) { + xor_word(k + i + 0, k + i - 4); + } } // Encrypt a single block of 16 bytes void aes_encrypt_cbc_128(void *data, void *key, void *chain_block) { - uint8_t rc = 1; + uint8_t rc = 1; - if (chain_block) - xor_block(data, chain_block); + if (chain_block) { + xor_block(data, chain_block); + } - for (int round = 10; round; --round) - { - xor_block(data, key); // add_round_key - update_encrypt_key_128((uint8_t *)key, &rc); - shift_sub_rows(data); - if (round <= 1) continue; - mix_sub_columns(data); - } - xor_block(data, key); // add_round_key + for (int round = 10; round; --round) { + xor_block(data, key); // add_round_key + update_encrypt_key_128((uint8_t *)key, &rc); + shift_sub_rows(data); + if (round <= 1) { + continue; + } + mix_sub_columns(data); + } + xor_block(data, key); // add_round_key - if (chain_block) - copy_block(chain_block, data); + if (chain_block) { + copy_block(chain_block, data); + } } // 'on the fly' decryption key update for 128 bit keys void update_decrypt_key_128(uint8_t *k, uint8_t *rc) { - for (int i = 12; i; i -= 4) - xor_word(k + i + 0, k + i - 4); + for (int i = 12; i; i -= 4) { + xor_word(k + i + 0, k + i - 4); + } - *rc = (*rc >> 1) ^ ((*rc & 1) ? DPOLY : 0); + *rc = (*rc >> 1) ^ ((*rc & 1) ? DPOLY : 0); - xor_sub_rot_word(k + 0, k + 12, *rc); + xor_sub_rot_word(k + 0, k + 12, *rc); } // Decrypt a single block of 16 bytes void aes_decrypt_cbc_128(void *data, void *key, void *chain_block) { - uint8_t tmp_data[N_BLOCK]; + uint8_t tmp_data[N_BLOCK]; - uint8_t rc = 0x6c; + uint8_t rc = 0x6c; - copy_block(tmp_data, data); + copy_block(tmp_data, data); - xor_block(data, key); // add_round_key - for (int round = 10; round; --round) - { - inv_shift_sub_rows(data); - update_decrypt_key_128(key, &rc); - xor_block(data, key); // add_round_key - if (round <= 1) continue; - inv_mix_sub_columns(data); - } + xor_block(data, key); // add_round_key + for (int round = 10; round; --round) { + inv_shift_sub_rows(data); + update_decrypt_key_128(key, &rc); + xor_block(data, key); // add_round_key + if (round <= 1) { + continue; + } + inv_mix_sub_columns(data); + } - if (chain_block) - { - xor_block(data, chain_block); - copy_block(chain_block, tmp_data); - } + if (chain_block) { + xor_block(data, chain_block); + copy_block(chain_block, tmp_data); + } } void aes_decrypt_key_128_create(void *enc_key, void *dec_key) { - copy_block(dec_key, enc_key); + copy_block(dec_key, enc_key); - uint8_t rc = 1; - for (int i = 0; i < 10; i++) - update_encrypt_key_128(dec_key, &rc); + uint8_t rc = 1; + for (int i = 0; i < 10; i++) { + update_encrypt_key_128(dec_key, &rc); + } } // *********************************************************************************** @@ -371,105 +380,112 @@ void aes_decrypt_key_128_create(void *enc_key, void *dec_key) // 'on the fly' encryption key update for 256 bit keys void update_encrypt_key_256(uint8_t *k, uint8_t *rc) { - xor_sub_rot_word(k + 0, k + 28, *rc); + xor_sub_rot_word(k + 0, k + 28, *rc); - *rc = (*rc << 1) ^ ((*rc & 0x80) ? BPOLY : 0); + *rc = (*rc << 1) ^ ((*rc & 0x80) ? BPOLY : 0); - for (int i = 4; i < 16; i += 4) - xor_word(k + i + 0, k + i - 4); + for (int i = 4; i < 16; i += 4) { + xor_word(k + i + 0, k + i - 4); + } - xor_sub_word(k + 16, k + 12); + xor_sub_word(k + 16, k + 12); - for (int i = 20; i < 32; i += 4) - xor_word(k + i + 0, k + i - 4); + for (int i = 20; i < 32; i += 4) { + xor_word(k + i + 0, k + i - 4); + } } // Encrypt a single block of 16 bytes void aes_encrypt_cbc_256(void *data, void *key, void *chain_block) { - uint8_t rc = 1; + uint8_t rc = 1; - if (chain_block) - xor_block(data, chain_block); + if (chain_block) { + xor_block(data, chain_block); + } - for (int round = 7; round; --round) - { -// printf("key - "); -// uint8_t *p = key; -// for (int i = 0; i < 32; i++) printf("%0.2X ", *p++); -// printf("\r\n"); + for (int round = 7; round; --round) { +// printf("key - "); +// uint8_t *p = key; +// for (int i = 0; i < 32; i++) printf("%0.2X ", *p++); +// printf("\r\n"); - xor_block(data, key); // add_round_key - shift_sub_rows(data); - mix_sub_columns(data); - xor_block(data, (uint8_t*)key + 16); // add_round_key - update_encrypt_key_256(key, &rc); - shift_sub_rows(data); - if (round <= 1) continue; - mix_sub_columns(data); - } - xor_block(data, key); // add_round_key + xor_block(data, key); // add_round_key + shift_sub_rows(data); + mix_sub_columns(data); + xor_block(data, (uint8_t *)key + 16); // add_round_key + update_encrypt_key_256(key, &rc); + shift_sub_rows(data); + if (round <= 1) { + continue; + } + mix_sub_columns(data); + } + xor_block(data, key); // add_round_key - if (chain_block) - copy_block(chain_block, data); + if (chain_block) { + copy_block(chain_block, data); + } } // 'on the fly' decryption key update for 256 bit keys void update_decrypt_key_256(uint8_t *k, uint8_t *rc) { - for (int i = 28; i >= 20; i -= 4) - xor_word(k + i + 0, k + i - 4); + for (int i = 28; i >= 20; i -= 4) { + xor_word(k + i + 0, k + i - 4); + } - xor_sub_word(k + 16, k + 12); + xor_sub_word(k + 16, k + 12); - for (int i = 12; i; i -= 4) - xor_word(k + i + 0, k + i - 4); + for (int i = 12; i; i -= 4) { + xor_word(k + i + 0, k + i - 4); + } - *rc = (*rc >> 1) ^ ((*rc & 1) ? DPOLY : 0); + *rc = (*rc >> 1) ^ ((*rc & 1) ? DPOLY : 0); - xor_sub_rot_word(k + 0, k + 28, *rc); + xor_sub_rot_word(k + 0, k + 28, *rc); } // Decrypt a single block of 16 bytes void aes_decrypt_cbc_256(void *data, void *key, void *chain_block) { - uint8_t tmp_data[N_BLOCK]; + uint8_t tmp_data[N_BLOCK]; - uint8_t rc = 0x80; + uint8_t rc = 0x80; - copy_block(tmp_data, data); + copy_block(tmp_data, data); - xor_block(data, key); // add_round_key - for (int round = 7; round; --round) - { - inv_shift_sub_rows(data); - update_decrypt_key_256(key, &rc); - xor_block(data, (uint8_t*)key + 16); // add_round_key - inv_mix_sub_columns(data); - inv_shift_sub_rows(data); - xor_block(data, key); // add_round_key - if (round <= 1) continue; - inv_mix_sub_columns(data); - } + xor_block(data, key); // add_round_key + for (int round = 7; round; --round) { + inv_shift_sub_rows(data); + update_decrypt_key_256(key, &rc); + xor_block(data, (uint8_t *)key + 16); // add_round_key + inv_mix_sub_columns(data); + inv_shift_sub_rows(data); + xor_block(data, key); // add_round_key + if (round <= 1) { + continue; + } + inv_mix_sub_columns(data); + } - if (chain_block) - { - xor_block(data, chain_block); - copy_block(chain_block, tmp_data); - } + if (chain_block) { + xor_block(data, chain_block); + copy_block(chain_block, tmp_data); + } } void aes_decrypt_key_256_create(void *enc_key, void *dec_key) { - if (dec_key != enc_key) - { - copy_block(dec_key, enc_key); - copy_block((uint8_t*)dec_key + 16, (uint8_t*)enc_key + 16); - } + if (dec_key != enc_key) { + copy_block(dec_key, enc_key); + copy_block((uint8_t *)dec_key + 16, (uint8_t *)enc_key + 16); + } - uint8_t rc = 1; - for (int i = 7; i; --i) - update_encrypt_key_256(dec_key, &rc); + uint8_t rc = 1; + for (int i = 7; i; --i) { + update_encrypt_key_256(dec_key, &rc); + } } // *********************************************************************************** diff --git a/flight/libraries/alarms.c b/flight/libraries/alarms.c index 54899e986..20e6753eb 100644 --- a/flight/libraries/alarms.c +++ b/flight/libraries/alarms.c @@ -48,9 +48,9 @@ int32_t AlarmsInitialize(void) SystemAlarmsInitialize(); lock = xSemaphoreCreateRecursiveMutex(); - //do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that - //AlarmsClearAll(); - //AlarmsDefaultAll(); + // do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that + // AlarmsClearAll(); + // AlarmsDefaultAll(); return 0; } @@ -82,7 +82,6 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity // Release lock xSemaphoreGiveRecursive(lock); return 0; - } /** @@ -94,9 +93,9 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity * @return 0 if success, -1 if an error */ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm, - SystemAlarmsAlarmOptions severity, - SystemAlarmsExtendedAlarmStatusOptions status, - uint8_t subStatus) + SystemAlarmsAlarmOptions severity, + SystemAlarmsExtendedAlarmStatusOptions status, + uint8_t subStatus) { SystemAlarmsData alarms; @@ -111,7 +110,7 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm, // Read alarm and update its severity only if it was changed SystemAlarmsGet(&alarms); if (alarms.Alarm[alarm] != severity) { - alarms.ExtendedAlarmStatus[alarm] = status; + alarms.ExtendedAlarmStatus[alarm] = status; alarms.ExtendedAlarmSubStatus[alarm] = subStatus; alarms.Alarm[alarm] = severity; SystemAlarmsSet(&alarms); diff --git a/flight/libraries/fifo_buffer.c b/flight/libraries/fifo_buffer.c index 7b68fcf09..396ee6ec7 100644 --- a/flight/libraries/fifo_buffer.c +++ b/flight/libraries/fifo_buffer.c @@ -31,233 +31,239 @@ // circular buffer functions uint16_t fifoBuf_getSize(t_fifo_buffer *buf) -{ // return the usable size of the buffer - +{ // return the usable size of the buffer uint16_t buf_size = buf->buf_size; - if (buf_size > 0) - return (buf_size - 1); - else - return 0; + if (buf_size > 0) { + return buf_size - 1; + } else { + return 0; + } } uint16_t fifoBuf_getUsed(t_fifo_buffer *buf) -{ // return the number of bytes available in the rx buffer - +{ // return the number of bytes available in the rx buffer uint16_t rd = buf->rd; uint16_t wr = buf->wr; - uint16_t buf_size = buf->buf_size; + uint16_t buf_size = buf->buf_size; uint16_t num_bytes = wr - rd; - if (wr < rd) + + if (wr < rd) { num_bytes = (buf_size - rd) + wr; + } return num_bytes; } uint16_t fifoBuf_getFree(t_fifo_buffer *buf) -{ // return the free space size in the buffer - - uint16_t buf_size = buf->buf_size; +{ // return the free space size in the buffer + uint16_t buf_size = buf->buf_size; uint16_t num_bytes = fifoBuf_getUsed(buf); - return ((buf_size - num_bytes) - 1); + return (buf_size - num_bytes) - 1; } void fifoBuf_clearData(t_fifo_buffer *buf) -{ // remove all data from the buffer - buf->rd = buf->wr; +{ // remove all data from the buffer + buf->rd = buf->wr; } void fifoBuf_removeData(t_fifo_buffer *buf, uint16_t len) -{ // remove a number of bytes from the buffer - +{ // remove a number of bytes from the buffer uint16_t rd = buf->rd; - uint16_t buf_size = buf->buf_size; + uint16_t buf_size = buf->buf_size; // get number of bytes available uint16_t num_bytes = fifoBuf_getUsed(buf); - if (num_bytes > len) + if (num_bytes > len) { num_bytes = len; + } - if (num_bytes < 1) - return; // nothing to remove - + if (num_bytes < 1) { + return; // nothing to remove + } rd += num_bytes; - if (rd >= buf_size) + if (rd >= buf_size) { rd -= buf_size; + } buf->rd = rd; } int16_t fifoBuf_getBytePeek(t_fifo_buffer *buf) -{ // get a data byte from the buffer without removing it - +{ // get a data byte from the buffer without removing it uint16_t rd = buf->rd; // get number of bytes available uint16_t num_bytes = fifoBuf_getUsed(buf); - if (num_bytes < 1) - return -1; // no byte retuened - - return buf->buf_ptr[rd]; // return the byte + if (num_bytes < 1) { + return -1; // no byte retuened + } + return buf->buf_ptr[rd]; // return the byte } int16_t fifoBuf_getByte(t_fifo_buffer *buf) -{ // get a data byte from the buffer - - uint16_t rd = buf->rd; - uint16_t buf_size = buf->buf_size; - uint8_t *buff = buf->buf_ptr; +{ // get a data byte from the buffer + uint16_t rd = buf->rd; + uint16_t buf_size = buf->buf_size; + uint8_t *buff = buf->buf_ptr; // get number of bytes available uint16_t num_bytes = fifoBuf_getUsed(buf); - if (num_bytes < 1) - return -1; // no byte returned - + if (num_bytes < 1) { + return -1; // no byte returned + } uint8_t b = buff[rd]; - if (++rd >= buf_size) + if (++rd >= buf_size) { rd = 0; + } buf->rd = rd; - return b; // return the byte + return b; // return the byte } uint16_t fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, uint16_t len) -{ // get data from the buffer without removing it - - uint16_t rd = buf->rd; - uint16_t buf_size = buf->buf_size; - uint8_t *buff = buf->buf_ptr; +{ // get data from the buffer without removing it + uint16_t rd = buf->rd; + uint16_t buf_size = buf->buf_size; + uint8_t *buff = buf->buf_ptr; // get number of bytes available uint16_t num_bytes = fifoBuf_getUsed(buf); - if (num_bytes > len) + if (num_bytes > len) { num_bytes = len; + } - if (num_bytes < 1) - return 0; // return number of bytes copied - + if (num_bytes < 1) { + return 0; // return number of bytes copied + } uint8_t *p = (uint8_t *)data; uint16_t i = 0; - while (num_bytes > 0) - { + while (num_bytes > 0) { uint16_t j = buf_size - rd; - if (j > num_bytes) + if (j > num_bytes) { j = num_bytes; + } memcpy(p + i, buff + rd, j); - i += j; + i += j; num_bytes -= j; rd += j; - if (rd >= buf_size) + if (rd >= buf_size) { rd = 0; + } } - return i; // return number of bytes copied + return i; // return number of bytes copied } uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len) -{ // get data from our rx buffer - - uint16_t rd = buf->rd; - uint16_t buf_size = buf->buf_size; - uint8_t *buff = buf->buf_ptr; +{ // get data from our rx buffer + uint16_t rd = buf->rd; + uint16_t buf_size = buf->buf_size; + uint8_t *buff = buf->buf_ptr; // get number of bytes available uint16_t num_bytes = fifoBuf_getUsed(buf); - if (num_bytes > len) + if (num_bytes > len) { num_bytes = len; + } - if (num_bytes < 1) - return 0; // return number of bytes copied - + if (num_bytes < 1) { + return 0; // return number of bytes copied + } uint8_t *p = (uint8_t *)data; uint16_t i = 0; - while (num_bytes > 0) - { + while (num_bytes > 0) { uint16_t j = buf_size - rd; - if (j > num_bytes) + if (j > num_bytes) { j = num_bytes; + } memcpy(p + i, buff + rd, j); - i += j; + i += j; num_bytes -= j; rd += j; - if (rd >= buf_size) + if (rd >= buf_size) { rd = 0; + } } buf->rd = rd; - return i; // return number of bytes copied + return i; // return number of bytes copied } uint16_t fifoBuf_putByte(t_fifo_buffer *buf, const uint8_t b) -{ // add a data byte to the buffer - - uint16_t wr = buf->wr; - uint16_t buf_size = buf->buf_size; - uint8_t *buff = buf->buf_ptr; +{ // add a data byte to the buffer + uint16_t wr = buf->wr; + uint16_t buf_size = buf->buf_size; + uint8_t *buff = buf->buf_ptr; uint16_t num_bytes = fifoBuf_getFree(buf); - if (num_bytes < 1) + + if (num_bytes < 1) { return 0; + } buff[wr] = b; - if (++wr >= buf_size) + if (++wr >= buf_size) { wr = 0; - - buf->wr = wr; - - return 1; // return number of bytes copied -} - -uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len) -{ // add data to the buffer - - uint16_t wr = buf->wr; - uint16_t buf_size = buf->buf_size; - uint8_t *buff = buf->buf_ptr; - - uint16_t num_bytes = fifoBuf_getFree(buf); - if (num_bytes > len) - num_bytes = len; - - if (num_bytes < 1) - return 0; // return number of bytes copied - - uint8_t *p = (uint8_t *)data; - uint16_t i = 0; - - while (num_bytes > 0) - { - uint16_t j = buf_size - wr; - if (j > num_bytes) - j = num_bytes; - memcpy(buff + wr, p + i, j); - i += j; - num_bytes -= j; - wr += j; - if (wr >= buf_size) - wr = 0; } buf->wr = wr; - return i; // return number of bytes copied + return 1; // return number of bytes copied +} + +uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len) +{ // add data to the buffer + uint16_t wr = buf->wr; + uint16_t buf_size = buf->buf_size; + uint8_t *buff = buf->buf_ptr; + + uint16_t num_bytes = fifoBuf_getFree(buf); + + if (num_bytes > len) { + num_bytes = len; + } + + if (num_bytes < 1) { + return 0; // return number of bytes copied + } + uint8_t *p = (uint8_t *)data; + uint16_t i = 0; + + while (num_bytes > 0) { + uint16_t j = buf_size - wr; + if (j > num_bytes) { + j = num_bytes; + } + memcpy(buff + wr, p + i, j); + i += j; + num_bytes -= j; + wr += j; + if (wr >= buf_size) { + wr = 0; + } + } + + buf->wr = wr; + + return i; // return number of bytes copied } void fifoBuf_init(t_fifo_buffer *buf, const void *buffer, const uint16_t buffer_size) { - buf->buf_ptr = (uint8_t *)buffer; + buf->buf_ptr = (uint8_t *)buffer; buf->rd = 0; buf->wr = 0; buf->buf_size = buffer_size; diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h index 020628d72..46b100e0f 100644 --- a/flight/libraries/inc/CoordinateConversions.h +++ b/flight/libraries/inc/CoordinateConversions.h @@ -30,45 +30,45 @@ #ifndef COORDINATECONVERSIONS_H_ #define COORDINATECONVERSIONS_H_ - // ****** convert Lat,Lon,Alt to ECEF ************ +// ****** convert Lat,Lon,Alt to ECEF ************ void LLA2ECEF(float LLA[3], float ECEF[3]); - // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* +// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* uint16_t ECEF2LLA(float ECEF[3], float LLA[3]); void RneFromLLA(float LLA[3], float Rne[3][3]); - // ****** find rotation matrix from rotation vector +// ****** find rotation matrix from rotation vector void Rv2Rot(float Rv[3], float R[3][3]); - // ****** find roll, pitch, yaw from quaternion ******** +// ****** find roll, pitch, yaw from quaternion ******** void Quaternion2RPY(const float q[4], float rpy[3]); - // ****** find quaternion from roll, pitch, yaw ******** +// ****** find quaternion from roll, pitch, yaw ******** void RPY2Quaternion(const float rpy[3], float q[4]); - //** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** void Quaternion2R(float q[4], float Rbe[3][3]); - // ****** Express LLA in a local NED Base Frame ******** +// ****** Express LLA in a local NED Base Frame ******** void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3]); - // ****** Express ECEF in a local NED Base Frame ******** +// ****** Express ECEF in a local NED Base Frame ******** void ECEF2Base(float ECEF[3], float BaseECEF[3], float Rne[3][3], float NED[3]); - // ****** convert Rotation Matrix to Quaternion ******** - // ****** if R converts from e to b, q is rotation from e to b **** +// ****** convert Rotation Matrix to Quaternion ******** +// ****** if R converts from e to b, q is rotation from e to b **** void R2Quaternion(float R[3][3], float q[4]); - // ****** Rotation Matrix from Two Vector Directions ******** - // ****** given two vector directions (v1 and v2) known in two frames (b and e) find Rbe *** - // ****** solution is approximate if can't be exact *** +// ****** Rotation Matrix from Two Vector Directions ******** +// ****** given two vector directions (v1 and v2) known in two frames (b and e) find Rbe *** +// ****** solution is approximate if can't be exact *** uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[3], const float v2e[3], float Rbe[3][3]); - // ****** Vector Cross Product ******** +// ****** Vector Cross Product ******** void CrossProduct(const float v1[3], const float v2[3], float result[3]); - // ****** Vector Magnitude ******** +// ****** Vector Magnitude ******** float VectorMagnitude(const float v[3]); void quat_inverse(float q[4]); diff --git a/flight/libraries/inc/WMMInternal.h b/flight/libraries/inc/WMMInternal.h index 5604a0773..aab5269b8 100644 --- a/flight/libraries/inc/WMMInternal.h +++ b/flight/libraries/inc/WMMInternal.h @@ -28,131 +28,130 @@ #define WMMINTERNAL_H_ #include - // internal constants -#define TRUE ((uint16_t)1) -#define FALSE ((uint16_t)0) -#define WMM_MAX_MODEL_DEGREES 12 -#define WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES 12 -#define NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES+1)*(WMM_MAX_MODEL_DEGREES+2)/2); -#define NUMPCUP 92 // NUMTERMS +1 -#define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1 +// internal constants +#define TRUE ((uint16_t)1) +#define FALSE ((uint16_t)0) +#define WMM_MAX_MODEL_DEGREES 12 +#define WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES 12 +#define NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES+1)*(WMM_MAX_MODEL_DEGREES+2)/2); +#define NUMPCUP 92 // NUMTERMS +1 +#define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1 - // internal structure definitions +// internal structure definitions typedef struct { - float EditionDate; - float epoch; //Base time of Geomagnetic model epoch (yrs) - char ModelName[20]; -// float Main_Field_Coeff_G[NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) -// float Main_Field_Coeff_H[NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) -// float Secular_Var_Coeff_G[NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) -// float Secular_Var_Coeff_H[NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) - uint16_t nMax; // Maximum degree of spherical harmonic model - uint16_t nMaxSecVar; // Maxumum degree of spherical harmonic secular model - uint16_t SecularVariationUsed; // Whether or not the magnetic secular variation vector will be needed by program + float EditionDate; + float epoch; // Base time of Geomagnetic model epoch (yrs) + char ModelName[20]; +// float Main_Field_Coeff_G[NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) +// float Main_Field_Coeff_H[NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) +// float Secular_Var_Coeff_G[NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) +// float Secular_Var_Coeff_H[NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) + uint16_t nMax; // Maximum degree of spherical harmonic model + uint16_t nMaxSecVar; // Maxumum degree of spherical harmonic secular model + uint16_t SecularVariationUsed; // Whether or not the magnetic secular variation vector will be needed by program } WMMtype_MagneticModel; typedef struct { - float a; // semi-major axis of the ellipsoid - float b; // semi-minor axis of the ellipsoid - float fla; // flattening - float epssq; // first eccentricity squared - float eps; // first eccentricity - float re; // mean radius of ellipsoid + float a; // semi-major axis of the ellipsoid + float b; // semi-minor axis of the ellipsoid + float fla; // flattening + float epssq; // first eccentricity squared + float eps; // first eccentricity + float re; // mean radius of ellipsoid } WMMtype_Ellipsoid; typedef struct { - float lambda; // longitude - float phi; // geodetic latitude - float HeightAboveEllipsoid; // height above the ellipsoid (HaE) + float lambda; // longitude + float phi; // geodetic latitude + float HeightAboveEllipsoid; // height above the ellipsoid (HaE) } WMMtype_CoordGeodetic; typedef struct { - float lambda; // longitude - float phig; // geocentric latitude - float r; // distance from the center of the ellipsoid + float lambda; // longitude + float phig; // geocentric latitude + float r; // distance from the center of the ellipsoid } WMMtype_CoordSpherical; typedef struct { - uint16_t Year; - uint16_t Month; - uint16_t Day; - float DecimalYear; + uint16_t Year; + uint16_t Month; + uint16_t Day; + float DecimalYear; } WMMtype_Date; typedef struct { - float Pcup[NUMPCUP]; // Legendre Function - float dPcup[NUMPCUP]; // Derivative of Lagendre fn + float Pcup[NUMPCUP]; // Legendre Function + float dPcup[NUMPCUP]; // Derivative of Lagendre fn } WMMtype_LegendreFunction; typedef struct { - float Bx; // North - float By; // East - float Bz; // Down + float Bx; // North + float By; // East + float Bz; // Down } WMMtype_MagneticResults; typedef struct { - - float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES + 1]; // [earth_reference_radius_km / sph. radius ]^n - float cos_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // cp(m) - cosine of (m*spherical coord. longitude - float sin_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // sp(m) - sine of (m*spherical coord. longitude) + float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES + 1]; // [earth_reference_radius_km / sph. radius ]^n + float cos_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // cp(m) - cosine of (m*spherical coord. longitude + float sin_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // sp(m) - sine of (m*spherical coord. longitude) } WMMtype_SphericalHarmonicVariables; typedef struct { - float Decl; /* 1. Angle between the magnetic field vector and true north, positive east */ - float Incl; /*2. Angle between the magnetic field vector and the horizontal plane, positive down */ - float F; /*3. Magnetic Field Strength */ - float H; /*4. Horizontal Magnetic Field Strength */ - float X; /*5. Northern component of the magnetic field vector */ - float Y; /*6. Eastern component of the magnetic field vector */ - float Z; /*7. Downward component of the magnetic field vector */ - float GV; /*8. The Grid Variation */ - float Decldot; /*9. Yearly Rate of change in declination */ - float Incldot; /*10. Yearly Rate of change in inclination */ - float Fdot; /*11. Yearly rate of change in Magnetic field strength */ - float Hdot; /*12. Yearly rate of change in horizontal field strength */ - float Xdot; /*13. Yearly rate of change in the northern component */ - float Ydot; /*14. Yearly rate of change in the eastern component */ - float Zdot; /*15. Yearly rate of change in the downward component */ - float GVdot; /*16. Yearly rate of chnage in grid variation */ + float Decl; /* 1. Angle between the magnetic field vector and true north, positive east */ + float Incl; /*2. Angle between the magnetic field vector and the horizontal plane, positive down */ + float F; /*3. Magnetic Field Strength */ + float H; /*4. Horizontal Magnetic Field Strength */ + float X; /*5. Northern component of the magnetic field vector */ + float Y; /*6. Eastern component of the magnetic field vector */ + float Z; /*7. Downward component of the magnetic field vector */ + float GV; /*8. The Grid Variation */ + float Decldot; /*9. Yearly Rate of change in declination */ + float Incldot; /*10. Yearly Rate of change in inclination */ + float Fdot; /*11. Yearly rate of change in Magnetic field strength */ + float Hdot; /*12. Yearly rate of change in horizontal field strength */ + float Xdot; /*13. Yearly rate of change in the northern component */ + float Ydot; /*14. Yearly rate of change in the eastern component */ + float Zdot; /*15. Yearly rate of change in the downward component */ + float GVdot; /*16. Yearly rate of chnage in grid variation */ } WMMtype_GeoMagneticElements; - // Internal Function Prototypes +// Internal Function Prototypes void WMM_Set_Coeff_Array(); -int WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_CoordSpherical * CoordSpherical); +int WMM_GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical); int WMM_DateToYear(uint16_t month, uint16_t day, uint16_t year); -int WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical, - WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_GeoMagneticElements * GeoMagneticElements); +int WMM_Geomag(WMMtype_CoordSpherical *CoordSpherical, + WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_GeoMagneticElements *GeoMagneticElements); -int WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical * CoordSpherical, uint16_t nMax, WMMtype_LegendreFunction * LegendreFunction); +int WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical *CoordSpherical, uint16_t nMax, WMMtype_LegendreFunction *LegendreFunction); -int WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults * MagneticResultsGeo, WMMtype_GeoMagneticElements * GeoMagneticElements); +int WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements); -int WMM_CalculateSecularVariation(WMMtype_MagneticResults * MagneticVariation, WMMtype_GeoMagneticElements * MagneticElements); +int WMM_CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements); int WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical * - CoordSpherical, uint16_t nMax, WMMtype_SphericalHarmonicVariables * SphVariables); + CoordSpherical, uint16_t nMax, WMMtype_SphericalHarmonicVariables *SphVariables); int WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax); int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax); int WMM_RotateMagneticVector(WMMtype_CoordSpherical *, - WMMtype_CoordGeodetic * CoordGeodetic, - WMMtype_MagneticResults * MagneticResultsSph, WMMtype_MagneticResults * MagneticResultsGeo); + WMMtype_CoordGeodetic *CoordGeodetic, + WMMtype_MagneticResults *MagneticResultsSph, WMMtype_MagneticResults *MagneticResultsGeo); -int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction, - WMMtype_SphericalHarmonicVariables * - SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults); +int WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables * + SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); int WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * - SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults); + SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); -int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction, - WMMtype_SphericalHarmonicVariables * SphVariables, - WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults); +int WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables *SphVariables, + WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); int WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables * - SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults); + SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); float WMM_get_main_field_coeff_g(uint16_t index); float WMM_get_main_field_coeff_h(uint16_t index); diff --git a/flight/libraries/inc/WorldMagModel.h b/flight/libraries/inc/WorldMagModel.h index 0e1405af9..6251399c3 100644 --- a/flight/libraries/inc/WorldMagModel.h +++ b/flight/libraries/inc/WorldMagModel.h @@ -27,7 +27,7 @@ #ifndef WORLDMAGMODEL_H_ #define WORLDMAGMODEL_H_ - // Exposed Function Prototypes +// Exposed Function Prototypes int WMM_Initialize(); int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, uint16_t Day, uint16_t Year, float B[3]); diff --git a/flight/libraries/inc/aes.h b/flight/libraries/inc/aes.h index 44e12ba58..43bcdb926 100644 --- a/flight/libraries/inc/aes.h +++ b/flight/libraries/inc/aes.h @@ -1,10 +1,9 @@ - #ifndef _AES_H_ #define _AES_H_ -#define N_ROW 4 -#define N_COL 4 -#define N_BLOCK (N_ROW * N_COL) +#define N_ROW 4 +#define N_COL 4 +#define N_BLOCK (N_ROW * N_COL) void aes_encrypt_cbc_128(void *data, void *key, void *chain_block); void aes_decrypt_cbc_128(void *data, void *key, void *chain_block); diff --git a/flight/libraries/inc/alarms.h b/flight/libraries/inc/alarms.h index 8bb4b8873..1d7a4ed3b 100644 --- a/flight/libraries/inc/alarms.h +++ b/flight/libraries/inc/alarms.h @@ -34,9 +34,9 @@ int32_t AlarmsInitialize(void); int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity); int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm, - SystemAlarmsAlarmOptions severity, - SystemAlarmsExtendedAlarmStatusOptions status, - uint8_t subStatus); + SystemAlarmsAlarmOptions severity, + SystemAlarmsExtendedAlarmStatusOptions status, + uint8_t subStatus); SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm); int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm); void AlarmsDefaultAll(); diff --git a/flight/libraries/inc/fifo_buffer.h b/flight/libraries/inc/fifo_buffer.h index 6ba85877e..d653529d7 100644 --- a/flight/libraries/inc/fifo_buffer.h +++ b/flight/libraries/inc/fifo_buffer.h @@ -30,9 +30,8 @@ // ********************* -typedef struct -{ - uint8_t *buf_ptr; +typedef struct { + uint8_t *buf_ptr; volatile uint16_t rd; volatile uint16_t wr; uint16_t buf_size; @@ -62,4 +61,4 @@ void fifoBuf_init(t_fifo_buffer *buf, const void *buffer, const uint16_t buffer_ // ********************* -#endif +#endif // ifndef _FIFO_BUFFER_H_ diff --git a/flight/libraries/inc/insgps.h b/flight/libraries/inc/insgps.h index ea8e11e23..c77d44510 100644 --- a/flight/libraries/inc/insgps.h +++ b/flight/libraries/inc/insgps.h @@ -1,6 +1,6 @@ /** ****************************************************************************** - * @addtogroup AHRS + * @addtogroup AHRS * @{ * @addtogroup INSGPS * @{ @@ -35,24 +35,24 @@ #include "stdint.h" /** - * @addtogroup Constants - * @{ - */ -#define POS_SENSORS 0x007 + * @addtogroup Constants + * @{ + */ +#define POS_SENSORS 0x007 #define HORIZ_POS_SENSORS 0x003 -#define VER_POS_SENSORS 0x004 -#define HORIZ_SENSORS 0x018 -#define VERT_SENSORS 0x020 -#define MAG_SENSORS 0x1C0 -#define BARO_SENSOR 0x200 +#define VER_POS_SENSORS 0x004 +#define HORIZ_SENSORS 0x018 +#define VERT_SENSORS 0x020 +#define MAG_SENSORS 0x1C0 +#define BARO_SENSOR 0x200 -#define FULL_SENSORS 0x3FF +#define FULL_SENSORS 0x3FF /** - * @} - */ + * @} + */ -// Exposed Function Prototypes +// Exposed Function Prototypes void INSGPSInit(); void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT); void INSCovariancePrediction(float dT); @@ -74,20 +74,20 @@ void INSPosVelReset(float pos[3], float vel[3]); void MagCorrection(float mag_data[3]); void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt); void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], - float BaroAlt); + float BaroAlt); void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt); void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]); void VelBaroCorrection(float Vel[3], float BaroAlt); uint16_t ins_get_num_states(); -// Nav structure containing current solution +// Nav structure containing current solution extern struct NavStruct { - float Pos[3]; // Position in meters and relative to a local NED frame - float Vel[3]; // Velocity in meters and in NED - float q[4]; // unit quaternion rotation relative to NED - float gyro_bias[3]; - float accel_bias[3]; + float Pos[3]; // Position in meters and relative to a local NED frame + float Vel[3]; // Velocity in meters and in NED + float q[4]; // unit quaternion rotation relative to NED + float gyro_bias[3]; + float accel_bias[3]; } Nav; /** diff --git a/flight/libraries/inc/op_dfu.h b/flight/libraries/inc/op_dfu.h index 262852fc4..356e6827c 100644 --- a/flight/libraries/inc/op_dfu.h +++ b/flight/libraries/inc/op_dfu.h @@ -30,23 +30,23 @@ /* Includes ------------------------------------------------------------------*/ /* Exported types ------------------------------------------------------------*/ typedef struct { - uint8_t programmingType; - uint8_t readWriteFlags; - uint32_t startOfUserCode; - uint32_t sizeOfCode; - uint8_t sizeOfDescription; - uint8_t BL_Version; - uint16_t devID; - DeviceType devType; - uint32_t FW_Crc; + uint8_t programmingType; + uint8_t readWriteFlags; + uint32_t startOfUserCode; + uint32_t sizeOfCode; + uint8_t sizeOfDescription; + uint8_t BL_Version; + uint16_t devID; + DeviceType devType; + uint32_t FW_Crc; } Device; /* Exported constants --------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ /* Exported define -----------------------------------------------------------*/ -#define COMMAND 0 -#define COUNT 1 -#define DATA 5 +#define COMMAND 0 +#define COUNT 1 +#define DATA 5 /* Exported functions ------------------------------------------------------- */ void processComand(uint8_t *Receive_Buffer); @@ -54,7 +54,7 @@ uint32_t baseOfAdressType(uint8_t type); uint8_t isBiggerThanAvailable(uint8_t type, uint32_t size); void OPDfuIni(uint8_t discover); void DataDownload(DownloadAction); -bool flash_read(uint8_t * buffer, uint32_t adr, DFUProgType type); +bool flash_read(uint8_t *buffer, uint32_t adr, DFUProgType type); #endif /* __OP_DFU_H */ /******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/ diff --git a/flight/libraries/inc/packet_handler.h b/flight/libraries/inc/packet_handler.h index 855347a20..3df4f8609 100644 --- a/flight/libraries/inc/packet_handler.h +++ b/flight/libraries/inc/packet_handler.h @@ -1,16 +1,16 @@ /** -****************************************************************************** -* @addtogroup OpenPilotSystem OpenPilot System -* @{ -* @addtogroup OpenPilotLibraries OpenPilot System Libraries -* @{ -* -* @file packet_handler.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A packet handler for handeling radio packet transmission. -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotLibraries OpenPilot System Libraries + * @{ + * + * @file packet_handler.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A packet handler for handeling radio packet transmission. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -36,70 +36,70 @@ #include // Public defines / macros -#define PHPacketSize(p) ((uint8_t*)(p->data) + p->header.data_size - (uint8_t*)p) -#define PHPacketSizeECC(p) ((uint8_t*)(p->data) + p->header.data_size + RS_ECC_NPARITY - (uint8_t*)p) +#define PHPacketSize(p) ((uint8_t *)(p->data) + p->header.data_size - (uint8_t *)p) +#define PHPacketSizeECC(p) ((uint8_t *)(p->data) + p->header.data_size + RS_ECC_NPARITY - (uint8_t *)p) // Public types typedef enum { PACKET_TYPE_NONE = 0, - PACKET_TYPE_STATUS, // broadcasts status of this modem - PACKET_TYPE_CON_REQUEST, // request a connection to another modem - PACKET_TYPE_DATA, // data packet (packet contains user data) - PACKET_TYPE_DUPLICATE_DATA, // a duplicate data packet - PACKET_TYPE_PPM, // PPM relay values - PACKET_TYPE_ACK, // Acknowlege the receipt of a packet - PACKET_TYPE_NACK, // Acknowlege the receipt of an uncorrectable packet + PACKET_TYPE_STATUS, // broadcasts status of this modem + PACKET_TYPE_CON_REQUEST, // request a connection to another modem + PACKET_TYPE_DATA, // data packet (packet contains user data) + PACKET_TYPE_DUPLICATE_DATA, // a duplicate data packet + PACKET_TYPE_PPM, // PPM relay values + PACKET_TYPE_ACK, // Acknowlege the receipt of a packet + PACKET_TYPE_NACK, // Acknowlege the receipt of an uncorrectable packet } PHPacketType; typedef struct { - uint32_t destination_id; + uint32_t destination_id; portTickType prev_tx_time; - uint16_t seq_num; + uint16_t seq_num; uint8_t type; uint8_t data_size; } PHPacketHeader; #define PH_MAX_DATA (PIOS_PH_MAX_PACKET - sizeof(PHPacketHeader) - RS_ECC_NPARITY) -#define PH_PACKET_SIZE(p) ((p)->data + (p)->header.data_size - (uint8_t*)(p) + RS_ECC_NPARITY) +#define PH_PACKET_SIZE(p) ((p)->data + (p)->header.data_size - (uint8_t *)(p) + RS_ECC_NPARITY) typedef struct { PHPacketHeader header; uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY]; } PHPacket, *PHPacketHandle; -#define PH_ACK_NACK_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) +#define PH_ACK_NACK_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; uint8_t ecc[RS_ECC_NPARITY]; } PHAckNackPacket, *PHAckNackPacketHandle; -#define PH_PPM_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) +#define PH_PPM_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; int16_t channels[PIOS_RFM22B_RCVR_MAX_CHANNELS]; uint8_t ecc[RS_ECC_NPARITY]; } PHPpmPacket, *PHPpmPacketHandle; -#define PH_STATUS_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) +#define PH_STATUS_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; uint32_t source_id; - uint8_t link_quality; - int8_t received_rssi; - uint8_t ecc[RS_ECC_NPARITY]; + uint8_t link_quality; + int8_t received_rssi; + uint8_t ecc[RS_ECC_NPARITY]; } PHStatusPacket, *PHStatusPacketHandle; -#define PH_CONNECTION_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) +#define PH_CONNECTION_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; - uint32_t source_id; - uint32_t min_frequency; - uint32_t max_frequency; - uint32_t channel_spacing; + uint32_t source_id; + uint32_t min_frequency; + uint32_t max_frequency; + uint32_t channel_spacing; portTickType status_rx_time; - OPLinkSettingsMainPortOptions main_port; + OPLinkSettingsMainPortOptions main_port; OPLinkSettingsFlexiPortOptions flexi_port; - OPLinkSettingsVCPPortOptions vcp_port; - OPLinkSettingsComSpeedOptions com_speed; + OPLinkSettingsVCPPortOptions vcp_port; + OPLinkSettingsComSpeedOptions com_speed; uint8_t ecc[RS_ECC_NPARITY]; } PHConnectionPacket, *PHConnectionPacketHandle; diff --git a/flight/libraries/inc/paths.h b/flight/libraries/inc/paths.h index 39ed104cd..bbb3995fe 100644 --- a/flight/libraries/inc/paths.h +++ b/flight/libraries/inc/paths.h @@ -3,7 +3,7 @@ * * @file paths.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Header for path manipulation library + * @brief Header for path manipulation library * * @see The GNU Public License (GPL) Version 3 * @@ -28,12 +28,12 @@ #define PATHS_H_ struct path_status { - float fractional_progress; - float error; - float correction_direction[2]; - float path_direction[2]; + float fractional_progress; + float error; + float correction_direction[2]; + float path_direction[2]; }; -void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode); +void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode); #endif diff --git a/flight/libraries/inc/sanitycheck.h b/flight/libraries/inc/sanitycheck.h index 1ecaf8cc2..f324486ae 100644 --- a/flight/libraries/inc/sanitycheck.h +++ b/flight/libraries/inc/sanitycheck.h @@ -32,10 +32,10 @@ #include -#define SANITYCHECK_STATUS_ERROR_NONE SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE -#define SANITYCHECK_STATUS_ERROR_FLIGHTMODE SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE +#define SANITYCHECK_STATUS_ERROR_NONE SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE +#define SANITYCHECK_STATUS_ERROR_FLIGHTMODE SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE -#define BOOTFAULT_STATUS_ERROR_NONE SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE +#define BOOTFAULT_STATUS_ERROR_NONE SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE #define BOOTFAULT_STATUS_ERROR_REQUIRE_REBOOT SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED #if (SYSTEMALARMS_EXTENDEDALARMSTATUS_NUMELEM != SYSTEMALARMS_EXTENDEDALARMSUBSTATUS_NUMELEM) || \ diff --git a/flight/libraries/inc/stopwatch.h b/flight/libraries/inc/stopwatch.h index 0029a44ce..26922dff6 100644 --- a/flight/libraries/inc/stopwatch.h +++ b/flight/libraries/inc/stopwatch.h @@ -42,9 +42,9 @@ // Prototypes ///////////////////////////////////////////////////////////////////////////// -extern s32 STOPWATCH_Init(u32 resolution, TIM_TypeDef* TIM); -extern s32 STOPWATCH_Reset(TIM_TypeDef* TIM); -extern u32 STOPWATCH_ValueGet(TIM_TypeDef* TIM); +extern s32 STOPWATCH_Init(u32 resolution, TIM_TypeDef *TIM); +extern s32 STOPWATCH_Reset(TIM_TypeDef *TIM); +extern u32 STOPWATCH_ValueGet(TIM_TypeDef *TIM); ///////////////////////////////////////////////////////////////////////////// diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index b206f1772..414cbfb33 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -1,6 +1,6 @@ /** ****************************************************************************** - * @addtogroup AHRS + * @addtogroup AHRS * @{ * @addtogroup INSGPS * @{ @@ -35,21 +35,21 @@ #include // constants/macros/typdefs -#define NUMX 13 // number of states, X is the state vector -#define NUMW 9 // number of plant noise inputs, w is disturbance noise vector -#define NUMV 10 // number of measurements, v is the measurement noise vector -#define NUMU 6 // number of deterministic inputs, U is the input vector +#define NUMX 13 // number of states, X is the state vector +#define NUMW 9 // number of plant noise inputs, w is disturbance noise vector +#define NUMV 10 // number of measurements, v is the measurement noise vector +#define NUMU 6 // number of deterministic inputs, U is the input vector // Private functions void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], - float Q[NUMW], float dT, float P[NUMX][NUMX]); + float Q[NUMW], float dT, float P[NUMX][NUMX]); void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], - float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], float K[NUMX][NUMV], - uint16_t SensorsUsed); + float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], float K[NUMX][NUMV], + uint16_t SensorsUsed); void RungeKutta(float X[NUMX], float U[NUMU], float dT); void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]); void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX], - float G[NUMX][NUMW]); + float G[NUMX][NUMW]); void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]); void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); @@ -60,7 +60,7 @@ void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); // LinearizeFG() and LinearizeH(): // // usage F: usage G: usage H: -// 0123456789abc 012345678 0123456789abc +// 0123456789abc 012345678 0123456789abc // 0...X......... ......... X............ // 1....X........ ......... .X........... // 2.....X....... ......... ..X.......... @@ -75,747 +75,752 @@ void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); // b............. .......X. // c............. ........X -static const int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 7, 6, 6, 6,13,13,13 }; -static const int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9,12,12,12,12,-1,-1,-1 }; +static const int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 7, 6, 6, 6, 13, 13, 13 }; +static const int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 }; -static const int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 }; -static const int8_t GrowMax[NUMX] = { -1,-1,-1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 }; +static const int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 }; +static const int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 }; -static const int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 }; -static const int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 }; +static const int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 }; +static const int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 }; static struct EKFData { - // linearized system matrices - float F[NUMX][NUMX]; - float G[NUMX][NUMW]; - float H[NUMV][NUMX]; - // local magnetic unit vector in NED frame - float Be[3]; - // covariance matrix and state vector - float P[NUMX][NUMX]; - float X[NUMX]; - // input noise and measurement noise variances - float Q[NUMW]; - float R[NUMV]; - float K[NUMX][NUMV]; // feedback gain matrix + // linearized system matrices + float F[NUMX][NUMX]; + float G[NUMX][NUMW]; + float H[NUMV][NUMX]; + // local magnetic unit vector in NED frame + float Be[3]; + // covariance matrix and state vector + float P[NUMX][NUMX]; + float X[NUMX]; + // input noise and measurement noise variances + float Q[NUMW]; + float R[NUMV]; + float K[NUMX][NUMV]; // feedback gain matrix } ekf; // Global variables struct NavStruct Nav; -// ************* Exposed Functions **************** -// ************************************************* +// ************* Exposed Functions **************** +// ************************************************* -uint16_t ins_get_num_states() +uint16_t ins_get_num_states() { - return NUMX; + return NUMX; } -void INSGPSInit() //pretty much just a place holder for now +void INSGPSInit() // pretty much just a place holder for now { - ekf.Be[0] = 1.0f; - ekf.Be[1] = 0.0f; - ekf.Be[2] = 0.0f; // local magnetic unit vector + ekf.Be[0] = 1.0f; + ekf.Be[1] = 0.0f; + ekf.Be[2] = 0.0f; // local magnetic unit vector - for (int i = 0; i < NUMX; i++) { - for (int j = 0; j < NUMX; j++) { - ekf.P[i][j] = 0.0f; // zero all terms - ekf.F[i][j] = 0.0f; - } - - for (int j = 0; j < NUMW; j++) - ekf.G[i][j] = 0.0f; - - for (int j = 0; j < NUMV; j++) { - ekf.H[j][i] = 0.0f; - ekf.K[i][j] = 0.0f; - } - - ekf.X[i] = 0.0f; - } - for (int i = 0; i < NUMW; i++) - ekf.Q[i] = 0.0f; - for (int i = 0; i < NUMV; i++) - ekf.R[i] = 0.0f; + for (int i = 0; i < NUMX; i++) { + for (int j = 0; j < NUMX; j++) { + ekf.P[i][j] = 0.0f; // zero all terms + ekf.F[i][j] = 0.0f; + } - - ekf.P[0][0] = ekf.P[1][1] = ekf.P[2][2] = 25.0f; // initial position variance (m^2) - ekf.P[3][3] = ekf.P[4][4] = ekf.P[5][5] = 5.0f; // initial velocity variance (m/s)^2 - ekf.P[6][6] = ekf.P[7][7] = ekf.P[8][8] = ekf.P[9][9] = 1e-5f; // initial quaternion variance - ekf.P[10][10] = ekf.P[11][11] = ekf.P[12][12] = 1e-9f; // initial gyro bias variance (rad/s)^2 + for (int j = 0; j < NUMW; j++) { + ekf.G[i][j] = 0.0f; + } - ekf.X[0] = ekf.X[1] = ekf.X[2] = ekf.X[3] = ekf.X[4] = ekf.X[5] = 0.0f; // initial pos and vel (m) - ekf.X[6] = 1.0f; - ekf.X[7] = ekf.X[8] = ekf.X[9] = 0.0f; // initial quaternion (level and North) (m/s) - ekf.X[10] = ekf.X[11] = ekf.X[12] = 0.0f; // initial gyro bias (rad/s) + for (int j = 0; j < NUMV; j++) { + ekf.H[j][i] = 0.0f; + ekf.K[i][j] = 0.0f; + } - ekf.Q[0] = ekf.Q[1] = ekf.Q[2] = 50e-4f; // gyro noise variance (rad/s)^2 - ekf.Q[3] = ekf.Q[4] = ekf.Q[5] = 0.00001f; // accelerometer noise variance (m/s^2)^2 - ekf.Q[6] = ekf.Q[7] = ekf.Q[8] = 2e-8f; // gyro bias random walk variance (rad/s^2)^2 + ekf.X[i] = 0.0f; + } + for (int i = 0; i < NUMW; i++) { + ekf.Q[i] = 0.0f; + } + for (int i = 0; i < NUMV; i++) { + ekf.R[i] = 0.0f; + } - ekf.R[0] = ekf.R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2) - ekf.R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2) - ekf.R[3] = ekf.R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2 - ekf.R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2 - ekf.R[6] = ekf.R[7] = ekf.R[8] = 0.005f; // magnetometer unit vector noise variance - ekf.R[9] = .25f; // High freq altimeter noise variance (m^2) + + ekf.P[0][0] = ekf.P[1][1] = ekf.P[2][2] = 25.0f; // initial position variance (m^2) + ekf.P[3][3] = ekf.P[4][4] = ekf.P[5][5] = 5.0f; // initial velocity variance (m/s)^2 + ekf.P[6][6] = ekf.P[7][7] = ekf.P[8][8] = ekf.P[9][9] = 1e-5f; // initial quaternion variance + ekf.P[10][10] = ekf.P[11][11] = ekf.P[12][12] = 1e-9f; // initial gyro bias variance (rad/s)^2 + + ekf.X[0] = ekf.X[1] = ekf.X[2] = ekf.X[3] = ekf.X[4] = ekf.X[5] = 0.0f; // initial pos and vel (m) + ekf.X[6] = 1.0f; + ekf.X[7] = ekf.X[8] = ekf.X[9] = 0.0f; // initial quaternion (level and North) (m/s) + ekf.X[10] = ekf.X[11] = ekf.X[12] = 0.0f; // initial gyro bias (rad/s) + + ekf.Q[0] = ekf.Q[1] = ekf.Q[2] = 50e-4f; // gyro noise variance (rad/s)^2 + ekf.Q[3] = ekf.Q[4] = ekf.Q[5] = 0.00001f; // accelerometer noise variance (m/s^2)^2 + ekf.Q[6] = ekf.Q[7] = ekf.Q[8] = 2e-8f; // gyro bias random walk variance (rad/s^2)^2 + + ekf.R[0] = ekf.R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2) + ekf.R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2) + ekf.R[3] = ekf.R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2 + ekf.R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2 + ekf.R[6] = ekf.R[7] = ekf.R[8] = 0.005f; // magnetometer unit vector noise variance + ekf.R[9] = .25f; // High freq altimeter noise variance (m^2) } void INSResetP(float PDiag[NUMX]) { - uint8_t i,j; + uint8_t i, j; - // if PDiag[i] nonzero then clear row and column and set diagonal element - for (i=0;i // constants/macros/typdefs -#define NUMX 16 // number of states, X is the state vector -#define NUMW 12 // number of plant noise inputs, w is disturbance noise vector -#define NUMV 10 // number of measurements, v is the measurement noise vector -#define NUMU 6 // number of deterministic inputs, U is the input vector +#define NUMX 16 // number of states, X is the state vector +#define NUMW 12 // number of plant noise inputs, w is disturbance noise vector +#define NUMV 10 // number of measurements, v is the measurement noise vector +#define NUMU 6 // number of deterministic inputs, U is the input vector #if defined(GENERAL_COV) -// This might trick people so I have a note here. There is a slower but bigger version of the +// This might trick people so I have a note here. There is a slower but bigger version of the // code here but won't fit when debugging disabled (requires -Os) #define COVARIANCE_PREDICTION_GENERAL #endif // Private functions void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], - float Q[NUMW], float dT, float P[NUMX][NUMX]); + float Q[NUMW], float dT, float P[NUMX][NUMX]); void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], - float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], - uint16_t SensorsUsed); + float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], + uint16_t SensorsUsed); void RungeKutta(float X[NUMX], float U[NUMU], float dT); void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]); void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX], - float G[NUMX][NUMW]); + float G[NUMX][NUMW]); void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]); void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); // Private variables -float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices - // global to init to zero and maintain zero elements -float Be[3]; // local magnetic unit vector in NED frame -float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector -float Q[NUMW], R[NUMV]; // input noise and measurement noise variances -float K[NUMX][NUMV]; // feedback gain matrix +float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices +// global to init to zero and maintain zero elements +float Be[3]; // local magnetic unit vector in NED frame +float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector +float Q[NUMW], R[NUMV]; // input noise and measurement noise variances +float K[NUMX][NUMV]; // feedback gain matrix -// ************* Exposed Functions **************** -// ************************************************* +// ************* Exposed Functions **************** +// ************************************************* -uint16_t ins_get_num_states() +uint16_t ins_get_num_states() { - return NUMX; + return NUMX; } -void INSGPSInit() //pretty much just a place holder for now +void INSGPSInit() // pretty much just a place holder for now { - Be[0] = 1.0f; - Be[1] = 0; - Be[2] = 0; // local magnetic unit vector + Be[0] = 1.0f; + Be[1] = 0; + Be[2] = 0; // local magnetic unit vector - for (int i = 0; i < NUMX; i++) { - for (int j = 0; j < NUMX; j++) { - P[i][j] = 0.0f; // zero all terms - } - } - - P[0][0] = P[1][1] = P[2][2] = 25.0f; // initial position variance (m^2) - P[3][3] = P[4][4] = P[5][5] = 5.0f; // initial velocity variance (m/s)^2 - P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5f; // initial quaternion variance - P[10][10] = P[11][11] = P[12][12] = 1e-5f; // initial gyro bias variance (rad/s)^2 + for (int i = 0; i < NUMX; i++) { + for (int j = 0; j < NUMX; j++) { + P[i][j] = 0.0f; // zero all terms + } + } - X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0.0f; // initial pos and vel (m) - X[6] = 1.0f; - X[7] = X[8] = X[9] = 0.0f; // initial quaternion (level and North) (m/s) - X[10] = X[11] = X[12] = 0.0f; // initial gyro bias (rad/s) + P[0][0] = P[1][1] = P[2][2] = 25.0f; // initial position variance (m^2) + P[3][3] = P[4][4] = P[5][5] = 5.0f; // initial velocity variance (m/s)^2 + P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5f; // initial quaternion variance + P[10][10] = P[11][11] = P[12][12] = 1e-5f; // initial gyro bias variance (rad/s)^2 - Q[0] = Q[1] = Q[2] = 50e-8f; // gyro noise variance (rad/s)^2 - Q[3] = Q[4] = Q[5] = 0.01f; // accelerometer noise variance (m/s^2)^2 - Q[6] = Q[7] = Q[8] = 2e-9f; // gyro bias random walk variance (rad/s^2)^2 - Q[9] = Q[10] = Q[11] = 2e-20f; // accel bias random walk variance (m/s^3)^2 + X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0.0f; // initial pos and vel (m) + X[6] = 1.0f; + X[7] = X[8] = X[9] = 0.0f; // initial quaternion (level and North) (m/s) + X[10] = X[11] = X[12] = 0.0f; // initial gyro bias (rad/s) - R[0] = R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2) - R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2) - R[3] = R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2 - R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2 - R[6] = R[7] = R[8] = 0.005f; // magnetometer unit vector noise variance - R[9] = .05f; // High freq altimeter noise variance (m^2) + Q[0] = Q[1] = Q[2] = 50e-8f; // gyro noise variance (rad/s)^2 + Q[3] = Q[4] = Q[5] = 0.01f; // accelerometer noise variance (m/s^2)^2 + Q[6] = Q[7] = Q[8] = 2e-9f; // gyro bias random walk variance (rad/s^2)^2 + Q[9] = Q[10] = Q[11] = 2e-20f; // accel bias random walk variance (m/s^3)^2 + + R[0] = R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2) + R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2) + R[3] = R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2 + R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2 + R[6] = R[7] = R[8] = 0.005f; // magnetometer unit vector noise variance + R[9] = .05f; // High freq altimeter noise variance (m^2) } void INSResetP(float PDiag[NUMX]) { - uint8_t i,j; + uint8_t i, j; - // if PDiag[i] nonzero then clear row and column and set diagonal element - for (i=0;i 0.01) - dT = 0.01; - - // format data for INS algo - gyro[0] = gyro_data.filtered.x; - gyro[1] = gyro_data.filtered.y; - gyro[2] = gyro_data.filtered.z; - accel[0] = accel_data.filtered.x, - accel[1] = accel_data.filtered.y, - accel[2] = accel_data.filtered.z, - - INSStatePrediction(gyro, accel, dT); - attitude_data.quaternion.q1 = Nav.q[0]; - attitude_data.quaternion.q2 = Nav.q[1]; - attitude_data.quaternion.q3 = Nav.q[2]; - attitude_data.quaternion.q4 = Nav.q[3]; - send_attitude(); // get message out quickly - INSCovariancePrediction(dT); + static uint32_t ins_last_time; + float gyro[3], accel[3], vel[3]; + float dT; + uint16_t sensors; + static uint32_t gps_far_count = 0; - PositionActualData positionActual; - PositionActualGet(&positionActual); - positionActual.North = Nav.Pos[0]; - positionActual.East = Nav.Pos[1]; - positionActual.Down = Nav.Pos[2]; - PositionActualSet(&positionActual); - - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); - velocityActual.North = Nav.Vel[0]; - velocityActual.East = Nav.Vel[1]; - velocityActual.Down = Nav.Vel[2]; - VelocityActualSet(&velocityActual); + dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6; + ins_last_time = PIOS_DELAY_GetRaw(); - sensors = 0; - - if (gps_data.updated) - { - vel[0] = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD); - vel[1] = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD); - vel[2] = 0; - - if (abs(gps_data.NED[0] - Nav.Pos[0]) > INSGPS_GPS_FAR || - abs(gps_data.NED[1] - Nav.Pos[1]) > INSGPS_GPS_FAR || - abs(gps_data.NED[2] - Nav.Pos[2]) > INSGPS_GPS_FAR || - abs(vel[0] - Nav.Vel[0]) > INSGPS_GPS_FAR || - abs(vel[1] - Nav.Vel[1]) > INSGPS_GPS_FAR) { - gps_far_count++; - total_far_count++; - gps_data.updated = false; - - if(gps_far_count > 30) { - INSPosVelReset(gps_data.NED,vel); - relocated++; - } - } - else { - sensors |= HORIZ_SENSORS | POS_SENSORS; - - /* - * When using gps need to make sure that barometer is brought into NED frame - * we should try and see if the altitude from the home location is good enough - * to use for the offset but for now starting with this conservative filter - */ - if(fabs(gps_data.NED[2] + (altitude_data.altitude - baro_offset)) > 10) { - baro_offset = gps_data.NED[2] + altitude_data.altitude; - } else { - /* IIR filter with 100 second or so tau to keep them crudely in the same frame */ - baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001; - } - gps_data.updated = false; - } - } - - if(mag_data.updated) { - sensors |= MAG_SENSORS; - mag_data.updated = false; - } - - if(altitude_data.updated) { - sensors |= BARO_SENSOR; - altitude_data.updated = false; - } - - /* - * TODO: Need to add a general sanity check for all the inputs to make sure their kosher - * although probably should occur within INS itself - */ - INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude - baro_offset, sensors); - - if(fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) { - float zeros[3] = {0,0,0}; - INSSetGyroBias(zeros); - } + // This should only happen at start up or at mode switches + if (dT > 0.01) { + dT = 0.01; + } + + // format data for INS algo + gyro[0] = gyro_data.filtered.x; + gyro[1] = gyro_data.filtered.y; + gyro[2] = gyro_data.filtered.z; + accel[0] = accel_data.filtered.x, + accel[1] = accel_data.filtered.y, + accel[2] = accel_data.filtered.z, + + INSStatePrediction(gyro, accel, dT); + attitude_data.quaternion.q1 = Nav.q[0]; + attitude_data.quaternion.q2 = Nav.q[1]; + attitude_data.quaternion.q3 = Nav.q[2]; + attitude_data.quaternion.q4 = Nav.q[3]; + send_attitude(); // get message out quickly + INSCovariancePrediction(dT); + + PositionActualData positionActual; + PositionActualGet(&positionActual); + positionActual.North = Nav.Pos[0]; + positionActual.East = Nav.Pos[1]; + positionActual.Down = Nav.Pos[2]; + PositionActualSet(&positionActual); + + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); + velocityActual.North = Nav.Vel[0]; + velocityActual.East = Nav.Vel[1]; + velocityActual.Down = Nav.Vel[2]; + VelocityActualSet(&velocityActual); + + sensors = 0; + + if (gps_data.updated) { + vel[0] = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD); + vel[1] = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD); + vel[2] = 0; + + if (abs(gps_data.NED[0] - Nav.Pos[0]) > INSGPS_GPS_FAR || + abs(gps_data.NED[1] - Nav.Pos[1]) > INSGPS_GPS_FAR || + abs(gps_data.NED[2] - Nav.Pos[2]) > INSGPS_GPS_FAR || + abs(vel[0] - Nav.Vel[0]) > INSGPS_GPS_FAR || + abs(vel[1] - Nav.Vel[1]) > INSGPS_GPS_FAR) { + gps_far_count++; + total_far_count++; + gps_data.updated = false; + + if (gps_far_count > 30) { + INSPosVelReset(gps_data.NED, vel); + relocated++; + } + } else { + sensors |= HORIZ_SENSORS | POS_SENSORS; + + /* + * When using gps need to make sure that barometer is brought into NED frame + * we should try and see if the altitude from the home location is good enough + * to use for the offset but for now starting with this conservative filter + */ + if (fabs(gps_data.NED[2] + (altitude_data.altitude - baro_offset)) > 10) { + baro_offset = gps_data.NED[2] + altitude_data.altitude; + } else { + /* IIR filter with 100 second or so tau to keep them crudely in the same frame */ + baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001; + } + gps_data.updated = false; + } + } + + if (mag_data.updated) { + sensors |= MAG_SENSORS; + mag_data.updated = false; + } + + if (altitude_data.updated) { + sensors |= BARO_SENSOR; + altitude_data.updated = false; + } + + /* + * TODO: Need to add a general sanity check for all the inputs to make sure their kosher + * although probably should occur within INS itself + */ + INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude - baro_offset, sensors); + + if (fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) { + float zeros[3] = { 0, 0, 0 }; + INSSetGyroBias(zeros); + } } /** @@ -159,101 +157,102 @@ void ins_outdoor_update() */ void ins_indoor_update() { - static uint32_t updated_without_gps = 0; - - float gyro[3], accel[3]; - float zeros[3] = {0, 0, 0}; - static uint32_t ins_last_time = 0; - uint16_t sensors = 0; - float dT; + static uint32_t updated_without_gps = 0; - dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6; - ins_last_time = PIOS_DELAY_GetRaw(); - - // This should only happen at start up or at mode switches - if(dT > 0.01) - dT = 0.01; + float gyro[3], accel[3]; + float zeros[3] = { 0, 0, 0 }; + static uint32_t ins_last_time = 0; + uint16_t sensors = 0; + float dT; - // format data for INS algo - gyro[0] = gyro_data.filtered.x; - gyro[1] = gyro_data.filtered.y; - gyro[2] = gyro_data.filtered.z; - accel[0] = accel_data.filtered.x, - accel[1] = accel_data.filtered.y, - accel[2] = accel_data.filtered.z, - - INSStatePrediction(gyro, accel, dT); - attitude_data.quaternion.q1 = Nav.q[0]; - attitude_data.quaternion.q2 = Nav.q[1]; - attitude_data.quaternion.q3 = Nav.q[2]; - attitude_data.quaternion.q4 = Nav.q[3]; - send_attitude(); // get message out quickly - INSCovariancePrediction(dT); - - /* Indoors, update with zero position and velocity and high covariance */ - sensors = HORIZ_SENSORS | VERT_SENSORS; - - if(mag_data.updated && (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR)) { - sensors |= MAG_SENSORS; - mag_data.updated = false; - } - - if(altitude_data.updated) { - sensors |= BARO_SENSOR; - altitude_data.updated = false; - } - - if(gps_data.updated) { - PositionActualData positionActual; - PositionActualGet(&positionActual); - positionActual.North = gps_data.NED[0]; - positionActual.East = gps_data.NED[1]; - positionActual.Down = Nav.Pos[2]; - PositionActualSet(&positionActual); - - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); - velocityActual.North = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD); - velocityActual.East = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD); - velocityActual.Down = Nav.Vel[2]; - VelocityActualSet(&velocityActual); - - updated_without_gps = 0; - gps_data.updated = false; - } else { - PositionActualData positionActual; - PositionActualGet(&positionActual); + dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6; + ins_last_time = PIOS_DELAY_GetRaw(); - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); + // This should only happen at start up or at mode switches + if (dT > 0.01) { + dT = 0.01; + } - positionActual.Down = Nav.Pos[2]; - velocityActual.Down = Nav.Vel[2]; + // format data for INS algo + gyro[0] = gyro_data.filtered.x; + gyro[1] = gyro_data.filtered.y; + gyro[2] = gyro_data.filtered.z; + accel[0] = accel_data.filtered.x, + accel[1] = accel_data.filtered.y, + accel[2] = accel_data.filtered.z, - if(updated_without_gps > 500) { - // After 2-3 seconds without a GPS update set velocity estimate to NAN - positionActual.North = NAN; - positionActual.East = NAN; - velocityActual.North = NAN; - velocityActual.East = NAN; - } else - updated_without_gps++; + INSStatePrediction(gyro, accel, dT); + attitude_data.quaternion.q1 = Nav.q[0]; + attitude_data.quaternion.q2 = Nav.q[1]; + attitude_data.quaternion.q3 = Nav.q[2]; + attitude_data.quaternion.q4 = Nav.q[3]; + send_attitude(); // get message out quickly + INSCovariancePrediction(dT); - PositionActualSet(&positionActual); - VelocityActualSet(&velocityActual); - } - - /* - * TODO: Need to add a general sanity check for all the inputs to make sure their kosher - * although probably should occur within INS itself - */ - INSCorrection(mag_data.scaled.axis, zeros, zeros, altitude_data.altitude, sensors); - - if(fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) { - float zeros[3] = {0,0,0}; - INSSetGyroBias(zeros); - } + /* Indoors, update with zero position and velocity and high covariance */ + sensors = HORIZ_SENSORS | VERT_SENSORS; + if (mag_data.updated && (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR)) { + sensors |= MAG_SENSORS; + mag_data.updated = false; + } + + if (altitude_data.updated) { + sensors |= BARO_SENSOR; + altitude_data.updated = false; + } + + if (gps_data.updated) { + PositionActualData positionActual; + PositionActualGet(&positionActual); + positionActual.North = gps_data.NED[0]; + positionActual.East = gps_data.NED[1]; + positionActual.Down = Nav.Pos[2]; + PositionActualSet(&positionActual); + + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); + velocityActual.North = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD); + velocityActual.East = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD); + velocityActual.Down = Nav.Vel[2]; + VelocityActualSet(&velocityActual); + + updated_without_gps = 0; + gps_data.updated = false; + } else { + PositionActualData positionActual; + PositionActualGet(&positionActual); + + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); + + positionActual.Down = Nav.Pos[2]; + velocityActual.Down = Nav.Vel[2]; + + if (updated_without_gps > 500) { + // After 2-3 seconds without a GPS update set velocity estimate to NAN + positionActual.North = NAN; + positionActual.East = NAN; + velocityActual.North = NAN; + velocityActual.East = NAN; + } else { + updated_without_gps++; + } + + PositionActualSet(&positionActual); + VelocityActualSet(&velocityActual); + } + + /* + * TODO: Need to add a general sanity check for all the inputs to make sure their kosher + * although probably should occur within INS itself + */ + INSCorrection(mag_data.scaled.axis, zeros, zeros, altitude_data.altitude, sensors); + + if (fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) { + float zeros[3] = { 0, 0, 0 }; + INSSetGyroBias(zeros); + } } /** @@ -263,63 +262,64 @@ bool inited = false; float init_q[4]; void ins_init_algorithm() { - inited = true; - float Rbe[3][3], q[4], accels[3], rpy[3], mag; - float ge[3]={0,0,-9.81}, zeros[3]={0,0,0}, Pdiag[16]={25,25,25,5,5,5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-4,1e-4,1e-4}; - bool using_mags, using_gps; - - INSGPSInit(); - - HomeLocationData home; - HomeLocationGet(&home); - - accels[0]=accel_data.filtered.x; - accels[1]=accel_data.filtered.y; - accels[2]=accel_data.filtered.z; - - using_mags = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR); - using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0); /* only use mags when valid home location */ - - using_gps = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality >= INSGPS_GPS_MINSAT); - - /* Block till a data update */ - get_accel_gyro_data(); - - /* Ensure we get mag data in a timely manner */ - uint16_t fail_count = 50; // 50 at 200 Hz is up to 0.25 sec - while(using_mags && !mag_data.updated && fail_count--) { - get_mag_data(); - get_accel_gyro_data(); - AhrsPoll(); - PIOS_DELAY_WaituS(2000); - } - using_mags &= mag_data.updated; - - if (using_mags) { - RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe); - R2Quaternion(Rbe,q); - if (using_gps) - INSSetState(gps_data.NED, zeros, q, zeros, zeros); - else - INSSetState(zeros, zeros, q, zeros, zeros); - } else { - // assume yaw = 0 - mag = VectorMagnitude(accels); - rpy[1] = asinf(-accels[0]/mag); - rpy[0] = atan2(accels[1]/mag,accels[2]/mag); - rpy[2] = 0; - RPY2Quaternion(rpy,init_q); - if (using_gps) - INSSetState(gps_data.NED, zeros, init_q, zeros, zeros); - else { - for (uint32_t i = 0; i < 5; i++) { - INSSetState(zeros, zeros, init_q, zeros, zeros); - ins_indoor_update(); - } - } - } - - INSResetP(Pdiag); - - // TODO: include initial estimate of gyro bias? + inited = true; + float Rbe[3][3], q[4], accels[3], rpy[3], mag; + float ge[3] = { 0, 0, -9.81 }, zeros[3] = { 0, 0, 0 }, Pdiag[16] = { 25, 25, 25, 5, 5, 5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-4, 1e-4, 1e-4 }; + bool using_mags, using_gps; + + INSGPSInit(); + + HomeLocationData home; + HomeLocationGet(&home); + + accels[0] = accel_data.filtered.x; + accels[1] = accel_data.filtered.y; + accels[2] = accel_data.filtered.z; + + using_mags = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR); + using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0); /* only use mags when valid home location */ + + using_gps = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality >= INSGPS_GPS_MINSAT); + + /* Block till a data update */ + get_accel_gyro_data(); + + /* Ensure we get mag data in a timely manner */ + uint16_t fail_count = 50; // 50 at 200 Hz is up to 0.25 sec + while (using_mags && !mag_data.updated && fail_count--) { + get_mag_data(); + get_accel_gyro_data(); + AhrsPoll(); + PIOS_DELAY_WaituS(2000); + } + using_mags &= mag_data.updated; + + if (using_mags) { + RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe); + R2Quaternion(Rbe, q); + if (using_gps) { + INSSetState(gps_data.NED, zeros, q, zeros, zeros); + } else { + INSSetState(zeros, zeros, q, zeros, zeros); + } + } else { + // assume yaw = 0 + mag = VectorMagnitude(accels); + rpy[1] = asinf(-accels[0] / mag); + rpy[0] = atan2(accels[1] / mag, accels[2] / mag); + rpy[2] = 0; + RPY2Quaternion(rpy, init_q); + if (using_gps) { + INSSetState(gps_data.NED, zeros, init_q, zeros, zeros); + } else { + for (uint32_t i = 0; i < 5; i++) { + INSSetState(zeros, zeros, init_q, zeros, zeros); + ins_indoor_update(); + } + } + } + + INSResetP(Pdiag); + + // TODO: include initial estimate of gyro bias? } diff --git a/flight/libraries/math/pid.c b/flight/libraries/math/pid.c index eed613a73..2797b3e14 100644 --- a/flight/libraries/math/pid.c +++ b/flight/libraries/math/pid.c @@ -32,13 +32,13 @@ #include "pid.h" #include -//! Private method +// ! Private method static float bound(float val, float range); -//! Store the shared time constant for the derivative cutoff. -static float deriv_tau = 7.9577e-3f; +// ! Store the shared time constant for the derivative cutoff. +static float deriv_tau = 7.9577e-3f; -//! Store the setpoint weight to apply for the derivative term +// ! Store the setpoint weight to apply for the derivative term static float deriv_gamma = 1.0f; /** @@ -49,22 +49,21 @@ static float deriv_gamma = 1.0f; * @returns Output the computed controller value */ float pid_apply(struct pid *pid, const float err, float dT) -{ - // Scale up accumulator by 1000 while computing to avoid losing precision - pid->iAccumulator += err * (pid->i * dT * 1000.0f); - pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); +{ + // Scale up accumulator by 1000 while computing to avoid losing precision + pid->iAccumulator += err * (pid->i * dT * 1000.0f); + pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); - // Calculate DT1 term - float diff = (err - pid->lastErr); - float dterm = 0; - pid->lastErr = err; - if(pid->d > 0.0f && dT > 0.0f) - { - dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); - pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) - } // 7.9577e-3 means 20 Hz f_cutoff - - return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm); + // Calculate DT1 term + float diff = (err - pid->lastErr); + float dterm = 0; + pid->lastErr = err; + if (pid->d > 0.0f && dT > 0.0f) { + dterm = pid->lastDer + dT / (dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); + pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) + } // 7.9577e-3 means 20 Hz f_cutoff + + return (err * pid->p) + pid->iAccumulator / 1000.0f + dterm; } /** @@ -80,23 +79,22 @@ float pid_apply(struct pid *pid, const float err, float dT) */ float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT) { - float err = setpoint - measured; - - // Scale up accumulator by 1000 while computing to avoid losing precision - pid->iAccumulator += err * (pid->i * dT * 1000.0f); - pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); + float err = setpoint - measured; - // Calculate DT1 term, - float dterm = 0; - float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr); - pid->lastErr = (deriv_gamma * setpoint - measured); - if(pid->d > 0.0f && dT > 0.0f) - { - dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); - pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) - } // 7.9577e-3 means 20 Hz f_cutoff - - return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm); + // Scale up accumulator by 1000 while computing to avoid losing precision + pid->iAccumulator += err * (pid->i * dT * 1000.0f); + pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); + + // Calculate DT1 term, + float dterm = 0; + float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr); + pid->lastErr = (deriv_gamma * setpoint - measured); + if (pid->d > 0.0f && dT > 0.0f) { + dterm = pid->lastDer + dT / (dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); + pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) + } // 7.9577e-3 means 20 Hz f_cutoff + + return (err * pid->p) + pid->iAccumulator / 1000.0f + dterm; } /** @@ -105,12 +103,13 @@ float pid_apply_setpoint(struct pid *pid, const float setpoint, const float meas */ void pid_zero(struct pid *pid) { - if (!pid) - return; + if (!pid) { + return; + } - pid->iAccumulator = 0; - pid->lastErr = 0; - pid->lastDer = 0; + pid->iAccumulator = 0; + pid->lastErr = 0; + pid->lastDer = 0; } /** @@ -120,8 +119,8 @@ void pid_zero(struct pid *pid) */ void pid_configure_derivative(float cutoff, float g) { - deriv_tau = 1.0f / (2 * M_PI_F * cutoff); - deriv_gamma = g; + deriv_tau = 1.0f / (2 * M_PI_F * cutoff); + deriv_gamma = g; } /** @@ -133,13 +132,14 @@ void pid_configure_derivative(float cutoff, float g) */ void pid_configure(struct pid *pid, float p, float i, float d, float iLim) { - if (!pid) - return; + if (!pid) { + return; + } - pid->p = p; - pid->i = i; - pid->d = d; - pid->iLim = iLim; + pid->p = p; + pid->i = i; + pid->d = d; + pid->iLim = iLim; } /** @@ -147,11 +147,10 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim) */ static float bound(float val, float range) { - if(val < -range) { - val = -range; - } else if(val > range) { - val = range; - } - return val; + if (val < -range) { + val = -range; + } else if (val > range) { + val = range; + } + return val; } - diff --git a/flight/libraries/math/pid.h b/flight/libraries/math/pid.h index 602f560bf..7872ac559 100644 --- a/flight/libraries/math/pid.h +++ b/flight/libraries/math/pid.h @@ -31,22 +31,22 @@ #ifndef PID_H #define PID_H -//! +// ! struct pid { - float p; - float i; - float d; - float iLim; - float iAccumulator; - float lastErr; - float lastDer; + float p; + float i; + float d; + float iLim; + float iAccumulator; + float lastErr; + float lastDer; }; -//! Methods to use the pid structures +// ! Methods to use the pid structures float pid_apply(struct pid *pid, const float err, float dT); float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT); void pid_zero(struct pid *pid); void pid_configure(struct pid *pid, float p, float i, float d, float iLim); void pid_configure_derivative(float cutoff, float gamma); -#endif /* PID_H */ \ No newline at end of file +#endif /* PID_H */ diff --git a/flight/libraries/math/sin_lookup.c b/flight/libraries/math/sin_lookup.c index 9d98e2e86..6233dc592 100644 --- a/flight/libraries/math/sin_lookup.c +++ b/flight/libraries/math/sin_lookup.c @@ -36,36 +36,36 @@ #define FLASH_TABLE #ifdef FLASH_TABLE - /** Version of the code which precomputes the lookup table in flash **/ +/** Version of the code which precomputes the lookup table in flash **/ - // This is a precomputed sin lookup table over 90 degrees that - const float sin_table[] = { - 0.000000f,0.017452f,0.034899f,0.052336f,0.069756f,0.087156f,0.104528f,0.121869f,0.139173f,0.156434f, - 0.173648f,0.190809f,0.207912f,0.224951f,0.241922f,0.258819f,0.275637f,0.292372f,0.309017f,0.325568f, - 0.342020f,0.358368f,0.374607f,0.390731f,0.406737f,0.422618f,0.438371f,0.453990f,0.469472f,0.484810f, - 0.500000f,0.515038f,0.529919f,0.544639f,0.559193f,0.573576f,0.587785f,0.601815f,0.615661f,0.629320f, - 0.642788f,0.656059f,0.669131f,0.681998f,0.694658f,0.707107f,0.719340f,0.731354f,0.743145f,0.754710f, - 0.766044f,0.777146f,0.788011f,0.798636f,0.809017f,0.819152f,0.829038f,0.838671f,0.848048f,0.857167f, - 0.866025f,0.874620f,0.882948f,0.891007f,0.898794f,0.906308f,0.913545f,0.920505f,0.927184f,0.933580f, - 0.939693f,0.945519f,0.951057f,0.956305f,0.961262f,0.965926f,0.970296f,0.974370f,0.978148f,0.981627f, - 0.984808f,0.987688f,0.990268f,0.992546f,0.994522f,0.996195f,0.997564f,0.998630f,0.999391f,0.999848f, - 1.000000f,0.999848f,0.999391f,0.998630f,0.997564f,0.996195f,0.994522f,0.992546f,0.990268f,0.987688f, - 0.984808f,0.981627f,0.978148f,0.974370f,0.970296f,0.965926f,0.961262f,0.956305f,0.951057f,0.945519f, - 0.939693f,0.933580f,0.927184f,0.920505f,0.913545f,0.906308f,0.898794f,0.891007f,0.882948f,0.874620f, - 0.866025f,0.857167f,0.848048f,0.838671f,0.829038f,0.819152f,0.809017f,0.798636f,0.788011f,0.777146f, - 0.766044f,0.754710f,0.743145f,0.731354f,0.719340f,0.707107f,0.694658f,0.681998f,0.669131f,0.656059f, - 0.642788f,0.629320f,0.615661f,0.601815f,0.587785f,0.573576f,0.559193f,0.544639f,0.529919f,0.515038f, - 0.500000f,0.484810f,0.469472f,0.453990f,0.438371f,0.422618f,0.406737f,0.390731f,0.374607f,0.358368f, - 0.342020f,0.325568f,0.309017f,0.292372f,0.275637f,0.258819f,0.241922f,0.224951f,0.207912f,0.190809f, - 0.173648f,0.156434f,0.139173f,0.121869f,0.104528f,0.087156f,0.069756f,0.052336f,0.034899f,0.017452f - }; +// This is a precomputed sin lookup table over 90 degrees that +const float sin_table[] = { + 0.000000f, 0.017452f, 0.034899f, 0.052336f, 0.069756f, 0.087156f, 0.104528f, 0.121869f, 0.139173f, 0.156434f, + 0.173648f, 0.190809f, 0.207912f, 0.224951f, 0.241922f, 0.258819f, 0.275637f, 0.292372f, 0.309017f, 0.325568f, + 0.342020f, 0.358368f, 0.374607f, 0.390731f, 0.406737f, 0.422618f, 0.438371f, 0.453990f, 0.469472f, 0.484810f, + 0.500000f, 0.515038f, 0.529919f, 0.544639f, 0.559193f, 0.573576f, 0.587785f, 0.601815f, 0.615661f, 0.629320f, + 0.642788f, 0.656059f, 0.669131f, 0.681998f, 0.694658f, 0.707107f, 0.719340f, 0.731354f, 0.743145f, 0.754710f, + 0.766044f, 0.777146f, 0.788011f, 0.798636f, 0.809017f, 0.819152f, 0.829038f, 0.838671f, 0.848048f, 0.857167f, + 0.866025f, 0.874620f, 0.882948f, 0.891007f, 0.898794f, 0.906308f, 0.913545f, 0.920505f, 0.927184f, 0.933580f, + 0.939693f, 0.945519f, 0.951057f, 0.956305f, 0.961262f, 0.965926f, 0.970296f, 0.974370f, 0.978148f, 0.981627f, + 0.984808f, 0.987688f, 0.990268f, 0.992546f, 0.994522f, 0.996195f, 0.997564f, 0.998630f, 0.999391f, 0.999848f, + 1.000000f, 0.999848f, 0.999391f, 0.998630f, 0.997564f, 0.996195f, 0.994522f, 0.992546f, 0.990268f, 0.987688f, + 0.984808f, 0.981627f, 0.978148f, 0.974370f, 0.970296f, 0.965926f, 0.961262f, 0.956305f, 0.951057f, 0.945519f, + 0.939693f, 0.933580f, 0.927184f, 0.920505f, 0.913545f, 0.906308f, 0.898794f, 0.891007f, 0.882948f, 0.874620f, + 0.866025f, 0.857167f, 0.848048f, 0.838671f, 0.829038f, 0.819152f, 0.809017f, 0.798636f, 0.788011f, 0.777146f, + 0.766044f, 0.754710f, 0.743145f, 0.731354f, 0.719340f, 0.707107f, 0.694658f, 0.681998f, 0.669131f, 0.656059f, + 0.642788f, 0.629320f, 0.615661f, 0.601815f, 0.587785f, 0.573576f, 0.559193f, 0.544639f, 0.529919f, 0.515038f, + 0.500000f, 0.484810f, 0.469472f, 0.453990f, 0.438371f, 0.422618f, 0.406737f, 0.390731f, 0.374607f, 0.358368f, + 0.342020f, 0.325568f, 0.309017f, 0.292372f, 0.275637f, 0.258819f, 0.241922f, 0.224951f, 0.207912f, 0.190809f, + 0.173648f, 0.156434f, 0.139173f, 0.121869f, 0.104528f, 0.087156f, 0.069756f, 0.052336f, 0.034899f, 0.017452f +}; int sin_lookup_initalize() { - return 0; + return 0; } -#else +#else /* ifdef FLASH_TABLE */ /** Version of the code which allocates the lookup table in heap **/ const int SIN_RESOLUTION = 180; @@ -73,21 +73,24 @@ const int SIN_RESOLUTION = 180; static float *sin_table; int sin_lookup_initalize() { - // Previously initialized - if (sin_table) - return 0; + // Previously initialized + if (sin_table) { + return 0; + } - sin_table = (float *) pvPortMalloc(sizeof(float) * SIN_RESOLUTION); - if (sin_table == NULL) - return -1; + sin_table = (float *)pvPortMalloc(sizeof(float) * SIN_RESOLUTION); + if (sin_table == NULL) { + return -1; + } - for(uint32_t i = 0; i < 180; i++) - sin_table[i] = sinf(DEG2RAD((float)i)); + for (uint32_t i = 0; i < 180; i++) { + sin_table[i] = sinf(DEG2RAD((float)i)); + } - return 0; + return 0; } -#endif +#endif /* ifdef FLASH_TABLE */ /** * Use the lookup table to return sine(angle) where angle is in radians @@ -95,19 +98,21 @@ int sin_lookup_initalize() * 90 values * @param[in] angle Angle in degrees * @returns sin(angle) -*/ + */ float sin_lookup_deg(float angle) { - if (sin_table == NULL) - return 0; + if (sin_table == NULL) { + return 0; + } - int i_ang = ((int32_t) angle) % 360; - if (i_ang >= 180) // for 180 to 360 deg - return -sin_table[i_ang - 180]; - else // for 0 to 179 deg - return sin_table[i_ang]; + int i_ang = ((int32_t)angle) % 360; + if (i_ang >= 180) { // for 180 to 360 deg + return -sin_table[i_ang - 180]; + } else { // for 0 to 179 deg + return sin_table[i_ang]; + } - return 0; + return 0; } /** @@ -117,7 +122,7 @@ float sin_lookup_deg(float angle) */ float cos_lookup_deg(float angle) { - return sin_lookup_deg(angle + 90); + return sin_lookup_deg(angle + 90); } /** @@ -127,8 +132,9 @@ float cos_lookup_deg(float angle) */ float sin_lookup_rad(float angle) { - int degrees = RAD2DEG(angle); - return sin_lookup_deg(degrees); + int degrees = RAD2DEG(angle); + + return sin_lookup_deg(degrees); } /** @@ -138,6 +144,7 @@ float sin_lookup_rad(float angle) */ float cos_lookup_rad(float angle) { - int degrees = RAD2DEG(angle); - return cos_lookup_deg(degrees); + int degrees = RAD2DEG(angle); + + return cos_lookup_deg(degrees); } diff --git a/flight/libraries/math/sin_lookup.h b/flight/libraries/math/sin_lookup.h index adc29f66a..8c10219a3 100644 --- a/flight/libraries/math/sin_lookup.h +++ b/flight/libraries/math/sin_lookup.h @@ -37,4 +37,4 @@ float cos_lookup_deg(float angle); float sin_lookup_rad(float angle); float cos_lookup_rad(float angle); -#endif \ No newline at end of file +#endif diff --git a/flight/libraries/op_dfu.c b/flight/libraries/op_dfu.c index e231388bc..882b106a8 100644 --- a/flight/libraries/op_dfu.c +++ b/flight/libraries/op_dfu.c @@ -33,11 +33,11 @@ #include "pios_bl_helper.h" #include "pios_com_msg.h" #include -//programmable devices +// programmable devices Device devicesTable[10]; uint8_t numberOfDevices = 0; -DFUProgType currentProgrammingDestination; //flash, flash_trough spi +DFUProgType currentProgrammingDestination; // flash, flash_trough spi uint8_t currentDeviceCanRead; uint8_t currentDeviceCanWrite; Device currentDevice; @@ -45,15 +45,15 @@ Device currentDevice; uint8_t Buffer[64]; uint8_t echoBuffer[64]; uint8_t SendBuffer[64]; -uint8_t Command = 0; -uint8_t EchoReqFlag = 0; -uint8_t EchoAnsFlag = 0; -uint8_t StartFlag = 0; -uint32_t Aditionals = 0; -uint32_t SizeOfTransfer = 0; -uint32_t Expected_CRC = 0; +uint8_t Command = 0; +uint8_t EchoReqFlag = 0; +uint8_t EchoAnsFlag = 0; +uint8_t StartFlag = 0; +uint32_t Aditionals = 0; +uint32_t SizeOfTransfer = 0; +uint32_t Expected_CRC = 0; uint8_t SizeOfLastPacket = 0; -uint32_t Next_Packet = 0; +uint32_t Next_Packet = 0; uint8_t TransferType; uint32_t Count = 0; uint32_t Data; @@ -63,422 +63,430 @@ uint8_t Data2; uint8_t Data3; uint32_t Opt[3]; -//Download vars +// Download vars uint32_t downSizeOfLastPacket = 0; uint32_t downPacketTotal = 0; -uint32_t downPacketCurrent = 0; +uint32_t downPacketCurrent = 0; DFUTransfer downType = 0; /* Extern variables ----------------------------------------------------------*/ extern DFUStates DeviceState; extern uint8_t JumpToApp; /* Private function prototypes -----------------------------------------------*/ /* Private functions ---------------------------------------------------------*/ -void sendData(uint8_t * buf, uint16_t size); +void sendData(uint8_t *buf, uint16_t size); uint32_t CalcFirmCRC(void); -void DataDownload(__attribute__((unused)) DownloadAction action) { - if ((DeviceState == downloading)) { - - uint8_t packetSize; - uint32_t offset; - uint32_t partoffset; - SendBuffer[0] = 0x01; - SendBuffer[1] = Download; - SendBuffer[2] = downPacketCurrent >> 24; - SendBuffer[3] = downPacketCurrent >> 16; - SendBuffer[4] = downPacketCurrent >> 8; - SendBuffer[5] = downPacketCurrent; - if (downPacketCurrent == downPacketTotal - 1) { - packetSize = downSizeOfLastPacket; - } else { - packetSize = 14; - } - for (uint8_t x = 0; x < packetSize; ++x) { - partoffset = (downPacketCurrent * 14 * 4) + (x * 4); - offset = baseOfAdressType(downType) + partoffset; - if (!flash_read(SendBuffer + (6 + x * 4), offset, - currentProgrammingDestination)) { - DeviceState = Last_operation_failed; - } - } - downPacketCurrent = downPacketCurrent + 1; - if (downPacketCurrent > downPacketTotal - 1) { - DeviceState = Last_operation_Success; - Aditionals = (uint32_t) Download; - } - sendData(SendBuffer + 1, 63); - } +void DataDownload(__attribute__((unused)) DownloadAction action) +{ + if ((DeviceState == downloading)) { + uint8_t packetSize; + uint32_t offset; + uint32_t partoffset; + SendBuffer[0] = 0x01; + SendBuffer[1] = Download; + SendBuffer[2] = downPacketCurrent >> 24; + SendBuffer[3] = downPacketCurrent >> 16; + SendBuffer[4] = downPacketCurrent >> 8; + SendBuffer[5] = downPacketCurrent; + if (downPacketCurrent == downPacketTotal - 1) { + packetSize = downSizeOfLastPacket; + } else { + packetSize = 14; + } + for (uint8_t x = 0; x < packetSize; ++x) { + partoffset = (downPacketCurrent * 14 * 4) + (x * 4); + offset = baseOfAdressType(downType) + partoffset; + if (!flash_read(SendBuffer + (6 + x * 4), offset, + currentProgrammingDestination)) { + DeviceState = Last_operation_failed; + } + } + downPacketCurrent = downPacketCurrent + 1; + if (downPacketCurrent > downPacketTotal - 1) { + DeviceState = Last_operation_Success; + Aditionals = (uint32_t)Download; + } + sendData(SendBuffer + 1, 63); + } } -void processComand(uint8_t *xReceive_Buffer) { - - Command = xReceive_Buffer[COMMAND]; +void processComand(uint8_t *xReceive_Buffer) +{ + Command = xReceive_Buffer[COMMAND]; #ifdef DEBUG_SSP - char str[63]= {0}; - sprintf(str,"Received COMMAND:%d|",Command); - PIOS_COM_SendString(PIOS_COM_TELEM_USB,str); + char str[63] = { 0 }; + sprintf(str, "Received COMMAND:%d|", Command); + PIOS_COM_SendString(PIOS_COM_TELEM_USB, str); #endif - EchoReqFlag = (Command >> 7); - EchoAnsFlag = (Command >> 6) & 0x01; - StartFlag = (Command >> 5) & 0x01; - Count = xReceive_Buffer[COUNT] << 24; - Count += xReceive_Buffer[COUNT + 1] << 16; - Count += xReceive_Buffer[COUNT + 2] << 8; - Count += xReceive_Buffer[COUNT + 3]; + EchoReqFlag = (Command >> 7); + EchoAnsFlag = (Command >> 6) & 0x01; + StartFlag = (Command >> 5) & 0x01; + Count = xReceive_Buffer[COUNT] << 24; + Count += xReceive_Buffer[COUNT + 1] << 16; + Count += xReceive_Buffer[COUNT + 2] << 8; + Count += xReceive_Buffer[COUNT + 3]; - Data = xReceive_Buffer[DATA] << 24; - Data += xReceive_Buffer[DATA + 1] << 16; - Data += xReceive_Buffer[DATA + 2] << 8; - Data += xReceive_Buffer[DATA + 3]; - Data0 = xReceive_Buffer[DATA]; - Data1 = xReceive_Buffer[DATA + 1]; - Data2 = xReceive_Buffer[DATA + 2]; - Data3 = xReceive_Buffer[DATA + 3]; - for( uint32_t i=0; i < 3; i++ ) - { - Opt[i] = xReceive_Buffer[DATA + 4 * (i+1) ] << 24 | - xReceive_Buffer[DATA + 4 * (i+1) + 1] << 16 | - xReceive_Buffer[DATA + 4 * (i+1) + 2] << 8 | - xReceive_Buffer[DATA + 4 * (i+1) + 3]; - } - - Command = Command & 0b00011111; + Data = xReceive_Buffer[DATA] << 24; + Data += xReceive_Buffer[DATA + 1] << 16; + Data += xReceive_Buffer[DATA + 2] << 8; + Data += xReceive_Buffer[DATA + 3]; + Data0 = xReceive_Buffer[DATA]; + Data1 = xReceive_Buffer[DATA + 1]; + Data2 = xReceive_Buffer[DATA + 2]; + Data3 = xReceive_Buffer[DATA + 3]; + for (uint32_t i = 0; i < 3; i++) { + Opt[i] = xReceive_Buffer[DATA + 4 * (i + 1)] << 24 | + xReceive_Buffer[DATA + 4 * (i + 1) + 1] << 16 | + xReceive_Buffer[DATA + 4 * (i + 1) + 2] << 8 | + xReceive_Buffer[DATA + 4 * (i + 1) + 3]; + } - if (EchoReqFlag == 1) { - memcpy(echoBuffer, xReceive_Buffer, 64); - } - switch (Command) { - case EnterDFU: - if (((DeviceState == BLidle) && (Data0 < numberOfDevices)) - || (DeviceState == DFUidle)) { - if (Data0 > 0) - OPDfuIni(true); - DeviceState = DFUidle; - currentProgrammingDestination = devicesTable[Data0].programmingType; - currentDeviceCanRead = devicesTable[Data0].readWriteFlags & 0x01; - currentDeviceCanWrite = devicesTable[Data0].readWriteFlags >> 1 - & 0x01; - currentDevice = devicesTable[Data0]; - uint8_t result = 0; - switch (currentProgrammingDestination) { - case Self_flash: - result = PIOS_BL_HELPER_FLASH_Ini(); - break; - case Remote_flash_via_spi: - result = true; - break; - default: - DeviceState = Last_operation_failed; - Aditionals = (uint16_t) Command; - } - if (result != 1) { - DeviceState = Last_operation_failed; - Aditionals = (uint32_t) Command; - } - } - break; - case Upload: - if ((DeviceState == DFUidle) || (DeviceState == uploading)) { - if ((StartFlag == 1) && (Next_Packet == 0)) { - TransferType = Data0; - SizeOfTransfer = Count; - Next_Packet = 1; - Expected_CRC = Data2 << 24; - Expected_CRC += Data3 << 16; - Expected_CRC += xReceive_Buffer[DATA + 4] << 8; - Expected_CRC += xReceive_Buffer[DATA + 5]; - SizeOfLastPacket = Data1; + Command = Command & 0b00011111; - if (isBiggerThanAvailable(TransferType, (SizeOfTransfer - 1) - * 14 * 4 + SizeOfLastPacket * 4) == true) { - DeviceState = outsideDevCapabilities; - Aditionals = (uint32_t) Command; - } else { - uint8_t result = 1; - if (TransferType == FW) { - switch (currentProgrammingDestination) { - case Self_flash: - result = PIOS_BL_HELPER_FLASH_Start(); - break; - case Remote_flash_via_spi: - result = false; - break; - default: - break; - } - } - if (result != 1) { - DeviceState = Last_operation_failed; - Aditionals = (uint32_t) Command; - } else { + if (EchoReqFlag == 1) { + memcpy(echoBuffer, xReceive_Buffer, 64); + } + switch (Command) { + case EnterDFU: + if (((DeviceState == BLidle) && (Data0 < numberOfDevices)) + || (DeviceState == DFUidle)) { + if (Data0 > 0) { + OPDfuIni(true); + } + DeviceState = DFUidle; + currentProgrammingDestination = devicesTable[Data0].programmingType; + currentDeviceCanRead = devicesTable[Data0].readWriteFlags & 0x01; + currentDeviceCanWrite = devicesTable[Data0].readWriteFlags >> 1 + & 0x01; + currentDevice = devicesTable[Data0]; + uint8_t result = 0; + switch (currentProgrammingDestination) { + case Self_flash: + result = PIOS_BL_HELPER_FLASH_Ini(); + break; + case Remote_flash_via_spi: + result = true; + break; + default: + DeviceState = Last_operation_failed; + Aditionals = (uint16_t)Command; + } + if (result != 1) { + DeviceState = Last_operation_failed; + Aditionals = (uint32_t)Command; + } + } + break; + case Upload: + if ((DeviceState == DFUidle) || (DeviceState == uploading)) { + if ((StartFlag == 1) && (Next_Packet == 0)) { + TransferType = Data0; + SizeOfTransfer = Count; + Next_Packet = 1; + Expected_CRC = Data2 << 24; + Expected_CRC += Data3 << 16; + Expected_CRC += xReceive_Buffer[DATA + 4] << 8; + Expected_CRC += xReceive_Buffer[DATA + 5]; + SizeOfLastPacket = Data1; - DeviceState = uploading; - } - } - } else if ((StartFlag != 1) && (Next_Packet != 0)) { - if (Count > SizeOfTransfer) { - DeviceState = too_many_packets; - Aditionals = Count; - } else if (Count == Next_Packet - 1) { - uint8_t numberOfWords = 14; - if (Count == SizeOfTransfer - 1)//is this the last packet? - { - numberOfWords = SizeOfLastPacket; - } - uint8_t result = 0; - uint32_t offset; - uint32_t aux;; - switch (currentProgrammingDestination) { - case Self_flash: - for (uint8_t x = 0; x < numberOfWords; ++x) { - offset = 4 * x; - Data = xReceive_Buffer[DATA + offset] << 24; - Data += xReceive_Buffer[DATA + 1 + offset] << 16; - Data += xReceive_Buffer[DATA + 2 + offset] << 8; - Data += xReceive_Buffer[DATA + 3 + offset]; - aux = baseOfAdressType(TransferType) + (uint32_t)( - Count * 14 * 4 + x * 4); - result = 0; - for (int retry = 0; retry < MAX_WRI_RETRYS; ++retry) { - if (result == 0) { - result = (FLASH_ProgramWord(aux, Data) - == FLASH_COMPLETE) ? 1 : 0; - } - } - } - break; - case Remote_flash_via_spi: - result = false; // No support for this for the PipX - break; - default: - result = 0; - break; - } - if (result != 1) { - DeviceState = Last_operation_failed; - Aditionals = (uint32_t) Command; - } + if (isBiggerThanAvailable(TransferType, (SizeOfTransfer - 1) + * 14 * 4 + SizeOfLastPacket * 4) == true) { + DeviceState = outsideDevCapabilities; + Aditionals = (uint32_t)Command; + } else { + uint8_t result = 1; + if (TransferType == FW) { + switch (currentProgrammingDestination) { + case Self_flash: + result = PIOS_BL_HELPER_FLASH_Start(); + break; + case Remote_flash_via_spi: + result = false; + break; + default: + break; + } + } + if (result != 1) { + DeviceState = Last_operation_failed; + Aditionals = (uint32_t)Command; + } else { + DeviceState = uploading; + } + } + } else if ((StartFlag != 1) && (Next_Packet != 0)) { + if (Count > SizeOfTransfer) { + DeviceState = too_many_packets; + Aditionals = Count; + } else if (Count == Next_Packet - 1) { + uint8_t numberOfWords = 14; + if (Count == SizeOfTransfer - 1) { // is this the last packet? + numberOfWords = SizeOfLastPacket; + } + uint8_t result = 0; + uint32_t offset; + uint32_t aux;; + switch (currentProgrammingDestination) { + case Self_flash: + for (uint8_t x = 0; x < numberOfWords; ++x) { + offset = 4 * x; + Data = xReceive_Buffer[DATA + offset] << 24; + Data += xReceive_Buffer[DATA + 1 + offset] << 16; + Data += xReceive_Buffer[DATA + 2 + offset] << 8; + Data += xReceive_Buffer[DATA + 3 + offset]; + aux = baseOfAdressType(TransferType) + (uint32_t)( + Count * 14 * 4 + x * 4); + result = 0; + for (int retry = 0; retry < MAX_WRI_RETRYS; ++retry) { + if (result == 0) { + result = (FLASH_ProgramWord(aux, Data) + == FLASH_COMPLETE) ? 1 : 0; + } + } + } + break; + case Remote_flash_via_spi: + result = false; // No support for this for the PipX + break; + default: + result = 0; + break; + } + if (result != 1) { + DeviceState = Last_operation_failed; + Aditionals = (uint32_t)Command; + } - ++Next_Packet; - } else { - DeviceState = wrong_packet_received; - Aditionals = Count; - } - } else { - DeviceState = Last_operation_failed; - Aditionals = (uint32_t) Command; - } - } - break; - case Req_Capabilities: - OPDfuIni(true); - Buffer[0] = 0x01; - Buffer[1] = Rep_Capabilities; - if (Data0 == 0) { - Buffer[2] = 0; - Buffer[3] = 0; - Buffer[4] = 0; - Buffer[5] = 0; - Buffer[6] = 0; - Buffer[7] = numberOfDevices; - uint16_t WRFlags = 0; - for (int x = 0; x < numberOfDevices; ++x) { - WRFlags = ((devicesTable[x].readWriteFlags << (x * 2)) - | WRFlags); - } - Buffer[8] = WRFlags >> 8; - Buffer[9] = WRFlags; - } else { - Buffer[2] = devicesTable[Data0 - 1].sizeOfCode >> 24; - Buffer[3] = devicesTable[Data0 - 1].sizeOfCode >> 16; - Buffer[4] = devicesTable[Data0 - 1].sizeOfCode >> 8; - Buffer[5] = devicesTable[Data0 - 1].sizeOfCode; - Buffer[6] = Data0; - Buffer[7] = devicesTable[Data0 - 1].BL_Version; - Buffer[8] = devicesTable[Data0 - 1].sizeOfDescription; - Buffer[9] = devicesTable[Data0 - 1].devID; - Buffer[10] = devicesTable[Data0 - 1].FW_Crc >> 24; - Buffer[11] = devicesTable[Data0 - 1].FW_Crc >> 16; - Buffer[12] = devicesTable[Data0 - 1].FW_Crc >> 8; - Buffer[13] = devicesTable[Data0 - 1].FW_Crc; - Buffer[14] = devicesTable[Data0 - 1].devID >> 8; - Buffer[15] = devicesTable[Data0 - 1].devID; - } - sendData(Buffer + 1, 63); - break; - case JumpFW: - if (Data == 0x5AFE) - { - /* Force board into safe mode */ - PIOS_IAP_WriteBootCount(0xFFFF); - } - // pass any Opt value to the firmware - PIOS_IAP_WriteBootCmd(0, Opt[0]); - PIOS_IAP_WriteBootCmd(1, Opt[1]); - PIOS_IAP_WriteBootCmd(2, Opt[2]); + ++Next_Packet; + } else { + DeviceState = wrong_packet_received; + Aditionals = Count; + } + } else { + DeviceState = Last_operation_failed; + Aditionals = (uint32_t)Command; + } + } + break; + case Req_Capabilities: + OPDfuIni(true); + Buffer[0] = 0x01; + Buffer[1] = Rep_Capabilities; + if (Data0 == 0) { + Buffer[2] = 0; + Buffer[3] = 0; + Buffer[4] = 0; + Buffer[5] = 0; + Buffer[6] = 0; + Buffer[7] = numberOfDevices; + uint16_t WRFlags = 0; + for (int x = 0; x < numberOfDevices; ++x) { + WRFlags = ((devicesTable[x].readWriteFlags << (x * 2)) + | WRFlags); + } + Buffer[8] = WRFlags >> 8; + Buffer[9] = WRFlags; + } else { + Buffer[2] = devicesTable[Data0 - 1].sizeOfCode >> 24; + Buffer[3] = devicesTable[Data0 - 1].sizeOfCode >> 16; + Buffer[4] = devicesTable[Data0 - 1].sizeOfCode >> 8; + Buffer[5] = devicesTable[Data0 - 1].sizeOfCode; + Buffer[6] = Data0; + Buffer[7] = devicesTable[Data0 - 1].BL_Version; + Buffer[8] = devicesTable[Data0 - 1].sizeOfDescription; + Buffer[9] = devicesTable[Data0 - 1].devID; + Buffer[10] = devicesTable[Data0 - 1].FW_Crc >> 24; + Buffer[11] = devicesTable[Data0 - 1].FW_Crc >> 16; + Buffer[12] = devicesTable[Data0 - 1].FW_Crc >> 8; + Buffer[13] = devicesTable[Data0 - 1].FW_Crc; + Buffer[14] = devicesTable[Data0 - 1].devID >> 8; + Buffer[15] = devicesTable[Data0 - 1].devID; + } + sendData(Buffer + 1, 63); + break; + case JumpFW: + if (Data == 0x5AFE) { + /* Force board into safe mode */ + PIOS_IAP_WriteBootCount(0xFFFF); + } + // pass any Opt value to the firmware + PIOS_IAP_WriteBootCmd(0, Opt[0]); + PIOS_IAP_WriteBootCmd(1, Opt[1]); + PIOS_IAP_WriteBootCmd(2, Opt[2]); - FLASH_Lock(); - JumpToApp = 1; - break; - case Reset: - PIOS_SYS_Reset(); - break; - case Abort_Operation: - Next_Packet = 0; - DeviceState = DFUidle; - break; + FLASH_Lock(); + JumpToApp = 1; + break; + case Reset: + PIOS_SYS_Reset(); + break; + case Abort_Operation: + Next_Packet = 0; + DeviceState = DFUidle; + break; - case Op_END: - if (DeviceState == uploading) { - if (Next_Packet - 1 == SizeOfTransfer) { - Next_Packet = 0; - if ((TransferType != FW) || (Expected_CRC == CalcFirmCRC())) { - DeviceState = Last_operation_Success; - } else { - DeviceState = CRC_Fail; - } - } - if (Next_Packet - 1 < SizeOfTransfer) { - Next_Packet = 0; - DeviceState = too_few_packets; - } - } - break; - case Download_Req: + case Op_END: + if (DeviceState == uploading) { + if (Next_Packet - 1 == SizeOfTransfer) { + Next_Packet = 0; + if ((TransferType != FW) || (Expected_CRC == CalcFirmCRC())) { + DeviceState = Last_operation_Success; + } else { + DeviceState = CRC_Fail; + } + } + if (Next_Packet - 1 < SizeOfTransfer) { + Next_Packet = 0; + DeviceState = too_few_packets; + } + } + break; + case Download_Req: #ifdef DEBUG_SSP - sprintf(str,"COMMAND:DOWNLOAD_REQ 1 Status=%d|",DeviceState); - PIOS_COM_SendString(PIOS_COM_TELEM_USB,str); + sprintf(str, "COMMAND:DOWNLOAD_REQ 1 Status=%d|", DeviceState); + PIOS_COM_SendString(PIOS_COM_TELEM_USB, str); #endif - if (DeviceState == DFUidle) { + if (DeviceState == DFUidle) { #ifdef DEBUG_SSP - PIOS_COM_SendString(PIOS_COM_TELEM_USB,"COMMAND:DOWNLOAD_REQ 1|"); + PIOS_COM_SendString(PIOS_COM_TELEM_USB, "COMMAND:DOWNLOAD_REQ 1|"); #endif - downType = Data0; - downPacketTotal = Count; - downSizeOfLastPacket = Data1; - if (isBiggerThanAvailable(downType, (downPacketTotal - 1) * 14 - + downSizeOfLastPacket) == 1) { - DeviceState = outsideDevCapabilities; - Aditionals = (uint32_t) Command; + downType = Data0; + downPacketTotal = Count; + downSizeOfLastPacket = Data1; + if (isBiggerThanAvailable(downType, (downPacketTotal - 1) * 14 + + downSizeOfLastPacket) == 1) { + DeviceState = outsideDevCapabilities; + Aditionals = (uint32_t)Command; + } else { + downPacketCurrent = 0; + DeviceState = downloading; + } + } else { + DeviceState = Last_operation_failed; + Aditionals = (uint32_t)Command; + } + break; - } else { - downPacketCurrent = 0; - DeviceState = downloading; - } - } else { - DeviceState = Last_operation_failed; - Aditionals = (uint32_t) Command; - } - break; + case Status_Request: + Buffer[0] = 0x01; + Buffer[1] = Status_Rep; + if (DeviceState == wrong_packet_received) { + Buffer[2] = Aditionals >> 24; + Buffer[3] = Aditionals >> 16; + Buffer[4] = Aditionals >> 8; + Buffer[5] = Aditionals; + } else { + Buffer[2] = 0; + Buffer[3] = ((uint16_t)Aditionals) >> 8; + Buffer[4] = ((uint16_t)Aditionals); + Buffer[5] = 0; + } + Buffer[6] = DeviceState; + Buffer[7] = 0; + Buffer[8] = 0; + Buffer[9] = 0; + sendData(Buffer + 1, 63); + if (DeviceState == Last_operation_Success) { + DeviceState = DFUidle; + } + break; + case Status_Rep: - case Status_Request: - Buffer[0] = 0x01; - Buffer[1] = Status_Rep; - if (DeviceState == wrong_packet_received) { - Buffer[2] = Aditionals >> 24; - Buffer[3] = Aditionals >> 16; - Buffer[4] = Aditionals >> 8; - Buffer[5] = Aditionals; - } else { - Buffer[2] = 0; - Buffer[3] = ((uint16_t) Aditionals) >> 8; - Buffer[4] = ((uint16_t) Aditionals); - Buffer[5] = 0; - } - Buffer[6] = DeviceState; - Buffer[7] = 0; - Buffer[8] = 0; - Buffer[9] = 0; - sendData(Buffer + 1, 63); - if (DeviceState == Last_operation_Success) { - DeviceState = DFUidle; - } - break; - case Status_Rep: - - break; - - } - if (EchoReqFlag == 1) { - echoBuffer[1] = echoBuffer[1] | EchoAnsFlag; - sendData(echoBuffer + 1, 63); - } - return; + break; + } + if (EchoReqFlag == 1) { + echoBuffer[1] = echoBuffer[1] | EchoAnsFlag; + sendData(echoBuffer + 1, 63); + } } -void OPDfuIni(uint8_t discover) { - const struct pios_board_info * bdinfo = &pios_board_info_blob; - Device dev; +void OPDfuIni(uint8_t discover) +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + Device dev; - dev.programmingType = Self_flash; - dev.readWriteFlags = (BOARD_READABLE | (BOARD_WRITABLE << 1)); - dev.startOfUserCode = bdinfo->fw_base; - dev.sizeOfCode = bdinfo->fw_size; - dev.sizeOfDescription = bdinfo->desc_size; - dev.BL_Version = bdinfo->bl_rev; - dev.FW_Crc = CalcFirmCRC(); - dev.devID = (bdinfo->board_type << 8) | (bdinfo->board_rev); - dev.devType = bdinfo->hw_type; - numberOfDevices = 1; - devicesTable[0] = dev; - if (discover) { - //TODO check other devices trough spi or whatever - } + dev.programmingType = Self_flash; + dev.readWriteFlags = (BOARD_READABLE | (BOARD_WRITABLE << 1)); + dev.startOfUserCode = bdinfo->fw_base; + dev.sizeOfCode = bdinfo->fw_size; + dev.sizeOfDescription = bdinfo->desc_size; + dev.BL_Version = bdinfo->bl_rev; + dev.FW_Crc = CalcFirmCRC(); + dev.devID = (bdinfo->board_type << 8) | (bdinfo->board_rev); + dev.devType = bdinfo->hw_type; + numberOfDevices = 1; + devicesTable[0] = dev; + if (discover) { + // TODO check other devices trough spi or whatever + } } -uint32_t baseOfAdressType(DFUTransfer type) { - switch (type) { - case FW: - return currentDevice.startOfUserCode; - break; - case Descript: - return currentDevice.startOfUserCode + currentDevice.sizeOfCode; - break; - default: +uint32_t baseOfAdressType(DFUTransfer type) +{ + switch (type) { + case FW: + return currentDevice.startOfUserCode; - return 0; - } + break; + case Descript: + return currentDevice.startOfUserCode + currentDevice.sizeOfCode; + + break; + default: + + return 0; + } } -uint8_t isBiggerThanAvailable(DFUTransfer type, uint32_t size) { - switch (type) { - case FW: - return (size > currentDevice.sizeOfCode) ? 1 : 0; - break; - case Descript: - return (size > currentDevice.sizeOfDescription) ? 1 : 0; - break; - default: - return true; - } +uint8_t isBiggerThanAvailable(DFUTransfer type, uint32_t size) +{ + switch (type) { + case FW: + return (size > currentDevice.sizeOfCode) ? 1 : 0; + + break; + case Descript: + return (size > currentDevice.sizeOfDescription) ? 1 : 0; + + break; + default: + return true; + } } -uint32_t CalcFirmCRC() { - switch (currentProgrammingDestination) { - case Self_flash: - return PIOS_BL_HELPER_CRC_Memory_Calc(); - break; - case Remote_flash_via_spi: - return 0; - break; - default: - return 0; - break; - } +uint32_t CalcFirmCRC() +{ + switch (currentProgrammingDestination) { + case Self_flash: + return PIOS_BL_HELPER_CRC_Memory_Calc(); + break; + case Remote_flash_via_spi: + return 0; + + break; + default: + return 0; + + break; + } } -void sendData(uint8_t * buf, uint16_t size) { - PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, buf, size); +void sendData(uint8_t *buf, uint16_t size) +{ + PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, buf, size); } -bool flash_read(uint8_t * buffer, uint32_t adr, DFUProgType type) { - switch (type) { - case Remote_flash_via_spi: - return false; // We should not get this for the PipX - break; - case Self_flash: - for (uint8_t x = 0; x < 4; ++x) { - buffer[x] = *PIOS_BL_HELPER_FLASH_If_Read(adr + x); - } - return true; - break; - default: - return false; - } +bool flash_read(uint8_t *buffer, uint32_t adr, DFUProgType type) +{ + switch (type) { + case Remote_flash_via_spi: + return false; // We should not get this for the PipX + + break; + case Self_flash: + for (uint8_t x = 0; x < 4; ++x) { + buffer[x] = *PIOS_BL_HELPER_FLASH_If_Read(adr + x); + } + return true; + + break; + default: + return false; + } } diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 4b9394672..1cbddb7d2 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -3,7 +3,7 @@ * * @file paths.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Library path manipulation + * @brief Library path manipulation * * @see The GNU Public License (GPL) Version 3 * @@ -30,13 +30,13 @@ #include "paths.h" #include "uavobjectmanager.h" // <--. -#include "pathdesired.h" //<-- needed only for correct ENUM macro usage with path modes (PATHDESIRED_MODE_xxx, +#include "pathdesired.h" // <-- needed only for correct ENUM macro usage with path modes (PATHDESIRED_MODE_xxx, // no direct UAVObject usage allowed in this file // private functions -static void path_endpoint(float * start_point, float * end_point, float * cur_point, struct path_status * status); -static void path_vector(float * start_point, float * end_point, float * cur_point, struct path_status * status); -static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise); +static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status); +static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status); +static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise); /** * @brief Compute progress along path and deviation from it @@ -46,27 +46,31 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin * @param[in] mode Path following mode * @param[out] status Structure containing progress along path and deviation */ -void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode) +void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode) { switch (mode) { - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_DRIVEVECTOR: - return path_vector(start_point, end_point, cur_point, status); - break; - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - return path_circle(start_point, end_point, cur_point, status, 1); - break; - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - return path_circle(start_point, end_point, cur_point, status, 0); - break; - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_DRIVEENDPOINT: - default: - // use the endpoint as default failsafe if called in unknown modes - return path_endpoint(start_point, end_point, cur_point, status); - break; + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_DRIVEVECTOR: + return path_vector(start_point, end_point, cur_point, status); + + break; + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_DRIVECIRCLERIGHT: + return path_circle(start_point, end_point, cur_point, status, 1); + + break; + case PATHDESIRED_MODE_FLYCIRCLELEFT: + case PATHDESIRED_MODE_DRIVECIRCLELEFT: + return path_circle(start_point, end_point, cur_point, status, 0); + + break; + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_DRIVEENDPOINT: + default: + // use the endpoint as default failsafe if called in unknown modes + return path_endpoint(start_point, end_point, cur_point, status); + + break; } } @@ -77,7 +81,7 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation */ -static void path_endpoint(float * start_point, float * end_point, float * cur_point, struct path_status * status) +static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status) { float path_north, path_east, diff_north, diff_east; float dist_path, dist_diff; @@ -87,19 +91,19 @@ static void path_endpoint(float * start_point, float * end_point, float * cur_po // Distance to go path_north = end_point[0] - start_point[0]; - path_east = end_point[1] - start_point[1]; + path_east = end_point[1] - start_point[1]; // Current progress location relative to end diff_north = end_point[0] - cur_point[0]; - diff_east = end_point[1] - cur_point[1]; + diff_east = end_point[1] - cur_point[1]; - dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east); - dist_path = sqrtf(path_north * path_north + path_east * path_east); + dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east); + dist_path = sqrtf(path_north * path_north + path_east * path_east); - if (dist_diff < 1e-6f ) { + if (dist_diff < 1e-6f) { status->fractional_progress = 1; status->error = 0; - status->path_direction[0] = status->path_direction[1] = 0; + status->path_direction[0] = status->path_direction[1] = 0; return; } @@ -109,7 +113,6 @@ static void path_endpoint(float * start_point, float * end_point, float * cur_po // Compute direction to travel status->path_direction[0] = diff_north / dist_diff; status->path_direction[1] = diff_east / dist_diff; - } /** @@ -119,7 +122,7 @@ static void path_endpoint(float * start_point, float * end_point, float * cur_po * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation */ -static void path_vector(float * start_point, float * end_point, float * cur_point, struct path_status * status) +static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status) { float path_north, path_east, diff_north, diff_east; float dist_path; @@ -128,16 +131,16 @@ static void path_vector(float * start_point, float * end_point, float * cur_poin // Distance to go path_north = end_point[0] - start_point[0]; - path_east = end_point[1] - start_point[1]; + path_east = end_point[1] - start_point[1]; // Current progress location relative to start diff_north = cur_point[0] - start_point[0]; - diff_east = cur_point[1] - start_point[1]; + diff_east = cur_point[1] - start_point[1]; dot = path_north * diff_north + path_east * diff_east; - dist_path = sqrtf(path_north * path_north + path_east * path_east); + dist_path = sqrtf(path_north * path_north + path_east * path_east); - if (dist_path < 1e-6f){ + if (dist_path < 1e-6f) { // if the path is too short, we cannot determine vector direction. // Fly towards the endpoint to prevent flying away, // but assume progress=1 either way. @@ -147,8 +150,8 @@ static void path_vector(float * start_point, float * end_point, float * cur_poin } // Compute the normal to the path - normal[0] = -path_east / dist_path; - normal[1] = path_north / dist_path; + normal[0] = -path_east / dist_path; + normal[1] = path_north / dist_path; status->fractional_progress = dot / (dist_path * dist_path); status->error = normal[0] * diff_north + normal[1] * diff_east; @@ -163,7 +166,6 @@ static void path_vector(float * start_point, float * end_point, float * cur_poin // Compute direction to travel status->path_direction[0] = path_north / dist_path; status->path_direction[1] = path_east / dist_path; - } /** @@ -173,7 +175,7 @@ static void path_vector(float * start_point, float * end_point, float * cur_poin * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation */ -static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise) +static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise) { float radius_north, radius_east, diff_north, diff_east; float radius, cradius; @@ -183,18 +185,18 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin // Radius radius_north = end_point[0] - start_point[0]; - radius_east = end_point[1] - start_point[1]; + radius_east = end_point[1] - start_point[1]; // Current location relative to center - diff_north = cur_point[0] - end_point[0]; - diff_east = cur_point[1] - end_point[1]; + diff_north = cur_point[0] - end_point[0]; + diff_east = cur_point[1] - end_point[1]; - radius = sqrtf(powf(radius_north, 2) + powf(radius_east, 2)); + radius = sqrtf(powf(radius_north, 2) + powf(radius_east, 2)); cradius = sqrtf(powf(diff_north, 2) + powf(diff_east, 2)); - if (cradius < 1e-6f) { + if (cradius < 1e-6f) { // cradius is zero, just fly somewhere and make sure correction is still a normal - status->fractional_progress = 1; + status->fractional_progress = 1; status->error = radius; status->correction_direction[0] = 0; status->correction_direction[1] = 1; @@ -214,7 +216,7 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin } // normalize progress to 0..1 - a_diff = atan2f(diff_north, diff_east); + a_diff = atan2f(diff_north, diff_east); a_radius = atan2f(radius_north, radius_east); if (a_diff < 0) { diff --git a/flight/libraries/printf-stdarg.c b/flight/libraries/printf-stdarg.c index b579b9d53..e4c01552d 100644 --- a/flight/libraries/printf-stdarg.c +++ b/flight/libraries/printf-stdarg.c @@ -5,7 +5,7 @@ * * @file printf-stdarg.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief Formatted print functions * @see The GNU Public License (GPL) Version 3 * @@ -32,213 +32,221 @@ If not, uncomment the define below and replace outbyte(c) by your own function call. -#define putchar(c) outbyte(c) -*/ -//#define putchar(c) COMSendChar(c) + #define putchar(c) outbyte(c) + */ +// #define putchar(c) COMSendChar(c) #include -static void printchar(char **str, int c) +static void printchar(char * *str, int c) { - if (str) { - **str = c; - ++(*str); - } + if (str) { + **str = c; + ++(*str); + } #ifdef PIOS_COM_DEBUG - else - PIOS_COM_SendChar(PIOS_COM_DEBUG, c); + else { + PIOS_COM_SendChar(PIOS_COM_DEBUG, c); + } #endif - } #define PAD_RIGHT 1 -#define PAD_ZERO 2 +#define PAD_ZERO 2 -static int prints(char **out, const char *string, int width, int pad) +static int prints(char * *out, const char *string, int width, int pad) { - register int pc = 0, padchar = ' '; + register int pc = 0, padchar = ' '; - if (width > 0) { - register int len = 0; - register const char *ptr; - for (ptr = string; *ptr; ++ptr) - ++len; - if (len >= width) - width = 0; - else - width -= len; - if (pad & PAD_ZERO) - padchar = '0'; - } - if (!(pad & PAD_RIGHT)) { - for (; width > 0; --width) { - printchar(out, padchar); - ++pc; - } - } - for (; *string; ++string) { - printchar(out, *string); - ++pc; - } - for (; width > 0; --width) { - printchar(out, padchar); - ++pc; - } + if (width > 0) { + register int len = 0; + register const char *ptr; + for (ptr = string; *ptr; ++ptr) { + ++len; + } + if (len >= width) { + width = 0; + } else { + width -= len; + } + if (pad & PAD_ZERO) { + padchar = '0'; + } + } + if (!(pad & PAD_RIGHT)) { + for (; width > 0; --width) { + printchar(out, padchar); + ++pc; + } + } + for (; *string; ++string) { + printchar(out, *string); + ++pc; + } + for (; width > 0; --width) { + printchar(out, padchar); + ++pc; + } - return pc; + return pc; } /* the following should be enough for 32 bit int */ #define PRINT_BUF_LEN 12 -static int printi(char **out, int i, int b, int sg, int width, int pad, int letbase) +static int printi(char * *out, int i, int b, int sg, int width, int pad, int letbase) { - char print_buf[PRINT_BUF_LEN]; - register char *s; - register int t, neg = 0, pc = 0; - register unsigned int u = i; + char print_buf[PRINT_BUF_LEN]; + register char *s; + register int t, neg = 0, pc = 0; + register unsigned int u = i; - if (i == 0) { - print_buf[0] = '0'; - print_buf[1] = '\0'; - return prints(out, print_buf, width, pad); - } + if (i == 0) { + print_buf[0] = '0'; + print_buf[1] = '\0'; + return prints(out, print_buf, width, pad); + } - if (sg && b == 10 && i < 0) { - neg = 1; - u = -i; - } + if (sg && b == 10 && i < 0) { + neg = 1; + u = -i; + } - s = print_buf + PRINT_BUF_LEN - 1; - *s = '\0'; + s = print_buf + PRINT_BUF_LEN - 1; + *s = '\0'; - while (u) { - t = u % b; - if (t >= 10) - t += letbase - '0' - 10; - *--s = t + '0'; - u /= b; - } + while (u) { + t = u % b; + if (t >= 10) { + t += letbase - '0' - 10; + } + *--s = t + '0'; + u /= b; + } - if (neg) { - if (width && (pad & PAD_ZERO)) { - printchar(out, '-'); - ++pc; - --width; - } else { - *--s = '-'; - } - } + if (neg) { + if (width && (pad & PAD_ZERO)) { + printchar(out, '-'); + ++pc; + --width; + } else { + *--s = '-'; + } + } - return pc + prints(out, s, width, pad); + return pc + prints(out, s, width, pad); } -static int print(char **out, const char *format, va_list args) +static int print(char * *out, const char *format, va_list args) { - register int width, pad; - register int pc = 0; - char scr[2]; + register int width, pad; + register int pc = 0; + char scr[2]; - for (; *format != 0; ++format) { - if (*format == '%') { - ++format; - width = pad = 0; - if (*format == '\0') - break; - if (*format == '%') - goto out; - if (*format == '-') { - ++format; - pad = PAD_RIGHT; - } - while (*format == '0') { - ++format; - pad |= PAD_ZERO; - } - for (; *format >= '0' && *format <= '9'; ++format) { - width *= 10; - width += *format - '0'; - } - if (*format == 's') { - register char *s = (char *)va_arg(args, int); - pc += prints(out, s ? s : "(null)", width, pad); - continue; - } - if (*format == 'd') { - pc += printi(out, va_arg(args, int), 10, 1, width, pad, 'a'); - continue; - } - if (*format == 'x') { - pc += printi(out, va_arg(args, int), 16, 0, width, pad, 'a'); - continue; - } - if (*format == 'X') { - pc += printi(out, va_arg(args, int), 16, 0, width, pad, 'A'); - continue; - } - if (*format == 'u') { - pc += printi(out, va_arg(args, int), 10, 0, width, pad, 'a'); - continue; - } - if (*format == 'c') { - /* char are converted to int then pushed on the stack */ - scr[0] = (char)va_arg(args, int); - scr[1] = '\0'; - pc += prints(out, scr, width, pad); - continue; - } - } else { + for (; *format != 0; ++format) { + if (*format == '%') { + ++format; + width = pad = 0; + if (*format == '\0') { + break; + } + if (*format == '%') { + goto out; + } + if (*format == '-') { + ++format; + pad = PAD_RIGHT; + } + while (*format == '0') { + ++format; + pad |= PAD_ZERO; + } + for (; *format >= '0' && *format <= '9'; ++format) { + width *= 10; + width += *format - '0'; + } + if (*format == 's') { + register char *s = (char *)va_arg(args, int); + pc += prints(out, s ? s : "(null)", width, pad); + continue; + } + if (*format == 'd') { + pc += printi(out, va_arg(args, int), 10, 1, width, pad, 'a'); + continue; + } + if (*format == 'x') { + pc += printi(out, va_arg(args, int), 16, 0, width, pad, 'a'); + continue; + } + if (*format == 'X') { + pc += printi(out, va_arg(args, int), 16, 0, width, pad, 'A'); + continue; + } + if (*format == 'u') { + pc += printi(out, va_arg(args, int), 10, 0, width, pad, 'a'); + continue; + } + if (*format == 'c') { + /* char are converted to int then pushed on the stack */ + scr[0] = (char)va_arg(args, int); + scr[1] = '\0'; + pc += prints(out, scr, width, pad); + continue; + } + } else { out: - printchar(out, *format); - ++pc; - } - } - if (out) - **out = '\0'; - va_end(args); - return pc; + printchar(out, *format); + ++pc; + } + } + if (out) { + **out = '\0'; + } + va_end(args); + return pc; } int printf(const char *format, ...) { - va_list args; + va_list args; - va_start(args, format); - return print(0, format, args); + va_start(args, format); + return print(0, format, args); } // TK: added for alternative parameter passing int vprintf(const char *format, va_list args) { - return print(0, format, args); + return print(0, format, args); } int sprintf(char *out, const char *format, ...) { - va_list args; + va_list args; - va_start(args, format); - return print(&out, format, args); + va_start(args, format); + return print(&out, format, args); } // TK: added for alternative parameter passing int vsprintf(char *out, const char *format, va_list args) { - char *_out; - _out = out; - return print(&_out, format, args); + char *_out; + + _out = out; + return print(&_out, format, args); } int snprintf(char *buf, size_t count, const char *format, ...) { - va_list args; + va_list args; - (void)count; + (void)count; - va_start(args, format); - return print(&buf, format, args); + va_start(args, format); + return print(&buf, format, args); } /** - * @} - */ + * @} + */ diff --git a/flight/libraries/printf2.c b/flight/libraries/printf2.c index dee058976..93d11f467 100644 --- a/flight/libraries/printf2.c +++ b/flight/libraries/printf2.c @@ -1,62 +1,62 @@ /******************************************************************************* - Copyright 2001, 2002 Georges Menie () - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU Lesser General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Lesser General Public License for more details. - You should have received a copy of the GNU Lesser General Public License - along with this program; if not, write to the Free Software - Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + Copyright 2001, 2002 Georges Menie () + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Lesser General Public License for more details. + You should have received a copy of the GNU Lesser General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ /******************************************************************************* - putchar is the only external dependency for this file, - if you have a working putchar, just remove the following - define. If the function should be called something else, - replace outbyte(c) by your own function call. -*/ -//******************************************************************************* -// Updated by Daniel D Miller. Changes to the original Menie code are -// Copyright 2009-2012 Daniel D Miller -// All such changes are distributed under the same license as the original, -// as described above. -// 11/06/09 - adding floating-point support -// 03/19/10 - pad fractional portion of floating-point number with 0s -// 03/30/10 - document the \% bug -// 07/20/10 - Fix a round-off bug in floating-point conversions -// ( 0.99 with %.1f did not round to 1.0 ) -// 10/25/11 - Add support for %+ format (always show + on positive numbers) -// 01/19/12 - fix handling of %f with no decimal; it was defaulting to 0 -// decimal places, rather than printf's 6. -//******************************************************************************* -// BUGS -// If '%' is included in a format string, in the form \% with the intent -// of including the percent sign in the output string, this function will -// mis-handle the data entirely!! -// Essentially, it will just discard the character following the percent sign. -// This bug is not easy to fix in the existing code; -// for now, I'll just try to remember to use %% instead of \% ... -//******************************************************************************* + putchar is the only external dependency for this file, + if you have a working putchar, just remove the following + define. If the function should be called something else, + replace outbyte(c) by your own function call. + */ +// ******************************************************************************* +// Updated by Daniel D Miller. Changes to the original Menie code are +// Copyright 2009-2012 Daniel D Miller +// All such changes are distributed under the same license as the original, +// as described above. +// 11/06/09 - adding floating-point support +// 03/19/10 - pad fractional portion of floating-point number with 0s +// 03/30/10 - document the \% bug +// 07/20/10 - Fix a round-off bug in floating-point conversions +// ( 0.99 with %.1f did not round to 1.0 ) +// 10/25/11 - Add support for %+ format (always show + on positive numbers) +// 01/19/12 - fix handling of %f with no decimal; it was defaulting to 0 +// decimal places, rather than printf's 6. +// ******************************************************************************* +// BUGS +// If '%' is included in a format string, in the form \% with the intent +// of including the percent sign in the output string, this function will +// mis-handle the data entirely!! +// Essentially, it will just discard the character following the percent sign. +// This bug is not easy to fix in the existing code; +// for now, I'll just try to remember to use %% instead of \% ... +// ******************************************************************************* -//lint -esym(752, debug_output) -//lint -esym(766, stdio.h) +// lint -esym(752, debug_output) +// lint -esym(766, stdio.h) // #define TEST_PRINTF 1 #include -static uint use_leading_plus = 0 ; +static uint use_leading_plus = 0; /* based on a example-code from Keil for CS G++ */ /* for caddr_t (typedef char * caddr_t;) */ #include -//* NEWLIB STUBS *// +// * NEWLIB STUBS *// #include #include #include @@ -69,7 +69,7 @@ static uint use_leading_plus = 0 ; * environment, this empty list is adequate: */ char *__env[1] = { 0 }; -char **environ = __env; +char * *environ = __env; /*============================================================================== * Close a file. @@ -82,7 +82,7 @@ int _close(__attribute__((unused)) int file) /*============================================================================== * Transfer control to a new process. */ -int _execve(__attribute__((unused)) char *name, __attribute__((unused)) char **argv, __attribute__((unused)) char **env) +int _execve(__attribute__((unused)) char *name, __attribute__((unused)) char * *argv, __attribute__((unused)) char * *env) { errno = ENOMEM; return -1; @@ -91,13 +91,12 @@ int _execve(__attribute__((unused)) char *name, __attribute__((unused)) char **a /*============================================================================== * Exit a program without cleaning up files. */ -void _exit( __attribute__((unused)) int code ) +void _exit(__attribute__((unused)) int code) { - /* Should we force a system reset? */ - while( 1 ) - { - ; - } + /* Should we force a system reset? */ + while (1) { + ; + } } /*============================================================================== @@ -182,7 +181,7 @@ int _read(__attribute__((unused)) int file, __attribute__((unused)) char *ptr, _ * example to a serial port for debugging, you should make your minimal write * capable of doing this. */ -int _write_r( __attribute__((unused)) void * reent, __attribute__((unused)) int file, __attribute__((unused)) char * ptr, __attribute__((unused)) int len ) +int _write_r(__attribute__((unused)) void *reent, __attribute__((unused)) int file, __attribute__((unused)) char *ptr, __attribute__((unused)) int len) { return 0; } @@ -195,26 +194,24 @@ int _write_r( __attribute__((unused)) void * reent, __attribute__((unused)) int */ caddr_t _sbrk(int incr) { - extern char _end; /* Defined by the linker */ + extern char _end; /* Defined by the linker */ static char *heap_end; char *prev_heap_end; - char * stack_ptr; + char *stack_ptr; - if (heap_end == 0) - { - heap_end = &_end; + if (heap_end == 0) { + heap_end = &_end; } prev_heap_end = heap_end; - asm volatile ("MRS %0, msp" : "=r" (stack_ptr) ); - if (heap_end + incr > stack_ptr) - { - _write_r ((void *)0, 1, "Heap and stack collision\n", 25); - _exit (1); + asm volatile ("MRS %0, msp" : "=r" (stack_ptr)); + if (heap_end + incr > stack_ptr) { + _write_r((void *)0, 1, "Heap and stack collision\n", 25); + _exit(1); } heap_end += incr; - return (caddr_t) prev_heap_end; + return (caddr_t)prev_heap_end; } /*============================================================================== @@ -251,383 +248,395 @@ int _wait(__attribute__((unused)) int *status) errno = ECHILD; return -1; } -//* NEWLIB STUBS *// +// * NEWLIB STUBS *// -//**************************************************************************** -static void printchar (char **str, int c) +// **************************************************************************** +static void printchar(char * *str, int c) { - if (str) { - **str = c; - ++(*str); - } + if (str) { + **str = c; + ++(*str); + } #ifdef TEST_PRINTF - else { - extern int putchar (int c); - (void) putchar (c); - } + else { + extern int putchar(int c); + (void)putchar(c); + } #endif } -//**************************************************************************** +// **************************************************************************** static uint my_strlen(char *str) { - if (str == 0) - return 0; - uint slen = 0 ; - while (*str != 0) { - slen++ ; - str++ ; - } - return slen; + if (str == 0) { + return 0; + } + uint slen = 0; + while (*str != 0) { + slen++; + str++; + } + return slen; } -//**************************************************************************** -// This version returns the length of the output string. -// It is more useful when implementing a walking-string function. -//**************************************************************************** +// **************************************************************************** +// This version returns the length of the output string. +// It is more useful when implementing a walking-string function. +// **************************************************************************** static const double round_nums[8] = { - 0.5L, - 0.05L, - 0.005L, - 0.0005L, - 0.00005L, - 0.000005L, - 0.0000005L, - 0.00000005L -} ; + 0.5L, + 0.05L, + 0.005L, + 0.0005L, + 0.00005L, + 0.000005L, + 0.0000005L, + 0.00000005L +}; static unsigned dbl2stri(char *outbfr, double dbl, unsigned dec_digits) { - static char local_bfr[128] ; - char *output = (outbfr == 0) ? local_bfr : outbfr ; + static char local_bfr[128]; + char *output = (outbfr == 0) ? local_bfr : outbfr; - //******************************************* - // extract negative info - //******************************************* - if (dbl < 0.0L) { - *output++ = '-' ; - dbl *= -1.0L ; - } else { - if (use_leading_plus) { - *output++ = '+' ; - } + // ******************************************* + // extract negative info + // ******************************************* + if (dbl < 0.0L) { + *output++ = '-'; + dbl *= -1.0L; + } else { + if (use_leading_plus) { + *output++ = '+'; + } + } - } + // handling rounding by adding .5LSB to the floating-point data + if (dec_digits < 8) { + dbl += round_nums[dec_digits]; + } - // handling rounding by adding .5LSB to the floating-point data - if (dec_digits < 8) { - dbl += round_nums[dec_digits] ; - } + // ************************************************************************** + // construct fractional multiplier for specified number of digits. + // ************************************************************************** + uint mult = 1; + uint idx; + for (idx = 0; idx < dec_digits; idx++) { + mult *= 10; + } - //************************************************************************** - // construct fractional multiplier for specified number of digits. - //************************************************************************** - uint mult = 1 ; - uint idx ; - for (idx=0; idx < dec_digits; idx++) - mult *= 10 ; + // printf("mult=%u\n", mult) ; + uint wholeNum = (uint)dbl; + uint decimalNum = (uint)((dbl - wholeNum) * mult); - // printf("mult=%u\n", mult) ; - uint wholeNum = (uint) dbl ; - uint decimalNum = (uint) ((dbl - wholeNum) * mult); + // ******************************************* + // convert integer portion + // ******************************************* + char tbfr[40]; + idx = 0; + while (wholeNum != 0) { + tbfr[idx++] = '0' + (wholeNum % 10); + wholeNum /= 10; + } + // printf("%.3f: whole=%s, dec=%d\n", dbl, tbfr, decimalNum) ; + if (idx == 0) { + *output++ = '0'; + } else { + while (idx > 0) { + *output++ = tbfr[idx - 1]; // lint !e771 + idx--; + } + } + if (dec_digits > 0) { + *output++ = '.'; - //******************************************* - // convert integer portion - //******************************************* - char tbfr[40] ; - idx = 0 ; - while (wholeNum != 0) { - tbfr[idx++] = '0' + (wholeNum % 10) ; - wholeNum /= 10 ; - } - // printf("%.3f: whole=%s, dec=%d\n", dbl, tbfr, decimalNum) ; - if (idx == 0) { - *output++ = '0' ; - } else { - while (idx > 0) { - *output++ = tbfr[idx-1] ; //lint !e771 - idx-- ; - } - } - if (dec_digits > 0) { - *output++ = '.' ; + // ******************************************* + // convert fractional portion + // ******************************************* + idx = 0; + while (decimalNum != 0) { + tbfr[idx++] = '0' + (decimalNum % 10); + decimalNum /= 10; + } + // pad the decimal portion with 0s as necessary; + // We wouldn't want to report 3.093 as 3.93, would we?? + while (idx < dec_digits) { + tbfr[idx++] = '0'; + } + // printf("decimal=%s\n", tbfr) ; + if (idx == 0) { + *output++ = '0'; + } else { + while (idx > 0) { + *output++ = tbfr[idx - 1]; + idx--; + } + } + } + *output = 0; - //******************************************* - // convert fractional portion - //******************************************* - idx = 0 ; - while (decimalNum != 0) { - tbfr[idx++] = '0' + (decimalNum % 10) ; - decimalNum /= 10 ; - } - // pad the decimal portion with 0s as necessary; - // We wouldn't want to report 3.093 as 3.93, would we?? - while (idx < dec_digits) { - tbfr[idx++] = '0' ; - } - // printf("decimal=%s\n", tbfr) ; - if (idx == 0) { - *output++ = '0' ; - } else { - while (idx > 0) { - *output++ = tbfr[idx-1] ; - idx-- ; - } - } - } - *output = 0 ; - - // prepare output - output = (outbfr == 0) ? local_bfr : outbfr ; - return my_strlen(output) ; + // prepare output + output = (outbfr == 0) ? local_bfr : outbfr; + return my_strlen(output); } -//**************************************************************************** -#define PAD_RIGHT 1 -#define PAD_ZERO 2 +// **************************************************************************** +#define PAD_RIGHT 1 +#define PAD_ZERO 2 -static int prints (char **out, const char *string, int width, int pad) +static int prints(char * *out, const char *string, int width, int pad) { - register int pc = 0, padchar = ' '; - if (width > 0) { - int len = 0; - const char *ptr; - for (ptr = string; *ptr; ++ptr) - ++len; - if (len >= width) - width = 0; - else - width -= len; - if (pad & PAD_ZERO) - padchar = '0'; - } - if (!(pad & PAD_RIGHT)) { - for (; width > 0; --width) { - printchar (out, padchar); - ++pc; - } - } - for (; *string; ++string) { - printchar (out, *string); - ++pc; - } - for (; width > 0; --width) { - printchar (out, padchar); - ++pc; - } - return pc; + register int pc = 0, padchar = ' '; + + if (width > 0) { + int len = 0; + const char *ptr; + for (ptr = string; *ptr; ++ptr) { + ++len; + } + if (len >= width) { + width = 0; + } else { + width -= len; + } + if (pad & PAD_ZERO) { + padchar = '0'; + } + } + if (!(pad & PAD_RIGHT)) { + for (; width > 0; --width) { + printchar(out, padchar); + ++pc; + } + } + for (; *string; ++string) { + printchar(out, *string); + ++pc; + } + for (; width > 0; --width) { + printchar(out, padchar); + ++pc; + } + return pc; } -//**************************************************************************** +// **************************************************************************** /* the following should be enough for 32 bit int */ #define PRINT_BUF_LEN 12 -static int printi (char **out, int i, int b, int sg, int width, int pad, int letbase) +static int printi(char * *out, int i, int b, int sg, int width, int pad, int letbase) { - char print_buf[PRINT_BUF_LEN]; - char *s; - int t, neg = 0, pc = 0; - unsigned u = (unsigned) i; - if (i == 0) { - print_buf[0] = '0'; - print_buf[1] = '\0'; - return prints (out, print_buf, width, pad); - } - if (sg && b == 10 && i < 0) { - neg = 1; - u = (unsigned) -i; - } - // make sure print_buf is NULL-term - s = print_buf + PRINT_BUF_LEN - 1; - *s = '\0'; + char print_buf[PRINT_BUF_LEN]; + char *s; + int t, neg = 0, pc = 0; + unsigned u = (unsigned)i; + + if (i == 0) { + print_buf[0] = '0'; + print_buf[1] = '\0'; + return prints(out, print_buf, width, pad); + } + if (sg && b == 10 && i < 0) { + neg = 1; + u = (unsigned)-i; + } + // make sure print_buf is NULL-term + s = print_buf + PRINT_BUF_LEN - 1; + *s = '\0'; - while (u) { - t = u % b; //lint !e573 Warning 573: Signed-unsigned mix with divide - if (t >= 10) - t += letbase - '0' - 10; - *--s = t + '0'; - u /= b; //lint !e573 Warning 573: Signed-unsigned mix with divide - } - if (neg) { - if (width && (pad & PAD_ZERO)) { - printchar (out, '-'); - ++pc; - --width; - } - else { - *--s = '-'; - } - } else { - if (use_leading_plus) { - *--s = '+'; - } - } - return pc + prints (out, s, width, pad); + while (u) { + t = u % b; // lint !e573 Warning 573: Signed-unsigned mix with divide + if (t >= 10) { + t += letbase - '0' - 10; + } + *--s = t + '0'; + u /= b; // lint !e573 Warning 573: Signed-unsigned mix with divide + } + if (neg) { + if (width && (pad & PAD_ZERO)) { + printchar(out, '-'); + ++pc; + --width; + } else { + *--s = '-'; + } + } else { + if (use_leading_plus) { + *--s = '+'; + } + } + return pc + prints(out, s, width, pad); } -//**************************************************************************** -static int print (char **out, int *varg) +// **************************************************************************** +static int print(char * *out, int *varg) { - int post_decimal ; - int width, pad ; - unsigned dec_width = 6 ; - int pc = 0; - char *format = (char *) (*varg++); - char scr[2]; - use_leading_plus = 0 ; // start out with this clear - for (; *format != 0; ++format) { - if (*format == '%') { - dec_width = 6 ; - ++format; - width = pad = 0; - if (*format == '\0') - break; - if (*format == '%') - goto out_lbl; - if (*format == '-') { - ++format; - pad = PAD_RIGHT; - } - if (*format == '+') { - ++format; - use_leading_plus = 1 ; - } - while (*format == '0') { - ++format; - pad |= PAD_ZERO; - } - post_decimal = 0 ; - if (*format == '.' || - (*format >= '0' && *format <= '9')) { + int post_decimal; + int width, pad; + unsigned dec_width = 6; + int pc = 0; + char *format = (char *)(*varg++); + char scr[2]; - while (1) { - if (*format == '.') { - post_decimal = 1 ; - dec_width = 0 ; - format++ ; - } else if ((*format >= '0' && *format <= '9')) { - if (post_decimal) { - dec_width *= 10; - dec_width += *format - '0'; - } else { - width *= 10; - width += *format - '0'; - } - format++ ; - } else { - break; - } - } - } - if (*format == 'l') + use_leading_plus = 0; // start out with this clear + for (; *format != 0; ++format) { + if (*format == '%') { + dec_width = 6; ++format; - switch (*format) { - case 's': + width = pad = 0; + if (*format == '\0') { + break; + } + if (*format == '%') { + goto out_lbl; + } + if (*format == '-') { + ++format; + pad = PAD_RIGHT; + } + if (*format == '+') { + ++format; + use_leading_plus = 1; + } + while (*format == '0') { + ++format; + pad |= PAD_ZERO; + } + post_decimal = 0; + if (*format == '.' || + (*format >= '0' && *format <= '9')) { + while (1) { + if (*format == '.') { + post_decimal = 1; + dec_width = 0; + format++; + } else if ((*format >= '0' && *format <= '9')) { + if (post_decimal) { + dec_width *= 10; + dec_width += *format - '0'; + } else { + width *= 10; + width += *format - '0'; + } + format++; + } else { + break; + } + } + } + if (*format == 'l') { + ++format; + } + switch (*format) { + case 's': { - // char *s = *((char **) varg++); //lint !e740 - char *s = (char *) *varg++ ; //lint !e740 !e826 convert to double pointer - pc += prints (out, s ? s : "(null)", width, pad); - use_leading_plus = 0 ; // reset this flag after printing one value + // char *s = *((char **) varg++); //lint !e740 + char *s = (char *)*varg++; // lint !e740 !e826 convert to double pointer + pc += prints(out, s ? s : "(null)", width, pad); + use_leading_plus = 0; // reset this flag after printing one value } break; - case 'd': - pc += printi (out, *varg++, 10, 1, width, pad, 'a'); - use_leading_plus = 0 ; // reset this flag after printing one value - break; - case 'x': - pc += printi (out, *varg++, 16, 0, width, pad, 'a'); - use_leading_plus = 0 ; // reset this flag after printing one value - break; - case 'X': - pc += printi (out, *varg++, 16, 0, width, pad, 'A'); - use_leading_plus = 0 ; // reset this flag after printing one value - break; - case 'u': - pc += printi (out, *varg++, 10, 0, width, pad, 'a'); - use_leading_plus = 0 ; // reset this flag after printing one value - break; - case 'c': - /* char are converted to int then pushed on the stack */ - scr[0] = *varg++; - scr[1] = '\0'; - pc += prints (out, scr, width, pad); - use_leading_plus = 0 ; // reset this flag after printing one value - break; + case 'd': + pc += printi(out, *varg++, 10, 1, width, pad, 'a'); + use_leading_plus = 0; // reset this flag after printing one value + break; + case 'x': + pc += printi(out, *varg++, 16, 0, width, pad, 'a'); + use_leading_plus = 0; // reset this flag after printing one value + break; + case 'X': + pc += printi(out, *varg++, 16, 0, width, pad, 'A'); + use_leading_plus = 0; // reset this flag after printing one value + break; + case 'u': + pc += printi(out, *varg++, 10, 0, width, pad, 'a'); + use_leading_plus = 0; // reset this flag after printing one value + break; + case 'c': + /* char are converted to int then pushed on the stack */ + scr[0] = *varg++; + scr[1] = '\0'; + pc += prints(out, scr, width, pad); + use_leading_plus = 0; // reset this flag after printing one value + break; - case 'f': + case 'f': { - // http://wiki.debian.org/ArmEabiPort#Structpackingandalignment - // Stack alignment - // - // The ARM EABI requires 8-byte stack alignment at public function entry points, - // compared to the previous 4-byte alignment. + // http://wiki.debian.org/ArmEabiPort#Structpackingandalignment + // Stack alignment + // + // The ARM EABI requires 8-byte stack alignment at public function entry points, + // compared to the previous 4-byte alignment. #ifdef USE_NEWLIB - char *cptr = (char *) varg ; //lint !e740 !e826 convert to double pointer - uint caddr = (uint) cptr ; - if ((caddr & 0xF) != 0) { - cptr += 4 ; - } - double *dblptr = (double *) cptr ; //lint !e740 !e826 convert to double pointer + char *cptr = (char *)varg; // lint !e740 !e826 convert to double pointer + uint caddr = (uint)cptr; + if ((caddr & 0xF) != 0) { + cptr += 4; + } + double *dblptr = (double *)cptr; // lint !e740 !e826 convert to double pointer #else - double *dblptr = (double *) varg ; //lint !e740 !e826 convert to double pointer + double *dblptr = (double *)varg; // lint !e740 !e826 convert to double pointer #endif - double dbl = *dblptr++ ; // increment double pointer - varg = (int *) dblptr ; //lint !e740 copy updated pointer back to base pointer - char bfr[81] ; - // unsigned slen = - dbl2stri(bfr, dbl, dec_width) ; - // stuff_talkf("[%s], width=%u, dec_width=%u\n", bfr, width, dec_width) ; - pc += prints (out, bfr, width, pad); - use_leading_plus = 0 ; // reset this flag after printing one value + double dbl = *dblptr++; // increment double pointer + varg = (int *)dblptr; // lint !e740 copy updated pointer back to base pointer + char bfr[81]; + // unsigned slen = + dbl2stri(bfr, dbl, dec_width); + // stuff_talkf("[%s], width=%u, dec_width=%u\n", bfr, width, dec_width) ; + pc += prints(out, bfr, width, pad); + use_leading_plus = 0; // reset this flag after printing one value } break; - default: - printchar (out, '%'); - printchar (out, *format); - use_leading_plus = 0 ; // reset this flag after printing one value - break; - } - } else - // if (*format == '\\') { - // - // } else - { + default: + printchar(out, '%'); + printchar(out, *format); + use_leading_plus = 0; // reset this flag after printing one value + break; + } + } else { + // if (*format == '\\') { + // + // } else out_lbl: - printchar (out, *format); - ++pc; - } - } // for each char in format string - if (out) //lint !e850 - **out = '\0'; - return pc; + printchar(out, *format); + ++pc; + } + } // for each char in format string + if (out) { // lint !e850 + **out = '\0'; + } + return pc; } -//**************************************************************************** -int stringf (char *out, const char *format, ...) +// **************************************************************************** +int stringf(char *out, const char *format, ...) { - // create a pointer into the stack. - // Thematically this should be a void*, since the actual usage of the - // pointer will vary. However, int* works fine too. - // Either way, the called function will need to re-cast the pointer - // for any type which isn't sizeof(int) - int *varg = (int *) (char *) (&format); - return print (&out, varg); + // create a pointer into the stack. + // Thematically this should be a void*, since the actual usage of the + // pointer will vary. However, int* works fine too. + // Either way, the called function will need to re-cast the pointer + // for any type which isn't sizeof(int) + int *varg = (int *)(char *)(&format); + + return print(&out, varg); } int printf(const char *format, ...) { - int *varg = (int *) (char *) (&format); - return print (0, varg); + int *varg = (int *)(char *)(&format); + + return print(0, varg); } int sprintf(char *out, const char *format, ...) { - int *varg = (int *) (char *) (&format); - return print (&out, varg); + int *varg = (int *)(char *)(&format); + + return print(&out, varg); } /** - * @} - */ + * @} + */ diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index c7126ec8c..dfc7b2c31 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -38,12 +38,12 @@ #include /**************************** - * Current checks: - * 1. If a flight mode switch allows autotune and autotune module not running - * 2. If airframe is a multirotor and either manual is available or a stabilization mode uses "none" - ****************************/ +* Current checks: +* 1. If a flight mode switch allows autotune and autotune module not running +* 2. If airframe is a multirotor and either manual is available or a stabilization mode uses "none" +****************************/ -//! Check a stabilization mode switch position for safety +// ! Check a stabilization mode switch position for safety static int32_t check_stabilization_settings(int index, bool multirotor); /** @@ -56,28 +56,29 @@ int32_t configuration_check() SystemAlarmsExtendedAlarmStatusOptions alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE; uint8_t alarmsubstatus = 0; // Get board type - const struct pios_board_info * bdinfo = &pios_board_info_blob; - bool coptercontrol = bdinfo->board_type == 0x04; + const struct pios_board_info *bdinfo = &pios_board_info_blob; + bool coptercontrol = bdinfo->board_type == 0x04; // Classify airframe type bool multirotor = true; uint8_t airframe_type; + SystemSettingsAirframeTypeGet(&airframe_type); switch (airframe_type) { - case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX: - case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP: - case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA: - case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO: - case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX: - case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV: - case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP: - case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX: - case SYSTEMSETTINGS_AIRFRAMETYPE_TRI: - case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX: - multirotor = true; - break; - default: - multirotor = false; + case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX: + case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP: + case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO: + case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP: + case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX: + case SYSTEMSETTINGS_AIRFRAMETYPE_TRI: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX: + multirotor = true; + break; + default: + multirotor = false; } // For each available flight mode position sanity check the available @@ -89,96 +90,96 @@ int32_t configuration_check() for (uint32_t i = 0; i < num_modes; i++) { switch (modes[i]) { - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL: - if (multirotor) { - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1: - severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity; - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2: - severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity; - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3: - severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity; - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: - if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) { - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { - // Revo supports Position Hold - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { - // Revo supports AutoLand Mode - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { - // Revo supports POI Mode - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { - // Revo supports PathPlanner - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { - // Revo supports ReturnToBase - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - default: - // Uncovered modes are automatically an error + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL: + if (multirotor) { severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1: + severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity; + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2: + severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity; + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3: + severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity; + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: + if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) { + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { + // Revo supports Position Hold + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { + // Revo supports AutoLand Mode + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { + // Revo supports POI Mode + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { + // Revo supports PathPlanner + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { + // Revo supports ReturnToBase + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + default: + // Uncovered modes are automatically an error + severity = SYSTEMALARMS_ALARM_ERROR; } // mark the first encountered erroneous setting in status and substatus if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) { - alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE; + alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE; alarmsubstatus = i; } - } // TODO: Check on a multirotor no axis supports "None" - if (severity != SYSTEMALARMS_ALARM_OK) + if (severity != SYSTEMALARMS_ALARM_OK) { ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus); - else + } else { AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION); + } return 0; } @@ -193,31 +194,33 @@ static int32_t check_stabilization_settings(int index, bool multirotor) { // Make sure the modes have identical sizes if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM || - MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM) + MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM) { return SYSTEMALARMS_ALARM_ERROR; + } uint8_t modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM]; // Get the different axis modes for this switch position switch (index) { - case 1: - ManualControlSettingsStabilization1SettingsGet(modes); - break; - case 2: - ManualControlSettingsStabilization2SettingsGet(modes); - break; - case 3: - ManualControlSettingsStabilization3SettingsGet(modes); - break; - default: - return SYSTEMALARMS_ALARM_ERROR; + case 1: + ManualControlSettingsStabilization1SettingsGet(modes); + break; + case 2: + ManualControlSettingsStabilization2SettingsGet(modes); + break; + case 3: + ManualControlSettingsStabilization3SettingsGet(modes); + break; + default: + return SYSTEMALARMS_ALARM_ERROR; } // For multirotors verify that nothing is set to "none" if (multirotor) { for (uint32_t i = 0; i < NELEMENTS(modes); i++) { - if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE) + if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE) { return SYSTEMALARMS_ALARM_ERROR; + } } } diff --git a/flight/libraries/stopwatch.c b/flight/libraries/stopwatch.c index a42e52360..efa00d451 100644 --- a/flight/libraries/stopwatch.c +++ b/flight/libraries/stopwatch.c @@ -35,85 +35,92 @@ // Local definitions ///////////////////////////////////////////////////////////////////////////// -uint32_t STOPWATCH_Init(u32 resolution, TIM_TypeDef* TIM) { - uint32_t STOPWATCH_TIMER_RCC; - switch ((uint32_t) TIM) { - case (uint32_t)TIM1: - STOPWATCH_TIMER_RCC = RCC_APB2Periph_TIM1; - break; - case (uint32_t)TIM2: - STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM2; - break; - case (uint32_t)TIM3: - STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM3; - break; - case (uint32_t)TIM4: - STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM4; - break; - case (uint32_t)TIM5: - STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM5; - break; - case (uint32_t)TIM6: - STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM6; - break; - case (uint32_t)TIM7: - STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM7; - break; - case (uint32_t)TIM8: - STOPWATCH_TIMER_RCC = RCC_APB2Periph_TIM8; - break; - default: - /* Unsupported timer */ - while(1); - } +uint32_t STOPWATCH_Init(u32 resolution, TIM_TypeDef *TIM) +{ + uint32_t STOPWATCH_TIMER_RCC; - // enable timer clock - if (STOPWATCH_TIMER_RCC == RCC_APB2Periph_TIM1 || STOPWATCH_TIMER_RCC - == RCC_APB2Periph_TIM8) - RCC_APB2PeriphClockCmd(STOPWATCH_TIMER_RCC, ENABLE); - else - RCC_APB1PeriphClockCmd(STOPWATCH_TIMER_RCC, ENABLE); + switch ((uint32_t)TIM) { + case (uint32_t)TIM1: + STOPWATCH_TIMER_RCC = RCC_APB2Periph_TIM1; + break; + case (uint32_t)TIM2: + STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM2; + break; + case (uint32_t)TIM3: + STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM3; + break; + case (uint32_t)TIM4: + STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM4; + break; + case (uint32_t)TIM5: + STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM5; + break; + case (uint32_t)TIM6: + STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM6; + break; + case (uint32_t)TIM7: + STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM7; + break; + case (uint32_t)TIM8: + STOPWATCH_TIMER_RCC = RCC_APB2Periph_TIM8; + break; + default: + /* Unsupported timer */ + while (1) { + ; + } + } - // time base configuration - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - TIM_TimeBaseStructure.TIM_Period = 0xffff; // max period - TIM_TimeBaseStructure.TIM_Prescaler = (72 * resolution) - 1; // uS accuracy @ 72 MHz - TIM_TimeBaseStructure.TIM_ClockDivision = 0; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(TIM, &TIM_TimeBaseStructure); + // enable timer clock + if (STOPWATCH_TIMER_RCC == RCC_APB2Periph_TIM1 || STOPWATCH_TIMER_RCC + == RCC_APB2Periph_TIM8) { + RCC_APB2PeriphClockCmd(STOPWATCH_TIMER_RCC, ENABLE); + } else { + RCC_APB1PeriphClockCmd(STOPWATCH_TIMER_RCC, ENABLE); + } - // enable interrupt request - TIM_ITConfig(TIM, TIM_IT_Update, ENABLE); + // time base configuration + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + TIM_TimeBaseStructure.TIM_Period = 0xffff; // max period + TIM_TimeBaseStructure.TIM_Prescaler = (72 * resolution) - 1; // uS accuracy @ 72 MHz + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInit(TIM, &TIM_TimeBaseStructure); - // start counter - TIM_Cmd(TIM, ENABLE); + // enable interrupt request + TIM_ITConfig(TIM, TIM_IT_Update, ENABLE); - return 0; // no error + // start counter + TIM_Cmd(TIM, ENABLE); + + return 0; // no error } ///////////////////////////////////////////////////////////////////////////// -//! Resets the stopwatch -//! \return < 0 on errors +// ! Resets the stopwatch +// ! \return < 0 on errors ///////////////////////////////////////////////////////////////////////////// -uint32_t STOPWATCH_Reset(TIM_TypeDef* TIM) { - // reset counter - TIM->CNT = 1; // set to 1 instead of 0 to avoid new IRQ request - TIM_ClearITPendingBit(TIM, TIM_IT_Update); +uint32_t STOPWATCH_Reset(TIM_TypeDef *TIM) +{ + // reset counter + TIM->CNT = 1; // set to 1 instead of 0 to avoid new IRQ request + TIM_ClearITPendingBit(TIM, TIM_IT_Update); - return 0; // no error + return 0; // no error } ///////////////////////////////////////////////////////////////////////////// -//! Returns current value of stopwatch -//! \return 1..65535: valid stopwatch value -//! \return 0xffffffff: counter overrun +// ! Returns current value of stopwatch +// ! \return 1..65535: valid stopwatch value +// ! \return 0xffffffff: counter overrun ///////////////////////////////////////////////////////////////////////////// -uint32_t STOPWATCH_ValueGet(TIM_TypeDef* TIM) { - uint32_t value = TIM->CNT; +uint32_t STOPWATCH_ValueGet(TIM_TypeDef *TIM) +{ + uint32_t value = TIM->CNT; - if (TIM_GetITStatus(TIM, TIM_IT_Update) != RESET) - value = 0xffffffff; + if (TIM_GetITStatus(TIM, TIM_IT_Update) != RESET) { + value = 0xffffffff; + } - return value; + return value; } - diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index 9b5dcbf91..5e55c5b98 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -47,19 +47,19 @@ #include "taskinfo.h" // Private constants -#define MAX_QUEUE_SIZE 2 +#define MAX_QUEUE_SIZE 2 #if defined(PIOS_ACTUATOR_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_ACTUATOR_STACK_SIZE +#define STACK_SIZE_BYTES PIOS_ACTUATOR_STACK_SIZE #else -#define STACK_SIZE_BYTES 1312 +#define STACK_SIZE_BYTES 1312 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY+4) -#define FAILSAFE_TIMEOUT_MS 100 -#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) +#define FAILSAFE_TIMEOUT_MS 100 +#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM -#define CAMERA_BOOT_DELAY_MS 7000 +#define CAMERA_BOOT_DELAY_MS 7000 // Private types @@ -68,30 +68,30 @@ static xQueueHandle queue; static xTaskHandle taskHandle; -static float lastResult[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0}; -static float filterAccumulator[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0}; +static float lastResult[MAX_MIX_ACTUATORS] = { 0, 0, 0, 0, 0, 0, 0, 0 }; +static float filterAccumulator[MAX_MIX_ACTUATORS] = { 0, 0, 0, 0, 0, 0, 0, 0 }; // used to inform the actuator thread that actuator update rate is changed static volatile bool actuator_settings_updated; // used to inform the actuator thread that mixer settings are changed static volatile bool mixer_settings_updated; // Private functions -static void actuatorTask(void* parameters); +static void actuatorTask(void *parameters); static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral); -static void setFailsafe(const ActuatorSettingsData * actuatorSettings, const MixerSettingsData * mixerSettings); -static float MixerCurve(const float throttle, const float* curve, uint8_t elements); -static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData * actuatorSettings); -static void actuator_update_rate_if_changed(const ActuatorSettingsData * actuatorSettings, bool force_update); -static void MixerSettingsUpdatedCb(UAVObjEvent * ev); -static void ActuatorSettingsUpdatedCb(UAVObjEvent * ev); +static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const MixerSettingsData *mixerSettings); +static float MixerCurve(const float throttle, const float *curve, uint8_t elements); +static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings); +static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update); +static void MixerSettingsUpdatedCb(UAVObjEvent *ev); +static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev); float ProcessMixer(const int index, const float curve1, const float curve2, - const MixerSettingsData* mixerSettings, ActuatorDesiredData* desired, - const float period); + const MixerSettingsData *mixerSettings, ActuatorDesiredData *desired, + const float period); -//this structure is equivalent to the UAVObjects for one mixer. +// this structure is equivalent to the UAVObjects for one mixer. typedef struct { - uint8_t type; - int8_t matrix[5]; + uint8_t type; + int8_t matrix[5]; } __attribute__((packed)) Mixer_t; /** @@ -100,14 +100,14 @@ typedef struct { */ int32_t ActuatorStart() { - // Start main task - xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ACTUATOR, taskHandle); + // Start main task + xTaskCreate(actuatorTask, (signed char *)"Actuator", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ACTUATOR, taskHandle); #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR); + PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR); #endif - return 0; + return 0; } /** @@ -116,28 +116,28 @@ int32_t ActuatorStart() */ int32_t ActuatorInitialize() { - // Register for notification of changes to ActuatorSettings - ActuatorSettingsInitialize(); - ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb); + // Register for notification of changes to ActuatorSettings + ActuatorSettingsInitialize(); + ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb); - // Register for notification of changes to MixerSettings - MixerSettingsInitialize(); - MixerSettingsConnectCallback(MixerSettingsUpdatedCb); + // Register for notification of changes to MixerSettings + MixerSettingsInitialize(); + MixerSettingsConnectCallback(MixerSettingsUpdatedCb); - // Listen for ActuatorDesired updates (Primary input to this module) - ActuatorDesiredInitialize(); - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - ActuatorDesiredConnectQueue(queue); + // Listen for ActuatorDesired updates (Primary input to this module) + ActuatorDesiredInitialize(); + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + ActuatorDesiredConnectQueue(queue); - // Primary output of this module - ActuatorCommandInitialize(); + // Primary output of this module + ActuatorCommandInitialize(); #ifdef DIAG_MIXERSTATUS - // UAVO only used for inspecting the internal status of the mixer during debug - MixerStatusInitialize(); + // UAVO only used for inspecting the internal status of the mixer during debug + MixerStatusInitialize(); #endif - return 0; + return 0; } MODULE_INITCALL(ActuatorInitialize, ActuatorStart) @@ -154,351 +154,337 @@ MODULE_INITCALL(ActuatorInitialize, ActuatorStart) * * @return -1 if error, 0 if success */ -static void actuatorTask(__attribute__((unused)) void* parameters) +static void actuatorTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; - portTickType lastSysTime; - portTickType thisSysTime; - float dTSeconds; - uint32_t dTMilliseconds; + UAVObjEvent ev; + portTickType lastSysTime; + portTickType thisSysTime; + float dTSeconds; + uint32_t dTMilliseconds; - ActuatorCommandData command; - ActuatorDesiredData desired; - MixerStatusData mixerStatus; - FlightStatusData flightStatus; + ActuatorCommandData command; + ActuatorDesiredData desired; + MixerStatusData mixerStatus; + FlightStatusData flightStatus; - /* Read initial values of ActuatorSettings */ - ActuatorSettingsData actuatorSettings; - actuator_settings_updated = false; - ActuatorSettingsGet(&actuatorSettings); + /* Read initial values of ActuatorSettings */ + ActuatorSettingsData actuatorSettings; - /* Read initial values of MixerSettings */ - MixerSettingsData mixerSettings; - mixer_settings_updated = false; - MixerSettingsGet(&mixerSettings); + actuator_settings_updated = false; + ActuatorSettingsGet(&actuatorSettings); - /* Force an initial configuration of the actuator update rates */ - actuator_update_rate_if_changed(&actuatorSettings, true); + /* Read initial values of MixerSettings */ + MixerSettingsData mixerSettings; + mixer_settings_updated = false; + MixerSettingsGet(&mixerSettings); - // Go to the neutral (failsafe) values until an ActuatorDesired update is received - setFailsafe(&actuatorSettings, &mixerSettings); + /* Force an initial configuration of the actuator update rates */ + actuator_update_rate_if_changed(&actuatorSettings, true); - // Main task loop - lastSysTime = xTaskGetTickCount(); - while (1) - { + // Go to the neutral (failsafe) values until an ActuatorDesired update is received + setFailsafe(&actuatorSettings, &mixerSettings); + + // Main task loop + lastSysTime = xTaskGetTickCount(); + while (1) { #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR); + PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR); #endif - // Wait until the ActuatorDesired object is updated - uint8_t rc = xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS); + // Wait until the ActuatorDesired object is updated + uint8_t rc = xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS); - /* Process settings updated events even in timeout case so we always act on the latest settings */ - if (actuator_settings_updated) { - actuator_settings_updated = false; - ActuatorSettingsGet (&actuatorSettings); - actuator_update_rate_if_changed (&actuatorSettings, false); - } - if (mixer_settings_updated) { - mixer_settings_updated = false; - MixerSettingsGet (&mixerSettings); - } + /* Process settings updated events even in timeout case so we always act on the latest settings */ + if (actuator_settings_updated) { + actuator_settings_updated = false; + ActuatorSettingsGet(&actuatorSettings); + actuator_update_rate_if_changed(&actuatorSettings, false); + } + if (mixer_settings_updated) { + mixer_settings_updated = false; + MixerSettingsGet(&mixerSettings); + } - if (rc != pdTRUE) { - /* Update of ActuatorDesired timed out. Go to failsafe */ - setFailsafe(&actuatorSettings, &mixerSettings); - continue; - } + if (rc != pdTRUE) { + /* Update of ActuatorDesired timed out. Go to failsafe */ + setFailsafe(&actuatorSettings, &mixerSettings); + continue; + } - // Check how long since last update - thisSysTime = xTaskGetTickCount(); - dTMilliseconds = (thisSysTime == lastSysTime)? 1: (thisSysTime - lastSysTime) * portTICK_RATE_MS; - lastSysTime = thisSysTime; - dTSeconds = dTMilliseconds * 0.001f; + // Check how long since last update + thisSysTime = xTaskGetTickCount(); + dTMilliseconds = (thisSysTime == lastSysTime) ? 1 : (thisSysTime - lastSysTime) * portTICK_RATE_MS; + lastSysTime = thisSysTime; + dTSeconds = dTMilliseconds * 0.001f; - FlightStatusGet(&flightStatus); - ActuatorDesiredGet(&desired); - ActuatorCommandGet(&command); + FlightStatusGet(&flightStatus); + ActuatorDesiredGet(&desired); + ActuatorCommandGet(&command); #ifdef DIAG_MIXERSTATUS - MixerStatusGet(&mixerStatus); + MixerStatusGet(&mixerStatus); #endif - int nMixers = 0; - Mixer_t * mixers = (Mixer_t *)&mixerSettings.Mixer1Type; - for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++) - { - if(mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) - { - nMixers ++; - } - } - if((nMixers < 2) && !ActuatorCommandReadOnly()) //Nothing can fly with less than two mixers. - { - setFailsafe(&actuatorSettings, &mixerSettings); // So that channels like PWM buzzer keep working - continue; - } + int nMixers = 0; + Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; + for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) { + if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) { + nMixers++; + } + } + if ((nMixers < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers. + setFailsafe(&actuatorSettings, &mixerSettings); // So that channels like PWM buzzer keep working + continue; + } - AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); + AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); - bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; - bool positiveThrottle = desired.Throttle >= 0.00f; - bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; + bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; + bool positiveThrottle = desired.Throttle >= 0.00f; + bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; - float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1,MIXERSETTINGS_THROTTLECURVE1_NUMELEM); - - //The source for the secondary curve is selectable - float curve2 = 0; - AccessoryDesiredData accessory; - switch(mixerSettings.Curve2Source) { - case MIXERSETTINGS_CURVE2SOURCE_THROTTLE: - curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM); - break; - case MIXERSETTINGS_CURVE2SOURCE_ROLL: - curve2 = MixerCurve(desired.Roll,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM); - break; - case MIXERSETTINGS_CURVE2SOURCE_PITCH: - curve2 = MixerCurve(desired.Pitch,mixerSettings.ThrottleCurve2, - MIXERSETTINGS_THROTTLECURVE2_NUMELEM); - break; - case MIXERSETTINGS_CURVE2SOURCE_YAW: - curve2 = MixerCurve(desired.Yaw,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM); - break; - case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE: - ManualControlCommandCollectiveGet(&curve2); - curve2 = MixerCurve(curve2,mixerSettings.ThrottleCurve2, - MIXERSETTINGS_THROTTLECURVE2_NUMELEM); - break; - case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0: - case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1: - case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY2: - case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY3: - case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4: - case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5: - if(AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0,&accessory) == 0) - curve2 = MixerCurve(accessory.AccessoryVal,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM); - else - curve2 = 0; - break; - } + float curve1 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM); - float * status = (float *)&mixerStatus; //access status objects as an array of floats + // The source for the secondary curve is selectable + float curve2 = 0; + AccessoryDesiredData accessory; + switch (mixerSettings.Curve2Source) { + case MIXERSETTINGS_CURVE2SOURCE_THROTTLE: + curve2 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + break; + case MIXERSETTINGS_CURVE2SOURCE_ROLL: + curve2 = MixerCurve(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + break; + case MIXERSETTINGS_CURVE2SOURCE_PITCH: + curve2 = MixerCurve(desired.Pitch, mixerSettings.ThrottleCurve2, + MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + break; + case MIXERSETTINGS_CURVE2SOURCE_YAW: + curve2 = MixerCurve(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + break; + case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE: + ManualControlCommandCollectiveGet(&curve2); + curve2 = MixerCurve(curve2, mixerSettings.ThrottleCurve2, + MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + break; + case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0: + case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1: + case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY2: + case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY3: + case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4: + case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5: + if (AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0, &accessory) == 0) { + curve2 = MixerCurve(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + } else { + curve2 = 0; + } + break; + } - for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++) - { - // During boot all camera actuators should be completely disabled (PWM pulse = 0). - // command.Channel[i] is reused below as a channel PWM activity flag: - // 0 - PWM disabled, >0 - PWM set to real mixer value using scaleChannel() later. - // Setting it to 1 by default means "Rescale this channel and enable PWM on its output". - command.Channel[ct] = 1; + float *status = (float *)&mixerStatus; // access status objects as an array of floats - if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) { - // Set to minimum if disabled. This is not the same as saying PWM pulse = 0 us - status[ct] = -1; - continue; - } + for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) { + // During boot all camera actuators should be completely disabled (PWM pulse = 0). + // command.Channel[i] is reused below as a channel PWM activity flag: + // 0 - PWM disabled, >0 - PWM set to real mixer value using scaleChannel() later. + // Setting it to 1 by default means "Rescale this channel and enable PWM on its output". + command.Channel[ct] = 1; - if((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) - status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds); - else - status[ct] = -1; + if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) { + // Set to minimum if disabled. This is not the same as saying PWM pulse = 0 us + status[ct] = -1; + continue; + } - // Motors have additional protection for when to be on - if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { + if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) { + status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds); + } else { + status[ct] = -1; + } - // If not armed or motors aren't meant to spin all the time - if( !armed || - (!spinWhileArmed && !positiveThrottle)) - { - filterAccumulator[ct] = 0; - lastResult[ct] = 0; - status[ct] = -1; //force min throttle - } - // If armed meant to keep spinning, - else if ((spinWhileArmed && !positiveThrottle) || - (status[ct] < 0) ) - status[ct] = 0; - } + // Motors have additional protection for when to be on + if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { + // If not armed or motors aren't meant to spin all the time + if (!armed || + (!spinWhileArmed && !positiveThrottle)) { + filterAccumulator[ct] = 0; + lastResult[ct] = 0; + status[ct] = -1; // force min throttle + } + // If armed meant to keep spinning, + else if ((spinWhileArmed && !positiveThrottle) || + (status[ct] < 0)) { + status[ct] = 0; + } + } - // If an accessory channel is selected for direct bypass mode - // In this configuration the accessory channel is scaled and mapped - // directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING - // these also will not be updated in failsafe mode. I'm not sure what - // the correct behavior is since it seems domain specific. I don't love - // this code - if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) && - (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) - { - if(AccessoryDesiredInstGet(mixers[ct].type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0,&accessory) == 0) - status[ct] = accessory.AccessoryVal; - else - status[ct] = -1; - } + // If an accessory channel is selected for direct bypass mode + // In this configuration the accessory channel is scaled and mapped + // directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING + // these also will not be updated in failsafe mode. I'm not sure what + // the correct behavior is since it seems domain specific. I don't love + // this code + if ((mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) && + (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) { + if (AccessoryDesiredInstGet(mixers[ct].type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0, &accessory) == 0) { + status[ct] = accessory.AccessoryVal; + } else { + status[ct] = -1; + } + } - if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) && - (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) - { - CameraDesiredData cameraDesired; - if( CameraDesiredGet(&cameraDesired) == 0 ) { - switch(mixers[ct].type) { - case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1: - status[ct] = cameraDesired.RollOrServo1; - break; - case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCHORSERVO2: - status[ct] = cameraDesired.PitchOrServo2; - break; - case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW: - status[ct] = cameraDesired.Yaw; - break; - default: - break; - } - } - else - status[ct] = -1; + if ((mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) && + (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) { + CameraDesiredData cameraDesired; + if (CameraDesiredGet(&cameraDesired) == 0) { + switch (mixers[ct].type) { + case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1: + status[ct] = cameraDesired.RollOrServo1; + break; + case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCHORSERVO2: + status[ct] = cameraDesired.PitchOrServo2; + break; + case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW: + status[ct] = cameraDesired.Yaw; + break; + default: + break; + } + } else { + status[ct] = -1; + } - // Disable camera actuators for CAMERA_BOOT_DELAY_MS after boot - if (thisSysTime < (CAMERA_BOOT_DELAY_MS / portTICK_RATE_MS)) - command.Channel[ct] = 0; - } - } + // Disable camera actuators for CAMERA_BOOT_DELAY_MS after boot + if (thisSysTime < (CAMERA_BOOT_DELAY_MS / portTICK_RATE_MS)) { + command.Channel[ct] = 0; + } + } + } - // Set real actuator output values scaling them from mixers. All channels - // will be set except explicitly disabled (which will have PWM pulse = 0). - for (int i = 0; i < MAX_MIX_ACTUATORS; i++) - if (command.Channel[i]) - command.Channel[i] = scaleChannel(status[i], - actuatorSettings.ChannelMax[i], - actuatorSettings.ChannelMin[i], - actuatorSettings.ChannelNeutral[i]); + // Set real actuator output values scaling them from mixers. All channels + // will be set except explicitly disabled (which will have PWM pulse = 0). + for (int i = 0; i < MAX_MIX_ACTUATORS; i++) { + if (command.Channel[i]) { + command.Channel[i] = scaleChannel(status[i], + actuatorSettings.ChannelMax[i], + actuatorSettings.ChannelMin[i], + actuatorSettings.ChannelNeutral[i]); + } + } - // Store update time - command.UpdateTime = dTMilliseconds; - if (command.UpdateTime > command.MaxUpdateTime) - command.MaxUpdateTime = command.UpdateTime; - - // Update output object - ActuatorCommandSet(&command); - // Update in case read only (eg. during servo configuration) - ActuatorCommandGet(&command); + // Store update time + command.UpdateTime = dTMilliseconds; + if (command.UpdateTime > command.MaxUpdateTime) { + command.MaxUpdateTime = command.UpdateTime; + } + + // Update output object + ActuatorCommandSet(&command); + // Update in case read only (eg. during servo configuration) + ActuatorCommandGet(&command); #ifdef DIAG_MIXERSTATUS - MixerStatusSet(&mixerStatus); + MixerStatusSet(&mixerStatus); #endif - - // Update servo outputs - bool success = true; - for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) - { - success &= set_channel(n, command.Channel[n], &actuatorSettings); - } + // Update servo outputs + bool success = true; - if(!success) { - command.NumFailedUpdates++; - ActuatorCommandSet(&command); - AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); - } + for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { + success &= set_channel(n, command.Channel[n], &actuatorSettings); + } - } + if (!success) { + command.NumFailedUpdates++; + ActuatorCommandSet(&command); + AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); + } + } } - /** - *Process mixing for one actuator + * Process mixing for one actuator */ float ProcessMixer(const int index, const float curve1, const float curve2, - const MixerSettingsData* mixerSettings, ActuatorDesiredData* desired, const float period) + const MixerSettingsData *mixerSettings, ActuatorDesiredData *desired, const float period) { - static float lastFilteredResult[MAX_MIX_ACTUATORS]; - const Mixer_t * mixers = (Mixer_t *)&mixerSettings->Mixer1Type; //pointer to array of mixers in UAVObjects - const Mixer_t * mixer = &mixers[index]; + static float lastFilteredResult[MAX_MIX_ACTUATORS]; + const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects + const Mixer_t *mixer = &mixers[index]; - float result = (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1] / 128.0f) * curve1) + - (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2] / 128.0f) * curve2) + - (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] / 128.0f) * desired->Roll) + - (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) + - (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw); + float result = (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1] / 128.0f) * curve1) + + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2] / 128.0f) * curve2) + + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] / 128.0f) * desired->Roll) + + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) + + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw); - if(mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) - { - if(result < 0.0f) //idle throttle - { - result = 0.0f; - } + if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { + if (result < 0.0f) { // idle throttle + result = 0.0f; + } - //feed forward - float accumulator = filterAccumulator[index]; - accumulator += (result - lastResult[index]) * mixerSettings->FeedForward; - lastResult[index] = result; - result += accumulator; - if(period > 0.0f) - { - if(accumulator > 0.0f) - { - float filter = mixerSettings->AccelTime / period; - if(filter <1) - { - filter = 1; - } - accumulator -= accumulator / filter; - }else - { - float filter = mixerSettings->DecelTime / period; - if(filter <1) - { - filter = 1; - } - accumulator -= accumulator / filter; - } - } - filterAccumulator[index] = accumulator; - result += accumulator; + // feed forward + float accumulator = filterAccumulator[index]; + accumulator += (result - lastResult[index]) * mixerSettings->FeedForward; + lastResult[index] = result; + result += accumulator; + if (period > 0.0f) { + if (accumulator > 0.0f) { + float filter = mixerSettings->AccelTime / period; + if (filter < 1) { + filter = 1; + } + accumulator -= accumulator / filter; + } else { + float filter = mixerSettings->DecelTime / period; + if (filter < 1) { + filter = 1; + } + accumulator -= accumulator / filter; + } + } + filterAccumulator[index] = accumulator; + result += accumulator; - //acceleration limit - float dt = result - lastFilteredResult[index]; - float maxDt = mixerSettings->MaxAccel * period; - if(dt > maxDt) //we are accelerating too hard - { - result = lastFilteredResult[index] + maxDt; - } - lastFilteredResult[index] = result; - } + // acceleration limit + float dt = result - lastFilteredResult[index]; + float maxDt = mixerSettings->MaxAccel * period; + if (dt > maxDt) { // we are accelerating too hard + result = lastFilteredResult[index] + maxDt; + } + lastFilteredResult[index] = result; + } - return(result); + return result; } /** - *Interpolate a throttle curve. Throttle input should be in the range 0 to 1. - *Output is in the range 0 to 1. + * Interpolate a throttle curve. Throttle input should be in the range 0 to 1. + * Output is in the range 0 to 1. */ -static float MixerCurve(const float throttle, const float* curve, uint8_t elements) +static float MixerCurve(const float throttle, const float *curve, uint8_t elements) { - float scale = throttle * (float) (elements - 1); - int idx1 = scale; - scale -= (float)idx1; //remainder - if(curve[0] < -1) - { - return(throttle); - } - if (idx1 < 0) - { - idx1 = 0; //clamp to lowest entry in table - scale = 0; - } - int idx2 = idx1 + 1; - if(idx2 >= elements) - { - idx2 = elements -1; //clamp to highest entry in table - if(idx1 >= elements) - { - idx1 = elements -1; - } - } - return curve[idx1] * (1.0f - scale) + curve[idx2] * scale; + float scale = throttle * (float)(elements - 1); + int idx1 = scale; + + scale -= (float)idx1; // remainder + if (curve[0] < -1) { + return throttle; + } + if (idx1 < 0) { + idx1 = 0; // clamp to lowest entry in table + scale = 0; + } + int idx2 = idx1 + 1; + if (idx2 >= elements) { + idx2 = elements - 1; // clamp to highest entry in table + if (idx1 >= elements) { + idx1 = elements - 1; + } + } + return curve[idx1] * (1.0f - scale) + curve[idx2] * scale; } @@ -507,231 +493,226 @@ static float MixerCurve(const float throttle, const float* curve, uint8_t elemen */ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral) { - int16_t valueScaled; - // Scale - if ( value >= 0.0f) - { - valueScaled = (int16_t)(value*((float)(max-neutral))) + neutral; - } - else - { - valueScaled = (int16_t)(value*((float)(neutral-min))) + neutral; - } + int16_t valueScaled; - if (max>min) - { - if( valueScaled > max ) valueScaled = max; - if( valueScaled < min ) valueScaled = min; - } - else - { - if( valueScaled < max ) valueScaled = max; - if( valueScaled > min ) valueScaled = min; - } + // Scale + if (value >= 0.0f) { + valueScaled = (int16_t)(value * ((float)(max - neutral))) + neutral; + } else { + valueScaled = (int16_t)(value * ((float)(neutral - min))) + neutral; + } - return valueScaled; + if (max > min) { + if (valueScaled > max) { + valueScaled = max; + } + if (valueScaled < min) { + valueScaled = min; + } + } else { + if (valueScaled < max) { + valueScaled = max; + } + if (valueScaled > min) { + valueScaled = min; + } + } + + return valueScaled; } /** * Set actuator output to the neutral values (failsafe) */ -static void setFailsafe(const ActuatorSettingsData * actuatorSettings, const MixerSettingsData * mixerSettings) +static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const MixerSettingsData *mixerSettings) { - /* grab only the parts that we are going to use */ - int16_t Channel[ACTUATORCOMMAND_CHANNEL_NUMELEM]; - ActuatorCommandChannelGet(Channel); + /* grab only the parts that we are going to use */ + int16_t Channel[ACTUATORCOMMAND_CHANNEL_NUMELEM]; - const Mixer_t * mixers = (Mixer_t *)&mixerSettings->Mixer1Type; //pointer to array of mixers in UAVObjects + ActuatorCommandChannelGet(Channel); - // Reset ActuatorCommand to safe values - for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) - { + const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects - if(mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) - { - Channel[n] = actuatorSettings->ChannelMin[n]; - } - else if(mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO) - { - Channel[n] = actuatorSettings->ChannelNeutral[n]; - } - else - { - Channel[n] = 0; - } - - - } + // Reset ActuatorCommand to safe values + for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { + if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { + Channel[n] = actuatorSettings->ChannelMin[n]; + } else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO) { + Channel[n] = actuatorSettings->ChannelNeutral[n]; + } else { + Channel[n] = 0; + } + } - // Set alarm - AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); + // Set alarm + AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); - // Update servo outputs - for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) - { - set_channel(n, Channel[n], actuatorSettings); - } + // Update servo outputs + for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { + set_channel(n, Channel[n], actuatorSettings); + } - // Update output object's parts that we changed - ActuatorCommandChannelSet(Channel); + // Update output object's parts that we changed + ActuatorCommandChannelSet(Channel); } /** * determine buzzer or blink sequence **/ -typedef enum {BUZZ_BUZZER=0,BUZZ_ARMING=1,BUZZ_INFO=2,BUZZ_MAX=3} buzzertype; +typedef enum { BUZZ_BUZZER = 0, BUZZ_ARMING = 1, BUZZ_INFO = 2, BUZZ_MAX = 3 } buzzertype; static inline bool buzzerState(buzzertype type) { - // This is for buzzers that take a PWM input + // This is for buzzers that take a PWM input - static uint32_t tune[BUZZ_MAX]={0}; - static uint32_t tunestate[BUZZ_MAX]={0}; + static uint32_t tune[BUZZ_MAX] = { 0 }; + static uint32_t tunestate[BUZZ_MAX] = { 0 }; - uint32_t newTune = 0; - if(type==BUZZ_BUZZER) - { - // Decide what tune to play - if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) { - newTune = 0b11110110110000; // pause, short, short, short, long - } else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) { - newTune = 0x80000000; // pause, short - } else { - newTune = 0; - } + uint32_t newTune = 0; - } else { // BUZZ_ARMING || BUZZ_INFO - uint8_t arming; - FlightStatusArmedGet(&arming); - //base idle tune - newTune = 0x80000000; // 0b1000... - - // Merge the error pattern for InfoLed - if(type==BUZZ_INFO) - { - if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) - { - newTune |= 0b00000000001111111011111110000000; - } - else if(AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) - { - newTune |= 0b00000000000000110110110000000000; - } - } - // fast double blink pattern if armed - if (arming == FLIGHTSTATUS_ARMED_ARMED) - newTune |= 0xA0000000; // 0b101000... + if (type == BUZZ_BUZZER) { + // Decide what tune to play + if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) { + newTune = 0b11110110110000; // pause, short, short, short, long + } else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) { + newTune = 0x80000000; // pause, short + } else { + newTune = 0; + } + } else { // BUZZ_ARMING || BUZZ_INFO + uint8_t arming; + FlightStatusArmedGet(&arming); + // base idle tune + newTune = 0x80000000; // 0b1000... - } + // Merge the error pattern for InfoLed + if (type == BUZZ_INFO) { + if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) { + newTune |= 0b00000000001111111011111110000000; + } else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) { + newTune |= 0b00000000000000110110110000000000; + } + } + // fast double blink pattern if armed + if (arming == FLIGHTSTATUS_ARMED_ARMED) { + newTune |= 0xA0000000; // 0b101000... + } + } - // Do we need to change tune? - if (newTune != tune[type]) { - tune[type] = newTune; - // resynchronize all tunes on change, so they stay in sync - for (int i=0;i lastSysTime) { - dT = thisSysTime - lastSysTime; - } else { - lastSysTime = 0; // avoid the case where SysTimeMax-lastSysTime <80 - } + // For now, only look at the battery alarm, because functions like AlarmsHasCritical() can block for some time; to be discussed + if (tune[type]) { + if (thisSysTime > lastSysTime) { + dT = thisSysTime - lastSysTime; + } else { + lastSysTime = 0; // avoid the case where SysTimeMax-lastSysTime <80 + } - buzzOn = (tunestate[type]&1); + buzzOn = (tunestate[type] & 1); - if (dT > 80) { - // Go to next bit in alarm_seq_state - for (int i=0;i>=1; - if (tunestate[i]==0) // All done, re-start the tune - tunestate[i]=tune[i]; - } - lastSysTime = thisSysTime; - } - } - return buzzOn; + if (dT > 80) { + // Go to next bit in alarm_seq_state + for (int i = 0; i < BUZZ_MAX; i++) { + tunestate[i] >>= 1; + if (tunestate[i] == 0) { // All done, re-start the tune + tunestate[i] = tune[i]; + } + } + lastSysTime = thisSysTime; + } + } + return buzzOn; } #if defined(ARCH_POSIX) || defined(ARCH_WIN32) -static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData * actuatorSettings) +static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings) { - return true; + return true; } #else -static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData * actuatorSettings) +static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings) { - switch(actuatorSettings->ChannelType[mixer_channel]) { - case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: - PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], - buzzerState(BUZZ_BUZZER)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]); - return true; - case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED: - PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], - buzzerState(BUZZ_ARMING)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]); - return true; - case ACTUATORSETTINGS_CHANNELTYPE_INFOLED: - PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], - buzzerState(BUZZ_INFO)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]); - return true; - case ACTUATORSETTINGS_CHANNELTYPE_PWM: - PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value); - return true; + switch (actuatorSettings->ChannelType[mixer_channel]) { + case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: + PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], + buzzerState(BUZZ_BUZZER) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]); + return true; + + case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED: + PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], + buzzerState(BUZZ_ARMING) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]); + return true; + + case ACTUATORSETTINGS_CHANNELTYPE_INFOLED: + PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], + buzzerState(BUZZ_INFO) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]); + return true; + + case ACTUATORSETTINGS_CHANNELTYPE_PWM: + PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value); + return true; + #if defined(PIOS_INCLUDE_I2C_ESC) - case ACTUATORSETTINGS_CHANNELTYPE_MK: - return PIOS_SetMKSpeed(actuatorSettings->ChannelAddr[mixer_channel],value); - case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4: - return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel],value); + case ACTUATORSETTINGS_CHANNELTYPE_MK: + return PIOS_SetMKSpeed(actuatorSettings->ChannelAddr[mixer_channel], value); + + case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4: + return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel], value); + #endif - default: - return false; - } - - return false; + default: + return false; + } + return false; } -#endif +#endif /* if defined(ARCH_POSIX) || defined(ARCH_WIN32) */ /** * @brief Update the servo update rate */ -static void actuator_update_rate_if_changed(const ActuatorSettingsData * actuatorSettings, bool force_update) +static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update) { - static uint16_t prevChannelUpdateFreq[ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM]; + static uint16_t prevChannelUpdateFreq[ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM]; - // check if the any rate setting is changed - if (force_update || - memcmp (prevChannelUpdateFreq, - actuatorSettings->ChannelUpdateFreq, - sizeof(prevChannelUpdateFreq)) != 0) { - /* Something has changed, apply the settings to HW */ - memcpy (prevChannelUpdateFreq, - actuatorSettings->ChannelUpdateFreq, - sizeof(prevChannelUpdateFreq)); - PIOS_Servo_SetHz(actuatorSettings->ChannelUpdateFreq, ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM); - } + // check if the any rate setting is changed + if (force_update || + memcmp(prevChannelUpdateFreq, + actuatorSettings->ChannelUpdateFreq, + sizeof(prevChannelUpdateFreq)) != 0) { + /* Something has changed, apply the settings to HW */ + memcpy(prevChannelUpdateFreq, + actuatorSettings->ChannelUpdateFreq, + sizeof(prevChannelUpdateFreq)); + PIOS_Servo_SetHz(actuatorSettings->ChannelUpdateFreq, ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM); + } } static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - actuator_settings_updated = true; + actuator_settings_updated = true; } static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - mixer_settings_updated = true; + mixer_settings_updated = true; } /** diff --git a/flight/modules/Actuator/inc/actuator.h b/flight/modules/Actuator/inc/actuator.h index 315a99daa..86362a944 100644 --- a/flight/modules/Actuator/inc/actuator.h +++ b/flight/modules/Actuator/inc/actuator.h @@ -1,11 +1,11 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup ActuatorModule Actuator Module * @brief Compute servo/motor settings based on @ref ActuatorDesired "desired actuator positions" and aircraft type. * This is where all the mixing of channels is computed. - * @{ + * @{ * * @file actuator.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -39,6 +39,6 @@ int32_t ActuatorInitialize(); #endif // ACTUATOR_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/Airspeed/revolution/airspeed.c b/flight/modules/Airspeed/revolution/airspeed.c index 63df98072..340cb3315 100644 --- a/flight/modules/Airspeed/revolution/airspeed.c +++ b/flight/modules/Airspeed/revolution/airspeed.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Calculate airspeed from diverse sources and update @ref Airspeed "Airspeed UAV Object" - * @{ + * @{ * * @file airspeed.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. @@ -50,21 +50,21 @@ #define STACK_SIZE_BYTES 500 -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types // Private variables static xTaskHandle taskHandle; -static bool airspeedEnabled = false; +static bool airspeedEnabled = false; static AirspeedSettingsData airspeedSettings; -static int8_t airspeedADCPin=-1; +static int8_t airspeedADCPin = -1; // Private functions static void airspeedTask(void *parameters); -static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev); +static void AirspeedSettingsUpdatedCb(UAVObjEvent *ev); /** @@ -72,17 +72,16 @@ static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev); * \returns 0 on success or -1 if initialisation failed */ int32_t AirspeedStart() -{ - - //Check if module is enabled or not - if (airspeedEnabled == false) { - return -1; - } - - // Start main task - xTaskCreate(airspeedTask, (signed char *)"Airspeed", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AIRSPEED, taskHandle); - return 0; +{ + // Check if module is enabled or not + if (airspeedEnabled == false) { + return -1; + } + + // Start main task + xTaskCreate(airspeedTask, (signed char *)"Airspeed", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AIRSPEED, taskHandle); + return 0; } /** @@ -92,38 +91,38 @@ int32_t AirspeedStart() int32_t AirspeedInitialize() { #ifdef MODULE_AIRSPEED_BUILTIN - airspeedEnabled = true; + airspeedEnabled = true; #else - - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); - - - if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - airspeedEnabled = true; - } else { - airspeedEnabled = false; - return -1; - } + + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + + + if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + airspeedEnabled = true; + } else { + airspeedEnabled = false; + return -1; + } #endif - - uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM]; - HwSettingsADCRoutingGet(adcRouting); - - //Determine if the barometric airspeed sensor is routed to an ADC pin - for (int i=0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { - if (adcRouting[i] == HWSETTINGS_ADCROUTING_ANALOGAIRSPEED) { - airspeedADCPin = i; - } - } - - AirspeedSensorInitialize(); - AirspeedSettingsInitialize(); - - AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb); - - return 0; + + uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM]; + HwSettingsADCRoutingGet(adcRouting); + + // Determine if the barometric airspeed sensor is routed to an ADC pin + for (int i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { + if (adcRouting[i] == HWSETTINGS_ADCROUTING_ANALOGAIRSPEED) { + airspeedADCPin = i; + } + } + + AirspeedSensorInitialize(); + AirspeedSettingsInitialize(); + + AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb); + + return 0; } MODULE_INITCALL(AirspeedInitialize, AirspeedStart) @@ -133,59 +132,54 @@ MODULE_INITCALL(AirspeedInitialize, AirspeedStart) */ static void airspeedTask(__attribute__((unused)) void *parameters) { - AirspeedSettingsUpdatedCb(AirspeedSettingsHandle()); - - AirspeedSensorData airspeedData; - AirspeedSensorGet(&airspeedData); + AirspeedSettingsUpdatedCb(AirspeedSettingsHandle()); - AirspeedSettingsUpdatedCb(NULL); + AirspeedSensorData airspeedData; + AirspeedSensorGet(&airspeedData); + + AirspeedSettingsUpdatedCb(NULL); - airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - - // Main task loop - portTickType lastSysTime = xTaskGetTickCount(); - while (1) - { - vTaskDelayUntil(&lastSysTime, airspeedSettings.SamplePeriod / portTICK_RATE_MS); - - // Update the airspeed object - AirspeedSensorGet(&airspeedData); + airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - switch (airspeedSettings.AirspeedSensorType) { + // Main task loop + portTickType lastSysTime = xTaskGetTickCount(); + while (1) { + vTaskDelayUntil(&lastSysTime, airspeedSettings.SamplePeriod / portTICK_RATE_MS); + + // Update the airspeed object + AirspeedSensorGet(&airspeedData); + + switch (airspeedSettings.AirspeedSensorType) { #if defined(PIOS_INCLUDE_MPXV) - case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: - case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004: - //MPXV5004 and MPXV7002 sensors - baro_airspeedGetMPXV(&airspeedData, &airspeedSettings,airspeedADCPin); - break; + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004: + // MPXV5004 and MPXV7002 sensors + baro_airspeedGetMPXV(&airspeedData, &airspeedSettings, airspeedADCPin); + break; #endif #if defined(PIOS_INCLUDE_ETASV3) - case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3: - //Eagletree Airspeed v3 - baro_airspeedGetETASV3(&airspeedData, &airspeedSettings); - break; + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3: + // Eagletree Airspeed v3 + baro_airspeedGetETASV3(&airspeedData, &airspeedSettings); + break; #endif - default: - airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - } + default: + airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + } - //Set the UAVO - AirspeedSensorSet(&airspeedData); - - } + // Set the UAVO + AirspeedSensorSet(&airspeedData); + } } - - static void AirspeedSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - AirspeedSettingsGet(&airspeedSettings); - + AirspeedSettingsGet(&airspeedSettings); } - - + + /** * @} * @} diff --git a/flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c b/flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c index 3f6012c30..e26262ec2 100644 --- a/flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c +++ b/flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Communicate with airspeed sensors and return values - * @{ + * @{ * * @file baro_airspeed.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -39,12 +39,12 @@ #include "openpilot.h" #include "hwsettings.h" #include "airspeedsettings.h" -#include "airspeedsensor.h" // object that will be updated by the module +#include "airspeedsensor.h" // object that will be updated by the module #if defined(PIOS_INCLUDE_ETASV3) -#define CALIBRATION_IDLE_MS 2000 //Time to wait before calibrating, in [ms] -#define CALIBRATION_COUNT_MS 2000 //Time to spend calibrating, in [ms] +#define CALIBRATION_IDLE_MS 2000 // Time to wait before calibrating, in [ms] +#define CALIBRATION_COUNT_MS 2000 // Time to spend calibrating, in [ms] // Private types @@ -52,48 +52,47 @@ // Private functions -static uint16_t calibrationCount=0; -static uint16_t calibrationCount2=0; -static uint32_t calibrationSum = 0; +static uint16_t calibrationCount = 0; +static uint16_t calibrationCount2 = 0; +static uint32_t calibrationSum = 0; -void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings){ +void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings) +{ + // Check to see if airspeed sensor is returning airspeedSensor + airspeedSensor->SensorValue = PIOS_ETASV3_ReadAirspeed(); + if (airspeedSensor->SensorValue == (uint16_t)-1) { + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + airspeedSensor->CalibratedAirspeed = 0; + return; + } - //Check to see if airspeed sensor is returning airspeedSensor - airspeedSensor->SensorValue = PIOS_ETASV3_ReadAirspeed(); - if (airspeedSensor->SensorValue==(uint16_t)-1) { - airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - airspeedSensor->CalibratedAirspeed = 0; - return; - } - - // only calibrate if no stored calibration is available - if (!airspeedSettings->ZeroPoint) { - //Calibrate sensor by averaging zero point value - if (calibrationCount <= CALIBRATION_IDLE_MS/airspeedSettings->SamplePeriod) { - calibrationCount++; - calibrationCount2++; - return; - } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) { - calibrationCount++; - calibrationCount2++; - calibrationSum += airspeedSensor->SensorValue; - if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) { + // only calibrate if no stored calibration is available + if (!airspeedSettings->ZeroPoint) { + // Calibrate sensor by averaging zero point value + if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { + calibrationCount++; + calibrationCount2++; + return; + } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { + calibrationCount++; + calibrationCount2++; + calibrationSum += airspeedSensor->SensorValue; + if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { + airspeedSettings->ZeroPoint = (int16_t)(((float)calibrationSum) / calibrationCount2); + AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint); + } + return; + } + } - airspeedSettings->ZeroPoint = (int16_t) (((float)calibrationSum) / calibrationCount2); - AirspeedSettingsZeroPointSet( &airspeedSettings->ZeroPoint ); - } - return; - } - } - - //Compute airspeed - airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint)); - airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + // Compute airspeed + airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint)); + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; } -#endif +#endif /* if defined(PIOS_INCLUDE_ETASV3) */ /** * @} diff --git a/flight/modules/Airspeed/revolution/baro_airspeed_mpxv.c b/flight/modules/Airspeed/revolution/baro_airspeed_mpxv.c index 540a4a91d..c05322dfc 100644 --- a/flight/modules/Airspeed/revolution/baro_airspeed_mpxv.c +++ b/flight/modules/Airspeed/revolution/baro_airspeed_mpxv.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Communicate with airspeed sensors and return values - * @{ + * @{ * * @file baro_airspeed.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -39,13 +39,13 @@ #include "openpilot.h" #include "hwsettings.h" #include "airspeedsettings.h" -#include "airspeedsensor.h" // object that will be updated by the module +#include "airspeedsensor.h" // object that will be updated by the module #if defined(PIOS_INCLUDE_MPXV) -#define CALIBRATION_IDLE_MS 2000 //Time to wait before calibrating, in [ms] -#define CALIBRATION_COUNT_MS 2000 //Time to spend calibrating, in [ms] -#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 100 //low pass filter +#define CALIBRATION_IDLE_MS 2000 // Time to wait before calibrating, in [ms] +#define CALIBRATION_COUNT_MS 2000 // Time to spend calibrating, in [ms] +#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 100 // low pass filter // Private types @@ -53,64 +53,63 @@ // Private functions -static uint16_t calibrationCount=0; +static uint16_t calibrationCount = 0; -PIOS_MPXV_descriptor sensor = { .type=PIOS_MPXV_UNKNOWN }; +PIOS_MPXV_descriptor sensor = { .type = PIOS_MPXV_UNKNOWN }; -void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings, int8_t airspeedADCPin){ +void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings, int8_t airspeedADCPin) +{ + // Ensure that the ADC pin is properly configured + if (airspeedADCPin < 0) { + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - //Ensure that the ADC pin is properly configured - if(airspeedADCPin <0){ - airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - - return; - } - if (sensor.type==PIOS_MPXV_UNKNOWN) { - switch (airspeedSettings->AirspeedSensorType) { - case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: - sensor = PIOS_MPXV_7002_DESC(airspeedADCPin); - break; - case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004: - sensor = PIOS_MPXV_5004_DESC(airspeedADCPin); - break; - default: - airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - return; - } - } + return; + } + if (sensor.type == PIOS_MPXV_UNKNOWN) { + switch (airspeedSettings->AirspeedSensorType) { + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: + sensor = PIOS_MPXV_7002_DESC(airspeedADCPin); + break; + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004: + sensor = PIOS_MPXV_5004_DESC(airspeedADCPin); + break; + default: + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + return; + } + } - airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor); + airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor); - if (!airspeedSettings->ZeroPoint) { - //Calibrate sensor by averaging zero point value - if (calibrationCount < CALIBRATION_IDLE_MS/airspeedSettings->SamplePeriod) { //First let sensor warm up and stabilize. - calibrationCount++; - return; - } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) { //Then compute the average. - calibrationCount++; /*DO NOT MOVE FROM BEFORE sensorCalibration=... LINE, OR ELSE WILL HAVE DIVIDE BY ZERO */ + if (!airspeedSettings->ZeroPoint) { + // Calibrate sensor by averaging zero point value + if (calibrationCount < CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { // First let sensor warm up and stabilize. + calibrationCount++; + return; + } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { // Then compute the average. + calibrationCount++; /*DO NOT MOVE FROM BEFORE sensorCalibration=... LINE, OR ELSE WILL HAVE DIVIDE BY ZERO */ - PIOS_MPXV_Calibrate(&sensor,airspeedSensor->SensorValue); + PIOS_MPXV_Calibrate(&sensor, airspeedSensor->SensorValue); - //Set settings UAVO. The airspeed UAVO is set elsewhere in the function. - if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) { - airspeedSettings->ZeroPoint = sensor.zeroPoint; - AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint); - } - return; - } - } - sensor.zeroPoint = airspeedSettings->ZeroPoint; - - //Filter CAS - float alpha=airspeedSettings->SamplePeriod/(airspeedSettings->SamplePeriod + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter. - - airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor,airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed*(1.0f-alpha); - airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + // Set settings UAVO. The airspeed UAVO is set elsewhere in the function. + if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { + airspeedSettings->ZeroPoint = sensor.zeroPoint; + AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint); + } + return; + } + } + sensor.zeroPoint = airspeedSettings->ZeroPoint; + // Filter CAS + float alpha = airspeedSettings->SamplePeriod / (airspeedSettings->SamplePeriod + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); // Low pass filter. + + airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor, airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed * (1.0f - alpha); + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; } -#endif +#endif /* if defined(PIOS_INCLUDE_MPXV) */ /** * @} diff --git a/flight/modules/Airspeed/revolution/gps_airspeed.c b/flight/modules/Airspeed/revolution/gps_airspeed.c index ec85d10d5..d81f66fef 100644 --- a/flight/modules/Airspeed/revolution/gps_airspeed.c +++ b/flight/modules/Airspeed/revolution/gps_airspeed.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Use GPS data to estimate airspeed - * @{ + * @{ * * @file gps_airspeed.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. @@ -39,17 +39,17 @@ // Private constants -#define GPS_AIRSPEED_BIAS_KP 0.1f //Needs to be settable in a UAVO -#define GPS_AIRSPEED_BIAS_KI 0.1f //Needs to be settable in a UAVO -#define SAMPLING_DELAY_MS_GPS 100 //Needs to be settable in a UAVO -#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f //Needs to be settable in a UAVO +#define GPS_AIRSPEED_BIAS_KP 0.1f // Needs to be settable in a UAVO +#define GPS_AIRSPEED_BIAS_KI 0.1f // Needs to be settable in a UAVO +#define SAMPLING_DELAY_MS_GPS 100 // Needs to be settable in a UAVO +#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f // Needs to be settable in a UAVO // Private types struct GPSGlobals { - float RbeCol1_old[3]; - float gpsVelOld_N; - float gpsVelOld_E; - float gpsVelOld_D; + float RbeCol1_old[3]; + float gpsVelOld_N; + float gpsVelOld_E; + float gpsVelOld_D; }; // Private variables @@ -62,89 +62,83 @@ static struct GPSGlobals *gps; */ void gps_airspeedInitialize() { - //This method saves memory in case we don't use the GPS module. - gps=(struct GPSGlobals *)pvPortMalloc(sizeof(struct GPSGlobals)); - - //GPS airspeed calculation variables - GPSVelocityData gpsVelData; - GPSVelocityGet(&gpsVelData); - - gps->gpsVelOld_N=gpsVelData.North; - gps->gpsVelOld_E=gpsVelData.East; - gps->gpsVelOld_D=gpsVelData.Down; - - AttitudeActualData attData; - AttitudeActualGet(&attData); - - float Rbe[3][3]; - float q[4] ={attData.q1, attData.q2, attData.q3, attData.q4}; - - //Calculate rotation matrix - Quaternion2R(q, Rbe); - - gps->RbeCol1_old[0]=Rbe[0][0]; - gps->RbeCol1_old[1]=Rbe[0][1]; - gps->RbeCol1_old[2]=Rbe[0][2]; + // This method saves memory in case we don't use the GPS module. + gps = (struct GPSGlobals *)pvPortMalloc(sizeof(struct GPSGlobals)); + + // GPS airspeed calculation variables + GPSVelocityData gpsVelData; + GPSVelocityGet(&gpsVelData); + + gps->gpsVelOld_N = gpsVelData.North; + gps->gpsVelOld_E = gpsVelData.East; + gps->gpsVelOld_D = gpsVelData.Down; + + AttitudeActualData attData; + AttitudeActualGet(&attData); + + float Rbe[3][3]; + float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 }; + + // Calculate rotation matrix + Quaternion2R(q, Rbe); + + gps->RbeCol1_old[0] = Rbe[0][0]; + gps->RbeCol1_old[1] = Rbe[0][1]; + gps->RbeCol1_old[2] = Rbe[0][2]; } /* * Calculate airspeed as a function of GPS groundspeed and vehicle attitude. * From "IMU Wind Estimation (Theory)", by William Premerlani. - * The idea is that V_gps=V_air+V_wind. If we assume wind constant, => + * The idea is that V_gps=V_air+V_wind. If we assume wind constant, => * V_gps_2-V_gps_1 = (V_air_2+V_wind_2) -(V_air_1+V_wind_1) = V_air_2 - V_air_1. * If we assume airspeed constant, => V_gps_2-V_gps_1 = |V|*(f_2 - f1), * where "f" is the fuselage vector in earth coordinates. * We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|. */ void gps_airspeedGet(float *v_air_GPS) -{ - float Rbe[3][3]; - - { //Scoping to save memory. We really just need Rbe. - AttitudeActualData attData; - AttitudeActualGet(&attData); - - float q[4] ={attData.q1, attData.q2, attData.q3, attData.q4}; - - //Calculate rotation matrix - Quaternion2R(q, Rbe); - } - - //Calculate the cos(angle) between the two fuselage basis vectors - float cosDiff=(Rbe[0][0]*gps->RbeCol1_old[0]) + (Rbe[0][1]*gps->RbeCol1_old[1]) + (Rbe[0][2]*gps->RbeCol1_old[2]); - - //If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue. - if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) { - GPSVelocityData gpsVelData; - GPSVelocityGet(&gpsVelData); - - //Calculate the norm^2 of the difference between the two GPS vectors - float normDiffGPS2 = powf(gpsVelData.North-gps->gpsVelOld_N,2.0f) + powf(gpsVelData.East-gps->gpsVelOld_E,2.0f) + powf(gpsVelData.Down-gps->gpsVelOld_D,2.0f); +{ + float Rbe[3][3]; - //Calculate the norm^2 of the difference between the two fuselage vectors - float normDiffAttitude2=powf(Rbe[0][0]-gps->RbeCol1_old[0],2.0f) + powf(Rbe[0][1]-gps->RbeCol1_old[1],2.0f) + powf(Rbe[0][2]-gps->RbeCol1_old[2],2.0f); - - //Airspeed magnitude is the ratio between the two difference norms - *v_air_GPS = sqrtf(normDiffGPS2/normDiffAttitude2); - - //Save old variables for next pass - gps->gpsVelOld_N=gpsVelData.North; - gps->gpsVelOld_E=gpsVelData.East; - gps->gpsVelOld_D=gpsVelData.Down; - - gps->RbeCol1_old[0]=Rbe[0][0]; - gps->RbeCol1_old[1]=Rbe[0][1]; - gps->RbeCol1_old[2]=Rbe[0][2]; - } - else { - - } + { // Scoping to save memory. We really just need Rbe. + AttitudeActualData attData; + AttitudeActualGet(&attData); - + float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 }; + + // Calculate rotation matrix + Quaternion2R(q, Rbe); + } + + // Calculate the cos(angle) between the two fuselage basis vectors + float cosDiff = (Rbe[0][0] * gps->RbeCol1_old[0]) + (Rbe[0][1] * gps->RbeCol1_old[1]) + (Rbe[0][2] * gps->RbeCol1_old[2]); + + // If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue. + if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) { + GPSVelocityData gpsVelData; + GPSVelocityGet(&gpsVelData); + + // Calculate the norm^2 of the difference between the two GPS vectors + float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f); + + // Calculate the norm^2 of the difference between the two fuselage vectors + float normDiffAttitude2 = powf(Rbe[0][0] - gps->RbeCol1_old[0], 2.0f) + powf(Rbe[0][1] - gps->RbeCol1_old[1], 2.0f) + powf(Rbe[0][2] - gps->RbeCol1_old[2], 2.0f); + + // Airspeed magnitude is the ratio between the two difference norms + *v_air_GPS = sqrtf(normDiffGPS2 / normDiffAttitude2); + + // Save old variables for next pass + gps->gpsVelOld_N = gpsVelData.North; + gps->gpsVelOld_E = gpsVelData.East; + gps->gpsVelOld_D = gpsVelData.Down; + + gps->RbeCol1_old[0] = Rbe[0][0]; + gps->RbeCol1_old[1] = Rbe[0][1]; + gps->RbeCol1_old[2] = Rbe[0][2]; + } else {} } - /** * @} * @} diff --git a/flight/modules/Airspeed/revolution/inc/airspeed.h b/flight/modules/Airspeed/revolution/inc/airspeed.h index c1bd9c8a7..5b28f1c92 100644 --- a/flight/modules/Airspeed/revolution/inc/airspeed.h +++ b/flight/modules/Airspeed/revolution/inc/airspeed.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Communicate with EagleTree Airspeed Sensor and update @ref BaroAirspeed "BaroAirspeed UAV Object" - * @{ + * @{ * * @file airspeed.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. diff --git a/flight/modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h b/flight/modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h index b19a2b631..77f09cf95 100644 --- a/flight/modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h +++ b/flight/modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements - * @{ + * @{ * * @file baro_airspeed_etasv3.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. diff --git a/flight/modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h b/flight/modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h index bb98da565..5f0072872 100644 --- a/flight/modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h +++ b/flight/modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements - * @{ + * @{ * * @file baro_airspeed_mpxv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. diff --git a/flight/modules/Airspeed/revolution/inc/gps_airspeed.h b/flight/modules/Airspeed/revolution/inc/gps_airspeed.h index 2e744a551..6e0903b8d 100644 --- a/flight/modules/Airspeed/revolution/inc/gps_airspeed.h +++ b/flight/modules/Airspeed/revolution/inc/gps_airspeed.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements - * @{ + * @{ * * @file gps_airspeed.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. diff --git a/flight/modules/Altitude/altitude.c b/flight/modules/Altitude/altitude.c index 8d40706b3..c6302c39f 100644 --- a/flight/modules/Altitude/altitude.c +++ b/flight/modules/Altitude/altitude.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AltitudeModule Altitude Module * @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object" - * @{ + * @{ * * @file altitude.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -41,17 +41,17 @@ #include "hwsettings.h" #include "altitude.h" #if defined(PIOS_INCLUDE_BMP085) -#include "baroaltitude.h" // object that will be updated by the module +#include "baroaltitude.h" // object that will be updated by the module #endif #if defined(PIOS_INCLUDE_HCSR04) -#include "sonaraltitude.h" // object that will be updated by the module +#include "sonaraltitude.h" // object that will be updated by the module #endif #include "taskinfo.h" // Private constants #define STACK_SIZE_BYTES 500 -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) -#define UPDATE_PERIOD 50 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define UPDATE_PERIOD 50 // Private types @@ -60,16 +60,16 @@ static xTaskHandle taskHandle; // down sampling variables #if defined(PIOS_INCLUDE_BMP085) -#define alt_ds_size 4 +#define alt_ds_size 4 static int32_t alt_ds_temp = 0; static int32_t alt_ds_pres = 0; -static int alt_ds_count = 0; +static int alt_ds_count = 0; #endif static bool altitudeEnabled; static uint8_t hwsettings_rcvrport;; - // Private functions +// Private functions static void altitudeTask(void *parameters); /** @@ -78,21 +78,20 @@ static void altitudeTask(void *parameters); */ int32_t AltitudeStart() { - - if (altitudeEnabled) { + if (altitudeEnabled) { #if defined(PIOS_INCLUDE_BMP085) - BaroAltitudeInitialize(); + BaroAltitudeInitialize(); #endif #if defined(PIOS_INCLUDE_HCSR04) - SonarAltitudeInitialize(); + SonarAltitudeInitialize(); #endif - // Start main task - xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle); - return 0; - } - return -1; + // Start main task + xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle); + return 0; + } + return -1; } /** @@ -102,26 +101,26 @@ int32_t AltitudeStart() int32_t AltitudeInitialize() { #ifdef MODULE_ALTITUDE_BUILTIN - altitudeEnabled = 1; + altitudeEnabled = 1; #else - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_ALTITUDE] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - altitudeEnabled = 1; - } else { - altitudeEnabled = 0; - } + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + if (optionalModules[HWSETTINGS_OPTIONALMODULES_ALTITUDE] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + altitudeEnabled = 1; + } else { + altitudeEnabled = 0; + } #endif #if defined(PIOS_INCLUDE_BMP085) - // init down-sampling data - alt_ds_temp = 0; - alt_ds_pres = 0; - alt_ds_count = 0; + // init down-sampling data + alt_ds_temp = 0; + alt_ds_pres = 0; + alt_ds_count = 0; #endif - HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport); - return 0; + HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport); + return 0; } MODULE_INITCALL(AltitudeInitialize, AltitudeStart) /** @@ -129,93 +128,91 @@ MODULE_INITCALL(AltitudeInitialize, AltitudeStart) */ static void altitudeTask(__attribute__((unused)) void *parameters) { - portTickType lastSysTime; - + portTickType lastSysTime; + #if defined(PIOS_INCLUDE_HCSR04) - SonarAltitudeData sonardata; - int32_t value = 0, timeout = 5; - float coeff = 0.25, height_out = 0, height_in = 0; - if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) { - PIOS_HCSR04_Trigger(); - } + SonarAltitudeData sonardata; + int32_t value = 0, timeout = 5; + float coeff = 0.25, height_out = 0, height_in = 0; + if (hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_DISABLED) { + PIOS_HCSR04_Trigger(); + } #endif #if defined(PIOS_INCLUDE_BMP085) - BaroAltitudeData data; - PIOS_BMP085_Init(); + BaroAltitudeData data; + PIOS_BMP085_Init(); #endif - - // Main task loop - lastSysTime = xTaskGetTickCount(); - while (1) - { + + // Main task loop + lastSysTime = xTaskGetTickCount(); + while (1) { #if defined(PIOS_INCLUDE_HCSR04) - // Compute the current altitude - if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) { - if(PIOS_HCSR04_Completed()) { - value = PIOS_HCSR04_Get(); - //from 3.4cm to 5.1m - if((value > 100) && (value < 15000)) { - height_in = value * 0.00034f / 2.0f; - height_out = (height_out * (1 - coeff)) + (height_in * coeff); - sonardata.Altitude = height_out; // m/us - } + // Compute the current altitude + if (hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_DISABLED) { + if (PIOS_HCSR04_Completed()) { + value = PIOS_HCSR04_Get(); + // from 3.4cm to 5.1m + if ((value > 100) && (value < 15000)) { + height_in = value * 0.00034f / 2.0f; + height_out = (height_out * (1 - coeff)) + (height_in * coeff); + sonardata.Altitude = height_out; // m/us + } - // Update the AltitudeActual UAVObject - SonarAltitudeSet(&sonardata); - timeout = 5; - PIOS_HCSR04_Trigger(); - } - if(!(timeout--)) { - //retrigger - timeout = 5; - PIOS_HCSR04_Trigger(); - } - } -#endif + // Update the AltitudeActual UAVObject + SonarAltitudeSet(&sonardata); + timeout = 5; + PIOS_HCSR04_Trigger(); + } + if (!(timeout--)) { + // retrigger + timeout = 5; + PIOS_HCSR04_Trigger(); + } + } +#endif /* if defined(PIOS_INCLUDE_HCSR04) */ #if defined(PIOS_INCLUDE_BMP085) - // Update the temperature data - PIOS_BMP085_StartADC(TemperatureConv); + // Update the temperature data + PIOS_BMP085_StartADC(TemperatureConv); #ifdef PIOS_BMP085_HAS_GPIOS - xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); + xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); #else - vTaskDelay(5 / portTICK_RATE_MS); + vTaskDelay(5 / portTICK_RATE_MS); #endif - PIOS_BMP085_ReadADC(); - alt_ds_temp += PIOS_BMP085_GetTemperature(); - - // Update the pressure data - PIOS_BMP085_StartADC(PressureConv); + PIOS_BMP085_ReadADC(); + alt_ds_temp += PIOS_BMP085_GetTemperature(); + + // Update the pressure data + PIOS_BMP085_StartADC(PressureConv); #ifdef PIOS_BMP085_HAS_GPIOS - xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); + xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); #else - vTaskDelay(26 / portTICK_RATE_MS); + vTaskDelay(26 / portTICK_RATE_MS); #endif - PIOS_BMP085_ReadADC(); - alt_ds_pres += PIOS_BMP085_GetPressure(); - - if (++alt_ds_count >= alt_ds_size) - { - alt_ds_count = 0; + PIOS_BMP085_ReadADC(); + alt_ds_pres += PIOS_BMP085_GetPressure(); - // Convert from 1/10ths of degC to degC - data.Temperature = alt_ds_temp / (10.0 * alt_ds_size); - alt_ds_temp = 0; + if (++alt_ds_count >= alt_ds_size) { + alt_ds_count = 0; - // Convert from Pa to kPa - data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size); - alt_ds_pres = 0; + // Convert from 1/10ths of degC to degC + data.Temperature = alt_ds_temp / (10.0 * alt_ds_size); + alt_ds_temp = 0; - // Compute the current altitude (all pressures in kPa) - data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255))); + // Convert from Pa to kPa + data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size); + alt_ds_pres = 0; - // Update the AltitudeActual UAVObject - BaroAltitudeSet(&data); - } -#endif + // Compute the current altitude (all pressures in kPa) + data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255))); - // Delay until it is time to read the next sample - vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS); - } + // Update the AltitudeActual UAVObject + BaroAltitudeSet(&data); + } +#endif /* if defined(PIOS_INCLUDE_BMP085) */ + + // Delay until it is time to read the next sample + vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS); + } } /** diff --git a/flight/modules/Altitude/inc/altitude.h b/flight/modules/Altitude/inc/altitude.h index 2dad6acc2..651f98072 100644 --- a/flight/modules/Altitude/inc/altitude.h +++ b/flight/modules/Altitude/inc/altitude.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AltitudeModule Altitude Module * @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object" - * @{ + * @{ * * @file altitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index c6449bb4d..0c0c59664 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AltitudeModule Altitude Module * @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object" - * @{ + * @{ * * @file altitude.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -39,15 +39,15 @@ #include #include "altitude.h" -#include "baroaltitude.h" // object that will be updated by the module +#include "baroaltitude.h" // object that will be updated by the module #if defined(PIOS_INCLUDE_HCSR04) -#include "sonaraltitude.h" // object that will be updated by the module +#include "sonaraltitude.h" // object that will be updated by the module #endif #include "taskinfo.h" // Private constants #define STACK_SIZE_BYTES 500 -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types @@ -62,12 +62,12 @@ static void altitudeTask(void *parameters); * \returns 0 on success or -1 if initialisation failed */ int32_t AltitudeStart() -{ - // Start main task - xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle); +{ + // Start main task + xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle); - return 0; + return 0; } /** @@ -76,11 +76,11 @@ int32_t AltitudeStart() */ int32_t AltitudeInitialize() { - BaroAltitudeInitialize(); + BaroAltitudeInitialize(); #if defined(PIOS_INCLUDE_HCSR04) - SonarAltitudeInitialize(); + SonarAltitudeInitialize(); #endif - return 0; + return 0; } MODULE_INITCALL(AltitudeInitialize, AltitudeStart) /** @@ -88,85 +88,83 @@ MODULE_INITCALL(AltitudeInitialize, AltitudeStart) */ static void altitudeTask(__attribute__((unused)) void *parameters) { - BaroAltitudeData data; - + BaroAltitudeData data; + #if defined(PIOS_INCLUDE_HCSR04) - SonarAltitudeData sonardata; - int32_t value = 0, timeout = 10, sample_rate = 0; - float coeff = 0.25, height_out = 0, height_in = 0; - PIOS_HCSR04_Trigger(); + SonarAltitudeData sonardata; + int32_t value = 0, timeout = 10, sample_rate = 0; + float coeff = 0.25, height_out = 0, height_in = 0; + PIOS_HCSR04_Trigger(); #endif - // TODO: Check the pressure sensor and set a warning if it fails test - + // TODO: Check the pressure sensor and set a warning if it fails test + // Option to change the interleave between Temp and Pressure conversions // Undef for normal operation -//#define PIOS_MS5611_SLOW_TEMP_RATE 20 - +// #define PIOS_MS5611_SLOW_TEMP_RATE 20 + #ifdef PIOS_MS5611_SLOW_TEMP_RATE - uint8_t temp_press_interleave_count = 1; + uint8_t temp_press_interleave_count = 1; #endif - // Main task loop - while (1) - { + // Main task loop + while (1) { #if defined(PIOS_INCLUDE_HCSR04) - // Compute the current altitude - // depends on baro samplerate - if(!(sample_rate--)) { - if(PIOS_HCSR04_Completed()) { - value = PIOS_HCSR04_Get(); - //from 3.4cm to 5.1m - if((value > 100) && (value < 15000)) { - height_in = value * 0.00034f / 2.0f; - height_out = (height_out * (1 - coeff)) + (height_in * coeff); - sonardata.Altitude = height_out; // m/us - } + // Compute the current altitude + // depends on baro samplerate + if (!(sample_rate--)) { + if (PIOS_HCSR04_Completed()) { + value = PIOS_HCSR04_Get(); + // from 3.4cm to 5.1m + if ((value > 100) && (value < 15000)) { + height_in = value * 0.00034f / 2.0f; + height_out = (height_out * (1 - coeff)) + (height_in * coeff); + sonardata.Altitude = height_out; // m/us + } - // Update the SonarAltitude UAVObject - SonarAltitudeSet(&sonardata); - timeout = 10; - PIOS_HCSR04_Trigger(); - } - if(!(timeout--)) { - //retrigger - timeout = 10; - PIOS_HCSR04_Trigger(); - } - sample_rate = 25; - } -#endif - float temp, press; + // Update the SonarAltitude UAVObject + SonarAltitudeSet(&sonardata); + timeout = 10; + PIOS_HCSR04_Trigger(); + } + if (!(timeout--)) { + // retrigger + timeout = 10; + PIOS_HCSR04_Trigger(); + } + sample_rate = 25; + } +#endif /* if defined(PIOS_INCLUDE_HCSR04) */ + float temp, press; #ifdef PIOS_MS5611_SLOW_TEMP_RATE - temp_press_interleave_count --; - if(temp_press_interleave_count == 0) - { + temp_press_interleave_count--; + if (temp_press_interleave_count == 0) { #endif - // Update the temperature data - PIOS_MS5611_StartADC(TemperatureConv); - vTaskDelay(PIOS_MS5611_GetDelay()); - PIOS_MS5611_ReadADC(); - -#ifdef PIOS_MS5611_SLOW_TEMP_RATE - temp_press_interleave_count=PIOS_MS5611_SLOW_TEMP_RATE; - } -#endif - - // Update the pressure data - PIOS_MS5611_StartADC(PressureConv); - vTaskDelay(PIOS_MS5611_GetDelay()); - PIOS_MS5611_ReadADC(); + // Update the temperature data + PIOS_MS5611_StartADC(TemperatureConv); + vTaskDelay(PIOS_MS5611_GetDelay()); + PIOS_MS5611_ReadADC(); - - temp = PIOS_MS5611_GetTemperature(); - press = PIOS_MS5611_GetPressure(); - - data.Temperature = temp; - data.Pressure = press; - data.Altitude = 44330.0f * (1.0f - powf(data.Pressure / MS5611_P0, (1.0f / 5.255f))); - - // Update the AltitudeActual UAVObject - BaroAltitudeSet(&data); - } +#ifdef PIOS_MS5611_SLOW_TEMP_RATE + temp_press_interleave_count = PIOS_MS5611_SLOW_TEMP_RATE; + } +#endif + + // Update the pressure data + PIOS_MS5611_StartADC(PressureConv); + vTaskDelay(PIOS_MS5611_GetDelay()); + PIOS_MS5611_ReadADC(); + + + temp = PIOS_MS5611_GetTemperature(); + press = PIOS_MS5611_GetPressure(); + + data.Temperature = temp; + data.Pressure = press; + data.Altitude = 44330.0f * (1.0f - powf(data.Pressure / MS5611_P0, (1.0f / 5.255f))); + + // Update the AltitudeActual UAVObject + BaroAltitudeSet(&data); + } } /** diff --git a/flight/modules/Altitude/revolution/inc/altitude.h b/flight/modules/Altitude/revolution/inc/altitude.h index 2dad6acc2..651f98072 100644 --- a/flight/modules/Altitude/revolution/inc/altitude.h +++ b/flight/modules/Altitude/revolution/inc/altitude.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AltitudeModule Altitude Module * @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object" - * @{ + * @{ * * @file altitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index e7eaabbc7..823be87ca 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -3,7 +3,7 @@ * * @file guidance.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint + * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint * and sets @ref AttitudeDesired. It only does this when the FlightMode field * of @ref ManualControlCommand is Auto. * @@ -50,7 +50,7 @@ #include #include #include -#include // object that will be updated by the module +#include // object that will be updated by the module #include #include #include @@ -59,9 +59,9 @@ #include #include // Private constants -#define MAX_QUEUE_SIZE 2 +#define MAX_QUEUE_SIZE 2 #define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define ACCEL_DOWNSAMPLE 4 // Private types @@ -72,7 +72,7 @@ static AltitudeHoldSettingsData altitudeHoldSettings; // Private functions static void altitudeHoldTask(void *parameters); -static void SettingsUpdatedCb(UAVObjEvent * ev); +static void SettingsUpdatedCb(UAVObjEvent *ev); /** * Initialise the module, called on startup @@ -80,11 +80,11 @@ static void SettingsUpdatedCb(UAVObjEvent * ev); */ int32_t AltitudeHoldStart() { - // Start main task - xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle); + // Start main task + xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle); - return 0; + return 0; } /** @@ -93,16 +93,16 @@ int32_t AltitudeHoldStart() */ int32_t AltitudeHoldInitialize() { - AltitudeHoldSettingsInitialize(); - AltitudeHoldDesiredInitialize(); - AltHoldSmoothedInitialize(); + AltitudeHoldSettingsInitialize(); + AltitudeHoldDesiredInitialize(); + AltHoldSmoothedInitialize(); - // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + // Create object queue + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); + AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); - return 0; + return 0; } MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart) @@ -122,274 +122,278 @@ float starting_altitude; */ static void altitudeHoldTask(__attribute__((unused)) void *parameters) { - AltitudeHoldDesiredData altitudeHoldDesired; - StabilizationDesiredData stabilizationDesired; + AltitudeHoldDesiredData altitudeHoldDesired; + StabilizationDesiredData stabilizationDesired; - portTickType thisTime, lastUpdateTime; - UAVObjEvent ev; - - // Force update of the settings - SettingsUpdatedCb(&ev); + portTickType thisTime, lastUpdateTime; + UAVObjEvent ev; - // Listen for updates. - AltitudeHoldDesiredConnectQueue(queue); - BaroAltitudeConnectQueue(queue); - FlightStatusConnectQueue(queue); - AccelsConnectQueue(queue); - - BaroAltitudeAltitudeGet(&smoothed_altitude); - running = false; - enum init_state {WAITING_BARO, WAITIING_INIT, INITED} init = WAITING_BARO; + // Force update of the settings + SettingsUpdatedCb(&ev); - // Main task loop - bool baro_updated = false; - while (1) { - // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe - if ( xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE ) - { - if(!running) - throttleIntegral = 0; + // Listen for updates. + AltitudeHoldDesiredConnectQueue(queue); + BaroAltitudeConnectQueue(queue); + FlightStatusConnectQueue(queue); + AccelsConnectQueue(queue); - // Todo: Add alarm if it should be running - continue; - } else if (ev.obj == BaroAltitudeHandle()) { - baro_updated = true; - - init = (init == WAITING_BARO) ? WAITIING_INIT : init; - } else if (ev.obj == FlightStatusHandle()) { - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); + BaroAltitudeAltitudeGet(&smoothed_altitude); + running = false; + enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO; - if(flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !running) { - // Copy the current throttle as a starting point for integral - StabilizationDesiredThrottleGet(&throttleIntegral); - switchThrottle = throttleIntegral; - error = 0; - velocity = 0; - running = true; + // Main task loop + bool baro_updated = false; + while (1) { + // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe + if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) { + if (!running) { + throttleIntegral = 0; + } - AltHoldSmoothedData altHold; - AltHoldSmoothedGet(&altHold); - starting_altitude = altHold.Altitude; - } else if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) - running = false; - } else if (ev.obj == AccelsHandle()) { - static uint32_t timeval; - - static float z[4] = {0, 0, 0, 0}; - float z_new[4]; - float P[4][4], K[4][2], x[2]; - float G[4] = {1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7f}; - static float V[4][4] = {{10.0f, 0.0f, 0.0f, 0.0f}, - {0.0f, 100.0f, 0.0f, 0.0f}, - {0.0f, 0.0f, 100.0f, 0.0f}, - {0.0f, 0.0f, 0.0f, 1000.0f}}; - static uint32_t accel_downsample_count = 0; - static float accels_accum[3] = {0.0f, 0.0f, 0.0f}; - float dT; - static float S[2] = {1.0f,10.0f}; + // Todo: Add alarm if it should be running + continue; + } else if (ev.obj == BaroAltitudeHandle()) { + baro_updated = true; - /* Somehow this always assigns to zero. Compiler bug? Race condition? */ - S[0] = altitudeHoldSettings.PressureNoise; - S[1] = altitudeHoldSettings.AccelNoise; - G[2] = altitudeHoldSettings.AccelDrift; + init = (init == WAITING_BARO) ? WAITIING_INIT : init; + } else if (ev.obj == FlightStatusHandle()) { + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); - AccelsData accels; - AccelsGet(&accels); - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - BaroAltitudeData baro; - BaroAltitudeGet(&baro); + if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !running) { + // Copy the current throttle as a starting point for integral + StabilizationDesiredThrottleGet(&throttleIntegral); + switchThrottle = throttleIntegral; + error = 0; + velocity = 0; + running = true; - /* Downsample accels to stop this calculation consuming too much CPU */ - accels_accum[0] += accels.x; - accels_accum[1] += accels.y; - accels_accum[2] += accels.z; - accel_downsample_count++; + AltHoldSmoothedData altHold; + AltHoldSmoothedGet(&altHold); + starting_altitude = altHold.Altitude; + } else if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) { + running = false; + } + } else if (ev.obj == AccelsHandle()) { + static uint32_t timeval; - if (accel_downsample_count < ACCEL_DOWNSAMPLE) - continue; + static float z[4] = { 0, 0, 0, 0 }; + float z_new[4]; + float P[4][4], K[4][2], x[2]; + float G[4] = { 1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7f }; + static float V[4][4] = { + { 10.0f, 0.0f, 0.0f, 0.0f }, + { 0.0f, 100.0f, 0.0f, 0.0f }, + { 0.0f, 0.0f, 100.0f, 0.0f }, + { 0.0f, 0.0f, 0.0f, 1000.0f } + }; + static uint32_t accel_downsample_count = 0; + static float accels_accum[3] = { 0.0f, 0.0f, 0.0f }; + float dT; + static float S[2] = { 1.0f, 10.0f }; - accel_downsample_count = 0; - accels.x = accels_accum[0] / ACCEL_DOWNSAMPLE; - accels.y = accels_accum[1] / ACCEL_DOWNSAMPLE; - accels.z = accels_accum[2] / ACCEL_DOWNSAMPLE; - accels_accum[0] = accels_accum[1] = accels_accum[2] = 0; + /* Somehow this always assigns to zero. Compiler bug? Race condition? */ + S[0] = altitudeHoldSettings.PressureNoise; + S[1] = altitudeHoldSettings.AccelNoise; + G[2] = altitudeHoldSettings.AccelDrift; - thisTime = xTaskGetTickCount(); + AccelsData accels; + AccelsGet(&accels); + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); + BaroAltitudeData baro; + BaroAltitudeGet(&baro); - if (init == WAITIING_INIT) { - z[0] = baro.Altitude; - z[1] = 0; - z[2] = accels.z; - z[3] = 0; - init = INITED; - } else if (init == WAITING_BARO) - continue; + /* Downsample accels to stop this calculation consuming too much CPU */ + accels_accum[0] += accels.x; + accels_accum[1] += accels.y; + accels_accum[2] += accels.z; + accel_downsample_count++; - x[0] = baro.Altitude; - //rotate avg accels into earth frame and store it - if(1) { - float q[4], Rbe[3][3]; - q[0]=attitudeActual.q1; - q[1]=attitudeActual.q2; - q[2]=attitudeActual.q3; - q[3]=attitudeActual.q4; - Quaternion2R(q, Rbe); - x[1] = -(Rbe[0][2]*accels.x+ Rbe[1][2]*accels.y + Rbe[2][2]*accels.z + 9.81f); - } else { - x[1] = -accels.z + 9.81f; - } + if (accel_downsample_count < ACCEL_DOWNSAMPLE) { + continue; + } - dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; - timeval = PIOS_DELAY_GetRaw(); + accel_downsample_count = 0; + accels.x = accels_accum[0] / ACCEL_DOWNSAMPLE; + accels.y = accels_accum[1] / ACCEL_DOWNSAMPLE; + accels.z = accels_accum[2] / ACCEL_DOWNSAMPLE; + accels_accum[0] = accels_accum[1] = accels_accum[2] = 0; - P[0][0] = dT*(V[0][1]+dT*V[1][1])+V[0][0]+G[0]+dT*V[1][0]; - P[0][1] = dT*(V[0][2]+dT*V[1][2])+V[0][1]+dT*V[1][1]; - P[0][2] = V[0][2]+dT*V[1][2]; - P[0][3] = V[0][3]+dT*V[1][3]; - P[1][0] = dT*(V[1][1]+dT*V[2][1])+V[1][0]+dT*V[2][0]; - P[1][1] = dT*(V[1][2]+dT*V[2][2])+V[1][1]+G[1]+dT*V[2][1]; - P[1][2] = V[1][2]+dT*V[2][2]; - P[1][3] = V[1][3]+dT*V[2][3]; - P[2][0] = V[2][0]+dT*V[2][1]; - P[2][1] = V[2][1]+dT*V[2][2]; - P[2][2] = V[2][2]+G[2]; - P[2][3] = V[2][3]; - P[3][0] = V[3][0]+dT*V[3][1]; - P[3][1] = V[3][1]+dT*V[3][2]; - P[3][2] = V[3][2]; - P[3][3] = V[3][3]+G[3]; + thisTime = xTaskGetTickCount(); - if (baro_updated) { - K[0][0] = -(V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3])+1.0f; - K[0][1] = ((V[0][2]+V[0][3])*S[0]+dT*(V[1][2]+V[1][3])*S[0])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); - K[1][0] = (V[1][0]*G[2]+V[1][0]*G[3]+V[1][0]*S[1]+V[1][0]*V[2][2]-V[2][0]*V[1][2]+V[1][0]*V[2][3]+V[1][0]*V[3][2]-V[2][0]*V[1][3]-V[1][2]*V[3][0]+V[1][0]*V[3][3]-V[3][0]*V[1][3]+(dT*dT)*V[2][1]*V[3][2]-(dT*dT)*V[2][2]*V[3][1]+(dT*dT)*V[2][1]*V[3][3]-(dT*dT)*V[3][1]*V[2][3]+dT*V[1][1]*G[2]+dT*V[2][0]*G[2]+dT*V[1][1]*G[3]+dT*V[2][0]*G[3]+dT*V[1][1]*S[1]+dT*V[2][0]*S[1]+(dT*dT)*V[2][1]*G[2]+(dT*dT)*V[2][1]*G[3]+(dT*dT)*V[2][1]*S[1]+dT*V[1][1]*V[2][2]-dT*V[1][2]*V[2][1]+dT*V[1][1]*V[2][3]+dT*V[1][1]*V[3][2]+dT*V[2][0]*V[3][2]-dT*V[1][2]*V[3][1]-dT*V[2][1]*V[1][3]-dT*V[3][0]*V[2][2]+dT*V[1][1]*V[3][3]+dT*V[2][0]*V[3][3]-dT*V[3][0]*V[2][3]-dT*V[1][3]*V[3][1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); - K[1][1] = (V[1][2]*G[0]+V[1][3]*G[0]+V[1][2]*S[0]+V[1][3]*S[0]+V[0][0]*V[1][2]-V[1][0]*V[0][2]+V[0][0]*V[1][3]-V[1][0]*V[0][3]+(dT*dT)*V[0][1]*V[2][2]+(dT*dT)*V[1][0]*V[2][2]-(dT*dT)*V[0][2]*V[2][1]-(dT*dT)*V[2][0]*V[1][2]+(dT*dT)*V[0][1]*V[2][3]+(dT*dT)*V[1][0]*V[2][3]-(dT*dT)*V[2][0]*V[1][3]-(dT*dT)*V[0][3]*V[2][1]+(dT*dT*dT)*V[1][1]*V[2][2]-(dT*dT*dT)*V[1][2]*V[2][1]+(dT*dT*dT)*V[1][1]*V[2][3]-(dT*dT*dT)*V[2][1]*V[1][3]+dT*V[2][2]*G[0]+dT*V[2][3]*G[0]+dT*V[2][2]*S[0]+dT*V[2][3]*S[0]+dT*V[0][0]*V[2][2]+dT*V[0][1]*V[1][2]-dT*V[0][2]*V[1][1]-dT*V[0][2]*V[2][0]+dT*V[0][0]*V[2][3]+dT*V[0][1]*V[1][3]-dT*V[1][1]*V[0][3]-dT*V[2][0]*V[0][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); - K[2][0] = (V[2][0]*G[3]-V[3][0]*G[2]+V[2][0]*S[1]+V[2][0]*V[3][2]-V[3][0]*V[2][2]+V[2][0]*V[3][3]-V[3][0]*V[2][3]+dT*V[2][1]*G[3]-dT*V[3][1]*G[2]+dT*V[2][1]*S[1]+dT*V[2][1]*V[3][2]-dT*V[2][2]*V[3][1]+dT*V[2][1]*V[3][3]-dT*V[3][1]*V[2][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); - K[2][1] = (V[0][0]*G[2]+V[2][2]*G[0]+V[2][3]*G[0]+V[2][2]*S[0]+V[2][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]-V[2][0]*V[0][3]+G[0]*G[2]+G[2]*S[0]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]-(dT*dT)*V[2][1]*V[1][3]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+(dT*dT)*V[1][1]*G[2]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[1][0]*V[2][3]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); - K[3][0] = (-V[2][0]*G[3]+V[3][0]*G[2]+V[3][0]*S[1]-V[2][0]*V[3][2]+V[3][0]*V[2][2]-V[2][0]*V[3][3]+V[3][0]*V[2][3]-dT*V[2][1]*G[3]+dT*V[3][1]*G[2]+dT*V[3][1]*S[1]-dT*V[2][1]*V[3][2]+dT*V[2][2]*V[3][1]-dT*V[2][1]*V[3][3]+dT*V[3][1]*V[2][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); - K[3][1] = (V[0][0]*G[3]+V[3][2]*G[0]+V[3][3]*G[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[3][2]-V[0][2]*V[3][0]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[3]+G[3]*S[0]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+(dT*dT)*V[1][1]*G[3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); - - z_new[0] = -K[0][0]*(dT*z[1]-x[0]+z[0])+dT*z[1]-K[0][1]*(-x[1]+z[2]+z[3])+z[0]; - z_new[1] = -K[1][0]*(dT*z[1]-x[0]+z[0])+dT*z[2]-K[1][1]*(-x[1]+z[2]+z[3])+z[1]; - z_new[2] = -K[2][0]*(dT*z[1]-x[0]+z[0])-K[2][1]*(-x[1]+z[2]+z[3])+z[2]; - z_new[3] = -K[3][0]*(dT*z[1]-x[0]+z[0])-K[3][1]*(-x[1]+z[2]+z[3])+z[3]; + if (init == WAITIING_INIT) { + z[0] = baro.Altitude; + z[1] = 0; + z[2] = accels.z; + z[3] = 0; + init = INITED; + } else if (init == WAITING_BARO) { + continue; + } - memcpy(z, z_new, sizeof(z_new)); - - V[0][0] = -K[0][1]*P[2][0]-K[0][1]*P[3][0]-P[0][0]*(K[0][0]-1.0f); - V[0][1] = -K[0][1]*P[2][1]-K[0][1]*P[3][2]-P[0][1]*(K[0][0]-1.0f); - V[0][2] = -K[0][1]*P[2][2]-K[0][1]*P[3][2]-P[0][2]*(K[0][0]-1.0f); - V[0][3] = -K[0][1]*P[2][3]-K[0][1]*P[3][3]-P[0][3]*(K[0][0]-1.0f); - V[1][0] = P[1][0]-K[1][0]*P[0][0]-K[1][1]*P[2][0]-K[1][1]*P[3][0]; - V[1][1] = P[1][1]-K[1][0]*P[0][1]-K[1][1]*P[2][1]-K[1][1]*P[3][2]; - V[1][2] = P[1][2]-K[1][0]*P[0][2]-K[1][1]*P[2][2]-K[1][1]*P[3][2]; - V[1][3] = P[1][3]-K[1][0]*P[0][3]-K[1][1]*P[2][3]-K[1][1]*P[3][3]; - V[2][0] = -K[2][0]*P[0][0]-K[2][1]*P[3][0]-P[2][0]*(K[2][1]-1.0f); - V[2][1] = -K[2][0]*P[0][1]-K[2][1]*P[3][2]-P[2][1]*(K[2][1]-1.0f); - V[2][2] = -K[2][0]*P[0][2]-K[2][1]*P[3][2]-P[2][2]*(K[2][1]-1.0f); - V[2][3] = -K[2][0]*P[0][3]-K[2][1]*P[3][3]-P[2][3]*(K[2][1]-1.0f); - V[3][0] = -K[3][0]*P[0][0]-K[3][1]*P[2][0]-P[3][0]*(K[3][1]-1.0f); - V[3][1] = -K[3][0]*P[0][1]-K[3][1]*P[2][1]-P[3][2]*(K[3][1]-1.0f); - V[3][2] = -K[3][0]*P[0][2]-K[3][1]*P[2][2]-P[3][2]*(K[3][1]-1.0f); - V[3][3] = -K[3][0]*P[0][3]-K[3][1]*P[2][3]-P[3][3]*(K[3][1]-1.0f); - + x[0] = baro.Altitude; + // rotate avg accels into earth frame and store it + if (1) { + float q[4], Rbe[3][3]; + q[0] = attitudeActual.q1; + q[1] = attitudeActual.q2; + q[2] = attitudeActual.q3; + q[3] = attitudeActual.q4; + Quaternion2R(q, Rbe); + x[1] = -(Rbe[0][2] * accels.x + Rbe[1][2] * accels.y + Rbe[2][2] * accels.z + 9.81f); + } else { + x[1] = -accels.z + 9.81f; + } - baro_updated = false; - } else { - K[0][0] = (V[0][2]+V[0][3]+dT*V[1][2]+dT*V[1][3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]); - K[1][0] = (V[1][2]+V[1][3]+dT*V[2][2]+dT*V[2][3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]); - K[2][0] = (V[2][2]+V[2][3]+G[2])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]); - K[3][0] = (V[3][2]+V[3][3]+G[3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]); - + dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; + timeval = PIOS_DELAY_GetRaw(); - z_new[0] = dT*z[1]-K[0][0]*(-x[1]+z[2]+z[3])+z[0]; - z_new[1] = dT*z[2]-K[1][0]*(-x[1]+z[2]+z[3])+z[1]; - z_new[2] = -K[2][0]*(-x[1]+z[2]+z[3])+z[2]; - z_new[3] = -K[3][0]*(-x[1]+z[2]+z[3])+z[3]; + P[0][0] = dT * (V[0][1] + dT * V[1][1]) + V[0][0] + G[0] + dT * V[1][0]; + P[0][1] = dT * (V[0][2] + dT * V[1][2]) + V[0][1] + dT * V[1][1]; + P[0][2] = V[0][2] + dT * V[1][2]; + P[0][3] = V[0][3] + dT * V[1][3]; + P[1][0] = dT * (V[1][1] + dT * V[2][1]) + V[1][0] + dT * V[2][0]; + P[1][1] = dT * (V[1][2] + dT * V[2][2]) + V[1][1] + G[1] + dT * V[2][1]; + P[1][2] = V[1][2] + dT * V[2][2]; + P[1][3] = V[1][3] + dT * V[2][3]; + P[2][0] = V[2][0] + dT * V[2][1]; + P[2][1] = V[2][1] + dT * V[2][2]; + P[2][2] = V[2][2] + G[2]; + P[2][3] = V[2][3]; + P[3][0] = V[3][0] + dT * V[3][1]; + P[3][1] = V[3][1] + dT * V[3][2]; + P[3][2] = V[3][2]; + P[3][3] = V[3][3] + G[3]; - memcpy(z, z_new, sizeof(z_new)); + if (baro_updated) { + K[0][0] = -(V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) + 1.0f; + K[0][1] = ((V[0][2] + V[0][3]) * S[0] + dT * (V[1][2] + V[1][3]) * S[0]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); + K[1][0] = (V[1][0] * G[2] + V[1][0] * G[3] + V[1][0] * S[1] + V[1][0] * V[2][2] - V[2][0] * V[1][2] + V[1][0] * V[2][3] + V[1][0] * V[3][2] - V[2][0] * V[1][3] - V[1][2] * V[3][0] + V[1][0] * V[3][3] - V[3][0] * V[1][3] + (dT * dT) * V[2][1] * V[3][2] - (dT * dT) * V[2][2] * V[3][1] + (dT * dT) * V[2][1] * V[3][3] - (dT * dT) * V[3][1] * V[2][3] + dT * V[1][1] * G[2] + dT * V[2][0] * G[2] + dT * V[1][1] * G[3] + dT * V[2][0] * G[3] + dT * V[1][1] * S[1] + dT * V[2][0] * S[1] + (dT * dT) * V[2][1] * G[2] + (dT * dT) * V[2][1] * G[3] + (dT * dT) * V[2][1] * S[1] + dT * V[1][1] * V[2][2] - dT * V[1][2] * V[2][1] + dT * V[1][1] * V[2][3] + dT * V[1][1] * V[3][2] + dT * V[2][0] * V[3][2] - dT * V[1][2] * V[3][1] - dT * V[2][1] * V[1][3] - dT * V[3][0] * V[2][2] + dT * V[1][1] * V[3][3] + dT * V[2][0] * V[3][3] - dT * V[3][0] * V[2][3] - dT * V[1][3] * V[3][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); + K[1][1] = (V[1][2] * G[0] + V[1][3] * G[0] + V[1][2] * S[0] + V[1][3] * S[0] + V[0][0] * V[1][2] - V[1][0] * V[0][2] + V[0][0] * V[1][3] - V[1][0] * V[0][3] + (dT * dT) * V[0][1] * V[2][2] + (dT * dT) * V[1][0] * V[2][2] - (dT * dT) * V[0][2] * V[2][1] - (dT * dT) * V[2][0] * V[1][2] + (dT * dT) * V[0][1] * V[2][3] + (dT * dT) * V[1][0] * V[2][3] - (dT * dT) * V[2][0] * V[1][3] - (dT * dT) * V[0][3] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][2] - (dT * dT * dT) * V[1][2] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][3] - (dT * dT * dT) * V[2][1] * V[1][3] + dT * V[2][2] * G[0] + dT * V[2][3] * G[0] + dT * V[2][2] * S[0] + dT * V[2][3] * S[0] + dT * V[0][0] * V[2][2] + dT * V[0][1] * V[1][2] - dT * V[0][2] * V[1][1] - dT * V[0][2] * V[2][0] + dT * V[0][0] * V[2][3] + dT * V[0][1] * V[1][3] - dT * V[1][1] * V[0][3] - dT * V[2][0] * V[0][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); + K[2][0] = (V[2][0] * G[3] - V[3][0] * G[2] + V[2][0] * S[1] + V[2][0] * V[3][2] - V[3][0] * V[2][2] + V[2][0] * V[3][3] - V[3][0] * V[2][3] + dT * V[2][1] * G[3] - dT * V[3][1] * G[2] + dT * V[2][1] * S[1] + dT * V[2][1] * V[3][2] - dT * V[2][2] * V[3][1] + dT * V[2][1] * V[3][3] - dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); + K[2][1] = (V[0][0] * G[2] + V[2][2] * G[0] + V[2][3] * G[0] + V[2][2] * S[0] + V[2][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] - V[2][0] * V[0][3] + G[0] * G[2] + G[2] * S[0] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] - (dT * dT) * V[2][1] * V[1][3] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + (dT * dT) * V[1][1] * G[2] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[1][0] * V[2][3] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); + K[3][0] = (-V[2][0] * G[3] + V[3][0] * G[2] + V[3][0] * S[1] - V[2][0] * V[3][2] + V[3][0] * V[2][2] - V[2][0] * V[3][3] + V[3][0] * V[2][3] - dT * V[2][1] * G[3] + dT * V[3][1] * G[2] + dT * V[3][1] * S[1] - dT * V[2][1] * V[3][2] + dT * V[2][2] * V[3][1] - dT * V[2][1] * V[3][3] + dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); + K[3][1] = (V[0][0] * G[3] + V[3][2] * G[0] + V[3][3] * G[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[3][2] - V[0][2] * V[3][0] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[3] + G[3] * S[0] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + (dT * dT) * V[1][1] * G[3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); - V[0][0] = P[0][0]-K[0][0]*P[2][0]-K[0][0]*P[3][0]; - V[0][1] = P[0][1]-K[0][0]*P[2][1]-K[0][0]*P[3][2]; - V[0][2] = P[0][2]-K[0][0]*P[2][2]-K[0][0]*P[3][2]; - V[0][3] = P[0][3]-K[0][0]*P[2][3]-K[0][0]*P[3][3]; - V[1][0] = P[1][0]-K[1][0]*P[2][0]-K[1][0]*P[3][0]; - V[1][1] = P[1][1]-K[1][0]*P[2][1]-K[1][0]*P[3][2]; - V[1][2] = P[1][2]-K[1][0]*P[2][2]-K[1][0]*P[3][2]; - V[1][3] = P[1][3]-K[1][0]*P[2][3]-K[1][0]*P[3][3]; - V[2][0] = -K[2][0]*P[3][0]-P[2][0]*(K[2][0]-1.0f); - V[2][1] = -K[2][0]*P[3][2]-P[2][1]*(K[2][0]-1.0f); - V[2][2] = -K[2][0]*P[3][2]-P[2][2]*(K[2][0]-1.0f); - V[2][3] = -K[2][0]*P[3][3]-P[2][3]*(K[2][0]-1.0f); - V[3][0] = -K[3][0]*P[2][0]-P[3][0]*(K[3][0]-1.0f); - V[3][1] = -K[3][0]*P[2][1]-P[3][2]*(K[3][0]-1.0f); - V[3][2] = -K[3][0]*P[2][2]-P[3][2]*(K[3][0]-1.0f); - V[3][3] = -K[3][0]*P[2][3]-P[3][3]*(K[3][0]-1.0f); - } + z_new[0] = -K[0][0] * (dT * z[1] - x[0] + z[0]) + dT * z[1] - K[0][1] * (-x[1] + z[2] + z[3]) + z[0]; + z_new[1] = -K[1][0] * (dT * z[1] - x[0] + z[0]) + dT * z[2] - K[1][1] * (-x[1] + z[2] + z[3]) + z[1]; + z_new[2] = -K[2][0] * (dT * z[1] - x[0] + z[0]) - K[2][1] * (-x[1] + z[2] + z[3]) + z[2]; + z_new[3] = -K[3][0] * (dT * z[1] - x[0] + z[0]) - K[3][1] * (-x[1] + z[2] + z[3]) + z[3]; - AltHoldSmoothedData altHold; - AltHoldSmoothedGet(&altHold); - altHold.Altitude = z[0]; - altHold.Velocity = z[1]; - altHold.Accel = z[2]; - AltHoldSmoothedSet(&altHold); + memcpy(z, z_new, sizeof(z_new)); - AltHoldSmoothedGet(&altHold); + V[0][0] = -K[0][1] * P[2][0] - K[0][1] * P[3][0] - P[0][0] * (K[0][0] - 1.0f); + V[0][1] = -K[0][1] * P[2][1] - K[0][1] * P[3][2] - P[0][1] * (K[0][0] - 1.0f); + V[0][2] = -K[0][1] * P[2][2] - K[0][1] * P[3][2] - P[0][2] * (K[0][0] - 1.0f); + V[0][3] = -K[0][1] * P[2][3] - K[0][1] * P[3][3] - P[0][3] * (K[0][0] - 1.0f); + V[1][0] = P[1][0] - K[1][0] * P[0][0] - K[1][1] * P[2][0] - K[1][1] * P[3][0]; + V[1][1] = P[1][1] - K[1][0] * P[0][1] - K[1][1] * P[2][1] - K[1][1] * P[3][2]; + V[1][2] = P[1][2] - K[1][0] * P[0][2] - K[1][1] * P[2][2] - K[1][1] * P[3][2]; + V[1][3] = P[1][3] - K[1][0] * P[0][3] - K[1][1] * P[2][3] - K[1][1] * P[3][3]; + V[2][0] = -K[2][0] * P[0][0] - K[2][1] * P[3][0] - P[2][0] * (K[2][1] - 1.0f); + V[2][1] = -K[2][0] * P[0][1] - K[2][1] * P[3][2] - P[2][1] * (K[2][1] - 1.0f); + V[2][2] = -K[2][0] * P[0][2] - K[2][1] * P[3][2] - P[2][2] * (K[2][1] - 1.0f); + V[2][3] = -K[2][0] * P[0][3] - K[2][1] * P[3][3] - P[2][3] * (K[2][1] - 1.0f); + V[3][0] = -K[3][0] * P[0][0] - K[3][1] * P[2][0] - P[3][0] * (K[3][1] - 1.0f); + V[3][1] = -K[3][0] * P[0][1] - K[3][1] * P[2][1] - P[3][2] * (K[3][1] - 1.0f); + V[3][2] = -K[3][0] * P[0][2] - K[3][1] * P[2][2] - P[3][2] * (K[3][1] - 1.0f); + V[3][3] = -K[3][0] * P[0][3] - K[3][1] * P[2][3] - P[3][3] * (K[3][1] - 1.0f); - // Verify that we are in altitude hold mode - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || - flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) { - running = false; - } - if (!running) - continue; + baro_updated = false; + } else { + K[0][0] = (V[0][2] + V[0][3] + dT * V[1][2] + dT * V[1][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); + K[1][0] = (V[1][2] + V[1][3] + dT * V[2][2] + dT * V[2][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); + K[2][0] = (V[2][2] + V[2][3] + G[2]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); + K[3][0] = (V[3][2] + V[3][3] + G[3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); - // Compute the altitude error - error = (starting_altitude + altitudeHoldDesired.Altitude) - altHold.Altitude; - // Compute integral off altitude error - throttleIntegral += error * altitudeHoldSettings.Ki * dT; + z_new[0] = dT * z[1] - K[0][0] * (-x[1] + z[2] + z[3]) + z[0]; + z_new[1] = dT * z[2] - K[1][0] * (-x[1] + z[2] + z[3]) + z[1]; + z_new[2] = -K[2][0] * (-x[1] + z[2] + z[3]) + z[2]; + z_new[3] = -K[3][0] * (-x[1] + z[2] + z[3]) + z[3]; - // Only update stabilizationDesired less frequently - if((thisTime - lastUpdateTime) < 20) - continue; + memcpy(z, z_new, sizeof(z_new)); - lastUpdateTime = thisTime; + V[0][0] = P[0][0] - K[0][0] * P[2][0] - K[0][0] * P[3][0]; + V[0][1] = P[0][1] - K[0][0] * P[2][1] - K[0][0] * P[3][2]; + V[0][2] = P[0][2] - K[0][0] * P[2][2] - K[0][0] * P[3][2]; + V[0][3] = P[0][3] - K[0][0] * P[2][3] - K[0][0] * P[3][3]; + V[1][0] = P[1][0] - K[1][0] * P[2][0] - K[1][0] * P[3][0]; + V[1][1] = P[1][1] - K[1][0] * P[2][1] - K[1][0] * P[3][2]; + V[1][2] = P[1][2] - K[1][0] * P[2][2] - K[1][0] * P[3][2]; + V[1][3] = P[1][3] - K[1][0] * P[2][3] - K[1][0] * P[3][3]; + V[2][0] = -K[2][0] * P[3][0] - P[2][0] * (K[2][0] - 1.0f); + V[2][1] = -K[2][0] * P[3][2] - P[2][1] * (K[2][0] - 1.0f); + V[2][2] = -K[2][0] * P[3][2] - P[2][2] * (K[2][0] - 1.0f); + V[2][3] = -K[2][0] * P[3][3] - P[2][3] * (K[2][0] - 1.0f); + V[3][0] = -K[3][0] * P[2][0] - P[3][0] * (K[3][0] - 1.0f); + V[3][1] = -K[3][0] * P[2][1] - P[3][2] * (K[3][0] - 1.0f); + V[3][2] = -K[3][0] * P[2][2] - P[3][2] * (K[3][0] - 1.0f); + V[3][3] = -K[3][0] * P[2][3] - P[3][3] * (K[3][0] - 1.0f); + } - // Instead of explicit limit on integral you output limit feedback - StabilizationDesiredGet(&stabilizationDesired); - stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral - - altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; - if(stabilizationDesired.Throttle > 1) { - throttleIntegral -= (stabilizationDesired.Throttle - 1); - stabilizationDesired.Throttle = 1; - } - else if (stabilizationDesired.Throttle < 0) { - throttleIntegral -= stabilizationDesired.Throttle; - stabilizationDesired.Throttle = 0; - } + AltHoldSmoothedData altHold; + AltHoldSmoothedGet(&altHold); + altHold.Altitude = z[0]; + altHold.Velocity = z[1]; + altHold.Accel = z[2]; + AltHoldSmoothedSet(&altHold); - stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - stabilizationDesired.Roll = altitudeHoldDesired.Roll; - stabilizationDesired.Pitch = altitudeHoldDesired.Pitch; - stabilizationDesired.Yaw = altitudeHoldDesired.Yaw; - StabilizationDesiredSet(&stabilizationDesired); + AltHoldSmoothedGet(&altHold); - } else if (ev.obj == AltitudeHoldDesiredHandle()) { - AltitudeHoldDesiredGet(&altitudeHoldDesired); - } + // Verify that we are in altitude hold mode + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || + flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) { + running = false; + } - } + if (!running) { + continue; + } + + // Compute the altitude error + error = (starting_altitude + altitudeHoldDesired.Altitude) - altHold.Altitude; + + // Compute integral off altitude error + throttleIntegral += error * altitudeHoldSettings.Ki * dT; + + // Only update stabilizationDesired less frequently + if ((thisTime - lastUpdateTime) < 20) { + continue; + } + + lastUpdateTime = thisTime; + + // Instead of explicit limit on integral you output limit feedback + StabilizationDesiredGet(&stabilizationDesired); + stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral - + altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; + if (stabilizationDesired.Throttle > 1) { + throttleIntegral -= (stabilizationDesired.Throttle - 1); + stabilizationDesired.Throttle = 1; + } else if (stabilizationDesired.Throttle < 0) { + throttleIntegral -= stabilizationDesired.Throttle; + stabilizationDesired.Throttle = 0; + } + + stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabilizationDesired.Roll = altitudeHoldDesired.Roll; + stabilizationDesired.Pitch = altitudeHoldDesired.Pitch; + stabilizationDesired.Yaw = altitudeHoldDesired.Yaw; + StabilizationDesiredSet(&stabilizationDesired); + } else if (ev.obj == AltitudeHoldDesiredHandle()) { + AltitudeHoldDesiredGet(&altitudeHoldDesired); + } + } } static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - AltitudeHoldSettingsGet(&altitudeHoldSettings); + AltitudeHoldSettingsGet(&altitudeHoldSettings); } diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index 540cb154f..688d97dbc 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -66,11 +66,11 @@ // Private constants #define STACK_SIZE_BYTES 540 -#define TASK_PRIORITY (tskIDLE_PRIORITY+3) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) -#define SENSOR_PERIOD 4 -#define UPDATE_RATE 25.0f -#define GYRO_NEUTRAL 1665 +#define SENSOR_PERIOD 4 +#define UPDATE_RATE 25.0f +#define GYRO_NEUTRAL 1665 // Private types @@ -80,38 +80,38 @@ static xTaskHandle taskHandle; // Private functions static void AttitudeTask(void *parameters); -static float gyro_correct_int[3] = {0,0,0}; +static float gyro_correct_int[3] = { 0, 0, 0 }; static xQueueHandle gyro_queue; static int32_t updateSensors(AccelsData *, GyrosData *); -static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData); +static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData); static void updateAttitude(AccelsData *, GyrosData *); -static void settingsUpdatedCb(UAVObjEvent * objEv); +static void settingsUpdatedCb(UAVObjEvent *objEv); -static float accelKi = 0; -static float accelKp = 0; +static float accelKi = 0; +static float accelKp = 0; static float accel_alpha = 0; static bool accel_filter_enabled = false; static float accels_filtered[3]; static float grot_filtered[3]; static float yawBiasRate = 0; static float rollPitchBiasRate = 0.0f; -static float gyroGain = 0.42f; +static float gyroGain = 0.42f; static int16_t accelbias[3]; -static float q[4] = {1,0,0,0}; +static float q[4] = { 1, 0, 0, 0 }; static float R[3][3]; static int8_t rotate = 0; static bool zero_during_arming = false; -static bool bias_correct_gyro = true; +static bool bias_correct_gyro = true; // For running trim flights -static volatile bool trim_requested = false; +static volatile bool trim_requested = false; static volatile int32_t trim_accels[3]; static volatile int32_t trim_samples; int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535; -#define GRAV 9.81f -#define ACCEL_SCALE (GRAV * 0.004f) +#define GRAV 9.81f +#define ACCEL_SCALE (GRAV * 0.004f) /* 0.004f is gravity / LSB */ /** @@ -120,13 +120,12 @@ int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535; */ int32_t AttitudeStart(void) { - - // Start main task - xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, taskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); - - return 0; + // Start main task + xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, taskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); + + return 0; } /** @@ -135,38 +134,40 @@ int32_t AttitudeStart(void) */ int32_t AttitudeInitialize(void) { - AttitudeActualInitialize(); - AttitudeSettingsInitialize(); - AccelsInitialize(); - GyrosInitialize(); - - // Initialize quaternion - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - attitude.q1 = 1; - attitude.q2 = 0; - attitude.q3 = 0; - attitude.q4 = 0; - AttitudeActualSet(&attitude); - - // Cannot trust the values to init right above if BL runs - gyro_correct_int[0] = 0; - gyro_correct_int[1] = 0; - gyro_correct_int[2] = 0; - - q[0] = 1; - q[1] = 0; - q[2] = 0; - q[3] = 0; - for(uint8_t i = 0; i < 3; i++) - for(uint8_t j = 0; j < 3; j++) - R[i][j] = 0; - - trim_requested = false; - - AttitudeSettingsConnectCallback(&settingsUpdatedCb); - - return 0; + AttitudeActualInitialize(); + AttitudeSettingsInitialize(); + AccelsInitialize(); + GyrosInitialize(); + + // Initialize quaternion + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + attitude.q1 = 1; + attitude.q2 = 0; + attitude.q3 = 0; + attitude.q4 = 0; + AttitudeActualSet(&attitude); + + // Cannot trust the values to init right above if BL runs + gyro_correct_int[0] = 0; + gyro_correct_int[1] = 0; + gyro_correct_int[2] = 0; + + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; + for (uint8_t i = 0; i < 3; i++) { + for (uint8_t j = 0; j < 3; j++) { + R[i][j] = 0; + } + } + + trim_requested = false; + + AttitudeSettingsConnectCallback(&settingsUpdatedCb); + + return 0; } MODULE_INITCALL(AttitudeInitialize, AttitudeStart) @@ -174,100 +175,101 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart) /** * Module thread, should not return. */ - + int32_t accel_test; int32_t gyro_test; static void AttitudeTask(__attribute__((unused)) void *parameters) { - uint8_t init = 0; - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - - // Set critical error and wait until the accel is producing data - while(PIOS_ADXL345_FifoElements() == 0) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); - } - - const struct pios_board_info * bdinfo = &pios_board_info_blob; - - bool cc3d = bdinfo->board_rev == 0x02; + uint8_t init = 0; - if(cc3d) { + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + + // Set critical error and wait until the accel is producing data + while (PIOS_ADXL345_FifoElements() == 0) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); + } + + const struct pios_board_info *bdinfo = &pios_board_info_blob; + + bool cc3d = bdinfo->board_rev == 0x02; + + if (cc3d) { #if defined(PIOS_INCLUDE_MPU6000) - gyro_test = PIOS_MPU6000_Test(); + gyro_test = PIOS_MPU6000_Test(); #endif - } else { + } else { #if defined(PIOS_INCLUDE_ADXL345) - accel_test = PIOS_ADXL345_Test(); + accel_test = PIOS_ADXL345_Test(); #endif #if defined(PIOS_INCLUDE_ADC) - // Create queue for passing gyro data, allow 2 back samples in case - gyro_queue = xQueueCreate(1, sizeof(float) * 4); - PIOS_Assert(gyro_queue != NULL); - PIOS_ADC_SetQueue(gyro_queue); - PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE); + // Create queue for passing gyro data, allow 2 back samples in case + gyro_queue = xQueueCreate(1, sizeof(float) * 4); + PIOS_Assert(gyro_queue != NULL); + PIOS_ADC_SetQueue(gyro_queue); + PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE); #endif + } + // Force settings update to make sure rotation loaded + settingsUpdatedCb(AttitudeSettingsHandle()); - } - // Force settings update to make sure rotation loaded - settingsUpdatedCb(AttitudeSettingsHandle()); - - // Main task loop - while (1) { - - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - - if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { - // Use accels to initialise attitude and calculate gyro bias - accelKp = 1.0f; - accelKi = 0.0f; - yawBiasRate = 0.01f; - rollPitchBiasRate = 0.01f; - accel_filter_enabled = false; - init = 0; - } - else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { - accelKp = 1.0f; - accelKi = 0.0f; - yawBiasRate = 0.01f; - rollPitchBiasRate = 0.01f; - accel_filter_enabled = false; - init = 0; - } else if (init == 0) { - // Reload settings (all the rates) - AttitudeSettingsAccelKiGet(&accelKi); - AttitudeSettingsAccelKpGet(&accelKp); - AttitudeSettingsYawBiasRateGet(&yawBiasRate); - rollPitchBiasRate = 0.0f; - if (accel_alpha > 0.0f) - accel_filter_enabled = true; - init = 1; - } - - PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); + // Main task loop + while (1) { + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); - AccelsData accels; - GyrosData gyros; - int32_t retval = 0; + if ((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { + // Use accels to initialise attitude and calculate gyro bias + accelKp = 1.0f; + accelKi = 0.0f; + yawBiasRate = 0.01f; + rollPitchBiasRate = 0.01f; + accel_filter_enabled = false; + init = 0; + } else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { + accelKp = 1.0f; + accelKi = 0.0f; + yawBiasRate = 0.01f; + rollPitchBiasRate = 0.01f; + accel_filter_enabled = false; + init = 0; + } else if (init == 0) { + // Reload settings (all the rates) + AttitudeSettingsAccelKiGet(&accelKi); + AttitudeSettingsAccelKpGet(&accelKp); + AttitudeSettingsYawBiasRateGet(&yawBiasRate); + rollPitchBiasRate = 0.0f; + if (accel_alpha > 0.0f) { + accel_filter_enabled = true; + } + init = 1; + } - if (cc3d) - retval = updateSensorsCC3D(&accels, &gyros); - else - retval = updateSensors(&accels, &gyros); + PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); - // Only update attitude when sensor data is good - if (retval != 0) - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - else { - // Do not update attitude data in simulation mode - if (!AttitudeActualReadOnly()) - updateAttitude(&accels, &gyros); + AccelsData accels; + GyrosData gyros; + int32_t retval = 0; - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - } - } + if (cc3d) { + retval = updateSensorsCC3D(&accels, &gyros); + } else { + retval = updateSensors(&accels, &gyros); + } + + // Only update attitude when sensor data is good + if (retval != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + } else { + // Do not update attitude data in simulation mode + if (!AttitudeActualReadOnly()) { + updateAttitude(&accels, &gyros); + } + + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + } + } } float gyros_passed[3]; @@ -277,105 +279,107 @@ float gyros_passed[3]; * @param[in] attitudeRaw Populate the UAVO instead of saving right here * @return 0 if successfull, -1 if not */ -static int32_t updateSensors(AccelsData * accels, GyrosData * gyros) +static int32_t updateSensors(AccelsData *accels, GyrosData *gyros) { - struct pios_adxl345_data accel_data; - float gyro[4]; - - // Only wait the time for two nominal updates before setting an alarm - if(xQueueReceive(gyro_queue, (void * const) gyro, UPDATE_RATE * 2) == errQUEUE_EMPTY) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - return -1; - } + struct pios_adxl345_data accel_data; + float gyro[4]; - // Do not read raw sensor data in simulation mode - if (GyrosReadOnly() || AccelsReadOnly()) - return 0; + // Only wait the time for two nominal updates before setting an alarm + if (xQueueReceive(gyro_queue, (void *const)gyro, UPDATE_RATE * 2) == errQUEUE_EMPTY) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + return -1; + } - // No accel data available - if(PIOS_ADXL345_FifoElements() == 0) - return -1; - - // First sample is temperature - gyros->x = -(gyro[1] - GYRO_NEUTRAL) * gyroGain; - gyros->y = (gyro[2] - GYRO_NEUTRAL) * gyroGain; - gyros->z = -(gyro[3] - GYRO_NEUTRAL) * gyroGain; - - int32_t x = 0; - int32_t y = 0; - int32_t z = 0; - uint8_t i = 0; - uint8_t samples_remaining; - do { - i++; - samples_remaining = PIOS_ADXL345_Read(&accel_data); - x += accel_data.x; - y += -accel_data.y; - z += -accel_data.z; - } while ( (i < 32) && (samples_remaining > 0) ); - gyros->temperature = samples_remaining; + // Do not read raw sensor data in simulation mode + if (GyrosReadOnly() || AccelsReadOnly()) { + return 0; + } - float accel[3] = {(float) x / i, (float) y / i, (float) z / i}; - - if(rotate) { - // TODO: rotate sensors too so stabilization is well behaved - float vec_out[3]; - rot_mult(R, accel, vec_out); - accels->x = vec_out[0]; - accels->y = vec_out[1]; - accels->z = vec_out[2]; - rot_mult(R, &gyros->x, vec_out); - gyros->x = vec_out[0]; - gyros->y = vec_out[1]; - gyros->z = vec_out[2]; - } else { - accels->x = accel[0]; - accels->y = accel[1]; - accels->z = accel[2]; - } - - if (trim_requested) { - if (trim_samples >= MAX_TRIM_FLIGHT_SAMPLES) { - trim_requested = false; - } else { - uint8_t armed; - float throttle; - FlightStatusArmedGet(&armed); - ManualControlCommandThrottleGet(&throttle); // Until flight status indicates airborne - if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0.0f)) { - trim_samples++; - // Store the digitally scaled version since that is what we use for bias - trim_accels[0] += accels->x; - trim_accels[1] += accels->y; - trim_accels[2] += accels->z; - } - } - } - - // Scale accels and correct bias - accels->x = (accels->x - accelbias[0]) * ACCEL_SCALE; - accels->y = (accels->y - accelbias[1]) * ACCEL_SCALE; - accels->z = (accels->z - accelbias[2]) * ACCEL_SCALE; - - if(bias_correct_gyro) { - // Applying integral component here so it can be seen on the gyros and correct bias - gyros->x += gyro_correct_int[0]; - gyros->y += gyro_correct_int[1]; - gyros->z += gyro_correct_int[2]; - } + // No accel data available + if (PIOS_ADXL345_FifoElements() == 0) { + return -1; + } - // Force the roll & pitch gyro rates to average to zero during initialisation - gyro_correct_int[0] += - gyros->x * rollPitchBiasRate; - gyro_correct_int[1] += - gyros->y * rollPitchBiasRate; - - // Because most crafts wont get enough information from gravity to zero yaw gyro, we try - // and make it average zero (weakly) - gyro_correct_int[2] += - gyros->z * yawBiasRate; + // First sample is temperature + gyros->x = -(gyro[1] - GYRO_NEUTRAL) * gyroGain; + gyros->y = (gyro[2] - GYRO_NEUTRAL) * gyroGain; + gyros->z = -(gyro[3] - GYRO_NEUTRAL) * gyroGain; - GyrosSet(gyros); - AccelsSet(accels); + int32_t x = 0; + int32_t y = 0; + int32_t z = 0; + uint8_t i = 0; + uint8_t samples_remaining; + do { + i++; + samples_remaining = PIOS_ADXL345_Read(&accel_data); + x += accel_data.x; + y += -accel_data.y; + z += -accel_data.z; + } while ((i < 32) && (samples_remaining > 0)); + gyros->temperature = samples_remaining; - return 0; + float accel[3] = { (float)x / i, (float)y / i, (float)z / i }; + + if (rotate) { + // TODO: rotate sensors too so stabilization is well behaved + float vec_out[3]; + rot_mult(R, accel, vec_out); + accels->x = vec_out[0]; + accels->y = vec_out[1]; + accels->z = vec_out[2]; + rot_mult(R, &gyros->x, vec_out); + gyros->x = vec_out[0]; + gyros->y = vec_out[1]; + gyros->z = vec_out[2]; + } else { + accels->x = accel[0]; + accels->y = accel[1]; + accels->z = accel[2]; + } + + if (trim_requested) { + if (trim_samples >= MAX_TRIM_FLIGHT_SAMPLES) { + trim_requested = false; + } else { + uint8_t armed; + float throttle; + FlightStatusArmedGet(&armed); + ManualControlCommandThrottleGet(&throttle); // Until flight status indicates airborne + if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0.0f)) { + trim_samples++; + // Store the digitally scaled version since that is what we use for bias + trim_accels[0] += accels->x; + trim_accels[1] += accels->y; + trim_accels[2] += accels->z; + } + } + } + + // Scale accels and correct bias + accels->x = (accels->x - accelbias[0]) * ACCEL_SCALE; + accels->y = (accels->y - accelbias[1]) * ACCEL_SCALE; + accels->z = (accels->z - accelbias[2]) * ACCEL_SCALE; + + if (bias_correct_gyro) { + // Applying integral component here so it can be seen on the gyros and correct bias + gyros->x += gyro_correct_int[0]; + gyros->y += gyro_correct_int[1]; + gyros->z += gyro_correct_int[2]; + } + + // Force the roll & pitch gyro rates to average to zero during initialisation + gyro_correct_int[0] += -gyros->x * rollPitchBiasRate; + gyro_correct_int[1] += -gyros->y * rollPitchBiasRate; + + // Because most crafts wont get enough information from gravity to zero yaw gyro, we try + // and make it average zero (weakly) + gyro_correct_int[2] += -gyros->z * yawBiasRate; + + GyrosSet(gyros); + AccelsSet(accels); + + return 0; } /** @@ -384,263 +388,270 @@ static int32_t updateSensors(AccelsData * accels, GyrosData * gyros) * @return 0 if successfull, -1 if not */ struct pios_mpu6000_data mpu6000_data; -static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData) +static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData) { - float accels[3], gyros[3]; - + float accels[3], gyros[3]; + #if defined(PIOS_INCLUDE_MPU6000) - - xQueueHandle queue = PIOS_MPU6000_GetQueue(); - - if(xQueueReceive(queue, (void *) &mpu6000_data, SENSOR_PERIOD) == errQUEUE_EMPTY) - return -1; // Error, no data - // Do not read raw sensor data in simulation mode - if (GyrosReadOnly() || AccelsReadOnly()) - return 0; + xQueueHandle queue = PIOS_MPU6000_GetQueue(); - gyros[0] = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale(); - gyros[1] = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale(); - gyros[2] = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale(); - - accels[0] = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale(); - accels[1] = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale(); - accels[2] = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale(); + if (xQueueReceive(queue, (void *)&mpu6000_data, SENSOR_PERIOD) == errQUEUE_EMPTY) { + return -1; // Error, no data + } + // Do not read raw sensor data in simulation mode + if (GyrosReadOnly() || AccelsReadOnly()) { + return 0; + } - gyrosData->temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; - accelsData->temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; -#endif + gyros[0] = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale(); + gyros[1] = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale(); + gyros[2] = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale(); - if(rotate) { - // TODO: rotate sensors too so stabilization is well behaved - float vec_out[3]; - rot_mult(R, accels, vec_out); - accels[0] = vec_out[0]; - accels[1] = vec_out[1]; - accels[2] = vec_out[2]; - rot_mult(R, gyros, vec_out); - gyros[0] = vec_out[0]; - gyros[1] = vec_out[1]; - gyros[2] = vec_out[2]; - } + accels[0] = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale(); + accels[1] = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale(); + accels[2] = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale(); - accelsData->x = accels[0] - accelbias[0] * ACCEL_SCALE; // Applying arbitrary scale here to match CC v1 - accelsData->y = accels[1] - accelbias[1] * ACCEL_SCALE; - accelsData->z = accels[2] - accelbias[2] * ACCEL_SCALE; + gyrosData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; + accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; +#endif /* if defined(PIOS_INCLUDE_MPU6000) */ - gyrosData->x = gyros[0]; - gyrosData->y = gyros[1]; - gyrosData->z = gyros[2]; + if (rotate) { + // TODO: rotate sensors too so stabilization is well behaved + float vec_out[3]; + rot_mult(R, accels, vec_out); + accels[0] = vec_out[0]; + accels[1] = vec_out[1]; + accels[2] = vec_out[2]; + rot_mult(R, gyros, vec_out); + gyros[0] = vec_out[0]; + gyros[1] = vec_out[1]; + gyros[2] = vec_out[2]; + } - if(bias_correct_gyro) { - // Applying integral component here so it can be seen on the gyros and correct bias - gyrosData->x += gyro_correct_int[0]; - gyrosData->y += gyro_correct_int[1]; - gyrosData->z += gyro_correct_int[2]; - } + accelsData->x = accels[0] - accelbias[0] * ACCEL_SCALE; // Applying arbitrary scale here to match CC v1 + accelsData->y = accels[1] - accelbias[1] * ACCEL_SCALE; + accelsData->z = accels[2] - accelbias[2] * ACCEL_SCALE; - // Force the roll & pitch gyro rates to average to zero during initialisation - gyro_correct_int[0] += - gyrosData->x * rollPitchBiasRate; - gyro_correct_int[1] += - gyrosData->y * rollPitchBiasRate; + gyrosData->x = gyros[0]; + gyrosData->y = gyros[1]; + gyrosData->z = gyros[2]; - // Because most crafts wont get enough information from gravity to zero yaw gyro, we try - // and make it average zero (weakly) - gyro_correct_int[2] += - gyrosData->z * yawBiasRate; + if (bias_correct_gyro) { + // Applying integral component here so it can be seen on the gyros and correct bias + gyrosData->x += gyro_correct_int[0]; + gyrosData->y += gyro_correct_int[1]; + gyrosData->z += gyro_correct_int[2]; + } - GyrosSet(gyrosData); - AccelsSet(accelsData); + // Force the roll & pitch gyro rates to average to zero during initialisation + gyro_correct_int[0] += -gyrosData->x * rollPitchBiasRate; + gyro_correct_int[1] += -gyrosData->y * rollPitchBiasRate; - return 0; + // Because most crafts wont get enough information from gravity to zero yaw gyro, we try + // and make it average zero (weakly) + gyro_correct_int[2] += -gyrosData->z * yawBiasRate; + + GyrosSet(gyrosData); + AccelsSet(accelsData); + + return 0; } static inline void apply_accel_filter(const float *raw, float *filtered) { - if (accel_filter_enabled) { - filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha); - filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha); - filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha); - } else { - filtered[0] = raw[0]; - filtered[1] = raw[1]; - filtered[2] = raw[2]; - } + if (accel_filter_enabled) { + filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha); + filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha); + filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha); + } else { + filtered[0] = raw[0]; + filtered[1] = raw[1]; + filtered[2] = raw[2]; + } } -static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) +static void updateAttitude(AccelsData *accelsData, GyrosData *gyrosData) { - float dT; - portTickType thisSysTime = xTaskGetTickCount(); - static portTickType lastSysTime = 0; - - dT = (thisSysTime == lastSysTime) ? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f; - lastSysTime = thisSysTime; - - // Bad practice to assume structure order, but saves memory - float * gyros = &gyrosData->x; - float * accels = &accelsData->x; - - float grot[3]; - float accel_err[3]; + float dT; + portTickType thisSysTime = xTaskGetTickCount(); + static portTickType lastSysTime = 0; - // Apply smoothing to accel values, to reduce vibration noise before main calculations. - apply_accel_filter(accels, accels_filtered); - - // Rotate gravity unit vector to body frame, filter and cross with accels - grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); - grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); - grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); + dT = (thisSysTime == lastSysTime) ? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f; + lastSysTime = thisSysTime; - apply_accel_filter(grot, grot_filtered); - - CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); - - // Account for accel magnitude - float accel_mag = sqrtf(accels_filtered[0]*accels_filtered[0] + accels_filtered[1]*accels_filtered[1] + accels_filtered[2]*accels_filtered[2]); - if (accel_mag < 1.0e-3f) - return; + // Bad practice to assume structure order, but saves memory + float *gyros = &gyrosData->x; + float *accels = &accelsData->x; - // Account for filtered gravity vector magnitude - float grot_mag; + float grot[3]; + float accel_err[3]; - if (accel_filter_enabled) - grot_mag = sqrtf(grot_filtered[0]*grot_filtered[0] + grot_filtered[1]*grot_filtered[1] + grot_filtered[2]*grot_filtered[2]); - else - grot_mag = 1.0f; + // Apply smoothing to accel values, to reduce vibration noise before main calculations. + apply_accel_filter(accels, accels_filtered); - if (grot_mag < 1.0e-3f) - return; + // Rotate gravity unit vector to body frame, filter and cross with accels + grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); + grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); + grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); - accel_err[0] /= (accel_mag*grot_mag); - accel_err[1] /= (accel_mag*grot_mag); - accel_err[2] /= (accel_mag*grot_mag); - - // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s - gyro_correct_int[0] += accel_err[0] * accelKi; - gyro_correct_int[1] += accel_err[1] * accelKi; - - //gyro_correct_int[2] += accel_err[2] * accelKi; - - // Correct rates based on error, integral component dealt with in updateSensors - gyros[0] += accel_err[0] * accelKp / dT; - gyros[1] += accel_err[1] * accelKp / dT; - gyros[2] += accel_err[2] * accelKp / dT; - - { // scoping variables to save memory - // Work out time derivative from INSAlgo writeup - // Also accounts for the fact that gyros are in deg/s - float qdot[4]; - qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); - qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); - qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); - qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); - - // Take a time step - q[0] = q[0] + qdot[0]; - q[1] = q[1] + qdot[1]; - q[2] = q[2] + qdot[2]; - q[3] = q[3] + qdot[3]; - - if(q[0] < 0) { - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; - } - } - - // Renomalize - float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); - - // If quaternion has become inappropriately short or is nan reinit. - // THIS SHOULD NEVER ACTUALLY HAPPEN - if((fabsf(qmag) < 1e-3f) || isnan(qmag)) { - q[0] = 1; - q[1] = 0; - q[2] = 0; - q[3] = 0; - } else { - q[0] = q[0] / qmag; - q[1] = q[1] / qmag; - q[2] = q[2] / qmag; - q[3] = q[3] / qmag; - } - - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - - quat_copy(q, &attitudeActual.q1); - - // Convert into eueler degrees (makes assumptions about RPY order) - Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll); - - AttitudeActualSet(&attitudeActual); + apply_accel_filter(grot, grot_filtered); + + CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); + + // Account for accel magnitude + float accel_mag = sqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]); + if (accel_mag < 1.0e-3f) { + return; + } + + // Account for filtered gravity vector magnitude + float grot_mag; + + if (accel_filter_enabled) { + grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); + } else { + grot_mag = 1.0f; + } + + if (grot_mag < 1.0e-3f) { + return; + } + + accel_err[0] /= (accel_mag * grot_mag); + accel_err[1] /= (accel_mag * grot_mag); + accel_err[2] /= (accel_mag * grot_mag); + + // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s + gyro_correct_int[0] += accel_err[0] * accelKi; + gyro_correct_int[1] += accel_err[1] * accelKi; + + // gyro_correct_int[2] += accel_err[2] * accelKi; + + // Correct rates based on error, integral component dealt with in updateSensors + gyros[0] += accel_err[0] * accelKp / dT; + gyros[1] += accel_err[1] * accelKp / dT; + gyros[2] += accel_err[2] * accelKp / dT; + + { // scoping variables to save memory + // Work out time derivative from INSAlgo writeup + // Also accounts for the fact that gyros are in deg/s + float qdot[4]; + qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); + qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); + qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); + qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); + + // Take a time step + q[0] = q[0] + qdot[0]; + q[1] = q[1] + qdot[1]; + q[2] = q[2] + qdot[2]; + q[3] = q[3] + qdot[3]; + + if (q[0] < 0) { + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } + } + + // Renomalize + float qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + + // If quaternion has become inappropriately short or is nan reinit. + // THIS SHOULD NEVER ACTUALLY HAPPEN + if ((fabsf(qmag) < 1e-3f) || isnan(qmag)) { + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; + } else { + q[0] = q[0] / qmag; + q[1] = q[1] / qmag; + q[2] = q[2] / qmag; + q[3] = q[3] / qmag; + } + + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); + + quat_copy(q, &attitudeActual.q1); + + // Convert into eueler degrees (makes assumptions about RPY order) + Quaternion2RPY(&attitudeActual.q1, &attitudeActual.Roll); + + AttitudeActualSet(&attitudeActual); } -static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent * objEv) { - AttitudeSettingsData attitudeSettings; - AttitudeSettingsGet(&attitudeSettings); - - - accelKp = attitudeSettings.AccelKp; - accelKi = attitudeSettings.AccelKi; - yawBiasRate = attitudeSettings.YawBiasRate; - gyroGain = attitudeSettings.GyroGain; +static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) +{ + AttitudeSettingsData attitudeSettings; - // Calculate accel filter alpha, in the same way as for gyro data in stabilization module. - const float fakeDt = 0.0025f; - if (attitudeSettings.AccelTau < 0.0001f) { - accel_alpha = 0; // not trusting this to resolve to 0 - accel_filter_enabled = false; - } else { - accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau); - accel_filter_enabled = true; - } - - zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; - bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE; - - accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X]; - accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y]; - accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z]; - - gyro_correct_int[0] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f; - gyro_correct_int[1] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f; - gyro_correct_int[2] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f; - - // Indicates not to expend cycles on rotation - if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && - attitudeSettings.BoardRotation[2] == 0) { - rotate = 0; - - // Shouldn't be used but to be safe - float rotationQuat[4] = {1,0,0,0}; - Quaternion2R(rotationQuat, R); - } else { - float rotationQuat[4]; - const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL], - attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH], - attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]}; - RPY2Quaternion(rpy, rotationQuat); - Quaternion2R(rotationQuat, R); - rotate = 1; - } - - if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START) { - trim_accels[0] = 0; - trim_accels[1] = 0; - trim_accels[2] = 0; - trim_samples = 0; - trim_requested = true; - } else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) { - trim_requested = false; - attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X] = trim_accels[0] / trim_samples; - attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y] = trim_accels[1] / trim_samples; - // Z should average -grav - attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z] = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE; - attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL; - AttitudeSettingsSet(&attitudeSettings); - } else - trim_requested = false; + AttitudeSettingsGet(&attitudeSettings); + + + accelKp = attitudeSettings.AccelKp; + accelKi = attitudeSettings.AccelKi; + yawBiasRate = attitudeSettings.YawBiasRate; + gyroGain = attitudeSettings.GyroGain; + + // Calculate accel filter alpha, in the same way as for gyro data in stabilization module. + const float fakeDt = 0.0025f; + if (attitudeSettings.AccelTau < 0.0001f) { + accel_alpha = 0; // not trusting this to resolve to 0 + accel_filter_enabled = false; + } else { + accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau); + accel_filter_enabled = true; + } + + zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; + bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE; + + accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X]; + accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y]; + accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z]; + + gyro_correct_int[0] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f; + gyro_correct_int[1] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f; + gyro_correct_int[2] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f; + + // Indicates not to expend cycles on rotation + if (attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && + attitudeSettings.BoardRotation[2] == 0) { + rotate = 0; + + // Shouldn't be used but to be safe + float rotationQuat[4] = { 1, 0, 0, 0 }; + Quaternion2R(rotationQuat, R); + } else { + float rotationQuat[4]; + const float rpy[3] = { attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL], + attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH], + attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] }; + RPY2Quaternion(rpy, rotationQuat); + Quaternion2R(rotationQuat, R); + rotate = 1; + } + + if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START) { + trim_accels[0] = 0; + trim_accels[1] = 0; + trim_accels[2] = 0; + trim_samples = 0; + trim_requested = true; + } else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) { + trim_requested = false; + attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X] = trim_accels[0] / trim_samples; + attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y] = trim_accels[1] / trim_samples; + // Z should average -grav + attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z] = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE; + attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL; + AttitudeSettingsSet(&attitudeSettings); + } else { + trim_requested = false; + } } /** * @} diff --git a/flight/modules/Attitude/inc/attitude.h b/flight/modules/Attitude/inc/attitude.h index 3f90e56a6..64c556a8e 100644 --- a/flight/modules/Attitude/inc/attitude.h +++ b/flight/modules/Attitude/inc/attitude.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup Attitude Attitude Module - * @{ + * @{ * * @file attitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index 4321041f7..43f060c92 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -75,19 +75,19 @@ #include "CoordinateConversions.h" // Private constants -#define STACK_SIZE_BYTES 2048 -#define TASK_PRIORITY (tskIDLE_PRIORITY+3) +#define STACK_SIZE_BYTES 2048 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) #define FAILSAFE_TIMEOUT_MS 10 // low pass filter configuration to calculate offset // of barometric altitude sensor // reasoning: updates at: 10 Hz, tau= 300 s settle time // exp(-(1/f) / tau ) ~=~ 0.9997 -#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f +#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f // simple IAS to TAS aproximation - 2% increase per 1000ft // since we do not have flowing air temperature information -#define IAS2TAS(alt) (1.0f + (0.02f*(alt)/304.8f)) +#define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f)) // Private types @@ -108,38 +108,40 @@ static RevoCalibrationData revoCalibration; static EKFConfigurationData ekfConfiguration; static RevoSettingsData revoSettings; static FlightStatusData flightStatus; -const uint32_t SENSOR_QUEUE_SIZE = 10; +const uint32_t SENSOR_QUEUE_SIZE = 10; static bool volatile variance_error = true; static bool volatile initialization_required = true; -static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running +static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running // Private functions static void AttitudeTask(void *parameters); static int32_t updateAttitudeComplementary(bool first_run); static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode); -static void settingsUpdatedCb(UAVObjEvent * objEv); +static void settingsUpdatedCb(UAVObjEvent *objEv); -static int32_t getNED(GPSPositionData * gpsPosition, float * NED); +static int32_t getNED(GPSPositionData *gpsPosition, float *NED); // check for invalid values -static inline bool invalid(float data) { - if ( isnan(data) || isinf(data) ){ - return true; - } - return false; +static inline bool invalid(float data) +{ + if (isnan(data) || isinf(data)) { + return true; + } + return false; } // check for invalid variance values -static inline bool invalid_var(float data) { - if ( invalid(data) ) { - return true; - } - if ( data < 1e-15f ) { // var should not be close to zero. And not negative either. - return true; - } - return false; +static inline bool invalid_var(float data) +{ + if (invalid(data)) { + return true; + } + if (data < 1e-15f) { // var should not be close to zero. And not negative either. + return true; + } + return false; } /** @@ -157,46 +159,46 @@ static inline bool invalid_var(float data) { */ int32_t AttitudeInitialize(void) { - AttitudeActualInitialize(); - AirspeedActualInitialize(); - AirspeedSensorInitialize(); - AttitudeSettingsInitialize(); - PositionActualInitialize(); - VelocityActualInitialize(); - RevoSettingsInitialize(); - RevoCalibrationInitialize(); - EKFConfigurationInitialize(); - EKFStateVarianceInitialize(); - FlightStatusInitialize(); - - // Initialize this here while we aren't setting the homelocation in GPS - HomeLocationInitialize(); + AttitudeActualInitialize(); + AirspeedActualInitialize(); + AirspeedSensorInitialize(); + AttitudeSettingsInitialize(); + PositionActualInitialize(); + VelocityActualInitialize(); + RevoSettingsInitialize(); + RevoCalibrationInitialize(); + EKFConfigurationInitialize(); + EKFStateVarianceInitialize(); + FlightStatusInitialize(); - // Initialize quaternion - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - attitude.q1 = 1.0f; - attitude.q2 = 0.0f; - attitude.q3 = 0.0f; - attitude.q4 = 0.0f; - AttitudeActualSet(&attitude); - - // Cannot trust the values to init right above if BL runs - GyrosBiasData gyrosBias; - GyrosBiasGet(&gyrosBias); - gyrosBias.x = 0.0f; - gyrosBias.y = 0.0f; - gyrosBias.z = 0.0f; - GyrosBiasSet(&gyrosBias); - - AttitudeSettingsConnectCallback(&settingsUpdatedCb); - RevoSettingsConnectCallback(&settingsUpdatedCb); - RevoCalibrationConnectCallback(&settingsUpdatedCb); - HomeLocationConnectCallback(&settingsUpdatedCb); - EKFConfigurationConnectCallback(&settingsUpdatedCb); - FlightStatusConnectCallback(&settingsUpdatedCb); + // Initialize this here while we aren't setting the homelocation in GPS + HomeLocationInitialize(); - return 0; + // Initialize quaternion + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + attitude.q1 = 1.0f; + attitude.q2 = 0.0f; + attitude.q3 = 0.0f; + attitude.q4 = 0.0f; + AttitudeActualSet(&attitude); + + // Cannot trust the values to init right above if BL runs + GyrosBiasData gyrosBias; + GyrosBiasGet(&gyrosBias); + gyrosBias.x = 0.0f; + gyrosBias.y = 0.0f; + gyrosBias.z = 0.0f; + GyrosBiasSet(&gyrosBias); + + AttitudeSettingsConnectCallback(&settingsUpdatedCb); + RevoSettingsConnectCallback(&settingsUpdatedCb); + RevoCalibrationConnectCallback(&settingsUpdatedCb); + HomeLocationConnectCallback(&settingsUpdatedCb); + EKFConfigurationConnectCallback(&settingsUpdatedCb); + FlightStatusConnectCallback(&settingsUpdatedCb); + + return 0; } /** @@ -205,29 +207,29 @@ int32_t AttitudeInitialize(void) */ int32_t AttitudeStart(void) { - // Create the queues for the sensors - gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - accelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - magQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - baroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + // Create the queues for the sensors + gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + accelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + magQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + baroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - // Start main task - xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &attitudeTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); - - GyrosConnectQueue(gyroQueue); - AccelsConnectQueue(accelQueue); - MagnetometerConnectQueue(magQueue); - AirspeedSensorConnectQueue(airspeedQueue); - BaroAltitudeConnectQueue(baroQueue); - GPSPositionConnectQueue(gpsQueue); - GPSVelocityConnectQueue(gpsVelQueue); + // Start main task + xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &attitudeTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); - return 0; + GyrosConnectQueue(gyroQueue); + AccelsConnectQueue(accelQueue); + MagnetometerConnectQueue(magQueue); + AirspeedSensorConnectQueue(airspeedQueue); + BaroAltitudeConnectQueue(baroQueue); + GPSPositionConnectQueue(gpsQueue); + GPSVelocityConnectQueue(gpsVelQueue); + + return 0; } MODULE_INITCALL(AttitudeInitialize, AttitudeStart) @@ -237,48 +239,46 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart) */ static void AttitudeTask(__attribute__((unused)) void *parameters) { + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + // Force settings update to make sure rotation loaded + settingsUpdatedCb(NULL); - // Force settings update to make sure rotation loaded - settingsUpdatedCb(NULL); + // Wait for all the sensors be to read + vTaskDelay(100); - // Wait for all the sensors be to read - vTaskDelay(100); + // Main task loop - TODO: make it run as delayed callback + while (1) { + int32_t ret_val = -1; - // Main task loop - TODO: make it run as delayed callback - while (1) { + bool first_run = false; + if (initialization_required) { + initialization_required = false; + first_run = true; + } - int32_t ret_val = -1; + // This function blocks on data queue + switch (running_algorithm) { + case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: + ret_val = updateAttitudeComplementary(first_run); + break; + case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: + ret_val = updateAttitudeINSGPS(first_run, true); + break; + case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: + ret_val = updateAttitudeINSGPS(first_run, false); + break; + default: + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + break; + } - bool first_run = false; - if (initialization_required) { - initialization_required = false; - first_run = true; - } + if (ret_val != 0) { + initialization_required = true; + } - // This function blocks on data queue - switch (running_algorithm ) { - case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: - ret_val = updateAttitudeComplementary(first_run); - break; - case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: - ret_val = updateAttitudeINSGPS(first_run, true); - break; - case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: - ret_val = updateAttitudeINSGPS(first_run, false); - break; - default: - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL); - break; - } - - if(ret_val != 0) { - initialization_required = true; - } - - PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); - } + PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); + } } float accel_mag; @@ -290,282 +290,281 @@ float magKp = 0.01f; static int32_t updateAttitudeComplementary(bool first_run) { - UAVObjEvent ev; - GyrosData gyrosData; - AccelsData accelsData; - static int32_t timeval; - float dT; - static uint8_t init = 0; + UAVObjEvent ev; + GyrosData gyrosData; + AccelsData accelsData; + static int32_t timeval; + float dT; + static uint8_t init = 0; - // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe - if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE || - xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE ) - { - // When one of these is updated so should the other - // Do not set attitude timeout warnings in simulation mode - if (!AttitudeActualReadOnly()){ - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING); - return -1; - } - } + // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe + if (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE || + xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) { + // When one of these is updated so should the other + // Do not set attitude timeout warnings in simulation mode + if (!AttitudeActualReadOnly()) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); + return -1; + } + } - AccelsGet(&accelsData); + AccelsGet(&accelsData); - // During initialization and - if(first_run) { + // During initialization and + if (first_run) { #if defined(PIOS_INCLUDE_HMC5883) - // To initialize we need a valid mag reading - if ( xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE ) - return -1; - MagnetometerData magData; - MagnetometerGet(&magData); + // To initialize we need a valid mag reading + if (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE) { + return -1; + } + MagnetometerData magData; + MagnetometerGet(&magData); #else - MagnetometerData magData; - magData.x = 100.0f; - magData.y = 0.0f; - magData.z = 0.0f; + MagnetometerData magData; + magData.x = 100.0f; + magData.y = 0.0f; + magData.z = 0.0f; #endif - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - init = 0; + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); + init = 0; - // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, - // so pseudo "north" vector can be estimated even if the board is not level - attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z); - float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y; - float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z; + // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, + // so pseudo "north" vector can be estimated even if the board is not level + attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z); + float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y; + float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z; - // rotate accels z vector according to roll - float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y; - attitudeActual.Pitch = atan2f(accelsData.x, -azn); + // rotate accels z vector according to roll + float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y; + attitudeActual.Pitch = atan2f(accelsData.x, -azn); - float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn; + float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn; - attitudeActual.Yaw = atan2f(-yn, xn); - // TODO: This is still a hack - // Put this in a proper generic function in CoordinateConversion.c - // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) - // should calculate the rotation in 3d space using proper cross product math - // SUBTODO: formulate the math required + attitudeActual.Yaw = atan2f(-yn, xn); + // TODO: This is still a hack + // Put this in a proper generic function in CoordinateConversion.c + // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) + // should calculate the rotation in 3d space using proper cross product math + // SUBTODO: formulate the math required - attitudeActual.Roll = RAD2DEG(attitudeActual.Roll); - attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch); - attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw); + attitudeActual.Roll = RAD2DEG(attitudeActual.Roll); + attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch); + attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw); - RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); - AttitudeActualSet(&attitudeActual); + RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1); + AttitudeActualSet(&attitudeActual); - timeval = PIOS_DELAY_GetRaw(); + timeval = PIOS_DELAY_GetRaw(); - return 0; + return 0; + } - } + if ((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { + // For first 7 seconds use accels to get gyro bias + attitudeSettings.AccelKp = 1.0f; + attitudeSettings.AccelKi = 0.9f; + attitudeSettings.YawBiasRate = 0.23f; + magKp = 1.0f; + } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { + attitudeSettings.AccelKp = 1.0f; + attitudeSettings.AccelKi = 0.9f; + attitudeSettings.YawBiasRate = 0.23f; + magKp = 1.0f; + init = 0; + } else if (init == 0) { + // Reload settings (all the rates) + AttitudeSettingsGet(&attitudeSettings); + magKp = 0.01f; + init = 1; + } - if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { - // For first 7 seconds use accels to get gyro bias - attitudeSettings.AccelKp = 1.0f; - attitudeSettings.AccelKi = 0.9f; - attitudeSettings.YawBiasRate = 0.23f; - magKp = 1.0f; - } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { - attitudeSettings.AccelKp = 1.0f; - attitudeSettings.AccelKi = 0.9f; - attitudeSettings.YawBiasRate = 0.23f; - magKp = 1.0f; - init = 0; - } else if (init == 0) { - // Reload settings (all the rates) - AttitudeSettingsGet(&attitudeSettings); - magKp = 0.01f; - init = 1; - } + GyrosGet(&gyrosData); - GyrosGet(&gyrosData); + // Compute the dT using the cpu clock + dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; + timeval = PIOS_DELAY_GetRaw(); - // Compute the dT using the cpu clock - dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; - timeval = PIOS_DELAY_GetRaw(); + float q[4]; - float q[4]; + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); + float grot[3]; + float accel_err[3]; - float grot[3]; - float accel_err[3]; + // Get the current attitude estimate + quat_copy(&attitudeActual.q1, q); - // Get the current attitude estimate - quat_copy(&attitudeActual.q1, q); + // Rotate gravity to body frame and cross with accels + grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2])); + grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1])); + grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); + CrossProduct((const float *)&accelsData.x, (const float *)grot, accel_err); - // Rotate gravity to body frame and cross with accels - grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2])); - grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1])); - grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); - CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err); + // Account for accel magnitude + accel_mag = accelsData.x * accelsData.x + accelsData.y * accelsData.y + accelsData.z * accelsData.z; + accel_mag = sqrtf(accel_mag); + accel_err[0] /= accel_mag; + accel_err[1] /= accel_mag; + accel_err[2] /= accel_mag; - // Account for accel magnitude - accel_mag = accelsData.x*accelsData.x + accelsData.y*accelsData.y + accelsData.z*accelsData.z; - accel_mag = sqrtf(accel_mag); - accel_err[0] /= accel_mag; - accel_err[1] /= accel_mag; - accel_err[2] /= accel_mag; + if (xQueueReceive(magQueue, &ev, 0) != pdTRUE) { + // Rotate gravity to body frame and cross with accels + float brot[3]; + float Rbe[3][3]; + MagnetometerData mag; - if ( xQueueReceive(magQueue, &ev, 0) != pdTRUE ) - { - // Rotate gravity to body frame and cross with accels - float brot[3]; - float Rbe[3][3]; - MagnetometerData mag; - - Quaternion2R(q, Rbe); - MagnetometerGet(&mag); + Quaternion2R(q, Rbe); + MagnetometerGet(&mag); - // If the mag is producing bad data don't use it (normally bad calibration) - if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) { - rot_mult(Rbe, homeLocation.Be, brot); + // If the mag is producing bad data don't use it (normally bad calibration) + if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) { + rot_mult(Rbe, homeLocation.Be, brot); - float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z); - mag.x /= mag_len; - mag.y /= mag_len; - mag.z /= mag_len; + float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z); + mag.x /= mag_len; + mag.y /= mag_len; + mag.z /= mag_len; - float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]); - brot[0] /= bmag; - brot[1] /= bmag; - brot[2] /= bmag; + float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]); + brot[0] /= bmag; + brot[1] /= bmag; + brot[2] /= bmag; - // Only compute if neither vector is null - if (bmag < 1.0f || mag_len < 1.0f) - mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; - else - CrossProduct((const float *) &mag.x, (const float *) brot, mag_err); - } - } else { - mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; - } + // Only compute if neither vector is null + if (bmag < 1.0f || mag_len < 1.0f) { + mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; + } else { + CrossProduct((const float *)&mag.x, (const float *)brot, mag_err); + } + } + } else { + mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; + } - // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s - GyrosBiasData gyrosBias; - GyrosBiasGet(&gyrosBias); - gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi; - gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi; - gyrosBias.z -= mag_err[2] * magKi; - GyrosBiasSet(&gyrosBias); + // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s + GyrosBiasData gyrosBias; + GyrosBiasGet(&gyrosBias); + gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi; + gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi; + gyrosBias.z -= mag_err[2] * magKi; + GyrosBiasSet(&gyrosBias); - if (revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { - // if the raw values are not adjusted, we need to adjust here. - gyrosData.x -= gyrosBias.x; - gyrosData.y -= gyrosBias.y; - gyrosData.z -= gyrosBias.z; - } + if (revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { + // if the raw values are not adjusted, we need to adjust here. + gyrosData.x -= gyrosBias.x; + gyrosData.y -= gyrosBias.y; + gyrosData.z -= gyrosBias.z; + } - // Correct rates based on error, integral component dealt with in updateSensors - gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT; - gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT; - gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; + // Correct rates based on error, integral component dealt with in updateSensors + gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT; + gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT; + gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; - // Work out time derivative from INSAlgo writeup - // Also accounts for the fact that gyros are in deg/s - float qdot[4]; - qdot[0] = DEG2RAD(-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT / 2; - qdot[1] = DEG2RAD(q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT / 2; - qdot[2] = DEG2RAD(q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT / 2; - qdot[3] = DEG2RAD(-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT / 2; + // Work out time derivative from INSAlgo writeup + // Also accounts for the fact that gyros are in deg/s + float qdot[4]; + qdot[0] = DEG2RAD(-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT / 2; + qdot[1] = DEG2RAD(q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT / 2; + qdot[2] = DEG2RAD(q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT / 2; + qdot[3] = DEG2RAD(-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT / 2; - // Take a time step - q[0] = q[0] + qdot[0]; - q[1] = q[1] + qdot[1]; - q[2] = q[2] + qdot[2]; - q[3] = q[3] + qdot[3]; + // Take a time step + q[0] = q[0] + qdot[0]; + q[1] = q[1] + qdot[1]; + q[2] = q[2] + qdot[2]; + q[3] = q[3] + qdot[3]; - if(q[0] < 0.0f) { - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; - } + if (q[0] < 0.0f) { + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } - // Renomalize - qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); - q[0] = q[0] / qmag; - q[1] = q[1] / qmag; - q[2] = q[2] / qmag; - q[3] = q[3] / qmag; + // Renomalize + qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + q[0] = q[0] / qmag; + q[1] = q[1] / qmag; + q[2] = q[2] / qmag; + q[3] = q[3] / qmag; - // If quaternion has become inappropriately short or is nan reinit. - // THIS SHOULD NEVER ACTUALLY HAPPEN - if((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { - q[0] = 1.0f; - q[1] = 0.0f; - q[2] = 0.0f; - q[3] = 0.0f; - } + // If quaternion has become inappropriately short or is nan reinit. + // THIS SHOULD NEVER ACTUALLY HAPPEN + if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { + q[0] = 1.0f; + q[1] = 0.0f; + q[2] = 0.0f; + q[3] = 0.0f; + } - quat_copy(q, &attitudeActual.q1); + quat_copy(q, &attitudeActual.q1); - // Convert into eueler degrees (makes assumptions about RPY order) - Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll); + // Convert into eueler degrees (makes assumptions about RPY order) + Quaternion2RPY(&attitudeActual.q1, &attitudeActual.Roll); - AttitudeActualSet(&attitudeActual); + AttitudeActualSet(&attitudeActual); - // Flush these queues for avoid errors - xQueueReceive(baroQueue, &ev, 0); - if ( xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE ) { - float NED[3]; - // Transform the GPS position into NED coordinates - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - getNED(&gpsPosition, NED); - - PositionActualData positionActual; - PositionActualGet(&positionActual); - positionActual.North = NED[0]; - positionActual.East = NED[1]; - positionActual.Down = NED[2]; - PositionActualSet(&positionActual); - } + // Flush these queues for avoid errors + xQueueReceive(baroQueue, &ev, 0); + if (xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE) { + float NED[3]; + // Transform the GPS position into NED coordinates + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + getNED(&gpsPosition, NED); - if ( xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE ) { - // Transform the GPS position into NED coordinates - GPSVelocityData gpsVelocity; - GPSVelocityGet(&gpsVelocity); + PositionActualData positionActual; + PositionActualGet(&positionActual); + positionActual.North = NED[0]; + positionActual.East = NED[1]; + positionActual.Down = NED[2]; + PositionActualSet(&positionActual); + } - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); - velocityActual.North = gpsVelocity.North; - velocityActual.East = gpsVelocity.East; - velocityActual.Down = gpsVelocity.Down; - VelocityActualSet(&velocityActual); - } - - if ( xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE ) { - // Calculate true airspeed from indicated airspeed - AirspeedSensorData airspeedSensor; - AirspeedSensorGet(&airspeedSensor); + if (xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE) { + // Transform the GPS position into NED coordinates + GPSVelocityData gpsVelocity; + GPSVelocityGet(&gpsVelocity); - AirspeedActualData airspeed; - AirspeedActualGet(&airspeed); + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); + velocityActual.North = gpsVelocity.North; + velocityActual.East = gpsVelocity.East; + velocityActual.Down = gpsVelocity.Down; + VelocityActualSet(&velocityActual); + } - PositionActualData positionActual; - PositionActualGet(&positionActual); + if (xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE) { + // Calculate true airspeed from indicated airspeed + AirspeedSensorData airspeedSensor; + AirspeedSensorGet(&airspeedSensor); - if (airspeedSensor.SensorConnected==AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { - // we have airspeed available - airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed; - airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS( homeLocation.Altitude - positionActual.Down ); - AirspeedActualSet(&airspeed); - } - } + AirspeedActualData airspeed; + AirspeedActualGet(&airspeed); + + PositionActualData positionActual; + PositionActualGet(&positionActual); + + if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { + // we have airspeed available + airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed; + airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionActual.Down); + AirspeedActualSet(&airspeed); + } + } - if ( variance_error ) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - } + if (variance_error) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + } - return 0; + return 0; } #include "insgps.h" @@ -581,81 +580,80 @@ int32_t init_stage = 0; */ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) { - UAVObjEvent ev; - GyrosData gyrosData; - AccelsData accelsData; - MagnetometerData magData; - AirspeedSensorData airspeedData; - BaroAltitudeData baroData; - GPSPositionData gpsData; - GPSVelocityData gpsVelData; - GyrosBiasData gyrosBias; + UAVObjEvent ev; + GyrosData gyrosData; + AccelsData accelsData; + MagnetometerData magData; + AirspeedSensorData airspeedData; + BaroAltitudeData baroData; + GPSPositionData gpsData; + GPSVelocityData gpsVelData; + GyrosBiasData gyrosBias; - static bool mag_updated = false; - static bool baro_updated; - static bool airspeed_updated; - static bool gps_updated; - static bool gps_vel_updated; + static bool mag_updated = false; + static bool baro_updated; + static bool airspeed_updated; + static bool gps_updated; + static bool gps_vel_updated; - static bool value_error = false; + static bool value_error = false; - static float baroOffset = 0.0f; + static float baroOffset = 0.0f; - static uint32_t ins_last_time = 0; - static bool inited; + static uint32_t ins_last_time = 0; + static bool inited; - float NED[3] = {0.0f, 0.0f, 0.0f}; - float vel[3] = {0.0f, 0.0f, 0.0f}; - float zeros[3] = {0.0f, 0.0f, 0.0f}; + float NED[3] = { 0.0f, 0.0f, 0.0f }; + float vel[3] = { 0.0f, 0.0f, 0.0f }; + float zeros[3] = { 0.0f, 0.0f, 0.0f }; - // Perform the update - uint16_t sensors = 0; - float dT; + // Perform the update + uint16_t sensors = 0; + float dT; - // Wait until the gyro and accel object is updated, if a timeout then go to failsafe - if ( (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) || - (xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) ) - { - // Do not set attitude timeout warnings in simulation mode - if (!AttitudeActualReadOnly()){ - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING); - return -1; - } - } + // Wait until the gyro and accel object is updated, if a timeout then go to failsafe + if ((xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) || + (xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE)) { + // Do not set attitude timeout warnings in simulation mode + if (!AttitudeActualReadOnly()) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); + return -1; + } + } - if (inited) { - mag_updated = 0; - baro_updated = 0; - airspeed_updated = 0; - gps_updated = 0; - gps_vel_updated = 0; - } + if (inited) { + mag_updated = 0; + baro_updated = 0; + airspeed_updated = 0; + gps_updated = 0; + gps_vel_updated = 0; + } - if (first_run) { - inited = false; - init_stage = 0; + if (first_run) { + inited = false; + init_stage = 0; - mag_updated = 0; - baro_updated = 0; - airspeed_updated = 0; - gps_updated = 0; - gps_vel_updated = 0; + mag_updated = 0; + baro_updated = 0; + airspeed_updated = 0; + gps_updated = 0; + gps_vel_updated = 0; - ins_last_time = PIOS_DELAY_GetRaw(); + ins_last_time = PIOS_DELAY_GetRaw(); - return 0; - } + return 0; + } - mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE); - baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; - airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; + mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE); + baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; + airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; - // Check if we are running simulation - if (!GPSPositionReadOnly()) { - gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; - } else { - gps_updated |= pdTRUE && outdoor_mode; - } + // Check if we are running simulation + if (!GPSPositionReadOnly()) { + gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; + } else { + gps_updated |= pdTRUE && outdoor_mode; + } if (!GPSVelocityReadOnly()) { gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; @@ -663,373 +661,375 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) gps_vel_updated |= pdTRUE && outdoor_mode; } - // Get most recent data - GyrosGet(&gyrosData); - AccelsGet(&accelsData); - MagnetometerGet(&magData); - BaroAltitudeGet(&baroData); - AirspeedSensorGet(&airspeedData); - GPSPositionGet(&gpsData); - GPSVelocityGet(&gpsVelData); - GyrosBiasGet(&gyrosBias); + // Get most recent data + GyrosGet(&gyrosData); + AccelsGet(&accelsData); + MagnetometerGet(&magData); + BaroAltitudeGet(&baroData); + AirspeedSensorGet(&airspeedData); + GPSPositionGet(&gpsData); + GPSVelocityGet(&gpsVelData); + GyrosBiasGet(&gyrosBias); - value_error = false; - // safety checks - if ( invalid(gyrosData.x) || - invalid(gyrosData.y) || - invalid(gyrosData.z) || - invalid(accelsData.x) || - invalid(accelsData.y) || - invalid(accelsData.z) ) { - // cannot run process update, raise error! - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); - return 0; - } - if ( invalid(gyrosBias.x) || - invalid(gyrosBias.y) || - invalid(gyrosBias.z) ) { - gyrosBias.x = 0.0f; - gyrosBias.y = 0.0f; - gyrosBias.z = 0.0f; - } + value_error = false; + // safety checks + if (invalid(gyrosData.x) || + invalid(gyrosData.y) || + invalid(gyrosData.z) || + invalid(accelsData.x) || + invalid(accelsData.y) || + invalid(accelsData.z)) { + // cannot run process update, raise error! + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + return 0; + } + if (invalid(gyrosBias.x) || + invalid(gyrosBias.y) || + invalid(gyrosBias.z)) { + gyrosBias.x = 0.0f; + gyrosBias.y = 0.0f; + gyrosBias.z = 0.0f; + } - if ( invalid(magData.x) || - invalid(magData.y) || - invalid(magData.z) ) { + if (invalid(magData.x) || + invalid(magData.y) || + invalid(magData.z)) { + // magnetometers can be ignored for a while + mag_updated = false; + value_error = true; + } - // magnetometers can be ignored for a while - mag_updated = false; - value_error = true; - } + // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily + // switching between indoor and outdoor mode with Set = false) + if ((homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f)) { + mag_updated = false; + value_error = true; + } - // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily - // switching between indoor and outdoor mode with Set = false) - if ( (homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f) ) { - mag_updated = false; - value_error = true; - } + if (invalid(baroData.Altitude)) { + baro_updated = false; + value_error = true; + } - if ( invalid(baroData.Altitude) ) { - baro_updated = false; - value_error = true; - } + if (invalid(airspeedData.CalibratedAirspeed)) { + airspeed_updated = false; + value_error = true; + } - if ( invalid(airspeedData.CalibratedAirspeed) ) { - airspeed_updated = false; - value_error = true; - } + if (invalid(gpsData.Altitude)) { + gps_updated = false; + value_error = true; + } - if ( invalid(gpsData.Altitude) ) { - gps_updated = false; - value_error = true; - } + if (invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH]) || + invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST]) || + invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN]) || + invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH]) || + invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST]) || + invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN])) { + gps_updated = false; + value_error = true; + } - if ( invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN]) ) { - gps_updated = false; - value_error = true; - } + if (invalid(gpsVelData.North) || + invalid(gpsVelData.East) || + invalid(gpsVelData.Down)) { + gps_vel_updated = false; + value_error = true; + } - if ( invalid(gpsVelData.North) || - invalid(gpsVelData.East) || - invalid(gpsVelData.Down) ) { - gps_vel_updated = false; - value_error = true; - } + // Discard airspeed if sensor not connected + if (airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { + airspeed_updated = false; + } - // Discard airspeed if sensor not connected - if ( airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE ) { - airspeed_updated = false; - } + // Have a minimum requirement for gps usage + if ((gpsData.Satellites < 7) || + (gpsData.PDOP > 4.0f) || + (gpsData.Latitude == 0 && gpsData.Longitude == 0) || + (homeLocation.Set != HOMELOCATION_SET_TRUE)) { + gps_updated = false; + gps_vel_updated = false; + } - // Have a minimum requirement for gps usage - if ( ( gpsData.Satellites < 7 ) || - ( gpsData.PDOP > 4.0f ) || - ( gpsData.Latitude==0 && gpsData.Longitude==0 ) || - ( homeLocation.Set != HOMELOCATION_SET_TRUE ) ) { - gps_updated = false; - gps_vel_updated = false; - } + if (!inited) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + } else if (value_error) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + } else if (variance_error) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + } else if (outdoor_mode && gpsData.Satellites < 7) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + } - if ( !inited ) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); - } else if ( value_error ) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL); - } else if ( variance_error ) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL); - } else if (outdoor_mode && gpsData.Satellites < 7) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - } - - dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; - ins_last_time = PIOS_DELAY_GetRaw(); + dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; + ins_last_time = PIOS_DELAY_GetRaw(); - // This should only happen at start up or at mode switches - if(dT > 0.01f) { - dT = 0.01f; - } else if(dT <= 0.001f) { - dT = 0.001f; - } + // This should only happen at start up or at mode switches + if (dT > 0.01f) { + dT = 0.01f; + } else if (dT <= 0.001f) { + dT = 0.001f; + } - if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode) && !variance_error) { + if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode) && !variance_error) { + // Don't initialize until all sensors are read + if (init_stage == 0) { + // Reset the INS algorithm + INSGPSInit(); + INSSetMagVar((float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_MAGX], + ekfConfiguration.R[EKFCONFIGURATION_R_MAGY], + ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] } + ); + INSSetAccelVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX], + ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY], + ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } + ); + INSSetGyroVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX], + ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY], + ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } + ); + INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX], + ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY], + ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } + ); + INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]); - // Don't initialize until all sensors are read - if (init_stage == 0) { + // Initialize the gyro bias + float gyro_bias[3] = { 0.0f, 0.0f, 0.0f }; + INSSetGyroBias(gyro_bias); - // Reset the INS algorithm - INSGPSInit(); - INSSetMagVar( (float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_MAGX], - ekfConfiguration.R[EKFCONFIGURATION_R_MAGY], - ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] } ); - INSSetAccelVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX], - ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY], - ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } ); - INSSetGyroVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } ); - INSSetGyroBiasVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } ); - INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]); + float pos[3] = { 0.0f, 0.0f, 0.0f }; - // Initialize the gyro bias - float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; - INSSetGyroBias(gyro_bias); + if (outdoor_mode) { + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); - float pos[3] = {0.0f, 0.0f, 0.0f}; + // Transform the GPS position into NED coordinates + getNED(&gpsPosition, pos); - if (outdoor_mode) { + // Initialize barometric offset to current GPS NED coordinate + baroOffset = -pos[2] - baroData.Altitude; + } else { + // Initialize barometric offset to homelocation altitude + baroOffset = -baroData.Altitude; + pos[2] = -(baroData.Altitude + baroOffset); + } - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); + xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); + MagnetometerGet(&magData); - // Transform the GPS position into NED coordinates - getNED(&gpsPosition, pos); + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); - // Initialize barometric offset to current GPS NED coordinate - baroOffset = -pos[2] - baroData.Altitude; + // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, + // so pseudo "north" vector can be estimated even if the board is not level + attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z); + float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y; + float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z; - } else { + // rotate accels z vector according to roll + float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y; + attitudeActual.Pitch = atan2f(accelsData.x, -azn); - // Initialize barometric offset to homelocation altitude - baroOffset = -baroData.Altitude; - pos[2] = -(baroData.Altitude + baroOffset); + float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn; - } + attitudeActual.Yaw = atan2f(-yn, xn); + // TODO: This is still a hack + // Put this in a proper generic function in CoordinateConversion.c + // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) + // should calculate the rotation in 3d space using proper cross product math + // SUBTODO: formulate the math required - xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); - MagnetometerGet(&magData); + attitudeActual.Roll = RAD2DEG(attitudeActual.Roll); + attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch); + attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw); - AttitudeActualData attitudeActual; - AttitudeActualGet (&attitudeActual); + RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1); + AttitudeActualSet(&attitudeActual); - // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, - // so pseudo "north" vector can be estimated even if the board is not level - attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z); - float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y; - float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z; + float q[4] = { attitudeActual.q1, attitudeActual.q2, attitudeActual.q3, attitudeActual.q4 }; + INSSetState(pos, zeros, q, zeros, zeros); - // rotate accels z vector according to roll - float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y; - attitudeActual.Pitch = atan2f(accelsData.x, -azn); + INSResetP(ekfConfiguration.P); + } else { + // Run prediction a bit before any corrections - float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn; + // Because the sensor module remove the bias we need to add it + // back in here so that the INS algorithm can track it correctly + float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) }; + if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { + gyros[0] += DEG2RAD(gyrosBias.x); + gyros[1] += DEG2RAD(gyrosBias.y); + gyros[2] += DEG2RAD(gyrosBias.z); + } + INSStatePrediction(gyros, &accelsData.x, dT); - attitudeActual.Yaw = atan2f(-yn, xn); - // TODO: This is still a hack - // Put this in a proper generic function in CoordinateConversion.c - // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) - // should calculate the rotation in 3d space using proper cross product math - // SUBTODO: formulate the math required + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + attitude.q1 = Nav.q[0]; + attitude.q2 = Nav.q[1]; + attitude.q3 = Nav.q[2]; + attitude.q4 = Nav.q[3]; + Quaternion2RPY(&attitude.q1, &attitude.Roll); + AttitudeActualSet(&attitude); + } - attitudeActual.Roll = RAD2DEG(attitudeActual.Roll); - attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch); - attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw); + init_stage++; + if (init_stage > 10) { + inited = true; + } - RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); - AttitudeActualSet(&attitudeActual); + return 0; + } - float q[4] = { attitudeActual.q1, attitudeActual.q2, attitudeActual.q3, attitudeActual.q4 }; - INSSetState(pos, zeros, q, zeros, zeros); - - INSResetP(ekfConfiguration.P); + if (!inited) { + return 0; + } - } else { - // Run prediction a bit before any corrections + // Because the sensor module remove the bias we need to add it + // back in here so that the INS algorithm can track it correctly + float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) }; + if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { + gyros[0] += DEG2RAD(gyrosBias.x); + gyros[1] += DEG2RAD(gyrosBias.y); + gyros[2] += DEG2RAD(gyrosBias.z); + } - // Because the sensor module remove the bias we need to add it - // back in here so that the INS algorithm can track it correctly - float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) }; - if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { - gyros[0] += DEG2RAD(gyrosBias.x); - gyros[1] += DEG2RAD(gyrosBias.y); - gyros[2] += DEG2RAD(gyrosBias.z); - } - INSStatePrediction(gyros, &accelsData.x, dT); + // Advance the state estimate + INSStatePrediction(gyros, &accelsData.x, dT); - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - attitude.q1 = Nav.q[0]; - attitude.q2 = Nav.q[1]; - attitude.q3 = Nav.q[2]; - attitude.q4 = Nav.q[3]; - Quaternion2RPY(&attitude.q1,&attitude.Roll); - AttitudeActualSet(&attitude); - } + // Copy the attitude into the UAVO + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + attitude.q1 = Nav.q[0]; + attitude.q2 = Nav.q[1]; + attitude.q3 = Nav.q[2]; + attitude.q4 = Nav.q[3]; + Quaternion2RPY(&attitude.q1, &attitude.Roll); + AttitudeActualSet(&attitude); - init_stage++; - if(init_stage > 10) - inited = true; + // Advance the covariance estimate + INSCovariancePrediction(dT); - return 0; - } + if (mag_updated) { + sensors |= MAG_SENSORS; + } - if (!inited) - return 0; + if (baro_updated) { + sensors |= BARO_SENSOR; + } - // Because the sensor module remove the bias we need to add it - // back in here so that the INS algorithm can track it correctly - float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) }; - if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { - gyros[0] += DEG2RAD(gyrosBias.x); - gyros[1] += DEG2RAD(gyrosBias.y); - gyros[2] += DEG2RAD(gyrosBias.z); - } + INSSetMagNorth(homeLocation.Be); - // Advance the state estimate - INSStatePrediction(gyros, &accelsData.x, dT); + if (gps_updated && outdoor_mode) { + INSSetPosVelVar((float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH], + ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST], + ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] }, + (float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH], + ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST], + ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] } + ); + sensors |= POS_SENSORS; - // Copy the attitude into the UAVO - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - attitude.q1 = Nav.q[0]; - attitude.q2 = Nav.q[1]; - attitude.q3 = Nav.q[2]; - attitude.q4 = Nav.q[3]; - Quaternion2RPY(&attitude.q1,&attitude.Roll); - AttitudeActualSet(&attitude); + if (0) { // Old code to take horizontal velocity from GPS Position update + sensors |= HORIZ_SENSORS; + vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading)); + vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading)); + vel[2] = 0.0f; + } + // Transform the GPS position into NED coordinates + getNED(&gpsData, NED); - // Advance the covariance estimate - INSCovariancePrediction(dT); + // Track barometric altitude offset with a low pass filter + baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + + (1.0f - BARO_OFFSET_LOWPASS_ALPHA) + * (-NED[2] - baroData.Altitude); + } else if (!outdoor_mode) { + INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, + (float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] } + ); + vel[0] = vel[1] = vel[2] = 0.0f; + NED[0] = NED[1] = 0.0f; + NED[2] = -(baroData.Altitude + baroOffset); + sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS; + sensors |= POS_SENSORS | VERT_SENSORS; + } - if(mag_updated) - sensors |= MAG_SENSORS; + if (gps_vel_updated && outdoor_mode) { + sensors |= HORIZ_SENSORS | VERT_SENSORS; + vel[0] = gpsVelData.North; + vel[1] = gpsVelData.East; + vel[2] = gpsVelData.Down; + } - if(baro_updated) - sensors |= BARO_SENSOR; + if (airspeed_updated) { + // we have airspeed available + AirspeedActualData airspeed; + AirspeedActualGet(&airspeed); - INSSetMagNorth(homeLocation.Be); - - if (gps_updated && outdoor_mode) - { - INSSetPosVelVar((float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] }, - (float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] }); - sensors |= POS_SENSORS; + airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed; + airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - Nav.Pos[2]); + AirspeedActualSet(&airspeed); - if (0) { // Old code to take horizontal velocity from GPS Position update - sensors |= HORIZ_SENSORS; - vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading)); - vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading)); - vel[2] = 0.0f; - } - // Transform the GPS position into NED coordinates - getNED(&gpsData, NED); + if (!gps_vel_updated && !gps_updated) { + // feed airspeed into EKF, treat wind as 1e2 variance + sensors |= HORIZ_SENSORS | VERT_SENSORS; + INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, + (float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] } + ); + // rotate airspeed vector into NED frame - airspeed is measured in X axis only + float R[3][3]; + Quaternion2R(Nav.q, R); + float vtas[3] = { airspeed.TrueAirspeed, 0.0f, 0.0f }; + rot_mult(R, vtas, vel); + } + } - // Track barometric altitude offset with a low pass filter - baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + - (1.0f - BARO_OFFSET_LOWPASS_ALPHA ) - * ( -NED[2] - baroData.Altitude ); + /* + * TODO: Need to add a general sanity check for all the inputs to make sure their kosher + * although probably should occur within INS itself + */ + if (sensors) { + INSCorrection(&magData.x, NED, vel, (baroData.Altitude + baroOffset), sensors); + } - } else if (!outdoor_mode) { - INSSetPosVelVar((float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, - (float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] }); - vel[0] = vel[1] = vel[2] = 0.0f; - NED[0] = NED[1] = 0.0f; - NED[2] = -(baroData.Altitude + baroOffset); - sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS; - sensors |= POS_SENSORS |VERT_SENSORS; - } + // Copy the position and velocity into the UAVO + PositionActualData positionActual; + PositionActualGet(&positionActual); + positionActual.North = Nav.Pos[0]; + positionActual.East = Nav.Pos[1]; + positionActual.Down = Nav.Pos[2]; + PositionActualSet(&positionActual); - if (gps_vel_updated && outdoor_mode) { - sensors |= HORIZ_SENSORS | VERT_SENSORS; - vel[0] = gpsVelData.North; - vel[1] = gpsVelData.East; - vel[2] = gpsVelData.Down; - } + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); + velocityActual.North = Nav.Vel[0]; + velocityActual.East = Nav.Vel[1]; + velocityActual.Down = Nav.Vel[2]; + VelocityActualSet(&velocityActual); - if (airspeed_updated) { - // we have airspeed available - AirspeedActualData airspeed; - AirspeedActualGet(&airspeed); + gyrosBias.x = RAD2DEG(Nav.gyro_bias[0]); + gyrosBias.y = RAD2DEG(Nav.gyro_bias[1]); + gyrosBias.z = RAD2DEG(Nav.gyro_bias[2]); + GyrosBiasSet(&gyrosBias); - airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed; - airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS( homeLocation.Altitude - Nav.Pos[2] ); - AirspeedActualSet(&airspeed); - - if ( !gps_vel_updated && !gps_updated) { - // feed airspeed into EKF, treat wind as 1e2 variance - sensors |= HORIZ_SENSORS | VERT_SENSORS; - INSSetPosVelVar((float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, - (float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] }); - // rotate airspeed vector into NED frame - airspeed is measured in X axis only - float R[3][3]; - Quaternion2R(Nav.q,R); - float vtas[3] = {airspeed.TrueAirspeed,0.0f,0.0f}; - rot_mult(R,vtas,vel); - } - } - - /* - * TODO: Need to add a general sanity check for all the inputs to make sure their kosher - * although probably should occur within INS itself - */ - if (sensors) - INSCorrection(&magData.x, NED, vel, ( baroData.Altitude + baroOffset ), sensors); + EKFStateVarianceData vardata; + EKFStateVarianceGet(&vardata); + INSGetP(vardata.P); + EKFStateVarianceSet(&vardata); - // Copy the position and velocity into the UAVO - PositionActualData positionActual; - PositionActualGet(&positionActual); - positionActual.North = Nav.Pos[0]; - positionActual.East = Nav.Pos[1]; - positionActual.Down = Nav.Pos[2]; - PositionActualSet(&positionActual); - - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); - velocityActual.North = Nav.Vel[0]; - velocityActual.East = Nav.Vel[1]; - velocityActual.Down = Nav.Vel[2]; - VelocityActualSet(&velocityActual); - - gyrosBias.x = RAD2DEG(Nav.gyro_bias[0]); - gyrosBias.y = RAD2DEG(Nav.gyro_bias[1]); - gyrosBias.z = RAD2DEG(Nav.gyro_bias[2]); - GyrosBiasSet(&gyrosBias); - - EKFStateVarianceData vardata; - EKFStateVarianceGet(&vardata); - INSGetP(vardata.P); - EKFStateVarianceSet(&vardata); - - return 0; + return 0; } /** @@ -1042,90 +1042,90 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) * @returns 0 for success, -1 for failure */ float T[3]; -static int32_t getNED(GPSPositionData * gpsPosition, float * NED) +static int32_t getNED(GPSPositionData *gpsPosition, float *NED) { - float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f), - DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f), - (gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) }; + float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f), + DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f), + (gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) }; - NED[0] = T[0] * dL[0]; - NED[1] = T[1] * dL[1]; - NED[2] = T[2] * dL[2]; + NED[0] = T[0] * dL[0]; + NED[1] = T[1] * dL[1]; + NED[2] = T[2] * dL[2]; - return 0; + return 0; } -static void settingsUpdatedCb(UAVObjEvent * ev) +static void settingsUpdatedCb(UAVObjEvent *ev) { - if (ev == NULL || ev->obj == FlightStatusHandle()) { - FlightStatusGet(&flightStatus); - } - if (ev == NULL || ev->obj == RevoCalibrationHandle()) { - RevoCalibrationGet(&revoCalibration); - } - // change of these settings require reinitialization of the EKF - // when an error flag has been risen, we also listen to flightStatus updates, - // since we are waiting for the system to get disarmed so we can reinitialize safely. - if (ev == NULL || - ev->obj == EKFConfigurationHandle() || - ev->obj == RevoSettingsHandle() || - ( variance_error==true && ev->obj == FlightStatusHandle() ) - ) { + if (ev == NULL || ev->obj == FlightStatusHandle()) { + FlightStatusGet(&flightStatus); + } + if (ev == NULL || ev->obj == RevoCalibrationHandle()) { + RevoCalibrationGet(&revoCalibration); + } + // change of these settings require reinitialization of the EKF + // when an error flag has been risen, we also listen to flightStatus updates, + // since we are waiting for the system to get disarmed so we can reinitialize safely. + if (ev == NULL || + ev->obj == EKFConfigurationHandle() || + ev->obj == RevoSettingsHandle() || + (variance_error == true && ev->obj == FlightStatusHandle()) + ) { + bool error = false; - bool error = false; + EKFConfigurationGet(&ekfConfiguration); + int t; + for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { + if (invalid_var(ekfConfiguration.P[t])) { + error = true; + } + } + for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { + if (invalid_var(ekfConfiguration.Q[t])) { + error = true; + } + } + for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) { + if (invalid_var(ekfConfiguration.R[t])) { + error = true; + } + } - EKFConfigurationGet(&ekfConfiguration); - int t; - for (t=0; t < EKFCONFIGURATION_P_NUMELEM; t++) { - if (invalid_var(ekfConfiguration.P[t])) { - error = true; - } - } - for (t=0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { - if (invalid_var(ekfConfiguration.Q[t])) { - error = true; - } - } - for (t=0; t < EKFCONFIGURATION_R_NUMELEM; t++) { - if (invalid_var(ekfConfiguration.R[t])) { - error = true; - } - } + RevoSettingsGet(&revoSettings); - RevoSettingsGet(&revoSettings); + // Reinitialization of the EKF is not desired during flight. + // It will be delayed until the board is disarmed by raising the error flag. + // We will not prevent the initial initialization though, since the board could be in always armed mode. + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required) { + error = true; + } - // Reinitialization of the EKF is not desired during flight. - // It will be delayed until the board is disarmed by raising the error flag. - // We will not prevent the initial initialization though, since the board could be in always armed mode. - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required ) { - error = true; - } + if (error) { + variance_error = true; + } else { + // trigger reinitialization - possibly with new algorithm + running_algorithm = revoSettings.FusionAlgorithm; + variance_error = false; + initialization_required = true; + } + } + if (ev == NULL || ev->obj == HomeLocationHandle()) { + HomeLocationGet(&homeLocation); + // Compute matrix to convert deltaLLA to NED + float lat, alt; + lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); + alt = homeLocation.Altitude; - if (error) { - variance_error = true; - } else { - // trigger reinitialization - possibly with new algorithm - running_algorithm = revoSettings.FusionAlgorithm; - variance_error = false; - initialization_required = true; - } - } - if(ev == NULL || ev->obj == HomeLocationHandle()) { - HomeLocationGet(&homeLocation); - // Compute matrix to convert deltaLLA to NED - float lat, alt; - lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); - alt = homeLocation.Altitude; + T[0] = alt + 6.378137E6f; + T[1] = cosf(lat) * (alt + 6.378137E6f); + T[2] = -1.0f; - T[0] = alt+6.378137E6f; - T[1] = cosf(lat)*(alt+6.378137E6f); - T[2] = -1.0f; - - // TODO: convert positionActual to new reference frame and gracefully update EKF state! - // needed for long range flights where the reference coordinate is adjusted in flight - } - if (ev == NULL || ev->obj == AttitudeSettingsHandle()) - AttitudeSettingsGet(&attitudeSettings); + // TODO: convert positionActual to new reference frame and gracefully update EKF state! + // needed for long range flights where the reference coordinate is adjusted in flight + } + if (ev == NULL || ev->obj == AttitudeSettingsHandle()) { + AttitudeSettingsGet(&attitudeSettings); + } } /** * @} diff --git a/flight/modules/Attitude/revolution/inc/attitude.h b/flight/modules/Attitude/revolution/inc/attitude.h index 3f90e56a6..64c556a8e 100644 --- a/flight/modules/Attitude/revolution/inc/attitude.h +++ b/flight/modules/Attitude/revolution/inc/attitude.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup Attitude Attitude Module - * @{ + * @{ * * @file attitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. diff --git a/flight/modules/Autotune/autotune.c b/flight/modules/Autotune/autotune.c index 119791993..d14c75090 100644 --- a/flight/modules/Autotune/autotune.c +++ b/flight/modules/Autotune/autotune.c @@ -3,7 +3,7 @@ * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup Autotuning module - * @brief Reads from @ref ManualControlCommand and fakes a rate mode while + * @brief Reads from @ref ManualControlCommand and fakes a rate mode while * toggling the channels to relay mode * @{ * @@ -59,13 +59,13 @@ #include "stabilizationdesired.h" #include "stabilizationsettings.h" #include "taskinfo.h" - + // Private constants #define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY+2) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) // Private types -enum AUTOTUNE_STATE {AT_INIT, AT_START, AT_ROLL, AT_PITCH, AT_FINISHED, AT_SET}; +enum AUTOTUNE_STATE { AT_INIT, AT_START, AT_ROLL, AT_PITCH, AT_FINISHED, AT_SET }; // Private variables static xTaskHandle taskHandle; @@ -81,22 +81,23 @@ static void update_stabilization_settings(); */ int32_t AutotuneInitialize(void) { - // Create a queue, connect to manual control command and flightstatus + // Create a queue, connect to manual control command and flightstatus #ifdef MODULE_AUTOTUNE_BUILTIN - autotuneEnabled = true; + autotuneEnabled = true; #else - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_AUTOTUNE] == HWSETTINGS_OPTIONALMODULES_ENABLED) - autotuneEnabled = true; - else - autotuneEnabled = false; + if (optionalModules[HWSETTINGS_OPTIONALMODULES_AUTOTUNE] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + autotuneEnabled = true; + } else { + autotuneEnabled = false; + } #endif - return 0; + return 0; } /** @@ -105,14 +106,14 @@ int32_t AutotuneInitialize(void) */ int32_t AutotuneStart(void) { - // Start main task if it is enabled - if(autotuneEnabled) { - xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); + // Start main task if it is enabled + if (autotuneEnabled) { + xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE); - } - return 0; + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE); + } + return 0; } MODULE_INITCALL(AutotuneInitialize, AutotuneStart) @@ -122,210 +123,211 @@ MODULE_INITCALL(AutotuneInitialize, AutotuneStart) */ static void AutotuneTask(__attribute__((unused)) void *parameters) { - //AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - - enum AUTOTUNE_STATE state = AT_INIT; + // AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - portTickType lastUpdateTime = xTaskGetTickCount(); + enum AUTOTUNE_STATE state = AT_INIT; - while(1) { + portTickType lastUpdateTime = xTaskGetTickCount(); - PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); - // TODO: - // 1. get from queue - // 2. based on whether it is flightstatus or manualcontrol + while (1) { + PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); + // TODO: + // 1. get from queue + // 2. based on whether it is flightstatus or manualcontrol - portTickType diffTime; + portTickType diffTime; - const uint32_t PREPARE_TIME = 2000; - const uint32_t MEAURE_TIME = 30000; + const uint32_t PREPARE_TIME = 2000; + const uint32_t MEAURE_TIME = 30000; - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); - // Only allow this module to run when autotuning - if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { - state = AT_INIT; - vTaskDelay(50); - continue; - } + // Only allow this module to run when autotuning + if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { + state = AT_INIT; + vTaskDelay(50); + continue; + } - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); - StabilizationSettingsData stabSettings; - StabilizationSettingsGet(&stabSettings); + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); - ManualControlSettingsData manualSettings; - ManualControlSettingsGet(&manualSettings); + ManualControlSettingsData manualSettings; + ManualControlSettingsGet(&manualSettings); - ManualControlCommandData manualControl; - ManualControlCommandGet(&manualControl); + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); - RelayTuningSettingsData relaySettings; - RelayTuningSettingsGet(&relaySettings); + RelayTuningSettingsData relaySettings; + RelayTuningSettingsGet(&relaySettings); - bool rate = relaySettings.Mode == RELAYTUNINGSETTINGS_MODE_RATE; + bool rate = relaySettings.Mode == RELAYTUNINGSETTINGS_MODE_RATE; - if (rate) { // rate mode - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + if (rate) { // rate mode + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; - stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; - } else { - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; + stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; + } else { + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.Roll = manualControl.Roll * stabSettings.RollMax; - stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax; - } + stabDesired.Roll = manualControl.Roll * stabSettings.RollMax; + stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax; + } - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; - stabDesired.Throttle = manualControl.Throttle; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; + stabDesired.Throttle = manualControl.Throttle; - switch(state) { - case AT_INIT: + switch (state) { + case AT_INIT: - lastUpdateTime = xTaskGetTickCount(); + lastUpdateTime = xTaskGetTickCount(); - // Only start when armed and flying - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0) - state = AT_START; - break; + // Only start when armed and flying + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0) { + state = AT_START; + } + break; - case AT_START: + case AT_START: - diffTime = xTaskGetTickCount() - lastUpdateTime; + diffTime = xTaskGetTickCount() - lastUpdateTime; - // Spend the first block of time in normal rate mode to get airborne - if (diffTime > PREPARE_TIME) { - state = AT_ROLL; - lastUpdateTime = xTaskGetTickCount(); - } - break; + // Spend the first block of time in normal rate mode to get airborne + if (diffTime > PREPARE_TIME) { + state = AT_ROLL; + lastUpdateTime = xTaskGetTickCount(); + } + break; - case AT_ROLL: + case AT_ROLL: - diffTime = xTaskGetTickCount() - lastUpdateTime; + diffTime = xTaskGetTickCount() - lastUpdateTime; - // Run relay mode on the roll axis for the measurement time - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : - STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; - if (diffTime > MEAURE_TIME) { // Move on to next state - state = AT_PITCH; - lastUpdateTime = xTaskGetTickCount(); - } - break; + // Run relay mode on the roll axis for the measurement time + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : + STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; + if (diffTime > MEAURE_TIME) { // Move on to next state + state = AT_PITCH; + lastUpdateTime = xTaskGetTickCount(); + } + break; - case AT_PITCH: + case AT_PITCH: - diffTime = xTaskGetTickCount() - lastUpdateTime; + diffTime = xTaskGetTickCount() - lastUpdateTime; - // Run relay mode on the pitch axis for the measurement time - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : - STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; - if (diffTime > MEAURE_TIME) { // Move on to next state - state = AT_FINISHED; - lastUpdateTime = xTaskGetTickCount(); - } - break; + // Run relay mode on the pitch axis for the measurement time + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : + STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; + if (diffTime > MEAURE_TIME) { // Move on to next state + state = AT_FINISHED; + lastUpdateTime = xTaskGetTickCount(); + } + break; - case AT_FINISHED: + case AT_FINISHED: - // Wait until disarmed and landed before updating the settings - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0) - state = AT_SET; + // Wait until disarmed and landed before updating the settings + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0) { + state = AT_SET; + } - break; + break; - case AT_SET: - update_stabilization_settings(); - state = AT_INIT; - break; + case AT_SET: + update_stabilization_settings(); + state = AT_INIT; + break; - default: - // Set an alarm or some shit like that - break; - } + default: + // Set an alarm or some shit like that + break; + } - StabilizationDesiredSet(&stabDesired); + StabilizationDesiredSet(&stabDesired); - vTaskDelay(10); - } + vTaskDelay(10); + } } /** * Called after measuring roll and pitch to update the * stabilization settings - * + * * takes in @ref RelayTuning and outputs @ref StabilizationSettings */ static void update_stabilization_settings() { - RelayTuningData relayTuning; - RelayTuningGet(&relayTuning); + RelayTuningData relayTuning; - RelayTuningSettingsData relaySettings; - RelayTuningSettingsGet(&relaySettings); + RelayTuningGet(&relayTuning); - StabilizationSettingsData stabSettings; - StabilizationSettingsGet(&stabSettings); + RelayTuningSettingsData relaySettings; + RelayTuningSettingsGet(&relaySettings); - // Eventually get these settings from RelayTuningSettings - const float gain_ratio_r = 1.0f / 3.0f; - const float zero_ratio_r = 1.0f / 3.0f; - const float gain_ratio_p = 1.0f / 5.0f; - const float zero_ratio_p = 1.0f / 5.0f; + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); - // For now just run over roll and pitch - for (uint i = 0; i < 2; i++) { - float wu = 1000.0f * 2 * M_PI / relayTuning.Period[i]; // ultimate freq = output osc freq (rad/s) + // Eventually get these settings from RelayTuningSettings + const float gain_ratio_r = 1.0f / 3.0f; + const float zero_ratio_r = 1.0f / 3.0f; + const float gain_ratio_p = 1.0f / 5.0f; + const float zero_ratio_p = 1.0f / 5.0f; - float wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s) - float zc = wc * zero_ratio_r; // controller zero location (rad/s) - float kpu = 4.0f / M_PI / relayTuning.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity - float kp = kpu * gain_ratio_r; // proportional gain - float ki = zc * kp; // integral gain + // For now just run over roll and pitch + for (uint i = 0; i < 2; i++) { + float wu = 1000.0f * 2 * M_PI / relayTuning.Period[i]; // ultimate freq = output osc freq (rad/s) - // Now calculate gains for the next loop out knowing it is the integral of - // the inner loop -- the plant is position/velocity = scale*1/s - float wc2 = wc * gain_ratio_p; // crossover of the attitude loop - float kp2 = wc2; // kp of attitude - float ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude + float wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s) + float zc = wc * zero_ratio_r; // controller zero location (rad/s) + float kpu = 4.0f / M_PI / relayTuning.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity + float kp = kpu * gain_ratio_r; // proportional gain + float ki = zc * kp; // integral gain - switch(i) { - case 0: // roll - stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp; - stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki; - stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2; - stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2; - break; - case 1: // Pitch - stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp; - stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki; - stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2; - stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2; - break; - default: - // Oh shit oh shit oh shit - break; - } - } - switch(relaySettings.Behavior) { - case RELAYTUNINGSETTINGS_BEHAVIOR_MEASURE: - // Just measure, don't update the stab settings - break; - case RELAYTUNINGSETTINGS_BEHAVIOR_COMPUTE: - StabilizationSettingsSet(&stabSettings); - break; - case RELAYTUNINGSETTINGS_BEHAVIOR_SAVE: - StabilizationSettingsSet(&stabSettings); - UAVObjSave(StabilizationSettingsHandle(), 0); - break; - } - + // Now calculate gains for the next loop out knowing it is the integral of + // the inner loop -- the plant is position/velocity = scale*1/s + float wc2 = wc * gain_ratio_p; // crossover of the attitude loop + float kp2 = wc2; // kp of attitude + float ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude + + switch (i) { + case 0: // roll + stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp; + stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki; + stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2; + stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2; + break; + case 1: // Pitch + stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp; + stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki; + stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2; + stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2; + break; + default: + // Oh shit oh shit oh shit + break; + } + } + switch (relaySettings.Behavior) { + case RELAYTUNINGSETTINGS_BEHAVIOR_MEASURE: + // Just measure, don't update the stab settings + break; + case RELAYTUNINGSETTINGS_BEHAVIOR_COMPUTE: + StabilizationSettingsSet(&stabSettings); + break; + case RELAYTUNINGSETTINGS_BEHAVIOR_SAVE: + StabilizationSettingsSet(&stabSettings); + UAVObjSave(StabilizationSettingsHandle(), 0); + break; + } } /** diff --git a/flight/modules/Autotune/inc/autotune.h b/flight/modules/Autotune/inc/autotune.h index 3f90e56a6..64c556a8e 100644 --- a/flight/modules/Autotune/inc/autotune.h +++ b/flight/modules/Autotune/inc/autotune.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup Attitude Attitude Module - * @{ + * @{ * * @file attitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. diff --git a/flight/modules/Battery/battery.c b/flight/modules/Battery/battery.c index c0f6db89e..66ed8893a 100644 --- a/flight/modules/Battery/battery.c +++ b/flight/modules/Battery/battery.c @@ -54,21 +54,21 @@ // // Configuration // -#define SAMPLE_PERIOD_MS 500 +#define SAMPLE_PERIOD_MS 500 // Private types // Private variables static bool batteryEnabled = false; -//THESE COULD BE BETTER AS SOME KIND OF UNION OR STRUCT, BY WHICH 4 BITS ARE USED FOR EACH -//PIN VARIABLE, ONE OF WHICH INDICATES SIGN, AND THE OTHER 3 BITS INDICATE POSITION. THIS WILL -//WORK FOR QUITE SOMETIME, UNTIL MORE THAN 8 ADC ARE AVAILABLE. EVEN AT THIS POINT, THE STRUCTURE -//CAN SIMPLY BE MODIFIED TO SUPPORT 15 ADC PINS, BY USING ALL AVAILABLE BITS. -static int8_t voltageADCPin=-1; //ADC pin for voltage -static int8_t currentADCPin=-1; //ADC pin for current +// THESE COULD BE BETTER AS SOME KIND OF UNION OR STRUCT, BY WHICH 4 BITS ARE USED FOR EACH +// PIN VARIABLE, ONE OF WHICH INDICATES SIGN, AND THE OTHER 3 BITS INDICATE POSITION. THIS WILL +// WORK FOR QUITE SOMETIME, UNTIL MORE THAN 8 ADC ARE AVAILABLE. EVEN AT THIS POINT, THE STRUCTURE +// CAN SIMPLY BE MODIFIED TO SUPPORT 15 ADC PINS, BY USING ALL AVAILABLE BITS. +static int8_t voltageADCPin = -1; // ADC pin for voltage +static int8_t currentADCPin = -1; // ADC pin for current // Private functions -static void onTimer(UAVObjEvent* ev); +static void onTimer(UAVObjEvent *ev); /** * Initialise the module, called on startup @@ -76,137 +76,136 @@ static void onTimer(UAVObjEvent* ev); */ int32_t BatteryInitialize(void) { - - #ifdef MODULE_BATTERY_BUILTIN - batteryEnabled = true; + batteryEnabled = true; #else - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(optionalModules); - if ((optionalModules[HWSETTINGS_OPTIONALMODULES_BATTERY] == HWSETTINGS_OPTIONALMODULES_ENABLED)) - batteryEnabled = true; - else - batteryEnabled = false; + if ((optionalModules[HWSETTINGS_OPTIONALMODULES_BATTERY] == HWSETTINGS_OPTIONALMODULES_ENABLED)) { + batteryEnabled = true; + } else { + batteryEnabled = false; + } #endif - uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM]; - HwSettingsADCRoutingGet(adcRouting); - - //Determine if the battery sensors are routed to ADC pins - for (int i=0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { - if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYVOLTAGE) { - voltageADCPin = i; - } - if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYCURRENT) { - currentADCPin = i; - } - } - - //Don't enable module if no ADC pins are routed to the sensors - if (voltageADCPin <0 && currentADCPin <0) - batteryEnabled = false; + uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM]; + HwSettingsADCRoutingGet(adcRouting); - //Start module - if (batteryEnabled) { - FlightBatteryStateInitialize(); - FlightBatterySettingsInitialize(); - - static UAVObjEvent ev; + // Determine if the battery sensors are routed to ADC pins + for (int i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { + if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYVOLTAGE) { + voltageADCPin = i; + } + if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYCURRENT) { + currentADCPin = i; + } + } - memset(&ev,0,sizeof(UAVObjEvent)); - EventPeriodicCallbackCreate(&ev, onTimer, SAMPLE_PERIOD_MS / portTICK_RATE_MS); - } + // Don't enable module if no ADC pins are routed to the sensors + if (voltageADCPin < 0 && currentADCPin < 0) { + batteryEnabled = false; + } - return 0; + // Start module + if (batteryEnabled) { + FlightBatteryStateInitialize(); + FlightBatterySettingsInitialize(); + + static UAVObjEvent ev; + + memset(&ev, 0, sizeof(UAVObjEvent)); + EventPeriodicCallbackCreate(&ev, onTimer, SAMPLE_PERIOD_MS / portTICK_RATE_MS); + } + + return 0; } MODULE_INITCALL(BatteryInitialize, 0) -#define HAS_SENSOR(x) batterySettings.SensorType[x]==FLIGHTBATTERYSETTINGS_SENSORTYPE_ENABLED -static void onTimer(__attribute__((unused)) UAVObjEvent* ev) +#define HAS_SENSOR(x) batterySettings.SensorType[x] == FLIGHTBATTERYSETTINGS_SENSORTYPE_ENABLED +static void onTimer(__attribute__((unused)) UAVObjEvent *ev) { - static FlightBatteryStateData flightBatteryData; - FlightBatterySettingsData batterySettings; + static FlightBatteryStateData flightBatteryData; + FlightBatterySettingsData batterySettings; - FlightBatterySettingsGet(&batterySettings); + FlightBatterySettingsGet(&batterySettings); - static float dT = SAMPLE_PERIOD_MS / 1000.0f; - float energyRemaining; + static float dT = SAMPLE_PERIOD_MS / 1000.0f; + float energyRemaining; - //calculate the battery parameters - if (voltageADCPin >=0) { - flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(voltageADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts - } - else { - flightBatteryData.Voltage=1234; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC - } + // calculate the battery parameters + if (voltageADCPin >= 0) { + flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(voltageADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; // in Volts + } else { + flightBatteryData.Voltage = 1234; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC + } - if (currentADCPin >=0) { - flightBatteryData.Current = ((float)PIOS_ADC_PinGet(currentADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; //in Amps - if (flightBatteryData.Current > flightBatteryData.PeakCurrent) - flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps - } - else { //If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm - flightBatteryData.Current=-0.1234f; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC - } - - flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f) ;//in mAh - - //Apply a 2 second rise time low-pass filter to average the current - float alpha = 1.0f-dT/(dT+2.0f); - flightBatteryData.AvgCurrent=alpha*flightBatteryData.AvgCurrent+(1-alpha)*flightBatteryData.Current; //in Amps + if (currentADCPin >= 0) { + flightBatteryData.Current = ((float)PIOS_ADC_PinGet(currentADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; // in Amps + if (flightBatteryData.Current > flightBatteryData.PeakCurrent) { + flightBatteryData.PeakCurrent = flightBatteryData.Current; // in Amps + } + } else { // If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm + flightBatteryData.Current = -0.1234f; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC + } - /*The motor could regenerate power. Or we could have solar cells. - In short, is there any likelihood of measuring negative current? If it's a bad current reading we want to check, then - it makes sense to saturate at max and min values, because a misreading could as easily be very large, as negative. The simple - sign check doesn't catch this.*/ -// //sanity checks -// if (flightBatteryData.AvgCurrent<0) flightBatteryData.AvgCurrent=0.0f; -// if (flightBatteryData.PeakCurrent<0) flightBatteryData.PeakCurrent=0.0f; -// if (flightBatteryData.ConsumedEnergy<0) flightBatteryData.ConsumedEnergy=0.0f; + flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f); // in mAh - energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh - if (flightBatteryData.AvgCurrent > 0) - flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent*1000.0f))*3600.0f;//in Sec - else - flightBatteryData.EstimatedFlightTime = 9999; + // Apply a 2 second rise time low-pass filter to average the current + float alpha = 1.0f - dT / (dT + 2.0f); + flightBatteryData.AvgCurrent = alpha * flightBatteryData.AvgCurrent + (1 - alpha) * flightBatteryData.Current; // in Amps - //generate alarms where needed... - if ((flightBatteryData.Voltage<=0) && (flightBatteryData.Current<=0)) - { - //FIXME: There's no guarantee that a floating ADC will give 0. So this - // check might fail, even when there's nothing attached. - AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR); - AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR); - } - else - { - // FIXME: should make the timer alarms user configurable - if (flightBatteryData.EstimatedFlightTime < 30) - AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL); - else if (flightBatteryData.EstimatedFlightTime < 120) - AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING); - else - AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME); + /*The motor could regenerate power. Or we could have solar cells. + In short, is there any likelihood of measuring negative current? If it's a bad current reading we want to check, then + it makes sense to saturate at max and min values, because a misreading could as easily be very large, as negative. The simple + sign check doesn't catch this.*/ +////sanity checks +// if (flightBatteryData.AvgCurrent<0) flightBatteryData.AvgCurrent=0.0f; +// if (flightBatteryData.PeakCurrent<0) flightBatteryData.PeakCurrent=0.0f; +// if (flightBatteryData.ConsumedEnergy<0) flightBatteryData.ConsumedEnergy=0.0f; - // FIXME: should make the battery voltage detection dependent on battery type. - /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/ - if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM]) - AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); - else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING]) - AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); - else - AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); - } - - FlightBatteryStateSet(&flightBatteryData); + energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh + if (flightBatteryData.AvgCurrent > 0) { + flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent * 1000.0f)) * 3600.0f; // in Sec + } else { + flightBatteryData.EstimatedFlightTime = 9999; + } + + // generate alarms where needed... + if ((flightBatteryData.Voltage <= 0) && (flightBatteryData.Current <= 0)) { + // FIXME: There's no guarantee that a floating ADC will give 0. So this + // check might fail, even when there's nothing attached. + AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR); + } else { + // FIXME: should make the timer alarms user configurable + if (flightBatteryData.EstimatedFlightTime < 30) { + AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL); + } else if (flightBatteryData.EstimatedFlightTime < 120) { + AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME); + } + + // FIXME: should make the battery voltage detection dependent on battery type. + /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/ + if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM]) { + AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); + } else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING]) { + AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); + } + } + + FlightBatteryStateSet(&flightBatteryData); } -/** - * @} - */ - +/** + * @} + */ + /** * @} */ diff --git a/flight/modules/Battery/inc/battery.h b/flight/modules/Battery/inc/battery.h index bc2515696..206a9d92a 100644 --- a/flight/modules/Battery/inc/battery.h +++ b/flight/modules/Battery/inc/battery.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup BatteryModule Battery Module - * @{ + * @{ * * @file battery.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -37,6 +37,6 @@ int32_t BatteryInitialize(void); #endif // BATTERY_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/CallbackTest/callbacktest.c b/flight/modules/CallbackTest/callbacktest.c index 1f5a63b83..0b4ccf474 100644 --- a/flight/modules/CallbackTest/callbacktest.c +++ b/flight/modules/CallbackTest/callbacktest.c @@ -35,20 +35,20 @@ #include "openpilot.h" // Private constants -#define STACK_SIZE configMINIMAL_STACK_SIZE +#define STACK_SIZE configMINIMAL_STACK_SIZE #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW -#define TASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY +#define TASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY // Private types -//#define DEBUGPRINT(...) fprintf (stderr, __VA_ARGS__) -#define DEBUGPRINT(...) xSemaphoreTakeRecursive(mutex, portMAX_DELAY); fprintf(stderr, __VA_ARGS__ ); xSemaphoreGiveRecursive(mutex); +// #define DEBUGPRINT(...) fprintf (stderr, __VA_ARGS__) +#define DEBUGPRINT(...) xSemaphoreTakeRecursive(mutex, portMAX_DELAY); fprintf(stderr, __VA_ARGS__); xSemaphoreGiveRecursive(mutex); static xSemaphoreHandle mutex; // Private variables static DelayedCallbackInfo *cbinfo[10]; -static volatile int32_t counter[10]={0}; +static volatile int32_t counter[10] = { 0 }; static void DelayedCb0(); static void DelayedCb1(); @@ -63,70 +63,88 @@ static void DelayedCb6(); */ int32_t CallbackTestInitialize() { + mutex = xSemaphoreCreateRecursiveMutex(); - mutex = xSemaphoreCreateRecursiveMutex(); + cbinfo[0] = DelayedCallbackCreate(&DelayedCb0, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE); + cbinfo[1] = DelayedCallbackCreate(&DelayedCb1, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE); + cbinfo[2] = DelayedCallbackCreate(&DelayedCb2, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, STACK_SIZE); + cbinfo[3] = DelayedCallbackCreate(&DelayedCb3, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, STACK_SIZE); + cbinfo[4] = DelayedCallbackCreate(&DelayedCb4, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE); + cbinfo[5] = DelayedCallbackCreate(&DelayedCb5, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE); + cbinfo[6] = DelayedCallbackCreate(&DelayedCb6, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 20, STACK_SIZE); - cbinfo[0] = DelayedCallbackCreate(&DelayedCb0,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE); - cbinfo[1] = DelayedCallbackCreate(&DelayedCb1,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE); - cbinfo[2] = DelayedCallbackCreate(&DelayedCb2,CALLBACK_PRIORITY_CRITICAL,tskIDLE_PRIORITY+2,STACK_SIZE); - cbinfo[3] = DelayedCallbackCreate(&DelayedCb3,CALLBACK_PRIORITY_CRITICAL,tskIDLE_PRIORITY+2,STACK_SIZE); - cbinfo[4] = DelayedCallbackCreate(&DelayedCb4,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE); - cbinfo[5] = DelayedCallbackCreate(&DelayedCb5,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE); - cbinfo[6] = DelayedCallbackCreate(&DelayedCb6,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+20,STACK_SIZE); - - return 0; + return 0; } -int32_t CallbackTestStart() { - - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - DelayedCallbackDispatch(cbinfo[3]); - DelayedCallbackDispatch(cbinfo[2]); - DelayedCallbackDispatch(cbinfo[1]); - DelayedCallbackDispatch(cbinfo[0]); - // different callback priorities within a taskpriority - DelayedCallbackSchedule(cbinfo[4],30000,CALLBACK_UPDATEMODE_NONE); - DelayedCallbackSchedule(cbinfo[4],5000,CALLBACK_UPDATEMODE_OVERRIDE); - DelayedCallbackSchedule(cbinfo[4],4000,CALLBACK_UPDATEMODE_SOONER); - DelayedCallbackSchedule(cbinfo[4],10000,CALLBACK_UPDATEMODE_SOONER); - DelayedCallbackSchedule(cbinfo[4],1000,CALLBACK_UPDATEMODE_LATER); - DelayedCallbackSchedule(cbinfo[4],4800,CALLBACK_UPDATEMODE_LATER); - DelayedCallbackSchedule(cbinfo[4],48000,CALLBACK_UPDATEMODE_NONE); - // should be at 4.8 seconds after this, allowing for exactly 9 prints of the following - DelayedCallbackSchedule(cbinfo[5],500,CALLBACK_UPDATEMODE_NONE); - // delayed counter with 500 ms - DelayedCallbackDispatch(cbinfo[6]); - // high task prio - xSemaphoreGiveRecursive(mutex); - return 0; +int32_t CallbackTestStart() +{ + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + DelayedCallbackDispatch(cbinfo[3]); + DelayedCallbackDispatch(cbinfo[2]); + DelayedCallbackDispatch(cbinfo[1]); + DelayedCallbackDispatch(cbinfo[0]); + // different callback priorities within a taskpriority + DelayedCallbackSchedule(cbinfo[4], 30000, CALLBACK_UPDATEMODE_NONE); + DelayedCallbackSchedule(cbinfo[4], 5000, CALLBACK_UPDATEMODE_OVERRIDE); + DelayedCallbackSchedule(cbinfo[4], 4000, CALLBACK_UPDATEMODE_SOONER); + DelayedCallbackSchedule(cbinfo[4], 10000, CALLBACK_UPDATEMODE_SOONER); + DelayedCallbackSchedule(cbinfo[4], 1000, CALLBACK_UPDATEMODE_LATER); + DelayedCallbackSchedule(cbinfo[4], 4800, CALLBACK_UPDATEMODE_LATER); + DelayedCallbackSchedule(cbinfo[4], 48000, CALLBACK_UPDATEMODE_NONE); + // should be at 4.8 seconds after this, allowing for exactly 9 prints of the following + DelayedCallbackSchedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE); + // delayed counter with 500 ms + DelayedCallbackDispatch(cbinfo[6]); + // high task prio + xSemaphoreGiveRecursive(mutex); + return 0; } -static void DelayedCb0() { - DEBUGPRINT("delayed counter low prio 0 updated: %i\n",counter[0]); - if (++counter[0]<10) DelayedCallbackDispatch(cbinfo[0]); +static void DelayedCb0() +{ + DEBUGPRINT("delayed counter low prio 0 updated: %i\n", counter[0]); + if (++counter[0] < 10) { + DelayedCallbackDispatch(cbinfo[0]); + } } -static void DelayedCb1() { - DEBUGPRINT("delayed counter low prio 1 updated: %i\n",counter[1]); - if (++counter[1]<10) DelayedCallbackDispatch(cbinfo[1]); +static void DelayedCb1() +{ + DEBUGPRINT("delayed counter low prio 1 updated: %i\n", counter[1]); + if (++counter[1] < 10) { + DelayedCallbackDispatch(cbinfo[1]); + } } -static void DelayedCb2() { - DEBUGPRINT("delayed counter high prio 2 updated: %i\n",counter[2]); - if (++counter[2]<10) DelayedCallbackDispatch(cbinfo[2]); +static void DelayedCb2() +{ + DEBUGPRINT("delayed counter high prio 2 updated: %i\n", counter[2]); + if (++counter[2] < 10) { + DelayedCallbackDispatch(cbinfo[2]); + } } -static void DelayedCb3() { - DEBUGPRINT("delayed counter high prio 3 updated: %i\n",counter[3]); - if (++counter[3]<10) DelayedCallbackDispatch(cbinfo[3]); +static void DelayedCb3() +{ + DEBUGPRINT("delayed counter high prio 3 updated: %i\n", counter[3]); + if (++counter[3] < 10) { + DelayedCallbackDispatch(cbinfo[3]); + } } -static void DelayedCb4() { - DEBUGPRINT("delayed scheduled callback 4 reached!\n"); - exit(0); +static void DelayedCb4() +{ + DEBUGPRINT("delayed scheduled callback 4 reached!\n"); + exit(0); } -static void DelayedCb5() { - DEBUGPRINT("delayed scheduled counter 5 updated: %i\n",counter[5]); - if (++counter[5]<10) DelayedCallbackSchedule(cbinfo[5],500,CALLBACK_UPDATEMODE_NONE); - // it will likely only reach 8 due to cb4 aborting the run +static void DelayedCb5() +{ + DEBUGPRINT("delayed scheduled counter 5 updated: %i\n", counter[5]); + if (++counter[5] < 10) { + DelayedCallbackSchedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE); + } + // it will likely only reach 8 due to cb4 aborting the run } -static void DelayedCb6() { - DEBUGPRINT("delayed counter 6 (high task prio) updated: %i\n",counter[6]); - if (++counter[6]<10) DelayedCallbackDispatch(cbinfo[6]); +static void DelayedCb6() +{ + DEBUGPRINT("delayed counter 6 (high task prio) updated: %i\n", counter[6]); + if (++counter[6] < 10) { + DelayedCallbackDispatch(cbinfo[6]); + } } diff --git a/flight/modules/CameraStab/camerastab.c b/flight/modules/CameraStab/camerastab.c index 48f7a308f..04350c033 100644 --- a/flight/modules/CameraStab/camerastab.c +++ b/flight/modules/CameraStab/camerastab.c @@ -56,29 +56,28 @@ // // Configuration // -#define SAMPLE_PERIOD_MS 10 +#define SAMPLE_PERIOD_MS 10 // Private types // Private variables static struct CameraStab_data { - portTickType lastSysTime; - float inputs[CAMERASTABSETTINGS_INPUT_NUMELEM]; + portTickType lastSysTime; + float inputs[CAMERASTABSETTINGS_INPUT_NUMELEM]; #ifdef USE_GIMBAL_LPF - float attitudeFiltered[CAMERASTABSETTINGS_INPUT_NUMELEM]; + float attitudeFiltered[CAMERASTABSETTINGS_INPUT_NUMELEM]; #endif #ifdef USE_GIMBAL_FF - float ffLastAttitude[CAMERASTABSETTINGS_INPUT_NUMELEM]; - float ffLastAttitudeFiltered[CAMERASTABSETTINGS_INPUT_NUMELEM]; - float ffFilterAccumulator[CAMERASTABSETTINGS_INPUT_NUMELEM]; + float ffLastAttitude[CAMERASTABSETTINGS_INPUT_NUMELEM]; + float ffLastAttitudeFiltered[CAMERASTABSETTINGS_INPUT_NUMELEM]; + float ffFilterAccumulator[CAMERASTABSETTINGS_INPUT_NUMELEM]; #endif - } *csd; // Private functions -static void attitudeUpdated(UAVObjEvent* ev); +static void attitudeUpdated(UAVObjEvent *ev); static float bound(float val, float limit); #ifdef USE_GIMBAL_FF @@ -92,253 +91,258 @@ static void applyFeedForward(uint8_t index, float dT, float *attitude, CameraSta */ int32_t CameraStabInitialize(void) { - bool cameraStabEnabled; + bool cameraStabEnabled; #ifdef MODULE_CAMERASTAB_BUILTIN - cameraStabEnabled = true; + cameraStabEnabled = true; #else - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsInitialize(); - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsInitialize(); + HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_CAMERASTAB] == HWSETTINGS_OPTIONALMODULES_ENABLED) - cameraStabEnabled = true; - else - cameraStabEnabled = false; + if (optionalModules[HWSETTINGS_OPTIONALMODULES_CAMERASTAB] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + cameraStabEnabled = true; + } else { + cameraStabEnabled = false; + } #endif - if (cameraStabEnabled) { + if (cameraStabEnabled) { + // allocate and initialize the static data storage only if module is enabled + csd = (struct CameraStab_data *)pvPortMalloc(sizeof(struct CameraStab_data)); + if (!csd) { + return -1; + } - // allocate and initialize the static data storage only if module is enabled - csd = (struct CameraStab_data *) pvPortMalloc(sizeof(struct CameraStab_data)); - if (!csd) - return -1; + // initialize camera state variables + memset(csd, 0, sizeof(struct CameraStab_data)); + csd->lastSysTime = xTaskGetTickCount(); - // initialize camera state variables - memset(csd, 0, sizeof(struct CameraStab_data)); - csd->lastSysTime = xTaskGetTickCount(); + AttitudeActualInitialize(); + CameraStabSettingsInitialize(); + CameraDesiredInitialize(); - AttitudeActualInitialize(); - CameraStabSettingsInitialize(); - CameraDesiredInitialize(); + UAVObjEvent ev = { + .obj = AttitudeActualHandle(), + .instId = 0, + .event = 0, + }; + EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS); - UAVObjEvent ev = { - .obj = AttitudeActualHandle(), - .instId = 0, - .event = 0, - }; - EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS); + return 0; + } - return 0; - } - - return -1; + return -1; } /* stub: module has no module thread */ int32_t CameraStabStart(void) { - return 0; + return 0; } MODULE_INITCALL(CameraStabInitialize, CameraStabStart) -static void attitudeUpdated(UAVObjEvent* ev) +static void attitudeUpdated(UAVObjEvent *ev) { - if (ev->obj != AttitudeActualHandle()) - return; + if (ev->obj != AttitudeActualHandle()) { + return; + } - AccessoryDesiredData accessory; + AccessoryDesiredData accessory; - CameraStabSettingsData cameraStab; - CameraStabSettingsGet(&cameraStab); + CameraStabSettingsData cameraStab; + CameraStabSettingsGet(&cameraStab); - // check how long since last update, time delta between calls in ms - portTickType thisSysTime = xTaskGetTickCount(); - float dT_millis = (thisSysTime > csd->lastSysTime) ? - (float)((thisSysTime - csd->lastSysTime) * portTICK_RATE_MS) : - (float)SAMPLE_PERIOD_MS; - csd->lastSysTime = thisSysTime; + // check how long since last update, time delta between calls in ms + portTickType thisSysTime = xTaskGetTickCount(); + float dT_millis = (thisSysTime > csd->lastSysTime) ? + (float)((thisSysTime - csd->lastSysTime) * portTICK_RATE_MS) : + (float)SAMPLE_PERIOD_MS; + csd->lastSysTime = thisSysTime; - // storage for elevon roll component before the pitch component has been generated - // we are guaranteed that the iteration order of i is roll pitch yaw - // that guarnteees this won't be used uninited, but the compiler doesn't know that - // so we init it or turn the warning/error off for each compiler - float elevon_roll = 0.0f; + // storage for elevon roll component before the pitch component has been generated + // we are guaranteed that the iteration order of i is roll pitch yaw + // that guarnteees this won't be used uninited, but the compiler doesn't know that + // so we init it or turn the warning/error off for each compiler + float elevon_roll = 0.0f; - // process axes - for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUT_NUMELEM; i++) { + // process axes + for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUT_NUMELEM; i++) { + // read and process control input + if (cameraStab.Input[i] != CAMERASTABSETTINGS_INPUT_NONE) { + if (AccessoryDesiredInstGet(cameraStab.Input[i] - CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { + float input_rate; + switch (cameraStab.StabilizationMode[i]) { + case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE: + csd->inputs[i] = accessory.AccessoryVal * cameraStab.InputRange[i]; + break; + case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK: + input_rate = accessory.AccessoryVal * cameraStab.InputRate[i]; + if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) { + csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis, cameraStab.InputRange[i]); + } + break; + default: + PIOS_Assert(0); + } + } + } - // read and process control input - if (cameraStab.Input[i] != CAMERASTABSETTINGS_INPUT_NONE) { - if (AccessoryDesiredInstGet(cameraStab.Input[i] - CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { - float input_rate; - switch (cameraStab.StabilizationMode[i]) { - case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE: - csd->inputs[i] = accessory.AccessoryVal * cameraStab.InputRange[i]; - break; - case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK: - input_rate = accessory.AccessoryVal * cameraStab.InputRate[i]; - if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) - csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis, cameraStab.InputRange[i]); - break; - default: - PIOS_Assert(0); - } - } - } + // calculate servo output + float attitude; - // calculate servo output - float attitude; - - switch (i) { - case CAMERASTABSETTINGS_INPUT_ROLL: - AttitudeActualRollGet(&attitude); - break; - case CAMERASTABSETTINGS_INPUT_PITCH: - AttitudeActualPitchGet(&attitude); - break; - case CAMERASTABSETTINGS_INPUT_YAW: - AttitudeActualYawGet(&attitude); - break; - default: - PIOS_Assert(0); - } + switch (i) { + case CAMERASTABSETTINGS_INPUT_ROLL: + AttitudeActualRollGet(&attitude); + break; + case CAMERASTABSETTINGS_INPUT_PITCH: + AttitudeActualPitchGet(&attitude); + break; + case CAMERASTABSETTINGS_INPUT_YAW: + AttitudeActualYawGet(&attitude); + break; + default: + PIOS_Assert(0); + } #ifdef USE_GIMBAL_LPF - if (cameraStab.ResponseTime) { - float rt = (float)cameraStab.ResponseTime[i]; - attitude = csd->attitudeFiltered[i] = ((rt * csd->attitudeFiltered[i]) + (dT_millis * attitude)) / (rt + dT_millis); - } + if (cameraStab.ResponseTime) { + float rt = (float)cameraStab.ResponseTime[i]; + attitude = csd->attitudeFiltered[i] = ((rt * csd->attitudeFiltered[i]) + (dT_millis * attitude)) / (rt + dT_millis); + } #endif #ifdef USE_GIMBAL_FF - if (cameraStab.FeedForward[i]) - applyFeedForward(i, dT_millis, &attitude, &cameraStab); + if (cameraStab.FeedForward[i]) { + applyFeedForward(i, dT_millis, &attitude, &cameraStab); + } #endif - // bounding for elevon mixing occurs on the unmixed output - // to limit the range of the mixed output you must limit the range - // of both the unmixed pitch and unmixed roll - float output = bound((attitude + csd->inputs[i]) / cameraStab.OutputRange[i], 1.0f); + // bounding for elevon mixing occurs on the unmixed output + // to limit the range of the mixed output you must limit the range + // of both the unmixed pitch and unmixed roll + float output = bound((attitude + csd->inputs[i]) / cameraStab.OutputRange[i], 1.0f); - // set output channels - switch (i) { - case CAMERASTABSETTINGS_INPUT_ROLL: - // we are guaranteed that the iteration order of i is roll pitch yaw - // for elevon mixing we simply grab the value for later use - if (cameraStab.GimbalType == CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED) - elevon_roll = output; - else - CameraDesiredRollOrServo1Set(&output); - break; - case CAMERASTABSETTINGS_INPUT_PITCH: - // we are guaranteed that the iteration order of i is roll pitch yaw - // for elevon mixing we use the value we previously grabbed and set both s1 and s2 - if (cameraStab.GimbalType == CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED) { - float elevon_pitch = output; - // elevon reversing works like this: - // first use the normal reversing facilities to get servo 1 roll working in the correct direction - // then use the normal reversing facilities to get servo 2 roll working in the correct direction - // then use these new reversing switches to reverse servo 1 and/or 2 pitch as needed - // if servo 1 pitch is reversed - if (cameraStab.Servo1PitchReverse == CAMERASTABSETTINGS_SERVO1PITCHREVERSE_TRUE) { - // use (reversed pitch) + roll - output = ((1.0f - elevon_pitch) + elevon_roll) / 2.0f; - } else { - // use pitch + roll - output = (elevon_pitch + elevon_roll) / 2.0f; - } - CameraDesiredRollOrServo1Set(&output); - // if servo 2 pitch is reversed - if (cameraStab.Servo2PitchReverse == CAMERASTABSETTINGS_SERVO2PITCHREVERSE_TRUE) { - // use (reversed pitch) - roll - output = ((1.0f - elevon_pitch) - elevon_roll) / 2.0f; - } else { - // use pitch - roll - output = (elevon_pitch - elevon_roll) / 2.0f; - } - CameraDesiredPitchOrServo2Set(&output); - } else { - CameraDesiredPitchOrServo2Set(&output); - } - break; - case CAMERASTABSETTINGS_INPUT_YAW: - CameraDesiredYawSet(&output); - break; - default: - PIOS_Assert(0); - } - } + // set output channels + switch (i) { + case CAMERASTABSETTINGS_INPUT_ROLL: + // we are guaranteed that the iteration order of i is roll pitch yaw + // for elevon mixing we simply grab the value for later use + if (cameraStab.GimbalType == CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED) { + elevon_roll = output; + } else { + CameraDesiredRollOrServo1Set(&output); + } + break; + case CAMERASTABSETTINGS_INPUT_PITCH: + // we are guaranteed that the iteration order of i is roll pitch yaw + // for elevon mixing we use the value we previously grabbed and set both s1 and s2 + if (cameraStab.GimbalType == CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED) { + float elevon_pitch = output; + // elevon reversing works like this: + // first use the normal reversing facilities to get servo 1 roll working in the correct direction + // then use the normal reversing facilities to get servo 2 roll working in the correct direction + // then use these new reversing switches to reverse servo 1 and/or 2 pitch as needed + // if servo 1 pitch is reversed + if (cameraStab.Servo1PitchReverse == CAMERASTABSETTINGS_SERVO1PITCHREVERSE_TRUE) { + // use (reversed pitch) + roll + output = ((1.0f - elevon_pitch) + elevon_roll) / 2.0f; + } else { + // use pitch + roll + output = (elevon_pitch + elevon_roll) / 2.0f; + } + CameraDesiredRollOrServo1Set(&output); + // if servo 2 pitch is reversed + if (cameraStab.Servo2PitchReverse == CAMERASTABSETTINGS_SERVO2PITCHREVERSE_TRUE) { + // use (reversed pitch) - roll + output = ((1.0f - elevon_pitch) - elevon_roll) / 2.0f; + } else { + // use pitch - roll + output = (elevon_pitch - elevon_roll) / 2.0f; + } + CameraDesiredPitchOrServo2Set(&output); + } else { + CameraDesiredPitchOrServo2Set(&output); + } + break; + case CAMERASTABSETTINGS_INPUT_YAW: + CameraDesiredYawSet(&output); + break; + default: + PIOS_Assert(0); + } + } } float bound(float val, float limit) { - return (val > limit) ? limit : - (val < -limit) ? -limit : - val; + return (val > limit) ? limit : + (val < -limit) ? -limit : + val; } #ifdef USE_GIMBAL_FF void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraStabSettingsData *cameraStab) { - // compensate high feed forward values depending on gimbal type - float gimbalTypeCorrection = 1.0f; + // compensate high feed forward values depending on gimbal type + float gimbalTypeCorrection = 1.0f; - switch (cameraStab->GimbalType) { - case CAMERASTABSETTINGS_GIMBALTYPE_GENERIC: - case CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED: - // no correction - break; - case CAMERASTABSETTINGS_GIMBALTYPE_YAWROLLPITCH: - if (index == CAMERASTABSETTINGS_INPUT_ROLL) { - float pitch; - AttitudeActualPitchGet(&pitch); - gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabsf(pitch)) - / cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]; - } - break; - case CAMERASTABSETTINGS_GIMBALTYPE_YAWPITCHROLL: - if (index == CAMERASTABSETTINGS_INPUT_PITCH) { - float roll; - AttitudeActualRollGet(&roll); - gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabsf(roll)) - / cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]; - } - break; - default: - PIOS_Assert(0); - } + switch (cameraStab->GimbalType) { + case CAMERASTABSETTINGS_GIMBALTYPE_GENERIC: + case CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED: + // no correction + break; + case CAMERASTABSETTINGS_GIMBALTYPE_YAWROLLPITCH: + if (index == CAMERASTABSETTINGS_INPUT_ROLL) { + float pitch; + AttitudeActualPitchGet(&pitch); + gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabsf(pitch)) + / cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]; + } + break; + case CAMERASTABSETTINGS_GIMBALTYPE_YAWPITCHROLL: + if (index == CAMERASTABSETTINGS_INPUT_PITCH) { + float roll; + AttitudeActualRollGet(&roll); + gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabsf(roll)) + / cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]; + } + break; + default: + PIOS_Assert(0); + } - // apply feed forward - float accumulator = csd->ffFilterAccumulator[index]; - accumulator += (*attitude - csd->ffLastAttitude[index]) * (float)cameraStab->FeedForward[index] * gimbalTypeCorrection; - csd->ffLastAttitude[index] = *attitude; - *attitude += accumulator; + // apply feed forward + float accumulator = csd->ffFilterAccumulator[index]; + accumulator += (*attitude - csd->ffLastAttitude[index]) * (float)cameraStab->FeedForward[index] * gimbalTypeCorrection; + csd->ffLastAttitude[index] = *attitude; + *attitude += accumulator; - float filter = (float)((accumulator > 0.0f) ? cameraStab->AccelTime[index] : cameraStab->DecelTime[index]) / dT_millis; - if (filter < 1.0f) - filter = 1.0f; - accumulator -= accumulator / filter; - csd->ffFilterAccumulator[index] = accumulator; - *attitude += accumulator; + float filter = (float)((accumulator > 0.0f) ? cameraStab->AccelTime[index] : cameraStab->DecelTime[index]) / dT_millis; + if (filter < 1.0f) { + filter = 1.0f; + } + accumulator -= accumulator / filter; + csd->ffFilterAccumulator[index] = accumulator; + *attitude += accumulator; - // apply acceleration limit - float delta = *attitude - csd->ffLastAttitudeFiltered[index]; - float maxDelta = (float)cameraStab->MaxAccel * 0.001f * dT_millis; + // apply acceleration limit + float delta = *attitude - csd->ffLastAttitudeFiltered[index]; + float maxDelta = (float)cameraStab->MaxAccel * 0.001f * dT_millis; - if (fabsf(delta) > maxDelta) { - // we are accelerating too hard - *attitude = csd->ffLastAttitudeFiltered[index] + ((delta > 0.0f) ? maxDelta : -maxDelta); - } - csd->ffLastAttitudeFiltered[index] = *attitude; + if (fabsf(delta) > maxDelta) { + // we are accelerating too hard + *attitude = csd->ffLastAttitudeFiltered[index] + ((delta > 0.0f) ? maxDelta : -maxDelta); + } + csd->ffLastAttitudeFiltered[index] = *attitude; } #endif // USE_GIMBAL_FF /** - * @} - */ + * @} + */ /** * @} diff --git a/flight/modules/CameraStab/inc/camerastab.h b/flight/modules/CameraStab/inc/camerastab.h index 5d7bf9d06..fc3614044 100644 --- a/flight/modules/CameraStab/inc/camerastab.h +++ b/flight/modules/CameraStab/inc/camerastab.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup BatteryModule Battery Module - * @{ + * @{ * * @file battery.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -37,6 +37,6 @@ int32_t CameraStabInitialize(void); #endif // BATTERY_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/ComUsbBridge/ComUsbBridge.c b/flight/modules/ComUsbBridge/ComUsbBridge.c index 84c4e5ec7..09425e008 100644 --- a/flight/modules/ComUsbBridge/ComUsbBridge.c +++ b/flight/modules/ComUsbBridge/ComUsbBridge.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup ComUsbBridgeModule Com Port to USB VCP Bridge Module * @brief Bridge Com and USB VCP ports - * @{ + * @{ * * @file ComUsbBridge.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. @@ -47,10 +47,10 @@ static void updateSettings(); // **************** // Private constants -#define STACK_SIZE_BYTES 280 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define STACK_SIZE_BYTES 280 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define BRIDGE_BUF_LEN 10 +#define BRIDGE_BUF_LEN 10 // **************** // Private variables @@ -58,8 +58,8 @@ static void updateSettings(); static xTaskHandle com2UsbBridgeTaskHandle; static xTaskHandle usb2ComBridgeTaskHandle; -static uint8_t * com2usb_buf; -static uint8_t * usb2com_buf; +static uint8_t *com2usb_buf; +static uint8_t *usb2com_buf; static uint32_t usart_port; static uint32_t vcp_port; @@ -74,16 +74,16 @@ static bool bridge_enabled = false; static int32_t comUsbBridgeStart(void) { - if (bridge_enabled) { - // Start tasks - xTaskCreate(com2UsbBridgeTask, (signed char *)"Com2UsbBridge", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &com2UsbBridgeTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_COM2USBBRIDGE, com2UsbBridgeTaskHandle); - xTaskCreate(usb2ComBridgeTask, (signed char *)"Usb2ComBridge", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &usb2ComBridgeTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_USB2COMBRIDGE, usb2ComBridgeTaskHandle); - return 0; - } + if (bridge_enabled) { + // Start tasks + xTaskCreate(com2UsbBridgeTask, (signed char *)"Com2UsbBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &com2UsbBridgeTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_COM2USBBRIDGE, com2UsbBridgeTaskHandle); + xTaskCreate(usb2ComBridgeTask, (signed char *)"Usb2ComBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &usb2ComBridgeTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_USB2COMBRIDGE, usb2ComBridgeTaskHandle); + return 0; + } - return -1; + return -1; } /** * Initialise the module @@ -92,35 +92,36 @@ static int32_t comUsbBridgeStart(void) */ static int32_t comUsbBridgeInitialize(void) { - // TODO: Get from settings object - usart_port = PIOS_COM_BRIDGE; - vcp_port = PIOS_COM_VCP; + // TODO: Get from settings object + usart_port = PIOS_COM_BRIDGE; + vcp_port = PIOS_COM_VCP; #ifdef MODULE_COMUSBBRIDGE_BUILTIN - bridge_enabled = true; + bridge_enabled = true; #else - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(optionalModules); - if (usart_port && vcp_port && - (optionalModules[HWSETTINGS_OPTIONALMODULES_COMUSBBRIDGE] == HWSETTINGS_OPTIONALMODULES_ENABLED)) - bridge_enabled = true; - else - bridge_enabled = false; + if (usart_port && vcp_port && + (optionalModules[HWSETTINGS_OPTIONALMODULES_COMUSBBRIDGE] == HWSETTINGS_OPTIONALMODULES_ENABLED)) { + bridge_enabled = true; + } else { + bridge_enabled = false; + } #endif - if (bridge_enabled) { - com2usb_buf = pvPortMalloc(BRIDGE_BUF_LEN); - PIOS_Assert(com2usb_buf); - usb2com_buf = pvPortMalloc(BRIDGE_BUF_LEN); - PIOS_Assert(usb2com_buf); + if (bridge_enabled) { + com2usb_buf = pvPortMalloc(BRIDGE_BUF_LEN); + PIOS_Assert(com2usb_buf); + usb2com_buf = pvPortMalloc(BRIDGE_BUF_LEN); + PIOS_Assert(usb2com_buf); - updateSettings(); - } + updateSettings(); + } - return 0; + return 0; } MODULE_INITCALL(comUsbBridgeInitialize, comUsbBridgeStart) @@ -130,72 +131,73 @@ MODULE_INITCALL(comUsbBridgeInitialize, comUsbBridgeStart) static void com2UsbBridgeTask(__attribute__((unused)) void *parameters) { - /* Handle usart -> vcp direction */ - volatile uint32_t tx_errors = 0; - while (1) { - uint32_t rx_bytes; + /* Handle usart -> vcp direction */ + volatile uint32_t tx_errors = 0; - rx_bytes = PIOS_COM_ReceiveBuffer(usart_port, com2usb_buf, BRIDGE_BUF_LEN, 500); - if (rx_bytes > 0) { - /* Bytes available to transfer */ - if (PIOS_COM_SendBuffer(vcp_port, com2usb_buf, rx_bytes) != (int32_t)rx_bytes) { - /* Error on transmit */ - tx_errors++; - } - } - } + while (1) { + uint32_t rx_bytes; + + rx_bytes = PIOS_COM_ReceiveBuffer(usart_port, com2usb_buf, BRIDGE_BUF_LEN, 500); + if (rx_bytes > 0) { + /* Bytes available to transfer */ + if (PIOS_COM_SendBuffer(vcp_port, com2usb_buf, rx_bytes) != (int32_t)rx_bytes) { + /* Error on transmit */ + tx_errors++; + } + } + } } -static void usb2ComBridgeTask(__attribute__((unused)) void * parameters) +static void usb2ComBridgeTask(__attribute__((unused)) void *parameters) { - /* Handle vcp -> usart direction */ - volatile uint32_t tx_errors = 0; - while (1) { - uint32_t rx_bytes; + /* Handle vcp -> usart direction */ + volatile uint32_t tx_errors = 0; - rx_bytes = PIOS_COM_ReceiveBuffer(vcp_port, usb2com_buf, BRIDGE_BUF_LEN, 500); - if (rx_bytes > 0) { - /* Bytes available to transfer */ - if (PIOS_COM_SendBuffer(usart_port, usb2com_buf, rx_bytes) != (int32_t)rx_bytes) { - /* Error on transmit */ - tx_errors++; - } - } - } + while (1) { + uint32_t rx_bytes; + + rx_bytes = PIOS_COM_ReceiveBuffer(vcp_port, usb2com_buf, BRIDGE_BUF_LEN, 500); + if (rx_bytes > 0) { + /* Bytes available to transfer */ + if (PIOS_COM_SendBuffer(usart_port, usb2com_buf, rx_bytes) != (int32_t)rx_bytes) { + /* Error on transmit */ + tx_errors++; + } + } + } } static void updateSettings() { - if (usart_port) { + if (usart_port) { + // Retrieve settings + uint8_t speed; + HwSettingsComUsbBridgeSpeedGet(&speed); - // Retrieve settings - uint8_t speed; - HwSettingsComUsbBridgeSpeedGet(&speed); - - // Set port speed - switch (speed) { - case HWSETTINGS_COMUSBBRIDGESPEED_2400: - PIOS_COM_ChangeBaud(usart_port, 2400); - break; - case HWSETTINGS_COMUSBBRIDGESPEED_4800: - PIOS_COM_ChangeBaud(usart_port, 4800); - break; - case HWSETTINGS_COMUSBBRIDGESPEED_9600: - PIOS_COM_ChangeBaud(usart_port, 9600); - break; - case HWSETTINGS_COMUSBBRIDGESPEED_19200: - PIOS_COM_ChangeBaud(usart_port, 19200); - break; - case HWSETTINGS_COMUSBBRIDGESPEED_38400: - PIOS_COM_ChangeBaud(usart_port, 38400); - break; - case HWSETTINGS_COMUSBBRIDGESPEED_57600: - PIOS_COM_ChangeBaud(usart_port, 57600); - break; - case HWSETTINGS_COMUSBBRIDGESPEED_115200: - PIOS_COM_ChangeBaud(usart_port, 115200); - break; - } - } + // Set port speed + switch (speed) { + case HWSETTINGS_COMUSBBRIDGESPEED_2400: + PIOS_COM_ChangeBaud(usart_port, 2400); + break; + case HWSETTINGS_COMUSBBRIDGESPEED_4800: + PIOS_COM_ChangeBaud(usart_port, 4800); + break; + case HWSETTINGS_COMUSBBRIDGESPEED_9600: + PIOS_COM_ChangeBaud(usart_port, 9600); + break; + case HWSETTINGS_COMUSBBRIDGESPEED_19200: + PIOS_COM_ChangeBaud(usart_port, 19200); + break; + case HWSETTINGS_COMUSBBRIDGESPEED_38400: + PIOS_COM_ChangeBaud(usart_port, 38400); + break; + case HWSETTINGS_COMUSBBRIDGESPEED_57600: + PIOS_COM_ChangeBaud(usart_port, 57600); + break; + case HWSETTINGS_COMUSBBRIDGESPEED_115200: + PIOS_COM_ChangeBaud(usart_port, 115200); + break; + } + } } diff --git a/flight/modules/Example/example.c b/flight/modules/Example/example.c index d50809415..13bcaf539 100644 --- a/flight/modules/Example/example.c +++ b/flight/modules/Example/example.c @@ -51,12 +51,12 @@ void ExampleStart(void) { - ExampleModPeriodicInitialize(); - ExampleModThreadInitialize(); + ExampleModPeriodicInitialize(); + ExampleModThreadInitialize(); } void ExampleInitialize(void) { - ExampleModEventInitialize(); + ExampleModEventInitialize(); } MODULE_INITCALL(ExampleInitialize, ExampleStart) diff --git a/flight/modules/Example/examplemodcallback.c b/flight/modules/Example/examplemodcallback.c index 6faba152f..349b80934 100644 --- a/flight/modules/Example/examplemodcallback.c +++ b/flight/modules/Example/examplemodcallback.c @@ -45,14 +45,14 @@ */ #include "openpilot.h" -#include "exampleobject1.h" // object the module will listen for updates (input) -#include "exampleobject2.h" // object the module will update (output) -#include "examplesettings.h" // object holding module settings (input) +#include "exampleobject1.h" // object the module will listen for updates (input) +#include "exampleobject2.h" // object the module will update (output) +#include "examplesettings.h" // object holding module settings (input) // Private constants -#define STACK_SIZE configMINIMAL_STACK_SIZE +#define STACK_SIZE configMINIMAL_STACK_SIZE #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW -#define CBTASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY +#define CBTASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY // Private types // Private variables @@ -68,12 +68,12 @@ static void DelayedCb(); */ int32_t ExampleModCallbackInitialize() { - // Listen for ExampleObject1 updates, connect a callback function - ExampleObject1ConnectCallback(&ObjectUpdatedCb); + // Listen for ExampleObject1 updates, connect a callback function + ExampleObject1ConnectCallback(&ObjectUpdatedCb); - cbinfo = DelayedCallbackCreate(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE); - - return 0; + cbinfo = DelayedCallbackCreate(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE); + + return 0; } /** @@ -85,7 +85,7 @@ int32_t ExampleModCallbackInitialize() */ static void ObjectUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - DelayedCallbackDispatch(cbinfo); + DelayedCallbackDispatch(cbinfo); } /** @@ -101,40 +101,39 @@ static void ObjectUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) * do not block regular threads. They are therefore saver to use. */ static void DelayedCb(); - ExampleSettingsData settings; - ExampleObject1Data data1; - ExampleObject2Data data2; - int32_t step; +ExampleSettingsData settings; +ExampleObject1Data data1; +ExampleObject2Data data2; +int32_t step; - // Update settings with latest value - ExampleSettingsGet(&settings); +// Update settings with latest value +ExampleSettingsGet(&settings); - // Get the input object - ExampleObject1Get(&data1); +// Get the input object +ExampleObject1Get(&data1); - // Determine how to update the output object - if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { - step = settings.StepSize; - } else { - step = -settings.StepSize; - } - - // Update data - data2.Field1 = data1.Field1 + step; - data2.Field2 = data1.Field2 + step; - data2.Field3 = data1.Field3 + step; - data2.Field4[0] = data1.Field4[0] + step; - data2.Field4[1] = data1.Field4[1] + step; - - // Update the ExampleObject2, after this function is called - // notifications to any other modules listening to that object - // will be sent and the GCS object will be updated through the - // telemetry link. All operations will take place asynchronously - // and the following call will return immediately. - ExampleObject2Set(&data2); - - //call the module again 10 seconds later, - //even if the exampleobject has not been updated - DelayedCallbackSchedule(cbinfo, 10*1000, CALLBACK_UPDATEMODE_NONE); - +// Determine how to update the output object +if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { + step = settings.StepSize; +} else { + step = -settings.StepSize; +} + +// Update data +data2.Field1 = data1.Field1 + step; +data2.Field2 = data1.Field2 + step; +data2.Field3 = data1.Field3 + step; +data2.Field4[0] = data1.Field4[0] + step; +data2.Field4[1] = data1.Field4[1] + step; + +// Update the ExampleObject2, after this function is called +// notifications to any other modules listening to that object +// will be sent and the GCS object will be updated through the +// telemetry link. All operations will take place asynchronously +// and the following call will return immediately. +ExampleObject2Set(&data2); + +// call the module again 10 seconds later, +// even if the exampleobject has not been updated +DelayedCallbackSchedule(cbinfo, 10 * 1000, CALLBACK_UPDATEMODE_NONE); } diff --git a/flight/modules/Example/examplemodevent.c b/flight/modules/Example/examplemodevent.c index 5af686abf..f38bf294c 100644 --- a/flight/modules/Example/examplemodevent.c +++ b/flight/modules/Example/examplemodevent.c @@ -46,9 +46,9 @@ #include "openpilot.h" #include "examplemodevent.h" -#include "exampleobject1.h" // object the module will listen for updates (input) -#include "exampleobject2.h" // object the module will update (output) -#include "examplesettings.h" // object holding module settings (input) +#include "exampleobject1.h" // object the module will listen for updates (input) +#include "exampleobject2.h" // object the module will update (output) +#include "examplesettings.h" // object holding module settings (input) // Private constants @@ -57,7 +57,7 @@ // Private variables // Private functions -static void ObjectUpdatedCb(UAVObjEvent * ev); +static void ObjectUpdatedCb(UAVObjEvent *ev); /** * Initialise the module, called on startup. @@ -65,10 +65,10 @@ static void ObjectUpdatedCb(UAVObjEvent * ev); */ int32_t ExampleModEventInitialize() { - // Listen for ExampleObject1 updates, connect a callback function - ExampleObject1ConnectCallback(&ObjectUpdatedCb); + // Listen for ExampleObject1 updates, connect a callback function + ExampleObject1ConnectCallback(&ObjectUpdatedCb); - return 0; + return 0; } /** @@ -81,43 +81,43 @@ int32_t ExampleModEventInitialize() * important since all callbacks are executed from the same thread, so other * queued events can not be executed until the currently active callback returns. */ -static void ObjectUpdatedCb(UAVObjEvent * ev) +static void ObjectUpdatedCb(UAVObjEvent *ev) { - ExampleSettingsData settings; - ExampleObject1Data data1; - ExampleObject2Data data2; - int32_t step; + ExampleSettingsData settings; + ExampleObject1Data data1; + ExampleObject2Data data2; + int32_t step; - // Make sure that the object update is for ExampleObject1 - // This is redundant in this case since this callback will - // only be called for a single object, it is however possible - // to use the same callback for multiple object updates. - if (ev->obj == ExampleObject1Handle()) { - // Update settings with latest value - ExampleSettingsGet(&settings); + // Make sure that the object update is for ExampleObject1 + // This is redundant in this case since this callback will + // only be called for a single object, it is however possible + // to use the same callback for multiple object updates. + if (ev->obj == ExampleObject1Handle()) { + // Update settings with latest value + ExampleSettingsGet(&settings); - // Get the input object - ExampleObject1Get(&data1); + // Get the input object + ExampleObject1Get(&data1); - // Determine how to update the output object - if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { - step = settings.StepSize; - } else { - step = -settings.StepSize; - } + // Determine how to update the output object + if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { + step = settings.StepSize; + } else { + step = -settings.StepSize; + } - // Update data - data2.Field1 = data1.Field1 + step; - data2.Field2 = data1.Field2 + step; - data2.Field3 = data1.Field3 + step; - data2.Field4[0] = data1.Field4[0] + step; - data2.Field4[1] = data1.Field4[1] + step; + // Update data + data2.Field1 = data1.Field1 + step; + data2.Field2 = data1.Field2 + step; + data2.Field3 = data1.Field3 + step; + data2.Field4[0] = data1.Field4[0] + step; + data2.Field4[1] = data1.Field4[1] + step; - // Update the ExampleObject2, after this function is called - // notifications to any other modules listening to that object - // will be sent and the GCS object will be updated through the - // telemetry link. All operations will take place asynchronously - // and the following call will return immediately. - ExampleObject2Set(&data2); - } + // Update the ExampleObject2, after this function is called + // notifications to any other modules listening to that object + // will be sent and the GCS object will be updated through the + // telemetry link. All operations will take place asynchronously + // and the following call will return immediately. + ExampleObject2Set(&data2); + } } diff --git a/flight/modules/Example/examplemodperiodic.c b/flight/modules/Example/examplemodperiodic.c index 8bfe98211..4a45c71c1 100644 --- a/flight/modules/Example/examplemodperiodic.c +++ b/flight/modules/Example/examplemodperiodic.c @@ -46,12 +46,12 @@ #include "openpilot.h" #include "examplemodperiodic.h" -#include "exampleobject2.h" // object that will be updated by the module -#include "examplesettings.h" // object holding module settings +#include "exampleobject2.h" // object that will be updated by the module +#include "examplesettings.h" // object holding module settings // Private constants -#define STACK_SIZE configMINIMAL_STACK_SIZE -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) +#define STACK_SIZE configMINIMAL_STACK_SIZE +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types @@ -67,10 +67,10 @@ static void exampleTask(void *parameters); */ int32_t ExampleModPeriodicInitialize() { - // Start main task - xTaskCreate(exampleTask, (signed char *)"ExamplePeriodic", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); + // Start main task + xTaskCreate(exampleTask, (signed char *)"ExamplePeriodic", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); - return 0; + return 0; } /** @@ -78,45 +78,45 @@ int32_t ExampleModPeriodicInitialize() */ static void exampleTask(__attribute__((unused)) void *parameters) { - ExampleSettingsData settings; - ExampleObject2Data data; - int32_t step; - portTickType lastSysTime; + ExampleSettingsData settings; + ExampleObject2Data data; + int32_t step; + portTickType lastSysTime; - // Main task loop - lastSysTime = xTaskGetTickCount(); - while (1) { - // Update settings with latest value - ExampleSettingsGet(&settings); + // Main task loop + lastSysTime = xTaskGetTickCount(); + while (1) { + // Update settings with latest value + ExampleSettingsGet(&settings); - // Get the object data - ExampleObject2Get(&data); + // Get the object data + ExampleObject2Get(&data); - // Determine how to update the data - if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { - step = settings.StepSize; - } else { - step = -settings.StepSize; - } + // Determine how to update the data + if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { + step = settings.StepSize; + } else { + step = -settings.StepSize; + } - // Update the data - data.Field1 += step; - data.Field2 += step; - data.Field3 += step; - data.Field4[0] += step; - data.Field4[1] += step; + // Update the data + data.Field1 += step; + data.Field2 += step; + data.Field3 += step; + data.Field4[0] += step; + data.Field4[1] += step; - // Update the ExampleObject, after this function is called - // notifications to any other modules listening to that object - // will be sent and the GCS object will be updated through the - // telemetry link. All operations will take place asynchronously - // and the following call will return immediately. - ExampleObject2Set(&data); + // Update the ExampleObject, after this function is called + // notifications to any other modules listening to that object + // will be sent and the GCS object will be updated through the + // telemetry link. All operations will take place asynchronously + // and the following call will return immediately. + ExampleObject2Set(&data); - // Since this module executes at fixed time intervals, we need to - // block the task until it is time for the next update. - // The settings field is in ms, to convert to RTOS ticks we need - // to divide by portTICK_RATE_MS. - vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS); - } + // Since this module executes at fixed time intervals, we need to + // block the task until it is time for the next update. + // The settings field is in ms, to convert to RTOS ticks we need + // to divide by portTICK_RATE_MS. + vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS); + } } diff --git a/flight/modules/Example/examplemodthread.c b/flight/modules/Example/examplemodthread.c index 813a437e4..d0f49dfd6 100644 --- a/flight/modules/Example/examplemodthread.c +++ b/flight/modules/Example/examplemodthread.c @@ -45,14 +45,14 @@ #include "openpilot.h" #include "examplemodthread.h" -#include "exampleobject1.h" // object the module will listen for updates (input) -#include "exampleobject2.h" // object the module will update (output) -#include "examplesettings.h" // object holding module settings (input) +#include "exampleobject1.h" // object the module will listen for updates (input) +#include "exampleobject2.h" // object the module will update (output) +#include "examplesettings.h" // object holding module settings (input) // Private constants #define MAX_QUEUE_SIZE 20 -#define STACK_SIZE configMINIMAL_STACK_SIZE -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) +#define STACK_SIZE configMINIMAL_STACK_SIZE +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types @@ -69,16 +69,16 @@ static void exampleTask(void *parameters); */ int32_t ExampleModThreadInitialize() { - // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + // Create object queue + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - // Listen for ExampleObject1 updates - ExampleObject1ConnectQueue(queue); + // Listen for ExampleObject1 updates + ExampleObject1ConnectQueue(queue); - // Start main task - xTaskCreate(exampleTask, (signed char *)"ExampleThread", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); + // Start main task + xTaskCreate(exampleTask, (signed char *)"ExampleThread", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); - return 0; + return 0; } /** @@ -86,53 +86,55 @@ int32_t ExampleModThreadInitialize() */ static void exampleTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; - ExampleSettingsData settings; - ExampleObject1Data data1; - ExampleObject1Data data2; - int32_t step; + UAVObjEvent ev; + ExampleSettingsData settings; + ExampleObject1Data data1; + ExampleObject1Data data2; + int32_t step; - // Main task loop - while (1) { - // Check the event queue for any object updates. In this case - // we are only listening for the ExampleSettings object. However - // the module could listen to multiple objects. - // Since this object only executes on object updates, the task - // will block until an object event is pushed in the queue. - while (xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE) ; + // Main task loop + while (1) { + // Check the event queue for any object updates. In this case + // we are only listening for the ExampleSettings object. However + // the module could listen to multiple objects. + // Since this object only executes on object updates, the task + // will block until an object event is pushed in the queue. + while (xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE) { + ; + } - // Make sure that the object update is for ExampleObject1 - // This is redundant in this example since we have only - // registered a single object in the queue, however - // in most practical cases there will be more than one - // object connected in the queue. - if (ev.obj == ExampleObject1Handle()) { - // Update settings with latest value - ExampleSettingsGet(&settings); + // Make sure that the object update is for ExampleObject1 + // This is redundant in this example since we have only + // registered a single object in the queue, however + // in most practical cases there will be more than one + // object connected in the queue. + if (ev.obj == ExampleObject1Handle()) { + // Update settings with latest value + ExampleSettingsGet(&settings); - // Get the input object - ExampleObject1Get(&data1); + // Get the input object + ExampleObject1Get(&data1); - // Determine how to update the output object - if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { - step = settings.StepSize; - } else { - step = -settings.StepSize; - } + // Determine how to update the output object + if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { + step = settings.StepSize; + } else { + step = -settings.StepSize; + } - // Update data - data2.Field1 = data1.Field1 + step; - data2.Field2 = data1.Field2 + step; - data2.Field3 = data1.Field3 + step; - data2.Field4[0] = data1.Field4[0] + step; - data2.Field4[1] = data1.Field4[1] + step; + // Update data + data2.Field1 = data1.Field1 + step; + data2.Field2 = data1.Field2 + step; + data2.Field3 = data1.Field3 + step; + data2.Field4[0] = data1.Field4[0] + step; + data2.Field4[1] = data1.Field4[1] + step; - // Update the ExampleObject2, after this function is called - // notifications to any other modules listening to that object - // will be sent and the GCS object will be updated through the - // telemetry link. All operations will take place asynchronously - // and the following call will return immediately. - ExampleObject2Set(&data2); - } - } + // Update the ExampleObject2, after this function is called + // notifications to any other modules listening to that object + // will be sent and the GCS object will be updated through the + // telemetry link. All operations will take place asynchronously + // and the following call will return immediately. + ExampleObject2Set(&data2); + } + } } diff --git a/flight/modules/Example/inc/examplemodperiodic.h b/flight/modules/Example/inc/examplemodperiodic.h index 330d02ec1..8e877edca 100644 --- a/flight/modules/Example/inc/examplemodperiodic.h +++ b/flight/modules/Example/inc/examplemodperiodic.h @@ -29,4 +29,3 @@ int32_t ExampleModPeriodicInitialize(); #endif // EXAMPLEMODPERIODIC_H - diff --git a/flight/modules/Example/inc/examplemodthread.h b/flight/modules/Example/inc/examplemodthread.h index a3afba343..5cf4c0952 100644 --- a/flight/modules/Example/inc/examplemodthread.h +++ b/flight/modules/Example/inc/examplemodthread.h @@ -29,4 +29,3 @@ int32_t ExampleModThreadInitialize(); #endif // EXAMPLEMODTHREAD_H - diff --git a/flight/modules/Extensions/MagBaro/inc/magbaro.h b/flight/modules/Extensions/MagBaro/inc/magbaro.h index 14c8da6d5..ad495fb1a 100644 --- a/flight/modules/Extensions/MagBaro/inc/magbaro.h +++ b/flight/modules/Extensions/MagBaro/inc/magbaro.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AltitudeModule Altitude Module * @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object" - * @{ + * @{ * * @file altitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. diff --git a/flight/modules/Extensions/MagBaro/magbaro.c b/flight/modules/Extensions/MagBaro/magbaro.c index 81b8a722b..cc3835c54 100644 --- a/flight/modules/Extensions/MagBaro/magbaro.c +++ b/flight/modules/Extensions/MagBaro/magbaro.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AltitudeModule Altitude Module * @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object" - * @{ + * @{ * * @file altitude.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -40,14 +40,14 @@ #include "hwsettings.h" #include "magbaro.h" -#include "baroaltitude.h" // object that will be updated by the module +#include "baroaltitude.h" // object that will be updated by the module #include "magnetometer.h" #include "taskinfo.h" // Private constants #define STACK_SIZE_BYTES 600 -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) -#define UPDATE_PERIOD 100 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define UPDATE_PERIOD 100 // Private types @@ -58,19 +58,19 @@ static xTaskHandle taskHandle; static bool magbaroEnabled; #if defined(PIOS_INCLUDE_BMP085) -#define alt_ds_size 4 +#define alt_ds_size 4 static int32_t alt_ds_temp = 0; static int32_t alt_ds_pres = 0; -static int alt_ds_count = 0; +static int alt_ds_count = 0; #endif #if defined(PIOS_INCLUDE_HMC5883) int32_t mag_test; -static float mag_bias[3] = {0,0,0}; -static float mag_scale[3] = {1,1,1}; +static float mag_bias[3] = { 0, 0, 0 }; +static float mag_scale[3] = { 1, 1, 1 }; #endif - // Private functions +// Private functions static void magbaroTask(void *parameters); /** @@ -79,14 +79,13 @@ static void magbaroTask(void *parameters); */ int32_t MagBaroStart() { - - if (magbaroEnabled) { - // Start main task - xTaskCreate(magbaroTask, (signed char *)"MagBaro", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MAGBARO, taskHandle); - return 0; - } - return -1; + if (magbaroEnabled) { + // Start main task + xTaskCreate(magbaroTask, (signed char *)"MagBaro", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MAGBARO, taskHandle); + return 0; + } + return -1; } /** @@ -96,34 +95,33 @@ int32_t MagBaroStart() int32_t MagBaroInitialize() { #ifdef MODULE_MAGBARO_BUILTIN - magbaroEnabled = 1; + magbaroEnabled = 1; #else - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_MAGBARO] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - magbaroEnabled = 1; - } else { - magbaroEnabled = 0; - } + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + if (optionalModules[HWSETTINGS_OPTIONALMODULES_MAGBARO] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + magbaroEnabled = 1; + } else { + magbaroEnabled = 0; + } #endif - if(magbaroEnabled) - { + if (magbaroEnabled) { #if defined(PIOS_INCLUDE_HMC5883) - MagnetometerInitialize(); + MagnetometerInitialize(); #endif #if defined(PIOS_INCLUDE_BMP085) - BaroAltitudeInitialize(); + BaroAltitudeInitialize(); - // init down-sampling data - alt_ds_temp = 0; - alt_ds_pres = 0; - alt_ds_count = 0; + // init down-sampling data + alt_ds_temp = 0; + alt_ds_pres = 0; + alt_ds_count = 0; #endif - } - return 0; + } + return 0; } MODULE_INITCALL(MagBaroInitialize, MagBaroStart) /** @@ -132,94 +130,91 @@ MODULE_INITCALL(MagBaroInitialize, MagBaroStart) #if defined(PIOS_INCLUDE_HMC5883) static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { #ifdef PIOS_HMC5883_HAS_GPIOS - .exti_cfg = 0, + .exti_cfg = 0, #endif - .M_ODR = PIOS_HMC5883_ODR_15, - .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, - .Gain = PIOS_HMC5883_GAIN_1_9, - .Mode = PIOS_HMC5883_MODE_CONTINUOUS, - + .M_ODR = PIOS_HMC5883_ODR_15, + .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, + .Gain = PIOS_HMC5883_GAIN_1_9, + .Mode = PIOS_HMC5883_MODE_CONTINUOUS, }; #endif static void magbaroTask(__attribute__((unused)) void *parameters) { - portTickType lastSysTime; - + portTickType lastSysTime; + #if defined(PIOS_INCLUDE_BMP085) - BaroAltitudeData data; - PIOS_BMP085_Init(); + BaroAltitudeData data; + PIOS_BMP085_Init(); #endif #if defined(PIOS_INCLUDE_HMC5883) - MagnetometerData mag; - PIOS_HMC5883_Init(&pios_hmc5883_cfg); - uint32_t mag_update_time = PIOS_DELAY_GetRaw(); + MagnetometerData mag; + PIOS_HMC5883_Init(&pios_hmc5883_cfg); + uint32_t mag_update_time = PIOS_DELAY_GetRaw(); #endif - // Main task loop - lastSysTime = xTaskGetTickCount(); - while (1) - { + // Main task loop + lastSysTime = xTaskGetTickCount(); + while (1) { #if defined(PIOS_INCLUDE_BMP085) - // Update the temperature data - PIOS_BMP085_StartADC(TemperatureConv); + // Update the temperature data + PIOS_BMP085_StartADC(TemperatureConv); #ifdef PIOS_BMP085_HAS_GPIOS - xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); + xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); #else - vTaskDelay(5 / portTICK_RATE_MS); + vTaskDelay(5 / portTICK_RATE_MS); #endif - PIOS_BMP085_ReadADC(); - alt_ds_temp += PIOS_BMP085_GetTemperature(); + PIOS_BMP085_ReadADC(); + alt_ds_temp += PIOS_BMP085_GetTemperature(); - // Update the pressure data - PIOS_BMP085_StartADC(PressureConv); + // Update the pressure data + PIOS_BMP085_StartADC(PressureConv); #ifdef PIOS_BMP085_HAS_GPIOS - xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); + xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); #else - vTaskDelay(26 / portTICK_RATE_MS); + vTaskDelay(26 / portTICK_RATE_MS); #endif - PIOS_BMP085_ReadADC(); - alt_ds_pres += PIOS_BMP085_GetPressure(); + PIOS_BMP085_ReadADC(); + alt_ds_pres += PIOS_BMP085_GetPressure(); - if (++alt_ds_count >= alt_ds_size) - { - alt_ds_count = 0; + if (++alt_ds_count >= alt_ds_size) { + alt_ds_count = 0; - // Convert from 1/10ths of degC to degC - data.Temperature = alt_ds_temp / (10.0f * alt_ds_size); - alt_ds_temp = 0; + // Convert from 1/10ths of degC to degC + data.Temperature = alt_ds_temp / (10.0f * alt_ds_size); + alt_ds_temp = 0; - // Convert from Pa to kPa - data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size); - alt_ds_pres = 0; + // Convert from Pa to kPa + data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size); + alt_ds_pres = 0; - // Compute the current altitude (all pressures in kPa) - data.Altitude = 44330.0f * (1.0f - powf((data.Pressure / (BMP085_P0 / 1000.0f)), (1.0f / 5.255f))); + // Compute the current altitude (all pressures in kPa) + data.Altitude = 44330.0f * (1.0f - powf((data.Pressure / (BMP085_P0 / 1000.0f)), (1.0f / 5.255f))); - // Update the AltitudeActual UAVObject - BaroAltitudeSet(&data); - } -#endif + // Update the AltitudeActual UAVObject + BaroAltitudeSet(&data); + } +#endif /* if defined(PIOS_INCLUDE_BMP085) */ #if defined(PIOS_INCLUDE_HMC5883) - if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) { - int16_t values[3]; - PIOS_HMC5883_ReadMag(values); - float mags[3] = {(float) values[1] * mag_scale[0] - mag_bias[0], - (float) values[0] * mag_scale[1] - mag_bias[1], - -(float) values[2] * mag_scale[2] - mag_bias[2]}; - mag.x = mags[0]; - mag.y = mags[1]; - mag.z = mags[2]; - MagnetometerSet(&mag); - mag_update_time = PIOS_DELAY_GetRaw(); - } + if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) { + int16_t values[3]; + PIOS_HMC5883_ReadMag(values); + float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0], + (float)values[0] * mag_scale[1] - mag_bias[1], + -(float)values[2] * mag_scale[2] - mag_bias[2] }; + mag.x = mags[0]; + mag.y = mags[1]; + mag.z = mags[2]; + MagnetometerSet(&mag); + mag_update_time = PIOS_DELAY_GetRaw(); + } #endif - // Delay until it is time to read the next sample - vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS); - } + // Delay until it is time to read the next sample + vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS); + } } /** diff --git a/flight/modules/Fault/Fault.c b/flight/modules/Fault/Fault.c index 33b15f7cb..86c052ce0 100644 --- a/flight/modules/Fault/Fault.c +++ b/flight/modules/Fault/Fault.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup FaultModule Fault Module * @brief Insert various fault conditions for testing - * @{ + * @{ * * @file Fault.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -41,95 +41,102 @@ static uint8_t active_fault; static int32_t fault_initialize(void) { #ifdef MODULE_FAULT_BUILTIN - module_enabled = true; + module_enabled = true; #else - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_FAULT] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - module_enabled = true; - } else { - module_enabled = false; - } + if (optionalModules[HWSETTINGS_OPTIONALMODULES_FAULT] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + module_enabled = true; + } else { + module_enabled = false; + } #endif - /* Do this outside the module_enabled test so that it - * can be changed even when the module has been disabled. - * This is important so we can remove faults even when - * we've booted in BootFault recovery mode with all optional - * modules disabled. - */ - FaultSettingsInitialize(); + /* Do this outside the module_enabled test so that it + * can be changed even when the module has been disabled. + * This is important so we can remove faults even when + * we've booted in BootFault recovery mode with all optional + * modules disabled. + */ + FaultSettingsInitialize(); - if (module_enabled) { - FaultSettingsActivateFaultGet(&active_fault); + if (module_enabled) { + FaultSettingsActivateFaultGet(&active_fault); - switch (active_fault) { - case FAULTSETTINGS_ACTIVATEFAULT_MODULEINITASSERT: - /* Simulate an assert during module init */ - PIOS_Assert(0); - break; - case FAULTSETTINGS_ACTIVATEFAULT_INITOUTOFMEMORY: - /* Leak all available memory */ - while (pvPortMalloc(10)) ; - break; - case FAULTSETTINGS_ACTIVATEFAULT_INITBUSERROR: - { - /* Force a bad access */ - uint32_t * bad_ptr = (uint32_t *)0xFFFFFFFF; - *bad_ptr = 0xAA55AA55; - } - break; - } - } + switch (active_fault) { + case FAULTSETTINGS_ACTIVATEFAULT_MODULEINITASSERT: + /* Simulate an assert during module init */ + PIOS_Assert(0); + break; + case FAULTSETTINGS_ACTIVATEFAULT_INITOUTOFMEMORY: + /* Leak all available memory */ + while (pvPortMalloc(10)) { + ; + } + break; + case FAULTSETTINGS_ACTIVATEFAULT_INITBUSERROR: + { + /* Force a bad access */ + uint32_t *bad_ptr = (uint32_t *)0xFFFFFFFF; + *bad_ptr = 0xAA55AA55; + } + break; + } + } - return 0; + return 0; } static void fault_task(void *parameters); static int32_t fault_start(void) { - xTaskHandle fault_task_handle; + xTaskHandle fault_task_handle; - if (module_enabled) { - switch (active_fault) { - case FAULTSETTINGS_ACTIVATEFAULT_RUNAWAYTASK: - case FAULTSETTINGS_ACTIVATEFAULT_TASKOUTOFMEMORY: - xTaskCreate(fault_task, - (signed char *)"Fault", - configMINIMAL_STACK_SIZE, - NULL, - configMAX_PRIORITIES-1, - &fault_task_handle); - return 0; - break; - } - } - return -1; + if (module_enabled) { + switch (active_fault) { + case FAULTSETTINGS_ACTIVATEFAULT_RUNAWAYTASK: + case FAULTSETTINGS_ACTIVATEFAULT_TASKOUTOFMEMORY: + xTaskCreate(fault_task, + (signed char *)"Fault", + configMINIMAL_STACK_SIZE, + NULL, + configMAX_PRIORITIES - 1, + &fault_task_handle); + return 0; + + break; + } + } + return -1; } MODULE_INITCALL(fault_initialize, fault_start) -static void fault_task(__attribute__((unused))void *parameters) +static void fault_task(__attribute__((unused)) void *parameters) { - switch (active_fault) { - case FAULTSETTINGS_ACTIVATEFAULT_RUNAWAYTASK: - /* Consume all realtime, not letting the systemtask run */ - while(1); - break; - case FAULTSETTINGS_ACTIVATEFAULT_TASKOUTOFMEMORY: - /* Leak all available memory and then sleep */ - while (pvPortMalloc(10)) ; - while (1) { - vTaskDelay(1000); - } - break; - } + switch (active_fault) { + case FAULTSETTINGS_ACTIVATEFAULT_RUNAWAYTASK: + /* Consume all realtime, not letting the systemtask run */ + while (1) { + ; + } + break; + case FAULTSETTINGS_ACTIVATEFAULT_TASKOUTOFMEMORY: + /* Leak all available memory and then sleep */ + while (pvPortMalloc(10)) { + ; + } + while (1) { + vTaskDelay(1000); + } + break; + } } -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/modules/FirmwareIAP/firmwareiap.c b/flight/modules/FirmwareIAP/firmwareiap.c index 992971b3c..553eef2c6 100644 --- a/flight/modules/FirmwareIAP/firmwareiap.c +++ b/flight/modules/FirmwareIAP/firmwareiap.c @@ -4,7 +4,7 @@ * @file firmwareiap.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief In Application Programming module to support firmware upgrades by - * providing a means to enter the bootloader. + * providing a means to enter the bootloader. * * @see The GNU Public License (GPL) Version 3 * @@ -40,7 +40,7 @@ #define IAP_CMD_CRC 100 #define IAP_CMD_VERIFY 101 -#define IAP_CMD_VERSION 102 +#define IAP_CMD_VERSION 102 #define IAP_STATE_READY 0 #define IAP_STATE_STEP_1 1 @@ -49,13 +49,13 @@ #define RESET_DELAY 500 /* delay between sending reset ot INS */ -#define TICKS2MS(t) ((t)/portTICK_RATE_MS) -#define MS2TICKS(m) ((m)*portTICK_RATE_MS) +#define TICKS2MS(t) ((t) / portTICK_RATE_MS) +#define MS2TICKS(m) ((m) * portTICK_RATE_MS) -const uint32_t iap_time_2_low_end = 500; -const uint32_t iap_time_2_high_end = 5000; -const uint32_t iap_time_3_low_end = 500; -const uint32_t iap_time_3_high_end = 5000; +const uint32_t iap_time_2_low_end = 500; +const uint32_t iap_time_2_high_end = 5000; +const uint32_t iap_time_3_low_end = 500; +const uint32_t iap_time_3_high_end = 5000; // Private types @@ -64,9 +64,9 @@ static uint8_t reset_count = 0; static portTickType lastResetSysTime; // Private functions -static void FirmwareIAPCallback(UAVObjEvent* ev); +static void FirmwareIAPCallback(UAVObjEvent *ev); -static uint32_t get_time(void); +static uint32_t get_time(void); // Private types @@ -89,28 +89,29 @@ static void resetTask(UAVObjEvent *); MODULE_INITCALL(FirmwareIAPInitialize, 0) int32_t FirmwareIAPInitialize() { - - FirmwareIAPObjInitialize(); - - const struct pios_board_info * bdinfo = &pios_board_info_blob; + FirmwareIAPObjInitialize(); - FirmwareIAPObjData data; - FirmwareIAPObjGet(&data); + const struct pios_board_info *bdinfo = &pios_board_info_blob; - data.BoardType= bdinfo->board_type; - PIOS_BL_HELPER_FLASH_Read_Description(data.Description,FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM); - PIOS_SYS_SerialNumberGetBinary(data.CPUSerial); - data.BoardRevision= bdinfo->board_rev; - data.ArmReset=0; - data.crc = 0; - FirmwareIAPObjSet( &data ); - if(bdinfo->magic==PIOS_BOARD_INFO_BLOB_MAGIC) FirmwareIAPObjConnectCallback( &FirmwareIAPCallback ); - return 0; + FirmwareIAPObjData data; + FirmwareIAPObjGet(&data); + + data.BoardType = bdinfo->board_type; + PIOS_BL_HELPER_FLASH_Read_Description(data.Description, FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM); + PIOS_SYS_SerialNumberGetBinary(data.CPUSerial); + data.BoardRevision = bdinfo->board_rev; + data.ArmReset = 0; + data.crc = 0; + FirmwareIAPObjSet(&data); + if (bdinfo->magic == PIOS_BOARD_INFO_BLOB_MAGIC) { + FirmwareIAPObjConnectCallback(&FirmwareIAPCallback); + } + return 0; } int32_t FirmwareIAPStart() { - return 0; + return 0; } /*! @@ -121,100 +122,98 @@ int32_t FirmwareIAPStart() * \note * */ -static uint8_t iap_state = IAP_STATE_READY; -static void FirmwareIAPCallback(UAVObjEvent* ev) +static uint8_t iap_state = IAP_STATE_READY; +static void FirmwareIAPCallback(UAVObjEvent *ev) { - const struct pios_board_info * bdinfo = &pios_board_info_blob; - static uint32_t last_time = 0; - uint32_t this_time; - uint32_t delta; - - if(iap_state == IAP_STATE_RESETTING) - return; + const struct pios_board_info *bdinfo = &pios_board_info_blob; + static uint32_t last_time = 0; + uint32_t this_time; + uint32_t delta; - FirmwareIAPObjData data; - FirmwareIAPObjGet(&data); - - if ( ev->obj == FirmwareIAPObjHandle() ) { - // Get the input object data - FirmwareIAPObjGet(&data); - this_time = get_time(); - delta = this_time - last_time; - last_time = this_time; - if((data.BoardType==bdinfo->board_type)&&(data.crc != PIOS_BL_HELPER_CRC_Memory_Calc())) - { - PIOS_BL_HELPER_FLASH_Read_Description(data.Description,FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM); - PIOS_SYS_SerialNumberGetBinary(data.CPUSerial); - data.BoardRevision=bdinfo->board_rev; - data.crc = PIOS_BL_HELPER_CRC_Memory_Calc(); - FirmwareIAPObjSet( &data ); - } - if((data.ArmReset==1)&&(iap_state!=IAP_STATE_RESETTING)) - { - data.ArmReset=0; - FirmwareIAPObjSet( &data ); - } - switch(iap_state) { - case IAP_STATE_READY: - if( data.Command == IAP_CMD_STEP_1 ) { - iap_state = IAP_STATE_STEP_1; - } break; - case IAP_STATE_STEP_1: - if( data.Command == IAP_CMD_STEP_2 ) { - if( delta > iap_time_2_low_end && delta < iap_time_2_high_end ) { - iap_state = IAP_STATE_STEP_2; - } else { - iap_state = IAP_STATE_READY; - } - } else { - iap_state = IAP_STATE_READY; - } - break; - case IAP_STATE_STEP_2: - if( data.Command == IAP_CMD_STEP_3 ) { - if( delta > iap_time_3_low_end && delta < iap_time_3_high_end ) { - - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - - if(flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) { - // Abort any attempts if not disarmed - iap_state = IAP_STATE_READY; - break; - } - - // we've met the three sequence of command numbers - // we've met the time requirements. - PIOS_IAP_SetRequest1(); - PIOS_IAP_SetRequest2(); - - /* Note: Cant just wait timeout value, because first time is randomized */ - reset_count = 0; - lastResetSysTime = xTaskGetTickCount(); - UAVObjEvent *event = pvPortMalloc(sizeof(UAVObjEvent)); - memset(event, 0, sizeof(UAVObjEvent)); - EventPeriodicCallbackCreate(event, resetTask, 100); - iap_state = IAP_STATE_RESETTING; - } else { - iap_state = IAP_STATE_READY; - } - } else { - iap_state = IAP_STATE_READY; - } - break; - case IAP_STATE_RESETTING: - // stay here permanentally, should reboot - break; - default: - iap_state = IAP_STATE_READY; - last_time = 0; // Reset the time counter, as we are not doing a IAP reset - break; - } - } + if (iap_state == IAP_STATE_RESETTING) { + return; + } + + FirmwareIAPObjData data; + FirmwareIAPObjGet(&data); + + if (ev->obj == FirmwareIAPObjHandle()) { + // Get the input object data + FirmwareIAPObjGet(&data); + this_time = get_time(); + delta = this_time - last_time; + last_time = this_time; + if ((data.BoardType == bdinfo->board_type) && (data.crc != PIOS_BL_HELPER_CRC_Memory_Calc())) { + PIOS_BL_HELPER_FLASH_Read_Description(data.Description, FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM); + PIOS_SYS_SerialNumberGetBinary(data.CPUSerial); + data.BoardRevision = bdinfo->board_rev; + data.crc = PIOS_BL_HELPER_CRC_Memory_Calc(); + FirmwareIAPObjSet(&data); + } + if ((data.ArmReset == 1) && (iap_state != IAP_STATE_RESETTING)) { + data.ArmReset = 0; + FirmwareIAPObjSet(&data); + } + switch (iap_state) { + case IAP_STATE_READY: + if (data.Command == IAP_CMD_STEP_1) { + iap_state = IAP_STATE_STEP_1; + } + break; + case IAP_STATE_STEP_1: + if (data.Command == IAP_CMD_STEP_2) { + if (delta > iap_time_2_low_end && delta < iap_time_2_high_end) { + iap_state = IAP_STATE_STEP_2; + } else { + iap_state = IAP_STATE_READY; + } + } else { + iap_state = IAP_STATE_READY; + } + break; + case IAP_STATE_STEP_2: + if (data.Command == IAP_CMD_STEP_3) { + if (delta > iap_time_3_low_end && delta < iap_time_3_high_end) { + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + if (flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) { + // Abort any attempts if not disarmed + iap_state = IAP_STATE_READY; + break; + } + + // we've met the three sequence of command numbers + // we've met the time requirements. + PIOS_IAP_SetRequest1(); + PIOS_IAP_SetRequest2(); + + /* Note: Cant just wait timeout value, because first time is randomized */ + reset_count = 0; + lastResetSysTime = xTaskGetTickCount(); + UAVObjEvent *event = pvPortMalloc(sizeof(UAVObjEvent)); + memset(event, 0, sizeof(UAVObjEvent)); + EventPeriodicCallbackCreate(event, resetTask, 100); + iap_state = IAP_STATE_RESETTING; + } else { + iap_state = IAP_STATE_READY; + } + } else { + iap_state = IAP_STATE_READY; + } + break; + case IAP_STATE_RESETTING: + // stay here permanentally, should reboot + break; + default: + iap_state = IAP_STATE_READY; + last_time = 0; // Reset the time counter, as we are not doing a IAP reset + break; + } + } } - // Returns number of milliseconds from the start of the kernel. /*! @@ -228,39 +227,38 @@ static void FirmwareIAPCallback(UAVObjEvent* ev) static uint32_t get_time(void) { - portTickType ticks; - - ticks = xTaskGetTickCount(); - - return TICKS2MS(ticks); + portTickType ticks; + + ticks = xTaskGetTickCount(); + + return TICKS2MS(ticks); } /** - * Executed by event dispatcher callback to reset INS before resetting OP + * Executed by event dispatcher callback to reset INS before resetting OP */ -static void resetTask(__attribute__((unused)) UAVObjEvent * ev) +static void resetTask(__attribute__((unused)) UAVObjEvent *ev) { -#if defined (PIOS_LED_HEARTBEAT) - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ -#if defined (PIOS_LED_ALARM) - PIOS_LED_Toggle(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ +#if defined(PIOS_LED_ALARM) + PIOS_LED_Toggle(PIOS_LED_ALARM); +#endif /* PIOS_LED_ALARM */ - FirmwareIAPObjData data; - FirmwareIAPObjGet(&data); + FirmwareIAPObjData data; + FirmwareIAPObjGet(&data); - if((portTickType) (xTaskGetTickCount() - lastResetSysTime) > RESET_DELAY / portTICK_RATE_MS) { - lastResetSysTime = xTaskGetTickCount(); - data.BoardType=0xFF; - data.ArmReset=1; - data.crc=reset_count; /* Must change a value for this to get to INS */ - FirmwareIAPObjSet(&data); - ++reset_count; - if(reset_count>3) - { - PIOS_SYS_Reset(); - } - } + if ((portTickType)(xTaskGetTickCount() - lastResetSysTime) > RESET_DELAY / portTICK_RATE_MS) { + lastResetSysTime = xTaskGetTickCount(); + data.BoardType = 0xFF; + data.ArmReset = 1; + data.crc = reset_count; /* Must change a value for this to get to INS */ + FirmwareIAPObjSet(&data); + ++reset_count; + if (reset_count > 3) { + PIOS_SYS_Reset(); + } + } } diff --git a/flight/modules/FirmwareIAP/inc/firmwareiap.h b/flight/modules/FirmwareIAP/inc/firmwareiap.h index f62a51d4a..81c39d880 100644 --- a/flight/modules/FirmwareIAP/inc/firmwareiap.h +++ b/flight/modules/FirmwareIAP/inc/firmwareiap.h @@ -30,4 +30,3 @@ int32_t FirmwareIAPInitialize(); int32_t FirmwareIAPStart(); #endif // FIRMWAREIAP_H - diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 66e69f4c5..fec984a87 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -3,7 +3,7 @@ * * @file fixedwingpathfollower.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint + * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint * and sets @ref AttitudeDesired. It only does this when the FlightMode field * of @ref ManualControlCommand is Auto. * @@ -48,7 +48,7 @@ #include "accels.h" #include "hwsettings.h" #include "attitudeactual.h" -#include "pathdesired.h" // object that will be updated by the module +#include "pathdesired.h" // object that will be updated by the module #include "positionactual.h" #include "manualcontrol.h" #include "flightstatus.h" @@ -70,9 +70,9 @@ #include "CoordinateConversions.h" // Private constants -#define MAX_QUEUE_SIZE 4 +#define MAX_QUEUE_SIZE 4 #define STACK_SIZE_BYTES 1548 -#define TASK_PRIORITY (tskIDLE_PRIORITY+2) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) // Private variables static bool followerEnabled = false; @@ -83,11 +83,11 @@ static FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings; // Private functions static void pathfollowerTask(void *parameters); -static void SettingsUpdatedCb(UAVObjEvent * ev); +static void SettingsUpdatedCb(UAVObjEvent *ev); static void updatePathVelocity(); static uint8_t updateFixedDesiredAttitude(); static void updateFixedAttitude(); -static void airspeedActualUpdatedCb(UAVObjEvent * ev); +static void airspeedActualUpdatedCb(UAVObjEvent *ev); static float bound(float val, float min, float max); /** @@ -96,13 +96,13 @@ static float bound(float val, float min, float max); */ int32_t FixedWingPathFollowerStart() { - if (followerEnabled) { - // Start main task - xTaskCreate(pathfollowerTask, (signed char *)"PathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle); - } + if (followerEnabled) { + // Start main task + xTaskCreate(pathfollowerTask, (signed char *)"PathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle); + } - return 0; + return 0; } /** @@ -111,32 +111,32 @@ int32_t FixedWingPathFollowerStart() */ int32_t FixedWingPathFollowerInitialize() { - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_FIXEDWINGPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - followerEnabled = true; - FixedWingPathFollowerSettingsInitialize(); - FixedWingPathFollowerStatusInitialize(); - PathDesiredInitialize(); - PathStatusInitialize(); - VelocityDesiredInitialize(); - AirspeedActualInitialize(); - } else { - followerEnabled = false; - } - return 0; + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + if (optionalModules[HWSETTINGS_OPTIONALMODULES_FIXEDWINGPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + followerEnabled = true; + FixedWingPathFollowerSettingsInitialize(); + FixedWingPathFollowerStatusInitialize(); + PathDesiredInitialize(); + PathStatusInitialize(); + VelocityDesiredInitialize(); + AirspeedActualInitialize(); + } else { + followerEnabled = false; + } + return 0; } MODULE_INITCALL(FixedWingPathFollowerInitialize, FixedWingPathFollowerStart) static float northVelIntegral = 0; -static float eastVelIntegral = 0; -static float downVelIntegral = 0; +static float eastVelIntegral = 0; +static float downVelIntegral = 0; -static float bearingIntegral = 0; -static float speedIntegral = 0; -static float powerIntegral = 0; -static float airspeedErrorInt=0; +static float bearingIntegral = 0; +static float speedIntegral = 0; +static float powerIntegral = 0; +static float airspeedErrorInt = 0; // correct speed by measured airspeed static float indicatedAirspeedActualBias = 0; @@ -146,197 +146,205 @@ static float indicatedAirspeedActualBias = 0; */ static void pathfollowerTask(__attribute__((unused)) void *parameters) { - SystemSettingsData systemSettings; - FlightStatusData flightStatus; - - portTickType lastUpdateTime; - - AirspeedActualConnectCallback(airspeedActualUpdatedCb); - FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb); - PathDesiredConnectCallback(SettingsUpdatedCb); - - FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); - PathDesiredGet(&pathDesired); - - // Main task loop - lastUpdateTime = xTaskGetTickCount(); - while (1) { + SystemSettingsData systemSettings; + FlightStatusData flightStatus; - // Conditions when this runs: - // 1. Must have FixedWing type airframe - // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR - // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path + portTickType lastUpdateTime; - SystemSettingsGet(&systemSettings); - if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) ) - { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING); - vTaskDelay(1000); - continue; - } + AirspeedActualConnectCallback(airspeedActualUpdatedCb); + FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb); + PathDesiredConnectCallback(SettingsUpdatedCb); - // Continue collecting data if not enough time - vTaskDelayUntil(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS); + FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); + PathDesiredGet(&pathDesired); - - FlightStatusGet(&flightStatus); - PathStatusGet(&pathStatus); - - uint8_t result; - // Check the combinations of flightmode and pathdesired mode - switch(flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updatePathVelocity(); - result = updateFixedDesiredAttitude(); - if (result) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING); - } - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR); - } - break; - case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: - pathStatus.UID = pathDesired.UID; - pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; - switch(pathDesired.Mode) { - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - updatePathVelocity(); - result = updateFixedDesiredAttitude(); - if (result) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); - } else { - pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING); - } - break; - case PATHDESIRED_MODE_FIXEDATTITUDE: - updateFixedAttitude(pathDesired.ModeParameters); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_DISARMALARM: - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_CRITICAL); - break; - default: - pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR); - break; - } - break; - default: - // Be cleaner and get rid of global variables - northVelIntegral = 0; - eastVelIntegral = 0; - downVelIntegral = 0; - bearingIntegral = 0; - speedIntegral = 0; - powerIntegral = 0; + // Main task loop + lastUpdateTime = xTaskGetTickCount(); + while (1) { + // Conditions when this runs: + // 1. Must have FixedWing type airframe + // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR + // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path - break; - } - PathStatusSet(&pathStatus); - } + SystemSettingsGet(&systemSettings); + if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) && + (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) && + (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL)) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + vTaskDelay(1000); + continue; + } + + // Continue collecting data if not enough time + vTaskDelayUntil(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS); + + + FlightStatusGet(&flightStatus); + PathStatusGet(&pathStatus); + + uint8_t result; + // Check the combinations of flightmode and pathdesired mode + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { + updatePathVelocity(); + result = updateFixedDesiredAttitude(); + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + } + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + } + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + pathStatus.UID = pathDesired.UID; + pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + updatePathVelocity(); + result = updateFixedDesiredAttitude(); + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + } + break; + case PATHDESIRED_MODE_FIXEDATTITUDE: + updateFixedAttitude(pathDesired.ModeParameters); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + break; + case PATHDESIRED_MODE_DISARMALARM: + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + break; + default: + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + break; + } + break; + default: + // Be cleaner and get rid of global variables + northVelIntegral = 0; + eastVelIntegral = 0; + downVelIntegral = 0; + bearingIntegral = 0; + speedIntegral = 0; + powerIntegral = 0; + + break; + } + PathStatusSet(&pathStatus); + } } /** * Compute desired velocity from the current position and path * - * Takes in @ref PositionActual and compares it to @ref PathDesired + * Takes in @ref PositionActual and compares it to @ref PathDesired * and computes @ref VelocityDesired */ static void updatePathVelocity() { - PositionActualData positionActual; - PositionActualGet(&positionActual); - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); + PositionActualData positionActual; - // look ahead fixedwingpathfollowerSettings.HeadingFeedForward seconds - float cur[3] = {positionActual.North + (velocityActual.North * fixedwingpathfollowerSettings.HeadingFeedForward), - positionActual.East + (velocityActual.East * fixedwingpathfollowerSettings.HeadingFeedForward), - positionActual.Down + (velocityActual.Down * fixedwingpathfollowerSettings.HeadingFeedForward) - }; - struct path_status progress; + PositionActualGet(&positionActual); + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); - path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); - - float groundspeed; - float altitudeSetpoint; - switch (pathDesired.Mode) { - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - groundspeed = pathDesired.EndingVelocity; - altitudeSetpoint = pathDesired.End[2]; - break; - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_DRIVEENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_DRIVEVECTOR: - default: - groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * - bound(progress.fractional_progress,0,1); - altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * - bound(progress.fractional_progress,0,1); - break; - } - // make sure groundspeed is not zero - if (groundspeed<1e-2f) groundspeed=1e-2f; - - // calculate velocity - can be zero if waypoints are too close - VelocityDesiredData velocityDesired; - velocityDesired.North = progress.path_direction[0]; - velocityDesired.East = progress.path_direction[1]; - - float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP; + // look ahead fixedwingpathfollowerSettings.HeadingFeedForward seconds + float cur[3] = { positionActual.North + (velocityActual.North * fixedwingpathfollowerSettings.HeadingFeedForward), + positionActual.East + (velocityActual.East * fixedwingpathfollowerSettings.HeadingFeedForward), + positionActual.Down + (velocityActual.Down * fixedwingpathfollowerSettings.HeadingFeedForward) }; + struct path_status progress; - // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) - // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector - // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading) - // leading to an S-shape snake course the wrong way - // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't - // turn steep unless there is enough space complete the turn before crossing the flightpath - // in this case the plane effectively needs to be turned around - // indicators: - // difference between correction_direction and velocityactual >90 degrees and - // difference between path_direction and velocityactual >90 degrees ( 4th sector, facing away from eerything ) - // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) - float angle1=RAD2DEG( atan2f(progress.path_direction[1],progress.path_direction[0]) - atan2f(velocityActual.East,velocityActual.North)); - float angle2=RAD2DEG( atan2f(progress.correction_direction[1],progress.correction_direction[0]) - atan2f(velocityActual.East,velocityActual.North)); - if (angle1<-180.0f) angle1+=360.0f; - if (angle1>180.0f) angle1-=360.0f; - if (angle2<-180.0f) angle2+=360.0f; - if (angle2>180.0f) angle2-=360.0f; - if (fabsf(angle1)>=90.0f && fabsf(angle2)>=90.0f) { - error_speed=0; - } + path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); - // calculate correction - can also be zero if correction vector is 0 or no error present - velocityDesired.North += progress.correction_direction[0] * error_speed; - velocityDesired.East += progress.correction_direction[1] * error_speed; + float groundspeed; + float altitudeSetpoint; + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_DRIVECIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + case PATHDESIRED_MODE_DRIVECIRCLELEFT: + groundspeed = pathDesired.EndingVelocity; + altitudeSetpoint = pathDesired.End[2]; + break; + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_DRIVEENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_DRIVEVECTOR: + default: + groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * + bound(progress.fractional_progress, 0, 1); + altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * + bound(progress.fractional_progress, 0, 1); + break; + } + // make sure groundspeed is not zero + if (groundspeed < 1e-2f) { + groundspeed = 1e-2f; + } - //scale to correct length - float l=sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East); - velocityDesired.North *= groundspeed/l; - velocityDesired.East *= groundspeed/l; - - float downError = altitudeSetpoint - positionActual.Down; - velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP; + // calculate velocity - can be zero if waypoints are too close + VelocityDesiredData velocityDesired; + velocityDesired.North = progress.path_direction[0]; + velocityDesired.East = progress.path_direction[1]; - // update pathstatus - pathStatus.error = progress.error; - pathStatus.fractional_progress = progress.fractional_progress; + float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP; - VelocityDesiredSet(&velocityDesired); + // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) + // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector + // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading) + // leading to an S-shape snake course the wrong way + // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't + // turn steep unless there is enough space complete the turn before crossing the flightpath + // in this case the plane effectively needs to be turned around + // indicators: + // difference between correction_direction and velocityactual >90 degrees and + // difference between path_direction and velocityactual >90 degrees ( 4th sector, facing away from eerything ) + // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) + float angle1 = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0]) - atan2f(velocityActual.East, velocityActual.North)); + float angle2 = RAD2DEG(atan2f(progress.correction_direction[1], progress.correction_direction[0]) - atan2f(velocityActual.East, velocityActual.North)); + if (angle1 < -180.0f) { + angle1 += 360.0f; + } + if (angle1 > 180.0f) { + angle1 -= 360.0f; + } + if (angle2 < -180.0f) { + angle2 += 360.0f; + } + if (angle2 > 180.0f) { + angle2 -= 360.0f; + } + if (fabsf(angle1) >= 90.0f && fabsf(angle2) >= 90.0f) { + error_speed = 0; + } + + // calculate correction - can also be zero if correction vector is 0 or no error present + velocityDesired.North += progress.correction_direction[0] * error_speed; + velocityDesired.East += progress.correction_direction[1] * error_speed; + + // scale to correct length + float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); + velocityDesired.North *= groundspeed / l; + velocityDesired.East *= groundspeed / l; + + float downError = altitudeSetpoint - positionActual.Down; + velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP; + + // update pathstatus + pathStatus.error = progress.error; + pathStatus.fractional_progress = progress.fractional_progress; + + VelocityDesiredSet(&velocityDesired); } @@ -344,273 +352,277 @@ static void updatePathVelocity() * Compute desired attitude from a fixed preset * */ -static void updateFixedAttitude(float* attitude) +static void updateFixedAttitude(float *attitude) { - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); - stabDesired.Roll = attitude[0]; - stabDesired.Pitch = attitude[1]; - stabDesired.Yaw = attitude[2]; - stabDesired.Throttle = attitude[3]; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - StabilizationDesiredSet(&stabDesired); + StabilizationDesiredData stabDesired; + + StabilizationDesiredGet(&stabDesired); + stabDesired.Roll = attitude[0]; + stabDesired.Pitch = attitude[1]; + stabDesired.Yaw = attitude[2]; + stabDesired.Throttle = attitude[3]; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + StabilizationDesiredSet(&stabDesired); } /** * Compute desired attitude from the desired velocity * - * Takes in @ref NedActual which has the acceleration in the - * NED frame as the feedback term and then compares the + * Takes in @ref NedActual which has the acceleration in the + * NED frame as the feedback term and then compares the * @ref VelocityActual against the @ref VelocityDesired */ static uint8_t updateFixedDesiredAttitude() { + uint8_t result = 1; - uint8_t result = 1; + float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; // Convert from [ms] to [s] - float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; //Convert from [ms] to [s] + VelocityDesiredData velocityDesired; + VelocityActualData velocityActual; + StabilizationDesiredData stabDesired; + AttitudeActualData attitudeActual; + AccelsData accels; + StabilizationSettingsData stabSettings; + FixedWingPathFollowerStatusData fixedwingpathfollowerStatus; + AirspeedActualData airspeedActual; - VelocityDesiredData velocityDesired; - VelocityActualData velocityActual; - StabilizationDesiredData stabDesired; - AttitudeActualData attitudeActual; - AccelsData accels; - StabilizationSettingsData stabSettings; - FixedWingPathFollowerStatusData fixedwingpathfollowerStatus; - AirspeedActualData airspeedActual; - - float groundspeedActual; - float groundspeedDesired; - float indicatedAirspeedActual; - float indicatedAirspeedDesired; - float airspeedError; - - float pitchCommand; + float groundspeedActual; + float groundspeedDesired; + float indicatedAirspeedActual; + float indicatedAirspeedDesired; + float airspeedError; - float descentspeedDesired; - float descentspeedError; - float powerCommand; + float pitchCommand; - float bearingError; - float bearingCommand; + float descentspeedDesired; + float descentspeedError; + float powerCommand; - FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus); - - VelocityActualGet(&velocityActual); - StabilizationDesiredGet(&stabDesired); - VelocityDesiredGet(&velocityDesired); - AttitudeActualGet(&attitudeActual); - AccelsGet(&accels); - StabilizationSettingsGet(&stabSettings); - AirspeedActualGet(&airspeedActual); + float bearingError; + float bearingCommand; + + FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus); + + VelocityActualGet(&velocityActual); + StabilizationDesiredGet(&stabDesired); + VelocityDesiredGet(&velocityDesired); + AttitudeActualGet(&attitudeActual); + AccelsGet(&accels); + StabilizationSettingsGet(&stabSettings); + AirspeedActualGet(&airspeedActual); - /** - * Compute speed error (required for throttle and pitch) - */ + /** + * Compute speed error (required for throttle and pitch) + */ - // Current ground speed - groundspeedActual = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North ); - // note that airspeedActualBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement, - // but thanks to accelerometers, groundspeed reacts faster to changes in direction - // than airspeed and gps sensors alone - indicatedAirspeedActual = groundspeedActual + indicatedAirspeedActualBias; + // Current ground speed + groundspeedActual = sqrtf(velocityActual.East * velocityActual.East + velocityActual.North * velocityActual.North); + // note that airspeedActualBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement, + // but thanks to accelerometers, groundspeed reacts faster to changes in direction + // than airspeed and gps sensors alone + indicatedAirspeedActual = groundspeedActual + indicatedAirspeedActualBias; - // Desired ground speed - groundspeedDesired = sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East); - indicatedAirspeedDesired = bound( groundspeedDesired + indicatedAirspeedActualBias, - fixedwingpathfollowerSettings.BestClimbRateSpeed, - fixedwingpathfollowerSettings.CruiseSpeed); + // Desired ground speed + groundspeedDesired = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); + indicatedAirspeedDesired = bound(groundspeedDesired + indicatedAirspeedActualBias, + fixedwingpathfollowerSettings.BestClimbRateSpeed, + fixedwingpathfollowerSettings.CruiseSpeed); - // Airspeed error - airspeedError = indicatedAirspeedDesired - indicatedAirspeedActual; + // Airspeed error + airspeedError = indicatedAirspeedDesired - indicatedAirspeedActual; - // Vertical speed error - descentspeedDesired = bound ( - velocityDesired.Down, - -fixedwingpathfollowerSettings.VerticalVelMax, - fixedwingpathfollowerSettings.VerticalVelMax); - descentspeedError = descentspeedDesired - velocityActual.Down; + // Vertical speed error + descentspeedDesired = bound( + velocityDesired.Down, + -fixedwingpathfollowerSettings.VerticalVelMax, + fixedwingpathfollowerSettings.VerticalVelMax); + descentspeedError = descentspeedDesired - velocityActual.Down; - // Error condition: wind speed is higher than maximum allowed speed. We are forced backwards! - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0; - if (groundspeedDesired - indicatedAirspeedActualBias <= 0 ) { - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1; - result = 0; - } - // Error condition: plane too slow or too fast - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0; - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0; - if ( indicatedAirspeedActual > fixedwingpathfollowerSettings.AirSpeedMax) { - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1; - result = 0; - } - if ( indicatedAirspeedActual > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) { - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1; - result = 0; - } - if (indicatedAirspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { //The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; - result = 0; - } - if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { //Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; - result = 0; - } - if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) { - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; - result = 0; - } - - if (indicatedAirspeedActual<1e-6f) { - // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes - // also we cannot handle planes flying backwards, lets just wait until the nose drops - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; - return 0; - } + // Error condition: wind speed is higher than maximum allowed speed. We are forced backwards! + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0; + if (groundspeedDesired - indicatedAirspeedActualBias <= 0) { + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1; + result = 0; + } + // Error condition: plane too slow or too fast + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0; + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0; + if (indicatedAirspeedActual > fixedwingpathfollowerSettings.AirSpeedMax) { + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1; + result = 0; + } + if (indicatedAirspeedActual > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) { + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1; + result = 0; + } + if (indicatedAirspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { // The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; + result = 0; + } + if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { // Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; + result = 0; + } + if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) { + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; + result = 0; + } - /** - * Compute desired throttle command - */ - // compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant. - if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] >0) { - powerIntegral = bound(powerIntegral + -descentspeedError * dT, - -fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI], - fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] - )*(1.0f-1.0f/(1.0f+30.0f/dT)); - } else powerIntegral = 0; - - //Compute the cross feed from vertical speed to pitch, with saturation - float speedErrorToPowerCommandComponent = bound ( - (airspeedError/fixedwingpathfollowerSettings.BestClimbRateSpeed)* fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_KP] , - -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX], - fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX] - ); + if (indicatedAirspeedActual < 1e-6f) { + // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes + // also we cannot handle planes flying backwards, lets just wait until the nose drops + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; + return 0; + } - // Compute final throttle response - powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] + - powerIntegral* fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] + - speedErrorToPowerCommandComponent; + /** + * Compute desired throttle command + */ + // compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant. + if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] > 0) { + powerIntegral = bound(powerIntegral + -descentspeedError * dT, + -fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT] / fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI], + fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT] / fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] + ) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); + } else { powerIntegral = 0; } - //Output internal state to telemetry - fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = descentspeedError; - fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_POWER] = powerIntegral; - fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_POWER] = powerCommand; + // Compute the cross feed from vertical speed to pitch, with saturation + float speedErrorToPowerCommandComponent = bound( + (airspeedError / fixedwingpathfollowerSettings.BestClimbRateSpeed) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_KP], + -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX], + fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX] + ); - // set throttle - stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL] + powerCommand, - fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN], - fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]); + // Compute final throttle response + powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] + + powerIntegral * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] + + speedErrorToPowerCommandComponent; - // Error condition: plane cannot hold altitude at current speed. - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0; - if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] && // throttle at maximum - velocityActual.Down > 0 && // we ARE going down - descentspeedDesired < 0 && // we WANT to go up - airspeedError > 0) { // we are too slow already - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1; - result = 0; - } - // Error condition: plane keeps climbing despite minimum throttle (opposite of above) - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0; - if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] && // throttle at minimum - velocityActual.Down < 0 && // we ARE going up - descentspeedDesired > 0 && // we WANT to go down - airspeedError < 0 ) { // we are too fast already - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1; - result = 0; - } + // Output internal state to telemetry + fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = descentspeedError; + fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_POWER] = powerIntegral; + fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_POWER] = powerCommand; + + // set throttle + stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL] + powerCommand, + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN], + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]); + + // Error condition: plane cannot hold altitude at current speed. + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0; + if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] && // throttle at maximum + velocityActual.Down > 0 && // we ARE going down + descentspeedDesired < 0 && // we WANT to go up + airspeedError > 0) { // we are too slow already + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1; + result = 0; + } + // Error condition: plane keeps climbing despite minimum throttle (opposite of above) + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0; + if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] && // throttle at minimum + velocityActual.Down < 0 && // we ARE going up + descentspeedDesired > 0 && // we WANT to go down + airspeedError < 0) { // we are too fast already + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1; + result = 0; + } - /** - * Compute desired pitch command - */ - - if (fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] > 0){ - //Integrate with saturation - airspeedErrorInt=bound(airspeedErrorInt + airspeedError * dT, - -fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI], - fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]); - } - - //Compute the cross feed from vertical speed to pitch, with saturation - float verticalSpeedToPitchCommandComponent=bound (-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP], - -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX], - fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX] - ); - - //Compute the pitch command as err*Kp + errInt*Ki + X_feed. - pitchCommand= -(airspeedError*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KP] - + airspeedErrorInt*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] - ) + verticalSpeedToPitchCommandComponent; - - fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError; - fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt; - fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand; + /** + * Compute desired pitch command + */ - stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] + pitchCommand, - fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN], - fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]); + if (fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] > 0) { + // Integrate with saturation + airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT, + -fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT] / fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI], + fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT] / fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]); + } - // Error condition: high speed dive - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0; - if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] && // pitch demand is full up - velocityActual.Down > 0 && // we ARE going down - descentspeedDesired < 0 && // we WANT to go up - airspeedError < 0 ) { // we are too fast already - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1; - result = 0; - } + // Compute the cross feed from vertical speed to pitch, with saturation + float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP], + -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX], + fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX] + ); - /** - * Compute desired roll command - */ - if (groundspeedDesired> 1e-6f) { - bearingError = RAD2DEG(atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North)); - } else { - // if we are not supposed to move, run in a circle - bearingError = -90.0f; - } - - if (bearingError<-180.0f) bearingError+=360.0f; - if (bearingError>180.0f) bearingError-=360.0f; + // Compute the pitch command as err*Kp + errInt*Ki + X_feed. + pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KP] + + airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] + ) + verticalSpeedToPitchCommandComponent; - bearingIntegral = bound(bearingIntegral + bearingError * dT * fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_KI], - -fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_ILIMIT], - fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_ILIMIT]); - bearingCommand = (bearingError * fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_KP] + - bearingIntegral); + fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError; + fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt; + fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand; - fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_BEARING] = bearingError; - fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_BEARING] = bearingIntegral; - fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_BEARING] = bearingCommand; - - stabDesired.Roll = bound( fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] + - bearingCommand, - fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN], - fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MAX] ); + stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] + pitchCommand, + fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN], + fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]); - // TODO: find a check to determine loss of directional control. Likely needs some check of derivative + // Error condition: high speed dive + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0; + if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] && // pitch demand is full up + velocityActual.Down > 0 && // we ARE going down + descentspeedDesired < 0 && // we WANT to go up + airspeedError < 0) { // we are too fast already + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1; + result = 0; + } + + /** + * Compute desired roll command + */ + if (groundspeedDesired > 1e-6f) { + bearingError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityActual.East, velocityActual.North)); + } else { + // if we are not supposed to move, run in a circle + bearingError = -90.0f; + } + + if (bearingError < -180.0f) { + bearingError += 360.0f; + } + if (bearingError > 180.0f) { + bearingError -= 360.0f; + } + + bearingIntegral = bound(bearingIntegral + bearingError * dT * fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_KI], + -fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_ILIMIT], + fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_ILIMIT]); + bearingCommand = (bearingError * fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_KP] + + bearingIntegral); + + fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_BEARING] = bearingError; + fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_BEARING] = bearingIntegral; + fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_BEARING] = bearingCommand; + + stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] + + bearingCommand, + fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN], + fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MAX]); + + // TODO: find a check to determine loss of directional control. Likely needs some check of derivative - /** - * Compute desired yaw command - */ - // TODO implement raw control mode for yaw and base on Accels.Y - stabDesired.Yaw = 0; + /** + * Compute desired yaw command + */ + // TODO implement raw control mode for yaw and base on Accels.Y + stabDesired.Yaw = 0; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE; - - StabilizationDesiredSet(&stabDesired); + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE; - FixedWingPathFollowerStatusSet(&fixedwingpathfollowerStatus); + StabilizationDesiredSet(&stabDesired); - return result; + FixedWingPathFollowerStatusSet(&fixedwingpathfollowerStatus); + + return result; } @@ -619,33 +631,31 @@ static uint8_t updateFixedDesiredAttitude() */ static float bound(float val, float min, float max) { - if (val < min) { - val = min; - } else if (val > max) { - val = max; - } - return val; + if (val < min) { + val = min; + } else if (val > max) { + val = max; + } + return val; } static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); - PathDesiredGet(&pathDesired); + FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); + PathDesiredGet(&pathDesired); } static void airspeedActualUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { + AirspeedActualData airspeedActual; + VelocityActualData velocityActual; - AirspeedActualData airspeedActual; - VelocityActualData velocityActual; + AirspeedActualGet(&airspeedActual); + VelocityActualGet(&velocityActual); + float groundspeed = sqrtf(velocityActual.East * velocityActual.East + velocityActual.North * velocityActual.North); - AirspeedActualGet(&airspeedActual); - VelocityActualGet(&velocityActual); - float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North ); - - - indicatedAirspeedActualBias = airspeedActual.CalibratedAirspeed - groundspeed; - // note - we do fly by Indicated Airspeed (== calibrated airspeed) - // however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement. + indicatedAirspeedActualBias = airspeedActual.CalibratedAirspeed - groundspeed; + // note - we do fly by Indicated Airspeed (== calibrated airspeed) + // however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement. } diff --git a/flight/modules/FlightPlan/flightplan.c b/flight/modules/FlightPlan/flightplan.c index 0529f1f0d..545b4b437 100644 --- a/flight/modules/FlightPlan/flightplan.c +++ b/flight/modules/FlightPlan/flightplan.c @@ -41,8 +41,8 @@ // Private constants #define STACK_SIZE_BYTES 1500 -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) -#define MAX_QUEUE_SIZE 2 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define MAX_QUEUE_SIZE 2 // Private types @@ -52,7 +52,7 @@ static xQueueHandle queue; // Private functions static void flightPlanTask(void *parameters); -static void objectUpdatedCb(UAVObjEvent * ev); +static void objectUpdatedCb(UAVObjEvent *ev); // External variables (temporary, TODO: this will be loaded from the SD card) extern unsigned char usrlib_img[]; @@ -62,13 +62,13 @@ extern unsigned char usrlib_img[]; */ int32_t FlightPlanStart() { - taskHandle = NULL; + taskHandle = NULL; - // Start VM thread - xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle); + // Start VM thread + xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle); - return 0; + return 0; } /** @@ -76,22 +76,22 @@ int32_t FlightPlanStart() */ int32_t FlightPlanInitialize() { - taskHandle = NULL; - - FlightPlanStatusInitialize(); - FlightPlanControlInitialize(); - FlightPlanSettingsInitialize(); - - // Listen for object updates - FlightPlanControlConnectCallback(&objectUpdatedCb); + taskHandle = NULL; - // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + FlightPlanStatusInitialize(); + FlightPlanControlInitialize(); + FlightPlanSettingsInitialize(); - // Listen for FlightPlanControl updates - FlightPlanControlConnectQueue(queue); + // Listen for object updates + FlightPlanControlConnectCallback(&objectUpdatedCb); - return 0; + // Create object queue + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + + // Listen for FlightPlanControl updates + FlightPlanControlConnectQueue(queue); + + return 0; } MODULE_INITCALL(FlightPlanInitialize, FlightPlanStart) /** @@ -99,195 +99,151 @@ MODULE_INITCALL(FlightPlanInitialize, FlightPlanStart) */ static void flightPlanTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; - PmReturn_t retval; - FlightPlanStatusData status; - FlightPlanControlData control; + UAVObjEvent ev; + PmReturn_t retval; + FlightPlanStatusData status; + FlightPlanControlData control; - // Setup status object - status.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; - status.ErrorFileID = 0; - status.ErrorLineNum = 0; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; - status.Debug[0] = 0.0; - status.Debug[1] = 0.0; - FlightPlanStatusSet(&status); + // Setup status object + status.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; + status.ErrorFileID = 0; + status.ErrorLineNum = 0; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; + status.Debug[0] = 0.0; + status.Debug[1] = 0.0; + FlightPlanStatusSet(&status); - // Main thread loop - while (1) - { - // Wait for FlightPlanControl updates - while (xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE) ; + // Main thread loop + while (1) { + // Wait for FlightPlanControl updates + while (xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE) { + ; + } - // Get object and check if a start command was sent - FlightPlanControlGet(&control); - if ( control.Command == FLIGHTPLANCONTROL_COMMAND_START ) - { - // Init PyMite - retval = pm_init(MEMSPACE_PROG, usrlib_img); - if (retval == PM_RET_OK) - { - // Update status - FlightPlanStatusGet(&status); - status.Status = FLIGHTPLANSTATUS_STATUS_RUNNING; - FlightPlanStatusSet(&status); - // Run the test script (TODO: load from SD card) - retval = pm_run((uint8_t *)"test"); - // Check if an error or exception was thrown - if (retval == PM_RET_OK || retval == PM_RET_EX_EXIT) - { - status.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; - } - else if (retval == PM_RET_EX) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_EXCEPTION; - } - else if (retval == PM_RET_EX_IO) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IOERROR; - } - else if (retval == PM_RET_EX_ZDIV) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_DIVBYZERO; - } - else if (retval == PM_RET_EX_ASSRT) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ASSERTERROR; - } - else if (retval == PM_RET_EX_ATTR) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ATTRIBUTEERROR; - } - else if (retval == PM_RET_EX_IMPRT) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IMPORTERROR; - } - else if (retval == PM_RET_EX_INDX) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_INDEXERROR; - } - else if (retval == PM_RET_EX_KEY) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_KEYERROR; - } - else if (retval == PM_RET_EX_MEM) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_MEMORYERROR; - } - else if (retval == PM_RET_EX_NAME) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NAMEERROR; - } - else if (retval == PM_RET_EX_SYNTAX) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYNTAXERROR; - } - else if (retval == PM_RET_EX_SYS) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYSTEMERROR; - } - else if (retval == PM_RET_EX_TYPE) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_TYPEERROR; - } - else if (retval == PM_RET_EX_VAL) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VALUEERROR; - } - else if (retval == PM_RET_EX_STOP) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_STOPITERATION; - } - else if (retval == PM_RET_EX_WARN) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_WARNING; - } - else - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_UNKNOWNERROR; - } - // Get file ID and line number of error (if one) - status.ErrorFileID = gVmGlobal.errFileId; - status.ErrorLineNum = gVmGlobal.errLineNum; - } - else - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VMINITERROR; - } + // Get object and check if a start command was sent + FlightPlanControlGet(&control); + if (control.Command == FLIGHTPLANCONTROL_COMMAND_START) { + // Init PyMite + retval = pm_init(MEMSPACE_PROG, usrlib_img); + if (retval == PM_RET_OK) { + // Update status + FlightPlanStatusGet(&status); + status.Status = FLIGHTPLANSTATUS_STATUS_RUNNING; + FlightPlanStatusSet(&status); + // Run the test script (TODO: load from SD card) + retval = pm_run((uint8_t *)"test"); + // Check if an error or exception was thrown + if (retval == PM_RET_OK || retval == PM_RET_EX_EXIT) { + status.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; + } else if (retval == PM_RET_EX) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_EXCEPTION; + } else if (retval == PM_RET_EX_IO) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IOERROR; + } else if (retval == PM_RET_EX_ZDIV) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_DIVBYZERO; + } else if (retval == PM_RET_EX_ASSRT) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ASSERTERROR; + } else if (retval == PM_RET_EX_ATTR) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ATTRIBUTEERROR; + } else if (retval == PM_RET_EX_IMPRT) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IMPORTERROR; + } else if (retval == PM_RET_EX_INDX) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_INDEXERROR; + } else if (retval == PM_RET_EX_KEY) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_KEYERROR; + } else if (retval == PM_RET_EX_MEM) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_MEMORYERROR; + } else if (retval == PM_RET_EX_NAME) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NAMEERROR; + } else if (retval == PM_RET_EX_SYNTAX) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYNTAXERROR; + } else if (retval == PM_RET_EX_SYS) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYSTEMERROR; + } else if (retval == PM_RET_EX_TYPE) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_TYPEERROR; + } else if (retval == PM_RET_EX_VAL) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VALUEERROR; + } else if (retval == PM_RET_EX_STOP) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_STOPITERATION; + } else if (retval == PM_RET_EX_WARN) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_WARNING; + } else { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_UNKNOWNERROR; + } + // Get file ID and line number of error (if one) + status.ErrorFileID = gVmGlobal.errFileId; + status.ErrorLineNum = gVmGlobal.errLineNum; + } else { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VMINITERROR; + } - // Update status object - FlightPlanStatusSet(&status); - } - } + // Update status object + FlightPlanStatusSet(&status); + } + } } /** * Function called in response to object updates. * Used to force kill the VM thread. */ -static void objectUpdatedCb(UAVObjEvent * ev) +static void objectUpdatedCb(UAVObjEvent *ev) { - FlightPlanControlData controlData; - FlightPlanStatusData statusData; + FlightPlanControlData controlData; + FlightPlanStatusData statusData; - // If the object updated was the FlightPlanControl execute requested action - if ( ev->obj == FlightPlanControlHandle() ) - { - // Get data - FlightPlanControlGet(&controlData); - // Execute command - if ( controlData.Command == FLIGHTPLANCONTROL_COMMAND_START ) - { - // Start VM task if not running already - if ( taskHandle == NULL ) - { - xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle); - } - } - else if ( controlData.Command == FLIGHTPLANCONTROL_COMMAND_KILL ) - { - // Force kill VM task if it is already running - // (NOTE: the STOP command is preferred as it allows the script to terminate without killing the VM) - if ( taskHandle != NULL ) - { - // Kill VM - PIOS_TASK_MONITOR_UnregisterTask(TASKINFO_RUNNING_FLIGHTPLAN); - vTaskDelete(taskHandle); - taskHandle = NULL; - // Update status object - statusData.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; - statusData.ErrorFileID = 0; - statusData.ErrorLineNum = 0; - statusData.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; - statusData.Debug[0] = 0.0; - statusData.Debug[1] = 0.0; - FlightPlanStatusSet(&statusData); - } - } - } + // If the object updated was the FlightPlanControl execute requested action + if (ev->obj == FlightPlanControlHandle()) { + // Get data + FlightPlanControlGet(&controlData); + // Execute command + if (controlData.Command == FLIGHTPLANCONTROL_COMMAND_START) { + // Start VM task if not running already + if (taskHandle == NULL) { + xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle); + } + } else if (controlData.Command == FLIGHTPLANCONTROL_COMMAND_KILL) { + // Force kill VM task if it is already running + // (NOTE: the STOP command is preferred as it allows the script to terminate without killing the VM) + if (taskHandle != NULL) { + // Kill VM + PIOS_TASK_MONITOR_UnregisterTask(TASKINFO_RUNNING_FLIGHTPLAN); + vTaskDelete(taskHandle); + taskHandle = NULL; + // Update status object + statusData.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; + statusData.ErrorFileID = 0; + statusData.ErrorLineNum = 0; + statusData.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; + statusData.Debug[0] = 0.0; + statusData.Debug[1] = 0.0; + FlightPlanStatusSet(&statusData); + } + } + } } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index d241998a9..0fdb9142e 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup GSPModule GPS Module * @brief Process GPS information - * @{ + * @{ * * @file GPS.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -56,28 +56,28 @@ static void gpsTask(void *parameters); static void updateSettings(); #ifdef PIOS_GPS_SETS_HOMELOCATION -static void setHomeLocation(GPSPositionData * gpsData); +static void setHomeLocation(GPSPositionData *gpsData); static float GravityAccel(float latitude, float longitude, float altitude); #endif // **************** // Private constants -#define GPS_TIMEOUT_MS 500 +#define GPS_TIMEOUT_MS 500 #ifdef PIOS_GPS_SETS_HOMELOCATION // Unfortunately need a good size stack for the WMM calculation - #define STACK_SIZE_BYTES 850 + #define STACK_SIZE_BYTES 850 #else #if defined(PIOS_GPS_MINIMAL) - #define STACK_SIZE_BYTES 500 + #define STACK_SIZE_BYTES 500 #else - #define STACK_SIZE_BYTES 650 + #define STACK_SIZE_BYTES 650 #endif // PIOS_GPS_MINIMAL #endif // PIOS_GPS_SETS_HOMELOCATION -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // **************** // Private variables @@ -87,7 +87,7 @@ static bool gpsEnabled = false; static xTaskHandle gpsTaskHandle; -static char* gps_rx_buffer; +static char *gps_rx_buffer; static uint32_t timeOfLastCommandMs; static uint32_t timeOfLastUpdateMs; @@ -105,17 +105,17 @@ static struct GPS_RX_STATS gpsRxStats; int32_t GPSStart(void) { - if (gpsEnabled) { - if (gpsPort) { - // Start gps task - xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &gpsTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle); - return 0; - } + if (gpsEnabled) { + if (gpsPort) { + // Start gps task + xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &gpsTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle); + return 0; + } - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); - } - return -1; + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); + } + return -1; } /** @@ -125,68 +125,69 @@ int32_t GPSStart(void) */ int32_t GPSInitialize(void) { - gpsPort = PIOS_COM_GPS; - uint8_t gpsProtocol; + gpsPort = PIOS_COM_GPS; + uint8_t gpsProtocol; #ifdef MODULE_GPS_BUILTIN - gpsEnabled = true; + gpsEnabled = true; #else - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_GPS] == HWSETTINGS_OPTIONALMODULES_ENABLED) - gpsEnabled = true; - else - gpsEnabled = false; + if (optionalModules[HWSETTINGS_OPTIONALMODULES_GPS] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + gpsEnabled = true; + } else { + gpsEnabled = false; + } #endif #if defined(REVOLUTION) - // These objects MUST be initialized for Revolution - // because the rest of the system expects to just - // attach to their queues - GPSPositionInitialize(); - GPSVelocityInitialize(); - GPSTimeInitialize(); - GPSSatellitesInitialize(); - HomeLocationInitialize(); - updateSettings(); + // These objects MUST be initialized for Revolution + // because the rest of the system expects to just + // attach to their queues + GPSPositionInitialize(); + GPSVelocityInitialize(); + GPSTimeInitialize(); + GPSSatellitesInitialize(); + HomeLocationInitialize(); + updateSettings(); #else - if (gpsPort && gpsEnabled) { - GPSPositionInitialize(); - GPSVelocityInitialize(); + if (gpsPort && gpsEnabled) { + GPSPositionInitialize(); + GPSVelocityInitialize(); #if !defined(PIOS_GPS_MINIMAL) - GPSTimeInitialize(); - GPSSatellitesInitialize(); + GPSTimeInitialize(); + GPSSatellitesInitialize(); #endif #ifdef PIOS_GPS_SETS_HOMELOCATION - HomeLocationInitialize(); -#endif - updateSettings(); - } + HomeLocationInitialize(); #endif + updateSettings(); + } +#endif /* if defined(REVOLUTION) */ - if (gpsPort && gpsEnabled) { - SystemSettingsInitialize(); - SystemSettingsGPSDataProtocolGet(&gpsProtocol); - switch (gpsProtocol) { - case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA: - gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); - break; - case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX: - gps_rx_buffer = pvPortMalloc(sizeof(struct UBXPacket)); - break; - default: - gps_rx_buffer = NULL; - } - PIOS_Assert(gps_rx_buffer); + if (gpsPort && gpsEnabled) { + SystemSettingsInitialize(); + SystemSettingsGPSDataProtocolGet(&gpsProtocol); + switch (gpsProtocol) { + case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA: + gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); + break; + case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX: + gps_rx_buffer = pvPortMalloc(sizeof(struct UBXPacket)); + break; + default: + gps_rx_buffer = NULL; + } + PIOS_Assert(gps_rx_buffer); - return 0; - } + return 0; + } - return -1; + return -1; } MODULE_INITCALL(GPSInitialize, GPSStart) @@ -198,79 +199,78 @@ MODULE_INITCALL(GPSInitialize, GPSStart) static void gpsTask(__attribute__((unused)) void *parameters) { - portTickType xDelay = 100 / portTICK_RATE_MS; - uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + portTickType xDelay = 100 / portTICK_RATE_MS; + uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - GPSPositionData gpsposition; - uint8_t gpsProtocol; + GPSPositionData gpsposition; + uint8_t gpsProtocol; - SystemSettingsGPSDataProtocolGet(&gpsProtocol); + SystemSettingsGPSDataProtocolGet(&gpsProtocol); - timeOfLastUpdateMs = timeNowMs; - timeOfLastCommandMs = timeNowMs; + timeOfLastUpdateMs = timeNowMs; + timeOfLastCommandMs = timeNowMs; - GPSPositionGet(&gpsposition); - // Loop forever - while (1) - { - uint8_t c; + GPSPositionGet(&gpsposition); + // Loop forever + while (1) { + uint8_t c; - // This blocks the task until there is something on the buffer - while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) - { - int res; - switch (gpsProtocol) { + // This blocks the task until there is something on the buffer + while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) { + int res; + switch (gpsProtocol) { #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) - case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA: - res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats); - break; + case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA: + res = parse_nmea_stream(c, gps_rx_buffer, &gpsposition, &gpsRxStats); + break; #endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) - case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX: - res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats); - break; + case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX: + res = parse_ubx_stream(c, gps_rx_buffer, &gpsposition, &gpsRxStats); + break; #endif - default: - res = NO_PARSER; // this should not happen - break; - } + default: + res = NO_PARSER; // this should not happen + break; + } - if (res == PARSER_COMPLETE) { - timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - timeOfLastUpdateMs = timeNowMs; - timeOfLastCommandMs = timeNowMs; - } - } + if (res == PARSER_COMPLETE) { + timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + timeOfLastUpdateMs = timeNowMs; + timeOfLastCommandMs = timeNowMs; + } + } - // Check for GPS timeout - timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) { - // we have not received any valid GPS sentences for a while. - // either the GPS is not plugged in or a hardware problem or the GPS has locked up. - uint8_t status = GPSPOSITION_STATUS_NOGPS; - GPSPositionStatusSet(&status); - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); - } else { - // we appear to be receiving GPS sentences OK, we've had an update - //criteria for GPS-OK taken from this post... - //http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 - if ((gpsposition.PDOP < 3.5f) && (gpsposition.Satellites >= 7) && - (gpsposition.Status == GPSPOSITION_STATUS_FIX3D)) { - AlarmsClear(SYSTEMALARMS_ALARM_GPS); + // Check for GPS timeout + timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) { + // we have not received any valid GPS sentences for a while. + // either the GPS is not plugged in or a hardware problem or the GPS has locked up. + uint8_t status = GPSPOSITION_STATUS_NOGPS; + GPSPositionStatusSet(&status); + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); + } else { + // we appear to be receiving GPS sentences OK, we've had an update + // criteria for GPS-OK taken from this post... + // http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 + if ((gpsposition.PDOP < 3.5f) && (gpsposition.Satellites >= 7) && + (gpsposition.Status == GPSPOSITION_STATUS_FIX3D)) { + AlarmsClear(SYSTEMALARMS_ALARM_GPS); #ifdef PIOS_GPS_SETS_HOMELOCATION - HomeLocationData home; - HomeLocationGet(&home); + HomeLocationData home; + HomeLocationGet(&home); - if (home.Set == HOMELOCATION_SET_FALSE) - setHomeLocation(&gpsposition); + if (home.Set == HOMELOCATION_SET_FALSE) { + setHomeLocation(&gpsposition); + } #endif - } else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D) - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); - else - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); - } - - } + } else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D) { + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); + } + } + } } #ifdef PIOS_GPS_SETS_HOMELOCATION @@ -279,51 +279,51 @@ static void gpsTask(__attribute__((unused)) void *parameters) */ static float GravityAccel(float latitude, __attribute__((unused)) float longitude, float altitude) { - /* WGS84 gravity model. The effect of gravity over latitude is strong - * enough to change the estimated accelerometer bias in those apps. */ - float sinsq = sinf(latitude); - sinsq *= sinsq; - /* Likewise, over the altitude range of a high-altitude balloon, the effect - * due to change in altitude can also affect the model. */ - return (9.7803267714f * (1.0f + 0.00193185138639f*sinsq) / sqrtf(1.0f - 0.00669437999013f*sinsq) - - 3.086e-6f*altitude); + /* WGS84 gravity model. The effect of gravity over latitude is strong + * enough to change the estimated accelerometer bias in those apps. */ + float sinsq = sinf(latitude); + + sinsq *= sinsq; + /* Likewise, over the altitude range of a high-altitude balloon, the effect + * due to change in altitude can also affect the model. */ + return 9.7803267714f * (1.0f + 0.00193185138639f * sinsq) / sqrtf(1.0f - 0.00669437999013f * sinsq) + - 3.086e-6f * altitude; } // **************** -static void setHomeLocation(GPSPositionData * gpsData) +static void setHomeLocation(GPSPositionData *gpsData) { - HomeLocationData home; - HomeLocationGet(&home); - GPSTimeData gps; - GPSTimeGet(&gps); + HomeLocationData home; - if (gps.Year >= 2000) - { - /* Store LLA */ - home.Latitude = gpsData->Latitude; - home.Longitude = gpsData->Longitude; - home.Altitude = gpsData->Altitude + gpsData->GeoidSeparation; + HomeLocationGet(&home); + GPSTimeData gps; + GPSTimeGet(&gps); - /* Compute home ECEF coordinates and the rotation matrix into NED - * Note that floats are used here - they should give enough precision - * for this application.*/ + if (gps.Year >= 2000) { + /* Store LLA */ + home.Latitude = gpsData->Latitude; + home.Longitude = gpsData->Longitude; + home.Altitude = gpsData->Altitude + gpsData->GeoidSeparation; - float LLA[3] = { (home.Latitude)/10e6f, (home.Longitude)/10e6f, (home.Altitude) }; + /* Compute home ECEF coordinates and the rotation matrix into NED + * Note that floats are used here - they should give enough precision + * for this application.*/ - /* Compute magnetic flux direction at home location */ - if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) == 0) { + float LLA[3] = { (home.Latitude) / 10e6f, (home.Longitude) / 10e6f, (home.Altitude) }; - /*Compute local acceleration due to gravity. Vehicles that span a very large - * range of altitude (say, weather balloons) may need to update this during the - * flight. */ - home.g_e = GravityAccel(LLA[0], LLA[1], LLA[2]); - home.Set = HOMELOCATION_SET_TRUE; - HomeLocationSet(&home); - } - } + /* Compute magnetic flux direction at home location */ + if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) == 0) { + /*Compute local acceleration due to gravity. Vehicles that span a very large + * range of altitude (say, weather balloons) may need to update this during the + * flight. */ + home.g_e = GravityAccel(LLA[0], LLA[1], LLA[2]); + home.Set = HOMELOCATION_SET_TRUE; + HomeLocationSet(&home); + } + } } -#endif +#endif /* ifdef PIOS_GPS_SETS_HOMELOCATION */ /** * Update the GPS settings, called on startup. @@ -334,40 +334,39 @@ static void setHomeLocation(GPSPositionData * gpsData) */ static void updateSettings() { - if (gpsPort) { + if (gpsPort) { + // Retrieve settings + uint8_t speed; + HwSettingsGPSSpeedGet(&speed); - // Retrieve settings - uint8_t speed; - HwSettingsGPSSpeedGet(&speed); - - // Set port speed - switch (speed) { - case HWSETTINGS_GPSSPEED_2400: - PIOS_COM_ChangeBaud(gpsPort, 2400); - break; - case HWSETTINGS_GPSSPEED_4800: - PIOS_COM_ChangeBaud(gpsPort, 4800); - break; - case HWSETTINGS_GPSSPEED_9600: - PIOS_COM_ChangeBaud(gpsPort, 9600); - break; - case HWSETTINGS_GPSSPEED_19200: - PIOS_COM_ChangeBaud(gpsPort, 19200); - break; - case HWSETTINGS_GPSSPEED_38400: - PIOS_COM_ChangeBaud(gpsPort, 38400); - break; - case HWSETTINGS_GPSSPEED_57600: - PIOS_COM_ChangeBaud(gpsPort, 57600); - break; - case HWSETTINGS_GPSSPEED_115200: - PIOS_COM_ChangeBaud(gpsPort, 115200); - break; - } - } + // Set port speed + switch (speed) { + case HWSETTINGS_GPSSPEED_2400: + PIOS_COM_ChangeBaud(gpsPort, 2400); + break; + case HWSETTINGS_GPSSPEED_4800: + PIOS_COM_ChangeBaud(gpsPort, 4800); + break; + case HWSETTINGS_GPSSPEED_9600: + PIOS_COM_ChangeBaud(gpsPort, 9600); + break; + case HWSETTINGS_GPSSPEED_19200: + PIOS_COM_ChangeBaud(gpsPort, 19200); + break; + case HWSETTINGS_GPSSPEED_38400: + PIOS_COM_ChangeBaud(gpsPort, 38400); + break; + case HWSETTINGS_GPSSPEED_57600: + PIOS_COM_ChangeBaud(gpsPort, 57600); + break; + case HWSETTINGS_GPSSPEED_115200: + PIOS_COM_ChangeBaud(gpsPort, 115200); + break; + } + } } -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/modules/GPS/NMEA.c b/flight/modules/GPS/NMEA.c index 98cfe48cc..d5be3131d 100644 --- a/flight/modules/GPS/NMEA.c +++ b/flight/modules/GPS/NMEA.c @@ -39,24 +39,23 @@ #include "gpssatellites.h" #include "GPS.h" -//#define ENABLE_DEBUG_MSG ///< define to enable debug-messages -#define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is ued for debug-messages - +// #define ENABLE_DEBUG_MSG ///< define to enable debug-messages +#define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is ued for debug-messages // Debugging #ifdef ENABLE_DEBUG_MSG -//#define DEBUG_MSG_IN ///< define to display the incoming NMEA messages -//#define DEBUG_PARAMS ///< define to display the incoming NMEA messages split into its parameters -//#define DEBUG_MGSID_IN ///< define to display the the names of the incoming NMEA messages -//#define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages -//#define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages -//#define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages -//#define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages -//#define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages -//#define NMEA_DEBUG_GSV ///< define to enable debug of GSV messages -//#define NMEA_DEBUG_ZDA ///< define to enable debug of ZDA messages -#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__) +// #define DEBUG_MSG_IN ///< define to display the incoming NMEA messages +// #define DEBUG_PARAMS ///< define to display the incoming NMEA messages split into its parameters +// #define DEBUG_MGSID_IN ///< define to display the the names of the incoming NMEA messages +// #define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages +// #define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages +// #define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages +// #define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages +// #define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages +// #define NMEA_DEBUG_GSV ///< define to enable debug of GSV messages +// #define NMEA_DEBUG_ZDA ///< define to enable debug of ZDA messages +#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format,##__VA_ARGS__) #else #define DEBUG_MSG(format, ...) #endif @@ -65,148 +64,137 @@ /* NMEA sentence parsers */ struct nmea_parser { - const char *prefix; - bool(*handler) (GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); + const char *prefix; + bool (*handler)(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); }; -static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); -static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); -static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); -static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); +static bool nmeaProcessGPGGA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +static bool nmeaProcessGPRMC(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +static bool nmeaProcessGPVTG(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +static bool nmeaProcessGPGSA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); #if !defined(PIOS_GPS_MINIMAL) - static bool nmeaProcessGPZDA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); -#endif //PIOS_GPS_MINIMAL +static bool nmeaProcessGPZDA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +static bool nmeaProcessGPGSV(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +#endif // PIOS_GPS_MINIMAL static const struct nmea_parser nmea_parsers[] = { - { - .prefix = "GPGGA", - .handler = nmeaProcessGPGGA, - }, - { - .prefix = "GPVTG", - .handler = nmeaProcessGPVTG, - }, - { - .prefix = "GPGSA", - .handler = nmeaProcessGPGSA, - }, - { - .prefix = "GPRMC", - .handler = nmeaProcessGPRMC, - }, + { + .prefix = "GPGGA", + .handler = nmeaProcessGPGGA, + }, + { + .prefix = "GPVTG", + .handler = nmeaProcessGPVTG, + }, + { + .prefix = "GPGSA", + .handler = nmeaProcessGPGSA, + }, + { + .prefix = "GPRMC", + .handler = nmeaProcessGPRMC, + }, #if !defined(PIOS_GPS_MINIMAL) - { - .prefix = "GPZDA", - .handler = nmeaProcessGPZDA, - }, - { - .prefix = "GPGSV", - .handler = nmeaProcessGPGSV, - }, -#endif //PIOS_GPS_MINIMAL + { + .prefix = "GPZDA", + .handler = nmeaProcessGPZDA, + }, + { + .prefix = "GPGSV", + .handler = nmeaProcessGPGSV, + }, +#endif // PIOS_GPS_MINIMAL }; -int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats) +int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats) { - static uint8_t rx_count = 0; - static bool start_flag = false; - static bool found_cr = false; + static uint8_t rx_count = 0; + static bool start_flag = false; + static bool found_cr = false; - // detect start while acquiring stream - if (!start_flag && (c == '$')) // NMEA identifier found - { - start_flag = true; - found_cr = false; - rx_count = 0; - } - else - if (!start_flag) - return PARSER_ERROR; + // detect start while acquiring stream + if (!start_flag && (c == '$')) { // NMEA identifier found + start_flag = true; + found_cr = false; + rx_count = 0; + } else if (!start_flag) { + return PARSER_ERROR; + } - if (rx_count >= NMEA_MAX_PACKET_LENGTH) - { - // The buffer is already full and we haven't found a valid NMEA sentence. - // Flush the buffer and note the overflow event. - gpsRxStats->gpsRxOverflow++; - start_flag = false; - found_cr = false; - rx_count = 0; - return PARSER_OVERRUN; - } - else - { - gps_rx_buffer[rx_count] = c; - rx_count++; - } + if (rx_count >= NMEA_MAX_PACKET_LENGTH) { + // The buffer is already full and we haven't found a valid NMEA sentence. + // Flush the buffer and note the overflow event. + gpsRxStats->gpsRxOverflow++; + start_flag = false; + found_cr = false; + rx_count = 0; + return PARSER_OVERRUN; + } else { + gps_rx_buffer[rx_count] = c; + rx_count++; + } - // look for ending '\r\n' sequence - if (!found_cr && (c == '\r') ) - found_cr = true; - else - if (found_cr && (c != '\n') ) - found_cr = false; // false end flag - else - if (found_cr && (c == '\n') ) - { - // The NMEA functions require a zero-terminated string - // As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n - gps_rx_buffer[rx_count-2] = 0; + // look for ending '\r\n' sequence + if (!found_cr && (c == '\r')) { + found_cr = true; + } else if (found_cr && (c != '\n')) { + found_cr = false; // false end flag + } else if (found_cr && (c == '\n')) { + // The NMEA functions require a zero-terminated string + // As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n + gps_rx_buffer[rx_count - 2] = 0; - // prepare to parse next sentence - start_flag = false; - found_cr = false; - rx_count = 0; - // Our rxBuffer must look like this now: - // [0] = '$' - // ... = zero or more bytes of sentence payload - // [end_pos - 1] = '\r' - // [end_pos] = '\n' - // - // Prepare to consume the sentence from the buffer + // prepare to parse next sentence + start_flag = false; + found_cr = false; + rx_count = 0; + // Our rxBuffer must look like this now: + // [0] = '$' + // ... = zero or more bytes of sentence payload + // [end_pos - 1] = '\r' + // [end_pos] = '\n' + // + // Prepare to consume the sentence from the buffer - // Validate the checksum over the sentence - if (!NMEA_checksum(&gps_rx_buffer[1])) - { // Invalid checksum. May indicate dropped characters on Rx. - //PIOS_DEBUG_PinHigh(2); - gpsRxStats->gpsRxChkSumError++; - //PIOS_DEBUG_PinLow(2); - return PARSER_ERROR; - } - else - { // Valid checksum, use this packet to update the GPS position - if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) { - //PIOS_DEBUG_PinHigh(2); - gpsRxStats->gpsRxParserError++; - //PIOS_DEBUG_PinLow(2); - } - else - gpsRxStats->gpsRxReceived++;; + // Validate the checksum over the sentence + if (!NMEA_checksum(&gps_rx_buffer[1])) { // Invalid checksum. May indicate dropped characters on Rx. + // PIOS_DEBUG_PinHigh(2); + gpsRxStats->gpsRxChkSumError++; + // PIOS_DEBUG_PinLow(2); + return PARSER_ERROR; + } else { // Valid checksum, use this packet to update the GPS position + if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) { + // PIOS_DEBUG_PinHigh(2); + gpsRxStats->gpsRxParserError++; + // PIOS_DEBUG_PinLow(2); + } else { + gpsRxStats->gpsRxReceived++; + }; - return PARSER_COMPLETE; - } - } - return PARSER_INCOMPLETE; + return PARSER_COMPLETE; + } + } + return PARSER_INCOMPLETE; } static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix) { - if (!prefix) { - return (NULL); - } + if (!prefix) { + return NULL; + } - for (uint8_t i = 0; i < NELEMENTS(nmea_parsers); i++) { - const struct nmea_parser *parser = &nmea_parsers[i]; + for (uint8_t i = 0; i < NELEMENTS(nmea_parsers); i++) { + const struct nmea_parser *parser = &nmea_parsers[i]; - /* Use strcmp to check for exact equality over the entire prefix */ - if (!strcmp(prefix, parser->prefix)) { - /* Found an appropriate parser */ - return (parser); - } - } + /* Use strcmp to check for exact equality over the entire prefix */ + if (!strcmp(prefix, parser->prefix)) { + /* Found an appropriate parser */ + return parser; + } + } - /* No matching parser for this prefix */ - return (NULL); + /* No matching parser for this prefix */ + return NULL; } /** @@ -217,26 +205,26 @@ static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix) */ bool NMEA_checksum(char *nmea_sentence) { - uint8_t checksum_computed = 0; - uint8_t checksum_received; + uint8_t checksum_computed = 0; + uint8_t checksum_received; - while (*nmea_sentence != '\0' && *nmea_sentence != '*') { - checksum_computed ^= *nmea_sentence; - nmea_sentence++; - } + while (*nmea_sentence != '\0' && *nmea_sentence != '*') { + checksum_computed ^= *nmea_sentence; + nmea_sentence++; + } - /* Make sure we're now pointing at the checksum */ - if (*nmea_sentence == '\0') { - /* Buffer ran out before we found a checksum marker */ - return false; - } + /* Make sure we're now pointing at the checksum */ + if (*nmea_sentence == '\0') { + /* Buffer ran out before we found a checksum marker */ + return false; + } - /* Load the checksum from the buffer */ - checksum_received = strtol(nmea_sentence + 1, NULL, 16); + /* Load the checksum from the buffer */ + checksum_received = strtol(nmea_sentence + 1, NULL, 16); - //PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum_computed); + // PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum_computed); - return (checksum_computed == checksum_received); + return checksum_computed == checksum_received; } /* @@ -252,49 +240,49 @@ bool NMEA_checksum(char *nmea_sentence) * The fract_units field indicates the units of the fractional part as * 1 whole = 10^fract_units fract */ -static bool NMEA_parse_real(int32_t * whole, uint32_t * fract, uint8_t * fract_units, char *field) +static bool NMEA_parse_real(int32_t *whole, uint32_t *fract, uint8_t *fract_units, char *field) { - char *s = field; - char *field_w; - char *field_f; + char *s = field; + char *field_w; + char *field_f; - PIOS_DEBUG_Assert(whole); - PIOS_DEBUG_Assert(fract); - PIOS_DEBUG_Assert(fract_units); + PIOS_DEBUG_Assert(whole); + PIOS_DEBUG_Assert(fract); + PIOS_DEBUG_Assert(fract_units); - field_w = strsep(&s, "."); - field_f = s; + field_w = strsep(&s, "."); + field_f = s; - *whole = strtol(field_w, NULL, 10); + *whole = strtol(field_w, NULL, 10); - if (field_w) { - /* decimal was found so we may have a fractional part */ - *fract = strtoul(field_f, NULL, 10); - *fract_units = strlen(field_f); - } else { - /* no decimal was found, fractional part is zero */ - *fract = 0; - *fract_units = 0; - } + if (field_w) { + /* decimal was found so we may have a fractional part */ + *fract = strtoul(field_f, NULL, 10); + *fract_units = strlen(field_f); + } else { + /* no decimal was found, fractional part is zero */ + *fract = 0; + *fract_units = 0; + } - return true; + return true; } static float NMEA_real_to_float(char *nmea_real) { - int32_t whole; - uint32_t fract; - uint8_t fract_units; + int32_t whole; + uint32_t fract; + uint8_t fract_units; - /* Sanity checks */ - PIOS_DEBUG_Assert(nmea_real); + /* Sanity checks */ + PIOS_DEBUG_Assert(nmea_real); - if (!NMEA_parse_real(&whole, &fract, &fract_units, nmea_real)) { - return false; - } + if (!NMEA_parse_real(&whole, &fract, &fract_units, nmea_real)) { + return false; + } - /* Convert to float */ - return (((float)whole) + fract * powf(10.0f, -fract_units)); + /* Convert to float */ + return ((float)whole) + fract * powf(10.0f, -fract_units); } /* @@ -302,62 +290,63 @@ static float NMEA_real_to_float(char *nmea_real) * DD[D]MM.mmmm[mm] * into a fixed-point representation in units of (degrees * 1e-7) */ -static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool negative) +static bool NMEA_latlon_to_fixed_point(int32_t *latlon, char *nmea_latlon, bool negative) { - int32_t num_DDDMM; - uint32_t num_m; - uint8_t units; + int32_t num_DDDMM; + uint32_t num_m; + uint8_t units; - /* Sanity checks */ - PIOS_DEBUG_Assert(nmea_latlon); - PIOS_DEBUG_Assert(latlon); + /* Sanity checks */ + PIOS_DEBUG_Assert(nmea_latlon); + PIOS_DEBUG_Assert(latlon); - if (*nmea_latlon == '\0') { /* empty lat/lon field */ - return false; - } + if (*nmea_latlon == '\0') { /* empty lat/lon field */ + return false; + } - if (!NMEA_parse_real(&num_DDDMM, &num_m, &units, nmea_latlon)) { - return false; - } + if (!NMEA_parse_real(&num_DDDMM, &num_m, &units, nmea_latlon)) { + return false; + } - /* scale up the mmmm[mm] field apropriately depending on # of digits */ - /* not using 1eN notation because that forces fixed point and lost precision */ - switch (units) { - case 0: - /* no digits, value is zero so no scaling */ - break; - case 1: /* m */ - num_m *= 1000000; /* m000000 */ - break; - case 2: /* mm */ - num_m *= 100000; /* mm00000 */ - break; - case 3: /* mmm */ - num_m *= 10000; /* mmm0000 */ - break; - case 4: /* mmmm */ - num_m *= 1000; /* mmmm000 */ - break; - case 5: /* mmmmm */ - num_m *= 100; /* mmmmm00 */ - break; - case 6: /* mmmmmm */ - num_m *= 10; /* mmmmmm0 */ - break; - default: - /* unhandled format */ - num_m = 0.0f; - break; - } + /* scale up the mmmm[mm] field apropriately depending on # of digits */ + /* not using 1eN notation because that forces fixed point and lost precision */ + switch (units) { + case 0: + /* no digits, value is zero so no scaling */ + break; + case 1: /* m */ + num_m *= 1000000; /* m000000 */ + break; + case 2: /* mm */ + num_m *= 100000; /* mm00000 */ + break; + case 3: /* mmm */ + num_m *= 10000; /* mmm0000 */ + break; + case 4: /* mmmm */ + num_m *= 1000; /* mmmm000 */ + break; + case 5: /* mmmmm */ + num_m *= 100; /* mmmmm00 */ + break; + case 6: /* mmmmmm */ + num_m *= 10; /* mmmmmm0 */ + break; + default: + /* unhandled format */ + num_m = 0.0f; + break; + } - *latlon = (num_DDDMM / 100) * 10000000; /* scale the whole degrees */ - *latlon += (num_DDDMM % 100) * 10000000 / 60; /* add in the scaled decimal whole minutes */ - *latlon += num_m / 60; /* add in the scaled decimal fractional minutes */ + *latlon = (num_DDDMM / 100) * 10000000; /* scale the whole degrees */ + *latlon += (num_DDDMM % 100) * 10000000 / 60; /* add in the scaled decimal whole minutes */ + *latlon += num_m / 60; /* add in the scaled decimal fractional minutes */ - if (negative) - *latlon *= -1; + if (negative) { + *latlon *= -1; + } - return true; + return true; } @@ -369,86 +358,87 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool */ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) { - char* p = nmea_sentence; - char* params[MAX_NB_PARAMS]; - uint8_t nbParams; + char *p = nmea_sentence; + char *params[MAX_NB_PARAMS]; + uint8_t nbParams; #ifdef DEBUG_MSG_IN - DEBUG_MSG("\"%s\"\n", nmea_sentence); + DEBUG_MSG("\"%s\"\n", nmea_sentence); #endif - // Split the nmea sentence it its parameters, separated by "," - // Sample NMEA message: "GPRMC,000131.736,V,,,,,0.00,0.00,060180,,,N*43" + // Split the nmea sentence it its parameters, separated by "," + // Sample NMEA message: "GPRMC,000131.736,V,,,,,0.00,0.00,060180,,,N*43" - // The first parameter starts at the beginning of the message - params[0] = p; - nbParams = 1; - while (*p != 0) { - if (*p == '*') { - // After the * comes the "CRC", we are done, - *p = 0; // Zero-terminate this parameter - break; - } else if (*p == ',') { - // This is the end of this parameter - *p = 0; // Zero-terminate this parameter - // Start new parameter - if (nbParams==MAX_NB_PARAMS) - break; - params[nbParams] = p+1; // For sure there is something at p+1 because at p there is "," - nbParams++; - } - p++; - } + // The first parameter starts at the beginning of the message + params[0] = p; + nbParams = 1; + while (*p != 0) { + if (*p == '*') { + // After the * comes the "CRC", we are done, + *p = 0; // Zero-terminate this parameter + break; + } else if (*p == ',') { + // This is the end of this parameter + *p = 0; // Zero-terminate this parameter + // Start new parameter + if (nbParams == MAX_NB_PARAMS) { + break; + } + params[nbParams] = p + 1; // For sure there is something at p+1 because at p there is "," + nbParams++; + } + p++; + } #ifdef DEBUG_PARAMS - int i; - for (i=0;ihandler(GpsData, &gpsDataUpdated, params, nbParams)) { - // Parse failed - DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]); - if (gpsDataUpdated && (GpsData->Status == GPSPOSITION_STATUS_NOFIX)) { - GPSPositionSet(GpsData); - } - return false; - } + if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) { + // Parse failed + DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]); + if (gpsDataUpdated && (GpsData->Status == GPSPOSITION_STATUS_NOFIX)) { + GPSPositionSet(GpsData); + } + return false; + } - // All is fine :) Update object if data has changed - if (gpsDataUpdated) { - #ifdef DEBUG_MGSID_IN - DEBUG_MSG("U"); - #endif - GPSPositionSet(GpsData); - } + // All is fine :) Update object if data has changed + if (gpsDataUpdated) { + #ifdef DEBUG_MGSID_IN + DEBUG_MSG("U"); + #endif + GPSPositionSet(GpsData); + } - #ifdef DEBUG_MGSID_IN - DEBUG_MSG("\n"); - #endif - return true; + #ifdef DEBUG_MGSID_IN + DEBUG_MSG("\n"); + #endif + return true; } @@ -457,52 +447,52 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) * \param[in] A pointer to a GPSPosition UAVObject to be updated. * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam) +static bool nmeaProcessGPGGA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { - - if (nbParam != 15) - return false; + if (nbParam != 15) { + return false; + } #ifdef NMEA_DEBUG_GGA - DEBUG_MSG("\n UTC=%s\n", param[1]); - DEBUG_MSG(" Lat=%s %s\n", param[2], param[3]); - DEBUG_MSG(" Long=%s %s\n", param[4], param[5]); - DEBUG_MSG(" Fix=%s\n", param[6]); - DEBUG_MSG(" Sat=%s\n", param[7]); - DEBUG_MSG(" HDOP=%s\n", param[8]); - DEBUG_MSG(" Alt=%s %s\n", param[9], param[10]); - DEBUG_MSG(" GeoidSep=%s %s\n\n", param[11]); + DEBUG_MSG("\n UTC=%s\n", param[1]); + DEBUG_MSG(" Lat=%s %s\n", param[2], param[3]); + DEBUG_MSG(" Long=%s %s\n", param[4], param[5]); + DEBUG_MSG(" Fix=%s\n", param[6]); + DEBUG_MSG(" Sat=%s\n", param[7]); + DEBUG_MSG(" HDOP=%s\n", param[8]); + DEBUG_MSG(" Alt=%s %s\n", param[9], param[10]); + DEBUG_MSG(" GeoidSep=%s %s\n\n", param[11]); #endif - *gpsDataUpdated = true; + *gpsDataUpdated = true; - // check for invalid GPS fix - // do this first to make sure we get this information, even if later checks exit - // this function early - if (param[6][0] == '0') { - GpsData->Status = GPSPOSITION_STATUS_NOFIX; // treat invalid fix as NOFIX - } + // check for invalid GPS fix + // do this first to make sure we get this information, even if later checks exit + // this function early + if (param[6][0] == '0') { + GpsData->Status = GPSPOSITION_STATUS_NOFIX; // treat invalid fix as NOFIX + } - // get latitude [DDMM.mmmmm] [N|S] - if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[2], param[3][0] == 'S')) { - return false; - } + // get latitude [DDMM.mmmmm] [N|S] + if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[2], param[3][0] == 'S')) { + return false; + } - // get longitude [dddmm.mmmmm] [E|W] - if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, param[4], param[5][0] == 'W')) { - return false; - } + // get longitude [dddmm.mmmmm] [E|W] + if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, param[4], param[5][0] == 'W')) { + return false; + } - // get number of satellites used in GPS solution - GpsData->Satellites = atoi(param[7]); + // get number of satellites used in GPS solution + GpsData->Satellites = atoi(param[7]); - // get altitude (in meters mm.m) - GpsData->Altitude = NMEA_real_to_float(param[9]); + // get altitude (in meters mm.m) + GpsData->Altitude = NMEA_real_to_float(param[9]); - // geoid separation - GpsData->GeoidSeparation = NMEA_real_to_float(param[11]); + // geoid separation + GpsData->GeoidSeparation = NMEA_real_to_float(param[11]); - return true; + return true; } /** @@ -510,66 +500,67 @@ static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch * \param[in] A pointer to a GPSPosition UAVObject to be updated. * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam) +static bool nmeaProcessGPRMC(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { - if (nbParam != 13) - return false; + if (nbParam != 13) { + return false; + } #ifdef NMEA_DEBUG_RMC - DEBUG_MSG("\n UTC=%s\n", param[1]); - DEBUG_MSG(" Lat=%s %s\n", param[3], param[4]); - DEBUG_MSG(" Long=%s %s\n", param[5], param[6]); - DEBUG_MSG(" Speed=%s\n", param[7]); - DEBUG_MSG(" Course=%s\n", param[8]); - DEBUG_MSG(" DateOfFix=%s\n\n", param[9]); + DEBUG_MSG("\n UTC=%s\n", param[1]); + DEBUG_MSG(" Lat=%s %s\n", param[3], param[4]); + DEBUG_MSG(" Long=%s %s\n", param[5], param[6]); + DEBUG_MSG(" Speed=%s\n", param[7]); + DEBUG_MSG(" Course=%s\n", param[8]); + DEBUG_MSG(" DateOfFix=%s\n\n", param[9]); #endif - *gpsDataUpdated = false; + *gpsDataUpdated = false; #if !defined(PIOS_GPS_MINIMAL) - GPSTimeData gpst; - GPSTimeGet(&gpst); + GPSTimeData gpst; + GPSTimeGet(&gpst); - // get UTC time [hhmmss.sss] - float hms = NMEA_real_to_float(param[1]); - gpst.Second = (int)hms % 100; - gpst.Minute = (((int)hms - gpst.Second) / 100) % 100; - gpst.Hour = (int)hms / 10000; -#endif //PIOS_GPS_MINIMAL + // get UTC time [hhmmss.sss] + float hms = NMEA_real_to_float(param[1]); + gpst.Second = (int)hms % 100; + gpst.Minute = (((int)hms - gpst.Second) / 100) % 100; + gpst.Hour = (int)hms / 10000; +#endif // PIOS_GPS_MINIMAL - // don't process void sentences - if (param[2][0] == 'V') { - return false; - } + // don't process void sentences + if (param[2][0] == 'V') { + return false; + } - // get latitude [DDMM.mmmmm] [N|S] - if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[3], param[4][0] == 'S')) { - return false; - } + // get latitude [DDMM.mmmmm] [N|S] + if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[3], param[4][0] == 'S')) { + return false; + } - // get longitude [dddmm.mmmmm] [E|W] - if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, param[5], param[6][0] == 'W')) { - return false; - } + // get longitude [dddmm.mmmmm] [E|W] + if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, param[5], param[6][0] == 'W')) { + return false; + } - // get speed in knots - GpsData->Groundspeed = NMEA_real_to_float(param[7]) * 0.51444f; // to m/s + // get speed in knots + GpsData->Groundspeed = NMEA_real_to_float(param[7]) * 0.51444f; // to m/s - // get True course - GpsData->Heading = NMEA_real_to_float(param[8]); + // get True course + GpsData->Heading = NMEA_real_to_float(param[8]); #if !defined(PIOS_GPS_MINIMAL) - // get Date of fix - // TODO: Should really not use a float here to be safe - float date = NMEA_real_to_float(param[9]); - gpst.Year = (int)date % 100; - gpst.Month = (((int)date - gpst.Year) / 100) % 100; - gpst.Day = (int)(date / 10000); - gpst.Year += 2000; - GPSTimeSet(&gpst); -#endif //PIOS_GPS_MINIMAL + // get Date of fix + // TODO: Should really not use a float here to be safe + float date = NMEA_real_to_float(param[9]); + gpst.Year = (int)date % 100; + gpst.Month = (((int)date - gpst.Year) / 100) % 100; + gpst.Day = (int)(date / 10000); + gpst.Year += 2000; + GPSTimeSet(&gpst); +#endif // PIOS_GPS_MINIMAL - return true; + return true; } /** @@ -577,22 +568,23 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, ch * \param[in] A pointer to a GPSPosition UAVObject to be updated. * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam) +static bool nmeaProcessGPVTG(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { - if (nbParam != 9 && nbParam != 10 /*GTOP GPS seems to gemnerate an extra parameter...*/) - return false; + if (nbParam != 9 && nbParam != 10 /*GTOP GPS seems to gemnerate an extra parameter...*/) { + return false; + } #ifdef NMEA_DEBUG_RMC - DEBUG_MSG("\n Heading=%s %s\n", param[1], param[2]); - DEBUG_MSG(" GroundSpeed=%s %s\n", param[5], param[6]); + DEBUG_MSG("\n Heading=%s %s\n", param[1], param[2]); + DEBUG_MSG(" GroundSpeed=%s %s\n", param[5], param[6]); #endif - *gpsDataUpdated = false; + *gpsDataUpdated = false; - GpsData->Heading = NMEA_real_to_float(param[1]); - GpsData->Groundspeed = NMEA_real_to_float(param[5]) * 0.51444f; // to m/s + GpsData->Heading = NMEA_real_to_float(param[1]); + GpsData->Groundspeed = NMEA_real_to_float(param[5]) * 0.51444f; // to m/s - return true; + return true; } #if !defined(PIOS_GPS_MINIMAL) @@ -601,35 +593,36 @@ static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, ch * \param[in] A pointer to a GPSPosition UAVObject to be updated (unused). * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPZDA(__attribute__((unused)) GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam) +static bool nmeaProcessGPZDA(__attribute__((unused)) GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { - if (nbParam != 7) - return false; + if (nbParam != 7) { + return false; + } - #ifdef NMEA_DEBUG_ZDA - DEBUG_MSG("\n Time=%s (hhmmss.ss)\n", param[1]); - DEBUG_MSG(" Date=%s/%s/%s (d/m/y)\n", param[2], param[3], param[4]); - #endif + #ifdef NMEA_DEBUG_ZDA + DEBUG_MSG("\n Time=%s (hhmmss.ss)\n", param[1]); + DEBUG_MSG(" Date=%s/%s/%s (d/m/y)\n", param[2], param[3], param[4]); + #endif - *gpsDataUpdated = false; // Here we will never provide a new GPS value + *gpsDataUpdated = false; // Here we will never provide a new GPS value - // No new data data extracted - GPSTimeData gpst; - GPSTimeGet(&gpst); + // No new data data extracted + GPSTimeData gpst; + GPSTimeGet(&gpst); - // get UTC time [hhmmss.sss] - float hms = NMEA_real_to_float(param[1]); - gpst.Second = (int)hms % 100; - gpst.Minute = (((int)hms - gpst.Second) / 100) % 100; - gpst.Hour = (int)hms / 10000; + // get UTC time [hhmmss.sss] + float hms = NMEA_real_to_float(param[1]); + gpst.Second = (int)hms % 100; + gpst.Minute = (((int)hms - gpst.Second) / 100) % 100; + gpst.Hour = (int)hms / 10000; - // Get Date - gpst.Day = atoi(param[2]); - gpst.Month = atoi(param[3]); - gpst.Year = atoi(param[4]); + // Get Date + gpst.Day = atoi(param[2]); + gpst.Month = atoi(param[3]); + gpst.Year = atoi(param[4]); - GPSTimeSet(&gpst); - return true; + GPSTimeSet(&gpst); + return true; } static GPSSatellitesData gsv_partial; @@ -640,131 +633,135 @@ static uint8_t gsv_processed_mask; static uint16_t gsv_incomplete_error; static uint16_t gsv_duplicate_error; -static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam) +static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { - if (nbParam < 4) - return false; + if (nbParam < 4) { + return false; + } #ifdef NMEA_DEBUG_GSV - DEBUG_MSG("\n Sentence=%s/%s\n", param[2], param[1]); - DEBUG_MSG(" Sats=%s\n", param[3]); + DEBUG_MSG("\n Sentence=%s/%s\n", param[2], param[1]); + DEBUG_MSG(" Sats=%s\n", param[3]); #endif - uint8_t nbSentences = atoi(param[1]); - uint8_t currSentence = atoi(param[2]); + uint8_t nbSentences = atoi(param[1]); + uint8_t currSentence = atoi(param[2]); - *gpsDataUpdated = false; + *gpsDataUpdated = false; - if (nbSentences < 1 || nbSentences > 8 || currSentence < 1 || currSentence > nbSentences) - return false; + if (nbSentences < 1 || nbSentences > 8 || currSentence < 1 || currSentence > nbSentences) { + return false; + } - gsv_partial.SatsInView = atoi(param[3]); + gsv_partial.SatsInView = atoi(param[3]); - // Find out if this is the first sentence in the GSV set - if (currSentence == 1) { - if (gsv_expected_mask != gsv_processed_mask) { - // We are starting over when we haven't yet finished our previous GSV group - gsv_incomplete_error++; - } + // Find out if this is the first sentence in the GSV set + if (currSentence == 1) { + if (gsv_expected_mask != gsv_processed_mask) { + // We are starting over when we haven't yet finished our previous GSV group + gsv_incomplete_error++; + } - // First GSV sentence in the sequence, reset our expected_mask - gsv_expected_mask = (1 << nbSentences) - 1; - } + // First GSV sentence in the sequence, reset our expected_mask + gsv_expected_mask = (1 << nbSentences) - 1; + } - uint8_t current_sentence_id = (1 << (currSentence - 1)); - if (gsv_processed_mask & current_sentence_id) { - /* Duplicate sentence in this GSV set */ - gsv_duplicate_error++; - } else { - /* Note that we've seen this sentence */ - gsv_processed_mask |= current_sentence_id; - } + uint8_t current_sentence_id = (1 << (currSentence - 1)); + if (gsv_processed_mask & current_sentence_id) { + /* Duplicate sentence in this GSV set */ + gsv_duplicate_error++; + } else { + /* Note that we've seen this sentence */ + gsv_processed_mask |= current_sentence_id; + } - uint8_t parIdx = 4; + uint8_t parIdx = 4; #ifdef NMEA_DEBUG_GSV - DEBUG_MSG(" PRN:"); + DEBUG_MSG(" PRN:"); #endif - /* Make sure this sentence can fit in our GPSSatellites object */ - if ((currSentence * 4) <= NELEMENTS(gsv_partial.PRN)) { - /* Process 4 blocks of satellite info */ - for (uint8_t i = 0; parIdx+4 <= nbParam && i < 4; i++) { - uint8_t sat_index = ((currSentence - 1) * 4) + i; + /* Make sure this sentence can fit in our GPSSatellites object */ + if ((currSentence * 4) <= NELEMENTS(gsv_partial.PRN)) { + /* Process 4 blocks of satellite info */ + for (uint8_t i = 0; parIdx + 4 <= nbParam && i < 4; i++) { + uint8_t sat_index = ((currSentence - 1) * 4) + i; - // Get sat info - gsv_partial.PRN[sat_index] = atoi(param[parIdx++]); - gsv_partial.Elevation[sat_index] = NMEA_real_to_float(param[parIdx++]); - gsv_partial.Azimuth[sat_index] = NMEA_real_to_float(param[parIdx++]); - gsv_partial.SNR[sat_index] = atoi(param[parIdx++]); + // Get sat info + gsv_partial.PRN[sat_index] = atoi(param[parIdx++]); + gsv_partial.Elevation[sat_index] = NMEA_real_to_float(param[parIdx++]); + gsv_partial.Azimuth[sat_index] = NMEA_real_to_float(param[parIdx++]); + gsv_partial.SNR[sat_index] = atoi(param[parIdx++]); #ifdef NMEA_DEBUG_GSV - DEBUG_MSG(" %d", gsv_partial.PRN[sat_index]); + DEBUG_MSG(" %d", gsv_partial.PRN[sat_index]); #endif - } - } + } + } #ifdef NMEA_DEBUG_GSV - DEBUG_MSG("\n"); + DEBUG_MSG("\n"); #endif - /* Find out if we're finished processing all GSV sentences in the set */ - if ((gsv_expected_mask != 0) && (gsv_processed_mask == gsv_expected_mask)) { - /* GSV set has been fully processed. Update the GPSSatellites object. */ - GPSSatellitesSet(&gsv_partial); - memset((void *)&gsv_partial, 0, sizeof(gsv_partial)); - gsv_expected_mask = 0; - gsv_processed_mask = 0; - } + /* Find out if we're finished processing all GSV sentences in the set */ + if ((gsv_expected_mask != 0) && (gsv_processed_mask == gsv_expected_mask)) { + /* GSV set has been fully processed. Update the GPSSatellites object. */ + GPSSatellitesSet(&gsv_partial); + memset((void *)&gsv_partial, 0, sizeof(gsv_partial)); + gsv_expected_mask = 0; + gsv_processed_mask = 0; + } - return true; + return true; } -#endif //PIOS_GPS_MINIMAL +#endif // PIOS_GPS_MINIMAL /** * Parse an NMEA GPGSA sentence and update the given UAVObject * \param[in] A pointer to a GPSPosition UAVObject to be updated. * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam) +static bool nmeaProcessGPGSA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { - if (nbParam != 18) - return false; + if (nbParam != 18) { + return false; + } #ifdef NMEA_DEBUG_GSA - DEBUG_MSG("\n Status=%s\n", param[2]); - DEBUG_MSG(" PDOP=%s\n", param[15]); - DEBUG_MSG(" HDOP=%s\n", param[16]); - DEBUG_MSG(" VDOP=%s\n", param[17]); + DEBUG_MSG("\n Status=%s\n", param[2]); + DEBUG_MSG(" PDOP=%s\n", param[15]); + DEBUG_MSG(" HDOP=%s\n", param[16]); + DEBUG_MSG(" VDOP=%s\n", param[17]); #endif - *gpsDataUpdated = false; + *gpsDataUpdated = false; - switch (atoi(param[2])) { - case 1: - GpsData->Status = GPSPOSITION_STATUS_NOFIX; - break; - case 2: - GpsData->Status = GPSPOSITION_STATUS_FIX2D; - break; - case 3: - GpsData->Status = GPSPOSITION_STATUS_FIX3D; - break; - default: - /* Unhandled */ - return false; - break; - } + switch (atoi(param[2])) { + case 1: + GpsData->Status = GPSPOSITION_STATUS_NOFIX; + break; + case 2: + GpsData->Status = GPSPOSITION_STATUS_FIX2D; + break; + case 3: + GpsData->Status = GPSPOSITION_STATUS_FIX3D; + break; + default: + /* Unhandled */ + return false; - // next field: PDOP - GpsData->PDOP = NMEA_real_to_float(param[15]); + break; + } - // next field: HDOP - GpsData->HDOP = NMEA_real_to_float(param[16]); + // next field: PDOP + GpsData->PDOP = NMEA_real_to_float(param[15]); - // next field: VDOP - GpsData->VDOP = NMEA_real_to_float(param[17]); + // next field: HDOP + GpsData->HDOP = NMEA_real_to_float(param[16]); - return true; + // next field: VDOP + GpsData->VDOP = NMEA_real_to_float(param[17]); + + return true; } #endif // PIOS_INCLUDE_GPS_NMEA_PARSER diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 92d5e08bf..bf8fe8039 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -38,304 +38,306 @@ // parse incoming character stream for messages in UBX binary format -int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats) +int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats) { - enum proto_states { - START, - UBX_SY2, - UBX_CLASS, - UBX_ID, - UBX_LEN1, - UBX_LEN2, - UBX_PAYLOAD, - UBX_CHK1, - UBX_CHK2, - FINISHED - }; + enum proto_states { + START, + UBX_SY2, + UBX_CLASS, + UBX_ID, + UBX_LEN1, + UBX_LEN2, + UBX_PAYLOAD, + UBX_CHK1, + UBX_CHK2, + FINISHED + }; - static enum proto_states proto_state = START; - static uint8_t rx_count = 0; - struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer; + static enum proto_states proto_state = START; + static uint8_t rx_count = 0; + struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer; - switch (proto_state) { - case START: // detect protocol - if (c == UBX_SYNC1) // first UBX sync char found - proto_state = UBX_SY2; - break; - case UBX_SY2: - if (c == UBX_SYNC2) // second UBX sync char found - proto_state = UBX_CLASS; - else - proto_state = START; // reset state - break; - case UBX_CLASS: - ubx->header.class = c; - proto_state = UBX_ID; - break; - case UBX_ID: - ubx->header.id = c; - proto_state = UBX_LEN1; - break; - case UBX_LEN1: - ubx->header.len = c; - proto_state = UBX_LEN2; - break; - case UBX_LEN2: - ubx->header.len += (c << 8); - if (ubx->header.len > sizeof(UBXPayload)) { - gpsRxStats->gpsRxOverflow++; - proto_state = START; - } else { - rx_count = 0; - proto_state = UBX_PAYLOAD; - } - break; - case UBX_PAYLOAD: - if (rx_count < ubx->header.len) { - ubx->payload.payload[rx_count] = c; - if (++rx_count == ubx->header.len) - proto_state = UBX_CHK1; - } else { - gpsRxStats->gpsRxOverflow++; - proto_state = START; - } - break; - case UBX_CHK1: - ubx->header.ck_a = c; - proto_state = UBX_CHK2; - break; - case UBX_CHK2: - ubx->header.ck_b = c; - if (checksum_ubx_message(ubx)) { // message complete and valid - parse_ubx_message(ubx, GpsData); - proto_state = FINISHED; - } else { - gpsRxStats->gpsRxChkSumError++; - proto_state = START; - } - break; - default: break; - } + switch (proto_state) { + case START: // detect protocol + if (c == UBX_SYNC1) { // first UBX sync char found + proto_state = UBX_SY2; + } + break; + case UBX_SY2: + if (c == UBX_SYNC2) { // second UBX sync char found + proto_state = UBX_CLASS; + } else { + proto_state = START; // reset state + } + break; + case UBX_CLASS: + ubx->header.class = c; + proto_state = UBX_ID; + break; + case UBX_ID: + ubx->header.id = c; + proto_state = UBX_LEN1; + break; + case UBX_LEN1: + ubx->header.len = c; + proto_state = UBX_LEN2; + break; + case UBX_LEN2: + ubx->header.len += (c << 8); + if (ubx->header.len > sizeof(UBXPayload)) { + gpsRxStats->gpsRxOverflow++; + proto_state = START; + } else { + rx_count = 0; + proto_state = UBX_PAYLOAD; + } + break; + case UBX_PAYLOAD: + if (rx_count < ubx->header.len) { + ubx->payload.payload[rx_count] = c; + if (++rx_count == ubx->header.len) { + proto_state = UBX_CHK1; + } + } else { + gpsRxStats->gpsRxOverflow++; + proto_state = START; + } + break; + case UBX_CHK1: + ubx->header.ck_a = c; + proto_state = UBX_CHK2; + break; + case UBX_CHK2: + ubx->header.ck_b = c; + if (checksum_ubx_message(ubx)) { // message complete and valid + parse_ubx_message(ubx, GpsData); + proto_state = FINISHED; + } else { + gpsRxStats->gpsRxChkSumError++; + proto_state = START; + } + break; + default: break; + } - if (proto_state == START) - return PARSER_ERROR; // parser couldn't use this byte - else if (proto_state == FINISHED) { - gpsRxStats->gpsRxReceived++; - proto_state = START; - return PARSER_COMPLETE; // message complete & processed - } + if (proto_state == START) { + return PARSER_ERROR; // parser couldn't use this byte + } else if (proto_state == FINISHED) { + gpsRxStats->gpsRxReceived++; + proto_state = START; + return PARSER_COMPLETE; // message complete & processed + } - return PARSER_INCOMPLETE; // message not (yet) complete + return PARSER_INCOMPLETE; // message not (yet) complete } // Keep track of various GPS messages needed to make up a single UAVO update // time-of-week timestamp is used to correlate matching messages -#define POSLLH_RECEIVED (1 << 0) -#define STATUS_RECEIVED (1 << 1) -#define DOP_RECEIVED (1 << 2) -#define VELNED_RECEIVED (1 << 3) -#define SOL_RECEIVED (1 << 4) -#define ALL_RECEIVED (SOL_RECEIVED | VELNED_RECEIVED | DOP_RECEIVED | POSLLH_RECEIVED) -#define NONE_RECEIVED 0 +#define POSLLH_RECEIVED (1 << 0) +#define STATUS_RECEIVED (1 << 1) +#define DOP_RECEIVED (1 << 2) +#define VELNED_RECEIVED (1 << 3) +#define SOL_RECEIVED (1 << 4) +#define ALL_RECEIVED (SOL_RECEIVED | VELNED_RECEIVED | DOP_RECEIVED | POSLLH_RECEIVED) +#define NONE_RECEIVED 0 -static struct msgtracker{ - uint32_t currentTOW; // TOW of the message set currently in progress - uint8_t msg_received; // keep track of received message types - } msgtracker; +static struct msgtracker { + uint32_t currentTOW; // TOW of the message set currently in progress + uint8_t msg_received; // keep track of received message types +} msgtracker; // Check if a message belongs to the current data set and register it as 'received' -bool check_msgtracker (uint32_t tow, uint8_t msg_flag) +bool check_msgtracker(uint32_t tow, uint8_t msg_flag) { + if (tow > msgtracker.currentTOW ? true // start of a new message set + : (msgtracker.currentTOW - tow > 6 * 24 * 3600 * 1000)) { // 6 days, TOW wrap around occured + msgtracker.currentTOW = tow; + msgtracker.msg_received = NONE_RECEIVED; + } else if (tow < msgtracker.currentTOW) { // message outdated (don't process) + return false; + } - if (tow > msgtracker.currentTOW ? true // start of a new message set - : (msgtracker.currentTOW - tow > 6*24*3600*1000)) { // 6 days, TOW wrap around occured - msgtracker.currentTOW = tow; - msgtracker.msg_received = NONE_RECEIVED; - } else if (tow < msgtracker.currentTOW) // message outdated (don't process) - return false; - - msgtracker.msg_received |= msg_flag; // register reception of this msg type - return true; + msgtracker.msg_received |= msg_flag; // register reception of this msg type + return true; } -bool checksum_ubx_message (struct UBXPacket *ubx) +bool checksum_ubx_message(struct UBXPacket *ubx) { - int i; - uint8_t ck_a, ck_b; + int i; + uint8_t ck_a, ck_b; - ck_a = ubx->header.class; - ck_b = ck_a; + ck_a = ubx->header.class; + ck_b = ck_a; - ck_a += ubx->header.id; - ck_b += ck_a; + ck_a += ubx->header.id; + ck_b += ck_a; - ck_a += ubx->header.len & 0xff; - ck_b += ck_a; + ck_a += ubx->header.len & 0xff; + ck_b += ck_a; - ck_a += ubx->header.len >> 8; - ck_b += ck_a; + ck_a += ubx->header.len >> 8; + ck_b += ck_a; - for (i = 0; i < ubx->header.len; i++) { - ck_a += ubx->payload.payload[i]; - ck_b += ck_a; - } - - if (ubx->header.ck_a == ck_a && - ubx->header.ck_b == ck_b) - return true; - else - return false; + for (i = 0; i < ubx->header.len; i++) { + ck_a += ubx->payload.payload[i]; + ck_b += ck_a; + } + if (ubx->header.ck_a == ck_a && + ubx->header.ck_b == ck_b) { + return true; + } else { + return false; + } } -void parse_ubx_nav_posllh (struct UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPosition) +void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPosition) { - if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) { - if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { - GpsPosition->Altitude = (float)posllh->hMSL*0.001f; - GpsPosition->GeoidSeparation = (float)(posllh->height - posllh->hMSL)*0.001f; - GpsPosition->Latitude = posllh->lat; - GpsPosition->Longitude = posllh->lon; - } - } + if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) { + if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { + GpsPosition->Altitude = (float)posllh->hMSL * 0.001f; + GpsPosition->GeoidSeparation = (float)(posllh->height - posllh->hMSL) * 0.001f; + GpsPosition->Latitude = posllh->lat; + GpsPosition->Longitude = posllh->lon; + } + } } -void parse_ubx_nav_sol (struct UBX_NAV_SOL *sol, GPSPositionData *GpsPosition) +void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionData *GpsPosition) { - if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) { - GpsPosition->Satellites = sol->numSV; + if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) { + GpsPosition->Satellites = sol->numSV; - if (sol->flags & STATUS_FLAGS_GPSFIX_OK) { - switch (sol->gpsFix) { - case STATUS_GPSFIX_2DFIX: - GpsPosition->Status = GPSPOSITION_STATUS_FIX2D; - break; - case STATUS_GPSFIX_3DFIX: - GpsPosition->Status = GPSPOSITION_STATUS_FIX3D; - break; - default: GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; - } - } - else // fix is not valid so we make sure to treat is as NOFIX - GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; - } + if (sol->flags & STATUS_FLAGS_GPSFIX_OK) { + switch (sol->gpsFix) { + case STATUS_GPSFIX_2DFIX: + GpsPosition->Status = GPSPOSITION_STATUS_FIX2D; + break; + case STATUS_GPSFIX_3DFIX: + GpsPosition->Status = GPSPOSITION_STATUS_FIX3D; + break; + default: GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; + } + } else { // fix is not valid so we make sure to treat is as NOFIX + GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; + } + } } -void parse_ubx_nav_dop (struct UBX_NAV_DOP *dop, GPSPositionData *GpsPosition) +void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionData *GpsPosition) { - if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) { - GpsPosition->HDOP = (float)dop->hDOP * 0.01f; - GpsPosition->VDOP = (float)dop->vDOP * 0.01f; - GpsPosition->PDOP = (float)dop->pDOP * 0.01f; - } + if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) { + GpsPosition->HDOP = (float)dop->hDOP * 0.01f; + GpsPosition->VDOP = (float)dop->vDOP * 0.01f; + GpsPosition->PDOP = (float)dop->pDOP * 0.01f; + } } -void parse_ubx_nav_velned (struct UBX_NAV_VELNED *velned, GPSPositionData *GpsPosition) +void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionData *GpsPosition) { - GPSVelocityData GpsVelocity; + GPSVelocityData GpsVelocity; - if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) { - if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { - GpsVelocity.North = (float)velned->velN/100.0f; - GpsVelocity.East = (float)velned->velE/100.0f; - GpsVelocity.Down = (float)velned->velD/100.0f; - GPSVelocitySet(&GpsVelocity); - GpsPosition->Groundspeed = (float)velned->gSpeed * 0.01f; - GpsPosition->Heading = (float)velned->heading * 1.0e-5f; - } - } + if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) { + if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { + GpsVelocity.North = (float)velned->velN / 100.0f; + GpsVelocity.East = (float)velned->velE / 100.0f; + GpsVelocity.Down = (float)velned->velD / 100.0f; + GPSVelocitySet(&GpsVelocity); + GpsPosition->Groundspeed = (float)velned->gSpeed * 0.01f; + GpsPosition->Heading = (float)velned->heading * 1.0e-5f; + } + } } #if !defined(PIOS_GPS_MINIMAL) -void parse_ubx_nav_timeutc (struct UBX_NAV_TIMEUTC *timeutc) +void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc) { - if (!(timeutc->valid & TIMEUTC_VALIDUTC)) - return; + if (!(timeutc->valid & TIMEUTC_VALIDUTC)) { + return; + } - GPSTimeData GpsTime; + GPSTimeData GpsTime; - GpsTime.Year = timeutc->year; - GpsTime.Month = timeutc->month; - GpsTime.Day = timeutc->day; - GpsTime.Hour = timeutc->hour; - GpsTime.Minute = timeutc->min; - GpsTime.Second = timeutc->sec; + GpsTime.Year = timeutc->year; + GpsTime.Month = timeutc->month; + GpsTime.Day = timeutc->day; + GpsTime.Hour = timeutc->hour; + GpsTime.Minute = timeutc->min; + GpsTime.Second = timeutc->sec; - GPSTimeSet(&GpsTime); + GPSTimeSet(&GpsTime); } #endif #if !defined(PIOS_GPS_MINIMAL) -void parse_ubx_nav_svinfo (struct UBX_NAV_SVINFO *svinfo) +void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo) { - uint8_t chan; - GPSSatellitesData svdata; + uint8_t chan; + GPSSatellitesData svdata; - svdata.SatsInView = 0; - for (chan = 0; chan < svinfo->numCh; chan++) { - if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) { - svdata.Azimuth[svdata.SatsInView] = (float)svinfo->sv[chan].azim; - svdata.Elevation[svdata.SatsInView] = (float)svinfo->sv[chan].elev; - svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid; - svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno; - svdata.SatsInView++; - } + svdata.SatsInView = 0; + for (chan = 0; chan < svinfo->numCh; chan++) { + if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) { + svdata.Azimuth[svdata.SatsInView] = (float)svinfo->sv[chan].azim; + svdata.Elevation[svdata.SatsInView] = (float)svinfo->sv[chan].elev; + svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid; + svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno; + svdata.SatsInView++; + } + } + // fill remaining slots (if any) + for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) { + svdata.Azimuth[chan] = (float)0.0f; + svdata.Elevation[chan] = (float)0.0f; + svdata.PRN[chan] = 0; + svdata.SNR[chan] = 0; + } - } - // fill remaining slots (if any) - for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) { - svdata.Azimuth[chan] = (float)0.0f; - svdata.Elevation[chan] = (float)0.0f; - svdata.PRN[chan] = 0; - svdata.SNR[chan] = 0; - } - - GPSSatellitesSet(&svdata); + GPSSatellitesSet(&svdata); } -#endif +#endif /* if !defined(PIOS_GPS_MINIMAL) */ // UBX message parser // returns UAVObjectID if a UAVObject structure is ready for further processing -uint32_t parse_ubx_message (struct UBXPacket *ubx, GPSPositionData *GpsPosition) +uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionData *GpsPosition) { - uint32_t id = 0; + uint32_t id = 0; - switch (ubx->header.class) { - case UBX_CLASS_NAV: - switch (ubx->header.id) { - case UBX_ID_POSLLH: - parse_ubx_nav_posllh (&ubx->payload.nav_posllh, GpsPosition); - break; - case UBX_ID_DOP: - parse_ubx_nav_dop (&ubx->payload.nav_dop, GpsPosition); - break; - case UBX_ID_SOL: - parse_ubx_nav_sol (&ubx->payload.nav_sol, GpsPosition); - break; - case UBX_ID_VELNED: - parse_ubx_nav_velned (&ubx->payload.nav_velned, GpsPosition); - break; + switch (ubx->header.class) { + case UBX_CLASS_NAV: + switch (ubx->header.id) { + case UBX_ID_POSLLH: + parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition); + break; + case UBX_ID_DOP: + parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition); + break; + case UBX_ID_SOL: + parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition); + break; + case UBX_ID_VELNED: + parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition); + break; #if !defined(PIOS_GPS_MINIMAL) - case UBX_ID_TIMEUTC: - parse_ubx_nav_timeutc (&ubx->payload.nav_timeutc); - break; - case UBX_ID_SVINFO: - parse_ubx_nav_svinfo (&ubx->payload.nav_svinfo); - break; + case UBX_ID_TIMEUTC: + parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc); + break; + case UBX_ID_SVINFO: + parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo); + break; #endif - } - break; - } - if (msgtracker.msg_received == ALL_RECEIVED) { - GPSPositionSet(GpsPosition); - msgtracker.msg_received = NONE_RECEIVED; - id = GPSPOSITION_OBJID; - } - return id; + } + break; + } + if (msgtracker.msg_received == ALL_RECEIVED) { + GPSPositionSet(GpsPosition); + msgtracker.msg_received = NONE_RECEIVED; + id = GPSPOSITION_OBJID; + } + return id; } #endif // PIOS_INCLUDE_GPS_UBX_PARSER - diff --git a/flight/modules/GPS/inc/GPS.h b/flight/modules/GPS/inc/GPS.h index 6e243c21f..ecc7470d8 100644 --- a/flight/modules/GPS/inc/GPS.h +++ b/flight/modules/GPS/inc/GPS.h @@ -1,16 +1,16 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup GSPModule GPS Module * @brief Process GPS information - * @{ + * @{ * * @file GPS.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Include file of the GPS module. - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and * objects. * @see The GNU Public License (GPL) Version 3 * @@ -39,17 +39,17 @@ #include "gpsposition.h" #include "gpstime.h" -#define NO_PARSER -3 // no parser available -#define PARSER_OVERRUN -2 // message buffer overrun before completing the message -#define PARSER_ERROR -1 // message unparsable by this parser -#define PARSER_INCOMPLETE 0 // parser needs more data to complete the message -#define PARSER_COMPLETE 1 // parser has received a complete message and finished processing +#define NO_PARSER -3 // no parser available +#define PARSER_OVERRUN -2 // message buffer overrun before completing the message +#define PARSER_ERROR -1 // message unparsable by this parser +#define PARSER_INCOMPLETE 0 // parser needs more data to complete the message +#define PARSER_COMPLETE 1 // parser has received a complete message and finished processing struct GPS_RX_STATS { - uint16_t gpsRxReceived; - uint16_t gpsRxChkSumError; - uint16_t gpsRxOverflow; - uint16_t gpsRxParserError; + uint16_t gpsRxReceived; + uint16_t gpsRxChkSumError; + uint16_t gpsRxOverflow; + uint16_t gpsRxParserError; }; int32_t GPSInitialize(void); @@ -57,6 +57,6 @@ int32_t GPSInitialize(void); #endif // GPS_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/GPS/inc/NMEA.h b/flight/modules/GPS/inc/NMEA.h index 49213e716..a1e94a24e 100644 --- a/flight/modules/GPS/inc/NMEA.h +++ b/flight/modules/GPS/inc/NMEA.h @@ -35,7 +35,7 @@ #include #include "GPS.h" -#define NMEA_MAX_PACKET_LENGTH 96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed +#define NMEA_MAX_PACKET_LENGTH 96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed extern bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData); extern bool NMEA_checksum(char *nmea_sentence); diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index f7718bb3c..9a1c86eeb 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -35,190 +35,190 @@ #include "GPS.h" -#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters -#define UBX_SYNC2 0x62 +#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters +#define UBX_SYNC2 0x62 // From u-blox6 receiver protocol specification // Messages classes -#define UBX_CLASS_NAV 0x01 +#define UBX_CLASS_NAV 0x01 // Message IDs -#define UBX_ID_POSLLH 0x02 -#define UBX_ID_STATUS 0x03 -#define UBX_ID_DOP 0x04 -#define UBX_ID_SOL 0x06 -#define UBX_ID_VELNED 0x12 -#define UBX_ID_TIMEUTC 0x21 -#define UBX_ID_SVINFO 0x30 +#define UBX_ID_POSLLH 0x02 +#define UBX_ID_STATUS 0x03 +#define UBX_ID_DOP 0x04 +#define UBX_ID_SOL 0x06 +#define UBX_ID_VELNED 0x12 +#define UBX_ID_TIMEUTC 0x21 +#define UBX_ID_SVINFO 0x30 // private structures // Geodetic Position Solution struct UBX_NAV_POSLLH { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - int32_t lon; // Longitude (deg*1e-7) - int32_t lat; // Latitude (deg*1e-7) - int32_t height; // Height above Ellipsoid (mm) - int32_t hMSL; // Height above mean sea level (mm) - uint32_t hAcc; // Horizontal Accuracy Estimate (mm) - uint32_t vAcc; // Vertical Accuracy Estimate (mm) + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + int32_t lon; // Longitude (deg*1e-7) + int32_t lat; // Latitude (deg*1e-7) + int32_t height; // Height above Ellipsoid (mm) + int32_t hMSL; // Height above mean sea level (mm) + uint32_t hAcc; // Horizontal Accuracy Estimate (mm) + uint32_t vAcc; // Vertical Accuracy Estimate (mm) }; // Receiver Navigation Status -#define STATUS_GPSFIX_NOFIX 0x00 -#define STATUS_GPSFIX_DRONLY 0x01 -#define STATUS_GPSFIX_2DFIX 0x02 -#define STATUS_GPSFIX_3DFIX 0x03 -#define STATUS_GPSFIX_GPSDR 0x04 -#define STATUS_GPSFIX_TIMEONLY 0x05 +#define STATUS_GPSFIX_NOFIX 0x00 +#define STATUS_GPSFIX_DRONLY 0x01 +#define STATUS_GPSFIX_2DFIX 0x02 +#define STATUS_GPSFIX_3DFIX 0x03 +#define STATUS_GPSFIX_GPSDR 0x04 +#define STATUS_GPSFIX_TIMEONLY 0x05 -#define STATUS_FLAGS_GPSFIX_OK (1 << 0) -#define STATUS_FLAGS_DIFFSOLN (1 << 1) -#define STATUS_FLAGS_WKNSET (1 << 2) -#define STATUS_FLAGS_TOWSET (1 << 3) +#define STATUS_FLAGS_GPSFIX_OK (1 << 0) +#define STATUS_FLAGS_DIFFSOLN (1 << 1) +#define STATUS_FLAGS_WKNSET (1 << 2) +#define STATUS_FLAGS_TOWSET (1 << 3) struct UBX_NAV_STATUS { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - uint8_t gpsFix; // GPS fix type - uint8_t flags; // Navigation Status Flags - uint8_t fixStat; // Fix Status Information - uint8_t flags2; // Additional navigation output information - uint32_t ttff; // Time to first fix (ms) - uint32_t msss; // Milliseconds since startup/reset (ms) + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint8_t gpsFix; // GPS fix type + uint8_t flags; // Navigation Status Flags + uint8_t fixStat; // Fix Status Information + uint8_t flags2; // Additional navigation output information + uint32_t ttff; // Time to first fix (ms) + uint32_t msss; // Milliseconds since startup/reset (ms) }; // Dilution of precision struct UBX_NAV_DOP { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - uint16_t gDOP; // Geometric DOP - uint16_t pDOP; // Position DOP - uint16_t tDOP; // Time DOP - uint16_t vDOP; // Vertical DOP - uint16_t hDOP; // Horizontal DOP - uint16_t nDOP; // Northing DOP - uint16_t eDOP; // Easting DOP + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint16_t gDOP; // Geometric DOP + uint16_t pDOP; // Position DOP + uint16_t tDOP; // Time DOP + uint16_t vDOP; // Vertical DOP + uint16_t hDOP; // Horizontal DOP + uint16_t nDOP; // Northing DOP + uint16_t eDOP; // Easting DOP }; // Navigation solution struct UBX_NAV_SOL { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - int32_t fTOW; // fractional nanoseconds (ns) - int16_t week; // GPS week - uint8_t gpsFix; // GPS fix type - uint8_t flags; // Fix status flags - int32_t ecefX; // ECEF X coordinate (cm) - int32_t ecefY; // ECEF Y coordinate (cm) - int32_t ecefZ; // ECEF Z coordinate (cm) - uint32_t pAcc; // 3D Position Accuracy Estimate (cm) - int32_t ecefVX; // ECEF X coordinate (cm/s) - int32_t ecefVY; // ECEF Y coordinate (cm/s) - int32_t ecefVZ; // ECEF Z coordinate (cm/s) - uint32_t sAcc; // Speed Accuracy Estimate - uint16_t pDOP; // Position DOP - uint8_t reserved1; // Reserved - uint8_t numSV; // Number of SVs used in Nav Solution - uint32_t reserved2; // Reserved + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + int32_t fTOW; // fractional nanoseconds (ns) + int16_t week; // GPS week + uint8_t gpsFix; // GPS fix type + uint8_t flags; // Fix status flags + int32_t ecefX; // ECEF X coordinate (cm) + int32_t ecefY; // ECEF Y coordinate (cm) + int32_t ecefZ; // ECEF Z coordinate (cm) + uint32_t pAcc; // 3D Position Accuracy Estimate (cm) + int32_t ecefVX; // ECEF X coordinate (cm/s) + int32_t ecefVY; // ECEF Y coordinate (cm/s) + int32_t ecefVZ; // ECEF Z coordinate (cm/s) + uint32_t sAcc; // Speed Accuracy Estimate + uint16_t pDOP; // Position DOP + uint8_t reserved1; // Reserved + uint8_t numSV; // Number of SVs used in Nav Solution + uint32_t reserved2; // Reserved }; // North/East/Down velocity struct UBX_NAV_VELNED { - uint32_t iTOW; // ms GPS Millisecond Time of Week - int32_t velN; // cm/s NED north velocity - int32_t velE; // cm/s NED east velocity - int32_t velD; // cm/s NED down velocity - uint32_t speed; // cm/s Speed (3-D) - uint32_t gSpeed; // cm/s Ground Speed (2-D) - int32_t heading; // 1e-5 *deg Heading of motion 2-D - uint32_t sAcc; // cm/s Speed Accuracy Estimate - uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate + uint32_t iTOW; // ms GPS Millisecond Time of Week + int32_t velN; // cm/s NED north velocity + int32_t velE; // cm/s NED east velocity + int32_t velD; // cm/s NED down velocity + uint32_t speed; // cm/s Speed (3-D) + uint32_t gSpeed; // cm/s Ground Speed (2-D) + int32_t heading; // 1e-5 *deg Heading of motion 2-D + uint32_t sAcc; // cm/s Speed Accuracy Estimate + uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate }; // UTC Time Solution -#define TIMEUTC_VALIDTOW (1 << 0) -#define TIMEUTC_VALIDWKN (1 << 1) -#define TIMEUTC_VALIDUTC (1 << 2) +#define TIMEUTC_VALIDTOW (1 << 0) +#define TIMEUTC_VALIDWKN (1 << 1) +#define TIMEUTC_VALIDUTC (1 << 2) struct UBX_NAV_TIMEUTC { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - uint32_t tAcc; // Time Accuracy Estimate (ns) - int32_t nano; // Nanoseconds of second - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t min; - uint8_t sec; - uint8_t valid; // Validity Flags + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint32_t tAcc; // Time Accuracy Estimate (ns) + int32_t nano; // Nanoseconds of second + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; // Validity Flags }; // Space Vehicle (SV) Information // Single SV information block -#define SVUSED (1 << 0) // This SV is used for navigation -#define DIFFCORR (1 << 1) // Differential correction available -#define ORBITAVAIL (1 << 2) // Orbit information available -#define ORBITEPH (1 << 3) // Orbit information is Ephemeris -#define UNHEALTHY (1 << 4) // SV is unhealthy -#define ORBITALM (1 << 5) // Orbit information is Almanac Plus -#define ORBITAOP (1 << 6) // Orbit information is AssistNow Autonomous -#define SMOOTHED (1 << 7) // Carrier smoothed pseudoranges used +#define SVUSED (1 << 0) // This SV is used for navigation +#define DIFFCORR (1 << 1) // Differential correction available +#define ORBITAVAIL (1 << 2) // Orbit information available +#define ORBITEPH (1 << 3) // Orbit information is Ephemeris +#define UNHEALTHY (1 << 4) // SV is unhealthy +#define ORBITALM (1 << 5) // Orbit information is Almanac Plus +#define ORBITAOP (1 << 6) // Orbit information is AssistNow Autonomous +#define SMOOTHED (1 << 7) // Carrier smoothed pseudoranges used struct UBX_NAV_SVINFO_SV { - uint8_t chn; // Channel number - uint8_t svid; // Satellite ID - uint8_t flags; // Misc SV information - uint8_t quality; // Misc quality indicators - uint8_t cno; // Carrier to Noise Ratio (dbHz) - int8_t elev; // Elevation (integer degrees) - int16_t azim; // Azimuth (integer degrees) - int32_t prRes; // Pseudo range residual (cm) + uint8_t chn; // Channel number + uint8_t svid; // Satellite ID + uint8_t flags; // Misc SV information + uint8_t quality; // Misc quality indicators + uint8_t cno; // Carrier to Noise Ratio (dbHz) + int8_t elev; // Elevation (integer degrees) + int16_t azim; // Azimuth (integer degrees) + int32_t prRes; // Pseudo range residual (cm) }; // SV information message -#define MAX_SVS 16 +#define MAX_SVS 16 struct UBX_NAV_SVINFO { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - uint8_t numCh; // Number of channels - uint8_t globalFlags; // - uint16_t reserved2; // Reserved - struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint8_t numCh; // Number of channels + uint8_t globalFlags; // + uint16_t reserved2; // Reserved + struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times }; typedef union { - uint8_t payload[0]; - struct UBX_NAV_POSLLH nav_posllh; - struct UBX_NAV_STATUS nav_status; - struct UBX_NAV_DOP nav_dop; - struct UBX_NAV_SOL nav_sol; - struct UBX_NAV_VELNED nav_velned; + uint8_t payload[0]; + struct UBX_NAV_POSLLH nav_posllh; + struct UBX_NAV_STATUS nav_status; + struct UBX_NAV_DOP nav_dop; + struct UBX_NAV_SOL nav_sol; + struct UBX_NAV_VELNED nav_velned; #if !defined(PIOS_GPS_MINIMAL) - struct UBX_NAV_TIMEUTC nav_timeutc; - struct UBX_NAV_SVINFO nav_svinfo; + struct UBX_NAV_TIMEUTC nav_timeutc; + struct UBX_NAV_SVINFO nav_svinfo; #endif } UBXPayload; struct UBXHeader { - uint8_t class; - uint8_t id; - uint16_t len; - uint8_t ck_a; - uint8_t ck_b; + uint8_t class; + uint8_t id; + uint16_t len; + uint8_t ck_a; + uint8_t ck_b; }; struct UBXPacket { - struct UBXHeader header; - UBXPayload payload; + struct UBXHeader header; + UBXPayload payload; }; bool checksum_ubx_message(struct UBXPacket *); uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionData *); -int parse_ubx_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *); +int parse_ubx_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *); #endif /* UBX_H */ diff --git a/flight/modules/MK/MKSerial/MKSerial.c b/flight/modules/MK/MKSerial/MKSerial.c index ed1cb7006..53f19d737 100644 --- a/flight/modules/MK/MKSerial/MKSerial.c +++ b/flight/modules/MK/MKSerial/MKSerial.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup MKSerialModule MK Serial Control Module * @brief Connect to MK module - * @{ + * @{ * * @file MKSerial.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -31,20 +31,20 @@ #include "openpilot.h" #include "MkSerial.h" -#include "attitudeactual.h" // object that will be updated by the module +#include "attitudeactual.h" // object that will be updated by the module #include "positionactual.h" #include "flightbatterystate.h" // // Configuration // -#define PORT PIOS_COM_AUX -#define DEBUG_PORT PIOS_COM_GPS -#define STACK_SIZE 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) -#define MAX_NB_PARS 100 -//#define ENABLE_DEBUG_MSG -//#define GENERATE_BATTERY_INFO // The MK can report battery voltage, but normally the current sensor will be used, so this module should not report battery state +#define PORT PIOS_COM_AUX +#define DEBUG_PORT PIOS_COM_GPS +#define STACK_SIZE 1024 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) +#define MAX_NB_PARS 100 +// #define ENABLE_DEBUG_MSG +// #define GENERATE_BATTERY_INFO // The MK can report battery voltage, but normally the current sensor will be used, so this module should not report battery state #if PORT == PIOS_COM_AUX #ifndef PIOS_ENABLE_AUX_UART @@ -54,27 +54,27 @@ // // Private constants // -#define MSGCMD_ANY 0 -#define MSGCMD_GET_DEBUG 'd' -#define MSGCMD_DEBUG 'D' -#define MSGCMD_GET_VERSION 'v' -#define MSGCMD_VERSION 'V' -#define MSGCMD_GET_OSD 'o' -#define MSGCMD_OSD 'O' +#define MSGCMD_ANY 0 +#define MSGCMD_GET_DEBUG 'd' +#define MSGCMD_DEBUG 'D' +#define MSGCMD_GET_VERSION 'v' +#define MSGCMD_VERSION 'V' +#define MSGCMD_GET_OSD 'o' +#define MSGCMD_OSD 'O' -#define DEBUG_MSG_NICK_IDX (2+2*2) -#define DEBUG_MSG_ROLL_IDX (2+3*2) +#define DEBUG_MSG_NICK_IDX (2 + 2 * 2) +#define DEBUG_MSG_ROLL_IDX (2 + 3 * 2) -#define OSD_MSG_CURRPOS_IDX 1 -#define OSD_MSG_NB_SATS_IDX 50 -#define OSD_MSG_BATT_IDX 57 +#define OSD_MSG_CURRPOS_IDX 1 +#define OSD_MSG_NB_SATS_IDX 50 +#define OSD_MSG_BATT_IDX 57 #define OSD_MSG_GNDSPEED_IDX 58 -#define OSD_MSG_COMPHEADING_IDX 62 -#define OSD_MSG_NICK_IDX 64 -#define OSD_MSG_ROLL_IDX 65 +#define OSD_MSG_COMPHEADING_IDX 62 +#define OSD_MSG_NICK_IDX 64 +#define OSD_MSG_ROLL_IDX 65 #ifdef ENABLE_DEBUG_MSG -#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__) +#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format,##__VA_ARGS__) #else #define DEBUG_MSG(format, ...) #endif @@ -83,24 +83,24 @@ // Private types // typedef struct { - uint8_t address; - uint8_t cmd; - uint8_t nbPars; - uint8_t pars[MAX_NB_PARS]; + uint8_t address; + uint8_t cmd; + uint8_t nbPars; + uint8_t pars[MAX_NB_PARS]; } MkMsg_t; typedef struct { - float longitute; - float latitude; - float altitude; - uint8_t status; + float longitute; + float latitude; + float altitude; + uint8_t status; } GpsPosition_t; enum { - MK_ADDR_ALL = 0, - MK_ADDR_FC = 1, - MK_ADDR_NC = 2, - MK_ADDR_MAG = 3, + MK_ADDR_ALL = 0, + MK_ADDR_FC = 1, + MK_ADDR_NC = 2, + MK_ADDR_MAG = 3, }; // @@ -111,433 +111,439 @@ enum { // Private functions // static void OnError(int line); -//static void PrintMsg(const MkMsg_t* msg); -static int16_t Par2Int16(const MkMsg_t * msg, uint8_t index); -static int32_t Par2Int32(const MkMsg_t * msg, uint8_t index); -static int8_t Par2Int8(const MkMsg_t * msg, uint8_t index); -static void GetGpsPos(const MkMsg_t * msg, uint8_t index, GpsPosition_t * pos); -static uint8_t WaitForBytes(uint8_t * buf, uint8_t nbBytes, portTickType xTicksToWait); -static bool WaitForMsg(uint8_t cmd, MkMsg_t * msg, portTickType xTicksToWait); -static void SendMsg(const MkMsg_t * msg); +// static void PrintMsg(const MkMsg_t* msg); +static int16_t Par2Int16(const MkMsg_t *msg, uint8_t index); +static int32_t Par2Int32(const MkMsg_t *msg, uint8_t index); +static int8_t Par2Int8(const MkMsg_t *msg, uint8_t index); +static void GetGpsPos(const MkMsg_t *msg, uint8_t index, GpsPosition_t *pos); +static uint8_t WaitForBytes(uint8_t *buf, uint8_t nbBytes, portTickType xTicksToWait); +static bool WaitForMsg(uint8_t cmd, MkMsg_t *msg, portTickType xTicksToWait); +static void SendMsg(const MkMsg_t *msg); static void SendMsgParNone(uint8_t address, uint8_t cmd); static void SendMsgPar8(uint8_t address, uint8_t cmd, uint8_t par0); static void MkSerialTask(void *parameters); static void OnError(int line) { - DEBUG_MSG("MKProcol error %d\n\r", line); + DEBUG_MSG("MKProcol error %d\n\r", line); } #if 0 -static void PrintMsg(const MkMsg_t * msg) +static void PrintMsg(const MkMsg_t *msg) { - switch (msg->address) { - case MK_ADDR_ALL: - DEBUG_MSG("ALL "); - break; - case MK_ADDR_FC: - DEBUG_MSG("FC "); - break; - case MK_ADDR_NC: - DEBUG_MSG("NC "); - break; - case MK_ADDR_MAG: - DEBUG_MSG("MAG "); - break; - default: - DEBUG_MSG("??? "); - break; - } + switch (msg->address) { + case MK_ADDR_ALL: + DEBUG_MSG("ALL "); + break; + case MK_ADDR_FC: + DEBUG_MSG("FC "); + break; + case MK_ADDR_NC: + DEBUG_MSG("NC "); + break; + case MK_ADDR_MAG: + DEBUG_MSG("MAG "); + break; + default: + DEBUG_MSG("??? "); + break; + } - DEBUG_MSG("%c ", msg->cmd); + DEBUG_MSG("%c ", msg->cmd); - for (int i = 0; i < msg->nbPars; i++) { - DEBUG_MSG("%02x ", msg->pars[i]); - } - DEBUG_MSG("\n\r"); + for (int i = 0; i < msg->nbPars; i++) { + DEBUG_MSG("%02x ", msg->pars[i]); + } + DEBUG_MSG("\n\r"); } -#endif +#endif /* if 0 */ -static int16_t Par2Int16(const MkMsg_t * msg, uint8_t index) +static int16_t Par2Int16(const MkMsg_t *msg, uint8_t index) { - int16_t res; + int16_t res; - res = (int)(msg->pars[index + 1]) * 256 + msg->pars[index]; - if (res > 0xFFFF / 2) - res -= 0xFFFF; - return res; + res = (int)(msg->pars[index + 1]) * 256 + msg->pars[index]; + if (res > 0xFFFF / 2) { + res -= 0xFFFF; + } + return res; } -static int32_t Par2Int32(const MkMsg_t * msg, uint8_t index) +static int32_t Par2Int32(const MkMsg_t *msg, uint8_t index) { - uint32_t val = 0; + uint32_t val = 0; - val = (((int)msg->pars[index]) << 0) + (((int)msg->pars[index + 1]) << 8); - val += (((int)msg->pars[index + 2]) << 16) + ((int)msg->pars[index + 3] << 24); - if (val > 0xFFFFFFFF / 2) - val -= 0xFFFFFFFF; - return (int32_t) val; + val = (((int)msg->pars[index]) << 0) + (((int)msg->pars[index + 1]) << 8); + val += (((int)msg->pars[index + 2]) << 16) + ((int)msg->pars[index + 3] << 24); + if (val > 0xFFFFFFFF / 2) { + val -= 0xFFFFFFFF; + } + return (int32_t)val; } -static int8_t Par2Int8(const MkMsg_t * msg, uint8_t index) +static int8_t Par2Int8(const MkMsg_t *msg, uint8_t index) { - if (msg->pars[index] > 127) - return msg->pars[index] - 256; - else - return msg->pars[index]; + if (msg->pars[index] > 127) { + return msg->pars[index] - 256; + } else { + return msg->pars[index]; + } } -static void GetGpsPos(const MkMsg_t * msg, uint8_t index, GpsPosition_t * pos) +static void GetGpsPos(const MkMsg_t *msg, uint8_t index, GpsPosition_t *pos) { - pos->longitute = (float)Par2Int32(msg, index) * (float)1e-7; - pos->latitude = (float)Par2Int32(msg, index + 4) * (float)1e-7; - pos->altitude = (float)Par2Int32(msg, index + 8) * (float)1e-3; - pos->status = msg->pars[index + 12]; + pos->longitute = (float)Par2Int32(msg, index) * (float)1e-7; + pos->latitude = (float)Par2Int32(msg, index + 4) * (float)1e-7; + pos->altitude = (float)Par2Int32(msg, index + 8) * (float)1e-3; + pos->status = msg->pars[index + 12]; } -static uint8_t WaitForBytes(uint8_t * buf, uint8_t nbBytes, portTickType xTicksToWait) +static uint8_t WaitForBytes(uint8_t *buf, uint8_t nbBytes, portTickType xTicksToWait) { - uint8_t nbBytesLeft = nbBytes; - xTimeOutType xTimeOut; + uint8_t nbBytesLeft = nbBytes; + xTimeOutType xTimeOut; - vTaskSetTimeOutState(&xTimeOut); + vTaskSetTimeOutState(&xTimeOut); - // Loop until - // - all bytes are received - // - \r is seen - // - Timeout occurs - do { - // Check if timeout occured - if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait)) - break; + // Loop until + // - all bytes are received + // - \r is seen + // - Timeout occurs + do { + // Check if timeout occured + if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait)) { + break; + } - // Check if there are some bytes - if (PIOS_COM_ReceiveBufferUsed(PORT)) { - *buf = PIOS_COM_ReceiveBuffer(PORT); + // Check if there are some bytes + if (PIOS_COM_ReceiveBufferUsed(PORT)) { + *buf = PIOS_COM_ReceiveBuffer(PORT); - nbBytesLeft--; - if (buf[0] == '\r') - break; - buf++; - } else { - // Avoid tight loop - // FIXME: should be blocking - vTaskDelay(5); - } - } while (nbBytesLeft); + nbBytesLeft--; + if (buf[0] == '\r') { + break; + } + buf++; + } else { + // Avoid tight loop + // FIXME: should be blocking + vTaskDelay(5); + } + } while (nbBytesLeft); - return nbBytes - nbBytesLeft; + return nbBytes - nbBytesLeft; } -static bool WaitForMsg(uint8_t cmd, MkMsg_t * msg, portTickType xTicksToWait) +static bool WaitForMsg(uint8_t cmd, MkMsg_t *msg, portTickType xTicksToWait) { - uint8_t buf[10]; - uint8_t n; - bool done = FALSE; - bool error = FALSE; - unsigned int checkVal; - xTimeOutType xTimeOut; + uint8_t buf[10]; + uint8_t n; + bool done = FALSE; + bool error = FALSE; + unsigned int checkVal; + xTimeOutType xTimeOut; - vTaskSetTimeOutState(&xTimeOut); + vTaskSetTimeOutState(&xTimeOut); - while (!done && !error) { - // When we are here, it means we did not encounter the message we are waiting for - // Check if we did not timeout yet. + while (!done && !error) { + // When we are here, it means we did not encounter the message we are waiting for + // Check if we did not timeout yet. - // Wait for start - buf[0] = 0; - do { - if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait)) { - return FALSE; - } - WaitForBytes(buf, 1, 100 / portTICK_RATE_MS); - } while (buf[0] != '#'); + // Wait for start + buf[0] = 0; + do { + if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait)) { + return FALSE; + } + WaitForBytes(buf, 1, 100 / portTICK_RATE_MS); + } while (buf[0] != '#'); - // Wait for cmd and address - if (WaitForBytes(buf, 2, 10 / portTICK_RATE_MS) != 2) { - OnError(__LINE__); - continue; - } - // Is this the command we are waiting for? - if (cmd == 0 || cmd == buf[1]) { - // OK follow this message to the end - msg->address = buf[0] - 'a'; - msg->cmd = buf[1]; + // Wait for cmd and address + if (WaitForBytes(buf, 2, 10 / portTICK_RATE_MS) != 2) { + OnError(__LINE__); + continue; + } + // Is this the command we are waiting for? + if (cmd == 0 || cmd == buf[1]) { + // OK follow this message to the end + msg->address = buf[0] - 'a'; + msg->cmd = buf[1]; - checkVal = '#' + buf[0] + buf[1]; + checkVal = '#' + buf[0] + buf[1]; - // Parse parameters - msg->nbPars = 0; - while (!done && !error) { - n = WaitForBytes(buf, 4, 10 / portTICK_RATE_MS); - if (n > 0 && buf[n - 1] == '\r') { - n--; - // This is the end of the message - // Get check bytes - if (n >= 2) { - unsigned int msgCeckVal; - msgCeckVal = (buf[n - 1] - '=') + (buf[n - 2] - '=') * 64; - //printf("%x %x\n", msgCeckVal, checkVal&0xFFF); - n -= 2; + // Parse parameters + msg->nbPars = 0; + while (!done && !error) { + n = WaitForBytes(buf, 4, 10 / portTICK_RATE_MS); + if (n > 0 && buf[n - 1] == '\r') { + n--; + // This is the end of the message + // Get check bytes + if (n >= 2) { + unsigned int msgCeckVal; + msgCeckVal = (buf[n - 1] - '=') + (buf[n - 2] - '=') * 64; + // printf("%x %x\n", msgCeckVal, checkVal&0xFFF); + n -= 2; - if (msgCeckVal == (checkVal & 0xFFF)) { - done = TRUE; - } else { - OnError(__LINE__); - error = TRUE; - } - } else { - OnError(__LINE__); - error = TRUE; - } - } else if (n == 4) { - // Parse parameters - int i; - for (i = 0; i < 4; i++) { - checkVal += buf[i]; - buf[i] -= '='; - } - if (msg->nbPars < MAX_NB_PARS) { - msg->pars[msg->nbPars] = (((buf[0] << 2) & 0xFF) | ((buf[1] >> 4))); - msg->nbPars++; - } - if (msg->nbPars < MAX_NB_PARS) { - msg->pars[msg->nbPars] = ((buf[1] & 0x0F) << 4 | (buf[2] >> 2)); - msg->nbPars++; - } - if (msg->nbPars < MAX_NB_PARS) { - msg->pars[msg->nbPars] = ((buf[2] & 0x03) << 6 | buf[3]); - msg->nbPars++; - } - } else { - OnError(__LINE__); - error = TRUE; - } - } - } - } + if (msgCeckVal == (checkVal & 0xFFF)) { + done = TRUE; + } else { + OnError(__LINE__); + error = TRUE; + } + } else { + OnError(__LINE__); + error = TRUE; + } + } else if (n == 4) { + // Parse parameters + int i; + for (i = 0; i < 4; i++) { + checkVal += buf[i]; + buf[i] -= '='; + } + if (msg->nbPars < MAX_NB_PARS) { + msg->pars[msg->nbPars] = (((buf[0] << 2) & 0xFF) | ((buf[1] >> 4))); + msg->nbPars++; + } + if (msg->nbPars < MAX_NB_PARS) { + msg->pars[msg->nbPars] = ((buf[1] & 0x0F) << 4 | (buf[2] >> 2)); + msg->nbPars++; + } + if (msg->nbPars < MAX_NB_PARS) { + msg->pars[msg->nbPars] = ((buf[2] & 0x03) << 6 | buf[3]); + msg->nbPars++; + } + } else { + OnError(__LINE__); + error = TRUE; + } + } + } + } - return (done && !error); + return done && !error; } -static void SendMsg(const MkMsg_t * msg) +static void SendMsg(const MkMsg_t *msg) { - uint8_t buf[10]; - uint16_t checkVal; - uint8_t nbParsRemaining; - const uint8_t *pPar; + uint8_t buf[10]; + uint16_t checkVal; + uint8_t nbParsRemaining; + const uint8_t *pPar; - // Header - buf[0] = '#'; - buf[1] = msg->address + 'a'; - buf[2] = msg->cmd; - PIOS_COM_SendBuffer(PORT, buf, 3); - checkVal = (unsigned int)'#' + buf[1] + buf[2]; + // Header + buf[0] = '#'; + buf[1] = msg->address + 'a'; + buf[2] = msg->cmd; + PIOS_COM_SendBuffer(PORT, buf, 3); + checkVal = (unsigned int)'#' + buf[1] + buf[2]; - // Parameters - nbParsRemaining = msg->nbPars; - pPar = msg->pars; - while (nbParsRemaining) { - uint8_t a, b, c; + // Parameters + nbParsRemaining = msg->nbPars; + pPar = msg->pars; + while (nbParsRemaining) { + uint8_t a, b, c; - a = *pPar; - b = 0; - c = 0; + a = *pPar; + b = 0; + c = 0; - nbParsRemaining--; - pPar++; - if (nbParsRemaining) { - b = *pPar; - nbParsRemaining--; - pPar++; - if (nbParsRemaining) { - c = *pPar; - nbParsRemaining--; - pPar++; - } - } + nbParsRemaining--; + pPar++; + if (nbParsRemaining) { + b = *pPar; + nbParsRemaining--; + pPar++; + if (nbParsRemaining) { + c = *pPar; + nbParsRemaining--; + pPar++; + } + } - buf[0] = (a >> 2) + '='; - buf[1] = (((a & 0x03) << 4) | ((b & 0xf0) >> 4)) + '='; - buf[2] = (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)) + '='; - buf[3] = (c & 0x3f) + '='; - checkVal += buf[0]; - checkVal += buf[1]; - checkVal += buf[2]; - checkVal += buf[3]; + buf[0] = (a >> 2) + '='; + buf[1] = (((a & 0x03) << 4) | ((b & 0xf0) >> 4)) + '='; + buf[2] = (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)) + '='; + buf[3] = (c & 0x3f) + '='; + checkVal += buf[0]; + checkVal += buf[1]; + checkVal += buf[2]; + checkVal += buf[3]; - PIOS_COM_SendBuffer(PORT, buf, 4); - } + PIOS_COM_SendBuffer(PORT, buf, 4); + } - checkVal &= 0xFFF; - buf[0] = (checkVal / 64) + '='; - buf[1] = (checkVal % 64) + '='; - buf[2] = '\r'; - PIOS_COM_SendBuffer(PORT, buf, 3); + checkVal &= 0xFFF; + buf[0] = (checkVal / 64) + '='; + buf[1] = (checkVal % 64) + '='; + buf[2] = '\r'; + PIOS_COM_SendBuffer(PORT, buf, 3); } static void SendMsgParNone(uint8_t address, uint8_t cmd) { - MkMsg_t msg; + MkMsg_t msg; - msg.address = address; - msg.cmd = cmd; - msg.nbPars = 0; + msg.address = address; + msg.cmd = cmd; + msg.nbPars = 0; - SendMsg(&msg); + SendMsg(&msg); } static void SendMsgPar8(uint8_t address, uint8_t cmd, uint8_t par0) { - MkMsg_t msg; + MkMsg_t msg; - msg.address = address; - msg.cmd = cmd; - msg.nbPars = 1; - msg.pars[0] = par0; + msg.address = address; + msg.cmd = cmd; + msg.nbPars = 1; + msg.pars[0] = par0; - SendMsg(&msg); + SendMsg(&msg); } -static uint16_t VersionMsg_GetVersion(const MkMsg_t * msg) +static uint16_t VersionMsg_GetVersion(const MkMsg_t *msg) { - return msg->pars[0] * 100 + msg->pars[1]; + return msg->pars[0] * 100 + msg->pars[1]; } static void DoConnectedToFC(void) { - AttitudeActualData attitudeData; - MkMsg_t msg; + AttitudeActualData attitudeData; + MkMsg_t msg; - DEBUG_MSG("FC\n\r"); + DEBUG_MSG("FC\n\r"); - memset(&attitudeData, 0, sizeof(attitudeData)); + memset(&attitudeData, 0, sizeof(attitudeData)); - // Configure FC for fast reporting of the debug-message - SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_DEBUG, 10); + // Configure FC for fast reporting of the debug-message + SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_DEBUG, 10); - while (TRUE) { - if (WaitForMsg(MSGCMD_DEBUG, &msg, 500 / portTICK_RATE_MS)) { - int16_t nick; - int16_t roll; + while (TRUE) { + if (WaitForMsg(MSGCMD_DEBUG, &msg, 500 / portTICK_RATE_MS)) { + int16_t nick; + int16_t roll; - //PrintMsg(&msg); - nick = Par2Int16(&msg, DEBUG_MSG_NICK_IDX); - roll = Par2Int16(&msg, DEBUG_MSG_ROLL_IDX); + // PrintMsg(&msg); + nick = Par2Int16(&msg, DEBUG_MSG_NICK_IDX); + roll = Par2Int16(&msg, DEBUG_MSG_ROLL_IDX); - DEBUG_MSG("Att: Nick=%5d Roll=%5d\n\r", nick, roll); + DEBUG_MSG("Att: Nick=%5d Roll=%5d\n\r", nick, roll); - attitudeData.Pitch = -(float)nick / 10; - attitudeData.Roll = -(float)roll / 10; - AttitudeActualSet(&attitudeData); - } else { - DEBUG_MSG("TO\n\r"); - break; - } - } + attitudeData.Pitch = -(float)nick / 10; + attitudeData.Roll = -(float)roll / 10; + AttitudeActualSet(&attitudeData); + } else { + DEBUG_MSG("TO\n\r"); + break; + } + } } static void DoConnectedToNC(void) { - MkMsg_t msg; - GpsPosition_t pos; - AttitudeActualData attitudeData; - PositionActualData positionData; - FlightBatteryStateData flightBatteryData; + MkMsg_t msg; + GpsPosition_t pos; + AttitudeActualData attitudeData; + PositionActualData positionData; + FlightBatteryStateData flightBatteryData; + #ifdef GENERATE_BATTERY_INFO - uint8_t battStateCnt = 0; + uint8_t battStateCnt = 0; #endif - DEBUG_MSG("NC\n\r"); + DEBUG_MSG("NC\n\r"); - memset(&attitudeData, 0, sizeof(attitudeData)); - memset(&positionData, 0, sizeof(positionData)); - memset(&flightBatteryData, 0, sizeof(flightBatteryData)); + memset(&attitudeData, 0, sizeof(attitudeData)); + memset(&positionData, 0, sizeof(positionData)); + memset(&flightBatteryData, 0, sizeof(flightBatteryData)); - // Configure NC for fast reporting of the osd-message - SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_OSD, 10); + // Configure NC for fast reporting of the osd-message + SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_OSD, 10); - while (TRUE) { - if (WaitForMsg(MSGCMD_OSD, &msg, 500 / portTICK_RATE_MS)) { - //PrintMsg(&msg); - GetGpsPos(&msg, OSD_MSG_CURRPOS_IDX, &pos); + while (TRUE) { + if (WaitForMsg(MSGCMD_OSD, &msg, 500 / portTICK_RATE_MS)) { + // PrintMsg(&msg); + GetGpsPos(&msg, OSD_MSG_CURRPOS_IDX, &pos); #if 0 - DEBUG_MSG("Bat=%d\n\r", msg.pars[OSD_MSG_BATT_IDX]); - DEBUG_MSG("Nick=%d Roll=%d\n\r", Par2Int8(&msg, OSD_MSG_NICK_IDX), Par2Int8(&msg, OSD_MSG_ROLL_IDX)); - DEBUG_MSG("POS #Sats=%d stat=%d lat=%d lon=%d alt=%d\n\r", msg.pars[OSD_MSG_NB_SATS_IDX], pos.status, (int)pos.latitude, - (int)pos.longitute, (int)pos.altitude); + DEBUG_MSG("Bat=%d\n\r", msg.pars[OSD_MSG_BATT_IDX]); + DEBUG_MSG("Nick=%d Roll=%d\n\r", Par2Int8(&msg, OSD_MSG_NICK_IDX), Par2Int8(&msg, OSD_MSG_ROLL_IDX)); + DEBUG_MSG("POS #Sats=%d stat=%d lat=%d lon=%d alt=%d\n\r", msg.pars[OSD_MSG_NB_SATS_IDX], pos.status, (int)pos.latitude, + (int)pos.longitute, (int)pos.altitude); #else - DEBUG_MSG("."); + DEBUG_MSG("."); #endif - attitudeData.Pitch = -Par2Int8(&msg, OSD_MSG_NICK_IDX); - attitudeData.Roll = -Par2Int8(&msg, OSD_MSG_ROLL_IDX); - AttitudeActualSet(&attitudeData); + attitudeData.Pitch = -Par2Int8(&msg, OSD_MSG_NICK_IDX); + attitudeData.Roll = -Par2Int8(&msg, OSD_MSG_ROLL_IDX); + AttitudeActualSet(&attitudeData); - positionData.Longitude = pos.longitute; - positionData.Latitude = pos.latitude; - positionData.Altitude = pos.altitude; - positionData.Satellites = msg.pars[OSD_MSG_NB_SATS_IDX]; - positionData.Heading = Par2Int16(&msg, OSD_MSG_COMPHEADING_IDX); - positionData.Groundspeed = ((float)Par2Int16(&msg, OSD_MSG_GNDSPEED_IDX)) / 100 /* cm/s => m/s */ ; - if (positionData.Satellites < 5) { - positionData.Status = POSITIONACTUAL_STATUS_NOFIX; - } else { - positionData.Status = POSITIONACTUAL_STATUS_FIX3D; - } - PositionActualSet(&positionData); + positionData.Longitude = pos.longitute; + positionData.Latitude = pos.latitude; + positionData.Altitude = pos.altitude; + positionData.Satellites = msg.pars[OSD_MSG_NB_SATS_IDX]; + positionData.Heading = Par2Int16(&msg, OSD_MSG_COMPHEADING_IDX); + positionData.Groundspeed = ((float)Par2Int16(&msg, OSD_MSG_GNDSPEED_IDX)) / 100 /* cm/s => m/s */; + if (positionData.Satellites < 5) { + positionData.Status = POSITIONACTUAL_STATUS_NOFIX; + } else { + positionData.Status = POSITIONACTUAL_STATUS_FIX3D; + } + PositionActualSet(&positionData); #if GENERATE_BATTERY_INFO - if (++battStateCnt > 2) { - flightBatteryData.Voltage = (float)msg.pars[OSD_MSG_BATT_IDX] / 10; - FlightBatteryStateSet(&flightBatteryData); - battStateCnt = 0; - } + if (++battStateCnt > 2) { + flightBatteryData.Voltage = (float)msg.pars[OSD_MSG_BATT_IDX] / 10; + FlightBatteryStateSet(&flightBatteryData); + battStateCnt = 0; + } #endif - } else { - DEBUG_MSG("TO\n\r"); - break; - } - } + } else { + DEBUG_MSG("TO\n\r"); + break; + } + } } static void MkSerialTask(__attribute__((unused)) void *parameters) { - MkMsg_t msg; - uint32_t version; - bool connectionOk = FALSE; + MkMsg_t msg; + uint32_t version; + bool connectionOk = FALSE; - PIOS_COM_ChangeBaud(PORT, 57600); - PIOS_COM_ChangeBaud(DEBUG_PORT, 57600); + PIOS_COM_ChangeBaud(PORT, 57600); + PIOS_COM_ChangeBaud(DEBUG_PORT, 57600); - DEBUG_MSG("MKSerial Started\n\r"); + DEBUG_MSG("MKSerial Started\n\r"); - while (1) { - // Wait until we get version from MK - while (!connectionOk) { - SendMsgParNone(MK_ADDR_ALL, MSGCMD_GET_VERSION); - DEBUG_MSG("Version... "); - if (WaitForMsg(MSGCMD_VERSION, &msg, 250 / portTICK_RATE_MS)) { - version = VersionMsg_GetVersion(&msg); - DEBUG_MSG("%d\n\r", version); - connectionOk = TRUE; - } else { - DEBUG_MSG("TO\n\r"); - } - } + while (1) { + // Wait until we get version from MK + while (!connectionOk) { + SendMsgParNone(MK_ADDR_ALL, MSGCMD_GET_VERSION); + DEBUG_MSG("Version... "); + if (WaitForMsg(MSGCMD_VERSION, &msg, 250 / portTICK_RATE_MS)) { + version = VersionMsg_GetVersion(&msg); + DEBUG_MSG("%d\n\r", version); + connectionOk = TRUE; + } else { + DEBUG_MSG("TO\n\r"); + } + } - // Dependent on version, decide it we are connected to NC or FC - // TODO: use slave-addr to distinguish FC/NC -> much safer - if (version < 60) { - DoConnectedToNC(); // Will only return after an error - } else { - DoConnectedToFC(); // Will only return after an error - } + // Dependent on version, decide it we are connected to NC or FC + // TODO: use slave-addr to distinguish FC/NC -> much safer + if (version < 60) { + DoConnectedToNC(); // Will only return after an error + } else { + DoConnectedToFC(); // Will only return after an error + } - connectionOk = FALSE; + connectionOk = FALSE; - vTaskDelay(250 / portTICK_RATE_MS); - } + vTaskDelay(250 / portTICK_RATE_MS); + } } /** @@ -547,13 +553,13 @@ static void MkSerialTask(__attribute__((unused)) void *parameters) */ int32_t MKSerialInitialize(void) { - // Start gps task - xTaskCreate(MkSerialTask, (signed char *)"MkSerial", STACK_SIZE, NULL, TASK_PRIORITY, NULL); + // Start gps task + xTaskCreate(MkSerialTask, (signed char *)"MkSerial", STACK_SIZE, NULL, TASK_PRIORITY, NULL); - return 0; + return 0; } -/** - * @} +/** + * @} * @} */ diff --git a/flight/modules/MK/MKSerial/inc/MkSerial.h b/flight/modules/MK/MKSerial/inc/MkSerial.h index 85915fe86..264d2bb3f 100644 --- a/flight/modules/MK/MKSerial/inc/MkSerial.h +++ b/flight/modules/MK/MKSerial/inc/MkSerial.h @@ -1,16 +1,16 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup MKSerialModule MK Serial Control Module * @brief Connect to MK module - * @{ + * @{ * * @file GPS.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Include file of the GPS module. - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and * objects. * @see The GNU Public License (GPL) Version 3 * @@ -38,7 +38,7 @@ int32_t MKSerialInitialize(void); #endif // MK_SER_INPUT_H -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/modules/ManualControl/inc/manualcontrol.h b/flight/modules/ManualControl/inc/manualcontrol.h index 7b841a035..6278daa45 100644 --- a/flight/modules/ManualControl/inc/manualcontrol.h +++ b/flight/modules/ManualControl/inc/manualcontrol.h @@ -32,23 +32,24 @@ #include "manualcontrolcommand.h" -typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3, FLIGHTMODE_TUNING = 4} flightmode_path; +typedef enum { FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3, FLIGHTMODE_TUNING = 4 } flightmode_path; -#define PARSE_FLIGHT_MODE(x) ( \ - (x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \ - (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \ - (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \ - (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \ - (x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_LAND) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : \ - (x == FLIGHTSTATUS_FLIGHTMODE_POI) ? FLIGHTMODE_GUIDANCE : \ - FLIGHTMODE_UNDEFINED \ - ) +#define PARSE_FLIGHT_MODE(x) \ + ( \ + (x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \ + (x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_LAND) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : \ + (x == FLIGHTSTATUS_FLIGHTMODE_POI) ? FLIGHTMODE_GUIDANCE : \ + FLIGHTMODE_UNDEFINED \ + ) int32_t ManualControlInitialize(); @@ -57,72 +58,79 @@ int32_t ManualControlInitialize(); * These are assumptions we make in the flight code about the order of settings and their consistency between * objects. Please keep this synchronized to the UAVObjects */ -#define assumptions1 ( \ -((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ -) +#define assumptions1 \ + ( \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ) -#define assumptions3 ( \ -((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ -) +#define assumptions3 \ + ( \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ) -#define assumptions5 ( \ -((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ -) +#define assumptions5 \ + ( \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ) -#define ARMING_CHANNEL_ROLL 0 -#define ARMING_CHANNEL_PITCH 1 -#define ARMING_CHANNEL_YAW 2 +#define ARMING_CHANNEL_ROLL 0 +#define ARMING_CHANNEL_PITCH 1 +#define ARMING_CHANNEL_YAW 2 -#define assumptions7 ( \ -( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \ -) +#define assumptions7 \ + ( \ + (((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_ROLL) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_ROLL) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_PITCH) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_PITCH) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_YAW) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_YAW) \ + ) -#define assumptions8 ( \ -( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \ -) +#define assumptions8 \ + ( \ + (((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 == 0) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 != 0) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 == 0) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 != 0) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 == 0) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 != 0) \ + ) -#define assumptions_flightmode ( \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int) FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int) FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND == (int) FLIGHTSTATUS_FLIGHTMODE_LAND) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int) FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI == (int) FLIGHTSTATUS_FLIGHTMODE_POI) \ -) +#define assumptions_flightmode \ + ( \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int)FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \ + ) -#define assumptions_channelcount ( \ -( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM ) && \ -( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM ) && \ -( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM ) && \ -( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM ) && \ -( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM ) ) +#define assumptions_channelcount \ + ( \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM)) #endif // MANUALCONTROL_H diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 91bac9c0e..aac310600 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -56,31 +56,30 @@ #if defined(PIOS_INCLUDE_USB_RCTX) #include "pios_usb_rctx.h" -#endif /* PIOS_INCLUDE_USB_RCTX */ +#endif /* PIOS_INCLUDE_USB_RCTX */ // Private constants #if defined(PIOS_MANUAL_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE +#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE #else -#define STACK_SIZE_BYTES 1024 +#define STACK_SIZE_BYTES 1024 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY+4) -#define UPDATE_PERIOD_MS 20 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) +#define UPDATE_PERIOD_MS 20 #define THROTTLE_FAILSAFE -0.1f -#define ARMED_TIME_MS 1000 -#define ARMED_THRESHOLD 0.50f -//safe band to allow a bit of calibration error or trim offset (in microseconds) +#define ARMED_TIME_MS 1000 +#define ARMED_THRESHOLD 0.50f +// safe band to allow a bit of calibration error or trim offset (in microseconds) #define CONNECTION_OFFSET 250 // Private types -typedef enum -{ - ARM_STATE_DISARMED, - ARM_STATE_ARMING_MANUAL, - ARM_STATE_ARMED, - ARM_STATE_DISARMING_MANUAL, - ARM_STATE_DISARMING_TIMEOUT +typedef enum { + ARM_STATE_DISARMED, + ARM_STATE_ARMING_MANUAL, + ARM_STATE_ARMED, + ARM_STATE_DISARMING_MANUAL, + ARM_STATE_DISARMING_TIMEOUT } ArmState_t; // Private variables @@ -94,15 +93,15 @@ static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM]; #endif // Private functions -static void updateActuatorDesired(ManualControlCommandData * cmd); -static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); -static void updateLandDesired(ManualControlCommandData * cmd, bool changed); -static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed); -static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home); -static void processFlightMode(ManualControlSettingsData * settings, float flightMode); -static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); +static void updateActuatorDesired(ManualControlCommandData *cmd); +static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings); +static void updateLandDesired(ManualControlCommandData *cmd, bool changed); +static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed); +static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home); +static void processFlightMode(ManualControlSettingsData *settings, float flightMode); +static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings); static void setArmedIfChanged(uint8_t val); -static void configurationUpdatedCb(UAVObjEvent * ev); +static void configurationUpdatedCb(UAVObjEvent *ev); static void manualControlTask(void *parameters); static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral); @@ -116,17 +115,16 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel #endif #define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 -#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10 -struct rcvr_activity_fsm -{ +#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10 +struct rcvr_activity_fsm { ManualControlSettingsChannelGroupsOptions group; uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; uint8_t sample_count; }; static struct rcvr_activity_fsm activity_fsm; -static void resetRcvrActivity(struct rcvr_activity_fsm * fsm); -static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm); +static void resetRcvrActivity(struct rcvr_activity_fsm *fsm); +static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm); #define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode && assumptions_channelcount) @@ -136,7 +134,7 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm); int32_t ManualControlStart() { // Start main task - xTaskCreate(manualControlTask, (signed char *) "ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); @@ -150,10 +148,10 @@ int32_t ManualControlStart() */ int32_t ManualControlInitialize() { - /* Check the assumptions about uavobject enum's are correct */ - if (!assumptions) + if (!assumptions) { return -1; + } AccessoryDesiredInitialize(); ManualControlCommandInitialize(); @@ -164,7 +162,7 @@ int32_t ManualControlInitialize() return 0; } -MODULE_INITCALL( ManualControlInitialize, ManualControlStart) +MODULE_INITCALL(ManualControlInitialize, ManualControlStart) /** * Module task @@ -177,7 +175,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) float flightMode = 0; uint8_t disconnected_count = 0; - uint8_t connected_count = 0; + uint8_t connected_count = 0; // For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically // this includes not even registering it if not used @@ -206,10 +204,9 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // Main task loop lastSysTime = xTaskGetTickCount(); - float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = {0}; + float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = { 0 }; while (1) { - // Wait until next update vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); #ifdef PIOS_INCLUDE_WDG @@ -244,7 +241,6 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } if (!ManualControlCommandReadOnly()) { - bool valid_input_detected = true; // Read channel values in us @@ -259,37 +255,37 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // If a channel has timed out this is not valid data and we shouldn't update anything // until we decide to go to failsafe - if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) + if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) { valid_input_detected = false; - else + } else { scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); + } } // Check settings, if error raise alarm if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || - // Check all channel mappings are valid - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID - || - // Check the driver exists - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER || - // Check the FlightModeNumber is valid - settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM || - // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care - ((settings.FlightModeNumber > 1) - && (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) { - + || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || + // Check all channel mappings are valid + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID + || + // Check the driver exists + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER || + // Check the FlightModeNumber is valid + settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM || + // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care + ((settings.FlightModeNumber > 1) + && (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); @@ -303,32 +299,32 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // decide if we have valid manual input or not valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) - && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) - && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) - && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) + && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) + && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) + && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); // Implement hysteresis loop on connection status if (valid_input_detected && (++connected_count > 10)) { - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; - connected_count = 0; + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; + connected_count = 0; disconnected_count = 0; } else if (!valid_input_detected && (++disconnected_count > 10)) { - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; - connected_count = 0; + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + connected_count = 0; disconnected_count = 0; } if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { - cmd.Throttle = -1; // Shut down engine with no control - cmd.Roll = 0; + cmd.Throttle = -1; // Shut down engine with no control + cmd.Roll = 0; cmd.Yaw = 0; - cmd.Pitch = 0; + cmd.Pitch = 0; cmd.Collective = 0; - //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning + // cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, // or leave throttle at IDLE speed or above when going into AUTO-failsafe. AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); @@ -337,31 +333,33 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // Set Accessory 0 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; - if (AccessoryDesiredInstSet(0, &accessory) != 0) + if (AccessoryDesiredInstSet(0, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } // Set Accessory 1 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; - if (AccessoryDesiredInstSet(1, &accessory) != 0) + if (AccessoryDesiredInstSet(1, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } // Set Accessory 2 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; - if (AccessoryDesiredInstSet(2, &accessory) != 0) + if (AccessoryDesiredInstSet(2, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } - } else if (valid_input_detected) { AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); // Scale channels to -1 -> +1 range - cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; - cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; - cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; + cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; + cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; + cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; - flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; + flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; // Apply deadband for Roll/Pitch/Yaw stick inputs if (settings.Deadband > 0.0f) { @@ -373,18 +371,19 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // Apply Low Pass Filter to input channels, time delta between calls in ms portTickType thisSysTime = xTaskGetTickCount(); float dT = (thisSysTime > lastSysTimeLPF) ? - (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : - (float)UPDATE_PERIOD_MS; + (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : + (float)UPDATE_PERIOD_MS; lastSysTimeLPF = thisSysTime; applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT); applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT); applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT); #endif // USE_INPUT_LPF - if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID - && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER - && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT) + if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID + && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER + && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) { cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; + } AccessoryDesiredData accessory; // Set Accessory 0 @@ -393,8 +392,9 @@ static void manualControlTask(__attribute__((unused)) void *parameters) #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); #endif - if (AccessoryDesiredInstSet(0, &accessory) != 0) + if (AccessoryDesiredInstSet(0, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } // Set Accessory 1 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { @@ -402,8 +402,9 @@ static void manualControlTask(__attribute__((unused)) void *parameters) #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); #endif - if (AccessoryDesiredInstSet(1, &accessory) != 0) + if (AccessoryDesiredInstSet(1, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } // Set Accessory 2 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { @@ -411,12 +412,12 @@ static void manualControlTask(__attribute__((unused)) void *parameters) #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); #endif - if (AccessoryDesiredInstSet(2, &accessory) != 0) + if (AccessoryDesiredInstSet(2, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } processFlightMode(&settings, flightMode); - } // Process arming outside conditional so system will disarm when disconnected @@ -427,13 +428,12 @@ static void manualControlTask(__attribute__((unused)) void *parameters) #if defined(PIOS_INCLUDE_USB_RCTX) if (pios_usb_rctx_id) { PIOS_USB_RCTX_Update(pios_usb_rctx_id, - cmd.Channel, - settings.ChannelMin, - settings.ChannelMax, - NELEMENTS(cmd.Channel)); + cmd.Channel, + settings.ChannelMin, + settings.ChannelMax, + NELEMENTS(cmd.Channel)); } -#endif /* PIOS_INCLUDE_USB_RCTX */ - +#endif /* PIOS_INCLUDE_USB_RCTX */ } else { ManualControlCommandGet(&cmd); /* Under GCS control */ } @@ -443,48 +443,48 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // Depending on the mode update the Stabilization or Actuator objects static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { - case FLIGHTMODE_UNDEFINED: - // This reflects a bug in the code architecture! + case FLIGHTMODE_UNDEFINED: + // This reflects a bug in the code architecture! + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + break; + case FLIGHTMODE_MANUAL: + updateActuatorDesired(&cmd); + break; + case FLIGHTMODE_STABILIZED: + updateStabilizationDesired(&cmd, &settings); + break; + case FLIGHTMODE_TUNING: + // Tuning takes settings directly from manualcontrolcommand. No need to + // call anything else. This just avoids errors. + break; + case FLIGHTMODE_GUIDANCE: + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: + altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode); + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + case FLIGHTSTATUS_FLIGHTMODE_POI: + updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false); + break; + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true); + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + // No need to call anything. This just avoids errors. + break; + case FLIGHTSTATUS_FLIGHTMODE_LAND: + updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode); + break; + default: AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - break; - case FLIGHTMODE_MANUAL: - updateActuatorDesired(&cmd); - break; - case FLIGHTMODE_STABILIZED: - updateStabilizationDesired(&cmd, &settings); - break; - case FLIGHTMODE_TUNING: - // Tuning takes settings directly from manualcontrolcommand. No need to - // call anything else. This just avoids errors. - break; - case FLIGHTMODE_GUIDANCE: - switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: - altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode); - break; - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - case FLIGHTSTATUS_FLIGHTMODE_POI: - updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false); - break; - case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true); - break; - case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: - // No need to call anything. This just avoids errors. - break; - case FLIGHTSTATUS_FLIGHTMODE_LAND: - updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode); - break; - default: - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - } - break; + } + break; } lastFlightMode = flightStatus.FlightMode; } } -static void resetRcvrActivity(struct rcvr_activity_fsm * fsm) +static void resetRcvrActivity(struct rcvr_activity_fsm *fsm) { ReceiverActivityData data; bool updated = false; @@ -492,7 +492,7 @@ static void resetRcvrActivity(struct rcvr_activity_fsm * fsm) /* Clear all channel activity flags */ ReceiverActivityGet(&data); if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) { - data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; + data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; data.ActiveChannel = 255; updated = true; } @@ -513,14 +513,14 @@ static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8 } } -static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm * fsm) +static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm *fsm) { bool activity_updated = false; /* Compare the current value to the previous sampled value */ for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) { uint16_t delta; - uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed + uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel); if (curr > prev) { delta = curr - prev; @@ -534,41 +534,41 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */ switch (fsm->group) { - case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: - group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: - group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: - group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: - group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK: - group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK; - break; - default: - PIOS_Assert(0); - break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: + group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: + group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK: + group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK; + break; + default: + PIOS_Assert(0); + break; } - ReceiverActivityActiveGroupSet((uint8_t*) &group); + ReceiverActivityActiveGroupSet((uint8_t *)&group); ReceiverActivityActiveChannelSet(&channel); activity_updated = true; } } - return (activity_updated); + return activity_updated; } -static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) +static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm) { bool activity_updated = false; @@ -587,13 +587,13 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) /* Take a sample of each channel in this group */ updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev)); fsm->sample_count++; - return (false); + return false; } /* Compare with previous sample */ activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm); - group_completed: +group_completed: /* Reset the sample counter */ fsm->sample_count = 0; @@ -617,87 +617,89 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) } } - return (activity_updated); + return activity_updated; } -static void updateActuatorDesired(ManualControlCommandData * cmd) +static void updateActuatorDesired(ManualControlCommandData *cmd) { ActuatorDesiredData actuator; + ActuatorDesiredGet(&actuator); - actuator.Roll = cmd->Roll; - actuator.Pitch = cmd->Pitch; - actuator.Yaw = cmd->Yaw; + actuator.Roll = cmd->Roll; + actuator.Pitch = cmd->Pitch; + actuator.Yaw = cmd->Yaw; actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; ActuatorDesiredSet(&actuator); } -static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings) +static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings) { StabilizationDesiredData stabilization; + StabilizationDesiredGet(&stabilization); StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); - uint8_t * stab_settings; + uint8_t *stab_settings; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - stab_settings = settings->Stabilization1Settings; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - stab_settings = settings->Stabilization2Settings; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - stab_settings = settings->Stabilization3Settings; - break; - default: - // Major error, this should not occur because only enter this block when one of these is true - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - return; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + stab_settings = settings->Stabilization1Settings; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + stab_settings = settings->Stabilization2Settings; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + stab_settings = settings->Stabilization3Settings; + break; + default: + // Major error, this should not occur because only enter this block when one of these is true + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + return; } // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; stabilization.Roll = - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? - cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? + cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? + cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode ; stabilization.Pitch = - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? - cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? - cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? + cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? + cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? + cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode stabilization.Yaw = - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? + cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; StabilizationDesiredSet(&stabilization); @@ -709,14 +711,14 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon * @brief Update the position desired to current location when * enabled and allow the waypoint to be moved by transmitter */ -static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * cmd, bool changed,bool home) +static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed, bool home) { /* - static portTickType lastSysTime; - portTickType thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); - lastSysTime = thisSysTime; - */ + static portTickType lastSysTime; + portTickType thisSysTime = xTaskGetTickCount(); + dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); + lastSysTime = thisSysTime; + */ if (home && changed) { // Simple Return To Base mode - keep altitude the same, fly to home position @@ -726,16 +728,16 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * PathDesiredData pathDesired; PathDesiredGet(&pathDesired); pathDesired.Start[PATHDESIRED_START_NORTH] = 0; - pathDesired.Start[PATHDESIRED_START_EAST] = 0; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10; - pathDesired.End[PATHDESIRED_END_NORTH] = 0; + pathDesired.Start[PATHDESIRED_START_EAST] = 0; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10; + pathDesired.End[PATHDESIRED_END_NORTH] = 0; pathDesired.End[PATHDESIRED_END_EAST] = 0; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10; - pathDesired.StartingVelocity=1; - pathDesired.EndingVelocity=0; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); - } else if(changed) { + } else if (changed) { // After not being in this mode for a while init at current height PositionActualData positionActual; PositionActualGet(&positionActual); @@ -743,57 +745,58 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * PathDesiredData pathDesired; PathDesiredGet(&pathDesired); pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; - pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; + pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; - pathDesired.StartingVelocity=1; - pathDesired.EndingVelocity=0; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. - } else { - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; - pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); + } else { + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; + pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); */ } } -static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * cmd, bool changed) +static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed) { /* - static portTickType lastSysTime; - portTickType thisSysTime; - float dT; + static portTickType lastSysTime; + portTickType thisSysTime; + float dT; - thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); - lastSysTime = thisSysTime; - */ + thisSysTime = xTaskGetTickCount(); + dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); + lastSysTime = thisSysTime; + */ PositionActualData positionActual; + PositionActualGet(&positionActual); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - if(changed) { + if (changed) { // After not being in this mode for a while init at current height pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; - pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; + pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; - pathDesired.StartingVelocity=1; - pathDesired.EndingVelocity=0; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; } - pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down+5; + pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down + 5; PathDesiredSet(&pathDesired); } @@ -802,7 +805,7 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * * enabled and enable altitude mode for stabilization * @todo: Need compile flag to exclude this from copter control */ -static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) +static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) { const float DEADBAND = 0.10f; const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; @@ -818,6 +821,7 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) float dT; AltitudeHoldDesiredData altitudeHoldDesiredData; + AltitudeHoldDesiredGet(&altitudeHoldDesiredData); AltitudeHoldSettingsThrottleExpGet(&throttleExp); @@ -826,17 +830,17 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); - thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTimeAH)? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f); - lastSysTimeAH = thisSysTime; + thisSysTime = xTaskGetTickCount(); + dT = ((thisSysTime == lastSysTimeAH) ? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f); + lastSysTimeAH = thisSysTime; - altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; + altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; - altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; + altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; float currentDown; PositionActualDownGet(¤tDown); - if(changed) { + if (changed) { // After not being in this mode for a while init at current height altitudeHoldDesiredData.Altitude = 0; zeroed = false; @@ -845,7 +849,7 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) // then apply an "exp" f(x,k) = (k∙x∙x∙x + x∙(256−k)) / 256 altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT; } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT; + altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT; } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) { // Require the stick to enter the dead band before they can move height zeroed = true; @@ -853,30 +857,30 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) AltitudeHoldDesiredSet(&altitudeHoldDesiredData); } -#else +#else /* if defined(REVOLUTION) */ // TODO: These functions should never be accessible on CC. Any configuration that // could allow them to be called should already throw an error to prevent this happening // in flight -static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * cmd, - __attribute__((unused)) bool changed, - __attribute__((unused)) bool home) +static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, + __attribute__((unused)) bool changed, + __attribute__((unused)) bool home) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); } -static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * cmd, - __attribute__((unused)) bool changed) +static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, + __attribute__((unused)) bool changed) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); } -static void altitudeHoldDesired(__attribute__((unused)) ManualControlCommandData * cmd, - __attribute__((unused)) bool changed) +static void altitudeHoldDesired(__attribute__((unused)) ManualControlCommandData *cmd, + __attribute__((unused)) bool changed) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); } -#endif +#endif /* if defined(REVOLUTION) */ /** * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. */ @@ -887,13 +891,13 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr // Scale if ((max > min && value >= neutral) || (min > max && value <= neutral)) { if (max != neutral) { - valueScaled = (float) (value - neutral) / (float) (max - neutral); + valueScaled = (float)(value - neutral) / (float)(max - neutral); } else { valueScaled = 0; } } else { if (min != neutral) { - valueScaled = (float) (value - neutral) / (float) (neutral - min); + valueScaled = (float)(value - neutral) / (float)(neutral - min); } else { valueScaled = 0; } @@ -909,8 +913,9 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr return valueScaled; } -static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) { - return (end_time - start_time) * portTICK_RATE_MS; +static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) +{ + return (end_time - start_time) * portTICK_RATE_MS; } /** @@ -926,27 +931,27 @@ static bool okToArm(void) // Check each alarm for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { - if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set + if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { continue; - } + } return false; } } - uint8_t flightMode; - FlightStatusFlightModeGet(&flightMode); - switch(flightMode) { - case FLIGHTSTATUS_FLIGHTMODE_MANUAL: - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - return true; - default: - return false; + uint8_t flightMode; + FlightStatusFlightModeGet(&flightMode); + switch (flightMode) { + case FLIGHTSTATUS_FLIGHTMODE_MANUAL: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + return true; - } + default: + return false; + } } /** * @brief Determine if the aircraft is forced to disarm by an explicit alarm @@ -956,6 +961,7 @@ static bool forcedDisArm(void) { // read alarms SystemAlarmsData alarms; + SystemAlarmsGet(&alarms); if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) { @@ -971,6 +977,7 @@ static bool forcedDisArm(void) static void setArmedIfChanged(uint8_t val) { FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); if (flightStatus.Armed != val) { @@ -984,9 +991,8 @@ static void setArmedIfChanged(uint8_t val) * @param[out] cmd The structure to set the armed in * @param[in] settings Settings indicating the necessary position */ -static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) +static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings) { - bool lowThrottle = cmd->Throttle <= 0; if (forcedDisArm()) { @@ -1000,22 +1006,23 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); } else { // Not really needed since this function not called when disconnected - if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) + if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { lowThrottle = true; + } // The throttle is not low, in case we where arming or disarming, abort if (!lowThrottle) { switch (armState) { - case ARM_STATE_DISARMING_MANUAL: - case ARM_STATE_DISARMING_TIMEOUT: - armState = ARM_STATE_ARMED; - break; - case ARM_STATE_ARMING_MANUAL: - armState = ARM_STATE_DISARMED; - break; - default: - // Nothing needs to be done in the other states - break; + case ARM_STATE_DISARMING_MANUAL: + case ARM_STATE_DISARMING_TIMEOUT: + armState = ARM_STATE_ARMED; + break; + case ARM_STATE_ARMING_MANUAL: + armState = ARM_STATE_DISARMED; + break; + default: + // Nothing needs to be done in the other states + break; } return; } @@ -1035,72 +1042,76 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData // Calc channel see assumptions7 int8_t sign = ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2) ? -1 : 1; switch ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) { - case ARMING_CHANNEL_ROLL: - armingInputLevel = sign * cmd->Roll; - break; - case ARMING_CHANNEL_PITCH: - armingInputLevel = sign * cmd->Pitch; - break; - case ARMING_CHANNEL_YAW: - armingInputLevel = sign * cmd->Yaw; - break; + case ARMING_CHANNEL_ROLL: + armingInputLevel = sign * cmd->Roll; + break; + case ARMING_CHANNEL_PITCH: + armingInputLevel = sign * cmd->Pitch; + break; + case ARMING_CHANNEL_YAW: + armingInputLevel = sign * cmd->Yaw; + break; } - bool manualArm = false; + bool manualArm = false; bool manualDisarm = false; - if (armingInputLevel <= -ARMED_THRESHOLD) + if (armingInputLevel <= -ARMED_THRESHOLD) { manualArm = true; - else if (armingInputLevel >= +ARMED_THRESHOLD) + } else if (armingInputLevel >= +ARMED_THRESHOLD) { manualDisarm = true; + } switch (armState) { - case ARM_STATE_DISARMED: - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + case ARM_STATE_DISARMED: + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - // only allow arming if it's OK too - if (manualArm && okToArm()) { - armedDisarmStart = lastSysTime; - armState = ARM_STATE_ARMING_MANUAL; - } - break; - - case ARM_STATE_ARMING_MANUAL: - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); - - if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) - armState = ARM_STATE_ARMED; - else if (!manualArm) - armState = ARM_STATE_DISARMED; - break; - - case ARM_STATE_ARMED: - // When we get here, the throttle is low, - // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled + // only allow arming if it's OK too + if (manualArm && okToArm()) { armedDisarmStart = lastSysTime; - armState = ARM_STATE_DISARMING_TIMEOUT; - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); - break; + armState = ARM_STATE_ARMING_MANUAL; + } + break; - case ARM_STATE_DISARMING_TIMEOUT: - // We get here when armed while throttle low, even when the arming timeout is not enabled - if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) - armState = ARM_STATE_DISARMED; + case ARM_STATE_ARMING_MANUAL: + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); - // Switch to disarming due to manual control when needed - if (manualDisarm) { - armedDisarmStart = lastSysTime; - armState = ARM_STATE_DISARMING_MANUAL; - } - break; + if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) { + armState = ARM_STATE_ARMED; + } else if (!manualArm) { + armState = ARM_STATE_DISARMED; + } + break; - case ARM_STATE_DISARMING_MANUAL: - if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) - armState = ARM_STATE_DISARMED; - else if (!manualDisarm) - armState = ARM_STATE_ARMED; - break; - } // End Switch + case ARM_STATE_ARMED: + // When we get here, the throttle is low, + // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled + armedDisarmStart = lastSysTime; + armState = ARM_STATE_DISARMING_TIMEOUT; + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); + break; + + case ARM_STATE_DISARMING_TIMEOUT: + // We get here when armed while throttle low, even when the arming timeout is not enabled + if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) { + armState = ARM_STATE_DISARMED; + } + + // Switch to disarming due to manual control when needed + if (manualDisarm) { + armedDisarmStart = lastSysTime; + armState = ARM_STATE_DISARMING_MANUAL; + } + break; + + case ARM_STATE_DISARMING_MANUAL: + if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) { + armState = ARM_STATE_DISARMED; + } else if (!manualDisarm) { + armState = ARM_STATE_ARMED; + } + break; + } // End Switch } } @@ -1113,12 +1124,14 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData static void processFlightMode(ManualControlSettingsData *settings, float flightMode) { FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); // Convert flightMode value into the switch position in the range [0..N-1] uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9; - if (pos >= settings->FlightModeNumber) + if (pos >= settings->FlightModeNumber) { pos = settings->FlightModeNumber - 1; + } uint8_t newMode = settings->FlightModePosition[pos]; @@ -1139,7 +1152,7 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value) min = max; max = tmp; } - return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET); + return value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET; } /** @@ -1147,12 +1160,13 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value) */ static void applyDeadband(float *value, float deadband) { - if (fabsf(*value) < deadband) + if (fabsf(*value) < deadband) { *value = 0.0f; - else if (*value > 0.0f) + } else if (*value > 0.0f) { *value -= deadband; - else + } else { *value += deadband; + } } #ifdef USE_INPUT_LPF diff --git a/flight/modules/OPLink/inc/oplinkmod.h b/flight/modules/OPLink/inc/oplinkmod.h index faeebad1b..e1481149b 100644 --- a/flight/modules/OPLink/inc/oplinkmod.h +++ b/flight/modules/OPLink/inc/oplinkmod.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup OPLinkModule OPLink Module - * @{ + * @{ * * @file oplinkmod.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. diff --git a/flight/modules/OPLink/oplinkmod.c b/flight/modules/OPLink/oplinkmod.c index ea7c58293..3947ee569 100644 --- a/flight/modules/OPLink/oplinkmod.c +++ b/flight/modules/OPLink/oplinkmod.c @@ -8,12 +8,12 @@ # which then contains hardware specific implementations * (currently only STM32 supported) * - * @{ + * @{ * @addtogroup OPLinkModule OPLink Module * @brief Initializes PIOS and other modules runs monitoring * After initializing all the modules runs basic monitoring and * alarms. - * @{ + * @{ * * @file oplinkmod.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. @@ -51,12 +51,12 @@ #define SYSTEM_UPDATE_PERIOD_MS 1000 #if defined(PIOS_SYSTEM_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE +#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE #else -#define STACK_SIZE_BYTES 924 +#define STACK_SIZE_BYTES 924 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY+2) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) // Private types @@ -76,15 +76,15 @@ static void systemTask(void *parameters); */ int32_t OPLinkModStart(void) { - // Initialize vars - stackOverflow = false; - mallocFailed = false; - // Create oplink system task - xTaskCreate(systemTask, (signed char *)"OPLink", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &systemTaskHandle); - // Register task - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle); + // Initialize vars + stackOverflow = false; + mallocFailed = false; + // Create oplink system task + xTaskCreate(systemTask, (signed char *)"OPLink", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle); + // Register task + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle); - return 0; + return 0; } /** @@ -93,29 +93,28 @@ int32_t OPLinkModStart(void) */ int32_t OPLinkModInitialize(void) { + // Must registers objects here for system thread because ObjectManager started in OpenPilotInit - // Must registers objects here for system thread because ObjectManager started in OpenPilotInit + // Initialize out status object. + OPLinkStatusInitialize(); + OPLinkStatusData oplinkStatus; + OPLinkStatusGet(&oplinkStatus); - // Initialize out status object. - OPLinkStatusInitialize(); - OPLinkStatusData oplinkStatus; - OPLinkStatusGet(&oplinkStatus); + // Get our hardware information. + const struct pios_board_info *bdinfo = &pios_board_info_blob; - // Get our hardware information. - const struct pios_board_info * bdinfo = &pios_board_info_blob; + oplinkStatus.BoardType = bdinfo->board_type; + PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); + PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); + oplinkStatus.BoardRevision = bdinfo->board_rev; - oplinkStatus.BoardType= bdinfo->board_type; - PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); - PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); - oplinkStatus.BoardRevision= bdinfo->board_rev; + // Update the object + OPLinkStatusSet(&oplinkStatus); - // Update the object - OPLinkStatusSet(&oplinkStatus); + // Call the module start function. + OPLinkModStart(); - // Call the module start function. - OPLinkModStart(); - - return 0; + return 0; } MODULE_INITCALL(OPLinkModInitialize, 0) @@ -125,88 +124,87 @@ MODULE_INITCALL(OPLinkModInitialize, 0) */ static void systemTask(__attribute__((unused)) void *parameters) { - portTickType lastSysTime; - uint16_t prev_tx_count = 0; - uint16_t prev_rx_count = 0; - bool first_time = true; + portTickType lastSysTime; + uint16_t prev_tx_count = 0; + uint16_t prev_rx_count = 0; + bool first_time = true; - /* create all modules thread */ - MODULE_TASKCREATE_ALL; + /* create all modules thread */ + MODULE_TASKCREATE_ALL; - if (mallocFailed) { - /* We failed to malloc during task creation, - * system behaviour is undefined. Reset and let - * the BootFault code recover for us. - */ - PIOS_SYS_Reset(); - } + if (mallocFailed) { + /* We failed to malloc during task creation, + * system behaviour is undefined. Reset and let + * the BootFault code recover for us. + */ + PIOS_SYS_Reset(); + } - // Initialize vars - idleCounter = 0; - idleCounterClear = 0; - lastSysTime = xTaskGetTickCount(); + // Initialize vars + idleCounter = 0; + idleCounterClear = 0; + lastSysTime = xTaskGetTickCount(); - // Main system loop - while (1) { - - // Flash the heartbeat LED + // Main system loop + while (1) { + // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ - // Update the OPLinkStatus UAVO - OPLinkStatusData oplinkStatus; - OPLinkStatusGet(&oplinkStatus); + // Update the OPLinkStatus UAVO + OPLinkStatusData oplinkStatus; + OPLinkStatusGet(&oplinkStatus); - // Get the other device stats. - PIOS_RFM2B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM); + // Get the other device stats. + PIOS_RFM2B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM); - // Get the stats from the radio device - struct rfm22b_stats radio_stats; - PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); + // Get the stats from the radio device + struct rfm22b_stats radio_stats; + PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); - // Update the status - oplinkStatus.HeapRemaining = xPortGetFreeHeapSize(); - oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); - oplinkStatus.RxGood = radio_stats.rx_good; - oplinkStatus.RxCorrected = radio_stats.rx_corrected; - oplinkStatus.RxErrors = radio_stats.rx_error; - oplinkStatus.RxMissed = radio_stats.rx_missed; - oplinkStatus.RxFailure = radio_stats.rx_failure; - oplinkStatus.TxDropped = radio_stats.tx_dropped; - oplinkStatus.TxResent = radio_stats.tx_resent; - oplinkStatus.TxFailure = radio_stats.tx_failure; - oplinkStatus.Resets = radio_stats.resets; - oplinkStatus.Timeouts = radio_stats.timeouts; - oplinkStatus.RSSI = radio_stats.rssi; - oplinkStatus.LinkQuality = radio_stats.link_quality; - if (first_time) - first_time = false; - else - { - uint16_t tx_count = radio_stats.tx_byte_count; - uint16_t rx_count = radio_stats.rx_byte_count; - uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count); - uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count); - oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); - oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); - prev_tx_count = tx_count; - prev_rx_count = rx_count; - } - oplinkStatus.TXSeq = radio_stats.tx_seq; - oplinkStatus.RXSeq = radio_stats.rx_seq; - oplinkStatus.LinkState = radio_stats.link_state; - if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) - LINK_LED_ON; - else - LINK_LED_OFF; + // Update the status + oplinkStatus.HeapRemaining = xPortGetFreeHeapSize(); + oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); + oplinkStatus.RxGood = radio_stats.rx_good; + oplinkStatus.RxCorrected = radio_stats.rx_corrected; + oplinkStatus.RxErrors = radio_stats.rx_error; + oplinkStatus.RxMissed = radio_stats.rx_missed; + oplinkStatus.RxFailure = radio_stats.rx_failure; + oplinkStatus.TxDropped = radio_stats.tx_dropped; + oplinkStatus.TxResent = radio_stats.tx_resent; + oplinkStatus.TxFailure = radio_stats.tx_failure; + oplinkStatus.Resets = radio_stats.resets; + oplinkStatus.Timeouts = radio_stats.timeouts; + oplinkStatus.RSSI = radio_stats.rssi; + oplinkStatus.LinkQuality = radio_stats.link_quality; + if (first_time) { + first_time = false; + } else { + uint16_t tx_count = radio_stats.tx_byte_count; + uint16_t rx_count = radio_stats.rx_byte_count; + uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count); + uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count); + oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); + oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); + prev_tx_count = tx_count; + prev_rx_count = rx_count; + } + oplinkStatus.TXSeq = radio_stats.tx_seq; + oplinkStatus.RXSeq = radio_stats.rx_seq; + oplinkStatus.LinkState = radio_stats.link_state; + if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) { + LINK_LED_ON; + } else { + LINK_LED_OFF; + } - // Update the object - OPLinkStatusSet(&oplinkStatus); + // Update the object + OPLinkStatusSet(&oplinkStatus); - // Wait until next period - vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS); - } + // Wait until next period + vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS); + } } /** @@ -214,27 +212,29 @@ static void systemTask(__attribute__((unused)) void *parameters) */ void vApplicationIdleHook(void) { - // Called when the scheduler has no tasks to run - if (idleCounterClear == 0) { - ++idleCounter; - } else { - idleCounter = 0; - idleCounterClear = 0; - } + // Called when the scheduler has no tasks to run + if (idleCounterClear == 0) { + ++idleCounter; + } else { + idleCounter = 0; + idleCounterClear = 0; + } } /** * Called by the RTOS when a stack overflow is detected. */ #define DEBUG_STACK_OVERFLOW 0 -void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle * pxTask, - __attribute__((unused)) signed portCHAR * pcTaskName) +void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask, + __attribute__((unused)) signed portCHAR *pcTaskName) { - stackOverflow = true; + stackOverflow = true; #if DEBUG_STACK_OVERFLOW - static volatile bool wait_here = true; - while(wait_here); - wait_here = true; + static volatile bool wait_here = true; + while (wait_here) { + ; + } + wait_here = true; #endif } @@ -244,15 +244,17 @@ void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle * pxTask, #define DEBUG_MALLOC_FAILURES 0 void vApplicationMallocFailedHook(void) { - mallocFailed = true; + mallocFailed = true; #if DEBUG_MALLOC_FAILURES - static volatile bool wait_here = true; - while(wait_here); - wait_here = true; + static volatile bool wait_here = true; + while (wait_here) { + ; + } + wait_here = true; #endif } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/Osd/OsdEtStd/OsdEtStd.c b/flight/modules/Osd/OsdEtStd/OsdEtStd.c index 2c806ad64..363f125e1 100644 --- a/flight/modules/Osd/OsdEtStd/OsdEtStd.c +++ b/flight/modules/Osd/OsdEtStd/OsdEtStd.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup OSDModule OSD Module * @brief On screen display support - * @{ + * @{ * * @file OsdEtStd.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -39,59 +39,59 @@ // // Configuration // -#define DEBUG_PORT PIOS_COM_GPS -//#define ENABLE_DEBUG_MSG -//#define PIOS_ENABLE_DEBUG_PINS -//#define DUMP_CONFIG // Enable this do read and dump the OSD config +#define DEBUG_PORT PIOS_COM_GPS +// #define ENABLE_DEBUG_MSG +// #define PIOS_ENABLE_DEBUG_PINS +// #define DUMP_CONFIG // Enable this do read and dump the OSD config // // Private constants // #ifdef ENABLE_DEBUG_MSG -#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__) +#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format,##__VA_ARGS__) #else #define DEBUG_MSG(format, ...) #endif -#define CONFIG_LENGTH 6726 -#define MIN(a,b) ((a)<(b)?(a):(b)) +#define CONFIG_LENGTH 6726 +#define MIN(a, b) ((a) < (b) ? (a) : (b)) -#define SUPPORTED_VERSION 115 +#define SUPPORTED_VERSION 115 -#define OSD_ADDRESS 0x30 +#define OSD_ADDRESS 0x30 -#define OSDMSG_V_LS_IDX 10 -#define OSDMSG_BALT_IDX1 11 -#define OSDMSG_BALT_IDX2 12 -#define OSDMSG_A_LS_IDX 17 -#define OSDMSG_VA_MS_IDX 18 -#define OSDMSG_LAT_IDX 33 -#define OSDMSG_LON_IDX 37 -#define OSDMSG_HOME_IDX 47 -#define OSDMSG_ALT_IDX 49 -#define OSDMSG_NB_SATS 58 -#define OSDMSG_GPS_STAT 59 +#define OSDMSG_V_LS_IDX 10 +#define OSDMSG_BALT_IDX1 11 +#define OSDMSG_BALT_IDX2 12 +#define OSDMSG_A_LS_IDX 17 +#define OSDMSG_VA_MS_IDX 18 +#define OSDMSG_LAT_IDX 33 +#define OSDMSG_LON_IDX 37 +#define OSDMSG_HOME_IDX 47 +#define OSDMSG_ALT_IDX 49 +#define OSDMSG_NB_SATS 58 +#define OSDMSG_GPS_STAT 59 -#define OSDMSG_GPS_STAT_NOFIX 0x03 -#define OSDMSG_GPS_STAT_FIX 0x2B -#define OSDMSG_GPS_STAT_HB_FLAG 0x10 +#define OSDMSG_GPS_STAT_NOFIX 0x03 +#define OSDMSG_GPS_STAT_FIX 0x2B +#define OSDMSG_GPS_STAT_HB_FLAG 0x10 #ifdef PIOS_ENABLE_DEBUG_PINS - #define DEBUG_PIN_RUNNING 0 - #define DEBUG_PIN_I2C 1 - #define DebugPinHigh(x) PIOS_DEBUG_PinHigh(x) - #define DebugPinLow(x) PIOS_DEBUG_PinLow(x) + #define DEBUG_PIN_RUNNING 0 + #define DEBUG_PIN_I2C 1 + #define DebugPinHigh(x) PIOS_DEBUG_PinHigh(x) + #define DebugPinLow(x) PIOS_DEBUG_PinLow(x) #else - #define DebugPinHigh(x) - #define DebugPinLow(x) + #define DebugPinHigh(x) + #define DebugPinLow(x) #endif static const char *UpdateConfFilePath = "/etosd/update.ocf"; #ifdef DUMP_CONFIG -static const char *DumpConfFilePath = "/etosd/dump.ocf"; +static const char *DumpConfFilePath = "/etosd/dump.ocf"; #endif // @@ -102,23 +102,21 @@ static const char *DumpConfFilePath = "/etosd/dump.ocf"; // Private variables // -// | Header / cmd?| | V | E3 | V | | LAT: 37 57.0000 | LONG: 24 00.4590 | | Hom<-179| Alt 545.4 m | |#sat|stat| -// 00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 +// | Header / cmd?| | V | E3 | V | | LAT: 37 57.0000 | LONG: 24 00.4590 | | Hom<-179| Alt 545.4 m | |#sat|stat| +// 00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 uint8_t msg[63] = - { 0x03, 0x3F, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x0A, 0x00, 0xE4, 0x30, 0x00, 0x00, 0x00, 0x00, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x60, 0x10, 0x02, 0x00, 0x00, 0x90, 0x00, -0x54, 0x54, 0x00, 0x00, 0x33, 0x28, 0x13, 0x00, 0x00, 0x08, 0x00, 0x00, 0x90, 0x0A }; -static volatile bool newPosData = FALSE; +{ 0x03, 0x3F, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x0A, 0x00, 0xE4, 0x30, 0x00, 0x00, 0x00, 0x00, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x60, 0x10, 0x02, 0x00, 0x00, 0x90,0x00, + 0x54, 0x54, 0x00, 0x00, 0x33, 0x28, 0x13, 0x00, 0x00, 0x08, 0x00, 0x00, 0x90, 0x0A }; +static volatile bool newPosData = FALSE; static volatile bool newBattData = FALSE; static volatile bool newBaroData = FALSE; -static enum -{ - STATE_DETECT, - STATE_UPDATE_CONF, - STATE_DUMP_CONF, - STTE_RUNNING - +static enum { + STATE_DETECT, + STATE_UPDATE_CONF, + STATE_DUMP_CONF, + STTE_RUNNING } state; static UAVObjEvent ev; @@ -130,408 +128,407 @@ static uint32_t version = 0; // static void WriteToMsg8(uint8_t index, uint8_t value) { - if (value > 100) - value = 100; + if (value > 100) { + value = 100; + } - msg[index] = ((value / 10) << 4) + (value % 10); + msg[index] = ((value / 10) << 4) + (value % 10); } static void WriteToMsg16(uint8_t index, uint16_t value) { - WriteToMsg8(index, value % 100); - WriteToMsg8(index + 1, value / 100); + WriteToMsg8(index, value % 100); + WriteToMsg8(index + 1, value / 100); } static void WriteToMsg24(uint8_t index, uint32_t value) { - WriteToMsg16(index, value % 10000); - WriteToMsg8(index + 2, value / 10000); + WriteToMsg16(index, value % 10000); + WriteToMsg8(index + 2, value / 10000); } static void WriteToMsg32(uint8_t index, uint32_t value) { - WriteToMsg16(index, value % 10000); - WriteToMsg16(index + 2, value / 10000); + WriteToMsg16(index, value % 10000); + WriteToMsg16(index + 2, value / 10000); } static void SetCoord(uint8_t index, uint32_t coord) { - #define E7 10000000 - uint8_t deg = coord / E7; - float sec = (float)(coord - deg*E7) / ((float)E7/(60.0*10000)); - - WriteToMsg8(index + 3, deg); - WriteToMsg24(index, sec); + uint8_t deg = coord / E7; + float sec = (float)(coord - deg * E7) / ((float)E7 / (60.0 * 10000)); + WriteToMsg8(index + 3, deg); + WriteToMsg24(index, sec); } static void SetCourse(uint16_t dir) { - WriteToMsg16(OSDMSG_HOME_IDX, dir); + WriteToMsg16(OSDMSG_HOME_IDX, dir); } static void SetBaroAltitude(int16_t altitudeMeter) { - // calculated formula - // ET OSD uses first update as zeropoint and then +- from that - altitudeMeter=(4571-altitudeMeter)/0.37; - msg[OSDMSG_BALT_IDX1] = (uint8_t)(altitudeMeter&0x00FF); - msg[OSDMSG_BALT_IDX2] = (altitudeMeter >> 8)&0x3F; + // calculated formula + // ET OSD uses first update as zeropoint and then +- from that + altitudeMeter = (4571 - altitudeMeter) / 0.37; + msg[OSDMSG_BALT_IDX1] = (uint8_t)(altitudeMeter & 0x00FF); + msg[OSDMSG_BALT_IDX2] = (altitudeMeter >> 8) & 0x3F; } static void SetAltitude(uint32_t altitudeMeter) { - WriteToMsg32(OSDMSG_ALT_IDX, altitudeMeter * 10); + WriteToMsg32(OSDMSG_ALT_IDX, altitudeMeter * 10); } static void SetVoltage(uint32_t milliVolt) { - msg[OSDMSG_VA_MS_IDX] &= 0x0F; - msg[OSDMSG_VA_MS_IDX] |= (milliVolt / 6444) << 4; - msg[OSDMSG_V_LS_IDX] = (milliVolt % 6444) * 256 / 6444; + msg[OSDMSG_VA_MS_IDX] &= 0x0F; + msg[OSDMSG_VA_MS_IDX] |= (milliVolt / 6444) << 4; + msg[OSDMSG_V_LS_IDX] = (milliVolt % 6444) * 256 / 6444; } static void SetCurrent(uint32_t milliAmp) { - uint32_t value = (milliAmp * 16570 / 1000000) + 0x7FA; - msg[OSDMSG_VA_MS_IDX] &= 0xF0; - msg[OSDMSG_VA_MS_IDX] |= ((value >> 8) & 0x0F); - msg[OSDMSG_A_LS_IDX] = (value & 0xFF); + uint32_t value = (milliAmp * 16570 / 1000000) + 0x7FA; + + msg[OSDMSG_VA_MS_IDX] &= 0xF0; + msg[OSDMSG_VA_MS_IDX] |= ((value >> 8) & 0x0F); + msg[OSDMSG_A_LS_IDX] = (value & 0xFF); } static void SetNbSats(uint8_t nb) { - msg[OSDMSG_NB_SATS] = nb; + msg[OSDMSG_NB_SATS] = nb; } static void FlightBatteryStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - newBattData = TRUE; + newBattData = TRUE; } static void GPSPositionUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - newPosData = TRUE; + newPosData = TRUE; } static void BaroAltitudeUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - newBaroData = TRUE; + newBaroData = TRUE; } -static bool Read(uint32_t start, uint8_t length, uint8_t * buffer) +static bool Read(uint32_t start, uint8_t length, uint8_t *buffer) { - uint8_t cmd[5]; + uint8_t cmd[5]; - const struct pios_i2c_txn txn_list[] = { - { - .addr = OSD_ADDRESS, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(cmd), - .buf = cmd, - } - , - { - .addr = OSD_ADDRESS, - .rw = PIOS_I2C_TXN_READ, - .len = length, - .buf = buffer, - } - , - }; + const struct pios_i2c_txn txn_list[] = { + { + .addr = OSD_ADDRESS, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(cmd), + .buf = cmd, + } + , + { + .addr = OSD_ADDRESS, + .rw = PIOS_I2C_TXN_READ, + .len = length, + .buf = buffer, + } + , + }; - cmd[0] = 0x02; - cmd[1] = 0x05; - cmd[2] = (uint8_t) (start & 0xFF); - cmd[3] = (uint8_t) (start >> 8); - cmd[4] = length; + cmd[0] = 0x02; + cmd[1] = 0x05; + cmd[2] = (uint8_t)(start & 0xFF); + cmd[3] = (uint8_t)(start >> 8); + cmd[4] = length; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)) == 0; + return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)) == 0; } -static bool Write(uint32_t start, uint8_t length, const uint8_t * buffer) +static bool Write(uint32_t start, uint8_t length, const uint8_t *buffer) { - uint8_t cmd[125]; - uint8_t ack[3]; + uint8_t cmd[125]; + uint8_t ack[3]; - const struct pios_i2c_txn txn_list1[] = { - { - .addr = OSD_ADDRESS, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(cmd), - .buf = cmd, - } - , - { - .addr = OSD_ADDRESS, - .rw = PIOS_I2C_TXN_READ, - .len = sizeof(ack), - .buf = ack, - } - , - }; + const struct pios_i2c_txn txn_list1[] = { + { + .addr = OSD_ADDRESS, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(cmd), + .buf = cmd, + } + , + { + .addr = OSD_ADDRESS, + .rw = PIOS_I2C_TXN_READ, + .len = sizeof(ack), + .buf = ack, + } + , + }; - if (length + 5 > sizeof(cmd)) { - // Too big - return FALSE; - } + if (length + 5 > sizeof(cmd)) { + // Too big + return FALSE; + } - cmd[0] = 0x01; - cmd[1] = 0x7D; - cmd[2] = (uint8_t) (start & 0xFF); - cmd[3] = (uint8_t) (start >> 8); - cmd[4] = length; - memcpy(&cmd[5], buffer, length); + cmd[0] = 0x01; + cmd[1] = 0x7D; + cmd[2] = (uint8_t)(start & 0xFF); + cmd[3] = (uint8_t)(start >> 8); + cmd[4] = length; + memcpy(&cmd[5], buffer, length); - ack[0] = 0; + ack[0] = 0; - // - // FIXME: See OP-305, the driver seems to return FALSE while all is OK - // - PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list1, NELEMENTS(txn_list1)); -// if (PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list1, NELEMENTS(txn_list1))) { - //DEBUG_MSG("ACK=%d ", ack[0]); - if (ack[0] == 49) { - return TRUE; - } -// } + // + // FIXME: See OP-305, the driver seems to return FALSE while all is OK + // + PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list1, NELEMENTS(txn_list1)); +// if (PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list1, NELEMENTS(txn_list1))) { + // DEBUG_MSG("ACK=%d ", ack[0]); + if (ack[0] == 49) { + return TRUE; + } +// } - return FALSE; + return FALSE; } static uint32_t ReadSwVersion(void) { - uint8_t buf[4]; - uint32_t version; + uint8_t buf[4]; + uint32_t version; - if (Read(0, 4, buf)) { - version = (buf[0] - '0') * 100; - version += (buf[2] - '0') * 10; - version += (buf[3] - '0'); - } else { - version = 0; - } + if (Read(0, 4, buf)) { + version = (buf[0] - '0') * 100; + version += (buf[2] - '0') * 10; + version += (buf[3] - '0'); + } else { + version = 0; + } - return version; + return version; } static void UpdateConfig(void) { - uint8_t buf[120]; - uint32_t addr = 0; - uint32_t n; - FILEINFO file; - uint32_t res; + uint8_t buf[120]; + uint32_t addr = 0; + uint32_t n; + FILEINFO file; + uint32_t res; - // Try to open the file that contains a new config - res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *) UpdateConfFilePath, DFS_READ, PIOS_SDCARD_Sector, &file); - if (res == DFS_OK) { - uint32_t bytesRead; - bool ok = TRUE; + // Try to open the file that contains a new config + res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)UpdateConfFilePath, DFS_READ, PIOS_SDCARD_Sector, &file); + if (res == DFS_OK) { + uint32_t bytesRead; + bool ok = TRUE; - DEBUG_MSG("Updating Config "); + DEBUG_MSG("Updating Config "); - // Write the config-data in blocks to OSD - while (addr < CONFIG_LENGTH && ok) { - n = MIN(CONFIG_LENGTH - addr, sizeof(buf)); - res = DFS_ReadFile(&file, PIOS_SDCARD_Sector, buf, &bytesRead, n); - if (res == DFS_OK && bytesRead == n) { - ok = Write(addr, n, buf); - if (ok) { - //DEBUG_MSG(" %d %d\n", addr, n); - DEBUG_MSG("."); - addr += n; - } - } else { - DEBUG_MSG(" FILEREAD FAILED "); - } - } + // Write the config-data in blocks to OSD + while (addr < CONFIG_LENGTH && ok) { + n = MIN(CONFIG_LENGTH - addr, sizeof(buf)); + res = DFS_ReadFile(&file, PIOS_SDCARD_Sector, buf, &bytesRead, n); + if (res == DFS_OK && bytesRead == n) { + ok = Write(addr, n, buf); + if (ok) { + // DEBUG_MSG(" %d %d\n", addr, n); + DEBUG_MSG("."); + addr += n; + } + } else { + DEBUG_MSG(" FILEREAD FAILED "); + } + } - DEBUG_MSG(ok ? " OK\n" : "FAILURE\n"); + DEBUG_MSG(ok ? " OK\n" : "FAILURE\n"); - // If writing is OK, read the data back and verify - if (ok) { - DEBUG_MSG("Verify Config "); - DFS_Seek(&file, 0, PIOS_SDCARD_Sector); + // If writing is OK, read the data back and verify + if (ok) { + DEBUG_MSG("Verify Config "); + DFS_Seek(&file, 0, PIOS_SDCARD_Sector); - addr = 0; - while (addr < CONFIG_LENGTH && ok) { - // First half of the buffer is used to store the data read from the OSD, the second half will contain the data from the file - n = MIN(CONFIG_LENGTH - addr, sizeof(buf) / 2); - ok = Read(addr, n, buf); - if (ok) { - uint32_t bytesRead; - res = DFS_ReadFile(&file, PIOS_SDCARD_Sector, buf + sizeof(buf) / 2, &bytesRead, n); - if (res == DFS_OK && bytesRead == n) { - DEBUG_MSG("."); - addr += n; + addr = 0; + while (addr < CONFIG_LENGTH && ok) { + // First half of the buffer is used to store the data read from the OSD, the second half will contain the data from the file + n = MIN(CONFIG_LENGTH - addr, sizeof(buf) / 2); + ok = Read(addr, n, buf); + if (ok) { + uint32_t bytesRead; + res = DFS_ReadFile(&file, PIOS_SDCARD_Sector, buf + sizeof(buf) / 2, &bytesRead, n); + if (res == DFS_OK && bytesRead == n) { + DEBUG_MSG("."); + addr += n; - if (memcmp(buf, buf + sizeof(buf) / 2, n) != 0) { - DEBUG_MSG(" MISMATCH "); - ok = FALSE; - } - } - } - } - DEBUG_MSG(ok ? " OK\n" : "FAILURE\n"); - } + if (memcmp(buf, buf + sizeof(buf) / 2, n) != 0) { + DEBUG_MSG(" MISMATCH "); + ok = FALSE; + } + } + } + } + DEBUG_MSG(ok ? " OK\n" : "FAILURE\n"); + } - DFS_Close(&file); + DFS_Close(&file); - // When the config was updated correctly, remove the config-file - if (ok) { - DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *) UpdateConfFilePath, PIOS_SDCARD_Sector); - } - - } + // When the config was updated correctly, remove the config-file + if (ok) { + DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)UpdateConfFilePath, PIOS_SDCARD_Sector); + } + } } static void DumpConfig(void) { #ifdef DUMP_CONFIG - uint8_t buf[100]; - uint32_t addr = 0; - uint32_t n; - FILEINFO file; - uint32_t res; + uint8_t buf[100]; + uint32_t addr = 0; + uint32_t n; + FILEINFO file; + uint32_t res; - DEBUG_MSG("Dumping Config "); + DEBUG_MSG("Dumping Config "); - res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *) DumpConfFilePath, DFS_WRITE, PIOS_SDCARD_Sector, &file); - if (res == DFS_OK) { - uint32_t bytesWritten; - bool ok = TRUE; + res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)DumpConfFilePath, DFS_WRITE, PIOS_SDCARD_Sector, &file); + if (res == DFS_OK) { + uint32_t bytesWritten; + bool ok = TRUE; - while (addr < CONFIG_LENGTH && ok) { - n = MIN(CONFIG_LENGTH - addr, sizeof(buf)); - ok = Read(addr, n, buf); - if (ok) { - res = DFS_WriteFile(&file, PIOS_SDCARD_Sector, buf, &bytesWritten, n); - if (res == DFS_OK && bytesWritten == n) { - //DEBUG_MSG(" %d %d\n", addr, n); - DEBUG_MSG("."); - addr += n; - } - } - } + while (addr < CONFIG_LENGTH && ok) { + n = MIN(CONFIG_LENGTH - addr, sizeof(buf)); + ok = Read(addr, n, buf); + if (ok) { + res = DFS_WriteFile(&file, PIOS_SDCARD_Sector, buf, &bytesWritten, n); + if (res == DFS_OK && bytesWritten == n) { + // DEBUG_MSG(" %d %d\n", addr, n); + DEBUG_MSG("."); + addr += n; + } + } + } - DEBUG_MSG(ok ? " OK\n" : "FAILURE\n"); + DEBUG_MSG(ok ? " OK\n" : "FAILURE\n"); - DFS_Close(&file); - } else { - DEBUG_MSG("Error Opening File %x\n", res); - } -#endif + DFS_Close(&file); + } else { + DEBUG_MSG("Error Opening File %x\n", res); + } +#endif /* ifdef DUMP_CONFIG */ } static void Run(void) { - static uint32_t cnt = 0; + static uint32_t cnt = 0; - if (newBattData) { - FlightBatteryStateData flightBatteryData; + if (newBattData) { + FlightBatteryStateData flightBatteryData; - FlightBatteryStateGet(&flightBatteryData); + FlightBatteryStateGet(&flightBatteryData); - //DEBUG_MSG("%5d Batt: V=%dmV\n\r", cnt, (uint32_t)(flightBatteryData.Voltage*1000)); + // DEBUG_MSG("%5d Batt: V=%dmV\n\r", cnt, (uint32_t)(flightBatteryData.Voltage*1000)); - SetVoltage((uint32_t) (flightBatteryData.Voltage * 1000)); - SetCurrent((uint32_t) (flightBatteryData.Current * 1000)); - newBattData = FALSE; - } + SetVoltage((uint32_t)(flightBatteryData.Voltage * 1000)); + SetCurrent((uint32_t)(flightBatteryData.Current * 1000)); + newBattData = FALSE; + } - if (newPosData) { - GPSPositionData positionData; - AttitudeActualData attitudeActualData; + if (newPosData) { + GPSPositionData positionData; + AttitudeActualData attitudeActualData; - GPSPositionGet(&positionData); - AttitudeActualGet(&attitudeActualData); + GPSPositionGet(&positionData); + AttitudeActualGet(&attitudeActualData); - //DEBUG_MSG("%5d Pos: #stat=%d #sats=%d alt=%d\n\r", cnt, - // positionData.Status, positionData.Satellites, (uint32_t)positionData.Altitude); + // DEBUG_MSG("%5d Pos: #stat=%d #sats=%d alt=%d\n\r", cnt, + // positionData.Status, positionData.Satellites, (uint32_t)positionData.Altitude); - // GPS Status - if (positionData.Status == GPSPOSITION_STATUS_FIX3D) - msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_FIX; - else - msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_NOFIX; - msg[OSDMSG_GPS_STAT] |= OSDMSG_GPS_STAT_HB_FLAG; + // GPS Status + if (positionData.Status == GPSPOSITION_STATUS_FIX3D) { + msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_FIX; + } else { + msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_NOFIX; + } + msg[OSDMSG_GPS_STAT] |= OSDMSG_GPS_STAT_HB_FLAG; - // GPS info - SetCoord(OSDMSG_LAT_IDX, positionData.Latitude); - SetCoord(OSDMSG_LON_IDX, positionData.Longitude); - SetAltitude(positionData.Altitude); - SetNbSats(positionData.Satellites); - SetCourse(attitudeActualData.Yaw); + // GPS info + SetCoord(OSDMSG_LAT_IDX, positionData.Latitude); + SetCoord(OSDMSG_LON_IDX, positionData.Longitude); + SetAltitude(positionData.Altitude); + SetNbSats(positionData.Satellites); + SetCourse(attitudeActualData.Yaw); - newPosData = FALSE; - } else { - msg[OSDMSG_GPS_STAT] &= ~OSDMSG_GPS_STAT_HB_FLAG; - } - if (newBaroData) { - BaroAltitudeData baroData; + newPosData = FALSE; + } else { + msg[OSDMSG_GPS_STAT] &= ~OSDMSG_GPS_STAT_HB_FLAG; + } + if (newBaroData) { + BaroAltitudeData baroData; - BaroAltitudeGet(&baroData); - SetBaroAltitude(baroData.Altitude); + BaroAltitudeGet(&baroData); + SetBaroAltitude(baroData.Altitude); - newBaroData = FALSE; - } + newBaroData = FALSE; + } - DEBUG_MSG("SendMsg %d\n",cnt); - { - DebugPinHigh(DEBUG_PIN_I2C); - const struct pios_i2c_txn txn_list[] = { - { - .addr = OSD_ADDRESS, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(msg), - .buf = msg, - } - , - }; + DEBUG_MSG("SendMsg %d\n", cnt); + { + DebugPinHigh(DEBUG_PIN_I2C); + const struct pios_i2c_txn txn_list[] = { + { + .addr = OSD_ADDRESS, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(msg), + .buf = msg, + } + , + }; - PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); - DebugPinLow(DEBUG_PIN_I2C); - } + PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + DebugPinLow(DEBUG_PIN_I2C); + } - cnt++; + cnt++; } -static void onTimer(UAVObjEvent * ev) +static void onTimer(UAVObjEvent *ev) { - DebugPinHigh(DEBUG_PIN_RUNNING); + DebugPinHigh(DEBUG_PIN_RUNNING); - #ifdef ENABLE_DEBUG_MSG - PIOS_COM_ChangeBaud(DEBUG_PORT, 57600); - #endif + #ifdef ENABLE_DEBUG_MSG + PIOS_COM_ChangeBaud(DEBUG_PORT, 57600); + #endif - if (state == STATE_DETECT) { - version = ReadSwVersion(); - DEBUG_MSG("SW: %d ", version); + if (state == STATE_DETECT) { + version = ReadSwVersion(); + DEBUG_MSG("SW: %d ", version); - if (version == SUPPORTED_VERSION) { - DEBUG_MSG("OK\n"); - state++; - } else { - DEBUG_MSG("INVALID\n"); - } - } else if (state == STATE_UPDATE_CONF) { - UpdateConfig(); - state++; - } else if (state == STATE_DUMP_CONF) { - DumpConfig(); - state++; - } else if (state == STTE_RUNNING) { - Run(); - } else { - // should not happen.. - state = STATE_DETECT; - } - - DebugPinLow(DEBUG_PIN_RUNNING); + if (version == SUPPORTED_VERSION) { + DEBUG_MSG("OK\n"); + state++; + } else { + DEBUG_MSG("INVALID\n"); + } + } else if (state == STATE_UPDATE_CONF) { + UpdateConfig(); + state++; + } else if (state == STATE_DUMP_CONF) { + DumpConfig(); + state++; + } else if (state == STTE_RUNNING) { + Run(); + } else { + // should not happen.. + state = STATE_DETECT; + } + DebugPinLow(DEBUG_PIN_RUNNING); } // @@ -546,12 +543,12 @@ static void onTimer(UAVObjEvent * ev) */ int32_t OsdEtStdInitialize(void) { - GPSPositionConnectCallback(GPSPositionUpdatedCb); - FlightBatteryStateConnectCallback(FlightBatteryStateUpdatedCb); - BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb); + GPSPositionConnectCallback(GPSPositionUpdatedCb); + FlightBatteryStateConnectCallback(FlightBatteryStateUpdatedCb); + BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb); - memset(&ev,0,sizeof(UAVObjEvent)); - EventPeriodicCallbackCreate(&ev, onTimer, 100 / portTICK_RATE_MS); + memset(&ev, 0, sizeof(UAVObjEvent)); + EventPeriodicCallbackCreate(&ev, onTimer, 100 / portTICK_RATE_MS); - return 0; + return 0; } diff --git a/flight/modules/Osd/WavPlayer/wavplayer.c b/flight/modules/Osd/WavPlayer/wavplayer.c index 2bf8949ea..bb1fd93dd 100644 --- a/flight/modules/Osd/WavPlayer/wavplayer.c +++ b/flight/modules/Osd/WavPlayer/wavplayer.c @@ -39,9 +39,9 @@ static void WavPlayerTask(void *parameters); // **************** // Private constants -#define STACK_SIZE_BYTES 1600 +#define STACK_SIZE_BYTES 1600 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // **************** // Private variables @@ -55,7 +55,7 @@ static uint32_t timeOfLastUpdateMs; int32_t WavPlayerStart(void) { // Start WavPlayer task - xTaskCreate(WavPlayerTask, (signed char *) "WavPlayer", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &WavPlayerTaskHandle); + xTaskCreate(WavPlayerTask, (signed char *)"WavPlayer", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &WavPlayerTaskHandle); return 0; } @@ -66,10 +66,9 @@ int32_t WavPlayerStart(void) */ int32_t WavPlayerInitialize(void) { - return 0; } -MODULE_INITCALL( WavPlayerInitialize, WavPlayerStart) +MODULE_INITCALL(WavPlayerInitialize, WavPlayerStart) // **************** /** @@ -79,11 +78,12 @@ MODULE_INITCALL( WavPlayerInitialize, WavPlayerStart) static void WavPlayerTask(__attribute__((unused)) void *parameters) { portTickType lastSysTime; + // Loop forever lastSysTime = xTaskGetTickCount(); uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - timeOfLastUpdateMs = timeNowMs; + timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; #if defined(PIOS_INCLUDE_WAVE) WavePlayer_Start(); diff --git a/flight/modules/Osd/osdgen/inc/osdgen.h b/flight/modules/Osd/osdgen/inc/osdgen.h index ac5621f51..35d7c3020 100644 --- a/flight/modules/Osd/osdgen/inc/osdgen.h +++ b/flight/modules/Osd/osdgen/inc/osdgen.h @@ -39,21 +39,21 @@ int32_t osdgenInitialize(void); // Size of an array (num items.) #define SIZEOF_ARRAY(x) (sizeof(x) / sizeof((x)[0])) -#define HUD_VSCALE_FLAG_CLEAR 1 -#define HUD_VSCALE_FLAG_NO_NEGATIVE 2 +#define HUD_VSCALE_FLAG_CLEAR 1 +#define HUD_VSCALE_FLAG_NO_NEGATIVE 2 // Macros for computing addresses and bit positions. // NOTE: /16 in y is because we are addressing by word not byte. -#define CALC_BUFF_ADDR(x, y) (((x) / 8) + ((y) * (GRAPHICS_WIDTH_REAL / 8))) -#define CALC_BIT_IN_WORD(x) ((x) & 7) +#define CALC_BUFF_ADDR(x, y) (((x) / 8) + ((y) * (GRAPHICS_WIDTH_REAL / 8))) +#define CALC_BIT_IN_WORD(x) ((x) & 7) #define DEBUG_DELAY // Macro for writing a word with a mode (NAND = clear, OR = set, XOR = toggle) // at a given position #define WRITE_WORD_MODE(buff, addr, mask, mode) \ - switch(mode) { \ - case 0: buff[addr] &= ~mask; break; \ - case 1: buff[addr] |= mask; break; \ - case 2: buff[addr] ^= mask; break; } + switch (mode) { \ + case 0: buff[addr] &= ~mask; break; \ + case 1: buff[addr] |= mask; break; \ + case 2: buff[addr] ^= mask; break; } #define WRITE_WORD_NAND(buff, addr, mask) { buff[addr] &= ~mask; DEBUG_DELAY; } #define WRITE_WORD_OR(buff, addr, mask) { buff[addr] |= mask; DEBUG_DELAY; } @@ -61,103 +61,102 @@ int32_t osdgenInitialize(void); // Horizontal line calculations. // Edge cases. -#define COMPUTE_HLINE_EDGE_L_MASK(b) ((1 << (8 - (b))) - 1) -#define COMPUTE_HLINE_EDGE_R_MASK(b) (~((1 << (7 - (b))) - 1)) +#define COMPUTE_HLINE_EDGE_L_MASK(b) ((1 << (8 - (b))) - 1) +#define COMPUTE_HLINE_EDGE_R_MASK(b) (~((1 << (7 - (b))) - 1)) // This computes an island mask. #define COMPUTE_HLINE_ISLAND_MASK(b0, b1) (COMPUTE_HLINE_EDGE_L_MASK(b0) ^ COMPUTE_HLINE_EDGE_L_MASK(b1)); // Macro for initializing stroke/fill modes. Add new modes here // if necessary. #define SETUP_STROKE_FILL(stroke, fill, mode) \ - stroke = 0; fill = 0; \ - if(mode == 0) { stroke = 0; fill = 1; } \ - if(mode == 1) { stroke = 1; fill = 0; } \ + stroke = 0; fill = 0; \ + if (mode == 0) { stroke = 0; fill = 1; } \ + if (mode == 1) { stroke = 1; fill = 0; } \ // Line endcaps (for horizontal and vertical lines.) -#define ENDCAP_NONE 0 -#define ENDCAP_ROUND 1 -#define ENDCAP_FLAT 2 +#define ENDCAP_NONE 0 +#define ENDCAP_ROUND 1 +#define ENDCAP_FLAT 2 #define DRAW_ENDCAP_HLINE(e, x, y, s, f, l) \ - if((e) == ENDCAP_ROUND) /* single pixel endcap */ \ - { write_pixel_lm(x, y, f, l); } \ - else if((e) == ENDCAP_FLAT) /* flat endcap: FIXME, quicker to draw a vertical line(?) */ \ - { write_pixel_lm(x, y - 1, s, l); write_pixel_lm(x, y, s, l); write_pixel_lm(x, y + 1, s, l); } + if ((e) == ENDCAP_ROUND) /* single pixel endcap */ \ + { write_pixel_lm(x, y, f, l); } \ + else if ((e) == ENDCAP_FLAT) /* flat endcap: FIXME, quicker to draw a vertical line(?) */ \ + { write_pixel_lm(x, y - 1, s, l); write_pixel_lm(x, y, s, l); write_pixel_lm(x, y + 1, s, l); } #define DRAW_ENDCAP_VLINE(e, x, y, s, f, l) \ - if((e) == ENDCAP_ROUND) /* single pixel endcap */ \ - { write_pixel_lm(x, y, f, l); } \ - else if((e) == ENDCAP_FLAT) /* flat endcap: FIXME, quicker to draw a horizontal line(?) */ \ - { write_pixel_lm(x - 1, y, s, l); write_pixel_lm(x, y, s, l); write_pixel_lm(x + 1, y, s, l); } + if ((e) == ENDCAP_ROUND) /* single pixel endcap */ \ + { write_pixel_lm(x, y, f, l); } \ + else if ((e) == ENDCAP_FLAT) /* flat endcap: FIXME, quicker to draw a horizontal line(?) */ \ + { write_pixel_lm(x - 1, y, s, l); write_pixel_lm(x, y, s, l); write_pixel_lm(x + 1, y, s, l); } // Macros for writing pixels in a midpoint circle algorithm. #define CIRCLE_PLOT_8(buff, cx, cy, x, y, mode) \ - CIRCLE_PLOT_4(buff, cx, cy, x, y, mode); \ - if((x) != (y)) CIRCLE_PLOT_4(buff, cx, cy, y, x, mode); + CIRCLE_PLOT_4(buff, cx, cy, x, y, mode); \ + if ((x) != (y)) { CIRCLE_PLOT_4(buff, cx, cy, y, x, mode); } #define CIRCLE_PLOT_4(buff, cx, cy, x, y, mode) \ - write_pixel(buff, (cx) + (x), (cy) + (y), mode); \ - write_pixel(buff, (cx) - (x), (cy) + (y), mode); \ - write_pixel(buff, (cx) + (x), (cy) - (y), mode); \ - write_pixel(buff, (cx) - (x), (cy) - (y), mode); + write_pixel(buff, (cx) + (x), (cy) + (y), mode); \ + write_pixel(buff, (cx) - (x), (cy) + (y), mode); \ + write_pixel(buff, (cx) + (x), (cy) - (y), mode); \ + write_pixel(buff, (cx) - (x), (cy) - (y), mode); // Font flags. -#define FONT_BOLD 1 // bold text (no outline) -#define FONT_INVERT 2 // invert: border white, inside black +#define FONT_BOLD 1 // bold text (no outline) +#define FONT_INVERT 2 // invert: border white, inside black // Text alignments. -#define TEXT_VA_TOP 0 -#define TEXT_VA_MIDDLE 1 -#define TEXT_VA_BOTTOM 2 -#define TEXT_HA_LEFT 0 -#define TEXT_HA_CENTER 1 -#define TEXT_HA_RIGHT 2 +#define TEXT_VA_TOP 0 +#define TEXT_VA_MIDDLE 1 +#define TEXT_VA_BOTTOM 2 +#define TEXT_HA_LEFT 0 +#define TEXT_HA_CENTER 1 +#define TEXT_HA_RIGHT 2 // Text dimension structures. -struct FontDimensions -{ +struct FontDimensions { int width, height; }; // Max/Min macros. -#define MAX(a, b) ((a) > (b) ? (a) : (b)) -#define MIN(a, b) ((a) < (b) ? (a) : (b)) -#define MAX3(a, b, c) MAX(a, MAX(b, c)) -#define MIN3(a, b, c) MIN(a, MIN(b, c)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#define MAX3(a, b, c) MAX(a, MAX(b, c)) +#define MIN3(a, b, c) MIN(a, MIN(b, c)) // Apply DeadBand -#define APPLY_DEADBAND(x, y) { x = (x)+GRAPHICS_HDEADBAND; y=(y)+GRAPHICS_VDEADBAND; } -#define APPLY_VDEADBAND(y) ((y)+GRAPHICS_VDEADBAND) -#define APPLY_HDEADBAND(x) ((x)+GRAPHICS_HDEADBAND) +#define APPLY_DEADBAND(x, y) { x = (x) + GRAPHICS_HDEADBAND; y = (y) + GRAPHICS_VDEADBAND; } +#define APPLY_VDEADBAND(y) ((y) + GRAPHICS_VDEADBAND) +#define APPLY_HDEADBAND(x) ((x) + GRAPHICS_HDEADBAND) // Check if coordinates are valid. If not, return. Assumes unsigned coordinate -#define CHECK_COORDS(x, y) if(x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) return; -#define CHECK_COORD_X(x) if(x >= GRAPHICS_WIDTH_REAL) return; -#define CHECK_COORD_Y(y) if(y >= GRAPHICS_HEIGHT_REAL) return; +#define CHECK_COORDS(x, y) if (x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) { return; } +#define CHECK_COORD_X(x) if (x >= GRAPHICS_WIDTH_REAL) { return; } +#define CHECK_COORD_Y(y) if (y >= GRAPHICS_HEIGHT_REAL) { return; } // Clip coordinates out of range - assumes unsigned coordinate -#define CLIP_COORD_X(x) { x = MIN(x, GRAPHICS_WIDTH_REAL); } -#define CLIP_COORD_Y(y) { y = MIN(y, GRAPHICS_HEIGHT_REAL); } -#define CLIP_COORDS(x, y) { CLIP_COORD_X(x); CLIP_COORD_Y(y); } +#define CLIP_COORD_X(x) { x = MIN(x, GRAPHICS_WIDTH_REAL); } +#define CLIP_COORD_Y(y) { y = MIN(y, GRAPHICS_HEIGHT_REAL); } +#define CLIP_COORDS(x, y) { CLIP_COORD_X(x); CLIP_COORD_Y(y); } // Macro to swap two variables using XOR swap. -#define SWAP(a, b) { a ^= b; b ^= a; a ^= b; } +#define SWAP(a, b) { a ^= b; b ^= a; a ^= b; } // Line triggering -#define LAST_LINE 312 //625/2 //PAL -//#define LAST_LINE 525/2 //NTSC +#define LAST_LINE 312 // 625/2 //PAL +// #define LAST_LINE 525/2 //NTSC // Global vars -#define DELAY_1_NOP() asm("nop\r\n") -#define DELAY_2_NOP() asm("nop\r\nnop\r\n") -#define DELAY_3_NOP() asm("nop\r\nnop\r\nnop\r\n") -#define DELAY_4_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\n") -#define DELAY_5_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") -#define DELAY_6_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") -#define DELAY_7_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") -#define DELAY_8_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") -#define DELAY_9_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") -#define DELAY_10_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") +#define DELAY_1_NOP() asm ("nop\r\n") +#define DELAY_2_NOP() asm ("nop\r\nnop\r\n") +#define DELAY_3_NOP() asm ("nop\r\nnop\r\nnop\r\n") +#define DELAY_4_NOP() asm ("nop\r\nnop\r\nnop\r\nnop\r\n") +#define DELAY_5_NOP() asm ("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") +#define DELAY_6_NOP() asm ("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") +#define DELAY_7_NOP() asm ("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") +#define DELAY_8_NOP() asm ("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") +#define DELAY_9_NOP() asm ("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") +#define DELAY_10_NOP() asm ("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") uint8_t getCharData(uint16_t charPos); void introText(); @@ -166,7 +165,7 @@ void clearGraphics(); uint8_t validPos(uint16_t x, uint16_t y); void setPixel(uint16_t x, uint16_t y, uint8_t state); void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius); -void swap(uint16_t* a, uint16_t* b); +void swap(uint16_t *a, uint16_t *b); void drawLine(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1); void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2); void drawArrow(uint16_t x, uint16_t y, uint16_t angle, uint16_t size); @@ -197,9 +196,9 @@ void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsi void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff); void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff); void write_word_misaligned_lm(uint16_t wordl, uint16_t wordm, unsigned int addr, unsigned int xoff, int lmode, int mmode); -//int fetch_font_info(char ch, int font, struct FontEntry *font_info, char *lookup); +// int fetch_font_info(char ch, int font, struct FontEntry *font_info, char *lookup); void write_char(char ch, unsigned int x, unsigned int y, int flags, int font); -//void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, struct FontDimensions *dim); +// void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, struct FontDimensions *dim); void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags, int font); void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags); diff --git a/flight/modules/Osd/osdgen/osdgen.c b/flight/modules/Osd/osdgen/osdgen.c index 9e9d94d8a..654b0aeef 100644 --- a/flight/modules/Osd/osdgen/osdgen.c +++ b/flight/modules/Osd/osdgen/osdgen.c @@ -51,22 +51,22 @@ #include "splash.h" /* - static uint16_t angleA=0; - static int16_t angleB=90; - static int16_t angleC=0; - static int16_t sum=2; + static uint16_t angleA=0; + static int16_t angleB=90; + static int16_t angleC=0; + static int16_t sum=2; - static int16_t m_pitch=0; - static int16_t m_roll=0; - static int16_t m_yaw=0; - static int16_t m_batt=0; - static int16_t m_alt=0; + static int16_t m_pitch=0; + static int16_t m_roll=0; + static int16_t m_yaw=0; + static int16_t m_batt=0; + static int16_t m_alt=0; - static uint8_t m_gpsStatus=0; - static int32_t m_gpsLat=0; - static int32_t m_gpsLon=0; - static float m_gpsAlt=0; - static float m_gpsSpd=0;*/ + static uint8_t m_gpsStatus=0; + static int32_t m_gpsLat=0; + static int32_t m_gpsLon=0; + static float m_gpsAlt=0; + static float m_gpsSpd=0;*/ extern uint8_t *draw_buffer_level; extern uint8_t *draw_buffer_mask; @@ -82,75 +82,74 @@ static void osdgenTask(void *parameters); // **************** // Private constants -#define LONG_TIME 0xffff +#define LONG_TIME 0xffff xSemaphoreHandle osdSemaphore = NULL; -#define STACK_SIZE_BYTES 4096 +#define STACK_SIZE_BYTES 4096 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) -#define UPDATE_PERIOD 100 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) +#define UPDATE_PERIOD 100 // **************** // Private variables static xTaskHandle osdgenTaskHandle; -struct splashEntry -{ - unsigned int width, height; +struct splashEntry { + unsigned int width, height; const uint16_t *level; const uint16_t *mask; }; struct splashEntry splash[3] = { - { oplogo_width, - oplogo_height, - oplogo_bits, - oplogo_mask_bits }, - { level_width, - level_height, - level_bits, - level_mask_bits }, - { llama_width, - llama_height, - llama_bits, - llama_mask_bits } + { oplogo_width, + oplogo_height, + oplogo_bits, + oplogo_mask_bits }, + { level_width, + level_height, + level_bits, + level_mask_bits }, + { llama_width, + llama_height, + llama_bits, + llama_mask_bits } }; uint16_t mirror(uint16_t source) { int result = ((source & 0x8000) >> 7) | ((source & 0x4000) >> 5) | ((source & 0x2000) >> 3) | ((source & 0x1000) >> 1) | ((source & 0x0800) << 1) - | ((source & 0x0400) << 3) | ((source & 0x0200) << 5) | ((source & 0x0100) << 7) | ((source & 0x0080) >> 7) | ((source & 0x0040) >> 5) - | ((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) | ((source & 0x0008) << 1) | ((source & 0x0004) << 3) | ((source & 0x0002) << 5) - | ((source & 0x0001) << 7); + | ((source & 0x0400) << 3) | ((source & 0x0200) << 5) | ((source & 0x0100) << 7) | ((source & 0x0080) >> 7) | ((source & 0x0040) >> 5) + | ((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) | ((source & 0x0008) << 1) | ((source & 0x0004) << 3) | ((source & 0x0002) << 5) + | ((source & 0x0001) << 7); return result; } void clearGraphics() { - memset((uint8_t *) draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); - memset((uint8_t *) draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); + memset((uint8_t *)draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); + memset((uint8_t *)draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); } void copyimage(uint16_t offsetx, uint16_t offsety, int image) { - //check top/left position + // check top/left position if (!validPos(offsetx, offsety)) { return; } struct splashEntry splash_info; splash_info = splash[image]; - offsetx = offsetx / 8; + offsetx = offsetx / 8; for (uint16_t y = offsety; y < ((splash_info.height) + offsety); y++) { uint16_t x1 = offsetx; for (uint16_t x = offsetx; x < (((splash_info.width) / 16) + offsetx); x++) { draw_buffer_level[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)( - mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8); - draw_buffer_level[y * GRAPHICS_WIDTH + x1] = (uint8_t)( - mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF); - draw_buffer_mask[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)( - mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8); + mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8); + draw_buffer_level[y * GRAPHICS_WIDTH + x1] = (uint8_t)( + mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF); + draw_buffer_mask[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)( + mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8); draw_buffer_mask[y * GRAPHICS_WIDTH + x1] = (uint8_t)(mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF); x1 += 2; } @@ -168,11 +167,11 @@ uint8_t validPos(uint16_t x, uint16_t y) // Credit for this one goes to wikipedia! :-) void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) { - int f = 1 - radius; + int f = 1 - radius; int ddF_x = 1; int ddF_y = -2 * radius; - int x = 0; - int y = radius; + int x = 0; + int y = radius; write_pixel_lm(x0, y0 + radius, 1, 1); write_pixel_lm(x0, y0 - radius, 1, 1); @@ -186,11 +185,11 @@ void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) if (f >= 0) { y--; ddF_y += 2; - f += ddF_y; + f += ddF_y; } x++; ddF_x += 2; - f += ddF_x; + f += ddF_x; write_pixel_lm(x0 + x, y0 + y, 1, 1); write_pixel_lm(x0 - x, y0 + y, 1, 1); write_pixel_lm(x0 + x, y0 - y, 1, 1); @@ -202,26 +201,28 @@ void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) } } -void swap(uint16_t* a, uint16_t* b) +void swap(uint16_t *a, uint16_t *b) { uint16_t temp = *a; + *a = *b; *b = temp; } static const int8_t sinData[91] = -{ 0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33, 34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62, 63, 64, - 66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97, 97, 98, - 98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100 }; +{ 0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33, 34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62, 63, 64, + 66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97, 97, 98, + 98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100 }; static int8_t mySin(uint16_t angle) { uint16_t pos = 0; + pos = angle % 360; - int8_t mult = 1; + int8_t mult = 1; // 180-359 is same as 0-179 but negative. if (pos >= 180) { - pos = pos - 180; + pos = pos - 180; mult = -1; } // 0-89 is equal to 90-179 except backwards. @@ -250,10 +251,10 @@ static int8_t myCos(uint16_t angle) /// \param color the color to draw the pixels with. void plotFourQuadrants(int32_t centerX, int32_t centerY, int32_t deltaX, int32_t deltaY) { - write_pixel_lm(centerX + deltaX, centerY + deltaY, 1, 1); // Ist Quadrant - write_pixel_lm(centerX - deltaX, centerY + deltaY, 1, 1); // IInd Quadrant - write_pixel_lm(centerX - deltaX, centerY - deltaY, 1, 1); // IIIrd Quadrant - write_pixel_lm(centerX + deltaX, centerY - deltaY, 1, 1); // IVth Quadrant + write_pixel_lm(centerX + deltaX, centerY + deltaY, 1, 1); // Ist Quadrant + write_pixel_lm(centerX - deltaX, centerY + deltaY, 1, 1); // IInd Quadrant + write_pixel_lm(centerX - deltaX, centerY - deltaY, 1, 1); // IIIrd Quadrant + write_pixel_lm(centerX + deltaX, centerY - deltaY, 1, 1); // IVth Quadrant } /// Implements the midpoint ellipse drawing algorithm which is a bresenham @@ -267,7 +268,7 @@ void plotFourQuadrants(int32_t centerX, int32_t centerY, int32_t deltaX, int32_t void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius) { int64_t doubleHorizontalRadius = horizontalRadius * horizontalRadius; - int64_t doubleVerticalRadius = verticalRadius * verticalRadius; + int64_t doubleVerticalRadius = verticalRadius * verticalRadius; int64_t error = doubleVerticalRadius - doubleHorizontalRadius * verticalRadius + (doubleVerticalRadius >> 2); @@ -282,13 +283,13 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius) x++; deltaX += (doubleVerticalRadius << 1); - error += deltaX + doubleVerticalRadius; + error += deltaX + doubleVerticalRadius; if (error >= 0) { y--; deltaY -= (doubleHorizontalRadius << 1); - error -= deltaY; + error -= deltaY; } plotFourQuadrants(centerX, centerY, x, y); } @@ -296,15 +297,15 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius) error = (int64_t)(doubleVerticalRadius * (x + 1 / 2.0f) * (x + 1 / 2.0f) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius); while (y >= 0) { - error += doubleHorizontalRadius; + error += doubleHorizontalRadius; y--; deltaY -= (doubleHorizontalRadius << 1); - error -= deltaY; + error -= deltaY; if (error <= 0) { x++; deltaX += (doubleVerticalRadius << 1); - error += deltaX; + error += deltaX; } plotFourQuadrants(centerX, centerY, x, y); @@ -315,20 +316,21 @@ void drawArrow(uint16_t x, uint16_t y, uint16_t angle, uint16_t size) { int16_t a = myCos(angle); int16_t b = mySin(angle); + a = (a * (size / 2)) / 100; b = (b * (size / 2)) / 100; - write_line_lm((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 1, 1); //Direction line - //write_line_lm((GRAPHICS_SIZE/2)-1 + a/2, (GRAPHICS_SIZE/2)-1 + b/2, (GRAPHICS_SIZE/2)-1 - a/2, (GRAPHICS_SIZE/2)-1 - b/2, 1, 1); //Arrow bottom line + write_line_lm((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 1, 1); // Direction line + // write_line_lm((GRAPHICS_SIZE/2)-1 + a/2, (GRAPHICS_SIZE/2)-1 + b/2, (GRAPHICS_SIZE/2)-1 - a/2, (GRAPHICS_SIZE/2)-1 - b/2, 1, 1); //Arrow bottom line write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 - a / 2, (y) - 1 - b / 2, 1, 1); // Arrow "wings" write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 + a / 2, (y) - 1 + b / 2, 1, 1); } void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2) { - write_line_lm(x1, y1, x2, y1, 1, 1); //top - write_line_lm(x1, y1, x1, y2, 1, 1); //left - write_line_lm(x2, y1, x2, y2, 1, 1); //right - write_line_lm(x1, y2, x2, y2, 1, 1); //bottom + write_line_lm(x1, y1, x2, y1, 1, 1); // top + write_line_lm(x1, y1, x1, y2, 1, 1); // left + write_line_lm(x2, y1, x2, y2, 1, 1); // right + write_line_lm(x1, y2, x2, y2, 1, 1); // bottom } // simple routines @@ -348,8 +350,8 @@ void write_pixel(uint8_t *buff, unsigned int x, unsigned int y, int mode) CHECK_COORDS(x, y); // Determine the bit in the word to be set and the word // index to set it in. - int bitnum = CALC_BIT_IN_WORD(x); - int wordnum = CALC_BUFF_ADDR(x, y); + int bitnum = CALC_BIT_IN_WORD(x); + int wordnum = CALC_BUFF_ADDR(x, y); // Apply a mask. uint16_t mask = 1 << (7 - bitnum); WRITE_WORD_MODE(buff, wordnum, mask, mode); @@ -369,8 +371,8 @@ void write_pixel_lm(unsigned int x, unsigned int y, int mmode, int lmode) CHECK_COORDS(x, y); // Determine the bit in the word to be set and the word // index to set it in. - int bitnum = CALC_BIT_IN_WORD(x); - int wordnum = CALC_BUFF_ADDR(x, y); + int bitnum = CALC_BIT_IN_WORD(x); + int wordnum = CALC_BUFF_ADDR(x, y); // Apply the masks. uint16_t mask = 1 << (7 - bitnum); WRITE_WORD_MODE(draw_buffer_mask, wordnum, mask, mmode); @@ -397,9 +399,9 @@ void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y return; } /* This is an optimised algorithm for writing horizontal lines. - * We begin by finding the addresses of the x0 and x1 points. */ - int addr0 = CALC_BUFF_ADDR(x0, y); - int addr1 = CALC_BUFF_ADDR(x1, y); + * We begin by finding the addresses of the x0 and x1 points. */ + int addr0 = CALC_BUFF_ADDR(x0, y); + int addr1 = CALC_BUFF_ADDR(x1, y); int addr0_bit = CALC_BIT_IN_WORD(x0); int addr1_bit = CALC_BIT_IN_WORD(x1); int mask, mask_l, mask_r, i; @@ -454,6 +456,7 @@ void write_hline_lm(unsigned int x0, unsigned int x1, unsigned int y, int lmode, void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int endcap0, int endcap1, int mode, int mmode) { int stroke, fill; + SETUP_STROKE_FILL(stroke, fill, mode) if (x0 > x1) { SWAP(x0, x1); @@ -479,6 +482,7 @@ void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1, int mode) { unsigned int a; + CLIP_COORDS(x, y0); CLIP_COORDS(x, y1); if (y0 > y1) { @@ -489,8 +493,8 @@ void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1 } /* This is an optimised algorithm for writing vertical lines. * We begin by finding the addresses of the x,y0 and x,y1 points. */ - unsigned int addr0 = CALC_BUFF_ADDR(x, y0); - unsigned int addr1 = CALC_BUFF_ADDR(x, y1); + unsigned int addr0 = CALC_BUFF_ADDR(x, y0); + unsigned int addr1 = CALC_BUFF_ADDR(x, y1); /* Then we calculate the pixel data to be written. */ unsigned int bitnum = CALC_BIT_IN_WORD(x); uint16_t mask = 1 << (7 - bitnum); @@ -533,6 +537,7 @@ void write_vline_lm(unsigned int x, unsigned int y0, unsigned int y1, int lmode, void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int endcap0, int endcap1, int mode, int mmode) { int stroke, fill; + if (y0 > y1) { SWAP(y0, y1); } @@ -563,6 +568,7 @@ void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsigned int width, unsigned int height, int mode) { unsigned int yy, addr0_old, addr1_old; + CHECK_COORDS(x, y); CHECK_COORD_X(x + width); CHECK_COORD_Y(y + height); @@ -571,8 +577,8 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig } // Calculate as if the rectangle was only a horizontal line. We then // step these addresses through each row until we iterate `height` times. - unsigned int addr0 = CALC_BUFF_ADDR(x, y); - unsigned int addr1 = CALC_BUFF_ADDR(x + width, y); + unsigned int addr0 = CALC_BUFF_ADDR(x, y); + unsigned int addr1 = CALC_BUFF_ADDR(x + width, y); unsigned int addr0_bit = CALC_BIT_IN_WORD(x); unsigned int addr1_bit = CALC_BIT_IN_WORD(x + width); unsigned int mask, mask_l, mask_r, i; @@ -585,10 +591,10 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig } } else { // Otherwise we need to write the edges and then the middle repeatedly. - mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit); - mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit); + mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit); + mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit); // Write edges first. - yy = 0; + yy = 0; addr0_old = addr0; addr1_old = addr1; while (yy < height) { @@ -599,7 +605,7 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig yy++; } // Now write 0xffff words from start+1 to end-1 for each row. - yy = 0; + yy = 0; addr0 = addr0_old; addr1 = addr1_old; while (yy < height) { @@ -643,10 +649,10 @@ void write_filled_rectangle_lm(unsigned int x, unsigned int y, unsigned int widt */ void write_rectangle_outlined(unsigned int x, unsigned int y, int width, int height, int mode, int mmode) { - //CHECK_COORDS(x, y); - //CHECK_COORDS(x + width, y + height); - //if((x + width) > DISP_WIDTH) width = DISP_WIDTH - x; - //if((y + height) > DISP_HEIGHT) height = DISP_HEIGHT - y; + // CHECK_COORDS(x, y); + // CHECK_COORDS(x + width, y + height); + // if((x + width) > DISP_WIDTH) width = DISP_WIDTH - x; + // if((y + height) > DISP_HEIGHT) height = DISP_HEIGHT - y; write_hline_outlined(x, x + width, y, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); write_hline_outlined(x, x + width, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); write_vline_outlined(x, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); @@ -695,6 +701,7 @@ void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, unsigned int dashp, int bmode, int mode, int mmode) { int stroke, fill; + CHECK_COORDS(cx, cy); SETUP_STROKE_FILL(stroke, fill, mode); // This is a two step procedure. First, we draw the outline of the @@ -725,8 +732,8 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns } } error = -r; - x = r; - y = 0; + x = r; + y = 0; while (x >= y) { if (dashp == 0 || (y % dashp) < (dashp / 2)) { CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y, mmode); @@ -775,7 +782,7 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign y++; if (error >= 0) { --x; - xch = 1; + xch = 1; error -= x * 2; } } @@ -798,7 +805,8 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mode) { // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm - unsigned int steep = abs(y1 - y0) > abs(x1 - x0); + unsigned int steep = abs(y1 - y0) > abs(x1 - x0); + if (steep) { SWAP(x0, y0); SWAP(x1, y1); @@ -807,12 +815,12 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1 SWAP(x0, x1); SWAP(y0, y1); } - int deltax = x1 - x0; + int deltax = x1 - x0; unsigned int deltay = abs(y1 - y0); - int error = deltax / 2; + int error = deltax / 2; int ystep; unsigned int y = y0; - unsigned int x; //, lasty = y, stox = 0; + unsigned int x; // , lasty = y, stox = 0; if (y0 < y1) { ystep = 1; } else { @@ -826,7 +834,7 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1 } error -= deltay; if (error < 0) { - y += ystep; + y += ystep; error += deltax; } } @@ -862,12 +870,13 @@ void write_line_lm(unsigned int x0, unsigned int y0, unsigned int x1, unsigned i * @param mmode 0 = clear, 1 = set, 2 = toggle */ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, - __attribute__((unused)) int endcap0, __attribute__((unused)) int endcap1, - int mode, int mmode) + __attribute__((unused)) int endcap0, __attribute__((unused)) int endcap1, + int mode, int mmode) { // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm // This could be improved for speed. int omode, imode; + if (mode == 0) { omode = 0; imode = 1; @@ -884,9 +893,9 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi SWAP(x0, x1); SWAP(y0, y1); } - int deltax = x1 - x0; + int deltax = x1 - x0; unsigned int deltay = abs(y1 - y0); - int error = deltax / 2; + int error = deltax / 2; int ystep; unsigned int y = y0; unsigned int x; @@ -910,13 +919,13 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi } error -= deltay; if (error < 0) { - y += ystep; + y += ystep; error += deltax; } } // Now draw the innards. error = deltax / 2; - y = y0; + y = y0; for (x = x0; x < x1; x++) { if (steep) { write_pixel_lm(y, x, mmode, imode); @@ -925,7 +934,7 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi } error -= deltay; if (error < 0) { - y += ystep; + y += ystep; error += deltax; } } @@ -946,11 +955,12 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff, int mode) { int16_t firstmask = word >> xoff; - int16_t lastmask = word << (16 - xoff); - WRITE_WORD_MODE(buff, addr+1, firstmask && 0x00ff, mode); + int16_t lastmask = word << (16 - xoff); + + WRITE_WORD_MODE(buff, addr + 1, firstmask && 0x00ff, mode); WRITE_WORD_MODE(buff, addr, (firstmask & 0xff00) >> 8, mode); if (xoff > 0) { - WRITE_WORD_MODE(buff, addr+2, (lastmask & 0xff00) >> 8, mode); + WRITE_WORD_MODE(buff, addr + 2, (lastmask & 0xff00) >> 8, mode); } } @@ -972,11 +982,12 @@ void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsi void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff) { uint16_t firstmask = word >> xoff; - uint16_t lastmask = word << (16 - xoff); - WRITE_WORD_NAND(buff, addr+1, firstmask & 0x00ff); + uint16_t lastmask = word << (16 - xoff); + + WRITE_WORD_NAND(buff, addr + 1, firstmask & 0x00ff); WRITE_WORD_NAND(buff, addr, (firstmask & 0xff00) >> 8); if (xoff > 0) { - WRITE_WORD_NAND(buff, addr+2, (lastmask & 0xff00) >> 8); + WRITE_WORD_NAND(buff, addr + 2, (lastmask & 0xff00) >> 8); } } @@ -998,13 +1009,13 @@ void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr, void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff) { uint16_t firstmask = word >> xoff; - uint16_t lastmask = word << (16 - xoff); - WRITE_WORD_OR(buff, addr+1, firstmask & 0x00ff); + uint16_t lastmask = word << (16 - xoff); + + WRITE_WORD_OR(buff, addr + 1, firstmask & 0x00ff); WRITE_WORD_OR(buff, addr, (firstmask & 0xff00) >> 8); if (xoff > 0) { WRITE_WORD_OR(buff, addr + 2, (lastmask & 0xff00) >> 8); } - } /** @@ -1065,7 +1076,8 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font) unsigned int yy, addr_temp, row, row_temp, xshift; uint16_t and_mask, or_mask, levels; struct FontEntry font_info; - //char lookup = 0; + + // char lookup = 0; fetch_font_info(0, font, &font_info, NULL); // Compute starting address (for x,y) of character. @@ -1081,10 +1093,10 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font) return; } // Load data pointer. - row = ch * font_info.height; - row_temp = row; + row = ch * font_info.height; + row_temp = row; addr_temp = addr; - xshift = 16 - font_info.width; + xshift = 16 - font_info.width; // We can write mask words easily. for (yy = y; yy < y + font_info.height; yy++) { if (font == 3) { @@ -1099,25 +1111,25 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font) // level bits, but only where the mask bit is set; otherwise, // we need to leave them alone. To do this, for each word, we // construct an AND mask and an OR mask, and apply each individually. - row = row_temp; + row = row_temp; addr = addr_temp; for (yy = y; yy < y + font_info.height; yy++) { if (font == 3) { - levels = font_frame12x18[row]; - //if(!(flags & FONT_INVERT)) // data is normally inverted - levels = ~levels; - or_mask = font_mask12x18[row] << xshift; + levels = font_frame12x18[row]; + // if(!(flags & FONT_INVERT)) // data is normally inverted + levels = ~levels; + or_mask = font_mask12x18[row] << xshift; and_mask = (font_mask12x18[row] & levels) << xshift; } else { - levels = font_frame8x10[row]; - //if(!(flags & FONT_INVERT)) // data is normally inverted - levels = ~levels; - or_mask = font_mask8x10[row] << xshift; + levels = font_frame8x10[row]; + // if(!(flags & FONT_INVERT)) // data is normally inverted + levels = ~levels; + or_mask = font_mask8x10[row] << xshift; and_mask = (font_mask8x10[row] & levels) << xshift; } write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit); // If we're not bold write the AND mask. - //if(!(flags & FONT_BOLD)) + // if(!(flags & FONT_BOLD)) write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit); addr += GRAPHICS_WIDTH_REAL / 8; row++; @@ -1142,6 +1154,7 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) uint16_t and_mask, or_mask, levels; struct FontEntry font_info; char lookup = 0; + fetch_font_info(ch, font, &font_info, &lookup); // Compute starting address (for x,y) of character. unsigned int addr = CALC_BUFF_ADDR(x, y); @@ -1149,9 +1162,9 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) // If font only supports lowercase or uppercase, make the letter // lowercase or uppercase. /*if(font_info.flags & FONT_LOWERCASE_ONLY) - ch = tolower(ch); - if(font_info.flags & FONT_UPPERCASE_ONLY) - ch = toupper(ch);*/ + ch = tolower(ch); + if(font_info.flags & FONT_UPPERCASE_ONLY) + ch = toupper(ch);*/ fetch_font_info(ch, font, &font_info, &lookup); // How big is the character? We handle characters up to 8 pixels // wide for now. Support for large characters may be added in future. @@ -1161,10 +1174,10 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) return; } // Load data pointer. - row = lookup * font_info.height * 2; - row_temp = row; + row = lookup * font_info.height * 2; + row_temp = row; addr_temp = addr; - xshift = 16 - font_info.width; + xshift = 16 - font_info.width; // We can write mask words easily. for (yy = y; yy < y + font_info.height; yy++) { write_word_misaligned_OR(draw_buffer_mask, font_info.data[row] << xshift, addr, wbit); @@ -1175,7 +1188,7 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) // level bits, but only where the mask bit is set; otherwise, // we need to leave them alone. To do this, for each word, we // construct an AND mask and an OR mask, and apply each individually. - row = row_temp; + row = row_temp; addr = addr_temp; for (yy = y; yy < y + font_info.height; yy++) { levels = font_info.data[row + font_info.height]; @@ -1183,11 +1196,11 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) // data is normally inverted levels = ~levels; } - or_mask = font_info.data[row] << xshift; + or_mask = font_info.data[row] << xshift; and_mask = (font_info.data[row] & levels) << xshift; write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit); // If we're not bold write the AND mask. - //if(!(flags & FONT_BOLD)) + // if(!(flags & FONT_BOLD)) write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit); addr += GRAPHICS_WIDTH_REAL / 8; row++; @@ -1209,6 +1222,7 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, struct FontDimensions *dim) { int max_length = 0, line_length = 0, lines = 1; + while (*str != 0) { line_length++; if (*str == '\n' || *str == '\r') { @@ -1223,7 +1237,7 @@ void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, stru if (line_length > max_length) { max_length = line_length; } - dim->width = max_length * (font.width + xs); + dim->width = max_length * (font.width + xs); dim->height = lines * (font.height + ys); } @@ -1246,37 +1260,38 @@ void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, un int xx = 0, yy = 0, xx_original = 0; struct FontEntry font_info; struct FontDimensions dim; + // Determine font info and dimensions/position of the string. fetch_font_info(0, font, &font_info, NULL); calc_text_dimensions(str, font_info, xs, ys, &dim); switch (va) { - case TEXT_VA_TOP: - yy = y; - break; - case TEXT_VA_MIDDLE: - yy = y - (dim.height / 2); - break; - case TEXT_VA_BOTTOM: - yy = y - dim.height; - break; + case TEXT_VA_TOP: + yy = y; + break; + case TEXT_VA_MIDDLE: + yy = y - (dim.height / 2); + break; + case TEXT_VA_BOTTOM: + yy = y - dim.height; + break; } switch (ha) { - case TEXT_HA_LEFT: - xx = x; - break; - case TEXT_HA_CENTER: - xx = x - (dim.width / 2); - break; - case TEXT_HA_RIGHT: - xx = x - dim.width; - break; + case TEXT_HA_LEFT: + xx = x; + break; + case TEXT_HA_CENTER: + xx = x - (dim.width / 2); + break; + case TEXT_HA_RIGHT: + xx = x - dim.width; + break; } // Then write each character. xx_original = xx; while (*str != 0) { if (*str == '\n' || *str == '\r') { yy += ys + font_info.height; - xx = xx_original; + xx = xx_original; } else { if (xx >= 0 && xx < GRAPHICS_WIDTH_REAL) { if (font_info.id < 2) { @@ -1305,15 +1320,16 @@ void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, un * @param flags flags (passed to write_char) */ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, - __attribute__((unused)) int va, __attribute__((unused)) int ha, int flags) + __attribute__((unused)) int va, __attribute__((unused)) int ha, int flags) { int fcode = 0, fptr = 0, font = 0, fwidth = 0, fheight = 0, xx = x, yy = y, max_xx = 0, max_height = 0; struct FontEntry font_info; + // Retrieve sizes of the fonts: bigfont and smallfont. fetch_font_info(0, 0, &font_info, NULL); int smallfontwidth = font_info.width, smallfontheight = font_info.height; fetch_font_info(0, 1, &font_info, NULL); - int bigfontwidth = font_info.width, bigfontheight = font_info.height; + int bigfontwidth = font_info.width, bigfontheight = font_info.height; // 11 byte stack with last byte as NUL. char fstack[11]; fstack[10] = '\0'; @@ -1329,17 +1345,17 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned if (*str == '<' && fcode == 0) { // begin format code? fcode = 1; - fptr = 0; + fptr = 0; } if (*str == '>' && fcode == 1) { fcode = 0; if (strcmp(fstack, "B")) { // switch to "big" font (font #1) - fwidth = bigfontwidth; + fwidth = bigfontwidth; fheight = bigfontheight; } else if (strcmp(fstack, "S")) { // switch to "small" font (font #0) - fwidth = smallfontwidth; + fwidth = smallfontwidth; fheight = smallfontheight; } if (fheight > max_height) { @@ -1357,7 +1373,7 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned continue; } fstack[fptr++] = *str; - fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.) + fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.) } if (fcode == 0) { // Not a format code, raw text. @@ -1366,7 +1382,7 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned if (xx > max_xx) { max_xx = xx; } - xx = x; + xx = x; yy += fheight + ys; } } @@ -1377,27 +1393,27 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned // Now we've parsed it and got a bbox, we need to work out the dimensions of it // and how to align it. /*int width = max_xx - x; - int height = yy - y; - int ay, ax; - switch(va) - { - case TEXT_VA_TOP: ay = yy; break; - case TEXT_VA_MIDDLE: ay = yy - (height / 2); break; - case TEXT_VA_BOTTOM: ay = yy - height; break; - } - switch(ha) - { - case TEXT_HA_LEFT: ax = x; break; - case TEXT_HA_CENTER: ax = x - (width / 2); break; - case TEXT_HA_RIGHT: ax = x - width; break; - }*/ + int height = yy - y; + int ay, ax; + switch(va) + { + case TEXT_VA_TOP: ay = yy; break; + case TEXT_VA_MIDDLE: ay = yy - (height / 2); break; + case TEXT_VA_BOTTOM: ay = yy - height; break; + } + switch(ha) + { + case TEXT_HA_LEFT: ax = x; break; + case TEXT_HA_CENTER: ax = x - (width / 2); break; + case TEXT_HA_RIGHT: ax = x - width; break; + }*/ // So ax,ay is our new text origin. Parse the text format again and paint // the text on the display. fcode = 0; - fptr = 0; - font = 0; - xx = 0; - yy = 0; + fptr = 0; + font = 0; + xx = 0; + yy = 0; while (*str) { if (*str == '<' && fcode == 1) { // escape code: skip @@ -1406,20 +1422,20 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned if (*str == '<' && fcode == 0) { // begin format code? fcode = 1; - fptr = 0; + fptr = 0; } if (*str == '>' && fcode == 1) { fcode = 0; if (strcmp(fstack, "B")) { // switch to "big" font (font #1) - fwidth = bigfontwidth; + fwidth = bigfontwidth; fheight = bigfontheight; - font = 1; + font = 1; } else if (strcmp(fstack, "S")) { // switch to "small" font (font #0) - fwidth = smallfontwidth; + fwidth = smallfontwidth; fheight = smallfontheight; - font = 0; + font = 0; } // Skip over this byte. Go to next byte. str++; @@ -1433,7 +1449,7 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned continue; } fstack[fptr++] = *str; - fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.) + fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.) } if (fcode == 0) { // Not a format code, raw text. So we draw it. @@ -1444,7 +1460,7 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned if (xx > max_xx) { max_xx = xx; } - xx = x; + xx = x; yy += fheight + ys; } } @@ -1452,7 +1468,7 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned } } -//SUPEROSD- +// SUPEROSD- // graphics @@ -1495,61 +1511,62 @@ void drawAttitude(uint16_t x, uint16_t y, int16_t pitch, int16_t roll, uint16_t l = b * pitch / 90; // scale - //0 - //drawLine((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1); - //drawLine((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1); + // 0 + // drawLine((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1); + // drawLine((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1); write_line_outlined((x) - 1 - (size / 2 + 4), (y) - 1, (x) - 1 - (size / 2 + 1), (y) - 1, 0, 0, 0, 1); write_line_outlined((x) - 1 + (size / 2 + 4), (y) - 1, (x) - 1 + (size / 2 + 1), (y) - 1, 0, 0, 0, 1); - //30 - //drawLine((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2); - //drawLine((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2); + // 30 + // drawLine((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2); + // drawLine((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2); write_line_outlined((x) - 1 + indi30x1, (y) - 1 - indi30y1, (x) - 1 + indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1); write_line_outlined((x) - 1 - indi30x1, (y) - 1 - indi30y1, (x) - 1 - indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1); - //60 - //drawLine((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2); - //drawLine((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2); + // 60 + // drawLine((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2); + // drawLine((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2); write_line_outlined((x) - 1 + indi60x1, (y) - 1 - indi60y1, (x) - 1 + indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1); write_line_outlined((x) - 1 - indi60x1, (y) - 1 - indi60y1, (x) - 1 - indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1); - //90 - //drawLine((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1)); + // 90 + // drawLine((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1)); write_line_outlined((x) - 1, (y) - 1 - (size / 2 + 4), (x) - 1, (y) - 1 - (size / 2 + 1), 0, 0, 0, 1); - //roll - //drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line - write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1); //Direction line - //"wingtips" - //drawLine((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c); - //drawLine((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a); + // roll + // drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line + write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1); // Direction line + // "wingtips" + // drawLine((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c); + // drawLine((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a); write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 - b + d, (y) - 1 + a - c, 0, 0, 0, 1); write_line_outlined((x) - 1 + b + d, (y) - 1 - a - c, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1); - //pitch - //drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l); + // pitch + // drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l); write_line_outlined((x) - 1, (y) - 1, (x) - 1 - k, (y) - 1 - l, 0, 0, 0, 1); - //drawCircle(x-1, y-1, 5); - //write_circle_outlined(x-1, y-1, 5,0,0,0,1); - //drawCircle(x-1, y-1, size/2+4); - //write_circle_outlined(x-1, y-1, size/2+4,0,0,0,1); + // drawCircle(x-1, y-1, 5); + // write_circle_outlined(x-1, y-1, 5,0,0,0,1); + // drawCircle(x-1, y-1, size/2+4); + // write_circle_outlined(x-1, y-1, size/2+4,0,0,0,1); } void drawBattery(uint16_t x, uint16_t y, uint8_t battery, uint16_t size) { int i = 0; int batteryLines; - //top + + // top /*drawLine((x)-1+(size/2-size/4), (y)-1, (x)-1 + (size/2+size/4), (y)-1); - drawLine((x)-1+(size/2-size/4), (y)-1+1, (x)-1 + (size/2+size/4), (y)-1+1); + drawLine((x)-1+(size/2-size/4), (y)-1+1, (x)-1 + (size/2+size/4), (y)-1+1); - drawLine((x)-1, (y)-1+2, (x)-1 + size, (y)-1+2); - //bottom - drawLine((x)-1, (y)-1+size*3, (x)-1 + size, (y)-1+size*3); - //left - drawLine((x)-1, (y)-1+2, (x)-1, (y)-1+size*3); + drawLine((x)-1, (y)-1+2, (x)-1 + size, (y)-1+2); + //bottom + drawLine((x)-1, (y)-1+size*3, (x)-1 + size, (y)-1+size*3); + //left + drawLine((x)-1, (y)-1+2, (x)-1, (y)-1+size*3); - //right - drawLine((x)-1+size, (y)-1+2, (x)-1+size, (y)-1+size*3);*/ + //right + drawLine((x)-1+size, (y)-1+2, (x)-1+size, (y)-1+size*3);*/ write_rectangle_outlined((x) - 1, (y) - 1 + 2, size, size * 3, 0, 1); write_vline_lm((x) - 1 + (size / 2 + size / 4) + 1, (y) - 2, (y) - 1 + 1, 0, 1); @@ -1568,26 +1585,27 @@ void printTime(uint16_t x, uint16_t y) { char temp[9] = { 0 }; + sprintf(temp, "%02d:%02d:%02d", timex.hour, timex.min, timex.sec); - //printTextFB(x,y,temp); + // printTextFB(x,y,temp); write_string(temp, x, y, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); } /* - void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) { + void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) { - char temp[9]={0}; - char updown=' '; - uint16_t charx=x/16; - if(dir==0) - updown=24; - if(dir==1) - updown=25; - sprintf(temp,"%c%6dm",updown,alt); - printTextFB(charx,y+2,temp); - // frame - drawBox(charx*16-3,y,charx*16+strlen(temp)*8+3,y+11); - }*/ + char temp[9]={0}; + char updown=' '; + uint16_t charx=x/16; + if(dir==0) + updown=24; + if(dir==1) + updown=25; + sprintf(temp,"%c%6dm",updown,alt); + printTextFB(charx,y+2,temp); + // frame + drawBox(charx*16-3,y,charx*16+strlen(temp)*8+3,y+11); + }*/ /** * hud_draw_vertical_scale: Draw a vertical scale. @@ -1598,8 +1616,8 @@ void printTime(uint16_t x, uint16_t y) * @param x x displacement (typ. 0) * @param y y displacement (typ. half display height) * @param height height of scale - * @param mintick_step how often a minor tick is shown - * @param majtick_step how often a major tick is shown + * @param mintick_step how often a minor tick is shown + * @param majtick_step how often a major tick is shown * @param mintick_len minor tick length * @param majtick_len major tick length * @param boundtick_len boundary tick length @@ -1607,88 +1625,96 @@ void printTime(uint16_t x, uint16_t y) * @param flags special flags (see hud.h.) */ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int height, int mintick_step, int majtick_step, int mintick_len, int majtick_len, - int boundtick_len, __attribute__((unused)) int max_val, int flags) + int boundtick_len, __attribute__((unused)) int max_val, int flags) { - char temp[15]; //, temp2[15]; + char temp[15]; // , temp2[15]; struct FontEntry font_info; struct FontDimensions dim; // Halign should be in a small span. - //MY_ASSERT(halign >= -1 && halign <= 1); + // MY_ASSERT(halign >= -1 && halign <= 1); // Compute the position of the elements. int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, boundtick_start = 0, boundtick_end = 0; + if (halign == -1) { - majtick_start = x; - majtick_end = x + majtick_len; - mintick_start = x; - mintick_end = x + mintick_len; + majtick_start = x; + majtick_end = x + majtick_len; + mintick_start = x; + mintick_end = x + mintick_len; boundtick_start = x; - boundtick_end = x + boundtick_len; + boundtick_end = x + boundtick_len; } else if (halign == +1) { x = x - GRAPHICS_HDEADBAND; - majtick_start = GRAPHICS_WIDTH_REAL - x - 1; - majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1; - mintick_start = GRAPHICS_WIDTH_REAL - x - 1; - mintick_end = GRAPHICS_WIDTH_REAL - x - mintick_len - 1; + majtick_start = GRAPHICS_WIDTH_REAL - x - 1; + majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1; + mintick_start = GRAPHICS_WIDTH_REAL - x - 1; + mintick_end = GRAPHICS_WIDTH_REAL - x - mintick_len - 1; boundtick_start = GRAPHICS_WIDTH_REAL - x - 1; - boundtick_end = GRAPHICS_WIDTH_REAL - x - boundtick_len - 1; + boundtick_end = GRAPHICS_WIDTH_REAL - x - boundtick_len - 1; } // Retrieve width of large font (font #0); from this calculate the x spacing. fetch_font_info(0, 0, &font_info, NULL); - int arrow_len = (font_info.height / 2) + 1; // FIXME, font info being loaded correctly?? + int arrow_len = (font_info.height / 2) + 1; // FIXME, font info being loaded correctly?? int text_x_spacing = arrow_len; - int max_text_y = 0, text_length = 0; + int max_text_y = 0, text_length = 0; int small_font_char_width = font_info.width + 1; // +1 for horizontal spacing = 1 // For -(range / 2) to +(range / 2), draw the scale. - int range_2 = range / 2; //, height_2 = height / 2; - int r = 0, rr = 0, rv = 0, ys = 0, style = 0; //calc_ys = 0, + int range_2 = range / 2; // , height_2 = height / 2; + int r = 0, rr = 0, rv = 0, ys = 0, style = 0; // calc_ys = 0, // Iterate through each step. for (r = -range_2; r <= +range_2; r++) { style = 0; - rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape - rv = -rr + range_2; // for number display - if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE) + rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape + rv = -rr + range_2; // for number display + if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE) { rr += majtick_step / 2; - if (rr % majtick_step == 0) + } + if (rr % majtick_step == 0) { style = 1; // major tick - else if (rr % mintick_step == 0) + } else if (rr % mintick_step == 0) { style = 2; // minor tick - else + } else { style = 0; - if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0) + } + if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0) { continue; + } if (style) { // Calculate y position. - ys = ((long int) (r * height) / (long int) range) + y; - //sprintf(temp, "ys=%d", ys); - //con_puts(temp, 0); + ys = ((long int)(r * height) / (long int)range) + y; + // sprintf(temp, "ys=%d", ys); + // con_puts(temp, 0); // Depending on style, draw a minor or a major tick. if (style == 1) { write_hline_outlined(majtick_start, majtick_end, ys, 2, 2, 0, 1); memset(temp, ' ', 10); - //my_itoa(rv, temp); + // my_itoa(rv, temp); sprintf(temp, "%d", rv); text_length = (strlen(temp) + 1) * small_font_char_width; // add 1 for margin - if (text_length > max_text_y) + if (text_length > max_text_y) { max_text_y = text_length; - if (halign == -1) + } + if (halign == -1) { write_string(temp, majtick_end + text_x_spacing, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 1); - else + } else { write_string(temp, majtick_end - text_x_spacing + 1, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 1); - } else if (style == 2) + } + } else if (style == 2) { write_hline_outlined(mintick_start, mintick_end, ys, 2, 2, 0, 1); + } } } // Generate the string for the value, as well as calculating its dimensions. memset(temp, ' ', 10); - //my_itoa(v, temp); + // my_itoa(v, temp); sprintf(temp, "%d", v); // TODO: add auto-sizing. calc_text_dimensions(temp, font_info, 1, 0, &dim); int xx = 0, i = 0; - if (halign == -1) + if (halign == -1) { xx = majtick_end + text_x_spacing; - else + } else { xx = majtick_end - text_x_spacing; + } // Draw an arrow from the number to the point. for (i = 0; i < arrow_len; i++) { if (halign == -1) { @@ -1716,18 +1742,19 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei write_vline_lm(xx - dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1); } // Draw the text. - if (halign == -1) + if (halign == -1) { write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 0); - else + } else { write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 0); + } // Then, add a slow cut off on the edges, so the text doesn't sharply // disappear. We simply clear the areas above and below the ticker, and we // use little markers on the edges. if (halign == -1) { write_filled_rectangle_lm(majtick_end + text_x_spacing, y + (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, - 0); + 0); write_filled_rectangle_lm(majtick_end + text_x_spacing, y - (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, - 0); + 0); } else { write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y + (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0); write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y - (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0); @@ -1757,23 +1784,24 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, textoffset = 0; char headingstr[4]; majtick_start = y; - majtick_end = y - majtick_len; + majtick_end = y - majtick_len; mintick_start = y; - mintick_end = y - mintick_len; - textoffset = 8; + mintick_end = y - mintick_len; + textoffset = 8; int r, style, rr, xs; // rv, int range_2 = range / 2; for (r = -range_2; r <= +range_2; r++) { style = 0; - rr = (v + r + 360) % 360; // normalise range for modulo, add to move compass track - //rv = -rr + range_2; // for number display - if (rr % majtick_step == 0) + rr = (v + r + 360) % 360; // normalise range for modulo, add to move compass track + // rv = -rr + range_2; // for number display + if (rr % majtick_step == 0) { style = 1; // major tick - else if (rr % mintick_step == 0) + } else if (rr % mintick_step == 0) { style = 2; // minor tick + } if (style) { // Calculate x position. - xs = ((long int) (r * width) / (long int) range) + x; + xs = ((long int)(r * width) / (long int)range) + x; // Draw it. if (style == 1) { write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1); @@ -1788,18 +1816,18 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint headingstr[3] = 0; // nul to terminate } else { switch (rr) { - case 0: - headingstr[0] = 'N'; - break; - case 90: - headingstr[0] = 'E'; - break; - case 180: - headingstr[0] = 'S'; - break; - case 270: - headingstr[0] = 'W'; - break; + case 0: + headingstr[0] = 'N'; + break; + case 90: + headingstr[0] = 'E'; + break; + case 180: + headingstr[0] = 'S'; + break; + case 270: + headingstr[0] = 'W'; + break; } headingstr[1] = 0; headingstr[2] = 0; @@ -1807,8 +1835,9 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint } // +1 fudge...! write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1); - } else if (style == 2) + } else if (style == 2) { write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1); + } } } // Then, draw a rectangle with the present heading in it. @@ -1834,24 +1863,25 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, int16_t x1, x2; int16_t y1, y2; int16_t refx, refy; + alpha = DEG2RAD(angle); - refx = l_x + size / 2; - refy = l_y + size / 2; + refx = l_x + size / 2; + refy = l_y + size / 2; // - float k = 0; - float dx = sinf(alpha) * (pitch / 90.0f * (size / 2)); - float dy = cosf(alpha) * (pitch / 90.0f * (size / 2)); + float k = 0; + float dx = sinf(alpha) * (pitch / 90.0f * (size / 2)); + float dy = cosf(alpha) * (pitch / 90.0f * (size / 2)); int16_t x0 = (size / 2) - dx; int16_t y0 = (size / 2) + dy; // calculate the line function if ((angle < 90.0f) && (angle > -90)) { - vertical = 0; - if(fabsf(angle) < 1e-5f) { - horizontal = 1; - } else { - k = tanf(alpha); - } + vertical = 0; + if (fabsf(angle) < 1e-5f) { + horizontal = 1; + } else { + k = tanf(alpha); + } } else { vertical = 1; } @@ -1873,8 +1903,8 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, x1 = ((y1 - y0) + k * x0) / k; } // left crossing point - x = size; - y = k * (x - x0) + y0; + x = size; + y = k * (x - x0) + y0; x2 = x; y2 = y; if (y < 0) { @@ -1888,9 +1918,9 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, // move to location // horizon line write_line_outlined(x1 + l_x, y1 + l_y, x2 + l_x, y2 + l_y, 0, 0, 0, 1); - //fill + // fill if (angle <= 0.0f && angle > -90.0f) { - //write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = y2; i < size; i++) { x2 = ((i - y0) + k * x0) / k; if (x2 > size) { @@ -1902,7 +1932,7 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, write_hline_lm(x2 + l_x, size + l_x, i + l_y, 1, 1); } } else if (angle < -90.0f) { - //write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = 0; i < y2; i++) { x2 = ((i - y0) + k * x0) / k; if (x2 > size) { @@ -1914,7 +1944,7 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, write_hline_lm(size + l_x, x2 + l_x, i + l_y, 1, 1); } } else if (angle > 0.0f && angle < 90.0f) { - //write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = y1; i < size; i++) { x2 = ((i - y0) + k * x0) / k; if (x2 > size) { @@ -1926,7 +1956,7 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, write_hline_lm(0 + l_x, x2 + l_x, i + l_y, 1, 1); } } else if (angle > 90.0f) { - //write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = 0; i < y1; i++) { x2 = ((i - y0) + k * x0) / k; if (x2 > size) { @@ -1942,12 +1972,12 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, // horizon line write_line_outlined(x0 + l_x, 0 + l_y, x0 + l_x, size + l_y, 0, 0, 0, 1); if (angle >= 90.0f) { - //write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = 0; i < size; i++) { write_hline_lm(0 + l_x, x0 + l_x, i + l_y, 1, 1); } } else { - //write_string("6", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("6", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = 0; i < size; i++) { write_hline_lm(size + l_x, x0 + l_x, i + l_y, 1, 1); } @@ -1956,22 +1986,22 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, // horizon line write_hline_outlined(0 + l_x, size + l_x, y0 + l_y, 0, 0, 0, 1); if (angle < 0) { - //write_string("7", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("7", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = 0; i < y0; i++) { write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1); } } else { - //write_string("8", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("8", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = y0; i < size; i++) { write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1); } } } - //sides + // sides write_line_outlined(l_x, l_y, l_x, l_y + size, 0, 0, 0, 1); write_line_outlined(l_x + size, l_y, l_x + size, l_y + size, 0, 0, 0, 1); - //plane + // plane write_line_outlined(refx - 5, refy, refx + 6, refy, 0, 0, 0, 1); write_line_outlined(refx, refy, refx, refy - 3, 0, 0, 0, 1); } @@ -1986,12 +2016,13 @@ void introGraphics() /* logo */ int image = 0; struct splashEntry splash_info; + splash_info = splash[image]; copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - (splash_info.width) / 2), APPLY_VDEADBAND(GRAPHICS_BOTTOM / 2 - (splash_info.height) / 2), image); /* frame */ - drawBox(APPLY_HDEADBAND(0), APPLY_VDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT-8), APPLY_VDEADBAND(GRAPHICS_BOTTOM)); + drawBox(APPLY_HDEADBAND(0), APPLY_VDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT - 8), APPLY_VDEADBAND(GRAPHICS_BOTTOM)); // Must mask out last half-word because SPI keeps clocking it out otherwise for (uint32_t i = 0; i < 8; i++) { @@ -2003,6 +2034,7 @@ void introGraphics() void calcHomeArrow(int16_t m_yaw) { HomeLocationData home; + HomeLocationGet(&home); GPSPositionData gpsData; GPSPositionGet(&gpsData); @@ -2012,7 +2044,7 @@ void calcHomeArrow(int16_t m_yaw) float elevation; float gcsAlt = home.Altitude; // Home MSL altitude float uavAlt = gpsData.Altitude; // UAV MSL altitude - float dAlt = uavAlt - gcsAlt; // Altitude difference + float dAlt = uavAlt - gcsAlt; // Altitude difference // Convert to radians lat1 = DEG2RAD(home.Latitude) / 10000000.0f; // Home lat @@ -2022,14 +2054,14 @@ void calcHomeArrow(int16_t m_yaw) // Bearing /** - var y = Math.sin(dLon) * Math.cos(lat2); - var x = Math.cos(lat1)*Math.sin(lat2) - - Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon); - var brng = Math.atan2(y, x).toDeg(); + var y = Math.sin(dLon) * Math.cos(lat2); + var x = Math.cos(lat1)*Math.sin(lat2) - + Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon); + var brng = Math.atan2(y, x).toDeg(); **/ - y = sinf(lon2 - lon1) * cosf(lat2); - x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2 - lon1); - brng = RAD2DEG(atan2f(y,x)); + y = sinf(lon2 - lon1) * cosf(lat2); + x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2 - lon1); + brng = RAD2DEG(atan2f(y, x)); if (brng < 0) { brng += 360.0f; } @@ -2042,39 +2074,40 @@ void calcHomeArrow(int16_t m_yaw) // Haversine formula for distance /** - var R = 6371; // km - var dLat = (lat2-lat1).toRad(); - var dLon = (lon2-lon1).toRad(); - var a = Math.sin(dLat/2) * Math.sin(dLat/2) + - Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) * - Math.sin(dLon/2) * Math.sin(dLon/2); - var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)); - var d = R * c; + var R = 6371; // km + var dLat = (lat2-lat1).toRad(); + var dLon = (lon2-lon1).toRad(); + var a = Math.sin(dLat/2) * Math.sin(dLat/2) + + Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) * + Math.sin(dLon/2) * Math.sin(dLon/2); + var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)); + var d = R * c; **/ a = sinf((lat2 - lat1) / 2) * sinf((lat2 - lat1) / 2) + cosf(lat1) * cosf(lat2) * sinf((lon2 - lon1) / 2) * sinf((lon2 - lon1) / 2); c = 2.0f * atan2f(sqrtf(a), sqrtf(1.0f - a)); d = 6371.0f * 1000.0f * c; // Elevation v depends servo direction - if(d > 0.0f) - elevation = 90.0f - RAD2DEG(atanf(dAlt/d)); - else + if (d > 0.0f) { + elevation = 90.0f - RAD2DEG(atanf(dAlt / d)); + } else { elevation = 0.0f; - //! TODO: sanity check + } + // ! TODO: sanity check char temp[50] = { 0 }; - sprintf(temp, "hea:%d", (int) brng); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp, "ele:%d", (int) elevation); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp, "dis:%d", (int) d); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp, "u2g:%d", (int) u2g); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "hea:%d", (int)brng); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - 30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "ele:%d", (int)elevation); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - 30), APPLY_VDEADBAND(30 + 10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "dis:%d", (int)d); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - 30), APPLY_VDEADBAND(30 + 10 + 10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "u2g:%d", (int)u2g); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - 30), APPLY_VDEADBAND(30 + 10 + 10 + 10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp, "%c%c", (int) (u2g / 22.5f) * 2 + 0x90, (int) (u2g / 22.5f) * 2 + 0x91); - write_string(temp, APPLY_HDEADBAND(250), APPLY_VDEADBAND(40+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); + sprintf(temp, "%c%c", (int)(u2g / 22.5f) * 2 + 0x90, (int)(u2g / 22.5f) * 2 + 0x91); + write_string(temp, APPLY_HDEADBAND(250), APPLY_VDEADBAND(40 + 10 + 10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); } int lama = 10; @@ -2084,10 +2117,10 @@ void lamas(void) { char temp[10] = { 0 }; + lama++; if (lama % 10 == 0) { for (int z = 0; z < 30; z++) { - lama_loc[0][z] = rand() % (GRAPHICS_RIGHT - 10); lama_loc[1][z] = rand() % (GRAPHICS_BOTTOM - 10); } @@ -2098,10 +2131,11 @@ void lamas(void) } } -//main draw function +// main draw function void updateGraphics() { OsdSettingsData OsdSettings; + OsdSettingsGet(&OsdSettings); AttitudeActualData attitude; AttitudeActualGet(&attitude); @@ -2118,230 +2152,232 @@ void updateGraphics() PIOS_Servo_Set(1, OsdSettings.Black); switch (OsdSettings.Screen) { - case 0: // Dave simple - { - if (home.Set == HOMELOCATION_SET_FALSE) { - char temps[20] = - { 0 }; - sprintf(temps, "HOME NOT SET"); - //printTextFB(x,y,temp); - write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT/2), (GRAPHICS_BOTTOM / 2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3); - } - - char temp[50] = + case 0: // Dave simple + { + if (home.Set == HOMELOCATION_SET_FALSE) { + char temps[20] = { 0 }; - memset(temp, ' ', 40); - // Note: cast to double required due to -Wdouble-promotion compiler option is - // being used, and there is no way in C to pass a float to a variadic function like sprintf() - sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f)); - write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3); - sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f)); - write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3); - sprintf(temp, "Sat:%d", (int) gpsData.Satellites); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT-40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage FLIGHT*/ - sprintf(temp, "V:%5.2fV", (double)(PIOS_ADC_PinGet(2) * 3 * 6.1f / 4096)); - write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); - - if (gpsData.Heading > 180) - calcHomeArrow((int16_t)(gpsData.Heading - 360)); - else - calcHomeArrow((int16_t)(gpsData.Heading)); + sprintf(temps, "HOME NOT SET"); + // printTextFB(x,y,temp); + write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2), (GRAPHICS_BOTTOM / 2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3); } - break; - case 1: - { - /*drawBox(2,2,GRAPHICS_WIDTH_REAL-4,GRAPHICS_HEIGHT_REAL-4); - write_filled_rectangle(draw_buffer_mask,0,0,GRAPHICS_WIDTH_REAL-2,GRAPHICS_HEIGHT_REAL-2,0); - write_filled_rectangle(draw_buffer_mask,2,2,GRAPHICS_WIDTH_REAL-4-2,GRAPHICS_HEIGHT_REAL-4-2,2); - write_filled_rectangle(draw_buffer_mask,3,3,GRAPHICS_WIDTH_REAL-4-1,GRAPHICS_HEIGHT_REAL-4-1,0);*/ - //write_filled_rectangle(draw_buffer_mask,5,5,GRAPHICS_WIDTH_REAL-4-5,GRAPHICS_HEIGHT_REAL-4-5,0); - //write_rectangle_outlined(10,10,GRAPHICS_WIDTH_REAL-20,GRAPHICS_HEIGHT_REAL-20,0,0); - //drawLine(GRAPHICS_WIDTH_REAL-1, GRAPHICS_HEIGHT_REAL-1,(GRAPHICS_WIDTH_REAL/2)-1, GRAPHICS_HEIGHT_REAL-1 ); - //drawCircle((GRAPHICS_WIDTH_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1); - //drawCircle((GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-2); - //drawLine(0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1, (GRAPHICS_SIZE/2)-1); - //drawLine((GRAPHICS_SIZE/2)-1, 0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1); - /*angleA++; - if(angleB<=-90) - { - sum=2; - } - if(angleB>=90) - { - sum=-2; - } - angleB+=sum; - angleC+=2;*/ - // GPS HACK - if (gpsData.Heading > 180) - calcHomeArrow((int16_t)(gpsData.Heading - 360)); - else - calcHomeArrow((int16_t)(gpsData.Heading)); + char temp[50] = + { 0 }; + memset(temp, ' ', 40); + // Note: cast to double required due to -Wdouble-promotion compiler option is + // being used, and there is no way in C to pass a float to a variadic function like sprintf() + sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f)); + write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM - 30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3); + sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f)); + write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM - 10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3); + sprintf(temp, "Sat:%d", (int)gpsData.Satellites); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT - 40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - /* Draw Attitude Indicator */ - if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) { - drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]), - APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]), attitude.Pitch, attitude.Roll, 96); - } - //write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0); - //printText16( 60, 12,"Hello OP-OSD"); + /* Print ADC voltage FLIGHT*/ + sprintf(temp, "V:%5.2fV", (double)(PIOS_ADC_PinGet(2) * 3 * 6.1f / 4096)); + write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); - char temp[50] = - { 0 }; - memset(temp, ' ', 40); - sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f)); - write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f)); - write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp, "Fix:%d", (int) gpsData.Status); - write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp, "Sat:%d", (int) gpsData.Satellites); - write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - - /* Print RTC time */ - if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) { - printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]), APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y])); - } - - /* Print Number of detected video Lines */ - sprintf(temp, "Lines:%4d", PIOS_Video_GetOSDLines()); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage */ - //sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096)); - //write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - sprintf(temp, "Rssi:%4.2fV", (double)(PIOS_ADC_PinGet(5) * 3.0f / 4096.0f)); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print CPU temperature */ - sprintf(temp, "Temp:%4.2fC", (double)(PIOS_ADC_PinGet(3) * 0.29296875f - 264)); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage FLIGHT*/ - sprintf(temp, "FltV:%4.2fV", (double)(PIOS_ADC_PinGet(2) * 3.0f * 6.1f / 4096.0f)); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage VIDEO*/ - sprintf(temp, "VidV:%4.2fV", (double)(PIOS_ADC_PinGet(4) * 3.0f * 6.1f / 4096.0f)); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage RSSI */ - //sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096)); - //write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - /* Draw Battery Gauge */ - /*m_batt++; - uint8_t dir=3; - if(m_batt==101) - m_batt=0; - if(m_pitch>0) - { - dir=0; - m_alt+=m_pitch/2; - } - else if(m_pitch<0) - { - dir=1; - m_alt+=m_pitch/2; - }*/ - - /*if(OsdSettings.Battery == OSDSETTINGS_BATTERY_ENABLED) - { - drawBattery(APPLY_HDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_X]),APPLY_VDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_Y]),m_batt,16); - }*/ - - //drawAltitude(200,50,m_alt,dir); - //drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32); - // Draw airspeed (left side.) - if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) { - hud_draw_vertical_scale((int) gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]), - APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE); - } - // Draw altimeter (right side.) - if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) { - hud_draw_vertical_scale((int) gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]), - APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0); - } - // Draw compass. - if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) { - if (attitude.Yaw < 0) { - hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), - APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0); - } else { - hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), - APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0); - } - } + if (gpsData.Heading > 180) { + calcHomeArrow((int16_t)(gpsData.Heading - 360)); + } else { + calcHomeArrow((int16_t)(gpsData.Heading)); } - break; - case 2: - { - int size = 64; - int x = ((GRAPHICS_RIGHT / 2) - (size / 2)), y = (GRAPHICS_BOTTOM - size - 2); - draw_artificial_horizon(-attitude.Roll, attitude.Pitch, APPLY_HDEADBAND(x), APPLY_VDEADBAND(y), size); - hud_draw_vertical_scale((int) gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT-(x-1)), APPLY_VDEADBAND(y+(size/2)), size, 5, 10, 4, 7, - 10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE); - if (OsdSettings.AltitudeSource == OSDSETTINGS_ALTITUDESOURCE_BARO) { - hud_draw_vertical_scale((int) baro.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0); + } + break; + case 1: + { + /*drawBox(2,2,GRAPHICS_WIDTH_REAL-4,GRAPHICS_HEIGHT_REAL-4); + write_filled_rectangle(draw_buffer_mask,0,0,GRAPHICS_WIDTH_REAL-2,GRAPHICS_HEIGHT_REAL-2,0); + write_filled_rectangle(draw_buffer_mask,2,2,GRAPHICS_WIDTH_REAL-4-2,GRAPHICS_HEIGHT_REAL-4-2,2); + write_filled_rectangle(draw_buffer_mask,3,3,GRAPHICS_WIDTH_REAL-4-1,GRAPHICS_HEIGHT_REAL-4-1,0);*/ + // write_filled_rectangle(draw_buffer_mask,5,5,GRAPHICS_WIDTH_REAL-4-5,GRAPHICS_HEIGHT_REAL-4-5,0); + // write_rectangle_outlined(10,10,GRAPHICS_WIDTH_REAL-20,GRAPHICS_HEIGHT_REAL-20,0,0); + // drawLine(GRAPHICS_WIDTH_REAL-1, GRAPHICS_HEIGHT_REAL-1,(GRAPHICS_WIDTH_REAL/2)-1, GRAPHICS_HEIGHT_REAL-1 ); + // drawCircle((GRAPHICS_WIDTH_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1); + // drawCircle((GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-2); + // drawLine(0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1, (GRAPHICS_SIZE/2)-1); + // drawLine((GRAPHICS_SIZE/2)-1, 0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1); + /*angleA++; + if(angleB<=-90) + { + sum=2; + } + if(angleB>=90) + { + sum=-2; + } + angleB+=sum; + angleC+=2;*/ + + // GPS HACK + if (gpsData.Heading > 180) { + calcHomeArrow((int16_t)(gpsData.Heading - 360)); + } else { + calcHomeArrow((int16_t)(gpsData.Heading)); + } + + /* Draw Attitude Indicator */ + if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) { + drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]), + APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]), attitude.Pitch, attitude.Roll, 96); + } + // write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0); + // printText16( 60, 12,"Hello OP-OSD"); + + char temp[50] = + { 0 }; + memset(temp, ' ', 40); + sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f)); + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f)); + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "Fix:%d", (int)gpsData.Status); + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "Sat:%d", (int)gpsData.Satellites); + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + + /* Print RTC time */ + if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) { + printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]), APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y])); + } + + /* Print Number of detected video Lines */ + sprintf(temp, "Lines:%4d", PIOS_Video_GetOSDLines()); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print ADC voltage */ + // sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096)); + // write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + sprintf(temp, "Rssi:%4.2fV", (double)(PIOS_ADC_PinGet(5) * 3.0f / 4096.0f)); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print CPU temperature */ + sprintf(temp, "Temp:%4.2fC", (double)(PIOS_ADC_PinGet(3) * 0.29296875f - 264)); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print ADC voltage FLIGHT*/ + sprintf(temp, "FltV:%4.2fV", (double)(PIOS_ADC_PinGet(2) * 3.0f * 6.1f / 4096.0f)); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print ADC voltage VIDEO*/ + sprintf(temp, "VidV:%4.2fV", (double)(PIOS_ADC_PinGet(4) * 3.0f * 6.1f / 4096.0f)); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print ADC voltage RSSI */ + // sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096)); + // write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + /* Draw Battery Gauge */ + /*m_batt++; + uint8_t dir=3; + if(m_batt==101) + m_batt=0; + if(m_pitch>0) + { + dir=0; + m_alt+=m_pitch/2; + } + else if(m_pitch<0) + { + dir=1; + m_alt+=m_pitch/2; + }*/ + + /*if(OsdSettings.Battery == OSDSETTINGS_BATTERY_ENABLED) + { + drawBattery(APPLY_HDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_X]),APPLY_VDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_Y]),m_batt,16); + }*/ + + // drawAltitude(200,50,m_alt,dir); + // drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32); + // Draw airspeed (left side.) + if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) { + hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]), + APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE); + } + // Draw altimeter (right side.) + if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) { + hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]), + APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0); + } + // Draw compass. + if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) { + if (attitude.Yaw < 0) { + hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), + APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0); } else { - hud_draw_vertical_scale((int) gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, - 0); + hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), + APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0); } + } + } + break; + case 2: + { + int size = 64; + int x = ((GRAPHICS_RIGHT / 2) - (size / 2)), y = (GRAPHICS_BOTTOM - size - 2); + draw_artificial_horizon(-attitude.Roll, attitude.Pitch, APPLY_HDEADBAND(x), APPLY_VDEADBAND(y), size); + hud_draw_vertical_scale((int)gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT - (x - 1)), APPLY_VDEADBAND(y + (size / 2)), size, 5, 10, 4, 7, + 10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE); + if (OsdSettings.AltitudeSource == OSDSETTINGS_ALTITUDESOURCE_BARO) { + hud_draw_vertical_scale((int)baro.Altitude, 50, -1, APPLY_HDEADBAND((x + size + 1)), APPLY_VDEADBAND(y + (size / 2)), size, 10, 20, 4, 7, 10, 500, 0); + } else { + hud_draw_vertical_scale((int)gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x + size + 1)), APPLY_VDEADBAND(y + (size / 2)), size, 10, 20, 4, 7, 10, 500, + 0); + } - char temp[50] = - { 0 }; - memset(temp, ' ', 50); - switch (status.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_MANUAL: - sprintf(temp, "Man"); - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - sprintf(temp, "Stab1"); - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - sprintf(temp, "Stab2"); - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - sprintf(temp, "Stab3"); - break; - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - sprintf(temp, "PH"); - break; - case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - sprintf(temp, "RTB"); - break; - case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: - sprintf(temp, "PATH"); - break; - default: - sprintf(temp, "Mode: %d", status.FlightMode); - break; - } - write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - } + char temp[50] = + { 0 }; + memset(temp, ' ', 50); + switch (status.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_MANUAL: + sprintf(temp, "Man"); break; - case 3: - { - lamas(); - } + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + sprintf(temp, "Stab1"); break; - case 4: - case 5: - case 6: - { - int image = OsdSettings.Screen - 4; - struct splashEntry splash_info; - splash_info = splash[image]; - - copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2), image); - } + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + sprintf(temp, "Stab2"); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + sprintf(temp, "Stab3"); + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + sprintf(temp, "PH"); + break; + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + sprintf(temp, "RTB"); + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + sprintf(temp, "PATH"); break; default: - write_vline_lm(APPLY_HDEADBAND(GRAPHICS_RIGHT/2), APPLY_VDEADBAND(0), APPLY_VDEADBAND(GRAPHICS_BOTTOM), 1, 1); - write_hline_lm(APPLY_HDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2), 1, 1); + sprintf(temp, "Mode: %d", status.FlightMode); break; + } + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + } + break; + case 3: + { + lamas(); + } + break; + case 4: + case 5: + case 6: + { + int image = OsdSettings.Screen - 4; + struct splashEntry splash_info; + splash_info = splash[image]; + + copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - (splash_info.width) / 2), APPLY_VDEADBAND(GRAPHICS_BOTTOM / 2 - (splash_info.height) / 2), image); + } + break; + default: + write_vline_lm(APPLY_HDEADBAND(GRAPHICS_RIGHT / 2), APPLY_VDEADBAND(0), APPLY_VDEADBAND(GRAPHICS_BOTTOM), 1, 1); + write_hline_lm(APPLY_HDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT), APPLY_VDEADBAND(GRAPHICS_BOTTOM / 2), 1, 1); + break; } // Must mask out last half-word because SPI keeps clocking it out otherwise @@ -2368,7 +2404,7 @@ int32_t osdgenStart(void) { // Start gps task vSemaphoreCreateBinary(osdSemaphore); - xTaskCreate(osdgenTask, (signed char *) "OSDGEN", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdgenTaskHandle); + xTaskCreate(osdgenTask, (signed char *)"OSDGEN", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdgenTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OSDGEN, osdgenTaskHandle); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_OSDGEN); @@ -2400,7 +2436,7 @@ int32_t osdgenInitialize(void) return 0; } -MODULE_INITCALL( osdgenInitialize, osdgenStart) +MODULE_INITCALL(osdgenInitialize, osdgenStart) // **************** /** @@ -2409,10 +2445,11 @@ MODULE_INITCALL( osdgenInitialize, osdgenStart) static void osdgenTask(__attribute__((unused)) void *parameters) { - //portTickType lastSysTime; + // portTickType lastSysTime; // Loop forever - //lastSysTime = xTaskGetTickCount(); + // lastSysTime = xTaskGetTickCount(); OsdSettingsData OsdSettings; + OsdSettingsGet(&OsdSettings); PIOS_Servo_Set(0, OsdSettings.White); @@ -2446,8 +2483,8 @@ static void osdgenTask(__attribute__((unused)) void *parameters) #endif updateOnceEveryFrame(); } - //xSemaphoreTake(osdSemaphore, portMAX_DELAY); - //vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS); + // xSemaphoreTake(osdSemaphore, portMAX_DELAY); + // vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS); } } diff --git a/flight/modules/Osd/osdinput/osdinput.c b/flight/modules/Osd/osdinput/osdinput.c index 0b74109d0..6387eeb27 100644 --- a/flight/modules/Osd/osdinput/osdinput.c +++ b/flight/modules/Osd/osdinput/osdinput.c @@ -48,8 +48,8 @@ static void osdinputTask(void *parameters); // **************** // Private constants -#define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) +#define STACK_SIZE_BYTES 1024 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) #define MAX_PACKET_LENGTH 33 // **************** // Private variables @@ -58,11 +58,10 @@ static uint32_t oposdPort; static xTaskHandle osdinputTaskHandle; -static char* oposd_rx_buffer; +static char *oposd_rx_buffer; t_fifo_buffer rx; -enum osd_pkt_type -{ +enum osd_pkt_type { OSD_PKT_TYPE_MISC = 0, OSD_PKT_TYPE_NAV = 1, OSD_PKT_TYPE_MAINT = 2, OSD_PKT_TYPE_ATT = 3, OSD_PKT_TYPE_MODE = 4, }; @@ -76,7 +75,7 @@ enum osd_pkt_type int32_t osdinputStart(void) { // Start osdinput task - xTaskCreate(osdinputTask, (signed char *) "OSDINPUT", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdinputTaskHandle); + xTaskCreate(osdinputTask, (signed char *)"OSDINPUT", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdinputTaskHandle); return 0; } @@ -92,13 +91,13 @@ int32_t osdinputInitialize(void) // Initialize quaternion AttitudeActualData attitude; AttitudeActualGet(&attitude); - attitude.q1 = 1; - attitude.q2 = 0; - attitude.q3 = 0; - attitude.q4 = 0; - attitude.Roll = 0; + attitude.q1 = 1; + attitude.q2 = 0; + attitude.q3 = 0; + attitude.q4 = 0; + attitude.Roll = 0; attitude.Pitch = 0; - attitude.Yaw = 0; + attitude.Yaw = 0; AttitudeActualSet(&attitude); oposdPort = PIOS_COM_OSD; @@ -108,7 +107,7 @@ int32_t osdinputInitialize(void) return 0; } -MODULE_INITCALL( osdinputInitialize, osdinputStart) +MODULE_INITCALL(osdinputInitialize, osdinputStart) // **************** /** @@ -119,21 +118,21 @@ static void osdinputTask(__attribute__((unused)) void *parameters) { portTickType xDelay = 100 / portTICK_RATE_MS; portTickType lastSysTime; + lastSysTime = xTaskGetTickCount(); uint8_t rx_count = 0; - bool start_flag = false; + bool start_flag = false; int32_t osdRxOverflow = 0; uint8_t c = 0xAA; // Loop forever while (1) { // This blocks the task until there is something on the buffer while (PIOS_COM_ReceiveBuffer(oposdPort, &c, 1, xDelay) > 0) { - // detect start while acquiring stream if (!start_flag && ((c == 0xCB) || (c == 0x34))) { start_flag = true; - rx_count = 0; + rx_count = 0; } else if (!start_flag) { continue; } @@ -142,7 +141,7 @@ static void osdinputTask(__attribute__((unused)) void *parameters) // Flush the buffer and note the overflow event. osdRxOverflow++; start_flag = false; - rx_count = 0; + rx_count = 0; } else { oposd_rx_buffer[rx_count] = c; rx_count++; @@ -151,13 +150,13 @@ static void osdinputTask(__attribute__((unused)) void *parameters) if (oposd_rx_buffer[1] == OSD_PKT_TYPE_ATT) { AttitudeActualData attitude; AttitudeActualGet(&attitude); - attitude.q1 = 1; - attitude.q2 = 0; - attitude.q3 = 0; - attitude.q4 = 0; - attitude.Roll = (float) ((int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4] << 8)) / 10.0f; - attitude.Pitch = (float) ((int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6] << 8)) / 10.0f; - attitude.Yaw = (float) ((int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8] << 8)) / 10.0f; + attitude.q1 = 1; + attitude.q2 = 0; + attitude.q3 = 0; + attitude.q4 = 0; + attitude.Roll = (float)((int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4] << 8)) / 10.0f; + attitude.Pitch = (float)((int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6] << 8)) / 10.0f; + attitude.Yaw = (float)((int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8] << 8)) / 10.0f; AttitudeActualSet(&attitude); } else if (oposd_rx_buffer[1] == OSD_PKT_TYPE_MODE) { FlightStatusData status; @@ -166,9 +165,9 @@ static void osdinputTask(__attribute__((unused)) void *parameters) status.FlightMode = oposd_rx_buffer[3]; FlightStatusSet(&status); } - //frame completed + // frame completed start_flag = false; - rx_count = 0; + rx_count = 0; } } vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS); diff --git a/flight/modules/Osd/osdoutput/inc/osdoutput.h b/flight/modules/Osd/osdoutput/inc/osdoutput.h index a6f2e80bd..d03c29e88 100644 --- a/flight/modules/Osd/osdoutput/inc/osdoutput.h +++ b/flight/modules/Osd/osdoutput/inc/osdoutput.h @@ -32,7 +32,7 @@ int32_t osdoutputInitialize(void); -#endif /* OSDOUTPUT_H */ +#endif /* OSDOUTPUT_H */ /** * @} diff --git a/flight/modules/Osd/osdoutput/osdoutput.c b/flight/modules/Osd/osdoutput/osdoutput.c index 5b329831e..e6e5ff023 100644 --- a/flight/modules/Osd/osdoutput/osdoutput.c +++ b/flight/modules/Osd/osdoutput/osdoutput.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup OSDOUTPUTModule OSDOutput Module * @brief On screen display support - * @{ + * @{ * * @file osdoutput.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -45,103 +45,93 @@ static bool osdoutputEnabled; -enum osd_hk_sync -{ +enum osd_hk_sync { OSD_HK_SYNC_A = 0xCB, OSD_HK_SYNC_B = 0x34, }; -enum osd_hk_pkt_type -{ +enum osd_hk_pkt_type { OSD_HK_PKT_TYPE_MISC = 0, OSD_HK_PKT_TYPE_NAV = 1, OSD_HK_PKT_TYPE_MAINT = 2, OSD_HK_PKT_TYPE_ATT = 3, OSD_HK_PKT_TYPE_MODE = 4, }; -enum osd_hk_control_mode -{ +enum osd_hk_control_mode { OSD_HK_CONTROL_MODE_MANUAL = 0, OSD_HK_CONTROL_MODE_STABILIZED = 1, OSD_HK_CONTROL_MODE_AUTO = 2, }; -struct osd_hk_blob_misc -{ - uint8_t type; /* Always OSD_HK_PKT_TYPE_MISC */ - int16_t roll; - int16_t pitch; - //uint16_t home; /* Big Endian */ +struct osd_hk_blob_misc { + uint8_t type; /* Always OSD_HK_PKT_TYPE_MISC */ + int16_t roll; + int16_t pitch; + // uint16_t home; /* Big Endian */ enum osd_hk_control_mode control_mode; - uint8_t low_battery; + uint8_t low_battery; uint16_t current; /* Big Endian */ -}__attribute__((packed)); +} __attribute__((packed)); -struct osd_hk_blob_att -{ +struct osd_hk_blob_att { uint8_t type; /* Always OSD_HK_PKT_TYPE_ATT */ int16_t roll; int16_t pitch; int16_t yaw; int16_t speed; /* Big Endian */ -}__attribute__((packed)); +} __attribute__((packed)); -struct osd_hk_blob_nav -{ - uint8_t type; /* Always OSD_HK_PKT_TYPE_NAV */ +struct osd_hk_blob_nav { + uint8_t type; /* Always OSD_HK_PKT_TYPE_NAV */ uint32_t gps_lat; /* Big Endian */ uint32_t gps_lon; /* Big Endian */ -}__attribute__((packed)); +} __attribute__((packed)); -struct osd_hk_blob_maint -{ - uint8_t type; /* Always OSD_HK_PKT_TYPE_MAINT */ - uint8_t gps_speed; +struct osd_hk_blob_maint { + uint8_t type; /* Always OSD_HK_PKT_TYPE_MAINT */ + uint8_t gps_speed; uint16_t gps_alt; /* Big Endian */ uint16_t gps_dis; /* Big Endian */ - uint8_t status; - uint8_t config; - uint8_t emerg; -}__attribute__((packed)); + uint8_t status; + uint8_t config; + uint8_t emerg; +} __attribute__((packed)); -struct osd_hk_blob_mode -{ - uint8_t type; /* Always OSD_HK_PKT_TYPE_MODE */ - uint8_t fltmode; +struct osd_hk_blob_mode { + uint8_t type; /* Always OSD_HK_PKT_TYPE_MODE */ + uint8_t fltmode; uint16_t gps_alt; /* Big Endian */ uint16_t gps_dis; /* Big Endian */ - uint8_t armed; - uint8_t config; - uint8_t emerg; -}__attribute__((packed)); + uint8_t armed; + uint8_t config; + uint8_t emerg; +} __attribute__((packed)); -union osd_hk_pkt_blobs -{ - struct osd_hk_blob_misc misc; - struct osd_hk_blob_nav nav; +union osd_hk_pkt_blobs { + struct osd_hk_blob_misc misc; + struct osd_hk_blob_nav nav; struct osd_hk_blob_maint maint; - struct osd_hk_blob_att att; - struct osd_hk_blob_mode mode; -}__attribute__((packed)); + struct osd_hk_blob_att att; + struct osd_hk_blob_mode mode; +} __attribute__((packed)); -struct osd_hk_msg -{ - enum osd_hk_sync sync; - enum osd_hk_pkt_type t; +struct osd_hk_msg { + enum osd_hk_sync sync; + enum osd_hk_pkt_type t; union osd_hk_pkt_blobs v; -}__attribute__((packed)); +} __attribute__((packed)); static struct osd_hk_msg osd_hk_msg_buf; static volatile bool newPositionActualData = false; -static volatile bool newBattData = false; +static volatile bool newBattData = false; static volatile bool newAttitudeData = false; -static volatile bool newAlarmData = false; +static volatile bool newAlarmData = false; static uint32_t osd_hk_com_id; static uint8_t osd_hk_msg_dropped; static uint8_t osd_packet; -static void send_update(__attribute__((unused)) UAVObjEvent * ev) +static void send_update(__attribute__((unused)) UAVObjEvent *ev) { static enum osd_hk_sync sync = OSD_HK_SYNC_A; - struct osd_hk_msg * msg = &osd_hk_msg_buf; - union osd_hk_pkt_blobs * blob = &(osd_hk_msg_buf.v); + struct osd_hk_msg *msg = &osd_hk_msg_buf; + union osd_hk_pkt_blobs *blob = &(osd_hk_msg_buf.v); /* Make sure we have a COM port bound */ if (!osd_hk_com_id) { @@ -156,57 +146,57 @@ static void send_update(__attribute__((unused)) UAVObjEvent * ev) msg->sync = sync; switch (osd_packet) { - case OSD_HK_PKT_TYPE_MISC: - break; - case OSD_HK_PKT_TYPE_NAV: - break; - case OSD_HK_PKT_TYPE_MAINT: - break; - case OSD_HK_PKT_TYPE_ATT: - msg->t = OSD_HK_PKT_TYPE_ATT; - float roll; - AttitudeActualRollGet(&roll); - blob->att.roll = (int16_t)(roll * 10); + case OSD_HK_PKT_TYPE_MISC: + break; + case OSD_HK_PKT_TYPE_NAV: + break; + case OSD_HK_PKT_TYPE_MAINT: + break; + case OSD_HK_PKT_TYPE_ATT: + msg->t = OSD_HK_PKT_TYPE_ATT; + float roll; + AttitudeActualRollGet(&roll); + blob->att.roll = (int16_t)(roll * 10); - float pitch; - AttitudeActualPitchGet(&pitch); - blob->att.pitch = (int16_t)(pitch * 10); + float pitch; + AttitudeActualPitchGet(&pitch); + blob->att.pitch = (int16_t)(pitch * 10); - float yaw; - AttitudeActualYawGet(&yaw); - blob->att.yaw = (int16_t)(yaw * 10); - break; - case OSD_HK_PKT_TYPE_MODE: - msg->t = OSD_HK_PKT_TYPE_MODE; - FlightStatusGet(&flightStatus); - blob->mode.fltmode = flightStatus.FlightMode; - blob->mode.armed = flightStatus.Armed; - break; - default: - break; + float yaw; + AttitudeActualYawGet(&yaw); + blob->att.yaw = (int16_t)(yaw * 10); + break; + case OSD_HK_PKT_TYPE_MODE: + msg->t = OSD_HK_PKT_TYPE_MODE; + FlightStatusGet(&flightStatus); + blob->mode.fltmode = flightStatus.FlightMode; + blob->mode.armed = flightStatus.Armed; + break; + default: + break; } /* Field not supported yet */ -//blob->misc.control_mode = 0; +// blob->misc.control_mode = 0; /*if (newAlarmData) { - SystemAlarmsData alarms; - SystemAlarmsGet(&alarms); + SystemAlarmsData alarms; + SystemAlarmsGet(&alarms); - switch (alarms.Alarm[SYSTEMALARMS_ALARM_BATTERY]) { - case SYSTEMALARMS_ALARM_UNINITIALISED: - case SYSTEMALARMS_ALARM_OK: - blob->misc.low_battery = 0; - break; - case SYSTEMALARMS_ALARM_WARNING: - case SYSTEMALARMS_ALARM_ERROR: - case SYSTEMALARMS_ALARM_CRITICAL: - default: - blob->misc.low_battery = 1; - break; - } + switch (alarms.Alarm[SYSTEMALARMS_ALARM_BATTERY]) { + case SYSTEMALARMS_ALARM_UNINITIALISED: + case SYSTEMALARMS_ALARM_OK: + blob->misc.low_battery = 0; + break; + case SYSTEMALARMS_ALARM_WARNING: + case SYSTEMALARMS_ALARM_ERROR: + case SYSTEMALARMS_ALARM_CRITICAL: + default: + blob->misc.low_battery = 1; + break; + } - newAlarmData = false; - }*/ + newAlarmData = false; + }*/ #if FLIGHTBATTERYSUPPORTED if (newBattData) { @@ -217,13 +207,13 @@ static void send_update(__attribute__((unused)) UAVObjEvent * ev) /* convert to big endian */ blob->misc.current = ( - (current & 0xFF00 >> 8) | - (current & 0x00FF << 8)); + (current & 0xFF00 >> 8) | + (current & 0x00FF << 8)); newBattData = false; } #else -//blob->misc.current = 0; +// blob->misc.current = 0; #endif #if POSITIONACTUAL_SUPPORTED @@ -233,24 +223,24 @@ static void send_update(__attribute__((unused)) UAVObjEvent * ev) /* compute 3D distance */ float d = sqrt( - pow(position.North,2) + - pow(position.East,2) + - pow(position.Down,2)); + pow(position.North, 2) + + pow(position.East, 2) + + pow(position.Down, 2)); /* convert from cm to dm (10ths of m) */ uint16_t home = (uint16_t)(d / 10); /* convert to big endian */ blob->misc.home = ( - (home & 0xFF00 >> 8) | - (home & 0x00FF << 8)); + (home & 0xFF00 >> 8) | + (home & 0x00FF << 8)); newPositionActualData = false; } #else -//blob->misc.home = 0; -#endif +// blob->misc.home = 0; +#endif /* if POSITIONACTUAL_SUPPORTED */ - if (!PIOS_COM_SendBufferNonBlocking(osd_hk_com_id, (uint8_t *) &osd_hk_msg_buf, sizeof(osd_hk_msg_buf))) { + if (!PIOS_COM_SendBufferNonBlocking(osd_hk_com_id, (uint8_t *)&osd_hk_msg_buf, sizeof(osd_hk_msg_buf))) { /* Sent a packet, flip to the opposite sync */ if (sync == OSD_HK_SYNC_A) { sync = OSD_HK_SYNC_B; @@ -281,7 +271,7 @@ static int32_t osdoutputStart(void) static int32_t osdoutputInitialize(void) { - osd_hk_com_id = PIOS_COM_OSDHK; + osd_hk_com_id = PIOS_COM_OSDHK; #ifdef MODULE_OSDOUTPUT_BUILTIN osdoutputEnabled = 1; #else diff --git a/flight/modules/OveroSync/inc/overosync.h b/flight/modules/OveroSync/inc/overosync.h index b4e449c9c..c3a19cb95 100644 --- a/flight/modules/OveroSync/inc/overosync.h +++ b/flight/modules/OveroSync/inc/overosync.h @@ -1,18 +1,18 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup TelemetryModule Telemetry Module * @brief Main telemetry module * Starts three tasks (RX, TX, and priority TX) that watch event queues * and handle all the telemetry of the UAVobjects - * @{ + * @{ * * @file telemetry.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Include file of the telemetry module. - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and * objects. * @see The GNU Public License (GPL) Version 3 * @@ -41,6 +41,6 @@ int32_t TelemetryInitialize(void); #endif // TELEMETRY_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/OveroSync/overosync.c b/flight/modules/OveroSync/overosync.c index 5a2f3f5c9..039bd35cb 100644 --- a/flight/modules/OveroSync/overosync.c +++ b/flight/modules/OveroSync/overosync.c @@ -1,12 +1,12 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup Overo Sync Module * @brief Overo sync module * Starts a sync tasks that watch event queues * and push to Overo spi port UAVobjects - * @{ + * @{ * * @file overosync.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -42,7 +42,7 @@ // Private constants #define MAX_QUEUE_SIZE 200 #define STACK_SIZE_BYTES 512 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 0) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 0) // Private types @@ -54,7 +54,7 @@ static bool overoEnabled; // Private functions static void overoSyncTask(void *parameters); -static int32_t packData(uint8_t * data, int32_t length); +static int32_t packData(uint8_t *data, int32_t length); static void registerObject(UAVObjHandle obj); // External variables @@ -62,10 +62,10 @@ extern uint32_t pios_com_overo_id; extern uint32_t pios_overo_id; struct overosync { - uint32_t sent_bytes; - uint32_t sent_objects; - uint32_t failed_objects; - uint32_t received_objects; + uint32_t sent_bytes; + uint32_t sent_objects; + uint32_t failed_objects; + uint32_t received_objects; }; struct overosync *overosync; @@ -77,34 +77,33 @@ struct overosync *overosync; */ int32_t OveroSyncInitialize(void) { - #ifdef MODULE_OVEROSYNC_BUILTIN - overoEnabled = true; + overoEnabled = true; #else - - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); - - if (optionalModules[HWSETTINGS_OPTIONALMODULES_OVERO] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - overoEnabled = true; - - // Create object queues - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - } else { - overoEnabled = false; - return -1; - } + + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + + if (optionalModules[HWSETTINGS_OPTIONALMODULES_OVERO] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + overoEnabled = true; + + // Create object queues + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + } else { + overoEnabled = false; + return -1; + } #endif - - - OveroSyncStatsInitialize(); - // Initialise UAVTalk - uavTalkCon = UAVTalkInitialize(&packData); + OveroSyncStatsInitialize(); - return 0; + + // Initialise UAVTalk + uavTalkCon = UAVTalkInitialize(&packData); + + return 0; } /** @@ -114,26 +113,27 @@ int32_t OveroSyncInitialize(void) */ int32_t OveroSyncStart(void) { - //Check if module is enabled or not - if (overoEnabled == false) { - return -1; - } - - overosync = (struct overosync *) pvPortMalloc(sizeof(*overosync)); - if(overosync == NULL) - return -1; + // Check if module is enabled or not + if (overoEnabled == false) { + return -1; + } - overosync->sent_bytes = 0; + overosync = (struct overosync *)pvPortMalloc(sizeof(*overosync)); + if (overosync == NULL) { + return -1; + } - // Process all registered objects and connect queue for updates - UAVObjIterate(®isterObject); - - // Start overosync tasks - xTaskCreate(overoSyncTask, (signed char *)"OveroSync", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &overoSyncTaskHandle); - - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle); - - return 0; + overosync->sent_bytes = 0; + + // Process all registered objects and connect queue for updates + UAVObjIterate(®isterObject); + + // Start overosync tasks + xTaskCreate(overoSyncTask, (signed char *)"OveroSync", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &overoSyncTaskHandle); + + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle); + + return 0; } MODULE_INITCALL(OveroSyncInitialize, OveroSyncStart) @@ -145,12 +145,13 @@ MODULE_INITCALL(OveroSyncInitialize, OveroSyncStart) */ static void registerObject(UAVObjHandle obj) { - int32_t eventMask; - eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - if (UAVObjIsMetaobject(obj)) { - eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) - } - UAVObjConnectQueue(obj, queue, eventMask); + int32_t eventMask; + + eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; + if (UAVObjIsMetaobject(obj)) { + eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) + } + UAVObjConnectQueue(obj, queue, eventMask); } /** @@ -164,41 +165,40 @@ static void registerObject(UAVObjHandle obj) */ static void overoSyncTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; + UAVObjEvent ev; - // Kick off SPI transfers (once one is completed another will automatically transmit) - overosync->sent_objects = 0; - overosync->failed_objects = 0; - overosync->received_objects = 0; - - portTickType lastUpdateTime = xTaskGetTickCount(); - portTickType updateTime; + // Kick off SPI transfers (once one is completed another will automatically transmit) + overosync->sent_objects = 0; + overosync->failed_objects = 0; + overosync->received_objects = 0; - // Loop forever - while (1) { - // Wait for queue message - if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) { + portTickType lastUpdateTime = xTaskGetTickCount(); + portTickType updateTime; - // Process event. This calls transmitData - UAVTalkSendObjectTimestamped(uavTalkCon, ev.obj, ev.instId, false, 0); - - updateTime = xTaskGetTickCount(); - if(((portTickType) (updateTime - lastUpdateTime)) > 1000) { - // Update stats. This will trigger a local send event too - OveroSyncStatsData syncStats; - syncStats.Send = overosync->sent_bytes; - syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE; - syncStats.DroppedUpdates = overosync->failed_objects; - syncStats.Packets = PIOS_OVERO_GetPacketCount(pios_overo_id); - OveroSyncStatsSet(&syncStats); - overosync->failed_objects = 0; - overosync->sent_bytes = 0; - lastUpdateTime = updateTime; - } + // Loop forever + while (1) { + // Wait for queue message + if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) { + // Process event. This calls transmitData + UAVTalkSendObjectTimestamped(uavTalkCon, ev.obj, ev.instId, false, 0); - // TODO: Check the receive buffer - } - } + updateTime = xTaskGetTickCount(); + if (((portTickType)(updateTime - lastUpdateTime)) > 1000) { + // Update stats. This will trigger a local send event too + OveroSyncStatsData syncStats; + syncStats.Send = overosync->sent_bytes; + syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE; + syncStats.DroppedUpdates = overosync->failed_objects; + syncStats.Packets = PIOS_OVERO_GetPacketCount(pios_overo_id); + OveroSyncStatsSet(&syncStats); + overosync->failed_objects = 0; + overosync->sent_bytes = 0; + lastUpdateTime = updateTime; + } + + // TODO: Check the receive buffer + } + } } /** @@ -208,21 +208,22 @@ static void overoSyncTask(__attribute__((unused)) void *parameters) * \return -1 on failure * \return number of bytes transmitted on success */ -static int32_t packData(uint8_t * data, int32_t length) +static int32_t packData(uint8_t *data, int32_t length) { - if( PIOS_COM_SendBufferNonBlocking(pios_com_overo_id, data, length) < 0) - goto fail; + if (PIOS_COM_SendBufferNonBlocking(pios_com_overo_id, data, length) < 0) { + goto fail; + } - overosync->sent_bytes += length; + overosync->sent_bytes += length; - return length; + return length; fail: - overosync->failed_objects++; - return -1; + overosync->failed_objects++; + return -1; } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/OveroSync/simulated/inc/overosync.h b/flight/modules/OveroSync/simulated/inc/overosync.h index b4e449c9c..c3a19cb95 100644 --- a/flight/modules/OveroSync/simulated/inc/overosync.h +++ b/flight/modules/OveroSync/simulated/inc/overosync.h @@ -1,18 +1,18 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup TelemetryModule Telemetry Module * @brief Main telemetry module * Starts three tasks (RX, TX, and priority TX) that watch event queues * and handle all the telemetry of the UAVobjects - * @{ + * @{ * * @file telemetry.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Include file of the telemetry module. - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and * objects. * @see The GNU Public License (GPL) Version 3 * @@ -41,6 +41,6 @@ int32_t TelemetryInitialize(void); #endif // TELEMETRY_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/OveroSync/simulated/overosync.c b/flight/modules/OveroSync/simulated/overosync.c index d8fa0058a..291765f8a 100644 --- a/flight/modules/OveroSync/simulated/overosync.c +++ b/flight/modules/OveroSync/simulated/overosync.c @@ -1,12 +1,12 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup TelemetryModule Telemetry Module * @brief Main telemetry module * Starts three tasks (RX, TX, and priority TX) that watch event queues * and handle all the telemetry of the UAVobjects - * @{ + * @{ * * @file telemetry.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -40,9 +40,9 @@ // Private constants #define OVEROSYNC_PACKET_SIZE 1024 -#define MAX_QUEUE_SIZE 40 -#define STACK_SIZE_BYTES 512 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 0) +#define MAX_QUEUE_SIZE 40 +#define STACK_SIZE_BYTES 512 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 0) // Private types @@ -52,31 +52,31 @@ static UAVTalkConnection uavTalkCon; static xTaskHandle overoSyncTaskHandle; volatile bool buffer_swap_failed; volatile uint32_t buffer_swap_timeval; -FILE * fid; +FILE *fid; // Private functions static void overoSyncTask(void *parameters); -static int32_t packData(uint8_t * data, int32_t length); +static int32_t packData(uint8_t *data, int32_t length); static void registerObject(UAVObjHandle obj); struct dma_transaction { - uint8_t tx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__ ((aligned(4))); - uint8_t rx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__ ((aligned(4))); + uint8_t tx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__((aligned(4))); + uint8_t rx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__((aligned(4))); }; struct overosync { - struct dma_transaction transactions[2]; - uint32_t active_transaction_id; - uint32_t loading_transaction_id; - xSemaphoreHandle transaction_lock; - xSemaphoreHandle buffer_lock; - volatile bool transaction_done; - uint32_t sent_bytes; - uint32_t write_pointer; - uint32_t sent_objects; - uint32_t failed_objects; - uint32_t received_objects; - uint32_t framesync_error; + struct dma_transaction transactions[2]; + uint32_t active_transaction_id; + uint32_t loading_transaction_id; + xSemaphoreHandle transaction_lock; + xSemaphoreHandle buffer_lock; + volatile bool transaction_done; + uint32_t sent_bytes; + uint32_t write_pointer; + uint32_t sent_objects; + uint32_t failed_objects; + uint32_t received_objects; + uint32_t framesync_error; }; struct overosync *overosync; @@ -88,15 +88,15 @@ struct overosync *overosync; */ int32_t OveroSyncInitialize(void) { - OveroSyncStatsInitialize(); + OveroSyncStatsInitialize(); - // Create object queues - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + // Create object queues + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - // Initialise UAVTalk - uavTalkCon = UAVTalkInitialize(&packData); + // Initialise UAVTalk + uavTalkCon = UAVTalkInitialize(&packData); - return 0; + return 0; } /** @@ -106,33 +106,36 @@ int32_t OveroSyncInitialize(void) */ int32_t OveroSyncStart(void) { - overosync = (struct overosync *) pvPortMalloc(sizeof(*overosync)); - if(overosync == NULL) - return -1; + overosync = (struct overosync *)pvPortMalloc(sizeof(*overosync)); + if (overosync == NULL) { + return -1; + } - overosync->transaction_lock = xSemaphoreCreateMutex(); - if(overosync->transaction_lock == NULL) - return -1; + overosync->transaction_lock = xSemaphoreCreateMutex(); + if (overosync->transaction_lock == NULL) { + return -1; + } - overosync->buffer_lock = xSemaphoreCreateMutex(); - if(overosync->buffer_lock == NULL) - return -1; + overosync->buffer_lock = xSemaphoreCreateMutex(); + if (overosync->buffer_lock == NULL) { + return -1; + } - overosync->active_transaction_id = 0; - overosync->loading_transaction_id = 0; - overosync->write_pointer = 0; - overosync->sent_bytes = 0; - overosync->framesync_error = 0; + overosync->active_transaction_id = 0; + overosync->loading_transaction_id = 0; + overosync->write_pointer = 0; + overosync->sent_bytes = 0; + overosync->framesync_error = 0; - // Process all registered objects and connect queue for updates - UAVObjIterate(®isterObject); - - // Start telemetry tasks - xTaskCreate(overoSyncTask, (signed char *)"OveroSync", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &overoSyncTaskHandle); - - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle); - - return 0; + // Process all registered objects and connect queue for updates + UAVObjIterate(®isterObject); + + // Start telemetry tasks + xTaskCreate(overoSyncTask, (signed char *)"OveroSync", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &overoSyncTaskHandle); + + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle); + + return 0; } MODULE_INITCALL(OveroSyncInitialize, OveroSyncStart) @@ -144,12 +147,13 @@ MODULE_INITCALL(OveroSyncInitialize, OveroSyncStart) */ static void registerObject(UAVObjHandle obj) { - int32_t eventMask; - eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - if (UAVObjIsMetaobject(obj)) { - eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) - } - UAVObjConnectQueue(obj, queue, eventMask); + int32_t eventMask; + + eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; + if (UAVObjIsMetaobject(obj)) { + eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) + } + UAVObjConnectQueue(obj, queue, eventMask); } /** @@ -163,48 +167,47 @@ static void registerObject(UAVObjHandle obj) */ static void overoSyncTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; + UAVObjEvent ev; - // Kick off SPI transfers (once one is completed another will automatically transmit) - overosync->transaction_done = true; - overosync->sent_objects = 0; - overosync->failed_objects = 0; - overosync->received_objects = 0; - - portTickType lastUpdateTime = xTaskGetTickCount(); - portTickType updateTime; - - fid = fopen("sim_log.opl", "w"); + // Kick off SPI transfers (once one is completed another will automatically transmit) + overosync->transaction_done = true; + overosync->sent_objects = 0; + overosync->failed_objects = 0; + overosync->received_objects = 0; - // Loop forever - while (1) { - // Wait for queue message - if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) { - - // Check it will fit before packetizing - if ((overosync->write_pointer + UAVObjGetNumBytes(ev.obj) + 12) >= - sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer)) { - overosync->failed_objects ++; - } else { - // Process event. This calls transmitData - UAVTalkSendObject(uavTalkCon, ev.obj, ev.instId, false, 0); - } + portTickType lastUpdateTime = xTaskGetTickCount(); + portTickType updateTime; - updateTime = xTaskGetTickCount(); - if(((portTickType) (updateTime - lastUpdateTime)) > 1000) { - // Update stats. This will trigger a local send event too - OveroSyncStatsData syncStats; - syncStats.Send = overosync->sent_bytes; - syncStats.Received = 0; - syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE; - syncStats.DroppedUpdates = overosync->failed_objects; - OveroSyncStatsSet(&syncStats); - overosync->failed_objects = 0; - overosync->sent_bytes = 0; - lastUpdateTime = updateTime; - } - } - } + fid = fopen("sim_log.opl", "w"); + + // Loop forever + while (1) { + // Wait for queue message + if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) { + // Check it will fit before packetizing + if ((overosync->write_pointer + UAVObjGetNumBytes(ev.obj) + 12) >= + sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer)) { + overosync->failed_objects++; + } else { + // Process event. This calls transmitData + UAVTalkSendObject(uavTalkCon, ev.obj, ev.instId, false, 0); + } + + updateTime = xTaskGetTickCount(); + if (((portTickType)(updateTime - lastUpdateTime)) > 1000) { + // Update stats. This will trigger a local send event too + OveroSyncStatsData syncStats; + syncStats.Send = overosync->sent_bytes; + syncStats.Received = 0; + syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE; + syncStats.DroppedUpdates = overosync->failed_objects; + OveroSyncStatsSet(&syncStats); + overosync->failed_objects = 0; + overosync->sent_bytes = 0; + lastUpdateTime = updateTime; + } + } + } } /** @@ -214,25 +217,25 @@ static void overoSyncTask(__attribute__((unused)) void *parameters) * \return -1 on failure * \return number of bytes transmitted on success */ -static int32_t packData(uint8_t * data, int32_t length) +static int32_t packData(uint8_t *data, int32_t length) { - // Get the lock for manipulating the buffer - xSemaphoreTake(overosync->buffer_lock, portMAX_DELAY); + // Get the lock for manipulating the buffer + xSemaphoreTake(overosync->buffer_lock, portMAX_DELAY); - portTickType tickTime = xTaskGetTickCount(); - uint64_t packetSize = data[2] + (data[3] << 8); - fwrite((void *) &tickTime, 1, sizeof(tickTime), fid); - fwrite((void *) &packetSize, sizeof(packetSize), 1, fid); - fwrite((void *) data, 1, length, fid); - overosync->sent_bytes += length; - overosync->sent_objects++; + portTickType tickTime = xTaskGetTickCount(); + uint64_t packetSize = data[2] + (data[3] << 8); + fwrite((void *)&tickTime, 1, sizeof(tickTime), fid); + fwrite((void *)&packetSize, sizeof(packetSize), 1, fid); + fwrite((void *)data, 1, length, fid); + overosync->sent_bytes += length; + overosync->sent_objects++; - xSemaphoreGive(overosync->buffer_lock); - - return length; + xSemaphoreGive(overosync->buffer_lock); + + return length; } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index d9bd44724..30bd42fa3 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -45,16 +45,16 @@ #include "paths.h" // Private constants -#define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) -#define MAX_QUEUE_SIZE 2 +#define STACK_SIZE_BYTES 1024 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define MAX_QUEUE_SIZE 2 #define PATH_PLANNER_UPDATE_RATE_MS 20 // Private types // Private functions static void pathPlannerTask(void *parameters); -static void updatePathDesired(UAVObjEvent * ev); +static void updatePathDesired(UAVObjEvent *ev); static void setWaypoint(uint16_t num); static uint8_t pathConditionCheck(); @@ -83,13 +83,13 @@ static bool pathplanner_active = false; */ int32_t PathPlannerStart() { - taskHandle = NULL; + taskHandle = NULL; - // Start VM thread - xTaskCreate(pathPlannerTask, (signed char *)"PathPlanner", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHPLANNER, taskHandle); + // Start VM thread + xTaskCreate(pathPlannerTask, (signed char *)"PathPlanner", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHPLANNER, taskHandle); - return 0; + return 0; } /** @@ -97,18 +97,18 @@ int32_t PathPlannerStart() */ int32_t PathPlannerInitialize() { - taskHandle = NULL; + taskHandle = NULL; - PathActionInitialize(); - PathStatusInitialize(); - PathDesiredInitialize(); - PositionActualInitialize(); - AirspeedActualInitialize(); - VelocityActualInitialize(); - WaypointInitialize(); - WaypointActiveInitialize(); - - return 0; + PathActionInitialize(); + PathStatusInitialize(); + PathDesiredInitialize(); + PositionActualInitialize(); + AirspeedActualInitialize(); + VelocityActualInitialize(); + WaypointInitialize(); + WaypointActiveInitialize(); + + return 0; } MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart) @@ -118,227 +118,242 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart) */ static void pathPlannerTask(__attribute__((unused)) void *parameters) { - // when the active waypoint changes, update pathDesired - WaypointConnectCallback(updatePathDesired); - WaypointActiveConnectCallback(updatePathDesired); - PathActionConnectCallback(updatePathDesired); + // when the active waypoint changes, update pathDesired + WaypointConnectCallback(updatePathDesired); + WaypointActiveConnectCallback(updatePathDesired); + PathActionConnectCallback(updatePathDesired); - FlightStatusData flightStatus; - PathDesiredData pathDesired; - PathStatusData pathStatus; - - // Main thread loop - bool endCondition = false; - while (1) - { + FlightStatusData flightStatus; + PathDesiredData pathDesired; + PathStatusData pathStatus; - vTaskDelay(PATH_PLANNER_UPDATE_RATE_MS); + // Main thread loop + bool endCondition = false; + while (1) { + vTaskDelay(PATH_PLANNER_UPDATE_RATE_MS); - FlightStatusGet(&flightStatus); - if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) { - pathplanner_active = false; - continue; - } + FlightStatusGet(&flightStatus); + if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) { + pathplanner_active = false; + continue; + } - WaypointActiveGet(&waypointActive); + WaypointActiveGet(&waypointActive); - if(pathplanner_active == false) { + if (pathplanner_active == false) { + pathplanner_active = true; - pathplanner_active = true; + // This triggers callback to update variable + waypointActive.Index = 0; + WaypointActiveSet(&waypointActive); - // This triggers callback to update variable - waypointActive.Index = 0; - WaypointActiveSet(&waypointActive); + continue; + } - continue; - } + WaypointInstGet(waypointActive.Index, &waypoint); + PathActionInstGet(waypoint.Action, &pathAction); + PathStatusGet(&pathStatus); + PathDesiredGet(&pathDesired); - WaypointInstGet(waypointActive.Index,&waypoint); - PathActionInstGet(waypoint.Action, &pathAction); - PathStatusGet(&pathStatus); - PathDesiredGet(&pathDesired); + // delay next step until path follower has acknowledged the path mode + if (pathStatus.UID != pathDesired.UID) { + continue; + } - // delay next step until path follower has acknowledged the path mode - if (pathStatus.UID != pathDesired.UID) { - continue; - } + // negative destinations DISABLE this feature + if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) { + setWaypoint(pathAction.ErrorDestination); + continue; + } - // negative destinations DISABLE this feature - if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) { - setWaypoint(pathAction.ErrorDestination); - continue; - } + // check if condition has been met + endCondition = pathConditionCheck(); - // check if condition has been met - endCondition = pathConditionCheck(); - - // decide what to do - switch (pathAction.Command) { - case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT: - endCondition = !endCondition; - case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT: - if (endCondition) setWaypoint(waypointActive.Index+1); - break; - case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT: - endCondition = !endCondition; - case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT: - if (endCondition) { - if (pathAction.JumpDestination<0) { - // waypoint ids <0 code relative jumps - setWaypoint(waypointActive.Index - pathAction.JumpDestination ); - } else { - setWaypoint(pathAction.JumpDestination); - } - } - break; - case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: - if (endCondition) { - if (pathAction.JumpDestination<0) { - // waypoint ids <0 code relative jumps - setWaypoint(waypointActive.Index - pathAction.JumpDestination ); - } else { - setWaypoint(pathAction.JumpDestination); - } - } else { - setWaypoint(waypointActive.Index+1); - } - break; - } - } + // decide what to do + switch (pathAction.Command) { + case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT: + endCondition = !endCondition; + case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT: + if (endCondition) { + setWaypoint(waypointActive.Index + 1); + } + break; + case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT: + endCondition = !endCondition; + case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT: + if (endCondition) { + if (pathAction.JumpDestination < 0) { + // waypoint ids <0 code relative jumps + setWaypoint(waypointActive.Index - pathAction.JumpDestination); + } else { + setWaypoint(pathAction.JumpDestination); + } + } + break; + case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: + if (endCondition) { + if (pathAction.JumpDestination < 0) { + // waypoint ids <0 code relative jumps + setWaypoint(waypointActive.Index - pathAction.JumpDestination); + } else { + setWaypoint(pathAction.JumpDestination); + } + } else { + setWaypoint(waypointActive.Index + 1); + } + break; + } + } } // callback function when waypoints changed in any way, update pathDesired -void updatePathDesired(__attribute__((unused)) UAVObjEvent * ev) { +void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev) +{ + // only ever touch pathDesired if pathplanner is enabled + if (!pathplanner_active) { + return; + } - // only ever touch pathDesired if pathplanner is enabled - if (!pathplanner_active) return; + // use local variables, dont use stack since this is huge and a callback, + // dont use the globals because we cant use mutexes here + static WaypointActiveData waypointActiveData; + static PathActionData pathActionData; + static WaypointData waypointData; + static PathDesiredData pathDesired; - // use local variables, dont use stack since this is huge and a callback, - // dont use the globals because we cant use mutexes here - static WaypointActiveData waypointActiveData; - static PathActionData pathActionData; - static WaypointData waypointData; - static PathDesiredData pathDesired; + // find out current waypoint + WaypointActiveGet(&waypointActiveData); - // find out current waypoint - WaypointActiveGet(&waypointActiveData); + WaypointInstGet(waypointActiveData.Index, &waypointData); + PathActionInstGet(waypointData.Action, &pathActionData); - WaypointInstGet(waypointActiveData.Index,&waypointData); - PathActionInstGet(waypointData.Action, &pathActionData); + pathDesired.End[PATHDESIRED_END_NORTH] = waypointData.Position[WAYPOINT_POSITION_NORTH]; + pathDesired.End[PATHDESIRED_END_EAST] = waypointData.Position[WAYPOINT_POSITION_EAST]; + pathDesired.End[PATHDESIRED_END_DOWN] = waypointData.Position[WAYPOINT_POSITION_DOWN]; + pathDesired.EndingVelocity = waypointData.Velocity; + pathDesired.Mode = pathActionData.Mode; + pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0]; + pathDesired.ModeParameters[1] = pathActionData.ModeParameters[1]; + pathDesired.ModeParameters[2] = pathActionData.ModeParameters[2]; + pathDesired.ModeParameters[3] = pathActionData.ModeParameters[3]; + pathDesired.UID = waypointActiveData.Index; - pathDesired.End[PATHDESIRED_END_NORTH] = waypointData.Position[WAYPOINT_POSITION_NORTH]; - pathDesired.End[PATHDESIRED_END_EAST] = waypointData.Position[WAYPOINT_POSITION_EAST]; - pathDesired.End[PATHDESIRED_END_DOWN] = waypointData.Position[WAYPOINT_POSITION_DOWN]; - pathDesired.EndingVelocity = waypointData.Velocity; - pathDesired.Mode = pathActionData.Mode; - pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0]; - pathDesired.ModeParameters[1] = pathActionData.ModeParameters[1]; - pathDesired.ModeParameters[2] = pathActionData.ModeParameters[2]; - pathDesired.ModeParameters[3] = pathActionData.ModeParameters[3]; - pathDesired.UID = waypointActiveData.Index; + if (waypointActiveData.Index == 0) { + PositionActualData positionActual; + PositionActualGet(&positionActual); + // First waypoint has itself as start point (used to be home position but that proved dangerous when looping) - if(waypointActiveData.Index == 0) { - PositionActualData positionActual; - PositionActualGet(&positionActual); - // First waypoint has itself as start point (used to be home position but that proved dangerous when looping) + /*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; + pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST]; + pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/ + pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; + pathDesired.StartingVelocity = pathDesired.EndingVelocity; + } else { + // Get previous waypoint as start point + WaypointData waypointPrev; + WaypointInstGet(waypointActive.Index - 1, &waypointPrev); - /*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; - pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST]; - pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/ - pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; - pathDesired.StartingVelocity = pathDesired.EndingVelocity; - } else { - // Get previous waypoint as start point - WaypointData waypointPrev; - WaypointInstGet(waypointActive.Index - 1, &waypointPrev); - - pathDesired.Start[PATHDESIRED_START_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH]; - pathDesired.Start[PATHDESIRED_START_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST]; - pathDesired.Start[PATHDESIRED_START_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN]; - pathDesired.StartingVelocity = waypointPrev.Velocity; - } - PathDesiredSet(&pathDesired); + pathDesired.Start[PATHDESIRED_START_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH]; + pathDesired.Start[PATHDESIRED_START_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST]; + pathDesired.Start[PATHDESIRED_START_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN]; + pathDesired.StartingVelocity = waypointPrev.Velocity; + } + PathDesiredSet(&pathDesired); } // helper function to go to a specific waypoint -static void setWaypoint(uint16_t num) { - // path plans wrap around - if (num>=UAVObjGetNumInstances(WaypointHandle())) { - num = 0; - } - - waypointActive.Index = num; - WaypointActiveSet(&waypointActive); +static void setWaypoint(uint16_t num) +{ + // path plans wrap around + if (num >= UAVObjGetNumInstances(WaypointHandle())) { + num = 0; + } + waypointActive.Index = num; + WaypointActiveSet(&waypointActive); } // execute the apropriate condition and report result -static uint8_t pathConditionCheck() { - // i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's - switch (pathAction.EndCondition) { - case PATHACTION_ENDCONDITION_NONE: - return conditionNone(); - break; - case PATHACTION_ENDCONDITION_TIMEOUT: - return conditionTimeOut(); - break; - case PATHACTION_ENDCONDITION_DISTANCETOTARGET: - return conditionDistanceToTarget(); - break; - case PATHACTION_ENDCONDITION_LEGREMAINING: - return conditionLegRemaining(); - break; - case PATHACTION_ENDCONDITION_BELOWERROR: - return conditionBelowError(); - break; - case PATHACTION_ENDCONDITION_ABOVEALTITUDE: - return conditionAboveAltitude(); - break; - case PATHACTION_ENDCONDITION_ABOVESPEED: - return conditionAboveSpeed(); - break; - case PATHACTION_ENDCONDITION_POINTINGTOWARDSNEXT: - return conditionPointingTowardsNext(); - break; - case PATHACTION_ENDCONDITION_PYTHONSCRIPT: - return conditionPythonScript(); - break; - case PATHACTION_ENDCONDITION_IMMEDIATE: - return conditionImmediate(); - break; - default: - // invalid endconditions are always true to prevent freezes - return true; - break; - } +static uint8_t pathConditionCheck() +{ + // i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's + switch (pathAction.EndCondition) { + case PATHACTION_ENDCONDITION_NONE: + return conditionNone(); + + break; + case PATHACTION_ENDCONDITION_TIMEOUT: + return conditionTimeOut(); + + break; + case PATHACTION_ENDCONDITION_DISTANCETOTARGET: + return conditionDistanceToTarget(); + + break; + case PATHACTION_ENDCONDITION_LEGREMAINING: + return conditionLegRemaining(); + + break; + case PATHACTION_ENDCONDITION_BELOWERROR: + return conditionBelowError(); + + break; + case PATHACTION_ENDCONDITION_ABOVEALTITUDE: + return conditionAboveAltitude(); + + break; + case PATHACTION_ENDCONDITION_ABOVESPEED: + return conditionAboveSpeed(); + + break; + case PATHACTION_ENDCONDITION_POINTINGTOWARDSNEXT: + return conditionPointingTowardsNext(); + + break; + case PATHACTION_ENDCONDITION_PYTHONSCRIPT: + return conditionPythonScript(); + + break; + case PATHACTION_ENDCONDITION_IMMEDIATE: + return conditionImmediate(); + + break; + default: + // invalid endconditions are always true to prevent freezes + return true; + + break; + } } /* the None condition is always false */ -static uint8_t conditionNone() { - return false; +static uint8_t conditionNone() +{ + return false; } /** * the Timeout condition measures time this waypoint is active * Parameter 0: timeout in seconds */ -static uint8_t conditionTimeOut() { - static uint16_t toWaypoint; - static uint32_t toStarttime; +static uint8_t conditionTimeOut() +{ + static uint16_t toWaypoint; + static uint32_t toStarttime; - // reset timer if waypoint changed - if (waypointActive.Index!=toWaypoint) { - toWaypoint = waypointActive.Index; - toStarttime = PIOS_DELAY_GetRaw(); - } - if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6f * pathAction.ConditionParameters[0]) { - // make sure we reinitialize even if the same waypoint comes twice - toWaypoint = 0xFFFF; - return true; - } - return false; + // reset timer if waypoint changed + if (waypointActive.Index != toWaypoint) { + toWaypoint = waypointActive.Index; + toStarttime = PIOS_DELAY_GetRaw(); + } + if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6f * pathAction.ConditionParameters[0]) { + // make sure we reinitialize even if the same waypoint comes twice + toWaypoint = 0xFFFF; + return true; + } + return false; } /** @@ -347,110 +362,116 @@ static uint8_t conditionTimeOut() { * Parameter 0: distance in meters * Parameter 1: flag: 0=2d 1=3d */ -static uint8_t conditionDistanceToTarget() { - float distance; - PositionActualData positionActual; +static uint8_t conditionDistanceToTarget() +{ + float distance; + PositionActualData positionActual; - PositionActualGet(&positionActual); - if (pathAction.ConditionParameters[1]>0.5f) { - distance=sqrtf(powf( waypoint.Position[0]-positionActual.North ,2) - +powf( waypoint.Position[1]-positionActual.East ,2) - +powf( waypoint.Position[1]-positionActual.Down ,2)); - } else { - distance=sqrtf(powf( waypoint.Position[0]-positionActual.North ,2) - +powf( waypoint.Position[1]-positionActual.East ,2)); - } + PositionActualGet(&positionActual); + if (pathAction.ConditionParameters[1] > 0.5f) { + distance = sqrtf(powf(waypoint.Position[0] - positionActual.North, 2) + + powf(waypoint.Position[1] - positionActual.East, 2) + + powf(waypoint.Position[1] - positionActual.Down, 2)); + } else { + distance = sqrtf(powf(waypoint.Position[0] - positionActual.North, 2) + + powf(waypoint.Position[1] - positionActual.East, 2)); + } - if (distance<=pathAction.ConditionParameters[0]) { - return true; - } - return false; + if (distance <= pathAction.ConditionParameters[0]) { + return true; + } + return false; } /** - * the LegRemaining measures how far the pathfollower got on a linear path segment + * the LegRemaining measures how far the pathfollower got on a linear path segment * returns true if closer to destination (path more complete) * Parameter 0: relative distance (0= complete, 1= just starting) */ -static uint8_t conditionLegRemaining() { +static uint8_t conditionLegRemaining() +{ + PathDesiredData pathDesired; + PositionActualData positionActual; - PathDesiredData pathDesired; - PositionActualData positionActual; - PathDesiredGet(&pathDesired); - PositionActualGet(&positionActual); + PathDesiredGet(&pathDesired); + PositionActualGet(&positionActual); - float cur[3] = {positionActual.North, positionActual.East, positionActual.Down}; - struct path_status progress; + float cur[3] = { positionActual.North, positionActual.East, positionActual.Down }; + struct path_status progress; - path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); - if ( progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0] ) { - return true; - } - return false; + path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); + if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) { + return true; + } + return false; } /** - * the BelowError measures the error on a path segment + * the BelowError measures the error on a path segment * returns true if error is below margin * Parameter 0: error margin (in m) */ -static uint8_t conditionBelowError() { +static uint8_t conditionBelowError() +{ + PathDesiredData pathDesired; + PositionActualData positionActual; - PathDesiredData pathDesired; - PositionActualData positionActual; - PathDesiredGet(&pathDesired); - PositionActualGet(&positionActual); + PathDesiredGet(&pathDesired); + PositionActualGet(&positionActual); - float cur[3] = {positionActual.North, positionActual.East, positionActual.Down}; - struct path_status progress; + float cur[3] = { positionActual.North, positionActual.East, positionActual.Down }; + struct path_status progress; - path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); - if ( progress.error <= pathAction.ConditionParameters[0] ) { - return true; - } - return false; + path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); + if (progress.error <= pathAction.ConditionParameters[0]) { + return true; + } + return false; } /** - * the AboveAltitude measures the flight altitude relative to home position + * the AboveAltitude measures the flight altitude relative to home position * returns true if above critical altitude * WARNING! Altitudes are always negative (down coordinate) * Parameter 0: altitude in meters (negative!) */ -static uint8_t conditionAboveAltitude() { - PositionActualData positionActual; - PositionActualGet(&positionActual); - - if (positionActual.Down <= pathAction.ConditionParameters[0]) { - return true; - } - return false; +static uint8_t conditionAboveAltitude() +{ + PositionActualData positionActual; + + PositionActualGet(&positionActual); + + if (positionActual.Down <= pathAction.ConditionParameters[0]) { + return true; + } + return false; } /** - * the AboveSpeed measures the movement speed (3d) + * the AboveSpeed measures the movement speed (3d) * returns true if above critical speed * Parameter 0: speed in m/s * Parameter 1: flag: 0=groundspeed 1=airspeed */ -static uint8_t conditionAboveSpeed() { +static uint8_t conditionAboveSpeed() +{ + VelocityActualData velocityActual; - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); - float velocity = sqrtf( velocityActual.North*velocityActual.North + velocityActual.East*velocityActual.East + velocityActual.Down*velocityActual.Down ); + VelocityActualGet(&velocityActual); + float velocity = sqrtf(velocityActual.North * velocityActual.North + velocityActual.East * velocityActual.East + velocityActual.Down * velocityActual.Down); - // use airspeed if requested and available - if (pathAction.ConditionParameters[1]>0.5f) { - AirspeedActualData airspeed; - AirspeedActualGet (&airspeed); - velocity = airspeed.CalibratedAirspeed; - } + // use airspeed if requested and available + if (pathAction.ConditionParameters[1] > 0.5f) { + AirspeedActualData airspeed; + AirspeedActualGet(&airspeed); + velocity = airspeed.CalibratedAirspeed; + } - if ( velocity >= pathAction.ConditionParameters[0]) { - return true; - } - return false; + if (velocity >= pathAction.ConditionParameters[0]) { + return true; + } + return false; } @@ -461,29 +482,32 @@ static uint8_t conditionAboveSpeed() { * returns true if within a certain angular margin * Parameter 0: degrees variation allowed */ -static uint8_t conditionPointingTowardsNext() { - uint16_t nextWaypointId = waypointActive.Index+1; - if (nextWaypointId>=UAVObjGetNumInstances(WaypointHandle())) { - nextWaypointId = 0; - } - WaypointData nextWaypoint; - WaypointInstGet(nextWaypointId,&nextWaypoint); +static uint8_t conditionPointingTowardsNext() +{ + uint16_t nextWaypointId = waypointActive.Index + 1; - float angle1 = atan2f((nextWaypoint.Position[0]-waypoint.Position[0]),(nextWaypoint.Position[1]-waypoint.Position[1])); + if (nextWaypointId >= UAVObjGetNumInstances(WaypointHandle())) { + nextWaypointId = 0; + } + WaypointData nextWaypoint; + WaypointInstGet(nextWaypointId, &nextWaypoint); - VelocityActualData velocity; - VelocityActualGet (&velocity); - float angle2 = atan2f(velocity.North,velocity.East); + float angle1 = atan2f((nextWaypoint.Position[0] - waypoint.Position[0]), (nextWaypoint.Position[1] - waypoint.Position[1])); - // calculate the absolute angular difference - angle1 = fabsf(RAD2DEG(angle1 - angle2)); - while (angle1>360) angle1-=360; + VelocityActualData velocity; + VelocityActualGet(&velocity); + float angle2 = atan2f(velocity.North, velocity.East); - if (angle1 <= pathAction.ConditionParameters[0]) { - return true; - } - return false; + // calculate the absolute angular difference + angle1 = fabsf(RAD2DEG(angle1 - angle2)); + while (angle1 > 360) { + angle1 -= 360; + } + if (angle1 <= pathAction.ConditionParameters[0]) { + return true; + } + return false; } /** @@ -493,13 +517,15 @@ static uint8_t conditionPointingTowardsNext() { * returns always true until implemented * Parameter 0-3: defined by user script */ -static uint8_t conditionPythonScript() { - return true; +static uint8_t conditionPythonScript() +{ + return true; } /* the immediate condition is always true */ -static uint8_t conditionImmediate() { - return true; +static uint8_t conditionImmediate() +{ + return true; } /** diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index 8e047b3b8..3e0d6bd19 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -1,17 +1,17 @@ /** -****************************************************************************** -* @addtogroup OpenPilotModules OpenPilot Modules -* @{ -* @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module -* @brief Bridge Com and Radio ports -* @{ -* -* @file RadioComBridge.c -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief Bridges selected Com Port to the COM VCP emulated serial port -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module + * @brief Bridge Com and Radio ports + * @{ + * + * @file RadioComBridge.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Bridges selected Com Port to the COM VCP emulated serial port + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -56,19 +56,18 @@ void PIOS_InitPPMFlexiPort(bool input); // Private constants #define STACK_SIZE_BYTES 150 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define MAX_RETRIES 2 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define MAX_RETRIES 2 #define RETRY_TIMEOUT_MS 20 #define EVENT_QUEUE_SIZE 10 -#define MAX_PORT_DELAY 200 -#define EV_SEND_ACK 0x20 -#define EV_SEND_NACK 0x30 +#define MAX_PORT_DELAY 200 +#define EV_SEND_ACK 0x20 +#define EV_SEND_NACK 0x30 // **************** // Private types typedef struct { - // The task handles. xTaskHandle telemetryTxTaskHandle; xTaskHandle telemetryRxTaskHandle; @@ -84,10 +83,10 @@ typedef struct { xQueueHandle uavtalkEventQueue; // Error statistics. - uint32_t comTxErrors; - uint32_t comTxRetries; - uint32_t UAVTalkErrors; - uint32_t droppedPackets; + uint32_t comTxErrors; + uint32_t comTxRetries; + uint32_t UAVTalkErrors; + uint32_t droppedPackets; // Should we parse UAVTalk? bool parseUAVTalk; @@ -97,7 +96,6 @@ typedef struct { // The current configured uart speed OPLinkSettingsComSpeedOptions comSpeed; - } RadioComBridgeData; // **************** @@ -112,8 +110,8 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length); static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte); static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type); static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, - uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); static void updateSettings(); // **************** @@ -128,21 +126,19 @@ static RadioComBridgeData *data; */ static int32_t RadioComBridgeStart(void) { - if(data) { - + if (data) { // Configure the com port configuration callback PIOS_RFM22B_SetComConfigCallback(pios_rfm22b_id, &configureComCallback); - // Get the settings. - OPLinkSettingsData oplinkSettings; - OPLinkSettingsGet(&oplinkSettings); + // Get the settings. + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); // Set the baudrates, etc. bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); if (is_coordinator) { - - // Set the frequency range. - PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency, oplinkSettings.ChannelSpacing); + // Set the frequency range. + PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency, oplinkSettings.ChannelSpacing); // Reinitilize the modem. PIOS_RFM22B_Reinit(pios_rfm22b_id); @@ -184,8 +180,8 @@ static int32_t RadioComBridgeStart(void) break; } - // Set the initial frequency. - PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, oplinkSettings.InitFrequency); + // Set the initial frequency. + PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, oplinkSettings.InitFrequency); // Start the primary tasks for receiving/sending UAVTalk packets from the GCS. xTaskCreate(telemetryTxTask, (signed char *)"telemetryTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle)); @@ -213,7 +209,6 @@ static int32_t RadioComBridgeStart(void) */ static int32_t RadioComBridgeInitialize(void) { - // allocate and initialize the static data storage only if module is enabled data = (RadioComBridgeData *)pvPortMalloc(sizeof(RadioComBridgeData)); if (!data) { @@ -225,8 +220,8 @@ static int32_t RadioComBridgeInitialize(void) ObjectPersistenceInitialize(); // Initialise UAVTalk - data->outUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); - data->inUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); + data->outUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); + data->inUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); // Initialize the queues. data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); @@ -239,11 +234,11 @@ static int32_t RadioComBridgeInitialize(void) #endif // Initialize the statistics. - data->comTxErrors = 0; - data->comTxRetries = 0; + data->comTxErrors = 0; + data->comTxRetries = 0; data->UAVTalkErrors = 0; - data->parseUAVTalk = true; - data->configured = false; + data->parseUAVTalk = true; + data->configured = false; data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; PIOS_COM_RADIO = PIOS_COM_RFM22B; @@ -270,31 +265,34 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) { // Send update (with retries) uint32_t retries = 0; - int32_t success = -1; + int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendObject(data->outUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; - if (!success) + if (!success) { ++retries; + } } data->comTxRetries += retries; - } else if(ev.event == EV_SEND_ACK) { + } else if (ev.event == EV_SEND_ACK) { // Send the ACK uint32_t retries = 0; - int32_t success = -1; + int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendAck(data->outUAVTalkCon, ev.obj, ev.instId) == 0; - if (!success) + if (!success) { ++retries; + } } data->comTxRetries += retries; - } else if(ev.event == EV_SEND_NACK) { + } else if (ev.event == EV_SEND_NACK) { // Send the NACK uint32_t retries = 0; - int32_t success = -1; + int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendNack(data->outUAVTalkCon, UAVObjGetID(ev.obj)) == 0; - if (!success) + if (!success) { ++retries; + } } data->comTxRetries += retries; } @@ -378,12 +376,13 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) inputPort = PIOS_COM_TELEM_USB_HID; } #endif /* PIOS_INCLUDE_USB */ - if(inputPort) { + if (inputPort) { uint8_t serial_data[1]; uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), MAX_PORT_DELAY); if (bytes_to_process > 0) { - for (uint8_t i = 0; i < bytes_to_process; i++) + for (uint8_t i = 0; i < bytes_to_process; i++) { ProcessInputStream(data->inUAVTalkCon, serial_data[i]); + } } } else { vTaskDelay(5); @@ -402,13 +401,14 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) { uint32_t outputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0; + #if defined(PIOS_INCLUDE_USB) // Determine output port (USB takes priority over telemetry port) if (PIOS_COM_TELEM_USB_HID && PIOS_COM_Available(PIOS_COM_TELEM_USB_HID)) { outputPort = PIOS_COM_TELEM_USB_HID; } #endif /* PIOS_INCLUDE_USB */ - if(outputPort) { + if (outputPort) { return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); } else { return -1; @@ -426,8 +426,9 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) static int32_t RadioSendHandler(uint8_t *buf, int32_t length) { uint32_t outputPort = PIOS_COM_RADIO; + // Don't send any data unless the radio port is available. - if(outputPort && PIOS_COM_Available(outputPort)) { + if (outputPort && PIOS_COM_Available(outputPort)) { return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); } else { // For some reason, if this function returns failure, it prevents saving settings. @@ -445,7 +446,7 @@ static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyt { // Keep reading until we receive a completed packet. UAVTalkRxState state = UAVTalkRelayInputStream(connectionHandle, rxbyte); - UAVTalkConnectionData *connection = (UAVTalkConnectionData*)(connectionHandle); + UAVTalkConnectionData *connection = (UAVTalkConnectionData *)(connectionHandle); UAVTalkInputProcessor *iproc = &(connection->iproc); if (state == UAVTALK_STATE_COMPLETE) { @@ -453,7 +454,7 @@ static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyt // We only generate GcsReceiver ojects, we don't consume them. if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID)) { // We treat the ObjectPersistence object differently - if(iproc->objId == OBJECTPERSISTENCE_OBJID) { + if (iproc->objId == OBJECTPERSISTENCE_OBJID) { // Unpack object, if the instance does not exist it will be created! UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); @@ -464,7 +465,7 @@ static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyt // Is this concerning or setting object? if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) { // Queue up the ACK. - queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK); + queueEvent(data->uavtalkEventQueue, (void *)iproc->obj, iproc->instId, EV_SEND_ACK); // Is this a save, load, or delete? bool success = true; @@ -474,10 +475,11 @@ static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyt #if defined(PIOS_INCLUDE_FLASH_EEPROM) // Load the settings. OPLinkSettingsData oplinkSettings; - if (PIOS_EEPROM_Load((uint8_t*)&oplinkSettings, sizeof(OPLinkSettingsData)) == 0) + if (PIOS_EEPROM_Load((uint8_t *)&oplinkSettings, sizeof(OPLinkSettingsData)) == 0) { OPLinkSettingsSet(&oplinkSettings); - else + } else { success = false; + } #endif break; } @@ -487,9 +489,10 @@ static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyt // Save the settings. OPLinkSettingsData oplinkSettings; OPLinkSettingsGet(&oplinkSettings); - int32_t ret = PIOS_EEPROM_Save((uint8_t*)&oplinkSettings, sizeof(OPLinkSettingsData)); - if (ret != 0) + int32_t ret = PIOS_EEPROM_Save((uint8_t *)&oplinkSettings, sizeof(OPLinkSettingsData)); + if (ret != 0) { success = false; + } #endif break; } @@ -498,11 +501,12 @@ static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyt #if defined(PIOS_INCLUDE_FLASH_EEPROM) // Erase the settings. OPLinkSettingsData oplinkSettings; - uint8_t *ptr = (uint8_t*)&oplinkSettings; + uint8_t *ptr = (uint8_t *)&oplinkSettings; memset(ptr, 0, sizeof(OPLinkSettingsData)); - int32_t ret = PIOS_EEPROM_Save(ptr, sizeof(OPLinkSettingsData)); - if (ret != 0) + int32_t ret = PIOS_EEPROM_Save(ptr, sizeof(OPLinkSettingsData)); + if (ret != 0) { success = false; + } #endif break; } @@ -522,22 +526,22 @@ static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyt break; case UAVTALK_TYPE_OBJ_REQ: // Queue up an object send request. - queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_UPDATE_REQ); + queueEvent(data->uavtalkEventQueue, (void *)iproc->obj, iproc->instId, EV_UPDATE_REQ); break; case UAVTALK_TYPE_OBJ_ACK: - if (UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer) == 0) + if (UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer) == 0) { // Queue up an ACK - queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK); + queueEvent(data->uavtalkEventQueue, (void *)iproc->obj, iproc->instId, EV_SEND_ACK); + } break; } } } - - } else if(state == UAVTALK_STATE_ERROR) { + } else if (state == UAVTALK_STATE_ERROR) { data->UAVTalkErrors++; // Send a NACK if required. - if((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK)) { + if ((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK)) { // Queue up a NACK queueEvent(data->uavtalkEventQueue, iproc->obj, iproc->instId, EV_SEND_NACK); } @@ -554,9 +558,10 @@ static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyt static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type) { UAVObjEvent ev; - ev.obj = (UAVObjHandle)obj; + + ev.obj = (UAVObjHandle)obj; ev.instId = instId; - ev.event = type; + ev.event = type; xQueueSend(queue, &ev, portMAX_DELAY); } @@ -567,56 +572,54 @@ static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEve * @param[in] com_speed The com port speed */ static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, - uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing) + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing) { - // Update the com baud rate data->comSpeed = com_speed; // Set the output main/flexi/vcp port and speed. bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); if (!is_coordinator) { - // Get the settings. OPLinkSettingsData oplinkSettings; OPLinkSettingsGet(&oplinkSettings); - switch (main_port) { - case OPLINKSETTINGS_REMOTEMAINPORT_DISABLED: + switch (main_port) { + case OPLINKSETTINGS_REMOTEMAINPORT_DISABLED: oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_DISABLED; break; - case OPLINKSETTINGS_REMOTEMAINPORT_SERIAL: + case OPLINKSETTINGS_REMOTEMAINPORT_SERIAL: oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_SERIAL; break; - case OPLINKSETTINGS_REMOTEMAINPORT_PPM: + case OPLINKSETTINGS_REMOTEMAINPORT_PPM: oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_PPM; break; - } + } - switch (flexi_port) { - case OPLINKSETTINGS_REMOTEFLEXIPORT_DISABLED: + switch (flexi_port) { + case OPLINKSETTINGS_REMOTEFLEXIPORT_DISABLED: oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_DISABLED; break; - case OPLINKSETTINGS_REMOTEFLEXIPORT_SERIAL: + case OPLINKSETTINGS_REMOTEFLEXIPORT_SERIAL: oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_SERIAL; break; - case OPLINKSETTINGS_REMOTEFLEXIPORT_PPM: + case OPLINKSETTINGS_REMOTEFLEXIPORT_PPM: oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_PPM; break; - } + } - switch (vcp_port) { - case OPLINKSETTINGS_REMOTEVCPPORT_DISABLED: + switch (vcp_port) { + case OPLINKSETTINGS_REMOTEVCPPORT_DISABLED: oplinkSettings.VCPPort = OPLINKSETTINGS_VCPPORT_DISABLED; break; - case OPLINKSETTINGS_REMOTEVCPPORT_SERIAL: + case OPLINKSETTINGS_REMOTEVCPPORT_SERIAL: oplinkSettings.VCPPort = OPLINKSETTINGS_VCPPORT_SERIAL; break; - } + } - // Set the frequency range. - PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, min_frequency, max_frequency, channel_spacing); + // Set the frequency range. + PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, min_frequency, max_frequency, channel_spacing); // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port). data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && @@ -638,6 +641,7 @@ static void updateSettings() { // Get the settings. OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); // We can only configure the hardware once. diff --git a/flight/modules/RadioComBridge/inc/radiocombridge.h b/flight/modules/RadioComBridge/inc/radiocombridge.h index cb9862b9a..f5b6ea509 100644 --- a/flight/modules/RadioComBridge/inc/radiocombridge.h +++ b/flight/modules/RadioComBridge/inc/radiocombridge.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module * @brief Bridge Com and Radio ports - * @{ + * @{ * * @file radiocombridge.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -34,6 +34,6 @@ #endif // RADIOCOMBRIDGE_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/Sensors/inc/sensors.h b/flight/modules/Sensors/inc/sensors.h index 16e24daab..6abdbdade 100644 --- a/flight/modules/Sensors/inc/sensors.h +++ b/flight/modules/Sensors/inc/sensors.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup Sensors Sensors Module - * @{ + * @{ * * @file attitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index feefbe1e2..ddfbf29c7 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -3,7 +3,7 @@ * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup Sensors - * @brief Acquires sensor data + * @brief Acquires sensor data * Specifically updates the the @ref Gyros, @ref Accels, and @ref Magnetometer objects * @{ * @@ -66,15 +66,15 @@ // Private constants #define STACK_SIZE_BYTES 1000 -#define TASK_PRIORITY (tskIDLE_PRIORITY+3) -#define SENSOR_PERIOD 2 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) +#define SENSOR_PERIOD 2 // Private types // Private functions static void SensorsTask(void *parameters); -static void settingsUpdatedCb(UAVObjEvent * objEv); +static void settingsUpdatedCb(UAVObjEvent *objEv); static void magOffsetEstimation(MagnetometerData *mag); // Private variables @@ -84,14 +84,16 @@ RevoCalibrationData cal; // These values are initialized by settings but can be updated by the attitude algorithm static bool bias_correct_gyro = true; -static float mag_bias[3] = {0,0,0}; -static float mag_scale[3] = {0,0,0}; -static float accel_bias[3] = {0,0,0}; -static float accel_scale[3] = {0,0,0}; -static float gyro_staticbias[3] = {0,0,0}; -static float gyro_scale[3] = {0,0,0}; +static float mag_bias[3] = { 0, 0, 0 }; +static float mag_scale[3] = { 0, 0, 0 }; +static float accel_bias[3] = { 0, 0, 0 }; +static float accel_scale[3] = { 0, 0, 0 }; +static float gyro_staticbias[3] = { 0, 0, 0 }; +static float gyro_scale[3] = { 0, 0, 0 }; -static float R[3][3] = {{0}}; +static float R[3][3] = { + { 0 } +}; static int8_t rotate = 0; /** @@ -109,20 +111,20 @@ static int8_t rotate = 0; */ int32_t SensorsInitialize(void) { - GyrosInitialize(); - GyrosBiasInitialize(); - AccelsInitialize(); - MagnetometerInitialize(); - MagBiasInitialize(); - RevoCalibrationInitialize(); - AttitudeSettingsInitialize(); + GyrosInitialize(); + GyrosBiasInitialize(); + AccelsInitialize(); + MagnetometerInitialize(); + MagBiasInitialize(); + RevoCalibrationInitialize(); + AttitudeSettingsInitialize(); - rotate = 0; + rotate = 0; - RevoCalibrationConnectCallback(&settingsUpdatedCb); - AttitudeSettingsConnectCallback(&settingsUpdatedCb); + RevoCalibrationConnectCallback(&settingsUpdatedCb); + AttitudeSettingsConnectCallback(&settingsUpdatedCb); - return 0; + return 0; } /** @@ -131,12 +133,12 @@ int32_t SensorsInitialize(void) */ int32_t SensorsStart(void) { - // Start main task - xTaskCreate(SensorsTask, (signed char *)"Sensors", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &sensorsTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS); + // Start main task + xTaskCreate(SensorsTask, (signed char *)"Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS); - return 0; + return 0; } MODULE_INITCALL(SensorsInitialize, SensorsStart) @@ -144,13 +146,13 @@ MODULE_INITCALL(SensorsInitialize, SensorsStart) int32_t accel_test; int32_t gyro_test; int32_t mag_test; -//int32_t pressure_test; +// int32_t pressure_test; /** * The sensor task. This polls the gyros at 500 Hz and pumps that data to * stabilization and to the attitude loop - * + * * This function has a lot of if/defs right now to allow these configurations: * 1. BMA180 accel and MPU6000 gyro * 2. MPU6000 gyro and accel @@ -160,423 +162,424 @@ int32_t mag_test; uint32_t sensor_dt_us; static void SensorsTask(__attribute__((unused)) void *parameters) { - portTickType lastSysTime; - uint32_t accel_samples = 0; - uint32_t gyro_samples = 0; - int32_t accel_accum[3] = {0, 0, 0}; - int32_t gyro_accum[3] = {0,0,0}; - float gyro_scaling = 0; - float accel_scaling = 0; - static int32_t timeval; + portTickType lastSysTime; + uint32_t accel_samples = 0; + uint32_t gyro_samples = 0; + int32_t accel_accum[3] = { 0, 0, 0 }; + int32_t gyro_accum[3] = { 0, 0, 0 }; + float gyro_scaling = 0; + float accel_scaling = 0; + static int32_t timeval; - AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); + AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); - UAVObjEvent ev; - settingsUpdatedCb(&ev); + UAVObjEvent ev; + settingsUpdatedCb(&ev); - const struct pios_board_info * bdinfo = &pios_board_info_blob; + const struct pios_board_info *bdinfo = &pios_board_info_blob; - switch(bdinfo->board_rev) { - case 0x01: + switch (bdinfo->board_rev) { + case 0x01: #if defined(PIOS_INCLUDE_L3GD20) - gyro_test = PIOS_L3GD20_Test(); + gyro_test = PIOS_L3GD20_Test(); #endif #if defined(PIOS_INCLUDE_BMA180) - accel_test = PIOS_BMA180_Test(); + accel_test = PIOS_BMA180_Test(); #endif - break; - case 0x02: + break; + case 0x02: #if defined(PIOS_INCLUDE_MPU6000) - gyro_test = PIOS_MPU6000_Test(); - accel_test = gyro_test; + gyro_test = PIOS_MPU6000_Test(); + accel_test = gyro_test; #endif - break; - default: - PIOS_DEBUG_Assert(0); - } + break; + default: + PIOS_DEBUG_Assert(0); + } #if defined(PIOS_INCLUDE_HMC5883) - mag_test = PIOS_HMC5883_Test(); + mag_test = PIOS_HMC5883_Test(); #else - mag_test = 0; + mag_test = 0; #endif - if(accel_test < 0 || gyro_test < 0 || mag_test < 0) { - AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); - while(1) { - PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); - vTaskDelay(10); - } - } - - // Main task loop - lastSysTime = xTaskGetTickCount(); - bool error = false; - uint32_t mag_update_time = PIOS_DELAY_GetRaw(); - while (1) { - // TODO: add timeouts to the sensor reads and set an error if the fail - sensor_dt_us = PIOS_DELAY_DiffuS(timeval); - timeval = PIOS_DELAY_GetRaw(); + if (accel_test < 0 || gyro_test < 0 || mag_test < 0) { + AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); + while (1) { + PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); + vTaskDelay(10); + } + } - if (error) { - PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); - lastSysTime = xTaskGetTickCount(); - vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS); - AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); - error = false; - } else { - AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); - } + // Main task loop + lastSysTime = xTaskGetTickCount(); + bool error = false; + uint32_t mag_update_time = PIOS_DELAY_GetRaw(); + while (1) { + // TODO: add timeouts to the sensor reads and set an error if the fail + sensor_dt_us = PIOS_DELAY_DiffuS(timeval); + timeval = PIOS_DELAY_GetRaw(); + + if (error) { + PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); + lastSysTime = xTaskGetTickCount(); + vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS); + AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); + error = false; + } else { + AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); + } - for (int i = 0; i < 3; i++) { - accel_accum[i] = 0; - gyro_accum[i] = 0; - } - accel_samples = 0; - gyro_samples = 0; + for (int i = 0; i < 3; i++) { + accel_accum[i] = 0; + gyro_accum[i] = 0; + } + accel_samples = 0; + gyro_samples = 0; - AccelsData accelsData; - GyrosData gyrosData; + AccelsData accelsData; + GyrosData gyrosData; - switch(bdinfo->board_rev) { - case 0x01: // L3GD20 + BMA180 board + switch (bdinfo->board_rev) { + case 0x01: // L3GD20 + BMA180 board #if defined(PIOS_INCLUDE_BMA180) - { - struct pios_bma180_data accel; - - int32_t read_good; - int32_t count; - - count = 0; - while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) - error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error; - if (error) { - // Unfortunately if the BMA180 ever misses getting read, then it will not - // trigger more interrupts. In this case we must force a read to kickstarts - // it. - struct pios_bma180_data data; - PIOS_BMA180_ReadAccels(&data); - continue; - } - while(read_good == 0) { - count++; - - accel_accum[1] += accel.x; - accel_accum[0] += accel.y; - accel_accum[2] -= accel.z; - - read_good = PIOS_BMA180_ReadFifo(&accel); - } - accel_samples = count; - accel_scaling = PIOS_BMA180_GetScale(); - - // Get temp from last reading - accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f; - } -#endif + { + struct pios_bma180_data accel; + + int32_t read_good; + int32_t count; + + count = 0; + while ((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) { + error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error; + } + if (error) { + // Unfortunately if the BMA180 ever misses getting read, then it will not + // trigger more interrupts. In this case we must force a read to kickstarts + // it. + struct pios_bma180_data data; + PIOS_BMA180_ReadAccels(&data); + continue; + } + while (read_good == 0) { + count++; + + accel_accum[1] += accel.x; + accel_accum[0] += accel.y; + accel_accum[2] -= accel.z; + + read_good = PIOS_BMA180_ReadFifo(&accel); + } + accel_samples = count; + accel_scaling = PIOS_BMA180_GetScale(); + + // Get temp from last reading + accelsData.temperature = 25.0f + ((float)accel.temperature - 2.0f) / 2.0f; + } +#endif /* if defined(PIOS_INCLUDE_BMA180) */ #if defined(PIOS_INCLUDE_L3GD20) - { - struct pios_l3gd20_data gyro; - gyro_samples = 0; - xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue(); - - if(xQueueReceive(gyro_queue, (void *) &gyro, 4) == errQUEUE_EMPTY) { - error = true; - continue; - } - - gyro_samples = 1; - gyro_accum[1] += gyro.gyro_x; - gyro_accum[0] += gyro.gyro_y; - gyro_accum[2] -= gyro.gyro_z; - - gyro_scaling = PIOS_L3GD20_GetScale(); + { + struct pios_l3gd20_data gyro; + gyro_samples = 0; + xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue(); - // Get temp from last reading - gyrosData.temperature = gyro.temperature; - } -#endif - break; - case 0x02: // MPU6000 board - case 0x03: // MPU6000 board + if (xQueueReceive(gyro_queue, (void *)&gyro, 4) == errQUEUE_EMPTY) { + error = true; + continue; + } + + gyro_samples = 1; + gyro_accum[1] += gyro.gyro_x; + gyro_accum[0] += gyro.gyro_y; + gyro_accum[2] -= gyro.gyro_z; + + gyro_scaling = PIOS_L3GD20_GetScale(); + + // Get temp from last reading + gyrosData.temperature = gyro.temperature; + } +#endif /* if defined(PIOS_INCLUDE_L3GD20) */ + break; + case 0x02: // MPU6000 board + case 0x03: // MPU6000 board #if defined(PIOS_INCLUDE_MPU6000) - { - struct pios_mpu6000_data mpu6000_data; - xQueueHandle queue = PIOS_MPU6000_GetQueue(); + { + struct pios_mpu6000_data mpu6000_data; + xQueueHandle queue = PIOS_MPU6000_GetQueue(); - while(xQueueReceive(queue, (void *) &mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY) - { - gyro_accum[0] += mpu6000_data.gyro_x; - gyro_accum[1] += mpu6000_data.gyro_y; - gyro_accum[2] += mpu6000_data.gyro_z; + while (xQueueReceive(queue, (void *)&mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY) { + gyro_accum[0] += mpu6000_data.gyro_x; + gyro_accum[1] += mpu6000_data.gyro_y; + gyro_accum[2] += mpu6000_data.gyro_z; - accel_accum[0] += mpu6000_data.accel_x; - accel_accum[1] += mpu6000_data.accel_y; - accel_accum[2] += mpu6000_data.accel_z; + accel_accum[0] += mpu6000_data.accel_x; + accel_accum[1] += mpu6000_data.accel_y; + accel_accum[2] += mpu6000_data.accel_z; - gyro_samples ++; - accel_samples ++; - } + gyro_samples++; + accel_samples++; + } - if (gyro_samples == 0) { - PIOS_MPU6000_ReadGyros(&mpu6000_data); - error = true; - continue; - } + if (gyro_samples == 0) { + PIOS_MPU6000_ReadGyros(&mpu6000_data); + error = true; + continue; + } - gyro_scaling = PIOS_MPU6000_GetScale(); - accel_scaling = PIOS_MPU6000_GetAccelScale(); + gyro_scaling = PIOS_MPU6000_GetScale(); + accel_scaling = PIOS_MPU6000_GetAccelScale(); - gyrosData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; - accelsData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; - } + gyrosData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; + accelsData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; + } #endif /* PIOS_INCLUDE_MPU6000 */ - break; - default: - PIOS_DEBUG_Assert(0); - } + break; + default: + PIOS_DEBUG_Assert(0); + } - // Scale the accels - float accels[3] = {(float) accel_accum[0] / accel_samples, - (float) accel_accum[1] / accel_samples, - (float) accel_accum[2] / accel_samples}; - float accels_out[3] = {accels[0] * accel_scaling * accel_scale[0] - accel_bias[0], - accels[1] * accel_scaling * accel_scale[1] - accel_bias[1], - accels[2] * accel_scaling * accel_scale[2] - accel_bias[2]}; - if (rotate) { - rot_mult(R, accels_out, accels); - accelsData.x = accels[0]; - accelsData.y = accels[1]; - accelsData.z = accels[2]; - } else { - accelsData.x = accels_out[0]; - accelsData.y = accels_out[1]; - accelsData.z = accels_out[2]; - } - AccelsSet(&accelsData); + // Scale the accels + float accels[3] = { (float)accel_accum[0] / accel_samples, + (float)accel_accum[1] / accel_samples, + (float)accel_accum[2] / accel_samples }; + float accels_out[3] = { accels[0] * accel_scaling * accel_scale[0] - accel_bias[0], + accels[1] * accel_scaling * accel_scale[1] - accel_bias[1], + accels[2] * accel_scaling * accel_scale[2] - accel_bias[2] }; + if (rotate) { + rot_mult(R, accels_out, accels); + accelsData.x = accels[0]; + accelsData.y = accels[1]; + accelsData.z = accels[2]; + } else { + accelsData.x = accels_out[0]; + accelsData.y = accels_out[1]; + accelsData.z = accels_out[2]; + } + AccelsSet(&accelsData); - // Scale the gyros - float gyros[3] = {(float) gyro_accum[0] / gyro_samples, - (float) gyro_accum[1] / gyro_samples, - (float) gyro_accum[2] / gyro_samples}; - float gyros_out[3] = {gyros[0] * gyro_scaling * gyro_scale[0] - gyro_staticbias[0], - gyros[1] * gyro_scaling * gyro_scale[1] - gyro_staticbias[1], - gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2]}; - if (rotate) { - rot_mult(R, gyros_out, gyros); - gyrosData.x = gyros[0]; - gyrosData.y = gyros[1]; - gyrosData.z = gyros[2]; - } else { - gyrosData.x = gyros_out[0]; - gyrosData.y = gyros_out[1]; - gyrosData.z = gyros_out[2]; - } - - if (bias_correct_gyro) { - // Apply bias correction to the gyros from the state estimator - GyrosBiasData gyrosBias; - GyrosBiasGet(&gyrosBias); - gyrosData.x -= gyrosBias.x; - gyrosData.y -= gyrosBias.y; - gyrosData.z -= gyrosBias.z; - } - GyrosSet(&gyrosData); - - // Because most crafts wont get enough information from gravity to zero yaw gyro, we try - // and make it average zero (weakly) + // Scale the gyros + float gyros[3] = { (float)gyro_accum[0] / gyro_samples, + (float)gyro_accum[1] / gyro_samples, + (float)gyro_accum[2] / gyro_samples }; + float gyros_out[3] = { gyros[0] * gyro_scaling * gyro_scale[0] - gyro_staticbias[0], + gyros[1] * gyro_scaling * gyro_scale[1] - gyro_staticbias[1], + gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2] }; + if (rotate) { + rot_mult(R, gyros_out, gyros); + gyrosData.x = gyros[0]; + gyrosData.y = gyros[1]; + gyrosData.z = gyros[2]; + } else { + gyrosData.x = gyros_out[0]; + gyrosData.y = gyros_out[1]; + gyrosData.z = gyros_out[2]; + } + + if (bias_correct_gyro) { + // Apply bias correction to the gyros from the state estimator + GyrosBiasData gyrosBias; + GyrosBiasGet(&gyrosBias); + gyrosData.x -= gyrosBias.x; + gyrosData.y -= gyrosBias.y; + gyrosData.z -= gyrosBias.z; + } + GyrosSet(&gyrosData); + + // Because most crafts wont get enough information from gravity to zero yaw gyro, we try + // and make it average zero (weakly) #if defined(PIOS_INCLUDE_HMC5883) - MagnetometerData mag; - if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { - int16_t values[3]; - PIOS_HMC5883_ReadMag(values); - float mags[3] = {(float) values[1] * mag_scale[0] - mag_bias[0], - (float) values[0] * mag_scale[1] - mag_bias[1], - -(float) values[2] * mag_scale[2] - mag_bias[2]}; - if (rotate) { - float mag_out[3]; - rot_mult(R, mags, mag_out); - mag.x = mag_out[0]; - mag.y = mag_out[1]; - mag.z = mag_out[2]; - } else { - mag.x = mags[0]; - mag.y = mags[1]; - mag.z = mags[2]; - } - - // Correct for mag bias and update if the rate is non zero - if(cal.MagBiasNullingRate > 0) - magOffsetEstimation(&mag); + MagnetometerData mag; + if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { + int16_t values[3]; + PIOS_HMC5883_ReadMag(values); + float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0], + (float)values[0] * mag_scale[1] - mag_bias[1], + -(float)values[2] * mag_scale[2] - mag_bias[2] }; + if (rotate) { + float mag_out[3]; + rot_mult(R, mags, mag_out); + mag.x = mag_out[0]; + mag.y = mag_out[1]; + mag.z = mag_out[2]; + } else { + mag.x = mags[0]; + mag.y = mags[1]; + mag.z = mags[2]; + } - MagnetometerSet(&mag); - mag_update_time = PIOS_DELAY_GetRaw(); - } -#endif + // Correct for mag bias and update if the rate is non zero + if (cal.MagBiasNullingRate > 0) { + magOffsetEstimation(&mag); + } - PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); + MagnetometerSet(&mag); + mag_update_time = PIOS_DELAY_GetRaw(); + } +#endif /* if defined(PIOS_INCLUDE_HMC5883) */ - lastSysTime = xTaskGetTickCount(); - } + PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); + + lastSysTime = xTaskGetTickCount(); + } } /** * Perform an update of the @ref MagBias based on - * Magnetometer Offset Cancellation: Theory and Implementation, + * Magnetometer Offset Cancellation: Theory and Implementation, * revisited William Premerlani, October 14, 2011 */ static void magOffsetEstimation(MagnetometerData *mag) { #if 0 - // Constants, to possibly go into a UAVO - static const float MIN_NORM_DIFFERENCE = 50; + // Constants, to possibly go into a UAVO + static const float MIN_NORM_DIFFERENCE = 50; - static float B2[3] = {0, 0, 0}; + static float B2[3] = { 0, 0, 0 }; - MagBiasData magBias; - MagBiasGet(&magBias); + MagBiasData magBias; + MagBiasGet(&magBias); - // Remove the current estimate of the bias - mag->x -= magBias.x; - mag->y -= magBias.y; - mag->z -= magBias.z; + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; - // First call - if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { - B2[0] = mag->x; - B2[1] = mag->y; - B2[2] = mag->z; - return; - } + // First call + if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { + B2[0] = mag->x; + B2[1] = mag->y; + B2[2] = mag->z; + return; + } - float B1[3] = {mag->x, mag->y, mag->z}; - float norm_diff = sqrtf(powf(B2[0] - B1[0],2) + powf(B2[1] - B1[1],2) + powf(B2[2] - B1[2],2)); - if (norm_diff > MIN_NORM_DIFFERENCE) { - float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]); - float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]); - float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; - float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale}; + float B1[3] = { mag->x, mag->y, mag->z }; + float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2)); + if (norm_diff > MIN_NORM_DIFFERENCE) { + float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]); + float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]); + float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; + float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale }; - magBias.x += b_error[0]; - magBias.y += b_error[1]; - magBias.z += b_error[2]; + magBias.x += b_error[0]; + magBias.y += b_error[1]; + magBias.z += b_error[2]; - MagBiasSet(&magBias); + MagBiasSet(&magBias); - // Store this value to compare against next update - B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; - } -#else - MagBiasData magBias; - MagBiasGet(&magBias); - - // Remove the current estimate of the bias - mag->x -= magBias.x; - mag->y -= magBias.y; - mag->z -= magBias.z; - - HomeLocationData homeLocation; - HomeLocationGet(&homeLocation); - - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - - const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]); - const float Rz = homeLocation.Be[2]; - - const float rate = cal.MagBiasNullingRate; - float Rot[3][3]; - float B_e[3]; - float xy[2]; - float delta[3]; - - // Get the rotation matrix - Quaternion2R(&attitude.q1, R); - - // Rotate the mag into the NED frame - B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z; - B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z; - B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z; - - float cy = cosf(DEG2RAD(attitude.Yaw)); - float sy = sinf(DEG2RAD(attitude.Yaw)); - - xy[0] = cy * B_e[0] + sy * B_e[1]; - xy[1] = -sy * B_e[0] + cy * B_e[1]; - - float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]); - - delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); - delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); - delta[2] = -rate * (Rz - B_e[2]); - - if (!isnan(delta[0]) && !isinf(delta[0]) && - !isnan(delta[1]) && !isinf(delta[1]) && - !isnan(delta[2]) && !isinf(delta[2])) { - magBias.x += delta[0]; - magBias.y += delta[1]; - magBias.z += delta[2]; - MagBiasSet(&magBias); - } -#endif + // Store this value to compare against next update + B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; + } +#else /* if 0 */ + MagBiasData magBias; + MagBiasGet(&magBias); + + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; + + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); + + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + + const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]); + const float Rz = homeLocation.Be[2]; + + const float rate = cal.MagBiasNullingRate; + float Rot[3][3]; + float B_e[3]; + float xy[2]; + float delta[3]; + + // Get the rotation matrix + Quaternion2R(&attitude.q1, R); + + // Rotate the mag into the NED frame + B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z; + B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z; + B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z; + + float cy = cosf(DEG2RAD(attitude.Yaw)); + float sy = sinf(DEG2RAD(attitude.Yaw)); + + xy[0] = cy * B_e[0] + sy * B_e[1]; + xy[1] = -sy * B_e[0] + cy * B_e[1]; + + float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]); + + delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); + delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); + delta[2] = -rate * (Rz - B_e[2]); + + if (!isnan(delta[0]) && !isinf(delta[0]) && + !isnan(delta[1]) && !isinf(delta[1]) && + !isnan(delta[2]) && !isinf(delta[2])) { + magBias.x += delta[0]; + magBias.y += delta[1]; + magBias.z += delta[2]; + MagBiasSet(&magBias); + } +#endif /* if 0 */ } /** * Locally cache some variables from the AtttitudeSettings object */ -static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent * objEv) { - RevoCalibrationGet(&cal); - - mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X]; - mag_bias[1] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Y]; - mag_bias[2] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Z]; - mag_scale[0] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_X]; - mag_scale[1] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Y]; - mag_scale[2] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Z]; - accel_bias[0] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_X]; - accel_bias[1] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Y]; - accel_bias[2] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Z]; - accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X]; - accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y]; - accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z]; - gyro_staticbias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X]; - gyro_staticbias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y]; - gyro_staticbias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z]; - gyro_scale[0] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_X]; - gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y]; - gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z]; - - // Zero out any adaptive tracking - MagBiasData magBias; - MagBiasGet(&magBias); - magBias.x = 0; - magBias.y = 0; - magBias.z = 0; - MagBiasSet(&magBias); - +static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) +{ + RevoCalibrationGet(&cal); - AttitudeSettingsData attitudeSettings; - AttitudeSettingsGet(&attitudeSettings); - bias_correct_gyro = (cal.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE); + mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X]; + mag_bias[1] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Y]; + mag_bias[2] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Z]; + mag_scale[0] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_X]; + mag_scale[1] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Y]; + mag_scale[2] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Z]; + accel_bias[0] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_X]; + accel_bias[1] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Y]; + accel_bias[2] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Z]; + accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X]; + accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y]; + accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z]; + gyro_staticbias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X]; + gyro_staticbias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y]; + gyro_staticbias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z]; + gyro_scale[0] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_X]; + gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y]; + gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z]; - // Indicates not to expend cycles on rotation - if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && - attitudeSettings.BoardRotation[2] == 0) { - rotate = 0; - } else { - float rotationQuat[4]; - const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL], - attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH], - attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]}; - RPY2Quaternion(rpy, rotationQuat); - Quaternion2R(rotationQuat, R); - rotate = 1; - } + // Zero out any adaptive tracking + MagBiasData magBias; + MagBiasGet(&magBias); + magBias.x = 0; + magBias.y = 0; + magBias.z = 0; + MagBiasSet(&magBias); + + AttitudeSettingsData attitudeSettings; + AttitudeSettingsGet(&attitudeSettings); + bias_correct_gyro = (cal.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE); + + // Indicates not to expend cycles on rotation + if (attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && + attitudeSettings.BoardRotation[2] == 0) { + rotate = 0; + } else { + float rotationQuat[4]; + const float rpy[3] = { attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL], + attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH], + attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] }; + RPY2Quaternion(rpy, rotationQuat); + Quaternion2R(rotationQuat, R); + rotate = 1; + } } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/Sensors/simulated/inc/sensors.h b/flight/modules/Sensors/simulated/inc/sensors.h index 16e24daab..6abdbdade 100644 --- a/flight/modules/Sensors/simulated/inc/sensors.h +++ b/flight/modules/Sensors/simulated/inc/sensors.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup Sensors Sensors Module - * @{ + * @{ * * @file attitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. diff --git a/flight/modules/Sensors/simulated/sensors.c b/flight/modules/Sensors/simulated/sensors.c index 929051b67..6f973f2f5 100644 --- a/flight/modules/Sensors/simulated/sensors.c +++ b/flight/modules/Sensors/simulated/sensors.c @@ -3,7 +3,7 @@ * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup Sensors - * @brief Acquires sensor data + * @brief Acquires sensor data * Specifically updates the the @ref Gyros, @ref Accels, and @ref Magnetometer objects * @{ * @@ -73,10 +73,10 @@ // Private constants #define STACK_SIZE_BYTES 1540 -#define TASK_PRIORITY (tskIDLE_PRIORITY+3) -#define SENSOR_PERIOD 2 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) +#define SENSOR_PERIOD 2 -#define F_PI 3.14159265358979323846f +#define F_PI 3.14159265358979323846f #define PI_MOD(x) (fmod(x + F_PI, F_PI * 2) - F_PI) // Private types @@ -96,7 +96,7 @@ static float accel_bias[3]; static float rand_gauss(); -enum sensor_sim_type {CONSTANT, MODEL_AGNOSTIC, MODEL_QUADCOPTER, MODEL_AIRPLANE} sensor_sim_type; +enum sensor_sim_type { CONSTANT, MODEL_AGNOSTIC, MODEL_QUADCOPTER, MODEL_AIRPLANE } sensor_sim_type; #define GRAV 9.81 /** @@ -105,38 +105,37 @@ enum sensor_sim_type {CONSTANT, MODEL_AGNOSTIC, MODEL_QUADCOPTER, MODEL_AIRPLANE */ int32_t SensorsInitialize(void) { + accel_bias[0] = rand_gauss() / 10; + accel_bias[1] = rand_gauss() / 10; + accel_bias[2] = rand_gauss() / 10; - accel_bias[0] = rand_gauss() / 10; - accel_bias[1] = rand_gauss() / 10; - accel_bias[2] = rand_gauss() / 10; + AccelsInitialize(); + AttitudeSimulatedInitialize(); + BaroAltitudeInitialize(); + AirspeedSensorInitialize(); + GyrosInitialize(); + GyrosBiasInitialize(); + GPSPositionInitialize(); + GPSVelocityInitialize(); + MagnetometerInitialize(); + MagBiasInitialize(); + RevoCalibrationInitialize(); - AccelsInitialize(); - AttitudeSimulatedInitialize(); - BaroAltitudeInitialize(); - AirspeedSensorInitialize(); - GyrosInitialize(); - GyrosBiasInitialize(); - GPSPositionInitialize(); - GPSVelocityInitialize(); - MagnetometerInitialize(); - MagBiasInitialize(); - RevoCalibrationInitialize(); - - return 0; + return 0; } /** * Start the task. Expects all objects to be initialized by this point. - *pick \returns 0 on success or -1 if initialisation failed + * pick \returns 0 on success or -1 if initialisation failed */ int32_t SensorsStart(void) { - // Start main task - xTaskCreate(SensorsTask, (signed char *)"Sensors", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &sensorsTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS); + // Start main task + xTaskCreate(SensorsTask, (signed char *)"Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS); - return 0; + return 0; } MODULE_INITCALL(SensorsInitialize, SensorsStart) @@ -147,414 +146,419 @@ MODULE_INITCALL(SensorsInitialize, SensorsStart) int sensors_count; static void SensorsTask(__attribute__((unused)) void *parameters) { - portTickType lastSysTime; + portTickType lastSysTime; - AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); - -// HomeLocationData homeLocation; -// HomeLocationGet(&homeLocation); -// homeLocation.Latitude = 0; -// homeLocation.Longitude = 0; -// homeLocation.Altitude = 0; -// homeLocation.Be[0] = 26000; -// homeLocation.Be[1] = 400; -// homeLocation.Be[2] = 40000; -// homeLocation.Set = HOMELOCATION_SET_TRUE; -// HomeLocationSet(&homeLocation); + AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); + +// HomeLocationData homeLocation; +// HomeLocationGet(&homeLocation); +// homeLocation.Latitude = 0; +// homeLocation.Longitude = 0; +// homeLocation.Altitude = 0; +// homeLocation.Be[0] = 26000; +// homeLocation.Be[1] = 400; +// homeLocation.Be[2] = 40000; +// homeLocation.Set = HOMELOCATION_SET_TRUE; +// HomeLocationSet(&homeLocation); - // Main task loop - lastSysTime = xTaskGetTickCount(); - uint32_t last_time = PIOS_DELAY_GetRaw(); - while (1) { - PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); + // Main task loop + lastSysTime = xTaskGetTickCount(); + uint32_t last_time = PIOS_DELAY_GetRaw(); + while (1) { + PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); - SystemSettingsData systemSettings; - SystemSettingsGet(&systemSettings); + SystemSettingsData systemSettings; + SystemSettingsGet(&systemSettings); - switch(systemSettings.AirframeType) { - case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING: - case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON: - case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL: - sensor_sim_type = MODEL_AIRPLANE; - break; - case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX: - case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP: - case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL: - case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA: - case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO: - sensor_sim_type = MODEL_QUADCOPTER; - break; - default: - sensor_sim_type = MODEL_AGNOSTIC; - } - - static int i; - i++; - if (i % 5000 == 0) { - //float dT = PIOS_DELAY_DiffuS(last_time) / 10.0e6; - //fprintf(stderr, "Sensor relative timing: %f\n", dT); - last_time = PIOS_DELAY_GetRaw(); - } - - sensors_count++; + switch (systemSettings.AirframeType) { + case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING: + case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON: + case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL: + sensor_sim_type = MODEL_AIRPLANE; + break; + case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX: + case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP: + case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL: + case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO: + sensor_sim_type = MODEL_QUADCOPTER; + break; + default: + sensor_sim_type = MODEL_AGNOSTIC; + } - switch(sensor_sim_type) { - case CONSTANT: - simulateConstant(); - break; - case MODEL_AGNOSTIC: - simulateModelAgnostic(); - break; - case MODEL_QUADCOPTER: - simulateModelQuadcopter(); - break; - case MODEL_AIRPLANE: - simulateModelAirplane(); - } + static int i; + i++; + if (i % 5000 == 0) { + // float dT = PIOS_DELAY_DiffuS(last_time) / 10.0e6; + // fprintf(stderr, "Sensor relative timing: %f\n", dT); + last_time = PIOS_DELAY_GetRaw(); + } - vTaskDelay(2 / portTICK_RATE_MS); + sensors_count++; - } + switch (sensor_sim_type) { + case CONSTANT: + simulateConstant(); + break; + case MODEL_AGNOSTIC: + simulateModelAgnostic(); + break; + case MODEL_QUADCOPTER: + simulateModelQuadcopter(); + break; + case MODEL_AIRPLANE: + simulateModelAirplane(); + } + + vTaskDelay(2 / portTICK_RATE_MS); + } } static void simulateConstant() { - AccelsData accelsData; // Skip get as we set all the fields - accelsData.x = 0; - accelsData.y = 0; - accelsData.z = -GRAV; - accelsData.temperature = 0; - AccelsSet(&accelsData); + AccelsData accelsData; // Skip get as we set all the fields - GyrosData gyrosData; // Skip get as we set all the fields - gyrosData.x = 0; - gyrosData.y = 0; - gyrosData.z = 0; + accelsData.x = 0; + accelsData.y = 0; + accelsData.z = -GRAV; + accelsData.temperature = 0; + AccelsSet(&accelsData); - // Apply bias correction to the gyros - GyrosBiasData gyrosBias; - GyrosBiasGet(&gyrosBias); - gyrosData.x += gyrosBias.x; - gyrosData.y += gyrosBias.y; - gyrosData.z += gyrosBias.z; + GyrosData gyrosData; // Skip get as we set all the fields + gyrosData.x = 0; + gyrosData.y = 0; + gyrosData.z = 0; - GyrosSet(&gyrosData); + // Apply bias correction to the gyros + GyrosBiasData gyrosBias; + GyrosBiasGet(&gyrosBias); + gyrosData.x += gyrosBias.x; + gyrosData.y += gyrosBias.y; + gyrosData.z += gyrosBias.z; - BaroAltitudeData baroAltitude; - BaroAltitudeGet(&baroAltitude); - baroAltitude.Altitude = 1; - BaroAltitudeSet(&baroAltitude); + GyrosSet(&gyrosData); - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - gpsPosition.Latitude = 0; - gpsPosition.Longitude = 0; - gpsPosition.Altitude = 0; - GPSPositionSet(&gpsPosition); + BaroAltitudeData baroAltitude; + BaroAltitudeGet(&baroAltitude); + baroAltitude.Altitude = 1; + BaroAltitudeSet(&baroAltitude); - // Because most crafts wont get enough information from gravity to zero yaw gyro, we try - // and make it average zero (weakly) - MagnetometerData mag; - mag.x = 400; - mag.y = 0; - mag.z = 800; - MagnetometerSet(&mag); + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + gpsPosition.Latitude = 0; + gpsPosition.Longitude = 0; + gpsPosition.Altitude = 0; + GPSPositionSet(&gpsPosition); + + // Because most crafts wont get enough information from gravity to zero yaw gyro, we try + // and make it average zero (weakly) + MagnetometerData mag; + mag.x = 400; + mag.y = 0; + mag.z = 800; + MagnetometerSet(&mag); } static void simulateModelAgnostic() { - float Rbe[3][3]; - float q[4]; + float Rbe[3][3]; + float q[4]; - // Simulate accels based on current attitude - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - q[0] = attitudeActual.q1; - q[1] = attitudeActual.q2; - q[2] = attitudeActual.q3; - q[3] = attitudeActual.q4; - Quaternion2R(q,Rbe); + // Simulate accels based on current attitude + AttitudeActualData attitudeActual; - AccelsData accelsData; // Skip get as we set all the fields - accelsData.x = -GRAV * Rbe[0][2]; - accelsData.y = -GRAV * Rbe[1][2]; - accelsData.z = -GRAV * Rbe[2][2]; - accelsData.temperature = 30; - AccelsSet(&accelsData); + AttitudeActualGet(&attitudeActual); + q[0] = attitudeActual.q1; + q[1] = attitudeActual.q2; + q[2] = attitudeActual.q3; + q[3] = attitudeActual.q4; + Quaternion2R(q, Rbe); - RateDesiredData rateDesired; - RateDesiredGet(&rateDesired); + AccelsData accelsData; // Skip get as we set all the fields + accelsData.x = -GRAV * Rbe[0][2]; + accelsData.y = -GRAV * Rbe[1][2]; + accelsData.z = -GRAV * Rbe[2][2]; + accelsData.temperature = 30; + AccelsSet(&accelsData); - GyrosData gyrosData; // Skip get as we set all the fields - gyrosData.x = rateDesired.Roll + rand_gauss(); - gyrosData.y = rateDesired.Pitch + rand_gauss(); - gyrosData.z = rateDesired.Yaw + rand_gauss(); + RateDesiredData rateDesired; + RateDesiredGet(&rateDesired); - // Apply bias correction to the gyros - GyrosBiasData gyrosBias; - GyrosBiasGet(&gyrosBias); - gyrosData.x += gyrosBias.x; - gyrosData.y += gyrosBias.y; - gyrosData.z += gyrosBias.z; + GyrosData gyrosData; // Skip get as we set all the fields + gyrosData.x = rateDesired.Roll + rand_gauss(); + gyrosData.y = rateDesired.Pitch + rand_gauss(); + gyrosData.z = rateDesired.Yaw + rand_gauss(); - GyrosSet(&gyrosData); + // Apply bias correction to the gyros + GyrosBiasData gyrosBias; + GyrosBiasGet(&gyrosBias); + gyrosData.x += gyrosBias.x; + gyrosData.y += gyrosBias.y; + gyrosData.z += gyrosBias.z; - BaroAltitudeData baroAltitude; - BaroAltitudeGet(&baroAltitude); - baroAltitude.Altitude = 1; - BaroAltitudeSet(&baroAltitude); + GyrosSet(&gyrosData); - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - gpsPosition.Latitude = 0; - gpsPosition.Longitude = 0; - gpsPosition.Altitude = 0; - GPSPositionSet(&gpsPosition); + BaroAltitudeData baroAltitude; + BaroAltitudeGet(&baroAltitude); + baroAltitude.Altitude = 1; + BaroAltitudeSet(&baroAltitude); - // Because most crafts wont get enough information from gravity to zero yaw gyro, we try - // and make it average zero (weakly) - MagnetometerData mag; - mag.x = 400; - mag.y = 0; - mag.z = 800; - MagnetometerSet(&mag); + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + gpsPosition.Latitude = 0; + gpsPosition.Longitude = 0; + gpsPosition.Altitude = 0; + GPSPositionSet(&gpsPosition); + + // Because most crafts wont get enough information from gravity to zero yaw gyro, we try + // and make it average zero (weakly) + MagnetometerData mag; + mag.x = 400; + mag.y = 0; + mag.z = 800; + MagnetometerSet(&mag); } -float thrustToDegs = 50; +float thrustToDegs = 50; bool overideAttitude = false; static void simulateModelQuadcopter() { - static double pos[3] = {0,0,0}; - static double vel[3] = {0,0,0}; - static double ned_accel[3] = {0,0,0}; - static float q[4] = {1,0,0,0}; - static float rpy[3] = {0,0,0}; // Low pass filtered actuator - static float baro_offset = 0.0f; - float Rbe[3][3]; - - const float ACTUATOR_ALPHA = 0.8; - const float MAX_THRUST = GRAV * 2; - const float K_FRICTION = 1; - const float GPS_PERIOD = 0.1; - const float MAG_PERIOD = 1.0 / 75.0; - const float BARO_PERIOD = 1.0 / 20.0; - - static uint32_t last_time; - - float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6); - if(dT < 1e-3) - dT = 2e-3; - last_time = PIOS_DELAY_GetRaw(); - - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - ActuatorDesiredData actuatorDesired; - ActuatorDesiredGet(&actuatorDesired); + static double pos[3] = { 0, 0, 0 }; + static double vel[3] = { 0, 0, 0 }; + static double ned_accel[3] = { 0, 0, 0 }; + static float q[4] = { 1, 0, 0, 0 }; + static float rpy[3] = { 0, 0, 0 }; // Low pass filtered actuator + static float baro_offset = 0.0f; + float Rbe[3][3]; - float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0; - if (thrust < 0) - thrust = 0; - - if (thrust != thrust) - thrust = 0; - -// float control_scaling = thrust * thrustToDegs; -// // In rad/s -// rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; -// rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; -// rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; -// -// GyrosData gyrosData; // Skip get as we set all the fields -// gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss(); -// gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss(); -// gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss(); - - RateDesiredData rateDesired; - RateDesiredGet(&rateDesired); - - rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; - rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; - rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; - - GyrosData gyrosData; // Skip get as we set all the fields - gyrosData.x = rpy[0] + rand_gauss(); - gyrosData.y = rpy[1] + rand_gauss(); - gyrosData.z = rpy[2] + rand_gauss(); - GyrosSet(&gyrosData); - - // Predict the attitude forward in time - float qdot[4]; - qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * M_PI / 180 / 2; - qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * M_PI / 180 / 2; - qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * M_PI / 180 / 2; - qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * M_PI / 180 / 2; - - // Take a time step - q[0] = q[0] + qdot[0]; - q[1] = q[1] + qdot[1]; - q[2] = q[2] + qdot[2]; - q[3] = q[3] + qdot[3]; - - float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); - q[0] = q[0] / qmag; - q[1] = q[1] / qmag; - q[2] = q[2] / qmag; - q[3] = q[3] / qmag; - - if(overideAttitude){ - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - attitudeActual.q1 = q[0]; - attitudeActual.q2 = q[1]; - attitudeActual.q3 = q[2]; - attitudeActual.q4 = q[3]; - AttitudeActualSet(&attitudeActual); - } - - static float wind[3] = {0,0,0}; - wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0; - wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0; - wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0; - - Quaternion2R(q,Rbe); - // Make thrust negative as down is positive - ned_accel[0] = -thrust * Rbe[2][0]; - ned_accel[1] = -thrust * Rbe[2][1]; - // Gravity causes acceleration of 9.81 in the down direction - ned_accel[2] = -thrust * Rbe[2][2] + GRAV; - - // Apply acceleration based on velocity - ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]); - ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]); - ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]); + const float ACTUATOR_ALPHA = 0.8; + const float MAX_THRUST = GRAV * 2; + const float K_FRICTION = 1; + const float GPS_PERIOD = 0.1; + const float MAG_PERIOD = 1.0 / 75.0; + const float BARO_PERIOD = 1.0 / 20.0; - // Predict the velocity forward in time - vel[0] = vel[0] + ned_accel[0] * dT; - vel[1] = vel[1] + ned_accel[1] * dT; - vel[2] = vel[2] + ned_accel[2] * dT; + static uint32_t last_time; - // Predict the position forward in time - pos[0] = pos[0] + vel[0] * dT; - pos[1] = pos[1] + vel[1] * dT; - pos[2] = pos[2] + vel[2] * dT; + float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6); - // Simulate hitting ground - if(pos[2] > 0) { - pos[2] = 0; - vel[2] = 0; - ned_accel[2] = 0; - } - - // Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0) - ned_accel[2] -= 9.81; - - // Transform the accels back in to body frame - AccelsData accelsData; // Skip get as we set all the fields - accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0]; - accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1]; - accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2]; - accelsData.temperature = 30; - AccelsSet(&accelsData); + if (dT < 1e-3) { + dT = 2e-3; + } + last_time = PIOS_DELAY_GetRaw(); - if(baro_offset == 0) { - // Hacky initialization - baro_offset = 50;// * rand_gauss(); - } else { - // Very small drift process - baro_offset += rand_gauss() / 100; - } - // Update baro periodically - static uint32_t last_baro_time = 0; - if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) { - BaroAltitudeData baroAltitude; - BaroAltitudeGet(&baroAltitude); - baroAltitude.Altitude = -pos[2] + baro_offset; - BaroAltitudeSet(&baroAltitude); - last_baro_time = PIOS_DELAY_GetRaw(); - } - - HomeLocationData homeLocation; - HomeLocationGet(&homeLocation); + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + ActuatorDesiredData actuatorDesired; + ActuatorDesiredGet(&actuatorDesired); - static float gps_vel_drift[3] = {0,0,0}; - gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0; - gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0; - gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0; + float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0; + if (thrust < 0) { + thrust = 0; + } - // Update GPS periodically - static uint32_t last_gps_time = 0; - if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) { - // Use double precision here as simulating what GPS produces - double T[3]; - T[0] = homeLocation.Altitude+6.378137E6f * M_PI / 180.0; - T[1] = cos(homeLocation.Latitude / 10e6 * M_PI / 180.0f)*(homeLocation.Altitude+6.378137E6) * M_PI / 180.0; - T[2] = -1.0; - - static float gps_drift[3] = {0,0,0}; - gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0; - gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0; - gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0; + if (thrust != thrust) { + thrust = 0; + } - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6); - gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6); - gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]); - gpsPosition.Groundspeed = sqrt(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2)); - gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]); - gpsPosition.Satellites = 7; - gpsPosition.PDOP = 1; - GPSPositionSet(&gpsPosition); - last_gps_time = PIOS_DELAY_GetRaw(); - } - - // Update GPS Velocity measurements - static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond - if(PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) { - GPSVelocityData gpsVelocity; - GPSVelocityGet(&gpsVelocity); - gpsVelocity.North = vel[0] + gps_vel_drift[0]; - gpsVelocity.East = vel[1] + gps_vel_drift[1]; - gpsVelocity.Down = vel[2] + gps_vel_drift[2]; - GPSVelocitySet(&gpsVelocity); - last_gps_vel_time = PIOS_DELAY_GetRaw(); - } +// float control_scaling = thrust * thrustToDegs; +//// In rad/s +// rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; +// rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; +// rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; +// +// GyrosData gyrosData; // Skip get as we set all the fields +// gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss(); +// gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss(); +// gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss(); - // Update mag periodically - static uint32_t last_mag_time = 0; - if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { - MagnetometerData mag; - mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; - mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; - mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; + RateDesiredData rateDesired; + RateDesiredGet(&rateDesired); - // Run the offset compensation algorithm from the firmware - magOffsetEstimation(&mag); + rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; + rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; + rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; - MagnetometerSet(&mag); - last_mag_time = PIOS_DELAY_GetRaw(); - } - - AttitudeSimulatedData attitudeSimulated; - AttitudeSimulatedGet(&attitudeSimulated); - attitudeSimulated.q1 = q[0]; - attitudeSimulated.q2 = q[1]; - attitudeSimulated.q3 = q[2]; - attitudeSimulated.q4 = q[3]; - Quaternion2RPY(q,&attitudeSimulated.Roll); - attitudeSimulated.Position[0] = pos[0]; - attitudeSimulated.Position[1] = pos[1]; - attitudeSimulated.Position[2] = pos[2]; - attitudeSimulated.Velocity[0] = vel[0]; - attitudeSimulated.Velocity[1] = vel[1]; - attitudeSimulated.Velocity[2] = vel[2]; - AttitudeSimulatedSet(&attitudeSimulated); + GyrosData gyrosData; // Skip get as we set all the fields + gyrosData.x = rpy[0] + rand_gauss(); + gyrosData.y = rpy[1] + rand_gauss(); + gyrosData.z = rpy[2] + rand_gauss(); + GyrosSet(&gyrosData); + + // Predict the attitude forward in time + float qdot[4]; + qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * M_PI / 180 / 2; + qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * M_PI / 180 / 2; + qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * M_PI / 180 / 2; + qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * M_PI / 180 / 2; + + // Take a time step + q[0] = q[0] + qdot[0]; + q[1] = q[1] + qdot[1]; + q[2] = q[2] + qdot[2]; + q[3] = q[3] + qdot[3]; + + float qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + q[0] = q[0] / qmag; + q[1] = q[1] / qmag; + q[2] = q[2] / qmag; + q[3] = q[3] / qmag; + + if (overideAttitude) { + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); + attitudeActual.q1 = q[0]; + attitudeActual.q2 = q[1]; + attitudeActual.q3 = q[2]; + attitudeActual.q4 = q[3]; + AttitudeActualSet(&attitudeActual); + } + + static float wind[3] = { 0, 0, 0 }; + wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0; + wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0; + wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0; + + Quaternion2R(q, Rbe); + // Make thrust negative as down is positive + ned_accel[0] = -thrust * Rbe[2][0]; + ned_accel[1] = -thrust * Rbe[2][1]; + // Gravity causes acceleration of 9.81 in the down direction + ned_accel[2] = -thrust * Rbe[2][2] + GRAV; + + // Apply acceleration based on velocity + ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]); + ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]); + ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]); + + // Predict the velocity forward in time + vel[0] = vel[0] + ned_accel[0] * dT; + vel[1] = vel[1] + ned_accel[1] * dT; + vel[2] = vel[2] + ned_accel[2] * dT; + + // Predict the position forward in time + pos[0] = pos[0] + vel[0] * dT; + pos[1] = pos[1] + vel[1] * dT; + pos[2] = pos[2] + vel[2] * dT; + + // Simulate hitting ground + if (pos[2] > 0) { + pos[2] = 0; + vel[2] = 0; + ned_accel[2] = 0; + } + + // Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0) + ned_accel[2] -= 9.81; + + // Transform the accels back in to body frame + AccelsData accelsData; // Skip get as we set all the fields + accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0]; + accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1]; + accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2]; + accelsData.temperature = 30; + AccelsSet(&accelsData); + + if (baro_offset == 0) { + // Hacky initialization + baro_offset = 50; // * rand_gauss(); + } else { + // Very small drift process + baro_offset += rand_gauss() / 100; + } + // Update baro periodically + static uint32_t last_baro_time = 0; + if (PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) { + BaroAltitudeData baroAltitude; + BaroAltitudeGet(&baroAltitude); + baroAltitude.Altitude = -pos[2] + baro_offset; + BaroAltitudeSet(&baroAltitude); + last_baro_time = PIOS_DELAY_GetRaw(); + } + + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); + + static float gps_vel_drift[3] = { 0, 0, 0 }; + gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0; + gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0; + gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0; + + // Update GPS periodically + static uint32_t last_gps_time = 0; + if (PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) { + // Use double precision here as simulating what GPS produces + double T[3]; + T[0] = homeLocation.Altitude + 6.378137E6f * M_PI / 180.0; + T[1] = cos(homeLocation.Latitude / 10e6 * M_PI / 180.0f) * (homeLocation.Altitude + 6.378137E6) * M_PI / 180.0; + T[2] = -1.0; + + static float gps_drift[3] = { 0, 0, 0 }; + gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0; + gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0; + gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0; + + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6); + gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1]) / T[1] * 10.0e6); + gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]); + gpsPosition.Groundspeed = sqrt(pow(vel[0] + gps_vel_drift[0], 2) + pow(vel[1] + gps_vel_drift[1], 2)); + gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1], vel[0] + gps_vel_drift[0]); + gpsPosition.Satellites = 7; + gpsPosition.PDOP = 1; + GPSPositionSet(&gpsPosition); + last_gps_time = PIOS_DELAY_GetRaw(); + } + + // Update GPS Velocity measurements + static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond + if (PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) { + GPSVelocityData gpsVelocity; + GPSVelocityGet(&gpsVelocity); + gpsVelocity.North = vel[0] + gps_vel_drift[0]; + gpsVelocity.East = vel[1] + gps_vel_drift[1]; + gpsVelocity.Down = vel[2] + gps_vel_drift[2]; + GPSVelocitySet(&gpsVelocity); + last_gps_vel_time = PIOS_DELAY_GetRaw(); + } + + // Update mag periodically + static uint32_t last_mag_time = 0; + if (PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { + MagnetometerData mag; + mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; + mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; + mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; + + // Run the offset compensation algorithm from the firmware + magOffsetEstimation(&mag); + + MagnetometerSet(&mag); + last_mag_time = PIOS_DELAY_GetRaw(); + } + + AttitudeSimulatedData attitudeSimulated; + AttitudeSimulatedGet(&attitudeSimulated); + attitudeSimulated.q1 = q[0]; + attitudeSimulated.q2 = q[1]; + attitudeSimulated.q3 = q[2]; + attitudeSimulated.q4 = q[3]; + Quaternion2RPY(q, &attitudeSimulated.Roll); + attitudeSimulated.Position[0] = pos[0]; + attitudeSimulated.Position[1] = pos[1]; + attitudeSimulated.Position[2] = pos[2]; + attitudeSimulated.Velocity[0] = vel[0]; + attitudeSimulated.Velocity[1] = vel[1]; + attitudeSimulated.Velocity[2] = vel[2]; + AttitudeSimulatedSet(&attitudeSimulated); } /** * This method performs a simple simulation of a quadcopter - * + * * It takes in the ActuatorDesired command to rotate the aircraft and performs * a simple kinetic model where the throttle increases the energy and drag decreases * it. Changing altitude moves energy from kinetic to potential. @@ -564,391 +568,396 @@ static void simulateModelQuadcopter() */ static void simulateModelAirplane() { - static double pos[3] = {0,0,0}; - static double vel[3] = {0,0,0}; - static double ned_accel[3] = {0,0,0}; - static float q[4] = {1,0,0,0}; - static float rpy[3] = {0,0,0}; // Low pass filtered actuator - static float baro_offset = 0.0f; - float Rbe[3][3]; - - const float LIFT_SPEED = 8; // (m/s) where achieve lift for zero pitch - const float ACTUATOR_ALPHA = 0.8; - const float MAX_THRUST = 9.81 * 2; - const float K_FRICTION = 0.2; - const float GPS_PERIOD = 0.1; - const float MAG_PERIOD = 1.0 / 75.0; - const float BARO_PERIOD = 1.0 / 20.0; - const float ROLL_HEADING_COUPLING = 0.1; // (deg/s) heading change per deg of roll - const float PITCH_THRUST_COUPLING = 0.2; // (m/s^2) of forward acceleration per deg of pitch - - static uint32_t last_time; - - float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6); - if(dT < 1e-3) - dT = 2e-3; - last_time = PIOS_DELAY_GetRaw(); - - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - ActuatorDesiredData actuatorDesired; - ActuatorDesiredGet(&actuatorDesired); - - float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0; - if (thrust < 0) - thrust = 0; - - if (thrust != thrust) - thrust = 0; - - // float control_scaling = thrust * thrustToDegs; - // // In rad/s - // rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; - // rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; - // rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; - // - // GyrosData gyrosData; // Skip get as we set all the fields - // gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss(); - // gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss(); - // gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss(); - - /**** 1. Update attitude ****/ - RateDesiredData rateDesired; - RateDesiredGet(&rateDesired); - - // Need to get roll angle for easy cross coupling - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - double roll = attitudeActual.Roll; - double pitch = attitudeActual.Pitch; + static double pos[3] = { 0, 0, 0 }; + static double vel[3] = { 0, 0, 0 }; + static double ned_accel[3] = { 0, 0, 0 }; + static float q[4] = { 1, 0, 0, 0 }; + static float rpy[3] = { 0, 0, 0 }; // Low pass filtered actuator + static float baro_offset = 0.0f; + float Rbe[3][3]; - rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; - rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; - rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; - rpy[2] += roll * ROLL_HEADING_COUPLING; - + const float LIFT_SPEED = 8; // (m/s) where achieve lift for zero pitch + const float ACTUATOR_ALPHA = 0.8; + const float MAX_THRUST = 9.81 * 2; + const float K_FRICTION = 0.2; + const float GPS_PERIOD = 0.1; + const float MAG_PERIOD = 1.0 / 75.0; + const float BARO_PERIOD = 1.0 / 20.0; + const float ROLL_HEADING_COUPLING = 0.1; // (deg/s) heading change per deg of roll + const float PITCH_THRUST_COUPLING = 0.2; // (m/s^2) of forward acceleration per deg of pitch - GyrosData gyrosData; // Skip get as we set all the fields - gyrosData.x = rpy[0] + rand_gauss(); - gyrosData.y = rpy[1] + rand_gauss(); - gyrosData.z = rpy[2] + rand_gauss(); - GyrosSet(&gyrosData); - - // Predict the attitude forward in time - float qdot[4]; - qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * M_PI / 180 / 2; - qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * M_PI / 180 / 2; - qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * M_PI / 180 / 2; - qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * M_PI / 180 / 2; - - // Take a time step - q[0] = q[0] + qdot[0]; - q[1] = q[1] + qdot[1]; - q[2] = q[2] + qdot[2]; - q[3] = q[3] + qdot[3]; - - float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); - q[0] = q[0] / qmag; - q[1] = q[1] / qmag; - q[2] = q[2] / qmag; - q[3] = q[3] / qmag; - - if(overideAttitude){ - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - attitudeActual.q1 = q[0]; - attitudeActual.q2 = q[1]; - attitudeActual.q3 = q[2]; - attitudeActual.q4 = q[3]; - AttitudeActualSet(&attitudeActual); - } - - /**** 2. Update position based on velocity ****/ - static float wind[3] = {0,0,0}; - wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0; - wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0; - wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0; - wind[0] = 0; - wind[1] = 0; - wind[2] = 0; - - // Rbe takes a vector from body to earth. If we take (1,0,0)^T through this and then dot with airspeed - // we get forward airspeed - Quaternion2R(q,Rbe); + static uint32_t last_time; - double airspeed[3] = {vel[0] - wind[0], vel[1] - wind[1], vel[2] - wind[2]}; - double forwardAirspeed = Rbe[0][0] * airspeed[0] + Rbe[0][1] * airspeed[1] + Rbe[0][2] * airspeed[2]; - double sidewaysAirspeed = Rbe[1][0] * airspeed[0] + Rbe[1][1] * airspeed[1] + Rbe[1][2] * airspeed[2]; - double downwardAirspeed = Rbe[2][0] * airspeed[0] + Rbe[2][1] * airspeed[1] + Rbe[2][2] * airspeed[2]; - - /* Compute aerodynamic forces in body referenced frame. Later use more sophisticated equations */ - /* TODO: This should become more accurate. Use the force equations to calculate lift from the */ - /* various surfaces based on AoA and airspeed. From that compute torques and forces. For later */ - double forces[3]; // X, Y, Z - forces[0] = thrust - pitch * PITCH_THRUST_COUPLING - forwardAirspeed * K_FRICTION; // Friction is applied in all directions in NED - forces[1] = 0 - sidewaysAirspeed * K_FRICTION * 100; // No side slip - forces[2] = GRAV * (forwardAirspeed - LIFT_SPEED) + downwardAirspeed * K_FRICTION * 100; // Stupidly simple, always have gravity lift when straight and level - - // Negate force[2] as NED defines down as possitive, aircraft convention is Z up is positive (?) - ned_accel[0] = forces[0] * Rbe[0][0] + forces[1] * Rbe[1][0] - forces[2] * Rbe[2][0]; - ned_accel[1] = forces[0] * Rbe[0][1] + forces[1] * Rbe[1][1] - forces[2] * Rbe[2][1]; - ned_accel[2] = forces[0] * Rbe[0][2] + forces[1] * Rbe[1][2] - forces[2] * Rbe[2][2]; - // Gravity causes acceleration of 9.81 in the down direction - ned_accel[2] += 9.81; + float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6); - // Apply acceleration based on velocity - ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]); - ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]); - ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]); - - // Predict the velocity forward in time - vel[0] = vel[0] + ned_accel[0] * dT; - vel[1] = vel[1] + ned_accel[1] * dT; - vel[2] = vel[2] + ned_accel[2] * dT; - - // Predict the position forward in time - pos[0] = pos[0] + vel[0] * dT; - pos[1] = pos[1] + vel[1] * dT; - pos[2] = pos[2] + vel[2] * dT; - - // Simulate hitting ground - if(pos[2] > 0) { - pos[2] = 0; - vel[2] = 0; - ned_accel[2] = 0; - } - - // Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0) - ned_accel[2] -= GRAV; - - // Transform the accels back in to body frame - AccelsData accelsData; // Skip get as we set all the fields - accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0]; - accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1]; - accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2]; - accelsData.temperature = 30; - AccelsSet(&accelsData); - - if(baro_offset == 0) { - // Hacky initialization - baro_offset = 50;// * rand_gauss(); - } else { - // Very small drift process - baro_offset += rand_gauss() / 100; - } - // Update baro periodically - static uint32_t last_baro_time = 0; - if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) { - BaroAltitudeData baroAltitude; - BaroAltitudeGet(&baroAltitude); - baroAltitude.Altitude = -pos[2] + baro_offset; - BaroAltitudeSet(&baroAltitude); - last_baro_time = PIOS_DELAY_GetRaw(); - } - - // Update baro airpseed periodically - static uint32_t last_airspeed_time = 0; - if(PIOS_DELAY_DiffuS(last_airspeed_time) / 1.0e6 > BARO_PERIOD) { - AirspeedSensorData airspeedSensor; - airspeedSensor.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; - airspeedSensor.CalibratedAirspeed = forwardAirspeed; - AirspeedSensorSet(&airspeedSensor); - last_airspeed_time = PIOS_DELAY_GetRaw(); - } - - HomeLocationData homeLocation; - HomeLocationGet(&homeLocation); - - static float gps_vel_drift[3] = {0,0,0}; - gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0; - gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0; - gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0; - - // Update GPS periodically - static uint32_t last_gps_time = 0; - if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) { - // Use double precision here as simulating what GPS produces - double T[3]; - T[0] = homeLocation.Altitude+6.378137E6f * M_PI / 180.0; - T[1] = cos(homeLocation.Latitude / 10e6 * M_PI / 180.0f)*(homeLocation.Altitude+6.378137E6) * M_PI / 180.0; - T[2] = -1.0; - - static float gps_drift[3] = {0,0,0}; - gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0; - gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0; - gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0; - - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6); - gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6); - gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]); - gpsPosition.Groundspeed = sqrt(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2)); - gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]); - gpsPosition.Satellites = 7; - gpsPosition.PDOP = 1; - GPSPositionSet(&gpsPosition); - last_gps_time = PIOS_DELAY_GetRaw(); - } - - // Update GPS Velocity measurements - static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond - if(PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) { - GPSVelocityData gpsVelocity; - GPSVelocityGet(&gpsVelocity); - gpsVelocity.North = vel[0] + gps_vel_drift[0]; - gpsVelocity.East = vel[1] + gps_vel_drift[1]; - gpsVelocity.Down = vel[2] + gps_vel_drift[2]; - GPSVelocitySet(&gpsVelocity); - last_gps_vel_time = PIOS_DELAY_GetRaw(); - } - - // Update mag periodically - static uint32_t last_mag_time = 0; - if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { - MagnetometerData mag; - mag.x = 100+homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; - mag.y = 100+homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; - mag.z = 100+homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; - magOffsetEstimation(&mag); - MagnetometerSet(&mag); - last_mag_time = PIOS_DELAY_GetRaw(); - } - - AttitudeSimulatedData attitudeSimulated; - AttitudeSimulatedGet(&attitudeSimulated); - attitudeSimulated.q1 = q[0]; - attitudeSimulated.q2 = q[1]; - attitudeSimulated.q3 = q[2]; - attitudeSimulated.q4 = q[3]; - Quaternion2RPY(q,&attitudeSimulated.Roll); - attitudeSimulated.Position[0] = pos[0]; - attitudeSimulated.Position[1] = pos[1]; - attitudeSimulated.Position[2] = pos[2]; - attitudeSimulated.Velocity[0] = vel[0]; - attitudeSimulated.Velocity[1] = vel[1]; - attitudeSimulated.Velocity[2] = vel[2]; - AttitudeSimulatedSet(&attitudeSimulated); + if (dT < 1e-3) { + dT = 2e-3; + } + last_time = PIOS_DELAY_GetRaw(); + + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + ActuatorDesiredData actuatorDesired; + ActuatorDesiredGet(&actuatorDesired); + + float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0; + if (thrust < 0) { + thrust = 0; + } + + if (thrust != thrust) { + thrust = 0; + } + + // float control_scaling = thrust * thrustToDegs; + //// In rad/s + // rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; + // rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; + // rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; + // + // GyrosData gyrosData; // Skip get as we set all the fields + // gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss(); + // gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss(); + // gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss(); + + /**** 1. Update attitude ****/ + RateDesiredData rateDesired; + RateDesiredGet(&rateDesired); + + // Need to get roll angle for easy cross coupling + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); + double roll = attitudeActual.Roll; + double pitch = attitudeActual.Pitch; + + rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; + rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; + rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; + rpy[2] += roll * ROLL_HEADING_COUPLING; + + + GyrosData gyrosData; // Skip get as we set all the fields + gyrosData.x = rpy[0] + rand_gauss(); + gyrosData.y = rpy[1] + rand_gauss(); + gyrosData.z = rpy[2] + rand_gauss(); + GyrosSet(&gyrosData); + + // Predict the attitude forward in time + float qdot[4]; + qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * M_PI / 180 / 2; + qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * M_PI / 180 / 2; + qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * M_PI / 180 / 2; + qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * M_PI / 180 / 2; + + // Take a time step + q[0] = q[0] + qdot[0]; + q[1] = q[1] + qdot[1]; + q[2] = q[2] + qdot[2]; + q[3] = q[3] + qdot[3]; + + float qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + q[0] = q[0] / qmag; + q[1] = q[1] / qmag; + q[2] = q[2] / qmag; + q[3] = q[3] / qmag; + + if (overideAttitude) { + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); + attitudeActual.q1 = q[0]; + attitudeActual.q2 = q[1]; + attitudeActual.q3 = q[2]; + attitudeActual.q4 = q[3]; + AttitudeActualSet(&attitudeActual); + } + + /**** 2. Update position based on velocity ****/ + static float wind[3] = { 0, 0, 0 }; + wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0; + wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0; + wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0; + wind[0] = 0; + wind[1] = 0; + wind[2] = 0; + + // Rbe takes a vector from body to earth. If we take (1,0,0)^T through this and then dot with airspeed + // we get forward airspeed + Quaternion2R(q, Rbe); + + double airspeed[3] = { vel[0] - wind[0], vel[1] - wind[1], vel[2] - wind[2] }; + double forwardAirspeed = Rbe[0][0] * airspeed[0] + Rbe[0][1] * airspeed[1] + Rbe[0][2] * airspeed[2]; + double sidewaysAirspeed = Rbe[1][0] * airspeed[0] + Rbe[1][1] * airspeed[1] + Rbe[1][2] * airspeed[2]; + double downwardAirspeed = Rbe[2][0] * airspeed[0] + Rbe[2][1] * airspeed[1] + Rbe[2][2] * airspeed[2]; + + /* Compute aerodynamic forces in body referenced frame. Later use more sophisticated equations */ + /* TODO: This should become more accurate. Use the force equations to calculate lift from the */ + /* various surfaces based on AoA and airspeed. From that compute torques and forces. For later */ + double forces[3]; // X, Y, Z + forces[0] = thrust - pitch * PITCH_THRUST_COUPLING - forwardAirspeed * K_FRICTION; // Friction is applied in all directions in NED + forces[1] = 0 - sidewaysAirspeed * K_FRICTION * 100; // No side slip + forces[2] = GRAV * (forwardAirspeed - LIFT_SPEED) + downwardAirspeed * K_FRICTION * 100; // Stupidly simple, always have gravity lift when straight and level + + // Negate force[2] as NED defines down as possitive, aircraft convention is Z up is positive (?) + ned_accel[0] = forces[0] * Rbe[0][0] + forces[1] * Rbe[1][0] - forces[2] * Rbe[2][0]; + ned_accel[1] = forces[0] * Rbe[0][1] + forces[1] * Rbe[1][1] - forces[2] * Rbe[2][1]; + ned_accel[2] = forces[0] * Rbe[0][2] + forces[1] * Rbe[1][2] - forces[2] * Rbe[2][2]; + // Gravity causes acceleration of 9.81 in the down direction + ned_accel[2] += 9.81; + + // Apply acceleration based on velocity + ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]); + ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]); + ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]); + + // Predict the velocity forward in time + vel[0] = vel[0] + ned_accel[0] * dT; + vel[1] = vel[1] + ned_accel[1] * dT; + vel[2] = vel[2] + ned_accel[2] * dT; + + // Predict the position forward in time + pos[0] = pos[0] + vel[0] * dT; + pos[1] = pos[1] + vel[1] * dT; + pos[2] = pos[2] + vel[2] * dT; + + // Simulate hitting ground + if (pos[2] > 0) { + pos[2] = 0; + vel[2] = 0; + ned_accel[2] = 0; + } + + // Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0) + ned_accel[2] -= GRAV; + + // Transform the accels back in to body frame + AccelsData accelsData; // Skip get as we set all the fields + accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0]; + accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1]; + accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2]; + accelsData.temperature = 30; + AccelsSet(&accelsData); + + if (baro_offset == 0) { + // Hacky initialization + baro_offset = 50; // * rand_gauss(); + } else { + // Very small drift process + baro_offset += rand_gauss() / 100; + } + // Update baro periodically + static uint32_t last_baro_time = 0; + if (PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) { + BaroAltitudeData baroAltitude; + BaroAltitudeGet(&baroAltitude); + baroAltitude.Altitude = -pos[2] + baro_offset; + BaroAltitudeSet(&baroAltitude); + last_baro_time = PIOS_DELAY_GetRaw(); + } + + // Update baro airpseed periodically + static uint32_t last_airspeed_time = 0; + if (PIOS_DELAY_DiffuS(last_airspeed_time) / 1.0e6 > BARO_PERIOD) { + AirspeedSensorData airspeedSensor; + airspeedSensor.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + airspeedSensor.CalibratedAirspeed = forwardAirspeed; + AirspeedSensorSet(&airspeedSensor); + last_airspeed_time = PIOS_DELAY_GetRaw(); + } + + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); + + static float gps_vel_drift[3] = { 0, 0, 0 }; + gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0; + gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0; + gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0; + + // Update GPS periodically + static uint32_t last_gps_time = 0; + if (PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) { + // Use double precision here as simulating what GPS produces + double T[3]; + T[0] = homeLocation.Altitude + 6.378137E6f * M_PI / 180.0; + T[1] = cos(homeLocation.Latitude / 10e6 * M_PI / 180.0f) * (homeLocation.Altitude + 6.378137E6) * M_PI / 180.0; + T[2] = -1.0; + + static float gps_drift[3] = { 0, 0, 0 }; + gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0; + gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0; + gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0; + + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6); + gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1]) / T[1] * 10.0e6); + gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]); + gpsPosition.Groundspeed = sqrt(pow(vel[0] + gps_vel_drift[0], 2) + pow(vel[1] + gps_vel_drift[1], 2)); + gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1], vel[0] + gps_vel_drift[0]); + gpsPosition.Satellites = 7; + gpsPosition.PDOP = 1; + GPSPositionSet(&gpsPosition); + last_gps_time = PIOS_DELAY_GetRaw(); + } + + // Update GPS Velocity measurements + static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond + if (PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) { + GPSVelocityData gpsVelocity; + GPSVelocityGet(&gpsVelocity); + gpsVelocity.North = vel[0] + gps_vel_drift[0]; + gpsVelocity.East = vel[1] + gps_vel_drift[1]; + gpsVelocity.Down = vel[2] + gps_vel_drift[2]; + GPSVelocitySet(&gpsVelocity); + last_gps_vel_time = PIOS_DELAY_GetRaw(); + } + + // Update mag periodically + static uint32_t last_mag_time = 0; + if (PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { + MagnetometerData mag; + mag.x = 100 + homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; + mag.y = 100 + homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; + mag.z = 100 + homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; + magOffsetEstimation(&mag); + MagnetometerSet(&mag); + last_mag_time = PIOS_DELAY_GetRaw(); + } + + AttitudeSimulatedData attitudeSimulated; + AttitudeSimulatedGet(&attitudeSimulated); + attitudeSimulated.q1 = q[0]; + attitudeSimulated.q2 = q[1]; + attitudeSimulated.q3 = q[2]; + attitudeSimulated.q4 = q[3]; + Quaternion2RPY(q, &attitudeSimulated.Roll); + attitudeSimulated.Position[0] = pos[0]; + attitudeSimulated.Position[1] = pos[1]; + attitudeSimulated.Position[2] = pos[2]; + attitudeSimulated.Velocity[0] = vel[0]; + attitudeSimulated.Velocity[1] = vel[1]; + attitudeSimulated.Velocity[2] = vel[2]; + AttitudeSimulatedSet(&attitudeSimulated); } -static float rand_gauss (void) { - float v1,v2,s; - - do { - v1 = 2.0 * ((float) rand()/RAND_MAX) - 1; - v2 = 2.0 * ((float) rand()/RAND_MAX) - 1; - - s = v1*v1 + v2*v2; - } while ( s >= 1.0 ); - - if (s == 0.0) - return 0.0; - else - return (v1*sqrt(-2.0 * log(s) / s)); +static float rand_gauss(void) +{ + float v1, v2, s; + + do { + v1 = 2.0 * ((float)rand() / RAND_MAX) - 1; + v2 = 2.0 * ((float)rand() / RAND_MAX) - 1; + + s = v1 * v1 + v2 * v2; + } while (s >= 1.0); + + if (s == 0.0) { + return 0.0; + } else { + return v1 * sqrt(-2.0 * log(s) / s); + } } /** * Perform an update of the @ref MagBias based on - * Magnetometer Offset Cancellation: Theory and Implementation, + * Magnetometer Offset Cancellation: Theory and Implementation, * revisited William Premerlani, October 14, 2011 */ static void magOffsetEstimation(MagnetometerData *mag) { #if 0 - RevoCalibrationData cal; - RevoCalibrationGet(&cal); + RevoCalibrationData cal; + RevoCalibrationGet(&cal); - // Constants, to possibly go into a UAVO - static const float MIN_NORM_DIFFERENCE = 50; + // Constants, to possibly go into a UAVO + static const float MIN_NORM_DIFFERENCE = 50; - static float B2[3] = {0, 0, 0}; + static float B2[3] = { 0, 0, 0 }; - MagBiasData magBias; - MagBiasGet(&magBias); + MagBiasData magBias; + MagBiasGet(&magBias); - // Remove the current estimate of the bias - mag->x -= magBias.x; - mag->y -= magBias.y; - mag->z -= magBias.z; + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; - // First call - if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { - B2[0] = mag->x; - B2[1] = mag->y; - B2[2] = mag->z; - return; - } + // First call + if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { + B2[0] = mag->x; + B2[1] = mag->y; + B2[2] = mag->z; + return; + } - float B1[3] = {mag->x, mag->y, mag->z}; - float norm_diff = sqrtf(powf(B2[0] - B1[0],2) + powf(B2[1] - B1[1],2) + powf(B2[2] - B1[2],2)); - if (norm_diff > MIN_NORM_DIFFERENCE) { - float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]); - float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]); - float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; - float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale}; + float B1[3] = { mag->x, mag->y, mag->z }; + float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2)); + if (norm_diff > MIN_NORM_DIFFERENCE) { + float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]); + float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]); + float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; + float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale }; - magBias.x += b_error[0]; - magBias.y += b_error[1]; - magBias.z += b_error[2]; + magBias.x += b_error[0]; + magBias.y += b_error[1]; + magBias.z += b_error[2]; - MagBiasSet(&magBias); + MagBiasSet(&magBias); - // Store this value to compare against next update - B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; - } -#else - HomeLocationData homeLocation; - HomeLocationGet(&homeLocation); - - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - - MagBiasData magBias; - MagBiasGet(&magBias); - - // Remove the current estimate of the bias - mag->x -= magBias.x; - mag->y -= magBias.y; - mag->z -= magBias.z; - - const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]); - const float Rz = homeLocation.Be[2]; - - const float rate = 0.01; - float R[3][3]; - float B_e[3]; - float xy[2]; - float delta[3]; - - // Get the rotation matrix - Quaternion2R(&attitude.q1, R); - - // Rotate the mag into the NED frame - B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z; - B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z; - B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z; - - float cy = cosf(attitude.Yaw * M_PI / 180.0f); - float sy = sinf(attitude.Yaw * M_PI / 180.0f); + // Store this value to compare against next update + B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; + } +#else /* if 0 */ + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); - xy[0] = cy * B_e[0] + sy * B_e[1]; - xy[1] = -sy * B_e[0] + cy * B_e[1]; - - float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]); - - delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); - delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); - delta[2] = -rate * (Rz - B_e[2]); - - magBias.x += delta[0]; - magBias.y += delta[1]; - magBias.z += delta[2]; - MagBiasSet(&magBias); -#endif + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + MagBiasData magBias; + MagBiasGet(&magBias); + + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; + + const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]); + const float Rz = homeLocation.Be[2]; + + const float rate = 0.01; + float R[3][3]; + float B_e[3]; + float xy[2]; + float delta[3]; + + // Get the rotation matrix + Quaternion2R(&attitude.q1, R); + + // Rotate the mag into the NED frame + B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z; + B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z; + B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z; + + float cy = cosf(attitude.Yaw * M_PI / 180.0f); + float sy = sinf(attitude.Yaw * M_PI / 180.0f); + + xy[0] = cy * B_e[0] + sy * B_e[1]; + xy[1] = -sy * B_e[0] + cy * B_e[1]; + + float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]); + + delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); + delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); + delta[2] = -rate * (Rz - B_e[2]); + + magBias.x += delta[0]; + magBias.y += delta[1]; + magBias.z += delta[2]; + MagBiasSet(&magBias); +#endif /* if 0 */ } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index 81adb6235..9d8192c43 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -1,12 +1,12 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup StabilizationModule Stabilization Module * @brief Stabilization PID loops in an airframe type independent manner - * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the + * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual" - * @{ + * @{ * * @file stabilization.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -33,13 +33,13 @@ #ifndef STABILIZATION_H #define STABILIZATION_H -enum {ROLL,PITCH,YAW,MAX_AXES}; +enum { ROLL, PITCH, YAW, MAX_AXES }; int32_t StabilizationInitialize(); #endif // STABILIZATION_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/Stabilization/inc/virtualflybar.h b/flight/modules/Stabilization/inc/virtualflybar.h index 8ef54ce52..d92de4801 100644 --- a/flight/modules/Stabilization/inc/virtualflybar.h +++ b/flight/modules/Stabilization/inc/virtualflybar.h @@ -39,4 +39,4 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings); int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT); - #endif /* VIRTUALFLYBAR_H */ \ No newline at end of file + #endif /* VIRTUALFLYBAR_H */ diff --git a/flight/modules/Stabilization/relay_tuning.c b/flight/modules/Stabilization/relay_tuning.c index be22585ed..f9b85c190 100644 --- a/flight/modules/Stabilization/relay_tuning.c +++ b/flight/modules/Stabilization/relay_tuning.c @@ -38,7 +38,7 @@ #include "sin_lookup.h" /** - * Apply a step function for the stabilization controller and monitor the + * Apply a step function for the stabilization controller and monitor the * result * * Used to Replace the rate PID with a relay to measure the critical properties of this axis @@ -46,101 +46,101 @@ */ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) { - RelayTuningData relay; - RelayTuningGet(&relay); + RelayTuningData relay; - static portTickType lastHighTime; - static portTickType lastLowTime; + RelayTuningGet(&relay); - static float accum_sin, accum_cos; - static uint32_t accumulated = 0; + static portTickType lastHighTime; + static portTickType lastLowTime; - const uint16_t DEGLITCH_TIME = 20; // ms - const float AMPLITUDE_ALPHA = 0.95f; - const float PERIOD_ALPHA = 0.95f; + static float accum_sin, accum_cos; + static uint32_t accumulated = 0; - portTickType thisTime = xTaskGetTickCount(); + const uint16_t DEGLITCH_TIME = 20; // ms + const float AMPLITUDE_ALPHA = 0.95f; + const float PERIOD_ALPHA = 0.95f; - static bool rateRelayRunning[MAX_AXES]; + portTickType thisTime = xTaskGetTickCount(); - // This indicates the current estimate of the smoothed error. So when it is high - // we are waiting for it to go low. - static bool high = false; + static bool rateRelayRunning[MAX_AXES]; - // On first run initialize estimates to something reasonable - if(reinit) { - rateRelayRunning[axis] = false; - relay.Period[axis] = 200; - relay.Gain[axis] = 0; + // This indicates the current estimate of the smoothed error. So when it is high + // we are waiting for it to go low. + static bool high = false; - accum_sin = 0; - accum_cos = 0; - accumulated = 0; + // On first run initialize estimates to something reasonable + if (reinit) { + rateRelayRunning[axis] = false; + relay.Period[axis] = 200; + relay.Gain[axis] = 0; - // These should get reinitialized anyway - high = true; - lastHighTime = thisTime; - lastLowTime = thisTime; - RelayTuningSet(&relay); - } + accum_sin = 0; + accum_cos = 0; + accumulated = 0; + + // These should get reinitialized anyway + high = true; + lastHighTime = thisTime; + lastLowTime = thisTime; + RelayTuningSet(&relay); + } - RelayTuningSettingsData relaySettings; - RelayTuningSettingsGet(&relaySettings); + RelayTuningSettingsData relaySettings; + RelayTuningSettingsGet(&relaySettings); - // Compute output, simple threshold on error - *output = high ? relaySettings.Amplitude : -relaySettings.Amplitude; + // Compute output, simple threshold on error + *output = high ? relaySettings.Amplitude : -relaySettings.Amplitude; - /**** The code below here is to estimate the properties of the oscillation ****/ + /**** The code below here is to estimate the properties of the oscillation ****/ - // Make sure the period can't go below limit - if (relay.Period[axis] < DEGLITCH_TIME) - relay.Period[axis] = DEGLITCH_TIME; + // Make sure the period can't go below limit + if (relay.Period[axis] < DEGLITCH_TIME) { + relay.Period[axis] = DEGLITCH_TIME; + } - // Project the error onto a sine and cosine of the same frequency - // to accumulate the average amplitude - int32_t dT = thisTime - lastHighTime; - float phase = ((float)360 * (float)dT) / relay.Period[axis]; - if(phase >= 360) - phase = 0; - accum_sin += sin_lookup_deg(phase) * error; - accum_cos += cos_lookup_deg(phase) * error; - accumulated ++; + // Project the error onto a sine and cosine of the same frequency + // to accumulate the average amplitude + int32_t dT = thisTime - lastHighTime; + float phase = ((float)360 * (float)dT) / relay.Period[axis]; + if (phase >= 360) { + phase = 0; + } + accum_sin += sin_lookup_deg(phase) * error; + accum_cos += cos_lookup_deg(phase) * error; + accumulated++; - // Make sure we've had enough time since last transition then check for a change in the output - bool time_hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME; + // Make sure we've had enough time since last transition then check for a change in the output + bool time_hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME; - if ( !high && time_hysteresis && error > relaySettings.HysteresisThresh ){ - /* POSITIVE CROSSING DETECTED */ + if (!high && time_hysteresis && error > relaySettings.HysteresisThresh) { + /* POSITIVE CROSSING DETECTED */ - float this_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated; - float this_gain = this_amplitude / relaySettings.Amplitude; + float this_amplitude = 2 * sqrtf(accum_sin * accum_sin + accum_cos * accum_cos) / accumulated; + float this_gain = this_amplitude / relaySettings.Amplitude; - accumulated = 0; - accum_sin = 0; - accum_cos = 0; + accumulated = 0; + accum_sin = 0; + accum_cos = 0; - if(rateRelayRunning[axis] == false) { - rateRelayRunning[axis] = true; - relay.Period[axis] = 200; - relay.Gain[axis] = 0; - } else { - // Low pass filter each amplitude and period - relay.Gain[axis] = relay.Gain[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); - relay.Period[axis] = relay.Period[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); - } - lastHighTime = thisTime; - high = true; - RelayTuningSet(&relay); + if (rateRelayRunning[axis] == false) { + rateRelayRunning[axis] = true; + relay.Period[axis] = 200; + relay.Gain[axis] = 0; + } else { + // Low pass filter each amplitude and period + relay.Gain[axis] = relay.Gain[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); + relay.Period[axis] = relay.Period[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); + } + lastHighTime = thisTime; + high = true; + RelayTuningSet(&relay); + } else if (high && time_hysteresis && error < -relaySettings.HysteresisThresh) { + /* FALLING CROSSING DETECTED */ - } else if ( high && time_hysteresis && error < -relaySettings.HysteresisThresh ) { - /* FALLING CROSSING DETECTED */ + lastLowTime = thisTime; + high = false; + } - lastLowTime = thisTime; - high = false; - - } - - return 0; + return 0; } - diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index d9b17f1c2..3a8a464e8 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -59,18 +59,18 @@ #include "relay_tuning.h" // Private constants -#define MAX_QUEUE_SIZE 1 +#define MAX_QUEUE_SIZE 1 #if defined(PIOS_STABILIZATION_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE +#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE #else -#define STACK_SIZE_BYTES 724 +#define STACK_SIZE_BYTES 724 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY+4) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) #define FAILSAFE_TIMEOUT_MS 30 -enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX}; +enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX }; // Private variables @@ -78,10 +78,10 @@ static xTaskHandle taskHandle; static StabilizationSettingsData settings; static xQueueHandle queue; float gyro_alpha = 0; -float axis_lock_accum[3] = {0,0,0}; -uint8_t max_axis_lock = 0; +float axis_lock_accum[3] = { 0, 0, 0 }; +uint8_t max_axis_lock = 0; uint8_t max_axislock_rate = 0; -float weak_leveling_kp = 0; +float weak_leveling_kp = 0; uint8_t weak_leveling_max = 0; bool lowThrottleZeroIntegral; bool lowThrottleZeroAxis[MAX_AXES]; @@ -89,34 +89,34 @@ float vbar_decay = 0.991f; struct pid pids[PID_MAX]; // Private functions -static void stabilizationTask(void* parameters); +static void stabilizationTask(void *parameters); static float bound(float val, float range); static void ZeroPids(void); -static void SettingsUpdatedCb(UAVObjEvent * ev); +static void SettingsUpdatedCb(UAVObjEvent *ev); /** * Module initialization */ int32_t StabilizationStart() { - // Initialize variables - // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + // Initialize variables + // Create object queue + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - // Listen for updates. - // AttitudeActualConnectQueue(queue); - GyrosConnectQueue(queue); - - StabilizationSettingsConnectCallback(SettingsUpdatedCb); - SettingsUpdatedCb(StabilizationSettingsHandle()); - - // Start main task - xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle); + // Listen for updates. + // AttitudeActualConnectQueue(queue); + GyrosConnectQueue(queue); + + StabilizationSettingsConnectCallback(SettingsUpdatedCb); + SettingsUpdatedCb(StabilizationSettingsHandle()); + + // Start main task + xTaskCreate(stabilizationTask, (signed char *)"Stabilization", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle); #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION); + PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION); #endif - return 0; + return 0; } /** @@ -124,18 +124,18 @@ int32_t StabilizationStart() */ int32_t StabilizationInitialize() { - // Initialize variables - StabilizationSettingsInitialize(); - ActuatorDesiredInitialize(); + // Initialize variables + StabilizationSettingsInitialize(); + ActuatorDesiredInitialize(); #ifdef DIAG_RATEDESIRED - RateDesiredInitialize(); + RateDesiredInitialize(); #endif - // Code required for relay tuning - sin_lookup_initalize(); - RelayTuningSettingsInitialize(); - RelayTuningInitialize(); + // Code required for relay tuning + sin_lookup_initalize(); + RelayTuningSettingsInitialize(); + RelayTuningInitialize(); - return 0; + return 0; } MODULE_INITCALL(StabilizationInitialize, StabilizationStart) @@ -143,271 +143,282 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart) /** * Module task */ -static void stabilizationTask(__attribute__((unused)) void* parameters) +static void stabilizationTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; - - uint32_t timeval = PIOS_DELAY_GetRaw(); - - ActuatorDesiredData actuatorDesired; - StabilizationDesiredData stabDesired; - RateDesiredData rateDesired; - AttitudeActualData attitudeActual; - GyrosData gyrosData; - FlightStatusData flightStatus; - - SettingsUpdatedCb((UAVObjEvent *) NULL); - - // Main task loop - ZeroPids(); - while(1) { - float dT; - + UAVObjEvent ev; + + uint32_t timeval = PIOS_DELAY_GetRaw(); + + ActuatorDesiredData actuatorDesired; + StabilizationDesiredData stabDesired; + RateDesiredData rateDesired; + AttitudeActualData attitudeActual; + GyrosData gyrosData; + FlightStatusData flightStatus; + + SettingsUpdatedCb((UAVObjEvent *)NULL); + + // Main task loop + ZeroPids(); + while (1) { + float dT; + #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); + PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); #endif - - // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe - if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ) - { - AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); - continue; - } - - dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f; - timeval = PIOS_DELAY_GetRaw(); - - FlightStatusGet(&flightStatus); - StabilizationDesiredGet(&stabDesired); - AttitudeActualGet(&attitudeActual); - GyrosGet(&gyrosData); + + // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe + if (xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) { + AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING); + continue; + } + + dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f; + timeval = PIOS_DELAY_GetRaw(); + + FlightStatusGet(&flightStatus); + StabilizationDesiredGet(&stabDesired); + AttitudeActualGet(&attitudeActual); + GyrosGet(&gyrosData); #ifdef DIAG_RATEDESIRED - RateDesiredGet(&rateDesired); + RateDesiredGet(&rateDesired); #endif - + #if defined(PIOS_QUATERNION_STABILIZATION) - // Quaternion calculation of error in each axis. Uses more memory. - float rpy_desired[3]; - float q_desired[4]; - float q_error[4]; - float local_error[3]; - - // Essentially zero errors for anything in rate or none - if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) - rpy_desired[0] = stabDesired.Roll; - else - rpy_desired[0] = attitudeActual.Roll; - - if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) - rpy_desired[1] = stabDesired.Pitch; - else - rpy_desired[1] = attitudeActual.Pitch; - - if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) - rpy_desired[2] = stabDesired.Yaw; - else - rpy_desired[2] = attitudeActual.Yaw; - - RPY2Quaternion(rpy_desired, q_desired); - quat_inverse(q_desired); - quat_mult(q_desired, &attitudeActual.q1, q_error); - quat_inverse(q_error); - Quaternion2RPY(q_error, local_error); - -#else - // Simpler algorithm for CC, less memory - float local_error[3] = {stabDesired.Roll - attitudeActual.Roll, - stabDesired.Pitch - attitudeActual.Pitch, - stabDesired.Yaw - attitudeActual.Yaw}; - // find shortest way - float modulo = fmodf(local_error[2] + 180.0f, 360.0f); - if(modulo<0) - local_error[2] = modulo + 180.0f; - else - local_error[2] = modulo - 180.0f; -#endif + // Quaternion calculation of error in each axis. Uses more memory. + float rpy_desired[3]; + float q_desired[4]; + float q_error[4]; + float local_error[3]; - float gyro_filtered[3]; - gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha); - gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha); - gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha); + // Essentially zero errors for anything in rate or none + if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { + rpy_desired[0] = stabDesired.Roll; + } else { + rpy_desired[0] = attitudeActual.Roll; + } - float *attitudeDesiredAxis = &stabDesired.Roll; - float *actuatorDesiredAxis = &actuatorDesired.Roll; - float *rateDesiredAxis = &rateDesired.Roll; + if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { + rpy_desired[1] = stabDesired.Pitch; + } else { + rpy_desired[1] = attitudeActual.Pitch; + } - ActuatorDesiredGet(&actuatorDesired); + if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { + rpy_desired[2] = stabDesired.Yaw; + } else { + rpy_desired[2] = attitudeActual.Yaw; + } - // A flag to track which stabilization mode each axis is in - static uint8_t previous_mode[MAX_AXES] = {255,255,255}; - bool error = false; + RPY2Quaternion(rpy_desired, q_desired); + quat_inverse(q_desired); + quat_mult(q_desired, &attitudeActual.q1, q_error); + quat_inverse(q_error); + Quaternion2RPY(q_error, local_error); - //Run the selected stabilization algorithm on each axis: - for(uint8_t i=0; i< MAX_AXES; i++) - { - // Check whether this axis mode needs to be reinitialized - bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]); - previous_mode[i] = stabDesired.StabilizationMode[i]; +#else /* if defined(PIOS_QUATERNION_STABILIZATION) */ + // Simpler algorithm for CC, less memory + float local_error[3] = { stabDesired.Roll - attitudeActual.Roll, + stabDesired.Pitch - attitudeActual.Pitch, + stabDesired.Yaw - attitudeActual.Yaw }; + // find shortest way + float modulo = fmodf(local_error[2] + 180.0f, 360.0f); + if (modulo < 0) { + local_error[2] = modulo + 180.0f; + } else { + local_error[2] = modulo - 180.0f; + } +#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */ - // Apply the selected control law - switch(stabDesired.StabilizationMode[i]) - { - case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: - if(reinit) - pids[PID_RATE_ROLL + i].iAccumulator = 0; + float gyro_filtered[3]; + gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha); + gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha); + gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha); - // Store to rate desired variable for storing to UAVO - rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); + float *attitudeDesiredAxis = &stabDesired.Roll; + float *actuatorDesiredAxis = &actuatorDesired.Roll; + float *rateDesiredAxis = &rateDesired.Roll; - // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + ActuatorDesiredGet(&actuatorDesired); - break; + // A flag to track which stabilization mode each axis is in + static uint8_t previous_mode[MAX_AXES] = { 255, 255, 255 }; + bool error = false; - case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: - if(reinit) { - pids[PID_ROLL + i].iAccumulator = 0; - pids[PID_RATE_ROLL + i].iAccumulator = 0; - } + // Run the selected stabilization algorithm on each axis: + for (uint8_t i = 0; i < MAX_AXES; i++) { + // Check whether this axis mode needs to be reinitialized + bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]); + previous_mode[i] = stabDesired.StabilizationMode[i]; - // Compute the outer loop - rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); - rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + // Apply the selected control law + switch (stabDesired.StabilizationMode[i]) { + case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: + if (reinit) { + pids[PID_RATE_ROLL + i].iAccumulator = 0; + } - // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + // Store to rate desired variable for storing to UAVO + rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); - break; + // Compute the inner loop + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); - case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: + break; - // Store for debugging output - rateDesiredAxis[i] = attitudeDesiredAxis[i]; + case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: + if (reinit) { + pids[PID_ROLL + i].iAccumulator = 0; + pids[PID_RATE_ROLL + i].iAccumulator = 0; + } - // Run a virtual flybar stabilization algorithm on this axis - stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings); + // Compute the outer loop + rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); - break; + // Compute the inner loop + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); - case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: - { - if (reinit) - pids[PID_RATE_ROLL + i].iAccumulator = 0; + break; - float weak_leveling = local_error[i] * weak_leveling_kp; - weak_leveling = bound(weak_leveling, weak_leveling_max); + case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: - // Compute desired rate as input biased towards leveling - rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + // Store for debugging output + rateDesiredAxis[i] = attitudeDesiredAxis[i]; - break; - } + // Run a virtual flybar stabilization algorithm on this axis + stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings); - case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: - if (reinit) - pids[PID_RATE_ROLL + i].iAccumulator = 0; + break; - if(fabsf(attitudeDesiredAxis[i]) > max_axislock_rate) { - // While getting strong commands act like rate mode - rateDesiredAxis[i] = attitudeDesiredAxis[i]; - axis_lock_accum[i] = 0; - } else { - // For weaker commands or no command simply attitude lock (almost) on no gyro change - axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT; - axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock); - rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT); - } + case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: + { + if (reinit) { + pids[PID_RATE_ROLL + i].iAccumulator = 0; + } - rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate[i]); + float weak_leveling = local_error[i] * weak_leveling_kp; + weak_leveling = bound(weak_leveling, weak_leveling_max); - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + // Compute desired rate as input biased towards leveling + rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); - break; + break; + } - case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: - // Store to rate desired variable for storing to UAVO - rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); + case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: + if (reinit) { + pids[PID_RATE_ROLL + i].iAccumulator = 0; + } - // Run the relay controller which also estimates the oscillation parameters - stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + if (fabsf(attitudeDesiredAxis[i]) > max_axislock_rate) { + // While getting strong commands act like rate mode + rateDesiredAxis[i] = attitudeDesiredAxis[i]; + axis_lock_accum[i] = 0; + } else { + // For weaker commands or no command simply attitude lock (almost) on no gyro change + axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT; + axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock); + rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT); + } - break; + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate[i]); - case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE: - if(reinit) - pids[PID_ROLL + i].iAccumulator = 0; + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); - // Compute the outer loop like attitude mode - rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); - rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + break; - // Run the relay controller which also estimates the oscillation parameters - stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: + // Store to rate desired variable for storing to UAVO + rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); - break; + // Run the relay controller which also estimates the oscillation parameters + stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); - case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: - actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i], 1.0f); - break; - default: - error = true; - break; - } - } + break; - if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) - stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT); + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE: + if (reinit) { + pids[PID_ROLL + i].iAccumulator = 0; + } + + // Compute the outer loop like attitude mode + rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + + // Run the relay controller which also estimates the oscillation parameters + stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + + break; + + case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: + actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i], 1.0f); + break; + default: + error = true; + break; + } + } + + if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) { + stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT); + } #ifdef DIAG_RATEDESIRED - RateDesiredSet(&rateDesired); + RateDesiredSet(&rateDesired); #endif - // Save dT - actuatorDesired.UpdateTime = dT * 1000; - actuatorDesired.Throttle = stabDesired.Throttle; + // Save dT + actuatorDesired.UpdateTime = dT * 1000; + actuatorDesired.Throttle = stabDesired.Throttle; - // Suppress desired output while disarmed or throttle low, for configured axis - if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || stabDesired.Throttle < 0) { - if (lowThrottleZeroAxis[ROLL]) - actuatorDesired.Roll = 0.0f; + // Suppress desired output while disarmed or throttle low, for configured axis + if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || stabDesired.Throttle < 0) { + if (lowThrottleZeroAxis[ROLL]) { + actuatorDesired.Roll = 0.0f; + } - if (lowThrottleZeroAxis[PITCH]) - actuatorDesired.Pitch = 0.0f; + if (lowThrottleZeroAxis[PITCH]) { + actuatorDesired.Pitch = 0.0f; + } - if (lowThrottleZeroAxis[YAW]) - actuatorDesired.Yaw = 0.0f; - } + if (lowThrottleZeroAxis[YAW]) { + actuatorDesired.Yaw = 0.0f; + } + } - if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) { - ActuatorDesiredSet(&actuatorDesired); - } else { - // Force all axes to reinitialize when engaged - for(uint8_t i=0; i< MAX_AXES; i++) - previous_mode[i] = 255; - } + if (PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) { + ActuatorDesiredSet(&actuatorDesired); + } else { + // Force all axes to reinitialize when engaged + for (uint8_t i = 0; i < MAX_AXES; i++) { + previous_mode[i] = 255; + } + } - if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || - (lowThrottleZeroIntegral && stabDesired.Throttle < 0)) - { - // Force all axes to reinitialize when engaged - for(uint8_t i=0; i< MAX_AXES; i++) - previous_mode[i] = 255; - } + if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || + (lowThrottleZeroIntegral && stabDesired.Throttle < 0)) { + // Force all axes to reinitialize when engaged + for (uint8_t i = 0; i < MAX_AXES; i++) { + previous_mode[i] = 255; + } + } - // Clear or set alarms. Done like this to prevent toggline each cycle - // and hammering system alarms - if (error) - AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_ERROR); - else - AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); - } + // Clear or set alarms. Done like this to prevent toggline each cycle + // and hammering system alarms + if (error) { + AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); + } + } } @@ -416,12 +427,14 @@ static void stabilizationTask(__attribute__((unused)) void* parameters) */ static void ZeroPids(void) { - for(uint32_t i = 0; i < PID_MAX; i++) - pid_zero(&pids[i]); + for (uint32_t i = 0; i < PID_MAX; i++) { + pid_zero(&pids[i]); + } - for(uint8_t i = 0; i < 3; i++) - axis_lock_accum[i] = 0.0f; + for (uint8_t i = 0; i < 3; i++) { + axis_lock_accum[i] = 0.0f; + } } @@ -430,83 +443,84 @@ static void ZeroPids(void) */ static float bound(float val, float range) { - if(val < -range) { - val = -range; - } else if(val > range) { - val = range; - } - return val; + if (val < -range) { + val = -range; + } else if (val > range) { + val = range; + } + return val; } -static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent * ev) +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - StabilizationSettingsGet(&settings); - - // Set the roll rate PID constants - pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], - settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], - pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], - pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]); - - // Set the pitch rate PID constants - pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], - pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], - pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], - pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]); - - // Set the yaw rate PID constants - pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], - pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], - pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], - pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]); - - // Set the roll attitude PI constants - pid_configure(&pids[PID_ROLL], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], - settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0, - pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]); - - // Set the pitch attitude PI constants - pid_configure(&pids[PID_PITCH], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], - pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0, - settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]); - - // Set the yaw attitude PI constants - pid_configure(&pids[PID_YAW], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], - settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0, - settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]); - - // Set up the derivative term - pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); + StabilizationSettingsGet(&settings); - // Maximum deviation to accumulate for axis lock - max_axis_lock = settings.MaxAxisLock; - max_axislock_rate = settings.MaxAxisLockRate; - - // Settings for weak leveling - weak_leveling_kp = settings.WeakLevelingKp; - weak_leveling_max = settings.MaxWeakLevelingRate; - - // Whether to zero the PID integrals while throttle is low - lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; + // Set the roll rate PID constants + pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], + settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], + pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], + pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]); - // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low - lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_ROLL] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; - lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_PITCH] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; - lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_YAW] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + // Set the pitch rate PID constants + pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], + pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], + pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], + pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]); - // The dT has some jitter iteration to iteration that we don't want to - // make thie result unpredictable. Still, it's nicer to specify the constant - // based on a time (in ms) rather than a fixed multiplier. The error between - // update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this - // calculation - const float fakeDt = 0.0025f; - if(settings.GyroTau < 0.0001f) - gyro_alpha = 0; // not trusting this to resolve to 0 - else - gyro_alpha = expf(-fakeDt / settings.GyroTau); - - // Compute time constant for vbar decay term based on a tau - vbar_decay = expf(-fakeDt / settings.VbarTau); + // Set the yaw rate PID constants + pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], + pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], + pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], + pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]); + + // Set the roll attitude PI constants + pid_configure(&pids[PID_ROLL], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], + settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0, + pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]); + + // Set the pitch attitude PI constants + pid_configure(&pids[PID_PITCH], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], + pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0, + settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]); + + // Set the yaw attitude PI constants + pid_configure(&pids[PID_YAW], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], + settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0, + settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]); + + // Set up the derivative term + pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); + + // Maximum deviation to accumulate for axis lock + max_axis_lock = settings.MaxAxisLock; + max_axislock_rate = settings.MaxAxisLockRate; + + // Settings for weak leveling + weak_leveling_kp = settings.WeakLevelingKp; + weak_leveling_max = settings.MaxWeakLevelingRate; + + // Whether to zero the PID integrals while throttle is low + lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; + + // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low + lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_ROLL] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_PITCH] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_YAW] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + + // The dT has some jitter iteration to iteration that we don't want to + // make thie result unpredictable. Still, it's nicer to specify the constant + // based on a time (in ms) rather than a fixed multiplier. The error between + // update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this + // calculation + const float fakeDt = 0.0025f; + if (settings.GyroTau < 0.0001f) { + gyro_alpha = 0; // not trusting this to resolve to 0 + } else { + gyro_alpha = expf(-fakeDt / settings.GyroTau); + } + + // Compute time constant for vbar decay term based on a tau + vbar_decay = expf(-fakeDt / settings.VbarTau); } @@ -514,4 +528,3 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent * ev) * @} * @} */ - diff --git a/flight/modules/Stabilization/virtualflybar.c b/flight/modules/Stabilization/virtualflybar.c index 0b9412081..153615c57 100644 --- a/flight/modules/Stabilization/virtualflybar.c +++ b/flight/modules/Stabilization/virtualflybar.c @@ -35,55 +35,55 @@ #include "stabilization.h" #include "stabilizationsettings.h" -//! Private variables +// ! Private variables static float vbar_integral[MAX_AXES]; static float vbar_decay = 0.991f; -//! Private methods +// ! Private methods static float bound(float val, float range); int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings) { - float gyro_gain = 1.0f; - float kp = 0, ki = 0; + float gyro_gain = 1.0f; + float kp = 0, ki = 0; - if(reinit) - vbar_integral[axis] = 0; + if (reinit) { + vbar_integral[axis] = 0; + } - // Track the angle of the virtual flybar which includes a slow decay - vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT; - vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle); + // Track the angle of the virtual flybar which includes a slow decay + vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT; + vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle); - // Command signal can indicate how much to disregard the gyro feedback (fast flips) - if (settings->VbarGyroSuppress > 0) { - gyro_gain = (1.0f - fabsf(command) * settings->VbarGyroSuppress / 100.0f); - gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain; - } + // Command signal can indicate how much to disregard the gyro feedback (fast flips) + if (settings->VbarGyroSuppress > 0) { + gyro_gain = (1.0f - fabsf(command) * settings->VbarGyroSuppress / 100.0f); + gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain; + } - // Get the settings for the correct axis - switch(axis) - { - case ROLL: - kp = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; - ki = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; - break; - case PITCH: - kp = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; - ki = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; - break; - case YAW: - kp = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; - ki = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; - break; - default: - PIOS_DEBUG_Assert(0); - } + // Get the settings for the correct axis + switch (axis) { + case ROLL: + kp = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; + ki = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + break; + case PITCH: + kp = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; + ki = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + break; + case YAW: + kp = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; + ki = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + break; + default: + PIOS_DEBUG_Assert(0); + } - // Command signal is composed of stick input added to the gyro and virtual flybar - *output = command * settings->VbarSensitivity[axis] - - gyro_gain * (vbar_integral[axis] * ki + gyro * kp); + // Command signal is composed of stick input added to the gyro and virtual flybar + *output = command * settings->VbarSensitivity[axis] - + gyro_gain * (vbar_integral[axis] * ki + gyro * kp); - return 0; + return 0; } /** @@ -93,16 +93,16 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float */ int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT) { - float cy = cosf(DEG2RAD(z_gyro) * dT); - float sy = sinf(DEG2RAD(z_gyro) * dT); + float cy = cosf(DEG2RAD(z_gyro) * dT); + float sy = sinf(DEG2RAD(z_gyro) * dT); - float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0]; - float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0]; + float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0]; + float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0]; - vbar_integral[1] = vbar_pitch; - vbar_integral[0] = vbar_roll; + vbar_integral[1] = vbar_pitch; + vbar_integral[0] = vbar_roll; - return 0; + return 0; } /** @@ -110,10 +110,10 @@ int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT) */ static float bound(float val, float range) { - if(val < -range) { - val = -range; - } else if(val > range) { - val = range; - } - return val; + if (val < -range) { + val = -range; + } else if (val > range) { + val = range; + } + return val; } diff --git a/flight/modules/System/inc/systemmod.h b/flight/modules/System/inc/systemmod.h index c487f4811..bd709f404 100644 --- a/flight/modules/System/inc/systemmod.h +++ b/flight/modules/System/inc/systemmod.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup SystemModule System Module - * @{ + * @{ * * @file systemmod.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 283d48c7c..4a3046af8 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -1,19 +1,19 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @brief The OpenPilot Modules do the majority of the control in OpenPilot. The + * @brief The OpenPilot Modules do the majority of the control in OpenPilot. The * @ref SystemModule "System Module" starts all the other modules that then take care - * of all the telemetry and control algorithms and such. This is done through the @ref PIOS + * of all the telemetry and control algorithms and such. This is done through the @ref PIOS * "PIOS Hardware abstraction layer" which then contains hardware specific implementations * (currently only STM32 supported) * - * @{ + * @{ * @addtogroup SystemModule System Module * @brief Initializes PIOS and other modules runs monitoring * After initializing all the modules (currently selected by Makefile but in * future controlled by configuration on SD card) runs basic monitoring and * alarms. - * @{ + * @{ * * @file systemmod.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -58,20 +58,20 @@ #include -//#define DEBUG_THIS_FILE +// #define DEBUG_THIS_FILE #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(DEBUG_THIS_FILE) -#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, format, ## __VA_ARGS__) +#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, format,##__VA_ARGS__) #else #define DEBUG_MSG(format, ...) #endif // Private constants -#define SYSTEM_UPDATE_PERIOD_MS 1000 -#define LED_BLINK_RATE_HZ 5 +#define SYSTEM_UPDATE_PERIOD_MS 1000 +#define LED_BLINK_RATE_HZ 5 #ifndef IDLE_COUNTS_PER_SEC_AT_NO_LOAD -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 // calibrated by running tests/test_cpuload.c +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 // calibrated by running tests/test_cpuload.c // must be updated if the FreeRTOS or compiler // optimisation options are changed. #endif @@ -82,7 +82,7 @@ #define STACK_SIZE_BYTES 924 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types @@ -96,8 +96,8 @@ static bool mallocFailed; static HwSettingsData bootHwSettings; // Private functions -static void objectUpdatedCb(UAVObjEvent * ev); -static void hwSettingsUpdatedCb(UAVObjEvent * ev); +static void objectUpdatedCb(UAVObjEvent *ev); +static void hwSettingsUpdatedCb(UAVObjEvent *ev); #ifdef DIAG_TASKS static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_info *task_info, void *context); #endif @@ -116,7 +116,7 @@ int32_t SystemModStart(void) { // Initialize vars stackOverflow = false; - mallocFailed = false; + mallocFailed = false; // Create system task xTaskCreate(systemTask, (signed char *)"System", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle); // Register task @@ -131,7 +131,6 @@ int32_t SystemModStart(void) */ int32_t SystemModInitialize(void) { - // Must registers objects here for system thread because ObjectManager started in OpenPilotInit SystemSettingsInitialize(); SystemStatsInitialize(); @@ -146,8 +145,9 @@ int32_t SystemModInitialize(void) #endif objectPersistenceQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - if (objectPersistenceQueue == NULL) + if (objectPersistenceQueue == NULL) { return -1; + } SystemModStart(); @@ -158,13 +158,13 @@ MODULE_INITCALL(SystemModInitialize, 0) /** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ -static void systemTask(__attribute__((unused))void *parameters) +static void systemTask(__attribute__((unused)) void *parameters) { - /* start the delayed callback scheduler */ - CallbackSchedulerStart(); + /* start the delayed callback scheduler */ + CallbackSchedulerStart(); - /* create all modules thread */ - MODULE_TASKCREATE_ALL; + /* create all modules thread */ + MODULE_TASKCREATE_ALL; if (mallocFailed) { /* We failed to malloc during task creation, @@ -186,19 +186,19 @@ static void systemTask(__attribute__((unused))void *parameters) // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectQueue(objectPersistenceQueue); - // Load a copy of HwSetting active at boot time - HwSettingsGet(&bootHwSettings); + // Load a copy of HwSetting active at boot time + HwSettingsGet(&bootHwSettings); // Whenever the configuration changes, make sure it is safe to fly HwSettingsConnectCallback(hwSettingsUpdatedCb); #ifdef DIAG_TASKS - TaskInfoData taskInfoData; + TaskInfoData taskInfoData; #endif - // Main system loop - while (1) { - // Update the system statistics - updateStats(); + // Main system loop + while (1) { + // Update the system statistics + updateStats(); // Update the system alarms updateSystemAlarms(); @@ -208,35 +208,35 @@ static void systemTask(__attribute__((unused))void *parameters) #endif #ifdef DIAG_TASKS - // Update the task status object - PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData); - TaskInfoSet(&taskInfoData); + // Update the task status object + PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData); + TaskInfoSet(&taskInfoData); #endif // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF); -#endif /* PIOS_LED_HEARTBEAT */ +#endif /* PIOS_LED_HEARTBEAT */ // Turn on the error LED if an alarm is set -#if defined (PIOS_LED_ALARM) +#if defined(PIOS_LED_ALARM) if (AlarmsHasWarnings()) { PIOS_LED_On(PIOS_LED_ALARM); } else { PIOS_LED_Off(PIOS_LED_ALARM); } -#endif /* PIOS_LED_ALARM */ +#endif /* PIOS_LED_ALARM */ FlightStatusData flightStatus; FlightStatusGet(&flightStatus); UAVObjEvent ev; int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ? - SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) : - SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS; + SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) : + SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS; - if(xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) { + if (xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) { // If object persistence is updated call the callback objectUpdatedCb(&ev); } @@ -246,7 +246,7 @@ static void systemTask(__attribute__((unused))void *parameters) /** * Function called in response to object updates */ -static void objectUpdatedCb(UAVObjEvent * ev) +static void objectUpdatedCb(UAVObjEvent *ev) { ObjectPersistenceData objper; UAVObjHandle obj; @@ -296,8 +296,9 @@ static void objectUpdatedCb(UAVObjEvent * ev) vTaskDelay(10); // Verify saving worked - if (retval == 0) + if (retval == 0) { retval = UAVObjLoad(obj, objper.InstanceID); + } } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) { retval = UAVObjSaveSettings(); } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) { @@ -321,20 +322,20 @@ static void objectUpdatedCb(UAVObjEvent * ev) #if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) retval = PIOS_FLASHFS_Format(0); #else - retval = -1; + retval = -1; #endif } switch (retval) { - case 0: - objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; - ObjectPersistenceSet(&objper); - break; - case -1: - objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR; - ObjectPersistenceSet(&objper); - break; - default: - break; + case 0: + objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; + ObjectPersistenceSet(&objper); + break; + case -1: + objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR; + ObjectPersistenceSet(&objper); + break; + default: + break; } } } @@ -342,9 +343,10 @@ static void objectUpdatedCb(UAVObjEvent * ev) /** * Called whenever hardware settings changed */ -static void hwSettingsUpdatedCb(__attribute__((unused))UAVObjEvent * ev) +static void hwSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { HwSettingsData currentHwSettings; + HwSettingsGet(¤tHwSettings); // check whether the Hw Configuration has changed from the one used at boot time if (memcmp(&bootHwSettings, ¤tHwSettings, sizeof(HwSettingsData)) != 0) { @@ -355,18 +357,19 @@ static void hwSettingsUpdatedCb(__attribute__((unused))UAVObjEvent * ev) #ifdef DIAG_TASKS static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_info *task_info, void *context) { - TaskInfoData *taskData = (TaskInfoData *)context; - // By convention, there is a direct mapping between task monitor task_id's and members - // of the TaskInfoXXXXElem enums - PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM); - taskData->Running[task_id] = task_info->is_running? TASKINFO_RUNNING_TRUE: TASKINFO_RUNNING_FALSE; - taskData->StackRemaining[task_id] = task_info->stack_remaining; - taskData->RunningTime[task_id] = task_info->running_time_percentage; + TaskInfoData *taskData = (TaskInfoData *)context; + + // By convention, there is a direct mapping between task monitor task_id's and members + // of the TaskInfoXXXXElem enums + PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM); + taskData->Running[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE; + taskData->StackRemaining[task_id] = task_info->stack_remaining; + taskData->RunningTime[task_id] = task_info->running_time_percentage; } #endif /** - * Called periodically to update the I2C statistics + * Called periodically to update the I2C statistics */ #ifdef DIAG_I2C_WDG_STATS static void updateI2Cstats() @@ -378,7 +381,7 @@ static void updateI2Cstats() struct pios_i2c_fault_history history; PIOS_I2C_GetDiagnostics(&history, &i2cStats.event_errors); - for(uint8_t i = 0; (i < I2C_LOG_DEPTH) && (i < I2CSTATS_EVENT_LOG_NUMELEM); i++) { + for (uint8_t i = 0; (i < I2C_LOG_DEPTH) && (i < I2CSTATS_EVENT_LOG_NUMELEM); i++) { i2cStats.evirq_log[i] = history.evirq[i]; i2cStats.erirq_log[i] = history.erirq[i]; i2cStats.event_log[i] = history.event[i]; @@ -392,11 +395,12 @@ static void updateI2Cstats() static void updateWDGstats() { WatchdogStatusData watchdogStatus; + watchdogStatus.BootupFlags = PIOS_WDG_GetBootupFlags(); watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags(); WatchdogStatusSet(&watchdogStatus); } -#endif +#endif /* ifdef DIAG_I2C_WDG_STATS */ /** * Called periodically to update the system stats @@ -408,35 +412,28 @@ static uint16_t GetFreeIrqStackSize(void) #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) extern uint32_t _irq_stack_top; extern uint32_t _irq_stack_end; - uint32_t pattern = 0x0000A5A5; - uint32_t *ptr = &_irq_stack_end; + uint32_t pattern = 0x0000A5A5; + uint32_t *ptr = &_irq_stack_end; #if 1 /* the ugly way accurate but takes more time, useful for debugging */ - uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3 ) / 4; + uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3) / 4; - for (i=0; i< stack_size; i++) - { - if (ptr[i] != pattern) - { - i=i*4; + for (i = 0; i < stack_size; i++) { + if (ptr[i] != pattern) { + i = i * 4; break; } } #else /* faster way but not accurate */ - if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern) - { + if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern) { i = IRQSTACK_LIMIT_CRITICAL - 1; - } - else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern) - { + } else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern) { i = IRQSTACK_LIMIT_WARNING - 1; - } - else - { + } else { i = IRQSTACK_LIMIT_WARNING; } #endif -#endif +#endif /* if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) */ return i; } @@ -450,7 +447,7 @@ static void updateStats() // Get stats and update SystemStatsGet(&stats); - stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS; + stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS; #if defined(ARCH_POSIX) || defined(ARCH_WIN32) // POSIX port of FreeRTOS doesn't have xPortGetFreeHeapSize() stats.HeapRemaining = 10240; @@ -468,10 +465,10 @@ static void updateStats() portTickType now = xTaskGetTickCount(); if (now > lastTickCount) { - uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms - stats.CPULoad = 100 - (uint8_t) roundf(100.0f * ((float) idleCounter / ((float) dT / 1000.0f)) / (float) IDLE_COUNTS_PER_SEC_AT_NO_LOAD); - } //else: TickCount has wrapped, do not calc now - lastTickCount = now; + uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms + stats.CPULoad = 100 - (uint8_t)roundf(100.0f * ((float)idleCounter / ((float)dT / 1000.0f)) / (float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD); + } // else: TickCount has wrapped, do not calc now + lastTickCount = now; idleCounterClear = 1; #if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR) @@ -479,12 +476,12 @@ static void updateStats() float temp_voltage = 3.3f * PIOS_ADC_PinGet(3) / ((1 << 12) - 1); const float STM32_TEMP_V25 = 0.76f; /* V */ const float STM32_TEMP_AVG_SLOPE = 2.5f; /* mV/C */ - stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f; + stats.CPUTemp = (temp_voltage - STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f; #else float temp_voltage = 3.3f * PIOS_ADC_PinGet(0) / ((1 << 12) - 1); const float STM32_TEMP_V25 = 1.43f; /* V */ const float STM32_TEMP_AVG_SLOPE = 4.3f; /* mV/C */ - stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f; + stats.CPUTemp = (temp_voltage - STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f; #endif #endif SystemStatsSet(&stats); @@ -498,20 +495,21 @@ static void updateSystemAlarms() SystemStatsData stats; UAVObjStats objStats; EventStats evStats; + SystemStatsGet(&stats); // Check heap, IRQ stack and malloc failures if (mallocFailed || (stats.HeapRemaining < HEAP_LIMIT_CRITICAL) #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) - || (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL) + || (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL) #endif - ) { + ) { AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL); } else if ((stats.HeapRemaining < HEAP_LIMIT_WARNING) #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) - || (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING) + || (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING) #endif - ) { + ) { AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY); @@ -547,12 +545,11 @@ static void updateSystemAlarms() if (objStats.lastCallbackErrorID || objStats.lastQueueErrorID || evStats.lastErrorID) { SystemStatsData sysStats; SystemStatsGet(&sysStats); - sysStats.EventSystemWarningID = evStats.lastErrorID; + sysStats.EventSystemWarningID = evStats.lastErrorID; sysStats.ObjectManagerCallbackID = objStats.lastCallbackErrorID; - sysStats.ObjectManagerQueueID = objStats.lastQueueErrorID; + sysStats.ObjectManagerQueueID = objStats.lastQueueErrorID; SystemStatsSet(&sysStats); } - } /** @@ -573,13 +570,15 @@ void vApplicationIdleHook(void) * Called by the RTOS when a stack overflow is detected. */ #define DEBUG_STACK_OVERFLOW 0 -void vApplicationStackOverflowHook(__attribute__((unused))xTaskHandle * pxTask, - __attribute__((unused))signed portCHAR * pcTaskName) +void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask, + __attribute__((unused)) signed portCHAR *pcTaskName) { stackOverflow = true; #if DEBUG_STACK_OVERFLOW static volatile bool wait_here = true; - while(wait_here); + while (wait_here) { + ; + } wait_here = true; #endif } @@ -593,7 +592,9 @@ void vApplicationMallocFailedHook(void) mallocFailed = true; #if DEBUG_MALLOC_FAILURES static volatile bool wait_here = true; - while(wait_here); + while (wait_here) { + ; + } wait_here = true; #endif } diff --git a/flight/modules/Telemetry/inc/telemetry.h b/flight/modules/Telemetry/inc/telemetry.h index b4e449c9c..c3a19cb95 100644 --- a/flight/modules/Telemetry/inc/telemetry.h +++ b/flight/modules/Telemetry/inc/telemetry.h @@ -1,18 +1,18 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup TelemetryModule Telemetry Module * @brief Main telemetry module * Starts three tasks (RX, TX, and priority TX) that watch event queues * and handle all the telemetry of the UAVobjects - * @{ + * @{ * * @file telemetry.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Include file of the telemetry module. - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and * objects. * @see The GNU Public License (GPL) Version 3 * @@ -41,6 +41,6 @@ int32_t TelemetryInitialize(void); #endif // TELEMETRY_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 5dc576abd..a2abfffe5 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -1,12 +1,12 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup TelemetryModule Telemetry Module * @brief Main telemetry module * Starts three tasks (RX, TX, and priority TX) that watch event queues * and handle all the telemetry of the UAVobjects - * @{ + * @{ * * @file telemetry.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -40,15 +40,15 @@ #include "taskinfo.h" // Private constants -#define MAX_QUEUE_SIZE TELEM_QUEUE_SIZE -#define STACK_SIZE_BYTES PIOS_TELEM_STACK_SIZE -#define TASK_PRIORITY_RX (tskIDLE_PRIORITY + 2) -#define TASK_PRIORITY_TX (tskIDLE_PRIORITY + 2) -#define TASK_PRIORITY_TXPRI (tskIDLE_PRIORITY + 2) -#define REQ_TIMEOUT_MS 250 -#define MAX_RETRIES 2 +#define MAX_QUEUE_SIZE TELEM_QUEUE_SIZE +#define STACK_SIZE_BYTES PIOS_TELEM_STACK_SIZE +#define TASK_PRIORITY_RX (tskIDLE_PRIORITY + 2) +#define TASK_PRIORITY_TX (tskIDLE_PRIORITY + 2) +#define TASK_PRIORITY_TXPRI (tskIDLE_PRIORITY + 2) +#define REQ_TIMEOUT_MS 250 +#define MAX_RETRIES 2 #define STATS_UPDATE_PERIOD_MS 4000 -#define CONNECTION_TIMEOUT_MS 8000 +#define CONNECTION_TIMEOUT_MS 8000 // Private types @@ -74,11 +74,11 @@ static UAVTalkConnection uavTalkCon; // Private functions static void telemetryTxTask(void *parameters); static void telemetryRxTask(void *parameters); -static int32_t transmitData(uint8_t * data, int32_t length); +static int32_t transmitData(uint8_t *data, int32_t length); static void registerObject(UAVObjHandle obj); static void updateObject(UAVObjHandle obj, int32_t eventType); static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs); -static void processObjEvent(UAVObjEvent * ev); +static void processObjEvent(UAVObjEvent *ev); static void updateTelemetryStats(); static void gcsTelemetryStatsUpdated(); static void updateSettings(); @@ -91,24 +91,24 @@ static uint32_t getComPort(); */ int32_t TelemetryStart(void) { - // Process all registered objects and connect queue for updates - UAVObjIterate(®isterObject); - - // Listen to objects of interest - GCSTelemetryStatsConnectQueue(priorityQueue); - - // Start telemetry tasks - xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle); - xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle); + // Process all registered objects and connect queue for updates + UAVObjIterate(®isterObject); + + // Listen to objects of interest + GCSTelemetryStatsConnectQueue(priorityQueue); + + // Start telemetry tasks + xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle); + xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle); #if defined(PIOS_TELEM_PRIORITY_QUEUE) - xTaskCreate(telemetryTxPriTask, (signed char *)"TelPriTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle); + xTaskCreate(telemetryTxPriTask, (signed char *)"TelPriTx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle); #endif - return 0; + return 0; } /** @@ -118,34 +118,34 @@ int32_t TelemetryStart(void) */ int32_t TelemetryInitialize(void) { - FlightTelemetryStatsInitialize(); - GCSTelemetryStatsInitialize(); + FlightTelemetryStatsInitialize(); + GCSTelemetryStatsInitialize(); - // Initialize vars - timeOfLastObjectUpdate = 0; + // Initialize vars + timeOfLastObjectUpdate = 0; - // Create object queues - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + // Create object queues + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); #if defined(PIOS_TELEM_PRIORITY_QUEUE) - priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); #endif - // Update telemetry settings - telemetryPort = PIOS_COM_TELEM_RF; - HwSettingsInitialize(); - updateSettings(); - - // Initialise UAVTalk - uavTalkCon = UAVTalkInitialize(&transmitData); - - // Create periodic event that will be used to update the telemetry stats - txErrors = 0; - txRetries = 0; - UAVObjEvent ev; - memset(&ev, 0, sizeof(UAVObjEvent)); - EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS); + // Update telemetry settings + telemetryPort = PIOS_COM_TELEM_RF; + HwSettingsInitialize(); + updateSettings(); - return 0; + // Initialise UAVTalk + uavTalkCon = UAVTalkInitialize(&transmitData); + + // Create periodic event that will be used to update the telemetry stats + txErrors = 0; + txRetries = 0; + UAVObjEvent ev; + memset(&ev, 0, sizeof(UAVObjEvent)); + EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS); + + return 0; } MODULE_INITCALL(TelemetryInitialize, TelemetryStart) @@ -157,31 +157,31 @@ MODULE_INITCALL(TelemetryInitialize, TelemetryStart) */ static void registerObject(UAVObjHandle obj) { - if (UAVObjIsMetaobject(obj)) { - /* Only connect change notifications for meta objects. No periodic updates */ - UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES); - return; - } else { - UAVObjMetadata metadata; - UAVObjUpdateMode updateMode; - UAVObjGetMetadata(obj, &metadata); - updateMode = UAVObjGetTelemetryUpdateMode(&metadata); + if (UAVObjIsMetaobject(obj)) { + /* Only connect change notifications for meta objects. No periodic updates */ + UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES); + return; + } else { + UAVObjMetadata metadata; + UAVObjUpdateMode updateMode; + UAVObjGetMetadata(obj, &metadata); + updateMode = UAVObjGetTelemetryUpdateMode(&metadata); - /* Only create a periodic event for objects that are periodic */ - if ((updateMode == UPDATEMODE_PERIODIC) || - (updateMode == UPDATEMODE_THROTTLED)) { - // Setup object for periodic updates - UAVObjEvent ev = { - .obj = obj, - .instId = UAVOBJ_ALL_INSTANCES, - .event = EV_UPDATED_PERIODIC, - }; - EventPeriodicQueueCreate(&ev, queue, 0); - } + /* Only create a periodic event for objects that are periodic */ + if ((updateMode == UPDATEMODE_PERIODIC) || + (updateMode == UPDATEMODE_THROTTLED)) { + // Setup object for periodic updates + UAVObjEvent ev = { + .obj = obj, + .instId = UAVOBJ_ALL_INSTANCES, + .event = EV_UPDATED_PERIODIC, + }; + EventPeriodicQueueCreate(&ev, queue, 0); + } - // Setup object for telemetry updates - updateObject(obj, EV_NONE); - } + // Setup object for telemetry updates + updateObject(obj, EV_NONE); + } } /** @@ -190,118 +190,119 @@ static void registerObject(UAVObjHandle obj) */ static void updateObject(UAVObjHandle obj, int32_t eventType) { - UAVObjMetadata metadata; - UAVObjUpdateMode updateMode; - int32_t eventMask; + UAVObjMetadata metadata; + UAVObjUpdateMode updateMode; + int32_t eventMask; - if (UAVObjIsMetaobject(obj)) { - /* This function updates the periodic updates for the object. - * Meta Objects cannot have periodic updates. - */ - PIOS_Assert(false); - return; - } + if (UAVObjIsMetaobject(obj)) { + /* This function updates the periodic updates for the object. + * Meta Objects cannot have periodic updates. + */ + PIOS_Assert(false); + return; + } - // Get metadata - UAVObjGetMetadata(obj, &metadata); - updateMode = UAVObjGetTelemetryUpdateMode(&metadata); + // Get metadata + UAVObjGetMetadata(obj, &metadata); + updateMode = UAVObjGetTelemetryUpdateMode(&metadata); - // Setup object depending on update mode - switch (updateMode) { - case UPDATEMODE_PERIODIC: - // Set update period - setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); - // Connect queue - eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - UAVObjConnectQueue(obj, priorityQueue, eventMask); - break; - case UPDATEMODE_ONCHANGE: - // Set update period - setUpdatePeriod(obj, 0); - // Connect queue - eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - UAVObjConnectQueue(obj, priorityQueue, eventMask); - break; - case UPDATEMODE_THROTTLED: - if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) { - // If we received a periodic update, we can change back to update on change - eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - // Set update period on initialization and metadata change - if (eventType == EV_NONE) - setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); - } else { - // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates - eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - } - UAVObjConnectQueue(obj, priorityQueue, eventMask); - break; - case UPDATEMODE_MANUAL: - // Set update period - setUpdatePeriod(obj, 0); - // Connect queue - eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ; - UAVObjConnectQueue(obj, priorityQueue, eventMask); - break; - } + // Setup object depending on update mode + switch (updateMode) { + case UPDATEMODE_PERIODIC: + // Set update period + setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); + // Connect queue + eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ; + UAVObjConnectQueue(obj, priorityQueue, eventMask); + break; + case UPDATEMODE_ONCHANGE: + // Set update period + setUpdatePeriod(obj, 0); + // Connect queue + eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; + UAVObjConnectQueue(obj, priorityQueue, eventMask); + break; + case UPDATEMODE_THROTTLED: + if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) { + // If we received a periodic update, we can change back to update on change + eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; + // Set update period on initialization and metadata change + if (eventType == EV_NONE) { + setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); + } + } else { + // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates + eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ; + } + UAVObjConnectQueue(obj, priorityQueue, eventMask); + break; + case UPDATEMODE_MANUAL: + // Set update period + setUpdatePeriod(obj, 0); + // Connect queue + eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ; + UAVObjConnectQueue(obj, priorityQueue, eventMask); + break; + } } /** * Processes queue events */ -static void processObjEvent(UAVObjEvent * ev) +static void processObjEvent(UAVObjEvent *ev) { - UAVObjMetadata metadata; - UAVObjUpdateMode updateMode; - FlightTelemetryStatsData flightStats; - int32_t retries; - int32_t success; + UAVObjMetadata metadata; + UAVObjUpdateMode updateMode; + FlightTelemetryStatsData flightStats; + int32_t retries; + int32_t success; - if (ev->obj == 0) { - updateTelemetryStats(); - } else if (ev->obj == GCSTelemetryStatsHandle()) { - gcsTelemetryStatsUpdated(); - } else { - FlightTelemetryStatsGet(&flightStats); - // Get object metadata - UAVObjGetMetadata(ev->obj, &metadata); - updateMode = UAVObjGetTelemetryUpdateMode(&metadata); + if (ev->obj == 0) { + updateTelemetryStats(); + } else if (ev->obj == GCSTelemetryStatsHandle()) { + gcsTelemetryStatsUpdated(); + } else { + FlightTelemetryStatsGet(&flightStats); + // Get object metadata + UAVObjGetMetadata(ev->obj, &metadata); + updateMode = UAVObjGetTelemetryUpdateMode(&metadata); - // Act on event - retries = 0; - success = -1; - if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) { - // Send update to GCS (with retries) - while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout - ++retries; - } - // Update stats - txRetries += (retries - 1); - if (success == -1) { - ++txErrors; - } - } else if (ev->event == EV_UPDATE_REQ) { - // Request object update from GCS (with retries) - while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout - ++retries; - } - // Update stats - txRetries += (retries - 1); - if (success == -1) { - ++txErrors; - } - } - // If this is a metaobject then make necessary telemetry updates - if (UAVObjIsMetaobject(ev->obj)) { - updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); // linked object will be the actual object the metadata are for - } else { - if (updateMode == UPDATEMODE_THROTTLED) { - // If this is UPDATEMODE_THROTTLED, the event mask changes on every event. - updateObject(ev->obj, ev->event); - } - } - } + // Act on event + retries = 0; + success = -1; + if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) { + // Send update to GCS (with retries) + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout + ++retries; + } + // Update stats + txRetries += (retries - 1); + if (success == -1) { + ++txErrors; + } + } else if (ev->event == EV_UPDATE_REQ) { + // Request object update from GCS (with retries) + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout + ++retries; + } + // Update stats + txRetries += (retries - 1); + if (success == -1) { + ++txErrors; + } + } + // If this is a metaobject then make necessary telemetry updates + if (UAVObjIsMetaobject(ev->obj)) { + updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); // linked object will be the actual object the metadata are for + } else { + if (updateMode == UPDATEMODE_THROTTLED) { + // If this is UPDATEMODE_THROTTLED, the event mask changes on every event. + updateObject(ev->obj, ev->event); + } + } + } } /** @@ -309,16 +310,16 @@ static void processObjEvent(UAVObjEvent * ev) */ static void telemetryTxTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; + UAVObjEvent ev; - // Loop forever - while (1) { - // Wait for queue message - if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) { - // Process event - processObjEvent(&ev); - } - } + // Loop forever + while (1) { + // Wait for queue message + if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) { + // Process event + processObjEvent(&ev); + } + } } /** @@ -327,16 +328,16 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) #if defined(PIOS_TELEM_PRIORITY_QUEUE) static void telemetryTxPriTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; + UAVObjEvent ev; - // Loop forever - while (1) { - // Wait for queue message - if (xQueueReceive(priorityQueue, &ev, portMAX_DELAY) == pdTRUE) { - // Process event - processObjEvent(&ev); - } - } + // Loop forever + while (1) { + // Wait for queue message + if (xQueueReceive(priorityQueue, &ev, portMAX_DELAY) == pdTRUE) { + // Process event + processObjEvent(&ev); + } + } } #endif @@ -345,26 +346,25 @@ static void telemetryTxPriTask(__attribute__((unused)) void *parameters) */ static void telemetryRxTask(__attribute__((unused)) void *parameters) { + // Task loop + while (1) { + uint32_t inputPort = getComPort(); - // Task loop - while (1) { - uint32_t inputPort = getComPort(); + if (inputPort) { + // Block until data are available + uint8_t serial_data[1]; + uint16_t bytes_to_process; - if (inputPort) { - // Block until data are available - uint8_t serial_data[1]; - uint16_t bytes_to_process; - - bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500); - if (bytes_to_process > 0) { - for (uint8_t i = 0; i < bytes_to_process; i++) { - UAVTalkProcessInputStream(uavTalkCon,serial_data[i]); - } - } - } else { - vTaskDelay(5); - } - } + bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500); + if (bytes_to_process > 0) { + for (uint8_t i = 0; i < bytes_to_process; i++) { + UAVTalkProcessInputStream(uavTalkCon, serial_data[i]); + } + } + } else { + vTaskDelay(5); + } + } } /** @@ -374,14 +374,15 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) * \return -1 on failure * \return number of bytes transmitted on success */ -static int32_t transmitData(uint8_t * data, int32_t length) +static int32_t transmitData(uint8_t *data, int32_t length) { - uint32_t outputPort = getComPort(); + uint32_t outputPort = getComPort(); - if (outputPort) - return PIOS_COM_SendBuffer(outputPort, data, length); + if (outputPort) { + return PIOS_COM_SendBuffer(outputPort, data, length); + } - return -1; + return -1; } /** @@ -393,13 +394,13 @@ static int32_t transmitData(uint8_t * data, int32_t length) */ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs) { - UAVObjEvent ev; + UAVObjEvent ev; - // Add object for periodic updates - ev.obj = obj; - ev.instId = UAVOBJ_ALL_INSTANCES; - ev.event = EV_UPDATED_PERIODIC; - return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs); + // Add object for periodic updates + ev.obj = obj; + ev.instId = UAVOBJ_ALL_INSTANCES; + ev.event = EV_UPDATED_PERIODIC; + return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs); } /** @@ -409,13 +410,14 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs) */ static void gcsTelemetryStatsUpdated() { - FlightTelemetryStatsData flightStats; - GCSTelemetryStatsData gcsStats; - FlightTelemetryStatsGet(&flightStats); - GCSTelemetryStatsGet(&gcsStats); - if (flightStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED) { - updateTelemetryStats(); - } + FlightTelemetryStatsData flightStats; + GCSTelemetryStatsData gcsStats; + + FlightTelemetryStatsGet(&flightStats); + GCSTelemetryStatsGet(&gcsStats); + if (flightStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED) { + updateTelemetryStats(); + } } /** @@ -423,89 +425,89 @@ static void gcsTelemetryStatsUpdated() */ static void updateTelemetryStats() { - UAVTalkStats utalkStats; - FlightTelemetryStatsData flightStats; - GCSTelemetryStatsData gcsStats; - uint8_t forceUpdate; - uint8_t connectionTimeout; - uint32_t timeNow; + UAVTalkStats utalkStats; + FlightTelemetryStatsData flightStats; + GCSTelemetryStatsData gcsStats; + uint8_t forceUpdate; + uint8_t connectionTimeout; + uint32_t timeNow; - // Get stats - UAVTalkGetStats(uavTalkCon, &utalkStats); - UAVTalkResetStats(uavTalkCon); + // Get stats + UAVTalkGetStats(uavTalkCon, &utalkStats); + UAVTalkResetStats(uavTalkCon); - // Get object data - FlightTelemetryStatsGet(&flightStats); - GCSTelemetryStatsGet(&gcsStats); + // Get object data + FlightTelemetryStatsGet(&flightStats); + GCSTelemetryStatsGet(&gcsStats); - // Update stats object - if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { - flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); - flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); - flightStats.RxFailures += utalkStats.rxErrors; - flightStats.TxFailures += txErrors; - flightStats.TxRetries += txRetries; - txErrors = 0; - txRetries = 0; - } else { - flightStats.RxDataRate = 0; - flightStats.TxDataRate = 0; - flightStats.RxFailures = 0; - flightStats.TxFailures = 0; - flightStats.TxRetries = 0; - txErrors = 0; - txRetries = 0; - } + // Update stats object + if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { + flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); + flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); + flightStats.RxFailures += utalkStats.rxErrors; + flightStats.TxFailures += txErrors; + flightStats.TxRetries += txRetries; + txErrors = 0; + txRetries = 0; + } else { + flightStats.RxDataRate = 0; + flightStats.TxDataRate = 0; + flightStats.RxFailures = 0; + flightStats.TxFailures = 0; + flightStats.TxRetries = 0; + txErrors = 0; + txRetries = 0; + } - // Check for connection timeout - timeNow = xTaskGetTickCount() * portTICK_RATE_MS; - if (utalkStats.rxObjects > 0) { - timeOfLastObjectUpdate = timeNow; - } - if ((timeNow - timeOfLastObjectUpdate) > CONNECTION_TIMEOUT_MS) { - connectionTimeout = 1; - } else { - connectionTimeout = 0; - } + // Check for connection timeout + timeNow = xTaskGetTickCount() * portTICK_RATE_MS; + if (utalkStats.rxObjects > 0) { + timeOfLastObjectUpdate = timeNow; + } + if ((timeNow - timeOfLastObjectUpdate) > CONNECTION_TIMEOUT_MS) { + connectionTimeout = 1; + } else { + connectionTimeout = 0; + } - // Update connection state - forceUpdate = 1; - if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED) { - // Wait for connection request - if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_HANDSHAKEREQ) { - flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK; - } - } else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK) { - // Wait for connection - if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_CONNECTED) { - flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED; - } else if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_DISCONNECTED) { - flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED; - } - } else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { - if (gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED || connectionTimeout) { - flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED; - } else { - forceUpdate = 0; - } - } else { - flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED; - } + // Update connection state + forceUpdate = 1; + if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED) { + // Wait for connection request + if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_HANDSHAKEREQ) { + flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK; + } + } else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK) { + // Wait for connection + if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_CONNECTED) { + flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED; + } else if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_DISCONNECTED) { + flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED; + } + } else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { + if (gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED || connectionTimeout) { + flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED; + } else { + forceUpdate = 0; + } + } else { + flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED; + } - // Update the telemetry alarm - if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { - AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR); - } + // Update the telemetry alarm + if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { + AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR); + } - // Update object - FlightTelemetryStatsSet(&flightStats); + // Update object + FlightTelemetryStatsSet(&flightStats); - // Force telemetry update if not connected - if (forceUpdate) { - FlightTelemetryStatsUpdated(); - } + // Force telemetry update if not connected + if (forceUpdate) { + FlightTelemetryStatsUpdated(); + } } /** @@ -517,55 +519,56 @@ static void updateTelemetryStats() */ static void updateSettings() { - - if (telemetryPort) { - // Retrieve settings - uint8_t speed; - HwSettingsTelemetrySpeedGet(&speed); + if (telemetryPort) { + // Retrieve settings + uint8_t speed; + HwSettingsTelemetrySpeedGet(&speed); - // Set port speed - switch (speed) { - case HWSETTINGS_TELEMETRYSPEED_2400: - PIOS_COM_ChangeBaud(telemetryPort, 2400); - break; - case HWSETTINGS_TELEMETRYSPEED_4800: - PIOS_COM_ChangeBaud(telemetryPort, 4800); - break; - case HWSETTINGS_TELEMETRYSPEED_9600: - PIOS_COM_ChangeBaud(telemetryPort, 9600); - break; - case HWSETTINGS_TELEMETRYSPEED_19200: - PIOS_COM_ChangeBaud(telemetryPort, 19200); - break; - case HWSETTINGS_TELEMETRYSPEED_38400: - PIOS_COM_ChangeBaud(telemetryPort, 38400); - break; - case HWSETTINGS_TELEMETRYSPEED_57600: - PIOS_COM_ChangeBaud(telemetryPort, 57600); - break; - case HWSETTINGS_TELEMETRYSPEED_115200: - PIOS_COM_ChangeBaud(telemetryPort, 115200); - break; - } - } + // Set port speed + switch (speed) { + case HWSETTINGS_TELEMETRYSPEED_2400: + PIOS_COM_ChangeBaud(telemetryPort, 2400); + break; + case HWSETTINGS_TELEMETRYSPEED_4800: + PIOS_COM_ChangeBaud(telemetryPort, 4800); + break; + case HWSETTINGS_TELEMETRYSPEED_9600: + PIOS_COM_ChangeBaud(telemetryPort, 9600); + break; + case HWSETTINGS_TELEMETRYSPEED_19200: + PIOS_COM_ChangeBaud(telemetryPort, 19200); + break; + case HWSETTINGS_TELEMETRYSPEED_38400: + PIOS_COM_ChangeBaud(telemetryPort, 38400); + break; + case HWSETTINGS_TELEMETRYSPEED_57600: + PIOS_COM_ChangeBaud(telemetryPort, 57600); + break; + case HWSETTINGS_TELEMETRYSPEED_115200: + PIOS_COM_ChangeBaud(telemetryPort, 115200); + break; + } + } } /** - * Determine input/output com port as highest priority available + * Determine input/output com port as highest priority available */ -static uint32_t getComPort() { +static uint32_t getComPort() +{ #if defined(PIOS_INCLUDE_USB) - if ( PIOS_COM_Available(PIOS_COM_TELEM_USB) ) - return PIOS_COM_TELEM_USB; - else + if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) { + return PIOS_COM_TELEM_USB; + } else #endif /* PIOS_INCLUDE_USB */ - if ( PIOS_COM_Available(telemetryPort) ) - return telemetryPort; - else - return 0; + if (PIOS_COM_Available(telemetryPort)) { + return telemetryPort; + } else { + return 0; + } } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/TxPID/inc/txpid.h b/flight/modules/TxPID/inc/txpid.h index c5a2e4fa3..44e61cbc1 100644 --- a/flight/modules/TxPID/inc/txpid.h +++ b/flight/modules/TxPID/inc/txpid.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup TxPIDModule TxPID Module - * @{ + * @{ * * @file txpid.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. @@ -37,6 +37,6 @@ int32_t TxPIDInitialize(void); #endif // TXPID_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/TxPID/txpid.c b/flight/modules/TxPID/txpid.c index 4c784894b..9f9c5f2cf 100644 --- a/flight/modules/TxPID/txpid.c +++ b/flight/modules/TxPID/txpid.c @@ -62,8 +62,8 @@ // // Configuration // -#define SAMPLE_PERIOD_MS 200 -#define TELEMETRY_UPDATE_PERIOD_MS 0 // 0 = update on change (default) +#define SAMPLE_PERIOD_MS 200 +#define TELEMETRY_UPDATE_PERIOD_MS 0 // 0 = update on change (default) // Sanity checks #if (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_INPUTS_NUMELEM) || \ @@ -77,7 +77,7 @@ // Private variables // Private functions -static void updatePIDs(UAVObjEvent* ev); +static void updatePIDs(UAVObjEvent *ev); static uint8_t update(float *var, float val); static float scale(float val, float inMin, float inMax, float outMin, float outMax); @@ -87,55 +87,55 @@ static float scale(float val, float inMin, float inMax, float outMin, float outM */ int32_t TxPIDInitialize(void) { - bool txPIDEnabled; - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + bool txPIDEnabled; + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsInitialize(); - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsInitialize(); + HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_TXPID] == HWSETTINGS_OPTIONALMODULES_ENABLED) - txPIDEnabled = true; - else - txPIDEnabled = false; + if (optionalModules[HWSETTINGS_OPTIONALMODULES_TXPID] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + txPIDEnabled = true; + } else { + txPIDEnabled = false; + } - if (txPIDEnabled) { + if (txPIDEnabled) { + TxPIDSettingsInitialize(); + AccessoryDesiredInitialize(); - TxPIDSettingsInitialize(); - AccessoryDesiredInitialize(); - - UAVObjEvent ev = { - .obj = AccessoryDesiredHandle(), - .instId = 0, - .event = 0, - }; - EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS); + UAVObjEvent ev = { + .obj = AccessoryDesiredHandle(), + .instId = 0, + .event = 0, + }; + EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS); #if (TELEMETRY_UPDATE_PERIOD_MS != 0) - // Change StabilizationSettings update rate from OnChange to periodic - // to prevent telemetry link flooding with frequent updates in case of - // control channel jitter. - // Warning: saving to flash with this code active will change the - // StabilizationSettings update rate permanently. Use Metadata via - // browser to reset to defaults (telemetryAcked=true, OnChange). - UAVObjMetadata metadata; - StabilizationSettingsInitialize(); - StabilizationSettingsGetMetadata(&metadata); - metadata.telemetryAcked = 0; - metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC; - metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS; - StabilizationSettingsSetMetadata(&metadata); + // Change StabilizationSettings update rate from OnChange to periodic + // to prevent telemetry link flooding with frequent updates in case of + // control channel jitter. + // Warning: saving to flash with this code active will change the + // StabilizationSettings update rate permanently. Use Metadata via + // browser to reset to defaults (telemetryAcked=true, OnChange). + UAVObjMetadata metadata; + StabilizationSettingsInitialize(); + StabilizationSettingsGetMetadata(&metadata); + metadata.telemetryAcked = 0; + metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC; + metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS; + StabilizationSettingsSetMetadata(&metadata); #endif - return 0; - } + return 0; + } - return -1; + return -1; } /* stub: module has no module thread */ int32_t TxPIDStart(void) { - return 0; + return 0; } MODULE_INITCALL(TxPIDInitialize, TxPIDStart) @@ -143,148 +143,151 @@ MODULE_INITCALL(TxPIDInitialize, TxPIDStart) /** * Update PIDs callback function */ -static void updatePIDs(UAVObjEvent* ev) +static void updatePIDs(UAVObjEvent *ev) { - if (ev->obj != AccessoryDesiredHandle()) - return; + if (ev->obj != AccessoryDesiredHandle()) { + return; + } - TxPIDSettingsData inst; - TxPIDSettingsGet(&inst); + TxPIDSettingsData inst; + TxPIDSettingsGet(&inst); - if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER) - return; + if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER) { + return; + } - uint8_t armed; - FlightStatusArmedGet(&armed); - if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) && - (armed == FLIGHTSTATUS_ARMED_DISARMED)) - return; + uint8_t armed; + FlightStatusArmedGet(&armed); + if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) && + (armed == FLIGHTSTATUS_ARMED_DISARMED)) { + return; + } - StabilizationSettingsData stab; - StabilizationSettingsGet(&stab); - AccessoryDesiredData accessory; + StabilizationSettingsData stab; + StabilizationSettingsGet(&stab); + AccessoryDesiredData accessory; - uint8_t needsUpdate = 0; + uint8_t needsUpdate = 0; - // Loop through every enabled instance - for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) { - if (inst.PIDs[i] != TXPIDSETTINGS_PIDS_DISABLED) { + // Loop through every enabled instance + for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) { + if (inst.PIDs[i] != TXPIDSETTINGS_PIDS_DISABLED) { + float value; + if (inst.Inputs[i] == TXPIDSETTINGS_INPUTS_THROTTLE) { + ManualControlCommandThrottleGet(&value); + value = scale(value, + inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MIN], + inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX], + inst.MinPID[i], inst.MaxPID[i]); + } else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) { + value = scale(accessory.AccessoryVal, -1.0f, 1.0f, inst.MinPID[i], inst.MaxPID[i]); + } else { + continue; + } - float value; - if (inst.Inputs[i] == TXPIDSETTINGS_INPUTS_THROTTLE) { - ManualControlCommandThrottleGet(&value); - value = scale(value, - inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MIN], - inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX], - inst.MinPID[i], inst.MaxPID[i]); - } else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) { - value = scale(accessory.AccessoryVal, -1.0f, 1.0f, inst.MinPID[i], inst.MaxPID[i]); - } else { - continue; - } - - switch (inst.PIDs[i]) { - case TXPIDSETTINGS_PIDS_ROLLRATEKP: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); - break; - case TXPIDSETTINGS_PIDS_ROLLRATEKI: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); - break; - case TXPIDSETTINGS_PIDS_ROLLRATEKD: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); - break; - case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); - break; - case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); - break; - case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_PITCHRATEKP: - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); - break; - case TXPIDSETTINGS_PIDS_PITCHRATEKI: - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); - break; - case TXPIDSETTINGS_PIDS_PITCHRATEKD: - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); - break; - case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT: - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP: - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); - break; - case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI: - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); - break; - case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT: - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); - break; - case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); - break; - case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); - break; - case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value); - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); - break; - case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); - break; - case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value); - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_YAWRATEKP: - needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value); - break; - case TXPIDSETTINGS_PIDS_YAWRATEKI: - needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value); - break; - case TXPIDSETTINGS_PIDS_YAWRATEKD: - needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value); - break; - case TXPIDSETTINGS_PIDS_YAWRATEILIMIT: - needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_YAWATTITUDEKP: - needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value); - break; - case TXPIDSETTINGS_PIDS_YAWATTITUDEKI: - needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value); - break; - case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT: - needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_GYROTAU: - needsUpdate |= update(&stab.GyroTau, value); - break; - default: - PIOS_Assert(0); - } - } - } - if (needsUpdate) - StabilizationSettingsSet(&stab); + switch (inst.PIDs[i]) { + case TXPIDSETTINGS_PIDS_ROLLRATEKP: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); + break; + case TXPIDSETTINGS_PIDS_ROLLRATEKI: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); + break; + case TXPIDSETTINGS_PIDS_ROLLRATEKD: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); + break; + case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); + break; + case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); + break; + case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_PITCHRATEKP: + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); + break; + case TXPIDSETTINGS_PIDS_PITCHRATEKI: + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); + break; + case TXPIDSETTINGS_PIDS_PITCHRATEKD: + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); + break; + case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT: + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP: + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); + break; + case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI: + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); + break; + case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT: + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value); + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value); + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_YAWRATEKP: + needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value); + break; + case TXPIDSETTINGS_PIDS_YAWRATEKI: + needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value); + break; + case TXPIDSETTINGS_PIDS_YAWRATEKD: + needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value); + break; + case TXPIDSETTINGS_PIDS_YAWRATEILIMIT: + needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_YAWATTITUDEKP: + needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value); + break; + case TXPIDSETTINGS_PIDS_YAWATTITUDEKI: + needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value); + break; + case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT: + needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_GYROTAU: + needsUpdate |= update(&stab.GyroTau, value); + break; + default: + PIOS_Assert(0); + } + } + } + if (needsUpdate) { + StabilizationSettingsSet(&stab); + } } /** @@ -296,25 +299,30 @@ static void updatePIDs(UAVObjEvent* ev) */ static float scale(float val, float inMin, float inMax, float outMin, float outMax) { - // bound input value - if (val > inMax) val = inMax; - if (val < inMin) val = inMin; + // bound input value + if (val > inMax) { + val = inMax; + } + if (val < inMin) { + val = inMin; + } - // normalize input value to [0..1] - if (inMax <= inMin) - val = 0.0f; - else - val = (val - inMin) / (inMax - inMin); + // normalize input value to [0..1] + if (inMax <= inMin) { + val = 0.0f; + } else { + val = (val - inMin) / (inMax - inMin); + } - // update output bounds - if (outMin > outMax) { - float t = outMin; - outMin = outMax; - outMax = t; - val = 1.0f - val; - } + // update output bounds + if (outMin > outMax) { + float t = outMin; + outMin = outMax; + outMax = t; + val = 1.0f - val; + } - return (outMax - outMin) * val + outMin; + return (outMax - outMin) * val + outMin; } /** @@ -323,22 +331,22 @@ static float scale(float val, float inMin, float inMax, float outMin, float outM */ static uint8_t update(float *var, float val) { - /* FIXME: this is not an entirely correct way - * to check if the two floating point - * numbers are 'not equal'. - * Epsilon of 1e-9 is probably okay for the range - * of numbers we see here*/ - if (fabsf(*var - val) > 1e-9f) { - *var = val; - return 1; - } - return 0; + /* FIXME: this is not an entirely correct way + * to check if the two floating point + * numbers are 'not equal'. + * Epsilon of 1e-9 is probably okay for the range + * of numbers we see here*/ + if (fabsf(*var - val) > 1e-9f) { + *var = val; + return 1; + } + return 0; } -/** - * @} - */ - +/** + * @} + */ + /** * @} */ diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index f18be2605..b07601c0c 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -3,7 +3,7 @@ * * @file vtolpathfollower.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief This module compared @ref PositionActual to @ref PathDesired + * @brief This module compared @ref PositionActual to @ref PathDesired * and sets @ref Stabilization. It only does this when the FlightMode field * of @ref FlightStatus is PathPlanner or RTH. * @@ -32,7 +32,7 @@ * Input object: PositionActual * Output object: StabilizationDesired * - * This module will periodically update the value of the @ref StabilizationDesired object based on + * This module will periodically update the value of the @ref StabilizationDesired object based on * @ref PathDesired and @PositionActual when the Flight Mode selected in @FlightStatus is supported * by this module. Otherwise another module (e.g. @ref ManualControlCommand) is expected to be * writing to @ref StabilizationDesired. @@ -53,7 +53,7 @@ #include "accels.h" #include "attitudeactual.h" #include "hwsettings.h" -#include "pathdesired.h" // object that will be updated by the module +#include "pathdesired.h" // object that will be updated by the module #include "positionactual.h" #include "manualcontrol.h" #include "flightstatus.h" @@ -79,9 +79,9 @@ #include "accessorydesired.h" // Private constants -#define MAX_QUEUE_SIZE 4 +#define MAX_QUEUE_SIZE 4 #define STACK_SIZE_BYTES 1548 -#define TASK_PRIORITY (tskIDLE_PRIORITY+2) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) // Private types @@ -93,16 +93,16 @@ static float poiRadius; // Private functions static void vtolPathFollowerTask(void *parameters); -static void SettingsUpdatedCb(UAVObjEvent * ev); +static void SettingsUpdatedCb(UAVObjEvent *ev); static void updateNedAccel(); static void updatePOIBearing(); static void updatePathVelocity(); static void updateEndpointVelocity(); -static void updateFixedAttitude(float* attitude); +static void updateFixedAttitude(float *attitude); static void updateVtolDesiredAttitude(bool yaw_attitude); static float bound(float val, float min, float max); static bool vtolpathfollower_enabled; -static void accessoryUpdated(UAVObjEvent* ev); +static void accessoryUpdated(UAVObjEvent *ev); /** * Initialise the module, called on startup @@ -147,17 +147,17 @@ int32_t VtolPathFollowerInitialize() return 0; } -MODULE_INITCALL( VtolPathFollowerInitialize, VtolPathFollowerStart) +MODULE_INITCALL(VtolPathFollowerInitialize, VtolPathFollowerStart) static float northVelIntegral = 0; -static float eastVelIntegral = 0; -static float downVelIntegral = 0; +static float eastVelIntegral = 0; +static float downVelIntegral = 0; static float northPosIntegral = 0; -static float eastPosIntegral = 0; -static float downPosIntegral = 0; +static float eastPosIntegral = 0; +static float downPosIntegral = 0; -static float throttleOffset = 0; +static float throttleOffset = 0; /** * Module thread, should not return. */ @@ -176,19 +176,18 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) // Main task loop lastUpdateTime = xTaskGetTickCount(); while (1) { - // Conditions when this runs: // 1. Must have VTOL type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR - // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path + // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path SystemSettingsGet(&systemSettings); if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI)) { + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI)) { AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); vTaskDelay(1000); continue; @@ -207,72 +206,71 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) // Check the combinations of flightmode and pathdesired mode switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_LAND: - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updateEndpointVelocity(); - updateVtolDesiredAttitude(false); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); - } + case FLIGHTSTATUS_FLIGHTMODE_LAND: + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { + updateEndpointVelocity(); + updateVtolDesiredAttitude(false); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + } + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + pathStatus.UID = pathDesired.UID; + pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; + switch (pathDesired.Mode) { + // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + updatePathVelocity(); + updateVtolDesiredAttitude(false); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); break; - case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: - pathStatus.UID = pathDesired.UID; - pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; - switch (pathDesired.Mode) { - // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - updatePathVelocity(); - updateVtolDesiredAttitude(false); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_FIXEDATTITUDE: - updateFixedAttitude(pathDesired.ModeParameters); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_DISARMALARM: - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); - break; - default: - pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); - break; - } - PathStatusSet(&pathStatus); + case PATHDESIRED_MODE_FIXEDATTITUDE: + updateFixedAttitude(pathDesired.ModeParameters); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); break; - case FLIGHTSTATUS_FLIGHTMODE_POI: - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updateEndpointVelocity(); - updateVtolDesiredAttitude(true); - updatePOIBearing(); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); - } + case PATHDESIRED_MODE_DISARMALARM: + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); break; default: - // Be cleaner and get rid of global variables - northVelIntegral = 0; - eastVelIntegral = 0; - downVelIntegral = 0; - northPosIntegral = 0; - eastPosIntegral = 0; - downPosIntegral = 0; - - // Track throttle before engaging this mode. Cheap system ident - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); - throttleOffset = stabDesired.Throttle; - + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); break; + } + PathStatusSet(&pathStatus); + break; + case FLIGHTSTATUS_FLIGHTMODE_POI: + if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { + updateEndpointVelocity(); + updateVtolDesiredAttitude(true); + updatePOIBearing(); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + } + break; + default: + // Be cleaner and get rid of global variables + northVelIntegral = 0; + eastVelIntegral = 0; + downVelIntegral = 0; + northPosIntegral = 0; + eastPosIntegral = 0; + downPosIntegral = 0; + + // Track throttle before engaging this mode. Cheap system ident + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + throttleOffset = stabDesired.Throttle; + + break; } AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE); - } } @@ -282,10 +280,11 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) static void updatePOIBearing() { const float DEADBAND_HIGH = 0.10f; - const float DEADBAND_LOW = -0.10f; + const float DEADBAND_LOW = -0.10f; float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); PositionActualData positionActual; PositionActualGet(&positionActual); @@ -305,9 +304,9 @@ static void updatePOIBearing() dLoc[2] = positionActual.Down - poi.Down; if (dLoc[1] < 0) { - yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) + 180.0f; + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f; } else { - yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) - 180.0f; + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f; } // distance @@ -339,23 +338,23 @@ static void updatePOIBearing() if (poiRadius >= 3.0f || changeRadius > 0) { if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) { pathDesired.End[PATHDESIRED_END_NORTH] = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); - pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); pathDesired.StartingVelocity = 1.0f; - pathDesired.EndingVelocity = 0.0f; + pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); } } - //not above + // not above if (distance >= 3.0f) { - //You can feed this into camerastabilization + // You can feed this into camerastabilization /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/ stabDesired.Yaw = yaw + (pathAngle / 2.0f); stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - //cameraDesired.Yaw=yaw; - //cameraDesired.PitchOrServo2=elevation; + // cameraDesired.Yaw=yaw; + // cameraDesired.PitchOrServo2=elevation; CameraDesiredSet(&cameraDesired); StabilizationDesiredSet(&stabDesired); @@ -365,7 +364,7 @@ static void updatePOIBearing() /** * Compute desired velocity from the current position and path * - * Takes in @ref PositionActual and compares it to @ref PathDesired + * Takes in @ref PositionActual and compares it to @ref PathDesired * and computes @ref VelocityDesired */ static void updatePathVelocity() @@ -374,6 +373,7 @@ static void updatePathVelocity() float downCommand; PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); PositionActualData positionActual; PositionActualGet(&positionActual); @@ -386,55 +386,58 @@ static void updatePathVelocity() float groundspeed; switch (pathDesired.Mode) { - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - groundspeed = pathDesired.EndingVelocity; - break; - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_DRIVEENDPOINT: - groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1); - if (progress.fractional_progress > 1) - groundspeed = 0; - break; - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_DRIVEVECTOR: - default: - groundspeed = pathDesired.StartingVelocity - + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1); - if (progress.fractional_progress > 1) - groundspeed = 0; - break; + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_DRIVECIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + case PATHDESIRED_MODE_DRIVECIRCLELEFT: + groundspeed = pathDesired.EndingVelocity; + break; + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_DRIVEENDPOINT: + groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1); + if (progress.fractional_progress > 1) { + groundspeed = 0; + } + break; + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_DRIVEVECTOR: + default: + groundspeed = pathDesired.StartingVelocity + + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1); + if (progress.fractional_progress > 1) { + groundspeed = 0; + } + break; } VelocityDesiredData velocityDesired; velocityDesired.North = progress.path_direction[0] * groundspeed; - velocityDesired.East = progress.path_direction[1] * groundspeed; + velocityDesired.East = progress.path_direction[1] * groundspeed; float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP]; float correction_velocity[2] = { progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed }; float total_vel = sqrtf(powf(correction_velocity[0], 2) + powf(correction_velocity[1], 2)); - float scale = 1; - if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) + float scale = 1; + if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) { scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel; + } velocityDesired.North += progress.correction_direction[0] * error_speed * scale; - velocityDesired.East += progress.correction_direction[1] * error_speed * scale; + velocityDesired.East += progress.correction_direction[1] * error_speed * scale; float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound(progress.fractional_progress, 0, 1); float downError = altitudeSetpoint - positionActual.Down; downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], - -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], - vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); + -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], + vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); + downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); // update pathstatus - pathStatus.error = progress.error; + pathStatus.error = progress.error; pathStatus.fractional_progress = progress.fractional_progress; VelocityDesiredSet(&velocityDesired); @@ -443,7 +446,7 @@ static void updatePathVelocity() /** * Compute desired velocity from the current position * - * Takes in @ref PositionActual and compares it to @ref PositionDesired + * Takes in @ref PositionActual and compares it to @ref PositionDesired * and computes @ref VelocityDesired */ void updateEndpointVelocity() @@ -451,6 +454,7 @@ void updateEndpointVelocity() float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); PositionActualData positionActual; @@ -468,65 +472,66 @@ void updateEndpointVelocity() float northPos = 0, eastPos = 0, downPos = 0; switch (vtolpathfollowerSettings.PositionSource) { - case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF: - northPos = positionActual.North; - eastPos = positionActual.East; - downPos = positionActual.Down; - break; - case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS: - { - // this used to work with the NEDposition UAVObject - // however this UAVObject has been removed - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - HomeLocationData homeLocation; - HomeLocationGet(&homeLocation); - float lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); - float alt = homeLocation.Altitude; - float T[3] = { alt+6.378137E6f, - cosf(lat)*(alt+6.378137E6f), - -1.0f}; - float NED[3] = {T[0] * (DEG2RAD((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f)), - T[1] * (DEG2RAD((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f)), - T[2] * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude))}; + case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF: + northPos = positionActual.North; + eastPos = positionActual.East; + downPos = positionActual.Down; + break; + case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS: + { + // this used to work with the NEDposition UAVObject + // however this UAVObject has been removed + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); + float lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); + float alt = homeLocation.Altitude; + float T[3] = { alt + 6.378137E6f, + cosf(lat) * (alt + 6.378137E6f), + -1.0f }; + float NED[3] = { T[0] * (DEG2RAD((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f)), + T[1] * (DEG2RAD((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f)), + T[2] * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude)) }; - northPos = NED[0]; - eastPos = NED[1]; - downPos = NED[2]; - } - break; - default: - PIOS_Assert(0); - break; + northPos = NED[0]; + eastPos = NED[1]; + downPos = NED[2]; + } + break; + default: + PIOS_Assert(0); + break; } // Compute desired north command northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos; northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], - -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], - vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); - northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral); + -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], + vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); + northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral); eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos; - eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], - -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], - vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); + eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], + -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], + vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral); // Limit the maximum velocity float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2)); - float scale = 1; - if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) + float scale = 1; + if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) { scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel; + } velocityDesired.North = northCommand * scale; - velocityDesired.East = eastCommand * scale; + velocityDesired.East = eastCommand * scale; downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos; downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], - -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], - vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); + -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], + vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); + downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); VelocityDesiredSet(&velocityDesired); @@ -536,25 +541,26 @@ void updateEndpointVelocity() * Compute desired attitude from a fixed preset * */ -static void updateFixedAttitude(float* attitude) +static void updateFixedAttitude(float *attitude) { StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); - stabDesired.Roll = attitude[0]; - stabDesired.Pitch = attitude[1]; - stabDesired.Yaw = attitude[2]; + stabDesired.Roll = attitude[0]; + stabDesired.Pitch = attitude[1]; + stabDesired.Yaw = attitude[2]; stabDesired.Throttle = attitude[3]; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; StabilizationDesiredSet(&stabDesired); } /** * Compute desired attitude from the desired velocity * - * Takes in @ref NedActual which has the acceleration in the - * NED frame as the feedback term and then compares the + * Takes in @ref NedActual which has the acceleration in the + * NED frame as the feedback term and then compares the * @ref VelocityActual against the @ref VelocityDesired */ static void updateVtolDesiredAttitude(bool yaw_attitude) @@ -589,32 +595,32 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) float northVel = 0, eastVel = 0, downVel = 0; switch (vtolpathfollowerSettings.VelocitySource) { - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF: - northVel = velocityActual.North; - eastVel = velocityActual.East; - downVel = velocityActual.Down; - break; - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: - { - GPSVelocityData gpsVelocity; - GPSVelocityGet(&gpsVelocity); - northVel = gpsVelocity.North; - eastVel = gpsVelocity.East; - downVel = gpsVelocity.Down; - } - break; - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS: - { - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading)); - eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading)); - downVel = velocityActual.Down; - } - break; - default: - PIOS_Assert(0); - break; + case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF: + northVel = velocityActual.North; + eastVel = velocityActual.East; + downVel = velocityActual.Down; + break; + case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: + { + GPSVelocityData gpsVelocity; + GPSVelocityGet(&gpsVelocity); + northVel = gpsVelocity.North; + eastVel = gpsVelocity.East; + downVel = gpsVelocity.Down; + } + break; + case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS: + { + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading)); + eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading)); + downVel = velocityActual.Down; + } + break; + default: + PIOS_Assert(0); + break; } // Testing code - refactor into manual control command @@ -624,41 +630,41 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) // Compute desired north command northError = velocityDesired.North - northVel; northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], - -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], - vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); - northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral - - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] - + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward); + -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], + vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); + northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral + - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] + + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward); // Compute desired east command eastError = velocityDesired.East - eastVel; eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], - -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], - vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); - eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral - - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] - + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward); + -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], + vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); + eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral + - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] + + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward); // Compute desired down command downError = velocityDesired.Down - downVel; // Must flip this sign downError = -downError; downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI], - -vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT], - vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]); - downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral - - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]); + -vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT], + vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]); + downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral + - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]); stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1); // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the // craft should move similarly for 5 deg roll versus 5 deg pitch - stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeActual.Yaw)) + - -eastCommand * sinf(DEG2RAD(attitudeActual.Yaw)), - -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); - stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeActual.Yaw)) + - eastCommand * cosf(DEG2RAD(attitudeActual.Yaw)), - -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); + stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeActual.Yaw)) + + -eastCommand * sinf(DEG2RAD(attitudeActual.Yaw)), + -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); + stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeActual.Yaw)) + + eastCommand * cosf(DEG2RAD(attitudeActual.Yaw)), + -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { // For now override throttle with manual control. Disable at your risk, quad goes to China. @@ -667,7 +673,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) stabDesired.Throttle = manualControl.Throttle; } - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; if (yaw_attitude) { stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; @@ -690,12 +696,13 @@ static void updateNedAccel() // Collect downsampled attitude data AccelsData accels; + AccelsGet(&accels); accel[0] = accels.x; accel[1] = accels.y; accel[2] = accels.z; - //rotate avg accels into earth frame and store it + // rotate avg accels into earth frame and store it AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); q[0] = attitudeActual.q1; @@ -705,16 +712,17 @@ static void updateNedAccel() Quaternion2R(q, Rbe); for (uint8_t i = 0; i < 3; i++) { accel_ned[i] = 0; - for (uint8_t j = 0; j < 3; j++) + for (uint8_t j = 0; j < 3; j++) { accel_ned[i] += Rbe[j][i] * accel[j]; + } } accel_ned[2] += 9.81f; NedAccelData accelData; NedAccelGet(&accelData); accelData.North = accel_ned[0]; - accelData.East = accel_ned[1]; - accelData.Down = accel_ned[2]; + accelData.East = accel_ned[1]; + accelData.Down = accel_ned[2]; NedAccelSet(&accelData); } @@ -736,10 +744,11 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); } -static void accessoryUpdated(UAVObjEvent* ev) +static void accessoryUpdated(UAVObjEvent *ev) { - if (ev->obj != AccessoryDesiredHandle()) + if (ev->obj != AccessoryDesiredHandle()) { return; + } AccessoryDesiredData accessory; PoiLearnSettingsData poiLearn; @@ -753,11 +762,10 @@ static void accessoryUpdated(UAVObjEvent* ev) PoiLocationData poi; PoiLocationGet(&poi); poi.North = positionActual.North; - poi.East = positionActual.East; - poi.Down = positionActual.Down; + poi.East = positionActual.East; + poi.Down = positionActual.Down; PoiLocationSet(&poi); } } } } - diff --git a/flight/pios/common/pios_adxl345.c b/flight/pios/common/pios_adxl345.c index 0b2b08a97..da2d1677c 100644 --- a/flight/pios/common/pios_adxl345.c +++ b/flight/pios/common/pios_adxl345.c @@ -33,21 +33,21 @@ #ifdef PIOS_INCLUDE_ADXL345 enum pios_adxl345_dev_magic { - PIOS_ADXL345_DEV_MAGIC = 0xcb55aa55, + PIOS_ADXL345_DEV_MAGIC = 0xcb55aa55, }; struct adxl345_dev { - uint32_t spi_id; - uint32_t slave_num; - enum pios_adxl345_dev_magic magic; + uint32_t spi_id; + uint32_t slave_num; + enum pios_adxl345_dev_magic magic; }; -//! Global structure for this device device -static struct adxl345_dev * dev; +// ! Global structure for this device device +static struct adxl345_dev *dev; -//! Private functions -static struct adxl345_dev * PIOS_ADXL345_alloc(void); -static int32_t PIOS_ADXL345_Validate(struct adxl345_dev * dev); +// ! Private functions +static struct adxl345_dev *PIOS_ADXL345_alloc(void); +static int32_t PIOS_ADXL345_Validate(struct adxl345_dev *dev); static int32_t PIOS_ADXL345_ClaimBus(); static int32_t PIOS_ADXL345_ReleaseBus(); static int32_t PIOS_ADXL345_FifoDepth(uint8_t depth); @@ -55,15 +55,17 @@ static int32_t PIOS_ADXL345_FifoDepth(uint8_t depth); /** * @brief Allocate a new device */ -static struct adxl345_dev * PIOS_ADXL345_alloc(void) +static struct adxl345_dev *PIOS_ADXL345_alloc(void) { - struct adxl345_dev * adxl345_dev; - - adxl345_dev = (struct adxl345_dev *)pvPortMalloc(sizeof(*adxl345_dev)); - if (!adxl345_dev) return (NULL); - - adxl345_dev->magic = PIOS_ADXL345_DEV_MAGIC; - return(adxl345_dev); + struct adxl345_dev *adxl345_dev; + + adxl345_dev = (struct adxl345_dev *)pvPortMalloc(sizeof(*adxl345_dev)); + if (!adxl345_dev) { + return NULL; + } + + adxl345_dev->magic = PIOS_ADXL345_DEV_MAGIC; + return adxl345_dev; } /** @@ -72,30 +74,35 @@ static struct adxl345_dev * PIOS_ADXL345_alloc(void) */ static int32_t PIOS_ADXL345_Validate(struct adxl345_dev *vdev) { - if (vdev == NULL) - return -1; - if (vdev->magic != PIOS_ADXL345_DEV_MAGIC) - return -2; - if (vdev->spi_id == 0) - return -3; - return 0; + if (vdev == NULL) { + return -1; + } + if (vdev->magic != PIOS_ADXL345_DEV_MAGIC) { + return -2; + } + if (vdev->spi_id == 0) { + return -3; + } + return 0; } /** * @brief Claim the SPI bus for the accel communications and select this chip * @return 0 for succesful claiming of bus or -1 otherwise */ -static int32_t PIOS_ADXL345_ClaimBus() +static int32_t PIOS_ADXL345_ClaimBus() { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } - if(PIOS_SPI_ClaimBus(dev->spi_id) != 0) - return -2; + if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { + return -2; + } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); - - return 0; + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + + return 0; } /** @@ -104,62 +111,68 @@ static int32_t PIOS_ADXL345_ClaimBus() */ static int32_t PIOS_ADXL345_ReleaseBus() { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - if(PIOS_SPI_ReleaseBus(dev->spi_id) != 0) - return -2; - - return 0; + if (PIOS_SPI_ReleaseBus(dev->spi_id) != 0) { + return -2; + } + + return 0; } /** * @brief Select the sampling rate of the chip - * + * * This also puts it into high power mode */ -int32_t PIOS_ADXL345_SelectRate(uint8_t rate) +int32_t PIOS_ADXL345_SelectRate(uint8_t rate) { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } - if(PIOS_ADXL345_ClaimBus() != 0) - return -2; + if (PIOS_ADXL345_ClaimBus() != 0) { + return -2; + } - uint8_t out[2] = {ADXL_RATE_ADDR, rate & 0x0F}; - if(PIOS_SPI_TransferBlock(dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_ADXL345_ReleaseBus(); - return -3; - } + uint8_t out[2] = { ADXL_RATE_ADDR, rate & 0x0F }; + if (PIOS_SPI_TransferBlock(dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_ADXL345_ReleaseBus(); + return -3; + } - PIOS_ADXL345_ReleaseBus(); - - return 0; + PIOS_ADXL345_ReleaseBus(); + + return 0; } /** * @brief Set the range of the accelerometer and set the data to be right justified * with sign extension. Also keep device in 4 wire mode. */ -int32_t PIOS_ADXL345_SetRange(uint8_t range) +int32_t PIOS_ADXL345_SetRange(uint8_t range) { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; - - if(PIOS_ADXL345_ClaimBus() != 0) - return -2; - - uint8_t out[2] = {ADXL_FORMAT_ADDR, (range & 0x03) | ADXL_FULL_RES | ADXL_4WIRE}; - if(PIOS_SPI_TransferBlock(dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_ADXL345_ReleaseBus(); - return -3; - } - - PIOS_ADXL345_ReleaseBus(); - - return 0; + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } + + if (PIOS_ADXL345_ClaimBus() != 0) { + return -2; + } + + uint8_t out[2] = { ADXL_FORMAT_ADDR, (range & 0x03) | ADXL_FULL_RES | ADXL_4WIRE }; + if (PIOS_SPI_TransferBlock(dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_ADXL345_ReleaseBus(); + return -3; + } + + PIOS_ADXL345_ReleaseBus(); + + return 0; } /** @@ -167,21 +180,23 @@ int32_t PIOS_ADXL345_SetRange(uint8_t range) */ static int32_t PIOS_ADXL345_FifoDepth(uint8_t depth) { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; - - if(PIOS_ADXL345_ClaimBus() != 0) - return -2; - - uint8_t out[2] = {ADXL_FIFO_ADDR, (depth & 0x1f) | ADXL_FIFO_STREAM}; - if(PIOS_SPI_TransferBlock(dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_ADXL345_ReleaseBus(); - return -3; - } + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } - PIOS_ADXL345_ReleaseBus(); - - return 0; + if (PIOS_ADXL345_ClaimBus() != 0) { + return -2; + } + + uint8_t out[2] = { ADXL_FIFO_ADDR, (depth & 0x1f) | ADXL_FIFO_STREAM }; + if (PIOS_SPI_TransferBlock(dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_ADXL345_ReleaseBus(); + return -3; + } + + PIOS_ADXL345_ReleaseBus(); + + return 0; } /** @@ -189,21 +204,23 @@ static int32_t PIOS_ADXL345_FifoDepth(uint8_t depth) */ static int32_t PIOS_ADXL345_SetMeasure(__attribute__((unused)) uint8_t enable) { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; - - if(PIOS_ADXL345_ClaimBus() != 0) - return -2; - - uint8_t out[2] = {ADXL_POWER_ADDR, ADXL_MEAURE}; - if(PIOS_SPI_TransferBlock(dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_ADXL345_ReleaseBus(); - return -3; - } + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } - PIOS_ADXL345_ReleaseBus(); - - return 0; + if (PIOS_ADXL345_ClaimBus() != 0) { + return -2; + } + + uint8_t out[2] = { ADXL_POWER_ADDR, ADXL_MEAURE }; + if (PIOS_SPI_TransferBlock(dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_ADXL345_ReleaseBus(); + return -3; + } + + PIOS_ADXL345_ReleaseBus(); + + return 0; } /** @@ -211,20 +228,21 @@ static int32_t PIOS_ADXL345_SetMeasure(__attribute__((unused)) uint8_t enable) */ int32_t PIOS_ADXL345_Init(uint32_t spi_id, uint32_t slave_num) { - dev = PIOS_ADXL345_alloc(); - if(dev == NULL) - return -1; - - dev->spi_id = spi_id; - dev->slave_num = slave_num; - - PIOS_ADXL345_ReleaseBus(); - PIOS_ADXL345_SelectRate(ADXL_RATE_3200); - PIOS_ADXL345_SetRange(ADXL_RANGE_8G); - PIOS_ADXL345_FifoDepth(16); - PIOS_ADXL345_SetMeasure(1); - - return 0; + dev = PIOS_ADXL345_alloc(); + if (dev == NULL) { + return -1; + } + + dev->spi_id = spi_id; + dev->slave_num = slave_num; + + PIOS_ADXL345_ReleaseBus(); + PIOS_ADXL345_SelectRate(ADXL_RATE_3200); + PIOS_ADXL345_SetRange(ADXL_RANGE_8G); + PIOS_ADXL345_FifoDepth(16); + PIOS_ADXL345_SetMeasure(1); + + return 0; } /** @@ -232,24 +250,26 @@ int32_t PIOS_ADXL345_Init(uint32_t spi_id, uint32_t slave_num) */ int32_t PIOS_ADXL345_Test() { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } - if(PIOS_ADXL345_ClaimBus() != 0) - return -2; + if (PIOS_ADXL345_ClaimBus() != 0) { + return -2; + } - uint8_t buf[2] = {0,0}; - uint8_t rec[2] = {0,0}; - buf[0] = ADXL_WHOAMI | ADXL_READ_BIT; + uint8_t buf[2] = { 0, 0 }; + uint8_t rec[2] = { 0, 0 }; + buf[0] = ADXL_WHOAMI | ADXL_READ_BIT; - if(PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],sizeof(buf),NULL) < 0) { - PIOS_ADXL345_ReleaseBus(); - return -3; - } + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { + PIOS_ADXL345_ReleaseBus(); + return -3; + } - PIOS_ADXL345_ReleaseBus(); + PIOS_ADXL345_ReleaseBus(); - return (rec[1] == ADXL_DEVICE_ID) ? 0 : -4; + return (rec[1] == ADXL_DEVICE_ID) ? 0 : -4; } /** @@ -257,56 +277,60 @@ int32_t PIOS_ADXL345_Test() */ int32_t PIOS_ADXL345_FifoElements() { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; - - if(PIOS_ADXL345_ClaimBus() != 0) - return -2; - - uint8_t buf[2] = {0,0}; - uint8_t rec[2] = {0,0}; - buf[0] = ADXL_FIFOSTATUS_ADDR | ADXL_READ_BIT ; // Read fifo status - - if(PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],sizeof(buf),NULL) < 0) { - PIOS_ADXL345_ReleaseBus(); - return -3; - } - - PIOS_ADXL345_ReleaseBus(); - - return rec[1] & 0x3f; + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } + + if (PIOS_ADXL345_ClaimBus() != 0) { + return -2; + } + + uint8_t buf[2] = { 0, 0 }; + uint8_t rec[2] = { 0, 0 }; + buf[0] = ADXL_FIFOSTATUS_ADDR | ADXL_READ_BIT; // Read fifo status + + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { + PIOS_ADXL345_ReleaseBus(); + return -3; + } + + PIOS_ADXL345_ReleaseBus(); + + return rec[1] & 0x3f; } /** * @brief Read a single set of values from the x y z channels * @returns The number of samples remaining in the fifo */ -uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data * data) +uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data *data) { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; - - if(PIOS_ADXL345_ClaimBus() != 0) - return -2; - - // To save memory use same buffer for in and out but offset by - // a byte - uint8_t buf[9] = {0,0,0,0,0,0,0,0}; - uint8_t rec[9] = {0,0,0,0,0,0,0,0}; - buf[0] = ADXL_X0_ADDR | ADXL_MULTI_BIT | ADXL_READ_BIT ; // Multibyte read starting at X0 - - if(PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],9,NULL) < 0) { - PIOS_ADXL345_ReleaseBus(); - return -3; - } + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } - PIOS_ADXL345_ReleaseBus(); - - data->x = rec[1] + (rec[2] << 8); - data->y = rec[3] + (rec[4] << 8); - data->z = rec[5] + (rec[6] << 8); - - return rec[8] & 0x7F; // return number of remaining entries + if (PIOS_ADXL345_ClaimBus() != 0) { + return -2; + } + + // To save memory use same buffer for in and out but offset by + // a byte + uint8_t buf[9] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + uint8_t rec[9] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + buf[0] = ADXL_X0_ADDR | ADXL_MULTI_BIT | ADXL_READ_BIT; // Multibyte read starting at X0 + + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], 9, NULL) < 0) { + PIOS_ADXL345_ReleaseBus(); + return -3; + } + + PIOS_ADXL345_ReleaseBus(); + + data->x = rec[1] + (rec[2] << 8); + data->y = rec[3] + (rec[4] << 8); + data->z = rec[5] + (rec[6] << 8); + + return rec[8] & 0x7F; // return number of remaining entries } #endif /* PIOS_INCLUDE_ADXL345 */ diff --git a/flight/pios/common/pios_bma180.c b/flight/pios/common/pios_bma180.c index c6a7210b5..5f3fb9451 100644 --- a/flight/pios/common/pios_bma180.c +++ b/flight/pios/common/pios_bma180.c @@ -36,27 +36,27 @@ #include "fifo_buffer.h" enum pios_bma180_dev_magic { - PIOS_BMA180_DEV_MAGIC = 0xcbb93755, + PIOS_BMA180_DEV_MAGIC = 0xcbb93755, }; #define PIOS_BMA180_MAX_DOWNSAMPLE 10 struct bma180_dev { - uint32_t spi_id; - uint32_t slave_num; - int16_t buffer[PIOS_BMA180_MAX_DOWNSAMPLE * sizeof(struct pios_bma180_data)]; - t_fifo_buffer fifo; - const struct pios_bma180_cfg * cfg; - enum bma180_bandwidth bandwidth; - enum bma180_range range; - enum pios_bma180_dev_magic magic; + uint32_t spi_id; + uint32_t slave_num; + int16_t buffer[PIOS_BMA180_MAX_DOWNSAMPLE * sizeof(struct pios_bma180_data)]; + t_fifo_buffer fifo; + const struct pios_bma180_cfg *cfg; + enum bma180_bandwidth bandwidth; + enum bma180_range range; + enum pios_bma180_dev_magic magic; }; -//! Global structure for this device device -static struct bma180_dev * dev; +// ! Global structure for this device device +static struct bma180_dev *dev; -//! Private functions -static struct bma180_dev * PIOS_BMA180_alloc(void); -static int32_t PIOS_BMA180_Validate(struct bma180_dev * dev); +// ! Private functions +static struct bma180_dev *PIOS_BMA180_alloc(void); +static int32_t PIOS_BMA180_Validate(struct bma180_dev *dev); static int32_t PIOS_BMA180_GetReg(uint8_t reg); static int32_t PIOS_BMA180_SetReg(uint8_t reg, uint8_t data); static int32_t PIOS_BMA180_SelectBW(enum bma180_bandwidth bw); @@ -69,17 +69,19 @@ static int32_t PIOS_BMA180_EnableIrq(); /** * @brief Allocate a new device */ -static struct bma180_dev * PIOS_BMA180_alloc(void) +static struct bma180_dev *PIOS_BMA180_alloc(void) { - struct bma180_dev * bma180_dev; - - bma180_dev = (struct bma180_dev *)pvPortMalloc(sizeof(*bma180_dev)); - if (!bma180_dev) return (NULL); - - fifoBuf_init(&bma180_dev->fifo, (uint8_t *) bma180_dev->buffer, sizeof(bma180_dev->buffer)); + struct bma180_dev *bma180_dev; - bma180_dev->magic = PIOS_BMA180_DEV_MAGIC; - return(bma180_dev); + bma180_dev = (struct bma180_dev *)pvPortMalloc(sizeof(*bma180_dev)); + if (!bma180_dev) { + return NULL; + } + + fifoBuf_init(&bma180_dev->fifo, (uint8_t *)bma180_dev->buffer, sizeof(bma180_dev->buffer)); + + bma180_dev->magic = PIOS_BMA180_DEV_MAGIC; + return bma180_dev; } /** @@ -88,38 +90,45 @@ static struct bma180_dev * PIOS_BMA180_alloc(void) */ static int32_t PIOS_BMA180_Validate(struct bma180_dev *vdev) { - if (vdev == NULL) - return -1; - if (vdev->magic != PIOS_BMA180_DEV_MAGIC) - return -2; - if (vdev->spi_id == 0) - return -3; - return 0; + if (vdev == NULL) { + return -1; + } + if (vdev->magic != PIOS_BMA180_DEV_MAGIC) { + return -2; + } + if (vdev->spi_id == 0) { + return -3; + } + return 0; } /** * @brief Initialize with good default settings * @returns 0 for success, -1 for failure */ -int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_bma180_cfg * cfg) -{ - dev = PIOS_BMA180_alloc(); - if(dev == NULL) - return -1; - - dev->spi_id = spi_id; - dev->slave_num = slave_num; - dev->cfg = cfg; - - if(PIOS_BMA180_Config() < 0) - return -1; - PIOS_DELAY_WaituS(50); - - PIOS_EXTI_Init(dev->cfg->exti_cfg); +int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_bma180_cfg *cfg) +{ + dev = PIOS_BMA180_alloc(); + if (dev == NULL) { + return -1; + } - while(PIOS_BMA180_EnableIrq() != 0); - - return 0; + dev->spi_id = spi_id; + dev->slave_num = slave_num; + dev->cfg = cfg; + + if (PIOS_BMA180_Config() < 0) { + return -1; + } + PIOS_DELAY_WaituS(50); + + PIOS_EXTI_Init(dev->cfg->exti_cfg); + + while (PIOS_BMA180_EnableIrq() != 0) { + ; + } + + return 0; } /** @@ -128,17 +137,17 @@ int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_ */ int32_t PIOS_BMA180_ClaimBus() { - if (PIOS_BMA180_Validate(dev) != 0) { - return -1; - } + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { - return -1; - } + if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { + return -1; + } - PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0); + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); - return 0; + return 0; } /** @@ -149,16 +158,16 @@ int32_t PIOS_BMA180_ClaimBus() */ int32_t PIOS_BMA180_ClaimBusISR(bool *woken) { - if (PIOS_BMA180_Validate(dev) != 0) { - return -1; - } + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { - return -1; - } + if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { + return -1; + } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); - return 0; + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + return 0; } /** @@ -167,12 +176,12 @@ int32_t PIOS_BMA180_ClaimBusISR(bool *woken) */ int32_t PIOS_BMA180_ReleaseBus() { - if (PIOS_BMA180_Validate(dev) != 0) { - return -1; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBus(dev->spi_id); + return PIOS_SPI_ReleaseBus(dev->spi_id); } /** @@ -183,12 +192,13 @@ int32_t PIOS_BMA180_ReleaseBus() */ int32_t PIOS_BMA180_ReleaseBusISR(bool *woken) { - if(PIOS_BMA180_Validate(dev) != 0) - return -1; + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); + return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); } /** @@ -198,19 +208,21 @@ int32_t PIOS_BMA180_ReleaseBusISR(bool *woken) */ int32_t PIOS_BMA180_GetReg(uint8_t reg) { - if(PIOS_BMA180_Validate(dev) != 0) - return -1; + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - uint8_t data; - - if(PIOS_BMA180_ClaimBus() != 0) - return -1; + uint8_t data; - PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte - data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response - - PIOS_BMA180_ReleaseBus(); - return data; + if (PIOS_BMA180_ClaimBus() != 0) { + return -1; + } + + PIOS_SPI_TransferByte(dev->spi_id, (0x80 | reg)); // request byte + data = PIOS_SPI_TransferByte(dev->spi_id, 0); // receive response + + PIOS_BMA180_ReleaseBus(); + return data; } /** @@ -221,88 +233,115 @@ int32_t PIOS_BMA180_GetReg(uint8_t reg) */ int32_t PIOS_BMA180_SetReg(uint8_t reg, uint8_t data) { - if(PIOS_BMA180_ClaimBus() != 0) - return -1; - - PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg); - PIOS_SPI_TransferByte(dev->spi_id, data); + if (PIOS_BMA180_ClaimBus() != 0) { + return -1; + } - PIOS_BMA180_ReleaseBus(); - - return 0; + PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg); + PIOS_SPI_TransferByte(dev->spi_id, data); + + PIOS_BMA180_ReleaseBus(); + + return 0; } -static int32_t PIOS_BMA180_EnableEeprom() { - // Enable EEPROM writing - int32_t byte = PIOS_BMA180_GetReg(BMA_CTRREG0); - if(byte < 0) - return -1; - byte |= 0x10; // Set bit 4 - if(PIOS_BMA180_SetReg(BMA_CTRREG0,(uint8_t) byte) < 0) // Have to set ee_w to - return -1; - return 0; +static int32_t PIOS_BMA180_EnableEeprom() +{ + // Enable EEPROM writing + int32_t byte = PIOS_BMA180_GetReg(BMA_CTRREG0); + + if (byte < 0) { + return -1; + } + byte |= 0x10; // Set bit 4 + if (PIOS_BMA180_SetReg(BMA_CTRREG0, (uint8_t)byte) < 0) { // Have to set ee_w to + return -1; + } + return 0; } -static int32_t PIOS_BMA180_DisableEeprom() { - // Enable EEPROM writing - int32_t byte = PIOS_BMA180_GetReg(BMA_CTRREG0); - if(byte < 0) - return -1; - byte |= 0x10; // Set bit 4 - if(PIOS_BMA180_SetReg(BMA_CTRREG0,(uint8_t) byte) < 0) // Have to set ee_w to - return -1; - return 0; +static int32_t PIOS_BMA180_DisableEeprom() +{ + // Enable EEPROM writing + int32_t byte = PIOS_BMA180_GetReg(BMA_CTRREG0); + + if (byte < 0) { + return -1; + } + byte |= 0x10; // Set bit 4 + if (PIOS_BMA180_SetReg(BMA_CTRREG0, (uint8_t)byte) < 0) { // Have to set ee_w to + return -1; + } + return 0; } /** * @brief Set the default register settings * @return 0 if successful, -1 if not */ -static int32_t PIOS_BMA180_Config() +static int32_t PIOS_BMA180_Config() { - /* - 0x35 = 0x81 //smp-skip = 1 for less interrupts - 0x33 = 0x81 //shadow-dis = 1, update MSB and LSB synchronously - 0x27 = 0x01 //dis-i2c - 0x21 = 0x02 //new_data_int = 1 - */ - - PIOS_DELAY_WaituS(20); + /* + 0x35 = 0x81 //smp-skip = 1 for less interrupts + 0x33 = 0x81 //shadow-dis = 1, update MSB and LSB synchronously + 0x27 = 0x01 //dis-i2c + 0x21 = 0x02 //new_data_int = 1 + */ - if(PIOS_BMA180_Validate(dev) != 0) - return -1; + PIOS_DELAY_WaituS(20); - while(PIOS_BMA180_EnableEeprom() != 0); - while(PIOS_BMA180_SetReg(BMA_RESET, BMA_RESET_CODE) != 0); - while(PIOS_BMA180_SetReg(BMA_OFFSET_LSB1, 0x81) != 0); - while(PIOS_BMA180_SetReg(BMA_GAIN_Y, 0x81) != 0); - while(PIOS_BMA180_SetReg(BMA_CTRREG3, 0xFF) != 0); - while(PIOS_BMA180_SelectBW(dev->cfg->bandwidth) != 0); - while(PIOS_BMA180_SetRange(dev->cfg->range) != 0); - while(PIOS_BMA180_DisableEeprom() != 0); + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - return 0; + while (PIOS_BMA180_EnableEeprom() != 0) { + ; + } + while (PIOS_BMA180_SetReg(BMA_RESET, BMA_RESET_CODE) != 0) { + ; + } + while (PIOS_BMA180_SetReg(BMA_OFFSET_LSB1, 0x81) != 0) { + ; + } + while (PIOS_BMA180_SetReg(BMA_GAIN_Y, 0x81) != 0) { + ; + } + while (PIOS_BMA180_SetReg(BMA_CTRREG3, 0xFF) != 0) { + ; + } + while (PIOS_BMA180_SelectBW(dev->cfg->bandwidth) != 0) { + ; + } + while (PIOS_BMA180_SetRange(dev->cfg->range) != 0) { + ; + } + while (PIOS_BMA180_DisableEeprom() != 0) { + ; + } + + return 0; } /** * @brief Select the bandwidth the digital filter pass allows. * @return 0 if successful, -1 if not * @param rate[in] Bandwidth setting to be used - * + * * EEPROM must be write-enabled before calling this function. */ static int32_t PIOS_BMA180_SelectBW(enum bma180_bandwidth bw) { - if(PIOS_BMA180_Validate(dev) != 0) - return -1; + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - dev->bandwidth = bw; - - uint8_t reg; - reg = PIOS_BMA180_GetReg(BMA_BW_ADDR); - reg = (reg & ~BMA_BW_MASK) | ((bw << BMA_BW_SHIFT) & BMA_BW_MASK); - return PIOS_BMA180_SetReg(BMA_BW_ADDR, reg); + dev->bandwidth = bw; + + uint8_t reg; + reg = PIOS_BMA180_GetReg(BMA_BW_ADDR); + reg = (reg & ~BMA_BW_MASK) | ((bw << BMA_BW_SHIFT) & BMA_BW_MASK); + return PIOS_BMA180_SetReg(BMA_BW_ADDR, reg); } /** @@ -311,32 +350,35 @@ static int32_t PIOS_BMA180_SelectBW(enum bma180_bandwidth bw) * @param rate[in] Range setting to be used * */ -static int32_t PIOS_BMA180_SetRange(enum bma180_range new_range) -{ - if(PIOS_BMA180_Validate(dev) != 0) - return -1; +static int32_t PIOS_BMA180_SetRange(enum bma180_range new_range) +{ + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - uint8_t reg; - - dev->range = new_range; - reg = PIOS_BMA180_GetReg(BMA_RANGE_ADDR); - reg = (reg & ~BMA_RANGE_MASK) | ((dev->range << BMA_RANGE_SHIFT) & BMA_RANGE_MASK); - return PIOS_BMA180_SetReg(BMA_RANGE_ADDR, reg); + uint8_t reg; + + dev->range = new_range; + reg = PIOS_BMA180_GetReg(BMA_RANGE_ADDR); + reg = (reg & ~BMA_RANGE_MASK) | ((dev->range << BMA_RANGE_SHIFT) & BMA_RANGE_MASK); + return PIOS_BMA180_SetReg(BMA_RANGE_ADDR, reg); } -static int32_t PIOS_BMA180_EnableIrq() +static int32_t PIOS_BMA180_EnableIrq() { + if (PIOS_BMA180_EnableEeprom() < 0) { + return -1; + } - if(PIOS_BMA180_EnableEeprom() < 0) - return -1; + if (PIOS_BMA180_SetReg(BMA_CTRREG3, BMA_NEW_DAT_INT) < 0) { + return -1; + } - if(PIOS_BMA180_SetReg(BMA_CTRREG3, BMA_NEW_DAT_INT) < 0) - return -1; + if (PIOS_BMA180_DisableEeprom() < 0) { + return -1; + } - if(PIOS_BMA180_DisableEeprom() < 0) - return -1; - - return 0; + return 0; } /** @@ -346,28 +388,30 @@ static int32_t PIOS_BMA180_EnableIrq() * @retval -1 unable to claim bus * @retval -2 unable to transfer data */ -int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data * data) +int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data *data) { - // To save memory use same buffer for in and out but offset by - // a byte - uint8_t buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0}; - uint8_t rec[7] = {0,0,0,0,0,0}; - - if(PIOS_BMA180_ClaimBus() != 0) - return -1; - if(PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],7,NULL) != 0) - return -2; - PIOS_BMA180_ReleaseBus(); - - // | MSB | LSB | 0 | new_data | - data->x = ((rec[2] << 8) | rec[1]); - data->y = ((rec[4] << 8) | rec[3]); - data->z = ((rec[6] << 8) | rec[5]); - data->x /= 4; - data->y /= 4; - data->z /= 4; - - return 0; // return number of remaining entries + // To save memory use same buffer for in and out but offset by + // a byte + uint8_t buf[7] = { BMA_X_LSB_ADDR | 0x80, 0, 0, 0, 0, 0 }; + uint8_t rec[7] = { 0, 0, 0, 0, 0, 0 }; + + if (PIOS_BMA180_ClaimBus() != 0) { + return -1; + } + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], 7, NULL) != 0) { + return -2; + } + PIOS_BMA180_ReleaseBus(); + + // | MSB | LSB | 0 | new_data | + data->x = ((rec[2] << 8) | rec[1]); + data->y = ((rec[4] << 8) | rec[3]); + data->z = ((rec[6] << 8) | rec[5]); + data->x /= 4; + data->y /= 4; + data->z /= 4; + + return 0; // return number of remaining entries } /** @@ -376,26 +420,33 @@ int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data * data) */ float PIOS_BMA180_GetScale() { - if(PIOS_BMA180_Validate(dev) != 0) - return -1; + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - switch (dev->cfg->range) { - case BMA_RANGE_1G: - return GRAV / 8192.0f; - case BMA_RANGE_1_5G: - return GRAV / 5460.0f; - case BMA_RANGE_2G: - return GRAV / 4096.0f; - case BMA_RANGE_3G: - return GRAV / 2730.0f; - case BMA_RANGE_4G: - return GRAV / 2048.0f; - case BMA_RANGE_8G: - return GRAV / 1024.0f; - case BMA_RANGE_16G: - return GRAV / 512.0f; - } - return 0; + switch (dev->cfg->range) { + case BMA_RANGE_1G: + return GRAV / 8192.0f; + + case BMA_RANGE_1_5G: + return GRAV / 5460.0f; + + case BMA_RANGE_2G: + return GRAV / 4096.0f; + + case BMA_RANGE_3G: + return GRAV / 2730.0f; + + case BMA_RANGE_4G: + return GRAV / 2048.0f; + + case BMA_RANGE_8G: + return GRAV / 1024.0f; + + case BMA_RANGE_16G: + return GRAV / 512.0f; + } + return 0; } /** @@ -403,17 +454,19 @@ float PIOS_BMA180_GetScale() * @param [out] buffer pointer to a @ref pios_bma180_data structure to receive data * @return 0 for success, -1 for failure (no data available) */ -int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data * buffer) +int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data *buffer) { - if(PIOS_BMA180_Validate(dev) != 0) - return -1; + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - if(fifoBuf_getUsed(&dev->fifo) < sizeof(*buffer)) - return -1; - - fifoBuf_getData(&dev->fifo, (uint8_t *) buffer, sizeof(*buffer)); - - return 0; + if (fifoBuf_getUsed(&dev->fifo) < sizeof(*buffer)) { + return -1; + } + + fifoBuf_getData(&dev->fifo, (uint8_t *)buffer, sizeof(*buffer)); + + return 0; } @@ -424,30 +477,35 @@ int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data * buffer) */ int32_t PIOS_BMA180_Test() { - // Read chip ID then version ID - uint8_t buf[3] = {0x80 | BMA_CHIPID_ADDR, 0, 0}; - uint8_t rec[3] = {0,0, 0}; - int32_t retval; + // Read chip ID then version ID + uint8_t buf[3] = { 0x80 | BMA_CHIPID_ADDR, 0, 0 }; + uint8_t rec[3] = { 0, 0, 0 }; + int32_t retval; - if(PIOS_BMA180_ClaimBus() != 0) - return -1; - retval = PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],sizeof(buf),NULL); - PIOS_BMA180_ReleaseBus(); - - if(retval != 0) - return -2; - - struct pios_bma180_data data; - if(PIOS_BMA180_ReadAccels(&data) != 0) - return -3; - - if(rec[1] != 0x3) - return -4; - - if(rec[2] < 0x12) - return -5; + if (PIOS_BMA180_ClaimBus() != 0) { + return -1; + } + retval = PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL); + PIOS_BMA180_ReleaseBus(); - return 0; + if (retval != 0) { + return -2; + } + + struct pios_bma180_data data; + if (PIOS_BMA180_ReadAccels(&data) != 0) { + return -3; + } + + if (rec[1] != 0x3) { + return -4; + } + + if (rec[2] < 0x12) { + return -5; + } + + return 0; } /** @@ -458,43 +516,44 @@ int32_t PIOS_BMA180_Test() int32_t bma180_irqs = 0; bool PIOS_BMA180_IRQHandler(void) { - bool woken = false; - static const uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0}; - uint8_t pios_bma180_dmabuf[8]; - bma180_irqs++; - - // If we can't get the bus then just move on for efficiency - if (PIOS_BMA180_ClaimBusISR(&woken) != 0) { - return woken; // Something else is using bus, miss this data - } - - PIOS_SPI_TransferBlock(dev->spi_id,pios_bma180_req_buf,(uint8_t *) pios_bma180_dmabuf, - sizeof(pios_bma180_dmabuf), NULL); + bool woken = false; + static const uint8_t pios_bma180_req_buf[7] = { BMA_X_LSB_ADDR | 0x80, 0, 0, 0, 0, 0 }; + uint8_t pios_bma180_dmabuf[8]; - // TODO: Make this conversion depend on configuration scale - struct pios_bma180_data data; - - // Don't release bus till data has copied - PIOS_BMA180_ReleaseBusISR(&woken); - - // Must not return before releasing bus - if (fifoBuf_getFree(&dev->fifo) < sizeof(data)) { - return woken; - } - - // Bottom two bits indicate new data and are constant zeros. Don't right - // shift because it drops sign bit - data.x = ((pios_bma180_dmabuf[2] << 8) | pios_bma180_dmabuf[1]); - data.y = ((pios_bma180_dmabuf[4] << 8) | pios_bma180_dmabuf[3]); - data.z = ((pios_bma180_dmabuf[6] << 8) | pios_bma180_dmabuf[5]); - data.x /= 4; - data.y /= 4; - data.z /= 4; - data.temperature = pios_bma180_dmabuf[7]; - - fifoBuf_putData(&dev->fifo, (uint8_t *) &data, sizeof(data)); - - return woken; + bma180_irqs++; + + // If we can't get the bus then just move on for efficiency + if (PIOS_BMA180_ClaimBusISR(&woken) != 0) { + return woken; // Something else is using bus, miss this data + } + + PIOS_SPI_TransferBlock(dev->spi_id, pios_bma180_req_buf, (uint8_t *)pios_bma180_dmabuf, + sizeof(pios_bma180_dmabuf), NULL); + + // TODO: Make this conversion depend on configuration scale + struct pios_bma180_data data; + + // Don't release bus till data has copied + PIOS_BMA180_ReleaseBusISR(&woken); + + // Must not return before releasing bus + if (fifoBuf_getFree(&dev->fifo) < sizeof(data)) { + return woken; + } + + // Bottom two bits indicate new data and are constant zeros. Don't right + // shift because it drops sign bit + data.x = ((pios_bma180_dmabuf[2] << 8) | pios_bma180_dmabuf[1]); + data.y = ((pios_bma180_dmabuf[4] << 8) | pios_bma180_dmabuf[3]); + data.z = ((pios_bma180_dmabuf[6] << 8) | pios_bma180_dmabuf[5]); + data.x /= 4; + data.y /= 4; + data.z /= 4; + data.temperature = pios_bma180_dmabuf[7]; + + fifoBuf_putData(&dev->fifo, (uint8_t *)&data, sizeof(data)); + + return woken; } #endif /* PIOS_INCLUDE_BMA180 */ diff --git a/flight/pios/common/pios_bmp085.c b/flight/pios/common/pios_bmp085.c index 83a77e95e..28d57c5dd 100644 --- a/flight/pios/common/pios_bmp085.c +++ b/flight/pios/common/pios_bmp085.c @@ -1,30 +1,30 @@ /** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_BMP085 BMP085 Functions - * @brief Hardware functions to deal with the altitude pressure sensor - * @{ - * - * @file pios_bmp085.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief BMP085 Pressure Sensor Routines - * @see The GNU Public License (GPL) Version 3 - * - ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_BMP085 BMP085 Functions + * @brief Hardware functions to deal with the altitude pressure sensor + * @{ + * + * @file pios_bmp085.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief BMP085 Pressure Sensor Routines + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -58,277 +58,285 @@ xSemaphoreHandle PIOS_BMP085_EOC; int32_t PIOS_BMP085_EOC; #endif -void PIOS_BMP085_EndOfConversion (void) +void PIOS_BMP085_EndOfConversion(void) { #if defined(PIOS_INCLUDE_FREERTOS) - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #endif - /* Read the ADC Value */ + /* Read the ADC Value */ #if defined(PIOS_INCLUDE_FREERTOS) - xSemaphoreGiveFromISR(PIOS_BMP085_EOC, &xHigherPriorityTaskWoken); + xSemaphoreGiveFromISR(PIOS_BMP085_EOC, &xHigherPriorityTaskWoken); #else - PIOS_BMP085_EOC=1; + PIOS_BMP085_EOC = 1; #endif #if defined(PIOS_INCLUDE_FREERTOS) - /* Yield From ISR if needed */ - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + /* Yield From ISR if needed */ + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } static const struct pios_exti_cfg pios_exti_bmp085_cfg __exti_config = { - .vector = PIOS_BMP085_EndOfConversion, - .line = PIOS_BMP085_EOC_EXTI_LINE, - .pin = { - .gpio = PIOS_BMP085_EOC_GPIO_PORT, - .init = { - .GPIO_Pin = PIOS_BMP085_EOC_GPIO_PIN, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = PIOS_BMP085_EOC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_BMP085_EOC_PRIO, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = PIOS_BMP085_EOC_EXTI_LINE, - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_BMP085_EndOfConversion, + .line = PIOS_BMP085_EOC_EXTI_LINE, + .pin = { + .gpio = PIOS_BMP085_EOC_GPIO_PORT, + .init = { + .GPIO_Pin = PIOS_BMP085_EOC_GPIO_PIN, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = PIOS_BMP085_EOC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_BMP085_EOC_PRIO, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = PIOS_BMP085_EOC_EXTI_LINE, + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; #endif /* PIOS_BMP085_HAS_GPIOS */ /** -* Initialise the BMP085 sensor -*/ + * Initialise the BMP085 sensor + */ void PIOS_BMP085_Init(void) { #ifdef PIOS_BMP085_HAS_GPIOS #if defined(PIOS_INCLUDE_FREERTOS) - /* Semaphore used by ISR to signal End-Of-Conversion */ - vSemaphoreCreateBinary(PIOS_BMP085_EOC); - /* Must start off empty so that first transfer waits for EOC */ - xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); + /* Semaphore used by ISR to signal End-Of-Conversion */ + vSemaphoreCreateBinary(PIOS_BMP085_EOC); + /* Must start off empty so that first transfer waits for EOC */ + xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); #else - PIOS_BMP085_EOC = 0; + PIOS_BMP085_EOC = 0; #endif - /* Enable EOC GPIO clock */ - RCC_APB2PeriphClockCmd(PIOS_BMP085_EOC_CLK | RCC_APB2Periph_AFIO, ENABLE); + /* Enable EOC GPIO clock */ + RCC_APB2PeriphClockCmd(PIOS_BMP085_EOC_CLK | RCC_APB2Periph_AFIO, ENABLE); - if (PIOS_EXTI_Init(&pios_exti_bmp085_cfg)) { - PIOS_Assert(0); - } + if (PIOS_EXTI_Init(&pios_exti_bmp085_cfg)) { + PIOS_Assert(0); + } - /* Configure XCLR pin as push/pull alternate funtion output */ - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Pin = PIOS_BMP085_XCLR_GPIO_PIN; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; - GPIO_Init(PIOS_BMP085_XCLR_GPIO_PORT, &GPIO_InitStructure); + /* Configure XCLR pin as push/pull alternate funtion output */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.GPIO_Pin = PIOS_BMP085_XCLR_GPIO_PIN; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; + GPIO_Init(PIOS_BMP085_XCLR_GPIO_PORT, &GPIO_InitStructure); -#endif /* PIOS_BMP085_HAS_GPIOS */ +#endif /* PIOS_BMP085_HAS_GPIOS */ - /* Read all 22 bytes of calibration data in one transfer, this is a very optimized way of doing things */ - uint8_t Data[BMP085_CALIB_LEN]; - while (PIOS_BMP085_Read(BMP085_CALIB_ADDR, Data, BMP085_CALIB_LEN) != 0) - continue; + /* Read all 22 bytes of calibration data in one transfer, this is a very optimized way of doing things */ + uint8_t Data[BMP085_CALIB_LEN]; + while (PIOS_BMP085_Read(BMP085_CALIB_ADDR, Data, BMP085_CALIB_LEN) != 0) { + continue; + } - /* Parameters AC1-AC6 */ - CalibData.AC1 = (Data[0] << 8) | Data[1]; - CalibData.AC2 = (Data[2] << 8) | Data[3]; - CalibData.AC3 = (Data[4] << 8) | Data[5]; - CalibData.AC4 = (Data[6] << 8) | Data[7]; - CalibData.AC5 = (Data[8] << 8) | Data[9]; - CalibData.AC6 = (Data[10] << 8) | Data[11]; + /* Parameters AC1-AC6 */ + CalibData.AC1 = (Data[0] << 8) | Data[1]; + CalibData.AC2 = (Data[2] << 8) | Data[3]; + CalibData.AC3 = (Data[4] << 8) | Data[5]; + CalibData.AC4 = (Data[6] << 8) | Data[7]; + CalibData.AC5 = (Data[8] << 8) | Data[9]; + CalibData.AC6 = (Data[10] << 8) | Data[11]; - /* Parameters B1, B2 */ - CalibData.B1 = (Data[12] << 8) | Data[13]; - CalibData.B2 = (Data[14] << 8) | Data[15]; + /* Parameters B1, B2 */ + CalibData.B1 = (Data[12] << 8) | Data[13]; + CalibData.B2 = (Data[14] << 8) | Data[15]; - /* Parameters MB, MC, MD */ - CalibData.MB = (Data[16] << 8) | Data[17]; - CalibData.MC = (Data[18] << 8) | Data[19]; - CalibData.MD = (Data[20] << 8) | Data[21]; + /* Parameters MB, MC, MD */ + CalibData.MB = (Data[16] << 8) | Data[17]; + CalibData.MC = (Data[18] << 8) | Data[19]; + CalibData.MD = (Data[20] << 8) | Data[21]; } /** -* Start the ADC conversion -* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR -* \return Raw ADC value -*/ + * Start the ADC conversion + * \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR + * \return Raw ADC value + */ void PIOS_BMP085_StartADC(ConversionTypeTypeDef Type) { - /* Start the conversion */ - if (Type == TemperatureConv) { - while (PIOS_BMP085_Write(BMP085_CTRL_ADDR, BMP085_TEMP_ADDR) != 0) - continue; - } else if (Type == PressureConv) { - while (PIOS_BMP085_Write(BMP085_CTRL_ADDR, BMP085_PRES_ADDR) != 0) - continue; - } + /* Start the conversion */ + if (Type == TemperatureConv) { + while (PIOS_BMP085_Write(BMP085_CTRL_ADDR, BMP085_TEMP_ADDR) != 0) { + continue; + } + } else if (Type == PressureConv) { + while (PIOS_BMP085_Write(BMP085_CTRL_ADDR, BMP085_PRES_ADDR) != 0) { + continue; + } + } - CurrentRead = Type; + CurrentRead = Type; } /** -* Read the ADC conversion value (once ADC conversion has completed) -* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR -* \return Raw ADC value -*/ + * Read the ADC conversion value (once ADC conversion has completed) + * \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR + * \return Raw ADC value + */ void PIOS_BMP085_ReadADC(void) { - uint8_t Data[3]; - Data[0] = 0; - Data[1] = 0; - Data[2] = 0; + uint8_t Data[3]; - /* Read and store the 16bit result */ - if (CurrentRead == TemperatureConv) { - /* Read the temperature conversion */ - while (PIOS_BMP085_Read(BMP085_ADC_MSB, Data, 2) != 0) - continue; - RawTemperature = ((Data[0] << 8) | Data[1]); + Data[0] = 0; + Data[1] = 0; + Data[2] = 0; - X1 = (RawTemperature - CalibData.AC6) * CalibData.AC5 >> 15; - X2 = ((int32_t) CalibData.MC << 11) / (X1 + CalibData.MD); - B5 = X1 + X2; - Temperature = (B5 + 8) >> 4; - } else { - /* Read the pressure conversion */ - while (PIOS_BMP085_Read(BMP085_ADC_MSB, Data, 3) != 0) - continue; - RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]) >> (8 - BMP085_OVERSAMPLING); + /* Read and store the 16bit result */ + if (CurrentRead == TemperatureConv) { + /* Read the temperature conversion */ + while (PIOS_BMP085_Read(BMP085_ADC_MSB, Data, 2) != 0) { + continue; + } + RawTemperature = ((Data[0] << 8) | Data[1]); - B6 = B5 - 4000; - X1 = (CalibData.B2 * (B6 * B6 >> 12)) >> 11; - X2 = CalibData.AC2 * B6 >> 11; - X3 = X1 + X2; - B3 = ((((int32_t) CalibData.AC1 * 4 + X3) << BMP085_OVERSAMPLING) + 2) >> 2; - X1 = CalibData.AC3 * B6 >> 13; - X2 = (CalibData.B1 * (B6 * B6 >> 12)) >> 16; - X3 = ((X1 + X2) + 2) >> 2; - B4 = (CalibData.AC4 * (uint32_t) (X3 + 32768)) >> 15; - B7 = ((uint32_t) RawPressure - B3) * (50000 >> BMP085_OVERSAMPLING); - P = B7 < 0x80000000 ? (B7 * 2) / B4 : (B7 / B4) * 2; + X1 = (RawTemperature - CalibData.AC6) * CalibData.AC5 >> 15; + X2 = ((int32_t)CalibData.MC << 11) / (X1 + CalibData.MD); + B5 = X1 + X2; + Temperature = (B5 + 8) >> 4; + } else { + /* Read the pressure conversion */ + while (PIOS_BMP085_Read(BMP085_ADC_MSB, Data, 3) != 0) { + continue; + } + RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]) >> (8 - BMP085_OVERSAMPLING); - X1 = (P >> 8) * (P >> 8); - X1 = (X1 * 3038) >> 16; - X2 = (-7357 * P) >> 16; - Pressure = P + ((X1 + X2 + 3791) >> 4); - } + B6 = B5 - 4000; + X1 = (CalibData.B2 * (B6 * B6 >> 12)) >> 11; + X2 = CalibData.AC2 * B6 >> 11; + X3 = X1 + X2; + B3 = ((((int32_t)CalibData.AC1 * 4 + X3) << BMP085_OVERSAMPLING) + 2) >> 2; + X1 = CalibData.AC3 * B6 >> 13; + X2 = (CalibData.B1 * (B6 * B6 >> 12)) >> 16; + X3 = ((X1 + X2) + 2) >> 2; + B4 = (CalibData.AC4 * (uint32_t)(X3 + 32768)) >> 15; + B7 = ((uint32_t)RawPressure - B3) * (50000 >> BMP085_OVERSAMPLING); + P = B7 < 0x80000000 ? (B7 * 2) / B4 : (B7 / B4) * 2; + + X1 = (P >> 8) * (P >> 8); + X1 = (X1 * 3038) >> 16; + X2 = (-7357 * P) >> 16; + Pressure = P + ((X1 + X2 + 3791) >> 4); + } } int16_t PIOS_BMP085_GetTemperature(void) { - return Temperature; + return Temperature; } int32_t PIOS_BMP085_GetPressure(void) { - return Pressure; + return Pressure; } /** -* Reads one or more bytes into a buffer -* \param[in] address BMP085 register address (depends on size) -* \param[out] buffer destination buffer -* \param[in] len number of bytes which should be read -* \return 0 if operation was successful -* \return -1 if error during I2C transfer -* \return -2 if BMP085 blocked by another task (retry it!) -* \return -4 if invalid length -*/ -bool PIOS_BMP085_Read(uint8_t address, uint8_t * buffer, uint8_t len) + * Reads one or more bytes into a buffer + * \param[in] address BMP085 register address (depends on size) + * \param[out] buffer destination buffer + * \param[in] len number of bytes which should be read + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + * \return -2 if BMP085 blocked by another task (retry it!) + * \return -4 if invalid length + */ +bool PIOS_BMP085_Read(uint8_t address, uint8_t *buffer, uint8_t len) { - uint8_t addr_buffer[] = { - address, - }; + uint8_t addr_buffer[] = { + address, + }; - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = BMP085_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(addr_buffer), - .buf = addr_buffer, - } - , - { - .info = __func__, - .addr = BMP085_I2C_ADDR, - .rw = PIOS_I2C_TXN_READ, - .len = len, - .buf = buffer, - } - }; + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = BMP085_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(addr_buffer), + .buf = addr_buffer, + } + , + { + .info = __func__, + .addr = BMP085_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; - return PIOS_I2C_Transfer(PIOS_I2C_BMP085_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_BMP085_ADAPTER, txn_list, NELEMENTS(txn_list)); } /** -* Writes one or more bytes to the BMP085 -* \param[in] address Register address -* \param[in] buffer source buffer -* \return 0 if operation was successful -* \return -1 if error during I2C transfer -* \return -2 if BMP085 blocked by another task (retry it!) -*/ + * Writes one or more bytes to the BMP085 + * \param[in] address Register address + * \param[in] buffer source buffer + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + * \return -2 if BMP085 blocked by another task (retry it!) + */ bool PIOS_BMP085_Write(uint8_t address, uint8_t buffer) { - uint8_t data[] = { - address, - buffer, - }; + uint8_t data[] = { + address, + buffer, + }; - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = BMP085_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(data), - .buf = data, - } - , - }; + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = BMP085_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(data), + .buf = data, + } + , + }; - return PIOS_I2C_Transfer(PIOS_I2C_BMP085_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_BMP085_ADAPTER, txn_list, NELEMENTS(txn_list)); } /** -* @brief Run self-test operation. -* \return 0 if self-test failed -* \return any non-0 number if test passed -*/ + * @brief Run self-test operation. + * \return 0 if self-test failed + * \return any non-0 number if test passed + */ int32_t PIOS_BMP085_Test() { - // TODO: Is there a better way to test this than just checking that pressure/temperature has changed? - uint32_t passed = 1; - uint32_t cur_value = 0; + // TODO: Is there a better way to test this than just checking that pressure/temperature has changed? + uint32_t passed = 1; + uint32_t cur_value = 0; - cur_value = Temperature; - PIOS_BMP085_StartADC(TemperatureConv); - PIOS_DELAY_WaitmS(5); - PIOS_BMP085_ReadADC(); - if (cur_value == Temperature) - passed = 0; + cur_value = Temperature; + PIOS_BMP085_StartADC(TemperatureConv); + PIOS_DELAY_WaitmS(5); + PIOS_BMP085_ReadADC(); + if (cur_value == Temperature) { + passed = 0; + } - cur_value=Pressure; - PIOS_BMP085_StartADC(PressureConv); - PIOS_DELAY_WaitmS(26); - PIOS_BMP085_ReadADC(); - if (cur_value == Pressure) - passed = 0; + cur_value = Pressure; + PIOS_BMP085_StartADC(PressureConv); + PIOS_DELAY_WaitmS(26); + PIOS_BMP085_ReadADC(); + if (cur_value == Pressure) { + passed = 0; + } - return passed; + return passed; } #endif /* PIOS_INCLUDE_BMP085 */ diff --git a/flight/pios/common/pios_board_info.c b/flight/pios/common/pios_board_info.c index c3a5a4dfb..97d9eb79f 100644 --- a/flight/pios/common/pios_board_info.c +++ b/flight/pios/common/pios_board_info.c @@ -3,17 +3,17 @@ #include "pios_board_info.h" const struct pios_board_info __attribute__((__used__)) __attribute__((__section__(".boardinfo"))) pios_board_info_blob = { - .magic = PIOS_BOARD_INFO_BLOB_MAGIC, - .board_type = BOARD_TYPE, - .board_rev = BOARD_REVISION, - .bl_rev = BOOTLOADER_VERSION, - .hw_type = HW_TYPE, - .fw_base = FW_BANK_BASE, - .fw_size = FW_BANK_SIZE - FW_DESC_SIZE, - .desc_base = FW_BANK_BASE + FW_BANK_SIZE - FW_DESC_SIZE, - .desc_size = FW_DESC_SIZE, + .magic = PIOS_BOARD_INFO_BLOB_MAGIC, + .board_type = BOARD_TYPE, + .board_rev = BOARD_REVISION, + .bl_rev = BOOTLOADER_VERSION, + .hw_type = HW_TYPE, + .fw_base = FW_BANK_BASE, + .fw_size = FW_BANK_SIZE - FW_DESC_SIZE, + .desc_base = FW_BANK_BASE + FW_BANK_SIZE - FW_DESC_SIZE, + .desc_size = FW_DESC_SIZE, #ifdef EE_BANK_BASE - .ee_base = EE_BANK_BASE, - .ee_size = EE_BANK_SIZE, + .ee_base = EE_BANK_BASE, + .ee_size = EE_BANK_SIZE, #endif }; diff --git a/flight/pios/common/pios_com.c b/flight/pios/common/pios_com.c index b15a176ef..86fbeca58 100644 --- a/flight/pios/common/pios_com.c +++ b/flight/pios/common/pios_com.c @@ -6,25 +6,25 @@ * @brief Hardware communication layer * @{ * - * @file pios_com.c + * @file pios_com.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief COM layer functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,470 +36,477 @@ #include #ifndef PIOS_INCLUDE_FREERTOS -#include "pios_delay.h" /* PIOS_DELAY_WaitmS */ +#include "pios_delay.h" /* PIOS_DELAY_WaitmS */ #endif enum pios_com_dev_magic { - PIOS_COM_DEV_MAGIC = 0xaa55aa55, + PIOS_COM_DEV_MAGIC = 0xaa55aa55, }; struct pios_com_dev { - enum pios_com_dev_magic magic; - uint32_t lower_id; - const struct pios_com_driver * driver; + enum pios_com_dev_magic magic; + uint32_t lower_id; + const struct pios_com_driver *driver; #if defined(PIOS_INCLUDE_FREERTOS) - xSemaphoreHandle tx_sem; - xSemaphoreHandle rx_sem; + xSemaphoreHandle tx_sem; + xSemaphoreHandle rx_sem; #endif - bool has_rx; - bool has_tx; + bool has_rx; + bool has_tx; - t_fifo_buffer rx; - t_fifo_buffer tx; + t_fifo_buffer rx; + t_fifo_buffer tx; }; -static bool PIOS_COM_validate(struct pios_com_dev * com_dev) +static bool PIOS_COM_validate(struct pios_com_dev *com_dev) { - return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC)); + return com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_com_dev * PIOS_COM_alloc(void) +static struct pios_com_dev *PIOS_COM_alloc(void) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - com_dev = (struct pios_com_dev *)pvPortMalloc(sizeof(*com_dev)); - if (!com_dev) return (NULL); + com_dev = (struct pios_com_dev *)pvPortMalloc(sizeof(*com_dev)); + if (!com_dev) { + return NULL; + } - com_dev->magic = PIOS_COM_DEV_MAGIC; - return(com_dev); + com_dev->magic = PIOS_COM_DEV_MAGIC; + return com_dev; } #else static struct pios_com_dev pios_com_devs[PIOS_COM_MAX_DEVS]; static uint8_t pios_com_num_devs; -static struct pios_com_dev * PIOS_COM_alloc(void) +static struct pios_com_dev *PIOS_COM_alloc(void) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) { - return (NULL); - } + if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) { + return NULL; + } - com_dev = &pios_com_devs[pios_com_num_devs++]; - com_dev->magic = PIOS_COM_DEV_MAGIC; + com_dev = &pios_com_devs[pios_com_num_devs++]; + com_dev->magic = PIOS_COM_DEV_MAGIC; - return (com_dev); + return com_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); -static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); -static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield); -static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield); +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); +static void PIOS_COM_UnblockRx(struct pios_com_dev *com_dev, bool *need_yield); +static void PIOS_COM_UnblockTx(struct pios_com_dev *com_dev, bool *need_yield); /** - * Initialises COM layer - * \param[out] handle - * \param[in] driver - * \param[in] id - * \return < 0 if initialisation failed - */ -int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len) + * Initialises COM layer + * \param[out] handle + * \param[in] driver + * \param[in] id + * \return < 0 if initialisation failed + */ +int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len) { - PIOS_Assert(com_id); - PIOS_Assert(driver); + PIOS_Assert(com_id); + PIOS_Assert(driver); - bool has_rx = (rx_buffer && rx_buffer_len > 0); - bool has_tx = (tx_buffer && tx_buffer_len > 0); - PIOS_Assert(has_rx || has_tx); - PIOS_Assert(driver->bind_tx_cb || !has_tx); - PIOS_Assert(driver->bind_rx_cb || !has_rx); + bool has_rx = (rx_buffer && rx_buffer_len > 0); + bool has_tx = (tx_buffer && tx_buffer_len > 0); + PIOS_Assert(has_rx || has_tx); + PIOS_Assert(driver->bind_tx_cb || !has_tx); + PIOS_Assert(driver->bind_rx_cb || !has_rx); - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - com_dev = (struct pios_com_dev *) PIOS_COM_alloc(); - if (!com_dev) goto out_fail; + com_dev = (struct pios_com_dev *)PIOS_COM_alloc(); + if (!com_dev) { + goto out_fail; + } - com_dev->driver = driver; - com_dev->lower_id = lower_id; + com_dev->driver = driver; + com_dev->lower_id = lower_id; - com_dev->has_rx = has_rx; - com_dev->has_tx = has_tx; + com_dev->has_rx = has_rx; + com_dev->has_tx = has_tx; - if (has_rx) { - fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len); + if (has_rx) { + fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len); #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(com_dev->rx_sem); -#endif /* PIOS_INCLUDE_FREERTOS */ - (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, (uint32_t)com_dev); - if (com_dev->driver->rx_start) { - /* Start the receiver */ - (com_dev->driver->rx_start)(com_dev->lower_id, - fifoBuf_getFree(&com_dev->rx)); - } - } + vSemaphoreCreateBinary(com_dev->rx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, (uint32_t)com_dev); + if (com_dev->driver->rx_start) { + /* Start the receiver */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } + } - if (has_tx) { - fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); + if (has_tx) { + fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(com_dev->tx_sem); -#endif /* PIOS_INCLUDE_FREERTOS */ - (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, (uint32_t)com_dev); - } + vSemaphoreCreateBinary(com_dev->tx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, (uint32_t)com_dev); + } - *com_id = (uint32_t)com_dev; - return(0); + *com_id = (uint32_t)com_dev; + return 0; out_fail: - return(-1); + return -1; } -static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield) +static void PIOS_COM_UnblockRx(struct pios_com_dev *com_dev, bool *need_yield) { #if defined(PIOS_INCLUDE_FREERTOS) - static signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken); + static signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken != pdFALSE) { - *need_yield = true; - } else { - *need_yield = false; - } + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } #else - *need_yield = false; + *need_yield = false; #endif } -static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield) +static void PIOS_COM_UnblockTx(struct pios_com_dev *com_dev, bool *need_yield) { #if defined(PIOS_INCLUDE_FREERTOS) - static signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken); + static signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken != pdFALSE) { - *need_yield = true; - } else { - *need_yield = false; - } + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } #else - *need_yield = false; + *need_yield = false; #endif } -static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield) { - struct pios_com_dev * com_dev = (struct pios_com_dev *)context; + struct pios_com_dev *com_dev = (struct pios_com_dev *)context; - bool valid = PIOS_COM_validate(com_dev); - PIOS_Assert(valid); - PIOS_Assert(com_dev->has_rx); + bool valid = PIOS_COM_validate(com_dev); - uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len); + PIOS_Assert(valid); + PIOS_Assert(com_dev->has_rx); - if (bytes_into_fifo > 0) { - /* Data has been added to the buffer */ - PIOS_COM_UnblockRx(com_dev, need_yield); - } + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len); - if (headroom) { - *headroom = fifoBuf_getFree(&com_dev->rx); - } + if (bytes_into_fifo > 0) { + /* Data has been added to the buffer */ + PIOS_COM_UnblockRx(com_dev, need_yield); + } - return (bytes_into_fifo); + if (headroom) { + *headroom = fifoBuf_getFree(&com_dev->rx); + } + + return bytes_into_fifo; } -static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield) { - struct pios_com_dev * com_dev = (struct pios_com_dev *)context; + struct pios_com_dev *com_dev = (struct pios_com_dev *)context; - bool valid = PIOS_COM_validate(com_dev); - PIOS_Assert(valid); - PIOS_Assert(buf); - PIOS_Assert(buf_len); - PIOS_Assert(com_dev->has_tx); + bool valid = PIOS_COM_validate(com_dev); - uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len); + PIOS_Assert(valid); + PIOS_Assert(buf); + PIOS_Assert(buf_len); + PIOS_Assert(com_dev->has_tx); - if (bytes_from_fifo > 0) { - /* More space has been made in the buffer */ - PIOS_COM_UnblockTx(com_dev, need_yield); - } + uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len); - if (headroom) { - *headroom = fifoBuf_getUsed(&com_dev->tx); - } + if (bytes_from_fifo > 0) { + /* More space has been made in the buffer */ + PIOS_COM_UnblockTx(com_dev, need_yield); + } - return (bytes_from_fifo); + if (headroom) { + *headroom = fifoBuf_getUsed(&com_dev->tx); + } + + return bytes_from_fifo; } /** -* Change the port speed without re-initializing -* \param[in] port COM port -* \param[in] baud Requested baud rate -* \return -1 if port not available -* \return 0 on success -*/ + * Change the port speed without re-initializing + * \param[in] port COM port + * \param[in] baud Requested baud rate + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) { - struct pios_com_dev * com_dev = (struct pios_com_dev *)com_id; + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - /* Invoke the driver function if it exists */ - if (com_dev->driver->set_baud) { - com_dev->driver->set_baud(com_dev->lower_id, baud); - } + /* Invoke the driver function if it exists */ + if (com_dev->driver->set_baud) { + com_dev->driver->set_baud(com_dev->lower_id, baud); + } - return 0; + return 0; } /** -* Sends a package over given port -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return number of bytes transmitted on success -*/ + * Sends a package over given port + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return number of bytes transmitted on success + */ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev = (struct pios_com_dev *)com_id; + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - PIOS_Assert(com_dev->has_tx); + PIOS_Assert(com_dev->has_tx); - if (len > fifoBuf_getFree(&com_dev->tx)) { - /* Buffer cannot accept all requested bytes (retry) */ - return -2; - } + if (len > fifoBuf_getFree(&com_dev->tx)) { + /* Buffer cannot accept all requested bytes (retry) */ + return -2; + } - uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len); + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len); - if (bytes_into_fifo > 0) { - /* More data has been put in the tx buffer, make sure the tx is started */ - if (com_dev->driver->tx_start) { - com_dev->driver->tx_start(com_dev->lower_id, - fifoBuf_getUsed(&com_dev->tx)); - } - } + if (bytes_into_fifo > 0) { + /* More data has been put in the tx buffer, make sure the tx is started */ + if (com_dev->driver->tx_start) { + com_dev->driver->tx_start(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } + } - return (bytes_into_fifo); + return bytes_into_fifo; } /** -* Sends a package over given port -* (blocking function) -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return number of bytes transmitted on success -*/ + * Sends a package over given port + * (blocking function) + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return number of bytes transmitted on success + */ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev = (struct pios_com_dev *)com_id; + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - PIOS_Assert(com_dev->has_tx); + PIOS_Assert(com_dev->has_tx); - uint32_t max_frag_len = fifoBuf_getSize(&com_dev->tx); - uint32_t bytes_to_send = len; - while (bytes_to_send) { - uint32_t frag_size; + uint32_t max_frag_len = fifoBuf_getSize(&com_dev->tx); + uint32_t bytes_to_send = len; + while (bytes_to_send) { + uint32_t frag_size; - if (bytes_to_send > max_frag_len) { - frag_size = max_frag_len; - } else { - frag_size = bytes_to_send; - } - int32_t rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, frag_size); - if (rc >= 0) { - bytes_to_send -= rc; - buffer += rc; - } else { - switch (rc) { - case -1: - /* Device is invalid, this will never work */ - return -1; - case -2: - /* Device is busy, wait for the underlying device to free some space and retry */ - /* Make sure the transmitter is running while we wait */ - if (com_dev->driver->tx_start) { - (com_dev->driver->tx_start)(com_dev->lower_id, - fifoBuf_getUsed(&com_dev->tx)); - } + if (bytes_to_send > max_frag_len) { + frag_size = max_frag_len; + } else { + frag_size = bytes_to_send; + } + int32_t rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, frag_size); + if (rc >= 0) { + bytes_to_send -= rc; + buffer += rc; + } else { + switch (rc) { + case -1: + /* Device is invalid, this will never work */ + return -1; + + case -2: + /* Device is busy, wait for the underlying device to free some space and retry */ + /* Make sure the transmitter is running while we wait */ + if (com_dev->driver->tx_start) { + (com_dev->driver->tx_start)(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreTake(com_dev->tx_sem, 5000) != pdTRUE) { - return -3; - } + if (xSemaphoreTake(com_dev->tx_sem, 5000) != pdTRUE) { + return -3; + } #endif - continue; - default: - /* Unhandled return code */ - return rc; - } - } - } + continue; + default: + /* Unhandled return code */ + return rc; + } + } + } - return len; + return len; } /** -* Sends a single character over given port -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a single character over given port + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c) { - return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1); + return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1); } /** -* Sends a single character over given port -* (blocking function) -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a single character over given port + * (blocking function) + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendChar(uint32_t com_id, char c) { - return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1); + return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1); } /** -* Sends a string over given port -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a string over given port + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str) { - return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str)); + return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str)); } /** -* Sends a string over given port -* (blocking function) -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a string over given port + * (blocking function) + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendString(uint32_t com_id, const char *str) { - return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str)); + return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str)); } /** -* Sends a formatted string (-> printf) over given port -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* 128 characters supported maximum! -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * 128 characters supported maximum! + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Sends a formatted string (-> printf) over given port -* (blocking function) -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * (blocking function) + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Transfer bytes from port buffers into another buffer -* \param[in] port COM port -* \returns Byte from buffer -*/ -uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms) + * Transfer bytes from port buffers into another buffer + * \param[in] port COM port + * \returns Byte from buffer + */ +uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t *buf, uint16_t buf_len, uint32_t timeout_ms) { - PIOS_Assert(buf); - PIOS_Assert(buf_len); - uint16_t bytes_from_fifo; + PIOS_Assert(buf); + PIOS_Assert(buf_len); + uint16_t bytes_from_fifo; - struct pios_com_dev * com_dev = (struct pios_com_dev *)com_id; + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - PIOS_Assert(0); - } - PIOS_Assert(com_dev->has_rx); + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + PIOS_Assert(0); + } + PIOS_Assert(com_dev->has_rx); - check_again: - bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len); +check_again: + bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len); - if (bytes_from_fifo == 0) { - /* No more bytes in receive buffer */ - /* Make sure the receiver is running while we wait */ - if (com_dev->driver->rx_start) { - /* Notify the lower layer that there is now room in the rx buffer */ - (com_dev->driver->rx_start)(com_dev->lower_id, - fifoBuf_getFree(&com_dev->rx)); - } - if (timeout_ms > 0) { + if (bytes_from_fifo == 0) { + /* No more bytes in receive buffer */ + /* Make sure the receiver is running while we wait */ + if (com_dev->driver->rx_start) { + /* Notify the lower layer that there is now room in the rx buffer */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } + if (timeout_ms > 0) { #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) { - /* Make sure we don't come back here again */ - timeout_ms = 0; - goto check_again; - } + if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) { + /* Make sure we don't come back here again */ + timeout_ms = 0; + goto check_again; + } #else - PIOS_DELAY_WaitmS(1); - timeout_ms--; - goto check_again; + PIOS_DELAY_WaitmS(1); + timeout_ms--; + goto check_again; #endif - } - } + } + } - /* Return received byte */ - return (bytes_from_fifo); + /* Return received byte */ + return bytes_from_fifo; } /** @@ -509,18 +516,19 @@ uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len */ bool PIOS_COM_Available(uint32_t com_id) { - struct pios_com_dev * com_dev = (struct pios_com_dev *)com_id; + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; - if (!PIOS_COM_validate(com_dev)) { - return false; - } + if (!PIOS_COM_validate(com_dev)) { + return false; + } - // If a driver does not provide a query method assume always - // available if valid - if (com_dev->driver->available == NULL) - return true; + // If a driver does not provide a query method assume always + // available if valid + if (com_dev->driver->available == NULL) { + return true; + } - return (com_dev->driver->available)(com_dev->lower_id); + return (com_dev->driver->available)(com_dev->lower_id); } #endif /* PIOS_INCLUDE_COM */ diff --git a/flight/pios/common/pios_com_msg.c b/flight/pios/common/pios_com_msg.c index 91e285a3d..0bd40805c 100644 --- a/flight/pios/common/pios_com_msg.c +++ b/flight/pios/common/pios_com_msg.c @@ -6,25 +6,25 @@ * @brief Hardware communication layer * @{ * - * @file pios_com_msg.c + * @file pios_com_msg.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief COM MSG layer functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,146 +37,146 @@ #define PIOS_COM_MSG_MAX_LEN 63 struct pios_com_msg_dev { - uint32_t lower_id; - const struct pios_com_driver * driver; + uint32_t lower_id; + const struct pios_com_driver *driver; - uint8_t rx_msg_buffer[PIOS_COM_MSG_MAX_LEN]; - volatile bool rx_msg_full; + uint8_t rx_msg_buffer[PIOS_COM_MSG_MAX_LEN]; + volatile bool rx_msg_full; - uint8_t tx_msg_buffer[PIOS_COM_MSG_MAX_LEN]; - volatile bool tx_msg_full; + uint8_t tx_msg_buffer[PIOS_COM_MSG_MAX_LEN]; + volatile bool tx_msg_full; }; static struct pios_com_msg_dev com_msg_dev; -static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); -static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); +static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); +static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); -int32_t PIOS_COM_MSG_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id) +int32_t PIOS_COM_MSG_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id) { - PIOS_Assert(com_id); - PIOS_Assert(driver); + PIOS_Assert(com_id); + PIOS_Assert(driver); - PIOS_Assert(driver->bind_tx_cb); - PIOS_Assert(driver->bind_rx_cb); + PIOS_Assert(driver->bind_tx_cb); + PIOS_Assert(driver->bind_rx_cb); - struct pios_com_msg_dev * com_dev = &com_msg_dev; + struct pios_com_msg_dev *com_dev = &com_msg_dev; - com_dev->driver = driver; - com_dev->lower_id = lower_id; + com_dev->driver = driver; + com_dev->lower_id = lower_id; - com_dev->rx_msg_full = false; - (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_MSG_RxInCallback, (uint32_t)com_dev); - (com_dev->driver->rx_start)(com_dev->lower_id, sizeof(com_dev->rx_msg_buffer)); + com_dev->rx_msg_full = false; + (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_MSG_RxInCallback, (uint32_t)com_dev); + (com_dev->driver->rx_start)(com_dev->lower_id, sizeof(com_dev->rx_msg_buffer)); - com_dev->tx_msg_full = false; - (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_MSG_TxOutCallback, (uint32_t)com_dev); + com_dev->tx_msg_full = false; + (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_MSG_TxOutCallback, (uint32_t)com_dev); - *com_id = (uint32_t)com_dev; - return(0); + *com_id = (uint32_t)com_dev; + return 0; } -static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, __attribute__((unused)) bool * need_yield) +static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, __attribute__((unused)) bool *need_yield) { - struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)context; + struct pios_com_msg_dev *com_dev = (struct pios_com_msg_dev *)context; - PIOS_Assert(buf); - PIOS_Assert(buf_len); + PIOS_Assert(buf); + PIOS_Assert(buf_len); - uint16_t bytes_from_fifo = 0; + uint16_t bytes_from_fifo = 0; - if (com_dev->tx_msg_full && (buf_len >= sizeof(com_dev->tx_msg_buffer))) { - /* Room for an entire message, send it */ - memcpy(buf, com_dev->tx_msg_buffer, sizeof(com_dev->tx_msg_buffer)); - bytes_from_fifo = sizeof(com_dev->tx_msg_buffer); - com_dev->tx_msg_full = false; - } + if (com_dev->tx_msg_full && (buf_len >= sizeof(com_dev->tx_msg_buffer))) { + /* Room for an entire message, send it */ + memcpy(buf, com_dev->tx_msg_buffer, sizeof(com_dev->tx_msg_buffer)); + bytes_from_fifo = sizeof(com_dev->tx_msg_buffer); + com_dev->tx_msg_full = false; + } - if (headroom) { - if (com_dev->tx_msg_full) { - *headroom = sizeof(com_dev->tx_msg_buffer); - } else { - *headroom = 0; - } - } + if (headroom) { + if (com_dev->tx_msg_full) { + *headroom = sizeof(com_dev->tx_msg_buffer); + } else { + *headroom = 0; + } + } - return (bytes_from_fifo); + return bytes_from_fifo; } -static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, __attribute__((unused)) bool * need_yield) +static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, __attribute__((unused)) bool *need_yield) { - struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)context; + struct pios_com_msg_dev *com_dev = (struct pios_com_msg_dev *)context; - uint16_t bytes_into_fifo = 0; + uint16_t bytes_into_fifo = 0; - if (!com_dev->rx_msg_full && (buf_len >= sizeof(com_dev->rx_msg_buffer))) { - memcpy(com_dev->rx_msg_buffer, buf, sizeof(com_dev->rx_msg_buffer)); - bytes_into_fifo = sizeof(com_dev->rx_msg_buffer); - com_dev->rx_msg_full = true; - } + if (!com_dev->rx_msg_full && (buf_len >= sizeof(com_dev->rx_msg_buffer))) { + memcpy(com_dev->rx_msg_buffer, buf, sizeof(com_dev->rx_msg_buffer)); + bytes_into_fifo = sizeof(com_dev->rx_msg_buffer); + com_dev->rx_msg_full = true; + } - if (headroom) { - if (!com_dev->rx_msg_full) { - *headroom = sizeof(com_dev->rx_msg_buffer); - } else { - *headroom = 0; - } - } + if (headroom) { + if (!com_dev->rx_msg_full) { + *headroom = sizeof(com_dev->rx_msg_buffer); + } else { + *headroom = 0; + } + } - return (bytes_into_fifo); + return bytes_into_fifo; } int32_t PIOS_COM_MSG_Send(uint32_t com_id, const uint8_t *msg, uint16_t msg_len) { - PIOS_Assert(msg); - PIOS_Assert(msg_len); + PIOS_Assert(msg); + PIOS_Assert(msg_len); - struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)com_id; + struct pios_com_msg_dev *com_dev = (struct pios_com_msg_dev *)com_id; - PIOS_Assert(msg_len == sizeof(com_dev->tx_msg_buffer)); + PIOS_Assert(msg_len == sizeof(com_dev->tx_msg_buffer)); - /* Wait forever for room in the tx buffer */ - while (com_dev->tx_msg_full) { - /* Kick the transmitter while we wait */ - if (com_dev->driver->tx_start) { - (com_dev->driver->tx_start)(com_dev->lower_id, sizeof(com_dev->tx_msg_buffer)); - } - } + /* Wait forever for room in the tx buffer */ + while (com_dev->tx_msg_full) { + /* Kick the transmitter while we wait */ + if (com_dev->driver->tx_start) { + (com_dev->driver->tx_start)(com_dev->lower_id, sizeof(com_dev->tx_msg_buffer)); + } + } - memcpy((void *) com_dev->tx_msg_buffer, msg, msg_len); - com_dev->tx_msg_full = true; + memcpy((void *)com_dev->tx_msg_buffer, msg, msg_len); + com_dev->tx_msg_full = true; - /* Kick the transmitter now that we've queued our message */ - if (com_dev->driver->tx_start) { - (com_dev->driver->tx_start)(com_dev->lower_id, sizeof(com_dev->tx_msg_buffer)); - } + /* Kick the transmitter now that we've queued our message */ + if (com_dev->driver->tx_start) { + (com_dev->driver->tx_start)(com_dev->lower_id, sizeof(com_dev->tx_msg_buffer)); + } - return 0; + return 0; } -uint16_t PIOS_COM_MSG_Receive(uint32_t com_id, uint8_t * msg, uint16_t msg_len) +uint16_t PIOS_COM_MSG_Receive(uint32_t com_id, uint8_t *msg, uint16_t msg_len) { - PIOS_Assert(msg); - PIOS_Assert(msg_len); + PIOS_Assert(msg); + PIOS_Assert(msg_len); - struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)com_id; + struct pios_com_msg_dev *com_dev = (struct pios_com_msg_dev *)com_id; - PIOS_Assert(msg_len == sizeof(com_dev->rx_msg_buffer)); + PIOS_Assert(msg_len == sizeof(com_dev->rx_msg_buffer)); - if (!com_dev->rx_msg_full) { - /* There's room in our buffer, kick the receiver */ - (com_dev->driver->rx_start)(com_dev->lower_id, sizeof(com_dev->rx_msg_buffer)); - } else { - memcpy(msg, com_dev->rx_msg_buffer, msg_len); - com_dev->rx_msg_full = false; + if (!com_dev->rx_msg_full) { + /* There's room in our buffer, kick the receiver */ + (com_dev->driver->rx_start)(com_dev->lower_id, sizeof(com_dev->rx_msg_buffer)); + } else { + memcpy(msg, com_dev->rx_msg_buffer, msg_len); + com_dev->rx_msg_full = false; - return msg_len; - } + return msg_len; + } - return 0; + return 0; } -#endif /* PIOS_INCLUDE_COM_MSG */ +#endif /* PIOS_INCLUDE_COM_MSG */ /** * @} diff --git a/flight/pios/common/pios_crc.c b/flight/pios/common/pios_crc.c index dcd8c80c7..9b2128f06 100644 --- a/flight/pios/common/pios_crc.c +++ b/flight/pios/common/pios_crc.c @@ -11,93 +11,93 @@ // CRC lookup table static const uint8_t crc_table[256] = { - 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, - 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, - 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, - 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, - 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, - 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, - 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, - 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, - 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, - 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, - 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, - 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, - 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, - 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, - 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, - 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 + 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, + 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, + 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, + 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, + 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, + 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, + 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, + 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, + 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, + 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, + 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, + 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, + 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, + 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, + 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, + 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 }; -static const uint16_t CRC_Table16[] = { // HDLC polynomial - 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, - 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, - 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, - 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, - 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, - 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, - 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, - 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, - 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, - 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, - 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, - 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, - 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, - 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, - 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, - 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, - 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, - 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, - 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, - 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, - 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, - 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, - 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, - 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, - 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, - 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, - 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, - 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, - 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, - 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, - 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, - 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 +static const uint16_t CRC_Table16[] = { // HDLC polynomial + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 }; -static const uint32_t CRC_Table32[] = { - 0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005, - 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd, - 0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9, 0x5f15adac, 0x5bd4b01b, 0x569796c2, 0x52568b75, - 0x6a1936c8, 0x6ed82b7f, 0x639b0da6, 0x675a1011, 0x791d4014, 0x7ddc5da3, 0x709f7b7a, 0x745e66cd, - 0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039, 0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5, - 0xbe2b5b58, 0xbaea46ef, 0xb7a96036, 0xb3687d81, 0xad2f2d84, 0xa9ee3033, 0xa4ad16ea, 0xa06c0b5d, - 0xd4326d90, 0xd0f37027, 0xddb056fe, 0xd9714b49, 0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95, - 0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1, 0xe13ef6f4, 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d, - 0x34867077, 0x30476dc0, 0x3d044b19, 0x39c556ae, 0x278206ab, 0x23431b1c, 0x2e003dc5, 0x2ac12072, - 0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16, 0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca, - 0x7897ab07, 0x7c56b6b0, 0x71159069, 0x75d48dde, 0x6b93dddb, 0x6f52c06c, 0x6211e6b5, 0x66d0fb02, - 0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1, 0x53dc6066, 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba, - 0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e, 0xbfa1b04b, 0xbb60adfc, 0xb6238b25, 0xb2e29692, - 0x8aad2b2f, 0x8e6c3698, 0x832f1041, 0x87ee0df6, 0x99a95df3, 0x9d684044, 0x902b669d, 0x94ea7b2a, - 0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e, 0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2, - 0xc6bcf05f, 0xc27dede8, 0xcf3ecb31, 0xcbffd686, 0xd5b88683, 0xd1799b34, 0xdc3abded, 0xd8fba05a, - 0x690ce0ee, 0x6dcdfd59, 0x608edb80, 0x644fc637, 0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb, - 0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f, 0x5c007b8a, 0x58c1663d, 0x558240e4, 0x51435d53, - 0x251d3b9e, 0x21dc2629, 0x2c9f00f0, 0x285e1d47, 0x36194d42, 0x32d850f5, 0x3f9b762c, 0x3b5a6b9b, - 0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff, 0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623, - 0xf12f560e, 0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7, 0xe22b20d2, 0xe6ea3d65, 0xeba91bbc, 0xef68060b, - 0xd727bbb6, 0xd3e6a601, 0xdea580d8, 0xda649d6f, 0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3, - 0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7, 0xae3afba2, 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b, - 0x9b3660c6, 0x9ff77d71, 0x92b45ba8, 0x9675461f, 0x8832161a, 0x8cf30bad, 0x81b02d74, 0x857130c3, - 0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640, 0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c, - 0x7b827d21, 0x7f436096, 0x7200464f, 0x76c15bf8, 0x68860bfd, 0x6c47164a, 0x61043093, 0x65c52d24, - 0x119b4be9, 0x155a565e, 0x18197087, 0x1cd86d30, 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec, - 0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088, 0x2497d08d, 0x2056cd3a, 0x2d15ebe3, 0x29d4f654, - 0xc5a92679, 0xc1683bce, 0xcc2b1d17, 0xc8ea00a0, 0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, 0xdbee767c, - 0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18, 0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4, - 0x89b8fd09, 0x8d79e0be, 0x803ac667, 0x84fbdbd0, 0x9abc8bd5, 0x9e7d9662, 0x933eb0bb, 0x97ffad0c, - 0xafb010b1, 0xab710d06, 0xa6322bdf, 0xa2f33668, 0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4 - }; +static const uint32_t CRC_Table32[] = { + 0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005, + 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd, + 0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9, 0x5f15adac, 0x5bd4b01b, 0x569796c2, 0x52568b75, + 0x6a1936c8, 0x6ed82b7f, 0x639b0da6, 0x675a1011, 0x791d4014, 0x7ddc5da3, 0x709f7b7a, 0x745e66cd, + 0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039, 0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5, + 0xbe2b5b58, 0xbaea46ef, 0xb7a96036, 0xb3687d81, 0xad2f2d84, 0xa9ee3033, 0xa4ad16ea, 0xa06c0b5d, + 0xd4326d90, 0xd0f37027, 0xddb056fe, 0xd9714b49, 0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95, + 0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1, 0xe13ef6f4, 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d, + 0x34867077, 0x30476dc0, 0x3d044b19, 0x39c556ae, 0x278206ab, 0x23431b1c, 0x2e003dc5, 0x2ac12072, + 0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16, 0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca, + 0x7897ab07, 0x7c56b6b0, 0x71159069, 0x75d48dde, 0x6b93dddb, 0x6f52c06c, 0x6211e6b5, 0x66d0fb02, + 0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1, 0x53dc6066, 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba, + 0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e, 0xbfa1b04b, 0xbb60adfc, 0xb6238b25, 0xb2e29692, + 0x8aad2b2f, 0x8e6c3698, 0x832f1041, 0x87ee0df6, 0x99a95df3, 0x9d684044, 0x902b669d, 0x94ea7b2a, + 0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e, 0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2, + 0xc6bcf05f, 0xc27dede8, 0xcf3ecb31, 0xcbffd686, 0xd5b88683, 0xd1799b34, 0xdc3abded, 0xd8fba05a, + 0x690ce0ee, 0x6dcdfd59, 0x608edb80, 0x644fc637, 0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb, + 0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f, 0x5c007b8a, 0x58c1663d, 0x558240e4, 0x51435d53, + 0x251d3b9e, 0x21dc2629, 0x2c9f00f0, 0x285e1d47, 0x36194d42, 0x32d850f5, 0x3f9b762c, 0x3b5a6b9b, + 0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff, 0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623, + 0xf12f560e, 0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7, 0xe22b20d2, 0xe6ea3d65, 0xeba91bbc, 0xef68060b, + 0xd727bbb6, 0xd3e6a601, 0xdea580d8, 0xda649d6f, 0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3, + 0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7, 0xae3afba2, 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b, + 0x9b3660c6, 0x9ff77d71, 0x92b45ba8, 0x9675461f, 0x8832161a, 0x8cf30bad, 0x81b02d74, 0x857130c3, + 0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640, 0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c, + 0x7b827d21, 0x7f436096, 0x7200464f, 0x76c15bf8, 0x68860bfd, 0x6c47164a, 0x61043093, 0x65c52d24, + 0x119b4be9, 0x155a565e, 0x18197087, 0x1cd86d30, 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec, + 0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088, 0x2497d08d, 0x2056cd3a, 0x2d15ebe3, 0x29d4f654, + 0xc5a92679, 0xc1683bce, 0xcc2b1d17, 0xc8ea00a0, 0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, 0xdbee767c, + 0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18, 0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4, + 0x89b8fd09, 0x8d79e0be, 0x803ac667, 0x84fbdbd0, 0x9abc8bd5, 0x9e7d9662, 0x933eb0bb, 0x97ffad0c, + 0xafb010b1, 0xab710d06, 0xa6322bdf, 0xa2f33668, 0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4 +}; /** * Update the crc value with new data. @@ -119,7 +119,7 @@ static const uint32_t CRC_Table32[] = { */ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) { - return crc_table[crc ^ data]; + return crc_table[crc ^ data]; } /** @@ -129,17 +129,18 @@ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) * @param[in] length Number of bytes to process * @returns Updated CRC */ -uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length) +uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t *data, int32_t length) { - // use registers for speed - register int32_t len = length; - register uint8_t crc8 = crc; - register const uint8_t *p = data; - - while (len--) - crc8 = crc_table[crc8 ^ *p++]; - - return crc8; + // use registers for speed + register int32_t len = length; + register uint8_t crc8 = crc; + register const uint8_t *p = data; + + while (len--) { + crc8 = crc_table[crc8 ^ *p++]; + } + + return crc8; } /** @@ -151,7 +152,7 @@ uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length) */ uint16_t PIOS_CRC16_updateByte(uint16_t crc, const uint8_t data) { - return ((crc >> 8) ^ CRC_Table16[(crc & 0xff) ^ data]); + return (crc >> 8) ^ CRC_Table16[(crc & 0xff) ^ data]; } /** @@ -161,13 +162,15 @@ uint16_t PIOS_CRC16_updateByte(uint16_t crc, const uint8_t data) * @param[in] length Number of bytes to process * @returns Updated CRC */ -uint16_t PIOS_CRC16_updateCRC(uint16_t crc, const uint8_t* data, int32_t length) +uint16_t PIOS_CRC16_updateCRC(uint16_t crc, const uint8_t *data, int32_t length) { - register uint8_t *p = (uint8_t *)data; - register uint16_t _crc = crc; - for (register uint32_t i = length; i > 0; i--) - _crc = (_crc >> 8) ^ CRC_Table16[(_crc ^ *p++) & 0xff]; - return _crc; + register uint8_t *p = (uint8_t *)data; + register uint16_t _crc = crc; + + for (register uint32_t i = length; i > 0; i--) { + _crc = (_crc >> 8) ^ CRC_Table16[(_crc ^ *p++) & 0xff]; + } + return _crc; } /** @@ -179,7 +182,7 @@ uint16_t PIOS_CRC16_updateCRC(uint16_t crc, const uint8_t* data, int32_t length) */ uint32_t PIOS_CRC32_updateByte(uint32_t crc, const uint8_t data) { - return ((crc << 8) ^ CRC_Table32[(crc >> 24) ^ data]); + return (crc << 8) ^ CRC_Table32[(crc >> 24) ^ data]; } /** @@ -189,11 +192,13 @@ uint32_t PIOS_CRC32_updateByte(uint32_t crc, const uint8_t data) * @param[in] length Number of bytes to process * @returns Updated CRC */ -uint32_t PIOS_CRC32_updateCRC(uint32_t crc, const uint8_t* data, int32_t length) +uint32_t PIOS_CRC32_updateCRC(uint32_t crc, const uint8_t *data, int32_t length) { - register uint8_t *p = (uint8_t *)data; - register uint32_t _crc = crc; - for (register uint32_t i = length; i > 0; i--) - _crc = (_crc << 8) ^ CRC_Table32[(_crc >> 24) ^ *p++]; - return _crc; + register uint8_t *p = (uint8_t *)data; + register uint32_t _crc = crc; + + for (register uint32_t i = length; i > 0; i--) { + _crc = (_crc << 8) ^ CRC_Table32[(_crc >> 24) ^ *p++]; + } + return _crc; } diff --git a/flight/pios/common/pios_etasv3.c b/flight/pios/common/pios_etasv3.c index 33090ba5e..7d773d95e 100644 --- a/flight/pios/common/pios_etasv3.c +++ b/flight/pios/common/pios_etasv3.c @@ -1,30 +1,30 @@ /** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_ETASV3 ETASV3 Functions - * @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3 - * @{ - * - * @file pios_etasv3.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief ETASV3 Airspeed Sensor Driver - * @see The GNU Public License (GPL) Version 3 - * - ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_ETASV3 ETASV3 Functions + * @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3 + * @{ + * + * @file pios_etasv3.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief ETASV3 Airspeed Sensor Driver + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,31 +32,31 @@ #ifdef PIOS_INCLUDE_ETASV3 -static bool PIOS_ETASV3_Read(uint8_t * buffer, uint8_t len) +static bool PIOS_ETASV3_Read(uint8_t *buffer, uint8_t len) { - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = ETASV3_I2C_ADDR, - .rw = PIOS_I2C_TXN_READ, - .len = len, - .buf = buffer, - } - }; + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = ETASV3_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; - return PIOS_I2C_Transfer(PIOS_I2C_ETASV3_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_ETASV3_ADAPTER, txn_list, NELEMENTS(txn_list)); } -int16_t PIOS_ETASV3_ReadAirspeed (void) +int16_t PIOS_ETASV3_ReadAirspeed(void) { - uint8_t airspeed_raw[2]; + uint8_t airspeed_raw[2]; - if (PIOS_ETASV3_Read(airspeed_raw, sizeof(airspeed_raw)) != 0) { - /* Failed to read airspeed */ - return -1; - } + if (PIOS_ETASV3_Read(airspeed_raw, sizeof(airspeed_raw)) != 0) { + /* Failed to read airspeed */ + return -1; + } - return (airspeed_raw[0] | (airspeed_raw[1]<<8)); + return airspeed_raw[0] | (airspeed_raw[1] << 8); } #endif /* PIOS_INCLUDE_ETASV3 */ diff --git a/flight/pios/common/pios_flash_jedec.c b/flight/pios/common/pios_flash_jedec.c index a05431e19..6d5050990 100644 --- a/flight/pios/common/pios_flash_jedec.c +++ b/flight/pios/common/pios_flash_jedec.c @@ -36,122 +36,129 @@ #include "pios_flash_jedec_priv.h" #include "pios_flash_jedec_catalog.h" -#define JEDEC_WRITE_ENABLE 0x06 -#define JEDEC_WRITE_DISABLE 0x04 -#define JEDEC_READ_STATUS 0x05 -#define JEDEC_WRITE_STATUS 0x01 -#define JEDEC_READ_DATA 0x03 -#define JEDEC_FAST_READ 0x0b -#define JEDEC_DEVICE_ID 0x9F -#define JEDEC_PAGE_WRITE 0x02 +#define JEDEC_WRITE_ENABLE 0x06 +#define JEDEC_WRITE_DISABLE 0x04 +#define JEDEC_READ_STATUS 0x05 +#define JEDEC_WRITE_STATUS 0x01 +#define JEDEC_READ_DATA 0x03 +#define JEDEC_FAST_READ 0x0b +#define JEDEC_DEVICE_ID 0x9F +#define JEDEC_PAGE_WRITE 0x02 -#define JEDEC_STATUS_BUSY 0x01 -#define JEDEC_STATUS_WRITEPROTECT 0x02 -#define JEDEC_STATUS_BP0 0x04 -#define JEDEC_STATUS_BP1 0x08 -#define JEDEC_STATUS_BP2 0x10 -#define JEDEC_STATUS_TP 0x20 -#define JEDEC_STATUS_SEC 0x40 -#define JEDEC_STATUS_SRP0 0x80 +#define JEDEC_STATUS_BUSY 0x01 +#define JEDEC_STATUS_WRITEPROTECT 0x02 +#define JEDEC_STATUS_BP0 0x04 +#define JEDEC_STATUS_BP1 0x08 +#define JEDEC_STATUS_BP2 0x10 +#define JEDEC_STATUS_TP 0x20 +#define JEDEC_STATUS_SEC 0x40 +#define JEDEC_STATUS_SRP0 0x80 enum pios_jedec_dev_magic { - PIOS_JEDEC_DEV_MAGIC = 0xcb55aa55, + PIOS_JEDEC_DEV_MAGIC = 0xcb55aa55, }; -//! Device handle structure +// ! Device handle structure struct jedec_flash_dev { - uint32_t spi_id; - uint32_t slave_num; - bool claimed; + uint32_t spi_id; + uint32_t slave_num; + bool claimed; - uint8_t manufacturer; - uint8_t memorytype; - uint8_t capacity; + uint8_t manufacturer; + uint8_t memorytype; + uint8_t capacity; - const struct pios_flash_jedec_cfg * cfg; + const struct pios_flash_jedec_cfg *cfg; #if defined(FLASH_FREERTOS) - xSemaphoreHandle transaction_lock; + xSemaphoreHandle transaction_lock; #endif - enum pios_jedec_dev_magic magic; + enum pios_jedec_dev_magic magic; }; -//! Private functions -static int32_t PIOS_Flash_Jedec_Validate(struct jedec_flash_dev * flash_dev); -static struct jedec_flash_dev * PIOS_Flash_Jedec_alloc(void); +// ! Private functions +static int32_t PIOS_Flash_Jedec_Validate(struct jedec_flash_dev *flash_dev); +static struct jedec_flash_dev *PIOS_Flash_Jedec_alloc(void); -static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev * flash_dev); -static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev * flash_dev); -static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev * flash_dev); -static int32_t PIOS_Flash_Jedec_ReleaseBus(struct jedec_flash_dev * flash_dev); -static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev * flash_dev); -static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev * flash_dev); +static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev *flash_dev); +static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev); +static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev); +static int32_t PIOS_Flash_Jedec_ReleaseBus(struct jedec_flash_dev *flash_dev); +static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev); +static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev *flash_dev); /** * @brief Allocate a new device */ -static struct jedec_flash_dev * PIOS_Flash_Jedec_alloc(void) +static struct jedec_flash_dev *PIOS_Flash_Jedec_alloc(void) { - struct jedec_flash_dev * flash_dev; + struct jedec_flash_dev *flash_dev; - flash_dev = (struct jedec_flash_dev *)pvPortMalloc(sizeof(*flash_dev)); - if (!flash_dev) return (NULL); + flash_dev = (struct jedec_flash_dev *)pvPortMalloc(sizeof(*flash_dev)); + if (!flash_dev) { + return NULL; + } - flash_dev->claimed = false; - flash_dev->magic = PIOS_JEDEC_DEV_MAGIC; + flash_dev->claimed = false; + flash_dev->magic = PIOS_JEDEC_DEV_MAGIC; #if defined(FLASH_FREERTOS) - flash_dev->transaction_lock = xSemaphoreCreateMutex(); + flash_dev->transaction_lock = xSemaphoreCreateMutex(); #endif - return(flash_dev); + return flash_dev; } /** * @brief Validate the handle to the spi device */ -static int32_t PIOS_Flash_Jedec_Validate(struct jedec_flash_dev * flash_dev) { - if (flash_dev == NULL) - return -1; - if (flash_dev->magic != PIOS_JEDEC_DEV_MAGIC) - return -2; - if (flash_dev->spi_id == 0) - return -3; - return 0; +static int32_t PIOS_Flash_Jedec_Validate(struct jedec_flash_dev *flash_dev) +{ + if (flash_dev == NULL) { + return -1; + } + if (flash_dev->magic != PIOS_JEDEC_DEV_MAGIC) { + return -2; + } + if (flash_dev->spi_id == 0) { + return -3; + } + return 0; } /** * @brief Initialize the flash device and enable write access */ -int32_t PIOS_Flash_Jedec_Init(uintptr_t * flash_id, uint32_t spi_id, uint32_t slave_num) +int32_t PIOS_Flash_Jedec_Init(uintptr_t *flash_id, uint32_t spi_id, uint32_t slave_num) { - struct jedec_flash_dev * flash_dev = PIOS_Flash_Jedec_alloc(); - if (!flash_dev) { - return -1; - } + struct jedec_flash_dev *flash_dev = PIOS_Flash_Jedec_alloc(); - flash_dev->spi_id = spi_id; - flash_dev->slave_num = slave_num; - flash_dev->cfg = NULL; + if (!flash_dev) { + return -1; + } - (void) PIOS_Flash_Jedec_ReadID(flash_dev); + flash_dev->spi_id = spi_id; + flash_dev->slave_num = slave_num; + flash_dev->cfg = NULL; - for (uint32_t i = 0; i < pios_flash_jedec_catalog_size; ++i) { - const struct pios_flash_jedec_cfg flash_jedec_entry = pios_flash_jedec_catalog[i]; + (void)PIOS_Flash_Jedec_ReadID(flash_dev); - if ((flash_dev->manufacturer == flash_jedec_entry.expect_manufacturer) - && (flash_dev->memorytype == flash_jedec_entry.expect_memorytype) - && (flash_dev->capacity == flash_jedec_entry.expect_capacity)) { - flash_dev->cfg = &pios_flash_jedec_catalog[i]; - break; - } - } + for (uint32_t i = 0; i < pios_flash_jedec_catalog_size; ++i) { + const struct pios_flash_jedec_cfg flash_jedec_entry = pios_flash_jedec_catalog[i]; - if (!flash_dev->cfg) { - return -1; - } + if ((flash_dev->manufacturer == flash_jedec_entry.expect_manufacturer) + && (flash_dev->memorytype == flash_jedec_entry.expect_memorytype) + && (flash_dev->capacity == flash_jedec_entry.expect_capacity)) { + flash_dev->cfg = &pios_flash_jedec_catalog[i]; + break; + } + } - /* Give back a handle to this flash device */ - *flash_id = (uintptr_t) flash_dev; + if (!flash_dev->cfg) { + return -1; + } - return 0; + /* Give back a handle to this flash device */ + *flash_id = (uintptr_t)flash_dev; + + return 0; } @@ -159,99 +166,105 @@ int32_t PIOS_Flash_Jedec_Init(uintptr_t * flash_id, uint32_t spi_id, uint32_t sl * @brief Claim the SPI bus for flash use and assert CS pin * @return 0 for sucess, -1 for failure to get semaphore */ -static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev * flash_dev) +static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev) { - if (PIOS_SPI_ClaimBus(flash_dev->spi_id) < 0) - return -1; + if (PIOS_SPI_ClaimBus(flash_dev->spi_id) < 0) { + return -1; + } - PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 0); - flash_dev->claimed = true; + PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 0); + flash_dev->claimed = true; - return 0; + return 0; } /** * @brief Release the SPI bus sempahore and ensure flash chip not using bus */ -static int32_t PIOS_Flash_Jedec_ReleaseBus(struct jedec_flash_dev * flash_dev) +static int32_t PIOS_Flash_Jedec_ReleaseBus(struct jedec_flash_dev *flash_dev) { - PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 1); - PIOS_SPI_ReleaseBus(flash_dev->spi_id); - flash_dev->claimed = false; - return 0; + PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 1); + PIOS_SPI_ReleaseBus(flash_dev->spi_id); + flash_dev->claimed = false; + return 0; } /** * @brief Returns if the flash chip is busy * @returns -1 for failure, 0 for not busy, 1 for busy */ -static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev * flash_dev) +static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev *flash_dev) { - int32_t status = PIOS_Flash_Jedec_ReadStatus(flash_dev); - if (status < 0) - return -1; - return status & JEDEC_STATUS_BUSY; + int32_t status = PIOS_Flash_Jedec_ReadStatus(flash_dev); + + if (status < 0) { + return -1; + } + return status & JEDEC_STATUS_BUSY; } /** * @brief Execute the write enable instruction and returns the status * @returns 0 if successful, -1 if unable to claim bus */ -static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev * flash_dev) +static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev) { - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + return -1; + } - uint8_t out[] = {JEDEC_WRITE_ENABLE}; - PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL); - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + uint8_t out[] = { JEDEC_WRITE_ENABLE }; + PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return 0; + return 0; } /** * @brief Read the status register from flash chip and return it */ -static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev * flash_dev) +static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev) { - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) - return -1; + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) { + return -1; + } - uint8_t out[2] = {JEDEC_READ_STATUS, 0}; - uint8_t in[2] = {0,0}; - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,in,sizeof(out),NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -2; - } + uint8_t out[2] = { JEDEC_READ_STATUS, 0 }; + uint8_t in[2] = { 0, 0 }; + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, in, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -2; + } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return in[1]; + return in[1]; } /** * @brief Read the status register from flash chip and return it */ -static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev * flash_dev) +static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev *flash_dev) { - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) - return -2; + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) { + return -2; + } - uint8_t out[] = {JEDEC_DEVICE_ID, 0, 0, 0}; - uint8_t in[4]; - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,in,sizeof(out),NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -3; - } + uint8_t out[] = { JEDEC_DEVICE_ID, 0, 0, 0 }; + uint8_t in[4]; + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, in, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -3; + } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - flash_dev->manufacturer = in[1]; - flash_dev->memorytype = in[2]; - flash_dev->capacity = in[3]; + flash_dev->manufacturer = in[1]; + flash_dev->memorytype = in[2]; + flash_dev->capacity = in[3]; - return flash_dev->manufacturer; + return flash_dev->manufacturer; } /********************************** @@ -269,17 +282,19 @@ static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev * flash_dev) */ static int32_t PIOS_Flash_Jedec_StartTransaction(uintptr_t flash_id) { - struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id; + struct jedec_flash_dev *flash_dev = (struct jedec_flash_dev *)flash_id; - if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { + return -1; + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreTake(flash_dev->transaction_lock, portMAX_DELAY) != pdTRUE) - return -2; + if (xSemaphoreTake(flash_dev->transaction_lock, portMAX_DELAY) != pdTRUE) { + return -2; + } #endif - return 0; + return 0; } /** @@ -288,32 +303,34 @@ static int32_t PIOS_Flash_Jedec_StartTransaction(uintptr_t flash_id) */ static int32_t PIOS_Flash_Jedec_EndTransaction(uintptr_t flash_id) { - struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id; + struct jedec_flash_dev *flash_dev = (struct jedec_flash_dev *)flash_id; - if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { + return -1; + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreGive(flash_dev->transaction_lock) != pdTRUE) - return -2; + if (xSemaphoreGive(flash_dev->transaction_lock) != pdTRUE) { + return -2; + } #endif - return 0; + return 0; } -#else /* FLASH_USE_FREERTOS_LOCKS */ +#else /* FLASH_USE_FREERTOS_LOCKS */ static int32_t PIOS_Flash_Jedec_StartTransaction(__attribute__((unused)) uintptr_t flash_id) { - return 0; + return 0; } static int32_t PIOS_Flash_Jedec_EndTransaction(__attribute__((unused)) uintptr_t flash_id) { - return 0; + return 0; } -#endif /* FLASH_USE_FREERTOS_LOCKS */ +#endif /* FLASH_USE_FREERTOS_LOCKS */ /** * @brief Erase a sector on the flash chip @@ -324,35 +341,38 @@ static int32_t PIOS_Flash_Jedec_EndTransaction(__attribute__((unused)) uintptr_t */ static int32_t PIOS_Flash_Jedec_EraseSector(uintptr_t flash_id, uint32_t addr) { - struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id; + struct jedec_flash_dev *flash_dev = (struct jedec_flash_dev *)flash_id; - if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { + return -1; + } - uint8_t ret; - uint8_t out[] = {flash_dev->cfg->sector_erase, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff}; + uint8_t ret; + uint8_t out[] = { flash_dev->cfg->sector_erase, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff }; - if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) - return ret; + if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) { + return ret; + } - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + return -1; + } - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -2; - } + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -2; + } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - // Keep polling when bus is busy too - while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) { + // Keep polling when bus is busy too + while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) { #if defined(FLASH_FREERTOS) - vTaskDelay(1); + vTaskDelay(1); #endif - } + } - return 0; + return 0; } /** @@ -361,45 +381,47 @@ static int32_t PIOS_Flash_Jedec_EraseSector(uintptr_t flash_id, uint32_t addr) */ static int32_t PIOS_Flash_Jedec_EraseChip(uintptr_t flash_id) { - struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id; + struct jedec_flash_dev *flash_dev = (struct jedec_flash_dev *)flash_id; - if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { + return -1; + } - uint8_t ret; - uint8_t out[] = {flash_dev->cfg->chip_erase}; + uint8_t ret; + uint8_t out[] = { flash_dev->cfg->chip_erase }; - if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) - return ret; + if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) { + return ret; + } - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + return -1; + } - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -2; - } + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -2; + } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - // Keep polling when bus is busy too - int i = 0; - while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) { + // Keep polling when bus is busy too + int i = 0; + while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) { #if defined(FLASH_FREERTOS) - vTaskDelay(1); - if ((i++) % 100 == 0) { + vTaskDelay(1); + if ((i++) % 100 == 0) { #else - if ((i++) % 10000 == 0) { + if ((i++) % 10000 == 0) { #endif #ifdef PIOS_LED_HEARTBEAT - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); #endif - } + } + } -} - - return 0; + return 0; } @@ -413,62 +435,70 @@ static int32_t PIOS_Flash_Jedec_EraseChip(uintptr_t flash_id) * @retval -2 Size exceeds 256 bytes * @retval -3 Length to write would wrap around page boundary */ -static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len) +static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) { - struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id; + struct jedec_flash_dev *flash_dev = (struct jedec_flash_dev *)flash_id; - if(PIOS_Flash_Jedec_Validate(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { + return -1; + } - uint8_t ret; - uint8_t out[4] = {JEDEC_PAGE_WRITE, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff}; + uint8_t ret; + uint8_t out[4] = { JEDEC_PAGE_WRITE, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff }; - /* Can only write one page at a time */ - if (len > 0x100) - return -2; + /* Can only write one page at a time */ + if (len > 0x100) { + return -2; + } - /* Ensure number of bytes fits after starting address before end of page */ - if (((addr & 0xff) + len) > 0x100) - return -3; + /* Ensure number of bytes fits after starting address before end of page */ + if (((addr & 0xff) + len) > 0x100) { + return -3; + } - if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) - return ret; + if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) { + return ret; + } - /* Execute write page command and clock in address. Keep CS asserted */ - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) - return -1; + /* Execute write page command and clock in address. Keep CS asserted */ + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + return -1; + } - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -1; - } + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -1; + } - /* Clock out data to flash */ - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,data,NULL,len,NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -1; - } + /* Clock out data to flash */ + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, data, NULL, len, NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -1; + } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - // Keep polling when bus is busy too + // Keep polling when bus is busy too #if defined(FLASH_FREERTOS) - while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) { - vTaskDelay(1); - } + while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) { + vTaskDelay(1); + } #else - // Query status this way to prevent accel chip locking us out - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) - return -1; + // Query status this way to prevent accel chip locking us out + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) { + return -1; + } - PIOS_SPI_TransferByte(flash_dev->spi_id, JEDEC_READ_STATUS); - while (PIOS_SPI_TransferByte(flash_dev->spi_id, JEDEC_READ_STATUS) & JEDEC_STATUS_BUSY); + PIOS_SPI_TransferByte(flash_dev->spi_id, JEDEC_READ_STATUS); + while (PIOS_SPI_TransferByte(flash_dev->spi_id, JEDEC_READ_STATUS) & JEDEC_STATUS_BUSY) { + ; + } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); #endif - return 0; + return 0; } /** @@ -483,53 +513,58 @@ static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uin */ static int32_t PIOS_Flash_Jedec_WriteChunks(uintptr_t flash_id, uint32_t addr, struct pios_flash_chunk chunks[], uint32_t num) { - struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id; + struct jedec_flash_dev *flash_dev = (struct jedec_flash_dev *)flash_id; - if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { + return -1; + } - uint8_t ret; - uint8_t out[4] = {JEDEC_PAGE_WRITE, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff}; + uint8_t ret; + uint8_t out[4] = { JEDEC_PAGE_WRITE, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff }; - /* Can only write one page at a time */ - uint32_t len = 0; - for (uint32_t i = 0; i < num; i++) - len += chunks[i].len; + /* Can only write one page at a time */ + uint32_t len = 0; + for (uint32_t i = 0; i < num; i++) { + len += chunks[i].len; + } - if (len > 0x100) - return -2; + if (len > 0x100) { + return -2; + } - /* Ensure number of bytes fits after starting address before end of page */ - if (((addr & 0xff) + len) > 0x100) - return -3; + /* Ensure number of bytes fits after starting address before end of page */ + if (((addr & 0xff) + len) > 0x100) { + return -3; + } - if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) - return ret; + if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) { + return ret; + } - /* Execute write page command and clock in address. Keep CS asserted */ - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) - return -1; + /* Execute write page command and clock in address. Keep CS asserted */ + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + return -1; + } - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -1; - } + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -1; + } - for (uint32_t i = 0; i < num; i++) { - struct pios_flash_chunk * chunk = &chunks[i]; + for (uint32_t i = 0; i < num; i++) { + struct pios_flash_chunk *chunk = &chunks[i]; - /* Clock out data to flash */ - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,chunk->addr,NULL,chunk->len,NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -1; - } + /* Clock out data to flash */ + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, chunk->addr, NULL, chunk->len, NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -1; + } + } + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + // Skip checking for busy with this to get OS running again fast - // Skip checking for busy with this to get OS running again fast - - return 0; + return 0; } /** @@ -540,44 +575,46 @@ static int32_t PIOS_Flash_Jedec_WriteChunks(uintptr_t flash_id, uint32_t addr, s * @return Zero if success or error code * @retval -1 Unable to claim SPI bus */ -static int32_t PIOS_Flash_Jedec_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len) +static int32_t PIOS_Flash_Jedec_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) { - struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id; + struct jedec_flash_dev *flash_dev = (struct jedec_flash_dev *)flash_id; - if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { + return -1; + } - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) == -1) - return -1; + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) == -1) { + return -1; + } - /* Execute read command and clock in address. Keep CS asserted */ - uint8_t out[] = {JEDEC_READ_DATA, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff}; + /* Execute read command and clock in address. Keep CS asserted */ + uint8_t out[] = { JEDEC_READ_DATA, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff }; - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -2; - } + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -2; + } - /* Copy the transfer data to the buffer */ - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,NULL,data,len,NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -3; - } + /* Copy the transfer data to the buffer */ + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, NULL, data, len, NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -3; + } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return 0; + return 0; } /* Provide a flash driver to external drivers */ const struct pios_flash_driver pios_jedec_flash_driver = { - .start_transaction = PIOS_Flash_Jedec_StartTransaction, - .end_transaction = PIOS_Flash_Jedec_EndTransaction, - .erase_chip = PIOS_Flash_Jedec_EraseChip, - .erase_sector = PIOS_Flash_Jedec_EraseSector, - .write_chunks = PIOS_Flash_Jedec_WriteChunks, - .write_data = PIOS_Flash_Jedec_WriteData, - .read_data = PIOS_Flash_Jedec_ReadData, + .start_transaction = PIOS_Flash_Jedec_StartTransaction, + .end_transaction = PIOS_Flash_Jedec_EndTransaction, + .erase_chip = PIOS_Flash_Jedec_EraseChip, + .erase_sector = PIOS_Flash_Jedec_EraseSector, + .write_chunks = PIOS_Flash_Jedec_WriteChunks, + .write_data = PIOS_Flash_Jedec_WriteData, + .read_data = PIOS_Flash_Jedec_ReadData, }; #endif /* PIOS_INCLUDE_FLASH */ diff --git a/flight/pios/common/pios_flashfs_logfs.c b/flight/pios/common/pios_flashfs_logfs.c index ed6b95131..d0961c797 100644 --- a/flight/pios/common/pios_flashfs_logfs.c +++ b/flight/pios/common/pios_flashfs_logfs.c @@ -40,20 +40,20 @@ */ struct logfs_state { - const struct flashfs_logfs_cfg * cfg; - bool mounted; - uint8_t active_arena_id; + const struct flashfs_logfs_cfg *cfg; + bool mounted; + uint8_t active_arena_id; - /* NOTE: num_active_slots + num_free_slots will not typically add - * up to the number of slots in the arena since some of the - * slots will be obsolete or otherwise invalidated - */ - uint16_t num_free_slots; /* slots in free state */ - uint16_t num_active_slots; /* slots in active state */ + /* NOTE: num_active_slots + num_free_slots will not typically add + * up to the number of slots in the arena since some of the + * slots will be obsolete or otherwise invalidated + */ + uint16_t num_free_slots; /* slots in free state */ + uint16_t num_active_slots; /* slots in active state */ - /* Underlying flash driver glue */ - const struct pios_flash_driver * driver; - uintptr_t flash_id; + /* Underlying flash driver glue */ + const struct pios_flash_driver *driver; + uintptr_t flash_id; }; static struct logfs_state logfs; @@ -68,12 +68,12 @@ static struct logfs_state logfs; */ static uintptr_t logfs_get_addr(uint8_t arena_id, uint16_t slot_id) { - PIOS_Assert(arena_id < (logfs.cfg->total_fs_size / logfs.cfg->arena_size)); - PIOS_Assert(slot_id < (logfs.cfg->arena_size / logfs.cfg->slot_size)); + PIOS_Assert(arena_id < (logfs.cfg->total_fs_size / logfs.cfg->arena_size)); + PIOS_Assert(slot_id < (logfs.cfg->arena_size / logfs.cfg->slot_size)); - return (logfs.cfg->start_offset + - (arena_id * logfs.cfg->arena_size) + - (slot_id * logfs.cfg->slot_size)); + return logfs.cfg->start_offset + + (arena_id * logfs.cfg->arena_size) + + (slot_id * logfs.cfg->slot_size); } /* @@ -82,30 +82,30 @@ static uintptr_t logfs_get_addr(uint8_t arena_id, uint16_t slot_id) * of earlier ones in NOR flash without an erase cycle. */ enum arena_state { - /* - * The STM32F30X flash subsystem is only capable of - * writing words or halfwords. In this case we use halfwords. - * In addition to that it is only capable to write to erased - * cells (0xffff) or write a cell from anything to (0x0000). - * To cope with this, the F3 needs carefully crafted enum values. - * For this to work the underlying flash driver has to - * check each halfword if it has changed before writing. - */ - ARENA_STATE_ERASED = 0xFFFFFFFF, - ARENA_STATE_RESERVED = 0xE6E6FFFF, - ARENA_STATE_ACTIVE = 0xE6E66666, - ARENA_STATE_OBSOLETE = 0x00000000, + /* + * The STM32F30X flash subsystem is only capable of + * writing words or halfwords. In this case we use halfwords. + * In addition to that it is only capable to write to erased + * cells (0xffff) or write a cell from anything to (0x0000). + * To cope with this, the F3 needs carefully crafted enum values. + * For this to work the underlying flash driver has to + * check each halfword if it has changed before writing. + */ + ARENA_STATE_ERASED = 0xFFFFFFFF, + ARENA_STATE_RESERVED = 0xE6E6FFFF, + ARENA_STATE_ACTIVE = 0xE6E66666, + ARENA_STATE_OBSOLETE = 0x00000000, }; struct arena_header { - uint32_t magic; - enum arena_state state; + uint32_t magic; + enum arena_state state; } __attribute__((packed)); /**************************************** - * Arena life-cycle transition functions - ****************************************/ +* Arena life-cycle transition functions +****************************************/ /** * @brief Erases all sectors within the given arena and sets arena to erased state. @@ -114,33 +114,33 @@ struct arena_header { */ static int32_t logfs_erase_arena(uint8_t arena_id) { - uintptr_t arena_addr = logfs_get_addr (arena_id, 0); + uintptr_t arena_addr = logfs_get_addr(arena_id, 0); - /* Erase all of the sectors in the arena */ - for (uint8_t sector_id = 0; - sector_id < (logfs.cfg->arena_size / logfs.cfg->sector_size); - sector_id++) { - if (logfs.driver->erase_sector(logfs.flash_id, - arena_addr + (sector_id * logfs.cfg->sector_size))) { - return -1; - } - } + /* Erase all of the sectors in the arena */ + for (uint8_t sector_id = 0; + sector_id < (logfs.cfg->arena_size / logfs.cfg->sector_size); + sector_id++) { + if (logfs.driver->erase_sector(logfs.flash_id, + arena_addr + (sector_id * logfs.cfg->sector_size))) { + return -1; + } + } - /* Mark this arena as fully erased */ - struct arena_header arena_hdr = { - .magic = logfs.cfg->fs_magic, - .state = ARENA_STATE_ERASED, - }; + /* Mark this arena as fully erased */ + struct arena_header arena_hdr = { + .magic = logfs.cfg->fs_magic, + .state = ARENA_STATE_ERASED, + }; - if (logfs.driver->write_data(logfs.flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof(arena_hdr)) != 0) { - return -2; - } + if (logfs.driver->write_data(logfs.flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + return -2; + } - /* Arena is ready to be activated */ - return 0; + /* Arena is ready to be activated */ + return 0; } /** @@ -149,36 +149,37 @@ static int32_t logfs_erase_arena(uint8_t arena_id) * @note Arena must have been previously erased before calling this * @note Must be called while holding the flash transaction lock */ -static int32_t logfs_reserve_arena (uint8_t arena_id) +static int32_t logfs_reserve_arena(uint8_t arena_id) { - uintptr_t arena_addr = logfs_get_addr (arena_id, 0); + uintptr_t arena_addr = logfs_get_addr(arena_id, 0); - /* Read in the current arena header */ - struct arena_header arena_hdr; - if (logfs.driver->read_data(logfs.flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof(arena_hdr)) != 0) { - return -1; - } - if (arena_hdr.state != ARENA_STATE_ERASED) { - /* Arena was not erased, can't reserve it */ - return -2; - } + /* Read in the current arena header */ + struct arena_header arena_hdr; - /* Set the arena state to reserved */ - arena_hdr.state = ARENA_STATE_RESERVED; + if (logfs.driver->read_data(logfs.flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + return -1; + } + if (arena_hdr.state != ARENA_STATE_ERASED) { + /* Arena was not erased, can't reserve it */ + return -2; + } - /* Write the arena header back to flash */ - if (logfs.driver->write_data(logfs.flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof(arena_hdr)) != 0) { - return -3; - } + /* Set the arena state to reserved */ + arena_hdr.state = ARENA_STATE_RESERVED; - /* Arena is ready to be filled */ - return 0; + /* Write the arena header back to flash */ + if (logfs.driver->write_data(logfs.flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + return -3; + } + + /* Arena is ready to be filled */ + return 0; } /** @@ -188,17 +189,18 @@ static int32_t logfs_reserve_arena (uint8_t arena_id) */ static int32_t logfs_erase_all_arenas() { - uint16_t num_arenas = logfs.cfg->total_fs_size / logfs.cfg->arena_size; + uint16_t num_arenas = logfs.cfg->total_fs_size / logfs.cfg->arena_size; - for (uint16_t arena = 0; arena < num_arenas; arena++) { + for (uint16_t arena = 0; arena < num_arenas; arena++) { #ifdef PIOS_LED_HEARTBEAT - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); #endif - if (logfs_erase_arena(arena) != 0) - return -1; - } + if (logfs_erase_arena(arena) != 0) { + return -1; + } + } - return 0; + return 0; } /** @@ -209,34 +211,35 @@ static int32_t logfs_erase_all_arenas() */ static int32_t logfs_activate_arena(uint8_t arena_id) { - uintptr_t arena_addr = logfs_get_addr(arena_id, 0); + uintptr_t arena_addr = logfs_get_addr(arena_id, 0); - /* Make sure this arena has been previously erased */ - struct arena_header arena_hdr; - if (logfs.driver->read_data(logfs.flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof (arena_hdr)) != 0) { - /* Failed to read arena header */ - return -1; - } - if ((arena_hdr.state != ARENA_STATE_RESERVED) && - (arena_hdr.state != ARENA_STATE_ERASED)) { - /* Arena was not erased or reserved, can't activate it */ - return -2; - } + /* Make sure this arena has been previously erased */ + struct arena_header arena_hdr; - /* Mark this arena as active */ - arena_hdr.state = ARENA_STATE_ACTIVE; - if (logfs.driver->write_data(logfs.flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof(arena_hdr)) != 0) { - return -3; - } + if (logfs.driver->read_data(logfs.flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + /* Failed to read arena header */ + return -1; + } + if ((arena_hdr.state != ARENA_STATE_RESERVED) && + (arena_hdr.state != ARENA_STATE_ERASED)) { + /* Arena was not erased or reserved, can't activate it */ + return -2; + } - /* The arena is now activated and the log may be mounted */ - return 0; + /* Mark this arena as active */ + arena_hdr.state = ARENA_STATE_ACTIVE; + if (logfs.driver->write_data(logfs.flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + return -3; + } + + /* The arena is now activated and the log may be mounted */ + return 0; } /** @@ -247,37 +250,37 @@ static int32_t logfs_activate_arena(uint8_t arena_id) */ static int32_t logfs_obsolete_arena(uint8_t arena_id) { - uintptr_t arena_addr = logfs_get_addr (arena_id, 0); + uintptr_t arena_addr = logfs_get_addr(arena_id, 0); - /* We shouldn't be retiring the currently active arena */ - PIOS_Assert(!logfs.mounted); + /* We shouldn't be retiring the currently active arena */ + PIOS_Assert(!logfs.mounted); - /* Make sure this arena was previously active */ - struct arena_header arena_hdr; - if (logfs.driver->read_data(logfs.flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof (arena_hdr)) != 0) { - /* Failed to read arena header */ - return -1; - } + /* Make sure this arena was previously active */ + struct arena_header arena_hdr; + if (logfs.driver->read_data(logfs.flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + /* Failed to read arena header */ + return -1; + } - if (arena_hdr.state != ARENA_STATE_ACTIVE) { - /* Arena was not previously active, can't obsolete it */ - return -2; - } + if (arena_hdr.state != ARENA_STATE_ACTIVE) { + /* Arena was not previously active, can't obsolete it */ + return -2; + } - /* Mark this arena as obsolete */ - arena_hdr.state = ARENA_STATE_OBSOLETE; - if (logfs.driver->write_data(logfs.flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof(arena_hdr)) != 0) { - return -3; - } + /* Mark this arena as obsolete */ + arena_hdr.state = ARENA_STATE_OBSOLETE; + if (logfs.driver->write_data(logfs.flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + return -3; + } - /* Arena is now obsoleted */ - return 0; + /* Arena is now obsoleted */ + return 0; } /** @@ -289,28 +292,28 @@ static int32_t logfs_obsolete_arena(uint8_t arena_id) */ static int32_t logfs_find_active_arena() { - /* Search for the lowest numbered active arena */ - for (uint8_t arena_id = 0; - arena_id < logfs.cfg->total_fs_size / logfs.cfg->arena_size; - arena_id++) { - uintptr_t arena_addr = logfs_get_addr (arena_id, 0); - /* Load the arena header */ - struct arena_header arena_hdr; - if (logfs.driver->read_data(logfs.flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof (arena_hdr)) != 0) { - return -2; - } - if ((arena_hdr.state == ARENA_STATE_ACTIVE) && - (arena_hdr.magic == logfs.cfg->fs_magic)) { - /* This is the first active arena */ - return arena_id; - } - } + /* Search for the lowest numbered active arena */ + for (uint8_t arena_id = 0; + arena_id < logfs.cfg->total_fs_size / logfs.cfg->arena_size; + arena_id++) { + uintptr_t arena_addr = logfs_get_addr(arena_id, 0); + /* Load the arena header */ + struct arena_header arena_hdr; + if (logfs.driver->read_data(logfs.flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + return -2; + } + if ((arena_hdr.state == ARENA_STATE_ACTIVE) && + (arena_hdr.magic == logfs.cfg->fs_magic)) { + /* This is the first active arena */ + return arena_id; + } + } - /* Didn't find an active arena */ - return -1; + /* Didn't find an active arena */ + return -1; } /* @@ -319,69 +322,69 @@ static int32_t logfs_find_active_arena() * of earlier ones in NOR flash without an erase cycle. */ enum slot_state { - /* - * The STM32F30X flash subsystem is only capable of - * writing words or halfwords. In this case we use halfwords. - * In addition to that it is only capable to write to erased - * cells (0xffff) or write a cell from anything to (0x0000). - * To cope with this, the F3 needs carfully crafted enum values. - * For this to work the underlying flash driver has to - * check each halfword if it has changed before writing. - */ - SLOT_STATE_EMPTY = 0xFFFFFFFF, - SLOT_STATE_RESERVED = 0xFAFAFFFF, - SLOT_STATE_ACTIVE = 0xFAFAAAAA, - SLOT_STATE_OBSOLETE = 0x00000000, + /* + * The STM32F30X flash subsystem is only capable of + * writing words or halfwords. In this case we use halfwords. + * In addition to that it is only capable to write to erased + * cells (0xffff) or write a cell from anything to (0x0000). + * To cope with this, the F3 needs carfully crafted enum values. + * For this to work the underlying flash driver has to + * check each halfword if it has changed before writing. + */ + SLOT_STATE_EMPTY = 0xFFFFFFFF, + SLOT_STATE_RESERVED = 0xFAFAFFFF, + SLOT_STATE_ACTIVE = 0xFAFAAAAA, + SLOT_STATE_OBSOLETE = 0x00000000, }; struct slot_header { - enum slot_state state; - uint32_t obj_id; - uint16_t obj_inst_id; - uint16_t obj_size; + enum slot_state state; + uint32_t obj_id; + uint16_t obj_inst_id; + uint16_t obj_size; } __attribute__((packed)); /* NOTE: Must be called while holding the flash transaction lock */ -static int32_t logfs_raw_copy_bytes (uintptr_t src_addr, uint16_t src_size, uintptr_t dst_addr) +static int32_t logfs_raw_copy_bytes(uintptr_t src_addr, uint16_t src_size, uintptr_t dst_addr) { #define RAW_COPY_BLOCK_SIZE 16 - uint8_t data_block[RAW_COPY_BLOCK_SIZE]; + uint8_t data_block[RAW_COPY_BLOCK_SIZE]; - while (src_size) { - uint16_t blk_size; - if (src_size >= RAW_COPY_BLOCK_SIZE) { - /* Copy a full block */ - blk_size = RAW_COPY_BLOCK_SIZE; - } else { - /* Copy the remainder */ - blk_size = src_size; - } + while (src_size) { + uint16_t blk_size; + if (src_size >= RAW_COPY_BLOCK_SIZE) { + /* Copy a full block */ + blk_size = RAW_COPY_BLOCK_SIZE; + } else { + /* Copy the remainder */ + blk_size = src_size; + } - /* Read a block of data from source */ - if (logfs.driver->read_data(logfs.flash_id, - src_addr, - data_block, - blk_size) != 0) { - /* Failed to read next chunk from source */ - return -1; - } + /* Read a block of data from source */ + if (logfs.driver->read_data(logfs.flash_id, + src_addr, + data_block, + blk_size) != 0) { + /* Failed to read next chunk from source */ + return -1; + } - /* Write a block of data to destination */ - if (logfs.driver->write_data(logfs.flash_id, - dst_addr, - data_block, - blk_size) != 0) { - /* Failed to write chunk to destination */ - return -2; - } + /* Write a block of data to destination */ + if (logfs.driver->write_data(logfs.flash_id, + dst_addr, + data_block, + blk_size) != 0) { + /* Failed to write chunk to destination */ + return -2; + } - /* Update the src/dst pointers */ - src_size -= blk_size; - src_addr += blk_size; - dst_addr += blk_size; - } + /* Update the src/dst pointers */ + src_size -= blk_size; + src_addr += blk_size; + dst_addr += blk_size; + } - return 0; + return 0; } /* @@ -391,7 +394,7 @@ static int32_t logfs_raw_copy_bytes (uintptr_t src_addr, uint16_t src_size, uint */ static bool logfs_fs_is_full(void) { - return (logfs.num_active_slots == (logfs.cfg->arena_size / logfs.cfg->slot_size) - 1); + return logfs.num_active_slots == (logfs.cfg->arena_size / logfs.cfg->slot_size) - 1; } /* @@ -401,396 +404,403 @@ static bool logfs_fs_is_full(void) */ static bool logfs_log_is_full(void) { - return (logfs.num_free_slots == 0); + return logfs.num_free_slots == 0; } static int32_t logfs_unmount_log(void) { - PIOS_Assert (logfs.mounted); + PIOS_Assert(logfs.mounted); - logfs.num_active_slots = 0; - logfs.num_free_slots = 0; - logfs.mounted = false; + logfs.num_active_slots = 0; + logfs.num_free_slots = 0; + logfs.mounted = false; - return 0; + return 0; } static int32_t logfs_mount_log(uint8_t arena_id) { - PIOS_Assert (!logfs.mounted); + PIOS_Assert(!logfs.mounted); - logfs.num_active_slots = 0; - logfs.num_free_slots = 0; - logfs.active_arena_id = arena_id; + logfs.num_active_slots = 0; + logfs.num_free_slots = 0; + logfs.active_arena_id = arena_id; - /* Scan the log to find out how full it is */ - for (uint16_t slot_id = 1; - slot_id < (logfs.cfg->arena_size / logfs.cfg->slot_size); - slot_id++) { - struct slot_header slot_hdr; - uintptr_t slot_addr = logfs_get_addr (logfs.active_arena_id, slot_id); - if (logfs.driver->read_data(logfs.flash_id, - slot_addr, - (uint8_t *)&slot_hdr, - sizeof (slot_hdr)) != 0) { - return -1; - } + /* Scan the log to find out how full it is */ + for (uint16_t slot_id = 1; + slot_id < (logfs.cfg->arena_size / logfs.cfg->slot_size); + slot_id++) { + struct slot_header slot_hdr; + uintptr_t slot_addr = logfs_get_addr(logfs.active_arena_id, slot_id); + if (logfs.driver->read_data(logfs.flash_id, + slot_addr, + (uint8_t *)&slot_hdr, + sizeof(slot_hdr)) != 0) { + return -1; + } - /* - * Empty slots must be in a continguous block at the - * end of the arena. - */ - PIOS_Assert (slot_hdr.state == SLOT_STATE_EMPTY || - logfs.num_free_slots == 0); + /* + * Empty slots must be in a continguous block at the + * end of the arena. + */ + PIOS_Assert(slot_hdr.state == SLOT_STATE_EMPTY || + logfs.num_free_slots == 0); - switch (slot_hdr.state) { - case SLOT_STATE_EMPTY: - logfs.num_free_slots++; - break; - case SLOT_STATE_ACTIVE: - logfs.num_active_slots++; - break; - case SLOT_STATE_RESERVED: - case SLOT_STATE_OBSOLETE: - break; - } - } + switch (slot_hdr.state) { + case SLOT_STATE_EMPTY: + logfs.num_free_slots++; + break; + case SLOT_STATE_ACTIVE: + logfs.num_active_slots++; + break; + case SLOT_STATE_RESERVED: + case SLOT_STATE_OBSOLETE: + break; + } + } - /* Scan is complete, mark the arena mounted */ - logfs.active_arena_id = arena_id; - logfs.mounted = true; + /* Scan is complete, mark the arena mounted */ + logfs.active_arena_id = arena_id; + logfs.mounted = true; - return 0; + return 0; } /** * @brief Initialize the flash object setting FS * @return 0 if success, -1 if failure */ -int32_t PIOS_FLASHFS_Logfs_Init(uintptr_t * fs_id, const struct flashfs_logfs_cfg * cfg, const struct pios_flash_driver * driver, uintptr_t flash_id) +int32_t PIOS_FLASHFS_Logfs_Init(uintptr_t *fs_id, const struct flashfs_logfs_cfg *cfg, const struct pios_flash_driver *driver, uintptr_t flash_id) { - PIOS_Assert(cfg); - PIOS_Assert(fs_id); - PIOS_Assert(driver); + PIOS_Assert(cfg); + PIOS_Assert(fs_id); + PIOS_Assert(driver); - /* We must have at least 2 arenas for garbage collection to work */ - PIOS_Assert((cfg->total_fs_size / cfg->arena_size > 1)); + /* We must have at least 2 arenas for garbage collection to work */ + PIOS_Assert((cfg->total_fs_size / cfg->arena_size > 1)); - /* Make sure the underlying flash driver provides the minimal set of required methods */ - PIOS_Assert(driver->start_transaction); - PIOS_Assert(driver->end_transaction); - PIOS_Assert(driver->erase_sector); - PIOS_Assert(driver->write_data); - PIOS_Assert(driver->read_data); + /* Make sure the underlying flash driver provides the minimal set of required methods */ + PIOS_Assert(driver->start_transaction); + PIOS_Assert(driver->end_transaction); + PIOS_Assert(driver->erase_sector); + PIOS_Assert(driver->write_data); + PIOS_Assert(driver->read_data); - /* Bind configuration parameters to this filesystem instance */ - logfs.cfg = cfg; /* filesystem configuration */ - logfs.driver = driver; /* lower-level flash driver */ - logfs.flash_id = flash_id; /* lower-level flash device id */ - logfs.mounted = false; + /* Bind configuration parameters to this filesystem instance */ + logfs.cfg = cfg; /* filesystem configuration */ + logfs.driver = driver; /* lower-level flash driver */ + logfs.flash_id = flash_id; /* lower-level flash device id */ + logfs.mounted = false; - int8_t rc; + int8_t rc; - if (logfs.driver->start_transaction(logfs.flash_id) != 0) { - rc = -1; - goto out_exit; - } + if (logfs.driver->start_transaction(logfs.flash_id) != 0) { + rc = -1; + goto out_exit; + } - bool found = false; - int32_t arena_id; - for (uint8_t try = 0; !found && try < 2; try++) { - /* Find the active arena */ - arena_id = logfs_find_active_arena(); - if (arena_id >= 0) { - /* Found the active arena */ - found = true; - break; - } else { - /* No active arena found, erase and activate arena 0 */ - if (logfs_erase_arena(0) != 0) - break; - if (logfs_activate_arena(0) != 0) - break; - } - } + bool found = false; + int32_t arena_id; + for (uint8_t try = 0; !found && try < 2; try++) { + /* Find the active arena */ + arena_id = logfs_find_active_arena(); + if (arena_id >= 0) { + /* Found the active arena */ + found = true; + break; + } else { + /* No active arena found, erase and activate arena 0 */ + if (logfs_erase_arena(0) != 0) { + break; + } + if (logfs_activate_arena(0) != 0) { + break; + } + } + } - if (!found) { - /* Still no active arena, something is broken */ - rc = -2; - goto out_end_trans; - } + if (!found) { + /* Still no active arena, something is broken */ + rc = -2; + goto out_end_trans; + } - /* We've found an active arena, mount it */ - if (logfs_mount_log(arena_id) != 0) { - /* Failed to mount the log, something is broken */ - rc = -3; - goto out_end_trans; - } + /* We've found an active arena, mount it */ + if (logfs_mount_log(arena_id) != 0) { + /* Failed to mount the log, something is broken */ + rc = -3; + goto out_end_trans; + } - /* Log has been mounted */ - rc = 0; + /* Log has been mounted */ + rc = 0; - *fs_id = (uintptr_t) &logfs; + *fs_id = (uintptr_t)&logfs; out_end_trans: - logfs.driver->end_transaction(logfs.flash_id); + logfs.driver->end_transaction(logfs.flash_id); out_exit: - return rc; + return rc; } /* NOTE: Must be called while holding the flash transaction lock */ -static int32_t logfs_garbage_collect (void) { - PIOS_Assert (logfs.mounted); - - /* Source arena is the active arena */ - uint8_t src_arena_id = logfs.active_arena_id; - - /* Compute destination arena */ - uint8_t dst_arena_id = (logfs.active_arena_id + 1) % (logfs.cfg->total_fs_size / logfs.cfg->arena_size); - - /* Erase destination arena */ - if (logfs_erase_arena (dst_arena_id) != 0) { - return -1; - } - - /* Reserve the destination arena so we can start filling it */ - if (logfs_reserve_arena (dst_arena_id) != 0) { - /* Unable to reserve the arena */ - return -2; - } - - /* Copy active slots from active arena to destination arena */ - uint16_t dst_slot_id = 1; - for (uint16_t src_slot_id = 1; - src_slot_id < (logfs.cfg->arena_size / logfs.cfg->slot_size); - src_slot_id++) { - struct slot_header slot_hdr; - uintptr_t src_addr = logfs_get_addr (src_arena_id, src_slot_id); - if (logfs.driver->read_data(logfs.flash_id, - src_addr, - (uint8_t *)&slot_hdr, - sizeof (slot_hdr)) != 0) { - return -3; - } - - if (slot_hdr.state == SLOT_STATE_ACTIVE) { - uintptr_t dst_addr = logfs_get_addr (dst_arena_id, dst_slot_id); - if (logfs_raw_copy_bytes(src_addr, - sizeof(slot_hdr) + slot_hdr.obj_size, - dst_addr) != 0) { - /* Failed to copy all bytes */ - return -4; - } - dst_slot_id++; - } - } - - /* Activate the destination arena */ - if (logfs_activate_arena (dst_arena_id) != 0) { - return -5; - } - - /* Unmount the source arena */ - if (logfs_unmount_log () != 0) { - return -6; - } - - /* Obsolete the source arena */ - if (logfs_obsolete_arena (src_arena_id) != 0) { - return -7; - } - - /* Mount the new arena */ - if (logfs_mount_log (dst_arena_id) != 0) { - return -8; - } - - return 0; -} - -/* NOTE: Must be called while holding the flash transaction lock */ -static int16_t logfs_object_find_next (struct slot_header * slot_hdr, uint16_t * curr_slot, uint32_t obj_id, uint16_t obj_inst_id) +static int32_t logfs_garbage_collect(void) { - PIOS_Assert(slot_hdr); - PIOS_Assert(curr_slot); + PIOS_Assert(logfs.mounted); - /* First slot in the arena is reserved for arena header, skip it. */ - if (*curr_slot == 0) *curr_slot = 1; + /* Source arena is the active arena */ + uint8_t src_arena_id = logfs.active_arena_id; - for (uint16_t slot_id = *curr_slot; - slot_id < (logfs.cfg->arena_size / logfs.cfg->slot_size); - slot_id++) { - uintptr_t slot_addr = logfs_get_addr (logfs.active_arena_id, slot_id); + /* Compute destination arena */ + uint8_t dst_arena_id = (logfs.active_arena_id + 1) % (logfs.cfg->total_fs_size / logfs.cfg->arena_size); - if (logfs.driver->read_data(logfs.flash_id, - slot_addr, - (uint8_t *)slot_hdr, - sizeof (*slot_hdr)) != 0) { - return -2; - } - if (slot_hdr->state == SLOT_STATE_EMPTY) { - /* We hit the end of the log */ - break; - } - if (slot_hdr->state == SLOT_STATE_ACTIVE && - slot_hdr->obj_id == obj_id && - slot_hdr->obj_inst_id == obj_inst_id) { - /* Found what we were looking for */ - *curr_slot = slot_id; - return 0; - } - } + /* Erase destination arena */ + if (logfs_erase_arena(dst_arena_id) != 0) { + return -1; + } - /* No matching entry was found */ - return -1; + /* Reserve the destination arena so we can start filling it */ + if (logfs_reserve_arena(dst_arena_id) != 0) { + /* Unable to reserve the arena */ + return -2; + } + + /* Copy active slots from active arena to destination arena */ + uint16_t dst_slot_id = 1; + for (uint16_t src_slot_id = 1; + src_slot_id < (logfs.cfg->arena_size / logfs.cfg->slot_size); + src_slot_id++) { + struct slot_header slot_hdr; + uintptr_t src_addr = logfs_get_addr(src_arena_id, src_slot_id); + if (logfs.driver->read_data(logfs.flash_id, + src_addr, + (uint8_t *)&slot_hdr, + sizeof(slot_hdr)) != 0) { + return -3; + } + + if (slot_hdr.state == SLOT_STATE_ACTIVE) { + uintptr_t dst_addr = logfs_get_addr(dst_arena_id, dst_slot_id); + if (logfs_raw_copy_bytes(src_addr, + sizeof(slot_hdr) + slot_hdr.obj_size, + dst_addr) != 0) { + /* Failed to copy all bytes */ + return -4; + } + dst_slot_id++; + } + } + + /* Activate the destination arena */ + if (logfs_activate_arena(dst_arena_id) != 0) { + return -5; + } + + /* Unmount the source arena */ + if (logfs_unmount_log() != 0) { + return -6; + } + + /* Obsolete the source arena */ + if (logfs_obsolete_arena(src_arena_id) != 0) { + return -7; + } + + /* Mount the new arena */ + if (logfs_mount_log(dst_arena_id) != 0) { + return -8; + } + + return 0; +} + +/* NOTE: Must be called while holding the flash transaction lock */ +static int16_t logfs_object_find_next(struct slot_header *slot_hdr, uint16_t *curr_slot, uint32_t obj_id, uint16_t obj_inst_id) +{ + PIOS_Assert(slot_hdr); + PIOS_Assert(curr_slot); + + /* First slot in the arena is reserved for arena header, skip it. */ + if (*curr_slot == 0) { + *curr_slot = 1; + } + + for (uint16_t slot_id = *curr_slot; + slot_id < (logfs.cfg->arena_size / logfs.cfg->slot_size); + slot_id++) { + uintptr_t slot_addr = logfs_get_addr(logfs.active_arena_id, slot_id); + + if (logfs.driver->read_data(logfs.flash_id, + slot_addr, + (uint8_t *)slot_hdr, + sizeof(*slot_hdr)) != 0) { + return -2; + } + if (slot_hdr->state == SLOT_STATE_EMPTY) { + /* We hit the end of the log */ + break; + } + if (slot_hdr->state == SLOT_STATE_ACTIVE && + slot_hdr->obj_id == obj_id && + slot_hdr->obj_inst_id == obj_inst_id) { + /* Found what we were looking for */ + *curr_slot = slot_id; + return 0; + } + } + + /* No matching entry was found */ + return -1; } /* NOTE: Must be called while holding the flash transaction lock */ /* OPTIMIZE: could trust that there is at most one active version of every object and terminate the search when we find one */ -static int8_t logfs_delete_object (uint32_t obj_id, uint16_t obj_inst_id) +static int8_t logfs_delete_object(uint32_t obj_id, uint16_t obj_inst_id) { - int8_t rc; + int8_t rc; - bool more = true; - uint16_t curr_slot_id = 0; - do { - struct slot_header slot_hdr; - switch (logfs_object_find_next (&slot_hdr, &curr_slot_id, obj_id, obj_inst_id)) { - case 0: - /* Found a matching slot. Obsolete it. */ - slot_hdr.state = SLOT_STATE_OBSOLETE; - uintptr_t slot_addr = logfs_get_addr (logfs.active_arena_id, curr_slot_id); + bool more = true; + uint16_t curr_slot_id = 0; - if (logfs.driver->write_data(logfs.flash_id, - slot_addr, - (uint8_t *)&slot_hdr, - sizeof(slot_hdr)) != 0) { - rc = -2; - goto out_exit; - } - /* Object has been successfully obsoleted and is no longer active */ - logfs.num_active_slots--; - break; - case -1: - /* Search completed, object not found */ - more = false; - rc = 0; - break; - default: - /* Error occurred during search */ - rc = -1; - goto out_exit; - } - } while (more); + do { + struct slot_header slot_hdr; + switch (logfs_object_find_next(&slot_hdr, &curr_slot_id, obj_id, obj_inst_id)) { + case 0: + /* Found a matching slot. Obsolete it. */ + slot_hdr.state = SLOT_STATE_OBSOLETE; + uintptr_t slot_addr = logfs_get_addr(logfs.active_arena_id, curr_slot_id); + + if (logfs.driver->write_data(logfs.flash_id, + slot_addr, + (uint8_t *)&slot_hdr, + sizeof(slot_hdr)) != 0) { + rc = -2; + goto out_exit; + } + /* Object has been successfully obsoleted and is no longer active */ + logfs.num_active_slots--; + break; + case -1: + /* Search completed, object not found */ + more = false; + rc = 0; + break; + default: + /* Error occurred during search */ + rc = -1; + goto out_exit; + } + } while (more); out_exit: - return rc; + return rc; } /* NOTE: Must be called while holding the flash transaction lock */ -static int8_t logfs_reserve_free_slot (uint16_t * slot_id, struct slot_header * slot_hdr, uint32_t obj_id, uint16_t obj_inst_id, uint16_t obj_size) +static int8_t logfs_reserve_free_slot(uint16_t *slot_id, struct slot_header *slot_hdr, uint32_t obj_id, uint16_t obj_inst_id, uint16_t obj_size) { - PIOS_Assert(slot_id); - PIOS_Assert(slot_hdr); + PIOS_Assert(slot_id); + PIOS_Assert(slot_hdr); - if (logfs.num_free_slots < 1) { - /* No free slots to allocate */ - return -1; - } + if (logfs.num_free_slots < 1) { + /* No free slots to allocate */ + return -1; + } - if (obj_size > (logfs.cfg->slot_size - sizeof (slot_hdr))) { - /* This object is too big for the slot */ - return -2; - } + if (obj_size > (logfs.cfg->slot_size - sizeof(slot_hdr))) { + /* This object is too big for the slot */ + return -2; + } - uint16_t candidate_slot_id = (logfs.cfg->arena_size / logfs.cfg->slot_size) - logfs.num_free_slots; - PIOS_Assert(candidate_slot_id > 0); + uint16_t candidate_slot_id = (logfs.cfg->arena_size / logfs.cfg->slot_size) - logfs.num_free_slots; + PIOS_Assert(candidate_slot_id > 0); - uintptr_t slot_addr = logfs_get_addr (logfs.active_arena_id, candidate_slot_id); + uintptr_t slot_addr = logfs_get_addr(logfs.active_arena_id, candidate_slot_id); - if (logfs.driver->read_data(logfs.flash_id, - slot_addr, - (uint8_t *)slot_hdr, - sizeof (*slot_hdr)) != 0) { - /* Failed to read slot header for candidate slot */ - return -3; - } + if (logfs.driver->read_data(logfs.flash_id, + slot_addr, + (uint8_t *)slot_hdr, + sizeof(*slot_hdr)) != 0) { + /* Failed to read slot header for candidate slot */ + return -3; + } - if (slot_hdr->state != SLOT_STATE_EMPTY) { - /* Candidate slot isn't empty! Something is broken. */ - PIOS_DEBUG_Assert(0); - return -4; - } + if (slot_hdr->state != SLOT_STATE_EMPTY) { + /* Candidate slot isn't empty! Something is broken. */ + PIOS_DEBUG_Assert(0); + return -4; + } - /* Mark this slot as RESERVED */ - slot_hdr->state = SLOT_STATE_RESERVED; - slot_hdr->obj_id = obj_id; - slot_hdr->obj_inst_id = obj_inst_id; - slot_hdr->obj_size = obj_size; + /* Mark this slot as RESERVED */ + slot_hdr->state = SLOT_STATE_RESERVED; + slot_hdr->obj_id = obj_id; + slot_hdr->obj_inst_id = obj_inst_id; + slot_hdr->obj_size = obj_size; - if (logfs.driver->write_data(logfs.flash_id, - slot_addr, - (uint8_t *)slot_hdr, - sizeof(*slot_hdr)) != 0) { - /* Failed to write the slot header */ - return -5; - } + if (logfs.driver->write_data(logfs.flash_id, + slot_addr, + (uint8_t *)slot_hdr, + sizeof(*slot_hdr)) != 0) { + /* Failed to write the slot header */ + return -5; + } - /* FIXME: If the header write (above) failed, may have partially written data, thus corrupting that slot but we would have missed decrementing this counter */ - logfs.num_free_slots--; + /* FIXME: If the header write (above) failed, may have partially written data, thus corrupting that slot but we would have missed decrementing this counter */ + logfs.num_free_slots--; - *slot_id = candidate_slot_id; - return 0; + *slot_id = candidate_slot_id; + return 0; } /* NOTE: Must be called while holding the flash transaction lock */ -static int8_t logfs_append_to_log (uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size) +static int8_t logfs_append_to_log(uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size) { - /* Reserve a free slot for our new object */ - uint16_t free_slot_id; - struct slot_header slot_hdr; - if (logfs_reserve_free_slot (&free_slot_id, &slot_hdr, obj_id, obj_inst_id, obj_size) != 0) { - /* Failed to reserve a free slot */ - return -1; - } + /* Reserve a free slot for our new object */ + uint16_t free_slot_id; + struct slot_header slot_hdr; - /* Compute slot address */ - uintptr_t slot_addr = logfs_get_addr (logfs.active_arena_id, free_slot_id); + if (logfs_reserve_free_slot(&free_slot_id, &slot_hdr, obj_id, obj_inst_id, obj_size) != 0) { + /* Failed to reserve a free slot */ + return -1; + } - /* Write the data into the reserved slot, starting after the slot header */ - uintptr_t slot_offset = sizeof(slot_hdr); - while (obj_size > 0) { - /* Individual writes must fit entirely within a single page buffer. */ - uint16_t page_remaining = logfs.cfg->page_size - (slot_offset % logfs.cfg->page_size); - uint16_t write_size = MIN(obj_size, page_remaining); - if (logfs.driver->write_data (logfs.flash_id, - slot_addr + slot_offset, - obj_data, - write_size) != 0) { - /* Failed to write the object data to the slot */ - return -2; - } + /* Compute slot address */ + uintptr_t slot_addr = logfs_get_addr(logfs.active_arena_id, free_slot_id); - /* Update our accounting */ - obj_data += write_size; - slot_offset += write_size; - obj_size -= write_size; - } + /* Write the data into the reserved slot, starting after the slot header */ + uintptr_t slot_offset = sizeof(slot_hdr); + while (obj_size > 0) { + /* Individual writes must fit entirely within a single page buffer. */ + uint16_t page_remaining = logfs.cfg->page_size - (slot_offset % logfs.cfg->page_size); + uint16_t write_size = MIN(obj_size, page_remaining); + if (logfs.driver->write_data(logfs.flash_id, + slot_addr + slot_offset, + obj_data, + write_size) != 0) { + /* Failed to write the object data to the slot */ + return -2; + } - /* Mark this slot active in one atomic step */ - slot_hdr.state = SLOT_STATE_ACTIVE; - if (logfs.driver->write_data (logfs.flash_id, - slot_addr, - (uint8_t *)&slot_hdr, - sizeof(slot_hdr)) != 0) { - /* Failed to mark the slot active */ - return -4; - } + /* Update our accounting */ + obj_data += write_size; + slot_offset += write_size; + obj_size -= write_size; + } - /* Object has been successfully written to the slot */ - logfs.num_active_slots++; - return 0; + /* Mark this slot active in one atomic step */ + slot_hdr.state = SLOT_STATE_ACTIVE; + if (logfs.driver->write_data(logfs.flash_id, + slot_addr, + (uint8_t *)&slot_hdr, + sizeof(slot_hdr)) != 0) { + /* Failed to mark the slot active */ + return -4; + } + + /* Object has been successfully written to the slot */ + logfs.num_active_slots++; + return 0; } @@ -799,7 +809,7 @@ static int8_t logfs_append_to_log (uint32_t obj_id, uint16_t obj_inst_id, uint8_ * Provide a PIOS_FLASHFS_* driver * *********************************/ -#include "pios_flashfs.h" /* API for flash filesystem */ +#include "pios_flashfs.h" /* API for flash filesystem */ /** * @brief Saves one object instance to the filesystem @@ -816,69 +826,69 @@ static int8_t logfs_append_to_log (uint32_t obj_id, uint16_t obj_inst_id, uint8_ * @retval -5 if filesystem is full even after garbage collection should have freed space * @retval -6 if writing the new object to the filesystem failed */ -int32_t PIOS_FLASHFS_ObjSave(__attribute__((unused)) uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size) +int32_t PIOS_FLASHFS_ObjSave(__attribute__((unused)) uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size) { - int8_t rc; + int8_t rc; - PIOS_Assert(obj_size <= (logfs.cfg->slot_size - sizeof(struct slot_header))); + PIOS_Assert(obj_size <= (logfs.cfg->slot_size - sizeof(struct slot_header))); - if (logfs.driver->start_transaction(logfs.flash_id) != 0) { - rc = -1; - goto out_exit; - } + if (logfs.driver->start_transaction(logfs.flash_id) != 0) { + rc = -1; + goto out_exit; + } - if (logfs_delete_object (obj_id, obj_inst_id) != 0) { - rc = -2; - goto out_end_trans; - } + if (logfs_delete_object(obj_id, obj_inst_id) != 0) { + rc = -2; + goto out_end_trans; + } - /* - * All old versions of this object + instance have been invalidated. - * Write the new object. - */ + /* + * All old versions of this object + instance have been invalidated. + * Write the new object. + */ - /* Check if the arena is entirely full. */ - if (logfs_fs_is_full()) { - /* Note: Filesystem Full means we're full of *active* records so gc won't help at all. */ - rc = -3; - goto out_end_trans; - } + /* Check if the arena is entirely full. */ + if (logfs_fs_is_full()) { + /* Note: Filesystem Full means we're full of *active* records so gc won't help at all. */ + rc = -3; + goto out_end_trans; + } - /* Is garbage collection required? */ - if (logfs_log_is_full()) { - /* Note: Log Full means the log is full but may contain obsolete slots so gc may free some space */ - if (logfs_garbage_collect() != 0) { - rc = -4; - goto out_end_trans; - } - /* Check one more time just to be sure we actually free'd some space */ - if (logfs_log_is_full()) { - /* - * Log is still full even after gc! - * NOTE: This should not happen since the filesystem wasn't full - * when we checked above so gc should have helped. - */ - PIOS_DEBUG_Assert(0); - rc = -5; - goto out_end_trans; - } - } + /* Is garbage collection required? */ + if (logfs_log_is_full()) { + /* Note: Log Full means the log is full but may contain obsolete slots so gc may free some space */ + if (logfs_garbage_collect() != 0) { + rc = -4; + goto out_end_trans; + } + /* Check one more time just to be sure we actually free'd some space */ + if (logfs_log_is_full()) { + /* + * Log is still full even after gc! + * NOTE: This should not happen since the filesystem wasn't full + * when we checked above so gc should have helped. + */ + PIOS_DEBUG_Assert(0); + rc = -5; + goto out_end_trans; + } + } - /* We have room for our new object. Append it to the log. */ - if (logfs_append_to_log(obj_id, obj_inst_id, obj_data, obj_size) != 0) { - /* Error during append */ - rc = -6; - goto out_end_trans; - } + /* We have room for our new object. Append it to the log. */ + if (logfs_append_to_log(obj_id, obj_inst_id, obj_data, obj_size) != 0) { + /* Error during append */ + rc = -6; + goto out_end_trans; + } - /* Object successfully written to the log */ - rc = 0; + /* Object successfully written to the log */ + rc = 0; out_end_trans: - logfs.driver->end_transaction(logfs.flash_id); + logfs.driver->end_transaction(logfs.flash_id); out_exit: - return rc; + return rc; } /** @@ -894,54 +904,54 @@ out_exit: * @retval -3 if object size in filesystem does not exactly match buffer size * @retval -4 if reading the object data from flash fails */ -int32_t PIOS_FLASHFS_ObjLoad(__attribute__((unused)) uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size) +int32_t PIOS_FLASHFS_ObjLoad(__attribute__((unused)) uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size) { - int8_t rc; + int8_t rc; - PIOS_Assert(obj_size <= (logfs.cfg->slot_size - sizeof(struct slot_header))); + PIOS_Assert(obj_size <= (logfs.cfg->slot_size - sizeof(struct slot_header))); - if (logfs.driver->start_transaction(logfs.flash_id) != 0) { - rc = -1; - goto out_exit; - } + if (logfs.driver->start_transaction(logfs.flash_id) != 0) { + rc = -1; + goto out_exit; + } - /* Find the object in the log */ - uint16_t slot_id = 0; - struct slot_header slot_hdr; - if (logfs_object_find_next (&slot_hdr, &slot_id, obj_id, obj_inst_id) != 0) { - /* Object does not exist in fs */ - rc = -2; - goto out_end_trans; - } + /* Find the object in the log */ + uint16_t slot_id = 0; + struct slot_header slot_hdr; + if (logfs_object_find_next(&slot_hdr, &slot_id, obj_id, obj_inst_id) != 0) { + /* Object does not exist in fs */ + rc = -2; + goto out_end_trans; + } - /* Sanity check what we've found */ - if (slot_hdr.obj_size != obj_size) { - /* Object sizes don't match. Not safe to copy contents. */ - rc = -3; - goto out_end_trans; - } + /* Sanity check what we've found */ + if (slot_hdr.obj_size != obj_size) { + /* Object sizes don't match. Not safe to copy contents. */ + rc = -3; + goto out_end_trans; + } - /* Read the contents of the object from the log */ - if (obj_size > 0) { - uintptr_t slot_addr = logfs_get_addr (logfs.active_arena_id, slot_id); - if (logfs.driver->read_data(logfs.flash_id, - slot_addr + sizeof(slot_hdr), - (uint8_t *)obj_data, - obj_size) != 0) { - /* Failed to read object data from the log */ - rc = -4; - goto out_end_trans; - } - } + /* Read the contents of the object from the log */ + if (obj_size > 0) { + uintptr_t slot_addr = logfs_get_addr(logfs.active_arena_id, slot_id); + if (logfs.driver->read_data(logfs.flash_id, + slot_addr + sizeof(slot_hdr), + (uint8_t *)obj_data, + obj_size) != 0) { + /* Failed to read object data from the log */ + rc = -4; + goto out_end_trans; + } + } - /* Object successfully loaded */ - rc = 0; + /* Object successfully loaded */ + rc = 0; out_end_trans: - logfs.driver->end_transaction(logfs.flash_id); + logfs.driver->end_transaction(logfs.flash_id); out_exit: - return rc; + return rc; } /** @@ -955,26 +965,26 @@ out_exit: */ int32_t PIOS_FLASHFS_ObjDelete(__attribute__((unused)) uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id) { - int8_t rc; + int8_t rc; - if (logfs.driver->start_transaction(logfs.flash_id) != 0) { - rc = -1; - goto out_exit; - } + if (logfs.driver->start_transaction(logfs.flash_id) != 0) { + rc = -1; + goto out_exit; + } - if (logfs_delete_object (obj_id, obj_inst_id) != 0) { - rc = -2; - goto out_end_trans; - } + if (logfs_delete_object(obj_id, obj_inst_id) != 0) { + rc = -2; + goto out_end_trans; + } - /* Object successfully deleted from the log */ - rc = 0; + /* Object successfully deleted from the log */ + rc = 0; out_end_trans: - logfs.driver->end_transaction(logfs.flash_id); + logfs.driver->end_transaction(logfs.flash_id); out_exit: - return rc; + return rc; } /** @@ -988,42 +998,42 @@ out_exit: */ int32_t PIOS_FLASHFS_Format(__attribute__((unused)) uint32_t fs_id) { - int32_t rc; + int32_t rc; - if (logfs.mounted) { - logfs_unmount_log(); - } + if (logfs.mounted) { + logfs_unmount_log(); + } - if (logfs.driver->start_transaction(logfs.flash_id) != 0) { - rc = -1; - goto out_exit; - } + if (logfs.driver->start_transaction(logfs.flash_id) != 0) { + rc = -1; + goto out_exit; + } - if (logfs_erase_all_arenas() != 0) { - rc = -2; - goto out_end_trans; - } + if (logfs_erase_all_arenas() != 0) { + rc = -2; + goto out_end_trans; + } - /* Reinitialize arena 0 */ - if (logfs_activate_arena(0) != 0) { - rc = -3; - goto out_end_trans; - } + /* Reinitialize arena 0 */ + if (logfs_activate_arena(0) != 0) { + rc = -3; + goto out_end_trans; + } - /* Mount arena 0 */ - if (logfs_mount_log(0) != 0) { - rc = -4; - goto out_end_trans; - } + /* Mount arena 0 */ + if (logfs_mount_log(0) != 0) { + rc = -4; + goto out_end_trans; + } - /* Chip erased and log remounted successfully */ - rc = 0; + /* Chip erased and log remounted successfully */ + rc = 0; out_end_trans: - logfs.driver->end_transaction(logfs.flash_id); + logfs.driver->end_transaction(logfs.flash_id); out_exit: - return rc; + return rc; } #endif /* PIOS_INCLUDE_FLASH */ diff --git a/flight/pios/common/pios_gcsrcvr.c b/flight/pios/common/pios_gcsrcvr.c index e7c281d9f..95ed04d7b 100644 --- a/flight/pios/common/pios_gcsrcvr.c +++ b/flight/pios/common/pios_gcsrcvr.c @@ -43,99 +43,103 @@ static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel); static void PIOS_gcsrcvr_Supervisor(uint32_t ppm_id); const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver = { - .read = PIOS_GCSRCVR_Get, + .read = PIOS_GCSRCVR_Get, }; /* Local Variables */ enum pios_gcsrcvr_dev_magic { - PIOS_GCSRCVR_DEV_MAGIC = 0xe9da5c56, + PIOS_GCSRCVR_DEV_MAGIC = 0xe9da5c56, }; struct pios_gcsrcvr_dev { - enum pios_gcsrcvr_dev_magic magic; + enum pios_gcsrcvr_dev_magic magic; - uint8_t supv_timer; - bool Fresh; + uint8_t supv_timer; + bool Fresh; }; static struct pios_gcsrcvr_dev *global_gcsrcvr_dev; static bool PIOS_gcsrcvr_validate(struct pios_gcsrcvr_dev *gcsrcvr_dev) { - return (gcsrcvr_dev->magic == PIOS_GCSRCVR_DEV_MAGIC); + return gcsrcvr_dev->magic == PIOS_GCSRCVR_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void) { - struct pios_gcsrcvr_dev * gcsrcvr_dev; + struct pios_gcsrcvr_dev *gcsrcvr_dev; - gcsrcvr_dev = (struct pios_gcsrcvr_dev *)pvPortMalloc(sizeof(*gcsrcvr_dev)); - if (!gcsrcvr_dev) return(NULL); + gcsrcvr_dev = (struct pios_gcsrcvr_dev *)pvPortMalloc(sizeof(*gcsrcvr_dev)); + if (!gcsrcvr_dev) { + return NULL; + } - gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; - gcsrcvr_dev->Fresh = false; - gcsrcvr_dev->supv_timer = 0; + gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; + gcsrcvr_dev->Fresh = false; + gcsrcvr_dev->supv_timer = 0; - /* The update callback cannot receive the device pointer, so set it in a global */ - global_gcsrcvr_dev = gcsrcvr_dev; + /* The update callback cannot receive the device pointer, so set it in a global */ + global_gcsrcvr_dev = gcsrcvr_dev; - return(gcsrcvr_dev); + return gcsrcvr_dev; } #else static struct pios_gcsrcvr_dev pios_gcsrcvr_devs[PIOS_GCSRCVR_MAX_DEVS]; static uint8_t pios_gcsrcvr_num_devs; static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void) { - struct pios_gcsrcvr_dev *gcsrcvr_dev; + struct pios_gcsrcvr_dev *gcsrcvr_dev; - if (pios_gcsrcvr_num_devs >= PIOS_GCSRCVR_MAX_DEVS) { - return (NULL); - } + if (pios_gcsrcvr_num_devs >= PIOS_GCSRCVR_MAX_DEVS) { + return NULL; + } - gcsrcvr_dev = &pios_gcsrcvr_devs[pios_gcsrcvr_num_devs++]; - gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; - gcsrcvr_dev->Fresh = false; - gcsrcvr_dev->supv_timer = 0; + gcsrcvr_dev = &pios_gcsrcvr_devs[pios_gcsrcvr_num_devs++]; + gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; + gcsrcvr_dev->Fresh = false; + gcsrcvr_dev->supv_timer = 0; - global_gcsrcvr_dev = gcsrcvr_dev; + global_gcsrcvr_dev = gcsrcvr_dev; - return (gcsrcvr_dev); + return gcsrcvr_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void gcsreceiver_updated(UAVObjEvent * ev) +static void gcsreceiver_updated(UAVObjEvent *ev) { - struct pios_gcsrcvr_dev *gcsrcvr_dev = global_gcsrcvr_dev; - if (ev->obj == GCSReceiverHandle()) { - GCSReceiverGet(&gcsreceiverdata); - gcsrcvr_dev->Fresh = true; - } + struct pios_gcsrcvr_dev *gcsrcvr_dev = global_gcsrcvr_dev; + + if (ev->obj == GCSReceiverHandle()) { + GCSReceiverGet(&gcsreceiverdata); + gcsrcvr_dev->Fresh = true; + } } extern int32_t PIOS_GCSRCVR_Init(__attribute__((unused)) uint32_t *gcsrcvr_id) { - struct pios_gcsrcvr_dev *gcsrcvr_dev; + struct pios_gcsrcvr_dev *gcsrcvr_dev; - /* Allocate the device structure */ - gcsrcvr_dev = (struct pios_gcsrcvr_dev *)PIOS_gcsrcvr_alloc(); - if (!gcsrcvr_dev) - return -1; + /* Allocate the device structure */ + gcsrcvr_dev = (struct pios_gcsrcvr_dev *)PIOS_gcsrcvr_alloc(); + if (!gcsrcvr_dev) { + return -1; + } - for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; i++) { - /* Flush channels */ - gcsreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT; - } + for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; i++) { + /* Flush channels */ + gcsreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT; + } - /* Register uavobj callback */ - GCSReceiverConnectCallback (gcsreceiver_updated); + /* Register uavobj callback */ + GCSReceiverConnectCallback(gcsreceiver_updated); - /* Register the failsafe timer callback. */ - if (!PIOS_RTC_RegisterTickCallback(PIOS_gcsrcvr_Supervisor, (uint32_t)gcsrcvr_dev)) { - PIOS_DEBUG_Assert(0); - } + /* Register the failsafe timer callback. */ + if (!PIOS_RTC_RegisterTickCallback(PIOS_gcsrcvr_Supervisor, (uint32_t)gcsrcvr_dev)) { + PIOS_DEBUG_Assert(0); + } - return 0; + return 0; } /** @@ -147,41 +151,44 @@ extern int32_t PIOS_GCSRCVR_Init(__attribute__((unused)) uint32_t *gcsrcvr_id) */ static int32_t PIOS_GCSRCVR_Get(__attribute__((unused)) uint32_t rcvr_id, uint8_t channel) { - if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) { - /* channel is out of range */ - return PIOS_RCVR_INVALID; - } + if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) { + /* channel is out of range */ + return PIOS_RCVR_INVALID; + } - return (gcsreceiverdata.Channel[channel]); + return gcsreceiverdata.Channel[channel]; } -static void PIOS_gcsrcvr_Supervisor(uint32_t gcsrcvr_id) { - /* Recover our device context */ - struct pios_gcsrcvr_dev * gcsrcvr_dev = (struct pios_gcsrcvr_dev *)gcsrcvr_id; +static void PIOS_gcsrcvr_Supervisor(uint32_t gcsrcvr_id) +{ + /* Recover our device context */ + struct pios_gcsrcvr_dev *gcsrcvr_dev = (struct pios_gcsrcvr_dev *)gcsrcvr_id; - if (!PIOS_gcsrcvr_validate(gcsrcvr_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_gcsrcvr_validate(gcsrcvr_dev)) { + /* Invalid device specified */ + return; + } - /* - * RTC runs at 625Hz. - */ - if(++(gcsrcvr_dev->supv_timer) < (PIOS_GCSRCVR_TIMEOUT_MS * 1000 / 625)) { - return; - } - gcsrcvr_dev->supv_timer = 0; + /* + * RTC runs at 625Hz. + */ + if (++(gcsrcvr_dev->supv_timer) < (PIOS_GCSRCVR_TIMEOUT_MS * 1000 / 625)) { + return; + } + gcsrcvr_dev->supv_timer = 0; - if (!gcsrcvr_dev->Fresh) - for (int32_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; i++) - gcsreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT; + if (!gcsrcvr_dev->Fresh) { + for (int32_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; i++) { + gcsreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT; + } + } - gcsrcvr_dev->Fresh = false; + gcsrcvr_dev->Fresh = false; } #endif /* PIOS_INCLUDE_GCSRCVR */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/common/pios_hcsr04.c b/flight/pios/common/pios_hcsr04.c index 925c5a175..da0c8f76d 100644 --- a/flight/pios/common/pios_hcsr04.c +++ b/flight/pios/common/pios_hcsr04.c @@ -1,30 +1,30 @@ /** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_HCSR04 HCSR04 Functions - * @brief Hardware functions to deal with the altitude pressure sensor - * @{ - * - * @file pios_hcsr04.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief HCSR04 sonar Sensor Routines - * @see The GNU Public License (GPL) Version 3 - * - ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_HCSR04 HCSR04 Functions + * @brief Hardware functions to deal with the altitude pressure sensor + * @{ + * + * @file pios_hcsr04.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief HCSR04 sonar Sensor Routines + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,258 +41,257 @@ /* 100 ms timeout without updates on channels */ const static uint32_t PWM_SUPERVISOR_TIMEOUT = 100000; -struct pios_hcsr04_dev * hcsr04_dev_loc; +struct pios_hcsr04_dev *hcsr04_dev_loc; enum pios_hcsr04_dev_magic { - PIOS_HCSR04_DEV_MAGIC = 0xab3029AA, + PIOS_HCSR04_DEV_MAGIC = 0xab3029AA, }; struct pios_hcsr04_dev { - enum pios_hcsr04_dev_magic magic; - const struct pios_hcsr04_cfg * cfg; + enum pios_hcsr04_dev_magic magic; + const struct pios_hcsr04_cfg *cfg; - uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; - uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; - uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; - uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; - uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; - uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; + uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; + uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; + uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; + uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; }; -static bool PIOS_HCSR04_validate(struct pios_hcsr04_dev * hcsr04_dev) +static bool PIOS_HCSR04_validate(struct pios_hcsr04_dev *hcsr04_dev) { - return (hcsr04_dev->magic == PIOS_HCSR04_DEV_MAGIC); + return hcsr04_dev->magic == PIOS_HCSR04_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_hcsr04_dev * PIOS_PWM_alloc(void) +static struct pios_hcsr04_dev *PIOS_PWM_alloc(void) { - struct pios_hcsr04_dev * hcsr04_dev; + struct pios_hcsr04_dev *hcsr04_dev; - hcsr04_dev = (struct pios_hcsr04_dev *)pvPortMalloc(sizeof(*hcsr04_dev)); - if (!hcsr04_dev) return(NULL); + hcsr04_dev = (struct pios_hcsr04_dev *)pvPortMalloc(sizeof(*hcsr04_dev)); + if (!hcsr04_dev) { + return NULL; + } - hcsr04_dev->magic = PIOS_HCSR04_DEV_MAGIC; - return(hcsr04_dev); + hcsr04_dev->magic = PIOS_HCSR04_DEV_MAGIC; + return hcsr04_dev; } #else static struct pios_hcsr04_dev pios_hcsr04_devs[PIOS_PWM_MAX_DEVS]; static uint8_t pios_hcsr04_num_devs; -static struct pios_hcsr04_dev * PIOS_PWM_alloc(void) +static struct pios_hcsr04_dev *PIOS_PWM_alloc(void) { - struct pios_hcsr04_dev * hcsr04_dev; + struct pios_hcsr04_dev *hcsr04_dev; - if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) { - return (NULL); - } + if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) { + return NULL; + } - hcsr04_dev = &pios_hcsr04_devs[pios_hcsr04_num_devs++]; - hcsr04_dev->magic = PIOS_HCSR04_DEV_MAGIC; + hcsr04_dev = &pios_hcsr04_devs[pios_hcsr04_num_devs++]; + hcsr04_dev->magic = PIOS_HCSR04_DEV_MAGIC; - return (hcsr04_dev); + return hcsr04_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void PIOS_HCSR04_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); -static void PIOS_HCSR04_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_HCSR04_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_HCSR04_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); const static struct pios_tim_callbacks tim_callbacks = { - .overflow = PIOS_HCSR04_tim_overflow_cb, - .edge = PIOS_HCSR04_tim_edge_cb, + .overflow = PIOS_HCSR04_tim_overflow_cb, + .edge = PIOS_HCSR04_tim_edge_cb, }; /** -* Initialises all the pins -*/ -int32_t PIOS_HCSR04_Init(uint32_t * pwm_id, const struct pios_hcsr04_cfg * cfg) + * Initialises all the pins + */ +int32_t PIOS_HCSR04_Init(uint32_t *pwm_id, const struct pios_hcsr04_cfg *cfg) { - PIOS_DEBUG_Assert(pwm_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(pwm_id); + PIOS_DEBUG_Assert(cfg); - struct pios_hcsr04_dev * hcsr04_dev; + struct pios_hcsr04_dev *hcsr04_dev; - hcsr04_dev = (struct pios_hcsr04_dev *) PIOS_PWM_alloc(); - if (!hcsr04_dev) goto out_fail; + hcsr04_dev = (struct pios_hcsr04_dev *)PIOS_PWM_alloc(); + if (!hcsr04_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - hcsr04_dev->cfg = cfg; - hcsr04_dev_loc = hcsr04_dev; + /* Bind the configuration to the device instance */ + hcsr04_dev->cfg = cfg; + hcsr04_dev_loc = hcsr04_dev; - for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { - /* Flush counter variables */ - hcsr04_dev->CaptureState[i] = 0; - hcsr04_dev->RiseValue[i] = 0; - hcsr04_dev->FallValue[i] = 0; - hcsr04_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - } + for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { + /* Flush counter variables */ + hcsr04_dev->CaptureState[i] = 0; + hcsr04_dev->RiseValue[i] = 0; + hcsr04_dev->FallValue[i] = 0; + hcsr04_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)hcsr04_dev)) { - return -1; - } + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)hcsr04_dev)) { + return -1; + } - /* Configure the channels to be in capture/compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel * chan = &cfg->channels[i]; + /* Configure the channels to be in capture/compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; - /* Configure timer for input capture */ - TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); + /* Configure timer for input capture */ + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); - /* Enable the Capture Compare Interrupt Request */ - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); - break; - case TIM_Channel_2: - TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); - break; - case TIM_Channel_3: - TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); - break; - case TIM_Channel_4: - TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); - break; - } + /* Enable the Capture Compare Interrupt Request */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); + break; + } - // Need the update event for that timer to detect timeouts - TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); - - } + // Need the update event for that timer to detect timeouts + TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); + } #ifndef STM32F4XX - /* Enable the peripheral clock for the GPIO */ - switch ((uint32_t)hcsr04_dev->cfg->trigger.gpio) { - case (uint32_t) GPIOA: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); - break; - case (uint32_t) GPIOB: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - break; - case (uint32_t) GPIOC: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); - break; - default: - PIOS_Assert(0); - break; - } + /* Enable the peripheral clock for the GPIO */ + switch ((uint32_t)hcsr04_dev->cfg->trigger.gpio) { + case (uint32_t)GPIOA: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); + break; + case (uint32_t)GPIOB: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + break; + case (uint32_t)GPIOC: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); + break; + default: + PIOS_Assert(0); + break; + } #endif - GPIO_Init(hcsr04_dev->cfg->trigger.gpio, &hcsr04_dev->cfg->trigger.init); + GPIO_Init(hcsr04_dev->cfg->trigger.gpio, &hcsr04_dev->cfg->trigger.init); - *pwm_id = (uint32_t) hcsr04_dev; + *pwm_id = (uint32_t)hcsr04_dev; - return (0); + return 0; out_fail: - return (-1); + return -1; } void PIOS_HCSR04_Trigger(void) { - GPIO_SetBits(hcsr04_dev_loc->cfg->trigger.gpio,hcsr04_dev_loc->cfg->trigger.init.GPIO_Pin); - PIOS_DELAY_WaituS(15); - GPIO_ResetBits(hcsr04_dev_loc->cfg->trigger.gpio,hcsr04_dev_loc->cfg->trigger.init.GPIO_Pin); + GPIO_SetBits(hcsr04_dev_loc->cfg->trigger.gpio, hcsr04_dev_loc->cfg->trigger.init.GPIO_Pin); + PIOS_DELAY_WaituS(15); + GPIO_ResetBits(hcsr04_dev_loc->cfg->trigger.gpio, hcsr04_dev_loc->cfg->trigger.init.GPIO_Pin); } /** -* Get the value of an input channel -* \param[in] Channel Number of the channel desired -* \output -1 Channel not available -* \output >0 Channel value -*/ + * Get the value of an input channel + * \param[in] Channel Number of the channel desired + * \output -1 Channel not available + * \output >0 Channel value + */ int32_t PIOS_HCSR04_Get(void) { - return hcsr04_dev_loc->CaptureValue[0]; + return hcsr04_dev_loc->CaptureValue[0]; } int32_t PIOS_HCSR04_Completed(void) { - return hcsr04_dev_loc->CapCounter[0]; + return hcsr04_dev_loc->CapCounter[0]; } -static void PIOS_HCSR04_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) +static void PIOS_HCSR04_tim_overflow_cb(uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) { - struct pios_hcsr04_dev * hcsr04_dev = (struct pios_hcsr04_dev *)context; + struct pios_hcsr04_dev *hcsr04_dev = (struct pios_hcsr04_dev *)context; - if (!PIOS_HCSR04_validate(hcsr04_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_HCSR04_validate(hcsr04_dev)) { + /* Invalid device specified */ + return; + } - if (channel >= hcsr04_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (channel >= hcsr04_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - hcsr04_dev->us_since_update[channel] += count; - if(hcsr04_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { - hcsr04_dev->CaptureState[channel] = 0; - hcsr04_dev->RiseValue[channel] = 0; - hcsr04_dev->FallValue[channel] = 0; - hcsr04_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; - hcsr04_dev->us_since_update[channel] = 0; - } - - return; + hcsr04_dev->us_since_update[channel] += count; + if (hcsr04_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { + hcsr04_dev->CaptureState[channel] = 0; + hcsr04_dev->RiseValue[channel] = 0; + hcsr04_dev->FallValue[channel] = 0; + hcsr04_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; + hcsr04_dev->us_since_update[channel] = 0; + } } -static void PIOS_HCSR04_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) +static void PIOS_HCSR04_tim_edge_cb(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) { - /* Recover our device context */ - struct pios_hcsr04_dev * hcsr04_dev = (struct pios_hcsr04_dev *)context; + /* Recover our device context */ + struct pios_hcsr04_dev *hcsr04_dev = (struct pios_hcsr04_dev *)context; - if (!PIOS_HCSR04_validate(hcsr04_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_HCSR04_validate(hcsr04_dev)) { + /* Invalid device specified */ + return; + } - if (chan_idx >= hcsr04_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (chan_idx >= hcsr04_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - const struct pios_tim_channel * chan = &hcsr04_dev->cfg->channels[chan_idx]; + const struct pios_tim_channel *chan = &hcsr04_dev->cfg->channels[chan_idx]; - if (hcsr04_dev->CaptureState[chan_idx] == 0) { - hcsr04_dev->RiseValue[chan_idx] = count; - hcsr04_dev->us_since_update[chan_idx] = 0; - } else { - hcsr04_dev->FallValue[chan_idx] = count; - } + if (hcsr04_dev->CaptureState[chan_idx] == 0) { + hcsr04_dev->RiseValue[chan_idx] = count; + hcsr04_dev->us_since_update[chan_idx] = 0; + } else { + hcsr04_dev->FallValue[chan_idx] = count; + } - // flip state machine and capture value here - /* Simple rise or fall state machine */ - TIM_ICInitTypeDef TIM_ICInitStructure = hcsr04_dev->cfg->tim_ic_init; - if (hcsr04_dev->CaptureState[chan_idx] == 0) { - /* Switch states */ - hcsr04_dev->CaptureState[chan_idx] = 1; + // flip state machine and capture value here + /* Simple rise or fall state machine */ + TIM_ICInitTypeDef TIM_ICInitStructure = hcsr04_dev->cfg->tim_ic_init; + if (hcsr04_dev->CaptureState[chan_idx] == 0) { + /* Switch states */ + hcsr04_dev->CaptureState[chan_idx] = 1; - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - } else { - /* Capture computation */ - if (hcsr04_dev->FallValue[chan_idx] > hcsr04_dev->RiseValue[chan_idx]) { - hcsr04_dev->CaptureValue[chan_idx] = (hcsr04_dev->FallValue[chan_idx] - hcsr04_dev->RiseValue[chan_idx]); - } else { - hcsr04_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - hcsr04_dev->RiseValue[chan_idx]) + hcsr04_dev->FallValue[chan_idx]); - } + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } else { + /* Capture computation */ + if (hcsr04_dev->FallValue[chan_idx] > hcsr04_dev->RiseValue[chan_idx]) { + hcsr04_dev->CaptureValue[chan_idx] = (hcsr04_dev->FallValue[chan_idx] - hcsr04_dev->RiseValue[chan_idx]); + } else { + hcsr04_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - hcsr04_dev->RiseValue[chan_idx]) + hcsr04_dev->FallValue[chan_idx]); + } - /* Switch states */ - hcsr04_dev->CaptureState[chan_idx] = 0; + /* Switch states */ + hcsr04_dev->CaptureState[chan_idx] = 0; - /* Increase supervisor counter */ - hcsr04_dev->CapCounter[chan_idx]++; - - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - } + /* Increase supervisor counter */ + hcsr04_dev->CapCounter[chan_idx]++; + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } } #endif /* PIOS_INCLUDE_HCSR04 */ - diff --git a/flight/pios/common/pios_hmc5843.c b/flight/pios/common/pios_hmc5843.c index 83dcc07bf..767e9fbc5 100644 --- a/flight/pios/common/pios_hmc5843.c +++ b/flight/pios/common/pios_hmc5843.c @@ -13,19 +13,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,343 +36,363 @@ #include /* HMC5843 Addresses */ -#define PIOS_HMC5843_I2C_ADDR 0x1E -#define PIOS_HMC5843_CONFIG_REG_A (uint8_t)0x00 -#define PIOS_HMC5843_CONFIG_REG_B (uint8_t)0x01 -#define PIOS_HMC5843_MODE_REG (uint8_t)0x02 -#define PIOS_HMC5843_DATAOUT_XMSB_REG 0x03 -#define PIOS_HMC5843_DATAOUT_XLSB_REG 0x04 -#define PIOS_HMC5843_DATAOUT_YMSB_REG 0x05 -#define PIOS_HMC5843_DATAOUT_YLSB_REG 0x06 -#define PIOS_HMC5843_DATAOUT_ZMSB_REG 0x07 -#define PIOS_HMC5843_DATAOUT_ZLSB_REG 0x08 -#define PIOS_HMC5843_DATAOUT_STATUS_REG 0x09 -#define PIOS_HMC5843_DATAOUT_IDA_REG 0x0A -#define PIOS_HMC5843_DATAOUT_IDB_REG 0x0B -#define PIOS_HMC5843_DATAOUT_IDC_REG 0x0C +#define PIOS_HMC5843_I2C_ADDR 0x1E +#define PIOS_HMC5843_CONFIG_REG_A (uint8_t)0x00 +#define PIOS_HMC5843_CONFIG_REG_B (uint8_t)0x01 +#define PIOS_HMC5843_MODE_REG (uint8_t)0x02 +#define PIOS_HMC5843_DATAOUT_XMSB_REG 0x03 +#define PIOS_HMC5843_DATAOUT_XLSB_REG 0x04 +#define PIOS_HMC5843_DATAOUT_YMSB_REG 0x05 +#define PIOS_HMC5843_DATAOUT_YLSB_REG 0x06 +#define PIOS_HMC5843_DATAOUT_ZMSB_REG 0x07 +#define PIOS_HMC5843_DATAOUT_ZLSB_REG 0x08 +#define PIOS_HMC5843_DATAOUT_STATUS_REG 0x09 +#define PIOS_HMC5843_DATAOUT_IDA_REG 0x0A +#define PIOS_HMC5843_DATAOUT_IDB_REG 0x0B +#define PIOS_HMC5843_DATAOUT_IDC_REG 0x0C /* Output Data Rate */ -#define PIOS_HMC5843_ODR_05 0x00 -#define PIOS_HMC5843_ODR_1 0x04 -#define PIOS_HMC5843_ODR_2 0x08 -#define PIOS_HMC5843_ODR_5 0x0C -#define PIOS_HMC5843_ODR_10 0x10 -#define PIOS_HMC5843_ODR_20 0x14 -#define PIOS_HMC5843_ODR_50 0x18 +#define PIOS_HMC5843_ODR_05 0x00 +#define PIOS_HMC5843_ODR_1 0x04 +#define PIOS_HMC5843_ODR_2 0x08 +#define PIOS_HMC5843_ODR_5 0x0C +#define PIOS_HMC5843_ODR_10 0x10 +#define PIOS_HMC5843_ODR_20 0x14 +#define PIOS_HMC5843_ODR_50 0x18 /* Measure configuration */ -#define PIOS_HMC5843_MEASCONF_NORMAL 0x00 -#define PIOS_HMC5843_MEASCONF_BIAS_POS 0x01 -#define PIOS_HMC5843_MEASCONF_BIAS_NEG 0x02 +#define PIOS_HMC5843_MEASCONF_NORMAL 0x00 +#define PIOS_HMC5843_MEASCONF_BIAS_POS 0x01 +#define PIOS_HMC5843_MEASCONF_BIAS_NEG 0x02 /* Gain settings */ -#define PIOS_HMC5843_GAIN_0_7 0x00 -#define PIOS_HMC5843_GAIN_1 0x20 -#define PIOS_HMC5843_GAIN_1_5 0x40 -#define PIOS_HMC5843_GAIN_2 0x60 -#define PIOS_HMC5843_GAIN_3_2 0x80 -#define PIOS_HMC5843_GAIN_3_8 0xA0 -#define PIOS_HMC5843_GAIN_4_5 0xC0 -#define PIOS_HMC5843_GAIN_6_5 0xE0 +#define PIOS_HMC5843_GAIN_0_7 0x00 +#define PIOS_HMC5843_GAIN_1 0x20 +#define PIOS_HMC5843_GAIN_1_5 0x40 +#define PIOS_HMC5843_GAIN_2 0x60 +#define PIOS_HMC5843_GAIN_3_2 0x80 +#define PIOS_HMC5843_GAIN_3_8 0xA0 +#define PIOS_HMC5843_GAIN_4_5 0xC0 +#define PIOS_HMC5843_GAIN_6_5 0xE0 /* Modes */ -#define PIOS_HMC5843_MODE_CONTINUOUS 0x00 -#define PIOS_HMC5843_MODE_SINGLE 0x01 -#define PIOS_HMC5843_MODE_IDLE 0x02 -#define PIOS_HMC5843_MODE_SLEEP 0x02 +#define PIOS_HMC5843_MODE_CONTINUOUS 0x00 +#define PIOS_HMC5843_MODE_SINGLE 0x01 +#define PIOS_HMC5843_MODE_IDLE 0x02 +#define PIOS_HMC5843_MODE_SLEEP 0x02 /* Sensitivity Conversion Values */ -#define PIOS_HMC5843_Sensitivity_0_7Ga 1602 // LSB/Ga -#define PIOS_HMC5843_Sensitivity_1Ga 1300 // LSB/Ga -#define PIOS_HMC5843_Sensitivity_1_5Ga 970 // LSB/Ga -#define PIOS_HMC5843_Sensitivity_2Ga 780 // LSB/Ga -#define PIOS_HMC5843_Sensitivity_3_2Ga 530 // LSB/Ga -#define PIOS_HMC5843_Sensitivity_3_8Ga 460 // LSB/Ga -#define PIOS_HMC5843_Sensitivity_4_5Ga 390 // LSB/Ga -#define PIOS_HMC5843_Sensitivity_6_5Ga 280 // LSB/Ga --> NOT RECOMMENDED +#define PIOS_HMC5843_Sensitivity_0_7Ga 1602 // LSB/Ga +#define PIOS_HMC5843_Sensitivity_1Ga 1300 // LSB/Ga +#define PIOS_HMC5843_Sensitivity_1_5Ga 970 // LSB/Ga +#define PIOS_HMC5843_Sensitivity_2Ga 780 // LSB/Ga +#define PIOS_HMC5843_Sensitivity_3_2Ga 530 // LSB/Ga +#define PIOS_HMC5843_Sensitivity_3_8Ga 460 // LSB/Ga +#define PIOS_HMC5843_Sensitivity_4_5Ga 390 // LSB/Ga +#define PIOS_HMC5843_Sensitivity_6_5Ga 280 // LSB/Ga --> NOT RECOMMENDED /* Global Variables */ /* Local Types */ typedef struct { - uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ - uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ - uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ - uint8_t Mode; + uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ + uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t Mode; } PIOS_HMC5843_ConfigTypeDef; /* Local Variables */ volatile bool pios_hmc5843_data_ready; -static void PIOS_HMC5843_Config(PIOS_HMC5843_ConfigTypeDef * HMC5843_Config_Struct); -static bool PIOS_HMC5843_Read(uint8_t address, uint8_t * buffer, uint8_t len); +static void PIOS_HMC5843_Config(PIOS_HMC5843_ConfigTypeDef *HMC5843_Config_Struct); +static bool PIOS_HMC5843_Read(uint8_t address, uint8_t *buffer, uint8_t len); static bool PIOS_HMC5843_Write(uint8_t address, uint8_t buffer); -void PIOS_HMC5843_EndOfConversion (void) +void PIOS_HMC5843_EndOfConversion(void) { - pios_hmc5843_data_ready = true; + pios_hmc5843_data_ready = true; } static const struct pios_exti_cfg pios_exti_hmc5843_cfg __exti_config = { - .vector = PIOS_HMC5843_EndOfConversion, - .line = PIOS_HMC5843_DRDY_EXTI_LINE, - .pin = { - .gpio = PIOS_HMC5843_DRDY_GPIO_PORT, - .init = { - .GPIO_Pin = PIOS_HMC5843_DRDY_GPIO_PIN, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = PIOS_HMC5843_DRDY_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_HMC5843_DRDY_PRIO, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = PIOS_HMC5843_DRDY_EXTI_LINE, - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_HMC5843_EndOfConversion, + .line = PIOS_HMC5843_DRDY_EXTI_LINE, + .pin = { + .gpio = PIOS_HMC5843_DRDY_GPIO_PORT, + .init = { + .GPIO_Pin = PIOS_HMC5843_DRDY_GPIO_PIN, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = PIOS_HMC5843_DRDY_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_HMC5843_DRDY_PRIO, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = PIOS_HMC5843_DRDY_EXTI_LINE, + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; /** - * @brief Initialise the HMC5843 sensor - */ + * @brief Initialise the HMC5843 sensor + */ void PIOS_HMC5843_Init(void) { - /* Enable DRDY GPIO clock */ - RCC_APB2PeriphClockCmd(PIOS_HMC5843_DRDY_CLK | RCC_APB2Periph_AFIO, ENABLE); + /* Enable DRDY GPIO clock */ + RCC_APB2PeriphClockCmd(PIOS_HMC5843_DRDY_CLK | RCC_APB2Periph_AFIO, ENABLE); - PIOS_EXTI_Init(&pios_exti_hmc5843_cfg); + PIOS_EXTI_Init(&pios_exti_hmc5843_cfg); - /* Configure the HMC5843 Sensor */ - PIOS_HMC5843_ConfigTypeDef HMC5843_InitStructure; - HMC5843_InitStructure.M_ODR = PIOS_HMC5843_ODR_10; - HMC5843_InitStructure.Meas_Conf = PIOS_HMC5843_MEASCONF_NORMAL; - HMC5843_InitStructure.Gain = PIOS_HMC5843_GAIN_2; - HMC5843_InitStructure.Mode = PIOS_HMC5843_MODE_CONTINUOUS; - PIOS_HMC5843_Config(&HMC5843_InitStructure); + /* Configure the HMC5843 Sensor */ + PIOS_HMC5843_ConfigTypeDef HMC5843_InitStructure; + HMC5843_InitStructure.M_ODR = PIOS_HMC5843_ODR_10; + HMC5843_InitStructure.Meas_Conf = PIOS_HMC5843_MEASCONF_NORMAL; + HMC5843_InitStructure.Gain = PIOS_HMC5843_GAIN_2; + HMC5843_InitStructure.Mode = PIOS_HMC5843_MODE_CONTINUOUS; + PIOS_HMC5843_Config(&HMC5843_InitStructure); - pios_hmc5843_data_ready = false; + pios_hmc5843_data_ready = false; } /** -* Initialise the HMC5843 sensor -* -* CTRL_REGA: Control Register A -* Read Write -* Default value: 0x10 -* 7:5 0 These bits must be cleared for correct operation. -* 4:2 DO2-DO0: Data Output Rate Bits -* DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz) -* ------------------------------------------------------ -* 0 | 0 | 0 | 0.5 -* 0 | 0 | 1 | 1 -* 0 | 1 | 0 | 2 -* 0 | 1 | 1 | 5 -* 1 | 0 | 0 | 10 (default) -* 1 | 0 | 1 | 20 -* 1 | 1 | 0 | 50 -* 1 | 1 | 1 | Not Used -* 1:0 MS1-MS0: Measurement Configuration Bits -* MS1 | MS0 | MODE -* ------------------------------ -* 0 | 0 | Normal -* 0 | 1 | Positive Bias -* 1 | 0 | Negative Bias -* 1 | 1 | Not Used -* -* CTRL_REGB: Control RegisterB -* Read Write -* Default value: 0x20 -* 7:5 GN2-GN0: Gain Configuration Bits. -* GN2 | GN1 | GN0 | Mag Input | Gain | Output Range -* | | | Range[Ga] | [LSB/mGa] | -* ------------------------------------------------------ -* 0 | 0 | 0 | ±0.7Ga | 1620 | 0xF800–0x07FF (-2048:2047) -* 0 | 0 | 1 | ±1.0Ga (def) | 1300 | 0xF800–0x07FF (-2048:2047) -* 0 | 1 | 0 | ±1.5Ga | 970 | 0xF800–0x07FF (-2048:2047) -* 0 | 1 | 1 | ±2.0Ga | 780 | 0xF800–0x07FF (-2048:2047) -* 1 | 0 | 0 | ±3.2Ga | 530 | 0xF800–0x07FF (-2048:2047) -* 1 | 0 | 1 | ±3.8Ga | 460 | 0xF800–0x07FF (-2048:2047) -* 1 | 1 | 0 | ±4.5Ga | 390 | 0xF800–0x07FF (-2048:2047) -* 1 | 1 | 1 | ±6.5Ga | 280 | 0xF800–0x07FF (-2048:2047) -* |Not recommended| -* -* 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation. -* -* _MODE_REG: Mode Register -* Read Write -* Default value: 0x02 -* 7:2 0 These bits must be cleared for correct operation. -* 1:0 MD1-MD0: Mode Select Bits -* MS1 | MS0 | MODE -* ------------------------------ -* 0 | 0 | Continuous-Conversion Mode. -* 0 | 1 | Single-Conversion Mode -* 1 | 0 | Negative Bias -* 1 | 1 | Sleep Mode -*/ -static void PIOS_HMC5843_Config(PIOS_HMC5843_ConfigTypeDef * HMC5843_Config_Struct) + * Initialise the HMC5843 sensor + * + * CTRL_REGA: Control Register A + * Read Write + * Default value: 0x10 + * 7:5 0 These bits must be cleared for correct operation. + * 4:2 DO2-DO0: Data Output Rate Bits + * DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz) + * ------------------------------------------------------ + * 0 | 0 | 0 | 0.5 + * 0 | 0 | 1 | 1 + * 0 | 1 | 0 | 2 + * 0 | 1 | 1 | 5 + * 1 | 0 | 0 | 10 (default) + * 1 | 0 | 1 | 20 + * 1 | 1 | 0 | 50 + * 1 | 1 | 1 | Not Used + * 1:0 MS1-MS0: Measurement Configuration Bits + * MS1 | MS0 | MODE + * ------------------------------ + * 0 | 0 | Normal + * 0 | 1 | Positive Bias + * 1 | 0 | Negative Bias + * 1 | 1 | Not Used + * + * CTRL_REGB: Control RegisterB + * Read Write + * Default value: 0x20 + * 7:5 GN2-GN0: Gain Configuration Bits. + * GN2 | GN1 | GN0 | Mag Input | Gain | Output Range + * | | | Range[Ga] | [LSB/mGa] | + * ------------------------------------------------------ + * 0 | 0 | 0 | ±0.7Ga | 1620 | 0xF800–0x07FF (-2048:2047) + * 0 | 0 | 1 | ±1.0Ga (def) | 1300 | 0xF800–0x07FF (-2048:2047) + * 0 | 1 | 0 | ±1.5Ga | 970 | 0xF800–0x07FF (-2048:2047) + * 0 | 1 | 1 | ±2.0Ga | 780 | 0xF800–0x07FF (-2048:2047) + * 1 | 0 | 0 | ±3.2Ga | 530 | 0xF800–0x07FF (-2048:2047) + * 1 | 0 | 1 | ±3.8Ga | 460 | 0xF800–0x07FF (-2048:2047) + * 1 | 1 | 0 | ±4.5Ga | 390 | 0xF800–0x07FF (-2048:2047) + * 1 | 1 | 1 | ±6.5Ga | 280 | 0xF800–0x07FF (-2048:2047) + * |Not recommended| + * + * 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation. + * + * _MODE_REG: Mode Register + * Read Write + * Default value: 0x02 + * 7:2 0 These bits must be cleared for correct operation. + * 1:0 MD1-MD0: Mode Select Bits + * MS1 | MS0 | MODE + * ------------------------------ + * 0 | 0 | Continuous-Conversion Mode. + * 0 | 1 | Single-Conversion Mode + * 1 | 0 | Negative Bias + * 1 | 1 | Sleep Mode + */ +static void PIOS_HMC5843_Config(PIOS_HMC5843_ConfigTypeDef *HMC5843_Config_Struct) { - uint8_t CRTLA = 0x00; - uint8_t CRTLB = 0x00; - uint8_t MODE = 0x00; + uint8_t CRTLA = 0x00; + uint8_t CRTLB = 0x00; + uint8_t MODE = 0x00; - CRTLA |= (uint8_t) (HMC5843_Config_Struct->M_ODR | HMC5843_Config_Struct->Meas_Conf); - CRTLB |= (uint8_t) (HMC5843_Config_Struct->Gain); - MODE |= (uint8_t) (HMC5843_Config_Struct->Mode); + CRTLA |= (uint8_t)(HMC5843_Config_Struct->M_ODR | HMC5843_Config_Struct->Meas_Conf); + CRTLB |= (uint8_t)(HMC5843_Config_Struct->Gain); + MODE |= (uint8_t)(HMC5843_Config_Struct->Mode); - // CRTL_REGA - while (!PIOS_HMC5843_Write(PIOS_HMC5843_CONFIG_REG_A, CRTLA)) ; + // CRTL_REGA + while (!PIOS_HMC5843_Write(PIOS_HMC5843_CONFIG_REG_A, CRTLA)) { + ; + } - // CRTL_REGB - while (!PIOS_HMC5843_Write(PIOS_HMC5843_CONFIG_REG_B, CRTLB)) ; + // CRTL_REGB + while (!PIOS_HMC5843_Write(PIOS_HMC5843_CONFIG_REG_B, CRTLB)) { + ; + } - // Mode register - while (!PIOS_HMC5843_Write(PIOS_HMC5843_MODE_REG, MODE)) ; + // Mode register + while (!PIOS_HMC5843_Write(PIOS_HMC5843_MODE_REG, MODE)) { + ; + } } /** -* Read the magnetic readings from the sensor -*/ + * Read the magnetic readings from the sensor + */ void PIOS_HMC5843_ReadMag(int16_t out[3]) { - uint8_t buffer[6]; - uint8_t crtlB; + uint8_t buffer[6]; + uint8_t crtlB; - pios_hmc5843_data_ready = false; + pios_hmc5843_data_ready = false; - while (!PIOS_HMC5843_Read(PIOS_HMC5843_CONFIG_REG_B, &crtlB, 1)) ; - while (!PIOS_HMC5843_Read(PIOS_HMC5843_DATAOUT_XMSB_REG, buffer, 6)) ; + while (!PIOS_HMC5843_Read(PIOS_HMC5843_CONFIG_REG_B, &crtlB, 1)) { + ; + } + while (!PIOS_HMC5843_Read(PIOS_HMC5843_DATAOUT_XMSB_REG, buffer, 6)) { + ; + } - switch (crtlB & 0xE0) { - case 0x00: - for (int i = 0; i < 3; i++) - out[i] = ((int16_t) ((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_0_7Ga; - break; - case 0x20: - for (int i = 0; i < 3; i++) - out[i] = ((int16_t) ((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_1Ga; - break; - case 0x40: - for (int i = 0; i < 3; i++) - out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_1_5Ga; - break; - case 0x60: - for (int i = 0; i < 3; i++) - out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_2Ga; - break; - case 0x80: - for (int i = 0; i < 3; i++) - out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_3_2Ga; - break; - case 0xA0: - for (int i = 0; i < 3; i++) - out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_3_8Ga; - break; - case 0xC0: - for (int i = 0; i < 3; i++) - out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_4_5Ga; - break; - case 0xE0: - for (int i = 0; i < 3; i++) - out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_6_5Ga; - break; - } + switch (crtlB & 0xE0) { + case 0x00: + for (int i = 0; i < 3; i++) { + out[i] = ((int16_t)((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_0_7Ga; + } + break; + case 0x20: + for (int i = 0; i < 3; i++) { + out[i] = ((int16_t)((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_1Ga; + } + break; + case 0x40: + for (int i = 0; i < 3; i++) { + out[i] = (int16_t)(((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_1_5Ga; + } + break; + case 0x60: + for (int i = 0; i < 3; i++) { + out[i] = (int16_t)(((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_2Ga; + } + break; + case 0x80: + for (int i = 0; i < 3; i++) { + out[i] = (int16_t)(((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_3_2Ga; + } + break; + case 0xA0: + for (int i = 0; i < 3; i++) { + out[i] = (int16_t)(((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_3_8Ga; + } + break; + case 0xC0: + for (int i = 0; i < 3; i++) { + out[i] = (int16_t)(((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_4_5Ga; + } + break; + case 0xE0: + for (int i = 0; i < 3; i++) { + out[i] = (int16_t)(((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_6_5Ga; + } + break; + } } /** -* Read the identification bytes from the sensor -*/ + * Read the identification bytes from the sensor + */ void PIOS_HMC5843_ReadID(uint8_t out[4]) { - while (!PIOS_HMC5843_Read(PIOS_HMC5843_DATAOUT_IDA_REG, out, 3)) ; - out[3] = '\0'; + while (!PIOS_HMC5843_Read(PIOS_HMC5843_DATAOUT_IDA_REG, out, 3)) { + ; + } + out[3] = '\0'; } bool PIOS_HMC5843_NewDataAvailable(void) { - return (pios_hmc5843_data_ready); + return pios_hmc5843_data_ready; } /** -* Reads one or more bytes into a buffer -* \param[in] address HMC5843 register address (depends on size) -* \param[out] buffer destination buffer -* \param[in] len number of bytes which should be read -* \return 0 if operation was successful -* \return -1 if error during I2C transfer -* \return -2 if unable to claim i2c device -*/ -static bool PIOS_HMC5843_Read(uint8_t address, uint8_t * buffer, uint8_t len) + * Reads one or more bytes into a buffer + * \param[in] address HMC5843 register address (depends on size) + * \param[out] buffer destination buffer + * \param[in] len number of bytes which should be read + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + * \return -2 if unable to claim i2c device + */ +static bool PIOS_HMC5843_Read(uint8_t address, uint8_t *buffer, uint8_t len) { - uint8_t addr_buffer[] = { - address, - }; + uint8_t addr_buffer[] = { + address, + }; - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = PIOS_HMC5843_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(addr_buffer), - .buf = addr_buffer, - } - , - { - .info = __func__, - .addr = PIOS_HMC5843_I2C_ADDR, - .rw = PIOS_I2C_TXN_READ, - .len = len, - .buf = buffer, - } - }; + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = PIOS_HMC5843_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(addr_buffer), + .buf = addr_buffer, + } + , + { + .info = __func__, + .addr = PIOS_HMC5843_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); } /** -* Writes one or more bytes to the HMC5843 -* \param[in] address Register address -* \param[in] buffer source buffer -* \return 0 if operation was successful -* \retval -1 if error during I2C transfer -* \retval -2 if unable to claim i2c device -*/ + * Writes one or more bytes to the HMC5843 + * \param[in] address Register address + * \param[in] buffer source buffer + * \return 0 if operation was successful + * \retval -1 if error during I2C transfer + * \retval -2 if unable to claim i2c device + */ static bool PIOS_HMC5843_Write(uint8_t address, uint8_t buffer) { - uint8_t data[] = { - address, - buffer, - }; + uint8_t data[] = { + address, + buffer, + }; - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = PIOS_HMC5843_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(data), - .buf = data, - } - , - }; + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = PIOS_HMC5843_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(data), + .buf = data, + } + , + }; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); } #endif /* PIOS_INCLUDE_HMC5843 */ diff --git a/flight/pios/common/pios_hmc5883.c b/flight/pios/common/pios_hmc5883.c index 1e676fbcb..d425e16dc 100644 --- a/flight/pios/common/pios_hmc5883.c +++ b/flight/pios/common/pios_hmc5883.c @@ -12,19 +12,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,29 +39,29 @@ /* Local Variables */ volatile bool pios_hmc5883_data_ready; -static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg * cfg); -static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t * buffer, uint8_t len); +static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg *cfg); +static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t *buffer, uint8_t len); static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer); -static const struct pios_hmc5883_cfg * dev_cfg; +static const struct pios_hmc5883_cfg *dev_cfg; /** * @brief Initialize the HMC5883 magnetometer sensor. * @return none */ -void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg * cfg) +void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg *cfg) { - dev_cfg = cfg; // store config before enabling interrupt + dev_cfg = cfg; // store config before enabling interrupt #ifdef PIOS_HMC5883_HAS_GPIOS - PIOS_EXTI_Init(cfg->exti_cfg); + PIOS_EXTI_Init(cfg->exti_cfg); #endif - int32_t val = PIOS_HMC5883_Config(cfg); - - PIOS_Assert(val == 0); - - pios_hmc5883_data_ready = false; + int32_t val = PIOS_HMC5883_Config(cfg); + + PIOS_Assert(val == 0); + + pios_hmc5883_data_ready = false; } /** @@ -124,29 +124,33 @@ void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg * cfg) * 1 | 1 | Sleep Mode */ static uint8_t CTRLB = 0x00; -static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg * cfg) +static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg *cfg) { - uint8_t CTRLA = 0x00; - uint8_t MODE = 0x00; - CTRLB = 0; - - CTRLA |= (uint8_t) (cfg->M_ODR | cfg->Meas_Conf); - CTRLB |= (uint8_t) (cfg->Gain); - MODE |= (uint8_t) (cfg->Mode); - - // CRTL_REGA - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, CTRLA) != 0) - return -1; - - // CRTL_REGB - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, CTRLB) != 0) - return -1; - - // Mode register - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, MODE) != 0) - return -1; - - return 0; + uint8_t CTRLA = 0x00; + uint8_t MODE = 0x00; + + CTRLB = 0; + + CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf); + CTRLB |= (uint8_t)(cfg->Gain); + MODE |= (uint8_t)(cfg->Mode); + + // CRTL_REGA + if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, CTRLA) != 0) { + return -1; + } + + // CRTL_REGB + if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, CTRLB) != 0) { + return -1; + } + + // Mode register + if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, MODE) != 0) { + return -1; + } + + return 0; } /** @@ -156,58 +160,58 @@ static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg * cfg) */ int32_t PIOS_HMC5883_ReadMag(int16_t out[3]) { - pios_hmc5883_data_ready = false; - uint8_t buffer[6]; - int32_t temp; - int32_t sensitivity; - - if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_XMSB_REG, buffer, 6) != 0) { - return -1; - } - - switch (CTRLB & 0xE0) { - case 0x00: - sensitivity = PIOS_HMC5883_Sensitivity_0_88Ga; - break; - case 0x20: - sensitivity = PIOS_HMC5883_Sensitivity_1_3Ga; - break; - case 0x40: - sensitivity = PIOS_HMC5883_Sensitivity_1_9Ga; - break; - case 0x60: - sensitivity = PIOS_HMC5883_Sensitivity_2_5Ga; - break; - case 0x80: - sensitivity = PIOS_HMC5883_Sensitivity_4_0Ga; - break; - case 0xA0: - sensitivity = PIOS_HMC5883_Sensitivity_4_7Ga; - break; - case 0xC0: - sensitivity = PIOS_HMC5883_Sensitivity_5_6Ga; - break; - case 0xE0: - sensitivity = PIOS_HMC5883_Sensitivity_8_1Ga; - break; - default: - PIOS_Assert(0); - } - - for (int i = 0; i < 3; i++) { - temp = ((int16_t) ((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / sensitivity; - out[i] = temp; - } - // Data reads out as X,Z,Y - temp = out[2]; - out[2] = out[1]; - out[1] = temp; - - // This should not be necessary but for some reason it is coming out of continuous conversion mode - PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_CONTINUOUS); - - return 0; + pios_hmc5883_data_ready = false; + uint8_t buffer[6]; + int32_t temp; + int32_t sensitivity; + + if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_XMSB_REG, buffer, 6) != 0) { + return -1; + } + + switch (CTRLB & 0xE0) { + case 0x00: + sensitivity = PIOS_HMC5883_Sensitivity_0_88Ga; + break; + case 0x20: + sensitivity = PIOS_HMC5883_Sensitivity_1_3Ga; + break; + case 0x40: + sensitivity = PIOS_HMC5883_Sensitivity_1_9Ga; + break; + case 0x60: + sensitivity = PIOS_HMC5883_Sensitivity_2_5Ga; + break; + case 0x80: + sensitivity = PIOS_HMC5883_Sensitivity_4_0Ga; + break; + case 0xA0: + sensitivity = PIOS_HMC5883_Sensitivity_4_7Ga; + break; + case 0xC0: + sensitivity = PIOS_HMC5883_Sensitivity_5_6Ga; + break; + case 0xE0: + sensitivity = PIOS_HMC5883_Sensitivity_8_1Ga; + break; + default: + PIOS_Assert(0); + } + + for (int i = 0; i < 3; i++) { + temp = ((int16_t)((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / sensitivity; + out[i] = temp; + } + // Data reads out as X,Z,Y + temp = out[2]; + out[2] = out[1]; + out[1] = temp; + + // This should not be necessary but for some reason it is coming out of continuous conversion mode + PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_CONTINUOUS); + + return 0; } @@ -218,9 +222,10 @@ int32_t PIOS_HMC5883_ReadMag(int16_t out[3]) */ uint8_t PIOS_HMC5883_ReadID(uint8_t out[4]) { - uint8_t retval = PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_IDA_REG, out, 3); - out[3] = '\0'; - return retval; + uint8_t retval = PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_IDA_REG, out, 3); + + out[3] = '\0'; + return retval; } /** @@ -230,7 +235,7 @@ uint8_t PIOS_HMC5883_ReadID(uint8_t out[4]) */ bool PIOS_HMC5883_NewDataAvailable(void) { - return (pios_hmc5883_data_ready); + return pios_hmc5883_data_ready; } /** @@ -242,31 +247,31 @@ bool PIOS_HMC5883_NewDataAvailable(void) * \return -1 if error during I2C transfer * \return -2 if unable to claim i2c device */ -static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t * buffer, uint8_t len) +static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t *buffer, uint8_t len) { - uint8_t addr_buffer[] = { - address, - }; - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = PIOS_HMC5883_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(addr_buffer), - .buf = addr_buffer, - } - , - { - .info = __func__, - .addr = PIOS_HMC5883_I2C_ADDR, - .rw = PIOS_I2C_TXN_READ, - .len = len, - .buf = buffer, - } - }; - - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + uint8_t addr_buffer[] = { + address, + }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = PIOS_HMC5883_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(addr_buffer), + .buf = addr_buffer, + } + , + { + .info = __func__, + .addr = PIOS_HMC5883_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); } /** @@ -279,23 +284,24 @@ static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t * buffer, uint8_t len) */ static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer) { - uint8_t data[] = { - address, - buffer, - }; - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = PIOS_HMC5883_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(data), - .buf = data, - } - , - }; - ; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + uint8_t data[] = { + address, + buffer, + }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = PIOS_HMC5883_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(data), + .buf = data, + } + , + }; + + ; + return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); } /** @@ -304,89 +310,101 @@ static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer) */ int32_t PIOS_HMC5883_Test(void) { - int32_t failed = 0; - uint8_t registers[3] = {0,0,0}; - uint8_t status; - uint8_t ctrl_a_read; - uint8_t ctrl_b_read; - uint8_t mode_read; - int16_t values[3]; - - - - /* Verify that ID matches (HMC5883 ID is null-terminated ASCII string "H43") */ - char id[4]; - PIOS_HMC5883_ReadID((uint8_t *)id); - if((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) // Expect H43 - return -1; - - /* Backup existing configuration */ - if (PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A,registers,3) != 0) - return -1; - - /* Stop the device and read out last value */ - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_IDLE) != 0) - return -1; - if( PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status,1) != 0) - return -1; - if (PIOS_HMC5883_ReadMag(values) != 0) - return -1; - - /* - * Put HMC5883 into self test mode - * This is done by placing measurement config into positive (0x01) or negative (0x10) bias - * and then placing the mode register into single-measurement mode. This causes the HMC5883 - * to create an artificial magnetic field of ~1.1 Gauss. - * - * If gain were PIOS_HMC5883_GAIN_2_5, for example, X and Y will read around +766 LSB - * (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga) - * - * Changing measurement config back to PIOS_HMC5883_MEASCONF_NORMAL will leave self-test mode. - */ - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, PIOS_HMC5883_MEASCONF_BIAS_POS | PIOS_HMC5883_ODR_15) != 0) - return -1; - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, PIOS_HMC5883_GAIN_8_1) != 0) - return -1; - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_SINGLE) != 0) - return -1; - - /* Must wait for value to be updated */ - PIOS_DELAY_WaitmS(200); - - if (PIOS_HMC5883_ReadMag(values) != 0) - return -1; - - /* - if(abs(values[0] - 766) > 20) - failed |= 1; - if(abs(values[1] - 766) > 20) - failed |= 1; - if(abs(values[2] - 713) > 20) - failed |= 1; - */ - - PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, &ctrl_a_read,1); - PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_B, &ctrl_b_read,1); - PIOS_HMC5883_Read(PIOS_HMC5883_MODE_REG, &mode_read,1); - PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status,1); - - - /* Restore backup configuration */ - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, registers[0]) != 0) - return -1; - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, registers[1]) != 0) - return -1; - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, registers[2]) != 0) - return -1; - - return failed; + int32_t failed = 0; + uint8_t registers[3] = { 0, 0, 0 }; + uint8_t status; + uint8_t ctrl_a_read; + uint8_t ctrl_b_read; + uint8_t mode_read; + int16_t values[3]; + + + /* Verify that ID matches (HMC5883 ID is null-terminated ASCII string "H43") */ + char id[4]; + + PIOS_HMC5883_ReadID((uint8_t *)id); + if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43 + return -1; + } + + /* Backup existing configuration */ + if (PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, registers, 3) != 0) { + return -1; + } + + /* Stop the device and read out last value */ + PIOS_DELAY_WaitmS(10); + if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_IDLE) != 0) { + return -1; + } + if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status, 1) != 0) { + return -1; + } + if (PIOS_HMC5883_ReadMag(values) != 0) { + return -1; + } + + /* + * Put HMC5883 into self test mode + * This is done by placing measurement config into positive (0x01) or negative (0x10) bias + * and then placing the mode register into single-measurement mode. This causes the HMC5883 + * to create an artificial magnetic field of ~1.1 Gauss. + * + * If gain were PIOS_HMC5883_GAIN_2_5, for example, X and Y will read around +766 LSB + * (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga) + * + * Changing measurement config back to PIOS_HMC5883_MEASCONF_NORMAL will leave self-test mode. + */ + PIOS_DELAY_WaitmS(10); + if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, PIOS_HMC5883_MEASCONF_BIAS_POS | PIOS_HMC5883_ODR_15) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, PIOS_HMC5883_GAIN_8_1) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_SINGLE) != 0) { + return -1; + } + + /* Must wait for value to be updated */ + PIOS_DELAY_WaitmS(200); + + if (PIOS_HMC5883_ReadMag(values) != 0) { + return -1; + } + + /* + if(abs(values[0] - 766) > 20) + failed |= 1; + if(abs(values[1] - 766) > 20) + failed |= 1; + if(abs(values[2] - 713) > 20) + failed |= 1; + */ + + PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, &ctrl_a_read, 1); + PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_B, &ctrl_b_read, 1); + PIOS_HMC5883_Read(PIOS_HMC5883_MODE_REG, &mode_read, 1); + PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status, 1); + + + /* Restore backup configuration */ + PIOS_DELAY_WaitmS(10); + if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, registers[0]) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, registers[1]) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, registers[2]) != 0) { + return -1; + } + + return failed; } /** @@ -394,9 +412,9 @@ int32_t PIOS_HMC5883_Test(void) */ bool PIOS_HMC5883_IRQHandler(void) { - pios_hmc5883_data_ready = true; - - return false; + pios_hmc5883_data_ready = true; + + return false; } #endif /* PIOS_INCLUDE_HMC5883 */ diff --git a/flight/pios/common/pios_i2c_esc.c b/flight/pios/common/pios_i2c_esc.c index 9409b9a68..4800f1bc5 100644 --- a/flight/pios/common/pios_i2c_esc.c +++ b/flight/pios/common/pios_i2c_esc.c @@ -13,19 +13,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,8 +34,8 @@ #ifdef PIOS_INCLUDE_I2C_ESC /* Known i2c ESC addresses */ -#define MK_I2C_ADDR 0x29 -#define ASTEC4_I2C_ADDR 0x02 +#define MK_I2C_ADDR 0x29 +#define ASTEC4_I2C_ADDR 0x02 bool PIOS_SetMKSpeed(uint8_t motornum, uint8_t speed); @@ -43,124 +43,134 @@ uint8_t base_address = MK_I2C_ADDR; uint8_t valid_motors = 0; bool PIOS_I2C_ESC_Config() { - base_address = MK_I2C_ADDR; - valid_motors = 0; - for(uint8_t i = 0; i < 4; i ++) - if(PIOS_SetMKSpeed(i, 0)) - valid_motors |= (1 << i); - - return true; + base_address = MK_I2C_ADDR; + valid_motors = 0; + for (uint8_t i = 0; i < 4; i++) { + if (PIOS_SetMKSpeed(i, 0)) { + valid_motors |= (1 << i); + } + } + + return true; } bool PIOS_I2C_ESC_SetSpeed(uint8_t speed[4]) { - /*bool success = true; - for(uint8_t i = 0; i < 4; i++) { - //if(valid_motors & (1 << i)) - success &= PIOS_SetMKSpeed(i, speed[i]); - } - return success; */ - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = MK_I2C_ADDR + 0, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(speed[0]), - .buf = &speed[0], - } , - { - .info = __func__, - .addr = MK_I2C_ADDR + 1, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(speed[1]), - .buf = &speed[1], - }, - { - .info = __func__, - .addr = MK_I2C_ADDR + 2, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(speed[2]), - .buf = &speed[2], - }, - { - .info = __func__, - .addr = MK_I2C_ADDR + 3, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(speed[3]), - .buf = &speed[3], - } - }; - - return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); + /*bool success = true; + for(uint8_t i = 0; i < 4; i++) { + //if(valid_motors & (1 << i)) + success &= PIOS_SetMKSpeed(i, speed[i]); + } + return success; */ + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = MK_I2C_ADDR + 0, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed[0]), + .buf = &speed[0], + }, + { + .info = __func__, + .addr = MK_I2C_ADDR + 1, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed[1]), + .buf = &speed[1], + }, + { + .info = __func__, + .addr = MK_I2C_ADDR + 2, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed[2]), + .buf = &speed[2], + }, + { + .info = __func__, + .addr = MK_I2C_ADDR + 3, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed[3]), + .buf = &speed[3], + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); } -bool PIOS_SetMKSpeed(uint8_t motornum, uint8_t speed) { - static uint8_t speeds[4] = {0,0,0,0}; - - if(motornum >= 4) - return false; - - if(speeds[motornum] == speed) - return true; - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = MK_I2C_ADDR + motornum, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(speed), - .buf = &speed, - } - }; - - return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); +bool PIOS_SetMKSpeed(uint8_t motornum, uint8_t speed) +{ + static uint8_t speeds[4] = { 0, 0, 0, 0 }; + + if (motornum >= 4) { + return false; + } + + if (speeds[motornum] == speed) { + return true; + } + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = MK_I2C_ADDR + motornum, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed), + .buf = &speed, + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); } -bool PIOS_SetAstec4Address(uint8_t new_address) { - if((new_address < 0) || (new_address > 4)) - return false; - - uint8_t data[4] = {250, 0, new_address, 230+new_address}; - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = ASTEC4_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(data), - .buf = &data[0], - } - }; - - return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); +bool PIOS_SetAstec4Address(uint8_t new_address) +{ + if ((new_address < 0) || (new_address > 4)) { + return false; + } + + uint8_t data[4] = { 250, 0, new_address, 230 + new_address }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = ASTEC4_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(data), + .buf = &data[0], + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); } -bool PIOS_SetAstec4Speed(uint8_t motornum, uint8_t speed) { - static uint8_t speeds[5] = {0,0,0,0}; - - if((motornum < 0) || (motornum >= 4)) - return false; - - speeds[motornum] = speed; - - if(motornum != 3) - return true; - - /* Write in chunks of four */ - speeds[4] = 0xAA + speeds[0] + speeds[1] + speeds[2] + speeds[3]; - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = ASTEC4_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(speeds), - .buf = &speeds[0], - } - }; - - return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); +bool PIOS_SetAstec4Speed(uint8_t motornum, uint8_t speed) +{ + static uint8_t speeds[5] = { 0, 0, 0, 0 }; + + if ((motornum < 0) || (motornum >= 4)) { + return false; + } + + speeds[motornum] = speed; + + if (motornum != 3) { + return true; + } + + /* Write in chunks of four */ + speeds[4] = 0xAA + speeds[0] + speeds[1] + speeds[2] + speeds[3]; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = ASTEC4_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speeds), + .buf = &speeds[0], + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); } #endif /* PIOS_INCLUDE_I2C_ESC */ diff --git a/flight/pios/common/pios_iap.c b/flight/pios/common/pios_iap.c index 77a18b00c..b991bcf30 100644 --- a/flight/pios/common/pios_iap.c +++ b/flight/pios/common/pios_iap.c @@ -6,65 +6,64 @@ * @brief Common IAP functionality * @{ * - * @file pios_iap.c + * @file pios_iap.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief In application programming functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ #include #include #ifdef PIOS_INCLUDE_IAP /**************************************************************************************** - * Private Definitions/Macros - ****************************************************************************************/ +* Private Definitions/Macros +****************************************************************************************/ /* these definitions reside here for protection and privacy. */ -#define IAP_MAGIC_WORD_1 0x1122 -#define IAP_MAGIC_WORD_2 0xAA55 +#define IAP_MAGIC_WORD_1 0x1122 +#define IAP_MAGIC_WORD_2 0xAA55 -#define UPPERWORD16(lw) (uint16_t)((uint32_t)(lw)>>16) -#define LOWERWORD16(lw) (uint16_t)((uint32_t)(lw)&0x0000ffff) -#define UPPERBYTE(w) (uint8_t)((w)>>8) -#define LOWERBYTE(w) (uint8_t)((w)&0x00ff) +#define UPPERWORD16(lw) (uint16_t)((uint32_t)(lw) >> 16) +#define LOWERWORD16(lw) (uint16_t)((uint32_t)(lw) & 0x0000ffff) +#define UPPERBYTE(w) (uint8_t)((w) >> 8) +#define LOWERBYTE(w) (uint8_t)((w) & 0x00ff) /**************************************************************************************** - * Private Functions - ****************************************************************************************/ +* Private Functions +****************************************************************************************/ /**************************************************************************************** - * Private (static) Data - ****************************************************************************************/ -const uint16_t pios_iap_cmd_list[] = - { - IAP_CMD1, - IAP_CMD2, - IAP_CMD3 - }; +* Private (static) Data +****************************************************************************************/ +const uint16_t pios_iap_cmd_list[] = { + IAP_CMD1, + IAP_CMD2, + IAP_CMD3 +}; /**************************************************************************************** - * Public/Global Data - ****************************************************************************************/ +* Public/Global Data +****************************************************************************************/ /*! * \brief PIOS_IAP_Init - performs required initializations for iap module. @@ -74,104 +73,97 @@ const uint16_t pios_iap_cmd_list[] = * * Created: Sep 8, 2010 10:10:48 PM by joe */ -void PIOS_IAP_Init( void ) +void PIOS_IAP_Init(void) { - PIOS_BKP_Init(); - PIOS_BKP_EnableWrite(); + PIOS_BKP_Init(); + PIOS_BKP_EnableWrite(); } /*! * \brief Determines if an In-Application-Programming request has been made. * \param *comm - Which communication stream to use for the IAP (USB, Telemetry, I2C, SPI, etc) * \return TRUE - if correct sequence found, along with 'comm' updated. - * FALSE - Note that 'comm' will have an invalid comm identifier. + * FALSE - Note that 'comm' will have an invalid comm identifier. * \retval * */ -uint32_t PIOS_IAP_CheckRequest( void ) +uint32_t PIOS_IAP_CheckRequest(void) { - uint32_t retval = false; - uint16_t reg1; - uint16_t reg2; + uint32_t retval = false; + uint16_t reg1; + uint16_t reg2; - reg1 = PIOS_BKP_ReadRegister(MAGIC_REG_1); - reg2 = PIOS_BKP_ReadRegister(MAGIC_REG_2); + reg1 = PIOS_BKP_ReadRegister(MAGIC_REG_1); + reg2 = PIOS_BKP_ReadRegister(MAGIC_REG_2); - if( reg1 == IAP_MAGIC_WORD_1 && reg2 == IAP_MAGIC_WORD_2 ) { - // We have a match. - retval = true; - } else { - retval = false; - } - return retval; + if (reg1 == IAP_MAGIC_WORD_1 && reg2 == IAP_MAGIC_WORD_2) { + // We have a match. + retval = true; + } else { + retval = false; + } + return retval; } - /*! * \brief Sets the 1st word of the request sequence. * \param n/a * \return n/a * \retval */ -void PIOS_IAP_SetRequest1(void) +void PIOS_IAP_SetRequest1(void) { - PIOS_BKP_WriteRegister(MAGIC_REG_1, IAP_MAGIC_WORD_1); + PIOS_BKP_WriteRegister(MAGIC_REG_1, IAP_MAGIC_WORD_1); } -void PIOS_IAP_SetRequest2(void) +void PIOS_IAP_SetRequest2(void) { - PIOS_BKP_WriteRegister(MAGIC_REG_2, IAP_MAGIC_WORD_2); + PIOS_BKP_WriteRegister(MAGIC_REG_2, IAP_MAGIC_WORD_2); } -void PIOS_IAP_ClearRequest(void) +void PIOS_IAP_ClearRequest(void) { - PIOS_BKP_WriteRegister(MAGIC_REG_1, 0); - PIOS_BKP_WriteRegister(MAGIC_REG_2, 0); + PIOS_BKP_WriteRegister(MAGIC_REG_1, 0); + PIOS_BKP_WriteRegister(MAGIC_REG_2, 0); } uint16_t PIOS_IAP_ReadBootCount(void) { - return PIOS_BKP_ReadRegister(IAP_BOOTCOUNT); + return PIOS_BKP_ReadRegister(IAP_BOOTCOUNT); } -void PIOS_IAP_WriteBootCount (uint16_t boot_count) +void PIOS_IAP_WriteBootCount(uint16_t boot_count) { - PIOS_BKP_WriteRegister(IAP_BOOTCOUNT, boot_count); + PIOS_BKP_WriteRegister(IAP_BOOTCOUNT, boot_count); } /** - * @brief Return one of the IAP command values passed from bootloader. - * @param number: the index of the command value (0..2). - * @retval the selected command value. - */ + * @brief Return one of the IAP command values passed from bootloader. + * @param number: the index of the command value (0..2). + * @retval the selected command value. + */ uint32_t PIOS_IAP_ReadBootCmd(uint8_t number) { - if(PIOS_IAP_CMD_COUNT < number) - { - PIOS_Assert(0); - } - else - { - return PIOS_BKP_ReadRegister(pios_iap_cmd_list[number]); - } + if (PIOS_IAP_CMD_COUNT < number) { + PIOS_Assert(0); + } else { + return PIOS_BKP_ReadRegister(pios_iap_cmd_list[number]); + } } /** - * @brief Write one of the IAP command values to be passed to firmware from bootloader. - * @param number: the index of the command value (0..2). - * @param value: value to be written. - */ + * @brief Write one of the IAP command values to be passed to firmware from bootloader. + * @param number: the index of the command value (0..2). + * @param value: value to be written. + */ void PIOS_IAP_WriteBootCmd(uint8_t number, uint32_t value) { - if(PIOS_IAP_CMD_COUNT < number) - { - PIOS_Assert(0); - } - else - { - PIOS_BKP_WriteRegister(pios_iap_cmd_list[number], value); - } + if (PIOS_IAP_CMD_COUNT < number) { + PIOS_Assert(0); + } else { + PIOS_BKP_WriteRegister(pios_iap_cmd_list[number], value); + } } #endif /* PIOS_INCLUDE_IAP */ diff --git a/flight/pios/common/pios_l3gd20.c b/flight/pios/common/pios_l3gd20.c index fb621fffd..0dfe32bee 100644 --- a/flight/pios/common/pios_l3gd20.c +++ b/flight/pios/common/pios_l3gd20.c @@ -13,19 +13,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,27 +37,27 @@ /* Global Variables */ enum pios_l3gd20_dev_magic { - PIOS_L3GD20_DEV_MAGIC = 0x9d39bced, + PIOS_L3GD20_DEV_MAGIC = 0x9d39bced, }; #define PIOS_L3GD20_MAX_DOWNSAMPLE 2 struct l3gd20_dev { - uint32_t spi_id; - uint32_t slave_num; - xQueueHandle queue; - const struct pios_l3gd20_cfg * cfg; - enum pios_l3gd20_filter bandwidth; - enum pios_l3gd20_range range; - enum pios_l3gd20_dev_magic magic; + uint32_t spi_id; + uint32_t slave_num; + xQueueHandle queue; + const struct pios_l3gd20_cfg *cfg; + enum pios_l3gd20_filter bandwidth; + enum pios_l3gd20_range range; + enum pios_l3gd20_dev_magic magic; }; -//! Global structure for this device device -static struct l3gd20_dev * dev; +// ! Global structure for this device device +static struct l3gd20_dev *dev; -//! Private functions -static struct l3gd20_dev * PIOS_L3GD20_alloc(void); -static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev * dev); -static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const * cfg); +// ! Private functions +static struct l3gd20_dev *PIOS_L3GD20_alloc(void); +static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev *dev); +static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const *cfg); static int32_t PIOS_L3GD20_SetReg(uint8_t address, uint8_t buffer); static int32_t PIOS_L3GD20_GetReg(uint8_t address); static int32_t PIOS_L3GD20_GetRegISR(uint8_t address, bool *woken); @@ -73,22 +73,24 @@ volatile bool l3gd20_configured = false; /** * @brief Allocate a new device */ -static struct l3gd20_dev * PIOS_L3GD20_alloc(void) +static struct l3gd20_dev *PIOS_L3GD20_alloc(void) { - struct l3gd20_dev * l3gd20_dev; + struct l3gd20_dev *l3gd20_dev; - l3gd20_dev = (struct l3gd20_dev *)pvPortMalloc(sizeof(*l3gd20_dev)); - if (!l3gd20_dev) return (NULL); + l3gd20_dev = (struct l3gd20_dev *)pvPortMalloc(sizeof(*l3gd20_dev)); + if (!l3gd20_dev) { + return NULL; + } - l3gd20_dev->magic = PIOS_L3GD20_DEV_MAGIC; + l3gd20_dev->magic = PIOS_L3GD20_DEV_MAGIC; - l3gd20_dev->queue = xQueueCreate(PIOS_L3GD20_MAX_DOWNSAMPLE, sizeof(struct pios_l3gd20_data)); - if(l3gd20_dev->queue == NULL) { - vPortFree(l3gd20_dev); - return NULL; - } + l3gd20_dev->queue = xQueueCreate(PIOS_L3GD20_MAX_DOWNSAMPLE, sizeof(struct pios_l3gd20_data)); + if (l3gd20_dev->queue == NULL) { + vPortFree(l3gd20_dev); + return NULL; + } - return(l3gd20_dev); + return l3gd20_dev; } /** @@ -97,13 +99,16 @@ static struct l3gd20_dev * PIOS_L3GD20_alloc(void) */ static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev *vdev) { - if (vdev == NULL) - return -1; - if (vdev->magic != PIOS_L3GD20_DEV_MAGIC) - return -2; - if (vdev->spi_id == 0) - return -3; - return 0; + if (vdev == NULL) { + return -1; + } + if (vdev->magic != PIOS_L3GD20_DEV_MAGIC) { + return -2; + } + if (vdev->spi_id == 0) { + return -3; + } + return 0; } /** @@ -111,52 +116,65 @@ static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev *vdev) * @return none */ #include -int32_t PIOS_L3GD20_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_l3gd20_cfg * cfg) +int32_t PIOS_L3GD20_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_l3gd20_cfg *cfg) { - dev = PIOS_L3GD20_alloc(); - if(dev == NULL) - return -1; + dev = PIOS_L3GD20_alloc(); + if (dev == NULL) { + return -1; + } - dev->spi_id = spi_id; - dev->slave_num = slave_num; - dev->cfg = cfg; + dev->spi_id = spi_id; + dev->slave_num = slave_num; + dev->cfg = cfg; - /* Configure the MPU6050 Sensor */ - PIOS_L3GD20_Config(cfg); + /* Configure the MPU6050 Sensor */ + PIOS_L3GD20_Config(cfg); - /* Set up EXTI */ - PIOS_EXTI_Init(cfg->exti_cfg); - - // An initial read is needed to get it running - struct pios_l3gd20_data data; - PIOS_L3GD20_ReadGyros(&data); + /* Set up EXTI */ + PIOS_EXTI_Init(cfg->exti_cfg); - return 0; + // An initial read is needed to get it running + struct pios_l3gd20_data data; + PIOS_L3GD20_ReadGyros(&data); + + return 0; } /** * @brief Initialize the L3GD20 3-axis gyro sensor * \return none * \param[in] PIOS_L3GD20_ConfigTypeDef struct to be used to configure sensor. -* -*/ -static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const * cfg) + * + */ +static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const *cfg) { - // This register enables the channels and sets the bandwidth - while(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG1, PIOS_L3GD20_CTRL1_FASTEST | - PIOS_L3GD20_CTRL1_PD | PIOS_L3GD20_CTRL1_ZEN | - PIOS_L3GD20_CTRL1_YEN | PIOS_L3GD20_CTRL1_XEN) != 0); - - // Disable the high pass filters - while(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG2, 0) != 0); - // Set int2 to go high on data ready - while(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG3, 0x08) != 0); - // Select SPI interface, 500 deg/s, endianness? - while(PIOS_L3GD20_SetRange(cfg->range) != 0); - // Enable FIFO, disable HPF - while(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG5, 0x40) != 0); - // Fifo stream mode - while(PIOS_L3GD20_SetReg(PIOS_L3GD20_FIFO_CTRL_REG, 0x40) != 0); + // This register enables the channels and sets the bandwidth + while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG1, PIOS_L3GD20_CTRL1_FASTEST | + PIOS_L3GD20_CTRL1_PD | PIOS_L3GD20_CTRL1_ZEN | + PIOS_L3GD20_CTRL1_YEN | PIOS_L3GD20_CTRL1_XEN) != 0) { + ; + } + + // Disable the high pass filters + while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG2, 0) != 0) { + ; + } + // Set int2 to go high on data ready + while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG3, 0x08) != 0) { + ; + } + // Select SPI interface, 500 deg/s, endianness? + while (PIOS_L3GD20_SetRange(cfg->range) != 0) { + ; + } + // Enable FIFO, disable HPF + while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG5, 0x40) != 0) { + ; + } + // Fifo stream mode + while (PIOS_L3GD20_SetReg(PIOS_L3GD20_FIFO_CTRL_REG, 0x40) != 0) { + ; + } } /** @@ -165,14 +183,16 @@ static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const * cfg) */ int32_t PIOS_L3GD20_SetRange(enum pios_l3gd20_range range) { - if(PIOS_L3GD20_Validate(dev) != 0) - return -1; + if (PIOS_L3GD20_Validate(dev) != 0) { + return -1; + } - dev->range = range; - if(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG4, dev->range) != 0) - return -2; - - return 0; + dev->range = range; + if (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG4, dev->range) != 0) { + return -2; + } + + return 0; } /** @@ -181,14 +201,16 @@ int32_t PIOS_L3GD20_SetRange(enum pios_l3gd20_range range) */ static int32_t PIOS_L3GD20_ClaimBus() { - if(PIOS_L3GD20_Validate(dev) != 0) - return -1; + if (PIOS_L3GD20_Validate(dev) != 0) { + return -1; + } - if(PIOS_SPI_ClaimBus(dev->spi_id) != 0) - return -2; + if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { + return -2; + } - PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0); - return 0; + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + return 0; } /** @@ -199,14 +221,14 @@ static int32_t PIOS_L3GD20_ClaimBus() */ static int32_t PIOS_L3GD20_ClaimBusISR(bool *woken) { - if(PIOS_L3GD20_Validate(dev) != 0) { - return -1; - } - if(PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { - return -2; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); - return 0; + if (PIOS_L3GD20_Validate(dev) != 0) { + return -1; + } + if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { + return -2; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + return 0; } /** @@ -215,11 +237,11 @@ static int32_t PIOS_L3GD20_ClaimBusISR(bool *woken) */ int32_t PIOS_L3GD20_ReleaseBus() { - if(PIOS_L3GD20_Validate(dev) != 0) { - return -1; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBus(dev->spi_id); + if (PIOS_L3GD20_Validate(dev) != 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + return PIOS_SPI_ReleaseBus(dev->spi_id); } /** @@ -230,11 +252,11 @@ int32_t PIOS_L3GD20_ReleaseBus() */ int32_t PIOS_L3GD20_ReleaseBusISR(bool *woken) { - if(PIOS_L3GD20_Validate(dev) != 0) { - return -1; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); + if (PIOS_L3GD20_Validate(dev) != 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); } /** @@ -244,16 +266,17 @@ int32_t PIOS_L3GD20_ReleaseBusISR(bool *woken) */ static int32_t PIOS_L3GD20_GetReg(uint8_t reg) { - uint8_t data; + uint8_t data; - if(PIOS_L3GD20_ClaimBus() != 0) - return -1; - - PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte - data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response - - PIOS_L3GD20_ReleaseBus(); - return data; + if (PIOS_L3GD20_ClaimBus() != 0) { + return -1; + } + + PIOS_SPI_TransferByte(dev->spi_id, (0x80 | reg)); // request byte + data = PIOS_SPI_TransferByte(dev->spi_id, 0); // receive response + + PIOS_L3GD20_ReleaseBus(); + return data; } /** @@ -265,15 +288,15 @@ static int32_t PIOS_L3GD20_GetReg(uint8_t reg) */ static int32_t PIOS_L3GD20_GetRegISR(uint8_t reg, bool *woken) { - uint8_t data; + uint8_t data; - if(PIOS_L3GD20_ClaimBusISR(woken) != 0) { - return -1; - } - PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte - data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response - PIOS_L3GD20_ReleaseBusISR(woken); - return data; + if (PIOS_L3GD20_ClaimBusISR(woken) != 0) { + return -1; + } + PIOS_SPI_TransferByte(dev->spi_id, (0x80 | reg)); // request byte + data = PIOS_SPI_TransferByte(dev->spi_id, 0); // receive response + PIOS_L3GD20_ReleaseBusISR(woken); + return data; } /** @@ -286,15 +309,16 @@ static int32_t PIOS_L3GD20_GetRegISR(uint8_t reg, bool *woken) */ static int32_t PIOS_L3GD20_SetReg(uint8_t reg, uint8_t data) { - if(PIOS_L3GD20_ClaimBus() != 0) - return -1; - - PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg); - PIOS_SPI_TransferByte(dev->spi_id, data); - - PIOS_L3GD20_ReleaseBus(); - - return 0; + if (PIOS_L3GD20_ClaimBus() != 0) { + return -1; + } + + PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg); + PIOS_SPI_TransferByte(dev->spi_id, data); + + PIOS_L3GD20_ReleaseBus(); + + return 0; } /** @@ -303,41 +327,44 @@ static int32_t PIOS_L3GD20_SetReg(uint8_t reg, uint8_t data) * \returns The number of samples remaining in the fifo */ uint32_t l3gd20_irq = 0; -int32_t PIOS_L3GD20_ReadGyros(struct pios_l3gd20_data * data) +int32_t PIOS_L3GD20_ReadGyros(struct pios_l3gd20_data *data) { - uint8_t buf[7] = {PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0}; - uint8_t rec[7]; - - if(PIOS_L3GD20_ClaimBus() != 0) - return -1; + uint8_t buf[7] = { PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0 }; + uint8_t rec[7]; - if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { - PIOS_L3GD20_ReleaseBus(); - data->gyro_x = 0; - data->gyro_y = 0; - data->gyro_z = 0; - data->temperature = 0; - return -2; - } - - PIOS_L3GD20_ReleaseBus(); - - memcpy((uint8_t *) &(data->gyro_x), &rec[1], 6); - data->temperature = PIOS_L3GD20_GetReg(PIOS_L3GD20_OUT_TEMP); - - return 0; + if (PIOS_L3GD20_ClaimBus() != 0) { + return -1; + } + + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { + PIOS_L3GD20_ReleaseBus(); + data->gyro_x = 0; + data->gyro_y = 0; + data->gyro_z = 0; + data->temperature = 0; + return -2; + } + + PIOS_L3GD20_ReleaseBus(); + + memcpy((uint8_t *)&(data->gyro_x), &rec[1], 6); + data->temperature = PIOS_L3GD20_GetReg(PIOS_L3GD20_OUT_TEMP); + + return 0; } /** * @brief Read the identification bytes from the MPU6050 sensor * \return ID read from MPU6050 or -1 if failure -*/ + */ int32_t PIOS_L3GD20_ReadID() { - int32_t l3gd20_id = PIOS_L3GD20_GetReg(PIOS_L3GD20_WHOAMI); - if(l3gd20_id < 0) - return -1; - return l3gd20_id; + int32_t l3gd20_id = PIOS_L3GD20_GetReg(PIOS_L3GD20_WHOAMI); + + if (l3gd20_id < 0) { + return -1; + } + return l3gd20_id; } /** @@ -346,26 +373,30 @@ int32_t PIOS_L3GD20_ReadID() */ xQueueHandle PIOS_L3GD20_GetQueue() { - if(PIOS_L3GD20_Validate(dev) != 0) - return (xQueueHandle) NULL; + if (PIOS_L3GD20_Validate(dev) != 0) { + return (xQueueHandle)NULL; + } - return dev->queue; + return dev->queue; } -float PIOS_L3GD20_GetScale() +float PIOS_L3GD20_GetScale() { - if(PIOS_L3GD20_Validate(dev) != 0) - return -1; + if (PIOS_L3GD20_Validate(dev) != 0) { + return -1; + } - switch (dev->range) { - case PIOS_L3GD20_SCALE_250_DEG: - return 0.00875f; - case PIOS_L3GD20_SCALE_500_DEG: - return 0.01750f; - case PIOS_L3GD20_SCALE_2000_DEG: - return 0.070f; - } - return 0; + switch (dev->range) { + case PIOS_L3GD20_SCALE_250_DEG: + return 0.00875f; + + case PIOS_L3GD20_SCALE_500_DEG: + return 0.01750f; + + case PIOS_L3GD20_SCALE_2000_DEG: + return 0.070f; + } + return 0; } /** @@ -375,50 +406,53 @@ float PIOS_L3GD20_GetScale() */ uint8_t PIOS_L3GD20_Test(void) { - int32_t l3gd20_id = PIOS_L3GD20_ReadID(); - if(l3gd20_id < 0) - return -1; + int32_t l3gd20_id = PIOS_L3GD20_ReadID(); - uint8_t id = l3gd20_id; - if(id == 0xD4) - return 0; + if (l3gd20_id < 0) { + return -1; + } - return -2; + uint8_t id = l3gd20_id; + if (id == 0xD4) { + return 0; + } + + return -2; } /** -* @brief EXTI IRQ Handler. Read all the data from onboard buffer + * @brief EXTI IRQ Handler. Read all the data from onboard buffer * @return a boolean to the EXTI IRQ Handler wrapper indicating if a * higher priority task is now eligible to run -*/ + */ bool PIOS_L3GD20_IRQHandler(void) { - bool woken = false; - struct pios_l3gd20_data data; - uint8_t buf[7] = {PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0}; - uint8_t rec[7]; + bool woken = false; + struct pios_l3gd20_data data; + uint8_t buf[7] = { PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0 }; + uint8_t rec[7]; - l3gd20_irq++; + l3gd20_irq++; - /* This code duplicates ReadGyros above but uses ClaimBusIsr */ - if (PIOS_L3GD20_ClaimBusISR(&woken) != 0) { - return woken; - } - - if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { - PIOS_L3GD20_ReleaseBusISR(&woken); - return woken; - } - - PIOS_L3GD20_ReleaseBusISR(&woken); - - memcpy((uint8_t *) &(data.gyro_x), &rec[1], 6); - data.temperature = PIOS_L3GD20_GetRegISR(PIOS_L3GD20_OUT_TEMP, &woken); - - signed portBASE_TYPE higherPriorityTaskWoken; - xQueueSendToBackFromISR(dev->queue, (void *) &data, &higherPriorityTaskWoken); - - return (woken || higherPriorityTaskWoken == pdTRUE); + /* This code duplicates ReadGyros above but uses ClaimBusIsr */ + if (PIOS_L3GD20_ClaimBusISR(&woken) != 0) { + return woken; + } + + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { + PIOS_L3GD20_ReleaseBusISR(&woken); + return woken; + } + + PIOS_L3GD20_ReleaseBusISR(&woken); + + memcpy((uint8_t *)&(data.gyro_x), &rec[1], 6); + data.temperature = PIOS_L3GD20_GetRegISR(PIOS_L3GD20_OUT_TEMP, &woken); + + signed portBASE_TYPE higherPriorityTaskWoken; + xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken); + + return woken || higherPriorityTaskWoken == pdTRUE; } #endif /* PIOS_INCLUDE_L3GD20 */ diff --git a/flight/pios/common/pios_mpu6000.c b/flight/pios/common/pios_mpu6000.c index acbc9d4c5..371068e0c 100644 --- a/flight/pios/common/pios_mpu6000.c +++ b/flight/pios/common/pios_mpu6000.c @@ -13,19 +13,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,29 +38,29 @@ /* Global Variables */ enum pios_mpu6000_dev_magic { - PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed, + PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed, }; #define PIOS_MPU6000_MAX_DOWNSAMPLE 2 struct mpu6000_dev { - uint32_t spi_id; - uint32_t slave_num; - xQueueHandle queue; - const struct pios_mpu6000_cfg * cfg; - enum pios_mpu6000_range gyro_range; - enum pios_mpu6000_accel_range accel_range; - enum pios_mpu6000_filter filter; - enum pios_mpu6000_dev_magic magic; + uint32_t spi_id; + uint32_t slave_num; + xQueueHandle queue; + const struct pios_mpu6000_cfg *cfg; + enum pios_mpu6000_range gyro_range; + enum pios_mpu6000_accel_range accel_range; + enum pios_mpu6000_filter filter; + enum pios_mpu6000_dev_magic magic; }; -//! Global structure for this device device -static struct mpu6000_dev * dev; +// ! Global structure for this device device +static struct mpu6000_dev *dev; volatile bool mpu6000_configured = false; -//! Private functions -static struct mpu6000_dev * PIOS_MPU6000_alloc(void); -static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev * dev); -static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg); +// ! Private functions +static struct mpu6000_dev *PIOS_MPU6000_alloc(void); +static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev); +static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg); static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer); static int32_t PIOS_MPU6000_GetReg(uint8_t address); @@ -69,22 +69,24 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t address); /** * @brief Allocate a new device */ -static struct mpu6000_dev * PIOS_MPU6000_alloc(void) +static struct mpu6000_dev *PIOS_MPU6000_alloc(void) { - struct mpu6000_dev * mpu6000_dev; - - mpu6000_dev = (struct mpu6000_dev *)pvPortMalloc(sizeof(*mpu6000_dev)); - if (!mpu6000_dev) return (NULL); - - mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC; - - mpu6000_dev->queue = xQueueCreate(PIOS_MPU6000_MAX_DOWNSAMPLE, sizeof(struct pios_mpu6000_data)); - if(mpu6000_dev->queue == NULL) { - vPortFree(mpu6000_dev); - return NULL; - } - - return(mpu6000_dev); + struct mpu6000_dev *mpu6000_dev; + + mpu6000_dev = (struct mpu6000_dev *)pvPortMalloc(sizeof(*mpu6000_dev)); + if (!mpu6000_dev) { + return NULL; + } + + mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC; + + mpu6000_dev->queue = xQueueCreate(PIOS_MPU6000_MAX_DOWNSAMPLE, sizeof(struct pios_mpu6000_data)); + if (mpu6000_dev->queue == NULL) { + vPortFree(mpu6000_dev); + return NULL; + } + + return mpu6000_dev; } /** @@ -93,37 +95,41 @@ static struct mpu6000_dev * PIOS_MPU6000_alloc(void) */ static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *vdev) { - if (vdev == NULL) - return -1; - if (vdev->magic != PIOS_MPU6000_DEV_MAGIC) - return -2; - if (vdev->spi_id == 0) - return -3; - return 0; + if (vdev == NULL) { + return -1; + } + if (vdev->magic != PIOS_MPU6000_DEV_MAGIC) { + return -2; + } + if (vdev->spi_id == 0) { + return -3; + } + return 0; } /** * @brief Initialize the MPU6000 3-axis gyro sensor. * @return 0 for success, -1 for failure */ -int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg * cfg) +int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *cfg) { - dev = PIOS_MPU6000_alloc(); - if(dev == NULL) - return -1; - - dev->spi_id = spi_id; - dev->slave_num = slave_num; - dev->cfg = cfg; + dev = PIOS_MPU6000_alloc(); + if (dev == NULL) { + return -1; + } - /* Configure the MPU6000 Sensor */ - PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256); - PIOS_MPU6000_Config(cfg); - PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16); + dev->spi_id = spi_id; + dev->slave_num = slave_num; + dev->cfg = cfg; - /* Set up EXTI line */ - PIOS_EXTI_Init(cfg->exti_cfg); - return 0; + /* Configure the MPU6000 Sensor */ + PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256); + PIOS_MPU6000_Config(cfg); + PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16); + + /* Set up EXTI line */ + PIOS_EXTI_Init(cfg->exti_cfg); + return 0; } /** @@ -132,99 +138,131 @@ int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios * \param[in] PIOS_MPU6000_ConfigTypeDef struct to be used to configure sensor. * */ -static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg) +static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg) { + PIOS_MPU6000_Test(); - PIOS_MPU6000_Test(); + // Reset chip + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, PIOS_MPU6000_PWRMGMT_IMU_RST) != 0) { + ; + } + PIOS_DELAY_WaitmS(50); - // Reset chip - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, PIOS_MPU6000_PWRMGMT_IMU_RST) != 0); - PIOS_DELAY_WaitmS(50); + // Reset chip and fifo + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, + PIOS_MPU6000_USERCTL_GYRO_RST | + PIOS_MPU6000_USERCTL_SIG_COND | + PIOS_MPU6000_USERCTL_FIFO_RST) != 0) { + ; + } - // Reset chip and fifo - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, - PIOS_MPU6000_USERCTL_GYRO_RST | - PIOS_MPU6000_USERCTL_SIG_COND | - PIOS_MPU6000_USERCTL_FIFO_RST) != 0); + // Wait for reset to finish + while (PIOS_MPU6000_GetReg(PIOS_MPU6000_USER_CTRL_REG) & + (PIOS_MPU6000_USERCTL_GYRO_RST | + PIOS_MPU6000_USERCTL_SIG_COND | + PIOS_MPU6000_USERCTL_FIFO_RST)) { + ; + } + PIOS_DELAY_WaitmS(10); + // Power management configuration + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) { + ; + } - // Wait for reset to finish - while (PIOS_MPU6000_GetReg(PIOS_MPU6000_USER_CTRL_REG) & - (PIOS_MPU6000_USERCTL_GYRO_RST | - PIOS_MPU6000_USERCTL_SIG_COND | - PIOS_MPU6000_USERCTL_FIFO_RST)); - PIOS_DELAY_WaitmS(10); - //Power management configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0); + // Interrupt configuration + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) { + ; + } - // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0); + // Interrupt configuration + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) { + ; + } - // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0); - - // FIFO storage + // FIFO storage #if defined(PIOS_MPU6000_ACCEL) - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store | PIOS_MPU6000_ACCEL_OUT) != 0); + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store | PIOS_MPU6000_ACCEL_OUT) != 0) { + ; + } #else - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0); + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0) { + ; + } #endif - PIOS_MPU6000_ConfigureRanges(cfg->gyro_range, cfg->accel_range, cfg->filter); - // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) ; - - // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) ; - - // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) ; + PIOS_MPU6000_ConfigureRanges(cfg->gyro_range, cfg->accel_range, cfg->filter); + // Interrupt configuration + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) { + ; + } - // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) ; - if((PIOS_MPU6000_GetReg(PIOS_MPU6000_INT_EN_REG)) != cfg->interrupt_en) - return; - - mpu6000_configured = true; + // Interrupt configuration + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) { + ; + } + + // Interrupt configuration + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) { + ; + } + + // Interrupt configuration + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) { + ; + } + if ((PIOS_MPU6000_GetReg(PIOS_MPU6000_INT_EN_REG)) != cfg->interrupt_en) { + return; + } + + mpu6000_configured = true; } /** * @brief Configures Gyro, accel and Filter ranges/setings * @return 0 if successful, -1 if device has not been initialized */ int32_t PIOS_MPU6000_ConfigureRanges( - enum pios_mpu6000_range gyroRange, - enum pios_mpu6000_accel_range accelRange, - enum pios_mpu6000_filter filterSetting - ) + enum pios_mpu6000_range gyroRange, + enum pios_mpu6000_accel_range accelRange, + enum pios_mpu6000_filter filterSetting) { - if(dev == NULL) - return -1; + if (dev == NULL) { + return -1; + } - // TODO: check that changing the SPI clock speed is safe - // to do here given that interrupts are enabled and the bus has - // not been claimed/is not claimed during this call - PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256); + // TODO: check that changing the SPI clock speed is safe + // to do here given that interrupts are enabled and the bus has + // not been claimed/is not claimed during this call + PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256); - // update filter settings - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0); - - // Sample rate divider, chosen upon digital filtering settings - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_SMPLRT_DIV_REG, - filterSetting == PIOS_MPU6000_LOWPASS_256_HZ ? - dev->cfg->Smpl_rate_div_no_dlp : dev->cfg->Smpl_rate_div_dlp) != 0); - - dev->filter = filterSetting; - - // Gyro range - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_GYRO_CFG_REG, gyroRange) != 0); - - dev->gyro_range = gyroRange; + // update filter settings + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0) { + ; + } + + // Sample rate divider, chosen upon digital filtering settings + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_SMPLRT_DIV_REG, + filterSetting == PIOS_MPU6000_LOWPASS_256_HZ ? + dev->cfg->Smpl_rate_div_no_dlp : dev->cfg->Smpl_rate_div_dlp) != 0) { + ; + } + + dev->filter = filterSetting; + + // Gyro range + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_GYRO_CFG_REG, gyroRange) != 0) { + ; + } + + dev->gyro_range = gyroRange; #if defined(PIOS_MPU6000_ACCEL) - // Set the accel range - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0); - - dev->accel_range = accelRange; + // Set the accel range + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0) { + ; + } + + dev->accel_range = accelRange; #endif - PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16); - return 0; + PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16); + return 0; } /** @@ -233,14 +271,14 @@ int32_t PIOS_MPU6000_ConfigureRanges( */ static int32_t PIOS_MPU6000_ClaimBus() { - if(PIOS_MPU6000_Validate(dev) != 0) { - return -1; - } - if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { - return -2; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); - return 0; + if (PIOS_MPU6000_Validate(dev) != 0) { + return -1; + } + if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { + return -2; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + return 0; } /** @@ -251,14 +289,14 @@ static int32_t PIOS_MPU6000_ClaimBus() */ static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken) { - if (PIOS_MPU6000_Validate(dev) != 0) { - return -1; - } - if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { - return -2; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); - return 0; + if (PIOS_MPU6000_Validate(dev) != 0) { + return -1; + } + if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { + return -2; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + return 0; } /** @@ -267,11 +305,11 @@ static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken) */ static int32_t PIOS_MPU6000_ReleaseBus() { - if(PIOS_MPU6000_Validate(dev) != 0) { - return -1; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBus(dev->spi_id); + if (PIOS_MPU6000_Validate(dev) != 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + return PIOS_SPI_ReleaseBus(dev->spi_id); } /** @@ -282,11 +320,11 @@ static int32_t PIOS_MPU6000_ReleaseBus() */ static int32_t PIOS_MPU6000_ReleaseBusISR(bool *woken) { - if(PIOS_MPU6000_Validate(dev) != 0) { - return -1; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); + if (PIOS_MPU6000_Validate(dev) != 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); } /** @@ -296,17 +334,17 @@ static int32_t PIOS_MPU6000_ReleaseBusISR(bool *woken) */ static int32_t PIOS_MPU6000_GetReg(uint8_t reg) { - uint8_t data; - - if(PIOS_MPU6000_ClaimBus() != 0) { - return -1; - } - - PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte - data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response - - PIOS_MPU6000_ReleaseBus(); - return data; + uint8_t data; + + if (PIOS_MPU6000_ClaimBus() != 0) { + return -1; + } + + PIOS_SPI_TransferByte(dev->spi_id, (0x80 | reg)); // request byte + data = PIOS_SPI_TransferByte(dev->spi_id, 0); // receive response + + PIOS_MPU6000_ReleaseBus(); + return data; } /** @@ -319,23 +357,23 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg) */ static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data) { - if (PIOS_MPU6000_ClaimBus() != 0) { - return -1; - } - - if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) { - PIOS_MPU6000_ReleaseBus(); - return -2; - } - - if (PIOS_SPI_TransferByte(dev->spi_id, data) != 0) { - PIOS_MPU6000_ReleaseBus(); - return -3; - } + if (PIOS_MPU6000_ClaimBus() != 0) { + return -1; + } - PIOS_MPU6000_ReleaseBus(); - - return 0; + if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) { + PIOS_MPU6000_ReleaseBus(); + return -2; + } + + if (PIOS_SPI_TransferByte(dev->spi_id, data) != 0) { + PIOS_MPU6000_ReleaseBus(); + return -3; + } + + PIOS_MPU6000_ReleaseBus(); + + return 0; } /** @@ -343,39 +381,41 @@ static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data) * \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings * \returns 0 if succesful */ -int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data * data) +int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data) { - // THIS FUNCTION IS DEPRECATED AND DOES NOT PERFORM A ROTATION - uint8_t buf[7] = {PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0}; - uint8_t rec[7]; - - if (PIOS_MPU6000_ClaimBus() != 0) { - return -1; - } + // THIS FUNCTION IS DEPRECATED AND DOES NOT PERFORM A ROTATION + uint8_t buf[7] = { PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0 }; + uint8_t rec[7]; - if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { - return -2; - } + if (PIOS_MPU6000_ClaimBus() != 0) { + return -1; + } - PIOS_MPU6000_ReleaseBus(); - - data->gyro_x = rec[1] << 8 | rec[2]; - data->gyro_y = rec[3] << 8 | rec[4]; - data->gyro_z = rec[5] << 8 | rec[6]; - - return 0; + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { + return -2; + } + + PIOS_MPU6000_ReleaseBus(); + + data->gyro_x = rec[1] << 8 | rec[2]; + data->gyro_y = rec[3] << 8 | rec[4]; + data->gyro_z = rec[5] << 8 | rec[6]; + + return 0; } /* * @brief Read the identification bytes from the MPU6000 sensor * \return ID read from MPU6000 or -1 if failure -*/ + */ int32_t PIOS_MPU6000_ReadID() { - int32_t mpu6000_id = PIOS_MPU6000_GetReg(PIOS_MPU6000_WHOAMI); - if(mpu6000_id < 0) - return -1; - return mpu6000_id; + int32_t mpu6000_id = PIOS_MPU6000_GetReg(PIOS_MPU6000_WHOAMI); + + if (mpu6000_id < 0) { + return -1; + } + return mpu6000_id; } /** @@ -384,41 +424,48 @@ int32_t PIOS_MPU6000_ReadID() */ xQueueHandle PIOS_MPU6000_GetQueue() { - if(PIOS_MPU6000_Validate(dev) != 0) - return (xQueueHandle) NULL; - - return dev->queue; + if (PIOS_MPU6000_Validate(dev) != 0) { + return (xQueueHandle)NULL; + } + + return dev->queue; } -float PIOS_MPU6000_GetScale() +float PIOS_MPU6000_GetScale() { - switch (dev->gyro_range) { - case PIOS_MPU6000_SCALE_250_DEG: - return 1.0f / 131.0f; - case PIOS_MPU6000_SCALE_500_DEG: - return 1.0f / 65.5f; - case PIOS_MPU6000_SCALE_1000_DEG: - return 1.0f / 32.8f; - case PIOS_MPU6000_SCALE_2000_DEG: - return 1.0f / 16.4f; - } - return 0; + switch (dev->gyro_range) { + case PIOS_MPU6000_SCALE_250_DEG: + return 1.0f / 131.0f; + + case PIOS_MPU6000_SCALE_500_DEG: + return 1.0f / 65.5f; + + case PIOS_MPU6000_SCALE_1000_DEG: + return 1.0f / 32.8f; + + case PIOS_MPU6000_SCALE_2000_DEG: + return 1.0f / 16.4f; + } + return 0; } float PIOS_MPU6000_GetAccelScale() { - switch (dev->accel_range) { - case PIOS_MPU6000_ACCEL_2G: - return GRAV / 16384.0f; - case PIOS_MPU6000_ACCEL_4G: - return GRAV / 8192.0f; - case PIOS_MPU6000_ACCEL_8G: - return GRAV / 4096.0f; - case PIOS_MPU6000_ACCEL_16G: - return GRAV / 2048.0f; - } - return 0; + switch (dev->accel_range) { + case PIOS_MPU6000_ACCEL_2G: + return GRAV / 16384.0f; + + case PIOS_MPU6000_ACCEL_4G: + return GRAV / 8192.0f; + + case PIOS_MPU6000_ACCEL_8G: + return GRAV / 4096.0f; + + case PIOS_MPU6000_ACCEL_16G: + return GRAV / 2048.0f; + } + return 0; } /** @@ -428,15 +475,18 @@ float PIOS_MPU6000_GetAccelScale() */ int32_t PIOS_MPU6000_Test(void) { - /* Verify that ID matches (MPU6000 ID is 0x69) */ - int32_t mpu6000_id = PIOS_MPU6000_ReadID(); - if(mpu6000_id < 0) - return -1; - - if(mpu6000_id != 0x68) - return -2; - - return 0; + /* Verify that ID matches (MPU6000 ID is 0x69) */ + int32_t mpu6000_id = PIOS_MPU6000_ReadID(); + + if (mpu6000_id < 0) { + return -1; + } + + if (mpu6000_id != 0x68) { + return -2; + } + + return 0; } /** @@ -447,31 +497,31 @@ int32_t PIOS_MPU6000_Test(void) */ static int32_t PIOS_MPU6000_FifoDepthISR(bool *woken) { - uint8_t mpu6000_send_buf[3] = {PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0}; - uint8_t mpu6000_rec_buf[3]; + uint8_t mpu6000_send_buf[3] = { PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0 }; + uint8_t mpu6000_rec_buf[3]; - if(PIOS_MPU6000_ClaimBusISR(woken) != 0) { - return -1; - } + if (PIOS_MPU6000_ClaimBusISR(woken) != 0) { + return -1; + } - if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { - PIOS_MPU6000_ReleaseBusISR(woken); - return -1; - } + if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { + PIOS_MPU6000_ReleaseBusISR(woken); + return -1; + } - PIOS_MPU6000_ReleaseBusISR(woken); - - return (mpu6000_rec_buf[1] << 8) | mpu6000_rec_buf[2]; + PIOS_MPU6000_ReleaseBusISR(woken); + + return (mpu6000_rec_buf[1] << 8) | mpu6000_rec_buf[2]; } /** -* @brief EXTI IRQ Handler. Read all the data from onboard buffer -* @return a boolean to the EXTI IRQ Handler wrapper indicating if a -* higher priority task is now eligible to run -*/ + * @brief EXTI IRQ Handler. Read all the data from onboard buffer + * @return a boolean to the EXTI IRQ Handler wrapper indicating if a + * higher priority task is now eligible to run + */ uint32_t mpu6000_irq = 0; int32_t mpu6000_count; -uint32_t mpu6000_fifo_backup = 0; +uint32_t mpu6000_fifo_backup = 0; uint8_t mpu6000_last_read_count = 0; uint32_t mpu6000_fails = 0; @@ -482,120 +532,122 @@ uint32_t mpu6000_transfer_size; bool PIOS_MPU6000_IRQHandler(void) { - bool woken = false; - static uint32_t timeval; - mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval); - timeval = PIOS_DELAY_GetRaw(); + bool woken = false; + static uint32_t timeval; - if (!mpu6000_configured) { - return false; - } + mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval); + timeval = PIOS_DELAY_GetRaw(); - mpu6000_count = PIOS_MPU6000_FifoDepthISR(&woken); - if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data)) { - return woken; - } + if (!mpu6000_configured) { + return false; + } - if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) { - return woken; - } + mpu6000_count = PIOS_MPU6000_FifoDepthISR(&woken); + if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data)) { + return woken; + } - static uint8_t mpu6000_send_buf[1 + sizeof(struct pios_mpu6000_data) ] = {PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0}; - static uint8_t mpu6000_rec_buf[1 + sizeof(struct pios_mpu6000_data) ]; + if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) { + return woken; + } - if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { - PIOS_MPU6000_ReleaseBusISR(&woken); - mpu6000_fails++; - return woken; - } + static uint8_t mpu6000_send_buf[1 + sizeof(struct pios_mpu6000_data)] = { PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0 }; + static uint8_t mpu6000_rec_buf[1 + sizeof(struct pios_mpu6000_data)]; - PIOS_MPU6000_ReleaseBusISR(&woken); + if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { + PIOS_MPU6000_ReleaseBusISR(&woken); + mpu6000_fails++; + return woken; + } - static struct pios_mpu6000_data data; + PIOS_MPU6000_ReleaseBusISR(&woken); - // In the case where extras samples backed up grabbed an extra - if (mpu6000_count >= (int32_t)(sizeof(data) * 2)) { - mpu6000_fifo_backup++; - if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) - return woken; + static struct pios_mpu6000_data data; - if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { - PIOS_MPU6000_ReleaseBusISR(&woken); - mpu6000_fails++; - return woken; - } + // In the case where extras samples backed up grabbed an extra + if (mpu6000_count >= (int32_t)(sizeof(data) * 2)) { + mpu6000_fifo_backup++; + if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) { + return woken; + } - PIOS_MPU6000_ReleaseBusISR(&woken); - } + if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { + PIOS_MPU6000_ReleaseBusISR(&woken); + mpu6000_fails++; + return woken; + } - // Rotate the sensor to OP convention. The datasheet defines X as towards the right - // and Y as forward. OP convention transposes this. Also the Z is defined negatively - // to our convention + PIOS_MPU6000_ReleaseBusISR(&woken); + } + + // Rotate the sensor to OP convention. The datasheet defines X as towards the right + // and Y as forward. OP convention transposes this. Also the Z is defined negatively + // to our convention #if defined(PIOS_MPU6000_ACCEL) - // Currently we only support rotations on top so switch X/Y accordingly - switch (dev->cfg->orientation) { - case PIOS_MPU6000_TOP_0DEG: - data.accel_y = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X - data.accel_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y - data.gyro_y = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X - data.gyro_x = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y - break; - case PIOS_MPU6000_TOP_90DEG: - // -1 to bring it back to -32768 +32767 range - data.accel_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y - data.accel_x = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X - data.gyro_y = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y - data.gyro_x = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X - break; - case PIOS_MPU6000_TOP_180DEG: - data.accel_y = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X - data.accel_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y - data.gyro_y = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X - data.gyro_x = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y - break; - case PIOS_MPU6000_TOP_270DEG: - data.accel_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y - data.accel_x = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X - data.gyro_y = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y - data.gyro_x = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X - break; - } - data.gyro_z = -1 - (mpu6000_rec_buf[13] << 8 | mpu6000_rec_buf[14]); - data.accel_z = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); - data.temperature = mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]; -#else - data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; - data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; - switch (dev->cfg->orientation) { - case PIOS_MPU6000_TOP_0DEG: - data.gyro_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; - data.gyro_x = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; - break; - case PIOS_MPU6000_TOP_90DEG: - data.gyro_y = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); // chip Y - data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip X - break; - case PIOS_MPU6000_TOP_180DEG: - data.gyro_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); - data.gyro_x = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); - break; - case PIOS_MPU6000_TOP_270DEG: - data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; // chip Y - data.gyro_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip X - break; - } - data.gyro_z = -1 - (mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]); - data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; -#endif + // Currently we only support rotations on top so switch X/Y accordingly + switch (dev->cfg->orientation) { + case PIOS_MPU6000_TOP_0DEG: + data.accel_y = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X + data.accel_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y + data.gyro_y = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X + data.gyro_x = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y + break; + case PIOS_MPU6000_TOP_90DEG: + // -1 to bring it back to -32768 +32767 range + data.accel_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y + data.accel_x = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X + data.gyro_y = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y + data.gyro_x = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X + break; + case PIOS_MPU6000_TOP_180DEG: + data.accel_y = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X + data.accel_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y + data.gyro_y = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X + data.gyro_x = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y + break; + case PIOS_MPU6000_TOP_270DEG: + data.accel_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y + data.accel_x = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X + data.gyro_y = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y + data.gyro_x = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X + break; + } + data.gyro_z = -1 - (mpu6000_rec_buf[13] << 8 | mpu6000_rec_buf[14]); + data.accel_z = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); + data.temperature = mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]; +#else /* if defined(PIOS_MPU6000_ACCEL) */ + data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; + data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; + switch (dev->cfg->orientation) { + case PIOS_MPU6000_TOP_0DEG: + data.gyro_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; + data.gyro_x = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; + break; + case PIOS_MPU6000_TOP_90DEG: + data.gyro_y = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); // chip Y + data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip X + break; + case PIOS_MPU6000_TOP_180DEG: + data.gyro_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); + data.gyro_x = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); + break; + case PIOS_MPU6000_TOP_270DEG: + data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; // chip Y + data.gyro_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip X + break; + } + data.gyro_z = -1 - (mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]); + data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; +#endif /* if defined(PIOS_MPU6000_ACCEL) */ - signed portBASE_TYPE higherPriorityTaskWoken; - xQueueSendToBackFromISR(dev->queue, (void *) &data, &higherPriorityTaskWoken); + signed portBASE_TYPE higherPriorityTaskWoken; + xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken); - mpu6000_irq++; + mpu6000_irq++; - mpu6000_time_us = PIOS_DELAY_DiffuS(timeval); + mpu6000_time_us = PIOS_DELAY_DiffuS(timeval); - return (woken || higherPriorityTaskWoken == pdTRUE); + return woken || higherPriorityTaskWoken == pdTRUE; } #endif /* PIOS_INCLUDE_MPU6000 */ diff --git a/flight/pios/common/pios_mpxv.c b/flight/pios/common/pios_mpxv.c index eae443852..4e6e3ce82 100644 --- a/flight/pios/common/pios_mpxv.c +++ b/flight/pios/common/pios_mpxv.c @@ -3,8 +3,8 @@ * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ * @addtogroup PIOS_MPXV Functions - * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV*. - * This is a differential sensor, so the value returned is first converted into + * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV*. + * This is a differential sensor, so the value returned is first converted into * calibrated airspeed, using http://en.wikipedia.org/wiki/Calibrated_airspeed * @{ * @@ -14,19 +14,19 @@ * @see The GNU Public License (GPL) Version 3 * ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,9 @@ #ifdef PIOS_INCLUDE_MPXV -#define A0 340.27f //speed of sound at standard sea level in [m/s] -#define P0 101.325f //static air pressure at standard sea level in kPa -#define POWER (2.0f/7.0f) +#define A0 340.27f // speed of sound at standard sea level in [m/s] +#define P0 101.325f // static air pressure at standard sea level in kPa +#define POWER (2.0f / 7.0f) #include "pios_mpxv.h" @@ -45,15 +45,17 @@ */ uint16_t PIOS_MPXV_Measure(PIOS_MPXV_descriptor *desc) { - if (desc) - return PIOS_ADC_PinGet(desc->airspeedADCPin); + if (desc) { + return PIOS_ADC_PinGet(desc->airspeedADCPin); + } return 0; } /* - *Returns zeroPoint so that the user can inspect the calibration vs. the sensor value + * Returns zeroPoint so that the user can inspect the calibration vs. the sensor value */ -uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc,uint16_t measurement) { +uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc, uint16_t measurement) +{ desc->calibrationSum += measurement; desc->calibrationCount++; desc->zeroPoint = (uint16_t)(((float)desc->calibrationSum) / desc->calibrationCount); @@ -61,27 +63,27 @@ uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc,uint16_t measurement) { } /* - * Reads the airspeed and returns CAS (calibrated airspeed) in the case of success. + * Reads the airspeed and returns CAS (calibrated airspeed) in the case of success. * In the case of a failed read, returns -1. */ -float PIOS_MPXV_CalcAirspeed(PIOS_MPXV_descriptor *desc,uint16_t measurement) +float PIOS_MPXV_CalcAirspeed(PIOS_MPXV_descriptor *desc, uint16_t measurement) { - //Calculate dynamic pressure, as per docs - float Qc = 3.3f/4096.0f * (float)(measurement - desc->zeroPoint); + // Calculate dynamic pressure, as per docs + float Qc = 3.3f / 4096.0f * (float)(measurement - desc->zeroPoint); - //Saturate Qc on the lower bound, in order to make sure we don't have negative airspeeds. No need + // Saturate Qc on the lower bound, in order to make sure we don't have negative airspeeds. No need // to saturate on the upper bound, we'll handle that later with calibratedAirspeed. if (Qc < 0) { - Qc=0; + Qc = 0; } - //Compute calibrated airspeed, as per http://en.wikipedia.org/wiki/Calibrated_airspeed - float calibratedAirspeed = A0 * sqrtf( 5.0f * (powf(Qc / P0 + 1.0f, POWER) - 1.0f)); + // Compute calibrated airspeed, as per http://en.wikipedia.org/wiki/Calibrated_airspeed + float calibratedAirspeed = A0 * sqrtf(5.0f * (powf(Qc / P0 + 1.0f, POWER) - 1.0f)); - //Upper bound airspeed. No need to lower bound it, that comes from Qc - //in [m/s] + // Upper bound airspeed. No need to lower bound it, that comes from Qc + // in [m/s] if (calibratedAirspeed > desc->maxSpeed) { - calibratedAirspeed=desc->maxSpeed; + calibratedAirspeed = desc->maxSpeed; } return calibratedAirspeed; diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c index 7e4358b35..24ec4c006 100644 --- a/flight/pios/common/pios_ms5611.c +++ b/flight/pios/common/pios_ms5611.c @@ -6,25 +6,25 @@ * @brief Hardware functions to deal with the altitude pressure sensor * @{ * - * @file pios_ms5611.c + * @file pios_ms5611.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief MS5611 Pressure Sensor Routines * @see The GNU Public License (GPL) Version 3 * ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -49,12 +49,12 @@ static uint32_t RawPressure; static int64_t Pressure; static int64_t Temperature; -static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t * buffer, uint8_t len); +static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len); static int32_t PIOS_MS5611_WriteCommand(uint8_t command); // Move into proper driver structure with cfg stored static uint32_t oversampling; -static const struct pios_ms5611_cfg * dev_cfg; +static const struct pios_ms5611_cfg *dev_cfg; static int32_t i2c_id; @@ -62,107 +62,117 @@ static int32_t i2c_id; * Initialise the MS5611 sensor */ int32_t ms5611_read_flag; -void PIOS_MS5611_Init(const struct pios_ms5611_cfg * cfg, int32_t i2c_device) +void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device) { - i2c_id = i2c_device; + i2c_id = i2c_device; - oversampling = cfg->oversampling; - dev_cfg = cfg; // Store cfg before enabling interrupt + oversampling = cfg->oversampling; + dev_cfg = cfg; // Store cfg before enabling interrupt - PIOS_MS5611_WriteCommand(MS5611_RESET); - PIOS_DELAY_WaitmS(20); + PIOS_MS5611_WriteCommand(MS5611_RESET); + PIOS_DELAY_WaitmS(20); - uint8_t data[2]; + uint8_t data[2]; - /* Calibration parameters */ - for (int i = 0; i < 6; i++) { - PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2); - CalibData.C[i] = (data[0] << 8) | data[1]; - } + /* Calibration parameters */ + for (int i = 0; i < 6; i++) { + PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2); + CalibData.C[i] = (data[0] << 8) | data[1]; + } } /** -* Start the ADC conversion -* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR -* \return 0 for success, -1 for failure (conversion completed and not read) -*/ + * Start the ADC conversion + * \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR + * \return 0 for success, -1 for failure (conversion completed and not read) + */ int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type) { - /* Start the conversion */ - if (Type == TemperatureConv) { - while (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) - continue; - } else if (Type == PressureConv) { - while (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) - continue; - } + /* Start the conversion */ + if (Type == TemperatureConv) { + while (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) { + continue; + } + } else if (Type == PressureConv) { + while (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) { + continue; + } + } - CurrentRead = Type; - - return 0; + CurrentRead = Type; + + return 0; } /** * @brief Return the delay for the current osr */ -int32_t PIOS_MS5611_GetDelay() { - switch(oversampling) { - case MS5611_OSR_256: - return 2; - case MS5611_OSR_512: - return 2; - case MS5611_OSR_1024: - return 3; - case MS5611_OSR_2048: - return 5; - case MS5611_OSR_4096: - return 10; - default: - break; - } - return 10; +int32_t PIOS_MS5611_GetDelay() +{ + switch (oversampling) { + case MS5611_OSR_256: + return 2; + + case MS5611_OSR_512: + return 2; + + case MS5611_OSR_1024: + return 3; + + case MS5611_OSR_2048: + return 5; + + case MS5611_OSR_4096: + return 10; + + default: + break; + } + return 10; } /** -* Read the ADC conversion value (once ADC conversion has completed) -* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR -* \return 0 if successfully read the ADC, -1 if failed -*/ + * Read the ADC conversion value (once ADC conversion has completed) + * \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR + * \return 0 if successfully read the ADC, -1 if failed + */ int32_t PIOS_MS5611_ReadADC(void) { - uint8_t Data[3]; - Data[0] = 0; - Data[1] = 0; - Data[2] = 0; - - static int64_t deltaTemp; + uint8_t Data[3]; - /* Read and store the 16bit result */ - if (CurrentRead == TemperatureConv) { - /* Read the temperature conversion */ - if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) - return -1; + Data[0] = 0; + Data[1] = 0; + Data[2] = 0; - RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2]; - - deltaTemp = ((int32_t) RawTemperature) - (CalibData.C[4] << 8); - Temperature = 2000l + ((deltaTemp * CalibData.C[5]) >> 23); + static int64_t deltaTemp; - } else { - int64_t Offset; - int64_t Sens; + /* Read and store the 16bit result */ + if (CurrentRead == TemperatureConv) { + /* Read the temperature conversion */ + if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { + return -1; + } - /* Read the pressure conversion */ - if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) - return -1; - RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); - - Offset = (((int64_t) CalibData.C[1]) << 16) + ((((int64_t) CalibData.C[3]) * deltaTemp) >> 7); - Sens = ((int64_t) CalibData.C[0]) << 15; - Sens = Sens + ((((int64_t) CalibData.C[2]) * deltaTemp) >> 8); - - Pressure = (((((int64_t) RawPressure) * Sens) >> 21) - Offset) >> 15; - } - return 0; + RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2]; + + deltaTemp = ((int32_t)RawTemperature) - (CalibData.C[4] << 8); + Temperature = 2000l + ((deltaTemp * CalibData.C[5]) >> 23); + } else { + int64_t Offset; + int64_t Sens; + + /* Read the pressure conversion */ + if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { + return -1; + } + RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); + + Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7); + Sens = ((int64_t)CalibData.C[0]) << 15; + Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8); + + Pressure = (((((int64_t)RawPressure) * Sens) >> 21) - Offset) >> 15; + } + return 0; } /** @@ -170,7 +180,7 @@ int32_t PIOS_MS5611_ReadADC(void) */ float PIOS_MS5611_GetTemperature(void) { - return ((float) Temperature) / 100.0f; + return ((float)Temperature) / 100.0f; } /** @@ -178,88 +188,89 @@ float PIOS_MS5611_GetTemperature(void) */ float PIOS_MS5611_GetPressure(void) { - return ((float) Pressure) / 1000.0f; + return ((float)Pressure) / 1000.0f; } /** -* Reads one or more bytes into a buffer -* \param[in] the command indicating the address to read -* \param[out] buffer destination buffer -* \param[in] len number of bytes which should be read -* \return 0 if operation was successful -* \return -1 if error during I2C transfer -*/ -int32_t PIOS_MS5611_Read(uint8_t address, uint8_t * buffer, uint8_t len) + * Reads one or more bytes into a buffer + * \param[in] the command indicating the address to read + * \param[out] buffer destination buffer + * \param[in] len number of bytes which should be read + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + */ +int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len) { + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = MS5611_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = 1, + .buf = &address, + } + , + { + .info = __func__, + .addr = MS5611_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = MS5611_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = 1, - .buf = &address, - } - , - { - .info = __func__, - .addr = MS5611_I2C_ADDR, - .rw = PIOS_I2C_TXN_READ, - .len = len, - .buf = buffer, - } - }; - - return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); } /** -* Writes one or more bytes to the MS5611 -* \param[in] address Register address -* \param[in] buffer source buffer -* \return 0 if operation was successful -* \return -1 if error during I2C transfer -*/ + * Writes one or more bytes to the MS5611 + * \param[in] address Register address + * \param[in] buffer source buffer + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + */ int32_t PIOS_MS5611_WriteCommand(uint8_t command) { - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = MS5611_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = 1, - .buf = &command, - } - , - }; + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = MS5611_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = 1, + .buf = &command, + } + , + }; - return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); } /** -* @brief Run self-test operation. -* \return 0 if self-test succeed, -1 if failed -*/ + * @brief Run self-test operation. + * \return 0 if self-test succeed, -1 if failed + */ int32_t PIOS_MS5611_Test() { - // TODO: Is there a better way to test this than just checking that pressure/temperature has changed? - int32_t cur_value = 0; + // TODO: Is there a better way to test this than just checking that pressure/temperature has changed? + int32_t cur_value = 0; - cur_value = Temperature; - PIOS_MS5611_StartADC(TemperatureConv); - PIOS_DELAY_WaitmS(5); - PIOS_MS5611_ReadADC(); - if (cur_value == Temperature) - return -1; + cur_value = Temperature; + PIOS_MS5611_StartADC(TemperatureConv); + PIOS_DELAY_WaitmS(5); + PIOS_MS5611_ReadADC(); + if (cur_value == Temperature) { + return -1; + } - cur_value=Pressure; - PIOS_MS5611_StartADC(PressureConv); - PIOS_DELAY_WaitmS(26); - PIOS_MS5611_ReadADC(); - if (cur_value == Pressure) - return -1; - - return 0; + cur_value = Pressure; + PIOS_MS5611_StartADC(PressureConv); + PIOS_DELAY_WaitmS(26); + PIOS_MS5611_ReadADC(); + if (cur_value == Pressure) { + return -1; + } + + return 0; } #endif /* PIOS_INCLUDE_MS5611 */ diff --git a/flight/pios/common/pios_rcvr.c b/flight/pios/common/pios_rcvr.c index 4533d874b..5e2e856da 100644 --- a/flight/pios/common/pios_rcvr.c +++ b/flight/pios/common/pios_rcvr.c @@ -5,74 +5,78 @@ #include enum pios_rcvr_dev_magic { - PIOS_RCVR_DEV_MAGIC = 0x99aabbcc, + PIOS_RCVR_DEV_MAGIC = 0x99aabbcc, }; struct pios_rcvr_dev { - enum pios_rcvr_dev_magic magic; - uint32_t lower_id; - const struct pios_rcvr_driver * driver; + enum pios_rcvr_dev_magic magic; + uint32_t lower_id; + const struct pios_rcvr_driver *driver; }; -static bool PIOS_RCVR_validate(struct pios_rcvr_dev * rcvr_dev) +static bool PIOS_RCVR_validate(struct pios_rcvr_dev *rcvr_dev) { - return (rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC); + return rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) +static struct pios_rcvr_dev *PIOS_RCVR_alloc(void) { - struct pios_rcvr_dev * rcvr_dev; + struct pios_rcvr_dev *rcvr_dev; - rcvr_dev = (struct pios_rcvr_dev *)pvPortMalloc(sizeof(*rcvr_dev)); - if (!rcvr_dev) return (NULL); + rcvr_dev = (struct pios_rcvr_dev *)pvPortMalloc(sizeof(*rcvr_dev)); + if (!rcvr_dev) { + return NULL; + } - rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; - return(rcvr_dev); + rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; + return rcvr_dev; } #else static struct pios_rcvr_dev pios_rcvr_devs[PIOS_RCVR_MAX_DEVS]; static uint8_t pios_rcvr_num_devs; -static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) +static struct pios_rcvr_dev *PIOS_RCVR_alloc(void) { - struct pios_rcvr_dev * rcvr_dev; + struct pios_rcvr_dev *rcvr_dev; - if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) { - return (NULL); - } + if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) { + return NULL; + } - rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++]; - rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; + rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++]; + rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; - return (rcvr_dev); + return rcvr_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /** - * Initialises RCVR layer - * \param[out] handle - * \param[in] driver - * \param[in] id - * \return < 0 if initialisation failed - */ -int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, uint32_t lower_id) + * Initialises RCVR layer + * \param[out] handle + * \param[in] driver + * \param[in] id + * \return < 0 if initialisation failed + */ +int32_t PIOS_RCVR_Init(uint32_t *rcvr_id, const struct pios_rcvr_driver *driver, uint32_t lower_id) { - PIOS_DEBUG_Assert(rcvr_id); - PIOS_DEBUG_Assert(driver); + PIOS_DEBUG_Assert(rcvr_id); + PIOS_DEBUG_Assert(driver); - struct pios_rcvr_dev * rcvr_dev; + struct pios_rcvr_dev *rcvr_dev; - rcvr_dev = (struct pios_rcvr_dev *) PIOS_RCVR_alloc(); - if (!rcvr_dev) goto out_fail; + rcvr_dev = (struct pios_rcvr_dev *)PIOS_RCVR_alloc(); + if (!rcvr_dev) { + goto out_fail; + } - rcvr_dev->driver = driver; - rcvr_dev->lower_id = lower_id; + rcvr_dev->driver = driver; + rcvr_dev->lower_id = lower_id; - *rcvr_id = (uint32_t)rcvr_dev; - return(0); + *rcvr_id = (uint32_t)rcvr_dev; + return 0; out_fail: - return(-1); + return -1; } /** @@ -86,25 +90,27 @@ out_fail: */ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) { - // Publicly facing API uses channel 1 for first channel - if(channel == 0) - return PIOS_RCVR_INVALID; - else - channel--; + // Publicly facing API uses channel 1 for first channel + if (channel == 0) { + return PIOS_RCVR_INVALID; + } else { + channel--; + } - if (rcvr_id == 0) - return PIOS_RCVR_NODRIVER; + if (rcvr_id == 0) { + return PIOS_RCVR_NODRIVER; + } - struct pios_rcvr_dev * rcvr_dev = (struct pios_rcvr_dev *)rcvr_id; + struct pios_rcvr_dev *rcvr_dev = (struct pios_rcvr_dev *)rcvr_id; - if (!PIOS_RCVR_validate(rcvr_dev)) { - /* Undefined RCVR port for this board (see pios_board.c) */ - PIOS_Assert(0); - } + if (!PIOS_RCVR_validate(rcvr_dev)) { + /* Undefined RCVR port for this board (see pios_board.c) */ + PIOS_Assert(0); + } - PIOS_DEBUG_Assert(rcvr_dev->driver->read); + PIOS_DEBUG_Assert(rcvr_dev->driver->read); - return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); + return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); } #endif /* PIOS_INCLUDE_RCVR */ diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index a3542d946..8282ce9d7 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -1,30 +1,30 @@ /** -****************************************************************************** -* @addtogroup PIOS PIOS Core hardware abstraction layer -* @{ -* @addtogroup PIOS_RFM22B Radio Functions -* @brief PIOS interface for for the RFM22B radio -* @{ -* -* @file pios_rfm22b.c -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief Implements a driver the the RFM22B driver -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RFM22B Radio Functions + * @brief PIOS interface for for the RFM22B radio + * @{ + * + * @file pios_rfm22b.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Implements a driver the the RFM22B driver + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -60,55 +60,55 @@ #include /* Local Defines */ -#define STACK_SIZE_BYTES 200 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) -#define ISR_TIMEOUT 2 // ms -#define EVENT_QUEUE_SIZE 5 -#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600 -#define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_0 -#define RFM22B_LINK_QUALITY_THRESHOLD 20 +#define STACK_SIZE_BYTES 200 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) +#define ISR_TIMEOUT 2 // ms +#define EVENT_QUEUE_SIZE 5 +#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600 +#define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_0 +#define RFM22B_LINK_QUALITY_THRESHOLD 20 #define RFM22B_NOMINAL_CARRIER_FREQUENCY 430000000 -#define RFM22B_MAXIMUM_FREQUENCY 440000000 -#define RFM22B_DEFAULT_FREQUENCY 433000000 -#define RFM22B_FREQUENCY_HOP_STEP_SIZE 75000 +#define RFM22B_MAXIMUM_FREQUENCY 440000000 +#define RFM22B_DEFAULT_FREQUENCY 433000000 +#define RFM22B_FREQUENCY_HOP_STEP_SIZE 75000 // The maximum amount of time since the last message received to consider the connection broken. -#define DISCONNECT_TIMEOUT_MS 1000 // ms +#define DISCONNECT_TIMEOUT_MS 1000 // ms // The maximum amount of time without activity before initiating a reset. -#define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms +#define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms // The time between updates for sending stats the radio link. -#define RADIOSTATS_UPDATE_PERIOD_MS 250 +#define RADIOSTATS_UPDATE_PERIOD_MS 250 // The number of stats updates that a modem can miss before it's considered disconnected -#define MAX_RADIOSTATS_MISS_COUNT 3 +#define MAX_RADIOSTATS_MISS_COUNT 3 // The time between PPM updates -#define PPM_UPDATE_PERIOD_MS 20 +#define PPM_UPDATE_PERIOD_MS 20 // this is too adjust the RF module so that it is on frequency -#define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default +#define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default -#define TX_PREAMBLE_NIBBLES 12 // 7 to 511 (number of nibbles) -#define RX_PREAMBLE_NIBBLES 6 // 5 to 31 (number of nibbles) +#define TX_PREAMBLE_NIBBLES 12 // 7 to 511 (number of nibbles) +#define RX_PREAMBLE_NIBBLES 6 // 5 to 31 (number of nibbles) // the size of the rf modules internal FIFO buffers -#define FIFO_SIZE 64 +#define FIFO_SIZE 64 -#define TX_FIFO_HI_WATERMARK 62 // 0-63 -#define TX_FIFO_LO_WATERMARK 32 // 0-63 +#define TX_FIFO_HI_WATERMARK 62 // 0-63 +#define TX_FIFO_LO_WATERMARK 32 // 0-63 -#define RX_FIFO_HI_WATERMARK 32 // 0-63 +#define RX_FIFO_HI_WATERMARK 32 // 0-63 // preamble byte (preceeds SYNC_BYTE's) -#define PREAMBLE_BYTE 0x55 +#define PREAMBLE_BYTE 0x55 // RF sync bytes (32-bit in all) -#define SYNC_BYTE_1 0x2D -#define SYNC_BYTE_2 0xD4 -#define SYNC_BYTE_3 0x4B -#define SYNC_BYTE_4 0x59 +#define SYNC_BYTE_1 0x2D +#define SYNC_BYTE_2 0xD4 +#define SYNC_BYTE_3 0x4B +#define SYNC_BYTE_4 0x59 #ifndef RX_LED_ON #define RX_LED_ON @@ -124,40 +124,42 @@ /* Local type definitions */ struct pios_rfm22b_transition { - enum pios_radio_event (*entry_fn) (struct pios_rfm22b_dev *rfm22b_dev); + enum pios_radio_event (*entry_fn)(struct pios_rfm22b_dev *rfm22b_dev); enum pios_radio_state next_state[RADIO_EVENT_NUM_EVENTS]; }; // Must ensure these prefilled arrays match the define sizes static const uint8_t FULL_PREAMBLE[FIFO_SIZE] = { - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE}; // 64 bytes -static const uint8_t HEADER[(TX_PREAMBLE_NIBBLES + 1)/2 + 2] = {PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,PREAMBLE_BYTE, PREAMBLE_BYTE, SYNC_BYTE_1, SYNC_BYTE_2}; + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE +}; // 64 bytes +static const uint8_t HEADER[(TX_PREAMBLE_NIBBLES + 1) / 2 + 2] = { PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, SYNC_BYTE_1, SYNC_BYTE_2 }; static const uint8_t OUT_FF[64] = { - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}; + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF +}; /* Local function forwared declarations */ static void pios_rfm22_task(void *parameters); static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev); -static void pios_rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening); +static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening); static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev); static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event, bool inISR); static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev); @@ -209,221 +211,219 @@ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr); /* The state transition table */ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_STATES] = { - // Initialization thread - [RADIO_STATE_UNINITIALIZED] = { - .entry_fn = 0, - .next_state = { + [RADIO_STATE_UNINITIALIZED] = { + .entry_fn = 0, + .next_state = { [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, }, }, - [RADIO_STATE_INITIALIZING] = { - .entry_fn = rfm22_init, - .next_state = { + [RADIO_STATE_INITIALIZING] = { + .entry_fn = rfm22_init, + .next_state = { [RADIO_EVENT_INITIALIZED] = RADIO_STATE_RX_MODE, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, [RADIO_STATE_REQUESTING_CONNECTION] = { - .entry_fn = rfm22_requestConnection, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + .entry_fn = rfm22_requestConnection, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR }, }, - [RADIO_STATE_ACCEPTING_CONNECTION] = { - .entry_fn = rfm22_acceptConnection, - .next_state = { - [RADIO_EVENT_DEFAULT] = RADIO_STATE_SENDING_ACK, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_ACCEPTING_CONNECTION] = { + .entry_fn = rfm22_acceptConnection, + .next_state = { + [RADIO_EVENT_DEFAULT] = RADIO_STATE_SENDING_ACK, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_RX_MODE] = { - .entry_fn = radio_setRxMode, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK, + [RADIO_STATE_RX_MODE] = { + .entry_fn = radio_setRxMode, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK, [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA, - [RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_RX_DATA] = { - .entry_fn = radio_rxData, - .next_state = { - [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA, - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_REQUEST_CONNECTION] = RADIO_STATE_REQUESTING_CONNECTION, - [RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK, - [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_SENDING_ACK, + [RADIO_STATE_RX_DATA] = { + .entry_fn = radio_rxData, + .next_state = { + [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA, + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_REQUEST_CONNECTION] = RADIO_STATE_REQUESTING_CONNECTION, + [RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK, + [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_SENDING_ACK, [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_STATUS_RECEIVED] = RADIO_STATE_RECEIVING_STATUS, + [RADIO_EVENT_STATUS_RECEIVED] = RADIO_STATE_RECEIVING_STATUS, [RADIO_EVENT_CONNECTION_REQUESTED] = RADIO_STATE_ACCEPTING_CONNECTION, - [RADIO_EVENT_PACKET_ACKED] = RADIO_STATE_RECEIVING_ACK, - [RADIO_EVENT_PACKET_NACKED] = RADIO_STATE_RECEIVING_NACK, - [RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_PACKET_ACKED] = RADIO_STATE_RECEIVING_ACK, + [RADIO_EVENT_PACKET_NACKED] = RADIO_STATE_RECEIVING_NACK, + [RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_RECEIVING_ACK] = { - .entry_fn = rfm22_receiveAck, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_RECEIVING_ACK] = { + .entry_fn = rfm22_receiveAck, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_RECEIVING_NACK] = { - .entry_fn = rfm22_receiveNack, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_RECEIVING_NACK] = { + .entry_fn = rfm22_receiveNack, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_RECEIVING_STATUS] = { - .entry_fn = rfm22_receiveStatus, - .next_state = { + [RADIO_STATE_RECEIVING_STATUS] = { + .entry_fn = rfm22_receiveStatus, + .next_state = { [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_TX_START] = { - .entry_fn = radio_txStart, - .next_state = { + [RADIO_STATE_TX_START] = { + .entry_fn = radio_txStart, + .next_state = { [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA, - [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_TX_DATA] = { - .entry_fn = radio_txData, - .next_state = { + [RADIO_STATE_TX_DATA] = { + .entry_fn = radio_txData, + .next_state = { [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA, - [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_FAILURE] = RADIO_STATE_TX_FAILURE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, + [RADIO_EVENT_FAILURE] = RADIO_STATE_TX_FAILURE, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_TX_FAILURE] = { - .entry_fn = rfm22_txFailure, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_TX_FAILURE] = { + .entry_fn = rfm22_txFailure, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_SENDING_ACK] = { - .entry_fn = rfm22_sendAck, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_SENDING_ACK] = { + .entry_fn = rfm22_sendAck, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_SENDING_NACK] = { - .entry_fn = rfm22_sendNack, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_SENDING_NACK] = { + .entry_fn = rfm22_sendNack, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_TIMEOUT] = { - .entry_fn = rfm22_timeout, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_TIMEOUT] = { + .entry_fn = rfm22_timeout, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_ERROR] = { - .entry_fn = rfm22_error, - .next_state = { - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_ERROR] = { + .entry_fn = rfm22_error, + .next_state = { + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_FATAL_ERROR] = { - .entry_fn = rfm22_fatal_error, - .next_state = { - }, + [RADIO_STATE_FATAL_ERROR] = { + .entry_fn = rfm22_fatal_error, + .next_state = {}, }, }; // xtal 10 ppm, 434MHz -static const uint32_t data_rate[] = { 500, 1000, 2000, 4000, 8000, 9600, 16000, 19200, 24000, 32000, 57600, 64000, 128000, 192000, 256000}; -static const uint8_t modulation_index[] = { 16, 8, 4, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; +static const uint32_t data_rate[] = { 500, 1000, 2000, 4000, 8000, 9600, 16000, 19200, 24000, 32000, 57600, 64000, 128000, 192000, 256000 }; +static const uint8_t modulation_index[] = { 16, 8, 4, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }; -static const uint8_t reg_1C[] = { 0x37, 0x37, 0x37, 0x37, 0x3A, 0x3B, 0x26, 0x28, 0x2E, 0x16, 0x06, 0x07, 0x83, 0x8A, 0x8C}; // rfm22_if_filter_bandwidth +static const uint8_t reg_1C[] = { 0x37, 0x37, 0x37, 0x37, 0x3A, 0x3B, 0x26, 0x28, 0x2E, 0x16, 0x06, 0x07, 0x83, 0x8A, 0x8C }; // rfm22_if_filter_bandwidth -static const uint8_t reg_1D[] = { 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44}; // rfm22_afc_loop_gearshift_override -static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02}; // rfm22_afc_timing_control +static const uint8_t reg_1D[] = { 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44 }; // rfm22_afc_loop_gearshift_override +static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02 }; // rfm22_afc_timing_control -static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03}; // rfm22_clk_recovery_gearshift_override -static const uint8_t reg_20[] = { 0xE8, 0xF4, 0xFA, 0x70, 0x3F, 0x34, 0x3F, 0x34, 0x2A, 0x3F, 0x45, 0x3F, 0x5E, 0x3F, 0x2F}; // rfm22_clk_recovery_oversampling_ratio -static const uint8_t reg_21[] = { 0x60, 0x20, 0x00, 0x01, 0x02, 0x02, 0x02, 0x02, 0x03, 0x02, 0x01, 0x02, 0x01, 0x02, 0x02}; // rfm22_clk_recovery_offset2 -static const uint8_t reg_22[] = { 0x20, 0x41, 0x83, 0x06, 0x0C, 0x75, 0x0C, 0x75, 0x12, 0x0C, 0xD7, 0x0c, 0x5D, 0x0C, 0xBB}; // rfm22_clk_recovery_offset1 -static const uint8_t reg_23[] = { 0xC5, 0x89, 0x12, 0x25, 0x4A, 0x25, 0x4A, 0x25, 0x6F, 0x4A, 0xDC, 0x4A, 0x86, 0x4A, 0x0D}; // rfm22_clk_recovery_offset0 -static const uint8_t reg_24[] = { 0x00, 0x00, 0x00, 0x02, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07}; // rfm22_clk_recovery_timing_loop_gain1 -static const uint8_t reg_25[] = { 0x0A, 0x23, 0x85, 0x0E, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x6E, 0xFF, 0x74, 0xFF, 0xFF}; // rfm22_clk_recovery_timing_loop_gain0 +static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 }; // rfm22_clk_recovery_gearshift_override +static const uint8_t reg_20[] = { 0xE8, 0xF4, 0xFA, 0x70, 0x3F, 0x34, 0x3F, 0x34, 0x2A, 0x3F, 0x45, 0x3F, 0x5E, 0x3F, 0x2F }; // rfm22_clk_recovery_oversampling_ratio +static const uint8_t reg_21[] = { 0x60, 0x20, 0x00, 0x01, 0x02, 0x02, 0x02, 0x02, 0x03, 0x02, 0x01, 0x02, 0x01, 0x02, 0x02 }; // rfm22_clk_recovery_offset2 +static const uint8_t reg_22[] = { 0x20, 0x41, 0x83, 0x06, 0x0C, 0x75, 0x0C, 0x75, 0x12, 0x0C, 0xD7, 0x0c, 0x5D, 0x0C, 0xBB }; // rfm22_clk_recovery_offset1 +static const uint8_t reg_23[] = { 0xC5, 0x89, 0x12, 0x25, 0x4A, 0x25, 0x4A, 0x25, 0x6F, 0x4A, 0xDC, 0x4A, 0x86, 0x4A, 0x0D }; // rfm22_clk_recovery_offset0 +static const uint8_t reg_24[] = { 0x00, 0x00, 0x00, 0x02, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07 }; // rfm22_clk_recovery_timing_loop_gain1 +static const uint8_t reg_25[] = { 0x0A, 0x23, 0x85, 0x0E, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x6E, 0xFF, 0x74, 0xFF, 0xFF }; // rfm22_clk_recovery_timing_loop_gain0 -static const uint8_t reg_2A[] = { 0x0E, 0x0E, 0x0E, 0x0E, 0x0E, 0x0D, 0x0D, 0x0E, 0x12, 0x17, 0x2D, 0x31, 0x50, 0x50, 0x50}; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz +static const uint8_t reg_2A[] = { 0x0E, 0x0E, 0x0E, 0x0E, 0x0E, 0x0D, 0x0D, 0x0E, 0x12, 0x17, 0x2D, 0x31, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz -static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80}; // rfm22_cpcuu -static const uint8_t reg_69[] = { 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x60, 0x20, 0x20, 0x20, 0x20}; // rfm22_agc_override1 -static const uint8_t reg_6E[] = { 0x04, 0x08, 0x10, 0x20, 0x41, 0x4E, 0x83, 0x9D, 0xC4, 0x08, 0x0E, 0x10, 0x20, 0x31, 0x41}; // rfm22_tx_data_rate1 -static const uint8_t reg_6F[] = { 0x19, 0x31, 0x62, 0xC5, 0x89, 0xA5, 0x12, 0x49, 0x9C, 0x31, 0xBF, 0x62, 0xC5, 0x27, 0x89}; // rfm22_tx_data_rate0 +static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80 }; // rfm22_cpcuu +static const uint8_t reg_69[] = { 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x60, 0x20, 0x20, 0x20, 0x20 }; // rfm22_agc_override1 +static const uint8_t reg_6E[] = { 0x04, 0x08, 0x10, 0x20, 0x41, 0x4E, 0x83, 0x9D, 0xC4, 0x08, 0x0E, 0x10, 0x20, 0x31, 0x41 }; // rfm22_tx_data_rate1 +static const uint8_t reg_6F[] = { 0x19, 0x31, 0x62, 0xC5, 0x89, 0xA5, 0x12, 0x49, 0x9C, 0x31, 0xBF, 0x62, 0xC5, 0x27, 0x89 }; // rfm22_tx_data_rate0 -static const uint8_t reg_70[] = { 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x0D, 0x0C, 0x0C, 0x0D, 0x0D, 0x0D}; // rfm22_modulation_mode_control1 -static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23}; // rfm22_modulation_mode_control2 +static const uint8_t reg_70[] = { 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x0D, 0x0C, 0x0C, 0x0D, 0x0D, 0x0D }; // rfm22_modulation_mode_control1 +static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23 }; // rfm22_modulation_mode_control2 -static const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x06, 0x08, 0x0D, 0x0F, 0x13, 0x1A, 0x2E, 0x33, 0x66, 0x9A, 0xCD}; // rfm22_frequency_deviation +static const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x06, 0x08, 0x0D, 0x0F, 0x13, 0x1A, 0x2E, 0x33, 0x66, 0x9A, 0xCD }; // rfm22_frequency_deviation -static struct pios_rfm22b_dev * g_rfm22b_dev = NULL; +static struct pios_rfm22b_dev *g_rfm22b_dev = NULL; /***************************************************************************** - * External Interface Functions - *****************************************************************************/ +* External Interface Functions +*****************************************************************************/ /** * Initialise an RFM22B device @@ -441,40 +441,40 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu // Allocate the device structure. struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)pios_rfm22_alloc(); if (!rfm22b_dev) { - return(-1); + return -1; } - *rfm22b_id = (uint32_t)rfm22b_dev; + *rfm22b_id = (uint32_t)rfm22b_dev; g_rfm22b_dev = rfm22b_dev; // Store the SPI handle - rfm22b_dev->slave_num = slave_num; - rfm22b_dev->spi_id = spi_id; + rfm22b_dev->slave_num = slave_num; + rfm22b_dev->spi_id = spi_id; // Initialize our configuration parameters - rfm22b_dev->send_ppm = false; - rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; - rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; + rfm22b_dev->send_ppm = false; + rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; + rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; // Initialize the com callbacks. rfm22b_dev->com_config_cb = NULL; - rfm22b_dev->rx_in_cb = NULL; - rfm22b_dev->tx_out_cb = NULL; + rfm22b_dev->rx_in_cb = NULL; + rfm22b_dev->tx_out_cb = NULL; // Initialize the stats. rfm22b_dev->stats.packets_per_sec = 0; rfm22b_dev->stats.rx_good = 0; - rfm22b_dev->stats.rx_corrected = 0; - rfm22b_dev->stats.rx_error = 0; - rfm22b_dev->stats.rx_missed = 0; - rfm22b_dev->stats.tx_dropped = 0; - rfm22b_dev->stats.tx_resent = 0; - rfm22b_dev->stats.resets = 0; - rfm22b_dev->stats.timeouts = 0; + rfm22b_dev->stats.rx_corrected = 0; + rfm22b_dev->stats.rx_error = 0; + rfm22b_dev->stats.rx_missed = 0; + rfm22b_dev->stats.tx_dropped = 0; + rfm22b_dev->stats.tx_resent = 0; + rfm22b_dev->stats.resets = 0; + rfm22b_dev->stats.timeouts = 0; rfm22b_dev->stats.link_quality = 0; rfm22b_dev->stats.rssi = 0; - rfm22b_dev->stats.tx_seq = 0; - rfm22b_dev->stats.rx_seq = 0; - rfm22b_dev->stats.tx_failure = 0; + rfm22b_dev->stats.tx_seq = 0; + rfm22b_dev->stats.rx_seq = 0; + rfm22b_dev->stats.tx_failure = 0; // Initialize the frequencies. PIOS_RFM22B_SetInitialFrequency(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY); @@ -487,13 +487,13 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->coordinator = false; // Create the event queue - rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event)); + rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event)); // Bind the configuration to the device instance rfm22b_dev->cfg = *cfg; // Create a semaphore to know if an ISR needs responding to - vSemaphoreCreateBinary( rfm22b_dev->isrPending ); + vSemaphoreCreateBinary(rfm22b_dev->isrPending); // Create our (hopefully) unique 32 bit id from the processor serial number. uint8_t crcs[] = { 0, 0, 0, 0 }; @@ -501,8 +501,9 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu char serial_no_str[33]; PIOS_SYS_SerialNumberGet(serial_no_str); // Create a 32 bit value using 4 8 bit CRC values. - for (uint8_t i = 0; serial_no_str[i] != 0; ++i) + for (uint8_t i = 0; serial_no_str[i] != 0; ++i) { crcs[i % 4] = PIOS_CRC_updateByte(crcs[i % 4], serial_no_str[i]); + } } rfm22b_dev->deviceID = crcs[0] | crcs[1] << 8 | crcs[2] << 16 | crcs[3] << 24; DEBUG_PRINTF(2, "RF device ID: %x\n\r", rfm22b_dev->deviceID); @@ -530,9 +531,9 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu pios_rfm22_inject_event(rfm22b_dev, RADIO_EVENT_INITIALIZE, false); // Start the driver task. This task controls the radio state machine and removed all of the IO from the IRQ handler. - xTaskCreate(pios_rfm22_task, (signed char *)"PIOS_RFM22B_Task", STACK_SIZE_BYTES, (void*)rfm22b_dev, TASK_PRIORITY, &(rfm22b_dev->taskHandle)); + xTaskCreate(pios_rfm22_task, (signed char *)"PIOS_RFM22B_Task", STACK_SIZE_BYTES, (void *)rfm22b_dev, TASK_PRIORITY, &(rfm22b_dev->taskHandle)); - return(0); + return 0; } /** @@ -543,6 +544,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu void PIOS_RFM22B_Reinit(uint32_t rfm22b_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (PIOS_RFM22B_Validate(rfm22b_dev)) { pios_rfm22_inject_event(rfm22b_dev, RADIO_EVENT_INITIALIZE, false); } @@ -571,6 +573,7 @@ bool PIOS_RFM22_EXT_Int(void) uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (PIOS_RFM22B_Validate(rfm22b_dev)) { return rfm22b_dev->deviceID; } @@ -586,6 +589,7 @@ uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id) bool PIOS_RFM22B_IsCoordinator(uint32_t rfm22b_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (PIOS_RFM22B_Validate(rfm22b_dev)) { return rfm22b_dev->coordinator; } @@ -601,8 +605,9 @@ bool PIOS_RFM22B_IsCoordinator(uint32_t rfm22b_id) bool PIOS_RFM22B_InRxWait(uint32_t rfm22b_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (PIOS_RFM22B_Validate(rfm22b_dev)) { - return ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_TRANSITION)); + return (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_TRANSITION); } return false; } @@ -616,6 +621,7 @@ bool PIOS_RFM22B_InRxWait(uint32_t rfm22b_id) void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } @@ -633,11 +639,12 @@ void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr) void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_freq, uint32_t max_freq, uint32_t step_size) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } - rfm22b_dev->con_packet.min_frequency = min_freq; - rfm22b_dev->con_packet.max_frequency = max_freq; + rfm22b_dev->con_packet.min_frequency = min_freq; + rfm22b_dev->con_packet.max_frequency = max_freq; rfm22b_dev->con_packet.channel_spacing = step_size; } @@ -650,6 +657,7 @@ void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_freq, uint32 void PIOS_RFM22B_SetInitialFrequency(uint32_t rfm22b_id, uint32_t init_freq) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } @@ -665,7 +673,8 @@ void PIOS_RFM22B_SetInitialFrequency(uint32_t rfm22b_id, uint32_t init_freq) void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigCallback cb) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_Validate(rfm22b_dev)) { + + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } rfm22b_dev->com_config_cb = cb; @@ -682,21 +691,22 @@ void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigC * @param[in] comSpeeds The array of com port speeds. */ void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindingPairIDs[], const uint8_t mainPortSettings[], - const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]) + const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_Validate(rfm22b_dev)) { + + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } // This modem will be considered a coordinator if any bindings have been set. rfm22b_dev->coordinator = false; for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { - rfm22b_dev->bindings[i].pairID = bindingPairIDs[i]; - rfm22b_dev->bindings[i].main_port = mainPortSettings[i]; + rfm22b_dev->bindings[i].pairID = bindingPairIDs[i]; + rfm22b_dev->bindings[i].main_port = mainPortSettings[i]; rfm22b_dev->bindings[i].flexi_port = flexiPortSettings[i]; - rfm22b_dev->bindings[i].vcp_port = vcpPortSettings[i]; - rfm22b_dev->bindings[i].com_speed = comSpeeds[i]; + rfm22b_dev->bindings[i].vcp_port = vcpPortSettings[i]; + rfm22b_dev->bindings[i].com_speed = comSpeeds[i]; rfm22b_dev->coordinator |= (rfm22b_dev->bindings[i].pairID != 0); } } @@ -707,9 +717,11 @@ void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindingPairIDs[] * @param[in] rfm22b_id The RFM22B device index. * @param[out] stats The stats are returned in this structure */ -void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) { +void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) +{ struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_Validate(rfm22b_dev)) { + + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } @@ -741,6 +753,7 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) { uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return 0; } @@ -763,10 +776,11 @@ uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_Validate(rfm22b_dev)) { + + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return false; } - return (rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD)); + return rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD); } /** @@ -776,8 +790,10 @@ bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id) * @param[in] p The packet to receive into. * @return true if Rx mode was entered sucessfully. */ -bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) { +bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) +{ struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return false; } @@ -808,7 +824,7 @@ bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) { rfm22b_dev->rx_buffer_wr = 0; // Clear the TX buffer. - rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; + rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; // clear FIFOs rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx); @@ -816,9 +832,9 @@ bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) { // enable RX interrupts rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_encrcerror | RFM22_ie1_enpkvalid | - RFM22_ie1_enrxffafull | RFM22_ie1_enfferr); + RFM22_ie1_enrxffafull | RFM22_ie1_enfferr); rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, RFM22_ie2_enpreainval | RFM22_ie2_enpreaval | - RFM22_ie2_enswdet); + RFM22_ie2_enswdet); // enable the receiver rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon); @@ -839,8 +855,10 @@ bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) { * @param[in] p The packet to transmit. * @return true if there if the packet was queued for transmission. */ -bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) { +bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) +{ struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return false; } @@ -871,7 +889,7 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) { // Set the destination address in the transmit header. // The destination address is the first 4 bytes of the message. - uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet); + uint8_t *tx_buffer = (uint8_t *)(rfm22b_dev->tx_packet); rfm22_write(rfm22b_dev, RFM22_transmit_header0, tx_buffer[0]); rfm22_write(rfm22b_dev, RFM22_transmit_header1, tx_buffer[1]); rfm22_write(rfm22b_dev, RFM22_transmit_header2, tx_buffer[2]); @@ -892,7 +910,7 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) { rfm22_assertCs(rfm22b_dev); PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80); int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd); - bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE: bytes_to_write; + bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE : bytes_to_write; PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL); rfm22b_dev->tx_data_rd += bytes_to_write; rfm22_deassertCs(rfm22b_dev); @@ -920,8 +938,10 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) { * @param[in] rfm22b_id The rfm22b device. * @return PIOS_RFM22B_TX_COMPLETE on completed Tx, or PIOS_RFM22B_INT_SUCCESS/PIOS_RFM22B_INT_FAILURE. */ -pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) { +pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) +{ struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return PIOS_RFM22B_INT_FAILURE; } @@ -934,24 +954,21 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) { // TX FIFO almost empty, it needs filling up if (rfm22b_dev->status_regs.int_status_1.tx_fifo_almost_empty) { - // Add data to the TX FIFO buffer - uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet); + uint8_t *tx_buffer = (uint8_t *)(rfm22b_dev->tx_packet); uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1; rfm22_claimBus(rfm22b_dev); rfm22_assertCs(rfm22b_dev); PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80); int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd); - bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes: bytes_to_write; + bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes : bytes_to_write; PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL); rfm22b_dev->tx_data_rd += bytes_to_write; rfm22_deassertCs(rfm22b_dev); rfm22_releaseBus(rfm22b_dev); return PIOS_RFM22B_INT_SUCCESS; - } else if (rfm22b_dev->status_regs.int_status_1.packet_sent_interrupt) { - // Transition out of Tx mode. rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION; @@ -967,12 +984,14 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) { * @param[in] rfm22b_id The rfm22b device. * @return PIOS_RFM22B_RX_COMPLETE on completed Rx, or PIOS_RFM22B_INT_SUCCESS/PIOS_RFM22B_INT_FAILURE. */ -pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { +pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) +{ struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return PIOS_RFM22B_INT_FAILURE; } - uint8_t *rx_buffer = (uint8_t*)(rfm22b_dev->rx_packet_handle); + uint8_t *rx_buffer = (uint8_t *)(rfm22b_dev->rx_packet_handle); pios_rfm22b_int_result ret = PIOS_RFM22B_INT_SUCCESS; // Read the device status registers @@ -990,7 +1009,6 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { // Valid packet received if (rfm22b_dev->status_regs.int_status_1.valid_packet_received) { - // Claim the SPI bus. rfm22_claimBus(rfm22b_dev); @@ -1002,12 +1020,12 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr; // Fetch the data from the RX FIFO rfm22_assertCs(rfm22b_dev); - PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F); - rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], + PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access & 0x7F); + rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], bytes_to_read, NULL) == 0) ? bytes_to_read : 0; rfm22_deassertCs(rfm22b_dev); } - + // Release the SPI bus. rfm22_releaseBus(rfm22b_dev); @@ -1024,9 +1042,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION; ret = PIOS_RFM22B_RX_COMPLETE; - } else if (rfm22b_dev->status_regs.int_status_1.rx_fifo_almost_full) { - // RX FIFO almost full, it needs emptying // read data from the rf chips FIFO buffer @@ -1045,7 +1061,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { // Fetch the data from the RX FIFO rfm22_assertCs(rfm22b_dev); - PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F); + PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access & 0x7F); rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], RX_FIFO_HI_WATERMARK, NULL) == 0) ? RX_FIFO_HI_WATERMARK : 0; rfm22_deassertCs(rfm22b_dev); @@ -1055,9 +1071,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { // Make sure that we're in RX mode. rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE; - } else if (rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) { - // Valid preamble detected RX_LED_ON; #ifdef PIOS_RFM22B_DEBUG_ON_TELEM @@ -1066,9 +1080,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { // We detected the preamble, now wait for sync. rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_WAIT_SYNC; - } else if (rfm22b_dev->status_regs.int_status_2.sync_word_detected) { - // Sync word detected // Claim the SPI bus. @@ -1078,7 +1090,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { // bits 9 to 2 uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8; // bits 1 & 0 - afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0; + afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0; afc_correction >>= 6; // convert the afc value to Hz int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f); @@ -1094,9 +1106,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { // Indicate that we're in RX mode. rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE; - } else if ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) && !rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) { - // Waiting for the preamble timed out. rfm22_rxFailure(rfm22b_dev); return PIOS_RFM22B_INT_FAILURE; @@ -1106,8 +1116,9 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { if ((rfm22b_dev->packet_start_ticks == 0) && ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC))) { rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); - if (rfm22b_dev->packet_start_ticks == 0) + if (rfm22b_dev->packet_start_ticks == 0) { rfm22b_dev->packet_start_ticks = 1; + } } return ret; @@ -1120,13 +1131,13 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { */ inline bool PIOS_RFM22B_Validate(struct pios_rfm22b_dev *rfm22b_dev) { - return (rfm22b_dev != NULL && rfm22b_dev->magic == PIOS_RFM22B_DEV_MAGIC); + return rfm22b_dev != NULL && rfm22b_dev->magic == PIOS_RFM22B_DEV_MAGIC; } /***************************************************************************** - * The Device Control Thread - *****************************************************************************/ +* The Device Control Thread +*****************************************************************************/ /** * The task that controls the radio state machine. @@ -1136,29 +1147,31 @@ inline bool PIOS_RFM22B_Validate(struct pios_rfm22b_dev *rfm22b_dev) static void pios_rfm22_task(void *parameters) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)parameters; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } - portTickType lastEventTicks = xTaskGetTickCount(); + portTickType lastEventTicks = xTaskGetTickCount(); portTickType lastStatusTicks = lastEventTicks; - portTickType lastPPMTicks = lastEventTicks; + portTickType lastPPMTicks = lastEventTicks; - while(1) { + while (1) { #ifdef PIOS_WDG_RFM22B // Update the watchdog timer PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B); #endif /* PIOS_WDG_RFM22B */ // Wait for a signal indicating an external interrupt or a pending send/receive request. - if (xSemaphoreTake(rfm22b_dev->isrPending, ISR_TIMEOUT / portTICK_RATE_MS) == pdTRUE) { + if (xSemaphoreTake(rfm22b_dev->isrPending, ISR_TIMEOUT / portTICK_RATE_MS) == pdTRUE) { lastEventTicks = xTaskGetTickCount(); // Process events through the state machine. enum pios_radio_event event; while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) { if ((event == RADIO_EVENT_INT_RECEIVED) && - ((rfm22b_dev->state == RADIO_STATE_UNINITIALIZED) || (rfm22b_dev->state == RADIO_STATE_INITIALIZING))) + ((rfm22b_dev->state == RADIO_STATE_UNINITIALIZED) || (rfm22b_dev->state == RADIO_STATE_INITIALIZING))) { continue; + } rfm22_process_event(rfm22b_dev, event); } } else { @@ -1183,7 +1196,7 @@ static void pios_rfm22_task(void *parameters) } portTickType curTicks = xTaskGetTickCount(); - uint32_t last_rec_ms = (rfm22b_dev->rx_complete_ticks == 0) ? 0 : pios_rfm22_time_difference_ms(rfm22b_dev->rx_complete_ticks, curTicks); + uint32_t last_rec_ms = (rfm22b_dev->rx_complete_ticks == 0) ? 0 : pios_rfm22_time_difference_ms(rfm22b_dev->rx_complete_ticks, curTicks); // Have we been sending / receiving this packet too long? if ((rfm22b_dev->packet_start_ticks > 0) && (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) { @@ -1192,19 +1205,15 @@ static void pios_rfm22_task(void *parameters) // Has it been too long since we received a packet rfm22_process_event(rfm22b_dev, RADIO_EVENT_ERROR); } else { - // Are we waiting for an ACK? if (rfm22b_dev->prev_tx_packet) { - // Should we resend the packet? if ((pios_rfm22_time_difference_ms(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) && PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) { rfm22b_dev->tx_complete_ticks = curTicks; rfm22_process_event(rfm22b_dev, RADIO_EVENT_ACK_TIMEOUT); } - } else { - // Queue up a PPM packet if it's time. if (pios_rfm22_time_difference_ms(lastPPMTicks, curTicks) > PPM_UPDATE_PERIOD_MS) { rfm22_sendPPM(rfm22b_dev); @@ -1217,7 +1226,6 @@ static void pios_rfm22_task(void *parameters) lastStatusTicks = curTicks; } } - } // Send a packet if it's our time slice @@ -1237,8 +1245,8 @@ static void pios_rfm22_task(void *parameters) /***************************************************************************** - * The State Machine Functions - *****************************************************************************/ +* The State Machine Functions +*****************************************************************************/ /** * Inject an event into the RFM22B state machine. @@ -1252,8 +1260,9 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio if (inISR) { // Store the event. portBASE_TYPE pxHigherPriorityTaskWoken1; - if (xQueueSendFromISR(rfm22b_dev->eventQueue, &event, &pxHigherPriorityTaskWoken1) != pdTRUE) + if (xQueueSendFromISR(rfm22b_dev->eventQueue, &event, &pxHigherPriorityTaskWoken1) != pdTRUE) { return; + } // Signal the semaphore to wake up the handler thread. portBASE_TYPE pxHigherPriorityTaskWoken2; if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken2) != pdTRUE) { @@ -1263,8 +1272,9 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken2 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE)); } else { // Store the event. - if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE) + if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE) { return; + } // Signal the semaphore to wake up the handler thread. if (xSemaphoreGive(rfm22b_dev->isrPending) != pdTRUE) { // Something went fairly seriously wrong @@ -1282,7 +1292,6 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio */ static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event) { - // No event if (event >= RADIO_EVENT_NUM_EVENTS) { return RADIO_EVENT_NUM_EVENTS; @@ -1297,7 +1306,7 @@ static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_d /* * Move to the next state * - * This is done prior to calling the new state's entry function to + * This is done prior to calling the new state's entry function to * guarantee that the entry function never depends on the previous * state. This way, it cannot ever know what the previous state was. */ @@ -1321,15 +1330,15 @@ static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_d static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event) { // Process all state transitions. - while(event != RADIO_EVENT_NUM_EVENTS) { + while (event != RADIO_EVENT_NUM_EVENTS) { event = rfm22_process_state_transition(rfm22b_dev, event); } } /***************************************************************************** - * The Device Initialization / Configuration Functions - *****************************************************************************/ +* The Device Initialization / Configuration Functions +*****************************************************************************/ /** * Initialize (or re-initialize) the RFM22B radio device. @@ -1339,12 +1348,11 @@ static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_ra */ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) { - // Initialize the register values. - rfm22b_dev->status_regs.int_status_1.raw = 0; - rfm22b_dev->status_regs.int_status_2.raw = 0; + rfm22b_dev->status_regs.int_status_1.raw = 0; + rfm22b_dev->status_regs.int_status_2.raw = 0; rfm22b_dev->status_regs.device_status.raw = 0; - rfm22b_dev->status_regs.ezmac_status.raw = 0; + rfm22b_dev->status_regs.ezmac_status.raw = 0; // Clean the LEDs rfm22_clearLEDs(); @@ -1352,7 +1360,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // Initialize the detected device statistics. for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) { rfm22b_dev->pair_stats[i].pairID = 0; - rfm22b_dev->pair_stats[i].rssi = -127; + rfm22b_dev->pair_stats[i].rssi = -127; rfm22b_dev->pair_stats[i].afc_correction = 0; rfm22b_dev->pair_stats[i].lastContact = 0; } @@ -1364,37 +1372,37 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // Initialize the state rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; - rfm22b_dev->destination_id = 0xffffffff; + rfm22b_dev->destination_id = 0xffffffff; rfm22b_dev->send_status = false; rfm22b_dev->send_connection_request = false; // Initialize the packets. - rfm22b_dev->rx_packet_len = 0; - rfm22b_dev->tx_packet = NULL; + rfm22b_dev->rx_packet_len = 0; + rfm22b_dev->tx_packet = NULL; rfm22b_dev->prev_tx_packet = NULL; rfm22b_dev->data_packet.header.data_size = 0; // Initialize the devide state rfm22b_dev->rx_buffer_wr = 0; - rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; + rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; rfm22b_dev->frequency_hop_channel = 0; - rfm22b_dev->afc_correction_Hz = 0; - rfm22b_dev->packet_start_ticks = 0; - rfm22b_dev->tx_complete_ticks = 0; - rfm22b_dev->rx_complete_ticks = 0; + rfm22b_dev->afc_correction_Hz = 0; + rfm22b_dev->packet_start_ticks = 0; + rfm22b_dev->tx_complete_ticks = 0; + rfm22b_dev->rx_complete_ticks = 0; rfm22b_dev->rfm22b_state = RFM22B_STATE_INITIALIZING; // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) rfm22_write_claim(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); for (uint8_t i = 0; i < 50; ++i) { - // read the status registers pios_rfm22_readStatus(rfm22b_dev); // Is the chip ready? - if (rfm22b_dev->status_regs.int_status_2.chip_ready) + if (rfm22b_dev->status_regs.int_status_2.chip_ready) { break; + } // Wait 1ms if not. PIOS_DELAY_WaitmS(1); @@ -1415,7 +1423,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // read the RF chip ID bytes // read the device type - uint8_t device_type = rfm22_read(rfm22b_dev, RFM22_DEVICE_TYPE) & RFM22_DT_MASK; + uint8_t device_type = rfm22_read(rfm22b_dev, RFM22_DEVICE_TYPE) & RFM22_DT_MASK; // read the device version uint8_t device_version = rfm22_read(rfm22b_dev, RFM22_DEVICE_VERSION) & RFM22_DV_MASK; @@ -1463,7 +1471,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) } else { // GPIO0 = TX State (to control RF Switch) rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_rxstate); - // GPIO1 = RX State (to control RF Switch) + // GPIO1 = RX State (to control RF Switch) rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_txstate); } // GPIO2 = Clear Channel Assessment @@ -1504,14 +1512,14 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // header control - using a 4 by header with broadcast of 0xffffffff rfm22_write(rfm22b_dev, RFM22_header_control1, - RFM22_header_cntl1_bcen_0 | - RFM22_header_cntl1_bcen_1 | - RFM22_header_cntl1_bcen_2 | - RFM22_header_cntl1_bcen_3 | - RFM22_header_cntl1_hdch_0 | - RFM22_header_cntl1_hdch_1 | - RFM22_header_cntl1_hdch_2 | - RFM22_header_cntl1_hdch_3); + RFM22_header_cntl1_bcen_0 | + RFM22_header_cntl1_bcen_1 | + RFM22_header_cntl1_bcen_2 | + RFM22_header_cntl1_bcen_3 | + RFM22_header_cntl1_hdch_0 | + RFM22_header_cntl1_hdch_1 | + RFM22_header_cntl1_hdch_2 | + RFM22_header_cntl1_hdch_3); // Check all bit of all bytes of the header rfm22_write(rfm22b_dev, RFM22_header_enable0, 0xff); rfm22_write(rfm22b_dev, RFM22_header_enable1, 0xff); @@ -1525,9 +1533,9 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22_write(rfm22b_dev, RFM22_check_header3, (id >> 24) & 0xff); // 4 header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header. rfm22_write(rfm22b_dev, RFM22_header_control2, - RFM22_header_cntl2_hdlen_3210 | - RFM22_header_cntl2_synclen_3210 | - ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); + RFM22_header_cntl2_hdlen_3210 | + RFM22_header_cntl2_synclen_3210 | + ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); // sync word rfm22_write(rfm22b_dev, RFM22_sync_word3, SYNC_BYTE_1); @@ -1573,6 +1581,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening) { uint32_t datarate_bps = data_rate[datarate]; + rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5f); // Generate a pseudo-random number from 0-8 to add to the delay @@ -1620,7 +1629,7 @@ static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm2 rfm22_write(rfm22b_dev, 0x70, reg_70[datarate] & ~RFM22_mmc1_enwhite); } else { // rfm22_modulation_mode_control1 - rfm22_write(rfm22b_dev, 0x70, reg_70[datarate] | RFM22_mmc1_enwhite); + rfm22_write(rfm22b_dev, 0x70, reg_70[datarate] | RFM22_mmc1_enwhite); } // rfm22_modulation_mode_control2 @@ -1652,7 +1661,8 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz = min_frequency; // holds the hbsel (1 or 2) - uint8_t hbsel; + uint8_t hbsel; + if (frequency_hz < 480000000) { hbsel = 0; } else { @@ -1660,11 +1670,11 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, } float freq_mhz = (float)(frequency_hz) / 1000000.0f; float xtal_freq_khz = 30000.0f; - float sfreq = freq_mhz / (10.0f * (xtal_freq_khz / 30000.0f) * (1 + hbsel)); - uint32_t fb = (uint32_t)sfreq - 24 + (64 + 32 * hbsel); - uint32_t fc = (uint32_t)((sfreq - (uint32_t)sfreq) * 64000.0f); - uint8_t fch = (fc >> 8) & 0xff; - uint8_t fcl = fc & 0xff; + float sfreq = freq_mhz / (10.0f * (xtal_freq_khz / 30000.0f) * (1 + hbsel)); + uint32_t fb = (uint32_t)sfreq - 24 + (64 + 32 * hbsel); + uint32_t fc = (uint32_t)((sfreq - (uint32_t)sfreq) * 64000.0f); + uint8_t fch = (fc >> 8) & 0xff; + uint8_t fcl = fc & 0xff; // Claim the SPI bus. rfm22_claimBus(rfm22b_dev); @@ -1680,7 +1690,7 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, (uint8_t)freq_hop_step_size); // frequency hopping channel (0-255) - rfm22b_dev->frequency_step_size = 156.25f * hbsel; + rfm22b_dev->frequency_step_size = 156.25f * hbsel; // frequency hopping channel (0-255) rfm22b_dev->frequency_hop_channel = 0; @@ -1726,22 +1736,21 @@ static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t */ static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev) { - // 1. Read the interrupt statuses with burst read - rfm22_claimBus(rfm22b_dev); // Set RC and the semaphore - uint8_t write_buf[3] = {RFM22_interrupt_status1 & 0x7f, 0xFF, 0xFF}; + rfm22_claimBus(rfm22b_dev); // Set RC and the semaphore + uint8_t write_buf[3] = { RFM22_interrupt_status1 &0x7f, 0xFF, 0xFF }; uint8_t read_buf[3]; rfm22_assertCs(rfm22b_dev); PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, write_buf, read_buf, sizeof(write_buf), NULL); rfm22_deassertCs(rfm22b_dev); - rfm22b_dev->status_regs.int_status_1.raw = read_buf[1]; - rfm22b_dev->status_regs.int_status_2.raw = read_buf[2]; + rfm22b_dev->status_regs.int_status_1.raw = read_buf[1]; + rfm22b_dev->status_regs.int_status_2.raw = read_buf[2]; // Device status rfm22b_dev->status_regs.device_status.raw = rfm22_read(rfm22b_dev, RFM22_device_status); // EzMAC status - rfm22b_dev->status_regs.ezmac_status.raw = rfm22_read(rfm22b_dev, RFM22_ezmac_status); + rfm22b_dev->status_regs.ezmac_status.raw = rfm22_read(rfm22b_dev, RFM22_ezmac_status); // Release the bus rfm22_releaseBus(rfm22b_dev); @@ -1774,8 +1783,8 @@ static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev) /***************************************************************************** - * Radio Transmit and Receive functions. - *****************************************************************************/ +* Radio Transmit and Receive functions. +*****************************************************************************/ /** * Start a transmit if possible @@ -1795,9 +1804,7 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) // See if there's a packet ready to send. if (radio_dev->tx_packet) { p = radio_dev->tx_packet; - } else { - // Don't send a packet if we're waiting for an ACK if (radio_dev->prev_tx_packet) { return RADIO_EVENT_RX_MODE; @@ -1805,26 +1812,26 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) // Send a connection request? if (!p && radio_dev->send_connection_request) { - p = (PHPacketHandle)&(radio_dev->con_packet); + p = (PHPacketHandle) & (radio_dev->con_packet); radio_dev->send_connection_request = false; } #ifdef PIOS_PPM_RECEIVER // Send a PPM packet? - if (!p && radio_dev->send_ppm) { - p = (PHPacketHandle)&(radio_dev->ppm_packet); + if (!p && radio_dev->send_ppm) { + p = (PHPacketHandle) & (radio_dev->ppm_packet); radio_dev->send_ppm = false; } #endif // Send status? if (!p && radio_dev->send_status) { - p = (PHPacketHandle)&(radio_dev->status_packet); + p = (PHPacketHandle) & (radio_dev->status_packet); radio_dev->send_status = false; } // Try to get some data to send - if (!p) { + if (!p) { bool need_yield = false; p = &radio_dev->data_packet; p->header.type = PACKET_TYPE_DATA; @@ -1862,7 +1869,7 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) } // Add the error correcting code. - encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p); + encode_data((unsigned char *)p, PHPacketSize(p), (unsigned char *)p); // Transmit the packet. PIOS_RFM22B_TransmitPacket((uint32_t)radio_dev, p); @@ -1884,27 +1891,23 @@ static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *radio_dev) // Is the transmition complete if (res == PIOS_RFM22B_TX_COMPLETE) { radio_dev->stats.tx_byte_count += PH_PACKET_SIZE(radio_dev->tx_packet); - radio_dev->tx_complete_ticks = xTaskGetTickCount(); + radio_dev->tx_complete_ticks = xTaskGetTickCount(); // Is this an ACK? bool is_ack = (radio_dev->tx_packet->header.type == PACKET_TYPE_ACK); ret_event = RADIO_EVENT_RX_MODE; if (is_ack) { - // If this is an ACK for a connection request message we need to // configure this modem from the connection request message. if (radio_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) { rfm22_setConnectionParameters(radio_dev); } - } else if ((radio_dev->tx_packet->header.type != PACKET_TYPE_NACK) && (radio_dev->tx_packet->header.type != PACKET_TYPE_PPM) && (radio_dev->tx_packet->header.type != PACKET_TYPE_STATUS) && (radio_dev->tx_packet->header.type != PACKET_TYPE_PPM)) { - // We need to wait for an ACK if this packet it not an ACK, NACK, or PPM. radio_dev->prev_tx_packet = radio_dev->tx_packet; - } - radio_dev->tx_packet = 0; + radio_dev->tx_packet = 0; radio_dev->tx_data_wr = radio_dev->tx_data_rd = 0; // Start a new transaction radio_dev->packet_start_ticks = 0; @@ -1947,12 +1950,12 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d portTickType curTicks = xTaskGetTickCount(); // Attempt to correct any errors in the packet. - decode_data((unsigned char*)p, rx_len); + decode_data((unsigned char *)p, rx_len); bool good_packet = check_syndrome() == 0; bool corrected_packet = false; // We have an error. Try to correct it. - if(!good_packet && (correct_errors_erasures((unsigned char*)p, rx_len, 0, 0) != 0)) { + if (!good_packet && (correct_errors_erasures((unsigned char *)p, rx_len, 0, 0) != 0)) { // We corrected it corrected_packet = true; } @@ -1960,7 +1963,7 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d // Set the packet status if (good_packet) { rfm22b_add_rx_status(radio_dev, RADIO_GOOD_RX_PACKET); - } else if(corrected_packet) { + } else if (corrected_packet) { // We corrected the error. rfm22b_add_rx_status(radio_dev, RADIO_CORRECTED_RX_PACKET); } else { @@ -1976,7 +1979,7 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d // Send a connection request message if we're not connected, and this is a status message from a modem that we're bound to. if (radio_dev->coordinator && !rfm22_isConnected(radio_dev)) { - PHStatusPacketHandle status = (PHStatusPacketHandle)&(radio_dev->rx_packet); + PHStatusPacketHandle status = (PHStatusPacketHandle) & (radio_dev->rx_packet); uint32_t source_id = status->source_id; for (uint8_t i = 0; OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { if (radio_dev->bindings[i].pairID == source_id) { @@ -1995,8 +1998,9 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d { // Send the data to the com port bool rx_need_yield; - if (radio_dev->rx_in_cb) + if (radio_dev->rx_in_cb) { (radio_dev->rx_in_cb)(radio_dev->rx_in_context, p->data, p->header.data_size, NULL, &rx_need_yield); + } break; } case PACKET_TYPE_DUPLICATE_DATA: @@ -2017,11 +2021,11 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d #endif #if defined(PIOS_INCLUDE_RFM22B_RCVR) ppm_output = true; - radio_dev->ppm_fresh = true; + radio_dev->ppm_fresh = true; for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { radio_dev->ppm_channel[i] = ppmp->channels[i]; } -#endif +#endif #if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT) if (PIOS_PPM_OUTPUT) { ppm_output = true; @@ -2036,7 +2040,7 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) { gcsRcvr.Channel[i] = ppmp->channels[i]; } - GCSReceiverSet(&gcsRcvr); + GCSReceiverSet(&gcsRcvr); } #endif break; @@ -2047,36 +2051,32 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d uint16_t seq_num = radio_dev->rx_packet.header.seq_num; if (rfm22_isConnected(radio_dev)) { - // Adjust the clock syncronization if this is the remote modem. // The coordinator sends the time that the previous packet was finised transmitting, // which should match the time that the last packet was received. uint16_t prev_seq_num = radio_dev->stats.rx_seq; if (seq_num == (prev_seq_num + 1)) { - portTickType local_rx_time = radio_dev->rx_complete_ticks; + portTickType local_rx_time = radio_dev->rx_complete_ticks; portTickType remote_tx_time = radio_dev->rx_packet.header.prev_tx_time; radio_dev->time_delta = remote_tx_time - local_rx_time; - } else if (seq_num > prev_seq_num) { - // Add any missed packets into the stats. uint16_t missed_packets = seq_num - prev_seq_num - 1; radio_dev->stats.rx_missed += missed_packets; } - } // Update the sequence number radio_dev->stats.rx_seq = seq_num; - } else { ret_event = RADIO_EVENT_RX_COMPLETE; } // Log the time that the packet was received. radio_dev->rx_complete_ticks = curTicks; - if (radio_dev->rx_complete_ticks == 0) + if (radio_dev->rx_complete_ticks == 0) { radio_dev->rx_complete_ticks = 1; + } return ret_event; } @@ -2093,7 +2093,6 @@ static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *radio_dev) pios_rfm22b_int_result res = PIOS_RFM22B_ProcessRx((uint32_t)radio_dev); switch (res) { - case PIOS_RFM22B_RX_COMPLETE: // Receive the packet. @@ -2121,8 +2120,8 @@ static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *radio_dev) } /***************************************************************************** - * Packet Transmition Functions - *****************************************************************************/ +* Packet Transmition Functions +*****************************************************************************/ /** * Send a radio status message. @@ -2147,14 +2146,12 @@ static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) } else { rfm22b_dev->status_packet.header.destination_id = 0xffffffff; // Broadcast } - rfm22b_dev->status_packet.header.type = PACKET_TYPE_STATUS; + rfm22b_dev->status_packet.header.type = PACKET_TYPE_STATUS; rfm22b_dev->status_packet.header.data_size = PH_STATUS_DATA_SIZE(&(rfm22b_dev->status_packet)); - rfm22b_dev->status_packet.source_id = rfm22b_dev->deviceID; - rfm22b_dev->status_packet.link_quality = rfm22b_dev->stats.link_quality; - rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm; + rfm22b_dev->status_packet.source_id = rfm22b_dev->deviceID; + rfm22b_dev->status_packet.link_quality = rfm22b_dev->stats.link_quality; + rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm; rfm22b_dev->send_status = true; - - return; } /** @@ -2179,8 +2176,9 @@ static void rfm22_sendPPM(__attribute__((unused)) struct pios_rfm22b_dev *rfm22b bool valid_input_detected = false; for (uint8_t i = 0; i < PIOS_PPM_NUM_INPUTS; ++i) { rfm22b_dev->ppm_packet.channels[i] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i + 1); - if((rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_INVALID) && (rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_TIMEOUT)) + if ((rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_INVALID) && (rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_TIMEOUT)) { valid_input_detected = true; + } } // Send the PPM packet if it's valid @@ -2190,7 +2188,7 @@ static void rfm22_sendPPM(__attribute__((unused)) struct pios_rfm22b_dev *rfm22b rfm22b_dev->ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&(rfm22b_dev->ppm_packet)); rfm22b_dev->send_ppm = true; } -#endif +#endif /* ifdef PIOS_PPM_RECEIVER */ } /** @@ -2221,6 +2219,7 @@ static enum pios_radio_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_radio_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev) { PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); + aph->header.destination_id = rfm22b_dev->destination_id; aph->header.type = PACKET_TYPE_NACK; aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); @@ -2242,16 +2241,16 @@ static enum pios_radio_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTING; // Fill in the connection request - rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID; - cph->header.destination_id = rfm22b_dev->destination_id; - cph->header.type = PACKET_TYPE_CON_REQUEST; + rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID; + cph->header.destination_id = rfm22b_dev->destination_id; + cph->header.type = PACKET_TYPE_CON_REQUEST; cph->header.data_size = PH_CONNECTION_DATA_SIZE(&(rfm22b_dev->con_packet)); - cph->source_id = rfm22b_dev->deviceID; - cph->status_rx_time = rfm22b_dev->rx_complete_ticks; - cph->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port; - cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port; + cph->source_id = rfm22b_dev->deviceID; + cph->status_rx_time = rfm22b_dev->rx_complete_ticks; + cph->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port; + cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port; cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port; - cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; + cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; rfm22b_dev->send_connection_request = true; rfm22b_dev->prev_tx_packet = NULL; @@ -2260,8 +2259,8 @@ static enum pios_radio_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm /***************************************************************************** - * Packet Receipt Functions - *****************************************************************************/ +* Packet Receipt Functions +*****************************************************************************/ /** * Receive an ACK. @@ -2298,7 +2297,6 @@ static enum pios_radio_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev */ static enum pios_radio_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev) { - // Resend the previous TX packet. rfm22b_dev->tx_packet = rfm22b_dev->prev_tx_packet; rfm22b_dev->prev_tx_packet = NULL; @@ -2318,39 +2316,40 @@ static enum pios_radio_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_de */ static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev) { - PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet); - int8_t rssi = rfm22b_dev->rssi_dBm; - int8_t afc = rfm22b_dev->afc_correction_Hz; - uint32_t id = status->source_id; + PHStatusPacketHandle status = (PHStatusPacketHandle) & (rfm22b_dev->rx_packet); + int8_t rssi = rfm22b_dev->rssi_dBm; + int8_t afc = rfm22b_dev->afc_correction_Hz; + uint32_t id = status->source_id; // Have we seen this device recently? - bool found = false; + bool found = false; uint8_t id_idx = 0; - for ( ; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) { - if(rfm22b_dev->pair_stats[id_idx].pairID == id) { + + for (; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) { + if (rfm22b_dev->pair_stats[id_idx].pairID == id) { found = true; break; } } // If we have seen it, update the RSSI and reset the last contact couter - if(found) { + if (found) { rfm22b_dev->pair_stats[id_idx].rssi = rssi; rfm22b_dev->pair_stats[id_idx].afc_correction = afc; - rfm22b_dev->pair_stats[id_idx].lastContact = 0; + rfm22b_dev->pair_stats[id_idx].lastContact = 0; - // If we haven't seen it, find a slot to put it in. + // If we haven't seen it, find a slot to put it in. } else { uint8_t min_idx = 0; int8_t min_rssi = rfm22b_dev->pair_stats[0].rssi; for (id_idx = 1; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) { - if(rfm22b_dev->pair_stats[id_idx].rssi < min_rssi) { + if (rfm22b_dev->pair_stats[id_idx].rssi < min_rssi) { min_rssi = rfm22b_dev->pair_stats[id_idx].rssi; - min_idx = id_idx; + min_idx = id_idx; } } rfm22b_dev->pair_stats[min_idx].pairID = id; - rfm22b_dev->pair_stats[min_idx].rssi = rssi; + rfm22b_dev->pair_stats[min_idx].rssi = rssi; rfm22b_dev->pair_stats[min_idx].afc_correction = afc; rfm22b_dev->pair_stats[min_idx].lastContact = 0; } @@ -2360,8 +2359,8 @@ static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_ /***************************************************************************** - * Link Statistics Functions - *****************************************************************************/ +* Link Statistics Functions +*****************************************************************************/ /** * Calculate the link quality from the packet receipt, tranmittion statistics. @@ -2371,10 +2370,10 @@ static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev) { // Add the RX packet statistics - rfm22b_dev->stats.rx_good = 0; + rfm22b_dev->stats.rx_good = 0; rfm22b_dev->stats.rx_corrected = 0; - rfm22b_dev->stats.rx_error = 0; - rfm22b_dev->stats.tx_resent = 0; + rfm22b_dev->stats.rx_error = 0; + rfm22b_dev->stats.tx_resent = 0; for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) { uint32_t val = rfm22b_dev->rx_packet_stats[i]; for (uint8_t j = 0; j < 16; ++j) { @@ -2419,8 +2418,8 @@ static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_r /***************************************************************************** - * Connection Handling Functions - *****************************************************************************/ +* Connection Handling Functions +*****************************************************************************/ /** * Are we connected to the remote modem? @@ -2429,7 +2428,7 @@ static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_r */ static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev) { - return (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED); + return rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED; } /** @@ -2467,16 +2466,16 @@ static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm2 rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; // Copy the connection packet - PHConnectionPacketHandle cph = (PHConnectionPacketHandle)(&(rfm22b_dev->rx_packet)); + PHConnectionPacketHandle cph = (PHConnectionPacketHandle)(&(rfm22b_dev->rx_packet)); PHConnectionPacketHandle lcph = (PHConnectionPacketHandle)(&(rfm22b_dev->con_packet)); - memcpy((uint8_t*)lcph, (uint8_t*)cph, PH_PACKET_SIZE((PHPacketHandle)cph)); + memcpy((uint8_t *)lcph, (uint8_t *)cph, PH_PACKET_SIZE((PHPacketHandle)cph)); // Set the destination ID to the source of the connection request message. rfm22b_dev->destination_id = cph->source_id; // The remote modem sets the time delta between the two modems using the differene between the clock // on the local modem when it sent the status packet and the time on the coordinator modem when it was received. - portTickType local_tx_time = rfm22b_dev->tx_complete_ticks; + portTickType local_tx_time = rfm22b_dev->tx_complete_ticks; portTickType remote_rx_time = cph->status_rx_time; rfm22b_dev->time_delta = remote_rx_time - local_tx_time; @@ -2485,8 +2484,8 @@ static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm2 /***************************************************************************** - * Frequency Hopping Functions - *****************************************************************************/ +* Frequency Hopping Functions +*****************************************************************************/ /** * Return the extimated current clock ticks count on the coordinator modem. @@ -2512,6 +2511,7 @@ static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev) portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); // Divide time into 8ms blocks. Coordinator sends in firs 2 ms, and remote send in 5th and 6th ms. bool tts = (rfm22b_dev->coordinator) ? ((time & 0x06) == 0) : (((time + 4) & 0x06) == 0); + // Noone starts a transmit just prior to a channel change. return tts && ((time & 0x7e) < 0x7b); } @@ -2526,6 +2526,7 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev) portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); // We change channels every 128 ms. uint16_t n = (time >> 7) & 0xffff; + // The channel is calculated using the 16 bit CRC as the pseudo random number generator. n = PIOS_CRC16_updateByte(n, 0); float num_channels = rfm22b_dev->num_channels; @@ -2547,8 +2548,8 @@ static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev) /***************************************************************************** - * Error Handling Functions - *****************************************************************************/ +* Error Handling Functions +*****************************************************************************/ /** * Recover from a transmit failure. @@ -2615,7 +2616,7 @@ static enum pios_radio_event rfm22_fatal_error(__attribute__((unused)) struct pi { // RF module error .. flash the LED's rfm22_clearLEDs(); - for(unsigned int j = 0; j < 16; j++) { + for (unsigned int j = 0; j < 16; j++) { USB_LED_ON; LINK_LED_ON; RX_LED_OFF; @@ -2640,8 +2641,8 @@ static enum pios_radio_event rfm22_fatal_error(__attribute__((unused)) struct pi /***************************************************************************** - * Utility Functions - *****************************************************************************/ +* Utility Functions +*****************************************************************************/ /** * Calculate the time difference between the start time and end time. @@ -2652,7 +2653,7 @@ static enum pios_radio_event rfm22_fatal_error(__attribute__((unused)) struct pi */ static uint32_t pios_rfm22_time_difference_ms(portTickType start_time, portTickType end_time) { - if(end_time >= start_time) { + if (end_time >= start_time) { return (end_time - start_time) * portTICK_RATE_MS; } // Rollover @@ -2665,7 +2666,7 @@ static uint32_t pios_rfm22_time_difference_ms(portTickType start_time, portTickT #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_rfm22b_dev *pios_rfm22_alloc(void) { - struct pios_rfm22b_dev * rfm22b_dev; + struct pios_rfm22b_dev *rfm22b_dev; rfm22b_dev = (struct pios_rfm22b_dev *)pvPortMalloc(sizeof(*rfm22b_dev)); rfm22b_dev->spi_id = 0; @@ -2674,14 +2675,14 @@ static struct pios_rfm22b_dev *pios_rfm22_alloc(void) } rfm22b_dev->magic = PIOS_RFM22B_DEV_MAGIC; - return(rfm22b_dev); + return rfm22b_dev; } #else static struct pios_rfm22b_dev pios_rfm22b_devs[PIOS_RFM22B_MAX_DEVS]; static uint8_t pios_rfm22b_num_devs; static struct pios_rfm22b_dev *pios_rfm22_alloc(void) { - struct pios_rfm22b_dev * rfm22b_dev; + struct pios_rfm22b_dev *rfm22b_dev; if (pios_rfm22b_num_devs >= PIOS_RFM22B_MAX_DEVS) { return NULL; @@ -2690,14 +2691,15 @@ static struct pios_rfm22b_dev *pios_rfm22_alloc(void) rfm22b_dev = &pios_rfm22b_devs[pios_rfm22b_num_devs++]; rfm22b_dev->magic = PIOS_RFM22B_DEV_MAGIC; - return (rfm22b_dev); + return rfm22b_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /** * Turn off all of the LEDs */ -static void rfm22_clearLEDs(void) { +static void rfm22_clearLEDs(void) +{ LINK_LED_OFF; RX_LED_OFF; TX_LED_OFF; @@ -2711,8 +2713,8 @@ static void rfm22_clearLEDs(void) { /***************************************************************************** - * SPI Read/Write Functions - *****************************************************************************/ +* SPI Read/Write Functions +*****************************************************************************/ /** * Assert the chip select line. @@ -2722,7 +2724,7 @@ static void rfm22_clearLEDs(void) { static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev) { PIOS_DELAY_WaituS(1); - if(rfm22b_dev->spi_id != 0) { + if (rfm22b_dev->spi_id != 0) { PIOS_SPI_RC_PinSet(rfm22b_dev->spi_id, rfm22b_dev->slave_num, 0); } } @@ -2734,7 +2736,7 @@ static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev) */ static void rfm22_deassertCs(struct pios_rfm22b_dev *rfm22b_dev) { - if(rfm22b_dev->spi_id != 0) { + if (rfm22b_dev->spi_id != 0) { PIOS_SPI_RC_PinSet(rfm22b_dev->spi_id, rfm22b_dev->slave_num, 1); } } @@ -2746,7 +2748,7 @@ static void rfm22_deassertCs(struct pios_rfm22b_dev *rfm22b_dev) */ static void rfm22_claimBus(struct pios_rfm22b_dev *rfm22b_dev) { - if(rfm22b_dev->spi_id != 0) { + if (rfm22b_dev->spi_id != 0) { PIOS_SPI_ClaimBus(rfm22b_dev->spi_id); } } @@ -2758,7 +2760,7 @@ static void rfm22_claimBus(struct pios_rfm22b_dev *rfm22b_dev) */ static void rfm22_releaseBus(struct pios_rfm22b_dev *rfm22b_dev) { - if(rfm22b_dev->spi_id != 0) { + if (rfm22b_dev->spi_id != 0) { PIOS_SPI_ReleaseBus(rfm22b_dev->spi_id); } } @@ -2774,7 +2776,7 @@ static void rfm22_write_claim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, { rfm22_claimBus(rfm22b_dev); rfm22_assertCs(rfm22b_dev); - uint8_t buf[2] = {addr | 0x80, data}; + uint8_t buf[2] = { addr | 0x80, data }; PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL); rfm22_deassertCs(rfm22b_dev); rfm22_releaseBus(rfm22b_dev); @@ -2790,7 +2792,7 @@ static void rfm22_write_claim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data) { rfm22_assertCs(rfm22b_dev); - uint8_t buf[2] = {addr | 0x80, data}; + uint8_t buf[2] = { addr | 0x80, data }; PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL); rfm22_deassertCs(rfm22b_dev); } @@ -2804,8 +2806,9 @@ static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_ */ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr) { - uint8_t out[2] = {addr & 0x7F, 0xFF}; + uint8_t out[2] = { addr &0x7F, 0xFF }; uint8_t in[2]; + rfm22_assertCs(rfm22b_dev); PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, out, in, sizeof(out), NULL); rfm22_deassertCs(rfm22b_dev); diff --git a/flight/pios/common/pios_rfm22b_com.c b/flight/pios/common/pios_rfm22b_com.c index e0800eb27..9275d48b6 100644 --- a/flight/pios/common/pios_rfm22b_com.c +++ b/flight/pios/common/pios_rfm22b_com.c @@ -1,30 +1,30 @@ /** -****************************************************************************** -* @addtogroup PIOS PIOS Core hardware abstraction layer -* @{ -* @addtogroup PIOS_RFM22B Radio Functions -* @brief PIOS COM interface for for the RFM22B radio -* @{ -* -* @file pios_rfm22b_com.c -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief Implements a driver the the RFM22B driver -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RFM22B Radio Functions + * @brief PIOS COM interface for for the RFM22B radio + * @{ + * + * @file pios_rfm22b_com.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Implements a driver the the RFM22B driver + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -61,27 +61,29 @@ const struct pios_com_driver pios_rfm22b_com_driver = { static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex. enum rfm22b_datarate datarate = RFM22_datarate_64000; - if (baud <= 1024) + if (baud <= 1024) { datarate = RFM22_datarate_500; - else if (baud <= 2048) + } else if (baud <= 2048) { datarate = RFM22_datarate_1000; - else if (baud <= 4096) + } else if (baud <= 4096) { datarate = RFM22_datarate_8000; - else if (baud <= 9600) + } else if (baud <= 9600) { datarate = RFM22_datarate_16000; - else if (baud <= 19200) + } else if (baud <= 19200) { datarate = RFM22_datarate_32000; - else if (baud <= 38400) + } else if (baud <= 38400) { datarate = RFM22_datarate_57600; - else if (baud <= 57600) + } else if (baud <= 57600) { datarate = RFM22_datarate_128000; - else if (baud <= 115200) + } else if (baud <= 115200) { datarate = RFM22_datarate_192000; + } rfm22b_dev->datarate = datarate; } @@ -92,9 +94,8 @@ static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) * @param[in] rx_bytes_available The number of bytes available to receive */ static void PIOS_RFM22B_COM_RxStart(__attribute__((unused)) uint32_t rfm22b_id, - __attribute__((unused)) uint16_t rx_bytes_avail) -{ -} + __attribute__((unused)) uint16_t rx_bytes_avail) +{} /** * Start a transmit from the COM device @@ -105,13 +106,14 @@ static void PIOS_RFM22B_COM_RxStart(__attribute__((unused)) uint32_t rfm22b_id, static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, __attribute__((unused)) uint16_t tx_bytes_avail) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } } /** - * Register the callback to pass received data to + * Register the callback to pass received data to * * @param[in] rfm22b_dev The device ID. * @param[in] rx_in_cb The Rx callback function. @@ -120,10 +122,12 @@ static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, __attribute__((unused)) static void PIOS_RFM22B_COM_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_Validate(rfm22b_dev)) - return; - /* + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } + + /* * Order is important in these assignments since ISR uses _cb * field to determine if it's ok to dereference _cb and _context */ @@ -141,11 +145,12 @@ static void PIOS_RFM22B_COM_RegisterRxCallback(uint32_t rfm22b_id, pios_com_call static void PIOS_RFM22B_COM_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } - /* + /* * Order is important in these assignments since ISR uses _cb * field to determine if it's ok to dereference _cb and _context */ diff --git a/flight/pios/common/pios_rfm22b_rcvr.c b/flight/pios/common/pios_rfm22b_rcvr.c index e718e8481..81f43efa8 100644 --- a/flight/pios/common/pios_rfm22b_rcvr.c +++ b/flight/pios/common/pios_rfm22b_rcvr.c @@ -1,17 +1,17 @@ /** -****************************************************************************** -* @addtogroup PIOS PIOS Core hardware abstraction layer -* @{ -* @addtogroup PIOS_RFM22B_RCVR RFM22B Receiver Input Functions -* @brief Code to output the PPM signal from the RFM22B -* @{ -* -* @file pios_rfm22b_rcvr.c -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief Implements a receiver interface to the RFM22B device -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RFM22B_RCVR RFM22B Receiver Input Functions + * @brief Code to output the PPM signal from the RFM22B + * @{ + * + * @file pios_rfm22b_rcvr.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Implements a receiver interface to the RFM22B device + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -51,6 +51,7 @@ const struct pios_rcvr_driver pios_rfm22b_rcvr_driver = { int32_t PIOS_RFM22B_RCVR_Init(uint32_t rcvr_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return -1; } @@ -78,6 +79,7 @@ int32_t PIOS_RFM22B_RCVR_Init(uint32_t rcvr_id) static int32_t PIOS_RFM22B_RCVR_Get(uint32_t rcvr_id, uint8_t channel) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return -1; } @@ -95,8 +97,10 @@ static int32_t PIOS_RFM22B_RCVR_Get(uint32_t rcvr_id, uint8_t channel) * * @param[in] rcvr_id The receiver ID. */ -static void PIOS_RFM22B_RCVR_Supervisor(uint32_t rcvr_id) { +static void PIOS_RFM22B_RCVR_Supervisor(uint32_t rcvr_id) +{ struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } @@ -116,9 +120,9 @@ static void PIOS_RFM22B_RCVR_Supervisor(uint32_t rcvr_id) { rfm22b_dev->ppm_fresh = false; } -#endif /* PIOS_INCLUDE_RFM22B_RCVR */ +#endif /* PIOS_INCLUDE_RFM22B_RCVR */ -/** +/** * @} * @} */ diff --git a/flight/pios/common/pios_sbus.c b/flight/pios/common/pios_sbus.c index 0444f15ab..cf9b3579c 100644 --- a/flight/pios/common/pios_sbus.c +++ b/flight/pios/common/pios_sbus.c @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,128 +37,132 @@ /* Forward Declarations */ static int32_t PIOS_SBus_Get(uint32_t rcvr_id, uint8_t channel); static uint16_t PIOS_SBus_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield); + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield); static void PIOS_SBus_Supervisor(uint32_t sbus_id); /* Local Variables */ const struct pios_rcvr_driver pios_sbus_rcvr_driver = { - .read = PIOS_SBus_Get, + .read = PIOS_SBus_Get, }; enum pios_sbus_dev_magic { - PIOS_SBUS_DEV_MAGIC = 0x53427573, + PIOS_SBUS_DEV_MAGIC = 0x53427573, }; struct pios_sbus_state { - uint16_t channel_data[PIOS_SBUS_NUM_INPUTS]; - uint8_t received_data[SBUS_FRAME_LENGTH - 2]; - uint8_t receive_timer; - uint8_t failsafe_timer; - uint8_t frame_found; - uint8_t byte_count; + uint16_t channel_data[PIOS_SBUS_NUM_INPUTS]; + uint8_t received_data[SBUS_FRAME_LENGTH - 2]; + uint8_t receive_timer; + uint8_t failsafe_timer; + uint8_t frame_found; + uint8_t byte_count; }; struct pios_sbus_dev { - enum pios_sbus_dev_magic magic; - const struct pios_sbus_cfg *cfg; - struct pios_sbus_state state; + enum pios_sbus_dev_magic magic; + const struct pios_sbus_cfg *cfg; + struct pios_sbus_state state; }; /* Allocate S.Bus device descriptor */ #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_sbus_dev *PIOS_SBus_Alloc(void) { - struct pios_sbus_dev *sbus_dev; + struct pios_sbus_dev *sbus_dev; - sbus_dev = (struct pios_sbus_dev *)pvPortMalloc(sizeof(*sbus_dev)); - if (!sbus_dev) return(NULL); + sbus_dev = (struct pios_sbus_dev *)pvPortMalloc(sizeof(*sbus_dev)); + if (!sbus_dev) { + return NULL; + } - sbus_dev->magic = PIOS_SBUS_DEV_MAGIC; - return(sbus_dev); + sbus_dev->magic = PIOS_SBUS_DEV_MAGIC; + return sbus_dev; } #else static struct pios_sbus_dev pios_sbus_devs[PIOS_SBUS_MAX_DEVS]; static uint8_t pios_sbus_num_devs; static struct pios_sbus_dev *PIOS_SBus_Alloc(void) { - struct pios_sbus_dev *sbus_dev; + struct pios_sbus_dev *sbus_dev; - if (pios_sbus_num_devs >= PIOS_SBUS_MAX_DEVS) { - return (NULL); - } + if (pios_sbus_num_devs >= PIOS_SBUS_MAX_DEVS) { + return NULL; + } - sbus_dev = &pios_sbus_devs[pios_sbus_num_devs++]; - sbus_dev->magic = PIOS_SBUS_DEV_MAGIC; + sbus_dev = &pios_sbus_devs[pios_sbus_num_devs++]; + sbus_dev->magic = PIOS_SBUS_DEV_MAGIC; - return (sbus_dev); + return sbus_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /* Validate S.Bus device descriptor */ static bool PIOS_SBus_Validate(struct pios_sbus_dev *sbus_dev) { - return (sbus_dev->magic == PIOS_SBUS_DEV_MAGIC); + return sbus_dev->magic == PIOS_SBUS_DEV_MAGIC; } /* Reset channels in case of lost signal or explicit failsafe receiver flag */ static void PIOS_SBus_ResetChannels(struct pios_sbus_state *state) { - for (int i = 0; i < PIOS_SBUS_NUM_INPUTS; i++) { - state->channel_data[i] = PIOS_RCVR_TIMEOUT; - } + for (int i = 0; i < PIOS_SBUS_NUM_INPUTS; i++) { + state->channel_data[i] = PIOS_RCVR_TIMEOUT; + } } /* Reset S.Bus receiver state */ static void PIOS_SBus_ResetState(struct pios_sbus_state *state) { - state->receive_timer = 0; - state->failsafe_timer = 0; - state->frame_found = 0; - PIOS_SBus_ResetChannels(state); + state->receive_timer = 0; + state->failsafe_timer = 0; + state->frame_found = 0; + PIOS_SBus_ResetChannels(state); } /* Initialise S.Bus receiver interface */ int32_t PIOS_SBus_Init(uint32_t *sbus_id, - const struct pios_sbus_cfg *cfg, - const struct pios_com_driver *driver, - uint32_t lower_id) + const struct pios_sbus_cfg *cfg, + const struct pios_com_driver *driver, + uint32_t lower_id) { - PIOS_DEBUG_Assert(sbus_id); - PIOS_DEBUG_Assert(cfg); - PIOS_DEBUG_Assert(driver); + PIOS_DEBUG_Assert(sbus_id); + PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(driver); - struct pios_sbus_dev *sbus_dev; + struct pios_sbus_dev *sbus_dev; - sbus_dev = (struct pios_sbus_dev *)PIOS_SBus_Alloc(); - if (!sbus_dev) goto out_fail; + sbus_dev = (struct pios_sbus_dev *)PIOS_SBus_Alloc(); + if (!sbus_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - sbus_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + sbus_dev->cfg = cfg; - PIOS_SBus_ResetState(&(sbus_dev->state)); + PIOS_SBus_ResetState(&(sbus_dev->state)); - *sbus_id = (uint32_t)sbus_dev; + *sbus_id = (uint32_t)sbus_dev; - /* Enable inverter clock and enable the inverter */ - (*cfg->gpio_clk_func)(cfg->gpio_clk_periph, ENABLE); - GPIO_Init(cfg->inv.gpio, &cfg->inv.init); - GPIO_WriteBit(cfg->inv.gpio, cfg->inv.init.GPIO_Pin, cfg->gpio_inv_enable); + /* Enable inverter clock and enable the inverter */ + (*cfg->gpio_clk_func)(cfg->gpio_clk_periph, ENABLE); + GPIO_Init(cfg->inv.gpio, &cfg->inv.init); + GPIO_WriteBit(cfg->inv.gpio, cfg->inv.init.GPIO_Pin, cfg->gpio_inv_enable); - /* Set comm driver callback */ - (driver->bind_rx_cb)(lower_id, PIOS_SBus_RxInCallback, *sbus_id); + /* Set comm driver callback */ + (driver->bind_rx_cb)(lower_id, PIOS_SBus_RxInCallback, *sbus_id); - if (!PIOS_RTC_RegisterTickCallback(PIOS_SBus_Supervisor, *sbus_id)) { - PIOS_DEBUG_Assert(0); - } + if (!PIOS_RTC_RegisterTickCallback(PIOS_SBus_Supervisor, *sbus_id)) { + PIOS_DEBUG_Assert(0); + } - return 0; + return 0; out_fail: - return -1; + return -1; } /** @@ -170,17 +174,18 @@ out_fail: */ static int32_t PIOS_SBus_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)rcvr_id; + struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)rcvr_id; - if (!PIOS_SBus_Validate(sbus_dev)) - return PIOS_RCVR_INVALID; + if (!PIOS_SBus_Validate(sbus_dev)) { + return PIOS_RCVR_INVALID; + } - /* return error if channel is not available */ - if (channel >= PIOS_SBUS_NUM_INPUTS) { - return PIOS_RCVR_INVALID; - } + /* return error if channel is not available */ + if (channel >= PIOS_SBUS_NUM_INPUTS) { + return PIOS_RCVR_INVALID; + } - return sbus_dev->state.channel_data[channel]; + return sbus_dev->state.channel_data[channel]; } /** @@ -190,111 +195,114 @@ static int32_t PIOS_SBus_Get(uint32_t rcvr_id, uint8_t channel) */ static void PIOS_SBus_UnrollChannels(struct pios_sbus_state *state) { - uint8_t *s = state->received_data; - uint16_t *d = state->channel_data; + uint8_t *s = state->received_data; + uint16_t *d = state->channel_data; -#define F(v,s) (((v) >> (s)) & 0x7ff) +#define F(v, s) (((v) >> (s)) & 0x7ff) - /* unroll channels 1-8 */ - *d++ = F(s[0] | s[1] << 8, 0); - *d++ = F(s[1] | s[2] << 8, 3); - *d++ = F(s[2] | s[3] << 8 | s[4] << 16, 6); - *d++ = F(s[4] | s[5] << 8, 1); - *d++ = F(s[5] | s[6] << 8, 4); - *d++ = F(s[6] | s[7] << 8 | s[8] << 16, 7); - *d++ = F(s[8] | s[9] << 8, 2); - *d++ = F(s[9] | s[10] << 8, 5); + /* unroll channels 1-8 */ + *d++ = F(s[0] | s[1] << 8, 0); + *d++ = F(s[1] | s[2] << 8, 3); + *d++ = F(s[2] | s[3] << 8 | s[4] << 16, 6); + *d++ = F(s[4] | s[5] << 8, 1); + *d++ = F(s[5] | s[6] << 8, 4); + *d++ = F(s[6] | s[7] << 8 | s[8] << 16, 7); + *d++ = F(s[8] | s[9] << 8, 2); + *d++ = F(s[9] | s[10] << 8, 5); - /* unroll channels 9-16 */ - *d++ = F(s[11] | s[12] << 8, 0); - *d++ = F(s[12] | s[13] << 8, 3); - *d++ = F(s[13] | s[14] << 8 | s[15] << 16, 6); - *d++ = F(s[15] | s[16] << 8, 1); - *d++ = F(s[16] | s[17] << 8, 4); - *d++ = F(s[17] | s[18] << 8 | s[19] << 16, 7); - *d++ = F(s[19] | s[20] << 8, 2); - *d++ = F(s[20] | s[21] << 8, 5); + /* unroll channels 9-16 */ + *d++ = F(s[11] | s[12] << 8, 0); + *d++ = F(s[12] | s[13] << 8, 3); + *d++ = F(s[13] | s[14] << 8 | s[15] << 16, 6); + *d++ = F(s[15] | s[16] << 8, 1); + *d++ = F(s[16] | s[17] << 8, 4); + *d++ = F(s[17] | s[18] << 8 | s[19] << 16, 7); + *d++ = F(s[19] | s[20] << 8, 2); + *d++ = F(s[20] | s[21] << 8, 5); - /* unroll discrete channels 17 and 18 */ - *d++ = (s[22] & SBUS_FLAG_DC1) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN; - *d++ = (s[22] & SBUS_FLAG_DC2) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN; + /* unroll discrete channels 17 and 18 */ + *d++ = (s[22] & SBUS_FLAG_DC1) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN; + *d++ = (s[22] & SBUS_FLAG_DC2) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN; } /* Update decoder state processing input byte from the S.Bus stream */ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b) { - /* should not process any data until new frame is found */ - if (!state->frame_found) - return; + /* should not process any data until new frame is found */ + if (!state->frame_found) { + return; + } - if (state->byte_count == 0) { - if (b != SBUS_SOF_BYTE) { - /* discard the whole frame if the 1st byte is not correct */ - state->frame_found = 0; - } else { - /* do not store the SOF byte */ - state->byte_count++; - } - return; - } + if (state->byte_count == 0) { + if (b != SBUS_SOF_BYTE) { + /* discard the whole frame if the 1st byte is not correct */ + state->frame_found = 0; + } else { + /* do not store the SOF byte */ + state->byte_count++; + } + return; + } - /* do not store last frame byte as well */ - if (state->byte_count < SBUS_FRAME_LENGTH - 1) { - /* store next byte */ - state->received_data[state->byte_count - 1] = b; - state->byte_count++; - } else { - if (b == SBUS_EOF_BYTE) { - /* full frame received */ - uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3]; - if (flags & SBUS_FLAG_FL) { - /* frame lost, do not update */ - } else if (flags & SBUS_FLAG_FS) { - /* failsafe flag active */ - PIOS_SBus_ResetChannels(state); - } else { - /* data looking good */ - PIOS_SBus_UnrollChannels(state); - state->failsafe_timer = 0; - } - } else { - /* discard whole frame */ - } + /* do not store last frame byte as well */ + if (state->byte_count < SBUS_FRAME_LENGTH - 1) { + /* store next byte */ + state->received_data[state->byte_count - 1] = b; + state->byte_count++; + } else { + if (b == SBUS_EOF_BYTE) { + /* full frame received */ + uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3]; + if (flags & SBUS_FLAG_FL) { + /* frame lost, do not update */ + } else if (flags & SBUS_FLAG_FS) { + /* failsafe flag active */ + PIOS_SBus_ResetChannels(state); + } else { + /* data looking good */ + PIOS_SBus_UnrollChannels(state); + state->failsafe_timer = 0; + } + } else { + /* discard whole frame */ + } - /* prepare for the next frame */ - state->frame_found = 0; - } + /* prepare for the next frame */ + state->frame_found = 0; + } } /* Comm byte received callback */ static uint16_t PIOS_SBus_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield) + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield) { - struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)context; + struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)context; - bool valid = PIOS_SBus_Validate(sbus_dev); - PIOS_Assert(valid); + bool valid = PIOS_SBus_Validate(sbus_dev); - struct pios_sbus_state *state = &(sbus_dev->state); + PIOS_Assert(valid); - /* process byte(s) and clear receive timer */ - for (uint8_t i = 0; i < buf_len; i++) { - PIOS_SBus_UpdateState(state, buf[i]); - state->receive_timer = 0; - } + struct pios_sbus_state *state = &(sbus_dev->state); - /* Always signal that we can accept another byte */ - if (headroom) - *headroom = SBUS_FRAME_LENGTH; + /* process byte(s) and clear receive timer */ + for (uint8_t i = 0; i < buf_len; i++) { + PIOS_SBus_UpdateState(state, buf[i]); + state->receive_timer = 0; + } - /* We never need a yield */ - *need_yield = false; + /* Always signal that we can accept another byte */ + if (headroom) { + *headroom = SBUS_FRAME_LENGTH; + } - /* Always indicate that all bytes were consumed */ - return buf_len; + /* We never need a yield */ + *need_yield = false; + + /* Always indicate that all bytes were consumed */ + return buf_len; } /** @@ -311,30 +319,31 @@ static uint16_t PIOS_SBus_RxInCallback(uint32_t context, */ static void PIOS_SBus_Supervisor(uint32_t sbus_id) { - struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)sbus_id; + struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)sbus_id; - bool valid = PIOS_SBus_Validate(sbus_dev); - PIOS_Assert(valid); + bool valid = PIOS_SBus_Validate(sbus_dev); - struct pios_sbus_state *state = &(sbus_dev->state); + PIOS_Assert(valid); - /* waiting for new frame if no bytes were received in 3.2ms */ - if (++state->receive_timer > 2) { - state->frame_found = 1; - state->byte_count = 0; - state->receive_timer = 0; - } + struct pios_sbus_state *state = &(sbus_dev->state); - /* activate failsafe if no frames have arrived in 102.4ms */ - if (++state->failsafe_timer > 64) { - PIOS_SBus_ResetChannels(state); - state->failsafe_timer = 0; - } + /* waiting for new frame if no bytes were received in 3.2ms */ + if (++state->receive_timer > 2) { + state->frame_found = 1; + state->byte_count = 0; + state->receive_timer = 0; + } + + /* activate failsafe if no frames have arrived in 102.4ms */ + if (++state->failsafe_timer > 64) { + PIOS_SBus_ResetChannels(state); + state->failsafe_timer = 0; + } } #endif /* PIOS_INCLUDE_SBUS */ -/** +/** * @} * @} */ diff --git a/flight/pios/common/pios_sdcard.c b/flight/pios/common/pios_sdcard.c index 1ae2ea50e..e5ba0dbf8 100644 --- a/flight/pios/common/pios_sdcard.c +++ b/flight/pios/common/pios_sdcard.c @@ -6,26 +6,26 @@ * @brief Code to deal with reading and writing to flash cards * @{ * - * @file pios_sdcard.c + * @file pios_sdcard.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief Sets up basic system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,765 +39,771 @@ uint8_t PIOS_SDCARD_Sector[SECTOR_SIZE]; /* Local Definitions */ #if !defined(SDCARD_MUTEX_TAKE) -#define SDCARD_MUTEX_TAKE {} -#define SDCARD_MUTEX_GIVE {} +#define SDCARD_MUTEX_TAKE {} +#define SDCARD_MUTEX_GIVE {} #endif /* Definitions for MMC/SDC command */ -#define SDCMD_GO_IDLE_STATE (0x40+0) -#define SDCMD_GO_IDLE_STATE_CRC 0x95 +#define SDCMD_GO_IDLE_STATE (0x40 + 0) +#define SDCMD_GO_IDLE_STATE_CRC 0x95 -#define SDCMD_SEND_OP_COND (0x40+1) -#define SDCMD_SEND_OP_COND_CRC 0xf9 +#define SDCMD_SEND_OP_COND (0x40 + 1) +#define SDCMD_SEND_OP_COND_CRC 0xf9 -#define SDCMD_SEND_OP_COND_SDC (0xC0+41) -#define SDCMD_SEND_OP_COND_SDC_CRC 0xff +#define SDCMD_SEND_OP_COND_SDC (0xC0 + 41) +#define SDCMD_SEND_OP_COND_SDC_CRC 0xff -#define SDCMD_READ_OCR (0x40+58) -#define SDCMD_READ_OCR_CRC 0xff +#define SDCMD_READ_OCR (0x40 + 58) +#define SDCMD_READ_OCR_CRC 0xff -#define SDCMD_APP_CMD (0x40+55) -#define SDCMD_APP_CMD_CRC 0xff +#define SDCMD_APP_CMD (0x40 + 55) +#define SDCMD_APP_CMD_CRC 0xff -#define SDCMD_SEND_IF_COND (0x40+8) -#define SDCMD_SEND_IF_COND_CRC 0x87 +#define SDCMD_SEND_IF_COND (0x40 + 8) +#define SDCMD_SEND_IF_COND_CRC 0x87 -#define SDCMD_SEND_CSD (0x40+9) -#define SDCMD_SEND_CSD_CRC 0xff +#define SDCMD_SEND_CSD (0x40 + 9) +#define SDCMD_SEND_CSD_CRC 0xff -#define SDCMD_SEND_CID (0x40+10) -#define SDCMD_SEND_CID_CRC 0xff +#define SDCMD_SEND_CID (0x40 + 10) +#define SDCMD_SEND_CID_CRC 0xff -#define SDCMD_SEND_STATUS (0x40+13) -#define SDCMD_SEND_STATUS_CRC 0xaf +#define SDCMD_SEND_STATUS (0x40 + 13) +#define SDCMD_SEND_STATUS_CRC 0xaf -#define SDCMD_READ_SINGLE_BLOCK (0x40+17) -#define SDCMD_READ_SINGLE_BLOCK_CRC 0xff +#define SDCMD_READ_SINGLE_BLOCK (0x40 + 17) +#define SDCMD_READ_SINGLE_BLOCK_CRC 0xff -#define SDCMD_SET_BLOCKLEN (0x40+16) -#define SDCMD_SET_BLOCKLEN_CRC 0xff +#define SDCMD_SET_BLOCKLEN (0x40 + 16) +#define SDCMD_SET_BLOCKLEN_CRC 0xff -#define SDCMD_WRITE_SINGLE_BLOCK (0x40+24) -#define SDCMD_WRITE_SINGLE_BLOCK_CRC 0xff +#define SDCMD_WRITE_SINGLE_BLOCK (0x40 + 24) +#define SDCMD_WRITE_SINGLE_BLOCK_CRC 0xff /* Card type flags (CardType) */ -#define CT_MMC 0x01 -#define CT_SD1 0x02 -#define CT_SD2 0x04 -#define CT_SDC (CT_SD1|CT_SD2) -#define CT_BLOCK 0x08 +#define CT_MMC 0x01 +#define CT_SD1 0x02 +#define CT_SD2 0x04 +#define CT_SDC (CT_SD1 | CT_SD2) +#define CT_BLOCK 0x08 static uint8_t CardType; static int32_t sdcard_mounted; static uint32_t PIOS_SDCARD_SPI; /** -* Initialises SPI pins and peripheral to access MMC/SD Card -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ + * Initialises SPI pins and peripheral to access MMC/SD Card + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ int32_t PIOS_SDCARD_Init(uint32_t spi_id) { - SDCARD_MUTEX_TAKE; + SDCARD_MUTEX_TAKE; - sdcard_mounted = 0; + sdcard_mounted = 0; - PIOS_SDCARD_SPI = spi_id; + PIOS_SDCARD_SPI = spi_id; - /* Init SPI port for slow frequency access (ca. 0.3 MBit/s) */ - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_256); + /* Init SPI port for slow frequency access (ca. 0.3 MBit/s) */ + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_256); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xFF); - SDCARD_MUTEX_GIVE; + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xFF); + SDCARD_MUTEX_GIVE; - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Connects to SD Card -* \return < 0 if initialisation sequence failed -*/ + * Connects to SD Card + * \return < 0 if initialisation sequence failed + */ int32_t PIOS_SDCARD_PowerOn(void) { - int32_t status; - int i; + int32_t status; + int i; - SDCARD_MUTEX_TAKE; - /* Ensure that chip select line deactivated */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + SDCARD_MUTEX_TAKE; + /* Ensure that chip select line deactivated */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* Init SPI port for slow frequency access (ca. 0.3 MBit/s) */ - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_256); + /* Init SPI port for slow frequency access (ca. 0.3 MBit/s) */ + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_256); - /* Send 80 clock cycles to start up */ - for (i = 0; i < 10; ++i) { - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - } + /* Send 80 clock cycles to start up */ + for (i = 0; i < 10; ++i) { + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + } - /* Activate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ + /* Activate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ - /* wait for 1 mS */ - PIOS_DELAY_WaituS(1000); + /* wait for 1 mS */ + PIOS_DELAY_WaituS(1000); - /* Send CMD0 to reset the media */ - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_GO_IDLE_STATE, 0, SDCMD_GO_IDLE_STATE_CRC)) < 0) { - goto error; - } + /* Send CMD0 to reset the media */ + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_GO_IDLE_STATE, 0, SDCMD_GO_IDLE_STATE_CRC)) < 0) { + goto error; + } - CardType = 0; + CardType = 0; - /* A card is detected, what type is it? Use SEND_IF_COND (CMD8) to find out */ - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_IF_COND, 0x1AA, SDCMD_SEND_IF_COND_CRC)) == 0x01aa) { - /* SDHC Card Detected. */ + /* A card is detected, what type is it? Use SEND_IF_COND (CMD8) to find out */ + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_IF_COND, 0x1AA, SDCMD_SEND_IF_COND_CRC)) == 0x01aa) { + /* SDHC Card Detected. */ - /* We now check to see if we should use block mode or byte mode. Command is SEND_OP_COND_SDC (ACMD41) with HCS (bit 30) set */ - for (i = 0; i < 16384; i++) - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND_SDC, 0x01 << 30, SDCMD_SEND_OP_COND_SDC_CRC)) == 0) - break; + /* We now check to see if we should use block mode or byte mode. Command is SEND_OP_COND_SDC (ACMD41) with HCS (bit 30) set */ + for (i = 0; i < 16384; i++) { + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND_SDC, 0x01 << 30, SDCMD_SEND_OP_COND_SDC_CRC)) == 0) { + break; + } + } - if (i < 16384) { - status = PIOS_SDCARD_SendSDCCmd(SDCMD_READ_OCR, 0, SDCMD_READ_OCR_CRC); - CardType = ((status >> 24) & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2; - status = 0; - } else { - /* We waited long enough! */ - status = -2; - } - } else { - /* Card is SDv1 or MMC. */ + if (i < 16384) { + status = PIOS_SDCARD_SendSDCCmd(SDCMD_READ_OCR, 0, SDCMD_READ_OCR_CRC); + CardType = ((status >> 24) & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2; + status = 0; + } else { + /* We waited long enough! */ + status = -2; + } + } else { + /* Card is SDv1 or MMC. */ - /* MMC will accept ACMD41 (SDv1 won't and requires CMD1) so send ACMD41 first and then CMD1 if that fails */ - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND_SDC, 0, SDCMD_SEND_OP_COND_SDC_CRC)) <= 1) { - CardType = CT_SD1; - } else { - CardType = CT_MMC; - } + /* MMC will accept ACMD41 (SDv1 won't and requires CMD1) so send ACMD41 first and then CMD1 if that fails */ + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND_SDC, 0, SDCMD_SEND_OP_COND_SDC_CRC)) <= 1) { + CardType = CT_SD1; + } else { + CardType = CT_MMC; + } - for (i = 0; i < 16384; i++) { - if (CardType == CT_SD1) { - status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND_SDC, 0, SDCMD_SEND_OP_COND_SDC_CRC); - } else { - status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND, 0, SDCMD_SEND_OP_COND_CRC); - } + for (i = 0; i < 16384; i++) { + if (CardType == CT_SD1) { + status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND_SDC, 0, SDCMD_SEND_OP_COND_SDC_CRC); + } else { + status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND, 0, SDCMD_SEND_OP_COND_CRC); + } - if (status < 0) { - break; - } - if (status == 0) { - break; - } - } + if (status < 0) { + break; + } + if (status == 0) { + break; + } + } - /* The block size should already be 512 bytes but re-initialize just in case (ignore if it fails) */ - PIOS_SDCARD_SendSDCCmd(SDCMD_SET_BLOCKLEN, 512, SDCMD_SEND_OP_COND_CRC); - } + /* The block size should already be 512 bytes but re-initialize just in case (ignore if it fails) */ + PIOS_SDCARD_SendSDCCmd(SDCMD_SET_BLOCKLEN, 512, SDCMD_SEND_OP_COND_CRC); + } - if (i == 16384 || CardType == 0) { - /* The last loop timed out or the cardtype was not detected... */ - status = -2; - } + if (i == 16384 || CardType == 0) { + /* The last loop timed out or the cardtype was not detected... */ + status = -2; + } error: - /* Deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* Deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* Send dummy byte once deactivated to drop cards DO */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - SDCARD_MUTEX_GIVE; - return status; /* Status should be 0 if nothing went wrong! */ + /* Send dummy byte once deactivated to drop cards DO */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + SDCARD_MUTEX_GIVE; + return status; /* Status should be 0 if nothing went wrong! */ } /** -* Disconnects from SD Card -* \return < 0 on errors -*/ + * Disconnects from SD Card + * \return < 0 on errors + */ int32_t PIOS_SDCARD_PowerOff(void) { - return 0; // no error + return 0; // no error } /** -* If SD card was previously available: Checks if the SD Card is still -* available by sending the STATUS command.
-* This takes ca. 10 uS -* -* If SD card was previously not available: Checks if the SD Card is -* available by sending the IDLE command at low speed.
-* This takes ca. 500 uS!
-* Once we got a positive response, SDCARD_PowerOn() will be -* called by this function to initialize the card completely. -* -* Example for Connection/Disconnection detection: -* \code -* // this function is called each second from a low-priority task -* // If multiple tasks are accessing the SD card, add a semaphore/mutex -* // to avoid IO access collisions with other tasks! -* u8 sdcard_available; -* int32_t CheckSDCard(void) -* { -* // check if SD card is available -* // High speed access if the card was previously available -* u8 prev_sdcard_available = sdcard_available; -* sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); -* -* if(sdcard_available && !prev_sdcard_available) { -* // SD Card has been connected -* -* // now it's possible to read/write sectors -* -* } else if( !sdcard_available && prev_sdcard_available ) { -* // SD Card has been disconnected -* -* // here you can notify your application about this state -* } -* -* return 0; // no error -* } -* \endcode -* \param[in] was_available should only be set if the SD card was previously available -* \return 0 if no response from SD Card -* \return 1 if SD card is accessible -*/ + * If SD card was previously available: Checks if the SD Card is still + * available by sending the STATUS command.
+ * This takes ca. 10 uS + * + * If SD card was previously not available: Checks if the SD Card is + * available by sending the IDLE command at low speed.
+ * This takes ca. 500 uS!
+ * Once we got a positive response, SDCARD_PowerOn() will be + * called by this function to initialize the card completely. + * + * Example for Connection/Disconnection detection: + * \code + * // this function is called each second from a low-priority task + * // If multiple tasks are accessing the SD card, add a semaphore/mutex + * // to avoid IO access collisions with other tasks! + * u8 sdcard_available; + * int32_t CheckSDCard(void) + * { + * // check if SD card is available + * // High speed access if the card was previously available + * u8 prev_sdcard_available = sdcard_available; + * sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); + * + * if(sdcard_available && !prev_sdcard_available) { + * // SD Card has been connected + * + * // now it's possible to read/write sectors + * + * } else if( !sdcard_available && prev_sdcard_available ) { + * // SD Card has been disconnected + * + * // here you can notify your application about this state + * } + * + * return 0; // no error + * } + * \endcode + * \param[in] was_available should only be set if the SD card was previously available + * \return 0 if no response from SD Card + * \return 1 if SD card is accessible + */ int32_t PIOS_SDCARD_CheckAvailable(uint8_t was_available) { - int32_t ret; - SDCARD_MUTEX_TAKE; + int32_t ret; - if (was_available) { - /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ - /* This is required for the case that the SPI port is shared with other devices */ - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); + SDCARD_MUTEX_TAKE; - /* Activate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ + if (was_available) { + /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ + /* This is required for the case that the SPI port is shared with other devices */ + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); - /* Send STATUS command to check if media is available */ - ret = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_STATUS, 0, SDCMD_SEND_STATUS_CRC); - } else { - /* Ensure that SPI interface is clocked at low speed */ - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_256); + /* Activate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ - /* Deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* send 80 clock cycles to start up */ - uint8_t i; - for (i = 0; i < 10; ++i) { - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - } + /* Send STATUS command to check if media is available */ + ret = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_STATUS, 0, SDCMD_SEND_STATUS_CRC); + } else { + /* Ensure that SPI interface is clocked at low speed */ + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_256); - /* Activate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ + /* Deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* send 80 clock cycles to start up */ + uint8_t i; + for (i = 0; i < 10; ++i) { + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + } - /* Send CMD0 to reset the media */ - if ((ret = (PIOS_SDCARD_SendSDCCmd(SDCMD_GO_IDLE_STATE, 0, SDCMD_GO_IDLE_STATE_CRC))) < 0) { - goto not_available; - } + /* Activate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ - /* Deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* Send CMD0 to reset the media */ + if ((ret = (PIOS_SDCARD_SendSDCCmd(SDCMD_GO_IDLE_STATE, 0, SDCMD_GO_IDLE_STATE_CRC))) < 0) { + goto not_available; + } - SDCARD_MUTEX_GIVE; - /* Run power-on sequence (negative return = not available) */ - ret = PIOS_SDCARD_PowerOn(); - } + /* Deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + + SDCARD_MUTEX_GIVE; + /* Run power-on sequence (negative return = not available) */ + ret = PIOS_SDCARD_PowerOn(); + } not_available: - /* Deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* Send dummy byte once deactivated to drop cards DO */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - SDCARD_MUTEX_GIVE; + /* Deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* Send dummy byte once deactivated to drop cards DO */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + SDCARD_MUTEX_GIVE; - return (ret == 0) ? 1 : 0; /* 1 = available, 0 = not available. */ + return (ret == 0) ? 1 : 0; /* 1 = available, 0 = not available. */ } /** -* Sends command to SD card -* \param[in] cmd SD card command -* \param[in] addr 32bit address -* \param[in] crc precalculated CRC -* \return >= 0x00 if command has been sent successfully (contains received byte) -* \return -1 if no response from SD Card (timeout) -*/ + * Sends command to SD card + * \param[in] cmd SD card command + * \param[in] addr 32bit address + * \param[in] crc precalculated CRC + * \return >= 0x00 if command has been sent successfully (contains received byte) + * \return -1 if no response from SD Card (timeout) + */ int32_t PIOS_SDCARD_SendSDCCmd(uint8_t cmd, uint32_t addr, uint8_t crc) { - int i; - int32_t ret; + int i; + int32_t ret; - if (cmd & 0x80) { /* ACMD is the command sequence of CMD55-CMD */ - cmd &= 0x7F; - ret = PIOS_SDCARD_SendSDCCmd(SDCMD_APP_CMD, 0, SDCMD_APP_CMD_CRC); - if (ret > 1) { - return ret; - } - } + if (cmd & 0x80) { /* ACMD is the command sequence of CMD55-CMD */ + cmd &= 0x7F; + ret = PIOS_SDCARD_SendSDCCmd(SDCMD_APP_CMD, 0, SDCMD_APP_CMD_CRC); + if (ret > 1) { + return ret; + } + } - /* Activate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ + /* Activate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ - /* Transfer to card */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (uint8_t) cmd); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 24) & 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 16) & 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 8) & 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 0) & 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, crc); + /* Transfer to card */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (uint8_t)cmd); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 24) & 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 16) & 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 8) & 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 0) & 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, crc); - uint8_t timeout = 0; + uint8_t timeout = 0; - if (cmd == SDCMD_SEND_STATUS) { - /* One dummy read */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + if (cmd == SDCMD_SEND_STATUS) { + /* One dummy read */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* Read two bytes (only last value will be returned) */ - ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Read two bytes (only last value will be returned) */ + ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* All-one read: we expect that SD card is not connected: notify timeout! */ - if (ret == 0xff) { - timeout = 1; - } - } else { - /* Wait for standard R1 response */ - for (i = 0; i < 8; ++i) { - if (!((ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff)) & 0x80)) { - break; - } - } - if (i == 8) { - timeout = 1; - } - } + /* All-one read: we expect that SD card is not connected: notify timeout! */ + if (ret == 0xff) { + timeout = 1; + } + } else { + /* Wait for standard R1 response */ + for (i = 0; i < 8; ++i) { + if (!((ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff)) & 0x80)) { + break; + } + } + if (i == 8) { + timeout = 1; + } + } - if ((cmd == SDCMD_SEND_IF_COND || cmd == SDCMD_READ_OCR) && timeout != 1) { - /* This is a 4 byte R3 or R7 response. */ - ret = (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 24); - ret |= (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 16); - ret |= (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 8); - ret |= (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 0); - } + if ((cmd == SDCMD_SEND_IF_COND || cmd == SDCMD_READ_OCR) && timeout != 1) { + /* This is a 4 byte R3 or R7 response. */ + ret = (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 24); + ret |= (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 16); + ret |= (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 8); + ret |= (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 0); + } - /* Required clocking (see spec) */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Required clocking (see spec) */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* Deactivate chip-select on timeout, and return error code */ - if (timeout) { - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* Deactivate chip-select on timeout, and return error code */ + if (timeout) { + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* Send dummy byte once deactivated to drop cards DO */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - return -1; - } + /* Send dummy byte once deactivated to drop cards DO */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + return -1; + } - /* Else return received value - don't deactivate chip select or mutex (for continuous access) */ - return ret; + /* Else return received value - don't deactivate chip select or mutex (for continuous access) */ + return ret; } /** -* Reads 512 bytes from selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ -int32_t PIOS_SDCARD_SectorRead(uint32_t sector, uint8_t * buffer) + * Reads 512 bytes from selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ +int32_t PIOS_SDCARD_SectorRead(uint32_t sector, uint8_t *buffer) { - int32_t status; - int i; - if (!(CardType & CT_BLOCK)) { - sector *= 512; - } + int32_t status; + int i; - SDCARD_MUTEX_TAKE; + if (!(CardType & CT_BLOCK)) { + sector *= 512; + } - /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ - /* this is required for the case that the SPI port is shared with other devices */ - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); + SDCARD_MUTEX_TAKE; - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_READ_SINGLE_BLOCK, sector, SDCMD_READ_SINGLE_BLOCK_CRC))) { - status = (status < 0) ? -256 : status; /* return timeout indicator or error flags */ - goto error; - } + /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ + /* this is required for the case that the SPI port is shared with other devices */ + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); - /* Wait for start token of the data block */ - for (i = 0; i < 65536; ++i) { // TODO: check if sufficient - uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - if (ret != 0xff) - break; - } + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_READ_SINGLE_BLOCK, sector, SDCMD_READ_SINGLE_BLOCK_CRC))) { + status = (status < 0) ? -256 : status; /* return timeout indicator or error flags */ + goto error; + } - if (i == 65536) { - status = -257; - goto error; - } + /* Wait for start token of the data block */ + for (i = 0; i < 65536; ++i) { // TODO: check if sufficient + uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + if (ret != 0xff) { + break; + } + } - /* Read 512 bytes via DMA */ - PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, NULL, buffer, 512, NULL); + if (i == 65536) { + status = -257; + goto error; + } - /* Read (and ignore) CRC */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Read 512 bytes via DMA */ + PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, NULL, buffer, 512, NULL); - /* Required for clocking (see spec) */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Read (and ignore) CRC */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + + /* Required for clocking (see spec) */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); error: - /* Deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); // spi, pin_value + /* Deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); // spi, pin_value - /* Send dummy byte once deactivated to drop cards DO */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - SDCARD_MUTEX_GIVE; - return status; + /* Send dummy byte once deactivated to drop cards DO */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + SDCARD_MUTEX_GIVE; + return status; } /** -* Writes 512 bytes into selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if write operation not accepted -* \return -258 if timeout during write operation -*/ -int32_t PIOS_SDCARD_SectorWrite(uint32_t sector, uint8_t * buffer) + * Writes 512 bytes into selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if write operation not accepted + * \return -258 if timeout during write operation + */ +int32_t PIOS_SDCARD_SectorWrite(uint32_t sector, uint8_t *buffer) { - int32_t status; - int i; + int32_t status; + int i; - SDCARD_MUTEX_TAKE; + SDCARD_MUTEX_TAKE; - if (!(CardType & CT_BLOCK)) { - sector *= 512; - } + if (!(CardType & CT_BLOCK)) { + sector *= 512; + } - /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ - /* This is required for the case that the SPI port is shared with other devices */ - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); + /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ + /* This is required for the case that the SPI port is shared with other devices */ + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_WRITE_SINGLE_BLOCK, sector, SDCMD_WRITE_SINGLE_BLOCK_CRC))) { - status = (status < 0) ? -256 : status; /* Return timeout indicator or error flags */ - goto error; - } + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_WRITE_SINGLE_BLOCK, sector, SDCMD_WRITE_SINGLE_BLOCK_CRC))) { + status = (status < 0) ? -256 : status; /* Return timeout indicator or error flags */ + goto error; + } - /* Send start token */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xfe); + /* Send start token */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xfe); - /* Send 512 bytes of data via DMA */ - PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, buffer, NULL, 512, NULL); + /* Send 512 bytes of data via DMA */ + PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, buffer, NULL, 512, NULL); - /* Send CRC */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Send CRC */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* Read response */ - uint8_t response = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - if ((response & 0x0f) != 0x5) { - status = -257; - goto error; - } + /* Read response */ + uint8_t response = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + if ((response & 0x0f) != 0x5) { + status = -257; + goto error; + } - /* Wait for write completion */ - for (i = 0; i < 32 * 65536; ++i) { /* TODO: check if sufficient */ - uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - if (ret != 0x00) { - break; - } - } - if (i == 32 * 65536) { - status = -258; - goto error; - } + /* Wait for write completion */ + for (i = 0; i < 32 * 65536; ++i) { /* TODO: check if sufficient */ + uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + if (ret != 0x00) { + break; + } + } + if (i == 32 * 65536) { + status = -258; + goto error; + } - /* Required for clocking (see spec) */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Required for clocking (see spec) */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); error: - /* Deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* Send dummy byte once deactivated to drop cards DO */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* Send dummy byte once deactivated to drop cards DO */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - SDCARD_MUTEX_GIVE; + SDCARD_MUTEX_GIVE; - return status; + return status; } /** -* Reads the CID informations from SD Card -* \param[in] *cid pointer to buffer which holds the CID informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ -int32_t PIOS_SDCARD_CIDRead(SDCARDCidTypeDef * cid) + * Reads the CID informations from SD Card + * \param[in] *cid pointer to buffer which holds the CID informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ +int32_t PIOS_SDCARD_CIDRead(SDCARDCidTypeDef *cid) { - int32_t status; - int i; + int32_t status; + int i; - /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ - /* This is required for the case that the SPI port is shared with other devices */ - SDCARD_MUTEX_TAKE; + /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ + /* This is required for the case that the SPI port is shared with other devices */ + SDCARD_MUTEX_TAKE; - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_CID, 0, SDCMD_SEND_CID_CRC))) { - status = (status < 0) ? -256 : status; /* return timeout indicator or error flags */ - goto error; - } + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_CID, 0, SDCMD_SEND_CID_CRC))) { + status = (status < 0) ? -256 : status; /* return timeout indicator or error flags */ + goto error; + } - /* Wait for start token of the data block */ - for (i = 0; i < 65536; ++i) { /* TODO: check if sufficient */ - uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - if (ret != 0xff) { - break; - } - } - if (i == 65536) { - status = -257; - } + /* Wait for start token of the data block */ + for (i = 0; i < 65536; ++i) { /* TODO: check if sufficient */ + uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + if (ret != 0xff) { + break; + } + } + if (i == 65536) { + status = -257; + } - /* Read 16 bytes via DMA */ - uint8_t cid_buffer[16]; - PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, NULL, cid_buffer, 16, NULL); + /* Read 16 bytes via DMA */ + uint8_t cid_buffer[16]; + PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, NULL, cid_buffer, 16, NULL); - /* Read (and ignore) CRC */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Read (and ignore) CRC */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* Required for clocking (see spec) */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Required for clocking (see spec) */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* Sort returned informations into CID structure */ - /* From STM Mass Storage example */ - /* Byte 0 */ - cid->ManufacturerID = cid_buffer[0]; - /* Byte 1 */ - cid->OEM_AppliID = cid_buffer[1] << 8; - /* Byte 2 */ - cid->OEM_AppliID |= cid_buffer[2]; - /* Byte 3..7 */ - for (i = 0; i < 5; ++i) - cid->ProdName[i] = cid_buffer[3 + i]; - cid->ProdName[5] = 0; /* string terminator */ - /* Byte 8 */ - cid->ProdRev = cid_buffer[8]; - /* Byte 9 */ - cid->ProdSN = cid_buffer[9] << 24; - /* Byte 10 */ - cid->ProdSN |= cid_buffer[10] << 16; - /* Byte 11 */ - cid->ProdSN |= cid_buffer[11] << 8; - /* Byte 12 */ - cid->ProdSN |= cid_buffer[12]; - /* Byte 13 */ - cid->Reserved1 |= (cid_buffer[13] & 0xF0) >> 4; - /* Byte 14 */ - cid->ManufactDate = (cid_buffer[13] & 0x0F) << 8; - /* Byte 15 */ - cid->ManufactDate |= cid_buffer[14]; - /* Byte 16 */ - cid->msd_CRC = (cid_buffer[15] & 0xFE) >> 1; - cid->Reserved2 = 1; + /* Sort returned informations into CID structure */ + /* From STM Mass Storage example */ + /* Byte 0 */ + cid->ManufacturerID = cid_buffer[0]; + /* Byte 1 */ + cid->OEM_AppliID = cid_buffer[1] << 8; + /* Byte 2 */ + cid->OEM_AppliID |= cid_buffer[2]; + /* Byte 3..7 */ + for (i = 0; i < 5; ++i) { + cid->ProdName[i] = cid_buffer[3 + i]; + } + cid->ProdName[5] = 0; /* string terminator */ + /* Byte 8 */ + cid->ProdRev = cid_buffer[8]; + /* Byte 9 */ + cid->ProdSN = cid_buffer[9] << 24; + /* Byte 10 */ + cid->ProdSN |= cid_buffer[10] << 16; + /* Byte 11 */ + cid->ProdSN |= cid_buffer[11] << 8; + /* Byte 12 */ + cid->ProdSN |= cid_buffer[12]; + /* Byte 13 */ + cid->Reserved1 |= (cid_buffer[13] & 0xF0) >> 4; + /* Byte 14 */ + cid->ManufactDate = (cid_buffer[13] & 0x0F) << 8; + /* Byte 15 */ + cid->ManufactDate |= cid_buffer[14]; + /* Byte 16 */ + cid->msd_CRC = (cid_buffer[15] & 0xFE) >> 1; + cid->Reserved2 = 1; error: - /* deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* Send dummy byte once deactivated to drop cards DO */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - SDCARD_MUTEX_GIVE; + /* deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* Send dummy byte once deactivated to drop cards DO */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + SDCARD_MUTEX_GIVE; - return status; + return status; } /** -* Reads the CSD informations from SD Card -* \param[in] *csd pointer to buffer which holds the CSD informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ -int32_t PIOS_SDCARD_CSDRead(SDCARDCsdTypeDef * csd) + * Reads the CSD informations from SD Card + * \param[in] *csd pointer to buffer which holds the CSD informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ +int32_t PIOS_SDCARD_CSDRead(SDCARDCsdTypeDef *csd) { - int32_t status; - int i; + int32_t status; + int i; - SDCARD_MUTEX_TAKE; + SDCARD_MUTEX_TAKE; - /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ - /* This is required for the case that the SPI port is shared with other devices */ - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); + /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ + /* This is required for the case that the SPI port is shared with other devices */ + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_CSD, 0, SDCMD_SEND_CSD_CRC))) { - status = (status < 0) ? -256 : status; /* Return timeout indicator or error flags */ - goto error; - } - // wait for start token of the data block - for (i = 0; i < 65536; ++i) { /* TODO: check if sufficient */ - uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - if (ret != 0xff) { - break; - } - } - if (i == 65536) { - status = -257; - goto error; - } + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_CSD, 0, SDCMD_SEND_CSD_CRC))) { + status = (status < 0) ? -256 : status; /* Return timeout indicator or error flags */ + goto error; + } + // wait for start token of the data block + for (i = 0; i < 65536; ++i) { /* TODO: check if sufficient */ + uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + if (ret != 0xff) { + break; + } + } + if (i == 65536) { + status = -257; + goto error; + } - /* Read 16 bytes via DMA */ - uint8_t csd_buffer[16]; - PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, NULL, csd_buffer, 16, NULL); + /* Read 16 bytes via DMA */ + uint8_t csd_buffer[16]; + PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, NULL, csd_buffer, 16, NULL); - /* Read (and ignore) CRC */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Read (and ignore) CRC */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* Required for clocking (see spec) */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Required for clocking (see spec) */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* Sort returned informations into CSD structure */ - /* from STM Mass Storage example */ - /* Byte 0 */ - csd->CSDStruct = (csd_buffer[0] & 0xC0) >> 6; - csd->SysSpecVersion = (csd_buffer[0] & 0x3C) >> 2; - csd->Reserved1 = csd_buffer[0] & 0x03; - /* Byte 1 */ - csd->TAAC = csd_buffer[1]; - /* Byte 2 */ - csd->NSAC = csd_buffer[2]; - /* Byte 3 */ - csd->MaxBusClkFrec = csd_buffer[3]; - /* Byte 4 */ - csd->CardComdClasses = csd_buffer[4] << 4; - /* Byte 5 */ - csd->CardComdClasses |= (csd_buffer[5] & 0xF0) >> 4; - csd->RdBlockLen = csd_buffer[5] & 0x0F; - /* Byte 6 */ - csd->PartBlockRead = (csd_buffer[6] & 0x80) >> 7; - csd->WrBlockMisalign = (csd_buffer[6] & 0x40) >> 6; - csd->RdBlockMisalign = (csd_buffer[6] & 0x20) >> 5; - csd->DSRImpl = (csd_buffer[6] & 0x10) >> 4; - csd->Reserved2 = 0; /* Reserved */ - csd->DeviceSize = (csd_buffer[6] & 0x03) << 10; - /* Byte 7 */ - csd->DeviceSize |= (csd_buffer[7]) << 2; - /* Byte 8 */ - csd->DeviceSize |= (csd_buffer[8] & 0xC0) >> 6; - csd->MaxRdCurrentVDDMin = (csd_buffer[8] & 0x38) >> 3; - csd->MaxRdCurrentVDDMax = (csd_buffer[8] & 0x07); - /* Byte 9 */ - csd->MaxWrCurrentVDDMin = (csd_buffer[9] & 0xE0) >> 5; - csd->MaxWrCurrentVDDMax = (csd_buffer[9] & 0x1C) >> 2; - csd->DeviceSizeMul = (csd_buffer[9] & 0x03) << 1; - /* Byte 10 */ - csd->DeviceSizeMul |= (csd_buffer[10] & 0x80) >> 7; - csd->EraseGrSize = (csd_buffer[10] & 0x7C) >> 2; - csd->EraseGrMul = (csd_buffer[10] & 0x03) << 3; - /* Byte 11 */ - csd->EraseGrMul |= (csd_buffer[11] & 0xE0) >> 5; - csd->WrProtectGrSize = (csd_buffer[11] & 0x1F); - /* Byte 12 */ - csd->WrProtectGrEnable = (csd_buffer[12] & 0x80) >> 7; - csd->ManDeflECC = (csd_buffer[12] & 0x60) >> 5; - csd->WrSpeedFact = (csd_buffer[12] & 0x1C) >> 2; - csd->MaxWrBlockLen = (csd_buffer[12] & 0x03) << 2; - /* Byte 13 */ - csd->MaxWrBlockLen |= (csd_buffer[13] & 0xc0) >> 6; - csd->WriteBlockPaPartial = (csd_buffer[13] & 0x20) >> 5; - csd->Reserved3 = 0; - csd->ContentProtectAppli = (csd_buffer[13] & 0x01); - /* Byte 14 */ - csd->FileFormatGrouop = (csd_buffer[14] & 0x80) >> 7; - csd->CopyFlag = (csd_buffer[14] & 0x40) >> 6; - csd->PermWrProtect = (csd_buffer[14] & 0x20) >> 5; - csd->TempWrProtect = (csd_buffer[14] & 0x10) >> 4; - csd->FileFormat = (csd_buffer[14] & 0x0C) >> 2; - csd->ECC = (csd_buffer[14] & 0x03); - /* Byte 15 */ - csd->msd_CRC = (csd_buffer[15] & 0xFE) >> 1; - csd->Reserved4 = 1; + /* Sort returned informations into CSD structure */ + /* from STM Mass Storage example */ + /* Byte 0 */ + csd->CSDStruct = (csd_buffer[0] & 0xC0) >> 6; + csd->SysSpecVersion = (csd_buffer[0] & 0x3C) >> 2; + csd->Reserved1 = csd_buffer[0] & 0x03; + /* Byte 1 */ + csd->TAAC = csd_buffer[1]; + /* Byte 2 */ + csd->NSAC = csd_buffer[2]; + /* Byte 3 */ + csd->MaxBusClkFrec = csd_buffer[3]; + /* Byte 4 */ + csd->CardComdClasses = csd_buffer[4] << 4; + /* Byte 5 */ + csd->CardComdClasses |= (csd_buffer[5] & 0xF0) >> 4; + csd->RdBlockLen = csd_buffer[5] & 0x0F; + /* Byte 6 */ + csd->PartBlockRead = (csd_buffer[6] & 0x80) >> 7; + csd->WrBlockMisalign = (csd_buffer[6] & 0x40) >> 6; + csd->RdBlockMisalign = (csd_buffer[6] & 0x20) >> 5; + csd->DSRImpl = (csd_buffer[6] & 0x10) >> 4; + csd->Reserved2 = 0; /* Reserved */ + csd->DeviceSize = (csd_buffer[6] & 0x03) << 10; + /* Byte 7 */ + csd->DeviceSize |= (csd_buffer[7]) << 2; + /* Byte 8 */ + csd->DeviceSize |= (csd_buffer[8] & 0xC0) >> 6; + csd->MaxRdCurrentVDDMin = (csd_buffer[8] & 0x38) >> 3; + csd->MaxRdCurrentVDDMax = (csd_buffer[8] & 0x07); + /* Byte 9 */ + csd->MaxWrCurrentVDDMin = (csd_buffer[9] & 0xE0) >> 5; + csd->MaxWrCurrentVDDMax = (csd_buffer[9] & 0x1C) >> 2; + csd->DeviceSizeMul = (csd_buffer[9] & 0x03) << 1; + /* Byte 10 */ + csd->DeviceSizeMul |= (csd_buffer[10] & 0x80) >> 7; + csd->EraseGrSize = (csd_buffer[10] & 0x7C) >> 2; + csd->EraseGrMul = (csd_buffer[10] & 0x03) << 3; + /* Byte 11 */ + csd->EraseGrMul |= (csd_buffer[11] & 0xE0) >> 5; + csd->WrProtectGrSize = (csd_buffer[11] & 0x1F); + /* Byte 12 */ + csd->WrProtectGrEnable = (csd_buffer[12] & 0x80) >> 7; + csd->ManDeflECC = (csd_buffer[12] & 0x60) >> 5; + csd->WrSpeedFact = (csd_buffer[12] & 0x1C) >> 2; + csd->MaxWrBlockLen = (csd_buffer[12] & 0x03) << 2; + /* Byte 13 */ + csd->MaxWrBlockLen |= (csd_buffer[13] & 0xc0) >> 6; + csd->WriteBlockPaPartial = (csd_buffer[13] & 0x20) >> 5; + csd->Reserved3 = 0; + csd->ContentProtectAppli = (csd_buffer[13] & 0x01); + /* Byte 14 */ + csd->FileFormatGrouop = (csd_buffer[14] & 0x80) >> 7; + csd->CopyFlag = (csd_buffer[14] & 0x40) >> 6; + csd->PermWrProtect = (csd_buffer[14] & 0x20) >> 5; + csd->TempWrProtect = (csd_buffer[14] & 0x10) >> 4; + csd->FileFormat = (csd_buffer[14] & 0x0C) >> 2; + csd->ECC = (csd_buffer[14] & 0x03); + /* Byte 15 */ + csd->msd_CRC = (csd_buffer[15] & 0xFE) >> 1; + csd->Reserved4 = 1; error: - /* Deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* Send dummy byte once deactivated to drop cards DO */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - SDCARD_MUTEX_GIVE; + /* Deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* Send dummy byte once deactivated to drop cards DO */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + SDCARD_MUTEX_GIVE; - return status; + return status; } /** -* Attempts to write a startup log to the SDCard -* return 0 No errors -* return -1 Error deleting file -* return -2 Error opening file -* return -3 Error writing file -*/ + * Attempts to write a startup log to the SDCard + * return 0 No errors + * return -1 Error deleting file + * return -2 Error opening file + * return -3 Error writing file + */ int32_t PIOS_SDCARD_StartupLog(void) { - FILEINFO File; - char Buffer[1024]; - uint32_t Cache; + FILEINFO File; + char Buffer[1024]; + uint32_t Cache; - /* Delete the file if it exists - ignore errors */ - DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *) LOG_FILENAME, PIOS_SDCARD_Sector); + /* Delete the file if it exists - ignore errors */ + DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)LOG_FILENAME, PIOS_SDCARD_Sector); - if (DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *) LOG_FILENAME, DFS_WRITE, PIOS_SDCARD_Sector, &File)) { - /* Error opening file */ - return -2; - } + if (DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)LOG_FILENAME, DFS_WRITE, PIOS_SDCARD_Sector, &File)) { + /* Error opening file */ + return -2; + } - sprintf(Buffer, "PiOS Startup Log\r\n\r\nLog file creation completed.\r\n"); - if (DFS_WriteFile(&File, PIOS_SDCARD_Sector, (uint8_t *) Buffer, &Cache, strlen(Buffer))) { - /* Error writing to file */ - return -3; - } + sprintf(Buffer, "PiOS Startup Log\r\n\r\nLog file creation completed.\r\n"); + if (DFS_WriteFile(&File, PIOS_SDCARD_Sector, (uint8_t *)Buffer, &Cache, strlen(Buffer))) { + /* Error writing to file */ + return -3; + } - sprintf(Buffer, "------------------------------\r\nSD Card Information\r\n------------------------------\r\n"); - if (DFS_WriteFile(&File, PIOS_SDCARD_Sector, (uint8_t *) Buffer, &Cache, strlen(Buffer))) { - /* Error writing to file */ - return -2; - } + sprintf(Buffer, "------------------------------\r\nSD Card Information\r\n------------------------------\r\n"); + if (DFS_WriteFile(&File, PIOS_SDCARD_Sector, (uint8_t *)Buffer, &Cache, strlen(Buffer))) { + /* Error writing to file */ + return -2; + } - /* Disabled because it takes ca. 7 seconds with my 2GB card */ - /* sprintf(Buffer, "Total Space: %u MB\r\nFree Space: %u MB\r\n", (uint16_t)((VolInfo.numclusters * (VolInfo.secperclus / 2)) / 1024), (uint16_t)(PIOS_SDCARD_GetFree() / 1048576)); */ + /* Disabled because it takes ca. 7 seconds with my 2GB card */ + /* sprintf(Buffer, "Total Space: %u MB\r\nFree Space: %u MB\r\n", (uint16_t)((VolInfo.numclusters * (VolInfo.secperclus / 2)) / 1024), (uint16_t)(PIOS_SDCARD_GetFree() / 1048576)); */ - sprintf(Buffer, "Total Space: %u MB\r\n\r\n", (uint16_t) ((PIOS_SDCARD_VolInfo.numclusters * (PIOS_SDCARD_VolInfo.secperclus / 2)) / 1024)); - if (DFS_WriteFile(&File, PIOS_SDCARD_Sector, (uint8_t *) Buffer, &Cache, strlen(Buffer))) { - /* Error writing to file */ - return -2; - } + sprintf(Buffer, "Total Space: %u MB\r\n\r\n", (uint16_t)((PIOS_SDCARD_VolInfo.numclusters * (PIOS_SDCARD_VolInfo.secperclus / 2)) / 1024)); + if (DFS_WriteFile(&File, PIOS_SDCARD_Sector, (uint8_t *)Buffer, &Cache, strlen(Buffer))) { + /* Error writing to file */ + return -2; + } - /* No errors */ - return 0; + /* No errors */ + return 0; } /** @@ -807,201 +813,201 @@ int32_t PIOS_SDCARD_StartupLog(void) */ int32_t PIOS_SDCARD_IsMounted() { - return sdcard_mounted; + return sdcard_mounted; } /** -* Mounts the file system -* param[in] CreateStartupLog 1 = True, 0 = False -* return 0 No errors -* return -1 SDCard not available -* return -2 Cannot find first partition -* return -3 No volume information -* return -4 Error writing startup log file -*/ + * Mounts the file system + * param[in] CreateStartupLog 1 = True, 0 = False + * return 0 No errors + * return -1 SDCard not available + * return -2 Cannot find first partition + * return -3 No volume information + * return -4 Error writing startup log file + */ int32_t PIOS_SDCARD_MountFS(uint32_t CreateStartupLog) { - uint32_t pstart, psize; - uint8_t pactive, ptype; - uint8_t sdcard_available = 0; + uint32_t pstart, psize; + uint8_t pactive, ptype; + uint8_t sdcard_available = 0; - sdcard_available = PIOS_SDCARD_CheckAvailable(0); + sdcard_available = PIOS_SDCARD_CheckAvailable(0); - if (!sdcard_available) { - /* Disconnected */ - return -1; - } + if (!sdcard_available) { + /* Disconnected */ + return -1; + } - pstart = DFS_GetPtnStart(0, PIOS_SDCARD_Sector, 0, &pactive, &ptype, &psize); - if (pstart == 0xffffffff) { - /* Cannot find first partition */ - return -2; - } + pstart = DFS_GetPtnStart(0, PIOS_SDCARD_Sector, 0, &pactive, &ptype, &psize); + if (pstart == 0xffffffff) { + /* Cannot find first partition */ + return -2; + } - if (DFS_GetVolInfo(0, PIOS_SDCARD_Sector, pstart, &PIOS_SDCARD_VolInfo) != DFS_OK) { - /* No volume information */ - return -3; - } + if (DFS_GetVolInfo(0, PIOS_SDCARD_Sector, pstart, &PIOS_SDCARD_VolInfo) != DFS_OK) { + /* No volume information */ + return -3; + } - if (CreateStartupLog == 1) { - if (PIOS_SDCARD_StartupLog()) { - /* Error writing startup log file */ - return -4; - } - } + if (CreateStartupLog == 1) { + if (PIOS_SDCARD_StartupLog()) { + /* Error writing startup log file */ + return -4; + } + } - /* No errors */ - sdcard_mounted = 1; - return 0; + /* No errors */ + sdcard_mounted = 1; + return 0; } /** -* Mounts the file system -* return Amount of free bytes -*/ + * Mounts the file system + * return Amount of free bytes + */ int32_t PIOS_SDCARD_GetFree(void) { - uint32_t VolFreeBytes = 0xffffffff; - uint32_t ScratchCache = 0; + uint32_t VolFreeBytes = 0xffffffff; + uint32_t ScratchCache = 0; - /* This takes very long, since it scans ALL clusters */ - /* It takes ca. 7 seconds to determine free clusters out of ~47000 (2Gb) */ + /* This takes very long, since it scans ALL clusters */ + /* It takes ca. 7 seconds to determine free clusters out of ~47000 (2Gb) */ - /* Scan all the clusters */ - for (uint32_t i = 2; i < PIOS_SDCARD_VolInfo.numclusters; ++i) { - if (!DFS_GetFAT(&PIOS_SDCARD_VolInfo, PIOS_SDCARD_Sector, &ScratchCache, i)) { - VolFreeBytes += PIOS_SDCARD_VolInfo.secperclus * SECTOR_SIZE; - } - } + /* Scan all the clusters */ + for (uint32_t i = 2; i < PIOS_SDCARD_VolInfo.numclusters; ++i) { + if (!DFS_GetFAT(&PIOS_SDCARD_VolInfo, PIOS_SDCARD_Sector, &ScratchCache, i)) { + VolFreeBytes += PIOS_SDCARD_VolInfo.secperclus * SECTOR_SIZE; + } + } - return VolFreeBytes; + return VolFreeBytes; } /** -* Read from file -* return 0 No error -* return -1 DFS_ReadFile failed -* return -2 Less bytes read than expected -*/ -int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t * buffer, uint32_t len) + * Read from file + * return 0 No error + * return -1 DFS_ReadFile failed + * return -2 Less bytes read than expected + */ +int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len) { - uint32_t SuccessCount; + uint32_t SuccessCount; - if (DFS_ReadFile(fileinfo, PIOS_SDCARD_Sector, buffer, &SuccessCount, len)) { - /* DFS_ReadFile failed */ - return -1; - } - if (SuccessCount != len) { - /* Less bytes read than expected */ - return -2; - } + if (DFS_ReadFile(fileinfo, PIOS_SDCARD_Sector, buffer, &SuccessCount, len)) { + /* DFS_ReadFile failed */ + return -1; + } + if (SuccessCount != len) { + /* Less bytes read than expected */ + return -2; + } - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Read a line from file -* returns Number of bytes read -*/ -int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t * buffer, uint32_t max_len) + * Read a line from file + * returns Number of bytes read + */ +int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len) { - int32_t status; - uint32_t num_read = 0; + int32_t status; + uint32_t num_read = 0; - while (fileinfo->pointer < fileinfo->filelen) { - status = PIOS_SDCARD_ReadBuffer(fileinfo, buffer, 1); + while (fileinfo->pointer < fileinfo->filelen) { + status = PIOS_SDCARD_ReadBuffer(fileinfo, buffer, 1); - if (status < 0) { - return status; - } + if (status < 0) { + return status; + } - ++num_read; + ++num_read; - if (*buffer == '\n' || *buffer == '\r') { - break; - } + if (*buffer == '\n' || *buffer == '\r') { + break; + } - if (num_read < max_len) { - ++buffer; - } - } + if (num_read < max_len) { + ++buffer; + } + } - /* Replace newline by terminator */ - *buffer = 0; + /* Replace newline by terminator */ + *buffer = 0; - return num_read; + return num_read; } /** -* Copy a file -* WARNING: This will overwrite the destination file even if it exists -* param[in] *Source Path to file to copy -* param[in] *Destination Path to destination file -* return 0 No errors -* return -1 Source file doesn't exist -* return -2 Failed to create destination file -* return -3 DFS_ReadFile failed -* return -4 DFS_WriteFile failed -*/ + * Copy a file + * WARNING: This will overwrite the destination file even if it exists + * param[in] *Source Path to file to copy + * param[in] *Destination Path to destination file + * return 0 No errors + * return -1 Source file doesn't exist + * return -2 Failed to create destination file + * return -3 DFS_ReadFile failed + * return -4 DFS_WriteFile failed + */ int32_t PIOS_SDCARD_FileCopy(char *Source, char *Destination) { - FILEINFO SourceFile, DestFile; + FILEINFO SourceFile, DestFile; - /* Disable caching to avoid file inconsistencies while using different sector buffers! */ - DFS_CachingEnabledSet(0); + /* Disable caching to avoid file inconsistencies while using different sector buffers! */ + DFS_CachingEnabledSet(0); - if (DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *) Source, DFS_READ, PIOS_SDCARD_Sector, &SourceFile)) { - /* Source file doesn't exist */ - return -1; - } else { - /* Delete destination file if it already exists - ignore errors */ - DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *) Destination, PIOS_SDCARD_Sector); + if (DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)Source, DFS_READ, PIOS_SDCARD_Sector, &SourceFile)) { + /* Source file doesn't exist */ + return -1; + } else { + /* Delete destination file if it already exists - ignore errors */ + DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)Destination, PIOS_SDCARD_Sector); - if (DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *) Destination, DFS_WRITE, PIOS_SDCARD_Sector, &DestFile)) { - /* Failed to create destination file */ - return -2; - } - } + if (DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)Destination, DFS_WRITE, PIOS_SDCARD_Sector, &DestFile)) { + /* Failed to create destination file */ + return -2; + } + } - /* Copy operation */ - uint8_t WriteBuffer[SECTOR_SIZE]; - uint32_t SuccessCountRead; - uint32_t SuccessCountWrite; - do { - if (DFS_ReadFile(&SourceFile, PIOS_SDCARD_Sector, WriteBuffer, &SuccessCountRead, SECTOR_SIZE)) { - /* DFS_ReadFile failed */ - return -3; - } else if (DFS_WriteFile(&DestFile, PIOS_SDCARD_Sector, WriteBuffer, &SuccessCountWrite, SuccessCountRead)) { - /* DFS_WriteFile failed */ - return -4; - } - } while (SuccessCountRead > 0); + /* Copy operation */ + uint8_t WriteBuffer[SECTOR_SIZE]; + uint32_t SuccessCountRead; + uint32_t SuccessCountWrite; + do { + if (DFS_ReadFile(&SourceFile, PIOS_SDCARD_Sector, WriteBuffer, &SuccessCountRead, SECTOR_SIZE)) { + /* DFS_ReadFile failed */ + return -3; + } else if (DFS_WriteFile(&DestFile, PIOS_SDCARD_Sector, WriteBuffer, &SuccessCountWrite, SuccessCountRead)) { + /* DFS_WriteFile failed */ + return -4; + } + } while (SuccessCountRead > 0); - /* No errors */ - return 0; + /* No errors */ + return 0; } /** -* Delete a file -* param[in] *Filename File to delete -* return 0 No errors -* return -1 Error deleting file -*/ + * Delete a file + * param[in] *Filename File to delete + * return 0 No errors + * return -1 Error deleting file + */ int32_t PIOS_SDCARD_FileDelete(char *Filename) { - if (DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *) Filename, PIOS_SDCARD_Sector)) { - /* Error deleting file */ - return -1; - } + if (DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)Filename, PIOS_SDCARD_Sector)) { + /* Error deleting file */ + return -1; + } - /* No errors */ - return 0; + /* No errors */ + return 0; } #endif /* PIOS_INCLUDE_SDCARD */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/common/pios_task_monitor.c b/flight/pios/common/pios_task_monitor.c index c3e3f27d0..7a3c13234 100644 --- a/flight/pios/common/pios_task_monitor.c +++ b/flight/pios/common/pios_task_monitor.c @@ -30,45 +30,49 @@ #ifdef PIOS_INCLUDE_TASK_MONITOR // Private variables -static xSemaphoreHandle mLock; -static xTaskHandle *mTaskHandles; -static uint32_t mLastMonitorTime; -static uint16_t mMaxTasks; +static xSemaphoreHandle mLock; +static xTaskHandle *mTaskHandles; +static uint32_t mLastMonitorTime; +static uint16_t mMaxTasks; /** * Initialize the Task Monitor */ int32_t PIOS_TASK_MONITOR_Initialize(uint16_t max_tasks) { - mLock = xSemaphoreCreateRecursiveMutex(); - if (!mLock) { return -1; } + mLock = xSemaphoreCreateRecursiveMutex(); + if (!mLock) { + return -1; + } - mTaskHandles = (xTaskHandle*)pvPortMalloc(max_tasks * sizeof(xTaskHandle)); - if (!mTaskHandles) { return -1; } - memset(mTaskHandles, 0, max_tasks * sizeof(xTaskHandle)); + mTaskHandles = (xTaskHandle *)pvPortMalloc(max_tasks * sizeof(xTaskHandle)); + if (!mTaskHandles) { + return -1; + } + memset(mTaskHandles, 0, max_tasks * sizeof(xTaskHandle)); - mMaxTasks = max_tasks; + mMaxTasks = max_tasks; #if (configGENERATE_RUN_TIME_STATS == 1) - mLastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE(); + mLastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE(); #else - mLastMonitorTime = 0; + mLastMonitorTime = 0; #endif - return 0; + return 0; } /** - * Register a task handle + * Register a task handle */ int32_t PIOS_TASK_MONITOR_RegisterTask(uint16_t task_id, xTaskHandle handle) { - if (mTaskHandles && task_id < mMaxTasks) { - xSemaphoreTakeRecursive(mLock, portMAX_DELAY); - mTaskHandles[task_id] = handle; - xSemaphoreGiveRecursive(mLock); - return 0; - } else { - return -1; - } + if (mTaskHandles && task_id < mMaxTasks) { + xSemaphoreTakeRecursive(mLock, portMAX_DELAY); + mTaskHandles[task_id] = handle; + xSemaphoreGiveRecursive(mLock); + return 0; + } else { + return -1; + } } /** @@ -76,14 +80,14 @@ int32_t PIOS_TASK_MONITOR_RegisterTask(uint16_t task_id, xTaskHandle handle) */ int32_t PIOS_TASK_MONITOR_UnregisterTask(uint16_t task_id) { - if (mTaskHandles && task_id < mMaxTasks) { - xSemaphoreTakeRecursive(mLock, portMAX_DELAY); - mTaskHandles[task_id] = 0; - xSemaphoreGiveRecursive(mLock); - return 0; - } else { - return -1; - } + if (mTaskHandles && task_id < mMaxTasks) { + xSemaphoreTakeRecursive(mLock, portMAX_DELAY); + mTaskHandles[task_id] = 0; + xSemaphoreGiveRecursive(mLock); + return 0; + } else { + return -1; + } } /** @@ -91,7 +95,7 @@ int32_t PIOS_TASK_MONITOR_UnregisterTask(uint16_t task_id) */ bool PIOS_TASK_MONITOR_IsRunning(uint16_t task_id) { - return (mTaskHandles && task_id <= mMaxTasks && mTaskHandles[task_id]); + return mTaskHandles && task_id <= mMaxTasks && mTaskHandles[task_id]; } /** @@ -99,46 +103,47 @@ bool PIOS_TASK_MONITOR_IsRunning(uint16_t task_id) */ void PIOS_TASK_MONITOR_ForEachTask(TaskMonitorTaskInfoCallback callback, void *context) { - if (!mTaskHandles) { return; } + if (!mTaskHandles) { + return; + } - xSemaphoreTakeRecursive(mLock, portMAX_DELAY); + xSemaphoreTakeRecursive(mLock, portMAX_DELAY); #if (configGENERATE_RUN_TIME_STATS == 1) - /* Calculate the amount of elapsed run time between the last time we - * measured and now. Scale so that we can convert task run times - * directly to percentages. */ - uint32_t currentTime = portGET_RUN_TIME_COUNTER_VALUE(); - /* avoid divide-by-zero if the interval is too small */ - uint32_t deltaTime = ((currentTime - mLastMonitorTime)/100) ? : 1; - mLastMonitorTime = currentTime; + /* Calculate the amount of elapsed run time between the last time we + * measured and now. Scale so that we can convert task run times + * directly to percentages. */ + uint32_t currentTime = portGET_RUN_TIME_COUNTER_VALUE(); + /* avoid divide-by-zero if the interval is too small */ + uint32_t deltaTime = ((currentTime - mLastMonitorTime) / 100) ? : 1; + mLastMonitorTime = currentTime; #endif - /* Update all task information */ - for (uint16_t n = 0; n < mMaxTasks; ++n) { - struct pios_task_info info; - if (mTaskHandles[n]) { - info.is_running = true; + /* Update all task information */ + for (uint16_t n = 0; n < mMaxTasks; ++n) { + struct pios_task_info info; + if (mTaskHandles[n]) { + info.is_running = true; #if defined(ARCH_POSIX) || defined(ARCH_WIN32) - info.stack_remaining = 10000; + info.stack_remaining = 10000; #else - info.stack_remaining = uxTaskGetStackHighWaterMark(mTaskHandles[n]) * 4; + info.stack_remaining = uxTaskGetStackHighWaterMark(mTaskHandles[n]) * 4; #endif #if (configGENERATE_RUN_TIME_STATS == 1) - /* Generate run time percentage stats */ - info.running_time_percentage = uxTaskGetRunTime(mTaskHandles[n])/deltaTime; + /* Generate run time percentage stats */ + info.running_time_percentage = uxTaskGetRunTime(mTaskHandles[n]) / deltaTime; #else - info.running_time_percentage = 0; -#endif - } else { - info.is_running = false; - info.stack_remaining = 0; - info.running_time_percentage = 0; - } - /* Pass the information for this task back to the caller */ - callback(n, &info, context); - } + info.running_time_percentage = 0; +#endif + } else { + info.is_running = false; + info.stack_remaining = 0; + info.running_time_percentage = 0; + } + /* Pass the information for this task back to the caller */ + callback(n, &info, context); + } - xSemaphoreGiveRecursive(mLock); + xSemaphoreGiveRecursive(mLock); } #endif // PIOS_INCLUDE_TASK_MONITOR - diff --git a/flight/pios/common/pios_usb_desc_hid_cdc.c b/flight/pios/common/pios_usb_desc_hid_cdc.c index e102aefe3..137578c94 100644 --- a/flight/pios/common/pios_usb_desc_hid_cdc.c +++ b/flight/pios/common/pios_usb_desc_hid_cdc.c @@ -33,300 +33,300 @@ #ifdef PIOS_INCLUDE_USB #include "pios_usb_desc_hid_cdc_priv.h" /* exported API */ -#include "pios_usb_defs.h" /* struct usb_*, USB_* */ -#include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ -#include "pios_usbhook.h" /* PIOS_USBHOOK_Register* */ -#include "pios_usb_hid.h" /* PIOS_USB_HID_Register* */ +#include "pios_usb_defs.h" /* struct usb_*, USB_* */ +#include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ +#include "pios_usbhook.h" /* PIOS_USBHOOK_Register* */ +#include "pios_usb_hid.h" /* PIOS_USB_HID_Register* */ static const struct usb_device_desc device_desc = { - .bLength = sizeof(struct usb_device_desc), - .bDescriptorType = USB_DESC_TYPE_DEVICE, - .bcdUSB = htousbs(0x0200), - .bDeviceClass = 0xef, - .bDeviceSubClass = 0x02, - .bDeviceProtocol = 0x01, - .bMaxPacketSize0 = 64, /* Must be 64 for high-speed devices */ - .idVendor = htousbs(USB_VENDOR_ID_OPENPILOT), - .idProduct = htousbs(PIOS_USB_BOARD_PRODUCT_ID), - .bcdDevice = htousbs(PIOS_USB_BOARD_DEVICE_VER), - .iManufacturer = USB_STRING_DESC_VENDOR, - .iProduct = USB_STRING_DESC_PRODUCT, - .iSerialNumber = USB_STRING_DESC_SERIAL, - .bNumConfigurations = 1, + .bLength = sizeof(struct usb_device_desc), + .bDescriptorType = USB_DESC_TYPE_DEVICE, + .bcdUSB = htousbs(0x0200), + .bDeviceClass = 0xef, + .bDeviceSubClass = 0x02, + .bDeviceProtocol = 0x01, + .bMaxPacketSize0 = 64, /* Must be 64 for high-speed devices */ + .idVendor = htousbs(USB_VENDOR_ID_OPENPILOT), + .idProduct = htousbs(PIOS_USB_BOARD_PRODUCT_ID), + .bcdDevice = htousbs(PIOS_USB_BOARD_DEVICE_VER), + .iManufacturer = USB_STRING_DESC_VENDOR, + .iProduct = USB_STRING_DESC_PRODUCT, + .iSerialNumber = USB_STRING_DESC_SERIAL, + .bNumConfigurations = 1, }; static const uint8_t hid_report_desc[89] = { - HID_GLOBAL_ITEM_2 (HID_TAG_GLOBAL_USAGE_PAGE), - 0x9C, 0xFF, /* Usage Page 0xFF9C (Vendor Defined) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x01, /* Usage ID 0x0001 (0x01-0x1F uaually for top-level collections) */ + HID_GLOBAL_ITEM_2(HID_TAG_GLOBAL_USAGE_PAGE), + 0x9C, 0xFF, /* Usage Page 0xFF9C (Vendor Defined) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x01, /* Usage ID 0x0001 (0x01-0x1F uaually for top-level collections) */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_COLLECTION), - 0x01, /* Application */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_COLLECTION), + 0x01, /* Application */ - /* Device -> Host emulated serial channel */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_ID), - 0x01, /* OpenPilot emulated serial channel (Device -> Host) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x02, - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MIN), - 0x00, /* Values range from min = 0x00 */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MAX), - 0xFF, /* Values range to max = 0xFF */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x08, /* 8 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - PIOS_USB_BOARD_HID_DATA_LENGTH-1, /* Need to leave room for a report ID */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_INPUT), - 0x03, /* Variable, Constant (read-only) */ + /* Device -> Host emulated serial channel */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_ID), + 0x01, /* OpenPilot emulated serial channel (Device -> Host) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x02, + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MIN), + 0x00, /* Values range from min = 0x00 */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MAX), + 0xFF, /* Values range to max = 0xFF */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_SIZE), + 0x08, /* 8 bits wide */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_CNT), + PIOS_USB_BOARD_HID_DATA_LENGTH - 1, /* Need to leave room for a report ID */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_INPUT), + 0x03, /* Variable, Constant (read-only) */ - /* Host -> Host emulated serial channel */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_ID), - 0x02, /* OpenPilot emulated Serial Channel (Host -> Device) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x02, - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MIN), - 0x00, /* Values range from min = 0x00 */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MAX), - 0xFF, /* Values range to max = 0xFF */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x08, /* 8 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - PIOS_USB_BOARD_HID_DATA_LENGTH-1, /* Need to leave room for a report ID */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_OUTPUT), - 0x82, /* Volatile, Variable */ + /* Host -> Host emulated serial channel */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_ID), + 0x02, /* OpenPilot emulated Serial Channel (Host -> Device) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x02, + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MIN), + 0x00, /* Values range from min = 0x00 */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MAX), + 0xFF, /* Values range to max = 0xFF */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_SIZE), + 0x08, /* 8 bits wide */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_CNT), + PIOS_USB_BOARD_HID_DATA_LENGTH - 1, /* Need to leave room for a report ID */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_OUTPUT), + 0x82, /* Volatile, Variable */ - HID_MAIN_ITEM_0 (HID_TAG_MAIN_ENDCOLLECTION), + HID_MAIN_ITEM_0(HID_TAG_MAIN_ENDCOLLECTION), /* 36 bytes to here */ - /* Emulate a Joystick */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_USAGE_PAGE), - 0x01, /* Usage Page 0x01 (Generic Desktop Controls) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x04, /* Usage ID 0x0004 (Joystick) */ + /* Emulate a Joystick */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_USAGE_PAGE), + 0x01, /* Usage Page 0x01 (Generic Desktop Controls) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x04, /* Usage ID 0x0004 (Joystick) */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MIN), - 0x00, /* Values range from min = 0x00 */ - HID_GLOBAL_ITEM_4 (HID_TAG_GLOBAL_LOGICAL_MAX), - 0xFF, 0xFF, 0x00, 0x00, /* Values range to max = 0x0000FFFF */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MIN), + 0x00, /* Values range from min = 0x00 */ + HID_GLOBAL_ITEM_4(HID_TAG_GLOBAL_LOGICAL_MAX), + 0xFF, 0xFF,0x00, 0x00, /* Values range to max = 0x0000FFFF */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_COLLECTION), - 0x01, /* Application */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_COLLECTION), - 0x00, /* Physical */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_ID), - 0x03, /* OpenPilot Emulated joystick */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_COLLECTION), + 0x01, /* Application */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_COLLECTION), + 0x00, /* Physical */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_ID), + 0x03, /* OpenPilot Emulated joystick */ - /* X + Y controls */ + /* X + Y controls */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x30, /* Usage ID 0x00010030 (Generic Desktop: X) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x31, /* Usage ID 0x00010031 (Generic Desktop: Y) */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x10, /* 16 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - 2, - HID_MAIN_ITEM_1 (HID_TAG_MAIN_INPUT), - 0x82, /* Data, Var, Abs, Vol */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x30, /* Usage ID 0x00010030 (Generic Desktop: X) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x31, /* Usage ID 0x00010031 (Generic Desktop: Y) */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_SIZE), + 0x10, /* 16 bits wide */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_CNT), + 2, + HID_MAIN_ITEM_1(HID_TAG_MAIN_INPUT), + 0x82, /* Data, Var, Abs, Vol */ - /* Y + Rx controls */ + /* Y + Rx controls */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x32, /* Usage ID 0x00010032 (Generic Desktop: Z) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x33, /* Usage ID 0x00010031 (Generic Desktop: Rx) */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x10, /* 16 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - 2, - HID_MAIN_ITEM_1 (HID_TAG_MAIN_INPUT), - 0x82, /* Data, Var, Abs, Vol */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x32, /* Usage ID 0x00010032 (Generic Desktop: Z) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x33, /* Usage ID 0x00010031 (Generic Desktop: Rx) */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_SIZE), + 0x10, /* 16 bits wide */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_CNT), + 2, + HID_MAIN_ITEM_1(HID_TAG_MAIN_INPUT), + 0x82, /* Data, Var, Abs, Vol */ - /* Ry, Rz, Slider + Dial controls */ + /* Ry, Rz, Slider + Dial controls */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x34, /* Usage ID 0x00010034 (Generic Desktop: Ry) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x35, /* Usage ID 0x00010035 (Generic Desktop: Rz) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x36, /* Usage ID 0x00010036 (Generic Desktop: Slider) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x37, /* Usage ID 0x00010037 (Generic Desktop: Dial) */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x10, /* 16 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - 4, - HID_MAIN_ITEM_1 (HID_TAG_MAIN_INPUT), - 0x82, /* Data, Var, Abs, Vol */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x34, /* Usage ID 0x00010034 (Generic Desktop: Ry) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x35, /* Usage ID 0x00010035 (Generic Desktop: Rz) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x36, /* Usage ID 0x00010036 (Generic Desktop: Slider) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x37, /* Usage ID 0x00010037 (Generic Desktop: Dial) */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_SIZE), + 0x10, /* 16 bits wide */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_CNT), + 4, + HID_MAIN_ITEM_1(HID_TAG_MAIN_INPUT), + 0x82, /* Data, Var, Abs, Vol */ - HID_MAIN_ITEM_0 (HID_TAG_MAIN_ENDCOLLECTION), + HID_MAIN_ITEM_0(HID_TAG_MAIN_ENDCOLLECTION), - HID_MAIN_ITEM_0 (HID_TAG_MAIN_ENDCOLLECTION), + HID_MAIN_ITEM_0(HID_TAG_MAIN_ENDCOLLECTION), /* 89 bytes to here */ }; struct usb_config_hid_cdc { - struct usb_configuration_desc config; - struct usb_interface_association_desc iad; - struct usb_interface_desc cdc_control_if; - struct usb_cdc_header_func_desc cdc_header; - struct usb_cdc_callmgmt_func_desc cdc_callmgmt; - struct usb_cdc_acm_func_desc cdc_acm; - struct usb_cdc_union_func_desc cdc_union; - struct usb_endpoint_desc cdc_mgmt_in; - struct usb_interface_desc cdc_data_if; - struct usb_endpoint_desc cdc_in; - struct usb_endpoint_desc cdc_out; - struct usb_interface_desc hid_if; - struct usb_hid_desc hid; - struct usb_endpoint_desc hid_in; - struct usb_endpoint_desc hid_out; + struct usb_configuration_desc config; + struct usb_interface_association_desc iad; + struct usb_interface_desc cdc_control_if; + struct usb_cdc_header_func_desc cdc_header; + struct usb_cdc_callmgmt_func_desc cdc_callmgmt; + struct usb_cdc_acm_func_desc cdc_acm; + struct usb_cdc_union_func_desc cdc_union; + struct usb_endpoint_desc cdc_mgmt_in; + struct usb_interface_desc cdc_data_if; + struct usb_endpoint_desc cdc_in; + struct usb_endpoint_desc cdc_out; + struct usb_interface_desc hid_if; + struct usb_hid_desc hid; + struct usb_endpoint_desc hid_in; + struct usb_endpoint_desc hid_out; } __attribute__((packed)); static const struct usb_config_hid_cdc config_hid_cdc = { - .config = { - .bLength = sizeof(struct usb_configuration_desc), - .bDescriptorType = USB_DESC_TYPE_CONFIGURATION, - .wTotalLength = htousbs(sizeof(struct usb_config_hid_cdc)), - .bNumInterfaces = 3, - .bConfigurationValue = 1, - .iConfiguration = 0, - .bmAttributes = 0xC0, - .bMaxPower = 250/2, /* in units of 2ma */ - }, - .iad = { - .bLength = sizeof(struct usb_interface_association_desc), - .bDescriptorType = USB_DESC_TYPE_IAD, - .bFirstInterface = 0, - .bInterfaceCount = 2, - .bFunctionClass = USB_INTERFACE_CLASS_CDC, /* Communication */ - .bFunctionSubClass = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL, /* Abstract Control Model */ - .bFunctionProtocol = 1, /* V.25ter, Common AT commands */ - .iInterface = 0, - }, - .cdc_control_if = { - .bLength = sizeof(struct usb_interface_desc), - .bDescriptorType = USB_DESC_TYPE_INTERFACE, - .bInterfaceNumber = 0, - .bAlternateSetting = 0, - .bNumEndpoints = 1, - .bInterfaceClass = USB_INTERFACE_CLASS_CDC, - .bInterfaceSubClass = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL, /* Abstract Control Model */ - .nInterfaceProtocol = 1, /* V.25ter, Common AT commands */ - .iInterface = 0, - }, - .cdc_header = { - .bLength = sizeof(struct usb_cdc_header_func_desc), - .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, - .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_HEADER, - .bcdCDC = htousbs(0x0110), - }, - .cdc_callmgmt = { - .bLength = sizeof(struct usb_cdc_callmgmt_func_desc), - .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, - .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_CALLMGMT, - .bmCapabilities = 0x00, /* No call handling */ - .bDataInterface = 1, - }, - .cdc_acm = { - .bLength = sizeof(struct usb_cdc_acm_func_desc), - .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, - .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL, - .bmCapabilities = 0x00, - }, - .cdc_union = { - .bLength = sizeof(struct usb_cdc_union_func_desc), - .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, - .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_UNION, - .bMasterInterface = 0, - .bSlaveInterface = 1, - }, - .cdc_mgmt_in = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_IN(2), - .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_MGMT_LENGTH), - .bInterval = 4, /* ms */ - }, - .cdc_data_if = { - .bLength = sizeof(struct usb_interface_desc), - .bDescriptorType = USB_DESC_TYPE_INTERFACE, - .bInterfaceNumber = 1, - .bAlternateSetting = 0, - .bNumEndpoints = 2, - .bInterfaceClass = USB_INTERFACE_CLASS_DATA, - .bInterfaceSubClass = 0, - .nInterfaceProtocol = 0, /* No class specific protocol */ - .iInterface = 0, - }, - .cdc_in = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_IN(3), - .bmAttributes = USB_EP_ATTR_TT_BULK, - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_DATA_LENGTH), - .bInterval = 0, /* ms */ - }, - .cdc_out = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_OUT(3), - .bmAttributes = USB_EP_ATTR_TT_BULK, /* Bulk */ - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_DATA_LENGTH), - .bInterval = 0, /* ms */ - }, - .hid_if = { - .bLength = sizeof(struct usb_interface_desc), - .bDescriptorType = USB_DESC_TYPE_INTERFACE, - .bInterfaceNumber = 2, - .bAlternateSetting = 0, - .bNumEndpoints = 2, - .bInterfaceClass = USB_INTERFACE_CLASS_HID, - .bInterfaceSubClass = 0, /* no boot */ - .nInterfaceProtocol = 0, /* none */ - .iInterface = 0, - }, - .hid = { - .bLength = sizeof(struct usb_hid_desc), - .bDescriptorType = USB_DESC_TYPE_HID, - .bcdHID = htousbs(0x0110), - .bCountryCode = 0, - .bNumDescriptors = 1, - .bClassDescriptorType = USB_DESC_TYPE_REPORT, - .wItemLength = htousbs(sizeof(hid_report_desc)), - }, - .hid_in = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_IN(1), - .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), - .bInterval = 4, /* ms */ - }, - .hid_out = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_OUT(1), - .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), - .bInterval = 4, /* ms */ - }, + .config = { + .bLength = sizeof(struct usb_configuration_desc), + .bDescriptorType = USB_DESC_TYPE_CONFIGURATION, + .wTotalLength = htousbs(sizeof(struct usb_config_hid_cdc)), + .bNumInterfaces = 3, + .bConfigurationValue = 1, + .iConfiguration = 0, + .bmAttributes = 0xC0, + .bMaxPower = 250 / 2, /* in units of 2ma */ + }, + .iad = { + .bLength = sizeof(struct usb_interface_association_desc), + .bDescriptorType = USB_DESC_TYPE_IAD, + .bFirstInterface = 0, + .bInterfaceCount = 2, + .bFunctionClass = USB_INTERFACE_CLASS_CDC, /* Communication */ + .bFunctionSubClass = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL, /* Abstract Control Model */ + .bFunctionProtocol = 1, /* V.25ter, Common AT commands */ + .iInterface = 0, + }, + .cdc_control_if = { + .bLength = sizeof(struct usb_interface_desc), + .bDescriptorType = USB_DESC_TYPE_INTERFACE, + .bInterfaceNumber = 0, + .bAlternateSetting = 0, + .bNumEndpoints = 1, + .bInterfaceClass = USB_INTERFACE_CLASS_CDC, + .bInterfaceSubClass = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL, /* Abstract Control Model */ + .nInterfaceProtocol = 1, /* V.25ter, Common AT commands */ + .iInterface = 0, + }, + .cdc_header = { + .bLength = sizeof(struct usb_cdc_header_func_desc), + .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, + .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_HEADER, + .bcdCDC = htousbs(0x0110), + }, + .cdc_callmgmt = { + .bLength = sizeof(struct usb_cdc_callmgmt_func_desc), + .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, + .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_CALLMGMT, + .bmCapabilities = 0x00, /* No call handling */ + .bDataInterface = 1, + }, + .cdc_acm = { + .bLength = sizeof(struct usb_cdc_acm_func_desc), + .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, + .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL, + .bmCapabilities = 0x00, + }, + .cdc_union = { + .bLength = sizeof(struct usb_cdc_union_func_desc), + .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, + .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_UNION, + .bMasterInterface = 0, + .bSlaveInterface = 1, + }, + .cdc_mgmt_in = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_IN(2), + .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_MGMT_LENGTH), + .bInterval = 4, /* ms */ + }, + .cdc_data_if = { + .bLength = sizeof(struct usb_interface_desc), + .bDescriptorType = USB_DESC_TYPE_INTERFACE, + .bInterfaceNumber = 1, + .bAlternateSetting = 0, + .bNumEndpoints = 2, + .bInterfaceClass = USB_INTERFACE_CLASS_DATA, + .bInterfaceSubClass = 0, + .nInterfaceProtocol = 0, /* No class specific protocol */ + .iInterface = 0, + }, + .cdc_in = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_IN(3), + .bmAttributes = USB_EP_ATTR_TT_BULK, + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_DATA_LENGTH), + .bInterval = 0, /* ms */ + }, + .cdc_out = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_OUT(3), + .bmAttributes = USB_EP_ATTR_TT_BULK, /* Bulk */ + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_DATA_LENGTH), + .bInterval = 0, /* ms */ + }, + .hid_if = { + .bLength = sizeof(struct usb_interface_desc), + .bDescriptorType = USB_DESC_TYPE_INTERFACE, + .bInterfaceNumber = 2, + .bAlternateSetting = 0, + .bNumEndpoints = 2, + .bInterfaceClass = USB_INTERFACE_CLASS_HID, + .bInterfaceSubClass = 0, /* no boot */ + .nInterfaceProtocol = 0, /* none */ + .iInterface = 0, + }, + .hid = { + .bLength = sizeof(struct usb_hid_desc), + .bDescriptorType = USB_DESC_TYPE_HID, + .bcdHID = htousbs(0x0110), + .bCountryCode = 0, + .bNumDescriptors = 1, + .bClassDescriptorType = USB_DESC_TYPE_REPORT, + .wItemLength = htousbs(sizeof(hid_report_desc)), + }, + .hid_in = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_IN(1), + .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), + .bInterval = 4, /* ms */ + }, + .hid_out = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_OUT(1), + .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), + .bInterval = 4, /* ms */ + }, }; int32_t PIOS_USB_DESC_HID_CDC_Init(void) { - PIOS_USBHOOK_RegisterConfig(1, (uint8_t *)&config_hid_cdc, sizeof(config_hid_cdc)); + PIOS_USBHOOK_RegisterConfig(1, (uint8_t *)&config_hid_cdc, sizeof(config_hid_cdc)); - PIOS_USBHOOK_RegisterDevice((uint8_t *)&device_desc, sizeof(device_desc)); + PIOS_USBHOOK_RegisterDevice((uint8_t *)&device_desc, sizeof(device_desc)); - PIOS_USB_HID_RegisterHidDescriptor((uint8_t *)&(config_hid_cdc.hid), sizeof(config_hid_cdc.hid)); - PIOS_USB_HID_RegisterHidReport((uint8_t *)hid_report_desc, sizeof(hid_report_desc)); + PIOS_USB_HID_RegisterHidDescriptor((uint8_t *)&(config_hid_cdc.hid), sizeof(config_hid_cdc.hid)); + PIOS_USB_HID_RegisterHidReport((uint8_t *)hid_report_desc, sizeof(hid_report_desc)); - return 0; + return 0; } #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/common/pios_usb_desc_hid_only.c b/flight/pios/common/pios_usb_desc_hid_only.c index 0757d0937..89cf9ebe4 100644 --- a/flight/pios/common/pios_usb_desc_hid_only.c +++ b/flight/pios/common/pios_usb_desc_hid_only.c @@ -33,139 +33,139 @@ #ifdef PIOS_INCLUDE_USB #include "pios_usb_desc_hid_only_priv.h" /* exported API */ -#include "pios_usb_defs.h" /* struct usb_*, USB_* */ -#include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ -#include "pios_usbhook.h" /* PIOS_USBHOOK_Register* */ -#include "pios_usb_hid.h" /* PIOS_USB_HID_Register* */ +#include "pios_usb_defs.h" /* struct usb_*, USB_* */ +#include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ +#include "pios_usbhook.h" /* PIOS_USBHOOK_Register* */ +#include "pios_usb_hid.h" /* PIOS_USB_HID_Register* */ static const struct usb_device_desc device_desc = { - .bLength = sizeof(struct usb_device_desc), - .bDescriptorType = USB_DESC_TYPE_DEVICE, - .bcdUSB = htousbs(0x0200), - .bDeviceClass = 0x00, - .bDeviceSubClass = 0x00, - .bDeviceProtocol = 0x00, - .bMaxPacketSize0 = 64, /* Must be 64 for high-speed devices */ - .idVendor = htousbs(USB_VENDOR_ID_OPENPILOT), - .idProduct = htousbs(PIOS_USB_BOARD_PRODUCT_ID), - .bcdDevice = htousbs(PIOS_USB_BOARD_DEVICE_VER), - .iManufacturer = 1, - .iProduct = 2, - .iSerialNumber = 3, - .bNumConfigurations = 1, + .bLength = sizeof(struct usb_device_desc), + .bDescriptorType = USB_DESC_TYPE_DEVICE, + .bcdUSB = htousbs(0x0200), + .bDeviceClass = 0x00, + .bDeviceSubClass = 0x00, + .bDeviceProtocol = 0x00, + .bMaxPacketSize0 = 64, /* Must be 64 for high-speed devices */ + .idVendor = htousbs(USB_VENDOR_ID_OPENPILOT), + .idProduct = htousbs(PIOS_USB_BOARD_PRODUCT_ID), + .bcdDevice = htousbs(PIOS_USB_BOARD_DEVICE_VER), + .iManufacturer = 1, + .iProduct = 2, + .iSerialNumber = 3, + .bNumConfigurations = 1, }; static const uint8_t hid_report_desc[36] = { - HID_GLOBAL_ITEM_2 (HID_TAG_GLOBAL_USAGE_PAGE), - 0x9C, 0xFF, /* Usage Page 0xFF9C (Vendor Defined) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x01, /* Usage ID 0x0001 (0x01-0x1F uaually for top-level collections) */ + HID_GLOBAL_ITEM_2(HID_TAG_GLOBAL_USAGE_PAGE), + 0x9C, 0xFF, /* Usage Page 0xFF9C (Vendor Defined) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x01, /* Usage ID 0x0001 (0x01-0x1F uaually for top-level collections) */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_COLLECTION), - 0x01, /* Application */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_COLLECTION), + 0x01, /* Application */ - /* Device -> Host emulated serial channel */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_ID), - 0x01, /* OpenPilot emulated serial channel (Device -> Host) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x02, - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MIN), - 0x00, /* Values range from min = 0x00 */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MAX), - 0xFF, /* Values range to max = 0xFF */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x08, /* 8 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - PIOS_USB_BOARD_HID_DATA_LENGTH-1, /* Need to leave room for a report ID */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_INPUT), - 0x03, /* Variable, Constant (read-only) */ + /* Device -> Host emulated serial channel */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_ID), + 0x01, /* OpenPilot emulated serial channel (Device -> Host) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x02, + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MIN), + 0x00, /* Values range from min = 0x00 */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MAX), + 0xFF, /* Values range to max = 0xFF */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_SIZE), + 0x08, /* 8 bits wide */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_CNT), + PIOS_USB_BOARD_HID_DATA_LENGTH - 1, /* Need to leave room for a report ID */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_INPUT), + 0x03, /* Variable, Constant (read-only) */ - /* Host -> Device emulated serial channel */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_ID), - 0x02, /* OpenPilot emulated Serial Channel (Host -> Device) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x02, - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MIN), - 0x00, /* Values range from min = 0x00 */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MAX), - 0xFF, /* Values range to max = 0xFF */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x08, /* 8 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - PIOS_USB_BOARD_HID_DATA_LENGTH-1, /* Need to leave room for a report ID */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_OUTPUT), - 0x82, /* Volatile, Variable */ + /* Host -> Device emulated serial channel */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_ID), + 0x02, /* OpenPilot emulated Serial Channel (Host -> Device) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x02, + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MIN), + 0x00, /* Values range from min = 0x00 */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MAX), + 0xFF, /* Values range to max = 0xFF */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_SIZE), + 0x08, /* 8 bits wide */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_CNT), + PIOS_USB_BOARD_HID_DATA_LENGTH - 1, /* Need to leave room for a report ID */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_OUTPUT), + 0x82, /* Volatile, Variable */ - HID_MAIN_ITEM_0 (HID_TAG_MAIN_ENDCOLLECTION), + HID_MAIN_ITEM_0(HID_TAG_MAIN_ENDCOLLECTION), }; struct usb_config_hid_only { - struct usb_configuration_desc config; - struct usb_interface_desc hid_if; - struct usb_hid_desc hid; - struct usb_endpoint_desc hid_in; - struct usb_endpoint_desc hid_out; + struct usb_configuration_desc config; + struct usb_interface_desc hid_if; + struct usb_hid_desc hid; + struct usb_endpoint_desc hid_in; + struct usb_endpoint_desc hid_out; } __attribute__((packed)); const struct usb_config_hid_only config_hid_only = { - .config = { - .bLength = sizeof(struct usb_configuration_desc), - .bDescriptorType = USB_DESC_TYPE_CONFIGURATION, - .wTotalLength = htousbs(sizeof(struct usb_config_hid_only)), - .bNumInterfaces = 1, - .bConfigurationValue = 1, - .iConfiguration = 0, - .bmAttributes = 0xC0, - .bMaxPower = 250/2, /* in units of 2ma */ - }, - .hid_if = { - .bLength = sizeof(struct usb_interface_desc), - .bDescriptorType = USB_DESC_TYPE_INTERFACE, - .bInterfaceNumber = 0, - .bAlternateSetting = 0, - .bNumEndpoints = 2, - .bInterfaceClass = USB_INTERFACE_CLASS_HID, - .bInterfaceSubClass = 0, /* no boot */ - .nInterfaceProtocol = 0, /* none */ - .iInterface = 0, - }, - .hid = { - .bLength = sizeof(struct usb_hid_desc), - .bDescriptorType = USB_DESC_TYPE_HID, - .bcdHID = htousbs(0x0110), - .bCountryCode = 0, - .bNumDescriptors = 1, - .bClassDescriptorType = USB_DESC_TYPE_REPORT, - .wItemLength = htousbs(sizeof(hid_report_desc)), - }, - .hid_in = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_IN(1), - .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), - .bInterval = 4, /* ms */ - }, - .hid_out = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_OUT(1), - .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), - .bInterval = 4, /* ms */ - }, + .config = { + .bLength = sizeof(struct usb_configuration_desc), + .bDescriptorType = USB_DESC_TYPE_CONFIGURATION, + .wTotalLength = htousbs(sizeof(struct usb_config_hid_only)), + .bNumInterfaces = 1, + .bConfigurationValue = 1, + .iConfiguration = 0, + .bmAttributes = 0xC0, + .bMaxPower = 250 / 2, /* in units of 2ma */ + }, + .hid_if = { + .bLength = sizeof(struct usb_interface_desc), + .bDescriptorType = USB_DESC_TYPE_INTERFACE, + .bInterfaceNumber = 0, + .bAlternateSetting = 0, + .bNumEndpoints = 2, + .bInterfaceClass = USB_INTERFACE_CLASS_HID, + .bInterfaceSubClass = 0, /* no boot */ + .nInterfaceProtocol = 0, /* none */ + .iInterface = 0, + }, + .hid = { + .bLength = sizeof(struct usb_hid_desc), + .bDescriptorType = USB_DESC_TYPE_HID, + .bcdHID = htousbs(0x0110), + .bCountryCode = 0, + .bNumDescriptors = 1, + .bClassDescriptorType = USB_DESC_TYPE_REPORT, + .wItemLength = htousbs(sizeof(hid_report_desc)), + }, + .hid_in = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_IN(1), + .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), + .bInterval = 4, /* ms */ + }, + .hid_out = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_OUT(1), + .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), + .bInterval = 4, /* ms */ + }, }; int32_t PIOS_USB_DESC_HID_ONLY_Init(void) { - PIOS_USBHOOK_RegisterConfig(1, (uint8_t *)&config_hid_only, sizeof(config_hid_only)); + PIOS_USBHOOK_RegisterConfig(1, (uint8_t *)&config_hid_only, sizeof(config_hid_only)); - PIOS_USBHOOK_RegisterDevice((uint8_t *)&device_desc, sizeof(device_desc)); + PIOS_USBHOOK_RegisterDevice((uint8_t *)&device_desc, sizeof(device_desc)); - PIOS_USB_HID_RegisterHidDescriptor((uint8_t *)&(config_hid_only.hid), sizeof(config_hid_only.hid)); - PIOS_USB_HID_RegisterHidReport((uint8_t *)hid_report_desc, sizeof(hid_report_desc)); + PIOS_USB_HID_RegisterHidDescriptor((uint8_t *)&(config_hid_only.hid), sizeof(config_hid_only.hid)); + PIOS_USB_HID_RegisterHidReport((uint8_t *)hid_report_desc, sizeof(hid_report_desc)); - return 0; + return 0; } #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/common/pios_usb_util.c b/flight/pios/common/pios_usb_util.c index 9f7c8c94e..6b169566e 100644 --- a/flight/pios/common/pios_usb_util.c +++ b/flight/pios/common/pios_usb_util.c @@ -34,15 +34,15 @@ #include "pios_usb_util.h" -uint8_t * PIOS_USB_UTIL_AsciiToUtf8(uint8_t * dst, uint8_t * src, uint16_t srclen) +uint8_t *PIOS_USB_UTIL_AsciiToUtf8(uint8_t *dst, uint8_t *src, uint16_t srclen) { - for (uint8_t i = 0; i < srclen; i++) { - *dst = *src; - dst += 2; - src += 1; - } + for (uint8_t i = 0; i < srclen; i++) { + *dst = *src; + dst += 2; + src += 1; + } - return dst; + return dst; } #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/common/pios_video.c b/flight/pios/common/pios_video.c index 8e211d68c..3fcea2070 100644 --- a/flight/pios/common/pios_video.c +++ b/flight/pios/common/pios_video.c @@ -42,28 +42,27 @@ static void flush_spi(); // Private variables extern xSemaphoreHandle osdSemaphore; -static const struct pios_video_cfg * dev_cfg; +static const struct pios_video_cfg *dev_cfg; // Define the buffers. // For 256x192 pixel mode: -// buffer0_level/buffer0_mask becomes buffer_level; and -// buffer1_level/buffer1_mask becomes buffer_mask; +// buffer0_level/buffer0_mask becomes buffer_level; and +// buffer1_level/buffer1_mask becomes buffer_mask; // For 192x128 pixel mode, allocations are as the names are written. // divide by 8 because two bytes to a word. // Must be allocated in one block, so it is in a struct. -struct _buffers -{ - uint8_t buffer0_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; - uint8_t buffer0_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; - uint8_t buffer1_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; - uint8_t buffer1_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; -}buffers; +struct _buffers { + uint8_t buffer0_level[GRAPHICS_HEIGHT * GRAPHICS_WIDTH]; + uint8_t buffer0_mask[GRAPHICS_HEIGHT * GRAPHICS_WIDTH]; + uint8_t buffer1_level[GRAPHICS_HEIGHT * GRAPHICS_WIDTH]; + uint8_t buffer1_mask[GRAPHICS_HEIGHT * GRAPHICS_WIDTH]; +} buffers; // Remove the struct definition (makes it easier to write for.) -#define buffer0_level (buffers.buffer0_level) -#define buffer0_mask (buffers.buffer0_mask) -#define buffer1_level (buffers.buffer1_level) -#define buffer1_mask (buffers.buffer1_mask) +#define buffer0_level (buffers.buffer0_level) +#define buffer0_mask (buffers.buffer0_mask) +#define buffer1_level (buffers.buffer1_level) +#define buffer1_mask (buffers.buffer1_mask) // We define pointers to each of these buffers. uint8_t *draw_buffer_level; @@ -71,13 +70,13 @@ uint8_t *draw_buffer_mask; uint8_t *disp_buffer_level; uint8_t *disp_buffer_mask; -volatile uint8_t gLineType = LINE_TYPE_UNKNOWN; -volatile uint16_t gActiveLine = 0; +volatile uint8_t gLineType = LINE_TYPE_UNKNOWN; +volatile uint16_t gActiveLine = 0; volatile uint16_t gActivePixmapLine = 0; -volatile uint16_t line=0; -volatile uint16_t Vsync_update=0; -volatile uint16_t Hsync_update=0; -static int16_t m_osdLines=0; +volatile uint16_t line = 0; +volatile uint16_t Vsync_update = 0; +volatile uint16_t Hsync_update = 0; +static int16_t m_osdLines = 0; /** * swap_buffers: Swaps the two buffers. Contents in the display @@ -90,6 +89,7 @@ void swap_buffers() // dependable and it's only called a few times per second. // Many compliers should optimise these to EXCH instructions. uint8_t *tmp; + SWAP_BUFFS(tmp, disp_buffer_mask, draw_buffer_mask); SWAP_BUFFS(tmp, disp_buffer_level, draw_buffer_level); } @@ -97,7 +97,7 @@ void swap_buffers() bool PIOS_Hsync_ISR() { // On tenth line prepare data which will start clocking out on GRAPHICS_LINE+1 - if(Hsync_update==GRAPHICS_LINE) { + if (Hsync_update == GRAPHICS_LINE) { prepare_line(0); gActiveLine = 1; } @@ -105,7 +105,8 @@ bool PIOS_Hsync_ISR() return true; } -bool PIOS_Vsync_ISR() { +bool PIOS_Vsync_ISR() +{ static portBASE_TYPE xHigherPriorityTaskWoken; xHigherPriorityTaskWoken = pdFALSE; @@ -118,30 +119,30 @@ bool PIOS_Vsync_ISR() { flush_spi(); TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE); - gActiveLine = 0; + gActiveLine = 0; Hsync_update = 0; Vsync_update++; - if(Vsync_update>=2) - { + if (Vsync_update >= 2) { // load second image buffer swap_buffers(); - Vsync_update=0; + Vsync_update = 0; // trigger redraw every second field xHigherPriorityTaskWoken = xSemaphoreGiveFromISR(osdSemaphore, &xHigherPriorityTaskWoken); } - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); //portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); // portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); return xHigherPriorityTaskWoken == pdTRUE; } -uint16_t PIOS_Video_GetOSDLines(void) { +uint16_t PIOS_Video_GetOSDLines(void) +{ return m_osdLines; } /** - * Stops the pixel clock and ensures it ignores the rising edge. To be used after a + * Stops the pixel clock and ensures it ignores the rising edge. To be used after a * vsync until the first line is to be displayed */ static void stop_hsync_timers() @@ -153,21 +154,21 @@ static void stop_hsync_timers() const struct pios_tim_callbacks px_callback = { .overflow = NULL, - .edge = NULL, + .edge = NULL, }; #ifdef PAL const uint32_t period = 10; -const uint32_t dc = (10 / 2); +const uint32_t dc = (10 / 2); #else const uint32_t period = 11; -const uint32_t dc = (11 / 2); +const uint32_t dc = (11 / 2); #endif /** * Reset the timer and configure for next call. Keeps them synced. Ideally this won't even be needed * since I don't think the slave mode gets lost, and this can simply be disable timer */ -uint32_t failcount = 0; +uint32_t failcount = 0; static void reset_hsync_timers() { // Stop both timers @@ -176,25 +177,26 @@ static void reset_hsync_timers() uint32_t tim_id; const struct pios_tim_channel *channels = &dev_cfg->hsync_capture; - //BUG: This is nuts this line is needed. It simply results in allocating - //all the memory but somehow leaving it out breaks the timer functionality. + // BUG: This is nuts this line is needed. It simply results in allocating + // all the memory but somehow leaving it out breaks the timer functionality. // I do not see how these can be related if (failcount == 0) { - if(PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0) < 0) - failcount++; + if (PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0) < 0) { + failcount++; + } } - dev_cfg->pixel_timer.timer->CNT = 0xFFFF - 100; //dc; + dev_cfg->pixel_timer.timer->CNT = 0xFFFF - 100; // dc; // Listen to Channel1 (HSYNC) - switch(dev_cfg->hsync_capture.timer_chan) { - case TIM_Channel_1: + switch (dev_cfg->hsync_capture.timer_chan) { + case TIM_Channel_1: TIM_SelectInputTrigger(dev_cfg->pixel_timer.timer, TIM_TS_TI1FP1); break; - case TIM_Channel_2: + case TIM_Channel_2: TIM_SelectInputTrigger(dev_cfg->pixel_timer.timer, TIM_TS_TI2FP2); break; - default: + default: PIOS_Assert(0); } TIM_SelectSlaveMode(dev_cfg->pixel_timer.timer, TIM_SlaveMode_Trigger); @@ -220,41 +222,41 @@ static void configure_hsync_timers() // Configure the input capture channel TIM_ICInitTypeDef TIM_ICInitStructure; - switch(dev_cfg->hsync_capture.timer_chan) { - case TIM_Channel_1: + switch (dev_cfg->hsync_capture.timer_chan) { + case TIM_Channel_1: TIM_ICInitStructure.TIM_Channel = TIM_Channel_1; break; - case TIM_Channel_2: + case TIM_Channel_2: TIM_ICInitStructure.TIM_Channel = TIM_Channel_2; break; - default: + default: PIOS_Assert(0); } - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; - //TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + // TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; - TIM_ICInitStructure.TIM_ICFilter = 0; + TIM_ICInitStructure.TIM_ICFilter = 0; TIM_ICInit(dev_cfg->pixel_timer.timer, &TIM_ICInitStructure); // Set up the channel to output the pixel clock - switch(dev_cfg->pixel_timer.timer_chan) { - case TIM_Channel_1: + switch (dev_cfg->pixel_timer.timer_chan) { + case TIM_Channel_1: TIM_OC1Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); TIM_OC1PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); TIM_SetCompare1(dev_cfg->pixel_timer.timer, dc); break; - case TIM_Channel_2: + case TIM_Channel_2: TIM_OC2Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); TIM_OC2PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); TIM_SetCompare2(dev_cfg->pixel_timer.timer, dc); break; - case TIM_Channel_3: + case TIM_Channel_3: TIM_OC3Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); TIM_OC3PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); TIM_SetCompare3(dev_cfg->pixel_timer.timer, dc); break; - case TIM_Channel_4: + case TIM_Channel_4: TIM_OC4Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); TIM_OC4PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); TIM_SetCompare4(dev_cfg->pixel_timer.timer, dc); @@ -269,11 +271,11 @@ static void configure_hsync_timers() TIM_SetAutoreload(dev_cfg->pixel_timer.timer, period); } -DMA_TypeDef * main_dma; -DMA_TypeDef * mask_dma; -DMA_Stream_TypeDef * main_stream; -DMA_Stream_TypeDef * mask_stream; -void PIOS_Video_Init(const struct pios_video_cfg * cfg) +DMA_TypeDef *main_dma; +DMA_TypeDef *mask_dma; +DMA_Stream_TypeDef *main_stream; +DMA_Stream_TypeDef *mask_stream; +void PIOS_Video_Init(const struct pios_video_cfg *cfg) { dev_cfg = cfg; // store config before enabling interrupt @@ -281,43 +283,42 @@ void PIOS_Video_Init(const struct pios_video_cfg * cfg) /* needed for HW hack */ const GPIO_InitTypeDef initStruct = { - .GPIO_Pin = GPIO_Pin_12, + .GPIO_Pin = GPIO_Pin_12, .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN , + .GPIO_Mode = GPIO_Mode_IN, .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL + .GPIO_PuPd = GPIO_PuPd_NOPULL }; GPIO_Init(GPIOC, &initStruct); /* SPI3 - MASKBUFFER */ - GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->mask.sclk.init)); - GPIO_Init(cfg->mask.miso.gpio, (GPIO_InitTypeDef*)&(cfg->mask.miso.init)); + GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef *)&(cfg->mask.sclk.init)); + GPIO_Init(cfg->mask.miso.gpio, (GPIO_InitTypeDef *)&(cfg->mask.miso.init)); /* SPI1 SLAVE FRAMEBUFFER */ - GPIO_Init(cfg->level.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->level.sclk.init)); - GPIO_Init(cfg->level.miso.gpio, (GPIO_InitTypeDef*)&(cfg->level.miso.init)); + GPIO_Init(cfg->level.sclk.gpio, (GPIO_InitTypeDef *)&(cfg->level.sclk.init)); + GPIO_Init(cfg->level.miso.gpio, (GPIO_InitTypeDef *)&(cfg->level.miso.init)); if (cfg->mask.remap) { GPIO_PinAFConfig(cfg->mask.sclk.gpio, - __builtin_ctz(cfg->mask.sclk.init.GPIO_Pin), - cfg->mask.remap); + __builtin_ctz(cfg->mask.sclk.init.GPIO_Pin), + cfg->mask.remap); GPIO_PinAFConfig(cfg->mask.miso.gpio, - __builtin_ctz(cfg->mask.miso.init.GPIO_Pin), - cfg->mask.remap); + __builtin_ctz(cfg->mask.miso.init.GPIO_Pin), + cfg->mask.remap); } - if (cfg->level.remap) - { + if (cfg->level.remap) { GPIO_PinAFConfig(cfg->level.sclk.gpio, - __builtin_ctz(cfg->level.sclk.init.GPIO_Pin), - cfg->level.remap); + __builtin_ctz(cfg->level.sclk.init.GPIO_Pin), + cfg->level.remap); GPIO_PinAFConfig(cfg->level.miso.gpio, - __builtin_ctz(cfg->level.miso.init.GPIO_Pin), - cfg->level.remap); + __builtin_ctz(cfg->level.miso.init.GPIO_Pin), + cfg->level.remap); } /* Initialize the SPI block */ - SPI_Init(cfg->level.regs, (SPI_InitTypeDef*)&(cfg->level.init)); - SPI_Init(cfg->mask.regs, (SPI_InitTypeDef*)&(cfg->mask.init)); + SPI_Init(cfg->level.regs, (SPI_InitTypeDef *)&(cfg->level.init)); + SPI_Init(cfg->mask.regs, (SPI_InitTypeDef *)&(cfg->mask.init)); /* Enable SPI */ SPI_Cmd(cfg->level.regs, ENABLE); @@ -325,24 +326,24 @@ void PIOS_Video_Init(const struct pios_video_cfg * cfg) /* Configure DMA for SPI Tx SLAVE Maskbuffer */ DMA_Cmd(cfg->mask.dma.tx.channel, DISABLE); - DMA_Init(cfg->mask.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->mask.dma.tx.init)); + DMA_Init(cfg->mask.dma.tx.channel, (DMA_InitTypeDef *)&(cfg->mask.dma.tx.init)); /* Configure DMA for SPI Tx SLAVE Framebuffer*/ DMA_Cmd(cfg->level.dma.tx.channel, DISABLE); - DMA_Init(cfg->level.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->level.dma.tx.init)); + DMA_Init(cfg->level.dma.tx.channel, (DMA_InitTypeDef *)&(cfg->level.dma.tx.init)); /* Trigger interrupt when for half conversions too to indicate double buffer */ DMA_ITConfig(cfg->level.dma.tx.channel, DMA_IT_TC, ENABLE); /* Configure and clear buffers */ draw_buffer_level = buffer0_level; - draw_buffer_mask = buffer0_mask; + draw_buffer_mask = buffer0_mask; disp_buffer_level = buffer1_level; - disp_buffer_mask = buffer1_mask; - memset(disp_buffer_mask, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); - memset(disp_buffer_level, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); - memset(draw_buffer_mask, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); - memset(draw_buffer_level, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); + disp_buffer_mask = buffer1_mask; + memset(disp_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); + memset(disp_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); + memset(draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); + memset(draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); /* Configure DMA interrupt */ @@ -352,17 +353,17 @@ void PIOS_Video_Init(const struct pios_video_cfg * cfg) SPI_I2S_DMACmd(cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE); SPI_I2S_DMACmd(cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE); - mask_dma = DMA1; - main_dma = DMA2; + mask_dma = DMA1; + main_dma = DMA2; main_stream = cfg->level.dma.tx.channel; mask_stream = cfg->mask.dma.tx.channel; /* Configure the Video Line interrupt */ PIOS_EXTI_Init(cfg->hsync); PIOS_EXTI_Init(cfg->vsync); - //set levels to zero - PIOS_Servo_Set(0,0); - PIOS_Servo_Set(1,0); + // set levels to zero + PIOS_Servo_Set(0, 0); + PIOS_Servo_Set(1, 0); } /** @@ -371,8 +372,7 @@ void PIOS_Video_Init(const struct pios_video_cfg * cfg) */ static void prepare_line(uint32_t line_num) { - if(line_numpixel_timer.timer->CNT = dc; @@ -381,8 +381,8 @@ static void prepare_line(uint32_t line_num) DMA_ClearFlag(dev_cfg->level.dma.tx.channel, DMA_FLAG_FEIF5 | DMA_FLAG_TEIF5); // Load new line - DMA_MemoryTargetConfig(dev_cfg->level.dma.tx.channel,(uint32_t)&disp_buffer_level[buf_offset],DMA_Memory_0); - DMA_MemoryTargetConfig(dev_cfg->mask.dma.tx.channel,(uint32_t)&disp_buffer_mask[buf_offset],DMA_Memory_0); + DMA_MemoryTargetConfig(dev_cfg->level.dma.tx.channel, (uint32_t)&disp_buffer_level[buf_offset], DMA_Memory_0); + DMA_MemoryTargetConfig(dev_cfg->mask.dma.tx.channel, (uint32_t)&disp_buffer_mask[buf_offset], DMA_Memory_0); // Enable DMA, Slave first DMA_SetCurrDataCounter(dev_cfg->level.dma.tx.channel, BUFFER_LINE_LENGTH); @@ -402,24 +402,23 @@ static void prepare_line(uint32_t line_num) } void PIOS_VIDEO_DMA_Handler(void); -void DMA1_Stream7_IRQHandler(void) __attribute__ ((alias("PIOS_VIDEO_DMA_Handler"))); -void DMA2_Stream5_IRQHandler(void) __attribute__ ((alias("PIOS_VIDEO_DMA_Handler"))); +void DMA1_Stream7_IRQHandler(void) __attribute__((alias("PIOS_VIDEO_DMA_Handler"))); +void DMA2_Stream5_IRQHandler(void) __attribute__((alias("PIOS_VIDEO_DMA_Handler"))); /** * Check both SPI for the stop sequence before disabling them */ static void flush_spi() { - bool level_empty = false; - bool mask_empty = false; + bool level_empty = false; + bool mask_empty = false; bool level_stopped = false; - bool mask_stopped = false; + bool mask_stopped = false; // Can't flush if clock not running - while((dev_cfg->pixel_timer.timer->CR1 & 0x0001) && ( !level_stopped | !mask_stopped )) { - - level_empty |= SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == SET; - mask_empty |= SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == SET; + while ((dev_cfg->pixel_timer.timer->CR1 & 0x0001) && (!level_stopped | !mask_stopped)) { + level_empty |= SPI_I2S_GetFlagStatus(dev_cfg->level.regs, SPI_I2S_FLAG_TXE) == SET; + mask_empty |= SPI_I2S_GetFlagStatus(dev_cfg->mask.regs, SPI_I2S_FLAG_TXE) == SET; if (level_empty && !level_stopped) { // && SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == RESET) { SPI_Cmd(dev_cfg->level.regs, DISABLE); @@ -432,11 +431,11 @@ static void flush_spi() } } /* - uint32_t i = 0; - while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; - while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; - while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++; - while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++;*/ + uint32_t i = 0; + while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; + while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; + while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++; + while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++;*/ SPI_Cmd(dev_cfg->mask.regs, DISABLE); SPI_Cmd(dev_cfg->level.regs, DISABLE); } @@ -447,20 +446,17 @@ static void flush_spi() void PIOS_VIDEO_DMA_Handler(void) { // Handle flags from stream channel - if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel,DMA_FLAG_TCIF5)) { // whole double buffer filled - DMA_ClearFlag(dev_cfg->level.dma.tx.channel,DMA_FLAG_TCIF5); - if(gActiveLine < GRAPHICS_HEIGHT) - { + if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel, DMA_FLAG_TCIF5)) { // whole double buffer filled + DMA_ClearFlag(dev_cfg->level.dma.tx.channel, DMA_FLAG_TCIF5); + if (gActiveLine < GRAPHICS_HEIGHT) { flush_spi(); stop_hsync_timers(); dev_cfg->pixel_timer.timer->CNT = dc; prepare_line(gActiveLine); - } - else if(gActiveLine >= GRAPHICS_HEIGHT) - { - //last line completed + } else if (gActiveLine >= GRAPHICS_HEIGHT) { + // last line completed flush_spi(); stop_hsync_timers(); @@ -469,12 +465,9 @@ void PIOS_VIDEO_DMA_Handler(void) DMA_Cmd(dev_cfg->level.dma.tx.channel, DISABLE); } gActiveLine++; - } - else if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel,DMA_FLAG_HTIF5)) { - DMA_ClearFlag(dev_cfg->level.dma.tx.channel,DMA_FLAG_HTIF5); - } - else { - } + } else if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel, DMA_FLAG_HTIF5)) { + DMA_ClearFlag(dev_cfg->level.dma.tx.channel, DMA_FLAG_HTIF5); + } else {} } #endif /* PIOS_INCLUDE_VIDEO */ diff --git a/flight/pios/common/pios_wavplay.c b/flight/pios/common/pios_wavplay.c index d2b82d65a..564e7b859 100644 --- a/flight/pios/common/pios_wavplay.c +++ b/flight/pios/common/pios_wavplay.c @@ -33,518 +33,488 @@ #ifdef PIOS_INCLUDE_WAVE -static const struct pios_dac_cfg * dev_cfg; +static const struct pios_dac_cfg *dev_cfg; -typedef enum -{ - LittleEndian, - BigEndian -}Endianness; +typedef enum { + LittleEndian, + BigEndian +} Endianness; /** - * @} - */ + * @} + */ /** @defgroup WAVEPLAYER_Private_Defines - * @{ - */ -#define CHUNK_ID 0x52494646 /* correspond to the letters 'RIFF' */ -#define FILE_FORMAT 0x57415645 /* correspond to the letters 'WAVE' */ -#define FORMAT_ID 0x666D7420 /* correspond to the letters 'fmt ' */ -#define DATA_ID 0x64617461 /* correspond to the letters 'data' */ -#define FACT_ID 0x66616374 /* correspond to the letters 'fact' */ -#define WAVE_FORMAT_PCM 0x01 -#define FORMAT_CHNUK_SIZE 0x10 -#define CHANNEL_MONO 0x01 -#define SAMPLE_RATE_8000 8000 -#define SAMPLE_RATE_11025 11025 -#define SAMPLE_RATE_22050 22050 -#define SAMPLE_RATE_44100 44100 -#define BITS_PER_SAMPLE_8 8 -#define WAVE_DUMMY_BYTE 0xA5 -#define DAC_DHLCD_REG_8LCD_REG_1_ADDRESS 0x40007410 + * @{ + */ +#define CHUNK_ID 0x52494646 /* correspond to the letters 'RIFF' */ +#define FILE_FORMAT 0x57415645 /* correspond to the letters 'WAVE' */ +#define FORMAT_ID 0x666D7420 /* correspond to the letters 'fmt ' */ +#define DATA_ID 0x64617461 /* correspond to the letters 'data' */ +#define FACT_ID 0x66616374 /* correspond to the letters 'fact' */ +#define WAVE_FORMAT_PCM 0x01 +#define FORMAT_CHNUK_SIZE 0x10 +#define CHANNEL_MONO 0x01 +#define SAMPLE_RATE_8000 8000 +#define SAMPLE_RATE_11025 11025 +#define SAMPLE_RATE_22050 22050 +#define SAMPLE_RATE_44100 44100 +#define BITS_PER_SAMPLE_8 8 +#define WAVE_DUMMY_BYTE 0xA5 +#define DAC_DHLCD_REG_8LCD_REG_1_ADDRESS 0x40007410 -typedef struct -{ - uint32_t RIFFchunksize; - uint16_t FormatTag; - uint16_t NumChannels; - uint32_t SampleRate; - uint32_t ByteRate; - uint16_t BlockAlign; - uint16_t BitsPerSample; - uint32_t DataSize; -} -WAVE_FormatTypeDef; -typedef enum -{ - Valid_WAVE_File = 0, - Unvalid_RIFF_ID, - Unvalid_WAVE_Format, - Unvalid_FormatChunk_ID, - Unsupporetd_FormatTag, - Unsupporetd_Number_Of_Channel, - Unsupporetd_Sample_Rate, - Unsupporetd_Bits_Per_Sample, - Unvalid_DataChunk_ID, - Unsupporetd_ExtraFormatBytes, - Unvalid_FactChunk_ID +typedef struct { + uint32_t RIFFchunksize; + uint16_t FormatTag; + uint16_t NumChannels; + uint32_t SampleRate; + uint32_t ByteRate; + uint16_t BlockAlign; + uint16_t BitsPerSample; + uint32_t DataSize; +} WAVE_FormatTypeDef; +typedef enum { + Valid_WAVE_File = 0, + Unvalid_RIFF_ID, + Unvalid_WAVE_Format, + Unvalid_FormatChunk_ID, + Unsupporetd_FormatTag, + Unsupporetd_Number_Of_Channel, + Unsupporetd_Sample_Rate, + Unsupporetd_Bits_Per_Sample, + Unvalid_DataChunk_ID, + Unsupporetd_ExtraFormatBytes, + Unvalid_FactChunk_ID } ErrorCode; /** - * @} - */ + * @} + */ /** @defgroup WAVEPLAYER_Exported_Constants - * @{ - */ -#define SpeechReadAddr 0x0 /* Speech wave start read address */ + * @{ + */ +#define SpeechReadAddr 0x0 /* Speech wave start read address */ /* Audio Play STATUS */ -#define AudioPlayStatus_STOPPED 0 -#define AudioPlayStatus_PLAYING 1 -#define AudioPlayStatus_PAUSED 2 +#define AudioPlayStatus_STOPPED 0 +#define AudioPlayStatus_PLAYING 1 +#define AudioPlayStatus_PAUSED 2 -#define MAX_WAVE_FILES 25 +#define MAX_WAVE_FILES 25 /** - * @} - */ + * @} + */ /** @defgroup WAVEPLAYER_Private_Macros - * @{ - */ + * @{ + */ /** - * @} - */ + * @} + */ /** @defgroup WAVEPLAYER_Private_Variables - * @{ - */ + * @{ + */ static WAVE_FormatTypeDef WAVE_Format; -static ErrorCode WaveFileStatus = Unvalid_RIFF_ID; -static uint16_t TIM6ARRValue = 1088; +static ErrorCode WaveFileStatus = Unvalid_RIFF_ID; +static uint16_t TIM6ARRValue = 1088; uint32_t WaveDataLength = 0; static uint32_t SpeechDataOffset = 0x00; static uint32_t wavelen = 0; FILEINFO fiwave; FILEINFO file; -static uint8_t buffer1[BUFFERSIZE], buffer2[BUFFERSIZE]={0}; //Two cycling buffers which contain the WAV data. +static uint8_t buffer1[BUFFERSIZE], buffer2[BUFFERSIZE] = { 0 }; // Two cycling buffers which contain the WAV data. uint32_t wavecounter; -typedef struct -{ - unsigned int format; - unsigned int sample_rate; - unsigned int bits_per_sample; -}wave_format; +typedef struct { + unsigned int format; + unsigned int sample_rate; + unsigned int bits_per_sample; +} wave_format; wave_format wave_info; /** - * @brief Decrements the played wave data length. - * @param None - * @retval Current value of WaveDataLength variable. - */ + * @brief Decrements the played wave data length. + * @param None + * @retval Current value of WaveDataLength variable. + */ uint32_t Decrement_WaveDataLength(void) { - if (WaveDataLength != 0x00) - { - WaveDataLength--; - } - return (WaveDataLength); + if (WaveDataLength != 0x00) { + WaveDataLength--; + } + return WaveDataLength; } /** - * @brief Decrements the played wave data length. - * @param None - * @retval Current value of WaveDataLength variable. - */ + * @brief Decrements the played wave data length. + * @param None + * @retval Current value of WaveDataLength variable. + */ void Set_WaveDataLength(uint32_t value) { - WaveDataLength = value; + WaveDataLength = value; } /** - * @brief Reads a number of bytes from the SPI Flash and reorder them in Big - * or little endian. - * @param NbrOfBytes: number of bytes to read. - * This parameter must be a number between 1 and 4. - * @param ReadAddr: external memory address to read from. - * @param Endians: specifies the bytes endianness. - * This parameter can be one of the following values: - * - LittleEndian - * - BigEndian - * @retval Bytes read from the SPI Flash. - */ + * @brief Reads a number of bytes from the SPI Flash and reorder them in Big + * or little endian. + * @param NbrOfBytes: number of bytes to read. + * This parameter must be a number between 1 and 4. + * @param ReadAddr: external memory address to read from. + * @param Endians: specifies the bytes endianness. + * This parameter can be one of the following values: + * - LittleEndian + * - BigEndian + * @retval Bytes read from the SPI Flash. + */ static uint32_t ReadUnit(uint8_t *buffer, uint8_t idx, uint8_t NbrOfBytes, Endianness BytesFormat) { - uint32_t index = 0; - uint32_t Temp = 0; + uint32_t index = 0; + uint32_t Temp = 0; - for (index = 0; index < NbrOfBytes; index++) - { - Temp |= buffer[idx + index] << (index * 8); - } + for (index = 0; index < NbrOfBytes; index++) { + Temp |= buffer[idx + index] << (index * 8); + } - if (BytesFormat == BigEndian) - { - Temp = __REV(Temp); - } - return Temp; + if (BytesFormat == BigEndian) { + Temp = __REV(Temp); + } + return Temp; } /** - * @brief Checks the format of the .WAV file and gets information about - * the audio format. This is done by reading the value of a - * number of parameters stored in the file header and comparing - * these to the values expected authenticates the format of a - * standard .WAV file (44 bytes will be read). If it is a valid - * .WAV file format, it continues reading the header to determine - * the audio format such as the sample rate and the sampled data - * size. If the audio format is supported by this application, - * it retrieves the audio format in WAVE_Format structure and - * returns a zero value. Otherwise the function fails and the - * return value is nonzero.In this case, the return value specifies - * the cause of the function fails. The error codes that can be - * returned by this function are declared in the header file. - * @param None - * @retval Zero value if the function succeed, otherwise it return - * a nonzero value which specifies the error code. - */ + * @brief Checks the format of the .WAV file and gets information about + * the audio format. This is done by reading the value of a + * number of parameters stored in the file header and comparing + * these to the values expected authenticates the format of a + * standard .WAV file (44 bytes will be read). If it is a valid + * .WAV file format, it continues reading the header to determine + * the audio format such as the sample rate and the sampled data + * size. If the audio format is supported by this application, + * it retrieves the audio format in WAVE_Format structure and + * returns a zero value. Otherwise the function fails and the + * return value is nonzero.In this case, the return value specifies + * the cause of the function fails. The error codes that can be + * returned by this function are declared in the header file. + * @param None + * @retval Zero value if the function succeed, otherwise it return + * a nonzero value which specifies the error code. + */ static ErrorCode WavePlayer_WaveParsing(uint8_t *DirName, uint8_t *FileName, uint32_t *FileLen) { - uint32_t Temp = 0x00; - uint32_t ExtraFormatBytes = 0; - __IO uint32_t err = 0; - uint32_t number_of_clusters; - uint32_t i; + uint32_t Temp = 0x00; + uint32_t ExtraFormatBytes = 0; + __IO uint32_t err = 0; + uint32_t number_of_clusters; + uint32_t i; - /* Directory enumeration test */ - if (PIOS_FOPEN_READ(FileName, file)) - { - err = 1; - } - else - { - *FileLen = file.filelen; - number_of_clusters = file.filelen / 512; - if ((file.filelen % SECTOR_SIZE) > 0) - { - number_of_clusters ++; + /* Directory enumeration test */ + if (PIOS_FOPEN_READ(FileName, file)) { + err = 1; + } else { + *FileLen = file.filelen; + number_of_clusters = file.filelen / 512; + if ((file.filelen % SECTOR_SIZE) > 0) { + number_of_clusters++; + } } - } - PIOS_FREAD(&file, buffer1, 44, &i); - //DFS_ReadFile(&file, sector, buffer1, &i, SECTOR_SIZE); + PIOS_FREAD(&file, buffer1, 44, &i); + // DFS_ReadFile(&file, sector, buffer1, &i, SECTOR_SIZE); - /* Read chunkID, must be 'RIFF' ----------------------------------------------*/ - Temp = ReadUnit(buffer1, 0, 4, BigEndian); - if (Temp != CHUNK_ID) - { - return(Unvalid_RIFF_ID); - } + /* Read chunkID, must be 'RIFF' ----------------------------------------------*/ + Temp = ReadUnit(buffer1, 0, 4, BigEndian); + if (Temp != CHUNK_ID) { + return Unvalid_RIFF_ID; + } - /* Read the file length ----------------------------------------------------*/ - WAVE_Format.RIFFchunksize = ReadUnit(buffer1, 4, 4, LittleEndian); + /* Read the file length ----------------------------------------------------*/ + WAVE_Format.RIFFchunksize = ReadUnit(buffer1, 4, 4, LittleEndian); - /* Read the file format, must be 'WAVE' ------------------------------------*/ - Temp = ReadUnit(buffer1, 8, 4, BigEndian); - if (Temp != FILE_FORMAT) - { - return(Unvalid_WAVE_Format); - } + /* Read the file format, must be 'WAVE' ------------------------------------*/ + Temp = ReadUnit(buffer1, 8, 4, BigEndian); + if (Temp != FILE_FORMAT) { + return Unvalid_WAVE_Format; + } - /* Read the format chunk, must be'fmt ' --------------------------------------*/ - Temp = ReadUnit(buffer1, 12, 4, BigEndian); - if (Temp != FORMAT_ID) - { - return(Unvalid_FormatChunk_ID); - } - /* Read the length of the 'fmt' data, must be 0x10 -------------------------*/ - Temp = ReadUnit(buffer1, 16, 4, LittleEndian); - if (Temp != 0x10) - { - ExtraFormatBytes = 1; - } - /* Read the audio format, must be 0x01 (PCM) -------------------------------*/ - WAVE_Format.FormatTag = ReadUnit(buffer1, 20, 2, LittleEndian); - if (WAVE_Format.FormatTag != WAVE_FORMAT_PCM) - { - return(Unsupporetd_FormatTag); - } + /* Read the format chunk, must be'fmt ' --------------------------------------*/ + Temp = ReadUnit(buffer1, 12, 4, BigEndian); + if (Temp != FORMAT_ID) { + return Unvalid_FormatChunk_ID; + } + /* Read the length of the 'fmt' data, must be 0x10 -------------------------*/ + Temp = ReadUnit(buffer1, 16, 4, LittleEndian); + if (Temp != 0x10) { + ExtraFormatBytes = 1; + } + /* Read the audio format, must be 0x01 (PCM) -------------------------------*/ + WAVE_Format.FormatTag = ReadUnit(buffer1, 20, 2, LittleEndian); + if (WAVE_Format.FormatTag != WAVE_FORMAT_PCM) { + return Unsupporetd_FormatTag; + } - /* Read the number of channels, must be 0x01 (Mono) ------------------------*/ - WAVE_Format.NumChannels = ReadUnit(buffer1, 22, 2, LittleEndian); - if (WAVE_Format.NumChannels != CHANNEL_MONO) - { - return(Unsupporetd_Number_Of_Channel); - } + /* Read the number of channels, must be 0x01 (Mono) ------------------------*/ + WAVE_Format.NumChannels = ReadUnit(buffer1, 22, 2, LittleEndian); + if (WAVE_Format.NumChannels != CHANNEL_MONO) { + return Unsupporetd_Number_Of_Channel; + } - /* Read the Sample Rate ----------------------------------------------------*/ - WAVE_Format.SampleRate = ReadUnit(buffer1, 24, 4, LittleEndian); - /* Update the OCA value according to the .WAV file Sample Rate */ - switch (WAVE_Format.SampleRate) - { - case SAMPLE_RATE_8000 : - TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK)/8000; - break; /* 8KHz = 24MHz / 3000 */ + /* Read the Sample Rate ----------------------------------------------------*/ + WAVE_Format.SampleRate = ReadUnit(buffer1, 24, 4, LittleEndian); + /* Update the OCA value according to the .WAV file Sample Rate */ + switch (WAVE_Format.SampleRate) { + case SAMPLE_RATE_8000: + TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK) / 8000; + break; /* 8KHz = 24MHz / 3000 */ case SAMPLE_RATE_11025: - TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK)/11025; - break; /* 11.025KHz = 24MHz / 2176 */ + TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK) / 11025; + break; /* 11.025KHz = 24MHz / 2176 */ case SAMPLE_RATE_22050: - TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK)/22050; - break; /* 22.05KHz = 24MHz / 1088 */ + TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK) / 22050; + break; /* 22.05KHz = 24MHz / 1088 */ case SAMPLE_RATE_44100: - TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK)/44100; - break; /* 44.1KHz = 24MHz / 544 */ + TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK) / 44100; + break; /* 44.1KHz = 24MHz / 544 */ default: - return(Unsupporetd_Sample_Rate); - } - - /* Read the Byte Rate ------------------------------------------------------*/ - WAVE_Format.ByteRate = ReadUnit(buffer1, 28, 4, LittleEndian); - - /* Read the block alignment ------------------------------------------------*/ - WAVE_Format.BlockAlign = ReadUnit(buffer1, 32, 2, LittleEndian); - - /* Read the number of bits per sample --------------------------------------*/ - WAVE_Format.BitsPerSample = ReadUnit(buffer1, 34, 2, LittleEndian); - if (WAVE_Format.BitsPerSample != BITS_PER_SAMPLE_8) - { - return(Unsupporetd_Bits_Per_Sample); - } - SpeechDataOffset = 36; - /* If there is Extra format bytes, these bytes will be defined in "Fact Chunk" */ - if (ExtraFormatBytes == 1) - { - /* Read th Extra format bytes, must be 0x00 ------------------------------*/ - Temp = ReadUnit(buffer1, 36, 2, LittleEndian); - if (Temp != 0x00) - { - return(Unsupporetd_ExtraFormatBytes); + return Unsupporetd_Sample_Rate; } - /* Read the Fact chunk, must be 'fact' -----------------------------------*/ - Temp = ReadUnit(buffer1, 38, 4, BigEndian); - if (Temp != FACT_ID) - { - return(Unvalid_FactChunk_ID); + + /* Read the Byte Rate ------------------------------------------------------*/ + WAVE_Format.ByteRate = ReadUnit(buffer1, 28, 4, LittleEndian); + + /* Read the block alignment ------------------------------------------------*/ + WAVE_Format.BlockAlign = ReadUnit(buffer1, 32, 2, LittleEndian); + + /* Read the number of bits per sample --------------------------------------*/ + WAVE_Format.BitsPerSample = ReadUnit(buffer1, 34, 2, LittleEndian); + if (WAVE_Format.BitsPerSample != BITS_PER_SAMPLE_8) { + return Unsupporetd_Bits_Per_Sample; } - /* Read Fact chunk data Size ---------------------------------------------*/ - Temp = ReadUnit(buffer1, 42, 4, LittleEndian); + SpeechDataOffset = 36; + /* If there is Extra format bytes, these bytes will be defined in "Fact Chunk" */ + if (ExtraFormatBytes == 1) { + /* Read th Extra format bytes, must be 0x00 ------------------------------*/ + Temp = ReadUnit(buffer1, 36, 2, LittleEndian); + if (Temp != 0x00) { + return Unsupporetd_ExtraFormatBytes; + } + /* Read the Fact chunk, must be 'fact' -----------------------------------*/ + Temp = ReadUnit(buffer1, 38, 4, BigEndian); + if (Temp != FACT_ID) { + return Unvalid_FactChunk_ID; + } + /* Read Fact chunk data Size ---------------------------------------------*/ + Temp = ReadUnit(buffer1, 42, 4, LittleEndian); - SpeechDataOffset += 10 + Temp; - } - /* Read the Data chunk, must be 'data' ---------------------------------------*/ - Temp = ReadUnit(buffer1, SpeechDataOffset, 4, BigEndian); - SpeechDataOffset += 4; - if (Temp != DATA_ID) - { - return(Unvalid_DataChunk_ID); - } + SpeechDataOffset += 10 + Temp; + } + /* Read the Data chunk, must be 'data' ---------------------------------------*/ + Temp = ReadUnit(buffer1, SpeechDataOffset, 4, BigEndian); + SpeechDataOffset += 4; + if (Temp != DATA_ID) { + return Unvalid_DataChunk_ID; + } - /* Read the number of sample data ------------------------------------------*/ - WAVE_Format.DataSize = ReadUnit(buffer1, SpeechDataOffset, 4, LittleEndian); - SpeechDataOffset += 4; - wavecounter = SpeechDataOffset; - PIOS_FREAD(&file, buffer1, SECTOR_SIZE, &i); - PIOS_FREAD(&file, buffer2, SECTOR_SIZE, &i); - return(Valid_WAVE_File); + /* Read the number of sample data ------------------------------------------*/ + WAVE_Format.DataSize = ReadUnit(buffer1, SpeechDataOffset, 4, LittleEndian); + SpeechDataOffset += 4; + wavecounter = SpeechDataOffset; + PIOS_FREAD(&file, buffer1, SECTOR_SIZE, &i); + PIOS_FREAD(&file, buffer2, SECTOR_SIZE, &i); + return Valid_WAVE_File; } /** - * @brief Start wave playing - * @param None - * @retval None - */ -int wavfile=0; -const uint8_t table[5][20] = {"openpilo.wav","uav.wav","beepsoun.wav", "warning.wav", "lowaltit.wav"}; + * @brief Start wave playing + * @param None + * @retval None + */ +int wavfile = 0; +const uint8_t table[5][20] = { "openpilo.wav", "uav.wav", "beepsoun.wav", "warning.wav", "lowaltit.wav" }; uint8_t WavePlayer_Start(void) { - // Check for file system availability - if (PIOS_SDCARD_IsMounted() == 0) { - return -1; - } + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return -1; + } - /* Read the Speech wave file status */ + /* Read the Speech wave file status */ - if(wavfile<5) - { - WaveFileStatus = WavePlayer_WaveParsing(" ", table[wavfile++], &wavelen); - if(wavfile>4) - { - wavfile=5; - } - //TIM_ITConfig(TIM6, TIM_IT_Update, ENABLE); - //WaveDataLength = WAVE_Format.DataSize; - //TIM_Cmd(TIM6, ENABLE); - if (WaveFileStatus == Valid_WAVE_File) /* the .WAV file is valid */ - { - /* Set WaveDataLenght to the Speech wave length */ - WaveDataLength = WAVE_Format.DataSize; + if (wavfile < 5) { + WaveFileStatus = WavePlayer_WaveParsing(" ", table[wavfile++], &wavelen); + if (wavfile > 4) { + wavfile = 5; + } + // TIM_ITConfig(TIM6, TIM_IT_Update, ENABLE); + // WaveDataLength = WAVE_Format.DataSize; + // TIM_Cmd(TIM6, ENABLE); + if (WaveFileStatus == Valid_WAVE_File) { /* the .WAV file is valid */ + /* Set WaveDataLenght to the Speech wave length */ + WaveDataLength = WAVE_Format.DataSize; - TIM_Cmd(dev_cfg->timer, DISABLE); - TIM_SetAutoreload(dev_cfg->timer, TIM6ARRValue); - /* Start TIM6 */ - TIM_Cmd(dev_cfg->timer, ENABLE); - } - else - { - return -1; - } - } - return 0; + TIM_Cmd(dev_cfg->timer, DISABLE); + TIM_SetAutoreload(dev_cfg->timer, TIM6ARRValue); + /* Start TIM6 */ + TIM_Cmd(dev_cfg->timer, ENABLE); + } else { + return -1; + } + } + return 0; } -#define TIM6_PERIOD (PIOS_PERIPHERAL_APB1_CLOCK)/44100 -void PIOS_WavPlay_Init(const struct pios_dac_cfg * cfg){ - - dev_cfg = cfg; // store config before enabling interrupt +#define TIM6_PERIOD (PIOS_PERIPHERAL_APB1_CLOCK) / 44100 +void PIOS_WavPlay_Init(const struct pios_dac_cfg *cfg) +{ + dev_cfg = cfg; // store config before enabling interrupt #if 0 - GPIO_InitTypeDef GPIO_InitStructure; - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - DMA_InitTypeDef DMA_InitStructure; - NVIC_InitTypeDef NVIC_InitStructure; - DAC_InitTypeDef DAC_InitStructure; + GPIO_InitTypeDef GPIO_InitStructure; + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + DMA_InitTypeDef DMA_InitStructure; + NVIC_InitTypeDef NVIC_InitStructure; + DAC_InitTypeDef DAC_InitStructure; - /* DAC channel 1 & 2 (DAC_OUT1 = PA.4)(DAC_OUT2 = PA.5) configuration */ - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_Init(GPIOA, &GPIO_InitStructure); + /* DAC channel 1 & 2 (DAC_OUT1 = PA.4)(DAC_OUT2 = PA.5) configuration */ + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_Init(GPIOA, &GPIO_InitStructure); - NVIC_InitStructure.NVIC_IRQChannel = TIM6_DAC_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); + NVIC_InitStructure.NVIC_IRQChannel = TIM6_DAC_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); - TIM_DeInit(TIM6); + TIM_DeInit(TIM6); - /* Time base configuration */ - TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Period = TIM6_PERIOD; - TIM_TimeBaseStructure.TIM_Prescaler = 0; - TIM_TimeBaseStructure.TIM_ClockDivision = 0; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(TIM6, &TIM_TimeBaseStructure); + /* Time base configuration */ + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + TIM_TimeBaseStructure.TIM_Period = TIM6_PERIOD; + TIM_TimeBaseStructure.TIM_Prescaler = 0; + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInit(TIM6, &TIM_TimeBaseStructure); - /* TIM6 TRGO selection */ - TIM_SelectOutputTrigger(TIM6, TIM_TRGOSource_Update); + /* TIM6 TRGO selection */ + TIM_SelectOutputTrigger(TIM6, TIM_TRGOSource_Update); - NVIC_InitStructure.NVIC_IRQChannel = DMA1_Stream5_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); + NVIC_InitStructure.NVIC_IRQChannel = DMA1_Stream5_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); - /* DMA1_Stream5 channel7 configuration **************************************/ - DMA_DeInit(DMA1_Stream5); - DMA_InitStructure.DMA_Channel = DMA_Channel_7; - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&DAC->DHR8R1; - DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&buffer1; - DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; - DMA_InitStructure.DMA_BufferSize = BUFFERSIZE; - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; - DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; - DMA_InitStructure.DMA_Priority = DMA_Priority_High; - DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; - DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full; - DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; - DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; - DMA_Init(DMA1_Stream5, &DMA_InitStructure); - /* Configure double buffering */ - DMA_DoubleBufferModeConfig(DMA1_Stream5,(uint32_t)&buffer2,DMA_Memory_0); - DMA_DoubleBufferModeCmd(DMA1_Stream5,ENABLE); + /* DMA1_Stream5 channel7 configuration **************************************/ + DMA_DeInit(DMA1_Stream5); + DMA_InitStructure.DMA_Channel = DMA_Channel_7; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&DAC->DHR8R1; + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&buffer1; + DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; + DMA_InitStructure.DMA_BufferSize = BUFFERSIZE; + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; + DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; + DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full; + DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + DMA_Init(DMA1_Stream5, &DMA_InitStructure); + /* Configure double buffering */ + DMA_DoubleBufferModeConfig(DMA1_Stream5, (uint32_t)&buffer2, DMA_Memory_0); + DMA_DoubleBufferModeCmd(DMA1_Stream5, ENABLE); - /* Enable double buffering */ - DMA_Cmd(DMA1_Stream5, ENABLE); - DMA_ITConfig(DMA1_Stream5, DMA_IT_TC, ENABLE); + /* Enable double buffering */ + DMA_Cmd(DMA1_Stream5, ENABLE); + DMA_ITConfig(DMA1_Stream5, DMA_IT_TC, ENABLE); - /* DAC channel1 Configuration */ - DAC_DeInit(); - DAC_StructInit(&DAC_InitStructure); + /* DAC channel1 Configuration */ + DAC_DeInit(); + DAC_StructInit(&DAC_InitStructure); - DAC_InitStructure.DAC_Trigger = DAC_Trigger_T6_TRGO; - DAC_InitStructure.DAC_WaveGeneration = DAC_WaveGeneration_None; - DAC_InitStructure.DAC_OutputBuffer = DAC_OutputBuffer_Enable; - DAC_Init(DAC_Channel_1, &DAC_InitStructure); + DAC_InitStructure.DAC_Trigger = DAC_Trigger_T6_TRGO; + DAC_InitStructure.DAC_WaveGeneration = DAC_WaveGeneration_None; + DAC_InitStructure.DAC_OutputBuffer = DAC_OutputBuffer_Enable; + DAC_Init(DAC_Channel_1, &DAC_InitStructure); - DAC_Cmd(DAC_Channel_1, ENABLE); - DAC_DMACmd(DAC_Channel_1, ENABLE); -#endif + DAC_Cmd(DAC_Channel_1, ENABLE); + DAC_DMACmd(DAC_Channel_1, ENABLE); +#endif /* if 0 */ #if 1 - GPIO_Init(cfg->dac_io.gpio, (GPIO_InitTypeDef*)&(cfg->dac_io.init)); + GPIO_Init(cfg->dac_io.gpio, (GPIO_InitTypeDef *)&(cfg->dac_io.init)); - /* Configure the dividers for this timer */ - TIM_TimeBaseInit(cfg->timer, &cfg->time_base_init); + /* Configure the dividers for this timer */ + TIM_TimeBaseInit(cfg->timer, &cfg->time_base_init); - /* Enable Interrupts */ - NVIC_Init(&cfg->irq.init); + /* Enable Interrupts */ + NVIC_Init(&cfg->irq.init); - TIM_SelectOutputTrigger(cfg->timer, TIM_TRGOSource_Update); + TIM_SelectOutputTrigger(cfg->timer, TIM_TRGOSource_Update); - NVIC_Init(&cfg->dma.irq.init); + NVIC_Init(&cfg->dma.irq.init); - DMA_Cmd(cfg->dma.tx.channel, DISABLE); - DMA_Init(cfg->dma.tx.channel, (DMA_InitTypeDef*)&(cfg->dma.tx.init)); + DMA_Cmd(cfg->dma.tx.channel, DISABLE); + DMA_Init(cfg->dma.tx.channel, (DMA_InitTypeDef *)&(cfg->dma.tx.init)); - /* Enable double buffering */ - DMA_MemoryTargetConfig(cfg->dma.tx.channel,(uint32_t)&buffer1,DMA_Memory_0); - DMA_DoubleBufferModeConfig(cfg->dma.tx.channel,(uint32_t)&buffer2,DMA_Memory_0); - DMA_DoubleBufferModeCmd(cfg->dma.tx.channel,ENABLE); + /* Enable double buffering */ + DMA_MemoryTargetConfig(cfg->dma.tx.channel, (uint32_t)&buffer1, DMA_Memory_0); + DMA_DoubleBufferModeConfig(cfg->dma.tx.channel, (uint32_t)&buffer2, DMA_Memory_0); + DMA_DoubleBufferModeCmd(cfg->dma.tx.channel, ENABLE); - DMA_Cmd(cfg->dma.tx.channel, ENABLE); - DMA_ITConfig(cfg->dma.tx.channel, DMA_IT_TC, ENABLE); + DMA_Cmd(cfg->dma.tx.channel, ENABLE); + DMA_ITConfig(cfg->dma.tx.channel, DMA_IT_TC, ENABLE); - DAC_Init(cfg->channel, (DAC_InitTypeDef*)&(cfg->dac_init)); + DAC_Init(cfg->channel, (DAC_InitTypeDef *)&(cfg->dac_init)); - DAC_Cmd(cfg->channel, ENABLE); - DAC_DMACmd(cfg->channel, ENABLE); -#endif - //WavePlayer_Start(); + DAC_Cmd(cfg->channel, ENABLE); + DAC_DMACmd(cfg->channel, ENABLE); +#endif /* if 1 */ + // WavePlayer_Start(); } void WavePlayer_Stop(void) { - /* Disable TIM6 update interrupt */ - TIM_ITConfig(dev_cfg->timer, TIM_IT_Update, DISABLE); - /* Disable TIM6 */ - TIM_Cmd(dev_cfg->timer, DISABLE); + /* Disable TIM6 update interrupt */ + TIM_ITConfig(dev_cfg->timer, TIM_IT_Update, DISABLE); + /* Disable TIM6 */ + TIM_Cmd(dev_cfg->timer, DISABLE); } void DAC_TIM_Handler(void); -void TIM6_DAC_IRQHandler(void) __attribute__ ((alias("DAC_TIM_Handler"))); +void TIM6_DAC_IRQHandler(void) __attribute__((alias("DAC_TIM_Handler"))); /** - * @brief This function handles TIM6 global interrupt request. - * @param None - * @retval None - */ + * @brief This function handles TIM6 global interrupt request. + * @param None + * @retval None + */ void DAC_TIM_Handler(void) { - if (TIM_GetITStatus(dev_cfg->timer, TIM_IT_Update) != RESET) - { - /* Clear TIM6 update interrupt */ - TIM_ClearITPendingBit(dev_cfg->timer, TIM_IT_Update); - } + if (TIM_GetITStatus(dev_cfg->timer, TIM_IT_Update) != RESET) { + /* Clear TIM6 update interrupt */ + TIM_ClearITPendingBit(dev_cfg->timer, TIM_IT_Update); + } } void DAC_DMA_Handler(void); -void DMA1_Stream5_IRQHandler(void) __attribute__ ((alias("DAC_DMA_Handler"))); +void DMA1_Stream5_IRQHandler(void) __attribute__((alias("DAC_DMA_Handler"))); /** * @brief Interrupt for half and full buffer transfer @@ -557,54 +527,46 @@ void DMA1_Stream5_IRQHandler(void) __attribute__ ((alias("DAC_DMA_Handler"))); */ void DAC_DMA_Handler(void) { - uint8_t status=0; - uint32_t bytesRead=0; - if (DMA_GetFlagStatus(dev_cfg->dma.tx.channel,DMA_FLAG_TCIF5)) { // whole double buffer filled - if (WaveDataLength) - { - if(DMA_GetCurrentMemoryTarget(dev_cfg->dma.tx.channel) == 0) - { - //DMA_MemoryTargetConfig(DMA1_Stream5,(uint32_t)&buffer2,DMA_Memory_1); - PIOS_FREAD(&file, buffer2, BUFFERSIZE, &bytesRead); - if (bytesRead != BUFFERSIZE) { - status=2; - } - } - else - { - //DMA_MemoryTargetConfig(DMA1_Stream5,(uint32_t)&buffer1,DMA_Memory_0); - PIOS_FREAD(&file, buffer1, BUFFERSIZE, &bytesRead); - if (bytesRead != BUFFERSIZE) { - status=1; - } - } - if(status) - { - // STOP DMA, master first - /*DMA_Cmd(DMA1_Stream5, DISABLE);*/ - //PIOS_FCLOSE(file); - //LoadWav(); + uint8_t status = 0; + uint32_t bytesRead = 0; - } - WaveDataLength -= 512; - } - if (WaveDataLength < 512) WaveDataLength = 0; - /* If we reach the WaveDataLength of the wave to play */ - if (WaveDataLength == 0) - { - /* Stop wave playing */ - WavePlayer_Stop(); - PIOS_FCLOSE(file); - WavePlayer_Start(); - } - DMA_ClearFlag(dev_cfg->dma.tx.channel,DMA_FLAG_TCIF5); - } - else if (DMA_GetFlagStatus(dev_cfg->dma.tx.channel,DMA_FLAG_HTIF5)) { - DMA_ClearFlag(dev_cfg->dma.tx.channel,DMA_FLAG_HTIF5); - } - else { - - } + if (DMA_GetFlagStatus(dev_cfg->dma.tx.channel, DMA_FLAG_TCIF5)) { // whole double buffer filled + if (WaveDataLength) { + if (DMA_GetCurrentMemoryTarget(dev_cfg->dma.tx.channel) == 0) { + // DMA_MemoryTargetConfig(DMA1_Stream5,(uint32_t)&buffer2,DMA_Memory_1); + PIOS_FREAD(&file, buffer2, BUFFERSIZE, &bytesRead); + if (bytesRead != BUFFERSIZE) { + status = 2; + } + } else { + // DMA_MemoryTargetConfig(DMA1_Stream5,(uint32_t)&buffer1,DMA_Memory_0); + PIOS_FREAD(&file, buffer1, BUFFERSIZE, &bytesRead); + if (bytesRead != BUFFERSIZE) { + status = 1; + } + } + if (status) { + // STOP DMA, master first + /*DMA_Cmd(DMA1_Stream5, DISABLE);*/ + // PIOS_FCLOSE(file); + // LoadWav(); + } + WaveDataLength -= 512; + } + if (WaveDataLength < 512) { + WaveDataLength = 0; + } + /* If we reach the WaveDataLength of the wave to play */ + if (WaveDataLength == 0) { + /* Stop wave playing */ + WavePlayer_Stop(); + PIOS_FCLOSE(file); + WavePlayer_Start(); + } + DMA_ClearFlag(dev_cfg->dma.tx.channel, DMA_FLAG_TCIF5); + } else if (DMA_GetFlagStatus(dev_cfg->dma.tx.channel, DMA_FLAG_HTIF5)) { + DMA_ClearFlag(dev_cfg->dma.tx.channel, DMA_FLAG_HTIF5); + } else {} } #endif /* PIOS_INCLUDE_WAVE */ diff --git a/flight/pios/inc/pios_adc.h b/flight/pios/inc/pios_adc.h index f72b163ee..2d32641b5 100644 --- a/flight/pios/inc/pios_adc.h +++ b/flight/pios/inc/pios_adc.h @@ -6,25 +6,25 @@ * @brief STM32 ADC PIOS interface * @{ * - * @file pios_adc.h + * @file pios_adc.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief ADC functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,14 +32,14 @@ #define PIOS_ADC_H // Maximum of 50 oversampled points -#define PIOS_ADC_MAX_SAMPLES ((((PIOS_ADC_NUM_CHANNELS + PIOS_ADC_USE_ADC2) >> PIOS_ADC_USE_ADC2) << PIOS_ADC_USE_ADC2)* PIOS_ADC_MAX_OVERSAMPLING * 2) +#define PIOS_ADC_MAX_SAMPLES ((((PIOS_ADC_NUM_CHANNELS + PIOS_ADC_USE_ADC2) >> PIOS_ADC_USE_ADC2) << PIOS_ADC_USE_ADC2) * PIOS_ADC_MAX_OVERSAMPLING * 2) -typedef void (*ADCCallback) (float * data); +typedef void (*ADCCallback)(float *data); /* Public Functions */ void PIOS_ADC_Config(uint32_t oversampling); int32_t PIOS_ADC_PinGet(uint32_t pin); -int16_t * PIOS_ADC_GetRawBuffer(void); +int16_t *PIOS_ADC_GetRawBuffer(void); uint8_t PIOS_ADC_GetOverSampling(void); void PIOS_ADC_SetCallback(ADCCallback new_function); #if defined(PIOS_INCLUDE_FREERTOS) @@ -50,6 +50,6 @@ extern void PIOS_ADC_DMA_Handler(void); #endif /* PIOS_ADC_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_adc_priv.h b/flight/pios/inc/pios_adc_priv.h index 34b73f9ca..25dadbda0 100644 --- a/flight/pios/inc/pios_adc_priv.h +++ b/flight/pios/inc/pios_adc_priv.h @@ -37,14 +37,14 @@ #include struct pios_adc_cfg { - ADC_TypeDef* adc_dev; - struct stm32_dma dma; - uint32_t half_flag; - uint32_t full_flag; - uint16_t max_downsample; + ADC_TypeDef *adc_dev; + struct stm32_dma dma; + uint32_t half_flag; + uint32_t full_flag; + uint16_t max_downsample; }; -int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg); +int32_t PIOS_ADC_Init(const struct pios_adc_cfg *cfg); #endif /* PIOS_ADC_PRIV_H */ @@ -52,4 +52,3 @@ int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg); * @} * @} */ - diff --git a/flight/pios/inc/pios_adxl345.h b/flight/pios/inc/pios_adxl345.h index 10022569f..9118784f0 100644 --- a/flight/pios/inc/pios_adxl345.h +++ b/flight/pios/inc/pios_adxl345.h @@ -7,7 +7,7 @@ * @{ * @file pios_adxl345.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief PiOS SPI ADXL345 + * @brief PiOS SPI ADXL345 * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ @@ -31,53 +31,53 @@ // Defined by data rate, not BW -#define ADXL_READ_BIT 0x80 -#define ADXL_MULTI_BIT 0x40 +#define ADXL_READ_BIT 0x80 +#define ADXL_MULTI_BIT 0x40 -#define ADXL_WHOAMI 0x00 -#define ADXL_DEVICE_ID 0xE5 -#define ADXL_X0_ADDR 0x32 +#define ADXL_WHOAMI 0x00 +#define ADXL_DEVICE_ID 0xE5 +#define ADXL_X0_ADDR 0x32 #define ADXL_FIFOSTATUS_ADDR 0x39 -#define ADXL_RATE_ADDR 0x2C -#define ADXL_RATE_100 0x0A -#define ADXL_RATE_200 0x0B -#define ADXL_RATE_400 0x0C -#define ADXL_RATE_800 0x0D -#define ADXL_RATE_1600 0x0E -#define ADXL_RATE_3200 0x0F +#define ADXL_RATE_ADDR 0x2C +#define ADXL_RATE_100 0x0A +#define ADXL_RATE_200 0x0B +#define ADXL_RATE_400 0x0C +#define ADXL_RATE_800 0x0D +#define ADXL_RATE_1600 0x0E +#define ADXL_RATE_3200 0x0F -#define ADXL_POWER_ADDR 0x2D -#define ADXL_MEAURE 0x08 +#define ADXL_POWER_ADDR 0x2D +#define ADXL_MEAURE 0x08 -#define ADXL_FORMAT_ADDR 0x31 -#define ADXL_FULL_RES 0x08 -#define ADXL_4WIRE 0x00 -#define ADXL_RANGE_2G 0x00 -#define ADXL_RANGE_4G 0x01 -#define ADXL_RANGE_8G 0x02 -#define ADXL_RANGE_16G 0x03 +#define ADXL_FORMAT_ADDR 0x31 +#define ADXL_FULL_RES 0x08 +#define ADXL_4WIRE 0x00 +#define ADXL_RANGE_2G 0x00 +#define ADXL_RANGE_4G 0x01 +#define ADXL_RANGE_8G 0x02 +#define ADXL_RANGE_16G 0x03 -#define ADXL_FIFO_ADDR 0x38 -#define ADXL_FIFO_STREAM 0x80 +#define ADXL_FIFO_ADDR 0x38 +#define ADXL_FIFO_STREAM 0x80 struct pios_adxl345_data { - int16_t x; - int16_t y; - int16_t z; + int16_t x; + int16_t y; + int16_t z; }; -int32_t PIOS_ADXL345_SelectRate(uint8_t rate); +int32_t PIOS_ADXL345_SelectRate(uint8_t rate); int32_t PIOS_ADXL345_SetRange(uint8_t range); int32_t PIOS_ADXL345_Init(uint32_t spi_id, uint32_t slave_num); -uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data * data); +uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data *data); int32_t PIOS_ADXL345_FifoElements(); int32_t PIOS_ADXL345_Test(); #endif /* PIOS_ADXL345_H */ -/** +/** * @} * @} */ diff --git a/flight/pios/inc/pios_bkp.h b/flight/pios/inc/pios_bkp.h index 35dca462a..cc4d6a6e3 100644 --- a/flight/pios/inc/pios_bkp.h +++ b/flight/pios/inc/pios_bkp.h @@ -6,25 +6,25 @@ * @brief Hardware abstraction layer for backup sram * @{ * - * @file pios_bkp.c + * @file pios_bkp.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief IAP functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,85 +34,84 @@ /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ /***************************************************************************************** * Public Definitions/Macros ****************************************************************************************/ // Backup registers definitions // registers reserved for PIOS usage -#define PIOS_BKP_RESERVED_1 0 // IAP_MAGIC_REG_1 -#define PIOS_BKP_RESERVED_2 1 // IAP_MAGIC_REG_2 -#define PIOS_BKP_RESERVED_3 2 // IAP_BOOTCOUNT -#define PIOS_BKP_RESERVED_4 3 // PIOS_WDG_REGISTER -#define PIOS_BKP_RESERVED_5 4 // IAP_CMD1 -#define PIOS_BKP_RESERVED_6 5 // IAP_CMD2 -#define PIOS_BKP_RESERVED_7 6 // IAP_CMD3 -#define PIOS_BKP_RESERVED_8 7 -#define PIOS_BKP_RESERVED_9 8 -#define PIOS_BKP_RESERVED_10 9 +#define PIOS_BKP_RESERVED_1 0 // IAP_MAGIC_REG_1 +#define PIOS_BKP_RESERVED_2 1 // IAP_MAGIC_REG_2 +#define PIOS_BKP_RESERVED_3 2 // IAP_BOOTCOUNT +#define PIOS_BKP_RESERVED_4 3 // PIOS_WDG_REGISTER +#define PIOS_BKP_RESERVED_5 4 // IAP_CMD1 +#define PIOS_BKP_RESERVED_6 5 // IAP_CMD2 +#define PIOS_BKP_RESERVED_7 6 // IAP_CMD3 +#define PIOS_BKP_RESERVED_8 7 +#define PIOS_BKP_RESERVED_9 8 +#define PIOS_BKP_RESERVED_10 9 // registers reserved for BOARD specific usage #define PIOS_BKP_BOARD_RESERVED_1 10 #define PIOS_BKP_BOARD_RESERVED_2 11 #define PIOS_BKP_BOARD_RESERVED_3 12 // registers reserved for APP usage -#define PIOS_BKP_APP_RESERVED_1 13 -#define PIOS_BKP_APP_RESERVED_2 14 -#define PIOS_BKP_APP_RESERVED_3 15 -#define PIOS_BKP_APP_RESERVED_4 16 +#define PIOS_BKP_APP_RESERVED_1 13 +#define PIOS_BKP_APP_RESERVED_2 14 +#define PIOS_BKP_APP_RESERVED_3 15 +#define PIOS_BKP_APP_RESERVED_4 16 /**************************************************************************************** - * Public Functions - ****************************************************************************************/ +* Public Functions +****************************************************************************************/ /** @defgroup PIOS_BKP_Public_Functions - * @{ - */ + * @{ + */ /** - * @brief Initialize the Backup Register hardware - * @param None - * @retval None - */ -void PIOS_BKP_Init(void); + * @brief Initialize the Backup Register hardware + * @param None + * @retval None + */ +void PIOS_BKP_Init(void); /** - * @brief Reads data from the specified Backup Register. - * @param regnumber: specifies the Backup Register. - * @retval The content of the specified Data Backup Register - */ -uint16_t PIOS_BKP_ReadRegister(uint32_t regnumber); + * @brief Reads data from the specified Backup Register. + * @param regnumber: specifies the Backup Register. + * @retval The content of the specified Data Backup Register + */ +uint16_t PIOS_BKP_ReadRegister(uint32_t regnumber); /** - * @brief Writes user data to the specified Backup Register. - * @param regnumber: specifies the Data Backup Register. - * @param data: data to write - * @retval None - */ -void PIOS_BKP_WriteRegister(uint32_t regnumber,uint16_t data); + * @brief Writes user data to the specified Backup Register. + * @param regnumber: specifies the Data Backup Register. + * @param data: data to write + * @retval None + */ +void PIOS_BKP_WriteRegister(uint32_t regnumber, uint16_t data); /** - * @brief Enable Backup registers write access - * @param None - * @retval None - */ -void PIOS_BKP_EnableWrite(void); + * @brief Enable Backup registers write access + * @param None + * @retval None + */ +void PIOS_BKP_EnableWrite(void); /** - * @brief Disable Backup registers write access - * @param None - * @retval None - */ -void PIOS_BKP_DisableWrite(void); + * @brief Disable Backup registers write access + * @param None + * @retval None + */ +void PIOS_BKP_DisableWrite(void); /** - * @} - */ - + * @} + */ /**************************************************************************************** - * Public Data - ****************************************************************************************/ +* Public Data +****************************************************************************************/ -#endif /* PIOS_BKP_H_ */ \ No newline at end of file +#endif /* PIOS_BKP_H_ */ diff --git a/flight/pios/inc/pios_bl_helper.h b/flight/pios/inc/pios_bl_helper.h index 3a0c6bdb0..855ec3160 100644 --- a/flight/pios/inc/pios_bl_helper.h +++ b/flight/pios/inc/pios_bl_helper.h @@ -34,7 +34,7 @@ extern uint8_t *PIOS_BL_HELPER_FLASH_If_Read(uint32_t SectorAddress); extern uint8_t PIOS_BL_HELPER_FLASH_Ini(); extern uint32_t PIOS_BL_HELPER_CRC_Memory_Calc(); -extern void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t * array, uint8_t size); +extern void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t *array, uint8_t size); extern uint8_t PIOS_BL_HELPER_FLASH_Start(); extern uint8_t PIOS_BL_HELPER_FLASH_Erase_Bootloader(); extern void PIOS_BL_HELPER_CRC_Ini(); diff --git a/flight/pios/inc/pios_bma180.h b/flight/pios/inc/pios_bma180.h index 84c310dd8..01164658a 100644 --- a/flight/pios/inc/pios_bma180.h +++ b/flight/pios/inc/pios_bma180.h @@ -37,71 +37,70 @@ #include /* BMA180 Addresses */ -#define BMA_CHIPID_ADDR 0x00 -#define BMA_VERSION_ADDR 0x00 -#define BMA_X_LSB_ADDR 0x02 -#define BMA_Y_LSB_ADDR 0x04 -#define BMA_Z_LSB_ADDR 0x06 -#define BMA_WE_ADDR 0x0D -#define BMA_RESET 0x10 -#define BMA_BW_ADDR 0x20 -#define BMA_RANGE_ADDR 0x35 -#define BMA_OFFSET_LSB1 0x35 -#define BMA_GAIN_Y 0x33 -#define BMA_CTRREG3 0x21 -#define BMA_CTRREG0 0x0D +#define BMA_CHIPID_ADDR 0x00 +#define BMA_VERSION_ADDR 0x00 +#define BMA_X_LSB_ADDR 0x02 +#define BMA_Y_LSB_ADDR 0x04 +#define BMA_Z_LSB_ADDR 0x06 +#define BMA_WE_ADDR 0x0D +#define BMA_RESET 0x10 +#define BMA_BW_ADDR 0x20 +#define BMA_RANGE_ADDR 0x35 +#define BMA_OFFSET_LSB1 0x35 +#define BMA_GAIN_Y 0x33 +#define BMA_CTRREG3 0x21 +#define BMA_CTRREG0 0x0D -#define BMA_RESET_CODE 0x6B +#define BMA_RESET_CODE 0x6B /* Accel range */ -#define BMA_RANGE_MASK 0x0E -#define BMA_RANGE_SHIFT 1 -enum bma180_range { BMA_RANGE_1G = 0x00, - BMA_RANGE_1_5G = 0x01, - BMA_RANGE_2G = 0x02, - BMA_RANGE_3G = 0x03, - BMA_RANGE_4G = 0x04, - BMA_RANGE_8G = 0x05, - BMA_RANGE_16G = 0x06 -}; +#define BMA_RANGE_MASK 0x0E +#define BMA_RANGE_SHIFT 1 +enum bma180_range { BMA_RANGE_1G = 0x00, + BMA_RANGE_1_5G = 0x01, + BMA_RANGE_2G = 0x02, + BMA_RANGE_3G = 0x03, + BMA_RANGE_4G = 0x04, + BMA_RANGE_8G = 0x05, + BMA_RANGE_16G = 0x06 }; /* Measurement bandwidth */ -#define BMA_BW_MASK 0xF0 -#define BMA_BW_SHIFT 4 -enum bma180_bandwidth { BMA_BW_10HZ = 0x00, - BMA_BW_20HZ = 0x01, - BMA_BW_40HZ = 0x02, - BMA_BW_75HZ = 0x03, - BMA_BW_150HZ = 0x04, - BMA_BW_300HZ = 0x05, - BMA_BW_600HZ = 0x06, - BMA_BW_1200HZ =0x07, - BMA_BW_HP1HZ = 0x08, // High-pass, 1 Hz - BMA_BW_BP0_300HZ = 0x09 // Band-pass, 0.3Hz-300Hz +#define BMA_BW_MASK 0xF0 +#define BMA_BW_SHIFT 4 +enum bma180_bandwidth { BMA_BW_10HZ = 0x00, + BMA_BW_20HZ = 0x01, + BMA_BW_40HZ = 0x02, + BMA_BW_75HZ = 0x03, + BMA_BW_150HZ = 0x04, + BMA_BW_300HZ = 0x05, + BMA_BW_600HZ = 0x06, + BMA_BW_1200HZ = 0x07, + BMA_BW_HP1HZ = 0x08, // High-pass, 1 Hz + BMA_BW_BP0_300HZ = 0x09 // Band-pass, 0.3Hz-300Hz }; -#define BMA_NEW_DAT_INT 0x02 +#define BMA_NEW_DAT_INT 0x02 struct pios_bma180_data { - int16_t x; - int16_t y; - int16_t z; - int8_t temperature; + int16_t x; + int16_t y; + int16_t z; + int8_t temperature; }; struct pios_bma180_cfg { - const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ - enum bma180_bandwidth bandwidth; - enum bma180_range range; + const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ + enum bma180_bandwidth bandwidth; + enum bma180_range range; }; /* Public Functions */ -extern int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_bma180_cfg * cfg); +extern int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_bma180_cfg *cfg); extern void PIOS_BMA180_Attach(uint32_t spi_id); extern float PIOS_BMA180_GetScale(); -extern int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data * buffer); -extern int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data * data); +extern int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data *buffer); +extern int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data *data); extern int32_t PIOS_BMA180_Test(); extern bool PIOS_BMA180_IRQHandler(); diff --git a/flight/pios/inc/pios_bmp085.h b/flight/pios/inc/pios_bmp085.h index 8cd796453..c1e8477b4 100644 --- a/flight/pios/inc/pios_bmp085.h +++ b/flight/pios/inc/pios_bmp085.h @@ -6,25 +6,25 @@ * @brief Hardware functions to deal with the altitude pressure sensor * @{ * - * @file pios_bmp085.h + * @file pios_bmp085.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief BMP085 functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,34 +32,34 @@ #define PIOS_BMP085_H /* BMP085 Addresses */ -#define BMP085_I2C_ADDR 0x77 -#define BMP085_CALIB_ADDR 0xAA -#define BMP085_CALIB_LEN 22 -#define BMP085_CTRL_ADDR 0xF4 -#define BMP085_OVERSAMPLING PIOS_BMP085_OVERSAMPLING -#define BMP085_PRES_ADDR (0x34 + (BMP085_OVERSAMPLING << 6)) -#define BMP085_TEMP_ADDR 0x2E -#define BMP085_ADC_MSB 0xF6 -#define BMP085_P0 101325 +#define BMP085_I2C_ADDR 0x77 +#define BMP085_CALIB_ADDR 0xAA +#define BMP085_CALIB_LEN 22 +#define BMP085_CTRL_ADDR 0xF4 +#define BMP085_OVERSAMPLING PIOS_BMP085_OVERSAMPLING +#define BMP085_PRES_ADDR (0x34 + (BMP085_OVERSAMPLING << 6)) +#define BMP085_TEMP_ADDR 0x2E +#define BMP085_ADC_MSB 0xF6 +#define BMP085_P0 101325 /* Local Types */ typedef struct { - int16_t AC1; - int16_t AC2; - int16_t AC3; - uint16_t AC4; - uint16_t AC5; - uint16_t AC6; - int16_t B1; - int16_t B2; - int16_t MB; - int16_t MC; - int16_t MD; + int16_t AC1; + int16_t AC2; + int16_t AC3; + uint16_t AC4; + uint16_t AC5; + uint16_t AC6; + int16_t B1; + int16_t B2; + int16_t MB; + int16_t MC; + int16_t MD; } BMP085CalibDataTypeDef; typedef enum { - PressureConv, - TemperatureConv + PressureConv, + TemperatureConv } ConversionTypeTypeDef; /* Global Variables */ @@ -75,13 +75,13 @@ extern void PIOS_BMP085_StartADC(ConversionTypeTypeDef Type); extern void PIOS_BMP085_ReadADC(void); extern int16_t PIOS_BMP085_GetTemperature(void); extern int32_t PIOS_BMP085_GetPressure(void); -extern bool PIOS_BMP085_Read(uint8_t address, uint8_t * buffer, uint8_t len); +extern bool PIOS_BMP085_Read(uint8_t address, uint8_t *buffer, uint8_t len); extern bool PIOS_BMP085_Write(uint8_t address, uint8_t buffer); extern int32_t PIOS_BMP085_Test(); #endif /* PIOS_BMP085_H */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_board_info.h b/flight/pios/inc/pios_board_info.h index 3ff7aa886..2e549f090 100644 --- a/flight/pios/inc/pios_board_info.h +++ b/flight/pios/inc/pios_board_info.h @@ -1,22 +1,22 @@ #ifndef PIOS_BOARD_INFO_H #define PIOS_BOARD_INFO_H -#include /* uint* */ +#include /* uint* */ #define PIOS_BOARD_INFO_BLOB_MAGIC 0xBDBDBDBD struct pios_board_info { - uint32_t magic; - uint8_t board_type; - uint8_t board_rev; - uint8_t bl_rev; - uint8_t hw_type; - uint32_t fw_base; - uint32_t fw_size; - uint32_t desc_base; - uint32_t desc_size; - uint32_t ee_base; - uint32_t ee_size; + uint32_t magic; + uint8_t board_type; + uint8_t board_rev; + uint8_t bl_rev; + uint8_t hw_type; + uint32_t fw_base; + uint32_t fw_size; + uint32_t desc_base; + uint32_t desc_size; + uint32_t ee_base; + uint32_t ee_size; } __attribute__((packed)); extern const struct pios_board_info pios_board_info_blob; diff --git a/flight/pios/inc/pios_com.h b/flight/pios/inc/pios_com.h index 176a870b6..e2a9639c5 100644 --- a/flight/pios/inc/pios_com.h +++ b/flight/pios/inc/pios_com.h @@ -6,44 +6,44 @@ * @brief Hardware communication layer * @{ * - * @file pios_com.h + * @file pios_com.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief COM layer functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_COM_H #define PIOS_COM_H -#include /* uint*_t */ -#include /* bool */ +#include /* uint*_t */ +#include /* bool */ -typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * task_woken); +typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *task_woken); struct pios_com_driver { - void (*init)(uint32_t id); - void (*set_baud)(uint32_t id, uint32_t baud); - void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail); - void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); - void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); - void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); - bool (*available)(uint32_t id); + void (*init)(uint32_t id); + void (*set_baud)(uint32_t id, uint32_t baud); + void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail); + void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); + void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); + void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); + bool (*available)(uint32_t id); }; /* Public Functions */ @@ -56,12 +56,12 @@ extern int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str); extern int32_t PIOS_COM_SendString(uint32_t com_id, const char *str); extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...); extern int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...); -extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms); +extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t *buf, uint16_t buf_len, uint32_t timeout_ms); extern bool PIOS_COM_Available(uint32_t com_id); #endif /* PIOS_COM_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_com_msg.h b/flight/pios/inc/pios_com_msg.h index 1caaeabeb..f37bcb2c0 100644 --- a/flight/pios/inc/pios_com_msg.h +++ b/flight/pios/inc/pios_com_msg.h @@ -6,40 +6,40 @@ * @brief Hardware communication layer * @{ * - * @file pios_com_msg.h + * @file pios_com_msg.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief COM MSG layer functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_COM_MSG_H #define PIOS_COM_MSG_H -#include /* uint*_t */ +#include /* uint*_t */ /* Public Functions */ extern int32_t PIOS_COM_MSG_Send(uint32_t com_id, const uint8_t *msg, uint16_t msg_len); -extern uint16_t PIOS_COM_MSG_Receive(uint32_t com_id, uint8_t * buf, uint16_t buf_len); +extern uint16_t PIOS_COM_MSG_Receive(uint32_t com_id, uint8_t *buf, uint16_t buf_len); #endif /* PIOS_COM_MSG_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_com_msg_priv.h b/flight/pios/inc/pios_com_msg_priv.h index 20992de47..3e775ff9e 100644 --- a/flight/pios/inc/pios_com_msg_priv.h +++ b/flight/pios/inc/pios_com_msg_priv.h @@ -31,14 +31,14 @@ #ifndef PIOS_COM_MSG_PRIV_H #define PIOS_COM_MSG_PRIV_H -#include /* uint*_t */ -#include "pios_com_priv.h" /* struct pios_com_driver */ +#include /* uint*_t */ +#include "pios_com_priv.h" /* struct pios_com_driver */ -extern int32_t PIOS_COM_MSG_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id); +extern int32_t PIOS_COM_MSG_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id); #endif /* PIOS_COM_MSG_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_com_priv.h b/flight/pios/inc/pios_com_priv.h index c39522f79..565dae3ef 100644 --- a/flight/pios/inc/pios_com_priv.h +++ b/flight/pios/inc/pios_com_priv.h @@ -31,11 +31,11 @@ #ifndef PIOS_COM_PRIV_H #define PIOS_COM_PRIV_H -extern int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len); +extern int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len); #endif /* PIOS_COM_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_constants.h b/flight/pios/inc/pios_constants.h index a1889b099..c4bbde1f8 100644 --- a/flight/pios/inc/pios_constants.h +++ b/flight/pios/inc/pios_constants.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_constants.h + * @file pios_constants.h * @author The OpenPilot Team, http://www.openpilot.org Copyright_F (Cf) 2013. * @brief Shared phisical constants * -- * @see The GNU Public License_F (GPLf) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or *_F (at your optionf) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -29,106 +29,106 @@ // Constants borrowed straight from GSL http://www.gnu.org/software/gsl/ -#define PIOS_CONST_MKS_SPEED_OF_LIGHT_F (2.99792458e8f) /* m / s */ -#define PIOS_CONST_MKS_GRAVITATIONAL_CONSTANT_F (6.673e-11f) /* m^3 / kg s^2 */ -#define PIOS_CONST_MKS_PLANCKS_CONSTANT_H_F (6.62606896e-34f) /* kg m^2 / s */ -#define PIOS_CONST_MKS_PLANCKS_CONSTANT_HBAR_F (1.05457162825e-34f) /* kg m^2 / s */ -#define PIOS_CONST_MKS_ASTRONOMICAL_UNIT_F (1.49597870691e11f) /* m */ -#define PIOS_CONST_MKS_LIGHT_YEAR_F (9.46053620707e15f) /* m */ -#define PIOS_CONST_MKS_PARSEC_F (3.08567758135e16f) /* m */ -#define PIOS_CONST_MKS_GRAV_ACCEL_F (9.80665e0f) /* m / s^2 */ -#define PIOS_CONST_MKS_ELECTRON_VOLT_F (1.602176487e-19f) /* kg m^2 / s^2 */ -#define PIOS_CONST_MKS_MASS_ELECTRON_F (9.10938188e-31f) /* kg */ -#define PIOS_CONST_MKS_MASS_MUON_F (1.88353109e-28f) /* kg */ -#define PIOS_CONST_MKS_MASS_PROTON_F (1.67262158e-27f) /* kg */ -#define PIOS_CONST_MKS_MASS_NEUTRON_F (1.67492716e-27f) /* kg */ -#define PIOS_CONST_MKS_RYDBERG_F (2.17987196968e-18f) /* kg m^2 / s^2 */ -#define PIOS_CONST_MKS_BOLTZMANN_F (1.3806504e-23f) /* kg m^2 / K s^2 */ -#define PIOS_CONST_MKS_MOLAR_GAS_F (8.314472e0f) /* kg m^2 / K mol s^2 */ -#define PIOS_CONST_MKS_STANDARD_GAS_VOLUME_F (2.2710981e-2f) /* m^3 / mol */ -#define PIOS_CONST_MKS_MINUTE_F (6e1f) /* s */ -#define PIOS_CONST_MKS_HOUR_F (3.6e3f) /* s */ -#define PIOS_CONST_MKS_DAY_F (8.64e4f) /* s */ -#define PIOS_CONST_MKS_WEEK_F (6.048e5f) /* s */ -#define PIOS_CONST_MKS_INCH_F (2.54e-2f) /* m */ -#define PIOS_CONST_MKS_FOOT_F (3.048e-1f) /* m */ -#define PIOS_CONST_MKS_YARD_F (9.144e-1f) /* m */ -#define PIOS_CONST_MKS_MILE_F (1.609344e3f) /* m */ -#define PIOS_CONST_MKS_NAUTICAL_MILE_F (1.852e3f) /* m */ -#define PIOS_CONST_MKS_FATHOM_F (1.8288e0f) /* m */ -#define PIOS_CONST_MKS_MIL_F (2.54e-5f) /* m */ -#define PIOS_CONST_MKS_POINT_F (3.52777777778e-4f) /* m */ -#define PIOS_CONST_MKS_TEXPOINT_F (3.51459803515e-4f) /* m */ -#define PIOS_CONST_MKS_MICRON_F (1e-6f) /* m */ -#define PIOS_CONST_MKS_ANGSTROM_F (1e-10f) /* m */ -#define PIOS_CONST_MKS_HECTARE_F (1e4f) /* m^2 */ -#define PIOS_CONST_MKS_ACRE_F (4.04685642241e3f) /* m^2 */ -#define PIOS_CONST_MKS_BARN_F (1e-28f) /* m^2 */ -#define PIOS_CONST_MKS_LITER_F (1e-3f) /* m^3 */ -#define PIOS_CONST_MKS_US_GALLON_F (3.78541178402e-3f) /* m^3 */ -#define PIOS_CONST_MKS_QUART_F (9.46352946004e-4f) /* m^3 */ -#define PIOS_CONST_MKS_PINT_F (4.73176473002e-4f) /* m^3 */ -#define PIOS_CONST_MKS_CUP_F (2.36588236501e-4f) /* m^3 */ -#define PIOS_CONST_MKS_FLUID_OUNCE_F (2.95735295626e-5f) /* m^3 */ -#define PIOS_CONST_MKS_TABLESPOON_F (1.47867647813e-5f) /* m^3 */ -#define PIOS_CONST_MKS_TEASPOON_F (4.92892159375e-6f) /* m^3 */ -#define PIOS_CONST_MKS_CANADIAN_GALLON_F (4.54609e-3f) /* m^3 */ -#define PIOS_CONST_MKS_UK_GALLON_F (4.546092e-3f) /* m^3 */ -#define PIOS_CONST_MKS_MILES_PER_HOUR_F (4.4704e-1f) /* m / s */ -#define PIOS_CONST_MKS_KILOMETERS_PER_HOUR_F (2.77777777778e-1f) /* m / s */ -#define PIOS_CONST_MKS_KNOT_F (5.14444444444e-1f) /* m / s */ -#define PIOS_CONST_MKS_POUND_MASS_F (4.5359237e-1f) /* kg */ -#define PIOS_CONST_MKS_OUNCE_MASS_F (2.8349523125e-2f) /* kg */ -#define PIOS_CONST_MKS_TON_F (9.0718474e2f) /* kg */ -#define PIOS_CONST_MKS_METRIC_TON_F (1e3f) /* kg */ -#define PIOS_CONST_MKS_UK_TON_F (1.0160469088e3f) /* kg */ -#define PIOS_CONST_MKS_TROY_OUNCE_F (3.1103475e-2f) /* kg */ -#define PIOS_CONST_MKS_CARAT_F (2e-4f) /* kg */ -#define PIOS_CONST_MKS_UNIFIED_ATOMIC_MASS_F (1.660538782e-27f) /* kg */ -#define PIOS_CONST_MKS_GRAM_FORCE_F (9.80665e-3f) /* kg m / s^2 */ -#define PIOS_CONST_MKS_POUND_FORCE_F (4.44822161526e0f) /* kg m / s^2 */ -#define PIOS_CONST_MKS_KILOPOUND_FORCE_F (4.44822161526e3f) /* kg m / s^2 */ -#define PIOS_CONST_MKS_POUNDAL_F (1.38255e-1f) /* kg m / s^2 */ -#define PIOS_CONST_MKS_CALORIE_F (4.1868e0f) /* kg m^2 / s^2 */ -#define PIOS_CONST_MKS_BTU_F (1.05505585262e3f) /* kg m^2 / s^2 */ -#define PIOS_CONST_MKS_THERM_F (1.05506e8f) /* kg m^2 / s^2 */ -#define PIOS_CONST_MKS_HORSEPOWER_F (7.457e2f) /* kg m^2 / s^3 */ -#define PIOS_CONST_MKS_BAR_F (1e5f) /* kg / m s^2 */ -#define PIOS_CONST_MKS_STD_ATMOSPHERE_F (1.01325e5f) /* kg / m s^2 */ -#define PIOS_CONST_MKS_TORR_F (1.33322368421e2f) /* kg / m s^2 */ -#define PIOS_CONST_MKS_METER_OF_MERCURY_F (1.33322368421e5f) /* kg / m s^2 */ -#define PIOS_CONST_MKS_INCH_OF_MERCURY_F (3.38638815789e3f) /* kg / m s^2 */ -#define PIOS_CONST_MKS_INCH_OF_WATER_F (2.490889e2f) /* kg / m s^2 */ -#define PIOS_CONST_MKS_PSI_F (6.89475729317e3f) /* kg / m s^2 */ -#define PIOS_CONST_MKS_POISE_F (1e-1f) /* kg m^-1 s^-1 */ -#define PIOS_CONST_MKS_STOKES_F (1e-4f) /* m^2 / s */ -#define PIOS_CONST_MKS_STILB_F (1e4f) /* cd / m^2 */ -#define PIOS_CONST_MKS_LUMEN_F (1e0f) /* cd sr */ -#define PIOS_CONST_MKS_LUX_F (1e0f) /* cd sr / m^2 */ -#define PIOS_CONST_MKS_PHOT_F (1e4f) /* cd sr / m^2 */ -#define PIOS_CONST_MKS_FOOTCANDLE_F (1.076e1f) /* cd sr / m^2 */ -#define PIOS_CONST_MKS_LAMBERT_F (1e4f) /* cd sr / m^2 */ -#define PIOS_CONST_MKS_FOOTLAMBERT_F (1.07639104e1f) /* cd sr / m^2 */ -#define PIOS_CONST_MKS_CURIE_F (3.7e10f) /* 1 / s */ -#define PIOS_CONST_MKS_ROENTGEN_F (2.58e-4f) /* A s / kg */ -#define PIOS_CONST_MKS_RAD_F (1e-2f) /* m^2 / s^2 */ -#define PIOS_CONST_MKS_SOLAR_MASS_F (1.98892e30f) /* kg */ -#define PIOS_CONST_MKS_BOHR_RADIUS_F (5.291772083e-11f) /* m */ -#define PIOS_CONST_MKS_NEWTON_F (1e0f) /* kg m / s^2 */ -#define PIOS_CONST_MKS_DYNE_F (1e-5f) /* kg m / s^2 */ -#define PIOS_CONST_MKS_JOULE_F (1e0f) /* kg m^2 / s^2 */ -#define PIOS_CONST_MKS_ERG_F (1e-7f) /* kg m^2 / s^2 */ +#define PIOS_CONST_MKS_SPEED_OF_LIGHT_F (2.99792458e8f) /* m / s */ +#define PIOS_CONST_MKS_GRAVITATIONAL_CONSTANT_F (6.673e-11f) /* m^3 / kg s^2 */ +#define PIOS_CONST_MKS_PLANCKS_CONSTANT_H_F (6.62606896e-34f) /* kg m^2 / s */ +#define PIOS_CONST_MKS_PLANCKS_CONSTANT_HBAR_F (1.05457162825e-34f) /* kg m^2 / s */ +#define PIOS_CONST_MKS_ASTRONOMICAL_UNIT_F (1.49597870691e11f) /* m */ +#define PIOS_CONST_MKS_LIGHT_YEAR_F (9.46053620707e15f) /* m */ +#define PIOS_CONST_MKS_PARSEC_F (3.08567758135e16f) /* m */ +#define PIOS_CONST_MKS_GRAV_ACCEL_F (9.80665e0f) /* m / s^2 */ +#define PIOS_CONST_MKS_ELECTRON_VOLT_F (1.602176487e-19f) /* kg m^2 / s^2 */ +#define PIOS_CONST_MKS_MASS_ELECTRON_F (9.10938188e-31f) /* kg */ +#define PIOS_CONST_MKS_MASS_MUON_F (1.88353109e-28f) /* kg */ +#define PIOS_CONST_MKS_MASS_PROTON_F (1.67262158e-27f) /* kg */ +#define PIOS_CONST_MKS_MASS_NEUTRON_F (1.67492716e-27f) /* kg */ +#define PIOS_CONST_MKS_RYDBERG_F (2.17987196968e-18f) /* kg m^2 / s^2 */ +#define PIOS_CONST_MKS_BOLTZMANN_F (1.3806504e-23f) /* kg m^2 / K s^2 */ +#define PIOS_CONST_MKS_MOLAR_GAS_F (8.314472e0f) /* kg m^2 / K mol s^2 */ +#define PIOS_CONST_MKS_STANDARD_GAS_VOLUME_F (2.2710981e-2f) /* m^3 / mol */ +#define PIOS_CONST_MKS_MINUTE_F (6e1f) /* s */ +#define PIOS_CONST_MKS_HOUR_F (3.6e3f) /* s */ +#define PIOS_CONST_MKS_DAY_F (8.64e4f) /* s */ +#define PIOS_CONST_MKS_WEEK_F (6.048e5f) /* s */ +#define PIOS_CONST_MKS_INCH_F (2.54e-2f) /* m */ +#define PIOS_CONST_MKS_FOOT_F (3.048e-1f) /* m */ +#define PIOS_CONST_MKS_YARD_F (9.144e-1f) /* m */ +#define PIOS_CONST_MKS_MILE_F (1.609344e3f) /* m */ +#define PIOS_CONST_MKS_NAUTICAL_MILE_F (1.852e3f) /* m */ +#define PIOS_CONST_MKS_FATHOM_F (1.8288e0f) /* m */ +#define PIOS_CONST_MKS_MIL_F (2.54e-5f) /* m */ +#define PIOS_CONST_MKS_POINT_F (3.52777777778e-4f) /* m */ +#define PIOS_CONST_MKS_TEXPOINT_F (3.51459803515e-4f) /* m */ +#define PIOS_CONST_MKS_MICRON_F (1e-6f) /* m */ +#define PIOS_CONST_MKS_ANGSTROM_F (1e-10f) /* m */ +#define PIOS_CONST_MKS_HECTARE_F (1e4f) /* m^2 */ +#define PIOS_CONST_MKS_ACRE_F (4.04685642241e3f) /* m^2 */ +#define PIOS_CONST_MKS_BARN_F (1e-28f) /* m^2 */ +#define PIOS_CONST_MKS_LITER_F (1e-3f) /* m^3 */ +#define PIOS_CONST_MKS_US_GALLON_F (3.78541178402e-3f) /* m^3 */ +#define PIOS_CONST_MKS_QUART_F (9.46352946004e-4f) /* m^3 */ +#define PIOS_CONST_MKS_PINT_F (4.73176473002e-4f) /* m^3 */ +#define PIOS_CONST_MKS_CUP_F (2.36588236501e-4f) /* m^3 */ +#define PIOS_CONST_MKS_FLUID_OUNCE_F (2.95735295626e-5f) /* m^3 */ +#define PIOS_CONST_MKS_TABLESPOON_F (1.47867647813e-5f) /* m^3 */ +#define PIOS_CONST_MKS_TEASPOON_F (4.92892159375e-6f) /* m^3 */ +#define PIOS_CONST_MKS_CANADIAN_GALLON_F (4.54609e-3f) /* m^3 */ +#define PIOS_CONST_MKS_UK_GALLON_F (4.546092e-3f) /* m^3 */ +#define PIOS_CONST_MKS_MILES_PER_HOUR_F (4.4704e-1f) /* m / s */ +#define PIOS_CONST_MKS_KILOMETERS_PER_HOUR_F (2.77777777778e-1f) /* m / s */ +#define PIOS_CONST_MKS_KNOT_F (5.14444444444e-1f) /* m / s */ +#define PIOS_CONST_MKS_POUND_MASS_F (4.5359237e-1f) /* kg */ +#define PIOS_CONST_MKS_OUNCE_MASS_F (2.8349523125e-2f) /* kg */ +#define PIOS_CONST_MKS_TON_F (9.0718474e2f) /* kg */ +#define PIOS_CONST_MKS_METRIC_TON_F (1e3f) /* kg */ +#define PIOS_CONST_MKS_UK_TON_F (1.0160469088e3f) /* kg */ +#define PIOS_CONST_MKS_TROY_OUNCE_F (3.1103475e-2f) /* kg */ +#define PIOS_CONST_MKS_CARAT_F (2e-4f) /* kg */ +#define PIOS_CONST_MKS_UNIFIED_ATOMIC_MASS_F (1.660538782e-27f) /* kg */ +#define PIOS_CONST_MKS_GRAM_FORCE_F (9.80665e-3f) /* kg m / s^2 */ +#define PIOS_CONST_MKS_POUND_FORCE_F (4.44822161526e0f) /* kg m / s^2 */ +#define PIOS_CONST_MKS_KILOPOUND_FORCE_F (4.44822161526e3f) /* kg m / s^2 */ +#define PIOS_CONST_MKS_POUNDAL_F (1.38255e-1f) /* kg m / s^2 */ +#define PIOS_CONST_MKS_CALORIE_F (4.1868e0f) /* kg m^2 / s^2 */ +#define PIOS_CONST_MKS_BTU_F (1.05505585262e3f) /* kg m^2 / s^2 */ +#define PIOS_CONST_MKS_THERM_F (1.05506e8f) /* kg m^2 / s^2 */ +#define PIOS_CONST_MKS_HORSEPOWER_F (7.457e2f) /* kg m^2 / s^3 */ +#define PIOS_CONST_MKS_BAR_F (1e5f) /* kg / m s^2 */ +#define PIOS_CONST_MKS_STD_ATMOSPHERE_F (1.01325e5f) /* kg / m s^2 */ +#define PIOS_CONST_MKS_TORR_F (1.33322368421e2f) /* kg / m s^2 */ +#define PIOS_CONST_MKS_METER_OF_MERCURY_F (1.33322368421e5f) /* kg / m s^2 */ +#define PIOS_CONST_MKS_INCH_OF_MERCURY_F (3.38638815789e3f) /* kg / m s^2 */ +#define PIOS_CONST_MKS_INCH_OF_WATER_F (2.490889e2f) /* kg / m s^2 */ +#define PIOS_CONST_MKS_PSI_F (6.89475729317e3f) /* kg / m s^2 */ +#define PIOS_CONST_MKS_POISE_F (1e-1f) /* kg m^-1 s^-1 */ +#define PIOS_CONST_MKS_STOKES_F (1e-4f) /* m^2 / s */ +#define PIOS_CONST_MKS_STILB_F (1e4f) /* cd / m^2 */ +#define PIOS_CONST_MKS_LUMEN_F (1e0f) /* cd sr */ +#define PIOS_CONST_MKS_LUX_F (1e0f) /* cd sr / m^2 */ +#define PIOS_CONST_MKS_PHOT_F (1e4f) /* cd sr / m^2 */ +#define PIOS_CONST_MKS_FOOTCANDLE_F (1.076e1f) /* cd sr / m^2 */ +#define PIOS_CONST_MKS_LAMBERT_F (1e4f) /* cd sr / m^2 */ +#define PIOS_CONST_MKS_FOOTLAMBERT_F (1.07639104e1f) /* cd sr / m^2 */ +#define PIOS_CONST_MKS_CURIE_F (3.7e10f) /* 1 / s */ +#define PIOS_CONST_MKS_ROENTGEN_F (2.58e-4f) /* A s / kg */ +#define PIOS_CONST_MKS_RAD_F (1e-2f) /* m^2 / s^2 */ +#define PIOS_CONST_MKS_SOLAR_MASS_F (1.98892e30f) /* kg */ +#define PIOS_CONST_MKS_BOHR_RADIUS_F (5.291772083e-11f) /* m */ +#define PIOS_CONST_MKS_NEWTON_F (1e0f) /* kg m / s^2 */ +#define PIOS_CONST_MKS_DYNE_F (1e-5f) /* kg m / s^2 */ +#define PIOS_CONST_MKS_JOULE_F (1e0f) /* kg m^2 / s^2 */ +#define PIOS_CONST_MKS_ERG_F (1e-7f) /* kg m^2 / s^2 */ #define PIOS_CONST_MKS_STEFAN_BOLTZMANN_CONSTANT_F (5.67040047374e-8f) /* kg / K^4 s^3 */ -#define PIOS_CONST_MKS_THOMSON_CROSS_SECTION_F (6.65245893699e-29f) /* m^2 */ -#define PIOS_CONST_MKS_BOHR_MAGNETON_F (9.27400899e-24f) /* A m^2 */ -#define PIOS_CONST_MKS_NUCLEAR_MAGNETON_F (5.05078317e-27f) /* A m^2 */ -#define PIOS_CONST_MKS_ELECTRON_MAGNETIC_MOMENT_F (9.28476362e-24f) /* A m^2 */ -#define PIOS_CONST_MKS_PROTON_MAGNETIC_MOMENT_F (1.410606633e-26f) /* A m^2 */ -#define PIOS_CONST_MKS_FARADAY_F (9.64853429775e4f) /* A s / mol */ -#define PIOS_CONST_MKS_ELECTRON_CHARGE_F (1.602176487e-19f) /* A s */ -#define PIOS_CONST_MKS_VACUUM_PERMITTIVITY_F (8.854187817e-12f) /* A^2 s^4 / kg m^3 */ -#define PIOS_CONST_MKS_VACUUM_PERMEABILITY_F (1.25663706144e-6f) /* kg m / A^2 s^2 */ -#define PIOS_CONST_MKS_DEBYE_F (3.33564095198e-30f) /* A s^2 / m^2 */ -#define PIOS_CONST_MKS_GAUSS_F (1e-4f) /* kg / A s^2 */ +#define PIOS_CONST_MKS_THOMSON_CROSS_SECTION_F (6.65245893699e-29f) /* m^2 */ +#define PIOS_CONST_MKS_BOHR_MAGNETON_F (9.27400899e-24f) /* A m^2 */ +#define PIOS_CONST_MKS_NUCLEAR_MAGNETON_F (5.05078317e-27f) /* A m^2 */ +#define PIOS_CONST_MKS_ELECTRON_MAGNETIC_MOMENT_F (9.28476362e-24f) /* A m^2 */ +#define PIOS_CONST_MKS_PROTON_MAGNETIC_MOMENT_F (1.410606633e-26f) /* A m^2 */ +#define PIOS_CONST_MKS_FARADAY_F (9.64853429775e4f) /* A s / mol */ +#define PIOS_CONST_MKS_ELECTRON_CHARGE_F (1.602176487e-19f) /* A s */ +#define PIOS_CONST_MKS_VACUUM_PERMITTIVITY_F (8.854187817e-12f) /* A^2 s^4 / kg m^3 */ +#define PIOS_CONST_MKS_VACUUM_PERMEABILITY_F (1.25663706144e-6f) /* kg m / A^2 s^2 */ +#define PIOS_CONST_MKS_DEBYE_F (3.33564095198e-30f) /* A s^2 / m^2 */ +#define PIOS_CONST_MKS_GAUSS_F (1e-4f) /* kg / A s^2 */ #endif /* PIOS_CONSTANTS_H */ diff --git a/flight/pios/inc/pios_crc.h b/flight/pios/inc/pios_crc.h index 7e35dee02..c155f1fc9 100644 --- a/flight/pios/inc/pios_crc.h +++ b/flight/pios/inc/pios_crc.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_CRC CRC Functions * @{ * - * @file pios_crc.h + * @file pios_crc.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief CRC functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,12 +31,12 @@ #define PIOS_CRC_H uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data); -uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length); +uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t *data, int32_t length); uint16_t PIOS_CRC16_updateByte(uint16_t crc, const uint8_t data); -uint16_t PIOS_CRC16_updateCRC(uint16_t crc, const uint8_t* data, int32_t length); +uint16_t PIOS_CRC16_updateCRC(uint16_t crc, const uint8_t *data, int32_t length); uint32_t PIOS_CRC32_updateByte(uint32_t crc, const uint8_t data); -uint32_t PIOS_CRC32_updateCRC(uint32_t crc, const uint8_t* data, int32_t length); +uint32_t PIOS_CRC32_updateCRC(uint32_t crc, const uint8_t *data, int32_t length); #endif /* PIOS_CRC_H */ diff --git a/flight/pios/inc/pios_debug.h b/flight/pios/inc/pios_debug.h index 6cd151e30..8fd7016a9 100644 --- a/flight/pios/inc/pios_debug.h +++ b/flight/pios/inc/pios_debug.h @@ -6,25 +6,25 @@ * @brief Debugging functionality * @{ * - * @file pios_i2c.h + * @file pios_i2c.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Debug helper functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,7 +33,7 @@ #ifdef PIOS_INCLUDE_DEBUG_CONSOLE #ifndef DEBUG_LEVEL -#define DEBUG_LEVEL 0 +#define DEBUG_LEVEL 0 #endif #define DEBUG_PRINTF(level, ...) \ { \ @@ -43,7 +43,7 @@ } #else #define DEBUG_PRINTF(level, ...) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ extern const char *PIOS_DEBUG_AssertMsg; @@ -51,7 +51,7 @@ extern const char *PIOS_DEBUG_AssertMsg; void PIOS_DEBUG_Init(void); #else #include -void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_channels); +void PIOS_DEBUG_Init(const struct pios_tim_channel *channels, uint8_t num_channels); #endif void PIOS_DEBUG_PinHigh(uint8_t pin); @@ -61,22 +61,24 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value); void PIOS_DEBUG_Panic(const char *msg); #ifdef DEBUG -#define PIOS_DEBUG_Assert(test) if (!(test)) PIOS_DEBUG_Panic(PIOS_DEBUG_AssertMsg); -#define PIOS_Assert(test) PIOS_DEBUG_Assert(test) +#define PIOS_DEBUG_Assert(test) if (!(test)) { PIOS_DEBUG_Panic(PIOS_DEBUG_AssertMsg); } +#define PIOS_Assert(test) PIOS_DEBUG_Assert(test) #else #define PIOS_DEBUG_Assert(test) -#define PIOS_Assert(test) if (!(test)) while (1); +#define PIOS_Assert(test) \ + if (!(test)) { while (1) {; } \ + } #endif /* Static (compile-time) assertion for use in a function. If test evaluates to 0 (ie fails) at compile time then compilation will fail with the error: "size of unnamed array is negative" */ -#define PIOS_STATIC_ASSERT(test) ((void)sizeof(int[1 - 2*!(test)])) +#define PIOS_STATIC_ASSERT(test) ((void)sizeof(int[1 - 2 * !(test)])) #endif /* PIOS_DEBUG_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_delay.h b/flight/pios/inc/pios_delay.h index 4b79e3ef1..4a2da6347 100644 --- a/flight/pios/inc/pios_delay.h +++ b/flight/pios/inc/pios_delay.h @@ -8,23 +8,23 @@ * * @file pios_settings.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Settings functions header + * @brief Settings functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,6 +43,6 @@ extern uint32_t PIOS_DELAY_DiffuS(uint32_t raw); #endif /* PIOS_DELAY_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_dsm.h b/flight/pios/inc/pios_dsm.h index 4bd9ed481..815a1cf23 100644 --- a/flight/pios/inc/pios_dsm.h +++ b/flight/pios/inc/pios_dsm.h @@ -11,19 +11,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/inc/pios_dsm_priv.h b/flight/pios/inc/pios_dsm_priv.h index 480273e39..1cdf88f75 100644 --- a/flight/pios/inc/pios_dsm_priv.h +++ b/flight/pios/inc/pios_dsm_priv.h @@ -48,21 +48,21 @@ * for DSM2/DSMJ: * 1 byte - lost frame counter (8 bit) * 1 byte - data format (for master receiver bound with 3 or 5 pulses), - * or unknown (for slave receiver bound with 4 or 6 pulses, + * or unknown (for slave receiver bound with 4 or 6 pulses, * some sources call it also the lost frame counter) * for DSMX: * 1 byte - unknown data (does not look like lost frame counter) * 1 byte - unknown data, has been seen only 0xB2 so far * 14 bytes - up to 7 channels (16 bit word per channel) with encoded channel - * number, channel value, the "2nd frame in a sequence" flag. - * Unused channels have FF FF instead of data bytes. + * number, channel value, the "2nd frame in a sequence" flag. + * Unused channels have FF FF instead of data bytes. * * Data format identification: * - for DSM2/DSMJ: [0 0 0 R 0 0 N1 N0] * where - * R is data resolution (0 - 10 bits, 1 - 11 bits), - * N1..N0 is the number of frames required to receive all channel + * R is data resolution (0 - 10 bits, 1 - 11 bits), + * N1..N0 is the number of frames required to receive all channel * data (01 or 10 are known to the moment, which means 1 or 2 frames). * Three values for the transmitter information byte have been seen * thus far: 0x01, 0x02, 0x12. @@ -78,7 +78,7 @@ * - for 10 bit: [F 0 C3 C2 C1 C0 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0] * - for 11 bit: [F C3 C2 C1 C0 D10 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0] * where - * F is normally 0 but set to 1 for the first channel of the 2nd frame, + * F is normally 0 but set to 1 for the first channel of the 2nd frame, * C3 to C0 is the channel number, 4 bit, zero-based, in any order, * Dx..D0 - channel data (10 or 11 bits) * @@ -98,37 +98,37 @@ * master. */ -#define DSM_CHANNELS_PER_FRAME 7 -#define DSM_FRAME_LENGTH (1+1+DSM_CHANNELS_PER_FRAME*2) -#define DSM_DSM2_RES_MASK 0x0010 -#define DSM_2ND_FRAME_MASK 0x8000 +#define DSM_CHANNELS_PER_FRAME 7 +#define DSM_FRAME_LENGTH (1 + 1 + DSM_CHANNELS_PER_FRAME * 2) +#define DSM_DSM2_RES_MASK 0x0010 +#define DSM_2ND_FRAME_MASK 0x8000 /* * Include lost frame counter and provide it as a last channel value * for debugging. Currently is not used by the receiver layer. */ -//#define DSM_LOST_FRAME_COUNTER +// #define DSM_LOST_FRAME_COUNTER /* DSM protocol variations */ enum pios_dsm_proto { - PIOS_DSM_PROTO_DSM2, - PIOS_DSM_PROTO_DSMX10BIT, - PIOS_DSM_PROTO_DSMX11BIT, + PIOS_DSM_PROTO_DSM2, + PIOS_DSM_PROTO_DSMX10BIT, + PIOS_DSM_PROTO_DSMX11BIT, }; /* DSM receiver instance configuration */ struct pios_dsm_cfg { - struct stm32_gpio bind; + struct stm32_gpio bind; }; extern const struct pios_rcvr_driver pios_dsm_rcvr_driver; extern int32_t PIOS_DSM_Init(uint32_t *dsm_id, - const struct pios_dsm_cfg *cfg, - const struct pios_com_driver *driver, - uint32_t lower_id, - enum pios_dsm_proto proto, - uint8_t bind); + const struct pios_dsm_cfg *cfg, + const struct pios_com_driver *driver, + uint32_t lower_id, + enum pios_dsm_proto proto, + uint8_t bind); #endif /* PIOS_DSM_PRIV_H */ diff --git a/flight/pios/inc/pios_eeprom.h b/flight/pios/inc/pios_eeprom.h index 2ab9a0a90..ee83f8901 100644 --- a/flight/pios/inc/pios_eeprom.h +++ b/flight/pios/inc/pios_eeprom.h @@ -10,21 +10,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief COM layer functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,8 +33,8 @@ /* Public Structures */ struct pios_eeprom_cfg { - uint32_t base_address; - uint32_t max_size; + uint32_t base_address; + uint32_t max_size; }; /* Public Functions */ @@ -45,6 +45,6 @@ extern int32_t PIOS_EEPROM_Load(uint8_t *data, uint32_t len); #endif /* PIOS_EEPROM_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_etasv3.h b/flight/pios/inc/pios_etasv3.h index fab354e5a..177cdb02e 100644 --- a/flight/pios/inc/pios_etasv3.h +++ b/flight/pios/inc/pios_etasv3.h @@ -1,38 +1,38 @@ /** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_ETASV3 ETASV3 Functions - * @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3 - * @{ - * - * @file pios_etasv3.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief ETASV3 Airspeed Sensor Driver - * @see The GNU Public License (GPL) Version 3 - * - ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_ETASV3 ETASV3 Functions + * @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3 + * @{ + * + * @file pios_etasv3.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief ETASV3 Airspeed Sensor Driver + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_ETASV3_H #define PIOS_ETASV3_H -#define ETASV3_I2C_ADDR 0x75 +#define ETASV3_I2C_ADDR 0x75 -int16_t PIOS_ETASV3_ReadAirspeed (void); +int16_t PIOS_ETASV3_ReadAirspeed(void); #endif /* PIOS_ETASV3_H */ diff --git a/flight/pios/inc/pios_exti.h b/flight/pios/inc/pios_exti.h index 4b1203aa2..529e8ee2c 100644 --- a/flight/pios/inc/pios_exti.h +++ b/flight/pios/inc/pios_exti.h @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,21 +36,21 @@ #include struct pios_exti_cfg { - bool (* vector)(void); - uint32_t line; /* use EXTI_LineN macros */ - struct stm32_gpio pin; - struct stm32_irq irq; - struct stm32_exti exti; + bool (*vector)(void); + uint32_t line; /* use EXTI_LineN macros */ + struct stm32_gpio pin; + struct stm32_irq irq; + struct stm32_exti exti; }; /* must be added to any pios_exti_cfg definition for it to be valid */ -#define __exti_config __attribute__((section("_exti"))) +#define __exti_config __attribute__((section("_exti"))) -extern int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg); +extern int32_t PIOS_EXTI_Init(const struct pios_exti_cfg *cfg); #endif /* PIOS_EXTI_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_flash.h b/flight/pios/inc/pios_flash.h index a9de63593..e8e44f67b 100644 --- a/flight/pios/inc/pios_flash.h +++ b/flight/pios/inc/pios_flash.h @@ -30,18 +30,18 @@ #include struct pios_flash_chunk { - uint8_t * addr; - uint32_t len; + uint8_t *addr; + uint32_t len; }; struct pios_flash_driver { - int32_t (*start_transaction)(uintptr_t flash_id); - int32_t (*end_transaction)(uintptr_t flash_id); - int32_t (*erase_chip)(uintptr_t flash_id); - int32_t (*erase_sector)(uintptr_t flash_id, uint32_t addr); - int32_t (*write_data)(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len); - int32_t (*write_chunks)(uintptr_t flash_id, uint32_t addr, struct pios_flash_chunk chunks[], uint32_t num_chunks); - int32_t (*read_data)(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len); + int32_t (*start_transaction)(uintptr_t flash_id); + int32_t (*end_transaction)(uintptr_t flash_id); + int32_t (*erase_chip)(uintptr_t flash_id); + int32_t (*erase_sector)(uintptr_t flash_id, uint32_t addr); + int32_t (*write_data)(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len); + int32_t (*write_chunks)(uintptr_t flash_id, uint32_t addr, struct pios_flash_chunk chunks[], uint32_t num_chunks); + int32_t (*read_data)(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len); }; -#endif /* PIOS_FLASH_H */ +#endif /* PIOS_FLASH_H */ diff --git a/flight/pios/inc/pios_flash_internal_priv.h b/flight/pios/inc/pios_flash_internal_priv.h index 15be5df5f..e8edeed07 100644 --- a/flight/pios/inc/pios_flash_internal_priv.h +++ b/flight/pios/inc/pios_flash_internal_priv.h @@ -2,39 +2,39 @@ ****************************************************************************** * @file pios_flash_internal_priv.h * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 - * @addtogroup + * @addtogroup * @{ - * @addtogroup + * @addtogroup * @{ * @brief Provides a flash driver for the STM32 internal flash sectors *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_FLASH_INTERNAL_H #define PIOS_FLASH_INTERNAL_H -#include "pios_flash.h" /* API definition for flash drivers */ +#include "pios_flash.h" /* API definition for flash drivers */ extern const struct pios_flash_driver pios_internal_flash_driver; struct pios_flash_internal_cfg { - ; + ; }; -extern int32_t PIOS_Flash_Internal_Init(uintptr_t * flash_id, const struct pios_flash_internal_cfg * cfg); +extern int32_t PIOS_Flash_Internal_Init(uintptr_t *flash_id, const struct pios_flash_internal_cfg *cfg); -#endif /* PIOS_FLASH_INTERNAL_H */ +#endif /* PIOS_FLASH_INTERNAL_H */ diff --git a/flight/pios/inc/pios_flash_jedec_catalog.h b/flight/pios/inc/pios_flash_jedec_catalog.h index 4c6349be0..cb15fa8b9 100644 --- a/flight/pios/inc/pios_flash_jedec_catalog.h +++ b/flight/pios/inc/pios_flash_jedec_catalog.h @@ -34,36 +34,36 @@ #include // this structure contains the catalog of all "known" flash chip used -const struct pios_flash_jedec_cfg pios_flash_jedec_catalog [] = +const struct pios_flash_jedec_cfg pios_flash_jedec_catalog[] = { - { // m25p16 - .expect_manufacturer = JEDEC_MANUFACTURER_ST, - .expect_memorytype = 0x20, - .expect_capacity = 0x15, - .sector_erase = 0xD8, - .chip_erase = 0xC7, - }, - { // m25px16 - .expect_manufacturer = JEDEC_MANUFACTURER_ST, - .expect_memorytype = 0x71, - .expect_capacity = 0x15, - .sector_erase = 0xD8, - .chip_erase = 0xC7, - }, - { //w25x - .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, - .expect_memorytype = 0x30, - .expect_capacity = 0x13, - .sector_erase = 0x20, - .chip_erase = 0x60 - }, - { //25q16 - .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, - .expect_memorytype = 0x40, - .expect_capacity = 0x15, - .sector_erase = 0x20, - .chip_erase = 0x60 - } + { // m25p16 + .expect_manufacturer = JEDEC_MANUFACTURER_ST, + .expect_memorytype = 0x20, + .expect_capacity = 0x15, + .sector_erase = 0xD8, + .chip_erase = 0xC7, + }, + { // m25px16 + .expect_manufacturer = JEDEC_MANUFACTURER_ST, + .expect_memorytype = 0x71, + .expect_capacity = 0x15, + .sector_erase = 0xD8, + .chip_erase = 0xC7, + }, + { // w25x + .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, + .expect_memorytype = 0x30, + .expect_capacity = 0x13, + .sector_erase = 0x20, + .chip_erase = 0x60 + }, + { // 25q16 + .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, + .expect_memorytype = 0x40, + .expect_capacity = 0x15, + .sector_erase = 0x20, + .chip_erase = 0x60 + } }; const uint32_t pios_flash_jedec_catalog_size = NELEMENTS(pios_flash_jedec_catalog); diff --git a/flight/pios/inc/pios_flash_jedec_priv.h b/flight/pios/inc/pios_flash_jedec_priv.h index 845d4e296..fb021ee3c 100644 --- a/flight/pios/inc/pios_flash_jedec_priv.h +++ b/flight/pios/inc/pios_flash_jedec_priv.h @@ -31,7 +31,7 @@ #ifndef PIOS_FLASH_JEDEC_H #define PIOS_FLASH_JEDEC_H -#include "pios_flash.h" /* API definition for flash drivers */ +#include "pios_flash.h" /* API definition for flash drivers */ extern const struct pios_flash_driver pios_jedec_flash_driver; @@ -40,13 +40,13 @@ extern const struct pios_flash_driver pios_jedec_flash_driver; #define JEDEC_MANUFACTURER_WINBOND 0xEF struct pios_flash_jedec_cfg { - uint8_t expect_manufacturer; - uint8_t expect_memorytype; - uint8_t expect_capacity; - uint32_t sector_erase; - uint32_t chip_erase; + uint8_t expect_manufacturer; + uint8_t expect_memorytype; + uint8_t expect_capacity; + uint32_t sector_erase; + uint32_t chip_erase; }; -int32_t PIOS_Flash_Jedec_Init(uintptr_t * flash_id, uint32_t spi_id, uint32_t slave_num); +int32_t PIOS_Flash_Jedec_Init(uintptr_t *flash_id, uint32_t spi_id, uint32_t slave_num); -#endif /* PIOS_FLASH_JEDEC_H */ +#endif /* PIOS_FLASH_JEDEC_H */ diff --git a/flight/pios/inc/pios_flashfs.h b/flight/pios/inc/pios_flashfs.h index 073cf9490..0e2dd2844 100644 --- a/flight/pios/inc/pios_flashfs.h +++ b/flight/pios/inc/pios_flashfs.h @@ -30,8 +30,8 @@ #include int32_t PIOS_FLASHFS_Format(uint32_t fs_id); -int32_t PIOS_FLASHFS_ObjSave(uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size); -int32_t PIOS_FLASHFS_ObjLoad(uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size); +int32_t PIOS_FLASHFS_ObjSave(uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size); +int32_t PIOS_FLASHFS_ObjLoad(uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size); int32_t PIOS_FLASHFS_ObjDelete(uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id); -#endif /* PIOS_FLASHFS_H */ +#endif /* PIOS_FLASHFS_H */ diff --git a/flight/pios/inc/pios_flashfs_logfs_priv.h b/flight/pios/inc/pios_flashfs_logfs_priv.h index 88ce65109..940dc8513 100644 --- a/flight/pios/inc/pios_flashfs_logfs_priv.h +++ b/flight/pios/inc/pios_flashfs_logfs_priv.h @@ -28,19 +28,19 @@ #define PIOS_FLASHFS_LOGFS_PRIV_H #include -#include "pios_flash.h" /* struct pios_flash_driver */ +#include "pios_flash.h" /* struct pios_flash_driver */ struct flashfs_logfs_cfg { - uint32_t fs_magic; - uint32_t total_fs_size; /* Total size of all generations of the filesystem */ - uint32_t arena_size; /* Max size of one generation of the filesystem */ - uint32_t slot_size; /* Max size of a "file" */ + uint32_t fs_magic; + uint32_t total_fs_size; /* Total size of all generations of the filesystem */ + uint32_t arena_size; /* Max size of one generation of the filesystem */ + uint32_t slot_size; /* Max size of a "file" */ - uint32_t start_offset; /* Offset into flash where this filesystem starts */ - uint32_t sector_size; /* Size of a flash erase block */ - uint32_t page_size; /* Maximum flash burst write size */ + uint32_t start_offset; /* Offset into flash where this filesystem starts */ + uint32_t sector_size; /* Size of a flash erase block */ + uint32_t page_size; /* Maximum flash burst write size */ }; -int32_t PIOS_FLASHFS_Logfs_Init(uintptr_t * fs_id, const struct flashfs_logfs_cfg * cfg, const struct pios_flash_driver * driver, uintptr_t flash_id); +int32_t PIOS_FLASHFS_Logfs_Init(uintptr_t *fs_id, const struct flashfs_logfs_cfg *cfg, const struct pios_flash_driver *driver, uintptr_t flash_id); -#endif /* PIOS_FLASHFS_LOGFS_PRIV_H */ +#endif /* PIOS_FLASHFS_LOGFS_PRIV_H */ diff --git a/flight/pios/inc/pios_gpio.h b/flight/pios/inc/pios_gpio.h index a611ff053..aec0ba417 100644 --- a/flight/pios/inc/pios_gpio.h +++ b/flight/pios/inc/pios_gpio.h @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,6 +41,6 @@ extern void PIOS_GPIO_Toggle(uint8_t Pin); #endif /* PIOS_GPIO_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_hcsr04.h b/flight/pios/inc/pios_hcsr04.h index fe735e336..9de75ad62 100644 --- a/flight/pios/inc/pios_hcsr04.h +++ b/flight/pios/inc/pios_hcsr04.h @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,7 +38,7 @@ extern void PIOS_HCSR04_Trigger(void); #endif /* PIOS_HCSR04_H */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_hcsr04_priv.h b/flight/pios/inc/pios_hcsr04_priv.h index c53bdddf1..a60138bf5 100644 --- a/flight/pios/inc/pios_hcsr04_priv.h +++ b/flight/pios/inc/pios_hcsr04_priv.h @@ -37,15 +37,15 @@ #include struct pios_hcsr04_cfg { - TIM_ICInitTypeDef tim_ic_init; - const struct pios_tim_channel * channels; - uint8_t num_channels; - struct stm32_gpio trigger; + TIM_ICInitTypeDef tim_ic_init; + const struct pios_tim_channel *channels; + uint8_t num_channels; + struct stm32_gpio trigger; }; extern const struct pios_rcvr_driver pios_pwm_rcvr_driver; -extern int32_t PIOS_HCSR04_Init(uint32_t * pwm_id, const struct pios_hcsr04_cfg * cfg); +extern int32_t PIOS_HCSR04_Init(uint32_t *pwm_id, const struct pios_hcsr04_cfg *cfg); #endif /* PIOS_HCSR04_PRIV_H */ diff --git a/flight/pios/inc/pios_hmc5843.h b/flight/pios/inc/pios_hmc5843.h index 21b3f426a..72f1e02b0 100644 --- a/flight/pios/inc/pios_hmc5843.h +++ b/flight/pios/inc/pios_hmc5843.h @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,7 +39,7 @@ extern void PIOS_HMC5843_ReadID(uint8_t out[4]); #endif /* PIOS_HMC5843_H */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_hmc5883.h b/flight/pios/inc/pios_hmc5883.h index 921b99d07..6d34dae20 100644 --- a/flight/pios/inc/pios_hmc5883.h +++ b/flight/pios/inc/pios_hmc5883.h @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,77 +32,76 @@ #define PIOS_HMC5883_H /* HMC5883 Addresses */ -#define PIOS_HMC5883_I2C_ADDR 0x1E +#define PIOS_HMC5883_I2C_ADDR 0x1E #define PIOS_HMC5883_I2C_READ_ADDR 0x3D #define PIOS_HMC5883_I2C_WRITE_ADDR 0x3C -#define PIOS_HMC5883_CONFIG_REG_A (uint8_t)0x00 -#define PIOS_HMC5883_CONFIG_REG_B (uint8_t)0x01 -#define PIOS_HMC5883_MODE_REG (uint8_t)0x02 -#define PIOS_HMC5883_DATAOUT_XMSB_REG 0x03 -#define PIOS_HMC5883_DATAOUT_XLSB_REG 0x04 -#define PIOS_HMC5883_DATAOUT_ZMSB_REG 0x05 -#define PIOS_HMC5883_DATAOUT_ZLSB_REG 0x06 -#define PIOS_HMC5883_DATAOUT_YMSB_REG 0x07 -#define PIOS_HMC5883_DATAOUT_YLSB_REG 0x08 -#define PIOS_HMC5883_DATAOUT_STATUS_REG 0x09 -#define PIOS_HMC5883_DATAOUT_IDA_REG 0x0A -#define PIOS_HMC5883_DATAOUT_IDB_REG 0x0B -#define PIOS_HMC5883_DATAOUT_IDC_REG 0x0C +#define PIOS_HMC5883_CONFIG_REG_A (uint8_t)0x00 +#define PIOS_HMC5883_CONFIG_REG_B (uint8_t)0x01 +#define PIOS_HMC5883_MODE_REG (uint8_t)0x02 +#define PIOS_HMC5883_DATAOUT_XMSB_REG 0x03 +#define PIOS_HMC5883_DATAOUT_XLSB_REG 0x04 +#define PIOS_HMC5883_DATAOUT_ZMSB_REG 0x05 +#define PIOS_HMC5883_DATAOUT_ZLSB_REG 0x06 +#define PIOS_HMC5883_DATAOUT_YMSB_REG 0x07 +#define PIOS_HMC5883_DATAOUT_YLSB_REG 0x08 +#define PIOS_HMC5883_DATAOUT_STATUS_REG 0x09 +#define PIOS_HMC5883_DATAOUT_IDA_REG 0x0A +#define PIOS_HMC5883_DATAOUT_IDB_REG 0x0B +#define PIOS_HMC5883_DATAOUT_IDC_REG 0x0C /* Output Data Rate */ -#define PIOS_HMC5883_ODR_0_75 0x00 -#define PIOS_HMC5883_ODR_1_5 0x04 -#define PIOS_HMC5883_ODR_3 0x08 -#define PIOS_HMC5883_ODR_7_5 0x0C -#define PIOS_HMC5883_ODR_15 0x10 -#define PIOS_HMC5883_ODR_30 0x14 -#define PIOS_HMC5883_ODR_75 0x18 +#define PIOS_HMC5883_ODR_0_75 0x00 +#define PIOS_HMC5883_ODR_1_5 0x04 +#define PIOS_HMC5883_ODR_3 0x08 +#define PIOS_HMC5883_ODR_7_5 0x0C +#define PIOS_HMC5883_ODR_15 0x10 +#define PIOS_HMC5883_ODR_30 0x14 +#define PIOS_HMC5883_ODR_75 0x18 /* Measure configuration */ -#define PIOS_HMC5883_MEASCONF_NORMAL 0x00 -#define PIOS_HMC5883_MEASCONF_BIAS_POS 0x01 -#define PIOS_HMC5883_MEASCONF_BIAS_NEG 0x02 +#define PIOS_HMC5883_MEASCONF_NORMAL 0x00 +#define PIOS_HMC5883_MEASCONF_BIAS_POS 0x01 +#define PIOS_HMC5883_MEASCONF_BIAS_NEG 0x02 /* Gain settings */ -#define PIOS_HMC5883_GAIN_0_88 0x00 -#define PIOS_HMC5883_GAIN_1_3 0x20 -#define PIOS_HMC5883_GAIN_1_9 0x40 -#define PIOS_HMC5883_GAIN_2_5 0x60 -#define PIOS_HMC5883_GAIN_4_0 0x80 -#define PIOS_HMC5883_GAIN_4_7 0xA0 -#define PIOS_HMC5883_GAIN_5_6 0xC0 -#define PIOS_HMC5883_GAIN_8_1 0xE0 +#define PIOS_HMC5883_GAIN_0_88 0x00 +#define PIOS_HMC5883_GAIN_1_3 0x20 +#define PIOS_HMC5883_GAIN_1_9 0x40 +#define PIOS_HMC5883_GAIN_2_5 0x60 +#define PIOS_HMC5883_GAIN_4_0 0x80 +#define PIOS_HMC5883_GAIN_4_7 0xA0 +#define PIOS_HMC5883_GAIN_5_6 0xC0 +#define PIOS_HMC5883_GAIN_8_1 0xE0 /* Modes */ -#define PIOS_HMC5883_MODE_CONTINUOUS 0x00 -#define PIOS_HMC5883_MODE_SINGLE 0x01 -#define PIOS_HMC5883_MODE_IDLE 0x02 -#define PIOS_HMC5883_MODE_SLEEP 0x03 +#define PIOS_HMC5883_MODE_CONTINUOUS 0x00 +#define PIOS_HMC5883_MODE_SINGLE 0x01 +#define PIOS_HMC5883_MODE_IDLE 0x02 +#define PIOS_HMC5883_MODE_SLEEP 0x03 /* Sensitivity Conversion Values */ -#define PIOS_HMC5883_Sensitivity_0_88Ga 1370 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_1_3Ga 1090 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_1_9Ga 820 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_2_5Ga 660 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_4_0Ga 440 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_4_7Ga 390 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_5_6Ga 330 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED +#define PIOS_HMC5883_Sensitivity_0_88Ga 1370 // LSB/Ga +#define PIOS_HMC5883_Sensitivity_1_3Ga 1090 // LSB/Ga +#define PIOS_HMC5883_Sensitivity_1_9Ga 820 // LSB/Ga +#define PIOS_HMC5883_Sensitivity_2_5Ga 660 // LSB/Ga +#define PIOS_HMC5883_Sensitivity_4_0Ga 440 // LSB/Ga +#define PIOS_HMC5883_Sensitivity_4_7Ga 390 // LSB/Ga +#define PIOS_HMC5883_Sensitivity_5_6Ga 330 // LSB/Ga +#define PIOS_HMC5883_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED struct pios_hmc5883_cfg { #ifdef PIOS_HMC5883_HAS_GPIOS - const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ + const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ #endif - uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ - uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ - uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ - uint8_t Mode; - + uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ + uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t Mode; }; /* Public Functions */ -extern void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg * cfg); +extern void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg *cfg); extern bool PIOS_HMC5883_NewDataAvailable(void); extern int32_t PIOS_HMC5883_ReadMag(int16_t out[3]); extern uint8_t PIOS_HMC5883_ReadID(uint8_t out[4]); @@ -111,7 +110,7 @@ extern bool PIOS_HMC5883_IRQHandler(); #endif /* PIOS_HMC5883_H */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_i2c.h b/flight/pios/inc/pios_i2c.h index b34acf9e6..db6636f37 100644 --- a/flight/pios/inc/pios_i2c.h +++ b/flight/pios/inc/pios_i2c.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_I2C I2C Functions * @{ * - * @file pios_i2c.h + * @file pios_i2c.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief I2C functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,31 +36,31 @@ /* Global Types */ enum pios_i2c_txn_direction { - PIOS_I2C_TXN_READ, - PIOS_I2C_TXN_WRITE + PIOS_I2C_TXN_READ, + PIOS_I2C_TXN_WRITE }; struct pios_i2c_txn { - const char *info; - uint16_t addr; - enum pios_i2c_txn_direction rw; - uint32_t len; - uint8_t *buf; + const char *info; + uint16_t addr; + enum pios_i2c_txn_direction rw; + uint32_t len; + uint8_t *buf; }; #define I2C_LOG_DEPTH 20 enum pios_i2c_error_type { - PIOS_I2C_ERROR_EVENT, - PIOS_I2C_ERROR_FSM, - PIOS_I2C_ERROR_INTERRUPT + PIOS_I2C_ERROR_EVENT, + PIOS_I2C_ERROR_FSM, + PIOS_I2C_ERROR_INTERRUPT }; struct pios_i2c_fault_history { - enum pios_i2c_error_type type; - uint32_t evirq[I2C_LOG_DEPTH]; - uint32_t erirq[I2C_LOG_DEPTH]; - uint8_t event[I2C_LOG_DEPTH]; - uint8_t state[I2C_LOG_DEPTH]; + enum pios_i2c_error_type type; + uint32_t evirq[I2C_LOG_DEPTH]; + uint32_t erirq[I2C_LOG_DEPTH]; + uint8_t event[I2C_LOG_DEPTH]; + uint8_t state[I2C_LOG_DEPTH]; }; /* Public Functions */ @@ -68,11 +68,11 @@ extern int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_ extern int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns, void *callback); extern void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id); extern void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id); -extern void PIOS_I2C_GetDiagnostics(struct pios_i2c_fault_history * data, uint8_t * error_counts); +extern void PIOS_I2C_GetDiagnostics(struct pios_i2c_fault_history *data, uint8_t *error_counts); #endif /* PIOS_I2C_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_i2c_esc.h b/flight/pios/inc/pios_i2c_esc.h index c403030b3..67e84b80c 100644 --- a/flight/pios/inc/pios_i2c_esc.h +++ b/flight/pios/inc/pios_i2c_esc.h @@ -13,19 +13,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,7 +43,7 @@ bool PIOS_SetAstec4Speed(uint8_t motornum, uint8_t speed); #endif /* PIOS_I2C_ESC_H */ -/** +/** * @} * @} */ diff --git a/flight/pios/inc/pios_i2c_priv.h b/flight/pios/inc/pios_i2c_priv.h index 8a2769321..aeeff5648 100644 --- a/flight/pios/inc/pios_i2c_priv.h +++ b/flight/pios/inc/pios_i2c_priv.h @@ -31,81 +31,81 @@ #include struct pios_i2c_adapter_cfg { - I2C_TypeDef *regs; - uint32_t remap; - I2C_InitTypeDef init; + I2C_TypeDef *regs; + uint32_t remap; + I2C_InitTypeDef init; - uint32_t transfer_timeout_ms; - struct stm32_gpio scl; - struct stm32_gpio sda; - struct stm32_irq event; - struct stm32_irq error; + uint32_t transfer_timeout_ms; + struct stm32_gpio scl; + struct stm32_gpio sda; + struct stm32_irq event; + struct stm32_irq error; }; enum i2c_adapter_state { - I2C_STATE_FSM_FAULT = 0, /* Must be zero so undefined transitions land here */ + I2C_STATE_FSM_FAULT = 0, /* Must be zero so undefined transitions land here */ - I2C_STATE_BUS_ERROR, + I2C_STATE_BUS_ERROR, - I2C_STATE_STOPPED, - I2C_STATE_STOPPING, - I2C_STATE_STARTING, + I2C_STATE_STOPPED, + I2C_STATE_STOPPING, + I2C_STATE_STARTING, - I2C_STATE_R_MORE_TXN_ADDR, - I2C_STATE_R_MORE_TXN_PRE_ONE, - I2C_STATE_R_MORE_TXN_PRE_FIRST, - I2C_STATE_R_MORE_TXN_PRE_MIDDLE, - I2C_STATE_R_MORE_TXN_PRE_LAST, - I2C_STATE_R_MORE_TXN_POST_LAST, + I2C_STATE_R_MORE_TXN_ADDR, + I2C_STATE_R_MORE_TXN_PRE_ONE, + I2C_STATE_R_MORE_TXN_PRE_FIRST, + I2C_STATE_R_MORE_TXN_PRE_MIDDLE, + I2C_STATE_R_MORE_TXN_PRE_LAST, + I2C_STATE_R_MORE_TXN_POST_LAST, - I2C_STATE_R_LAST_TXN_ADDR, - I2C_STATE_R_LAST_TXN_PRE_ONE, - I2C_STATE_R_LAST_TXN_PRE_FIRST, - I2C_STATE_R_LAST_TXN_PRE_MIDDLE, - I2C_STATE_R_LAST_TXN_PRE_LAST, - I2C_STATE_R_LAST_TXN_POST_LAST, + I2C_STATE_R_LAST_TXN_ADDR, + I2C_STATE_R_LAST_TXN_PRE_ONE, + I2C_STATE_R_LAST_TXN_PRE_FIRST, + I2C_STATE_R_LAST_TXN_PRE_MIDDLE, + I2C_STATE_R_LAST_TXN_PRE_LAST, + I2C_STATE_R_LAST_TXN_POST_LAST, - I2C_STATE_W_MORE_TXN_ADDR, - I2C_STATE_W_MORE_TXN_MIDDLE, - I2C_STATE_W_MORE_TXN_LAST, + I2C_STATE_W_MORE_TXN_ADDR, + I2C_STATE_W_MORE_TXN_MIDDLE, + I2C_STATE_W_MORE_TXN_LAST, - I2C_STATE_W_LAST_TXN_ADDR, - I2C_STATE_W_LAST_TXN_MIDDLE, - I2C_STATE_W_LAST_TXN_LAST, - - I2C_STATE_NACK, - - I2C_STATE_NUM_STATES /* Must be last */ + I2C_STATE_W_LAST_TXN_ADDR, + I2C_STATE_W_LAST_TXN_MIDDLE, + I2C_STATE_W_LAST_TXN_LAST, + + I2C_STATE_NACK, + + I2C_STATE_NUM_STATES /* Must be last */ }; enum pios_i2c_adapter_magic { - PIOS_I2C_DEV_MAGIC = 0xa9a9b8b8, + PIOS_I2C_DEV_MAGIC = 0xa9a9b8b8, }; struct pios_i2c_adapter { - enum pios_i2c_adapter_magic magic; - const struct pios_i2c_adapter_cfg * cfg; + enum pios_i2c_adapter_magic magic; + const struct pios_i2c_adapter_cfg *cfg; #ifdef PIOS_INCLUDE_FREERTOS - xSemaphoreHandle sem_busy; - xSemaphoreHandle sem_ready; + xSemaphoreHandle sem_busy; + xSemaphoreHandle sem_ready; #else - uint8_t busy; + uint8_t busy; #endif - bool bus_error; - bool nack; + bool bus_error; + bool nack; - volatile enum i2c_adapter_state curr_state; - const struct pios_i2c_txn *first_txn; - const struct pios_i2c_txn *active_txn; - const struct pios_i2c_txn *last_txn; + volatile enum i2c_adapter_state curr_state; + const struct pios_i2c_txn *first_txn; + const struct pios_i2c_txn *active_txn; + const struct pios_i2c_txn *last_txn; - void (*callback) (); - - uint8_t *active_byte; - uint8_t *last_byte; + void (*callback)(); + + uint8_t *active_byte; + uint8_t *last_byte; }; -int32_t PIOS_I2C_Init(uint32_t * i2c_id, const struct pios_i2c_adapter_cfg * cfg); +int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg); #endif /* PIOS_I2C_PRIV_H */ diff --git a/flight/pios/inc/pios_iap.h b/flight/pios/inc/pios_iap.h index 20a119f4d..0c85ec6ba 100644 --- a/flight/pios/inc/pios_iap.h +++ b/flight/pios/inc/pios_iap.h @@ -6,25 +6,25 @@ * @brief In-Application-Programming Module * @{ * - * @file pios_iap.c + * @file pios_iap.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief IAP functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,54 +33,53 @@ #define PIOS_IAP_H - /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ #include /***************************************************************************************** * Public Definitions/Macros ****************************************************************************************/ -#define MAGIC_REG_1 PIOS_BKP_RESERVED_1 -#define MAGIC_REG_2 PIOS_BKP_RESERVED_2 -#define IAP_BOOTCOUNT PIOS_BKP_RESERVED_3 -#define IAP_CMD1 PIOS_BKP_RESERVED_5 -#define IAP_CMD2 PIOS_BKP_RESERVED_6 -#define IAP_CMD3 PIOS_BKP_RESERVED_7 +#define MAGIC_REG_1 PIOS_BKP_RESERVED_1 +#define MAGIC_REG_2 PIOS_BKP_RESERVED_2 +#define IAP_BOOTCOUNT PIOS_BKP_RESERVED_3 +#define IAP_CMD1 PIOS_BKP_RESERVED_5 +#define IAP_CMD2 PIOS_BKP_RESERVED_6 +#define IAP_CMD3 PIOS_BKP_RESERVED_7 #define PIOS_IAP_CLEAR_FLASH_CMD_0 0xFA5F #define PIOS_IAP_CLEAR_FLASH_CMD_1 0x0001 #define PIOS_IAP_CLEAR_FLASH_CMD_2 0x0000 -#define PIOS_IAP_CMD_COUNT 3 +#define PIOS_IAP_CMD_COUNT 3 /**************************************************************************************** - * Public Functions - ****************************************************************************************/ -void PIOS_IAP_Init(void); -uint32_t PIOS_IAP_CheckRequest( void ); -void PIOS_IAP_SetRequest1(void); -void PIOS_IAP_SetRequest2(void); -void PIOS_IAP_ClearRequest(void); -uint16_t PIOS_IAP_ReadBootCount(void); -void PIOS_IAP_WriteBootCount(uint16_t); +* Public Functions +****************************************************************************************/ +void PIOS_IAP_Init(void); +uint32_t PIOS_IAP_CheckRequest(void); +void PIOS_IAP_SetRequest1(void); +void PIOS_IAP_SetRequest2(void); +void PIOS_IAP_ClearRequest(void); +uint16_t PIOS_IAP_ReadBootCount(void); +void PIOS_IAP_WriteBootCount(uint16_t); /** - * @brief Return one of the IAP command values passed from bootloader. - * @param number: the index of the command value (0..2). - * @retval the selected command value. - */ + * @brief Return one of the IAP command values passed from bootloader. + * @param number: the index of the command value (0..2). + * @retval the selected command value. + */ uint32_t PIOS_IAP_ReadBootCmd(uint8_t number); /** - * @brief Write one of the IAP command values to be passed to firmware from bootloader. - * @param number: the index of the command value (0..2). - * @param value: value to be written. - */ + * @brief Write one of the IAP command values to be passed to firmware from bootloader. + * @param number: the index of the command value (0..2). + * @param value: value to be written. + */ void PIOS_IAP_WriteBootCmd(uint8_t number, uint32_t value); /**************************************************************************************** - * Public Data - ****************************************************************************************/ +* Public Data +****************************************************************************************/ #endif /* PIOS_IAP_H */ diff --git a/flight/pios/inc/pios_initcall.h b/flight/pios/inc/pios_initcall.h index 410c3c72a..6614b48dd 100644 --- a/flight/pios/inc/pios_initcall.h +++ b/flight/pios/inc/pios_initcall.h @@ -6,32 +6,32 @@ * @brief Initcall Macros * @{ * - * @file pios_initcall.h + * @file pios_initcall.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. * @brief Initcall header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_INITCALL_H #define PIOS_INITCALL_H -/* +/* * This implementation is heavily based on the Linux Kernel initcall * infrastructure: * http://lxr.linux.no/#linux/include/linux/init.h @@ -44,8 +44,8 @@ typedef int32_t (*initcall_t)(void); typedef struct { - initcall_t fn_minit; - initcall_t fn_tinit; + initcall_t fn_minit; + initcall_t fn_tinit; } initmodule_t; /* Init module section */ @@ -58,20 +58,22 @@ extern void StartModules(); #define MODULE_INITCALL(ifn, sfn) -#define MODULE_TASKCREATE_ALL { \ - /* Start all module threads */ \ - StartModules(); \ - } +#define MODULE_TASKCREATE_ALL \ + { \ + /* Start all module threads */ \ + StartModules(); \ + } -#define MODULE_INITIALISE_ALL { \ - /* Initialize modules */ \ - InitModules(); \ - /* Initialize the system thread */ \ - SystemModInitialize();} +#define MODULE_INITIALISE_ALL \ + { \ + /* Initialize modules */ \ + InitModules(); \ + /* Initialize the system thread */ \ + SystemModInitialize(); } #else -/* initcalls are now grouped by functionality into separate +/* initcalls are now grouped by functionality into separate * subsections. Ordering inside the subsections is determined * by link order. * @@ -79,27 +81,33 @@ extern void StartModules(); * can point at the same handler without causing duplicate-symbol build errors. */ -#define __define_initcall(level,fn,id) \ - static initcall_t __initcall_##fn##id __attribute__((__used__)) \ - __attribute__((__section__(".initcall" level ".init"))) = fn +#define __define_initcall(level, fn, id) \ + static initcall_t __initcall_##fn##id __attribute__((__used__)) \ + __attribute__((__section__(".initcall" level ".init"))) = fn #define __define_module_initcall(level, ifn, sfn) \ - static initmodule_t __initcall_##fn __attribute__((__used__)) \ - __attribute__((__section__(".initcall" level ".init"))) = { .fn_minit = ifn, .fn_tinit = sfn }; + static initmodule_t __initcall_##fn __attribute__((__used__)) \ + __attribute__((__section__(".initcall" level ".init"))) = { .fn_minit = ifn, .fn_tinit = sfn }; -#define MODULE_INITCALL(ifn, sfn) __define_module_initcall("module", ifn, sfn) +#define MODULE_INITCALL(ifn, sfn) __define_module_initcall("module", ifn, sfn) -#define MODULE_INITIALISE_ALL { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) \ - if (fn->fn_minit) \ - (fn->fn_minit)(); } +#define MODULE_INITIALISE_ALL \ + { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { \ + if (fn->fn_minit) { \ + (fn->fn_minit)(); } \ + } \ + } -#define MODULE_TASKCREATE_ALL { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) \ - if (fn->fn_tinit) \ - (fn->fn_tinit)(); } +#define MODULE_TASKCREATE_ALL \ + { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { \ + if (fn->fn_tinit) { \ + (fn->fn_tinit)(); } \ + } \ + } #endif /* USE_SIM_POSIX */ -#endif /* PIOS_INITCALL_H */ +#endif /* PIOS_INITCALL_H */ /** * @} diff --git a/flight/pios/inc/pios_irq.h b/flight/pios/inc/pios_irq.h index f9b9dbb97..e934ae1f6 100644 --- a/flight/pios/inc/pios_irq.h +++ b/flight/pios/inc/pios_irq.h @@ -5,26 +5,26 @@ * @addtogroup PIOS_IRQ IRQ Setup Functions * @{ * - * @file pios_irq.h + * @file pios_irq.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief IRQ functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/inc/pios_l3gd20.h b/flight/pios/inc/pios_l3gd20.h index 794c5c2f2..0c6b3e43c 100644 --- a/flight/pios/inc/pios_l3gd20.h +++ b/flight/pios/inc/pios_l3gd20.h @@ -13,19 +13,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -103,38 +103,38 @@ #define PIOS_L3GD20_PWRMGMT_STOP_CLK 0X07 enum pios_l3gd20_range { - PIOS_L3GD20_SCALE_250_DEG = 0x00, - PIOS_L3GD20_SCALE_500_DEG = 0x10, - PIOS_L3GD20_SCALE_2000_DEG = 0x3 + PIOS_L3GD20_SCALE_250_DEG = 0x00, + PIOS_L3GD20_SCALE_500_DEG = 0x10, + PIOS_L3GD20_SCALE_2000_DEG = 0x3 }; enum pios_l3gd20_filter { - PIOS_L3GD20_LOWPASS_256_HZ = 0x00, - PIOS_L3GD20_LOWPASS_188_HZ = 0x01, - PIOS_L3GD20_LOWPASS_98_HZ = 0x02, - PIOS_L3GD20_LOWPASS_42_HZ = 0x03, - PIOS_L3GD20_LOWPASS_20_HZ = 0x04, - PIOS_L3GD20_LOWPASS_10_HZ = 0x05, - PIOS_L3GD20_LOWPASS_5_HZ = 0x06 + PIOS_L3GD20_LOWPASS_256_HZ = 0x00, + PIOS_L3GD20_LOWPASS_188_HZ = 0x01, + PIOS_L3GD20_LOWPASS_98_HZ = 0x02, + PIOS_L3GD20_LOWPASS_42_HZ = 0x03, + PIOS_L3GD20_LOWPASS_20_HZ = 0x04, + PIOS_L3GD20_LOWPASS_10_HZ = 0x05, + PIOS_L3GD20_LOWPASS_5_HZ = 0x06 }; struct pios_l3gd20_data { - int16_t gyro_x; - int16_t gyro_y; - int16_t gyro_z; - int16_t temperature; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + int16_t temperature; }; struct pios_l3gd20_cfg { - const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ + const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ - enum pios_l3gd20_range range; + enum pios_l3gd20_range range; }; /* Public Functions */ -extern int32_t PIOS_L3GD20_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_l3gd20_cfg * cfg); +extern int32_t PIOS_L3GD20_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_l3gd20_cfg *cfg); extern xQueueHandle PIOS_L3GD20_GetQueue(); -extern int32_t PIOS_L3GD20_ReadGyros(struct pios_l3gd20_data * buffer); +extern int32_t PIOS_L3GD20_ReadGyros(struct pios_l3gd20_data *buffer); extern int32_t PIOS_L3GD20_SetRange(enum pios_l3gd20_range range); extern float PIOS_L3GD20_GetScale(); extern int32_t PIOS_L3GD20_ReadID(); @@ -143,7 +143,7 @@ extern bool PIOS_L3GD20_IRQHandler(); #endif /* PIOS_L3GD20_H */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_led.h b/flight/pios/inc/pios_led.h index 2506b75cf..2732ad56c 100644 --- a/flight/pios/inc/pios_led.h +++ b/flight/pios/inc/pios_led.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_LED LED Functions * @{ * - * @file pios_led.h + * @file pios_led.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief LED functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/inc/pios_led_priv.h b/flight/pios/inc/pios_led_priv.h index 36a1ee3f7..e0ce76a65 100644 --- a/flight/pios/inc/pios_led_priv.h +++ b/flight/pios/inc/pios_led_priv.h @@ -35,21 +35,21 @@ #include struct pios_led { - struct stm32_gpio pin; - uint32_t remap; - bool active_high; + struct stm32_gpio pin; + uint32_t remap; + bool active_high; }; struct pios_led_cfg { - const struct pios_led * leds; - uint8_t num_leds; + const struct pios_led *leds; + uint8_t num_leds; }; -extern int32_t PIOS_LED_Init(const struct pios_led_cfg * cfg); +extern int32_t PIOS_LED_Init(const struct pios_led_cfg *cfg); #endif /* PIOS_LED_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h index d81249e56..47c8d798b 100644 --- a/flight/pios/inc/pios_math.h +++ b/flight/pios/inc/pios_math.h @@ -37,7 +37,7 @@ #define M_PI_2_F 1.57079632679489661923132169164f /* pi/2 */ #define M_PI_4_F 0.78539816339744830961566084582f /* pi/4 */ #define M_SQRTPI_F 1.77245385090551602729816748334f /* sqrt(pi) */ -#define M_2_SQRTPI_F 1.12837916709551257389615890312f /* 2/sqrt(pi) */ +#define M_2_SQRTPI_F 1.12837916709551257389615890312f /* 2/sqrt(pi) */ #define M_1_PI_F 0.31830988618379067153776752675f /* 1/pi */ #define M_2_PI_F 0.63661977236758134307553505349f /* 2/pi */ #define M_LN10_F 2.30258509299404568401799145468f /* ln(10) */ @@ -46,11 +46,11 @@ #define M_EULER_F 0.57721566490153286060651209008f /* Euler constant */ // Conversion macro -#define RAD2DEG(rad) ((rad)*(180.0f/M_PI_F)) -#define DEG2RAD(deg) ((deg)*(M_PI_F/180.0f)) +#define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F)) +#define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f)) // Useful math macros -#define MAX(a,b) ((a) > (b) ? (a) : (b)) -#define MIN(a,b) ((a) < (b) ? (a) : (b)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#define MIN(a, b) ((a) < (b) ? (a) : (b)) #endif // PIOS_MATH_H diff --git a/flight/pios/inc/pios_mpu6000.h b/flight/pios/inc/pios_mpu6000.h index 80b924331..91ea89d3e 100644 --- a/flight/pios/inc/pios_mpu6000.h +++ b/flight/pios/inc/pios_mpu6000.h @@ -13,19 +13,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -100,71 +100,71 @@ #define PIOS_MPU6000_PWRMGMT_STOP_CLK 0X07 enum pios_mpu6000_range { - PIOS_MPU6000_SCALE_250_DEG = 0x00, - PIOS_MPU6000_SCALE_500_DEG = 0x08, - PIOS_MPU6000_SCALE_1000_DEG = 0x10, - PIOS_MPU6000_SCALE_2000_DEG = 0x18 + PIOS_MPU6000_SCALE_250_DEG = 0x00, + PIOS_MPU6000_SCALE_500_DEG = 0x08, + PIOS_MPU6000_SCALE_1000_DEG = 0x10, + PIOS_MPU6000_SCALE_2000_DEG = 0x18 }; enum pios_mpu6000_filter { - PIOS_MPU6000_LOWPASS_256_HZ = 0x00, - PIOS_MPU6000_LOWPASS_188_HZ = 0x01, - PIOS_MPU6000_LOWPASS_98_HZ = 0x02, - PIOS_MPU6000_LOWPASS_42_HZ = 0x03, - PIOS_MPU6000_LOWPASS_20_HZ = 0x04, - PIOS_MPU6000_LOWPASS_10_HZ = 0x05, - PIOS_MPU6000_LOWPASS_5_HZ = 0x06 + PIOS_MPU6000_LOWPASS_256_HZ = 0x00, + PIOS_MPU6000_LOWPASS_188_HZ = 0x01, + PIOS_MPU6000_LOWPASS_98_HZ = 0x02, + PIOS_MPU6000_LOWPASS_42_HZ = 0x03, + PIOS_MPU6000_LOWPASS_20_HZ = 0x04, + PIOS_MPU6000_LOWPASS_10_HZ = 0x05, + PIOS_MPU6000_LOWPASS_5_HZ = 0x06 }; enum pios_mpu6000_accel_range { - PIOS_MPU6000_ACCEL_2G = 0x00, - PIOS_MPU6000_ACCEL_4G = 0x08, - PIOS_MPU6000_ACCEL_8G = 0x10, - PIOS_MPU6000_ACCEL_16G = 0x18 + PIOS_MPU6000_ACCEL_2G = 0x00, + PIOS_MPU6000_ACCEL_4G = 0x08, + PIOS_MPU6000_ACCEL_8G = 0x10, + PIOS_MPU6000_ACCEL_16G = 0x18 }; enum pios_mpu6000_orientation { // clockwise rotation from board forward - PIOS_MPU6000_TOP_0DEG = 0x00, - PIOS_MPU6000_TOP_90DEG = 0x01, - PIOS_MPU6000_TOP_180DEG = 0x02, - PIOS_MPU6000_TOP_270DEG = 0x03 + PIOS_MPU6000_TOP_0DEG = 0x00, + PIOS_MPU6000_TOP_90DEG = 0x01, + PIOS_MPU6000_TOP_180DEG = 0x02, + PIOS_MPU6000_TOP_270DEG = 0x03 }; struct pios_mpu6000_data { - int16_t gyro_x; - int16_t gyro_y; - int16_t gyro_z; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; #if defined(PIOS_MPU6000_ACCEL) - int16_t accel_x; - int16_t accel_y; - int16_t accel_z; + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; #endif /* PIOS_MPU6000_ACCEL */ - int16_t temperature; + int16_t temperature; }; struct pios_mpu6000_cfg { - const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ - - uint8_t Fifo_store; /* FIFO storage of different readings (See datasheet page 31 for more details) */ - - /* Sample rate divider to use (See datasheet page 32 for more details).*/ - uint8_t Smpl_rate_div_no_dlp; /* used when no dlp is applied (fs=8KHz)*/ - uint8_t Smpl_rate_div_dlp; /* used when dlp is on (fs=1kHz)*/ - uint8_t interrupt_cfg; /* Interrupt configuration (See datasheet page 35 for more details) */ - uint8_t interrupt_en; /* Interrupt configuration (See datasheet page 35 for more details) */ - uint8_t User_ctl; /* User control settings (See datasheet page 41 for more details) */ - uint8_t Pwr_mgmt_clk; /* Power management and clock selection (See datasheet page 32 for more details) */ - enum pios_mpu6000_accel_range accel_range; - enum pios_mpu6000_range gyro_range; - enum pios_mpu6000_filter filter; - enum pios_mpu6000_orientation orientation; + const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ + + uint8_t Fifo_store; /* FIFO storage of different readings (See datasheet page 31 for more details) */ + + /* Sample rate divider to use (See datasheet page 32 for more details).*/ + uint8_t Smpl_rate_div_no_dlp; /* used when no dlp is applied (fs=8KHz)*/ + uint8_t Smpl_rate_div_dlp; /* used when dlp is on (fs=1kHz)*/ + uint8_t interrupt_cfg; /* Interrupt configuration (See datasheet page 35 for more details) */ + uint8_t interrupt_en; /* Interrupt configuration (See datasheet page 35 for more details) */ + uint8_t User_ctl; /* User control settings (See datasheet page 41 for more details) */ + uint8_t Pwr_mgmt_clk; /* Power management and clock selection (See datasheet page 32 for more details) */ + enum pios_mpu6000_accel_range accel_range; + enum pios_mpu6000_range gyro_range; + enum pios_mpu6000_filter filter; + enum pios_mpu6000_orientation orientation; }; /* Public Functions */ -extern int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg * new_cfg); -extern int32_t PIOS_MPU6000_ConfigureRanges(enum pios_mpu6000_range gyroRange, enum pios_mpu6000_accel_range accelRange,enum pios_mpu6000_filter filterSetting); +extern int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *new_cfg); +extern int32_t PIOS_MPU6000_ConfigureRanges(enum pios_mpu6000_range gyroRange, enum pios_mpu6000_accel_range accelRange, enum pios_mpu6000_filter filterSetting); extern xQueueHandle PIOS_MPU6000_GetQueue(); -extern int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data * buffer); +extern int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *buffer); extern int32_t PIOS_MPU6000_ReadID(); extern int32_t PIOS_MPU6000_Test(); extern float PIOS_MPU6000_GetScale(); @@ -173,7 +173,7 @@ extern bool PIOS_MPU6000_IRQHandler(void); #endif /* PIOS_MPU6000_H */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_mpu6000_config.h b/flight/pios/inc/pios_mpu6000_config.h index 59e355c9a..9594d9394 100644 --- a/flight/pios/inc/pios_mpu6000_config.h +++ b/flight/pios/inc/pios_mpu6000_config.h @@ -13,45 +13,48 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_MPU6000_CONFIG_H -#define PIOS_MPU6000_CONFIG_H +#define PIOS_MPU6000_CONFIG_H #include "mpu6000settings.h" #include "pios_mpu6000.h" -#define PIOS_MPU6000_CONFIG_MAP_GYROSCALE(x) (x == MPU6000SETTINGS_GYROSCALE_SCALE_250 ? PIOS_MPU6000_SCALE_250_DEG : \ - x == MPU6000SETTINGS_GYROSCALE_SCALE_500 ? PIOS_MPU6000_SCALE_500_DEG : \ - x == MPU6000SETTINGS_GYROSCALE_SCALE_1000 ? PIOS_MPU6000_SCALE_1000_DEG : \ - PIOS_MPU6000_SCALE_2000_DEG) +#define PIOS_MPU6000_CONFIG_MAP_GYROSCALE(x) \ + (x == MPU6000SETTINGS_GYROSCALE_SCALE_250 ? PIOS_MPU6000_SCALE_250_DEG : \ + x == MPU6000SETTINGS_GYROSCALE_SCALE_500 ? PIOS_MPU6000_SCALE_500_DEG : \ + x == MPU6000SETTINGS_GYROSCALE_SCALE_1000 ? PIOS_MPU6000_SCALE_1000_DEG : \ + PIOS_MPU6000_SCALE_2000_DEG) -#define PIOS_MPU6000_CONFIG_MAP_ACCELSCALE(x) (x == MPU6000SETTINGS_ACCELSCALE_SCALE_2G ? PIOS_MPU6000_ACCEL_2G : \ - x == MPU6000SETTINGS_ACCELSCALE_SCALE_4G ? PIOS_MPU6000_ACCEL_4G : \ - x == MPU6000SETTINGS_ACCELSCALE_SCALE_16G ? PIOS_MPU6000_ACCEL_16G : \ - PIOS_MPU6000_ACCEL_8G) +#define PIOS_MPU6000_CONFIG_MAP_ACCELSCALE(x) \ + (x == MPU6000SETTINGS_ACCELSCALE_SCALE_2G ? PIOS_MPU6000_ACCEL_2G : \ + x == MPU6000SETTINGS_ACCELSCALE_SCALE_4G ? PIOS_MPU6000_ACCEL_4G : \ + x == MPU6000SETTINGS_ACCELSCALE_SCALE_16G ? PIOS_MPU6000_ACCEL_16G : \ + PIOS_MPU6000_ACCEL_8G) -#define PIOS_MPU6000_CONFIG_MAP_FILTERSETTING(x) (x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_188_HZ ? PIOS_MPU6000_LOWPASS_188_HZ : \ - x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_98_HZ ? PIOS_MPU6000_LOWPASS_98_HZ : \ - x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_42_HZ ? PIOS_MPU6000_LOWPASS_42_HZ : \ - x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_20_HZ ? PIOS_MPU6000_LOWPASS_20_HZ : \ - x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_10_HZ ? PIOS_MPU6000_LOWPASS_10_HZ : \ - x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_5_HZ ? PIOS_MPU6000_LOWPASS_5_HZ : \ - PIOS_MPU6000_LOWPASS_256_HZ) +#define PIOS_MPU6000_CONFIG_MAP_FILTERSETTING(x) \ + (x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_188_HZ ? PIOS_MPU6000_LOWPASS_188_HZ : \ + x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_98_HZ ? PIOS_MPU6000_LOWPASS_98_HZ : \ + x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_42_HZ ? PIOS_MPU6000_LOWPASS_42_HZ : \ + x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_20_HZ ? PIOS_MPU6000_LOWPASS_20_HZ : \ + x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_10_HZ ? PIOS_MPU6000_LOWPASS_10_HZ : \ + x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_5_HZ ? PIOS_MPU6000_LOWPASS_5_HZ : \ + PIOS_MPU6000_LOWPASS_256_HZ) /** * @brief Updates MPU6000 config based on Mpu6000Settings UAVO * @returns 0 if succeed or -1 otherwise @@ -62,11 +65,10 @@ int32_t PIOS_MPU6000_CONFIG_Configure() Mpu6000SettingsData mpu6000settings; Mpu6000SettingsGet(&mpu6000settings); return PIOS_MPU6000_ConfigureRanges( - PIOS_MPU6000_CONFIG_MAP_GYROSCALE (mpu6000settings.GyroScale), - PIOS_MPU6000_CONFIG_MAP_ACCELSCALE(mpu6000settings.AccelScale), - PIOS_MPU6000_CONFIG_MAP_FILTERSETTING(mpu6000settings.FilterSetting) - ); + PIOS_MPU6000_CONFIG_MAP_GYROSCALE(mpu6000settings.GyroScale), + PIOS_MPU6000_CONFIG_MAP_ACCELSCALE(mpu6000settings.AccelScale), + PIOS_MPU6000_CONFIG_MAP_FILTERSETTING(mpu6000settings.FilterSetting) + ); } -#endif /* PIOS_MPU6000_CONFIG_H */ - +#endif /* PIOS_MPU6000_CONFIG_H */ diff --git a/flight/pios/inc/pios_mpxv.h b/flight/pios/inc/pios_mpxv.h index 4e7dffaba..6b60eacdf 100644 --- a/flight/pios/inc/pios_mpxv.h +++ b/flight/pios/inc/pios_mpxv.h @@ -1,66 +1,66 @@ /** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_MPXV MPXV* Functions - * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV5004, 7002 or similar - * @{ - * - * @file pios_mpxv.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief ETASV3 Airspeed Sensor Driver - * @see The GNU Public License (GPL) Version 3 - * - ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_MPXV MPXV* Functions + * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV5004, 7002 or similar + * @{ + * + * @file pios_mpxv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief ETASV3 Airspeed Sensor Driver + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_MPXV_H #define PIOS_MPXV_H -typedef enum{ PIOS_MPXV_UNKNOWN,PIOS_MPXV_5004,PIOS_MPXV_7002 } PIOS_MPXV_sensortype; -typedef struct{ - PIOS_MPXV_sensortype type; - uint8_t airspeedADCPin; - uint16_t calibrationCount; - uint32_t calibrationSum; - uint16_t zeroPoint; - float maxSpeed; +typedef enum { PIOS_MPXV_UNKNOWN, PIOS_MPXV_5004, PIOS_MPXV_7002 } PIOS_MPXV_sensortype; +typedef struct { + PIOS_MPXV_sensortype type; + uint8_t airspeedADCPin; + uint16_t calibrationCount; + uint32_t calibrationSum; + uint16_t zeroPoint; + float maxSpeed; } PIOS_MPXV_descriptor; #define PIOS_MPXV_5004_DESC(pin) \ - (PIOS_MPXV_descriptor){ \ - .type = PIOS_MPXV_5004, \ - .airspeedADCPin = pin, \ - .maxSpeed = 80.0f, \ - .calibrationCount = 0, \ - .calibrationSum = 0, \ - } + (PIOS_MPXV_descriptor) { \ + .type = PIOS_MPXV_5004, \ + .airspeedADCPin = pin, \ + .maxSpeed = 80.0f, \ + .calibrationCount = 0, \ + .calibrationSum = 0, \ + } #define PIOS_MPXV_7002_DESC(pin) \ - (PIOS_MPXV_descriptor){ \ - .type = PIOS_MPXV_7002, \ - .airspeedADCPin = pin, \ - .maxSpeed = 56.0f, \ - .calibrationCount = 0, \ - .calibrationSum = 0, \ - } + (PIOS_MPXV_descriptor) { \ + .type = PIOS_MPXV_7002, \ + .airspeedADCPin = pin, \ + .maxSpeed = 56.0f, \ + .calibrationCount = 0, \ + .calibrationSum = 0, \ + } uint16_t PIOS_MPXV_Measure(PIOS_MPXV_descriptor *desc); uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc, uint16_t measurement); -float PIOS_MPXV_CalcAirspeed (PIOS_MPXV_descriptor *desc,uint16_t measurement); +float PIOS_MPXV_CalcAirspeed(PIOS_MPXV_descriptor *desc, uint16_t measurement); #endif /* PIOS_MPXV_H */ diff --git a/flight/pios/inc/pios_ms5611.h b/flight/pios/inc/pios_ms5611.h index ac0a79305..307b7296c 100644 --- a/flight/pios/inc/pios_ms5611.h +++ b/flight/pios/inc/pios_ms5611.h @@ -6,25 +6,25 @@ * @brief Hardware functions to deal with the altitude pressure sensor * @{ * - * @file pios_ms5611.h + * @file pios_ms5611.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief MS5611 functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,40 +32,40 @@ #define PIOS_MS5611_H /* BMP085 Addresses */ -#define MS5611_I2C_ADDR 0x77 -#define MS5611_RESET 0x1E -#define MS5611_CALIB_ADDR 0xA2 /* First sample is factory stuff */ -#define MS5611_CALIB_LEN 16 -#define MS5611_ADC_READ 0x00 -#define MS5611_PRES_ADDR 0x40 -#define MS5611_TEMP_ADDR 0x50 -#define MS5611_ADC_MSB 0xF6 -#define MS5611_P0 101.3250f +#define MS5611_I2C_ADDR 0x77 +#define MS5611_RESET 0x1E +#define MS5611_CALIB_ADDR 0xA2 /* First sample is factory stuff */ +#define MS5611_CALIB_LEN 16 +#define MS5611_ADC_READ 0x00 +#define MS5611_PRES_ADDR 0x40 +#define MS5611_TEMP_ADDR 0x50 +#define MS5611_ADC_MSB 0xF6 +#define MS5611_P0 101.3250f /* Local Types */ typedef struct { - uint16_t C[6]; + uint16_t C[6]; } MS5611CalibDataTypeDef; typedef enum { - PressureConv, - TemperatureConv + PressureConv, + TemperatureConv } ConversionTypeTypeDef; struct pios_ms5611_cfg { - uint32_t oversampling; + uint32_t oversampling; }; enum pios_ms5611_osr { - MS5611_OSR_256 = 0, - MS5611_OSR_512 = 2, - MS5611_OSR_1024 = 4, - MS5611_OSR_2048 = 6, - MS5611_OSR_4096 = 8, + MS5611_OSR_256 = 0, + MS5611_OSR_512 = 2, + MS5611_OSR_1024 = 4, + MS5611_OSR_2048 = 6, + MS5611_OSR_4096 = 8, }; /* Public Functions */ -extern void PIOS_MS5611_Init(const struct pios_ms5611_cfg * cfg, int32_t i2c_device); +extern void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device); extern int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type); extern int32_t PIOS_MS5611_ReadADC(void); extern float PIOS_MS5611_GetTemperature(void); @@ -75,7 +75,7 @@ extern int32_t PIOS_MS5611_GetDelay(); #endif /* PIOS_MS5611_H */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_overo_priv.h b/flight/pios/inc/pios_overo_priv.h index 990a922ac..cc4b9f60c 100644 --- a/flight/pios/inc/pios_overo_priv.h +++ b/flight/pios/inc/pios_overo_priv.h @@ -36,19 +36,19 @@ extern const struct pios_com_driver pios_overo_com_driver; struct pios_overo_cfg { - SPI_TypeDef *regs; - uint32_t remap; /* GPIO_Remap_* or GPIO_AF_* */ - SPI_InitTypeDef init; - bool use_crc; - struct stm32_dma dma; - struct stm32_gpio sclk; - struct stm32_gpio miso; - struct stm32_gpio mosi; - uint32_t slave_count; - struct stm32_gpio ssel[]; + SPI_TypeDef *regs; + uint32_t remap; /* GPIO_Remap_* or GPIO_AF_* */ + SPI_InitTypeDef init; + bool use_crc; + struct stm32_dma dma; + struct stm32_gpio sclk; + struct stm32_gpio miso; + struct stm32_gpio mosi; + uint32_t slave_count; + struct stm32_gpio ssel[]; }; -extern int32_t PIOS_OVERO_Init(uint32_t * overo_id, const struct pios_overo_cfg * cfg); +extern int32_t PIOS_OVERO_Init(uint32_t *overo_id, const struct pios_overo_cfg *cfg); #endif /* PIOS_OVERO_H */ diff --git a/flight/pios/inc/pios_posix.h b/flight/pios/inc/pios_posix.h index 7b6fe7fb1..74483b404 100644 --- a/flight/pios/inc/pios_posix.h +++ b/flight/pios/inc/pios_posix.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file posix.h + * @file posix.h * @author Corvus Corax Copyright (C) 2010. * @brief Definitions to run PiOS on posix * @see The GNU Public License (GPL) Version 2 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -28,16 +28,16 @@ #include -typedef enum {FALSE = 0, TRUE = !FALSE} bool; +typedef enum { FALSE = 0, TRUE = !FALSE } bool; #ifndef false - #define false FALSE - #define true TRUE + #define false FALSE + #define true TRUE #endif -//#define FILEINFO FILE* +// #define FILEINFO FILE* -//#define PIOS_SERVO_NUM_OUTPUTS 8 -//#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS +// #define PIOS_SERVO_NUM_OUTPUTS 8 +// #define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS #endif /* PIOS_POSIX_H */ diff --git a/flight/pios/inc/pios_ppm_out_priv.h b/flight/pios/inc/pios_ppm_out_priv.h index 7ecc98ffc..013599765 100644 --- a/flight/pios/inc/pios_ppm_out_priv.h +++ b/flight/pios/inc/pios_ppm_out_priv.h @@ -34,11 +34,11 @@ #include struct pios_ppm_out_cfg { - TIM_OCInitTypeDef tim_oc_init; - const struct pios_tim_channel * channel; + TIM_OCInitTypeDef tim_oc_init; + const struct pios_tim_channel *channel; }; -extern int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg * cfg); +extern int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg *cfg); extern void PIOS_PPM_OUT_Set(uint32_t ppm_out_id, uint8_t servo, uint16_t position); #endif /* PIOS_PPM_PRIV_H */ diff --git a/flight/pios/inc/pios_ppm_priv.h b/flight/pios/inc/pios_ppm_priv.h index 74d3aecfa..66ccd6eb7 100644 --- a/flight/pios/inc/pios_ppm_priv.h +++ b/flight/pios/inc/pios_ppm_priv.h @@ -35,14 +35,14 @@ #include struct pios_ppm_cfg { - TIM_ICInitTypeDef tim_ic_init; - const struct pios_tim_channel * channels; - uint8_t num_channels; + TIM_ICInitTypeDef tim_ic_init; + const struct pios_tim_channel *channels; + uint8_t num_channels; }; extern const struct pios_rcvr_driver pios_ppm_rcvr_driver; -extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg); +extern int32_t PIOS_PPM_Init(uint32_t *ppm_id, const struct pios_ppm_cfg *cfg); #endif /* PIOS_PPM_PRIV_H */ diff --git a/flight/pios/inc/pios_pwm_priv.h b/flight/pios/inc/pios_pwm_priv.h index fe11b3d60..fd6a253bd 100644 --- a/flight/pios/inc/pios_pwm_priv.h +++ b/flight/pios/inc/pios_pwm_priv.h @@ -37,14 +37,14 @@ #include struct pios_pwm_cfg { - TIM_ICInitTypeDef tim_ic_init; - const struct pios_tim_channel * channels; - uint8_t num_channels; + TIM_ICInitTypeDef tim_ic_init; + const struct pios_tim_channel *channels; + uint8_t num_channels; }; extern const struct pios_rcvr_driver pios_pwm_rcvr_driver; -extern int32_t PIOS_PWM_Init(uint32_t * pwm_id, const struct pios_pwm_cfg * cfg); +extern int32_t PIOS_PWM_Init(uint32_t *pwm_id, const struct pios_pwm_cfg *cfg); #endif /* PIOS_PWM_PRIV_H */ diff --git a/flight/pios/inc/pios_rcvr.h b/flight/pios/inc/pios_rcvr.h index 6b74c9b52..450e04628 100644 --- a/flight/pios/inc/pios_rcvr.h +++ b/flight/pios/inc/pios_rcvr.h @@ -6,25 +6,25 @@ * @brief Hardware communication layer * @{ * - * @file pios_rcvr.h + * @file pios_rcvr.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RCVR layer functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,8 +32,8 @@ #define PIOS_RCVR_H struct pios_rcvr_driver { - void (*init)(uint32_t id); - int32_t (*read)(uint32_t id, uint8_t channel); + void (*init)(uint32_t id); + int32_t (*read)(uint32_t id, uint8_t channel); }; /* Public Functions */ @@ -41,17 +41,17 @@ extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel); /*! Define error codes for PIOS_RCVR_Get */ enum PIOS_RCVR_errors { - /*! Indicates that a failsafe condition or missing receiver detected for that channel */ - PIOS_RCVR_TIMEOUT = -1, - /*! Channel is invalid for this driver (usually out of range supported) */ - PIOS_RCVR_INVALID = -2, - /*! Indicates that the driver for this channel has not been initialized */ - PIOS_RCVR_NODRIVER = -3 + /*! Indicates that a failsafe condition or missing receiver detected for that channel */ + PIOS_RCVR_TIMEOUT = -1, + /*! Channel is invalid for this driver (usually out of range supported) */ + PIOS_RCVR_INVALID = -2, + /*! Indicates that the driver for this channel has not been initialized */ + PIOS_RCVR_NODRIVER = -3 }; #endif /* PIOS_RCVR_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_rcvr_priv.h b/flight/pios/inc/pios_rcvr_priv.h index 46ced2e24..9f8193399 100644 --- a/flight/pios/inc/pios_rcvr_priv.h +++ b/flight/pios/inc/pios_rcvr_priv.h @@ -33,13 +33,13 @@ #include -extern int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id); +extern int32_t PIOS_RCVR_Init(uint32_t *rcvr_id, const struct pios_rcvr_driver *driver, const uint32_t lower_id); extern void PIOS_RCVR_IRQ_Handler(uint32_t rcvr_id); #endif /* PIOS_RCVR_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_rfm22b.h b/flight/pios/inc/pios_rfm22b.h index b1ce7a24b..0763a6ae1 100644 --- a/flight/pios/inc/pios_rfm22b.h +++ b/flight/pios/inc/pios_rfm22b.h @@ -1,30 +1,30 @@ /** -****************************************************************************** -* @addtogroup PIOS PIOS Core hardware abstraction layer -* @{ -* @addtogroup PIOS_RFM22B Radio Functions -* @brief PIOS interface for RFM22B Radio -* @{ -* -* @file pios_rfm22b.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief RFM22B functions header. -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RFM22B Radio Functions + * @brief PIOS interface for RFM22B Radio + * @{ + * + * @file pios_rfm22b.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief RFM22B functions header. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,41 +35,41 @@ #include /* Constant definitions */ -enum gpio_direction {GPIO0_TX_GPIO1_RX, GPIO0_RX_GPIO1_TX}; +enum gpio_direction { GPIO0_TX_GPIO1_RX, GPIO0_RX_GPIO1_TX }; /* Global Types */ struct pios_rfm22b_cfg { - const struct pios_spi_cfg * spi_cfg; /* Pointer to SPI interface configuration */ - const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ + const struct pios_spi_cfg *spi_cfg; /* Pointer to SPI interface configuration */ + const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ uint8_t RFXtalCap; uint8_t slave_num; enum gpio_direction gpio_direction; }; enum rfm22b_tx_power { - RFM22_tx_pwr_txpow_0 = 0x00, // +1dBm .. 1.25mW - RFM22_tx_pwr_txpow_1 = 0x01, // +2dBm .. 1.6mW - RFM22_tx_pwr_txpow_2 = 0x02, // +5dBm .. 3.16mW - RFM22_tx_pwr_txpow_3 = 0x03, // +8dBm .. 6.3mW - RFM22_tx_pwr_txpow_4 = 0x04, // +11dBm .. 12.6mW - RFM22_tx_pwr_txpow_5 = 0x05, // +14dBm .. 25mW - RFM22_tx_pwr_txpow_6 = 0x06, // +17dBm .. 50mW - RFM22_tx_pwr_txpow_7 = 0x07 // +20dBm .. 100mW + RFM22_tx_pwr_txpow_0 = 0x00, // +1dBm .. 1.25mW + RFM22_tx_pwr_txpow_1 = 0x01, // +2dBm .. 1.6mW + RFM22_tx_pwr_txpow_2 = 0x02, // +5dBm .. 3.16mW + RFM22_tx_pwr_txpow_3 = 0x03, // +8dBm .. 6.3mW + RFM22_tx_pwr_txpow_4 = 0x04, // +11dBm .. 12.6mW + RFM22_tx_pwr_txpow_5 = 0x05, // +14dBm .. 25mW + RFM22_tx_pwr_txpow_6 = 0x06, // +17dBm .. 50mW + RFM22_tx_pwr_txpow_7 = 0x07 // +20dBm .. 100mW }; enum rfm22b_datarate { - RFM22_datarate_500 = 0, - RFM22_datarate_1000 = 1, - RFM22_datarate_2000 = 2, - RFM22_datarate_4000 = 3, - RFM22_datarate_8000 = 4, - RFM22_datarate_9600 = 5, - RFM22_datarate_16000 = 6, - RFM22_datarate_19200 = 7, - RFM22_datarate_24000 = 8, - RFM22_datarate_32000 = 9, - RFM22_datarate_57600 = 10, - RFM22_datarate_64000 = 11, + RFM22_datarate_500 = 0, + RFM22_datarate_1000 = 1, + RFM22_datarate_2000 = 2, + RFM22_datarate_4000 = 3, + RFM22_datarate_8000 = 4, + RFM22_datarate_9600 = 5, + RFM22_datarate_16000 = 6, + RFM22_datarate_19200 = 7, + RFM22_datarate_24000 = 8, + RFM22_datarate_32000 = 9, + RFM22_datarate_57600 = 10, + RFM22_datarate_64000 = 11, RFM22_datarate_128000 = 12, RFM22_datarate_192000 = 13, RFM22_datarate_256000 = 14, @@ -88,26 +88,26 @@ struct rfm22b_stats { uint16_t rx_byte_count; uint16_t tx_seq; uint16_t rx_seq; - uint8_t rx_good; - uint8_t rx_corrected; - uint8_t rx_error; - uint8_t rx_missed; - uint8_t rx_failure; - uint8_t tx_dropped; - uint8_t tx_resent; - uint8_t tx_failure; - uint8_t resets; - uint8_t timeouts; - uint8_t link_quality; - int8_t rssi; - int8_t afc_correction; - uint8_t link_state; + uint8_t rx_good; + uint8_t rx_corrected; + uint8_t rx_error; + uint8_t rx_missed; + uint8_t rx_failure; + uint8_t tx_dropped; + uint8_t tx_resent; + uint8_t tx_failure; + uint8_t resets; + uint8_t timeouts; + uint8_t link_quality; + int8_t rssi; + int8_t afc_correction; + uint8_t link_state; }; /* Callback function prototypes */ typedef void (*PIOS_RFM22B_ComConfigCallback)(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, - uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); /* Public Functions */ extern int32_t PIOS_RFM22B_Init(uint32_t *rfb22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg); @@ -118,7 +118,7 @@ extern void PIOS_RFM22B_SetInitialFrequency(uint32_t rfm22b_id, uint32_t init_fr extern void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id); extern void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigCallback cb); extern void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindingPairIDs[], const uint8_t mainPortSettings[], - const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]); + const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); extern bool PIOS_RFM22B_IsCoordinator(uint32_t rfb22b_id); extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats); diff --git a/flight/pios/inc/pios_rfm22b_com.h b/flight/pios/inc/pios_rfm22b_com.h index f48c0af90..f461809a1 100644 --- a/flight/pios/inc/pios_rfm22b_com.h +++ b/flight/pios/inc/pios_rfm22b_com.h @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,6 +36,6 @@ extern const struct pios_com_driver pios_rfm22b_com_driver; #endif /* PIOS_RFM22B_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_rfm22b_priv.h b/flight/pios/inc/pios_rfm22b_priv.h index 7cecb0a36..467ace794 100644 --- a/flight/pios/inc/pios_rfm22b_priv.h +++ b/flight/pios/inc/pios_rfm22b_priv.h @@ -1,17 +1,17 @@ /** -****************************************************************************** -* @addtogroup PIOS PIOS Core hardware abstraction layer -* @{ -* @addtogroup PIOS_RFM22B Radio Functions -* @brief PIOS interface for RFM22B Radio -* @{ -* -* @file pios_rfm22b_priv.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief RFM22B private definitions. -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RFM22B Radio Functions + * @brief PIOS interface for RFM22B Radio + * @{ + * + * @file pios_rfm22b_priv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief RFM22B private definitions. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -40,493 +40,493 @@ // ************************************ -#define RFM22_DEVICE_VERSION_V2 0x02 -#define RFM22_DEVICE_VERSION_A0 0x04 -#define RFM22_DEVICE_VERSION_B1 0x06 +#define RFM22_DEVICE_VERSION_V2 0x02 +#define RFM22_DEVICE_VERSION_A0 0x04 +#define RFM22_DEVICE_VERSION_B1 0x06 // ************************************ -#define RFM22_MIN_CARRIER_FREQUENCY_HZ 240000000ul -#define RFM22_MAX_CARRIER_FREQUENCY_HZ 930000000ul +#define RFM22_MIN_CARRIER_FREQUENCY_HZ 240000000ul +#define RFM22_MAX_CARRIER_FREQUENCY_HZ 930000000ul // ************************************ -#define BIT0 (1u << 0) -#define BIT1 (1u << 1) -#define BIT2 (1u << 2) -#define BIT3 (1u << 3) -#define BIT4 (1u << 4) -#define BIT5 (1u << 5) -#define BIT6 (1u << 6) -#define BIT7 (1u << 7) +#define BIT0 (1u << 0) +#define BIT1 (1u << 1) +#define BIT2 (1u << 2) +#define BIT3 (1u << 3) +#define BIT4 (1u << 4) +#define BIT5 (1u << 5) +#define BIT6 (1u << 6) +#define BIT7 (1u << 7) // ************************************ -#define RFM22_DEVICE_TYPE 0x00 // R -#define RFM22_DT_MASK 0x1F +#define RFM22_DEVICE_TYPE 0x00 // R +#define RFM22_DT_MASK 0x1F -#define RFM22_DEVICE_VERSION 0x01 // R -#define RFM22_DV_MASK 0x1F +#define RFM22_DEVICE_VERSION 0x01 // R +#define RFM22_DV_MASK 0x1F -#define RFM22_device_status 0x02 // R -#define RFM22_ds_cps_mask 0x03 // Chip Power State mask -#define RFM22_ds_cps_idle 0x00 // IDLE Chip Power State -#define RFM22_ds_cps_rx 0x01 // RX Chip Power State -#define RFM22_ds_cps_tx 0x02 // TX Chip Power State -//#define RFM22_ds_lockdet 0x04 // -//#define RFM22_ds_freqerr 0x08 // -#define RFM22_ds_headerr 0x10 // Header Error Status. Indicates if the received packet has a header check error -#define RFM22_ds_rxffem 0x20 // RX FIFO Empty Status -#define RFM22_ds_ffunfl 0x40 // RX/TX FIFO Underflow Status -#define RFM22_ds_ffovfl 0x80 // RX/TX FIFO Overflow Status +#define RFM22_device_status 0x02 // R +#define RFM22_ds_cps_mask 0x03 // Chip Power State mask +#define RFM22_ds_cps_idle 0x00 // IDLE Chip Power State +#define RFM22_ds_cps_rx 0x01 // RX Chip Power State +#define RFM22_ds_cps_tx 0x02 // TX Chip Power State +// #define RFM22_ds_lockdet 0x04 // +// #define RFM22_ds_freqerr 0x08 // +#define RFM22_ds_headerr 0x10 // Header Error Status. Indicates if the received packet has a header check error +#define RFM22_ds_rxffem 0x20 // RX FIFO Empty Status +#define RFM22_ds_ffunfl 0x40 // RX/TX FIFO Underflow Status +#define RFM22_ds_ffovfl 0x80 // RX/TX FIFO Overflow Status -#define RFM22_interrupt_status1 0x03 // R -#define RFM22_is1_icrerror BIT0 // CRC Error. When set to 1 the cyclic redundancy check is failed. -#define RFM22_is1_ipkvalid BIT1 // Valid Packet Received.When set to 1 a valid packet has been received. -#define RFM22_is1_ipksent BIT2 // Packet Sent Interrupt. When set to1 a valid packet has been transmitted. -#define RFM22_is1_iext BIT3 // External Interrupt. When set to 1 an interrupt occurred on one of the GPIO�s if it is programmed so. The status can be checked in register 0Eh. See GPIOx Configuration section for the details. -#define RFM22_is1_irxffafull BIT4 // RX FIFO Almost Full.When set to 1 the RX FIFO has met its almost full threshold and needs to be read by the microcontroller. -#define RFM22_is1_ixtffaem BIT5 // TX FIFO Almost Empty. When set to 1 the TX FIFO is almost empty and needs to be filled. -#define RFM22_is1_itxffafull BIT6 // TX FIFO Almost Full. When set to 1 the TX FIFO has met its almost full threshold and needs to be transmitted. -#define RFM22_is1_ifferr BIT7 // FIFO Underflow/Overflow Error. When set to 1 the TX or RX FIFO has overflowed or underflowed. +#define RFM22_interrupt_status1 0x03 // R +#define RFM22_is1_icrerror BIT0 // CRC Error. When set to 1 the cyclic redundancy check is failed. +#define RFM22_is1_ipkvalid BIT1 // Valid Packet Received.When set to 1 a valid packet has been received. +#define RFM22_is1_ipksent BIT2 // Packet Sent Interrupt. When set to1 a valid packet has been transmitted. +#define RFM22_is1_iext BIT3 // External Interrupt. When set to 1 an interrupt occurred on one of the GPIO�s if it is programmed so. The status can be checked in register 0Eh. See GPIOx Configuration section for the details. +#define RFM22_is1_irxffafull BIT4 // RX FIFO Almost Full.When set to 1 the RX FIFO has met its almost full threshold and needs to be read by the microcontroller. +#define RFM22_is1_ixtffaem BIT5 // TX FIFO Almost Empty. When set to 1 the TX FIFO is almost empty and needs to be filled. +#define RFM22_is1_itxffafull BIT6 // TX FIFO Almost Full. When set to 1 the TX FIFO has met its almost full threshold and needs to be transmitted. +#define RFM22_is1_ifferr BIT7 // FIFO Underflow/Overflow Error. When set to 1 the TX or RX FIFO has overflowed or underflowed. -#define RFM22_interrupt_status2 0x04 // R -#define RFM22_is2_ipor BIT0 // Power-on-Reset (POR). When the chip detects a Power on Reset above the desired setting this bit will be set to 1. -#define RFM22_is2_ichiprdy BIT1 // Chip Ready (XTAL). When a chip ready event has been detected this bit will be set to 1. -#define RFM22_is2_ilbd BIT2 // Low Battery Detect. When a low battery event is been detected this bit will be set to 1. This interrupt event is saved even if it is not enabled by the mask register bit and causes an interrupt after it is enabled. -#define RFM22_is2_iwut BIT3 // Wake-Up-Timer. On the expiration of programmed wake-up timer this bit will be set to 1. -#define RFM22_is2_irssi BIT4 // RSSI. When RSSI level exceeds the programmed threshold this bit will be set to 1. -#define RFM22_is2_ipreainval BIT5 // Invalid Preamble Detected. When the preamble is not found within a period of time set by the invalid preamble detection threshold in Register 54h, this bit will be set to 1. -#define RFM22_is2_ipreaval BIT6 // Valid Preamble Detected. When a preamble is detected this bit will be set to 1. -#define RFM22_is2_iswdet BIT7 // Sync Word Detected. When a sync word is detected this bit will be set to 1. +#define RFM22_interrupt_status2 0x04 // R +#define RFM22_is2_ipor BIT0 // Power-on-Reset (POR). When the chip detects a Power on Reset above the desired setting this bit will be set to 1. +#define RFM22_is2_ichiprdy BIT1 // Chip Ready (XTAL). When a chip ready event has been detected this bit will be set to 1. +#define RFM22_is2_ilbd BIT2 // Low Battery Detect. When a low battery event is been detected this bit will be set to 1. This interrupt event is saved even if it is not enabled by the mask register bit and causes an interrupt after it is enabled. +#define RFM22_is2_iwut BIT3 // Wake-Up-Timer. On the expiration of programmed wake-up timer this bit will be set to 1. +#define RFM22_is2_irssi BIT4 // RSSI. When RSSI level exceeds the programmed threshold this bit will be set to 1. +#define RFM22_is2_ipreainval BIT5 // Invalid Preamble Detected. When the preamble is not found within a period of time set by the invalid preamble detection threshold in Register 54h, this bit will be set to 1. +#define RFM22_is2_ipreaval BIT6 // Valid Preamble Detected. When a preamble is detected this bit will be set to 1. +#define RFM22_is2_iswdet BIT7 // Sync Word Detected. When a sync word is detected this bit will be set to 1. -#define RFM22_interrupt_enable1 0x05 // R/W -#define RFM22_ie1_encrcerror BIT0 // Enable CRC Error. When set to 1 the CRC Error interrupt will be enabled. -#define RFM22_ie1_enpkvalid BIT1 // Enable Valid Packet Received. When ipkvalid = 1 the Valid Packet Received Interrupt will be enabled. -#define RFM22_ie1_enpksent BIT2 // Enable Packet Sent. When ipksent =1 the Packet Sense Interrupt will be enabled. -#define RFM22_ie1_enext BIT3 // Enable External Interrupt. When set to 1 the External Interrupt will be enabled. -#define RFM22_ie1_enrxffafull BIT4 // Enable RX FIFO Almost Full. When set to 1 the RX FIFO Almost Full interrupt will be enabled. -#define RFM22_ie1_entxffaem BIT5 // Enable TX FIFO Almost Empty. When set to 1 the TX FIFO Almost Empty interrupt will be enabled. -#define RFM22_ie1_entxffafull BIT6 // Enable TX FIFO Almost Full. When set to 1 the TX FIFO Almost Full interrupt will be enabled. -#define RFM22_ie1_enfferr BIT7 // Enable FIFO Underflow/Overflow. When set to 1 the FIFO Underflow/Overflow interrupt will be enabled. +#define RFM22_interrupt_enable1 0x05 // R/W +#define RFM22_ie1_encrcerror BIT0 // Enable CRC Error. When set to 1 the CRC Error interrupt will be enabled. +#define RFM22_ie1_enpkvalid BIT1 // Enable Valid Packet Received. When ipkvalid = 1 the Valid Packet Received Interrupt will be enabled. +#define RFM22_ie1_enpksent BIT2 // Enable Packet Sent. When ipksent =1 the Packet Sense Interrupt will be enabled. +#define RFM22_ie1_enext BIT3 // Enable External Interrupt. When set to 1 the External Interrupt will be enabled. +#define RFM22_ie1_enrxffafull BIT4 // Enable RX FIFO Almost Full. When set to 1 the RX FIFO Almost Full interrupt will be enabled. +#define RFM22_ie1_entxffaem BIT5 // Enable TX FIFO Almost Empty. When set to 1 the TX FIFO Almost Empty interrupt will be enabled. +#define RFM22_ie1_entxffafull BIT6 // Enable TX FIFO Almost Full. When set to 1 the TX FIFO Almost Full interrupt will be enabled. +#define RFM22_ie1_enfferr BIT7 // Enable FIFO Underflow/Overflow. When set to 1 the FIFO Underflow/Overflow interrupt will be enabled. -#define RFM22_interrupt_enable2 0x06 // R/W -#define RFM22_ie2_enpor BIT0 // Enable POR. When set to 1 the POR interrupt will be enabled. -#define RFM22_ie2_enchiprdy BIT1 // Enable Chip Ready (XTAL). When set to 1 the Chip Ready interrupt will be enabled. -#define RFM22_ie2_enlbd BIT2 // Enable Low Battery Detect. When set to 1 the Low Battery Detect interrupt will be enabled. -#define RFM22_ie2_enwut BIT3 // Enable Wake-Up Timer. When set to 1 the Wake-Up Timer interrupt will be enabled. -#define RFM22_ie2_enrssi BIT4 // Enable RSSI. When set to 1 the RSSI Interrupt will be enabled. -#define RFM22_ie2_enpreainval BIT5 // Enable Invalid Preamble Detected. When mpreadet =1 the Invalid Preamble Detected Interrupt will be enabled. -#define RFM22_ie2_enpreaval BIT6 // Enable Valid Preamble Detected. When mpreadet =1 the Valid Preamble Detected Interrupt will be enabled. -#define RFM22_ie2_enswdet BIT7 // Enable Sync Word Detected. When mpreadet =1 the Preamble Detected Interrupt will be enabled. +#define RFM22_interrupt_enable2 0x06 // R/W +#define RFM22_ie2_enpor BIT0 // Enable POR. When set to 1 the POR interrupt will be enabled. +#define RFM22_ie2_enchiprdy BIT1 // Enable Chip Ready (XTAL). When set to 1 the Chip Ready interrupt will be enabled. +#define RFM22_ie2_enlbd BIT2 // Enable Low Battery Detect. When set to 1 the Low Battery Detect interrupt will be enabled. +#define RFM22_ie2_enwut BIT3 // Enable Wake-Up Timer. When set to 1 the Wake-Up Timer interrupt will be enabled. +#define RFM22_ie2_enrssi BIT4 // Enable RSSI. When set to 1 the RSSI Interrupt will be enabled. +#define RFM22_ie2_enpreainval BIT5 // Enable Invalid Preamble Detected. When mpreadet =1 the Invalid Preamble Detected Interrupt will be enabled. +#define RFM22_ie2_enpreaval BIT6 // Enable Valid Preamble Detected. When mpreadet =1 the Valid Preamble Detected Interrupt will be enabled. +#define RFM22_ie2_enswdet BIT7 // Enable Sync Word Detected. When mpreadet =1 the Preamble Detected Interrupt will be enabled. -#define RFM22_op_and_func_ctrl1 0x07 // R/W -#define RFM22_opfc1_xton 0x01 // READY Mode (Xtal is ON). -#define RFM22_opfc1_pllon 0x02 // TUNE Mode (PLL is ON). When pllon = 1 the PLL will remain enabled in Idle State. This will for faster turn-around time at the cost of increased current consumption in Idle State. -#define RFM22_opfc1_rxon 0x04 // RX on in Manual Receiver Mode. Automatically cleared if Multiple Packets config. is disabled and a valid packet received. -#define RFM22_opfc1_txon 0x08 // TX on in Manual Transmit Mode. Automatically cleared in FIFO mode once the packet is sent. Transmission can be aborted during packet transmission, however, when no data has been sent yet, transmission can only be aborted after the device is programmed to �unmodulated carrier� ("Register 71h. Modulation Mode Control 2"). -#define RFM22_opfc1_x32ksel 0x10 // 32,768 kHz Crystal Oscillator Select. 0: RC oscillator 1: 32 kHz crystal -#define RFM22_opfc1_enwt 0x20 // Enable Wake-Up-Timer. Enabled when enwt = 1. If the Wake-up-Timer function is enabled it will operate in any mode and notify the microcontroller through the GPIO interrupt when the timer expires. -#define RFM22_opfc1_enlbd 0x40 // Enable Low Battery Detect. When this bit is set to 1 the Low Battery Detector circuit and threshold comparison will be enabled. -#define RFM22_opfc1_swres 0x80 // Software Register Reset Bit. This bit may be used to reset all registers simultaneously to a DEFAULT state, without the need for sequentially writing to each individual register. The RESET is accomplished by setting swres = 1. This bit will be automatically cleared. +#define RFM22_op_and_func_ctrl1 0x07 // R/W +#define RFM22_opfc1_xton 0x01 // READY Mode (Xtal is ON). +#define RFM22_opfc1_pllon 0x02 // TUNE Mode (PLL is ON). When pllon = 1 the PLL will remain enabled in Idle State. This will for faster turn-around time at the cost of increased current consumption in Idle State. +#define RFM22_opfc1_rxon 0x04 // RX on in Manual Receiver Mode. Automatically cleared if Multiple Packets config. is disabled and a valid packet received. +#define RFM22_opfc1_txon 0x08 // TX on in Manual Transmit Mode. Automatically cleared in FIFO mode once the packet is sent. Transmission can be aborted during packet transmission, however, when no data has been sent yet, transmission can only be aborted after the device is programmed to �unmodulated carrier� ("Register 71h. Modulation Mode Control 2"). +#define RFM22_opfc1_x32ksel 0x10 // 32,768 kHz Crystal Oscillator Select. 0: RC oscillator 1: 32 kHz crystal +#define RFM22_opfc1_enwt 0x20 // Enable Wake-Up-Timer. Enabled when enwt = 1. If the Wake-up-Timer function is enabled it will operate in any mode and notify the microcontroller through the GPIO interrupt when the timer expires. +#define RFM22_opfc1_enlbd 0x40 // Enable Low Battery Detect. When this bit is set to 1 the Low Battery Detector circuit and threshold comparison will be enabled. +#define RFM22_opfc1_swres 0x80 // Software Register Reset Bit. This bit may be used to reset all registers simultaneously to a DEFAULT state, without the need for sequentially writing to each individual register. The RESET is accomplished by setting swres = 1. This bit will be automatically cleared. -#define RFM22_op_and_func_ctrl2 0x08 // R/W -#define RFM22_opfc2_ffclrtx 0x01 // TX FIFO Reset/Clear. This has to be a two writes operation: Setting ffclrtx =1 followed by ffclrtx = 0 will clear the contents of the TX FIFO. -#define RFM22_opfc2_ffclrrx 0x02 // RX FIFO Reset/Clear. This has to be a two writes operation: Setting ffclrrx =1 followed by ffclrrx = 0 will clear the contents of the RX FIFO. -#define RFM22_opfc2_enldm 0x04 // Enable Low Duty Cycle Mode. If this bit is set to 1 then the chip turns on the RX regularly. The frequency should be set in the Wake-Up Timer Period register, while the minimum ON time should be set in the Low-Duty Cycle Mode Duration register. The FIFO mode should be enabled also. -#define RFM22_opfc2_autotx 0x08 // Automatic Transmission. When autotx = 1 the transceiver will enter automatically TX State when the FIFO is almost full. When the FIFO is empty it will automatically return to the Idle State. -#define RFM22_opfc2_rxmpk 0x10 // RX Multi Packet. When the chip is selected to use FIFO Mode (dtmod[1:0]) and RX Packet Handling (enpacrx) then it will fill up the FIFO with multiple valid packets if this bit is set, otherwise the transceiver will automatically leave the RX State after the first valid packet has been received. -#define RFM22_opfc2_antdiv_mask 0xE0 // Enable Antenna Diversity. The GPIO must be configured for Antenna Diversity for the algorithm to work properly. +#define RFM22_op_and_func_ctrl2 0x08 // R/W +#define RFM22_opfc2_ffclrtx 0x01 // TX FIFO Reset/Clear. This has to be a two writes operation: Setting ffclrtx =1 followed by ffclrtx = 0 will clear the contents of the TX FIFO. +#define RFM22_opfc2_ffclrrx 0x02 // RX FIFO Reset/Clear. This has to be a two writes operation: Setting ffclrrx =1 followed by ffclrrx = 0 will clear the contents of the RX FIFO. +#define RFM22_opfc2_enldm 0x04 // Enable Low Duty Cycle Mode. If this bit is set to 1 then the chip turns on the RX regularly. The frequency should be set in the Wake-Up Timer Period register, while the minimum ON time should be set in the Low-Duty Cycle Mode Duration register. The FIFO mode should be enabled also. +#define RFM22_opfc2_autotx 0x08 // Automatic Transmission. When autotx = 1 the transceiver will enter automatically TX State when the FIFO is almost full. When the FIFO is empty it will automatically return to the Idle State. +#define RFM22_opfc2_rxmpk 0x10 // RX Multi Packet. When the chip is selected to use FIFO Mode (dtmod[1:0]) and RX Packet Handling (enpacrx) then it will fill up the FIFO with multiple valid packets if this bit is set, otherwise the transceiver will automatically leave the RX State after the first valid packet has been received. +#define RFM22_opfc2_antdiv_mask 0xE0 // Enable Antenna Diversity. The GPIO must be configured for Antenna Diversity for the algorithm to work properly. -#define RFM22_xtal_osc_load_cap 0x09 // R/W -#define RFM22_xolc_xlc_mask 0x7F // Tuning Capacitance for the 30 MHz XTAL. -#define RFM22_xolc_xtalshft 0x80 // Additional capacitance to course shift the frequency if xlc[6:0] is not sufficient. Not binary with xlc[6:0]. +#define RFM22_xtal_osc_load_cap 0x09 // R/W +#define RFM22_xolc_xlc_mask 0x7F // Tuning Capacitance for the 30 MHz XTAL. +#define RFM22_xolc_xtalshft 0x80 // Additional capacitance to course shift the frequency if xlc[6:0] is not sufficient. Not binary with xlc[6:0]. -#define RFM22_cpu_output_clk 0x0A // R/W -#define RFM22_coc_30MHz 0x00 -#define RFM22_coc_15MHz 0x01 -#define RFM22_coc_10MHz 0x02 -#define RFM22_coc_4MHz 0x03 -#define RFM22_coc_3MHz 0x04 -#define RFM22_coc_2MHz 0x05 -#define RFM22_coc_1MHz 0x06 -#define RFM22_coc_32768Hz 0x07 -#define RFM22_coc_enlfc 0x08 -#define RFM22_coc_0cycle 0x00 -#define RFM22_coc_128cycles 0x10 -#define RFM22_coc_256cycles 0x20 -#define RFM22_coc_512cycles 0x30 +#define RFM22_cpu_output_clk 0x0A // R/W +#define RFM22_coc_30MHz 0x00 +#define RFM22_coc_15MHz 0x01 +#define RFM22_coc_10MHz 0x02 +#define RFM22_coc_4MHz 0x03 +#define RFM22_coc_3MHz 0x04 +#define RFM22_coc_2MHz 0x05 +#define RFM22_coc_1MHz 0x06 +#define RFM22_coc_32768Hz 0x07 +#define RFM22_coc_enlfc 0x08 +#define RFM22_coc_0cycle 0x00 +#define RFM22_coc_128cycles 0x10 +#define RFM22_coc_256cycles 0x20 +#define RFM22_coc_512cycles 0x30 -#define RFM22_gpio0_config 0x0B // R/W -#define RFM22_gpio0_config_por 0x00 // Power-On-Reset (output) -#define RFM22_gpio0_config_wut 0x01 // Wake-Up Timer: 1 when WUT has expired (output) -#define RFM22_gpio0_config_lbd 0x02 // Low Battery Detect: 1 when battery is below threshold setting (output) -#define RFM22_gpio0_config_ddi 0x03 // Direct Digital Input -#define RFM22_gpio0_config_eife 0x04 // External Interrupt, falling edge (input) -#define RFM22_gpio0_config_eire 0x05 // External Interrupt, rising edge (input) -#define RFM22_gpio0_config_eisc 0x06 // External Interrupt, state change (input) -#define RFM22_gpio0_config_ai 0x07 // ADC Analog Input -#define RFM22_gpio0_config_atni 0x08 // Reserved (Analog Test N Input) -#define RFM22_gpio0_config_atpi 0x09 // Reserved (Analog Test P Input) -#define RFM22_gpio0_config_ddo 0x0A // Direct Digital Output -#define RFM22_gpio0_config_dto 0x0B // Reserved (Digital Test Output) -#define RFM22_gpio0_config_atno 0x0C // Reserved (Analog Test N Output) -#define RFM22_gpio0_config_atpo 0x0D // Reserved (Analog Test P Output) -#define RFM22_gpio0_config_rv 0xOE // Reference Voltage (output) -#define RFM22_gpio0_config_dclk 0x0F // TX/RX Data CLK output to be used in conjunction with TX/RX Data pin (output) -#define RFM22_gpio0_config_txd 0x10 // TX Data input for direct modulation (input) -#define RFM22_gpio0_config_err 0x11 // External Retransmission Request (input) -#define RFM22_gpio0_config_txstate 0x12 // TX State (output) -#define RFM22_gpio0_config_txfifoaf 0x13 // TX FIFO Almost Full (output) -#define RFM22_gpio0_config_rxd 0x14 // RX Data (output) -#define RFM22_gpio0_config_rxstate 0x15 // RX State (output) -#define RFM22_gpio0_config_rxfifoaf 0x16 // RX FIFO Almost Full (output) -#define RFM22_gpio0_config_antswt1 0x17 // Antenna 1 Switch used for antenna diversity (output) -#define RFM22_gpio0_config_antswt2 0x18 // Antenna 2 Switch used for antenna diversity (output) -#define RFM22_gpio0_config_vpd 0x19 // Valid Preamble Detected (output) -#define RFM22_gpio0_config_ipd 0x1A // Invalid Preamble Detected (output) -#define RFM22_gpio0_config_swd 0x1B // Sync Word Detected (output) -#define RFM22_gpio0_config_cca 0x1C // Clear Channel Assessment (output) -#define RFM22_gpio0_config_vdd 0x1D // VDD -#define RFM22_gpio0_config_pup 0x20 -#define RFM22_gpio0_config_drv0 0x00 // output drive level -#define RFM22_gpio0_config_drv1 0x40 // output drive level -#define RFM22_gpio0_config_drv2 0x80 // output drive level -#define RFM22_gpio0_config_drv3 0xC0 // output drive level +#define RFM22_gpio0_config 0x0B // R/W +#define RFM22_gpio0_config_por 0x00 // Power-On-Reset (output) +#define RFM22_gpio0_config_wut 0x01 // Wake-Up Timer: 1 when WUT has expired (output) +#define RFM22_gpio0_config_lbd 0x02 // Low Battery Detect: 1 when battery is below threshold setting (output) +#define RFM22_gpio0_config_ddi 0x03 // Direct Digital Input +#define RFM22_gpio0_config_eife 0x04 // External Interrupt, falling edge (input) +#define RFM22_gpio0_config_eire 0x05 // External Interrupt, rising edge (input) +#define RFM22_gpio0_config_eisc 0x06 // External Interrupt, state change (input) +#define RFM22_gpio0_config_ai 0x07 // ADC Analog Input +#define RFM22_gpio0_config_atni 0x08 // Reserved (Analog Test N Input) +#define RFM22_gpio0_config_atpi 0x09 // Reserved (Analog Test P Input) +#define RFM22_gpio0_config_ddo 0x0A // Direct Digital Output +#define RFM22_gpio0_config_dto 0x0B // Reserved (Digital Test Output) +#define RFM22_gpio0_config_atno 0x0C // Reserved (Analog Test N Output) +#define RFM22_gpio0_config_atpo 0x0D // Reserved (Analog Test P Output) +#define RFM22_gpio0_config_rv 0xOE // Reference Voltage (output) +#define RFM22_gpio0_config_dclk 0x0F // TX/RX Data CLK output to be used in conjunction with TX/RX Data pin (output) +#define RFM22_gpio0_config_txd 0x10 // TX Data input for direct modulation (input) +#define RFM22_gpio0_config_err 0x11 // External Retransmission Request (input) +#define RFM22_gpio0_config_txstate 0x12 // TX State (output) +#define RFM22_gpio0_config_txfifoaf 0x13 // TX FIFO Almost Full (output) +#define RFM22_gpio0_config_rxd 0x14 // RX Data (output) +#define RFM22_gpio0_config_rxstate 0x15 // RX State (output) +#define RFM22_gpio0_config_rxfifoaf 0x16 // RX FIFO Almost Full (output) +#define RFM22_gpio0_config_antswt1 0x17 // Antenna 1 Switch used for antenna diversity (output) +#define RFM22_gpio0_config_antswt2 0x18 // Antenna 2 Switch used for antenna diversity (output) +#define RFM22_gpio0_config_vpd 0x19 // Valid Preamble Detected (output) +#define RFM22_gpio0_config_ipd 0x1A // Invalid Preamble Detected (output) +#define RFM22_gpio0_config_swd 0x1B // Sync Word Detected (output) +#define RFM22_gpio0_config_cca 0x1C // Clear Channel Assessment (output) +#define RFM22_gpio0_config_vdd 0x1D // VDD +#define RFM22_gpio0_config_pup 0x20 +#define RFM22_gpio0_config_drv0 0x00 // output drive level +#define RFM22_gpio0_config_drv1 0x40 // output drive level +#define RFM22_gpio0_config_drv2 0x80 // output drive level +#define RFM22_gpio0_config_drv3 0xC0 // output drive level -#define RFM22_gpio1_config 0x0C // R/W -#define RFM22_gpio1_config_ipor 0x00 // Inverted Power-On-Reset (output) -#define RFM22_gpio1_config_wut 0x01 // Wake-Up Timer: 1 when WUT has expired (output) -#define RFM22_gpio1_config_lbd 0x02 // Low Battery Detect: 1 when battery is below threshold setting (output) -#define RFM22_gpio1_config_ddi 0x03 // Direct Digital Input -#define RFM22_gpio1_config_eife 0x04 // External Interrupt, falling edge (input) -#define RFM22_gpio1_config_eire 0x05 // External Interrupt, rising edge (input) -#define RFM22_gpio1_config_eisc 0x06 // External Interrupt, state change (input) -#define RFM22_gpio1_config_ai 0x07 // ADC Analog Input -#define RFM22_gpio1_config_atni 0x08 // Reserved (Analog Test N Input) -#define RFM22_gpio1_config_atpi 0x09 // Reserved (Analog Test P Input) -#define RFM22_gpio1_config_ddo 0x0A // Direct Digital Output -#define RFM22_gpio1_config_dto 0x0B // Reserved (Digital Test Output) -#define RFM22_gpio1_config_atno 0x0C // Reserved (Analog Test N Output) -#define RFM22_gpio1_config_atpo 0x0D // Reserved (Analog Test P Output) -#define RFM22_gpio1_config_rv 0xOE // Reference Voltage (output) -#define RFM22_gpio1_config_dclk 0x0F // TX/RX Data CLK output to be used in conjunction with TX/RX Data pin (output) -#define RFM22_gpio1_config_txd 0x10 // TX Data input for direct modulation (input) -#define RFM22_gpio1_config_err 0x11 // External Retransmission Request (input) -#define RFM22_gpio1_config_txstate 0x12 // TX State (output) -#define RFM22_gpio1_config_txfifoaf 0x13 // TX FIFO Almost Full (output) -#define RFM22_gpio1_config_rxd 0x14 // RX Data (output) -#define RFM22_gpio1_config_rxstate 0x15 // RX State (output) -#define RFM22_gpio1_config_rxfifoaf 0x16 // RX FIFO Almost Full (output) -#define RFM22_gpio1_config_antswt1 0x17 // Antenna 1 Switch used for antenna diversity (output) -#define RFM22_gpio1_config_antswt2 0x18 // Antenna 2 Switch used for antenna diversity (output) -#define RFM22_gpio1_config_vpd 0x19 // Valid Preamble Detected (output) -#define RFM22_gpio1_config_ipd 0x1A // Invalid Preamble Detected (output) -#define RFM22_gpio1_config_swd 0x1B // Sync Word Detected (output) -#define RFM22_gpio1_config_cca 0x1C // Clear Channel Assessment (output) -#define RFM22_gpio1_config_vdd 0x1D // VDD -#define RFM22_gpio1_config_pup 0x20 -#define RFM22_gpio1_config_drv0 0x00 // output drive level -#define RFM22_gpio1_config_drv1 0x40 // output drive level -#define RFM22_gpio1_config_drv2 0x80 // output drive level -#define RFM22_gpio1_config_drv3 0xC0 // output drive level +#define RFM22_gpio1_config 0x0C // R/W +#define RFM22_gpio1_config_ipor 0x00 // Inverted Power-On-Reset (output) +#define RFM22_gpio1_config_wut 0x01 // Wake-Up Timer: 1 when WUT has expired (output) +#define RFM22_gpio1_config_lbd 0x02 // Low Battery Detect: 1 when battery is below threshold setting (output) +#define RFM22_gpio1_config_ddi 0x03 // Direct Digital Input +#define RFM22_gpio1_config_eife 0x04 // External Interrupt, falling edge (input) +#define RFM22_gpio1_config_eire 0x05 // External Interrupt, rising edge (input) +#define RFM22_gpio1_config_eisc 0x06 // External Interrupt, state change (input) +#define RFM22_gpio1_config_ai 0x07 // ADC Analog Input +#define RFM22_gpio1_config_atni 0x08 // Reserved (Analog Test N Input) +#define RFM22_gpio1_config_atpi 0x09 // Reserved (Analog Test P Input) +#define RFM22_gpio1_config_ddo 0x0A // Direct Digital Output +#define RFM22_gpio1_config_dto 0x0B // Reserved (Digital Test Output) +#define RFM22_gpio1_config_atno 0x0C // Reserved (Analog Test N Output) +#define RFM22_gpio1_config_atpo 0x0D // Reserved (Analog Test P Output) +#define RFM22_gpio1_config_rv 0xOE // Reference Voltage (output) +#define RFM22_gpio1_config_dclk 0x0F // TX/RX Data CLK output to be used in conjunction with TX/RX Data pin (output) +#define RFM22_gpio1_config_txd 0x10 // TX Data input for direct modulation (input) +#define RFM22_gpio1_config_err 0x11 // External Retransmission Request (input) +#define RFM22_gpio1_config_txstate 0x12 // TX State (output) +#define RFM22_gpio1_config_txfifoaf 0x13 // TX FIFO Almost Full (output) +#define RFM22_gpio1_config_rxd 0x14 // RX Data (output) +#define RFM22_gpio1_config_rxstate 0x15 // RX State (output) +#define RFM22_gpio1_config_rxfifoaf 0x16 // RX FIFO Almost Full (output) +#define RFM22_gpio1_config_antswt1 0x17 // Antenna 1 Switch used for antenna diversity (output) +#define RFM22_gpio1_config_antswt2 0x18 // Antenna 2 Switch used for antenna diversity (output) +#define RFM22_gpio1_config_vpd 0x19 // Valid Preamble Detected (output) +#define RFM22_gpio1_config_ipd 0x1A // Invalid Preamble Detected (output) +#define RFM22_gpio1_config_swd 0x1B // Sync Word Detected (output) +#define RFM22_gpio1_config_cca 0x1C // Clear Channel Assessment (output) +#define RFM22_gpio1_config_vdd 0x1D // VDD +#define RFM22_gpio1_config_pup 0x20 +#define RFM22_gpio1_config_drv0 0x00 // output drive level +#define RFM22_gpio1_config_drv1 0x40 // output drive level +#define RFM22_gpio1_config_drv2 0x80 // output drive level +#define RFM22_gpio1_config_drv3 0xC0 // output drive level -#define RFM22_gpio2_config 0x0D // R/W -#define RFM22_gpio2_config_mc 0x00 // Microcontroller Clock (output) -#define RFM22_gpio2_config_wut 0x01 // Wake-Up Timer: 1 when WUT has expired (output) -#define RFM22_gpio2_config_lbd 0x02 // Low Battery Detect: 1 when battery is below threshold setting (output) -#define RFM22_gpio2_config_ddi 0x03 // Direct Digital Input -#define RFM22_gpio2_config_eife 0x04 // External Interrupt, falling edge (input) -#define RFM22_gpio2_config_eire 0x05 // External Interrupt, rising edge (input) -#define RFM22_gpio2_config_eisc 0x06 // External Interrupt, state change (input) -#define RFM22_gpio2_config_ai 0x07 // ADC Analog Input -#define RFM22_gpio2_config_atni 0x08 // Reserved (Analog Test N Input) -#define RFM22_gpio2_config_atpi 0x09 // Reserved (Analog Test P Input) -#define RFM22_gpio2_config_ddo 0x0A // Direct Digital Output -#define RFM22_gpio2_config_dto 0x0B // Reserved (Digital Test Output) -#define RFM22_gpio2_config_atno 0x0C // Reserved (Analog Test N Output) -#define RFM22_gpio2_config_atpo 0x0D // Reserved (Analog Test P Output) -#define RFM22_gpio2_config_rv 0xOE // Reference Voltage (output) -#define RFM22_gpio2_config_dclk 0x0F // TX/RX Data CLK output to be used in conjunction with TX/RX Data pin (output) -#define RFM22_gpio2_config_txd 0x10 // TX Data input for direct modulation (input) -#define RFM22_gpio2_config_err 0x11 // External Retransmission Request (input) -#define RFM22_gpio2_config_txstate 0x12 // TX State (output) -#define RFM22_gpio2_config_txfifoaf 0x13 // TX FIFO Almost Full (output) -#define RFM22_gpio2_config_rxd 0x14 // RX Data (output) -#define RFM22_gpio2_config_rxstate 0x15 // RX State (output) -#define RFM22_gpio2_config_rxfifoaf 0x16 // RX FIFO Almost Full (output) -#define RFM22_gpio2_config_antswt1 0x17 // Antenna 1 Switch used for antenna diversity (output) -#define RFM22_gpio2_config_antswt2 0x18 // Antenna 2 Switch used for antenna diversity (output) -#define RFM22_gpio2_config_vpd 0x19 // Valid Preamble Detected (output) -#define RFM22_gpio2_config_ipd 0x1A // Invalid Preamble Detected (output) -#define RFM22_gpio2_config_swd 0x1B // Sync Word Detected (output) -#define RFM22_gpio2_config_cca 0x1C // Clear Channel Assessment (output) -#define RFM22_gpio2_config_vdd 0x1D // VDD -#define RFM22_gpio2_config_pup 0x20 -#define RFM22_gpio2_config_drv0 0x00 // output drive level -#define RFM22_gpio2_config_drv1 0x40 // output drive level -#define RFM22_gpio2_config_drv2 0x80 // output drive level -#define RFM22_gpio2_config_drv3 0xC0 // output drive level +#define RFM22_gpio2_config 0x0D // R/W +#define RFM22_gpio2_config_mc 0x00 // Microcontroller Clock (output) +#define RFM22_gpio2_config_wut 0x01 // Wake-Up Timer: 1 when WUT has expired (output) +#define RFM22_gpio2_config_lbd 0x02 // Low Battery Detect: 1 when battery is below threshold setting (output) +#define RFM22_gpio2_config_ddi 0x03 // Direct Digital Input +#define RFM22_gpio2_config_eife 0x04 // External Interrupt, falling edge (input) +#define RFM22_gpio2_config_eire 0x05 // External Interrupt, rising edge (input) +#define RFM22_gpio2_config_eisc 0x06 // External Interrupt, state change (input) +#define RFM22_gpio2_config_ai 0x07 // ADC Analog Input +#define RFM22_gpio2_config_atni 0x08 // Reserved (Analog Test N Input) +#define RFM22_gpio2_config_atpi 0x09 // Reserved (Analog Test P Input) +#define RFM22_gpio2_config_ddo 0x0A // Direct Digital Output +#define RFM22_gpio2_config_dto 0x0B // Reserved (Digital Test Output) +#define RFM22_gpio2_config_atno 0x0C // Reserved (Analog Test N Output) +#define RFM22_gpio2_config_atpo 0x0D // Reserved (Analog Test P Output) +#define RFM22_gpio2_config_rv 0xOE // Reference Voltage (output) +#define RFM22_gpio2_config_dclk 0x0F // TX/RX Data CLK output to be used in conjunction with TX/RX Data pin (output) +#define RFM22_gpio2_config_txd 0x10 // TX Data input for direct modulation (input) +#define RFM22_gpio2_config_err 0x11 // External Retransmission Request (input) +#define RFM22_gpio2_config_txstate 0x12 // TX State (output) +#define RFM22_gpio2_config_txfifoaf 0x13 // TX FIFO Almost Full (output) +#define RFM22_gpio2_config_rxd 0x14 // RX Data (output) +#define RFM22_gpio2_config_rxstate 0x15 // RX State (output) +#define RFM22_gpio2_config_rxfifoaf 0x16 // RX FIFO Almost Full (output) +#define RFM22_gpio2_config_antswt1 0x17 // Antenna 1 Switch used for antenna diversity (output) +#define RFM22_gpio2_config_antswt2 0x18 // Antenna 2 Switch used for antenna diversity (output) +#define RFM22_gpio2_config_vpd 0x19 // Valid Preamble Detected (output) +#define RFM22_gpio2_config_ipd 0x1A // Invalid Preamble Detected (output) +#define RFM22_gpio2_config_swd 0x1B // Sync Word Detected (output) +#define RFM22_gpio2_config_cca 0x1C // Clear Channel Assessment (output) +#define RFM22_gpio2_config_vdd 0x1D // VDD +#define RFM22_gpio2_config_pup 0x20 +#define RFM22_gpio2_config_drv0 0x00 // output drive level +#define RFM22_gpio2_config_drv1 0x40 // output drive level +#define RFM22_gpio2_config_drv2 0x80 // output drive level +#define RFM22_gpio2_config_drv3 0xC0 // output drive level -#define RFM22_io_port_config 0x0E // R/W -#define RFM22_io_port_extitst2 0x40 // External Interrupt Status. If the GPIO2 is programmed to be external interrupt sources then the status can be read here. -#define RFM22_io_port_extitst1 0x20 // External Interrupt Status. If the GPIO1 is programmed to be external interrupt sources then the status can be read here. -#define RFM22_io_port_extitst0 0x10 // External Interrupt Status. If the GPIO0 is programmed to be external interrupt sources then the status can be read here. -#define RFM22_io_port_itsdo 0x08 // Interrupt Request Output on the SDO Pin. nIRQ output is present on the SDO pin if this bit is set and the nSEL input is inactive (high). -#define RFM22_io_port_dio2 0x04 // Direct I/O for GPIO2. If the GPIO2 is configured to be a direct output then the value on the GPIO pin can be set here. If the GPIO2 is configured to be a direct input then the value of the pin can be read here. -#define RFM22_io_port_dio1 0x02 // Direct I/O for GPIO1. If the GPIO1 is configured to be a direct output then the value on the GPIO pin can be set here. If the GPIO1 is configured to be a direct input then the value of the pin can be read here. -#define RFM22_io_port_dio0 0x01 // Direct I/O for GPIO0. If the GPIO0 is configured to be a direct output then the value on the GPIO pin can be set here. If the GPIO0 is configured to be a direct input then the value of the pin can be read here. -#define RFM22_io_port_default 0x00 // GPIO pins are default +#define RFM22_io_port_config 0x0E // R/W +#define RFM22_io_port_extitst2 0x40 // External Interrupt Status. If the GPIO2 is programmed to be external interrupt sources then the status can be read here. +#define RFM22_io_port_extitst1 0x20 // External Interrupt Status. If the GPIO1 is programmed to be external interrupt sources then the status can be read here. +#define RFM22_io_port_extitst0 0x10 // External Interrupt Status. If the GPIO0 is programmed to be external interrupt sources then the status can be read here. +#define RFM22_io_port_itsdo 0x08 // Interrupt Request Output on the SDO Pin. nIRQ output is present on the SDO pin if this bit is set and the nSEL input is inactive (high). +#define RFM22_io_port_dio2 0x04 // Direct I/O for GPIO2. If the GPIO2 is configured to be a direct output then the value on the GPIO pin can be set here. If the GPIO2 is configured to be a direct input then the value of the pin can be read here. +#define RFM22_io_port_dio1 0x02 // Direct I/O for GPIO1. If the GPIO1 is configured to be a direct output then the value on the GPIO pin can be set here. If the GPIO1 is configured to be a direct input then the value of the pin can be read here. +#define RFM22_io_port_dio0 0x01 // Direct I/O for GPIO0. If the GPIO0 is configured to be a direct output then the value on the GPIO pin can be set here. If the GPIO0 is configured to be a direct input then the value of the pin can be read here. +#define RFM22_io_port_default 0x00 // GPIO pins are default -#define RFM22_adc_config 0x0F // R/W -#define RFM22_ac_adcgain0 0x00 -#define RFM22_ac_adcgain1 0x01 -#define RFM22_ac_adcgain2 0x02 -#define RFM22_ac_adcgain3 0x03 -#define RFM22_ac_adcref_bg 0x00 -#define RFM22_ac_adcref_vdd3 0x08 -#define RFM22_ac_adcref_vdd2 0x0C -#define RFM22_ac_adcsel_temp_sensor 0x00 -#define RFM22_ac_adcsel_gpio0 0x10 -#define RFM22_ac_adcsel_gpio1 0x20 -#define RFM22_ac_adcsel_gpio2 0x30 -#define RFM22_ac_adcsel_gpio01 0x40 -#define RFM22_ac_adcsel_gpio12 0x50 -#define RFM22_ac_adcsel_gpio02 0x60 -#define RFM22_ac_adcsel_gpio_gnd 0x70 -#define RFM22_ac_adcstartbusy 0x80 +#define RFM22_adc_config 0x0F // R/W +#define RFM22_ac_adcgain0 0x00 +#define RFM22_ac_adcgain1 0x01 +#define RFM22_ac_adcgain2 0x02 +#define RFM22_ac_adcgain3 0x03 +#define RFM22_ac_adcref_bg 0x00 +#define RFM22_ac_adcref_vdd3 0x08 +#define RFM22_ac_adcref_vdd2 0x0C +#define RFM22_ac_adcsel_temp_sensor 0x00 +#define RFM22_ac_adcsel_gpio0 0x10 +#define RFM22_ac_adcsel_gpio1 0x20 +#define RFM22_ac_adcsel_gpio2 0x30 +#define RFM22_ac_adcsel_gpio01 0x40 +#define RFM22_ac_adcsel_gpio12 0x50 +#define RFM22_ac_adcsel_gpio02 0x60 +#define RFM22_ac_adcsel_gpio_gnd 0x70 +#define RFM22_ac_adcstartbusy 0x80 -#define RFM22_adc_sensor_amp_offset 0x10 // R/W -#define RFM22_asao_adcoffs_mask 0x0F // ADC Sensor Amplifier Offset. The offset can be calculated as Offset = adcoffs[2:0] x VDD/1000; MSB = adcoffs[3] = Sign bit. +#define RFM22_adc_sensor_amp_offset 0x10 // R/W +#define RFM22_asao_adcoffs_mask 0x0F // ADC Sensor Amplifier Offset. The offset can be calculated as Offset = adcoffs[2:0] x VDD/1000; MSB = adcoffs[3] = Sign bit. -#define RFM22_adc_value 0x11 // R .. Internal 8 bit ADC Output Value. +#define RFM22_adc_value 0x11 // R .. Internal 8 bit ADC Output Value. -#define RFM22_temp_sensor_calib 0x12 // R/W -#define RFM22_tsc_tstrim_mask 0x0F // Temperature Sensor Trim Value. -#define RFM22_tsc_entstrim 0x10 // Temperature Sensor Trim Enable. -#define RFM22_tsc_entsoffs 0x20 // Temperature Sensor Offset to Convert from K to �C. -#define RFM22_tsc_tsrange0 0x00 // Temperature Sensor Range Selection. �64C to +64C 0.5C resolution -#define RFM22_tsc_tsrange1 0x40 // -40 to +85C with 1.0C resolution -#define RFM22_tsc_tsrange2 0x80 // 0C to 85C with 0.5C resolution -#define RFM22_tsc_tsrange3 0xC0 // -40F to 216F with 1.0F resolution +#define RFM22_temp_sensor_calib 0x12 // R/W +#define RFM22_tsc_tstrim_mask 0x0F // Temperature Sensor Trim Value. +#define RFM22_tsc_entstrim 0x10 // Temperature Sensor Trim Enable. +#define RFM22_tsc_entsoffs 0x20 // Temperature Sensor Offset to Convert from K to �C. +#define RFM22_tsc_tsrange0 0x00 // Temperature Sensor Range Selection. �64C to +64C 0.5C resolution +#define RFM22_tsc_tsrange1 0x40 // -40 to +85C with 1.0C resolution +#define RFM22_tsc_tsrange2 0x80 // 0C to 85C with 0.5C resolution +#define RFM22_tsc_tsrange3 0xC0 // -40F to 216F with 1.0F resolution -#define RFM22_temp_value_offset 0x13 // R/W +#define RFM22_temp_value_offset 0x13 // R/W -#define RFM22_wakeup_timer_period1 0x14 // R/W -#define RFM22_wakeup_timer_period2 0x15 // R/W -#define RFM22_wakeup_timer_period3 0x16 // R/W +#define RFM22_wakeup_timer_period1 0x14 // R/W +#define RFM22_wakeup_timer_period2 0x15 // R/W +#define RFM22_wakeup_timer_period3 0x16 // R/W -#define RFM22_wakeup_timer_value1 0x17 // R -#define RFM22_wakeup_timer_value2 0x18 // R +#define RFM22_wakeup_timer_value1 0x17 // R +#define RFM22_wakeup_timer_value2 0x18 // R -#define RFM22_low_dutycycle_mode_duration 0x19 // R/W -#define RFM22_low_battery_detector_threshold 0x1A // R/W +#define RFM22_low_dutycycle_mode_duration 0x19 // R/W +#define RFM22_low_battery_detector_threshold 0x1A // R/W -#define RFM22_battery_volateg_level 0x1B // R +#define RFM22_battery_volateg_level 0x1B // R -#define RFM22_if_filter_bandwidth 0x1C // R/W -#define RFM22_iffbw_filset_mask 0x0F -#define RFM22_iffbw_ndec_exp_mask 0x70 -#define RFM22_iffbw_dwn3_bypass 0x80 +#define RFM22_if_filter_bandwidth 0x1C // R/W +#define RFM22_iffbw_filset_mask 0x0F +#define RFM22_iffbw_ndec_exp_mask 0x70 +#define RFM22_iffbw_dwn3_bypass 0x80 -#define RFM22_afc_loop_gearshift_override 0x1D // R/W -#define RFM22_afc_lp_gs_ovrd_afcgearl_mask 0x07 // AFC Low Gear Setting. -#define RFM22_afc_lp_gs_ovrd_afcgearh_mask 0x38 // AFC High Gear Setting. -#define RFM22_afc_lp_gs_ovrd_enafc 0x40 // AFC Enable. -#define RFM22_afc_lp_gs_ovrd_afcbd 0x80 // If set, the tolerated AFC frequency error will be halved. +#define RFM22_afc_loop_gearshift_override 0x1D // R/W +#define RFM22_afc_lp_gs_ovrd_afcgearl_mask 0x07 // AFC Low Gear Setting. +#define RFM22_afc_lp_gs_ovrd_afcgearh_mask 0x38 // AFC High Gear Setting. +#define RFM22_afc_lp_gs_ovrd_enafc 0x40 // AFC Enable. +#define RFM22_afc_lp_gs_ovrd_afcbd 0x80 // If set, the tolerated AFC frequency error will be halved. -#define RFM22_afc_timing_control 0x1E // R/W +#define RFM22_afc_timing_control 0x1E // R/W -#define RFM22_clk_recovery_gearshift_override 0x1F // R/W -#define RFM22_clk_recovery_oversampling_ratio 0x20 // R/W -#define RFM22_clk_recovery_offset2 0x21 // R/W -#define RFM22_clk_recovery_offset1 0x22 // R/W -#define RFM22_clk_recovery_offset0 0x23 // R/W -#define RFM22_clk_recovery_timing_loop_gain1 0x24 // R/W -#define RFM22_clk_recovery_timing_loop_gain0 0x25 // R/W +#define RFM22_clk_recovery_gearshift_override 0x1F // R/W +#define RFM22_clk_recovery_oversampling_ratio 0x20 // R/W +#define RFM22_clk_recovery_offset2 0x21 // R/W +#define RFM22_clk_recovery_offset1 0x22 // R/W +#define RFM22_clk_recovery_offset0 0x23 // R/W +#define RFM22_clk_recovery_timing_loop_gain1 0x24 // R/W +#define RFM22_clk_recovery_timing_loop_gain0 0x25 // R/W -#define RFM22_rssi 0x26 // R -#define RFM22_rssi_threshold_clear_chan_indicator 0x27 // R/W +#define RFM22_rssi 0x26 // R +#define RFM22_rssi_threshold_clear_chan_indicator 0x27 // R/W -#define RFM22_antenna_diversity_register1 0x28 // R -#define RFM22_antenna_diversity_register2 0x29 // R +#define RFM22_antenna_diversity_register1 0x28 // R +#define RFM22_antenna_diversity_register2 0x29 // R -#define RFM22_afc_limiter 0x2A // R/W .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz +#define RFM22_afc_limiter 0x2A // R/W .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz -#define RFM22_afc_correction_read 0x2B // R +#define RFM22_afc_correction_read 0x2B // R -#define RFM22_ook_counter_value1 0x2C // R/W -#define RFM22_ook_counter_value2 0x2D // R/W +#define RFM22_ook_counter_value1 0x2C // R/W +#define RFM22_ook_counter_value2 0x2D // R/W -#define RFM22_slicer_peak_hold 0x2E // R/W +#define RFM22_slicer_peak_hold 0x2E // R/W -#define RFM22_data_access_control 0x30 // R/W -#define RFM22_dac_crc_ccitt 0x00 // -#define RFM22_dac_crc_crc16 0x01 // -#define RFM22_dac_crc_iec16 0x02 // -#define RFM22_dac_crc_biacheva 0x03 // -#define RFM22_dac_encrc 0x04 // CRC Enable. Cyclic Redundancy Check generation is enabled if this bit is set. -#define RFM22_dac_enpactx 0x08 // Enable Packet TX Handling. If FIFO Mode (dtmod = 10) is being used automatic packet handling may be enabled. Setting enpactx = 1 will enable automatic packet handling in the TX path. Register 30�4D allow for various configurations of the packet structure. Setting enpactx = 0 will not do any packet handling in the TX path. It will only transmit what is loaded to the FIFO. -#define RFM22_dac_skip2ph 0x10 // Skip 2nd Phase of Preamble Detection. If set, we skip the second phase of the preamble detection (under certain conditions) if antenna diversity is enabled. -#define RFM22_dac_crcdonly 0x20 // CRC Data Only Enable. When this bit is set to 1 the CRC is calculated on and checked against the packet data fields only. -#define RFM22_dac_lsbfrst 0x40 // LSB First Enable. The LSB of the data will be transmitted/received first if this bit is set. -#define RFM22_dac_enpacrx 0x80 // Enable Packet RX Handling. If FIFO Mode (dtmod = 10) is being used automatic packet handling may be enabled. Setting enpacrx = 1 will enable automatic packet handling in the RX path. Register 30�4D allow for various configurations of the packet structure. Setting enpacrx = 0 will not do any packet handling in the RX path. It will only receive everything after the sync word and fill up the RX FIFO. +#define RFM22_data_access_control 0x30 // R/W +#define RFM22_dac_crc_ccitt 0x00 // +#define RFM22_dac_crc_crc16 0x01 // +#define RFM22_dac_crc_iec16 0x02 // +#define RFM22_dac_crc_biacheva 0x03 // +#define RFM22_dac_encrc 0x04 // CRC Enable. Cyclic Redundancy Check generation is enabled if this bit is set. +#define RFM22_dac_enpactx 0x08 // Enable Packet TX Handling. If FIFO Mode (dtmod = 10) is being used automatic packet handling may be enabled. Setting enpactx = 1 will enable automatic packet handling in the TX path. Register 30�4D allow for various configurations of the packet structure. Setting enpactx = 0 will not do any packet handling in the TX path. It will only transmit what is loaded to the FIFO. +#define RFM22_dac_skip2ph 0x10 // Skip 2nd Phase of Preamble Detection. If set, we skip the second phase of the preamble detection (under certain conditions) if antenna diversity is enabled. +#define RFM22_dac_crcdonly 0x20 // CRC Data Only Enable. When this bit is set to 1 the CRC is calculated on and checked against the packet data fields only. +#define RFM22_dac_lsbfrst 0x40 // LSB First Enable. The LSB of the data will be transmitted/received first if this bit is set. +#define RFM22_dac_enpacrx 0x80 // Enable Packet RX Handling. If FIFO Mode (dtmod = 10) is being used automatic packet handling may be enabled. Setting enpacrx = 1 will enable automatic packet handling in the RX path. Register 30�4D allow for various configurations of the packet structure. Setting enpacrx = 0 will not do any packet handling in the RX path. It will only receive everything after the sync word and fill up the RX FIFO. -#define RFM22_ezmac_status 0x31 // R -#define RFM22_ezmac_status_pksent 0x01 // Packet Sent. A 1 a packet has been sent by the radio. (Same bit as in register 03, but reading it does not reset the IRQ) -#define RFM22_ezmac_status_pktx 0x02 // Packet Transmitting. When 1 the radio is currently transmitting a packet. -#define RFM22_ezmac_status_crcerror 0x04 // CRC Error. When 1 a Cyclic Redundancy Check error has been detected. (Same bit as in register 03, but reading it does not reset the IRQ) -#define RFM22_ezmac_status_pkvalid 0x08 // Valid Packet Received. When a 1 a valid packet has been received by the receiver. (Same bit as in register 03, but reading it does not reset the IRQ) -#define RFM22_ezmac_status_pkrx 0x10 // Packet Receiving. When 1 the radio is currently receiving a valid packet. -#define RFM22_ezmac_status_pksrch 0x20 // Packet Searching. When 1 the radio is searching for a valid packet. -#define RFM22_ezmac_status_rxcrc1 0x40 // If high, it indicates the last CRC received is all one�s. May indicated Transmitter underflow in case of CRC error. +#define RFM22_ezmac_status 0x31 // R +#define RFM22_ezmac_status_pksent 0x01 // Packet Sent. A 1 a packet has been sent by the radio. (Same bit as in register 03, but reading it does not reset the IRQ) +#define RFM22_ezmac_status_pktx 0x02 // Packet Transmitting. When 1 the radio is currently transmitting a packet. +#define RFM22_ezmac_status_crcerror 0x04 // CRC Error. When 1 a Cyclic Redundancy Check error has been detected. (Same bit as in register 03, but reading it does not reset the IRQ) +#define RFM22_ezmac_status_pkvalid 0x08 // Valid Packet Received. When a 1 a valid packet has been received by the receiver. (Same bit as in register 03, but reading it does not reset the IRQ) +#define RFM22_ezmac_status_pkrx 0x10 // Packet Receiving. When 1 the radio is currently receiving a valid packet. +#define RFM22_ezmac_status_pksrch 0x20 // Packet Searching. When 1 the radio is searching for a valid packet. +#define RFM22_ezmac_status_rxcrc1 0x40 // If high, it indicates the last CRC received is all one�s. May indicated Transmitter underflow in case of CRC error. -#define RFM22_header_control1 0x32 // R/W -#define RFM22_header_cntl1_bcen_none 0x00 // No broadcast address enable. -#define RFM22_header_cntl1_bcen_0 0x10 // Broadcast address enable for header byte 0. -#define RFM22_header_cntl1_bcen_1 0x20 // Broadcast address enable for header byte 1. -#define RFM22_header_cntl1_bcen_2 0x40 // Broadcast address enable for header byte 2. -#define RFM22_header_cntl1_bcen_3 0x80 // Broadcast address enable for header byte 3. -#define RFM22_header_cntl1_hdch_none 0x00 // No Received Header check -#define RFM22_header_cntl1_hdch_0 0x01 // Received Header check for byte 0. -#define RFM22_header_cntl1_hdch_1 0x02 // Received Header check for byte 1. -#define RFM22_header_cntl1_hdch_2 0x04 // Received Header check for byte 2. -#define RFM22_header_cntl1_hdch_3 0x08 // Received Header check for byte 3. +#define RFM22_header_control1 0x32 // R/W +#define RFM22_header_cntl1_bcen_none 0x00 // No broadcast address enable. +#define RFM22_header_cntl1_bcen_0 0x10 // Broadcast address enable for header byte 0. +#define RFM22_header_cntl1_bcen_1 0x20 // Broadcast address enable for header byte 1. +#define RFM22_header_cntl1_bcen_2 0x40 // Broadcast address enable for header byte 2. +#define RFM22_header_cntl1_bcen_3 0x80 // Broadcast address enable for header byte 3. +#define RFM22_header_cntl1_hdch_none 0x00 // No Received Header check +#define RFM22_header_cntl1_hdch_0 0x01 // Received Header check for byte 0. +#define RFM22_header_cntl1_hdch_1 0x02 // Received Header check for byte 1. +#define RFM22_header_cntl1_hdch_2 0x04 // Received Header check for byte 2. +#define RFM22_header_cntl1_hdch_3 0x08 // Received Header check for byte 3. -#define RFM22_header_control2 0x33 // R/W -#define RFM22_header_cntl2_prealen 0x01 // MSB of Preamble Length. See register Preamble Length. -#define RFM22_header_cntl2_synclen_3 0x00 // Synchronization Word 3 -#define RFM22_header_cntl2_synclen_32 0x02 // Synchronization Word 3 followed by 2 -#define RFM22_header_cntl2_synclen_321 0x04 // Synchronization Word 3 followed by 2 followed by 1 -#define RFM22_header_cntl2_synclen_3210 0x06 // Synchronization Word 3 followed by 2 followed by 1 followed by 0 -#define RFM22_header_cntl2_fixpklen 0x08 // Fix Packet Length. When fixpklen = 1 the packet length (pklen[7:0]) is not included in the header. When fixpklen = 0 the packet length is included in the header. -#define RFM22_header_cntl2_hdlen_none 0x00 // no header -#define RFM22_header_cntl2_hdlen_3 0x10 // header 3 -#define RFM22_header_cntl2_hdlen_32 0x20 // header 3 and 2 -#define RFM22_header_cntl2_hdlen_321 0x30 // header 3 and 2 and 1 -#define RFM22_header_cntl2_hdlen_3210 0x40 // header 3 and 2 and 1 and 0 -#define RFM22_header_cntl2_skipsyn 0x80 // If high, the system will ignore the syncword search timeout reset. The chip will not return to searching for Preamble, but instead will remain searching for Sync word. +#define RFM22_header_control2 0x33 // R/W +#define RFM22_header_cntl2_prealen 0x01 // MSB of Preamble Length. See register Preamble Length. +#define RFM22_header_cntl2_synclen_3 0x00 // Synchronization Word 3 +#define RFM22_header_cntl2_synclen_32 0x02 // Synchronization Word 3 followed by 2 +#define RFM22_header_cntl2_synclen_321 0x04 // Synchronization Word 3 followed by 2 followed by 1 +#define RFM22_header_cntl2_synclen_3210 0x06 // Synchronization Word 3 followed by 2 followed by 1 followed by 0 +#define RFM22_header_cntl2_fixpklen 0x08 // Fix Packet Length. When fixpklen = 1 the packet length (pklen[7:0]) is not included in the header. When fixpklen = 0 the packet length is included in the header. +#define RFM22_header_cntl2_hdlen_none 0x00 // no header +#define RFM22_header_cntl2_hdlen_3 0x10 // header 3 +#define RFM22_header_cntl2_hdlen_32 0x20 // header 3 and 2 +#define RFM22_header_cntl2_hdlen_321 0x30 // header 3 and 2 and 1 +#define RFM22_header_cntl2_hdlen_3210 0x40 // header 3 and 2 and 1 and 0 +#define RFM22_header_cntl2_skipsyn 0x80 // If high, the system will ignore the syncword search timeout reset. The chip will not return to searching for Preamble, but instead will remain searching for Sync word. -#define RFM22_preamble_length 0x34 // R/W +#define RFM22_preamble_length 0x34 // R/W -#define RFM22_preamble_detection_ctrl1 0x35 // R/W -#define RFM22_pre_det_ctrl1_preath_mask 0xF8 // Number of nibbles processed during detection. -#define RFM22_pre_det_ctrl1_rssi_offset_mask 0x07 // Value added as offset to RSSI calculation. Every increment in this register results in an increment of +4 dB in the RSSI. +#define RFM22_preamble_detection_ctrl1 0x35 // R/W +#define RFM22_pre_det_ctrl1_preath_mask 0xF8 // Number of nibbles processed during detection. +#define RFM22_pre_det_ctrl1_rssi_offset_mask 0x07 // Value added as offset to RSSI calculation. Every increment in this register results in an increment of +4 dB in the RSSI. -#define RFM22_sync_word3 0x36 // R/W -#define RFM22_sync_word2 0x37 // R/W -#define RFM22_sync_word1 0x38 // R/W -#define RFM22_sync_word0 0x39 // R/W +#define RFM22_sync_word3 0x36 // R/W +#define RFM22_sync_word2 0x37 // R/W +#define RFM22_sync_word1 0x38 // R/W +#define RFM22_sync_word0 0x39 // R/W -#define RFM22_transmit_header3 0x3A // R/W -#define RFM22_transmit_header2 0x3B // R/W -#define RFM22_transmit_header1 0x3C // R/W -#define RFM22_transmit_header0 0x3D // R/W +#define RFM22_transmit_header3 0x3A // R/W +#define RFM22_transmit_header2 0x3B // R/W +#define RFM22_transmit_header1 0x3C // R/W +#define RFM22_transmit_header0 0x3D // R/W -#define RFM22_transmit_packet_length 0x3E // R/W +#define RFM22_transmit_packet_length 0x3E // R/W -#define RFM22_check_header3 0x3F // R/W -#define RFM22_check_header2 0x40 // R/W -#define RFM22_check_header1 0x41 // R/W -#define RFM22_check_header0 0x42 // R/W +#define RFM22_check_header3 0x3F // R/W +#define RFM22_check_header2 0x40 // R/W +#define RFM22_check_header1 0x41 // R/W +#define RFM22_check_header0 0x42 // R/W -#define RFM22_header_enable3 0x43 // R/W -#define RFM22_header_enable2 0x44 // R/W -#define RFM22_header_enable1 0x45 // R/W -#define RFM22_header_enable0 0x46 // R/W +#define RFM22_header_enable3 0x43 // R/W +#define RFM22_header_enable2 0x44 // R/W +#define RFM22_header_enable1 0x45 // R/W +#define RFM22_header_enable0 0x46 // R/W -#define RFM22_received_header3 0x47 // R -#define RFM22_received_header2 0x48 // R -#define RFM22_received_header1 0x49 // R -#define RFM22_received_header0 0x4A // R +#define RFM22_received_header3 0x47 // R +#define RFM22_received_header2 0x48 // R +#define RFM22_received_header1 0x49 // R +#define RFM22_received_header0 0x4A // R -#define RFM22_received_packet_length 0x4B // R +#define RFM22_received_packet_length 0x4B // R -#define RFM22_adc8_control 0x4F // R/W +#define RFM22_adc8_control 0x4F // R/W -#define RFM22_channel_filter_coeff_addr 0x60 // R/W -#define RFM22_ch_fil_coeff_ad_inv_pre_th_mask 0xF0 // -#define RFM22_ch_fil_coeff_ad_chfiladd_mask 0x0F // Channel Filter Coefficient Look-up Table Address. The address for channel filter coefficients used in the RX path. +#define RFM22_channel_filter_coeff_addr 0x60 // R/W +#define RFM22_ch_fil_coeff_ad_inv_pre_th_mask 0xF0 // +#define RFM22_ch_fil_coeff_ad_chfiladd_mask 0x0F // Channel Filter Coefficient Look-up Table Address. The address for channel filter coefficients used in the RX path. -#define RFM22_xtal_osc_por_ctrl 0x62 // R/W -#define RFM22_xtal_osc_por_ctrl_pwst_mask 0xE0 // Internal Power States of the Chip. -#define RFM22_xtal_osc_por_ctrl_clkhyst 0x10 // Clock Hysteresis Setting. -#define RFM22_xtal_osc_por_ctrl_enbias2x 0x08 // 2 Times Higher Bias Current Enable. -#define RFM22_xtal_osc_por_ctrl_enamp2x 0x04 // 2 Times Higher Amplification Enable. -#define RFM22_xtal_osc_por_ctrl_bufovr 0x02 // Output Buffer Enable Override. -#define RFM22_xtal_osc_por_ctrl_enbuf 0x01 // Output Buffer Enable. +#define RFM22_xtal_osc_por_ctrl 0x62 // R/W +#define RFM22_xtal_osc_por_ctrl_pwst_mask 0xE0 // Internal Power States of the Chip. +#define RFM22_xtal_osc_por_ctrl_clkhyst 0x10 // Clock Hysteresis Setting. +#define RFM22_xtal_osc_por_ctrl_enbias2x 0x08 // 2 Times Higher Bias Current Enable. +#define RFM22_xtal_osc_por_ctrl_enamp2x 0x04 // 2 Times Higher Amplification Enable. +#define RFM22_xtal_osc_por_ctrl_bufovr 0x02 // Output Buffer Enable Override. +#define RFM22_xtal_osc_por_ctrl_enbuf 0x01 // Output Buffer Enable. -#define RFM22_agc_override1 0x69 // R/W -#define RFM22_agc_ovr1_sgi 0x40 // AGC Loop, Set Gain Increase. If set to 0 then gain increasing will not be allowed. If set to 1 then gain increasing is allowed, default is 0. -#define RFM22_agc_ovr1_agcen 0x20 // Automatic Gain Control Enable. When this bit is set then the result of the control can be read out from bits [4:0], otherwise the gain can be controlled manually by writing into bits [4:0]. -#define RFM22_agc_ovr1_lnagain 0x10 // LNA Gain Select. 0 = min gain = 5dB, 1 = max gain = 25 dB. -#define RFM22_agc_ovr1_pga_mask 0x0F // PGA Gain Override Value. +#define RFM22_agc_override1 0x69 // R/W +#define RFM22_agc_ovr1_sgi 0x40 // AGC Loop, Set Gain Increase. If set to 0 then gain increasing will not be allowed. If set to 1 then gain increasing is allowed, default is 0. +#define RFM22_agc_ovr1_agcen 0x20 // Automatic Gain Control Enable. When this bit is set then the result of the control can be read out from bits [4:0], otherwise the gain can be controlled manually by writing into bits [4:0]. +#define RFM22_agc_ovr1_lnagain 0x10 // LNA Gain Select. 0 = min gain = 5dB, 1 = max gain = 25 dB. +#define RFM22_agc_ovr1_pga_mask 0x0F // PGA Gain Override Value. -#define RFM22_tx_power 0x6D // R/W -#define RFM22_tx_pwr_lna_sw 0x08 // LNA Switch Controller. If set, lna_sw control from the digital will go high during TX modes, and low during other times. If reset, the digital control signal is low at all times. +#define RFM22_tx_power 0x6D // R/W +#define RFM22_tx_pwr_lna_sw 0x08 // LNA Switch Controller. If set, lna_sw control from the digital will go high during TX modes, and low during other times. If reset, the digital control signal is low at all times. -#define RFM22_tx_data_rate1 0x6E // R/W -#define RFM22_tx_data_rate0 0x6F // R/W +#define RFM22_tx_data_rate1 0x6E // R/W +#define RFM22_tx_data_rate0 0x6F // R/W -#define RFM22_modulation_mode_control1 0x70 // R/W -#define RFM22_mmc1_enwhite 0x01 // Data Whitening is Enabled if this bit is set. -#define RFM22_mmc1_enmanch 0x02 // Manchester Coding is Enabled if this bit is set. -#define RFM22_mmc1_enmaninv 0x04 // Manchester Data Inversion is Enabled if this bit is set. -#define RFM22_mmc1_manppol 0x08 // Manchester Preamble Polarity (will transmit a series of 1 if set, or series of 0 if reset). -#define RFM22_mmc1_enphpwdn 0x10 // If set, the Packet Handler will be powered down when chip is in low power mode. -#define RFM22_mmc1_txdtrtscale 0x20 // This bit should be set for Data Rates below 30 kbps. +#define RFM22_modulation_mode_control1 0x70 // R/W +#define RFM22_mmc1_enwhite 0x01 // Data Whitening is Enabled if this bit is set. +#define RFM22_mmc1_enmanch 0x02 // Manchester Coding is Enabled if this bit is set. +#define RFM22_mmc1_enmaninv 0x04 // Manchester Data Inversion is Enabled if this bit is set. +#define RFM22_mmc1_manppol 0x08 // Manchester Preamble Polarity (will transmit a series of 1 if set, or series of 0 if reset). +#define RFM22_mmc1_enphpwdn 0x10 // If set, the Packet Handler will be powered down when chip is in low power mode. +#define RFM22_mmc1_txdtrtscale 0x20 // This bit should be set for Data Rates below 30 kbps. -#define RFM22_modulation_mode_control2 0x71 // R/W -#define RFM22_mmc2_modtyp_mask 0x03 // Modulation type. -#define RFM22_mmc2_modtyp_none 0x00 // -#define RFM22_mmc2_modtyp_ook 0x01 // -#define RFM22_mmc2_modtyp_fsk 0x02 // -#define RFM22_mmc2_modtyp_gfsk 0x03 // -#define RFM22_mmc2_fd 0x04 // MSB of Frequency Deviation Setting, see "Register 72h. Frequency Deviation". -#define RFM22_mmc2_eninv 0x08 // Invert TX and RX Data. -#define RFM22_mmc2_dtmod_mask 0x30 // Modulation source. -#define RFM22_mmc2_dtmod_dm_gpio 0x00 // -#define RFM22_mmc2_dtmod_dm_sdi 0x10 // -#define RFM22_mmc2_dtmod_fifo 0x20 // -#define RFM22_mmc2_dtmod_pn9 0x30 // -#define RFM22_mmc2_trclk_mask 0xC0 // TX Data Clock Configuration. -#define RFM22_mmc2_trclk_clk_none 0x00 // -#define RFM22_mmc2_trclk_clk_gpio 0x40 // -#define RFM22_mmc2_trclk_clk_sdo 0x80 // -#define RFM22_mmc2_trclk_clk_nirq 0xC0 // +#define RFM22_modulation_mode_control2 0x71 // R/W +#define RFM22_mmc2_modtyp_mask 0x03 // Modulation type. +#define RFM22_mmc2_modtyp_none 0x00 // +#define RFM22_mmc2_modtyp_ook 0x01 // +#define RFM22_mmc2_modtyp_fsk 0x02 // +#define RFM22_mmc2_modtyp_gfsk 0x03 // +#define RFM22_mmc2_fd 0x04 // MSB of Frequency Deviation Setting, see "Register 72h. Frequency Deviation". +#define RFM22_mmc2_eninv 0x08 // Invert TX and RX Data. +#define RFM22_mmc2_dtmod_mask 0x30 // Modulation source. +#define RFM22_mmc2_dtmod_dm_gpio 0x00 // +#define RFM22_mmc2_dtmod_dm_sdi 0x10 // +#define RFM22_mmc2_dtmod_fifo 0x20 // +#define RFM22_mmc2_dtmod_pn9 0x30 // +#define RFM22_mmc2_trclk_mask 0xC0 // TX Data Clock Configuration. +#define RFM22_mmc2_trclk_clk_none 0x00 // +#define RFM22_mmc2_trclk_clk_gpio 0x40 // +#define RFM22_mmc2_trclk_clk_sdo 0x80 // +#define RFM22_mmc2_trclk_clk_nirq 0xC0 // -#define RFM22_frequency_deviation 0x72 // R/W +#define RFM22_frequency_deviation 0x72 // R/W -#define RFM22_frequency_offset1 0x73 // R/W -#define RFM22_frequency_offset2 0x74 // R/W +#define RFM22_frequency_offset1 0x73 // R/W +#define RFM22_frequency_offset2 0x74 // R/W -#define RFM22_frequency_band_select 0x75 // R/W -#define RFM22_fb_mask 0x1F -#define RFM22_fbs_hbsel 0x20 -#define RFM22_fbs_sbse 0x40 +#define RFM22_frequency_band_select 0x75 // R/W +#define RFM22_fb_mask 0x1F +#define RFM22_fbs_hbsel 0x20 +#define RFM22_fbs_sbse 0x40 -#define RFM22_nominal_carrier_frequency1 0x76 // R/W -#define RFM22_nominal_carrier_frequency0 0x77 // R/W +#define RFM22_nominal_carrier_frequency1 0x76 // R/W +#define RFM22_nominal_carrier_frequency0 0x77 // R/W -#define RFM22_frequency_hopping_channel_select 0x79 // R/W -#define RFM22_frequency_hopping_step_size 0x7A // R/W +#define RFM22_frequency_hopping_channel_select 0x79 // R/W +#define RFM22_frequency_hopping_step_size 0x7A // R/W -#define RFM22_tx_fifo_control1 0x7C // R/W .. TX FIFO Almost Full Threshold (0 - 63) -#define RFM22_tx_fifo_control1_mask 0x3F +#define RFM22_tx_fifo_control1 0x7C // R/W .. TX FIFO Almost Full Threshold (0 - 63) +#define RFM22_tx_fifo_control1_mask 0x3F -#define RFM22_tx_fifo_control2 0x7D // R/W .. TX FIFO Almost Empty Threshold (0 - 63) -#define RFM22_tx_fifo_control2_mask 0x3F +#define RFM22_tx_fifo_control2 0x7D // R/W .. TX FIFO Almost Empty Threshold (0 - 63) +#define RFM22_tx_fifo_control2_mask 0x3F -#define RFM22_rx_fifo_control 0x7E // R/W .. RX FIFO Almost Full Threshold (0 - 63) -#define RFM22_rx_fifo_control_mask 0x3F +#define RFM22_rx_fifo_control 0x7E // R/W .. RX FIFO Almost Full Threshold (0 - 63) +#define RFM22_rx_fifo_control_mask 0x3F -#define RFM22_fifo_access 0x7F // R/W +#define RFM22_fifo_access 0x7F // R/W // External type definitions -typedef int16_t (*t_rfm22_TxDataByteCallback) (void); -typedef bool (*t_rfm22_RxDataCallback) (void *data, uint8_t len); +typedef int16_t (*t_rfm22_TxDataByteCallback)(void); +typedef bool (*t_rfm22_RxDataCallback)(void *data, uint8_t len); enum pios_rfm22b_dev_magic { PIOS_RFM22B_DEV_MAGIC = 0x68e971b6, }; @@ -573,7 +573,7 @@ enum pios_radio_event { RADIO_EVENT_ERROR, RADIO_EVENT_FATAL_ERROR, - RADIO_EVENT_NUM_EVENTS // Must be last + RADIO_EVENT_NUM_EVENTS // Must be last }; enum pios_rfm22b_state { @@ -590,31 +590,31 @@ enum pios_rfm22b_state { #define RFM22B_RX_PACKET_STATS_LEN 4 enum pios_rfm22b_rx_packet_status { - RADIO_GOOD_RX_PACKET = 0x00, + RADIO_GOOD_RX_PACKET = 0x00, RADIO_CORRECTED_RX_PACKET = 0x01, - RADIO_ERROR_RX_PACKET = 0x2, - RADIO_RESENT_TX_PACKET = 0x3 + RADIO_ERROR_RX_PACKET = 0x2, + RADIO_RESENT_TX_PACKET = 0x3 }; typedef struct { uint32_t pairID; - int8_t rssi; - int8_t afc_correction; - uint8_t lastContact; + int8_t rssi; + int8_t afc_correction; + uint8_t lastContact; } rfm22b_pair_stats; typedef struct { uint32_t pairID; - OPLinkSettingsRemoteMainPortOptions main_port; + OPLinkSettingsRemoteMainPortOptions main_port; OPLinkSettingsRemoteFlexiPortOptions flexi_port; - OPLinkSettingsRemoteVCPPortOptions vcp_port; + OPLinkSettingsRemoteVCPPortOptions vcp_port; OPLinkSettingsComSpeedOptions com_speed; } rfm22b_binding; enum pios_rfm22b_chip_power_state { - RFM22B_IDLE_STATE = 0x00, - RFM22B_RX_STATE = 0x01, - RFM22B_TX_STATE = 0x10, + RFM22B_IDLE_STATE = 0x00, + RFM22B_RX_STATE = 0x01, + RFM22B_TX_STATE = 0x10, RFM22B_INVALID_STATE = 0x11 }; @@ -622,11 +622,11 @@ enum pios_rfm22b_chip_power_state { typedef union { struct { uint8_t state : 2; - bool frequency_error : 1; - bool header_error : 1; - bool rx_fifo_empty : 1; - bool fifo_underflow : 1; - bool fifo_overflow : 1; + bool frequency_error : 1; + bool header_error : 1; + bool rx_fifo_empty : 1; + bool fifo_underflow : 1; + bool fifo_overflow : 1; }; uint8_t raw; } rfm22b_device_status_reg; @@ -686,7 +686,7 @@ typedef struct { struct pios_rfm22b_dev { enum pios_rfm22b_dev_magic magic; - struct pios_rfm22b_cfg cfg; + struct pios_rfm22b_cfg cfg; // The SPI bus information uint32_t spi_id; @@ -700,10 +700,10 @@ struct pios_rfm22b_dev { // The list of bound radios. rfm22b_binding bindings[OPLINKSETTINGS_BINDINGS_NUMELEM]; - uint8_t cur_binding; + uint8_t cur_binding; // Is this device a coordinator? - bool coordinator; + bool coordinator; // The task handle xTaskHandle taskHandle; @@ -712,7 +712,7 @@ struct pios_rfm22b_dev { rfm22b_pair_stats pair_stats[OPLINKSTATUS_PAIRIDS_NUMELEM]; // ISR pending semaphore - xSemaphoreHandle isrPending; + xSemaphoreHandle isrPending; // The com configuration callback PIOS_RFM22B_ComConfigCallback com_config_cb; @@ -724,10 +724,10 @@ struct pios_rfm22b_dev { uint32_t tx_out_context; // the transmit power to use for data transmissions - uint8_t tx_power; + uint8_t tx_power; // The RF datarate lookup index. - uint8_t datarate; + uint8_t datarate; // The radio state machine state enum pios_radio_state state; @@ -746,13 +746,13 @@ struct pios_rfm22b_dev { enum pios_rfm22b_state rfm22b_state; // The packet statistics - struct rfm22b_stats stats; + struct rfm22b_stats stats; // Stats uint16_t errors; // RSSI in dBm - int8_t rssi_dBm; + int8_t rssi_dBm; // The tx data packet PHPacket data_packet; @@ -777,7 +777,7 @@ struct pios_rfm22b_dev { uint16_t rx_packet_len; // The status packet - PHStatusPacket status_packet; + PHStatusPacket status_packet; // The ACK/NACK packet PHAckNackPacket ack_nack_packet; @@ -791,21 +791,21 @@ struct pios_rfm22b_dev { PHConnectionPacket con_packet; // Send flags - bool send_status; - bool send_ppm; - bool send_connection_request; + bool send_status; + bool send_ppm; + bool send_connection_request; // The initial frequency - uint32_t init_frequency; + uint32_t init_frequency; // The number of frequency hopping channels. - uint16_t num_channels; + uint16_t num_channels; // The frequency hopping step size - float frequency_step_size; + float frequency_step_size; // current frequency hop channel - uint8_t frequency_hop_channel; + uint8_t frequency_hop_channel; // afc correction reading (in Hz) - int8_t afc_correction_Hz; + int8_t afc_correction_Hz; // The packet timers. portTickType packet_start_ticks; @@ -814,16 +814,16 @@ struct pios_rfm22b_dev { portTickType time_delta; // The maximum time (ms) that it should take to transmit / receive a packet. - uint32_t max_packet_time; + uint32_t max_packet_time; // The maximum time to wait for an ACK. - uint8_t max_ack_delay; + uint8_t max_ack_delay; #ifdef PIOS_INCLUDE_RFM22B_RCVR // The PPM channel values uint16_t ppm_channel[PIOS_RFM22B_RCVR_MAX_CHANNELS]; uint32_t ppm_supv_timer; - bool ppm_fresh; + bool ppm_fresh; #endif }; diff --git a/flight/pios/inc/pios_rtc.h b/flight/pios/inc/pios_rtc.h index 2e01912b9..196704927 100644 --- a/flight/pios/inc/pios_rtc.h +++ b/flight/pios/inc/pios_rtc.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_RTC RTC Functions * @{ * - * @file pios_rtc.h + * @file pios_rtc.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RTC functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/inc/pios_rtc_priv.h b/flight/pios/inc/pios_rtc_priv.h index 0a8c1c9b1..53c8a4024 100644 --- a/flight/pios/inc/pios_rtc_priv.h +++ b/flight/pios/inc/pios_rtc_priv.h @@ -35,12 +35,12 @@ #include struct pios_rtc_cfg { - uint32_t clksrc; - uint32_t prescaler; - struct stm32_irq irq; + uint32_t clksrc; + uint32_t prescaler; + struct stm32_irq irq; }; -extern void PIOS_RTC_Init(const struct pios_rtc_cfg * cfg); +extern void PIOS_RTC_Init(const struct pios_rtc_cfg *cfg); extern void PIOS_RTC_irq_handler(void); #endif /* PIOS_RTC_PRIV_H */ @@ -49,4 +49,3 @@ extern void PIOS_RTC_irq_handler(void); * @} * @} */ - diff --git a/flight/pios/inc/pios_sbus.h b/flight/pios/inc/pios_sbus.h index fd8580f2e..0c4dd6bc3 100644 --- a/flight/pios/inc/pios_sbus.h +++ b/flight/pios/inc/pios_sbus.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_SBus Futaba S.Bus receiver functions * @{ * - * @file pios_sbus.h + * @file pios_sbus.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. * @brief Futaba S.Bus functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/inc/pios_sbus_priv.h b/flight/pios/inc/pios_sbus_priv.h index b9aa88515..61a435e16 100644 --- a/flight/pios/inc/pios_sbus_priv.h +++ b/flight/pios/inc/pios_sbus_priv.h @@ -51,43 +51,43 @@ * 0xf0 - reserved * 1 byte - 0x00 (end of frame byte) */ -#define SBUS_FRAME_LENGTH (1+22+1+1) -#define SBUS_SOF_BYTE 0x0f -#define SBUS_EOF_BYTE 0x00 -#define SBUS_FLAG_DC1 0x01 -#define SBUS_FLAG_DC2 0x02 -#define SBUS_FLAG_FL 0x04 -#define SBUS_FLAG_FS 0x08 +#define SBUS_FRAME_LENGTH (1 + 22 + 1 + 1) +#define SBUS_SOF_BYTE 0x0f +#define SBUS_EOF_BYTE 0x00 +#define SBUS_FLAG_DC1 0x01 +#define SBUS_FLAG_DC2 0x02 +#define SBUS_FLAG_FL 0x04 +#define SBUS_FLAG_FS 0x08 /* * S.Bus protocol provides 16 proportional and 2 discrete channels. * Do not change unless driver code is updated accordingly. */ -#if (PIOS_SBUS_NUM_INPUTS != (16+2)) +#if (PIOS_SBUS_NUM_INPUTS != (16 + 2)) #error "S.Bus protocol provides 16 proportional and 2 discrete channels" #endif /* Discrete channels represented as bits, provide values for them */ -#define SBUS_VALUE_MIN 352 -#define SBUS_VALUE_MAX 1696 +#define SBUS_VALUE_MIN 352 +#define SBUS_VALUE_MAX 1696 /* * S.Bus configuration programmable invertor */ struct pios_sbus_cfg { - struct stm32_gpio inv; - void (*gpio_clk_func)(uint32_t periph, FunctionalState state); - uint32_t gpio_clk_periph; - BitAction gpio_inv_enable; - BitAction gpio_inv_disable; + struct stm32_gpio inv; + void (*gpio_clk_func)(uint32_t periph, FunctionalState state); + uint32_t gpio_clk_periph; + BitAction gpio_inv_enable; + BitAction gpio_inv_disable; }; extern const struct pios_rcvr_driver pios_sbus_rcvr_driver; extern int32_t PIOS_SBus_Init(uint32_t *sbus_id, - const struct pios_sbus_cfg *cfg, - const struct pios_com_driver *driver, - uint32_t lower_id); + const struct pios_sbus_cfg *cfg, + const struct pios_com_driver *driver, + uint32_t lower_id); #endif /* PIOS_SBUS_PRIV_H */ diff --git a/flight/pios/inc/pios_sdcard.h b/flight/pios/inc/pios_sdcard.h index df55d83f8..c428c8fc0 100644 --- a/flight/pios/inc/pios_sdcard.h +++ b/flight/pios/inc/pios_sdcard.h @@ -4,27 +4,27 @@ * @{ * @addtogroup PIOS_SDCARD SDCard Functions * @{ - * - * @file pios_sdcard.h + * + * @file pios_sdcard.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief System and hardware Init functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,56 +33,56 @@ /* Public Functions */ typedef struct { - uint8_t CSDStruct; /* CSD structure */ - uint8_t SysSpecVersion; /* System specification version */ - uint8_t Reserved1; /* Reserved */ - uint8_t TAAC; /* Data read access-time 1 */ - uint8_t NSAC; /* Data read access-time 2 in CLK cycles */ - uint8_t MaxBusClkFrec; /* Max. bus clock frequency */ - uint16_t CardComdClasses; /* Card command classes */ - uint8_t RdBlockLen; /* Max. read data block length */ - uint8_t PartBlockRead; /* Partial blocks for read allowed */ - uint8_t WrBlockMisalign; /* Write block misalignment */ - uint8_t RdBlockMisalign; /* Read block misalignment */ - uint8_t DSRImpl; /* DSR implemented */ - uint8_t Reserved2; /* Reserved */ - uint16_t DeviceSize; /* Device Size */ - uint8_t MaxRdCurrentVDDMin; /* Max. read current @ VDD min */ - uint8_t MaxRdCurrentVDDMax; /* Max. read current @ VDD max */ - uint8_t MaxWrCurrentVDDMin; /* Max. write current @ VDD min */ - uint8_t MaxWrCurrentVDDMax; /* Max. write current @ VDD max */ - uint8_t DeviceSizeMul; /* Device size multiplier */ - uint8_t EraseGrSize; /* Erase group size */ - uint8_t EraseGrMul; /* Erase group size multiplier */ - uint8_t WrProtectGrSize; /* Write protect group size */ - uint8_t WrProtectGrEnable; /* Write protect group enable */ - uint8_t ManDeflECC; /* Manufacturer default ECC */ - uint8_t WrSpeedFact; /* Write speed factor */ - uint8_t MaxWrBlockLen; /* Max. write data block length */ - uint8_t WriteBlockPaPartial; /* Partial blocks for write allowed */ - uint8_t Reserved3; /* Reserved */ - uint8_t ContentProtectAppli; /* Content protection application */ - uint8_t FileFormatGrouop; /* File format group */ - uint8_t CopyFlag; /* Copy flag (OTP) */ - uint8_t PermWrProtect; /* Permanent write protection */ - uint8_t TempWrProtect; /* Temporary write protection */ - uint8_t FileFormat; /* File Format */ - uint8_t ECC; /* ECC code */ - uint8_t msd_CRC; /* CRC */ - uint8_t Reserved4; /* always 1 */ + uint8_t CSDStruct; /* CSD structure */ + uint8_t SysSpecVersion; /* System specification version */ + uint8_t Reserved1; /* Reserved */ + uint8_t TAAC; /* Data read access-time 1 */ + uint8_t NSAC; /* Data read access-time 2 in CLK cycles */ + uint8_t MaxBusClkFrec; /* Max. bus clock frequency */ + uint16_t CardComdClasses; /* Card command classes */ + uint8_t RdBlockLen; /* Max. read data block length */ + uint8_t PartBlockRead; /* Partial blocks for read allowed */ + uint8_t WrBlockMisalign; /* Write block misalignment */ + uint8_t RdBlockMisalign; /* Read block misalignment */ + uint8_t DSRImpl; /* DSR implemented */ + uint8_t Reserved2; /* Reserved */ + uint16_t DeviceSize; /* Device Size */ + uint8_t MaxRdCurrentVDDMin; /* Max. read current @ VDD min */ + uint8_t MaxRdCurrentVDDMax; /* Max. read current @ VDD max */ + uint8_t MaxWrCurrentVDDMin; /* Max. write current @ VDD min */ + uint8_t MaxWrCurrentVDDMax; /* Max. write current @ VDD max */ + uint8_t DeviceSizeMul; /* Device size multiplier */ + uint8_t EraseGrSize; /* Erase group size */ + uint8_t EraseGrMul; /* Erase group size multiplier */ + uint8_t WrProtectGrSize; /* Write protect group size */ + uint8_t WrProtectGrEnable; /* Write protect group enable */ + uint8_t ManDeflECC; /* Manufacturer default ECC */ + uint8_t WrSpeedFact; /* Write speed factor */ + uint8_t MaxWrBlockLen; /* Max. write data block length */ + uint8_t WriteBlockPaPartial; /* Partial blocks for write allowed */ + uint8_t Reserved3; /* Reserved */ + uint8_t ContentProtectAppli; /* Content protection application */ + uint8_t FileFormatGrouop; /* File format group */ + uint8_t CopyFlag; /* Copy flag (OTP) */ + uint8_t PermWrProtect; /* Permanent write protection */ + uint8_t TempWrProtect; /* Temporary write protection */ + uint8_t FileFormat; /* File Format */ + uint8_t ECC; /* ECC code */ + uint8_t msd_CRC; /* CRC */ + uint8_t Reserved4; /* always 1 */ } SDCARDCsdTypeDef; /* Structure taken from Mass Storage Driver example provided by STM */ typedef struct { - uint8_t ManufacturerID; /* ManufacturerID */ - uint16_t OEM_AppliID; /* OEM/Application ID */ - char ProdName[6]; /* Product Name */ - uint8_t ProdRev; /* Product Revision */ - uint32_t ProdSN; /* Product Serial Number */ - uint8_t Reserved1; /* Reserved1 */ - uint16_t ManufactDate; /* Manufacturing Date */ - uint8_t msd_CRC; /* CRC */ - uint8_t Reserved2; /* always 1 */ + uint8_t ManufacturerID; /* ManufacturerID */ + uint16_t OEM_AppliID; /* OEM/Application ID */ + char ProdName[6]; /* Product Name */ + uint8_t ProdRev; /* Product Revision */ + uint32_t ProdSN; /* Product Serial Number */ + uint8_t Reserved1; /* Reserved1 */ + uint16_t ManufactDate; /* Manufacturing Date */ + uint8_t msd_CRC; /* CRC */ + uint8_t Reserved2; /* always 1 */ } SDCARDCidTypeDef; #ifndef USE_SIM_POSIX /* Global Variables */ @@ -95,18 +95,18 @@ extern int32_t PIOS_SDCARD_PowerOn(void); extern int32_t PIOS_SDCARD_PowerOff(void); extern int32_t PIOS_SDCARD_CheckAvailable(uint8_t was_available); extern int32_t PIOS_SDCARD_SendSDCCmd(uint8_t cmd, uint32_t addr, uint8_t crc); -extern int32_t PIOS_SDCARD_SectorRead(uint32_t sector, uint8_t * buffer); -extern int32_t PIOS_SDCARD_SectorWrite(uint32_t sector, uint8_t * buffer); -extern int32_t PIOS_SDCARD_CIDRead(SDCARDCidTypeDef * cid); -extern int32_t PIOS_SDCARD_CSDRead(SDCARDCsdTypeDef * csd); +extern int32_t PIOS_SDCARD_SectorRead(uint32_t sector, uint8_t *buffer); +extern int32_t PIOS_SDCARD_SectorWrite(uint32_t sector, uint8_t *buffer); +extern int32_t PIOS_SDCARD_CIDRead(SDCARDCidTypeDef *cid); +extern int32_t PIOS_SDCARD_CSDRead(SDCARDCsdTypeDef *csd); extern int32_t PIOS_SDCARD_StartupLog(void); extern int32_t PIOS_SDCARD_IsMounted(); extern int32_t PIOS_SDCARD_MountFS(uint32_t StartupLog); extern int32_t PIOS_SDCARD_GetFree(void); #ifndef USE_SIM_POSIX -extern int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t * buffer, uint32_t len); -extern int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t * buffer, uint32_t max_len); +extern int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len); +extern int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len); #endif extern int32_t PIOS_SDCARD_FileCopy(char *Source, char *Destination); extern int32_t PIOS_SDCARD_FileDelete(char *Filename); diff --git a/flight/pios/inc/pios_servo.h b/flight/pios/inc/pios_servo.h index 1a33149ea..fd905edbe 100644 --- a/flight/pios/inc/pios_servo.h +++ b/flight/pios/inc/pios_servo.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_SERVO RC Servo Functions * @{ * - * @file pios_servo.h + * @file pios_servo.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RC Servo functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,12 +31,12 @@ #define PIOS_SERVO_H /* Public Functions */ -extern void PIOS_Servo_SetHz(const uint16_t * update_rates, uint8_t banks); +extern void PIOS_Servo_SetHz(const uint16_t *update_rates, uint8_t banks); extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position); #endif /* PIOS_SERVO_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_servo_priv.h b/flight/pios/inc/pios_servo_priv.h index f5ed86e25..be692cc26 100644 --- a/flight/pios/inc/pios_servo_priv.h +++ b/flight/pios/inc/pios_servo_priv.h @@ -36,15 +36,15 @@ #include struct pios_servo_cfg { - TIM_TimeBaseInitTypeDef tim_base_init; - TIM_OCInitTypeDef tim_oc_init; - GPIO_InitTypeDef gpio_init; - uint32_t remap; - const struct pios_tim_channel * channels; - uint8_t num_channels; + TIM_TimeBaseInitTypeDef tim_base_init; + TIM_OCInitTypeDef tim_oc_init; + GPIO_InitTypeDef gpio_init; + uint32_t remap; + const struct pios_tim_channel *channels; + uint8_t num_channels; }; -extern int32_t PIOS_Servo_Init(const struct pios_servo_cfg * cfg); +extern int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg); #endif /* PIOS_SERVO_PRIV_H */ diff --git a/flight/pios/inc/pios_spi.h b/flight/pios/inc/pios_spi.h index 5740e1e67..93218d667 100644 --- a/flight/pios/inc/pios_spi.h +++ b/flight/pios/inc/pios_spi.h @@ -32,14 +32,14 @@ /* Global types */ typedef enum { - PIOS_SPI_PRESCALER_2 = 0, - PIOS_SPI_PRESCALER_4 = 1, - PIOS_SPI_PRESCALER_8 = 2, - PIOS_SPI_PRESCALER_16 = 3, - PIOS_SPI_PRESCALER_32 = 4, - PIOS_SPI_PRESCALER_64 = 5, - PIOS_SPI_PRESCALER_128 = 6, - PIOS_SPI_PRESCALER_256 = 7 + PIOS_SPI_PRESCALER_2 = 0, + PIOS_SPI_PRESCALER_4 = 1, + PIOS_SPI_PRESCALER_8 = 2, + PIOS_SPI_PRESCALER_16 = 3, + PIOS_SPI_PRESCALER_32 = 4, + PIOS_SPI_PRESCALER_64 = 5, + PIOS_SPI_PRESCALER_128 = 6, + PIOS_SPI_PRESCALER_256 = 7 } SPIPrescalerTypeDef; /* Public Functions */ diff --git a/flight/pios/inc/pios_spi_priv.h b/flight/pios/inc/pios_spi_priv.h index f89f52495..0a7f6a3da 100644 --- a/flight/pios/inc/pios_spi_priv.h +++ b/flight/pios/inc/pios_spi_priv.h @@ -35,35 +35,35 @@ #include struct pios_spi_cfg { - SPI_TypeDef *regs; - uint32_t remap; /* GPIO_Remap_* or GPIO_AF_* */ - SPI_InitTypeDef init; - bool use_crc; - struct stm32_dma dma; - struct stm32_gpio sclk; - struct stm32_gpio miso; - struct stm32_gpio mosi; - uint32_t slave_count; - struct stm32_gpio ssel[]; + SPI_TypeDef *regs; + uint32_t remap; /* GPIO_Remap_* or GPIO_AF_* */ + SPI_InitTypeDef init; + bool use_crc; + struct stm32_dma dma; + struct stm32_gpio sclk; + struct stm32_gpio miso; + struct stm32_gpio mosi; + uint32_t slave_count; + struct stm32_gpio ssel[]; }; struct pios_spi_dev { - const struct pios_spi_cfg * cfg; - void (*callback) (uint8_t, uint8_t); - uint8_t tx_dummy_byte; - uint8_t rx_dummy_byte; + const struct pios_spi_cfg *cfg; + void (*callback)(uint8_t, uint8_t); + uint8_t tx_dummy_byte; + uint8_t rx_dummy_byte; #if defined(PIOS_INCLUDE_FREERTOS) - xSemaphoreHandle busy; + xSemaphoreHandle busy; #else - uint8_t busy; + uint8_t busy; #endif }; -extern int32_t PIOS_SPI_Init(uint32_t * spi_id, const struct pios_spi_cfg * cfg); +extern int32_t PIOS_SPI_Init(uint32_t *spi_id, const struct pios_spi_cfg *cfg); #endif /* PIOS_SPI_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_stm32.h b/flight/pios/inc/pios_stm32.h index f9ca892e2..01aefe059 100644 --- a/flight/pios/inc/pios_stm32.h +++ b/flight/pios/inc/pios_stm32.h @@ -3,7 +3,7 @@ * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ * @addtogroup PIOS_STM32 STM32 HAL - * @brief STM32 specific global data structures + * @brief STM32 specific global data structures * @{ * * @file pios_stm32.h @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,40 +32,40 @@ #define PIOS_STM32_H struct stm32_irq { - void (*handler) (uint32_t); - uint32_t flags; - NVIC_InitTypeDef init; + void (*handler)(uint32_t); + uint32_t flags; + NVIC_InitTypeDef init; }; struct stm32_exti { - EXTI_InitTypeDef init; + EXTI_InitTypeDef init; }; struct stm32_dma_chan { #if defined(STM32F2XX) || defined(STM32F4XX) - DMA_Stream_TypeDef *channel; + DMA_Stream_TypeDef *channel; #else - DMA_Channel_TypeDef *channel; + DMA_Channel_TypeDef *channel; #endif - DMA_InitTypeDef init; + DMA_InitTypeDef init; }; struct stm32_dma { - uint32_t ahb_clk; /* ignored on STM32F2XX */ - struct stm32_irq irq; - struct stm32_dma_chan rx; - struct stm32_dma_chan tx; + uint32_t ahb_clk; /* ignored on STM32F2XX */ + struct stm32_irq irq; + struct stm32_dma_chan rx; + struct stm32_dma_chan tx; }; struct stm32_gpio { - GPIO_TypeDef *gpio; - GPIO_InitTypeDef init; - uint8_t pin_source; + GPIO_TypeDef *gpio; + GPIO_InitTypeDef init; + uint8_t pin_source; }; /** - * @} - * @} - */ + * @} + * @} + */ #endif /* PIOS_STM32_H */ diff --git a/flight/pios/inc/pios_struct_helper.h b/flight/pios/inc/pios_struct_helper.h index f061a6e01..1198b6fb1 100644 --- a/flight/pios/inc/pios_struct_helper.h +++ b/flight/pios/inc/pios_struct_helper.h @@ -11,8 +11,10 @@ #ifndef PIOS_STRUCT_HELPER_H #define PIOS_STRUCT_HELPER_H -#define container_of(ptr, type, member) ({ \ - const typeof( ((type *)0)->member ) *__mptr = (ptr); \ - (type *)( (char *)__mptr - offsetof(type,member) );}) +#define container_of(ptr, type, member) \ + ({ \ + const typeof(((type *)0)->member) * __mptr = (ptr); \ + (type *)((char *)__mptr - offsetof(type, member)); } \ + ) #endif /* PIOS_STRUCT_HELPER_H */ diff --git a/flight/pios/inc/pios_sys.h b/flight/pios/inc/pios_sys.h index 7f183a892..a67411db6 100644 --- a/flight/pios/inc/pios_sys.h +++ b/flight/pios/inc/pios_sys.h @@ -6,26 +6,26 @@ * @brief PIOS System Initialization code * @{ * - * @file pios_sys.h + * @file pios_sys.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief System and hardware Init functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,18 +33,18 @@ #define PIOS_SYS_H #define PIOS_SYS_SERIAL_NUM_BINARY_LEN 12 -#define PIOS_SYS_SERIAL_NUM_ASCII_LEN (PIOS_SYS_SERIAL_NUM_BINARY_LEN * 2) +#define PIOS_SYS_SERIAL_NUM_ASCII_LEN (PIOS_SYS_SERIAL_NUM_BINARY_LEN * 2) /* Public Functions */ extern void PIOS_SYS_Init(void); extern int32_t PIOS_SYS_Reset(void); extern uint32_t PIOS_SYS_getCPUFlashSize(void); extern int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t array[PIOS_SYS_SERIAL_NUM_BINARY_LEN]); -extern int32_t PIOS_SYS_SerialNumberGet(char str[PIOS_SYS_SERIAL_NUM_ASCII_LEN+1]); +extern int32_t PIOS_SYS_SerialNumberGet(char str[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]); #endif /* PIOS_SYS_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_task_monitor.h b/flight/pios/inc/pios_task_monitor.h index e969af3d8..ed3d1d519 100644 --- a/flight/pios/inc/pios_task_monitor.h +++ b/flight/pios/inc/pios_task_monitor.h @@ -71,22 +71,22 @@ extern int32_t PIOS_TASK_MONITOR_UnregisterTask(uint16_t task_id); */ extern bool PIOS_TASK_MONITOR_IsRunning(uint16_t task_id); -/** +/** * Information about a running task that has been registered * via a call to PIOS_TASK_MONITOR_Add(). */ struct pios_task_info { - /** Remaining task stack in bytes. */ - uint32_t stack_remaining; - /** Flag indicating whether or not the task is running. */ - bool is_running; - /** Percentage of cpu time used by the task since the last call - * to PIOS_TASK_MONITOR_ForEachTask(). Low-load tasks may - * report 0% load even though they have run during the interval. */ - uint8_t running_time_percentage; + /** Remaining task stack in bytes. */ + uint32_t stack_remaining; + /** Flag indicating whether or not the task is running. */ + bool is_running; + /** Percentage of cpu time used by the task since the last call + * to PIOS_TASK_MONITOR_ForEachTask(). Low-load tasks may + * report 0% load even though they have run during the interval. */ + uint8_t running_time_percentage; }; -/** +/** * Iterator callback, called for each monitored task by PIOS_TASK_MONITOR_TasksIterate(). * * @param task_id The id of the task the task_info refers to. diff --git a/flight/pios/inc/pios_tim.h b/flight/pios/inc/pios_tim.h index 0bad07f41..3ff1e5feb 100644 --- a/flight/pios/inc/pios_tim.h +++ b/flight/pios/inc/pios_tim.h @@ -1,4 +1,4 @@ #ifndef PIOS_TIM_H #define PIOS_TIM_H -#endif /* PIOS_TIM_H */ +#endif /* PIOS_TIM_H */ diff --git a/flight/pios/inc/pios_tim_priv.h b/flight/pios/inc/pios_tim_priv.h index 7f05719f8..e607f7033 100644 --- a/flight/pios/inc/pios_tim_priv.h +++ b/flight/pios/inc/pios_tim_priv.h @@ -4,25 +4,25 @@ #include struct pios_tim_clock_cfg { - TIM_TypeDef * timer; - const TIM_TimeBaseInitTypeDef * time_base_init; - struct stm32_irq irq; + TIM_TypeDef *timer; + const TIM_TimeBaseInitTypeDef *time_base_init; + struct stm32_irq irq; }; struct pios_tim_channel { - TIM_TypeDef * timer; - uint8_t timer_chan; + TIM_TypeDef *timer; + uint8_t timer_chan; - struct stm32_gpio pin; - uint32_t remap; + struct stm32_gpio pin; + uint32_t remap; }; struct pios_tim_callbacks { - void (*overflow)(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count); - void (*edge)(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count); + void (*overflow)(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count); + void (*edge)(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count); }; -extern int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg * cfg); -extern int32_t PIOS_TIM_InitChannels(uint32_t * tim_id, const struct pios_tim_channel * channels, uint8_t num_channels, const struct pios_tim_callbacks * callbacks, uint32_t context); +extern int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg *cfg); +extern int32_t PIOS_TIM_InitChannels(uint32_t *tim_id, const struct pios_tim_channel *channels, uint8_t num_channels, const struct pios_tim_callbacks *callbacks, uint32_t context); -#endif /* PIOS_TIM_PRIV_H */ +#endif /* PIOS_TIM_PRIV_H */ diff --git a/flight/pios/inc/pios_udp.h b/flight/pios/inc/pios_udp.h index 48a3b0987..28a095dce 100644 --- a/flight/pios/inc/pios_udp.h +++ b/flight/pios/inc/pios_udp.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_usart.h + * @file pios_usart.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief UDP functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/inc/pios_udp_priv.h b/flight/pios/inc/pios_udp_priv.h index 5ce32a01a..4105d9189 100644 --- a/flight/pios/inc/pios_udp_priv.h +++ b/flight/pios/inc/pios_udp_priv.h @@ -3,7 +3,7 @@ * * @file pios_udp_priv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief UDP private definitions. * @see The GNU Public License (GPL) Version 3 * @@ -39,36 +39,36 @@ #include struct pios_udp_cfg { - const char * ip; - uint16_t port; + const char *ip; + uint16_t port; }; typedef struct { - const struct pios_udp_cfg * cfg; + const struct pios_udp_cfg *cfg; #if defined(PIOS_INCLUDE_FREERTOS) - xTaskHandle rxThread; + xTaskHandle rxThread; #else - pthread_t rxThread; + pthread_t rxThread; #endif - int socket; - struct sockaddr_in server; - struct sockaddr_in client; - uint32_t clientLength; + int socket; + struct sockaddr_in server; + struct sockaddr_in client; + uint32_t clientLength; - pthread_cond_t cond; - pthread_mutex_t mutex; + pthread_cond_t cond; + pthread_mutex_t mutex; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; - uint8_t rx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; - uint8_t tx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; + uint8_t rx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; + uint8_t tx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; } pios_udp_dev; -extern int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg); +extern int32_t PIOS_UDP_Init(uint32_t *udp_id, const struct pios_udp_cfg *cfg); extern const struct pios_com_driver pios_udp_com_driver; diff --git a/flight/pios/inc/pios_usart.h b/flight/pios/inc/pios_usart.h index dd98a660f..379223a6f 100644 --- a/flight/pios/inc/pios_usart.h +++ b/flight/pios/inc/pios_usart.h @@ -6,25 +6,25 @@ * @brief PIOS interface for USART port * @{ * - * @file pios_usart.h + * @file pios_usart.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief USART functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,6 +37,6 @@ #endif /* PIOS_USART_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usart_priv.h b/flight/pios/inc/pios_usart_priv.h index defe3bb8a..52394e5f4 100644 --- a/flight/pios/inc/pios_usart_priv.h +++ b/flight/pios/inc/pios_usart_priv.h @@ -39,20 +39,20 @@ extern const struct pios_com_driver pios_usart_com_driver; struct pios_usart_cfg { - USART_TypeDef *regs; - uint32_t remap; /* GPIO_Remap_* */ - USART_InitTypeDef init; - struct stm32_gpio rx; - struct stm32_gpio tx; - struct stm32_irq irq; + USART_TypeDef *regs; + uint32_t remap; /* GPIO_Remap_* */ + USART_InitTypeDef init; + struct stm32_gpio rx; + struct stm32_gpio tx; + struct stm32_irq irq; }; -extern int32_t PIOS_USART_Init(uint32_t * usart_id, const struct pios_usart_cfg * cfg); -extern const struct pios_usart_cfg * PIOS_USART_GetConfig(uint32_t usart_id); +extern int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg); +extern const struct pios_usart_cfg *PIOS_USART_GetConfig(uint32_t usart_id); #endif /* PIOS_USART_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb.h b/flight/pios/inc/pios_usb.h index 24d1a0e3e..554f6b817 100644 --- a/flight/pios/inc/pios_usb.h +++ b/flight/pios/inc/pios_usb.h @@ -41,6 +41,6 @@ extern bool PIOS_USB_CheckAvailable(uint32_t id); #endif /* PIOS_USB_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_board_data_priv.h b/flight/pios/inc/pios_usb_board_data_priv.h index f4e7f7c05..9ee149ed1 100644 --- a/flight/pios/inc/pios_usb_board_data_priv.h +++ b/flight/pios/inc/pios_usb_board_data_priv.h @@ -11,19 +11,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,6 +35,6 @@ extern int32_t PIOS_USB_BOARD_DATA_Init(void); #endif /* PIOS_USB_BOARD_DATA_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_cdc_priv.h b/flight/pios/inc/pios_usb_cdc_priv.h index b87bd50cc..513303a47 100644 --- a/flight/pios/inc/pios_usb_cdc_priv.h +++ b/flight/pios/inc/pios_usb_cdc_priv.h @@ -32,21 +32,21 @@ #define PIOS_USB_CDC_PRIV_H struct pios_usb_cdc_cfg { - uint8_t ctrl_if; - uint8_t ctrl_tx_ep; + uint8_t ctrl_if; + uint8_t ctrl_tx_ep; - uint8_t data_if; - uint8_t data_rx_ep; - uint8_t data_tx_ep; + uint8_t data_if; + uint8_t data_rx_ep; + uint8_t data_tx_ep; }; extern const struct pios_com_driver pios_usb_cdc_com_driver; -extern int32_t PIOS_USB_CDC_Init(uint32_t * usbcdc_id, const struct pios_usb_cdc_cfg * cfg, uint32_t lower_id); +extern int32_t PIOS_USB_CDC_Init(uint32_t *usbcdc_id, const struct pios_usb_cdc_cfg *cfg, uint32_t lower_id); #endif /* PIOS_USB_CDC_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_defs.h b/flight/pios/inc/pios_usb_defs.h index 925e287ec..164d31fb3 100644 --- a/flight/pios/inc/pios_usb_defs.h +++ b/flight/pios/inc/pios_usb_defs.h @@ -31,62 +31,64 @@ #ifndef PIOS_USB_DEFS_H #define PIOS_USB_DEFS_H -#include /* uint*_t */ +#include /* uint*_t */ enum usb_desc_types { - USB_DESC_TYPE_DEVICE = 0x01, - USB_DESC_TYPE_CONFIGURATION = 0x02, - USB_DESC_TYPE_STRING = 0x03, - USB_DESC_TYPE_INTERFACE = 0x04, - USB_DESC_TYPE_ENDPOINT = 0x05, - USB_DESC_TYPE_IAD = 0x0B, - USB_DESC_TYPE_HID = 0x21, - USB_DESC_TYPE_REPORT = 0x22, - USB_DESC_TYPE_CLASS_SPECIFIC = 0x24, + USB_DESC_TYPE_DEVICE = 0x01, + USB_DESC_TYPE_CONFIGURATION = 0x02, + USB_DESC_TYPE_STRING = 0x03, + USB_DESC_TYPE_INTERFACE = 0x04, + USB_DESC_TYPE_ENDPOINT = 0x05, + USB_DESC_TYPE_IAD = 0x0B, + USB_DESC_TYPE_HID = 0x21, + USB_DESC_TYPE_REPORT = 0x22, + USB_DESC_TYPE_CLASS_SPECIFIC = 0x24, } __attribute__((packed)); enum usb_interface_class { - USB_INTERFACE_CLASS_CDC = 0x02, - USB_INTERFACE_CLASS_HID = 0x03, - USB_INTERFACE_CLASS_DATA = 0x0A, + USB_INTERFACE_CLASS_CDC = 0x02, + USB_INTERFACE_CLASS_HID = 0x03, + USB_INTERFACE_CLASS_DATA = 0x0A, } __attribute__((packed)); enum usb_cdc_desc_subtypes { - USB_CDC_DESC_SUBTYPE_HEADER = 0x00, - USB_CDC_DESC_SUBTYPE_CALLMGMT = 0x01, - USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL = 0x02, - USB_CDC_DESC_SUBTYPE_UNION = 0x06, + USB_CDC_DESC_SUBTYPE_HEADER = 0x00, + USB_CDC_DESC_SUBTYPE_CALLMGMT = 0x01, + USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL = 0x02, + USB_CDC_DESC_SUBTYPE_UNION = 0x06, } __attribute__((packed)); enum usb_ep_attr { - USB_EP_ATTR_TT_CONTROL = 0x00, - USB_EP_ATTR_TT_ISOCHRONOUS = 0x01, - USB_EP_ATTR_TT_BULK = 0x02, - USB_EP_ATTR_TT_INTERRUPT = 0x03, + USB_EP_ATTR_TT_CONTROL = 0x00, + USB_EP_ATTR_TT_ISOCHRONOUS = 0x01, + USB_EP_ATTR_TT_BULK = 0x02, + USB_EP_ATTR_TT_INTERRUPT = 0x03, } __attribute__((packed)); /* Standard macros to convert from host endian to USB endian (ie. little endian) */ #if __BIG_ENDIAN__ -#define htousbs(v) ((uint16_t)(\ - ((((v) >> 0) & 0xFF) << 8) | \ - ((((v) >> 8) & 0xFF) << 0))) -#define htousbl(v) ((uint32_t)(\ - ((((v) >> 0) & 0xFF) << 24) | \ - ((((v) >> 8) & 0xFF) << 16) | \ - ((((v) >> 16) & 0xFF) << 8) | \ - ((((v) >> 24) & 0xFF) << 0))) +#define htousbs(v) \ + ((uint16_t)( \ + ((((v) >> 0) & 0xFF) << 8) | \ + ((((v) >> 8) & 0xFF) << 0))) +#define htousbl(v) \ + ((uint32_t)( \ + ((((v) >> 0) & 0xFF) << 24) | \ + ((((v) >> 8) & 0xFF) << 16) | \ + ((((v) >> 16) & 0xFF) << 8) | \ + ((((v) >> 24) & 0xFF) << 0))) #else -#define htousbs(v) (v) -#define htousbl(v) (v) +#define htousbs(v) (v) +#define htousbl(v) (v) #endif -#define USB_EP_IN(ep) ((uint8_t) (0x80 | ((ep) & 0xF))) -#define USB_EP_OUT(ep) ((uint8_t) (0x00 | ((ep) & 0xF))) +#define USB_EP_IN(ep) ((uint8_t)(0x80 | ((ep) & 0xF))) +#define USB_EP_OUT(ep) ((uint8_t)(0x00 | ((ep) & 0xF))) -#define HID_ITEM_TYPE_MAIN 0x0 -#define HID_ITEM_TYPE_GLOBAL 0x1 -#define HID_ITEM_TYPE_LOCAL 0x2 -#define HID_ITEM_TYPE_RSVD 0x3 +#define HID_ITEM_TYPE_MAIN 0x0 +#define HID_ITEM_TYPE_GLOBAL 0x1 +#define HID_ITEM_TYPE_LOCAL 0x2 +#define HID_ITEM_TYPE_RSVD 0x3 #define HID_TAG_GLOBAL_USAGE_PAGE 0x0 /* 0b0000 */ #define HID_TAG_GLOBAL_LOGICAL_MIN 0x1 /* 0b0001 */ @@ -121,225 +123,226 @@ enum usb_ep_attr { #define HID_TAG_RSVD 0xF /* 0b1111 */ -#define HID_ITEM_SIZE_0 0 -#define HID_ITEM_SIZE_1 1 -#define HID_ITEM_SIZE_2 2 -#define HID_ITEM_SIZE_4 3 /* Yes, 4 bytes is represented with a size field = 3 */ +#define HID_ITEM_SIZE_0 0 +#define HID_ITEM_SIZE_1 1 +#define HID_ITEM_SIZE_2 2 +#define HID_ITEM_SIZE_4 3 /* Yes, 4 bytes is represented with a size field = 3 */ -#define HID_SHORT_ITEM(tag,type,size) (\ - (((tag) & 0xF) << 4) | \ - (((type) & 0x3) << 2) | \ - (((size) & 0x3) << 0)) +#define HID_SHORT_ITEM(tag, type, size) \ + ( \ + (((tag) & 0xF) << 4) | \ + (((type) & 0x3) << 2) | \ + (((size) & 0x3) << 0)) /* Long items have a fixed prefix */ -#define HID_LONG_ITEM HID_SHORT_ITEM(HID_TAG_RSVD, HID_ITEM_TYPE_RSVD, HID_ITEM_SIZE_2) +#define HID_LONG_ITEM HID_SHORT_ITEM(HID_TAG_RSVD, HID_ITEM_TYPE_RSVD, HID_ITEM_SIZE_2) -#define HID_MAIN_ITEM_0(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_0) -#define HID_MAIN_ITEM_1(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_1) -#define HID_MAIN_ITEM_2(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_2) -#define HID_MAIN_ITEM_4(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_4) +#define HID_MAIN_ITEM_0(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_0) +#define HID_MAIN_ITEM_1(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_1) +#define HID_MAIN_ITEM_2(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_2) +#define HID_MAIN_ITEM_4(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_4) #define HID_GLOBAL_ITEM_0(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_GLOBAL, HID_ITEM_SIZE_0) #define HID_GLOBAL_ITEM_1(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_GLOBAL, HID_ITEM_SIZE_1) #define HID_GLOBAL_ITEM_2(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_GLOBAL, HID_ITEM_SIZE_2) #define HID_GLOBAL_ITEM_4(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_GLOBAL, HID_ITEM_SIZE_4) -#define HID_LOCAL_ITEM_0(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_0) -#define HID_LOCAL_ITEM_1(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_1) -#define HID_LOCAL_ITEM_2(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_2) -#define HID_LOCAL_ITEM_4(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_4) +#define HID_LOCAL_ITEM_0(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_0) +#define HID_LOCAL_ITEM_1(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_1) +#define HID_LOCAL_ITEM_2(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_2) +#define HID_LOCAL_ITEM_4(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_4) struct usb_device_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint16_t bcdUSB; - uint8_t bDeviceClass; - uint8_t bDeviceSubClass; - uint8_t bDeviceProtocol; - uint8_t bMaxPacketSize0; - uint16_t idVendor; - uint16_t idProduct; - uint16_t bcdDevice; - uint8_t iManufacturer; - uint8_t iProduct; - uint8_t iSerialNumber; - uint8_t bNumConfigurations; + uint8_t bLength; + uint8_t bDescriptorType; + uint16_t bcdUSB; + uint8_t bDeviceClass; + uint8_t bDeviceSubClass; + uint8_t bDeviceProtocol; + uint8_t bMaxPacketSize0; + uint16_t idVendor; + uint16_t idProduct; + uint16_t bcdDevice; + uint8_t iManufacturer; + uint8_t iProduct; + uint8_t iSerialNumber; + uint8_t bNumConfigurations; } __attribute__((packed)); struct usb_configuration_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint16_t wTotalLength; - uint8_t bNumInterfaces; - uint8_t bConfigurationValue; - uint8_t iConfiguration; - uint8_t bmAttributes; - uint8_t bMaxPower; + uint8_t bLength; + uint8_t bDescriptorType; + uint16_t wTotalLength; + uint8_t bNumInterfaces; + uint8_t bConfigurationValue; + uint8_t iConfiguration; + uint8_t bmAttributes; + uint8_t bMaxPower; } __attribute__((packed)); struct usb_interface_association_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bFirstInterface; - uint8_t bInterfaceCount; - uint8_t bFunctionClass; - uint8_t bFunctionSubClass; - uint8_t bFunctionProtocol; - uint8_t iInterface; + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bFirstInterface; + uint8_t bInterfaceCount; + uint8_t bFunctionClass; + uint8_t bFunctionSubClass; + uint8_t bFunctionProtocol; + uint8_t iInterface; } __attribute__((packed)); struct usb_interface_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bInterfaceNumber; - uint8_t bAlternateSetting; - uint8_t bNumEndpoints; - uint8_t bInterfaceClass; - uint8_t bInterfaceSubClass; - uint8_t nInterfaceProtocol; - uint8_t iInterface; + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bInterfaceNumber; + uint8_t bAlternateSetting; + uint8_t bNumEndpoints; + uint8_t bInterfaceClass; + uint8_t bInterfaceSubClass; + uint8_t nInterfaceProtocol; + uint8_t iInterface; } __attribute__((packed)); struct usb_hid_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint16_t bcdHID; - uint8_t bCountryCode; - uint8_t bNumDescriptors; - uint8_t bClassDescriptorType; - uint16_t wItemLength; + uint8_t bLength; + uint8_t bDescriptorType; + uint16_t bcdHID; + uint8_t bCountryCode; + uint8_t bNumDescriptors; + uint8_t bClassDescriptorType; + uint16_t wItemLength; } __attribute__((packed)); struct usb_endpoint_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bEndpointAddress; - uint8_t bmAttributes; - uint16_t wMaxPacketSize; - uint8_t bInterval; + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bEndpointAddress; + uint8_t bmAttributes; + uint16_t wMaxPacketSize; + uint8_t bInterval; } __attribute__((packed)); struct usb_setup_request { - uint8_t bmRequestType; - uint8_t bRequest; - uint16_t wValue; - uint16_t wIndex; - uint16_t wLength; + uint8_t bmRequestType; + uint8_t bRequest; + uint16_t wValue; + uint16_t wIndex; + uint16_t wLength; } __attribute__((packed)); -#define USB_REQ_TYPE_STANDARD 0x00 -#define USB_REQ_TYPE_CLASS 0x20 -#define USB_REQ_TYPE_VENDOR 0x40 -#define USB_REQ_TYPE_MASK 0x60 +#define USB_REQ_TYPE_STANDARD 0x00 +#define USB_REQ_TYPE_CLASS 0x20 +#define USB_REQ_TYPE_VENDOR 0x40 +#define USB_REQ_TYPE_MASK 0x60 -#define USB_REQ_RECIPIENT_DEVICE 0x00 -#define USB_REQ_RECIPIENT_INTERFACE 0x01 -#define USB_REQ_RECIPIENT_ENDPOINT 0x02 -#define USB_REQ_RECIPIENT_MASK 0x03 +#define USB_REQ_RECIPIENT_DEVICE 0x00 +#define USB_REQ_RECIPIENT_INTERFACE 0x01 +#define USB_REQ_RECIPIENT_ENDPOINT 0x02 +#define USB_REQ_RECIPIENT_MASK 0x03 enum usb_standard_requests { - USB_REQ_GET_STATUS = 0x00, - USB_REQ_CLEAR_FEATURE = 0x01, - /* what is 0x02? */ - USB_REQ_SET_FEATURE = 0x03, - /* what is 0x04? */ - USB_REQ_SET_ADDRESS = 0x05, - USB_REQ_GET_DESCRIPTOR = 0x06, - USB_REQ_SET_DESCRIPTOR = 0x07, - USB_REQ_GET_CONFIGURATION = 0x08, - USB_REQ_SET_CONFIGURATION = 0x09, - USB_REQ_GET_INTERFACE = 0x0A, - USB_REQ_SET_INTERFACE = 0x0B, - USB_REQ_SYNCH_FRAME = 0x0C, + USB_REQ_GET_STATUS = 0x00, + USB_REQ_CLEAR_FEATURE = 0x01, + /* what is 0x02? */ + USB_REQ_SET_FEATURE = 0x03, + /* what is 0x04? */ + USB_REQ_SET_ADDRESS = 0x05, + USB_REQ_GET_DESCRIPTOR = 0x06, + USB_REQ_SET_DESCRIPTOR = 0x07, + USB_REQ_GET_CONFIGURATION = 0x08, + USB_REQ_SET_CONFIGURATION = 0x09, + USB_REQ_GET_INTERFACE = 0x0A, + USB_REQ_SET_INTERFACE = 0x0B, + USB_REQ_SYNCH_FRAME = 0x0C, }; enum usb_hid_requests { - USB_HID_REQ_GET_REPORT = 0x01, - USB_HID_REQ_GET_IDLE = 0x02, - USB_HID_REQ_GET_PROTOCOL = 0x03, - /* 0x04-0x08 Reserved */ - USB_HID_REQ_SET_REPORT = 0x09, - USB_HID_REQ_SET_IDLE = 0x0A, - USB_HID_REQ_SET_PROTOCOL = 0x0B, + USB_HID_REQ_GET_REPORT = 0x01, + USB_HID_REQ_GET_IDLE = 0x02, + USB_HID_REQ_GET_PROTOCOL = 0x03, + /* 0x04-0x08 Reserved */ + USB_HID_REQ_SET_REPORT = 0x09, + USB_HID_REQ_SET_IDLE = 0x0A, + USB_HID_REQ_SET_PROTOCOL = 0x0B, }; enum usb_cdc_requests { - USB_CDC_REQ_SET_LINE_CODING = 0x20, - USB_CDC_REQ_GET_LINE_CODING = 0x21, + USB_CDC_REQ_SET_LINE_CODING = 0x20, + USB_CDC_REQ_GET_LINE_CODING = 0x21, - USB_CDC_REQ_SET_CONTROL_LINE_STATE = 0x22, + USB_CDC_REQ_SET_CONTROL_LINE_STATE = 0x22, }; struct usb_cdc_header_func_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bDescriptorSubType; - uint16_t bcdCDC; + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; + uint16_t bcdCDC; } __attribute__((packed)); struct usb_cdc_callmgmt_func_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bDescriptorSubType; - uint8_t bmCapabilities; - uint8_t bDataInterface; + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; + uint8_t bmCapabilities; + uint8_t bDataInterface; } __attribute__((packed)); struct usb_cdc_acm_func_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bDescriptorSubType; - uint8_t bmCapabilities; + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; + uint8_t bmCapabilities; } __attribute__((packed)); struct usb_cdc_union_func_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bDescriptorSubType; - uint8_t bMasterInterface; - uint8_t bSlaveInterface; + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; + uint8_t bMasterInterface; + uint8_t bSlaveInterface; } __attribute__((packed)); #define USB_LANGID_ENGLISH_US 0x0409 struct usb_string_langid { - uint8_t bLength; - uint8_t bDescriptorType; - uint16_t bLangID; + uint8_t bLength; + uint8_t bDescriptorType; + uint16_t bLangID; } __attribute__((packed)); struct usb_cdc_line_coding { - uint32_t dwDTERate; - uint8_t bCharFormat; - uint8_t bParityType; - uint8_t bDataBits; + uint32_t dwDTERate; + uint8_t bCharFormat; + uint8_t bParityType; + uint8_t bDataBits; } __attribute__((packed)); enum usb_cdc_line_coding_stop { - USB_CDC_LINE_CODING_STOP_1 = 0, - USB_CDC_LINE_CODING_STOP_1_5 = 1, - USB_CDC_LINE_CODING_STOP_2 = 2, + USB_CDC_LINE_CODING_STOP_1 = 0, + USB_CDC_LINE_CODING_STOP_1_5 = 1, + USB_CDC_LINE_CODING_STOP_2 = 2, } __attribute__((packed)); enum usb_cdc_line_coding_parity { - USB_CDC_LINE_CODING_PARITY_NONE = 0, - USB_CDC_LINE_CODING_PARITY_ODD = 1, - USB_CDC_LINE_CODING_PARITY_EVEN = 2, - USB_CDC_LINE_CODING_PARITY_MARK = 3, - USB_CDC_LINE_CODING_PARITY_SPACE = 4, + USB_CDC_LINE_CODING_PARITY_NONE = 0, + USB_CDC_LINE_CODING_PARITY_ODD = 1, + USB_CDC_LINE_CODING_PARITY_EVEN = 2, + USB_CDC_LINE_CODING_PARITY_MARK = 3, + USB_CDC_LINE_CODING_PARITY_SPACE = 4, } __attribute__((packed)); struct usb_cdc_serial_state_report { - uint8_t bmRequestType; - uint8_t bNotification; - uint16_t wValue; - uint16_t wIndex; - uint16_t wLength; - uint16_t bmUartState; + uint8_t bmRequestType; + uint8_t bNotification; + uint16_t wValue; + uint16_t wIndex; + uint16_t wLength; + uint16_t bmUartState; } __attribute__((packed)); enum usb_cdc_notification { - USB_CDC_NOTIFICATION_SERIAL_STATE = 0x20, + USB_CDC_NOTIFICATION_SERIAL_STATE = 0x20, } __attribute__((packed)); /* @@ -349,31 +352,32 @@ enum usb_cdc_notification { #define USB_VENDOR_ID_OPENPILOT 0x20A0 enum usb_product_ids { - USB_PRODUCT_ID_OPENPILOT_MAIN = 0x415A, - USB_PRODUCT_ID_COPTERCONTROL = 0x415B, - USB_PRODUCT_ID_OPLINK = 0x415C, - USB_PRODUCT_ID_CC3D = 0x415D, - USB_PRODUCT_ID_REVOLUTION = 0x415E, - USB_PRODUCT_ID_OSD = 0x4194, - USB_PRODUCT_ID_SPARE = 0x4195, + USB_PRODUCT_ID_OPENPILOT_MAIN = 0x415A, + USB_PRODUCT_ID_COPTERCONTROL = 0x415B, + USB_PRODUCT_ID_OPLINK = 0x415C, + USB_PRODUCT_ID_CC3D = 0x415D, + USB_PRODUCT_ID_REVOLUTION = 0x415E, + USB_PRODUCT_ID_OSD = 0x4194, + USB_PRODUCT_ID_SPARE = 0x4195, } __attribute__((packed)); enum usb_op_board_ids { - USB_OP_BOARD_ID_OPENPILOT_MAIN = 1, - /* Board ID 2 may be unused or AHRS */ - USB_OP_BOARD_ID_OPLINK = 3, - USB_OP_BOARD_ID_COPTERCONTROL = 4, - USB_OP_BOARD_ID_REVOLUTION = 5, - USB_OP_BOARD_ID_OSD = 6, + USB_OP_BOARD_ID_OPENPILOT_MAIN = 1, + /* Board ID 2 may be unused or AHRS */ + USB_OP_BOARD_ID_OPLINK = 3, + USB_OP_BOARD_ID_COPTERCONTROL = 4, + USB_OP_BOARD_ID_REVOLUTION = 5, + USB_OP_BOARD_ID_OSD = 6, } __attribute__((packed)); enum usb_op_board_modes { - USB_OP_BOARD_MODE_BL = 1, - USB_OP_BOARD_MODE_FW = 2, + USB_OP_BOARD_MODE_BL = 1, + USB_OP_BOARD_MODE_FW = 2, } __attribute__((packed)); -#define USB_OP_DEVICE_VER(board_id, board_mode) (\ - ((board_id & 0xFF) << 8) | \ - ((board_mode & 0xFF) << 0)) +#define USB_OP_DEVICE_VER(board_id, board_mode) \ + ( \ + ((board_id & 0xFF) << 8) | \ + ((board_mode & 0xFF) << 0)) #endif /* PIOS_USB_DEFS_H */ diff --git a/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h b/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h index 8a56d6b19..a828a1c6f 100644 --- a/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h +++ b/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h @@ -11,19 +11,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,6 +37,6 @@ extern int32_t PIOS_USB_DESC_HID_CDC_Init(void); #endif /* PIOS_USB_DESC_HID_CDC_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_desc_hid_only_priv.h b/flight/pios/inc/pios_usb_desc_hid_only_priv.h index 9e7d12401..6d98c3967 100644 --- a/flight/pios/inc/pios_usb_desc_hid_only_priv.h +++ b/flight/pios/inc/pios_usb_desc_hid_only_priv.h @@ -11,19 +11,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,6 +37,6 @@ extern int32_t PIOS_USB_DESC_HID_ONLY_Init(void); #endif /* PIOS_USB_DESC_HID_ONLY_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_hid.h b/flight/pios/inc/pios_usb_hid.h index 2f7a1d598..6d66a332d 100644 --- a/flight/pios/inc/pios_usb_hid.h +++ b/flight/pios/inc/pios_usb_hid.h @@ -35,12 +35,12 @@ extern int32_t PIOS_USB_HID_Reenumerate(void); extern int32_t PIOS_USB_HID_ChangeConnectionState(uint32_t Connected); extern bool PIOS_USB_HID_CheckAvailable(uint8_t id); -extern void PIOS_USB_HID_RegisterHidDescriptor(const uint8_t * desc, uint16_t length); -extern void PIOS_USB_HID_RegisterHidReport(const uint8_t * desc, uint16_t length); +extern void PIOS_USB_HID_RegisterHidDescriptor(const uint8_t *desc, uint16_t length); +extern void PIOS_USB_HID_RegisterHidReport(const uint8_t *desc, uint16_t length); #endif /* PIOS_USB_HID_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_hid_istr.h b/flight/pios/inc/pios_usb_hid_istr.h index 292680342..9ae0f7491 100644 --- a/flight/pios/inc/pios_usb_hid_istr.h +++ b/flight/pios/inc/pios_usb_hid_istr.h @@ -89,7 +89,7 @@ void ESOF_Callback(void); /* Interrupt subroutines user callbacks prototypes. These callbacks are called into the respective interrupt sunroutine functinos and can be tailored for various user application purposes. - Note: Make sure that the correspondant interrupt is enabled through the + Note: Make sure that the correspondant interrupt is enabled through the definition in usb_conf.h file */ void INTR_MODEMISMATCH_Callback(void); void INTR_SOFINTR_Callback(void); diff --git a/flight/pios/inc/pios_usb_hid_priv.h b/flight/pios/inc/pios_usb_hid_priv.h index fba585c18..30e99b2a6 100644 --- a/flight/pios/inc/pios_usb_hid_priv.h +++ b/flight/pios/inc/pios_usb_hid_priv.h @@ -32,18 +32,18 @@ #define PIOS_USB_HID_PRIV_H struct pios_usb_hid_cfg { - uint8_t data_if; - uint8_t data_rx_ep; - uint8_t data_tx_ep; + uint8_t data_if; + uint8_t data_rx_ep; + uint8_t data_tx_ep; }; extern const struct pios_com_driver pios_usb_hid_com_driver; -extern int32_t PIOS_USB_HID_Init(uint32_t * usbhid_id, const struct pios_usb_hid_cfg * cfg, uint32_t lower_id); +extern int32_t PIOS_USB_HID_Init(uint32_t *usbhid_id, const struct pios_usb_hid_cfg *cfg, uint32_t lower_id); #endif /* PIOS_USB_HID_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_hid_pwr.h b/flight/pios/inc/pios_usb_hid_pwr.h index 614344437..d1cb5da58 100644 --- a/flight/pios/inc/pios_usb_hid_pwr.h +++ b/flight/pios/inc/pios_usb_hid_pwr.h @@ -22,23 +22,23 @@ /* Includes ------------------------------------------------------------------*/ /* Exported types ------------------------------------------------------------*/ typedef enum _RESUME_STATE { - RESUME_EXTERNAL, - RESUME_INTERNAL, - RESUME_LATER, - RESUME_WAIT, - RESUME_START, - RESUME_ON, - RESUME_OFF, - RESUME_ESOF + RESUME_EXTERNAL, + RESUME_INTERNAL, + RESUME_LATER, + RESUME_WAIT, + RESUME_START, + RESUME_ON, + RESUME_OFF, + RESUME_ESOF } RESUME_STATE; typedef enum _DEVICE_STATE { - UNCONNECTED, - ATTACHED, - POWERED, - SUSPENDED, - ADDRESSED, - CONFIGURED + UNCONNECTED, + ATTACHED, + POWERED, + SUSPENDED, + ADDRESSED, + CONFIGURED } DEVICE_STATE; /* Exported constants --------------------------------------------------------*/ @@ -50,9 +50,9 @@ void Resume(RESUME_STATE eResumeSetVal); RESULT PowerOn(void); RESULT PowerOff(void); /* External variables --------------------------------------------------------*/ -extern __IO uint32_t bDeviceState; /* USB device status */ -extern __IO bool fSuspendEnabled; /* true when suspend is possible */ +extern __IO uint32_t bDeviceState; /* USB device status */ +extern __IO bool fSuspendEnabled; /* true when suspend is possible */ -#endif /*__USB_PWR_H*/ +#endif /*__USB_PWR_H*/ /******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/inc/pios_usb_priv.h b/flight/pios/inc/pios_usb_priv.h index ee3c26bd9..043671fd5 100644 --- a/flight/pios/inc/pios_usb_priv.h +++ b/flight/pios/inc/pios_usb_priv.h @@ -1,4 +1,4 @@ - /** +/** ****************************************************************************** * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ @@ -35,16 +35,16 @@ #include struct pios_usb_cfg { - struct stm32_irq irq; - struct stm32_gpio vsense; - bool vsense_active_low; + struct stm32_irq irq; + struct stm32_gpio vsense; + bool vsense_active_low; }; -extern int32_t PIOS_USB_Init(uint32_t * usb_id, const struct pios_usb_cfg * cfg); +extern int32_t PIOS_USB_Init(uint32_t *usb_id, const struct pios_usb_cfg *cfg); #endif /* PIOS_USB_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_rctx_priv.h b/flight/pios/inc/pios_usb_rctx_priv.h index cf9aab2b6..8074c3f1a 100644 --- a/flight/pios/inc/pios_usb_rctx_priv.h +++ b/flight/pios/inc/pios_usb_rctx_priv.h @@ -34,15 +34,15 @@ #include "pios_usb_rctx.h" struct pios_usb_rctx_cfg { - uint8_t data_if; - uint8_t data_tx_ep; + uint8_t data_if; + uint8_t data_tx_ep; }; -extern int32_t PIOS_USB_RCTX_Init(uint32_t * usbrctx_id, const struct pios_usb_rctx_cfg * cfg, uint32_t lower_id); +extern int32_t PIOS_USB_RCTX_Init(uint32_t *usbrctx_id, const struct pios_usb_rctx_cfg *cfg, uint32_t lower_id); #endif /* PIOS_USB_RCTX_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_util.h b/flight/pios/inc/pios_usb_util.h index 111b49239..fe714db3d 100644 --- a/flight/pios/inc/pios_usb_util.h +++ b/flight/pios/inc/pios_usb_util.h @@ -31,8 +31,8 @@ #ifndef PIOS_USB_UTIL_H #define PIOS_USB_UTIL_H -#include /* uint8_t */ +#include /* uint8_t */ -uint8_t * PIOS_USB_UTIL_AsciiToUtf8(uint8_t * dst, uint8_t * src, uint16_t srclen); +uint8_t *PIOS_USB_UTIL_AsciiToUtf8(uint8_t *dst, uint8_t *src, uint16_t srclen); -#endif /* PIOS_USB_UTIL_H */ +#endif /* PIOS_USB_UTIL_H */ diff --git a/flight/pios/inc/pios_usbhook.h b/flight/pios/inc/pios_usbhook.h index d6ca8648f..8a923f824 100644 --- a/flight/pios/inc/pios_usbhook.h +++ b/flight/pios/inc/pios_usbhook.h @@ -33,32 +33,32 @@ #include #include -#include "pios_usb_defs.h" /* usb_setup_request */ +#include "pios_usb_defs.h" /* usb_setup_request */ struct pios_usbhook_descriptor { - const uint8_t * descriptor; - uint16_t length; + const uint8_t *descriptor; + uint16_t length; }; enum usb_string_desc { - USB_STRING_DESC_LANG = 0, - USB_STRING_DESC_VENDOR = 1, - USB_STRING_DESC_PRODUCT = 2, - USB_STRING_DESC_SERIAL = 3, + USB_STRING_DESC_LANG = 0, + USB_STRING_DESC_VENDOR = 1, + USB_STRING_DESC_PRODUCT = 2, + USB_STRING_DESC_SERIAL = 3, } __attribute__((packed)); -extern void PIOS_USBHOOK_RegisterDevice(const uint8_t * desc, uint16_t desc_size); -extern void PIOS_USBHOOK_RegisterConfig(uint8_t config_id, const uint8_t * desc, uint16_t desc_size); -extern void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t * desc, uint16_t desc_size); +extern void PIOS_USBHOOK_RegisterDevice(const uint8_t *desc, uint16_t desc_size); +extern void PIOS_USBHOOK_RegisterConfig(uint8_t config_id, const uint8_t *desc, uint16_t desc_size); +extern void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t *desc, uint16_t desc_size); struct pios_usb_ifops { - void (*init)(uint32_t context); - void (*deinit)(uint32_t context); - bool (*setup)(uint32_t context, struct usb_setup_request * req); - void (*ctrl_data_out)(uint32_t context, struct usb_setup_request * req); + void (*init)(uint32_t context); + void (*deinit)(uint32_t context); + bool (*setup)(uint32_t context, struct usb_setup_request *req); + void (*ctrl_data_out)(uint32_t context, struct usb_setup_request *req); }; -extern void PIOS_USBHOOK_RegisterIfOps(uint8_t ifnum, struct pios_usb_ifops * ifops, uint32_t context); +extern void PIOS_USBHOOK_RegisterIfOps(uint8_t ifnum, struct pios_usb_ifops *ifops, uint32_t context); typedef bool (*pios_usbhook_epcb)(uint32_t context, uint8_t epnum, uint16_t len); @@ -75,4 +75,3 @@ extern void PIOS_USBHOOK_Activate(void); extern void PIOS_USBHOOK_Deactivate(void); #endif /* PIOS_USBHOOK_H */ - diff --git a/flight/pios/inc/pios_video.h b/flight/pios/inc/pios_video.h index 1689b0a09..8781aa911 100644 --- a/flight/pios/inc/pios_video.h +++ b/flight/pios/inc/pios_video.h @@ -36,36 +36,36 @@ #include struct pios_video_cfg { - const struct pios_spi_cfg mask; - const struct pios_spi_cfg level; + const struct pios_spi_cfg mask; + const struct pios_spi_cfg level; - const struct pios_exti_cfg * hsync; - const struct pios_exti_cfg * vsync; + const struct pios_exti_cfg *hsync; + const struct pios_exti_cfg *vsync; - struct pios_tim_channel pixel_timer; - struct pios_tim_channel hsync_capture; + struct pios_tim_channel pixel_timer; + struct pios_tim_channel hsync_capture; - TIM_OCInitTypeDef tim_oc_init; + TIM_OCInitTypeDef tim_oc_init; }; // Time vars typedef struct { - uint8_t sec; - uint8_t min; - uint8_t hour; + uint8_t sec; + uint8_t min; + uint8_t hour; } TTime; extern TTime timex; -extern void PIOS_Video_Init(const struct pios_video_cfg * cfg); +extern void PIOS_Video_Init(const struct pios_video_cfg *cfg); uint16_t PIOS_Video_GetOSDLines(void); extern bool PIOS_Hsync_ISR(); extern bool PIOS_Vsync_ISR(); // First OSD line -#define GRAPHICS_LINE 25 +#define GRAPHICS_LINE 25 -//top/left deadband +// top/left deadband #define GRAPHICS_HDEADBAND 80 #define GRAPHICS_VDEADBAND 0 @@ -73,30 +73,30 @@ extern bool PIOS_Vsync_ISR(); // Real OSD size #ifdef PAL -//#define GRAPHICS_WIDTH_REAL (352+GRAPHICS_HDEADBAND) -#define GRAPHICS_WIDTH_REAL 416 - #define GRAPHICS_HEIGHT_REAL (270+GRAPHICS_VDEADBAND) +// #define GRAPHICS_WIDTH_REAL (352+GRAPHICS_HDEADBAND) +#define GRAPHICS_WIDTH_REAL 416 + #define GRAPHICS_HEIGHT_REAL (270 + GRAPHICS_VDEADBAND) #else - #define GRAPHICS_WIDTH_REAL (312+GRAPHICS_HDEADBAND) - #define GRAPHICS_HEIGHT_REAL (225+GRAPHICS_VDEADBAND) + #define GRAPHICS_WIDTH_REAL (312 + GRAPHICS_HDEADBAND) + #define GRAPHICS_HEIGHT_REAL (225 + GRAPHICS_VDEADBAND) #endif -//draw area -#define GRAPHICS_TOP 0 -#define GRAPHICS_LEFT 0 -#define GRAPHICS_BOTTOM (GRAPHICS_HEIGHT_REAL-GRAPHICS_VDEADBAND-1) -#define GRAPHICS_RIGHT (GRAPHICS_WIDTH_REAL-GRAPHICS_HDEADBAND-1) +// draw area +#define GRAPHICS_TOP 0 +#define GRAPHICS_LEFT 0 +#define GRAPHICS_BOTTOM (GRAPHICS_HEIGHT_REAL - GRAPHICS_VDEADBAND - 1) +#define GRAPHICS_RIGHT (GRAPHICS_WIDTH_REAL - GRAPHICS_HDEADBAND - 1) -#define GRAPHICS_WIDTH (GRAPHICS_WIDTH_REAL/8) -#define GRAPHICS_HEIGHT GRAPHICS_HEIGHT_REAL +#define GRAPHICS_WIDTH (GRAPHICS_WIDTH_REAL / 8) +#define GRAPHICS_HEIGHT GRAPHICS_HEIGHT_REAL // dma lenght -#define BUFFER_LINE_LENGTH (GRAPHICS_WIDTH) //Yes, in bytes. +#define BUFFER_LINE_LENGTH (GRAPHICS_WIDTH) // Yes, in bytes. // line types -#define LINE_TYPE_UNKNOWN 0 -#define LINE_TYPE_GRAPHICS 2 +#define LINE_TYPE_UNKNOWN 0 +#define LINE_TYPE_GRAPHICS 2 // Macro to swap buffers given a temporary pointer. #define SWAP_BUFFS(tmp, a, b) { tmp = a; a = b; b = tmp; } diff --git a/flight/pios/inc/pios_wavplay.h b/flight/pios/inc/pios_wavplay.h index 8d55ec5c4..e3fbb7757 100644 --- a/flight/pios/inc/pios_wavplay.h +++ b/flight/pios/inc/pios_wavplay.h @@ -34,19 +34,19 @@ #include -#define BUFFERSIZE 512 //Defines the buffer size to hold data from the SD card. +#define BUFFERSIZE 512 // Defines the buffer size to hold data from the SD card. struct pios_dac_cfg { - TIM_TypeDef *timer; - TIM_TimeBaseInitTypeDef time_base_init; - struct stm32_irq irq; - struct stm32_dma dma; - uint32_t channel; - DAC_InitTypeDef dac_init; - struct stm32_gpio dac_io; + TIM_TypeDef *timer; + TIM_TimeBaseInitTypeDef time_base_init; + struct stm32_irq irq; + struct stm32_dma dma; + uint32_t channel; + DAC_InitTypeDef dac_init; + struct stm32_gpio dac_io; }; -extern void PIOS_WavPlay_Init(const struct pios_dac_cfg * cfg); +extern void PIOS_WavPlay_Init(const struct pios_dac_cfg *cfg); extern uint8_t WavePlayer_Start(void); #endif /* PIOS_WAVPLAY_H */ diff --git a/flight/pios/inc/pios_wdg.h b/flight/pios/inc/pios_wdg.h index b8fa1bae5..d6583442e 100644 --- a/flight/pios/inc/pios_wdg.h +++ b/flight/pios/inc/pios_wdg.h @@ -7,7 +7,7 @@ * * @file pios_wdg.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief SPI functions header. * @see The GNU Public License (GPL) Version 3 * diff --git a/flight/pios/inc/stm32f10x_conf.h b/flight/pios/inc/stm32f10x_conf.h index dda33751a..1683690f4 100644 --- a/flight/pios/inc/stm32f10x_conf.h +++ b/flight/pios/inc/stm32f10x_conf.h @@ -1,22 +1,22 @@ /** - ****************************************************************************** - * @file Project/Template/stm32f10x_conf.h - * @author MCD Application Team - * @version V3.1.2 - * @date 09/28/2009 - * @brief Library configuration file. - ****************************************************************************** - * @copy - * - * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS - * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE - * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY - * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING - * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE - * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. - * - *

© COPYRIGHT 2009 STMicroelectronics

- */ + ****************************************************************************** + * @file Project/Template/stm32f10x_conf.h + * @author MCD Application Team + * @version V3.1.2 + * @date 09/28/2009 + * @brief Library configuration file. + ****************************************************************************** + * @copy + * + * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS + * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE + * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY + * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING + * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * + *

© COPYRIGHT 2009 STMicroelectronics

+ */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef __STM32F10x_CONF_H @@ -45,28 +45,28 @@ #include "stm32f10x_tim.h" #include "stm32f10x_usart.h" #include "stm32f10x_wwdg.h" -#include "misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */ +#include "misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */ /* Exported types ------------------------------------------------------------*/ /* Exported constants --------------------------------------------------------*/ -/* Uncomment the line below to expanse the "assert_param" macro in the +/* Uncomment the line below to expanse the "assert_param" macro in the Standard Peripheral Library drivers code */ -//#define USE_FULL_ASSERT 1 +// #define USE_FULL_ASSERT 1 /* Exported macro ------------------------------------------------------------*/ #ifdef USE_FULL_ASSERT /** -* The assert_param macro is used for function's parameters check. -* \param[in] expr: If expr is false, it calls assert_failed function -* which reports the name of the source file and the source -* line number of the call that failed. -* If expr is true, it returns no value. -* \retval None -*/ + * The assert_param macro is used for function's parameters check. + * \param[in] expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * \retval None + */ #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) /* Exported functions ------------------------------------------------------- */ -extern void assert_failed(uint8_t * file, uint32_t line); +extern void assert_failed(uint8_t *file, uint32_t line); #else #define assert_param(expr) ((void)0) #endif /* USE_FULL_ASSERT */ diff --git a/flight/pios/inc/stm32f2xx_conf.h b/flight/pios/inc/stm32f2xx_conf.h index bafb3715a..c1d4af1ce 100644 --- a/flight/pios/inc/stm32f2xx_conf.h +++ b/flight/pios/inc/stm32f2xx_conf.h @@ -1,23 +1,23 @@ /** - ****************************************************************************** - * @file Project/STM32F2xx_StdPeriph_Template/stm32f2xx_conf.h - * @author MCD Application Team - * @version V0.0.4 - * @date 13-January-2011 - * @brief Library configuration file. - ****************************************************************************** - * @attention - * - * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS - * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE - * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY - * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING - * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE - * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. - * - *

© COPYRIGHT 2011 STMicroelectronics

- ****************************************************************************** - */ + ****************************************************************************** + * @file Project/STM32F2xx_StdPeriph_Template/stm32f2xx_conf.h + * @author MCD Application Team + * @version V0.0.4 + * @date 13-January-2011 + * @brief Library configuration file. + ****************************************************************************** + * @attention + * + * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS + * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE + * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY + * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING + * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * + *

© COPYRIGHT 2011 STMicroelectronics

+ ****************************************************************************** + */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef __STM32F2xx_CONF_H @@ -26,25 +26,25 @@ /* Includes ------------------------------------------------------------------*/ /* Uncomment the line below to enable peripheral header file inclusion */ #include "stm32f2xx_adc.h" -//#include "stm32f2xx_can.h" +// #include "stm32f2xx_can.h" #include "stm32f2xx_crc.h" -//#include "stm32f2xx_cryp.h" +// #include "stm32f2xx_cryp.h" #include "stm32f2xx_dac.h" -//#include "stm32f2xx_dbgmcu.h" -//#include "stm32f2xx_dcmi.h" +// #include "stm32f2xx_dbgmcu.h" +// #include "stm32f2xx_dcmi.h" #include "stm32f2xx_dma.h" #include "stm32f2xx_exti.h" #include "stm32f2xx_flash.h" -//#include "stm32f2xx_fsmc.h" -//#include "stm32f2xx_hash.h" +// #include "stm32f2xx_fsmc.h" +// #include "stm32f2xx_hash.h" #include "stm32f2xx_gpio.h" #include "stm32f2xx_i2c.h" -//#include "stm32f2xx_iwdg.h" +// #include "stm32f2xx_iwdg.h" #include "stm32f2xx_pwr.h" #include "stm32f2xx_rcc.h" -//#include "stm32f2xx_rng.h" +// #include "stm32f2xx_rng.h" #include "stm32f2xx_rtc.h" -//#include "stm32f2xx_sdio.h" +// #include "stm32f2xx_sdio.h" #include "stm32f2xx_spi.h" #include "stm32f2xx_syscfg.h" #include "stm32f2xx_tim.h" @@ -55,13 +55,13 @@ /* Exported types ------------------------------------------------------------*/ /* Exported constants --------------------------------------------------------*/ -/* If an external clock source is used, then the value of the following define - should be set to the value of the external clock source, else, if no external +/* If an external clock source is used, then the value of the following define + should be set to the value of the external clock source, else, if no external clock is used, keep this define commented */ /*#define I2S_EXTERNAL_CLOCK_VAL 12288000 */ /* Value of the external clock in Hz */ -/* Uncomment the line below to expanse the "assert_param" macro in the +/* Uncomment the line below to expanse the "assert_param" macro in the Standard Peripheral Library drivers code */ /* #define USE_FULL_ASSERT 1 */ @@ -69,16 +69,16 @@ #ifdef USE_FULL_ASSERT /** - * @brief The assert_param macro is used for function's parameters check. - * @param expr: If expr is false, it calls assert_failed function - * which reports the name of the source file and the source - * line number of the call that failed. - * If expr is true, it returns no value. - * @retval None - */ + * @brief The assert_param macro is used for function's parameters check. + * @param expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) /* Exported functions ------------------------------------------------------- */ - void assert_failed(uint8_t* file, uint32_t line); +void assert_failed(uint8_t *file, uint32_t line); #else #define assert_param(expr) ((void)0) #endif /* USE_FULL_ASSERT */ diff --git a/flight/pios/inc/stm32f4xx_conf.h b/flight/pios/inc/stm32f4xx_conf.h index 4618691d2..d61e4f368 100644 --- a/flight/pios/inc/stm32f4xx_conf.h +++ b/flight/pios/inc/stm32f4xx_conf.h @@ -1,23 +1,23 @@ /** - ****************************************************************************** - * @file Project/STM32F4xx_StdPeriph_Template/stm32f4xx_conf.h - * @author MCD Application Team - * @version V0.0.4 - * @date 13-January-2011 - * @brief Library configuration file. - ****************************************************************************** - * @attention - * - * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS - * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE - * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY - * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING - * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE - * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. - * - *

© COPYRIGHT 2011 STMicroelectronics

- ****************************************************************************** - */ + ****************************************************************************** + * @file Project/STM32F4xx_StdPeriph_Template/stm32f4xx_conf.h + * @author MCD Application Team + * @version V0.0.4 + * @date 13-January-2011 + * @brief Library configuration file. + ****************************************************************************** + * @attention + * + * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS + * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE + * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY + * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING + * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * + *

© COPYRIGHT 2011 STMicroelectronics

+ ****************************************************************************** + */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef __STM32F4xx_CONF_H @@ -26,25 +26,25 @@ /* Includes ------------------------------------------------------------------*/ /* Uncomment the line below to enable peripheral header file inclusion */ #include "stm32f4xx_adc.h" -//#include "stm32f4xx_can.h" +// #include "stm32f4xx_can.h" #include "stm32f4xx_crc.h" -//#include "stm32f2xx_cryp.h" +// #include "stm32f2xx_cryp.h" #include "stm32f4xx_dac.h" -//#include "stm32f4xx_dbgmcu.h" -//#include "stm32f4xx_dcmi.h" +// #include "stm32f4xx_dbgmcu.h" +// #include "stm32f4xx_dcmi.h" #include "stm32f4xx_dma.h" #include "stm32f4xx_exti.h" #include "stm32f4xx_flash.h" -//#include "stm32f4xx_fsmc.h" -//#include "stm32f4xx_hash.h" +// #include "stm32f4xx_fsmc.h" +// #include "stm32f4xx_hash.h" #include "stm32f4xx_gpio.h" #include "stm32f4xx_i2c.h" -//#include "stm32f4xx_iwdg.h" +// #include "stm32f4xx_iwdg.h" #include "stm32f4xx_pwr.h" #include "stm32f4xx_rcc.h" -//#include "stm32f4xx_rng.h" +// #include "stm32f4xx_rng.h" #include "stm32f4xx_rtc.h" -//#include "stm32f4xx_sdio.h" +// #include "stm32f4xx_sdio.h" #include "stm32f4xx_spi.h" #include "stm32f4xx_syscfg.h" #include "stm32f4xx_tim.h" @@ -55,13 +55,13 @@ /* Exported types ------------------------------------------------------------*/ /* Exported constants --------------------------------------------------------*/ -/* If an external clock source is used, then the value of the following define - should be set to the value of the external clock source, else, if no external +/* If an external clock source is used, then the value of the following define + should be set to the value of the external clock source, else, if no external clock is used, keep this define commented */ /*#define I2S_EXTERNAL_CLOCK_VAL 12288000 */ /* Value of the external clock in Hz */ -/* Uncomment the line below to expanse the "assert_param" macro in the +/* Uncomment the line below to expanse the "assert_param" macro in the Standard Peripheral Library drivers code */ /* #define USE_FULL_ASSERT 1 */ @@ -69,16 +69,16 @@ #ifdef USE_FULL_ASSERT /** - * @brief The assert_param macro is used for function's parameters check. - * @param expr: If expr is false, it calls assert_failed function - * which reports the name of the source file and the source - * line number of the call that failed. - * If expr is true, it returns no value. - * @retval None - */ + * @brief The assert_param macro is used for function's parameters check. + * @param expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) /* Exported functions ------------------------------------------------------- */ - void assert_failed(uint8_t* file, uint32_t line); +void assert_failed(uint8_t *file, uint32_t line); #else #define assert_param(expr) ((void)0) #endif /* USE_FULL_ASSERT */ diff --git a/flight/pios/inc/usb_conf.h b/flight/pios/inc/usb_conf.h index f174eaf62..6c9ec9008 100644 --- a/flight/pios/inc/usb_conf.h +++ b/flight/pios/inc/usb_conf.h @@ -30,27 +30,27 @@ /*-------------------------------------------------------------*/ /* buffer table base address */ /* buffer table base address */ -#define BTABLE_ADDRESS (0x00) +#define BTABLE_ADDRESS (0x00) /* EP0 */ /* rx/tx buffer base address */ -#define ENDP0_RXADDR (0x20) -#define ENDP0_TXADDR (0x40) +#define ENDP0_RXADDR (0x20) +#define ENDP0_TXADDR (0x40) /* EP1 */ /* rx/tx buffer base address */ -#define ENDP1_TXADDR (0x60) -#define ENDP1_RXADDR (0x80) +#define ENDP1_TXADDR (0x60) +#define ENDP1_RXADDR (0x80) /* EP2 */ /* rx/tx buffer base address */ -#define ENDP2_TXADDR (0x100) -#define ENDP2_RXADDR (0x140) +#define ENDP2_TXADDR (0x100) +#define ENDP2_RXADDR (0x140) /* EP3 */ /* rx/tx buffer base address */ -#define ENDP3_TXADDR (0x180) -#define ENDP3_RXADDR (0x1C0) +#define ENDP3_TXADDR (0x180) +#define ENDP3_RXADDR (0x1C0) /*-------------------------------------------------------------*/ /* ------------------- ISTR events -------------------------*/ @@ -58,15 +58,16 @@ /* IMR_MSK */ /* mask defining which events has to be handled */ /* by the device application software */ -#define IMR_MSK (CNTR_CTRM | CNTR_WKUPM | CNTR_SUSPM | CNTR_ERRM | CNTR_SOFM \ - | CNTR_ESOFM | CNTR_RESETM ) +#define IMR_MSK \ + (CNTR_CTRM | CNTR_WKUPM | CNTR_SUSPM | CNTR_ERRM | CNTR_SOFM \ + | CNTR_ESOFM | CNTR_RESETM) #endif /* STM32F10X_CL */ #ifdef STM32F10X_CL /******************************************************************************* * FIFO Size Configuration -* +* * (i) Dedicated data FIFO SPRAM of 1.25 Kbytes = 1280 bytes = 320 32-bits words * available for the endpoints IN and OUT. * Device mode features: @@ -74,115 +75,115 @@ * -3 IN EPs to support any kind of Bulk, Interrupt or Isochronous transfer * -3 OUT EPs to support any kind of Bulk, Interrupt or Isochronous transfer * -* ii) Receive data FIFO size = RAM for setup packets + +* ii) Receive data FIFO size = RAM for setup packets + * OUT endpoint control information + * data OUT packets + miscellaneous * Space = ONE 32-bits words * --> RAM for setup packets = 4 * n + 6 space -* (n is the nbr of CTRL EPs the device core supports) +* (n is the nbr of CTRL EPs the device core supports) * --> OUT EP CTRL info = 1 space -* (one space for status information written to the FIFO along with each +* (one space for status information written to the FIFO along with each * received packet) -* --> data OUT packets = (Largest Packet Size / 4) + 1 spaces +* --> data OUT packets = (Largest Packet Size / 4) + 1 spaces * (MINIMUM to receive packets) -* --> OR data OUT packets = at least 2*(Largest Packet Size / 4) + 1 spaces +* --> OR data OUT packets = at least 2*(Largest Packet Size / 4) + 1 spaces * (if high-bandwidth EP is enabled or multiple isochronous EPs) * --> miscellaneous = 1 space per OUT EP -* (one space for transfer complete status information also pushed to the +* (one space for transfer complete status information also pushed to the * FIFO with each endpoint's last packet) * -* (iii)MINIMUM RAM space required for each IN EP Tx FIFO = MAX packet size for +* (iii)MINIMUM RAM space required for each IN EP Tx FIFO = MAX packet size for * that particular IN EP. More space allocated in the IN EP Tx FIFO results * in a better performance on the USB and can hide latencies on the AHB. * * (iv) TXn min size = 16 words. (n : Transmit FIFO index) -* (v) When a TxFIFO is not used, the Configuration should be as follows: +* (v) When a TxFIFO is not used, the Configuration should be as follows: * case 1 : n > m and Txn is not used (n,m : Transmit FIFO indexes) * --> Txm can use the space allocated for Txn. * case2 : n < m and Txn is not used (n,m : Transmit FIFO indexes) * --> Txn should be configured with the minimum space of 16 words -* (vi) The FIFO is used optimally when used TxFIFOs are allocated in the top +* (vi) The FIFO is used optimally when used TxFIFOs are allocated in the top * of the FIFO.Ex: use EP1 and EP2 as IN instead of EP1 and EP3 as IN ones. *******************************************************************************/ -#define RX_FIFO_SIZE 128 -#define TX0_FIFO_SIZE 64 -#define TX1_FIFO_SIZE 64 -#define TX2_FIFO_SIZE 16 -#define TX3_FIFO_SIZE 16 +#define RX_FIFO_SIZE 128 +#define TX0_FIFO_SIZE 64 +#define TX1_FIFO_SIZE 64 +#define TX2_FIFO_SIZE 16 +#define TX3_FIFO_SIZE 16 /* OTGD-FS-DEVICE IP interrupts Enable definitions */ /* Uncomment the define to enable the selected interrupt */ -//#define INTR_MODEMISMATCH +// #define INTR_MODEMISMATCH #define INTR_SOFINTR -#define INTR_RXSTSQLVL /* Mandatory */ -//#define INTR_NPTXFEMPTY -//#define INTR_GINNAKEFF -//#define INTR_GOUTNAKEFF -//#define INTR_ERLYSUSPEND -#define INTR_USBSUSPEND /* Mandatory */ -#define INTR_USBRESET /* Mandatory */ -#define INTR_ENUMDONE /* Mandatory */ -//#define INTR_ISOOUTDROP -//#define INTR_EOPFRAME -//#define INTR_EPMISMATCH -#define INTR_INEPINTR /* Mandatory */ -#define INTR_OUTEPINTR /* Mandatory */ -//#define INTR_INCOMPLISOIN -//#define INTR_INCOMPLISOOUT -#define INTR_WKUPINTR /* Mandatory */ +#define INTR_RXSTSQLVL /* Mandatory */ +// #define INTR_NPTXFEMPTY +// #define INTR_GINNAKEFF +// #define INTR_GOUTNAKEFF +// #define INTR_ERLYSUSPEND +#define INTR_USBSUSPEND /* Mandatory */ +#define INTR_USBRESET /* Mandatory */ +#define INTR_ENUMDONE /* Mandatory */ +// #define INTR_ISOOUTDROP +// #define INTR_EOPFRAME +// #define INTR_EPMISMATCH +#define INTR_INEPINTR /* Mandatory */ +#define INTR_OUTEPINTR /* Mandatory */ +// #define INTR_INCOMPLISOIN +// #define INTR_INCOMPLISOOUT +#define INTR_WKUPINTR /* Mandatory */ /* OTGD-FS-DEVICE IP interrupts subroutines */ /* Comment the define to enable the selected interrupt subroutine and replace it by user code */ -#define INTR_MODEMISMATCH_Callback NOP_Process -#define INTR_SOFINTR_Callback NOP_Process -#define INTR_RXSTSQLVL_Callback NOP_Process -#define INTR_NPTXFEMPTY_Callback NOP_Process -#define INTR_NPTXFEMPTY_Callback NOP_Process -#define INTR_GINNAKEFF_Callback NOP_Process -#define INTR_GOUTNAKEFF_Callback NOP_Process -#define INTR_ERLYSUSPEND_Callback NOP_Process -#define INTR_USBSUSPEND_Callback NOP_Process -#define INTR_USBRESET_Callback NOP_Process -#define INTR_ENUMDONE_Callback NOP_Process -#define INTR_ISOOUTDROP_Callback NOP_Process -#define INTR_EOPFRAME_Callback NOP_Process -#define INTR_EPMISMATCH_Callback NOP_Process -#define INTR_INEPINTR_Callback NOP_Process -#define INTR_OUTEPINTR_Callback NOP_Process -#define INTR_INCOMPLISOIN_Callback NOP_Process -#define INTR_INCOMPLISOOUT_Callback NOP_Process -#define INTR_WKUPINTR_Callback NOP_Process +#define INTR_MODEMISMATCH_Callback NOP_Process +#define INTR_SOFINTR_Callback NOP_Process +#define INTR_RXSTSQLVL_Callback NOP_Process +#define INTR_NPTXFEMPTY_Callback NOP_Process +#define INTR_NPTXFEMPTY_Callback NOP_Process +#define INTR_GINNAKEFF_Callback NOP_Process +#define INTR_GOUTNAKEFF_Callback NOP_Process +#define INTR_ERLYSUSPEND_Callback NOP_Process +#define INTR_USBSUSPEND_Callback NOP_Process +#define INTR_USBRESET_Callback NOP_Process +#define INTR_ENUMDONE_Callback NOP_Process +#define INTR_ISOOUTDROP_Callback NOP_Process +#define INTR_EOPFRAME_Callback NOP_Process +#define INTR_EPMISMATCH_Callback NOP_Process +#define INTR_INEPINTR_Callback NOP_Process +#define INTR_OUTEPINTR_Callback NOP_Process +#define INTR_INCOMPLISOIN_Callback NOP_Process +#define INTR_INCOMPLISOOUT_Callback NOP_Process +#define INTR_WKUPINTR_Callback NOP_Process /* Isochronous data update */ -#define INTR_RXSTSQLVL_ISODU_Callback NOP_Process +#define INTR_RXSTSQLVL_ISODU_Callback NOP_Process /* Isochronous transfer parameters */ /* Size of a single Isochronous buffer (size of a single transfer) */ -#define ISOC_BUFFER_SZE 1 +#define ISOC_BUFFER_SZE 1 /* Number of sub-buffers (number of single buffers/transfers), should be even */ -#define NUM_SUB_BUFFERS 2 +#define NUM_SUB_BUFFERS 2 #endif /* STM32F10X_CL */ /* CTR service routines */ /* associated to defined endpoints */ -#define EP1_IN_Callback NOP_Process -#define EP2_IN_Callback NOP_Process -#define EP3_IN_Callback NOP_Process -#define EP4_IN_Callback NOP_Process -#define EP5_IN_Callback NOP_Process -#define EP6_IN_Callback NOP_Process -#define EP7_IN_Callback NOP_Process +#define EP1_IN_Callback NOP_Process +#define EP2_IN_Callback NOP_Process +#define EP3_IN_Callback NOP_Process +#define EP4_IN_Callback NOP_Process +#define EP5_IN_Callback NOP_Process +#define EP6_IN_Callback NOP_Process +#define EP7_IN_Callback NOP_Process -#define EP1_OUT_Callback NOP_Process -#define EP2_OUT_Callback NOP_Process -#define EP3_OUT_Callback NOP_Process -#define EP4_OUT_Callback NOP_Process -#define EP5_OUT_Callback NOP_Process -#define EP6_OUT_Callback NOP_Process -#define EP7_OUT_Callback NOP_Process +#define EP1_OUT_Callback NOP_Process +#define EP2_OUT_Callback NOP_Process +#define EP3_OUT_Callback NOP_Process +#define EP4_OUT_Callback NOP_Process +#define EP5_OUT_Callback NOP_Process +#define EP6_OUT_Callback NOP_Process +#define EP7_OUT_Callback NOP_Process #endif /*__USB_CONF_H*/ diff --git a/flight/pios/osx/inc/FreeRTOSConfig.h b/flight/pios/osx/inc/FreeRTOSConfig.h index 46bb2dcb4..b2a58a77c 100644 --- a/flight/pios/osx/inc/FreeRTOSConfig.h +++ b/flight/pios/osx/inc/FreeRTOSConfig.h @@ -1,112 +1,110 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ -/* Notes: We use 5 task priorities */ +/* Notes: We use 5 task priorities */ -#ifdef __APPLE__ - #define COND_SIGNALING - #define CHECK_TASK_RESUMES - #define RUNNING_THREAD_MUTEX -// #define TICK_SIGNAL -// #define TICK_SIGWAIT - #define IDLE_SLEEPS +#ifdef __APPLE__ + #define COND_SIGNALING + #define CHECK_TASK_RESUMES + #define RUNNING_THREAD_MUTEX +// #define TICK_SIGNAL +// #define TICK_SIGWAIT + #define IDLE_SLEEPS - #define configUSE_PREEMPTION 1 - #define configIDLE_SHOULD_YIELD 0 + #define configUSE_PREEMPTION 1 + #define configIDLE_SHOULD_YIELD 0 #endif #ifdef __CYGWIN__ - #define COND_SIGNALING - #define CHECK_TASK_RESUMES -// #define RUNNING_THREAD_MUTEX -// #define TICK_SIGNAL - #define TICK_SIGWAIT - #define IDLE_SLEEPS + #define COND_SIGNALING + #define CHECK_TASK_RESUMES +// #define RUNNING_THREAD_MUTEX +// #define TICK_SIGNAL + #define TICK_SIGWAIT + #define IDLE_SLEEPS - #define configUSE_PREEMPTION 0 - #define configIDLE_SHOULD_YIELD 1 + #define configUSE_PREEMPTION 0 + #define configIDLE_SHOULD_YIELD 1 #endif #ifdef __linux__ - #define COND_SIGNALING - #define CHECK_TASK_RESUMES - #define RUNNING_THREAD_MUTEX -// #define TICK_SIGNAL -// #define TICK_SIGWAIT - #define IDLE_SLEEPS + #define COND_SIGNALING + #define CHECK_TASK_RESUMES + #define RUNNING_THREAD_MUTEX +// #define TICK_SIGNAL +// #define TICK_SIGWAIT + #define IDLE_SLEEPS - #define configUSE_PREEMPTION 1 - #define configIDLE_SHOULD_YIELD 0 + #define configUSE_PREEMPTION 1 + #define configIDLE_SHOULD_YIELD 0 #endif -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 ) -#define configTICK_RATE_HZ ( ( portTickType ) 1000 ) -#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) -#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 256 ) -#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 45 * 1024 ) ) -#define configMAX_TASK_NAME_LEN ( 16 ) -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)256) +#define configTOTAL_HEAP_SIZE ((size_t)(45 * 1024)) +#define configMAX_TASK_NAME_LEN (16) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ + to exclude the API function. */ -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 0 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 0 +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 0 /* Enable run time stats collection */ -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 #define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() #define portGET_RUN_TIME_COUNTER_VALUE() clock() #error Here /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 #endif /* FREERTOS_CONFIG_H */ - diff --git a/flight/pios/osx/inc/pios_bl_helper.h b/flight/pios/osx/inc/pios_bl_helper.h index e2e9e694e..88d67719f 100644 --- a/flight/pios/osx/inc/pios_bl_helper.h +++ b/flight/pios/osx/inc/pios_bl_helper.h @@ -37,7 +37,7 @@ extern uint8_t PIOS_BL_HELPER_FLASH_Ini(); extern uint32_t PIOS_BL_HELPER_CRC_Memory_Calc(); -extern void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t * array, uint8_t size); +extern void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t *array, uint8_t size); extern uint8_t PIOS_BL_HELPER_FLASH_Start(); diff --git a/flight/pios/osx/inc/pios_board_info.h b/flight/pios/osx/inc/pios_board_info.h index 3ff7aa886..2e549f090 100644 --- a/flight/pios/osx/inc/pios_board_info.h +++ b/flight/pios/osx/inc/pios_board_info.h @@ -1,22 +1,22 @@ #ifndef PIOS_BOARD_INFO_H #define PIOS_BOARD_INFO_H -#include /* uint* */ +#include /* uint* */ #define PIOS_BOARD_INFO_BLOB_MAGIC 0xBDBDBDBD struct pios_board_info { - uint32_t magic; - uint8_t board_type; - uint8_t board_rev; - uint8_t bl_rev; - uint8_t hw_type; - uint32_t fw_base; - uint32_t fw_size; - uint32_t desc_base; - uint32_t desc_size; - uint32_t ee_base; - uint32_t ee_size; + uint32_t magic; + uint8_t board_type; + uint8_t board_rev; + uint8_t bl_rev; + uint8_t hw_type; + uint32_t fw_base; + uint32_t fw_size; + uint32_t desc_base; + uint32_t desc_size; + uint32_t ee_base; + uint32_t ee_size; } __attribute__((packed)); extern const struct pios_board_info pios_board_info_blob; diff --git a/flight/pios/osx/inc/pios_com.h b/flight/pios/osx/inc/pios_com.h index badba0128..edac1e181 100644 --- a/flight/pios/osx/inc/pios_com.h +++ b/flight/pios/osx/inc/pios_com.h @@ -6,46 +6,46 @@ * @brief Hardware communication layer * @{ * - * @file pios_com.h + * @file pios_com.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM layer functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_COM_H #define PIOS_COM_H -typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * task_woken); +typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *task_woken); struct pios_com_driver { - void (*init)(uint32_t id); - void (*set_baud)(uint32_t id, uint32_t baud); - void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail); - void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); - void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); - void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); - bool (*available)(uint32_t id); + void (*init)(uint32_t id); + void (*set_baud)(uint32_t id, uint32_t baud); + void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail); + void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); + void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); + void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); + bool (*available)(uint32_t id); }; /* Public Functions */ -extern int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len); +extern int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len); extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud); extern int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c); extern int32_t PIOS_COM_SendChar(uint32_t com_id, char c); @@ -55,12 +55,12 @@ extern int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str); extern int32_t PIOS_COM_SendString(uint32_t com_id, const char *str); extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...); extern int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...); -extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms); +extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t *buf, uint16_t buf_len, uint32_t timeout_ms); extern bool PIOS_COM_Available(uint32_t com_id); #endif /* PIOS_COM_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_com_priv.h b/flight/pios/osx/inc/pios_com_priv.h index 54af82bcb..79caf2380 100644 --- a/flight/pios/osx/inc/pios_com_priv.h +++ b/flight/pios/osx/inc/pios_com_priv.h @@ -8,7 +8,7 @@ * * @file pios_com_priv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM private definitions. * @see The GNU Public License (GPL) Version 3 * @@ -39,6 +39,6 @@ extern int32_t PIOS_COM_ReceiveHandler(uint32_t com_id); #endif /* PIOS_COM_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_crc.h b/flight/pios/osx/inc/pios_crc.h index 3a64f8bab..5a737ade1 100644 --- a/flight/pios/osx/inc/pios_crc.h +++ b/flight/pios/osx/inc/pios_crc.h @@ -5,27 +5,27 @@ * @addtogroup PIOS_CRC CRC Functions * @{ * - * @file pios_crc.h + * @file pios_crc.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief CRC functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data); -uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length); +uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t *data, int32_t length); diff --git a/flight/pios/osx/inc/pios_debug.h b/flight/pios/osx/inc/pios_debug.h index e663d224e..1d4b88595 100644 --- a/flight/pios/osx/inc/pios_debug.h +++ b/flight/pios/osx/inc/pios_debug.h @@ -6,25 +6,25 @@ * @brief Debugging functionality * @{ * - * @file pios_i2c.h + * @file pios_i2c.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Debug helper functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,16 +41,18 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value); void PIOS_DEBUG_Panic(const char *msg); #ifdef DEBUG -#define PIOS_DEBUG_Assert(test) if (!(test)) PIOS_DEBUG_Panic(PIOS_DEBUG_AssertMsg); -#define PIOS_Assert(test) PIOS_DEBUG_Assert(test) +#define PIOS_DEBUG_Assert(test) if (!(test)) { PIOS_DEBUG_Panic(PIOS_DEBUG_AssertMsg); } +#define PIOS_Assert(test) PIOS_DEBUG_Assert(test) #else #define PIOS_DEBUG_Assert(test) -#define PIOS_Assert(test) if (!(test)) while (1); +#define PIOS_Assert(test) \ + if (!(test)) { while (1) {; } \ + } #endif #endif /* PIOS_DEBUG_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_delay.h b/flight/pios/osx/inc/pios_delay.h index 2039ce880..8b1caeed5 100644 --- a/flight/pios/osx/inc/pios_delay.h +++ b/flight/pios/osx/inc/pios_delay.h @@ -8,23 +8,23 @@ * * @file pios_settings.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Settings functions header + * @brief Settings functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,6 +43,6 @@ extern uint32_t PIOS_DELAY_DiffuS(uint32_t raw); #endif /* PIOS_DELAY_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_iap.h b/flight/pios/osx/inc/pios_iap.h index 5119711d5..a67ef11c4 100644 --- a/flight/pios/osx/inc/pios_iap.h +++ b/flight/pios/osx/inc/pios_iap.h @@ -1,5 +1,5 @@ /*! - * @File iap.h + * @File iap.h * @Brief Header file for the In-Application-Programming Module * * Created on: Sep 6, 2010 @@ -11,35 +11,35 @@ /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ /***************************************************************************************** * Public Definitions/Macros ****************************************************************************************/ #if defined(STM32F4XX) -#define MAGIC_REG_1 RTC_BKP_DR1 -#define MAGIC_REG_2 RTC_BKP_DR2 -#define IAP_BOOTCOUNT RTC_BKP_DR3 +#define MAGIC_REG_1 RTC_BKP_DR1 +#define MAGIC_REG_2 RTC_BKP_DR2 +#define IAP_BOOTCOUNT RTC_BKP_DR3 #else -#define MAGIC_REG_1 BKP_DR1 -#define MAGIC_REG_2 BKP_DR2 -#define IAP_BOOTCOUNT BKP_DR3 +#define MAGIC_REG_1 BKP_DR1 +#define MAGIC_REG_2 BKP_DR2 +#define IAP_BOOTCOUNT BKP_DR3 #endif /**************************************************************************************** - * Public Functions - ****************************************************************************************/ -void PIOS_IAP_Init(void); -uint32_t PIOS_IAP_CheckRequest( void ); -void PIOS_IAP_SetRequest1(void); -void PIOS_IAP_SetRequest2(void); -void PIOS_IAP_ClearRequest(void); -uint16_t PIOS_IAP_ReadBootCount(void); -void PIOS_IAP_WriteBootCount(uint16_t); +* Public Functions +****************************************************************************************/ +void PIOS_IAP_Init(void); +uint32_t PIOS_IAP_CheckRequest(void); +void PIOS_IAP_SetRequest1(void); +void PIOS_IAP_SetRequest2(void); +void PIOS_IAP_ClearRequest(void); +uint16_t PIOS_IAP_ReadBootCount(void); +void PIOS_IAP_WriteBootCount(uint16_t); /**************************************************************************************** - * Public Data - ****************************************************************************************/ +* Public Data +****************************************************************************************/ #endif /* PIOS_IAP_H_ */ diff --git a/flight/pios/osx/inc/pios_initcall.h b/flight/pios/osx/inc/pios_initcall.h index f88081c33..6b1c4421a 100644 --- a/flight/pios/osx/inc/pios_initcall.h +++ b/flight/pios/osx/inc/pios_initcall.h @@ -6,25 +6,25 @@ * @brief Initcall Macros * @{ * - * @file pios_initcall.h + * @file pios_initcall.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. * @brief Initcall header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,8 +41,8 @@ typedef int32_t (*initcall_t)(void); typedef struct { - initcall_t fn_minit; - initcall_t fn_tinit; + initcall_t fn_minit; + initcall_t fn_tinit; } initmodule_t; /* Init module section */ @@ -53,18 +53,20 @@ extern void StartModules(); #define MODULE_INITCALL(ifn, sfn) -#define MODULE_TASKCREATE_ALL { \ - /* Start all module threads */ \ - StartModules(); \ - } +#define MODULE_TASKCREATE_ALL \ + { \ + /* Start all module threads */ \ + StartModules(); \ + } -#define MODULE_INITIALISE_ALL { \ - /* Initialize modules */ \ - InitModules(); \ - /* Initialize the system thread */ \ - SystemModInitialize();} +#define MODULE_INITIALISE_ALL \ + { \ + /* Initialize modules */ \ + InitModules(); \ + /* Initialize the system thread */ \ + SystemModInitialize(); } -#endif /* PIOS_INITCALL_H */ +#endif /* PIOS_INITCALL_H */ /** * @} diff --git a/flight/pios/osx/inc/pios_irq.h b/flight/pios/osx/inc/pios_irq.h index f9b9dbb97..e934ae1f6 100644 --- a/flight/pios/osx/inc/pios_irq.h +++ b/flight/pios/osx/inc/pios_irq.h @@ -5,26 +5,26 @@ * @addtogroup PIOS_IRQ IRQ Setup Functions * @{ * - * @file pios_irq.h + * @file pios_irq.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief IRQ functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/osx/inc/pios_led.h b/flight/pios/osx/inc/pios_led.h index ba56b1f4a..92ed0f70e 100644 --- a/flight/pios/osx/inc/pios_led.h +++ b/flight/pios/osx/inc/pios_led.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_LED LED Functions * @{ * - * @file pios_led.h + * @file pios_led.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief LED functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/osx/inc/pios_posix.h b/flight/pios/osx/inc/pios_posix.h index b458f77b6..99dcbc001 100644 --- a/flight/pios/osx/inc/pios_posix.h +++ b/flight/pios/osx/inc/pios_posix.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file posix.h + * @file posix.h * @author Corvus Corax Copyright (C) 2010. * @brief Definitions to run PiOS on posix * @see The GNU Public License (GPL) Version 2 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -28,17 +28,16 @@ #include -typedef enum {FALSE = 0, TRUE = !FALSE} bool; +typedef enum { FALSE = 0, TRUE = !FALSE } bool; #ifndef false - #define false FALSE - #define true TRUE + #define false FALSE + #define true TRUE #endif -#define FILEINFO FILE* +#define FILEINFO FILE * #define PIOS_SERVO_NUM_OUTPUTS 8 -#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS +#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS #endif - diff --git a/flight/pios/osx/inc/pios_rcvr.h b/flight/pios/osx/inc/pios_rcvr.h index 472a4b33d..274ec80b9 100644 --- a/flight/pios/osx/inc/pios_rcvr.h +++ b/flight/pios/osx/inc/pios_rcvr.h @@ -6,25 +6,25 @@ * @brief Hardware communication layer * @{ * - * @file pios_rcvr.h + * @file pios_rcvr.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RCVR layer functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,15 +32,15 @@ #define PIOS_RCVR_H struct pios_rcvr_channel_map { - uint32_t id; - uint8_t channel; + uint32_t id; + uint8_t channel; }; extern struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[]; struct pios_rcvr_driver { - void (*init)(uint32_t id); - int32_t (*read)(uint32_t id, uint8_t channel); + void (*init)(uint32_t id); + int32_t (*read)(uint32_t id, uint8_t channel); }; /* Public Functions */ @@ -48,17 +48,17 @@ extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel); /*! Define error codes for PIOS_RCVR_Get */ enum PIOS_RCVR_errors { - /*! Indicates that a failsafe condition or missing receiver detected for that channel */ - PIOS_RCVR_TIMEOUT = 0, - /*! Channel is invalid for this driver (usually out of range supported) */ - PIOS_RCVR_INVALID = -1, - /*! Indicates that the driver for this channel has not been initialized */ - PIOS_RCVR_NODRIVER = -2 + /*! Indicates that a failsafe condition or missing receiver detected for that channel */ + PIOS_RCVR_TIMEOUT = 0, + /*! Channel is invalid for this driver (usually out of range supported) */ + PIOS_RCVR_INVALID = -1, + /*! Indicates that the driver for this channel has not been initialized */ + PIOS_RCVR_NODRIVER = -2 }; #endif /* PIOS_RCVR_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_rcvr_priv.h b/flight/pios/osx/inc/pios_rcvr_priv.h index 968dc2116..bde247d33 100644 --- a/flight/pios/osx/inc/pios_rcvr_priv.h +++ b/flight/pios/osx/inc/pios_rcvr_priv.h @@ -8,7 +8,7 @@ * * @file pios_rcvr_priv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief USART private definitions. * @see The GNU Public License (GPL) Version 3 * @@ -36,13 +36,13 @@ extern uint32_t pios_rcvr_max_channel; -extern int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id); +extern int32_t PIOS_RCVR_Init(uint32_t *rcvr_id, const struct pios_rcvr_driver *driver, const uint32_t lower_id); extern void PIOS_RCVR_IRQ_Handler(uint32_t rcvr_id); #endif /* PIOS_RCVR_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_sdcard.h b/flight/pios/osx/inc/pios_sdcard.h index 2927def2d..72bd9d042 100644 --- a/flight/pios/osx/inc/pios_sdcard.h +++ b/flight/pios/osx/inc/pios_sdcard.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_sdcard.h + * @file pios_sdcard.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief System and hardware Init functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,61 +31,61 @@ /* Public Functions */ typedef struct { - uint8_t CSDStruct; /* CSD structure */ - uint8_t SysSpecVersion; /* System specification version */ - uint8_t Reserved1; /* Reserved */ - uint8_t TAAC; /* Data read access-time 1 */ - uint8_t NSAC; /* Data read access-time 2 in CLK cycles */ - uint8_t MaxBusClkFrec; /* Max. bus clock frequency */ - uint16_t CardComdClasses; /* Card command classes */ - uint8_t RdBlockLen; /* Max. read data block length */ - uint8_t PartBlockRead; /* Partial blocks for read allowed */ - uint8_t WrBlockMisalign; /* Write block misalignment */ - uint8_t RdBlockMisalign; /* Read block misalignment */ - uint8_t DSRImpl; /* DSR implemented */ - uint8_t Reserved2; /* Reserved */ - uint16_t DeviceSize; /* Device Size */ - uint8_t MaxRdCurrentVDDMin; /* Max. read current @ VDD min */ - uint8_t MaxRdCurrentVDDMax; /* Max. read current @ VDD max */ - uint8_t MaxWrCurrentVDDMin; /* Max. write current @ VDD min */ - uint8_t MaxWrCurrentVDDMax; /* Max. write current @ VDD max */ - uint8_t DeviceSizeMul; /* Device size multiplier */ - uint8_t EraseGrSize; /* Erase group size */ - uint8_t EraseGrMul; /* Erase group size multiplier */ - uint8_t WrProtectGrSize; /* Write protect group size */ - uint8_t WrProtectGrEnable; /* Write protect group enable */ - uint8_t ManDeflECC; /* Manufacturer default ECC */ - uint8_t WrSpeedFact; /* Write speed factor */ - uint8_t MaxWrBlockLen; /* Max. write data block length */ - uint8_t WriteBlockPaPartial; /* Partial blocks for write allowed */ - uint8_t Reserved3; /* Reserved */ - uint8_t ContentProtectAppli; /* Content protection application */ - uint8_t FileFormatGrouop; /* File format group */ - uint8_t CopyFlag; /* Copy flag (OTP) */ - uint8_t PermWrProtect; /* Permanent write protection */ - uint8_t TempWrProtect; /* Temporary write protection */ - uint8_t FileFormat; /* File Format */ - uint8_t ECC; /* ECC code */ - uint8_t msd_CRC; /* CRC */ - uint8_t Reserved4; /* always 1*/ + uint8_t CSDStruct; /* CSD structure */ + uint8_t SysSpecVersion; /* System specification version */ + uint8_t Reserved1; /* Reserved */ + uint8_t TAAC; /* Data read access-time 1 */ + uint8_t NSAC; /* Data read access-time 2 in CLK cycles */ + uint8_t MaxBusClkFrec; /* Max. bus clock frequency */ + uint16_t CardComdClasses; /* Card command classes */ + uint8_t RdBlockLen; /* Max. read data block length */ + uint8_t PartBlockRead; /* Partial blocks for read allowed */ + uint8_t WrBlockMisalign; /* Write block misalignment */ + uint8_t RdBlockMisalign; /* Read block misalignment */ + uint8_t DSRImpl; /* DSR implemented */ + uint8_t Reserved2; /* Reserved */ + uint16_t DeviceSize; /* Device Size */ + uint8_t MaxRdCurrentVDDMin; /* Max. read current @ VDD min */ + uint8_t MaxRdCurrentVDDMax; /* Max. read current @ VDD max */ + uint8_t MaxWrCurrentVDDMin; /* Max. write current @ VDD min */ + uint8_t MaxWrCurrentVDDMax; /* Max. write current @ VDD max */ + uint8_t DeviceSizeMul; /* Device size multiplier */ + uint8_t EraseGrSize; /* Erase group size */ + uint8_t EraseGrMul; /* Erase group size multiplier */ + uint8_t WrProtectGrSize; /* Write protect group size */ + uint8_t WrProtectGrEnable; /* Write protect group enable */ + uint8_t ManDeflECC; /* Manufacturer default ECC */ + uint8_t WrSpeedFact; /* Write speed factor */ + uint8_t MaxWrBlockLen; /* Max. write data block length */ + uint8_t WriteBlockPaPartial; /* Partial blocks for write allowed */ + uint8_t Reserved3; /* Reserved */ + uint8_t ContentProtectAppli; /* Content protection application */ + uint8_t FileFormatGrouop; /* File format group */ + uint8_t CopyFlag; /* Copy flag (OTP) */ + uint8_t PermWrProtect; /* Permanent write protection */ + uint8_t TempWrProtect; /* Temporary write protection */ + uint8_t FileFormat; /* File Format */ + uint8_t ECC; /* ECC code */ + uint8_t msd_CRC; /* CRC */ + uint8_t Reserved4; /* always 1*/ } SDCARDCsdTypeDef; /* Structure taken from Mass Storage Driver example provided by STM */ typedef struct { - uint8_t ManufacturerID; /* ManufacturerID */ - uint16_t OEM_AppliID; /* OEM/Application ID */ - char ProdName[6]; /* Product Name */ - uint8_t ProdRev; /* Product Revision */ - uint32_t ProdSN; /* Product Serial Number */ - uint8_t Reserved1; /* Reserved1 */ - uint16_t ManufactDate; /* Manufacturing Date */ - uint8_t msd_CRC; /* CRC */ - uint8_t Reserved2; /* always 1*/ + uint8_t ManufacturerID; /* ManufacturerID */ + uint16_t OEM_AppliID; /* OEM/Application ID */ + char ProdName[6]; /* Product Name */ + uint8_t ProdRev; /* Product Revision */ + uint32_t ProdSN; /* Product Serial Number */ + uint8_t Reserved1; /* Reserved1 */ + uint16_t ManufactDate; /* Manufacturing Date */ + uint8_t msd_CRC; /* CRC */ + uint8_t Reserved2; /* always 1*/ } SDCARDCidTypeDef; /* Global Variables */ -//extern VOLINFO PIOS_SDCARD_VolInfo; -//extern uint8_t PIOS_SDCARD_Sector[SECTOR_SIZE]; +// extern VOLINFO PIOS_SDCARD_VolInfo; +// extern uint8_t PIOS_SDCARD_Sector[SECTOR_SIZE]; /* Prototypes */ extern int32_t PIOS_SDCARD_Init(void); @@ -103,11 +103,11 @@ extern int32_t PIOS_SDCARD_IsMounted(); extern int32_t PIOS_SDCARD_MountFS(uint32_t StartupLog); extern int32_t PIOS_SDCARD_GetFree(void); -//extern int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len); -//extern int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len); +// extern int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len); +// extern int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len); extern int32_t PIOS_SDCARD_FileCopy(char *Source, char *Destination); extern int32_t PIOS_SDCARD_FileDelete(char *Filename); -#endif +#endif // if defined(PIOS_INCLUDE_SDCARD) #endif /* PIOS_SDCARD_H */ diff --git a/flight/pios/osx/inc/pios_servo.h b/flight/pios/osx/inc/pios_servo.h index 952baa5a0..5e10b57d8 100644 --- a/flight/pios/osx/inc/pios_servo.h +++ b/flight/pios/osx/inc/pios_servo.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_SERVO RC Servo Functions * @{ * - * @file pios_servo.h + * @file pios_servo.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RC Servo functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,12 +32,12 @@ /* Public Functions */ extern void PIOS_Servo_Init(void); -extern void PIOS_Servo_SetHz(const uint16_t * speeds, uint8_t num_banks); +extern void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t num_banks); extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position); #endif /* PIOS_SERVO_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_sim.h b/flight/pios/osx/inc/pios_sim.h index 79d6d5650..65d7eb9b5 100644 --- a/flight/pios/osx/inc/pios_sim.h +++ b/flight/pios/osx/inc/pios_sim.h @@ -1,10 +1,9 @@ - #ifndef PIOS_SIM_H #define PIOS_SIM_H int PIOS_SIM_Init(); int PIOS_SIM_Step(float dT); -void PIOS_SIM_SetActuator(float * actuator_int, int nchannels); +void PIOS_SIM_SetActuator(float *actuator_int, int nchannels); void PIOS_SIM_GetAccels(float *); void PIOS_SIM_GetGyros(float *); void PIOS_SIM_GetAttitude(float *); diff --git a/flight/pios/osx/inc/pios_sim_priv.h b/flight/pios/osx/inc/pios_sim_priv.h index 2d9aff845..c7dad004c 100644 --- a/flight/pios/osx/inc/pios_sim_priv.h +++ b/flight/pios/osx/inc/pios_sim_priv.h @@ -1,4 +1,3 @@ - #ifndef PIOS_SIM_PRIV_H #define PIOS_SIM_PRIV_H @@ -6,14 +5,14 @@ * State of inputs and outputs to the simulation model */ struct pios_sim_state { - float accels[3]; - float gyros[3]; - float mag[3]; - float baro[1]; - float q[4]; - float velocity[3]; - float position[3]; - float actuator[8]; + float accels[3]; + float gyros[3]; + float mag[3]; + float baro[1]; + float q[4]; + float velocity[3]; + float position[3]; + float actuator[8]; }; #endif /* PIOS_SIM_PRIV */ diff --git a/flight/pios/osx/inc/pios_struct_helper.h b/flight/pios/osx/inc/pios_struct_helper.h index 6196d9b26..24898914c 100644 --- a/flight/pios/osx/inc/pios_struct_helper.h +++ b/flight/pios/osx/inc/pios_struct_helper.h @@ -7,6 +7,8 @@ * @member: the name of the member within the struct. * */ -#define container_of(ptr, type, member) ({ \ - const typeof( ((type *)0)->member ) *__mptr = (ptr); \ - (type *)( (char *)__mptr - offsetof(type,member) );}) +#define container_of(ptr, type, member) \ + ({ \ + const typeof(((type *)0)->member) * __mptr = (ptr); \ + (type *)((char *)__mptr - offsetof(type, member)); } \ + ) diff --git a/flight/pios/osx/inc/pios_sys.h b/flight/pios/osx/inc/pios_sys.h index 7f183a892..a67411db6 100644 --- a/flight/pios/osx/inc/pios_sys.h +++ b/flight/pios/osx/inc/pios_sys.h @@ -6,26 +6,26 @@ * @brief PIOS System Initialization code * @{ * - * @file pios_sys.h + * @file pios_sys.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief System and hardware Init functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,18 +33,18 @@ #define PIOS_SYS_H #define PIOS_SYS_SERIAL_NUM_BINARY_LEN 12 -#define PIOS_SYS_SERIAL_NUM_ASCII_LEN (PIOS_SYS_SERIAL_NUM_BINARY_LEN * 2) +#define PIOS_SYS_SERIAL_NUM_ASCII_LEN (PIOS_SYS_SERIAL_NUM_BINARY_LEN * 2) /* Public Functions */ extern void PIOS_SYS_Init(void); extern int32_t PIOS_SYS_Reset(void); extern uint32_t PIOS_SYS_getCPUFlashSize(void); extern int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t array[PIOS_SYS_SERIAL_NUM_BINARY_LEN]); -extern int32_t PIOS_SYS_SerialNumberGet(char str[PIOS_SYS_SERIAL_NUM_ASCII_LEN+1]); +extern int32_t PIOS_SYS_SerialNumberGet(char str[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]); #endif /* PIOS_SYS_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_tcp.h b/flight/pios/osx/inc/pios_tcp.h index 589c90978..daa2d42af 100644 --- a/flight/pios/osx/inc/pios_tcp.h +++ b/flight/pios/osx/inc/pios_tcp.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_usart.h + * @file pios_usart.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief UDP functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/osx/inc/pios_tcp_priv.h b/flight/pios/osx/inc/pios_tcp_priv.h index a44301f2f..31ff754de 100644 --- a/flight/pios/osx/inc/pios_tcp_priv.h +++ b/flight/pios/osx/inc/pios_tcp_priv.h @@ -39,31 +39,31 @@ #include "fifo_buffer.h" struct pios_tcp_cfg { - const char *ip; - uint16_t port; + const char *ip; + uint16_t port; }; typedef struct { - const struct pios_tcp_cfg * cfg; - pthread_t rxThread; - - int socket; - struct sockaddr_in server; - struct sockaddr_in client; - uint32_t clientLength; - int socket_connection; - - pthread_cond_t cond; - pthread_mutex_t mutex; - - pios_com_callback tx_out_cb; - uint32_t tx_out_context; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - - t_fifo_buffer rx_fifo; - uint8_t rx_buffer[PIOS_TCP_RX_BUFFER_SIZE]; - uint8_t tx_buffer[PIOS_TCP_RX_BUFFER_SIZE]; + const struct pios_tcp_cfg *cfg; + pthread_t rxThread; + + int socket; + struct sockaddr_in server; + struct sockaddr_in client; + uint32_t clientLength; + int socket_connection; + + pthread_cond_t cond; + pthread_mutex_t mutex; + + pios_com_callback tx_out_cb; + uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + + t_fifo_buffer rx_fifo; + uint8_t rx_buffer[PIOS_TCP_RX_BUFFER_SIZE]; + uint8_t tx_buffer[PIOS_TCP_RX_BUFFER_SIZE]; } pios_tcp_dev; extern int32_t PIOS_TCP_Init(uint32_t *tcp_id, const struct pios_tcp_cfg *cfg); diff --git a/flight/pios/osx/inc/pios_udp.h b/flight/pios/osx/inc/pios_udp.h index 48a3b0987..28a095dce 100644 --- a/flight/pios/osx/inc/pios_udp.h +++ b/flight/pios/osx/inc/pios_udp.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_usart.h + * @file pios_usart.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief UDP functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/osx/inc/pios_udp_priv.h b/flight/pios/osx/inc/pios_udp_priv.h index 67ae42057..57516ee63 100644 --- a/flight/pios/osx/inc/pios_udp_priv.h +++ b/flight/pios/osx/inc/pios_udp_priv.h @@ -3,7 +3,7 @@ * * @file pios_udp_priv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief UDP private definitions. * @see The GNU Public License (GPL) Version 3 * @@ -39,33 +39,32 @@ #include struct pios_udp_cfg { - const char * ip; - uint16_t port; + const char *ip; + uint16_t port; }; typedef struct { - const struct pios_udp_cfg * cfg; - xTaskHandle rxThread; + const struct pios_udp_cfg *cfg; + xTaskHandle rxThread; - int socket; - struct sockaddr_in server; - struct sockaddr_in client; - uint32_t clientLength; + int socket; + struct sockaddr_in server; + struct sockaddr_in client; + uint32_t clientLength; - pthread_cond_t cond; - pthread_mutex_t mutex; + pthread_cond_t cond; + pthread_mutex_t mutex; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; - uint8_t rx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; - uint8_t tx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; + uint8_t rx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; + uint8_t tx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; } pios_udp_dev; -extern int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg); - +extern int32_t PIOS_UDP_Init(uint32_t *udp_id, const struct pios_udp_cfg *cfg); #endif /* PIOS_UDP_PRIV_H */ diff --git a/flight/pios/osx/inc/pios_wdg.h b/flight/pios/osx/inc/pios_wdg.h index 4d8f9d944..3337b67ff 100644 --- a/flight/pios/osx/inc/pios_wdg.h +++ b/flight/pios/osx/inc/pios_wdg.h @@ -7,7 +7,7 @@ * * @file pios_wdg.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief SPI functions header. * @see The GNU Public License (GPL) Version 3 * @@ -39,10 +39,10 @@ uint16_t PIOS_WDG_GetActiveFlags(); void PIOS_WDG_Clear(void); bool PIOS_WDG_Check(); -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_SENSORS 0x0010 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_SENSORS 0x0010 #endif diff --git a/flight/pios/osx/inc/sim_model.h b/flight/pios/osx/inc/sim_model.h index 60c15c487..16d28fa2f 100644 --- a/flight/pios/osx/inc/sim_model.h +++ b/flight/pios/osx/inc/sim_model.h @@ -1,6 +1,5 @@ - #include "pios_sim_priv.h" extern int sim_model_init(); extern int sim_model_terminate(); -extern int sim_model_step(float dT, struct pios_sim_state * state); +extern int sim_model_step(float dT, struct pios_sim_state *state); diff --git a/flight/pios/osx/osx/pios_bl_helper.c b/flight/pios/osx/osx/pios_bl_helper.c index a7920e5d2..ec122564b 100644 --- a/flight/pios/osx/osx/pios_bl_helper.c +++ b/flight/pios/osx/osx/pios_bl_helper.c @@ -35,19 +35,19 @@ uint32_t PIOS_BL_HELPER_CRC_Memory_Calc() { - return 0; + return 0; } extern const struct fw_version_info fw_version_blob; -void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t * array, uint8_t size) +void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t *array, uint8_t size) { - uint8_t * desc = (uint8_t *) &fw_version_blob; - for (uint32_t i = 0; i < size; i++) { - array[i] = desc[i]; - } + uint8_t *desc = (uint8_t *)&fw_version_blob; + + for (uint32_t i = 0; i < size; i++) { + array[i] = desc[i]; + } } void PIOS_BL_HELPER_CRC_Ini() -{ -} +{} #endif diff --git a/flight/pios/osx/osx/pios_board_info.c b/flight/pios/osx/osx/pios_board_info.c index c273a881d..2ef651a21 100644 --- a/flight/pios/osx/osx/pios_board_info.c +++ b/flight/pios/osx/osx/pios_board_info.c @@ -4,17 +4,17 @@ #include "pios_board_info.h" const struct pios_board_info pios_board_info_blob = { - .magic = PIOS_BOARD_INFO_BLOB_MAGIC, - .board_type = BOARD_TYPE, - .board_rev = BOARD_REVISION, - .bl_rev = BOOTLOADER_VERSION, - .hw_type = HW_TYPE, - .fw_base = FW_BANK_BASE, - .fw_size = FW_BANK_SIZE - FW_DESC_SIZE, - .desc_base = FW_BANK_BASE + FW_BANK_SIZE - FW_DESC_SIZE, - .desc_size = FW_DESC_SIZE, + .magic = PIOS_BOARD_INFO_BLOB_MAGIC, + .board_type = BOARD_TYPE, + .board_rev = BOARD_REVISION, + .bl_rev = BOOTLOADER_VERSION, + .hw_type = HW_TYPE, + .fw_base = FW_BANK_BASE, + .fw_size = FW_BANK_SIZE - FW_DESC_SIZE, + .desc_base = FW_BANK_BASE + FW_BANK_SIZE - FW_DESC_SIZE, + .desc_size = FW_DESC_SIZE, #ifdef EE_BANK_BASE - .ee_base = EE_BANK_BASE, - .ee_size = EE_BANK_SIZE, + .ee_base = EE_BANK_BASE, + .ee_size = EE_BANK_SIZE, #endif }; diff --git a/flight/pios/osx/osx/pios_com.c b/flight/pios/osx/osx/pios_com.c index 4f2e1e3c9..8eab84a4e 100644 --- a/flight/pios/osx/osx/pios_com.c +++ b/flight/pios/osx/osx/pios_com.c @@ -6,26 +6,26 @@ * @brief Hardware communication layer * @{ * - * @file pios_com.c + * @file pios_com.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM layer functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,462 +38,470 @@ #include #if !defined(PIOS_INCLUDE_FREERTOS) -#include "pios_delay.h" /* PIOS_DELAY_WaitmS */ +#include "pios_delay.h" /* PIOS_DELAY_WaitmS */ #endif enum pios_com_dev_magic { - PIOS_COM_DEV_MAGIC = 0xaa55aa55, + PIOS_COM_DEV_MAGIC = 0xaa55aa55, }; struct pios_com_dev { - enum pios_com_dev_magic magic; - uint32_t lower_id; - const struct pios_com_driver * driver; + enum pios_com_dev_magic magic; + uint32_t lower_id; + const struct pios_com_driver *driver; #if defined(PIOS_INCLUDE_FREERTOS) - xSemaphoreHandle tx_sem; - xSemaphoreHandle rx_sem; + xSemaphoreHandle tx_sem; + xSemaphoreHandle rx_sem; #endif - bool has_rx; - bool has_tx; + bool has_rx; + bool has_tx; - t_fifo_buffer rx; - t_fifo_buffer tx; + t_fifo_buffer rx; + t_fifo_buffer tx; }; -static bool PIOS_COM_validate(struct pios_com_dev * com_dev) +static bool PIOS_COM_validate(struct pios_com_dev *com_dev) { - return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC)); + return com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) && 0 // static struct pios_com_dev * PIOS_COM_alloc(void) -//{ -// struct pios_com_dev * com_dev; +// { +// struct pios_com_dev * com_dev; // -// com_dev = (struct pios_com_dev *)malloc(sizeof(*com_dev)); -// if (!com_dev) return (NULL); +// com_dev = (struct pios_com_dev *)malloc(sizeof(*com_dev)); +// if (!com_dev) return (NULL); // -// com_dev->magic = PIOS_COM_DEV_MAGIC; -// return(com_dev); -//} +// com_dev->magic = PIOS_COM_DEV_MAGIC; +// return(com_dev); +// } #else static struct pios_com_dev pios_com_devs[PIOS_COM_MAX_DEVS]; static uint8_t pios_com_num_devs; static uint32_t PIOS_COM_create(void) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) { - return (PIOS_COM_MAX_DEVS+1); - } + if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) { + return PIOS_COM_MAX_DEVS + 1; + } - com_dev = &pios_com_devs[pios_com_num_devs++]; - com_dev->magic = PIOS_COM_DEV_MAGIC; + com_dev = &pios_com_devs[pios_com_num_devs++]; + com_dev->magic = PIOS_COM_DEV_MAGIC; - return (pios_com_num_devs); + return pios_com_num_devs; } -static struct pios_com_dev * PIOS_COM_find_dev(uint32_t com_dev_id) +static struct pios_com_dev *PIOS_COM_find_dev(uint32_t com_dev_id) { - if (!com_dev_id) return NULL; - if (com_dev_id>pios_com_num_devs+1) return NULL; - return &pios_com_devs[com_dev_id-1]; + if (!com_dev_id) { + return NULL; + } + if (com_dev_id > pios_com_num_devs + 1) { + return NULL; + } + return &pios_com_devs[com_dev_id - 1]; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) && 0 */ -static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); -static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); -static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield); -static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield); +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); +static void PIOS_COM_UnblockRx(struct pios_com_dev *com_dev, bool *need_yield); +static void PIOS_COM_UnblockTx(struct pios_com_dev *com_dev, bool *need_yield); /** - * Initialises COM layer - * \param[out] handle - * \param[in] driver - * \param[in] id - * \return < 0 if initialisation failed - */ -int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len) + * Initialises COM layer + * \param[out] handle + * \param[in] driver + * \param[in] id + * \return < 0 if initialisation failed + */ +int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len) { - PIOS_Assert(com_id); - PIOS_Assert(driver); + PIOS_Assert(com_id); + PIOS_Assert(driver); - bool has_rx = (rx_buffer && rx_buffer_len > 0); - bool has_tx = (tx_buffer && tx_buffer_len > 0); - PIOS_Assert(has_rx || has_tx); - PIOS_Assert(driver->bind_tx_cb || !has_tx); - PIOS_Assert(driver->bind_rx_cb || !has_rx); + bool has_rx = (rx_buffer && rx_buffer_len > 0); + bool has_tx = (tx_buffer && tx_buffer_len > 0); + PIOS_Assert(has_rx || has_tx); + PIOS_Assert(driver->bind_tx_cb || !has_tx); + PIOS_Assert(driver->bind_rx_cb || !has_rx); - uint32_t com_dev_id; - struct pios_com_dev * com_dev; + uint32_t com_dev_id; + struct pios_com_dev *com_dev; - com_dev_id = PIOS_COM_create(); - com_dev = PIOS_COM_find_dev(com_dev_id); - if (!com_dev) goto out_fail; + com_dev_id = PIOS_COM_create(); + com_dev = PIOS_COM_find_dev(com_dev_id); + if (!com_dev) { + goto out_fail; + } - com_dev->driver = driver; - com_dev->lower_id = lower_id; + com_dev->driver = driver; + com_dev->lower_id = lower_id; - com_dev->has_rx = has_rx; - com_dev->has_tx = has_tx; + com_dev->has_rx = has_rx; + com_dev->has_tx = has_tx; - if (has_rx) { - fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len); + if (has_rx) { + fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len); #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(com_dev->rx_sem); -#endif /* PIOS_INCLUDE_FREERTOS */ - (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, com_dev_id); - if (com_dev->driver->rx_start) { - /* Start the receiver */ - (com_dev->driver->rx_start)(com_dev->lower_id, - fifoBuf_getFree(&com_dev->rx)); - } - } + vSemaphoreCreateBinary(com_dev->rx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, com_dev_id); + if (com_dev->driver->rx_start) { + /* Start the receiver */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } + } - if (has_tx) { - fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); + if (has_tx) { + fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(com_dev->tx_sem); -#endif /* PIOS_INCLUDE_FREERTOS */ - (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, com_dev_id); - } + vSemaphoreCreateBinary(com_dev->tx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, com_dev_id); + } - *com_id = com_dev_id; - return(0); + *com_id = com_dev_id; + return 0; out_fail: - return(-1); + return -1; } -static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield) +static void PIOS_COM_UnblockRx(struct pios_com_dev *com_dev, bool *need_yield) { #if defined(PIOS_INCLUDE_FREERTOS) - static signed portBASE_TYPE xHigherPriorityTaskWoken; - xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken); + static signed portBASE_TYPE xHigherPriorityTaskWoken; + xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken != pdFALSE) { - *need_yield = true; - } else { - *need_yield = false; - } + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } #else - *need_yield = false; + *need_yield = false; #endif } -static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield) +static void PIOS_COM_UnblockTx(struct pios_com_dev *com_dev, bool *need_yield) { #if defined(PIOS_INCLUDE_FREERTOS) - static signed portBASE_TYPE xHigherPriorityTaskWoken; - xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken); + static signed portBASE_TYPE xHigherPriorityTaskWoken; + xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken != pdFALSE) { - *need_yield = true; - } else { - *need_yield = false; - } + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } #else - *need_yield = false; + *need_yield = false; #endif } -static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(context); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(context); - bool valid = PIOS_COM_validate(com_dev); - PIOS_Assert(valid); - PIOS_Assert(com_dev->has_rx); + bool valid = PIOS_COM_validate(com_dev); - PIOS_IRQ_Disable(); - uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len); - PIOS_IRQ_Enable(); + PIOS_Assert(valid); + PIOS_Assert(com_dev->has_rx); - if (bytes_into_fifo > 0) { - /* Data has been added to the buffer */ - PIOS_COM_UnblockRx(com_dev, need_yield); - } + PIOS_IRQ_Disable(); + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len); + PIOS_IRQ_Enable(); - if (headroom) { - *headroom = fifoBuf_getFree(&com_dev->rx); - } + if (bytes_into_fifo > 0) { + /* Data has been added to the buffer */ + PIOS_COM_UnblockRx(com_dev, need_yield); + } - return (bytes_into_fifo); + if (headroom) { + *headroom = fifoBuf_getFree(&com_dev->rx); + } + + return bytes_into_fifo; } -static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(context); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(context); - bool valid = PIOS_COM_validate(com_dev); - PIOS_Assert(valid); - PIOS_Assert(buf); - PIOS_Assert(buf_len); - PIOS_Assert(com_dev->has_tx); + bool valid = PIOS_COM_validate(com_dev); - PIOS_IRQ_Disable(); - uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len); - PIOS_IRQ_Enable(); + PIOS_Assert(valid); + PIOS_Assert(buf); + PIOS_Assert(buf_len); + PIOS_Assert(com_dev->has_tx); - if (bytes_from_fifo > 0) { - /* More space has been made in the buffer */ - PIOS_COM_UnblockTx(com_dev, need_yield); - } + PIOS_IRQ_Disable(); + uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len); + PIOS_IRQ_Enable(); - if (headroom) { - *headroom = fifoBuf_getUsed(&com_dev->tx); - } + if (bytes_from_fifo > 0) { + /* More space has been made in the buffer */ + PIOS_COM_UnblockTx(com_dev, need_yield); + } - return (bytes_from_fifo); + if (headroom) { + *headroom = fifoBuf_getUsed(&com_dev->tx); + } + + return bytes_from_fifo; } /** -* Change the port speed without re-initializing -* \param[in] port COM port -* \param[in] baud Requested baud rate -* \return -1 if port not available -* \return 0 on success -*/ + * Change the port speed without re-initializing + * \param[in] port COM port + * \param[in] baud Requested baud rate + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - /* Invoke the driver function if it exists */ - if (com_dev->driver->set_baud) { - com_dev->driver->set_baud(com_dev->lower_id, baud); - } + /* Invoke the driver function if it exists */ + if (com_dev->driver->set_baud) { + com_dev->driver->set_baud(com_dev->lower_id, baud); + } - return 0; + return 0; } /** -* Sends a package over given port -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a package over given port + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - PIOS_Assert(com_dev->has_tx); + PIOS_Assert(com_dev->has_tx); - if (len >= fifoBuf_getFree(&com_dev->tx)) { - /* Buffer cannot accept all requested bytes (retry) */ - return -2; - } + if (len >= fifoBuf_getFree(&com_dev->tx)) { + /* Buffer cannot accept all requested bytes (retry) */ + return -2; + } - PIOS_IRQ_Disable(); - uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len); - PIOS_IRQ_Enable(); + PIOS_IRQ_Disable(); + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len); + PIOS_IRQ_Enable(); - if (bytes_into_fifo > 0) { - /* More data has been put in the tx buffer, make sure the tx is started */ - if (com_dev->driver->tx_start) { - com_dev->driver->tx_start(com_dev->lower_id, - fifoBuf_getUsed(&com_dev->tx)); - } - } + if (bytes_into_fifo > 0) { + /* More data has been put in the tx buffer, make sure the tx is started */ + if (com_dev->driver->tx_start) { + com_dev->driver->tx_start(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } + } - return (0); + return 0; } /** -* Sends a package over given port -* (blocking function) -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a package over given port + * (blocking function) + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - PIOS_Assert(com_dev->has_tx); + PIOS_Assert(com_dev->has_tx); - int32_t rc; - do { - rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len); + int32_t rc; + do { + rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len); #if defined(PIOS_INCLUDE_FREERTOS) - if (rc == -2) { - /* Make sure the transmitter is running while we wait */ - if (com_dev->driver->tx_start) { - (com_dev->driver->tx_start)(com_dev->lower_id, - fifoBuf_getUsed(&com_dev->tx)); - } - if (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) { - return -3; - } - } + if (rc == -2) { + /* Make sure the transmitter is running while we wait */ + if (com_dev->driver->tx_start) { + (com_dev->driver->tx_start)(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } + if (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) { + return -3; + } + } #endif - } while (rc == -2); + } while (rc == -2); - return rc; + return rc; } /** -* Sends a single character over given port -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a single character over given port + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c) { - return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1); + return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1); } /** -* Sends a single character over given port -* (blocking function) -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a single character over given port + * (blocking function) + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendChar(uint32_t com_id, char c) { - return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1); + return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1); } /** -* Sends a string over given port -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a string over given port + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str) { - return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str)); + return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str)); } /** -* Sends a string over given port -* (blocking function) -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a string over given port + * (blocking function) + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendString(uint32_t com_id, const char *str) { - return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str)); + return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str)); } /** -* Sends a formatted string (-> printf) over given port -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* 128 characters supported maximum! -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * 128 characters supported maximum! + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Sends a formatted string (-> printf) over given port -* (blocking function) -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * (blocking function) + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Transfer bytes from port buffers into another buffer -* \param[in] port COM port -* \returns Byte from buffer -*/ -uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms) + * Transfer bytes from port buffers into another buffer + * \param[in] port COM port + * \returns Byte from buffer + */ +uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t *buf, uint16_t buf_len, uint32_t timeout_ms) { - PIOS_Assert(buf); - PIOS_Assert(buf_len); + PIOS_Assert(buf); + PIOS_Assert(buf_len); - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - PIOS_Assert(0); - } - PIOS_Assert(com_dev->has_rx); + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + PIOS_Assert(0); + } + PIOS_Assert(com_dev->has_rx); - check_again: - PIOS_IRQ_Disable(); - uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len); - PIOS_IRQ_Enable(); +check_again: + PIOS_IRQ_Disable(); + uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len); + PIOS_IRQ_Enable(); - if (bytes_from_fifo == 0 && timeout_ms > 0) { - /* No more bytes in receive buffer */ - /* Make sure the receiver is running while we wait */ - if (com_dev->driver->rx_start) { - /* Notify the lower layer that there is now room in the rx buffer */ - (com_dev->driver->rx_start)(com_dev->lower_id, - fifoBuf_getFree(&com_dev->rx)); - } + if (bytes_from_fifo == 0 && timeout_ms > 0) { + /* No more bytes in receive buffer */ + /* Make sure the receiver is running while we wait */ + if (com_dev->driver->rx_start) { + /* Notify the lower layer that there is now room in the rx buffer */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) { - /* Make sure we don't come back here again */ - timeout_ms = 0; - goto check_again; - } + if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) { + /* Make sure we don't come back here again */ + timeout_ms = 0; + goto check_again; + } #else - PIOS_DELAY_WaitmS(1); - timeout_ms--; - goto check_again; + PIOS_DELAY_WaitmS(1); + timeout_ms--; + goto check_again; #endif - } + } - /* Return received byte */ - return (bytes_from_fifo); + /* Return received byte */ + return bytes_from_fifo; } /** @@ -503,21 +511,22 @@ uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len */ bool PIOS_COM_Available(uint32_t com_id) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - return false; - } + if (!PIOS_COM_validate(com_dev)) { + return false; + } - // If a driver does not provide a query method assume always - // available if valid - if (com_dev->driver->available == NULL) - return true; + // If a driver does not provide a query method assume always + // available if valid + if (com_dev->driver->available == NULL) { + return true; + } - return (com_dev->driver->available)(com_dev->lower_id); + return (com_dev->driver->available)(com_dev->lower_id); } -#endif +#endif /* if defined(PIOS_INCLUDE_COM) */ /** * @} diff --git a/flight/pios/osx/osx/pios_crc.c b/flight/pios/osx/osx/pios_crc.c index 548ba2648..0e4cd5dec 100644 --- a/flight/pios/osx/osx/pios_crc.c +++ b/flight/pios/osx/osx/pios_crc.c @@ -11,22 +11,22 @@ // CRC lookup table static const uint8_t crc_table[256] = { - 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, - 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, - 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, - 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, - 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, - 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, - 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, - 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, - 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, - 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, - 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, - 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, - 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, - 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, - 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, - 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 + 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, + 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, + 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, + 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, + 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, + 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, + 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, + 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, + 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, + 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, + 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, + 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, + 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, + 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, + 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, + 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 }; /** @@ -49,7 +49,7 @@ static const uint8_t crc_table[256] = { */ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) { - return crc_table[crc ^ data]; + return crc_table[crc ^ data]; } /* @@ -59,16 +59,16 @@ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) * @param[in] length Number of bytes to process * @returns Updated CRC */ -uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length) +uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t *data, int32_t length) { - // use registers for speed - register int32_t len = length; - register uint8_t crc8 = crc; - register const uint8_t *p = data; - - while (len--) - crc8 = crc_table[crc8 ^ *p++]; - - return crc8; -} + // use registers for speed + register int32_t len = length; + register uint8_t crc8 = crc; + register const uint8_t *p = data; + while (len--) { + crc8 = crc_table[crc8 ^ *p++]; + } + + return crc8; +} diff --git a/flight/pios/osx/osx/pios_debug.c b/flight/pios/osx/osx/pios_debug.c index 5956a9d04..71120a39d 100644 --- a/flight/pios/osx/osx/pios_debug.c +++ b/flight/pios/osx/osx/pios_debug.c @@ -10,21 +10,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Debugging Functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,36 +37,31 @@ const char *PIOS_DEBUG_AssertMsg = "ASSERT FAILED"; /* Private Function Prototypes */ /** -* Initialise Debug-features -*/ + * Initialise Debug-features + */ void PIOS_DEBUG_Init(void) -{ -} +{} /** -* Set debug-pin high -* \param pin 0 for S1 output -*/ + * Set debug-pin high + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinHigh(uint8_t Pin) -{ -} +{} /** -* Set debug-pin low -* \param pin 0 for S1 output -*/ + * Set debug-pin low + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinLow(uint8_t Pin) -{ -} +{} void PIOS_DEBUG_PinValue8Bit(uint8_t value) -{ -} +{} void PIOS_DEBUG_PinValue4BitL(uint8_t value) -{ -} +{} /** @@ -75,22 +70,21 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value) void PIOS_DEBUG_Panic(const char *msg) { #ifdef PIOS_COM_DEBUG - register int *lr asm("lr"); // Link-register holds the PC of the caller - PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s @0x%x\r", msg, lr); + register int *lr asm ("lr"); // Link-register holds the PC of the caller + PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s @0x%x\r", msg, lr); #endif - // tell the user whats going on on commandline too - fprintf(stderr,"CRITICAL ERROR: %s\n",msg); - - // this helps debugging: causing a div by zero allows a backtrace - // and/or ends execution - int b = 0; - int a = (2/b); - b=a; + // tell the user whats going on on commandline too + fprintf(stderr, "CRITICAL ERROR: %s\n", msg); + // this helps debugging: causing a div by zero allows a backtrace + // and/or ends execution + int b = 0; + int a = (2 / b); + b = a; } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/osx/pios_delay.c b/flight/pios/osx/osx/pios_delay.c index eb4082268..a42d9023a 100644 --- a/flight/pios/osx/osx/pios_delay.c +++ b/flight/pios/osx/osx/pios_delay.c @@ -3,27 +3,27 @@ * * @file pios_delay.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) - * @brief Delay Functions - * - Provides a micro-second granular delay using a TIM + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * @brief Delay Functions + * - Provides a micro-second granular delay using a TIM * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_DELAY Delay Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,78 +35,82 @@ #if defined(PIOS_INCLUDE_DELAY) /** -* Initialises the Timer used by PIOS_DELAY functions
-* This is called from pios.c as part of the main() function -* at system start up. -* \return < 0 if initialisation failed -*/ + * Initialises the Timer used by PIOS_DELAY functions
+ * This is called from pios.c as part of the main() function + * at system start up. + * \return < 0 if initialisation failed + */ #include int32_t PIOS_DELAY_Init(void) { - // stub + // stub - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Waits for a specific number of uS
-* Example:
-* \code -* // Wait for 500 uS -* PIOS_DELAY_Wait_uS(500); -* \endcode -* \param[in] uS delay (1..65535 microseconds) -* \return < 0 on errors -*/ + * Waits for a specific number of uS
+ * Example:
+ * \code + * // Wait for 500 uS + * PIOS_DELAY_Wait_uS(500); + * \endcode + * \param[in] uS delay (1..65535 microseconds) + * \return < 0 on errors + */ int32_t PIOS_DELAY_WaituS(uint32_t uS) { - struct timespec wait,rest; - wait.tv_sec=0; - wait.tv_nsec=1000*uS; - while (!nanosleep(&wait,&rest)) { - wait=rest; - } + struct timespec wait, rest; - /* No error */ - return 0; + wait.tv_sec = 0; + wait.tv_nsec = 1000 * uS; + while (!nanosleep(&wait, &rest)) { + wait = rest; + } + + /* No error */ + return 0; } /** -* Waits for a specific number of mS
-* Example:
-* \code -* // Wait for 500 mS -* PIOS_DELAY_Wait_mS(500); -* \endcode -* \param[in] mS delay (1..65535 milliseconds) -* \return < 0 on errors -*/ + * Waits for a specific number of mS
+ * Example:
+ * \code + * // Wait for 500 mS + * PIOS_DELAY_Wait_mS(500); + * \endcode + * \param[in] mS delay (1..65535 milliseconds) + * \return < 0 on errors + */ int32_t PIOS_DELAY_WaitmS(uint32_t mS) { - struct timespec wait,rest; - wait.tv_sec=mS/1000; - wait.tv_nsec=(mS%1000)*1000000; - while (!nanosleep(&wait,&rest)) { - wait=rest; - } + struct timespec wait, rest; - /* No error */ - return 0; + wait.tv_sec = mS / 1000; + wait.tv_nsec = (mS % 1000) * 1000000; + while (!nanosleep(&wait, &rest)) { + wait = rest; + } + + /* No error */ + return 0; } uint32_t PIOS_DELAY_GetRaw() { - uint32_t raw_us = clock(); - return raw_us; + uint32_t raw_us = clock(); + + return raw_us; } uint32_t PIOS_DELAY_DiffuS(uint32_t ref) { - uint32_t diff_clock = clock() - ref; - uint32_t diff_us = diff_clock; // (CLOCKS_PER_SEC / 1000); - return diff_us; + uint32_t diff_clock = clock() - ref; + uint32_t diff_us = diff_clock; // (CLOCKS_PER_SEC / 1000); + + return diff_us; } -#endif +#endif /* if defined(PIOS_INCLUDE_DELAY) */ diff --git a/flight/pios/osx/osx/pios_gcsrcvr.c b/flight/pios/osx/osx/pios_gcsrcvr.c index f9c19cca3..1618c27c0 100644 --- a/flight/pios/osx/osx/pios_gcsrcvr.c +++ b/flight/pios/osx/osx/pios_gcsrcvr.c @@ -41,19 +41,19 @@ static GCSReceiverData gcsreceiverdata; static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver = { - .read = PIOS_GCSRCVR_Get, + .read = PIOS_GCSRCVR_Get, }; /* Local Variables */ enum pios_gcsrcvr_dev_magic { - PIOS_GCSRCVR_DEV_MAGIC = 0xe9da5c56, + PIOS_GCSRCVR_DEV_MAGIC = 0xe9da5c56, }; struct pios_gcsrcvr_dev { - enum pios_gcsrcvr_dev_magic magic; + enum pios_gcsrcvr_dev_magic magic; - uint8_t supv_timer; - bool Fresh; + uint8_t supv_timer; + bool Fresh; }; static struct pios_gcsrcvr_dev *global_gcsrcvr_dev; @@ -61,79 +61,83 @@ static struct pios_gcsrcvr_dev *global_gcsrcvr_dev; #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void) { - struct pios_gcsrcvr_dev * gcsrcvr_dev; + struct pios_gcsrcvr_dev *gcsrcvr_dev; - gcsrcvr_dev = (struct pios_gcsrcvr_dev *)pvPortMalloc(sizeof(*gcsrcvr_dev)); - if (!gcsrcvr_dev) return(NULL); + gcsrcvr_dev = (struct pios_gcsrcvr_dev *)pvPortMalloc(sizeof(*gcsrcvr_dev)); + if (!gcsrcvr_dev) { + return NULL; + } - gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; - gcsrcvr_dev->Fresh = FALSE; - gcsrcvr_dev->supv_timer = 0; + gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; + gcsrcvr_dev->Fresh = FALSE; + gcsrcvr_dev->supv_timer = 0; - /* The update callback cannot receive the device pointer, so set it in a global */ - global_gcsrcvr_dev = gcsrcvr_dev; + /* The update callback cannot receive the device pointer, so set it in a global */ + global_gcsrcvr_dev = gcsrcvr_dev; - return(gcsrcvr_dev); + return gcsrcvr_dev; } #else static struct pios_gcsrcvr_dev pios_gcsrcvr_devs[PIOS_GCSRCVR_MAX_DEVS]; static uint8_t pios_gcsrcvr_num_devs; static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void) { - struct pios_gcsrcvr_dev *gcsrcvr_dev; + struct pios_gcsrcvr_dev *gcsrcvr_dev; - if (pios_gcsrcvr_num_devs >= PIOS_GCSRCVR_MAX_DEVS) { - return (NULL); - } + if (pios_gcsrcvr_num_devs >= PIOS_GCSRCVR_MAX_DEVS) { + return NULL; + } - gcsrcvr_dev = &pios_gcsrcvr_devs[pios_gcsrcvr_num_devs++]; - gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; - gcsrcvr_dev->Fresh = FALSE; - gcsrcvr_dev->supv_timer = 0; + gcsrcvr_dev = &pios_gcsrcvr_devs[pios_gcsrcvr_num_devs++]; + gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; + gcsrcvr_dev->Fresh = FALSE; + gcsrcvr_dev->supv_timer = 0; - global_gcsrcvr_dev = gcsrcvr_dev; + global_gcsrcvr_dev = gcsrcvr_dev; - return (gcsrcvr_dev); + return gcsrcvr_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void gcsreceiver_updated(UAVObjEvent * ev) +static void gcsreceiver_updated(UAVObjEvent *ev) { - struct pios_gcsrcvr_dev *gcsrcvr_dev = global_gcsrcvr_dev; - if (ev->obj == GCSReceiverHandle()) { - GCSReceiverGet(&gcsreceiverdata); - gcsrcvr_dev->Fresh = TRUE; - } + struct pios_gcsrcvr_dev *gcsrcvr_dev = global_gcsrcvr_dev; + + if (ev->obj == GCSReceiverHandle()) { + GCSReceiverGet(&gcsreceiverdata); + gcsrcvr_dev->Fresh = TRUE; + } } extern int32_t PIOS_GCSRCVR_Init(uint32_t *gcsrcvr_id) { - struct pios_gcsrcvr_dev *gcsrcvr_dev; + struct pios_gcsrcvr_dev *gcsrcvr_dev; - /* Allocate the device structure */ - gcsrcvr_dev = (struct pios_gcsrcvr_dev *)PIOS_gcsrcvr_alloc(); - if (!gcsrcvr_dev) - return -1; + /* Allocate the device structure */ + gcsrcvr_dev = (struct pios_gcsrcvr_dev *)PIOS_gcsrcvr_alloc(); + if (!gcsrcvr_dev) { + return -1; + } - /* Register uavobj callback */ - GCSReceiverConnectCallback (gcsreceiver_updated); + /* Register uavobj callback */ + GCSReceiverConnectCallback(gcsreceiver_updated); - return 0; + return 0; } static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel) { - if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) { - /* channel is out of range */ - return -1; - } + if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) { + /* channel is out of range */ + return -1; + } - return (gcsreceiverdata.Channel[channel]); + return gcsreceiverdata.Channel[channel]; } -#endif /* PIOS_INCLUDE_GCSRCVR */ +#endif /* PIOS_INCLUDE_GCSRCVR */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/osx/osx/pios_iap.c b/flight/pios/osx/osx/pios_iap.c index aef9f6c54..e38bf1d6d 100644 --- a/flight/pios/osx/osx/pios_iap.c +++ b/flight/pios/osx/osx/pios_iap.c @@ -1,6 +1,6 @@ /*! - * @File iap.c - * @Brief + * @File iap.c + * @Brief * * Created on: Sep 6, 2010 * Author: joe @@ -8,8 +8,8 @@ /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ #include /*! @@ -20,50 +20,42 @@ * * Created: Sep 8, 2010 10:10:48 PM by joe */ -void PIOS_IAP_Init( void ) -{ - -} +void PIOS_IAP_Init(void) +{} /*! * \brief Determines if an In-Application-Programming request has been made. * \param *comm - Which communication stream to use for the IAP (USB, Telemetry, I2C, SPI, etc) * \return TRUE - if correct sequence found, along with 'comm' updated. - * FALSE - Note that 'comm' will have an invalid comm identifier. + * FALSE - Note that 'comm' will have an invalid comm identifier. * \retval * */ -uint32_t PIOS_IAP_CheckRequest( void ) +uint32_t PIOS_IAP_CheckRequest(void) { - - return false; + return false; } - /*! * \brief Sets the 1st word of the request sequence. * \param n/a * \return n/a * \retval */ -void PIOS_IAP_SetRequest1(void) -{ -} +void PIOS_IAP_SetRequest1(void) +{} -void PIOS_IAP_SetRequest2(void) -{ -} +void PIOS_IAP_SetRequest2(void) +{} -void PIOS_IAP_ClearRequest(void) -{ -} +void PIOS_IAP_ClearRequest(void) +{} uint16_t PIOS_IAP_ReadBootCount(void) { - return 0; + return 0; } -void PIOS_IAP_WriteBootCount (uint16_t boot_count) -{ -} +void PIOS_IAP_WriteBootCount(uint16_t boot_count) +{} diff --git a/flight/pios/osx/osx/pios_irq.c b/flight/pios/osx/osx/pios_irq.c index b30d09fee..793df9ccf 100644 --- a/flight/pios/osx/osx/pios_irq.c +++ b/flight/pios/osx/osx/pios_irq.c @@ -6,26 +6,26 @@ * @brief STM32 Hardware code to enable and disable interrupts * @{ * - * @file pios_irq.c + * @file pios_irq.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief IRQ Enable/Disable routines * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,33 +37,33 @@ /* Private Function Prototypes */ /** -* Disables all interrupts (nested) -* \return < 0 On errors -*/ + * Disables all interrupts (nested) + * \return < 0 On errors + */ int32_t PIOS_IRQ_Disable(void) { #if defined(PIOS_INCLUDE_FREERTOS) - taskENTER_CRITICAL(); + taskENTER_CRITICAL(); #endif - return 0; + return 0; } /** -* Enables all interrupts (nested) -* \return < 0 on errors -* \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) -*/ + * Enables all interrupts (nested) + * \return < 0 on errors + * \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) + */ int32_t PIOS_IRQ_Enable(void) { #if defined(PIOS_INCLUDE_FREERTOS) - taskEXIT_CRITICAL(); + taskEXIT_CRITICAL(); #endif - return 0; + return 0; } -#endif +#endif /* if defined(PIOS_INCLUDE_IRQ) */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/osx/pios_led.c b/flight/pios/osx/osx/pios_led.c index 770e8b2c4..912929cbe 100644 --- a/flight/pios/osx/osx/pios_led.c +++ b/flight/pios/osx/osx/pios_led.c @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file pios_led.c + * @file pios_led.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief LED functions, init, toggle, on & off. * @see The GNU Public License (GPL) Version 3 @@ -9,19 +9,19 @@ * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,49 +38,50 @@ static uint8_t LED_GPIO[PIOS_LED_NUM]; -static inline void PIOS_SetLED(uint32_t LED,uint8_t stat) { - //printf("PIOS: LED %i status %i\n",LED,stat); - LED_GPIO[LED]=stat; +static inline void PIOS_SetLED(uint32_t LED, uint8_t stat) +{ + // printf("PIOS: LED %i status %i\n",LED,stat); + LED_GPIO[LED] = stat; } /** -* Initialises all the LED's -*/ + * Initialises all the LED's + */ void PIOS_LED_Init(void) { - for(int LEDNum = 0; LEDNum < PIOS_LED_NUM; LEDNum++) { - LED_GPIO[LEDNum]=0; - } + for (int LEDNum = 0; LEDNum < PIOS_LED_NUM; LEDNum++) { + LED_GPIO[LEDNum] = 0; + } } /** -* Turn on LED -* \param[in] LED LED Name (LED1, LED2) -*/ + * Turn on LED + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_On(uint32_t led) { - PIOS_SetLED(led,1); + PIOS_SetLED(led, 1); } /** -* Turn off LED -* \param[in] LED LED Name (LED1, LED2) -*/ + * Turn off LED + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_Off(uint32_t led) { - PIOS_SetLED(led,0); + PIOS_SetLED(led, 0); } /** -* Toggle LED on/off -* \param[in] LED LED Name (LED1, LED2) -*/ + * Toggle LED on/off + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_Toggle(uint32_t led) { - PIOS_SetLED(led,LED_GPIO[led]?0:1); + PIOS_SetLED(led, LED_GPIO[led] ? 0 : 1); } -#endif +#endif /* if defined(PIOS_INCLUDE_LED) */ diff --git a/flight/pios/osx/osx/pios_rcvr.c b/flight/pios/osx/osx/pios_rcvr.c index 0a2a8cd87..87d9b2e9b 100644 --- a/flight/pios/osx/osx/pios_rcvr.c +++ b/flight/pios/osx/osx/pios_rcvr.c @@ -6,102 +6,108 @@ #include enum pios_rcvr_dev_magic { - PIOS_RCVR_DEV_MAGIC = 0x99aabbcc, + PIOS_RCVR_DEV_MAGIC = 0x99aabbcc, }; struct pios_rcvr_dev { - enum pios_rcvr_dev_magic magic; - uint32_t lower_id; - const struct pios_rcvr_driver * driver; + enum pios_rcvr_dev_magic magic; + uint32_t lower_id; + const struct pios_rcvr_driver *driver; }; -static bool PIOS_RCVR_validate(struct pios_rcvr_dev * rcvr_dev) +static bool PIOS_RCVR_validate(struct pios_rcvr_dev *rcvr_dev) { - return (rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC); + return rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) && 0 -static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) +static struct pios_rcvr_dev *PIOS_RCVR_alloc(void) { - struct pios_rcvr_dev * rcvr_dev; + struct pios_rcvr_dev *rcvr_dev; - rcvr_dev = (struct pios_rcvr_dev *)malloc(sizeof(*rcvr_dev)); - if (!rcvr_dev) return (NULL); + rcvr_dev = (struct pios_rcvr_dev *)malloc(sizeof(*rcvr_dev)); + if (!rcvr_dev) { + return NULL; + } - rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; - return(rcvr_dev); + rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; + return rcvr_dev; } #else static struct pios_rcvr_dev pios_rcvr_devs[PIOS_RCVR_MAX_DEVS]; static uint8_t pios_rcvr_num_devs; -static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) +static struct pios_rcvr_dev *PIOS_RCVR_alloc(void) { - struct pios_rcvr_dev * rcvr_dev; + struct pios_rcvr_dev *rcvr_dev; - if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) { - return (NULL); - } + if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) { + return NULL; + } - rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++]; - rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; + rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++]; + rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; - return (rcvr_dev); + return rcvr_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) && 0 */ /** - * Initialises RCVR layer - * \param[out] handle - * \param[in] driver - * \param[in] id - * \return < 0 if initialisation failed - */ -int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id) + * Initialises RCVR layer + * \param[out] handle + * \param[in] driver + * \param[in] id + * \return < 0 if initialisation failed + */ +int32_t PIOS_RCVR_Init(uint32_t *rcvr_id, const struct pios_rcvr_driver *driver, const uint32_t lower_id) { - PIOS_DEBUG_Assert(rcvr_id); - PIOS_DEBUG_Assert(driver); + PIOS_DEBUG_Assert(rcvr_id); + PIOS_DEBUG_Assert(driver); - struct pios_rcvr_dev * rcvr_dev; + struct pios_rcvr_dev *rcvr_dev; - rcvr_dev = (struct pios_rcvr_dev *) PIOS_RCVR_alloc(); - if (!rcvr_dev) goto out_fail; + rcvr_dev = (struct pios_rcvr_dev *)PIOS_RCVR_alloc(); + if (!rcvr_dev) { + goto out_fail; + } - rcvr_dev->driver = driver; - rcvr_dev->lower_id = lower_id; + rcvr_dev->driver = driver; + rcvr_dev->lower_id = lower_id; - *rcvr_id = pios_rcvr_num_devs - 1; + *rcvr_id = pios_rcvr_num_devs - 1; - return(0); + return 0; out_fail: - return(-1); + return -1; } int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) { - // Publicly facing API uses channel 1 for first channel - if(channel == 0) - return PIOS_RCVR_INVALID; - else - channel--; + // Publicly facing API uses channel 1 for first channel + if (channel == 0) { + return PIOS_RCVR_INVALID; + } else { + channel--; + } - if (rcvr_id == 0) - return PIOS_RCVR_NODRIVER; + if (rcvr_id == 0) { + return PIOS_RCVR_NODRIVER; + } - struct pios_rcvr_dev * rcvr_dev = &pios_rcvr_devs[rcvr_id]; + struct pios_rcvr_dev *rcvr_dev = &pios_rcvr_devs[rcvr_id]; - if (!PIOS_RCVR_validate(rcvr_dev)) { - /* Undefined RCVR port for this board (see pios_board.c) */ - PIOS_DEBUG_Assert(0); - } + if (!PIOS_RCVR_validate(rcvr_dev)) { + /* Undefined RCVR port for this board (see pios_board.c) */ + PIOS_DEBUG_Assert(0); + } - PIOS_DEBUG_Assert(rcvr_dev->driver->read); + PIOS_DEBUG_Assert(rcvr_dev->driver->read); - return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); + return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); } -#endif +#endif /* if defined(PIOS_INCLUDE_RCVR) */ /** * @} diff --git a/flight/pios/osx/osx/pios_sdcard.c b/flight/pios/osx/osx/pios_sdcard.c index 78da48e46..a1190499e 100644 --- a/flight/pios/osx/osx/pios_sdcard.c +++ b/flight/pios/osx/osx/pios_sdcard.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_sdcard.c + * @file pios_sdcard.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief Sets up basic system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_SDCARD SDCard Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,186 +34,186 @@ /** -* Initialises SPI pins and peripheral to access MMC/SD Card -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ + * Initialises SPI pins and peripheral to access MMC/SD Card + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ int32_t PIOS_SDCARD_Init(void) { - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Connects to SD Card -* \return < 0 if initialisation sequence failed -*/ + * Connects to SD Card + * \return < 0 if initialisation sequence failed + */ int32_t PIOS_SDCARD_PowerOn(void) { - return 0; /* Status should be 0 if nothing went wrong! */ + return 0; /* Status should be 0 if nothing went wrong! */ } /** -* Disconnects from SD Card -* \return < 0 on errors -*/ + * Disconnects from SD Card + * \return < 0 on errors + */ int32_t PIOS_SDCARD_PowerOff(void) { - return 0; // no error + return 0; // no error } /** -* If SD card was previously available: Checks if the SD Card is still -* available by sending the STATUS command.
-* This takes ca. 10 uS -* -* If SD card was previously not available: Checks if the SD Card is -* available by sending the IDLE command at low speed.
-* This takes ca. 500 uS!
-* Once we got a positive response, SDCARD_PowerOn() will be -* called by this function to initialize the card completely. -* -* Example for Connection/Disconnection detection: -* \code -* // this function is called each second from a low-priority task -* // If multiple tasks are accessing the SD card, add a semaphore/mutex -* // to avoid IO access collisions with other tasks! -* u8 sdcard_available; -* int32_t CheckSDCard(void) -* { -* // check if SD card is available -* // High speed access if the card was previously available -* u8 prev_sdcard_available = sdcard_available; -* sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); -* -* if(sdcard_available && !prev_sdcard_available) { -* // SD Card has been connected -* -* // now it's possible to read/write sectors -* -* } else if( !sdcard_available && prev_sdcard_available ) { -* // SD Card has been disconnected -* -* // here you can notify your application about this state -* } -* -* return 0; // no error -* } -* \endcode -* \param[in] was_available should only be set if the SD card was previously available -* \return 0 if no response from SD Card -* \return 1 if SD card is accessible -*/ + * If SD card was previously available: Checks if the SD Card is still + * available by sending the STATUS command.
+ * This takes ca. 10 uS + * + * If SD card was previously not available: Checks if the SD Card is + * available by sending the IDLE command at low speed.
+ * This takes ca. 500 uS!
+ * Once we got a positive response, SDCARD_PowerOn() will be + * called by this function to initialize the card completely. + * + * Example for Connection/Disconnection detection: + * \code + * // this function is called each second from a low-priority task + * // If multiple tasks are accessing the SD card, add a semaphore/mutex + * // to avoid IO access collisions with other tasks! + * u8 sdcard_available; + * int32_t CheckSDCard(void) + * { + * // check if SD card is available + * // High speed access if the card was previously available + * u8 prev_sdcard_available = sdcard_available; + * sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); + * + * if(sdcard_available && !prev_sdcard_available) { + * // SD Card has been connected + * + * // now it's possible to read/write sectors + * + * } else if( !sdcard_available && prev_sdcard_available ) { + * // SD Card has been disconnected + * + * // here you can notify your application about this state + * } + * + * return 0; // no error + * } + * \endcode + * \param[in] was_available should only be set if the SD card was previously available + * \return 0 if no response from SD Card + * \return 1 if SD card is accessible + */ int32_t PIOS_SDCARD_CheckAvailable(uint8_t was_available) { - return 1; /* 1 = available, 0 = not available. */ + return 1; /* 1 = available, 0 = not available. */ } /** -* Sends command to SD card -* \param[in] cmd SD card command -* \param[in] addr 32bit address -* \param[in] crc precalculated CRC -* \return >= 0x00 if command has been sent successfully (contains received byte) -* \return -1 if no response from SD Card (timeout) -*/ + * Sends command to SD card + * \param[in] cmd SD card command + * \param[in] addr 32bit address + * \param[in] crc precalculated CRC + * \return >= 0x00 if command has been sent successfully (contains received byte) + * \return -1 if no response from SD Card (timeout) + */ int32_t PIOS_SDCARD_SendSDCCmd(uint8_t cmd, uint32_t addr, uint8_t crc) { - return -1; + return -1; } /** -* Reads 512 bytes from selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads 512 bytes from selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_SectorRead(uint32_t sector, uint8_t *buffer) { - return -256; + return -256; } /** -* Writes 512 bytes into selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if write operation not accepted -* \return -258 if timeout during write operation -*/ + * Writes 512 bytes into selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if write operation not accepted + * \return -258 if timeout during write operation + */ int32_t PIOS_SDCARD_SectorWrite(uint32_t sector, uint8_t *buffer) { - return -256; + return -256; } /** -* Reads the CID informations from SD Card -* \param[in] *cid pointer to buffer which holds the CID informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads the CID informations from SD Card + * \param[in] *cid pointer to buffer which holds the CID informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_CIDRead(SDCARDCidTypeDef *cid) { - return -256; + return -256; } /** -* Reads the CSD informations from SD Card -* \param[in] *csd pointer to buffer which holds the CSD informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads the CSD informations from SD Card + * \param[in] *csd pointer to buffer which holds the CSD informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_CSDRead(SDCARDCsdTypeDef *csd) { - return -256; + return -256; } /** -* Attempts to write a startup log to the SDCard -* return 0 No errors -* return -1 Error deleting file -* return -2 Error opening file -* return -3 Error writing file -*/ + * Attempts to write a startup log to the SDCard + * return 0 No errors + * return -1 Error deleting file + * return -2 Error opening file + * return -3 Error writing file + */ int32_t PIOS_SDCARD_StartupLog(void) { - return -3; + return -3; } /** @@ -223,78 +223,78 @@ int32_t PIOS_SDCARD_StartupLog(void) */ int32_t PIOS_SDCARD_IsMounted() { - return 1; + return 1; } /** -* Mounts the file system -* param[in] CreateStartupLog 1 = True, 0 = False -* return 0 No errors -* return -1 SDCard not available -* return -2 Cannot find first partition -* return -3 No volume information -* return -4 Error writing startup log file -*/ + * Mounts the file system + * param[in] CreateStartupLog 1 = True, 0 = False + * return 0 No errors + * return -1 SDCard not available + * return -2 Cannot find first partition + * return -3 No volume information + * return -4 Error writing startup log file + */ int32_t PIOS_SDCARD_MountFS(uint32_t CreateStartupLog) { - return 0; + return 0; } /** -* Mounts the file system -* return Amount of free bytes -*/ + * Mounts the file system + * return Amount of free bytes + */ int32_t PIOS_SDCARD_GetFree(void) { - return 10240; + return 10240; } /** -* Read from file -* return 0 No error -* return -1 DFS_ReadFile failed -* return -2 Less bytes read than expected -*/ -//int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len) -//{ - /* No error */ -// return -1; -//} + * Read from file + * return 0 No error + * return -1 DFS_ReadFile failed + * return -2 Less bytes read than expected + */ +// int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len) +// { +/* No error */ +// return -1; +// } /** -* Read a line from file -* returns Number of bytes read -*/ -//int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len) -//{ -// return -1; -//} + * Read a line from file + * returns Number of bytes read + */ +// int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len) +// { +// return -1; +// } /** -* Copy a file -* WARNING: This will overwrite the destination file even if it exists -* param[in] *Source Path to file to copy -* param[in] *Destination Path to destination file -* return 0 No errors -* return -1 Source file doesn't exist -* return -2 Failed to create destination file -* return -3 DFS_ReadFile failed -* return -4 DFS_WriteFile failed -*/ + * Copy a file + * WARNING: This will overwrite the destination file even if it exists + * param[in] *Source Path to file to copy + * param[in] *Destination Path to destination file + * return 0 No errors + * return -1 Source file doesn't exist + * return -2 Failed to create destination file + * return -3 DFS_ReadFile failed + * return -4 DFS_WriteFile failed + */ int32_t PIOS_SDCARD_FileCopy(char *Source, char *Destination) { - return -2; + return -2; } /** -* Delete a file -* param[in] *Filename File to delete -* return 0 No errors -* return -1 Error deleting file -*/ + * Delete a file + * param[in] *Filename File to delete + * return 0 No errors + * return -1 Error deleting file + */ int32_t PIOS_SDCARD_FileDelete(char *Filename) { - return -1; + return -1; } -#endif +#endif /* if defined(PIOS_INCLUDE_SDCARD) */ diff --git a/flight/pios/osx/osx/pios_servo.c b/flight/pios/osx/osx/pios_servo.c index d0cf530d3..c3b4ba05b 100644 --- a/flight/pios/osx/osx/pios_servo.c +++ b/flight/pios/osx/osx/pios_servo.c @@ -6,25 +6,25 @@ * @brief Code to do set RC servo output * @{ * - * @file pios_servo.c + * @file pios_servo.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RC Servo routines (STM32 dependent) * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,36 +39,33 @@ static volatile uint16_t ServoPosition[PIOS_SERVO_NUM_TIMERS]; /** -* Initialise Servos -*/ + * Initialise Servos + */ void PIOS_Servo_Init(void) -{ -} +{} /** -* Set the servo update rate (Max 500Hz) -* \param[in] onetofour Rate for outputs 1 to 4 (Hz) -* \param[in] fivetoeight Rate for outputs 5 to 8 (Hz) -*/ -void PIOS_Servo_SetHz(const uint16_t * banks, uint8_t num_banks) -{ -} + * Set the servo update rate (Max 500Hz) + * \param[in] onetofour Rate for outputs 1 to 4 (Hz) + * \param[in] fivetoeight Rate for outputs 5 to 8 (Hz) + */ +void PIOS_Servo_SetHz(const uint16_t *banks, uint8_t num_banks) +{} /** -* Set servo position -* \param[in] Servo Servo number (0-7) -* \param[in] Position Servo position in milliseconds -*/ + * Set servo position + * \param[in] Servo Servo number (0-7) + * \param[in] Position Servo position in milliseconds + */ void PIOS_Servo_Set(uint8_t Servo, uint16_t Position) { #ifndef PIOS_ENABLE_DEBUG_PINS - /* Make sure servo exists */ - if (Servo < PIOS_SERVO_NUM_OUTPUTS) { - /* Update the position */ - ServoPosition[Servo] = Position; - - } + /* Make sure servo exists */ + if (Servo < PIOS_SERVO_NUM_OUTPUTS) { + /* Update the position */ + ServoPosition[Servo] = Position; + } #endif // PIOS_ENABLE_DEBUG_PINS } -#endif +#endif /* if defined(PIOS_INCLUDE_SERVO) */ diff --git a/flight/pios/osx/osx/pios_sim.c b/flight/pios/osx/osx/pios_sim.c index 0a866a4f7..f8e351dd1 100644 --- a/flight/pios/osx/osx/pios_sim.c +++ b/flight/pios/osx/osx/pios_sim.c @@ -1,40 +1,41 @@ - #include "pios.h" #include "pios_sim_priv.h" #include "sim_model.h" struct pios_sim_state pios_sim_state = { - .accels = {0, 0, 0}, - .gyros = {0, 0, 0}, - .mag = {0, 0, 0}, - .baro = {0}, - .q = {1, 0, 0, 0}, - .velocity = {0, 0, 0}, - .position = {0, 0, 0}, - .actuator = {0, 0, 0, 0, 0, 0, 0, 0} + .accels = { 0, 0, 0 }, + .gyros = { 0, 0, 0 }, + .mag = { 0, 0, 0 }, + .baro = { 0 }, + .q = { 1, 0, 0, 0}, + .velocity = { 0, 0, 0 }, + .position = { 0, 0, 0 }, + .actuator = { 0, 0, 0, 0, 0, 0, 0, 0} }; /** * Initialize the model in the external library * @returns 0 for success, -1 if fails to initialize external library */ -int PIOS_SIM_Init() +int PIOS_SIM_Init() { - if (sim_model_init() != 0) - return -1; - return 0; + if (sim_model_init() != 0) { + return -1; + } + return 0; } /** * Step the model simulation in the external library * @returns 0 for success, -1 for failure to step external library */ -int PIOS_SIM_Step(float dT) +int PIOS_SIM_Step(float dT) { - if (sim_model_step(dT, &pios_sim_state) != 0) - return -1; + if (sim_model_step(dT, &pios_sim_state) != 0) { + return -1; + } - return 0; + return 0; } /** @@ -42,40 +43,44 @@ int PIOS_SIM_Step(float dT) * @param[in] actuator pointer to an array of actuators to set * @param[in] nchannels number of channels that are valid coming in */ -void PIOS_SIM_SetActuator(float * actuator, int nchannels) +void PIOS_SIM_SetActuator(float *actuator, int nchannels) { - for (int i = 0; i < NELEMENTS(pios_sim_state.actuator) && i < nchannels; i++) - pios_sim_state.actuator[i] = actuator[i]; + for (int i = 0; i < NELEMENTS(pios_sim_state.actuator) && i < nchannels; i++) { + pios_sim_state.actuator[i] = actuator[i]; + } } /** * Get the accelerometer data from the simulation model * @param[out] pointer to store the accelerometer data in */ -void PIOS_SIM_GetAccels(float * accels) +void PIOS_SIM_GetAccels(float *accels) { - for (int i = 0; i < NELEMENTS(pios_sim_state.accels); i++) - accels[i] = pios_sim_state.accels[i]; + for (int i = 0; i < NELEMENTS(pios_sim_state.accels); i++) { + accels[i] = pios_sim_state.accels[i]; + } } /** * Get the gyro data from the simulation model * @param[out] pointer to store the gyro data in */ -void PIOS_SIM_GetGyros(float * gyros) +void PIOS_SIM_GetGyros(float *gyros) { - for (int i = 0; i < NELEMENTS(pios_sim_state.accels); i++) - gyros[i] = pios_sim_state.gyros[i]; + for (int i = 0; i < NELEMENTS(pios_sim_state.accels); i++) { + gyros[i] = pios_sim_state.gyros[i]; + } } /** * Get the current attitude from the simulation model * @param[out] quat pointer to store the quaternion attitude in */ -void PIOS_SIM_GetAttitude(float * q) +void PIOS_SIM_GetAttitude(float *q) { - for (int i = 0; i < NELEMENTS(pios_sim_state.q); i++) - q[i] = pios_sim_state.q[i]; + for (int i = 0; i < NELEMENTS(pios_sim_state.q); i++) { + q[i] = pios_sim_state.q[i]; + } } /** @@ -83,10 +88,11 @@ void PIOS_SIM_GetAttitude(float * q) * @param[out] position pointer to store the current position in (cm in NED * frame) */ -void PIOS_SIM_GetVelocity(float * velocity) +void PIOS_SIM_GetVelocity(float *velocity) { - for (int i = 0; i < NELEMENTS(pios_sim_state.velocity); i++) - velocity[i] = pios_sim_state.velocity[i]; + for (int i = 0; i < NELEMENTS(pios_sim_state.velocity); i++) { + velocity[i] = pios_sim_state.velocity[i]; + } } /** @@ -94,8 +100,9 @@ void PIOS_SIM_GetVelocity(float * velocity) * @param[out] position pointer to store the current position in (cm in NED * frame) */ -void PIOS_SIM_GetPosition(float * position) +void PIOS_SIM_GetPosition(float *position) { - for (int i = 0; i < NELEMENTS(pios_sim_state.position); i++) - position[i] = pios_sim_state.position[i]; + for (int i = 0; i < NELEMENTS(pios_sim_state.position); i++) { + position[i] = pios_sim_state.position[i]; + } } diff --git a/flight/pios/osx/osx/pios_sys.c b/flight/pios/osx/osx/pios_sys.c index d2c15c7b5..fac9e85e0 100644 --- a/flight/pios/osx/osx/pios_sys.c +++ b/flight/pios/osx/osx/pios_sys.c @@ -6,26 +6,26 @@ * @brief PIOS System Initialization code * @{ * - * @file pios_sys.c + * @file pios_sys.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Sets up basic STM32 system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,76 +36,75 @@ /** -* Initialises all system peripherals -*/ + * Initialises all system peripherals + */ void PIOS_SYS_Init(void) -{ - -} +{} /** -* Shutdown PIOS and reset the microcontroller:
-*
    -*
  • Disable all RTOS tasks -*
  • Disable all interrupts -*
  • Turn off all board LEDs -*
  • Reset STM32 -*
-* \return < 0 if reset failed -*/ + * Shutdown PIOS and reset the microcontroller:
+ *
    + *
  • Disable all RTOS tasks + *
  • Disable all interrupts + *
  • Turn off all board LEDs + *
  • Reset STM32 + *
+ * \return < 0 if reset failed + */ int32_t PIOS_SYS_Reset(void) { - /* We will never reach this point */ - return -1; + /* We will never reach this point */ + return -1; } /** -* Returns the CPU's flash size (in bytes) -*/ + * Returns the CPU's flash size (in bytes) + */ uint32_t PIOS_SYS_getCPUFlashSize(void) { - return 1024000; + return 1024000; } /** -* Returns the serial number as a string -* param[out] uint8_t pointer to a string which can store at least 12 bytes -* (12 bytes returned for STM32) -* return < 0 if feature not supported -*/ + * Returns the serial number as a string + * param[out] uint8_t pointer to a string which can store at least 12 bytes + * (12 bytes returned for STM32) + * return < 0 if feature not supported + */ int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t *array) { - /* Stored in the so called "electronic signature" */ - for (int i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) { - array[i] = 0xff; - } + /* Stored in the so called "electronic signature" */ + for (int i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) { + array[i] = 0xff; + } - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Returns the serial number as a string -* param[out] str pointer to a string which can store at least 32 digits + zero terminator! -* (24 digits returned for STM32) -* return < 0 if feature not supported -*/ + * Returns the serial number as a string + * param[out] str pointer to a string which can store at least 32 digits + zero terminator! + * (24 digits returned for STM32) + * return < 0 if feature not supported + */ int32_t PIOS_SYS_SerialNumberGet(char *str) { - /* Stored in the so called "electronic signature" */ - int i; - for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { - str[i] = 'F'; - } - str[i] = '\0'; + /* Stored in the so called "electronic signature" */ + int i; - /* No error */ - return 0; + for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { + str[i] = 'F'; + } + str[i] = '\0'; + + /* No error */ + return 0; } -#endif +#endif /* if defined(PIOS_INCLUDE_SYS) */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/osx/pios_tcp.c b/flight/pios/osx/osx/pios_tcp.c index 3938cb2b7..5d6671386 100644 --- a/flight/pios/osx/osx/pios_tcp.c +++ b/flight/pios/osx/osx/pios_tcp.c @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file pios_tcp.c + * @file pios_tcp.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief TCP commands. Inits UDPs, controls UDPs & Interupt handlers. * @see The GNU Public License (GPL) Version 3 @@ -9,19 +9,19 @@ * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,7 +42,6 @@ static int8_t pios_tcp_num_devices = 0; static pios_tcp_dev pios_tcp_devices[PIOS_TCP_MAX_DEV]; - /* Provide a COM driver */ static void PIOS_TCP_ChangeBaud(uint32_t udp_id, uint32_t baud); static void PIOS_TCP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context); @@ -51,103 +50,101 @@ static void PIOS_TCP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail); static void PIOS_TCP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_tcp_com_driver = { - .set_baud = PIOS_TCP_ChangeBaud, - .tx_start = PIOS_TCP_TxStart, - .rx_start = PIOS_TCP_RxStart, - .bind_tx_cb = PIOS_TCP_RegisterTxCallback, - .bind_rx_cb = PIOS_TCP_RegisterRxCallback, + .set_baud = PIOS_TCP_ChangeBaud, + .tx_start = PIOS_TCP_TxStart, + .rx_start = PIOS_TCP_RxStart, + .bind_tx_cb = PIOS_TCP_RegisterTxCallback, + .bind_rx_cb = PIOS_TCP_RegisterRxCallback, }; -static pios_tcp_dev * find_tcp_dev_by_id (uint8_t tcp) +static pios_tcp_dev *find_tcp_dev_by_id(uint8_t tcp) { - if (tcp >= pios_tcp_num_devices) { - /* Undefined UDP port for this board (see pios_board.c) */ - PIOS_Assert(0); - return NULL; - } - - /* Get a handle for the device configuration */ - return &(pios_tcp_devices[tcp]); + if (tcp >= pios_tcp_num_devices) { + /* Undefined UDP port for this board (see pios_board.c) */ + PIOS_Assert(0); + return NULL; + } + + /* Get a handle for the device configuration */ + return &(pios_tcp_devices[tcp]); } /** * RxThread */ - + static void PIOS_TCP_RxTask(void *tcp_dev_n) { - bool rx_need_yield; + bool rx_need_yield; - pios_tcp_dev *tcp_dev = (pios_tcp_dev *) tcp_dev_n; - while(1) { - - if (tcp_dev->rx_in_cb) { - char buffer[PIOS_TCP_RX_BUFFER_SIZE]; - int received = fifoBuf_getData(&tcp_dev->rx_fifo, buffer, PIOS_TCP_RX_BUFFER_SIZE); - (void) (tcp_dev->rx_in_cb)(tcp_dev->rx_in_context, (uint8_t *) buffer, received, NULL, &rx_need_yield); + pios_tcp_dev *tcp_dev = (pios_tcp_dev *)tcp_dev_n; - //fprintf(stderr, "Received %d\n", received); + while (1) { + if (tcp_dev->rx_in_cb) { + char buffer[PIOS_TCP_RX_BUFFER_SIZE]; + int received = fifoBuf_getData(&tcp_dev->rx_fifo, buffer, PIOS_TCP_RX_BUFFER_SIZE); + (void)(tcp_dev->rx_in_cb)(tcp_dev->rx_in_context, (uint8_t *)buffer, received, NULL, &rx_need_yield); + + // fprintf(stderr, "Received %d\n", received); #if defined(PIOS_INCLUDE_FREERTOS) - // Not sure about this - if (rx_need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ - - } else - fifoBuf_clearData(&tcp_dev->rx_fifo); - - vTaskDelay(1); - } + // Not sure about this + if (rx_need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ + } else { + fifoBuf_clearData(&tcp_dev->rx_fifo); + } + + vTaskDelay(1); + } } static void *PIOS_TCP_RxThread(void *tcp_dev_n) { - - /* needed because of FreeRTOS.posix scheduling */ - sigset_t set; - sigfillset(&set); - sigprocmask(SIG_BLOCK, &set, NULL); - - pios_tcp_dev *tcp_dev = (pios_tcp_dev*) tcp_dev_n; - - const int INCOMING_BUFFER_SIZE = 16; - char incoming_buffer[INCOMING_BUFFER_SIZE]; - /** - * com devices never get closed except by application "reboot" - * we also never give up our mutex except for waiting - */ - while(1) { - - tcp_dev->socket_connection = accept(tcp_dev->socket, NULL, NULL); - if (0 > tcp_dev->socket_connection) { - perror("Accept failed"); - close(tcp_dev->socket); - exit(EXIT_FAILURE); - } - - fprintf(stderr, "Connection accepted\n"); - - int received; - do { - // Received is used to track the scoket whereas the dev variable is only updated when it can be - received = read(tcp_dev->socket_connection, incoming_buffer, INCOMING_BUFFER_SIZE); - - //while(fifoBuf_getFree(&tcp_dev->rx_fifo) < received); - - fifoBuf_putData(&tcp_dev->rx_fifo, incoming_buffer, received); - } while(received > 0); - - if (-1 == shutdown(tcp_dev->socket_connection, SHUT_RDWR)) - { - //perror("can not shutdown socket"); - //close(tcp_dev->socket_connection); - //exit(EXIT_FAILURE); - } - close(tcp_dev->socket_connection); - tcp_dev->socket_connection = 0; - } + /* needed because of FreeRTOS.posix scheduling */ + sigset_t set; + + sigfillset(&set); + sigprocmask(SIG_BLOCK, &set, NULL); + + pios_tcp_dev *tcp_dev = (pios_tcp_dev *)tcp_dev_n; + + const int INCOMING_BUFFER_SIZE = 16; + char incoming_buffer[INCOMING_BUFFER_SIZE]; + /** + * com devices never get closed except by application "reboot" + * we also never give up our mutex except for waiting + */ + while (1) { + tcp_dev->socket_connection = accept(tcp_dev->socket, NULL, NULL); + if (0 > tcp_dev->socket_connection) { + perror("Accept failed"); + close(tcp_dev->socket); + exit(EXIT_FAILURE); + } + + fprintf(stderr, "Connection accepted\n"); + + int received; + do { + // Received is used to track the scoket whereas the dev variable is only updated when it can be + received = read(tcp_dev->socket_connection, incoming_buffer, INCOMING_BUFFER_SIZE); + + // while(fifoBuf_getFree(&tcp_dev->rx_fifo) < received); + + fifoBuf_putData(&tcp_dev->rx_fifo, incoming_buffer, received); + } while (received > 0); + + if (-1 == shutdown(tcp_dev->socket_connection, SHUT_RDWR)) { + // perror("can not shutdown socket"); + // close(tcp_dev->socket_connection); + // exit(EXIT_FAILURE); + } + close(tcp_dev->socket_connection); + tcp_dev->socket_connection = 0; + } } @@ -155,132 +152,130 @@ static void *PIOS_TCP_RxThread(void *tcp_dev_n) * Open UDP socket */ xTaskHandle tcpRxTaskHandle; -int32_t PIOS_TCP_Init(uint32_t *tcp_id, const struct pios_tcp_cfg * cfg) +int32_t PIOS_TCP_Init(uint32_t *tcp_id, const struct pios_tcp_cfg *cfg) { - - pios_tcp_dev *tcp_dev = &pios_tcp_devices[pios_tcp_num_devices]; - - pios_tcp_num_devices++; - - - /* initialize */ - tcp_dev->rx_in_cb = NULL; - tcp_dev->tx_out_cb = NULL; - tcp_dev->cfg=cfg; - - /* assign socket */ - tcp_dev->socket = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP); - memset(&tcp_dev->server,0,sizeof(tcp_dev->server)); - memset(&tcp_dev->client,0,sizeof(tcp_dev->client)); - tcp_dev->server.sin_family = AF_INET; - tcp_dev->server.sin_addr.s_addr = INADDR_ANY; //inet_addr(tcp_dev->cfg->ip); - tcp_dev->server.sin_port = htons(tcp_dev->cfg->port); - int res= bind(tcp_dev->socket, (struct sockaddr *)&tcp_dev->server,sizeof(tcp_dev->server)); - if (res == -1) { - perror("Binding socket failed\n"); - exit(EXIT_FAILURE); - } - - res = listen(tcp_dev->socket, 10); - if (res == -1) { - perror("Socket listen failed\n"); - exit(EXIT_FAILURE); - } - - fifoBuf_init(&tcp_dev->rx_fifo, tcp_dev->rx_buffer, PIOS_TCP_RX_BUFFER_SIZE); - - pthread_create(&tcp_dev->rxThread, NULL, PIOS_TCP_RxThread, (void*)tcp_dev); + pios_tcp_dev *tcp_dev = &pios_tcp_devices[pios_tcp_num_devices]; - xTaskCreate(PIOS_TCP_RxTask, (signed char *)"TcpRx", 1024, (void*)tcp_dev, 2, &tcpRxTaskHandle); - - printf("udp dev %i - socket %i opened - result %i\n",pios_tcp_num_devices-1,tcp_dev->socket,res); - - *tcp_id = pios_tcp_num_devices-1; - - return res; + pios_tcp_num_devices++; + + + /* initialize */ + tcp_dev->rx_in_cb = NULL; + tcp_dev->tx_out_cb = NULL; + tcp_dev->cfg = cfg; + + /* assign socket */ + tcp_dev->socket = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP); + memset(&tcp_dev->server, 0, sizeof(tcp_dev->server)); + memset(&tcp_dev->client, 0, sizeof(tcp_dev->client)); + tcp_dev->server.sin_family = AF_INET; + tcp_dev->server.sin_addr.s_addr = INADDR_ANY; // inet_addr(tcp_dev->cfg->ip); + tcp_dev->server.sin_port = htons(tcp_dev->cfg->port); + int res = bind(tcp_dev->socket, (struct sockaddr *)&tcp_dev->server, sizeof(tcp_dev->server)); + if (res == -1) { + perror("Binding socket failed\n"); + exit(EXIT_FAILURE); + } + + res = listen(tcp_dev->socket, 10); + if (res == -1) { + perror("Socket listen failed\n"); + exit(EXIT_FAILURE); + } + + fifoBuf_init(&tcp_dev->rx_fifo, tcp_dev->rx_buffer, PIOS_TCP_RX_BUFFER_SIZE); + + pthread_create(&tcp_dev->rxThread, NULL, PIOS_TCP_RxThread, (void *)tcp_dev); + + xTaskCreate(PIOS_TCP_RxTask, (signed char *)"TcpRx", 1024, (void *)tcp_dev, 2, &tcpRxTaskHandle); + + printf("udp dev %i - socket %i opened - result %i\n", pios_tcp_num_devices - 1, tcp_dev->socket, res); + + *tcp_id = pios_tcp_num_devices - 1; + + return res; } void PIOS_TCP_ChangeBaud(uint32_t tcp_id, uint32_t baud) { - /** - * doesn't apply! - */ + /** + * doesn't apply! + */ } static void PIOS_TCP_RxStart(uint32_t tp_id, uint16_t rx_bytes_avail) { - /** - * lazy! - */ + /** + * lazy! + */ } static void PIOS_TCP_TxStart(uint32_t tcp_id, uint16_t tx_bytes_avail) { - pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id); - - PIOS_Assert(tcp_dev); - - int32_t length,len,rem; - - /** - * we send everything directly whenever notified of data to send (lazy!) - */ - if (tcp_dev->tx_out_cb) { - while (tx_bytes_avail>0) { - bool tx_need_yield = false; - length = (tcp_dev->tx_out_cb)(tcp_dev->tx_out_context, tcp_dev->tx_buffer, PIOS_TCP_RX_BUFFER_SIZE, NULL, &tx_need_yield); - rem = length; - while (rem>0) { - if(tcp_dev->socket_connection != 0) { - len = write(tcp_dev->socket_connection, tcp_dev->tx_buffer, length); - } - if (len<=0) { - rem=0; - } else { - rem -= len; - } - } - tx_bytes_avail -= length; + pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id); + + PIOS_Assert(tcp_dev); + + int32_t length, len, rem; + + /** + * we send everything directly whenever notified of data to send (lazy!) + */ + if (tcp_dev->tx_out_cb) { + while (tx_bytes_avail > 0) { + bool tx_need_yield = false; + length = (tcp_dev->tx_out_cb)(tcp_dev->tx_out_context, tcp_dev->tx_buffer, PIOS_TCP_RX_BUFFER_SIZE, NULL, &tx_need_yield); + rem = length; + while (rem > 0) { + if (tcp_dev->socket_connection != 0) { + len = write(tcp_dev->socket_connection, tcp_dev->tx_buffer, length); + } + if (len <= 0) { + rem = 0; + } else { + rem -= len; + } + } + tx_bytes_avail -= length; #if defined(PIOS_INCLUDE_FREERTOS) - // Not sure about this - if (tx_need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ - } - } - + // Not sure about this + if (tx_need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ + } + } } static void PIOS_TCP_RegisterRxCallback(uint32_t tcp_id, pios_com_callback rx_in_cb, uint32_t context) { - pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id); - - PIOS_Assert(tcp_dev); - - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - tcp_dev->rx_in_context = context; - tcp_dev->rx_in_cb = rx_in_cb; + pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id); + + PIOS_Assert(tcp_dev); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + tcp_dev->rx_in_context = context; + tcp_dev->rx_in_cb = rx_in_cb; } static void PIOS_TCP_RegisterTxCallback(uint32_t tcp_id, pios_com_callback tx_out_cb, uint32_t context) { - pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id); - - PIOS_Assert(tcp_dev); - - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - tcp_dev->tx_out_context = context; - tcp_dev->tx_out_cb = tx_out_cb; + pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id); + + PIOS_Assert(tcp_dev); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + tcp_dev->tx_out_context = context; + tcp_dev->tx_out_cb = tx_out_cb; } -#endif +#endif /* if defined(PIOS_INCLUDE_TCP) */ diff --git a/flight/pios/osx/osx/pios_udp.c b/flight/pios/osx/osx/pios_udp.c index a263116d4..3f2425f19 100644 --- a/flight/pios/osx/osx/pios_udp.c +++ b/flight/pios/osx/osx/pios_udp.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_udp.c + * @file pios_udp.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief UDP commands. Inits UDPs, controls UDPs & Interupt handlers. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_UDP UDP Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,7 +43,6 @@ static int8_t pios_udp_num_devices = 0; static pios_udp_dev pios_udp_devices[PIOS_UDP_MAX_DEV]; - /* Provide a COM driver */ static void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud); static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context); @@ -52,193 +51,182 @@ static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail); static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_udp_com_driver = { - .set_baud = PIOS_UDP_ChangeBaud, - .tx_start = PIOS_UDP_TxStart, - .rx_start = PIOS_UDP_RxStart, - .bind_tx_cb = PIOS_UDP_RegisterTxCallback, - .bind_rx_cb = PIOS_UDP_RegisterRxCallback, + .set_baud = PIOS_UDP_ChangeBaud, + .tx_start = PIOS_UDP_TxStart, + .rx_start = PIOS_UDP_RxStart, + .bind_tx_cb = PIOS_UDP_RegisterTxCallback, + .bind_rx_cb = PIOS_UDP_RegisterRxCallback, }; -static pios_udp_dev * find_udp_dev_by_id (uint8_t udp) +static pios_udp_dev *find_udp_dev_by_id(uint8_t udp) { - if (udp >= pios_udp_num_devices) { - /* Undefined UDP port for this board (see pios_board.c) */ - PIOS_Assert(0); - return NULL; - } + if (udp >= pios_udp_num_devices) { + /* Undefined UDP port for this board (see pios_board.c) */ + PIOS_Assert(0); + return NULL; + } - /* Get a handle for the device configuration */ - return &(pios_udp_devices[udp]); + /* Get a handle for the device configuration */ + return &(pios_udp_devices[udp]); } /** * RxThread */ -void PIOS_UDP_RxThread(void * udp_dev_n) +void PIOS_UDP_RxThread(void *udp_dev_n) { + /* needed because of FreeRTOS.posix scheduling */ + sigset_t set; - /* needed because of FreeRTOS.posix scheduling */ - sigset_t set; - sigfillset(&set); - sigprocmask(SIG_BLOCK, &set, NULL); + sigfillset(&set); + sigprocmask(SIG_BLOCK, &set, NULL); - pios_udp_dev * udp_dev = (pios_udp_dev*) udp_dev_n; + pios_udp_dev *udp_dev = (pios_udp_dev *)udp_dev_n; - /** - * com devices never get closed except by application "reboot" - * we also never give up our mutex except for waiting - */ - while(1) { - - /** - * receive - */ - int received; - udp_dev->clientLength=sizeof(udp_dev->client); - if ((received = recvfrom(udp_dev->socket, - &udp_dev->rx_buffer, - PIOS_UDP_RX_BUFFER_SIZE, - 0, - (struct sockaddr *) &udp_dev->client, - (socklen_t*)&udp_dev->clientLength)) >= 0) - { - - /* copy received data to buffer if possible */ - /* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */ - /* (thats what the USART driver does too!) */ - bool rx_need_yield = false; - if (udp_dev->rx_in_cb) { - (void) (udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield); - } + /** + * com devices never get closed except by application "reboot" + * we also never give up our mutex except for waiting + */ + while (1) { + /** + * receive + */ + int received; + udp_dev->clientLength = sizeof(udp_dev->client); + if ((received = recvfrom(udp_dev->socket, + &udp_dev->rx_buffer, + PIOS_UDP_RX_BUFFER_SIZE, + 0, + (struct sockaddr *)&udp_dev->client, + (socklen_t *)&udp_dev->clientLength)) >= 0) { + /* copy received data to buffer if possible */ + /* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */ + /* (thats what the USART driver does too!) */ + bool rx_need_yield = false; + if (udp_dev->rx_in_cb) { + (void)(udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (rx_need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ - - } - - - } + if (rx_need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ + } + } } /** -* Open UDP socket -*/ -int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg) + * Open UDP socket + */ +int32_t PIOS_UDP_Init(uint32_t *udp_id, const struct pios_udp_cfg *cfg) { + pios_udp_dev *udp_dev = &pios_udp_devices[pios_udp_num_devices]; - pios_udp_dev * udp_dev = &pios_udp_devices[pios_udp_num_devices]; - - pios_udp_num_devices++; + pios_udp_num_devices++; - /* initialize */ - udp_dev->rx_in_cb = NULL; - udp_dev->tx_out_cb = NULL; - udp_dev->cfg=cfg; + /* initialize */ + udp_dev->rx_in_cb = NULL; + udp_dev->tx_out_cb = NULL; + udp_dev->cfg = cfg; - /* assign socket */ - udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - memset(&udp_dev->server,0,sizeof(udp_dev->server)); - memset(&udp_dev->client,0,sizeof(udp_dev->client)); - udp_dev->server.sin_family = AF_INET; - udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip); - udp_dev->server.sin_port = htons(udp_dev->cfg->port); - int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server)); + /* assign socket */ + udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + memset(&udp_dev->server, 0, sizeof(udp_dev->server)); + memset(&udp_dev->client, 0, sizeof(udp_dev->client)); + udp_dev->server.sin_family = AF_INET; + udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip); + udp_dev->server.sin_port = htons(udp_dev->cfg->port); + int res = bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server, sizeof(udp_dev->server)); - /* Create transmit thread for this connection */ - xTaskCreate(PIOS_UDP_RxThread, (signed char *)"UdpRx", 1024, (void*)udp_dev, 2, &udp_dev->rxThread); + /* Create transmit thread for this connection */ + xTaskCreate(PIOS_UDP_RxThread, (signed char *)"UdpRx", 1024, (void *)udp_dev, 2, &udp_dev->rxThread); - printf("udp dev %i - socket %i opened - result %i\n",pios_udp_num_devices-1,udp_dev->socket,res); + printf("udp dev %i - socket %i opened - result %i\n", pios_udp_num_devices - 1, udp_dev->socket, res); - *udp_id = pios_udp_num_devices-1; + *udp_id = pios_udp_num_devices - 1; - return res; + return res; } void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud) { - /** - * doesn't apply! - */ + /** + * doesn't apply! + */ } static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail) { - /** - * lazy! - */ + /** + * lazy! + */ } static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail) { - pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id); - PIOS_Assert(udp_dev); + PIOS_Assert(udp_dev); - int32_t length,len,rem; - - /** - * we send everything directly whenever notified of data to send (lazy!) - */ - if (udp_dev->tx_out_cb) { - while (tx_bytes_avail>0) { - bool tx_need_yield = false; - length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield); - rem = length; - while (rem>0) { - len = sendto(udp_dev->socket, udp_dev->tx_buffer, length, 0, - (struct sockaddr *) &udp_dev->client, - sizeof(udp_dev->client)); - if (len<=0) { - rem=0; - } else { - rem -= len; - } - } - tx_bytes_avail -= length; - } - } + int32_t length, len, rem; + /** + * we send everything directly whenever notified of data to send (lazy!) + */ + if (udp_dev->tx_out_cb) { + while (tx_bytes_avail > 0) { + bool tx_need_yield = false; + length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield); + rem = length; + while (rem > 0) { + len = sendto(udp_dev->socket, udp_dev->tx_buffer, length, 0, + (struct sockaddr *)&udp_dev->client, + sizeof(udp_dev->client)); + if (len <= 0) { + rem = 0; + } else { + rem -= len; + } + } + tx_bytes_avail -= length; + } + } } static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context) { - pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id); - PIOS_Assert(udp_dev); + PIOS_Assert(udp_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - udp_dev->rx_in_context = context; - udp_dev->rx_in_cb = rx_in_cb; + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + udp_dev->rx_in_context = context; + udp_dev->rx_in_cb = rx_in_cb; } static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context) { - pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id); - PIOS_Assert(udp_dev); + PIOS_Assert(udp_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - udp_dev->tx_out_context = context; - udp_dev->tx_out_cb = tx_out_cb; + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + udp_dev->tx_out_context = context; + udp_dev->tx_out_cb = tx_out_cb; } - - - -#endif +#endif /* if defined(PIOS_INCLUDE_UDP) */ diff --git a/flight/pios/osx/osx/pios_wdg.c b/flight/pios/osx/osx/pios_wdg.c index f2c35dac2..59dfcf858 100644 --- a/flight/pios/osx/osx/pios_wdg.c +++ b/flight/pios/osx/osx/pios_wdg.c @@ -8,12 +8,12 @@ * * @file pios_spi.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Hardware Abstraction Layer for SPI ports of STM32 * @see The GNU Public License (GPL) Version 3 * @notes * - * The PIOS Watchdog provides a HAL to initialize a watchdog + * The PIOS Watchdog provides a HAL to initialize a watchdog * *****************************************************************************/ /* @@ -40,17 +40,17 @@ unsigned int wdg_cleared_time; unsigned int wdg_last_update_time; bool wdg_expired; -/** +/** * @brief Initialize the watchdog timer for a specified timeout * - * It is important to note that this function returns the achieved timeout - * for this hardware. For hardware indendence this should be checked when - * scheduling updates. Other hardware dependent details may need to be - * considered such as a window time which sets a minimum update time, - * and this function should return a recommended delay for clearing. + * It is important to note that this function returns the achieved timeout + * for this hardware. For hardware indendence this should be checked when + * scheduling updates. Other hardware dependent details may need to be + * considered such as a window time which sets a minimum update time, + * and this function should return a recommended delay for clearing. * * For the STM32 nominal clock rate is 32 khz, but for the maximum clock rate of - * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is + * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is * set in the watchdog assumes the nominal clock rate, but the delay for FreeRTOS * to use is 75% of the minimal delay. * @@ -59,74 +59,74 @@ bool wdg_expired; */ void PIOS_WDG_Init() { - wdg_registered_flags = 0; - wdg_updated_flags = 0; + wdg_registered_flags = 0; + wdg_updated_flags = 0; } /** - * @brief Register a module against the watchdog - * + * @brief Register a module against the watchdog + * * There are two ways to use PIOS WDG: this is for when - * multiple modules must be monitored. In this case they - * must first register against the watchdog system and + * multiple modules must be monitored. In this case they + * must first register against the watchdog system and * only when all of the modules have been updated with the - * watchdog be cleared. Each module must have its own - * bit in the 16 bit + * watchdog be cleared. Each module must have its own + * bit in the 16 bit * * @param[in] flag the bit this module wants to use * @returns True if that bit is unregistered */ -bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) +bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) { - wdg_registered_flags |= flag_requested; - return true; + wdg_registered_flags |= flag_requested; + return true; } /** * @brief Function called by modules to indicate they are still running * - * This function will set this flag in the active flags register (which is + * This function will set this flag in the active flags register (which is * a backup regsiter) and if all the registered flags are set will clear * the watchdog and set only this flag in the backup register * * @param[in] flag the flag to set * @return true if the watchdog cleared, false if flags are pending */ -bool PIOS_WDG_UpdateFlag(uint16_t flag) -{ - PIOS_WDG_Check(); - wdg_updated_flags |= flag; - if( wdg_updated_flags == wdg_registered_flags) { - wdg_last_update_time = PIOS_DELAY_DiffuS(wdg_cleared_time); - wdg_updated_flags = 0; - wdg_cleared_time = PIOS_DELAY_GetRaw(); - } - return true; +bool PIOS_WDG_UpdateFlag(uint16_t flag) +{ + PIOS_WDG_Check(); + wdg_updated_flags |= flag; + if (wdg_updated_flags == wdg_registered_flags) { + wdg_last_update_time = PIOS_DELAY_DiffuS(wdg_cleared_time); + wdg_updated_flags = 0; + wdg_cleared_time = PIOS_DELAY_GetRaw(); + } + return true; } -/** +/** * @brief Returns the flags that were set at bootup - * - * This is used for diagnostics, if only one flag not set this + * + * This is used for diagnostics, if only one flag not set this * was likely the module that wasn't running before reset - * + * * @return The active flags register from bootup */ uint16_t PIOS_WDG_GetBootupFlags() { - return (uint16_t) 0xffff; + return (uint16_t)0xffff; } -/** +/** * @brief Returns the currently active flags - * + * * For external monitoring - * + * * @return The active flags register */ uint16_t PIOS_WDG_GetActiveFlags() { - return (uint16_t) 0xffff; + return (uint16_t)0xffff; } /** @@ -135,19 +135,19 @@ uint16_t PIOS_WDG_GetActiveFlags() * This function must be called at the appropriate delay to prevent a reset event occuring */ void PIOS_WDG_Clear(void) -{ -} +{} /** - * @brief This function returns true if the watchdog would + * @brief This function returns true if the watchdog would * have expired */ bool PIOS_WDG_Check() { - if(PIOS_DELAY_DiffuS(wdg_cleared_time) > 250000) { - if(!wdg_expired) - fprintf(stderr, "Watchdog fired!\r\n"); - wdg_expired = true; - } - return wdg_expired; -} \ No newline at end of file + if (PIOS_DELAY_DiffuS(wdg_cleared_time) > 250000) { + if (!wdg_expired) { + fprintf(stderr, "Watchdog fired!\r\n"); + } + wdg_expired = true; + } + return wdg_expired; +} diff --git a/flight/pios/osx/pios.h b/flight/pios/osx/pios.h index cc49e781e..2a59131bd 100644 --- a/flight/pios/osx/pios.h +++ b/flight/pios/osx/pios.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios.h + * @file pios.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Main PiOS header. + * @brief Main PiOS header. * - Central header for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,7 +39,7 @@ #include "queue.h" #include "semphr.h" -#define vPortInitialiseBlocks(); +#define vPortInitialiseBlocks() ; #endif /* C Lib Includes */ diff --git a/flight/pios/pios.h b/flight/pios/pios.h index 32b647ac2..35e8a0aec 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -1,24 +1,24 @@ /** ****************************************************************************** - * @file pios.h + * @file pios.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013 * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 * @brief Main PiOS header. * @see The GNU Public License (GPL) Version 3 *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/pios_sim_posix.h b/flight/pios/pios_sim_posix.h index 86fb8bf41..642c0b6e9 100644 --- a/flight/pios/pios_sim_posix.h +++ b/flight/pios/pios_sim_posix.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios.h + * @file pios.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Main PiOS header. + * @brief Main PiOS header. * - Central header for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/posix/pios_bl_helper.c b/flight/pios/posix/pios_bl_helper.c index a7920e5d2..ec122564b 100644 --- a/flight/pios/posix/pios_bl_helper.c +++ b/flight/pios/posix/pios_bl_helper.c @@ -35,19 +35,19 @@ uint32_t PIOS_BL_HELPER_CRC_Memory_Calc() { - return 0; + return 0; } extern const struct fw_version_info fw_version_blob; -void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t * array, uint8_t size) +void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t *array, uint8_t size) { - uint8_t * desc = (uint8_t *) &fw_version_blob; - for (uint32_t i = 0; i < size; i++) { - array[i] = desc[i]; - } + uint8_t *desc = (uint8_t *)&fw_version_blob; + + for (uint32_t i = 0; i < size; i++) { + array[i] = desc[i]; + } } void PIOS_BL_HELPER_CRC_Ini() -{ -} +{} #endif diff --git a/flight/pios/posix/pios_board_info.c b/flight/pios/posix/pios_board_info.c index c273a881d..2ef651a21 100644 --- a/flight/pios/posix/pios_board_info.c +++ b/flight/pios/posix/pios_board_info.c @@ -4,17 +4,17 @@ #include "pios_board_info.h" const struct pios_board_info pios_board_info_blob = { - .magic = PIOS_BOARD_INFO_BLOB_MAGIC, - .board_type = BOARD_TYPE, - .board_rev = BOARD_REVISION, - .bl_rev = BOOTLOADER_VERSION, - .hw_type = HW_TYPE, - .fw_base = FW_BANK_BASE, - .fw_size = FW_BANK_SIZE - FW_DESC_SIZE, - .desc_base = FW_BANK_BASE + FW_BANK_SIZE - FW_DESC_SIZE, - .desc_size = FW_DESC_SIZE, + .magic = PIOS_BOARD_INFO_BLOB_MAGIC, + .board_type = BOARD_TYPE, + .board_rev = BOARD_REVISION, + .bl_rev = BOOTLOADER_VERSION, + .hw_type = HW_TYPE, + .fw_base = FW_BANK_BASE, + .fw_size = FW_BANK_SIZE - FW_DESC_SIZE, + .desc_base = FW_BANK_BASE + FW_BANK_SIZE - FW_DESC_SIZE, + .desc_size = FW_DESC_SIZE, #ifdef EE_BANK_BASE - .ee_base = EE_BANK_BASE, - .ee_size = EE_BANK_SIZE, + .ee_base = EE_BANK_BASE, + .ee_size = EE_BANK_SIZE, #endif }; diff --git a/flight/pios/posix/pios_com.c b/flight/pios/posix/pios_com.c index 4f2e1e3c9..8eab84a4e 100644 --- a/flight/pios/posix/pios_com.c +++ b/flight/pios/posix/pios_com.c @@ -6,26 +6,26 @@ * @brief Hardware communication layer * @{ * - * @file pios_com.c + * @file pios_com.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM layer functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,462 +38,470 @@ #include #if !defined(PIOS_INCLUDE_FREERTOS) -#include "pios_delay.h" /* PIOS_DELAY_WaitmS */ +#include "pios_delay.h" /* PIOS_DELAY_WaitmS */ #endif enum pios_com_dev_magic { - PIOS_COM_DEV_MAGIC = 0xaa55aa55, + PIOS_COM_DEV_MAGIC = 0xaa55aa55, }; struct pios_com_dev { - enum pios_com_dev_magic magic; - uint32_t lower_id; - const struct pios_com_driver * driver; + enum pios_com_dev_magic magic; + uint32_t lower_id; + const struct pios_com_driver *driver; #if defined(PIOS_INCLUDE_FREERTOS) - xSemaphoreHandle tx_sem; - xSemaphoreHandle rx_sem; + xSemaphoreHandle tx_sem; + xSemaphoreHandle rx_sem; #endif - bool has_rx; - bool has_tx; + bool has_rx; + bool has_tx; - t_fifo_buffer rx; - t_fifo_buffer tx; + t_fifo_buffer rx; + t_fifo_buffer tx; }; -static bool PIOS_COM_validate(struct pios_com_dev * com_dev) +static bool PIOS_COM_validate(struct pios_com_dev *com_dev) { - return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC)); + return com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) && 0 // static struct pios_com_dev * PIOS_COM_alloc(void) -//{ -// struct pios_com_dev * com_dev; +// { +// struct pios_com_dev * com_dev; // -// com_dev = (struct pios_com_dev *)malloc(sizeof(*com_dev)); -// if (!com_dev) return (NULL); +// com_dev = (struct pios_com_dev *)malloc(sizeof(*com_dev)); +// if (!com_dev) return (NULL); // -// com_dev->magic = PIOS_COM_DEV_MAGIC; -// return(com_dev); -//} +// com_dev->magic = PIOS_COM_DEV_MAGIC; +// return(com_dev); +// } #else static struct pios_com_dev pios_com_devs[PIOS_COM_MAX_DEVS]; static uint8_t pios_com_num_devs; static uint32_t PIOS_COM_create(void) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) { - return (PIOS_COM_MAX_DEVS+1); - } + if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) { + return PIOS_COM_MAX_DEVS + 1; + } - com_dev = &pios_com_devs[pios_com_num_devs++]; - com_dev->magic = PIOS_COM_DEV_MAGIC; + com_dev = &pios_com_devs[pios_com_num_devs++]; + com_dev->magic = PIOS_COM_DEV_MAGIC; - return (pios_com_num_devs); + return pios_com_num_devs; } -static struct pios_com_dev * PIOS_COM_find_dev(uint32_t com_dev_id) +static struct pios_com_dev *PIOS_COM_find_dev(uint32_t com_dev_id) { - if (!com_dev_id) return NULL; - if (com_dev_id>pios_com_num_devs+1) return NULL; - return &pios_com_devs[com_dev_id-1]; + if (!com_dev_id) { + return NULL; + } + if (com_dev_id > pios_com_num_devs + 1) { + return NULL; + } + return &pios_com_devs[com_dev_id - 1]; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) && 0 */ -static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); -static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); -static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield); -static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield); +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); +static void PIOS_COM_UnblockRx(struct pios_com_dev *com_dev, bool *need_yield); +static void PIOS_COM_UnblockTx(struct pios_com_dev *com_dev, bool *need_yield); /** - * Initialises COM layer - * \param[out] handle - * \param[in] driver - * \param[in] id - * \return < 0 if initialisation failed - */ -int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len) + * Initialises COM layer + * \param[out] handle + * \param[in] driver + * \param[in] id + * \return < 0 if initialisation failed + */ +int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len) { - PIOS_Assert(com_id); - PIOS_Assert(driver); + PIOS_Assert(com_id); + PIOS_Assert(driver); - bool has_rx = (rx_buffer && rx_buffer_len > 0); - bool has_tx = (tx_buffer && tx_buffer_len > 0); - PIOS_Assert(has_rx || has_tx); - PIOS_Assert(driver->bind_tx_cb || !has_tx); - PIOS_Assert(driver->bind_rx_cb || !has_rx); + bool has_rx = (rx_buffer && rx_buffer_len > 0); + bool has_tx = (tx_buffer && tx_buffer_len > 0); + PIOS_Assert(has_rx || has_tx); + PIOS_Assert(driver->bind_tx_cb || !has_tx); + PIOS_Assert(driver->bind_rx_cb || !has_rx); - uint32_t com_dev_id; - struct pios_com_dev * com_dev; + uint32_t com_dev_id; + struct pios_com_dev *com_dev; - com_dev_id = PIOS_COM_create(); - com_dev = PIOS_COM_find_dev(com_dev_id); - if (!com_dev) goto out_fail; + com_dev_id = PIOS_COM_create(); + com_dev = PIOS_COM_find_dev(com_dev_id); + if (!com_dev) { + goto out_fail; + } - com_dev->driver = driver; - com_dev->lower_id = lower_id; + com_dev->driver = driver; + com_dev->lower_id = lower_id; - com_dev->has_rx = has_rx; - com_dev->has_tx = has_tx; + com_dev->has_rx = has_rx; + com_dev->has_tx = has_tx; - if (has_rx) { - fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len); + if (has_rx) { + fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len); #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(com_dev->rx_sem); -#endif /* PIOS_INCLUDE_FREERTOS */ - (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, com_dev_id); - if (com_dev->driver->rx_start) { - /* Start the receiver */ - (com_dev->driver->rx_start)(com_dev->lower_id, - fifoBuf_getFree(&com_dev->rx)); - } - } + vSemaphoreCreateBinary(com_dev->rx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, com_dev_id); + if (com_dev->driver->rx_start) { + /* Start the receiver */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } + } - if (has_tx) { - fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); + if (has_tx) { + fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(com_dev->tx_sem); -#endif /* PIOS_INCLUDE_FREERTOS */ - (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, com_dev_id); - } + vSemaphoreCreateBinary(com_dev->tx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, com_dev_id); + } - *com_id = com_dev_id; - return(0); + *com_id = com_dev_id; + return 0; out_fail: - return(-1); + return -1; } -static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield) +static void PIOS_COM_UnblockRx(struct pios_com_dev *com_dev, bool *need_yield) { #if defined(PIOS_INCLUDE_FREERTOS) - static signed portBASE_TYPE xHigherPriorityTaskWoken; - xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken); + static signed portBASE_TYPE xHigherPriorityTaskWoken; + xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken != pdFALSE) { - *need_yield = true; - } else { - *need_yield = false; - } + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } #else - *need_yield = false; + *need_yield = false; #endif } -static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield) +static void PIOS_COM_UnblockTx(struct pios_com_dev *com_dev, bool *need_yield) { #if defined(PIOS_INCLUDE_FREERTOS) - static signed portBASE_TYPE xHigherPriorityTaskWoken; - xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken); + static signed portBASE_TYPE xHigherPriorityTaskWoken; + xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken != pdFALSE) { - *need_yield = true; - } else { - *need_yield = false; - } + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } #else - *need_yield = false; + *need_yield = false; #endif } -static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(context); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(context); - bool valid = PIOS_COM_validate(com_dev); - PIOS_Assert(valid); - PIOS_Assert(com_dev->has_rx); + bool valid = PIOS_COM_validate(com_dev); - PIOS_IRQ_Disable(); - uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len); - PIOS_IRQ_Enable(); + PIOS_Assert(valid); + PIOS_Assert(com_dev->has_rx); - if (bytes_into_fifo > 0) { - /* Data has been added to the buffer */ - PIOS_COM_UnblockRx(com_dev, need_yield); - } + PIOS_IRQ_Disable(); + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len); + PIOS_IRQ_Enable(); - if (headroom) { - *headroom = fifoBuf_getFree(&com_dev->rx); - } + if (bytes_into_fifo > 0) { + /* Data has been added to the buffer */ + PIOS_COM_UnblockRx(com_dev, need_yield); + } - return (bytes_into_fifo); + if (headroom) { + *headroom = fifoBuf_getFree(&com_dev->rx); + } + + return bytes_into_fifo; } -static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(context); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(context); - bool valid = PIOS_COM_validate(com_dev); - PIOS_Assert(valid); - PIOS_Assert(buf); - PIOS_Assert(buf_len); - PIOS_Assert(com_dev->has_tx); + bool valid = PIOS_COM_validate(com_dev); - PIOS_IRQ_Disable(); - uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len); - PIOS_IRQ_Enable(); + PIOS_Assert(valid); + PIOS_Assert(buf); + PIOS_Assert(buf_len); + PIOS_Assert(com_dev->has_tx); - if (bytes_from_fifo > 0) { - /* More space has been made in the buffer */ - PIOS_COM_UnblockTx(com_dev, need_yield); - } + PIOS_IRQ_Disable(); + uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len); + PIOS_IRQ_Enable(); - if (headroom) { - *headroom = fifoBuf_getUsed(&com_dev->tx); - } + if (bytes_from_fifo > 0) { + /* More space has been made in the buffer */ + PIOS_COM_UnblockTx(com_dev, need_yield); + } - return (bytes_from_fifo); + if (headroom) { + *headroom = fifoBuf_getUsed(&com_dev->tx); + } + + return bytes_from_fifo; } /** -* Change the port speed without re-initializing -* \param[in] port COM port -* \param[in] baud Requested baud rate -* \return -1 if port not available -* \return 0 on success -*/ + * Change the port speed without re-initializing + * \param[in] port COM port + * \param[in] baud Requested baud rate + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - /* Invoke the driver function if it exists */ - if (com_dev->driver->set_baud) { - com_dev->driver->set_baud(com_dev->lower_id, baud); - } + /* Invoke the driver function if it exists */ + if (com_dev->driver->set_baud) { + com_dev->driver->set_baud(com_dev->lower_id, baud); + } - return 0; + return 0; } /** -* Sends a package over given port -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a package over given port + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - PIOS_Assert(com_dev->has_tx); + PIOS_Assert(com_dev->has_tx); - if (len >= fifoBuf_getFree(&com_dev->tx)) { - /* Buffer cannot accept all requested bytes (retry) */ - return -2; - } + if (len >= fifoBuf_getFree(&com_dev->tx)) { + /* Buffer cannot accept all requested bytes (retry) */ + return -2; + } - PIOS_IRQ_Disable(); - uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len); - PIOS_IRQ_Enable(); + PIOS_IRQ_Disable(); + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len); + PIOS_IRQ_Enable(); - if (bytes_into_fifo > 0) { - /* More data has been put in the tx buffer, make sure the tx is started */ - if (com_dev->driver->tx_start) { - com_dev->driver->tx_start(com_dev->lower_id, - fifoBuf_getUsed(&com_dev->tx)); - } - } + if (bytes_into_fifo > 0) { + /* More data has been put in the tx buffer, make sure the tx is started */ + if (com_dev->driver->tx_start) { + com_dev->driver->tx_start(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } + } - return (0); + return 0; } /** -* Sends a package over given port -* (blocking function) -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a package over given port + * (blocking function) + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - PIOS_Assert(com_dev->has_tx); + PIOS_Assert(com_dev->has_tx); - int32_t rc; - do { - rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len); + int32_t rc; + do { + rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len); #if defined(PIOS_INCLUDE_FREERTOS) - if (rc == -2) { - /* Make sure the transmitter is running while we wait */ - if (com_dev->driver->tx_start) { - (com_dev->driver->tx_start)(com_dev->lower_id, - fifoBuf_getUsed(&com_dev->tx)); - } - if (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) { - return -3; - } - } + if (rc == -2) { + /* Make sure the transmitter is running while we wait */ + if (com_dev->driver->tx_start) { + (com_dev->driver->tx_start)(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } + if (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) { + return -3; + } + } #endif - } while (rc == -2); + } while (rc == -2); - return rc; + return rc; } /** -* Sends a single character over given port -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a single character over given port + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c) { - return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1); + return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1); } /** -* Sends a single character over given port -* (blocking function) -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a single character over given port + * (blocking function) + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendChar(uint32_t com_id, char c) { - return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1); + return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1); } /** -* Sends a string over given port -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a string over given port + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str) { - return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str)); + return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str)); } /** -* Sends a string over given port -* (blocking function) -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a string over given port + * (blocking function) + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendString(uint32_t com_id, const char *str) { - return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str)); + return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str)); } /** -* Sends a formatted string (-> printf) over given port -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* 128 characters supported maximum! -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * 128 characters supported maximum! + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Sends a formatted string (-> printf) over given port -* (blocking function) -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * (blocking function) + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Transfer bytes from port buffers into another buffer -* \param[in] port COM port -* \returns Byte from buffer -*/ -uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms) + * Transfer bytes from port buffers into another buffer + * \param[in] port COM port + * \returns Byte from buffer + */ +uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t *buf, uint16_t buf_len, uint32_t timeout_ms) { - PIOS_Assert(buf); - PIOS_Assert(buf_len); + PIOS_Assert(buf); + PIOS_Assert(buf_len); - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - PIOS_Assert(0); - } - PIOS_Assert(com_dev->has_rx); + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + PIOS_Assert(0); + } + PIOS_Assert(com_dev->has_rx); - check_again: - PIOS_IRQ_Disable(); - uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len); - PIOS_IRQ_Enable(); +check_again: + PIOS_IRQ_Disable(); + uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len); + PIOS_IRQ_Enable(); - if (bytes_from_fifo == 0 && timeout_ms > 0) { - /* No more bytes in receive buffer */ - /* Make sure the receiver is running while we wait */ - if (com_dev->driver->rx_start) { - /* Notify the lower layer that there is now room in the rx buffer */ - (com_dev->driver->rx_start)(com_dev->lower_id, - fifoBuf_getFree(&com_dev->rx)); - } + if (bytes_from_fifo == 0 && timeout_ms > 0) { + /* No more bytes in receive buffer */ + /* Make sure the receiver is running while we wait */ + if (com_dev->driver->rx_start) { + /* Notify the lower layer that there is now room in the rx buffer */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) { - /* Make sure we don't come back here again */ - timeout_ms = 0; - goto check_again; - } + if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) { + /* Make sure we don't come back here again */ + timeout_ms = 0; + goto check_again; + } #else - PIOS_DELAY_WaitmS(1); - timeout_ms--; - goto check_again; + PIOS_DELAY_WaitmS(1); + timeout_ms--; + goto check_again; #endif - } + } - /* Return received byte */ - return (bytes_from_fifo); + /* Return received byte */ + return bytes_from_fifo; } /** @@ -503,21 +511,22 @@ uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len */ bool PIOS_COM_Available(uint32_t com_id) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - return false; - } + if (!PIOS_COM_validate(com_dev)) { + return false; + } - // If a driver does not provide a query method assume always - // available if valid - if (com_dev->driver->available == NULL) - return true; + // If a driver does not provide a query method assume always + // available if valid + if (com_dev->driver->available == NULL) { + return true; + } - return (com_dev->driver->available)(com_dev->lower_id); + return (com_dev->driver->available)(com_dev->lower_id); } -#endif +#endif /* if defined(PIOS_INCLUDE_COM) */ /** * @} diff --git a/flight/pios/posix/pios_crc.c b/flight/pios/posix/pios_crc.c index 548ba2648..0e4cd5dec 100644 --- a/flight/pios/posix/pios_crc.c +++ b/flight/pios/posix/pios_crc.c @@ -11,22 +11,22 @@ // CRC lookup table static const uint8_t crc_table[256] = { - 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, - 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, - 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, - 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, - 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, - 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, - 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, - 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, - 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, - 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, - 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, - 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, - 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, - 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, - 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, - 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 + 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, + 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, + 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, + 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, + 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, + 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, + 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, + 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, + 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, + 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, + 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, + 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, + 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, + 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, + 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, + 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 }; /** @@ -49,7 +49,7 @@ static const uint8_t crc_table[256] = { */ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) { - return crc_table[crc ^ data]; + return crc_table[crc ^ data]; } /* @@ -59,16 +59,16 @@ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) * @param[in] length Number of bytes to process * @returns Updated CRC */ -uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length) +uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t *data, int32_t length) { - // use registers for speed - register int32_t len = length; - register uint8_t crc8 = crc; - register const uint8_t *p = data; - - while (len--) - crc8 = crc_table[crc8 ^ *p++]; - - return crc8; -} + // use registers for speed + register int32_t len = length; + register uint8_t crc8 = crc; + register const uint8_t *p = data; + while (len--) { + crc8 = crc_table[crc8 ^ *p++]; + } + + return crc8; +} diff --git a/flight/pios/posix/pios_debug.c b/flight/pios/posix/pios_debug.c index 02d41ee64..ac67e89a0 100644 --- a/flight/pios/posix/pios_debug.c +++ b/flight/pios/posix/pios_debug.c @@ -10,21 +10,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Debugging Functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,36 +37,31 @@ const char *PIOS_DEBUG_AssertMsg = "ASSERT FAILED"; /* Private Function Prototypes */ /** -* Initialise Debug-features -*/ + * Initialise Debug-features + */ void PIOS_DEBUG_Init(void) -{ -} +{} /** -* Set debug-pin high -* \param pin 0 for S1 output -*/ + * Set debug-pin high + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinHigh(uint8_t Pin) -{ -} +{} /** -* Set debug-pin low -* \param pin 0 for S1 output -*/ + * Set debug-pin low + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinLow(uint8_t Pin) -{ -} +{} void PIOS_DEBUG_PinValue8Bit(uint8_t value) -{ -} +{} void PIOS_DEBUG_PinValue4BitL(uint8_t value) -{ -} +{} /** @@ -75,21 +70,20 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value) void PIOS_DEBUG_Panic(const char *msg) { #ifdef PIOS_COM_AUX - PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s\r", msg); + PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s\r", msg); #endif - // tell the user whats going on on commandline too - fprintf(stderr,"CRITICAL ERROR: %s\n",msg); - - // this helps debugging: causing a div by zero allows a backtrace - // and/or ends execution - int b = 0; - int a = (2/b); - b=a; + // tell the user whats going on on commandline too + fprintf(stderr, "CRITICAL ERROR: %s\n", msg); + // this helps debugging: causing a div by zero allows a backtrace + // and/or ends execution + int b = 0; + int a = (2 / b); + b = a; } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/posix/pios_delay.c b/flight/pios/posix/pios_delay.c index f871e11fa..d812613f1 100644 --- a/flight/pios/posix/pios_delay.c +++ b/flight/pios/posix/pios_delay.c @@ -3,27 +3,27 @@ * * @file pios_delay.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) - * @brief Delay Functions - * - Provides a micro-second granular delay using a TIM + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * @brief Delay Functions + * - Provides a micro-second granular delay using a TIM * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_DELAY Delay Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,80 +34,83 @@ #if defined(PIOS_INCLUDE_DELAY) /** -* Initialises the Timer used by PIOS_DELAY functions
-* This is called from pios.c as part of the main() function -* at system start up. -* \return < 0 if initialisation failed -*/ + * Initialises the Timer used by PIOS_DELAY functions
+ * This is called from pios.c as part of the main() function + * at system start up. + * \return < 0 if initialisation failed + */ #include int32_t PIOS_DELAY_Init(void) { - // stub + // stub - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Waits for a specific number of uS
-* Example:
-* \code -* // Wait for 500 uS -* PIOS_DELAY_Wait_uS(500); -* \endcode -* \param[in] uS delay (1..65535 microseconds) -* \return < 0 on errors -*/ + * Waits for a specific number of uS
+ * Example:
+ * \code + * // Wait for 500 uS + * PIOS_DELAY_Wait_uS(500); + * \endcode + * \param[in] uS delay (1..65535 microseconds) + * \return < 0 on errors + */ int32_t PIOS_DELAY_WaituS(uint32_t uS) { - static struct timespec wait,rest; - wait.tv_sec=0; - wait.tv_nsec=1000*uS; - while (nanosleep(&wait,&rest)!=0) { - wait=rest; - } + static struct timespec wait, rest; - /* No error */ - return 0; + wait.tv_sec = 0; + wait.tv_nsec = 1000 * uS; + while (nanosleep(&wait, &rest) != 0) { + wait = rest; + } + + /* No error */ + return 0; } /** -* Waits for a specific number of mS
-* Example:
-* \code -* // Wait for 500 mS -* PIOS_DELAY_Wait_mS(500); -* \endcode -* \param[in] mS delay (1..65535 milliseconds) -* \return < 0 on errors -*/ + * Waits for a specific number of mS
+ * Example:
+ * \code + * // Wait for 500 mS + * PIOS_DELAY_Wait_mS(500); + * \endcode + * \param[in] mS delay (1..65535 milliseconds) + * \return < 0 on errors + */ int32_t PIOS_DELAY_WaitmS(uint32_t mS) { - //for(int i = 0; i < mS; i++) { - // PIOS_DELAY_WaituS(1000); - static struct timespec wait,rest; - wait.tv_sec=mS/1000; - wait.tv_nsec=(mS%1000)*1000000; - while (nanosleep(&wait,&rest)!=0) { - wait=rest; - } - //} + // for(int i = 0; i < mS; i++) { + // PIOS_DELAY_WaituS(1000); + static struct timespec wait, rest; - /* No error */ - return 0; + wait.tv_sec = mS / 1000; + wait.tv_nsec = (mS % 1000) * 1000000; + while (nanosleep(&wait, &rest) != 0) { + wait = rest; + } + // } + + /* No error */ + return 0; } /** - * @brief Query the Delay timer for the current uS + * @brief Query the Delay timer for the current uS * @return A microsecond value */ uint32_t PIOS_DELAY_GetuS() { - static struct timespec current; - clock_gettime(CLOCK_REALTIME, ¤t); - return ((current.tv_sec * 1000000) + (current.tv_nsec / 1000)); + static struct timespec current; + + clock_gettime(CLOCK_REALTIME, ¤t); + return (current.tv_sec * 1000000) + (current.tv_nsec / 1000); } /** @@ -117,7 +120,7 @@ uint32_t PIOS_DELAY_GetuS() */ uint32_t PIOS_DELAY_GetuSSince(uint32_t t) { - return (PIOS_DELAY_GetuS() - t); + return PIOS_DELAY_GetuS() - t; } /** @@ -126,17 +129,17 @@ uint32_t PIOS_DELAY_GetuSSince(uint32_t t) */ uint32_t PIOS_DELAY_GetRaw() { - return (PIOS_DELAY_GetuS()); + return PIOS_DELAY_GetuS(); } /** - * @brief Compare to raw times to and convert to us + * @brief Compare to raw times to and convert to us * @return A microsecond value */ uint32_t PIOS_DELAY_DiffuS(uint32_t raw) { - return ( PIOS_DELAY_GetuS() - raw ); + return PIOS_DELAY_GetuS() - raw; } -#endif +#endif /* if defined(PIOS_INCLUDE_DELAY) */ diff --git a/flight/pios/posix/pios_iap.c b/flight/pios/posix/pios_iap.c index aef9f6c54..e38bf1d6d 100644 --- a/flight/pios/posix/pios_iap.c +++ b/flight/pios/posix/pios_iap.c @@ -1,6 +1,6 @@ /*! - * @File iap.c - * @Brief + * @File iap.c + * @Brief * * Created on: Sep 6, 2010 * Author: joe @@ -8,8 +8,8 @@ /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ #include /*! @@ -20,50 +20,42 @@ * * Created: Sep 8, 2010 10:10:48 PM by joe */ -void PIOS_IAP_Init( void ) -{ - -} +void PIOS_IAP_Init(void) +{} /*! * \brief Determines if an In-Application-Programming request has been made. * \param *comm - Which communication stream to use for the IAP (USB, Telemetry, I2C, SPI, etc) * \return TRUE - if correct sequence found, along with 'comm' updated. - * FALSE - Note that 'comm' will have an invalid comm identifier. + * FALSE - Note that 'comm' will have an invalid comm identifier. * \retval * */ -uint32_t PIOS_IAP_CheckRequest( void ) +uint32_t PIOS_IAP_CheckRequest(void) { - - return false; + return false; } - /*! * \brief Sets the 1st word of the request sequence. * \param n/a * \return n/a * \retval */ -void PIOS_IAP_SetRequest1(void) -{ -} +void PIOS_IAP_SetRequest1(void) +{} -void PIOS_IAP_SetRequest2(void) -{ -} +void PIOS_IAP_SetRequest2(void) +{} -void PIOS_IAP_ClearRequest(void) -{ -} +void PIOS_IAP_ClearRequest(void) +{} uint16_t PIOS_IAP_ReadBootCount(void) { - return 0; + return 0; } -void PIOS_IAP_WriteBootCount (uint16_t boot_count) -{ -} +void PIOS_IAP_WriteBootCount(uint16_t boot_count) +{} diff --git a/flight/pios/posix/pios_irq.c b/flight/pios/posix/pios_irq.c index b30d09fee..793df9ccf 100644 --- a/flight/pios/posix/pios_irq.c +++ b/flight/pios/posix/pios_irq.c @@ -6,26 +6,26 @@ * @brief STM32 Hardware code to enable and disable interrupts * @{ * - * @file pios_irq.c + * @file pios_irq.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief IRQ Enable/Disable routines * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,33 +37,33 @@ /* Private Function Prototypes */ /** -* Disables all interrupts (nested) -* \return < 0 On errors -*/ + * Disables all interrupts (nested) + * \return < 0 On errors + */ int32_t PIOS_IRQ_Disable(void) { #if defined(PIOS_INCLUDE_FREERTOS) - taskENTER_CRITICAL(); + taskENTER_CRITICAL(); #endif - return 0; + return 0; } /** -* Enables all interrupts (nested) -* \return < 0 on errors -* \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) -*/ + * Enables all interrupts (nested) + * \return < 0 on errors + * \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) + */ int32_t PIOS_IRQ_Enable(void) { #if defined(PIOS_INCLUDE_FREERTOS) - taskEXIT_CRITICAL(); + taskEXIT_CRITICAL(); #endif - return 0; + return 0; } -#endif +#endif /* if defined(PIOS_INCLUDE_IRQ) */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/posix/pios_led.c b/flight/pios/posix/pios_led.c index db833e1c3..c3bd4e27d 100644 --- a/flight/pios/posix/pios_led.c +++ b/flight/pios/posix/pios_led.c @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file pios_led.c + * @file pios_led.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief LED functions, init, toggle, on & off. * @see The GNU Public License (GPL) Version 3 @@ -9,19 +9,19 @@ * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,68 +35,69 @@ /* Local Variables */ -//static GPIO_TypeDef* LED_GPIO_PORT[PIOS_LED_NUM] = PIOS_LED_PORTS; -//static const uint32_t LED_GPIO_PIN[PIOS_LED_NUM] = PIOS_LED_PINS; -//static const uint32_t LED_GPIO_CLK[PIOS_LED_NUM] = PIOS_LED_CLKS; +// static GPIO_TypeDef* LED_GPIO_PORT[PIOS_LED_NUM] = PIOS_LED_PORTS; +// static const uint32_t LED_GPIO_PIN[PIOS_LED_NUM] = PIOS_LED_PINS; +// static const uint32_t LED_GPIO_CLK[PIOS_LED_NUM] = PIOS_LED_CLKS; static uint8_t LED_GPIO[PIOS_LED_NUM]; -static inline void PIOS_SetLED(uint32_t LED,uint8_t stat) { - printf("PIOS: LED %i status %i\n",LED,stat); - LED_GPIO[LED]=stat; +static inline void PIOS_SetLED(uint32_t LED, uint8_t stat) +{ + printf("PIOS: LED %i status %i\n", LED, stat); + LED_GPIO[LED] = stat; } /** -* Initialises all the LED's -*/ + * Initialises all the LED's + */ void PIOS_LED_Init(void) { - //GPIO_InitTypeDef GPIO_InitStructure; - //GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - - for(int LEDNum = 0; LEDNum < PIOS_LED_NUM; LEDNum++) { - //RCC_APB2PeriphClockCmd(LED_GPIO_CLK[LEDNum], ENABLE); - //GPIO_InitStructure.GPIO_Pin = LED_GPIO_PIN[LEDNum]; - //GPIO_Init(LED_GPIO_PORT[LEDNum], &GPIO_InitStructure); + // GPIO_InitTypeDef GPIO_InitStructure; + // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; + // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - /* LED's Off */ - //LED_GPIO_PORT[LEDNum]->BSRR = LED_GPIO_PIN[LEDNum]; - LED_GPIO[LEDNum]=0; - } + for (int LEDNum = 0; LEDNum < PIOS_LED_NUM; LEDNum++) { + // RCC_APB2PeriphClockCmd(LED_GPIO_CLK[LEDNum], ENABLE); + // GPIO_InitStructure.GPIO_Pin = LED_GPIO_PIN[LEDNum]; + // GPIO_Init(LED_GPIO_PORT[LEDNum], &GPIO_InitStructure); + + /* LED's Off */ + // LED_GPIO_PORT[LEDNum]->BSRR = LED_GPIO_PIN[LEDNum]; + LED_GPIO[LEDNum] = 0; + } } /** -* Turn on LED -* \param[in] LED LED Name (LED1, LED2) -*/ + * Turn on LED + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_On(uint32_t LED) { - //LED_GPIO_PORT[LED]->BRR = LED_GPIO_PIN[LED]; - PIOS_SetLED(LED,1); + // LED_GPIO_PORT[LED]->BRR = LED_GPIO_PIN[LED]; + PIOS_SetLED(LED, 1); } /** -* Turn off LED -* \param[in] LED LED Name (LED1, LED2) -*/ + * Turn off LED + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_Off(uint32_t LED) { - //LED_GPIO_PORT[LED]->BSRR = LED_GPIO_PIN[LED]; - PIOS_SetLED(LED,0); + // LED_GPIO_PORT[LED]->BSRR = LED_GPIO_PIN[LED]; + PIOS_SetLED(LED, 0); } /** -* Toggle LED on/off -* \param[in] LED LED Name (LED1, LED2) -*/ + * Toggle LED on/off + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_Toggle(uint32_t LED) { - //LED_GPIO_PORT[LED]->ODR ^= LED_GPIO_PIN[LED]; - PIOS_SetLED(LED,LED_GPIO[LED]?0:1); + // LED_GPIO_PORT[LED]->ODR ^= LED_GPIO_PIN[LED]; + PIOS_SetLED(LED, LED_GPIO[LED] ? 0 : 1); } -#endif +#endif /* if defined(PIOS_INCLUDE_LED) */ diff --git a/flight/pios/posix/pios_rcvr.c b/flight/pios/posix/pios_rcvr.c index 719cb9769..362ceb1a8 100644 --- a/flight/pios/posix/pios_rcvr.c +++ b/flight/pios/posix/pios_rcvr.c @@ -6,83 +6,89 @@ #include enum pios_rcvr_dev_magic { - PIOS_RCVR_DEV_MAGIC = 0x99aabbcc, + PIOS_RCVR_DEV_MAGIC = 0x99aabbcc, }; struct pios_rcvr_dev { - enum pios_rcvr_dev_magic magic; - uint32_t lower_id; - const struct pios_rcvr_driver * driver; + enum pios_rcvr_dev_magic magic; + uint32_t lower_id; + const struct pios_rcvr_driver *driver; }; -static bool PIOS_RCVR_validate(struct pios_rcvr_dev * rcvr_dev) +static bool PIOS_RCVR_validate(struct pios_rcvr_dev *rcvr_dev) { - return (rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC); + return rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) && 0 -//static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) -//{ -// struct pios_rcvr_dev * rcvr_dev; +// static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) +// { +// struct pios_rcvr_dev * rcvr_dev; // -// rcvr_dev = (struct pios_rcvr_dev *)pvPortMalloc(sizeof(*rcvr_dev)); -// if (!rcvr_dev) return (NULL); +// rcvr_dev = (struct pios_rcvr_dev *)pvPortMalloc(sizeof(*rcvr_dev)); +// if (!rcvr_dev) return (NULL); // -// rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; -// return(rcvr_dev); -//} +// rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; +// return(rcvr_dev); +// } #else static struct pios_rcvr_dev pios_rcvr_devs[PIOS_RCVR_MAX_DEVS]; static uint8_t pios_rcvr_num_devs; static uint32_t PIOS_RCVR_alloc(void) { - struct pios_rcvr_dev * rcvr_dev; + struct pios_rcvr_dev *rcvr_dev; - if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) { - return (PIOS_RCVR_MAX_DEVS+1); - } + if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) { + return PIOS_RCVR_MAX_DEVS + 1; + } - rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++]; - rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; + rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++]; + rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; - return (pios_rcvr_num_devs); + return pios_rcvr_num_devs; } -static struct pios_rcvr_dev * PIOS_RCVR_find_dev(uint32_t rcvr_dev_id) +static struct pios_rcvr_dev *PIOS_RCVR_find_dev(uint32_t rcvr_dev_id) { - if (!rcvr_dev_id) return NULL; - if (rcvr_dev_id>pios_rcvr_num_devs+1) return NULL; - return &pios_rcvr_devs[rcvr_dev_id-1]; + if (!rcvr_dev_id) { + return NULL; + } + if (rcvr_dev_id > pios_rcvr_num_devs + 1) { + return NULL; + } + return &pios_rcvr_devs[rcvr_dev_id - 1]; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) && 0 */ /** - * Initialises RCVR layer - * \param[out] handle - * \param[in] driver - * \param[in] id - * \return < 0 if initialisation failed - */ -int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, uint32_t lower_id) + * Initialises RCVR layer + * \param[out] handle + * \param[in] driver + * \param[in] id + * \return < 0 if initialisation failed + */ +int32_t PIOS_RCVR_Init(uint32_t *rcvr_id, const struct pios_rcvr_driver *driver, uint32_t lower_id) { - PIOS_DEBUG_Assert(rcvr_id); - PIOS_DEBUG_Assert(driver); + PIOS_DEBUG_Assert(rcvr_id); + PIOS_DEBUG_Assert(driver); - uint32_t rcvr_dev_id; - struct pios_rcvr_dev * rcvr_dev; + uint32_t rcvr_dev_id; + struct pios_rcvr_dev *rcvr_dev; - rcvr_dev_id = PIOS_RCVR_alloc(); - rcvr_dev = PIOS_RCVR_find_dev(rcvr_dev_id); - if (!rcvr_dev) goto out_fail; + rcvr_dev_id = PIOS_RCVR_alloc(); + rcvr_dev = PIOS_RCVR_find_dev(rcvr_dev_id); + if (!rcvr_dev) { + goto out_fail; + } - rcvr_dev->driver = driver; - rcvr_dev->lower_id = lower_id; + rcvr_dev->driver = driver; + rcvr_dev->lower_id = lower_id; - *rcvr_id = rcvr_dev_id; - return(0); + *rcvr_id = rcvr_dev_id; + return 0; out_fail: - return(-1); + return -1; } /** @@ -96,28 +102,30 @@ out_fail: */ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) { - // Publicly facing API uses channel 1 for first channel - if(channel == 0) - return PIOS_RCVR_INVALID; - else - channel--; + // Publicly facing API uses channel 1 for first channel + if (channel == 0) { + return PIOS_RCVR_INVALID; + } else { + channel--; + } - if (rcvr_id == 0) - return PIOS_RCVR_NODRIVER; + if (rcvr_id == 0) { + return PIOS_RCVR_NODRIVER; + } - struct pios_rcvr_dev * rcvr_dev = PIOS_RCVR_find_dev(rcvr_id); + struct pios_rcvr_dev *rcvr_dev = PIOS_RCVR_find_dev(rcvr_id); - if (!PIOS_RCVR_validate(rcvr_dev)) { - /* Undefined RCVR port for this board (see pios_board.c) */ - PIOS_Assert(0); - } + if (!PIOS_RCVR_validate(rcvr_dev)) { + /* Undefined RCVR port for this board (see pios_board.c) */ + PIOS_Assert(0); + } - PIOS_DEBUG_Assert(rcvr_dev->driver->read); + PIOS_DEBUG_Assert(rcvr_dev->driver->read); - return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); + return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); } -#endif +#endif /* if defined(PIOS_INCLUDE_RCVR) */ /** * @} diff --git a/flight/pios/posix/pios_sdcard.c b/flight/pios/posix/pios_sdcard.c index 9aff090f0..ff7f330cd 100644 --- a/flight/pios/posix/pios_sdcard.c +++ b/flight/pios/posix/pios_sdcard.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_sdcard.c + * @file pios_sdcard.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief Sets up basic system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_SDCARD SDCard Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,186 +34,186 @@ /** -* Initialises SPI pins and peripheral to access MMC/SD Card -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ + * Initialises SPI pins and peripheral to access MMC/SD Card + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ int32_t PIOS_SDCARD_Init(uint32_t spi_id) { - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Connects to SD Card -* \return < 0 if initialisation sequence failed -*/ + * Connects to SD Card + * \return < 0 if initialisation sequence failed + */ int32_t PIOS_SDCARD_PowerOn(void) { - return 0; /* Status should be 0 if nothing went wrong! */ + return 0; /* Status should be 0 if nothing went wrong! */ } /** -* Disconnects from SD Card -* \return < 0 on errors -*/ + * Disconnects from SD Card + * \return < 0 on errors + */ int32_t PIOS_SDCARD_PowerOff(void) { - return 0; // no error + return 0; // no error } /** -* If SD card was previously available: Checks if the SD Card is still -* available by sending the STATUS command.
-* This takes ca. 10 uS -* -* If SD card was previously not available: Checks if the SD Card is -* available by sending the IDLE command at low speed.
-* This takes ca. 500 uS!
-* Once we got a positive response, SDCARD_PowerOn() will be -* called by this function to initialize the card completely. -* -* Example for Connection/Disconnection detection: -* \code -* // this function is called each second from a low-priority task -* // If multiple tasks are accessing the SD card, add a semaphore/mutex -* // to avoid IO access collisions with other tasks! -* u8 sdcard_available; -* int32_t CheckSDCard(void) -* { -* // check if SD card is available -* // High speed access if the card was previously available -* u8 prev_sdcard_available = sdcard_available; -* sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); -* -* if(sdcard_available && !prev_sdcard_available) { -* // SD Card has been connected -* -* // now it's possible to read/write sectors -* -* } else if( !sdcard_available && prev_sdcard_available ) { -* // SD Card has been disconnected -* -* // here you can notify your application about this state -* } -* -* return 0; // no error -* } -* \endcode -* \param[in] was_available should only be set if the SD card was previously available -* \return 0 if no response from SD Card -* \return 1 if SD card is accessible -*/ + * If SD card was previously available: Checks if the SD Card is still + * available by sending the STATUS command.
+ * This takes ca. 10 uS + * + * If SD card was previously not available: Checks if the SD Card is + * available by sending the IDLE command at low speed.
+ * This takes ca. 500 uS!
+ * Once we got a positive response, SDCARD_PowerOn() will be + * called by this function to initialize the card completely. + * + * Example for Connection/Disconnection detection: + * \code + * // this function is called each second from a low-priority task + * // If multiple tasks are accessing the SD card, add a semaphore/mutex + * // to avoid IO access collisions with other tasks! + * u8 sdcard_available; + * int32_t CheckSDCard(void) + * { + * // check if SD card is available + * // High speed access if the card was previously available + * u8 prev_sdcard_available = sdcard_available; + * sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); + * + * if(sdcard_available && !prev_sdcard_available) { + * // SD Card has been connected + * + * // now it's possible to read/write sectors + * + * } else if( !sdcard_available && prev_sdcard_available ) { + * // SD Card has been disconnected + * + * // here you can notify your application about this state + * } + * + * return 0; // no error + * } + * \endcode + * \param[in] was_available should only be set if the SD card was previously available + * \return 0 if no response from SD Card + * \return 1 if SD card is accessible + */ int32_t PIOS_SDCARD_CheckAvailable(uint8_t was_available) { - return 1; /* 1 = available, 0 = not available. */ + return 1; /* 1 = available, 0 = not available. */ } /** -* Sends command to SD card -* \param[in] cmd SD card command -* \param[in] addr 32bit address -* \param[in] crc precalculated CRC -* \return >= 0x00 if command has been sent successfully (contains received byte) -* \return -1 if no response from SD Card (timeout) -*/ + * Sends command to SD card + * \param[in] cmd SD card command + * \param[in] addr 32bit address + * \param[in] crc precalculated CRC + * \return >= 0x00 if command has been sent successfully (contains received byte) + * \return -1 if no response from SD Card (timeout) + */ int32_t PIOS_SDCARD_SendSDCCmd(uint8_t cmd, uint32_t addr, uint8_t crc) { - return -1; + return -1; } /** -* Reads 512 bytes from selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads 512 bytes from selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_SectorRead(uint32_t sector, uint8_t *buffer) { - return -256; + return -256; } /** -* Writes 512 bytes into selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if write operation not accepted -* \return -258 if timeout during write operation -*/ + * Writes 512 bytes into selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if write operation not accepted + * \return -258 if timeout during write operation + */ int32_t PIOS_SDCARD_SectorWrite(uint32_t sector, uint8_t *buffer) { - return -256; + return -256; } /** -* Reads the CID informations from SD Card -* \param[in] *cid pointer to buffer which holds the CID informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads the CID informations from SD Card + * \param[in] *cid pointer to buffer which holds the CID informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_CIDRead(SDCARDCidTypeDef *cid) { - return -256; + return -256; } /** -* Reads the CSD informations from SD Card -* \param[in] *csd pointer to buffer which holds the CSD informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads the CSD informations from SD Card + * \param[in] *csd pointer to buffer which holds the CSD informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_CSDRead(SDCARDCsdTypeDef *csd) { - return -256; + return -256; } /** -* Attempts to write a startup log to the SDCard -* return 0 No errors -* return -1 Error deleting file -* return -2 Error opening file -* return -3 Error writing file -*/ + * Attempts to write a startup log to the SDCard + * return 0 No errors + * return -1 Error deleting file + * return -2 Error opening file + * return -3 Error writing file + */ int32_t PIOS_SDCARD_StartupLog(void) { - return -3; + return -3; } /** @@ -223,78 +223,78 @@ int32_t PIOS_SDCARD_StartupLog(void) */ int32_t PIOS_SDCARD_IsMounted() { - return 1; + return 1; } /** -* Mounts the file system -* param[in] CreateStartupLog 1 = True, 0 = False -* return 0 No errors -* return -1 SDCard not available -* return -2 Cannot find first partition -* return -3 No volume information -* return -4 Error writing startup log file -*/ + * Mounts the file system + * param[in] CreateStartupLog 1 = True, 0 = False + * return 0 No errors + * return -1 SDCard not available + * return -2 Cannot find first partition + * return -3 No volume information + * return -4 Error writing startup log file + */ int32_t PIOS_SDCARD_MountFS(uint32_t CreateStartupLog) { - return 0; + return 0; } /** -* Mounts the file system -* return Amount of free bytes -*/ + * Mounts the file system + * return Amount of free bytes + */ int32_t PIOS_SDCARD_GetFree(void) { - return 10240; + return 10240; } /** -* Read from file -* return 0 No error -* return -1 DFS_ReadFile failed -* return -2 Less bytes read than expected -*/ -//int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len) -//{ - /* No error */ -// return -1; -//} + * Read from file + * return 0 No error + * return -1 DFS_ReadFile failed + * return -2 Less bytes read than expected + */ +// int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len) +// { +/* No error */ +// return -1; +// } /** -* Read a line from file -* returns Number of bytes read -*/ -//int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len) -//{ -// return -1; -//} + * Read a line from file + * returns Number of bytes read + */ +// int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len) +// { +// return -1; +// } /** -* Copy a file -* WARNING: This will overwrite the destination file even if it exists -* param[in] *Source Path to file to copy -* param[in] *Destination Path to destination file -* return 0 No errors -* return -1 Source file doesn't exist -* return -2 Failed to create destination file -* return -3 DFS_ReadFile failed -* return -4 DFS_WriteFile failed -*/ + * Copy a file + * WARNING: This will overwrite the destination file even if it exists + * param[in] *Source Path to file to copy + * param[in] *Destination Path to destination file + * return 0 No errors + * return -1 Source file doesn't exist + * return -2 Failed to create destination file + * return -3 DFS_ReadFile failed + * return -4 DFS_WriteFile failed + */ int32_t PIOS_SDCARD_FileCopy(char *Source, char *Destination) { - return -2; + return -2; } /** -* Delete a file -* param[in] *Filename File to delete -* return 0 No errors -* return -1 Error deleting file -*/ + * Delete a file + * param[in] *Filename File to delete + * return 0 No errors + * return -1 Error deleting file + */ int32_t PIOS_SDCARD_FileDelete(char *Filename) { - return -1; + return -1; } -#endif +#endif /* if defined(PIOS_INCLUDE_SDCARD) */ diff --git a/flight/pios/posix/pios_servo.c b/flight/pios/posix/pios_servo.c index d0cf530d3..c3b4ba05b 100644 --- a/flight/pios/posix/pios_servo.c +++ b/flight/pios/posix/pios_servo.c @@ -6,25 +6,25 @@ * @brief Code to do set RC servo output * @{ * - * @file pios_servo.c + * @file pios_servo.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RC Servo routines (STM32 dependent) * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,36 +39,33 @@ static volatile uint16_t ServoPosition[PIOS_SERVO_NUM_TIMERS]; /** -* Initialise Servos -*/ + * Initialise Servos + */ void PIOS_Servo_Init(void) -{ -} +{} /** -* Set the servo update rate (Max 500Hz) -* \param[in] onetofour Rate for outputs 1 to 4 (Hz) -* \param[in] fivetoeight Rate for outputs 5 to 8 (Hz) -*/ -void PIOS_Servo_SetHz(const uint16_t * banks, uint8_t num_banks) -{ -} + * Set the servo update rate (Max 500Hz) + * \param[in] onetofour Rate for outputs 1 to 4 (Hz) + * \param[in] fivetoeight Rate for outputs 5 to 8 (Hz) + */ +void PIOS_Servo_SetHz(const uint16_t *banks, uint8_t num_banks) +{} /** -* Set servo position -* \param[in] Servo Servo number (0-7) -* \param[in] Position Servo position in milliseconds -*/ + * Set servo position + * \param[in] Servo Servo number (0-7) + * \param[in] Position Servo position in milliseconds + */ void PIOS_Servo_Set(uint8_t Servo, uint16_t Position) { #ifndef PIOS_ENABLE_DEBUG_PINS - /* Make sure servo exists */ - if (Servo < PIOS_SERVO_NUM_OUTPUTS) { - /* Update the position */ - ServoPosition[Servo] = Position; - - } + /* Make sure servo exists */ + if (Servo < PIOS_SERVO_NUM_OUTPUTS) { + /* Update the position */ + ServoPosition[Servo] = Position; + } #endif // PIOS_ENABLE_DEBUG_PINS } -#endif +#endif /* if defined(PIOS_INCLUDE_SERVO) */ diff --git a/flight/pios/posix/pios_sys.c b/flight/pios/posix/pios_sys.c index d4645139b..7e9d5df7b 100644 --- a/flight/pios/posix/pios_sys.c +++ b/flight/pios/posix/pios_sys.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_sys.c + * @file pios_sys.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Sets up basic system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_SYS System Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,159 +38,161 @@ void NVIC_Configuration(void); void SysTick_Handler(void); /* Local Macros */ -#define MEM8(addr) (*((volatile uint8_t *)(addr))) +#define MEM8(addr) (*((volatile uint8_t *)(addr))) /** -* Initialises all system peripherals -*/ + * Initialises all system peripherals + */ void PIOS_SYS_Init(void) { + /** + * stub + */ + printf("PIOS_SYS_Init\n"); - /** - * stub - */ - printf("PIOS_SYS_Init\n"); - - /* Initialise Basic NVIC */ - NVIC_Configuration(); + /* Initialise Basic NVIC */ + NVIC_Configuration(); #if defined(PIOS_INCLUDE_LED) - /* Initialise LEDs */ - PIOS_LED_Init(); + /* Initialise LEDs */ + PIOS_LED_Init(); #endif } /** -* Shutdown PIOS and reset the microcontroller:
-*
    -*
  • Disable all RTOS tasks -*
  • Disable all interrupts -*
  • Turn off all board LEDs -*
  • Reset STM32 -*
-* \return < 0 if reset failed -*/ + * Shutdown PIOS and reset the microcontroller:
+ *
    + *
  • Disable all RTOS tasks + *
  • Disable all interrupts + *
  • Turn off all board LEDs + *
  • Reset STM32 + *
+ * \return < 0 if reset failed + */ int32_t PIOS_SYS_Reset(void) { - /** - * stub - */ - printf("PIOS_SYS_Reset\n"); - /* Disable all RTOS tasks */ + /** + * stub + */ + printf("PIOS_SYS_Reset\n"); + /* Disable all RTOS tasks */ #if defined(PIOS_INCLUDE_FREERTOS) - /* port specific FreeRTOS function to disable tasks (nested) */ - portENTER_CRITICAL(); + /* port specific FreeRTOS function to disable tasks (nested) */ + portENTER_CRITICAL(); #endif - // disable all interrupts - //PIOS_IRQ_Disable(); + // disable all interrupts + // PIOS_IRQ_Disable(); - // turn off all board LEDs - #if (PIOS_LED_NUM == 1) - PIOS_LED_Off(LED1); - #elif (PIOS_LED_NUM == 2) - PIOS_LED_Off(LED1); - PIOS_LED_Off(LED2); - #endif + // turn off all board LEDs + #if (PIOS_LED_NUM == 1) + PIOS_LED_Off(LED1); + #elif (PIOS_LED_NUM == 2) + PIOS_LED_Off(LED1); + PIOS_LED_Off(LED2); + #endif + /* Reset STM32 */ + // RCC_APB2PeriphResetCmd(0xfffffff8, ENABLE); /* MBHP_CORE_STM32: don't reset GPIOA/AF due to USB pins */ + // RCC_APB1PeriphResetCmd(0xff7fffff, ENABLE); /* don't reset USB, so that the connection can survive! */ - /* Reset STM32 */ - //RCC_APB2PeriphResetCmd(0xfffffff8, ENABLE); /* MBHP_CORE_STM32: don't reset GPIOA/AF due to USB pins */ - //RCC_APB1PeriphResetCmd(0xff7fffff, ENABLE); /* don't reset USB, so that the connection can survive! */ + // RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + // RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + // SCB->AIRCR = NVIC_AIRCR_VECTKEY | (1 << NVIC_VECTRESET); + exit(1); - //RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - //RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - //SCB->AIRCR = NVIC_AIRCR_VECTKEY | (1 << NVIC_VECTRESET); - exit(1); + while (1) { + ; + } - while(1); - - /* We will never reach this point */ - return -1; + /* We will never reach this point */ + return -1; } /** -* Returns the serial number as a string -* param[out] uint8_t pointer to a string which can store at least 12 bytes -* (12 bytes returned for STM32) -* return < 0 if feature not supported -*/ + * Returns the serial number as a string + * param[out] uint8_t pointer to a string which can store at least 12 bytes + * (12 bytes returned for STM32) + * return < 0 if feature not supported + */ int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t *array) { - /* Stored in the so called "electronic signature" */ - for (int i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) { - array[i] = 0xff; - } + /* Stored in the so called "electronic signature" */ + for (int i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) { + array[i] = 0xff; + } - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Returns the serial number as a string -* param[out] str pointer to a string which can store at least 32 digits + zero terminator! -* (24 digits returned for STM32) -* return < 0 if feature not supported -*/ + * Returns the serial number as a string + * param[out] str pointer to a string which can store at least 32 digits + zero terminator! + * (24 digits returned for STM32) + * return < 0 if feature not supported + */ int32_t PIOS_SYS_SerialNumberGet(char *str) { - /* Stored in the so called "electronic signature" */ - int i; - for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { - str[i] = 'F'; - } - str[i] = '\0'; + /* Stored in the so called "electronic signature" */ + int i; - /* No error */ - return 0; + for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { + str[i] = 'F'; + } + str[i] = '\0'; + + /* No error */ + return 0; } /** -* Configures Vector Table base location and SysTick -*/ + * Configures Vector Table base location and SysTick + */ void NVIC_Configuration(void) { - /** - * stub - */ - printf("NVIC_Configuration\n"); - /* Set the Vector Table base address as specified in .ld file */ - //NVIC_SetVectorTable(PIOS_NVIC_VECTTAB_FLASH, 0x0); + /** + * stub + */ + printf("NVIC_Configuration\n"); + /* Set the Vector Table base address as specified in .ld file */ + // NVIC_SetVectorTable(PIOS_NVIC_VECTTAB_FLASH, 0x0); - /* 4 bits for Interrupt priorities so no sub priorities */ - //NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + /* 4 bits for Interrupt priorities so no sub priorities */ + // NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); - /* Configure HCLK clock as SysTick clock source. */ - //SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK); + /* Configure HCLK clock as SysTick clock source. */ + // SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK); } #ifdef USE_FULL_ASSERT /** -* Reports the name of the source file and the source line number -* where the assert_param error has occurred. -* \param[in] file pointer to the source file name -* \param[in] line assert_param error line source number -* \retval None -*/ -void assert_failed(uint8_t* file, uint32_t line) + * Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * \param[in] file pointer to the source file name + * \param[in] line assert_param error line source number + * \retval None + */ +void assert_failed(uint8_t *file, uint32_t line) { - /* When serial debugging is implemented, use something like this. */ - /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ - printf("Wrong parameters value: file %s on line %d\r\n", file, line); + /* When serial debugging is implemented, use something like this. */ + /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ + printf("Wrong parameters value: file %s on line %d\r\n", file, line); - /* Setup the LEDs to Alternate */ - PIOS_LED_On(LED1); - PIOS_LED_Off(LED2); + /* Setup the LEDs to Alternate */ + PIOS_LED_On(LED1); + PIOS_LED_Off(LED2); - /* Infinite loop */ - while (1) - { - PIOS_LED_Toggle(LED1); - PIOS_LED_Toggle(LED2); - for(int i = 0; i < 1000000; i++); - } + /* Infinite loop */ + while (1) { + PIOS_LED_Toggle(LED1); + PIOS_LED_Toggle(LED2); + for (int i = 0; i < 1000000; i++) { + ; + } + } } -#endif +#endif /* ifdef USE_FULL_ASSERT */ -#endif +#endif /* if defined(PIOS_INCLUDE_SYS) */ diff --git a/flight/pios/posix/pios_udp.c b/flight/pios/posix/pios_udp.c index c3e495f23..8f25561ac 100644 --- a/flight/pios/posix/pios_udp.c +++ b/flight/pios/posix/pios_udp.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_udp.c + * @file pios_udp.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief UDP commands. Inits UDPs, controls UDPs & Interupt handlers. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_UDP UDP Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,7 +43,6 @@ static int8_t pios_udp_num_devices = 0; static pios_udp_dev pios_udp_devices[PIOS_UDP_MAX_DEV]; - /* Provide a COM driver */ static void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud); static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context); @@ -52,194 +51,182 @@ static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail); static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_udp_com_driver = { - .set_baud = PIOS_UDP_ChangeBaud, - .tx_start = PIOS_UDP_TxStart, - .rx_start = PIOS_UDP_RxStart, - .bind_tx_cb = PIOS_UDP_RegisterTxCallback, - .bind_rx_cb = PIOS_UDP_RegisterRxCallback, + .set_baud = PIOS_UDP_ChangeBaud, + .tx_start = PIOS_UDP_TxStart, + .rx_start = PIOS_UDP_RxStart, + .bind_tx_cb = PIOS_UDP_RegisterTxCallback, + .bind_rx_cb = PIOS_UDP_RegisterRxCallback, }; -static pios_udp_dev * find_udp_dev_by_id (uint8_t udp) +static pios_udp_dev *find_udp_dev_by_id(uint8_t udp) { - if (udp >= pios_udp_num_devices) { - /* Undefined UDP port for this board (see pios_board.c) */ - PIOS_Assert(0); - return NULL; - } + if (udp >= pios_udp_num_devices) { + /* Undefined UDP port for this board (see pios_board.c) */ + PIOS_Assert(0); + return NULL; + } - /* Get a handle for the device configuration */ - return &(pios_udp_devices[udp]); + /* Get a handle for the device configuration */ + return &(pios_udp_devices[udp]); } /** * RxThread */ -void * PIOS_UDP_RxThread(void * udp_dev_n) +void *PIOS_UDP_RxThread(void *udp_dev_n) { + pios_udp_dev *udp_dev = (pios_udp_dev *)udp_dev_n; - pios_udp_dev * udp_dev = (pios_udp_dev*) udp_dev_n; - - /** - * com devices never get closed except by application "reboot" - * we also never give up our mutex except for waiting - */ - while(1) { - - /** - * receive - */ - int received; - udp_dev->clientLength=sizeof(udp_dev->client); - if ((received = recvfrom(udp_dev->socket, - &udp_dev->rx_buffer, - PIOS_UDP_RX_BUFFER_SIZE, - 0, - (struct sockaddr *) &udp_dev->client, - (socklen_t*)&udp_dev->clientLength)) >= 0) - { - - /* copy received data to buffer if possible */ - /* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */ - /* (thats what the USART driver does too!) */ - bool rx_need_yield = false; - if (udp_dev->rx_in_cb) { - (void) (udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield); - } + /** + * com devices never get closed except by application "reboot" + * we also never give up our mutex except for waiting + */ + while (1) { + /** + * receive + */ + int received; + udp_dev->clientLength = sizeof(udp_dev->client); + if ((received = recvfrom(udp_dev->socket, + &udp_dev->rx_buffer, + PIOS_UDP_RX_BUFFER_SIZE, + 0, + (struct sockaddr *)&udp_dev->client, + (socklen_t *)&udp_dev->clientLength)) >= 0) { + /* copy received data to buffer if possible */ + /* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */ + /* (thats what the USART driver does too!) */ + bool rx_need_yield = false; + if (udp_dev->rx_in_cb) { + (void)(udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (rx_need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ - - } - - - } + if (rx_need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ + } + } } /** -* Open UDP socket -*/ -int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg) + * Open UDP socket + */ +int32_t PIOS_UDP_Init(uint32_t *udp_id, const struct pios_udp_cfg *cfg) { + pios_udp_dev *udp_dev = &pios_udp_devices[pios_udp_num_devices]; - pios_udp_dev * udp_dev = &pios_udp_devices[pios_udp_num_devices]; - - pios_udp_num_devices++; + pios_udp_num_devices++; - /* initialize */ - udp_dev->rx_in_cb = NULL; - udp_dev->tx_out_cb = NULL; - udp_dev->cfg=cfg; + /* initialize */ + udp_dev->rx_in_cb = NULL; + udp_dev->tx_out_cb = NULL; + udp_dev->cfg = cfg; - /* assign socket */ - udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - memset(&udp_dev->server,0,sizeof(udp_dev->server)); - memset(&udp_dev->client,0,sizeof(udp_dev->client)); - udp_dev->server.sin_family = AF_INET; - udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip); - udp_dev->server.sin_port = htons(udp_dev->cfg->port); - int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server)); + /* assign socket */ + udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + memset(&udp_dev->server, 0, sizeof(udp_dev->server)); + memset(&udp_dev->client, 0, sizeof(udp_dev->client)); + udp_dev->server.sin_family = AF_INET; + udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip); + udp_dev->server.sin_port = htons(udp_dev->cfg->port); + int res = bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server, sizeof(udp_dev->server)); - /* Create transmit thread for this connection */ + /* Create transmit thread for this connection */ #if defined(PIOS_INCLUDE_FREERTOS) -//( pdTASK_CODE pvTaskCode, const portCHAR * const pcName, unsigned portSHORT usStackDepth, void *pvParameters, unsigned portBASE_TYPE uxPriority, xTaskHandle *pvCreatedTask ); - xTaskCreate((pdTASK_CODE)PIOS_UDP_RxThread, (const signed char *)"UDP_Rx_Thread",1024,(void*)udp_dev,(tskIDLE_PRIORITY + 1),&udp_dev->rxThread); +// ( pdTASK_CODE pvTaskCode, const portCHAR * const pcName, unsigned portSHORT usStackDepth, void *pvParameters, unsigned portBASE_TYPE uxPriority, xTaskHandle *pvCreatedTask ); + xTaskCreate((pdTASK_CODE)PIOS_UDP_RxThread, (const signed char *)"UDP_Rx_Thread", 1024, (void *)udp_dev, (tskIDLE_PRIORITY + 1), &udp_dev->rxThread); #else - pthread_create(&udp_dev->rxThread, NULL, PIOS_UDP_RxThread, (void*)udp_dev); + pthread_create(&udp_dev->rxThread, NULL, PIOS_UDP_RxThread, (void *)udp_dev); #endif - printf("udp dev %i - socket %i opened - result %i\n",pios_udp_num_devices-1,udp_dev->socket,res); + printf("udp dev %i - socket %i opened - result %i\n", pios_udp_num_devices - 1, udp_dev->socket, res); - *udp_id = pios_udp_num_devices-1; + *udp_id = pios_udp_num_devices - 1; - return res; + return res; } void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud) { - /** - * doesn't apply! - */ + /** + * doesn't apply! + */ } static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail) { - /** - * lazy! - */ + /** + * lazy! + */ } static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail) { - pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id); - PIOS_Assert(udp_dev); + PIOS_Assert(udp_dev); - int32_t length,len,rem; - - /** - * we send everything directly whenever notified of data to send (lazy!) - */ - if (udp_dev->tx_out_cb) { - while (tx_bytes_avail>0) { - bool tx_need_yield = false; - length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield); - rem = length; - while (rem>0) { - len = sendto(udp_dev->socket, udp_dev->tx_buffer+length-rem, rem, 0, - (struct sockaddr *) &udp_dev->client, - sizeof(udp_dev->client)); - if (len<=0) { - rem=0; - } else { - rem -= len; - } - } - tx_bytes_avail -= length; - } - } + int32_t length, len, rem; + /** + * we send everything directly whenever notified of data to send (lazy!) + */ + if (udp_dev->tx_out_cb) { + while (tx_bytes_avail > 0) { + bool tx_need_yield = false; + length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield); + rem = length; + while (rem > 0) { + len = sendto(udp_dev->socket, udp_dev->tx_buffer + length - rem, rem, 0, + (struct sockaddr *)&udp_dev->client, + sizeof(udp_dev->client)); + if (len <= 0) { + rem = 0; + } else { + rem -= len; + } + } + tx_bytes_avail -= length; + } + } } static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context) { - pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id); - PIOS_Assert(udp_dev); + PIOS_Assert(udp_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - udp_dev->rx_in_context = context; - udp_dev->rx_in_cb = rx_in_cb; + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + udp_dev->rx_in_context = context; + udp_dev->rx_in_cb = rx_in_cb; } static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context) { - pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id); - PIOS_Assert(udp_dev); + PIOS_Assert(udp_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - udp_dev->tx_out_context = context; - udp_dev->tx_out_cb = tx_out_cb; + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + udp_dev->tx_out_context = context; + udp_dev->tx_out_cb = tx_out_cb; } - - - -#endif +#endif /* if defined(PIOS_INCLUDE_UDP) */ diff --git a/flight/pios/posix/pios_wdg.c b/flight/pios/posix/pios_wdg.c index eeb4d759a..11261c035 100644 --- a/flight/pios/posix/pios_wdg.c +++ b/flight/pios/posix/pios_wdg.c @@ -8,12 +8,12 @@ * * @file pios_spi.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Hardware Abstraction Layer for SPI ports of STM32 * @see The GNU Public License (GPL) Version 3 * @notes * - * The PIOS Watchdog provides a HAL to initialize a watchdog + * The PIOS Watchdog provides a HAL to initialize a watchdog * *****************************************************************************/ /* @@ -34,17 +34,17 @@ #include "pios.h" -/** +/** * @brief Initialize the watchdog timer for a specified timeout * - * It is important to note that this function returns the achieved timeout - * for this hardware. For hardware indendence this should be checked when - * scheduling updates. Other hardware dependent details may need to be - * considered such as a window time which sets a minimum update time, - * and this function should return a recommended delay for clearing. + * It is important to note that this function returns the achieved timeout + * for this hardware. For hardware indendence this should be checked when + * scheduling updates. Other hardware dependent details may need to be + * considered such as a window time which sets a minimum update time, + * and this function should return a recommended delay for clearing. * * For the STM32 nominal clock rate is 32 khz, but for the maximum clock rate of - * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is + * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is * set in the watchdog assumes the nominal clock rate, but the delay for FreeRTOS * to use is 75% of the minimal delay. * @@ -53,65 +53,65 @@ */ uint16_t PIOS_WDG_Init() { - return 0; + return 0; } /** - * @brief Register a module against the watchdog - * + * @brief Register a module against the watchdog + * * There are two ways to use PIOS WDG: this is for when - * multiple modules must be monitored. In this case they - * must first register against the watchdog system and + * multiple modules must be monitored. In this case they + * must first register against the watchdog system and * only when all of the modules have been updated with the - * watchdog be cleared. Each module must have its own - * bit in the 16 bit + * watchdog be cleared. Each module must have its own + * bit in the 16 bit * * @param[in] flag the bit this module wants to use * @returns True if that bit is unregistered */ -bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) +bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) { - return true; + return true; } /** * @brief Function called by modules to indicate they are still running * - * This function will set this flag in the active flags register (which is + * This function will set this flag in the active flags register (which is * a backup regsiter) and if all the registered flags are set will clear * the watchdog and set only this flag in the backup register * * @param[in] flag the flag to set * @return true if the watchdog cleared, false if flags are pending */ -bool PIOS_WDG_UpdateFlag(uint16_t flag) -{ - return true; +bool PIOS_WDG_UpdateFlag(uint16_t flag) +{ + return true; } -/** +/** * @brief Returns the flags that were set at bootup - * - * This is used for diagnostics, if only one flag not set this + * + * This is used for diagnostics, if only one flag not set this * was likely the module that wasn't running before reset - * + * * @return The active flags register from bootup */ uint16_t PIOS_WDG_GetBootupFlags() { - return (uint16_t) 0xffff; + return (uint16_t)0xffff; } -/** +/** * @brief Returns the currently active flags - * + * * For external monitoring - * + * * @return The active flags register */ uint16_t PIOS_WDG_GetActiveFlags() { - return (uint16_t) 0xffff; + return (uint16_t)0xffff; } /** @@ -120,5 +120,4 @@ uint16_t PIOS_WDG_GetActiveFlags() * This function must be called at the appropriate delay to prevent a reset event occuring */ void PIOS_WDG_Clear(void) -{ -} +{} diff --git a/flight/pios/stm32f10x/pios_adc.c b/flight/pios/stm32f10x/pios_adc.c index 2f5ed4198..c9ef301e3 100644 --- a/flight/pios/stm32f10x/pios_adc.c +++ b/flight/pios/stm32f10x/pios_adc.c @@ -6,24 +6,24 @@ * @brief STM32 ADC PIOS interface * @{ * - * @file pios_adc.c + * @file pios_adc.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Analog to Digital converstion routines * @see The GNU Public License (GPL) Version 3 *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,62 +36,65 @@ // Private types enum pios_adc_dev_magic { - PIOS_ADC_DEV_MAGIC = 0x58375124, + PIOS_ADC_DEV_MAGIC = 0x58375124, }; struct pios_adc_dev { - const struct pios_adc_cfg * cfg; - ADCCallback callback_function; + const struct pios_adc_cfg *cfg; + ADCCallback callback_function; #if defined(PIOS_INCLUDE_FREERTOS) - xQueueHandle data_queue; + xQueueHandle data_queue; #endif - volatile int16_t *valid_data_buffer; - volatile uint8_t adc_oversample; - uint8_t dma_block_size; - uint16_t dma_half_buffer_size; + volatile int16_t *valid_data_buffer; + volatile uint8_t adc_oversample; + uint8_t dma_block_size; + uint16_t dma_half_buffer_size; #if defined(PIOS_INCLUDE_ADC) - int16_t fir_coeffs[PIOS_ADC_MAX_SAMPLES+1] __attribute__ ((aligned(4))); - volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES] __attribute__ ((aligned(4))); // Double buffer that DMA just used - float downsampled_buffer[PIOS_ADC_NUM_CHANNELS] __attribute__ ((aligned(4))); + int16_t fir_coeffs[PIOS_ADC_MAX_SAMPLES + 1] __attribute__((aligned(4))); + volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES] __attribute__((aligned(4))); // Double buffer that DMA just used + float downsampled_buffer[PIOS_ADC_NUM_CHANNELS] __attribute__((aligned(4))); #endif - enum pios_adc_dev_magic magic; + enum pios_adc_dev_magic magic; }; #if defined(PIOS_INCLUDE_FREERTOS) -struct pios_adc_dev * pios_adc_dev; +struct pios_adc_dev *pios_adc_dev; #endif // Private functions void PIOS_ADC_downsample_data(); -static struct pios_adc_dev * PIOS_ADC_Allocate(); +static struct pios_adc_dev *PIOS_ADC_Allocate(); static bool PIOS_ADC_validate(struct pios_adc_dev *); /* Local Variables */ static GPIO_TypeDef *ADC_GPIO_PORT[PIOS_ADC_NUM_PINS] = PIOS_ADC_PORTS; static const uint32_t ADC_GPIO_PIN[PIOS_ADC_NUM_PINS] = PIOS_ADC_PINS; -static const uint32_t ADC_CHANNEL[PIOS_ADC_NUM_PINS] = PIOS_ADC_CHANNELS; +static const uint32_t ADC_CHANNEL[PIOS_ADC_NUM_PINS] = PIOS_ADC_CHANNELS; -static ADC_TypeDef *ADC_MAPPING[PIOS_ADC_NUM_PINS] = PIOS_ADC_MAPPING; +static ADC_TypeDef *ADC_MAPPING[PIOS_ADC_NUM_PINS] = PIOS_ADC_MAPPING; static const uint32_t ADC_CHANNEL_MAPPING[PIOS_ADC_NUM_PINS] = PIOS_ADC_CHANNEL_MAPPING; -static bool PIOS_ADC_validate(struct pios_adc_dev * dev) +static bool PIOS_ADC_validate(struct pios_adc_dev *dev) { - if (dev == NULL) - return false; + if (dev == NULL) { + return false; + } - return (dev->magic == PIOS_ADC_DEV_MAGIC); + return dev->magic == PIOS_ADC_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_adc_dev * PIOS_ADC_Allocate() +static struct pios_adc_dev *PIOS_ADC_Allocate() { - struct pios_adc_dev * adc_dev; - - adc_dev = (struct pios_adc_dev *)pvPortMalloc(sizeof(*adc_dev)); - if (!adc_dev) return (NULL); - - adc_dev->magic = PIOS_ADC_DEV_MAGIC; - return(adc_dev); + struct pios_adc_dev *adc_dev; + + adc_dev = (struct pios_adc_dev *)pvPortMalloc(sizeof(*adc_dev)); + if (!adc_dev) { + return NULL; + } + + adc_dev->magic = PIOS_ADC_DEV_MAGIC; + return adc_dev; } #else #error Not implemented @@ -100,34 +103,35 @@ static struct pios_adc_dev * PIOS_ADC_Allocate() /** * @brief Initialise the ADC Peripheral, configure to run at the max oversampling */ -int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg) +int32_t PIOS_ADC_Init(const struct pios_adc_cfg *cfg) { - pios_adc_dev = PIOS_ADC_Allocate(); - if (pios_adc_dev == NULL) - return -1; + pios_adc_dev = PIOS_ADC_Allocate(); + if (pios_adc_dev == NULL) { + return -1; + } + + pios_adc_dev->cfg = cfg; + pios_adc_dev->callback_function = NULL; - pios_adc_dev->cfg = cfg; - pios_adc_dev->callback_function = NULL; - #if defined(PIOS_INCLUDE_FREERTOS) - pios_adc_dev->data_queue = NULL; + pios_adc_dev->data_queue = NULL; #endif - - /* Setup analog pins */ - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; - - /* Enable each ADC pin in the array */ - for (int32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) { - GPIO_InitStructure.GPIO_Pin = ADC_GPIO_PIN[i]; - GPIO_Init(ADC_GPIO_PORT[i], &GPIO_InitStructure); - } - PIOS_ADC_Config(PIOS_ADC_MAX_OVERSAMPLING); - - return 0; + /* Setup analog pins */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; + + /* Enable each ADC pin in the array */ + for (int32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) { + GPIO_InitStructure.GPIO_Pin = ADC_GPIO_PIN[i]; + GPIO_Init(ADC_GPIO_PORT[i], &GPIO_InitStructure); + } + + PIOS_ADC_Config(PIOS_ADC_MAX_OVERSAMPLING); + + return 0; } /** @@ -135,100 +139,109 @@ int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg) * @param[in] oversampling the amount of oversampling to run at */ void PIOS_ADC_Config(uint32_t oversampling) -{ - pios_adc_dev->adc_oversample = (oversampling > PIOS_ADC_MAX_OVERSAMPLING) ? PIOS_ADC_MAX_OVERSAMPLING : oversampling; +{ + pios_adc_dev->adc_oversample = (oversampling > PIOS_ADC_MAX_OVERSAMPLING) ? PIOS_ADC_MAX_OVERSAMPLING : oversampling; + + ADC_DeInit(ADC1); + ADC_DeInit(ADC2); + + /* Disable interrupts */ + DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->dma.irq.flags, DISABLE); + + /* Enable ADC clocks */ + PIOS_ADC_CLOCK_FUNCTION; + + /* Map channels to conversion slots depending on the channel selection mask */ + for (int32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) { + ADC_RegularChannelConfig(ADC_MAPPING[i], ADC_CHANNEL[i], + ADC_CHANNEL_MAPPING[i], + PIOS_ADC_SAMPLE_TIME); + } - ADC_DeInit(ADC1); - ADC_DeInit(ADC2); - - /* Disable interrupts */ - DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->dma.irq.flags, DISABLE); - - /* Enable ADC clocks */ - PIOS_ADC_CLOCK_FUNCTION; - - /* Map channels to conversion slots depending on the channel selection mask */ - for (int32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) { - ADC_RegularChannelConfig(ADC_MAPPING[i], ADC_CHANNEL[i], - ADC_CHANNEL_MAPPING[i], - PIOS_ADC_SAMPLE_TIME); - } - #if (PIOS_ADC_USE_TEMP_SENSOR) - ADC_TempSensorVrefintCmd(ENABLE); - ADC_RegularChannelConfig(PIOS_ADC_TEMP_SENSOR_ADC, ADC_Channel_16, - PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL, - PIOS_ADC_SAMPLE_TIME); + ADC_TempSensorVrefintCmd(ENABLE); + ADC_RegularChannelConfig(PIOS_ADC_TEMP_SENSOR_ADC, ADC_Channel_16, + PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL, + PIOS_ADC_SAMPLE_TIME); #endif - // return - /* Configure ADCs */ - ADC_InitTypeDef ADC_InitStructure; - ADC_StructInit(&ADC_InitStructure); - ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult; - ADC_InitStructure.ADC_ScanConvMode = ENABLE; - ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; - ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; - ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; - ADC_InitStructure.ADC_NbrOfChannel = ((PIOS_ADC_NUM_CHANNELS + 1) >> 1); - ADC_Init(ADC1, &ADC_InitStructure); - -#if (PIOS_ADC_USE_ADC2) - ADC_Init(ADC2, &ADC_InitStructure); - - /* Enable ADC2 external trigger conversion (to synch with ADC1) */ - ADC_ExternalTrigConvCmd(ADC2, ENABLE); -#endif - - RCC_ADCCLKConfig(PIOS_ADC_ADCCLK); - - /* Enable ADC1->DMA request */ - ADC_DMACmd(ADC1, ENABLE); - - /* ADC1 calibration */ - ADC_Cmd(ADC1, ENABLE); - ADC_ResetCalibration(ADC1); - while (ADC_GetResetCalibrationStatus(ADC1)) ; - ADC_StartCalibration(ADC1); - while (ADC_GetCalibrationStatus(ADC1)) ; - -#if (PIOS_ADC_USE_ADC2) - /* ADC2 calibration */ - ADC_Cmd(ADC2, ENABLE); - ADC_ResetCalibration(ADC2); - while (ADC_GetResetCalibrationStatus(ADC2)) ; - ADC_StartCalibration(ADC2); - while (ADC_GetCalibrationStatus(ADC2)) ; -#endif - - /* This makes sure we have an even number of transfers if using ADC2 */ - pios_adc_dev->dma_block_size = ((PIOS_ADC_NUM_CHANNELS + PIOS_ADC_USE_ADC2) >> PIOS_ADC_USE_ADC2) << PIOS_ADC_USE_ADC2; - pios_adc_dev->dma_half_buffer_size = pios_adc_dev->dma_block_size * pios_adc_dev->adc_oversample; + // return + /* Configure ADCs */ + ADC_InitTypeDef ADC_InitStructure; + ADC_StructInit(&ADC_InitStructure); + ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult; + ADC_InitStructure.ADC_ScanConvMode = ENABLE; + ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; + ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; + ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; + ADC_InitStructure.ADC_NbrOfChannel = ((PIOS_ADC_NUM_CHANNELS + 1) >> 1); + ADC_Init(ADC1, &ADC_InitStructure); - /* Configure DMA channel */ - DMA_InitTypeDef dma_init = pios_adc_dev->cfg->dma.rx.init; - dma_init.DMA_MemoryBaseAddr = (uint32_t) &pios_adc_dev->raw_data_buffer[0]; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - dma_init.DMA_BufferSize = pios_adc_dev->dma_half_buffer_size; /* x2 for double buffer /2 for 32-bit xfr */ - DMA_Init(pios_adc_dev->cfg->dma.rx.channel, &dma_init); - DMA_Cmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE); - - /* Trigger interrupt when for half conversions too to indicate double buffer */ - DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_TC, ENABLE); - DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_HT, ENABLE); - - /* Configure DMA interrupt */ - NVIC_Init(&pios_adc_dev->cfg->dma.irq.init); - - /* Finally start initial conversion */ - ADC_SoftwareStartConvCmd(ADC1, ENABLE); - - /* Use simple averaging filter for now */ - for (int32_t i = 0; i < pios_adc_dev->adc_oversample; i++) - pios_adc_dev->fir_coeffs[i] = 1; - pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample] = pios_adc_dev->adc_oversample; - - /* Enable DMA1 clock */ - RCC_AHBPeriphClockCmd(pios_adc_dev->cfg->dma.ahb_clk, ENABLE); +#if (PIOS_ADC_USE_ADC2) + ADC_Init(ADC2, &ADC_InitStructure); + + /* Enable ADC2 external trigger conversion (to synch with ADC1) */ + ADC_ExternalTrigConvCmd(ADC2, ENABLE); +#endif + + RCC_ADCCLKConfig(PIOS_ADC_ADCCLK); + + /* Enable ADC1->DMA request */ + ADC_DMACmd(ADC1, ENABLE); + + /* ADC1 calibration */ + ADC_Cmd(ADC1, ENABLE); + ADC_ResetCalibration(ADC1); + while (ADC_GetResetCalibrationStatus(ADC1)) { + ; + } + ADC_StartCalibration(ADC1); + while (ADC_GetCalibrationStatus(ADC1)) { + ; + } + +#if (PIOS_ADC_USE_ADC2) + /* ADC2 calibration */ + ADC_Cmd(ADC2, ENABLE); + ADC_ResetCalibration(ADC2); + while (ADC_GetResetCalibrationStatus(ADC2)) { + ; + } + ADC_StartCalibration(ADC2); + while (ADC_GetCalibrationStatus(ADC2)) { + ; + } +#endif + + /* This makes sure we have an even number of transfers if using ADC2 */ + pios_adc_dev->dma_block_size = ((PIOS_ADC_NUM_CHANNELS + PIOS_ADC_USE_ADC2) >> PIOS_ADC_USE_ADC2) << PIOS_ADC_USE_ADC2; + pios_adc_dev->dma_half_buffer_size = pios_adc_dev->dma_block_size * pios_adc_dev->adc_oversample; + + /* Configure DMA channel */ + DMA_InitTypeDef dma_init = pios_adc_dev->cfg->dma.rx.init; + dma_init.DMA_MemoryBaseAddr = (uint32_t)&pios_adc_dev->raw_data_buffer[0]; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + dma_init.DMA_BufferSize = pios_adc_dev->dma_half_buffer_size; /* x2 for double buffer /2 for 32-bit xfr */ + DMA_Init(pios_adc_dev->cfg->dma.rx.channel, &dma_init); + DMA_Cmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE); + + /* Trigger interrupt when for half conversions too to indicate double buffer */ + DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_TC, ENABLE); + DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_HT, ENABLE); + + /* Configure DMA interrupt */ + NVIC_Init(&pios_adc_dev->cfg->dma.irq.init); + + /* Finally start initial conversion */ + ADC_SoftwareStartConvCmd(ADC1, ENABLE); + + /* Use simple averaging filter for now */ + for (int32_t i = 0; i < pios_adc_dev->adc_oversample; i++) { + pios_adc_dev->fir_coeffs[i] = 1; + } + pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample] = pios_adc_dev->adc_oversample; + + /* Enable DMA1 clock */ + RCC_AHBPeriphClockCmd(pios_adc_dev->cfg->dma.ahb_clk, ENABLE); } /** @@ -239,48 +252,48 @@ void PIOS_ADC_Config(uint32_t oversampling) */ int32_t PIOS_ADC_PinGet(uint32_t pin) { - /* Check if pin exists */ - if (pin >= PIOS_ADC_NUM_CHANNELS) { - return -1; - } - - /* Return last conversion result */ - return pios_adc_dev->downsampled_buffer[pin]; + /* Check if pin exists */ + if (pin >= PIOS_ADC_NUM_CHANNELS) { + return -1; + } + + /* Return last conversion result */ + return pios_adc_dev->downsampled_buffer[pin]; } /** * @brief Set a callback function that is executed whenever - * the ADC double buffer swaps + * the ADC double buffer swaps */ -void PIOS_ADC_SetCallback(ADCCallback new_function) +void PIOS_ADC_SetCallback(ADCCallback new_function) { - pios_adc_dev->callback_function = new_function; + pios_adc_dev->callback_function = new_function; } #if defined(PIOS_INCLUDE_FREERTOS) /** - * @brief Register a queue to add data to when downsampled + * @brief Register a queue to add data to when downsampled */ -void PIOS_ADC_SetQueue(xQueueHandle data_queue) +void PIOS_ADC_SetQueue(xQueueHandle data_queue) { - pios_adc_dev->data_queue = data_queue; + pios_adc_dev->data_queue = data_queue; } #endif /** - * @brief Return the address of the downsampled data buffer + * @brief Return the address of the downsampled data buffer */ -float * PIOS_ADC_GetBuffer(void) +float *PIOS_ADC_GetBuffer(void) { - return pios_adc_dev->downsampled_buffer; + return pios_adc_dev->downsampled_buffer; } /** - * @brief Return the address of the raw data data buffer + * @brief Return the address of the raw data data buffer */ -int16_t * PIOS_ADC_GetRawBuffer(void) +int16_t *PIOS_ADC_GetRawBuffer(void) { - return (int16_t *) pios_adc_dev->valid_data_buffer; + return (int16_t *)pios_adc_dev->valid_data_buffer; } /** @@ -288,85 +301,86 @@ int16_t * PIOS_ADC_GetRawBuffer(void) */ uint8_t PIOS_ADC_GetOverSampling(void) { - return pios_adc_dev->adc_oversample; + return pios_adc_dev->adc_oversample; } /** - * @brief Set the fir coefficients. Takes as many samples as the + * @brief Set the fir coefficients. Takes as many samples as the * current filter order plus one (normalization) * * @param new_filter Array of adc_oversampling floats plus one for the * filter coefficients */ -void PIOS_ADC_SetFIRCoefficients(float * new_filter) +void PIOS_ADC_SetFIRCoefficients(float *new_filter) { - // Less than or equal to get normalization constant - for(int i = 0; i <= pios_adc_dev->adc_oversample; i++) - pios_adc_dev->fir_coeffs[i] = new_filter[i]; + // Less than or equal to get normalization constant + for (int i = 0; i <= pios_adc_dev->adc_oversample; i++) { + pios_adc_dev->fir_coeffs[i] = new_filter[i]; + } } /** * @brief Downsample the data for each of the channels then call * callback function if installed - */ + */ void PIOS_ADC_downsample_data() { - uint16_t chan; - uint16_t sample; - float * downsampled_buffer = &pios_adc_dev->downsampled_buffer[0]; - - for (chan = 0; chan < PIOS_ADC_NUM_CHANNELS; chan++) { - int32_t sum = 0; - for (sample = 0; sample < pios_adc_dev->adc_oversample; sample++) { - sum += pios_adc_dev->valid_data_buffer[chan + sample * pios_adc_dev->dma_block_size] * pios_adc_dev->fir_coeffs[sample]; - } - downsampled_buffer[chan] = (float) sum / pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample]; - } - + uint16_t chan; + uint16_t sample; + float *downsampled_buffer = &pios_adc_dev->downsampled_buffer[0]; + + for (chan = 0; chan < PIOS_ADC_NUM_CHANNELS; chan++) { + int32_t sum = 0; + for (sample = 0; sample < pios_adc_dev->adc_oversample; sample++) { + sum += pios_adc_dev->valid_data_buffer[chan + sample * pios_adc_dev->dma_block_size] * pios_adc_dev->fir_coeffs[sample]; + } + downsampled_buffer[chan] = (float)sum / pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample]; + } + #if defined(PIOS_INCLUDE_FREERTOS) - if(pios_adc_dev->data_queue) { - static portBASE_TYPE xHigherPriorityTaskWoken; - xQueueSendFromISR(pios_adc_dev->data_queue, pios_adc_dev->downsampled_buffer, &xHigherPriorityTaskWoken); - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); - } + if (pios_adc_dev->data_queue) { + static portBASE_TYPE xHigherPriorityTaskWoken; + xQueueSendFromISR(pios_adc_dev->data_queue, pios_adc_dev->downsampled_buffer, &xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + } #endif - if(pios_adc_dev->callback_function) - pios_adc_dev->callback_function(pios_adc_dev->downsampled_buffer); + if (pios_adc_dev->callback_function) { + pios_adc_dev->callback_function(pios_adc_dev->downsampled_buffer); + } } /** * @brief Interrupt for half and full buffer transfer - * + * * This interrupt handler swaps between the two halfs of the double buffer to make * sure the ahrs uses the most recent data. Only swaps data when AHRS is idle, but - * really this is a pretense of a sanity check since the DMA engine is consantly + * really this is a pretense of a sanity check since the DMA engine is consantly * running in the background. Keep an eye on the ekf_too_slow variable to make sure * it's keeping up. */ void PIOS_ADC_DMA_Handler(void) { - if (!PIOS_ADC_validate(pios_adc_dev)) - return; + if (!PIOS_ADC_validate(pios_adc_dev)) { + return; + } - if (DMA_GetFlagStatus(pios_adc_dev->cfg->full_flag /*DMA1_IT_TC1*/)) { // whole double buffer filled - pios_adc_dev->valid_data_buffer = &pios_adc_dev->raw_data_buffer[pios_adc_dev->dma_half_buffer_size]; - DMA_ClearFlag(pios_adc_dev->cfg->full_flag); - PIOS_ADC_downsample_data(); - } - else if (DMA_GetFlagStatus(pios_adc_dev->cfg->half_flag /*DMA1_IT_HT1*/)) { - pios_adc_dev->valid_data_buffer = &pios_adc_dev->raw_data_buffer[0]; - DMA_ClearFlag(pios_adc_dev->cfg->half_flag); - PIOS_ADC_downsample_data(); - } - else { - // This should not happen, probably due to transfer errors - DMA_ClearFlag(pios_adc_dev->cfg->dma.irq.flags /*DMA1_FLAG_GL1*/); - } + if (DMA_GetFlagStatus(pios_adc_dev->cfg->full_flag /*DMA1_IT_TC1*/)) { // whole double buffer filled + pios_adc_dev->valid_data_buffer = &pios_adc_dev->raw_data_buffer[pios_adc_dev->dma_half_buffer_size]; + DMA_ClearFlag(pios_adc_dev->cfg->full_flag); + PIOS_ADC_downsample_data(); + } else if (DMA_GetFlagStatus(pios_adc_dev->cfg->half_flag /*DMA1_IT_HT1*/)) { + pios_adc_dev->valid_data_buffer = &pios_adc_dev->raw_data_buffer[0]; + DMA_ClearFlag(pios_adc_dev->cfg->half_flag); + PIOS_ADC_downsample_data(); + } else { + // This should not happen, probably due to transfer errors + DMA_ClearFlag(pios_adc_dev->cfg->dma.irq.flags /*DMA1_FLAG_GL1*/); + } } #endif /* PIOS_INCLUDE_ADC */ -/** +/** * @} * @} */ diff --git a/flight/pios/stm32f10x/pios_bkp.c b/flight/pios/stm32f10x/pios_bkp.c index b35e9177e..6238ee724 100644 --- a/flight/pios/stm32f10x/pios_bkp.c +++ b/flight/pios/stm32f10x/pios_bkp.c @@ -6,25 +6,25 @@ * @brief Hardware abstraction layer for backup sram * @{ * - * @file pios_bkp.c + * @file pios_bkp.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief IAP functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,115 +35,110 @@ #include /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ /***************************************************************************************** * Public Definitions/Macros ****************************************************************************************/ /**************************************************************************************** - * Public Functions - ****************************************************************************************/ -const uint32_t pios_bkp_registers_map[] = - { - BKP_DR1 , - BKP_DR2 , - BKP_DR3 , - BKP_DR4 , - BKP_DR5 , - BKP_DR6 , - BKP_DR7 , - BKP_DR8 , - BKP_DR9 , - BKP_DR10, - BKP_DR11, - BKP_DR12, - BKP_DR13, - BKP_DR14, - BKP_DR15, - BKP_DR16, - BKP_DR17, - BKP_DR18, - BKP_DR19, +* Public Functions +****************************************************************************************/ +const uint32_t pios_bkp_registers_map[] = { + BKP_DR1, + BKP_DR2, + BKP_DR3, + BKP_DR4, + BKP_DR5, + BKP_DR6, + BKP_DR7, + BKP_DR8, + BKP_DR9, + BKP_DR10, + BKP_DR11, + BKP_DR12, + BKP_DR13, + BKP_DR14, + BKP_DR15, + BKP_DR16, + BKP_DR17, + BKP_DR18, + BKP_DR19, -#if FALSE /* Not enabled as stm32f4 needs some modifications to - * accomodate more than 20 registers (like storing 2 uint16_t - * regs in one uint32_t bkp location) - */ - BKP_DR20, - BKP_DR21, - BKP_DR22, - BKP_DR23, - BKP_DR24, - BKP_DR25, - BKP_DR26, - BKP_DR27, - BKP_DR28, - BKP_DR29, - BKP_DR30, - BKP_DR32, - BKP_DR33, - BKP_DR34, - BKP_DR35, - BKP_DR36, - BKP_DR37, - BKP_DR38, - BKP_DR39, - BKP_DR40, - BKP_DR41, - BKP_DR42, -#endif - - }; +#if FALSE /* Not enabled as stm32f4 needs some modifications to + * accomodate more than 20 registers (like storing 2 uint16_t + * regs in one uint32_t bkp location) + */ + BKP_DR20, + BKP_DR21, + BKP_DR22, + BKP_DR23, + BKP_DR24, + BKP_DR25, + BKP_DR26, + BKP_DR27, + BKP_DR28, + BKP_DR29, + BKP_DR30, + BKP_DR32, + BKP_DR33, + BKP_DR34, + BKP_DR35, + BKP_DR36, + BKP_DR37, + BKP_DR38, + BKP_DR39, + BKP_DR40, + BKP_DR41, + BKP_DR42, +#endif /* if FALSE */ +}; #define PIOS_BKP_REGISTERS_COUNT NELEMENTS(pios_bkp_registers_map) void PIOS_BKP_Init(void) { - /* Enable CRC clock */ - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); + /* Enable CRC clock */ + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); - /* Enable PWR and BKP clock */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR | RCC_APB1Periph_BKP, ENABLE); + /* Enable PWR and BKP clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR | RCC_APB1Periph_BKP, ENABLE); - /* Clear Tamper pin Event(TE) pending flag */ - BKP_ClearFlag(); + /* Clear Tamper pin Event(TE) pending flag */ + BKP_ClearFlag(); } uint16_t PIOS_BKP_ReadRegister(uint32_t regnumber) { - if(PIOS_BKP_REGISTERS_COUNT < regnumber) - { - PIOS_Assert(0); - } else { - return (uint16_t) BKP_ReadBackupRegister(pios_bkp_registers_map[regnumber]); - } + if (PIOS_BKP_REGISTERS_COUNT < regnumber) { + PIOS_Assert(0); + } else { + return (uint16_t)BKP_ReadBackupRegister(pios_bkp_registers_map[regnumber]); + } } -void PIOS_BKP_WriteRegister(uint32_t regnumber,uint16_t data) +void PIOS_BKP_WriteRegister(uint32_t regnumber, uint16_t data) { - if(PIOS_BKP_REGISTERS_COUNT < regnumber) - { - PIOS_Assert(0); - } else { - BKP_WriteBackupRegister(pios_bkp_registers_map[regnumber],(uint32_t)data); - } + if (PIOS_BKP_REGISTERS_COUNT < regnumber) { + PIOS_Assert(0); + } else { + BKP_WriteBackupRegister(pios_bkp_registers_map[regnumber], (uint32_t)data); + } } void PIOS_BKP_EnableWrite(void) { - /* Enable write access to Backup domain */ - PWR_BackupAccessCmd(ENABLE); + /* Enable write access to Backup domain */ + PWR_BackupAccessCmd(ENABLE); } void PIOS_BKP_DisableWrite(void) { - /* Enable write access to Backup domain */ - PWR_BackupAccessCmd(DISABLE); + /* Enable write access to Backup domain */ + PWR_BackupAccessCmd(DISABLE); } - /**************************************************************************************** - * Public Data - ****************************************************************************************/ +* Public Data +****************************************************************************************/ diff --git a/flight/pios/stm32f10x/pios_bl_helper.c b/flight/pios/stm32f10x/pios_bl_helper.c index 0c2d7b7d9..56d4eb323 100644 --- a/flight/pios/stm32f10x/pios_bl_helper.c +++ b/flight/pios/stm32f10x/pios_bl_helper.c @@ -38,7 +38,7 @@ uint8_t *PIOS_BL_HELPER_FLASH_If_Read(uint32_t SectorAddress) { - return (uint8_t *) (SectorAddress); + return (uint8_t *)(SectorAddress); } #if defined(PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT) @@ -53,28 +53,31 @@ uint8_t PIOS_BL_HELPER_FLASH_Ini() uint8_t PIOS_BL_HELPER_FLASH_Start() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; + const struct pios_board_info *bdinfo = &pios_board_info_blob; uint32_t startAddress = bdinfo->fw_base; - uint32_t endAddress = bdinfo->fw_base + bdinfo->fw_size + bdinfo->desc_size; + uint32_t endAddress = bdinfo->fw_base + bdinfo->fw_size + bdinfo->desc_size; bool success = erase_flash(startAddress, endAddress); return (success) ? 1 : 0; } -uint8_t PIOS_BL_HELPER_FLASH_Erase_Bootloader() { +uint8_t PIOS_BL_HELPER_FLASH_Erase_Bootloader() +{ /// Bootloader memory space erase uint32_t startAddress = BL_BANK_BASE; - uint32_t endAddress = BL_BANK_BASE + BL_BANK_SIZE; + uint32_t endAddress = BL_BANK_BASE + BL_BANK_SIZE; bool success = erase_flash(startAddress, endAddress); return (success) ? 1 : 0; } -static bool erase_flash(uint32_t startAddress, uint32_t endAddress) { +static bool erase_flash(uint32_t startAddress, uint32_t endAddress) +{ uint32_t pageAddress = startAddress; uint8_t fail = false; + while ((pageAddress < endAddress) && (fail == false)) { for (int retry = 0; retry < MAX_DEL_RETRYS; ++retry) { if (FLASH_ErasePage(pageAddress) == FLASH_COMPLETE) { @@ -87,39 +90,42 @@ static bool erase_flash(uint32_t startAddress, uint32_t endAddress) { #ifdef STM32F10X_HD pageAddress += 2048; -#elif defined (STM32F10X_MD) +#elif defined(STM32F10X_MD) pageAddress += 1024; #endif } return !fail; } -#endif +#endif /* if defined(PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT) */ uint32_t PIOS_BL_HELPER_CRC_Memory_Calc() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; + const struct pios_board_info *bdinfo = &pios_board_info_blob; - PIOS_BL_HELPER_CRC_Ini(); - CRC_ResetDR(); - CRC_CalcBlockCRC((uint32_t *) bdinfo->fw_base, (bdinfo->fw_size) >> 2); - return CRC_GetCRC(); + PIOS_BL_HELPER_CRC_Ini(); + CRC_ResetDR(); + CRC_CalcBlockCRC((uint32_t *)bdinfo->fw_base, (bdinfo->fw_size) >> 2); + return CRC_GetCRC(); } -void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t * array, uint8_t size) +void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t *array, uint8_t size) { - const struct pios_board_info * bdinfo = &pios_board_info_blob; - uint8_t x = 0; - if (size > bdinfo->desc_size) size = bdinfo->desc_size; - for (uint32_t i = bdinfo->fw_base + bdinfo->fw_size; i < bdinfo->fw_base + bdinfo->fw_size + size; ++i) { - array[x] = *PIOS_BL_HELPER_FLASH_If_Read(i); - ++x; - } + const struct pios_board_info *bdinfo = &pios_board_info_blob; + uint8_t x = 0; + + if (size > bdinfo->desc_size) { + size = bdinfo->desc_size; + } + for (uint32_t i = bdinfo->fw_base + bdinfo->fw_size; i < bdinfo->fw_base + bdinfo->fw_size + size; ++i) { + array[x] = *PIOS_BL_HELPER_FLASH_If_Read(i); + ++x; + } } void PIOS_BL_HELPER_CRC_Ini() { - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); } #endif /* PIOS_INCLUDE_BL_HELPER */ diff --git a/flight/pios/stm32f10x/pios_debug.c b/flight/pios/stm32f10x/pios_debug.c index f0f6dbcaf..9f641f162 100644 --- a/flight/pios/stm32f10x/pios_debug.c +++ b/flight/pios/stm32f10x/pios_debug.c @@ -10,21 +10,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief Debugging Functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,76 +34,76 @@ const char *PIOS_DEBUG_AssertMsg = "ASSERT FAILED"; #ifdef PIOS_ENABLE_DEBUG_PINS -static const struct pios_tim_channel * debug_channels; +static const struct pios_tim_channel *debug_channels; static uint8_t debug_num_channels; -#endif /* PIOS_ENABLE_DEBUG_PINS */ +#endif /* PIOS_ENABLE_DEBUG_PINS */ /** -* Initialise Debug-features -*/ -void PIOS_DEBUG_Init(__attribute__((unused)) const struct pios_tim_channel * channels, - __attribute__((unused)) uint8_t num_channels) + * Initialise Debug-features + */ +void PIOS_DEBUG_Init(__attribute__((unused)) const struct pios_tim_channel *channels, + __attribute__((unused)) uint8_t num_channels) { #ifdef PIOS_ENABLE_DEBUG_PINS - PIOS_Assert(channels); - PIOS_Assert(num_channels); + PIOS_Assert(channels); + PIOS_Assert(num_channels); - /* Store away the GPIOs we've been given */ - debug_channels = channels; - debug_num_channels = num_channels; + /* Store away the GPIOs we've been given */ + debug_channels = channels; + debug_num_channels = num_channels; - /* Configure the GPIOs we've been given */ - for (uint8_t i = 0; i < num_channels; i++) { - const struct pios_tim_channel * chan = &channels[i]; + /* Configure the GPIOs we've been given */ + for (uint8_t i = 0; i < num_channels; i++) { + const struct pios_tim_channel *chan = &channels[i]; - // Initialise pins as standard output pins - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_InitStructure.GPIO_Pin = chan->pin.init.GPIO_Pin; + // Initialise pins as standard output pins + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_Pin = chan->pin.init.GPIO_Pin; - /* Initialize the GPIO */ - GPIO_Init(chan->pin.gpio, &GPIO_InitStructure); + /* Initialize the GPIO */ + GPIO_Init(chan->pin.gpio, &GPIO_InitStructure); - /* Set the pin low */ - GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); - } + /* Set the pin low */ + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); + } #endif // PIOS_ENABLE_DEBUG_PINS } /** -* Set debug-pin high -* \param pin 0 for S1 output -*/ + * Set debug-pin high + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinHigh(__attribute__((unused)) uint8_t pin) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels || pin >= debug_num_channels) { - return; - } + if (!debug_channels || pin >= debug_num_channels) { + return; + } - const struct pios_tim_channel * chan = &debug_channels[pin]; + const struct pios_tim_channel *chan = &debug_channels[pin]; - GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_SET); + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_SET); #endif // PIOS_ENABLE_DEBUG_PINS } /** -* Set debug-pin low -* \param pin 0 for S1 output -*/ + * Set debug-pin low + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinLow(__attribute__((unused)) uint8_t pin) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels || pin >= debug_num_channels) { - return; - } + if (!debug_channels || pin >= debug_num_channels) { + return; + } - const struct pios_tim_channel * chan = &debug_channels[pin]; + const struct pios_tim_channel *chan = &debug_channels[pin]; - GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); #endif // PIOS_ENABLE_DEBUG_PINS } @@ -112,39 +112,39 @@ void PIOS_DEBUG_PinLow(__attribute__((unused)) uint8_t pin) void PIOS_DEBUG_PinValue8Bit(__attribute__((unused)) uint8_t value) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels) { - return; - } + if (!debug_channels) { + return; + } - uint32_t bsrr_l = ( ((~value)&0x0F)<<(16+6) ) | ((value & 0x0F)<<6); - uint32_t bsrr_h = ( ((~value)&0xF0)<<(16+6-4) ) | ((value & 0xF0)<<(6-4)); + uint32_t bsrr_l = (((~value) & 0x0F) << (16 + 6)) | ((value & 0x0F) << 6); + uint32_t bsrr_h = (((~value) & 0xF0) << (16 + 6 - 4)) | ((value & 0xF0) << (6 - 4)); - PIOS_IRQ_Disable(); + PIOS_IRQ_Disable(); - /* - * This is sketchy since it assumes a particular ordering - * and bitwise layout of the channels provided to the debug code. - */ - debug_channels[0].pin.gpio->BSRR = bsrr_l; - debug_channels[4].pin.gpio->BSRR = bsrr_h; + /* + * This is sketchy since it assumes a particular ordering + * and bitwise layout of the channels provided to the debug code. + */ + debug_channels[0].pin.gpio->BSRR = bsrr_l; + debug_channels[4].pin.gpio->BSRR = bsrr_h; - PIOS_IRQ_Enable(); + PIOS_IRQ_Enable(); #endif // PIOS_ENABLE_DEBUG_PINS } void PIOS_DEBUG_PinValue4BitL(__attribute__((unused)) uint8_t value) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels) { - return; - } + if (!debug_channels) { + return; + } - /* - * This is sketchy since it assumes a particular ordering - * and bitwise layout of the channels provided to the debug code. - */ - uint32_t bsrr_l = ((~(value & 0x0F)<<(16+6))) | ((value & 0x0F)<<6); - debug_channels[0].pin.gpio->BSRR = bsrr_l; + /* + * This is sketchy since it assumes a particular ordering + * and bitwise layout of the channels provided to the debug code. + */ + uint32_t bsrr_l = ((~(value & 0x0F) << (16 + 6))) | ((value & 0x0F) << 6); + debug_channels[0].pin.gpio->BSRR = bsrr_l; #endif // PIOS_ENABLE_DEBUG_PINS } @@ -155,15 +155,17 @@ void PIOS_DEBUG_PinValue4BitL(__attribute__((unused)) uint8_t value) void PIOS_DEBUG_Panic(__attribute__((unused)) const char *msg) { #ifdef PIOS_INCLUDE_DEBUG_CONSOLE - register int *lr asm("lr"); // Link-register holds the PC of the caller - DEBUG_PRINTF(0, "\r%s @0x%x\r", msg, lr); + register int *lr asm ("lr"); // Link-register holds the PC of the caller + DEBUG_PRINTF(0, "\r%s @0x%x\r", msg, lr); #endif - // Stay put - while (1) ; + // Stay put + while (1) { + ; + } } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_delay.c b/flight/pios/stm32f10x/pios_delay.c index a61ab45f8..9effa1ec7 100644 --- a/flight/pios/stm32f10x/pios_delay.c +++ b/flight/pios/stm32f10x/pios_delay.c @@ -8,25 +8,25 @@ * * @file pios_delay.c * @author Michael Smith Copyright (C) 2011 - * @brief Delay Functions + * @brief Delay Functions * - Provides a micro-second granular delay using the CPU * cycle counter. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,9 @@ #ifdef PIOS_INCLUDE_DELAY /* these should be defined by CMSIS, but they aren't */ -#define DWT_CTRL (*(volatile uint32_t *)0xe0001000) -#define CYCCNTENA (1<<0) -#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004) +#define DWT_CTRL (*(volatile uint32_t *)0xe0001000) +#define CYCCNTENA (1 << 0) +#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004) /* cycles per microsecond */ @@ -51,20 +51,20 @@ static uint32_t us_ticks; int32_t PIOS_DELAY_Init(void) { - RCC_ClocksTypeDef clocks; + RCC_ClocksTypeDef clocks; - /* compute the number of system clocks per microsecond */ - RCC_GetClocksFreq(&clocks); - us_ticks = clocks.SYSCLK_Frequency / 1000000; - PIOS_DEBUG_Assert(us_ticks > 1); + /* compute the number of system clocks per microsecond */ + RCC_GetClocksFreq(&clocks); + us_ticks = clocks.SYSCLK_Frequency / 1000000; + PIOS_DEBUG_Assert(us_ticks > 1); - /* turn on access to the DWT registers */ - CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + /* turn on access to the DWT registers */ + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; - /* enable the CPU cycle counter */ - DWT_CTRL |= CYCCNTENA; + /* enable the CPU cycle counter */ + DWT_CTRL |= CYCCNTENA; - return 0; + return 0; } /** @@ -80,31 +80,32 @@ int32_t PIOS_DELAY_Init(void) */ int32_t PIOS_DELAY_WaituS(uint32_t uS) { - uint32_t elapsed = 0; - uint32_t last_count = DWT_CYCCNT; - - for (;;) { - uint32_t current_count = DWT_CYCCNT; - uint32_t elapsed_uS; + uint32_t elapsed = 0; + uint32_t last_count = DWT_CYCCNT; - /* measure the time elapsed since the last time we checked */ - elapsed += current_count - last_count; - last_count = current_count; + for (;;) { + uint32_t current_count = DWT_CYCCNT; + uint32_t elapsed_uS; - /* convert to microseconds */ - elapsed_uS = elapsed / us_ticks; - if (elapsed_uS >= uS) - break; + /* measure the time elapsed since the last time we checked */ + elapsed += current_count - last_count; + last_count = current_count; - /* reduce the delay by the elapsed time */ - uS -= elapsed_uS; + /* convert to microseconds */ + elapsed_uS = elapsed / us_ticks; + if (elapsed_uS >= uS) { + break; + } - /* keep fractional microseconds for the next iteration */ - elapsed %= us_ticks; - } + /* reduce the delay by the elapsed time */ + uS -= elapsed_uS; - /* No error */ - return 0; + /* keep fractional microseconds for the next iteration */ + elapsed %= us_ticks; + } + + /* No error */ + return 0; } /** @@ -120,21 +121,21 @@ int32_t PIOS_DELAY_WaituS(uint32_t uS) */ int32_t PIOS_DELAY_WaitmS(uint32_t mS) { - while (mS--) { - PIOS_DELAY_WaituS(1000); - } + while (mS--) { + PIOS_DELAY_WaituS(1000); + } - /* No error */ - return 0; + /* No error */ + return 0; } /** - * @brief Query the Delay timer for the current uS + * @brief Query the Delay timer for the current uS * @return A microsecond value */ uint32_t PIOS_DELAY_GetuS(void) { - return DWT_CYCCNT / us_ticks; + return DWT_CYCCNT / us_ticks; } /** @@ -144,7 +145,7 @@ uint32_t PIOS_DELAY_GetuS(void) */ uint32_t PIOS_DELAY_GetuSSince(uint32_t t) { - return (PIOS_DELAY_GetuS() - t); + return PIOS_DELAY_GetuS() - t; } /** @@ -153,22 +154,23 @@ uint32_t PIOS_DELAY_GetuSSince(uint32_t t) */ uint32_t PIOS_DELAY_GetRaw() { - return DWT_CYCCNT; + return DWT_CYCCNT; } /** - * @brief Compare to raw times to and convert to us + * @brief Compare to raw times to and convert to us * @return A microsecond value */ uint32_t PIOS_DELAY_DiffuS(uint32_t raw) { - uint32_t diff = DWT_CYCCNT - raw; - return diff / us_ticks; + uint32_t diff = DWT_CYCCNT - raw; + + return diff / us_ticks; } #endif /* PIOS_INCLUDE_DELAY */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_dsm.c b/flight/pios/stm32f10x/pios_dsm.c index 9ec036efa..d854d7b9c 100644 --- a/flight/pios/stm32f10x/pios_dsm.c +++ b/flight/pios/stm32f10x/pios_dsm.c @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,132 +38,138 @@ /* Forward Declarations */ static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel); static uint16_t PIOS_DSM_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield); + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield); static void PIOS_DSM_Supervisor(uint32_t dsm_id); /* Local Variables */ const struct pios_rcvr_driver pios_dsm_rcvr_driver = { - .read = PIOS_DSM_Get, + .read = PIOS_DSM_Get, }; enum pios_dsm_dev_magic { - PIOS_DSM_DEV_MAGIC = 0x44534d78, + PIOS_DSM_DEV_MAGIC = 0x44534d78, }; struct pios_dsm_state { - uint16_t channel_data[PIOS_DSM_NUM_INPUTS]; - uint8_t received_data[DSM_FRAME_LENGTH]; - uint8_t receive_timer; - uint8_t failsafe_timer; - uint8_t frame_found; - uint8_t byte_count; + uint16_t channel_data[PIOS_DSM_NUM_INPUTS]; + uint8_t received_data[DSM_FRAME_LENGTH]; + uint8_t receive_timer; + uint8_t failsafe_timer; + uint8_t frame_found; + uint8_t byte_count; #ifdef DSM_LOST_FRAME_COUNTER - uint8_t frames_lost_last; - uint16_t frames_lost; + uint8_t frames_lost_last; + uint16_t frames_lost; #endif }; struct pios_dsm_dev { - enum pios_dsm_dev_magic magic; - const struct pios_dsm_cfg *cfg; - enum pios_dsm_proto proto; - struct pios_dsm_state state; + enum pios_dsm_dev_magic magic; + const struct pios_dsm_cfg *cfg; + enum pios_dsm_proto proto; + struct pios_dsm_state state; }; /* Allocate DSM device descriptor */ #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_dsm_dev *PIOS_DSM_Alloc(void) { - struct pios_dsm_dev *dsm_dev; + struct pios_dsm_dev *dsm_dev; - dsm_dev = (struct pios_dsm_dev *)pvPortMalloc(sizeof(*dsm_dev)); - if (!dsm_dev) - return NULL; + dsm_dev = (struct pios_dsm_dev *)pvPortMalloc(sizeof(*dsm_dev)); + if (!dsm_dev) { + return NULL; + } - dsm_dev->magic = PIOS_DSM_DEV_MAGIC; - return dsm_dev; + dsm_dev->magic = PIOS_DSM_DEV_MAGIC; + return dsm_dev; } #else static struct pios_dsm_dev pios_dsm_devs[PIOS_DSM_MAX_DEVS]; static uint8_t pios_dsm_num_devs; static struct pios_dsm_dev *PIOS_DSM_Alloc(void) { - struct pios_dsm_dev *dsm_dev; + struct pios_dsm_dev *dsm_dev; - if (pios_dsm_num_devs >= PIOS_DSM_MAX_DEVS) - return NULL; + if (pios_dsm_num_devs >= PIOS_DSM_MAX_DEVS) { + return NULL; + } - dsm_dev = &pios_dsm_devs[pios_dsm_num_devs++]; - dsm_dev->magic = PIOS_DSM_DEV_MAGIC; + dsm_dev = &pios_dsm_devs[pios_dsm_num_devs++]; + dsm_dev->magic = PIOS_DSM_DEV_MAGIC; - return dsm_dev; + return dsm_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /* Validate DSM device descriptor */ static bool PIOS_DSM_Validate(struct pios_dsm_dev *dsm_dev) { - return (dsm_dev->magic == PIOS_DSM_DEV_MAGIC); + return dsm_dev->magic == PIOS_DSM_DEV_MAGIC; } /* Try to bind DSMx satellite using specified number of pulses */ static void PIOS_DSM_Bind(struct pios_dsm_dev *dsm_dev, uint8_t bind) { - const struct pios_dsm_cfg *cfg = dsm_dev->cfg; + const struct pios_dsm_cfg *cfg = dsm_dev->cfg; - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Pin = cfg->bind.init.GPIO_Pin; - GPIO_InitStructure.GPIO_Speed = cfg->bind.init.GPIO_Speed; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; + GPIO_InitTypeDef GPIO_InitStructure; - /* just to limit bind pulses */ - if (bind > 10) - bind = 10; + GPIO_InitStructure.GPIO_Pin = cfg->bind.init.GPIO_Pin; + GPIO_InitStructure.GPIO_Speed = cfg->bind.init.GPIO_Speed; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; - GPIO_Init(cfg->bind.gpio, &cfg->bind.init); + /* just to limit bind pulses */ + if (bind > 10) { + bind = 10; + } - /* RX line, set high */ - GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + GPIO_Init(cfg->bind.gpio, &cfg->bind.init); - /* on CC works up to 140ms, guess bind window is around 20-140ms after power up */ - PIOS_DELAY_WaitmS(20); + /* RX line, set high */ + GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - for (int i = 0; i < bind ; i++) { - /* RX line, drive low for 120us */ - GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - PIOS_DELAY_WaituS(120); - /* RX line, drive high for 120us */ - GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - PIOS_DELAY_WaituS(120); - } - /* RX line, set input and wait for data */ - GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure); + /* on CC works up to 140ms, guess bind window is around 20-140ms after power up */ + PIOS_DELAY_WaitmS(20); + + for (int i = 0; i < bind; i++) { + /* RX line, drive low for 120us */ + GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + PIOS_DELAY_WaituS(120); + /* RX line, drive high for 120us */ + GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + PIOS_DELAY_WaituS(120); + } + /* RX line, set input and wait for data */ + GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure); } /* Reset channels in case of lost signal or explicit failsafe receiver flag */ static void PIOS_DSM_ResetChannels(struct pios_dsm_dev *dsm_dev) { - struct pios_dsm_state *state = &(dsm_dev->state); - for (int i = 0; i < PIOS_DSM_NUM_INPUTS; i++) { - state->channel_data[i] = PIOS_RCVR_TIMEOUT; - } + struct pios_dsm_state *state = &(dsm_dev->state); + + for (int i = 0; i < PIOS_DSM_NUM_INPUTS; i++) { + state->channel_data[i] = PIOS_RCVR_TIMEOUT; + } } /* Reset DSM receiver state */ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev) { - struct pios_dsm_state *state = &(dsm_dev->state); - state->receive_timer = 0; - state->failsafe_timer = 0; - state->frame_found = 0; + struct pios_dsm_state *state = &(dsm_dev->state); + + state->receive_timer = 0; + state->failsafe_timer = 0; + state->frame_found = 0; #ifdef DSM_LOST_FRAME_COUNTER - state->frames_lost_last = 0; - state->frames_lost = 0; + state->frames_lost_last = 0; + state->frames_lost = 0; #endif - PIOS_DSM_ResetChannels(dsm_dev); + PIOS_DSM_ResetChannels(dsm_dev); } /** @@ -173,174 +179,182 @@ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev) */ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) { - struct pios_dsm_state *state = &(dsm_dev->state); - uint8_t resolution; + struct pios_dsm_state *state = &(dsm_dev->state); + uint8_t resolution; #ifdef DSM_LOST_FRAME_COUNTER - /* increment the lost frame counter */ - uint8_t frames_lost = state->received_data[0]; - state->frames_lost += (frames_lost - state->frames_lost_last); - state->frames_lost_last = frames_lost; + /* increment the lost frame counter */ + uint8_t frames_lost = state->received_data[0]; + state->frames_lost += (frames_lost - state->frames_lost_last); + state->frames_lost_last = frames_lost; #endif - /* check the frame type assuming master satellite stream */ - uint8_t type = state->received_data[1]; - switch (type) { - case 0x01: - case 0x02: - case 0x12: - /* DSM2, DSMJ stream */ - if (dsm_dev->proto == PIOS_DSM_PROTO_DSM2) { - /* DSM2/DSMJ resolution is known from the header */ - resolution = (type & DSM_DSM2_RES_MASK) ? 11 : 10; - } else { - /* DSMX resolution should explicitly be selected */ - goto stream_error; - } - break; - case 0xA2: - case 0xB2: - /* DSMX stream */ - if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) { - resolution = 10; - } else if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) { - resolution = 11; - } else { - /* DSMX resolution should explicitly be selected */ - goto stream_error; - } - break; - default: - /* unknown yet data stream */ - goto stream_error; - } + /* check the frame type assuming master satellite stream */ + uint8_t type = state->received_data[1]; + switch (type) { + case 0x01: + case 0x02: + case 0x12: + /* DSM2, DSMJ stream */ + if (dsm_dev->proto == PIOS_DSM_PROTO_DSM2) { + /* DSM2/DSMJ resolution is known from the header */ + resolution = (type & DSM_DSM2_RES_MASK) ? 11 : 10; + } else { + /* DSMX resolution should explicitly be selected */ + goto stream_error; + } + break; + case 0xA2: + case 0xB2: + /* DSMX stream */ + if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) { + resolution = 10; + } else if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) { + resolution = 11; + } else { + /* DSMX resolution should explicitly be selected */ + goto stream_error; + } + break; + default: + /* unknown yet data stream */ + goto stream_error; + } - /* unroll channels */ - uint8_t *s = &(state->received_data[2]); - uint16_t mask = (resolution == 10) ? 0x03ff : 0x07ff; + /* unroll channels */ + uint8_t *s = &(state->received_data[2]); + uint16_t mask = (resolution == 10) ? 0x03ff : 0x07ff; - for (int i = 0; i < DSM_CHANNELS_PER_FRAME; i++) { - uint16_t word = ((uint16_t)s[0] << 8) | s[1]; - s += 2; + for (int i = 0; i < DSM_CHANNELS_PER_FRAME; i++) { + uint16_t word = ((uint16_t)s[0] << 8) | s[1]; + s += 2; - /* skip empty channel slot */ - if (word == 0xffff) - continue; + /* skip empty channel slot */ + if (word == 0xffff) { + continue; + } - /* minimal data validation */ - if ((i > 0) && (word & DSM_2ND_FRAME_MASK)) { - /* invalid frame data, ignore rest of the frame */ - goto stream_error; - } + /* minimal data validation */ + if ((i > 0) && (word & DSM_2ND_FRAME_MASK)) { + /* invalid frame data, ignore rest of the frame */ + goto stream_error; + } - /* extract and save the channel value */ - uint8_t channel_num = (word >> resolution) & 0x0f; - if (channel_num < PIOS_DSM_NUM_INPUTS) - state->channel_data[channel_num] = (word & mask); - } + /* extract and save the channel value */ + uint8_t channel_num = (word >> resolution) & 0x0f; + if (channel_num < PIOS_DSM_NUM_INPUTS) { + state->channel_data[channel_num] = (word & mask); + } + } #ifdef DSM_LOST_FRAME_COUNTER - /* put lost frames counter into the last channel for debugging */ - state->channel_data[PIOS_DSM_NUM_INPUTS-1] = state->frames_lost; + /* put lost frames counter into the last channel for debugging */ + state->channel_data[PIOS_DSM_NUM_INPUTS - 1] = state->frames_lost; #endif - /* all channels processed */ - return 0; + /* all channels processed */ + return 0; stream_error: - /* either DSM2 selected with DSMX stream found, or vice-versa */ - return -1; + /* either DSM2 selected with DSMX stream found, or vice-versa */ + return -1; } /* Update decoder state processing input byte from the DSMx stream */ static void PIOS_DSM_UpdateState(struct pios_dsm_dev *dsm_dev, uint8_t byte) { - struct pios_dsm_state *state = &(dsm_dev->state); - if (state->frame_found) { - /* receiving the data frame */ - if (state->byte_count < DSM_FRAME_LENGTH) { - /* store next byte */ - state->received_data[state->byte_count++] = byte; - if (state->byte_count == DSM_FRAME_LENGTH) { - /* full frame received - process and wait for new one */ - if (!PIOS_DSM_UnrollChannels(dsm_dev)) - /* data looking good */ - state->failsafe_timer = 0; + struct pios_dsm_state *state = &(dsm_dev->state); - /* prepare for the next frame */ - state->frame_found = 0; - } - } - } + if (state->frame_found) { + /* receiving the data frame */ + if (state->byte_count < DSM_FRAME_LENGTH) { + /* store next byte */ + state->received_data[state->byte_count++] = byte; + if (state->byte_count == DSM_FRAME_LENGTH) { + /* full frame received - process and wait for new one */ + if (!PIOS_DSM_UnrollChannels(dsm_dev)) { + /* data looking good */ + state->failsafe_timer = 0; + } + + /* prepare for the next frame */ + state->frame_found = 0; + } + } + } } /* Initialise DSM receiver interface */ int32_t PIOS_DSM_Init(uint32_t *dsm_id, - const struct pios_dsm_cfg *cfg, - const struct pios_com_driver *driver, - uint32_t lower_id, - enum pios_dsm_proto proto, - uint8_t bind) + const struct pios_dsm_cfg *cfg, + const struct pios_com_driver *driver, + uint32_t lower_id, + enum pios_dsm_proto proto, + uint8_t bind) { - PIOS_DEBUG_Assert(dsm_id); - PIOS_DEBUG_Assert(cfg); - PIOS_DEBUG_Assert(driver); + PIOS_DEBUG_Assert(dsm_id); + PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(driver); - struct pios_dsm_dev *dsm_dev; + struct pios_dsm_dev *dsm_dev; - dsm_dev = (struct pios_dsm_dev *)PIOS_DSM_Alloc(); - if (!dsm_dev) - return -1; + dsm_dev = (struct pios_dsm_dev *)PIOS_DSM_Alloc(); + if (!dsm_dev) { + return -1; + } - /* Bind the configuration to the device instance */ - dsm_dev->cfg = cfg; - dsm_dev->proto = proto; + /* Bind the configuration to the device instance */ + dsm_dev->cfg = cfg; + dsm_dev->proto = proto; - /* Bind the receiver if requested */ - if (bind) - PIOS_DSM_Bind(dsm_dev, bind); + /* Bind the receiver if requested */ + if (bind) { + PIOS_DSM_Bind(dsm_dev, bind); + } - PIOS_DSM_ResetState(dsm_dev); + PIOS_DSM_ResetState(dsm_dev); - *dsm_id = (uint32_t)dsm_dev; + *dsm_id = (uint32_t)dsm_dev; - /* Set comm driver callback */ - (driver->bind_rx_cb)(lower_id, PIOS_DSM_RxInCallback, *dsm_id); + /* Set comm driver callback */ + (driver->bind_rx_cb)(lower_id, PIOS_DSM_RxInCallback, *dsm_id); - if (!PIOS_RTC_RegisterTickCallback(PIOS_DSM_Supervisor, *dsm_id)) { - PIOS_DEBUG_Assert(0); - } + if (!PIOS_RTC_RegisterTickCallback(PIOS_DSM_Supervisor, *dsm_id)) { + PIOS_DEBUG_Assert(0); + } - return 0; + return 0; } /* Comm byte received callback */ static uint16_t PIOS_DSM_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield) + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield) { - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)context; + struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)context; - bool valid = PIOS_DSM_Validate(dsm_dev); - PIOS_Assert(valid); + bool valid = PIOS_DSM_Validate(dsm_dev); - /* process byte(s) and clear receive timer */ - for (uint8_t i = 0; i < buf_len; i++) { - PIOS_DSM_UpdateState(dsm_dev, buf[i]); - dsm_dev->state.receive_timer = 0; - } + PIOS_Assert(valid); - /* Always signal that we can accept another byte */ - if (headroom) - *headroom = DSM_FRAME_LENGTH; + /* process byte(s) and clear receive timer */ + for (uint8_t i = 0; i < buf_len; i++) { + PIOS_DSM_UpdateState(dsm_dev, buf[i]); + dsm_dev->state.receive_timer = 0; + } - /* We never need a yield */ - *need_yield = false; + /* Always signal that we can accept another byte */ + if (headroom) { + *headroom = DSM_FRAME_LENGTH; + } - /* Always indicate that all bytes were consumed */ - return buf_len; + /* We never need a yield */ + *need_yield = false; + + /* Always indicate that all bytes were consumed */ + return buf_len; } /** @@ -352,17 +366,19 @@ static uint16_t PIOS_DSM_RxInCallback(uint32_t context, */ static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)rcvr_id; + struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)rcvr_id; - if (!PIOS_DSM_Validate(dsm_dev)) - return PIOS_RCVR_INVALID; + if (!PIOS_DSM_Validate(dsm_dev)) { + return PIOS_RCVR_INVALID; + } - /* return error if channel is not available */ - if (channel >= PIOS_DSM_NUM_INPUTS) - return PIOS_RCVR_INVALID; + /* return error if channel is not available */ + if (channel >= PIOS_DSM_NUM_INPUTS) { + return PIOS_RCVR_INVALID; + } - /* may also be PIOS_RCVR_TIMEOUT set by other function */ - return dsm_dev->state.channel_data[channel]; + /* may also be PIOS_RCVR_TIMEOUT set by other function */ + return dsm_dev->state.channel_data[channel]; } /** @@ -379,30 +395,31 @@ static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel) */ static void PIOS_DSM_Supervisor(uint32_t dsm_id) { - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id; + struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id; - bool valid = PIOS_DSM_Validate(dsm_dev); - PIOS_Assert(valid); + bool valid = PIOS_DSM_Validate(dsm_dev); - struct pios_dsm_state *state = &(dsm_dev->state); + PIOS_Assert(valid); - /* waiting for new frame if no bytes were received in 8ms */ - if (++state->receive_timer > 4) { - state->frame_found = 1; - state->byte_count = 0; - state->receive_timer = 0; - } + struct pios_dsm_state *state = &(dsm_dev->state); - /* activate failsafe if no frames have arrived in 102.4ms */ - if (++state->failsafe_timer > 64) { - PIOS_DSM_ResetChannels(dsm_dev); - state->failsafe_timer = 0; - } + /* waiting for new frame if no bytes were received in 8ms */ + if (++state->receive_timer > 4) { + state->frame_found = 1; + state->byte_count = 0; + state->receive_timer = 0; + } + + /* activate failsafe if no frames have arrived in 102.4ms */ + if (++state->failsafe_timer > 64) { + PIOS_DSM_ResetChannels(dsm_dev); + state->failsafe_timer = 0; + } } #endif /* PIOS_INCLUDE_DSM */ -/** +/** * @} * @} */ diff --git a/flight/pios/stm32f10x/pios_eeprom.c b/flight/pios/stm32f10x/pios_eeprom.c index 8771a522a..75fbced04 100644 --- a/flight/pios/stm32f10x/pios_eeprom.c +++ b/flight/pios/stm32f10x/pios_eeprom.c @@ -1,30 +1,30 @@ /** -****************************************************************************** -* @addtogroup PIOS PIOS Core hardware abstraction layer -* @{ -* @addtogroup PIOS_EEPROM EEPROM reading/writing functions -* @brief PIOS EEPROM reading/writing functions -* @{ -* -* @file pios_eeprom.c -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief COM layer functions -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_EEPROM EEPROM reading/writing functions + * @brief PIOS EEPROM reading/writing functions + * @{ + * + * @file pios_eeprom.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief COM layer functions + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,7 +43,7 @@ static struct pios_eeprom_cfg config; */ void PIOS_EEPROM_Init(const struct pios_eeprom_cfg *cfg) { - config = *cfg; + config = *cfg; } /** @@ -54,71 +54,78 @@ void PIOS_EEPROM_Init(const struct pios_eeprom_cfg *cfg) */ int32_t PIOS_EEPROM_Save(uint8_t *data, uint32_t len) { + // We need to write 32 bit words, so extend the length to be an even multiple of 4 bytes, + // and include 4 bytes for the 32 bit CRC. + uint32_t nwords = (len / 4) + 1 + (len % 4 ? 1 : 0); + uint32_t size = nwords * 4; - // We need to write 32 bit words, so extend the length to be an even multiple of 4 bytes, - // and include 4 bytes for the 32 bit CRC. - uint32_t nwords = (len / 4) + 1 + (len % 4 ? 1 : 0); - uint32_t size = nwords * 4; + // Ensure that the length is not longer than the max size. + if (size > config.max_size) { + return -1; + } - // Ensure that the length is not longer than the max size. - if (size > config.max_size) - return -1; + // Calculate a 32 bit CRC of the data. + uint32_t crc = PIOS_CRC32_updateCRC(0xffffffff, data, len); - // Calculate a 32 bit CRC of the data. - uint32_t crc = PIOS_CRC32_updateCRC(0xffffffff, data, len); + // Unlock the Flash Program Erase controller + FLASH_Unlock(); - // Unlock the Flash Program Erase controller - FLASH_Unlock(); + // See if we have to write the data. + if ((memcmp(data, (uint8_t *)config.base_address, len) == 0) && + (memcmp((uint8_t *)&crc, (uint8_t *)config.base_address + size - 4, 4) == 0)) { + return 0; + } - // See if we have to write the data. - if ((memcmp(data, (uint8_t*)config.base_address, len) == 0) && - (memcmp((uint8_t*)&crc, (uint8_t*)config.base_address + size - 4, 4) == 0)) - return 0; + // TODO: Check that the area isn't already erased - // TODO: Check that the area isn't already erased + // Erase page + FLASH_Status fs = FLASH_ErasePage(config.base_address); + if (fs != FLASH_COMPLETE) { // error + FLASH_Lock(); + return -2; + } - // Erase page - FLASH_Status fs = FLASH_ErasePage(config.base_address); - if (fs != FLASH_COMPLETE) - { // error - FLASH_Lock(); - return -2; - } - - // write 4 bytes at a time into program flash area (emulated EEPROM area) - uint8_t *p1 = data; - uint32_t *p3 = (uint32_t *)config.base_address; - for (uint32_t i = 0; i < size; p3++) - { - uint32_t value = 0; + // write 4 bytes at a time into program flash area (emulated EEPROM area) + uint8_t *p1 = data; + uint32_t *p3 = (uint32_t *)config.base_address; + for (uint32_t i = 0; i < size; p3++) { + uint32_t value = 0; - if (i == (size - 4)) - { - // write the CRC. - value = crc; - i += 4; - } - else - { - if (i < len) value |= (uint32_t)*p1++ << 0; else value |= 0x000000ff; i++; - if (i < len) value |= (uint32_t)*p1++ << 8; else value |= 0x0000ff00; i++; - if (i < len) value |= (uint32_t)*p1++ << 16; else value |= 0x00ff0000; i++; - if (i < len) value |= (uint32_t)*p1++ << 24; else value |= 0xff000000; i++; - } + if (i == (size - 4)) { + // write the CRC. + value = crc; + i += 4; + } else { + if (i < len) { + value |= (uint32_t)*p1++ << 0; + } else { value |= 0x000000ff; } + i++; + if (i < len) { + value |= (uint32_t)*p1++ << 8; + } else { value |= 0x0000ff00; } + i++; + if (i < len) { + value |= (uint32_t)*p1++ << 16; + } else { value |= 0x00ff0000; } + i++; + if (i < len) { + value |= (uint32_t)*p1++ << 24; + } else { value |= 0xff000000; } + i++; + } - // write a 32-bit value - fs = FLASH_ProgramWord((uint32_t)p3, value); - if (fs != FLASH_COMPLETE) - { - FLASH_Lock(); - return -3; - } - } - - // Lock the Flash Program Erase controller - FLASH_Lock(); + // write a 32-bit value + fs = FLASH_ProgramWord((uint32_t)p3, value); + if (fs != FLASH_COMPLETE) { + FLASH_Lock(); + return -3; + } + } - return 0; + // Lock the Flash Program Erase controller + FLASH_Lock(); + + return 0; } /** @@ -129,28 +136,29 @@ int32_t PIOS_EEPROM_Save(uint8_t *data, uint32_t len) */ int32_t PIOS_EEPROM_Load(uint8_t *data, uint32_t len) { + // We need to write 32 bit words, so the length should have been extended + // to an even multiple of 4 bytes, and should include 4 bytes for the 32 bit CRC. + uint32_t nwords = (len / 4) + 1 + (len % 4 ? 1 : 0); + uint32_t size = nwords * 4; - // We need to write 32 bit words, so the length should have been extended - // to an even multiple of 4 bytes, and should include 4 bytes for the 32 bit CRC. - uint32_t nwords = (len / 4) + 1 + (len % 4 ? 1 : 0); - uint32_t size = nwords * 4; + // Ensure that the length is not longer than the max size. + if (size > config.max_size) { + return -1; + } - // Ensure that the length is not longer than the max size. - if (size > config.max_size) - return -1; + // Read the data from flash. + memcpy(data, (uint8_t *)config.base_address, len); - // Read the data from flash. - memcpy(data, (uint8_t*)config.base_address, len); + // Read the CRC. + uint32_t crc_flash = *((uint32_t *)(config.base_address + size - 4)); - // Read the CRC. - uint32_t crc_flash = *((uint32_t*)(config.base_address + size - 4)); + // Calculate a 32 bit CRC of the data. + uint32_t crc = PIOS_CRC32_updateCRC(0xffffffff, data, len); + if (crc != crc_flash) { + return -2; + } - // Calculate a 32 bit CRC of the data. - uint32_t crc = PIOS_CRC32_updateCRC(0xffffffff, data, len); - if(crc != crc_flash) - return -2; - - return 0; + return 0; } #endif /* PIOS_INCLUDE_FLASH_EEPROM */ diff --git a/flight/pios/stm32f10x/pios_exti.c b/flight/pios/stm32f10x/pios_exti.c index 9d1980bcc..efdda068e 100644 --- a/flight/pios/stm32f10x/pios_exti.c +++ b/flight/pios/stm32f10x/pios_exti.c @@ -13,19 +13,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,262 +34,298 @@ #ifdef PIOS_INCLUDE_EXTI /* Map EXTI line to full config */ -#define EXTI_MAX_LINES 16 +#define EXTI_MAX_LINES 16 #define PIOS_EXTI_INVALID 0xFF static uint8_t pios_exti_line_to_cfg_map[EXTI_MAX_LINES] = { - [0 ... EXTI_MAX_LINES-1] = PIOS_EXTI_INVALID, + [0 ... EXTI_MAX_LINES - 1] = PIOS_EXTI_INVALID, }; /* Table of exti configs registered at compile time */ extern struct pios_exti_cfg __start__exti __attribute__((weak)); extern struct pios_exti_cfg __stop__exti __attribute__((weak)); -static uint8_t PIOS_EXTI_line_to_index (uint32_t line) +static uint8_t PIOS_EXTI_line_to_index(uint32_t line) { - switch (line) { - case EXTI_Line0: return 0; - case EXTI_Line1: return 1; - case EXTI_Line2: return 2; - case EXTI_Line3: return 3; - case EXTI_Line4: return 4; - case EXTI_Line5: return 5; - case EXTI_Line6: return 6; - case EXTI_Line7: return 7; - case EXTI_Line8: return 8; - case EXTI_Line9: return 9; - case EXTI_Line10: return 10; - case EXTI_Line11: return 11; - case EXTI_Line12: return 12; - case EXTI_Line13: return 13; - case EXTI_Line14: return 14; - case EXTI_Line15: return 15; - } + switch (line) { + case EXTI_Line0: return 0; - PIOS_Assert(0); - return 0xFF; + case EXTI_Line1: return 1; + + case EXTI_Line2: return 2; + + case EXTI_Line3: return 3; + + case EXTI_Line4: return 4; + + case EXTI_Line5: return 5; + + case EXTI_Line6: return 6; + + case EXTI_Line7: return 7; + + case EXTI_Line8: return 8; + + case EXTI_Line9: return 9; + + case EXTI_Line10: return 10; + + case EXTI_Line11: return 11; + + case EXTI_Line12: return 12; + + case EXTI_Line13: return 13; + + case EXTI_Line14: return 14; + + case EXTI_Line15: return 15; + } + + PIOS_Assert(0); + return 0xFF; } -uint8_t PIOS_EXTI_gpio_port_to_exti_source_port(GPIO_TypeDef * gpio_port) +uint8_t PIOS_EXTI_gpio_port_to_exti_source_port(GPIO_TypeDef *gpio_port) { - switch((uint32_t)gpio_port) { - case (uint32_t)GPIOA: return (GPIO_PortSourceGPIOA); - case (uint32_t)GPIOB: return (GPIO_PortSourceGPIOB); - case (uint32_t)GPIOC: return (GPIO_PortSourceGPIOC); - case (uint32_t)GPIOD: return (GPIO_PortSourceGPIOD); - case (uint32_t)GPIOE: return (GPIO_PortSourceGPIOE); - case (uint32_t)GPIOF: return (GPIO_PortSourceGPIOF); - case (uint32_t)GPIOG: return (GPIO_PortSourceGPIOG); - } + switch ((uint32_t)gpio_port) { + case (uint32_t)GPIOA: return GPIO_PortSourceGPIOA; - PIOS_Assert(0); - return 0xFF; + case (uint32_t)GPIOB: return GPIO_PortSourceGPIOB; + + case (uint32_t)GPIOC: return GPIO_PortSourceGPIOC; + + case (uint32_t)GPIOD: return GPIO_PortSourceGPIOD; + + case (uint32_t)GPIOE: return GPIO_PortSourceGPIOE; + + case (uint32_t)GPIOF: return GPIO_PortSourceGPIOF; + + case (uint32_t)GPIOG: return GPIO_PortSourceGPIOG; + } + + PIOS_Assert(0); + return 0xFF; } uint8_t PIOS_EXTI_gpio_pin_to_exti_source_pin(uint32_t gpio_pin) { - switch((uint32_t)gpio_pin) { - case GPIO_Pin_0: return (GPIO_PinSource0); - case GPIO_Pin_1: return (GPIO_PinSource1); - case GPIO_Pin_2: return (GPIO_PinSource2); - case GPIO_Pin_3: return (GPIO_PinSource3); - case GPIO_Pin_4: return (GPIO_PinSource4); - case GPIO_Pin_5: return (GPIO_PinSource5); - case GPIO_Pin_6: return (GPIO_PinSource6); - case GPIO_Pin_7: return (GPIO_PinSource7); - case GPIO_Pin_8: return (GPIO_PinSource8); - case GPIO_Pin_9: return (GPIO_PinSource9); - case GPIO_Pin_10: return (GPIO_PinSource10); - case GPIO_Pin_11: return (GPIO_PinSource11); - case GPIO_Pin_12: return (GPIO_PinSource12); - case GPIO_Pin_13: return (GPIO_PinSource13); - case GPIO_Pin_14: return (GPIO_PinSource14); - case GPIO_Pin_15: return (GPIO_PinSource15); - } + switch ((uint32_t)gpio_pin) { + case GPIO_Pin_0: return GPIO_PinSource0; - PIOS_Assert(0); - return 0xFF; + case GPIO_Pin_1: return GPIO_PinSource1; + + case GPIO_Pin_2: return GPIO_PinSource2; + + case GPIO_Pin_3: return GPIO_PinSource3; + + case GPIO_Pin_4: return GPIO_PinSource4; + + case GPIO_Pin_5: return GPIO_PinSource5; + + case GPIO_Pin_6: return GPIO_PinSource6; + + case GPIO_Pin_7: return GPIO_PinSource7; + + case GPIO_Pin_8: return GPIO_PinSource8; + + case GPIO_Pin_9: return GPIO_PinSource9; + + case GPIO_Pin_10: return GPIO_PinSource10; + + case GPIO_Pin_11: return GPIO_PinSource11; + + case GPIO_Pin_12: return GPIO_PinSource12; + + case GPIO_Pin_13: return GPIO_PinSource13; + + case GPIO_Pin_14: return GPIO_PinSource14; + + case GPIO_Pin_15: return GPIO_PinSource15; + } + + PIOS_Assert(0); + return 0xFF; } -int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg) +int32_t PIOS_EXTI_Init(const struct pios_exti_cfg *cfg) { - PIOS_Assert(cfg); - PIOS_Assert(&__start__exti); - PIOS_Assert(cfg >= &__start__exti); - PIOS_Assert(cfg < &__stop__exti); + PIOS_Assert(cfg); + PIOS_Assert(&__start__exti); + PIOS_Assert(cfg >= &__start__exti); + PIOS_Assert(cfg < &__stop__exti); - uint8_t cfg_index = cfg - &__start__exti; + uint8_t cfg_index = cfg - &__start__exti; - /* Connect this config to the requested vector */ - uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line); + /* Connect this config to the requested vector */ + uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line); - if (pios_exti_line_to_cfg_map[line_index] != PIOS_EXTI_INVALID) { - /* Someone else already has this mapped */ - goto out_fail; - } + if (pios_exti_line_to_cfg_map[line_index] != PIOS_EXTI_INVALID) { + /* Someone else already has this mapped */ + goto out_fail; + } - /* Bind the config to the exti line */ - pios_exti_line_to_cfg_map[line_index] = cfg_index; + /* Bind the config to the exti line */ + pios_exti_line_to_cfg_map[line_index] = cfg_index; - /* Initialize the GPIO pin */ - GPIO_Init(cfg->pin.gpio, &cfg->pin.init); + /* Initialize the GPIO pin */ + GPIO_Init(cfg->pin.gpio, &cfg->pin.init); - /* Set up the EXTI interrupt source */ - uint8_t exti_source_port = PIOS_EXTI_gpio_port_to_exti_source_port(cfg->pin.gpio); - uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin); - GPIO_EXTILineConfig(exti_source_port, exti_source_pin); - EXTI_Init(&cfg->exti.init); + /* Set up the EXTI interrupt source */ + uint8_t exti_source_port = PIOS_EXTI_gpio_port_to_exti_source_port(cfg->pin.gpio); + uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin); + GPIO_EXTILineConfig(exti_source_port, exti_source_pin); + EXTI_Init(&cfg->exti.init); - /* Enable the interrupt channel */ - NVIC_Init(&cfg->irq.init); + /* Enable the interrupt channel */ + NVIC_Init(&cfg->irq.init); - return 0; + return 0; out_fail: - return -1; + return -1; } static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index) { - uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index]; + uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index]; - PIOS_Assert(&__start__exti); + PIOS_Assert(&__start__exti); - if (cfg_index > NELEMENTS(pios_exti_line_to_cfg_map) || - cfg_index == PIOS_EXTI_INVALID) { - /* Unconfigured interrupt just fired! */ - return false; - } + if (cfg_index > NELEMENTS(pios_exti_line_to_cfg_map) || + cfg_index == PIOS_EXTI_INVALID) { + /* Unconfigured interrupt just fired! */ + return false; + } - struct pios_exti_cfg * cfg = &__start__exti + cfg_index; - return cfg->vector(); + struct pios_exti_cfg *cfg = &__start__exti + cfg_index; + return cfg->vector(); } #ifdef PIOS_INCLUDE_FREERTOS -#define PIOS_EXTI_HANDLE_LINE(line, woken) \ - if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ - EXTI_ClearITPendingBit(EXTI_Line##line); \ - woken = PIOS_EXTI_generic_irq_handler(line) ? pdTRUE : woken; \ - } +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + woken = PIOS_EXTI_generic_irq_handler(line) ? pdTRUE : woken; \ + } #else -#define PIOS_EXTI_HANDLE_LINE(line, woken) \ - if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ - EXTI_ClearITPendingBit(EXTI_Line##line); \ - PIOS_EXTI_generic_irq_handler(line); \ - } +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + PIOS_EXTI_generic_irq_handler(line); \ + } #endif /* Bind Interrupt Handlers */ -static void PIOS_EXTI_0_irq_handler (void) +static void PIOS_EXTI_0_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; // dummy variable + bool xHigherPriorityTaskWoken; // dummy variable #endif - PIOS_EXTI_HANDLE_LINE(0, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(0, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI0_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_0_irq_handler"))); +void EXTI0_IRQHandler(void) __attribute__((alias("PIOS_EXTI_0_irq_handler"))); -static void PIOS_EXTI_1_irq_handler (void) +static void PIOS_EXTI_1_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; // dummy variable + bool xHigherPriorityTaskWoken; // dummy variable #endif - PIOS_EXTI_HANDLE_LINE(1, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(1, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI1_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_1_irq_handler"))); +void EXTI1_IRQHandler(void) __attribute__((alias("PIOS_EXTI_1_irq_handler"))); -static void PIOS_EXTI_2_irq_handler (void) +static void PIOS_EXTI_2_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; // dummy variable + bool xHigherPriorityTaskWoken; // dummy variable #endif - PIOS_EXTI_HANDLE_LINE(2, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(2, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI2_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_2_irq_handler"))); +void EXTI2_IRQHandler(void) __attribute__((alias("PIOS_EXTI_2_irq_handler"))); -static void PIOS_EXTI_3_irq_handler (void) +static void PIOS_EXTI_3_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; // dummy variable + bool xHigherPriorityTaskWoken; // dummy variable #endif - PIOS_EXTI_HANDLE_LINE(3, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(3, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI3_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_3_irq_handler"))); +void EXTI3_IRQHandler(void) __attribute__((alias("PIOS_EXTI_3_irq_handler"))); -static void PIOS_EXTI_4_irq_handler (void) +static void PIOS_EXTI_4_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; // dummy variable + bool xHigherPriorityTaskWoken; // dummy variable #endif - PIOS_EXTI_HANDLE_LINE(4, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(4, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI4_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_4_irq_handler"))); +void EXTI4_IRQHandler(void) __attribute__((alias("PIOS_EXTI_4_irq_handler"))); -static void PIOS_EXTI_9_5_irq_handler (void) +static void PIOS_EXTI_9_5_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; // dummy variable + bool xHigherPriorityTaskWoken; // dummy variable #endif - PIOS_EXTI_HANDLE_LINE(5, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(6, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(7, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(8, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(9, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(5, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(6, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(7, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(8, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(9, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI9_5_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_9_5_irq_handler"))); +void EXTI9_5_IRQHandler(void) __attribute__((alias("PIOS_EXTI_9_5_irq_handler"))); -static void PIOS_EXTI_15_10_irq_handler (void) +static void PIOS_EXTI_15_10_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; // dummy variable + bool xHigherPriorityTaskWoken; // dummy variable #endif - PIOS_EXTI_HANDLE_LINE(10, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(11, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(12, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(13, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(14, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(15, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(10, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(11, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(12, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(13, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(14, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(15, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI15_10_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_15_10_irq_handler"))); +void EXTI15_10_IRQHandler(void) __attribute__((alias("PIOS_EXTI_15_10_irq_handler"))); #endif /* PIOS_INCLUDE_EXTI */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_gpio.c b/flight/pios/stm32f10x/pios_gpio.c index abedc162d..81bdac8f2 100644 --- a/flight/pios/stm32f10x/pios_gpio.c +++ b/flight/pios/stm32f10x/pios_gpio.c @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,61 +40,62 @@ static const uint32_t GPIO_PIN[PIOS_GPIO_NUM] = PIOS_GPIO_PINS; static const uint32_t GPIO_CLK[PIOS_GPIO_NUM] = PIOS_GPIO_CLKS; /** -* Initialises all the GPIO's -*/ + * Initialises all the GPIO's + */ void PIOS_GPIO_Init(void) { - /* Do nothing */ + /* Do nothing */ } /** -* Enable a GPIO Pin -* \param[in] Pin Pin Number -*/ + * Enable a GPIO Pin + * \param[in] Pin Pin Number + */ void PIOS_GPIO_Enable(uint8_t Pin) { - //RCC_APB2PeriphClockCmd(GPIO_CLK[Pin], ENABLE); + // RCC_APB2PeriphClockCmd(GPIO_CLK[Pin], ENABLE); - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_InitStructure.GPIO_Pin = GPIO_PIN[Pin]; - GPIO_Init(GPIO_PORT[Pin], &GPIO_InitStructure); + GPIO_InitTypeDef GPIO_InitStructure; - /* GPIO's Off */ - GPIO_PORT[Pin]->BSRR = GPIO_PIN[Pin]; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_Pin = GPIO_PIN[Pin]; + GPIO_Init(GPIO_PORT[Pin], &GPIO_InitStructure); + + /* GPIO's Off */ + GPIO_PORT[Pin]->BSRR = GPIO_PIN[Pin]; } /** -* Turn on Pin -* \param[in] Pin Pin Number -*/ + * Turn on Pin + * \param[in] Pin Pin Number + */ void PIOS_GPIO_On(uint8_t Pin) { - GPIO_PORT[Pin]->BRR = GPIO_PIN[Pin]; + GPIO_PORT[Pin]->BRR = GPIO_PIN[Pin]; } /** -* Turn off Pin -* \param[in] Pin Pin Number -*/ + * Turn off Pin + * \param[in] Pin Pin Number + */ void PIOS_GPIO_Off(uint8_t Pin) { - GPIO_PORT[Pin]->BSRR = GPIO_PIN[Pin]; + GPIO_PORT[Pin]->BSRR = GPIO_PIN[Pin]; } /** -* Toggle Pin on/off -* \param[in] Pin Pin Number -*/ + * Toggle Pin on/off + * \param[in] Pin Pin Number + */ void PIOS_GPIO_Toggle(uint8_t Pin) { - GPIO_PORT[Pin]->ODR ^= GPIO_PIN[Pin]; + GPIO_PORT[Pin]->ODR ^= GPIO_PIN[Pin]; } #endif /* PIOS_INCLUDE_GPIO */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_i2c.c b/flight/pios/stm32f10x/pios_i2c.c index 054cc1454..4a95b7ee5 100644 --- a/flight/pios/stm32f10x/pios_i2c.c +++ b/flight/pios/stm32f10x/pios_i2c.c @@ -6,25 +6,25 @@ * @brief STM32 Hardware dependent I2C functionality * @{ * - * @file pios_i2c.c + * @file pios_i2c.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief I2C Enable/Disable routines * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,28 +38,28 @@ #include -//#define I2C_HALT_ON_ERRORS +// #define I2C_HALT_ON_ERRORS enum i2c_adapter_event { - I2C_EVENT_BUS_ERROR, - I2C_EVENT_START, - I2C_EVENT_STARTED_MORE_TXN_READ, - I2C_EVENT_STARTED_MORE_TXN_WRITE, - I2C_EVENT_STARTED_LAST_TXN_READ, - I2C_EVENT_STARTED_LAST_TXN_WRITE, - I2C_EVENT_ADDR_SENT_LEN_EQ_0, - I2C_EVENT_ADDR_SENT_LEN_EQ_1, - I2C_EVENT_ADDR_SENT_LEN_EQ_2, - I2C_EVENT_ADDR_SENT_LEN_GT_2, - I2C_EVENT_TRANSFER_DONE_LEN_EQ_0, - I2C_EVENT_TRANSFER_DONE_LEN_EQ_1, - I2C_EVENT_TRANSFER_DONE_LEN_EQ_2, - I2C_EVENT_TRANSFER_DONE_LEN_GT_2, - I2C_EVENT_NACK, - I2C_EVENT_STOPPED, - I2C_EVENT_AUTO, /* FIXME: remove this */ + I2C_EVENT_BUS_ERROR, + I2C_EVENT_START, + I2C_EVENT_STARTED_MORE_TXN_READ, + I2C_EVENT_STARTED_MORE_TXN_WRITE, + I2C_EVENT_STARTED_LAST_TXN_READ, + I2C_EVENT_STARTED_LAST_TXN_WRITE, + I2C_EVENT_ADDR_SENT_LEN_EQ_0, + I2C_EVENT_ADDR_SENT_LEN_EQ_1, + I2C_EVENT_ADDR_SENT_LEN_EQ_2, + I2C_EVENT_ADDR_SENT_LEN_GT_2, + I2C_EVENT_TRANSFER_DONE_LEN_EQ_0, + I2C_EVENT_TRANSFER_DONE_LEN_EQ_1, + I2C_EVENT_TRANSFER_DONE_LEN_EQ_2, + I2C_EVENT_TRANSFER_DONE_LEN_GT_2, + I2C_EVENT_NACK, + I2C_EVENT_STOPPED, + I2C_EVENT_AUTO, /* FIXME: remove this */ - I2C_EVENT_NUM_EVENTS /* Must be last */ + I2C_EVENT_NUM_EVENTS /* Must be last */ }; #if defined(PIOS_I2C_DIAGNOSTICS) @@ -77,11 +77,11 @@ volatile uint8_t i2c_state_history_pointer = 0; volatile enum i2c_adapter_event i2c_state_event_history[I2C_LOG_DEPTH]; volatile uint8_t i2c_state_event_history_pointer; -static uint8_t i2c_fsm_fault_count = 0; +static uint8_t i2c_fsm_fault_count = 0; static uint8_t i2c_bad_event_counter = 0; static uint8_t i2c_error_interrupt_counter = 0; static uint8_t i2c_nack_counter = 0; -static uint8_t i2c_timeout_counter = 0; +static uint8_t i2c_timeout_counter = 0; #endif static void go_fsm_fault(struct pios_i2c_adapter *i2c_adapter); @@ -114,8 +114,8 @@ static void go_w_last_txn_last(struct pios_i2c_adapter *i2c_adapter); static void go_nack(struct pios_i2c_adapter *i2c_adapter); struct i2c_adapter_transition { - void (*entry_fn) (struct pios_i2c_adapter * i2c_adapter); - enum i2c_adapter_state next_state[I2C_EVENT_NUM_EVENTS]; + void (*entry_fn)(struct pios_i2c_adapter *i2c_adapter); + enum i2c_adapter_state next_state[I2C_EVENT_NUM_EVENTS]; }; static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter); @@ -127,613 +127,619 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter); static void i2c_adapter_log_fault(enum pios_i2c_error_type type); static const struct i2c_adapter_transition i2c_adapter_transitions[I2C_STATE_NUM_STATES] = { - [I2C_STATE_FSM_FAULT] = { - .entry_fn = go_fsm_fault, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, - [I2C_STATE_BUS_ERROR] = { - .entry_fn = go_bus_error, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, + [I2C_STATE_FSM_FAULT] = { + .entry_fn = go_fsm_fault, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, + [I2C_STATE_BUS_ERROR] = { + .entry_fn = go_bus_error, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, - [I2C_STATE_STOPPED] = { - .entry_fn = go_stopped, - .next_state = { - [I2C_EVENT_START] = I2C_STATE_STARTING, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_STOPPED] = { + .entry_fn = go_stopped, + .next_state = { + [I2C_EVENT_START] = I2C_STATE_STARTING, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_STOPPING] = { - .entry_fn = go_stopping, - .next_state = { - [I2C_EVENT_STOPPED] = I2C_STATE_STOPPED, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_STOPPING] = { + .entry_fn = go_stopping, + .next_state = { + [I2C_EVENT_STOPPED] = I2C_STATE_STOPPED, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_STARTING] = { - .entry_fn = go_starting, - .next_state = { - [I2C_EVENT_STARTED_MORE_TXN_READ] = I2C_STATE_R_MORE_TXN_ADDR, - [I2C_EVENT_STARTED_MORE_TXN_WRITE] = I2C_STATE_W_MORE_TXN_ADDR, - [I2C_EVENT_STARTED_LAST_TXN_READ] = I2C_STATE_R_LAST_TXN_ADDR, - [I2C_EVENT_STARTED_LAST_TXN_WRITE] = I2C_STATE_W_LAST_TXN_ADDR, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_STARTING] = { + .entry_fn = go_starting, + .next_state = { + [I2C_EVENT_STARTED_MORE_TXN_READ] = I2C_STATE_R_MORE_TXN_ADDR, + [I2C_EVENT_STARTED_MORE_TXN_WRITE] = I2C_STATE_W_MORE_TXN_ADDR, + [I2C_EVENT_STARTED_LAST_TXN_READ] = I2C_STATE_R_LAST_TXN_ADDR, + [I2C_EVENT_STARTED_LAST_TXN_WRITE] = I2C_STATE_W_LAST_TXN_ADDR, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - /* - * Read with restart - */ + /* + * Read with restart + */ - [I2C_STATE_R_MORE_TXN_ADDR] = { - .entry_fn = go_r_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_PRE_ONE, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_ADDR] = { + .entry_fn = go_r_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_PRE_ONE, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_ONE] = { - .entry_fn = go_r_more_txn_pre_one, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_ONE] = { + .entry_fn = go_r_more_txn_pre_one, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_FIRST] = { - .entry_fn = go_r_any_txn_pre_first, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_FIRST] = { + .entry_fn = go_r_any_txn_pre_first, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_MIDDLE] = { - .entry_fn = go_r_any_txn_pre_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_MIDDLE] = { + .entry_fn = go_r_any_txn_pre_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_LAST] = { - .entry_fn = go_r_more_txn_pre_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_LAST] = { + .entry_fn = go_r_more_txn_pre_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_POST_LAST] = { - .entry_fn = go_r_any_txn_post_last, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STARTING, - }, - }, + [I2C_STATE_R_MORE_TXN_POST_LAST] = { + .entry_fn = go_r_any_txn_post_last, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STARTING, + }, + }, - /* - * Read - */ + /* + * Read + */ - [I2C_STATE_R_LAST_TXN_ADDR] = { - .entry_fn = go_r_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_PRE_ONE, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_ADDR] = { + .entry_fn = go_r_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_PRE_ONE, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_ONE] = { - .entry_fn = go_r_last_txn_pre_one, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_ONE] = { + .entry_fn = go_r_last_txn_pre_one, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_FIRST] = { - .entry_fn = go_r_any_txn_pre_first, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_FIRST] = { + .entry_fn = go_r_any_txn_pre_first, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_MIDDLE] = { - .entry_fn = go_r_any_txn_pre_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_MIDDLE] = { + .entry_fn = go_r_any_txn_pre_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_LAST] = { - .entry_fn = go_r_last_txn_pre_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_LAST] = { + .entry_fn = go_r_last_txn_pre_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_POST_LAST] = { - .entry_fn = go_r_any_txn_post_last, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, + [I2C_STATE_R_LAST_TXN_POST_LAST] = { + .entry_fn = go_r_any_txn_post_last, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, - /* - * Write with restart - */ + /* + * Write with restart + */ - [I2C_STATE_W_MORE_TXN_ADDR] = { - .entry_fn = go_w_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_MORE_TXN_ADDR] = { + .entry_fn = go_w_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_MORE_TXN_MIDDLE] = { - .entry_fn = go_w_any_txn_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_MORE_TXN_MIDDLE] = { + .entry_fn = go_w_any_txn_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_MORE_TXN_LAST] = { - .entry_fn = go_w_more_txn_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STARTING, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_MORE_TXN_LAST] = { + .entry_fn = go_w_more_txn_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STARTING, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - /* - * Write - */ + /* + * Write + */ - [I2C_STATE_W_LAST_TXN_ADDR] = { - .entry_fn = go_w_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_LAST_TXN_ADDR] = { + .entry_fn = go_w_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_LAST_TXN_MIDDLE] = { - .entry_fn = go_w_any_txn_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_LAST_TXN_MIDDLE] = { + .entry_fn = go_w_any_txn_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_LAST_TXN_LAST] = { - .entry_fn = go_w_last_txn_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STOPPING, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, - [I2C_STATE_NACK] = { - .entry_fn = go_nack, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, + [I2C_STATE_W_LAST_TXN_LAST] = { + .entry_fn = go_w_last_txn_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STOPPING, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, + [I2C_STATE_NACK] = { + .entry_fn = go_nack, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, }; static void go_fsm_fault(struct pios_i2c_adapter *i2c_adapter) { #if defined(I2C_HALT_ON_ERRORS) - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); #endif - /* Note that this transfer has hit a bus error */ - i2c_adapter->bus_error = true; + /* Note that this transfer has hit a bus error */ + i2c_adapter->bus_error = true; - i2c_adapter_reset_bus(i2c_adapter); - + i2c_adapter_reset_bus(i2c_adapter); } static void go_bus_error(struct pios_i2c_adapter *i2c_adapter) { - /* Note that this transfer has hit a bus error */ - i2c_adapter->bus_error = true; + /* Note that this transfer has hit a bus error */ + i2c_adapter->bus_error = true; - i2c_adapter_reset_bus(i2c_adapter); + i2c_adapter_reset_bus(i2c_adapter); } static void go_stopping(struct pios_i2c_adapter *i2c_adapter) { #ifdef USE_FREERTOS - signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; + signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; #endif - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); #ifdef USE_FREERTOS - if (xSemaphoreGiveFromISR(i2c_adapter->sem_ready, &pxHigherPriorityTaskWoken) != pdTRUE) { + if (xSemaphoreGiveFromISR(i2c_adapter->sem_ready, &pxHigherPriorityTaskWoken) != pdTRUE) { #if defined(I2C_HALT_ON_ERRORS) - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); #endif - } - portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for this? */ + } + portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for this? */ #endif /* USE_FREERTOS */ } static void go_stopped(struct pios_i2c_adapter *i2c_adapter) { - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); } static void go_starting(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]); - i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); + i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]); + i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); - I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); - if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) { - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); - } else { - // For write operations, do not enable the IT_BUF events. - // The current driver does not act when the TX data register is not full, only when the complete byte is sent. - // With the IT_BUF enabled, we constantly get IRQs, See OP-326 - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_ERR, ENABLE); - } + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); + if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) { + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); + } else { + // For write operations, do not enable the IT_BUF events. + // The current driver does not act when the TX data register is not full, only when the complete byte is sent. + // With the IT_BUF enabled, we constantly get IRQs, See OP-326 + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_ERR, ENABLE); + } } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_addr(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ); + PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ); - I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Receiver); + I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Receiver); } static void go_r_more_txn_pre_one(struct pios_i2c_adapter *i2c_adapter) { - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); } static void go_r_last_txn_pre_one(struct pios_i2c_adapter *i2c_adapter) { - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_pre_first(struct pios_i2c_adapter *i2c_adapter) { - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_pre_middle(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } static void go_r_more_txn_pre_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - PIOS_IRQ_Disable(); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); - PIOS_IRQ_Enable(); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + PIOS_IRQ_Disable(); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); + PIOS_IRQ_Enable(); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } static void go_r_last_txn_pre_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - PIOS_IRQ_Disable(); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); - PIOS_IRQ_Enable(); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + PIOS_IRQ_Disable(); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); + PIOS_IRQ_Enable(); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_post_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - /* Move to the next byte */ - i2c_adapter->active_byte++; + /* Move to the next byte */ + i2c_adapter->active_byte++; - /* Move to the next transaction */ - i2c_adapter->active_txn++; + /* Move to the next transaction */ + i2c_adapter->active_txn++; } /* Common to 'more' and 'last' transaction */ static void go_w_any_txn_addr(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_WRITE); + PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_WRITE); - I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Transmitter); + I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Transmitter); } static void go_w_any_txn_middle(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte < i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte < i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); + I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } static void go_w_more_txn_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); + I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); - /* Move to the next byte */ - i2c_adapter->active_byte++; + /* Move to the next byte */ + i2c_adapter->active_byte++; - /* Move to the next transaction */ - i2c_adapter->active_txn++; - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + /* Move to the next transaction */ + i2c_adapter->active_txn++; + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); } static void go_w_last_txn_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_BUF, DISABLE); - I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_BUF, DISABLE); + I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); // SHOULD MOVE THIS INTO A STOPPING STATE AND SET IT ONLY AFTER THE BYTE WAS SENT - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); - /* Move to the next byte */ - i2c_adapter->active_byte++; + /* Move to the next byte */ + i2c_adapter->active_byte++; } -static void go_nack(struct pios_i2c_adapter *i2c_adapter) +static void go_nack(struct pios_i2c_adapter *i2c_adapter) { - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); } static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum i2c_adapter_event event) { - PIOS_IRQ_Disable(); + PIOS_IRQ_Disable(); -#if defined(PIOS_I2C_DIAGNOSTICS) - i2c_state_event_history[i2c_state_event_history_pointer] = event; - i2c_state_event_history_pointer = (i2c_state_event_history_pointer + 1) % I2C_LOG_DEPTH; +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_state_event_history[i2c_state_event_history_pointer] = event; + i2c_state_event_history_pointer = (i2c_state_event_history_pointer + 1) % I2C_LOG_DEPTH; - i2c_state_history[i2c_state_history_pointer] = i2c_adapter->curr_state; - i2c_state_history_pointer = (i2c_state_history_pointer + 1) % I2C_LOG_DEPTH; - - if(i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event] == I2C_STATE_FSM_FAULT) - i2c_adapter_log_fault(PIOS_I2C_ERROR_FSM); -#endif - /* - * Move to the next state - * - * This is done prior to calling the new state's entry function to - * guarantee that the entry function never depends on the previous - * state. This way, it cannot ever know what the previous state was. - */ - i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event]; + i2c_state_history[i2c_state_history_pointer] = i2c_adapter->curr_state; + i2c_state_history_pointer = (i2c_state_history_pointer + 1) % I2C_LOG_DEPTH; - /* Call the entry function (if any) for the next state. */ - if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { - i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); - } + if (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event] == I2C_STATE_FSM_FAULT) { + i2c_adapter_log_fault(PIOS_I2C_ERROR_FSM); + } +#endif + /* + * Move to the next state + * + * This is done prior to calling the new state's entry function to + * guarantee that the entry function never depends on the previous + * state. This way, it cannot ever know what the previous state was. + */ + i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event]; - /* Process any AUTO transitions in the FSM */ - i2c_adapter_process_auto(i2c_adapter); + /* Call the entry function (if any) for the next state. */ + if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { + i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); + } - PIOS_IRQ_Enable(); + /* Process any AUTO transitions in the FSM */ + i2c_adapter_process_auto(i2c_adapter); + + PIOS_IRQ_Enable(); } static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter) { - PIOS_IRQ_Disable(); - while (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]) { - i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]; + PIOS_IRQ_Disable(); + while (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]) { + i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]; - /* Call the entry function (if any) for the next state. */ - if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { - i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); - } - } + /* Call the entry function (if any) for the next state. */ + if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { + i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); + } + } - PIOS_IRQ_Enable(); + PIOS_IRQ_Enable(); } static void i2c_adapter_fsm_init(struct pios_i2c_adapter *i2c_adapter) { - i2c_adapter_reset_bus(i2c_adapter); - i2c_adapter->curr_state = I2C_STATE_STOPPED; + i2c_adapter_reset_bus(i2c_adapter); + i2c_adapter->curr_state = I2C_STATE_STOPPED; } static bool i2c_adapter_wait_for_stopped(struct pios_i2c_adapter *i2c_adapter) { - uint32_t guard; + uint32_t guard; - /* - * Wait for the bus to return to the stopped state. - * This was pulled out of the FSM due to occasional - * failures at this transition which previously resulted - * in spinning on this bit in the ISR forever. - */ + /* + * Wait for the bus to return to the stopped state. + * This was pulled out of the FSM due to occasional + * failures at this transition which previously resulted + * in spinning on this bit in the ISR forever. + */ #define I2C_CR1_STOP_REQUESTED 0x0200 - for (guard = 1000000; /* FIXME: should use the configured bus timeout */ - guard && (i2c_adapter->cfg->regs->CR1 & I2C_CR1_STOP_REQUESTED); guard--) - continue; - if (!guard) { - /* We timed out waiting for the stop condition */ - return false; - } + for (guard = 1000000; /* FIXME: should use the configured bus timeout */ + guard && (i2c_adapter->cfg->regs->CR1 & I2C_CR1_STOP_REQUESTED); guard--) { + continue; + } + if (!guard) { + /* We timed out waiting for the stop condition */ + return false; + } - return true; + return true; } static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) { - /* Reset the I2C block */ - I2C_DeInit(i2c_adapter->cfg->regs); + /* Reset the I2C block */ + I2C_DeInit(i2c_adapter->cfg->regs); - /* Make sure the bus is free by clocking it until any slaves release the bus. */ - GPIO_InitTypeDef scl_gpio_init; - scl_gpio_init = i2c_adapter->cfg->scl.init; - scl_gpio_init.GPIO_Mode = GPIO_Mode_Out_OD; - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - GPIO_Init(i2c_adapter->cfg->scl.gpio, &scl_gpio_init); + /* Make sure the bus is free by clocking it until any slaves release the bus. */ + GPIO_InitTypeDef scl_gpio_init; + scl_gpio_init = i2c_adapter->cfg->scl.init; + scl_gpio_init.GPIO_Mode = GPIO_Mode_Out_OD; + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + GPIO_Init(i2c_adapter->cfg->scl.gpio, &scl_gpio_init); - GPIO_InitTypeDef sda_gpio_init; - sda_gpio_init = i2c_adapter->cfg->sda.init; - sda_gpio_init.GPIO_Mode = GPIO_Mode_Out_OD; - GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - GPIO_Init(i2c_adapter->cfg->sda.gpio, &sda_gpio_init); + GPIO_InitTypeDef sda_gpio_init; + sda_gpio_init = i2c_adapter->cfg->sda.init; + sda_gpio_init.GPIO_Mode = GPIO_Mode_Out_OD; + GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + GPIO_Init(i2c_adapter->cfg->sda.gpio, &sda_gpio_init); - /* Check SDA line to determine if slave is asserting bus and clock out if so, this may */ - /* have to be repeated (due to futher bus errors) but better than clocking 0xFF into an */ - /* ESC */ - //bool sda_hung = GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET; - while(GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET) { - - /* Set clock high and wait for any clock stretching to finish. */ - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) ; - PIOS_DELAY_WaituS(2); - - /* Set clock low */ - GPIO_ResetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - - /* Clock high again */ - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - } - - /* Generate a start then stop condition */ - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); + /* Check SDA line to determine if slave is asserting bus and clock out if so, this may */ + /* have to be repeated (due to futher bus errors) but better than clocking 0xFF into an */ + /* ESC */ + // bool sda_hung = GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET; + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET) { + /* Set clock high and wait for any clock stretching to finish. */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) { + ; + } + PIOS_DELAY_WaituS(2); - /* Set data and clock high and wait for any clock stretching to finish. */ - GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) ; - /* Wait for data to be high */ - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) != Bit_SET) ; + /* Set clock low */ + GPIO_ResetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); - - /* Bus signals are guaranteed to be high (ie. free) after this point */ - /* Initialize the GPIO pins to the peripheral function */ - GPIO_Init(i2c_adapter->cfg->scl.gpio, &(i2c_adapter->cfg->scl.init)); - GPIO_Init(i2c_adapter->cfg->sda.gpio, &(i2c_adapter->cfg->sda.init)); + /* Clock high again */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + } - /* Reset the I2C block */ - I2C_DeInit(i2c_adapter->cfg->regs); + /* Generate a start then stop condition */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); - /* Initialize the I2C block */ - I2C_Init(i2c_adapter->cfg->regs, &(i2c_adapter->cfg->init)); + /* Set data and clock high and wait for any clock stretching to finish. */ + GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) { + ; + } + /* Wait for data to be high */ + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) != Bit_SET) { + ; + } + + + /* Bus signals are guaranteed to be high (ie. free) after this point */ + /* Initialize the GPIO pins to the peripheral function */ + GPIO_Init(i2c_adapter->cfg->scl.gpio, &(i2c_adapter->cfg->scl.init)); + GPIO_Init(i2c_adapter->cfg->sda.gpio, &(i2c_adapter->cfg->sda.init)); + + /* Reset the I2C block */ + I2C_DeInit(i2c_adapter->cfg->regs); + + /* Initialize the I2C block */ + I2C_Init(i2c_adapter->cfg->regs, &(i2c_adapter->cfg->init)); #define I2C_BUSY 0x20 - if (i2c_adapter->cfg->regs->SR2 & I2C_BUSY) { - /* Reset the I2C block */ - I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, ENABLE); - I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, DISABLE); - } + if (i2c_adapter->cfg->regs->SR2 & I2C_BUSY) { + /* Reset the I2C block */ + I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, ENABLE); + I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, DISABLE); + } } #include @@ -741,13 +747,14 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) /* Return true if the FSM is in a terminal state */ static bool i2c_adapter_fsm_terminated(struct pios_i2c_adapter *i2c_adapter) { - switch (i2c_adapter->curr_state) { - case I2C_STATE_STOPPING: - case I2C_STATE_STOPPED: - return (true); - default: - return (false); - } + switch (i2c_adapter->curr_state) { + case I2C_STATE_STOPPING: + case I2C_STATE_STOPPED: + return true; + + default: + return false; + } } /** @@ -758,29 +765,29 @@ static bool i2c_adapter_fsm_terminated(struct pios_i2c_adapter *i2c_adapter) void i2c_adapter_log_fault(enum pios_i2c_error_type type) { #if defined(PIOS_I2C_DIAGNOSTICS) - i2c_adapter_fault_history.type = type; - for(uint8_t i = 0; i < I2C_LOG_DEPTH; i++) { - i2c_adapter_fault_history.evirq[i] = - i2c_evirq_history[(I2C_LOG_DEPTH + i2c_evirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - i2c_adapter_fault_history.erirq[i] = - i2c_erirq_history[(I2C_LOG_DEPTH + i2c_erirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - i2c_adapter_fault_history.event[i] = - i2c_state_event_history[(I2C_LOG_DEPTH + i2c_state_event_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - i2c_adapter_fault_history.state[i] = - i2c_state_history[(I2C_LOG_DEPTH + i2c_state_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - } - switch(type) { - case PIOS_I2C_ERROR_EVENT: - i2c_bad_event_counter++; - break; - case PIOS_I2C_ERROR_FSM: - i2c_fsm_fault_count++; - break; - case PIOS_I2C_ERROR_INTERRUPT: - i2c_error_interrupt_counter++; - break; - } -#endif + i2c_adapter_fault_history.type = type; + for (uint8_t i = 0; i < I2C_LOG_DEPTH; i++) { + i2c_adapter_fault_history.evirq[i] = + i2c_evirq_history[(I2C_LOG_DEPTH + i2c_evirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.erirq[i] = + i2c_erirq_history[(I2C_LOG_DEPTH + i2c_erirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.event[i] = + i2c_state_event_history[(I2C_LOG_DEPTH + i2c_state_event_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.state[i] = + i2c_state_history[(I2C_LOG_DEPTH + i2c_state_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + } + switch (type) { + case PIOS_I2C_ERROR_EVENT: + i2c_bad_event_counter++; + break; + case PIOS_I2C_ERROR_FSM: + i2c_fsm_fault_count++; + break; + case PIOS_I2C_ERROR_INTERRUPT: + i2c_error_interrupt_counter++; + break; + } +#endif /* if defined(PIOS_I2C_DIAGNOSTICS) */ } @@ -788,359 +795,376 @@ void i2c_adapter_log_fault(enum pios_i2c_error_type type) * Logs the last N state transitions and N IRQ events due to * an error condition * \param[out] data address where to copy the pios_i2c_fault_history structure to - * \param[out] counts three uint16 that receive the bad event, fsm, and error irq + * \param[out] counts three uint16 that receive the bad event, fsm, and error irq * counts */ -void PIOS_I2C_GetDiagnostics(struct pios_i2c_fault_history * data, uint8_t * counts) +void PIOS_I2C_GetDiagnostics(struct pios_i2c_fault_history *data, uint8_t *counts) { #if defined(PIOS_I2C_DIAGNOSTICS) - memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); - counts[0] = i2c_bad_event_counter; - counts[1] = i2c_fsm_fault_count; - counts[2] = i2c_error_interrupt_counter; - counts[3] = i2c_nack_counter; - counts[4] = i2c_timeout_counter; + memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); + counts[0] = i2c_bad_event_counter; + counts[1] = i2c_fsm_fault_count; + counts[2] = i2c_error_interrupt_counter; + counts[3] = i2c_nack_counter; + counts[4] = i2c_timeout_counter; #else - struct pios_i2c_fault_history i2c_adapter_fault_history; - i2c_adapter_fault_history.type = PIOS_I2C_ERROR_EVENT; + struct pios_i2c_fault_history i2c_adapter_fault_history; + i2c_adapter_fault_history.type = PIOS_I2C_ERROR_EVENT; - memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); - counts[0] = counts[1] = counts[2] = 0; + memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); + counts[0] = counts[1] = counts[2] = 0; #endif } -static bool PIOS_I2C_validate(struct pios_i2c_adapter * i2c_adapter) +static bool PIOS_I2C_validate(struct pios_i2c_adapter *i2c_adapter) { - return (i2c_adapter->magic == PIOS_I2C_DEV_MAGIC); + return i2c_adapter->magic == PIOS_I2C_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_i2c_adapter * PIOS_I2C_alloc(void) +static struct pios_i2c_adapter *PIOS_I2C_alloc(void) { - struct pios_i2c_adapter * i2c_adapter; + struct pios_i2c_adapter *i2c_adapter; - i2c_adapter = (struct pios_i2c_adapter *)pvPortMalloc(sizeof(*i2c_adapter)); - if (!i2c_adapter) return(NULL); + i2c_adapter = (struct pios_i2c_adapter *)pvPortMalloc(sizeof(*i2c_adapter)); + if (!i2c_adapter) { + return NULL; + } - i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; - return(i2c_adapter); + i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; + return i2c_adapter; } #else static struct pios_i2c_adapter pios_i2c_adapters[PIOS_I2C_MAX_DEVS]; static uint8_t pios_i2c_num_adapters; -static struct pios_i2c_adapter * PIOS_I2C_alloc(void) +static struct pios_i2c_adapter *PIOS_I2C_alloc(void) { - struct pios_i2c_adapter * i2c_adapter; + struct pios_i2c_adapter *i2c_adapter; - if (pios_i2c_num_adapters >= PIOS_I2C_MAX_DEVS) { - return (NULL); - } + if (pios_i2c_num_adapters >= PIOS_I2C_MAX_DEVS) { + return NULL; + } - i2c_adapter = &pios_i2c_adapters[pios_i2c_num_adapters++]; - i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; + i2c_adapter = &pios_i2c_adapters[pios_i2c_num_adapters++]; + i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; - return (i2c_adapter); + return i2c_adapter; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /** -* Initializes IIC driver -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ -int32_t PIOS_I2C_Init(uint32_t * i2c_id, const struct pios_i2c_adapter_cfg * cfg) + * Initializes IIC driver + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ +int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg) { - PIOS_DEBUG_Assert(i2c_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(i2c_id); + PIOS_DEBUG_Assert(cfg); - struct pios_i2c_adapter * i2c_adapter; + struct pios_i2c_adapter *i2c_adapter; - i2c_adapter = (struct pios_i2c_adapter *) PIOS_I2C_alloc(); - if (!i2c_adapter) goto out_fail; + i2c_adapter = (struct pios_i2c_adapter *)PIOS_I2C_alloc(); + if (!i2c_adapter) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - i2c_adapter->cfg = cfg; + /* Bind the configuration to the device instance */ + i2c_adapter->cfg = cfg; #ifdef USE_FREERTOS - /* - * Must be done prior to calling i2c_adapter_fsm_init() - * since the sem_ready mutex is used in the initial state. - */ - vSemaphoreCreateBinary(i2c_adapter->sem_ready); - i2c_adapter->sem_busy = xSemaphoreCreateMutex(); + /* + * Must be done prior to calling i2c_adapter_fsm_init() + * since the sem_ready mutex is used in the initial state. + */ + vSemaphoreCreateBinary(i2c_adapter->sem_ready); + i2c_adapter->sem_busy = xSemaphoreCreateMutex(); #endif // USE_FREERTOS - /* Enable the associated peripheral clock */ - switch ((uint32_t) i2c_adapter->cfg->regs) { - case (uint32_t) I2C1: - /* Enable I2C peripheral clock (APB1 == slow speed) */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); - break; - case (uint32_t) I2C2: - /* Enable I2C peripheral clock (APB1 == slow speed) */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); - break; - } + /* Enable the associated peripheral clock */ + switch ((uint32_t)i2c_adapter->cfg->regs) { + case (uint32_t)I2C1: + /* Enable I2C peripheral clock (APB1 == slow speed) */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); + break; + case (uint32_t)I2C2: + /* Enable I2C peripheral clock (APB1 == slow speed) */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); + break; + } - if (i2c_adapter->cfg->remap) { - GPIO_PinRemapConfig(i2c_adapter->cfg->remap, ENABLE); - } + if (i2c_adapter->cfg->remap) { + GPIO_PinRemapConfig(i2c_adapter->cfg->remap, ENABLE); + } - /* Initialize the state machine */ - i2c_adapter_fsm_init(i2c_adapter); + /* Initialize the state machine */ + i2c_adapter_fsm_init(i2c_adapter); - *i2c_id = (uint32_t)i2c_adapter; + *i2c_id = (uint32_t)i2c_adapter; - /* Configure and enable I2C interrupts */ - NVIC_Init(&(i2c_adapter->cfg->event.init)); - NVIC_Init(&(i2c_adapter->cfg->error.init)); - - /* No error */ - return 0; + /* Configure and enable I2C interrupts */ + NVIC_Init(&(i2c_adapter->cfg->event.init)); + NVIC_Init(&(i2c_adapter->cfg->error.init)); + + /* No error */ + return 0; out_fail: - return(-1); + return -1; } /** * @brief Perform a series of I2C transactions * @returns 0 if success or error code - * @retval -1 for failed transaction + * @retval -1 for failed transaction * @retval -2 for failure to get semaphore */ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns) { - struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_Assert(valid) + bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_DEBUG_Assert(txn_list); - PIOS_DEBUG_Assert(num_txns); + PIOS_Assert(valid) - bool semaphore_success = true; + PIOS_DEBUG_Assert(txn_list); + PIOS_DEBUG_Assert(num_txns); + + bool semaphore_success = true; #ifdef USE_FREERTOS - /* Lock the bus */ - portTickType timeout; - timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; - if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) - return -2; + /* Lock the bus */ + portTickType timeout; + timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; + if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { + return -2; + } #else - uint32_t timeout = 0xfff; - while(i2c_adapter->busy && --timeout); - if(timeout == 0) //timed out - return false; - - PIOS_IRQ_Disable(); - if(i2c_adapter->busy) - return false; - i2c_adapter->busy = 1; - PIOS_IRQ_Enable(); + uint32_t timeout = 0xfff; + while (i2c_adapter->busy && --timeout) { + ; + } + if (timeout == 0) { // timed out + return false; + } + + PIOS_IRQ_Disable(); + if (i2c_adapter->busy) { + return false; + } + i2c_adapter->busy = 1; + PIOS_IRQ_Enable(); #endif /* USE_FREERTOS */ - PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); + PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); - i2c_adapter->first_txn = &txn_list[0]; - i2c_adapter->last_txn = &txn_list[num_txns - 1]; - i2c_adapter->active_txn = i2c_adapter->first_txn; + i2c_adapter->first_txn = &txn_list[0]; + i2c_adapter->last_txn = &txn_list[num_txns - 1]; + i2c_adapter->active_txn = i2c_adapter->first_txn; #ifdef USE_FREERTOS - /* Make sure the done/ready semaphore is consumed before we start */ - semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); + /* Make sure the done/ready semaphore is consumed before we start */ + semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); #endif - i2c_adapter->bus_error = false; - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); + i2c_adapter->bus_error = false; + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); - /* Wait for the transfer to complete */ + /* Wait for the transfer to complete */ #ifdef USE_FREERTOS - semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); - xSemaphoreGive(i2c_adapter->sem_ready); + semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); + xSemaphoreGive(i2c_adapter->sem_ready); #else - PIOS_IRQ_Disable(); - i2c_adapter->busy = 0; - PIOS_IRQ_Enable(); + PIOS_IRQ_Disable(); + i2c_adapter->busy = 0; + PIOS_IRQ_Enable(); #endif /* USE_FREERTOS */ - /* Spin waiting for the transfer to finish */ - while (!i2c_adapter_fsm_terminated(i2c_adapter)) ; + /* Spin waiting for the transfer to finish */ + while (!i2c_adapter_fsm_terminated(i2c_adapter)) { + ; + } - if (i2c_adapter_wait_for_stopped(i2c_adapter)) { - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOPPED); - } else { - i2c_adapter_fsm_init(i2c_adapter); - } + if (i2c_adapter_wait_for_stopped(i2c_adapter)) { + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOPPED); + } else { + i2c_adapter_fsm_init(i2c_adapter); + } #ifdef USE_FREERTOS - /* Unlock the bus */ - xSemaphoreGive(i2c_adapter->sem_busy); - if(!semaphore_success) - i2c_timeout_counter++; + /* Unlock the bus */ + xSemaphoreGive(i2c_adapter->sem_busy); + if (!semaphore_success) { + i2c_timeout_counter++; + } #endif /* USE_FREERTOS */ - return !semaphore_success ? -2 : - i2c_adapter->bus_error ? -1 : - 0; + return !semaphore_success ? -2 : + i2c_adapter->bus_error ? -1 : + 0; } void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id) { - struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_Assert(valid) + bool valid = PIOS_I2C_validate(i2c_adapter); - uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + PIOS_Assert(valid) -#if defined(PIOS_I2C_DIAGNOSTICS) - /* Store event for diagnostics */ - i2c_evirq_history[i2c_evirq_history_pointer] = event; - i2c_evirq_history_pointer = (i2c_evirq_history_pointer + 1) % I2C_LOG_DEPTH; + uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + +#if defined(PIOS_I2C_DIAGNOSTICS) + /* Store event for diagnostics */ + i2c_evirq_history[i2c_evirq_history_pointer] = event; + i2c_evirq_history_pointer = (i2c_evirq_history_pointer + 1) % I2C_LOG_DEPTH; #endif - + #define EVENT_MASK 0x000700FF - event &= EVENT_MASK; - - - switch (event) { /* Mask out all the bits we don't care about */ - case (I2C_EVENT_MASTER_MODE_SELECT | 0x40): - /* Unexplained event: EV5 + RxNE : Extraneous Rx. Probably a late NACK from previous read. */ - /* Clean up the extra Rx until the root cause is identified and just keep going */ - (void)I2C_ReceiveData(i2c_adapter->cfg->regs); - /* Fall through */ - case I2C_EVENT_MASTER_MODE_SELECT: /* EV5 */ - switch (i2c_adapter->active_txn->rw) { - case PIOS_I2C_TXN_READ: - if (i2c_adapter->active_txn == i2c_adapter->last_txn) { - /* Final transaction */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_READ); - } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { - /* More transactions follow */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_READ); - } else { - PIOS_DEBUG_Assert(0); - } - break; - case PIOS_I2C_TXN_WRITE: - if (i2c_adapter->active_txn == i2c_adapter->last_txn) { - /* Final transaction */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_WRITE); - } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { - /* More transactions follow */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_WRITE); - } else { - PIOS_DEBUG_Assert(0); - } - break; - default: - PIOS_DEBUG_Assert(0); - break; - } - break; - case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED: /* EV6 */ - case I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED: /* EV6 */ - switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { - case 0: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_0); - break; - case 1: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_1); - break; - case 2: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_2); - break; - default: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_GT_2); - break; - } - break; - case 0x80: /* TxE only. TRA + MSL + BUSY have been cleared before we got here. */ - /* Ignore */ - { - static volatile bool halt = FALSE; - while (halt) ; - } - break; - case 0: /* This triggers an FSM fault sometimes, but not having it stops things working */ - case 0x40: /* RxNE only. MSL + BUSY have already been cleared by HW. */ - case 0x44: /* RxNE + BTF. MSL + BUSY have already been cleared by HW. */ - case I2C_EVENT_MASTER_BYTE_RECEIVED: /* EV7 */ - case (I2C_EVENT_MASTER_BYTE_RECEIVED | 0x4): /* EV7 + BTF */ - case I2C_EVENT_MASTER_BYTE_TRANSMITTED: /* EV8_2 */ - case 0x84: /* TxE + BTF. EV8_2 but TRA + MSL + BUSY have already been cleared by HW. */ - switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { - case 0: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_0); - break; - case 1: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_1); - break; - case 2: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_2); - break; - default: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_GT_2); - break; - } - break; - case I2C_EVENT_MASTER_BYTE_TRANSMITTING: /* EV8 */ - /* Ignore this event and wait for TRANSMITTED in case we can't keep up */ - goto skip_event; - break; - case 0x30084: /* Occurs between byte tranmistted and master mode selected */ - case 0x30000: /* Need to throw away this spurious event */ - case 0x30403 & EVENT_MASK: /* Detected this after got a NACK, probably stop bit */ - goto skip_event; - break; - default: - i2c_adapter_log_fault(PIOS_I2C_ERROR_EVENT); + event &= EVENT_MASK; + + + switch (event) { /* Mask out all the bits we don't care about */ + case (I2C_EVENT_MASTER_MODE_SELECT | 0x40): + /* Unexplained event: EV5 + RxNE : Extraneous Rx. Probably a late NACK from previous read. */ + /* Clean up the extra Rx until the root cause is identified and just keep going */ + (void)I2C_ReceiveData(i2c_adapter->cfg->regs); + /* Fall through */ + case I2C_EVENT_MASTER_MODE_SELECT: /* EV5 */ + switch (i2c_adapter->active_txn->rw) { + case PIOS_I2C_TXN_READ: + if (i2c_adapter->active_txn == i2c_adapter->last_txn) { + /* Final transaction */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_READ); + } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { + /* More transactions follow */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_READ); + } else { + PIOS_DEBUG_Assert(0); + } + break; + case PIOS_I2C_TXN_WRITE: + if (i2c_adapter->active_txn == i2c_adapter->last_txn) { + /* Final transaction */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_WRITE); + } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { + /* More transactions follow */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_WRITE); + } else { + PIOS_DEBUG_Assert(0); + } + break; + default: + PIOS_DEBUG_Assert(0); + break; + } + break; + case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED: /* EV6 */ + case I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED: /* EV6 */ + switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { + case 0: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_0); + break; + case 1: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_1); + break; + case 2: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_2); + break; + default: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_GT_2); + break; + } + break; + case 0x80: /* TxE only. TRA + MSL + BUSY have been cleared before we got here. */ + /* Ignore */ + { + static volatile bool halt = FALSE; + while (halt) { + ; + } + } + break; + case 0: /* This triggers an FSM fault sometimes, but not having it stops things working */ + case 0x40: /* RxNE only. MSL + BUSY have already been cleared by HW. */ + case 0x44: /* RxNE + BTF. MSL + BUSY have already been cleared by HW. */ + case I2C_EVENT_MASTER_BYTE_RECEIVED: /* EV7 */ + case (I2C_EVENT_MASTER_BYTE_RECEIVED | 0x4): /* EV7 + BTF */ + case I2C_EVENT_MASTER_BYTE_TRANSMITTED: /* EV8_2 */ + case 0x84: /* TxE + BTF. EV8_2 but TRA + MSL + BUSY have already been cleared by HW. */ + switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { + case 0: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_0); + break; + case 1: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_1); + break; + case 2: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_2); + break; + default: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_GT_2); + break; + } + break; + case I2C_EVENT_MASTER_BYTE_TRANSMITTING: /* EV8 */ + /* Ignore this event and wait for TRANSMITTED in case we can't keep up */ + goto skip_event; + break; + case 0x30084: /* Occurs between byte tranmistted and master mode selected */ + case 0x30000: /* Need to throw away this spurious event */ + case 0x30403 & EVENT_MASK: /* Detected this after got a NACK, probably stop bit */ + goto skip_event; + break; + default: + i2c_adapter_log_fault(PIOS_I2C_ERROR_EVENT); #if defined(I2C_HALT_ON_ERRORS) - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); #endif - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); - break; - } + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); + break; + } skip_event: - ; + ; } void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id) { - struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_Assert(valid) + bool valid = PIOS_I2C_validate(i2c_adapter); + + PIOS_Assert(valid) #if defined(PIOS_I2C_DIAGNOSTICS) - uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + + i2c_erirq_history[i2c_erirq_history_pointer] = event; + i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % 5; - i2c_erirq_history[i2c_erirq_history_pointer] = event; - i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % 5; - #endif - if(event & I2C_FLAG_AF) { - i2c_nack_counter++; + if (event & I2C_FLAG_AF) { + i2c_nack_counter++; - I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_AF); + I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_AF); - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_NACK); - } else { /* Mostly bus errors here */ - i2c_adapter_log_fault(PIOS_I2C_ERROR_INTERRUPT); - - /* Fail hard on any errors for now */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); - } + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_NACK); + } else { /* Mostly bus errors here */ + i2c_adapter_log_fault(PIOS_I2C_ERROR_INTERRUPT); + + /* Fail hard on any errors for now */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); + } } #endif /* PIOS_INCLUDE_I2C */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_irq.c b/flight/pios/stm32f10x/pios_irq.c index 77b3a3b7a..3a2a79b5d 100644 --- a/flight/pios/stm32f10x/pios_irq.c +++ b/flight/pios/stm32f10x/pios_irq.c @@ -6,26 +6,26 @@ * @brief STM32 Hardware code to enable and disable interrupts * @{ * - * @file pios_irq.c + * @file pios_irq.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief IRQ Enable/Disable routines * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,55 +43,55 @@ static uint32_t nested_ctr; static uint32_t prev_primask; /** -* Disables all interrupts (nested) -* \return < 0 On errors -*/ + * Disables all interrupts (nested) + * \return < 0 On errors + */ int32_t PIOS_IRQ_Disable(void) { - /* Get current priority if nested level == 0 */ - if (!nested_ctr) { - __asm volatile (" mrs %0, primask\n":"=r" (prev_primask) - ); - } + /* Get current priority if nested level == 0 */ + if (!nested_ctr) { + __asm volatile (" mrs %0, primask\n" : "=r" (prev_primask) + ); + } - /* Disable interrupts */ - __asm volatile (" mov r0, #1 \n" " msr primask, r0\n":::"r0"); + /* Disable interrupts */ + __asm volatile (" mov r0, #1 \n" " msr primask, r0\n" ::: "r0"); - ++nested_ctr; + ++nested_ctr; - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Enables all interrupts (nested) -* \return < 0 on errors -* \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) -*/ + * Enables all interrupts (nested) + * \return < 0 on errors + * \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) + */ int32_t PIOS_IRQ_Enable(void) { - /* Check for nesting error */ - if (nested_ctr == 0) { - /* Nesting error */ - return -1; - } + /* Check for nesting error */ + if (nested_ctr == 0) { + /* Nesting error */ + return -1; + } - /* Decrease nesting level */ - --nested_ctr; + /* Decrease nesting level */ + --nested_ctr; - /* Set back previous priority once nested level reached 0 again */ - if (nested_ctr == 0) { - __asm volatile (" msr primask, %0\n"::"r" (prev_primask) - ); - } + /* Set back previous priority once nested level reached 0 again */ + if (nested_ctr == 0) { + __asm volatile (" msr primask, %0\n" ::"r" (prev_primask) + ); + } - /* No error */ - return 0; + /* No error */ + return 0; } #endif /* PIOS_INCLUDE_IRQ */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_led.c b/flight/pios/stm32f10x/pios_led.c index c9d26da8a..696544189 100644 --- a/flight/pios/stm32f10x/pios_led.c +++ b/flight/pios/stm32f10x/pios_led.c @@ -6,25 +6,25 @@ * @brief STM32 Hardware LED handling code * @{ * - * @file pios_led.c + * @file pios_led.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief LED functions, init, toggle, on & off. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,122 +34,126 @@ #include -static const struct pios_led_cfg * led_cfg; +static const struct pios_led_cfg *led_cfg; /** -* Initialises all the LED's -*/ -int32_t PIOS_LED_Init(const struct pios_led_cfg * cfg) + * Initialises all the LED's + */ +int32_t PIOS_LED_Init(const struct pios_led_cfg *cfg) { - PIOS_Assert(cfg); + PIOS_Assert(cfg); - /* Store away the config in a global used by API functions */ - led_cfg = cfg; + /* Store away the config in a global used by API functions */ + led_cfg = cfg; - for (uint8_t i = 0; i < cfg->num_leds; i++) { - const struct pios_led * led = &(cfg->leds[i]); + for (uint8_t i = 0; i < cfg->num_leds; i++) { + const struct pios_led *led = &(cfg->leds[i]); - /* Enable the peripheral clock for the GPIO */ - switch ((uint32_t)led->pin.gpio) { - case (uint32_t) GPIOA: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); - break; - case (uint32_t) GPIOB: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - break; - case (uint32_t) GPIOC: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); - break; - default: - PIOS_Assert(0); - break; - } + /* Enable the peripheral clock for the GPIO */ + switch ((uint32_t)led->pin.gpio) { + case (uint32_t)GPIOA: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); + break; + case (uint32_t)GPIOB: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + break; + case (uint32_t)GPIOC: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); + break; + default: + PIOS_Assert(0); + break; + } - if (led->remap) { - GPIO_PinRemapConfig(led->remap, ENABLE); - } + if (led->remap) { + GPIO_PinRemapConfig(led->remap, ENABLE); + } - GPIO_Init(led->pin.gpio, &led->pin.init); + GPIO_Init(led->pin.gpio, &led->pin.init); - PIOS_LED_Off(i); - } + PIOS_LED_Off(i); + } - return 0; + return 0; } /** -* Turn on LED -* \param[in] LED LED id -*/ + * Turn on LED + * \param[in] LED LED id + */ void PIOS_LED_On(uint32_t led_id) { - PIOS_Assert(led_cfg); + PIOS_Assert(led_cfg); - if (led_id >= led_cfg->num_leds) { - /* LED index out of range */ - return; - } + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } - const struct pios_led * led = &(led_cfg->leds[led_id]); + const struct pios_led *led = &(led_cfg->leds[led_id]); - if (led->active_high) - GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); - else - GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + if (led->active_high) { + GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } else { + GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } } /** -* Turn off LED -* \param[in] LED LED id -*/ + * Turn off LED + * \param[in] LED LED id + */ void PIOS_LED_Off(uint32_t led_id) { - PIOS_Assert(led_cfg); + PIOS_Assert(led_cfg); - if (led_id >= led_cfg->num_leds) { - /* LED index out of range */ - return; - } + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } - const struct pios_led * led = &(led_cfg->leds[led_id]); + const struct pios_led *led = &(led_cfg->leds[led_id]); - if (led->active_high) - GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); - else - GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + if (led->active_high) { + GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } else { + GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } } /** -* Toggle LED on/off -* \param[in] LED LED id -*/ + * Toggle LED on/off + * \param[in] LED LED id + */ void PIOS_LED_Toggle(uint32_t led_id) { - PIOS_Assert(led_cfg); + PIOS_Assert(led_cfg); - if (led_id >= led_cfg->num_leds) { - /* LED index out of range */ - return; - } + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } - const struct pios_led * led = &(led_cfg->leds[led_id]); + const struct pios_led *led = &(led_cfg->leds[led_id]); - if (GPIO_ReadOutputDataBit(led->pin.gpio, led->pin.init.GPIO_Pin) == Bit_SET) { - if (led->active_high) - PIOS_LED_Off(led_id); - else - PIOS_LED_On(led_id); - } else { - if (led->active_high) - PIOS_LED_On(led_id); - else - PIOS_LED_Off(led_id); - } + if (GPIO_ReadOutputDataBit(led->pin.gpio, led->pin.init.GPIO_Pin) == Bit_SET) { + if (led->active_high) { + PIOS_LED_Off(led_id); + } else { + PIOS_LED_On(led_id); + } + } else { + if (led->active_high) { + PIOS_LED_On(led_id); + } else { + PIOS_LED_Off(led_id); + } + } } #endif /* PIOS_INCLUDE_LED */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_ppm.c b/flight/pios/stm32f10x/pios_ppm.c index 8933d9a46..39bb2618d 100644 --- a/flight/pios/stm32f10x/pios_ppm.c +++ b/flight/pios/stm32f10x/pios_ppm.c @@ -39,161 +39,164 @@ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_ppm_rcvr_driver = { - .read = PIOS_PPM_Get, + .read = PIOS_PPM_Get, }; -#define PIOS_PPM_IN_MIN_NUM_CHANNELS 4 -#define PIOS_PPM_IN_MAX_NUM_CHANNELS PIOS_PPM_NUM_INPUTS -#define PIOS_PPM_STABLE_CHANNEL_COUNT 25 // frames -#define PIOS_PPM_IN_MIN_SYNC_PULSE_US 3000 // microseconds -#define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds -#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds +#define PIOS_PPM_IN_MIN_NUM_CHANNELS 4 +#define PIOS_PPM_IN_MAX_NUM_CHANNELS PIOS_PPM_NUM_INPUTS +#define PIOS_PPM_STABLE_CHANNEL_COUNT 25 // frames +#define PIOS_PPM_IN_MIN_SYNC_PULSE_US 3000 // microseconds +#define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds +#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds /* Local Variables */ -//static TIM_ICInitTypeDef TIM_ICInitStructure; +// static TIM_ICInitTypeDef TIM_ICInitStructure; static void PIOS_PPM_Supervisor(uint32_t ppm_id); enum pios_ppm_dev_magic { - PIOS_PPM_DEV_MAGIC = 0xee014d8b, + PIOS_PPM_DEV_MAGIC = 0xee014d8b, }; struct pios_ppm_dev { - enum pios_ppm_dev_magic magic; - const struct pios_ppm_cfg * cfg; + enum pios_ppm_dev_magic magic; + const struct pios_ppm_cfg *cfg; - uint8_t PulseIndex; - uint32_t PreviousTime; - uint32_t CurrentTime; - uint32_t DeltaTime; - uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS]; - uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS]; - uint32_t LargeCounter; - int8_t NumChannels; - int8_t NumChannelsPrevFrame; - uint8_t NumChannelCounter; + uint8_t PulseIndex; + uint32_t PreviousTime; + uint32_t CurrentTime; + uint32_t DeltaTime; + uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS]; + uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS]; + uint32_t LargeCounter; + int8_t NumChannels; + int8_t NumChannelsPrevFrame; + uint8_t NumChannelCounter; - uint8_t supv_timer; - bool Tracking; - bool Fresh; + uint8_t supv_timer; + bool Tracking; + bool Fresh; }; -static bool PIOS_PPM_validate(struct pios_ppm_dev * ppm_dev) +static bool PIOS_PPM_validate(struct pios_ppm_dev *ppm_dev) { - return (ppm_dev->magic == PIOS_PPM_DEV_MAGIC); + return ppm_dev->magic == PIOS_PPM_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_ppm_dev * PIOS_PPM_alloc(void) +static struct pios_ppm_dev *PIOS_PPM_alloc(void) { - struct pios_ppm_dev * ppm_dev; + struct pios_ppm_dev *ppm_dev; - ppm_dev = (struct pios_ppm_dev *)pvPortMalloc(sizeof(*ppm_dev)); - if (!ppm_dev) return(NULL); + ppm_dev = (struct pios_ppm_dev *)pvPortMalloc(sizeof(*ppm_dev)); + if (!ppm_dev) { + return NULL; + } - ppm_dev->magic = PIOS_PPM_DEV_MAGIC; - return(ppm_dev); + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; + return ppm_dev; } #else static struct pios_ppm_dev pios_ppm_devs[PIOS_PPM_MAX_DEVS]; static uint8_t pios_ppm_num_devs; -static struct pios_ppm_dev * PIOS_PPM_alloc(void) +static struct pios_ppm_dev *PIOS_PPM_alloc(void) { - struct pios_ppm_dev * ppm_dev; + struct pios_ppm_dev *ppm_dev; - if (pios_ppm_num_devs >= PIOS_PPM_MAX_DEVS) { - return (NULL); - } + if (pios_ppm_num_devs >= PIOS_PPM_MAX_DEVS) { + return NULL; + } - ppm_dev = &pios_ppm_devs[pios_ppm_num_devs++]; - ppm_dev->magic = PIOS_PPM_DEV_MAGIC; + ppm_dev = &pios_ppm_devs[pios_ppm_num_devs++]; + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; - return (ppm_dev); + return ppm_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void PIOS_PPM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); -static void PIOS_PPM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PPM_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PPM_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); static const struct pios_tim_callbacks tim_callbacks = { - .overflow = PIOS_PPM_tim_overflow_cb, - .edge = PIOS_PPM_tim_edge_cb, + .overflow = PIOS_PPM_tim_overflow_cb, + .edge = PIOS_PPM_tim_edge_cb, }; -extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg) +extern int32_t PIOS_PPM_Init(uint32_t *ppm_id, const struct pios_ppm_cfg *cfg) { - PIOS_DEBUG_Assert(ppm_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(ppm_id); + PIOS_DEBUG_Assert(cfg); - struct pios_ppm_dev * ppm_dev; + struct pios_ppm_dev *ppm_dev; - ppm_dev = (struct pios_ppm_dev *) PIOS_PPM_alloc(); - if (!ppm_dev) goto out_fail; + ppm_dev = (struct pios_ppm_dev *)PIOS_PPM_alloc(); + if (!ppm_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - ppm_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + ppm_dev->cfg = cfg; - /* Set up the state variables */ - ppm_dev->PulseIndex = 0; - ppm_dev->PreviousTime = 0; - ppm_dev->CurrentTime = 0; - ppm_dev->DeltaTime = 0; - ppm_dev->LargeCounter = 0; - ppm_dev->NumChannels = -1; - ppm_dev->NumChannelsPrevFrame = -1; - ppm_dev->NumChannelCounter = 0; - ppm_dev->Tracking = FALSE; - ppm_dev->Fresh = FALSE; + /* Set up the state variables */ + ppm_dev->PulseIndex = 0; + ppm_dev->PreviousTime = 0; + ppm_dev->CurrentTime = 0; + ppm_dev->DeltaTime = 0; + ppm_dev->LargeCounter = 0; + ppm_dev->NumChannels = -1; + ppm_dev->NumChannelsPrevFrame = -1; + ppm_dev->NumChannelCounter = 0; + ppm_dev->Tracking = FALSE; + ppm_dev->Fresh = FALSE; - for (uint8_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { - /* Flush counter variables */ - ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + for (uint8_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + /* Flush counter variables */ + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } - } + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)ppm_dev)) { + return -1; + } - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)ppm_dev)) { - return -1; - } + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; - TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + /* Configure the channels to be in capture/compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; - /* Configure the channels to be in capture/compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel * chan = &cfg->channels[i]; + /* Configure timer for input capture */ + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); - /* Configure timer for input capture */ - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); + /* Enable the Capture Compare Interrupt Request */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE); + break; + } + } - /* Enable the Capture Compare Interrupt Request */ - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE); - break; - case TIM_Channel_2: - TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE); - break; - case TIM_Channel_3: - TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE); - break; - case TIM_Channel_4: - TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE); - break; - } - } + ppm_dev->supv_timer = 0; + if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) { + PIOS_DEBUG_Assert(0); + } - ppm_dev->supv_timer = 0; - if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) { - PIOS_DEBUG_Assert(0); - } + *ppm_id = (uint32_t)ppm_dev; - *ppm_id = (uint32_t)ppm_dev; - - return(0); + return 0; out_fail: - return(-1); + return -1; } /** @@ -205,157 +208,154 @@ out_fail: */ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)rcvr_id; + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)rcvr_id; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return PIOS_RCVR_INVALID; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return PIOS_RCVR_INVALID; + } - if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { - /* Channel out of range */ - return PIOS_RCVR_INVALID; - } - return ppm_dev->CaptureValue[channel]; + if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* Channel out of range */ + return PIOS_RCVR_INVALID; + } + return ppm_dev->CaptureValue[channel]; } -static void PIOS_PPM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id, - uint32_t context, - __attribute__((unused)) uint8_t channel, - uint16_t count) +static void PIOS_PPM_tim_overflow_cb(__attribute__((unused)) uint32_t tim_id, + uint32_t context, + __attribute__((unused)) uint8_t channel, + uint16_t count) { - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context; + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)context; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } - ppm_dev->LargeCounter += count; - - return; + ppm_dev->LargeCounter += count; } -static void PIOS_PPM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, - uint32_t context, - uint8_t chan_idx, - uint16_t count) +static void PIOS_PPM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, + uint32_t context, + uint8_t chan_idx, + uint16_t count) { - /* Recover our device context */ - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context; + /* Recover our device context */ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)context; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } - if (chan_idx >= ppm_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (chan_idx >= ppm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - /* Shift the last measurement out */ - ppm_dev->PreviousTime = ppm_dev->CurrentTime; + /* Shift the last measurement out */ + ppm_dev->PreviousTime = ppm_dev->CurrentTime; - /* Grab the new count */ - ppm_dev->CurrentTime = count; + /* Grab the new count */ + ppm_dev->CurrentTime = count; - /* Convert to 32-bit timer result */ - ppm_dev->CurrentTime += ppm_dev->LargeCounter; + /* Convert to 32-bit timer result */ + ppm_dev->CurrentTime += ppm_dev->LargeCounter; - /* Capture computation */ - ppm_dev->DeltaTime = ppm_dev->CurrentTime - ppm_dev->PreviousTime; + /* Capture computation */ + ppm_dev->DeltaTime = ppm_dev->CurrentTime - ppm_dev->PreviousTime; - ppm_dev->PreviousTime = ppm_dev->CurrentTime; + ppm_dev->PreviousTime = ppm_dev->CurrentTime; - /* Sync pulse detection */ - if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) { - if (ppm_dev->PulseIndex == ppm_dev->NumChannelsPrevFrame - && ppm_dev->PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS - && ppm_dev->PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS) - { - /* If we see n simultaneous frames of the same - number of channels we save it as our frame size */ - if (ppm_dev->NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT) - ppm_dev->NumChannelCounter++; - else - ppm_dev->NumChannels = ppm_dev->PulseIndex; - } else { - ppm_dev->NumChannelCounter = 0; - } + /* Sync pulse detection */ + if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) { + if (ppm_dev->PulseIndex == ppm_dev->NumChannelsPrevFrame + && ppm_dev->PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS + && ppm_dev->PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* If we see n simultaneous frames of the same + number of channels we save it as our frame size */ + if (ppm_dev->NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT) { + ppm_dev->NumChannelCounter++; + } else { + ppm_dev->NumChannels = ppm_dev->PulseIndex; + } + } else { + ppm_dev->NumChannelCounter = 0; + } - /* Check if the last frame was well formed */ - if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) { - /* The last frame was well formed */ - for (int32_t i = 0; i < ppm_dev->NumChannels; i++) { - ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i]; - } - for (uint32_t i = ppm_dev->NumChannels; - i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { - ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - } - } + /* Check if the last frame was well formed */ + if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) { + /* The last frame was well formed */ + for (int32_t i = 0; i < ppm_dev->NumChannels; i++) { + ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i]; + } + for (uint32_t i = ppm_dev->NumChannels; + i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } + } - ppm_dev->Fresh = TRUE; - ppm_dev->Tracking = TRUE; - ppm_dev->NumChannelsPrevFrame = ppm_dev->PulseIndex; - ppm_dev->PulseIndex = 0; + ppm_dev->Fresh = TRUE; + ppm_dev->Tracking = TRUE; + ppm_dev->NumChannelsPrevFrame = ppm_dev->PulseIndex; + ppm_dev->PulseIndex = 0; - /* We rely on the supervisor to set CaptureValue to invalid - if no valid frame is found otherwise we ride over it */ - - } else if (ppm_dev->Tracking) { - /* Valid pulse duration 0.75 to 2.5 ms*/ - if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US - && ppm_dev->DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US - && ppm_dev->PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) { - - ppm_dev->CaptureValueNewFrame[ppm_dev->PulseIndex] = ppm_dev->DeltaTime; - ppm_dev->PulseIndex++; - } else { - /* Not a valid pulse duration */ - ppm_dev->Tracking = FALSE; - for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { - ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; - } - } - } + /* We rely on the supervisor to set CaptureValue to invalid + if no valid frame is found otherwise we ride over it */ + } else if (ppm_dev->Tracking) { + /* Valid pulse duration 0.75 to 2.5 ms*/ + if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US + && ppm_dev->DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US + && ppm_dev->PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) { + ppm_dev->CaptureValueNewFrame[ppm_dev->PulseIndex] = ppm_dev->DeltaTime; + ppm_dev->PulseIndex++; + } else { + /* Not a valid pulse duration */ + ppm_dev->Tracking = FALSE; + for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } + } + } } -static void PIOS_PPM_Supervisor(uint32_t ppm_id) { - /* Recover our device context */ - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)ppm_id; +static void PIOS_PPM_Supervisor(uint32_t ppm_id) +{ + /* Recover our device context */ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)ppm_id; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } - /* - * RTC runs at 625Hz so divide down the base rate so - * that this loop runs at 25Hz. - */ - if(++(ppm_dev->supv_timer) < 25) { - return; - } - ppm_dev->supv_timer = 0; + /* + * RTC runs at 625Hz so divide down the base rate so + * that this loop runs at 25Hz. + */ + if (++(ppm_dev->supv_timer) < 25) { + return; + } + ppm_dev->supv_timer = 0; - if (!ppm_dev->Fresh) { - ppm_dev->Tracking = FALSE; + if (!ppm_dev->Fresh) { + ppm_dev->Tracking = FALSE; - for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { - ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; - } - } + for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } + } - ppm_dev->Fresh = FALSE; + ppm_dev->Fresh = FALSE; } #endif /* PIOS_INCLUDE_PPM */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_ppm_out.c b/flight/pios/stm32f10x/pios_ppm_out.c index 1352d38d2..d8e545079 100644 --- a/flight/pios/stm32f10x/pios_ppm_out.c +++ b/flight/pios/stm32f10x/pios_ppm_out.c @@ -34,30 +34,30 @@ #include "pios_ppm_out_priv.h" -#define PIOS_PPM_OUT_MAX_DEVS 1 -#define PIOS_PPM_OUT_MAX_CHANNELS 8 -#define PIOS_PPM_OUT_FRAME_PERIOD_US 22500 // microseconds -#define PIOS_PPM_OUT_HIGH_PULSE_US 400 // microseconds -#define PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US 1000 // microseconds -#define PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US 2000 // microseconds +#define PIOS_PPM_OUT_MAX_DEVS 1 +#define PIOS_PPM_OUT_MAX_CHANNELS 8 +#define PIOS_PPM_OUT_FRAME_PERIOD_US 22500 // microseconds +#define PIOS_PPM_OUT_HIGH_PULSE_US 400 // microseconds +#define PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US 1000 // microseconds +#define PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US 2000 // microseconds enum pios_ppm_out_dev_magic { - PIOS_PPM_OUT_DEV_MAGIC = 0x0e0210e2 + PIOS_PPM_OUT_DEV_MAGIC = 0x0e0210e2 }; struct pios_ppm_out_dev { - enum pios_ppm_out_dev_magic magic; - const struct pios_ppm_out_cfg * cfg; + enum pios_ppm_out_dev_magic magic; + const struct pios_ppm_out_cfg *cfg; - uint32_t TriggeringPeriod; - uint32_t ChannelSum; - uint8_t NumChannelCounter; - uint16_t ChannelValue[PIOS_PPM_OUT_MAX_CHANNELS]; + uint32_t TriggeringPeriod; + uint32_t ChannelSum; + uint8_t NumChannelCounter; + uint16_t ChannelValue[PIOS_PPM_OUT_MAX_CHANNELS]; - uint8_t SupvTimer; - bool Fresh; - bool Tracking; - bool Enabled; + uint8_t SupvTimer; + bool Fresh; + bool Tracking; + bool Enabled; }; static void PIOS_PPM_Out_Supervisor(uint32_t ppm_id); @@ -65,228 +65,242 @@ static void PIOS_PPM_Out_Enable_Disable(struct pios_ppm_out_dev *ppm_dev, bool e static bool PIOS_PPM_Out_validate(struct pios_ppm_out_dev *ppm_dev) { - return (ppm_dev->magic == PIOS_PPM_OUT_DEV_MAGIC); + return ppm_dev->magic == PIOS_PPM_OUT_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_ppm_out_dev * PIOS_PPM_OUT_alloc(void) +static struct pios_ppm_out_dev *PIOS_PPM_OUT_alloc(void) { - struct pios_ppm_out_dev * ppm_dev; + struct pios_ppm_out_dev *ppm_dev; - ppm_dev = (struct pios_ppm_out_dev *)pvPortMalloc(sizeof(*ppm_dev)); - if (!ppm_dev) return(NULL); + ppm_dev = (struct pios_ppm_out_dev *)pvPortMalloc(sizeof(*ppm_dev)); + if (!ppm_dev) { + return NULL; + } - ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC; - return(ppm_dev); + ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC; + return ppm_dev; } #else static struct pios_ppm_out_dev pios_ppm_out_devs[PIOS_PPM_OUT_MAX_DEVS]; static uint8_t pios_ppm_out_num_devs; -static struct pios_ppm_out_dev * PIOS_PPM_alloc(void) +static struct pios_ppm_out_dev *PIOS_PPM_alloc(void) { - struct pios_ppm_out_dev * ppm_dev; + struct pios_ppm_out_dev *ppm_dev; - if (pios_ppm_out_num_devs >= PIOS_PPM_OUT_MAX_DEVS) { - return (NULL); - } + if (pios_ppm_out_num_devs >= PIOS_PPM_OUT_MAX_DEVS) { + return NULL; + } - ppm_dev = &pios_ppm_out_devs[pios_ppm_out_num_devs++]; - ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC; + ppm_dev = &pios_ppm_out_devs[pios_ppm_out_num_devs++]; + ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC; - return (ppm_dev); + return ppm_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void PIOS_PPM_OUT_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count); +static void PIOS_PPM_OUT_tim_edge_cb(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count); static const struct pios_tim_callbacks tim_out_callbacks = { - .overflow = NULL, - .edge = PIOS_PPM_OUT_tim_edge_cb, + .overflow = NULL, + .edge = PIOS_PPM_OUT_tim_edge_cb, }; -int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg * cfg) +int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg *cfg) { - PIOS_DEBUG_Assert(ppm_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(ppm_id); + PIOS_DEBUG_Assert(cfg); - // Allocate the device structure - struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)PIOS_PPM_OUT_alloc(); - if (!ppm_dev) - return -1; - ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC; - *ppm_out_id = (uint32_t)ppm_dev; + // Allocate the device structure + struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)PIOS_PPM_OUT_alloc(); + if (!ppm_dev) { + return -1; + } + ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC; + *ppm_out_id = (uint32_t)ppm_dev; - // Bind the configuration to the device instance - ppm_dev->cfg = cfg; + // Bind the configuration to the device instance + ppm_dev->cfg = cfg; - // Set up the state variables - ppm_dev->TriggeringPeriod = PIOS_PPM_OUT_HIGH_PULSE_US; - ppm_dev->ChannelSum = 0; - ppm_dev->NumChannelCounter = 0; + // Set up the state variables + ppm_dev->TriggeringPeriod = PIOS_PPM_OUT_HIGH_PULSE_US; + ppm_dev->ChannelSum = 0; + ppm_dev->NumChannelCounter = 0; - // Flush counter variables - for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; ++i) - ppm_dev->ChannelValue[i] = 1000; + // Flush counter variables + for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; ++i) { + ppm_dev->ChannelValue[i] = 1000; + } - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channel, 1, &tim_out_callbacks, (uint32_t)ppm_dev)) - return -1; + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channel, 1, &tim_out_callbacks, (uint32_t)ppm_dev)) { + return -1; + } - // Configure the channels to be in output compare mode - const struct pios_tim_channel *chan = cfg->channel; + // Configure the channels to be in output compare mode + const struct pios_tim_channel *chan = cfg->channel; - /* Set up for output compare function */ - switch(chan->timer_chan) { - case TIM_Channel_1: - TIM_OC1Init(chan->timer, &cfg->tim_oc_init); - TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_2: - TIM_OC2Init(chan->timer, &cfg->tim_oc_init); - TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_3: - TIM_OC3Init(chan->timer, &cfg->tim_oc_init); - TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_4: - TIM_OC4Init(chan->timer, &cfg->tim_oc_init); - TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - } + /* Set up for output compare function */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_OC1Init(chan->timer, &cfg->tim_oc_init); + TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(chan->timer, &cfg->tim_oc_init); + TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(chan->timer, &cfg->tim_oc_init); + TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(chan->timer, &cfg->tim_oc_init); + TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + } - TIM_ARRPreloadConfig(chan->timer, ENABLE); - TIM_CtrlPWMOutputs(chan->timer, ENABLE); - TIM_Cmd(chan->timer, ENABLE); + TIM_ARRPreloadConfig(chan->timer, ENABLE); + TIM_CtrlPWMOutputs(chan->timer, ENABLE); + TIM_Cmd(chan->timer, ENABLE); - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1; - TIM_TimeBaseStructure.TIM_Period = ((1000000 / 100) - 1); - TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure); - PIOS_PPM_Out_Enable_Disable(ppm_dev, false); + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1; + TIM_TimeBaseStructure.TIM_Period = ((1000000 / 100) - 1); + TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure); + PIOS_PPM_Out_Enable_Disable(ppm_dev, false); - // Configure the supervisor - ppm_dev->SupvTimer = 0; - ppm_dev->Fresh = FALSE; - ppm_dev->Tracking = FALSE; - ppm_dev->Enabled = FALSE; - if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Out_Supervisor, (uint32_t)ppm_dev)) { - PIOS_DEBUG_Assert(0); - } + // Configure the supervisor + ppm_dev->SupvTimer = 0; + ppm_dev->Fresh = FALSE; + ppm_dev->Tracking = FALSE; + ppm_dev->Enabled = FALSE; + if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Out_Supervisor, (uint32_t)ppm_dev)) { + PIOS_DEBUG_Assert(0); + } - return 0; + return 0; } void PIOS_PPM_OUT_Set(uint32_t ppm_out_id, uint8_t servo, uint16_t position) { - struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)ppm_out_id; - if (!PIOS_PPM_Out_validate(ppm_dev) || (servo >= PIOS_PPM_OUT_MAX_CHANNELS)) - return; + struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)ppm_out_id; - // Don't allow positions that are out of range. - if (position < PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US) - position = PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US; - if (position > PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US) - position = PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US; + if (!PIOS_PPM_Out_validate(ppm_dev) || (servo >= PIOS_PPM_OUT_MAX_CHANNELS)) { + return; + } - // Update the supervisor tracking variables. - ppm_dev->Fresh = TRUE; + // Don't allow positions that are out of range. + if (position < PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US) { + position = PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US; + } + if (position > PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US) { + position = PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US; + } - // Reenable the TIM if it's been turned off. - if (!ppm_dev->Tracking) { - ppm_dev->Tracking = TRUE; - PIOS_PPM_Out_Enable_Disable(ppm_dev, true); - } - - // Update the position - ppm_dev->ChannelValue[servo] = position; + // Update the supervisor tracking variables. + ppm_dev->Fresh = TRUE; + + // Reenable the TIM if it's been turned off. + if (!ppm_dev->Tracking) { + ppm_dev->Tracking = TRUE; + PIOS_PPM_Out_Enable_Disable(ppm_dev, true); + } + + // Update the position + ppm_dev->ChannelValue[servo] = position; } -static void PIOS_PPM_OUT_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, - uint32_t context, - __attribute__((unused)) uint8_t chan_idx, - __attribute__((unused)) uint16_t count) +static void PIOS_PPM_OUT_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, + uint32_t context, + __attribute__((unused)) uint8_t chan_idx, + __attribute__((unused)) uint16_t count) { - struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)context; - if (!PIOS_PPM_Out_validate(ppm_dev)) - return; + struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)context; - // Just return if the device is disabled. - if (!ppm_dev->Enabled) { - return; + if (!PIOS_PPM_Out_validate(ppm_dev)) { + return; + } + + // Just return if the device is disabled. + if (!ppm_dev->Enabled) { + return; + } + + // Turn off the PPM stream if we are no longer receiving update + // Note: This must happen between frames. + if ((ppm_dev->NumChannelCounter == 0) && !ppm_dev->Tracking) { + // Flush counter variables + for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; ++i) { + ppm_dev->ChannelValue[i] = 1000; } + PIOS_PPM_Out_Enable_Disable(ppm_dev, false); + return; + } - // Turn off the PPM stream if we are no longer receiving update - // Note: This must happen between frames. - if ((ppm_dev->NumChannelCounter == 0) && !ppm_dev->Tracking) { - // Flush counter variables - for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; ++i) { - ppm_dev->ChannelValue[i] = 1000; - } - PIOS_PPM_Out_Enable_Disable(ppm_dev, false); - return; - } + // Finish out the frame if we reached the last channel. + uint32_t pulse_width; + if ((ppm_dev->NumChannelCounter >= PIOS_PPM_OUT_MAX_CHANNELS)) { + pulse_width = PIOS_PPM_OUT_FRAME_PERIOD_US - ppm_dev->ChannelSum; + ppm_dev->NumChannelCounter = 0; + ppm_dev->ChannelSum = 0; + } else { + ppm_dev->ChannelSum += (pulse_width = ppm_dev->ChannelValue[ppm_dev->NumChannelCounter++]); + } - // Finish out the frame if we reached the last channel. - uint32_t pulse_width; - if ((ppm_dev->NumChannelCounter >= PIOS_PPM_OUT_MAX_CHANNELS)) { - pulse_width = PIOS_PPM_OUT_FRAME_PERIOD_US - ppm_dev->ChannelSum; - ppm_dev->NumChannelCounter = 0; - ppm_dev->ChannelSum = 0; - } else - ppm_dev->ChannelSum += (pulse_width = ppm_dev->ChannelValue[ppm_dev->NumChannelCounter++]); - - // Initiate the pulse - TIM_SetAutoreload(ppm_dev->cfg->channel->timer, pulse_width - 1); - - return; + // Initiate the pulse + TIM_SetAutoreload(ppm_dev->cfg->channel->timer, pulse_width - 1); } static void PIOS_PPM_Out_Enable_Disable(struct pios_ppm_out_dev *ppm_dev, bool enable) { - const struct pios_tim_channel *chan = ppm_dev->cfg->channel; - uint32_t trig = enable ? ppm_dev->TriggeringPeriod : 0; - FunctionalState state = enable ? ENABLE : DISABLE; - ppm_dev->Enabled = enable; - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, state); - TIM_SetCompare1(chan->timer, trig); - break; - case TIM_Channel_2: - TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, state); - TIM_SetCompare2(chan->timer, trig); - break; - case TIM_Channel_3: - TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, state); - TIM_SetCompare3(chan->timer, trig); - break; - case TIM_Channel_4: - TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, state); - TIM_SetCompare4(chan->timer, trig); - break; - } + const struct pios_tim_channel *chan = ppm_dev->cfg->channel; + uint32_t trig = enable ? ppm_dev->TriggeringPeriod : 0; + FunctionalState state = enable ? ENABLE : DISABLE; + + ppm_dev->Enabled = enable; + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, state); + TIM_SetCompare1(chan->timer, trig); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, state); + TIM_SetCompare2(chan->timer, trig); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, state); + TIM_SetCompare3(chan->timer, trig); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, state); + TIM_SetCompare4(chan->timer, trig); + break; + } } -static void PIOS_PPM_Out_Supervisor(uint32_t ppm_out_id) { - struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)ppm_out_id; - if (!PIOS_PPM_Out_validate(ppm_dev)) - return; +static void PIOS_PPM_Out_Supervisor(uint32_t ppm_out_id) +{ + struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)ppm_out_id; - // RTC runs at 625Hz so divide down the base rate so that this loop runs at 12.5Hz. - if(++(ppm_dev->SupvTimer) < 50) { - return; - } - ppm_dev->SupvTimer = 0; + if (!PIOS_PPM_Out_validate(ppm_dev)) { + return; + } - // Go into failsafe the channel values haven't been refreshed since the last time through. - if (!ppm_dev->Fresh) { - ppm_dev->Tracking = FALSE; - } + // RTC runs at 625Hz so divide down the base rate so that this loop runs at 12.5Hz. + if (++(ppm_dev->SupvTimer) < 50) { + return; + } + ppm_dev->SupvTimer = 0; - // Set Fresh to false to test if channel values are being refreshed. - ppm_dev->Fresh = FALSE; + // Go into failsafe the channel values haven't been refreshed since the last time through. + if (!ppm_dev->Fresh) { + ppm_dev->Tracking = FALSE; + } + + // Set Fresh to false to test if channel values are being refreshed. + ppm_dev->Fresh = FALSE; } #endif /* PIOS_INCLUDE_PPM_OUT */ diff --git a/flight/pios/stm32f10x/pios_pwm.c b/flight/pios/stm32f10x/pios_pwm.c index d93d37ebc..a11a5f121 100644 --- a/flight/pios/stm32f10x/pios_pwm.c +++ b/flight/pios/stm32f10x/pios_pwm.c @@ -39,7 +39,7 @@ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_pwm_rcvr_driver = { - .read = PIOS_PWM_Get, + .read = PIOS_PWM_Get, }; /* Local Variables */ @@ -47,127 +47,130 @@ const struct pios_rcvr_driver pios_pwm_rcvr_driver = { static const uint32_t PWM_SUPERVISOR_TIMEOUT = 100000; enum pios_pwm_dev_magic { - PIOS_PWM_DEV_MAGIC = 0xab30293c, + PIOS_PWM_DEV_MAGIC = 0xab30293c, }; struct pios_pwm_dev { - enum pios_pwm_dev_magic magic; - const struct pios_pwm_cfg * cfg; + enum pios_pwm_dev_magic magic; + const struct pios_pwm_cfg *cfg; - uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; - uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; - uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; - uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; - uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; - uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; + uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; + uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; + uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; + uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; }; -static bool PIOS_PWM_validate(struct pios_pwm_dev * pwm_dev) +static bool PIOS_PWM_validate(struct pios_pwm_dev *pwm_dev) { - return (pwm_dev->magic == PIOS_PWM_DEV_MAGIC); + return pwm_dev->magic == PIOS_PWM_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_pwm_dev * PIOS_PWM_alloc(void) +static struct pios_pwm_dev *PIOS_PWM_alloc(void) { - struct pios_pwm_dev * pwm_dev; + struct pios_pwm_dev *pwm_dev; - pwm_dev = (struct pios_pwm_dev *)pvPortMalloc(sizeof(*pwm_dev)); - if (!pwm_dev) return(NULL); + pwm_dev = (struct pios_pwm_dev *)pvPortMalloc(sizeof(*pwm_dev)); + if (!pwm_dev) { + return NULL; + } - pwm_dev->magic = PIOS_PWM_DEV_MAGIC; - return(pwm_dev); + pwm_dev->magic = PIOS_PWM_DEV_MAGIC; + return pwm_dev; } #else static struct pios_pwm_dev pios_pwm_devs[PIOS_PWM_MAX_DEVS]; static uint8_t pios_pwm_num_devs; -static struct pios_pwm_dev * PIOS_PWM_alloc(void) +static struct pios_pwm_dev *PIOS_PWM_alloc(void) { - struct pios_pwm_dev * pwm_dev; + struct pios_pwm_dev *pwm_dev; - if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) { - return (NULL); - } + if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) { + return NULL; + } - pwm_dev = &pios_pwm_devs[pios_pwm_num_devs++]; - pwm_dev->magic = PIOS_PWM_DEV_MAGIC; + pwm_dev = &pios_pwm_devs[pios_pwm_num_devs++]; + pwm_dev->magic = PIOS_PWM_DEV_MAGIC; - return (pwm_dev); + return pwm_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void PIOS_PWM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); -static void PIOS_PWM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PWM_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PWM_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); static const struct pios_tim_callbacks tim_callbacks = { - .overflow = PIOS_PWM_tim_overflow_cb, - .edge = PIOS_PWM_tim_edge_cb, + .overflow = PIOS_PWM_tim_overflow_cb, + .edge = PIOS_PWM_tim_edge_cb, }; /** -* Initialises all the pins -*/ -int32_t PIOS_PWM_Init(uint32_t * pwm_id, const struct pios_pwm_cfg * cfg) + * Initialises all the pins + */ +int32_t PIOS_PWM_Init(uint32_t *pwm_id, const struct pios_pwm_cfg *cfg) { - PIOS_DEBUG_Assert(pwm_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(pwm_id); + PIOS_DEBUG_Assert(cfg); - struct pios_pwm_dev * pwm_dev; + struct pios_pwm_dev *pwm_dev; - pwm_dev = (struct pios_pwm_dev *) PIOS_PWM_alloc(); - if (!pwm_dev) goto out_fail; + pwm_dev = (struct pios_pwm_dev *)PIOS_PWM_alloc(); + if (!pwm_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - pwm_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + pwm_dev->cfg = cfg; - for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { - /* Flush counter variables */ - pwm_dev->CaptureState[i] = 0; - pwm_dev->RiseValue[i] = 0; - pwm_dev->FallValue[i] = 0; - pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - } + for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { + /* Flush counter variables */ + pwm_dev->CaptureState[i] = 0; + pwm_dev->RiseValue[i] = 0; + pwm_dev->FallValue[i] = 0; + pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)pwm_dev)) { - return -1; - } + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)pwm_dev)) { + return -1; + } - /* Configure the channels to be in capture/compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel * chan = &cfg->channels[i]; + /* Configure the channels to be in capture/compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; - /* Configure timer for input capture */ - TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - - /* Enable the Capture Compare Interrupt Request */ - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); - break; - case TIM_Channel_2: - TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); - break; - case TIM_Channel_3: - TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); - break; - case TIM_Channel_4: - TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); - break; - } + /* Configure timer for input capture */ + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); - // Need the update event for that timer to detect timeouts - TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); + /* Enable the Capture Compare Interrupt Request */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); + break; + } - } + // Need the update event for that timer to detect timeouts + TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); + } - *pwm_id = (uint32_t) pwm_dev; + *pwm_id = (uint32_t)pwm_dev; - return (0); + return 0; out_fail: - return (-1); + return -1; } /** @@ -179,106 +182,103 @@ out_fail: */ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)rcvr_id; + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)rcvr_id; - if (!PIOS_PWM_validate(pwm_dev)) { - /* Invalid device specified */ - return PIOS_RCVR_INVALID; - } + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return PIOS_RCVR_INVALID; + } - if (channel >= PIOS_PWM_NUM_INPUTS) { - /* Channel out of range */ - return PIOS_RCVR_INVALID; - } - return pwm_dev->CaptureValue[channel]; + if (channel >= PIOS_PWM_NUM_INPUTS) { + /* Channel out of range */ + return PIOS_RCVR_INVALID; + } + return pwm_dev->CaptureValue[channel]; } -static void PIOS_PWM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) +static void PIOS_PWM_tim_overflow_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) { - struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context; + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)context; - if (!PIOS_PWM_validate(pwm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return; + } - if (channel >= pwm_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (channel >= pwm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - pwm_dev->us_since_update[channel] += count; - if(pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { - pwm_dev->CaptureState[channel] = 0; - pwm_dev->RiseValue[channel] = 0; - pwm_dev->FallValue[channel] = 0; - pwm_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; - pwm_dev->us_since_update[channel] = 0; - } - - return; + pwm_dev->us_since_update[channel] += count; + if (pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { + pwm_dev->CaptureState[channel] = 0; + pwm_dev->RiseValue[channel] = 0; + pwm_dev->FallValue[channel] = 0; + pwm_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; + pwm_dev->us_since_update[channel] = 0; + } } -static void PIOS_PWM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) +static void PIOS_PWM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) { - /* Recover our device context */ - struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context; + /* Recover our device context */ + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)context; - if (!PIOS_PWM_validate(pwm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return; + } - if (chan_idx >= pwm_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (chan_idx >= pwm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - const struct pios_tim_channel * chan = &pwm_dev->cfg->channels[chan_idx]; + const struct pios_tim_channel *chan = &pwm_dev->cfg->channels[chan_idx]; - if (pwm_dev->CaptureState[chan_idx] == 0) { - pwm_dev->RiseValue[chan_idx] = count; - pwm_dev->us_since_update[chan_idx] = 0; - } else { - pwm_dev->FallValue[chan_idx] = count; - } - - // flip state machine and capture value here - /* Simple rise or fall state machine */ - TIM_ICInitTypeDef TIM_ICInitStructure = pwm_dev->cfg->tim_ic_init; - if (pwm_dev->CaptureState[chan_idx] == 0) { - /* Switch states */ - pwm_dev->CaptureState[chan_idx] = 1; + if (pwm_dev->CaptureState[chan_idx] == 0) { + pwm_dev->RiseValue[chan_idx] = count; + pwm_dev->us_since_update[chan_idx] = 0; + } else { + pwm_dev->FallValue[chan_idx] = count; + } - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - } else { - /* Capture computation */ - if (pwm_dev->FallValue[chan_idx] > pwm_dev->RiseValue[chan_idx]) { - pwm_dev->CaptureValue[chan_idx] = (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]); - } else { - pwm_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - pwm_dev->RiseValue[chan_idx]) + pwm_dev->FallValue[chan_idx]); - } + // flip state machine and capture value here + /* Simple rise or fall state machine */ + TIM_ICInitTypeDef TIM_ICInitStructure = pwm_dev->cfg->tim_ic_init; + if (pwm_dev->CaptureState[chan_idx] == 0) { + /* Switch states */ + pwm_dev->CaptureState[chan_idx] = 1; - /* Switch states */ - pwm_dev->CaptureState[chan_idx] = 0; + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } else { + /* Capture computation */ + if (pwm_dev->FallValue[chan_idx] > pwm_dev->RiseValue[chan_idx]) { + pwm_dev->CaptureValue[chan_idx] = (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]); + } else { + pwm_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - pwm_dev->RiseValue[chan_idx]) + pwm_dev->FallValue[chan_idx]); + } - /* Increase supervisor counter */ - pwm_dev->CapCounter[chan_idx]++; + /* Switch states */ + pwm_dev->CaptureState[chan_idx] = 0; - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - } - + /* Increase supervisor counter */ + pwm_dev->CapCounter[chan_idx]++; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } } #endif /* PIOS_INCLUDE_PWM */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_rtc.c b/flight/pios/stm32f10x/pios_rtc.c index 9d8f45b66..dbf89be6a 100644 --- a/flight/pios/stm32f10x/pios_rtc.c +++ b/flight/pios/stm32f10x/pios_rtc.c @@ -39,40 +39,40 @@ #endif struct rtc_callback_entry { - void (*fn)(uint32_t); - uint32_t data; + void (*fn)(uint32_t); + uint32_t data; }; #define PIOS_RTC_MAX_CALLBACKS 3 struct rtc_callback_entry rtc_callback_list[PIOS_RTC_MAX_CALLBACKS]; static uint8_t rtc_callback_next = 0; -void PIOS_RTC_Init(const struct pios_rtc_cfg * cfg) +void PIOS_RTC_Init(const struct pios_rtc_cfg *cfg) { - RCC_APB1PeriphClockCmd(RCC_APB1Periph_BKP | RCC_APB1Periph_PWR, - ENABLE); - PWR_BackupAccessCmd(ENABLE); - - RCC_RTCCLKConfig(cfg->clksrc); - RCC_RTCCLKCmd(ENABLE); - RTC_WaitForLastTask(); - RTC_WaitForSynchro(); - RTC_WaitForLastTask(); + RCC_APB1PeriphClockCmd(RCC_APB1Periph_BKP | RCC_APB1Periph_PWR, + ENABLE); + PWR_BackupAccessCmd(ENABLE); - /* Configure and enable the RTC Second interrupt */ - NVIC_Init(&cfg->irq.init); - RTC_ITConfig( RTC_IT_SEC, ENABLE ); - RTC_WaitForLastTask(); + RCC_RTCCLKConfig(cfg->clksrc); + RCC_RTCCLKCmd(ENABLE); + RTC_WaitForLastTask(); + RTC_WaitForSynchro(); + RTC_WaitForLastTask(); - RTC_SetPrescaler(cfg->prescaler); - RTC_WaitForLastTask(); - RTC_SetCounter(0); - RTC_WaitForLastTask(); + /* Configure and enable the RTC Second interrupt */ + NVIC_Init(&cfg->irq.init); + RTC_ITConfig(RTC_IT_SEC, ENABLE); + RTC_WaitForLastTask(); + + RTC_SetPrescaler(cfg->prescaler); + RTC_WaitForLastTask(); + RTC_SetCounter(0); + RTC_WaitForLastTask(); } uint32_t PIOS_RTC_Counter() { - return RTC_GetCounter(); + return RTC_GetCounter(); } /* FIXME: This shouldn't use hard-coded clock rates, dividers or prescalers. @@ -80,51 +80,51 @@ uint32_t PIOS_RTC_Counter() */ float PIOS_RTC_Rate() { - return (float) (8e6f / 128.0f) / (1 + PIOS_RTC_PRESCALER); + return (float)(8e6f / 128.0f) / (1 + PIOS_RTC_PRESCALER); } -float PIOS_RTC_MsPerTick() +float PIOS_RTC_MsPerTick() { - return 1000.0f / PIOS_RTC_Rate(); + return 1000.0f / PIOS_RTC_Rate(); } /* TODO: This needs a mutex around rtc_callbacks[] */ bool PIOS_RTC_RegisterTickCallback(void (*fn)(uint32_t id), uint32_t data) { - struct rtc_callback_entry * cb; - if (rtc_callback_next >= PIOS_RTC_MAX_CALLBACKS) { - return false; - } + struct rtc_callback_entry *cb; - cb = &rtc_callback_list[rtc_callback_next++]; + if (rtc_callback_next >= PIOS_RTC_MAX_CALLBACKS) { + return false; + } - cb->fn = fn; - cb->data = data; - return true; + cb = &rtc_callback_list[rtc_callback_next++]; + + cb->fn = fn; + cb->data = data; + return true; } -void PIOS_RTC_irq_handler (void) +void PIOS_RTC_irq_handler(void) { - if (RTC_GetITStatus(RTC_IT_SEC)) - { - /* Call all registered callbacks */ - for (uint8_t i = 0; i < rtc_callback_next; i++) { - struct rtc_callback_entry * cb = &rtc_callback_list[i]; - if (cb->fn) { - (cb->fn)(cb->data); - } - } + if (RTC_GetITStatus(RTC_IT_SEC)) { + /* Call all registered callbacks */ + for (uint8_t i = 0; i < rtc_callback_next; i++) { + struct rtc_callback_entry *cb = &rtc_callback_list[i]; + if (cb->fn) { + (cb->fn)(cb->data); + } + } - /* Wait until last write operation on RTC registers has finished */ - RTC_WaitForLastTask(); - /* Clear the RTC Second interrupt */ - RTC_ClearITPendingBit(RTC_IT_SEC); - } + /* Wait until last write operation on RTC registers has finished */ + RTC_WaitForLastTask(); + /* Clear the RTC Second interrupt */ + RTC_ClearITPendingBit(RTC_IT_SEC); + } } #endif /* PIOS_INCLUDE_RTC */ -/** +/** * @} * @} */ diff --git a/flight/pios/stm32f10x/pios_servo.c b/flight/pios/stm32f10x/pios_servo.c index 5ec4a3550..a0ea16916 100644 --- a/flight/pios/stm32f10x/pios_servo.c +++ b/flight/pios/stm32f10x/pios_servo.c @@ -37,115 +37,117 @@ /* Private Function Prototypes */ -static const struct pios_servo_cfg * servo_cfg; +static const struct pios_servo_cfg *servo_cfg; /** -* Initialise Servos -*/ -int32_t PIOS_Servo_Init(const struct pios_servo_cfg * cfg) + * Initialise Servos + */ +int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg) { - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) { - return -1; - } + uint32_t tim_id; - /* Store away the requested configuration */ - servo_cfg = cfg; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) { + return -1; + } - /* Configure the channels to be in output compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel * chan = &cfg->channels[i]; + /* Store away the requested configuration */ + servo_cfg = cfg; - /* Set up for output compare function */ - switch(chan->timer_chan) { - case TIM_Channel_1: - TIM_OC1Init(chan->timer, &cfg->tim_oc_init); - TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_2: - TIM_OC2Init(chan->timer, &cfg->tim_oc_init); - TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_3: - TIM_OC3Init(chan->timer, &cfg->tim_oc_init); - TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_4: - TIM_OC4Init(chan->timer, &cfg->tim_oc_init); - TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - } + /* Configure the channels to be in output compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; - TIM_ARRPreloadConfig(chan->timer, ENABLE); - TIM_CtrlPWMOutputs(chan->timer, ENABLE); - TIM_Cmd(chan->timer, ENABLE); - } + /* Set up for output compare function */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_OC1Init(chan->timer, &cfg->tim_oc_init); + TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(chan->timer, &cfg->tim_oc_init); + TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(chan->timer, &cfg->tim_oc_init); + TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(chan->timer, &cfg->tim_oc_init); + TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + } - return 0; + TIM_ARRPreloadConfig(chan->timer, ENABLE); + TIM_CtrlPWMOutputs(chan->timer, ENABLE); + TIM_Cmd(chan->timer, ENABLE); + } + + return 0; } /** -* Set the servo update rate (Max 500Hz) -* \param[in] array of rates in Hz -* \param[in] maximum number of banks -*/ -void PIOS_Servo_SetHz(const uint16_t * speeds, uint8_t banks) + * Set the servo update rate (Max 500Hz) + * \param[in] array of rates in Hz + * \param[in] maximum number of banks + */ +void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks) { - if (!servo_cfg) { - return; - } + if (!servo_cfg) { + return; + } - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init; - TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1; + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init; + TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1; - uint8_t set = 0; + uint8_t set = 0; - for(uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) { - bool new = true; - const struct pios_tim_channel * chan = &servo_cfg->channels[i]; + for (uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) { + bool new = true; + const struct pios_tim_channel *chan = &servo_cfg->channels[i]; - /* See if any previous channels use that same timer */ - for(uint8_t j = 0; (j < i) && new; j++) - new &= chan->timer != servo_cfg->channels[j].timer; + /* See if any previous channels use that same timer */ + for (uint8_t j = 0; (j < i) && new; j++) { + new &= chan->timer != servo_cfg->channels[j].timer; + } - if(new) { - TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1); - TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure); - set++; - } - } + if (new) { + TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1); + TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure); + set++; + } + } } /** -* Set servo position -* \param[in] Servo Servo number (0-7) -* \param[in] Position Servo position in microseconds -*/ + * Set servo position + * \param[in] Servo Servo number (0-7) + * \param[in] Position Servo position in microseconds + */ void PIOS_Servo_Set(uint8_t servo, uint16_t position) { - /* Make sure servo exists */ - if (!servo_cfg || servo >= servo_cfg->num_channels) { - return; - } + /* Make sure servo exists */ + if (!servo_cfg || servo >= servo_cfg->num_channels) { + return; + } - /* Update the position */ - const struct pios_tim_channel * chan = &servo_cfg->channels[servo]; - switch(chan->timer_chan) { - case TIM_Channel_1: - TIM_SetCompare1(chan->timer, position); - break; - case TIM_Channel_2: - TIM_SetCompare2(chan->timer, position); - break; - case TIM_Channel_3: - TIM_SetCompare3(chan->timer, position); - break; - case TIM_Channel_4: - TIM_SetCompare4(chan->timer, position); - break; - } + /* Update the position */ + const struct pios_tim_channel *chan = &servo_cfg->channels[servo]; + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_SetCompare1(chan->timer, position); + break; + case TIM_Channel_2: + TIM_SetCompare2(chan->timer, position); + break; + case TIM_Channel_3: + TIM_SetCompare3(chan->timer, position); + break; + case TIM_Channel_4: + TIM_SetCompare4(chan->timer, position); + break; + } } #endif /* PIOS_INCLUDE_SERVO */ diff --git a/flight/pios/stm32f10x/pios_spi.c b/flight/pios/stm32f10x/pios_spi.c index 6b8d2c88e..1c0582458 100644 --- a/flight/pios/stm32f10x/pios_spi.c +++ b/flight/pios/stm32f10x/pios_spi.c @@ -39,195 +39,198 @@ #include -static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev * com_dev) +static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev *com_dev) { - /* Should check device magic here */ - return(true); + /* Should check device magic here */ + return true; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_spi_dev * PIOS_SPI_alloc(void) +static struct pios_spi_dev *PIOS_SPI_alloc(void) { - return (pvPortMalloc(sizeof(struct pios_spi_dev))); + return pvPortMalloc(sizeof(struct pios_spi_dev)); } #else static struct pios_spi_dev pios_spi_devs[PIOS_SPI_MAX_DEVS]; static uint8_t pios_spi_num_devs; -static struct pios_spi_dev * PIOS_SPI_alloc(void) +static struct pios_spi_dev *PIOS_SPI_alloc(void) { - if (pios_spi_num_devs >= PIOS_SPI_MAX_DEVS) { - return (NULL); - } + if (pios_spi_num_devs >= PIOS_SPI_MAX_DEVS) { + return NULL; + } - return (&pios_spi_devs[pios_spi_num_devs++]); + return &pios_spi_devs[pios_spi_num_devs++]; } #endif /** -* Initialises SPI pins -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed + * Initialises SPI pins + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed */ -int32_t PIOS_SPI_Init(uint32_t * spi_id, const struct pios_spi_cfg * cfg) +int32_t PIOS_SPI_Init(uint32_t *spi_id, const struct pios_spi_cfg *cfg) { - uint32_t init_ssel = 0; + uint32_t init_ssel = 0; + + PIOS_Assert(spi_id); + PIOS_Assert(cfg); + + struct pios_spi_dev *spi_dev; + + spi_dev = (struct pios_spi_dev *)PIOS_SPI_alloc(); + if (!spi_dev) { + goto out_fail; + } + + /* Bind the configuration to the device instance */ + spi_dev->cfg = cfg; - PIOS_Assert(spi_id); - PIOS_Assert(cfg); - - struct pios_spi_dev * spi_dev; - - spi_dev = (struct pios_spi_dev *) PIOS_SPI_alloc(); - if (!spi_dev) goto out_fail; - - /* Bind the configuration to the device instance */ - spi_dev->cfg = cfg; - #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(spi_dev->busy); - xSemaphoreGive(spi_dev->busy); + vSemaphoreCreateBinary(spi_dev->busy); + xSemaphoreGive(spi_dev->busy); #else - spi_dev->busy = 0; + spi_dev->busy = 0; #endif - - /* Disable callback function */ - spi_dev->callback = NULL; - - /* Set rx/tx dummy bytes to a known value */ - spi_dev->rx_dummy_byte = 0xFF; - spi_dev->tx_dummy_byte = 0xFF; - - switch (spi_dev->cfg->init.SPI_NSS) { - case SPI_NSS_Soft: - if (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) { - /* We're a master in soft NSS mode, make sure we see NSS high at all times. */ - SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Set); - /* Init as many slave selects as the config advertises. */ - init_ssel = spi_dev->cfg->slave_count; - } else { - /* We're a slave in soft NSS mode, make sure we see NSS low at all times. */ - SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Reset); - } - break; - case SPI_NSS_Hard: - /* only legal for single-slave config */ - PIOS_Assert(spi_dev->cfg->slave_count == 1); - init_ssel = 1; - /* FIXME: Should this also call SPI_SSOutputCmd()? */ - break; - - default: - PIOS_Assert(0); - } - - /* Initialize the GPIO pins */ - GPIO_Init(spi_dev->cfg->sclk.gpio, &(spi_dev->cfg->sclk.init)); - GPIO_Init(spi_dev->cfg->mosi.gpio, &(spi_dev->cfg->mosi.init)); - GPIO_Init(spi_dev->cfg->miso.gpio, &(spi_dev->cfg->miso.init)); - for (uint32_t i = 0; i < init_ssel; i++) { - /* Since we're driving the SSEL pin in software, ensure that the slave is deselected */ - /* XXX multi-slave support - maybe have another SPI_NSS_ mode? */ - GPIO_SetBits(spi_dev->cfg->ssel[i].gpio, spi_dev->cfg->ssel[i].init.GPIO_Pin); - GPIO_Init(spi_dev->cfg->ssel[i].gpio, (GPIO_InitTypeDef*)&(spi_dev->cfg->ssel[i].init)); - } - /* Enable the associated peripheral clock */ - switch ((uint32_t) spi_dev->cfg->regs) { - case (uint32_t) SPI1: - /* Enable SPI peripheral clock (APB2 == high speed) */ - RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); - break; - case (uint32_t) SPI2: - /* Enable SPI peripheral clock (APB1 == slow speed) */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE); - break; - case (uint32_t) SPI3: - /* Enable SPI peripheral clock (APB1 == slow speed) */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI3, ENABLE); - break; - } - - /* Enable DMA clock */ - RCC_AHBPeriphClockCmd(spi_dev->cfg->dma.ahb_clk, ENABLE); - - /* Configure DMA for SPI Rx */ - DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); - DMA_Init(spi_dev->cfg->dma.rx.channel, &(spi_dev->cfg->dma.rx.init)); - - /* Configure DMA for SPI Tx */ - DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); - DMA_Init(spi_dev->cfg->dma.tx.channel, &(spi_dev->cfg->dma.tx.init)); - - /* Initialize the SPI block */ - SPI_Init(spi_dev->cfg->regs, &(spi_dev->cfg->init)); - - /* Configure CRC calculation */ - if (spi_dev->cfg->use_crc) { - SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); - } else { - SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); - } - - /* Enable SPI */ - SPI_Cmd(spi_dev->cfg->regs, ENABLE); - - /* Enable SPI interrupts to DMA */ - SPI_I2S_DMACmd(spi_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); - - /* Configure DMA interrupt */ - NVIC_Init(&(spi_dev->cfg->dma.irq.init)); - - *spi_id = (uint32_t)spi_dev; - return(0); - + /* Disable callback function */ + spi_dev->callback = NULL; + + /* Set rx/tx dummy bytes to a known value */ + spi_dev->rx_dummy_byte = 0xFF; + spi_dev->tx_dummy_byte = 0xFF; + + switch (spi_dev->cfg->init.SPI_NSS) { + case SPI_NSS_Soft: + if (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) { + /* We're a master in soft NSS mode, make sure we see NSS high at all times. */ + SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Set); + /* Init as many slave selects as the config advertises. */ + init_ssel = spi_dev->cfg->slave_count; + } else { + /* We're a slave in soft NSS mode, make sure we see NSS low at all times. */ + SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Reset); + } + break; + case SPI_NSS_Hard: + /* only legal for single-slave config */ + PIOS_Assert(spi_dev->cfg->slave_count == 1); + init_ssel = 1; + /* FIXME: Should this also call SPI_SSOutputCmd()? */ + break; + + default: + PIOS_Assert(0); + } + + /* Initialize the GPIO pins */ + GPIO_Init(spi_dev->cfg->sclk.gpio, &(spi_dev->cfg->sclk.init)); + GPIO_Init(spi_dev->cfg->mosi.gpio, &(spi_dev->cfg->mosi.init)); + GPIO_Init(spi_dev->cfg->miso.gpio, &(spi_dev->cfg->miso.init)); + for (uint32_t i = 0; i < init_ssel; i++) { + /* Since we're driving the SSEL pin in software, ensure that the slave is deselected */ + /* XXX multi-slave support - maybe have another SPI_NSS_ mode? */ + GPIO_SetBits(spi_dev->cfg->ssel[i].gpio, spi_dev->cfg->ssel[i].init.GPIO_Pin); + GPIO_Init(spi_dev->cfg->ssel[i].gpio, (GPIO_InitTypeDef *)&(spi_dev->cfg->ssel[i].init)); + } + + /* Enable the associated peripheral clock */ + switch ((uint32_t)spi_dev->cfg->regs) { + case (uint32_t)SPI1: + /* Enable SPI peripheral clock (APB2 == high speed) */ + RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); + break; + case (uint32_t)SPI2: + /* Enable SPI peripheral clock (APB1 == slow speed) */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE); + break; + case (uint32_t)SPI3: + /* Enable SPI peripheral clock (APB1 == slow speed) */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI3, ENABLE); + break; + } + + /* Enable DMA clock */ + RCC_AHBPeriphClockCmd(spi_dev->cfg->dma.ahb_clk, ENABLE); + + /* Configure DMA for SPI Rx */ + DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); + DMA_Init(spi_dev->cfg->dma.rx.channel, &(spi_dev->cfg->dma.rx.init)); + + /* Configure DMA for SPI Tx */ + DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); + DMA_Init(spi_dev->cfg->dma.tx.channel, &(spi_dev->cfg->dma.tx.init)); + + /* Initialize the SPI block */ + SPI_Init(spi_dev->cfg->regs, &(spi_dev->cfg->init)); + + /* Configure CRC calculation */ + if (spi_dev->cfg->use_crc) { + SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); + } else { + SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); + } + + /* Enable SPI */ + SPI_Cmd(spi_dev->cfg->regs, ENABLE); + + /* Enable SPI interrupts to DMA */ + SPI_I2S_DMACmd(spi_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); + + /* Configure DMA interrupt */ + NVIC_Init(&(spi_dev->cfg->dma.irq.init)); + + *spi_id = (uint32_t)spi_dev; + return 0; + out_fail: - return(-1); + return -1; } /** -* (Re-)initialises SPI peripheral clock rate -* -* \param[in] spi SPI number (0 or 1) -* \param[in] spi_prescaler configures the SPI speed: -*
    -*
  • PIOS_SPI_PRESCALER_2: sets clock rate 27.7~ nS @ 72 MHz (36 MBit/s) (only supported for spi==0, spi1 uses 4 instead) -*
  • PIOS_SPI_PRESCALER_4: sets clock rate 55.5~ nS @ 72 MHz (18 MBit/s) -*
  • PIOS_SPI_PRESCALER_8: sets clock rate 111.1~ nS @ 72 MHz (9 MBit/s) -*
  • PIOS_SPI_PRESCALER_16: sets clock rate 222.2~ nS @ 72 MHz (4.5 MBit/s) -*
  • PIOS_SPI_PRESCALER_32: sets clock rate 444.4~ nS @ 72 MHz (2.25 MBit/s) -*
  • PIOS_SPI_PRESCALER_64: sets clock rate 888.8~ nS @ 72 MHz (1.125 MBit/s) -*
  • PIOS_SPI_PRESCALER_128: sets clock rate 1.7~ nS @ 72 MHz (0.562 MBit/s) -*
  • PIOS_SPI_PRESCALER_256: sets clock rate 3.5~ nS @ 72 MHz (0.281 MBit/s) -*
-* \return 0 if no error -* \return -1 if disabled SPI port selected -* \return -3 if invalid spi_prescaler selected -*/ + * (Re-)initialises SPI peripheral clock rate + * + * \param[in] spi SPI number (0 or 1) + * \param[in] spi_prescaler configures the SPI speed: + *
    + *
  • PIOS_SPI_PRESCALER_2: sets clock rate 27.7~ nS @ 72 MHz (36 MBit/s) (only supported for spi==0, spi1 uses 4 instead) + *
  • PIOS_SPI_PRESCALER_4: sets clock rate 55.5~ nS @ 72 MHz (18 MBit/s) + *
  • PIOS_SPI_PRESCALER_8: sets clock rate 111.1~ nS @ 72 MHz (9 MBit/s) + *
  • PIOS_SPI_PRESCALER_16: sets clock rate 222.2~ nS @ 72 MHz (4.5 MBit/s) + *
  • PIOS_SPI_PRESCALER_32: sets clock rate 444.4~ nS @ 72 MHz (2.25 MBit/s) + *
  • PIOS_SPI_PRESCALER_64: sets clock rate 888.8~ nS @ 72 MHz (1.125 MBit/s) + *
  • PIOS_SPI_PRESCALER_128: sets clock rate 1.7~ nS @ 72 MHz (0.562 MBit/s) + *
  • PIOS_SPI_PRESCALER_256: sets clock rate 3.5~ nS @ 72 MHz (0.281 MBit/s) + *
+ * \return 0 if no error + * \return -1 if disabled SPI port selected + * \return -3 if invalid spi_prescaler selected + */ int32_t PIOS_SPI_SetClockSpeed(uint32_t spi_id, SPIPrescalerTypeDef spi_prescaler) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - SPI_InitTypeDef SPI_InitStructure; + PIOS_Assert(valid) - if (spi_prescaler >= 8) { - /* Invalid prescaler selected */ - return -3; - } + SPI_InitTypeDef SPI_InitStructure; - /* Start with a copy of the default configuration for the peripheral */ - SPI_InitStructure = spi_dev->cfg->init; + if (spi_prescaler >= 8) { + /* Invalid prescaler selected */ + return -3; + } - /* Adjust the prescaler for the peripheral's clock */ - SPI_InitStructure.SPI_BaudRatePrescaler = ((uint16_t) spi_prescaler & 7) << 3; + /* Start with a copy of the default configuration for the peripheral */ + SPI_InitStructure = spi_dev->cfg->init; - /* Write back the new configuration */ - SPI_Init(spi_dev->cfg->regs, &SPI_InitStructure); + /* Adjust the prescaler for the peripheral's clock */ + SPI_InitStructure.SPI_BaudRatePrescaler = ((uint16_t)spi_prescaler & 7) << 3; - PIOS_SPI_TransferByte(spi_id, 0xFF); - return 0; + /* Write back the new configuration */ + SPI_Init(spi_dev->cfg->regs, &SPI_InitStructure); + + PIOS_SPI_TransferByte(spi_id, 0xFF); + return 0; } /** @@ -239,27 +242,32 @@ int32_t PIOS_SPI_SetClockSpeed(uint32_t spi_id, SPIPrescalerTypeDef spi_prescale int32_t PIOS_SPI_ClaimBus(uint32_t spi_id) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) - if (xSemaphoreTake(spi_dev->busy, 0xffff) != pdTRUE) - return -1; + if (xSemaphoreTake(spi_dev->busy, 0xffff) != pdTRUE) { + return -1; + } #else - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - uint32_t timeout = 0xffff; - while((PIOS_SPI_Busy(spi_id) || spi_dev->busy) && --timeout); - if(timeout == 0) //timed out - return -1; - - PIOS_IRQ_Disable(); - if(spi_dev->busy) - return -1; - spi_dev->busy = 1; - PIOS_IRQ_Enable(); -#endif - return 0; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + uint32_t timeout = 0xffff; + while ((PIOS_SPI_Busy(spi_id) || spi_dev->busy) && --timeout) { + ; + } + if (timeout == 0) { // timed out + return -1; + } + + PIOS_IRQ_Disable(); + if (spi_dev->busy) { + return -1; + } + spi_dev->busy = 1; + PIOS_IRQ_Enable(); +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + return 0; } /** @@ -273,24 +281,26 @@ int32_t PIOS_SPI_ClaimBus(uint32_t spi_id) int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) - - if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE){ - return -1; - } - if (woken) { - *woken = *woken || (higherPriorityTaskWoken == pdTRUE); - } - return 0; + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) + + if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE) { + return -1; + } + if (woken) { + *woken = *woken || (higherPriorityTaskWoken == pdTRUE); + } + return 0; + #else - if (woken) { - *woken = false; - } - return PIOS_SPI_ClaimBus(spi_id); + if (woken) { + *woken = false; + } + return PIOS_SPI_ClaimBus(spi_id); + #endif } @@ -302,19 +312,19 @@ int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken) int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) - xSemaphoreGive(spi_dev->busy); + xSemaphoreGive(spi_dev->busy); #else - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - PIOS_IRQ_Disable(); - spi_dev->busy = 0; - PIOS_IRQ_Enable(); + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + PIOS_IRQ_Disable(); + spi_dev->busy = 0; + PIOS_IRQ_Enable(); #endif - return 0; + return 0; } /** @@ -327,22 +337,24 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id) int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; bool valid = PIOS_SPI_validate(spi_dev); PIOS_Assert(valid) xSemaphoreGiveFromISR(spi_dev->busy, &higherPriorityTaskWoken); if (woken) { - *woken = *woken || (higherPriorityTaskWoken == pdTRUE); + *woken = *woken || (higherPriorityTaskWoken == pdTRUE); } - return 0; + return 0; + #else - if (woken) { - *woken = false; - } - return PIOS_SPI_ReleaseBus(spi_id); + if (woken) { + *woken = false; + } + return PIOS_SPI_ReleaseBus(spi_id); + #endif } @@ -355,272 +367,293 @@ int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken) */ int32_t PIOS_SPI_RC_PinSet(uint32_t spi_id, uint32_t slave_id, uint8_t pin_value) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) - PIOS_Assert(slave_id <= spi_dev->cfg->slave_count) - - /* XXX multi-slave support? */ - if (pin_value) { - GPIO_SetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); - } else { - GPIO_ResetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); - } - - return 0; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + + PIOS_Assert(valid) + PIOS_Assert(slave_id <= spi_dev->cfg->slave_count) + + /* XXX multi-slave support? */ + if (pin_value) { + GPIO_SetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); + } else { + GPIO_ResetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); + } + + return 0; } /** -* Transfers a byte to SPI output and reads back the return value from SPI input -* \param[in] spi SPI number (0 or 1) -* \param[in] b the byte which should be transfered -*/ + * Transfers a byte to SPI output and reads back the return value from SPI input + * \param[in] spi SPI number (0 or 1) + * \param[in] b the byte which should be transfered + */ static uint8_t dummy; int32_t PIOS_SPI_TransferByte(uint32_t spi_id, uint8_t b) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - uint8_t rx_byte; + PIOS_Assert(valid) - /* - * Procedure taken from STM32F10xxx Reference Manual section 23.3.5 - */ + uint8_t rx_byte; - /* Make sure the RXNE flag is cleared by reading the DR register */ - dummy = spi_dev->cfg->regs->DR; + /* + * Procedure taken from STM32F10xxx Reference Manual section 23.3.5 + */ - /* Start the transfer */ - spi_dev->cfg->regs->DR = b; + /* Make sure the RXNE flag is cleared by reading the DR register */ + dummy = spi_dev->cfg->regs->DR; - /* Wait until there is a byte to read */ - while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_RXNE)) ; + /* Start the transfer */ + spi_dev->cfg->regs->DR = b; - /* Read the rx'd byte */ - rx_byte = spi_dev->cfg->regs->DR; + /* Wait until there is a byte to read */ + while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_RXNE)) { + ; + } - /* Wait until the TXE goes high */ - while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) ; + /* Read the rx'd byte */ + rx_byte = spi_dev->cfg->regs->DR; - /* Wait for SPI transfer to have fully completed */ - while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) ; + /* Wait until the TXE goes high */ + while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) { + ; + } - /* Return received byte */ - return rx_byte; + /* Wait for SPI transfer to have fully completed */ + while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) { + ; + } + + /* Return received byte */ + return rx_byte; } /** -* Transfers a block of bytes via DMA. -* \param[in] spi SPI number (0 or 1) -* \param[in] send_buffer pointer to buffer which should be sent.
-* If NULL, 0xff (all-one) will be sent. -* \param[in] receive_buffer pointer to buffer which should get the received values.
-* If NULL, received bytes will be discarded. -* \param[in] len number of bytes which should be transfered -* \param[in] callback pointer to callback function which will be executed -* from DMA channel interrupt once the transfer is finished. -* If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will -* block until the transfer is finished. -* \return >= 0 if no error during transfer -* \return -1 if disabled SPI port selected -* \return -3 if function has been called during an ongoing DMA transfer -*/ + * Transfers a block of bytes via DMA. + * \param[in] spi SPI number (0 or 1) + * \param[in] send_buffer pointer to buffer which should be sent.
+ * If NULL, 0xff (all-one) will be sent. + * \param[in] receive_buffer pointer to buffer which should get the received values.
+ * If NULL, received bytes will be discarded. + * \param[in] len number of bytes which should be transfered + * \param[in] callback pointer to callback function which will be executed + * from DMA channel interrupt once the transfer is finished. + * If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will + * block until the transfer is finished. + * \return >= 0 if no error during transfer + * \return -1 if disabled SPI port selected + * \return -3 if function has been called during an ongoing DMA transfer + */ int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - DMA_InitTypeDef dma_init; + PIOS_Assert(valid) - /* Exit if ongoing transfer */ - if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { - return -3; - } + DMA_InitTypeDef dma_init; - /* Disable the SPI peripheral */ - SPI_Cmd(spi_dev->cfg->regs, DISABLE); + /* Exit if ongoing transfer */ + if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { + return -3; + } - /* Disable the DMA channels */ - DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); - DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); + /* Disable the SPI peripheral */ + SPI_Cmd(spi_dev->cfg->regs, DISABLE); - /* Set callback function */ - spi_dev->callback = callback; + /* Disable the DMA channels */ + DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); + DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); - /* - * Configure Rx channel - */ + /* Set callback function */ + spi_dev->callback = callback; - /* Start with the default configuration for this peripheral */ - dma_init = spi_dev->cfg->dma.rx.init; + /* + * Configure Rx channel + */ - if (receive_buffer != NULL) { - /* Enable memory addr. increment - bytes written into receive buffer */ - dma_init.DMA_MemoryBaseAddr = (uint32_t) receive_buffer; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - } else { - /* Disable memory addr. increment - bytes written into dummy buffer */ - spi_dev->rx_dummy_byte = 0xFF; - dma_init.DMA_MemoryBaseAddr = (uint32_t) &spi_dev->rx_dummy_byte; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; - } - if (spi_dev->cfg->use_crc) { - /* Make sure the CRC error flag is cleared before we start */ - SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); - } + /* Start with the default configuration for this peripheral */ + dma_init = spi_dev->cfg->dma.rx.init; - dma_init.DMA_BufferSize = len; - DMA_Init(spi_dev->cfg->dma.rx.channel, &(dma_init)); + if (receive_buffer != NULL) { + /* Enable memory addr. increment - bytes written into receive buffer */ + dma_init.DMA_MemoryBaseAddr = (uint32_t)receive_buffer; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + } else { + /* Disable memory addr. increment - bytes written into dummy buffer */ + spi_dev->rx_dummy_byte = 0xFF; + dma_init.DMA_MemoryBaseAddr = (uint32_t)&spi_dev->rx_dummy_byte; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; + } + if (spi_dev->cfg->use_crc) { + /* Make sure the CRC error flag is cleared before we start */ + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + } - /* - * Configure Tx channel - */ + dma_init.DMA_BufferSize = len; + DMA_Init(spi_dev->cfg->dma.rx.channel, &(dma_init)); - /* Start with the default configuration for this peripheral */ - dma_init = spi_dev->cfg->dma.tx.init; + /* + * Configure Tx channel + */ - if (send_buffer != NULL) { - /* Enable memory addr. increment - bytes written into receive buffer */ - dma_init.DMA_MemoryBaseAddr = (uint32_t) send_buffer; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - } else { - /* Disable memory addr. increment - bytes written into dummy buffer */ - spi_dev->tx_dummy_byte = 0xFF; - dma_init.DMA_MemoryBaseAddr = (uint32_t) &spi_dev->tx_dummy_byte; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; - } + /* Start with the default configuration for this peripheral */ + dma_init = spi_dev->cfg->dma.tx.init; - if (spi_dev->cfg->use_crc) { - /* The last byte of the payload will be replaced with the CRC8 */ - dma_init.DMA_BufferSize = len - 1; - } else { - dma_init.DMA_BufferSize = len; - } + if (send_buffer != NULL) { + /* Enable memory addr. increment - bytes written into receive buffer */ + dma_init.DMA_MemoryBaseAddr = (uint32_t)send_buffer; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + } else { + /* Disable memory addr. increment - bytes written into dummy buffer */ + spi_dev->tx_dummy_byte = 0xFF; + dma_init.DMA_MemoryBaseAddr = (uint32_t)&spi_dev->tx_dummy_byte; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; + } - DMA_Init(spi_dev->cfg->dma.tx.channel, &(dma_init)); + if (spi_dev->cfg->use_crc) { + /* The last byte of the payload will be replaced with the CRC8 */ + dma_init.DMA_BufferSize = len - 1; + } else { + dma_init.DMA_BufferSize = len; + } - /* Enable DMA interrupt if callback function active */ - DMA_ITConfig(spi_dev->cfg->dma.rx.channel, DMA_IT_TC, (callback != NULL) ? ENABLE : DISABLE); + DMA_Init(spi_dev->cfg->dma.tx.channel, &(dma_init)); - /* Flush out the CRC registers */ - SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); - (void)SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); - SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + /* Enable DMA interrupt if callback function active */ + DMA_ITConfig(spi_dev->cfg->dma.rx.channel, DMA_IT_TC, (callback != NULL) ? ENABLE : DISABLE); - /* Make sure to flush out the receive buffer */ - (void)SPI_I2S_ReceiveData(spi_dev->cfg->regs); + /* Flush out the CRC registers */ + SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); + (void)SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); - if (spi_dev->cfg->use_crc) { - /* Need a 0->1 transition to reset the CRC logic */ - SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); - } + /* Make sure to flush out the receive buffer */ + (void)SPI_I2S_ReceiveData(spi_dev->cfg->regs); - /* Start DMA transfers */ - DMA_Cmd(spi_dev->cfg->dma.rx.channel, ENABLE); - DMA_Cmd(spi_dev->cfg->dma.tx.channel, ENABLE); + if (spi_dev->cfg->use_crc) { + /* Need a 0->1 transition to reset the CRC logic */ + SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); + } - /* Reenable the SPI device */ - SPI_Cmd(spi_dev->cfg->regs, ENABLE); + /* Start DMA transfers */ + DMA_Cmd(spi_dev->cfg->dma.rx.channel, ENABLE); + DMA_Cmd(spi_dev->cfg->dma.tx.channel, ENABLE); - if (callback) { - /* User has requested a callback, don't wait for the transfer to complete. */ - return 0; - } + /* Reenable the SPI device */ + SPI_Cmd(spi_dev->cfg->regs, ENABLE); - /* Wait until all bytes have been transmitted/received */ - while (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)); + if (callback) { + /* User has requested a callback, don't wait for the transfer to complete. */ + return 0; + } - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))); + /* Wait until all bytes have been transmitted/received */ + while (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { + ; + } - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)); + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))) { + ; + } - /* Check the CRC on the transfer if enabled. */ - if (spi_dev->cfg->use_crc) { - /* Check the SPI CRC error flag */ - if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { - return -4; - } - } + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + ; + } - /* No error */ - return 0; + /* Check the CRC on the transfer if enabled. */ + if (spi_dev->cfg->use_crc) { + /* Check the SPI CRC error flag */ + if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { + return -4; + } + } + + /* No error */ + return 0; } /** -* Check if a transfer is in progress -* \param[in] spi SPI number (0 or 1) -* \return >= 0 if no transfer is in progress -* \return -1 if disabled SPI port selected -* \return -2 if unsupported SPI port selected -* \return -3 if function has been called during an ongoing DMA transfer -*/ + * Check if a transfer is in progress + * \param[in] spi SPI number (0 or 1) + * \return >= 0 if no transfer is in progress + * \return -1 if disabled SPI port selected + * \return -2 if unsupported SPI port selected + * \return -3 if function has been called during an ongoing DMA transfer + */ int32_t PIOS_SPI_Busy(uint32_t spi_id) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - /* DMA buffer has data or SPI transmit register not empty or SPI is busy*/ - if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel) || - !SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE) || - SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) - { - return -3; - } + PIOS_Assert(valid) - return(0); + /* DMA buffer has data or SPI transmit register not empty or SPI is busy*/ + if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel) || + !SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE) || + SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + return -3; + } + + return 0; } void PIOS_SPI_SetPrescalar(uint32_t spi_id, uint32_t prescaler) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid); - PIOS_Assert(IS_SPI_BAUDRATE_PRESCALER(prescaler)); - - spi_dev->cfg->regs->CR1 = (spi_dev->cfg->regs->CR1 & ~0x0038) | prescaler; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + + PIOS_Assert(valid); + PIOS_Assert(IS_SPI_BAUDRATE_PRESCALER(prescaler)); + + spi_dev->cfg->regs->CR1 = (spi_dev->cfg->regs->CR1 & ~0x0038) | prescaler; } void PIOS_SPI_IRQ_Handler(uint32_t spi_id) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - DMA_ClearFlag(spi_dev->cfg->dma.irq.flags); + PIOS_Assert(valid) - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))) ; + DMA_ClearFlag(spi_dev->cfg->dma.irq.flags); - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) ; + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))) { + ; + } - if (spi_dev->callback != NULL) { - bool crc_ok = TRUE; - uint8_t crc_val; + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + ; + } - if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { - crc_ok = FALSE; - SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); - } - crc_val = SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); - spi_dev->callback(crc_ok, crc_val); - } + if (spi_dev->callback != NULL) { + bool crc_ok = TRUE; + uint8_t crc_val; + + if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { + crc_ok = FALSE; + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + } + crc_val = SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); + spi_dev->callback(crc_ok, crc_val); + } } #endif /* PIOS_INCLUDE_SPI */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_sys.c b/flight/pios/stm32f10x/pios_sys.c index 9d52e34eb..63791fa22 100644 --- a/flight/pios/stm32f10x/pios_sys.c +++ b/flight/pios/stm32f10x/pios_sys.c @@ -6,26 +6,26 @@ * @brief PIOS System Initialization code * @{ * - * @file pios_sys.c + * @file pios_sys.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Sets up basic STM32 system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,185 +38,190 @@ void NVIC_Configuration(void); void SysTick_Handler(void); /* Local Macros */ -#define MEM8(addr) (*((volatile uint8_t *)(addr))) -#define MEM16(addr) (*((volatile uint16_t *)(addr))) -#define MEM32(addr) (*((volatile uint32_t *)(addr))) +#define MEM8(addr) (*((volatile uint8_t *)(addr))) +#define MEM16(addr) (*((volatile uint16_t *)(addr))) +#define MEM32(addr) (*((volatile uint32_t *)(addr))) /** -* Initialises all system peripherals -*/ + * Initialises all system peripherals + */ void PIOS_SYS_Init(void) { - /* Setup STM32 system (RCC, clock, PLL and Flash configuration) - CMSIS Function */ - SystemInit(); + /* Setup STM32 system (RCC, clock, PLL and Flash configuration) - CMSIS Function */ + SystemInit(); - /* Init the delay system */ - PIOS_DELAY_Init(); + /* Init the delay system */ + PIOS_DELAY_Init(); - /* Enable GPIOA, GPIOB, GPIOC, GPIOD, GPIOE and AFIO clocks */ - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD | RCC_APB2Periph_GPIOE | - RCC_APB2Periph_AFIO, ENABLE); + /* Enable GPIOA, GPIOB, GPIOC, GPIOD, GPIOE and AFIO clocks */ + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD | RCC_APB2Periph_GPIOE | + RCC_APB2Periph_AFIO, ENABLE); #if (PIOS_USB_ENABLED) - /* Ensure that pull-up is active on detect pin */ - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; - GPIO_InitStructure.GPIO_Pin = PIOS_USB_DETECT_GPIO_PIN; - GPIO_Init(PIOS_USB_DETECT_GPIO_PORT, &GPIO_InitStructure); + /* Ensure that pull-up is active on detect pin */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; + GPIO_InitStructure.GPIO_Pin = PIOS_USB_DETECT_GPIO_PIN; + GPIO_Init(PIOS_USB_DETECT_GPIO_PORT, &GPIO_InitStructure); #endif - /* Initialise Basic NVIC */ - NVIC_Configuration(); - + /* Initialise Basic NVIC */ + NVIC_Configuration(); } /** -* Shutdown PIOS and reset the microcontroller:
-*
    -*
  • Disable all RTOS tasks -*
  • Disable all interrupts -*
  • Turn off all board LEDs -*
  • Reset STM32 -*
-* \return < 0 if reset failed -*/ + * Shutdown PIOS and reset the microcontroller:
+ *
    + *
  • Disable all RTOS tasks + *
  • Disable all interrupts + *
  • Turn off all board LEDs + *
  • Reset STM32 + *
+ * \return < 0 if reset failed + */ int32_t PIOS_SYS_Reset(void) { - /* Disable all RTOS tasks */ + /* Disable all RTOS tasks */ #if defined(PIOS_INCLUDE_FREERTOS) - /* port specific FreeRTOS function to disable tasks (nested) */ - portENTER_CRITICAL(); + /* port specific FreeRTOS function to disable tasks (nested) */ + portENTER_CRITICAL(); #endif - // disable all interrupts - PIOS_IRQ_Disable(); + // disable all interrupts + PIOS_IRQ_Disable(); - // turn off all board LEDs + // turn off all board LEDs #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Off(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ + PIOS_LED_Off(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ #if defined(PIOS_LED_ALARM) - PIOS_LED_Off(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ + PIOS_LED_Off(PIOS_LED_ALARM); +#endif /* PIOS_LED_ALARM */ - RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - NVIC_SystemReset(); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + NVIC_SystemReset(); - while (1) ; + while (1) { + ; + } - /* We will never reach this point */ - return -1; + /* We will never reach this point */ + return -1; } /** -* Returns the CPU's flash size (in bytes) -*/ + * Returns the CPU's flash size (in bytes) + */ uint32_t PIOS_SYS_getCPUFlashSize(void) { - return ((uint32_t) MEM16(0x1FFFF7E0) * 1000); + return (uint32_t)MEM16(0x1FFFF7E0) * 1000; } /** -* Returns the serial number as a string -* param[out] uint8_t pointer to a string which can store at least 12 bytes -* (12 bytes returned for STM32) -* return < 0 if feature not supported -*/ + * Returns the serial number as a string + * param[out] uint8_t pointer to a string which can store at least 12 bytes + * (12 bytes returned for STM32) + * return < 0 if feature not supported + */ int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t *array) { - int i; + int i; - /* Stored in the so called "electronic signature" */ - for (i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) { - uint8_t b = MEM8(0x1ffff7e8 + i); + /* Stored in the so called "electronic signature" */ + for (i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) { + uint8_t b = MEM8(0x1ffff7e8 + i); - array[i] = b; - } + array[i] = b; + } - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Returns the serial number as a string -* param[out] str pointer to a string which can store at least 32 digits + zero terminator! -* (24 digits returned for STM32) -* return < 0 if feature not supported -*/ + * Returns the serial number as a string + * param[out] str pointer to a string which can store at least 32 digits + zero terminator! + * (24 digits returned for STM32) + * return < 0 if feature not supported + */ int32_t PIOS_SYS_SerialNumberGet(char *str) { - int i; + int i; - /* Stored in the so called "electronic signature" */ - for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { - uint8_t b = MEM8(0x1ffff7e8 + (i / 2)); - if (!(i & 1)) - b >>= 4; - b &= 0x0f; + /* Stored in the so called "electronic signature" */ + for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { + uint8_t b = MEM8(0x1ffff7e8 + (i / 2)); + if (!(i & 1)) { + b >>= 4; + } + b &= 0x0f; - str[i] = ((b > 9) ? ('A' - 10) : '0') + b; - } - str[i] = '\0'; + str[i] = ((b > 9) ? ('A' - 10) : '0') + b; + } + str[i] = '\0'; - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Configures Vector Table base location and SysTick -*/ + * Configures Vector Table base location and SysTick + */ void NVIC_Configuration(void) { - /* Set the Vector Table base address as specified in .ld file */ - extern void *pios_isr_vector_table_base; - NVIC_SetVectorTable((uint32_t)&pios_isr_vector_table_base, 0x0); + /* Set the Vector Table base address as specified in .ld file */ + extern void *pios_isr_vector_table_base; - /* 4 bits for Interrupt priorities so no sub priorities */ - NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + NVIC_SetVectorTable((uint32_t)&pios_isr_vector_table_base, 0x0); - /* Configure HCLK clock as SysTick clock source. */ - SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK); + /* 4 bits for Interrupt priorities so no sub priorities */ + NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + + /* Configure HCLK clock as SysTick clock source. */ + SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK); } #ifdef USE_FULL_ASSERT /** -* Reports the name of the source file and the source line number -* where the assert_param error has occurred. -* \param[in] file pointer to the source file name -* \param[in] line assert_param error line source number -* \retval None -*/ -void assert_failed(uint8_t * file, uint32_t line) + * Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * \param[in] file pointer to the source file name + * \param[in] line assert_param error line source number + * \retval None + */ +void assert_failed(uint8_t *file, uint32_t line) { - /* When serial debugging is implemented, use something like this. */ - /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ + /* When serial debugging is implemented, use something like this. */ + /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ - /* Setup the LEDs to Alternate */ + /* Setup the LEDs to Alternate */ #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_On(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ + PIOS_LED_On(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ #if defined(PIOS_LED_ALARM) - PIOS_LED_Off(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ + PIOS_LED_Off(PIOS_LED_ALARM); +#endif /* PIOS_LED_ALARM */ - /* Infinite loop */ - while (1) { + /* Infinite loop */ + while (1) { #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ #if defined(PIOS_LED_ALARM) - PIOS_LED_Toggle(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ - for (int i = 0; i < 1000000; i++) ; - } + PIOS_LED_Toggle(PIOS_LED_ALARM); +#endif /* PIOS_LED_ALARM */ + for (int i = 0; i < 1000000; i++) { + ; + } + } } -#endif +#endif /* ifdef USE_FULL_ASSERT */ #endif /* PIOS_INCLUDE_SYS */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_tim.c b/flight/pios/stm32f10x/pios_tim.c index 28a187341..a519240ee 100644 --- a/flight/pios/stm32f10x/pios_tim.c +++ b/flight/pios/stm32f10x/pios_tim.c @@ -5,425 +5,426 @@ #include "pios_tim_priv.h" enum pios_tim_dev_magic { - PIOS_TIM_DEV_MAGIC = 0x87654098, + PIOS_TIM_DEV_MAGIC = 0x87654098, }; struct pios_tim_dev { - enum pios_tim_dev_magic magic; + enum pios_tim_dev_magic magic; - const struct pios_tim_channel * channels; - uint8_t num_channels; + const struct pios_tim_channel *channels; + uint8_t num_channels; - const struct pios_tim_callbacks * callbacks; - uint32_t context; + const struct pios_tim_callbacks *callbacks; + uint32_t context; }; #if 0 -static bool PIOS_TIM_validate(struct pios_tim_dev * tim_dev) +static bool PIOS_TIM_validate(struct pios_tim_dev *tim_dev) { - return (tim_dev->magic == PIOS_TIM_DEV_MAGIC); + return tim_dev->magic == PIOS_TIM_DEV_MAGIC; } #endif #if defined(PIOS_INCLUDE_FREERTOS) && 0 -static struct pios_tim_dev * PIOS_TIM_alloc(void) +static struct pios_tim_dev *PIOS_TIM_alloc(void) { - struct pios_tim_dev * tim_dev; + struct pios_tim_dev *tim_dev; - tim_dev = (struct pios_tim_dev *)malloc(sizeof(*tim_dev)); - if (!tim_dev) return(NULL); + tim_dev = (struct pios_tim_dev *)malloc(sizeof(*tim_dev)); + if (!tim_dev) { + return NULL; + } - tim_dev->magic = PIOS_TIM_DEV_MAGIC; - return(tim_dev); + tim_dev->magic = PIOS_TIM_DEV_MAGIC; + return tim_dev; } #else static struct pios_tim_dev pios_tim_devs[PIOS_TIM_MAX_DEVS]; static uint8_t pios_tim_num_devs; -static struct pios_tim_dev * PIOS_TIM_alloc(void) +static struct pios_tim_dev *PIOS_TIM_alloc(void) { - struct pios_tim_dev * tim_dev; + struct pios_tim_dev *tim_dev; - if (pios_tim_num_devs >= PIOS_TIM_MAX_DEVS) { - return (NULL); - } + if (pios_tim_num_devs >= PIOS_TIM_MAX_DEVS) { + return NULL; + } - tim_dev = &pios_tim_devs[pios_tim_num_devs++]; - tim_dev->magic = PIOS_TIM_DEV_MAGIC; + tim_dev = &pios_tim_devs[pios_tim_num_devs++]; + tim_dev->magic = PIOS_TIM_DEV_MAGIC; - return (tim_dev); + return tim_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) && 0 */ - - -int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg * cfg) +int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg *cfg) { - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(cfg); - /* Enable appropriate clock to timer module */ - switch((uint32_t) cfg->timer) { - case (uint32_t)TIM1: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); - break; - case (uint32_t)TIM2: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); - break; - case (uint32_t)TIM3: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); - break; - case (uint32_t)TIM4: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); - break; + /* Enable appropriate clock to timer module */ + switch ((uint32_t)cfg->timer) { + case (uint32_t)TIM1: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); + break; + case (uint32_t)TIM2: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); + break; + case (uint32_t)TIM3: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); + break; + case (uint32_t)TIM4: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); + break; #ifdef STM32F10X_HD - case (uint32_t)TIM5: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); - break; - case (uint32_t)TIM6: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); - break; - case (uint32_t)TIM7: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); - break; - case (uint32_t)TIM8: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); - break; + case (uint32_t)TIM5: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); + break; + case (uint32_t)TIM6: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); + break; + case (uint32_t)TIM7: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); + break; + case (uint32_t)TIM8: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); + break; #endif - } + } - /* Configure the dividers for this timer */ - TIM_TimeBaseInit(cfg->timer, cfg->time_base_init); + /* Configure the dividers for this timer */ + TIM_TimeBaseInit(cfg->timer, cfg->time_base_init); - /* Configure internal timer clocks */ - TIM_InternalClockConfig(cfg->timer); + /* Configure internal timer clocks */ + TIM_InternalClockConfig(cfg->timer); - /* Enable timers */ - TIM_Cmd(cfg->timer, ENABLE); + /* Enable timers */ + TIM_Cmd(cfg->timer, ENABLE); - /* Enable Interrupts */ - NVIC_Init(&cfg->irq.init); + /* Enable Interrupts */ + NVIC_Init(&cfg->irq.init); - return 0; + return 0; } -int32_t PIOS_TIM_InitChannels(uint32_t * tim_id, const struct pios_tim_channel * channels, uint8_t num_channels, const struct pios_tim_callbacks * callbacks, uint32_t context) +int32_t PIOS_TIM_InitChannels(uint32_t *tim_id, const struct pios_tim_channel *channels, uint8_t num_channels, const struct pios_tim_callbacks *callbacks, uint32_t context) { - PIOS_Assert(channels); - PIOS_Assert(num_channels); + PIOS_Assert(channels); + PIOS_Assert(num_channels); - struct pios_tim_dev * tim_dev; - tim_dev = (struct pios_tim_dev *) PIOS_TIM_alloc(); - if (!tim_dev) goto out_fail; + struct pios_tim_dev *tim_dev; + tim_dev = (struct pios_tim_dev *)PIOS_TIM_alloc(); + if (!tim_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - tim_dev->channels = channels; - tim_dev->num_channels = num_channels; - tim_dev->callbacks = callbacks; - tim_dev->context = context; + /* Bind the configuration to the device instance */ + tim_dev->channels = channels; + tim_dev->num_channels = num_channels; + tim_dev->callbacks = callbacks; + tim_dev->context = context; - /* Configure the pins */ - for (uint8_t i = 0; i < num_channels; i++) { - const struct pios_tim_channel * chan = &(channels[i]); + /* Configure the pins */ + for (uint8_t i = 0; i < num_channels; i++) { + const struct pios_tim_channel *chan = &(channels[i]); - /* Enable the peripheral clock for the GPIO */ - switch ((uint32_t)chan->pin.gpio) { - case (uint32_t) GPIOA: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); - break; - case (uint32_t) GPIOB: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - break; - case (uint32_t) GPIOC: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); - break; - default: - PIOS_Assert(0); - break; - } - GPIO_Init(chan->pin.gpio, &chan->pin.init); + /* Enable the peripheral clock for the GPIO */ + switch ((uint32_t)chan->pin.gpio) { + case (uint32_t)GPIOA: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); + break; + case (uint32_t)GPIOB: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + break; + case (uint32_t)GPIOC: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); + break; + default: + PIOS_Assert(0); + break; + } + GPIO_Init(chan->pin.gpio, &chan->pin.init); - if (chan->remap) { - GPIO_PinRemapConfig(chan->remap, ENABLE); - } - } + if (chan->remap) { + GPIO_PinRemapConfig(chan->remap, ENABLE); + } + } - *tim_id = (uint32_t)tim_dev; + *tim_id = (uint32_t)tim_dev; - return(0); + return 0; out_fail: - return(-1); + return -1; } -static void PIOS_TIM_generic_irq_handler(TIM_TypeDef * timer) +static void PIOS_TIM_generic_irq_handler(TIM_TypeDef *timer) { - /* Iterate over all registered clients of the TIM layer to find channels on this timer */ - for (uint8_t i = 0; i < pios_tim_num_devs; i++) { - const struct pios_tim_dev * tim_dev = &pios_tim_devs[i]; + /* Iterate over all registered clients of the TIM layer to find channels on this timer */ + for (uint8_t i = 0; i < pios_tim_num_devs; i++) { + const struct pios_tim_dev *tim_dev = &pios_tim_devs[i]; - if (!tim_dev->channels || tim_dev->num_channels == 0) { - /* No channels to process on this client */ - continue; - } + if (!tim_dev->channels || tim_dev->num_channels == 0) { + /* No channels to process on this client */ + continue; + } - /* Check for an overflow event on this timer */ - bool overflow_event; - uint16_t overflow_count; - if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) { - TIM_ClearITPendingBit(timer, TIM_IT_Update); - overflow_count = timer->ARR; - overflow_event = true; - } else { - overflow_count = 0; - overflow_event = false; - } + /* Check for an overflow event on this timer */ + bool overflow_event; + uint16_t overflow_count; + if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) { + TIM_ClearITPendingBit(timer, TIM_IT_Update); + overflow_count = timer->ARR; + overflow_event = true; + } else { + overflow_count = 0; + overflow_event = false; + } - for (uint8_t j = 0; j < tim_dev->num_channels; j++) { - const struct pios_tim_channel * chan = &tim_dev->channels[j]; + for (uint8_t j = 0; j < tim_dev->num_channels; j++) { + const struct pios_tim_channel *chan = &tim_dev->channels[j]; - if (chan->timer != timer) { - /* channel is not on this timer */ - continue; - } + if (chan->timer != timer) { + /* channel is not on this timer */ + continue; + } - /* Figure out which interrupt bit we should be looking at */ - uint16_t timer_it; - switch (chan->timer_chan) { - case TIM_Channel_1: - timer_it = TIM_IT_CC1; - break; - case TIM_Channel_2: - timer_it = TIM_IT_CC2; - break; - case TIM_Channel_3: - timer_it = TIM_IT_CC3; - break; - case TIM_Channel_4: - timer_it = TIM_IT_CC4; - break; - default: - PIOS_Assert(0); - break; - } + /* Figure out which interrupt bit we should be looking at */ + uint16_t timer_it; + switch (chan->timer_chan) { + case TIM_Channel_1: + timer_it = TIM_IT_CC1; + break; + case TIM_Channel_2: + timer_it = TIM_IT_CC2; + break; + case TIM_Channel_3: + timer_it = TIM_IT_CC3; + break; + case TIM_Channel_4: + timer_it = TIM_IT_CC4; + break; + default: + PIOS_Assert(0); + break; + } - bool edge_event; - uint16_t edge_count; - if (TIM_GetITStatus(chan->timer, timer_it) == SET) { - TIM_ClearITPendingBit(chan->timer, timer_it); + bool edge_event; + uint16_t edge_count; + if (TIM_GetITStatus(chan->timer, timer_it) == SET) { + TIM_ClearITPendingBit(chan->timer, timer_it); - /* Read the current counter */ - switch(chan->timer_chan) { - case TIM_Channel_1: - edge_count = TIM_GetCapture1(chan->timer); - break; - case TIM_Channel_2: - edge_count = TIM_GetCapture2(chan->timer); - break; - case TIM_Channel_3: - edge_count = TIM_GetCapture3(chan->timer); - break; - case TIM_Channel_4: - edge_count = TIM_GetCapture4(chan->timer); - break; - default: - PIOS_Assert(0); - break; - } - edge_event = true; - } else { - edge_event = false; - edge_count = 0; - } + /* Read the current counter */ + switch (chan->timer_chan) { + case TIM_Channel_1: + edge_count = TIM_GetCapture1(chan->timer); + break; + case TIM_Channel_2: + edge_count = TIM_GetCapture2(chan->timer); + break; + case TIM_Channel_3: + edge_count = TIM_GetCapture3(chan->timer); + break; + case TIM_Channel_4: + edge_count = TIM_GetCapture4(chan->timer); + break; + default: + PIOS_Assert(0); + break; + } + edge_event = true; + } else { + edge_event = false; + edge_count = 0; + } - if (!tim_dev->callbacks) { - /* No callbacks registered, we're done with this channel */ - continue; - } + if (!tim_dev->callbacks) { + /* No callbacks registered, we're done with this channel */ + continue; + } - /* Generate the appropriate callbacks */ - if (overflow_event & edge_event) { - /* - * When both edge and overflow happen in the same interrupt, we - * need a heuristic to determine the order of the edge and overflow - * events so that the callbacks happen in the right order. If we - * get the order wrong, our pulse width calculations could be off by up - * to ARR ticks. That could be bad. - * - * Heuristic: If the edge_count is < 16 ticks above zero then we assume the - * edge happened just after the overflow. - */ + /* Generate the appropriate callbacks */ + if (overflow_event & edge_event) { + /* + * When both edge and overflow happen in the same interrupt, we + * need a heuristic to determine the order of the edge and overflow + * events so that the callbacks happen in the right order. If we + * get the order wrong, our pulse width calculations could be off by up + * to ARR ticks. That could be bad. + * + * Heuristic: If the edge_count is < 16 ticks above zero then we assume the + * edge happened just after the overflow. + */ - if (edge_count < 16) { - /* Call the overflow callback first */ - if (tim_dev->callbacks->overflow) { - (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, - tim_dev->context, - j, - overflow_count); - } - /* Call the edge callback second */ - if (tim_dev->callbacks->edge) { - (*tim_dev->callbacks->edge)((uint32_t)tim_dev, - tim_dev->context, - j, - edge_count); - } - } else { - /* Call the edge callback first */ - if (tim_dev->callbacks->edge) { - (*tim_dev->callbacks->edge)((uint32_t)tim_dev, - tim_dev->context, - j, - edge_count); - } - /* Call the overflow callback second */ - if (tim_dev->callbacks->overflow) { - (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, - tim_dev->context, - j, - overflow_count); - } - } - } else if (overflow_event && tim_dev->callbacks->overflow) { - (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, - tim_dev->context, - j, - overflow_count); - } else if (edge_event && tim_dev->callbacks->edge) { - (*tim_dev->callbacks->edge)((uint32_t)tim_dev, - tim_dev->context, - j, - edge_count); - } - } - } + if (edge_count < 16) { + /* Call the overflow callback first */ + if (tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } + /* Call the edge callback second */ + if (tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + } else { + /* Call the edge callback first */ + if (tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + /* Call the overflow callback second */ + if (tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } + } + } else if (overflow_event && tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } else if (edge_event && tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + } + } } #if 0 - uint16_t val = 0; - for(uint8_t i = 0; i < pios_pwm_cfg.num_channels; i++) { - struct pios_pwm_channel channel = pios_pwm_cfg.channels[i]; - if ((channel.timer == timer) && (TIM_GetITStatus(channel.timer, channel.ccr) == SET)) { - - TIM_ClearITPendingBit(channel.timer, channel.ccr); - - switch(channel.channel) { - case TIM_Channel_1: - val = TIM_GetCapture1(channel.timer); - break; - case TIM_Channel_2: - val = TIM_GetCapture2(channel.timer); - break; - case TIM_Channel_3: - val = TIM_GetCapture3(channel.timer); - break; - case TIM_Channel_4: - val = TIM_GetCapture4(channel.timer); - break; - } - - if (CaptureState[i] == 0) { - RiseValue[i] = val; - } else { - FallValue[i] = val; - } - - // flip state machine and capture value here - /* Simple rise or fall state machine */ - TIM_ICInitTypeDef TIM_ICInitStructure = pios_pwm_cfg.tim_ic_init; - if (CaptureState[i] == 0) { - /* Switch states */ - CaptureState[i] = 1; - - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; - TIM_ICInitStructure.TIM_Channel = channel.channel; - TIM_ICInit(channel.timer, &TIM_ICInitStructure); - } else { - /* Capture computation */ - if (FallValue[i] > RiseValue[i]) { - CaptureValue[i] = (FallValue[i] - RiseValue[i]); - } else { - CaptureValue[i] = ((channel.timer->ARR - RiseValue[i]) + FallValue[i]); - } - - /* Switch states */ - CaptureState[i] = 0; - - /* Increase supervisor counter */ - CapCounter[i]++; - - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; - TIM_ICInitStructure.TIM_Channel = channel.channel; - TIM_ICInit(channel.timer, &TIM_ICInitStructure); - } - } - } -#endif +uint16_t val = 0; +for (uint8_t i = 0; i < pios_pwm_cfg.num_channels; i++) { + struct pios_pwm_channel channel = pios_pwm_cfg.channels[i]; + if ((channel.timer == timer) && (TIM_GetITStatus(channel.timer, channel.ccr) == SET)) { + TIM_ClearITPendingBit(channel.timer, channel.ccr); + + switch (channel.channel) { + case TIM_Channel_1: + val = TIM_GetCapture1(channel.timer); + break; + case TIM_Channel_2: + val = TIM_GetCapture2(channel.timer); + break; + case TIM_Channel_3: + val = TIM_GetCapture3(channel.timer); + break; + case TIM_Channel_4: + val = TIM_GetCapture4(channel.timer); + break; + } + + if (CaptureState[i] == 0) { + RiseValue[i] = val; + } else { + FallValue[i] = val; + } + + // flip state machine and capture value here + /* Simple rise or fall state machine */ + TIM_ICInitTypeDef TIM_ICInitStructure = pios_pwm_cfg.tim_ic_init; + if (CaptureState[i] == 0) { + /* Switch states */ + CaptureState[i] = 1; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + TIM_ICInitStructure.TIM_Channel = channel.channel; + TIM_ICInit(channel.timer, &TIM_ICInitStructure); + } else { + /* Capture computation */ + if (FallValue[i] > RiseValue[i]) { + CaptureValue[i] = (FallValue[i] - RiseValue[i]); + } else { + CaptureValue[i] = ((channel.timer->ARR - RiseValue[i]) + FallValue[i]); + } + + /* Switch states */ + CaptureState[i] = 0; + + /* Increase supervisor counter */ + CapCounter[i]++; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_Channel = channel.channel; + TIM_ICInit(channel.timer, &TIM_ICInitStructure); + } + } +} +#endif /* if 0 */ /* Bind Interrupt Handlers * * Map all valid TIM IRQs to the common interrupt handler * and give it enough context to properly demux the various timers */ -void TIM1_UP_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_1_UP_irq_handler"))); -static void PIOS_TIM_1_UP_irq_handler (void) +void TIM1_UP_IRQHandler(void) __attribute__((alias("PIOS_TIM_1_UP_irq_handler"))); +static void PIOS_TIM_1_UP_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM1); + PIOS_TIM_generic_irq_handler(TIM1); } -void TIM1_CC_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_1_CC_irq_handler"))); -static void PIOS_TIM_1_CC_irq_handler (void) +void TIM1_CC_IRQHandler(void) __attribute__((alias("PIOS_TIM_1_CC_irq_handler"))); +static void PIOS_TIM_1_CC_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM1); + PIOS_TIM_generic_irq_handler(TIM1); } -void TIM2_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_2_irq_handler"))); -static void PIOS_TIM_2_irq_handler (void) +void TIM2_IRQHandler(void) __attribute__((alias("PIOS_TIM_2_irq_handler"))); +static void PIOS_TIM_2_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM2); + PIOS_TIM_generic_irq_handler(TIM2); } -void TIM3_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_3_irq_handler"))); -static void PIOS_TIM_3_irq_handler (void) +void TIM3_IRQHandler(void) __attribute__((alias("PIOS_TIM_3_irq_handler"))); +static void PIOS_TIM_3_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM3); + PIOS_TIM_generic_irq_handler(TIM3); } -void TIM4_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_4_irq_handler"))); -static void PIOS_TIM_4_irq_handler (void) +void TIM4_IRQHandler(void) __attribute__((alias("PIOS_TIM_4_irq_handler"))); +static void PIOS_TIM_4_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM4); + PIOS_TIM_generic_irq_handler(TIM4); } -void TIM5_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_5_irq_handler"))); -static void PIOS_TIM_5_irq_handler (void) +void TIM5_IRQHandler(void) __attribute__((alias("PIOS_TIM_5_irq_handler"))); +static void PIOS_TIM_5_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM5); + PIOS_TIM_generic_irq_handler(TIM5); } -void TIM6_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_6_irq_handler"))); -static void PIOS_TIM_6_irq_handler (void) +void TIM6_IRQHandler(void) __attribute__((alias("PIOS_TIM_6_irq_handler"))); +static void PIOS_TIM_6_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM6); + PIOS_TIM_generic_irq_handler(TIM6); } -void TIM7_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_7_irq_handler"))); -static void PIOS_TIM_7_irq_handler (void) +void TIM7_IRQHandler(void) __attribute__((alias("PIOS_TIM_7_irq_handler"))); +static void PIOS_TIM_7_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM7); + PIOS_TIM_generic_irq_handler(TIM7); } -void TIM8_UP_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_8_UP_irq_handler"))); -static void PIOS_TIM_8_UP_irq_handler (void) +void TIM8_UP_IRQHandler(void) __attribute__((alias("PIOS_TIM_8_UP_irq_handler"))); +static void PIOS_TIM_8_UP_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM8); + PIOS_TIM_generic_irq_handler(TIM8); } -void TIM8_CC_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_8_CC_irq_handler"))); -static void PIOS_TIM_8_CC_irq_handler (void) +void TIM8_CC_IRQHandler(void) __attribute__((alias("PIOS_TIM_8_CC_irq_handler"))); +static void PIOS_TIM_8_CC_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM8); + PIOS_TIM_generic_irq_handler(TIM8); } #endif /* PIOS_INCLUDE_TIM */ diff --git a/flight/pios/stm32f10x/pios_usart.c b/flight/pios/stm32f10x/pios_usart.c index 3691c0dfd..21d29006a 100644 --- a/flight/pios/stm32f10x/pios_usart.c +++ b/flight/pios/stm32f10x/pios_usart.c @@ -6,25 +6,25 @@ * @brief PIOS interface for USART port * @{ * - * @file pios_usart.c + * @file pios_usart.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief USART commands. Inits USARTs, controls USARTs & Interupt handlers. (STM32 dependent) * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,62 +42,64 @@ static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail); static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usart_com_driver = { - .set_baud = PIOS_USART_ChangeBaud, - .tx_start = PIOS_USART_TxStart, - .rx_start = PIOS_USART_RxStart, - .bind_tx_cb = PIOS_USART_RegisterTxCallback, - .bind_rx_cb = PIOS_USART_RegisterRxCallback, + .set_baud = PIOS_USART_ChangeBaud, + .tx_start = PIOS_USART_TxStart, + .rx_start = PIOS_USART_RxStart, + .bind_tx_cb = PIOS_USART_RegisterTxCallback, + .bind_rx_cb = PIOS_USART_RegisterRxCallback, }; enum pios_usart_dev_magic { - PIOS_USART_DEV_MAGIC = 0x11223344, + PIOS_USART_DEV_MAGIC = 0x11223344, }; struct pios_usart_dev { - enum pios_usart_dev_magic magic; - const struct pios_usart_cfg * cfg; + enum pios_usart_dev_magic magic; + const struct pios_usart_cfg *cfg; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; - uint32_t rx_dropped; + uint32_t rx_dropped; }; -static bool PIOS_USART_validate(struct pios_usart_dev * usart_dev) +static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) { - return (usart_dev->magic == PIOS_USART_DEV_MAGIC); + return usart_dev->magic == PIOS_USART_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_usart_dev * PIOS_USART_alloc(void) +static struct pios_usart_dev *PIOS_USART_alloc(void) { - struct pios_usart_dev * usart_dev; + struct pios_usart_dev *usart_dev; - usart_dev = (struct pios_usart_dev *)pvPortMalloc(sizeof(*usart_dev)); - if (!usart_dev) return(NULL); + usart_dev = (struct pios_usart_dev *)pvPortMalloc(sizeof(*usart_dev)); + if (!usart_dev) { + return NULL; + } - usart_dev->magic = PIOS_USART_DEV_MAGIC; - return(usart_dev); + usart_dev->magic = PIOS_USART_DEV_MAGIC; + return usart_dev; } #else static struct pios_usart_dev pios_usart_devs[PIOS_USART_MAX_DEVS]; static uint8_t pios_usart_num_devs; -static struct pios_usart_dev * PIOS_USART_alloc(void) +static struct pios_usart_dev *PIOS_USART_alloc(void) { - struct pios_usart_dev * usart_dev; + struct pios_usart_dev *usart_dev; - if (pios_usart_num_devs >= PIOS_USART_MAX_DEVS) { - return (NULL); - } + if (pios_usart_num_devs >= PIOS_USART_MAX_DEVS) { + return NULL; + } - usart_dev = &pios_usart_devs[pios_usart_num_devs++]; - usart_dev->magic = PIOS_USART_DEV_MAGIC; + usart_dev = &pios_usart_devs[pios_usart_num_devs++]; + usart_dev->magic = PIOS_USART_DEV_MAGIC; - return (usart_dev); + return usart_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /* Bind Interrupt Handlers * @@ -108,224 +110,232 @@ static struct pios_usart_dev * PIOS_USART_alloc(void) static void PIOS_USART_generic_irq_handler(uint32_t usart_id); static uint32_t PIOS_USART_1_id; -void USART1_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_1_irq_handler"))); -static void PIOS_USART_1_irq_handler (void) +void USART1_IRQHandler(void) __attribute__((alias("PIOS_USART_1_irq_handler"))); +static void PIOS_USART_1_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_1_id); + PIOS_USART_generic_irq_handler(PIOS_USART_1_id); } static uint32_t PIOS_USART_2_id; -void USART2_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_2_irq_handler"))); -static void PIOS_USART_2_irq_handler (void) +void USART2_IRQHandler(void) __attribute__((alias("PIOS_USART_2_irq_handler"))); +static void PIOS_USART_2_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_2_id); + PIOS_USART_generic_irq_handler(PIOS_USART_2_id); } static uint32_t PIOS_USART_3_id; -void USART3_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_3_irq_handler"))); -static void PIOS_USART_3_irq_handler (void) +void USART3_IRQHandler(void) __attribute__((alias("PIOS_USART_3_irq_handler"))); +static void PIOS_USART_3_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_3_id); + PIOS_USART_generic_irq_handler(PIOS_USART_3_id); } /** -* Initialise a single USART device -*/ -int32_t PIOS_USART_Init(uint32_t * usart_id, const struct pios_usart_cfg * cfg) + * Initialise a single USART device + */ +int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) { - PIOS_DEBUG_Assert(usart_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(usart_id); + PIOS_DEBUG_Assert(cfg); - struct pios_usart_dev * usart_dev; + struct pios_usart_dev *usart_dev; - usart_dev = (struct pios_usart_dev *) PIOS_USART_alloc(); - if (!usart_dev) goto out_fail; + usart_dev = (struct pios_usart_dev *)PIOS_USART_alloc(); + if (!usart_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usart_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + usart_dev->cfg = cfg; - /* Enable the USART Pins Software Remapping */ - if (usart_dev->cfg->remap) { - GPIO_PinRemapConfig(usart_dev->cfg->remap, ENABLE); - } + /* Enable the USART Pins Software Remapping */ + if (usart_dev->cfg->remap) { + GPIO_PinRemapConfig(usart_dev->cfg->remap, ENABLE); + } - /* Initialize the USART Rx and Tx pins */ - GPIO_Init(usart_dev->cfg->rx.gpio, &usart_dev->cfg->rx.init); - GPIO_Init(usart_dev->cfg->tx.gpio, &usart_dev->cfg->tx.init); + /* Initialize the USART Rx and Tx pins */ + GPIO_Init(usart_dev->cfg->rx.gpio, &usart_dev->cfg->rx.init); + GPIO_Init(usart_dev->cfg->tx.gpio, &usart_dev->cfg->tx.init); - /* Enable USART clock */ - switch ((uint32_t)usart_dev->cfg->regs) { - case (uint32_t)USART1: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); - break; - case (uint32_t)USART2: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); - break; - case (uint32_t)USART3: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); - break; - } - - /* Configure the USART */ - USART_Init(usart_dev->cfg->regs, &usart_dev->cfg->init); - - *usart_id = (uint32_t)usart_dev; + /* Enable USART clock */ + switch ((uint32_t)usart_dev->cfg->regs) { + case (uint32_t)USART1: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); + break; + case (uint32_t)USART2: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); + break; + case (uint32_t)USART3: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); + break; + } - /* Configure USART Interrupts */ - switch ((uint32_t)usart_dev->cfg->regs) { - case (uint32_t)USART1: - PIOS_USART_1_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART2: - PIOS_USART_2_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART3: - PIOS_USART_3_id = (uint32_t)usart_dev; - break; - } - NVIC_Init(&usart_dev->cfg->irq.init); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); - - /* Enable USART */ - USART_Cmd(usart_dev->cfg->regs, ENABLE); + /* Configure the USART */ + USART_Init(usart_dev->cfg->regs, &usart_dev->cfg->init); - return(0); + *usart_id = (uint32_t)usart_dev; + + /* Configure USART Interrupts */ + switch ((uint32_t)usart_dev->cfg->regs) { + case (uint32_t)USART1: + PIOS_USART_1_id = (uint32_t)usart_dev; + break; + case (uint32_t)USART2: + PIOS_USART_2_id = (uint32_t)usart_dev; + break; + case (uint32_t)USART3: + PIOS_USART_3_id = (uint32_t)usart_dev; + break; + } + NVIC_Init(&usart_dev->cfg->irq.init); + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); + + /* Enable USART */ + USART_Cmd(usart_dev->cfg->regs, ENABLE); + + return 0; out_fail: - return(-1); + return -1; } static void PIOS_USART_RxStart(uint32_t usart_id, __attribute__((unused)) uint16_t rx_bytes_avail) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); + bool valid = PIOS_USART_validate(usart_dev); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); + PIOS_Assert(valid); + + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); } static void PIOS_USART_TxStart(uint32_t usart_id, __attribute__((unused)) uint16_t tx_bytes_avail) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); + bool valid = PIOS_USART_validate(usart_dev); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); + PIOS_Assert(valid); + + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); } /** -* Changes the baud rate of the USART peripheral without re-initialising. -* \param[in] usart_id USART name (GPS, TELEM, AUX) -* \param[in] baud Requested baud rate -*/ + * Changes the baud rate of the USART peripheral without re-initialising. + * \param[in] usart_id USART name (GPS, TELEM, AUX) + * \param[in] baud Requested baud rate + */ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); + bool valid = PIOS_USART_validate(usart_dev); - USART_InitTypeDef USART_InitStructure; + PIOS_Assert(valid); - /* Start with a copy of the default configuration for the peripheral */ - USART_InitStructure = usart_dev->cfg->init; + USART_InitTypeDef USART_InitStructure; - /* Adjust the baud rate */ - USART_InitStructure.USART_BaudRate = baud; + /* Start with a copy of the default configuration for the peripheral */ + USART_InitStructure = usart_dev->cfg->init; - /* Write back the new configuration */ - USART_Init(usart_dev->cfg->regs, &USART_InitStructure); + /* Adjust the baud rate */ + USART_InitStructure.USART_BaudRate = baud; + + /* Write back the new configuration */ + USART_Init(usart_dev->cfg->regs, &USART_InitStructure); } static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); + bool valid = PIOS_USART_validate(usart_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usart_dev->rx_in_context = context; - usart_dev->rx_in_cb = rx_in_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usart_dev->rx_in_context = context; + usart_dev->rx_in_cb = rx_in_cb; } static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); + bool valid = PIOS_USART_validate(usart_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usart_dev->tx_out_context = context; - usart_dev->tx_out_cb = tx_out_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usart_dev->tx_out_context = context; + usart_dev->tx_out_cb = tx_out_cb; } static void PIOS_USART_generic_irq_handler(uint32_t usart_id) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); + bool valid = PIOS_USART_validate(usart_dev); - /* Force read of dr after sr to make sure to clear error flags */ - volatile uint16_t sr = usart_dev->cfg->regs->SR; - volatile uint8_t dr = usart_dev->cfg->regs->DR; + PIOS_Assert(valid); - /* Check if RXNE flag is set */ - bool rx_need_yield = false; - if (sr & USART_SR_RXNE) { - uint8_t byte = dr; - if (usart_dev->rx_in_cb) { - uint16_t rc; - rc = (usart_dev->rx_in_cb)(usart_dev->rx_in_context, &byte, 1, NULL, &rx_need_yield); - if (rc < 1) { - /* Lost bytes on rx */ - usart_dev->rx_dropped += 1; - } - } - } + /* Force read of dr after sr to make sure to clear error flags */ + volatile uint16_t sr = usart_dev->cfg->regs->SR; + volatile uint8_t dr = usart_dev->cfg->regs->DR; - /* Check if TXE flag is set */ - bool tx_need_yield = false; - if (sr & USART_SR_TXE) { - if (usart_dev->tx_out_cb) { - uint8_t b; - uint16_t bytes_to_send; + /* Check if RXNE flag is set */ + bool rx_need_yield = false; + if (sr & USART_SR_RXNE) { + uint8_t byte = dr; + if (usart_dev->rx_in_cb) { + uint16_t rc; + rc = (usart_dev->rx_in_cb)(usart_dev->rx_in_context, &byte, 1, NULL, &rx_need_yield); + if (rc < 1) { + /* Lost bytes on rx */ + usart_dev->rx_dropped += 1; + } + } + } - bytes_to_send = (usart_dev->tx_out_cb)(usart_dev->tx_out_context, &b, 1, NULL, &tx_need_yield); + /* Check if TXE flag is set */ + bool tx_need_yield = false; + if (sr & USART_SR_TXE) { + if (usart_dev->tx_out_cb) { + uint8_t b; + uint16_t bytes_to_send; - if (bytes_to_send > 0) { - /* Send the byte we've been given */ - usart_dev->cfg->regs->DR = b; - } else { - /* No bytes to send, disable TXE interrupt */ - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); - } - } else { - /* No bytes to send, disable TXE interrupt */ - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); - } - } + bytes_to_send = (usart_dev->tx_out_cb)(usart_dev->tx_out_context, &b, 1, NULL, &tx_need_yield); + + if (bytes_to_send > 0) { + /* Send the byte we've been given */ + usart_dev->cfg->regs->DR = b; + } else { + /* No bytes to send, disable TXE interrupt */ + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); + } + } else { + /* No bytes to send, disable TXE interrupt */ + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); + } + } #if defined(PIOS_INCLUDE_FREERTOS) - if (rx_need_yield || tx_need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (rx_need_yield || tx_need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } #endif /* PIOS_INCLUDE_USART */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_usb.c b/flight/pios/stm32f10x/pios_usb.c index bfecbe3ff..1f085199d 100644 --- a/flight/pios/stm32f10x/pios_usb.c +++ b/flight/pios/stm32f10x/pios_usb.c @@ -44,57 +44,61 @@ static bool transfer_possible = false; enum pios_usb_dev_magic { - PIOS_USB_DEV_MAGIC = 0x17365904, + PIOS_USB_DEV_MAGIC = 0x17365904, }; struct pios_usb_dev { - enum pios_usb_dev_magic magic; - const struct pios_usb_cfg * cfg; + enum pios_usb_dev_magic magic; + const struct pios_usb_cfg *cfg; }; /** * @brief Validate the usb device structure * @returns 0 if valid device or -1 otherwise */ -static int32_t PIOS_USB_validate(struct pios_usb_dev * usb_dev) +static int32_t PIOS_USB_validate(struct pios_usb_dev *usb_dev) { - if(usb_dev == NULL) - return -1; + if (usb_dev == NULL) { + return -1; + } - if (usb_dev->magic != PIOS_USB_DEV_MAGIC) - return -1; + if (usb_dev->magic != PIOS_USB_DEV_MAGIC) { + return -1; + } - return 0; + return 0; } #ifdef PIOS_INCLUDE_FREERTOS -static struct pios_usb_dev * PIOS_USB_alloc(void) +static struct pios_usb_dev *PIOS_USB_alloc(void) { - struct pios_usb_dev * usb_dev; + struct pios_usb_dev *usb_dev; - usb_dev = (struct pios_usb_dev *)pvPortMalloc(sizeof(*usb_dev)); - if (!usb_dev) return(NULL); + usb_dev = (struct pios_usb_dev *)pvPortMalloc(sizeof(*usb_dev)); + if (!usb_dev) { + return NULL; + } - usb_dev->magic = PIOS_USB_DEV_MAGIC; - return(usb_dev); + usb_dev->magic = PIOS_USB_DEV_MAGIC; + return usb_dev; } #else static struct pios_usb_dev pios_usb_devs[PIOS_USB_MAX_DEVS]; static uint8_t pios_usb_num_devs; -static struct pios_usb_dev * PIOS_USB_alloc(void) +static struct pios_usb_dev *PIOS_USB_alloc(void) { - struct pios_usb_dev * usb_dev; + struct pios_usb_dev *usb_dev; - if (pios_usb_num_devs >= PIOS_USB_MAX_DEVS) { - return (NULL); - } + if (pios_usb_num_devs >= PIOS_USB_MAX_DEVS) { + return NULL; + } - usb_dev = &pios_usb_devs[pios_usb_num_devs++]; - usb_dev->magic = PIOS_USB_DEV_MAGIC; + usb_dev = &pios_usb_devs[pios_usb_num_devs++]; + usb_dev->magic = PIOS_USB_DEV_MAGIC; - return (usb_dev); + return usb_dev; } -#endif +#endif /* ifdef PIOS_INCLUDE_FREERTOS */ /** @@ -103,44 +107,46 @@ static struct pios_usb_dev * PIOS_USB_alloc(void) * \note Applications shouldn't call this function directly, instead please use \ref PIOS_COM layer functions */ static uint32_t pios_usb_com_id; -int32_t PIOS_USB_Init(uint32_t * usb_id, const struct pios_usb_cfg * cfg) +int32_t PIOS_USB_Init(uint32_t *usb_id, const struct pios_usb_cfg *cfg) { - PIOS_Assert(usb_id); - PIOS_Assert(cfg); + PIOS_Assert(usb_id); + PIOS_Assert(cfg); - struct pios_usb_dev * usb_dev; + struct pios_usb_dev *usb_dev; - usb_dev = (struct pios_usb_dev *) PIOS_USB_alloc(); - if (!usb_dev) goto out_fail; + usb_dev = (struct pios_usb_dev *)PIOS_USB_alloc(); + if (!usb_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usb_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + usb_dev->cfg = cfg; - PIOS_USB_Reenumerate(); + PIOS_USB_Reenumerate(); - /* - * This is a horrible hack to make this available to - * the interrupt callbacks. This should go away ASAP. - */ - pios_usb_com_id = (uint32_t) usb_dev; + /* + * This is a horrible hack to make this available to + * the interrupt callbacks. This should go away ASAP. + */ + pios_usb_com_id = (uint32_t)usb_dev; - /* Enable the USB Interrupts */ - NVIC_Init(&usb_dev->cfg->irq.init); + /* Enable the USB Interrupts */ + NVIC_Init(&usb_dev->cfg->irq.init); - /* Select USBCLK source */ - RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_1Div5); - /* Enable the USB clock */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USB, ENABLE); + /* Select USBCLK source */ + RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_1Div5); + /* Enable the USB clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_USB, ENABLE); - USB_Init(); - USB_SIL_Init(); + USB_Init(); + USB_SIL_Init(); - *usb_id = (uint32_t) usb_dev; + *usb_id = (uint32_t)usb_dev; - return 0; /* No error */ + return 0; /* No error */ out_fail: - return(-1); + return -1; } /** @@ -151,71 +157,72 @@ out_fail: */ int32_t PIOS_USB_ChangeConnectionState(bool Connected) { - // In all cases: re-initialise USB HID driver - if (Connected) { - transfer_possible = true; + // In all cases: re-initialise USB HID driver + if (Connected) { + transfer_possible = true; - //TODO: Check SetEPRxValid(ENDP1); + // TODO: Check SetEPRxValid(ENDP1); #ifdef USB_LED_ON - USB_LED_ON; // turn the USB led on + USB_LED_ON; // turn the USB led on #endif - } else { - // Cable disconnected: disable transfers - transfer_possible = false; + } else { + // Cable disconnected: disable transfers + transfer_possible = false; #ifdef USB_LED_OFF - USB_LED_OFF; // turn the USB led off + USB_LED_OFF; // turn the USB led off #endif - } + } - return 0; + return 0; } int32_t PIOS_USB_Reenumerate() { - /* Force USB reset and power-down (this will also release the USB pins for direct GPIO control) */ - _SetCNTR(CNTR_FRES | CNTR_PDWN); + /* Force USB reset and power-down (this will also release the USB pins for direct GPIO control) */ + _SetCNTR(CNTR_FRES | CNTR_PDWN); - /* Using a "dirty" method to force a re-enumeration: */ - /* Force DPM (Pin PA12) low for ca. 10 mS before USB Tranceiver will be enabled */ - /* This overrules the external Pull-Up at PA12, and at least Windows & MacOS will enumerate again */ - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_Init(GPIOA, &GPIO_InitStructure); + /* Using a "dirty" method to force a re-enumeration: */ + /* Force DPM (Pin PA12) low for ca. 10 mS before USB Tranceiver will be enabled */ + /* This overrules the external Pull-Up at PA12, and at least Windows & MacOS will enumerate again */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_Init(GPIOA, &GPIO_InitStructure); - PIOS_DELAY_WaitmS(50); + PIOS_DELAY_WaitmS(50); - /* Release power-down, still hold reset */ - _SetCNTR(CNTR_PDWN); - PIOS_DELAY_WaituS(5); + /* Release power-down, still hold reset */ + _SetCNTR(CNTR_PDWN); + PIOS_DELAY_WaituS(5); - /* CNTR_FRES = 0 */ - _SetCNTR(0); + /* CNTR_FRES = 0 */ + _SetCNTR(0); - /* Clear pending interrupts */ - _SetISTR(0); + /* Clear pending interrupts */ + _SetISTR(0); - /* Configure USB clock */ - /* USBCLK = PLLCLK / 1.5 */ - RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_1Div5); - /* Enable USB clock */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USB, ENABLE); + /* Configure USB clock */ + /* USBCLK = PLLCLK / 1.5 */ + RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_1Div5); + /* Enable USB clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_USB, ENABLE); - return 0; + return 0; } bool PIOS_USB_CableConnected(__attribute__((unused)) uint8_t id) { - struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_com_id; + struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_com_id; - if (PIOS_USB_validate(usb_dev) != 0) - return false; + if (PIOS_USB_validate(usb_dev) != 0) { + return false; + } - return ((usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin) != 0) ^ usb_dev->cfg->vsense_active_low; + return ((usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin) != 0) ^ usb_dev->cfg->vsense_active_low; } /** @@ -226,12 +233,13 @@ bool PIOS_USB_CableConnected(__attribute__((unused)) uint8_t id) */ bool PIOS_USB_CheckAvailable(uint32_t id) { - struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_com_id; + struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_com_id; - if (PIOS_USB_validate(usb_dev) != 0) - return false; + if (PIOS_USB_validate(usb_dev) != 0) { + return false; + } - return PIOS_USB_CableConnected(id) && transfer_possible; + return PIOS_USB_CableConnected(id) && transfer_possible; } #endif /* PIOS_INCLUDE_USB_HID */ diff --git a/flight/pios/stm32f10x/pios_usb_cdc.c b/flight/pios/stm32f10x/pios_usb_cdc.c index 2d470dee9..9d3724306 100644 --- a/flight/pios/stm32f10x/pios_usb_cdc.c +++ b/flight/pios/stm32f10x/pios_usb_cdc.c @@ -45,68 +45,70 @@ static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail); static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usb_cdc_com_driver = { - .tx_start = PIOS_USB_CDC_TxStart, - .rx_start = PIOS_USB_CDC_RxStart, - .bind_tx_cb = PIOS_USB_CDC_RegisterTxCallback, - .bind_rx_cb = PIOS_USB_CDC_RegisterRxCallback, - .available = PIOS_USB_CheckAvailable, + .tx_start = PIOS_USB_CDC_TxStart, + .rx_start = PIOS_USB_CDC_RxStart, + .bind_tx_cb = PIOS_USB_CDC_RegisterTxCallback, + .bind_rx_cb = PIOS_USB_CDC_RegisterRxCallback, + .available = PIOS_USB_CheckAvailable, }; enum pios_usb_cdc_dev_magic { - PIOS_USB_CDC_DEV_MAGIC = 0xAABBCCDD, + PIOS_USB_CDC_DEV_MAGIC = 0xAABBCCDD, }; struct pios_usb_cdc_dev { - enum pios_usb_cdc_dev_magic magic; - const struct pios_usb_cdc_cfg * cfg; + enum pios_usb_cdc_dev_magic magic; + const struct pios_usb_cdc_cfg *cfg; - uint32_t lower_id; + uint32_t lower_id; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; - uint8_t rx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; - uint8_t tx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; + uint8_t rx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; + uint8_t tx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; - uint32_t rx_dropped; - uint32_t rx_oversize; + uint32_t rx_dropped; + uint32_t rx_oversize; }; -static bool PIOS_USB_CDC_validate(struct pios_usb_cdc_dev * usb_cdc_dev) +static bool PIOS_USB_CDC_validate(struct pios_usb_cdc_dev *usb_cdc_dev) { - return (usb_cdc_dev->magic == PIOS_USB_CDC_DEV_MAGIC); + return usb_cdc_dev->magic == PIOS_USB_CDC_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_usb_cdc_dev * PIOS_USB_CDC_alloc(void) +static struct pios_usb_cdc_dev *PIOS_USB_CDC_alloc(void) { - struct pios_usb_cdc_dev * usb_cdc_dev; + struct pios_usb_cdc_dev *usb_cdc_dev; - usb_cdc_dev = (struct pios_usb_cdc_dev *)pvPortMalloc(sizeof(*usb_cdc_dev)); - if (!usb_cdc_dev) return(NULL); + usb_cdc_dev = (struct pios_usb_cdc_dev *)pvPortMalloc(sizeof(*usb_cdc_dev)); + if (!usb_cdc_dev) { + return NULL; + } - usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; - return(usb_cdc_dev); + usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; + return usb_cdc_dev; } #else static struct pios_usb_cdc_dev pios_usb_cdc_devs[PIOS_USB_CDC_MAX_DEVS]; static uint8_t pios_usb_cdc_num_devs; -static struct pios_usb_cdc_dev * PIOS_USB_CDC_alloc(void) +static struct pios_usb_cdc_dev *PIOS_USB_CDC_alloc(void) { - struct pios_usb_cdc_dev * usb_cdc_dev; + struct pios_usb_cdc_dev *usb_cdc_dev; - if (pios_usb_cdc_num_devs >= PIOS_USB_CDC_MAX_DEVS) { - return (NULL); - } + if (pios_usb_cdc_num_devs >= PIOS_USB_CDC_MAX_DEVS) { + return NULL; + } - usb_cdc_dev = &pios_usb_cdc_devs[pios_usb_cdc_num_devs++]; - usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; + usb_cdc_dev = &pios_usb_cdc_devs[pios_usb_cdc_num_devs++]; + usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; - return (usb_cdc_dev); + return usb_cdc_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ static void PIOS_USB_CDC_DATA_EP_IN_Callback(void); static void PIOS_USB_CDC_DATA_EP_OUT_Callback(void); @@ -115,304 +117,316 @@ static void PIOS_USB_CDC_CTRL_EP_IN_Callback(void); static uint32_t pios_usb_cdc_id; /* Need a better way to pull these in */ -extern void (*pEpInt_IN[7])(void); -extern void (*pEpInt_OUT[7])(void); +extern void(*pEpInt_IN[7]) (void); +extern void(*pEpInt_OUT[7]) (void); -int32_t PIOS_USB_CDC_Init(uint32_t * usbcdc_id, const struct pios_usb_cdc_cfg * cfg, uint32_t lower_id) +int32_t PIOS_USB_CDC_Init(uint32_t *usbcdc_id, const struct pios_usb_cdc_cfg *cfg, uint32_t lower_id) { - PIOS_Assert(usbcdc_id); - PIOS_Assert(cfg); + PIOS_Assert(usbcdc_id); + PIOS_Assert(cfg); - struct pios_usb_cdc_dev * usb_cdc_dev; + struct pios_usb_cdc_dev *usb_cdc_dev; - usb_cdc_dev = (struct pios_usb_cdc_dev *) PIOS_USB_CDC_alloc(); - if (!usb_cdc_dev) goto out_fail; + usb_cdc_dev = (struct pios_usb_cdc_dev *)PIOS_USB_CDC_alloc(); + if (!usb_cdc_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usb_cdc_dev->cfg = cfg; - usb_cdc_dev->lower_id = lower_id; + /* Bind the configuration to the device instance */ + usb_cdc_dev->cfg = cfg; + usb_cdc_dev->lower_id = lower_id; - pios_usb_cdc_id = (uint32_t) usb_cdc_dev; + pios_usb_cdc_id = (uint32_t)usb_cdc_dev; - /* Bind lower level callbacks into the USB infrastructure */ - pEpInt_OUT[cfg->ctrl_tx_ep - 1] = PIOS_USB_CDC_CTRL_EP_IN_Callback; - pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_CDC_DATA_EP_IN_Callback; - pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_CDC_DATA_EP_OUT_Callback; + /* Bind lower level callbacks into the USB infrastructure */ + pEpInt_OUT[cfg->ctrl_tx_ep - 1] = PIOS_USB_CDC_CTRL_EP_IN_Callback; + pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_CDC_DATA_EP_IN_Callback; + pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_CDC_DATA_EP_OUT_Callback; - *usbcdc_id = (uint32_t) usb_cdc_dev; + *usbcdc_id = (uint32_t)usb_cdc_dev; - return 0; + return 0; out_fail: - return -1; + return -1; } - static void PIOS_USB_CDC_RegisterRxCallback(uint32_t usbcdc_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_cdc_dev->rx_in_context = context; - usb_cdc_dev->rx_in_cb = rx_in_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->rx_in_context = context; + usb_cdc_dev->rx_in_cb = rx_in_cb; } static void PIOS_USB_CDC_RegisterTxCallback(uint32_t usbcdc_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_cdc_dev->tx_out_context = context; - usb_cdc_dev->tx_out_cb = tx_out_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->tx_out_context = context; + usb_cdc_dev->tx_out_cb = tx_out_cb; } -static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); - - if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { - return; - } - - // If endpoint was stalled and there is now space make it valid - PIOS_IRQ_Disable(); - if ((GetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep) != EP_RX_VALID) && - (rx_bytes_avail >= sizeof(usb_cdc_dev->rx_packet_buffer))) { - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); - } - PIOS_IRQ_Enable(); -} - -static void PIOS_USB_CDC_SendData(struct pios_usb_cdc_dev * usb_cdc_dev) +static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail) { - uint16_t bytes_to_tx; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - if (!usb_cdc_dev->tx_out_cb) { - return; - } + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - bool need_yield = false; - bytes_to_tx = (usb_cdc_dev->tx_out_cb)(usb_cdc_dev->tx_out_context, - usb_cdc_dev->tx_packet_buffer, - sizeof(usb_cdc_dev->tx_packet_buffer), - NULL, - &need_yield); - if (bytes_to_tx == 0) { - return; - } + PIOS_Assert(valid); - UserToPMABufferCopy(usb_cdc_dev->tx_packet_buffer, - GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), - bytes_to_tx); - SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, bytes_to_tx); - SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); + if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { + return; + } + + // If endpoint was stalled and there is now space make it valid + PIOS_IRQ_Disable(); + if ((GetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep) != EP_RX_VALID) && + (rx_bytes_avail >= sizeof(usb_cdc_dev->rx_packet_buffer))) { + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); + } + PIOS_IRQ_Enable(); +} + +static void PIOS_USB_CDC_SendData(struct pios_usb_cdc_dev *usb_cdc_dev) +{ + uint16_t bytes_to_tx; + + if (!usb_cdc_dev->tx_out_cb) { + return; + } + + bool need_yield = false; + bytes_to_tx = (usb_cdc_dev->tx_out_cb)(usb_cdc_dev->tx_out_context, + usb_cdc_dev->tx_packet_buffer, + sizeof(usb_cdc_dev->tx_packet_buffer), + NULL, + &need_yield); + if (bytes_to_tx == 0) { + return; + } + + UserToPMABufferCopy(usb_cdc_dev->tx_packet_buffer, + GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), + bytes_to_tx); + SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, bytes_to_tx); + SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); #if defined(PIOS_INCLUDE_FREERTOS) - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, __attribute__((unused)) uint16_t tx_bytes_avail) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { - return; - } + PIOS_Assert(valid); - if (GetEPTxStatus(usb_cdc_dev->cfg->data_tx_ep) == EP_TX_VALID) { - /* Endpoint is already transmitting */ - return; - } + if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { + return; + } - PIOS_USB_CDC_SendData(usb_cdc_dev); + if (GetEPTxStatus(usb_cdc_dev->cfg->data_tx_ep) == EP_TX_VALID) { + /* Endpoint is already transmitting */ + return; + } + + PIOS_USB_CDC_SendData(usb_cdc_dev); } static void PIOS_USB_CDC_DATA_EP_IN_Callback(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_USB_CDC_SendData(usb_cdc_dev); + PIOS_Assert(valid); + + PIOS_USB_CDC_SendData(usb_cdc_dev); } static void PIOS_USB_CDC_DATA_EP_OUT_Callback(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - uint32_t DataLength; + PIOS_Assert(valid); - /* Get the number of received data on the selected Endpoint */ - DataLength = GetEPRxCount(usb_cdc_dev->cfg->data_rx_ep); - if (DataLength > sizeof(usb_cdc_dev->rx_packet_buffer)) { - usb_cdc_dev->rx_oversize++; - DataLength = sizeof(usb_cdc_dev->rx_packet_buffer); - } + uint32_t DataLength; - /* Use the memory interface function to read from the selected endpoint */ - PMAToUserBufferCopy((uint8_t *) usb_cdc_dev->rx_packet_buffer, - GetEPRxAddr(usb_cdc_dev->cfg->data_rx_ep), - DataLength); + /* Get the number of received data on the selected Endpoint */ + DataLength = GetEPRxCount(usb_cdc_dev->cfg->data_rx_ep); + if (DataLength > sizeof(usb_cdc_dev->rx_packet_buffer)) { + usb_cdc_dev->rx_oversize++; + DataLength = sizeof(usb_cdc_dev->rx_packet_buffer); + } - if (!usb_cdc_dev->rx_in_cb) { - /* No Rx call back registered, disable the receiver */ - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); - return; - } + /* Use the memory interface function to read from the selected endpoint */ + PMAToUserBufferCopy((uint8_t *)usb_cdc_dev->rx_packet_buffer, + GetEPRxAddr(usb_cdc_dev->cfg->data_rx_ep), + DataLength); - uint16_t headroom; - bool need_yield = false; - uint16_t rc; - rc = (usb_cdc_dev->rx_in_cb)(usb_cdc_dev->rx_in_context, - usb_cdc_dev->rx_packet_buffer, - DataLength, - &headroom, - &need_yield); + if (!usb_cdc_dev->rx_in_cb) { + /* No Rx call back registered, disable the receiver */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); + return; + } - if (rc < DataLength) { - /* Lost bytes on rx */ - usb_cdc_dev->rx_dropped += (DataLength - rc); - } + uint16_t headroom; + bool need_yield = false; + uint16_t rc; + rc = (usb_cdc_dev->rx_in_cb)(usb_cdc_dev->rx_in_context, + usb_cdc_dev->rx_packet_buffer, + DataLength, + &headroom, + &need_yield); - if (headroom >= sizeof(usb_cdc_dev->rx_packet_buffer)) { - /* We have room for a maximum length message */ - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); - } else { - /* Not enough room left for a message, apply backpressure */ - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); - } + if (rc < DataLength) { + /* Lost bytes on rx */ + usb_cdc_dev->rx_dropped += (DataLength - rc); + } + + if (headroom >= sizeof(usb_cdc_dev->rx_packet_buffer)) { + /* We have room for a maximum length message */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); + } else { + /* Not enough room left for a message, apply backpressure */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } static uint16_t control_line_state; RESULT PIOS_USB_CDC_SetControlLineState(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (!valid) { - /* No CDC interface is configured */ - return USB_UNSUPPORT; - } + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - uint8_t wValue0 = pInformation->USBwValue0; - uint8_t wValue1 = pInformation->USBwValue1; + if (!valid) { + /* No CDC interface is configured */ + return USB_UNSUPPORT; + } - control_line_state = wValue1 << 8 | wValue0; + uint8_t wValue0 = pInformation->USBwValue0; + uint8_t wValue1 = pInformation->USBwValue1; - return USB_SUCCESS; + control_line_state = wValue1 << 8 | wValue0; + + return USB_SUCCESS; } static struct usb_cdc_line_coding line_coding = { - .dwDTERate = htousbl(57600), - .bCharFormat = USB_CDC_LINE_CODING_STOP_1, - .bParityType = USB_CDC_LINE_CODING_PARITY_NONE, - .bDataBits = 8, + .dwDTERate = htousbl(57600), + .bCharFormat = USB_CDC_LINE_CODING_STOP_1, + .bParityType = USB_CDC_LINE_CODING_PARITY_NONE, + .bDataBits = 8, }; uint8_t *PIOS_USB_CDC_SetLineCoding(uint16_t Length) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (!valid) { - /* No CDC interface is configured */ - return NULL; - } + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (Length == 0) { - /* Report the number of bytes we're prepared to consume */ - pInformation->Ctrl_Info.Usb_wLength = sizeof(line_coding); - pInformation->Ctrl_Info.Usb_rLength = sizeof(line_coding); - return NULL; - } else { - /* Give out a pointer to the data struct */ - return ((uint8_t *) &line_coding); - } + if (!valid) { + /* No CDC interface is configured */ + return NULL; + } + + if (Length == 0) { + /* Report the number of bytes we're prepared to consume */ + pInformation->Ctrl_Info.Usb_wLength = sizeof(line_coding); + pInformation->Ctrl_Info.Usb_rLength = sizeof(line_coding); + return NULL; + } else { + /* Give out a pointer to the data struct */ + return (uint8_t *)&line_coding; + } } const uint8_t *PIOS_USB_CDC_GetLineCoding(uint16_t Length) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (!valid) { - /* No CDC interface is configured */ - return NULL; - } + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (Length == 0) { - pInformation->Ctrl_Info.Usb_wLength = sizeof(line_coding); - return NULL; - } else { - return ((uint8_t *) &line_coding); - } + if (!valid) { + /* No CDC interface is configured */ + return NULL; + } + + if (Length == 0) { + pInformation->Ctrl_Info.Usb_wLength = sizeof(line_coding); + return NULL; + } else { + return (uint8_t *)&line_coding; + } } struct usb_cdc_serial_state_report uart_state = { - .bmRequestType = 0xA1, - .bNotification = USB_CDC_NOTIFICATION_SERIAL_STATE, - .wValue = 0, - .wIndex = htousbs(1), - .wLength = htousbs(2), - .bmUartState = htousbs(0), + .bmRequestType = 0xA1, + .bNotification = USB_CDC_NOTIFICATION_SERIAL_STATE, + .wValue = 0, + .wIndex = htousbs(1), + .wLength = htousbs(2), + .bmUartState = htousbs(0), }; - + static void PIOS_USB_CDC_CTRL_EP_IN_Callback(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - /* Give back UART State Bitmap */ - /* UART State Bitmap - * 15-7: reserved - * 6: bOverRun overrun error - * 5: bParity parity error - * 4: bFraming framing error - * 3: bRingSignal RI - * 2: bBreak break reception - * 1: bTxCarrier DSR - * 0: bRxCarrier DCD - */ - uart_state.bmUartState = htousbs(0x0003); + PIOS_Assert(valid); - UserToPMABufferCopy((uint8_t *) &uart_state, - GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), - sizeof(uart_state)); - SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, PIOS_USB_BOARD_CDC_MGMT_LENGTH); - SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); + /* Give back UART State Bitmap */ + /* UART State Bitmap + * 15-7: reserved + * 6: bOverRun overrun error + * 5: bParity parity error + * 4: bFraming framing error + * 3: bRingSignal RI + * 2: bBreak break reception + * 1: bTxCarrier DSR + * 0: bRxCarrier DCD + */ + uart_state.bmUartState = htousbs(0x0003); + + UserToPMABufferCopy((uint8_t *)&uart_state, + GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), + sizeof(uart_state)); + SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, PIOS_USB_BOARD_CDC_MGMT_LENGTH); + SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); } #endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/pios/stm32f10x/pios_usb_hid.c b/flight/pios/stm32f10x/pios_usb_hid.c index 2656c3dce..44db1d902 100644 --- a/flight/pios/stm32f10x/pios_usb_hid.c +++ b/flight/pios/stm32f10x/pios_usb_hid.c @@ -45,68 +45,70 @@ static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, uint16_t tx_bytes_avail); static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usb_hid_com_driver = { - .tx_start = PIOS_USB_HID_TxStart, - .rx_start = PIOS_USB_HID_RxStart, - .bind_tx_cb = PIOS_USB_HID_RegisterTxCallback, - .bind_rx_cb = PIOS_USB_HID_RegisterRxCallback, - .available = PIOS_USB_CheckAvailable, + .tx_start = PIOS_USB_HID_TxStart, + .rx_start = PIOS_USB_HID_RxStart, + .bind_tx_cb = PIOS_USB_HID_RegisterTxCallback, + .bind_rx_cb = PIOS_USB_HID_RegisterRxCallback, + .available = PIOS_USB_CheckAvailable, }; enum pios_usb_hid_dev_magic { - PIOS_USB_HID_DEV_MAGIC = 0xAA00BB00, + PIOS_USB_HID_DEV_MAGIC = 0xAA00BB00, }; struct pios_usb_hid_dev { - enum pios_usb_hid_dev_magic magic; - const struct pios_usb_hid_cfg * cfg; + enum pios_usb_hid_dev_magic magic; + const struct pios_usb_hid_cfg *cfg; - uint32_t lower_id; + uint32_t lower_id; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; - uint8_t rx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; - uint8_t tx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; + uint8_t rx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; + uint8_t tx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; - uint32_t rx_dropped; - uint32_t rx_oversize; + uint32_t rx_dropped; + uint32_t rx_oversize; }; -static bool PIOS_USB_HID_validate(struct pios_usb_hid_dev * usb_hid_dev) +static bool PIOS_USB_HID_validate(struct pios_usb_hid_dev *usb_hid_dev) { - return (usb_hid_dev->magic == PIOS_USB_HID_DEV_MAGIC); + return usb_hid_dev->magic == PIOS_USB_HID_DEV_MAGIC; } #ifdef PIOS_INCLUDE_FREERTOS -static struct pios_usb_hid_dev * PIOS_USB_HID_alloc(void) +static struct pios_usb_hid_dev *PIOS_USB_HID_alloc(void) { - struct pios_usb_hid_dev * usb_hid_dev; + struct pios_usb_hid_dev *usb_hid_dev; - usb_hid_dev = (struct pios_usb_hid_dev *)pvPortMalloc(sizeof(*usb_hid_dev)); - if (!usb_hid_dev) return(NULL); + usb_hid_dev = (struct pios_usb_hid_dev *)pvPortMalloc(sizeof(*usb_hid_dev)); + if (!usb_hid_dev) { + return NULL; + } - usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; - return(usb_hid_dev); + usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; + return usb_hid_dev; } #else static struct pios_usb_hid_dev pios_usb_hid_devs[PIOS_USB_HID_MAX_DEVS]; static uint8_t pios_usb_hid_num_devs; -static struct pios_usb_hid_dev * PIOS_USB_HID_alloc(void) +static struct pios_usb_hid_dev *PIOS_USB_HID_alloc(void) { - struct pios_usb_hid_dev * usb_hid_dev; + struct pios_usb_hid_dev *usb_hid_dev; - if (pios_usb_hid_num_devs >= PIOS_USB_HID_MAX_DEVS) { - return (NULL); - } + if (pios_usb_hid_num_devs >= PIOS_USB_HID_MAX_DEVS) { + return NULL; + } - usb_hid_dev = &pios_usb_hid_devs[pios_usb_hid_num_devs++]; - usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; + usb_hid_dev = &pios_usb_hid_devs[pios_usb_hid_num_devs++]; + usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; - return (usb_hid_dev); + return usb_hid_dev; } -#endif +#endif /* ifdef PIOS_INCLUDE_FREERTOS */ static void PIOS_USB_HID_EP_IN_Callback(void); static void PIOS_USB_HID_EP_OUT_Callback(void); @@ -114,162 +116,167 @@ static void PIOS_USB_HID_EP_OUT_Callback(void); static uint32_t pios_usb_hid_id; /* Need a better way to pull these in */ -extern void (*pEpInt_IN[7])(void); -extern void (*pEpInt_OUT[7])(void); +extern void(*pEpInt_IN[7]) (void); +extern void(*pEpInt_OUT[7]) (void); -int32_t PIOS_USB_HID_Init(uint32_t * usbhid_id, const struct pios_usb_hid_cfg * cfg, uint32_t lower_id) +int32_t PIOS_USB_HID_Init(uint32_t *usbhid_id, const struct pios_usb_hid_cfg *cfg, uint32_t lower_id) { - PIOS_Assert(usbhid_id); - PIOS_Assert(cfg); + PIOS_Assert(usbhid_id); + PIOS_Assert(cfg); - struct pios_usb_hid_dev * usb_hid_dev; + struct pios_usb_hid_dev *usb_hid_dev; - usb_hid_dev = (struct pios_usb_hid_dev *) PIOS_USB_HID_alloc(); - if (!usb_hid_dev) goto out_fail; + usb_hid_dev = (struct pios_usb_hid_dev *)PIOS_USB_HID_alloc(); + if (!usb_hid_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usb_hid_dev->cfg = cfg; - usb_hid_dev->lower_id = lower_id; + /* Bind the configuration to the device instance */ + usb_hid_dev->cfg = cfg; + usb_hid_dev->lower_id = lower_id; - pios_usb_hid_id = (uint32_t) usb_hid_dev; + pios_usb_hid_id = (uint32_t)usb_hid_dev; - /* Bind lower level callbacks into the USB infrastructure */ - pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_HID_EP_IN_Callback; - pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_HID_EP_OUT_Callback; + /* Bind lower level callbacks into the USB infrastructure */ + pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_HID_EP_IN_Callback; + pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_HID_EP_OUT_Callback; - *usbhid_id = (uint32_t) usb_hid_dev; + *usbhid_id = (uint32_t)usb_hid_dev; - return 0; + return 0; out_fail: - return -1; + return -1; } - - -static void PIOS_USB_HID_SendReport(struct pios_usb_hid_dev * usb_hid_dev) +static void PIOS_USB_HID_SendReport(struct pios_usb_hid_dev *usb_hid_dev) { - uint16_t bytes_to_tx; + uint16_t bytes_to_tx; - if (!usb_hid_dev->tx_out_cb) { - return; - } + if (!usb_hid_dev->tx_out_cb) { + return; + } - bool need_yield = false; + bool need_yield = false; #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, - &usb_hid_dev->tx_packet_buffer[1], - sizeof(usb_hid_dev->tx_packet_buffer)-1, - NULL, - &need_yield); + bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, + &usb_hid_dev->tx_packet_buffer[1], + sizeof(usb_hid_dev->tx_packet_buffer) - 1, + NULL, + &need_yield); #else - bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, - &usb_hid_dev->tx_packet_buffer[2], - sizeof(usb_hid_dev->tx_packet_buffer)-2, - NULL, - &need_yield); + bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, + &usb_hid_dev->tx_packet_buffer[2], + sizeof(usb_hid_dev->tx_packet_buffer) - 2, + NULL, + &need_yield); #endif - if (bytes_to_tx == 0) { - return; - } + if (bytes_to_tx == 0) { + return; + } - /* Always set type as report ID */ - usb_hid_dev->tx_packet_buffer[0] = 1; + /* Always set type as report ID */ + usb_hid_dev->tx_packet_buffer[0] = 1; #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - UserToPMABufferCopy(usb_hid_dev->tx_packet_buffer, - GetEPTxAddr(usb_hid_dev->cfg->data_tx_ep), - bytes_to_tx + 1); + UserToPMABufferCopy(usb_hid_dev->tx_packet_buffer, + GetEPTxAddr(usb_hid_dev->cfg->data_tx_ep), + bytes_to_tx + 1); #else - usb_hid_dev->tx_packet_buffer[1] = bytes_to_tx; - UserToPMABufferCopy(usb_hid_dev->tx_packet_buffer, - GetEPTxAddr(usb_hid_dev->cfg->data_tx_ep), - bytes_to_tx + 2); + usb_hid_dev->tx_packet_buffer[1] = bytes_to_tx; + UserToPMABufferCopy(usb_hid_dev->tx_packet_buffer, + GetEPTxAddr(usb_hid_dev->cfg->data_tx_ep), + bytes_to_tx + 2); #endif - /* Is this correct? Why do we always send the whole buffer? */ - SetEPTxCount(usb_hid_dev->cfg->data_tx_ep, sizeof(usb_hid_dev->tx_packet_buffer)); - SetEPTxValid(usb_hid_dev->cfg->data_tx_ep); + /* Is this correct? Why do we always send the whole buffer? */ + SetEPTxCount(usb_hid_dev->cfg->data_tx_ep, sizeof(usb_hid_dev->tx_packet_buffer)); + SetEPTxValid(usb_hid_dev->cfg->data_tx_ep); #ifdef PIOS_INCLUDE_FREERTOS - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } -static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; +static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) +{ + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { - return; - } + PIOS_Assert(valid); - // If endpoint was stalled and there is now space make it valid + if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { + return; + } + + // If endpoint was stalled and there is now space make it valid #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; #else - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; #endif - PIOS_IRQ_Disable(); - if ((GetEPRxStatus(usb_hid_dev->cfg->data_rx_ep) != EP_RX_VALID) && - (rx_bytes_avail >= max_payload_length)) { - SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_VALID); - } - PIOS_IRQ_Enable(); + PIOS_IRQ_Disable(); + if ((GetEPRxStatus(usb_hid_dev->cfg->data_rx_ep) != EP_RX_VALID) && + (rx_bytes_avail >= max_payload_length)) { + SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_VALID); + } + PIOS_IRQ_Enable(); } static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, __attribute__((unused)) uint16_t tx_bytes_avail) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { - return; - } + PIOS_Assert(valid); - if (GetEPTxStatus(usb_hid_dev->cfg->data_tx_ep) == EP_TX_VALID) { - /* Endpoint is already transmitting */ - return; - } + if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { + return; + } - PIOS_USB_HID_SendReport(usb_hid_dev); + if (GetEPTxStatus(usb_hid_dev->cfg->data_tx_ep) == EP_TX_VALID) { + /* Endpoint is already transmitting */ + return; + } + + PIOS_USB_HID_SendReport(usb_hid_dev); } static void PIOS_USB_HID_RegisterRxCallback(uint32_t usbhid_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_hid_dev->rx_in_context = context; - usb_hid_dev->rx_in_cb = rx_in_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_hid_dev->rx_in_context = context; + usb_hid_dev->rx_in_cb = rx_in_cb; } static void PIOS_USB_HID_RegisterTxCallback(uint32_t usbhid_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_hid_dev->tx_out_context = context; - usb_hid_dev->tx_out_cb = tx_out_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_hid_dev->tx_out_context = context; + usb_hid_dev->tx_out_cb = tx_out_cb; } /** @@ -278,16 +285,17 @@ static void PIOS_USB_HID_RegisterTxCallback(uint32_t usbhid_id, pios_com_callbac */ static void PIOS_USB_HID_EP_IN_Callback(void) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)pios_usb_hid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)pios_usb_hid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { - return; - } + PIOS_Assert(valid); - PIOS_USB_HID_SendReport(usb_hid_dev); + if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { + return; + } + + PIOS_USB_HID_SendReport(usb_hid_dev); } /** @@ -295,68 +303,68 @@ static void PIOS_USB_HID_EP_IN_Callback(void) */ static void PIOS_USB_HID_EP_OUT_Callback(void) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)pios_usb_hid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)pios_usb_hid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - uint32_t DataLength; + PIOS_Assert(valid); - /* Read received data (63 bytes) */ - /* Get the number of received data on the selected Endpoint */ - DataLength = GetEPRxCount(usb_hid_dev->cfg->data_rx_ep); - if (DataLength > sizeof(usb_hid_dev->rx_packet_buffer)) { - DataLength = sizeof(usb_hid_dev->rx_packet_buffer); - } + uint32_t DataLength; - /* Use the memory interface function to read from the selected endpoint */ - PMAToUserBufferCopy((uint8_t *) usb_hid_dev->rx_packet_buffer, - GetEPRxAddr(usb_hid_dev->cfg->data_rx_ep), - DataLength); + /* Read received data (63 bytes) */ + /* Get the number of received data on the selected Endpoint */ + DataLength = GetEPRxCount(usb_hid_dev->cfg->data_rx_ep); + if (DataLength > sizeof(usb_hid_dev->rx_packet_buffer)) { + DataLength = sizeof(usb_hid_dev->rx_packet_buffer); + } - if (!usb_hid_dev->rx_in_cb) { - /* No Rx call back registered, disable the receiver */ - SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_NAK); - return; - } + /* Use the memory interface function to read from the selected endpoint */ + PMAToUserBufferCopy((uint8_t *)usb_hid_dev->rx_packet_buffer, + GetEPRxAddr(usb_hid_dev->cfg->data_rx_ep), + DataLength); - /* The first byte is report ID (not checked), the second byte is the valid data length */ - uint16_t headroom; - bool need_yield = false; + if (!usb_hid_dev->rx_in_cb) { + /* No Rx call back registered, disable the receiver */ + SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_NAK); + return; + } + + /* The first byte is report ID (not checked), the second byte is the valid data length */ + uint16_t headroom; + bool need_yield = false; #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, - &usb_hid_dev->rx_packet_buffer[1], - sizeof(usb_hid_dev->rx_packet_buffer)-1, - &headroom, - &need_yield); + (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, + &usb_hid_dev->rx_packet_buffer[1], + sizeof(usb_hid_dev->rx_packet_buffer) - 1, + &headroom, + &need_yield); #else - (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, - &usb_hid_dev->rx_packet_buffer[2], - usb_hid_dev->rx_packet_buffer[1], - &headroom, - &need_yield); + (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, + &usb_hid_dev->rx_packet_buffer[2], + usb_hid_dev->rx_packet_buffer[1], + &headroom, + &need_yield); #endif #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; #else - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; #endif - if (headroom >= max_payload_length) { - - /* We have room for a maximum length message */ - SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_VALID); - } else { - /* Not enough room left for a message, apply backpressure */ - SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_NAK); - } + if (headroom >= max_payload_length) { + /* We have room for a maximum length message */ + SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_VALID); + } else { + /* Not enough room left for a message, apply backpressure */ + SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_NAK); + } #ifdef PIOS_INCLUDE_FREERTOS - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ diff --git a/flight/pios/stm32f10x/pios_usb_hid_istr.c b/flight/pios/stm32f10x/pios_usb_hid_istr.c index e26dd0b4f..30492c02f 100644 --- a/flight/pios/stm32f10x/pios_usb_hid_istr.c +++ b/flight/pios/stm32f10x/pios_usb_hid_istr.c @@ -27,18 +27,20 @@ /* Private define ------------------------------------------------------------*/ /* Private macro -------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ -__IO uint16_t wIstr; /* ISTR register last read value */ -__IO uint8_t bIntPackSOF = 0; /* SOFs received between 2 consecutive packets */ +__IO uint16_t wIstr; /* ISTR register last read value */ +__IO uint8_t bIntPackSOF = 0; /* SOFs received between 2 consecutive packets */ /* Extern variables ----------------------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/ /* Private functions ---------------------------------------------------------*/ /* function pointers to non-control endpoints service routines */ -void (*pEpInt_IN[7]) (void) = { -EP1_IN_Callback, EP2_IN_Callback, EP3_IN_Callback, EP4_IN_Callback, EP5_IN_Callback, EP6_IN_Callback, EP7_IN_Callback,}; +void(*pEpInt_IN[7]) (void) = { + EP1_IN_Callback, EP2_IN_Callback, EP3_IN_Callback, EP4_IN_Callback, EP5_IN_Callback, EP6_IN_Callback, EP7_IN_Callback, +}; -void (*pEpInt_OUT[7]) (void) = { -EP1_OUT_Callback, EP2_OUT_Callback, EP3_OUT_Callback, EP4_OUT_Callback, EP5_OUT_Callback, EP6_OUT_Callback, EP7_OUT_Callback,}; +void(*pEpInt_OUT[7]) (void) = { + EP1_OUT_Callback, EP2_OUT_Callback, EP3_OUT_Callback, EP4_OUT_Callback, EP5_OUT_Callback, EP6_OUT_Callback, EP7_OUT_Callback, +}; #ifndef STM32F10X_CL @@ -49,101 +51,99 @@ EP1_OUT_Callback, EP2_OUT_Callback, EP3_OUT_Callback, EP4_OUT_Callback, EP5_OUT_ * Output : * Return : *******************************************************************************/ -void USB_LP_CAN1_RX0_IRQHandler(void) //USB_Istr(void) +void USB_LP_CAN1_RX0_IRQHandler(void) // USB_Istr(void) { - - wIstr = _GetISTR(); + wIstr = _GetISTR(); #if (IMR_MSK & ISTR_CTR) - if (wIstr & ISTR_CTR & wInterrupt_Mask) { - /* servicing of the endpoint correct transfer interrupt */ - /* clear of the CTR flag into the sub */ - CTR_LP(); + if (wIstr & ISTR_CTR & wInterrupt_Mask) { + /* servicing of the endpoint correct transfer interrupt */ + /* clear of the CTR flag into the sub */ + CTR_LP(); #ifdef CTR_CALLBACK - CTR_Callback(); + CTR_Callback(); #endif - } + } #endif - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #if (IMR_MSK & ISTR_RESET) - if (wIstr & ISTR_RESET & wInterrupt_Mask) { - _SetISTR((uint16_t) CLR_RESET); - Device_Property.Reset(); + if (wIstr & ISTR_RESET & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_RESET); + Device_Property.Reset(); #ifdef RESET_CALLBACK - RESET_Callback(); + RESET_Callback(); #endif - } + } #endif - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #if (IMR_MSK & ISTR_DOVR) - if (wIstr & ISTR_DOVR & wInterrupt_Mask) { - _SetISTR((uint16_t) CLR_DOVR); + if (wIstr & ISTR_DOVR & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_DOVR); #ifdef DOVR_CALLBACK - DOVR_Callback(); + DOVR_Callback(); #endif - } + } #endif - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #if (IMR_MSK & ISTR_ERR) - if (wIstr & ISTR_ERR & wInterrupt_Mask) { - _SetISTR((uint16_t) CLR_ERR); + if (wIstr & ISTR_ERR & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_ERR); #ifdef ERR_CALLBACK - ERR_Callback(); + ERR_Callback(); #endif - } + } #endif - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #if (IMR_MSK & ISTR_WKUP) - if (wIstr & ISTR_WKUP & wInterrupt_Mask) { - _SetISTR((uint16_t) CLR_WKUP); - Resume(RESUME_EXTERNAL); + if (wIstr & ISTR_WKUP & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_WKUP); + Resume(RESUME_EXTERNAL); #ifdef WKUP_CALLBACK - WKUP_Callback(); + WKUP_Callback(); #endif - } + } #endif - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #if (IMR_MSK & ISTR_SUSP) - if (wIstr & ISTR_SUSP & wInterrupt_Mask) { - - /* check if SUSPEND is possible */ - if (fSuspendEnabled) { - Suspend(); - } else { - /* if not possible then resume after xx ms */ - Resume(RESUME_LATER); - } - /* clear of the ISTR bit must be done after setting of CNTR_FSUSP */ - _SetISTR((uint16_t) CLR_SUSP); + if (wIstr & ISTR_SUSP & wInterrupt_Mask) { + /* check if SUSPEND is possible */ + if (fSuspendEnabled) { + Suspend(); + } else { + /* if not possible then resume after xx ms */ + Resume(RESUME_LATER); + } + /* clear of the ISTR bit must be done after setting of CNTR_FSUSP */ + _SetISTR((uint16_t)CLR_SUSP); #ifdef SUSP_CALLBACK - SUSP_Callback(); + SUSP_Callback(); #endif - } + } #endif - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #if (IMR_MSK & ISTR_SOF) - if (wIstr & ISTR_SOF & wInterrupt_Mask) { - _SetISTR((uint16_t) CLR_SOF); - bIntPackSOF++; + if (wIstr & ISTR_SOF & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_SOF); + bIntPackSOF++; #ifdef SOF_CALLBACK - SOF_Callback(); + SOF_Callback(); #endif - } + } #endif - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #if (IMR_MSK & ISTR_ESOF) - if (wIstr & ISTR_ESOF & wInterrupt_Mask) { - _SetISTR((uint16_t) CLR_ESOF); - /* resume handling timing is made with ESOFs */ - Resume(RESUME_ESOF); /* request without change of the machine state */ + if (wIstr & ISTR_ESOF & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_ESOF); + /* resume handling timing is made with ESOFs */ + Resume(RESUME_ESOF); /* request without change of the machine state */ #ifdef ESOF_CALLBACK - ESOF_Callback(); + ESOF_Callback(); #endif - } + } #endif -} /* USB_Istr */ +} /* USB_Istr */ /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #else /* STM32F10X_CL */ @@ -157,177 +157,176 @@ void USB_LP_CAN1_RX0_IRQHandler(void) //USB_Istr(void) *******************************************************************************/ u32 STM32_PCD_OTG_ISR_Handler(void) { - USB_OTG_GINTSTS_TypeDef gintr_status; - u32 retval = 0; + USB_OTG_GINTSTS_TypeDef gintr_status; + u32 retval = 0; - if (USBD_FS_IsDeviceMode()) { /* ensure that we are in device mode */ - gintr_status.d32 = OTGD_FS_ReadCoreItr(); + if (USBD_FS_IsDeviceMode()) { /* ensure that we are in device mode */ + gintr_status.d32 = OTGD_FS_ReadCoreItr(); - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* If there is no interrupt pending exit the interrupt routine */ - if (!gintr_status.d32) { - return 0; - } + /* If there is no interrupt pending exit the interrupt routine */ + if (!gintr_status.d32) { + return 0; + } - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Early Suspend interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Early Suspend interrupt */ #ifdef INTR_ERLYSUSPEND - if (gintr_status.b.erlysuspend) { - retval |= OTGD_FS_Handle_EarlySuspend_ISR(); - } + if (gintr_status.b.erlysuspend) { + retval |= OTGD_FS_Handle_EarlySuspend_ISR(); + } #endif /* INTR_ERLYSUSPEND */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* End of Periodic Frame interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* End of Periodic Frame interrupt */ #ifdef INTR_EOPFRAME - if (gintr_status.b.eopframe) { - retval |= OTGD_FS_Handle_EOPF_ISR(); - } + if (gintr_status.b.eopframe) { + retval |= OTGD_FS_Handle_EOPF_ISR(); + } #endif /* INTR_EOPFRAME */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Non Periodic Tx FIFO Emty interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Non Periodic Tx FIFO Emty interrupt */ #ifdef INTR_NPTXFEMPTY - if (gintr_status.b.nptxfempty) { - retval |= OTGD_FS_Handle_NPTxFE_ISR(); - } + if (gintr_status.b.nptxfempty) { + retval |= OTGD_FS_Handle_NPTxFE_ISR(); + } #endif /* INTR_NPTXFEMPTY */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Wakeup or RemoteWakeup interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Wakeup or RemoteWakeup interrupt */ #ifdef INTR_WKUPINTR - if (gintr_status.b.wkupintr) { - retval |= OTGD_FS_Handle_Wakeup_ISR(); - } + if (gintr_status.b.wkupintr) { + retval |= OTGD_FS_Handle_Wakeup_ISR(); + } #endif /* INTR_WKUPINTR */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Suspend interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Suspend interrupt */ #ifdef INTR_USBSUSPEND - if (gintr_status.b.usbsuspend) { - /* check if SUSPEND is possible */ - if (fSuspendEnabled) { - Suspend(); - } else { - /* if not possible then resume after xx ms */ - Resume(RESUME_LATER); /* This case shouldn't happen in OTG Device mode because - there's no ESOF interrupt to increment the ResumeS.bESOFcnt in the Resume state machine */ - } + if (gintr_status.b.usbsuspend) { + /* check if SUSPEND is possible */ + if (fSuspendEnabled) { + Suspend(); + } else { + /* if not possible then resume after xx ms */ + Resume(RESUME_LATER); /* This case shouldn't happen in OTG Device mode because + there's no ESOF interrupt to increment the ResumeS.bESOFcnt in the Resume state machine */ + } - retval |= OTGD_FS_Handle_USBSuspend_ISR(); - } + retval |= OTGD_FS_Handle_USBSuspend_ISR(); + } #endif /* INTR_USBSUSPEND */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Start of Frame interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Start of Frame interrupt */ #ifdef INTR_SOFINTR - if (gintr_status.b.sofintr) { - /* Update the frame number variable */ - bIntPackSOF++; + if (gintr_status.b.sofintr) { + /* Update the frame number variable */ + bIntPackSOF++; - retval |= OTGD_FS_Handle_Sof_ISR(); - } + retval |= OTGD_FS_Handle_Sof_ISR(); + } #endif /* INTR_SOFINTR */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Receive FIFO Queue Status Level interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Receive FIFO Queue Status Level interrupt */ #ifdef INTR_RXSTSQLVL - if (gintr_status.b.rxstsqlvl) { - retval |= OTGD_FS_Handle_RxStatusQueueLevel_ISR(); - } + if (gintr_status.b.rxstsqlvl) { + retval |= OTGD_FS_Handle_RxStatusQueueLevel_ISR(); + } #endif /* INTR_RXSTSQLVL */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Enumeration Done interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Enumeration Done interrupt */ #ifdef INTR_ENUMDONE - if (gintr_status.b.enumdone) { - retval |= OTGD_FS_Handle_EnumDone_ISR(); - } + if (gintr_status.b.enumdone) { + retval |= OTGD_FS_Handle_EnumDone_ISR(); + } #endif /* INTR_ENUMDONE */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Reset interrutp */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Reset interrutp */ #ifdef INTR_USBRESET - if (gintr_status.b.usbreset) { - retval |= OTGD_FS_Handle_UsbReset_ISR(); - } + if (gintr_status.b.usbreset) { + retval |= OTGD_FS_Handle_UsbReset_ISR(); + } #endif /* INTR_USBRESET */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* IN Endpoint interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* IN Endpoint interrupt */ #ifdef INTR_INEPINTR - if (gintr_status.b.inepint) { - retval |= OTGD_FS_Handle_InEP_ISR(); - } + if (gintr_status.b.inepint) { + retval |= OTGD_FS_Handle_InEP_ISR(); + } #endif /* INTR_INEPINTR */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* OUT Endpoint interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* OUT Endpoint interrupt */ #ifdef INTR_OUTEPINTR - if (gintr_status.b.outepintr) { - retval |= OTGD_FS_Handle_OutEP_ISR(); - } + if (gintr_status.b.outepintr) { + retval |= OTGD_FS_Handle_OutEP_ISR(); + } #endif /* INTR_OUTEPINTR */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Mode Mismatch interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Mode Mismatch interrupt */ #ifdef INTR_MODEMISMATCH - if (gintr_status.b.modemismatch) { - retval |= OTGD_FS_Handle_ModeMismatch_ISR(); - } + if (gintr_status.b.modemismatch) { + retval |= OTGD_FS_Handle_ModeMismatch_ISR(); + } #endif /* INTR_MODEMISMATCH */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Global IN Endpoints NAK Effective interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Global IN Endpoints NAK Effective interrupt */ #ifdef INTR_GINNAKEFF - if (gintr_status.b.ginnakeff) { - retval |= OTGD_FS_Handle_GInNakEff_ISR(); - } + if (gintr_status.b.ginnakeff) { + retval |= OTGD_FS_Handle_GInNakEff_ISR(); + } #endif /* INTR_GINNAKEFF */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Global OUT Endpoints NAK effective interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Global OUT Endpoints NAK effective interrupt */ #ifdef INTR_GOUTNAKEFF - if (gintr_status.b.goutnakeff) { - retval |= OTGD_FS_Handle_GOutNakEff_ISR(); - } + if (gintr_status.b.goutnakeff) { + retval |= OTGD_FS_Handle_GOutNakEff_ISR(); + } #endif /* INTR_GOUTNAKEFF */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Isochrounous Out packet Dropped interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Isochrounous Out packet Dropped interrupt */ #ifdef INTR_ISOOUTDROP - if (gintr_status.b.isooutdrop) { - retval |= OTGD_FS_Handle_IsoOutDrop_ISR(); - } + if (gintr_status.b.isooutdrop) { + retval |= OTGD_FS_Handle_IsoOutDrop_ISR(); + } #endif /* INTR_ISOOUTDROP */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Endpoint Mismatch error interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Endpoint Mismatch error interrupt */ #ifdef INTR_EPMISMATCH - if (gintr_status.b.epmismatch) { - retval |= OTGD_FS_Handle_EPMismatch_ISR(); - } + if (gintr_status.b.epmismatch) { + retval |= OTGD_FS_Handle_EPMismatch_ISR(); + } #endif /* INTR_EPMISMATCH */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Incomplete Isochrous IN tranfer error interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Incomplete Isochrous IN tranfer error interrupt */ #ifdef INTR_INCOMPLISOIN - if (gintr_status.b.incomplisoin) { - retval |= OTGD_FS_Handle_IncomplIsoIn_ISR(); - } + if (gintr_status.b.incomplisoin) { + retval |= OTGD_FS_Handle_IncomplIsoIn_ISR(); + } #endif /* INTR_INCOMPLISOIN */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Incomplete Isochrous OUT tranfer error interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Incomplete Isochrous OUT tranfer error interrupt */ #ifdef INTR_INCOMPLISOOUT - if (gintr_status.b.outepintr) { - retval |= OTGD_FS_Handle_IncomplIsoOut_ISR(); - } + if (gintr_status.b.outepintr) { + retval |= OTGD_FS_Handle_IncomplIsoOut_ISR(); + } #endif /* INTR_INCOMPLISOOUT */ - - } - return retval; + } + return retval; } #endif /* STM32F10X_CL */ diff --git a/flight/pios/stm32f10x/pios_usb_hid_pwr.c b/flight/pios/stm32f10x/pios_usb_hid_pwr.c index 888f2d748..ffa9c3495 100644 --- a/flight/pios/stm32f10x/pios_usb_hid_pwr.c +++ b/flight/pios/stm32f10x/pios_usb_hid_pwr.c @@ -24,12 +24,12 @@ /* Private define ------------------------------------------------------------*/ /* Private macro -------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ -__IO uint32_t bDeviceState = UNCONNECTED; /* USB device status */ -__IO bool fSuspendEnabled = TRUE; /* true when suspend is possible */ +__IO uint32_t bDeviceState = UNCONNECTED; /* USB device status */ +__IO bool fSuspendEnabled = TRUE; /* true when suspend is possible */ struct { - __IO RESUME_STATE eState; - __IO uint8_t bESOFcnt; + __IO RESUME_STATE eState; + __IO uint8_t bESOFcnt; } ResumeS; /* Extern variables ----------------------------------------------------------*/ @@ -38,15 +38,14 @@ struct { /* Private functions ---------------------------------------------------------*/ /******************************************************************************* - * Function Name : USB_Cable_Config. - * Description : Software Connection/Disconnection of USB Cable. - * Input : NewState: new state. - * Output : None. - * Return : None - *******************************************************************************/ +* Function Name : USB_Cable_Config. +* Description : Software Connection/Disconnection of USB Cable. +* Input : NewState: new state. +* Output : None. +* Return : None +*******************************************************************************/ void USB_Cable_Config(__attribute__((unused)) FunctionalState NewState) -{ -} +{} /******************************************************************************* * Function Name : PowerOn @@ -58,26 +57,26 @@ void USB_Cable_Config(__attribute__((unused)) FunctionalState NewState) RESULT PowerOn(void) { #ifndef STM32F10X_CL - uint16_t wRegVal; + uint16_t wRegVal; - /*** cable plugged-in ? ***/ - USB_Cable_Config(ENABLE); + /*** cable plugged-in ? ***/ + USB_Cable_Config(ENABLE); - /*** CNTR_PWDN = 0 ***/ - wRegVal = CNTR_FRES; - _SetCNTR(wRegVal); + /*** CNTR_PWDN = 0 ***/ + wRegVal = CNTR_FRES; + _SetCNTR(wRegVal); - /*** CNTR_FRES = 0 ***/ - wInterrupt_Mask = 0; - _SetCNTR(wInterrupt_Mask); - /*** Clear pending interrupts ***/ - _SetISTR(0); - /*** Set interrupt mask ***/ - wInterrupt_Mask = CNTR_RESETM | CNTR_SUSPM | CNTR_WKUPM; - _SetCNTR(wInterrupt_Mask); + /*** CNTR_FRES = 0 ***/ + wInterrupt_Mask = 0; + _SetCNTR(wInterrupt_Mask); + /*** Clear pending interrupts ***/ + _SetISTR(0); + /*** Set interrupt mask ***/ + wInterrupt_Mask = CNTR_RESETM | CNTR_SUSPM | CNTR_WKUPM; + _SetCNTR(wInterrupt_Mask); #endif /* STM32F10X_CL */ - return USB_SUCCESS; + return USB_SUCCESS; } /******************************************************************************* @@ -90,33 +89,33 @@ RESULT PowerOn(void) RESULT PowerOff() { #ifndef STM32F10X_CL - /* disable all ints and force USB reset */ - _SetCNTR(CNTR_FRES); - /* clear interrupt status register */ - _SetISTR(0); - /* Disable the Pull-Up */ - USB_Cable_Config(DISABLE); - /* switch-off device */ - _SetCNTR(CNTR_FRES + CNTR_PDWN); + /* disable all ints and force USB reset */ + _SetCNTR(CNTR_FRES); + /* clear interrupt status register */ + _SetISTR(0); + /* Disable the Pull-Up */ + USB_Cable_Config(DISABLE); + /* switch-off device */ + _SetCNTR(CNTR_FRES + CNTR_PDWN); #endif /* STM32F10X_CL */ - /* sw variables reset */ - /* ... */ + /* sw variables reset */ + /* ... */ - return USB_SUCCESS; + return USB_SUCCESS; } /******************************************************************************* - * Function Name : Enter_LowPowerMode. - * Description : Power-off system clocks and power while entering suspend mode. - * Input : None. - * Output : None. - * Return : None. - *******************************************************************************/ +* Function Name : Enter_LowPowerMode. +* Description : Power-off system clocks and power while entering suspend mode. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ void Enter_LowPowerMode(void) { - /* Set the device state to suspend */ - bDeviceState = SUSPENDED; + /* Set the device state to suspend */ + bDeviceState = SUSPENDED; } /******************************************************************************* @@ -129,51 +128,50 @@ void Enter_LowPowerMode(void) void Suspend(void) { #ifndef STM32F10X_CL - uint16_t wCNTR; - /* suspend preparation */ - /* ... */ + uint16_t wCNTR; + /* suspend preparation */ + /* ... */ - /* macrocell enters suspend mode */ - wCNTR = _GetCNTR(); - wCNTR |= CNTR_FSUSP; - _SetCNTR(wCNTR); + /* macrocell enters suspend mode */ + wCNTR = _GetCNTR(); + wCNTR |= CNTR_FSUSP; + _SetCNTR(wCNTR); #endif /* STM32F10X_CL */ - /* ------------------ ONLY WITH BUS-POWERED DEVICES ---------------------- */ - /* power reduction */ - /* ... on connected devices */ + /* ------------------ ONLY WITH BUS-POWERED DEVICES ---------------------- */ + /* power reduction */ + /* ... on connected devices */ #ifndef STM32F10X_CL - /* force low-power mode in the macrocell */ - wCNTR = _GetCNTR(); - wCNTR |= CNTR_LPMODE; - _SetCNTR(wCNTR); + /* force low-power mode in the macrocell */ + wCNTR = _GetCNTR(); + wCNTR |= CNTR_LPMODE; + _SetCNTR(wCNTR); #endif /* STM32F10X_CL */ - /* switch-off the clocks */ - /* ... */ - Enter_LowPowerMode(); - + /* switch-off the clocks */ + /* ... */ + Enter_LowPowerMode(); } /******************************************************************************* - * Function Name : Leave_LowPowerMode. - * Description : Restores system clocks and power while exiting suspend mode. - * Input : None. - * Output : None. - * Return : None. - *******************************************************************************/ +* Function Name : Leave_LowPowerMode. +* Description : Restores system clocks and power while exiting suspend mode. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ void Leave_LowPowerMode(void) { - DEVICE_INFO *pInfo = &Device_Info; + DEVICE_INFO *pInfo = &Device_Info; - /* Set the device state to the correct state */ - if (pInfo->Current_Configuration != 0) { - /* Device configured */ - bDeviceState = CONFIGURED; - } else { - bDeviceState = ATTACHED; - } + /* Set the device state to the correct state */ + if (pInfo->Current_Configuration != 0) { + /* Device configured */ + bDeviceState = CONFIGURED; + } else { + bDeviceState = ATTACHED; + } } /******************************************************************************* @@ -186,32 +184,31 @@ void Leave_LowPowerMode(void) void Resume_Init(void) { #ifndef STM32F10X_CL - uint16_t wCNTR; + uint16_t wCNTR; #endif /* STM32F10X_CL */ - /* ------------------ ONLY WITH BUS-POWERED DEVICES ---------------------- */ - /* restart the clocks */ - /* ... */ + /* ------------------ ONLY WITH BUS-POWERED DEVICES ---------------------- */ + /* restart the clocks */ + /* ... */ #ifndef STM32F10X_CL - /* CNTR_LPMODE = 0 */ - wCNTR = _GetCNTR(); - wCNTR &= (~CNTR_LPMODE); - _SetCNTR(wCNTR); + /* CNTR_LPMODE = 0 */ + wCNTR = _GetCNTR(); + wCNTR &= (~CNTR_LPMODE); + _SetCNTR(wCNTR); #endif /* STM32F10X_CL */ - /* restore full power */ - /* ... on connected devices */ - Leave_LowPowerMode(); + /* restore full power */ + /* ... on connected devices */ + Leave_LowPowerMode(); #ifndef STM32F10X_CL - /* reset FSUSP bit */ - _SetCNTR(IMR_MSK); + /* reset FSUSP bit */ + _SetCNTR(IMR_MSK); #endif /* STM32F10X_CL */ - /* reverse suspend preparation */ - /* ... */ - + /* reverse suspend preparation */ + /* ... */ } /******************************************************************************* @@ -229,64 +226,66 @@ void Resume_Init(void) void Resume(RESUME_STATE eResumeSetVal) { #ifndef STM32F10X_CL - uint16_t wCNTR; + uint16_t wCNTR; #endif /* STM32F10X_CL */ - if (eResumeSetVal != RESUME_ESOF) - ResumeS.eState = eResumeSetVal; + if (eResumeSetVal != RESUME_ESOF) { + ResumeS.eState = eResumeSetVal; + } - switch (ResumeS.eState) { - case RESUME_EXTERNAL: - Resume_Init(); - ResumeS.eState = RESUME_OFF; - break; - case RESUME_INTERNAL: - Resume_Init(); - ResumeS.eState = RESUME_START; - break; - case RESUME_LATER: - ResumeS.bESOFcnt = 2; - ResumeS.eState = RESUME_WAIT; - break; - case RESUME_WAIT: - ResumeS.bESOFcnt--; - if (ResumeS.bESOFcnt == 0) - ResumeS.eState = RESUME_START; - break; - case RESUME_START: + switch (ResumeS.eState) { + case RESUME_EXTERNAL: + Resume_Init(); + ResumeS.eState = RESUME_OFF; + break; + case RESUME_INTERNAL: + Resume_Init(); + ResumeS.eState = RESUME_START; + break; + case RESUME_LATER: + ResumeS.bESOFcnt = 2; + ResumeS.eState = RESUME_WAIT; + break; + case RESUME_WAIT: + ResumeS.bESOFcnt--; + if (ResumeS.bESOFcnt == 0) { + ResumeS.eState = RESUME_START; + } + break; + case RESUME_START: #ifdef STM32F10X_CL - OTGD_FS_SetRemoteWakeup(); + OTGD_FS_SetRemoteWakeup(); #else - wCNTR = _GetCNTR(); - wCNTR |= CNTR_RESUME; - _SetCNTR(wCNTR); + wCNTR = _GetCNTR(); + wCNTR |= CNTR_RESUME; + _SetCNTR(wCNTR); #endif /* STM32F10X_CL */ - ResumeS.eState = RESUME_ON; - ResumeS.bESOFcnt = 10; - break; - case RESUME_ON: + ResumeS.eState = RESUME_ON; + ResumeS.bESOFcnt = 10; + break; + case RESUME_ON: #ifndef STM32F10X_CL - ResumeS.bESOFcnt--; - if (ResumeS.bESOFcnt == 0) { + ResumeS.bESOFcnt--; + if (ResumeS.bESOFcnt == 0) { #endif /* STM32F10X_CL */ #ifdef STM32F10X_CL - OTGD_FS_ResetRemoteWakeup(); + OTGD_FS_ResetRemoteWakeup(); #else - wCNTR = _GetCNTR(); - wCNTR &= (~CNTR_RESUME); - _SetCNTR(wCNTR); + wCNTR = _GetCNTR(); + wCNTR &= (~CNTR_RESUME); + _SetCNTR(wCNTR); #endif /* STM32F10X_CL */ - ResumeS.eState = RESUME_OFF; + ResumeS.eState = RESUME_OFF; #ifndef STM32F10X_CL - } + } #endif /* STM32F10X_CL */ - break; - case RESUME_OFF: - case RESUME_ESOF: - default: - ResumeS.eState = RESUME_OFF; - break; - } + break; + case RESUME_OFF: + case RESUME_ESOF: + default: + ResumeS.eState = RESUME_OFF; + break; + } } /******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f10x/pios_usb_rctx.c b/flight/pios/stm32f10x/pios_usb_rctx.c index 11e333295..3fc57dcc8 100644 --- a/flight/pios/stm32f10x/pios_usb_rctx.c +++ b/flight/pios/stm32f10x/pios_usb_rctx.c @@ -40,170 +40,180 @@ #define PIOS_USB_RCTX_NUM_CHANNELS 8 enum pios_usb_rctx_dev_magic { - PIOS_USB_RCTX_DEV_MAGIC = 0xAB98B745, + PIOS_USB_RCTX_DEV_MAGIC = 0xAB98B745, }; struct pios_usb_rctx_dev { - enum pios_usb_rctx_dev_magic magic; - const struct pios_usb_rctx_cfg * cfg; + enum pios_usb_rctx_dev_magic magic; + const struct pios_usb_rctx_cfg *cfg; - uint32_t lower_id; + uint32_t lower_id; - struct { - uint8_t id; - uint16_t vals[PIOS_USB_RCTX_NUM_CHANNELS]; - } __attribute__((packed)) report; + struct { + uint8_t id; + uint16_t vals[PIOS_USB_RCTX_NUM_CHANNELS]; + } __attribute__((packed)) report; }; -static bool PIOS_USB_RCTX_validate(struct pios_usb_rctx_dev * usb_rctx_dev) +static bool PIOS_USB_RCTX_validate(struct pios_usb_rctx_dev *usb_rctx_dev) { - return (usb_rctx_dev->magic == PIOS_USB_RCTX_DEV_MAGIC); + return usb_rctx_dev->magic == PIOS_USB_RCTX_DEV_MAGIC; } #ifdef PIOS_INCLUDE_FREERTOS -static struct pios_usb_rctx_dev * PIOS_USB_RCTX_alloc(void) +static struct pios_usb_rctx_dev *PIOS_USB_RCTX_alloc(void) { - struct pios_usb_rctx_dev * usb_rctx_dev; + struct pios_usb_rctx_dev *usb_rctx_dev; - usb_rctx_dev = (struct pios_usb_rctx_dev *)pvPortMalloc(sizeof(*usb_rctx_dev)); - if (!usb_rctx_dev) return(NULL); + usb_rctx_dev = (struct pios_usb_rctx_dev *)pvPortMalloc(sizeof(*usb_rctx_dev)); + if (!usb_rctx_dev) { + return NULL; + } - usb_rctx_dev->magic = PIOS_USB_RCTX_DEV_MAGIC; - return(usb_rctx_dev); + usb_rctx_dev->magic = PIOS_USB_RCTX_DEV_MAGIC; + return usb_rctx_dev; } #else static struct pios_usb_rctx_dev pios_usb_rctx_devs[PIOS_USB_RCTX_MAX_DEVS]; static uint8_t pios_usb_rctx_num_devs; -static struct pios_usb_rctx_dev * PIOS_USB_RCTX_alloc(void) +static struct pios_usb_rctx_dev *PIOS_USB_RCTX_alloc(void) { - struct pios_usb_rctx_dev * usb_rctx_dev; + struct pios_usb_rctx_dev *usb_rctx_dev; - if (pios_usb_rctx_num_devs >= PIOS_USB_RCTX_MAX_DEVS) { - return (NULL); - } + if (pios_usb_rctx_num_devs >= PIOS_USB_RCTX_MAX_DEVS) { + return NULL; + } - usb_rctx_dev = &pios_usb_rctx_devs[pios_usb_rctx_num_devs++]; - usb_rctx_dev->magic = PIOS_USB_RCTX_DEV_MAGIC; + usb_rctx_dev = &pios_usb_rctx_devs[pios_usb_rctx_num_devs++]; + usb_rctx_dev->magic = PIOS_USB_RCTX_DEV_MAGIC; - return (usb_rctx_dev); + return usb_rctx_dev; } -#endif +#endif /* ifdef PIOS_INCLUDE_FREERTOS */ static void PIOS_USB_RCTX_EP_IN_Callback(void); -static void PIOS_USB_RCTX_SendReport(struct pios_usb_rctx_dev * usb_rctx_dev); +static void PIOS_USB_RCTX_SendReport(struct pios_usb_rctx_dev *usb_rctx_dev); /* Need a better way to pull these in */ -extern void (*pEpInt_IN[7])(void); +extern void(*pEpInt_IN[7]) (void); -int32_t PIOS_USB_RCTX_Init(uint32_t * usbrctx_id, const struct pios_usb_rctx_cfg * cfg, uint32_t lower_id) +int32_t PIOS_USB_RCTX_Init(uint32_t *usbrctx_id, const struct pios_usb_rctx_cfg *cfg, uint32_t lower_id) { - PIOS_Assert(usbrctx_id); - PIOS_Assert(cfg); + PIOS_Assert(usbrctx_id); + PIOS_Assert(cfg); - struct pios_usb_rctx_dev * usb_rctx_dev; + struct pios_usb_rctx_dev *usb_rctx_dev; - usb_rctx_dev = (struct pios_usb_rctx_dev *) PIOS_USB_RCTX_alloc(); - if (!usb_rctx_dev) goto out_fail; + usb_rctx_dev = (struct pios_usb_rctx_dev *)PIOS_USB_RCTX_alloc(); + if (!usb_rctx_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usb_rctx_dev->cfg = cfg; - usb_rctx_dev->lower_id = lower_id; + /* Bind the configuration to the device instance */ + usb_rctx_dev->cfg = cfg; + usb_rctx_dev->lower_id = lower_id; - /* Set the initial report buffer */ - memset(&usb_rctx_dev->report, 0, sizeof(usb_rctx_dev->report)); + /* Set the initial report buffer */ + memset(&usb_rctx_dev->report, 0, sizeof(usb_rctx_dev->report)); - pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_RCTX_EP_IN_Callback; + pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_RCTX_EP_IN_Callback; - *usbrctx_id = (uint32_t) usb_rctx_dev; + *usbrctx_id = (uint32_t)usb_rctx_dev; - return 0; + return 0; out_fail: - return -1; + return -1; } -static void PIOS_USB_RCTX_SendReport(struct pios_usb_rctx_dev * usb_rctx_dev) +static void PIOS_USB_RCTX_SendReport(struct pios_usb_rctx_dev *usb_rctx_dev) { #ifdef PIOS_INCLUDE_FREERTOS - bool need_yield = false; -#endif /* PIOS_INCLUDE_FREERTOS */ + bool need_yield = false; +#endif /* PIOS_INCLUDE_FREERTOS */ - usb_rctx_dev->report.id = 3; /* FIXME: shouldn't hard-code this report ID */ + usb_rctx_dev->report.id = 3; /* FIXME: shouldn't hard-code this report ID */ - UserToPMABufferCopy((uint8_t *) &usb_rctx_dev->report, - GetEPTxAddr(usb_rctx_dev->cfg->data_tx_ep), - sizeof(usb_rctx_dev->report)); + UserToPMABufferCopy((uint8_t *)&usb_rctx_dev->report, + GetEPTxAddr(usb_rctx_dev->cfg->data_tx_ep), + sizeof(usb_rctx_dev->report)); - SetEPTxCount(usb_rctx_dev->cfg->data_tx_ep, sizeof(usb_rctx_dev->report)); - SetEPTxValid(usb_rctx_dev->cfg->data_tx_ep); + SetEPTxCount(usb_rctx_dev->cfg->data_tx_ep, sizeof(usb_rctx_dev->report)); + SetEPTxValid(usb_rctx_dev->cfg->data_tx_ep); #ifdef PIOS_INCLUDE_FREERTOS - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } static void PIOS_USB_RCTX_EP_IN_Callback(void) { - struct pios_usb_rctx_dev * usb_rctx_dev = (struct pios_usb_rctx_dev *)pios_usb_rctx_id; + struct pios_usb_rctx_dev *usb_rctx_dev = (struct pios_usb_rctx_dev *)pios_usb_rctx_id; - bool valid = PIOS_USB_RCTX_validate(usb_rctx_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_RCTX_validate(usb_rctx_dev); - if (!PIOS_USB_CheckAvailable(usb_rctx_dev->lower_id)) { - return; - } + PIOS_Assert(valid); - PIOS_USB_RCTX_SendReport(usb_rctx_dev); + if (!PIOS_USB_CheckAvailable(usb_rctx_dev->lower_id)) { + return; + } + + PIOS_USB_RCTX_SendReport(usb_rctx_dev); } void PIOS_USB_RCTX_Update(uint32_t usbrctx_id, const uint16_t channel[], const int16_t channel_min[], const int16_t channel_max[], uint8_t num_channels) { - struct pios_usb_rctx_dev * usb_rctx_dev = (struct pios_usb_rctx_dev *)usbrctx_id; + struct pios_usb_rctx_dev *usb_rctx_dev = (struct pios_usb_rctx_dev *)usbrctx_id; - bool valid = PIOS_USB_RCTX_validate(usb_rctx_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_RCTX_validate(usb_rctx_dev); - if (!PIOS_USB_CheckAvailable(usb_rctx_dev->lower_id)) { - return; - } + PIOS_Assert(valid); - for (uint8_t i = 0; - i < PIOS_USB_RCTX_NUM_CHANNELS && i < num_channels; - i++) { - int16_t min = channel_min[i]; - int16_t max = channel_max[i]; - uint16_t val = channel[i]; + if (!PIOS_USB_CheckAvailable(usb_rctx_dev->lower_id)) { + return; + } - if (channel_min[i] > channel_max[i]) { - /* This channel is reversed, flip min and max */ - min = channel_max[i]; - max = channel_min[i]; + for (uint8_t i = 0; + i < PIOS_USB_RCTX_NUM_CHANNELS && i < num_channels; + i++) { + int16_t min = channel_min[i]; + int16_t max = channel_max[i]; + uint16_t val = channel[i]; - /* and flip val to be an offset from the lower end of the range */ - val = channel_min[i] - channel[i] + channel_max[i]; - } + if (channel_min[i] > channel_max[i]) { + /* This channel is reversed, flip min and max */ + min = channel_max[i]; + max = channel_min[i]; - /* Scale channel linearly between min and max */ - if (min == max) { - val = 0; - } else { - if (val < min) val = min; - if (val > max) val = max; + /* and flip val to be an offset from the lower end of the range */ + val = channel_min[i] - channel[i] + channel_max[i]; + } - val = (val - min) * (65535 / (max - min)); - } + /* Scale channel linearly between min and max */ + if (min == max) { + val = 0; + } else { + if (val < min) { + val = min; + } + if (val > max) { + val = max; + } - usb_rctx_dev->report.vals[i] = val; - } + val = (val - min) * (65535 / (max - min)); + } - if (GetEPTxStatus(usb_rctx_dev->cfg->data_tx_ep) == EP_TX_VALID) { - /* Endpoint is already transmitting */ - return; - } + usb_rctx_dev->report.vals[i] = val; + } - PIOS_USB_RCTX_SendReport(usb_rctx_dev); + if (GetEPTxStatus(usb_rctx_dev->cfg->data_tx_ep) == EP_TX_VALID) { + /* Endpoint is already transmitting */ + return; + } + + PIOS_USB_RCTX_SendReport(usb_rctx_dev); } #endif /* PIOS_INCLUDE_USB_RCTX */ diff --git a/flight/pios/stm32f10x/pios_usbhook.c b/flight/pios/stm32f10x/pios_usbhook.c index ba01d8fee..c1173db7d 100644 --- a/flight/pios/stm32f10x/pios_usbhook.c +++ b/flight/pios/stm32f10x/pios_usbhook.c @@ -32,11 +32,11 @@ #ifdef PIOS_INCLUDE_USB -#include "pios_usb.h" /* PIOS_USB_* */ +#include "pios_usb.h" /* PIOS_USB_* */ #include "pios_usbhook.h" -#include "pios_usb_defs.h" /* struct usb_* */ +#include "pios_usb_defs.h" /* struct usb_* */ #include "pios_usb_hid_pwr.h" -#include "pios_usb_cdc_priv.h" /* PIOS_USB_CDC_* */ +#include "pios_usb_cdc_priv.h" /* PIOS_USB_CDC_* */ #include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ /* STM32 USB Library Definitions */ @@ -44,54 +44,54 @@ static ONE_DESCRIPTOR Device_Descriptor; -void PIOS_USBHOOK_RegisterDevice(const uint8_t * desc, uint16_t desc_size) +void PIOS_USBHOOK_RegisterDevice(const uint8_t *desc, uint16_t desc_size) { - Device_Descriptor.Descriptor = desc; - Device_Descriptor.Descriptor_Size = desc_size; + Device_Descriptor.Descriptor = desc; + Device_Descriptor.Descriptor_Size = desc_size; } static ONE_DESCRIPTOR Config_Descriptor; -void PIOS_USBHOOK_RegisterConfig(__attribute__((unused)) uint8_t config_id, const uint8_t * desc, uint16_t desc_size) +void PIOS_USBHOOK_RegisterConfig(__attribute__((unused)) uint8_t config_id, const uint8_t *desc, uint16_t desc_size) { - Config_Descriptor.Descriptor = desc; - Config_Descriptor.Descriptor_Size = desc_size; + Config_Descriptor.Descriptor = desc; + Config_Descriptor.Descriptor_Size = desc_size; } static ONE_DESCRIPTOR String_Descriptor[4]; -void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t * desc, uint16_t desc_size) +void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t *desc, uint16_t desc_size) { - if (string_id < NELEMENTS(String_Descriptor)) { - String_Descriptor[string_id].Descriptor = desc; - String_Descriptor[string_id].Descriptor_Size = desc_size; - } + if (string_id < NELEMENTS(String_Descriptor)) { + String_Descriptor[string_id].Descriptor = desc; + String_Descriptor[string_id].Descriptor_Size = desc_size; + } } static ONE_DESCRIPTOR Hid_Descriptor; -void PIOS_USB_HID_RegisterHidDescriptor(const uint8_t * desc, uint16_t desc_size) +void PIOS_USB_HID_RegisterHidDescriptor(const uint8_t *desc, uint16_t desc_size) { - Hid_Descriptor.Descriptor = desc; - Hid_Descriptor.Descriptor_Size = desc_size; + Hid_Descriptor.Descriptor = desc; + Hid_Descriptor.Descriptor_Size = desc_size; } static ONE_DESCRIPTOR Hid_Report_Descriptor; -void PIOS_USB_HID_RegisterHidReport(const uint8_t * desc, uint16_t desc_size) +void PIOS_USB_HID_RegisterHidReport(const uint8_t *desc, uint16_t desc_size) { - Hid_Report_Descriptor.Descriptor = desc; - Hid_Report_Descriptor.Descriptor_Size = desc_size; + Hid_Report_Descriptor.Descriptor = desc; + Hid_Report_Descriptor.Descriptor_Size = desc_size; } -#include "stm32f10x.h" /* __IO */ +#include "stm32f10x.h" /* __IO */ __IO uint8_t EXTI_Enable; uint32_t ProtocolValue; DEVICE Device_Table = { - PIOS_USB_BOARD_EP_NUM, - 1 + PIOS_USB_BOARD_EP_NUM, + 1 }; static void PIOS_USBHOOK_Init(void); @@ -106,33 +106,33 @@ static const uint8_t *PIOS_USBHOOK_GetConfigDescriptor(uint16_t Length); static const uint8_t *PIOS_USBHOOK_GetStringDescriptor(uint16_t Length); DEVICE_PROP Device_Property = { - .Init = PIOS_USBHOOK_Init, - .Reset = PIOS_USBHOOK_Reset, - .Process_Status_IN = PIOS_USBHOOK_Status_In, - .Process_Status_OUT = PIOS_USBHOOK_Status_Out, - .Class_Data_Setup = PIOS_USBHOOK_Data_Setup, - .Class_NoData_Setup = PIOS_USBHOOK_NoData_Setup, - .Class_Get_Interface_Setting = PIOS_USBHOOK_Get_Interface_Setting, - .GetDeviceDescriptor = PIOS_USBHOOK_GetDeviceDescriptor, - .GetConfigDescriptor = PIOS_USBHOOK_GetConfigDescriptor, - .GetStringDescriptor = PIOS_USBHOOK_GetStringDescriptor, - .RxEP_buffer = 0, - .MaxPacketSize = 0x40, + .Init = PIOS_USBHOOK_Init, + .Reset = PIOS_USBHOOK_Reset, + .Process_Status_IN = PIOS_USBHOOK_Status_In, + .Process_Status_OUT = PIOS_USBHOOK_Status_Out, + .Class_Data_Setup = PIOS_USBHOOK_Data_Setup, + .Class_NoData_Setup = PIOS_USBHOOK_NoData_Setup, + .Class_Get_Interface_Setting = PIOS_USBHOOK_Get_Interface_Setting, + .GetDeviceDescriptor = PIOS_USBHOOK_GetDeviceDescriptor, + .GetConfigDescriptor = PIOS_USBHOOK_GetConfigDescriptor, + .GetStringDescriptor = PIOS_USBHOOK_GetStringDescriptor, + .RxEP_buffer = 0, + .MaxPacketSize = 0x40, }; static void PIOS_USBHOOK_SetConfiguration(void); static void PIOS_USBHOOK_SetDeviceAddress(void); USER_STANDARD_REQUESTS User_Standard_Requests = { - .User_GetConfiguration = NOP_Process, - .User_SetConfiguration = PIOS_USBHOOK_SetConfiguration, - .User_GetInterface = NOP_Process, - .User_SetInterface = NOP_Process, - .User_GetStatus = NOP_Process, - .User_ClearFeature = NOP_Process, - .User_SetEndPointFeature = NOP_Process, - .User_SetDeviceFeature = NOP_Process, - .User_SetDeviceAddress = PIOS_USBHOOK_SetDeviceAddress + .User_GetConfiguration = NOP_Process, + .User_SetConfiguration = PIOS_USBHOOK_SetConfiguration, + .User_GetInterface = NOP_Process, + .User_SetInterface = NOP_Process, + .User_GetStatus = NOP_Process, + .User_ClearFeature = NOP_Process, + .User_SetEndPointFeature = NOP_Process, + .User_SetDeviceFeature = NOP_Process, + .User_SetDeviceAddress = PIOS_USBHOOK_SetDeviceAddress }; static RESULT PIOS_USBHOOK_SetProtocol(void); @@ -149,15 +149,15 @@ static const uint8_t *PIOS_USBHOOK_GetHIDDescriptor(uint16_t Length); *******************************************************************************/ static void PIOS_USBHOOK_Init(void) { - pInformation->Current_Configuration = 0; + pInformation->Current_Configuration = 0; - /* Connect the device */ - PowerOn(); + /* Connect the device */ + PowerOn(); - /* Perform basic device initialization operations */ - USB_SIL_Init(); + /* Perform basic device initialization operations */ + USB_SIL_Init(); - bDeviceState = UNCONNECTED; + bDeviceState = UNCONNECTED; } /******************************************************************************* @@ -169,72 +169,72 @@ static void PIOS_USBHOOK_Init(void) *******************************************************************************/ static void PIOS_USBHOOK_Reset(void) { - /* Set DEVICE as not configured */ - pInformation->Current_Configuration = 0; - pInformation->Current_Interface = 0; /*the default Interface */ + /* Set DEVICE as not configured */ + pInformation->Current_Configuration = 0; + pInformation->Current_Interface = 0; /*the default Interface */ - /* Current Feature initialization */ - pInformation->Current_Feature = 0; + /* Current Feature initialization */ + pInformation->Current_Feature = 0; #ifdef STM32F10X_CL - /* EP0 is already configured in DFU_Init() by USB_SIL_Init() function */ + /* EP0 is already configured in DFU_Init() by USB_SIL_Init() function */ - /* Init EP1 IN as Interrupt endpoint */ - OTG_DEV_EP_Init(EP1_IN, OTG_DEV_EP_TYPE_INT, 2); + /* Init EP1 IN as Interrupt endpoint */ + OTG_DEV_EP_Init(EP1_IN, OTG_DEV_EP_TYPE_INT, 2); - /* Init EP1 OUT as Interrupt endpoint */ - OTG_DEV_EP_Init(EP1_OUT, OTG_DEV_EP_TYPE_INT, 2); + /* Init EP1 OUT as Interrupt endpoint */ + OTG_DEV_EP_Init(EP1_OUT, OTG_DEV_EP_TYPE_INT, 2); #else - SetBTABLE(BTABLE_ADDRESS); + SetBTABLE(BTABLE_ADDRESS); - /* Initialize Endpoint 0 (Control) */ - SetEPType(ENDP0, EP_CONTROL); - SetEPTxAddr(ENDP0, ENDP0_TXADDR); - SetEPTxStatus(ENDP0, EP_TX_STALL); - Clear_Status_Out(ENDP0); + /* Initialize Endpoint 0 (Control) */ + SetEPType(ENDP0, EP_CONTROL); + SetEPTxAddr(ENDP0, ENDP0_TXADDR); + SetEPTxStatus(ENDP0, EP_TX_STALL); + Clear_Status_Out(ENDP0); - SetEPRxAddr(ENDP0, ENDP0_RXADDR); - SetEPRxCount(ENDP0, Device_Property.MaxPacketSize); - SetEPRxValid(ENDP0); + SetEPRxAddr(ENDP0, ENDP0_RXADDR); + SetEPRxCount(ENDP0, Device_Property.MaxPacketSize); + SetEPRxValid(ENDP0); #if defined(PIOS_INCLUDE_USB_HID) - /* Initialize Endpoint 1 (HID) */ - SetEPType(ENDP1, EP_INTERRUPT); - SetEPTxAddr(ENDP1, ENDP1_TXADDR); - SetEPTxCount(ENDP1, PIOS_USB_BOARD_HID_DATA_LENGTH); - SetEPTxStatus(ENDP1, EP_TX_NAK); + /* Initialize Endpoint 1 (HID) */ + SetEPType(ENDP1, EP_INTERRUPT); + SetEPTxAddr(ENDP1, ENDP1_TXADDR); + SetEPTxCount(ENDP1, PIOS_USB_BOARD_HID_DATA_LENGTH); + SetEPTxStatus(ENDP1, EP_TX_NAK); - SetEPRxAddr(ENDP1, ENDP1_RXADDR); - SetEPRxCount(ENDP1, PIOS_USB_BOARD_HID_DATA_LENGTH); - SetEPRxStatus(ENDP1, EP_RX_VALID); -#endif /* PIOS_INCLUDE_USB_HID */ + SetEPRxAddr(ENDP1, ENDP1_RXADDR); + SetEPRxCount(ENDP1, PIOS_USB_BOARD_HID_DATA_LENGTH); + SetEPRxStatus(ENDP1, EP_RX_VALID); +#endif /* PIOS_INCLUDE_USB_HID */ #if defined(PIOS_INCLUDE_USB_CDC) - /* Initialize Endpoint 2 (CDC Call Control) */ - SetEPType(ENDP2, EP_INTERRUPT); - SetEPTxAddr(ENDP2, ENDP2_TXADDR); - SetEPTxStatus(ENDP2, EP_TX_NAK); + /* Initialize Endpoint 2 (CDC Call Control) */ + SetEPType(ENDP2, EP_INTERRUPT); + SetEPTxAddr(ENDP2, ENDP2_TXADDR); + SetEPTxStatus(ENDP2, EP_TX_NAK); - SetEPRxAddr(ENDP2, ENDP2_RXADDR); - SetEPRxCount(ENDP2, PIOS_USB_BOARD_CDC_MGMT_LENGTH); - SetEPRxStatus(ENDP2, EP_RX_DIS); + SetEPRxAddr(ENDP2, ENDP2_RXADDR); + SetEPRxCount(ENDP2, PIOS_USB_BOARD_CDC_MGMT_LENGTH); + SetEPRxStatus(ENDP2, EP_RX_DIS); - /* Initialize Endpoint 3 (CDC Data) */ - SetEPType(ENDP3, EP_BULK); - SetEPTxAddr(ENDP3, ENDP3_TXADDR); - SetEPTxStatus(ENDP3, EP_TX_NAK); + /* Initialize Endpoint 3 (CDC Data) */ + SetEPType(ENDP3, EP_BULK); + SetEPTxAddr(ENDP3, ENDP3_TXADDR); + SetEPTxStatus(ENDP3, EP_TX_NAK); - SetEPRxAddr(ENDP3, ENDP3_RXADDR); - SetEPRxCount(ENDP3, PIOS_USB_BOARD_CDC_DATA_LENGTH); - SetEPRxStatus(ENDP3, EP_RX_VALID); + SetEPRxAddr(ENDP3, ENDP3_RXADDR); + SetEPRxCount(ENDP3, PIOS_USB_BOARD_CDC_DATA_LENGTH); + SetEPRxStatus(ENDP3, EP_RX_VALID); -#endif /* PIOS_INCLUDE_USB_CDC */ +#endif /* PIOS_INCLUDE_USB_CDC */ - /* Set this device to response on default address */ - SetDeviceAddress(0); + /* Set this device to response on default address */ + SetDeviceAddress(0); #endif /* STM32F10X_CL */ - bDeviceState = ATTACHED; + bDeviceState = ATTACHED; } /******************************************************************************* @@ -246,13 +246,13 @@ static void PIOS_USBHOOK_Reset(void) *******************************************************************************/ static void PIOS_USBHOOK_SetConfiguration(void) { - if (pInformation->Current_Configuration != 0) { - /* Device configured */ - bDeviceState = CONFIGURED; - } + if (pInformation->Current_Configuration != 0) { + /* Device configured */ + bDeviceState = CONFIGURED; + } - /* Enable transfers */ - PIOS_USB_ChangeConnectionState(pInformation->Current_Configuration != 0); + /* Enable transfers */ + PIOS_USB_ChangeConnectionState(pInformation->Current_Configuration != 0); } /******************************************************************************* @@ -264,7 +264,7 @@ static void PIOS_USBHOOK_SetConfiguration(void) *******************************************************************************/ static void PIOS_USBHOOK_SetDeviceAddress(void) { - bDeviceState = ADDRESSED; + bDeviceState = ADDRESSED; } /******************************************************************************* @@ -275,8 +275,7 @@ static void PIOS_USBHOOK_SetDeviceAddress(void) * Return : None. *******************************************************************************/ static void PIOS_USBHOOK_Status_In(void) -{ -} +{} /******************************************************************************* * Function Name : PIOS_USBHOOK_Status_Out @@ -286,8 +285,7 @@ static void PIOS_USBHOOK_Status_In(void) * Return : None. *******************************************************************************/ static void PIOS_USBHOOK_Status_Out(void) -{ -} +{} /******************************************************************************* * Function Name : PIOS_USBHOOK_Data_Setup @@ -301,95 +299,95 @@ extern const uint8_t *PIOS_USB_CDC_GetLineCoding(uint16_t Length); static RESULT PIOS_USBHOOK_Data_Setup(uint8_t RequestNo) { - uint8_t *(*CopyOutRoutine) (uint16_t); - const uint8_t *(*CopyInRoutine) (uint16_t); + uint8_t *(*CopyOutRoutine)(uint16_t); + const uint8_t *(*CopyInRoutine)(uint16_t); - CopyInRoutine = NULL; - CopyOutRoutine = NULL; + CopyInRoutine = NULL; + CopyOutRoutine = NULL; - switch (Type_Recipient) { - case (STANDARD_REQUEST | INTERFACE_RECIPIENT): - switch (pInformation->USBwIndex0) { + switch (Type_Recipient) { + case (STANDARD_REQUEST | INTERFACE_RECIPIENT): + switch (pInformation->USBwIndex0) { #if defined(PIOS_INCLUDE_USB_CDC) - case 2: /* HID Interface */ + case 2: /* HID Interface */ #else - case 0: /* HID Interface */ + case 0: /* HID Interface */ #endif - switch (RequestNo) { - case GET_DESCRIPTOR: - switch (pInformation->USBwValue1) { - case USB_DESC_TYPE_REPORT: - CopyInRoutine = PIOS_USBHOOK_GetReportDescriptor; - break; - case USB_DESC_TYPE_HID: - CopyInRoutine = PIOS_USBHOOK_GetHIDDescriptor; - break; - } - } - } - break; + switch (RequestNo) { + case GET_DESCRIPTOR: + switch (pInformation->USBwValue1) { + case USB_DESC_TYPE_REPORT: + CopyInRoutine = PIOS_USBHOOK_GetReportDescriptor; + break; + case USB_DESC_TYPE_HID: + CopyInRoutine = PIOS_USBHOOK_GetHIDDescriptor; + break; + } + } + } + break; - case (CLASS_REQUEST | INTERFACE_RECIPIENT): - switch (pInformation->USBwIndex0) { + case (CLASS_REQUEST | INTERFACE_RECIPIENT): + switch (pInformation->USBwIndex0) { #if defined(PIOS_INCLUDE_USB_CDC) - case 2: /* HID Interface */ + case 2: /* HID Interface */ #else - case 0: /* HID Interface */ + case 0: /* HID Interface */ #endif - switch (RequestNo) { - case USB_HID_REQ_GET_PROTOCOL: - CopyInRoutine = PIOS_USBHOOK_GetProtocolValue; - break; - } + switch (RequestNo) { + case USB_HID_REQ_GET_PROTOCOL: + CopyInRoutine = PIOS_USBHOOK_GetProtocolValue; + break; + } - break; + break; #if defined(PIOS_INCLUDE_USB_CDC) - case 0: /* CDC Call Control Interface */ - switch (RequestNo) { - case USB_CDC_REQ_SET_LINE_CODING: - CopyOutRoutine = PIOS_USB_CDC_SetLineCoding; - break; - case USB_CDC_REQ_GET_LINE_CODING: - CopyInRoutine = PIOS_USB_CDC_GetLineCoding; - break; - } + case 0: /* CDC Call Control Interface */ + switch (RequestNo) { + case USB_CDC_REQ_SET_LINE_CODING: + CopyOutRoutine = PIOS_USB_CDC_SetLineCoding; + break; + case USB_CDC_REQ_GET_LINE_CODING: + CopyInRoutine = PIOS_USB_CDC_GetLineCoding; + break; + } - break; + break; - case 1: /* CDC Data Interface */ - switch (RequestNo) { - case 0: - break; - } + case 1: /* CDC Data Interface */ + switch (RequestNo) { + case 0: + break; + } - break; -#endif /* PIOS_INCLUDE_USB_CDC */ - } - break; - } + break; +#endif /* PIOS_INCLUDE_USB_CDC */ + } + break; + } - /* No registered copy routine */ - if ((CopyInRoutine == NULL) && (CopyOutRoutine == NULL)) { - return USB_UNSUPPORT; - } + /* No registered copy routine */ + if ((CopyInRoutine == NULL) && (CopyOutRoutine == NULL)) { + return USB_UNSUPPORT; + } - /* Registered copy in AND copy out routine */ - if ((CopyInRoutine != NULL) && (CopyOutRoutine != NULL)) { - /* This should never happen */ - return USB_UNSUPPORT; - } + /* Registered copy in AND copy out routine */ + if ((CopyInRoutine != NULL) && (CopyOutRoutine != NULL)) { + /* This should never happen */ + return USB_UNSUPPORT; + } - if (CopyInRoutine != NULL) { - pInformation->Ctrl_Info.CopyDataIn = CopyInRoutine; - pInformation->Ctrl_Info.Usb_wOffset = 0; - (*CopyInRoutine) (0); - } else if (CopyOutRoutine != NULL) { - pInformation->Ctrl_Info.CopyDataOut = CopyOutRoutine; - pInformation->Ctrl_Info.Usb_rOffset = 0; - (*CopyOutRoutine) (0); - } + if (CopyInRoutine != NULL) { + pInformation->Ctrl_Info.CopyDataIn = CopyInRoutine; + pInformation->Ctrl_Info.Usb_wOffset = 0; + (*CopyInRoutine)(0); + } else if (CopyOutRoutine != NULL) { + pInformation->Ctrl_Info.CopyDataOut = CopyOutRoutine; + pInformation->Ctrl_Info.Usb_rOffset = 0; + (*CopyOutRoutine)(0); + } - return USB_SUCCESS; + return USB_SUCCESS; } /******************************************************************************* @@ -403,38 +401,40 @@ extern RESULT PIOS_USB_CDC_SetControlLineState(void); static RESULT PIOS_USBHOOK_NoData_Setup(uint8_t RequestNo) { - switch (Type_Recipient) { - case (CLASS_REQUEST | INTERFACE_RECIPIENT): - switch (pInformation->USBwIndex0) { + switch (Type_Recipient) { + case (CLASS_REQUEST | INTERFACE_RECIPIENT): + switch (pInformation->USBwIndex0) { #if defined(PIOS_INCLUDE_USB_CDC) - case 2: /* HID */ + case 2: /* HID */ #else - case 0: /* HID */ + case 0: /* HID */ #endif - switch (RequestNo) { - case USB_HID_REQ_SET_PROTOCOL: - return PIOS_USBHOOK_SetProtocol(); - break; - } + switch (RequestNo) { + case USB_HID_REQ_SET_PROTOCOL: + return PIOS_USBHOOK_SetProtocol(); - break; + break; + } + + break; #if defined(PIOS_INCLUDE_USB_CDC) - case 0: /* CDC Call Control Interface */ - switch (RequestNo) { - case USB_CDC_REQ_SET_CONTROL_LINE_STATE: - return PIOS_USB_CDC_SetControlLineState(); - break; - } + case 0: /* CDC Call Control Interface */ + switch (RequestNo) { + case USB_CDC_REQ_SET_CONTROL_LINE_STATE: + return PIOS_USB_CDC_SetControlLineState(); - break; -#endif /* PIOS_INCLUDE_USB_CDC */ - } + break; + } - break; - } + break; +#endif /* PIOS_INCLUDE_USB_CDC */ + } - return USB_UNSUPPORT; + break; + } + + return USB_UNSUPPORT; } /******************************************************************************* @@ -446,7 +446,7 @@ static RESULT PIOS_USBHOOK_NoData_Setup(uint8_t RequestNo) *******************************************************************************/ static const uint8_t *PIOS_USBHOOK_GetDeviceDescriptor(uint16_t Length) { - return Standard_GetDescriptorData(Length, &Device_Descriptor); + return Standard_GetDescriptorData(Length, &Device_Descriptor); } /******************************************************************************* @@ -458,7 +458,7 @@ static const uint8_t *PIOS_USBHOOK_GetDeviceDescriptor(uint16_t Length) *******************************************************************************/ static const uint8_t *PIOS_USBHOOK_GetConfigDescriptor(uint16_t Length) { - return Standard_GetDescriptorData(Length, &Config_Descriptor); + return Standard_GetDescriptorData(Length, &Config_Descriptor); } /******************************************************************************* @@ -470,12 +470,13 @@ static const uint8_t *PIOS_USBHOOK_GetConfigDescriptor(uint16_t Length) *******************************************************************************/ static const uint8_t *PIOS_USBHOOK_GetStringDescriptor(uint16_t Length) { - uint8_t wValue0 = pInformation->USBwValue0; - if (wValue0 > 4) { - return NULL; - } else { - return Standard_GetDescriptorData(Length, &String_Descriptor[wValue0]); - } + uint8_t wValue0 = pInformation->USBwValue0; + + if (wValue0 > 4) { + return NULL; + } else { + return Standard_GetDescriptorData(Length, &String_Descriptor[wValue0]); + } } /******************************************************************************* @@ -487,7 +488,7 @@ static const uint8_t *PIOS_USBHOOK_GetStringDescriptor(uint16_t Length) *******************************************************************************/ static const uint8_t *PIOS_USBHOOK_GetReportDescriptor(uint16_t Length) { - return Standard_GetDescriptorData(Length, &Hid_Report_Descriptor); + return Standard_GetDescriptorData(Length, &Hid_Report_Descriptor); } /******************************************************************************* @@ -499,7 +500,7 @@ static const uint8_t *PIOS_USBHOOK_GetReportDescriptor(uint16_t Length) *******************************************************************************/ static const uint8_t *PIOS_USBHOOK_GetHIDDescriptor(uint16_t Length) { - return Standard_GetDescriptorData(Length, &Hid_Descriptor); + return Standard_GetDescriptorData(Length, &Hid_Descriptor); } /******************************************************************************* @@ -513,12 +514,12 @@ static const uint8_t *PIOS_USBHOOK_GetHIDDescriptor(uint16_t Length) *******************************************************************************/ static RESULT PIOS_USBHOOK_Get_Interface_Setting(uint8_t Interface, uint8_t AlternateSetting) { - if (AlternateSetting > 0) { - return USB_UNSUPPORT; - } else if (Interface > 0) { - return USB_UNSUPPORT; - } - return USB_SUCCESS; + if (AlternateSetting > 0) { + return USB_UNSUPPORT; + } else if (Interface > 0) { + return USB_UNSUPPORT; + } + return USB_SUCCESS; } /******************************************************************************* @@ -530,9 +531,10 @@ static RESULT PIOS_USBHOOK_Get_Interface_Setting(uint8_t Interface, uint8_t Alte *******************************************************************************/ static RESULT PIOS_USBHOOK_SetProtocol(void) { - uint8_t wValue0 = pInformation->USBwValue0; - ProtocolValue = wValue0; - return USB_SUCCESS; + uint8_t wValue0 = pInformation->USBwValue0; + + ProtocolValue = wValue0; + return USB_SUCCESS; } /******************************************************************************* @@ -544,12 +546,12 @@ static RESULT PIOS_USBHOOK_SetProtocol(void) *******************************************************************************/ static const uint8_t *PIOS_USBHOOK_GetProtocolValue(uint16_t Length) { - if (Length == 0) { - pInformation->Ctrl_Info.Usb_wLength = 1; - return NULL; - } else { - return (uint8_t *) (&ProtocolValue); - } + if (Length == 0) { + pInformation->Ctrl_Info.Usb_wLength = 1; + return NULL; + } else { + return (uint8_t *)(&ProtocolValue); + } } #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/stm32f10x/pios_wdg.c b/flight/pios/stm32f10x/pios_wdg.c index 0f3e9d600..3f800be2c 100644 --- a/flight/pios/stm32f10x/pios_wdg.c +++ b/flight/pios/stm32f10x/pios_wdg.c @@ -8,12 +8,12 @@ * * @file pios_spi.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Hardware Abstraction Layer for SPI ports of STM32 * @see The GNU Public License (GPL) Version 3 * @notes * - * The PIOS Watchdog provides a HAL to initialize a watchdog + * The PIOS Watchdog provides a HAL to initialize a watchdog * *****************************************************************************/ /* @@ -40,21 +40,21 @@ #include "stm32f10x_dbgmcu.h" static struct wdg_configuration { - uint16_t used_flags; - uint16_t bootup_flags; + uint16_t used_flags; + uint16_t bootup_flags; } wdg_configuration; -/** +/** * @brief Initialize the watchdog timer for a specified timeout * - * It is important to note that this function returns the achieved timeout - * for this hardware. For hardware indendence this should be checked when - * scheduling updates. Other hardware dependent details may need to be - * considered such as a window time which sets a minimum update time, - * and this function should return a recommended delay for clearing. + * It is important to note that this function returns the achieved timeout + * for this hardware. For hardware indendence this should be checked when + * scheduling updates. Other hardware dependent details may need to be + * considered such as a window time which sets a minimum update time, + * and this function should return a recommended delay for clearing. * * For the STM32 nominal clock rate is 32 khz, but for the maximum clock rate of - * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is + * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is * set in the watchdog assumes the nominal clock rate, but the delay for FreeRTOS * to use is 75% of the minimal delay. * @@ -62,107 +62,109 @@ static struct wdg_configuration { */ uint16_t PIOS_WDG_Init() { - uint16_t delay = ((uint32_t) PIOS_WATCHDOG_TIMEOUT * 60) / 16; - if (delay > 0x0fff) - delay = 0x0fff; -#if defined(PIOS_INCLUDE_WDG) - DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); // make the watchdog stop counting in debug mode - IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); - IWDG_SetPrescaler(IWDG_Prescaler_16); - IWDG_SetReload(delay); - IWDG_ReloadCounter(); - IWDG_Enable(); + uint16_t delay = ((uint32_t)PIOS_WATCHDOG_TIMEOUT * 60) / 16; - // watchdog flags now stored in backup registers - PWR_BackupAccessCmd(ENABLE); - - BKP_WriteBackupRegister(PIOS_WDG_REGISTER, 0x0); - wdg_configuration.bootup_flags = BKP_ReadBackupRegister(PIOS_WDG_REGISTER); + if (delay > 0x0fff) { + delay = 0x0fff; + } +#if defined(PIOS_INCLUDE_WDG) + DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); // make the watchdog stop counting in debug mode + IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); + IWDG_SetPrescaler(IWDG_Prescaler_16); + IWDG_SetReload(delay); + IWDG_ReloadCounter(); + IWDG_Enable(); + + // watchdog flags now stored in backup registers + PWR_BackupAccessCmd(ENABLE); + + BKP_WriteBackupRegister(PIOS_WDG_REGISTER, 0x0); + wdg_configuration.bootup_flags = BKP_ReadBackupRegister(PIOS_WDG_REGISTER); #endif - return delay; + return delay; } /** - * @brief Register a module against the watchdog - * + * @brief Register a module against the watchdog + * * There are two ways to use PIOS WDG: this is for when - * multiple modules must be monitored. In this case they - * must first register against the watchdog system and + * multiple modules must be monitored. In this case they + * must first register against the watchdog system and * only when all of the modules have been updated with the - * watchdog be cleared. Each module must have its own - * bit in the 16 bit + * watchdog be cleared. Each module must have its own + * bit in the 16 bit * * @param[in] flag the bit this module wants to use * @returns True if that bit is unregistered */ -bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) +bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) { - // flag are being registered so we are in module initialization phase - // clear the WDG to prevent timeout while initializing modules. (OP-815) - PIOS_WDG_Clear(); - - /* Fail if flag already registered */ - if(wdg_configuration.used_flags & flag_requested) - return false; - - // FIXME: Protect with semaphore - wdg_configuration.used_flags |= flag_requested; - - return true; + // flag are being registered so we are in module initialization phase + // clear the WDG to prevent timeout while initializing modules. (OP-815) + PIOS_WDG_Clear(); + + /* Fail if flag already registered */ + if (wdg_configuration.used_flags & flag_requested) { + return false; + } + + // FIXME: Protect with semaphore + wdg_configuration.used_flags |= flag_requested; + + return true; } /** * @brief Function called by modules to indicate they are still running * - * This function will set this flag in the active flags register (which is + * This function will set this flag in the active flags register (which is * a backup regsiter) and if all the registered flags are set will clear * the watchdog and set only this flag in the backup register * * @param[in] flag the flag to set * @return true if the watchdog cleared, false if flags are pending */ -bool PIOS_WDG_UpdateFlag(uint16_t flag) -{ - // we can probably avoid using a semaphore here which will be good for - // efficiency and not blocking critical tasks. race condition could - // overwrite their flag update, but unlikely to block _all_ of them - // for the timeout window - uint16_t cur_flags = BKP_ReadBackupRegister(PIOS_WDG_REGISTER); - - if((cur_flags | flag) == wdg_configuration.used_flags) { - PIOS_WDG_Clear(); - BKP_WriteBackupRegister(PIOS_WDG_REGISTER, flag); - return true; - } else { - BKP_WriteBackupRegister(PIOS_WDG_REGISTER, cur_flags | flag); - return false; - } - +bool PIOS_WDG_UpdateFlag(uint16_t flag) +{ + // we can probably avoid using a semaphore here which will be good for + // efficiency and not blocking critical tasks. race condition could + // overwrite their flag update, but unlikely to block _all_ of them + // for the timeout window + uint16_t cur_flags = BKP_ReadBackupRegister(PIOS_WDG_REGISTER); + + if ((cur_flags | flag) == wdg_configuration.used_flags) { + PIOS_WDG_Clear(); + BKP_WriteBackupRegister(PIOS_WDG_REGISTER, flag); + return true; + } else { + BKP_WriteBackupRegister(PIOS_WDG_REGISTER, cur_flags | flag); + return false; + } } -/** +/** * @brief Returns the flags that were set at bootup - * - * This is used for diagnostics, if only one flag not set this + * + * This is used for diagnostics, if only one flag not set this * was likely the module that wasn't running before reset - * + * * @return The active flags register from bootup */ uint16_t PIOS_WDG_GetBootupFlags() { - return wdg_configuration.bootup_flags; + return wdg_configuration.bootup_flags; } -/** +/** * @brief Returns the currently active flags - * + * * For external monitoring - * + * * @return The active flags register */ uint16_t PIOS_WDG_GetActiveFlags() { - return BKP_ReadBackupRegister(PIOS_WDG_REGISTER); + return BKP_ReadBackupRegister(PIOS_WDG_REGISTER); } /** @@ -173,7 +175,7 @@ uint16_t PIOS_WDG_GetActiveFlags() void PIOS_WDG_Clear(void) { #if defined(PIOS_INCLUDE_WDG) - IWDG_ReloadCounter(); + IWDG_ReloadCounter(); #endif } diff --git a/flight/pios/stm32f4xx/inc/pios_i2c_config.h b/flight/pios/stm32f4xx/inc/pios_i2c_config.h index e269c0fe4..adabdcdc4 100644 --- a/flight/pios/stm32f4xx/inc/pios_i2c_config.h +++ b/flight/pios/stm32f4xx/inc/pios_i2c_config.h @@ -11,57 +11,57 @@ /** * Generic I2C configuration for the STM32F4xx */ -#define I2C_CONFIG(_i2c, _scl_gpio, _scl_pin, _sda_gpio, _sda_pin) \ -{ \ - .regs = _i2c, \ - .remap = GPIO_AF_ ## _i2c, \ - .init = { \ - .I2C_ClockSpeed = 400000, /* bits/s */ \ - .I2C_Mode = I2C_Mode_I2C, \ - .I2C_DutyCycle = I2C_DutyCycle_2, \ - .I2C_OwnAddress1 = 0, \ - .I2C_Ack = I2C_Ack_Enable, \ - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, \ - }, \ - .transfer_timeout_ms = 50, \ - .scl = { \ - .gpio = _scl_gpio, \ - .init = { \ - .GPIO_Pin = _scl_pin, \ - .GPIO_Mode = GPIO_Mode_AF, \ - .GPIO_Speed = GPIO_Speed_50MHz, \ - .GPIO_OType = GPIO_OType_OD, \ - .GPIO_PuPd = GPIO_PuPd_NOPULL, \ - }, \ - }, \ - .sda = { \ - .gpio = _sda_gpio, \ - .init = { \ - .GPIO_Pin = _sda_pin, \ - .GPIO_Mode = GPIO_Mode_AF, \ - .GPIO_Speed = GPIO_Speed_50MHz, \ - .GPIO_OType = GPIO_OType_OD, \ - .GPIO_PuPd = GPIO_PuPd_NOPULL, \ - }, \ - }, \ - .event = { \ - .flags = 0, /* FIXME: check this */ \ - .init = { \ - .NVIC_IRQChannel = _i2c ## _EV_IRQn, \ - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, \ - .NVIC_IRQChannelSubPriority = 0, \ - .NVIC_IRQChannelCmd = ENABLE, \ - }, \ - }, \ - .error = { \ - .flags = 0, /* FIXME: check this */ \ - .init = { \ - .NVIC_IRQChannel = _i2c ## _ER_IRQn, \ - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, \ - .NVIC_IRQChannelSubPriority = 0, \ - .NVIC_IRQChannelCmd = ENABLE, \ - }, \ - }, \ -} +#define I2C_CONFIG(_i2c, _scl_gpio, _scl_pin, _sda_gpio, _sda_pin) \ + { \ + .regs = _i2c, \ + .remap = GPIO_AF_##_i2c, \ + .init = { \ + .I2C_ClockSpeed = 400000, /* bits/s */ \ + .I2C_Mode = I2C_Mode_I2C, \ + .I2C_DutyCycle = I2C_DutyCycle_2, \ + .I2C_OwnAddress1 = 0, \ + .I2C_Ack = I2C_Ack_Enable, \ + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, \ + }, \ + .transfer_timeout_ms = 50, \ + .scl = { \ + .gpio = _scl_gpio, \ + .init = { \ + .GPIO_Pin = _scl_pin, \ + .GPIO_Mode = GPIO_Mode_AF, \ + .GPIO_Speed = GPIO_Speed_50MHz, \ + .GPIO_OType = GPIO_OType_OD, \ + .GPIO_PuPd = GPIO_PuPd_NOPULL, \ + }, \ + }, \ + .sda = { \ + .gpio = _sda_gpio, \ + .init = { \ + .GPIO_Pin = _sda_pin, \ + .GPIO_Mode = GPIO_Mode_AF, \ + .GPIO_Speed = GPIO_Speed_50MHz, \ + .GPIO_OType = GPIO_OType_OD, \ + .GPIO_PuPd = GPIO_PuPd_NOPULL, \ + }, \ + }, \ + .event = { \ + .flags = 0, /* FIXME: check this */ \ + .init = { \ + .NVIC_IRQChannel = _i2c##_EV_IRQn, \ + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, \ + .NVIC_IRQChannelSubPriority = 0, \ + .NVIC_IRQChannelCmd = ENABLE, \ + }, \ + }, \ + .error = { \ + .flags = 0, /* FIXME: check this */ \ + .init = { \ + .NVIC_IRQChannel = _i2c##_ER_IRQn, \ + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, \ + .NVIC_IRQChannelSubPriority = 0, \ + .NVIC_IRQChannelCmd = ENABLE, \ + }, \ + }, \ + } #endif /* PIOS_I2C_CONFIG_H_ */ diff --git a/flight/pios/stm32f4xx/inc/pios_usart_config.h b/flight/pios/stm32f4xx/inc/pios_usart_config.h index 4abed4e15..7c6d549dc 100644 --- a/flight/pios/stm32f4xx/inc/pios_usart_config.h +++ b/flight/pios/stm32f4xx/inc/pios_usart_config.h @@ -12,45 +12,45 @@ * Generic USART configuration structure for an STM32F2xx port. */ #define USART_CONFIG(_usart, _baudrate, _rx_gpio, _rx_pin, _tx_gpio, _tx_pin) \ -{ \ - .regs = _usart, \ - .remap = GPIO_AF_ ## _usart, \ - .init = { \ - .USART_BaudRate = _baudrate, \ - .USART_WordLength = USART_WordLength_8b, \ - .USART_Parity = USART_Parity_No, \ - .USART_StopBits = USART_StopBits_1, \ - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, \ - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, \ - }, \ - .irq = { \ - .init = { \ - .NVIC_IRQChannel = _usart ## _IRQn, \ - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, \ - .NVIC_IRQChannelSubPriority = 0, \ - .NVIC_IRQChannelCmd = ENABLE, \ - }, \ - }, \ - .rx = { \ - .gpio = _rx_gpio, \ - .init = { \ - .GPIO_Pin = _rx_pin, \ - .GPIO_Mode = GPIO_Mode_AF, \ - .GPIO_Speed = GPIO_Speed_50MHz, \ - .GPIO_OType = GPIO_OType_PP, \ - .GPIO_PuPd = GPIO_PuPd_UP, \ - }, \ - }, \ - .tx = { \ - .gpio = _tx_gpio, \ - .init = { \ - .GPIO_Pin = _tx_pin, \ - .GPIO_Mode = GPIO_Mode_AF, \ - .GPIO_Speed = GPIO_Speed_50MHz, \ - .GPIO_OType = GPIO_OType_PP, \ - .GPIO_PuPd = GPIO_PuPd_UP, \ - }, \ - }, \ -} + { \ + .regs = _usart, \ + .remap = GPIO_AF_##_usart, \ + .init = { \ + .USART_BaudRate = _baudrate, \ + .USART_WordLength = USART_WordLength_8b, \ + .USART_Parity = USART_Parity_No, \ + .USART_StopBits = USART_StopBits_1, \ + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, \ + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, \ + }, \ + .irq = { \ + .init = { \ + .NVIC_IRQChannel = _usart##_IRQn, \ + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, \ + .NVIC_IRQChannelSubPriority = 0, \ + .NVIC_IRQChannelCmd = ENABLE, \ + }, \ + }, \ + .rx = { \ + .gpio = _rx_gpio, \ + .init = { \ + .GPIO_Pin = _rx_pin, \ + .GPIO_Mode = GPIO_Mode_AF, \ + .GPIO_Speed = GPIO_Speed_50MHz, \ + .GPIO_OType = GPIO_OType_PP, \ + .GPIO_PuPd = GPIO_PuPd_UP, \ + }, \ + }, \ + .tx = { \ + .gpio = _tx_gpio, \ + .init = { \ + .GPIO_Pin = _tx_pin, \ + .GPIO_Mode = GPIO_Mode_AF, \ + .GPIO_Speed = GPIO_Speed_50MHz, \ + .GPIO_OType = GPIO_OType_PP, \ + .GPIO_PuPd = GPIO_PuPd_UP, \ + }, \ + }, \ + } #endif /* PIOS_USART_CONFIG_H_ */ diff --git a/flight/pios/stm32f4xx/inc/stm32f4xx_conf.h b/flight/pios/stm32f4xx/inc/stm32f4xx_conf.h index 673f5f3d0..ff66b4ab6 100644 --- a/flight/pios/stm32f4xx/inc/stm32f4xx_conf.h +++ b/flight/pios/stm32f4xx/inc/stm32f4xx_conf.h @@ -1,23 +1,23 @@ /** - ****************************************************************************** - * @file Project/STM32F4xx_StdPeriph_Templates/stm32f4xx_conf.h - * @author MCD Application Team - * @version V1.0.0 - * @date 30-September-2011 - * @brief Library configuration file. - ****************************************************************************** - * @attention - * - * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS - * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE - * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY - * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING - * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE - * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. - * - *

© COPYRIGHT 2011 STMicroelectronics

- ****************************************************************************** - */ + ****************************************************************************** + * @file Project/STM32F4xx_StdPeriph_Templates/stm32f4xx_conf.h + * @author MCD Application Team + * @version V1.0.0 + * @date 30-September-2011 + * @brief Library configuration file. + ****************************************************************************** + * @attention + * + * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS + * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE + * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY + * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING + * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * + *

© COPYRIGHT 2011 STMicroelectronics

+ ****************************************************************************** + */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef __STM32F4xx_CONF_H @@ -55,13 +55,13 @@ /* Exported types ------------------------------------------------------------*/ /* Exported constants --------------------------------------------------------*/ -/* If an external clock source is used, then the value of the following define - should be set to the value of the external clock source, else, if no external +/* If an external clock source is used, then the value of the following define + should be set to the value of the external clock source, else, if no external clock is used, keep this define commented */ /*#define I2S_EXTERNAL_CLOCK_VAL 12288000 */ /* Value of the external clock in Hz */ -/* Uncomment the line below to expanse the "assert_param" macro in the +/* Uncomment the line below to expanse the "assert_param" macro in the Standard Peripheral Library drivers code */ /* #define USE_FULL_ASSERT 1 */ @@ -69,16 +69,16 @@ #ifdef USE_FULL_ASSERT /** - * @brief The assert_param macro is used for function's parameters check. - * @param expr: If expr is false, it calls assert_failed function - * which reports the name of the source file and the source - * line number of the call that failed. - * If expr is true, it returns no value. - * @retval None - */ + * @brief The assert_param macro is used for function's parameters check. + * @param expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) /* Exported functions ------------------------------------------------------- */ - void assert_failed(uint8_t* file, uint32_t line); +void assert_failed(uint8_t *file, uint32_t line); #else #define assert_param(expr) ((void)0) #endif /* USE_FULL_ASSERT */ diff --git a/flight/pios/stm32f4xx/pios_adc.c b/flight/pios/stm32f4xx/pios_adc.c index 1f26dee4e..d4b772b22 100644 --- a/flight/pios/stm32f4xx/pios_adc.c +++ b/flight/pios/stm32f4xx/pios_adc.c @@ -6,25 +6,25 @@ * @brief STM32F4xx ADC PIOS interface * @{ * - * @file pios_adc.c + * @file pios_adc.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @author Michael Smith Copyright (C) 2011. * @brief Analog to Digital converstion routines * @see The GNU Public License (GPL) Version 3 *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -50,7 +50,7 @@ #if !defined(PIOS_ADC_MAX_SAMPLES) -#define PIOS_ADC_MAX_SAMPLES 0 +#define PIOS_ADC_MAX_SAMPLES 0 #endif #if !defined(PIOS_ADC_MAX_OVERSAMPLING) @@ -58,39 +58,39 @@ #endif #if !defined(PIOS_ADC_USE_ADC2) -#define PIOS_ADC_USE_ADC2 0 +#define PIOS_ADC_USE_ADC2 0 #endif #if !defined(PIOS_ADC_NUM_CHANNELS) -#define PIOS_ADC_NUM_CHANNELS 0 +#define PIOS_ADC_NUM_CHANNELS 0 #endif // Private types enum pios_adc_dev_magic { - PIOS_ADC_DEV_MAGIC = 0x58375124, + PIOS_ADC_DEV_MAGIC = 0x58375124, }; struct pios_adc_dev { - const struct pios_adc_cfg * cfg; - ADCCallback callback_function; + const struct pios_adc_cfg *cfg; + ADCCallback callback_function; #if defined(PIOS_INCLUDE_FREERTOS) - xQueueHandle data_queue; + xQueueHandle data_queue; #endif - volatile int16_t *valid_data_buffer; - volatile uint8_t adc_oversample; - uint8_t dma_block_size; - uint16_t dma_half_buffer_size; -// int16_t fir_coeffs[PIOS_ADC_MAX_SAMPLES+1] __attribute__ ((aligned(4))); -// volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES] __attribute__ ((aligned(4))); -// float downsampled_buffer[PIOS_ADC_NUM_CHANNELS] __attribute__ ((aligned(4))); - enum pios_adc_dev_magic magic; + volatile int16_t *valid_data_buffer; + volatile uint8_t adc_oversample; + uint8_t dma_block_size; + uint16_t dma_half_buffer_size; +// int16_t fir_coeffs[PIOS_ADC_MAX_SAMPLES+1] __attribute__ ((aligned(4))); +// volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES] __attribute__ ((aligned(4))); +// float downsampled_buffer[PIOS_ADC_NUM_CHANNELS] __attribute__ ((aligned(4))); + enum pios_adc_dev_magic magic; }; -struct pios_adc_dev * pios_adc_dev; +struct pios_adc_dev *pios_adc_dev; // Private functions void PIOS_ADC_downsample_data(); -static struct pios_adc_dev * PIOS_ADC_Allocate(); +static struct pios_adc_dev *PIOS_ADC_Allocate(); static bool PIOS_ADC_validate(struct pios_adc_dev *); #if defined(PIOS_INCLUDE_ADC) @@ -100,19 +100,19 @@ static void init_adc(void); #endif struct dma_config { - GPIO_TypeDef *port; - uint32_t pin; - uint32_t channel; + GPIO_TypeDef *port; + uint32_t pin; + uint32_t channel; }; struct adc_accumulator { - uint32_t accumulator; - uint32_t count; + uint32_t accumulator; + uint32_t count; }; #if defined(PIOS_INCLUDE_ADC) static const struct dma_config config[] = PIOS_DMA_PIN_CONFIG; -#define PIOS_ADC_NUM_PINS (sizeof(config) / sizeof(config[0])) +#define PIOS_ADC_NUM_PINS (sizeof(config) / sizeof(config[0])) static struct adc_accumulator accumulator[PIOS_ADC_NUM_PINS]; @@ -121,163 +121,166 @@ static uint16_t adc_raw_buffer[2][PIOS_ADC_MAX_SAMPLES][PIOS_ADC_NUM_PINS]; #endif #if defined(PIOS_INCLUDE_ADC) -static void -init_pins(void) +static void init_pins(void) { - /* Setup analog pins */ - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; - - for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; ++i) { - if (config[i].port == NULL) - continue; - GPIO_InitStructure.GPIO_Pin = config[i].pin; - GPIO_Init(config[i].port, &GPIO_InitStructure); - } + /* Setup analog pins */ + GPIO_InitTypeDef GPIO_InitStructure; + + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; + + for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; ++i) { + if (config[i].port == NULL) { + continue; + } + GPIO_InitStructure.GPIO_Pin = config[i].pin; + GPIO_Init(config[i].port, &GPIO_InitStructure); + } } -static void -init_dma(void) +static void init_dma(void) { - /* Disable interrupts */ - DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->dma.irq.flags, DISABLE); + /* Disable interrupts */ + DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->dma.irq.flags, DISABLE); - /* Configure DMA channel */ - DMA_DeInit(pios_adc_dev->cfg->dma.rx.channel); - DMA_InitTypeDef DMAInit = pios_adc_dev->cfg->dma.rx.init; - DMAInit.DMA_Memory0BaseAddr = (uint32_t)&adc_raw_buffer[0]; - DMAInit.DMA_BufferSize = sizeof(adc_raw_buffer[0]) / sizeof(uint16_t); - DMAInit.DMA_DIR = DMA_DIR_PeripheralToMemory; - DMAInit.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMAInit.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMAInit.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; - DMAInit.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; - DMAInit.DMA_Mode = DMA_Mode_Circular; - DMAInit.DMA_Priority = DMA_Priority_Low; - DMAInit.DMA_FIFOMode = DMA_FIFOMode_Disable; - DMAInit.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; - DMAInit.DMA_MemoryBurst = DMA_MemoryBurst_Single; - DMAInit.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + /* Configure DMA channel */ + DMA_DeInit(pios_adc_dev->cfg->dma.rx.channel); + DMA_InitTypeDef DMAInit = pios_adc_dev->cfg->dma.rx.init; + DMAInit.DMA_Memory0BaseAddr = (uint32_t)&adc_raw_buffer[0]; + DMAInit.DMA_BufferSize = sizeof(adc_raw_buffer[0]) / sizeof(uint16_t); + DMAInit.DMA_DIR = DMA_DIR_PeripheralToMemory; + DMAInit.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMAInit.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMAInit.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; + DMAInit.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; + DMAInit.DMA_Mode = DMA_Mode_Circular; + DMAInit.DMA_Priority = DMA_Priority_Low; + DMAInit.DMA_FIFOMode = DMA_FIFOMode_Disable; + DMAInit.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; + DMAInit.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMAInit.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; - DMA_Init(pios_adc_dev->cfg->dma.rx.channel, &DMAInit); /* channel is actually stream ... */ + DMA_Init(pios_adc_dev->cfg->dma.rx.channel, &DMAInit); /* channel is actually stream ... */ - /* configure for double-buffered mode and interrupt on every buffer flip */ - DMA_DoubleBufferModeConfig(pios_adc_dev->cfg->dma.rx.channel, (uint32_t)&adc_raw_buffer[1], DMA_Memory_0); - DMA_DoubleBufferModeCmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE); - DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_TC, ENABLE); - //DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_HT, ENABLE); + /* configure for double-buffered mode and interrupt on every buffer flip */ + DMA_DoubleBufferModeConfig(pios_adc_dev->cfg->dma.rx.channel, (uint32_t)&adc_raw_buffer[1], DMA_Memory_0); + DMA_DoubleBufferModeCmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE); + DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_TC, ENABLE); + // DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_HT, ENABLE); - /* enable DMA */ - DMA_Cmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE); + /* enable DMA */ + DMA_Cmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE); - /* Configure DMA interrupt */ - NVIC_InitTypeDef NVICInit = pios_adc_dev->cfg->dma.irq.init; - NVIC_Init(&NVICInit); + /* Configure DMA interrupt */ + NVIC_InitTypeDef NVICInit = pios_adc_dev->cfg->dma.irq.init; + NVIC_Init(&NVICInit); } -static void -init_adc(void) +static void init_adc(void) { - RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); + RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); - ADC_DeInit(); + ADC_DeInit(); - /* turn on VREFInt in case we need it */ - ADC_TempSensorVrefintCmd(ENABLE); + /* turn on VREFInt in case we need it */ + ADC_TempSensorVrefintCmd(ENABLE); - /* Do common ADC init */ - ADC_CommonInitTypeDef ADC_CommonInitStructure; - ADC_CommonStructInit(&ADC_CommonInitStructure); - ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; - ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8; - ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; - ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; - ADC_CommonInit(&ADC_CommonInitStructure); + /* Do common ADC init */ + ADC_CommonInitTypeDef ADC_CommonInitStructure; + ADC_CommonStructInit(&ADC_CommonInitStructure); + ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; + ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8; + ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; + ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; + ADC_CommonInit(&ADC_CommonInitStructure); - ADC_InitTypeDef ADC_InitStructure; - ADC_StructInit(&ADC_InitStructure); - ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; - ADC_InitStructure.ADC_ScanConvMode = ENABLE; - ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; - ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; - ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; - ADC_InitStructure.ADC_NbrOfConversion = ((PIOS_ADC_NUM_PINS)/* >> 1*/); - ADC_Init(pios_adc_dev->cfg->adc_dev, &ADC_InitStructure); + ADC_InitTypeDef ADC_InitStructure; + ADC_StructInit(&ADC_InitStructure); + ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; + ADC_InitStructure.ADC_ScanConvMode = ENABLE; + ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; + ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; + ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; + ADC_InitStructure.ADC_NbrOfConversion = ((PIOS_ADC_NUM_PINS) /* >> 1*/); + ADC_Init(pios_adc_dev->cfg->adc_dev, &ADC_InitStructure); - /* Enable DMA request */ - ADC_DMACmd(pios_adc_dev->cfg->adc_dev, ENABLE); + /* Enable DMA request */ + ADC_DMACmd(pios_adc_dev->cfg->adc_dev, ENABLE); - /* Configure input scan */ - for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) { - ADC_RegularChannelConfig(pios_adc_dev->cfg->adc_dev, - config[i].channel, - i+1, - ADC_SampleTime_56Cycles); /* XXX this is totally arbitrary... */ - } + /* Configure input scan */ + for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) { + ADC_RegularChannelConfig(pios_adc_dev->cfg->adc_dev, + config[i].channel, + i + 1, + ADC_SampleTime_56Cycles); /* XXX this is totally arbitrary... */ + } - ADC_DMARequestAfterLastTransferCmd(pios_adc_dev->cfg->adc_dev, ENABLE); + ADC_DMARequestAfterLastTransferCmd(pios_adc_dev->cfg->adc_dev, ENABLE); - /* Finally start initial conversion */ - ADC_Cmd(pios_adc_dev->cfg->adc_dev, ENABLE); - ADC_ContinuousModeCmd(pios_adc_dev->cfg->adc_dev, ENABLE); - ADC_SoftwareStartConv(pios_adc_dev->cfg->adc_dev); + /* Finally start initial conversion */ + ADC_Cmd(pios_adc_dev->cfg->adc_dev, ENABLE); + ADC_ContinuousModeCmd(pios_adc_dev->cfg->adc_dev, ENABLE); + ADC_SoftwareStartConv(pios_adc_dev->cfg->adc_dev); } -#endif +#endif /* if defined(PIOS_INCLUDE_ADC) */ -static bool PIOS_ADC_validate(struct pios_adc_dev * dev) +static bool PIOS_ADC_validate(struct pios_adc_dev *dev) { - if (dev == NULL) - return false; - - return (dev->magic == PIOS_ADC_DEV_MAGIC); + if (dev == NULL) { + return false; + } + + return dev->magic == PIOS_ADC_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_adc_dev * PIOS_ADC_Allocate() +static struct pios_adc_dev *PIOS_ADC_Allocate() { - struct pios_adc_dev * adc_dev; - - adc_dev = (struct pios_adc_dev *)pvPortMalloc(sizeof(*adc_dev)); - if (!adc_dev) return (NULL); - - adc_dev->magic = PIOS_ADC_DEV_MAGIC; - return(adc_dev); + struct pios_adc_dev *adc_dev; + + adc_dev = (struct pios_adc_dev *)pvPortMalloc(sizeof(*adc_dev)); + if (!adc_dev) { + return NULL; + } + + adc_dev->magic = PIOS_ADC_DEV_MAGIC; + return adc_dev; } #else #if defined(PIOS_INCLUDE_ADC) #error Not implemented #endif -static struct pios_adc_dev * PIOS_ADC_Allocate() +static struct pios_adc_dev *PIOS_ADC_Allocate() { - return (struct pios_adc_dev *) NULL; + return (struct pios_adc_dev *)NULL; } #endif /** * @brief Init the ADC. */ -int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg) +int32_t PIOS_ADC_Init(const struct pios_adc_cfg *cfg) { - pios_adc_dev = PIOS_ADC_Allocate(); - if (pios_adc_dev == NULL) - return -1; - - pios_adc_dev->cfg = cfg; - pios_adc_dev->callback_function = NULL; - + pios_adc_dev = PIOS_ADC_Allocate(); + if (pios_adc_dev == NULL) { + return -1; + } + + pios_adc_dev->cfg = cfg; + pios_adc_dev->callback_function = NULL; + #if defined(PIOS_INCLUDE_FREERTOS) - pios_adc_dev->data_queue = NULL; + pios_adc_dev->data_queue = NULL; #endif #if defined(PIOS_INCLUDE_ADC) - init_pins(); - init_dma(); - init_adc(); + init_pins(); + init_dma(); + init_adc(); #endif - return 0; + return 0; } /** @@ -286,7 +289,7 @@ int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg) */ void PIOS_ADC_Config(__attribute__((unused)) uint32_t oversampling) { - /* we ignore this */ + /* we ignore this */ } /** @@ -300,44 +303,46 @@ int32_t last_conv_value; int32_t PIOS_ADC_PinGet(uint32_t pin) { #if defined(PIOS_INCLUDE_ADC) - int32_t result; - - /* Check if pin exists */ - if (pin >= PIOS_ADC_NUM_PINS) { - return -1; - } - - if (accumulator[pin].accumulator <= 0) - return -2; + int32_t result; - /* return accumulated result and clear accumulator */ - result = accumulator[pin].accumulator / (accumulator[pin].count ?: 1); - accumulator[pin].accumulator = result; - accumulator[pin].count = 1; + /* Check if pin exists */ + if (pin >= PIOS_ADC_NUM_PINS) { + return -1; + } + + if (accumulator[pin].accumulator <= 0) { + return -2; + } + + /* return accumulated result and clear accumulator */ + result = accumulator[pin].accumulator / (accumulator[pin].count ? : 1); + accumulator[pin].accumulator = result; + accumulator[pin].count = 1; + + return result; - return result; #endif - return -1; + return -1; } /** * @brief Set a callback function that is executed whenever - * the ADC double buffer swaps + * the ADC double buffer swaps * @note Not currently supported. */ -void PIOS_ADC_SetCallback(ADCCallback new_function) +void PIOS_ADC_SetCallback(ADCCallback new_function) { - pios_adc_dev->callback_function = new_function; + pios_adc_dev->callback_function = new_function; } #if defined(PIOS_INCLUDE_FREERTOS) /** - * @brief Register a queue to add data to when downsampled + * @brief Register a queue to add data to when downsampled * @note Not currently supported. */ -void PIOS_ADC_SetQueue(xQueueHandle data_queue) +void PIOS_ADC_SetQueue(xQueueHandle data_queue) { - pios_adc_dev->data_queue = data_queue; + pios_adc_dev->data_queue = data_queue; } #endif @@ -345,18 +350,18 @@ void PIOS_ADC_SetQueue(xQueueHandle data_queue) * @brief Return the address of the downsampled data buffer * @note Not currently supported. */ -float * PIOS_ADC_GetBuffer(void) +float *PIOS_ADC_GetBuffer(void) { - return NULL; + return NULL; } /** - * @brief Return the address of the raw data data buffer + * @brief Return the address of the raw data data buffer * @note Not currently supported. */ -int16_t * PIOS_ADC_GetRawBuffer(void) +int16_t *PIOS_ADC_GetRawBuffer(void) { - return NULL; + return NULL; } /** @@ -365,20 +370,20 @@ int16_t * PIOS_ADC_GetRawBuffer(void) */ uint8_t PIOS_ADC_GetOverSampling(void) { - return 1; + return 1; } /** - * @brief Set the fir coefficients. Takes as many samples as the + * @brief Set the fir coefficients. Takes as many samples as the * current filter order plus one (normalization) * * @param new_filter Array of adc_oversampling floats plus one for the * filter coefficients * @note Not currently supported. */ -void PIOS_ADC_SetFIRCoefficients(__attribute__((unused)) float * new_filter) +void PIOS_ADC_SetFIRCoefficients(__attribute__((unused)) float *new_filter) { - // not implemented + // not implemented } /** @@ -387,40 +392,39 @@ void PIOS_ADC_SetFIRCoefficients(__attribute__((unused)) float * new_filter) void accumulate(uint16_t *buffer, uint32_t count) { #if defined(PIOS_INCLUDE_ADC) - uint16_t *sp = buffer; + uint16_t *sp = buffer; + + /* + * Accumulate sampled values. + */ + while (count--) { + for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; ++i) { + accumulator[i].accumulator += *sp++; + accumulator[i].count++; + /* + * If the accumulator reaches half-full, rescale in order to + * make more space. + */ + if (accumulator[i].accumulator >= (((uint32_t)1) << 31)) { + accumulator[i].accumulator /= 2; + accumulator[i].count /= 2; + } + } + } - /* - * Accumulate sampled values. - */ - while (count--) { - for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; ++i) { - accumulator[i].accumulator += *sp++; - accumulator[i].count++; - /* - * If the accumulator reaches half-full, rescale in order to - * make more space. - */ - if (accumulator[i].accumulator >= (((uint32_t)1) << 31)) { - accumulator[i].accumulator /= 2; - accumulator[i].count /= 2; - } - } - } - #if defined(PIOS_INCLUDE_FREERTOS) - // XXX should do something with this - if (pios_adc_dev->data_queue) { - static portBASE_TYPE xHigherPriorityTaskWoken; -// xQueueSendFromISR(pios_adc_dev->data_queue, pios_adc_dev->downsampled_buffer, &xHigherPriorityTaskWoken); - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); - } + // XXX should do something with this + if (pios_adc_dev->data_queue) { + static portBASE_TYPE xHigherPriorityTaskWoken; +// xQueueSendFromISR(pios_adc_dev->data_queue, pios_adc_dev->downsampled_buffer, &xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + } #endif -#endif - -// if(pios_adc_dev->callback_function) -// pios_adc_dev->callback_function(pios_adc_dev->downsampled_buffer); +#endif /* if defined(PIOS_INCLUDE_ADC) */ +// if(pios_adc_dev->callback_function) +// pios_adc_dev->callback_function(pios_adc_dev->downsampled_buffer); } /** @@ -430,25 +434,25 @@ void accumulate(uint16_t *buffer, uint32_t count) */ void PIOS_ADC_DMA_Handler(void) { - if (!PIOS_ADC_validate(pios_adc_dev)) - return; + if (!PIOS_ADC_validate(pios_adc_dev)) { + return; + } #if defined(PIOS_INCLUDE_ADC) - /* terminal count, buffer has flipped */ - if (DMA_GetITStatus(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->full_flag)) { - DMA_ClearITPendingBit(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->full_flag); + /* terminal count, buffer has flipped */ + if (DMA_GetITStatus(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->full_flag)) { + DMA_ClearITPendingBit(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->full_flag); - /* accumulate results from the buffer that was just completed */ - accumulate(&adc_raw_buffer[DMA_GetCurrentMemoryTarget(pios_adc_dev->cfg->dma.rx.channel) ? 0 : 1][0][0], - PIOS_ADC_MAX_SAMPLES); - - } + /* accumulate results from the buffer that was just completed */ + accumulate(&adc_raw_buffer[DMA_GetCurrentMemoryTarget(pios_adc_dev->cfg->dma.rx.channel) ? 0 : 1][0][0], + PIOS_ADC_MAX_SAMPLES); + } #endif } #endif /* PIOS_INCLUDE_ADC */ -/** +/** * @} * @} */ diff --git a/flight/pios/stm32f4xx/pios_bkp.c b/flight/pios/stm32f4xx/pios_bkp.c index d47d39f9e..8b9357c5f 100644 --- a/flight/pios/stm32f4xx/pios_bkp.c +++ b/flight/pios/stm32f4xx/pios_bkp.c @@ -6,25 +6,25 @@ * @brief Hardware abstraction layer for backup sram * @{ * - * @file pios_bkp.c + * @file pios_bkp.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief IAP functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,87 +33,83 @@ #include /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ /***************************************************************************************** * Public Definitions/Macros ****************************************************************************************/ /**************************************************************************************** - * Public Functions - ****************************************************************************************/ -const uint32_t pios_bkp_registers_map[] = - { - RTC_BKP_DR0, - RTC_BKP_DR1, - RTC_BKP_DR2, - RTC_BKP_DR3, - RTC_BKP_DR4, - RTC_BKP_DR5, - RTC_BKP_DR6, - RTC_BKP_DR7, - RTC_BKP_DR8, - RTC_BKP_DR9, - RTC_BKP_DR10, - RTC_BKP_DR11, - RTC_BKP_DR12, - RTC_BKP_DR13, - RTC_BKP_DR14, - RTC_BKP_DR15, - RTC_BKP_DR16, - RTC_BKP_DR17, - RTC_BKP_DR18, - RTC_BKP_DR19 - }; +* Public Functions +****************************************************************************************/ +const uint32_t pios_bkp_registers_map[] = { + RTC_BKP_DR0, + RTC_BKP_DR1, + RTC_BKP_DR2, + RTC_BKP_DR3, + RTC_BKP_DR4, + RTC_BKP_DR5, + RTC_BKP_DR6, + RTC_BKP_DR7, + RTC_BKP_DR8, + RTC_BKP_DR9, + RTC_BKP_DR10, + RTC_BKP_DR11, + RTC_BKP_DR12, + RTC_BKP_DR13, + RTC_BKP_DR14, + RTC_BKP_DR15, + RTC_BKP_DR16, + RTC_BKP_DR17, + RTC_BKP_DR18, + RTC_BKP_DR19 +}; #define PIOS_BKP_REGISTERS_COUNT NELEMENTS(pios_bkp_registers_map) void PIOS_BKP_Init(void) { - /* Enable CRC clock */ - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_CRC | RCC_AHB1Periph_BKPSRAM, ENABLE); + /* Enable CRC clock */ + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_CRC | RCC_AHB1Periph_BKPSRAM, ENABLE); - /* Enable PWR and BKP clock */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); + /* Enable PWR and BKP clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); - /* Clear Tamper pin Event(TE) pending flag */ - RTC_ClearFlag(RTC_FLAG_TAMP1F); + /* Clear Tamper pin Event(TE) pending flag */ + RTC_ClearFlag(RTC_FLAG_TAMP1F); } uint16_t PIOS_BKP_ReadRegister(uint32_t regnumber) { - if(PIOS_BKP_REGISTERS_COUNT < regnumber) - { - PIOS_Assert(0); - } else { - return (uint16_t) RTC_ReadBackupRegister(pios_bkp_registers_map[regnumber]); - } + if (PIOS_BKP_REGISTERS_COUNT < regnumber) { + PIOS_Assert(0); + } else { + return (uint16_t)RTC_ReadBackupRegister(pios_bkp_registers_map[regnumber]); + } } -void PIOS_BKP_WriteRegister(uint32_t regnumber,uint16_t data) +void PIOS_BKP_WriteRegister(uint32_t regnumber, uint16_t data) { - if(PIOS_BKP_REGISTERS_COUNT < regnumber) - { - PIOS_Assert(0); - } else { - RTC_WriteBackupRegister(pios_bkp_registers_map[regnumber],(uint32_t)data); - } + if (PIOS_BKP_REGISTERS_COUNT < regnumber) { + PIOS_Assert(0); + } else { + RTC_WriteBackupRegister(pios_bkp_registers_map[regnumber], (uint32_t)data); + } } void PIOS_BKP_EnableWrite(void) { - /* Enable write access to Backup domain */ - PWR_BackupAccessCmd(ENABLE); + /* Enable write access to Backup domain */ + PWR_BackupAccessCmd(ENABLE); } void PIOS_BKP_DisableWrite(void) { - /* Enable write access to Backup domain */ - PWR_BackupAccessCmd(DISABLE); + /* Enable write access to Backup domain */ + PWR_BackupAccessCmd(DISABLE); } - /**************************************************************************************** - * Public Data - ****************************************************************************************/ +* Public Data +****************************************************************************************/ diff --git a/flight/pios/stm32f4xx/pios_bl_helper.c b/flight/pios/stm32f4xx/pios_bl_helper.c index 562587819..045779245 100644 --- a/flight/pios/stm32f4xx/pios_bl_helper.c +++ b/flight/pios/stm32f4xx/pios_bl_helper.c @@ -38,7 +38,7 @@ uint8_t *PIOS_BL_HELPER_FLASH_If_Read(uint32_t SectorAddress) { - return (uint8_t *) (SectorAddress); + return (uint8_t *)(SectorAddress); } #if defined(PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT) @@ -47,113 +47,114 @@ static bool erase_flash(uint32_t startAddress, uint32_t endAddress); uint8_t PIOS_BL_HELPER_FLASH_Ini() { - FLASH_Unlock(); - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); - return 1; + FLASH_Unlock(); + FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); + return 1; } struct device_flash_sector { - uint32_t start; - uint32_t size; - uint16_t st_sector; + uint32_t start; + uint32_t size; + uint16_t st_sector; }; static struct device_flash_sector flash_sectors[] = { - [0] = { - .start = 0x08000000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_0, - }, - [1] = { - .start = 0x08004000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_1, - }, - [2] = { - .start = 0x08008000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_2, - }, - [3] = { - .start = 0x0800C000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_3, - }, - [4] = { - .start = 0x08010000, - .size = 64 * 1024, - .st_sector = FLASH_Sector_4, - }, - [5] = { - .start = 0x08020000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_5, - }, - [6] = { - .start = 0x08040000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_6, - }, - [7] = { - .start = 0x08060000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_7, - }, - [8] = { - .start = 0x08080000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_8, - }, - [9] = { - .start = 0x080A0000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_9, - }, - [10] = { - .start = 0x080C0000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_10, - }, - [11] = { - .start = 0x080E0000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_11, - }, + [0] = { + .start = 0x08000000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_0, + }, + [1] = { + .start = 0x08004000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_1, + }, + [2] = { + .start = 0x08008000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_2, + }, + [3] = { + .start = 0x0800C000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_3, + }, + [4] = { + .start = 0x08010000, + .size = 64 * 1024, + .st_sector = FLASH_Sector_4, + }, + [5] = { + .start = 0x08020000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_5, + }, + [6] = { + .start = 0x08040000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_6, + }, + [7] = { + .start = 0x08060000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_7, + }, + [8] = { + .start = 0x08080000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_8, + }, + [9] = { + .start = 0x080A0000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_9, + }, + [10] = { + .start = 0x080C0000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_10, + }, + [11] = { + .start = 0x080E0000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_11, + }, }; -static bool PIOS_BL_HELPER_FLASH_GetSectorInfo(uint32_t address, uint8_t * sector_number, uint32_t *sector_start, uint32_t *sector_size) +static bool PIOS_BL_HELPER_FLASH_GetSectorInfo(uint32_t address, uint8_t *sector_number, uint32_t *sector_start, uint32_t *sector_size) { - for (uint8_t i = 0; i < NELEMENTS(flash_sectors); i++) { - struct device_flash_sector * sector = &flash_sectors[i]; - if ((address >= sector->start) && - (address < (sector->start + sector->size))) { - /* address lies within this sector */ - *sector_number = sector->st_sector; - *sector_start = sector->start; - *sector_size = sector->size; - return (true); - } - } + for (uint8_t i = 0; i < NELEMENTS(flash_sectors); i++) { + struct device_flash_sector *sector = &flash_sectors[i]; + if ((address >= sector->start) && + (address < (sector->start + sector->size))) { + /* address lies within this sector */ + *sector_number = sector->st_sector; + *sector_start = sector->start; + *sector_size = sector->size; + return true; + } + } - return (false); + return false; } uint8_t PIOS_BL_HELPER_FLASH_Start() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; - uint32_t startAddress = bdinfo->fw_base; - uint32_t endAddress = bdinfo->fw_base + bdinfo->fw_size + bdinfo->desc_size; + const struct pios_board_info *bdinfo = &pios_board_info_blob; + uint32_t startAddress = bdinfo->fw_base; + uint32_t endAddress = bdinfo->fw_base + bdinfo->fw_size + bdinfo->desc_size; - bool success = erase_flash(startAddress, endAddress); + bool success = erase_flash(startAddress, endAddress); return (success) ? 1 : 0; } -uint8_t PIOS_BL_HELPER_FLASH_Erase_Bootloader() { +uint8_t PIOS_BL_HELPER_FLASH_Erase_Bootloader() +{ /// Bootloader memory space erase uint32_t startAddress = BL_BANK_BASE; - uint32_t endAddress = BL_BANK_BASE + BL_BANK_SIZE; + uint32_t endAddress = BL_BANK_BASE + BL_BANK_SIZE; bool success = erase_flash(startAddress, endAddress); @@ -164,14 +165,15 @@ static bool erase_flash(uint32_t startAddress, uint32_t endAddress) { uint32_t pageAddress = startAddress; bool fail = false; + while ((pageAddress < endAddress) && (fail == false)) { uint8_t sector_number; uint32_t sector_start; uint32_t sector_size; if (!PIOS_BL_HELPER_FLASH_GetSectorInfo(pageAddress, - §or_number, - §or_start, - §or_size)) { + §or_number, + §or_start, + §or_size)) { /* We're asking for an invalid flash address */ PIOS_Assert(0); } @@ -189,31 +191,33 @@ static bool erase_flash(uint32_t startAddress, uint32_t endAddress) return !fail; } -#endif +#endif /* if defined(PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT) */ uint32_t PIOS_BL_HELPER_CRC_Memory_Calc() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; + const struct pios_board_info *bdinfo = &pios_board_info_blob; - PIOS_BL_HELPER_CRC_Ini(); - CRC_ResetDR(); - CRC_CalcBlockCRC((uint32_t *) bdinfo->fw_base, (bdinfo->fw_size) >> 2); - return CRC_GetCRC(); + PIOS_BL_HELPER_CRC_Ini(); + CRC_ResetDR(); + CRC_CalcBlockCRC((uint32_t *)bdinfo->fw_base, (bdinfo->fw_size) >> 2); + return CRC_GetCRC(); } -void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t * array, uint8_t size) +void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t *array, uint8_t size) { - const struct pios_board_info * bdinfo = &pios_board_info_blob; - uint8_t x = 0; - if (size > bdinfo->desc_size) size = bdinfo->desc_size; - for (uint32_t i = bdinfo->fw_base + bdinfo->fw_size; i < bdinfo->fw_base + bdinfo->fw_size + size; ++i) { - array[x] = *PIOS_BL_HELPER_FLASH_If_Read(i); - ++x; - } + const struct pios_board_info *bdinfo = &pios_board_info_blob; + uint8_t x = 0; + + if (size > bdinfo->desc_size) { + size = bdinfo->desc_size; + } + for (uint32_t i = bdinfo->fw_base + bdinfo->fw_size; i < bdinfo->fw_base + bdinfo->fw_size + size; ++i) { + array[x] = *PIOS_BL_HELPER_FLASH_If_Read(i); + ++x; + } } void PIOS_BL_HELPER_CRC_Ini() -{ -} +{} #endif /* PIOS_INCLUDE_BL_HELPER */ diff --git a/flight/pios/stm32f4xx/pios_debug.c b/flight/pios/stm32f4xx/pios_debug.c index af10d3918..a7c9bf53a 100644 --- a/flight/pios/stm32f4xx/pios_debug.c +++ b/flight/pios/stm32f4xx/pios_debug.c @@ -10,21 +10,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief Debugging Functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,77 +34,77 @@ const char *PIOS_DEBUG_AssertMsg = "ASSERT FAILED"; #ifdef PIOS_ENABLE_DEBUG_PINS -static const struct pios_tim_channel * debug_channels; +static const struct pios_tim_channel *debug_channels; static uint8_t debug_num_channels; -#endif /* PIOS_ENABLE_DEBUG_PINS */ +#endif /* PIOS_ENABLE_DEBUG_PINS */ /** -* Initialise Debug-features -*/ -void PIOS_DEBUG_Init(__attribute__((unused)) const struct pios_tim_channel * channels, __attribute__((unused)) uint8_t num_channels) + * Initialise Debug-features + */ +void PIOS_DEBUG_Init(__attribute__((unused)) const struct pios_tim_channel *channels, __attribute__((unused)) uint8_t num_channels) { #ifdef PIOS_ENABLE_DEBUG_PINS - PIOS_Assert(channels); - PIOS_Assert(num_channels); + PIOS_Assert(channels); + PIOS_Assert(num_channels); - /* Store away the GPIOs we've been given */ - debug_channels = channels; - debug_num_channels = num_channels; + /* Store away the GPIOs we've been given */ + debug_channels = channels; + debug_num_channels = num_channels; - /* Configure the GPIOs we've been given */ - for (uint8_t i = 0; i < num_channels; i++) { - const struct pios_tim_channel * chan = &channels[i]; + /* Configure the GPIOs we've been given */ + for (uint8_t i = 0; i < num_channels; i++) { + const struct pios_tim_channel *chan = &channels[i]; - // Initialise pins as standard output pins - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_InitStructure.GPIO_Pin = chan->pin.init.GPIO_Pin; + // Initialise pins as standard output pins + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_Pin = chan->pin.init.GPIO_Pin; - /* Initialize the GPIO */ - GPIO_Init(chan->pin.gpio, &GPIO_InitStructure); + /* Initialize the GPIO */ + GPIO_Init(chan->pin.gpio, &GPIO_InitStructure); - /* Set the pin low */ - GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); - } + /* Set the pin low */ + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); + } #endif // PIOS_ENABLE_DEBUG_PINS } /** -* Set debug-pin high -* \param pin 0 for S1 output -*/ + * Set debug-pin high + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinHigh(__attribute__((unused)) uint8_t pin) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels || pin >= debug_num_channels) { - return; - } + if (!debug_channels || pin >= debug_num_channels) { + return; + } - const struct pios_tim_channel * chan = &debug_channels[pin]; + const struct pios_tim_channel *chan = &debug_channels[pin]; - GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_SET); + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_SET); #endif // PIOS_ENABLE_DEBUG_PINS } /** -* Set debug-pin low -* \param pin 0 for S1 output -*/ + * Set debug-pin low + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinLow(__attribute__((unused)) uint8_t pin) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels || pin >= debug_num_channels) { - return; - } + if (!debug_channels || pin >= debug_num_channels) { + return; + } - const struct pios_tim_channel * chan = &debug_channels[pin]; + const struct pios_tim_channel *chan = &debug_channels[pin]; - GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); #endif // PIOS_ENABLE_DEBUG_PINS } @@ -113,45 +113,45 @@ void PIOS_DEBUG_PinLow(__attribute__((unused)) uint8_t pin) void PIOS_DEBUG_PinValue8Bit(__attribute__((unused)) uint8_t value) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels) { - return; - } + if (!debug_channels) { + return; + } #pragma message("This code is not portable and should be revised") - PIOS_Assert(0); + PIOS_Assert(0); - uint32_t bsrr_l = ( ((~value)&0x0F)<<(16+6) ) | ((value & 0x0F)<<6); - uint32_t bsrr_h = ( ((~value)&0xF0)<<(16+6-4) ) | ((value & 0xF0)<<(6-4)); + uint32_t bsrr_l = (((~value) & 0x0F) << (16 + 6)) | ((value & 0x0F) << 6); + uint32_t bsrr_h = (((~value) & 0xF0) << (16 + 6 - 4)) | ((value & 0xF0) << (6 - 4)); - PIOS_IRQ_Disable(); + PIOS_IRQ_Disable(); - /* - * This is sketchy since it assumes a particular ordering - * and bitwise layout of the channels provided to the debug code. - */ - //debug_channels[0].pin.gpio->BSRR = bsrr_l; - //debug_channels[4].pin.gpio->BSRR = bsrr_h; + /* + * This is sketchy since it assumes a particular ordering + * and bitwise layout of the channels provided to the debug code. + */ + // debug_channels[0].pin.gpio->BSRR = bsrr_l; + // debug_channels[4].pin.gpio->BSRR = bsrr_h; - PIOS_IRQ_Enable(); + PIOS_IRQ_Enable(); #endif // PIOS_ENABLE_DEBUG_PINS } void PIOS_DEBUG_PinValue4BitL(__attribute__((unused)) uint8_t value) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels) { - return; - } + if (!debug_channels) { + return; + } #pragma message("This code is not portable and should be revised") - PIOS_Assert(0); + PIOS_Assert(0); - /* - * This is sketchy since it assumes a particular ordering - * and bitwise layout of the channels provided to the debug code. - */ - uint32_t bsrr_l = ((~(value & 0x0F)<<(16+6))) | ((value & 0x0F)<<6); - //debug_channels[0].pin.gpio->BSRR = bsrr_l; + /* + * This is sketchy since it assumes a particular ordering + * and bitwise layout of the channels provided to the debug code. + */ + uint32_t bsrr_l = ((~(value & 0x0F) << (16 + 6))) | ((value & 0x0F) << 6); + // debug_channels[0].pin.gpio->BSRR = bsrr_l; #endif // PIOS_ENABLE_DEBUG_PINS } @@ -162,15 +162,17 @@ void PIOS_DEBUG_PinValue4BitL(__attribute__((unused)) uint8_t value) void PIOS_DEBUG_Panic(__attribute__((unused)) const char *msg) { #ifdef PIOS_INCLUDE_DEBUG_CONSOLE - register int *lr asm("lr"); // Link-register holds the PC of the caller - DEBUG_PRINTF(0, "\r%s @0x%x\r", msg, lr); + register int *lr asm ("lr"); // Link-register holds the PC of the caller + DEBUG_PRINTF(0, "\r%s @0x%x\r", msg, lr); #endif - // Stay put - while (1) ; + // Stay put + while (1) { + ; + } } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_delay.c b/flight/pios/stm32f4xx/pios_delay.c index 5faeef2c7..55970d203 100644 --- a/flight/pios/stm32f4xx/pios_delay.c +++ b/flight/pios/stm32f4xx/pios_delay.c @@ -8,25 +8,25 @@ * * @file pios_delay.c * @author Michael Smith Copyright (C) 2012 - * @brief Delay Functions + * @brief Delay Functions * - Provides a micro-second granular delay using the CPU * cycle counter. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,9 @@ #ifdef PIOS_INCLUDE_DELAY /* these should be defined by CMSIS, but they aren't */ -#define DWT_CTRL (*(volatile uint32_t *)0xe0001000) -#define CYCCNTENA (1<<0) -#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004) +#define DWT_CTRL (*(volatile uint32_t *)0xe0001000) +#define CYCCNTENA (1 << 0) +#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004) /* cycles per microsecond */ @@ -51,20 +51,20 @@ static uint32_t us_ticks; int32_t PIOS_DELAY_Init(void) { - RCC_ClocksTypeDef clocks; + RCC_ClocksTypeDef clocks; - /* compute the number of system clocks per microsecond */ - RCC_GetClocksFreq(&clocks); - us_ticks = clocks.SYSCLK_Frequency / 1000000; - PIOS_DEBUG_Assert(us_ticks > 1); + /* compute the number of system clocks per microsecond */ + RCC_GetClocksFreq(&clocks); + us_ticks = clocks.SYSCLK_Frequency / 1000000; + PIOS_DEBUG_Assert(us_ticks > 1); - /* turn on access to the DWT registers */ - CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + /* turn on access to the DWT registers */ + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; - /* enable the CPU cycle counter */ - DWT_CTRL |= CYCCNTENA; + /* enable the CPU cycle counter */ + DWT_CTRL |= CYCCNTENA; - return 0; + return 0; } /** @@ -80,31 +80,32 @@ int32_t PIOS_DELAY_Init(void) */ int32_t PIOS_DELAY_WaituS(uint32_t uS) { - uint32_t elapsed = 0; - uint32_t last_count = DWT_CYCCNT; - - for (;;) { - uint32_t current_count = DWT_CYCCNT; - uint32_t elapsed_uS; + uint32_t elapsed = 0; + uint32_t last_count = DWT_CYCCNT; - /* measure the time elapsed since the last time we checked */ - elapsed += current_count - last_count; - last_count = current_count; + for (;;) { + uint32_t current_count = DWT_CYCCNT; + uint32_t elapsed_uS; - /* convert to microseconds */ - elapsed_uS = elapsed / us_ticks; - if (elapsed_uS >= uS) - break; + /* measure the time elapsed since the last time we checked */ + elapsed += current_count - last_count; + last_count = current_count; - /* reduce the delay by the elapsed time */ - uS -= elapsed_uS; + /* convert to microseconds */ + elapsed_uS = elapsed / us_ticks; + if (elapsed_uS >= uS) { + break; + } - /* keep fractional microseconds for the next iteration */ - elapsed %= us_ticks; - } + /* reduce the delay by the elapsed time */ + uS -= elapsed_uS; - /* No error */ - return 0; + /* keep fractional microseconds for the next iteration */ + elapsed %= us_ticks; + } + + /* No error */ + return 0; } /** @@ -120,21 +121,21 @@ int32_t PIOS_DELAY_WaituS(uint32_t uS) */ int32_t PIOS_DELAY_WaitmS(uint32_t mS) { - while (mS--) { - PIOS_DELAY_WaituS(1000); - } + while (mS--) { + PIOS_DELAY_WaituS(1000); + } - /* No error */ - return 0; + /* No error */ + return 0; } /** - * @brief Query the Delay timer for the current uS + * @brief Query the Delay timer for the current uS * @return A microsecond value */ uint32_t PIOS_DELAY_GetuS() { - return DWT_CYCCNT / us_ticks; + return DWT_CYCCNT / us_ticks; } /** @@ -144,7 +145,7 @@ uint32_t PIOS_DELAY_GetuS() */ uint32_t PIOS_DELAY_GetuSSince(uint32_t t) { - return (PIOS_DELAY_GetuS() - t); + return PIOS_DELAY_GetuS() - t; } /** @@ -153,22 +154,23 @@ uint32_t PIOS_DELAY_GetuSSince(uint32_t t) */ uint32_t PIOS_DELAY_GetRaw() { - return DWT_CYCCNT; + return DWT_CYCCNT; } /** - * @brief Compare to raw times to and convert to us + * @brief Compare to raw times to and convert to us * @return A microsecond value */ uint32_t PIOS_DELAY_DiffuS(uint32_t raw) { - uint32_t diff = DWT_CYCCNT - raw; - return diff / us_ticks; + uint32_t diff = DWT_CYCCNT - raw; + + return diff / us_ticks; } #endif /* PIOS_INCLUDE_DELAY */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_dsm.c b/flight/pios/stm32f4xx/pios_dsm.c index 8ca685011..6b1071a5d 100644 --- a/flight/pios/stm32f4xx/pios_dsm.c +++ b/flight/pios/stm32f4xx/pios_dsm.c @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,130 +41,136 @@ /* Forward Declarations */ static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel); static uint16_t PIOS_DSM_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield); + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield); static void PIOS_DSM_Supervisor(uint32_t dsm_id); /* Local Variables */ const struct pios_rcvr_driver pios_dsm_rcvr_driver = { - .read = PIOS_DSM_Get, + .read = PIOS_DSM_Get, }; enum pios_dsm_dev_magic { - PIOS_DSM_DEV_MAGIC = 0x44534d78, + PIOS_DSM_DEV_MAGIC = 0x44534d78, }; struct pios_dsm_state { - uint16_t channel_data[PIOS_DSM_NUM_INPUTS]; - uint8_t received_data[DSM_FRAME_LENGTH]; - uint8_t receive_timer; - uint8_t failsafe_timer; - uint8_t frame_found; - uint8_t byte_count; + uint16_t channel_data[PIOS_DSM_NUM_INPUTS]; + uint8_t received_data[DSM_FRAME_LENGTH]; + uint8_t receive_timer; + uint8_t failsafe_timer; + uint8_t frame_found; + uint8_t byte_count; #ifdef DSM_LOST_FRAME_COUNTER - uint8_t frames_lost_last; - uint16_t frames_lost; + uint8_t frames_lost_last; + uint16_t frames_lost; #endif }; struct pios_dsm_dev { - enum pios_dsm_dev_magic magic; - const struct pios_dsm_cfg *cfg; - enum pios_dsm_proto proto; - struct pios_dsm_state state; + enum pios_dsm_dev_magic magic; + const struct pios_dsm_cfg *cfg; + enum pios_dsm_proto proto; + struct pios_dsm_state state; }; /* Allocate DSM device descriptor */ #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_dsm_dev *PIOS_DSM_Alloc(void) { - struct pios_dsm_dev *dsm_dev; + struct pios_dsm_dev *dsm_dev; - dsm_dev = (struct pios_dsm_dev *)pvPortMalloc(sizeof(*dsm_dev)); - if (!dsm_dev) - return NULL; + dsm_dev = (struct pios_dsm_dev *)pvPortMalloc(sizeof(*dsm_dev)); + if (!dsm_dev) { + return NULL; + } - dsm_dev->magic = PIOS_DSM_DEV_MAGIC; - return dsm_dev; + dsm_dev->magic = PIOS_DSM_DEV_MAGIC; + return dsm_dev; } #else static struct pios_dsm_dev pios_dsm_devs[PIOS_DSM_MAX_DEVS]; static uint8_t pios_dsm_num_devs; static struct pios_dsm_dev *PIOS_DSM_Alloc(void) { - struct pios_dsm_dev *dsm_dev; + struct pios_dsm_dev *dsm_dev; - if (pios_dsm_num_devs >= PIOS_DSM_MAX_DEVS) - return NULL; + if (pios_dsm_num_devs >= PIOS_DSM_MAX_DEVS) { + return NULL; + } - dsm_dev = &pios_dsm_devs[pios_dsm_num_devs++]; - dsm_dev->magic = PIOS_DSM_DEV_MAGIC; + dsm_dev = &pios_dsm_devs[pios_dsm_num_devs++]; + dsm_dev->magic = PIOS_DSM_DEV_MAGIC; - return dsm_dev; + return dsm_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /* Validate DSM device descriptor */ static bool PIOS_DSM_Validate(struct pios_dsm_dev *dsm_dev) { - return (dsm_dev->magic == PIOS_DSM_DEV_MAGIC); + return dsm_dev->magic == PIOS_DSM_DEV_MAGIC; } /* Try to bind DSMx satellite using specified number of pulses */ static void PIOS_DSM_Bind(struct pios_dsm_dev *dsm_dev, uint8_t bind) { - const struct pios_dsm_cfg *cfg = dsm_dev->cfg; + const struct pios_dsm_cfg *cfg = dsm_dev->cfg; - GPIO_InitTypeDef GPIO_InitStructure = cfg->bind.init; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_InitTypeDef GPIO_InitStructure = cfg->bind.init; - /* just to limit bind pulses */ - if (bind > 10) - bind = 10; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_Init(cfg->bind.gpio, &cfg->bind.init); + /* just to limit bind pulses */ + if (bind > 10) { + bind = 10; + } - /* RX line, set high */ - GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + GPIO_Init(cfg->bind.gpio, &cfg->bind.init); - /* on CC works up to 140ms, guess bind window is around 20-140ms after power up */ - PIOS_DELAY_WaitmS(60); + /* RX line, set high */ + GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - for (int i = 0; i < bind ; i++) { - /* RX line, drive low for 120us */ - GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - PIOS_DELAY_WaituS(120); - /* RX line, drive high for 120us */ - GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - PIOS_DELAY_WaituS(120); - } - /* RX line, set input and wait for data */ - GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure); + /* on CC works up to 140ms, guess bind window is around 20-140ms after power up */ + PIOS_DELAY_WaitmS(60); + + for (int i = 0; i < bind; i++) { + /* RX line, drive low for 120us */ + GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + PIOS_DELAY_WaituS(120); + /* RX line, drive high for 120us */ + GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + PIOS_DELAY_WaituS(120); + } + /* RX line, set input and wait for data */ + GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure); } /* Reset channels in case of lost signal or explicit failsafe receiver flag */ static void PIOS_DSM_ResetChannels(struct pios_dsm_dev *dsm_dev) { - struct pios_dsm_state *state = &(dsm_dev->state); - for (int i = 0; i < PIOS_DSM_NUM_INPUTS; i++) { - state->channel_data[i] = PIOS_RCVR_TIMEOUT; - } + struct pios_dsm_state *state = &(dsm_dev->state); + + for (int i = 0; i < PIOS_DSM_NUM_INPUTS; i++) { + state->channel_data[i] = PIOS_RCVR_TIMEOUT; + } } /* Reset DSM receiver state */ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev) { - struct pios_dsm_state *state = &(dsm_dev->state); - state->receive_timer = 0; - state->failsafe_timer = 0; - state->frame_found = 0; + struct pios_dsm_state *state = &(dsm_dev->state); + + state->receive_timer = 0; + state->failsafe_timer = 0; + state->frame_found = 0; #ifdef DSM_LOST_FRAME_COUNTER - state->frames_lost_last = 0; - state->frames_lost = 0; + state->frames_lost_last = 0; + state->frames_lost = 0; #endif - PIOS_DSM_ResetChannels(dsm_dev); + PIOS_DSM_ResetChannels(dsm_dev); } /** @@ -174,174 +180,182 @@ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev) */ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) { - struct pios_dsm_state *state = &(dsm_dev->state); - uint8_t resolution; + struct pios_dsm_state *state = &(dsm_dev->state); + uint8_t resolution; #ifdef DSM_LOST_FRAME_COUNTER - /* increment the lost frame counter */ - uint8_t frames_lost = state->received_data[0]; - state->frames_lost += (frames_lost - state->frames_lost_last); - state->frames_lost_last = frames_lost; + /* increment the lost frame counter */ + uint8_t frames_lost = state->received_data[0]; + state->frames_lost += (frames_lost - state->frames_lost_last); + state->frames_lost_last = frames_lost; #endif - /* check the frame type assuming master satellite stream */ - uint8_t type = state->received_data[1]; - switch (type) { - case 0x01: - case 0x02: - case 0x12: - /* DSM2, DSMJ stream */ - if (dsm_dev->proto == PIOS_DSM_PROTO_DSM2) { - /* DSM2/DSMJ resolution is known from the header */ - resolution = (type & DSM_DSM2_RES_MASK) ? 11 : 10; - } else { - /* DSMX resolution should explicitly be selected */ - goto stream_error; - } - break; - case 0xA2: - case 0xB2: - /* DSMX stream */ - if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) { - resolution = 10; - } else if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) { - resolution = 11; - } else { - /* DSMX resolution should explicitly be selected */ - goto stream_error; - } - break; - default: - /* unknown yet data stream */ - goto stream_error; - } + /* check the frame type assuming master satellite stream */ + uint8_t type = state->received_data[1]; + switch (type) { + case 0x01: + case 0x02: + case 0x12: + /* DSM2, DSMJ stream */ + if (dsm_dev->proto == PIOS_DSM_PROTO_DSM2) { + /* DSM2/DSMJ resolution is known from the header */ + resolution = (type & DSM_DSM2_RES_MASK) ? 11 : 10; + } else { + /* DSMX resolution should explicitly be selected */ + goto stream_error; + } + break; + case 0xA2: + case 0xB2: + /* DSMX stream */ + if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) { + resolution = 10; + } else if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) { + resolution = 11; + } else { + /* DSMX resolution should explicitly be selected */ + goto stream_error; + } + break; + default: + /* unknown yet data stream */ + goto stream_error; + } - /* unroll channels */ - uint8_t *s = &(state->received_data[2]); - uint16_t mask = (resolution == 10) ? 0x03ff : 0x07ff; + /* unroll channels */ + uint8_t *s = &(state->received_data[2]); + uint16_t mask = (resolution == 10) ? 0x03ff : 0x07ff; - for (int i = 0; i < DSM_CHANNELS_PER_FRAME; i++) { - uint16_t word = ((uint16_t)s[0] << 8) | s[1]; - s += 2; + for (int i = 0; i < DSM_CHANNELS_PER_FRAME; i++) { + uint16_t word = ((uint16_t)s[0] << 8) | s[1]; + s += 2; - /* skip empty channel slot */ - if (word == 0xffff) - continue; + /* skip empty channel slot */ + if (word == 0xffff) { + continue; + } - /* minimal data validation */ - if ((i > 0) && (word & DSM_2ND_FRAME_MASK)) { - /* invalid frame data, ignore rest of the frame */ - goto stream_error; - } + /* minimal data validation */ + if ((i > 0) && (word & DSM_2ND_FRAME_MASK)) { + /* invalid frame data, ignore rest of the frame */ + goto stream_error; + } - /* extract and save the channel value */ - uint8_t channel_num = (word >> resolution) & 0x0f; - if (channel_num < PIOS_DSM_NUM_INPUTS) - state->channel_data[channel_num] = (word & mask); - } + /* extract and save the channel value */ + uint8_t channel_num = (word >> resolution) & 0x0f; + if (channel_num < PIOS_DSM_NUM_INPUTS) { + state->channel_data[channel_num] = (word & mask); + } + } #ifdef DSM_LOST_FRAME_COUNTER - /* put lost frames counter into the last channel for debugging */ - state->channel_data[PIOS_DSM_NUM_INPUTS-1] = state->frames_lost; + /* put lost frames counter into the last channel for debugging */ + state->channel_data[PIOS_DSM_NUM_INPUTS - 1] = state->frames_lost; #endif - /* all channels processed */ - return 0; + /* all channels processed */ + return 0; stream_error: - /* either DSM2 selected with DSMX stream found, or vice-versa */ - return -1; + /* either DSM2 selected with DSMX stream found, or vice-versa */ + return -1; } /* Update decoder state processing input byte from the DSMx stream */ static void PIOS_DSM_UpdateState(struct pios_dsm_dev *dsm_dev, uint8_t byte) { - struct pios_dsm_state *state = &(dsm_dev->state); - if (state->frame_found) { - /* receiving the data frame */ - if (state->byte_count < DSM_FRAME_LENGTH) { - /* store next byte */ - state->received_data[state->byte_count++] = byte; - if (state->byte_count == DSM_FRAME_LENGTH) { - /* full frame received - process and wait for new one */ - if (!PIOS_DSM_UnrollChannels(dsm_dev)) - /* data looking good */ - state->failsafe_timer = 0; + struct pios_dsm_state *state = &(dsm_dev->state); - /* prepare for the next frame */ - state->frame_found = 0; - } - } - } + if (state->frame_found) { + /* receiving the data frame */ + if (state->byte_count < DSM_FRAME_LENGTH) { + /* store next byte */ + state->received_data[state->byte_count++] = byte; + if (state->byte_count == DSM_FRAME_LENGTH) { + /* full frame received - process and wait for new one */ + if (!PIOS_DSM_UnrollChannels(dsm_dev)) { + /* data looking good */ + state->failsafe_timer = 0; + } + + /* prepare for the next frame */ + state->frame_found = 0; + } + } + } } /* Initialise DSM receiver interface */ int32_t PIOS_DSM_Init(uint32_t *dsm_id, - const struct pios_dsm_cfg *cfg, - const struct pios_com_driver *driver, - uint32_t lower_id, - enum pios_dsm_proto proto, - uint8_t bind) + const struct pios_dsm_cfg *cfg, + const struct pios_com_driver *driver, + uint32_t lower_id, + enum pios_dsm_proto proto, + uint8_t bind) { - PIOS_DEBUG_Assert(dsm_id); - PIOS_DEBUG_Assert(cfg); - PIOS_DEBUG_Assert(driver); + PIOS_DEBUG_Assert(dsm_id); + PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(driver); - struct pios_dsm_dev *dsm_dev; + struct pios_dsm_dev *dsm_dev; - dsm_dev = (struct pios_dsm_dev *)PIOS_DSM_Alloc(); - if (!dsm_dev) - return -1; + dsm_dev = (struct pios_dsm_dev *)PIOS_DSM_Alloc(); + if (!dsm_dev) { + return -1; + } - /* Bind the configuration to the device instance */ - dsm_dev->cfg = cfg; - dsm_dev->proto = proto; + /* Bind the configuration to the device instance */ + dsm_dev->cfg = cfg; + dsm_dev->proto = proto; - /* Bind the receiver if requested */ - if (bind) - PIOS_DSM_Bind(dsm_dev, bind); + /* Bind the receiver if requested */ + if (bind) { + PIOS_DSM_Bind(dsm_dev, bind); + } - PIOS_DSM_ResetState(dsm_dev); + PIOS_DSM_ResetState(dsm_dev); - *dsm_id = (uint32_t)dsm_dev; + *dsm_id = (uint32_t)dsm_dev; - /* Set comm driver callback */ - (driver->bind_rx_cb)(lower_id, PIOS_DSM_RxInCallback, *dsm_id); + /* Set comm driver callback */ + (driver->bind_rx_cb)(lower_id, PIOS_DSM_RxInCallback, *dsm_id); - if (!PIOS_RTC_RegisterTickCallback(PIOS_DSM_Supervisor, *dsm_id)) { - PIOS_DEBUG_Assert(0); - } + if (!PIOS_RTC_RegisterTickCallback(PIOS_DSM_Supervisor, *dsm_id)) { + PIOS_DEBUG_Assert(0); + } - return 0; + return 0; } /* Comm byte received callback */ static uint16_t PIOS_DSM_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield) + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield) { - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)context; + struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)context; - bool valid = PIOS_DSM_Validate(dsm_dev); - PIOS_Assert(valid); + bool valid = PIOS_DSM_Validate(dsm_dev); - /* process byte(s) and clear receive timer */ - for (uint8_t i = 0; i < buf_len; i++) { - PIOS_DSM_UpdateState(dsm_dev, buf[i]); - dsm_dev->state.receive_timer = 0; - } + PIOS_Assert(valid); - /* Always signal that we can accept another byte */ - if (headroom) - *headroom = DSM_FRAME_LENGTH; + /* process byte(s) and clear receive timer */ + for (uint8_t i = 0; i < buf_len; i++) { + PIOS_DSM_UpdateState(dsm_dev, buf[i]); + dsm_dev->state.receive_timer = 0; + } - /* We never need a yield */ - *need_yield = false; + /* Always signal that we can accept another byte */ + if (headroom) { + *headroom = DSM_FRAME_LENGTH; + } - /* Always indicate that all bytes were consumed */ - return buf_len; + /* We never need a yield */ + *need_yield = false; + + /* Always indicate that all bytes were consumed */ + return buf_len; } /** @@ -353,17 +367,19 @@ static uint16_t PIOS_DSM_RxInCallback(uint32_t context, */ static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)rcvr_id; + struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)rcvr_id; - if (!PIOS_DSM_Validate(dsm_dev)) - return PIOS_RCVR_INVALID; + if (!PIOS_DSM_Validate(dsm_dev)) { + return PIOS_RCVR_INVALID; + } - /* return error if channel is not available */ - if (channel >= PIOS_DSM_NUM_INPUTS) - return PIOS_RCVR_INVALID; + /* return error if channel is not available */ + if (channel >= PIOS_DSM_NUM_INPUTS) { + return PIOS_RCVR_INVALID; + } - /* may also be PIOS_RCVR_TIMEOUT set by other function */ - return dsm_dev->state.channel_data[channel]; + /* may also be PIOS_RCVR_TIMEOUT set by other function */ + return dsm_dev->state.channel_data[channel]; } /** @@ -380,30 +396,31 @@ static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel) */ static void PIOS_DSM_Supervisor(uint32_t dsm_id) { - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id; + struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id; - bool valid = PIOS_DSM_Validate(dsm_dev); - PIOS_Assert(valid); + bool valid = PIOS_DSM_Validate(dsm_dev); - struct pios_dsm_state *state = &(dsm_dev->state); + PIOS_Assert(valid); - /* waiting for new frame if no bytes were received in 8ms */ - if (++state->receive_timer > 4) { - state->frame_found = 1; - state->byte_count = 0; - state->receive_timer = 0; - } + struct pios_dsm_state *state = &(dsm_dev->state); - /* activate failsafe if no frames have arrived in 102.4ms */ - if (++state->failsafe_timer > 64) { - PIOS_DSM_ResetChannels(dsm_dev); - state->failsafe_timer = 0; - } + /* waiting for new frame if no bytes were received in 8ms */ + if (++state->receive_timer > 4) { + state->frame_found = 1; + state->byte_count = 0; + state->receive_timer = 0; + } + + /* activate failsafe if no frames have arrived in 102.4ms */ + if (++state->failsafe_timer > 64) { + PIOS_DSM_ResetChannels(dsm_dev); + state->failsafe_timer = 0; + } } #endif /* PIOS_INCLUDE_DSM */ -/** +/** * @} * @} */ diff --git a/flight/pios/stm32f4xx/pios_exti.c b/flight/pios/stm32f4xx/pios_exti.c index 3dc1e1158..e32cc98b9 100644 --- a/flight/pios/stm32f4xx/pios_exti.c +++ b/flight/pios/stm32f4xx/pios_exti.c @@ -13,19 +13,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,262 +34,298 @@ #ifdef PIOS_INCLUDE_EXTI /* Map EXTI line to full config */ -#define EXTI_MAX_LINES 16 +#define EXTI_MAX_LINES 16 #define PIOS_EXTI_INVALID 0xFF static uint8_t pios_exti_line_to_cfg_map[EXTI_MAX_LINES] = { - [0 ... EXTI_MAX_LINES-1] = PIOS_EXTI_INVALID, + [0 ... EXTI_MAX_LINES - 1] = PIOS_EXTI_INVALID, }; /* Table of exti configs registered at compile time */ extern struct pios_exti_cfg __start__exti __attribute__((weak)); extern struct pios_exti_cfg __stop__exti __attribute__((weak)); -static uint8_t PIOS_EXTI_line_to_index (uint32_t line) +static uint8_t PIOS_EXTI_line_to_index(uint32_t line) { - switch (line) { - case EXTI_Line0: return 0; - case EXTI_Line1: return 1; - case EXTI_Line2: return 2; - case EXTI_Line3: return 3; - case EXTI_Line4: return 4; - case EXTI_Line5: return 5; - case EXTI_Line6: return 6; - case EXTI_Line7: return 7; - case EXTI_Line8: return 8; - case EXTI_Line9: return 9; - case EXTI_Line10: return 10; - case EXTI_Line11: return 11; - case EXTI_Line12: return 12; - case EXTI_Line13: return 13; - case EXTI_Line14: return 14; - case EXTI_Line15: return 15; - } + switch (line) { + case EXTI_Line0: return 0; - PIOS_Assert(0); - return 0xFF; + case EXTI_Line1: return 1; + + case EXTI_Line2: return 2; + + case EXTI_Line3: return 3; + + case EXTI_Line4: return 4; + + case EXTI_Line5: return 5; + + case EXTI_Line6: return 6; + + case EXTI_Line7: return 7; + + case EXTI_Line8: return 8; + + case EXTI_Line9: return 9; + + case EXTI_Line10: return 10; + + case EXTI_Line11: return 11; + + case EXTI_Line12: return 12; + + case EXTI_Line13: return 13; + + case EXTI_Line14: return 14; + + case EXTI_Line15: return 15; + } + + PIOS_Assert(0); + return 0xFF; } -uint8_t PIOS_EXTI_gpio_port_to_exti_source_port(GPIO_TypeDef * gpio_port) +uint8_t PIOS_EXTI_gpio_port_to_exti_source_port(GPIO_TypeDef *gpio_port) { - switch((uint32_t)gpio_port) { - case (uint32_t)GPIOA: return (EXTI_PortSourceGPIOA); - case (uint32_t)GPIOB: return (EXTI_PortSourceGPIOB); - case (uint32_t)GPIOC: return (EXTI_PortSourceGPIOC); - case (uint32_t)GPIOD: return (EXTI_PortSourceGPIOD); - case (uint32_t)GPIOE: return (EXTI_PortSourceGPIOE); - case (uint32_t)GPIOF: return (EXTI_PortSourceGPIOF); - case (uint32_t)GPIOG: return (EXTI_PortSourceGPIOG); - } + switch ((uint32_t)gpio_port) { + case (uint32_t)GPIOA: return EXTI_PortSourceGPIOA; - PIOS_Assert(0); - return 0xFF; + case (uint32_t)GPIOB: return EXTI_PortSourceGPIOB; + + case (uint32_t)GPIOC: return EXTI_PortSourceGPIOC; + + case (uint32_t)GPIOD: return EXTI_PortSourceGPIOD; + + case (uint32_t)GPIOE: return EXTI_PortSourceGPIOE; + + case (uint32_t)GPIOF: return EXTI_PortSourceGPIOF; + + case (uint32_t)GPIOG: return EXTI_PortSourceGPIOG; + } + + PIOS_Assert(0); + return 0xFF; } uint8_t PIOS_EXTI_gpio_pin_to_exti_source_pin(uint32_t gpio_pin) { - switch((uint32_t)gpio_pin) { - case GPIO_Pin_0: return (GPIO_PinSource0); - case GPIO_Pin_1: return (GPIO_PinSource1); - case GPIO_Pin_2: return (GPIO_PinSource2); - case GPIO_Pin_3: return (GPIO_PinSource3); - case GPIO_Pin_4: return (GPIO_PinSource4); - case GPIO_Pin_5: return (GPIO_PinSource5); - case GPIO_Pin_6: return (GPIO_PinSource6); - case GPIO_Pin_7: return (GPIO_PinSource7); - case GPIO_Pin_8: return (GPIO_PinSource8); - case GPIO_Pin_9: return (GPIO_PinSource9); - case GPIO_Pin_10: return (GPIO_PinSource10); - case GPIO_Pin_11: return (GPIO_PinSource11); - case GPIO_Pin_12: return (GPIO_PinSource12); - case GPIO_Pin_13: return (GPIO_PinSource13); - case GPIO_Pin_14: return (GPIO_PinSource14); - case GPIO_Pin_15: return (GPIO_PinSource15); - } + switch ((uint32_t)gpio_pin) { + case GPIO_Pin_0: return GPIO_PinSource0; - PIOS_Assert(0); - return 0xFF; + case GPIO_Pin_1: return GPIO_PinSource1; + + case GPIO_Pin_2: return GPIO_PinSource2; + + case GPIO_Pin_3: return GPIO_PinSource3; + + case GPIO_Pin_4: return GPIO_PinSource4; + + case GPIO_Pin_5: return GPIO_PinSource5; + + case GPIO_Pin_6: return GPIO_PinSource6; + + case GPIO_Pin_7: return GPIO_PinSource7; + + case GPIO_Pin_8: return GPIO_PinSource8; + + case GPIO_Pin_9: return GPIO_PinSource9; + + case GPIO_Pin_10: return GPIO_PinSource10; + + case GPIO_Pin_11: return GPIO_PinSource11; + + case GPIO_Pin_12: return GPIO_PinSource12; + + case GPIO_Pin_13: return GPIO_PinSource13; + + case GPIO_Pin_14: return GPIO_PinSource14; + + case GPIO_Pin_15: return GPIO_PinSource15; + } + + PIOS_Assert(0); + return 0xFF; } -int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg) +int32_t PIOS_EXTI_Init(const struct pios_exti_cfg *cfg) { - PIOS_Assert(cfg); - PIOS_Assert(&__start__exti); - PIOS_Assert(cfg >= &__start__exti); - PIOS_Assert(cfg < &__stop__exti); + PIOS_Assert(cfg); + PIOS_Assert(&__start__exti); + PIOS_Assert(cfg >= &__start__exti); + PIOS_Assert(cfg < &__stop__exti); - uint8_t cfg_index = cfg - &__start__exti; + uint8_t cfg_index = cfg - &__start__exti; - /* Connect this config to the requested vector */ - uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line); + /* Connect this config to the requested vector */ + uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line); - if (pios_exti_line_to_cfg_map[line_index] != PIOS_EXTI_INVALID) { - /* Someone else already has this mapped */ - goto out_fail; - } + if (pios_exti_line_to_cfg_map[line_index] != PIOS_EXTI_INVALID) { + /* Someone else already has this mapped */ + goto out_fail; + } - /* Bind the config to the exti line */ - pios_exti_line_to_cfg_map[line_index] = cfg_index; + /* Bind the config to the exti line */ + pios_exti_line_to_cfg_map[line_index] = cfg_index; - /* Initialize the GPIO pin */ - GPIO_Init(cfg->pin.gpio, &cfg->pin.init); + /* Initialize the GPIO pin */ + GPIO_Init(cfg->pin.gpio, &cfg->pin.init); - /* Set up the EXTI interrupt source */ - uint8_t exti_source_port = PIOS_EXTI_gpio_port_to_exti_source_port(cfg->pin.gpio); - uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin); - SYSCFG_EXTILineConfig(exti_source_port, exti_source_pin); - EXTI_Init(&cfg->exti.init); + /* Set up the EXTI interrupt source */ + uint8_t exti_source_port = PIOS_EXTI_gpio_port_to_exti_source_port(cfg->pin.gpio); + uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin); + SYSCFG_EXTILineConfig(exti_source_port, exti_source_pin); + EXTI_Init(&cfg->exti.init); - /* Enable the interrupt channel */ - NVIC_Init(&cfg->irq.init); + /* Enable the interrupt channel */ + NVIC_Init(&cfg->irq.init); - return 0; + return 0; out_fail: - return -1; + return -1; } static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index) { - uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index]; + uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index]; - PIOS_Assert(&__start__exti); + PIOS_Assert(&__start__exti); - if (cfg_index > NELEMENTS(pios_exti_line_to_cfg_map) || - cfg_index == PIOS_EXTI_INVALID) { - /* Unconfigured interrupt just fired! */ - return false; - } + if (cfg_index > NELEMENTS(pios_exti_line_to_cfg_map) || + cfg_index == PIOS_EXTI_INVALID) { + /* Unconfigured interrupt just fired! */ + return false; + } - struct pios_exti_cfg * cfg = &__start__exti + cfg_index; - return cfg->vector(); + struct pios_exti_cfg *cfg = &__start__exti + cfg_index; + return cfg->vector(); } /* Bind Interrupt Handlers */ #ifdef PIOS_INCLUDE_FREERTOS -#define PIOS_EXTI_HANDLE_LINE(line, woken) \ - if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ - EXTI_ClearITPendingBit(EXTI_Line##line); \ - woken = PIOS_EXTI_generic_irq_handler(line) ? pdTRUE : woken; \ - } +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + woken = PIOS_EXTI_generic_irq_handler(line) ? pdTRUE : woken; \ + } #else -#define PIOS_EXTI_HANDLE_LINE(line, woken) \ - if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ - EXTI_ClearITPendingBit(EXTI_Line##line); \ - PIOS_EXTI_generic_irq_handler(line); \ - } +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + PIOS_EXTI_generic_irq_handler(line); \ + } #endif -static void PIOS_EXTI_0_irq_handler (void) +static void PIOS_EXTI_0_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; + bool xHigherPriorityTaskWoken; #endif - PIOS_EXTI_HANDLE_LINE(0, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(0, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI0_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_0_irq_handler"))); +void EXTI0_IRQHandler(void) __attribute__((alias("PIOS_EXTI_0_irq_handler"))); -static void PIOS_EXTI_1_irq_handler (void) +static void PIOS_EXTI_1_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; + bool xHigherPriorityTaskWoken; #endif - PIOS_EXTI_HANDLE_LINE(1, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(1, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI1_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_1_irq_handler"))); +void EXTI1_IRQHandler(void) __attribute__((alias("PIOS_EXTI_1_irq_handler"))); -static void PIOS_EXTI_2_irq_handler (void) +static void PIOS_EXTI_2_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; + bool xHigherPriorityTaskWoken; #endif - PIOS_EXTI_HANDLE_LINE(2, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(2, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI2_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_2_irq_handler"))); +void EXTI2_IRQHandler(void) __attribute__((alias("PIOS_EXTI_2_irq_handler"))); -static void PIOS_EXTI_3_irq_handler (void) +static void PIOS_EXTI_3_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; + bool xHigherPriorityTaskWoken; #endif - PIOS_EXTI_HANDLE_LINE(3, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(3, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI3_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_3_irq_handler"))); +void EXTI3_IRQHandler(void) __attribute__((alias("PIOS_EXTI_3_irq_handler"))); -static void PIOS_EXTI_4_irq_handler (void) +static void PIOS_EXTI_4_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; + bool xHigherPriorityTaskWoken; #endif - PIOS_EXTI_HANDLE_LINE(4, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(4, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI4_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_4_irq_handler"))); +void EXTI4_IRQHandler(void) __attribute__((alias("PIOS_EXTI_4_irq_handler"))); -static void PIOS_EXTI_9_5_irq_handler (void) +static void PIOS_EXTI_9_5_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; + bool xHigherPriorityTaskWoken; #endif - PIOS_EXTI_HANDLE_LINE(5, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(6, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(7, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(8, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(9, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(5, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(6, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(7, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(8, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(9, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI9_5_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_9_5_irq_handler"))); +void EXTI9_5_IRQHandler(void) __attribute__((alias("PIOS_EXTI_9_5_irq_handler"))); -static void PIOS_EXTI_15_10_irq_handler (void) +static void PIOS_EXTI_15_10_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; + bool xHigherPriorityTaskWoken; #endif - PIOS_EXTI_HANDLE_LINE(10, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(11, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(12, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(13, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(14, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(15, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(10, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(11, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(12, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(13, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(14, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(15, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI15_10_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_15_10_irq_handler"))); +void EXTI15_10_IRQHandler(void) __attribute__((alias("PIOS_EXTI_15_10_irq_handler"))); #endif /* PIOS_INCLUDE_EXTI */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_flash_internal.c b/flight/pios/stm32f4xx/pios_flash_internal.c index c106573b3..f1beb506d 100644 --- a/flight/pios/stm32f4xx/pios_flash_internal.c +++ b/flight/pios/stm32f4xx/pios_flash_internal.c @@ -2,25 +2,25 @@ ****************************************************************************** * @file pios_flash_internal.c * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 - * @addtogroup + * @addtogroup * @{ - * @addtogroup + * @addtogroup * @{ * @brief Provides a flash driver for the STM32 internal flash sectors *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,153 +34,157 @@ #include struct device_flash_sector { - uint32_t start; - uint32_t size; - uint16_t st_sector; + uint32_t start; + uint32_t size; + uint16_t st_sector; }; static struct device_flash_sector flash_sectors[] = { - [0] = { - .start = 0x08000000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_0, - }, - [1] = { - .start = 0x08004000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_1, - }, - [2] = { - .start = 0x08008000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_2, - }, - [3] = { - .start = 0x0800C000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_3, - }, - [4] = { - .start = 0x08010000, - .size = 64 * 1024, - .st_sector = FLASH_Sector_4, - }, - [5] = { - .start = 0x08020000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_5, - }, - [6] = { - .start = 0x08040000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_6, - }, - [7] = { - .start = 0x08060000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_7, - }, - [8] = { - .start = 0x08080000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_8, - }, - [9] = { - .start = 0x080A0000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_9, - }, - [10] = { - .start = 0x080C0000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_10, - }, - [11] = { - .start = 0x080E0000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_11, - }, + [0] = { + .start = 0x08000000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_0, + }, + [1] = { + .start = 0x08004000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_1, + }, + [2] = { + .start = 0x08008000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_2, + }, + [3] = { + .start = 0x0800C000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_3, + }, + [4] = { + .start = 0x08010000, + .size = 64 * 1024, + .st_sector = FLASH_Sector_4, + }, + [5] = { + .start = 0x08020000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_5, + }, + [6] = { + .start = 0x08040000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_6, + }, + [7] = { + .start = 0x08060000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_7, + }, + [8] = { + .start = 0x08080000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_8, + }, + [9] = { + .start = 0x080A0000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_9, + }, + [10] = { + .start = 0x080C0000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_10, + }, + [11] = { + .start = 0x080E0000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_11, + }, }; -static bool PIOS_Flash_Internal_GetSectorInfo(uint32_t address, uint8_t * sector_number, uint32_t *sector_start, uint32_t *sector_size) +static bool PIOS_Flash_Internal_GetSectorInfo(uint32_t address, uint8_t *sector_number, uint32_t *sector_start, uint32_t *sector_size) { - for (uint8_t i = 0; i < NELEMENTS(flash_sectors); i++) { - struct device_flash_sector * sector = &flash_sectors[i]; - if ((address >= sector->start) && - (address < (sector->start + sector->size))) { - /* address lies within this sector */ - *sector_number = sector->st_sector; - *sector_start = sector->start; - *sector_size = sector->size; - return (true); - } - } + for (uint8_t i = 0; i < NELEMENTS(flash_sectors); i++) { + struct device_flash_sector *sector = &flash_sectors[i]; + if ((address >= sector->start) && + (address < (sector->start + sector->size))) { + /* address lies within this sector */ + *sector_number = sector->st_sector; + *sector_start = sector->start; + *sector_size = sector->size; + return true; + } + } - return (false); + return false; } enum pios_internal_flash_dev_magic { - PIOS_INTERNAL_FLASH_DEV_MAGIC = 0x33445902, + PIOS_INTERNAL_FLASH_DEV_MAGIC = 0x33445902, }; struct pios_internal_flash_dev { - enum pios_internal_flash_dev_magic magic; + enum pios_internal_flash_dev_magic magic; #if defined(PIOS_INCLUDE_FREERTOS) - xSemaphoreHandle transaction_lock; -#endif /* defined(PIOS_INCLUDE_FREERTOS) */ + xSemaphoreHandle transaction_lock; +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ }; -static bool PIOS_Flash_Internal_Validate(struct pios_internal_flash_dev * flash_dev) { - return (flash_dev && (flash_dev->magic == PIOS_INTERNAL_FLASH_DEV_MAGIC)); +static bool PIOS_Flash_Internal_Validate(struct pios_internal_flash_dev *flash_dev) +{ + return flash_dev && (flash_dev->magic == PIOS_INTERNAL_FLASH_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_internal_flash_dev * PIOS_Flash_Internal_alloc(void) +static struct pios_internal_flash_dev *PIOS_Flash_Internal_alloc(void) { - struct pios_internal_flash_dev * flash_dev; + struct pios_internal_flash_dev *flash_dev; - flash_dev = (struct pios_internal_flash_dev *)pvPortMalloc(sizeof(* flash_dev)); - if (!flash_dev) return (NULL); + flash_dev = (struct pios_internal_flash_dev *)pvPortMalloc(sizeof(*flash_dev)); + if (!flash_dev) { + return NULL; + } - flash_dev->magic = PIOS_INTERNAL_FLASH_DEV_MAGIC; + flash_dev->magic = PIOS_INTERNAL_FLASH_DEV_MAGIC; - return(flash_dev); + return flash_dev; } #else static struct pios_internal_flash_dev pios_internal_flash_devs[PIOS_INTERNAL_FLASH_MAX_DEVS]; static uint8_t pios_internal_flash_num_devs; -static struct pios_internal_flash_dev * PIOS_Flash_Internal_alloc(void) +static struct pios_internal_flash_dev *PIOS_Flash_Internal_alloc(void) { - struct pios_internal_flash_dev * flash_dev; + struct pios_internal_flash_dev *flash_dev; - if (pios_internal_flash_num_devs >= PIOS_INTERNAL_FLASH_MAX_DEVS) { - return (NULL); - } + if (pios_internal_flash_num_devs >= PIOS_INTERNAL_FLASH_MAX_DEVS) { + return NULL; + } - flash_dev = &pios_internal_flash_devs[pios_internal_flash_num_devs++]; - flash_dev->magic = PIOS_INTERNAL_FLASH_DEV_MAGIC; + flash_dev = &pios_internal_flash_devs[pios_internal_flash_num_devs++]; + flash_dev->magic = PIOS_INTERNAL_FLASH_DEV_MAGIC; - return (flash_dev); + return flash_dev; } #endif /* defined(PIOS_INCLUDE_FREERTOS) */ -int32_t PIOS_Flash_Internal_Init(uintptr_t * flash_id, __attribute__((unused)) const struct pios_flash_internal_cfg * cfg) +int32_t PIOS_Flash_Internal_Init(uintptr_t *flash_id, __attribute__((unused)) const struct pios_flash_internal_cfg *cfg) { - struct pios_internal_flash_dev * flash_dev; + struct pios_internal_flash_dev *flash_dev; - flash_dev = PIOS_Flash_Internal_alloc(); - if (flash_dev == NULL) - return -1; + flash_dev = PIOS_Flash_Internal_alloc(); + if (flash_dev == NULL) { + return -1; + } #if defined(PIOS_INCLUDE_FREERTOS) - flash_dev->transaction_lock = xSemaphoreCreateMutex(); -#endif /* defined(PIOS_INCLUDE_FREERTOS) */ + flash_dev->transaction_lock = xSemaphoreCreateMutex(); +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ - *flash_id = (uintptr_t) flash_dev; + *flash_id = (uintptr_t)flash_dev; - return 0; + return 0; } /********************************** @@ -192,146 +196,154 @@ int32_t PIOS_Flash_Internal_Init(uintptr_t * flash_id, __attribute__((unused)) c static int32_t PIOS_Flash_Internal_StartTransaction(uintptr_t flash_id) { - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; - if (!PIOS_Flash_Internal_Validate(flash_dev)) - return -1; + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreTake(flash_dev->transaction_lock, portMAX_DELAY) != pdTRUE) - return -2; -#endif /* defined(PIOS_INCLUDE_FREERTOS) */ + if (xSemaphoreTake(flash_dev->transaction_lock, portMAX_DELAY) != pdTRUE) { + return -2; + } +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ - /* Unlock the internal flash so we can write to it */ - FLASH_Unlock(); - return 0; + /* Unlock the internal flash so we can write to it */ + FLASH_Unlock(); + return 0; } static int32_t PIOS_Flash_Internal_EndTransaction(uintptr_t flash_id) { - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; - if (!PIOS_Flash_Internal_Validate(flash_dev)) - return -1; + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreGive(flash_dev->transaction_lock) != pdTRUE) - return -2; -#endif /* defined(PIOS_INCLUDE_FREERTOS) */ + if (xSemaphoreGive(flash_dev->transaction_lock) != pdTRUE) { + return -2; + } +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ - /* Lock the internal flash again so we can no longer write to it */ - FLASH_Lock(); + /* Lock the internal flash again so we can no longer write to it */ + FLASH_Lock(); - return 0; + return 0; } static int32_t PIOS_Flash_Internal_EraseSector(uintptr_t flash_id, uint32_t addr) { - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; - if (!PIOS_Flash_Internal_Validate(flash_dev)) - return -1; + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; + } - uint8_t sector_number; - uint32_t sector_start; - uint32_t sector_size; - if (!PIOS_Flash_Internal_GetSectorInfo(addr, - §or_number, - §or_start, - §or_size)) { - /* We're asking for an invalid flash address */ - return -2; - } + uint8_t sector_number; + uint32_t sector_start; + uint32_t sector_size; + if (!PIOS_Flash_Internal_GetSectorInfo(addr, + §or_number, + §or_start, + §or_size)) { + /* We're asking for an invalid flash address */ + return -2; + } - if (FLASH_EraseSector(sector_number, VoltageRange_3) != FLASH_COMPLETE) - return -3; + if (FLASH_EraseSector(sector_number, VoltageRange_3) != FLASH_COMPLETE) { + return -3; + } - return 0; + return 0; } -static int32_t PIOS_Flash_Internal_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len) +static int32_t PIOS_Flash_Internal_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) { - PIOS_Assert(data); + PIOS_Assert(data); - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; - if (!PIOS_Flash_Internal_Validate(flash_dev)) - return -1; + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; + } - uint8_t sector_number; - uint32_t sector_start; - uint32_t sector_size; + uint8_t sector_number; + uint32_t sector_start; + uint32_t sector_size; - /* Ensure that the base address is in a valid sector */ - if (!PIOS_Flash_Internal_GetSectorInfo(addr, - §or_number, - §or_start, - §or_size)) { - /* We're asking for an invalid flash address */ - return -2; - } + /* Ensure that the base address is in a valid sector */ + if (!PIOS_Flash_Internal_GetSectorInfo(addr, + §or_number, + §or_start, + §or_size)) { + /* We're asking for an invalid flash address */ + return -2; + } - /* Ensure that the entire write occurs within the same sector */ - if ((uintptr_t)addr + len > sector_start + sector_size) { - /* Write crosses the end of the sector */ - return -3; - } + /* Ensure that the entire write occurs within the same sector */ + if ((uintptr_t)addr + len > sector_start + sector_size) { + /* Write crosses the end of the sector */ + return -3; + } - /* Write the data */ - for (uint16_t i = 0; i < len; i++) { - FLASH_Status status; - /* - * This is inefficient. Should try to do word writes. - * Not sure if word writes need to be aligned though. - */ - status = FLASH_ProgramByte(addr + i, data[i]); - PIOS_Assert(status == FLASH_COMPLETE); - } + /* Write the data */ + for (uint16_t i = 0; i < len; i++) { + FLASH_Status status; + /* + * This is inefficient. Should try to do word writes. + * Not sure if word writes need to be aligned though. + */ + status = FLASH_ProgramByte(addr + i, data[i]); + PIOS_Assert(status == FLASH_COMPLETE); + } - return 0; + return 0; } -static int32_t PIOS_Flash_Internal_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len) +static int32_t PIOS_Flash_Internal_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) { - PIOS_Assert(data); + PIOS_Assert(data); - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; - if (!PIOS_Flash_Internal_Validate(flash_dev)) - return -1; + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; + } - uint8_t sector_number; - uint32_t sector_start; - uint32_t sector_size; + uint8_t sector_number; + uint32_t sector_start; + uint32_t sector_size; - /* Ensure that the base address is in a valid sector */ - if (!PIOS_Flash_Internal_GetSectorInfo(addr, - §or_number, - §or_start, - §or_size)) { - /* We're asking for an invalid flash address */ - return -2; - } + /* Ensure that the base address is in a valid sector */ + if (!PIOS_Flash_Internal_GetSectorInfo(addr, + §or_number, + §or_start, + §or_size)) { + /* We're asking for an invalid flash address */ + return -2; + } - /* Ensure that the entire read occurs within the same sector */ - if ((uintptr_t)addr + len > sector_start + sector_size) { - /* Read crosses the end of the sector */ - return -3; - } + /* Ensure that the entire read occurs within the same sector */ + if ((uintptr_t)addr + len > sector_start + sector_size) { + /* Read crosses the end of the sector */ + return -3; + } - /* Read the data into the buffer directly */ - memcpy(data, (void *)addr, len); + /* Read the data into the buffer directly */ + memcpy(data, (void *)addr, len); - return 0; + return 0; } /* Provide a flash driver to external drivers */ const struct pios_flash_driver pios_internal_flash_driver = { - .start_transaction = PIOS_Flash_Internal_StartTransaction, - .end_transaction = PIOS_Flash_Internal_EndTransaction, - .erase_sector = PIOS_Flash_Internal_EraseSector, - .write_data = PIOS_Flash_Internal_WriteData, - .read_data = PIOS_Flash_Internal_ReadData, + .start_transaction = PIOS_Flash_Internal_StartTransaction, + .end_transaction = PIOS_Flash_Internal_EndTransaction, + .erase_sector = PIOS_Flash_Internal_EraseSector, + .write_data = PIOS_Flash_Internal_WriteData, + .read_data = PIOS_Flash_Internal_ReadData, }; #endif /* PIOS_INCLUDE_FLASH_INTERNAL */ diff --git a/flight/pios/stm32f4xx/pios_gpio.c b/flight/pios/stm32f4xx/pios_gpio.c index 18c083d9b..46850f4c1 100644 --- a/flight/pios/stm32f4xx/pios_gpio.c +++ b/flight/pios/stm32f4xx/pios_gpio.c @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,61 +39,62 @@ static GPIO_TypeDef *GPIO_PORT[PIOS_GPIO_NUM] = PIOS_GPIO_PORTS; static const uint32_t GPIO_PIN[PIOS_GPIO_NUM] = PIOS_GPIO_PINS; /** -* Initialises all the GPIO's -*/ + * Initialises all the GPIO's + */ void PIOS_GPIO_Init(void) { - /* Do nothing */ + /* Do nothing */ } /** -* Enable a GPIO Pin -* \param[in] Pin Pin Number -*/ + * Enable a GPIO Pin + * \param[in] Pin Pin Number + */ void PIOS_GPIO_Enable(uint8_t Pin) { - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_InitStructure.GPIO_Pin = GPIO_PIN[Pin]; - GPIO_Init(GPIO_PORT[Pin], &GPIO_InitStructure); + GPIO_InitTypeDef GPIO_InitStructure; - /* GPIO's Off */ - PIOS_GPIO_Off(Pin); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_InitStructure.GPIO_Pin = GPIO_PIN[Pin]; + GPIO_Init(GPIO_PORT[Pin], &GPIO_InitStructure); + + /* GPIO's Off */ + PIOS_GPIO_Off(Pin); } /** -* Turn on Pin -* \param[in] Pin Pin Number -*/ + * Turn on Pin + * \param[in] Pin Pin Number + */ void PIOS_GPIO_On(uint8_t Pin) { - GPIO_ResetBits(GPIO_PORT[Pin], GPIO_PIN[Pin]); + GPIO_ResetBits(GPIO_PORT[Pin], GPIO_PIN[Pin]); } /** -* Turn off Pin -* \param[in] Pin Pin Number -*/ + * Turn off Pin + * \param[in] Pin Pin Number + */ void PIOS_GPIO_Off(uint8_t Pin) { - GPIO_SetBits(GPIO_PORT[Pin], GPIO_PIN[Pin]); + GPIO_SetBits(GPIO_PORT[Pin], GPIO_PIN[Pin]); } /** -* Toggle Pin on/off -* \param[in] Pin Pin Number -*/ + * Toggle Pin on/off + * \param[in] Pin Pin Number + */ void PIOS_GPIO_Toggle(uint8_t Pin) { - GPIO_ToggleBits(GPIO_PORT[Pin], GPIO_PIN[Pin]); + GPIO_ToggleBits(GPIO_PORT[Pin], GPIO_PIN[Pin]); } #endif /* PIOS_INCLUDE_GPIO */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_i2c.c b/flight/pios/stm32f4xx/pios_i2c.c index 9dec896da..4a98cff33 100644 --- a/flight/pios/stm32f4xx/pios_i2c.c +++ b/flight/pios/stm32f4xx/pios_i2c.c @@ -6,25 +6,25 @@ * @brief STM32F4xx Hardware dependent I2C functionality * @{ * - * @file pios_i2c.c + * @file pios_i2c.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief I2C Enable/Disable routines * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,28 +43,28 @@ #include -//#define I2C_HALT_ON_ERRORS +// #define I2C_HALT_ON_ERRORS enum i2c_adapter_event { - I2C_EVENT_BUS_ERROR, - I2C_EVENT_START, - I2C_EVENT_STARTED_MORE_TXN_READ, - I2C_EVENT_STARTED_MORE_TXN_WRITE, - I2C_EVENT_STARTED_LAST_TXN_READ, - I2C_EVENT_STARTED_LAST_TXN_WRITE, - I2C_EVENT_ADDR_SENT_LEN_EQ_0, - I2C_EVENT_ADDR_SENT_LEN_EQ_1, - I2C_EVENT_ADDR_SENT_LEN_EQ_2, - I2C_EVENT_ADDR_SENT_LEN_GT_2, - I2C_EVENT_TRANSFER_DONE_LEN_EQ_0, - I2C_EVENT_TRANSFER_DONE_LEN_EQ_1, - I2C_EVENT_TRANSFER_DONE_LEN_EQ_2, - I2C_EVENT_TRANSFER_DONE_LEN_GT_2, - I2C_EVENT_NACK, - I2C_EVENT_STOPPED, - I2C_EVENT_AUTO, /* FIXME: remove this */ + I2C_EVENT_BUS_ERROR, + I2C_EVENT_START, + I2C_EVENT_STARTED_MORE_TXN_READ, + I2C_EVENT_STARTED_MORE_TXN_WRITE, + I2C_EVENT_STARTED_LAST_TXN_READ, + I2C_EVENT_STARTED_LAST_TXN_WRITE, + I2C_EVENT_ADDR_SENT_LEN_EQ_0, + I2C_EVENT_ADDR_SENT_LEN_EQ_1, + I2C_EVENT_ADDR_SENT_LEN_EQ_2, + I2C_EVENT_ADDR_SENT_LEN_GT_2, + I2C_EVENT_TRANSFER_DONE_LEN_EQ_0, + I2C_EVENT_TRANSFER_DONE_LEN_EQ_1, + I2C_EVENT_TRANSFER_DONE_LEN_EQ_2, + I2C_EVENT_TRANSFER_DONE_LEN_GT_2, + I2C_EVENT_NACK, + I2C_EVENT_STOPPED, + I2C_EVENT_AUTO, /* FIXME: remove this */ - I2C_EVENT_NUM_EVENTS /* Must be last */ + I2C_EVENT_NUM_EVENTS /* Must be last */ }; #if defined(PIOS_I2C_DIAGNOSTICS) @@ -82,11 +82,11 @@ volatile uint8_t i2c_state_history_pointer = 0; volatile enum i2c_adapter_event i2c_state_event_history[I2C_LOG_DEPTH]; volatile uint8_t i2c_state_event_history_pointer; -static uint8_t i2c_fsm_fault_count = 0; +static uint8_t i2c_fsm_fault_count = 0; static uint8_t i2c_bad_event_counter = 0; static uint8_t i2c_error_interrupt_counter = 0; static uint8_t i2c_nack_counter = 0; -static uint8_t i2c_timeout_counter = 0; +static uint8_t i2c_timeout_counter = 0; #endif static void go_fsm_fault(struct pios_i2c_adapter *i2c_adapter); @@ -119,8 +119,8 @@ static void go_w_last_txn_last(struct pios_i2c_adapter *i2c_adapter); static void go_nack(struct pios_i2c_adapter *i2c_adapter); struct i2c_adapter_transition { - void (*entry_fn) (struct pios_i2c_adapter * i2c_adapter); - enum i2c_adapter_state next_state[I2C_EVENT_NUM_EVENTS]; + void (*entry_fn)(struct pios_i2c_adapter *i2c_adapter); + enum i2c_adapter_state next_state[I2C_EVENT_NUM_EVENTS]; }; static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter); @@ -133,624 +133,631 @@ static void i2c_adapter_log_fault(enum pios_i2c_error_type type); static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter); static const struct i2c_adapter_transition i2c_adapter_transitions[I2C_STATE_NUM_STATES] = { - [I2C_STATE_FSM_FAULT] = { - .entry_fn = go_fsm_fault, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, - [I2C_STATE_BUS_ERROR] = { - .entry_fn = go_bus_error, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, + [I2C_STATE_FSM_FAULT] = { + .entry_fn = go_fsm_fault, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, + [I2C_STATE_BUS_ERROR] = { + .entry_fn = go_bus_error, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, - [I2C_STATE_STOPPED] = { - .entry_fn = go_stopped, - .next_state = { - [I2C_EVENT_START] = I2C_STATE_STARTING, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_STOPPED] = { + .entry_fn = go_stopped, + .next_state = { + [I2C_EVENT_START] = I2C_STATE_STARTING, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_STOPPING] = { - .entry_fn = go_stopping, - .next_state = { - [I2C_EVENT_STOPPED] = I2C_STATE_STOPPED, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_STOPPING] = { + .entry_fn = go_stopping, + .next_state = { + [I2C_EVENT_STOPPED] = I2C_STATE_STOPPED, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_STARTING] = { - .entry_fn = go_starting, - .next_state = { - [I2C_EVENT_STARTED_MORE_TXN_READ] = I2C_STATE_R_MORE_TXN_ADDR, - [I2C_EVENT_STARTED_MORE_TXN_WRITE] = I2C_STATE_W_MORE_TXN_ADDR, - [I2C_EVENT_STARTED_LAST_TXN_READ] = I2C_STATE_R_LAST_TXN_ADDR, - [I2C_EVENT_STARTED_LAST_TXN_WRITE] = I2C_STATE_W_LAST_TXN_ADDR, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_STARTING] = { + .entry_fn = go_starting, + .next_state = { + [I2C_EVENT_STARTED_MORE_TXN_READ] = I2C_STATE_R_MORE_TXN_ADDR, + [I2C_EVENT_STARTED_MORE_TXN_WRITE] = I2C_STATE_W_MORE_TXN_ADDR, + [I2C_EVENT_STARTED_LAST_TXN_READ] = I2C_STATE_R_LAST_TXN_ADDR, + [I2C_EVENT_STARTED_LAST_TXN_WRITE] = I2C_STATE_W_LAST_TXN_ADDR, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - /* - * Read with restart - */ + /* + * Read with restart + */ - [I2C_STATE_R_MORE_TXN_ADDR] = { - .entry_fn = go_r_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_PRE_ONE, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_ADDR] = { + .entry_fn = go_r_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_PRE_ONE, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_ONE] = { - .entry_fn = go_r_more_txn_pre_one, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_ONE] = { + .entry_fn = go_r_more_txn_pre_one, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_FIRST] = { - .entry_fn = go_r_any_txn_pre_first, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_FIRST] = { + .entry_fn = go_r_any_txn_pre_first, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_MIDDLE] = { - .entry_fn = go_r_any_txn_pre_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_MIDDLE] = { + .entry_fn = go_r_any_txn_pre_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_LAST] = { - .entry_fn = go_r_more_txn_pre_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_LAST] = { + .entry_fn = go_r_more_txn_pre_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_POST_LAST] = { - .entry_fn = go_r_any_txn_post_last, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STARTING, - }, - }, + [I2C_STATE_R_MORE_TXN_POST_LAST] = { + .entry_fn = go_r_any_txn_post_last, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STARTING, + }, + }, - /* - * Read - */ + /* + * Read + */ - [I2C_STATE_R_LAST_TXN_ADDR] = { - .entry_fn = go_r_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_PRE_ONE, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_ADDR] = { + .entry_fn = go_r_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_PRE_ONE, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_ONE] = { - .entry_fn = go_r_last_txn_pre_one, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_ONE] = { + .entry_fn = go_r_last_txn_pre_one, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_FIRST] = { - .entry_fn = go_r_any_txn_pre_first, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_FIRST] = { + .entry_fn = go_r_any_txn_pre_first, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_MIDDLE] = { - .entry_fn = go_r_any_txn_pre_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_MIDDLE] = { + .entry_fn = go_r_any_txn_pre_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_LAST] = { - .entry_fn = go_r_last_txn_pre_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_LAST] = { + .entry_fn = go_r_last_txn_pre_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_POST_LAST] = { - .entry_fn = go_r_any_txn_post_last, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, + [I2C_STATE_R_LAST_TXN_POST_LAST] = { + .entry_fn = go_r_any_txn_post_last, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, - /* - * Write with restart - */ + /* + * Write with restart + */ - [I2C_STATE_W_MORE_TXN_ADDR] = { - .entry_fn = go_w_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_MORE_TXN_ADDR] = { + .entry_fn = go_w_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_MORE_TXN_MIDDLE] = { - .entry_fn = go_w_any_txn_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_MORE_TXN_MIDDLE] = { + .entry_fn = go_w_any_txn_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_MORE_TXN_LAST] = { - .entry_fn = go_w_more_txn_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STARTING, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_MORE_TXN_LAST] = { + .entry_fn = go_w_more_txn_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STARTING, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - /* - * Write - */ + /* + * Write + */ - [I2C_STATE_W_LAST_TXN_ADDR] = { - .entry_fn = go_w_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_LAST_TXN_ADDR] = { + .entry_fn = go_w_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_LAST_TXN_MIDDLE] = { - .entry_fn = go_w_any_txn_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_LAST_TXN_MIDDLE] = { + .entry_fn = go_w_any_txn_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_LAST_TXN_LAST] = { - .entry_fn = go_w_last_txn_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STOPPING, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, - [I2C_STATE_NACK] = { - .entry_fn = go_nack, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, + [I2C_STATE_W_LAST_TXN_LAST] = { + .entry_fn = go_w_last_txn_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STOPPING, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, + [I2C_STATE_NACK] = { + .entry_fn = go_nack, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, }; static void go_fsm_fault(struct pios_i2c_adapter *i2c_adapter) { #if defined(I2C_HALT_ON_ERRORS) - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); #endif - /* Note that this transfer has hit a bus error */ - i2c_adapter->bus_error = true; + /* Note that this transfer has hit a bus error */ + i2c_adapter->bus_error = true; - i2c_adapter_reset_bus(i2c_adapter); - + i2c_adapter_reset_bus(i2c_adapter); } static void go_bus_error(struct pios_i2c_adapter *i2c_adapter) { - /* Note that this transfer has hit a bus error */ - i2c_adapter->bus_error = true; + /* Note that this transfer has hit a bus error */ + i2c_adapter->bus_error = true; - i2c_adapter_reset_bus(i2c_adapter); + i2c_adapter_reset_bus(i2c_adapter); } static void go_stopping(struct pios_i2c_adapter *i2c_adapter) { #ifdef USE_FREERTOS - signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; + signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; #endif - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); #ifdef USE_FREERTOS - if (xSemaphoreGiveFromISR(i2c_adapter->sem_ready, &pxHigherPriorityTaskWoken) != pdTRUE) { + if (xSemaphoreGiveFromISR(i2c_adapter->sem_ready, &pxHigherPriorityTaskWoken) != pdTRUE) { #if defined(I2C_HALT_ON_ERRORS) - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); #endif - } - portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for this? */ + } + portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for this? */ #endif /* USE_FREERTOS */ - if(i2c_adapter->callback) - i2c_adapter_callback_handler(i2c_adapter); + if (i2c_adapter->callback) { + i2c_adapter_callback_handler(i2c_adapter); + } } static void go_stopped(struct pios_i2c_adapter *i2c_adapter) { - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); } static void go_starting(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]); - i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); + i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]); + i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); - I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); - if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) { - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); - } else { - // For write operations, do not enable the IT_BUF events. - // The current driver does not act when the TX data register is not full, only when the complete byte is sent. - // With the IT_BUF enabled, we constantly get IRQs, See OP-326 - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_ERR, ENABLE); - } + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); + if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) { + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); + } else { + // For write operations, do not enable the IT_BUF events. + // The current driver does not act when the TX data register is not full, only when the complete byte is sent. + // With the IT_BUF enabled, we constantly get IRQs, See OP-326 + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_ERR, ENABLE); + } } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_addr(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ); + PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ); - I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Receiver); + I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Receiver); } static void go_r_more_txn_pre_one(struct pios_i2c_adapter *i2c_adapter) { - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); } static void go_r_last_txn_pre_one(struct pios_i2c_adapter *i2c_adapter) { - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_pre_first(struct pios_i2c_adapter *i2c_adapter) { - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_pre_middle(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } static void go_r_more_txn_pre_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - PIOS_IRQ_Disable(); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); - PIOS_IRQ_Enable(); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + PIOS_IRQ_Disable(); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); + PIOS_IRQ_Enable(); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } static void go_r_last_txn_pre_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - PIOS_IRQ_Disable(); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); - PIOS_IRQ_Enable(); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + PIOS_IRQ_Disable(); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); + PIOS_IRQ_Enable(); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_post_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - /* Move to the next byte */ - i2c_adapter->active_byte++; + /* Move to the next byte */ + i2c_adapter->active_byte++; - /* Move to the next transaction */ - i2c_adapter->active_txn++; + /* Move to the next transaction */ + i2c_adapter->active_txn++; } /* Common to 'more' and 'last' transaction */ static void go_w_any_txn_addr(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_WRITE); + PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_WRITE); - I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Transmitter); + I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Transmitter); } static void go_w_any_txn_middle(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte < i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte < i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); + I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } static void go_w_more_txn_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); + I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); - /* Move to the next byte */ - i2c_adapter->active_byte++; + /* Move to the next byte */ + i2c_adapter->active_byte++; - /* Move to the next transaction */ - i2c_adapter->active_txn++; - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + /* Move to the next transaction */ + i2c_adapter->active_txn++; + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); } static void go_w_last_txn_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_BUF, DISABLE); - I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_BUF, DISABLE); + I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); // SHOULD MOVE THIS INTO A STOPPING STATE AND SET IT ONLY AFTER THE BYTE WAS SENT - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); - /* Move to the next byte */ - i2c_adapter->active_byte++; + /* Move to the next byte */ + i2c_adapter->active_byte++; } -static void go_nack(struct pios_i2c_adapter *i2c_adapter) +static void go_nack(struct pios_i2c_adapter *i2c_adapter) { - i2c_adapter->nack = true; - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + i2c_adapter->nack = true; + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); } static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum i2c_adapter_event event) { - PIOS_IRQ_Disable(); + PIOS_IRQ_Disable(); -#if defined(PIOS_I2C_DIAGNOSTICS) - i2c_state_event_history[i2c_state_event_history_pointer] = event; - i2c_state_event_history_pointer = (i2c_state_event_history_pointer + 1) % I2C_LOG_DEPTH; +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_state_event_history[i2c_state_event_history_pointer] = event; + i2c_state_event_history_pointer = (i2c_state_event_history_pointer + 1) % I2C_LOG_DEPTH; - i2c_state_history[i2c_state_history_pointer] = i2c_adapter->curr_state; - i2c_state_history_pointer = (i2c_state_history_pointer + 1) % I2C_LOG_DEPTH; - - if(i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event] == I2C_STATE_FSM_FAULT) - i2c_adapter_log_fault(PIOS_I2C_ERROR_FSM); -#endif - /* - * Move to the next state - * - * This is done prior to calling the new state's entry function to - * guarantee that the entry function never depends on the previous - * state. This way, it cannot ever know what the previous state was. - */ - i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event]; + i2c_state_history[i2c_state_history_pointer] = i2c_adapter->curr_state; + i2c_state_history_pointer = (i2c_state_history_pointer + 1) % I2C_LOG_DEPTH; - /* Call the entry function (if any) for the next state. */ - if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { - i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); - } + if (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event] == I2C_STATE_FSM_FAULT) { + i2c_adapter_log_fault(PIOS_I2C_ERROR_FSM); + } +#endif + /* + * Move to the next state + * + * This is done prior to calling the new state's entry function to + * guarantee that the entry function never depends on the previous + * state. This way, it cannot ever know what the previous state was. + */ + i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event]; - /* Process any AUTO transitions in the FSM */ - i2c_adapter_process_auto(i2c_adapter); + /* Call the entry function (if any) for the next state. */ + if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { + i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); + } - PIOS_IRQ_Enable(); + /* Process any AUTO transitions in the FSM */ + i2c_adapter_process_auto(i2c_adapter); + + PIOS_IRQ_Enable(); } static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter) { - PIOS_IRQ_Disable(); + PIOS_IRQ_Disable(); - while (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]) { - i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]; + while (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]) { + i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]; - /* Call the entry function (if any) for the next state. */ - if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { - i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); - } - } + /* Call the entry function (if any) for the next state. */ + if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { + i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); + } + } - PIOS_IRQ_Enable(); + PIOS_IRQ_Enable(); } static void i2c_adapter_fsm_init(struct pios_i2c_adapter *i2c_adapter) { - i2c_adapter_reset_bus(i2c_adapter); - i2c_adapter->curr_state = I2C_STATE_STOPPED; + i2c_adapter_reset_bus(i2c_adapter); + i2c_adapter->curr_state = I2C_STATE_STOPPED; } static bool i2c_adapter_wait_for_stopped(struct pios_i2c_adapter *i2c_adapter) { - uint32_t guard; + uint32_t guard; - /* - * Wait for the bus to return to the stopped state. - * This was pulled out of the FSM due to occasional - * failures at this transition which previously resulted - * in spinning on this bit in the ISR forever. - */ - for (guard = 1000000; /* FIXME: should use the configured bus timeout */ - guard && (i2c_adapter->cfg->regs->CR1 & I2C_CR1_STOP); guard--) - continue; - if (!guard) { - /* We timed out waiting for the stop condition */ - return false; - } + /* + * Wait for the bus to return to the stopped state. + * This was pulled out of the FSM due to occasional + * failures at this transition which previously resulted + * in spinning on this bit in the ISR forever. + */ + for (guard = 1000000; /* FIXME: should use the configured bus timeout */ + guard && (i2c_adapter->cfg->regs->CR1 & I2C_CR1_STOP); guard--) { + continue; + } + if (!guard) { + /* We timed out waiting for the stop condition */ + return false; + } - return true; + return true; } static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) { - /* Reset the I2C block */ - I2C_DeInit(i2c_adapter->cfg->regs); + /* Reset the I2C block */ + I2C_DeInit(i2c_adapter->cfg->regs); - /* Make sure the bus is free by clocking it until any slaves release the bus. */ - GPIO_InitTypeDef scl_gpio_init; - scl_gpio_init = i2c_adapter->cfg->scl.init; - scl_gpio_init.GPIO_Mode = GPIO_Mode_OUT; - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - GPIO_Init(i2c_adapter->cfg->scl.gpio, &scl_gpio_init); + /* Make sure the bus is free by clocking it until any slaves release the bus. */ + GPIO_InitTypeDef scl_gpio_init; + scl_gpio_init = i2c_adapter->cfg->scl.init; + scl_gpio_init.GPIO_Mode = GPIO_Mode_OUT; + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + GPIO_Init(i2c_adapter->cfg->scl.gpio, &scl_gpio_init); - GPIO_InitTypeDef sda_gpio_init; - sda_gpio_init = i2c_adapter->cfg->sda.init; - sda_gpio_init.GPIO_Mode = GPIO_Mode_OUT; - GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - GPIO_Init(i2c_adapter->cfg->sda.gpio, &sda_gpio_init); + GPIO_InitTypeDef sda_gpio_init; + sda_gpio_init = i2c_adapter->cfg->sda.init; + sda_gpio_init.GPIO_Mode = GPIO_Mode_OUT; + GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + GPIO_Init(i2c_adapter->cfg->sda.gpio, &sda_gpio_init); - /* Check SDA line to determine if slave is asserting bus and clock out if so, this may */ - /* have to be repeated (due to futher bus errors) but better than clocking 0xFF into an */ - /* ESC */ - //bool sda_hung = GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET; - while(GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET) { - - /* Set clock high and wait for any clock stretching to finish. */ - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) ; - PIOS_DELAY_WaituS(2); - - /* Set clock low */ - GPIO_ResetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - - /* Clock high again */ - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - } - - /* Generate a start then stop condition */ - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); + /* Check SDA line to determine if slave is asserting bus and clock out if so, this may */ + /* have to be repeated (due to futher bus errors) but better than clocking 0xFF into an */ + /* ESC */ + // bool sda_hung = GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET; + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET) { + /* Set clock high and wait for any clock stretching to finish. */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) { + ; + } + PIOS_DELAY_WaituS(2); - /* Set data and clock high and wait for any clock stretching to finish. */ - GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) ; - /* Wait for data to be high */ - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) != Bit_SET) ; + /* Set clock low */ + GPIO_ResetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); - - /* Bus signals are guaranteed to be high (ie. free) after this point */ - /* Initialize the GPIO pins to the peripheral function */ - if (i2c_adapter->cfg->remap) { - GPIO_PinAFConfig(i2c_adapter->cfg->scl.gpio, - __builtin_ctz(i2c_adapter->cfg->scl.init.GPIO_Pin), - i2c_adapter->cfg->remap); - GPIO_PinAFConfig(i2c_adapter->cfg->sda.gpio, - __builtin_ctz(i2c_adapter->cfg->sda.init.GPIO_Pin), - i2c_adapter->cfg->remap); - } - GPIO_Init(i2c_adapter->cfg->scl.gpio, (GPIO_InitTypeDef*)&(i2c_adapter->cfg->scl.init)); // Struct is const, function signature not - GPIO_Init(i2c_adapter->cfg->sda.gpio, (GPIO_InitTypeDef*)&(i2c_adapter->cfg->sda.init)); + /* Clock high again */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + } - /* Reset the I2C block */ - I2C_DeInit(i2c_adapter->cfg->regs); + /* Generate a start then stop condition */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); - /* Initialize the I2C block */ - I2C_Init(i2c_adapter->cfg->regs, (I2C_InitTypeDef*)&(i2c_adapter->cfg->init)); + /* Set data and clock high and wait for any clock stretching to finish. */ + GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) { + ; + } + /* Wait for data to be high */ + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) != Bit_SET) { + ; + } - if (i2c_adapter->cfg->regs->SR2 & I2C_FLAG_BUSY) { - /* Reset the I2C block */ - I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, ENABLE); - I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, DISABLE); - } + + /* Bus signals are guaranteed to be high (ie. free) after this point */ + /* Initialize the GPIO pins to the peripheral function */ + if (i2c_adapter->cfg->remap) { + GPIO_PinAFConfig(i2c_adapter->cfg->scl.gpio, + __builtin_ctz(i2c_adapter->cfg->scl.init.GPIO_Pin), + i2c_adapter->cfg->remap); + GPIO_PinAFConfig(i2c_adapter->cfg->sda.gpio, + __builtin_ctz(i2c_adapter->cfg->sda.init.GPIO_Pin), + i2c_adapter->cfg->remap); + } + GPIO_Init(i2c_adapter->cfg->scl.gpio, (GPIO_InitTypeDef *)&(i2c_adapter->cfg->scl.init)); // Struct is const, function signature not + GPIO_Init(i2c_adapter->cfg->sda.gpio, (GPIO_InitTypeDef *)&(i2c_adapter->cfg->sda.init)); + + /* Reset the I2C block */ + I2C_DeInit(i2c_adapter->cfg->regs); + + /* Initialize the I2C block */ + I2C_Init(i2c_adapter->cfg->regs, (I2C_InitTypeDef *)&(i2c_adapter->cfg->init)); + + if (i2c_adapter->cfg->regs->SR2 & I2C_FLAG_BUSY) { + /* Reset the I2C block */ + I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, ENABLE); + I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, DISABLE); + } } #include @@ -758,54 +765,59 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) /* Return true if the FSM is in a terminal state */ static bool i2c_adapter_fsm_terminated(struct pios_i2c_adapter *i2c_adapter) { - switch (i2c_adapter->curr_state) { - case I2C_STATE_STOPPING: - case I2C_STATE_STOPPED: - return (true); - default: - return (false); - } + switch (i2c_adapter->curr_state) { + case I2C_STATE_STOPPING: + case I2C_STATE_STOPPED: + return true; + + default: + return false; + } } uint32_t i2c_cb_count = 0; -static bool i2c_adapter_callback_handler(struct pios_i2c_adapter * i2c_adapter) +static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter) { - bool semaphore_success = true; - /* Wait for the transfer to complete */ + bool semaphore_success = true; + + /* Wait for the transfer to complete */ #ifdef USE_FREERTOS - portTickType timeout; - timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; - semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); - xSemaphoreGive(i2c_adapter->sem_ready); + portTickType timeout; + timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; + semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); + xSemaphoreGive(i2c_adapter->sem_ready); #endif /* USE_FREERTOS */ - - /* Spin waiting for the transfer to finish */ - while (!i2c_adapter_fsm_terminated(i2c_adapter)) ; - - if (i2c_adapter_wait_for_stopped(i2c_adapter)) { - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOPPED); - } else { - i2c_adapter_fsm_init(i2c_adapter); - } - - // Execute user supplied function - i2c_adapter->callback(); - - i2c_cb_count++; + + /* Spin waiting for the transfer to finish */ + while (!i2c_adapter_fsm_terminated(i2c_adapter)) { + ; + } + + if (i2c_adapter_wait_for_stopped(i2c_adapter)) { + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOPPED); + } else { + i2c_adapter_fsm_init(i2c_adapter); + } + + // Execute user supplied function + i2c_adapter->callback(); + + i2c_cb_count++; #ifdef USE_FREERTOS - /* Unlock the bus */ - xSemaphoreGive(i2c_adapter->sem_busy); - if(!semaphore_success) - i2c_timeout_counter++; + /* Unlock the bus */ + xSemaphoreGive(i2c_adapter->sem_busy); + if (!semaphore_success) { + i2c_timeout_counter++; + } #else - PIOS_IRQ_Disable(); - i2c_adapter->busy = 0; - PIOS_IRQ_Enable(); + PIOS_IRQ_Disable(); + i2c_adapter->busy = 0; + PIOS_IRQ_Enable(); #endif /* USE_FREERTOS */ - - return (!i2c_adapter->bus_error) && semaphore_success; + + return (!i2c_adapter->bus_error) && semaphore_success; } /** @@ -816,29 +828,29 @@ static bool i2c_adapter_callback_handler(struct pios_i2c_adapter * i2c_adapter) void i2c_adapter_log_fault(enum pios_i2c_error_type type) { #if defined(PIOS_I2C_DIAGNOSTICS) - i2c_adapter_fault_history.type = type; - for(uint8_t i = 0; i < I2C_LOG_DEPTH; i++) { - i2c_adapter_fault_history.evirq[i] = - i2c_evirq_history[(I2C_LOG_DEPTH + i2c_evirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - i2c_adapter_fault_history.erirq[i] = - i2c_erirq_history[(I2C_LOG_DEPTH + i2c_erirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - i2c_adapter_fault_history.event[i] = - i2c_state_event_history[(I2C_LOG_DEPTH + i2c_state_event_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - i2c_adapter_fault_history.state[i] = - i2c_state_history[(I2C_LOG_DEPTH + i2c_state_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - } - switch(type) { - case PIOS_I2C_ERROR_EVENT: - i2c_bad_event_counter++; - break; - case PIOS_I2C_ERROR_FSM: - i2c_fsm_fault_count++; - break; - case PIOS_I2C_ERROR_INTERRUPT: - i2c_error_interrupt_counter++; - break; - } -#endif + i2c_adapter_fault_history.type = type; + for (uint8_t i = 0; i < I2C_LOG_DEPTH; i++) { + i2c_adapter_fault_history.evirq[i] = + i2c_evirq_history[(I2C_LOG_DEPTH + i2c_evirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.erirq[i] = + i2c_erirq_history[(I2C_LOG_DEPTH + i2c_erirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.event[i] = + i2c_state_event_history[(I2C_LOG_DEPTH + i2c_state_event_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.state[i] = + i2c_state_history[(I2C_LOG_DEPTH + i2c_state_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + } + switch (type) { + case PIOS_I2C_ERROR_EVENT: + i2c_bad_event_counter++; + break; + case PIOS_I2C_ERROR_FSM: + i2c_fsm_fault_count++; + break; + case PIOS_I2C_ERROR_INTERRUPT: + i2c_error_interrupt_counter++; + break; + } +#endif /* if defined(PIOS_I2C_DIAGNOSTICS) */ } @@ -846,388 +858,404 @@ void i2c_adapter_log_fault(enum pios_i2c_error_type type) * Logs the last N state transitions and N IRQ events due to * an error condition * \param[out] data address where to copy the pios_i2c_fault_history structure to - * \param[out] counts three uint16 that receive the bad event, fsm, and error irq + * \param[out] counts three uint16 that receive the bad event, fsm, and error irq * counts */ -void PIOS_I2C_GetDiagnostics(struct pios_i2c_fault_history * data, uint8_t * counts) +void PIOS_I2C_GetDiagnostics(struct pios_i2c_fault_history *data, uint8_t *counts) { #if defined(PIOS_I2C_DIAGNOSTICS) - memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); - counts[0] = i2c_bad_event_counter; - counts[1] = i2c_fsm_fault_count; - counts[2] = i2c_error_interrupt_counter; - counts[3] = i2c_nack_counter; - counts[4] = i2c_timeout_counter; + memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); + counts[0] = i2c_bad_event_counter; + counts[1] = i2c_fsm_fault_count; + counts[2] = i2c_error_interrupt_counter; + counts[3] = i2c_nack_counter; + counts[4] = i2c_timeout_counter; #else - struct pios_i2c_fault_history i2c_adapter_fault_history; - i2c_adapter_fault_history.type = PIOS_I2C_ERROR_EVENT; + struct pios_i2c_fault_history i2c_adapter_fault_history; + i2c_adapter_fault_history.type = PIOS_I2C_ERROR_EVENT; - memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); - counts[0] = counts[1] = counts[2] = 0; + memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); + counts[0] = counts[1] = counts[2] = 0; #endif } -static bool PIOS_I2C_validate(struct pios_i2c_adapter * i2c_adapter) +static bool PIOS_I2C_validate(struct pios_i2c_adapter *i2c_adapter) { - return (i2c_adapter->magic == PIOS_I2C_DEV_MAGIC); + return i2c_adapter->magic == PIOS_I2C_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) && 0 -static struct pios_i2c_dev * PIOS_I2C_alloc(void) +static struct pios_i2c_dev *PIOS_I2C_alloc(void) { - struct pios_i2c_dev * i2c_adapter; + struct pios_i2c_dev *i2c_adapter; - i2c_adapter = (struct pios_i2c_adapter *)malloc(sizeof(*i2c_adapter)); - if (!i2c_adapter) return(NULL); + i2c_adapter = (struct pios_i2c_adapter *)malloc(sizeof(*i2c_adapter)); + if (!i2c_adapter) { + return NULL; + } - i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; - return(i2c_adapter); + i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; + return i2c_adapter; } #else static struct pios_i2c_adapter pios_i2c_adapters[PIOS_I2C_MAX_DEVS]; static uint8_t pios_i2c_num_adapters; -static struct pios_i2c_adapter * PIOS_I2C_alloc(void) +static struct pios_i2c_adapter *PIOS_I2C_alloc(void) { - struct pios_i2c_adapter * i2c_adapter; + struct pios_i2c_adapter *i2c_adapter; - if (pios_i2c_num_adapters >= PIOS_I2C_MAX_DEVS) { - return (NULL); - } + if (pios_i2c_num_adapters >= PIOS_I2C_MAX_DEVS) { + return NULL; + } - i2c_adapter = &pios_i2c_adapters[pios_i2c_num_adapters++]; - i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; + i2c_adapter = &pios_i2c_adapters[pios_i2c_num_adapters++]; + i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; - return (i2c_adapter); + return i2c_adapter; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) && 0 */ /** -* Initializes IIC driver -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ -int32_t PIOS_I2C_Init(uint32_t * i2c_id, const struct pios_i2c_adapter_cfg * cfg) + * Initializes IIC driver + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ +int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg) { - PIOS_DEBUG_Assert(i2c_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(i2c_id); + PIOS_DEBUG_Assert(cfg); - struct pios_i2c_adapter * i2c_adapter; + struct pios_i2c_adapter *i2c_adapter; - i2c_adapter = (struct pios_i2c_adapter *) PIOS_I2C_alloc(); - if (!i2c_adapter) goto out_fail; + i2c_adapter = (struct pios_i2c_adapter *)PIOS_I2C_alloc(); + if (!i2c_adapter) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - i2c_adapter->cfg = cfg; + /* Bind the configuration to the device instance */ + i2c_adapter->cfg = cfg; #ifdef USE_FREERTOS - /* - * Must be done prior to calling i2c_adapter_fsm_init() - * since the sem_ready mutex is used in the initial state. - */ - vSemaphoreCreateBinary(i2c_adapter->sem_ready); - i2c_adapter->sem_busy = xSemaphoreCreateMutex(); + /* + * Must be done prior to calling i2c_adapter_fsm_init() + * since the sem_ready mutex is used in the initial state. + */ + vSemaphoreCreateBinary(i2c_adapter->sem_ready); + i2c_adapter->sem_busy = xSemaphoreCreateMutex(); #else - i2c_adapter->busy = 0; + i2c_adapter->busy = 0; #endif // USE_FREERTOS - /* Initialize the state machine */ - i2c_adapter_fsm_init(i2c_adapter); + /* Initialize the state machine */ + i2c_adapter_fsm_init(i2c_adapter); - *i2c_id = (uint32_t)i2c_adapter; + *i2c_id = (uint32_t)i2c_adapter; - /* Configure and enable I2C interrupts */ - NVIC_Init((NVIC_InitTypeDef*)&(i2c_adapter->cfg->event.init)); - NVIC_Init((NVIC_InitTypeDef*)&(i2c_adapter->cfg->error.init)); - - /* No error */ - return 0; + /* Configure and enable I2C interrupts */ + NVIC_Init((NVIC_InitTypeDef *)&(i2c_adapter->cfg->event.init)); + NVIC_Init((NVIC_InitTypeDef *)&(i2c_adapter->cfg->error.init)); + + /* No error */ + return 0; out_fail: - return(-1); + return -1; } int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns) { - struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_Assert(valid) + bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_DEBUG_Assert(txn_list); - PIOS_DEBUG_Assert(num_txns); + PIOS_Assert(valid) - bool semaphore_success = true; + PIOS_DEBUG_Assert(txn_list); + PIOS_DEBUG_Assert(num_txns); + + bool semaphore_success = true; #ifdef USE_FREERTOS - /* Lock the bus */ - portTickType timeout; - timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; - if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) - return -2; -#else - PIOS_IRQ_Disable(); - if(i2c_adapter->busy) { - PIOS_IRQ_Enable(); - return -2; - } - i2c_adapter->busy = 1; - PIOS_IRQ_Enable(); + /* Lock the bus */ + portTickType timeout; + timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; + if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { + return -2; + } +#else + PIOS_IRQ_Disable(); + if (i2c_adapter->busy) { + PIOS_IRQ_Enable(); + return -2; + } + i2c_adapter->busy = 1; + PIOS_IRQ_Enable(); #endif /* USE_FREERTOS */ - PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); + PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); - i2c_adapter->first_txn = &txn_list[0]; - i2c_adapter->last_txn = &txn_list[num_txns - 1]; - i2c_adapter->active_txn = i2c_adapter->first_txn; + i2c_adapter->first_txn = &txn_list[0]; + i2c_adapter->last_txn = &txn_list[num_txns - 1]; + i2c_adapter->active_txn = i2c_adapter->first_txn; #ifdef USE_FREERTOS - /* Make sure the done/ready semaphore is consumed before we start */ - semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); + /* Make sure the done/ready semaphore is consumed before we start */ + semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); #endif - i2c_adapter->callback = NULL; - i2c_adapter->bus_error = false; - i2c_adapter->nack = false; - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); + i2c_adapter->callback = NULL; + i2c_adapter->bus_error = false; + i2c_adapter->nack = false; + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); - /* Wait for the transfer to complete */ + /* Wait for the transfer to complete */ #ifdef USE_FREERTOS - semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); - xSemaphoreGive(i2c_adapter->sem_ready); + semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); + xSemaphoreGive(i2c_adapter->sem_ready); #endif /* USE_FREERTOS */ - /* Spin waiting for the transfer to finish */ - while (!i2c_adapter_fsm_terminated(i2c_adapter)) ; + /* Spin waiting for the transfer to finish */ + while (!i2c_adapter_fsm_terminated(i2c_adapter)) { + ; + } - if (i2c_adapter_wait_for_stopped(i2c_adapter)) { - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOPPED); - } else { - i2c_adapter_fsm_init(i2c_adapter); - } + if (i2c_adapter_wait_for_stopped(i2c_adapter)) { + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOPPED); + } else { + i2c_adapter_fsm_init(i2c_adapter); + } #ifdef USE_FREERTOS - /* Unlock the bus */ - xSemaphoreGive(i2c_adapter->sem_busy); - if(!semaphore_success) - i2c_timeout_counter++; + /* Unlock the bus */ + xSemaphoreGive(i2c_adapter->sem_busy); + if (!semaphore_success) { + i2c_timeout_counter++; + } #else - PIOS_IRQ_Disable(); - i2c_adapter->busy = 0; - PIOS_IRQ_Enable(); + PIOS_IRQ_Disable(); + i2c_adapter->busy = 0; + PIOS_IRQ_Enable(); #endif /* USE_FREERTOS */ - return !semaphore_success ? -2 : - i2c_adapter->bus_error ? -1 : - i2c_adapter->nack ? -3 : - 0; + return !semaphore_success ? -2 : + i2c_adapter->bus_error ? -1 : + i2c_adapter->nack ? -3 : + 0; } int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns, void *callback) { - struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - - bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_Assert(valid) - PIOS_Assert(callback); - - PIOS_DEBUG_Assert(txn_list); - PIOS_DEBUG_Assert(num_txns); - - bool semaphore_success = true; - + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + + bool valid = PIOS_I2C_validate(i2c_adapter); + + PIOS_Assert(valid) + PIOS_Assert(callback); + + PIOS_DEBUG_Assert(txn_list); + PIOS_DEBUG_Assert(num_txns); + + bool semaphore_success = true; + #ifdef USE_FREERTOS - /* Lock the bus */ - portTickType timeout; - timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; - if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) - return -2; + /* Lock the bus */ + portTickType timeout; + timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; + if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { + return -2; + } #else - if(i2c_adapter->busy) { - PIOS_IRQ_Enable(); - return -2; - } + if (i2c_adapter->busy) { + PIOS_IRQ_Enable(); + return -2; + } #endif /* USE_FREERTOS */ - - PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); - - i2c_adapter->first_txn = &txn_list[0]; - i2c_adapter->last_txn = &txn_list[num_txns - 1]; - i2c_adapter->active_txn = i2c_adapter->first_txn; - + + PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); + + i2c_adapter->first_txn = &txn_list[0]; + i2c_adapter->last_txn = &txn_list[num_txns - 1]; + i2c_adapter->active_txn = i2c_adapter->first_txn; + #ifdef USE_FREERTOS - /* Make sure the done/ready semaphore is consumed before we start */ - semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); + /* Make sure the done/ready semaphore is consumed before we start */ + semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); #endif - - i2c_adapter->callback = callback; - i2c_adapter->bus_error = false; - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); - - return !semaphore_success ? -2 : 0; + + i2c_adapter->callback = callback; + i2c_adapter->bus_error = false; + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); + + return !semaphore_success ? -2 : 0; } void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id) { - struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_Assert(valid) + bool valid = PIOS_I2C_validate(i2c_adapter); - uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + PIOS_Assert(valid) -#if defined(PIOS_I2C_DIAGNOSTICS) - /* Store event for diagnostics */ - i2c_evirq_history[i2c_evirq_history_pointer] = event; - i2c_evirq_history_pointer = (i2c_evirq_history_pointer + 1) % I2C_LOG_DEPTH; + uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + +#if defined(PIOS_I2C_DIAGNOSTICS) + /* Store event for diagnostics */ + i2c_evirq_history[i2c_evirq_history_pointer] = event; + i2c_evirq_history_pointer = (i2c_evirq_history_pointer + 1) % I2C_LOG_DEPTH; #endif - + #define EVENT_MASK 0x000700FF - event &= EVENT_MASK; - - // This is very poor and inconsistent practice with the FSM since no other - // throw event depends on the current state. However when accelerated (-Os) - // we definitely catch this event twice and there is no clean way to do deal - // with that in the FMS short of a special state for it - if(i2c_adapter->curr_state == I2C_STATE_STARTING && event == 0x70084) - return; + event &= EVENT_MASK; - switch (event) { /* Mask out all the bits we don't care about */ - case (I2C_EVENT_MASTER_MODE_SELECT | 0x40): - /* Unexplained event: EV5 + RxNE : Extraneous Rx. Probably a late NACK from previous read. */ - /* Clean up the extra Rx until the root cause is identified and just keep going */ - (void)I2C_ReceiveData(i2c_adapter->cfg->regs); - /* Fall through */ - case I2C_EVENT_MASTER_MODE_SELECT: /* EV5 */ - switch (i2c_adapter->active_txn->rw) { - case PIOS_I2C_TXN_READ: - if (i2c_adapter->active_txn == i2c_adapter->last_txn) { - /* Final transaction */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_READ); - } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { - /* More transactions follow */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_READ); - } else { - PIOS_DEBUG_Assert(0); - } - break; - case PIOS_I2C_TXN_WRITE: - if (i2c_adapter->active_txn == i2c_adapter->last_txn) { - /* Final transaction */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_WRITE); - } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { - /* More transactions follow */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_WRITE); - } else { - PIOS_DEBUG_Assert(0); - } - break; - default: - PIOS_DEBUG_Assert(0); - break; - } - break; - case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED: /* EV6 */ - case I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED: /* EV6 */ - switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { - case 0: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_0); - break; - case 1: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_1); - break; - case 2: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_2); - break; - default: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_GT_2); - break; - } - break; - case 0x80: /* TxE only. TRA + MSL + BUSY have been cleared before we got here. */ - /* Ignore */ - { - static volatile bool halt = false; - while (halt) ; - } - break; - case 0: /* This triggers an FSM fault sometimes, but not having it stops things working */ - case 0x40: /* RxNE only. MSL + BUSY have already been cleared by HW. */ - case 0x44: /* RxNE + BTF. MSL + BUSY have already been cleared by HW. */ - case I2C_EVENT_MASTER_BYTE_RECEIVED: /* EV7 */ - case (I2C_EVENT_MASTER_BYTE_RECEIVED | 0x4): /* EV7 + BTF */ - case I2C_EVENT_MASTER_BYTE_TRANSMITTED: /* EV8_2 */ - case 0x84: /* TxE + BTF. EV8_2 but TRA + MSL + BUSY have already been cleared by HW. */ - switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { - case 0: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_0); - break; - case 1: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_1); - break; - case 2: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_2); - break; - default: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_GT_2); - break; - } - break; - case I2C_EVENT_MASTER_BYTE_TRANSMITTING: /* EV8 */ - /* Ignore this event and wait for TRANSMITTED in case we can't keep up */ - goto skip_event; - break; - case 0x30084: /* Occurs between byte tranmistted and master mode selected */ - case 0x30000: /* Need to throw away this spurious event */ - case 0x30403 & EVENT_MASK: /* Detected this after got a NACK, probably stop bit */ - goto skip_event; - break; - default: - i2c_adapter_log_fault(PIOS_I2C_ERROR_EVENT); + // This is very poor and inconsistent practice with the FSM since no other + // throw event depends on the current state. However when accelerated (-Os) + // we definitely catch this event twice and there is no clean way to do deal + // with that in the FMS short of a special state for it + if (i2c_adapter->curr_state == I2C_STATE_STARTING && event == 0x70084) { + return; + } + + switch (event) { /* Mask out all the bits we don't care about */ + case (I2C_EVENT_MASTER_MODE_SELECT | 0x40): + /* Unexplained event: EV5 + RxNE : Extraneous Rx. Probably a late NACK from previous read. */ + /* Clean up the extra Rx until the root cause is identified and just keep going */ + (void)I2C_ReceiveData(i2c_adapter->cfg->regs); + /* Fall through */ + case I2C_EVENT_MASTER_MODE_SELECT: /* EV5 */ + switch (i2c_adapter->active_txn->rw) { + case PIOS_I2C_TXN_READ: + if (i2c_adapter->active_txn == i2c_adapter->last_txn) { + /* Final transaction */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_READ); + } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { + /* More transactions follow */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_READ); + } else { + PIOS_DEBUG_Assert(0); + } + break; + case PIOS_I2C_TXN_WRITE: + if (i2c_adapter->active_txn == i2c_adapter->last_txn) { + /* Final transaction */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_WRITE); + } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { + /* More transactions follow */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_WRITE); + } else { + PIOS_DEBUG_Assert(0); + } + break; + default: + PIOS_DEBUG_Assert(0); + break; + } + break; + case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED: /* EV6 */ + case I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED: /* EV6 */ + switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { + case 0: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_0); + break; + case 1: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_1); + break; + case 2: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_2); + break; + default: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_GT_2); + break; + } + break; + case 0x80: /* TxE only. TRA + MSL + BUSY have been cleared before we got here. */ + /* Ignore */ + { + static volatile bool halt = false; + while (halt) { + ; + } + } + break; + case 0: /* This triggers an FSM fault sometimes, but not having it stops things working */ + case 0x40: /* RxNE only. MSL + BUSY have already been cleared by HW. */ + case 0x44: /* RxNE + BTF. MSL + BUSY have already been cleared by HW. */ + case I2C_EVENT_MASTER_BYTE_RECEIVED: /* EV7 */ + case (I2C_EVENT_MASTER_BYTE_RECEIVED | 0x4): /* EV7 + BTF */ + case I2C_EVENT_MASTER_BYTE_TRANSMITTED: /* EV8_2 */ + case 0x84: /* TxE + BTF. EV8_2 but TRA + MSL + BUSY have already been cleared by HW. */ + switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { + case 0: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_0); + break; + case 1: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_1); + break; + case 2: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_2); + break; + default: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_GT_2); + break; + } + break; + case I2C_EVENT_MASTER_BYTE_TRANSMITTING: /* EV8 */ + /* Ignore this event and wait for TRANSMITTED in case we can't keep up */ + goto skip_event; + break; + case 0x30084: /* Occurs between byte tranmistted and master mode selected */ + case 0x30000: /* Need to throw away this spurious event */ + case 0x30403 & EVENT_MASK: /* Detected this after got a NACK, probably stop bit */ + goto skip_event; + break; + default: + i2c_adapter_log_fault(PIOS_I2C_ERROR_EVENT); #if defined(I2C_HALT_ON_ERRORS) - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); #endif - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); - break; - } + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); + break; + } skip_event: - ; + ; } void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id) { - struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_Assert(valid) + bool valid = PIOS_I2C_validate(i2c_adapter); + + PIOS_Assert(valid) #if defined(PIOS_I2C_DIAGNOSTICS) - uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + + i2c_erirq_history[i2c_erirq_history_pointer] = event; + i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % 5; - i2c_erirq_history[i2c_erirq_history_pointer] = event; - i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % 5; - #endif - if(event & I2C_FLAG_AF) { - i2c_nack_counter++; + if (event & I2C_FLAG_AF) { + i2c_nack_counter++; - I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_AF); + I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_AF); - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_NACK); - } else { /* Mostly bus errors here */ - i2c_adapter_log_fault(PIOS_I2C_ERROR_INTERRUPT); - - /* Fail hard on any errors for now */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); - } + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_NACK); + } else { /* Mostly bus errors here */ + i2c_adapter_log_fault(PIOS_I2C_ERROR_INTERRUPT); + + /* Fail hard on any errors for now */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); + } } #endif /* PIOS_INCLUDE_I2C */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_irq.c b/flight/pios/stm32f4xx/pios_irq.c index d14120829..4c6e41008 100644 --- a/flight/pios/stm32f4xx/pios_irq.c +++ b/flight/pios/stm32f4xx/pios_irq.c @@ -6,25 +6,25 @@ * @brief STM32 Hardware code to enable and disable interrupts * @{ * - * @file pios_irq.c + * @file pios_irq.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012 * @brief IRQ Enable/Disable routines * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -46,55 +46,55 @@ static uint32_t nested_ctr; static uint32_t prev_primask; /** -* Disables all interrupts (nested) -* \return < 0 On errors -*/ + * Disables all interrupts (nested) + * \return < 0 On errors + */ int32_t PIOS_IRQ_Disable(void) { - /* Get current priority if nested level == 0 */ - if (!nested_ctr) { - __asm volatile (" mrs %0, primask\n":"=r" (prev_primask) - ); - } + /* Get current priority if nested level == 0 */ + if (!nested_ctr) { + __asm volatile (" mrs %0, primask\n" : "=r" (prev_primask) + ); + } - /* Disable interrupts */ - __asm volatile (" mov r0, #1 \n" " msr primask, r0\n":::"r0"); + /* Disable interrupts */ + __asm volatile (" mov r0, #1 \n" " msr primask, r0\n" ::: "r0"); - ++nested_ctr; + ++nested_ctr; - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Enables all interrupts (nested) -* \return < 0 on errors -* \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) -*/ + * Enables all interrupts (nested) + * \return < 0 on errors + * \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) + */ int32_t PIOS_IRQ_Enable(void) { - /* Check for nesting error */ - if (nested_ctr == 0) { - /* Nesting error */ - return -1; - } + /* Check for nesting error */ + if (nested_ctr == 0) { + /* Nesting error */ + return -1; + } - /* Decrease nesting level */ - --nested_ctr; + /* Decrease nesting level */ + --nested_ctr; - /* Set back previous priority once nested level reached 0 again */ - if (nested_ctr == 0) { - __asm volatile (" msr primask, %0\n"::"r" (prev_primask) - ); - } + /* Set back previous priority once nested level reached 0 again */ + if (nested_ctr == 0) { + __asm volatile (" msr primask, %0\n" ::"r" (prev_primask) + ); + } - /* No error */ - return 0; + /* No error */ + return 0; } #endif /* PIOS_INCLUDE_IRQ */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_led.c b/flight/pios/stm32f4xx/pios_led.c index b0d6a4f88..45174c645 100644 --- a/flight/pios/stm32f4xx/pios_led.c +++ b/flight/pios/stm32f4xx/pios_led.c @@ -6,25 +6,25 @@ * @brief STM32 Hardware LED handling code * @{ * - * @file pios_led.c + * @file pios_led.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief LED functions, init, toggle, on & off. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,65 +34,65 @@ #include -static const struct pios_led_cfg * led_cfg; +static const struct pios_led_cfg *led_cfg; /** * Initialises all the LED's */ -int32_t PIOS_LED_Init(const struct pios_led_cfg * cfg) +int32_t PIOS_LED_Init(const struct pios_led_cfg *cfg) { - PIOS_Assert(cfg); - - /* Store away the config in a global used by API functions */ - led_cfg = cfg; - - for (uint8_t i = 0; i < cfg->num_leds; i++) { - const struct pios_led * led = &(cfg->leds[i]); - - /* Enable the peripheral clock for the GPIO */ - switch ((uint32_t)led->pin.gpio) { - case (uint32_t) GPIOA: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); - break; - case (uint32_t) GPIOB: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); - break; - case (uint32_t) GPIOC: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); - break; - case (uint32_t) GPIOD: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE); - break; - case (uint32_t) GPIOE: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE); - break; - case (uint32_t) GPIOF: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOF, ENABLE); - break; - case (uint32_t) GPIOG: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOG, ENABLE); - break; - case (uint32_t) GPIOH: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOH, ENABLE); - break; - case (uint32_t) GPIOI: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOI, ENABLE); - break; - default: - PIOS_Assert(0); - break; - } - - if (led->remap) { - GPIO_PinAFConfig(led->pin.gpio, led->pin.init.GPIO_Pin, led->remap); - } - - GPIO_Init(led->pin.gpio, &led->pin.init); - - PIOS_LED_Off(i); - } - - return 0; + PIOS_Assert(cfg); + + /* Store away the config in a global used by API functions */ + led_cfg = cfg; + + for (uint8_t i = 0; i < cfg->num_leds; i++) { + const struct pios_led *led = &(cfg->leds[i]); + + /* Enable the peripheral clock for the GPIO */ + switch ((uint32_t)led->pin.gpio) { + case (uint32_t)GPIOA: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); + break; + case (uint32_t)GPIOB: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); + break; + case (uint32_t)GPIOC: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); + break; + case (uint32_t)GPIOD: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE); + break; + case (uint32_t)GPIOE: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE); + break; + case (uint32_t)GPIOF: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOF, ENABLE); + break; + case (uint32_t)GPIOG: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOG, ENABLE); + break; + case (uint32_t)GPIOH: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOH, ENABLE); + break; + case (uint32_t)GPIOI: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOI, ENABLE); + break; + default: + PIOS_Assert(0); + break; + } + + if (led->remap) { + GPIO_PinAFConfig(led->pin.gpio, led->pin.init.GPIO_Pin, led->remap); + } + + GPIO_Init(led->pin.gpio, &led->pin.init); + + PIOS_LED_Off(i); + } + + return 0; } /** @@ -101,19 +101,20 @@ int32_t PIOS_LED_Init(const struct pios_led_cfg * cfg) */ void PIOS_LED_On(uint32_t led_id) { - PIOS_Assert(led_cfg); - - if (led_id >= led_cfg->num_leds) { - /* LED index out of range */ - return; - } - - const struct pios_led * led = &(led_cfg->leds[led_id]); - - if (led->active_high) - GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); - else - GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + PIOS_Assert(led_cfg); + + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } + + const struct pios_led *led = &(led_cfg->leds[led_id]); + + if (led->active_high) { + GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } else { + GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } } /** @@ -122,19 +123,20 @@ void PIOS_LED_On(uint32_t led_id) */ void PIOS_LED_Off(uint32_t led_id) { - PIOS_Assert(led_cfg); - - if (led_id >= led_cfg->num_leds) { - /* LED index out of range */ - return; - } - - const struct pios_led * led = &(led_cfg->leds[led_id]); - - if (led->active_high) - GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); - else - GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + PIOS_Assert(led_cfg); + + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } + + const struct pios_led *led = &(led_cfg->leds[led_id]); + + if (led->active_high) { + GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } else { + GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } } /** @@ -143,26 +145,28 @@ void PIOS_LED_Off(uint32_t led_id) */ void PIOS_LED_Toggle(uint32_t led_id) { - PIOS_Assert(led_cfg); - - if (led_id >= led_cfg->num_leds) { - /* LED index out of range */ - return; - } - - const struct pios_led * led = &(led_cfg->leds[led_id]); - - if (GPIO_ReadOutputDataBit(led->pin.gpio, led->pin.init.GPIO_Pin) == Bit_SET) { - if (led->active_high) - PIOS_LED_Off(led_id); - else - PIOS_LED_On(led_id); - } else { - if (led->active_high) - PIOS_LED_On(led_id); - else - PIOS_LED_Off(led_id); - } + PIOS_Assert(led_cfg); + + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } + + const struct pios_led *led = &(led_cfg->leds[led_id]); + + if (GPIO_ReadOutputDataBit(led->pin.gpio, led->pin.init.GPIO_Pin) == Bit_SET) { + if (led->active_high) { + PIOS_LED_Off(led_id); + } else { + PIOS_LED_On(led_id); + } + } else { + if (led->active_high) { + PIOS_LED_On(led_id); + } else { + PIOS_LED_Off(led_id); + } + } } #endif /* PIOS_INCLUDE_LED */ diff --git a/flight/pios/stm32f4xx/pios_overo.c b/flight/pios/stm32f4xx/pios_overo.c index 609436295..ad8c73275 100644 --- a/flight/pios/stm32f4xx/pios_overo.c +++ b/flight/pios/stm32f4xx/pios_overo.c @@ -3,7 +3,7 @@ * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ * @addtogroup PIOS_OVERO OVERO Functions - * @brief PIOS interface to read and write to overo + * @brief PIOS interface to read and write to overo * @{ * * @file pios_overo.c @@ -53,61 +53,63 @@ static void PIOS_OVERO_TxStart(uint32_t overo_id, uint16_t tx_bytes_avail); static void PIOS_OVERO_RxStart(uint32_t overo_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_overo_com_driver = { - .set_baud = NULL, - .tx_start = PIOS_OVERO_TxStart, - .rx_start = PIOS_OVERO_RxStart, - .bind_tx_cb = PIOS_OVERO_RegisterTxCallback, - .bind_rx_cb = PIOS_OVERO_RegisterRxCallback, + .set_baud = NULL, + .tx_start = PIOS_OVERO_TxStart, + .rx_start = PIOS_OVERO_RxStart, + .bind_tx_cb = PIOS_OVERO_RegisterTxCallback, + .bind_rx_cb = PIOS_OVERO_RegisterRxCallback, }; -//! Data types +// ! Data types enum pios_overo_dev_magic { - PIOS_OVERO_DEV_MAGIC = 0x85A3834A, + PIOS_OVERO_DEV_MAGIC = 0x85A3834A, }; struct pios_overo_dev { - enum pios_overo_dev_magic magic; - const struct pios_overo_cfg * cfg; - - int8_t writing_buffer; - uint32_t writing_offset; - - uint32_t packets; - - uint8_t tx_buffer[2][PACKET_SIZE]; - uint8_t rx_buffer[2][PACKET_SIZE]; + enum pios_overo_dev_magic magic; + const struct pios_overo_cfg *cfg; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + int8_t writing_buffer; + uint32_t writing_offset; + + uint32_t packets; + + uint8_t tx_buffer[2][PACKET_SIZE]; + uint8_t rx_buffer[2][PACKET_SIZE]; + + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; }; #if defined(PIOS_INCLUDE_FREERTOS) -//! Private methods +// ! Private methods static void PIOS_OVERO_WriteData(struct pios_overo_dev *overo_dev); -static bool PIOS_OVERO_validate(struct pios_overo_dev * overo_dev); -static struct pios_overo_dev * PIOS_OVERO_alloc(void); +static bool PIOS_OVERO_validate(struct pios_overo_dev *overo_dev); +static struct pios_overo_dev *PIOS_OVERO_alloc(void); -static bool PIOS_OVERO_validate(struct pios_overo_dev * overo_dev) +static bool PIOS_OVERO_validate(struct pios_overo_dev *overo_dev) { - return (overo_dev->magic == PIOS_OVERO_DEV_MAGIC); + return overo_dev->magic == PIOS_OVERO_DEV_MAGIC; } -static struct pios_overo_dev * PIOS_OVERO_alloc(void) +static struct pios_overo_dev *PIOS_OVERO_alloc(void) { - struct pios_overo_dev * overo_dev; - - overo_dev = (struct pios_overo_dev *)pvPortMalloc(sizeof(*overo_dev)); - if (!overo_dev) return(NULL); - - overo_dev->rx_in_cb = 0; - overo_dev->rx_in_context = 0; - overo_dev->tx_out_cb = 0; - overo_dev->tx_out_context = 0; - overo_dev->packets = 0; - overo_dev->magic = PIOS_OVERO_DEV_MAGIC; - return(overo_dev); + struct pios_overo_dev *overo_dev; + + overo_dev = (struct pios_overo_dev *)pvPortMalloc(sizeof(*overo_dev)); + if (!overo_dev) { + return NULL; + } + + overo_dev->rx_in_cb = 0; + overo_dev->rx_in_context = 0; + overo_dev->tx_out_cb = 0; + overo_dev->tx_out_context = 0; + overo_dev->packets = 0; + overo_dev->magic = PIOS_OVERO_DEV_MAGIC; + return overo_dev; } /** @@ -116,27 +118,26 @@ static struct pios_overo_dev * PIOS_OVERO_alloc(void) */ static void PIOS_OVERO_WriteData(struct pios_overo_dev *overo_dev) { - // TODO: How do we protect against the DMA buffer swapping midway through adding data - // to this buffer. If we were writing at the beginning it could cause a weird race. - if (overo_dev->tx_out_cb) { + // TODO: How do we protect against the DMA buffer swapping midway through adding data + // to this buffer. If we were writing at the beginning it could cause a weird race. + if (overo_dev->tx_out_cb) { + int32_t max_bytes = PACKET_SIZE - overo_dev->writing_offset; - int32_t max_bytes = PACKET_SIZE - overo_dev->writing_offset; + if (max_bytes > 0) { + uint16_t bytes_added; + bool tx_need_yield = false; + uint8_t *writing_pointer = &overo_dev->tx_buffer[overo_dev->writing_buffer][overo_dev->writing_offset]; - if (max_bytes > 0) { - uint16_t bytes_added; - bool tx_need_yield = false; - uint8_t *writing_pointer = &overo_dev->tx_buffer[overo_dev->writing_buffer][overo_dev->writing_offset]; - - bytes_added = (overo_dev->tx_out_cb)(overo_dev->tx_out_context, writing_pointer, max_bytes, NULL, &tx_need_yield); + bytes_added = (overo_dev->tx_out_cb)(overo_dev->tx_out_context, writing_pointer, max_bytes, NULL, &tx_need_yield); #if defined(OVERO_USES_BLOCKING_WRITE) - if (tx_need_yield) { - vPortYieldFromISR(); - } + if (tx_need_yield) { + vPortYieldFromISR(); + } #endif - overo_dev->writing_offset += bytes_added; - } - } + overo_dev->writing_offset += bytes_added; + } + } } /** @@ -145,35 +146,37 @@ static void PIOS_OVERO_WriteData(struct pios_overo_dev *overo_dev) */ void PIOS_OVERO_DMA_irq_handler(uint32_t overo_id) { - struct pios_overo_dev * overo_dev = (struct pios_overo_dev *) overo_id; - if(!PIOS_OVERO_validate(overo_dev)) - return; - - DMA_ClearFlag(overo_dev->cfg->dma.tx.channel, overo_dev->cfg->dma.irq.flags); + struct pios_overo_dev *overo_dev = (struct pios_overo_dev *)overo_id; - overo_dev->writing_buffer = 1 - DMA_GetCurrentMemoryTarget(overo_dev->cfg->dma.tx.channel); - overo_dev->writing_offset = 0; + if (!PIOS_OVERO_validate(overo_dev)) { + return; + } + + DMA_ClearFlag(overo_dev->cfg->dma.tx.channel, overo_dev->cfg->dma.irq.flags); + + overo_dev->writing_buffer = 1 - DMA_GetCurrentMemoryTarget(overo_dev->cfg->dma.tx.channel); + overo_dev->writing_offset = 0; /* bool rx_need_yield; - // Get data from the Rx buffer and add to the fifo - (void) (overo_dev->rx_in_cb)(overo_dev->rx_in_context, - &overo_dev->rx_buffer[overo_dev->writing_buffer][0], - PACKET_SIZE, NULL, &rx_need_yield); + // Get data from the Rx buffer and add to the fifo + (void) (overo_dev->rx_in_cb)(overo_dev->rx_in_context, + &overo_dev->rx_buffer[overo_dev->writing_buffer][0], + PACKET_SIZE, NULL, &rx_need_yield); - if(rx_need_yield) { - vPortYieldFromISR(); - } + if(rx_need_yield) { + vPortYieldFromISR(); + } - // Fill the buffer with known value to prevent rereading these bytes - memset(&overo_dev->rx_buffer[overo_dev->writing_buffer][0], 0xFF, PACKET_SIZE); -*/ - // Fill the buffer with known value to prevent resending any bytes - memset(&overo_dev->tx_buffer[overo_dev->writing_buffer][0], 0xFF, PACKET_SIZE); - - // Load any pending bytes from TX fifo - PIOS_OVERO_WriteData(overo_dev); + // Fill the buffer with known value to prevent rereading these bytes + memset(&overo_dev->rx_buffer[overo_dev->writing_buffer][0], 0xFF, PACKET_SIZE); + */ + // Fill the buffer with known value to prevent resending any bytes + memset(&overo_dev->tx_buffer[overo_dev->writing_buffer][0], 0xFF, PACKET_SIZE); - overo_dev->packets++; + // Load any pending bytes from TX fifo + PIOS_OVERO_WriteData(overo_dev); + + overo_dev->packets++; } /** @@ -181,10 +184,11 @@ void PIOS_OVERO_DMA_irq_handler(uint32_t overo_id) */ int32_t PIOS_OVERO_GetPacketCount(uint32_t overo_id) { - struct pios_overo_dev * overo_dev = (struct pios_overo_dev *) overo_id; - PIOS_Assert(PIOS_OVERO_validate(overo_dev)); + struct pios_overo_dev *overo_dev = (struct pios_overo_dev *)overo_id; - return overo_dev->packets; + PIOS_Assert(PIOS_OVERO_validate(overo_dev)); + + return overo_dev->packets; } /** @@ -192,176 +196,183 @@ int32_t PIOS_OVERO_GetPacketCount(uint32_t overo_id) */ int32_t PIOS_OVERO_GetWrittenBytes(uint32_t overo_id) { - struct pios_overo_dev * overo_dev = (struct pios_overo_dev *) overo_id; - PIOS_Assert(PIOS_OVERO_validate(overo_dev)); - - return overo_dev->writing_offset; + struct pios_overo_dev *overo_dev = (struct pios_overo_dev *)overo_id; + + PIOS_Assert(PIOS_OVERO_validate(overo_dev)); + + return overo_dev->writing_offset; } /** * Initialise a single Overo device */ -int32_t PIOS_OVERO_Init(uint32_t * overo_id, const struct pios_overo_cfg * cfg) +int32_t PIOS_OVERO_Init(uint32_t *overo_id, const struct pios_overo_cfg *cfg) { - PIOS_DEBUG_Assert(overo_id); - PIOS_DEBUG_Assert(cfg); - - struct pios_overo_dev *overo_dev; - - overo_dev = (struct pios_overo_dev *) PIOS_OVERO_alloc(); - if (!overo_dev) goto out_fail; - - /* Bind the configuration to the device instance */ - overo_dev->cfg = cfg; - overo_dev->writing_buffer = 1; // First writes to second buffer + PIOS_DEBUG_Assert(overo_id); + PIOS_DEBUG_Assert(cfg); - /* Put buffers to a known state */ - memset(&overo_dev->tx_buffer[0][0], 0xFF, PACKET_SIZE); - memset(&overo_dev->tx_buffer[1][0], 0xFF, PACKET_SIZE); - memset(&overo_dev->rx_buffer[0][0], 0xFF, PACKET_SIZE); - memset(&overo_dev->rx_buffer[1][0], 0xFF, PACKET_SIZE); + struct pios_overo_dev *overo_dev; - /* - * Enable the SPI device - * - * 1. Enable the SPI port - * 2. Enable DMA with circular buffered DMA (validate config) - * 3. Enable the DMA Tx IRQ - */ - - //PIOS_Assert(overo_dev->cfg->dma.tx-> == CIRCULAR); - //PIOS_Assert(overo_dev->cfg->dma.rx-> == CIRCULAR); - - /* only legal for single-slave config */ - PIOS_Assert(overo_dev->cfg->slave_count == 1); - SPI_SSOutputCmd(overo_dev->cfg->regs, DISABLE); - - /* Initialize the GPIO pins */ - /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ - GPIO_PinAFConfig(overo_dev->cfg->sclk.gpio, - __builtin_ctz(overo_dev->cfg->sclk.init.GPIO_Pin), - overo_dev->cfg->remap); - GPIO_PinAFConfig(overo_dev->cfg->mosi.gpio, - __builtin_ctz(overo_dev->cfg->mosi.init.GPIO_Pin), - overo_dev->cfg->remap); - GPIO_PinAFConfig(overo_dev->cfg->miso.gpio, - __builtin_ctz(overo_dev->cfg->miso.init.GPIO_Pin), - overo_dev->cfg->remap); - GPIO_PinAFConfig(overo_dev->cfg->ssel[0].gpio, - __builtin_ctz(overo_dev->cfg->ssel[0].init.GPIO_Pin), - overo_dev->cfg->remap); - - GPIO_Init(overo_dev->cfg->sclk.gpio, (GPIO_InitTypeDef*)&(overo_dev->cfg->sclk.init)); - GPIO_Init(overo_dev->cfg->mosi.gpio, (GPIO_InitTypeDef*)&(overo_dev->cfg->mosi.init)); - GPIO_Init(overo_dev->cfg->miso.gpio, (GPIO_InitTypeDef*)&(overo_dev->cfg->miso.init)); - - /* Configure circular buffer targets. Configure 0 to be initially active */ - DMA_InitTypeDef dma_init; + overo_dev = (struct pios_overo_dev *)PIOS_OVERO_alloc(); + if (!overo_dev) { + goto out_fail; + } - DMA_DeInit(overo_dev->cfg->dma.rx.channel); - dma_init = overo_dev->cfg->dma.rx.init; - dma_init.DMA_Memory0BaseAddr = (uint32_t) overo_dev->rx_buffer[0]; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - dma_init.DMA_BufferSize = PACKET_SIZE; - DMA_Init(overo_dev->cfg->dma.rx.channel, &dma_init); - DMA_DoubleBufferModeConfig(overo_dev->cfg->dma.rx.channel, (uint32_t) overo_dev->rx_buffer[1], DMA_Memory_0); - DMA_DoubleBufferModeCmd(overo_dev->cfg->dma.rx.channel, ENABLE); - - DMA_DeInit(overo_dev->cfg->dma.tx.channel); - dma_init = overo_dev->cfg->dma.tx.init; - dma_init.DMA_Memory0BaseAddr = (uint32_t) overo_dev->tx_buffer[0]; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - dma_init.DMA_BufferSize = PACKET_SIZE; - DMA_Init(overo_dev->cfg->dma.tx.channel, &dma_init); - DMA_DoubleBufferModeConfig(overo_dev->cfg->dma.tx.channel, (uint32_t) overo_dev->tx_buffer[1], DMA_Memory_0); - DMA_DoubleBufferModeCmd(overo_dev->cfg->dma.tx.channel, ENABLE); + /* Bind the configuration to the device instance */ + overo_dev->cfg = cfg; + overo_dev->writing_buffer = 1; // First writes to second buffer - /* Set the packet size */ - DMA_SetCurrDataCounter(overo_dev->cfg->dma.rx.channel, PACKET_SIZE); - DMA_SetCurrDataCounter(overo_dev->cfg->dma.tx.channel, PACKET_SIZE); + /* Put buffers to a known state */ + memset(&overo_dev->tx_buffer[0][0], 0xFF, PACKET_SIZE); + memset(&overo_dev->tx_buffer[1][0], 0xFF, PACKET_SIZE); + memset(&overo_dev->rx_buffer[0][0], 0xFF, PACKET_SIZE); + memset(&overo_dev->rx_buffer[1][0], 0xFF, PACKET_SIZE); - /* Initialize the SPI block */ - SPI_DeInit(overo_dev->cfg->regs); - SPI_Init(overo_dev->cfg->regs, (SPI_InitTypeDef*)&(overo_dev->cfg->init)); - - SPI_CalculateCRC(overo_dev->cfg->regs, DISABLE); - - /* Enable SPI */ - SPI_Cmd(overo_dev->cfg->regs, ENABLE); - - /* Enable SPI interrupts to DMA */ - SPI_I2S_DMACmd(overo_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); + /* + * Enable the SPI device + * + * 1. Enable the SPI port + * 2. Enable DMA with circular buffered DMA (validate config) + * 3. Enable the DMA Tx IRQ + */ - /* Configure DMA interrupt */ - NVIC_Init((NVIC_InitTypeDef*)&(overo_dev->cfg->dma.irq.init)); - DMA_ITConfig(overo_dev->cfg->dma.tx.channel, DMA_IT_TC, ENABLE); + // PIOS_Assert(overo_dev->cfg->dma.tx-> == CIRCULAR); + // PIOS_Assert(overo_dev->cfg->dma.rx-> == CIRCULAR); - /* Enable the DMA channels */ - DMA_Cmd(overo_dev->cfg->dma.tx.channel, ENABLE); - DMA_Cmd(overo_dev->cfg->dma.rx.channel, ENABLE); - - *overo_id = (uint32_t) overo_dev; + /* only legal for single-slave config */ + PIOS_Assert(overo_dev->cfg->slave_count == 1); + SPI_SSOutputCmd(overo_dev->cfg->regs, DISABLE); + + /* Initialize the GPIO pins */ + /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ + GPIO_PinAFConfig(overo_dev->cfg->sclk.gpio, + __builtin_ctz(overo_dev->cfg->sclk.init.GPIO_Pin), + overo_dev->cfg->remap); + GPIO_PinAFConfig(overo_dev->cfg->mosi.gpio, + __builtin_ctz(overo_dev->cfg->mosi.init.GPIO_Pin), + overo_dev->cfg->remap); + GPIO_PinAFConfig(overo_dev->cfg->miso.gpio, + __builtin_ctz(overo_dev->cfg->miso.init.GPIO_Pin), + overo_dev->cfg->remap); + GPIO_PinAFConfig(overo_dev->cfg->ssel[0].gpio, + __builtin_ctz(overo_dev->cfg->ssel[0].init.GPIO_Pin), + overo_dev->cfg->remap); + + GPIO_Init(overo_dev->cfg->sclk.gpio, (GPIO_InitTypeDef *)&(overo_dev->cfg->sclk.init)); + GPIO_Init(overo_dev->cfg->mosi.gpio, (GPIO_InitTypeDef *)&(overo_dev->cfg->mosi.init)); + GPIO_Init(overo_dev->cfg->miso.gpio, (GPIO_InitTypeDef *)&(overo_dev->cfg->miso.init)); + + /* Configure circular buffer targets. Configure 0 to be initially active */ + DMA_InitTypeDef dma_init; + + DMA_DeInit(overo_dev->cfg->dma.rx.channel); + dma_init = overo_dev->cfg->dma.rx.init; + dma_init.DMA_Memory0BaseAddr = (uint32_t)overo_dev->rx_buffer[0]; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + dma_init.DMA_BufferSize = PACKET_SIZE; + DMA_Init(overo_dev->cfg->dma.rx.channel, &dma_init); + DMA_DoubleBufferModeConfig(overo_dev->cfg->dma.rx.channel, (uint32_t)overo_dev->rx_buffer[1], DMA_Memory_0); + DMA_DoubleBufferModeCmd(overo_dev->cfg->dma.rx.channel, ENABLE); + + DMA_DeInit(overo_dev->cfg->dma.tx.channel); + dma_init = overo_dev->cfg->dma.tx.init; + dma_init.DMA_Memory0BaseAddr = (uint32_t)overo_dev->tx_buffer[0]; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + dma_init.DMA_BufferSize = PACKET_SIZE; + DMA_Init(overo_dev->cfg->dma.tx.channel, &dma_init); + DMA_DoubleBufferModeConfig(overo_dev->cfg->dma.tx.channel, (uint32_t)overo_dev->tx_buffer[1], DMA_Memory_0); + DMA_DoubleBufferModeCmd(overo_dev->cfg->dma.tx.channel, ENABLE); + + /* Set the packet size */ + DMA_SetCurrDataCounter(overo_dev->cfg->dma.rx.channel, PACKET_SIZE); + DMA_SetCurrDataCounter(overo_dev->cfg->dma.tx.channel, PACKET_SIZE); + + /* Initialize the SPI block */ + SPI_DeInit(overo_dev->cfg->regs); + SPI_Init(overo_dev->cfg->regs, (SPI_InitTypeDef *)&(overo_dev->cfg->init)); + + SPI_CalculateCRC(overo_dev->cfg->regs, DISABLE); + + /* Enable SPI */ + SPI_Cmd(overo_dev->cfg->regs, ENABLE); + + /* Enable SPI interrupts to DMA */ + SPI_I2S_DMACmd(overo_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); + + /* Configure DMA interrupt */ + NVIC_Init((NVIC_InitTypeDef *)&(overo_dev->cfg->dma.irq.init)); + DMA_ITConfig(overo_dev->cfg->dma.tx.channel, DMA_IT_TC, ENABLE); + + /* Enable the DMA channels */ + DMA_Cmd(overo_dev->cfg->dma.tx.channel, ENABLE); + DMA_Cmd(overo_dev->cfg->dma.rx.channel, ENABLE); + + *overo_id = (uint32_t)overo_dev; + + return 0; - return(0); - out_fail: - return(-1); + return -1; } static void PIOS_OVERO_RxStart(uint32_t overo_id, __attribute__((unused)) uint16_t rx_bytes_avail) { - struct pios_overo_dev * overo_dev = (struct pios_overo_dev *)overo_id; - - bool valid = PIOS_OVERO_validate(overo_dev); - PIOS_Assert(valid); - - // DMA RX enable (enable IRQ) ? + struct pios_overo_dev *overo_dev = (struct pios_overo_dev *)overo_id; + + bool valid = PIOS_OVERO_validate(overo_dev); + + PIOS_Assert(valid); + + // DMA RX enable (enable IRQ) ? } static void PIOS_OVERO_TxStart(uint32_t overo_id, __attribute__((unused)) uint16_t tx_bytes_avail) { - struct pios_overo_dev * overo_dev = (struct pios_overo_dev *)overo_id; - - bool valid = PIOS_OVERO_validate(overo_dev); - PIOS_Assert(valid); - - // DMA TX enable (enable IRQ) ? + struct pios_overo_dev *overo_dev = (struct pios_overo_dev *)overo_id; - // Load any pending bytes from TX fifo - //PIOS_OVERO_WriteData(overo_dev); + bool valid = PIOS_OVERO_validate(overo_dev); + + PIOS_Assert(valid); + + // DMA TX enable (enable IRQ) ? + + // Load any pending bytes from TX fifo + // PIOS_OVERO_WriteData(overo_dev); } static void PIOS_OVERO_RegisterRxCallback(uint32_t overo_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_overo_dev * overo_dev = (struct pios_overo_dev *)overo_id; - - bool valid = PIOS_OVERO_validate(overo_dev); - PIOS_Assert(valid); - - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - overo_dev->rx_in_context = context; - overo_dev->rx_in_cb = rx_in_cb; + struct pios_overo_dev *overo_dev = (struct pios_overo_dev *)overo_id; + + bool valid = PIOS_OVERO_validate(overo_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + overo_dev->rx_in_context = context; + overo_dev->rx_in_cb = rx_in_cb; } static void PIOS_OVERO_RegisterTxCallback(uint32_t overo_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_overo_dev * overo_dev = (struct pios_overo_dev *)overo_id; - - bool valid = PIOS_OVERO_validate(overo_dev); - PIOS_Assert(valid); - - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - overo_dev->tx_out_context = context; - overo_dev->tx_out_cb = tx_out_cb; + struct pios_overo_dev *overo_dev = (struct pios_overo_dev *)overo_id; + + bool valid = PIOS_OVERO_validate(overo_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + overo_dev->tx_out_context = context; + overo_dev->tx_out_cb = tx_out_cb; } -#else +#else /* if defined(PIOS_INCLUDE_FREERTOS) */ static void PIOS_OVERO_RegisterTxCallback(uint32_t overo_id, pios_com_callback tx_out_cb, uint32_t context) {}; static void PIOS_OVERO_RegisterRxCallback(uint32_t overo_id, pios_com_callback rx_in_cb, uint32_t context) {}; @@ -375,6 +386,6 @@ static void PIOS_OVERO_RxStart(uint32_t overo_id, uint16_t rx_bytes_avail) {}; #endif /* PIOS_INCLUDE_OVERO */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_ppm.c b/flight/pios/stm32f4xx/pios_ppm.c index 9d976320b..91c5d2ea8 100644 --- a/flight/pios/stm32f4xx/pios_ppm.c +++ b/flight/pios/stm32f4xx/pios_ppm.c @@ -38,157 +38,160 @@ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_ppm_rcvr_driver = { - .read = PIOS_PPM_Get, + .read = PIOS_PPM_Get, }; -#define PIOS_PPM_IN_MIN_NUM_CHANNELS 4 -#define PIOS_PPM_IN_MAX_NUM_CHANNELS PIOS_PPM_NUM_INPUTS -#define PIOS_PPM_STABLE_CHANNEL_COUNT 25 // frames -#define PIOS_PPM_IN_MIN_SYNC_PULSE_US 3800 // microseconds -#define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds -#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds +#define PIOS_PPM_IN_MIN_NUM_CHANNELS 4 +#define PIOS_PPM_IN_MAX_NUM_CHANNELS PIOS_PPM_NUM_INPUTS +#define PIOS_PPM_STABLE_CHANNEL_COUNT 25 // frames +#define PIOS_PPM_IN_MIN_SYNC_PULSE_US 3800 // microseconds +#define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds +#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds static void PIOS_PPM_Supervisor(uint32_t ppm_id); enum pios_ppm_dev_magic { - PIOS_PPM_DEV_MAGIC = 0xee014d8b, + PIOS_PPM_DEV_MAGIC = 0xee014d8b, }; struct pios_ppm_dev { - enum pios_ppm_dev_magic magic; - const struct pios_ppm_cfg * cfg; + enum pios_ppm_dev_magic magic; + const struct pios_ppm_cfg *cfg; - uint8_t PulseIndex; - uint32_t PreviousTime; - uint32_t CurrentTime; - uint32_t DeltaTime; - uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS]; - uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS]; - uint32_t LargeCounter; - int8_t NumChannels; - int8_t NumChannelsPrevFrame; - uint8_t NumChannelCounter; + uint8_t PulseIndex; + uint32_t PreviousTime; + uint32_t CurrentTime; + uint32_t DeltaTime; + uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS]; + uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS]; + uint32_t LargeCounter; + int8_t NumChannels; + int8_t NumChannelsPrevFrame; + uint8_t NumChannelCounter; - uint8_t supv_timer; - bool Tracking; - bool Fresh; + uint8_t supv_timer; + bool Tracking; + bool Fresh; }; -static bool PIOS_PPM_validate(struct pios_ppm_dev * ppm_dev) +static bool PIOS_PPM_validate(struct pios_ppm_dev *ppm_dev) { - return (ppm_dev->magic == PIOS_PPM_DEV_MAGIC); + return ppm_dev->magic == PIOS_PPM_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_ppm_dev * PIOS_PPM_alloc(void) +static struct pios_ppm_dev *PIOS_PPM_alloc(void) { - struct pios_ppm_dev * ppm_dev; + struct pios_ppm_dev *ppm_dev; - ppm_dev = (struct pios_ppm_dev *)pvPortMalloc(sizeof(*ppm_dev)); - if (!ppm_dev) return(NULL); + ppm_dev = (struct pios_ppm_dev *)pvPortMalloc(sizeof(*ppm_dev)); + if (!ppm_dev) { + return NULL; + } - ppm_dev->magic = PIOS_PPM_DEV_MAGIC; - return(ppm_dev); + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; + return ppm_dev; } #else static struct pios_ppm_dev pios_ppm_devs[PIOS_PPM_MAX_DEVS]; static uint8_t pios_ppm_num_devs; -static struct pios_ppm_dev * PIOS_PPM_alloc(void) +static struct pios_ppm_dev *PIOS_PPM_alloc(void) { - struct pios_ppm_dev * ppm_dev; + struct pios_ppm_dev *ppm_dev; - if (pios_ppm_num_devs >= PIOS_PPM_MAX_DEVS) { - return (NULL); - } + if (pios_ppm_num_devs >= PIOS_PPM_MAX_DEVS) { + return NULL; + } - ppm_dev = &pios_ppm_devs[pios_ppm_num_devs++]; - ppm_dev->magic = PIOS_PPM_DEV_MAGIC; + ppm_dev = &pios_ppm_devs[pios_ppm_num_devs++]; + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; - return (ppm_dev); + return ppm_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void PIOS_PPM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); -static void PIOS_PPM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PPM_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PPM_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); static const struct pios_tim_callbacks tim_callbacks = { - .overflow = PIOS_PPM_tim_overflow_cb, - .edge = PIOS_PPM_tim_edge_cb, + .overflow = PIOS_PPM_tim_overflow_cb, + .edge = PIOS_PPM_tim_edge_cb, }; -extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg) +extern int32_t PIOS_PPM_Init(uint32_t *ppm_id, const struct pios_ppm_cfg *cfg) { - PIOS_DEBUG_Assert(ppm_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(ppm_id); + PIOS_DEBUG_Assert(cfg); - struct pios_ppm_dev * ppm_dev; + struct pios_ppm_dev *ppm_dev; - ppm_dev = (struct pios_ppm_dev *) PIOS_PPM_alloc(); - if (!ppm_dev) goto out_fail; + ppm_dev = (struct pios_ppm_dev *)PIOS_PPM_alloc(); + if (!ppm_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - ppm_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + ppm_dev->cfg = cfg; - /* Set up the state variables */ - ppm_dev->PulseIndex = 0; - ppm_dev->PreviousTime = 0; - ppm_dev->CurrentTime = 0; - ppm_dev->DeltaTime = 0; - ppm_dev->LargeCounter = 0; - ppm_dev->NumChannels = -1; - ppm_dev->NumChannelsPrevFrame = -1; - ppm_dev->NumChannelCounter = 0; - ppm_dev->Tracking = false; - ppm_dev->Fresh = false; + /* Set up the state variables */ + ppm_dev->PulseIndex = 0; + ppm_dev->PreviousTime = 0; + ppm_dev->CurrentTime = 0; + ppm_dev->DeltaTime = 0; + ppm_dev->LargeCounter = 0; + ppm_dev->NumChannels = -1; + ppm_dev->NumChannelsPrevFrame = -1; + ppm_dev->NumChannelCounter = 0; + ppm_dev->Tracking = false; + ppm_dev->Fresh = false; - for (uint8_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { - /* Flush counter variables */ - ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + for (uint8_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + /* Flush counter variables */ + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } - } + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)ppm_dev)) { + return -1; + } - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)ppm_dev)) { - return -1; - } + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; - TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + /* Configure the channels to be in capture/compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; - /* Configure the channels to be in capture/compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel * chan = &cfg->channels[i]; + /* Configure timer for input capture */ + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); - /* Configure timer for input capture */ - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); + /* Enable the Capture Compare Interrupt Request */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE); + break; + } + } - /* Enable the Capture Compare Interrupt Request */ - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE); - break; - case TIM_Channel_2: - TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE); - break; - case TIM_Channel_3: - TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE); - break; - case TIM_Channel_4: - TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE); - break; - } - } + if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) { + PIOS_DEBUG_Assert(0); + } - if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) { - PIOS_DEBUG_Assert(0); - } + *ppm_id = (uint32_t)ppm_dev; - *ppm_id = (uint32_t)ppm_dev; - - return(0); + return 0; out_fail: - return(-1); + return -1; } /** @@ -200,152 +203,149 @@ out_fail: */ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)rcvr_id; + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)rcvr_id; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return PIOS_RCVR_INVALID; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return PIOS_RCVR_INVALID; + } - if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { - /* Channel out of range */ - return PIOS_RCVR_INVALID; - } - return ppm_dev->CaptureValue[channel]; + if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* Channel out of range */ + return PIOS_RCVR_INVALID; + } + return ppm_dev->CaptureValue[channel]; } -static void PIOS_PPM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, - __attribute__((unused)) uint8_t channel, uint16_t count) +static void PIOS_PPM_tim_overflow_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, + __attribute__((unused)) uint8_t channel, uint16_t count) { - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context; + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)context; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } - ppm_dev->LargeCounter += count; - - return; + ppm_dev->LargeCounter += count; } -static void PIOS_PPM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) +static void PIOS_PPM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) { - /* Recover our device context */ - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context; + /* Recover our device context */ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)context; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } - if (chan_idx >= ppm_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (chan_idx >= ppm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - /* Shift the last measurement out */ - ppm_dev->PreviousTime = ppm_dev->CurrentTime; + /* Shift the last measurement out */ + ppm_dev->PreviousTime = ppm_dev->CurrentTime; - /* Grab the new count */ - ppm_dev->CurrentTime = count; + /* Grab the new count */ + ppm_dev->CurrentTime = count; - /* Convert to 32-bit timer result */ - ppm_dev->CurrentTime += ppm_dev->LargeCounter; + /* Convert to 32-bit timer result */ + ppm_dev->CurrentTime += ppm_dev->LargeCounter; - /* Capture computation */ - ppm_dev->DeltaTime = ppm_dev->CurrentTime - ppm_dev->PreviousTime; + /* Capture computation */ + ppm_dev->DeltaTime = ppm_dev->CurrentTime - ppm_dev->PreviousTime; - ppm_dev->PreviousTime = ppm_dev->CurrentTime; + ppm_dev->PreviousTime = ppm_dev->CurrentTime; - /* Sync pulse detection */ - if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) { - if (ppm_dev->PulseIndex == ppm_dev->NumChannelsPrevFrame - && ppm_dev->PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS - && ppm_dev->PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS) - { - /* If we see n simultaneous frames of the same - number of channels we save it as our frame size */ - if (ppm_dev->NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT) - ppm_dev->NumChannelCounter++; - else - ppm_dev->NumChannels = ppm_dev->PulseIndex; - } else { - ppm_dev->NumChannelCounter = 0; - } + /* Sync pulse detection */ + if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) { + if (ppm_dev->PulseIndex == ppm_dev->NumChannelsPrevFrame + && ppm_dev->PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS + && ppm_dev->PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* If we see n simultaneous frames of the same + number of channels we save it as our frame size */ + if (ppm_dev->NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT) { + ppm_dev->NumChannelCounter++; + } else { + ppm_dev->NumChannels = ppm_dev->PulseIndex; + } + } else { + ppm_dev->NumChannelCounter = 0; + } - /* Check if the last frame was well formed */ - if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) { - /* The last frame was well formed */ - for (int32_t i = 0; i < ppm_dev->NumChannels; i++) { - ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i]; - } - for (uint32_t i = ppm_dev->NumChannels; - i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { - ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - } - } + /* Check if the last frame was well formed */ + if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) { + /* The last frame was well formed */ + for (int32_t i = 0; i < ppm_dev->NumChannels; i++) { + ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i]; + } + for (uint32_t i = ppm_dev->NumChannels; + i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } + } - ppm_dev->Fresh = true; - ppm_dev->Tracking = true; - ppm_dev->NumChannelsPrevFrame = ppm_dev->PulseIndex; - ppm_dev->PulseIndex = 0; + ppm_dev->Fresh = true; + ppm_dev->Tracking = true; + ppm_dev->NumChannelsPrevFrame = ppm_dev->PulseIndex; + ppm_dev->PulseIndex = 0; - /* We rely on the supervisor to set CaptureValue to invalid - if no valid frame is found otherwise we ride over it */ - - } else if (ppm_dev->Tracking) { - /* Valid pulse duration 0.75 to 2.5 ms*/ - if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US - && ppm_dev->DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US - && ppm_dev->PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) { - - ppm_dev->CaptureValueNewFrame[ppm_dev->PulseIndex] = ppm_dev->DeltaTime; - ppm_dev->PulseIndex++; - } else { - /* Not a valid pulse duration */ - ppm_dev->Tracking = false; - for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { - ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; - } - } - } + /* We rely on the supervisor to set CaptureValue to invalid + if no valid frame is found otherwise we ride over it */ + } else if (ppm_dev->Tracking) { + /* Valid pulse duration 0.75 to 2.5 ms*/ + if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US + && ppm_dev->DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US + && ppm_dev->PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) { + ppm_dev->CaptureValueNewFrame[ppm_dev->PulseIndex] = ppm_dev->DeltaTime; + ppm_dev->PulseIndex++; + } else { + /* Not a valid pulse duration */ + ppm_dev->Tracking = false; + for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } + } + } } -static void PIOS_PPM_Supervisor(uint32_t ppm_id) { - /* Recover our device context */ - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)ppm_id; +static void PIOS_PPM_Supervisor(uint32_t ppm_id) +{ + /* Recover our device context */ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)ppm_id; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } - /* - * RTC runs at 625Hz so divide down the base rate so - * that this loop runs at 25Hz. - */ - if(++(ppm_dev->supv_timer) < 25) { - return; - } - ppm_dev->supv_timer = 0; + /* + * RTC runs at 625Hz so divide down the base rate so + * that this loop runs at 25Hz. + */ + if (++(ppm_dev->supv_timer) < 25) { + return; + } + ppm_dev->supv_timer = 0; - if (!ppm_dev->Fresh) { - ppm_dev->Tracking = false; + if (!ppm_dev->Fresh) { + ppm_dev->Tracking = false; - for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { - ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; - } - } + for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } + } - ppm_dev->Fresh = false; + ppm_dev->Fresh = false; } #endif /* PIOS_INCLUDE_PPM */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_pwm.c b/flight/pios/stm32f4xx/pios_pwm.c index 9c55cc624..5583196da 100644 --- a/flight/pios/stm32f4xx/pios_pwm.c +++ b/flight/pios/stm32f4xx/pios_pwm.c @@ -38,7 +38,7 @@ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_pwm_rcvr_driver = { - .read = PIOS_PWM_Get, + .read = PIOS_PWM_Get, }; /* Local Variables */ @@ -46,127 +46,130 @@ const struct pios_rcvr_driver pios_pwm_rcvr_driver = { static const uint32_t PWM_SUPERVISOR_TIMEOUT = 100000; enum pios_pwm_dev_magic { - PIOS_PWM_DEV_MAGIC = 0xab30293c, + PIOS_PWM_DEV_MAGIC = 0xab30293c, }; struct pios_pwm_dev { - enum pios_pwm_dev_magic magic; - const struct pios_pwm_cfg * cfg; + enum pios_pwm_dev_magic magic; + const struct pios_pwm_cfg *cfg; - uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; - uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; - uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; - uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; - uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; - uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; + uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; + uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; + uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; + uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; }; -static bool PIOS_PWM_validate(struct pios_pwm_dev * pwm_dev) +static bool PIOS_PWM_validate(struct pios_pwm_dev *pwm_dev) { - return (pwm_dev->magic == PIOS_PWM_DEV_MAGIC); + return pwm_dev->magic == PIOS_PWM_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_pwm_dev * PIOS_PWM_alloc(void) +static struct pios_pwm_dev *PIOS_PWM_alloc(void) { - struct pios_pwm_dev * pwm_dev; + struct pios_pwm_dev *pwm_dev; - pwm_dev = (struct pios_pwm_dev *)pvPortMalloc(sizeof(*pwm_dev)); - if (!pwm_dev) return(NULL); + pwm_dev = (struct pios_pwm_dev *)pvPortMalloc(sizeof(*pwm_dev)); + if (!pwm_dev) { + return NULL; + } - pwm_dev->magic = PIOS_PWM_DEV_MAGIC; - return(pwm_dev); + pwm_dev->magic = PIOS_PWM_DEV_MAGIC; + return pwm_dev; } #else static struct pios_pwm_dev pios_pwm_devs[PIOS_PWM_MAX_DEVS]; static uint8_t pios_pwm_num_devs; -static struct pios_pwm_dev * PIOS_PWM_alloc(void) +static struct pios_pwm_dev *PIOS_PWM_alloc(void) { - struct pios_pwm_dev * pwm_dev; + struct pios_pwm_dev *pwm_dev; - if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) { - return (NULL); - } + if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) { + return NULL; + } - pwm_dev = &pios_pwm_devs[pios_pwm_num_devs++]; - pwm_dev->magic = PIOS_PWM_DEV_MAGIC; + pwm_dev = &pios_pwm_devs[pios_pwm_num_devs++]; + pwm_dev->magic = PIOS_PWM_DEV_MAGIC; - return (pwm_dev); + return pwm_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void PIOS_PWM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); -static void PIOS_PWM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PWM_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PWM_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); static const struct pios_tim_callbacks tim_callbacks = { - .overflow = PIOS_PWM_tim_overflow_cb, - .edge = PIOS_PWM_tim_edge_cb, + .overflow = PIOS_PWM_tim_overflow_cb, + .edge = PIOS_PWM_tim_edge_cb, }; /** -* Initialises all the pins -*/ -int32_t PIOS_PWM_Init(uint32_t * pwm_id, const struct pios_pwm_cfg * cfg) + * Initialises all the pins + */ +int32_t PIOS_PWM_Init(uint32_t *pwm_id, const struct pios_pwm_cfg *cfg) { - PIOS_DEBUG_Assert(pwm_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(pwm_id); + PIOS_DEBUG_Assert(cfg); - struct pios_pwm_dev * pwm_dev; + struct pios_pwm_dev *pwm_dev; - pwm_dev = (struct pios_pwm_dev *) PIOS_PWM_alloc(); - if (!pwm_dev) goto out_fail; + pwm_dev = (struct pios_pwm_dev *)PIOS_PWM_alloc(); + if (!pwm_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - pwm_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + pwm_dev->cfg = cfg; - for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { - /* Flush counter variables */ - pwm_dev->CaptureState[i] = 0; - pwm_dev->RiseValue[i] = 0; - pwm_dev->FallValue[i] = 0; - pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - } + for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { + /* Flush counter variables */ + pwm_dev->CaptureState[i] = 0; + pwm_dev->RiseValue[i] = 0; + pwm_dev->FallValue[i] = 0; + pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)pwm_dev)) { - return -1; - } + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)pwm_dev)) { + return -1; + } - /* Configure the channels to be in capture/compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel * chan = &cfg->channels[i]; + /* Configure the channels to be in capture/compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; - /* Configure timer for input capture */ - TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - - /* Enable the Capture Compare Interrupt Request */ - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); - break; - case TIM_Channel_2: - TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); - break; - case TIM_Channel_3: - TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); - break; - case TIM_Channel_4: - TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); - break; - } + /* Configure timer for input capture */ + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); - // Need the update event for that timer to detect timeouts - TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); + /* Enable the Capture Compare Interrupt Request */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); + break; + } - } + // Need the update event for that timer to detect timeouts + TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); + } - *pwm_id = (uint32_t) pwm_dev; + *pwm_id = (uint32_t)pwm_dev; - return (0); + return 0; out_fail: - return (-1); + return -1; } /** @@ -178,106 +181,103 @@ out_fail: */ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)rcvr_id; + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)rcvr_id; - if (!PIOS_PWM_validate(pwm_dev)) { - /* Invalid device specified */ - return PIOS_RCVR_INVALID; - } + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return PIOS_RCVR_INVALID; + } - if (channel >= PIOS_PWM_NUM_INPUTS) { - /* Channel out of range */ - return PIOS_RCVR_INVALID; - } - return pwm_dev->CaptureValue[channel]; + if (channel >= PIOS_PWM_NUM_INPUTS) { + /* Channel out of range */ + return PIOS_RCVR_INVALID; + } + return pwm_dev->CaptureValue[channel]; } -static void PIOS_PWM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) +static void PIOS_PWM_tim_overflow_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) { - struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context; + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)context; - if (!PIOS_PWM_validate(pwm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return; + } - if (channel >= pwm_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (channel >= pwm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - pwm_dev->us_since_update[channel] += count; - if(pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { - pwm_dev->CaptureState[channel] = 0; - pwm_dev->RiseValue[channel] = 0; - pwm_dev->FallValue[channel] = 0; - pwm_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; - pwm_dev->us_since_update[channel] = 0; - } - - return; + pwm_dev->us_since_update[channel] += count; + if (pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { + pwm_dev->CaptureState[channel] = 0; + pwm_dev->RiseValue[channel] = 0; + pwm_dev->FallValue[channel] = 0; + pwm_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; + pwm_dev->us_since_update[channel] = 0; + } } -static void PIOS_PWM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) +static void PIOS_PWM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) { - /* Recover our device context */ - struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context; + /* Recover our device context */ + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)context; - if (!PIOS_PWM_validate(pwm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return; + } - if (chan_idx >= pwm_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (chan_idx >= pwm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - const struct pios_tim_channel * chan = &pwm_dev->cfg->channels[chan_idx]; + const struct pios_tim_channel *chan = &pwm_dev->cfg->channels[chan_idx]; - if (pwm_dev->CaptureState[chan_idx] == 0) { - pwm_dev->RiseValue[chan_idx] = count; - pwm_dev->us_since_update[chan_idx] = 0; - } else { - pwm_dev->FallValue[chan_idx] = count; - } - - // flip state machine and capture value here - /* Simple rise or fall state machine */ - TIM_ICInitTypeDef TIM_ICInitStructure = pwm_dev->cfg->tim_ic_init; - if (pwm_dev->CaptureState[chan_idx] == 0) { - /* Switch states */ - pwm_dev->CaptureState[chan_idx] = 1; + if (pwm_dev->CaptureState[chan_idx] == 0) { + pwm_dev->RiseValue[chan_idx] = count; + pwm_dev->us_since_update[chan_idx] = 0; + } else { + pwm_dev->FallValue[chan_idx] = count; + } - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - } else { - /* Capture computation */ - if (pwm_dev->FallValue[chan_idx] > pwm_dev->RiseValue[chan_idx]) { - pwm_dev->CaptureValue[chan_idx] = (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]); - } else { - pwm_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - pwm_dev->RiseValue[chan_idx]) + pwm_dev->FallValue[chan_idx]); - } + // flip state machine and capture value here + /* Simple rise or fall state machine */ + TIM_ICInitTypeDef TIM_ICInitStructure = pwm_dev->cfg->tim_ic_init; + if (pwm_dev->CaptureState[chan_idx] == 0) { + /* Switch states */ + pwm_dev->CaptureState[chan_idx] = 1; - /* Switch states */ - pwm_dev->CaptureState[chan_idx] = 0; + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } else { + /* Capture computation */ + if (pwm_dev->FallValue[chan_idx] > pwm_dev->RiseValue[chan_idx]) { + pwm_dev->CaptureValue[chan_idx] = (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]); + } else { + pwm_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - pwm_dev->RiseValue[chan_idx]) + pwm_dev->FallValue[chan_idx]); + } - /* Increase supervisor counter */ - pwm_dev->CapCounter[chan_idx]++; + /* Switch states */ + pwm_dev->CaptureState[chan_idx] = 0; - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - } - + /* Increase supervisor counter */ + pwm_dev->CapCounter[chan_idx]++; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } } #endif /* PIOS_INCLUDE_PWM */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_rtc.c b/flight/pios/stm32f4xx/pios_rtc.c index 12cf4b4ae..ac0f28f0a 100644 --- a/flight/pios/stm32f4xx/pios_rtc.c +++ b/flight/pios/stm32f4xx/pios_rtc.c @@ -39,48 +39,48 @@ #endif struct rtc_callback_entry { - void (*fn)(uint32_t); - uint32_t data; + void (*fn)(uint32_t); + uint32_t data; }; #define PIOS_RTC_MAX_CALLBACKS 3 struct rtc_callback_entry rtc_callback_list[PIOS_RTC_MAX_CALLBACKS]; static uint8_t rtc_callback_next = 0; -void PIOS_RTC_Init(const struct pios_rtc_cfg * cfg) +void PIOS_RTC_Init(const struct pios_rtc_cfg *cfg) { - RCC_BackupResetCmd(ENABLE); - RCC_BackupResetCmd(DISABLE); - RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); - PWR_BackupAccessCmd(ENABLE); - // Divide external 8 MHz clock to 1 MHz - RCC_RTCCLKConfig(cfg->clksrc); - RCC_RTCCLKCmd(ENABLE); + RCC_BackupResetCmd(ENABLE); + RCC_BackupResetCmd(DISABLE); + RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); + PWR_BackupAccessCmd(ENABLE); + // Divide external 8 MHz clock to 1 MHz + RCC_RTCCLKConfig(cfg->clksrc); + RCC_RTCCLKCmd(ENABLE); - RTC_WakeUpCmd(DISABLE); - // Divide 1 Mhz clock by 16 -> 62.5 khz - RTC_WakeUpClockConfig(RTC_WakeUpClock_RTCCLK_Div16); - // Divide 62.5 khz by 200 to get 625 Hz - RTC_SetWakeUpCounter(cfg->prescaler); //cfg->prescaler); - RTC_WakeUpCmd(ENABLE); - - /* Configure and enable the RTC Second interrupt */ - EXTI_InitTypeDef ExtiInit = { - .EXTI_Line = EXTI_Line22, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }; - EXTI_Init(&ExtiInit); - NVIC_Init(&cfg->irq.init); - RTC_ITConfig(RTC_IT_WUT, ENABLE); - - RTC_ClearFlag(RTC_FLAG_WUTF); + RTC_WakeUpCmd(DISABLE); + // Divide 1 Mhz clock by 16 -> 62.5 khz + RTC_WakeUpClockConfig(RTC_WakeUpClock_RTCCLK_Div16); + // Divide 62.5 khz by 200 to get 625 Hz + RTC_SetWakeUpCounter(cfg->prescaler); // cfg->prescaler); + RTC_WakeUpCmd(ENABLE); + + /* Configure and enable the RTC Second interrupt */ + EXTI_InitTypeDef ExtiInit = { + .EXTI_Line = EXTI_Line22, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }; + EXTI_Init(&ExtiInit); + NVIC_Init(&cfg->irq.init); + RTC_ITConfig(RTC_IT_WUT, ENABLE); + + RTC_ClearFlag(RTC_FLAG_WUTF); } uint32_t PIOS_RTC_Counter() { - return RTC_GetWakeUpCounter(); + return RTC_GetWakeUpCounter(); } /* FIXME: This shouldn't use hard-coded clock rates, dividers or prescalers. @@ -88,52 +88,53 @@ uint32_t PIOS_RTC_Counter() */ float PIOS_RTC_Rate() { - return (float) (8e6f / 128.0f) / (1 + PIOS_RTC_PRESCALER); + return (float)(8e6f / 128.0f) / (1 + PIOS_RTC_PRESCALER); } -float PIOS_RTC_MsPerTick() +float PIOS_RTC_MsPerTick() { - return 1000.0f / PIOS_RTC_Rate(); + return 1000.0f / PIOS_RTC_Rate(); } /* TODO: This needs a mutex around rtc_callbacks[] */ bool PIOS_RTC_RegisterTickCallback(void (*fn)(uint32_t id), uint32_t data) { - struct rtc_callback_entry * cb; - if (rtc_callback_next >= PIOS_RTC_MAX_CALLBACKS) { - return false; - } + struct rtc_callback_entry *cb; - cb = &rtc_callback_list[rtc_callback_next++]; + if (rtc_callback_next >= PIOS_RTC_MAX_CALLBACKS) { + return false; + } - cb->fn = fn; - cb->data = data; - return true; + cb = &rtc_callback_list[rtc_callback_next++]; + + cb->fn = fn; + cb->data = data; + return true; } -void PIOS_RTC_irq_handler (void) +void PIOS_RTC_irq_handler(void) { - if (RTC_GetITStatus(RTC_IT_WUT)) - { - /* Call all registered callbacks */ - for (uint8_t i = 0; i < rtc_callback_next; i++) { - struct rtc_callback_entry * cb = &rtc_callback_list[i]; - if (cb->fn) { - (cb->fn)(cb->data); - } - } + if (RTC_GetITStatus(RTC_IT_WUT)) { + /* Call all registered callbacks */ + for (uint8_t i = 0; i < rtc_callback_next; i++) { + struct rtc_callback_entry *cb = &rtc_callback_list[i]; + if (cb->fn) { + (cb->fn)(cb->data); + } + } - /* Clear the RTC Second interrupt */ - RTC_ClearITPendingBit(RTC_IT_WUT); - } + /* Clear the RTC Second interrupt */ + RTC_ClearITPendingBit(RTC_IT_WUT); + } - if (EXTI_GetITStatus(EXTI_Line22) != RESET) - EXTI_ClearITPendingBit(EXTI_Line22); + if (EXTI_GetITStatus(EXTI_Line22) != RESET) { + EXTI_ClearITPendingBit(EXTI_Line22); + } } #endif /* PIOS_INCLUDE_RTC */ -/** +/** * @} * @} */ diff --git a/flight/pios/stm32f4xx/pios_servo.c b/flight/pios/stm32f4xx/pios_servo.c index 5c5d6ef52..32803a57b 100644 --- a/flight/pios/stm32f4xx/pios_servo.c +++ b/flight/pios/stm32f4xx/pios_servo.c @@ -37,123 +37,124 @@ /* Private Function Prototypes */ -static const struct pios_servo_cfg * servo_cfg; +static const struct pios_servo_cfg *servo_cfg; /** -* Initialise Servos -*/ -int32_t PIOS_Servo_Init(const struct pios_servo_cfg * cfg) + * Initialise Servos + */ +int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg) { - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) { - return -1; - } + uint32_t tim_id; - /* Store away the requested configuration */ - servo_cfg = cfg; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) { + return -1; + } - /* Configure the channels to be in output compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel * chan = &cfg->channels[i]; + /* Store away the requested configuration */ + servo_cfg = cfg; - /* Set up for output compare function */ - switch(chan->timer_chan) { - case TIM_Channel_1: - TIM_OC1Init(chan->timer, &cfg->tim_oc_init); - TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_2: - TIM_OC2Init(chan->timer, &cfg->tim_oc_init); - TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_3: - TIM_OC3Init(chan->timer, &cfg->tim_oc_init); - TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_4: - TIM_OC4Init(chan->timer, &cfg->tim_oc_init); - TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - } + /* Configure the channels to be in output compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; - TIM_ARRPreloadConfig(chan->timer, ENABLE); - TIM_CtrlPWMOutputs(chan->timer, ENABLE); - TIM_Cmd(chan->timer, ENABLE); - } + /* Set up for output compare function */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_OC1Init(chan->timer, &cfg->tim_oc_init); + TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(chan->timer, &cfg->tim_oc_init); + TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(chan->timer, &cfg->tim_oc_init); + TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(chan->timer, &cfg->tim_oc_init); + TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + } - return 0; + TIM_ARRPreloadConfig(chan->timer, ENABLE); + TIM_CtrlPWMOutputs(chan->timer, ENABLE); + TIM_Cmd(chan->timer, ENABLE); + } + + return 0; } /** -* Set the servo update rate (Max 500Hz) -* \param[in] array of rates in Hz -* \param[in] maximum number of banks -*/ -void PIOS_Servo_SetHz(const uint16_t * speeds, uint8_t banks) + * Set the servo update rate (Max 500Hz) + * \param[in] array of rates in Hz + * \param[in] maximum number of banks + */ +void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks) { - if (!servo_cfg) { - return; - } + if (!servo_cfg) { + return; + } - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init; - TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - // + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init; + TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + // - uint8_t set = 0; + uint8_t set = 0; - for(uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) { - bool new = true; - const struct pios_tim_channel * chan = &servo_cfg->channels[i]; + for (uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) { + bool new = true; + const struct pios_tim_channel *chan = &servo_cfg->channels[i]; - /* See if any previous channels use that same timer */ - for(uint8_t j = 0; (j < i) && new; j++) - new &= chan->timer != servo_cfg->channels[j].timer; + /* See if any previous channels use that same timer */ + for (uint8_t j = 0; (j < i) && new; j++) { + new &= chan->timer != servo_cfg->channels[j].timer; + } - if(new) { - // Choose the correct prescaler value for the APB the timer is attached - if (chan->timer==TIM1 || chan->timer==TIM8 || chan->timer==TIM9 || chan->timer==TIM10 || chan->timer==TIM11 ){ - TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1; - } - else { - TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1; - } + if (new) { + // Choose the correct prescaler value for the APB the timer is attached + if (chan->timer == TIM1 || chan->timer == TIM8 || chan->timer == TIM9 || chan->timer == TIM10 || chan->timer == TIM11) { + TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1; + } else { + TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1; + } - TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1); - TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure); - set++; - } - } + TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1); + TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure); + set++; + } + } } /** -* Set servo position -* \param[in] Servo Servo number (0-7) -* \param[in] Position Servo position in microseconds -*/ + * Set servo position + * \param[in] Servo Servo number (0-7) + * \param[in] Position Servo position in microseconds + */ void PIOS_Servo_Set(uint8_t servo, uint16_t position) { - /* Make sure servo exists */ - if (!servo_cfg || servo >= servo_cfg->num_channels) { - return; - } + /* Make sure servo exists */ + if (!servo_cfg || servo >= servo_cfg->num_channels) { + return; + } - /* Update the position */ - const struct pios_tim_channel * chan = &servo_cfg->channels[servo]; - switch(chan->timer_chan) { - case TIM_Channel_1: - TIM_SetCompare1(chan->timer, position); - break; - case TIM_Channel_2: - TIM_SetCompare2(chan->timer, position); - break; - case TIM_Channel_3: - TIM_SetCompare3(chan->timer, position); - break; - case TIM_Channel_4: - TIM_SetCompare4(chan->timer, position); - break; - } + /* Update the position */ + const struct pios_tim_channel *chan = &servo_cfg->channels[servo]; + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_SetCompare1(chan->timer, position); + break; + case TIM_Channel_2: + TIM_SetCompare2(chan->timer, position); + break; + case TIM_Channel_3: + TIM_SetCompare3(chan->timer, position); + break; + case TIM_Channel_4: + TIM_SetCompare4(chan->timer, position); + break; + } } #endif /* PIOS_INCLUDE_SERVO */ diff --git a/flight/pios/stm32f4xx/pios_spi.c b/flight/pios/stm32f4xx/pios_spi.c index bc8304596..5442e5e26 100644 --- a/flight/pios/stm32f4xx/pios_spi.c +++ b/flight/pios/stm32f4xx/pios_spi.c @@ -42,157 +42,159 @@ #include -#define SPI_MAX_BLOCK_PIO 128 +#define SPI_MAX_BLOCK_PIO 128 -static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev * com_dev) +static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev *com_dev) { - /* Should check device magic here */ - return(true); + /* Should check device magic here */ + return true; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_spi_dev * PIOS_SPI_alloc(void) +static struct pios_spi_dev *PIOS_SPI_alloc(void) { - return (pvPortMalloc(sizeof(struct pios_spi_dev))); + return pvPortMalloc(sizeof(struct pios_spi_dev)); } #else static struct pios_spi_dev pios_spi_devs[PIOS_SPI_MAX_DEVS]; static uint8_t pios_spi_num_devs; -static struct pios_spi_dev * PIOS_SPI_alloc(void) +static struct pios_spi_dev *PIOS_SPI_alloc(void) { - if (pios_spi_num_devs >= PIOS_SPI_MAX_DEVS) { - return (NULL); - } + if (pios_spi_num_devs >= PIOS_SPI_MAX_DEVS) { + return NULL; + } - return (&pios_spi_devs[pios_spi_num_devs++]); + return &pios_spi_devs[pios_spi_num_devs++]; } #endif /** -* Initialises SPI pins -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ -int32_t PIOS_SPI_Init(uint32_t * spi_id, const struct pios_spi_cfg * cfg) + * Initialises SPI pins + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ +int32_t PIOS_SPI_Init(uint32_t *spi_id, const struct pios_spi_cfg *cfg) { - uint32_t init_ssel = 0; + uint32_t init_ssel = 0; - PIOS_Assert(spi_id); - PIOS_Assert(cfg); + PIOS_Assert(spi_id); + PIOS_Assert(cfg); - struct pios_spi_dev * spi_dev; + struct pios_spi_dev *spi_dev; - spi_dev = (struct pios_spi_dev *) PIOS_SPI_alloc(); - if (!spi_dev) goto out_fail; + spi_dev = (struct pios_spi_dev *)PIOS_SPI_alloc(); + if (!spi_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - spi_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + spi_dev->cfg = cfg; #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(spi_dev->busy); - xSemaphoreGive(spi_dev->busy); + vSemaphoreCreateBinary(spi_dev->busy); + xSemaphoreGive(spi_dev->busy); #endif - /* Disable callback function */ - spi_dev->callback = NULL; + /* Disable callback function */ + spi_dev->callback = NULL; - /* Set rx/tx dummy bytes to a known value */ - spi_dev->rx_dummy_byte = 0xFF; - spi_dev->tx_dummy_byte = 0xFF; + /* Set rx/tx dummy bytes to a known value */ + spi_dev->rx_dummy_byte = 0xFF; + spi_dev->tx_dummy_byte = 0xFF; - switch (spi_dev->cfg->init.SPI_NSS) { - case SPI_NSS_Soft: - if (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) { - /* We're a master in soft NSS mode, make sure we see NSS high at all times. */ - SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Set); - /* Init as many slave selects as the config advertises. */ - init_ssel = spi_dev->cfg->slave_count; - } else { - /* We're a slave in soft NSS mode, make sure we see NSS low at all times. */ - SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Reset); - } - break; - - case SPI_NSS_Hard: - /* only legal for single-slave config */ - PIOS_Assert(spi_dev->cfg->slave_count == 1); - init_ssel = 1; - SPI_SSOutputCmd(spi_dev->cfg->regs, (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) ? ENABLE : DISABLE); - /* FIXME: Should this also call SPI_SSOutputCmd()? */ - break; - - default: - PIOS_Assert(0); - } + switch (spi_dev->cfg->init.SPI_NSS) { + case SPI_NSS_Soft: + if (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) { + /* We're a master in soft NSS mode, make sure we see NSS high at all times. */ + SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Set); + /* Init as many slave selects as the config advertises. */ + init_ssel = spi_dev->cfg->slave_count; + } else { + /* We're a slave in soft NSS mode, make sure we see NSS low at all times. */ + SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Reset); + } + break; - /* Initialize the GPIO pins */ - /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ - if (spi_dev->cfg->remap) { - GPIO_PinAFConfig(spi_dev->cfg->sclk.gpio, - __builtin_ctz(spi_dev->cfg->sclk.init.GPIO_Pin), - spi_dev->cfg->remap); - GPIO_PinAFConfig(spi_dev->cfg->mosi.gpio, - __builtin_ctz(spi_dev->cfg->mosi.init.GPIO_Pin), - spi_dev->cfg->remap); - GPIO_PinAFConfig(spi_dev->cfg->miso.gpio, - __builtin_ctz(spi_dev->cfg->miso.init.GPIO_Pin), - spi_dev->cfg->remap); - for (uint32_t i = 0; i < init_ssel; i++) { - GPIO_PinAFConfig(spi_dev->cfg->ssel[i].gpio, - __builtin_ctz(spi_dev->cfg->ssel[i].init.GPIO_Pin), - spi_dev->cfg->remap); - } - } - GPIO_Init(spi_dev->cfg->sclk.gpio, (GPIO_InitTypeDef*)&(spi_dev->cfg->sclk.init)); - GPIO_Init(spi_dev->cfg->mosi.gpio, (GPIO_InitTypeDef*)&(spi_dev->cfg->mosi.init)); - GPIO_Init(spi_dev->cfg->miso.gpio, (GPIO_InitTypeDef*)&(spi_dev->cfg->miso.init)); - - if(spi_dev->cfg->init.SPI_NSS != SPI_NSS_Hard) { - for (uint32_t i = 0; i < init_ssel; i++) { - /* Since we're driving the SSEL pin in software, ensure that the slave is deselected */ - /* XXX multi-slave support - maybe have another SPI_NSS_ mode? */ - GPIO_SetBits(spi_dev->cfg->ssel[i].gpio, spi_dev->cfg->ssel[i].init.GPIO_Pin); - GPIO_Init(spi_dev->cfg->ssel[i].gpio, (GPIO_InitTypeDef*)&(spi_dev->cfg->ssel[i].init)); - } - } + case SPI_NSS_Hard: + /* only legal for single-slave config */ + PIOS_Assert(spi_dev->cfg->slave_count == 1); + init_ssel = 1; + SPI_SSOutputCmd(spi_dev->cfg->regs, (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) ? ENABLE : DISABLE); + /* FIXME: Should this also call SPI_SSOutputCmd()? */ + break; - /* Configure DMA for SPI Rx */ - DMA_DeInit(spi_dev->cfg->dma.rx.channel); - DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); - DMA_Init(spi_dev->cfg->dma.rx.channel, (DMA_InitTypeDef*)&(spi_dev->cfg->dma.rx.init)); + default: + PIOS_Assert(0); + } - /* Configure DMA for SPI Tx */ - DMA_DeInit(spi_dev->cfg->dma.tx.channel); - DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); - DMA_Init(spi_dev->cfg->dma.tx.channel, (DMA_InitTypeDef*)&(spi_dev->cfg->dma.tx.init)); + /* Initialize the GPIO pins */ + /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ + if (spi_dev->cfg->remap) { + GPIO_PinAFConfig(spi_dev->cfg->sclk.gpio, + __builtin_ctz(spi_dev->cfg->sclk.init.GPIO_Pin), + spi_dev->cfg->remap); + GPIO_PinAFConfig(spi_dev->cfg->mosi.gpio, + __builtin_ctz(spi_dev->cfg->mosi.init.GPIO_Pin), + spi_dev->cfg->remap); + GPIO_PinAFConfig(spi_dev->cfg->miso.gpio, + __builtin_ctz(spi_dev->cfg->miso.init.GPIO_Pin), + spi_dev->cfg->remap); + for (uint32_t i = 0; i < init_ssel; i++) { + GPIO_PinAFConfig(spi_dev->cfg->ssel[i].gpio, + __builtin_ctz(spi_dev->cfg->ssel[i].init.GPIO_Pin), + spi_dev->cfg->remap); + } + } + GPIO_Init(spi_dev->cfg->sclk.gpio, (GPIO_InitTypeDef *)&(spi_dev->cfg->sclk.init)); + GPIO_Init(spi_dev->cfg->mosi.gpio, (GPIO_InitTypeDef *)&(spi_dev->cfg->mosi.init)); + GPIO_Init(spi_dev->cfg->miso.gpio, (GPIO_InitTypeDef *)&(spi_dev->cfg->miso.init)); - /* Initialize the SPI block */ - SPI_DeInit(spi_dev->cfg->regs); - SPI_Init(spi_dev->cfg->regs, (SPI_InitTypeDef*)&(spi_dev->cfg->init)); + if (spi_dev->cfg->init.SPI_NSS != SPI_NSS_Hard) { + for (uint32_t i = 0; i < init_ssel; i++) { + /* Since we're driving the SSEL pin in software, ensure that the slave is deselected */ + /* XXX multi-slave support - maybe have another SPI_NSS_ mode? */ + GPIO_SetBits(spi_dev->cfg->ssel[i].gpio, spi_dev->cfg->ssel[i].init.GPIO_Pin); + GPIO_Init(spi_dev->cfg->ssel[i].gpio, (GPIO_InitTypeDef *)&(spi_dev->cfg->ssel[i].init)); + } + } - /* Configure CRC calculation */ - if (spi_dev->cfg->use_crc) { - SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); - } else { - SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); - } + /* Configure DMA for SPI Rx */ + DMA_DeInit(spi_dev->cfg->dma.rx.channel); + DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); + DMA_Init(spi_dev->cfg->dma.rx.channel, (DMA_InitTypeDef *)&(spi_dev->cfg->dma.rx.init)); - /* Enable SPI */ - SPI_Cmd(spi_dev->cfg->regs, ENABLE); + /* Configure DMA for SPI Tx */ + DMA_DeInit(spi_dev->cfg->dma.tx.channel); + DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); + DMA_Init(spi_dev->cfg->dma.tx.channel, (DMA_InitTypeDef *)&(spi_dev->cfg->dma.tx.init)); - /* Enable SPI interrupts to DMA */ - SPI_I2S_DMACmd(spi_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); + /* Initialize the SPI block */ + SPI_DeInit(spi_dev->cfg->regs); + SPI_Init(spi_dev->cfg->regs, (SPI_InitTypeDef *)&(spi_dev->cfg->init)); - /* Must store this before enabling interrupt */ - *spi_id = (uint32_t)spi_dev; + /* Configure CRC calculation */ + if (spi_dev->cfg->use_crc) { + SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); + } else { + SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); + } - /* Configure DMA interrupt */ - NVIC_Init((NVIC_InitTypeDef*)&(spi_dev->cfg->dma.irq.init)); + /* Enable SPI */ + SPI_Cmd(spi_dev->cfg->regs, ENABLE); - return(0); + /* Enable SPI interrupts to DMA */ + SPI_I2S_DMACmd(spi_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); + + /* Must store this before enabling interrupt */ + *spi_id = (uint32_t)spi_dev; + + /* Configure DMA interrupt */ + NVIC_Init((NVIC_InitTypeDef *)&(spi_dev->cfg->dma.irq.init)); + + return 0; out_fail: - return(-1); + return -1; } /** @@ -216,29 +218,30 @@ out_fail: */ int32_t PIOS_SPI_SetClockSpeed(uint32_t spi_id, SPIPrescalerTypeDef spi_prescaler) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) - - SPI_InitTypeDef SPI_InitStructure; - - if (spi_prescaler >= 8) { - /* Invalid prescaler selected */ - return -3; - } - - /* Start with a copy of the default configuration for the peripheral */ - SPI_InitStructure = spi_dev->cfg->init; - - /* Adjust the prescaler for the peripheral's clock */ - SPI_InitStructure.SPI_BaudRatePrescaler = ((uint16_t) spi_prescaler & 7) << 3; - - /* Write back the new configuration */ - SPI_Init(spi_dev->cfg->regs, &SPI_InitStructure); - - PIOS_SPI_TransferByte(spi_id, 0xFF); - return 0; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + + PIOS_Assert(valid) + + SPI_InitTypeDef SPI_InitStructure; + + if (spi_prescaler >= 8) { + /* Invalid prescaler selected */ + return -3; + } + + /* Start with a copy of the default configuration for the peripheral */ + SPI_InitStructure = spi_dev->cfg->init; + + /* Adjust the prescaler for the peripheral's clock */ + SPI_InitStructure.SPI_BaudRatePrescaler = ((uint16_t)spi_prescaler & 7) << 3; + + /* Write back the new configuration */ + SPI_Init(spi_dev->cfg->regs, &SPI_InitStructure); + + PIOS_SPI_TransferByte(spi_id, 0xFF); + return 0; } /** @@ -250,27 +253,32 @@ int32_t PIOS_SPI_SetClockSpeed(uint32_t spi_id, SPIPrescalerTypeDef spi_prescale int32_t PIOS_SPI_ClaimBus(uint32_t spi_id) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) - if (xSemaphoreTake(spi_dev->busy, 0xffff) != pdTRUE) - return -1; + if (xSemaphoreTake(spi_dev->busy, 0xffff) != pdTRUE) { + return -1; + } #else - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - uint32_t timeout = 0xffff; - while((PIOS_SPI_Busy(spi_id) || spi_dev->busy) && --timeout); - if(timeout == 0) //timed out - return -1; - - PIOS_IRQ_Disable(); - if(spi_dev->busy) - return -1; - spi_dev->busy = 1; - PIOS_IRQ_Enable(); -#endif - return 0; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + uint32_t timeout = 0xffff; + while ((PIOS_SPI_Busy(spi_id) || spi_dev->busy) && --timeout) { + ; + } + if (timeout == 0) { // timed out + return -1; + } + + PIOS_IRQ_Disable(); + if (spi_dev->busy) { + return -1; + } + spi_dev->busy = 1; + PIOS_IRQ_Enable(); +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + return 0; } /** @@ -284,24 +292,26 @@ int32_t PIOS_SPI_ClaimBus(uint32_t spi_id) int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) - - if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE){ - return -1; - } - if (woken) { - *woken = *woken || (higherPriorityTaskWoken == pdTRUE); - } - return 0; + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) + + if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE) { + return -1; + } + if (woken) { + *woken = *woken || (higherPriorityTaskWoken == pdTRUE); + } + return 0; + #else - if (woken) { - *woken = false; - } - return PIOS_SPI_ClaimBus(spi_id); + if (woken) { + *woken = false; + } + return PIOS_SPI_ClaimBus(spi_id); + #endif } @@ -313,19 +323,19 @@ int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken) int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) - xSemaphoreGive(spi_dev->busy); + xSemaphoreGive(spi_dev->busy); #else - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - PIOS_IRQ_Disable(); - spi_dev->busy = 0; - PIOS_IRQ_Enable(); + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + PIOS_IRQ_Disable(); + spi_dev->busy = 0; + PIOS_IRQ_Enable(); #endif - return 0; + return 0; } /** @@ -338,388 +348,422 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id) int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; bool valid = PIOS_SPI_validate(spi_dev); PIOS_Assert(valid) xSemaphoreGiveFromISR(spi_dev->busy, &higherPriorityTaskWoken); if (woken) { - *woken = *woken || (higherPriorityTaskWoken == pdTRUE); + *woken = *woken || (higherPriorityTaskWoken == pdTRUE); } - return 0; + return 0; + #else - if (woken) { - *woken = false; - } - return PIOS_SPI_ReleaseBus(spi_id); + if (woken) { + *woken = false; + } + return PIOS_SPI_ReleaseBus(spi_id); + #endif } /** -* Controls the RC (Register Clock alias Chip Select) pin of a SPI port -* \param[in] spi SPI number (0 or 1) -* \param[in] pin_value 0 or 1 -* \return 0 if no error -*/ + * Controls the RC (Register Clock alias Chip Select) pin of a SPI port + * \param[in] spi SPI number (0 or 1) + * \param[in] pin_value 0 or 1 + * \return 0 if no error + */ int32_t PIOS_SPI_RC_PinSet(uint32_t spi_id, uint32_t slave_id, uint8_t pin_value) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) - PIOS_Assert(slave_id <= spi_dev->cfg->slave_count) + bool valid = PIOS_SPI_validate(spi_dev); - /* XXX multi-slave support? */ - if (pin_value) { - GPIO_SetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); - } else { - GPIO_ResetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); - } + PIOS_Assert(valid) + PIOS_Assert(slave_id <= spi_dev->cfg->slave_count) - return 0; + /* XXX multi-slave support? */ + if (pin_value) { + GPIO_SetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); + } else { + GPIO_ResetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); + } + + return 0; } /** -* Transfers a byte to SPI output and reads back the return value from SPI input -* \param[in] spi SPI number (0 or 1) -* \param[in] b the byte which should be transfered -*/ + * Transfers a byte to SPI output and reads back the return value from SPI input + * \param[in] spi SPI number (0 or 1) + * \param[in] b the byte which should be transfered + */ int32_t PIOS_SPI_TransferByte(uint32_t spi_id, uint8_t b) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); -// uint8_t dummy; - uint8_t rx_byte; + PIOS_Assert(valid) - /* - * Procedure taken from STM32F10xxx Reference Manual section 23.3.5 - */ +// uint8_t dummy; + uint8_t rx_byte; - /* Make sure the RXNE flag is cleared by reading the DR register */ - /*dummy =*/(void)spi_dev->cfg->regs->DR; + /* + * Procedure taken from STM32F10xxx Reference Manual section 23.3.5 + */ - /* Start the transfer */ - spi_dev->cfg->regs->DR = b; + /* Make sure the RXNE flag is cleared by reading the DR register */ + /*dummy =*/ (void)spi_dev->cfg->regs->DR; - /* Wait until there is a byte to read */ - while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_RXNE)) ; + /* Start the transfer */ + spi_dev->cfg->regs->DR = b; - /* Read the rx'd byte */ - rx_byte = spi_dev->cfg->regs->DR; + /* Wait until there is a byte to read */ + while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_RXNE)) { + ; + } - /* Wait until the TXE goes high */ - while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) ; + /* Read the rx'd byte */ + rx_byte = spi_dev->cfg->regs->DR; - /* Wait for SPI transfer to have fully completed */ - while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) ; + /* Wait until the TXE goes high */ + while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) { + ; + } - /* Return received byte */ - return rx_byte; + /* Wait for SPI transfer to have fully completed */ + while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) { + ; + } + + /* Return received byte */ + return rx_byte; } /** -* Transfers a block of bytes via DMA. -* \param[in] spi SPI number (0 or 1) -* \param[in] send_buffer pointer to buffer which should be sent.
-* If NULL, 0xff (all-one) will be sent. -* \param[in] receive_buffer pointer to buffer which should get the received values.
-* If NULL, received bytes will be discarded. -* \param[in] len number of bytes which should be transfered -* \param[in] callback pointer to callback function which will be executed -* from DMA channel interrupt once the transfer is finished. -* If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will -* block until the transfer is finished. -* \return >= 0 if no error during transfer -* \return -1 if disabled SPI port selected -* \return -3 if function has been called during an ongoing DMA transfer -*/ + * Transfers a block of bytes via DMA. + * \param[in] spi SPI number (0 or 1) + * \param[in] send_buffer pointer to buffer which should be sent.
+ * If NULL, 0xff (all-one) will be sent. + * \param[in] receive_buffer pointer to buffer which should get the received values.
+ * If NULL, received bytes will be discarded. + * \param[in] len number of bytes which should be transfered + * \param[in] callback pointer to callback function which will be executed + * from DMA channel interrupt once the transfer is finished. + * If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will + * block until the transfer is finished. + * \return >= 0 if no error during transfer + * \return -1 if disabled SPI port selected + * \return -3 if function has been called during an ongoing DMA transfer + */ static int32_t SPI_DMA_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - DMA_InitTypeDef dma_init; + PIOS_Assert(valid) - /* Exit if ongoing transfer */ - if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { - return -3; - } + DMA_InitTypeDef dma_init; - /* Disable the DMA channels */ - DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); - DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); + /* Exit if ongoing transfer */ + if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { + return -3; + } - while(DMA_GetCmdStatus(spi_dev->cfg->dma.rx.channel) == ENABLE); - while(DMA_GetCmdStatus(spi_dev->cfg->dma.tx.channel) == ENABLE); + /* Disable the DMA channels */ + DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); + DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); - /* Disable the SPI peripheral */ - /* Initialize the SPI block */ - SPI_DeInit(spi_dev->cfg->regs); - SPI_Init(spi_dev->cfg->regs, (SPI_InitTypeDef*)&(spi_dev->cfg->init)); - SPI_Cmd(spi_dev->cfg->regs, DISABLE); - /* Configure CRC calculation */ - if (spi_dev->cfg->use_crc) { - SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); - } else { - SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); - } - - /* Enable SPI interrupts to DMA */ - SPI_I2S_DMACmd(spi_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); + while (DMA_GetCmdStatus(spi_dev->cfg->dma.rx.channel) == ENABLE) { + ; + } + while (DMA_GetCmdStatus(spi_dev->cfg->dma.tx.channel) == ENABLE) { + ; + } - /* Set callback function */ - spi_dev->callback = callback; + /* Disable the SPI peripheral */ + /* Initialize the SPI block */ + SPI_DeInit(spi_dev->cfg->regs); + SPI_Init(spi_dev->cfg->regs, (SPI_InitTypeDef *)&(spi_dev->cfg->init)); + SPI_Cmd(spi_dev->cfg->regs, DISABLE); + /* Configure CRC calculation */ + if (spi_dev->cfg->use_crc) { + SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); + } else { + SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); + } - /* - * Configure Rx channel - */ + /* Enable SPI interrupts to DMA */ + SPI_I2S_DMACmd(spi_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); - /* Start with the default configuration for this peripheral */ - dma_init = spi_dev->cfg->dma.rx.init; - DMA_DeInit(spi_dev->cfg->dma.rx.channel); - if (receive_buffer != NULL) { - /* Enable memory addr. increment - bytes written into receive buffer */ - dma_init.DMA_Memory0BaseAddr = (uint32_t) receive_buffer; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - } else { - /* Disable memory addr. increment - bytes written into dummy buffer */ - spi_dev->rx_dummy_byte = 0xFF; - dma_init.DMA_Memory0BaseAddr = (uint32_t) &spi_dev->rx_dummy_byte; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; - } - if (spi_dev->cfg->use_crc) { - /* Make sure the CRC error flag is cleared before we start */ - SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); - } + /* Set callback function */ + spi_dev->callback = callback; - dma_init.DMA_BufferSize = len; - DMA_Init(spi_dev->cfg->dma.rx.channel, &(dma_init)); + /* + * Configure Rx channel + */ - /* - * Configure Tx channel - */ + /* Start with the default configuration for this peripheral */ + dma_init = spi_dev->cfg->dma.rx.init; + DMA_DeInit(spi_dev->cfg->dma.rx.channel); + if (receive_buffer != NULL) { + /* Enable memory addr. increment - bytes written into receive buffer */ + dma_init.DMA_Memory0BaseAddr = (uint32_t)receive_buffer; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + } else { + /* Disable memory addr. increment - bytes written into dummy buffer */ + spi_dev->rx_dummy_byte = 0xFF; + dma_init.DMA_Memory0BaseAddr = (uint32_t)&spi_dev->rx_dummy_byte; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; + } + if (spi_dev->cfg->use_crc) { + /* Make sure the CRC error flag is cleared before we start */ + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + } - /* Start with the default configuration for this peripheral */ - dma_init = spi_dev->cfg->dma.tx.init; - DMA_DeInit(spi_dev->cfg->dma.tx.channel); - if (send_buffer != NULL) { - /* Enable memory addr. increment - bytes written into receive buffer */ - dma_init.DMA_Memory0BaseAddr = (uint32_t) send_buffer; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - } else { - /* Disable memory addr. increment - bytes written into dummy buffer */ - spi_dev->tx_dummy_byte = 0xFF; - dma_init.DMA_Memory0BaseAddr = (uint32_t) &spi_dev->tx_dummy_byte; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; - } + dma_init.DMA_BufferSize = len; + DMA_Init(spi_dev->cfg->dma.rx.channel, &(dma_init)); - if (spi_dev->cfg->use_crc) { - /* The last byte of the payload will be replaced with the CRC8 */ - dma_init.DMA_BufferSize = len - 1; - } else { - dma_init.DMA_BufferSize = len; - } + /* + * Configure Tx channel + */ - DMA_Init(spi_dev->cfg->dma.tx.channel, &(dma_init)); + /* Start with the default configuration for this peripheral */ + dma_init = spi_dev->cfg->dma.tx.init; + DMA_DeInit(spi_dev->cfg->dma.tx.channel); + if (send_buffer != NULL) { + /* Enable memory addr. increment - bytes written into receive buffer */ + dma_init.DMA_Memory0BaseAddr = (uint32_t)send_buffer; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + } else { + /* Disable memory addr. increment - bytes written into dummy buffer */ + spi_dev->tx_dummy_byte = 0xFF; + dma_init.DMA_Memory0BaseAddr = (uint32_t)&spi_dev->tx_dummy_byte; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; + } - /* Enable DMA interrupt if callback function active */ - DMA_ITConfig(spi_dev->cfg->dma.rx.channel, DMA_IT_TC, (callback != NULL) ? ENABLE : DISABLE); + if (spi_dev->cfg->use_crc) { + /* The last byte of the payload will be replaced with the CRC8 */ + dma_init.DMA_BufferSize = len - 1; + } else { + dma_init.DMA_BufferSize = len; + } - /* Flush out the CRC registers */ - SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); - (void)SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); - SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + DMA_Init(spi_dev->cfg->dma.tx.channel, &(dma_init)); - /* Make sure to flush out the receive buffer */ - (void)SPI_I2S_ReceiveData(spi_dev->cfg->regs); + /* Enable DMA interrupt if callback function active */ + DMA_ITConfig(spi_dev->cfg->dma.rx.channel, DMA_IT_TC, (callback != NULL) ? ENABLE : DISABLE); - if (spi_dev->cfg->use_crc) { - /* Need a 0->1 transition to reset the CRC logic */ - SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); - } + /* Flush out the CRC registers */ + SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); + (void)SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); - /* Start DMA transfers */ - DMA_Cmd(spi_dev->cfg->dma.rx.channel, ENABLE); - DMA_Cmd(spi_dev->cfg->dma.tx.channel, ENABLE); + /* Make sure to flush out the receive buffer */ + (void)SPI_I2S_ReceiveData(spi_dev->cfg->regs); - /* Reenable the SPI device */ - SPI_Cmd(spi_dev->cfg->regs, ENABLE); + if (spi_dev->cfg->use_crc) { + /* Need a 0->1 transition to reset the CRC logic */ + SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); + } - if (callback) { - /* User has requested a callback, don't wait for the transfer to complete. */ - return 0; - } + /* Start DMA transfers */ + DMA_Cmd(spi_dev->cfg->dma.rx.channel, ENABLE); + DMA_Cmd(spi_dev->cfg->dma.tx.channel, ENABLE); - /* Wait until all bytes have been transmitted/received */ - while (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)); + /* Reenable the SPI device */ + SPI_Cmd(spi_dev->cfg->regs, ENABLE); - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))); + if (callback) { + /* User has requested a callback, don't wait for the transfer to complete. */ + return 0; + } - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)); + /* Wait until all bytes have been transmitted/received */ + while (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { + ; + } - /* Check the CRC on the transfer if enabled. */ - if (spi_dev->cfg->use_crc) { - /* Check the SPI CRC error flag */ - if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { - return -4; - } - } + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))) { + ; + } - /* No error */ - return 0; + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + ; + } + + /* Check the CRC on the transfer if enabled. */ + if (spi_dev->cfg->use_crc) { + /* Check the SPI CRC error flag */ + if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { + return -4; + } + } + + /* No error */ + return 0; } /** -* Transfers a block of bytes via PIO. -* -* \param[in] spi_id SPI device handle -* \param[in] send_buffer pointer to buffer which should be sent.
-* If NULL, 0xff (all-one) will be sent. -* \param[in] receive_buffer pointer to buffer which should get the received values.
-* If NULL, received bytes will be discarded. -* \param[in] len number of bytes which should be transfered -* \return >= 0 if no error during transfer -* \return -1 if disabled SPI port selected -* \return -3 if function has been called during an ongoing DMA transfer -*/ + * Transfers a block of bytes via PIO. + * + * \param[in] spi_id SPI device handle + * \param[in] send_buffer pointer to buffer which should be sent.
+ * If NULL, 0xff (all-one) will be sent. + * \param[in] receive_buffer pointer to buffer which should get the received values.
+ * If NULL, received bytes will be discarded. + * \param[in] len number of bytes which should be transfered + * \return >= 0 if no error during transfer + * \return -1 if disabled SPI port selected + * \return -3 if function has been called during an ongoing DMA transfer + */ static int32_t SPI_PIO_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - uint8_t b; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + uint8_t b; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - /* Exit if ongoing transfer */ - if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { - return -3; - } + PIOS_Assert(valid) - /* Make sure the RXNE flag is cleared by reading the DR register */ - b = spi_dev->cfg->regs->DR; + /* Exit if ongoing transfer */ + if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { + return -3; + } - while (len--) { - /* get the byte to send */ - b = send_buffer ? *(send_buffer++) : 0xff; + /* Make sure the RXNE flag is cleared by reading the DR register */ + b = spi_dev->cfg->regs->DR; - /* Start the transfer */ - spi_dev->cfg->regs->DR = b; + while (len--) { + /* get the byte to send */ + b = send_buffer ? *(send_buffer++) : 0xff; - /* Wait until there is a byte to read */ - while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_RXNE)) ; + /* Start the transfer */ + spi_dev->cfg->regs->DR = b; - /* Read the rx'd byte */ - b = spi_dev->cfg->regs->DR; + /* Wait until there is a byte to read */ + while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_RXNE)) { + ; + } - /* save the received byte */ - if (receive_buffer) - *(receive_buffer++) = b; + /* Read the rx'd byte */ + b = spi_dev->cfg->regs->DR; - /* Wait until the TXE goes high */ - while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) ; - } + /* save the received byte */ + if (receive_buffer) { + *(receive_buffer++) = b; + } - /* Wait for SPI transfer to have fully completed */ - while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) ; + /* Wait until the TXE goes high */ + while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) { + ; + } + } - return 0; + /* Wait for SPI transfer to have fully completed */ + while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) { + ; + } + + return 0; } /** -* Transfers a block of bytes via PIO or DMA. -* \param[in] spi_id SPI device handle -* \param[in] send_buffer pointer to buffer which should be sent.
-* If NULL, 0xff (all-one) will be sent. -* \param[in] receive_buffer pointer to buffer which should get the received values.
-* If NULL, received bytes will be discarded. -* \param[in] len number of bytes which should be transfered -* \param[in] callback pointer to callback function which will be executed -* from DMA channel interrupt once the transfer is finished. -* If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will -* block until the transfer is finished. -* \return >= 0 if no error during transfer -* \return -1 if disabled SPI port selected -* \return -3 if function has been called during an ongoing DMA transfer -*/ + * Transfers a block of bytes via PIO or DMA. + * \param[in] spi_id SPI device handle + * \param[in] send_buffer pointer to buffer which should be sent.
+ * If NULL, 0xff (all-one) will be sent. + * \param[in] receive_buffer pointer to buffer which should get the received values.
+ * If NULL, received bytes will be discarded. + * \param[in] len number of bytes which should be transfered + * \param[in] callback pointer to callback function which will be executed + * from DMA channel interrupt once the transfer is finished. + * If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will + * block until the transfer is finished. + * \return >= 0 if no error during transfer + * \return -1 if disabled SPI port selected + * \return -3 if function has been called during an ongoing DMA transfer + */ int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback) { - if (callback || len > SPI_MAX_BLOCK_PIO) { - return SPI_DMA_TransferBlock(spi_id, send_buffer, receive_buffer, len, callback); - } - return SPI_PIO_TransferBlock(spi_id, send_buffer, receive_buffer, len); + if (callback || len > SPI_MAX_BLOCK_PIO) { + return SPI_DMA_TransferBlock(spi_id, send_buffer, receive_buffer, len, callback); + } + return SPI_PIO_TransferBlock(spi_id, send_buffer, receive_buffer, len); } /** -* Check if a transfer is in progress -* \param[in] spi SPI number (0 or 1) -* \return >= 0 if no transfer is in progress -* \return -1 if disabled SPI port selected -* \return -2 if unsupported SPI port selected -* \return -3 if function has been called during an ongoing DMA transfer -*/ + * Check if a transfer is in progress + * \param[in] spi SPI number (0 or 1) + * \return >= 0 if no transfer is in progress + * \return -1 if disabled SPI port selected + * \return -2 if unsupported SPI port selected + * \return -3 if function has been called during an ongoing DMA transfer + */ int32_t PIOS_SPI_Busy(uint32_t spi_id) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - /* DMA buffer has data or SPI transmit register not empty or SPI is busy*/ - if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel) || - !SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE) || - SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) - { - return -3; - } + PIOS_Assert(valid) - return(0); + /* DMA buffer has data or SPI transmit register not empty or SPI is busy*/ + if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel) || + !SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE) || + SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + return -3; + } + + return 0; } void PIOS_SPI_IRQ_Handler(uint32_t spi_id) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) - - // FIXME XXX Only RX channel or better clear flags for both channels? - DMA_ClearFlag(spi_dev->cfg->dma.rx.channel, spi_dev->cfg->dma.irq.flags); - - if(spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) { - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))) ; + bool valid = PIOS_SPI_validate(spi_dev); - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) ; - } + PIOS_Assert(valid) - if (spi_dev->callback != NULL) { - bool crc_ok = true; - uint8_t crc_val; + // FIXME XXX Only RX channel or better clear flags for both channels? + DMA_ClearFlag(spi_dev->cfg->dma.rx.channel, spi_dev->cfg->dma.irq.flags); - if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { - crc_ok = false; - SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); - } - crc_val = SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); - spi_dev->callback(crc_ok, crc_val); - } + if (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) { + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))) { + ; + } + + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + ; + } + } + + if (spi_dev->callback != NULL) { + bool crc_ok = true; + uint8_t crc_val; + + if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { + crc_ok = false; + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + } + crc_val = SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); + spi_dev->callback(crc_ok, crc_val); + } } #endif /* PIOS_INCLUDE_SPI */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_sys.c b/flight/pios/stm32f4xx/pios_sys.c index 7819513a3..79b17fc7f 100644 --- a/flight/pios/stm32f4xx/pios_sys.c +++ b/flight/pios/stm32f4xx/pios_sys.c @@ -6,26 +6,26 @@ * @brief PIOS System Initialization code * @{ * - * @file pios_sys.c + * @file pios_sys.c * @author Michael Smith Copyright (C) 2011 - * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Sets up basic STM32 system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,9 +38,9 @@ void NVIC_Configuration(void); void SysTick_Handler(void); /* Local Macros */ -#define MEM8(addr) (*((volatile uint8_t *)(addr))) -#define MEM16(addr) (*((volatile uint16_t *)(addr))) -#define MEM32(addr) (*((volatile uint32_t *)(addr))) +#define MEM8(addr) (*((volatile uint8_t *)(addr))) +#define MEM16(addr) (*((volatile uint16_t *)(addr))) +#define MEM32(addr) (*((volatile uint32_t *)(addr))) /** * Initialises all system peripherals @@ -69,81 +69,81 @@ void PIOS_SYS_Init(void) * light up everything we might reasonably use here and just leave it on. */ RCC_AHB1PeriphClockCmd( - RCC_AHB1Periph_GPIOA | - RCC_AHB1Periph_GPIOB | - RCC_AHB1Periph_GPIOC | - RCC_AHB1Periph_GPIOD | - RCC_AHB1Periph_GPIOE | - RCC_AHB1Periph_GPIOF | - RCC_AHB1Periph_GPIOG | - RCC_AHB1Periph_GPIOH | - RCC_AHB1Periph_GPIOI | - RCC_AHB1Periph_CRC | - RCC_AHB1Periph_FLITF | - RCC_AHB1Periph_SRAM1 | - RCC_AHB1Periph_SRAM2 | - RCC_AHB1Periph_BKPSRAM | - RCC_AHB1Periph_DMA1 | - RCC_AHB1Periph_DMA2 | - //RCC_AHB1Periph_ETH_MAC | No ethernet - //RCC_AHB1Periph_ETH_MAC_Tx | - //RCC_AHB1Periph_ETH_MAC_Rx | - //RCC_AHB1Periph_ETH_MAC_PTP | - //RCC_AHB1Periph_OTG_HS | No high-speed USB (requires external PHY) - //RCC_AHB1Periph_OTG_HS_ULPI | No ULPI PHY (see above) - 0, ENABLE); + RCC_AHB1Periph_GPIOA | + RCC_AHB1Periph_GPIOB | + RCC_AHB1Periph_GPIOC | + RCC_AHB1Periph_GPIOD | + RCC_AHB1Periph_GPIOE | + RCC_AHB1Periph_GPIOF | + RCC_AHB1Periph_GPIOG | + RCC_AHB1Periph_GPIOH | + RCC_AHB1Periph_GPIOI | + RCC_AHB1Periph_CRC | + RCC_AHB1Periph_FLITF | + RCC_AHB1Periph_SRAM1 | + RCC_AHB1Periph_SRAM2 | + RCC_AHB1Periph_BKPSRAM | + RCC_AHB1Periph_DMA1 | + RCC_AHB1Periph_DMA2 | + // RCC_AHB1Periph_ETH_MAC | No ethernet + // RCC_AHB1Periph_ETH_MAC_Tx | + // RCC_AHB1Periph_ETH_MAC_Rx | + // RCC_AHB1Periph_ETH_MAC_PTP | + // RCC_AHB1Periph_OTG_HS | No high-speed USB (requires external PHY) + // RCC_AHB1Periph_OTG_HS_ULPI | No ULPI PHY (see above) + 0, ENABLE); RCC_AHB2PeriphClockCmd( - //RCC_AHB2Periph_DCMI | No camera @todo might make sense later for basic vision support? - //RCC_AHB2Periph_CRYP | No crypto - //RCC_AHB2Periph_HASH | No hash generator - //RCC_AHB2Periph_RNG | No random numbers @todo might be good to have later if entropy is desired - //RCC_AHB2Periph_OTG_FS | - 0, ENABLE); + // RCC_AHB2Periph_DCMI | No camera @todo might make sense later for basic vision support? + // RCC_AHB2Periph_CRYP | No crypto + // RCC_AHB2Periph_HASH | No hash generator + // RCC_AHB2Periph_RNG | No random numbers @todo might be good to have later if entropy is desired + // RCC_AHB2Periph_OTG_FS | + 0, ENABLE); RCC_AHB3PeriphClockCmd( - //RCC_AHB3Periph_FSMC | No external static memory - 0, ENABLE); + // RCC_AHB3Periph_FSMC | No external static memory + 0, ENABLE); RCC_APB1PeriphClockCmd( - RCC_APB1Periph_TIM2 | - RCC_APB1Periph_TIM3 | - RCC_APB1Periph_TIM4 | - RCC_APB1Periph_TIM5 | - RCC_APB1Periph_TIM6 | - RCC_APB1Periph_TIM7 | - RCC_APB1Periph_TIM12 | - RCC_APB1Periph_TIM13 | - RCC_APB1Periph_TIM14 | - RCC_APB1Periph_WWDG | - RCC_APB1Periph_SPI2 | - RCC_APB1Periph_SPI3 | - RCC_APB1Periph_USART2 | - RCC_APB1Periph_USART3 | - RCC_APB1Periph_UART4 | - RCC_APB1Periph_UART5 | - RCC_APB1Periph_I2C1 | - RCC_APB1Periph_I2C2 | - RCC_APB1Periph_I2C3 | - RCC_APB1Periph_CAN1 | - RCC_APB1Periph_CAN2 | - RCC_APB1Periph_PWR | - RCC_APB1Periph_DAC | - 0, ENABLE); + RCC_APB1Periph_TIM2 | + RCC_APB1Periph_TIM3 | + RCC_APB1Periph_TIM4 | + RCC_APB1Periph_TIM5 | + RCC_APB1Periph_TIM6 | + RCC_APB1Periph_TIM7 | + RCC_APB1Periph_TIM12 | + RCC_APB1Periph_TIM13 | + RCC_APB1Periph_TIM14 | + RCC_APB1Periph_WWDG | + RCC_APB1Periph_SPI2 | + RCC_APB1Periph_SPI3 | + RCC_APB1Periph_USART2 | + RCC_APB1Periph_USART3 | + RCC_APB1Periph_UART4 | + RCC_APB1Periph_UART5 | + RCC_APB1Periph_I2C1 | + RCC_APB1Periph_I2C2 | + RCC_APB1Periph_I2C3 | + RCC_APB1Periph_CAN1 | + RCC_APB1Periph_CAN2 | + RCC_APB1Periph_PWR | + RCC_APB1Periph_DAC | + 0, ENABLE); RCC_APB2PeriphClockCmd( - RCC_APB2Periph_TIM1 | - RCC_APB2Periph_TIM8 | - RCC_APB2Periph_USART1 | - RCC_APB2Periph_USART6 | - RCC_APB2Periph_ADC | - RCC_APB2Periph_ADC1 | - RCC_APB2Periph_ADC2 | - RCC_APB2Periph_ADC3 | - RCC_APB2Periph_SDIO | - RCC_APB2Periph_SPI1 | - RCC_APB2Periph_SYSCFG | - RCC_APB2Periph_TIM9 | - RCC_APB2Periph_TIM10 | - RCC_APB2Periph_TIM11 | - 0, ENABLE); + RCC_APB2Periph_TIM1 | + RCC_APB2Periph_TIM8 | + RCC_APB2Periph_USART1 | + RCC_APB2Periph_USART6 | + RCC_APB2Periph_ADC | + RCC_APB2Periph_ADC1 | + RCC_APB2Periph_ADC2 | + RCC_APB2Periph_ADC3 | + RCC_APB2Periph_SDIO | + RCC_APB2Periph_SPI1 | + RCC_APB2Periph_SYSCFG | + RCC_APB2Periph_TIM9 | + RCC_APB2Periph_TIM10 | + RCC_APB2Periph_TIM11 | + 0, ENABLE); /* * Configure all pins as input / pullup to avoid issues with @@ -152,20 +152,20 @@ void PIOS_SYS_Init(void) */ GPIO_InitTypeDef GPIO_InitStructure; GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; // default is un-pulled input + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; // default is un-pulled input - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; #if (PIOS_USB_ENABLED) - GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_11 | GPIO_Pin_12); // leave USB D+/D- alone + GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_11 | GPIO_Pin_12); // leave USB D+/D- alone #endif - GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15); // leave JTAG pins alone + GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15); // leave JTAG pins alone GPIO_Init(GPIOA, &GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; - GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_3 | GPIO_Pin_4);// leave JTAG pins alone + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; + GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_3 | GPIO_Pin_4); // leave JTAG pins alone GPIO_Init(GPIOB, &GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; GPIO_Init(GPIOC, &GPIO_InitStructure); GPIO_Init(GPIOD, &GPIO_InitStructure); GPIO_Init(GPIOE, &GPIO_InitStructure); @@ -199,17 +199,19 @@ int32_t PIOS_SYS_Reset(void) // turn off all board LEDs #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Off(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ +#endif /* PIOS_LED_HEARTBEAT */ #if defined(PIOS_LED_ALARM) PIOS_LED_Off(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ +#endif /* PIOS_LED_ALARM */ /* XXX F10x port resets most (but not all) peripherals ... do we care? */ /* Reset STM32 */ NVIC_SystemReset(); - while (1); + while (1) { + ; + } /* We will never reach this point */ return -1; @@ -220,7 +222,7 @@ int32_t PIOS_SYS_Reset(void) */ uint32_t PIOS_SYS_getCPUFlashSize(void) { - return ((uint32_t) MEM16(0x1fff7a22) * 1024); // it might be possible to locate in the OTP area, but haven't looked and not documented + return (uint32_t)MEM16(0x1fff7a22) * 1024; // it might be possible to locate in the OTP area, but haven't looked and not documented } /** @@ -257,8 +259,9 @@ int32_t PIOS_SYS_SerialNumberGet(char *str) /* Stored in the so called "electronic signature" */ for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { uint8_t b = MEM8(0x1fff7a10 + (i / 2)); - if (!(i & 1)) - b >>= 4; + if (!(i & 1)) { + b >>= 4; + } b &= 0x0f; str[i] = ((b > 9) ? ('A' - 10) : '0') + b; @@ -276,6 +279,7 @@ void NVIC_Configuration(void) { /* Set the Vector Table base address as specified in .ld file */ extern void *pios_isr_vector_table_base; + NVIC_SetVectorTable((uint32_t)&pios_isr_vector_table_base, 0x0); /* 4 bits for Interrupt priorities so no sub priorities */ @@ -293,7 +297,7 @@ void NVIC_Configuration(void) * \param[in] line assert_param error line source number * \retval None */ -void assert_failed(uint8_t * file, uint32_t line) +void assert_failed(uint8_t *file, uint32_t line) { /* When serial debugging is implemented, use something like this. */ /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ @@ -301,23 +305,25 @@ void assert_failed(uint8_t * file, uint32_t line) /* Setup the LEDs to Alternate */ #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_On(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ +#endif /* PIOS_LED_HEARTBEAT */ #if defined(PIOS_LED_ALARM) PIOS_LED_Off(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ +#endif /* PIOS_LED_ALARM */ /* Infinite loop */ while (1) { #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ +#endif /* PIOS_LED_HEARTBEAT */ #if defined(PIOS_LED_ALARM) PIOS_LED_Toggle(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ - for (int i = 0; i < 1000000; i++); +#endif /* PIOS_LED_ALARM */ + for (int i = 0; i < 1000000; i++) { + ; + } } } -#endif +#endif /* ifdef USE_FULL_ASSERT */ #endif /* PIOS_INCLUDE_SYS */ diff --git a/flight/pios/stm32f4xx/pios_tim.c b/flight/pios/stm32f4xx/pios_tim.c index 4f3828d68..ec72a9c6f 100644 --- a/flight/pios/stm32f4xx/pios_tim.c +++ b/flight/pios/stm32f4xx/pios_tim.c @@ -6,25 +6,25 @@ * @brief PIOS Timer control code * @{ * - * @file pios_tim.c + * @file pios_tim.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Sets up timers and ways to register callbacks on them * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,296 +36,297 @@ #include "pios_tim_priv.h" enum pios_tim_dev_magic { - PIOS_TIM_DEV_MAGIC = 0x87654098, + PIOS_TIM_DEV_MAGIC = 0x87654098, }; struct pios_tim_dev { - enum pios_tim_dev_magic magic; + enum pios_tim_dev_magic magic; - const struct pios_tim_channel * channels; - uint8_t num_channels; + const struct pios_tim_channel *channels; + uint8_t num_channels; - const struct pios_tim_callbacks * callbacks; - uint32_t context; + const struct pios_tim_callbacks *callbacks; + uint32_t context; }; #define PIOS_TIM_ALL_FLAGS TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4 | TIM_IT_Trigger | TIM_IT_Update static struct pios_tim_dev pios_tim_devs[PIOS_TIM_MAX_DEVS]; static uint8_t pios_tim_num_devs; -static struct pios_tim_dev * PIOS_TIM_alloc(void) +static struct pios_tim_dev *PIOS_TIM_alloc(void) { - struct pios_tim_dev * tim_dev; + struct pios_tim_dev *tim_dev; - if (pios_tim_num_devs >= PIOS_TIM_MAX_DEVS) { - return (NULL); - } + if (pios_tim_num_devs >= PIOS_TIM_MAX_DEVS) { + return NULL; + } - tim_dev = &pios_tim_devs[pios_tim_num_devs++]; - tim_dev->magic = PIOS_TIM_DEV_MAGIC; + tim_dev = &pios_tim_devs[pios_tim_num_devs++]; + tim_dev->magic = PIOS_TIM_DEV_MAGIC; - return (tim_dev); + return tim_dev; } - -int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg * cfg) +int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg *cfg) { - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(cfg); - /* Enable appropriate clock to timer module */ - switch((uint32_t) cfg->timer) { - case (uint32_t)TIM1: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); - break; - case (uint32_t)TIM2: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); - break; - case (uint32_t)TIM3: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); - break; - case (uint32_t)TIM4: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); - break; - case (uint32_t)TIM5: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); - break; - case (uint32_t)TIM6: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); - break; - case (uint32_t)TIM7: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); - break; - case (uint32_t)TIM8: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); - break; - case (uint32_t)TIM9: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM9, ENABLE); - break; - case (uint32_t)TIM10: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM10, ENABLE); - break; - case (uint32_t)TIM11: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM11, ENABLE); - break; - case (uint32_t)TIM12: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12, ENABLE); - break; - case (uint32_t)TIM13: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM13, ENABLE); - break; - case (uint32_t)TIM14: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM14, ENABLE); - break; - } + /* Enable appropriate clock to timer module */ + switch ((uint32_t)cfg->timer) { + case (uint32_t)TIM1: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); + break; + case (uint32_t)TIM2: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); + break; + case (uint32_t)TIM3: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); + break; + case (uint32_t)TIM4: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); + break; + case (uint32_t)TIM5: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); + break; + case (uint32_t)TIM6: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); + break; + case (uint32_t)TIM7: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); + break; + case (uint32_t)TIM8: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); + break; + case (uint32_t)TIM9: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM9, ENABLE); + break; + case (uint32_t)TIM10: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM10, ENABLE); + break; + case (uint32_t)TIM11: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM11, ENABLE); + break; + case (uint32_t)TIM12: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12, ENABLE); + break; + case (uint32_t)TIM13: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM13, ENABLE); + break; + case (uint32_t)TIM14: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM14, ENABLE); + break; + } - /* Configure the dividers for this timer */ - TIM_TimeBaseInit(cfg->timer, cfg->time_base_init); + /* Configure the dividers for this timer */ + TIM_TimeBaseInit(cfg->timer, cfg->time_base_init); - /* Configure internal timer clocks */ - TIM_InternalClockConfig(cfg->timer); + /* Configure internal timer clocks */ + TIM_InternalClockConfig(cfg->timer); - /* Enable timers */ - TIM_Cmd(cfg->timer, ENABLE); + /* Enable timers */ + TIM_Cmd(cfg->timer, ENABLE); - /* Enable Interrupts */ - NVIC_Init(&cfg->irq.init); + /* Enable Interrupts */ + NVIC_Init(&cfg->irq.init); - return 0; + return 0; } -int32_t PIOS_TIM_InitChannels(uint32_t * tim_id, const struct pios_tim_channel * channels, uint8_t num_channels, const struct pios_tim_callbacks * callbacks, uint32_t context) +int32_t PIOS_TIM_InitChannels(uint32_t *tim_id, const struct pios_tim_channel *channels, uint8_t num_channels, const struct pios_tim_callbacks *callbacks, uint32_t context) { - PIOS_Assert(channels); - PIOS_Assert(num_channels); + PIOS_Assert(channels); + PIOS_Assert(num_channels); - struct pios_tim_dev * tim_dev; - tim_dev = (struct pios_tim_dev *) PIOS_TIM_alloc(); - if (!tim_dev) goto out_fail; + struct pios_tim_dev *tim_dev; + tim_dev = (struct pios_tim_dev *)PIOS_TIM_alloc(); + if (!tim_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - tim_dev->channels = channels; - tim_dev->num_channels = num_channels; - tim_dev->callbacks = callbacks; - tim_dev->context = context; + /* Bind the configuration to the device instance */ + tim_dev->channels = channels; + tim_dev->num_channels = num_channels; + tim_dev->callbacks = callbacks; + tim_dev->context = context; - /* Configure the pins */ - for (uint8_t i = 0; i < num_channels; i++) { - const struct pios_tim_channel * chan = &(channels[i]); + /* Configure the pins */ + for (uint8_t i = 0; i < num_channels; i++) { + const struct pios_tim_channel *chan = &(channels[i]); - /* Enable the peripheral clock for the GPIO */ + /* Enable the peripheral clock for the GPIO */ /* switch ((uint32_t)chan->pin.gpio) { - case (uint32_t) GPIOA: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); - break; - case (uint32_t) GPIOB: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - break; - case (uint32_t) GPIOC: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - break; - default: - PIOS_Assert(0); - break; - } -*/ // commented out for now as f4 starts all clocks - GPIO_Init(chan->pin.gpio, &chan->pin.init); + case (uint32_t) GPIOA: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); + break; + case (uint32_t) GPIOB: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + break; + case (uint32_t) GPIOC: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + break; + default: + PIOS_Assert(0); + break; + } + */ // commented out for now as f4 starts all clocks + GPIO_Init(chan->pin.gpio, &chan->pin.init); - PIOS_DEBUG_Assert(chan->remaP); - - // Second parameter should technically be PinSource but they are numerically the same - GPIO_PinAFConfig(chan->pin.gpio, chan->pin.pin_source,chan->remap); - } + PIOS_DEBUG_Assert(chan->remaP); - *tim_id = (uint32_t)tim_dev; + // Second parameter should technically be PinSource but they are numerically the same + GPIO_PinAFConfig(chan->pin.gpio, chan->pin.pin_source, chan->remap); + } - return(0); + *tim_id = (uint32_t)tim_dev; + + return 0; out_fail: - return(-1); + return -1; } -static void PIOS_TIM_generic_irq_handler(TIM_TypeDef * timer) +static void PIOS_TIM_generic_irq_handler(TIM_TypeDef *timer) { - /* Iterate over all registered clients of the TIM layer to find channels on this timer */ - for (uint8_t i = 0; i < pios_tim_num_devs; i++) { - const struct pios_tim_dev * tim_dev = &pios_tim_devs[i]; + /* Iterate over all registered clients of the TIM layer to find channels on this timer */ + for (uint8_t i = 0; i < pios_tim_num_devs; i++) { + const struct pios_tim_dev *tim_dev = &pios_tim_devs[i]; - if (!tim_dev->channels || tim_dev->num_channels == 0) { - /* No channels to process on this client */ - continue; - } + if (!tim_dev->channels || tim_dev->num_channels == 0) { + /* No channels to process on this client */ + continue; + } - /* Check for an overflow event on this timer */ - bool overflow_event; - uint16_t overflow_count; - if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) { - TIM_ClearITPendingBit(timer, TIM_IT_Update); - overflow_count = timer->ARR; - overflow_event = true; - } else { - overflow_count = 0; - overflow_event = false; - } + /* Check for an overflow event on this timer */ + bool overflow_event; + uint16_t overflow_count; + if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) { + TIM_ClearITPendingBit(timer, TIM_IT_Update); + overflow_count = timer->ARR; + overflow_event = true; + } else { + overflow_count = 0; + overflow_event = false; + } - for (uint8_t j = 0; j < tim_dev->num_channels; j++) { - const struct pios_tim_channel * chan = &tim_dev->channels[j]; + for (uint8_t j = 0; j < tim_dev->num_channels; j++) { + const struct pios_tim_channel *chan = &tim_dev->channels[j]; - if (chan->timer != timer) { - /* channel is not on this timer */ - continue; - } + if (chan->timer != timer) { + /* channel is not on this timer */ + continue; + } - /* Figure out which interrupt bit we should be looking at */ - uint16_t timer_it; - switch (chan->timer_chan) { - case TIM_Channel_1: - timer_it = TIM_IT_CC1; - break; - case TIM_Channel_2: - timer_it = TIM_IT_CC2; - break; - case TIM_Channel_3: - timer_it = TIM_IT_CC3; - break; - case TIM_Channel_4: - timer_it = TIM_IT_CC4; - break; - default: - PIOS_Assert(0); - break; - } + /* Figure out which interrupt bit we should be looking at */ + uint16_t timer_it; + switch (chan->timer_chan) { + case TIM_Channel_1: + timer_it = TIM_IT_CC1; + break; + case TIM_Channel_2: + timer_it = TIM_IT_CC2; + break; + case TIM_Channel_3: + timer_it = TIM_IT_CC3; + break; + case TIM_Channel_4: + timer_it = TIM_IT_CC4; + break; + default: + PIOS_Assert(0); + break; + } - bool edge_event; - uint16_t edge_count; - if (TIM_GetITStatus(chan->timer, timer_it) == SET) { - TIM_ClearITPendingBit(chan->timer, timer_it); + bool edge_event; + uint16_t edge_count; + if (TIM_GetITStatus(chan->timer, timer_it) == SET) { + TIM_ClearITPendingBit(chan->timer, timer_it); - /* Read the current counter */ - switch(chan->timer_chan) { - case TIM_Channel_1: - edge_count = TIM_GetCapture1(chan->timer); - break; - case TIM_Channel_2: - edge_count = TIM_GetCapture2(chan->timer); - break; - case TIM_Channel_3: - edge_count = TIM_GetCapture3(chan->timer); - break; - case TIM_Channel_4: - edge_count = TIM_GetCapture4(chan->timer); - break; - default: - PIOS_Assert(0); - break; - } - edge_event = true; - } else { - edge_event = false; - edge_count = 0; - } + /* Read the current counter */ + switch (chan->timer_chan) { + case TIM_Channel_1: + edge_count = TIM_GetCapture1(chan->timer); + break; + case TIM_Channel_2: + edge_count = TIM_GetCapture2(chan->timer); + break; + case TIM_Channel_3: + edge_count = TIM_GetCapture3(chan->timer); + break; + case TIM_Channel_4: + edge_count = TIM_GetCapture4(chan->timer); + break; + default: + PIOS_Assert(0); + break; + } + edge_event = true; + } else { + edge_event = false; + edge_count = 0; + } - if (!tim_dev->callbacks) { - /* No callbacks registered, we're done with this channel */ - continue; - } + if (!tim_dev->callbacks) { + /* No callbacks registered, we're done with this channel */ + continue; + } - /* Generate the appropriate callbacks */ - if (overflow_event & edge_event) { - /* - * When both edge and overflow happen in the same interrupt, we - * need a heuristic to determine the order of the edge and overflow - * events so that the callbacks happen in the right order. If we - * get the order wrong, our pulse width calculations could be off by up - * to ARR ticks. That could be bad. - * - * Heuristic: If the edge_count is < 16 ticks above zero then we assume the - * edge happened just after the overflow. - */ + /* Generate the appropriate callbacks */ + if (overflow_event & edge_event) { + /* + * When both edge and overflow happen in the same interrupt, we + * need a heuristic to determine the order of the edge and overflow + * events so that the callbacks happen in the right order. If we + * get the order wrong, our pulse width calculations could be off by up + * to ARR ticks. That could be bad. + * + * Heuristic: If the edge_count is < 16 ticks above zero then we assume the + * edge happened just after the overflow. + */ - if (edge_count < 16) { - /* Call the overflow callback first */ - if (tim_dev->callbacks->overflow) { - (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, - tim_dev->context, - j, - overflow_count); - } - /* Call the edge callback second */ - if (tim_dev->callbacks->edge) { - (*tim_dev->callbacks->edge)((uint32_t)tim_dev, - tim_dev->context, - j, - edge_count); - } - } else { - /* Call the edge callback first */ - if (tim_dev->callbacks->edge) { - (*tim_dev->callbacks->edge)((uint32_t)tim_dev, - tim_dev->context, - j, - edge_count); - } - /* Call the overflow callback second */ - if (tim_dev->callbacks->overflow) { - (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, - tim_dev->context, - j, - overflow_count); - } - } - } else if (overflow_event && tim_dev->callbacks->overflow) { - (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, - tim_dev->context, - j, - overflow_count); - } else if (edge_event && tim_dev->callbacks->edge) { - (*tim_dev->callbacks->edge)((uint32_t)tim_dev, - tim_dev->context, - j, - edge_count); - } - } - } + if (edge_count < 16) { + /* Call the overflow callback first */ + if (tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } + /* Call the edge callback second */ + if (tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + } else { + /* Call the edge callback first */ + if (tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + /* Call the overflow callback second */ + if (tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } + } + } else if (overflow_event && tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } else if (edge_event && tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + } + } } /* Bind Interrupt Handlers @@ -333,137 +334,119 @@ static void PIOS_TIM_generic_irq_handler(TIM_TypeDef * timer) * Map all valid TIM IRQs to the common interrupt handler * and give it enough context to properly demux the various timers */ -void TIM1_CC_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_1_CC_irq_handler"))); -static void PIOS_TIM_1_CC_irq_handler (void) +void TIM1_CC_IRQHandler(void) __attribute__((alias("PIOS_TIM_1_CC_irq_handler"))); +static void PIOS_TIM_1_CC_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM1); + PIOS_TIM_generic_irq_handler(TIM1); } -void TIM2_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_2_irq_handler"))); -static void PIOS_TIM_2_irq_handler (void) +void TIM2_IRQHandler(void) __attribute__((alias("PIOS_TIM_2_irq_handler"))); +static void PIOS_TIM_2_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM2); + PIOS_TIM_generic_irq_handler(TIM2); } -void TIM3_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_3_irq_handler"))); -static void PIOS_TIM_3_irq_handler (void) +void TIM3_IRQHandler(void) __attribute__((alias("PIOS_TIM_3_irq_handler"))); +static void PIOS_TIM_3_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM3); + PIOS_TIM_generic_irq_handler(TIM3); } -void TIM4_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_4_irq_handler"))); -static void PIOS_TIM_4_irq_handler (void) +void TIM4_IRQHandler(void) __attribute__((alias("PIOS_TIM_4_irq_handler"))); +static void PIOS_TIM_4_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM4); + PIOS_TIM_generic_irq_handler(TIM4); } -void TIM5_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_5_irq_handler"))); -static void PIOS_TIM_5_irq_handler (void) +void TIM5_IRQHandler(void) __attribute__((alias("PIOS_TIM_5_irq_handler"))); +static void PIOS_TIM_5_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM5); + PIOS_TIM_generic_irq_handler(TIM5); } -void TIM6_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_6_irq_handler"))); -static void PIOS_TIM_6_irq_handler (void) +void TIM6_IRQHandler(void) __attribute__((alias("PIOS_TIM_6_irq_handler"))); +static void PIOS_TIM_6_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM6); + PIOS_TIM_generic_irq_handler(TIM6); } -void TIM7_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_7_irq_handler"))); -static void PIOS_TIM_7_irq_handler (void) +void TIM7_IRQHandler(void) __attribute__((alias("PIOS_TIM_7_irq_handler"))); +static void PIOS_TIM_7_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM7); + PIOS_TIM_generic_irq_handler(TIM7); } -void TIM8_UP_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_8_UP_irq_handler"))); -static void PIOS_TIM_8_UP_irq_handler (void) +void TIM8_UP_IRQHandler(void) __attribute__((alias("PIOS_TIM_8_UP_irq_handler"))); +static void PIOS_TIM_8_UP_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM8); + PIOS_TIM_generic_irq_handler(TIM8); } -void TIM8_CC_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_8_CC_irq_handler"))); -static void PIOS_TIM_8_CC_irq_handler (void) +void TIM8_CC_IRQHandler(void) __attribute__((alias("PIOS_TIM_8_CC_irq_handler"))); +static void PIOS_TIM_8_CC_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM8); + PIOS_TIM_generic_irq_handler(TIM8); } // The rest of TIM1 interrupts are overlapped -void TIM1_BRK_TIM9_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_9_CC_irq_handler"))); -static void PIOS_TIM_9_CC_irq_handler (void) +void TIM1_BRK_TIM9_IRQHandler(void) __attribute__((alias("PIOS_TIM_9_CC_irq_handler"))); +static void PIOS_TIM_9_CC_irq_handler(void) { - if (TIM_GetITStatus(TIM1, TIM_IT_Break)) - { - PIOS_TIM_generic_irq_handler(TIM1); - } - else if (TIM_GetITStatus(TIM9, PIOS_TIM_ALL_FLAGS)) - { - PIOS_TIM_generic_irq_handler (TIM9); - } + if (TIM_GetITStatus(TIM1, TIM_IT_Break)) { + PIOS_TIM_generic_irq_handler(TIM1); + } else if (TIM_GetITStatus(TIM9, PIOS_TIM_ALL_FLAGS)) { + PIOS_TIM_generic_irq_handler(TIM9); + } } -void TIM1_UP_TIM10_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_10_CC_irq_handler"))); -static void PIOS_TIM_10_CC_irq_handler (void) +void TIM1_UP_TIM10_IRQHandler(void) __attribute__((alias("PIOS_TIM_10_CC_irq_handler"))); +static void PIOS_TIM_10_CC_irq_handler(void) { - if (TIM_GetITStatus(TIM1, TIM_IT_Update)) - { - PIOS_TIM_generic_irq_handler(TIM1); - } - else if (TIM_GetITStatus(TIM10, PIOS_TIM_ALL_FLAGS)) - { - PIOS_TIM_generic_irq_handler (TIM10); - } + if (TIM_GetITStatus(TIM1, TIM_IT_Update)) { + PIOS_TIM_generic_irq_handler(TIM1); + } else if (TIM_GetITStatus(TIM10, PIOS_TIM_ALL_FLAGS)) { + PIOS_TIM_generic_irq_handler(TIM10); + } } -void TIM1_TRG_COM_TIM11_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_11_CC_irq_handler"))); -static void PIOS_TIM_11_CC_irq_handler (void) +void TIM1_TRG_COM_TIM11_IRQHandler(void) __attribute__((alias("PIOS_TIM_11_CC_irq_handler"))); +static void PIOS_TIM_11_CC_irq_handler(void) { - if(TIM_GetITStatus(TIM1, TIM_IT_COM | TIM_IT_Trigger)) - { - PIOS_TIM_generic_irq_handler (TIM1); - } - else if (TIM_GetITStatus(TIM11, PIOS_TIM_ALL_FLAGS)) - { - PIOS_TIM_generic_irq_handler (TIM11); - } + if (TIM_GetITStatus(TIM1, TIM_IT_COM | TIM_IT_Trigger)) { + PIOS_TIM_generic_irq_handler(TIM1); + } else if (TIM_GetITStatus(TIM11, PIOS_TIM_ALL_FLAGS)) { + PIOS_TIM_generic_irq_handler(TIM11); + } } -void TIM8_BRK_TIM12_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_12_irq_handler"))); -static void PIOS_TIM_12_irq_handler (void) +void TIM8_BRK_TIM12_IRQHandler(void) __attribute__((alias("PIOS_TIM_12_irq_handler"))); +static void PIOS_TIM_12_irq_handler(void) { - if(TIM_GetITStatus(TIM8, TIM_IT_Break)) - { - PIOS_TIM_generic_irq_handler (TIM8); - } - else if (TIM_GetITStatus(TIM12, PIOS_TIM_ALL_FLAGS)) - { - PIOS_TIM_generic_irq_handler (TIM12); - } + if (TIM_GetITStatus(TIM8, TIM_IT_Break)) { + PIOS_TIM_generic_irq_handler(TIM8); + } else if (TIM_GetITStatus(TIM12, PIOS_TIM_ALL_FLAGS)) { + PIOS_TIM_generic_irq_handler(TIM12); + } } -void TIM8_UP_TIM13_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM8_UP_TIM13_IRQHandler"))); -static void PIOS_TIM8_UP_TIM13_IRQHandler (void) +void TIM8_UP_TIM13_IRQHandler(void) __attribute__((alias("PIOS_TIM8_UP_TIM13_IRQHandler"))); +static void PIOS_TIM8_UP_TIM13_IRQHandler(void) { - if(TIM_GetITStatus(TIM8, TIM_IT_Update)) - { - PIOS_TIM_generic_irq_handler (TIM8); - } - else if (TIM_GetITStatus(TIM13, PIOS_TIM_ALL_FLAGS)) - { - PIOS_TIM_generic_irq_handler (TIM13); - } + if (TIM_GetITStatus(TIM8, TIM_IT_Update)) { + PIOS_TIM_generic_irq_handler(TIM8); + } else if (TIM_GetITStatus(TIM13, PIOS_TIM_ALL_FLAGS)) { + PIOS_TIM_generic_irq_handler(TIM13); + } } -void TIM8_TRG_COM_TIM14_IRQHandler (void) __attribute__ ((alias ("PIOS_TIM8_TRG_COM_TIM14_IRQHandler"))); -static void PIOS_TIM8_TRG_COM_TIM14_IRQHandler (void) +void TIM8_TRG_COM_TIM14_IRQHandler(void) __attribute__((alias("PIOS_TIM8_TRG_COM_TIM14_IRQHandler"))); +static void PIOS_TIM8_TRG_COM_TIM14_IRQHandler(void) { - if(TIM_GetITStatus(TIM8, TIM_IT_COM | TIM_IT_Trigger)) - { - PIOS_TIM_generic_irq_handler (TIM8); - } - else if (TIM_GetITStatus(TIM14, PIOS_TIM_ALL_FLAGS)) - { - PIOS_TIM_generic_irq_handler (TIM14); - } + if (TIM_GetITStatus(TIM8, TIM_IT_COM | TIM_IT_Trigger)) { + PIOS_TIM_generic_irq_handler(TIM8); + } else if (TIM_GetITStatus(TIM14, PIOS_TIM_ALL_FLAGS)) { + PIOS_TIM_generic_irq_handler(TIM14); + } } #endif /* PIOS_INCLUDE_TIM */ diff --git a/flight/pios/stm32f4xx/pios_usart.c b/flight/pios/stm32f4xx/pios_usart.c index daa4d25bb..d16e41cee 100644 --- a/flight/pios/stm32f4xx/pios_usart.c +++ b/flight/pios/stm32f4xx/pios_usart.c @@ -6,25 +6,25 @@ * @brief PIOS interface for USART port * @{ * - * @file pios_usart.c + * @file pios_usart.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief USART commands. Inits USARTs, controls USARTs & Interupt handlers. (STM32 dependent) * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -46,64 +46,66 @@ static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail); static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usart_com_driver = { - .set_baud = PIOS_USART_ChangeBaud, - .tx_start = PIOS_USART_TxStart, - .rx_start = PIOS_USART_RxStart, - .bind_tx_cb = PIOS_USART_RegisterTxCallback, - .bind_rx_cb = PIOS_USART_RegisterRxCallback, + .set_baud = PIOS_USART_ChangeBaud, + .tx_start = PIOS_USART_TxStart, + .rx_start = PIOS_USART_RxStart, + .bind_tx_cb = PIOS_USART_RegisterTxCallback, + .bind_rx_cb = PIOS_USART_RegisterRxCallback, }; enum pios_usart_dev_magic { - PIOS_USART_DEV_MAGIC = 0x4152834A, + PIOS_USART_DEV_MAGIC = 0x4152834A, }; struct pios_usart_dev { - enum pios_usart_dev_magic magic; - const struct pios_usart_cfg * cfg; + enum pios_usart_dev_magic magic; + const struct pios_usart_cfg *cfg; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; }; -static bool PIOS_USART_validate(struct pios_usart_dev * usart_dev) +static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) { - return (usart_dev->magic == PIOS_USART_DEV_MAGIC); + return usart_dev->magic == PIOS_USART_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_usart_dev * PIOS_USART_alloc(void) +static struct pios_usart_dev *PIOS_USART_alloc(void) { - struct pios_usart_dev * usart_dev; + struct pios_usart_dev *usart_dev; - usart_dev = (struct pios_usart_dev *)pvPortMalloc(sizeof(*usart_dev)); - if (!usart_dev) return(NULL); + usart_dev = (struct pios_usart_dev *)pvPortMalloc(sizeof(*usart_dev)); + if (!usart_dev) { + return NULL; + } - usart_dev->rx_in_cb = 0; - usart_dev->rx_in_context = 0; - usart_dev->tx_out_cb = 0; - usart_dev->tx_out_context = 0; - usart_dev->magic = PIOS_USART_DEV_MAGIC; - return(usart_dev); + usart_dev->rx_in_cb = 0; + usart_dev->rx_in_context = 0; + usart_dev->tx_out_cb = 0; + usart_dev->tx_out_context = 0; + usart_dev->magic = PIOS_USART_DEV_MAGIC; + return usart_dev; } #else static struct pios_usart_dev pios_usart_devs[PIOS_USART_MAX_DEVS]; static uint8_t pios_usart_num_devs; -static struct pios_usart_dev * PIOS_USART_alloc(void) +static struct pios_usart_dev *PIOS_USART_alloc(void) { - struct pios_usart_dev * usart_dev; + struct pios_usart_dev *usart_dev; - if (pios_usart_num_devs >= PIOS_USART_MAX_DEVS) { - return (NULL); - } + if (pios_usart_num_devs >= PIOS_USART_MAX_DEVS) { + return NULL; + } - usart_dev = &pios_usart_devs[pios_usart_num_devs++]; - usart_dev->magic = PIOS_USART_DEV_MAGIC; + usart_dev = &pios_usart_devs[pios_usart_num_devs++]; + usart_dev->magic = PIOS_USART_DEV_MAGIC; - return (usart_dev); + return usart_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /* Bind Interrupt Handlers * @@ -114,244 +116,252 @@ static struct pios_usart_dev * PIOS_USART_alloc(void) static void PIOS_USART_generic_irq_handler(uint32_t usart_id); static uint32_t PIOS_USART_1_id; -void USART1_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_1_irq_handler"))); -static void PIOS_USART_1_irq_handler (void) +void USART1_IRQHandler(void) __attribute__((alias("PIOS_USART_1_irq_handler"))); +static void PIOS_USART_1_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_1_id); + PIOS_USART_generic_irq_handler(PIOS_USART_1_id); } static uint32_t PIOS_USART_2_id; -void USART2_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_2_irq_handler"))); -static void PIOS_USART_2_irq_handler (void) +void USART2_IRQHandler(void) __attribute__((alias("PIOS_USART_2_irq_handler"))); +static void PIOS_USART_2_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_2_id); + PIOS_USART_generic_irq_handler(PIOS_USART_2_id); } static uint32_t PIOS_USART_3_id; -void USART3_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_3_irq_handler"))); -static void PIOS_USART_3_irq_handler (void) +void USART3_IRQHandler(void) __attribute__((alias("PIOS_USART_3_irq_handler"))); +static void PIOS_USART_3_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_3_id); + PIOS_USART_generic_irq_handler(PIOS_USART_3_id); } static uint32_t PIOS_USART_4_id; -void USART4_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_4_irq_handler"))); -static void PIOS_USART_4_irq_handler (void) +void USART4_IRQHandler(void) __attribute__((alias("PIOS_USART_4_irq_handler"))); +static void PIOS_USART_4_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_4_id); + PIOS_USART_generic_irq_handler(PIOS_USART_4_id); } static uint32_t PIOS_USART_5_id; -void USART5_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_5_irq_handler"))); -static void PIOS_USART_5_irq_handler (void) +void USART5_IRQHandler(void) __attribute__((alias("PIOS_USART_5_irq_handler"))); +static void PIOS_USART_5_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_5_id); + PIOS_USART_generic_irq_handler(PIOS_USART_5_id); } static uint32_t PIOS_USART_6_id; -void USART6_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_6_irq_handler"))); -static void PIOS_USART_6_irq_handler (void) +void USART6_IRQHandler(void) __attribute__((alias("PIOS_USART_6_irq_handler"))); +static void PIOS_USART_6_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_6_id); + PIOS_USART_generic_irq_handler(PIOS_USART_6_id); } /** -* Initialise a single USART device -*/ -int32_t PIOS_USART_Init(uint32_t * usart_id, const struct pios_usart_cfg * cfg) + * Initialise a single USART device + */ +int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) { - PIOS_DEBUG_Assert(usart_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(usart_id); + PIOS_DEBUG_Assert(cfg); - struct pios_usart_dev * usart_dev; + struct pios_usart_dev *usart_dev; - usart_dev = (struct pios_usart_dev *) PIOS_USART_alloc(); - if (!usart_dev) goto out_fail; + usart_dev = (struct pios_usart_dev *)PIOS_USART_alloc(); + if (!usart_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usart_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + usart_dev->cfg = cfg; - /* Map pins to USART function */ - /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ - if (usart_dev->cfg->remap) { - GPIO_PinAFConfig(usart_dev->cfg->rx.gpio, - __builtin_ctz(usart_dev->cfg->rx.init.GPIO_Pin), - usart_dev->cfg->remap); - GPIO_PinAFConfig(usart_dev->cfg->tx.gpio, - __builtin_ctz(usart_dev->cfg->tx.init.GPIO_Pin), - usart_dev->cfg->remap); - } + /* Map pins to USART function */ + /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ + if (usart_dev->cfg->remap) { + GPIO_PinAFConfig(usart_dev->cfg->rx.gpio, + __builtin_ctz(usart_dev->cfg->rx.init.GPIO_Pin), + usart_dev->cfg->remap); + GPIO_PinAFConfig(usart_dev->cfg->tx.gpio, + __builtin_ctz(usart_dev->cfg->tx.init.GPIO_Pin), + usart_dev->cfg->remap); + } - /* Initialize the USART Rx and Tx pins */ - GPIO_Init(usart_dev->cfg->rx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->rx.init); - GPIO_Init(usart_dev->cfg->tx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->tx.init); + /* Initialize the USART Rx and Tx pins */ + GPIO_Init(usart_dev->cfg->rx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->rx.init); + GPIO_Init(usart_dev->cfg->tx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->tx.init); - /* Configure the USART */ - USART_Init(usart_dev->cfg->regs, (USART_InitTypeDef *)&usart_dev->cfg->init); + /* Configure the USART */ + USART_Init(usart_dev->cfg->regs, (USART_InitTypeDef *)&usart_dev->cfg->init); - *usart_id = (uint32_t)usart_dev; + *usart_id = (uint32_t)usart_dev; - /* Configure USART Interrupts */ - switch ((uint32_t)usart_dev->cfg->regs) { - case (uint32_t)USART1: - PIOS_USART_1_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART2: - PIOS_USART_2_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART3: - PIOS_USART_3_id = (uint32_t)usart_dev; - break; - case (uint32_t)UART4: - PIOS_USART_4_id = (uint32_t)usart_dev; - break; - case (uint32_t)UART5: - PIOS_USART_5_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART6: - PIOS_USART_6_id = (uint32_t)usart_dev; - break; - } - NVIC_Init((NVIC_InitTypeDef *)&(usart_dev->cfg->irq.init)); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); + /* Configure USART Interrupts */ + switch ((uint32_t)usart_dev->cfg->regs) { + case (uint32_t)USART1: + PIOS_USART_1_id = (uint32_t)usart_dev; + break; + case (uint32_t)USART2: + PIOS_USART_2_id = (uint32_t)usart_dev; + break; + case (uint32_t)USART3: + PIOS_USART_3_id = (uint32_t)usart_dev; + break; + case (uint32_t)UART4: + PIOS_USART_4_id = (uint32_t)usart_dev; + break; + case (uint32_t)UART5: + PIOS_USART_5_id = (uint32_t)usart_dev; + break; + case (uint32_t)USART6: + PIOS_USART_6_id = (uint32_t)usart_dev; + break; + } + NVIC_Init((NVIC_InitTypeDef *)&(usart_dev->cfg->irq.init)); + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); - // FIXME XXX Clear / reset uart here - sends NUL char else + // FIXME XXX Clear / reset uart here - sends NUL char else - /* Enable USART */ - USART_Cmd(usart_dev->cfg->regs, ENABLE); + /* Enable USART */ + USART_Cmd(usart_dev->cfg->regs, ENABLE); - return(0); + return 0; out_fail: - return(-1); + return -1; } static void PIOS_USART_RxStart(uint32_t usart_id, __attribute__((unused)) uint16_t rx_bytes_avail) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; - - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); - - USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); } static void PIOS_USART_TxStart(uint32_t usart_id, __attribute__((unused)) uint16_t tx_bytes_avail) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; - - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); - - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); } /** -* Changes the baud rate of the USART peripheral without re-initialising. -* \param[in] usart_id USART name (GPS, TELEM, AUX) -* \param[in] baud Requested baud rate -*/ + * Changes the baud rate of the USART peripheral without re-initialising. + * \param[in] usart_id USART name (GPS, TELEM, AUX) + * \param[in] baud Requested baud rate + */ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); + bool valid = PIOS_USART_validate(usart_dev); - USART_InitTypeDef USART_InitStructure; + PIOS_Assert(valid); - /* Start with a copy of the default configuration for the peripheral */ - USART_InitStructure = usart_dev->cfg->init; + USART_InitTypeDef USART_InitStructure; - /* Adjust the baud rate */ - USART_InitStructure.USART_BaudRate = baud; + /* Start with a copy of the default configuration for the peripheral */ + USART_InitStructure = usart_dev->cfg->init; - /* Write back the new configuration */ - USART_Init(usart_dev->cfg->regs, &USART_InitStructure); + /* Adjust the baud rate */ + USART_InitStructure.USART_BaudRate = baud; + + /* Write back the new configuration */ + USART_Init(usart_dev->cfg->regs, &USART_InitStructure); } static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); - - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usart_dev->rx_in_context = context; - usart_dev->rx_in_cb = rx_in_cb; + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usart_dev->rx_in_context = context; + usart_dev->rx_in_cb = rx_in_cb; } static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); - - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usart_dev->tx_out_context = context; - usart_dev->tx_out_cb = tx_out_cb; + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usart_dev->tx_out_context = context; + usart_dev->tx_out_cb = tx_out_cb; } static void PIOS_USART_generic_irq_handler(uint32_t usart_id) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + /* Force read of dr after sr to make sure to clear error flags */ + volatile uint16_t sr = usart_dev->cfg->regs->SR; + volatile uint8_t dr = usart_dev->cfg->regs->DR; + + /* Check if RXNE flag is set */ + bool rx_need_yield = false; + if (sr & USART_SR_RXNE) { + uint8_t byte = dr; + if (usart_dev->rx_in_cb) { + (void)(usart_dev->rx_in_cb)(usart_dev->rx_in_context, &byte, 1, NULL, &rx_need_yield); + } + } + + /* Check if TXE flag is set */ + bool tx_need_yield = false; + if (sr & USART_SR_TXE) { + if (usart_dev->tx_out_cb) { + uint8_t b; + uint16_t bytes_to_send; + + bytes_to_send = (usart_dev->tx_out_cb)(usart_dev->tx_out_context, &b, 1, NULL, &tx_need_yield); + + if (bytes_to_send > 0) { + /* Send the byte we've been given */ + usart_dev->cfg->regs->DR = b; + } else { + /* No bytes to send, disable TXE interrupt */ + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); + } + } else { + /* No bytes to send, disable TXE interrupt */ + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); + } + } - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); - - /* Force read of dr after sr to make sure to clear error flags */ - volatile uint16_t sr = usart_dev->cfg->regs->SR; - volatile uint8_t dr = usart_dev->cfg->regs->DR; - - /* Check if RXNE flag is set */ - bool rx_need_yield = false; - if (sr & USART_SR_RXNE) { - uint8_t byte = dr; - if (usart_dev->rx_in_cb) { - (void) (usart_dev->rx_in_cb)(usart_dev->rx_in_context, &byte, 1, NULL, &rx_need_yield); - } - } - - /* Check if TXE flag is set */ - bool tx_need_yield = false; - if (sr & USART_SR_TXE) { - if (usart_dev->tx_out_cb) { - uint8_t b; - uint16_t bytes_to_send; - - bytes_to_send = (usart_dev->tx_out_cb)(usart_dev->tx_out_context, &b, 1, NULL, &tx_need_yield); - - if (bytes_to_send > 0) { - /* Send the byte we've been given */ - usart_dev->cfg->regs->DR = b; - } else { - /* No bytes to send, disable TXE interrupt */ - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); - } - } else { - /* No bytes to send, disable TXE interrupt */ - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); - } - } - #if defined(PIOS_INCLUDE_FREERTOS) - if (rx_need_yield || tx_need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (rx_need_yield || tx_need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } #endif /* PIOS_INCLUDE_USART */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_usb.c b/flight/pios/stm32f4xx/pios_usb.c index 6666e120d..2184c303e 100644 --- a/flight/pios/stm32f4xx/pios_usb.c +++ b/flight/pios/stm32f4xx/pios_usb.c @@ -41,51 +41,53 @@ static uint8_t transfer_possible = 0; enum pios_usb_dev_magic { - PIOS_USB_DEV_MAGIC = 0x17365904, + PIOS_USB_DEV_MAGIC = 0x17365904, }; struct pios_usb_dev { - enum pios_usb_dev_magic magic; - const struct pios_usb_cfg * cfg; + enum pios_usb_dev_magic magic; + const struct pios_usb_cfg *cfg; }; /** * @brief Validate the usb device structure * @returns true if valid device or false otherwise */ -static bool PIOS_USB_validate(struct pios_usb_dev * usb_dev) +static bool PIOS_USB_validate(struct pios_usb_dev *usb_dev) { - return (usb_dev && (usb_dev->magic == PIOS_USB_DEV_MAGIC)); + return usb_dev && (usb_dev->magic == PIOS_USB_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_usb_dev * PIOS_USB_alloc(void) +static struct pios_usb_dev *PIOS_USB_alloc(void) { - struct pios_usb_dev * usb_dev; + struct pios_usb_dev *usb_dev; - usb_dev = (struct pios_usb_dev *)pvPortMalloc(sizeof(*usb_dev)); - if (!usb_dev) return(NULL); + usb_dev = (struct pios_usb_dev *)pvPortMalloc(sizeof(*usb_dev)); + if (!usb_dev) { + return NULL; + } - usb_dev->magic = PIOS_USB_DEV_MAGIC; - return(usb_dev); + usb_dev->magic = PIOS_USB_DEV_MAGIC; + return usb_dev; } #else static struct pios_usb_dev pios_usb_devs[PIOS_USB_MAX_DEVS]; static uint8_t pios_usb_num_devs; -static struct pios_usb_dev * PIOS_USB_alloc(void) +static struct pios_usb_dev *PIOS_USB_alloc(void) { - struct pios_usb_dev * usb_dev; + struct pios_usb_dev *usb_dev; - if (pios_usb_num_devs >= PIOS_USB_MAX_DEVS) { - return (NULL); - } + if (pios_usb_num_devs >= PIOS_USB_MAX_DEVS) { + return NULL; + } - usb_dev = &pios_usb_devs[pios_usb_num_devs++]; - usb_dev->magic = PIOS_USB_DEV_MAGIC; + usb_dev = &pios_usb_devs[pios_usb_num_devs++]; + usb_dev->magic = PIOS_USB_DEV_MAGIC; - return (usb_dev); + return usb_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /** @@ -93,31 +95,33 @@ static struct pios_usb_dev * PIOS_USB_alloc(void) * \return < 0 if initialisation failed */ static uint32_t pios_usb_id; -int32_t PIOS_USB_Init(uint32_t * usb_id, const struct pios_usb_cfg * cfg) +int32_t PIOS_USB_Init(uint32_t *usb_id, const struct pios_usb_cfg *cfg) { - PIOS_Assert(usb_id); - PIOS_Assert(cfg); + PIOS_Assert(usb_id); + PIOS_Assert(cfg); - struct pios_usb_dev * usb_dev; + struct pios_usb_dev *usb_dev; - usb_dev = (struct pios_usb_dev *) PIOS_USB_alloc(); - if (!usb_dev) goto out_fail; + usb_dev = (struct pios_usb_dev *)PIOS_USB_alloc(); + if (!usb_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usb_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + usb_dev->cfg = cfg; - /* - * This is a horrible hack to make this available to - * the interrupt callbacks. This should go away ASAP. - */ - pios_usb_id = (uint32_t) usb_dev; + /* + * This is a horrible hack to make this available to + * the interrupt callbacks. This should go away ASAP. + */ + pios_usb_id = (uint32_t)usb_dev; - *usb_id = (uint32_t) usb_dev; + *usb_id = (uint32_t)usb_dev; - return 0; /* No error */ + return 0; /* No error */ out_fail: - return(-1); + return -1; } /** @@ -128,25 +132,25 @@ out_fail: */ int32_t PIOS_USB_ChangeConnectionState(bool connected) { - // In all cases: re-initialise USB HID driver - if (connected) { - transfer_possible = 1; + // In all cases: re-initialise USB HID driver + if (connected) { + transfer_possible = 1; - //TODO: Check SetEPRxValid(ENDP1); + // TODO: Check SetEPRxValid(ENDP1); #if defined(USB_LED_ON) - USB_LED_ON; // turn the USB led on + USB_LED_ON; // turn the USB led on #endif - } else { - // Cable disconnected: disable transfers - transfer_possible = 0; + } else { + // Cable disconnected: disable transfers + transfer_possible = 0; #if defined(USB_LED_OFF) - USB_LED_OFF; // turn the USB led off + USB_LED_OFF; // turn the USB led off #endif - } + } - return 0; + return 0; } /** @@ -157,14 +161,16 @@ int32_t PIOS_USB_ChangeConnectionState(bool connected) uint32_t usb_found; bool PIOS_USB_CheckAvailable(__attribute__((unused)) uint32_t id) { - struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_id; + struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_id; - if(!PIOS_USB_validate(usb_dev)) - return false; + if (!PIOS_USB_validate(usb_dev)) { + return false; + } - usb_found = ((usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin) != 0) ^ usb_dev->cfg->vsense_active_low; - return usb_found; - return usb_found != 0 && transfer_possible ? 1 : 0; + usb_found = ((usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin) != 0) ^ usb_dev->cfg->vsense_active_low; + return usb_found; + + return usb_found != 0 && transfer_possible ? 1 : 0; } /* @@ -177,89 +183,83 @@ bool PIOS_USB_CheckAvailable(__attribute__((unused)) uint32_t id) void USB_OTG_BSP_Init(__attribute__((unused)) USB_OTG_CORE_HANDLE *pdev) { - struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_id; + struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_id; - bool valid = PIOS_USB_validate(usb_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_validate(usb_dev); + + PIOS_Assert(valid); #define FORCE_DISABLE_USB_IRQ 1 #if FORCE_DISABLE_USB_IRQ - /* Make sure we disable the USB interrupt since it may be left on by bootloader */ - NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure = usb_dev->cfg->irq.init; - NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE; - NVIC_Init(&NVIC_InitStructure); + /* Make sure we disable the USB interrupt since it may be left on by bootloader */ + NVIC_InitTypeDef NVIC_InitStructure; + NVIC_InitStructure = usb_dev->cfg->irq.init; + NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE; + NVIC_Init(&NVIC_InitStructure); #endif - /* Configure USB D-/D+ (DM/DP) pins */ - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_12; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_Init(GPIOA, &GPIO_InitStructure); + /* Configure USB D-/D+ (DM/DP) pins */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_12; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_Init(GPIOA, &GPIO_InitStructure); - GPIO_PinAFConfig(GPIOA, GPIO_PinSource11, GPIO_AF_OTG1_FS); - GPIO_PinAFConfig(GPIOA, GPIO_PinSource12, GPIO_AF_OTG1_FS); + GPIO_PinAFConfig(GPIOA, GPIO_PinSource11, GPIO_AF_OTG1_FS); + GPIO_PinAFConfig(GPIOA, GPIO_PinSource12, GPIO_AF_OTG1_FS); - /* Configure VBUS sense pin */ - GPIO_Init(usb_dev->cfg->vsense.gpio, &usb_dev->cfg->vsense.init); + /* Configure VBUS sense pin */ + GPIO_Init(usb_dev->cfg->vsense.gpio, &usb_dev->cfg->vsense.init); - /* Enable USB OTG Clock */ - RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, ENABLE); + /* Enable USB OTG Clock */ + RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, ENABLE); } void USB_OTG_BSP_EnableInterrupt(__attribute__((unused)) USB_OTG_CORE_HANDLE *pdev) { - struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_id; + struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_id; - bool valid = PIOS_USB_validate(usb_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_validate(usb_dev); - NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + PIOS_Assert(valid); - NVIC_Init(&usb_dev->cfg->irq.init); + NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + + NVIC_Init(&usb_dev->cfg->irq.init); } #ifdef USE_HOST_MODE void USB_OTG_BSP_DriveVBUS(USB_OTG_CORE_HANDLE *pdev, uint8_t state) -{ - -} +{} void USB_OTG_BSP_ConfigVBUS(USB_OTG_CORE_HANDLE *pdev) +{} +#endif /* USE_HOST_MODE */ + +void USB_OTG_BSP_TimeInit(void) +{} + +void USB_OTG_BSP_uDelay(const uint32_t usec) { + uint32_t count = 0; + const uint32_t utime = (120 * usec / 7); -} -#endif /* USE_HOST_MODE */ - -void USB_OTG_BSP_TimeInit ( void ) -{ - + do { + if (++count > utime) { + return; + } + } while (1); } -void USB_OTG_BSP_uDelay (const uint32_t usec) +void USB_OTG_BSP_mDelay(const uint32_t msec) { - uint32_t count = 0; - const uint32_t utime = (120 * usec / 7); - do { - if (++count > utime) { - return ; - } - } - while (1); + USB_OTG_BSP_uDelay(msec * 1000); } -void USB_OTG_BSP_mDelay (const uint32_t msec) -{ - USB_OTG_BSP_uDelay(msec * 1000); -} - -void USB_OTG_BSP_TimerIRQ (void) -{ - -} +void USB_OTG_BSP_TimerIRQ(void) +{} #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/stm32f4xx/pios_usb_cdc.c b/flight/pios/stm32f4xx/pios_usb_cdc.c index 30151330e..dbfd6d0cf 100644 --- a/flight/pios/stm32f4xx/pios_usb_cdc.c +++ b/flight/pios/stm32f4xx/pios_usb_cdc.c @@ -42,68 +42,70 @@ static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail); static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usb_cdc_com_driver = { - .tx_start = PIOS_USB_CDC_TxStart, - .rx_start = PIOS_USB_CDC_RxStart, - .bind_tx_cb = PIOS_USB_CDC_RegisterTxCallback, - .bind_rx_cb = PIOS_USB_CDC_RegisterRxCallback, - .available = PIOS_USB_CheckAvailable, + .tx_start = PIOS_USB_CDC_TxStart, + .rx_start = PIOS_USB_CDC_RxStart, + .bind_tx_cb = PIOS_USB_CDC_RegisterTxCallback, + .bind_rx_cb = PIOS_USB_CDC_RegisterRxCallback, + .available = PIOS_USB_CheckAvailable, }; enum pios_usb_cdc_dev_magic { - PIOS_USB_CDC_DEV_MAGIC = 0xAABBCCDD, + PIOS_USB_CDC_DEV_MAGIC = 0xAABBCCDD, }; struct pios_usb_cdc_dev { - enum pios_usb_cdc_dev_magic magic; - const struct pios_usb_cdc_cfg * cfg; + enum pios_usb_cdc_dev_magic magic; + const struct pios_usb_cdc_cfg *cfg; - uint32_t lower_id; + uint32_t lower_id; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; - uint8_t rx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; - uint8_t tx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; + uint8_t rx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; + uint8_t tx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; - uint32_t rx_dropped; - uint32_t rx_oversize; + uint32_t rx_dropped; + uint32_t rx_oversize; }; -static bool PIOS_USB_CDC_validate(struct pios_usb_cdc_dev * usb_cdc_dev) +static bool PIOS_USB_CDC_validate(struct pios_usb_cdc_dev *usb_cdc_dev) { - return (usb_cdc_dev && (usb_cdc_dev->magic == PIOS_USB_CDC_DEV_MAGIC)); + return usb_cdc_dev && (usb_cdc_dev->magic == PIOS_USB_CDC_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_usb_cdc_dev * PIOS_USB_CDC_alloc(void) +static struct pios_usb_cdc_dev *PIOS_USB_CDC_alloc(void) { - struct pios_usb_cdc_dev * usb_cdc_dev; + struct pios_usb_cdc_dev *usb_cdc_dev; - usb_cdc_dev = (struct pios_usb_cdc_dev *)pvPortMalloc(sizeof(*usb_cdc_dev)); - if (!usb_cdc_dev) return(NULL); + usb_cdc_dev = (struct pios_usb_cdc_dev *)pvPortMalloc(sizeof(*usb_cdc_dev)); + if (!usb_cdc_dev) { + return NULL; + } - usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; - return(usb_cdc_dev); + usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; + return usb_cdc_dev; } #else static struct pios_usb_cdc_dev pios_usb_cdc_devs[PIOS_USB_CDC_MAX_DEVS]; static uint8_t pios_usb_cdc_num_devs; -static struct pios_usb_cdc_dev * PIOS_USB_CDC_alloc(void) +static struct pios_usb_cdc_dev *PIOS_USB_CDC_alloc(void) { - struct pios_usb_cdc_dev * usb_cdc_dev; + struct pios_usb_cdc_dev *usb_cdc_dev; - if (pios_usb_cdc_num_devs >= PIOS_USB_CDC_MAX_DEVS) { - return (NULL); - } + if (pios_usb_cdc_num_devs >= PIOS_USB_CDC_MAX_DEVS) { + return NULL; + } - usb_cdc_dev = &pios_usb_cdc_devs[pios_usb_cdc_num_devs++]; - usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; + usb_cdc_dev = &pios_usb_cdc_devs[pios_usb_cdc_num_devs++]; + usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; - return (usb_cdc_dev); + return usb_cdc_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ static void PIOS_USB_CDC_DATA_EP_IN_Callback(void); static void PIOS_USB_CDC_DATA_EP_OUT_Callback(void); @@ -112,288 +114,300 @@ static void PIOS_USB_CDC_CTRL_EP_IN_Callback(void); static uint32_t pios_usb_cdc_id; /* Need a better way to pull these in */ -extern void (*pEpInt_IN[7])(void); -extern void (*pEpInt_OUT[7])(void); +extern void(*pEpInt_IN[7]) (void); +extern void(*pEpInt_OUT[7]) (void); -int32_t PIOS_USB_CDC_Init(uint32_t * usbcdc_id, const struct pios_usb_cdc_cfg * cfg, uint32_t lower_id) +int32_t PIOS_USB_CDC_Init(uint32_t *usbcdc_id, const struct pios_usb_cdc_cfg *cfg, uint32_t lower_id) { - PIOS_Assert(usbcdc_id); - PIOS_Assert(cfg); + PIOS_Assert(usbcdc_id); + PIOS_Assert(cfg); - struct pios_usb_cdc_dev * usb_cdc_dev; + struct pios_usb_cdc_dev *usb_cdc_dev; - usb_cdc_dev = (struct pios_usb_cdc_dev *) PIOS_USB_CDC_alloc(); - if (!usb_cdc_dev) goto out_fail; + usb_cdc_dev = (struct pios_usb_cdc_dev *)PIOS_USB_CDC_alloc(); + if (!usb_cdc_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usb_cdc_dev->cfg = cfg; - usb_cdc_dev->lower_id = lower_id; + /* Bind the configuration to the device instance */ + usb_cdc_dev->cfg = cfg; + usb_cdc_dev->lower_id = lower_id; - pios_usb_cdc_id = (uint32_t) usb_cdc_dev; + pios_usb_cdc_id = (uint32_t)usb_cdc_dev; - /* Bind lower level callbacks into the USB infrastructure */ - pEpInt_OUT[cfg->ctrl_tx_ep - 1] = PIOS_USB_CDC_CTRL_EP_IN_Callback; - pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_CDC_DATA_EP_IN_Callback; - pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_CDC_DATA_EP_OUT_Callback; + /* Bind lower level callbacks into the USB infrastructure */ + pEpInt_OUT[cfg->ctrl_tx_ep - 1] = PIOS_USB_CDC_CTRL_EP_IN_Callback; + pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_CDC_DATA_EP_IN_Callback; + pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_CDC_DATA_EP_OUT_Callback; - *usbcdc_id = (uint32_t) usb_cdc_dev; + *usbcdc_id = (uint32_t)usb_cdc_dev; - return 0; + return 0; out_fail: - return -1; + return -1; } - static void PIOS_USB_CDC_RegisterRxCallback(uint32_t usbcdc_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_cdc_dev->rx_in_context = context; - usb_cdc_dev->rx_in_cb = rx_in_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->rx_in_context = context; + usb_cdc_dev->rx_in_cb = rx_in_cb; } static void PIOS_USB_CDC_RegisterTxCallback(uint32_t usbcdc_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_cdc_dev->tx_out_context = context; - usb_cdc_dev->tx_out_cb = tx_out_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->tx_out_context = context; + usb_cdc_dev->tx_out_cb = tx_out_cb; } -static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); - - if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { - return; - } - - // If endpoint was stalled and there is now space make it valid - PIOS_IRQ_Disable(); - if ((GetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep) != EP_RX_VALID) && - (rx_bytes_avail >= sizeof(usb_cdc_dev->rx_packet_buffer))) { - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); - } - PIOS_IRQ_Enable(); -} - -static void PIOS_USB_CDC_SendData(struct pios_usb_cdc_dev * usb_cdc_dev) +static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail) { - uint16_t bytes_to_tx; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - if (!usb_cdc_dev->tx_out_cb) { - return; - } + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - bool need_yield = false; - bytes_to_tx = (usb_cdc_dev->tx_out_cb)(usb_cdc_dev->tx_out_context, - usb_cdc_dev->tx_packet_buffer, - sizeof(usb_cdc_dev->tx_packet_buffer), - NULL, - &need_yield); - if (bytes_to_tx == 0) { - return; - } + PIOS_Assert(valid); - UserToPMABufferCopy(usb_cdc_dev->tx_packet_buffer, - GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), - bytes_to_tx); - SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, bytes_to_tx); - SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); + if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { + return; + } + + // If endpoint was stalled and there is now space make it valid + PIOS_IRQ_Disable(); + if ((GetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep) != EP_RX_VALID) && + (rx_bytes_avail >= sizeof(usb_cdc_dev->rx_packet_buffer))) { + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); + } + PIOS_IRQ_Enable(); +} + +static void PIOS_USB_CDC_SendData(struct pios_usb_cdc_dev *usb_cdc_dev) +{ + uint16_t bytes_to_tx; + + if (!usb_cdc_dev->tx_out_cb) { + return; + } + + bool need_yield = false; + bytes_to_tx = (usb_cdc_dev->tx_out_cb)(usb_cdc_dev->tx_out_context, + usb_cdc_dev->tx_packet_buffer, + sizeof(usb_cdc_dev->tx_packet_buffer), + NULL, + &need_yield); + if (bytes_to_tx == 0) { + return; + } + + UserToPMABufferCopy(usb_cdc_dev->tx_packet_buffer, + GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), + bytes_to_tx); + SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, bytes_to_tx); + SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); #if defined(PIOS_INCLUDE_FREERTOS) - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { - return; - } + PIOS_Assert(valid); - if (GetEPTxStatus(usb_cdc_dev->cfg->data_tx_ep) == EP_TX_VALID) { - /* Endpoint is already transmitting */ - return; - } + if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { + return; + } - PIOS_USB_CDC_SendData(usb_cdc_dev); + if (GetEPTxStatus(usb_cdc_dev->cfg->data_tx_ep) == EP_TX_VALID) { + /* Endpoint is already transmitting */ + return; + } + + PIOS_USB_CDC_SendData(usb_cdc_dev); } static void PIOS_USB_CDC_DATA_EP_IN_Callback(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_USB_CDC_SendData(usb_cdc_dev); + PIOS_Assert(valid); + + PIOS_USB_CDC_SendData(usb_cdc_dev); } static void PIOS_USB_CDC_DATA_EP_OUT_Callback(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - uint32_t DataLength; + PIOS_Assert(valid); - /* Get the number of received data on the selected Endpoint */ - DataLength = GetEPRxCount(usb_cdc_dev->cfg->data_rx_ep); - if (DataLength > sizeof(usb_cdc_dev->rx_packet_buffer)) { - usb_cdc_dev->rx_oversize++; - DataLength = sizeof(usb_cdc_dev->rx_packet_buffer); - } + uint32_t DataLength; - /* Use the memory interface function to read from the selected endpoint */ - PMAToUserBufferCopy((uint8_t *) usb_cdc_dev->rx_packet_buffer, - GetEPRxAddr(usb_cdc_dev->cfg->data_rx_ep), - DataLength); + /* Get the number of received data on the selected Endpoint */ + DataLength = GetEPRxCount(usb_cdc_dev->cfg->data_rx_ep); + if (DataLength > sizeof(usb_cdc_dev->rx_packet_buffer)) { + usb_cdc_dev->rx_oversize++; + DataLength = sizeof(usb_cdc_dev->rx_packet_buffer); + } - if (!usb_cdc_dev->rx_in_cb) { - /* No Rx call back registered, disable the receiver */ - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); - return; - } + /* Use the memory interface function to read from the selected endpoint */ + PMAToUserBufferCopy((uint8_t *)usb_cdc_dev->rx_packet_buffer, + GetEPRxAddr(usb_cdc_dev->cfg->data_rx_ep), + DataLength); - uint16_t headroom; - bool need_yield = false; - uint16_t rc; - rc = (usb_cdc_dev->rx_in_cb)(usb_cdc_dev->rx_in_context, - usb_cdc_dev->rx_packet_buffer, - DataLength, - &headroom, - &need_yield); + if (!usb_cdc_dev->rx_in_cb) { + /* No Rx call back registered, disable the receiver */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); + return; + } - if (rc < DataLength) { - /* Lost bytes on rx */ - usb_cdc_dev->rx_dropped += (DataLength - rc); - } + uint16_t headroom; + bool need_yield = false; + uint16_t rc; + rc = (usb_cdc_dev->rx_in_cb)(usb_cdc_dev->rx_in_context, + usb_cdc_dev->rx_packet_buffer, + DataLength, + &headroom, + &need_yield); - if (headroom >= sizeof(usb_cdc_dev->rx_packet_buffer)) { - /* We have room for a maximum length message */ - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); - } else { - /* Not enough room left for a message, apply backpressure */ - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); - } + if (rc < DataLength) { + /* Lost bytes on rx */ + usb_cdc_dev->rx_dropped += (DataLength - rc); + } + + if (headroom >= sizeof(usb_cdc_dev->rx_packet_buffer)) { + /* We have room for a maximum length message */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); + } else { + /* Not enough room left for a message, apply backpressure */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } RESULT PIOS_USB_CDC_SetControlLineState(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - static uint16_t control_line_state; + PIOS_Assert(valid); - uint8_t wValue0 = pInformation->USBwValue0; - uint8_t wValue1 = pInformation->USBwValue1; + static uint16_t control_line_state; - control_line_state = wValue1 << 8 | wValue0; + uint8_t wValue0 = pInformation->USBwValue0; + uint8_t wValue1 = pInformation->USBwValue1; - return USB_SUCCESS; + control_line_state = wValue1 << 8 | wValue0; + + return USB_SUCCESS; } static struct usb_cdc_line_coding line_coding = { - .dwDTERate = htousbl(57600), - .bCharFormat = USB_CDC_LINE_CODING_STOP_1, - .bParityType = USB_CDC_LINE_CODING_PARITY_NONE, - .bDataBits = 8, + .dwDTERate = htousbl(57600), + .bCharFormat = USB_CDC_LINE_CODING_STOP_1, + .bParityType = USB_CDC_LINE_CODING_PARITY_NONE, + .bDataBits = 8, }; RESULT PIOS_USB_CDC_SetLineCoding(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - return USB_SUCCESS; + PIOS_Assert(valid); + + return USB_SUCCESS; } const uint8_t *PIOS_USB_CDC_GetLineCoding(uint16_t Length) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (Length == 0) { - pInformation->Ctrl_Info.Usb_wLength = sizeof(line_coding); - return NULL; - } else { - return ((uint8_t *) &line_coding); - } + PIOS_Assert(valid); + + if (Length == 0) { + pInformation->Ctrl_Info.Usb_wLength = sizeof(line_coding); + return NULL; + } else { + return (uint8_t *)&line_coding; + } } struct usb_cdc_serial_state_report uart_state = { - .bmRequestType = 0xA1, - .bNotification = USB_CDC_NOTIFICATION_SERIAL_STATE, - .wValue = 0, - .wIndex = htousbs(1), - .wLength = htousbs(2), - .bmUartState = htousbs(0), + .bmRequestType = 0xA1, + .bNotification = USB_CDC_NOTIFICATION_SERIAL_STATE, + .wValue = 0, + .wIndex = htousbs(1), + .wLength = htousbs(2), + .bmUartState = htousbs(0), }; - + static void PIOS_USB_CDC_CTRL_EP_IN_Callback(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - /* Give back UART State Bitmap */ - /* UART State Bitmap - * 15-7: reserved - * 6: bOverRun overrun error - * 5: bParity parity error - * 4: bFraming framing error - * 3: bRingSignal RI - * 2: bBreak break reception - * 1: bTxCarrier DSR - * 0: bRxCarrier DCD - */ - uart_state.bmUartState = htousbs(0x0003); + PIOS_Assert(valid); - UserToPMABufferCopy((uint8_t *) &uart_state, - GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), - sizeof(uart_state)); - SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, PIOS_USB_BOARD_CDC_MGMT_LENGTH); - SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); + /* Give back UART State Bitmap */ + /* UART State Bitmap + * 15-7: reserved + * 6: bOverRun overrun error + * 5: bParity parity error + * 4: bFraming framing error + * 3: bRingSignal RI + * 2: bBreak break reception + * 1: bTxCarrier DSR + * 0: bRxCarrier DCD + */ + uart_state.bmUartState = htousbs(0x0003); + + UserToPMABufferCopy((uint8_t *)&uart_state, + GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), + sizeof(uart_state)); + SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, PIOS_USB_BOARD_CDC_MGMT_LENGTH); + SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); } #endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/pios/stm32f4xx/pios_usb_hid.c b/flight/pios/stm32f4xx/pios_usb_hid.c index 3f7a736b7..16ecdb940 100644 --- a/flight/pios/stm32f4xx/pios_usb_hid.c +++ b/flight/pios/stm32f4xx/pios_usb_hid.c @@ -35,7 +35,7 @@ #include "pios_usb_hid_priv.h" #include "pios_usb_board_data.h" /* PIOS_BOARD_*_DATA_LENGTH */ -#include "pios_usbhook.h" /* PIOS_USBHOOK_* */ +#include "pios_usbhook.h" /* PIOS_USBHOOK_* */ static void PIOS_USB_HID_RegisterTxCallback(uint32_t usbhid_id, pios_com_callback tx_out_cb, uint32_t context); static void PIOS_USB_HID_RegisterRxCallback(uint32_t usbhid_id, pios_com_callback rx_in_cb, uint32_t context); @@ -43,73 +43,75 @@ static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, uint16_t tx_bytes_avail); static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usb_hid_com_driver = { - .tx_start = PIOS_USB_HID_TxStart, - .rx_start = PIOS_USB_HID_RxStart, - .bind_tx_cb = PIOS_USB_HID_RegisterTxCallback, - .bind_rx_cb = PIOS_USB_HID_RegisterRxCallback, - .available = PIOS_USB_CheckAvailable, + .tx_start = PIOS_USB_HID_TxStart, + .rx_start = PIOS_USB_HID_RxStart, + .bind_tx_cb = PIOS_USB_HID_RegisterTxCallback, + .bind_rx_cb = PIOS_USB_HID_RegisterRxCallback, + .available = PIOS_USB_CheckAvailable, }; enum pios_usb_hid_dev_magic { - PIOS_USB_HID_DEV_MAGIC = 0xAA00BB00, + PIOS_USB_HID_DEV_MAGIC = 0xAA00BB00, }; struct pios_usb_hid_dev { - enum pios_usb_hid_dev_magic magic; - const struct pios_usb_hid_cfg * cfg; + enum pios_usb_hid_dev_magic magic; + const struct pios_usb_hid_cfg *cfg; - uint32_t lower_id; + uint32_t lower_id; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; - bool usb_if_enabled; + bool usb_if_enabled; - uint8_t rx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; - bool rx_active; + uint8_t rx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; + bool rx_active; - uint8_t tx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; - bool tx_active; + uint8_t tx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; + bool tx_active; - uint32_t rx_dropped; - uint32_t rx_oversize; + uint32_t rx_dropped; + uint32_t rx_oversize; }; -static bool PIOS_USB_HID_validate(struct pios_usb_hid_dev * usb_hid_dev) +static bool PIOS_USB_HID_validate(struct pios_usb_hid_dev *usb_hid_dev) { - return (usb_hid_dev && (usb_hid_dev->magic == PIOS_USB_HID_DEV_MAGIC)); + return usb_hid_dev && (usb_hid_dev->magic == PIOS_USB_HID_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_usb_hid_dev * PIOS_USB_HID_alloc(void) +static struct pios_usb_hid_dev *PIOS_USB_HID_alloc(void) { - struct pios_usb_hid_dev * usb_hid_dev; + struct pios_usb_hid_dev *usb_hid_dev; - usb_hid_dev = (struct pios_usb_hid_dev *)pvPortMalloc(sizeof(*usb_hid_dev)); - if (!usb_hid_dev) return(NULL); + usb_hid_dev = (struct pios_usb_hid_dev *)pvPortMalloc(sizeof(*usb_hid_dev)); + if (!usb_hid_dev) { + return NULL; + } - usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; - return(usb_hid_dev); + usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; + return usb_hid_dev; } #else static struct pios_usb_hid_dev pios_usb_hid_devs[PIOS_USB_HID_MAX_DEVS]; static uint8_t pios_usb_hid_num_devs; -static struct pios_usb_hid_dev * PIOS_USB_HID_alloc(void) +static struct pios_usb_hid_dev *PIOS_USB_HID_alloc(void) { - struct pios_usb_hid_dev * usb_hid_dev; + struct pios_usb_hid_dev *usb_hid_dev; - if (pios_usb_hid_num_devs >= PIOS_USB_HID_MAX_DEVS) { - return (NULL); - } + if (pios_usb_hid_num_devs >= PIOS_USB_HID_MAX_DEVS) { + return NULL; + } - usb_hid_dev = &pios_usb_hid_devs[pios_usb_hid_num_devs++]; - usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; + usb_hid_dev = &pios_usb_hid_devs[pios_usb_hid_num_devs++]; + usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; - return (usb_hid_dev); + return usb_hid_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ static void PIOS_USB_HID_IF_Init(uint32_t usb_hid_id); static void PIOS_USB_HID_IF_DeInit(uint32_t usb_hid_id); @@ -117,232 +119,238 @@ static bool PIOS_USB_HID_IF_Setup(uint32_t usb_hid_id, struct usb_setup_request static void PIOS_USB_HID_IF_CtrlDataOut(uint32_t usb_hid_id, struct usb_setup_request *req); static struct pios_usb_ifops usb_hid_ifops = { - .init = PIOS_USB_HID_IF_Init, - .deinit = PIOS_USB_HID_IF_DeInit, - .setup = PIOS_USB_HID_IF_Setup, - .ctrl_data_out = PIOS_USB_HID_IF_CtrlDataOut, + .init = PIOS_USB_HID_IF_Init, + .deinit = PIOS_USB_HID_IF_DeInit, + .setup = PIOS_USB_HID_IF_Setup, + .ctrl_data_out = PIOS_USB_HID_IF_CtrlDataOut, }; static bool PIOS_USB_HID_EP_IN_Callback(uint32_t usb_hid_id, uint8_t epnum, uint16_t len); static bool PIOS_USB_HID_EP_OUT_Callback(uint32_t usb_hid_id, uint8_t epnum, uint16_t len); -int32_t PIOS_USB_HID_Init(uint32_t * usbhid_id, const struct pios_usb_hid_cfg * cfg, uint32_t lower_id) +int32_t PIOS_USB_HID_Init(uint32_t *usbhid_id, const struct pios_usb_hid_cfg *cfg, uint32_t lower_id) { - PIOS_Assert(usbhid_id); - PIOS_Assert(cfg); + PIOS_Assert(usbhid_id); + PIOS_Assert(cfg); - struct pios_usb_hid_dev * usb_hid_dev; + struct pios_usb_hid_dev *usb_hid_dev; - usb_hid_dev = (struct pios_usb_hid_dev *) PIOS_USB_HID_alloc(); - if (!usb_hid_dev) goto out_fail; + usb_hid_dev = (struct pios_usb_hid_dev *)PIOS_USB_HID_alloc(); + if (!usb_hid_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usb_hid_dev->cfg = cfg; - usb_hid_dev->lower_id = lower_id; + /* Bind the configuration to the device instance */ + usb_hid_dev->cfg = cfg; + usb_hid_dev->lower_id = lower_id; - /* Rx and Tx are not active yet */ - usb_hid_dev->rx_active = false; - usb_hid_dev->tx_active = false; + /* Rx and Tx are not active yet */ + usb_hid_dev->rx_active = false; + usb_hid_dev->tx_active = false; - /* Register class specific interface callbacks with the USBHOOK layer */ - usb_hid_dev->usb_if_enabled = false; - PIOS_USBHOOK_RegisterIfOps(cfg->data_if, &usb_hid_ifops, (uint32_t) usb_hid_dev); + /* Register class specific interface callbacks with the USBHOOK layer */ + usb_hid_dev->usb_if_enabled = false; + PIOS_USBHOOK_RegisterIfOps(cfg->data_if, &usb_hid_ifops, (uint32_t)usb_hid_dev); - *usbhid_id = (uint32_t) usb_hid_dev; + *usbhid_id = (uint32_t)usb_hid_dev; - return 0; + return 0; out_fail: - return -1; + return -1; } static struct pios_usbhook_descriptor hid_desc; -void PIOS_USB_HID_RegisterHidDescriptor(const uint8_t * desc, uint16_t length) +void PIOS_USB_HID_RegisterHidDescriptor(const uint8_t *desc, uint16_t length) { - hid_desc.descriptor = desc; - hid_desc.length = length; + hid_desc.descriptor = desc; + hid_desc.length = length; } static struct pios_usbhook_descriptor hid_report_desc; -void PIOS_USB_HID_RegisterHidReport(const uint8_t * desc, uint16_t length) +void PIOS_USB_HID_RegisterHidReport(const uint8_t *desc, uint16_t length) { - hid_report_desc.descriptor = desc; - hid_report_desc.length = length; + hid_report_desc.descriptor = desc; + hid_report_desc.length = length; } -static bool PIOS_USB_HID_SendReport(struct pios_usb_hid_dev * usb_hid_dev) +static bool PIOS_USB_HID_SendReport(struct pios_usb_hid_dev *usb_hid_dev) { - uint16_t bytes_to_tx; + uint16_t bytes_to_tx; - if (!usb_hid_dev->tx_out_cb) { - return false; - } + if (!usb_hid_dev->tx_out_cb) { + return false; + } - bool need_yield = false; + bool need_yield = false; #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, - &usb_hid_dev->tx_packet_buffer[1], - sizeof(usb_hid_dev->tx_packet_buffer)-1, - NULL, - &need_yield); + bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, + &usb_hid_dev->tx_packet_buffer[1], + sizeof(usb_hid_dev->tx_packet_buffer) - 1, + NULL, + &need_yield); #else - bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, - &usb_hid_dev->tx_packet_buffer[2], - sizeof(usb_hid_dev->tx_packet_buffer)-2, - NULL, - &need_yield); + bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, + &usb_hid_dev->tx_packet_buffer[2], + sizeof(usb_hid_dev->tx_packet_buffer) - 2, + NULL, + &need_yield); #endif - if (bytes_to_tx == 0) { - return false; - } + if (bytes_to_tx == 0) { + return false; + } - /* - * Mark this endpoint as being tx active _before_ actually transmitting - * to make sure we don't race with the Tx completion interrupt - */ - usb_hid_dev->tx_active = true; + /* + * Mark this endpoint as being tx active _before_ actually transmitting + * to make sure we don't race with the Tx completion interrupt + */ + usb_hid_dev->tx_active = true; - /* Always set type as report ID */ - usb_hid_dev->tx_packet_buffer[0] = 1; + /* Always set type as report ID */ + usb_hid_dev->tx_packet_buffer[0] = 1; #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - PIOS_USBHOOK_EndpointTx(usb_hid_dev->cfg->data_tx_ep, - usb_hid_dev->tx_packet_buffer, - sizeof(usb_hid_dev->tx_packet_buffer)); + PIOS_USBHOOK_EndpointTx(usb_hid_dev->cfg->data_tx_ep, + usb_hid_dev->tx_packet_buffer, + sizeof(usb_hid_dev->tx_packet_buffer)); #else - usb_hid_dev->tx_packet_buffer[1] = bytes_to_tx; - PIOS_USBHOOK_EndpointTx(usb_hid_dev->cfg->data_tx_ep, - usb_hid_dev->tx_packet_buffer, - sizeof(usb_hid_dev->tx_packet_buffer)); + usb_hid_dev->tx_packet_buffer[1] = bytes_to_tx; + PIOS_USBHOOK_EndpointTx(usb_hid_dev->cfg->data_tx_ep, + usb_hid_dev->tx_packet_buffer, + sizeof(usb_hid_dev->tx_packet_buffer)); #endif #if defined(PIOS_INCLUDE_FREERTOS) - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ - return true; + return true; } -static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; +static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) +{ + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - /* Make sure this USB interface has been initialized */ - if (!usb_hid_dev->usb_if_enabled) { - return; - } + PIOS_Assert(valid); - if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { - return; - } + /* Make sure this USB interface has been initialized */ + if (!usb_hid_dev->usb_if_enabled) { + return; + } - // If endpoint was stalled and there is now space make it valid + if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { + return; + } + + // If endpoint was stalled and there is now space make it valid #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; #else - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; #endif - if (!usb_hid_dev->rx_active && (rx_bytes_avail >= max_payload_length)) { - PIOS_USBHOOK_EndpointRx(usb_hid_dev->cfg->data_rx_ep, - usb_hid_dev->rx_packet_buffer, - sizeof(usb_hid_dev->rx_packet_buffer)); - usb_hid_dev->rx_active = true; - } + if (!usb_hid_dev->rx_active && (rx_bytes_avail >= max_payload_length)) { + PIOS_USBHOOK_EndpointRx(usb_hid_dev->cfg->data_rx_ep, + usb_hid_dev->rx_packet_buffer, + sizeof(usb_hid_dev->rx_packet_buffer)); + usb_hid_dev->rx_active = true; + } } static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, __attribute__((unused)) uint16_t tx_bytes_avail) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - /* Make sure this USB interface has been initialized */ - if (!usb_hid_dev->usb_if_enabled) { - return; - } + PIOS_Assert(valid); - if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { - return; - } + /* Make sure this USB interface has been initialized */ + if (!usb_hid_dev->usb_if_enabled) { + return; + } - if (!usb_hid_dev->tx_active) { - /* Transmitter is not currently active, send a report */ - PIOS_USB_HID_SendReport(usb_hid_dev); - } + if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { + return; + } + + if (!usb_hid_dev->tx_active) { + /* Transmitter is not currently active, send a report */ + PIOS_USB_HID_SendReport(usb_hid_dev); + } } static void PIOS_USB_HID_RegisterRxCallback(uint32_t usbhid_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_hid_dev->rx_in_context = context; - usb_hid_dev->rx_in_cb = rx_in_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_hid_dev->rx_in_context = context; + usb_hid_dev->rx_in_cb = rx_in_cb; } static void PIOS_USB_HID_RegisterTxCallback(uint32_t usbhid_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_hid_dev->tx_out_context = context; - usb_hid_dev->tx_out_cb = tx_out_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_hid_dev->tx_out_context = context; + usb_hid_dev->tx_out_cb = tx_out_cb; } static void PIOS_USB_HID_IF_Init(uint32_t usb_hid_id) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; - if (!PIOS_USB_HID_validate(usb_hid_dev)) { - return; - } - - /* Register endpoint specific callbacks with the USBHOOK layer */ - PIOS_USBHOOK_RegisterEpInCallback(usb_hid_dev->cfg->data_tx_ep, - sizeof(usb_hid_dev->tx_packet_buffer), - PIOS_USB_HID_EP_IN_Callback, - (uint32_t) usb_hid_dev); - PIOS_USBHOOK_RegisterEpOutCallback(usb_hid_dev->cfg->data_rx_ep, - sizeof(usb_hid_dev->rx_packet_buffer), - PIOS_USB_HID_EP_OUT_Callback, - (uint32_t) usb_hid_dev); - usb_hid_dev->usb_if_enabled = true; + if (!PIOS_USB_HID_validate(usb_hid_dev)) { + return; + } + /* Register endpoint specific callbacks with the USBHOOK layer */ + PIOS_USBHOOK_RegisterEpInCallback(usb_hid_dev->cfg->data_tx_ep, + sizeof(usb_hid_dev->tx_packet_buffer), + PIOS_USB_HID_EP_IN_Callback, + (uint32_t)usb_hid_dev); + PIOS_USBHOOK_RegisterEpOutCallback(usb_hid_dev->cfg->data_rx_ep, + sizeof(usb_hid_dev->rx_packet_buffer), + PIOS_USB_HID_EP_OUT_Callback, + (uint32_t)usb_hid_dev); + usb_hid_dev->usb_if_enabled = true; } static void PIOS_USB_HID_IF_DeInit(uint32_t usb_hid_id) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; - if (!PIOS_USB_HID_validate(usb_hid_dev)) { - return; - } + if (!PIOS_USB_HID_validate(usb_hid_dev)) { + return; + } - /* DeRegister endpoint specific callbacks with the USBHOOK layer */ - usb_hid_dev->usb_if_enabled = false; - PIOS_USBHOOK_DeRegisterEpInCallback(usb_hid_dev->cfg->data_tx_ep); - PIOS_USBHOOK_DeRegisterEpOutCallback(usb_hid_dev->cfg->data_rx_ep); + /* DeRegister endpoint specific callbacks with the USBHOOK layer */ + usb_hid_dev->usb_if_enabled = false; + PIOS_USBHOOK_DeRegisterEpInCallback(usb_hid_dev->cfg->data_tx_ep); + PIOS_USBHOOK_DeRegisterEpOutCallback(usb_hid_dev->cfg->data_rx_ep); } static uint8_t hid_protocol; @@ -350,85 +358,88 @@ static uint8_t hid_altset; static bool PIOS_USB_HID_IF_Setup(uint32_t usb_hid_id, struct usb_setup_request *req) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; - if (!PIOS_USB_HID_validate(usb_hid_dev)) { - return false; - } + if (!PIOS_USB_HID_validate(usb_hid_dev)) { + return false; + } - /* Make sure this is a request for an interface we know about */ - uint8_t ifnum = req->wIndex & 0xFF; - if (ifnum != usb_hid_dev->cfg->data_if) { - return (false); - } + /* Make sure this is a request for an interface we know about */ + uint8_t ifnum = req->wIndex & 0xFF; + if (ifnum != usb_hid_dev->cfg->data_if) { + return false; + } - switch (req->bmRequestType & (USB_REQ_TYPE_MASK | USB_REQ_RECIPIENT_MASK)) { - case (USB_REQ_TYPE_STANDARD | USB_REQ_RECIPIENT_INTERFACE): - switch (req->bRequest) { - case USB_REQ_GET_DESCRIPTOR: - switch (req->wValue >> 8) { - case USB_DESC_TYPE_REPORT: - PIOS_USBHOOK_CtrlTx(hid_report_desc.descriptor, - MIN(hid_report_desc.length, req->wLength)); - break; - case USB_DESC_TYPE_HID: - PIOS_USBHOOK_CtrlTx(hid_desc.descriptor, - MIN(hid_desc.length, req->wLength)); - break; - default: - /* Unhandled descriptor request */ - return false; - break; - } - break; - case USB_REQ_GET_INTERFACE: - PIOS_USBHOOK_CtrlTx(&hid_altset, 1); - break; - case USB_REQ_SET_INTERFACE: - hid_altset = (uint8_t)(req->wValue); - break; - default: - /* Unhandled standard request */ - return false; - break; - } - break; - case (USB_REQ_TYPE_CLASS | USB_REQ_RECIPIENT_INTERFACE): - switch (req->bRequest) { - case USB_HID_REQ_SET_PROTOCOL: - hid_protocol = (uint8_t)(req->wValue); - break; - case USB_HID_REQ_GET_PROTOCOL: - PIOS_USBHOOK_CtrlTx(&hid_protocol, 1); - break; - case USB_HID_REQ_GET_REPORT: - { - /* Give back a dummy input report */ - uint8_t dummy_report[2] = { - [0] = req->wValue >> 8, /* Report ID */ - [1] = 0x00, - }; - PIOS_USBHOOK_CtrlTx(dummy_report, sizeof(dummy_report)); - } - break; - default: - /* Unhandled class request */ - return false; - break; - } - break; - default: - /* Unhandled request */ - return false; - } + switch (req->bmRequestType & (USB_REQ_TYPE_MASK | USB_REQ_RECIPIENT_MASK)) { + case (USB_REQ_TYPE_STANDARD | USB_REQ_RECIPIENT_INTERFACE): + switch (req->bRequest) { + case USB_REQ_GET_DESCRIPTOR: + switch (req->wValue >> 8) { + case USB_DESC_TYPE_REPORT: + PIOS_USBHOOK_CtrlTx(hid_report_desc.descriptor, + MIN(hid_report_desc.length, req->wLength)); + break; + case USB_DESC_TYPE_HID: + PIOS_USBHOOK_CtrlTx(hid_desc.descriptor, + MIN(hid_desc.length, req->wLength)); + break; + default: + /* Unhandled descriptor request */ + return false; - return true; + break; + } + break; + case USB_REQ_GET_INTERFACE: + PIOS_USBHOOK_CtrlTx(&hid_altset, 1); + break; + case USB_REQ_SET_INTERFACE: + hid_altset = (uint8_t)(req->wValue); + break; + default: + /* Unhandled standard request */ + return false; + + break; + } + break; + case (USB_REQ_TYPE_CLASS | USB_REQ_RECIPIENT_INTERFACE): + switch (req->bRequest) { + case USB_HID_REQ_SET_PROTOCOL: + hid_protocol = (uint8_t)(req->wValue); + break; + case USB_HID_REQ_GET_PROTOCOL: + PIOS_USBHOOK_CtrlTx(&hid_protocol, 1); + break; + case USB_HID_REQ_GET_REPORT: + { + /* Give back a dummy input report */ + uint8_t dummy_report[2] = { + [0] = req->wValue >> 8, /* Report ID */ + [1] = 0x00, + }; + PIOS_USBHOOK_CtrlTx(dummy_report, sizeof(dummy_report)); + } + break; + default: + /* Unhandled class request */ + return false; + + break; + } + break; + default: + /* Unhandled request */ + return false; + } + + return true; } static void PIOS_USB_HID_IF_CtrlDataOut(__attribute__((unused)) uint32_t usb_hid_id, __attribute__((unused)) struct usb_setup_request *req) { - /* HID devices don't have any OUT data stages on the control endpoint */ - PIOS_Assert(0); + /* HID devices don't have any OUT data stages on the control endpoint */ + PIOS_Assert(0); } /** @@ -437,21 +448,21 @@ static void PIOS_USB_HID_IF_CtrlDataOut(__attribute__((unused)) uint32_t usb_hid */ static bool PIOS_USB_HID_EP_IN_Callback(uint32_t usb_hid_id, __attribute__((unused)) uint8_t epnum, __attribute__((unused)) uint16_t len) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; - if (!PIOS_USB_HID_validate(usb_hid_dev)) { - return false; - } + if (!PIOS_USB_HID_validate(usb_hid_dev)) { + return false; + } - if (PIOS_USB_CheckAvailable(usb_hid_dev->lower_id) && - PIOS_USB_HID_SendReport(usb_hid_dev)) { - /* More data has been queued, leave tx_active set to true */ - return true; - } else { - /* Nothing new sent, transmitter is now inactive */ - usb_hid_dev->tx_active = false; - return false; - } + if (PIOS_USB_CheckAvailable(usb_hid_dev->lower_id) && + PIOS_USB_HID_SendReport(usb_hid_dev)) { + /* More data has been queued, leave tx_active set to true */ + return true; + } else { + /* Nothing new sent, transmitter is now inactive */ + usb_hid_dev->tx_active = false; + return false; + } } /** @@ -459,65 +470,65 @@ static bool PIOS_USB_HID_EP_IN_Callback(uint32_t usb_hid_id, __attribute__((unus */ static bool PIOS_USB_HID_EP_OUT_Callback(uint32_t usb_hid_id, __attribute__((unused)) uint8_t epnum, uint16_t len) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; - if (!PIOS_USB_HID_validate(usb_hid_dev)) { - return false; - } + if (!PIOS_USB_HID_validate(usb_hid_dev)) { + return false; + } - if (len > sizeof(usb_hid_dev->rx_packet_buffer)) { - len = sizeof(usb_hid_dev->rx_packet_buffer); - } + if (len > sizeof(usb_hid_dev->rx_packet_buffer)) { + len = sizeof(usb_hid_dev->rx_packet_buffer); + } - if (!usb_hid_dev->rx_in_cb) { - /* No Rx call back registered, disable the receiver */ - usb_hid_dev->rx_active = false; - return false; - } + if (!usb_hid_dev->rx_in_cb) { + /* No Rx call back registered, disable the receiver */ + usb_hid_dev->rx_active = false; + return false; + } - /* The first byte is report ID (not checked), the second byte is the valid data length */ - uint16_t headroom; - bool need_yield = false; + /* The first byte is report ID (not checked), the second byte is the valid data length */ + uint16_t headroom; + bool need_yield = false; #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, - &usb_hid_dev->rx_packet_buffer[1], - len-1, - &headroom, - &need_yield); + (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, + &usb_hid_dev->rx_packet_buffer[1], + len - 1, + &headroom, + &need_yield); #else - (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, - &usb_hid_dev->rx_packet_buffer[2], - usb_hid_dev->rx_packet_buffer[1], - &headroom, - &need_yield); + (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, + &usb_hid_dev->rx_packet_buffer[2], + usb_hid_dev->rx_packet_buffer[1], + &headroom, + &need_yield); #endif #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; #else - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; #endif - bool rc; - if (headroom >= max_payload_length) { - /* We have room for a maximum length message */ - PIOS_USBHOOK_EndpointRx(usb_hid_dev->cfg->data_rx_ep, - usb_hid_dev->rx_packet_buffer, - sizeof(usb_hid_dev->rx_packet_buffer)); - rc = true; - } else { - /* Not enough room left for a message, apply backpressure */ - usb_hid_dev->rx_active = false; - rc = false; - } + bool rc; + if (headroom >= max_payload_length) { + /* We have room for a maximum length message */ + PIOS_USBHOOK_EndpointRx(usb_hid_dev->cfg->data_rx_ep, + usb_hid_dev->rx_packet_buffer, + sizeof(usb_hid_dev->rx_packet_buffer)); + rc = true; + } else { + /* Not enough room left for a message, apply backpressure */ + usb_hid_dev->rx_active = false; + rc = false; + } #if defined(PIOS_INCLUDE_FREERTOS) - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ - return rc; + return rc; } #endif /* PIOS_INCLUDE_USB_HID */ diff --git a/flight/pios/stm32f4xx/pios_usbhook.c b/flight/pios/stm32f4xx/pios_usbhook.c index df722b0c0..853102655 100644 --- a/flight/pios/stm32f4xx/pios_usbhook.c +++ b/flight/pios/stm32f4xx/pios_usbhook.c @@ -32,47 +32,47 @@ #ifdef PIOS_INCLUDE_USB -#include "pios_usb.h" /* PIOS_USB_* */ +#include "pios_usb.h" /* PIOS_USB_* */ #include "pios_usbhook.h" -#include "pios_usb_defs.h" /* struct usb_* */ -#include "pios_usb_cdc_priv.h" /* PIOS_USB_CDC_* */ +#include "pios_usb_defs.h" /* struct usb_* */ +#include "pios_usb_cdc_priv.h" /* PIOS_USB_CDC_* */ #include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ /* STM32 USB Library Definitions */ -#include "usb_core.h" /* USBD_Class_cb_TypeDef */ -#include "usbd_core.h" /* USBD_Init USBD_OK*/ -#include "usbd_ioreq.h" /* USBD_CtlPrepareRx, USBD_CtlSendData */ -#include "usbd_req.h" /* USBD_CtlError */ -#include "usb_dcd_int.h" /* USBD_OTG_ISR_Handler */ +#include "usb_core.h" /* USBD_Class_cb_TypeDef */ +#include "usbd_core.h" /* USBD_Init USBD_OK*/ +#include "usbd_ioreq.h" /* USBD_CtlPrepareRx, USBD_CtlSendData */ +#include "usbd_req.h" /* USBD_CtlError */ +#include "usb_dcd_int.h" /* USBD_OTG_ISR_Handler */ /* * External API */ static struct pios_usbhook_descriptor Device_Descriptor; -void PIOS_USBHOOK_RegisterDevice(const uint8_t * desc, uint16_t length) +void PIOS_USBHOOK_RegisterDevice(const uint8_t *desc, uint16_t length) { - Device_Descriptor.descriptor = desc; - Device_Descriptor.length = length; + Device_Descriptor.descriptor = desc; + Device_Descriptor.length = length; } static struct pios_usbhook_descriptor String_Descriptor[4]; -void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t * desc, uint16_t desc_size) +void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t *desc, uint16_t desc_size) { - if (string_id < NELEMENTS(String_Descriptor)) { - String_Descriptor[string_id].descriptor = desc; - String_Descriptor[string_id].length = desc_size; - } + if (string_id < NELEMENTS(String_Descriptor)) { + String_Descriptor[string_id].descriptor = desc; + String_Descriptor[string_id].length = desc_size; + } } static struct pios_usbhook_descriptor Config_Descriptor; -void PIOS_USBHOOK_RegisterConfig(__attribute__((unused)) uint8_t config_id, const uint8_t * desc, uint16_t desc_size) +void PIOS_USBHOOK_RegisterConfig(__attribute__((unused)) uint8_t config_id, const uint8_t *desc, uint16_t desc_size) { - Config_Descriptor.descriptor = desc; - Config_Descriptor.length = desc_size; + Config_Descriptor.descriptor = desc; + Config_Descriptor.length = desc_size; } static USB_OTG_CORE_HANDLE pios_usb_otg_core_handle; @@ -82,395 +82,394 @@ static USBD_Usr_cb_TypeDef user_callbacks; void PIOS_USBHOOK_Activate(void) { - USBD_Init(&pios_usb_otg_core_handle, - USB_OTG_FS_CORE_ID, - &device_callbacks, - &class_callbacks, - &user_callbacks); + USBD_Init(&pios_usb_otg_core_handle, + USB_OTG_FS_CORE_ID, + &device_callbacks, + &class_callbacks, + &user_callbacks); } void PIOS_USBHOOK_Deactivate(void) { - DCD_DevDisconnect(&pios_usb_otg_core_handle); - USBD_DeInit(&pios_usb_otg_core_handle); - USB_OTG_StopDevice(&pios_usb_otg_core_handle); + DCD_DevDisconnect(&pios_usb_otg_core_handle); + USBD_DeInit(&pios_usb_otg_core_handle); + USB_OTG_StopDevice(&pios_usb_otg_core_handle); } void OTG_FS_IRQHandler(void) { - if(!USBD_OTG_ISR_Handler(&pios_usb_otg_core_handle)) { - /* spurious interrupt, disable IRQ */ - - } + if (!USBD_OTG_ISR_Handler(&pios_usb_otg_core_handle)) { + /* spurious interrupt, disable IRQ */ + } } struct usb_if_entry { - struct pios_usb_ifops *ifops; - uint32_t context; + struct pios_usb_ifops *ifops; + uint32_t context; }; static struct usb_if_entry usb_if_table[3]; -void PIOS_USBHOOK_RegisterIfOps(uint8_t ifnum, struct pios_usb_ifops * ifops, uint32_t context) +void PIOS_USBHOOK_RegisterIfOps(uint8_t ifnum, struct pios_usb_ifops *ifops, uint32_t context) { - PIOS_Assert(ifnum < NELEMENTS(usb_if_table)); - PIOS_Assert(ifops); + PIOS_Assert(ifnum < NELEMENTS(usb_if_table)); + PIOS_Assert(ifops); - usb_if_table[ifnum].ifops = ifops; - usb_if_table[ifnum].context = context; + usb_if_table[ifnum].ifops = ifops; + usb_if_table[ifnum].context = context; } struct usb_ep_entry { - pios_usbhook_epcb cb; - uint32_t context; - uint16_t max_len; + pios_usbhook_epcb cb; + uint32_t context; + uint16_t max_len; }; static struct usb_ep_entry usb_epin_table[6]; void PIOS_USBHOOK_RegisterEpInCallback(uint8_t epnum, uint16_t max_len, pios_usbhook_epcb cb, uint32_t context) { - PIOS_Assert(epnum < NELEMENTS(usb_epin_table)); - PIOS_Assert(cb); + PIOS_Assert(epnum < NELEMENTS(usb_epin_table)); + PIOS_Assert(cb); - usb_epin_table[epnum].cb = cb; - usb_epin_table[epnum].context = context; - usb_epin_table[epnum].max_len = max_len; + usb_epin_table[epnum].cb = cb; + usb_epin_table[epnum].context = context; + usb_epin_table[epnum].max_len = max_len; - DCD_EP_Open(&pios_usb_otg_core_handle, - epnum | 0x80, - max_len, - USB_OTG_EP_INT); - /* - * FIXME do not hardcode endpoint type - */ + DCD_EP_Open(&pios_usb_otg_core_handle, + epnum | 0x80, + max_len, + USB_OTG_EP_INT); + /* + * FIXME do not hardcode endpoint type + */ } extern void PIOS_USBHOOK_DeRegisterEpInCallback(uint8_t epnum) { - PIOS_Assert(epnum < NELEMENTS(usb_epin_table)); + PIOS_Assert(epnum < NELEMENTS(usb_epin_table)); - usb_epin_table[epnum].cb = NULL; + usb_epin_table[epnum].cb = NULL; - DCD_EP_Close(&pios_usb_otg_core_handle, epnum | 0x80); + DCD_EP_Close(&pios_usb_otg_core_handle, epnum | 0x80); } static struct usb_ep_entry usb_epout_table[6]; void PIOS_USBHOOK_RegisterEpOutCallback(uint8_t epnum, uint16_t max_len, pios_usbhook_epcb cb, uint32_t context) { - PIOS_Assert(epnum < NELEMENTS(usb_epout_table)); - PIOS_Assert(cb); + PIOS_Assert(epnum < NELEMENTS(usb_epout_table)); + PIOS_Assert(cb); - usb_epout_table[epnum].cb = cb; - usb_epout_table[epnum].context = context; - usb_epout_table[epnum].max_len = max_len; + usb_epout_table[epnum].cb = cb; + usb_epout_table[epnum].context = context; + usb_epout_table[epnum].max_len = max_len; - DCD_EP_Open(&pios_usb_otg_core_handle, - epnum, - max_len, - USB_OTG_EP_INT); - /* - * FIXME do not hardcode endpoint type - */ + DCD_EP_Open(&pios_usb_otg_core_handle, + epnum, + max_len, + USB_OTG_EP_INT); + /* + * FIXME do not hardcode endpoint type + */ } extern void PIOS_USBHOOK_DeRegisterEpOutCallback(uint8_t epnum) { - PIOS_Assert(epnum < NELEMENTS(usb_epout_table)); + PIOS_Assert(epnum < NELEMENTS(usb_epout_table)); - usb_epout_table[epnum].cb = NULL; + usb_epout_table[epnum].cb = NULL; - DCD_EP_Close(&pios_usb_otg_core_handle, epnum); + DCD_EP_Close(&pios_usb_otg_core_handle, epnum); } void PIOS_USBHOOK_CtrlTx(const uint8_t *buf, uint16_t len) { - USBD_CtlSendData(&pios_usb_otg_core_handle, buf, len); + USBD_CtlSendData(&pios_usb_otg_core_handle, buf, len); } void PIOS_USBHOOK_CtrlRx(uint8_t *buf, uint16_t len) { - USBD_CtlPrepareRx(&pios_usb_otg_core_handle, buf, len); + USBD_CtlPrepareRx(&pios_usb_otg_core_handle, buf, len); } void PIOS_USBHOOK_EndpointTx(uint8_t epnum, const uint8_t *buf, uint16_t len) { - if (pios_usb_otg_core_handle.dev.device_status == USB_OTG_CONFIGURED) { - DCD_EP_Tx(&pios_usb_otg_core_handle, epnum, buf, len); - } + if (pios_usb_otg_core_handle.dev.device_status == USB_OTG_CONFIGURED) { + DCD_EP_Tx(&pios_usb_otg_core_handle, epnum, buf, len); + } } void PIOS_USBHOOK_EndpointRx(uint8_t epnum, uint8_t *buf, uint16_t len) { - DCD_EP_PrepareRx(&pios_usb_otg_core_handle, epnum, buf, len); + DCD_EP_PrepareRx(&pios_usb_otg_core_handle, epnum, buf, len); } /* * Device level hooks into STM USB library */ -static const uint8_t * PIOS_USBHOOK_DEV_GetDeviceDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_DEV_GetDeviceDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { - *length = Device_Descriptor.length; - return Device_Descriptor.descriptor; + *length = Device_Descriptor.length; + return Device_Descriptor.descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetLangIDStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_DEV_GetLangIDStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { - *length = String_Descriptor[USB_STRING_DESC_LANG].length; - return String_Descriptor[USB_STRING_DESC_LANG].descriptor; + *length = String_Descriptor[USB_STRING_DESC_LANG].length; + return String_Descriptor[USB_STRING_DESC_LANG].descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetManufacturerStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_DEV_GetManufacturerStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { - *length = String_Descriptor[USB_STRING_DESC_VENDOR].length; - return String_Descriptor[USB_STRING_DESC_VENDOR].descriptor; + *length = String_Descriptor[USB_STRING_DESC_VENDOR].length; + return String_Descriptor[USB_STRING_DESC_VENDOR].descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetProductStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_DEV_GetProductStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { - *length = String_Descriptor[USB_STRING_DESC_PRODUCT].length; - return String_Descriptor[USB_STRING_DESC_PRODUCT].descriptor; + *length = String_Descriptor[USB_STRING_DESC_PRODUCT].length; + return String_Descriptor[USB_STRING_DESC_PRODUCT].descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetSerialStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_DEV_GetSerialStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { - *length = String_Descriptor[USB_STRING_DESC_SERIAL].length; - return String_Descriptor[USB_STRING_DESC_SERIAL].descriptor; + *length = String_Descriptor[USB_STRING_DESC_SERIAL].length; + return String_Descriptor[USB_STRING_DESC_SERIAL].descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetConfigurationStrDescriptor(__attribute__((unused)) uint8_t speed, __attribute__((unused)) uint16_t *length) +static const uint8_t *PIOS_USBHOOK_DEV_GetConfigurationStrDescriptor(__attribute__((unused)) uint8_t speed, __attribute__((unused)) uint16_t *length) { - return NULL; + return NULL; } -static const uint8_t * PIOS_USBHOOK_DEV_GetInterfaceStrDescriptor(__attribute__((unused)) uint8_t speed, __attribute__((unused)) uint16_t *length) +static const uint8_t *PIOS_USBHOOK_DEV_GetInterfaceStrDescriptor(__attribute__((unused)) uint8_t speed, __attribute__((unused)) uint16_t *length) { - return NULL; + return NULL; } static USBD_DEVICE device_callbacks = { - .GetDeviceDescriptor = PIOS_USBHOOK_DEV_GetDeviceDescriptor, - .GetLangIDStrDescriptor = PIOS_USBHOOK_DEV_GetLangIDStrDescriptor, - .GetManufacturerStrDescriptor = PIOS_USBHOOK_DEV_GetManufacturerStrDescriptor, - .GetProductStrDescriptor = PIOS_USBHOOK_DEV_GetProductStrDescriptor, - .GetSerialStrDescriptor = PIOS_USBHOOK_DEV_GetSerialStrDescriptor, - .GetConfigurationStrDescriptor = PIOS_USBHOOK_DEV_GetConfigurationStrDescriptor, - .GetInterfaceStrDescriptor = PIOS_USBHOOK_DEV_GetInterfaceStrDescriptor, + .GetDeviceDescriptor = PIOS_USBHOOK_DEV_GetDeviceDescriptor, + .GetLangIDStrDescriptor = PIOS_USBHOOK_DEV_GetLangIDStrDescriptor, + .GetManufacturerStrDescriptor = PIOS_USBHOOK_DEV_GetManufacturerStrDescriptor, + .GetProductStrDescriptor = PIOS_USBHOOK_DEV_GetProductStrDescriptor, + .GetSerialStrDescriptor = PIOS_USBHOOK_DEV_GetSerialStrDescriptor, + .GetConfigurationStrDescriptor = PIOS_USBHOOK_DEV_GetConfigurationStrDescriptor, + .GetInterfaceStrDescriptor = PIOS_USBHOOK_DEV_GetInterfaceStrDescriptor, }; static void PIOS_USBHOOK_USR_Init(void) { - PIOS_USB_ChangeConnectionState(false); + PIOS_USB_ChangeConnectionState(false); #if 1 - /* Force a physical disconnect/reconnect */ - DCD_DevDisconnect(&pios_usb_otg_core_handle); - DCD_DevConnect(&pios_usb_otg_core_handle); + /* Force a physical disconnect/reconnect */ + DCD_DevDisconnect(&pios_usb_otg_core_handle); + DCD_DevConnect(&pios_usb_otg_core_handle); #endif } static void PIOS_USBHOOK_USR_DeviceReset(__attribute__((unused)) uint8_t speed) { - PIOS_USB_ChangeConnectionState(false); + PIOS_USB_ChangeConnectionState(false); } static void PIOS_USBHOOK_USR_DeviceConfigured(void) { - PIOS_USB_ChangeConnectionState(true); + PIOS_USB_ChangeConnectionState(true); } static void PIOS_USBHOOK_USR_DeviceSuspended(void) { - /* Unhandled */ + /* Unhandled */ } static void PIOS_USBHOOK_USR_DeviceResumed(void) { - /* Unhandled */ + /* Unhandled */ } static void PIOS_USBHOOK_USR_DeviceConnected(void) { - /* NOP */ + /* NOP */ } static void PIOS_USBHOOK_USR_DeviceDisconnected(void) { - PIOS_USB_ChangeConnectionState(false); + PIOS_USB_ChangeConnectionState(false); } static USBD_Usr_cb_TypeDef user_callbacks = { - .Init = PIOS_USBHOOK_USR_Init, - .DeviceReset = PIOS_USBHOOK_USR_DeviceReset, - .DeviceConfigured = PIOS_USBHOOK_USR_DeviceConfigured, - .DeviceSuspended = PIOS_USBHOOK_USR_DeviceSuspended, - .DeviceResumed = PIOS_USBHOOK_USR_DeviceResumed, - .DeviceConnected = PIOS_USBHOOK_USR_DeviceConnected, - .DeviceDisconnected = PIOS_USBHOOK_USR_DeviceDisconnected, + .Init = PIOS_USBHOOK_USR_Init, + .DeviceReset = PIOS_USBHOOK_USR_DeviceReset, + .DeviceConfigured = PIOS_USBHOOK_USR_DeviceConfigured, + .DeviceSuspended = PIOS_USBHOOK_USR_DeviceSuspended, + .DeviceResumed = PIOS_USBHOOK_USR_DeviceResumed, + .DeviceConnected = PIOS_USBHOOK_USR_DeviceConnected, + .DeviceDisconnected = PIOS_USBHOOK_USR_DeviceDisconnected, }; static uint8_t PIOS_USBHOOK_CLASS_Init(__attribute__((unused)) void *pdev, __attribute__((unused)) uint8_t cfgidx) { - /* Call all of the registered init callbacks */ - for (uint8_t i = 0; i < NELEMENTS(usb_if_table); i++) { - struct usb_if_entry * usb_if = &(usb_if_table[i]); - if (usb_if->ifops && usb_if->ifops->init) { - usb_if->ifops->init(usb_if->context); + /* Call all of the registered init callbacks */ + for (uint8_t i = 0; i < NELEMENTS(usb_if_table); i++) { + struct usb_if_entry *usb_if = &(usb_if_table[i]); + if (usb_if->ifops && usb_if->ifops->init) { + usb_if->ifops->init(usb_if->context); + } } - } - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_DeInit(__attribute__((unused)) void *pdev, __attribute__((unused)) uint8_t cfgidx) { - /* Call all of the registered deinit callbacks */ - for (uint8_t i = 0; i < NELEMENTS(usb_if_table); i++) { - struct usb_if_entry * usb_if = &(usb_if_table[i]); - if (usb_if->ifops && usb_if->ifops->deinit) { - usb_if->ifops->deinit(usb_if->context); + /* Call all of the registered deinit callbacks */ + for (uint8_t i = 0; i < NELEMENTS(usb_if_table); i++) { + struct usb_if_entry *usb_if = &(usb_if_table[i]); + if (usb_if->ifops && usb_if->ifops->deinit) { + usb_if->ifops->deinit(usb_if->context); + } } - } - return USBD_OK; + return USBD_OK; } static struct usb_setup_request usb_ep0_active_req; static uint8_t PIOS_USBHOOK_CLASS_Setup(__attribute__((unused)) void *pdev, USB_SETUP_REQ *req) { - switch (req->bmRequest & (USB_REQ_TYPE_MASK | USB_REQ_RECIPIENT_MASK)) { - case (USB_REQ_TYPE_STANDARD | USB_REQ_RECIPIENT_INTERFACE): - case (USB_REQ_TYPE_CLASS | USB_REQ_RECIPIENT_INTERFACE): - { - uint8_t ifnum = LOBYTE(req->wIndex); - if ((ifnum < NELEMENTS(usb_if_table)) && - (usb_if_table[ifnum].ifops && usb_if_table[ifnum].ifops->setup)) { - usb_if_table[ifnum].ifops->setup(usb_if_table[ifnum].context, - (struct usb_setup_request *)req); - if (req->bmRequest & 0x80 && req->wLength > 0) { - /* Request is a host-to-device data setup packet, keep track of the request details for the EP0_RxReady call */ - usb_ep0_active_req.bmRequestType = req->bmRequest; - usb_ep0_active_req.bRequest = req->bRequest; - usb_ep0_active_req.wValue = req->wValue; - usb_ep0_active_req.wIndex = req->wIndex; - usb_ep0_active_req.wLength = req->wLength; - } - } else { - /* No Setup handler or Setup handler failed */ - USBD_CtlError (&pios_usb_otg_core_handle, req); - } - break; - } - default: - /* Unhandled Setup */ - USBD_CtlError (&pios_usb_otg_core_handle, req); - break; - } + switch (req->bmRequest & (USB_REQ_TYPE_MASK | USB_REQ_RECIPIENT_MASK)) { + case (USB_REQ_TYPE_STANDARD | USB_REQ_RECIPIENT_INTERFACE): + case (USB_REQ_TYPE_CLASS | USB_REQ_RECIPIENT_INTERFACE): + { + uint8_t ifnum = LOBYTE(req->wIndex); + if ((ifnum < NELEMENTS(usb_if_table)) && + (usb_if_table[ifnum].ifops && usb_if_table[ifnum].ifops->setup)) { + usb_if_table[ifnum].ifops->setup(usb_if_table[ifnum].context, + (struct usb_setup_request *)req); + if (req->bmRequest & 0x80 && req->wLength > 0) { + /* Request is a host-to-device data setup packet, keep track of the request details for the EP0_RxReady call */ + usb_ep0_active_req.bmRequestType = req->bmRequest; + usb_ep0_active_req.bRequest = req->bRequest; + usb_ep0_active_req.wValue = req->wValue; + usb_ep0_active_req.wIndex = req->wIndex; + usb_ep0_active_req.wLength = req->wLength; + } + } else { + /* No Setup handler or Setup handler failed */ + USBD_CtlError(&pios_usb_otg_core_handle, req); + } + break; + } + default: + /* Unhandled Setup */ + USBD_CtlError(&pios_usb_otg_core_handle, req); + break; + } - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_EP0_TxSent(__attribute__((unused)) void *pdev) { - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_EP0_RxReady(__attribute__((unused)) void *pdev) { - uint8_t ifnum = LOBYTE(usb_ep0_active_req.wIndex); + uint8_t ifnum = LOBYTE(usb_ep0_active_req.wIndex); - if ((ifnum < NELEMENTS(usb_if_table)) && - (usb_if_table[ifnum].ifops && usb_if_table[ifnum].ifops->ctrl_data_out)) { - usb_if_table[ifnum].ifops->ctrl_data_out(usb_if_table[ifnum].context, - &usb_ep0_active_req); - } + if ((ifnum < NELEMENTS(usb_if_table)) && + (usb_if_table[ifnum].ifops && usb_if_table[ifnum].ifops->ctrl_data_out)) { + usb_if_table[ifnum].ifops->ctrl_data_out(usb_if_table[ifnum].context, + &usb_ep0_active_req); + } - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_DataIn(void *pdev, uint8_t epnum) { - /* Make sure the previous transfer has completed before starting a new one */ - DCD_EP_Flush(pdev, epnum); /* NOT SURE IF THIS IS REQUIRED */ + /* Make sure the previous transfer has completed before starting a new one */ + DCD_EP_Flush(pdev, epnum); /* NOT SURE IF THIS IS REQUIRED */ - /* Remove the direction bit so we can use this as an index */ - uint8_t epnum_idx = epnum & 0x7F; + /* Remove the direction bit so we can use this as an index */ + uint8_t epnum_idx = epnum & 0x7F; - if ((epnum_idx < NELEMENTS(usb_epin_table)) && usb_epin_table[epnum_idx].cb) { - struct usb_ep_entry *ep = &(usb_epin_table[epnum_idx]); - if (!ep->cb(ep->context, epnum_idx, ep->max_len)) { - /* NOTE: use real endpoint number including direction bit */ - DCD_SetEPStatus(pdev, epnum, USB_OTG_EP_TX_NAK); - } - } + if ((epnum_idx < NELEMENTS(usb_epin_table)) && usb_epin_table[epnum_idx].cb) { + struct usb_ep_entry *ep = &(usb_epin_table[epnum_idx]); + if (!ep->cb(ep->context, epnum_idx, ep->max_len)) { + /* NOTE: use real endpoint number including direction bit */ + DCD_SetEPStatus(pdev, epnum, USB_OTG_EP_TX_NAK); + } + } - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_DataOut(void *pdev, uint8_t epnum) { - /* Remove the direction bit so we can use this as an index */ - uint8_t epnum_idx = epnum & 0x7F; + /* Remove the direction bit so we can use this as an index */ + uint8_t epnum_idx = epnum & 0x7F; - if ((epnum_idx < NELEMENTS(usb_epout_table)) && usb_epout_table[epnum_idx].cb) { - struct usb_ep_entry *ep = &(usb_epout_table[epnum_idx]); - if (!ep->cb(ep->context, epnum_idx, ep->max_len)) { - /* NOTE: use real endpoint number including direction bit */ - DCD_SetEPStatus(pdev, epnum, USB_OTG_EP_RX_NAK); - } - } + if ((epnum_idx < NELEMENTS(usb_epout_table)) && usb_epout_table[epnum_idx].cb) { + struct usb_ep_entry *ep = &(usb_epout_table[epnum_idx]); + if (!ep->cb(ep->context, epnum_idx, ep->max_len)) { + /* NOTE: use real endpoint number including direction bit */ + DCD_SetEPStatus(pdev, epnum, USB_OTG_EP_RX_NAK); + } + } - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_SOF(__attribute__((unused)) void *pdev) { - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_IsoINIncomplete(__attribute__((unused)) void *pdev) { - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_IsoOUTIncomplete(__attribute__((unused)) void *pdev) { - return USBD_OK; + return USBD_OK; } -static const uint8_t * PIOS_USBHOOK_CLASS_GetConfigDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_CLASS_GetConfigDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { - *length = Config_Descriptor.length; - return Config_Descriptor.descriptor; + *length = Config_Descriptor.length; + return Config_Descriptor.descriptor; } #ifdef USB_OTG_HS_CORE -static const uint8_t * PIOS_USBHOOK_CLASS_GetOtherConfigDescriptor(uint8_t speed, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_CLASS_GetOtherConfigDescriptor(uint8_t speed, uint16_t *length) { - return PIOS_USBHOOK_CLASS_GetConfigDescriptor(speed, length); + return PIOS_USBHOOK_CLASS_GetConfigDescriptor(speed, length); } -#endif /* USB_OTG_HS_CORE */ +#endif /* USB_OTG_HS_CORE */ #ifdef USB_SUPPORT_USER_STRING_DESC -static const uint8_t * PIOS_USBHOOK_CLASS_GetUsrStrDescriptor(uint8_t speed, uint8_t index, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_CLASS_GetUsrStrDescriptor(uint8_t speed, uint8_t index, uint16_t *length) { - return NULL; + return NULL; } -#endif /* USB_SUPPORT_USER_STRING_DESC */ +#endif /* USB_SUPPORT_USER_STRING_DESC */ static USBD_Class_cb_TypeDef class_callbacks = { - .Init = PIOS_USBHOOK_CLASS_Init, - .DeInit = PIOS_USBHOOK_CLASS_DeInit, - .Setup = PIOS_USBHOOK_CLASS_Setup, - .EP0_TxSent = PIOS_USBHOOK_CLASS_EP0_TxSent, - .EP0_RxReady = PIOS_USBHOOK_CLASS_EP0_RxReady, - .DataIn = PIOS_USBHOOK_CLASS_DataIn, - .DataOut = PIOS_USBHOOK_CLASS_DataOut, - .SOF = PIOS_USBHOOK_CLASS_SOF, - .IsoINIncomplete = PIOS_USBHOOK_CLASS_IsoINIncomplete, - .IsoOUTIncomplete = PIOS_USBHOOK_CLASS_IsoOUTIncomplete, - .GetConfigDescriptor = PIOS_USBHOOK_CLASS_GetConfigDescriptor, + .Init = PIOS_USBHOOK_CLASS_Init, + .DeInit = PIOS_USBHOOK_CLASS_DeInit, + .Setup = PIOS_USBHOOK_CLASS_Setup, + .EP0_TxSent = PIOS_USBHOOK_CLASS_EP0_TxSent, + .EP0_RxReady = PIOS_USBHOOK_CLASS_EP0_RxReady, + .DataIn = PIOS_USBHOOK_CLASS_DataIn, + .DataOut = PIOS_USBHOOK_CLASS_DataOut, + .SOF = PIOS_USBHOOK_CLASS_SOF, + .IsoINIncomplete = PIOS_USBHOOK_CLASS_IsoINIncomplete, + .IsoOUTIncomplete = PIOS_USBHOOK_CLASS_IsoOUTIncomplete, + .GetConfigDescriptor = PIOS_USBHOOK_CLASS_GetConfigDescriptor, #ifdef USB_OTG_HS_CORE - .GetOtherConfigDescriptor = PIOS_USBHOOK_CLASS_GetOtherConfigDescriptor, -#endif /* USB_OTG_HS_CORE */ + .GetOtherConfigDescriptor = PIOS_USBHOOK_CLASS_GetOtherConfigDescriptor, +#endif /* USB_OTG_HS_CORE */ #ifdef USB_SUPPORT_USER_STRING_DESC - .GetUsrStrDescriptor = PIOS_USBHOOK_CLASS_GetUsrStrDescriptor, -#endif /* USB_SUPPORT_USER_STRING_DESC */ + .GetUsrStrDescriptor = PIOS_USBHOOK_CLASS_GetUsrStrDescriptor, +#endif /* USB_SUPPORT_USER_STRING_DESC */ }; #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/stm32f4xx/pios_wdg.c b/flight/pios/stm32f4xx/pios_wdg.c index f1a5e5d10..ab34a03ed 100644 --- a/flight/pios/stm32f4xx/pios_wdg.c +++ b/flight/pios/stm32f4xx/pios_wdg.c @@ -8,12 +8,12 @@ * * @file pios_spi.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Hardware Abstraction Layer for SPI ports of STM32 * @see The GNU Public License (GPL) Version 3 * @notes * - * The PIOS Watchdog provides a HAL to initialize a watchdog + * The PIOS Watchdog provides a HAL to initialize a watchdog * *****************************************************************************/ /* @@ -45,18 +45,18 @@ #include "stm32f4xx_rtc.h" static struct wdg_configuration { - uint16_t used_flags; - uint16_t bootup_flags; + uint16_t used_flags; + uint16_t bootup_flags; } wdg_configuration; -/** +/** * @brief Initialize the watchdog timer for a specified timeout * - * It is important to note that this function returns the achieved timeout + * It is important to note that this function returns the achieved timeout * for this hardware. For hardware independence this should be checked when - * scheduling updates. Other hardware dependent details may need to be - * considered such as a window time which sets a minimum update time, - * and this function should return a recommended delay for clearing. + * scheduling updates. Other hardware dependent details may need to be + * considered such as a window time which sets a minimum update time, + * and this function should return a recommended delay for clearing. * * For the STM32 nominal clock rate is 32 khz, but for the maximum clock rate of * 60 khz and a prescaler of 4 yields a clock rate of 15 khz. The delay that is @@ -67,106 +67,108 @@ static struct wdg_configuration { */ uint16_t PIOS_WDG_Init() { - uint16_t delay = ((uint32_t) PIOS_WATCHDOG_TIMEOUT * 60) / 16; - if (delay > 0x0fff) - delay = 0x0fff; -#if defined(PIOS_INCLUDE_WDG) - DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); // make the watchdog stop counting in debug mode - IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); - IWDG_SetPrescaler(IWDG_Prescaler_16); - IWDG_SetReload(delay); - IWDG_ReloadCounter(); - IWDG_Enable(); + uint16_t delay = ((uint32_t)PIOS_WATCHDOG_TIMEOUT * 60) / 16; - // watchdog flags now stored in backup registers - PWR_BackupAccessCmd(ENABLE); - - wdg_configuration.bootup_flags = RTC_ReadBackupRegister(PIOS_WDG_REGISTER); + if (delay > 0x0fff) { + delay = 0x0fff; + } +#if defined(PIOS_INCLUDE_WDG) + DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); // make the watchdog stop counting in debug mode + IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); + IWDG_SetPrescaler(IWDG_Prescaler_16); + IWDG_SetReload(delay); + IWDG_ReloadCounter(); + IWDG_Enable(); + + // watchdog flags now stored in backup registers + PWR_BackupAccessCmd(ENABLE); + + wdg_configuration.bootup_flags = RTC_ReadBackupRegister(PIOS_WDG_REGISTER); #endif - return delay; + return delay; } /** - * @brief Register a module against the watchdog - * + * @brief Register a module against the watchdog + * * There are two ways to use PIOS WDG: this is for when - * multiple modules must be monitored. In this case they - * must first register against the watchdog system and + * multiple modules must be monitored. In this case they + * must first register against the watchdog system and * only when all of the modules have been updated with the - * watchdog be cleared. Each module must have its own - * bit in the 16 bit + * watchdog be cleared. Each module must have its own + * bit in the 16 bit * * @param[in] flag the bit this module wants to use * @returns True if that bit is unregistered */ -bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) +bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) { - // flag are being registered so we are in module initialization phase - // clear the WDG to prevent timeout while initializing modules. (OP-815) - PIOS_WDG_Clear(); + // flag are being registered so we are in module initialization phase + // clear the WDG to prevent timeout while initializing modules. (OP-815) + PIOS_WDG_Clear(); - /* Fail if flag already registered */ - if(wdg_configuration.used_flags & flag_requested) - return false; - - // FIXME: Protect with semaphore - wdg_configuration.used_flags |= flag_requested; - - return true; + /* Fail if flag already registered */ + if (wdg_configuration.used_flags & flag_requested) { + return false; + } + + // FIXME: Protect with semaphore + wdg_configuration.used_flags |= flag_requested; + + return true; } /** * @brief Function called by modules to indicate they are still running * - * This function will set this flag in the active flags register (which is + * This function will set this flag in the active flags register (which is * a backup regsiter) and if all the registered flags are set will clear * the watchdog and set only this flag in the backup register * * @param[in] flag the flag to set * @return true if the watchdog cleared, false if flags are pending */ -bool PIOS_WDG_UpdateFlag(uint16_t flag) -{ - // we can probably avoid using a semaphore here which will be good for - // efficiency and not blocking critical tasks. race condition could - // overwrite their flag update, but unlikely to block _all_ of them - // for the timeout window - uint16_t cur_flags = RTC_ReadBackupRegister(PIOS_WDG_REGISTER); - - if((cur_flags | flag) == wdg_configuration.used_flags) { - PIOS_WDG_Clear(); - RTC_WriteBackupRegister(PIOS_WDG_REGISTER, flag); - return true; - } else { - RTC_WriteBackupRegister(PIOS_WDG_REGISTER, cur_flags | flag); - return false; - } - +bool PIOS_WDG_UpdateFlag(uint16_t flag) +{ + // we can probably avoid using a semaphore here which will be good for + // efficiency and not blocking critical tasks. race condition could + // overwrite their flag update, but unlikely to block _all_ of them + // for the timeout window + uint16_t cur_flags = RTC_ReadBackupRegister(PIOS_WDG_REGISTER); + + if ((cur_flags | flag) == wdg_configuration.used_flags) { + PIOS_WDG_Clear(); + RTC_WriteBackupRegister(PIOS_WDG_REGISTER, flag); + return true; + } else { + RTC_WriteBackupRegister(PIOS_WDG_REGISTER, cur_flags | flag); + return false; + } } -/** +/** * @brief Returns the flags that were set at bootup - * - * This is used for diagnostics, if only one flag not set this + * + * This is used for diagnostics, if only one flag not set this * was likely the module that wasn't running before reset - * + * * @return The active flags register from bootup */ uint16_t PIOS_WDG_GetBootupFlags() { - return wdg_configuration.bootup_flags; + return wdg_configuration.bootup_flags; } -/** +/** * @brief Returns the currently active flags - * + * * For external monitoring - * + * * @return The active flags register */ uint16_t PIOS_WDG_GetActiveFlags() { - return RTC_ReadBackupRegister(PIOS_WDG_REGISTER); + return RTC_ReadBackupRegister(PIOS_WDG_REGISTER); } /** @@ -177,7 +179,7 @@ uint16_t PIOS_WDG_GetActiveFlags() void PIOS_WDG_Clear(void) { #if defined(PIOS_INCLUDE_WDG) - IWDG_ReloadCounter(); + IWDG_ReloadCounter(); #endif } diff --git a/flight/pios/stm32f4xx/startup.c b/flight/pios/stm32f4xx/startup.c index 933481714..ab17d61c9 100644 --- a/flight/pios/stm32f4xx/startup.c +++ b/flight/pios/stm32f4xx/startup.c @@ -30,83 +30,83 @@ #include /* prototype for main() that tells us not to worry about it possibly returning */ -extern int main(void) __attribute__((noreturn)); +extern int main(void) __attribute__((noreturn)); /* prototype our _main() to avoid prolog/epilog insertion and other related junk */ -void _main(void) __attribute__((noreturn, naked, no_instrument_function)); +void _main(void) __attribute__((noreturn, naked, no_instrument_function)); /** default handler for CPU exceptions */ -static void default_cpu_handler(void) __attribute__((noreturn, naked, no_instrument_function)); +static void default_cpu_handler(void) __attribute__((noreturn, naked, no_instrument_function)); /** BSS symbols XXX should have a header that defines all of these */ -extern char _sbss, _ebss; +extern char _sbss, _ebss; /** DATA symbols XXX should have a header that defines all of these */ -extern char _sidata, _sdata, _edata, _sfast, _efast; +extern char _sidata, _sdata, _edata, _sfast, _efast; /** The bootstrap/IRQ stack XXX should define size somewhere else */ -char irq_stack[1024] __attribute__((section(".irqstack"))); +char irq_stack[1024] __attribute__((section(".irqstack"))); /** exception handler */ -typedef void (vector)(void); +typedef void (vector)(void); /** CortexM3 CPU vectors */ struct cm3_vectors { - void *initial_stack; - vector *entry; - vector *vectors[14]; + void *initial_stack; + vector *entry; + vector *vectors[14]; }; /** * Initial startup code. */ -void -_main(void) +void _main(void) { - // load the stack base for the current stack before we attempt to branch to any function - // that might bounds-check the stack - asm volatile ("mov r10, %0" : : "r" (&irq_stack[0]) : ); + // load the stack base for the current stack before we attempt to branch to any function + // that might bounds-check the stack + asm volatile ("mov r10, %0" : : "r" (&irq_stack[0]) :); - /* enable usage, bus and memory faults */ - SCB->SHCSR |= SCB_SHCSR_USGFAULTENA_Msk | SCB_SHCSR_BUSFAULTENA_Msk | SCB_SHCSR_MEMFAULTENA_Msk; + /* enable usage, bus and memory faults */ + SCB->SHCSR |= SCB_SHCSR_USGFAULTENA_Msk | SCB_SHCSR_BUSFAULTENA_Msk | SCB_SHCSR_MEMFAULTENA_Msk; - /* configure FP state save behaviour - automatic, lazy save */ - FPU->FPCCR |= FPU_FPCCR_ASPEN_Msk | FPU_FPCCR_LSPEN_Msk; + /* configure FP state save behaviour - automatic, lazy save */ + FPU->FPCCR |= FPU_FPCCR_ASPEN_Msk | FPU_FPCCR_LSPEN_Msk; - /* configure default FPU state */ - FPU->FPDSCR |= FPU_FPDSCR_DN_Msk; /* enable Default NaN */ + /* configure default FPU state */ + FPU->FPDSCR |= FPU_FPDSCR_DN_Msk; /* enable Default NaN */ - /* enable the FPU */ - SCB->CPACR |= (0xf << 20); // turn on CP10/11 for FP support on cores that implement it + /* enable the FPU */ + SCB->CPACR |= (0xf << 20); // turn on CP10/11 for FP support on cores that implement it - /* copy initialised data from flash to RAM */ - memcpy(&_sdata, &_sidata, &_edata - &_sdata); + /* copy initialised data from flash to RAM */ + memcpy(&_sdata, &_sidata, &_edata - &_sdata); - /* zero the BSS */ - memset(&_sbss, 0, &_ebss - &_sbss); + /* zero the BSS */ + memset(&_sbss, 0, &_ebss - &_sbss); - /* zero any 'fast' RAM that's been used */ - memset(&_sfast, 0, &_efast - &_sfast); + /* zero any 'fast' RAM that's been used */ + memset(&_sfast, 0, &_efast - &_sfast); - /* fill most of the IRQ/bootstrap stack with a watermark pattern so we can measure how much is used */ - /* leave a little space at the top in case memset() isn't a leaf with no locals */ - memset(&irq_stack, 0xa5, sizeof(irq_stack) - 64); + /* fill most of the IRQ/bootstrap stack with a watermark pattern so we can measure how much is used */ + /* leave a little space at the top in case memset() isn't a leaf with no locals */ + memset(&irq_stack, 0xa5, sizeof(irq_stack) - 64); - /* call main */ - (void)main(); + /* call main */ + (void)main(); } /** * Default handler for CPU exceptions. */ -static void -default_cpu_handler(void) +static void default_cpu_handler(void) { - for (;;) ; + for (;;) { + ; + } } /** Prototype for optional exception vector handlers */ -#define HANDLER(_name) extern vector _name __attribute__((weak, alias("default_cpu_handler"))) +#define HANDLER(_name) extern vector _name __attribute__((weak, alias("default_cpu_handler"))) /* standard CMSIS vector names */ HANDLER(NMI_Handler); @@ -122,25 +122,26 @@ HANDLER(xPortPendSVHandler); HANDLER(xPortSysTickHandler); /** CortexM3 vector table */ -struct cm3_vectors cpu_vectors __attribute((section(".cpu_vectors"))) = { - .initial_stack = &irq_stack[sizeof(irq_stack)], - .entry = (vector *)_main, - .vectors = { - NMI_Handler, - HardFault_Handler, - MemManage_Handler, - BusFault_Handler, - UsageFault_Handler, - 0, - 0, - 0, - 0, - vPortSVCHandler, - DebugMon_Handler, - 0, - xPortPendSVHandler, - xPortSysTickHandler, - } +struct cm3_vectors cpu_vectors __attribute((section(".cpu_vectors"))) = +{ + .initial_stack = &irq_stack[sizeof(irq_stack)], + .entry = (vector *)_main, + .vectors = { + NMI_Handler, + HardFault_Handler, + MemManage_Handler, + BusFault_Handler, + UsageFault_Handler, + 0, + 0, + 0, + 0, + vPortSVCHandler, + DebugMon_Handler, + 0, + xPortPendSVHandler, + xPortSysTickHandler, + } }; /** diff --git a/flight/pios/stm32f4xx/vectors_stm32f4xx.c b/flight/pios/stm32f4xx/vectors_stm32f4xx.c index f1a958010..f1ac30559 100644 --- a/flight/pios/stm32f4xx/vectors_stm32f4xx.c +++ b/flight/pios/stm32f4xx/vectors_stm32f4xx.c @@ -26,185 +26,186 @@ */ /** interrupt handler */ -typedef void (vector)(void); +typedef void (vector)(void); /** default interrupt handler */ -static void -default_io_handler(void) +static void default_io_handler(void) { - for (;;) ; + for (;;) { + ; + } } /** prototypes an interrupt handler */ -#define HANDLER(_name) extern vector _name __attribute__((weak, alias("default_io_handler"))) +#define HANDLER(_name) extern vector _name __attribute__((weak, alias("default_io_handler"))) -HANDLER(WWDG_IRQHandler); // Window WatchDog -HANDLER(PVD_IRQHandler); // PVD through EXTI Line detection -HANDLER(TAMP_STAMP_IRQHandler); // Tamper and TimeStamps through the EXTI line -HANDLER(RTC_WKUP_IRQHandler); // RTC Wakeup through the EXTI line -HANDLER(FLASH_IRQHandler); // FLASH -HANDLER(RCC_IRQHandler); // RCC -HANDLER(EXTI0_IRQHandler); // EXTI Line0 -HANDLER(EXTI1_IRQHandler); // EXTI Line1 -HANDLER(EXTI2_IRQHandler); // EXTI Line2 -HANDLER(EXTI3_IRQHandler); // EXTI Line3 -HANDLER(EXTI4_IRQHandler); // EXTI Line4 -HANDLER(DMA1_Stream0_IRQHandler); // DMA1 Stream 0 -HANDLER(DMA1_Stream1_IRQHandler); // DMA1 Stream 1 -HANDLER(DMA1_Stream2_IRQHandler); // DMA1 Stream 2 -HANDLER(DMA1_Stream3_IRQHandler); // DMA1 Stream 3 -HANDLER(DMA1_Stream4_IRQHandler); // DMA1 Stream 4 -HANDLER(DMA1_Stream5_IRQHandler); // DMA1 Stream 5 -HANDLER(DMA1_Stream6_IRQHandler); // DMA1 Stream 6 -HANDLER(ADC_IRQHandler); // ADC1, ADC2 and ADC3s -HANDLER(CAN1_TX_IRQHandler); // CAN1 TX -HANDLER(CAN1_RX0_IRQHandler); // CAN1 RX0 -HANDLER(CAN1_RX1_IRQHandler); // CAN1 RX1 -HANDLER(CAN1_SCE_IRQHandler); // CAN1 SCE -HANDLER(EXTI9_5_IRQHandler); // External Line[9:5]s -HANDLER(TIM1_BRK_TIM9_IRQHandler); // TIM1 Break and TIM9 -HANDLER(TIM1_UP_TIM10_IRQHandler); // TIM1 Update and TIM10 -HANDLER(TIM1_TRG_COM_TIM11_IRQHandler); // TIM1 Trigger and Commutation and TIM11 -HANDLER(TIM1_CC_IRQHandler); // TIM1 Capture Compare -HANDLER(TIM2_IRQHandler); // TIM2 -HANDLER(TIM3_IRQHandler); // TIM3 -HANDLER(TIM4_IRQHandler); // TIM4 -HANDLER(I2C1_EV_IRQHandler); // I2C1 Event -HANDLER(I2C1_ER_IRQHandler); // I2C1 Error -HANDLER(I2C2_EV_IRQHandler); // I2C2 Event -HANDLER(I2C2_ER_IRQHandler); // I2C2 Error -HANDLER(SPI1_IRQHandler); // SPI1 -HANDLER(SPI2_IRQHandler); // SPI2 -HANDLER(USART1_IRQHandler); // USART1 -HANDLER(USART2_IRQHandler); // USART2 -HANDLER(USART3_IRQHandler); // USART3 -HANDLER(EXTI15_10_IRQHandler); // External Line[15:10]s -HANDLER(RTC_Alarm_IRQHandler); // RTC Alarm (A and B) through EXTI Line -HANDLER(OTG_FS_WKUP_IRQHandler); // USB OTG FS Wakeup through EXTI line -HANDLER(TIM8_BRK_TIM12_IRQHandler); // TIM8 Break and TIM12 -HANDLER(TIM8_UP_TIM13_IRQHandler); // TIM8 Update and TIM13 -HANDLER(TIM8_TRG_COM_TIM14_IRQHandler); // TIM8 Trigger and Commutation and TIM14 -HANDLER(TIM8_CC_IRQHandler); // TIM8 Capture Compare -HANDLER(DMA1_Stream7_IRQHandler); // DMA1 Stream7 -HANDLER(FSMC_IRQHandler); // FSMC -HANDLER(SDIO_IRQHandler); // SDIO -HANDLER(TIM5_IRQHandler); // TIM5 -HANDLER(SPI3_IRQHandler); // SPI3 -HANDLER(USART4_IRQHandler); // UART4 -HANDLER(USART5_IRQHandler); // UART5 -HANDLER(TIM6_DAC_IRQHandler); // TIM6 and DAC1&2 underrun errors -HANDLER(TIM7_IRQHandler); // TIM7 -HANDLER(DMA2_Stream0_IRQHandler); // DMA2 Stream 0 -HANDLER(DMA2_Stream1_IRQHandler); // DMA2 Stream 1 -HANDLER(DMA2_Stream2_IRQHandler); // DMA2 Stream 2 -HANDLER(DMA2_Stream3_IRQHandler); // DMA2 Stream 3 -HANDLER(DMA2_Stream4_IRQHandler); // DMA2 Stream 4 -HANDLER(ETH_IRQHandler); // Ethernet -HANDLER(ETH_WKUP_IRQHandler); // Ethernet Wakeup through EXTI line -HANDLER(CAN2_TX_IRQHandler); // CAN2 TX -HANDLER(CAN2_RX0_IRQHandler); // CAN2 RX0 -HANDLER(CAN2_RX1_IRQHandler); // CAN2 RX1 -HANDLER(CAN2_SCE_IRQHandler); // CAN2 SCE -HANDLER(OTG_FS_IRQHandler); // USB OTG FS -HANDLER(DMA2_Stream5_IRQHandler); // DMA2 Stream 5 -HANDLER(DMA2_Stream6_IRQHandler); // DMA2 Stream 6 -HANDLER(DMA2_Stream7_IRQHandler); // DMA2 Stream 7 -HANDLER(USART6_IRQHandler); // USART6 -HANDLER(I2C3_EV_IRQHandler); // I2C3 event -HANDLER(I2C3_ER_IRQHandler); // I2C3 error -HANDLER(OTG_HS_EP1_OUT_IRQHandler); // USB OTG HS End Point 1 Out -HANDLER(OTG_HS_EP1_IN_IRQHandler); // USB OTG HS End Point 1 In -HANDLER(OTG_HS_WKUP_IRQHandler); // USB OTG HS Wakeup through EXTI -HANDLER(OTG_HS_IRQHandler); // USB OTG HS -HANDLER(DCMI_IRQHandler); // DCMI -HANDLER(CRYP_IRQHandler); // CRYP crypto -HANDLER(HASH_RNG_IRQHandler); // Hash and Rng -HANDLER(FPU_IRQHandler); // FPU +HANDLER(WWDG_IRQHandler); // Window WatchDog +HANDLER(PVD_IRQHandler); // PVD through EXTI Line detection +HANDLER(TAMP_STAMP_IRQHandler); // Tamper and TimeStamps through the EXTI line +HANDLER(RTC_WKUP_IRQHandler); // RTC Wakeup through the EXTI line +HANDLER(FLASH_IRQHandler); // FLASH +HANDLER(RCC_IRQHandler); // RCC +HANDLER(EXTI0_IRQHandler); // EXTI Line0 +HANDLER(EXTI1_IRQHandler); // EXTI Line1 +HANDLER(EXTI2_IRQHandler); // EXTI Line2 +HANDLER(EXTI3_IRQHandler); // EXTI Line3 +HANDLER(EXTI4_IRQHandler); // EXTI Line4 +HANDLER(DMA1_Stream0_IRQHandler); // DMA1 Stream 0 +HANDLER(DMA1_Stream1_IRQHandler); // DMA1 Stream 1 +HANDLER(DMA1_Stream2_IRQHandler); // DMA1 Stream 2 +HANDLER(DMA1_Stream3_IRQHandler); // DMA1 Stream 3 +HANDLER(DMA1_Stream4_IRQHandler); // DMA1 Stream 4 +HANDLER(DMA1_Stream5_IRQHandler); // DMA1 Stream 5 +HANDLER(DMA1_Stream6_IRQHandler); // DMA1 Stream 6 +HANDLER(ADC_IRQHandler); // ADC1, ADC2 and ADC3s +HANDLER(CAN1_TX_IRQHandler); // CAN1 TX +HANDLER(CAN1_RX0_IRQHandler); // CAN1 RX0 +HANDLER(CAN1_RX1_IRQHandler); // CAN1 RX1 +HANDLER(CAN1_SCE_IRQHandler); // CAN1 SCE +HANDLER(EXTI9_5_IRQHandler); // External Line[9:5]s +HANDLER(TIM1_BRK_TIM9_IRQHandler); // TIM1 Break and TIM9 +HANDLER(TIM1_UP_TIM10_IRQHandler); // TIM1 Update and TIM10 +HANDLER(TIM1_TRG_COM_TIM11_IRQHandler); // TIM1 Trigger and Commutation and TIM11 +HANDLER(TIM1_CC_IRQHandler); // TIM1 Capture Compare +HANDLER(TIM2_IRQHandler); // TIM2 +HANDLER(TIM3_IRQHandler); // TIM3 +HANDLER(TIM4_IRQHandler); // TIM4 +HANDLER(I2C1_EV_IRQHandler); // I2C1 Event +HANDLER(I2C1_ER_IRQHandler); // I2C1 Error +HANDLER(I2C2_EV_IRQHandler); // I2C2 Event +HANDLER(I2C2_ER_IRQHandler); // I2C2 Error +HANDLER(SPI1_IRQHandler); // SPI1 +HANDLER(SPI2_IRQHandler); // SPI2 +HANDLER(USART1_IRQHandler); // USART1 +HANDLER(USART2_IRQHandler); // USART2 +HANDLER(USART3_IRQHandler); // USART3 +HANDLER(EXTI15_10_IRQHandler); // External Line[15:10]s +HANDLER(RTC_Alarm_IRQHandler); // RTC Alarm (A and B) through EXTI Line +HANDLER(OTG_FS_WKUP_IRQHandler); // USB OTG FS Wakeup through EXTI line +HANDLER(TIM8_BRK_TIM12_IRQHandler); // TIM8 Break and TIM12 +HANDLER(TIM8_UP_TIM13_IRQHandler); // TIM8 Update and TIM13 +HANDLER(TIM8_TRG_COM_TIM14_IRQHandler); // TIM8 Trigger and Commutation and TIM14 +HANDLER(TIM8_CC_IRQHandler); // TIM8 Capture Compare +HANDLER(DMA1_Stream7_IRQHandler); // DMA1 Stream7 +HANDLER(FSMC_IRQHandler); // FSMC +HANDLER(SDIO_IRQHandler); // SDIO +HANDLER(TIM5_IRQHandler); // TIM5 +HANDLER(SPI3_IRQHandler); // SPI3 +HANDLER(USART4_IRQHandler); // UART4 +HANDLER(USART5_IRQHandler); // UART5 +HANDLER(TIM6_DAC_IRQHandler); // TIM6 and DAC1&2 underrun errors +HANDLER(TIM7_IRQHandler); // TIM7 +HANDLER(DMA2_Stream0_IRQHandler); // DMA2 Stream 0 +HANDLER(DMA2_Stream1_IRQHandler); // DMA2 Stream 1 +HANDLER(DMA2_Stream2_IRQHandler); // DMA2 Stream 2 +HANDLER(DMA2_Stream3_IRQHandler); // DMA2 Stream 3 +HANDLER(DMA2_Stream4_IRQHandler); // DMA2 Stream 4 +HANDLER(ETH_IRQHandler); // Ethernet +HANDLER(ETH_WKUP_IRQHandler); // Ethernet Wakeup through EXTI line +HANDLER(CAN2_TX_IRQHandler); // CAN2 TX +HANDLER(CAN2_RX0_IRQHandler); // CAN2 RX0 +HANDLER(CAN2_RX1_IRQHandler); // CAN2 RX1 +HANDLER(CAN2_SCE_IRQHandler); // CAN2 SCE +HANDLER(OTG_FS_IRQHandler); // USB OTG FS +HANDLER(DMA2_Stream5_IRQHandler); // DMA2 Stream 5 +HANDLER(DMA2_Stream6_IRQHandler); // DMA2 Stream 6 +HANDLER(DMA2_Stream7_IRQHandler); // DMA2 Stream 7 +HANDLER(USART6_IRQHandler); // USART6 +HANDLER(I2C3_EV_IRQHandler); // I2C3 event +HANDLER(I2C3_ER_IRQHandler); // I2C3 error +HANDLER(OTG_HS_EP1_OUT_IRQHandler); // USB OTG HS End Point 1 Out +HANDLER(OTG_HS_EP1_IN_IRQHandler); // USB OTG HS End Point 1 In +HANDLER(OTG_HS_WKUP_IRQHandler); // USB OTG HS Wakeup through EXTI +HANDLER(OTG_HS_IRQHandler); // USB OTG HS +HANDLER(DCMI_IRQHandler); // DCMI +HANDLER(CRYP_IRQHandler); // CRYP crypto +HANDLER(HASH_RNG_IRQHandler); // Hash and Rng +HANDLER(FPU_IRQHandler); // FPU /** stm32f4xx interrupt vector table */ vector *io_vectors[] __attribute__((section(".io_vectors"))) = { - WWDG_IRQHandler, // Window WatchDog - PVD_IRQHandler, // PVD through EXTI Line detection - TAMP_STAMP_IRQHandler, // Tamper and TimeStamps through the EXTI line - RTC_WKUP_IRQHandler, // RTC Wakeup through the EXTI line - FLASH_IRQHandler, // FLASH - RCC_IRQHandler, // RCC - EXTI0_IRQHandler, // EXTI Line0 - EXTI1_IRQHandler, // EXTI Line1 - EXTI2_IRQHandler, // EXTI Line2 - EXTI3_IRQHandler, // EXTI Line3 - EXTI4_IRQHandler, // EXTI Line4 - DMA1_Stream0_IRQHandler, // DMA1 Stream 0 - DMA1_Stream1_IRQHandler, // DMA1 Stream 1 - DMA1_Stream2_IRQHandler, // DMA1 Stream 2 - DMA1_Stream3_IRQHandler, // DMA1 Stream 3 - DMA1_Stream4_IRQHandler, // DMA1 Stream 4 - DMA1_Stream5_IRQHandler, // DMA1 Stream 5 - DMA1_Stream6_IRQHandler, // DMA1 Stream 6 - ADC_IRQHandler, // ADC1, ADC2 and ADC3s - CAN1_TX_IRQHandler, // CAN1 TX - CAN1_RX0_IRQHandler, // CAN1 RX0 - CAN1_RX1_IRQHandler, // CAN1 RX1 - CAN1_SCE_IRQHandler, // CAN1 SCE - EXTI9_5_IRQHandler, // External Line[9:5]s - TIM1_BRK_TIM9_IRQHandler, // TIM1 Break and TIM9 - TIM1_UP_TIM10_IRQHandler, // TIM1 Update and TIM10 - TIM1_TRG_COM_TIM11_IRQHandler, // TIM1 Trigger and Commutation and TIM11 - TIM1_CC_IRQHandler, // TIM1 Capture Compare - TIM2_IRQHandler, // TIM2 - TIM3_IRQHandler, // TIM3 - TIM4_IRQHandler, // TIM4 - I2C1_EV_IRQHandler, // I2C1 Event - I2C1_ER_IRQHandler, // I2C1 Error - I2C2_EV_IRQHandler, // I2C2 Event - I2C2_ER_IRQHandler, // I2C2 Error - SPI1_IRQHandler, // SPI1 - SPI2_IRQHandler, // SPI2 - USART1_IRQHandler, // USART1 - USART2_IRQHandler, // USART2 - USART3_IRQHandler, // USART3 - EXTI15_10_IRQHandler, // External Line[15:10]s - RTC_Alarm_IRQHandler, // RTC Alarm (A and B) through EXTI Line - OTG_FS_WKUP_IRQHandler, // USB OTG FS Wakeup through EXTI line - TIM8_BRK_TIM12_IRQHandler, // TIM8 Break and TIM12 - TIM8_UP_TIM13_IRQHandler, // TIM8 Update and TIM13 - TIM8_TRG_COM_TIM14_IRQHandler, // TIM8 Trigger and Commutation and TIM14 - TIM8_CC_IRQHandler, // TIM8 Capture Compare - DMA1_Stream7_IRQHandler, // DMA1 Stream7 - FSMC_IRQHandler, // FSMC - SDIO_IRQHandler, // SDIO - TIM5_IRQHandler, // TIM5 - SPI3_IRQHandler, // SPI3 - USART4_IRQHandler, // UART4 - USART5_IRQHandler, // UART5 - TIM6_DAC_IRQHandler, // TIM6 and DAC1&2 underrun errors - TIM7_IRQHandler, // TIM7 - DMA2_Stream0_IRQHandler, // DMA2 Stream 0 - DMA2_Stream1_IRQHandler, // DMA2 Stream 1 - DMA2_Stream2_IRQHandler, // DMA2 Stream 2 - DMA2_Stream3_IRQHandler, // DMA2 Stream 3 - DMA2_Stream4_IRQHandler, // DMA2 Stream 4 - ETH_IRQHandler, // Ethernet - ETH_WKUP_IRQHandler, // Ethernet Wakeup through EXTI line - CAN2_TX_IRQHandler, // CAN2 TX - CAN2_RX0_IRQHandler, // CAN2 RX0 - CAN2_RX1_IRQHandler, // CAN2 RX1 - CAN2_SCE_IRQHandler, // CAN2 SCE - OTG_FS_IRQHandler, // USB OTG FS - DMA2_Stream5_IRQHandler, // DMA2 Stream 5 - DMA2_Stream6_IRQHandler, // DMA2 Stream 6 - DMA2_Stream7_IRQHandler, // DMA2 Stream 7 - USART6_IRQHandler, // USART6 - I2C3_EV_IRQHandler, // I2C3 event - I2C3_ER_IRQHandler, // I2C3 error - OTG_HS_EP1_OUT_IRQHandler, // USB OTG HS End Point 1 Out - OTG_HS_EP1_IN_IRQHandler, // USB OTG HS End Point 1 In - OTG_HS_WKUP_IRQHandler, // USB OTG HS Wakeup through EXTI - OTG_HS_IRQHandler, // USB OTG HS - DCMI_IRQHandler, // DCMI - CRYP_IRQHandler, // CRYP crypto - HASH_RNG_IRQHandler, // Hash and Rng - FPU_IRQHandler, // FPU + WWDG_IRQHandler, // Window WatchDog + PVD_IRQHandler, // PVD through EXTI Line detection + TAMP_STAMP_IRQHandler, // Tamper and TimeStamps through the EXTI line + RTC_WKUP_IRQHandler, // RTC Wakeup through the EXTI line + FLASH_IRQHandler, // FLASH + RCC_IRQHandler, // RCC + EXTI0_IRQHandler, // EXTI Line0 + EXTI1_IRQHandler, // EXTI Line1 + EXTI2_IRQHandler, // EXTI Line2 + EXTI3_IRQHandler, // EXTI Line3 + EXTI4_IRQHandler, // EXTI Line4 + DMA1_Stream0_IRQHandler, // DMA1 Stream 0 + DMA1_Stream1_IRQHandler, // DMA1 Stream 1 + DMA1_Stream2_IRQHandler, // DMA1 Stream 2 + DMA1_Stream3_IRQHandler, // DMA1 Stream 3 + DMA1_Stream4_IRQHandler, // DMA1 Stream 4 + DMA1_Stream5_IRQHandler, // DMA1 Stream 5 + DMA1_Stream6_IRQHandler, // DMA1 Stream 6 + ADC_IRQHandler, // ADC1, ADC2 and ADC3s + CAN1_TX_IRQHandler, // CAN1 TX + CAN1_RX0_IRQHandler, // CAN1 RX0 + CAN1_RX1_IRQHandler, // CAN1 RX1 + CAN1_SCE_IRQHandler, // CAN1 SCE + EXTI9_5_IRQHandler, // External Line[9:5]s + TIM1_BRK_TIM9_IRQHandler, // TIM1 Break and TIM9 + TIM1_UP_TIM10_IRQHandler, // TIM1 Update and TIM10 + TIM1_TRG_COM_TIM11_IRQHandler, // TIM1 Trigger and Commutation and TIM11 + TIM1_CC_IRQHandler, // TIM1 Capture Compare + TIM2_IRQHandler, // TIM2 + TIM3_IRQHandler, // TIM3 + TIM4_IRQHandler, // TIM4 + I2C1_EV_IRQHandler, // I2C1 Event + I2C1_ER_IRQHandler, // I2C1 Error + I2C2_EV_IRQHandler, // I2C2 Event + I2C2_ER_IRQHandler, // I2C2 Error + SPI1_IRQHandler, // SPI1 + SPI2_IRQHandler, // SPI2 + USART1_IRQHandler, // USART1 + USART2_IRQHandler, // USART2 + USART3_IRQHandler, // USART3 + EXTI15_10_IRQHandler, // External Line[15:10]s + RTC_Alarm_IRQHandler, // RTC Alarm (A and B) through EXTI Line + OTG_FS_WKUP_IRQHandler, // USB OTG FS Wakeup through EXTI line + TIM8_BRK_TIM12_IRQHandler, // TIM8 Break and TIM12 + TIM8_UP_TIM13_IRQHandler, // TIM8 Update and TIM13 + TIM8_TRG_COM_TIM14_IRQHandler, // TIM8 Trigger and Commutation and TIM14 + TIM8_CC_IRQHandler, // TIM8 Capture Compare + DMA1_Stream7_IRQHandler, // DMA1 Stream7 + FSMC_IRQHandler, // FSMC + SDIO_IRQHandler, // SDIO + TIM5_IRQHandler, // TIM5 + SPI3_IRQHandler, // SPI3 + USART4_IRQHandler, // UART4 + USART5_IRQHandler, // UART5 + TIM6_DAC_IRQHandler, // TIM6 and DAC1&2 underrun errors + TIM7_IRQHandler, // TIM7 + DMA2_Stream0_IRQHandler, // DMA2 Stream 0 + DMA2_Stream1_IRQHandler, // DMA2 Stream 1 + DMA2_Stream2_IRQHandler, // DMA2 Stream 2 + DMA2_Stream3_IRQHandler, // DMA2 Stream 3 + DMA2_Stream4_IRQHandler, // DMA2 Stream 4 + ETH_IRQHandler, // Ethernet + ETH_WKUP_IRQHandler, // Ethernet Wakeup through EXTI line + CAN2_TX_IRQHandler, // CAN2 TX + CAN2_RX0_IRQHandler, // CAN2 RX0 + CAN2_RX1_IRQHandler, // CAN2 RX1 + CAN2_SCE_IRQHandler, // CAN2 SCE + OTG_FS_IRQHandler, // USB OTG FS + DMA2_Stream5_IRQHandler, // DMA2 Stream 5 + DMA2_Stream6_IRQHandler, // DMA2 Stream 6 + DMA2_Stream7_IRQHandler, // DMA2 Stream 7 + USART6_IRQHandler, // USART6 + I2C3_EV_IRQHandler, // I2C3 event + I2C3_ER_IRQHandler, // I2C3 error + OTG_HS_EP1_OUT_IRQHandler, // USB OTG HS End Point 1 Out + OTG_HS_EP1_IN_IRQHandler, // USB OTG HS End Point 1 In + OTG_HS_WKUP_IRQHandler, // USB OTG HS Wakeup through EXTI + OTG_HS_IRQHandler, // USB OTG HS + DCMI_IRQHandler, // DCMI + CRYP_IRQHandler, // CRYP crypto + HASH_RNG_IRQHandler, // Hash and Rng + FPU_IRQHandler, // FPU }; /** diff --git a/flight/pios/win32/inc/FreeRTOSConfig.h b/flight/pios/win32/inc/FreeRTOSConfig.h index 063a843a1..87f9020c7 100644 --- a/flight/pios/win32/inc/FreeRTOSConfig.h +++ b/flight/pios/win32/inc/FreeRTOSConfig.h @@ -1,76 +1,72 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ -/* Notes: We use 5 task priorities */ +/* Notes: We use 5 task priorities */ #define IDLE_SLEEPS -#define configUSE_PREEMPTION 1 -#define configIDLE_SHOULD_YIELD 1 +#define configUSE_PREEMPTION 1 +#define configIDLE_SHOULD_YIELD 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 ) -#define configTICK_RATE_HZ ( ( portTickType ) 100 ) -#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) -#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 256 ) -#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 45 * 1024 ) ) -#define configMAX_TASK_NAME_LEN ( 16 ) -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)100) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)256) +#define configTOTAL_HEAP_SIZE ((size_t)(45 * 1024)) +#define configMAX_TASK_NAME_LEN (16) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ - -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 0 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 0 - + to exclude the API function. */ +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 0 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 #endif /* FREERTOS_CONFIG_H */ - diff --git a/flight/pios/win32/inc/pios_com.h b/flight/pios/win32/inc/pios_com.h index b18cb3d52..dd91a4d81 100644 --- a/flight/pios/win32/inc/pios_com.h +++ b/flight/pios/win32/inc/pios_com.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_com.h + * @file pios_com.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM layer functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -45,12 +45,12 @@ extern int32_t PIOS_COM_ReceiveBufferUsed(uint8_t port); extern int32_t PIOS_COM_ReceiveHandler(void); struct pios_com_driver { - void (*init)(uint8_t id); - void (*set_baud)(uint8_t id, uint32_t baud); - int32_t (*tx_nb)(uint8_t id, char *buffer, uint16_t len); - int32_t (*tx)(uint8_t id, char *buffer, uint16_t len); - int32_t (*rx)(uint8_t id); - int32_t (*rx_avail)(uint8_t id); + void (*init)(uint8_t id); + void (*set_baud)(uint8_t id, uint32_t baud); + int32_t (*tx_nb)(uint8_t id, char *buffer, uint16_t len); + int32_t (*tx)(uint8_t id, char *buffer, uint16_t len); + int32_t (*rx)(uint8_t id); + int32_t (*rx_avail)(uint8_t id); }; #endif /* PIOS_COM_H */ diff --git a/flight/pios/win32/inc/pios_com_priv.h b/flight/pios/win32/inc/pios_com_priv.h index 5b285edac..1acd8b1b1 100644 --- a/flight/pios/win32/inc/pios_com_priv.h +++ b/flight/pios/win32/inc/pios_com_priv.h @@ -3,7 +3,7 @@ * * @file pios_com_priv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM private definitions. * @see The GNU Public License (GPL) Version 3 * @@ -30,12 +30,11 @@ #include struct pios_com_dev { - uint8_t id; - const struct pios_com_driver * const driver; + uint8_t id; + const struct pios_com_driver *const driver; }; extern struct pios_com_dev pios_com_devs[]; -extern const uint8_t pios_com_num_devices; +extern const uint8_t pios_com_num_devices; #endif /* PIOS_COM_PRIV_H */ - diff --git a/flight/pios/win32/inc/pios_crc.h b/flight/pios/win32/inc/pios_crc.h index 3a64f8bab..5a737ade1 100644 --- a/flight/pios/win32/inc/pios_crc.h +++ b/flight/pios/win32/inc/pios_crc.h @@ -5,27 +5,27 @@ * @addtogroup PIOS_CRC CRC Functions * @{ * - * @file pios_crc.h + * @file pios_crc.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief CRC functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data); -uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length); +uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t *data, int32_t length); diff --git a/flight/pios/win32/inc/pios_delay.h b/flight/pios/win32/inc/pios_delay.h index 8c49603c4..fdb7d4644 100644 --- a/flight/pios/win32/inc/pios_delay.h +++ b/flight/pios/win32/inc/pios_delay.h @@ -3,24 +3,24 @@ * * @file pios_settings.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) - * @brief Settings functions header + * Parts by Thorsten Klose (tk@midibox.org) + * @brief Settings functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/win32/inc/pios_initcall.h b/flight/pios/win32/inc/pios_initcall.h index 3805a88c6..e60b8755d 100644 --- a/flight/pios/win32/inc/pios_initcall.h +++ b/flight/pios/win32/inc/pios_initcall.h @@ -6,25 +6,25 @@ * @brief Initcall Macros * @{ * - * @file pios_initcall.h + * @file pios_initcall.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. * @brief Initcall header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,15 +42,16 @@ #define MODULE_TASKCREATE_ALL -#define MODULE_INITIALISE_ALL { \ - /* Initialize modules */ \ - InitModules(); \ - /* Start the FreeRTOS scheduler which never returns.*/ \ - /* Initialize the system thread */ \ - SystemModInitialize();} +#define MODULE_INITIALISE_ALL \ + { \ + /* Initialize modules */ \ + InitModules(); \ + /* Start the FreeRTOS scheduler which never returns.*/ \ + /* Initialize the system thread */ \ + SystemModInitialize(); } -#endif /* PIOS_INITCALL_H */ +#endif /* PIOS_INITCALL_H */ /** * @} diff --git a/flight/pios/win32/inc/pios_led.h b/flight/pios/win32/inc/pios_led.h index 94283e4b6..bbf728622 100644 --- a/flight/pios/win32/inc/pios_led.h +++ b/flight/pios/win32/inc/pios_led.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file pios_led.h + * @file pios_led.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief LED functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -29,9 +29,9 @@ /* Type Definitions */ #if (PIOS_LED_NUM == 1) -typedef enum {LED1 = 0} LedTypeDef; +typedef enum { LED1 = 0 } LedTypeDef; #elif (PIOS_LED_NUM == 2) -typedef enum {LED1 = 0, LED2 = 1} LedTypeDef; +typedef enum { LED1 = 0, LED2 = 1 } LedTypeDef; #endif /* Public Functions */ diff --git a/flight/pios/win32/inc/pios_posix.h b/flight/pios/win32/inc/pios_posix.h index 05b89533e..68300cfda 100644 --- a/flight/pios/win32/inc/pios_posix.h +++ b/flight/pios/win32/inc/pios_posix.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file posix.h + * @file posix.h * @author Corvus Corax Copyright (C) 2010. * @brief Definitions to run PiOS on posix * @see The GNU Public License (GPL) Version 2 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -28,10 +28,9 @@ #include -#define FILEINFO FILE* +#define FILEINFO FILE * #define PIOS_SERVO_NUM_OUTPUTS 8 -#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS +#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS #endif - diff --git a/flight/pios/win32/inc/pios_sdcard.h b/flight/pios/win32/inc/pios_sdcard.h index 2927def2d..72bd9d042 100644 --- a/flight/pios/win32/inc/pios_sdcard.h +++ b/flight/pios/win32/inc/pios_sdcard.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_sdcard.h + * @file pios_sdcard.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief System and hardware Init functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,61 +31,61 @@ /* Public Functions */ typedef struct { - uint8_t CSDStruct; /* CSD structure */ - uint8_t SysSpecVersion; /* System specification version */ - uint8_t Reserved1; /* Reserved */ - uint8_t TAAC; /* Data read access-time 1 */ - uint8_t NSAC; /* Data read access-time 2 in CLK cycles */ - uint8_t MaxBusClkFrec; /* Max. bus clock frequency */ - uint16_t CardComdClasses; /* Card command classes */ - uint8_t RdBlockLen; /* Max. read data block length */ - uint8_t PartBlockRead; /* Partial blocks for read allowed */ - uint8_t WrBlockMisalign; /* Write block misalignment */ - uint8_t RdBlockMisalign; /* Read block misalignment */ - uint8_t DSRImpl; /* DSR implemented */ - uint8_t Reserved2; /* Reserved */ - uint16_t DeviceSize; /* Device Size */ - uint8_t MaxRdCurrentVDDMin; /* Max. read current @ VDD min */ - uint8_t MaxRdCurrentVDDMax; /* Max. read current @ VDD max */ - uint8_t MaxWrCurrentVDDMin; /* Max. write current @ VDD min */ - uint8_t MaxWrCurrentVDDMax; /* Max. write current @ VDD max */ - uint8_t DeviceSizeMul; /* Device size multiplier */ - uint8_t EraseGrSize; /* Erase group size */ - uint8_t EraseGrMul; /* Erase group size multiplier */ - uint8_t WrProtectGrSize; /* Write protect group size */ - uint8_t WrProtectGrEnable; /* Write protect group enable */ - uint8_t ManDeflECC; /* Manufacturer default ECC */ - uint8_t WrSpeedFact; /* Write speed factor */ - uint8_t MaxWrBlockLen; /* Max. write data block length */ - uint8_t WriteBlockPaPartial; /* Partial blocks for write allowed */ - uint8_t Reserved3; /* Reserved */ - uint8_t ContentProtectAppli; /* Content protection application */ - uint8_t FileFormatGrouop; /* File format group */ - uint8_t CopyFlag; /* Copy flag (OTP) */ - uint8_t PermWrProtect; /* Permanent write protection */ - uint8_t TempWrProtect; /* Temporary write protection */ - uint8_t FileFormat; /* File Format */ - uint8_t ECC; /* ECC code */ - uint8_t msd_CRC; /* CRC */ - uint8_t Reserved4; /* always 1*/ + uint8_t CSDStruct; /* CSD structure */ + uint8_t SysSpecVersion; /* System specification version */ + uint8_t Reserved1; /* Reserved */ + uint8_t TAAC; /* Data read access-time 1 */ + uint8_t NSAC; /* Data read access-time 2 in CLK cycles */ + uint8_t MaxBusClkFrec; /* Max. bus clock frequency */ + uint16_t CardComdClasses; /* Card command classes */ + uint8_t RdBlockLen; /* Max. read data block length */ + uint8_t PartBlockRead; /* Partial blocks for read allowed */ + uint8_t WrBlockMisalign; /* Write block misalignment */ + uint8_t RdBlockMisalign; /* Read block misalignment */ + uint8_t DSRImpl; /* DSR implemented */ + uint8_t Reserved2; /* Reserved */ + uint16_t DeviceSize; /* Device Size */ + uint8_t MaxRdCurrentVDDMin; /* Max. read current @ VDD min */ + uint8_t MaxRdCurrentVDDMax; /* Max. read current @ VDD max */ + uint8_t MaxWrCurrentVDDMin; /* Max. write current @ VDD min */ + uint8_t MaxWrCurrentVDDMax; /* Max. write current @ VDD max */ + uint8_t DeviceSizeMul; /* Device size multiplier */ + uint8_t EraseGrSize; /* Erase group size */ + uint8_t EraseGrMul; /* Erase group size multiplier */ + uint8_t WrProtectGrSize; /* Write protect group size */ + uint8_t WrProtectGrEnable; /* Write protect group enable */ + uint8_t ManDeflECC; /* Manufacturer default ECC */ + uint8_t WrSpeedFact; /* Write speed factor */ + uint8_t MaxWrBlockLen; /* Max. write data block length */ + uint8_t WriteBlockPaPartial; /* Partial blocks for write allowed */ + uint8_t Reserved3; /* Reserved */ + uint8_t ContentProtectAppli; /* Content protection application */ + uint8_t FileFormatGrouop; /* File format group */ + uint8_t CopyFlag; /* Copy flag (OTP) */ + uint8_t PermWrProtect; /* Permanent write protection */ + uint8_t TempWrProtect; /* Temporary write protection */ + uint8_t FileFormat; /* File Format */ + uint8_t ECC; /* ECC code */ + uint8_t msd_CRC; /* CRC */ + uint8_t Reserved4; /* always 1*/ } SDCARDCsdTypeDef; /* Structure taken from Mass Storage Driver example provided by STM */ typedef struct { - uint8_t ManufacturerID; /* ManufacturerID */ - uint16_t OEM_AppliID; /* OEM/Application ID */ - char ProdName[6]; /* Product Name */ - uint8_t ProdRev; /* Product Revision */ - uint32_t ProdSN; /* Product Serial Number */ - uint8_t Reserved1; /* Reserved1 */ - uint16_t ManufactDate; /* Manufacturing Date */ - uint8_t msd_CRC; /* CRC */ - uint8_t Reserved2; /* always 1*/ + uint8_t ManufacturerID; /* ManufacturerID */ + uint16_t OEM_AppliID; /* OEM/Application ID */ + char ProdName[6]; /* Product Name */ + uint8_t ProdRev; /* Product Revision */ + uint32_t ProdSN; /* Product Serial Number */ + uint8_t Reserved1; /* Reserved1 */ + uint16_t ManufactDate; /* Manufacturing Date */ + uint8_t msd_CRC; /* CRC */ + uint8_t Reserved2; /* always 1*/ } SDCARDCidTypeDef; /* Global Variables */ -//extern VOLINFO PIOS_SDCARD_VolInfo; -//extern uint8_t PIOS_SDCARD_Sector[SECTOR_SIZE]; +// extern VOLINFO PIOS_SDCARD_VolInfo; +// extern uint8_t PIOS_SDCARD_Sector[SECTOR_SIZE]; /* Prototypes */ extern int32_t PIOS_SDCARD_Init(void); @@ -103,11 +103,11 @@ extern int32_t PIOS_SDCARD_IsMounted(); extern int32_t PIOS_SDCARD_MountFS(uint32_t StartupLog); extern int32_t PIOS_SDCARD_GetFree(void); -//extern int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len); -//extern int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len); +// extern int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len); +// extern int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len); extern int32_t PIOS_SDCARD_FileCopy(char *Source, char *Destination); extern int32_t PIOS_SDCARD_FileDelete(char *Filename); -#endif +#endif // if defined(PIOS_INCLUDE_SDCARD) #endif /* PIOS_SDCARD_H */ diff --git a/flight/pios/win32/inc/pios_servo.h b/flight/pios/win32/inc/pios_servo.h index 7cbf37a7d..31a5d6a72 100644 --- a/flight/pios/win32/inc/pios_servo.h +++ b/flight/pios/win32/inc/pios_servo.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_SERVO RC Servo Functions * @{ * - * @file pios_servo.h + * @file pios_servo.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RC Servo functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,12 +32,12 @@ /* Public Functions */ extern void PIOS_Servo_Init(void); -extern void PIOS_Servo_SetHz(uint16_t * speeds, uint8_t num_banks); +extern void PIOS_Servo_SetHz(uint16_t *speeds, uint8_t num_banks); extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position); #endif /* PIOS_SERVO_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/win32/inc/pios_sys.h b/flight/pios/win32/inc/pios_sys.h index b99232649..c9e17535f 100644 --- a/flight/pios/win32/inc/pios_sys.h +++ b/flight/pios/win32/inc/pios_sys.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_sys.h + * @file pios_sys.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief System and hardware Init functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/win32/inc/pios_udp.h b/flight/pios/win32/inc/pios_udp.h index f35fe903f..f9f27b88a 100644 --- a/flight/pios/win32/inc/pios_udp.h +++ b/flight/pios/win32/inc/pios_udp.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_usart.h + * @file pios_usart.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief UDP functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,7 +31,7 @@ /* Global Types */ /* Public Functions */ -//extern void PIOS_UDP_Init(void); +// extern void PIOS_UDP_Init(void); void PIOS_UDP_Init(void); void PIOS_UDP_Close(void); extern void PIOS_UDP_ChangeBaud(uint8_t usart, uint32_t baud); diff --git a/flight/pios/win32/inc/pios_udp_priv.h b/flight/pios/win32/inc/pios_udp_priv.h index e7ba88ee9..c60cb628f 100644 --- a/flight/pios/win32/inc/pios_udp_priv.h +++ b/flight/pios/win32/inc/pios_udp_priv.h @@ -3,7 +3,7 @@ * * @file pios_udp_priv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief UDP private definitions. * @see The GNU Public License (GPL) Version 3 * @@ -36,29 +36,28 @@ #include - struct pios_udp_cfg { - const char * ip; - uint16_t port; + const char *ip; + uint16_t port; }; struct pios_udp_buffer { - uint8_t buf[PIOS_UDP_RX_BUFFER_SIZE]; - uint16_t head; - uint16_t tail; - uint16_t size; + uint8_t buf[PIOS_UDP_RX_BUFFER_SIZE]; + uint16_t head; + uint16_t tail; + uint16_t size; }; struct pios_udp_dev { - const struct pios_udp_cfg * const cfg; - struct pios_udp_buffer rx; - SOCKET socket; - struct sockaddr_in server; - struct sockaddr_in client; - uint32_t clientLength; + const struct pios_udp_cfg *const cfg; + struct pios_udp_buffer rx; + SOCKET socket; + struct sockaddr_in server; + struct sockaddr_in client; + uint32_t clientLength; }; extern struct pios_udp_dev pios_udp_devs[]; -extern uint8_t pios_udp_num_devices; +extern uint8_t pios_udp_num_devices; #endif /* PIOS_UDP_PRIV_H */ diff --git a/flight/pios/win32/inc/pios_wdg.h b/flight/pios/win32/inc/pios_wdg.h index 26bab0c8b..7fde01f1e 100644 --- a/flight/pios/win32/inc/pios_wdg.h +++ b/flight/pios/win32/inc/pios_wdg.h @@ -7,7 +7,7 @@ * * @file pios_wdg.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief SPI functions header. * @see The GNU Public License (GPL) Version 3 * @@ -33,15 +33,15 @@ #include -//------------------------ +// ------------------------ // WATCHDOG_SETTINGS -//------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -#define PIOS_WDG_REGISTER BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_AHRS 0x0004 -#define PIOS_WDG_MANUAL 0x0008 +// ------------------------ +#define PIOS_WATCHDOG_TIMEOUT 250 +#define PIOS_WDG_REGISTER BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_AHRS 0x0004 +#define PIOS_WDG_MANUAL 0x0008 void PIOS_WDG_Init(); bool PIOS_WDG_RegisterFlag(uint16_t flag_requested); @@ -50,4 +50,4 @@ uint16_t PIOS_WDG_GetBootupFlags(); uint16_t PIOS_WDG_GetActiveFlags(); void PIOS_WDG_Clear(void); -#endif +#endif // ifndef PIOS_WDG diff --git a/flight/pios/win32/pios.h b/flight/pios/win32/pios.h index 590c6a4df..d8e116344 100644 --- a/flight/pios/win32/pios.h +++ b/flight/pios/win32/pios.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios.h + * @file pios.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Main PiOS header. + * @brief Main PiOS header. * - Central header for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/win32/win32/pios_com.c b/flight/pios/win32/win32/pios_com.c index 749a0bc38..365f056f0 100644 --- a/flight/pios/win32/win32/pios_com.c +++ b/flight/pios/win32/win32/pios_com.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_com.c + * @file pios_com.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM layer functions * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_COM COM layer functions * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,263 +34,263 @@ #include -static struct pios_com_dev * find_com_dev_by_id (uint8_t port) +static struct pios_com_dev *find_com_dev_by_id(uint8_t port) { - if (port >= pios_com_num_devices) { - /* Undefined COM port for this board (see pios_board.c) */ - return NULL; - } + if (port >= pios_com_num_devices) { + /* Undefined COM port for this board (see pios_board.c) */ + return NULL; + } - /* Get a handle for the device configuration */ - return &(pios_com_devs[port]); + /* Get a handle for the device configuration */ + return &(pios_com_devs[port]); } /** -* Initialises COM layer -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ + * Initialises COM layer + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ int32_t PIOS_COM_Init(void) { - int32_t ret = 0; + int32_t ret = 0; - /* If any COM assignment: */ + /* If any COM assignment: */ #if defined(PIOS_INCLUDE_SERIAL) - PIOS_SERIAL_Init(); + PIOS_SERIAL_Init(); #endif #if defined(PIOS_INCLUDE_UDP) - PIOS_UDP_Init(); + PIOS_UDP_Init(); #endif - return ret; + return ret; } int32_t PIOS_COM_Close(void) { - int32_t ret = 0; + int32_t ret = 0; #if defined(PIOS_INCLUDE_UDP) - PIOS_UDP_Close(); + PIOS_UDP_Close(); #endif - return ret; + return ret; } /** -* Change the port speed without re-initializing -* \param[in] port COM port -* \param[in] baud Requested baud rate -* \return -1 if port not available -* \return 0 on success -*/ + * Change the port speed without re-initializing + * \param[in] port COM port + * \param[in] baud Requested baud rate + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_ChangeBaud(uint8_t port, uint32_t baud) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - com_dev = find_com_dev_by_id (port); + com_dev = find_com_dev_by_id(port); - if (!com_dev) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!com_dev) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - /* Invoke the driver function if it exists */ - if (com_dev->driver->set_baud) { - com_dev->driver->set_baud(com_dev->id, baud); - } + /* Invoke the driver function if it exists */ + if (com_dev->driver->set_baud) { + com_dev->driver->set_baud(com_dev->id, baud); + } - return 0; + return 0; } /** -* Sends a package over given port -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a package over given port + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendBufferNonBlocking(uint8_t port, uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - com_dev = find_com_dev_by_id (port); + com_dev = find_com_dev_by_id(port); - if (!com_dev) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!com_dev) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - /* Invoke the driver function if it exists */ - if (com_dev->driver->tx_nb) { - return com_dev->driver->tx_nb(com_dev->id, (char *)buffer, len); - } + /* Invoke the driver function if it exists */ + if (com_dev->driver->tx_nb) { + return com_dev->driver->tx_nb(com_dev->id, (char *)buffer, len); + } - return 0; + return 0; } /** -* Sends a package over given port -* (blocking function) -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a package over given port + * (blocking function) + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendBuffer(uint8_t port, uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - com_dev = find_com_dev_by_id (port); + com_dev = find_com_dev_by_id(port); - if (!com_dev) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!com_dev) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - /* Invoke the driver function if it exists */ - if (com_dev->driver->tx) { - return com_dev->driver->tx(com_dev->id, (char *)buffer, len); - } + /* Invoke the driver function if it exists */ + if (com_dev->driver->tx) { + return com_dev->driver->tx(com_dev->id, (char *)buffer, len); + } - return 0; + return 0; } /** -* Sends a single character over given port -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a single character over given port + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendCharNonBlocking(uint8_t port, char c) { - return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)&c, 1); + return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)&c, 1); } /** -* Sends a single character over given port -* (blocking function) -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a single character over given port + * (blocking function) + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendChar(uint8_t port, char c) { - return PIOS_COM_SendBuffer(port, (uint8_t *)&c, 1); + return PIOS_COM_SendBuffer(port, (uint8_t *)&c, 1); } /** -* Sends a string over given port -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a string over given port + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendStringNonBlocking(uint8_t port, char *str) { - return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)str, (uint16_t)strlen(str)); + return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)str, (uint16_t)strlen(str)); } /** -* Sends a string over given port -* (blocking function) -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a string over given port + * (blocking function) + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendString(uint8_t port, char *str) { - return PIOS_COM_SendBuffer(port, (uint8_t *)str, strlen(str)); + return PIOS_COM_SendBuffer(port, (uint8_t *)str, strlen(str)); } /** -* Sends a formatted string (-> printf) over given port -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* 128 characters supported maximum! -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * 128 characters supported maximum! + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedStringNonBlocking(uint8_t port, char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBufferNonBlocking(port, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBufferNonBlocking(port, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Sends a formatted string (-> printf) over given port -* (blocking function) -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * (blocking function) + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedString(uint8_t port, char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBuffer(port, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBuffer(port, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Transfer bytes from port buffers into another buffer -* \param[in] port COM port -* \returns Byte from buffer -*/ + * Transfer bytes from port buffers into another buffer + * \param[in] port COM port + * \returns Byte from buffer + */ uint8_t PIOS_COM_ReceiveBuffer(uint8_t port) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - com_dev = find_com_dev_by_id (port); - //PIOS_DEBUG_Assert(com_dev); - //PIOS_DEBUG_Assert(com_dev->driver->rx); + com_dev = find_com_dev_by_id(port); + // PIOS_DEBUG_Assert(com_dev); + // PIOS_DEBUG_Assert(com_dev->driver->rx); - return com_dev->driver->rx(com_dev->id); + return com_dev->driver->rx(com_dev->id); } /** -* Get the number of bytes waiting in the buffer -* \param[in] port COM port -* \return Number of bytes used in buffer -*/ + * Get the number of bytes waiting in the buffer + * \param[in] port COM port + * \return Number of bytes used in buffer + */ int32_t PIOS_COM_ReceiveBufferUsed(uint8_t port) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - com_dev = find_com_dev_by_id (port); + com_dev = find_com_dev_by_id(port); - if (!com_dev) { - /* Undefined COM port for this board (see pios_board.c) */ - return 0; - } + if (!com_dev) { + /* Undefined COM port for this board (see pios_board.c) */ + return 0; + } - if (!com_dev->driver->rx_avail) { - return 0; - } + if (!com_dev->driver->rx_avail) { + return 0; + } - return com_dev->driver->rx_avail(com_dev->id); + return com_dev->driver->rx_avail(com_dev->id); } -#endif +#endif /* if defined(PIOS_INCLUDE_COM) */ diff --git a/flight/pios/win32/win32/pios_crc.c b/flight/pios/win32/win32/pios_crc.c index 548ba2648..0e4cd5dec 100644 --- a/flight/pios/win32/win32/pios_crc.c +++ b/flight/pios/win32/win32/pios_crc.c @@ -11,22 +11,22 @@ // CRC lookup table static const uint8_t crc_table[256] = { - 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, - 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, - 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, - 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, - 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, - 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, - 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, - 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, - 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, - 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, - 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, - 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, - 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, - 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, - 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, - 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 + 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, + 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, + 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, + 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, + 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, + 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, + 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, + 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, + 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, + 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, + 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, + 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, + 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, + 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, + 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, + 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 }; /** @@ -49,7 +49,7 @@ static const uint8_t crc_table[256] = { */ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) { - return crc_table[crc ^ data]; + return crc_table[crc ^ data]; } /* @@ -59,16 +59,16 @@ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) * @param[in] length Number of bytes to process * @returns Updated CRC */ -uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length) +uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t *data, int32_t length) { - // use registers for speed - register int32_t len = length; - register uint8_t crc8 = crc; - register const uint8_t *p = data; - - while (len--) - crc8 = crc_table[crc8 ^ *p++]; - - return crc8; -} + // use registers for speed + register int32_t len = length; + register uint8_t crc8 = crc; + register const uint8_t *p = data; + while (len--) { + crc8 = crc_table[crc8 ^ *p++]; + } + + return crc8; +} diff --git a/flight/pios/win32/win32/pios_delay.c b/flight/pios/win32/win32/pios_delay.c index 37a430462..ef0287a99 100644 --- a/flight/pios/win32/win32/pios_delay.c +++ b/flight/pios/win32/win32/pios_delay.c @@ -3,27 +3,27 @@ * * @file pios_delay.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) - * @brief Delay Functions - * - Provides a micro-second granular delay using a TIM + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * @brief Delay Functions + * - Provides a micro-second granular delay using a TIM * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_DELAY Delay Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,55 +34,55 @@ #if defined(PIOS_INCLUDE_DELAY) /** -* Initialises the Timer used by PIOS_DELAY functions
-* This is called from pios.c as part of the main() function -* at system start up. -* \return < 0 if initialisation failed -*/ + * Initialises the Timer used by PIOS_DELAY functions
+ * This is called from pios.c as part of the main() function + * at system start up. + * \return < 0 if initialisation failed + */ #include int32_t PIOS_DELAY_Init(void) { - // stub + // stub - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Waits for a specific number of uS
-* Example:
-* \code -* // Wait for 500 uS -* PIOS_DELAY_Wait_uS(500); -* \endcode -* \param[in] uS delay (1..65535 microseconds) -* \return < 0 on errors -*/ + * Waits for a specific number of uS
+ * Example:
+ * \code + * // Wait for 500 uS + * PIOS_DELAY_Wait_uS(500); + * \endcode + * \param[in] uS delay (1..65535 microseconds) + * \return < 0 on errors + */ int32_t PIOS_DELAY_WaituS(uint16_t uS) { - Sleep(uS/1000); + Sleep(uS / 1000); - return 0; + return 0; } /** -* Waits for a specific number of mS
-* Example:
-* \code -* // Wait for 500 mS -* PIOS_DELAY_Wait_mS(500); -* \endcode -* \param[in] mS delay (1..65535 milliseconds) -* \return < 0 on errors -*/ + * Waits for a specific number of mS
+ * Example:
+ * \code + * // Wait for 500 mS + * PIOS_DELAY_Wait_mS(500); + * \endcode + * \param[in] mS delay (1..65535 milliseconds) + * \return < 0 on errors + */ int32_t PIOS_DELAY_WaitmS(uint16_t mS) { - Sleep(mS); + Sleep(mS); - /* No error */ - return 0; + /* No error */ + return 0; } -#endif +#endif /* if defined(PIOS_INCLUDE_DELAY) */ diff --git a/flight/pios/win32/win32/pios_led.c b/flight/pios/win32/win32/pios_led.c index 4ac23cb2c..4e537e750 100644 --- a/flight/pios/win32/win32/pios_led.c +++ b/flight/pios/win32/win32/pios_led.c @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file pios_led.c + * @file pios_led.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief LED functions, init, toggle, on & off. * @see The GNU Public License (GPL) Version 3 @@ -9,19 +9,19 @@ * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,68 +35,69 @@ /* Local Variables */ -//static GPIO_TypeDef* LED_GPIO_PORT[PIOS_LED_NUM] = PIOS_LED_PORTS; -//static const uint32_t LED_GPIO_PIN[PIOS_LED_NUM] = PIOS_LED_PINS; -//static const uint32_t LED_GPIO_CLK[PIOS_LED_NUM] = PIOS_LED_CLKS; +// static GPIO_TypeDef* LED_GPIO_PORT[PIOS_LED_NUM] = PIOS_LED_PORTS; +// static const uint32_t LED_GPIO_PIN[PIOS_LED_NUM] = PIOS_LED_PINS; +// static const uint32_t LED_GPIO_CLK[PIOS_LED_NUM] = PIOS_LED_CLKS; static uint8_t LED_GPIO[PIOS_LED_NUM]; -static inline void PIOS_SetLED(LedTypeDef LED,uint8_t stat) { - printf("PIOS: LED %i status %i\n",LED,stat); - LED_GPIO[LED]=stat; +static inline void PIOS_SetLED(LedTypeDef LED, uint8_t stat) +{ + printf("PIOS: LED %i status %i\n", LED, stat); + LED_GPIO[LED] = stat; } /** -* Initialises all the LED's -*/ + * Initialises all the LED's + */ void PIOS_LED_Init(void) { - //GPIO_InitTypeDef GPIO_InitStructure; - //GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - - for(int LEDNum = 0; LEDNum < PIOS_LED_NUM; LEDNum++) { - //RCC_APB2PeriphClockCmd(LED_GPIO_CLK[LEDNum], ENABLE); - //GPIO_InitStructure.GPIO_Pin = LED_GPIO_PIN[LEDNum]; - //GPIO_Init(LED_GPIO_PORT[LEDNum], &GPIO_InitStructure); + // GPIO_InitTypeDef GPIO_InitStructure; + // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; + // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - /* LED's Off */ - //LED_GPIO_PORT[LEDNum]->BSRR = LED_GPIO_PIN[LEDNum]; - LED_GPIO[LEDNum]=0; - } + for (int LEDNum = 0; LEDNum < PIOS_LED_NUM; LEDNum++) { + // RCC_APB2PeriphClockCmd(LED_GPIO_CLK[LEDNum], ENABLE); + // GPIO_InitStructure.GPIO_Pin = LED_GPIO_PIN[LEDNum]; + // GPIO_Init(LED_GPIO_PORT[LEDNum], &GPIO_InitStructure); + + /* LED's Off */ + // LED_GPIO_PORT[LEDNum]->BSRR = LED_GPIO_PIN[LEDNum]; + LED_GPIO[LEDNum] = 0; + } } /** -* Turn on LED -* \param[in] LED LED Name (LED1, LED2) -*/ + * Turn on LED + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_On(LedTypeDef LED) { - //LED_GPIO_PORT[LED]->BRR = LED_GPIO_PIN[LED]; - PIOS_SetLED(LED,1); + // LED_GPIO_PORT[LED]->BRR = LED_GPIO_PIN[LED]; + PIOS_SetLED(LED, 1); } /** -* Turn off LED -* \param[in] LED LED Name (LED1, LED2) -*/ + * Turn off LED + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_Off(LedTypeDef LED) { - //LED_GPIO_PORT[LED]->BSRR = LED_GPIO_PIN[LED]; - PIOS_SetLED(LED,0); + // LED_GPIO_PORT[LED]->BSRR = LED_GPIO_PIN[LED]; + PIOS_SetLED(LED, 0); } /** -* Toggle LED on/off -* \param[in] LED LED Name (LED1, LED2) -*/ + * Toggle LED on/off + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_Toggle(LedTypeDef LED) { - //LED_GPIO_PORT[LED]->ODR ^= LED_GPIO_PIN[LED]; - PIOS_SetLED(LED,LED_GPIO[LED]?0:1); + // LED_GPIO_PORT[LED]->ODR ^= LED_GPIO_PIN[LED]; + PIOS_SetLED(LED, LED_GPIO[LED] ? 0 : 1); } -#endif +#endif /* if defined(PIOS_INCLUDE_LED) */ diff --git a/flight/pios/win32/win32/pios_sdcard.c b/flight/pios/win32/win32/pios_sdcard.c index 78da48e46..a1190499e 100644 --- a/flight/pios/win32/win32/pios_sdcard.c +++ b/flight/pios/win32/win32/pios_sdcard.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_sdcard.c + * @file pios_sdcard.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief Sets up basic system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_SDCARD SDCard Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,186 +34,186 @@ /** -* Initialises SPI pins and peripheral to access MMC/SD Card -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ + * Initialises SPI pins and peripheral to access MMC/SD Card + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ int32_t PIOS_SDCARD_Init(void) { - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Connects to SD Card -* \return < 0 if initialisation sequence failed -*/ + * Connects to SD Card + * \return < 0 if initialisation sequence failed + */ int32_t PIOS_SDCARD_PowerOn(void) { - return 0; /* Status should be 0 if nothing went wrong! */ + return 0; /* Status should be 0 if nothing went wrong! */ } /** -* Disconnects from SD Card -* \return < 0 on errors -*/ + * Disconnects from SD Card + * \return < 0 on errors + */ int32_t PIOS_SDCARD_PowerOff(void) { - return 0; // no error + return 0; // no error } /** -* If SD card was previously available: Checks if the SD Card is still -* available by sending the STATUS command.
-* This takes ca. 10 uS -* -* If SD card was previously not available: Checks if the SD Card is -* available by sending the IDLE command at low speed.
-* This takes ca. 500 uS!
-* Once we got a positive response, SDCARD_PowerOn() will be -* called by this function to initialize the card completely. -* -* Example for Connection/Disconnection detection: -* \code -* // this function is called each second from a low-priority task -* // If multiple tasks are accessing the SD card, add a semaphore/mutex -* // to avoid IO access collisions with other tasks! -* u8 sdcard_available; -* int32_t CheckSDCard(void) -* { -* // check if SD card is available -* // High speed access if the card was previously available -* u8 prev_sdcard_available = sdcard_available; -* sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); -* -* if(sdcard_available && !prev_sdcard_available) { -* // SD Card has been connected -* -* // now it's possible to read/write sectors -* -* } else if( !sdcard_available && prev_sdcard_available ) { -* // SD Card has been disconnected -* -* // here you can notify your application about this state -* } -* -* return 0; // no error -* } -* \endcode -* \param[in] was_available should only be set if the SD card was previously available -* \return 0 if no response from SD Card -* \return 1 if SD card is accessible -*/ + * If SD card was previously available: Checks if the SD Card is still + * available by sending the STATUS command.
+ * This takes ca. 10 uS + * + * If SD card was previously not available: Checks if the SD Card is + * available by sending the IDLE command at low speed.
+ * This takes ca. 500 uS!
+ * Once we got a positive response, SDCARD_PowerOn() will be + * called by this function to initialize the card completely. + * + * Example for Connection/Disconnection detection: + * \code + * // this function is called each second from a low-priority task + * // If multiple tasks are accessing the SD card, add a semaphore/mutex + * // to avoid IO access collisions with other tasks! + * u8 sdcard_available; + * int32_t CheckSDCard(void) + * { + * // check if SD card is available + * // High speed access if the card was previously available + * u8 prev_sdcard_available = sdcard_available; + * sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); + * + * if(sdcard_available && !prev_sdcard_available) { + * // SD Card has been connected + * + * // now it's possible to read/write sectors + * + * } else if( !sdcard_available && prev_sdcard_available ) { + * // SD Card has been disconnected + * + * // here you can notify your application about this state + * } + * + * return 0; // no error + * } + * \endcode + * \param[in] was_available should only be set if the SD card was previously available + * \return 0 if no response from SD Card + * \return 1 if SD card is accessible + */ int32_t PIOS_SDCARD_CheckAvailable(uint8_t was_available) { - return 1; /* 1 = available, 0 = not available. */ + return 1; /* 1 = available, 0 = not available. */ } /** -* Sends command to SD card -* \param[in] cmd SD card command -* \param[in] addr 32bit address -* \param[in] crc precalculated CRC -* \return >= 0x00 if command has been sent successfully (contains received byte) -* \return -1 if no response from SD Card (timeout) -*/ + * Sends command to SD card + * \param[in] cmd SD card command + * \param[in] addr 32bit address + * \param[in] crc precalculated CRC + * \return >= 0x00 if command has been sent successfully (contains received byte) + * \return -1 if no response from SD Card (timeout) + */ int32_t PIOS_SDCARD_SendSDCCmd(uint8_t cmd, uint32_t addr, uint8_t crc) { - return -1; + return -1; } /** -* Reads 512 bytes from selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads 512 bytes from selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_SectorRead(uint32_t sector, uint8_t *buffer) { - return -256; + return -256; } /** -* Writes 512 bytes into selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if write operation not accepted -* \return -258 if timeout during write operation -*/ + * Writes 512 bytes into selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if write operation not accepted + * \return -258 if timeout during write operation + */ int32_t PIOS_SDCARD_SectorWrite(uint32_t sector, uint8_t *buffer) { - return -256; + return -256; } /** -* Reads the CID informations from SD Card -* \param[in] *cid pointer to buffer which holds the CID informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads the CID informations from SD Card + * \param[in] *cid pointer to buffer which holds the CID informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_CIDRead(SDCARDCidTypeDef *cid) { - return -256; + return -256; } /** -* Reads the CSD informations from SD Card -* \param[in] *csd pointer to buffer which holds the CSD informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads the CSD informations from SD Card + * \param[in] *csd pointer to buffer which holds the CSD informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_CSDRead(SDCARDCsdTypeDef *csd) { - return -256; + return -256; } /** -* Attempts to write a startup log to the SDCard -* return 0 No errors -* return -1 Error deleting file -* return -2 Error opening file -* return -3 Error writing file -*/ + * Attempts to write a startup log to the SDCard + * return 0 No errors + * return -1 Error deleting file + * return -2 Error opening file + * return -3 Error writing file + */ int32_t PIOS_SDCARD_StartupLog(void) { - return -3; + return -3; } /** @@ -223,78 +223,78 @@ int32_t PIOS_SDCARD_StartupLog(void) */ int32_t PIOS_SDCARD_IsMounted() { - return 1; + return 1; } /** -* Mounts the file system -* param[in] CreateStartupLog 1 = True, 0 = False -* return 0 No errors -* return -1 SDCard not available -* return -2 Cannot find first partition -* return -3 No volume information -* return -4 Error writing startup log file -*/ + * Mounts the file system + * param[in] CreateStartupLog 1 = True, 0 = False + * return 0 No errors + * return -1 SDCard not available + * return -2 Cannot find first partition + * return -3 No volume information + * return -4 Error writing startup log file + */ int32_t PIOS_SDCARD_MountFS(uint32_t CreateStartupLog) { - return 0; + return 0; } /** -* Mounts the file system -* return Amount of free bytes -*/ + * Mounts the file system + * return Amount of free bytes + */ int32_t PIOS_SDCARD_GetFree(void) { - return 10240; + return 10240; } /** -* Read from file -* return 0 No error -* return -1 DFS_ReadFile failed -* return -2 Less bytes read than expected -*/ -//int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len) -//{ - /* No error */ -// return -1; -//} + * Read from file + * return 0 No error + * return -1 DFS_ReadFile failed + * return -2 Less bytes read than expected + */ +// int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len) +// { +/* No error */ +// return -1; +// } /** -* Read a line from file -* returns Number of bytes read -*/ -//int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len) -//{ -// return -1; -//} + * Read a line from file + * returns Number of bytes read + */ +// int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len) +// { +// return -1; +// } /** -* Copy a file -* WARNING: This will overwrite the destination file even if it exists -* param[in] *Source Path to file to copy -* param[in] *Destination Path to destination file -* return 0 No errors -* return -1 Source file doesn't exist -* return -2 Failed to create destination file -* return -3 DFS_ReadFile failed -* return -4 DFS_WriteFile failed -*/ + * Copy a file + * WARNING: This will overwrite the destination file even if it exists + * param[in] *Source Path to file to copy + * param[in] *Destination Path to destination file + * return 0 No errors + * return -1 Source file doesn't exist + * return -2 Failed to create destination file + * return -3 DFS_ReadFile failed + * return -4 DFS_WriteFile failed + */ int32_t PIOS_SDCARD_FileCopy(char *Source, char *Destination) { - return -2; + return -2; } /** -* Delete a file -* param[in] *Filename File to delete -* return 0 No errors -* return -1 Error deleting file -*/ + * Delete a file + * param[in] *Filename File to delete + * return 0 No errors + * return -1 Error deleting file + */ int32_t PIOS_SDCARD_FileDelete(char *Filename) { - return -1; + return -1; } -#endif +#endif /* if defined(PIOS_INCLUDE_SDCARD) */ diff --git a/flight/pios/win32/win32/pios_servo.c b/flight/pios/win32/win32/pios_servo.c index d0cf530d3..c3b4ba05b 100644 --- a/flight/pios/win32/win32/pios_servo.c +++ b/flight/pios/win32/win32/pios_servo.c @@ -6,25 +6,25 @@ * @brief Code to do set RC servo output * @{ * - * @file pios_servo.c + * @file pios_servo.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RC Servo routines (STM32 dependent) * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,36 +39,33 @@ static volatile uint16_t ServoPosition[PIOS_SERVO_NUM_TIMERS]; /** -* Initialise Servos -*/ + * Initialise Servos + */ void PIOS_Servo_Init(void) -{ -} +{} /** -* Set the servo update rate (Max 500Hz) -* \param[in] onetofour Rate for outputs 1 to 4 (Hz) -* \param[in] fivetoeight Rate for outputs 5 to 8 (Hz) -*/ -void PIOS_Servo_SetHz(const uint16_t * banks, uint8_t num_banks) -{ -} + * Set the servo update rate (Max 500Hz) + * \param[in] onetofour Rate for outputs 1 to 4 (Hz) + * \param[in] fivetoeight Rate for outputs 5 to 8 (Hz) + */ +void PIOS_Servo_SetHz(const uint16_t *banks, uint8_t num_banks) +{} /** -* Set servo position -* \param[in] Servo Servo number (0-7) -* \param[in] Position Servo position in milliseconds -*/ + * Set servo position + * \param[in] Servo Servo number (0-7) + * \param[in] Position Servo position in milliseconds + */ void PIOS_Servo_Set(uint8_t Servo, uint16_t Position) { #ifndef PIOS_ENABLE_DEBUG_PINS - /* Make sure servo exists */ - if (Servo < PIOS_SERVO_NUM_OUTPUTS) { - /* Update the position */ - ServoPosition[Servo] = Position; - - } + /* Make sure servo exists */ + if (Servo < PIOS_SERVO_NUM_OUTPUTS) { + /* Update the position */ + ServoPosition[Servo] = Position; + } #endif // PIOS_ENABLE_DEBUG_PINS } -#endif +#endif /* if defined(PIOS_INCLUDE_SERVO) */ diff --git a/flight/pios/win32/win32/pios_sys.c b/flight/pios/win32/win32/pios_sys.c index 999796407..382a94a87 100644 --- a/flight/pios/win32/win32/pios_sys.c +++ b/flight/pios/win32/win32/pios_sys.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_sys.c + * @file pios_sys.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Sets up basic system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_SYS System Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,149 +38,150 @@ void NVIC_Configuration(void); void SysTick_Handler(void); /* Local Macros */ -#define MEM8(addr) (*((volatile uint8_t *)(addr))) +#define MEM8(addr) (*((volatile uint8_t *)(addr))) /** -* Initialises all system peripherals -*/ + * Initialises all system peripherals + */ void PIOS_SYS_Init(void) { + /** + * stub + */ + printf("PIOS_SYS_Init\n"); - /** - * stub - */ - printf("PIOS_SYS_Init\n"); - - /* Initialise Basic NVIC */ - NVIC_Configuration(); + /* Initialise Basic NVIC */ + NVIC_Configuration(); #if defined(PIOS_INCLUDE_LED) - /* Initialise LEDs */ - PIOS_LED_Init(); + /* Initialise LEDs */ + PIOS_LED_Init(); #endif } /** -* Shutdown PIOS and reset the microcontroller:
-*
    -*
  • Disable all RTOS tasks -*
  • Disable all interrupts -*
  • Turn off all board LEDs -*
  • Reset STM32 -*
-* \return < 0 if reset failed -*/ + * Shutdown PIOS and reset the microcontroller:
+ *
    + *
  • Disable all RTOS tasks + *
  • Disable all interrupts + *
  • Turn off all board LEDs + *
  • Reset STM32 + *
+ * \return < 0 if reset failed + */ int32_t PIOS_SYS_Reset(void) { - /** - * stub - */ - printf("PIOS_SYS_Reset\n"); - /* Disable all RTOS tasks */ + /** + * stub + */ + printf("PIOS_SYS_Reset\n"); + /* Disable all RTOS tasks */ #if defined(PIOS_INCLUDE_FREERTOS) - /* port specific FreeRTOS function to disable tasks (nested) */ - portENTER_CRITICAL(); + /* port specific FreeRTOS function to disable tasks (nested) */ + portENTER_CRITICAL(); #endif - // disable all interrupts - //PIOS_IRQ_Disable(); + // disable all interrupts + // PIOS_IRQ_Disable(); - // turn off all board LEDs - #if (PIOS_LED_NUM == 1) - PIOS_LED_Off(LED1); - #elif (PIOS_LED_NUM == 2) - PIOS_LED_Off(LED1); - PIOS_LED_Off(LED2); - #endif + // turn off all board LEDs + #if (PIOS_LED_NUM == 1) + PIOS_LED_Off(LED1); + #elif (PIOS_LED_NUM == 2) + PIOS_LED_Off(LED1); + PIOS_LED_Off(LED2); + #endif + /* Reset STM32 */ + // RCC_APB2PeriphResetCmd(0xfffffff8, ENABLE); /* MBHP_CORE_STM32: don't reset GPIOA/AF due to USB pins */ + // RCC_APB1PeriphResetCmd(0xff7fffff, ENABLE); /* don't reset USB, so that the connection can survive! */ - /* Reset STM32 */ - //RCC_APB2PeriphResetCmd(0xfffffff8, ENABLE); /* MBHP_CORE_STM32: don't reset GPIOA/AF due to USB pins */ - //RCC_APB1PeriphResetCmd(0xff7fffff, ENABLE); /* don't reset USB, so that the connection can survive! */ + // RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + // RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + // SCB->AIRCR = NVIC_AIRCR_VECTKEY | (1 << NVIC_VECTRESET); + exit(1); - //RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - //RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - //SCB->AIRCR = NVIC_AIRCR_VECTKEY | (1 << NVIC_VECTRESET); - exit(1); + while (1) { + ; + } - while(1); - - /* We will never reach this point */ - return -1; + /* We will never reach this point */ + return -1; } /** -* Returns the serial number as a string -* param[out] str pointer to a string which can store at least 32 digits + zero terminator! -* (24 digits returned for STM32) -* return < 0 if feature not supported -*/ + * Returns the serial number as a string + * param[out] str pointer to a string which can store at least 32 digits + zero terminator! + * (24 digits returned for STM32) + * return < 0 if feature not supported + */ int32_t PIOS_SYS_SerialNumberGet(char *str) { - int i; + int i; - /* Stored in the so called "electronic signature" */ - for(i=0; i<24; ++i) { - //uint8_t b = MEM8(0x1ffff7e8 + (i/2)); - //if( !(i & 1) ) - //b >>= 4; - //b &= 0x0f; + /* Stored in the so called "electronic signature" */ + for (i = 0; i < 24; ++i) { + // uint8_t b = MEM8(0x1ffff7e8 + (i/2)); + // if( !(i & 1) ) + // b >>= 4; + // b &= 0x0f; - //str[i] = ((b > 9) ? ('A'-10) : '0') + b; - str[i]='6'; - } - str[i] = 0; + // str[i] = ((b > 9) ? ('A'-10) : '0') + b; + str[i] = '6'; + } + str[i] = 0; - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Configures Vector Table base location and SysTick -*/ + * Configures Vector Table base location and SysTick + */ void NVIC_Configuration(void) { - /** - * stub - */ - printf("NVIC_Configuration\n"); - /* Set the Vector Table base address as specified in .ld file */ - //NVIC_SetVectorTable(PIOS_NVIC_VECTTAB_FLASH, 0x0); + /** + * stub + */ + printf("NVIC_Configuration\n"); + /* Set the Vector Table base address as specified in .ld file */ + // NVIC_SetVectorTable(PIOS_NVIC_VECTTAB_FLASH, 0x0); - /* 4 bits for Interrupt priorities so no sub priorities */ - //NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + /* 4 bits for Interrupt priorities so no sub priorities */ + // NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); - /* Configure HCLK clock as SysTick clock source. */ - //SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK); + /* Configure HCLK clock as SysTick clock source. */ + // SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK); } #ifdef USE_FULL_ASSERT /** -* Reports the name of the source file and the source line number -* where the assert_param error has occurred. -* \param[in] file pointer to the source file name -* \param[in] line assert_param error line source number -* \retval None -*/ -void assert_failed(uint8_t* file, uint32_t line) + * Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * \param[in] file pointer to the source file name + * \param[in] line assert_param error line source number + * \retval None + */ +void assert_failed(uint8_t *file, uint32_t line) { - /* When serial debugging is implemented, use something like this. */ - /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ - printf("Wrong parameters value: file %s on line %d\r\n", file, line); + /* When serial debugging is implemented, use something like this. */ + /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ + printf("Wrong parameters value: file %s on line %d\r\n", file, line); - /* Setup the LEDs to Alternate */ - PIOS_LED_On(LED1); - PIOS_LED_Off(LED2); + /* Setup the LEDs to Alternate */ + PIOS_LED_On(LED1); + PIOS_LED_Off(LED2); - /* Infinite loop */ - while (1) - { - PIOS_LED_Toggle(LED1); - PIOS_LED_Toggle(LED2); - for(int i = 0; i < 1000000; i++); - } + /* Infinite loop */ + while (1) { + PIOS_LED_Toggle(LED1); + PIOS_LED_Toggle(LED2); + for (int i = 0; i < 1000000; i++) { + ; + } + } } -#endif +#endif /* ifdef USE_FULL_ASSERT */ -#endif +#endif /* if defined(PIOS_INCLUDE_SYS) */ diff --git a/flight/pios/win32/win32/pios_udp.c b/flight/pios/win32/win32/pios_udp.c index ed4342e4c..841156598 100644 --- a/flight/pios/win32/win32/pios_udp.c +++ b/flight/pios/win32/win32/pios_udp.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_udp.c + * @file pios_udp.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief UDP commands. Inits UDPs, controls UDPs & Interupt handlers. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_UDP UDP Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,61 +36,61 @@ /* Provide a COM driver */ const struct pios_com_driver pios_udp_com_driver = { - .set_baud = PIOS_UDP_ChangeBaud, - .tx_nb = PIOS_UDP_TxBufferPutMoreNonBlocking, - .tx = PIOS_UDP_TxBufferPutMore, - .rx = PIOS_UDP_RxBufferGet, - .rx_avail = PIOS_UDP_RxBufferUsed, + .set_baud = PIOS_UDP_ChangeBaud, + .tx_nb = PIOS_UDP_TxBufferPutMoreNonBlocking, + .tx = PIOS_UDP_TxBufferPutMore, + .rx = PIOS_UDP_RxBufferGet, + .rx_avail = PIOS_UDP_RxBufferUsed, }; WSADATA wsaData; -static struct pios_udp_dev * find_udp_dev_by_id (uint8_t udp) +static struct pios_udp_dev *find_udp_dev_by_id(uint8_t udp) { - if (udp >= pios_udp_num_devices) { - /* Undefined UDP port for this board (see pios_board.c) */ - return NULL; - } + if (udp >= pios_udp_num_devices) { + /* Undefined UDP port for this board (see pios_board.c) */ + return NULL; + } - /* Get a handle for the device configuration */ - return &(pios_udp_devs[udp]); + /* Get a handle for the device configuration */ + return &(pios_udp_devs[udp]); } /** -* Open some UDP sockets -*/ + * Open some UDP sockets + */ void PIOS_UDP_Init(void) { - struct pios_udp_dev * udp_dev; - uint8_t i; + struct pios_udp_dev *udp_dev; + uint8_t i; - WSAStartup(MAKEWORD(2, 0), &wsaData); + WSAStartup(MAKEWORD(2, 0), &wsaData); - for (i = 0; i < pios_udp_num_devices; i++) { - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(i); - //PIOS_DEBUG_Assert(udp_dev); + for (i = 0; i < pios_udp_num_devices; i++) { + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(i); + // PIOS_DEBUG_Assert(udp_dev); - /* Clear buffer counters */ - udp_dev->rx.head = udp_dev->rx.tail = udp_dev->rx.size = 0; + /* Clear buffer counters */ + udp_dev->rx.head = udp_dev->rx.tail = udp_dev->rx.size = 0; - /* assign socket */ - udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - memset(&udp_dev->server,0,sizeof(udp_dev->server)); - memset(&udp_dev->client,0,sizeof(udp_dev->client)); - udp_dev->server.sin_family = AF_INET; - udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip); - udp_dev->server.sin_port = htons(udp_dev->cfg->port); - BOOL tmp = TRUE; - setsockopt(udp_dev->socket, SOL_SOCKET, SO_BROADCAST, (char *)&tmp, sizeof(tmp)); - int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server)); - /* use nonblocking IO */ - u_long argp = 1; - ioctlsocket(udp_dev->socket, FIONBIO, &argp); - printf("udp dev %i - socket %i opened - result %i\n",i,udp_dev->socket,res); + /* assign socket */ + udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + memset(&udp_dev->server, 0, sizeof(udp_dev->server)); + memset(&udp_dev->client, 0, sizeof(udp_dev->client)); + udp_dev->server.sin_family = AF_INET; + udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip); + udp_dev->server.sin_port = htons(udp_dev->cfg->port); + BOOL tmp = TRUE; + setsockopt(udp_dev->socket, SOL_SOCKET, SO_BROADCAST, (char *)&tmp, sizeof(tmp)); + int res = bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server, sizeof(udp_dev->server)); + /* use nonblocking IO */ + u_long argp = 1; + ioctlsocket(udp_dev->socket, FIONBIO, &argp); + printf("udp dev %i - socket %i opened - result %i\n", i, udp_dev->socket, res); - /* TODO do some error handling - wait no, we can't - we are void anyway ;) */ - } + /* TODO do some error handling - wait no, we can't - we are void anyway ;) */ + } } /** @@ -98,381 +98,377 @@ void PIOS_UDP_Init(void) */ void PIOS_UDP_Close(void) { - WSACleanup(); + WSACleanup(); } /** -* Changes the baud rate of the UDP peripheral without re-initialising. -* \param[in] udp UDP name (GPS, TELEM, AUX) -* \param[in] baud Requested baud rate -*/ + * Changes the baud rate of the UDP peripheral without re-initialising. + * \param[in] udp UDP name (GPS, TELEM, AUX) + * \param[in] baud Requested baud rate + */ void PIOS_UDP_ChangeBaud(uint8_t udp, uint32_t baud) -{ -} +{} /** -* puts a byte onto the receive buffer -* \param[in] UDP UDP name -* \param[in] b byte which should be put into Rx buffer -* \return 0 if no error -* \return -1 if UDP not available -* \return -2 if buffer full (retry) -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * puts a byte onto the receive buffer + * \param[in] UDP UDP name + * \param[in] b byte which should be put into Rx buffer + * \return 0 if no error + * \return -1 if UDP not available + * \return -2 if buffer full (retry) + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_RxBufferPut(uint8_t udp, uint8_t b) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -1; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return -1; + } - if (udp_dev->rx.size >= sizeof(udp_dev->rx.buf)) { - /* Buffer full (retry) */ - return -2; - } + if (udp_dev->rx.size >= sizeof(udp_dev->rx.buf)) { + /* Buffer full (retry) */ + return -2; + } - /* Copy received byte into receive buffer */ - udp_dev->rx.buf[udp_dev->rx.head++] = b; - if (udp_dev->rx.head >= sizeof(udp_dev->rx.buf)) { - udp_dev->rx.head = 0; - } - udp_dev->rx.size++; + /* Copy received byte into receive buffer */ + udp_dev->rx.buf[udp_dev->rx.head++] = b; + if (udp_dev->rx.head >= sizeof(udp_dev->rx.buf)) { + udp_dev->rx.head = 0; + } + udp_dev->rx.size++; - /* No error */ - return 0; + /* No error */ + return 0; } /** * attempt to receive */ -void PIOS_UDP_RECV(uint8_t udp) { +void PIOS_UDP_RECV(uint8_t udp) +{ + struct pios_udp_dev *udp_dev; + char localbuffer[PIOS_UDP_RX_BUFFER_SIZE]; - struct pios_udp_dev * udp_dev; - char localbuffer[PIOS_UDP_RX_BUFFER_SIZE]; + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); - - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return; - } - - /* use nonblocking IO */ - u_long argp = 1; - ioctlsocket(udp_dev->socket, FIONBIO, &argp); - - /* receive data */ - int received; - udp_dev->clientLength=sizeof(udp_dev->client); - if ((received = recvfrom(udp_dev->socket, - localbuffer, - (PIOS_UDP_RX_BUFFER_SIZE - udp_dev->rx.size), - 0, - (struct sockaddr *) &udp_dev->client, - (int*)&udp_dev->clientLength)) == SOCKET_ERROR) { - - return; - } - /* copy received data to buffer */ - int t; - for (t=0;tsocket, FIONBIO, &argp); + /* receive data */ + int received; + udp_dev->clientLength = sizeof(udp_dev->client); + if ((received = recvfrom(udp_dev->socket, + localbuffer, + (PIOS_UDP_RX_BUFFER_SIZE - udp_dev->rx.size), + 0, + (struct sockaddr *)&udp_dev->client, + (int *)&udp_dev->clientLength)) == SOCKET_ERROR) { + return; + } + /* copy received data to buffer */ + int t; + for (t = 0; t < received; t++) { + PIOS_UDP_RxBufferPut(udp, localbuffer[t]); + } } /** -* Returns number of free bytes in receive buffer -* \param[in] UDP UDP name -* \return UDP number of free bytes -* \return 1: UDP available -* \return 0: UDP not available -*/ + * Returns number of free bytes in receive buffer + * \param[in] UDP UDP name + * \return UDP number of free bytes + * \return 1: UDP available + * \return 0: UDP not available + */ int32_t PIOS_UDP_RxBufferFree(uint8_t udp) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -2; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return -2; + } - /* fill buffer */ - PIOS_UDP_RECV(udp); + /* fill buffer */ + PIOS_UDP_RECV(udp); - return (sizeof(udp_dev->rx.buf) - udp_dev->rx.size); + return sizeof(udp_dev->rx.buf) - udp_dev->rx.size; } /** -* Returns number of used bytes in receive buffer -* \param[in] UDP UDP name -* \return > 0: number of used bytes -* \return 0 if UDP not available -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * Returns number of used bytes in receive buffer + * \param[in] UDP UDP name + * \return > 0: number of used bytes + * \return 0 if UDP not available + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_RxBufferUsed(uint8_t udp) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -2; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return -2; + } - /* fill buffer */ - PIOS_UDP_RECV(udp); + /* fill buffer */ + PIOS_UDP_RECV(udp); - return (udp_dev->rx.size); + return udp_dev->rx.size; } /** -* Gets a byte from the receive buffer -* \param[in] UDP UDP name -* \return -1 if UDP not available -* \return -2 if no new byte available -* \return >= 0: number of received bytes -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * Gets a byte from the receive buffer + * \param[in] UDP UDP name + * \return -1 if UDP not available + * \return -2 if no new byte available + * \return >= 0: number of received bytes + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_RxBufferGet(uint8_t udp) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -2; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return -2; + } - /* fill buffer */ - PIOS_UDP_RECV(udp); + /* fill buffer */ + PIOS_UDP_RECV(udp); - if (!udp_dev->rx.size) { - /* Nothing new in the buffer */ - //printf("PIOS_UDP: nothing new in the buffer\n"); - return -1; - } + if (!udp_dev->rx.size) { + /* Nothing new in the buffer */ + // printf("PIOS_UDP: nothing new in the buffer\n"); + return -1; + } - /* get byte */ - uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail++]; - if (udp_dev->rx.tail >= sizeof(udp_dev->rx.buf)) { - udp_dev->rx.tail = 0; - } - udp_dev->rx.size--; - - //printf("PIOS_UDP: received byte: %c\n", (char) b); + /* get byte */ + uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail++]; + if (udp_dev->rx.tail >= sizeof(udp_dev->rx.buf)) { + udp_dev->rx.tail = 0; + } + udp_dev->rx.size--; - /* Return received byte */ - return b; + // printf("PIOS_UDP: received byte: %c\n", (char) b); + + /* Return received byte */ + return b; } /** -* Returns the next byte of the receive buffer without taking it -* \param[in] UDP UDP name -* \return -1 if UDP not available -* \return -2 if no new byte available -* \return >= 0: number of received bytes -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * Returns the next byte of the receive buffer without taking it + * \param[in] UDP UDP name + * \return -1 if UDP not available + * \return -2 if no new byte available + * \return >= 0: number of received bytes + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_RxBufferPeek(uint8_t udp) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -2; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return -2; + } - /* fill buffer */ - PIOS_UDP_RECV(udp); + /* fill buffer */ + PIOS_UDP_RECV(udp); - if (!udp_dev->rx.size) { - /* Nothing new in the buffer */ - return -1; - } + if (!udp_dev->rx.size) { + /* Nothing new in the buffer */ + return -1; + } - /* get byte */ - uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail]; - - /* Return received byte */ - return b; + /* get byte */ + uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail]; + + /* Return received byte */ + return b; } /** -* returns number of free bytes in transmit buffer -* \param[in] UDP UDP name -* \return number of free bytes -* \return 0 if UDP not available -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * returns number of free bytes in transmit buffer + * \param[in] UDP UDP name + * \return number of free bytes + * \return 0 if UDP not available + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_TxBufferFree(uint8_t udp) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return 0; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return 0; + } - return PIOS_UDP_RX_BUFFER_SIZE; + return PIOS_UDP_RX_BUFFER_SIZE; } /** -* returns number of used bytes in transmit buffer -* \param[in] UDP UDP name -* \return number of used bytes -* \return 0 if UDP not available -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * returns number of used bytes in transmit buffer + * \param[in] UDP UDP name + * \return number of used bytes + * \return 0 if UDP not available + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_TxBufferUsed(uint8_t udp) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); + + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return 0; + } - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ return 0; - } - - return 0; } /** -* puts more than one byte onto the transmit buffer (used for atomic sends) -* \param[in] UDP UDP name -* \param[in] *buffer pointer to buffer to be sent -* \param[in] len number of bytes to be sent -* \return 0 if no error -* \return -1 if UDP not available -* \return -2 if buffer full or cannot get all requested bytes (retry) -* \return -3 if UDP not supported by UDPTxBufferPut Routine -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * puts more than one byte onto the transmit buffer (used for atomic sends) + * \param[in] UDP UDP name + * \param[in] *buffer pointer to buffer to be sent + * \param[in] len number of bytes to be sent + * \return 0 if no error + * \return -1 if UDP not available + * \return -2 if buffer full or cannot get all requested bytes (retry) + * \return -3 if UDP not supported by UDPTxBufferPut Routine + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_TxBufferPutMoreNonBlocking(uint8_t udp, char *buffer, uint16_t len) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -1; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return -1; + } - if (len >= PIOS_UDP_RX_BUFFER_SIZE) { - /* Buffer cannot accept all requested bytes (retry) */ - return -2; - } - /* send data to client - non blocking*/ + if (len >= PIOS_UDP_RX_BUFFER_SIZE) { + /* Buffer cannot accept all requested bytes (retry) */ + return -2; + } + /* send data to client - non blocking*/ - /* use nonblocking IO */ - u_long argp = 1; - ioctlsocket(udp_dev->socket, FIONBIO, &argp); - sendto(udp_dev->socket, buffer, len, 0, - (struct sockaddr *) &udp_dev->client, - (int)sizeof(udp_dev->client)); + /* use nonblocking IO */ + u_long argp = 1; + ioctlsocket(udp_dev->socket, FIONBIO, &argp); + sendto(udp_dev->socket, buffer, len, 0, + (struct sockaddr *)&udp_dev->client, + (int)sizeof(udp_dev->client)); - /* No error */ - return 0; + /* No error */ + return 0; } /** -* puts more than one byte onto the transmit buffer (used for atomic sends)
-* (blocking function) -* \param[in] UDP UDP name -* \param[in] *buffer pointer to buffer to be sent -* \param[in] len number of bytes to be sent -* \return 0 if no error -* \return -1 if UDP not available -* \return -3 if UDP not supported by UDPTxBufferPut Routine -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * puts more than one byte onto the transmit buffer (used for atomic sends)
+ * (blocking function) + * \param[in] UDP UDP name + * \param[in] *buffer pointer to buffer to be sent + * \param[in] len number of bytes to be sent + * \return 0 if no error + * \return -1 if UDP not available + * \return -3 if UDP not supported by UDPTxBufferPut Routine + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_TxBufferPutMore(uint8_t udp, char *buffer, uint16_t len) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -1; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return -1; + } - if (len >= PIOS_UDP_RX_BUFFER_SIZE) { - /* Buffer cannot accept all requested bytes (retry) */ - return -2; - } + if (len >= PIOS_UDP_RX_BUFFER_SIZE) { + /* Buffer cannot accept all requested bytes (retry) */ + return -2; + } - /* send data to client - blocking*/ - /* use blocking IO */ - u_long argp = 1; - ioctlsocket(udp_dev->socket, FIONBIO, &argp); - sendto(udp_dev->socket, buffer, len, 0, - (struct sockaddr *) &udp_dev->client, - sizeof(udp_dev->client)); + /* send data to client - blocking*/ + /* use blocking IO */ + u_long argp = 1; + ioctlsocket(udp_dev->socket, FIONBIO, &argp); + sendto(udp_dev->socket, buffer, len, 0, + (struct sockaddr *)&udp_dev->client, + sizeof(udp_dev->client)); - //printf("PIOS_UDP: sent data\n"); + // printf("PIOS_UDP: sent data\n"); - /* No error */ - return 0; + /* No error */ + return 0; } /** -* puts a byte onto the transmit buffer -* \param[in] UDP UDP name -* \param[in] b byte which should be put into Tx buffer -* \return 0 if no error -* \return -1 if UDP not available -* \return -2 if buffer full (retry) -* \return -3 if UDP not supported by UDPTxBufferPut Routine -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * puts a byte onto the transmit buffer + * \param[in] UDP UDP name + * \param[in] b byte which should be put into Tx buffer + * \return 0 if no error + * \return -1 if UDP not available + * \return -2 if buffer full (retry) + * \return -3 if UDP not supported by UDPTxBufferPut Routine + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_TxBufferPut_NonBlocking(uint8_t udp, char b) { - return PIOS_UDP_TxBufferPutMoreNonBlocking(udp, &b, 1); + return PIOS_UDP_TxBufferPutMoreNonBlocking(udp, &b, 1); } /** -* puts a byte onto the transmit buffer
-* (blocking function) -* \param[in] UDP UDP name -* \param[in] b byte which should be put into Tx buffer -* \return 0 if no error -* \return -1 if UDP not available -* \return -3 if UDP not supported by UDPTxBufferPut Routine -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * puts a byte onto the transmit buffer
+ * (blocking function) + * \param[in] UDP UDP name + * \param[in] b byte which should be put into Tx buffer + * \return 0 if no error + * \return -1 if UDP not available + * \return -3 if UDP not supported by UDPTxBufferPut Routine + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_TxBufferPut(uint8_t udp, char b) { - return PIOS_UDP_TxBufferPutMore(udp, &b, 1); + return PIOS_UDP_TxBufferPutMore(udp, &b, 1); } -#endif +#endif /* if defined(PIOS_INCLUDE_UDP) */ diff --git a/flight/pios/win32/win32/pios_wdg.c b/flight/pios/win32/win32/pios_wdg.c index e3b7701d4..c95907d1d 100644 --- a/flight/pios/win32/win32/pios_wdg.c +++ b/flight/pios/win32/win32/pios_wdg.c @@ -8,12 +8,12 @@ * * @file pios_spi.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Hardware Abstraction Layer for SPI ports of STM32 * @see The GNU Public License (GPL) Version 3 * @notes * - * The PIOS Watchdog provides a HAL to initialize a watchdog + * The PIOS Watchdog provides a HAL to initialize a watchdog * *****************************************************************************/ /* @@ -34,17 +34,17 @@ #include "pios.h" -/** +/** * @brief Initialize the watchdog timer for a specified timeout * - * It is important to note that this function returns the achieved timeout - * for this hardware. For hardware indendence this should be checked when - * scheduling updates. Other hardware dependent details may need to be - * considered such as a window time which sets a minimum update time, - * and this function should return a recommended delay for clearing. + * It is important to note that this function returns the achieved timeout + * for this hardware. For hardware indendence this should be checked when + * scheduling updates. Other hardware dependent details may need to be + * considered such as a window time which sets a minimum update time, + * and this function should return a recommended delay for clearing. * * For the STM32 nominal clock rate is 32 khz, but for the maximum clock rate of - * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is + * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is * set in the watchdog assumes the nominal clock rate, but the delay for FreeRTOS * to use is 75% of the minimal delay. * @@ -52,65 +52,64 @@ * @returns Maximum recommended delay between updates */ void PIOS_WDG_Init() -{ -} +{} /** - * @brief Register a module against the watchdog - * + * @brief Register a module against the watchdog + * * There are two ways to use PIOS WDG: this is for when - * multiple modules must be monitored. In this case they - * must first register against the watchdog system and + * multiple modules must be monitored. In this case they + * must first register against the watchdog system and * only when all of the modules have been updated with the - * watchdog be cleared. Each module must have its own - * bit in the 16 bit + * watchdog be cleared. Each module must have its own + * bit in the 16 bit * * @param[in] flag the bit this module wants to use * @returns True if that bit is unregistered */ -bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) +bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) { - return true; + return true; } /** * @brief Function called by modules to indicate they are still running * - * This function will set this flag in the active flags register (which is + * This function will set this flag in the active flags register (which is * a backup regsiter) and if all the registered flags are set will clear * the watchdog and set only this flag in the backup register * * @param[in] flag the flag to set * @return true if the watchdog cleared, false if flags are pending */ -bool PIOS_WDG_UpdateFlag(uint16_t flag) -{ - return true; +bool PIOS_WDG_UpdateFlag(uint16_t flag) +{ + return true; } -/** +/** * @brief Returns the flags that were set at bootup - * - * This is used for diagnostics, if only one flag not set this + * + * This is used for diagnostics, if only one flag not set this * was likely the module that wasn't running before reset - * + * * @return The active flags register from bootup */ uint16_t PIOS_WDG_GetBootupFlags() { - return (uint16_t) 0xffff; + return (uint16_t)0xffff; } -/** +/** * @brief Returns the currently active flags - * + * * For external monitoring - * + * * @return The active flags register */ uint16_t PIOS_WDG_GetActiveFlags() { - return (uint16_t) 0xffff; + return (uint16_t)0xffff; } /** @@ -119,5 +118,4 @@ uint16_t PIOS_WDG_GetActiveFlags() * This function must be called at the appropriate delay to prevent a reset event occuring */ void PIOS_WDG_Clear(void) -{ -} \ No newline at end of file +{} diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index ad042fada..4b1625e9e 100644 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -9,19 +9,19 @@ * @{ * @brief Defines board specific static initializers for hardware for the CopterControl board. *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,52 +32,54 @@ #include static const struct pios_led pios_leds_cc[] = { - [PIOS_LED_HEARTBEAT] = { - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, }; static const struct pios_led_cfg pios_led_cfg_cc = { - .leds = pios_leds_cc, - .num_leds = NELEMENTS(pios_leds_cc), + .leds = pios_leds_cc, + .num_leds = NELEMENTS(pios_leds_cc), }; static const struct pios_led pios_leds_cc3d[] = { - [PIOS_LED_HEARTBEAT] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - .remap = GPIO_Remap_SWJ_JTAGDisable, - }, + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + .remap = GPIO_Remap_SWJ_JTAGDisable, + }, }; static const struct pios_led_cfg pios_led_cfg_cc3d = { - .leds = pios_leds_cc3d, - .num_leds = NELEMENTS(pios_leds_cc3d), + .leds = pios_leds_cc3d, + .num_leds = NELEMENTS(pios_leds_cc3d), }; -const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (uint32_t board_revision) +const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(uint32_t board_revision) { - switch (board_revision) { - case BOARD_REVISION_CC: return &pios_led_cfg_cc; - case BOARD_REVISION_CC3D: return &pios_led_cfg_cc3d; - default: return NULL; - } + switch (board_revision) { + case BOARD_REVISION_CC: return &pios_led_cfg_cc; + + case BOARD_REVISION_CC3D: return &pios_led_cfg_cc3d; + + default: return NULL; + } } -#endif /* PIOS_INCLUDE_LED */ +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_SPI) @@ -85,349 +87,356 @@ const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (uint32_t board_revisio /* Gyro interface */ void PIOS_SPI_gyro_irq_handler(void); -void DMA1_Channel2_IRQHandler() __attribute__ ((alias ("PIOS_SPI_gyro_irq_handler"))); -void DMA1_Channel3_IRQHandler() __attribute__ ((alias ("PIOS_SPI_gyro_irq_handler"))); +void DMA1_Channel2_IRQHandler() __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); +void DMA1_Channel3_IRQHandler() __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); static const struct pios_spi_cfg pios_spi_gyro_cfg = { - .regs = SPI1, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */ - }, - .use_crc = false, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), - .init = { - .NVIC_IRQChannel = DMA1_Channel2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel2, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel3, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .sclk = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .mosi = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .slave_count = 1, - .ssel = {{ - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }}, + .regs = SPI1, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */ + }, + .use_crc = false, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), + .init = { + .NVIC_IRQChannel = DMA1_Channel2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel2, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel3, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .sclk = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .mosi = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + } + }, }; static uint32_t pios_spi_gyro_id; void PIOS_SPI_gyro_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); } /* Flash/Accel Interface - * - * NOTE: Leave this declared as const data so that it ends up in the + * + * NOTE: Leave this declared as const data so that it ends up in the * .rodata section (ie. Flash) rather than in the .bss section (RAM). */ void PIOS_SPI_flash_accel_irq_handler(void); -void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler"))); -void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler"))); +void DMA1_Channel4_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler"))); +void DMA1_Channel5_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler"))); static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = { - .regs = SPI2, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, - }, - .use_crc = false, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), - .init = { - .NVIC_IRQChannel = DMA1_Channel4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel4, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel5, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .slave_count = 2, - .ssel = {{ - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }},{ - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }}, + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, + }, + .use_crc = false, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .slave_count = 2, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + } + },{ + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + } + }, }; static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = { - .regs = SPI2, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, - }, - .use_crc = false, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), - .init = { - .NVIC_IRQChannel = DMA1_Channel4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel4, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel5, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .slave_count = 2, - .ssel = {{ - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }},{ - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }}, + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, + }, + .use_crc = false, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .slave_count = 2, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + } + },{ + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + } + }, }; static uint32_t pios_spi_flash_accel_id; void PIOS_SPI_flash_accel_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_flash_accel_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_flash_accel_id); } -#endif /* PIOS_INCLUDE_SPI */ +#endif /* PIOS_INCLUDE_SPI */ #if defined(PIOS_INCLUDE_FLASH) #include "pios_flashfs_logfs_priv.h" #include "pios_flash_jedec_priv.h" static const struct flashfs_logfs_cfg flashfs_w25x_cfg = { - .fs_magic = 0x99abcdef, - .total_fs_size = 0x00080000, /* 512K bytes (128 sectors = entire chip) */ - .arena_size = 0x00010000, /* 256 * slot size */ - .slot_size = 0x00000100, /* 256 bytes */ + .fs_magic = 0x99abcdef, + .total_fs_size = 0x00080000, /* 512K bytes (128 sectors = entire chip) */ + .arena_size = 0x00010000, /* 256 * slot size */ + .slot_size = 0x00000100, /* 256 bytes */ - .start_offset = 0, /* start at the beginning of the chip */ - .sector_size = 0x00001000, /* 4K bytes */ - .page_size = 0x00000100, /* 256 bytes */ + .start_offset = 0, /* start at the beginning of the chip */ + .sector_size = 0x00001000, /* 4K bytes */ + .page_size = 0x00000100, /* 256 bytes */ }; - static const struct flashfs_logfs_cfg flashfs_m25p_cfg = { - .fs_magic = 0x99abceef, - .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ - .arena_size = 0x00010000, /* 256 * slot size */ - .slot_size = 0x00000100, /* 256 bytes */ + .fs_magic = 0x99abceef, + .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ + .arena_size = 0x00010000, /* 256 * slot size */ + .slot_size = 0x00000100, /* 256 bytes */ - .start_offset = 0, /* start at the beginning of the chip */ - .sector_size = 0x00010000, /* 64K bytes */ - .page_size = 0x00000100, /* 256 bytes */ + .start_offset = 0, /* start at the beginning of the chip */ + .sector_size = 0x00010000, /* 64K bytes */ + .page_size = 0x00000100, /* 256 bytes */ }; #include "pios_flash.h" -#endif /* PIOS_INCLUDE_FLASH */ +#endif /* PIOS_INCLUDE_FLASH */ /* * ADC system @@ -435,398 +444,399 @@ static const struct flashfs_logfs_cfg flashfs_m25p_cfg = { #if defined(PIOS_INCLUDE_ADC) #include "pios_adc_priv.h" extern void PIOS_ADC_handler(void); -void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); +void DMA1_Channel1_IRQHandler() __attribute__((alias("PIOS_ADC_handler"))); // Remap the ADC DMA handler to this one static const struct pios_adc_cfg pios_adc_cfg = { - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - .irq = { - .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), - .init = { - .NVIC_IRQChannel = DMA1_Channel1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA1_Channel1, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR, - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - } - }, - .half_flag = DMA1_IT_HT1, - .full_flag = DMA1_IT_TC1, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + .irq = { + .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), + .init = { + .NVIC_IRQChannel = DMA1_Channel1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA1_Channel1, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR, + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + } + }, + .half_flag = DMA1_IT_HT1, + .full_flag = DMA1_IT_TC1, }; -void PIOS_ADC_handler() { - PIOS_ADC_DMA_Handler(); +void PIOS_ADC_handler() +{ + PIOS_ADC_DMA_Handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_ADC) */ #include "pios_tim_priv.h" static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, }; static const struct pios_tim_clock_cfg tim_1_cfg = { - .timer = TIM1, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM1, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_2_cfg = { - .timer = TIM2, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM2, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_3_cfg = { - .timer = TIM3, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM3, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_4_cfg = { - .timer = TIM4, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM4, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, }; static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, }; static const struct pios_tim_channel pios_tim_servoport_rcvrport_pins[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - - // Receiver port pins - // S3-S6 inputs are used as outputs in this case - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + + // Receiver port pins + // S3-S6 inputs are used as outputs in this case + { + .timer = TIM3, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, }; static const struct pios_tim_channel pios_tim_ppm_flexi_port = { - .timer = TIM2, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap2_TIM2, + .timer = TIM2, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap2_TIM2, }; #if defined(PIOS_INCLUDE_USART) @@ -834,75 +844,75 @@ static const struct pios_tim_channel pios_tim_ppm_flexi_port = { #include "pios_usart_priv.h" static const struct pios_usart_cfg pios_usart_generic_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, + .regs = USART1, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, }; static const struct pios_usart_cfg pios_usart_generic_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, + .regs = USART3, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, }; #if defined(PIOS_INCLUDE_DSM) @@ -912,100 +922,100 @@ static const struct pios_usart_cfg pios_usart_generic_flexi_cfg = { #include static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, + .regs = USART1, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, }; static const struct pios_dsm_cfg pios_dsm_main_cfg = { - .bind = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, + .bind = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, }; static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, + .regs = USART3, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, }; static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, + .bind = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, }; -#endif /* PIOS_INCLUDE_DSM */ +#endif /* PIOS_INCLUDE_DSM */ #if defined(PIOS_INCLUDE_SBUS) @@ -1015,141 +1025,141 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { #include static const struct pios_usart_cfg pios_usart_sbus_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 100000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_Even, - .USART_StopBits = USART_StopBits_2, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, + .regs = USART1, + .init = { + .USART_BaudRate = 100000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_Even, + .USART_StopBits = USART_StopBits_2, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, }; static const struct pios_sbus_cfg pios_sbus_cfg = { - /* Inverter configuration */ - .inv = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .gpio_clk_func = RCC_APB2PeriphClockCmd, - .gpio_clk_periph = RCC_APB2Periph_GPIOB, - .gpio_inv_enable = Bit_SET, + /* Inverter configuration */ + .inv = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .gpio_clk_func = RCC_APB2PeriphClockCmd, + .gpio_clk_periph = RCC_APB2Periph_GPIOB, + .gpio_inv_enable = Bit_SET, }; -#endif /* PIOS_INCLUDE_SBUS */ +#endif /* PIOS_INCLUDE_SBUS */ /* * HK OSD */ static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, + .regs = USART1, .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, }, - }, }; static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, + .regs = USART3, .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, }, - }, }; -#endif /* PIOS_INCLUDE_USART */ +#endif /* PIOS_INCLUDE_USART */ #if defined(PIOS_INCLUDE_COM) #include "pios_com_priv.h" -#endif /* PIOS_INCLUDE_COM */ +#endif /* PIOS_INCLUDE_COM */ #if defined(PIOS_INCLUDE_RTC) /* @@ -1157,65 +1167,65 @@ static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { */ #include -void PIOS_RTC_IRQ_Handler (void); -void RTC_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler"))); +void PIOS_RTC_IRQ_Handler(void); +void RTC_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); static const struct pios_rtc_cfg pios_rtc_main_cfg = { - .clksrc = RCC_RTCCLKSource_HSE_Div128, - .prescaler = 100, - .irq = { - .init = { - .NVIC_IRQChannel = RTC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .clksrc = RCC_RTCCLKSource_HSE_Div128, + .prescaler = 100, + .irq = { + .init = { + .NVIC_IRQChannel = RTC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; -void PIOS_RTC_IRQ_Handler (void) +void PIOS_RTC_IRQ_Handler(void) { - PIOS_RTC_irq_handler (); + PIOS_RTC_irq_handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_RTC) */ #if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM) -/* - * Servo outputs +/* + * Servo outputs */ #include const struct pios_servo_cfg pios_servo_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_all_pins, - .num_channels = NELEMENTS(pios_tim_servoport_all_pins), + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_all_pins, + .num_channels = NELEMENTS(pios_tim_servoport_all_pins), }; const struct pios_servo_cfg pios_servo_rcvr_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_rcvrport_pins, - .num_channels = NELEMENTS(pios_tim_servoport_rcvrport_pins), + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_rcvrport_pins, + .num_channels = NELEMENTS(pios_tim_servoport_rcvrport_pins), }; -#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ +#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ /* * PPM Inputs @@ -1224,66 +1234,65 @@ const struct pios_servo_cfg pios_servo_rcvr_cfg = { #include const struct pios_ppm_cfg pios_ppm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - /* Use only the first channel for ppm */ - .channels = &pios_tim_rcvrport_all_channels[0], - .num_channels = 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_rcvrport_all_channels[0], + .num_channels = 1, }; -#endif /* PIOS_INCLUDE_PPM */ +#endif /* PIOS_INCLUDE_PPM */ #if defined(PIOS_INCLUDE_PPM_FLEXI) #include const struct pios_ppm_cfg pios_ppm_flexi_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = &pios_tim_ppm_flexi_port, - .num_channels = 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_ppm_flexi_port, + .num_channels = 1, }; -#endif /* PIOS_INCLUDE_PPM_FLEXI */ +#endif /* PIOS_INCLUDE_PPM_FLEXI */ -/* - * PWM Inputs +/* + * PWM Inputs */ #if defined(PIOS_INCLUDE_PWM) #include const struct pios_pwm_cfg pios_pwm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = pios_tim_rcvrport_all_channels, - .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_rcvrport_all_channels, + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), }; const struct pios_pwm_cfg pios_pwm_with_ppm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - /* Leave the first channel for PPM use and use the rest for PWM */ - .channels = &pios_tim_rcvrport_all_channels[1], - .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + /* Leave the first channel for PPM use and use the rest for PWM */ + .channels = &pios_tim_rcvrport_all_channels[1], + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1, }; -#endif - +#endif /* if defined(PIOS_INCLUDE_PWM) */ /* @@ -1293,40 +1302,40 @@ const struct pios_pwm_cfg pios_pwm_with_ppm_cfg = { #include static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = { -{ - .timer = TIM3, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, -}, + { + .timer = TIM3, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, }; const struct pios_hcsr04_cfg pios_hcsr04_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = pios_tim_hcsr04_port_all_channels, - .num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels), - .trigger = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_hcsr04_port_all_channels, + .num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels), + .trigger = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }; -#endif +#endif /* if defined(PIOS_INCLUDE_HCSR04) */ #if defined(PIOS_INCLUDE_I2C) @@ -1338,74 +1347,74 @@ const struct pios_hcsr04_cfg pios_hcsr04_cfg = { void PIOS_I2C_flexi_adapter_ev_irq_handler(void); void PIOS_I2C_flexi_adapter_er_irq_handler(void); -void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexi_adapter_ev_irq_handler"))); -void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexi_adapter_er_irq_handler"))); +void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexi_adapter_ev_irq_handler"))); +void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexi_adapter_er_irq_handler"))); static const struct pios_i2c_adapter_cfg pios_i2c_flexi_adapter_cfg = { - .regs = I2C2, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, + .regs = I2C2, .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, }, - }, }; uint32_t pios_i2c_flexi_adapter_id; void PIOS_I2C_flexi_adapter_ev_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexi_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexi_adapter_id); } void PIOS_I2C_flexi_adapter_er_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexi_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexi_adapter_id); } #endif /* PIOS_INCLUDE_I2C */ #if defined(PIOS_INCLUDE_GCSRCVR) #include "pios_gcsrcvr_priv.h" -#endif /* PIOS_INCLUDE_GCSRCVR */ +#endif /* PIOS_INCLUDE_GCSRCVR */ #if defined(PIOS_INCLUDE_RCVR) #include "pios_rcvr_priv.h" @@ -1416,50 +1425,50 @@ void PIOS_I2C_flexi_adapter_er_irq_handler(void) #include "pios_usb_priv.h" static const struct pios_usb_cfg pios_usb_main_cfg_cc = { - .irq = { - .init = { - .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, }, - .vsense_active_low = false + }, + .vsense = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .vsense_active_low = false }; static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = { - .irq = { - .init = { - .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, }, - .vsense_active_low = false + }, + .vsense = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .vsense_active_low = false }; #include "pios_usb_board_data_priv.h" #include "pios_usb_desc_hid_cdc_priv.h" #include "pios_usb_desc_hid_only_priv.h" -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ #if defined(PIOS_INCLUDE_COM_MSG) @@ -1471,9 +1480,9 @@ static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = { #include const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 2, - .data_rx_ep = 1, - .data_tx_ep = 1, + .data_if = 2, + .data_rx_ep = 1, + .data_tx_ep = 1, }; #endif /* PIOS_INCLUDE_USB_HID */ @@ -1482,22 +1491,21 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { #include const struct pios_usb_rctx_cfg pios_usb_rctx_cfg = { - .data_if = 2, - .data_tx_ep = 1, + .data_if = 2, + .data_tx_ep = 1, }; -#endif /* PIOS_INCLUDE_USB_RCTX */ +#endif /* PIOS_INCLUDE_USB_RCTX */ #if defined(PIOS_INCLUDE_USB_CDC) #include const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 0, - .ctrl_tx_ep = 2, + .ctrl_if = 0, + .ctrl_tx_ep = 2, - .data_if = 1, - .data_rx_ep = 3, - .data_tx_ep = 3, + .data_if = 1, + .data_rx_ep = 3, + .data_tx_ep = 3, }; -#endif /* PIOS_INCLUDE_USB_CDC */ - +#endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/targets/boards/coptercontrol/bootloader/inc/common.h b/flight/targets/boards/coptercontrol/bootloader/inc/common.h index 9010fddd9..e64b89014 100644 --- a/flight/targets/boards/coptercontrol/bootloader/inc/common.h +++ b/flight/targets/boards/coptercontrol/bootloader/inc/common.h @@ -28,10 +28,10 @@ #ifndef COMMON_H_ #define COMMON_H_ -//#include "board.h" +// #include "board.h" typedef enum { - start, keepgoing, + start, keepgoing, } DownloadAction; /**************************************************/ @@ -39,77 +39,77 @@ typedef enum { /**************************************************/ typedef enum { - DFUidle, //0 - uploading, //1 - wrong_packet_received, //2 - too_many_packets, //3 - too_few_packets, //4 - Last_operation_Success, //5 - downloading, //6 - BLidle, //7 - Last_operation_failed, //8 - uploadingStarting, //9 - outsideDevCapabilities, //10 - CRC_Fail,//11 - failed_jump, -//12 + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + BLidle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, +// 12 } DFUStates; /**************************************************/ /* OP_DFU commands */ /**************************************************/ typedef enum { - Reserved, //0 - Req_Capabilities, //1 - Rep_Capabilities, //2 - EnterDFU, //3 - JumpFW, //4 - Reset, //5 - Abort_Operation, //6 - Upload, //7 - Op_END, //8 - Download_Req, //9 - Download, //10 - Status_Request, //11 - Status_Rep -//12 + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep +// 12 } DFUCommands; typedef enum { - High_Density, Medium_Density + High_Density, Medium_Density } DeviceType; /**************************************************/ /* OP_DFU transfer types */ /**************************************************/ typedef enum { - FW, //0 - Descript -//2 + FW, // 0 + Descript +// 2 } DFUTransfer; /**************************************************/ /* OP_DFU transfer port */ /**************************************************/ typedef enum { - Usb, //0 - Serial -//2 + Usb, // 0 + Serial +// 2 } DFUPort; /**************************************************/ /* OP_DFU programable programable HW types */ /**************************************************/ typedef enum { - Self_flash, //0 - Remote_flash_via_spi -//1 + Self_flash, // 0 + Remote_flash_via_spi +// 1 } DFUProgType; /**************************************************/ /* OP_DFU programable sources */ /**************************************************/ -#define USB 0 -#define SPI 1 +#define USB 0 +#define SPI 1 -#define DownloadDelay 100000 +#define DownloadDelay 100000 -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 +#define MAX_DEL_RETRYS 3 +#define MAX_WRI_RETRYS 3 #endif /* COMMON_H_ */ diff --git a/flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h b/flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h index d5e32c891..43182e435 100644 --- a/flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h @@ -3,28 +3,28 @@ * @addtogroup CopterControlBL CopterControl BootLoader * @brief These files contain the code to the CopterControl Bootloader. * @{ - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * In particular, pios_config.h is where you define which PiOS libraries * and features are included in the firmware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h index 47d7d8255..6be4c4b70 100644 --- a/flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h +++ b/flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h @@ -33,13 +33,13 @@ #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 2 +#define PIOS_USB_BOARD_EP_NUM 2 -#include /* struct usb_* */ +#include /* struct usb_* */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_BL) -#define PIOS_USB_BOARD_SN_SUFFIX "+BL" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" /* * The bootloader uses a simplified report structure @@ -49,4 +49,4 @@ */ #define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/coptercontrol/bootloader/main.c b/flight/targets/boards/coptercontrol/bootloader/main.c index d603ea145..4c30b51f4 100644 --- a/flight/targets/boards/coptercontrol/bootloader/main.c +++ b/flight/targets/boards/coptercontrol/bootloader/main.c @@ -8,21 +8,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief This is the file with the main function of the OpenPilot BootLoader * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -59,10 +59,10 @@ uint8_t tempcount = 0; /* Extern variables ----------------------------------------------------------*/ DFUStates DeviceState; -int16_t status = 0; -uint8_t JumpToApp = FALSE; -uint8_t GO_dfu = FALSE; -uint8_t USB_connected = FALSE; +int16_t status = 0; +uint8_t JumpToApp = FALSE; +uint8_t GO_dfu = FALSE; +uint8_t USB_connected = FALSE; uint8_t User_DFU_request = FALSE; static uint8_t mReceive_Buffer[63]; /* Private function prototypes -----------------------------------------------*/ @@ -70,136 +70,149 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); uint8_t processRX(); void jump_to_app(); -int main() { - PIOS_SYS_Init(); - PIOS_Board_Init(); - PIOS_IAP_Init(); - - USB_connected = PIOS_USB_CableConnected(0); +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); - if (PIOS_IAP_CheckRequest() == TRUE) { - PIOS_DELAY_WaitmS(1000); - User_DFU_request = TRUE; - PIOS_IAP_ClearRequest(); - } + USB_connected = PIOS_USB_CableConnected(0); - GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); + if (PIOS_IAP_CheckRequest() == TRUE) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = TRUE; + PIOS_IAP_ClearRequest(); + } - if (GO_dfu == TRUE) { - PIOS_Board_Init(); - if (User_DFU_request == TRUE) - DeviceState = DFUidle; - else - DeviceState = BLidle; - } else - JumpToApp = TRUE; + GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); - uint32_t stopwatch = 0; - uint32_t prev_ticks = PIOS_DELAY_GetuS(); - while (TRUE) { - /* Update the stopwatch */ - uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); - prev_ticks += elapsed_ticks; - stopwatch += elapsed_ticks; + if (GO_dfu == TRUE) { + PIOS_Board_Init(); + if (User_DFU_request == TRUE) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = TRUE; + } - if (JumpToApp == TRUE) - jump_to_app(); + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (TRUE) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; - switch (DeviceState) { - case Last_operation_Success: - case uploadingStarting: - case DFUidle: - period1 = 5000; - sweep_steps1 = 100; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case uploading: - period1 = 5000; - sweep_steps1 = 100; - period2 = 2500; - sweep_steps2 = 50; - break; - case downloading: - period1 = 2500; - sweep_steps1 = 50; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case BLidle: - period1 = 0; - PIOS_LED_On(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - default://error - period1 = 5000; - sweep_steps1 = 100; - period2 = 5000; - sweep_steps2 = 100; - } + if (JumpToApp == TRUE) { + jump_to_app(); + } - if (period1 != 0) { - if (LedPWM(period1, sweep_steps1, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_On(PIOS_LED_HEARTBEAT); + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } - if (period2 != 0) { - if (LedPWM(period2, sweep_steps2, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } - if (stopwatch > 50 * 1000 * 1000) - stopwatch = 0; - if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) - JumpToApp = TRUE; + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } - processRX(); - DataDownload(start); - } + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) { + JumpToApp = TRUE; + } + + processRX(); + DataDownload(start); + } } -void jump_to_app() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; - if (((*(__IO uint32_t*) bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */ - FLASH_Lock(); - RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - _SetCNTR(0); // clear interrupt mask - _SetISTR(0); // clear all requests - JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); - Jump_To_Application = (pFunction) JumpAddress; - /* Initialize user application's Stack Pointer */ - __set_MSP(*(__IO uint32_t*) bdinfo->fw_base); - Jump_To_Application(); - } else { - DeviceState = failed_jump; - return; - } + if (((*(__IO uint32_t *)bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */ + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + _SetCNTR(0); // clear interrupt mask + _SetISTR(0); // clear all requests + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + return; + } } -uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { - uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ - uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ - uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ - if (curr_sweep & 1) { - pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ - } - return ((count % pwm_period) > pwm_duty) ? 1 : 0; + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; } -uint8_t processRX() { - if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { - processComand(mReceive_Buffer); - } - return TRUE; +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return TRUE; } - diff --git a/flight/targets/boards/coptercontrol/bootloader/pios_board.c b/flight/targets/boards/coptercontrol/bootloader/pios_board.c index e24a08c75..bd437df42 100644 --- a/flight/targets/boards/coptercontrol/bootloader/pios_board.c +++ b/flight/targets/boards/coptercontrol/bootloader/pios_board.c @@ -44,66 +44,66 @@ uint32_t pios_com_telem_usb_id; * called from System/openpilot.c */ static bool board_init_complete = false; -void PIOS_Board_Init(void) { - if (board_init_complete) { - return; - } +void PIOS_Board_Init(void) +{ + if (board_init_complete) { + return; + } - /* Enable Prefetch Buffer */ - FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); + /* Enable Prefetch Buffer */ + FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); - /* Flash 2 wait state */ - FLASH_SetLatency(FLASH_Latency_2); + /* Flash 2 wait state */ + FLASH_SetLatency(FLASH_Latency_2); - /* Delay system */ - PIOS_DELAY_Init(); + /* Delay system */ + PIOS_DELAY_Init(); - /* Initialize the PiOS library */ - PIOS_GPIO_Init(); + /* Initialize the PiOS library */ + PIOS_GPIO_Init(); + + const struct pios_board_info *bdinfo = &pios_board_info_blob; - const struct pios_board_info * bdinfo = &pios_board_info_blob; - #if defined(PIOS_INCLUDE_LED) - const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); - PIOS_Assert(led_cfg); - PIOS_LED_Init(led_cfg); -#endif /* PIOS_INCLUDE_LED */ + const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Activate the HID-only USB configuration */ - PIOS_USB_DESC_HID_ONLY_Init(); + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); - uint32_t pios_usb_id; - switch(bdinfo->board_rev) { - case BOARD_REVISION_CC: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); - break; - case BOARD_REVISION_CC3D: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); - break; - default: - PIOS_Assert(0); - } + uint32_t pios_usb_id; + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); + break; + case BOARD_REVISION_CC3D: + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); + break; + default: + PIOS_Assert(0); + } #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { - PIOS_Assert(0); - } -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); // TODO Tirar - board_init_complete = true; + board_init_complete = true; } void PIOS_ADC_DMA_Handler() -{ -} +{} diff --git a/flight/targets/boards/coptercontrol/firmware/coptercontrol.c b/flight/targets/boards/coptercontrol/firmware/coptercontrol.c index 3a7cdeac0..e748f6317 100644 --- a/flight/targets/boards/coptercontrol/firmware/coptercontrol.c +++ b/flight/targets/boards/coptercontrol/firmware/coptercontrol.c @@ -9,25 +9,25 @@ * @brief This is where the OP firmware starts. Those files also define the compile-time * options of the firmware. * @{ - * @file openpilot.c + * @file openpilot.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Sets up and runs main OpenPilot tasks. * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,7 +36,7 @@ #include /* Task Priorities */ -#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) /* Global Variables */ @@ -45,73 +45,73 @@ extern void PIOS_Board_Init(void); extern void Stack_Change(void); /** -* OpenPilot Main function: -* -* Initialize PiOS
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
-* Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) -* If something goes wrong, blink LED1 and LED2 every 100ms -* -*/ + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) + * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ int main() { - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ - /* Brings up System using CMSIS functions, enables the LEDs. */ - PIOS_SYS_Init(); + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); - /* Architecture dependant Hardware and - * core subsystem initialisation - * (see pios_board.c for your arch) - * */ - PIOS_Board_Init(); + /* Architecture dependant Hardware and + * core subsystem initialisation + * (see pios_board.c for your arch) + * */ + PIOS_Board_Init(); #ifdef ERASE_FLASH - PIOS_Flash_Jedec_EraseChip(); + PIOS_Flash_Jedec_EraseChip(); #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Off(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ - while (1) ; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + while (1) { + ; + } #endif - /* Initialize modules */ - MODULE_INITIALISE_ALL + /* Initialize modules */ + MODULE_INITIALISE_ALL + /* swap the stack to use the IRQ stack */ + Stack_Change(); - /* swap the stack to use the IRQ stack */ - Stack_Change(); + /* Start the FreeRTOS scheduler, which should never return. + * + * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls + * (schedules) function files (modules). These functions never return from their + * while loops, which explains why each module has a while(1){} segment. Thus, + * the OpenPilot software actually starts at the vTaskStartScheduler() function, + * even though this is somewhat obscure. + * + * In addition, there are many main() functions in the OpenPilot firmware source tree + * This is because each main() refers to a separate hardware platform. Of course, + * C only allows one main(), so only the relevant main() function is compiled when + * making a specific firmware. + * + */ + vTaskStartScheduler(); - /* Start the FreeRTOS scheduler, which should never return. - * - * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls - * (schedules) function files (modules). These functions never return from their - * while loops, which explains why each module has a while(1){} segment. Thus, - * the OpenPilot software actually starts at the vTaskStartScheduler() function, - * even though this is somewhat obscure. - * - * In addition, there are many main() functions in the OpenPilot firmware source tree - * This is because each main() refers to a separate hardware platform. Of course, - * C only allows one main(), so only the relevant main() function is compiled when - * making a specific firmware. - * - */ - vTaskStartScheduler(); + /* If all is well we will never reach here as the scheduler will now be running. */ - /* If all is well we will never reach here as the scheduler will now be running. */ - - /* Do some indication to user that something bad just happened */ - while (1) { + /* Do some indication to user that something bad just happened */ + while (1) { #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ - PIOS_DELAY_WaitmS(100); - } + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + PIOS_DELAY_WaitmS(100); + } - return 0; + return 0; } /** * @} * @} */ - diff --git a/flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h index 0ed32c74d..fb0a70d05 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h @@ -1,75 +1,74 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ /** - * @addtogroup PIOS PIOS - * @{ - * @addtogroup FreeRTOS FreeRTOS - * @{ - */ + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ /* Notes: We use 5 task priorities */ -#define configUSE_PREEMPTION 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configUSE_MALLOC_FAILED_HOOK 1 -#define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 ) -#define configTICK_RATE_HZ ( ( portTickType ) 1000 ) -#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) -#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 48 ) -#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 53 * 256) ) -#define configMAX_TASK_NAME_LEN ( 16 ) -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configIDLE_SHOULD_YIELD 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_MALLOC_FAILED_HOOK 1 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)48) +#define configTOTAL_HEAP_SIZE ((size_t)(53 * 256)) +#define configMAX_TASK_NAME_LEN (16) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configQUEUE_REGISTRY_SIZE 10 /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ + to exclude the API function. */ -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 0 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) #define CHECK_IRQ_STACK @@ -77,22 +76,23 @@ NVIC value of 255. */ /* Enable run time stats collection */ #ifdef DIAG_TASKS -#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 -#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\ -do {\ -(*(unsigned long *)0xe000edfc) |= (1<<24);/* DEMCR |= DEMCR_TRCENA */\ -(*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\ -} while(0) -#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */ +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ #else -#define configCHECK_FOR_STACK_OVERFLOW 1 +#define configCHECK_FOR_STACK_OVERFLOW 1 #endif /** - * @} - */ + * @} + */ #endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h b/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h index 0dc13c2a8..963014e21 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h @@ -10,19 +10,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h index 84eadbeba..16364d7fa 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file pios_board.h + * @file pios_board.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -28,57 +28,54 @@ #define PIOS_BOARD_H - - -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -//#define PIOS_LED_LED1_GPIO_PORT GPIOC -//#define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12 -//#define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC -//#define PIOS_LED_LED2_GPIO_PORT GPIOC -//#define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13 -//#define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC -#define PIOS_LED_NUM 2 -//#define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT } -//#define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN } -//#define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK } +// ------------------------ +// #define PIOS_LED_LED1_GPIO_PORT GPIOC +// #define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12 +// #define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC +// #define PIOS_LED_LED2_GPIO_PORT GPIOC +// #define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13 +// #define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC +#define PIOS_LED_NUM 2 +// #define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT } +// #define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN } +// #define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK } -//------------------------- +// ------------------------- // COM // // See also pios_board_posix.c -//------------------------- -//#define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +// ------------------------- +// #define PIOS_USART_TX_BUFFER_SIZE 256 +#define PIOS_COM_BUFFER_SIZE 1024 +#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_COM_TELEM_RF 0 -#define PIOS_COM_GPS 1 -#define PIOS_COM_TELEM_USB 2 +#define PIOS_COM_TELEM_RF 0 +#define PIOS_COM_GPS 1 +#define PIOS_COM_TELEM_USB 2 #ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX 3 -#define PIOS_COM_DEBUG PIOS_COM_AUX +#define PIOS_COM_AUX 3 +#define PIOS_COM_DEBUG PIOS_COM_AUX #endif /** * glue macros for file IO * STM32 uses DOSFS for file IO */ -#define PIOS_FOPEN_READ(filename,file) (file=fopen((char*)filename,"r"))==NULL +#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL -#define PIOS_FOPEN_WRITE(filename,file) (file=fopen((char*)filename,"w"))==NULL +#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL -#define PIOS_FREAD(file,bufferadr,length,resultadr) (*resultadr=fread((uint8_t*)bufferadr,1,length,*file)) != length +#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length -#define PIOS_FWRITE(file,bufferadr,length,resultadr) *resultadr=fwrite((uint8_t*)bufferadr,1,length,*file) +#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) +#define PIOS_FCLOSE(file) fclose(file) -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char*)filename) +#define PIOS_FUNLINK(file) unlink((char *)filename) #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 9ac872997..1ad2a4e54 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -141,26 +141,26 @@ /* #define PIOS_QUATERNION_STABILIZATION */ /* Performance counters */ -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 /* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 220 -#define HEAP_LIMIT_CRITICAL 40 -#define IRQSTACK_LIMIT_WARNING 100 -#define IRQSTACK_LIMIT_CRITICAL 60 -#define CPULOAD_LIMIT_WARNING 85 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 220 +#define HEAP_LIMIT_CRITICAL 40 +#define IRQSTACK_LIMIT_WARNING 100 +#define IRQSTACK_LIMIT_CRITICAL 60 +#define CPULOAD_LIMIT_WARNING 85 +#define CPULOAD_LIMIT_CRITICAL 95 /* Task stack sizes */ -#define PIOS_ACTUATOR_STACK_SIZE 1020 -#define PIOS_MANUAL_STACK_SIZE 800 -#define PIOS_SYSTEM_STACK_SIZE 660 -#define PIOS_STABILIZATION_STACK_SIZE 524 -#define PIOS_TELEM_STACK_SIZE 500 -#define PIOS_EVENTDISPATCHER_STACK_SIZE 130 +#define PIOS_ACTUATOR_STACK_SIZE 1020 +#define PIOS_MANUAL_STACK_SIZE 800 +#define PIOS_SYSTEM_STACK_SIZE 660 +#define PIOS_STABILIZATION_STACK_SIZE 524 +#define PIOS_TELEM_STACK_SIZE 500 +#define PIOS_EVENTDISPATCHER_STACK_SIZE 130 /* This can't be too high to stop eventdispatcher thread overflowing */ -#define PIOS_EVENTDISAPTCHER_QUEUE 10 +#define PIOS_EVENTDISAPTCHER_QUEUE 10 /* Revolution series */ /* #define REVOLUTION */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h index 49f94d7f0..eda0a2511 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,7 +41,7 @@ /* Defaults for Logging */ -#define LOG_FILENAME "PIOS.LOG" -#define STARTUP_LOG_ENABLED 1 +#define LOG_FILENAME "PIOS.LOG" +#define STARTUP_LOG_ENABLED 1 #endif /* PIOS_CONFIG_POSIX_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h index 63439dcbb..248ef57d7 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h @@ -35,12 +35,12 @@ #define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 4 +#define PIOS_USB_BOARD_EP_NUM 4 -#include /* USB_* macros */ +#include /* USB_* macros */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_FW) -#define PIOS_USB_BOARD_SN_SUFFIX "+FW" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index 94f4d9be0..2668c6b95 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -9,19 +9,19 @@ * @{ * @brief Defines board specific static initializers for hardware for the CopterControl board. *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -49,23 +49,23 @@ */ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 32 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 12 +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 32 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 12 -#define PIOS_COM_GPS_RX_BUF_LEN 32 +#define PIOS_COM_GPS_RX_BUF_LEN 32 -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 -#define PIOS_COM_HKOSD_TX_BUF_LEN 22 +#define PIOS_COM_HKOSD_TX_BUF_LEN 22 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) #define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 uint32_t pios_com_debug_id; -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ uint32_t pios_com_telem_rf_id; uint32_t pios_com_telem_usb_id; @@ -83,49 +83,49 @@ uint32_t pios_usb_rctx_id; #include "pios_mpu6000.h" #include "pios_mpu6000_config.h" static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { - .vector = PIOS_MPU6000_IRQHandler, - .line = EXTI_Line3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line3, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_MPU6000_IRQHandler, + .line = EXTI_Line3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line3, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { - .exti_cfg = &pios_exti_mpu6000_cfg, - .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, - // Clock at 8 khz, downsampled by 16 for 500 Hz - .Smpl_rate_div_no_dlp = 15, - // Clock at 1 khz, downsampled by 2 for 500 Hz - .Smpl_rate_div_dlp = 1, - .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, - .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, - .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, - .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, - .accel_range = PIOS_MPU6000_ACCEL_8G, - .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ, - .orientation = PIOS_MPU6000_TOP_180DEG + .exti_cfg = &pios_exti_mpu6000_cfg, + .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, + // Clock at 8 khz, downsampled by 16 for 500 Hz + .Smpl_rate_div_no_dlp = 15, + // Clock at 1 khz, downsampled by 2 for 500 Hz + .Smpl_rate_div_dlp = 1, + .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, + .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, + .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_TOP_180DEG }; #endif /* PIOS_INCLUDE_MPU6000 */ @@ -135,741 +135,739 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { * called from System/openpilot.c */ int32_t init_test; -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); + const struct pios_board_info *bdinfo = &pios_board_info_blob; - const struct pios_board_info * bdinfo = &pios_board_info_blob; - #if defined(PIOS_INCLUDE_LED) - const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); - PIOS_Assert(led_cfg); - PIOS_LED_Init(led_cfg); -#endif /* PIOS_INCLUDE_LED */ + const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_SPI) - /* Set up the SPI interface to the serial flash */ + /* Set up the SPI interface to the serial flash */ - switch(bdinfo->board_rev) { - case BOARD_REVISION_CC: - if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) { - PIOS_Assert(0); - } - break; - case BOARD_REVISION_CC3D: - if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) { - PIOS_Assert(0); - } - break; - default: - PIOS_Assert(0); - } + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) { + PIOS_Assert(0); + } + break; + case BOARD_REVISION_CC3D: + if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) { + PIOS_Assert(0); + } + break; + default: + PIOS_Assert(0); + } #endif - uintptr_t flash_id; - uintptr_t fs_id; - switch(bdinfo->board_rev) { - case BOARD_REVISION_CC: - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 1)) { - PIOS_DEBUG_Assert(0); - } - if (PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_w25x_cfg, &pios_jedec_flash_driver, flash_id)) { - PIOS_DEBUG_Assert(0); - } - break; - case BOARD_REVISION_CC3D: - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 0)) { - PIOS_DEBUG_Assert(0); - } - if (PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id)) { - PIOS_DEBUG_Assert(0); - } - break; - default: - PIOS_DEBUG_Assert(0); - } + uintptr_t flash_id; + uintptr_t fs_id; + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 1)) { + PIOS_DEBUG_Assert(0); + } + if (PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_w25x_cfg, &pios_jedec_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } + break; + case BOARD_REVISION_CC3D: + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 0)) { + PIOS_DEBUG_Assert(0); + } + if (PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } + break; + default: + PIOS_DEBUG_Assert(0); + } - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); #if defined(PIOS_INCLUDE_RTC) - /* Initialize the real-time clock and its associated tick */ - PIOS_RTC_Init(&pios_rtc_main_cfg); + /* Initialize the real-time clock and its associated tick */ + PIOS_RTC_Init(&pios_rtc_main_cfg); #endif - PIOS_IAP_Init(); - // check for safe mode commands from gcs - if(PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && - PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && - PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) - { - PIOS_FLASHFS_Format(fs_id); - PIOS_IAP_WriteBootCmd(0,0); - PIOS_IAP_WriteBootCmd(1,0); - PIOS_IAP_WriteBootCmd(2,0); - } - - HwSettingsInitialize(); + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + PIOS_FLASHFS_Format(fs_id); + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } + + HwSettingsInitialize(); #ifndef ERASE_FLASH - /* Initialize watchdog as early as possible to catch faults during init */ - PIOS_WDG_Init(); + /* Initialize watchdog as early as possible to catch faults during init */ + PIOS_WDG_Init(); #endif - /* Initialize the alarms library */ - AlarmsInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Check for repeated boot failures */ - uint16_t boot_count = PIOS_IAP_ReadBootCount(); - if (boot_count < 3) { - PIOS_IAP_WriteBootCount(++boot_count); - AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); - } else { - /* Too many failed boot attempts, force hwsettings to defaults */ - HwSettingsSetDefaults(HwSettingsHandle(), 0); - AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); - } + /* Check for repeated boot failures */ + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); - /* Set up pulse timers */ - PIOS_TIM_InitClock(&tim_1_cfg); - PIOS_TIM_InitClock(&tim_2_cfg); - PIOS_TIM_InitClock(&tim_3_cfg); - PIOS_TIM_InitClock(&tim_4_cfg); + /* Set up pulse timers */ + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_4_cfg); #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; + /* Flags to determine if various USB interfaces are advertised */ + bool usb_hid_present = false; + bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; + usb_cdc_present = true; #else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; #endif - uint32_t pios_usb_id; - - switch(bdinfo->board_rev) { - case BOARD_REVISION_CC: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); - break; - case BOARD_REVISION_CC3D: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); - break; - default: - PIOS_Assert(0); - } + uint32_t pios_usb_id; + + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); + break; + case BOARD_REVISION_CC3D: + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); + break; + default: + PIOS_Assert(0); + } #if defined(PIOS_INCLUDE_USB_CDC) - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); + uint8_t hwsettings_usb_vcpport; + /* Configure the USB VCP port */ + HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } + if (!usb_cdc_present) { + /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ + hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; + } - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: + switch (hwsettings_usb_vcpport) { + case HWSETTINGS_USB_VCPPORT_DISABLED: + break; + case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: + { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: + { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, + tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ + { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + NULL, 0, + tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_COM */ + break; + } +#endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); + /* Configure the usb HID port */ + uint8_t hwsettings_usb_hidport; + HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } + if (!usb_hid_present) { + /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ + hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; + } - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: + switch (hwsettings_usb_hidport) { + case HWSETTINGS_USB_HIDPORT_DISABLED: + break; + case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER: + { + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER: #if defined(PIOS_INCLUDE_USB_RCTX) - { - if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_USB_RCTX */ - break; - } + { + if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_USB_RCTX */ + break; + } -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ - /* Configure the main IO port */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); - uint8_t hwsettings_cc_mainport; - HwSettingsCC_MainPortGet(&hwsettings_cc_mainport); + /* Configure the main IO port */ + uint8_t hwsettings_DSMxBind; + HwSettingsDSMxBindGet(&hwsettings_DSMxBind); + uint8_t hwsettings_cc_mainport; + HwSettingsCC_MainPortGet(&hwsettings_cc_mainport); - switch (hwsettings_cc_mainport) { - case HWSETTINGS_CC_MAINPORT_DISABLED: - break; - case HWSETTINGS_CC_MAINPORT_TELEMETRY: + switch (hwsettings_cc_mainport) { + case HWSETTINGS_CC_MAINPORT_DISABLED: + break; + case HWSETTINGS_CC_MAINPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { - PIOS_Assert(0); - } + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { + PIOS_Assert(0); + } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_TELEMETRY_RF */ - break; - case HWSETTINGS_CC_MAINPORT_SBUS: -#if defined(PIOS_INCLUDE_SBUS) - { - uint32_t pios_usart_sbus_id; - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_id; - if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; - - } -#endif /* PIOS_INCLUDE_SBUS */ - break; - case HWSETTINGS_CC_MAINPORT_GPS: -#if defined(PIOS_INCLUDE_GPS) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { - PIOS_Assert(0); - } - - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_GPS */ - break; - case HWSETTINGS_CC_MAINPORT_DSM2: - case HWSETTINGS_CC_MAINPORT_DSMX10BIT: - case HWSETTINGS_CC_MAINPORT_DSMX11BIT: -#if defined(PIOS_INCLUDE_DSM) - { - enum pios_dsm_proto proto; - switch (hwsettings_cc_mainport) { - case HWSETTINGS_CC_MAINPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_CC_MAINPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_CC_MAINPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } - - uint32_t pios_usart_dsm_id; - if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, - &pios_dsm_main_cfg, - &pios_usart_com_driver, - pios_usart_dsm_id, - proto, 0)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT] = pios_dsm_rcvr_id; - } -#endif /* PIOS_INCLUDE_DSM */ - break; - case HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { - PIOS_Assert(0); - } - - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_CC_MAINPORT_COMBRIDGE: - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { - PIOS_Assert(0); - } - - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } - break; - case HWSETTINGS_CC_MAINPORT_OSDHK: - { - uint32_t pios_usart_hkosd_id; - if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_main_cfg)) { - PIOS_Assert(0); - } - - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_HKOSD_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id, - NULL, 0, - tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } - break; - } - - /* Configure the flexi port */ - uint8_t hwsettings_cc_flexiport; - HwSettingsCC_FlexiPortGet(&hwsettings_cc_flexiport); - - switch (hwsettings_cc_flexiport) { - case HWSETTINGS_CC_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_CC_FLEXIPORT_TELEMETRY: -#if defined(PIOS_INCLUDE_TELEMETRY_RF) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ - break; - case HWSETTINGS_CC_FLEXIPORT_COMBRIDGE: - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { - PIOS_Assert(0); - } + break; + case HWSETTINGS_CC_MAINPORT_SBUS: +#if defined(PIOS_INCLUDE_SBUS) + { + uint32_t pios_usart_sbus_id; + if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { + PIOS_Assert(0); + } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } - break; - case HWSETTINGS_CC_FLEXIPORT_GPS: + uint32_t pios_sbus_id; + if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { + PIOS_Assert(0); + } + + uint32_t pios_sbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; + } +#endif /* PIOS_INCLUDE_SBUS */ + break; + case HWSETTINGS_CC_MAINPORT_GPS: #if defined(PIOS_INCLUDE_GPS) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_GPS */ - break; - case HWSETTINGS_CC_FLEXIPORT_PPM: -#if defined(PIOS_INCLUDE_PPM_FLEXI) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { + PIOS_Assert(0); + } - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PPM_FLEXI */ - break; - case HWSETTINGS_CC_FLEXIPORT_DSM2: - case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT: - case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT: + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_GPS */ + break; + case HWSETTINGS_CC_MAINPORT_DSM2: + case HWSETTINGS_CC_MAINPORT_DSMX10BIT: + case HWSETTINGS_CC_MAINPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) - { - enum pios_dsm_proto proto; - switch (hwsettings_cc_flexiport) { - case HWSETTINGS_CC_FLEXIPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } + { + enum pios_dsm_proto proto; + switch (hwsettings_cc_mainport) { + case HWSETTINGS_CC_MAINPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_CC_MAINPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_CC_MAINPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } - uint32_t pios_usart_dsm_id; - if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_flexi_cfg)) { - PIOS_Assert(0); - } + uint32_t pios_usart_dsm_id; + if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_main_cfg)) { + PIOS_Assert(0); + } - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, - &pios_dsm_flexi_cfg, - &pios_usart_com_driver, - pios_usart_dsm_id, - proto, hwsettings_DSMxBind)) { - PIOS_Assert(0); - } + uint32_t pios_dsm_id; + if (PIOS_DSM_Init(&pios_dsm_id, + &pios_dsm_main_cfg, + &pios_usart_com_driver, + pios_usart_dsm_id, + proto, 0)) { + PIOS_Assert(0); + } - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT] = pios_dsm_rcvr_id; - } -#endif /* PIOS_INCLUDE_DSM */ - break; - case HWSETTINGS_CC_FLEXIPORT_DEBUGCONSOLE: + uint32_t pios_dsm_rcvr_id; + if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT] = pios_dsm_rcvr_id; + } +#endif /* PIOS_INCLUDE_DSM */ + break; + case HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { - PIOS_Assert(0); - } + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { + PIOS_Assert(0); + } - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_CC_FLEXIPORT_I2C: + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id, + NULL, 0, + tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_CC_MAINPORT_COMBRIDGE: + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, + tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + break; + case HWSETTINGS_CC_MAINPORT_OSDHK: + { + uint32_t pios_usart_hkosd_id; + if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_main_cfg)) { + PIOS_Assert(0); + } + + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_HKOSD_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id, + NULL, 0, + tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + break; + } + + /* Configure the flexi port */ + uint8_t hwsettings_cc_flexiport; + HwSettingsCC_FlexiPortGet(&hwsettings_cc_flexiport); + + switch (hwsettings_cc_flexiport) { + case HWSETTINGS_CC_FLEXIPORT_DISABLED: + break; + case HWSETTINGS_CC_FLEXIPORT_TELEMETRY: +#if defined(PIOS_INCLUDE_TELEMETRY_RF) + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_TELEMETRY_RF */ + break; + case HWSETTINGS_CC_FLEXIPORT_COMBRIDGE: + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, + tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + break; + case HWSETTINGS_CC_FLEXIPORT_GPS: +#if defined(PIOS_INCLUDE_GPS) + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_GPS */ + break; + case HWSETTINGS_CC_FLEXIPORT_PPM: +#if defined(PIOS_INCLUDE_PPM_FLEXI) + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); + + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PPM_FLEXI */ + break; + case HWSETTINGS_CC_FLEXIPORT_DSM2: + case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT: + case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT: +#if defined(PIOS_INCLUDE_DSM) + { + enum pios_dsm_proto proto; + switch (hwsettings_cc_flexiport) { + case HWSETTINGS_CC_FLEXIPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } + + uint32_t pios_usart_dsm_id; + if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_flexi_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_id; + if (PIOS_DSM_Init(&pios_dsm_id, + &pios_dsm_flexi_cfg, + &pios_usart_com_driver, + pios_usart_dsm_id, + proto, hwsettings_DSMxBind)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_rcvr_id; + if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT] = pios_dsm_rcvr_id; + } +#endif /* PIOS_INCLUDE_DSM */ + break; + case HWSETTINGS_CC_FLEXIPORT_DEBUGCONSOLE: +#if defined(PIOS_INCLUDE_COM) +#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { + PIOS_Assert(0); + } + + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id, + NULL, 0, + tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_CC_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) - { - if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_I2C */ - break; - case HWSETTINGS_CC_FLEXIPORT_OSDHK: - { - uint32_t pios_usart_hkosd_id; - if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_flexi_cfg)) { - PIOS_Assert(0); - } + { + if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_I2C */ + break; + case HWSETTINGS_CC_FLEXIPORT_OSDHK: + { + uint32_t pios_usart_hkosd_id; + if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_flexi_cfg)) { + PIOS_Assert(0); + } - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_HKOSD_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id, - NULL, 0, - tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } - break; - } + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_HKOSD_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id, + NULL, 0, + tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + break; + } - /* Configure the rcvr port */ - uint8_t hwsettings_rcvrport; - HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport); + /* Configure the rcvr port */ + uint8_t hwsettings_rcvrport; + HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport); - switch (hwsettings_rcvrport) { - case HWSETTINGS_CC_RCVRPORT_DISABLED: + switch (hwsettings_rcvrport) { + case HWSETTINGS_CC_RCVRPORT_DISABLED: #if defined(PIOS_INCLUDE_HCSR04) - { - uint32_t pios_hcsr04_id; - PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg); - } + { + uint32_t pios_hcsr04_id; + PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg); + } #endif - break; - case HWSETTINGS_CC_RCVRPORT_PWM: + break; + case HWSETTINGS_CC_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) - { - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); + { + uint32_t pios_pwm_id; + PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PWM */ - break; - case HWSETTINGS_CC_RCVRPORT_PPM: - case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS: + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PWM */ + break; + case HWSETTINGS_CC_RCVRPORT_PPM: + case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS: #if defined(PIOS_INCLUDE_PPM) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PPM */ - break; - case HWSETTINGS_CC_RCVRPORT_PPMPWM: - /* This is a combination of PPM and PWM inputs */ + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PPM */ + break; + case HWSETTINGS_CC_RCVRPORT_PPMPWM: + /* This is a combination of PPM and PWM inputs */ #if defined(PIOS_INCLUDE_PPM) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PPM */ + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PPM */ #if defined(PIOS_INCLUDE_PWM) - { - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg); + { + uint32_t pios_pwm_id; + PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg); - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PWM */ - break; - } + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PWM */ + break; + } #if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ + GCSReceiverInitialize(); + uint32_t pios_gcsrcvr_id; + PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_GCSRCVR */ - /* Remap AFIO pin for PB4 (Servo 5 Out)*/ - GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); + /* Remap AFIO pin for PB4 (Servo 5 Out)*/ + GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE); #ifndef PIOS_ENABLE_DEBUG_PINS - switch (hwsettings_rcvrport) { - case HWSETTINGS_CC_RCVRPORT_DISABLED: - case HWSETTINGS_CC_RCVRPORT_PWM: - case HWSETTINGS_CC_RCVRPORT_PPM: - case HWSETTINGS_CC_RCVRPORT_PPMPWM: - PIOS_Servo_Init(&pios_servo_cfg); - break; - case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS: - case HWSETTINGS_CC_RCVRPORT_OUTPUTS: - PIOS_Servo_Init(&pios_servo_rcvr_cfg); - break; - } + switch (hwsettings_rcvrport) { + case HWSETTINGS_CC_RCVRPORT_DISABLED: + case HWSETTINGS_CC_RCVRPORT_PWM: + case HWSETTINGS_CC_RCVRPORT_PPM: + case HWSETTINGS_CC_RCVRPORT_PPMPWM: + PIOS_Servo_Init(&pios_servo_cfg); + break; + case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS: + case HWSETTINGS_CC_RCVRPORT_OUTPUTS: + PIOS_Servo_Init(&pios_servo_rcvr_cfg); + break; + } #else - PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); -#endif /* PIOS_ENABLE_DEBUG_PINS */ + PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); +#endif /* PIOS_ENABLE_DEBUG_PINS */ - switch(bdinfo->board_rev) { - case BOARD_REVISION_CC: - // Revision 1 with invensense gyros, start the ADC + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + // Revision 1 with invensense gyros, start the ADC #if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); + PIOS_ADC_Init(&pios_adc_cfg); #endif #if defined(PIOS_INCLUDE_ADXL345) - PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0); + PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0); #endif - break; - case BOARD_REVISION_CC3D: - // Revision 2 with MPU6000 gyros, start a SPI interface and connect to it - GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); + break; + case BOARD_REVISION_CC3D: + // Revision 2 with MPU6000 gyros, start a SPI interface and connect to it + GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); #if defined(PIOS_INCLUDE_MPU6000) - // Set up the SPI interface to the serial flash - if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { - PIOS_Assert(0); - } - PIOS_MPU6000_Init(pios_spi_gyro_id,0,&pios_mpu6000_cfg); - PIOS_MPU6000_CONFIG_Configure(); - init_test = PIOS_MPU6000_Test(); + // Set up the SPI interface to the serial flash + if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { + PIOS_Assert(0); + } + PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); + PIOS_MPU6000_CONFIG_Configure(); + init_test = PIOS_MPU6000_Test(); #endif /* PIOS_INCLUDE_MPU6000 */ - break; - default: - PIOS_Assert(0); - } + break; + default: + PIOS_Assert(0); + } - PIOS_GPIO_Init(); + PIOS_GPIO_Init(); - /* Make sure we have at least one telemetry link configured or else fail initialization */ - PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); + /* Make sure we have at least one telemetry link configured or else fail initialization */ + PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); } /** diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c b/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c index f63cfa7fd..5750bed21 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c @@ -7,19 +7,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,43 +33,42 @@ * initializes all the core systems on this specific hardware * called from System/openpilot.c */ -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Initialize the alarms library */ - AlarmsInitialize(); + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } - - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); - - /* Initialize the PiOS library */ - PIOS_COM_Init(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize the PiOS library */ + PIOS_COM_Init(); } const struct pios_udp_cfg pios_udp0_cfg = { - .ip = "0.0.0.0", - .port = 9000, + .ip = "0.0.0.0", + .port = 9000, }; const struct pios_udp_cfg pios_udp1_cfg = { - .ip = "0.0.0.0", - .port = 9001, + .ip = "0.0.0.0", + .port = 9001, }; const struct pios_udp_cfg pios_udp2_cfg = { - .ip = "0.0.0.0", - .port = 9002, + .ip = "0.0.0.0", + .port = 9002, }; #ifdef PIOS_COM_AUX @@ -77,8 +76,8 @@ const struct pios_udp_cfg pios_udp2_cfg = { * AUX USART */ const struct pios_udp_cfg pios_udp3_cfg = { - .ip = "0.0.0.0", - .port = 9003, + .ip = "0.0.0.0", + .port = 9003, }; #endif @@ -86,23 +85,23 @@ const struct pios_udp_cfg pios_udp3_cfg = { * Board specific number of devices. */ struct pios_udp_dev pios_udp_devs[] = { -#define PIOS_UDP_TELEM 0 - { - .cfg = &pios_udp0_cfg, - }, -#define PIOS_UDP_GPS 1 - { - .cfg = &pios_udp1_cfg, - }, -#define PIOS_UDP_LOCAL 2 - { - .cfg = &pios_udp2_cfg, - }, +#define PIOS_UDP_TELEM 0 + { + .cfg = &pios_udp0_cfg, + }, +#define PIOS_UDP_GPS 1 + { + .cfg = &pios_udp1_cfg, + }, +#define PIOS_UDP_LOCAL 2 + { + .cfg = &pios_udp2_cfg, + }, #ifdef PIOS_COM_AUX -#define PIOS_UDP_AUX 3 - { - .cfg = &pios_udp3_cfg, - }, +#define PIOS_UDP_AUX 3 + { + .cfg = &pios_udp3_cfg, + }, #endif }; @@ -119,23 +118,23 @@ extern const struct pios_com_driver pios_serial_com_driver; extern const struct pios_com_driver pios_udp_com_driver; struct pios_com_dev pios_com_devs[] = { - { - .id = PIOS_UDP_TELEM, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_GPS, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_LOCAL, - .driver = &pios_udp_com_driver, - }, + { + .id = PIOS_UDP_TELEM, + .driver = &pios_udp_com_driver, + }, + { + .id = PIOS_UDP_GPS, + .driver = &pios_udp_com_driver, + }, + { + .id = PIOS_UDP_LOCAL, + .driver = &pios_udp_com_driver, + }, #ifdef PIOS_COM_AUX - { - .id = PIOS_UDP_AUX, - .driver = &pios_udp_com_driver, - }, + { + .id = PIOS_UDP_AUX, + .driver = &pios_udp_com_driver, + }, #endif }; diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index 9b19be3a7..0408ecfa8 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -26,45 +26,45 @@ #ifndef PIOS_BOARD_H #define PIOS_BOARD_H -//------------------------ +// ------------------------ // Timers and Channels Used -//------------------------ +// ------------------------ /* - Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 - ------+-----------+-----------+-----------+---------- - TIM1 | Servo 4 | | | - TIM2 | RC In 5 | RC In 6 | Servo 6 | - TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 - TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 - ------+-----------+-----------+-----------+---------- + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | Servo 4 | | | + TIM2 | RC In 5 | RC In 6 | Servo 6 | + TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 + TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 + ------+-----------+-----------+-----------+---------- */ -//------------------------ +// ------------------------ // DMA Channels Used -//------------------------ -/* Channel 1 - */ -/* Channel 2 - */ -/* Channel 3 - */ -/* Channel 4 - */ -/* Channel 5 - */ -/* Channel 6 - */ -/* Channel 7 - */ -/* Channel 8 - */ -/* Channel 9 - */ -/* Channel 10 - */ -/* Channel 11 - */ -/* Channel 12 - */ +// ------------------------ +/* Channel 1 - */ +/* Channel 2 - */ +/* Channel 3 - */ +/* Channel 4 - */ +/* Channel 5 - */ +/* Channel 6 - */ +/* Channel 7 - */ +/* Channel 8 - */ +/* Channel 9 - */ +/* Channel 10 - */ +/* Channel 11 - */ +/* Channel 12 - */ -//------------------------ +// ------------------------ // BOOTLOADER_SETTINGS -//------------------------ -#define BOARD_READABLE TRUE -#define BOARD_WRITABLE TRUE -#define MAX_DEL_RETRYS 3 +// ------------------------ +#define BOARD_READABLE TRUE +#define BOARD_WRITABLE TRUE +#define MAX_DEL_RETRYS 3 -//------------------------ +// ------------------------ // WATCHDOG_SETTINGS -//------------------------ +// ------------------------ #define PIOS_WATCHDOG_TIMEOUT 250 #define PIOS_WDG_REGISTER BKP_DR4 #define PIOS_WDG_ACTUATOR 0x0001 @@ -73,205 +73,205 @@ #define PIOS_WDG_MANUAL 0x0008 #define PIOS_WDG_AUTOTUNE 0x0010 -//------------------------ +// ------------------------ // TELEMETRY -//------------------------ +// ------------------------ #define TELEM_QUEUE_SIZE 20 -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -#define PIOS_LED_HEARTBEAT 0 +// ------------------------ +#define PIOS_LED_HEARTBEAT 0 -//------------------------- +// ------------------------- // System Settings -//------------------------- -#define PIOS_MASTER_CLOCK 72000000 -#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) +// ------------------------- +#define PIOS_MASTER_CLOCK 72000000 +#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) -//------------------------- +// ------------------------- // Interrupt Priorities -//------------------------- -#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS -#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS -#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... -#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... -//------------------------ +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------ // PIOS_I2C // See also pios_board.c -//------------------------ -#define PIOS_I2C_MAX_DEVS 1 +// ------------------------ +#define PIOS_I2C_MAX_DEVS 1 extern uint32_t pios_i2c_flexi_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexi_adapter_id) -#define PIOS_I2C_ESC_ADAPTER (pios_i2c_flexi_adapter_id) -#define PIOS_I2C_BMP085_ADAPTER (pios_i2c_flexi_adapter_id) +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexi_adapter_id) +#define PIOS_I2C_ESC_ADAPTER (pios_i2c_flexi_adapter_id) +#define PIOS_I2C_BMP085_ADAPTER (pios_i2c_flexi_adapter_id) -//------------------------ +// ------------------------ // PIOS_BMP085 -//------------------------ -#define PIOS_BMP085_OVERSAMPLING 3 +// ------------------------ +#define PIOS_BMP085_OVERSAMPLING 3 -//------------------------- +// ------------------------- // SPI // // See also pios_board.c -//------------------------- -#define PIOS_SPI_MAX_DEVS 2 +// ------------------------- +#define PIOS_SPI_MAX_DEVS 2 -//------------------------- +// ------------------------- // PIOS_USART -//------------------------- -#define PIOS_USART_MAX_DEVS 2 +// ------------------------- +#define PIOS_USART_MAX_DEVS 2 -//------------------------- +// ------------------------- // PIOS_COM // // See also pios_board.c -//------------------------- -#define PIOS_COM_MAX_DEVS 3 +// ------------------------- +#define PIOS_COM_MAX_DEVS 3 extern uint32_t pios_com_telem_rf_id; -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) #if defined(PIOS_INCLUDE_GPS) extern uint32_t pios_com_gps_id; -#define PIOS_COM_GPS (pios_com_gps_id) -#endif /* PIOS_INCLUDE_GPS */ +#define PIOS_COM_GPS (pios_com_gps_id) +#endif /* PIOS_INCLUDE_GPS */ extern uint32_t pios_com_bridge_id; -#define PIOS_COM_BRIDGE (pios_com_bridge_id) +#define PIOS_COM_BRIDGE (pios_com_bridge_id) extern uint32_t pios_com_vcp_id; -#define PIOS_COM_VCP (pios_com_vcp_id) +#define PIOS_COM_VCP (pios_com_vcp_id) extern uint32_t pios_com_telem_usb_id; -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) extern uint32_t pios_com_debug_id; -#define PIOS_COM_DEBUG (pios_com_debug_id) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#define PIOS_COM_DEBUG (pios_com_debug_id) +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ extern uint32_t pios_com_hkosd_id; -#define PIOS_COM_OSDHK (pios_com_hkosd_id) +#define PIOS_COM_OSDHK (pios_com_hkosd_id) -//------------------------- +// ------------------------- // ADC // PIOS_ADC_PinGet(0) = Gyro Z // PIOS_ADC_PinGet(1) = Gyro Y // PIOS_ADC_PinGet(2) = Gyro X -//------------------------- -//#define PIOS_ADC_OVERSAMPLING_RATE 1 -#define PIOS_ADC_USE_TEMP_SENSOR 1 -#define PIOS_ADC_TEMP_SENSOR_ADC ADC1 -#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 1 +// ------------------------- +// #define PIOS_ADC_OVERSAMPLING_RATE 1 +#define PIOS_ADC_USE_TEMP_SENSOR 1 +#define PIOS_ADC_TEMP_SENSOR_ADC ADC1 +#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 1 -#define PIOS_ADC_PIN1_GPIO_PORT GPIOA // PA4 (Gyro X) -#define PIOS_ADC_PIN1_GPIO_PIN GPIO_Pin_4 // ADC12_IN4 -#define PIOS_ADC_PIN1_GPIO_CHANNEL ADC_Channel_4 -#define PIOS_ADC_PIN1_ADC ADC2 -#define PIOS_ADC_PIN1_ADC_NUMBER 1 +#define PIOS_ADC_PIN1_GPIO_PORT GPIOA // PA4 (Gyro X) +#define PIOS_ADC_PIN1_GPIO_PIN GPIO_Pin_4 // ADC12_IN4 +#define PIOS_ADC_PIN1_GPIO_CHANNEL ADC_Channel_4 +#define PIOS_ADC_PIN1_ADC ADC2 +#define PIOS_ADC_PIN1_ADC_NUMBER 1 -#define PIOS_ADC_PIN2_GPIO_PORT GPIOA // PA5 (Gyro Y) -#define PIOS_ADC_PIN2_GPIO_PIN GPIO_Pin_5 // ADC123_IN5 -#define PIOS_ADC_PIN2_GPIO_CHANNEL ADC_Channel_5 -#define PIOS_ADC_PIN2_ADC ADC1 -#define PIOS_ADC_PIN2_ADC_NUMBER 2 +#define PIOS_ADC_PIN2_GPIO_PORT GPIOA // PA5 (Gyro Y) +#define PIOS_ADC_PIN2_GPIO_PIN GPIO_Pin_5 // ADC123_IN5 +#define PIOS_ADC_PIN2_GPIO_CHANNEL ADC_Channel_5 +#define PIOS_ADC_PIN2_ADC ADC1 +#define PIOS_ADC_PIN2_ADC_NUMBER 2 -#define PIOS_ADC_PIN3_GPIO_PORT GPIOA // PA3 (Gyro Z) -#define PIOS_ADC_PIN3_GPIO_PIN GPIO_Pin_3 // ADC12_IN3 -#define PIOS_ADC_PIN3_GPIO_CHANNEL ADC_Channel_3 -#define PIOS_ADC_PIN3_ADC ADC2 -#define PIOS_ADC_PIN3_ADC_NUMBER 2 +#define PIOS_ADC_PIN3_GPIO_PORT GPIOA // PA3 (Gyro Z) +#define PIOS_ADC_PIN3_GPIO_PIN GPIO_Pin_3 // ADC12_IN3 +#define PIOS_ADC_PIN3_GPIO_CHANNEL ADC_Channel_3 +#define PIOS_ADC_PIN3_ADC ADC2 +#define PIOS_ADC_PIN3_ADC_NUMBER 2 -#define PIOS_ADC_NUM_PINS 3 +#define PIOS_ADC_NUM_PINS 3 -#define PIOS_ADC_PORTS { PIOS_ADC_PIN1_GPIO_PORT, PIOS_ADC_PIN2_GPIO_PORT, PIOS_ADC_PIN3_GPIO_PORT } -#define PIOS_ADC_PINS { PIOS_ADC_PIN1_GPIO_PIN, PIOS_ADC_PIN2_GPIO_PIN, PIOS_ADC_PIN3_GPIO_PIN } -#define PIOS_ADC_CHANNELS { PIOS_ADC_PIN1_GPIO_CHANNEL, PIOS_ADC_PIN2_GPIO_CHANNEL, PIOS_ADC_PIN3_GPIO_CHANNEL } -#define PIOS_ADC_MAPPING { PIOS_ADC_PIN1_ADC, PIOS_ADC_PIN2_ADC, PIOS_ADC_PIN3_ADC } -#define PIOS_ADC_CHANNEL_MAPPING { PIOS_ADC_PIN1_ADC_NUMBER, PIOS_ADC_PIN2_ADC_NUMBER, PIOS_ADC_PIN3_ADC_NUMBER } -#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR) -#define PIOS_ADC_NUM_ADC_CHANNELS 2 -#define PIOS_ADC_USE_ADC2 1 -#define PIOS_ADC_CLOCK_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE) -#define PIOS_ADC_ADCCLK RCC_PCLK2_Div8 +#define PIOS_ADC_PORTS { PIOS_ADC_PIN1_GPIO_PORT, PIOS_ADC_PIN2_GPIO_PORT, PIOS_ADC_PIN3_GPIO_PORT } +#define PIOS_ADC_PINS { PIOS_ADC_PIN1_GPIO_PIN, PIOS_ADC_PIN2_GPIO_PIN, PIOS_ADC_PIN3_GPIO_PIN } +#define PIOS_ADC_CHANNELS { PIOS_ADC_PIN1_GPIO_CHANNEL, PIOS_ADC_PIN2_GPIO_CHANNEL, PIOS_ADC_PIN3_GPIO_CHANNEL } +#define PIOS_ADC_MAPPING { PIOS_ADC_PIN1_ADC, PIOS_ADC_PIN2_ADC, PIOS_ADC_PIN3_ADC } +#define PIOS_ADC_CHANNEL_MAPPING { PIOS_ADC_PIN1_ADC_NUMBER, PIOS_ADC_PIN2_ADC_NUMBER, PIOS_ADC_PIN3_ADC_NUMBER } +#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR) +#define PIOS_ADC_NUM_ADC_CHANNELS 2 +#define PIOS_ADC_USE_ADC2 1 +#define PIOS_ADC_CLOCK_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE) +#define PIOS_ADC_ADCCLK RCC_PCLK2_Div8 /* RCC_PCLK2_Div2: ADC clock = PCLK2/2 */ /* RCC_PCLK2_Div4: ADC clock = PCLK2/4 */ /* RCC_PCLK2_Div6: ADC clock = PCLK2/6 */ /* RCC_PCLK2_Div8: ADC clock = PCLK2/8 */ -#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_239Cycles5 +#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_239Cycles5 /* Sample time: */ /* With an ADCCLK = 14 MHz and a sampling time of 239.5 cycles: */ /* Tconv = 239.5 + 12.5 = 252 cycles = 18�s */ /* (1 / (ADCCLK / CYCLES)) = Sample Time (�S) */ -#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_LOW +#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_LOW // Currently analog acquistion hard coded at 480 Hz // PCKL2 = HCLK / 16 // ADCCLK = PCLK2 / 2 -#define PIOS_ADC_RATE (72.0e6f / 1.0f / 8.0f / 252.0f / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) -#define PIOS_ADC_MAX_OVERSAMPLING 36 +#define PIOS_ADC_RATE (72.0e6f / 1.0f / 8.0f / 252.0f / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) +#define PIOS_ADC_MAX_OVERSAMPLING 36 -//------------------------ +// ------------------------ // PIOS_RCVR // See also pios_board.c -//------------------------ -#define PIOS_RCVR_MAX_DEVS 3 -#define PIOS_RCVR_MAX_CHANNELS 12 -#define PIOS_GCSRCVR_TIMEOUT_MS 100 +// ------------------------ +#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_GCSRCVR_TIMEOUT_MS 100 -//------------------------- +// ------------------------- // Receiver PPM input -//------------------------- -#define PIOS_PPM_MAX_DEVS 1 -#define PIOS_PPM_NUM_INPUTS 12 +// ------------------------- +#define PIOS_PPM_MAX_DEVS 1 +#define PIOS_PPM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver PWM input -//------------------------- -#define PIOS_PWM_MAX_DEVS 1 -#define PIOS_PWM_NUM_INPUTS 6 +// ------------------------- +#define PIOS_PWM_MAX_DEVS 1 +#define PIOS_PWM_NUM_INPUTS 6 -//------------------------- +// ------------------------- // Receiver DSM input -//------------------------- -#define PIOS_DSM_MAX_DEVS 2 -#define PIOS_DSM_NUM_INPUTS 12 +// ------------------------- +#define PIOS_DSM_MAX_DEVS 2 +#define PIOS_DSM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver S.Bus input -//------------------------- -#define PIOS_SBUS_MAX_DEVS 1 -#define PIOS_SBUS_NUM_INPUTS (16+2) +// ------------------------- +#define PIOS_SBUS_MAX_DEVS 1 +#define PIOS_SBUS_NUM_INPUTS (16 + 2) -//------------------------- +// ------------------------- // Servo outputs -//------------------------- -#define PIOS_SERVO_UPDATE_HZ 50 -#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ -//-------------------------- +// -------------------------- // Timer controller settings -//-------------------------- -#define PIOS_TIM_MAX_DEVS 3 +// -------------------------- +#define PIOS_TIM_MAX_DEVS 3 -//------------------------- +// ------------------------- // GPIO -//------------------------- -#define PIOS_GPIO_PORTS { } -#define PIOS_GPIO_PINS { } -#define PIOS_GPIO_CLKS { } -#define PIOS_GPIO_NUM 0 +// ------------------------- +#define PIOS_GPIO_PORTS {} +#define PIOS_GPIO_PINS {} +#define PIOS_GPIO_CLKS {} +#define PIOS_GPIO_NUM 0 -//------------------------- +// ------------------------- // USB -//------------------------- -#define PIOS_USB_HID_MAX_DEVS 1 +// ------------------------- +#define PIOS_USB_HID_MAX_DEVS 1 -#define PIOS_USB_ENABLED 1 -#define PIOS_USB_DETECT_GPIO_PORT GPIOC -#define PIOS_USB_MAX_DEVS 1 -#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 +#define PIOS_USB_ENABLED 1 +#define PIOS_USB_DETECT_GPIO_PORT GPIOC +#define PIOS_USB_MAX_DEVS 1 +#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/coptercontrol/pios_usb_board_data.c b/flight/targets/boards/coptercontrol/pios_usb_board_data.c index 67c19e6c3..20c49fdcf 100644 --- a/flight/targets/boards/coptercontrol/pios_usb_board_data.c +++ b/flight/targets/boards/coptercontrol/pios_usb_board_data.c @@ -28,74 +28,75 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ -#include /* PIOS_SYS_SerialNumberGet */ -#include /* PIOS_USBHOOK_* */ -#include /* PIOS_USB_UTIL_AsciiToUtf8 */ +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ static const uint8_t usb_product_id[28] = { - sizeof(usb_product_id), - USB_DESC_TYPE_STRING, - 'C', 0, - 'o', 0, - 'p', 0, - 't', 0, - 'e', 0, - 'r', 0, - 'C', 0, - 'o', 0, - 'n', 0, - 't', 0, - 'r', 0, - 'o', 0, - 'l', 0, + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'C', 0, + 'o', 0, + 'p', 0, + 't', 0, + 'e', 0, + 'r', 0, + 'C', 0, + 'o', 0, + 'n', 0, + 't', 0, + 'r', 0, + 'o', 0, + 'l', 0, }; -static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = { - sizeof(usb_serial_number), - USB_DESC_TYPE_STRING, +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, }; static const struct usb_string_langid usb_lang_id = { - .bLength = sizeof(usb_lang_id), - .bDescriptorType = USB_DESC_TYPE_STRING, - .bLangID = htousbs(USB_LANGID_ENGLISH_US), + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), }; static const uint8_t usb_vendor_id[28] = { - sizeof(usb_vendor_id), - USB_DESC_TYPE_STRING, - 'o', 0, - 'p', 0, - 'e', 0, - 'n', 0, - 'p', 0, - 'i', 0, - 'l', 0, - 'o', 0, - 't', 0, - '.', 0, - 'o', 0, - 'r', 0, - 'g', 0 + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 }; int32_t PIOS_USB_BOARD_DATA_Init(void) { - /* Load device serial number into serial number string */ - uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - PIOS_SYS_SerialNumberGet((char *)sn); + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ - uint8_t * utf8 = &(usb_serial_number[2]); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1); + PIOS_SYS_SerialNumberGet((char *)sn); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); - return 0; + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; } diff --git a/flight/targets/boards/oplinkmini/board_hw_defs.c b/flight/targets/boards/oplinkmini/board_hw_defs.c index b9b34081c..2601e8b0e 100644 --- a/flight/targets/boards/oplinkmini/board_hw_defs.c +++ b/flight/targets/boards/oplinkmini/board_hw_defs.c @@ -1,103 +1,102 @@ - #if defined(PIOS_INCLUDE_LED) #include static const struct pios_led pios_leds[] = { - [PIOS_LED_USB] = { - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, - [PIOS_LED_LINK] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, - [PIOS_LED_RX] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, - [PIOS_LED_TX] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, + [PIOS_LED_USB] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_LINK] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_RX] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_TX] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, #ifdef PIOS_RFM22B_DEBUG_ON_TELEM - [PIOS_LED_D1] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, - [PIOS_LED_D2] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, - [PIOS_LED_D3] = { - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, - [PIOS_LED_D4] = { - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, -#endif + [PIOS_LED_D1] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_D2] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_D3] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_D4] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, +#endif /* ifdef PIOS_RFM22B_DEBUG_ON_TELEM */ }; static const struct pios_led_cfg pios_led_cfg = { - .leds = pios_leds, - .num_leds = NELEMENTS(pios_leds), + .leds = pios_leds, + .num_leds = NELEMENTS(pios_leds), }; -const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (__attribute__((unused)) uint32_t board_revision) +const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision) { - return &pios_led_cfg; + return &pios_led_cfg; } -#endif /* PIOS_INCLUDE_LED */ +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_SPI) @@ -109,118 +108,108 @@ const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (__attribute__((unused) * .rodata section (ie. Flash) rather than in the .bss section (RAM). */ void PIOS_SPI_port_irq_handler(void); -void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_port_irq_handler"))); -void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_port_irq_handler"))); +void DMA1_Channel5_IRQHandler() __attribute__((alias("PIOS_SPI_port_irq_handler"))); +void DMA1_Channel4_IRQHandler() __attribute__((alias("PIOS_SPI_port_irq_handler"))); -static const struct pios_spi_cfg pios_spi_rfm22b_cfg = -{ - .regs = SPI1, +static const struct pios_spi_cfg pios_spi_rfm22b_cfg = { + .regs = SPI1, - .init = - { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 0, - .SPI_CPOL = SPI_CPOL_Low, - .SPI_CPHA = SPI_CPHA_1Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, // slowest SCLK - }, - .use_crc = FALSE, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 0, + .SPI_CPOL = SPI_CPOL_Low, + .SPI_CPHA = SPI_CPHA_1Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, // slowest SCLK + }, + .use_crc = FALSE, - .dma = - { - .ahb_clk = RCC_AHBPeriph_DMA1, - .irq = - { - .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), - .init = { - .NVIC_IRQChannel = DMA1_Channel2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + .irq = { + .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), + .init = { + .NVIC_IRQChannel = DMA1_Channel2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, - .rx = { - .channel = DMA1_Channel2, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel3, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .slave_count = 1, - .ssel = - {{ - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }}, - .sclk = - { - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = - { - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .mosi = - { - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, + .rx = { + .channel = DMA1_Channel2, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel3, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + } + }, + .sclk = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .mosi = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, }; uint32_t pios_spi_rfm22b_id; void PIOS_SPI_port_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_rfm22b_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_rfm22b_id); } #endif /* PIOS_INCLUDE_SPI */ @@ -230,47 +219,47 @@ void PIOS_SPI_port_irq_handler(void) #include static const struct pios_exti_cfg pios_exti_rfm22b_cfg __exti_config = { - .vector = PIOS_RFM22_EXT_Int, - .line = EXTI_Line2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line2, - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Falling, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_RFM22_EXT_Int, + .line = EXTI_Line2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line2, + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Falling, + .EXTI_LineCmd = ENABLE, + }, + }, }; #include struct pios_rfm22b_cfg pios_rfm22b_cfg = { - .spi_cfg = &pios_spi_rfm22b_cfg, - .exti_cfg = &pios_exti_rfm22b_cfg, - .RFXtalCap = 0x7f, - .slave_num = 0, - .gpio_direction = GPIO0_TX_GPIO1_RX, + .spi_cfg = &pios_spi_rfm22b_cfg, + .exti_cfg = &pios_exti_rfm22b_cfg, + .RFXtalCap = 0x7f, + .slave_num = 0, + .gpio_direction = GPIO0_TX_GPIO1_RX, }; -//! Compatibility layer for various hardware revisions -const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (__attribute__((unused)) uint32_t board_revision) +// ! Compatibility layer for various hardware revisions +const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22Cfg(__attribute__((unused)) uint32_t board_revision) { - return &pios_rfm22b_cfg; + return &pios_rfm22b_cfg; } #endif /* PIOS_INCLUDE_RFM22B */ @@ -282,146 +271,147 @@ const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (__attribute__((un */ #include "pios_adc_priv.h" extern void PIOS_ADC_handler(void); -void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); +void DMA1_Channel1_IRQHandler() __attribute__((alias("PIOS_ADC_handler"))); // Remap the ADC DMA handler to this one static const struct pios_adc_cfg pios_adc_cfg = { - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - .irq = { - .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), - .init = { - .NVIC_IRQChannel = DMA1_Channel1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA1_Channel1, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR, - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - } - }, - .half_flag = DMA1_IT_HT1, - .full_flag = DMA1_IT_TC1, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + .irq = { + .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), + .init = { + .NVIC_IRQChannel = DMA1_Channel1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA1_Channel1, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR, + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + } + }, + .half_flag = DMA1_IT_HT1, + .full_flag = DMA1_IT_TC1, }; struct pios_adc_dev pios_adc_devs[] = { - { - .cfg = &pios_adc_cfg, - .callback_function = NULL, - }, + { + .cfg = &pios_adc_cfg, + .callback_function = NULL, + }, }; uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs); -void PIOS_ADC_handler() { - PIOS_ADC_DMA_Handler(); +void PIOS_ADC_handler() +{ + PIOS_ADC_DMA_Handler(); } -#endif /* PIOS_INCLUDE_ADC */ +#endif /* PIOS_INCLUDE_ADC */ #if defined(PIOS_INCLUDE_TIM) #include "pios_tim_priv.h" static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, }; static const struct pios_tim_clock_cfg tim_1_cfg = { - .timer = TIM1, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM1, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_2_cfg = { - .timer = TIM2, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM2, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_3_cfg = { - .timer = TIM3, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM3, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_4_cfg = { - .timer = TIM4, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM4, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_channel pios_tim_ppm_flexi_port = { - .timer = TIM2, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap2_TIM2, + .timer = TIM2, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap2_TIM2, }; static const struct pios_tim_channel pios_tim_ppm_main_port = { - .timer = TIM1, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, + .timer = TIM1, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }; -#endif /* PIOS_INCLUDE_TIM */ +#endif /* PIOS_INCLUDE_TIM */ #if defined(PIOS_INCLUDE_USART) @@ -430,84 +420,76 @@ static const struct pios_tim_channel pios_tim_ppm_main_port = { /* * SERIAL USART */ -static const struct pios_usart_cfg pios_usart_serial_cfg = -{ - .regs = USART1, - .init = - { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = - { - .init = - { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = - { - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = - { - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, +static const struct pios_usart_cfg pios_usart_serial_cfg = { + .regs = USART1, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, }; static const struct pios_usart_cfg pios_usart_telem_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, + .regs = USART3, .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, }, - }, }; #endif /* PIOS_INCLUDE_USART */ @@ -524,27 +506,27 @@ static const struct pios_usart_cfg pios_usart_telem_flexi_cfg = { */ #include -void PIOS_RTC_IRQ_Handler (void); -void RTC_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler"))); +void PIOS_RTC_IRQ_Handler(void); +void RTC_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); static const struct pios_rtc_cfg pios_rtc_main_cfg = { - .clksrc = RCC_RTCCLKSource_HSE_Div128, - .prescaler = 100, - .irq = { - .init = { - .NVIC_IRQChannel = RTC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .clksrc = RCC_RTCCLKSource_HSE_Div128, + .prescaler = 100, + .irq = { + .init = { + .NVIC_IRQChannel = RTC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; -void PIOS_RTC_IRQ_Handler (void) +void PIOS_RTC_IRQ_Handler(void) { - PIOS_RTC_irq_handler (); + PIOS_RTC_irq_handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_RTC) */ /* * PPM Inputs @@ -553,28 +535,28 @@ void PIOS_RTC_IRQ_Handler (void) #include const struct pios_ppm_cfg pios_ppm_flexi_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = &pios_tim_ppm_flexi_port, - .num_channels = 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_ppm_flexi_port, + .num_channels = 1, }; const struct pios_ppm_cfg pios_ppm_main_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = &pios_tim_ppm_main_port, - .num_channels = 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_ppm_main_port, + .num_channels = 1, }; -#endif /* PIOS_INCLUDE_PPM */ +#endif /* PIOS_INCLUDE_PPM */ /* * PPM Output @@ -583,33 +565,33 @@ const struct pios_ppm_cfg pios_ppm_main_cfg = { #include static const struct pios_tim_channel pios_tim_ppmout[] = { - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_FullRemap_TIM2, - } + { + .timer = TIM2, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_FullRemap_TIM2, + } }; const struct pios_ppm_out_cfg pios_ppm_out_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_Low, - .TIM_OCNPolarity = TIM_OCPolarity_Low, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channel = pios_tim_ppmout, + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_Low, + .TIM_OCNPolarity = TIM_OCPolarity_Low, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channel = pios_tim_ppmout, }; #endif /* PIOS_INCLUDE_PPM_OUT */ @@ -623,30 +605,30 @@ const struct pios_ppm_out_cfg pios_ppm_out_cfg = { #include "pios_usb_priv.h" static const struct pios_usb_cfg pios_usb_main_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, }, - .vsense_active_low = false + }, + .vsense = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .vsense_active_low = false }; #include "pios_usb_board_data_priv.h" #include "pios_usb_desc_hid_cdc_priv.h" #include "pios_usb_desc_hid_only_priv.h" -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ #if defined(PIOS_INCLUDE_COM_MSG) @@ -658,9 +640,9 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { #include const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 2, - .data_rx_ep = 1, - .data_tx_ep = 1, + .data_if = 2, + .data_rx_ep = 1, + .data_tx_ep = 1, }; #endif /* PIOS_INCLUDE_USB_HID */ @@ -668,21 +650,21 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { #include const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 0, - .ctrl_tx_ep = 2, + .ctrl_if = 0, + .ctrl_tx_ep = 2, - .data_if = 1, - .data_rx_ep = 3, - .data_tx_ep = 3, + .data_if = 1, + .data_rx_ep = 3, + .data_tx_ep = 3, }; -#endif /* PIOS_INCLUDE_USB_CDC */ +#endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_FLASH_EEPROM) #include const struct pios_eeprom_cfg pios_eeprom_cfg = { - .base_address = PIOS_FLASH_EEPROM_ADDR, - .max_size = PIOS_FLASH_EEPROM_LEN, + .base_address = PIOS_FLASH_EEPROM_ADDR, + .max_size = PIOS_FLASH_EEPROM_LEN, }; #endif /* PIOS_INCLUDE_FLASH_EEPROM */ diff --git a/flight/targets/boards/oplinkmini/bootloader/inc/common.h b/flight/targets/boards/oplinkmini/bootloader/inc/common.h index 9973764e9..1212e06fd 100644 --- a/flight/targets/boards/oplinkmini/bootloader/inc/common.h +++ b/flight/targets/boards/oplinkmini/bootloader/inc/common.h @@ -28,10 +28,10 @@ #ifndef COMMON_H_ #define COMMON_H_ -//#include "board.h" +// #include "board.h" typedef enum { - start, keepgoing, + start, keepgoing, } DownloadAction; /**************************************************/ @@ -39,77 +39,77 @@ typedef enum { /**************************************************/ typedef enum { - DFUidle, //0 - uploading, //1 - wrong_packet_received, //2 - too_many_packets, //3 - too_few_packets, //4 - Last_operation_Success, //5 - downloading, //6 - BLidle, //7 - Last_operation_failed, //8 - uploadingStarting, //9 - outsideDevCapabilities, //10 - CRC_Fail,//11 - failed_jump, -//12 + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + BLidle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, +// 12 } DFUStates; /**************************************************/ /* OP_DFU commands */ /**************************************************/ typedef enum { - Reserved, //0 - Req_Capabilities, //1 - Rep_Capabilities, //2 - EnterDFU, //3 - JumpFW, //4 - Reset, //5 - Abort_Operation, //6 - Upload, //7 - Op_END, //8 - Download_Req, //9 - Download, //10 - Status_Request, //11 - Status_Rep -//12 + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep +// 12 } DFUCommands; typedef enum { - High_Density, Medium_Density + High_Density, Medium_Density } DeviceType; /**************************************************/ /* OP_DFU transfer types */ /**************************************************/ typedef enum { - FW, //0 - Descript -//2 + FW, // 0 + Descript +// 2 } DFUTransfer; /**************************************************/ /* OP_DFU transfer port */ /**************************************************/ typedef enum { - Usb, //0 - Serial -//2 + Usb, // 0 + Serial +// 2 } DFUPort; /**************************************************/ /* OP_DFU programable programable HW types */ /**************************************************/ typedef enum { - Self_flash, //0 - Remote_flash_via_spi -//1 + Self_flash, // 0 + Remote_flash_via_spi +// 1 } DFUProgType; /**************************************************/ /* OP_DFU programable sources */ /**************************************************/ -#define USB 0 -#define SPI 1 +#define USB 0 +#define SPI 1 -#define DownloadDelay 100000 +#define DownloadDelay 100000 -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 +#define MAX_DEL_RETRYS 3 +#define MAX_WRI_RETRYS 3 #endif /* COMMON_H_ */ diff --git a/flight/targets/boards/oplinkmini/bootloader/inc/pios_config.h b/flight/targets/boards/oplinkmini/bootloader/inc/pios_config.h index fbd493cc6..7a974ad83 100644 --- a/flight/targets/boards/oplinkmini/bootloader/inc/pios_config.h +++ b/flight/targets/boards/oplinkmini/bootloader/inc/pios_config.h @@ -2,28 +2,28 @@ ****************************************************************************** * @addtogroup OpenPilotBL OpenPilot BootLoader * @{ - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * In particular, pios_config.h is where you define which PiOS libraries * and features are included in the firmware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/oplinkmini/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/oplinkmini/bootloader/inc/pios_usb_board_data.h index ae6704b6e..e32e8fd9e 100644 --- a/flight/targets/boards/oplinkmini/bootloader/inc/pios_usb_board_data.h +++ b/flight/targets/boards/oplinkmini/bootloader/inc/pios_usb_board_data.h @@ -33,13 +33,13 @@ #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 2 +#define PIOS_USB_BOARD_EP_NUM 2 -#include /* struct usb_* */ +#include /* struct usb_* */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OPLINK -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OPLINK, USB_OP_BOARD_MODE_BL) -#define PIOS_USB_BOARD_SN_SUFFIX "+BL" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OPLINK +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OPLINK, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" /* * The bootloader uses a simplified report structure @@ -49,4 +49,4 @@ */ #define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/oplinkmini/bootloader/main.c b/flight/targets/boards/oplinkmini/bootloader/main.c index ae0fdd968..a243e8000 100644 --- a/flight/targets/boards/oplinkmini/bootloader/main.c +++ b/flight/targets/boards/oplinkmini/bootloader/main.c @@ -8,21 +8,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief This is the file with the main function of the OpenPilot BootLoader * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -59,10 +59,10 @@ uint8_t tempcount = 0; /* Extern variables ----------------------------------------------------------*/ DFUStates DeviceState; -int16_t status = 0; -uint8_t JumpToApp = FALSE; -uint8_t GO_dfu = FALSE; -uint8_t USB_connected = FALSE; +int16_t status = 0; +uint8_t JumpToApp = FALSE; +uint8_t GO_dfu = FALSE; +uint8_t USB_connected = FALSE; uint8_t User_DFU_request = FALSE; static uint8_t mReceive_Buffer[63]; /* Private function prototypes -----------------------------------------------*/ @@ -70,137 +70,150 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); uint8_t processRX(); void jump_to_app(); -int main() { - PIOS_SYS_Init(); - PIOS_Board_Init(); - PIOS_IAP_Init(); - - USB_connected = PIOS_USB_CableConnected(0); +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); - if (PIOS_IAP_CheckRequest() == TRUE) { - PIOS_DELAY_WaitmS(1000); - User_DFU_request = TRUE; - PIOS_IAP_ClearRequest(); - } + USB_connected = PIOS_USB_CableConnected(0); - GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); + if (PIOS_IAP_CheckRequest() == TRUE) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = TRUE; + PIOS_IAP_ClearRequest(); + } - if (GO_dfu == TRUE) { - PIOS_Board_Init(); - if (User_DFU_request == TRUE) - DeviceState = DFUidle; - else - DeviceState = BLidle; - } else - JumpToApp = TRUE; + GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); - uint32_t stopwatch = 0; - uint32_t prev_ticks = PIOS_DELAY_GetuS(); - while (TRUE) { - /* Update the stopwatch */ - uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); - prev_ticks += elapsed_ticks; - stopwatch += elapsed_ticks; + if (GO_dfu == TRUE) { + PIOS_Board_Init(); + if (User_DFU_request == TRUE) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = TRUE; + } - if (JumpToApp == TRUE) - jump_to_app(); + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (TRUE) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; - switch (DeviceState) { - case Last_operation_Success: - case uploadingStarting: - case DFUidle: - period1 = 5000; - sweep_steps1 = 100; - PIOS_LED_Off(PIOS_LED_ALARM); - period2 = 0; - break; - case uploading: - period1 = 5000; - sweep_steps1 = 100; - period2 = 2500; - sweep_steps2 = 50; - break; - case downloading: - period1 = 2500; - sweep_steps1 = 50; - PIOS_LED_Off(PIOS_LED_ALARM); - period2 = 0; - break; - case BLidle: - period1 = 0; - PIOS_LED_On(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - default://error - period1 = 5000; - sweep_steps1 = 100; - period2 = 5000; - sweep_steps2 = 100; - } + if (JumpToApp == TRUE) { + jump_to_app(); + } - if (period1 != 0) { - if (LedPWM(period1, sweep_steps1, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_On(PIOS_LED_HEARTBEAT); + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_ALARM); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_ALARM); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } - if (period2 != 0) { - if (LedPWM(period2, sweep_steps2, stopwatch)) - PIOS_LED_On(PIOS_LED_ALARM); - else - PIOS_LED_Off(PIOS_LED_ALARM); - } else - PIOS_LED_Off(PIOS_LED_ALARM); + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } - if (stopwatch > 50 * 1000 * 1000) - stopwatch = 0; - if ((stopwatch > 6 * 1000 * 1000) && (DeviceState - == BLidle)) - JumpToApp = TRUE; + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_ALARM); + } else { + PIOS_LED_Off(PIOS_LED_ALARM); + } + } else { + PIOS_LED_Off(PIOS_LED_ALARM); + } - processRX(); - DataDownload(start); - } + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && (DeviceState + == BLidle)) { + JumpToApp = TRUE; + } + + processRX(); + DataDownload(start); + } } -void jump_to_app() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; - if (((*(__IO uint32_t*) bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */ - FLASH_Lock(); - RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - _SetCNTR(0); // clear interrupt mask - _SetISTR(0); // clear all requests - JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); - Jump_To_Application = (pFunction) JumpAddress; - /* Initialize user application's Stack Pointer */ - __set_MSP(*(__IO uint32_t*) bdinfo->fw_base); - Jump_To_Application(); - } else { - DeviceState = failed_jump; - return; - } + if (((*(__IO uint32_t *)bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */ + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + _SetCNTR(0); // clear interrupt mask + _SetISTR(0); // clear all requests + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + return; + } } -uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { - uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ - uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ - uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ - if (curr_sweep & 1) { - pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ - } - return ((count % pwm_period) > pwm_duty) ? 1 : 0; + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; } -uint8_t processRX() { - if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { - processComand(mReceive_Buffer); - } - return TRUE; +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return TRUE; } - diff --git a/flight/targets/boards/oplinkmini/bootloader/pios_board.c b/flight/targets/boards/oplinkmini/bootloader/pios_board.c index f91f6a456..7bcd5ce8a 100644 --- a/flight/targets/boards/oplinkmini/bootloader/pios_board.c +++ b/flight/targets/boards/oplinkmini/bootloader/pios_board.c @@ -44,51 +44,52 @@ uint32_t pios_com_telem_usb_id; * called from System/openpilot.c */ static bool board_init_complete = false; -void PIOS_Board_Init(void) { - if (board_init_complete) { - return; - } +void PIOS_Board_Init(void) +{ + if (board_init_complete) { + return; + } - /* Enable Prefetch Buffer */ - FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); + /* Enable Prefetch Buffer */ + FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); - /* Flash 2 wait state */ - FLASH_SetLatency(FLASH_Latency_2); + /* Flash 2 wait state */ + FLASH_SetLatency(FLASH_Latency_2); - /* Delay system */ - PIOS_DELAY_Init(); + /* Delay system */ + PIOS_DELAY_Init(); - /* Initialize the PiOS library */ - PIOS_GPIO_Init(); + /* Initialize the PiOS library */ + PIOS_GPIO_Init(); #if defined(PIOS_INCLUDE_LED) - PIOS_LED_Init(&pios_led_cfg); -#endif /* PIOS_INCLUDE_LED */ + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Activate the HID-only USB configuration */ - PIOS_USB_DESC_HID_ONLY_Init(); + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); - uint32_t pios_usb_id; - if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) { - PIOS_Assert(0); - } + uint32_t pios_usb_id; + if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) { + PIOS_Assert(0); + } #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { - PIOS_Assert(0); - } -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); // TODO Tirar - board_init_complete = true; + board_init_complete = true; } diff --git a/flight/targets/boards/oplinkmini/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/oplinkmini/firmware/inc/FreeRTOSConfig.h index 38ff0bc7c..b40088c90 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/FreeRTOSConfig.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/FreeRTOSConfig.h @@ -1,75 +1,74 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ /** - * @addtogroup PIOS PIOS - * @{ - * @addtogroup FreeRTOS FreeRTOS - * @{ - */ + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ /* Notes: We use 5 task priorities */ -#define configUSE_PREEMPTION 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configUSE_MALLOC_FAILED_HOOK 1 -#define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 ) -#define configTICK_RATE_HZ ( ( portTickType ) 1000 ) -#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) -#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 48 ) -#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 54 * 256) ) -#define configMAX_TASK_NAME_LEN ( 16 ) -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configIDLE_SHOULD_YIELD 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_MALLOC_FAILED_HOOK 1 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)48) +#define configTOTAL_HEAP_SIZE ((size_t)(54 * 256)) +#define configMAX_TASK_NAME_LEN (16) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configQUEUE_REGISTRY_SIZE 10 /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ + to exclude the API function. */ -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 0 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) #define CHECK_IRQ_STACK @@ -77,22 +76,23 @@ NVIC value of 255. */ /* Enable run time stats collection */ #ifdef DIAG_TASKS -#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 -#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\ -do {\ -(*(unsigned long *)0xe000edfc) |= (1<<24);/* DEMCR |= DEMCR_TRCENA */\ -(*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\ -} while(0) -#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */ +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ #else -#define configCHECK_FOR_STACK_OVERFLOW 1 +#define configCHECK_FOR_STACK_OVERFLOW 1 #endif /** - * @} - */ + * @} + */ #endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h b/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h index 8c76835cd..3cceef152 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h @@ -10,19 +10,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_board_posix.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_board_posix.h index 84eadbeba..16364d7fa 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_board_posix.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_board_posix.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file pios_board.h + * @file pios_board.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -28,57 +28,54 @@ #define PIOS_BOARD_H - - -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -//#define PIOS_LED_LED1_GPIO_PORT GPIOC -//#define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12 -//#define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC -//#define PIOS_LED_LED2_GPIO_PORT GPIOC -//#define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13 -//#define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC -#define PIOS_LED_NUM 2 -//#define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT } -//#define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN } -//#define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK } +// ------------------------ +// #define PIOS_LED_LED1_GPIO_PORT GPIOC +// #define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12 +// #define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC +// #define PIOS_LED_LED2_GPIO_PORT GPIOC +// #define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13 +// #define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC +#define PIOS_LED_NUM 2 +// #define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT } +// #define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN } +// #define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK } -//------------------------- +// ------------------------- // COM // // See also pios_board_posix.c -//------------------------- -//#define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +// ------------------------- +// #define PIOS_USART_TX_BUFFER_SIZE 256 +#define PIOS_COM_BUFFER_SIZE 1024 +#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_COM_TELEM_RF 0 -#define PIOS_COM_GPS 1 -#define PIOS_COM_TELEM_USB 2 +#define PIOS_COM_TELEM_RF 0 +#define PIOS_COM_GPS 1 +#define PIOS_COM_TELEM_USB 2 #ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX 3 -#define PIOS_COM_DEBUG PIOS_COM_AUX +#define PIOS_COM_AUX 3 +#define PIOS_COM_DEBUG PIOS_COM_AUX #endif /** * glue macros for file IO * STM32 uses DOSFS for file IO */ -#define PIOS_FOPEN_READ(filename,file) (file=fopen((char*)filename,"r"))==NULL +#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL -#define PIOS_FOPEN_WRITE(filename,file) (file=fopen((char*)filename,"w"))==NULL +#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL -#define PIOS_FREAD(file,bufferadr,length,resultadr) (*resultadr=fread((uint8_t*)bufferadr,1,length,*file)) != length +#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length -#define PIOS_FWRITE(file,bufferadr,length,resultadr) *resultadr=fwrite((uint8_t*)bufferadr,1,length,*file) +#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) +#define PIOS_FCLOSE(file) fclose(file) -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char*)filename) +#define PIOS_FUNLINK(file) unlink((char *)filename) #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h index cb5372985..7ef58db98 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h @@ -141,26 +141,26 @@ /* #define PIOS_QUATERNION_STABILIZATION */ /* Performance counters */ -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 /* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 220 -#define HEAP_LIMIT_CRITICAL 40 -#define IRQSTACK_LIMIT_WARNING 100 -#define IRQSTACK_LIMIT_CRITICAL 60 -#define CPULOAD_LIMIT_WARNING 85 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 220 +#define HEAP_LIMIT_CRITICAL 40 +#define IRQSTACK_LIMIT_WARNING 100 +#define IRQSTACK_LIMIT_CRITICAL 60 +#define CPULOAD_LIMIT_WARNING 85 +#define CPULOAD_LIMIT_CRITICAL 95 /* Task stack sizes */ -#define PIOS_ACTUATOR_STACK_SIZE 1020 -#define PIOS_MANUAL_STACK_SIZE 724 -#define PIOS_SYSTEM_STACK_SIZE 460 -#define PIOS_STABILIZATION_STACK_SIZE 524 -#define PIOS_TELEM_STACK_SIZE 500 -#define PIOS_EVENTDISPATCHER_STACK_SIZE 130 +#define PIOS_ACTUATOR_STACK_SIZE 1020 +#define PIOS_MANUAL_STACK_SIZE 724 +#define PIOS_SYSTEM_STACK_SIZE 460 +#define PIOS_STABILIZATION_STACK_SIZE 524 +#define PIOS_TELEM_STACK_SIZE 500 +#define PIOS_EVENTDISPATCHER_STACK_SIZE 130 /* This can't be too high to stop eventdispatcher thread overflowing */ -#define PIOS_EVENTDISAPTCHER_QUEUE 10 +#define PIOS_EVENTDISAPTCHER_QUEUE 10 #endif /* PIOS_CONFIG_H */ /** diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h index 5e744a381..ecedcf3ce 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,14 +41,14 @@ /* Defaults for Logging */ -#define LOG_FILENAME "PIOS.LOG" -#define STARTUP_LOG_ENABLED 1 +#define LOG_FILENAME "PIOS.LOG" +#define STARTUP_LOG_ENABLED 1 /* COM Module */ -#define GPS_BAUDRATE 19200 -#define TELEM_BAUDRATE 19200 -#define AUXUART_ENABLED 0 -#define AUXUART_BAUDRATE 19200 +#define GPS_BAUDRATE 19200 +#define TELEM_BAUDRATE 19200 +#define AUXUART_ENABLED 0 +#define AUXUART_BAUDRATE 19200 #endif /* PIOS_CONFIG_POSIX_H */ diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_usb_board_data.h index 4ce97dfcc..736a7dbca 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_usb_board_data.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_usb_board_data.h @@ -35,12 +35,12 @@ #define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 4 +#define PIOS_USB_BOARD_EP_NUM 4 -#include /* USB_* macros */ +#include /* USB_* macros */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OPLINK -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OPLINK, USB_OP_BOARD_MODE_FW) -#define PIOS_USB_BOARD_SN_SUFFIX "+FW" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OPLINK +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OPLINK, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/oplinkmini/firmware/oplink.c b/flight/targets/boards/oplinkmini/firmware/oplink.c index dafbbacc3..07d4fa551 100644 --- a/flight/targets/boards/oplinkmini/firmware/oplink.c +++ b/flight/targets/boards/oplinkmini/firmware/oplink.c @@ -9,25 +9,25 @@ * @brief This is where the OP firmware starts. Those files also define the compile-time * options of the firmware. * @{ - * @file openpilot.c + * @file openpilot.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Sets up and runs main OpenPilot tasks. * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,7 +36,7 @@ #include /* Task Priorities */ -#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) /* Global Variables */ @@ -45,46 +45,45 @@ extern void PIOS_Board_Init(void); extern void Stack_Change(void); /** -* OpenPilot Main function: -* -* Initialize PiOS
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
-* Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) -* If something goes wrong, blink LED1 and LED2 every 100ms -* -*/ + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) + * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ int main() { - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ - /* Brings up System using CMSIS functions, enables the LEDs. */ - PIOS_SYS_Init(); + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); - /* Architecture dependant Hardware and - * core subsystem initialisation - * (see pios_board.c for your arch) - * */ - PIOS_Board_Init(); + /* Architecture dependant Hardware and + * core subsystem initialisation + * (see pios_board.c for your arch) + * */ + PIOS_Board_Init(); - /* Initialize modules */ - MODULE_INITIALISE_ALL + /* Initialize modules */ + MODULE_INITIALISE_ALL + /* swap the stack to use the IRQ stack */ + Stack_Change(); - /* swap the stack to use the IRQ stack */ - Stack_Change(); + /* Start the FreeRTOS scheduler which should never returns.*/ + vTaskStartScheduler(); - /* Start the FreeRTOS scheduler which should never returns.*/ - vTaskStartScheduler(); + /* If all is well we will never reach here as the scheduler will now be running. */ - /* If all is well we will never reach here as the scheduler will now be running. */ - - /* Do some indication to user that something bad just happened */ - while (1) { + /* Do some indication to user that something bad just happened */ + while (1) { #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ - PIOS_DELAY_WaitmS(100); - } + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + PIOS_DELAY_WaitmS(100); + } return 0; } @@ -93,4 +92,3 @@ int main() * @} * @} */ - diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index 92569c0b9..a5b00736d 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -1,29 +1,29 @@ /** -****************************************************************************** -* @addtogroup OpenPilotSystem OpenPilot System -* @{ -* @addtogroup OpenPilotCore OpenPilot Core -* @{ -* -* @file pios_board.c -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. -* @brief Defines board specific static initializers for hardware for the OpenPilot board. -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * + * @file pios_board.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Defines board specific static initializers for hardware for the OpenPilot board. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -51,8 +51,8 @@ #define PIOS_COM_RFM22B_RF_RX_BUF_LEN 256 #define PIOS_COM_RFM22B_RF_TX_BUF_LEN 256 -#define PIOS_COM_TELEM_RX_BUF_LEN 256 -#define PIOS_COM_TELEM_TX_BUF_LEN 256 +#define PIOS_COM_TELEM_RX_BUF_LEN 256 +#define PIOS_COM_TELEM_TX_BUF_LEN 256 uint32_t pios_com_telem_usb_id = 0; uint32_t pios_com_telem_vcp_id = 0; @@ -60,15 +60,15 @@ uint32_t pios_com_telem_uart_main_id = 0; uint32_t pios_com_telem_uart_flexi_id = 0; uint32_t pios_com_telemetry_id = 0; #if defined(PIOS_INCLUDE_PPM) -uint32_t pios_ppm_rcvr_id = 0; +uint32_t pios_ppm_rcvr_id = 0; #endif #if defined(PIOS_INCLUDE_PPM_OUT) -uint32_t pios_ppm_out_id = 0; +uint32_t pios_ppm_out_id = 0; #endif #if defined(PIOS_INCLUDE_RFM22B) -uint32_t pios_rfm22b_id = 0; +uint32_t pios_rfm22b_id = 0; uint32_t pios_com_rfm22b_id = 0; -uint32_t pios_com_radio_id = 0; +uint32_t pios_com_radio_id = 0; #endif uint8_t *pios_uart_rx_buffer; uint8_t *pios_uart_tx_buffer; @@ -78,248 +78,248 @@ uint8_t *pios_uart_tx_buffer; * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - - /* Set up the SPI interface to the rfm22b */ - if (PIOS_SPI_Init(&pios_spi_rfm22b_id, &pios_spi_rfm22b_cfg)) { - PIOS_DEBUG_Assert(0); - } + /* Set up the SPI interface to the rfm22b */ + if (PIOS_SPI_Init(&pios_spi_rfm22b_id, &pios_spi_rfm22b_cfg)) { + PIOS_DEBUG_Assert(0); + } #ifdef PIOS_INCLUDE_WDG - /* Initialize watchdog as early as possible to catch faults during init */ - PIOS_WDG_Init(); + /* Initialize watchdog as early as possible to catch faults during init */ + PIOS_WDG_Init(); #endif /* PIOS_INCLUDE_WDG */ #if defined(PIOS_INCLUDE_RTC) - /* Initialize the real-time clock and its associated tick */ - PIOS_RTC_Init(&pios_rtc_main_cfg); + /* Initialize the real-time clock and its associated tick */ + PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* PIOS_INCLUDE_RTC */ - OPLinkSettingsInitialize(); + OPLinkSettingsInitialize(); #if defined(PIOS_INCLUDE_LED) - PIOS_LED_Init(&pios_led_cfg); -#endif /* PIOS_INCLUDE_LED */ + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ - OPLinkSettingsData oplinkSettings; + OPLinkSettingsData oplinkSettings; #if defined(PIOS_INCLUDE_FLASH_EEPROM) - PIOS_EEPROM_Init(&pios_eeprom_cfg); + PIOS_EEPROM_Init(&pios_eeprom_cfg); - /* IAP System Setup */ - PIOS_IAP_Init(); - // check for safe mode commands from gcs - if(PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && - PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && - PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { - OPLinkSettingsGet(&oplinkSettings); - OPLinkSettingsSetDefaults(&oplinkSettings,0); - PIOS_EEPROM_Save((uint8_t*)&oplinkSettings, sizeof(OPLinkSettingsData)); - for (uint32_t i = 0; i < 10; i++) { - PIOS_DELAY_WaitmS(100); - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); - } - PIOS_IAP_WriteBootCmd(0,0); - PIOS_IAP_WriteBootCmd(1,0); - PIOS_IAP_WriteBootCmd(2,0); - } - /* Read the settings from flash. */ - /* NOTE: We probably need to save/restore the objID here incase the object changed but the size doesn't */ - if (PIOS_EEPROM_Load((uint8_t*)&oplinkSettings, sizeof(OPLinkSettingsData)) == 0) - OPLinkSettingsSet(&oplinkSettings); - else - OPLinkSettingsGet(&oplinkSettings); -#else - OPLinkSettingsGet(&oplinkSettings); + /* IAP System Setup */ + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + OPLinkSettingsGet(&oplinkSettings); + OPLinkSettingsSetDefaults(&oplinkSettings, 0); + PIOS_EEPROM_Save((uint8_t *)&oplinkSettings, sizeof(OPLinkSettingsData)); + for (uint32_t i = 0; i < 10; i++) { + PIOS_DELAY_WaitmS(100); + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); + } + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } + /* Read the settings from flash. */ + /* NOTE: We probably need to save/restore the objID here incase the object changed but the size doesn't */ + if (PIOS_EEPROM_Load((uint8_t *)&oplinkSettings, sizeof(OPLinkSettingsData)) == 0) { + OPLinkSettingsSet(&oplinkSettings); + } else { + OPLinkSettingsGet(&oplinkSettings); + } +#else /* if defined(PIOS_INCLUDE_FLASH_EEPROM) */ + OPLinkSettingsGet(&oplinkSettings); #endif /* PIOS_INCLUDE_FLASH_EEPROM */ - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); #if defined(PIOS_INCLUDE_TIM) - /* Set up pulse timers */ - PIOS_TIM_InitClock(&tim_1_cfg); - PIOS_TIM_InitClock(&tim_2_cfg); - PIOS_TIM_InitClock(&tim_3_cfg); - PIOS_TIM_InitClock(&tim_4_cfg); -#endif /* PIOS_INCLUDE_TIM */ + /* Set up pulse timers */ + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_4_cfg); +#endif /* PIOS_INCLUDE_TIM */ - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); #if defined(PIOS_INCLUDE_USB_CDC) - /* Flags to determine if various USB interfaces are advertised */ - bool usb_cdc_present = false; + /* Flags to determine if various USB interfaces are advertised */ + bool usb_cdc_present = false; - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_cdc_present = true; + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } + usb_cdc_present = true; #else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } #endif - /*Initialize the USB device */ - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + /*Initialize the USB device */ + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); - /* Configure the USB HID port */ - { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + /* Configure the USB HID port */ + { + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } - /* Configure the USB virtual com port (VCP) */ + /* Configure the USB virtual com port (VCP) */ #if defined(PIOS_INCLUDE_USB_CDC) - if (usb_cdc_present) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_VCP_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_VCP_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_VCP_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_VCP_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + if (usb_cdc_present) { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_VCP_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_VCP_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + rx_buffer, PIOS_COM_TELEM_VCP_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_VCP_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } #endif - /* Initalize the RFM22B radio COM device. */ + /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) - { - const struct pios_board_info * bdinfo = &pios_board_info_blob; - const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); - if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { - PIOS_Assert(0); - } + { + const struct pios_board_info *bdinfo = &pios_board_info_blob; + const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); + if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { + PIOS_Assert(0); + } - uint8_t *rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_rfm22b_id, &pios_rfm22b_com_driver, pios_rfm22b_id, - rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_rfm22b_id, &pios_rfm22b_com_driver, pios_rfm22b_id, + rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } - /* Set the RFM22B bindings. */ - PIOS_RFM22B_SetBindings(pios_rfm22b_id, oplinkSettings.Bindings, oplinkSettings.RemoteMainPort, - oplinkSettings.RemoteFlexiPort, oplinkSettings.RemoteVCPPort, oplinkSettings.ComSpeed); - } + /* Set the RFM22B bindings. */ + PIOS_RFM22B_SetBindings(pios_rfm22b_id, oplinkSettings.Bindings, oplinkSettings.RemoteMainPort, + oplinkSettings.RemoteFlexiPort, oplinkSettings.RemoteVCPPort, oplinkSettings.ComSpeed); + } #endif /* PIOS_INCLUDE_RFM22B */ - /* Allocate the uart buffers. */ - pios_uart_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); - pios_uart_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); + /* Allocate the uart buffers. */ + pios_uart_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); + pios_uart_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); - /* Remap AFIO pin */ - GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); + /* Remap AFIO pin */ + GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE); #ifdef PIOS_INCLUDE_ADC - PIOS_ADC_Init(); + PIOS_ADC_Init(); #endif - PIOS_GPIO_Init(); + PIOS_GPIO_Init(); } void PIOS_InitUartMainPort() { #ifndef PIOS_RFM22B_DEBUG_ON_TELEM - uint32_t pios_usart1_id; - if (PIOS_USART_Init(&pios_usart1_id, &pios_usart_serial_cfg)) { - PIOS_Assert(0); - } - PIOS_Assert(pios_uart_rx_buffer); - PIOS_Assert(pios_uart_tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_uart_main_id, &pios_usart_com_driver, pios_usart1_id, - pios_uart_rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, - pios_uart_tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { - PIOS_Assert(0); - } + uint32_t pios_usart1_id; + if (PIOS_USART_Init(&pios_usart1_id, &pios_usart_serial_cfg)) { + PIOS_Assert(0); + } + PIOS_Assert(pios_uart_rx_buffer); + PIOS_Assert(pios_uart_tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_uart_main_id, &pios_usart_com_driver, pios_usart1_id, + pios_uart_rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, + pios_uart_tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { + PIOS_Assert(0); + } #endif } void PIOS_InitUartFlexiPort() { - uint32_t pios_usart3_id; - if (PIOS_USART_Init(&pios_usart3_id, &pios_usart_telem_flexi_cfg)) { - PIOS_Assert(0); - } - PIOS_Assert(pios_uart_rx_buffer); - PIOS_Assert(pios_uart_tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_uart_flexi_id, &pios_usart_com_driver, pios_usart3_id, - pios_uart_rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, - pios_uart_tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { - PIOS_Assert(0); - } + uint32_t pios_usart3_id; + + if (PIOS_USART_Init(&pios_usart3_id, &pios_usart_telem_flexi_cfg)) { + PIOS_Assert(0); + } + PIOS_Assert(pios_uart_rx_buffer); + PIOS_Assert(pios_uart_tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_uart_flexi_id, &pios_usart_com_driver, pios_usart3_id, + pios_uart_rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, + pios_uart_tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { + PIOS_Assert(0); + } } void PIOS_InitPPMMainPort(bool input) { #if defined(PIOS_INCLUDE_PPM) - /* PPM input is configured on the coordinator modem and output on the remote modem. */ - if (input) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_main_cfg); + /* PPM input is configured on the coordinator modem and output on the remote modem. */ + if (input) { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_main_cfg); - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) - PIOS_Assert(0); - } - // For some reason, PPM output on the main port doesn't work. -#endif /* PIOS_INCLUDE_PPM */ + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + } + // For some reason, PPM output on the main port doesn't work. +#endif /* PIOS_INCLUDE_PPM */ } void PIOS_InitPPMFlexiPort(bool input) { #if defined(PIOS_INCLUDE_PPM) - /* PPM input is configured on the coordinator modem and output on the remote modem. */ - if (input) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); + /* PPM input is configured on the coordinator modem and output on the remote modem. */ + if (input) { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) - PIOS_Assert(0); - } + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + } #if defined(PIOS_INCLUDE_PPM_OUT) - else - { - PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_ppm_out_cfg); - } -#endif /* PIOS_INCLUDE_PPM_OUT */ -#endif /* PIOS_INCLUDE_PPM */ + else { + PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_ppm_out_cfg); + } +#endif /* PIOS_INCLUDE_PPM_OUT */ +#endif /* PIOS_INCLUDE_PPM */ } /** diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c b/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c index f63cfa7fd..5750bed21 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c @@ -7,19 +7,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,43 +33,42 @@ * initializes all the core systems on this specific hardware * called from System/openpilot.c */ -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Initialize the alarms library */ - AlarmsInitialize(); + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } - - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); - - /* Initialize the PiOS library */ - PIOS_COM_Init(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize the PiOS library */ + PIOS_COM_Init(); } const struct pios_udp_cfg pios_udp0_cfg = { - .ip = "0.0.0.0", - .port = 9000, + .ip = "0.0.0.0", + .port = 9000, }; const struct pios_udp_cfg pios_udp1_cfg = { - .ip = "0.0.0.0", - .port = 9001, + .ip = "0.0.0.0", + .port = 9001, }; const struct pios_udp_cfg pios_udp2_cfg = { - .ip = "0.0.0.0", - .port = 9002, + .ip = "0.0.0.0", + .port = 9002, }; #ifdef PIOS_COM_AUX @@ -77,8 +76,8 @@ const struct pios_udp_cfg pios_udp2_cfg = { * AUX USART */ const struct pios_udp_cfg pios_udp3_cfg = { - .ip = "0.0.0.0", - .port = 9003, + .ip = "0.0.0.0", + .port = 9003, }; #endif @@ -86,23 +85,23 @@ const struct pios_udp_cfg pios_udp3_cfg = { * Board specific number of devices. */ struct pios_udp_dev pios_udp_devs[] = { -#define PIOS_UDP_TELEM 0 - { - .cfg = &pios_udp0_cfg, - }, -#define PIOS_UDP_GPS 1 - { - .cfg = &pios_udp1_cfg, - }, -#define PIOS_UDP_LOCAL 2 - { - .cfg = &pios_udp2_cfg, - }, +#define PIOS_UDP_TELEM 0 + { + .cfg = &pios_udp0_cfg, + }, +#define PIOS_UDP_GPS 1 + { + .cfg = &pios_udp1_cfg, + }, +#define PIOS_UDP_LOCAL 2 + { + .cfg = &pios_udp2_cfg, + }, #ifdef PIOS_COM_AUX -#define PIOS_UDP_AUX 3 - { - .cfg = &pios_udp3_cfg, - }, +#define PIOS_UDP_AUX 3 + { + .cfg = &pios_udp3_cfg, + }, #endif }; @@ -119,23 +118,23 @@ extern const struct pios_com_driver pios_serial_com_driver; extern const struct pios_com_driver pios_udp_com_driver; struct pios_com_dev pios_com_devs[] = { - { - .id = PIOS_UDP_TELEM, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_GPS, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_LOCAL, - .driver = &pios_udp_com_driver, - }, + { + .id = PIOS_UDP_TELEM, + .driver = &pios_udp_com_driver, + }, + { + .id = PIOS_UDP_GPS, + .driver = &pios_udp_com_driver, + }, + { + .id = PIOS_UDP_LOCAL, + .driver = &pios_udp_com_driver, + }, #ifdef PIOS_COM_AUX - { - .id = PIOS_UDP_AUX, - .driver = &pios_udp_com_driver, - }, + { + .id = PIOS_UDP_AUX, + .driver = &pios_udp_com_driver, + }, #endif }; diff --git a/flight/targets/boards/oplinkmini/pios_board.h b/flight/targets/boards/oplinkmini/pios_board.h index 786508844..bc0a9f321 100644 --- a/flight/targets/boards/oplinkmini/pios_board.h +++ b/flight/targets/boards/oplinkmini/pios_board.h @@ -1,4 +1,4 @@ - /** +/** ****************************************************************************** * * @file pios_board.h @@ -28,150 +28,150 @@ #define ADD_ONE_ADC -//------------------------ +// ------------------------ // Timers and Channels Used -//------------------------ +// ------------------------ /* -Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 -------+-----------+-----------+-----------+---------- -TIM1 | Servo 4 | | | -TIM2 | RC In 5 | RC In 6 | Servo 6 | -TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 -TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 -------+-----------+-----------+-----------+---------- -*/ + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | Servo 4 | | | + TIM2 | RC In 5 | RC In 6 | Servo 6 | + TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 + TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 + ------+-----------+-----------+-----------+---------- + */ -//------------------------ +// ------------------------ // DMA Channels Used -//------------------------ -/* Channel 1 - */ -/* Channel 2 - */ -/* Channel 3 - */ -/* Channel 4 - */ -/* Channel 5 - */ -/* Channel 6 - */ -/* Channel 7 - */ -/* Channel 8 - */ -/* Channel 9 - */ -/* Channel 10 - */ -/* Channel 11 - */ -/* Channel 12 - */ +// ------------------------ +/* Channel 1 - */ +/* Channel 2 - */ +/* Channel 3 - */ +/* Channel 4 - */ +/* Channel 5 - */ +/* Channel 6 - */ +/* Channel 7 - */ +/* Channel 8 - */ +/* Channel 9 - */ +/* Channel 10 - */ +/* Channel 11 - */ +/* Channel 12 - */ -//------------------------ +// ------------------------ // BOOTLOADER_SETTINGS -//------------------------ -#define BOARD_READABLE TRUE -#define BOARD_WRITABLE TRUE -#define MAX_DEL_RETRYS 3 +// ------------------------ +#define BOARD_READABLE TRUE +#define BOARD_WRITABLE TRUE +#define MAX_DEL_RETRYS 3 -//------------------------ +// ------------------------ // WATCHDOG_SETTINGS -//------------------------ -#define PIOS_WATCHDOG_TIMEOUT 500 -#define PIOS_WDG_REGISTER BKP_DR4 -#define PIOS_WDG_TELEMETRYTX 0x0001 -#define PIOS_WDG_TELEMETRYRX 0x0002 -#define PIOS_WDG_RADIOTX 0x0004 -#define PIOS_WDG_RADIORX 0x0008 -#define PIOS_WDG_RFM22B 0x0016 +// ------------------------ +#define PIOS_WATCHDOG_TIMEOUT 500 +#define PIOS_WDG_REGISTER BKP_DR4 +#define PIOS_WDG_TELEMETRYTX 0x0001 +#define PIOS_WDG_TELEMETRYRX 0x0002 +#define PIOS_WDG_RADIOTX 0x0004 +#define PIOS_WDG_RADIORX 0x0008 +#define PIOS_WDG_RFM22B 0x0016 -//------------------------ +// ------------------------ // TELEMETRY -//------------------------ -#define TELEM_QUEUE_SIZE 20 +// ------------------------ +#define TELEM_QUEUE_SIZE 20 -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -#define PIOS_LED_USB 0 -#define PIOS_LED_LINK 1 -#define PIOS_LED_RX 2 -#define PIOS_LED_TX 3 +// ------------------------ +#define PIOS_LED_USB 0 +#define PIOS_LED_LINK 1 +#define PIOS_LED_RX 2 +#define PIOS_LED_TX 3 #ifdef PIOS_RFM22B_DEBUG_ON_TELEM -#define PIOS_LED_D1 4 -#define PIOS_LED_D2 5 -#define PIOS_LED_D3 6 -#define PIOS_LED_D4 7 +#define PIOS_LED_D1 4 +#define PIOS_LED_D2 5 +#define PIOS_LED_D3 6 +#define PIOS_LED_D4 7 #endif -#define PIOS_LED_HEARTBEAT PIOS_LED_LINK -#define PIOS_LED_ALARM PIOS_LED_TX +#define PIOS_LED_HEARTBEAT PIOS_LED_LINK +#define PIOS_LED_ALARM PIOS_LED_TX -#define USB_LED_ON PIOS_LED_On(PIOS_LED_USB) -#define USB_LED_OFF PIOS_LED_Off(PIOS_LED_USB) -#define USB_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_USB) +#define USB_LED_ON PIOS_LED_On(PIOS_LED_USB) +#define USB_LED_OFF PIOS_LED_Off(PIOS_LED_USB) +#define USB_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_USB) -#define LINK_LED_ON PIOS_LED_On(PIOS_LED_LINK) -#define LINK_LED_OFF PIOS_LED_Off(PIOS_LED_LINK) -#define LINK_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_LINK) +#define LINK_LED_ON PIOS_LED_On(PIOS_LED_LINK) +#define LINK_LED_OFF PIOS_LED_Off(PIOS_LED_LINK) +#define LINK_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_LINK) -#define RX_LED_ON PIOS_LED_On(PIOS_LED_RX) -#define RX_LED_OFF PIOS_LED_Off(PIOS_LED_RX) -#define RX_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_RX) +#define RX_LED_ON PIOS_LED_On(PIOS_LED_RX) +#define RX_LED_OFF PIOS_LED_Off(PIOS_LED_RX) +#define RX_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_RX) -#define TX_LED_ON PIOS_LED_On(PIOS_LED_TX) -#define TX_LED_OFF PIOS_LED_Off(PIOS_LED_TX) -#define TX_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_TX) +#define TX_LED_ON PIOS_LED_On(PIOS_LED_TX) +#define TX_LED_OFF PIOS_LED_Off(PIOS_LED_TX) +#define TX_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_TX) #ifdef PIOS_RFM22B_DEBUG_ON_TELEM -#define D1_LED_ON PIOS_LED_On(PIOS_LED_D1) -#define D1_LED_OFF PIOS_LED_Off(PIOS_LED_D1) -#define D1_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D1) +#define D1_LED_ON PIOS_LED_On(PIOS_LED_D1) +#define D1_LED_OFF PIOS_LED_Off(PIOS_LED_D1) +#define D1_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D1) -#define D2_LED_ON PIOS_LED_On(PIOS_LED_D2) -#define D2_LED_OFF PIOS_LED_Off(PIOS_LED_D2) -#define D2_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D2) +#define D2_LED_ON PIOS_LED_On(PIOS_LED_D2) +#define D2_LED_OFF PIOS_LED_Off(PIOS_LED_D2) +#define D2_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D2) -#define D3_LED_ON PIOS_LED_On(PIOS_LED_D3) -#define D3_LED_OFF PIOS_LED_Off(PIOS_LED_D3) -#define D3_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D3) +#define D3_LED_ON PIOS_LED_On(PIOS_LED_D3) +#define D3_LED_OFF PIOS_LED_Off(PIOS_LED_D3) +#define D3_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D3) -#define D4_LED_ON PIOS_LED_On(PIOS_LED_D4) -#define D4_LED_OFF PIOS_LED_Off(PIOS_LED_D4) -#define D4_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D4) +#define D4_LED_ON PIOS_LED_On(PIOS_LED_D4) +#define D4_LED_OFF PIOS_LED_Off(PIOS_LED_D4) +#define D4_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D4) #endif -//------------------------- +// ------------------------- // System Settings -//------------------------- -#define PIOS_MASTER_CLOCK 72000000 -#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) +// ------------------------- +#define PIOS_MASTER_CLOCK 72000000 +#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) -//------------------------- +// ------------------------- // Interrupt Priorities -//------------------------- -#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS -#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS -#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... -#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... -//------------------------ +// ------------------------ // PIOS_I2C // See also pios_board.c -//------------------------ -#define PIOS_I2C_MAX_DEVS 1 +// ------------------------ +#define PIOS_I2C_MAX_DEVS 1 extern uint32_t pios_i2c_flexi_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexi_adapter_id) +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexi_adapter_id) -//------------------------ +// ------------------------ // PIOS_SPI // See also pios_board.c -//------------------------ -#define PIOS_SPI_MAX_DEVS 1 +// ------------------------ +#define PIOS_SPI_MAX_DEVS 1 -//------------------------- +// ------------------------- // PIOS_USART -//------------------------- -#define PIOS_USART_MAX_DEVS 3 +// ------------------------- +#define PIOS_USART_MAX_DEVS 3 -//------------------------- +// ------------------------- // PIOS_COM // // See also pios_board.c -//------------------------- -#define PIOS_COM_MAX_DEVS 5 +// ------------------------- +#define PIOS_COM_MAX_DEVS 5 extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_telem_vcp_id; @@ -182,135 +182,135 @@ extern uint32_t pios_com_rfm22b_id; extern uint32_t pios_com_radio_id; extern uint32_t pios_ppm_rcvr_id; extern uint32_t pios_ppm_out_id; -#define PIOS_COM_TELEM_USB_HID (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_USB PIOS_COM_TELEM_USB_HID -#define PIOS_COM_TELEM_USB_VCP (pios_com_telem_vcp_id) -#define PIOS_COM_TELEM_UART_MAIN (pios_com_telem_uart_main_id) -#define PIOS_COM_TELEM_UART_FLEXI (pios_com_telem_uart_flexi_id) -#define PIOS_COM_TELEMETRY (pios_com_telemetry_id) -#define PIOS_COM_RFM22B (pios_com_rfm22b_id) -#define PIOS_COM_RADIO (pios_com_radio_id) -#define PIOS_PPM_RECEIVER (pios_ppm_rcvr_id) -#define PIOS_PPM_OUTPUT (pios_ppm_out_id) +#define PIOS_COM_TELEM_USB_HID (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_USB PIOS_COM_TELEM_USB_HID +#define PIOS_COM_TELEM_USB_VCP (pios_com_telem_vcp_id) +#define PIOS_COM_TELEM_UART_MAIN (pios_com_telem_uart_main_id) +#define PIOS_COM_TELEM_UART_FLEXI (pios_com_telem_uart_flexi_id) +#define PIOS_COM_TELEMETRY (pios_com_telemetry_id) +#define PIOS_COM_RFM22B (pios_com_rfm22b_id) +#define PIOS_COM_RADIO (pios_com_radio_id) +#define PIOS_PPM_RECEIVER (pios_ppm_rcvr_id) +#define PIOS_PPM_OUTPUT (pios_ppm_out_id) -#define RFM22_DEBUG 1 +#define RFM22_DEBUG 1 -//------------------------- +// ------------------------- // ADC // None -//------------------------- -//#define PIOS_ADC_OVERSAMPLING_RATE 1 -#define PIOS_ADC_USE_TEMP_SENSOR 0 -#define PIOS_ADC_TEMP_SENSOR_ADC ADC1 -#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 1 +// ------------------------- +// #define PIOS_ADC_OVERSAMPLING_RATE 1 +#define PIOS_ADC_USE_TEMP_SENSOR 0 +#define PIOS_ADC_TEMP_SENSOR_ADC ADC1 +#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 1 -#define PIOS_ADC_NUM_PINS 0 +#define PIOS_ADC_NUM_PINS 0 -#define PIOS_ADC_PORTS { } -#define PIOS_ADC_PINS { } -#define PIOS_ADC_CHANNELS { } -#define PIOS_ADC_MAPPING { } -#define PIOS_ADC_CHANNEL_MAPPING { } -#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR) -#define PIOS_ADC_NUM_ADC_CHANNELS 0 -#define PIOS_ADC_USE_ADC2 0 -#define PIOS_ADC_CLOCK_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE) -#define PIOS_ADC_ADCCLK RCC_PCLK2_Div8 +#define PIOS_ADC_PORTS {} +#define PIOS_ADC_PINS {} +#define PIOS_ADC_CHANNELS {} +#define PIOS_ADC_MAPPING {} +#define PIOS_ADC_CHANNEL_MAPPING {} +#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR) +#define PIOS_ADC_NUM_ADC_CHANNELS 0 +#define PIOS_ADC_USE_ADC2 0 +#define PIOS_ADC_CLOCK_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE) +#define PIOS_ADC_ADCCLK RCC_PCLK2_Div8 /* RCC_PCLK2_Div2: ADC clock = PCLK2/2 */ /* RCC_PCLK2_Div4: ADC clock = PCLK2/4 */ /* RCC_PCLK2_Div6: ADC clock = PCLK2/6 */ /* RCC_PCLK2_Div8: ADC clock = PCLK2/8 */ -#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_239Cycles5 +#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_239Cycles5 /* Sample time: */ /* With an ADCCLK = 14 MHz and a sampling time of 239.5 cycles: */ /* Tconv = 239.5 + 12.5 = 252 cycles = 18�s */ /* (1 / (ADCCLK / CYCLES)) = Sample Time (�S) */ -#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_LOW +#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_LOW // Currently analog acquistion hard coded at 480 Hz // PCKL2 = HCLK / 16 // ADCCLK = PCLK2 / 2 -#define PIOS_ADC_RATE (72.0e6 / 1.0 / 8.0 / 252.0 / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) -#define PIOS_ADC_MAX_OVERSAMPLING 36 +#define PIOS_ADC_RATE (72.0e6 / 1.0 / 8.0 / 252.0 / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) +#define PIOS_ADC_MAX_OVERSAMPLING 36 -//------------------------ +// ------------------------ // PIOS_RCVR // See also pios_board.c -//------------------------ -#define PIOS_RCVR_MAX_DEVS 3 -#define PIOS_RCVR_MAX_CHANNELS 12 -#define PIOS_GCSRCVR_TIMEOUT_MS 100 +// ------------------------ +#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_GCSRCVR_TIMEOUT_MS 100 -//------------------------- +// ------------------------- // Receiver PPM input -//------------------------- -#define PIOS_PPM_MAX_DEVS 1 -#define PIOS_PPM_NUM_INPUTS 8 +// ------------------------- +#define PIOS_PPM_MAX_DEVS 1 +#define PIOS_PPM_NUM_INPUTS 8 -//------------------------- +// ------------------------- // Servo outputs -//------------------------- -#define PIOS_SERVO_UPDATE_HZ 50 -#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ -//-------------------------- +// -------------------------- // Timer controller settings -//-------------------------- -#define PIOS_TIM_MAX_DEVS 3 +// -------------------------- +#define PIOS_TIM_MAX_DEVS 3 -//------------------------- +// ------------------------- // GPIO -//------------------------- -#define PIOS_GPIO_PORTS { } -#define PIOS_GPIO_PINS { } -#define PIOS_GPIO_CLKS { } -#define PIOS_GPIO_NUM 0 +// ------------------------- +#define PIOS_GPIO_PORTS {} +#define PIOS_GPIO_PINS {} +#define PIOS_GPIO_CLKS {} +#define PIOS_GPIO_NUM 0 -//------------------------- +// ------------------------- // USB -//------------------------- -#define PIOS_USB_HID_MAX_DEVS 1 -#define PIOS_USB_ENABLED 1 -#define PIOS_USB_HID_MAX_DEVS 1 -#define PIOS_USB_MAX_DEVS 1 -#define PIOS_USB_DETECT_GPIO_PORT GPIOC -#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 -#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line15 +// ------------------------- +#define PIOS_USB_HID_MAX_DEVS 1 +#define PIOS_USB_ENABLED 1 +#define PIOS_USB_HID_MAX_DEVS 1 +#define PIOS_USB_MAX_DEVS 1 +#define PIOS_USB_DETECT_GPIO_PORT GPIOC +#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 +#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line15 -//------------------------- +// ------------------------- // RFM22 -//------------------------- +// ------------------------- #if defined(PIOS_INCLUDE_RFM22B) extern uint32_t pios_spi_rfm22b_id; -#define PIOS_RFM22_SPI_PORT (pios_spi_rfm22b_id) +#define PIOS_RFM22_SPI_PORT (pios_spi_rfm22b_id) extern uint32_t pios_rfm22b_id; #endif /* PIOS_INCLUDE_RFM22B */ -//------------------------- +// ------------------------- // Packet Handler -//------------------------- +// ------------------------- extern uint32_t pios_packet_handler; -#define PIOS_PACKET_HANDLER (pios_packet_handler) -#define PIOS_PH_MAX_PACKET 255 -#define PIOS_PH_WIN_SIZE 3 +#define PIOS_PACKET_HANDLER (pios_packet_handler) +#define PIOS_PH_MAX_PACKET 255 +#define PIOS_PH_WIN_SIZE 3 #define PIOS_PH_MAX_CONNECTIONS 1 -#define RS_ECC_NPARITY 4 +#define RS_ECC_NPARITY 4 -//------------------------- +// ------------------------- // Reed-Solomon ECC -//------------------------- +// ------------------------- #define RS_ECC_NPARITY 4 -//------------------------- +// ------------------------- // Flash EEPROM Emulation -//------------------------- +// ------------------------- -#define PIOS_FLASH_SIZE 0x20000 +#define PIOS_FLASH_SIZE 0x20000 #define PIOS_FLASH_EEPROM_START_ADDR 0x08000000 -#define PIOS_FLASH_PAGE_SIZE 1024 -#define PIOS_FLASH_EEPROM_ADDR (PIOS_FLASH_EEPROM_START_ADDR + PIOS_FLASH_SIZE - PIOS_FLASH_PAGE_SIZE) -#define PIOS_FLASH_EEPROM_LEN PIOS_FLASH_PAGE_SIZE +#define PIOS_FLASH_PAGE_SIZE 1024 +#define PIOS_FLASH_EEPROM_ADDR (PIOS_FLASH_EEPROM_START_ADDR + PIOS_FLASH_SIZE - PIOS_FLASH_PAGE_SIZE) +#define PIOS_FLASH_EEPROM_LEN PIOS_FLASH_PAGE_SIZE #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/oplinkmini/pios_usb_board_data.c b/flight/targets/boards/oplinkmini/pios_usb_board_data.c index 070d8c392..5aa5ab5f9 100644 --- a/flight/targets/boards/oplinkmini/pios_usb_board_data.c +++ b/flight/targets/boards/oplinkmini/pios_usb_board_data.c @@ -28,70 +28,71 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ -#include /* PIOS_SYS_SerialNumberGet */ -#include /* PIOS_USBHOOK_* */ -#include /* PIOS_USB_UTIL_AsciiToUtf8 */ +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ static const uint8_t usb_product_id[20] = { - sizeof(usb_product_id), - USB_DESC_TYPE_STRING, - 'P', 0, - 'i', 0, - 'p', 0, - 'X', 0, - 't', 0, - 'r', 0, - 'e', 0, - 'm', 0, - 'e', 0, + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'P', 0, + 'i', 0, + 'p', 0, + 'X', 0, + 't', 0, + 'r', 0, + 'e', 0, + 'm', 0, + 'e', 0, }; -static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = { - sizeof(usb_serial_number), - USB_DESC_TYPE_STRING, +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, }; static const struct usb_string_langid usb_lang_id = { - .bLength = sizeof(usb_lang_id), - .bDescriptorType = USB_DESC_TYPE_STRING, - .bLangID = htousbs(USB_LANGID_ENGLISH_US), + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), }; static const uint8_t usb_vendor_id[28] = { - sizeof(usb_vendor_id), - USB_DESC_TYPE_STRING, - 'o', 0, - 'p', 0, - 'e', 0, - 'n', 0, - 'p', 0, - 'i', 0, - 'l', 0, - 'o', 0, - 't', 0, - '.', 0, - 'o', 0, - 'r', 0, - 'g', 0 + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 }; int32_t PIOS_USB_BOARD_DATA_Init(void) { - /* Load device serial number into serial number string */ - uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - PIOS_SYS_SerialNumberGet((char *)sn); + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ - uint8_t * utf8 = &(usb_serial_number[2]); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1); + PIOS_SYS_SerialNumberGet((char *)sn); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); - return 0; + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; } diff --git a/flight/targets/boards/osd/board_hw_defs.c b/flight/targets/boards/osd/board_hw_defs.c index 95f142788..c00c4ba69 100644 --- a/flight/targets/boards/osd/board_hw_defs.c +++ b/flight/targets/boards/osd/board_hw_defs.c @@ -11,19 +11,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,61 +31,60 @@ #include static const struct pios_led pios_leds[] = { - [PIOS_LED_HEARTBEAT] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_ALARM] = { - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_ALARM] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, }; static const struct pios_led_cfg pios_led_cfg = { - .leds = pios_leds, - .num_leds = NELEMENTS(pios_leds), + .leds = pios_leds, + .num_leds = NELEMENTS(pios_leds), }; -const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (__attribute__((unused)) uint32_t board_revision) +const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision) { - return &pios_led_cfg; + return &pios_led_cfg; } -#endif /* PIOS_INCLUDE_LED */ +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_FLASH) #include "pios_flashfs_logfs_priv.h" #include "pios_flash_internal_priv.h" -static const struct pios_flash_internal_cfg flash_internal_cfg = { -}; +static const struct pios_flash_internal_cfg flash_internal_cfg = {}; static const struct flashfs_logfs_cfg flashfs_internal_cfg = { -.fs_magic = 0x99abcfef, -.total_fs_size = EE_BANK_SIZE, /* 32K bytes (2x16KB sectors) */ -.arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 1 sector */ -.slot_size = 0x00000100, /* 256 bytes */ + .fs_magic = 0x99abcfef, + .total_fs_size = EE_BANK_SIZE, /* 32K bytes (2x16KB sectors) */ + .arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 1 sector */ + .slot_size = 0x00000100, /* 256 bytes */ -.start_offset = EE_BANK_BASE, /* start after the bootloader */ -.sector_size = 0x00004000, /* 16K bytes */ -.page_size = 0x00004000, /* 16K bytes */ + .start_offset = EE_BANK_BASE, /* start after the bootloader */ + .sector_size = 0x00004000, /* 16K bytes */ + .page_size = 0x00004000, /* 16K bytes */ }; #include "pios_flash.h" @@ -103,122 +102,123 @@ static const struct flashfs_logfs_cfg flashfs_internal_cfg = { * .rodata section (ie. Flash) rather than in the .bss section (RAM). */ void PIOS_SPI_sdcard_irq_handler(void); -void DMA1_Channel2_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler"))); -void DMA1_Channel3_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler"))); +void DMA1_Channel2_IRQHandler() __attribute__((alias("PIOS_SPI_sdcard_irq_handler"))); +void DMA1_Channel3_IRQHandler() __attribute__((alias("PIOS_SPI_sdcard_irq_handler"))); static const struct pios_spi_cfg pios_spi_sdcard_cfg = { - .regs = SPI2, - .remap = GPIO_AF_SPI2, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* Maximum divider (ie. slowest clock rate) */ - }, - .use_crc = false, - .dma = { - .irq = { - .flags = (DMA_IT_TCIF3|DMA_IT_TEIF3|DMA_IT_HTIF3|DMA_IT_TCIF3), - .init = { - .NVIC_IRQChannel = DMA1_Stream3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA1_Stream3, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_Single, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA1_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_Single, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .slave_count = 1, - .ssel = {{ - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }}, + .regs = SPI2, + .remap = GPIO_AF_SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* Maximum divider (ie. slowest clock rate) */ + }, + .use_crc = false, + .dma = { + .irq = { + .flags = (DMA_IT_TCIF3 | DMA_IT_TEIF3 | DMA_IT_HTIF3 | DMA_IT_TCIF3), + .init = { + .NVIC_IRQChannel = DMA1_Stream3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA1_Stream3, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + .tx = { + .channel = DMA1_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, }; static uint32_t pios_spi_sdcard_id; void PIOS_SPI_sdcard_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_sdcard_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_sdcard_id); } -#endif - +#endif /* if defined(PIOS_INCLUDE_SPI) */ #include @@ -229,45 +229,45 @@ void PIOS_SPI_sdcard_irq_handler(void) * GPS USART */ static const struct pios_usart_cfg pios_usart_gps_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART1, + .remap = GPIO_AF_USART1, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_INCLUDE_GPS */ @@ -276,45 +276,45 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = { * AUX USART */ static const struct pios_usart_cfg pios_usart_aux_cfg = { - .regs = USART2, - .remap = GPIO_AF_USART2, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART2, + .remap = GPIO_AF_USART2, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_COM_AUX */ @@ -324,45 +324,45 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { * Telemetry on main USART */ static const struct pios_usart_cfg pios_usart_telem_main_cfg = { - .regs = UART4, - .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = UART4, + .remap = GPIO_AF_UART4, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = UART4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_COM_TELEM */ @@ -382,74 +382,74 @@ static const struct pios_usart_cfg pios_usart_telem_main_cfg = { */ void PIOS_I2C_flexiport_adapter_ev_irq_handler(void); void PIOS_I2C_flexiport_adapter_er_irq_handler(void); -void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexiport_adapter_ev_irq_handler"))); -void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexiport_adapter_er_irq_handler"))); +void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_ev_irq_handler"))); +void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_er_irq_handler"))); static const struct pios_i2c_adapter_cfg pios_i2c_flexiport_adapter_cfg = { - .regs = I2C2, - .remap = GPIO_AF_I2C2, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, + .regs = I2C2, + .remap = GPIO_AF_I2C2, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; uint32_t pios_i2c_flexiport_adapter_id; void PIOS_I2C_flexiport_adapter_ev_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexiport_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexiport_adapter_id); } void PIOS_I2C_flexiport_adapter_er_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexiport_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexiport_adapter_id); } -#endif +#endif /* if defined(PIOS_INCLUDE_I2C) */ #if defined(PIOS_INCLUDE_RTC) /* @@ -457,53 +457,53 @@ void PIOS_I2C_flexiport_adapter_er_irq_handler(void) */ #include -void PIOS_RTC_IRQ_Handler (void); -void RTC_WKUP_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler"))); +void PIOS_RTC_IRQ_Handler(void); +void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); static const struct pios_rtc_cfg pios_rtc_main_cfg = { - .clksrc = RCC_RTCCLKSource_HSE_Div8, // Divide 8 Mhz crystal down to 1 - // For some reason it's acting like crystal is 16 Mhz. This clock is then divided - // by another 16 to give a nominal 62.5 khz clock - .prescaler = 100, // Every 100 cycles gives 625 Hz - .irq = { - .init = { - .NVIC_IRQChannel = RTC_WKUP_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .clksrc = RCC_RTCCLKSource_HSE_Div8, // Divide 8 Mhz crystal down to 1 + // For some reason it's acting like crystal is 16 Mhz. This clock is then divided + // by another 16 to give a nominal 62.5 khz clock + .prescaler = 100, // Every 100 cycles gives 625 Hz + .irq = { + .init = { + .NVIC_IRQChannel = RTC_WKUP_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; -void PIOS_RTC_IRQ_Handler (void) +void PIOS_RTC_IRQ_Handler(void) { - PIOS_RTC_irq_handler (); + PIOS_RTC_irq_handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_RTC) */ #if defined(PIOS_INCLUDE_USB) #include "pios_usb_priv.h" static const struct pios_usb_cfg pios_usb_main_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = OTG_FS_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_25MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - }, + .irq = { + .init = { + .NVIC_IRQChannel = OTG_FS_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, }, - .vsense_active_low = false + }, + .vsense = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_25MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + }, + }, + .vsense_active_low = false }; #include "pios_usb_board_data_priv.h" @@ -511,7 +511,7 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { #include "pios_usb_desc_hid_only_priv.h" #include "pios_usbhook.h" -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ #if defined(PIOS_INCLUDE_COM_MSG) @@ -523,9 +523,9 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { #include const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, + .data_if = 0, + .data_rx_ep = 1, + .data_tx_ep = 1, }; #endif /* PIOS_INCLUDE_USB_HID */ @@ -533,36 +533,36 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { #include const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 1, - .ctrl_tx_ep = 2, + .ctrl_if = 1, + .ctrl_tx_ep = 2, - .data_if = 2, - .data_rx_ep = 3, - .data_tx_ep = 3, + .data_if = 2, + .data_rx_ep = 3, + .data_tx_ep = 3, }; -#endif /* PIOS_INCLUDE_USB_CDC */ +#endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_VIDEO) static const TIM_TimeBaseInitTypeDef tim_8_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / 50000) - 1), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / 50000) - 1), + .TIM_RepetitionCounter = 0x0000, }; static const struct pios_tim_clock_cfg tim_8_cfg = { - .timer = TIM8, - .time_base_init = &tim_8_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM8_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM8, + .time_base_init = &tim_8_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; /** @@ -570,404 +570,401 @@ static const struct pios_tim_clock_cfg tim_8_cfg = { */ #include static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - { - .timer = TIM8, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource8, - }, - .remap = GPIO_AF_TIM8, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource9, - }, - .remap = GPIO_AF_TIM8, - }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource8, + }, + .remap = GPIO_AF_TIM8, + }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource9, + }, + .remap = GPIO_AF_TIM8, + }, }; const struct pios_servo_cfg pios_servo_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = 0, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_all_pins, - .num_channels = NELEMENTS(pios_tim_servoport_all_pins), + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = 0, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_all_pins, + .num_channels = NELEMENTS(pios_tim_servoport_all_pins), }; - #include static const struct pios_exti_cfg pios_exti_hsync_cfg __exti_config = { - .vector = PIOS_Hsync_ISR, - .line = EXTI_Line7, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line7, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - //.EXTI_Trigger = EXTI_Trigger_Rising_Falling, - .EXTI_Trigger = EXTI_Trigger_Falling, - //.EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_Hsync_ISR, + .line = EXTI_Line7, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line7, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + // .EXTI_Trigger = EXTI_Trigger_Rising_Falling, + .EXTI_Trigger = EXTI_Trigger_Falling, + // .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_exti_cfg pios_exti_vsync_cfg __exti_config = { - .vector = PIOS_Vsync_ISR, - .line = EXTI_Line5, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line5, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Falling, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_Vsync_ISR, + .line = EXTI_Line5, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line5, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Falling, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_video_cfg pios_video_cfg = { - .mask = { - .regs = SPI3, - .remap = GPIO_AF_SPI3, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_1Line_Tx, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_Low, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, - }, - .use_crc = false, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case RX) - .flags = (DMA_IT_TCIF7), - .init = { - .NVIC_IRQChannel = DMA1_Stream7_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - /*.rx = {},*/ - .tx = { - .channel = DMA1_Stream7, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_BufferSize = BUFFER_LINE_LENGTH, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_VeryHigh, - .DMA_FIFOMode = DMA_FIFOMode_Enable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_INC4, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .miso = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - /*.mosi = {},*/ - .slave_count = 1, - }, - .level = { - .regs = SPI1, - .remap = GPIO_AF_SPI1, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_1Line_Tx, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_Low, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, - }, - .use_crc = false, - .dma = { - .irq = { - .flags = (DMA_IT_TCIF5), - .init = { - .NVIC_IRQChannel = DMA2_Stream5_IRQn, - .NVIC_IRQChannelPreemptionPriority = 0, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - /*.rx = {},*/ - .tx = { - .channel = DMA2_Stream5, - .init = { - .DMA_Channel = DMA_Channel_3, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_BufferSize = BUFFER_LINE_LENGTH, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_VeryHigh, - .DMA_FIFOMode = DMA_FIFOMode_Enable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_INC4, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - /*.mosi = {},*/ - .slave_count = 1, + .mask = { + .regs = SPI3, + .remap = GPIO_AF_SPI3, + .init = { + .SPI_Mode = SPI_Mode_Slave, + .SPI_Direction = SPI_Direction_1Line_Tx, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_Low, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + }, + .use_crc = false, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case RX) + .flags = (DMA_IT_TCIF7), + .init = { + .NVIC_IRQChannel = DMA1_Stream7_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + /*.rx = {},*/ + .tx = { + .channel = DMA1_Stream7, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_BufferSize = BUFFER_LINE_LENGTH, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_VeryHigh, + .DMA_FIFOMode = DMA_FIFOMode_Enable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_INC4, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .sclk = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .miso = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + /*.mosi = {},*/ + .slave_count = 1, + }, + .level = { + .regs = SPI1, + .remap = GPIO_AF_SPI1, + .init = { + .SPI_Mode = SPI_Mode_Slave, + .SPI_Direction = SPI_Direction_1Line_Tx, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_Low, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + }, + .use_crc = false, + .dma = { + .irq = { + .flags = (DMA_IT_TCIF5), + .init = { + .NVIC_IRQChannel = DMA2_Stream5_IRQn, + .NVIC_IRQChannelPreemptionPriority = 0, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + /*.rx = {},*/ + .tx = { + .channel = DMA2_Stream5, + .init = { + .DMA_Channel = DMA_Channel_3, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_BufferSize = BUFFER_LINE_LENGTH, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_VeryHigh, + .DMA_FIFOMode = DMA_FIFOMode_Enable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_INC4, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + /*.mosi = {},*/ + .slave_count = 1, + }, - }, + .hsync = &pios_exti_hsync_cfg, + .vsync = &pios_exti_vsync_cfg, - .hsync = &pios_exti_hsync_cfg, - .vsync = &pios_exti_vsync_cfg, - - .pixel_timer = { - .timer = TIM4, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource6, - }, - .remap = GPIO_AF_TIM4, - }, - .hsync_capture = { - .timer = TIM4, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource7, - }, - .remap = GPIO_AF_TIM4, - }, - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = 1, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, + .pixel_timer = { + .timer = TIM4, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource6, + }, + .remap = GPIO_AF_TIM4, + }, + .hsync_capture = { + .timer = TIM4, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource7, + }, + .remap = GPIO_AF_TIM4, + }, + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = 1, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, }; -#endif +#endif /* if defined(PIOS_INCLUDE_VIDEO) */ #if defined(PIOS_INCLUDE_WAVE) #include #include static const TIM_TimeBaseInitTypeDef tim_6_time_base = { - .TIM_Prescaler = 0, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = (PIOS_PERIPHERAL_APB1_CLOCK / 44100), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = 0, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = (PIOS_PERIPHERAL_APB1_CLOCK / 44100), + .TIM_RepetitionCounter = 0x0000, }; static const struct pios_tim_clock_cfg tim_6_cfg = { - .timer = TIM6, - .time_base_init = &tim_6_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM6_DAC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM6, + .time_base_init = &tim_6_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM6_DAC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_dac_cfg pios_dac_cfg = { - .timer = TIM6, - .time_base_init = { - .TIM_Prescaler = 0, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = (PIOS_PERIPHERAL_APB1_CLOCK / 44100), - .TIM_RepetitionCounter = 0x0000, - }, - .irq = { - .init = { - .NVIC_IRQChannel = TIM6_DAC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case RX) - .flags = (DMA_IT_TCIF5), - .init = { - .NVIC_IRQChannel = DMA1_Stream5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM6, + .time_base_init = { + .TIM_Prescaler = 0, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = (PIOS_PERIPHERAL_APB1_CLOCK / 44100), + .TIM_RepetitionCounter = 0x0000, + }, + .irq = { + .init = { + .NVIC_IRQChannel = TIM6_DAC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case RX) + .flags = (DMA_IT_TCIF5), + .init = { + .NVIC_IRQChannel = DMA1_Stream5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, - .rx = {}, - .tx = { - .channel = DMA1_Stream5, - .init = { - .DMA_Channel = DMA_Channel_7, - .DMA_PeripheralBaseAddr = (uint32_t)&DAC->DHR8R1, - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_BufferSize = BUFFERSIZE, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_High, - .DMA_FIFOMode = DMA_FIFOMode_Disable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_Single, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .channel = DAC_Channel_1, - .dac_init = { - .DAC_Trigger = DAC_Trigger_T6_TRGO, - .DAC_WaveGeneration = DAC_WaveGeneration_None, - .DAC_OutputBuffer = DAC_OutputBuffer_Enable, - }, - .dac_io = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AN, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, + .rx = {}, + .tx = { + .channel = DMA1_Stream5, + .init = { + .DMA_Channel = DMA_Channel_7, + .DMA_PeripheralBaseAddr = (uint32_t)&DAC->DHR8R1, + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_BufferSize = BUFFERSIZE, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_High, + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .channel = DAC_Channel_1, + .dac_init = { + .DAC_Trigger = DAC_Trigger_T6_TRGO, + .DAC_WaveGeneration = DAC_WaveGeneration_None, + .DAC_OutputBuffer = DAC_OutputBuffer_Enable, + }, + .dac_io = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AN, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, }; -#endif - +#endif /* if defined(PIOS_INCLUDE_WAVE) */ diff --git a/flight/targets/boards/osd/bootloader/inc/common.h b/flight/targets/boards/osd/bootloader/inc/common.h index 9010fddd9..e64b89014 100644 --- a/flight/targets/boards/osd/bootloader/inc/common.h +++ b/flight/targets/boards/osd/bootloader/inc/common.h @@ -28,10 +28,10 @@ #ifndef COMMON_H_ #define COMMON_H_ -//#include "board.h" +// #include "board.h" typedef enum { - start, keepgoing, + start, keepgoing, } DownloadAction; /**************************************************/ @@ -39,77 +39,77 @@ typedef enum { /**************************************************/ typedef enum { - DFUidle, //0 - uploading, //1 - wrong_packet_received, //2 - too_many_packets, //3 - too_few_packets, //4 - Last_operation_Success, //5 - downloading, //6 - BLidle, //7 - Last_operation_failed, //8 - uploadingStarting, //9 - outsideDevCapabilities, //10 - CRC_Fail,//11 - failed_jump, -//12 + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + BLidle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, +// 12 } DFUStates; /**************************************************/ /* OP_DFU commands */ /**************************************************/ typedef enum { - Reserved, //0 - Req_Capabilities, //1 - Rep_Capabilities, //2 - EnterDFU, //3 - JumpFW, //4 - Reset, //5 - Abort_Operation, //6 - Upload, //7 - Op_END, //8 - Download_Req, //9 - Download, //10 - Status_Request, //11 - Status_Rep -//12 + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep +// 12 } DFUCommands; typedef enum { - High_Density, Medium_Density + High_Density, Medium_Density } DeviceType; /**************************************************/ /* OP_DFU transfer types */ /**************************************************/ typedef enum { - FW, //0 - Descript -//2 + FW, // 0 + Descript +// 2 } DFUTransfer; /**************************************************/ /* OP_DFU transfer port */ /**************************************************/ typedef enum { - Usb, //0 - Serial -//2 + Usb, // 0 + Serial +// 2 } DFUPort; /**************************************************/ /* OP_DFU programable programable HW types */ /**************************************************/ typedef enum { - Self_flash, //0 - Remote_flash_via_spi -//1 + Self_flash, // 0 + Remote_flash_via_spi +// 1 } DFUProgType; /**************************************************/ /* OP_DFU programable sources */ /**************************************************/ -#define USB 0 -#define SPI 1 +#define USB 0 +#define SPI 1 -#define DownloadDelay 100000 +#define DownloadDelay 100000 -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 +#define MAX_DEL_RETRYS 3 +#define MAX_WRI_RETRYS 3 #endif /* COMMON_H_ */ diff --git a/flight/targets/boards/osd/bootloader/inc/pios_config.h b/flight/targets/boards/osd/bootloader/inc/pios_config.h index 1d121c321..cf7b48ee1 100644 --- a/flight/targets/boards/osd/bootloader/inc/pios_config.h +++ b/flight/targets/boards/osd/bootloader/inc/pios_config.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * - Central compile time config for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/osd/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/osd/bootloader/inc/pios_usb_board_data.h index fe9072b8c..d7b10fea5 100644 --- a/flight/targets/boards/osd/bootloader/inc/pios_usb_board_data.h +++ b/flight/targets/boards/osd/bootloader/inc/pios_usb_board_data.h @@ -33,13 +33,13 @@ #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 2 +#define PIOS_USB_BOARD_EP_NUM 2 -#include /* struct usb_* */ +#include /* struct usb_* */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OSD -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OSD, USB_OP_BOARD_MODE_BL) -#define PIOS_USB_BOARD_SN_SUFFIX "+BL" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OSD +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OSD, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" /* * The bootloader uses a simplified report structure @@ -49,4 +49,4 @@ */ #define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/osd/bootloader/main.c b/flight/targets/boards/osd/bootloader/main.c index 82878dfbd..12abfd8f0 100644 --- a/flight/targets/boards/osd/bootloader/main.c +++ b/flight/targets/boards/osd/bootloader/main.c @@ -8,21 +8,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief This is the file with the main function of the Revolution BootLoader * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,7 +32,7 @@ #include #include #include -#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USBHOOK_* */ #include /* Prototype of PIOS_Board_Init() function */ @@ -60,10 +60,10 @@ uint8_t tempcount = 0; /* Extern variables ----------------------------------------------------------*/ DFUStates DeviceState; -int16_t status = 0; -bool JumpToApp = false; -bool GO_dfu = false; -bool USB_connected = false; +int16_t status = 0; +bool JumpToApp = false; +bool GO_dfu = false; +bool USB_connected = false; bool User_DFU_request = false; static uint8_t mReceive_Buffer[63]; /* Private function prototypes -----------------------------------------------*/ @@ -71,143 +71,156 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); uint8_t processRX(); void jump_to_app(); -int main() { - PIOS_SYS_Init(); - PIOS_Board_Init(); - PIOS_IAP_Init(); +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); - USB_connected = PIOS_USB_CheckAvailable(0); + USB_connected = PIOS_USB_CheckAvailable(0); - if (PIOS_IAP_CheckRequest() == true) { - PIOS_DELAY_WaitmS(1000); - User_DFU_request = true; - PIOS_IAP_ClearRequest(); - } + if (PIOS_IAP_CheckRequest() == true) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = true; + PIOS_IAP_ClearRequest(); + } - GO_dfu = (USB_connected == true) || (User_DFU_request == true); + GO_dfu = (USB_connected == true) || (User_DFU_request == true); - if (GO_dfu == true) { - PIOS_Board_Init(); - if (User_DFU_request == true) - DeviceState = DFUidle; - else - DeviceState = BLidle; - } else - JumpToApp = true; + if (GO_dfu == true) { + PIOS_Board_Init(); + if (User_DFU_request == true) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = true; + } - uint32_t stopwatch = 0; - uint32_t prev_ticks = PIOS_DELAY_GetuS(); - while (true) { - /* Update the stopwatch */ - uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); - prev_ticks += elapsed_ticks; - stopwatch += elapsed_ticks; + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (true) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; - if (JumpToApp == true) - jump_to_app(); + if (JumpToApp == true) { + jump_to_app(); + } - switch (DeviceState) { - case Last_operation_Success: - case uploadingStarting: - case DFUidle: - period1 = 5000; - sweep_steps1 = 100; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case uploading: - period1 = 5000; - sweep_steps1 = 100; - period2 = 2500; - sweep_steps2 = 50; - break; - case downloading: - period1 = 2500; - sweep_steps1 = 50; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case BLidle: - period1 = 0; - PIOS_LED_On(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - default://error - period1 = 5000; - sweep_steps1 = 100; - period2 = 5000; - sweep_steps2 = 100; - } + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } - if (period1 != 0) { - if (LedPWM(period1, sweep_steps1, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_On(PIOS_LED_HEARTBEAT); + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } - if (period2 != 0) { - if (LedPWM(period2, sweep_steps2, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } - if (stopwatch > 50 * 1000 * 1000) - stopwatch = 0; - if ((stopwatch > 6 * 1000 * 1000) && (DeviceState - == BLidle)) - JumpToApp = true; + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && (DeviceState + == BLidle)) { + JumpToApp = true; + } - processRX(); - DataDownload(start); - } + processRX(); + DataDownload(start); + } } -void jump_to_app() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; - PIOS_LED_On(PIOS_LED_HEARTBEAT); + PIOS_LED_On(PIOS_LED_HEARTBEAT); // Look at cm3_vectors struct in startup. In a fw image the first uint32_t contains the address of the top of irqstack - uint32_t fwIrqStackBase = (*(__IO uint32_t*) bdinfo->fw_base) & 0xFFFE0000; + uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000; // Check for the two possible irqstack locations (sram or core coupled sram) - if ( fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { + if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { /* Jump to user application */ - FLASH_Lock(); - RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - PIOS_USBHOOK_Deactivate(); + PIOS_USBHOOK_Deactivate(); - JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); - Jump_To_Application = (pFunction) JumpAddress; - /* Initialize user application's Stack Pointer */ - __set_MSP(*(__IO uint32_t*) bdinfo->fw_base); - Jump_To_Application(); - } else { - DeviceState = failed_jump; - return; - } + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + return; + } } -uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { - uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ - uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ - uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ - if (curr_sweep & 1) { - pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ - } - return ((count % pwm_period) > pwm_duty) ? 1 : 0; + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; } -uint8_t processRX() { - if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { - processComand(mReceive_Buffer); - } - return true; +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return true; } - diff --git a/flight/targets/boards/osd/bootloader/pios_board.c b/flight/targets/boards/osd/bootloader/pios_board.c index bca178733..7e9858f18 100644 --- a/flight/targets/boards/osd/bootloader/pios_board.c +++ b/flight/targets/boards/osd/bootloader/pios_board.c @@ -7,19 +7,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,41 +39,42 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; -void PIOS_Board_Init() { - if (board_init_complete) { - return; - } +void PIOS_Board_Init() +{ + if (board_init_complete) { + return; + } + + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); - #if defined(PIOS_INCLUDE_LED) - PIOS_LED_Init(&pios_led_cfg); -#endif /* PIOS_INCLUDE_LED */ + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Activate the HID-only USB configuration */ - PIOS_USB_DESC_HID_ONLY_Init(); + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { - PIOS_Assert(0); - } -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ - PIOS_USBHOOK_Activate(); + PIOS_USBHOOK_Activate(); -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ - board_init_complete = true; + board_init_complete = true; } diff --git a/flight/targets/boards/osd/firmware/cm3_fault_handlers.c b/flight/targets/boards/osd/firmware/cm3_fault_handlers.c index e59e3b79a..e74ffa76a 100644 --- a/flight/targets/boards/osd/firmware/cm3_fault_handlers.c +++ b/flight/targets/boards/osd/firmware/cm3_fault_handlers.c @@ -14,34 +14,34 @@ # include #endif -#define FAULT_TRAMPOLINE(_vec) \ -__attribute__((naked, no_instrument_function)) \ -void \ -_vec##_Handler(void) \ -{ \ - __asm( ".syntax unified\n" \ - "MOVS R0, #4 \n" \ - "MOV R1, LR \n" \ - "TST R0, R1 \n" \ - "BEQ 1f \n" \ - "MRS R0, PSP \n" \ - "B " #_vec "_Handler2 \n" \ - "1: \n" \ - "MRS R0, MSP \n" \ - "B " #_vec "_Handler2 \n" \ - ".syntax divided\n"); \ -} \ -struct hack +#define FAULT_TRAMPOLINE(_vec) \ + __attribute__((naked, no_instrument_function)) \ + void \ + _vec##_Handler(void) \ + { \ + __asm(".syntax unified\n" \ + "MOVS R0, #4 \n" \ + "MOV R1, LR \n" \ + "TST R0, R1 \n" \ + "BEQ 1f \n" \ + "MRS R0, PSP \n" \ + "B " #_vec "_Handler2 \n" \ + "1: \n" \ + "MRS R0, MSP \n" \ + "B " #_vec "_Handler2 \n" \ + ".syntax divided\n"); \ + } \ + struct hack struct cm3_frame { - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t pc; - uint32_t psr; + uint32_t r0; + uint32_t r1; + uint32_t r2; + uint32_t r3; + uint32_t r12; + uint32_t lr; + uint32_t pc; + uint32_t psr; }; FAULT_TRAMPOLINE(HardFault); @@ -49,39 +49,42 @@ FAULT_TRAMPOLINE(BusFault); FAULT_TRAMPOLINE(UsageFault); /* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */ -#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) +#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) -void -HardFault_Handler2(struct cm3_frame *frame) +void HardFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nHARD FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(HFSR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nHARD FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(HFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } } -void -BusFault_Handler2(struct cm3_frame *frame) +void BusFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nBUS FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(BFAR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nBUS FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(BFAR)); + dbg_write_char('\n'); + for (;;) { + ; + } } -void -UsageFault_Handler2(struct cm3_frame *frame) +void UsageFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nUSAGE FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nUSAGE FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } } diff --git a/flight/targets/boards/osd/firmware/dcc_stdio.c b/flight/targets/boards/osd/firmware/dcc_stdio.c index 102dd70a1..1a522e9a0 100644 --- a/flight/targets/boards/osd/firmware/dcc_stdio.c +++ b/flight/targets/boards/osd/firmware/dcc_stdio.c @@ -1,33 +1,33 @@ /*************************************************************************** - * Copyright (C) 2008 by Dominic Rath * - * Dominic.Rath@gmx.de * - * Copyright (C) 2008 by Spencer Oliver * - * spen@spen-soft.co.uk * - * Copyright (C) 2008 by Frederik Kriewtz * - * frederik@kriewitz.eu * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* Copyright (C) 2008 by Frederik Kriewtz * +* frederik@kriewitz.eu * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ #include "inc/dcc_stdio.h" -#define TARGET_REQ_TRACEMSG 0x00 -#define TARGET_REQ_DEBUGMSG_ASCII 0x01 -#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) -#define TARGET_REQ_DEBUGCHAR 0x02 +#define TARGET_REQ_TRACEMSG 0x00 +#define TARGET_REQ_DEBUGMSG_ASCII 0x01 +#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) +#define TARGET_REQ_DEBUGCHAR 0x02 /* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel * DCRDR[7:0] is used by target for status @@ -35,115 +35,115 @@ * DCRDR[23:16] is used for by host for status * DCRDR[31:24] is used for by host for write buffer */ -#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) +#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) -#define BUSY 1 +#define BUSY 1 void dbg_write(unsigned long dcc_data) { - int len = 4; + int len = 4; - while (len--) - { - /* wait for data ready */ - while (NVIC_DBG_DATA_R & BUSY); + while (len--) { + /* wait for data ready */ + while (NVIC_DBG_DATA_R & BUSY) { + ; + } - /* write our data and set write flag - tell host there is data*/ - NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); - dcc_data >>= 8; - } + /* write our data and set write flag - tell host there is data*/ + NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); + dcc_data >>= 8; + } } void dbg_trace_point(unsigned long number) { - dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); + dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); } void dbg_write_u32(const unsigned long *val, long len) { - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); - while (len > 0) - { - dbg_write(*val); + while (len > 0) { + dbg_write(*val); - val++; - len--; - } + val++; + len--; + } } void dbg_write_u16(const unsigned short *val, long len) { - unsigned long dcc_data; + unsigned long dcc_data; - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = val[0] - | ((len > 1) ? val[1] << 16: 0x0000); + while (len > 0) { + dcc_data = val[0] + | ((len > 1) ? val[1] << 16 : 0x0000); - dbg_write(dcc_data); + dbg_write(dcc_data); - val += 2; - len -= 2; - } + val += 2; + len -= 2; + } } void dbg_write_u8(const unsigned char *val, long len) { - unsigned long dcc_data; + unsigned long dcc_data; - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = val[0] - | ((len > 1) ? val[1] << 8 : 0x00) - | ((len > 2) ? val[2] << 16 : 0x00) - | ((len > 3) ? val[3] << 24 : 0x00); + while (len > 0) { + dcc_data = val[0] + | ((len > 1) ? val[1] << 8 : 0x00) + | ((len > 2) ? val[2] << 16 : 0x00) + | ((len > 3) ? val[3] << 24 : 0x00); - dbg_write(dcc_data); + dbg_write(dcc_data); - val += 4; - len -= 4; - } + val += 4; + len -= 4; + } } void dbg_write_str(const char *msg) { - long len; - unsigned long dcc_data; + long len; + unsigned long dcc_data; - for (len = 0; msg[len] && (len < 65536); len++); + for (len = 0; msg[len] && (len < 65536); len++) { + ; + } - dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = msg[0] - | ((len > 1) ? msg[1] << 8 : 0x00) - | ((len > 2) ? msg[2] << 16 : 0x00) - | ((len > 3) ? msg[3] << 24 : 0x00); - dbg_write(dcc_data); + while (len > 0) { + dcc_data = msg[0] + | ((len > 1) ? msg[1] << 8 : 0x00) + | ((len > 2) ? msg[2] << 16 : 0x00) + | ((len > 3) ? msg[3] << 24 : 0x00); + dbg_write(dcc_data); - msg += 4; - len -= 4; - } + msg += 4; + len -= 4; + } } void dbg_write_char(char msg) { - dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); + dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); } void dbg_write_hex32(const unsigned long val) { - static const char hextab[] = { - '0', '1', '2', '3', '4', '5', '6', '7', - '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' - }; + static const char hextab[] = { + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' + }; - for (int shift = 28; shift >= 0; shift -= 4) - dbg_write_char(hextab[(val >> shift) & 0xf]); + for (int shift = 28; shift >= 0; shift -= 4) { + dbg_write_char(hextab[(val >> shift) & 0xf]); + } } diff --git a/flight/targets/boards/osd/firmware/font_outlined8x14.c b/flight/targets/boards/osd/firmware/font_outlined8x14.c index 55b9a66dd..6ace4c164 100644 --- a/flight/targets/boards/osd/firmware/font_outlined8x14.c +++ b/flight/targets/boards/osd/firmware/font_outlined8x14.c @@ -6,263 +6,263 @@ * Commons licenses (version 3.0 BY-SA) and *not* the GPL. */ -// This data is generated by a Python script from +// This data is generated by a Python script from // the original font .pngs. // 0xff = indicates character not present // any other byte contains the index of the character. const char font_lookup_outlined8x14[256] = { - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0x00, 0x01, 0x02, 0x03, 0xff, 0x04, 0x05, 0x06, - 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, - 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, - 0x17, 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, - 0x1f, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, - 0x27, 0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, - 0x2f, 0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, - 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, - 0x3f, 0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, - 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, - 0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, - 0x57, 0x58, 0x59, 0x5a, 0x5b, 0x5c, 0x5d, 0xff, - 0x5e, 0x5f, 0x60, 0x61, 0x62, 0x63, 0x64, 0x65, - 0x66, 0x67, 0x68, 0x69, 0x6a, 0x6b, 0x6c, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0x00, 0x01, 0x02, 0x03, 0xff, 0x04, 0x05, 0x06, + 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, + 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, + 0x17, 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, + 0x1f, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, + 0x27, 0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, + 0x2f, 0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, + 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, + 0x3f, 0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, + 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, + 0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, + 0x57, 0x58, 0x59, 0x5a, 0x5b, 0x5c, 0x5d, 0xff, + 0x5e, 0x5f, 0x60, 0x61, 0x62, 0x63, 0x64, 0x65, + 0x66, 0x67, 0x68, 0x69, 0x6a, 0x6b, 0x6c, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }; const char font_data_outlined8x14[] = { - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x20 - 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, - 0x00, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, // 0x21 - 0x66, 0x66, 0x66, 0x66, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x24, 0x24, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x22 - 0x7e, 0xff, 0xff, 0xff, 0x7e, 0xff, 0xff, 0xff, 0x7e, 0xff, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x24, 0x7e, 0x24, 0x24, 0x24, 0x7e, 0x24, 0x24, 0x24, 0x7e, 0x24, 0x00, 0x00, // 0x23 - 0x00, 0x70, 0x70, 0x72, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x4e, 0x0e, 0x0e, 0x00, - 0x00, 0x00, 0x20, 0x00, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x00, 0x04, 0x00, 0x00, // 0x25 - 0x38, 0x3c, 0x7c, 0x7c, 0x7c, 0xfc, 0xee, 0xef, 0xef, 0xff, 0xff, 0x7e, 0x00, 0x00, - 0x00, 0x18, 0x28, 0x10, 0x28, 0x28, 0x44, 0x44, 0x46, 0x44, 0x3a, 0x00, 0x00, 0x00, // 0x26 - 0x38, 0x38, 0x38, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x27 - 0x1e, 0x3e, 0x7e, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x7e, 0x3e, 0x1e, 0x00, - 0x00, 0x1c, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x1c, 0x00, 0x00, // 0x28 - 0x78, 0x7c, 0x7e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x7e, 0x7c, 0x78, 0x00, - 0x00, 0x38, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x38, 0x00, 0x00, // 0x29 - 0x38, 0x7c, 0xfe, 0xfe, 0xfe, 0xee, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x10, 0x7c, 0x28, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2a - 0x00, 0x00, 0x00, 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x10, 0x10, 0x7c, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2b - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x38, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x10, 0x00, 0x00, // 0x2c - 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2d - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, // 0x2e - 0x00, 0x07, 0x07, 0x0f, 0x0e, 0x1c, 0x1c, 0x38, 0x38, 0x70, 0xf0, 0xe0, 0xe0, 0x00, - 0x00, 0x00, 0x02, 0x04, 0x04, 0x08, 0x08, 0x10, 0x10, 0x20, 0x20, 0x40, 0x00, 0x00, // 0x2f - 0x7e, 0xff, 0xff, 0xe7, 0xef, 0xff, 0xff, 0xf7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x46, 0x4a, 0x52, 0x62, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x30 - 0x3c, 0x7c, 0xfc, 0xfc, 0x5c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x38, 0x68, 0x48, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x7e, 0x00, 0x00, // 0x31 - 0x3e, 0x7f, 0xff, 0xe7, 0x47, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x3c, 0x62, 0x42, 0x02, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x7e, 0x00, 0x00, // 0x32 - 0xfe, 0xff, 0xff, 0x07, 0x07, 0x7f, 0x7e, 0x7f, 0x07, 0x07, 0xff, 0xff, 0xfe, 0x00, - 0x00, 0x7c, 0x02, 0x02, 0x02, 0x04, 0x38, 0x04, 0x02, 0x02, 0x02, 0x7c, 0x00, 0x00, // 0x33 - 0x04, 0x0e, 0x1e, 0x3e, 0x7e, 0xff, 0xff, 0xff, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x00, - 0x00, 0x04, 0x0c, 0x14, 0x24, 0x44, 0x7e, 0x04, 0x04, 0x04, 0x04, 0x04, 0x00, 0x00, // 0x34 - 0xff, 0xff, 0xff, 0xe0, 0xfc, 0xfe, 0xff, 0x0f, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, - 0x00, 0x7e, 0x40, 0x40, 0x40, 0x78, 0x04, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x35 - 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x3c, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x36 - 0xff, 0xff, 0xff, 0x07, 0x07, 0x0e, 0x0e, 0x1c, 0x1c, 0x38, 0x38, 0x70, 0x70, 0x00, - 0x00, 0x7e, 0x02, 0x02, 0x02, 0x04, 0x04, 0x08, 0x08, 0x10, 0x10, 0x20, 0x00, 0x00, // 0x37 - 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0x7e, 0x3c, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x38 - 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x7f, 0x7f, 0x7f, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x02, 0x3c, 0x00, 0x00, // 0x39 - 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, // 0x3a - 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x38, 0x00, - 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x10, 0x00, 0x00, // 0x3b - 0x00, 0x00, 0x04, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x70, 0x38, 0x1c, 0x0e, 0x04, 0x00, - 0x00, 0x00, 0x00, 0x04, 0x08, 0x10, 0x20, 0x40, 0x20, 0x10, 0x08, 0x04, 0x00, 0x00, // 0x3c - 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x00, 0x7e, 0x7e, 0x7e, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x3d - 0x00, 0x00, 0x20, 0x70, 0x38, 0x1c, 0x0e, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0x20, 0x00, - 0x00, 0x00, 0x00, 0x20, 0x10, 0x08, 0x04, 0x02, 0x04, 0x08, 0x10, 0x20, 0x00, 0x00, // 0x3e - 0x3c, 0xfe, 0xff, 0xe7, 0xe7, 0x0e, 0x1c, 0x38, 0x38, 0x00, 0x38, 0x38, 0x38, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x02, 0x04, 0x08, 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, // 0x3f - 0x18, 0x3c, 0x7e, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x7c, 0x3e, 0x1e, 0x00, - 0x00, 0x18, 0x24, 0x42, 0x42, 0x42, 0x4a, 0x52, 0x52, 0x4c, 0x20, 0x1e, 0x00, 0x00, // 0x40 - 0x7e, 0xff, 0xff, 0xff, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x3c, 0x66, 0x42, 0x42, 0x42, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x41 - 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0x00, - 0x00, 0x7c, 0x46, 0x42, 0x42, 0x42, 0x7c, 0x42, 0x42, 0x42, 0x46, 0x7c, 0x00, 0x00, // 0x42 - 0x7f, 0xff, 0xff, 0xf7, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xf7, 0xff, 0xff, 0x7f, 0x00, - 0x00, 0x3e, 0x62, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x62, 0x3e, 0x00, 0x00, // 0x43 - 0xfe, 0xff, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xef, 0xff, 0xff, 0xfe, 0x00, - 0x00, 0x7c, 0x46, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x46, 0x7c, 0x00, 0x00, // 0x44 - 0xff, 0xff, 0xff, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x45 - 0xff, 0xff, 0xff, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, - 0x00, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x46 - 0x7f, 0xff, 0xff, 0xf0, 0xe0, 0xef, 0xef, 0xef, 0xe7, 0xf7, 0xff, 0xff, 0x7f, 0x00, - 0x00, 0x3e, 0x60, 0x40, 0x40, 0x40, 0x46, 0x42, 0x42, 0x42, 0x62, 0x3e, 0x00, 0x00, // 0x47 - 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x48 - 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x7c, 0x00, 0x00, // 0x49 - 0xff, 0xff, 0xff, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x3c, 0xfc, 0xfc, 0xf8, 0x00, - 0x00, 0x7e, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x18, 0x70, 0x00, 0x00, // 0x4a - 0xe7, 0xef, 0xff, 0xfe, 0xfc, 0xf8, 0xf0, 0xf8, 0xfc, 0xfe, 0xff, 0xef, 0xe7, 0x00, - 0x00, 0x42, 0x44, 0x48, 0x50, 0x60, 0x40, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x4b - 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x4c - 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x42, 0x66, 0x5a, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x4d - 0xe7, 0xf7, 0xff, 0xff, 0xff, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x42, 0x62, 0x52, 0x4a, 0x46, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x4e - 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x4f - 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, - 0x00, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x50 - 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x02, 0x02, 0x00, 0x00, // 0x51 - 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xf8, 0xfc, 0xee, 0xe7, 0xe3, 0x00, - 0x00, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x52 - 0xff, 0xff, 0xff, 0xe0, 0xf0, 0xfe, 0xff, 0x7f, 0x0f, 0x07, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x7e, 0x40, 0x40, 0x40, 0x60, 0x3c, 0x06, 0x02, 0x02, 0x02, 0x7e, 0x00, 0x00, // 0x53 - 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, - 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x54 - 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0x7e, 0x3c, 0x00, - 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x55 - 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xfe, 0x7e, 0x7e, 0x7e, 0x3c, 0x3c, 0x00, - 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x24, 0x24, 0x24, 0x18, 0x18, 0x00, 0x00, // 0x56 - 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0x00, - 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x5a, 0x66, 0x42, 0x00, 0x00, // 0x57 - 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0x7f, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x42, 0x42, 0x42, 0x66, 0x3c, 0x18, 0x3c, 0x66, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x58 - 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, - 0x00, 0x44, 0x44, 0x28, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x59 - 0xff, 0xff, 0xff, 0x07, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x7e, 0x02, 0x02, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x5a - 0x7e, 0x7e, 0x7e, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x3c, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x3c, 0x00, 0x00, // 0x5b - 0x00, 0xe0, 0xe0, 0xf0, 0x70, 0x38, 0x38, 0x1c, 0x1c, 0x0e, 0x0f, 0x07, 0x07, 0x00, - 0x00, 0x00, 0x40, 0x20, 0x20, 0x10, 0x10, 0x08, 0x08, 0x04, 0x04, 0x02, 0x00, 0x00, // 0x5c - 0x7e, 0x7e, 0x7e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x3c, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x3c, 0x00, 0x00, // 0x5d - 0x3c, 0x7e, 0x7e, 0x7e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x18, 0x24, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x5e - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x00, 0x00, // 0x5f - 0x30, 0x38, 0x1c, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x10, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x60 - 0x00, 0x00, 0x00, 0x00, 0x7c, 0xfc, 0xfe, 0xee, 0xfe, 0xfe, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x38, 0x44, 0x04, 0x04, 0x3c, 0x44, 0x7a, 0x00, 0x00, // 0x61 - 0x00, 0xe0, 0xe0, 0xe0, 0xe0, 0xfe, 0xff, 0xff, 0xf7, 0xe7, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x00, 0x40, 0x40, 0x40, 0x40, 0x5c, 0x62, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x62 - 0x00, 0x00, 0x00, 0x00, 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xf0, 0xff, 0x7f, 0x3f, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x20, 0x1e, 0x00, 0x00, // 0x63 - 0x00, 0x07, 0x07, 0x07, 0x07, 0x7f, 0xff, 0xff, 0xef, 0xe7, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x00, 0x02, 0x02, 0x02, 0x02, 0x3a, 0x46, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x64 - 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xff, 0xff, 0xff, 0xff, 0x7f, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x5c, 0x40, 0x3e, 0x00, 0x00, // 0x65 - 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, - 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x66 - 0x7c, 0xfe, 0xff, 0xef, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, - 0x00, 0x38, 0x44, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x67 - 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xfc, 0xfe, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x40, 0x40, 0x40, 0x40, 0x40, 0x78, 0x44, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x68 - 0x1c, 0x1c, 0x1c, 0x00, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x00, - 0x00, 0x08, 0x00, 0x00, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x00, 0x00, // 0x69 - 0x1c, 0x1c, 0x1c, 0x00, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x3c, 0xfc, 0xfc, 0xf0, 0x00, - 0x00, 0x08, 0x00, 0x00, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x10, 0x60, 0x00, 0x00, // 0x6a - 0x00, 0xe2, 0xe7, 0xee, 0xfc, 0xf8, 0xf0, 0xf0, 0xf8, 0xfc, 0xee, 0xe7, 0xe2, 0x00, - 0x00, 0x00, 0x42, 0x44, 0x48, 0x50, 0x60, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x6b - 0xf0, 0xf0, 0xf0, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x78, 0x7f, 0x3f, 0x1f, 0x00, - 0x00, 0x60, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x10, 0x0e, 0x00, 0x00, // 0x6c - 0x00, 0x00, 0x00, 0x00, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x66, 0x5a, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x6d - 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x6e - 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x6f - 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x70 - 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x03, 0x02, 0x00, 0x00, // 0x71 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x72 - 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xfe, 0xff, 0x7f, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x40, 0x40, 0x3c, 0x02, 0x02, 0x7e, 0x00, 0x00, // 0x73 - 0x00, 0xe0, 0xe0, 0xe0, 0xe0, 0xfc, 0xfc, 0xfc, 0xe0, 0xf0, 0xfe, 0xfe, 0x7e, 0x00, - 0x00, 0x00, 0x40, 0x40, 0x40, 0x40, 0x78, 0x40, 0x40, 0x40, 0x60, 0x3c, 0x00, 0x00, // 0x74 - 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x75 - 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x3c, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x24, 0x18, 0x00, 0x00, // 0x76 - 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x5a, 0x66, 0x42, 0x00, 0x00, // 0x77 - 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0xff, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x24, 0x18, 0x24, 0x42, 0x42, 0x00, 0x00, // 0x78 - 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0x07, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, - 0x00, 0x42, 0x42, 0x42, 0x42, 0x7c, 0x02, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x79 - 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x0e, 0x1c, 0x38, 0x70, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x7e, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x7e, 0x00, 0x00, // 0x7a - 0x00, 0x3f, 0x7f, 0x7f, 0x70, 0xf0, 0xf0, 0xf0, 0xf0, 0x70, 0x7f, 0x7f, 0x3f, 0x00, - 0x00, 0x00, 0x1e, 0x20, 0x20, 0x20, 0x40, 0x40, 0x20, 0x20, 0x20, 0x1e, 0x00, 0x00, // 0x7b - 0x00, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, - 0x00, 0x00, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x7c - 0x00, 0xfc, 0xfe, 0xfe, 0x0e, 0x0f, 0x0f, 0x0f, 0x0f, 0x0e, 0xfe, 0xfe, 0xfc, 0x00, - 0x00, 0x00, 0x78, 0x04, 0x04, 0x04, 0x02, 0x02, 0x04, 0x04, 0x04, 0x78, 0x00, 0x00, // 0x7d - 0x00, 0x10, 0x3a, 0x7f, 0x2e, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x10, 0x2a, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x7e - 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x80 - 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x81 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x82 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x83 - 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x84 - 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x85 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x86 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x87 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x00, 0x00, // 0x88 - 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x7e, 0x7e, 0x3c, 0x00, - 0x00, 0x00, 0x18, 0x08, 0x18, 0x08, 0x18, 0x08, 0x18, 0x08, 0x3c, 0x3c, 0x00, 0x00, // 0x89 - 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x6e, 0x68, 0x08, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8a - 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x1f, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x69, 0x6a, 0x0c, 0x0c, 0x0a, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8b - 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x1c, 0x1c, 0x1c, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x6e, 0x68, 0x08, 0x0e, 0x08, 0x08, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8c - 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x7c, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x38, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x8d - 0x00, 0x00, 0x00, 0x3e, 0x7f, 0x7f, 0x7f, 0x7f, 0x7f, 0x3e, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x36, 0x36, 0x32, 0x3e, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00 // 0x8e + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x20 + 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, + 0x00, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, // 0x21 + 0x66, 0x66, 0x66, 0x66, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x24, 0x24, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x22 + 0x7e, 0xff, 0xff, 0xff, 0x7e, 0xff, 0xff, 0xff, 0x7e, 0xff, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x24, 0x7e, 0x24, 0x24, 0x24, 0x7e, 0x24, 0x24, 0x24, 0x7e, 0x24, 0x00, 0x00, // 0x23 + 0x00, 0x70, 0x70, 0x72, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x4e, 0x0e, 0x0e, 0x00, + 0x00, 0x00, 0x20, 0x00, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x00, 0x04, 0x00, 0x00, // 0x25 + 0x38, 0x3c, 0x7c, 0x7c, 0x7c, 0xfc, 0xee, 0xef, 0xef, 0xff, 0xff, 0x7e, 0x00, 0x00, + 0x00, 0x18, 0x28, 0x10, 0x28, 0x28, 0x44, 0x44, 0x46, 0x44, 0x3a, 0x00, 0x00, 0x00, // 0x26 + 0x38, 0x38, 0x38, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x27 + 0x1e, 0x3e, 0x7e, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x7e, 0x3e, 0x1e, 0x00, + 0x00, 0x1c, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x1c, 0x00, 0x00, // 0x28 + 0x78, 0x7c, 0x7e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x7e, 0x7c, 0x78, 0x00, + 0x00, 0x38, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x38, 0x00, 0x00, // 0x29 + 0x38, 0x7c, 0xfe, 0xfe, 0xfe, 0xee, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x10, 0x7c, 0x28, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2a + 0x00, 0x00, 0x00, 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x10, 0x10, 0x7c, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2b + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x38, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x10, 0x00, 0x00, // 0x2c + 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2d + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, // 0x2e + 0x00, 0x07, 0x07, 0x0f, 0x0e, 0x1c, 0x1c, 0x38, 0x38, 0x70, 0xf0, 0xe0, 0xe0, 0x00, + 0x00, 0x00, 0x02, 0x04, 0x04, 0x08, 0x08, 0x10, 0x10, 0x20, 0x20, 0x40, 0x00, 0x00, // 0x2f + 0x7e, 0xff, 0xff, 0xe7, 0xef, 0xff, 0xff, 0xf7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x46, 0x4a, 0x52, 0x62, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x30 + 0x3c, 0x7c, 0xfc, 0xfc, 0x5c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x38, 0x68, 0x48, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x7e, 0x00, 0x00, // 0x31 + 0x3e, 0x7f, 0xff, 0xe7, 0x47, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x3c, 0x62, 0x42, 0x02, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x7e, 0x00, 0x00, // 0x32 + 0xfe, 0xff, 0xff, 0x07, 0x07, 0x7f, 0x7e, 0x7f, 0x07, 0x07, 0xff, 0xff, 0xfe, 0x00, + 0x00, 0x7c, 0x02, 0x02, 0x02, 0x04, 0x38, 0x04, 0x02, 0x02, 0x02, 0x7c, 0x00, 0x00, // 0x33 + 0x04, 0x0e, 0x1e, 0x3e, 0x7e, 0xff, 0xff, 0xff, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x00, + 0x00, 0x04, 0x0c, 0x14, 0x24, 0x44, 0x7e, 0x04, 0x04, 0x04, 0x04, 0x04, 0x00, 0x00, // 0x34 + 0xff, 0xff, 0xff, 0xe0, 0xfc, 0xfe, 0xff, 0x0f, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, + 0x00, 0x7e, 0x40, 0x40, 0x40, 0x78, 0x04, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x35 + 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x3c, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x36 + 0xff, 0xff, 0xff, 0x07, 0x07, 0x0e, 0x0e, 0x1c, 0x1c, 0x38, 0x38, 0x70, 0x70, 0x00, + 0x00, 0x7e, 0x02, 0x02, 0x02, 0x04, 0x04, 0x08, 0x08, 0x10, 0x10, 0x20, 0x00, 0x00, // 0x37 + 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0x7e, 0x3c, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x38 + 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x7f, 0x7f, 0x7f, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x02, 0x3c, 0x00, 0x00, // 0x39 + 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, // 0x3a + 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x38, 0x00, + 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x10, 0x00, 0x00, // 0x3b + 0x00, 0x00, 0x04, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x70, 0x38, 0x1c, 0x0e, 0x04, 0x00, + 0x00, 0x00, 0x00, 0x04, 0x08, 0x10, 0x20, 0x40, 0x20, 0x10, 0x08, 0x04, 0x00, 0x00, // 0x3c + 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x00, 0x7e, 0x7e, 0x7e, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x3d + 0x00, 0x00, 0x20, 0x70, 0x38, 0x1c, 0x0e, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0x20, 0x00, + 0x00, 0x00, 0x00, 0x20, 0x10, 0x08, 0x04, 0x02, 0x04, 0x08, 0x10, 0x20, 0x00, 0x00, // 0x3e + 0x3c, 0xfe, 0xff, 0xe7, 0xe7, 0x0e, 0x1c, 0x38, 0x38, 0x00, 0x38, 0x38, 0x38, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x02, 0x04, 0x08, 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, // 0x3f + 0x18, 0x3c, 0x7e, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x7c, 0x3e, 0x1e, 0x00, + 0x00, 0x18, 0x24, 0x42, 0x42, 0x42, 0x4a, 0x52, 0x52, 0x4c, 0x20, 0x1e, 0x00, 0x00, // 0x40 + 0x7e, 0xff, 0xff, 0xff, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x3c, 0x66, 0x42, 0x42, 0x42, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x41 + 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0x00, + 0x00, 0x7c, 0x46, 0x42, 0x42, 0x42, 0x7c, 0x42, 0x42, 0x42, 0x46, 0x7c, 0x00, 0x00, // 0x42 + 0x7f, 0xff, 0xff, 0xf7, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xf7, 0xff, 0xff, 0x7f, 0x00, + 0x00, 0x3e, 0x62, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x62, 0x3e, 0x00, 0x00, // 0x43 + 0xfe, 0xff, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xef, 0xff, 0xff, 0xfe, 0x00, + 0x00, 0x7c, 0x46, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x46, 0x7c, 0x00, 0x00, // 0x44 + 0xff, 0xff, 0xff, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x45 + 0xff, 0xff, 0xff, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, + 0x00, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x46 + 0x7f, 0xff, 0xff, 0xf0, 0xe0, 0xef, 0xef, 0xef, 0xe7, 0xf7, 0xff, 0xff, 0x7f, 0x00, + 0x00, 0x3e, 0x60, 0x40, 0x40, 0x40, 0x46, 0x42, 0x42, 0x42, 0x62, 0x3e, 0x00, 0x00, // 0x47 + 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x48 + 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x7c, 0x00, 0x00, // 0x49 + 0xff, 0xff, 0xff, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x3c, 0xfc, 0xfc, 0xf8, 0x00, + 0x00, 0x7e, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x18, 0x70, 0x00, 0x00, // 0x4a + 0xe7, 0xef, 0xff, 0xfe, 0xfc, 0xf8, 0xf0, 0xf8, 0xfc, 0xfe, 0xff, 0xef, 0xe7, 0x00, + 0x00, 0x42, 0x44, 0x48, 0x50, 0x60, 0x40, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x4b + 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x4c + 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x42, 0x66, 0x5a, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x4d + 0xe7, 0xf7, 0xff, 0xff, 0xff, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x42, 0x62, 0x52, 0x4a, 0x46, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x4e + 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x4f + 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, + 0x00, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x50 + 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x02, 0x02, 0x00, 0x00, // 0x51 + 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xf8, 0xfc, 0xee, 0xe7, 0xe3, 0x00, + 0x00, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x52 + 0xff, 0xff, 0xff, 0xe0, 0xf0, 0xfe, 0xff, 0x7f, 0x0f, 0x07, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x7e, 0x40, 0x40, 0x40, 0x60, 0x3c, 0x06, 0x02, 0x02, 0x02, 0x7e, 0x00, 0x00, // 0x53 + 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, + 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x54 + 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0x7e, 0x3c, 0x00, + 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x55 + 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xfe, 0x7e, 0x7e, 0x7e, 0x3c, 0x3c, 0x00, + 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x24, 0x24, 0x24, 0x18, 0x18, 0x00, 0x00, // 0x56 + 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0x00, + 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x5a, 0x66, 0x42, 0x00, 0x00, // 0x57 + 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0x7f, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x42, 0x42, 0x42, 0x66, 0x3c, 0x18, 0x3c, 0x66, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x58 + 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, + 0x00, 0x44, 0x44, 0x28, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x59 + 0xff, 0xff, 0xff, 0x07, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x7e, 0x02, 0x02, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x5a + 0x7e, 0x7e, 0x7e, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x3c, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x3c, 0x00, 0x00, // 0x5b + 0x00, 0xe0, 0xe0, 0xf0, 0x70, 0x38, 0x38, 0x1c, 0x1c, 0x0e, 0x0f, 0x07, 0x07, 0x00, + 0x00, 0x00, 0x40, 0x20, 0x20, 0x10, 0x10, 0x08, 0x08, 0x04, 0x04, 0x02, 0x00, 0x00, // 0x5c + 0x7e, 0x7e, 0x7e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x3c, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x3c, 0x00, 0x00, // 0x5d + 0x3c, 0x7e, 0x7e, 0x7e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x18, 0x24, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x5e + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x00, 0x00, // 0x5f + 0x30, 0x38, 0x1c, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x10, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x60 + 0x00, 0x00, 0x00, 0x00, 0x7c, 0xfc, 0xfe, 0xee, 0xfe, 0xfe, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x38, 0x44, 0x04, 0x04, 0x3c, 0x44, 0x7a, 0x00, 0x00, // 0x61 + 0x00, 0xe0, 0xe0, 0xe0, 0xe0, 0xfe, 0xff, 0xff, 0xf7, 0xe7, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x00, 0x40, 0x40, 0x40, 0x40, 0x5c, 0x62, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x62 + 0x00, 0x00, 0x00, 0x00, 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xf0, 0xff, 0x7f, 0x3f, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x20, 0x1e, 0x00, 0x00, // 0x63 + 0x00, 0x07, 0x07, 0x07, 0x07, 0x7f, 0xff, 0xff, 0xef, 0xe7, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x00, 0x02, 0x02, 0x02, 0x02, 0x3a, 0x46, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x64 + 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xff, 0xff, 0xff, 0xff, 0x7f, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x5c, 0x40, 0x3e, 0x00, 0x00, // 0x65 + 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, + 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x66 + 0x7c, 0xfe, 0xff, 0xef, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, + 0x00, 0x38, 0x44, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x67 + 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xfc, 0xfe, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x40, 0x40, 0x40, 0x40, 0x40, 0x78, 0x44, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x68 + 0x1c, 0x1c, 0x1c, 0x00, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x00, + 0x00, 0x08, 0x00, 0x00, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x00, 0x00, // 0x69 + 0x1c, 0x1c, 0x1c, 0x00, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x3c, 0xfc, 0xfc, 0xf0, 0x00, + 0x00, 0x08, 0x00, 0x00, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x10, 0x60, 0x00, 0x00, // 0x6a + 0x00, 0xe2, 0xe7, 0xee, 0xfc, 0xf8, 0xf0, 0xf0, 0xf8, 0xfc, 0xee, 0xe7, 0xe2, 0x00, + 0x00, 0x00, 0x42, 0x44, 0x48, 0x50, 0x60, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x6b + 0xf0, 0xf0, 0xf0, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x78, 0x7f, 0x3f, 0x1f, 0x00, + 0x00, 0x60, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x10, 0x0e, 0x00, 0x00, // 0x6c + 0x00, 0x00, 0x00, 0x00, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x66, 0x5a, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x6d + 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x6e + 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x6f + 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x70 + 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x03, 0x02, 0x00, 0x00, // 0x71 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x72 + 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xfe, 0xff, 0x7f, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x40, 0x40, 0x3c, 0x02, 0x02, 0x7e, 0x00, 0x00, // 0x73 + 0x00, 0xe0, 0xe0, 0xe0, 0xe0, 0xfc, 0xfc, 0xfc, 0xe0, 0xf0, 0xfe, 0xfe, 0x7e, 0x00, + 0x00, 0x00, 0x40, 0x40, 0x40, 0x40, 0x78, 0x40, 0x40, 0x40, 0x60, 0x3c, 0x00, 0x00, // 0x74 + 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x75 + 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x3c, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x24, 0x18, 0x00, 0x00, // 0x76 + 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x5a, 0x66, 0x42, 0x00, 0x00, // 0x77 + 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0xff, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x24, 0x18, 0x24, 0x42, 0x42, 0x00, 0x00, // 0x78 + 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0x07, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, + 0x00, 0x42, 0x42, 0x42, 0x42, 0x7c, 0x02, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x79 + 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x0e, 0x1c, 0x38, 0x70, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x7e, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x7e, 0x00, 0x00, // 0x7a + 0x00, 0x3f, 0x7f, 0x7f, 0x70, 0xf0, 0xf0, 0xf0, 0xf0, 0x70, 0x7f, 0x7f, 0x3f, 0x00, + 0x00, 0x00, 0x1e, 0x20, 0x20, 0x20, 0x40, 0x40, 0x20, 0x20, 0x20, 0x1e, 0x00, 0x00, // 0x7b + 0x00, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, + 0x00, 0x00, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x7c + 0x00, 0xfc, 0xfe, 0xfe, 0x0e, 0x0f, 0x0f, 0x0f, 0x0f, 0x0e, 0xfe, 0xfe, 0xfc, 0x00, + 0x00, 0x00, 0x78, 0x04, 0x04, 0x04, 0x02, 0x02, 0x04, 0x04, 0x04, 0x78, 0x00, 0x00, // 0x7d + 0x00, 0x10, 0x3a, 0x7f, 0x2e, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x10, 0x2a, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x7e + 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x80 + 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x81 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x82 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x83 + 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x84 + 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x85 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x86 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x87 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x00, 0x00, // 0x88 + 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x7e, 0x7e, 0x3c, 0x00, + 0x00, 0x00, 0x18, 0x08, 0x18, 0x08, 0x18, 0x08, 0x18, 0x08, 0x3c, 0x3c, 0x00, 0x00, // 0x89 + 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x6e, 0x68, 0x08, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8a + 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x1f, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x69, 0x6a, 0x0c, 0x0c, 0x0a, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8b + 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x1c, 0x1c, 0x1c, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x6e, 0x68, 0x08, 0x0e, 0x08, 0x08, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8c + 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x7c, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x38, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x8d + 0x00, 0x00, 0x00, 0x3e, 0x7f, 0x7f, 0x7f, 0x7f, 0x7f, 0x3e, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x36, 0x36, 0x32, 0x3e, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00 // 0x8e }; diff --git a/flight/targets/boards/osd/firmware/font_outlined8x8.c b/flight/targets/boards/osd/firmware/font_outlined8x8.c index 78561fb48..806374f25 100644 --- a/flight/targets/boards/osd/firmware/font_outlined8x8.c +++ b/flight/targets/boards/osd/firmware/font_outlined8x8.c @@ -6,177 +6,177 @@ * Commons licenses (version 3.0 BY-SA) and *not* the GPL. */ -// This data is generated by a Python script from +// This data is generated by a Python script from // the original font .pngs. // 0xff = indicates character not present // any other byte contains the index of the character. const char font_lookup_outlined8x8[256] = { - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0x00, 0x01, 0x02, 0x03, 0xff, 0xff, 0xff, 0x04, - 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, - 0x0d, 0x0e, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, - 0x15, 0x16, 0x17, 0xff, 0x18, 0x19, 0x1a, 0x1b, - 0xff, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, 0x21, 0x22, - 0x23, 0x24, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2a, - 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31, 0x32, - 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0xff, 0x39, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f, 0x40, 0x41, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0x00, 0x01, 0x02, 0x03, 0xff, 0xff, 0xff, 0x04, + 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, + 0x0d, 0x0e, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, + 0x15, 0x16, 0x17, 0xff, 0x18, 0x19, 0x1a, 0x1b, + 0xff, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, 0x21, 0x22, + 0x23, 0x24, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2a, + 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31, 0x32, + 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0xff, 0x39, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f, 0x40, 0x41, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }; const char font_data_outlined8x8[] = { - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x20 - 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, - 0x00, 0x10, 0x10, 0x10, 0x00, 0x10, 0x00, 0x00, // 0x21 - 0x44, 0xee, 0xee, 0x44, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x44, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x22 - 0x28, 0x7c, 0xfe, 0x7c, 0xfe, 0x7c, 0x28, 0x00, - 0x00, 0x28, 0x7c, 0x28, 0x7c, 0x28, 0x00, 0x00, // 0x23 - 0x10, 0x38, 0x38, 0x10, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x27 - 0x10, 0x38, 0x70, 0x70, 0x70, 0x38, 0x10, 0x00, - 0x00, 0x10, 0x20, 0x20, 0x20, 0x10, 0x00, 0x00, // 0x28 - 0x20, 0x70, 0x38, 0x38, 0x38, 0x70, 0x20, 0x00, - 0x00, 0x20, 0x10, 0x10, 0x10, 0x20, 0x00, 0x00, // 0x29 - 0x28, 0x7c, 0x38, 0x7c, 0x28, 0x00, 0x00, 0x00, - 0x00, 0x28, 0x10, 0x28, 0x00, 0x00, 0x00, 0x00, // 0x2a - 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x00, - 0x00, 0x10, 0x10, 0x7c, 0x10, 0x10, 0x00, 0x00, // 0x2b - 0x00, 0x00, 0x78, 0x78, 0x78, 0x78, 0x38, 0x00, - 0x00, 0x00, 0x00, 0x30, 0x30, 0x10, 0x00, 0x00, // 0x2c - 0x00, 0x00, 0xfe, 0xfe, 0xfe, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, 0x00, 0x00, // 0x2d - 0x00, 0x00, 0x00, 0x78, 0x78, 0x78, 0x78, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x00, 0x00, // 0x2e - 0x04, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x40, 0x00, - 0x00, 0x04, 0x08, 0x10, 0x20, 0x40, 0x00, 0x00, // 0x2f - 0x7c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7c, 0x00, - 0x00, 0x38, 0x44, 0x54, 0x44, 0x38, 0x00, 0x00, // 0x30 - 0x3c, 0x7c, 0xfc, 0xfc, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x18, 0x28, 0x48, 0x08, 0x7c, 0x00, 0x00, // 0x31 - 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x78, 0x04, 0x78, 0x40, 0x7c, 0x00, 0x00, // 0x32 - 0xfe, 0xfe, 0xfe, 0x7e, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x04, 0x3c, 0x04, 0x7c, 0x00, 0x00, // 0x33 - 0xee, 0xee, 0xfe, 0xfe, 0x7e, 0x0e, 0x0e, 0x00, - 0x00, 0x44, 0x44, 0x3c, 0x04, 0x04, 0x00, 0x00, // 0x34 - 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, - 0x00, 0x7c, 0x40, 0x78, 0x04, 0x78, 0x00, 0x00, // 0x35 - 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x40, 0x7c, 0x44, 0x7c, 0x00, 0x00, // 0x36 - 0xfe, 0xfe, 0xfe, 0x38, 0x70, 0xe0, 0xc0, 0x00, - 0x00, 0x7c, 0x08, 0x10, 0x20, 0x40, 0x00, 0x00, // 0x37 - 0x7c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7c, 0x00, - 0x00, 0x38, 0x44, 0x7c, 0x44, 0x38, 0x00, 0x00, // 0x38 - 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7e, 0x7e, 0x00, - 0x00, 0x7c, 0x44, 0x7c, 0x04, 0x3c, 0x00, 0x00, // 0x39 - 0x78, 0x78, 0x78, 0x78, 0x78, 0x78, 0x78, 0x00, - 0x00, 0x30, 0x30, 0x00, 0x30, 0x30, 0x00, 0x00, // 0x3a - 0x08, 0x1c, 0x38, 0x70, 0x38, 0x1c, 0x08, 0x00, - 0x00, 0x08, 0x10, 0x20, 0x10, 0x08, 0x00, 0x00, // 0x3c - 0xfe, 0xfe, 0xfe, 0x00, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, // 0x3d - 0x20, 0x70, 0x38, 0x1c, 0x38, 0x70, 0x20, 0x00, - 0x00, 0x20, 0x10, 0x08, 0x10, 0x20, 0x00, 0x00, // 0x3e - 0x7c, 0xfe, 0xfe, 0xfe, 0x3c, 0x38, 0x38, 0x00, - 0x00, 0x38, 0x44, 0x18, 0x00, 0x10, 0x00, 0x00, // 0x3f - 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0xee, 0x00, - 0x00, 0x7c, 0x44, 0x7c, 0x44, 0x44, 0x00, 0x00, // 0x41 - 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, - 0x00, 0x78, 0x44, 0x7c, 0x44, 0x78, 0x00, 0x00, // 0x42 - 0xfe, 0xfe, 0xfe, 0xe0, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x40, 0x40, 0x40, 0x7c, 0x00, 0x00, // 0x43 - 0xfc, 0xfe, 0xfe, 0xee, 0xfe, 0xfe, 0xfc, 0x00, - 0x00, 0x78, 0x44, 0x44, 0x44, 0x78, 0x00, 0x00, // 0x44 - 0xfe, 0xfe, 0xfe, 0xfc, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x40, 0x78, 0x40, 0x7c, 0x00, 0x00, // 0x45 - 0xfe, 0xfe, 0xfe, 0xfc, 0xfc, 0xe0, 0xe0, 0x00, - 0x00, 0x7c, 0x40, 0x78, 0x40, 0x40, 0x00, 0x00, // 0x46 - 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x40, 0x5c, 0x44, 0x7c, 0x00, 0x00, // 0x47 - 0xee, 0xee, 0xfe, 0xfe, 0xfe, 0xee, 0xee, 0x00, - 0x00, 0x44, 0x44, 0x7c, 0x44, 0x44, 0x00, 0x00, // 0x48 - 0xfe, 0xfe, 0xfe, 0x38, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x10, 0x10, 0x10, 0x7c, 0x00, 0x00, // 0x49 - 0xfe, 0xfe, 0xfe, 0x1c, 0xfc, 0xfc, 0xfc, 0x00, - 0x00, 0x7c, 0x08, 0x08, 0x08, 0x78, 0x00, 0x00, // 0x4a - 0xe6, 0xee, 0xfc, 0xf8, 0xfc, 0xee, 0xe6, 0x00, - 0x00, 0x44, 0x48, 0x70, 0x48, 0x44, 0x00, 0x00, // 0x4b - 0xe0, 0xe0, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x00, 0x00, // 0x4c - 0x6c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0x00, - 0x00, 0x28, 0x54, 0x54, 0x54, 0x44, 0x00, 0x00, // 0x4d - 0xee, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0x00, - 0x00, 0x44, 0x64, 0x54, 0x4c, 0x44, 0x00, 0x00, // 0x4e - 0xfe, 0xfe, 0xfe, 0xee, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x44, 0x44, 0x44, 0x7c, 0x00, 0x00, // 0x4f - 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0x00, - 0x00, 0x7c, 0x44, 0x7c, 0x40, 0x40, 0x00, 0x00, // 0x50 - 0xfe, 0xfe, 0xfe, 0xee, 0xfc, 0xfe, 0xf6, 0x00, - 0x00, 0x7c, 0x44, 0x44, 0x48, 0x74, 0x00, 0x00, // 0x51 - 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0xe8, 0x00, - 0x00, 0x7c, 0x44, 0x7c, 0x50, 0x48, 0x00, 0x00, // 0x52 - 0x7e, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, - 0x00, 0x3c, 0x40, 0x7c, 0x04, 0x78, 0x00, 0x00, // 0x53 - 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x00, - 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x54 - 0xee, 0xee, 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x00, - 0x00, 0x44, 0x44, 0x44, 0x44, 0x38, 0x00, 0x00, // 0x55 - 0xee, 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x00, - 0x00, 0x44, 0x44, 0x44, 0x28, 0x10, 0x00, 0x00, // 0x56 - 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x6c, 0x00, - 0x00, 0x54, 0x54, 0x54, 0x54, 0x28, 0x00, 0x00, // 0x57 - 0xee, 0xfe, 0xfe, 0x7c, 0xfe, 0xfe, 0xee, 0x00, - 0x00, 0x44, 0x28, 0x10, 0x28, 0x44, 0x00, 0x00, // 0x58 - 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x38, 0x38, 0x00, - 0x00, 0x44, 0x28, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x59 - 0xfe, 0xfe, 0xfe, 0x78, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x08, 0x10, 0x20, 0x7c, 0x00, 0x00, // 0x5a - 0x38, 0x7c, 0x78, 0x70, 0x78, 0x7c, 0x38, 0x00, - 0x00, 0x38, 0x20, 0x20, 0x20, 0x38, 0x00, 0x00, // 0x5b - 0x40, 0xe0, 0x70, 0x38, 0x1c, 0x0e, 0x04, 0x00, - 0x00, 0x40, 0x20, 0x10, 0x08, 0x04, 0x00, 0x00, // 0x5c - 0x38, 0x7c, 0x3c, 0x1c, 0x3c, 0x7c, 0x38, 0x00, - 0x00, 0x38, 0x08, 0x08, 0x08, 0x38, 0x00, 0x00, // 0x5d - 0x00, 0x00, 0x00, 0x00, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, // 0x5f - 0x10, 0x38, 0x7c, 0xfe, 0xfe, 0x38, 0x38, 0x00, - 0x00, 0x10, 0x38, 0x54, 0x10, 0x10, 0x00, 0x00, // 0x80 - 0x3e, 0x3e, 0x1e, 0x3e, 0x76, 0xe0, 0x40, 0x00, - 0x00, 0x1c, 0x0c, 0x14, 0x20, 0x40, 0x00, 0x00, // 0x81 - 0x10, 0x38, 0xfc, 0xfe, 0xfc, 0x38, 0x10, 0x00, - 0x00, 0x10, 0x08, 0x7c, 0x08, 0x10, 0x00, 0x00, // 0x82 - 0x40, 0xe0, 0x76, 0x3e, 0x1e, 0x3e, 0x3e, 0x00, - 0x00, 0x40, 0x20, 0x14, 0x0c, 0x1c, 0x00, 0x00, // 0x83 - 0x38, 0x38, 0xfe, 0xfe, 0x7c, 0x38, 0x10, 0x00, - 0x00, 0x10, 0x10, 0x54, 0x38, 0x10, 0x00, 0x00, // 0x84 - 0x04, 0x0e, 0xdc, 0xf8, 0xf0, 0xf8, 0xf8, 0x00, - 0x00, 0x04, 0x08, 0x50, 0x60, 0x70, 0x00, 0x00, // 0x85 - 0x10, 0x38, 0x7e, 0xfe, 0x7e, 0x38, 0x10, 0x00, - 0x00, 0x10, 0x20, 0x7c, 0x20, 0x10, 0x00, 0x00, // 0x86 - 0xf8, 0xf8, 0xf0, 0xf8, 0xdc, 0x0e, 0x04, 0x00, - 0x00, 0x70, 0x60, 0x50, 0x08, 0x04, 0x00, 0x00 // 0x87 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x20 + 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, + 0x00, 0x10, 0x10, 0x10, 0x00, 0x10, 0x00, 0x00, // 0x21 + 0x44, 0xee, 0xee, 0x44, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x44, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x22 + 0x28, 0x7c, 0xfe, 0x7c, 0xfe, 0x7c, 0x28, 0x00, + 0x00, 0x28, 0x7c, 0x28, 0x7c, 0x28, 0x00, 0x00, // 0x23 + 0x10, 0x38, 0x38, 0x10, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x27 + 0x10, 0x38, 0x70, 0x70, 0x70, 0x38, 0x10, 0x00, + 0x00, 0x10, 0x20, 0x20, 0x20, 0x10, 0x00, 0x00, // 0x28 + 0x20, 0x70, 0x38, 0x38, 0x38, 0x70, 0x20, 0x00, + 0x00, 0x20, 0x10, 0x10, 0x10, 0x20, 0x00, 0x00, // 0x29 + 0x28, 0x7c, 0x38, 0x7c, 0x28, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x10, 0x28, 0x00, 0x00, 0x00, 0x00, // 0x2a + 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x00, + 0x00, 0x10, 0x10, 0x7c, 0x10, 0x10, 0x00, 0x00, // 0x2b + 0x00, 0x00, 0x78, 0x78, 0x78, 0x78, 0x38, 0x00, + 0x00, 0x00, 0x00, 0x30, 0x30, 0x10, 0x00, 0x00, // 0x2c + 0x00, 0x00, 0xfe, 0xfe, 0xfe, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, 0x00, 0x00, // 0x2d + 0x00, 0x00, 0x00, 0x78, 0x78, 0x78, 0x78, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x00, 0x00, // 0x2e + 0x04, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x40, 0x00, + 0x00, 0x04, 0x08, 0x10, 0x20, 0x40, 0x00, 0x00, // 0x2f + 0x7c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7c, 0x00, + 0x00, 0x38, 0x44, 0x54, 0x44, 0x38, 0x00, 0x00, // 0x30 + 0x3c, 0x7c, 0xfc, 0xfc, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x18, 0x28, 0x48, 0x08, 0x7c, 0x00, 0x00, // 0x31 + 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x78, 0x04, 0x78, 0x40, 0x7c, 0x00, 0x00, // 0x32 + 0xfe, 0xfe, 0xfe, 0x7e, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x04, 0x3c, 0x04, 0x7c, 0x00, 0x00, // 0x33 + 0xee, 0xee, 0xfe, 0xfe, 0x7e, 0x0e, 0x0e, 0x00, + 0x00, 0x44, 0x44, 0x3c, 0x04, 0x04, 0x00, 0x00, // 0x34 + 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, + 0x00, 0x7c, 0x40, 0x78, 0x04, 0x78, 0x00, 0x00, // 0x35 + 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x40, 0x7c, 0x44, 0x7c, 0x00, 0x00, // 0x36 + 0xfe, 0xfe, 0xfe, 0x38, 0x70, 0xe0, 0xc0, 0x00, + 0x00, 0x7c, 0x08, 0x10, 0x20, 0x40, 0x00, 0x00, // 0x37 + 0x7c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7c, 0x00, + 0x00, 0x38, 0x44, 0x7c, 0x44, 0x38, 0x00, 0x00, // 0x38 + 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7e, 0x7e, 0x00, + 0x00, 0x7c, 0x44, 0x7c, 0x04, 0x3c, 0x00, 0x00, // 0x39 + 0x78, 0x78, 0x78, 0x78, 0x78, 0x78, 0x78, 0x00, + 0x00, 0x30, 0x30, 0x00, 0x30, 0x30, 0x00, 0x00, // 0x3a + 0x08, 0x1c, 0x38, 0x70, 0x38, 0x1c, 0x08, 0x00, + 0x00, 0x08, 0x10, 0x20, 0x10, 0x08, 0x00, 0x00, // 0x3c + 0xfe, 0xfe, 0xfe, 0x00, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, // 0x3d + 0x20, 0x70, 0x38, 0x1c, 0x38, 0x70, 0x20, 0x00, + 0x00, 0x20, 0x10, 0x08, 0x10, 0x20, 0x00, 0x00, // 0x3e + 0x7c, 0xfe, 0xfe, 0xfe, 0x3c, 0x38, 0x38, 0x00, + 0x00, 0x38, 0x44, 0x18, 0x00, 0x10, 0x00, 0x00, // 0x3f + 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0xee, 0x00, + 0x00, 0x7c, 0x44, 0x7c, 0x44, 0x44, 0x00, 0x00, // 0x41 + 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, + 0x00, 0x78, 0x44, 0x7c, 0x44, 0x78, 0x00, 0x00, // 0x42 + 0xfe, 0xfe, 0xfe, 0xe0, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x40, 0x40, 0x40, 0x7c, 0x00, 0x00, // 0x43 + 0xfc, 0xfe, 0xfe, 0xee, 0xfe, 0xfe, 0xfc, 0x00, + 0x00, 0x78, 0x44, 0x44, 0x44, 0x78, 0x00, 0x00, // 0x44 + 0xfe, 0xfe, 0xfe, 0xfc, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x40, 0x78, 0x40, 0x7c, 0x00, 0x00, // 0x45 + 0xfe, 0xfe, 0xfe, 0xfc, 0xfc, 0xe0, 0xe0, 0x00, + 0x00, 0x7c, 0x40, 0x78, 0x40, 0x40, 0x00, 0x00, // 0x46 + 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x40, 0x5c, 0x44, 0x7c, 0x00, 0x00, // 0x47 + 0xee, 0xee, 0xfe, 0xfe, 0xfe, 0xee, 0xee, 0x00, + 0x00, 0x44, 0x44, 0x7c, 0x44, 0x44, 0x00, 0x00, // 0x48 + 0xfe, 0xfe, 0xfe, 0x38, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x10, 0x10, 0x10, 0x7c, 0x00, 0x00, // 0x49 + 0xfe, 0xfe, 0xfe, 0x1c, 0xfc, 0xfc, 0xfc, 0x00, + 0x00, 0x7c, 0x08, 0x08, 0x08, 0x78, 0x00, 0x00, // 0x4a + 0xe6, 0xee, 0xfc, 0xf8, 0xfc, 0xee, 0xe6, 0x00, + 0x00, 0x44, 0x48, 0x70, 0x48, 0x44, 0x00, 0x00, // 0x4b + 0xe0, 0xe0, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x00, 0x00, // 0x4c + 0x6c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0x00, + 0x00, 0x28, 0x54, 0x54, 0x54, 0x44, 0x00, 0x00, // 0x4d + 0xee, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0x00, + 0x00, 0x44, 0x64, 0x54, 0x4c, 0x44, 0x00, 0x00, // 0x4e + 0xfe, 0xfe, 0xfe, 0xee, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x44, 0x44, 0x44, 0x7c, 0x00, 0x00, // 0x4f + 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0x00, + 0x00, 0x7c, 0x44, 0x7c, 0x40, 0x40, 0x00, 0x00, // 0x50 + 0xfe, 0xfe, 0xfe, 0xee, 0xfc, 0xfe, 0xf6, 0x00, + 0x00, 0x7c, 0x44, 0x44, 0x48, 0x74, 0x00, 0x00, // 0x51 + 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0xe8, 0x00, + 0x00, 0x7c, 0x44, 0x7c, 0x50, 0x48, 0x00, 0x00, // 0x52 + 0x7e, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, + 0x00, 0x3c, 0x40, 0x7c, 0x04, 0x78, 0x00, 0x00, // 0x53 + 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x00, + 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x54 + 0xee, 0xee, 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x00, + 0x00, 0x44, 0x44, 0x44, 0x44, 0x38, 0x00, 0x00, // 0x55 + 0xee, 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x00, + 0x00, 0x44, 0x44, 0x44, 0x28, 0x10, 0x00, 0x00, // 0x56 + 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x6c, 0x00, + 0x00, 0x54, 0x54, 0x54, 0x54, 0x28, 0x00, 0x00, // 0x57 + 0xee, 0xfe, 0xfe, 0x7c, 0xfe, 0xfe, 0xee, 0x00, + 0x00, 0x44, 0x28, 0x10, 0x28, 0x44, 0x00, 0x00, // 0x58 + 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x38, 0x38, 0x00, + 0x00, 0x44, 0x28, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x59 + 0xfe, 0xfe, 0xfe, 0x78, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x08, 0x10, 0x20, 0x7c, 0x00, 0x00, // 0x5a + 0x38, 0x7c, 0x78, 0x70, 0x78, 0x7c, 0x38, 0x00, + 0x00, 0x38, 0x20, 0x20, 0x20, 0x38, 0x00, 0x00, // 0x5b + 0x40, 0xe0, 0x70, 0x38, 0x1c, 0x0e, 0x04, 0x00, + 0x00, 0x40, 0x20, 0x10, 0x08, 0x04, 0x00, 0x00, // 0x5c + 0x38, 0x7c, 0x3c, 0x1c, 0x3c, 0x7c, 0x38, 0x00, + 0x00, 0x38, 0x08, 0x08, 0x08, 0x38, 0x00, 0x00, // 0x5d + 0x00, 0x00, 0x00, 0x00, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, // 0x5f + 0x10, 0x38, 0x7c, 0xfe, 0xfe, 0x38, 0x38, 0x00, + 0x00, 0x10, 0x38, 0x54, 0x10, 0x10, 0x00, 0x00, // 0x80 + 0x3e, 0x3e, 0x1e, 0x3e, 0x76, 0xe0, 0x40, 0x00, + 0x00, 0x1c, 0x0c, 0x14, 0x20, 0x40, 0x00, 0x00, // 0x81 + 0x10, 0x38, 0xfc, 0xfe, 0xfc, 0x38, 0x10, 0x00, + 0x00, 0x10, 0x08, 0x7c, 0x08, 0x10, 0x00, 0x00, // 0x82 + 0x40, 0xe0, 0x76, 0x3e, 0x1e, 0x3e, 0x3e, 0x00, + 0x00, 0x40, 0x20, 0x14, 0x0c, 0x1c, 0x00, 0x00, // 0x83 + 0x38, 0x38, 0xfe, 0xfe, 0x7c, 0x38, 0x10, 0x00, + 0x00, 0x10, 0x10, 0x54, 0x38, 0x10, 0x00, 0x00, // 0x84 + 0x04, 0x0e, 0xdc, 0xf8, 0xf0, 0xf8, 0xf8, 0x00, + 0x00, 0x04, 0x08, 0x50, 0x60, 0x70, 0x00, 0x00, // 0x85 + 0x10, 0x38, 0x7e, 0xfe, 0x7e, 0x38, 0x10, 0x00, + 0x00, 0x10, 0x20, 0x7c, 0x20, 0x10, 0x00, 0x00, // 0x86 + 0xf8, 0xf8, 0xf0, 0xf8, 0xdc, 0x0e, 0x04, 0x00, + 0x00, 0x70, 0x60, 0x50, 0x08, 0x04, 0x00, 0x00 // 0x87 }; diff --git a/flight/targets/boards/osd/firmware/fonts.c b/flight/targets/boards/osd/firmware/fonts.c index 5b0c22d75..96b6bec9d 100644 --- a/flight/targets/boards/osd/firmware/fonts.c +++ b/flight/targets/boards/osd/firmware/fonts.c @@ -6,12 +6,12 @@ * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. - * + * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * + * * You should have received a copy of the GNU General Public License along * with this program; if not, write to the Free Software Foundation, Inc., * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. @@ -21,15 +21,15 @@ // Font table. Add new fonts here. The table must end with a -1 for the id. struct FontEntry fonts[NUM_FONTS + 1] = { - { 0, 8, 14, "Outlined8x14", - font_lookup_outlined8x14, - font_data_outlined8x14, - 0 }, - { 1, 8, 8, "Outlined8x8", - font_lookup_outlined8x8, - font_data_outlined8x8, - FONT_UPPERCASE_ONLY }, - { 2, 8, 10, "font8x10", 0, 0, 0 }, - { 3, 12, 18, "font12x18", 0, 0, 0 }, - { -1, -1, -1, "", 0, 0, 0 } // ends font table + { 0, 8, 14, "Outlined8x14", + font_lookup_outlined8x14, + font_data_outlined8x14, + 0 }, + { 1, 8, 8, "Outlined8x8", + font_lookup_outlined8x8, + font_data_outlined8x8, + FONT_UPPERCASE_ONLY }, + { 2, 8, 10, "font8x10", 0, 0, 0 }, + { 3, 12, 18, "font12x18", 0, 0, 0 }, + { -1, -1, -1, "", 0, 0, 0 } // ends font table }; diff --git a/flight/targets/boards/osd/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/osd/firmware/inc/FreeRTOSConfig.h index 023d20476..62eed5553 100644 --- a/flight/targets/boards/osd/firmware/inc/FreeRTOSConfig.h +++ b/flight/targets/boards/osd/firmware/inc/FreeRTOSConfig.h @@ -1,85 +1,84 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ /** - * @addtogroup PIOS PIOS - * @{ - * @addtogroup FreeRTOS FreeRTOS - * @{ - */ + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ /* Notes: We use 5 task priorities */ -#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... -#define configTICK_RATE_HZ ((portTickType )1000) -#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) -#define configMINIMAL_STACK_SIZE ((unsigned short)512) -#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total -#define configMAX_TASK_NAME_LEN (16) +#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total +#define configMAX_TASK_NAME_LEN (16) -#define configUSE_PREEMPTION 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configIDLE_SHOULD_YIELD 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 -#define configUSE_TIMERS 1 -#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ -#define configTIMER_QUEUE_LENGTH 10 -#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE +#define configUSE_TIMERS 1 +#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ + to exclude the API function. */ -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 1 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 1 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 /* Enable run time stats collection */ -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 /* * Once we move to CMSIS2 we can at least use: @@ -88,16 +87,17 @@ NVIC value of 255. */ * * (still nothing for the DWT registers, surprisingly) */ -#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ - do { \ - (*(unsigned long *)0xe000edfc) |= (1<<24); /* DEMCR |= DEMCR_TRCENA */ \ - (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ - } while(0) -#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ /** - * @} - */ + * @} + */ #endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/osd/firmware/inc/dcc_stdio.h b/flight/targets/boards/osd/firmware/inc/dcc_stdio.h index dbfd39342..3d7354918 100644 --- a/flight/targets/boards/osd/firmware/inc/dcc_stdio.h +++ b/flight/targets/boards/osd/firmware/inc/dcc_stdio.h @@ -1,24 +1,24 @@ /*************************************************************************** - * Copyright (C) 2008 by Dominic Rath * - * Dominic.Rath@gmx.de * - * Copyright (C) 2008 by Spencer Oliver * - * spen@spen-soft.co.uk * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ #ifndef DCC_STDIO_H #define DCC_STDIO_H @@ -33,4 +33,4 @@ void dbg_write_str(const char *msg); void dbg_write_char(char msg); void dbg_write_hex32(const unsigned long val); -#endif /* DCC_STDIO_H */ +#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/osd/firmware/inc/font12x18.h b/flight/targets/boards/osd/firmware/inc/font12x18.h index ef0a45e27..11a599fed 100644 --- a/flight/targets/boards/osd/firmware/inc/font12x18.h +++ b/flight/targets/boards/osd/firmware/inc/font12x18.h @@ -9,9737 +9,9735 @@ #define FONT12X18_H_ static const uint16_t font_frame12x18[] = { - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x03C0, - 0x0660, - 0x0460, - 0x00C0, - 0x0180, - 0x0300, - 0x07E0, - 0x0000, - 0x0078, - 0x006C, - 0x0066, - 0x0066, - 0x0066, - 0x0066, - 0x006C, - 0x0078, - 0x0000, - - 0x0000, - 0x07C0, - 0x0060, - 0x0060, - 0x03C0, - 0x0060, - 0x0060, - 0x07C0, - 0x0000, - 0x0078, - 0x006C, - 0x0066, - 0x0066, - 0x0066, - 0x0066, - 0x006C, - 0x0078, - 0x0000, - - 0x0000, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0003, - 0x0007, - 0x001F, - 0x0038, - 0x0060, - 0x00C0, - 0x00C0, - 0x0180, - 0x0180, - 0x0181, - - 0x0000, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0C00, - 0x0E00, - 0x0F80, - 0x01C0, - 0x0060, - 0x0030, - 0x0030, - 0x0018, - 0x0018, - 0x0818, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x07FE, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0700, - 0x08C6, - 0x0826, - 0x0818, - 0x0C18, - 0x0C24, - 0x0642, - 0x0382, - 0x0381, - 0x03C1, - 0x00F1, - 0x00BE, - 0x0080, - 0x0080, - 0x01C0, - 0x03E0, - 0x07F0, - 0x0000, - - 0x0000, - 0x01F8, - 0x030C, - 0x0606, - 0x0402, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0000, - 0x07FE, - 0x0606, - 0x06FE, - 0x06C6, - 0x06F6, - 0x0606, - 0x07FE, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x030C, - 0x0606, - 0x0402, - 0x0402, - 0x0000, - 0x07FE, - 0x0606, - 0x06FE, - 0x06C6, - 0x06F6, - 0x0606, - 0x07FE, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0208, - 0x030C, - 0x0390, - 0x03E0, - 0x03E0, - 0x01F0, - 0x00F8, - 0x007C, - 0x0080, - 0x0180, - 0x0280, - 0x0480, - 0x0000, - 0x07F8, - 0x0000, - - 0x0181, - 0x0180, - 0x0180, - 0x00C0, - 0x00C0, - 0x0060, - 0x0038, - 0x001F, - 0x0007, - 0x0003, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0000, - - 0x0818, - 0x0018, - 0x0018, - 0x0030, - 0x0030, - 0x0060, - 0x01C0, - 0x0F80, - 0x0E00, - 0x0C00, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0000, - - 0x07FE, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x00F0, - 0x0108, - 0x0108, - 0x0008, - 0x0008, - 0x0008, - 0x0000, - 0x01F8, - 0x030C, - 0x037C, - 0x030C, - 0x037C, - 0x037C, - 0x01F8, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x0108, - 0x0108, - 0x0108, - 0x0000, - 0x01F8, - 0x030C, - 0x037C, - 0x030C, - 0x037C, - 0x037C, - 0x01F8, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x00FF, - 0x01FF, - 0x0387, - 0x0387, - 0x0387, - 0x03FF, - 0x03FF, - 0x03FF, - 0x03FF, - 0x03FF, - 0x03FF, - 0x01FF, - 0x00FF, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FF8, - 0x0FFC, - 0x0FF2, - 0x0C32, - 0x081E, - 0x000E, - 0x000E, - 0x000E, - 0x000E, - 0x081E, - 0x0C3E, - 0x0FFC, - 0x0FF8, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FF8, - 0x0FFC, - 0x0FF2, - 0x0C32, - 0x081E, - 0x000E, - 0x03CE, - 0x07EE, - 0x07EE, - 0x07EE, - 0x07EE, - 0x0BDC, - 0x0C38, - 0x0000, - 0x0000, - 0x0000, - - 0x03F8, - 0x0030, - 0x0060, - 0x01C0, - 0x03F8, - 0x0000, - 0x03F8, - 0x0318, - 0x03F8, - 0x0000, - 0x03F8, - 0x0318, - 0x03F8, - 0x0000, - 0x03B8, - 0x0358, - 0x0318, - 0x0318, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0001, - 0x0003, - 0x0007, - 0x000F, - 0x001F, - 0x003F, - 0x003F, - 0x003F, - 0x003F, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0000, - - 0x0000, - 0x0000, - 0x0800, - 0x0C00, - 0x0E00, - 0x0F00, - 0x0F80, - 0x00C0, - 0x00C0, - 0x00C0, - 0x0FC0, - 0x00C0, - 0x00C0, - 0x00C0, - 0x00C0, - 0x00C0, - 0x00C0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x00F0, - 0x01F8, - 0x03FC, - 0x0000, - 0x07FE, - 0x07E2, - 0x07E2, - 0x0422, - 0x043E, - 0x043E, - 0x043E, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0108, - 0x039C, - 0x018C, - 0x0084, - 0x0084, - 0x0108, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0198, - 0x0198, - 0x0198, - 0x07FE, - 0x07FE, - 0x0198, - 0x0198, - 0x07FE, - 0x07FE, - 0x0198, - 0x0198, - 0x0198, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x01F8, - 0x03FC, - 0x036C, - 0x0360, - 0x03F0, - 0x01F8, - 0x006C, - 0x036C, - 0x03FC, - 0x01F8, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x031C, - 0x0038, - 0x0070, - 0x00E0, - 0x01C0, - 0x038C, - 0x030C, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00E0, - 0x0190, - 0x0110, - 0x0110, - 0x0120, - 0x00C0, - 0x00C0, - 0x0120, - 0x0214, - 0x0208, - 0x0214, - 0x01E2, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0020, - 0x0070, - 0x0030, - 0x0010, - 0x0010, - 0x0020, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x000C, - 0x0018, - 0x0030, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0030, - 0x0018, - 0x000C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0180, - 0x00C0, - 0x0060, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0060, - 0x00C0, - 0x0180, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0402, - 0x0606, - 0x030C, - 0x0198, - 0x00F0, - 0x07FE, - 0x00F0, - 0x0198, - 0x030C, - 0x0606, - 0x0402, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x01F8, - 0x01F8, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x00C0, - 0x0180, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x01F8, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0006, - 0x000E, - 0x001C, - 0x0038, - 0x0070, - 0x00E0, - 0x01C0, - 0x0380, - 0x0700, - 0x0600, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x00E0, - 0x01E0, - 0x01E0, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x01F8, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x031C, - 0x0038, - 0x0070, - 0x00E0, - 0x01C0, - 0x03FC, - 0x03FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03F8, - 0x03FC, - 0x000C, - 0x000C, - 0x00F8, - 0x00F8, - 0x000C, - 0x000C, - 0x03FC, - 0x03F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0038, - 0x0078, - 0x00D8, - 0x0198, - 0x0318, - 0x03FC, - 0x03FC, - 0x0018, - 0x0018, - 0x0018, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x0300, - 0x03F8, - 0x03FC, - 0x000C, - 0x000C, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03F8, - 0x0300, - 0x0300, - 0x03F8, - 0x03FC, - 0x030C, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x000C, - 0x001C, - 0x0038, - 0x0070, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x030C, - 0x01F8, - 0x01F8, - 0x030C, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x030C, - 0x03FC, - 0x01FC, - 0x000C, - 0x000C, - 0x03FC, - 0x03F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x00C0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0018, - 0x0030, - 0x0060, - 0x00C0, - 0x0180, - 0x0180, - 0x00C0, - 0x0060, - 0x0030, - 0x0018, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x01F8, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x01F8, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0300, - 0x0180, - 0x00C0, - 0x0060, - 0x0030, - 0x0030, - 0x0060, - 0x00C0, - 0x0180, - 0x0300, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x070E, - 0x0606, - 0x0006, - 0x000E, - 0x001C, - 0x0038, - 0x0070, - 0x0060, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x0108, - 0x0204, - 0x04D2, - 0x0532, - 0x0512, - 0x0532, - 0x04D4, - 0x0408, - 0x0200, - 0x0104, - 0x00F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x01F8, - 0x03FC, - 0x039C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03F0, - 0x03F8, - 0x031C, - 0x030C, - 0x030C, - 0x031C, - 0x03F8, - 0x03F8, - 0x031C, - 0x030C, - 0x030C, - 0x031C, - 0x03F8, - 0x03F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x01F8, - 0x039C, - 0x030C, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x030C, - 0x039C, - 0x01F8, - 0x00F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03F0, - 0x03F8, - 0x031C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x031C, - 0x03F8, - 0x03F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x03F0, - 0x03F0, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x03FC, - 0x03FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x03F0, - 0x03F0, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x01F0, - 0x03F8, - 0x039C, - 0x030C, - 0x030C, - 0x0300, - 0x0300, - 0x033C, - 0x033C, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x01FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x01F8, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x01F8, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00FC, - 0x00FC, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0330, - 0x0330, - 0x0330, - 0x0330, - 0x0330, - 0x03F0, - 0x01E0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0300, - 0x030C, - 0x031C, - 0x0338, - 0x0370, - 0x03E0, - 0x03C0, - 0x0380, - 0x03C0, - 0x03E0, - 0x0370, - 0x0338, - 0x031C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x03FC, - 0x03FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x039C, - 0x03FC, - 0x03FC, - 0x036C, - 0x036C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x038C, - 0x038C, - 0x03CC, - 0x03EC, - 0x037C, - 0x033C, - 0x031C, - 0x031C, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x01F8, - 0x039C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x039C, - 0x01F8, - 0x00F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03F0, - 0x03F8, - 0x031C, - 0x030C, - 0x030C, - 0x031C, - 0x03F8, - 0x03F0, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x01F8, - 0x039C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x036C, - 0x037C, - 0x03B8, - 0x01FC, - 0x00EC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03F0, - 0x03F8, - 0x031C, - 0x030C, - 0x030C, - 0x031C, - 0x03F8, - 0x03E0, - 0x03E0, - 0x0370, - 0x0338, - 0x031C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00F8, - 0x01FC, - 0x038C, - 0x0300, - 0x0300, - 0x0380, - 0x01F0, - 0x00F8, - 0x001C, - 0x000C, - 0x000C, - 0x031C, - 0x03F8, - 0x01F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x039C, - 0x01F8, - 0x00F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x039C, - 0x0198, - 0x01F8, - 0x00F0, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x036C, - 0x036C, - 0x036C, - 0x036C, - 0x036C, - 0x036C, - 0x03FC, - 0x01F8, - 0x0198, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x0198, - 0x00F0, - 0x0060, - 0x0060, - 0x00F0, - 0x0198, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x039C, - 0x01F8, - 0x00F0, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x000C, - 0x000C, - 0x001C, - 0x0038, - 0x0070, - 0x00E0, - 0x01C0, - 0x0380, - 0x0300, - 0x0300, - 0x03FC, - 0x03FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0078, - 0x0078, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0078, - 0x0078, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0600, - 0x0700, - 0x0380, - 0x01C0, - 0x00E0, - 0x0070, - 0x0038, - 0x001C, - 0x000E, - 0x0006, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x01E0, - 0x01E0, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x01E0, - 0x01E0, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x00F0, - 0x0198, - 0x0108, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x07FE, - 0x07FE, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0300, - 0x0180, - 0x00C0, - 0x0060, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x01F8, - 0x039C, - 0x030C, - 0x030C, - 0x03FC, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03F8, - 0x03FC, - 0x030C, - 0x030C, - 0x03F8, - 0x03F8, - 0x030C, - 0x030C, - 0x03FC, - 0x03F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03F0, - 0x03F8, - 0x031C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x031C, - 0x03F8, - 0x03F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x0300, - 0x0300, - 0x03F0, - 0x03F0, - 0x0300, - 0x0300, - 0x03FC, - 0x03FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x0300, - 0x0300, - 0x03F8, - 0x03F8, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x038C, - 0x0300, - 0x033C, - 0x033C, - 0x030C, - 0x038C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x00F0, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x00F0, - 0x00F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x007C, - 0x007C, - 0x0018, - 0x0018, - 0x0018, - 0x0018, - 0x0018, - 0x0198, - 0x01F8, - 0x00F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x031C, - 0x0338, - 0x0370, - 0x03E0, - 0x03E0, - 0x03F0, - 0x0338, - 0x031C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x03FC, - 0x03FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x039C, - 0x03FC, - 0x036C, - 0x036C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x038C, - 0x03CC, - 0x03EC, - 0x037C, - 0x033C, - 0x031C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03F8, - 0x03FC, - 0x030C, - 0x030C, - 0x03FC, - 0x03F8, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x033C, - 0x0338, - 0x03FC, - 0x01EC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03F8, - 0x03FC, - 0x030C, - 0x030C, - 0x03F8, - 0x03E0, - 0x0370, - 0x0338, - 0x031C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x0300, - 0x03F8, - 0x01FC, - 0x000C, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x039C, - 0x01F8, - 0x00F0, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x036C, - 0x036C, - 0x03FC, - 0x01F8, - 0x0198, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x039C, - 0x01F8, - 0x00F0, - 0x0060, - 0x0060, - 0x00F0, - 0x01F8, - 0x039C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x039C, - 0x01F8, - 0x00F0, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x001C, - 0x0038, - 0x0070, - 0x00E0, - 0x01C0, - 0x0380, - 0x03FC, - 0x03FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0038, - 0x0038, - 0x0078, - 0x0060, - 0x0060, - 0x0060, - 0x00C0, - 0x00C0, - 0x0060, - 0x0060, - 0x0060, - 0x0078, - 0x0038, - 0x0038, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x01C0, - 0x01E0, - 0x01E0, - 0x0060, - 0x0060, - 0x0060, - 0x0030, - 0x0030, - 0x0060, - 0x0060, - 0x0060, - 0x01E0, - 0x01E0, - 0x01C0, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00C2, - 0x01E6, - 0x033C, - 0x0618, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0280, - 0x02C0, - 0x02E0, - 0x02F0, - 0x02F8, - 0x02F0, - 0x02E0, - 0x02C0, - 0x0280, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0700, - 0x0000, - - 0x0000, - 0x0660, - 0x06C0, - 0x0700, - 0x06C0, - 0x0660, - 0x0000, - 0x06BE, - 0x0688, - 0x0588, - 0x0588, - 0x0000, - 0x0066, - 0x0066, - 0x007E, - 0x0066, - 0x0066, - 0x0000, - - 0x0000, - 0x0660, - 0x06C0, - 0x0700, - 0x06C0, - 0x0660, - 0x0000, - 0x01DC, - 0x01AC, - 0x01AC, - 0x018C, - 0x0000, - 0x0066, - 0x0066, - 0x007E, - 0x0066, - 0x0066, - 0x0000, - - 0x0000, - 0x07F0, - 0x06B0, - 0x06B0, - 0x0630, - 0x0000, - 0x0078, - 0x0048, - 0x00FC, - 0x00CC, - 0x00CC, - 0x0000, - 0x0066, - 0x0066, - 0x007E, - 0x0066, - 0x0066, - 0x0000, - - 0x0000, - 0x0600, - 0x0600, - 0x0600, - 0x07E0, - 0x07E0, - 0x0000, - 0x00F8, - 0x018C, - 0x01FC, - 0x018C, - 0x0000, - 0x007E, - 0x007E, - 0x0018, - 0x0018, - 0x0018, - 0x0000, - - 0x0000, - 0x0600, - 0x0600, - 0x0600, - 0x07E0, - 0x07E0, - 0x0000, - 0x01F8, - 0x039C, - 0x039C, - 0x01F8, - 0x0000, - 0x00E6, - 0x00F6, - 0x00FE, - 0x00DE, - 0x00CE, - 0x0000, - - 0x0000, - 0x03E0, - 0x0630, - 0x07F0, - 0x0630, - 0x0000, - 0x0180, - 0x0180, - 0x0180, - 0x01F8, - 0x01F8, - 0x0000, - 0x007E, - 0x007E, - 0x0018, - 0x0018, - 0x0018, - 0x0000, - - 0x0000, - 0x0660, - 0x0660, - 0x03C0, - 0x0180, - 0x0000, - 0x01F8, - 0x0180, - 0x01F0, - 0x0180, - 0x01F8, - 0x0000, - 0x0030, - 0x0030, - 0x0030, - 0x003E, - 0x003E, - 0x0000, - - 0x0000, - 0x07E0, - 0x07E0, - 0x0180, - 0x0180, - 0x0180, - 0x0000, - 0x00D8, - 0x00F8, - 0x00D8, - 0x00D8, - 0x0000, - 0x003C, - 0x0022, - 0x0026, - 0x003C, - 0x0036, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0660, - 0x07E0, - 0x05A0, - 0x0420, - 0x0000, - 0x07E0, - 0x0000, - 0x07E0, - 0x0400, - 0x07E0, - 0x0020, - 0x07E0, - 0x0000, - - 0x0000, - 0x03E0, - 0x0630, - 0x07F0, - 0x0630, - 0x0000, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - 0x007E, - 0x0046, - 0x0078, - 0x006C, - 0x0066, - 0x0000, - - 0x0000, - 0x0770, - 0x06B0, - 0x06B0, - 0x06B0, - 0x06B0, - 0x0000, - 0x00F8, - 0x018C, - 0x01FC, - 0x018C, - 0x0000, - 0x0066, - 0x0066, - 0x0066, - 0x003C, - 0x0018, - 0x0000, - - 0x0000, - 0x0200, - 0x020C, - 0x0212, - 0x0012, - 0x0712, - 0x0012, - 0x070C, - 0x0700, - 0x0700, - 0x0712, - 0x0712, - 0x0014, - 0x0218, - 0x0214, - 0x0212, - 0x0212, - 0x0200, - - 0x0000, - 0x0210, - 0x0210, - 0x0210, - 0x001E, - 0x0700, - 0x000C, - 0x0712, - 0x0712, - 0x0712, - 0x070C, - 0x0700, - 0x001E, - 0x0210, - 0x021E, - 0x0202, - 0x021E, - 0x0200, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0618, - 0x0738, - 0x07F8, - 0x06D8, - 0x06D8, - 0x0618, - 0x0618, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0630, - 0x0630, - 0x0630, - 0x0630, - 0x0360, - 0x03E0, - 0x01C0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01C0, - 0x03E0, - 0x0220, - 0x0630, - 0x07F0, - 0x0630, - 0x0630, - 0x0000, - - 0x0000, - 0x0000, - 0x0003, - 0x001F, - 0x000F, - 0x000F, - 0x000F, - 0x000F, - 0x000F, - 0x000F, - 0x000F, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0800, - 0x0F00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0007, - 0x0003, - 0x0007, - 0x000F, - 0x001F, - 0x003F, - 0x003F, - 0x0007, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0E00, - 0x0F00, - 0x0F80, - 0x0E00, - 0x0E00, - 0x0C00, - 0x0C00, - 0x0800, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0001, - 0x0003, - 0x001F, - 0x007F, - 0x00FF, - 0x007F, - 0x001C, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0E60, - 0x0800, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0001, - 0x003F, - 0x00FF, - 0x007F, - 0x0078, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0700, - 0x0FF0, - 0x0FE0, - 0x0FE0, - 0x08C0, - 0x0040, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x007F, - 0x007F, - 0x007F, - 0x007F, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0300, - 0x0FC0, - 0x0FF8, - 0x0FF0, - 0x0FE0, - 0x0180, - 0x0100, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0010, - 0x003F, - 0x007F, - 0x001F, - 0x0001, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0880, - 0x0FE0, - 0x0FF0, - 0x0FF8, - 0x07F0, - 0x0700, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x000C, - 0x001F, - 0x003F, - 0x001F, - 0x0007, - 0x0001, - 0x0001, - 0x0003, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0C00, - 0x0FC0, - 0x0FE0, - 0x0FE0, - 0x0FF0, - 0x0FF0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x001F, - 0x000F, - 0x0007, - 0x0007, - 0x0003, - 0x0007, - 0x000F, - 0x0001, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0800, - 0x0C00, - 0x0E00, - 0x0F60, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0180, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0007, - 0x0007, - 0x0007, - 0x0007, - 0x000F, - 0x003F, - 0x000F, - 0x0007, - 0x0003, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0F00, - 0x0FC0, - 0x0E00, - 0x0C00, - 0x0800, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0001, - 0x0003, - 0x0007, - 0x006F, - 0x003F, - 0x003F, - 0x003F, - 0x003F, - 0x0018, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0F80, - 0x0F00, - 0x0E00, - 0x0E00, - 0x0C00, - 0x0E00, - 0x0F00, - 0x0800, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0003, - 0x003F, - 0x007F, - 0x007F, - 0x00FF, - 0x00FF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0300, - 0x0F80, - 0x0FC0, - 0x0F80, - 0x0E00, - 0x0800, - 0x0800, - 0x0C00, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0011, - 0x007F, - 0x00FF, - 0x01FF, - 0x00FE, - 0x000E, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0080, - 0x0FC0, - 0x0FE0, - 0x0F80, - 0x0800, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x000C, - 0x003F, - 0x01FF, - 0x00FF, - 0x007F, - 0x0018, - 0x0008, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FE0, - 0x0FE0, - 0x0FE0, - 0x0FE0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x000E, - 0x00FF, - 0x007F, - 0x007F, - 0x0031, - 0x0020, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0800, - 0x0FC0, - 0x0FF0, - 0x0FE0, - 0x01E0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x003F, - 0x003F, - 0x003F, - 0x0067, - 0x0001, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0800, - 0x0C00, - 0x0F80, - 0x0FE0, - 0x0FF0, - 0x0FE0, - 0x0380, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0007, - 0x000F, - 0x001F, - 0x0007, - 0x0007, - 0x0003, - 0x0003, - 0x0001, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0E00, - 0x0C00, - 0x0E00, - 0x0F00, - 0x0F80, - 0x0FC0, - 0x0FC0, - 0x0E00, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0180, - 0x0240, - 0x0240, - 0x0180, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0060, - 0x00F0, - 0x01F8, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x01F8, - 0x00F0, - 0x0060, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0100, - 0x0300, - 0x07C0, - 0x07F0, - 0x0330, - 0x0118, - 0x0018, - 0x0018, - 0x0118, - 0x0330, - 0x07F0, - 0x07C0, - 0x0300, - 0x0100, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x00F0, - 0x07FE, - 0x0606, - 0x0462, - 0x0462, - 0x0462, - 0x0462, - 0x0462, - 0x0462, - 0x0462, - 0x0402, - 0x0462, - 0x0462, - 0x0402, - 0x0606, - 0x07FE, - 0x0000, - - 0x0000, - 0x00F0, - 0x07FE, - 0x070E, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x07FE, - 0x07FE, - 0x0000, - - 0x0000, - 0x00F0, - 0x07FE, - 0x070E, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x0000, - - 0x0000, - 0x00F0, - 0x07FE, - 0x070E, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x0000, - - 0x0000, - 0x00F0, - 0x07FE, - 0x070E, - 0x0606, - 0x0606, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x0000, - - 0x0000, - 0x00F0, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x0000, - - 0x0000, - 0x0000, - 0x03FF, - 0x03FF, - 0x0300, - 0x0300, - 0x033C, - 0x033C, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x01F8, - 0x030C, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x030C, - 0x01F8, - 0x00F0, - - 0x0000, - 0x0000, - 0x0F00, - 0x0F00, - 0x033F, - 0x033F, - 0x0333, - 0x0333, - 0x0333, - 0x0333, - 0x03F3, - 0x03F3, - 0x0303, - 0x0303, - 0x0303, - 0x0303, - 0x0303, - 0x0303, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x000F, - 0x000F, - 0x03FC, - 0x03FC, - 0x030F, - 0x030F, - 0x0300, - 0x0300, - 0x0F00, - 0x0F00, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0F00, - 0x0F00, - 0x0000, - 0x0000, - 0x0F00, - 0x0F00, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x00F0, - 0x00F0, - 0x01F8, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0040, - 0x0040, - 0x0E4F, - 0x0040, - 0x0040, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x038C, - 0x038C, - 0x03CC, - 0x036C, - 0x0B6D, - 0x033C, - 0x031C, - 0x030C, - 0x030C, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x01F8, - 0x0188, - 0x0180, - 0x01F0, - 0x0EFB, - 0x0018, - 0x0118, - 0x01F8, - 0x00F0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x01F8, - 0x0180, - 0x0180, - 0x01E0, - 0x0DEF, - 0x0180, - 0x0180, - 0x01F8, - 0x01F8, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x036C, - 0x0D6B, - 0x01F8, - 0x0198, - 0x0198, - 0x0198, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0E0F, - 0x0318, - 0x01B0, - 0x00E0, - 0x0040, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0070, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x007F, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00E0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FE0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0300, - 0x0300, - 0x03FF, - 0x03FF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0300, - 0x0300, - 0x0F00, - 0x0F00, - 0x0000, - 0x0000, - 0x0066, - 0x0066, - 0x0066, - 0x0066, - 0x0066, - 0x007E, - 0x003C, - 0x0018, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0180, - 0x0380, - 0x0380, - 0x0180, - 0x0180, - 0x0180, - 0x03C6, - 0x03C6, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03C0, - 0x0660, - 0x0660, - 0x03E0, - 0x0060, - 0x0260, - 0x07E0, - 0x03C0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x01F8, - 0x01F8, - 0x00F0, - 0x00F0, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0004, - 0x0004, - 0x0007, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0007, - 0x0004, - 0x0004, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0E00, - 0x0200, - 0x0200, - - 0x0200, - 0x0200, - 0x0E00, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0070, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0010, - 0x0018, - 0x001C, - 0x001E, - 0x001C, - 0x0018, - 0x0010, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00E0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0080, - 0x0180, - 0x0380, - 0x0780, - 0x0380, - 0x0180, - 0x0080, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x003F, - 0x003F, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x003F, - 0x003F, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FC0, - 0x0FC0, - 0x00C0, - 0x00C0, - 0x00C0, - 0x00C0, - - 0x00C0, - 0x00C0, - 0x00C0, - 0x00C0, - 0x0FC0, - 0x0FC0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0660, - 0x07E0, - 0x05A0, - 0x0420, - 0x0004, - 0x03C4, - 0x0664, - 0x0660, - 0x0660, - 0x03C4, - 0x0004, - 0x07C4, - 0x0660, - 0x0660, - 0x0660, - 0x07C0, - 0x0000, - - 0x0000, - 0x0040, - 0x0040, - 0x0040, - 0x07FC, - 0x0444, - 0x0550, - 0x0446, - 0x07F4, - 0x0604, - 0x0014, - 0x0054, - 0x0054, - 0x0154, - 0x0154, - 0x0554, - 0x0556, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0660, - 0x0660, - 0x0660, - 0x03C0, - 0x0186, - 0x0004, - 0x0004, - 0x03C4, - 0x0664, - 0x0664, - 0x07E4, - 0x0664, - 0x0004, - 0x0006, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0660, - 0x0660, - 0x0660, - 0x03C0, - 0x0186, - 0x0004, - 0x0004, - 0x07C4, - 0x0664, - 0x0784, - 0x0664, - 0x07C4, - 0x0004, - 0x0006, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03C0, - 0x0600, - 0x0600, - 0x0600, - 0x03C6, - 0x0004, - 0x0004, - 0x03C4, - 0x0664, - 0x0664, - 0x07E4, - 0x0664, - 0x0004, - 0x0006, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03C0, - 0x0600, - 0x0600, - 0x0600, - 0x03C6, - 0x0004, - 0x0004, - 0x07C4, - 0x0664, - 0x0784, - 0x0664, - 0x07C4, - 0x0004, - 0x0006, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0660, - 0x07E0, - 0x05A0, - 0x0420, - 0x0004, - 0x03C4, - 0x0664, - 0x0660, - 0x07E0, - 0x0664, - 0x0004, - 0x0664, - 0x0660, - 0x0660, - 0x03C0, - 0x0180, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x070E, - 0x0606, - 0x0402, - 0x0402, - 0x0402, - 0x0402, - 0x0606, - 0x070E, - 0x03FC, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x070E, - 0x0606, - 0x04F2, - 0x04F2, - 0x04F2, - 0x04F2, - 0x0606, - 0x070E, - 0x03FC, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0003, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0003, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0C00, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0C00, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFE, - 0x0402, - 0x050A, - 0x058A, - 0x05CA, - 0x05EA, - 0x05FA, - 0x05FA, - 0x05EA, - 0x05CA, - 0x058A, - 0x050A, - 0x0402, - 0x0FFE, - 0x0000, - 0x0000, - - 0x0000, - 0x0001, - 0x0001, - 0x0030, - 0x0033, - 0x0007, - 0x000C, - 0x0018, - 0x00D8, - 0x00D8, - 0x0018, - 0x000C, - 0x0007, - 0x0033, - 0x0030, - 0x0001, - 0x0001, - 0x0000, - - 0x0000, - 0x0800, - 0x0800, - 0x00C0, - 0x0CC0, - 0x0E00, - 0x0300, - 0x0180, - 0x01B0, - 0x01B0, - 0x0180, - 0x0300, - 0x0E00, - 0x0CC0, - 0x00C0, - 0x0800, - 0x0800, - 0x0000, - - 0x0000, - 0x0000, - 0x0003, - 0x000F, - 0x001C, - 0x0030, - 0x0030, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0030, - 0x0030, - 0x001C, - 0x000F, - 0x0003, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0C00, - 0x0F00, - 0x0F80, - 0x0FC0, - 0x0FC0, - 0x0FE0, - 0x0FE0, - 0x0FE0, - 0x0FE0, - 0x0FC0, - 0x0FC0, - 0x0F80, - 0x0F00, - 0x0C00, - 0x0000, - 0x0000, - - 0x0000, - 0x0006, - 0x000E, - 0x001A, - 0x0032, - 0x07E2, - 0x04C2, - 0x04C2, - 0x04C2, - 0x04C2, - 0x04C2, - 0x07E2, - 0x0032, - 0x001A, - 0x000E, - 0x0006, - 0x0000, - 0x0000, - - 0x0000, - 0x0060, - 0x0030, - 0x0010, - 0x0190, - 0x0098, - 0x04C8, - 0x0648, - 0x0248, - 0x0648, - 0x04C8, - 0x0098, - 0x0190, - 0x0010, - 0x0030, - 0x0060, - 0x0000, - 0x0000, - - 0x0000, - 0x07FE, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x0198, - 0x00F0, - 0x0060, - 0x0060, - 0x00F0, - 0x0198, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x07FE, - 0x0000, - - 0x0000, - 0x0000, - 0x0100, - 0x0180, - 0x01C0, - 0x01E0, - 0x01F0, - 0x01F8, - 0x01FC, - 0x01FE, - 0x01F0, - 0x01F0, - 0x0138, - 0x0038, - 0x001C, - 0x001C, - 0x0008, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0322, - 0x04B6, - 0x04AA, - 0x04AA, - 0x07A2, - 0x04A2, - 0x04A2, - 0x04A2, - 0x04A2, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0722, - 0x04B6, - 0x04AA, - 0x04AA, - 0x0722, - 0x0422, - 0x0422, - 0x0422, - 0x0422, - 0x0000, - - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x03C0, + 0x0660, + 0x0460, + 0x00C0, + 0x0180, + 0x0300, + 0x07E0, + 0x0000, + 0x0078, + 0x006C, + 0x0066, + 0x0066, + 0x0066, + 0x0066, + 0x006C, + 0x0078, + 0x0000, + + 0x0000, + 0x07C0, + 0x0060, + 0x0060, + 0x03C0, + 0x0060, + 0x0060, + 0x07C0, + 0x0000, + 0x0078, + 0x006C, + 0x0066, + 0x0066, + 0x0066, + 0x0066, + 0x006C, + 0x0078, + 0x0000, + + 0x0000, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0003, + 0x0007, + 0x001F, + 0x0038, + 0x0060, + 0x00C0, + 0x00C0, + 0x0180, + 0x0180, + 0x0181, + + 0x0000, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0C00, + 0x0E00, + 0x0F80, + 0x01C0, + 0x0060, + 0x0030, + 0x0030, + 0x0018, + 0x0018, + 0x0818, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x07FE, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0700, + 0x08C6, + 0x0826, + 0x0818, + 0x0C18, + 0x0C24, + 0x0642, + 0x0382, + 0x0381, + 0x03C1, + 0x00F1, + 0x00BE, + 0x0080, + 0x0080, + 0x01C0, + 0x03E0, + 0x07F0, + 0x0000, + + 0x0000, + 0x01F8, + 0x030C, + 0x0606, + 0x0402, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0000, + 0x07FE, + 0x0606, + 0x06FE, + 0x06C6, + 0x06F6, + 0x0606, + 0x07FE, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x030C, + 0x0606, + 0x0402, + 0x0402, + 0x0000, + 0x07FE, + 0x0606, + 0x06FE, + 0x06C6, + 0x06F6, + 0x0606, + 0x07FE, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0208, + 0x030C, + 0x0390, + 0x03E0, + 0x03E0, + 0x01F0, + 0x00F8, + 0x007C, + 0x0080, + 0x0180, + 0x0280, + 0x0480, + 0x0000, + 0x07F8, + 0x0000, + + 0x0181, + 0x0180, + 0x0180, + 0x00C0, + 0x00C0, + 0x0060, + 0x0038, + 0x001F, + 0x0007, + 0x0003, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0000, + + 0x0818, + 0x0018, + 0x0018, + 0x0030, + 0x0030, + 0x0060, + 0x01C0, + 0x0F80, + 0x0E00, + 0x0C00, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0000, + + 0x07FE, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x00F0, + 0x0108, + 0x0108, + 0x0008, + 0x0008, + 0x0008, + 0x0000, + 0x01F8, + 0x030C, + 0x037C, + 0x030C, + 0x037C, + 0x037C, + 0x01F8, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x0108, + 0x0108, + 0x0108, + 0x0000, + 0x01F8, + 0x030C, + 0x037C, + 0x030C, + 0x037C, + 0x037C, + 0x01F8, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x00FF, + 0x01FF, + 0x0387, + 0x0387, + 0x0387, + 0x03FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x01FF, + 0x00FF, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FF8, + 0x0FFC, + 0x0FF2, + 0x0C32, + 0x081E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x081E, + 0x0C3E, + 0x0FFC, + 0x0FF8, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FF8, + 0x0FFC, + 0x0FF2, + 0x0C32, + 0x081E, + 0x000E, + 0x03CE, + 0x07EE, + 0x07EE, + 0x07EE, + 0x07EE, + 0x0BDC, + 0x0C38, + 0x0000, + 0x0000, + 0x0000, + + 0x03F8, + 0x0030, + 0x0060, + 0x01C0, + 0x03F8, + 0x0000, + 0x03F8, + 0x0318, + 0x03F8, + 0x0000, + 0x03F8, + 0x0318, + 0x03F8, + 0x0000, + 0x03B8, + 0x0358, + 0x0318, + 0x0318, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0001, + 0x0003, + 0x0007, + 0x000F, + 0x001F, + 0x003F, + 0x003F, + 0x003F, + 0x003F, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0000, + + 0x0000, + 0x0000, + 0x0800, + 0x0C00, + 0x0E00, + 0x0F00, + 0x0F80, + 0x00C0, + 0x00C0, + 0x00C0, + 0x0FC0, + 0x00C0, + 0x00C0, + 0x00C0, + 0x00C0, + 0x00C0, + 0x00C0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x01F8, + 0x03FC, + 0x0000, + 0x07FE, + 0x07E2, + 0x07E2, + 0x0422, + 0x043E, + 0x043E, + 0x043E, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0108, + 0x039C, + 0x018C, + 0x0084, + 0x0084, + 0x0108, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0198, + 0x0198, + 0x0198, + 0x07FE, + 0x07FE, + 0x0198, + 0x0198, + 0x07FE, + 0x07FE, + 0x0198, + 0x0198, + 0x0198, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x01F8, + 0x03FC, + 0x036C, + 0x0360, + 0x03F0, + 0x01F8, + 0x006C, + 0x036C, + 0x03FC, + 0x01F8, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x031C, + 0x0038, + 0x0070, + 0x00E0, + 0x01C0, + 0x038C, + 0x030C, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00E0, + 0x0190, + 0x0110, + 0x0110, + 0x0120, + 0x00C0, + 0x00C0, + 0x0120, + 0x0214, + 0x0208, + 0x0214, + 0x01E2, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0020, + 0x0070, + 0x0030, + 0x0010, + 0x0010, + 0x0020, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x000C, + 0x0018, + 0x0030, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0030, + 0x0018, + 0x000C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0180, + 0x00C0, + 0x0060, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0060, + 0x00C0, + 0x0180, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0402, + 0x0606, + 0x030C, + 0x0198, + 0x00F0, + 0x07FE, + 0x00F0, + 0x0198, + 0x030C, + 0x0606, + 0x0402, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x01F8, + 0x01F8, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x00C0, + 0x0180, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x01F8, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0006, + 0x000E, + 0x001C, + 0x0038, + 0x0070, + 0x00E0, + 0x01C0, + 0x0380, + 0x0700, + 0x0600, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00E0, + 0x01E0, + 0x01E0, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x01F8, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x031C, + 0x0038, + 0x0070, + 0x00E0, + 0x01C0, + 0x03FC, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x03FC, + 0x000C, + 0x000C, + 0x00F8, + 0x00F8, + 0x000C, + 0x000C, + 0x03FC, + 0x03F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0038, + 0x0078, + 0x00D8, + 0x0198, + 0x0318, + 0x03FC, + 0x03FC, + 0x0018, + 0x0018, + 0x0018, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x0300, + 0x03F8, + 0x03FC, + 0x000C, + 0x000C, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03F8, + 0x0300, + 0x0300, + 0x03F8, + 0x03FC, + 0x030C, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x000C, + 0x001C, + 0x0038, + 0x0070, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x030C, + 0x01F8, + 0x01F8, + 0x030C, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x030C, + 0x03FC, + 0x01FC, + 0x000C, + 0x000C, + 0x03FC, + 0x03F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x00C0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0018, + 0x0030, + 0x0060, + 0x00C0, + 0x0180, + 0x0180, + 0x00C0, + 0x0060, + 0x0030, + 0x0018, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x01F8, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x01F8, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0300, + 0x0180, + 0x00C0, + 0x0060, + 0x0030, + 0x0030, + 0x0060, + 0x00C0, + 0x0180, + 0x0300, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x070E, + 0x0606, + 0x0006, + 0x000E, + 0x001C, + 0x0038, + 0x0070, + 0x0060, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x0108, + 0x0204, + 0x04D2, + 0x0532, + 0x0512, + 0x0532, + 0x04D4, + 0x0408, + 0x0200, + 0x0104, + 0x00F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x039C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03F0, + 0x03F8, + 0x031C, + 0x030C, + 0x030C, + 0x031C, + 0x03F8, + 0x03F8, + 0x031C, + 0x030C, + 0x030C, + 0x031C, + 0x03F8, + 0x03F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x039C, + 0x030C, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x030C, + 0x039C, + 0x01F8, + 0x00F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03F0, + 0x03F8, + 0x031C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x031C, + 0x03F8, + 0x03F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x03F0, + 0x03F0, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x03FC, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x03F0, + 0x03F0, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x01F0, + 0x03F8, + 0x039C, + 0x030C, + 0x030C, + 0x0300, + 0x0300, + 0x033C, + 0x033C, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x01FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x01F8, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x01F8, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00FC, + 0x00FC, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0330, + 0x0330, + 0x0330, + 0x0330, + 0x0330, + 0x03F0, + 0x01E0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0300, + 0x030C, + 0x031C, + 0x0338, + 0x0370, + 0x03E0, + 0x03C0, + 0x0380, + 0x03C0, + 0x03E0, + 0x0370, + 0x0338, + 0x031C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x03FC, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x039C, + 0x03FC, + 0x03FC, + 0x036C, + 0x036C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x038C, + 0x038C, + 0x03CC, + 0x03EC, + 0x037C, + 0x033C, + 0x031C, + 0x031C, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x039C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x039C, + 0x01F8, + 0x00F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03F0, + 0x03F8, + 0x031C, + 0x030C, + 0x030C, + 0x031C, + 0x03F8, + 0x03F0, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x039C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x036C, + 0x037C, + 0x03B8, + 0x01FC, + 0x00EC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03F0, + 0x03F8, + 0x031C, + 0x030C, + 0x030C, + 0x031C, + 0x03F8, + 0x03E0, + 0x03E0, + 0x0370, + 0x0338, + 0x031C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00F8, + 0x01FC, + 0x038C, + 0x0300, + 0x0300, + 0x0380, + 0x01F0, + 0x00F8, + 0x001C, + 0x000C, + 0x000C, + 0x031C, + 0x03F8, + 0x01F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x039C, + 0x01F8, + 0x00F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x039C, + 0x0198, + 0x01F8, + 0x00F0, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x036C, + 0x036C, + 0x036C, + 0x036C, + 0x036C, + 0x036C, + 0x03FC, + 0x01F8, + 0x0198, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x0198, + 0x00F0, + 0x0060, + 0x0060, + 0x00F0, + 0x0198, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x039C, + 0x01F8, + 0x00F0, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x000C, + 0x000C, + 0x001C, + 0x0038, + 0x0070, + 0x00E0, + 0x01C0, + 0x0380, + 0x0300, + 0x0300, + 0x03FC, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0078, + 0x0078, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0078, + 0x0078, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0600, + 0x0700, + 0x0380, + 0x01C0, + 0x00E0, + 0x0070, + 0x0038, + 0x001C, + 0x000E, + 0x0006, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x01E0, + 0x01E0, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x01E0, + 0x01E0, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x0198, + 0x0108, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x07FE, + 0x07FE, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0300, + 0x0180, + 0x00C0, + 0x0060, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x039C, + 0x030C, + 0x030C, + 0x03FC, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x03FC, + 0x030C, + 0x030C, + 0x03F8, + 0x03F8, + 0x030C, + 0x030C, + 0x03FC, + 0x03F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F0, + 0x03F8, + 0x031C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x031C, + 0x03F8, + 0x03F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x0300, + 0x0300, + 0x03F0, + 0x03F0, + 0x0300, + 0x0300, + 0x03FC, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x0300, + 0x0300, + 0x03F8, + 0x03F8, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x038C, + 0x0300, + 0x033C, + 0x033C, + 0x030C, + 0x038C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x00F0, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x00F0, + 0x00F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x007C, + 0x007C, + 0x0018, + 0x0018, + 0x0018, + 0x0018, + 0x0018, + 0x0198, + 0x01F8, + 0x00F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x031C, + 0x0338, + 0x0370, + 0x03E0, + 0x03E0, + 0x03F0, + 0x0338, + 0x031C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x03FC, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x039C, + 0x03FC, + 0x036C, + 0x036C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x038C, + 0x03CC, + 0x03EC, + 0x037C, + 0x033C, + 0x031C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x03FC, + 0x030C, + 0x030C, + 0x03FC, + 0x03F8, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x033C, + 0x0338, + 0x03FC, + 0x01EC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x03FC, + 0x030C, + 0x030C, + 0x03F8, + 0x03E0, + 0x0370, + 0x0338, + 0x031C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x0300, + 0x03F8, + 0x01FC, + 0x000C, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x039C, + 0x01F8, + 0x00F0, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x036C, + 0x036C, + 0x03FC, + 0x01F8, + 0x0198, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x039C, + 0x01F8, + 0x00F0, + 0x0060, + 0x0060, + 0x00F0, + 0x01F8, + 0x039C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x039C, + 0x01F8, + 0x00F0, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x001C, + 0x0038, + 0x0070, + 0x00E0, + 0x01C0, + 0x0380, + 0x03FC, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0038, + 0x0038, + 0x0078, + 0x0060, + 0x0060, + 0x0060, + 0x00C0, + 0x00C0, + 0x0060, + 0x0060, + 0x0060, + 0x0078, + 0x0038, + 0x0038, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x01C0, + 0x01E0, + 0x01E0, + 0x0060, + 0x0060, + 0x0060, + 0x0030, + 0x0030, + 0x0060, + 0x0060, + 0x0060, + 0x01E0, + 0x01E0, + 0x01C0, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00C2, + 0x01E6, + 0x033C, + 0x0618, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0280, + 0x02C0, + 0x02E0, + 0x02F0, + 0x02F8, + 0x02F0, + 0x02E0, + 0x02C0, + 0x0280, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0700, + 0x0000, + + 0x0000, + 0x0660, + 0x06C0, + 0x0700, + 0x06C0, + 0x0660, + 0x0000, + 0x06BE, + 0x0688, + 0x0588, + 0x0588, + 0x0000, + 0x0066, + 0x0066, + 0x007E, + 0x0066, + 0x0066, + 0x0000, + + 0x0000, + 0x0660, + 0x06C0, + 0x0700, + 0x06C0, + 0x0660, + 0x0000, + 0x01DC, + 0x01AC, + 0x01AC, + 0x018C, + 0x0000, + 0x0066, + 0x0066, + 0x007E, + 0x0066, + 0x0066, + 0x0000, + + 0x0000, + 0x07F0, + 0x06B0, + 0x06B0, + 0x0630, + 0x0000, + 0x0078, + 0x0048, + 0x00FC, + 0x00CC, + 0x00CC, + 0x0000, + 0x0066, + 0x0066, + 0x007E, + 0x0066, + 0x0066, + 0x0000, + + 0x0000, + 0x0600, + 0x0600, + 0x0600, + 0x07E0, + 0x07E0, + 0x0000, + 0x00F8, + 0x018C, + 0x01FC, + 0x018C, + 0x0000, + 0x007E, + 0x007E, + 0x0018, + 0x0018, + 0x0018, + 0x0000, + + 0x0000, + 0x0600, + 0x0600, + 0x0600, + 0x07E0, + 0x07E0, + 0x0000, + 0x01F8, + 0x039C, + 0x039C, + 0x01F8, + 0x0000, + 0x00E6, + 0x00F6, + 0x00FE, + 0x00DE, + 0x00CE, + 0x0000, + + 0x0000, + 0x03E0, + 0x0630, + 0x07F0, + 0x0630, + 0x0000, + 0x0180, + 0x0180, + 0x0180, + 0x01F8, + 0x01F8, + 0x0000, + 0x007E, + 0x007E, + 0x0018, + 0x0018, + 0x0018, + 0x0000, + + 0x0000, + 0x0660, + 0x0660, + 0x03C0, + 0x0180, + 0x0000, + 0x01F8, + 0x0180, + 0x01F0, + 0x0180, + 0x01F8, + 0x0000, + 0x0030, + 0x0030, + 0x0030, + 0x003E, + 0x003E, + 0x0000, + + 0x0000, + 0x07E0, + 0x07E0, + 0x0180, + 0x0180, + 0x0180, + 0x0000, + 0x00D8, + 0x00F8, + 0x00D8, + 0x00D8, + 0x0000, + 0x003C, + 0x0022, + 0x0026, + 0x003C, + 0x0036, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0660, + 0x07E0, + 0x05A0, + 0x0420, + 0x0000, + 0x07E0, + 0x0000, + 0x07E0, + 0x0400, + 0x07E0, + 0x0020, + 0x07E0, + 0x0000, + + 0x0000, + 0x03E0, + 0x0630, + 0x07F0, + 0x0630, + 0x0000, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + 0x007E, + 0x0046, + 0x0078, + 0x006C, + 0x0066, + 0x0000, + + 0x0000, + 0x0770, + 0x06B0, + 0x06B0, + 0x06B0, + 0x06B0, + 0x0000, + 0x00F8, + 0x018C, + 0x01FC, + 0x018C, + 0x0000, + 0x0066, + 0x0066, + 0x0066, + 0x003C, + 0x0018, + 0x0000, + + 0x0000, + 0x0200, + 0x020C, + 0x0212, + 0x0012, + 0x0712, + 0x0012, + 0x070C, + 0x0700, + 0x0700, + 0x0712, + 0x0712, + 0x0014, + 0x0218, + 0x0214, + 0x0212, + 0x0212, + 0x0200, + + 0x0000, + 0x0210, + 0x0210, + 0x0210, + 0x001E, + 0x0700, + 0x000C, + 0x0712, + 0x0712, + 0x0712, + 0x070C, + 0x0700, + 0x001E, + 0x0210, + 0x021E, + 0x0202, + 0x021E, + 0x0200, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0618, + 0x0738, + 0x07F8, + 0x06D8, + 0x06D8, + 0x0618, + 0x0618, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0630, + 0x0630, + 0x0630, + 0x0630, + 0x0360, + 0x03E0, + 0x01C0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01C0, + 0x03E0, + 0x0220, + 0x0630, + 0x07F0, + 0x0630, + 0x0630, + 0x0000, + + 0x0000, + 0x0000, + 0x0003, + 0x001F, + 0x000F, + 0x000F, + 0x000F, + 0x000F, + 0x000F, + 0x000F, + 0x000F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0800, + 0x0F00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0007, + 0x0003, + 0x0007, + 0x000F, + 0x001F, + 0x003F, + 0x003F, + 0x0007, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0E00, + 0x0F00, + 0x0F80, + 0x0E00, + 0x0E00, + 0x0C00, + 0x0C00, + 0x0800, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0001, + 0x0003, + 0x001F, + 0x007F, + 0x00FF, + 0x007F, + 0x001C, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0E60, + 0x0800, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0001, + 0x003F, + 0x00FF, + 0x007F, + 0x0078, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0700, + 0x0FF0, + 0x0FE0, + 0x0FE0, + 0x08C0, + 0x0040, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0300, + 0x0FC0, + 0x0FF8, + 0x0FF0, + 0x0FE0, + 0x0180, + 0x0100, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0010, + 0x003F, + 0x007F, + 0x001F, + 0x0001, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0880, + 0x0FE0, + 0x0FF0, + 0x0FF8, + 0x07F0, + 0x0700, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x000C, + 0x001F, + 0x003F, + 0x001F, + 0x0007, + 0x0001, + 0x0001, + 0x0003, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0C00, + 0x0FC0, + 0x0FE0, + 0x0FE0, + 0x0FF0, + 0x0FF0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x001F, + 0x000F, + 0x0007, + 0x0007, + 0x0003, + 0x0007, + 0x000F, + 0x0001, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0800, + 0x0C00, + 0x0E00, + 0x0F60, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0180, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x000F, + 0x003F, + 0x000F, + 0x0007, + 0x0003, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0F00, + 0x0FC0, + 0x0E00, + 0x0C00, + 0x0800, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0001, + 0x0003, + 0x0007, + 0x006F, + 0x003F, + 0x003F, + 0x003F, + 0x003F, + 0x0018, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0F80, + 0x0F00, + 0x0E00, + 0x0E00, + 0x0C00, + 0x0E00, + 0x0F00, + 0x0800, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0003, + 0x003F, + 0x007F, + 0x007F, + 0x00FF, + 0x00FF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0300, + 0x0F80, + 0x0FC0, + 0x0F80, + 0x0E00, + 0x0800, + 0x0800, + 0x0C00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0011, + 0x007F, + 0x00FF, + 0x01FF, + 0x00FE, + 0x000E, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0080, + 0x0FC0, + 0x0FE0, + 0x0F80, + 0x0800, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x000C, + 0x003F, + 0x01FF, + 0x00FF, + 0x007F, + 0x0018, + 0x0008, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x000E, + 0x00FF, + 0x007F, + 0x007F, + 0x0031, + 0x0020, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0800, + 0x0FC0, + 0x0FF0, + 0x0FE0, + 0x01E0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x003F, + 0x003F, + 0x003F, + 0x0067, + 0x0001, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0800, + 0x0C00, + 0x0F80, + 0x0FE0, + 0x0FF0, + 0x0FE0, + 0x0380, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0007, + 0x000F, + 0x001F, + 0x0007, + 0x0007, + 0x0003, + 0x0003, + 0x0001, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0E00, + 0x0C00, + 0x0E00, + 0x0F00, + 0x0F80, + 0x0FC0, + 0x0FC0, + 0x0E00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0180, + 0x0240, + 0x0240, + 0x0180, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x01F8, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x01F8, + 0x00F0, + 0x0060, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0100, + 0x0300, + 0x07C0, + 0x07F0, + 0x0330, + 0x0118, + 0x0018, + 0x0018, + 0x0118, + 0x0330, + 0x07F0, + 0x07C0, + 0x0300, + 0x0100, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x00F0, + 0x07FE, + 0x0606, + 0x0462, + 0x0462, + 0x0462, + 0x0462, + 0x0462, + 0x0462, + 0x0462, + 0x0402, + 0x0462, + 0x0462, + 0x0402, + 0x0606, + 0x07FE, + 0x0000, + + 0x0000, + 0x00F0, + 0x07FE, + 0x070E, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x07FE, + 0x07FE, + 0x0000, + + 0x0000, + 0x00F0, + 0x07FE, + 0x070E, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x0000, + + 0x0000, + 0x00F0, + 0x07FE, + 0x070E, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x0000, + + 0x0000, + 0x00F0, + 0x07FE, + 0x070E, + 0x0606, + 0x0606, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x0000, + + 0x0000, + 0x00F0, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x0000, + + 0x0000, + 0x0000, + 0x03FF, + 0x03FF, + 0x0300, + 0x0300, + 0x033C, + 0x033C, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x030C, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x030C, + 0x01F8, + 0x00F0, + + 0x0000, + 0x0000, + 0x0F00, + 0x0F00, + 0x033F, + 0x033F, + 0x0333, + 0x0333, + 0x0333, + 0x0333, + 0x03F3, + 0x03F3, + 0x0303, + 0x0303, + 0x0303, + 0x0303, + 0x0303, + 0x0303, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x000F, + 0x000F, + 0x03FC, + 0x03FC, + 0x030F, + 0x030F, + 0x0300, + 0x0300, + 0x0F00, + 0x0F00, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0F00, + 0x0F00, + 0x0000, + 0x0000, + 0x0F00, + 0x0F00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x00F0, + 0x00F0, + 0x01F8, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0040, + 0x0040, + 0x0E4F, + 0x0040, + 0x0040, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x038C, + 0x038C, + 0x03CC, + 0x036C, + 0x0B6D, + 0x033C, + 0x031C, + 0x030C, + 0x030C, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x0188, + 0x0180, + 0x01F0, + 0x0EFB, + 0x0018, + 0x0118, + 0x01F8, + 0x00F0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x01F8, + 0x0180, + 0x0180, + 0x01E0, + 0x0DEF, + 0x0180, + 0x0180, + 0x01F8, + 0x01F8, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x036C, + 0x0D6B, + 0x01F8, + 0x0198, + 0x0198, + 0x0198, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0E0F, + 0x0318, + 0x01B0, + 0x00E0, + 0x0040, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0070, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x007F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00E0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FE0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0300, + 0x0300, + 0x03FF, + 0x03FF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0300, + 0x0300, + 0x0F00, + 0x0F00, + 0x0000, + 0x0000, + 0x0066, + 0x0066, + 0x0066, + 0x0066, + 0x0066, + 0x007E, + 0x003C, + 0x0018, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0180, + 0x0380, + 0x0380, + 0x0180, + 0x0180, + 0x0180, + 0x03C6, + 0x03C6, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03C0, + 0x0660, + 0x0660, + 0x03E0, + 0x0060, + 0x0260, + 0x07E0, + 0x03C0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x01F8, + 0x01F8, + 0x00F0, + 0x00F0, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0004, + 0x0004, + 0x0007, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0007, + 0x0004, + 0x0004, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0E00, + 0x0200, + 0x0200, + + 0x0200, + 0x0200, + 0x0E00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0070, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0010, + 0x0018, + 0x001C, + 0x001E, + 0x001C, + 0x0018, + 0x0010, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00E0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0080, + 0x0180, + 0x0380, + 0x0780, + 0x0380, + 0x0180, + 0x0080, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x003F, + 0x003F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x003F, + 0x003F, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FC0, + 0x0FC0, + 0x00C0, + 0x00C0, + 0x00C0, + 0x00C0, + + 0x00C0, + 0x00C0, + 0x00C0, + 0x00C0, + 0x0FC0, + 0x0FC0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0660, + 0x07E0, + 0x05A0, + 0x0420, + 0x0004, + 0x03C4, + 0x0664, + 0x0660, + 0x0660, + 0x03C4, + 0x0004, + 0x07C4, + 0x0660, + 0x0660, + 0x0660, + 0x07C0, + 0x0000, + + 0x0000, + 0x0040, + 0x0040, + 0x0040, + 0x07FC, + 0x0444, + 0x0550, + 0x0446, + 0x07F4, + 0x0604, + 0x0014, + 0x0054, + 0x0054, + 0x0154, + 0x0154, + 0x0554, + 0x0556, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0660, + 0x0660, + 0x0660, + 0x03C0, + 0x0186, + 0x0004, + 0x0004, + 0x03C4, + 0x0664, + 0x0664, + 0x07E4, + 0x0664, + 0x0004, + 0x0006, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0660, + 0x0660, + 0x0660, + 0x03C0, + 0x0186, + 0x0004, + 0x0004, + 0x07C4, + 0x0664, + 0x0784, + 0x0664, + 0x07C4, + 0x0004, + 0x0006, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03C0, + 0x0600, + 0x0600, + 0x0600, + 0x03C6, + 0x0004, + 0x0004, + 0x03C4, + 0x0664, + 0x0664, + 0x07E4, + 0x0664, + 0x0004, + 0x0006, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03C0, + 0x0600, + 0x0600, + 0x0600, + 0x03C6, + 0x0004, + 0x0004, + 0x07C4, + 0x0664, + 0x0784, + 0x0664, + 0x07C4, + 0x0004, + 0x0006, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0660, + 0x07E0, + 0x05A0, + 0x0420, + 0x0004, + 0x03C4, + 0x0664, + 0x0660, + 0x07E0, + 0x0664, + 0x0004, + 0x0664, + 0x0660, + 0x0660, + 0x03C0, + 0x0180, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x070E, + 0x0606, + 0x0402, + 0x0402, + 0x0402, + 0x0402, + 0x0606, + 0x070E, + 0x03FC, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x070E, + 0x0606, + 0x04F2, + 0x04F2, + 0x04F2, + 0x04F2, + 0x0606, + 0x070E, + 0x03FC, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0003, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0003, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0C00, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0C00, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFE, + 0x0402, + 0x050A, + 0x058A, + 0x05CA, + 0x05EA, + 0x05FA, + 0x05FA, + 0x05EA, + 0x05CA, + 0x058A, + 0x050A, + 0x0402, + 0x0FFE, + 0x0000, + 0x0000, + + 0x0000, + 0x0001, + 0x0001, + 0x0030, + 0x0033, + 0x0007, + 0x000C, + 0x0018, + 0x00D8, + 0x00D8, + 0x0018, + 0x000C, + 0x0007, + 0x0033, + 0x0030, + 0x0001, + 0x0001, + 0x0000, + + 0x0000, + 0x0800, + 0x0800, + 0x00C0, + 0x0CC0, + 0x0E00, + 0x0300, + 0x0180, + 0x01B0, + 0x01B0, + 0x0180, + 0x0300, + 0x0E00, + 0x0CC0, + 0x00C0, + 0x0800, + 0x0800, + 0x0000, + + 0x0000, + 0x0000, + 0x0003, + 0x000F, + 0x001C, + 0x0030, + 0x0030, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0030, + 0x0030, + 0x001C, + 0x000F, + 0x0003, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0C00, + 0x0F00, + 0x0F80, + 0x0FC0, + 0x0FC0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FC0, + 0x0FC0, + 0x0F80, + 0x0F00, + 0x0C00, + 0x0000, + 0x0000, + + 0x0000, + 0x0006, + 0x000E, + 0x001A, + 0x0032, + 0x07E2, + 0x04C2, + 0x04C2, + 0x04C2, + 0x04C2, + 0x04C2, + 0x07E2, + 0x0032, + 0x001A, + 0x000E, + 0x0006, + 0x0000, + 0x0000, + + 0x0000, + 0x0060, + 0x0030, + 0x0010, + 0x0190, + 0x0098, + 0x04C8, + 0x0648, + 0x0248, + 0x0648, + 0x04C8, + 0x0098, + 0x0190, + 0x0010, + 0x0030, + 0x0060, + 0x0000, + 0x0000, + + 0x0000, + 0x07FE, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x0198, + 0x00F0, + 0x0060, + 0x0060, + 0x00F0, + 0x0198, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x07FE, + 0x0000, + + 0x0000, + 0x0000, + 0x0100, + 0x0180, + 0x01C0, + 0x01E0, + 0x01F0, + 0x01F8, + 0x01FC, + 0x01FE, + 0x01F0, + 0x01F0, + 0x0138, + 0x0038, + 0x001C, + 0x001C, + 0x0008, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0322, + 0x04B6, + 0x04AA, + 0x04AA, + 0x07A2, + 0x04A2, + 0x04A2, + 0x04A2, + 0x04A2, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0722, + 0x04B6, + 0x04AA, + 0x04AA, + 0x0722, + 0x0422, + 0x0422, + 0x0422, + 0x0422, + 0x0000, + + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, }; static const uint16_t font_mask12x18[] = { -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FFF, -0x0FFF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, - -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FFF, -0x0FFF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, - -0x0001, -0x0003, -0x0003, -0x0003, -0x0003, -0x0003, -0x0003, -0x0003, -0x0007, -0x001F, -0x003F, -0x007F, -0x00F8, -0x01E0, -0x01E0, -0x03C0, -0x03C1, -0x03C3, - -0x0800, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0E00, -0x0F80, -0x0FC0, -0x0FE0, -0x01F0, -0x0078, -0x0078, -0x003C, -0x083C, -0x0C3C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0FC6, -0x0FEF, -0x0FFF, -0x0FFE, -0x0FFC, -0x0FFE, -0x0FFF, -0x07FF, -0x07FF, -0x07FF, -0x03FF, -0x01FF, -0x01FE, -0x01C0, -0x03E0, -0x07F0, -0x0FFC, -0x0FFE, - -0x01F8, -0x03FC, -0x07FE, -0x0F0F, -0x0E07, -0x0407, -0x0007, -0x0007, -0x0007, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x0F0F, -0x0E07, -0x0E07, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x0000, -0x0000, -0x0208, -0x071C, -0x079E, -0x07FC, -0x07F0, -0x07F0, -0x03F8, -0x01FC, -0x00FE, -0x01FC, -0x03C0, -0x07C0, -0x0FC0, -0x0FF8, -0x0FFC, -0x0FFC, - -0x03C3, -0x03C1, -0x03C0, -0x01E0, -0x01E0, -0x00F8, -0x007F, -0x003F, -0x001F, -0x0007, -0x0003, -0x0003, -0x0003, -0x0003, -0x0003, -0x0003, -0x0003, -0x0001, - -0x0C3C, -0x083C, -0x003C, -0x0078, -0x0078, -0x01F0, -0x0FE0, -0x0FC0, -0x0F80, -0x0E00, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0800, - -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x00F0, -0x03FC, -0x03FC, -0x039C, -0x011C, -0x001C, -0x001C, -0x01FC, -0x03FC, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x0000, - -0x0000, -0x0000, -0x0000, -0x00F0, -0x03FC, -0x03FC, -0x039C, -0x039C, -0x03FC, -0x03FC, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x0000, - -0x0000, -0x01FF, -0x03FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x03FF, -0x01FF, -0x0000, -0x0000, - -0x0000, -0x0FFC, -0x0FFE, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFE, -0x0FFC, -0x0000, -0x0000, - -0x0000, -0x0FFC, -0x0FFE, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFE, -0x0FFC, -0x0000, -0x0000, - -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0001, -0x0003, -0x0007, -0x000F, -0x001F, -0x003F, -0x007F, -0x00FF, -0x01FF, -0x037F, -0x027F, -0x007F, -0x007F, -0x007F, -0x007F, -0x007F, -0x007F, -0x007F, - -0x0800, -0x0C00, -0x0E00, -0x0F00, -0x0F80, -0x0FC0, -0x0FE0, -0x0FF0, -0x0FF8, -0x0FEC, -0x0FE4, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, - -0x0000, -0x0000, -0x0060, -0x01F8, -0x03FC, -0x07FE, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0060, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0060, -0x0060, -0x00F0, -0x00F0, -0x0060, -0x0000, - -0x0000, -0x0108, -0x039C, -0x07FE, -0x03DE, -0x01CE, -0x01CE, -0x039C, -0x0108, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0198, -0x03FC, -0x03FC, -0x07FE, -0x0FFF, -0x0FFF, -0x07FE, -0x07FE, -0x0FFF, -0x0FFF, -0x07FE, -0x03FC, -0x03FC, -0x0198, -0x0000, -0x0000, - -0x0000, -0x0060, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x07F8, -0x03FC, -0x03FE, -0x07FE, -0x07FE, -0x03FC, -0x01F8, -0x00F0, -0x0060, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x07BE, -0x037C, -0x00F8, -0x01F0, -0x03EC, -0x07DE, -0x079E, -0x030C, -0x0000, - -0x0000, -0x0000, -0x0000, -0x00E0, -0x01F0, -0x03F8, -0x03B8, -0x03B8, -0x03F0, -0x01E0, -0x01E0, -0x03FC, -0x073E, -0x071C, -0x07FE, -0x03F7, -0x01E2, -0x0000, - -0x0000, -0x0020, -0x0070, -0x00F8, -0x0078, -0x0038, -0x0038, -0x0070, -0x0020, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x000C, -0x001E, -0x003C, -0x0078, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0078, -0x003C, -0x001E, -0x000C, - -0x0000, -0x0000, -0x0000, -0x0180, -0x03C0, -0x01E0, -0x00F0, -0x0078, -0x0078, -0x0078, -0x0078, -0x0078, -0x0078, -0x0078, -0x00F0, -0x01E0, -0x03C0, -0x0180, - -0x0000, -0x0000, -0x0402, -0x0E07, -0x0F0F, -0x079E, -0x03FC, -0x07FE, -0x0FFF, -0x07FE, -0x03FC, -0x079E, -0x0F0F, -0x0E07, -0x0402, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x01F8, -0x03FC, -0x03FC, -0x01F8, -0x00F0, -0x0060, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x01E0, -0x03C0, -0x0180, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x03FC, -0x01F8, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0006, -0x000F, -0x001F, -0x003E, -0x007C, -0x00F8, -0x01F0, -0x03E0, -0x07C0, -0x0F80, -0x0F00, -0x0600, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x01F0, -0x03F0, -0x03F0, -0x01F0, -0x00F0, -0x00F0, -0x01F8, -0x03FC, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x07BE, -0x037C, -0x00F8, -0x01F0, -0x03FC, -0x07FE, -0x07FE, -0x03FC, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03F8, -0x07FC, -0x07FE, -0x03FE, -0x00FE, -0x01FC, -0x01FC, -0x00FE, -0x03FE, -0x07FE, -0x07FC, -0x03F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0038, -0x007C, -0x00FC, -0x01FC, -0x03FC, -0x07FC, -0x07FE, -0x07FE, -0x03FC, -0x003C, -0x003C, -0x0018, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x07FC, -0x07FE, -0x03FE, -0x031E, -0x07FE, -0x07FE, -0x07FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FC, -0x07F8, -0x07F8, -0x07FC, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x03FE, -0x003E, -0x007C, -0x00F8, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x03FC, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x03FE, -0x01FE, -0x03FE, -0x07FE, -0x07FC, -0x03F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x0060, -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x0060, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x0060, -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x01E0, -0x00C0, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0018, -0x003C, -0x0078, -0x00F0, -0x01E0, -0x03C0, -0x03C0, -0x01E0, -0x00F0, -0x0078, -0x003C, -0x0018, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x03FC, -0x01F8, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x03FC, -0x01F8, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0300, -0x0780, -0x03C0, -0x01E0, -0x00F0, -0x0078, -0x0078, -0x00F0, -0x01E0, -0x03C0, -0x0780, -0x0300, - -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x0FFF, -0x0F0F, -0x060F, -0x001F, -0x003E, -0x007C, -0x00F8, -0x00F0, -0x0060, -0x0060, -0x00F0, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x07DE, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFE, -0x0EDC, -0x070C, -0x03FE, -0x01FC, -0x00F8, - -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x03F0, -0x07F8, -0x07FC, -0x07FE, -0x079E, -0x079E, -0x07FE, -0x07FC, -0x07FC, -0x07FE, -0x079E, -0x079E, -0x07FE, -0x07FC, -0x07F8, -0x03F0, - -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x079E, -0x078C, -0x0780, -0x0780, -0x0780, -0x0780, -0x078C, -0x079E, -0x07FE, -0x03FC, -0x01F8, -0x00F0, - -0x0000, -0x0000, -0x03F0, -0x07F8, -0x07FC, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FC, -0x07F8, -0x03F0, - -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x0780, -0x0780, -0x07F0, -0x07F8, -0x07F8, -0x07F0, -0x0780, -0x0780, -0x07FC, -0x07FE, -0x07FE, -0x03FC, - -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x0780, -0x0780, -0x07F0, -0x07F8, -0x07F8, -0x07F0, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0300, - -0x0000, -0x0000, -0x01F0, -0x03F8, -0x07FC, -0x07FE, -0x079E, -0x079E, -0x078C, -0x07BC, -0x07FE, -0x07FE, -0x07BE, -0x079E, -0x07FE, -0x07FE, -0x03FE, -0x01FC, - -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x01F8, -0x03FC, -0x03FC, -0x01F8, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x01F8, -0x03FC, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x00FC, -0x01FE, -0x01FE, -0x00FC, -0x0078, -0x0078, -0x0078, -0x0378, -0x07F8, -0x07F8, -0x07F8, -0x07F8, -0x07F8, -0x07F8, -0x03F0, -0x01E0, - -0x0000, -0x0000, -0x0300, -0x078C, -0x079E, -0x07BE, -0x07FC, -0x07F8, -0x07F0, -0x07E0, -0x07C0, -0x07E0, -0x07F0, -0x07F8, -0x07FC, -0x07BE, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0300, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x07FC, -0x07FE, -0x07FE, -0x03FC, - -0x0000, -0x0000, -0x030C, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x07DE, -0x07DE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07BE, -0x07BE, -0x079E, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x03FC, -0x01F8, -0x00F0, - -0x0000, -0x0000, -0x03F0, -0x07F8, -0x07FC, -0x07FE, -0x079E, -0x079E, -0x07FE, -0x07FC, -0x07F8, -0x07F0, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0300, - -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FC, -0x03FE, -0x01FE, -0x00EC, - -0x0000, -0x0000, -0x03F0, -0x07F8, -0x07FC, -0x07FE, -0x079E, -0x079E, -0x07FE, -0x07FC, -0x07F8, -0x07F0, -0x07F8, -0x07FC, -0x07BE, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x00F8, -0x01FC, -0x03FE, -0x07FE, -0x078C, -0x0780, -0x07F0, -0x03F8, -0x01FC, -0x00FE, -0x001E, -0x031E, -0x07FE, -0x07FC, -0x03F8, -0x01F0, - -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x03FC, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x03FC, -0x01F8, -0x00F0, - -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x03FC, -0x03FC, -0x01F8, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x03FC, -0x0198, - -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x03FC, -0x01F8, -0x00F0, -0x00F0, -0x01F8, -0x03FC, -0x079E, -0x079E, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x03FC, -0x01F8, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x03FE, -0x001E, -0x003E, -0x007C, -0x00F8, -0x01F0, -0x03E0, -0x07C0, -0x0780, -0x07FC, -0x07FE, -0x07FE, -0x03FC, - -0x0000, -0x0000, -0x0078, -0x00FC, -0x00FC, -0x00F8, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F8, -0x00FC, -0x00FC, -0x0078, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0600, -0x0F00, -0x0F80, -0x07C0, -0x03E0, -0x01F0, -0x00F8, -0x007C, -0x003E, -0x001F, -0x000F, -0x0006, -0x0000, -0x0000, - -0x0000, -0x0000, -0x01E0, -0x03F0, -0x03F0, -0x01F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x01F0, -0x03F0, -0x03F0, -0x01E0, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x01F8, -0x03FC, -0x039C, -0x030C, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x07FE, -0x0FFF, -0x0FFF, -0x07FE, -0x0000, - -0x0000, -0x0300, -0x0780, -0x03C0, -0x01E0, -0x00F0, -0x0060, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03F8, -0x07FC, -0x07FE, -0x07FE, -0x07FE, -0x07FC, -0x07FC, -0x07FE, -0x07FE, -0x07FE, -0x07FC, -0x03F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x078C, -0x0780, -0x0780, -0x078C, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03F0, -0x07F8, -0x07FC, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FC, -0x07F8, -0x03F0, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x07F0, -0x07F8, -0x07F8, -0x07F0, -0x07FC, -0x07FE, -0x07FE, -0x03FC, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x07F8, -0x07FC, -0x07FC, -0x07F8, -0x0780, -0x0780, -0x0780, -0x0300, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x00F0, -0x01F8, -0x01F8, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x01F8, -0x01F8, -0x00F0, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x007C, -0x00FE, -0x00FE, -0x007C, -0x003C, -0x003C, -0x003C, -0x01BC, -0x03FC, -0x03FC, -0x01F8, -0x00F0, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x07BE, -0x07FC, -0x07F8, -0x07F0, -0x07F0, -0x07F8, -0x07FC, -0x07BE, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0300, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x07FC, -0x07FE, -0x07FE, -0x03FC, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x07DE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07BE, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03F8, -0x07FC, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FC, -0x07F8, -0x0780, -0x0780, -0x0780, -0x0300, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x07BE, -0x07FE, -0x07FC, -0x07FE, -0x03FE, -0x01EC, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03F8, -0x07FC, -0x07FE, -0x07FE, -0x07FE, -0x07FC, -0x07F8, -0x07F8, -0x07FC, -0x07BE, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x07FC, -0x03FE, -0x03FE, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x03FC, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x03FC, -0x01F8, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x03FC, -0x0198, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x07FE, -0x03FC, -0x01F8, -0x00F0, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x07FE, -0x03FC, -0x01F8, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x03FE, -0x007C, -0x00F8, -0x01F0, -0x03E0, -0x07FC, -0x07FE, -0x07FE, -0x03FC, - -0x0000, -0x0038, -0x007C, -0x007C, -0x00FC, -0x00F8, -0x00F0, -0x00F0, -0x01E0, -0x01E0, -0x00F0, -0x00F0, -0x00F8, -0x00FC, -0x007C, -0x007C, -0x0038, -0x0000, - -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0060, -0x0000, -0x0000, - -0x0000, -0x01C0, -0x03E0, -0x03F0, -0x03F0, -0x01F0, -0x00F0, -0x00F0, -0x0078, -0x0078, -0x00F0, -0x00F0, -0x01F0, -0x03F0, -0x03F0, -0x03E0, -0x01C0, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x00C2, -0x01E7, -0x03FF, -0x07FE, -0x0F3C, -0x0618, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0780, -0x07C0, -0x07E0, -0x07F0, -0x07F8, -0x07FC, -0x07F8, -0x07F0, -0x07E0, -0x07C0, -0x0780, -0x0700, -0x0700, -0x0700, -0x0F80, -0x0F80, -0x0F80, - -0x07E0, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF0, -0x07FE, -0x0FFF, -0x0FFE, -0x0FFC, -0x0FFC, -0x07FE, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x007E, - -0x0660, -0x0FF0, -0x0FE0, -0x0FC0, -0x0FE0, -0x0FF0, -0x07FC, -0x03FE, -0x03FE, -0x03FE, -0x03FE, -0x01FE, -0x00E7, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x0066, - -0x07F0, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x07F8, -0x00FC, -0x00FC, -0x01FE, -0x01FE, -0x01FE, -0x00FE, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x0066, - -0x0600, -0x0F00, -0x0F00, -0x0FE0, -0x0FF0, -0x0FF0, -0x07F8, -0x01FC, -0x03FE, -0x03FE, -0x03FE, -0x01FF, -0x00FF, -0x00FF, -0x007E, -0x003C, -0x003C, -0x0018, - -0x0600, -0x0F00, -0x0F00, -0x0FE0, -0x0FF0, -0x0FF0, -0x07F8, -0x03FC, -0x07FE, -0x07FE, -0x03FC, -0x01FE, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x00FE, - -0x07F0, -0x0FF0, -0x0FF8, -0x0FF8, -0x0FF8, -0x07F0, -0x03C0, -0x03C0, -0x03F8, -0x03FC, -0x03FC, -0x01FE, -0x00FF, -0x00FF, -0x007E, -0x003C, -0x003C, -0x0018, - -0x0660, -0x0FF0, -0x0FF0, -0x07E0, -0x03C0, -0x01F8, -0x03FC, -0x03F8, -0x03F0, -0x03F8, -0x03FC, -0x01F8, -0x0078, -0x0078, -0x007E, -0x007F, -0x007F, -0x003E, - -0x07E0, -0x0FF0, -0x0FF0, -0x07E0, -0x03C0, -0x03C0, -0x01D8, -0x01FC, -0x01FC, -0x01FC, -0x01FC, -0x00FC, -0x007E, -0x007F, -0x007F, -0x007F, -0x007F, -0x0037, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0660, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF0, -0x07E0, -0x0FF0, -0x07E0, -0x0FF0, -0x0FE0, -0x0FF0, -0x07F0, -0x0FF0, -0x07E0, - -0x03E0, -0x07F0, -0x0FF8, -0x0FF8, -0x0FF8, -0x06F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00FE, -0x00FF, -0x00FF, -0x00FE, -0x00FE, -0x00FF, -0x0066, - -0x0770, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x01FC, -0x03FE, -0x03FE, -0x03FE, -0x03FE, -0x00FF, -0x00FF, -0x00FF, -0x007E, -0x003C, -0x0018, - -0x023F, -0x073F, -0x073F, -0x073F, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x073F, -0x073F, -0x073F, -0x073F, -0x073F, -0x073F, - -0x023F, -0x073F, -0x073F, -0x073F, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x073F, -0x073F, -0x073F, -0x073F, -0x073F, -0x073F, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0618, -0x0F3C, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0F3C, -0x0618, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0630, -0x0F78, -0x0F78, -0x0F78, -0x0F78, -0x07F0, -0x07F0, -0x03E0, -0x01C0, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01C0, -0x03E0, -0x07F0, -0x07F0, -0x0FF8, -0x0FF8, -0x0FF8, -0x0F78, -0x0630, - -0x0001, -0x0007, -0x001F, -0x007F, -0x01FF, -0x03FF, -0x03FF, -0x01FF, -0x003F, -0x003F, -0x003F, -0x003F, -0x003F, -0x003F, -0x001F, -0x0000, -0x0000, -0x0000, - -0x0C00, -0x0F80, -0x0FE0, -0x0FF8, -0x0FFC, -0x0FFC, -0x0FF8, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0001, -0x001F, -0x00FF, -0x00FF, -0x00FF, -0x003F, -0x007F, -0x00FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x001F, -0x0003, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0F80, -0x0FC0, -0x0FE0, -0x0FF0, -0x0FF8, -0x0FFC, -0x0FFC, -0x0FFC, -0x0F0C, -0x0F00, -0x0E00, -0x0E00, -0x0C00, -0x0800, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x001F, -0x001F, -0x001F, -0x007F, -0x01FF, -0x03FF, -0x03FF, -0x03FF, -0x01FF, -0x00FF, -0x007F, -0x003E, -0x0018, -0x0000, -0x0000, -0x0000, - -0x0000, -0x00E0, -0x0FE0, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0E78, -0x0818, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0003, -0x0007, -0x0007, -0x00FF, -0x03FF, -0x03FF, -0x03FF, -0x01FF, -0x01FF, -0x01FF, -0x00FE, -0x00F0, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0FC0, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FF8, -0x0FF8, -0x0FF0, -0x0FF0, -0x0FF0, -0x01E0, -0x00E0, -0x0040, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x00FF, -0x01FF, -0x01FF, -0x01FF, -0x03FF, -0x03FF, -0x03FF, -0x03FF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0400, -0x0780, -0x0FE0, -0x0FFC, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFE, -0x0FFC, -0x0FF0, -0x07E0, -0x07C0, -0x0700, -0x0600, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x003C, -0x007F, -0x01FF, -0x03FF, -0x03FF, -0x03FF, -0x03FF, -0x007F, -0x0007, -0x0007, -0x000F, -0x000F, -0x000E, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0180, -0x0FE0, -0x0FF0, -0x0FF8, -0x0FFC, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFC, -0x0FE0, -0x0E00, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x001E, -0x003F, -0x00FF, -0x01FF, -0x01FF, -0x01FF, -0x007F, -0x003F, -0x001F, -0x007F, -0x00FF, -0x00FF, -0x007F, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0800, -0x0E60, -0x0FF0, -0x0FF0, -0x0FF8, -0x0FF8, -0x0FFC, -0x0FFC, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFC, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0003, -0x003F, -0x007F, -0x007F, -0x003F, -0x003F, -0x003F, -0x01FF, -0x03FF, -0x03FF, -0x01FF, -0x003F, -0x0007, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0C00, -0x0C00, -0x0E00, -0x0F18, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FE0, -0x0FE0, -0x0FE0, -0x01E0, -0x0000, -0x0000, - -0x0000, -0x000F, -0x000F, -0x001F, -0x001F, -0x001F, -0x03FF, -0x03FF, -0x03FF, -0x01FF, -0x00FF, -0x007F, -0x003F, -0x001F, -0x0007, -0x0003, -0x0001, -0x0000, - -0x0000, -0x0F00, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FF8, -0x0FF0, -0x0FE0, -0x0FC0, -0x0F80, -0x0F00, -0x0E00, -0x0C00, -0x0000, - -0x0000, -0x0003, -0x0003, -0x0007, -0x018F, -0x01FF, -0x01FF, -0x01FF, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x007F, -0x007F, -0x007F, -0x0078, -0x0000, -0x0000, - -0x0000, -0x0C00, -0x0FC0, -0x0FE0, -0x0FE0, -0x0FC0, -0x0FC0, -0x0FC0, -0x0FF8, -0x0FFC, -0x0FFC, -0x0FF8, -0x0FC0, -0x0E00, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0001, -0x0067, -0x00FF, -0x00FF, -0x01FF, -0x01FF, -0x03FF, -0x03FF, -0x07FF, -0x07FF, -0x07FF, -0x03FF, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0780, -0x0FC0, -0x0FF0, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FE0, -0x0FC0, -0x0F80, -0x0FE0, -0x0FF0, -0x0FF0, -0x0FE0, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0018, -0x007F, -0x00FF, -0x01FF, -0x03FF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x03FF, -0x007F, -0x0007, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x03C0, -0x0FE0, -0x0FF8, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FE0, -0x0E00, -0x0E00, -0x0F00, -0x0F00, -0x0700, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0002, -0x001E, -0x007F, -0x03FF, -0x0FFF, -0x0FFF, -0x0FFF, -0x07FF, -0x03FF, -0x00FF, -0x007E, -0x003E, -0x000E, -0x0006, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0FF0, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x003F, -0x03FF, -0x03FF, -0x03FF, -0x01FF, -0x01FF, -0x00FF, -0x00FF, -0x00FF, -0x0078, -0x0070, -0x0020, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0C00, -0x0E00, -0x0E00, -0x0FF0, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FF8, -0x0FF8, -0x0FF8, -0x07F0, -0x00F0, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0070, -0x007F, -0x00FF, -0x00FF, -0x00FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01E7, -0x0181, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0F80, -0x0F80, -0x0F80, -0x0FE0, -0x0FF8, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FF8, -0x0FF0, -0x0FE0, -0x07C0, -0x0180, -0x0000, -0x0000, -0x0000, - -0x0000, -0x001F, -0x003F, -0x007F, -0x00FF, -0x01FF, -0x03FF, -0x03FF, -0x03FF, -0x030F, -0x000F, -0x0007, -0x0007, -0x0003, -0x0001, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0800, -0x0F80, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FC0, -0x0FE0, -0x0FF0, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0F80, -0x0C00, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0180, -0x03C0, -0x07E0, -0x07E0, -0x03C0, -0x0180, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0060, -0x00F0, -0x01F8, -0x03FC, -0x01F8, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x01F8, -0x03FC, -0x01F8, -0x00F0, -0x0060, -0x0000, - -0x0000, -0x0100, -0x0380, -0x07C0, -0x0FF0, -0x0FF8, -0x07F8, -0x03BC, -0x013C, -0x013C, -0x033C, -0x07F8, -0x0FF8, -0x0FF0, -0x07C0, -0x0380, -0x0100, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x01F8, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x01F8, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x01F8, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x01F8, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x01F8, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x01F8, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x0000, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x0F0F, -0x0F0F, -0x0F0F, -0x0F0F, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0F80, -0x0F80, -0x0FFF, -0x0FFF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x0787, -0x0787, -0x0787, -0x0787, -0x0787, - -0x0000, -0x0000, -0x0000, -0x0800, -0x0800, -0x0800, -0x0800, -0x081F, -0x081F, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0F9F, -0x0F9F, -0x0F80, -0x0F80, -0x0F80, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x01F8, -0x01F8, -0x03FC, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0040, -0x00E0, -0x0EEF, -0x0FFF, -0x0EEF, -0x00E0, -0x0040, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x07DE, -0x07FE, -0x07FE, -0x0FFF, -0x0FFF, -0x0FFF, -0x07FE, -0x07BE, -0x079E, -0x030C, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x03FC, -0x03C0, -0x0FFF, -0x0FFF, -0x0FFF, -0x03FC, -0x03FC, -0x03FC, -0x00F8, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x03FC, -0x03F8, -0x03C0, -0x0FFF, -0x0FFF, -0x0FFF, -0x03F0, -0x03FC, -0x03FC, -0x01F8, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x07FE, -0x0FFF, -0x0FFF, -0x0FFF, -0x03FC, -0x03FC, -0x03FC, -0x01F8, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0000, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0E0F, -0x0F1F, -0x0FBF, -0x03F8, -0x01F0, -0x00E0, -0x0040, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0070, -0x00F8, -0x0070, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x007F, -0x00FF, -0x007F, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x00E0, -0x01F0, -0x00E0, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FE0, -0x0FF0, -0x0FE0, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0780, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x00F0, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0787, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0066, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x007E, -0x003C, -0x0018, -0x0000, -0x0000, -0x0000, - -0x0F80, -0x0000, -0x0000, -0x0000, -0x0000, -0x0180, -0x03C0, -0x07C0, -0x07C0, -0x03C0, -0x03C0, -0x03C6, -0x07EF, -0x07EF, -0x03C6, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03C0, -0x07E0, -0x0FF0, -0x0FF0, -0x07E0, -0x03F0, -0x0FF0, -0x0FF0, -0x07E0, -0x03C0, -0x0000, -0x0000, -0x0000, - -0x01F8, -0x03FC, -0x03FC, -0x01F8, -0x01F8, -0x00F0, -0x00F0, -0x0060, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, - -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x000E, -0x000F, -0x000F, -0x000F, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x000F, -0x000F, -0x000F, -0x000E, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0F00, -0x0F00, -0x0F00, -0x0700, - -0x0700, -0x0F00, -0x0F00, -0x0F00, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0070, -0x00F8, -0x0070, -0x0000, -0x0000, -0x0000, -0x0010, -0x0038, -0x003C, -0x003E, -0x003F, -0x003E, -0x003C, -0x0038, -0x0010, -0x0000, - -0x0000, -0x0000, -0x00E0, -0x01F0, -0x00E0, -0x0000, -0x0000, -0x0000, -0x0080, -0x01C0, -0x03C0, -0x07C0, -0x0FC0, -0x07C0, -0x03C0, -0x01C0, -0x0080, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0078, -0x0078, -0x0078, -0x007F, -0x007F, -0x007F, -0x007F, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x007F, -0x007F, -0x007F, -0x007F, -0x0078, -0x0078, -0x0078, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x01E0, -0x01E0, -0x01E0, - -0x01E0, -0x01E0, -0x01E0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0660, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF4, -0x07EE, -0x07EE, -0x0FFE, -0x0FF4, -0x0FF4, -0x07EE, -0x07EE, -0x0FEE, -0x0FF4, -0x0FF0, -0x0FF0, -0x0FE0, -0x07C0, - -0x0040, -0x00E0, -0x00E0, -0x07FC, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFF, -0x0FFE, -0x0FFE, -0x07FE, -0x00FE, -0x01FE, -0x03FE, -0x07FE, -0x0FFE, -0x0FFF, -0x0556, - -0x0000, -0x0000, -0x0660, -0x0FF0, -0x0FF0, -0x0FF0, -0x07E6, -0x03CF, -0x018E, -0x03CE, -0x07EE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x066E, -0x000F, -0x0006, - -0x0000, -0x0000, -0x0660, -0x0FF0, -0x0FF0, -0x0FF0, -0x07E6, -0x03CF, -0x018E, -0x07CE, -0x0FEE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FEE, -0x07CE, -0x000F, -0x0006, - -0x0000, -0x0000, -0x03C0, -0x07E0, -0x0FC0, -0x0F00, -0x0FC6, -0x07EF, -0x03CE, -0x03CE, -0x07EE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x07EE, -0x000F, -0x0006, - -0x0000, -0x0000, -0x03C0, -0x07E0, -0x0FC0, -0x0F00, -0x0FC6, -0x07EF, -0x03CE, -0x07CE, -0x0FEE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FEE, -0x07CE, -0x000F, -0x0006, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0660, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF4, -0x07FE, -0x07FE, -0x0FFE, -0x0FF4, -0x0FF4, -0x0FFE, -0x067E, -0x0FFE, -0x0FF4, -0x0FF0, -0x07E0, -0x03C0, -0x0180, - -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x07FE, -0x03FC, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x07FE, -0x03FC, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0003, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0003, -0x0000, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0F00, -0x0F00, -0x0F00, -0x0F00, -0x0F00, -0x0F00, -0x0F00, -0x0F00, -0x0F00, -0x0F00, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, - -0x0000, -0x0C00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0C00, -0x0000, - -0x0000, -0x0FFE, -0x0FFF, -0x0FFF, -0x0F9F, -0x0FDF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FDF, -0x0F9F, -0x0FFF, -0x0FFF, -0x0FFE, -0x0000, - -0x0001, -0x0003, -0x0033, -0x007B, -0x007F, -0x003F, -0x001F, -0x00FC, -0x01FC, -0x01FC, -0x00FC, -0x001F, -0x003F, -0x007F, -0x007B, -0x0033, -0x0003, -0x0003, - -0x0800, -0x0C00, -0x0CC0, -0x0DE0, -0x0FE0, -0x0FC0, -0x0F80, -0x03F0, -0x03F8, -0x03F8, -0x03F0, -0x0F80, -0x0FC0, -0x0FE0, -0x0DE0, -0x0CC0, -0x0C00, -0x0800, - -0x0000, -0x0003, -0x000F, -0x001F, -0x003F, -0x007D, -0x0079, -0x00F1, -0x00F1, -0x00F1, -0x00F1, -0x0079, -0x007D, -0x003F, -0x001F, -0x000F, -0x0003, -0x0000, - -0x0000, -0x0C00, -0x0F00, -0x0F80, -0x0FC0, -0x0FE0, -0x0FE0, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FE0, -0x0FE0, -0x0FC0, -0x0F80, -0x0F00, -0x0C00, -0x0000, - -0x0006, -0x000F, -0x001F, -0x003F, -0x07FF, -0x0FF7, -0x0FE7, -0x0FE7, -0x0FE7, -0x0FE7, -0x0FE7, -0x0FF7, -0x07FF, -0x003F, -0x001F, -0x000F, -0x0006, -0x0000, - -0x0020, -0x0070, -0x0038, -0x01B8, -0x03F8, -0x05FC, -0x0FFC, -0x0FFC, -0x07FC, -0x0FFC, -0x0FFC, -0x05FC, -0x03F8, -0x01B8, -0x0038, -0x0070, -0x0020, -0x0000, - -0x07FE, -0x0FFF, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x03FC, -0x01F8, -0x00F0, -0x00F0, -0x01F8, -0x03FC, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x0FFF, -0x07FE, - -0x0000, -0x0300, -0x0380, -0x03C0, -0x03E0, -0x03F0, -0x03F8, -0x03FC, -0x03FE, -0x03FF, -0x03FF, -0x03FC, -0x03FC, -0x03FE, -0x003E, -0x003E, -0x003E, -0x0008, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0322, -0x07F7, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FF7, -0x0FF7, -0x0FF7, -0x0FF7, -0x04A2, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0722, -0x0FF7, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0F77, -0x0E77, -0x0E77, -0x0E77, -0x0422, - -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FFF, + 0x0FFF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FFF, + 0x0FFF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + + 0x0001, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0007, + 0x001F, + 0x003F, + 0x007F, + 0x00F8, + 0x01E0, + 0x01E0, + 0x03C0, + 0x03C1, + 0x03C3, + + 0x0800, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0E00, + 0x0F80, + 0x0FC0, + 0x0FE0, + 0x01F0, + 0x0078, + 0x0078, + 0x003C, + 0x083C, + 0x0C3C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0FC6, + 0x0FEF, + 0x0FFF, + 0x0FFE, + 0x0FFC, + 0x0FFE, + 0x0FFF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x03FF, + 0x01FF, + 0x01FE, + 0x01C0, + 0x03E0, + 0x07F0, + 0x0FFC, + 0x0FFE, + + 0x01F8, + 0x03FC, + 0x07FE, + 0x0F0F, + 0x0E07, + 0x0407, + 0x0007, + 0x0007, + 0x0007, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x0F0F, + 0x0E07, + 0x0E07, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x0000, + 0x0000, + 0x0208, + 0x071C, + 0x079E, + 0x07FC, + 0x07F0, + 0x07F0, + 0x03F8, + 0x01FC, + 0x00FE, + 0x01FC, + 0x03C0, + 0x07C0, + 0x0FC0, + 0x0FF8, + 0x0FFC, + 0x0FFC, + + 0x03C3, + 0x03C1, + 0x03C0, + 0x01E0, + 0x01E0, + 0x00F8, + 0x007F, + 0x003F, + 0x001F, + 0x0007, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0001, + + 0x0C3C, + 0x083C, + 0x003C, + 0x0078, + 0x0078, + 0x01F0, + 0x0FE0, + 0x0FC0, + 0x0F80, + 0x0E00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0800, + + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x00F0, + 0x03FC, + 0x03FC, + 0x039C, + 0x011C, + 0x001C, + 0x001C, + 0x01FC, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x03FC, + 0x03FC, + 0x039C, + 0x039C, + 0x03FC, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x0000, + + 0x0000, + 0x01FF, + 0x03FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x03FF, + 0x01FF, + 0x0000, + 0x0000, + + 0x0000, + 0x0FFC, + 0x0FFE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFE, + 0x0FFC, + 0x0000, + 0x0000, + + 0x0000, + 0x0FFC, + 0x0FFE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFE, + 0x0FFC, + 0x0000, + 0x0000, + + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0001, + 0x0003, + 0x0007, + 0x000F, + 0x001F, + 0x003F, + 0x007F, + 0x00FF, + 0x01FF, + 0x037F, + 0x027F, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + + 0x0800, + 0x0C00, + 0x0E00, + 0x0F00, + 0x0F80, + 0x0FC0, + 0x0FE0, + 0x0FF0, + 0x0FF8, + 0x0FEC, + 0x0FE4, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + + 0x0000, + 0x0000, + 0x0060, + 0x01F8, + 0x03FC, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0060, + 0x0060, + 0x00F0, + 0x00F0, + 0x0060, + 0x0000, + + 0x0000, + 0x0108, + 0x039C, + 0x07FE, + 0x03DE, + 0x01CE, + 0x01CE, + 0x039C, + 0x0108, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0198, + 0x03FC, + 0x03FC, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x07FE, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x07FE, + 0x03FC, + 0x03FC, + 0x0198, + 0x0000, + 0x0000, + + 0x0000, + 0x0060, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07F8, + 0x03FC, + 0x03FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x07BE, + 0x037C, + 0x00F8, + 0x01F0, + 0x03EC, + 0x07DE, + 0x079E, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00E0, + 0x01F0, + 0x03F8, + 0x03B8, + 0x03B8, + 0x03F0, + 0x01E0, + 0x01E0, + 0x03FC, + 0x073E, + 0x071C, + 0x07FE, + 0x03F7, + 0x01E2, + 0x0000, + + 0x0000, + 0x0020, + 0x0070, + 0x00F8, + 0x0078, + 0x0038, + 0x0038, + 0x0070, + 0x0020, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x000C, + 0x001E, + 0x003C, + 0x0078, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0078, + 0x003C, + 0x001E, + 0x000C, + + 0x0000, + 0x0000, + 0x0000, + 0x0180, + 0x03C0, + 0x01E0, + 0x00F0, + 0x0078, + 0x0078, + 0x0078, + 0x0078, + 0x0078, + 0x0078, + 0x0078, + 0x00F0, + 0x01E0, + 0x03C0, + 0x0180, + + 0x0000, + 0x0000, + 0x0402, + 0x0E07, + 0x0F0F, + 0x079E, + 0x03FC, + 0x07FE, + 0x0FFF, + 0x07FE, + 0x03FC, + 0x079E, + 0x0F0F, + 0x0E07, + 0x0402, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + 0x00F0, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x01E0, + 0x03C0, + 0x0180, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0006, + 0x000F, + 0x001F, + 0x003E, + 0x007C, + 0x00F8, + 0x01F0, + 0x03E0, + 0x07C0, + 0x0F80, + 0x0F00, + 0x0600, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x01F0, + 0x03F0, + 0x03F0, + 0x01F0, + 0x00F0, + 0x00F0, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07BE, + 0x037C, + 0x00F8, + 0x01F0, + 0x03FC, + 0x07FE, + 0x07FE, + 0x03FC, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x07FC, + 0x07FE, + 0x03FE, + 0x00FE, + 0x01FC, + 0x01FC, + 0x00FE, + 0x03FE, + 0x07FE, + 0x07FC, + 0x03F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0038, + 0x007C, + 0x00FC, + 0x01FC, + 0x03FC, + 0x07FC, + 0x07FE, + 0x07FE, + 0x03FC, + 0x003C, + 0x003C, + 0x0018, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07FC, + 0x07FE, + 0x03FE, + 0x031E, + 0x07FE, + 0x07FE, + 0x07FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FC, + 0x07F8, + 0x07F8, + 0x07FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x03FE, + 0x003E, + 0x007C, + 0x00F8, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FE, + 0x01FE, + 0x03FE, + 0x07FE, + 0x07FC, + 0x03F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x0060, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x0060, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x01E0, + 0x00C0, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0018, + 0x003C, + 0x0078, + 0x00F0, + 0x01E0, + 0x03C0, + 0x03C0, + 0x01E0, + 0x00F0, + 0x0078, + 0x003C, + 0x0018, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0300, + 0x0780, + 0x03C0, + 0x01E0, + 0x00F0, + 0x0078, + 0x0078, + 0x00F0, + 0x01E0, + 0x03C0, + 0x0780, + 0x0300, + + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x0FFF, + 0x0F0F, + 0x060F, + 0x001F, + 0x003E, + 0x007C, + 0x00F8, + 0x00F0, + 0x0060, + 0x0060, + 0x00F0, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07DE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFE, + 0x0EDC, + 0x070C, + 0x03FE, + 0x01FC, + 0x00F8, + + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x03F0, + 0x07F8, + 0x07FC, + 0x07FE, + 0x079E, + 0x079E, + 0x07FE, + 0x07FC, + 0x07FC, + 0x07FE, + 0x079E, + 0x079E, + 0x07FE, + 0x07FC, + 0x07F8, + 0x03F0, + + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x079E, + 0x078C, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x078C, + 0x079E, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + + 0x0000, + 0x0000, + 0x03F0, + 0x07F8, + 0x07FC, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FC, + 0x07F8, + 0x03F0, + + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x0780, + 0x0780, + 0x07F0, + 0x07F8, + 0x07F8, + 0x07F0, + 0x0780, + 0x0780, + 0x07FC, + 0x07FE, + 0x07FE, + 0x03FC, + + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x0780, + 0x0780, + 0x07F0, + 0x07F8, + 0x07F8, + 0x07F0, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0300, + + 0x0000, + 0x0000, + 0x01F0, + 0x03F8, + 0x07FC, + 0x07FE, + 0x079E, + 0x079E, + 0x078C, + 0x07BC, + 0x07FE, + 0x07FE, + 0x07BE, + 0x079E, + 0x07FE, + 0x07FE, + 0x03FE, + 0x01FC, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x00FC, + 0x01FE, + 0x01FE, + 0x00FC, + 0x0078, + 0x0078, + 0x0078, + 0x0378, + 0x07F8, + 0x07F8, + 0x07F8, + 0x07F8, + 0x07F8, + 0x07F8, + 0x03F0, + 0x01E0, + + 0x0000, + 0x0000, + 0x0300, + 0x078C, + 0x079E, + 0x07BE, + 0x07FC, + 0x07F8, + 0x07F0, + 0x07E0, + 0x07C0, + 0x07E0, + 0x07F0, + 0x07F8, + 0x07FC, + 0x07BE, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0300, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x07FC, + 0x07FE, + 0x07FE, + 0x03FC, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x07DE, + 0x07DE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07BE, + 0x07BE, + 0x079E, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + + 0x0000, + 0x0000, + 0x03F0, + 0x07F8, + 0x07FC, + 0x07FE, + 0x079E, + 0x079E, + 0x07FE, + 0x07FC, + 0x07F8, + 0x07F0, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0300, + + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FC, + 0x03FE, + 0x01FE, + 0x00EC, + + 0x0000, + 0x0000, + 0x03F0, + 0x07F8, + 0x07FC, + 0x07FE, + 0x079E, + 0x079E, + 0x07FE, + 0x07FC, + 0x07F8, + 0x07F0, + 0x07F8, + 0x07FC, + 0x07BE, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x00F8, + 0x01FC, + 0x03FE, + 0x07FE, + 0x078C, + 0x0780, + 0x07F0, + 0x03F8, + 0x01FC, + 0x00FE, + 0x001E, + 0x031E, + 0x07FE, + 0x07FC, + 0x03F8, + 0x01F0, + + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x03FC, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x03FC, + 0x03FC, + 0x01F8, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x03FC, + 0x0198, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x03FC, + 0x01F8, + 0x00F0, + 0x00F0, + 0x01F8, + 0x03FC, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x03FE, + 0x001E, + 0x003E, + 0x007C, + 0x00F8, + 0x01F0, + 0x03E0, + 0x07C0, + 0x0780, + 0x07FC, + 0x07FE, + 0x07FE, + 0x03FC, + + 0x0000, + 0x0000, + 0x0078, + 0x00FC, + 0x00FC, + 0x00F8, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F8, + 0x00FC, + 0x00FC, + 0x0078, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0600, + 0x0F00, + 0x0F80, + 0x07C0, + 0x03E0, + 0x01F0, + 0x00F8, + 0x007C, + 0x003E, + 0x001F, + 0x000F, + 0x0006, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x01E0, + 0x03F0, + 0x03F0, + 0x01F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x01F0, + 0x03F0, + 0x03F0, + 0x01E0, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x01F8, + 0x03FC, + 0x039C, + 0x030C, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x07FE, + 0x0000, + + 0x0000, + 0x0300, + 0x0780, + 0x03C0, + 0x01E0, + 0x00F0, + 0x0060, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x07FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FC, + 0x03F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x078C, + 0x0780, + 0x0780, + 0x078C, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F0, + 0x07F8, + 0x07FC, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FC, + 0x07F8, + 0x03F0, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07F0, + 0x07F8, + 0x07F8, + 0x07F0, + 0x07FC, + 0x07FE, + 0x07FE, + 0x03FC, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07F8, + 0x07FC, + 0x07FC, + 0x07F8, + 0x0780, + 0x0780, + 0x0780, + 0x0300, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x01F8, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x01F8, + 0x01F8, + 0x00F0, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x007C, + 0x00FE, + 0x00FE, + 0x007C, + 0x003C, + 0x003C, + 0x003C, + 0x01BC, + 0x03FC, + 0x03FC, + 0x01F8, + 0x00F0, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x07BE, + 0x07FC, + 0x07F8, + 0x07F0, + 0x07F0, + 0x07F8, + 0x07FC, + 0x07BE, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0300, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x07FC, + 0x07FE, + 0x07FE, + 0x03FC, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x07DE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07BE, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x07FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07F8, + 0x0780, + 0x0780, + 0x0780, + 0x0300, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x07BE, + 0x07FE, + 0x07FC, + 0x07FE, + 0x03FE, + 0x01EC, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x07FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07F8, + 0x07F8, + 0x07FC, + 0x07BE, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07FC, + 0x03FE, + 0x03FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x03FC, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x03FC, + 0x0198, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x03FE, + 0x007C, + 0x00F8, + 0x01F0, + 0x03E0, + 0x07FC, + 0x07FE, + 0x07FE, + 0x03FC, + + 0x0000, + 0x0038, + 0x007C, + 0x007C, + 0x00FC, + 0x00F8, + 0x00F0, + 0x00F0, + 0x01E0, + 0x01E0, + 0x00F0, + 0x00F0, + 0x00F8, + 0x00FC, + 0x007C, + 0x007C, + 0x0038, + 0x0000, + + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0060, + 0x0000, + 0x0000, + + 0x0000, + 0x01C0, + 0x03E0, + 0x03F0, + 0x03F0, + 0x01F0, + 0x00F0, + 0x00F0, + 0x0078, + 0x0078, + 0x00F0, + 0x00F0, + 0x01F0, + 0x03F0, + 0x03F0, + 0x03E0, + 0x01C0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00C2, + 0x01E7, + 0x03FF, + 0x07FE, + 0x0F3C, + 0x0618, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0780, + 0x07C0, + 0x07E0, + 0x07F0, + 0x07F8, + 0x07FC, + 0x07F8, + 0x07F0, + 0x07E0, + 0x07C0, + 0x0780, + 0x0700, + 0x0700, + 0x0700, + 0x0F80, + 0x0F80, + 0x0F80, + + 0x07E0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x07FE, + 0x0FFF, + 0x0FFE, + 0x0FFC, + 0x0FFC, + 0x07FE, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x007E, + + 0x0660, + 0x0FF0, + 0x0FE0, + 0x0FC0, + 0x0FE0, + 0x0FF0, + 0x07FC, + 0x03FE, + 0x03FE, + 0x03FE, + 0x03FE, + 0x01FE, + 0x00E7, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x0066, + + 0x07F0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x07F8, + 0x00FC, + 0x00FC, + 0x01FE, + 0x01FE, + 0x01FE, + 0x00FE, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x0066, + + 0x0600, + 0x0F00, + 0x0F00, + 0x0FE0, + 0x0FF0, + 0x0FF0, + 0x07F8, + 0x01FC, + 0x03FE, + 0x03FE, + 0x03FE, + 0x01FF, + 0x00FF, + 0x00FF, + 0x007E, + 0x003C, + 0x003C, + 0x0018, + + 0x0600, + 0x0F00, + 0x0F00, + 0x0FE0, + 0x0FF0, + 0x0FF0, + 0x07F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01FE, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x00FE, + + 0x07F0, + 0x0FF0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x07F0, + 0x03C0, + 0x03C0, + 0x03F8, + 0x03FC, + 0x03FC, + 0x01FE, + 0x00FF, + 0x00FF, + 0x007E, + 0x003C, + 0x003C, + 0x0018, + + 0x0660, + 0x0FF0, + 0x0FF0, + 0x07E0, + 0x03C0, + 0x01F8, + 0x03FC, + 0x03F8, + 0x03F0, + 0x03F8, + 0x03FC, + 0x01F8, + 0x0078, + 0x0078, + 0x007E, + 0x007F, + 0x007F, + 0x003E, + + 0x07E0, + 0x0FF0, + 0x0FF0, + 0x07E0, + 0x03C0, + 0x03C0, + 0x01D8, + 0x01FC, + 0x01FC, + 0x01FC, + 0x01FC, + 0x00FC, + 0x007E, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + 0x0037, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0660, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x07E0, + 0x0FF0, + 0x07E0, + 0x0FF0, + 0x0FE0, + 0x0FF0, + 0x07F0, + 0x0FF0, + 0x07E0, + + 0x03E0, + 0x07F0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x06F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00FE, + 0x00FF, + 0x00FF, + 0x00FE, + 0x00FE, + 0x00FF, + 0x0066, + + 0x0770, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x01FC, + 0x03FE, + 0x03FE, + 0x03FE, + 0x03FE, + 0x00FF, + 0x00FF, + 0x00FF, + 0x007E, + 0x003C, + 0x0018, + + 0x023F, + 0x073F, + 0x073F, + 0x073F, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x073F, + 0x073F, + 0x073F, + 0x073F, + 0x073F, + 0x073F, + + 0x023F, + 0x073F, + 0x073F, + 0x073F, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x073F, + 0x073F, + 0x073F, + 0x073F, + 0x073F, + 0x073F, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0618, + 0x0F3C, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0F3C, + 0x0618, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0630, + 0x0F78, + 0x0F78, + 0x0F78, + 0x0F78, + 0x07F0, + 0x07F0, + 0x03E0, + 0x01C0, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01C0, + 0x03E0, + 0x07F0, + 0x07F0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0F78, + 0x0630, + + 0x0001, + 0x0007, + 0x001F, + 0x007F, + 0x01FF, + 0x03FF, + 0x03FF, + 0x01FF, + 0x003F, + 0x003F, + 0x003F, + 0x003F, + 0x003F, + 0x003F, + 0x001F, + 0x0000, + 0x0000, + 0x0000, + + 0x0C00, + 0x0F80, + 0x0FE0, + 0x0FF8, + 0x0FFC, + 0x0FFC, + 0x0FF8, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0001, + 0x001F, + 0x00FF, + 0x00FF, + 0x00FF, + 0x003F, + 0x007F, + 0x00FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x001F, + 0x0003, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0F80, + 0x0FC0, + 0x0FE0, + 0x0FF0, + 0x0FF8, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0F0C, + 0x0F00, + 0x0E00, + 0x0E00, + 0x0C00, + 0x0800, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x001F, + 0x001F, + 0x001F, + 0x007F, + 0x01FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x01FF, + 0x00FF, + 0x007F, + 0x003E, + 0x0018, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x00E0, + 0x0FE0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0E78, + 0x0818, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0003, + 0x0007, + 0x0007, + 0x00FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x00FE, + 0x00F0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FC0, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FF8, + 0x0FF8, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x01E0, + 0x00E0, + 0x0040, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0400, + 0x0780, + 0x0FE0, + 0x0FFC, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFE, + 0x0FFC, + 0x0FF0, + 0x07E0, + 0x07C0, + 0x0700, + 0x0600, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x003C, + 0x007F, + 0x01FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x007F, + 0x0007, + 0x0007, + 0x000F, + 0x000F, + 0x000E, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0180, + 0x0FE0, + 0x0FF0, + 0x0FF8, + 0x0FFC, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFC, + 0x0FE0, + 0x0E00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x001E, + 0x003F, + 0x00FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x007F, + 0x003F, + 0x001F, + 0x007F, + 0x00FF, + 0x00FF, + 0x007F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0800, + 0x0E60, + 0x0FF0, + 0x0FF0, + 0x0FF8, + 0x0FF8, + 0x0FFC, + 0x0FFC, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFC, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0003, + 0x003F, + 0x007F, + 0x007F, + 0x003F, + 0x003F, + 0x003F, + 0x01FF, + 0x03FF, + 0x03FF, + 0x01FF, + 0x003F, + 0x0007, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0C00, + 0x0C00, + 0x0E00, + 0x0F18, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x01E0, + 0x0000, + 0x0000, + + 0x0000, + 0x000F, + 0x000F, + 0x001F, + 0x001F, + 0x001F, + 0x03FF, + 0x03FF, + 0x03FF, + 0x01FF, + 0x00FF, + 0x007F, + 0x003F, + 0x001F, + 0x0007, + 0x0003, + 0x0001, + 0x0000, + + 0x0000, + 0x0F00, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FF8, + 0x0FF0, + 0x0FE0, + 0x0FC0, + 0x0F80, + 0x0F00, + 0x0E00, + 0x0C00, + 0x0000, + + 0x0000, + 0x0003, + 0x0003, + 0x0007, + 0x018F, + 0x01FF, + 0x01FF, + 0x01FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x007F, + 0x007F, + 0x007F, + 0x0078, + 0x0000, + 0x0000, + + 0x0000, + 0x0C00, + 0x0FC0, + 0x0FE0, + 0x0FE0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FF8, + 0x0FFC, + 0x0FFC, + 0x0FF8, + 0x0FC0, + 0x0E00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0001, + 0x0067, + 0x00FF, + 0x00FF, + 0x01FF, + 0x01FF, + 0x03FF, + 0x03FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x03FF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0780, + 0x0FC0, + 0x0FF0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FE0, + 0x0FC0, + 0x0F80, + 0x0FE0, + 0x0FF0, + 0x0FF0, + 0x0FE0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0018, + 0x007F, + 0x00FF, + 0x01FF, + 0x03FF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x03FF, + 0x007F, + 0x0007, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x03C0, + 0x0FE0, + 0x0FF8, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FE0, + 0x0E00, + 0x0E00, + 0x0F00, + 0x0F00, + 0x0700, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0002, + 0x001E, + 0x007F, + 0x03FF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x07FF, + 0x03FF, + 0x00FF, + 0x007E, + 0x003E, + 0x000E, + 0x0006, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0FF0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x003F, + 0x03FF, + 0x03FF, + 0x03FF, + 0x01FF, + 0x01FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x0078, + 0x0070, + 0x0020, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0C00, + 0x0E00, + 0x0E00, + 0x0FF0, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x07F0, + 0x00F0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0070, + 0x007F, + 0x00FF, + 0x00FF, + 0x00FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01E7, + 0x0181, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0FE0, + 0x0FF8, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FF8, + 0x0FF0, + 0x0FE0, + 0x07C0, + 0x0180, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x001F, + 0x003F, + 0x007F, + 0x00FF, + 0x01FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x030F, + 0x000F, + 0x0007, + 0x0007, + 0x0003, + 0x0001, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0800, + 0x0F80, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FC0, + 0x0FE0, + 0x0FF0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0F80, + 0x0C00, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0180, + 0x03C0, + 0x07E0, + 0x07E0, + 0x03C0, + 0x0180, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0060, + 0x00F0, + 0x01F8, + 0x03FC, + 0x01F8, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x01F8, + 0x03FC, + 0x01F8, + 0x00F0, + 0x0060, + 0x0000, + + 0x0000, + 0x0100, + 0x0380, + 0x07C0, + 0x0FF0, + 0x0FF8, + 0x07F8, + 0x03BC, + 0x013C, + 0x013C, + 0x033C, + 0x07F8, + 0x0FF8, + 0x0FF0, + 0x07C0, + 0x0380, + 0x0100, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x01F8, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x01F8, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x01F8, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x01F8, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x01F8, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x01F8, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x0000, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x0F0F, + 0x0F0F, + 0x0F0F, + 0x0F0F, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0F80, + 0x0F80, + 0x0FFF, + 0x0FFF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x0787, + 0x0787, + 0x0787, + 0x0787, + 0x0787, + + 0x0000, + 0x0000, + 0x0000, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x081F, + 0x081F, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0F9F, + 0x0F9F, + 0x0F80, + 0x0F80, + 0x0F80, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x01F8, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0040, + 0x00E0, + 0x0EEF, + 0x0FFF, + 0x0EEF, + 0x00E0, + 0x0040, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x07DE, + 0x07FE, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x07FE, + 0x07BE, + 0x079E, + 0x030C, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x03FC, + 0x03C0, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x03FC, + 0x03FC, + 0x03FC, + 0x00F8, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x03FC, + 0x03F8, + 0x03C0, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x03F0, + 0x03FC, + 0x03FC, + 0x01F8, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x03FC, + 0x03FC, + 0x03FC, + 0x01F8, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0E0F, + 0x0F1F, + 0x0FBF, + 0x03F8, + 0x01F0, + 0x00E0, + 0x0040, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0070, + 0x00F8, + 0x0070, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x007F, + 0x00FF, + 0x007F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x00E0, + 0x01F0, + 0x00E0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FE0, + 0x0FF0, + 0x0FE0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0780, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x00F0, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0787, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0066, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x007E, + 0x003C, + 0x0018, + 0x0000, + 0x0000, + 0x0000, + + 0x0F80, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0180, + 0x03C0, + 0x07C0, + 0x07C0, + 0x03C0, + 0x03C0, + 0x03C6, + 0x07EF, + 0x07EF, + 0x03C6, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03C0, + 0x07E0, + 0x0FF0, + 0x0FF0, + 0x07E0, + 0x03F0, + 0x0FF0, + 0x0FF0, + 0x07E0, + 0x03C0, + 0x0000, + 0x0000, + 0x0000, + + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + 0x01F8, + 0x00F0, + 0x00F0, + 0x0060, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x000E, + 0x000F, + 0x000F, + 0x000F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x000F, + 0x000F, + 0x000F, + 0x000E, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0700, + + 0x0700, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0070, + 0x00F8, + 0x0070, + 0x0000, + 0x0000, + 0x0000, + 0x0010, + 0x0038, + 0x003C, + 0x003E, + 0x003F, + 0x003E, + 0x003C, + 0x0038, + 0x0010, + 0x0000, + + 0x0000, + 0x0000, + 0x00E0, + 0x01F0, + 0x00E0, + 0x0000, + 0x0000, + 0x0000, + 0x0080, + 0x01C0, + 0x03C0, + 0x07C0, + 0x0FC0, + 0x07C0, + 0x03C0, + 0x01C0, + 0x0080, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0078, + 0x0078, + 0x0078, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + 0x0078, + 0x0078, + 0x0078, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x01E0, + 0x01E0, + 0x01E0, + + 0x01E0, + 0x01E0, + 0x01E0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0660, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF4, + 0x07EE, + 0x07EE, + 0x0FFE, + 0x0FF4, + 0x0FF4, + 0x07EE, + 0x07EE, + 0x0FEE, + 0x0FF4, + 0x0FF0, + 0x0FF0, + 0x0FE0, + 0x07C0, + + 0x0040, + 0x00E0, + 0x00E0, + 0x07FC, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFF, + 0x0FFE, + 0x0FFE, + 0x07FE, + 0x00FE, + 0x01FE, + 0x03FE, + 0x07FE, + 0x0FFE, + 0x0FFF, + 0x0556, + + 0x0000, + 0x0000, + 0x0660, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x07E6, + 0x03CF, + 0x018E, + 0x03CE, + 0x07EE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x066E, + 0x000F, + 0x0006, + + 0x0000, + 0x0000, + 0x0660, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x07E6, + 0x03CF, + 0x018E, + 0x07CE, + 0x0FEE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FEE, + 0x07CE, + 0x000F, + 0x0006, + + 0x0000, + 0x0000, + 0x03C0, + 0x07E0, + 0x0FC0, + 0x0F00, + 0x0FC6, + 0x07EF, + 0x03CE, + 0x03CE, + 0x07EE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x07EE, + 0x000F, + 0x0006, + + 0x0000, + 0x0000, + 0x03C0, + 0x07E0, + 0x0FC0, + 0x0F00, + 0x0FC6, + 0x07EF, + 0x03CE, + 0x07CE, + 0x0FEE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FEE, + 0x07CE, + 0x000F, + 0x0006, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0660, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF4, + 0x07FE, + 0x07FE, + 0x0FFE, + 0x0FF4, + 0x0FF4, + 0x0FFE, + 0x067E, + 0x0FFE, + 0x0FF4, + 0x0FF0, + 0x07E0, + 0x03C0, + 0x0180, + + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x07FE, + 0x03FC, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x07FE, + 0x03FC, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0003, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0003, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + + 0x0000, + 0x0C00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0C00, + 0x0000, + + 0x0000, + 0x0FFE, + 0x0FFF, + 0x0FFF, + 0x0F9F, + 0x0FDF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FDF, + 0x0F9F, + 0x0FFF, + 0x0FFF, + 0x0FFE, + 0x0000, + + 0x0001, + 0x0003, + 0x0033, + 0x007B, + 0x007F, + 0x003F, + 0x001F, + 0x00FC, + 0x01FC, + 0x01FC, + 0x00FC, + 0x001F, + 0x003F, + 0x007F, + 0x007B, + 0x0033, + 0x0003, + 0x0003, + + 0x0800, + 0x0C00, + 0x0CC0, + 0x0DE0, + 0x0FE0, + 0x0FC0, + 0x0F80, + 0x03F0, + 0x03F8, + 0x03F8, + 0x03F0, + 0x0F80, + 0x0FC0, + 0x0FE0, + 0x0DE0, + 0x0CC0, + 0x0C00, + 0x0800, + + 0x0000, + 0x0003, + 0x000F, + 0x001F, + 0x003F, + 0x007D, + 0x0079, + 0x00F1, + 0x00F1, + 0x00F1, + 0x00F1, + 0x0079, + 0x007D, + 0x003F, + 0x001F, + 0x000F, + 0x0003, + 0x0000, + + 0x0000, + 0x0C00, + 0x0F00, + 0x0F80, + 0x0FC0, + 0x0FE0, + 0x0FE0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FE0, + 0x0FE0, + 0x0FC0, + 0x0F80, + 0x0F00, + 0x0C00, + 0x0000, + + 0x0006, + 0x000F, + 0x001F, + 0x003F, + 0x07FF, + 0x0FF7, + 0x0FE7, + 0x0FE7, + 0x0FE7, + 0x0FE7, + 0x0FE7, + 0x0FF7, + 0x07FF, + 0x003F, + 0x001F, + 0x000F, + 0x0006, + 0x0000, + + 0x0020, + 0x0070, + 0x0038, + 0x01B8, + 0x03F8, + 0x05FC, + 0x0FFC, + 0x0FFC, + 0x07FC, + 0x0FFC, + 0x0FFC, + 0x05FC, + 0x03F8, + 0x01B8, + 0x0038, + 0x0070, + 0x0020, + 0x0000, + + 0x07FE, + 0x0FFF, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x03FC, + 0x01F8, + 0x00F0, + 0x00F0, + 0x01F8, + 0x03FC, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x0FFF, + 0x07FE, + + 0x0000, + 0x0300, + 0x0380, + 0x03C0, + 0x03E0, + 0x03F0, + 0x03F8, + 0x03FC, + 0x03FE, + 0x03FF, + 0x03FF, + 0x03FC, + 0x03FC, + 0x03FE, + 0x003E, + 0x003E, + 0x003E, + 0x0008, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0322, + 0x07F7, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FF7, + 0x0FF7, + 0x0FF7, + 0x0FF7, + 0x04A2, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0722, + 0x0FF7, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0F77, + 0x0E77, + 0x0E77, + 0x0E77, + 0x0422, + + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, }; #endif /* FONT12X18_H_ */ diff --git a/flight/targets/boards/osd/firmware/inc/font8x10.h b/flight/targets/boards/osd/firmware/inc/font8x10.h index f199499de..d0fe73306 100644 --- a/flight/targets/boards/osd/firmware/inc/font8x10.h +++ b/flight/targets/boards/osd/firmware/inc/font8x10.h @@ -9,5640 +9,5638 @@ #define FONT8X10_H_ static const uint8_t font_frame8x10[] = { -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x60, -0x10, -0x20, -0x4C, -0x7A, -0x0A, -0x0C, -0x00, -0x00, - -0x00, -0x60, -0x20, -0x60, -0x2C, -0x6A, -0x0A, -0x0C, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x10, -0x10, -0x10, -0x10, -0x10, -0x00, -0x10, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x28, -0x28, -0x7C, -0x28, -0x7C, -0x28, -0x28, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x7C, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x30, -0x30, -0x00, -0x00, - -0x00, -0x00, -0x04, -0x08, -0x10, -0x20, -0x40, -0x00, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x4C, -0x54, -0x64, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x10, -0x30, -0x10, -0x10, -0x10, -0x10, -0x38, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x04, -0x08, -0x10, -0x20, -0x7C, -0x00, -0x00, - -0x00, -0x7C, -0x08, -0x10, -0x08, -0x04, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x08, -0x18, -0x28, -0x48, -0x7C, -0x08, -0x08, -0x00, -0x00, - -0x00, -0x7C, -0x40, -0x78, -0x04, -0x04, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x18, -0x20, -0x40, -0x78, -0x44, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x7C, -0x04, -0x08, -0x10, -0x20, -0x20, -0x20, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x38, -0x44, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x3C, -0x04, -0x08, -0x30, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x30, -0x30, -0x00, -0x30, -0x30, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x7C, -0x00, -0x7C, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x04, -0x34, -0x54, -0x54, -0x38, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x44, -0x7C, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x78, -0x44, -0x44, -0x78, -0x44, -0x44, -0x78, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x40, -0x40, -0x40, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x70, -0x48, -0x44, -0x44, -0x44, -0x48, -0x70, -0x00, -0x00, - -0x00, -0x7C, -0x40, -0x40, -0x78, -0x40, -0x40, -0x7C, -0x00, -0x00, - -0x00, -0x7C, -0x40, -0x40, -0x78, -0x40, -0x40, -0x40, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x40, -0x58, -0x44, -0x44, -0x3C, -0x00, -0x00, - -0x00, -0x44, -0x44, -0x44, -0x7C, -0x44, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x38, -0x10, -0x10, -0x10, -0x10, -0x10, -0x38, -0x00, -0x00, - -0x00, -0x1C, -0x08, -0x08, -0x08, -0x08, -0x48, -0x30, -0x00, -0x00, - -0x00, -0x44, -0x48, -0x50, -0x60, -0x50, -0x48, -0x44, -0x00, -0x00, - -0x00, -0x40, -0x40, -0x40, -0x40, -0x40, -0x40, -0x7C, -0x00, -0x00, - -0x00, -0x44, -0x6C, -0x54, -0x54, -0x44, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x44, -0x44, -0x64, -0x54, -0x4C, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x44, -0x44, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x78, -0x44, -0x44, -0x78, -0x40, -0x40, -0x40, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x44, -0x54, -0x48, -0x34, -0x00, -0x00, - -0x00, -0x78, -0x44, -0x44, -0x78, -0x50, -0x48, -0x44, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x40, -0x38, -0x04, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x7C, -0x10, -0x10, -0x10, -0x10, -0x10, -0x10, -0x00, -0x00, - -0x00, -0x44, -0x44, -0x44, -0x44, -0x44, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x44, -0x44, -0x44, -0x44, -0x44, -0x28, -0x10, -0x00, -0x00, - -0x00, -0x44, -0x44, -0x44, -0x54, -0x54, -0x54, -0x28, -0x00, -0x00, - -0x00, -0x44, -0x44, -0x28, -0x10, -0x28, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x44, -0x44, -0x44, -0x28, -0x10, -0x10, -0x10, -0x00, -0x00, - -0x00, -0x7C, -0x04, -0x08, -0x10, -0x20, -0x40, -0x7C, -0x00, -0x00, - -0x00, -0x38, -0x20, -0x20, -0x20, -0x20, -0x20, -0x38, -0x00, -0x00, - -0x00, -0x00, -0x40, -0x20, -0x10, -0x08, -0x04, -0x00, -0x00, -0x00, - -0x00, -0x38, -0x08, -0x08, -0x08, -0x08, -0x08, -0x38, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x38, -0x04, -0x3C, -0x44, -0x3C, -0x00, -0x00, - -0x00, -0x40, -0x40, -0x40, -0x78, -0x44, -0x44, -0x78, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x3C, -0x40, -0x40, -0x40, -0x3C, -0x00, -0x00, - -0x00, -0x04, -0x04, -0x04, -0x3C, -0x44, -0x44, -0x3C, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x38, -0x44, -0x7C, -0x40, -0x3C, -0x00, -0x00, - -0x00, -0x08, -0x14, -0x10, -0x7C, -0x10, -0x10, -0x10, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x3C, -0x44, -0x3C, -0x04, -0x78, -0x00, -0x00, - -0x00, -0x40, -0x40, -0x40, -0x78, -0x44, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x00, -0x10, -0x00, -0x10, -0x10, -0x10, -0x10, -0x00, -0x00, - -0x00, -0x08, -0x00, -0x08, -0x08, -0x08, -0x48, -0x30, -0x00, -0x00, - -0x00, -0x20, -0x20, -0x24, -0x28, -0x30, -0x28, -0x24, -0x00, -0x00, - -0x00, -0x30, -0x10, -0x10, -0x10, -0x10, -0x10, -0x38, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x68, -0x54, -0x54, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x58, -0x64, -0x44, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x38, -0x44, -0x44, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x78, -0x44, -0x78, -0x40, -0x40, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x3C, -0x44, -0x3C, -0x04, -0x04, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x58, -0x64, -0x40, -0x40, -0x40, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x3C, -0x40, -0x38, -0x04, -0x78, -0x00, -0x00, - -0x00, -0x10, -0x10, -0x7C, -0x10, -0x10, -0x14, -0x08, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x44, -0x44, -0x44, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x44, -0x44, -0x44, -0x28, -0x10, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x44, -0x44, -0x54, -0x54, -0x28, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x44, -0x28, -0x10, -0x28, -0x44, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x44, -0x28, -0x10, -0x10, -0x20, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x7C, -0x08, -0x10, -0x20, -0x7C, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x10, -0x10, -0x10, -0x10, -0x10, -0x10, -0x10, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x38, -0x54, -0x54, -0x54, -0x44, -0x54, -0x44, -0x7C, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x44, -0x44, -0x44, -0x44, -0x7C, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x44, -0x44, -0x7C, -0x7C, -0x7C, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x7C, -0x7C, -0x7C, -0x7C, -0x7C, -0x00, - -0x00, -0x38, -0x44, -0x7C, -0x7C, -0x7C, -0x7C, -0x7C, -0x7C, -0x00, - -0x00, -0x38, -0x7C, -0x7C, -0x7C, -0x7C, -0x7C, -0x7C, -0x7C, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0xFF, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x18, -0x18, -0xFF, -0x18, -0x18, -0x00, -0x00, - -0x00, -0x42, -0x42, -0x62, -0x52, -0xCB, -0x46, -0x42, -0x42, -0x00, - -0x00, -0x3C, -0x42, -0x40, -0x30, -0xCF, -0x02, -0x42, -0x3C, -0x00, - -0x00, -0x3E, -0x20, -0x20, -0x38, -0xA3, -0x20, -0x20, -0x3E, -0x00, - -0x00, -0x42, -0x42, -0x42, -0x42, -0xDB, -0x5A, -0x5A, -0x24, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x81, -0x42, -0x24, -0x18, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x18, -0x7E, -0x5E, -0x9F, -0x8F, -0x4E, -0x6E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0x9F, -0x8F, -0x46, -0x66, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0x8F, -0x42, -0x66, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xF1, -0x42, -0x66, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xF9, -0xF1, -0x62, -0x66, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7A, -0xF9, -0xF1, -0x72, -0x76, -0x18, -0x00, - -0x00, -0x18, -0x76, -0x72, -0xF1, -0xF9, -0x7A, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x66, -0x62, -0xF1, -0xF9, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x66, -0x42, -0xF1, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x66, -0x42, -0x8F, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x66, -0x46, -0x8F, -0x9F, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x6E, -0x4E, -0x8F, -0x9F, -0x5E, -0x7E, -0x18, -0x00, - -0x00, -0x07, -0x06, -0x04, -0x44, -0x3C, -0x3C, -0x24, -0x24, -0x00, - -0x00, -0xE0, -0x60, -0x20, -0x22, -0x3C, -0x3C, -0x24, -0x24, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x60, + 0x10, + 0x20, + 0x4C, + 0x7A, + 0x0A, + 0x0C, + 0x00, + 0x00, + + 0x00, + 0x60, + 0x20, + 0x60, + 0x2C, + 0x6A, + 0x0A, + 0x0C, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x00, + 0x10, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x28, + 0x28, + 0x7C, + 0x28, + 0x7C, + 0x28, + 0x28, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x7C, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x30, + 0x30, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x04, + 0x08, + 0x10, + 0x20, + 0x40, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x4C, + 0x54, + 0x64, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x10, + 0x30, + 0x10, + 0x10, + 0x10, + 0x10, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x04, + 0x08, + 0x10, + 0x20, + 0x7C, + 0x00, + 0x00, + + 0x00, + 0x7C, + 0x08, + 0x10, + 0x08, + 0x04, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x08, + 0x18, + 0x28, + 0x48, + 0x7C, + 0x08, + 0x08, + 0x00, + 0x00, + + 0x00, + 0x7C, + 0x40, + 0x78, + 0x04, + 0x04, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x18, + 0x20, + 0x40, + 0x78, + 0x44, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x7C, + 0x04, + 0x08, + 0x10, + 0x20, + 0x20, + 0x20, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x38, + 0x44, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x3C, + 0x04, + 0x08, + 0x30, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x30, + 0x30, + 0x00, + 0x30, + 0x30, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x7C, + 0x00, + 0x7C, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x04, + 0x34, + 0x54, + 0x54, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x44, + 0x7C, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x78, + 0x44, + 0x44, + 0x78, + 0x44, + 0x44, + 0x78, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x40, + 0x40, + 0x40, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x70, + 0x48, + 0x44, + 0x44, + 0x44, + 0x48, + 0x70, + 0x00, + 0x00, + + 0x00, + 0x7C, + 0x40, + 0x40, + 0x78, + 0x40, + 0x40, + 0x7C, + 0x00, + 0x00, + + 0x00, + 0x7C, + 0x40, + 0x40, + 0x78, + 0x40, + 0x40, + 0x40, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x40, + 0x58, + 0x44, + 0x44, + 0x3C, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x44, + 0x44, + 0x7C, + 0x44, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x1C, + 0x08, + 0x08, + 0x08, + 0x08, + 0x48, + 0x30, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x48, + 0x50, + 0x60, + 0x50, + 0x48, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x40, + 0x40, + 0x40, + 0x40, + 0x40, + 0x40, + 0x7C, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x6C, + 0x54, + 0x54, + 0x44, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x44, + 0x64, + 0x54, + 0x4C, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x44, + 0x44, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x78, + 0x44, + 0x44, + 0x78, + 0x40, + 0x40, + 0x40, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x44, + 0x54, + 0x48, + 0x34, + 0x00, + 0x00, + + 0x00, + 0x78, + 0x44, + 0x44, + 0x78, + 0x50, + 0x48, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x40, + 0x38, + 0x04, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x7C, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x44, + 0x44, + 0x44, + 0x44, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x44, + 0x44, + 0x44, + 0x44, + 0x28, + 0x10, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x44, + 0x44, + 0x54, + 0x54, + 0x54, + 0x28, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x44, + 0x28, + 0x10, + 0x28, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x44, + 0x44, + 0x28, + 0x10, + 0x10, + 0x10, + 0x00, + 0x00, + + 0x00, + 0x7C, + 0x04, + 0x08, + 0x10, + 0x20, + 0x40, + 0x7C, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x20, + 0x20, + 0x20, + 0x20, + 0x20, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x40, + 0x20, + 0x10, + 0x08, + 0x04, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x08, + 0x08, + 0x08, + 0x08, + 0x08, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x38, + 0x04, + 0x3C, + 0x44, + 0x3C, + 0x00, + 0x00, + + 0x00, + 0x40, + 0x40, + 0x40, + 0x78, + 0x44, + 0x44, + 0x78, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x3C, + 0x40, + 0x40, + 0x40, + 0x3C, + 0x00, + 0x00, + + 0x00, + 0x04, + 0x04, + 0x04, + 0x3C, + 0x44, + 0x44, + 0x3C, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x38, + 0x44, + 0x7C, + 0x40, + 0x3C, + 0x00, + 0x00, + + 0x00, + 0x08, + 0x14, + 0x10, + 0x7C, + 0x10, + 0x10, + 0x10, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x3C, + 0x44, + 0x3C, + 0x04, + 0x78, + 0x00, + 0x00, + + 0x00, + 0x40, + 0x40, + 0x40, + 0x78, + 0x44, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x10, + 0x00, + 0x10, + 0x10, + 0x10, + 0x10, + 0x00, + 0x00, + + 0x00, + 0x08, + 0x00, + 0x08, + 0x08, + 0x08, + 0x48, + 0x30, + 0x00, + 0x00, + + 0x00, + 0x20, + 0x20, + 0x24, + 0x28, + 0x30, + 0x28, + 0x24, + 0x00, + 0x00, + + 0x00, + 0x30, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x68, + 0x54, + 0x54, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x58, + 0x64, + 0x44, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x38, + 0x44, + 0x44, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x78, + 0x44, + 0x78, + 0x40, + 0x40, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x3C, + 0x44, + 0x3C, + 0x04, + 0x04, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x58, + 0x64, + 0x40, + 0x40, + 0x40, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x3C, + 0x40, + 0x38, + 0x04, + 0x78, + 0x00, + 0x00, + + 0x00, + 0x10, + 0x10, + 0x7C, + 0x10, + 0x10, + 0x14, + 0x08, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x44, + 0x44, + 0x44, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x44, + 0x44, + 0x44, + 0x28, + 0x10, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x44, + 0x44, + 0x54, + 0x54, + 0x28, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x44, + 0x28, + 0x10, + 0x28, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x44, + 0x28, + 0x10, + 0x10, + 0x20, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x7C, + 0x08, + 0x10, + 0x20, + 0x7C, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x54, + 0x54, + 0x54, + 0x44, + 0x54, + 0x44, + 0x7C, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x44, + 0x44, + 0x44, + 0x44, + 0x7C, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x44, + 0x44, + 0x7C, + 0x7C, + 0x7C, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x00, + + 0x00, + 0x38, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0xFF, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x18, + 0x18, + 0xFF, + 0x18, + 0x18, + 0x00, + 0x00, + + 0x00, + 0x42, + 0x42, + 0x62, + 0x52, + 0xCB, + 0x46, + 0x42, + 0x42, + 0x00, + + 0x00, + 0x3C, + 0x42, + 0x40, + 0x30, + 0xCF, + 0x02, + 0x42, + 0x3C, + 0x00, + + 0x00, + 0x3E, + 0x20, + 0x20, + 0x38, + 0xA3, + 0x20, + 0x20, + 0x3E, + 0x00, + + 0x00, + 0x42, + 0x42, + 0x42, + 0x42, + 0xDB, + 0x5A, + 0x5A, + 0x24, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x81, + 0x42, + 0x24, + 0x18, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x5E, + 0x9F, + 0x8F, + 0x4E, + 0x6E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0x9F, + 0x8F, + 0x46, + 0x66, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0x8F, + 0x42, + 0x66, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xF1, + 0x42, + 0x66, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xF9, + 0xF1, + 0x62, + 0x66, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7A, + 0xF9, + 0xF1, + 0x72, + 0x76, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x76, + 0x72, + 0xF1, + 0xF9, + 0x7A, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x66, + 0x62, + 0xF1, + 0xF9, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x66, + 0x42, + 0xF1, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x66, + 0x42, + 0x8F, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x66, + 0x46, + 0x8F, + 0x9F, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x6E, + 0x4E, + 0x8F, + 0x9F, + 0x5E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x07, + 0x06, + 0x04, + 0x44, + 0x3C, + 0x3C, + 0x24, + 0x24, + 0x00, + + 0x00, + 0xE0, + 0x60, + 0x20, + 0x22, + 0x3C, + 0x3C, + 0x24, + 0x24, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, }; static const uint8_t font_mask8x10[] = { -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0xF0, -0xF8, -0xFC, -0xFE, -0xFF, -0xFF, -0x7F, -0x3F, -0x1F, -0x00, - -0xF0, -0xF8, -0xFC, -0xFE, -0xFF, -0xFF, -0xFF, -0x3F, -0x1F, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x10, -0x38, -0x38, -0x38, -0x38, -0x38, -0x10, -0x38, -0x10, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x28, -0x7C, -0x7C, -0xFE, -0x7C, -0xFE, -0x7C, -0x7C, -0x28, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x7C, -0xFE, -0x7C, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x30, -0x78, -0x78, -0x30, -0x00, - -0x00, -0x04, -0x0E, -0x1C, -0x38, -0x70, -0xE0, -0x40, -0x00, -0x00, - -0x38, -0x7C, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, -0x38, -0x00, - -0x10, -0x38, -0x78, -0x38, -0x38, -0x38, -0x38, -0x7C, -0x38, -0x00, - -0x38, -0x7C, -0xFE, -0x4E, -0x1C, -0x38, -0x7C, -0xFE, -0x7C, -0x00, - -0x7C, -0xFE, -0x7C, -0x38, -0x1C, -0x4E, -0xFE, -0x7C, -0x38, -0x00, - -0x08, -0x1C, -0x3C, -0x7C, -0xFC, -0xFE, -0x7C, -0x1C, -0x08, -0x00, - -0x7C, -0xFE, -0xFC, -0xFC, -0x7E, -0x4E, -0xFE, -0x7C, -0x38, -0x00, - -0x18, -0x3C, -0x78, -0xF8, -0xFC, -0xFE, -0xFE, -0x7C, -0x38, -0x00, - -0x7C, -0xFE, -0x7E, -0x1C, -0x38, -0x70, -0x70, -0x70, -0x20, -0x00, - -0x38, -0x7C, -0xFE, -0xFE, -0x7C, -0xFE, -0xFE, -0x7C, -0x38, -0x00, - -0x38, -0x7C, -0xFE, -0xFE, -0x7E, -0x3E, -0x3C, -0x78, -0x30, -0x00, - -0x00, -0x00, -0x30, -0x78, -0x78, -0x30, -0x78, -0x78, -0x30, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x7C, -0xFE, -0x7C, -0xFE, -0x7C, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x38, -0x7C, -0xFE, -0x7E, -0x7E, -0xFE, -0xFE, -0x7C, -0x38, -0x00, - -0x38, -0x7C, -0xFE, -0xEE, -0xFE, -0xFE, -0xFE, -0xEE, -0x44, -0x00, - -0x78, -0xFC, -0xFE, -0xFE, -0xFC, -0xFE, -0xFE, -0xFC, -0x78, -0x00, - -0x38, -0x7C, -0xFE, -0xE4, -0xE0, -0xE4, -0xFE, -0x7C, -0x38, -0x00, - -0x70, -0xF8, -0xFC, -0xEE, -0xEE, -0xEE, -0xFC, -0xF8, -0x70, -0x00, - -0x7C, -0xFE, -0xFC, -0xF8, -0xFC, -0xF8, -0xFC, -0xFE, -0x7C, -0x00, - -0x7C, -0xFE, -0xFC, -0xF8, -0xFC, -0xF8, -0xE0, -0xE0, -0x40, -0x00, - -0x38, -0x7C, -0xFE, -0xFC, -0xFC, -0xFE, -0xFE, -0x7E, -0x3C, -0x00, - -0x44, -0xEE, -0xEE, -0xFE, -0xFE, -0xFE, -0xEE, -0xEE, -0x44, -0x00, - -0x38, -0x7C, -0x38, -0x38, -0x38, -0x38, -0x38, -0x7C, -0x38, -0x00, - -0x1C, -0x3E, -0x1C, -0x1C, -0x1C, -0x5C, -0xFC, -0x78, -0x30, -0x00, - -0x44, -0xEE, -0xFC, -0xF8, -0xF0, -0xF8, -0xFC, -0xEE, -0x44, -0x00, - -0x40, -0xE0, -0xE0, -0xE0, -0xE0, -0xE0, -0xFC, -0xFE, -0x7C, -0x00, - -0x44, -0xEE, -0xFE, -0xFE, -0xFE, -0xFE, -0xEE, -0xEE, -0x44, -0x00, - -0x44, -0xEE, -0xEE, -0xFE, -0xFE, -0xFE, -0xEE, -0xEE, -0x44, -0x00, - -0x38, -0x7C, -0xFE, -0xEE, -0xEE, -0xEE, -0xFE, -0x7C, -0x38, -0x00, - -0x78, -0xFC, -0xFE, -0xFE, -0xFC, -0xF8, -0xE0, -0xE0, -0x40, -0x00, - -0x38, -0x7C, -0xFE, -0xEE, -0xEE, -0xFE, -0xFC, -0x7E, -0x34, -0x00, - -0x78, -0xFC, -0xFE, -0xFE, -0xFC, -0xF8, -0xFC, -0xEE, -0x44, -0x00, - -0x38, -0x7C, -0xFE, -0xFC, -0x7C, -0x7E, -0xFE, -0x7C, -0x38, -0x00, - -0x7C, -0xFE, -0x7C, -0x38, -0x38, -0x38, -0x38, -0x38, -0x10, -0x00, - -0x44, -0xEE, -0xEE, -0xEE, -0xEE, -0xEE, -0xFE, -0x7C, -0x38, -0x00, - -0x44, -0xEE, -0xEE, -0xEE, -0xEE, -0xEE, -0x7C, -0x38, -0x10, -0x00, - -0x44, -0xEE, -0xEE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, -0x28, -0x00, - -0x44, -0xEE, -0xEE, -0x7C, -0x38, -0x7C, -0xEE, -0xEE, -0x44, -0x00, - -0x44, -0xEE, -0xEE, -0xEE, -0x7C, -0x38, -0x38, -0x38, -0x10, -0x00, - -0x7C, -0xFE, -0x7E, -0x1C, -0x38, -0x70, -0xFC, -0xFE, -0x7C, -0x00, - -0x38, -0x7C, -0x78, -0x70, -0x70, -0x70, -0x78, -0x7C, -0x38, -0x00, - -0x00, -0x40, -0xE0, -0x70, -0x38, -0x1C, -0x0E, -0x04, -0x00, -0x00, - -0x38, -0x7C, -0x3C, -0x1C, -0x1C, -0x1C, -0x3C, -0x7C, -0x38, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x38, -0x7C, -0x3E, -0x7E, -0xFE, -0x7E, -0x3C, -0x00, - -0x40, -0xE0, -0xE0, -0xF8, -0xFC, -0xFE, -0xFE, -0xFC, -0x78, -0x00, - -0x00, -0x00, -0x3C, -0x7E, -0xFC, -0xE0, -0xFC, -0x7E, -0x3C, -0x00, - -0x04, -0x0E, -0x0E, -0x3E, -0x7E, -0xFE, -0xFE, -0x7E, -0x3C, -0x00, - -0x00, -0x00, -0x38, -0x7C, -0xFE, -0xFE, -0xFC, -0x7E, -0x3C, -0x00, - -0x08, -0x1C, -0x3E, -0x7C, -0xFE, -0x7C, -0x38, -0x38, -0x10, -0x00, - -0x00, -0x00, -0x3C, -0x7E, -0xFE, -0x7E, -0x7E, -0xFC, -0x78, -0x00, - -0x40, -0xE0, -0xE0, -0xF8, -0xFC, -0xFE, -0xEE, -0xEE, -0x44, -0x00, - -0x00, -0x10, -0x38, -0x10, -0x38, -0x38, -0x38, -0x38, -0x10, -0x00, - -0x08, -0x1C, -0x08, -0x1C, -0x1C, -0x5C, -0xFC, -0x78, -0x30, -0x00, - -0x20, -0x70, -0x74, -0x7E, -0x7C, -0x78, -0x7C, -0x7E, -0x24, -0x00, - -0x30, -0x78, -0x38, -0x38, -0x38, -0x38, -0x38, -0x7C, -0x38, -0x00, - -0x00, -0x00, -0x68, -0xFC, -0xFE, -0xFE, -0xFE, -0xEE, -0x6C, -0x00, - -0x00, -0x00, -0x58, -0xFC, -0xFE, -0xEE, -0xEE, -0xEE, -0x44, -0x00, - -0x00, -0x00, -0x38, -0x7C, -0xFE, -0xEE, -0xFE, -0x7C, -0x38, -0x00, - -0x00, -0x00, -0x78, -0xFC, -0xFE, -0xFC, -0xF8, -0xE0, -0x40, -0x00, - -0x00, -0x00, -0x3C, -0x7E, -0xFE, -0x7E, -0x3E, -0x0E, -0x04, -0x00, - -0x00, -0x00, -0x58, -0xFC, -0xFE, -0xE4, -0xE0, -0xE0, -0x40, -0x00, - -0x00, -0x00, -0x3C, -0x7E, -0xFC, -0x7C, -0x7E, -0xFC, -0x78, -0x00, - -0x10, -0x38, -0x7C, -0xFE, -0x7C, -0x3C, -0x3E, -0x1C, -0x08, -0x00, - -0x00, -0x00, -0x44, -0xEE, -0xEE, -0xEE, -0xFE, -0x7C, -0x38, -0x00, - -0x00, -0x00, -0x44, -0xEE, -0xEE, -0xEE, -0x7C, -0x38, -0x10, -0x00, - -0x00, -0x00, -0x44, -0xEE, -0xFE, -0xFE, -0xFE, -0x7C, -0x28, -0x00, - -0x00, -0x00, -0x44, -0xEE, -0x7C, -0x38, -0x7C, -0xEE, -0x44, -0x00, - -0x00, -0x00, -0x44, -0xEE, -0x7C, -0x38, -0x38, -0x70, -0x20, -0x00, - -0x00, -0x00, -0x7C, -0xFE, -0x7C, -0x38, -0x7C, -0xFE, -0x7C, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x10, -0x38, -0x38, -0x38, -0x38, -0x38, -0x38, -0x38, -0x10, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x38, -0x7C, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, - -0x38, -0x7C, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, - -0x38, -0x7C, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, - -0x38, -0x7C, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, - -0x38, -0x7C, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, - -0x38, -0x7C, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0xFF, -0xFF, -0xFF, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x18, -0x3C, -0xFF, -0xFF, -0xFF, -0x3C, -0x18, -0x00, - -0x42, -0xE7, -0xE7, -0xF7, -0xFF, -0xFF, -0xEF, -0xE7, -0xE7, -0x42, - -0x3C, -0x7E, -0xFF, -0xF2, -0xFD, -0xFF, -0xCF, -0xFF, -0x7E, -0x3C, - -0x3E, -0x7F, -0x7E, -0x78, -0xFF, -0xFF, -0xF3, -0x7E, -0x7F, -0x3E, - -0x42, -0xE7, -0xE7, -0xE7, -0xFF, -0xFF, -0xFF, -0xFF, -0x7E, -0x24, - -0x00, -0x00, -0x00, -0x00, -0xFF, -0x00, -0xFF, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x81, -0xC3, -0xE7, -0x7E, -0x3C, -0x18, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x07, -0x0F, -0x0F, -0x4E, -0xFE, -0x7E, -0x7E, -0x7E, -0x7E, -0x24, - -0xE0, -0xF0, -0xF0, -0x72, -0x7F, -0x7E, -0x7E, -0x7E, -0x7E, -0x24, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0xF0, + 0xF8, + 0xFC, + 0xFE, + 0xFF, + 0xFF, + 0x7F, + 0x3F, + 0x1F, + 0x00, + + 0xF0, + 0xF8, + 0xFC, + 0xFE, + 0xFF, + 0xFF, + 0xFF, + 0x3F, + 0x1F, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x10, + 0x38, + 0x38, + 0x38, + 0x38, + 0x38, + 0x10, + 0x38, + 0x10, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x28, + 0x7C, + 0x7C, + 0xFE, + 0x7C, + 0xFE, + 0x7C, + 0x7C, + 0x28, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x7C, + 0xFE, + 0x7C, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x30, + 0x78, + 0x78, + 0x30, + 0x00, + + 0x00, + 0x04, + 0x0E, + 0x1C, + 0x38, + 0x70, + 0xE0, + 0x40, + 0x00, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x10, + 0x38, + 0x78, + 0x38, + 0x38, + 0x38, + 0x38, + 0x7C, + 0x38, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0x4E, + 0x1C, + 0x38, + 0x7C, + 0xFE, + 0x7C, + 0x00, + + 0x7C, + 0xFE, + 0x7C, + 0x38, + 0x1C, + 0x4E, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x08, + 0x1C, + 0x3C, + 0x7C, + 0xFC, + 0xFE, + 0x7C, + 0x1C, + 0x08, + 0x00, + + 0x7C, + 0xFE, + 0xFC, + 0xFC, + 0x7E, + 0x4E, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x18, + 0x3C, + 0x78, + 0xF8, + 0xFC, + 0xFE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x7C, + 0xFE, + 0x7E, + 0x1C, + 0x38, + 0x70, + 0x70, + 0x70, + 0x20, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0x7C, + 0xFE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0x7E, + 0x3E, + 0x3C, + 0x78, + 0x30, + 0x00, + + 0x00, + 0x00, + 0x30, + 0x78, + 0x78, + 0x30, + 0x78, + 0x78, + 0x30, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x7C, + 0xFE, + 0x7C, + 0xFE, + 0x7C, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0x7E, + 0x7E, + 0xFE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xEE, + 0xFE, + 0xFE, + 0xFE, + 0xEE, + 0x44, + 0x00, + + 0x78, + 0xFC, + 0xFE, + 0xFE, + 0xFC, + 0xFE, + 0xFE, + 0xFC, + 0x78, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xE4, + 0xE0, + 0xE4, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x70, + 0xF8, + 0xFC, + 0xEE, + 0xEE, + 0xEE, + 0xFC, + 0xF8, + 0x70, + 0x00, + + 0x7C, + 0xFE, + 0xFC, + 0xF8, + 0xFC, + 0xF8, + 0xFC, + 0xFE, + 0x7C, + 0x00, + + 0x7C, + 0xFE, + 0xFC, + 0xF8, + 0xFC, + 0xF8, + 0xE0, + 0xE0, + 0x40, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xFC, + 0xFC, + 0xFE, + 0xFE, + 0x7E, + 0x3C, + 0x00, + + 0x44, + 0xEE, + 0xEE, + 0xFE, + 0xFE, + 0xFE, + 0xEE, + 0xEE, + 0x44, + 0x00, + + 0x38, + 0x7C, + 0x38, + 0x38, + 0x38, + 0x38, + 0x38, + 0x7C, + 0x38, + 0x00, + + 0x1C, + 0x3E, + 0x1C, + 0x1C, + 0x1C, + 0x5C, + 0xFC, + 0x78, + 0x30, + 0x00, + + 0x44, + 0xEE, + 0xFC, + 0xF8, + 0xF0, + 0xF8, + 0xFC, + 0xEE, + 0x44, + 0x00, + + 0x40, + 0xE0, + 0xE0, + 0xE0, + 0xE0, + 0xE0, + 0xFC, + 0xFE, + 0x7C, + 0x00, + + 0x44, + 0xEE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xEE, + 0xEE, + 0x44, + 0x00, + + 0x44, + 0xEE, + 0xEE, + 0xFE, + 0xFE, + 0xFE, + 0xEE, + 0xEE, + 0x44, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xEE, + 0xEE, + 0xEE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x78, + 0xFC, + 0xFE, + 0xFE, + 0xFC, + 0xF8, + 0xE0, + 0xE0, + 0x40, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xEE, + 0xEE, + 0xFE, + 0xFC, + 0x7E, + 0x34, + 0x00, + + 0x78, + 0xFC, + 0xFE, + 0xFE, + 0xFC, + 0xF8, + 0xFC, + 0xEE, + 0x44, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xFC, + 0x7C, + 0x7E, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x7C, + 0xFE, + 0x7C, + 0x38, + 0x38, + 0x38, + 0x38, + 0x38, + 0x10, + 0x00, + + 0x44, + 0xEE, + 0xEE, + 0xEE, + 0xEE, + 0xEE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x44, + 0xEE, + 0xEE, + 0xEE, + 0xEE, + 0xEE, + 0x7C, + 0x38, + 0x10, + 0x00, + + 0x44, + 0xEE, + 0xEE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + 0x28, + 0x00, + + 0x44, + 0xEE, + 0xEE, + 0x7C, + 0x38, + 0x7C, + 0xEE, + 0xEE, + 0x44, + 0x00, + + 0x44, + 0xEE, + 0xEE, + 0xEE, + 0x7C, + 0x38, + 0x38, + 0x38, + 0x10, + 0x00, + + 0x7C, + 0xFE, + 0x7E, + 0x1C, + 0x38, + 0x70, + 0xFC, + 0xFE, + 0x7C, + 0x00, + + 0x38, + 0x7C, + 0x78, + 0x70, + 0x70, + 0x70, + 0x78, + 0x7C, + 0x38, + 0x00, + + 0x00, + 0x40, + 0xE0, + 0x70, + 0x38, + 0x1C, + 0x0E, + 0x04, + 0x00, + 0x00, + + 0x38, + 0x7C, + 0x3C, + 0x1C, + 0x1C, + 0x1C, + 0x3C, + 0x7C, + 0x38, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x38, + 0x7C, + 0x3E, + 0x7E, + 0xFE, + 0x7E, + 0x3C, + 0x00, + + 0x40, + 0xE0, + 0xE0, + 0xF8, + 0xFC, + 0xFE, + 0xFE, + 0xFC, + 0x78, + 0x00, + + 0x00, + 0x00, + 0x3C, + 0x7E, + 0xFC, + 0xE0, + 0xFC, + 0x7E, + 0x3C, + 0x00, + + 0x04, + 0x0E, + 0x0E, + 0x3E, + 0x7E, + 0xFE, + 0xFE, + 0x7E, + 0x3C, + 0x00, + + 0x00, + 0x00, + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFC, + 0x7E, + 0x3C, + 0x00, + + 0x08, + 0x1C, + 0x3E, + 0x7C, + 0xFE, + 0x7C, + 0x38, + 0x38, + 0x10, + 0x00, + + 0x00, + 0x00, + 0x3C, + 0x7E, + 0xFE, + 0x7E, + 0x7E, + 0xFC, + 0x78, + 0x00, + + 0x40, + 0xE0, + 0xE0, + 0xF8, + 0xFC, + 0xFE, + 0xEE, + 0xEE, + 0x44, + 0x00, + + 0x00, + 0x10, + 0x38, + 0x10, + 0x38, + 0x38, + 0x38, + 0x38, + 0x10, + 0x00, + + 0x08, + 0x1C, + 0x08, + 0x1C, + 0x1C, + 0x5C, + 0xFC, + 0x78, + 0x30, + 0x00, + + 0x20, + 0x70, + 0x74, + 0x7E, + 0x7C, + 0x78, + 0x7C, + 0x7E, + 0x24, + 0x00, + + 0x30, + 0x78, + 0x38, + 0x38, + 0x38, + 0x38, + 0x38, + 0x7C, + 0x38, + 0x00, + + 0x00, + 0x00, + 0x68, + 0xFC, + 0xFE, + 0xFE, + 0xFE, + 0xEE, + 0x6C, + 0x00, + + 0x00, + 0x00, + 0x58, + 0xFC, + 0xFE, + 0xEE, + 0xEE, + 0xEE, + 0x44, + 0x00, + + 0x00, + 0x00, + 0x38, + 0x7C, + 0xFE, + 0xEE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x00, + 0x00, + 0x78, + 0xFC, + 0xFE, + 0xFC, + 0xF8, + 0xE0, + 0x40, + 0x00, + + 0x00, + 0x00, + 0x3C, + 0x7E, + 0xFE, + 0x7E, + 0x3E, + 0x0E, + 0x04, + 0x00, + + 0x00, + 0x00, + 0x58, + 0xFC, + 0xFE, + 0xE4, + 0xE0, + 0xE0, + 0x40, + 0x00, + + 0x00, + 0x00, + 0x3C, + 0x7E, + 0xFC, + 0x7C, + 0x7E, + 0xFC, + 0x78, + 0x00, + + 0x10, + 0x38, + 0x7C, + 0xFE, + 0x7C, + 0x3C, + 0x3E, + 0x1C, + 0x08, + 0x00, + + 0x00, + 0x00, + 0x44, + 0xEE, + 0xEE, + 0xEE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x00, + 0x00, + 0x44, + 0xEE, + 0xEE, + 0xEE, + 0x7C, + 0x38, + 0x10, + 0x00, + + 0x00, + 0x00, + 0x44, + 0xEE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + 0x28, + 0x00, + + 0x00, + 0x00, + 0x44, + 0xEE, + 0x7C, + 0x38, + 0x7C, + 0xEE, + 0x44, + 0x00, + + 0x00, + 0x00, + 0x44, + 0xEE, + 0x7C, + 0x38, + 0x38, + 0x70, + 0x20, + 0x00, + + 0x00, + 0x00, + 0x7C, + 0xFE, + 0x7C, + 0x38, + 0x7C, + 0xFE, + 0x7C, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x10, + 0x38, + 0x38, + 0x38, + 0x38, + 0x38, + 0x38, + 0x38, + 0x10, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0xFF, + 0xFF, + 0xFF, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x18, + 0x3C, + 0xFF, + 0xFF, + 0xFF, + 0x3C, + 0x18, + 0x00, + + 0x42, + 0xE7, + 0xE7, + 0xF7, + 0xFF, + 0xFF, + 0xEF, + 0xE7, + 0xE7, + 0x42, + + 0x3C, + 0x7E, + 0xFF, + 0xF2, + 0xFD, + 0xFF, + 0xCF, + 0xFF, + 0x7E, + 0x3C, + + 0x3E, + 0x7F, + 0x7E, + 0x78, + 0xFF, + 0xFF, + 0xF3, + 0x7E, + 0x7F, + 0x3E, + + 0x42, + 0xE7, + 0xE7, + 0xE7, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0x7E, + 0x24, + + 0x00, + 0x00, + 0x00, + 0x00, + 0xFF, + 0x00, + 0xFF, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x81, + 0xC3, + 0xE7, + 0x7E, + 0x3C, + 0x18, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x07, + 0x0F, + 0x0F, + 0x4E, + 0xFE, + 0x7E, + 0x7E, + 0x7E, + 0x7E, + 0x24, + + 0xE0, + 0xF0, + 0xF0, + 0x72, + 0x7F, + 0x7E, + 0x7E, + 0x7E, + 0x7E, + 0x24, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, }; #endif /* FONT8X10_H_ */ diff --git a/flight/targets/boards/osd/firmware/inc/fonts.h b/flight/targets/boards/osd/firmware/inc/fonts.h index 8b7d35650..104392471 100644 --- a/flight/targets/boards/osd/firmware/inc/fonts.h +++ b/flight/targets/boards/osd/firmware/inc/fonts.h @@ -6,12 +6,12 @@ * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. - * + * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * + * * You should have received a copy of the GNU General Public License along * with this program; if not, write to the Free Software Foundation, Inc., * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. @@ -26,21 +26,20 @@ #include "font_outlined8x8.h" // This number must also be incremented for each new font. -#define NUM_FONTS 4 +#define NUM_FONTS 4 // Flags for fonts. -#define FONT_LOWERCASE_ONLY 1 -#define FONT_UPPERCASE_ONLY 2 +#define FONT_LOWERCASE_ONLY 1 +#define FONT_UPPERCASE_ONLY 2 // Font table. (Actual list of fonts in fonts.c.) -struct FontEntry -{ - int id; - unsigned char width, height; - const char *name; - const char *lookup; - const char *data; - int flags; +struct FontEntry { + int id; + unsigned char width, height; + const char *name; + const char *lookup; + const char *data; + int flags; }; extern struct FontEntry fonts[NUM_FONTS + 1]; diff --git a/flight/targets/boards/osd/firmware/inc/openpilot.h b/flight/targets/boards/osd/firmware/inc/openpilot.h index c0985e4c0..76cc3ccd6 100644 --- a/flight/targets/boards/osd/firmware/inc/openpilot.h +++ b/flight/targets/boards/osd/firmware/inc/openpilot.h @@ -10,19 +10,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/osd/firmware/inc/pios_config.h b/flight/targets/boards/osd/firmware/inc/pios_config.h index 8c2441c21..de099984c 100644 --- a/flight/targets/boards/osd/firmware/inc/pios_config.h +++ b/flight/targets/boards/osd/firmware/inc/pios_config.h @@ -142,15 +142,15 @@ #define PIOS_QUATERNION_STABILIZATION /* Performance counters */ -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 /* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 4000 -#define HEAP_LIMIT_CRITICAL 1000 -#define IRQSTACK_LIMIT_WARNING 150 -#define IRQSTACK_LIMIT_CRITICAL 80 -#define CPULOAD_LIMIT_WARNING 80 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 4000 +#define HEAP_LIMIT_CRITICAL 1000 +#define IRQSTACK_LIMIT_WARNING 150 +#define IRQSTACK_LIMIT_CRITICAL 80 +#define CPULOAD_LIMIT_WARNING 80 +#define CPULOAD_LIMIT_CRITICAL 95 /* Task stack sizes */ /* #define PIOS_ACTUATOR_STACK_SIZE 1020 */ diff --git a/flight/targets/boards/osd/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/osd/firmware/inc/pios_usb_board_data.h index 867849949..32e9f0ea6 100644 --- a/flight/targets/boards/osd/firmware/inc/pios_usb_board_data.h +++ b/flight/targets/boards/osd/firmware/inc/pios_usb_board_data.h @@ -35,12 +35,12 @@ #define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 4 +#define PIOS_USB_BOARD_EP_NUM 4 -#include /* USB_* macros */ +#include /* USB_* macros */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OSD -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OSD, USB_OP_BOARD_MODE_FW) -#define PIOS_USB_BOARD_SN_SUFFIX "+FW" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OSD +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OSD, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/osd/firmware/inc/splash.h b/flight/targets/boards/osd/firmware/inc/splash.h index 3ea31872c..1ba96618b 100644 --- a/flight/targets/boards/osd/firmware/inc/splash.h +++ b/flight/targets/boards/osd/firmware/inc/splash.h @@ -8,1442 +8,1448 @@ #ifndef SPLASH_H_ #define SPLASH_H_ -#define oplogo_width 144 +#define oplogo_width 144 #define oplogo_height 144 static const unsigned short oplogo_bits[] = { - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x001e, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0x01ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x07ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x0fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x7fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x0003, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x0007, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x000f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x001f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x07ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x0fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x1fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x3fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0x03ff, 0xffff, 0x0001, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0x03ff, 0xfffe, 0x0003, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0x01ff, 0xfff8, 0x000f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0x01ff, 0xffe0, 0x001f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0x01ff, 0xffc0, 0x003f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0x01ff, 0xff00, 0x007f, 0x0000, - 0x0000, 0x0000, 0x01f0, 0x0000, 0xff80, 0x00ff, 0xfe00, 0x01ff, 0x0000, - 0x0070, 0x0000, 0x07fc, 0x0000, 0xff80, 0x00ff, 0xf800, 0x03ff, 0x0000, - 0x07c0, 0x0000, 0x1fff, 0x0000, 0xffc0, 0x00ff, 0xf000, 0x07ff, 0x0000, - 0x7f80, 0xe000, 0x3fff, 0x0000, 0xffc0, 0x00ff, 0xc000, 0x0fff, 0x0000, - 0xfe00, 0xf803, 0x3fff, 0x0000, 0xffc0, 0x007f, 0x8000, 0x3fff, 0x0000, - 0xfc00, 0xfe3f, 0x1fff, 0x0000, 0xffc0, 0x007f, 0x0000, 0x7ffe, 0x0000, - 0xf000, 0xffff, 0x1fff, 0x0000, 0xffe0, 0x007f, 0x0000, 0xfffc, 0x0000, - 0xe000, 0xffff, 0x1fff, 0x0060, 0xffe0, 0x007f, 0x0000, 0xfff0, 0x0000, - 0x8000, 0xffff, 0x1fff, 0x01e0, 0xffe0, 0x003f, 0x0000, 0xffe0, 0x0001, - 0x0000, 0xffff, 0x0fff, 0x07f0, 0xfff0, 0x003f, 0x0000, 0xff80, 0x0003, - 0x0000, 0xfffc, 0x0fff, 0x1ff0, 0xfff0, 0x003f, 0x0000, 0xff00, 0x0003, - 0x0000, 0xfff8, 0x0fff, 0x7ff0, 0xfff0, 0x001f, 0x0000, 0xfe00, 0x0007, - 0x0000, 0xfff0, 0x0fff, 0xfff0, 0xfff0, 0x001f, 0x0000, 0xfc00, 0x0007, - 0x0000, 0xffc0, 0x0fff, 0xfff8, 0xffc3, 0x001f, 0x0000, 0xf800, 0x0007, - 0x0000, 0xffc0, 0x1fff, 0xfff8, 0xff0f, 0x001f, 0x0000, 0xf800, 0x0007, - 0x0000, 0xffc0, 0x3fff, 0xfff8, 0xfc3f, 0x000f, 0x0000, 0xf800, 0x0007, - 0x0000, 0xff80, 0x3fff, 0xfff8, 0xf8ff, 0x000f, 0x0000, 0xf800, 0x000f, - 0x0000, 0xff80, 0x3fff, 0xfffc, 0xe1ff, 0x000f, 0x0000, 0xf800, 0x000f, - 0x0000, 0xff00, 0x3fff, 0xfffc, 0x87ff, 0x000f, 0x0000, 0xf800, 0x000f, - 0x0000, 0xff00, 0x1fff, 0xfffc, 0x1fff, 0x0006, 0x0000, 0xf800, 0x000f, - 0x0000, 0xff00, 0x1fff, 0xfffc, 0x7fff, 0x0000, 0x0000, 0xf800, 0x000f, - 0x0000, 0xfe00, 0x1fff, 0xfffe, 0xffff, 0x0001, 0x0000, 0xf800, 0x000f, - 0x0000, 0xfe00, 0x0fff, 0xfffe, 0xffff, 0x0003, 0x0000, 0xf800, 0x000f, - 0x0000, 0xfc00, 0x01ff, 0xfffe, 0xffff, 0x000f, 0x0000, 0xf800, 0x000f, - 0x0000, 0xfc00, 0x01ff, 0xfffe, 0xffff, 0x003f, 0x0000, 0xf800, 0x000f, - 0x0000, 0xf800, 0x01ff, 0xffff, 0xffff, 0x00ff, 0x0000, 0xf800, 0x000f, - 0x0000, 0xf800, 0x00ff, 0xffff, 0xffff, 0x03ff, 0x0000, 0xf800, 0x000f, - 0x0000, 0xf800, 0x00ff, 0xffff, 0xffff, 0x0fff, 0x0000, 0xf800, 0x000f, - 0x0000, 0xf000, 0x00ff, 0xffff, 0xffff, 0x3fff, 0x0000, 0xf800, 0x000f, - 0x0000, 0xf000, 0x807f, 0xffff, 0xffff, 0x7fff, 0x0000, 0xf800, 0x000f, - 0x0000, 0xe000, 0x807f, 0xffff, 0xffff, 0xffff, 0x0001, 0xf800, 0x000f, - 0x0000, 0x8000, 0x807f, 0xffff, 0xffff, 0xffff, 0x0007, 0xfc00, 0x000f, - 0x0000, 0x0000, 0x807c, 0xffff, 0xffff, 0xffff, 0x001f, 0xfc00, 0x000f, - 0x0000, 0x0000, 0xc020, 0xffff, 0xffff, 0xffff, 0x007f, 0xfc00, 0x000f, - 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x00ff, 0xfc00, 0x000f, - 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x03ff, 0xfc00, 0x000f, - 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x0fff, 0xfc00, 0x000f, - 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0x3fff, 0xfc00, 0x000f, - 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xfc00, 0x000f, - 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xfc03, 0x000f, - 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xfe0f, 0x000f, - 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xfe1f, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0600, 0xc000, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x3e00, 0x0000, 0xfffe, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0xff00, 0x0001, 0xfff8, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0xff00, 0x000f, 0xffc0, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0xff00, 0x007f, 0xfe00, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0xff00, 0x03ff, 0xf000, 0xffff, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0xff80, 0x03ff, 0xc000, 0xffff, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0xff80, 0x03ff, 0x0000, 0xfffe, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0xff80, 0x03ff, 0x0000, 0xfff0, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0xffc0, 0x03ff, 0x0000, 0xff80, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0xffc0, 0x01ff, 0x0000, 0xfe00, 0xffff, 0x0003, - 0x0000, 0x0000, 0x0000, 0xffc0, 0x01ff, 0x0000, 0xf000, 0xffff, 0x0003, - 0x0000, 0x0000, 0x0000, 0xffc0, 0x01ff, 0x0000, 0x8000, 0xffff, 0x0001, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x01ff, 0x0000, 0x0000, 0xfffc, 0x0001, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x00ff, 0x0000, 0x0000, 0xfff0, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x00ff, 0x0000, 0x0000, 0x7f80, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x00ff, 0x0000, 0x0000, 0x0c00, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffc0, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff00, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfc00, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf000, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xc000, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x8000, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x000e, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1980, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x39c0, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3980, 0x0000, 0x0060, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3800, 0x0000, 0x0060, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3800, 0x0000, 0x00f0, - 0x0000, 0x03ff, 0x0ffc, 0x3ff0, 0xffc0, 0xff80, 0x3983, 0x7fe0, 0x07f8, - 0xc000, 0x07ff, 0x1fff, 0x7ffc, 0xfff0, 0xffc1, 0x39c7, 0xfff0, 0x07f9, - 0xe000, 0x8fff, 0x3fff, 0xfffe, 0xfff8, 0xffe3, 0x39cf, 0xfff8, 0x0063, - 0xe000, 0x9c00, 0x7003, 0xc00e, 0x8038, 0x00e3, 0x39ce, 0x801c, 0x0063, - 0x6000, 0x9c00, 0x7001, 0xc006, 0x0019, 0x0077, 0x39dc, 0x001c, 0x0063, - 0x6000, 0x9c00, 0x7001, 0xfffe, 0x0019, 0x0077, 0x39dc, 0x001c, 0x0063, - 0x6000, 0x9c00, 0x7001, 0xfffe, 0x0019, 0x0077, 0x39dc, 0x001c, 0x0063, - 0x6000, 0x9c00, 0x7001, 0x000e, 0x0018, 0x0077, 0x39dc, 0x001c, 0x0063, - 0xe000, 0x9c00, 0x7001, 0x000e, 0x0018, 0x0077, 0x39ce, 0x801c, 0x00e3, - 0xe000, 0xff8f, 0x3fff, 0x7ffe, 0x0018, 0xffff, 0xf1cf, 0xe0f8, 0x07e3, - 0xc000, 0xffff, 0x1fff, 0x7ffc, 0x0018, 0xffff, 0xf1c7, 0xfff0, 0x07c1, - 0x8000, 0xffff, 0x0fff, 0x7ff8, 0x0018, 0xffff, 0xc183, 0xffe0, 0x0780, - 0x0000, 0xc000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0060, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000 }; + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x001e, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0x01ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x07ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x0fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x7fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x0003, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x0007, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x000f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x001f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x07ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x0fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x1fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x3fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0x03ff, 0xffff, 0x0001, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0x03ff, 0xfffe, 0x0003, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0x01ff, 0xfff8, 0x000f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0x01ff, 0xffe0, 0x001f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0x01ff, 0xffc0, 0x003f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0x01ff, 0xff00, 0x007f, 0x0000, + 0x0000, 0x0000, 0x01f0, 0x0000, 0xff80, 0x00ff, 0xfe00, 0x01ff, 0x0000, + 0x0070, 0x0000, 0x07fc, 0x0000, 0xff80, 0x00ff, 0xf800, 0x03ff, 0x0000, + 0x07c0, 0x0000, 0x1fff, 0x0000, 0xffc0, 0x00ff, 0xf000, 0x07ff, 0x0000, + 0x7f80, 0xe000, 0x3fff, 0x0000, 0xffc0, 0x00ff, 0xc000, 0x0fff, 0x0000, + 0xfe00, 0xf803, 0x3fff, 0x0000, 0xffc0, 0x007f, 0x8000, 0x3fff, 0x0000, + 0xfc00, 0xfe3f, 0x1fff, 0x0000, 0xffc0, 0x007f, 0x0000, 0x7ffe, 0x0000, + 0xf000, 0xffff, 0x1fff, 0x0000, 0xffe0, 0x007f, 0x0000, 0xfffc, 0x0000, + 0xe000, 0xffff, 0x1fff, 0x0060, 0xffe0, 0x007f, 0x0000, 0xfff0, 0x0000, + 0x8000, 0xffff, 0x1fff, 0x01e0, 0xffe0, 0x003f, 0x0000, 0xffe0, 0x0001, + 0x0000, 0xffff, 0x0fff, 0x07f0, 0xfff0, 0x003f, 0x0000, 0xff80, 0x0003, + 0x0000, 0xfffc, 0x0fff, 0x1ff0, 0xfff0, 0x003f, 0x0000, 0xff00, 0x0003, + 0x0000, 0xfff8, 0x0fff, 0x7ff0, 0xfff0, 0x001f, 0x0000, 0xfe00, 0x0007, + 0x0000, 0xfff0, 0x0fff, 0xfff0, 0xfff0, 0x001f, 0x0000, 0xfc00, 0x0007, + 0x0000, 0xffc0, 0x0fff, 0xfff8, 0xffc3, 0x001f, 0x0000, 0xf800, 0x0007, + 0x0000, 0xffc0, 0x1fff, 0xfff8, 0xff0f, 0x001f, 0x0000, 0xf800, 0x0007, + 0x0000, 0xffc0, 0x3fff, 0xfff8, 0xfc3f, 0x000f, 0x0000, 0xf800, 0x0007, + 0x0000, 0xff80, 0x3fff, 0xfff8, 0xf8ff, 0x000f, 0x0000, 0xf800, 0x000f, + 0x0000, 0xff80, 0x3fff, 0xfffc, 0xe1ff, 0x000f, 0x0000, 0xf800, 0x000f, + 0x0000, 0xff00, 0x3fff, 0xfffc, 0x87ff, 0x000f, 0x0000, 0xf800, 0x000f, + 0x0000, 0xff00, 0x1fff, 0xfffc, 0x1fff, 0x0006, 0x0000, 0xf800, 0x000f, + 0x0000, 0xff00, 0x1fff, 0xfffc, 0x7fff, 0x0000, 0x0000, 0xf800, 0x000f, + 0x0000, 0xfe00, 0x1fff, 0xfffe, 0xffff, 0x0001, 0x0000, 0xf800, 0x000f, + 0x0000, 0xfe00, 0x0fff, 0xfffe, 0xffff, 0x0003, 0x0000, 0xf800, 0x000f, + 0x0000, 0xfc00, 0x01ff, 0xfffe, 0xffff, 0x000f, 0x0000, 0xf800, 0x000f, + 0x0000, 0xfc00, 0x01ff, 0xfffe, 0xffff, 0x003f, 0x0000, 0xf800, 0x000f, + 0x0000, 0xf800, 0x01ff, 0xffff, 0xffff, 0x00ff, 0x0000, 0xf800, 0x000f, + 0x0000, 0xf800, 0x00ff, 0xffff, 0xffff, 0x03ff, 0x0000, 0xf800, 0x000f, + 0x0000, 0xf800, 0x00ff, 0xffff, 0xffff, 0x0fff, 0x0000, 0xf800, 0x000f, + 0x0000, 0xf000, 0x00ff, 0xffff, 0xffff, 0x3fff, 0x0000, 0xf800, 0x000f, + 0x0000, 0xf000, 0x807f, 0xffff, 0xffff, 0x7fff, 0x0000, 0xf800, 0x000f, + 0x0000, 0xe000, 0x807f, 0xffff, 0xffff, 0xffff, 0x0001, 0xf800, 0x000f, + 0x0000, 0x8000, 0x807f, 0xffff, 0xffff, 0xffff, 0x0007, 0xfc00, 0x000f, + 0x0000, 0x0000, 0x807c, 0xffff, 0xffff, 0xffff, 0x001f, 0xfc00, 0x000f, + 0x0000, 0x0000, 0xc020, 0xffff, 0xffff, 0xffff, 0x007f, 0xfc00, 0x000f, + 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x00ff, 0xfc00, 0x000f, + 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x03ff, 0xfc00, 0x000f, + 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x0fff, 0xfc00, 0x000f, + 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0x3fff, 0xfc00, 0x000f, + 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xfc00, 0x000f, + 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xfc03, 0x000f, + 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xfe0f, 0x000f, + 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xfe1f, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0600, 0xc000, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x3e00, 0x0000, 0xfffe, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0xff00, 0x0001, 0xfff8, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0xff00, 0x000f, 0xffc0, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0xff00, 0x007f, 0xfe00, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0xff00, 0x03ff, 0xf000, 0xffff, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0xff80, 0x03ff, 0xc000, 0xffff, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0xff80, 0x03ff, 0x0000, 0xfffe, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0xff80, 0x03ff, 0x0000, 0xfff0, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0xffc0, 0x03ff, 0x0000, 0xff80, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0xffc0, 0x01ff, 0x0000, 0xfe00, 0xffff, 0x0003, + 0x0000, 0x0000, 0x0000, 0xffc0, 0x01ff, 0x0000, 0xf000, 0xffff, 0x0003, + 0x0000, 0x0000, 0x0000, 0xffc0, 0x01ff, 0x0000, 0x8000, 0xffff, 0x0001, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x01ff, 0x0000, 0x0000, 0xfffc, 0x0001, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x00ff, 0x0000, 0x0000, 0xfff0, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x00ff, 0x0000, 0x0000, 0x7f80, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x00ff, 0x0000, 0x0000, 0x0c00, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffc0, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff00, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfc00, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf000, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xc000, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x8000, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x000e, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1980, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x39c0, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3980, 0x0000, 0x0060, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3800, 0x0000, 0x0060, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3800, 0x0000, 0x00f0, + 0x0000, 0x03ff, 0x0ffc, 0x3ff0, 0xffc0, 0xff80, 0x3983, 0x7fe0, 0x07f8, + 0xc000, 0x07ff, 0x1fff, 0x7ffc, 0xfff0, 0xffc1, 0x39c7, 0xfff0, 0x07f9, + 0xe000, 0x8fff, 0x3fff, 0xfffe, 0xfff8, 0xffe3, 0x39cf, 0xfff8, 0x0063, + 0xe000, 0x9c00, 0x7003, 0xc00e, 0x8038, 0x00e3, 0x39ce, 0x801c, 0x0063, + 0x6000, 0x9c00, 0x7001, 0xc006, 0x0019, 0x0077, 0x39dc, 0x001c, 0x0063, + 0x6000, 0x9c00, 0x7001, 0xfffe, 0x0019, 0x0077, 0x39dc, 0x001c, 0x0063, + 0x6000, 0x9c00, 0x7001, 0xfffe, 0x0019, 0x0077, 0x39dc, 0x001c, 0x0063, + 0x6000, 0x9c00, 0x7001, 0x000e, 0x0018, 0x0077, 0x39dc, 0x001c, 0x0063, + 0xe000, 0x9c00, 0x7001, 0x000e, 0x0018, 0x0077, 0x39ce, 0x801c, 0x00e3, + 0xe000, 0xff8f, 0x3fff, 0x7ffe, 0x0018, 0xffff, 0xf1cf, 0xe0f8, 0x07e3, + 0xc000, 0xffff, 0x1fff, 0x7ffc, 0x0018, 0xffff, 0xf1c7, 0xfff0, 0x07c1, + 0x8000, 0xffff, 0x0fff, 0x7ff8, 0x0018, 0xffff, 0xc183, 0xffe0, 0x0780, + 0x0000, 0xc000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0060, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000 +}; static const unsigned short oplogo_mask_bits[] = { - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x003f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x03ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x0fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0x1fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0x7fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x0001, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x0007, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x000f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x001f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x003f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x03ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x0fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x1fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x3fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x7fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0001, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0003, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0007, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x001f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0x003f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0x8fff, 0xffff, 0x007f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0x07ff, 0xffff, 0x00ff, 0x0000, - 0x0000, 0x0000, 0x03f8, 0x0000, 0xffe0, 0x07ff, 0xfffc, 0x03ff, 0x0000, - 0x00f8, 0x0000, 0x0ffe, 0x0000, 0xffe0, 0x07ff, 0xfff0, 0x07ff, 0x0000, - 0x0ffc, 0x8000, 0x3fff, 0x0000, 0xffe0, 0x07ff, 0xffe0, 0x0fff, 0x0000, - 0xfffc, 0xf000, 0x7fff, 0x0000, 0xfff0, 0x03ff, 0xff80, 0x1fff, 0x0000, - 0xfffc, 0xfc07, 0xffff, 0x0000, 0xfff0, 0x03ff, 0xff00, 0x7fff, 0x0000, - 0xfff8, 0xff7f, 0xffff, 0x0000, 0xfff0, 0x03ff, 0xfc00, 0xffff, 0x0000, - 0xffe0, 0xffff, 0xffff, 0x0000, 0xfff0, 0x03ff, 0xf800, 0xffff, 0x0001, - 0xffc0, 0xffff, 0xffff, 0x00f0, 0xfff8, 0x01ff, 0xe000, 0xffff, 0x0003, - 0xff00, 0xffff, 0x7fff, 0x03f8, 0xfff8, 0x01ff, 0xc000, 0xffff, 0x0003, - 0xfe00, 0xffff, 0x7fff, 0x0ff8, 0xfff8, 0x01ff, 0x0000, 0xffff, 0x0007, - 0xf800, 0xffff, 0x7fff, 0x3ffc, 0xfffc, 0x01ff, 0x0000, 0xfffe, 0x000f, - 0xf000, 0xffff, 0x7fff, 0xfffc, 0xfffc, 0x00ff, 0x0000, 0xfff8, 0x000f, - 0xc000, 0xffff, 0x3fff, 0xfffc, 0xfffd, 0x00ff, 0x0000, 0xfff0, 0x001f, - 0x8000, 0xffff, 0x3fff, 0xfffc, 0xffff, 0x00ff, 0x0000, 0xffc0, 0x001f, - 0x0000, 0xfffe, 0x3fff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xff80, 0x001f, - 0x0000, 0xfffc, 0x7fff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xff00, 0x001f, - 0x0000, 0xfff8, 0xffff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xfe00, 0x001f, - 0x0000, 0xfff0, 0xffff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xff80, 0xbfff, 0xffff, 0xffff, 0x01ff, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xff00, 0xdfff, 0xffff, 0xffff, 0x07ff, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xff00, 0xc7ff, 0xffff, 0xffff, 0x1fff, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xfe00, 0xc7ff, 0xffff, 0xffff, 0x7fff, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xfe00, 0xc3ff, 0xffff, 0xffff, 0xffff, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xfe00, 0xe3ff, 0xffff, 0xffff, 0xffff, 0x0003, 0xfe00, 0x003f, - 0x0000, 0xfc00, 0xe3ff, 0xffff, 0xffff, 0xffff, 0x000f, 0xfe00, 0x003f, - 0x0000, 0xfc00, 0xe1ff, 0xffff, 0xffff, 0xffff, 0x003f, 0xff00, 0x003f, - 0x0000, 0xf800, 0xe1ff, 0xffff, 0xffff, 0xffff, 0x00ff, 0xff00, 0x003f, - 0x0000, 0xf000, 0xf1ff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xff00, 0x003f, - 0x0000, 0xc000, 0xf1ff, 0xffff, 0xffff, 0xffff, 0x07ff, 0xff00, 0x003f, - 0x0000, 0x0000, 0xf0fe, 0xffff, 0xffff, 0xffff, 0x1fff, 0xff00, 0x003f, - 0x0000, 0x0000, 0xf070, 0xffff, 0xffff, 0xffff, 0x7fff, 0xff00, 0x003f, - 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0xff01, 0x003f, - 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0xff07, 0x003f, - 0x0000, 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, 0xff1f, 0x003f, - 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0xffff, 0xffbf, 0x003f, - 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0x0f00, 0xfffc, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0x7f80, 0xffe0, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xff80, 0xff83, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xffc0, 0xfc1f, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xffc0, 0xe0ff, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xffc0, 0x07ff, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xffc0, 0x0fff, 0xfffc, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x0fff, 0xffe0, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x0fff, 0xff00, 0xffff, 0xffff, 0x001f, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x0fff, 0xf800, 0xffff, 0xffff, 0x001f, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x0fff, 0xe000, 0xffff, 0xffff, 0x001f, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x0fff, 0x0000, 0xffff, 0xffff, 0x001f, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x0fff, 0x0000, 0xfff8, 0xffff, 0x001f, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x07ff, 0x0000, 0xffc0, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x07ff, 0x0000, 0xff00, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x07ff, 0x0000, 0xf800, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x07ff, 0x0000, 0xc000, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0xfffc, 0x03ff, 0x0000, 0x0000, 0xfffe, 0x0003, - 0x0000, 0x0000, 0x0000, 0xfffc, 0x03ff, 0x0000, 0x0000, 0xfff8, 0x0001, - 0x0000, 0x0000, 0x0000, 0xfffc, 0x03ff, 0x0000, 0x0000, 0xffc0, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x1e00, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffc, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfe00, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf800, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe000, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xc000, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fc0, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7fe0, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x0000, 0x00f0, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x0000, 0x01f8, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x0000, 0x01f8, - 0x8000, 0x07ff, 0x1ffe, 0x7ff8, 0xffe0, 0xffc1, 0xffe7, 0xfff0, 0x0ffc, - 0xe000, 0x8fff, 0x3fff, 0xfffe, 0xfff8, 0xffe3, 0xffef, 0xfff8, 0x1fff, - 0xf000, 0xdfff, 0x7fff, 0xffff, 0xfffd, 0xfff7, 0xffff, 0xfffc, 0x1fff, - 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xfffe, 0x1fff, - 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, - 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, - 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, - 0xf800, 0xff01, 0xfc07, 0xffff, 0xc07f, 0x01ff, 0xffff, 0xc07f, 0x01ff, - 0xf800, 0xff01, 0xfc07, 0xffff, 0xc07f, 0x01ff, 0xffff, 0xc07f, 0x01ff, - 0xf800, 0xffdf, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xf1ff, 0x0fff, - 0xf800, 0xffff, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1fff, - 0xf800, 0xffff, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1fff, - 0xf800, 0xffff, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1fff, - 0xf000, 0xffff, 0x7fff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1ff7, - 0xe000, 0xffff, 0x3fff, 0xfffe, 0xc07f, 0xffff, 0xffef, 0xfffb, 0x1fe3, - 0xc000, 0xffff, 0x1fff, 0xfffc, 0x803c, 0xffff, 0xe3c7, 0xfff1, 0x0fc1, - 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01fc, 0x0000, 0x0000, 0x0000, - 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01fc, 0x0000, 0x0000, 0x0000, - 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01fc, 0x0000, 0x0000, 0x0000, - 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01f8, 0x0000, 0x0000, 0x0000, - 0x0000, 0xc000, 0x0003, 0x0000, 0x0000, 0x00f0, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000 }; + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x003f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x03ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x0fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0x1fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0x7fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x0001, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x0007, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x000f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x001f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x003f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x03ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x0fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x1fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x3fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x7fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0001, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0003, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0007, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x001f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0x003f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0x8fff, 0xffff, 0x007f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0x07ff, 0xffff, 0x00ff, 0x0000, + 0x0000, 0x0000, 0x03f8, 0x0000, 0xffe0, 0x07ff, 0xfffc, 0x03ff, 0x0000, + 0x00f8, 0x0000, 0x0ffe, 0x0000, 0xffe0, 0x07ff, 0xfff0, 0x07ff, 0x0000, + 0x0ffc, 0x8000, 0x3fff, 0x0000, 0xffe0, 0x07ff, 0xffe0, 0x0fff, 0x0000, + 0xfffc, 0xf000, 0x7fff, 0x0000, 0xfff0, 0x03ff, 0xff80, 0x1fff, 0x0000, + 0xfffc, 0xfc07, 0xffff, 0x0000, 0xfff0, 0x03ff, 0xff00, 0x7fff, 0x0000, + 0xfff8, 0xff7f, 0xffff, 0x0000, 0xfff0, 0x03ff, 0xfc00, 0xffff, 0x0000, + 0xffe0, 0xffff, 0xffff, 0x0000, 0xfff0, 0x03ff, 0xf800, 0xffff, 0x0001, + 0xffc0, 0xffff, 0xffff, 0x00f0, 0xfff8, 0x01ff, 0xe000, 0xffff, 0x0003, + 0xff00, 0xffff, 0x7fff, 0x03f8, 0xfff8, 0x01ff, 0xc000, 0xffff, 0x0003, + 0xfe00, 0xffff, 0x7fff, 0x0ff8, 0xfff8, 0x01ff, 0x0000, 0xffff, 0x0007, + 0xf800, 0xffff, 0x7fff, 0x3ffc, 0xfffc, 0x01ff, 0x0000, 0xfffe, 0x000f, + 0xf000, 0xffff, 0x7fff, 0xfffc, 0xfffc, 0x00ff, 0x0000, 0xfff8, 0x000f, + 0xc000, 0xffff, 0x3fff, 0xfffc, 0xfffd, 0x00ff, 0x0000, 0xfff0, 0x001f, + 0x8000, 0xffff, 0x3fff, 0xfffc, 0xffff, 0x00ff, 0x0000, 0xffc0, 0x001f, + 0x0000, 0xfffe, 0x3fff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xff80, 0x001f, + 0x0000, 0xfffc, 0x7fff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xff00, 0x001f, + 0x0000, 0xfff8, 0xffff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xfe00, 0x001f, + 0x0000, 0xfff0, 0xffff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xff80, 0xbfff, 0xffff, 0xffff, 0x01ff, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xff00, 0xdfff, 0xffff, 0xffff, 0x07ff, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xff00, 0xc7ff, 0xffff, 0xffff, 0x1fff, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xfe00, 0xc7ff, 0xffff, 0xffff, 0x7fff, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xfe00, 0xc3ff, 0xffff, 0xffff, 0xffff, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xfe00, 0xe3ff, 0xffff, 0xffff, 0xffff, 0x0003, 0xfe00, 0x003f, + 0x0000, 0xfc00, 0xe3ff, 0xffff, 0xffff, 0xffff, 0x000f, 0xfe00, 0x003f, + 0x0000, 0xfc00, 0xe1ff, 0xffff, 0xffff, 0xffff, 0x003f, 0xff00, 0x003f, + 0x0000, 0xf800, 0xe1ff, 0xffff, 0xffff, 0xffff, 0x00ff, 0xff00, 0x003f, + 0x0000, 0xf000, 0xf1ff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xff00, 0x003f, + 0x0000, 0xc000, 0xf1ff, 0xffff, 0xffff, 0xffff, 0x07ff, 0xff00, 0x003f, + 0x0000, 0x0000, 0xf0fe, 0xffff, 0xffff, 0xffff, 0x1fff, 0xff00, 0x003f, + 0x0000, 0x0000, 0xf070, 0xffff, 0xffff, 0xffff, 0x7fff, 0xff00, 0x003f, + 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0xff01, 0x003f, + 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0xff07, 0x003f, + 0x0000, 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, 0xff1f, 0x003f, + 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0xffff, 0xffbf, 0x003f, + 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0x0f00, 0xfffc, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0x7f80, 0xffe0, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xff80, 0xff83, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xffc0, 0xfc1f, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xffc0, 0xe0ff, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xffc0, 0x07ff, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xffc0, 0x0fff, 0xfffc, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x0fff, 0xffe0, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x0fff, 0xff00, 0xffff, 0xffff, 0x001f, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x0fff, 0xf800, 0xffff, 0xffff, 0x001f, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x0fff, 0xe000, 0xffff, 0xffff, 0x001f, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x0fff, 0x0000, 0xffff, 0xffff, 0x001f, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x0fff, 0x0000, 0xfff8, 0xffff, 0x001f, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x07ff, 0x0000, 0xffc0, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x07ff, 0x0000, 0xff00, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x07ff, 0x0000, 0xf800, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x07ff, 0x0000, 0xc000, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0xfffc, 0x03ff, 0x0000, 0x0000, 0xfffe, 0x0003, + 0x0000, 0x0000, 0x0000, 0xfffc, 0x03ff, 0x0000, 0x0000, 0xfff8, 0x0001, + 0x0000, 0x0000, 0x0000, 0xfffc, 0x03ff, 0x0000, 0x0000, 0xffc0, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x1e00, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffc, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfe00, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf800, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe000, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xc000, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fc0, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7fe0, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x0000, 0x00f0, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x0000, 0x01f8, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x0000, 0x01f8, + 0x8000, 0x07ff, 0x1ffe, 0x7ff8, 0xffe0, 0xffc1, 0xffe7, 0xfff0, 0x0ffc, + 0xe000, 0x8fff, 0x3fff, 0xfffe, 0xfff8, 0xffe3, 0xffef, 0xfff8, 0x1fff, + 0xf000, 0xdfff, 0x7fff, 0xffff, 0xfffd, 0xfff7, 0xffff, 0xfffc, 0x1fff, + 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xfffe, 0x1fff, + 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, + 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, + 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, + 0xf800, 0xff01, 0xfc07, 0xffff, 0xc07f, 0x01ff, 0xffff, 0xc07f, 0x01ff, + 0xf800, 0xff01, 0xfc07, 0xffff, 0xc07f, 0x01ff, 0xffff, 0xc07f, 0x01ff, + 0xf800, 0xffdf, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xf1ff, 0x0fff, + 0xf800, 0xffff, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1fff, + 0xf800, 0xffff, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1fff, + 0xf800, 0xffff, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1fff, + 0xf000, 0xffff, 0x7fff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1ff7, + 0xe000, 0xffff, 0x3fff, 0xfffe, 0xc07f, 0xffff, 0xffef, 0xfffb, 0x1fe3, + 0xc000, 0xffff, 0x1fff, 0xfffc, 0x803c, 0xffff, 0xe3c7, 0xfff1, 0x0fc1, + 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01fc, 0x0000, 0x0000, 0x0000, + 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01fc, 0x0000, 0x0000, 0x0000, + 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01fc, 0x0000, 0x0000, 0x0000, + 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01f8, 0x0000, 0x0000, 0x0000, + 0x0000, 0xc000, 0x0003, 0x0000, 0x0000, 0x00f0, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000 +}; -#define level_width 144 +#define level_width 144 #define level_height 129 static const unsigned short level_bits[] = { - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf800, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xe000, 0xffff, 0x1e01, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfe00, 0x7fef, 0x7ff8, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff00, 0x83c0, 0xffff, 0x0001, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x3f80, 0xcffe, 0xfffd, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x9fc0, 0xffff, 0xb07f, 0x001f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xcfe0, 0xffff, 0xcfce, 0x007e, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xe7f8, 0xffff, 0x9ff7, 0x00fd, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf7fc, 0xffe7, 0xbff7, 0x01ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf3fe, 0xfffb, 0xb9ff, 0x03ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfbff, 0xfe03, 0xb9f7, 0x07ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0xf9ff, 0xebfd, 0xb9e7, 0x0fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xc000, 0xffff, 0xd7fe, 0xcfdf, 0x0fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xe000, 0x7fff, 0xf7ff, 0xf037, 0x1fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xe000, 0x7fff, 0xf7ff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf000, 0xefff, 0xf73e, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf000, 0xefff, 0xf73e, 0xefff, 0x7fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf000, 0xefff, 0xf939, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf800, 0x87ff, 0xfe07, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf800, 0xffff, 0xdffe, 0xffff, 0xffdf, 0x0001, 0x0000, 0x0000, - 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xefff, 0x1fff, 0xfff8, 0x0003, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0x81ff, 0xfff1, 0x0007, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0xfc3f, 0xfff3, 0x000f, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0xff9f, 0xfff0, 0x000f, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0x3fcf, 0xfff6, 0x001f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0x0ee7, 0xfff7, 0x003f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xe003, 0xfff7, 0x003f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xf371, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xfff9, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xfffe, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0x7fff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0x7fff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xdfff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xdfff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xfdff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xbfff, 0xffff, 0xfdff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0x7fff, 0xfffb, 0xfdff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xfeff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xf3ff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xcfff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xbeff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xfeff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x7fff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xbfff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xbeff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xdeff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xdeff, 0x003f, 0x0000, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xefff, 0x003f, 0x0000, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xfb7f, 0x003f, 0x0000, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffbf, 0x003f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffbf, 0x003f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffdf, 0x013f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffef, 0x003f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0x3fff, 0xfff9, 0x003f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, - 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, - 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, - 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, - 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000 }; + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf800, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xe000, 0xffff, 0x1e01, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfe00, 0x7fef, 0x7ff8, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff00, 0x83c0, 0xffff, 0x0001, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x3f80, 0xcffe, 0xfffd, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x9fc0, 0xffff, 0xb07f, 0x001f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xcfe0, 0xffff, 0xcfce, 0x007e, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xe7f8, 0xffff, 0x9ff7, 0x00fd, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf7fc, 0xffe7, 0xbff7, 0x01ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf3fe, 0xfffb, 0xb9ff, 0x03ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfbff, 0xfe03, 0xb9f7, 0x07ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0xf9ff, 0xebfd, 0xb9e7, 0x0fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xc000, 0xffff, 0xd7fe, 0xcfdf, 0x0fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xe000, 0x7fff, 0xf7ff, 0xf037, 0x1fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xe000, 0x7fff, 0xf7ff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf000, 0xefff, 0xf73e, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf000, 0xefff, 0xf73e, 0xefff, 0x7fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf000, 0xefff, 0xf939, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf800, 0x87ff, 0xfe07, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf800, 0xffff, 0xdffe, 0xffff, 0xffdf, 0x0001, 0x0000, 0x0000, + 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xefff, 0x1fff, 0xfff8, 0x0003, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0x81ff, 0xfff1, 0x0007, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0xfc3f, 0xfff3, 0x000f, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0xff9f, 0xfff0, 0x000f, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0x3fcf, 0xfff6, 0x001f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0x0ee7, 0xfff7, 0x003f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xe003, 0xfff7, 0x003f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xf371, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xfff9, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xfffe, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0x7fff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0x7fff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xdfff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xdfff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xfdff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xbfff, 0xffff, 0xfdff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0x7fff, 0xfffb, 0xfdff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xfeff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xf3ff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xcfff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xbeff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xfeff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x7fff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xbfff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xbeff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xdeff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xdeff, 0x003f, 0x0000, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xefff, 0x003f, 0x0000, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xfb7f, 0x003f, 0x0000, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffbf, 0x003f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffbf, 0x003f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffdf, 0x013f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffef, 0x003f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0x3fff, 0xfff9, 0x003f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, + 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, + 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, + 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, + 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000 +}; -#define level_mask_width 144 +#define level_mask_width 144 #define level_mask_height 129 static const unsigned short level_mask_bits[] = { - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7c00, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0200, 0xfe00, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0740, 0xdc00, 0x0001, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x03fe, 0xce00, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x01ff, 0xde1f, 0x0001, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x802e, 0xfe1f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x4000, 0x0044, 0xc000, 0xc007, 0x7c1d, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfee8, 0x00ff, 0x8000, 0xc00f, 0x0e38, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffff, 0x17ff, 0x0000, 0xc3df, 0x1c3f, 0x0000, - 0x0000, 0x0000, 0xa000, 0xffff, 0x3fff, 0x0000, 0xe3ff, 0x0e3f, 0x0000, - 0x0000, 0x0000, 0xff00, 0xffff, 0x7fff, 0x0000, 0xc07f, 0x1c7d, 0x0000, - 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0000, 0xe03e, 0x0e30, 0x0000, - 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0x0017, 0xc03e, 0x1c71, 0x0000, - 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0x001f, 0x8038, 0x0020, 0x0000, - 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0x007f, 0x003c, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0x00ff, 0x0020, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0150, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x03f0, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x1ff8, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x1ef8, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x147c, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0038, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x707c, 0x0054, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0xf0fc, 0x00fc, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xf1fc, 0x01fc, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xf80e, 0x00ee, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xf00e, 0x00fc, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0xf808, 0x00fe, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x7800, 0x007e, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xe000, 0x000e, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x4000, 0x0006, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x000e, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0004, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0020, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0054, 0x007c, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x00f8, 0x00f8, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x117c, 0x01fc, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x3818, 0x00fc, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x7c5c, 0x00fc, - 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0xfef8, 0x000c, - 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x7ff8, 0x001c, - 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0xfe38, 0x000c, - 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x7e30, 0x001c, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0xee30, 0x000c, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x5e70, 0x001c, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0e20, 0x000c, - 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0c60, 0x000c, - 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0800, 0x0000, - 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x3c00, 0xfffe, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, - 0xd000, 0x7e41, 0xfffc, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, - 0xf800, 0xeee3, 0xfff8, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, - 0xff00, 0x6cfd, 0xfff0, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, 0x0000, - 0x3f80, 0xecf8, 0xff80, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, - 0x3f00, 0x7fdc, 0xff00, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, - 0xbe00, 0x3ffb, 0xa000, 0xbfbb, 0x2bbf, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfe00, 0x1ffb, 0x0000, 0x1f00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfc00, 0x1fbb, 0x0000, 0x3e00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x7c00, 0x1d39, 0x0000, 0x7c00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x3800, 0x1838, 0x0000, 0xf800, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x7000, 0x0018, 0x0000, 0xfc00, 0x0001, 0x07c0, 0x0000, 0x0000, 0x0000, - 0x6000, 0x0000, 0x0000, 0xf800, 0x0003, 0x0e80, 0x0000, 0x0000, 0x0000, - 0x7000, 0x0000, 0x0100, 0xfc00, 0x0007, 0x1d00, 0x0000, 0x0000, 0x0000, - 0x6000, 0x0000, 0x0080, 0x7800, 0x000e, 0x3800, 0x0000, 0x0000, 0x0000, - 0x4000, 0x0000, 0x00c0, 0xfc00, 0x001c, 0x7000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x00e0, 0xc800, 0x0038, 0xe000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x1060, 0xdc00, 0x0071, 0x4000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x3860, 0x8800, 0x00e0, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x1c60, 0x9c00, 0x01c1, 0x1c00, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0860, 0x8800, 0x0383, 0x3800, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x9c40, 0x1c01, 0x1f03, 0x7100, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x8e00, 0x1801, 0x1a03, 0x6300, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xcc00, 0x1c00, 0x3c07, 0x4700, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xec00, 0x0800, 0x2002, 0xc600, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x7000, 0x1c00, 0x3007, 0xc600, 0x0001, 0x0000, 0x0000, - 0x0000, 0x0000, 0x6000, 0x0c00, 0x3002, 0x8c00, 0x0201, 0x0000, 0x0000, - 0x0000, 0x0000, 0x6000, 0x0c00, 0x3006, 0x9c00, 0x0701, 0x0000, 0x0000, - 0x0000, 0x0000, 0xe000, 0x0e00, 0x2002, 0x1800, 0x0600, 0x0000, 0x0000, - 0x0000, 0x0000, 0x4000, 0x0700, 0x3007, 0x3800, 0x0700, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0300, 0x3802, 0x3800, 0x0600, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0780, 0x1806, 0x1000, 0x0400, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0380, 0x0806, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x01c0, 0x1c06, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0180, 0x0806, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x05c0, 0x1c06, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0f80, 0x080e, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x1f00, 0x0406, 0x0000, 0x0000, 0x0000, 0x0000 }; + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7c00, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0200, 0xfe00, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0740, 0xdc00, 0x0001, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x03fe, 0xce00, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x01ff, 0xde1f, 0x0001, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x802e, 0xfe1f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x4000, 0x0044, 0xc000, 0xc007, 0x7c1d, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfee8, 0x00ff, 0x8000, 0xc00f, 0x0e38, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffff, 0x17ff, 0x0000, 0xc3df, 0x1c3f, 0x0000, + 0x0000, 0x0000, 0xa000, 0xffff, 0x3fff, 0x0000, 0xe3ff, 0x0e3f, 0x0000, + 0x0000, 0x0000, 0xff00, 0xffff, 0x7fff, 0x0000, 0xc07f, 0x1c7d, 0x0000, + 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0000, 0xe03e, 0x0e30, 0x0000, + 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0x0017, 0xc03e, 0x1c71, 0x0000, + 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0x001f, 0x8038, 0x0020, 0x0000, + 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0x007f, 0x003c, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0x00ff, 0x0020, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0150, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x03f0, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x1ff8, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x1ef8, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x147c, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0038, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x707c, 0x0054, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0xf0fc, 0x00fc, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xf1fc, 0x01fc, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xf80e, 0x00ee, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xf00e, 0x00fc, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0xf808, 0x00fe, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x7800, 0x007e, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xe000, 0x000e, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x4000, 0x0006, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x000e, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0004, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0020, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0054, 0x007c, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x00f8, 0x00f8, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x117c, 0x01fc, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x3818, 0x00fc, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x7c5c, 0x00fc, + 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0xfef8, 0x000c, + 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x7ff8, 0x001c, + 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0xfe38, 0x000c, + 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x7e30, 0x001c, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0xee30, 0x000c, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x5e70, 0x001c, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0e20, 0x000c, + 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0c60, 0x000c, + 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0800, 0x0000, + 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x3c00, 0xfffe, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, + 0xd000, 0x7e41, 0xfffc, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, + 0xf800, 0xeee3, 0xfff8, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, + 0xff00, 0x6cfd, 0xfff0, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, 0x0000, + 0x3f80, 0xecf8, 0xff80, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, + 0x3f00, 0x7fdc, 0xff00, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, + 0xbe00, 0x3ffb, 0xa000, 0xbfbb, 0x2bbf, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfe00, 0x1ffb, 0x0000, 0x1f00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfc00, 0x1fbb, 0x0000, 0x3e00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x7c00, 0x1d39, 0x0000, 0x7c00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x3800, 0x1838, 0x0000, 0xf800, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x7000, 0x0018, 0x0000, 0xfc00, 0x0001, 0x07c0, 0x0000, 0x0000, 0x0000, + 0x6000, 0x0000, 0x0000, 0xf800, 0x0003, 0x0e80, 0x0000, 0x0000, 0x0000, + 0x7000, 0x0000, 0x0100, 0xfc00, 0x0007, 0x1d00, 0x0000, 0x0000, 0x0000, + 0x6000, 0x0000, 0x0080, 0x7800, 0x000e, 0x3800, 0x0000, 0x0000, 0x0000, + 0x4000, 0x0000, 0x00c0, 0xfc00, 0x001c, 0x7000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x00e0, 0xc800, 0x0038, 0xe000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x1060, 0xdc00, 0x0071, 0x4000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x3860, 0x8800, 0x00e0, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x1c60, 0x9c00, 0x01c1, 0x1c00, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0860, 0x8800, 0x0383, 0x3800, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x9c40, 0x1c01, 0x1f03, 0x7100, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x8e00, 0x1801, 0x1a03, 0x6300, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xcc00, 0x1c00, 0x3c07, 0x4700, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xec00, 0x0800, 0x2002, 0xc600, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x7000, 0x1c00, 0x3007, 0xc600, 0x0001, 0x0000, 0x0000, + 0x0000, 0x0000, 0x6000, 0x0c00, 0x3002, 0x8c00, 0x0201, 0x0000, 0x0000, + 0x0000, 0x0000, 0x6000, 0x0c00, 0x3006, 0x9c00, 0x0701, 0x0000, 0x0000, + 0x0000, 0x0000, 0xe000, 0x0e00, 0x2002, 0x1800, 0x0600, 0x0000, 0x0000, + 0x0000, 0x0000, 0x4000, 0x0700, 0x3007, 0x3800, 0x0700, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0300, 0x3802, 0x3800, 0x0600, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0780, 0x1806, 0x1000, 0x0400, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0380, 0x0806, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x01c0, 0x1c06, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0180, 0x0806, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x05c0, 0x1c06, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0f80, 0x080e, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x1f00, 0x0406, 0x0000, 0x0000, 0x0000, 0x0000 +}; -#define llama_width 240 +#define llama_width 240 #define llama_height 260 static const unsigned short llama_bits[] = { - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8400, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0800, 0x0000, 0x0004, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0680, 0xf000, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0600, 0xe000, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0700, 0xe000, 0x021f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x1780, 0xc000, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x07c0, 0x8000, 0x21ff, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x07c0, 0x8000, 0x07ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0fe0, 0x0000, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x07e4, 0x0000, 0x3ffe, - 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0ff0, 0x0000, 0x7ffc, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x07f0, 0x0000, 0xfff8, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2ff0, 0x0000, 0xfff1, - 0x0023, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0ff8, 0x0000, 0xfff0, 0x0007, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0ff8, 0x0000, 0xffe0, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0ff8, 0x0000, 0xffc0, - 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0ff8, 0x0000, 0xff80, 0x003f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0ffc, 0x0000, 0xff00, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0ffc, 0x0000, 0xfe00, - 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x4ff8, 0x0000, 0xfc00, 0x01ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0dfc, 0x0000, 0xfc80, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1bf8, 0x0000, 0xf800, - 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x93f8, 0x0000, 0xf000, 0x03ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x17f8, 0x0000, 0xe000, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0ff0, 0x0001, 0xc010, - 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x1ff0, 0x0000, 0xc000, 0x0fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x3fe0, 0x0800, 0x8000, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7fc4, 0x2082, 0x0040, - 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff88, 0x0c00, 0x0000, 0x0fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff00, 0x7103, 0x0000, 0x0ffe, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xe003, 0x0028, - 0x07fc, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x7800, 0x81f8, 0x0087, 0x07f8, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x3000, 0x9f00, 0x400f, 0x07f8, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x7e00, 0x004e, - 0x23fa, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfbfe, 0x005f, 0x03f6, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x8400, 0xf7ff, 0x003f, 0x03f6, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x80ff, - 0x11e6, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf080, 0xffff, 0x80ff, 0x01e6, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfc00, 0xffff, 0x01ff, 0x08ee, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x01ff, - 0x00ee, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x03ff, 0x056e, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0x07ff, 0x00ae, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xdfc0, 0xffff, 0x0fff, - 0x008e, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe1e0, 0xffff, 0x4ffe, 0x00ce, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf0e0, 0x07ff, 0x9ff0, 0x016e, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf874, 0x40ff, 0x3fe1, - 0x0066, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x3d30, 0x3f20, 0x7f8c, 0x0027, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0c08, 0x0f82, 0x6f90, 0x00b7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2208, 0x01c0, 0x1e40, - 0x0017, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0002, 0x00a8, 0xbe80, 0x0017, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0404, 0x0020, 0x7900, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe018, 0xf20f, - 0x000b, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf01c, 0xf63b, 0x000e, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x4002, 0x3c1e, 0xcc84, 0x0125, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2800, 0xdf1f, 0xd93f, - 0x0073, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xc000, 0xe7df, 0xb67f, 0x00f7, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xc000, 0xf3ff, 0x3cff, 0x04f7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe500, 0xfbff, 0x7dff, - 0x01f7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf200, 0xfdff, 0xfbff, 0x13fe, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf940, 0xfeff, 0xf7ff, 0x03fe, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc80, 0xff7f, 0xffff, - 0x27fd, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfe10, 0xff7f, 0xefff, 0x0ffb, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfe00, 0x3fbf, 0xeff8, 0x0ffb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0x01ff, 0xfe00, - 0x9fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0x007f, 0xd808, 0x1ff7, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xffc0, 0x0c07, 0xe7dc, 0x3ff7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe4, 0x0f0f, 0xdfc0, - 0x7fef, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x1f7f, 0xfff0, 0x7fef, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfff0, 0x7fff, 0xfffc, 0xffef, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff9, 0xffff, 0xffff, - 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffc, 0xfeff, 0xffff, 0xffff, 0x0001, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfffc, 0xfdff, 0xffff, 0xffdf, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, - 0xffdf, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x2000, 0xffff, 0xfbff, 0xdfff, 0xffdf, 0x0003, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xffff, 0xfbff, 0xdfff, 0xfddf, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0xe7ff, 0xefff, - 0xffdf, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x8000, 0xffff, 0xdfff, 0xf7ff, 0xff9f, 0x004f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, - 0xffff, 0xbfff, 0xf1ff, 0xff9f, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0x6fff, 0xfc7c, - 0xff9f, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xfe01, 0xfb9f, 0x001f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe200, - 0xffff, 0x9fff, 0xffff, 0xfb9f, 0x011f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x7fff, 0xffff, - 0xfb9f, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf100, 0xffff, 0xffff, 0xfff8, 0xfbdf, 0x003f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, - 0xfbff, 0xffff, 0xfe03, 0xf7df, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xfe7f, 0xfff9, 0xffff, - 0xf7cf, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf800, 0xffbf, 0xfffb, 0xffff, 0xf7c7, 0x007f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, - 0xffcf, 0xffff, 0xffff, 0xf7e3, 0x04ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xfff7, 0xfff7, 0x3fff, - 0xf7e0, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xe200, 0xfffb, 0xfff7, 0x07ff, 0xf7f8, 0x01ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, - 0xfffc, 0xfff7, 0xc07f, 0xf7ff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xfffe, 0xfff7, 0xf81f, - 0xf7ff, 0x09ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x2400, 0xffff, 0xfffb, 0xff07, 0xe7ff, 0x01ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7000, - 0xffff, 0xfff9, 0xfff0, 0xf7ff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xb800, 0xffff, 0x7ff8, 0xfffc, - 0xe7ff, 0x13ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x9c00, 0x3fff, 0x9ffe, 0xffff, 0xf7ff, 0x07ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xdc00, - 0x1fff, 0xe7ff, 0xffff, 0xe7ff, 0x05ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xef00, 0xc1ff, 0xf9ff, 0xffff, - 0xf7ff, 0x05ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xef80, 0xf01f, 0xfcff, 0xffff, 0xf1ff, 0x03ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x37c0, - 0xfc00, 0xff5f, 0xffff, 0xf1ff, 0x0bff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffaf, 0xffff, - 0xf1ff, 0x0bfb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff80, 0xffff, 0xffd7, 0xffff, 0xf3ff, 0x1ffb, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8800, 0xffff, - 0xffff, 0x0077, 0xfff8, 0xf2ff, 0x17fb, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xf03b, 0xffef, - 0xf2ff, 0x97fb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8800, 0xffff, 0xffff, 0xfc01, 0xffff, 0xf2ff, 0x27fb, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, - 0xffff, 0xe001, 0xffff, 0xf97f, 0x2ff3, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xfffe, 0xffff, 0xc380, 0xffff, - 0xf97f, 0x0ff7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfff8, 0xffff, 0x3fa0, 0xffff, 0xfc7f, 0x4ff7, 0x0001, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, - 0x7fff, 0x7f80, 0xfffe, 0xfc3f, 0x0ff7, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc4, 0x3fff, 0xffc1, 0xfffd, - 0xfcbf, 0x1ff7, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff80, 0x3fff, 0xffc0, 0xffff, 0xfe9f, 0x1ff7, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff10, - 0xbfff, 0xffe0, 0xffff, 0xfe9f, 0x1ff7, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0x1fff, 0xfff0, 0xffff, - 0xffdf, 0x3ffb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf800, 0x4fff, 0xfff8, 0xffff, 0xffcf, 0x3fef, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe100, - 0x07ff, 0xfff8, 0xffff, 0xffcf, 0x3fe9, 0x0001, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x87ff, 0xffdc, 0xffff, - 0xffe7, 0x3fef, 0x0008, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x03ff, 0xfffe, 0xffff, 0xffe7, 0x3fe9, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x00f8, 0xfff7, 0xffff, 0xffe7, 0x7fe9, 0x0002, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2000, 0xfff7, 0xffff, - 0xffe7, 0x7fe9, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x8000, 0xfff9, 0xffff, 0xfff3, 0xffe9, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xc002, 0xfff8, 0xffff, 0xfff3, 0xffe9, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4008, 0xfffc, 0xffff, - 0xfff1, 0xffc9, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xfff9, 0xffc9, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffff, 0xffff, 0xfff8, 0xffc9, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0xfffe, 0xffff, - 0xfff8, 0x3fd9, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x4000, 0xfffe, 0xffff, 0xfffc, 0x07d9, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xa000, 0xffff, 0x7fff, 0xfffc, 0xf0d9, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xd000, 0xffff, 0x7fff, - 0xfffc, 0xf859, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xc200, 0xffff, 0x7fff, 0xfffe, 0x7e19, 0x0008, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf000, 0xffff, 0x3fff, 0xfffe, 0x1f81, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x3fff, - 0xfffe, 0x07d1, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x3fff, 0xffff, 0x01f9, 0x0000, - 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x7e80, 0xfffe, 0x3fff, 0xffff, 0x0078, 0x8000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3c00, 0xffff, 0x3fff, - 0x7fff, 0x0008, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xbe00, 0xffff, 0x9fff, 0x1fff, 0x0000, 0x8000, - 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x9e00, 0xffff, 0x9fff, 0x07ff, 0x0200, 0x2000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xce00, 0xffff, 0x9fff, - 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xce00, 0xffff, 0xcfff, 0x001f, 0x0002, 0x4000, - 0x8010, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xc600, 0xffff, 0x0fff, 0x0000, 0x0000, 0x4000, 0x1fc0, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xa200, 0xffff, 0x0fff, - 0x0000, 0x0000, 0x4000, 0x79f0, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xa000, 0xffff, 0x0fff, 0x0080, 0x0000, 0x6800, - 0x8ff0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xe000, 0xffff, 0x0fff, 0x0000, 0x0000, 0x6000, 0xfff2, 0x0004, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xd400, 0xffff, 0x0fff, - 0x0000, 0x0000, 0x6000, 0xfff9, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xd000, 0xfffb, 0x0fff, 0x0000, 0x0000, 0x6000, - 0xfbf8, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xc200, 0xfffd, 0x07ff, 0x0000, 0x0000, 0x6000, 0x81fc, 0x001f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xfffd, 0x07ff, - 0x0000, 0x0000, 0x6000, 0x007c, 0x001c, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xc000, 0xfffd, 0x17ff, 0x0000, 0x0000, 0x6000, - 0x023c, 0x0060, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x8000, 0xfffd, 0x17ff, 0x0000, 0x0000, 0xe000, 0x003e, 0x0084, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x07ff, - 0x0000, 0x0000, 0xc000, 0x001e, 0x0040, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x17ff, 0x0000, 0x0000, 0xc000, - 0x200e, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xe000, 0xfffe, 0x0fff, 0x0000, 0x0000, 0xc000, 0x000f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x0fff, - 0x0000, 0x0000, 0xc000, 0x07e7, 0x0801, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x0fff, 0x0000, 0x0000, 0xc000, - 0x1ff7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x6000, 0xfffa, 0x0fff, 0x0000, 0x0000, 0xc000, 0x7e3b, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xfffa, 0x5fff, - 0x0000, 0x0000, 0x8000, 0xf00f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfffa, 0x0fff, 0x0000, 0x0000, 0x8000, - 0xc007, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfffa, 0x1fff, 0x0000, 0x0000, 0x9000, 0x1003, 0x0013, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1000, 0xfffa, 0x1fff, - 0x0000, 0x0000, 0x8000, 0x000b, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, 0x1fff, 0x0000, 0x0000, 0x8000, - 0x0001, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x8000, 0xfff0, 0x9fff, 0x0000, 0x0000, 0xa000, 0x0001, 0x002a, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x1fff, - 0x0000, 0x0000, 0x0000, 0x0005, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x1fff, 0x0000, 0x0000, 0x0000, - 0x0005, 0x0010, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffe0, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0001, 0x0040, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0x1fff, - 0x0000, 0x0000, 0x4000, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffc4, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffc0, 0x3fff, 0x0001, 0x0000, 0x0000, 0x0003, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe88, 0x3fff, - 0x0000, 0x0000, 0x0000, 0x0100, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfd80, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfb00, 0x3fff, 0x0000, 0x0000, 0x8000, 0x41ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfa00, 0x3fff, - 0x0000, 0x0000, 0xc800, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0x7fff, 0x0000, 0x0000, 0xe000, - 0x1fff, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xe000, 0x7fff, 0x0004, 0x0000, 0xf000, 0x7fff, 0x0010, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe080, 0xffff, - 0x0000, 0x0000, 0xf000, 0xffff, 0x0040, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xc900, 0xffff, 0x2000, 0x0000, 0xf000, - 0xffff, 0x0083, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8800, 0xffff, 0x0001, 0x0000, 0xf800, 0xffff, 0x0007, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, - 0x0001, 0x0002, 0xfc80, 0xffff, 0x041f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x2000, 0xffff, 0x0003, 0x0004, 0xfc00, - 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x7200, 0xffff, 0x1f87, 0x0000, 0xf440, 0xffff, 0x107f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7000, 0xfffe, - 0x3fc7, 0x0020, 0xf800, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x7800, 0xfffc, 0xffe3, 0x0001, 0xfc00, - 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x7c00, 0xfffc, 0xffe3, 0x0003, 0xfe00, 0xffff, 0x03ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7c00, 0xfffc, - 0xffe3, 0x000f, 0xff08, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x7e00, 0xfffa, 0xfff1, 0x083f, 0xffc0, - 0xffff, 0x07ff, 0x0010, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x7f00, 0xfffa, 0xfff9, 0x007f, 0xfff0, 0xffff, 0x07ff, 0x0040, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7f84, 0xfff2, - 0xfff8, 0x01ff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x7fc0, 0xfff6, 0xfffc, 0xc7ff, 0xffff, - 0xffff, 0x0fff, 0x0060, 0x0000, 0x2000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x7fe0, 0xfffe, 0xfffc, 0x1fff, 0xffff, 0xffff, 0x0fff, 0x00e0, - 0x0000, 0x0100, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7ff0, 0x7fee, - 0xfffe, 0xffff, 0xfffc, 0xffff, 0x0fff, 0x07c0, 0x0000, 0x0000, 0x0020, - 0x0000, 0x0000, 0x0000, 0x0000, 0x7ff8, 0x3fee, 0xfffe, 0xffff, 0xfff3, - 0xffff, 0x9fff, 0x07c0, 0x0000, 0xf020, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x7ffc, 0x3fde, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, 0x3fc1, - 0x0002, 0xff00, 0x0021, 0x0000, 0x0000, 0x0000, 0x0000, 0x7ffe, 0x3fff, - 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0x7ec0, 0x0000, 0x3f88, 0x0084, - 0x0000, 0x0000, 0x0000, 0x8000, 0x3fff, 0x9fff, 0xffff, 0xffff, 0xffff, - 0xffff, 0x7fff, 0x8006, 0x0001, 0x6080, 0x003e, 0x0000, 0x0000, 0x0000, - 0xc080, 0xbfff, 0x9fff, 0xffff, 0xffff, 0xffff, 0xbfff, 0x7fff, 0x00fe, - 0x0003, 0xbff0, 0x003f, 0x0000, 0x0000, 0x0000, 0xf000, 0x9fff, 0xdffd, - 0xffff, 0xffff, 0xffff, 0xe7ff, 0xffff, 0x1ffc, 0x0004, 0xdff8, 0x001f, - 0x0000, 0x0000, 0x0000, 0xfc00, 0x9fff, 0xcffd, 0xffff, 0xffff, 0xffff, - 0xf9ff, 0xffff, 0xfff9, 0x1019, 0xe7f8, 0x004f, 0x0000, 0x0000, 0x0000, - 0xfe04, 0xdfff, 0xeffe, 0xffff, 0xffff, 0xffff, 0xfcff, 0xffff, 0x0fd3, - 0x0070, 0xf7fc, 0x002f, 0x0000, 0x0000, 0x0000, 0xff80, 0x4fff, 0xf7fe, - 0xffff, 0xffff, 0xffff, 0xfe3f, 0xffff, 0xffe7, 0xf0fc, 0xfbfd, 0x0033, - 0x0000, 0x0000, 0x4000, 0xffe0, 0x6fff, 0xf7fe, 0xffff, 0xffff, 0xffff, - 0xfd9f, 0xffff, 0xff9f, 0xfffb, 0xfbfd, 0x003b, 0x0000, 0x0000, 0x0000, - 0xfff8, 0x27ff, 0xf3ff, 0xffff, 0xffff, 0xffff, 0xf9cf, 0xffff, 0xffbf, - 0xffe3, 0xfbfe, 0x001c, 0x0000, 0x0000, 0x0140, 0xffff, 0x13ff, 0xf9ff, - 0xffff, 0xffff, 0xffff, 0xe7e3, 0xffff, 0xffff, 0xffcf, 0x7bfe, 0x009f, - 0x0000, 0x0000, 0xe000, 0xffff, 0x8bff, 0xfdff, 0xffff, 0xffff, 0xffff, - 0xc7f1, 0xffff, 0xffff, 0x7f1f, 0x97fe, 0x000f, 0x0000, 0x8000, 0xff78, - 0xffff, 0x91ff, 0xfeff, 0xffff, 0xffff, 0xffff, 0xcff8, 0xffff, 0xffff, - 0xf87f, 0x30c3, 0x000f, 0x0000, 0x0000, 0xc1fc, 0xffff, 0xd9ff, 0xfe03, - 0xffff, 0xffff, 0x7fff, 0x9ff8, 0xffff, 0xffff, 0xe07f, 0xffbf, 0x0006, - 0x0000, 0x0000, 0x007e, 0xffff, 0xccff, 0xfc00, 0xffff, 0xffff, 0x7fff, - 0x9ffc, 0xffff, 0xffff, 0xc1ff, 0xffff, 0x0001, 0x0000, 0x9000, 0x3e1f, - 0xfffc, 0x07ff, 0xf0f8, 0xffff, 0xfff7, 0x3fff, 0x3ffe, 0xffff, 0xffff, - 0x01ff, 0xfffc, 0x0000, 0x0000, 0x8000, 0xff8f, 0xfff8, 0x07ff, 0xe1ff, - 0xffff, 0xffff, 0x1fff, 0x3ffe, 0xffff, 0xffff, 0x03ff, 0x7ff0, 0x0000, - 0x0000, 0xc000, 0xffe3, 0xffe3, 0xe07f, 0xc7ff, 0xffff, 0xfffb, 0x0fff, - 0x3dfb, 0xffff, 0xffff, 0x8fdf, 0x0000, 0x0000, 0x0000, 0xe000, 0xfff3, - 0xffc7, 0xf80f, 0x8fff, 0xffff, 0xfffb, 0x8fff, 0x33cf, 0xfffe, 0xffff, - 0x8fbf, 0x0000, 0x0004, 0x0000, 0xf000, 0xeff9, 0x0b1f, 0xffc0, 0x3fff, - 0xffff, 0xfffb, 0x07ff, 0x081c, 0xfffe, 0xffff, 0x18ff, 0x0001, 0x0001, - 0x0000, 0xf900, 0x9ffe, 0x007f, 0xfffe, 0x7fff, 0xfffe, 0xfffb, 0x07ff, - 0x0030, 0xfffe, 0xffff, 0x03ff, 0x2080, 0x0000, 0x0000, 0x7c80, 0x3ffe, - 0xfcff, 0xffff, 0x7fff, 0xfffe, 0xfffb, 0x03ff, 0x0001, 0xfffe, 0xffff, - 0x0f81, 0x0001, 0x0000, 0x0000, 0x7c00, 0x3801, 0xf3fc, 0xffff, 0xffff, - 0xfffc, 0xfffb, 0x03ff, 0x4020, 0xfffe, 0xffff, 0xe03f, 0x0002, 0x0000, - 0x0000, 0x3e00, 0x03f8, 0xcff9, 0xf77f, 0xffff, 0xfff9, 0xfffb, 0x11ff, - 0x0000, 0xffdf, 0xfffc, 0x00ff, 0x0002, 0x0000, 0x0000, 0x8f00, 0xbfff, - 0x3fe3, 0xfb7c, 0x3fff, 0xfffa, 0xfffb, 0x01ff, 0x4000, 0xf07e, 0x0003, - 0x1000, 0x0001, 0x0000, 0x0000, 0xc380, 0xffff, 0xffcf, 0xff82, 0xbfe5, - 0xfff1, 0xfff3, 0x00ff, 0x4000, 0x00fc, 0x0000, 0x0400, 0x0000, 0x0000, - 0x0000, 0xf3c0, 0xffff, 0xff8f, 0xfd9f, 0xbfde, 0xfff7, 0xfffb, 0x08ff, - 0x8000, 0x43e0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfce0, 0xffff, - 0xff00, 0xbcbf, 0xdfbf, 0xffc7, 0xfff3, 0x00ff, 0x0000, 0x0000, 0xa0d0, - 0x0006, 0x0000, 0x0000, 0x0000, 0xfe70, 0x1fff, 0xfe7c, 0xbc3f, 0xdfff, - 0x7fcf, 0xfff3, 0x047f, 0x0000, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xff3c, 0xc0ff, 0xf8fe, 0xddff, 0xde7f, 0x7f1f, 0xfff3, 0x003f, - 0x0000, 0x0480, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xff9e, 0xf81f, - 0xf1fe, 0xec07, 0xdeff, 0x7f3f, 0xfff3, 0x003f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x7fcd, 0x0ec0, 0x47fe, 0xe5e0, 0xdeff, - 0xfe3f, 0xfff2, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xc000, 0x07ee, 0x77f8, 0x0ffe, 0xf5fc, 0xddff, 0xfe7f, 0xfff6, 0x003f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x6000, 0x0007, 0x79ff, - 0x0ffe, 0xf0ff, 0xddff, 0xfcff, 0xfff4, 0x001f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xbe17, 0x7f7f, 0xe7ff, 0xf8ff, 0xd9ff, - 0xfcff, 0xfff4, 0x011f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xc000, 0xdf3b, 0x7f7f, 0x11ff, 0xf8fe, 0xdbff, 0xf8ff, 0xffe4, 0x001f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe200, 0xefbf, 0x3fbf, - 0xb1ff, 0xfcff, 0xd3ff, 0xf9ff, 0xffed, 0x001f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf800, 0xf79f, 0xbfdf, 0xcfff, 0xfcff, 0xd7ff, - 0xf1ff, 0xffe9, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfc40, 0xf79f, 0xdfdf, 0xe3ff, 0xfcff, 0xd7ff, 0xf3ff, 0xffe9, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff90, 0xfb9f, 0xcfe7, - 0xf800, 0xfeff, 0xd7ff, 0xf3ff, 0xffc3, 0x000f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfc00, 0xfd85, 0x07f3, 0xfc00, 0xfc7f, 0xafff, - 0xe3ff, 0xffd3, 0x0083, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0010, 0xfd80, 0x03f9, 0xde00, 0xfc7f, 0xafff, 0xe7ff, 0x7fc3, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0080, 0x7e80, 0x00fc, - 0xe700, 0xfc7f, 0xafff, 0xe7ff, 0x07c3, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x2000, 0x1e80, 0x203e, 0xf1c0, 0xfc7f, 0x8fff, - 0xe7ff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0300, 0x001e, 0xfc60, 0xf87f, 0x8fff, 0xe7ff, 0x0003, 0x0010, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0100, - 0xfe08, 0xf8ff, 0x4fff, 0xcfff, 0xc003, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe4, 0xe0ff, 0x4fff, - 0xcfff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff2, 0xc1ff, 0x4fff, 0xcfff, 0x0187, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0040, 0x0000, - 0xfffc, 0x83ff, 0x0fff, 0xcfff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xfffc, 0x0fff, 0x1ffe, - 0xcffe, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x1fff, 0x0ffc, 0xc7f8, 0x0007, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, - 0xf80f, 0x7fff, 0xcfe0, 0xe381, 0x004f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xffc0, 0xffff, 0xcf80, - 0xe007, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff00, 0xf7ff, 0xe007, 0xc00f, 0x0007, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0200, - 0xc000, 0xfc2f, 0xe01f, 0x303f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xf83d, - 0x003f, 0x0100, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0020, 0x8000, 0xff81, 0x0201, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x1fe0, 0x2040, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0001, - 0x0008, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x2000, 0x0000, 0x0002, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x4000, 0x2000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0202, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000 }; + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8400, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0800, 0x0000, 0x0004, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0680, 0xf000, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0600, 0xe000, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0700, 0xe000, 0x021f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x1780, 0xc000, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x07c0, 0x8000, 0x21ff, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x07c0, 0x8000, 0x07ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0fe0, 0x0000, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x07e4, 0x0000, 0x3ffe, + 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0ff0, 0x0000, 0x7ffc, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x07f0, 0x0000, 0xfff8, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2ff0, 0x0000, 0xfff1, + 0x0023, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0ff8, 0x0000, 0xfff0, 0x0007, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0ff8, 0x0000, 0xffe0, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0ff8, 0x0000, 0xffc0, + 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0ff8, 0x0000, 0xff80, 0x003f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0ffc, 0x0000, 0xff00, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0ffc, 0x0000, 0xfe00, + 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x4ff8, 0x0000, 0xfc00, 0x01ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0dfc, 0x0000, 0xfc80, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1bf8, 0x0000, 0xf800, + 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x93f8, 0x0000, 0xf000, 0x03ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x17f8, 0x0000, 0xe000, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0ff0, 0x0001, 0xc010, + 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x1ff0, 0x0000, 0xc000, 0x0fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x3fe0, 0x0800, 0x8000, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7fc4, 0x2082, 0x0040, + 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff88, 0x0c00, 0x0000, 0x0fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff00, 0x7103, 0x0000, 0x0ffe, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xe003, 0x0028, + 0x07fc, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x7800, 0x81f8, 0x0087, 0x07f8, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x3000, 0x9f00, 0x400f, 0x07f8, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x7e00, 0x004e, + 0x23fa, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfbfe, 0x005f, 0x03f6, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x8400, 0xf7ff, 0x003f, 0x03f6, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x80ff, + 0x11e6, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf080, 0xffff, 0x80ff, 0x01e6, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfc00, 0xffff, 0x01ff, 0x08ee, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x01ff, + 0x00ee, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x03ff, 0x056e, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0x07ff, 0x00ae, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xdfc0, 0xffff, 0x0fff, + 0x008e, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe1e0, 0xffff, 0x4ffe, 0x00ce, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf0e0, 0x07ff, 0x9ff0, 0x016e, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf874, 0x40ff, 0x3fe1, + 0x0066, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x3d30, 0x3f20, 0x7f8c, 0x0027, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0c08, 0x0f82, 0x6f90, 0x00b7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2208, 0x01c0, 0x1e40, + 0x0017, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0002, 0x00a8, 0xbe80, 0x0017, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0404, 0x0020, 0x7900, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe018, 0xf20f, + 0x000b, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf01c, 0xf63b, 0x000e, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x4002, 0x3c1e, 0xcc84, 0x0125, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2800, 0xdf1f, 0xd93f, + 0x0073, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xc000, 0xe7df, 0xb67f, 0x00f7, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xc000, 0xf3ff, 0x3cff, 0x04f7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe500, 0xfbff, 0x7dff, + 0x01f7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf200, 0xfdff, 0xfbff, 0x13fe, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf940, 0xfeff, 0xf7ff, 0x03fe, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc80, 0xff7f, 0xffff, + 0x27fd, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfe10, 0xff7f, 0xefff, 0x0ffb, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfe00, 0x3fbf, 0xeff8, 0x0ffb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0x01ff, 0xfe00, + 0x9fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0x007f, 0xd808, 0x1ff7, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xffc0, 0x0c07, 0xe7dc, 0x3ff7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe4, 0x0f0f, 0xdfc0, + 0x7fef, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x1f7f, 0xfff0, 0x7fef, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfff0, 0x7fff, 0xfffc, 0xffef, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff9, 0xffff, 0xffff, + 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffc, 0xfeff, 0xffff, 0xffff, 0x0001, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfffc, 0xfdff, 0xffff, 0xffdf, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, + 0xffdf, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x2000, 0xffff, 0xfbff, 0xdfff, 0xffdf, 0x0003, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xffff, 0xfbff, 0xdfff, 0xfddf, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0xe7ff, 0xefff, + 0xffdf, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x8000, 0xffff, 0xdfff, 0xf7ff, 0xff9f, 0x004f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, + 0xffff, 0xbfff, 0xf1ff, 0xff9f, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0x6fff, 0xfc7c, + 0xff9f, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xfe01, 0xfb9f, 0x001f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe200, + 0xffff, 0x9fff, 0xffff, 0xfb9f, 0x011f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x7fff, 0xffff, + 0xfb9f, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf100, 0xffff, 0xffff, 0xfff8, 0xfbdf, 0x003f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, + 0xfbff, 0xffff, 0xfe03, 0xf7df, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xfe7f, 0xfff9, 0xffff, + 0xf7cf, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf800, 0xffbf, 0xfffb, 0xffff, 0xf7c7, 0x007f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, + 0xffcf, 0xffff, 0xffff, 0xf7e3, 0x04ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xfff7, 0xfff7, 0x3fff, + 0xf7e0, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xe200, 0xfffb, 0xfff7, 0x07ff, 0xf7f8, 0x01ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, + 0xfffc, 0xfff7, 0xc07f, 0xf7ff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xfffe, 0xfff7, 0xf81f, + 0xf7ff, 0x09ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x2400, 0xffff, 0xfffb, 0xff07, 0xe7ff, 0x01ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7000, + 0xffff, 0xfff9, 0xfff0, 0xf7ff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xb800, 0xffff, 0x7ff8, 0xfffc, + 0xe7ff, 0x13ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x9c00, 0x3fff, 0x9ffe, 0xffff, 0xf7ff, 0x07ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xdc00, + 0x1fff, 0xe7ff, 0xffff, 0xe7ff, 0x05ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xef00, 0xc1ff, 0xf9ff, 0xffff, + 0xf7ff, 0x05ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xef80, 0xf01f, 0xfcff, 0xffff, 0xf1ff, 0x03ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x37c0, + 0xfc00, 0xff5f, 0xffff, 0xf1ff, 0x0bff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffaf, 0xffff, + 0xf1ff, 0x0bfb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff80, 0xffff, 0xffd7, 0xffff, 0xf3ff, 0x1ffb, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8800, 0xffff, + 0xffff, 0x0077, 0xfff8, 0xf2ff, 0x17fb, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xf03b, 0xffef, + 0xf2ff, 0x97fb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8800, 0xffff, 0xffff, 0xfc01, 0xffff, 0xf2ff, 0x27fb, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, + 0xffff, 0xe001, 0xffff, 0xf97f, 0x2ff3, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xfffe, 0xffff, 0xc380, 0xffff, + 0xf97f, 0x0ff7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfff8, 0xffff, 0x3fa0, 0xffff, 0xfc7f, 0x4ff7, 0x0001, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, + 0x7fff, 0x7f80, 0xfffe, 0xfc3f, 0x0ff7, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc4, 0x3fff, 0xffc1, 0xfffd, + 0xfcbf, 0x1ff7, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff80, 0x3fff, 0xffc0, 0xffff, 0xfe9f, 0x1ff7, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff10, + 0xbfff, 0xffe0, 0xffff, 0xfe9f, 0x1ff7, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0x1fff, 0xfff0, 0xffff, + 0xffdf, 0x3ffb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf800, 0x4fff, 0xfff8, 0xffff, 0xffcf, 0x3fef, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe100, + 0x07ff, 0xfff8, 0xffff, 0xffcf, 0x3fe9, 0x0001, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x87ff, 0xffdc, 0xffff, + 0xffe7, 0x3fef, 0x0008, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x03ff, 0xfffe, 0xffff, 0xffe7, 0x3fe9, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x00f8, 0xfff7, 0xffff, 0xffe7, 0x7fe9, 0x0002, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2000, 0xfff7, 0xffff, + 0xffe7, 0x7fe9, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x8000, 0xfff9, 0xffff, 0xfff3, 0xffe9, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xc002, 0xfff8, 0xffff, 0xfff3, 0xffe9, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4008, 0xfffc, 0xffff, + 0xfff1, 0xffc9, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xfff9, 0xffc9, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffff, 0xffff, 0xfff8, 0xffc9, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0xfffe, 0xffff, + 0xfff8, 0x3fd9, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x4000, 0xfffe, 0xffff, 0xfffc, 0x07d9, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xa000, 0xffff, 0x7fff, 0xfffc, 0xf0d9, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xd000, 0xffff, 0x7fff, + 0xfffc, 0xf859, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xc200, 0xffff, 0x7fff, 0xfffe, 0x7e19, 0x0008, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf000, 0xffff, 0x3fff, 0xfffe, 0x1f81, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x3fff, + 0xfffe, 0x07d1, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x3fff, 0xffff, 0x01f9, 0x0000, + 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x7e80, 0xfffe, 0x3fff, 0xffff, 0x0078, 0x8000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3c00, 0xffff, 0x3fff, + 0x7fff, 0x0008, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xbe00, 0xffff, 0x9fff, 0x1fff, 0x0000, 0x8000, + 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x9e00, 0xffff, 0x9fff, 0x07ff, 0x0200, 0x2000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xce00, 0xffff, 0x9fff, + 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xce00, 0xffff, 0xcfff, 0x001f, 0x0002, 0x4000, + 0x8010, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xc600, 0xffff, 0x0fff, 0x0000, 0x0000, 0x4000, 0x1fc0, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xa200, 0xffff, 0x0fff, + 0x0000, 0x0000, 0x4000, 0x79f0, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xa000, 0xffff, 0x0fff, 0x0080, 0x0000, 0x6800, + 0x8ff0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xe000, 0xffff, 0x0fff, 0x0000, 0x0000, 0x6000, 0xfff2, 0x0004, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xd400, 0xffff, 0x0fff, + 0x0000, 0x0000, 0x6000, 0xfff9, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xd000, 0xfffb, 0x0fff, 0x0000, 0x0000, 0x6000, + 0xfbf8, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xc200, 0xfffd, 0x07ff, 0x0000, 0x0000, 0x6000, 0x81fc, 0x001f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xfffd, 0x07ff, + 0x0000, 0x0000, 0x6000, 0x007c, 0x001c, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xc000, 0xfffd, 0x17ff, 0x0000, 0x0000, 0x6000, + 0x023c, 0x0060, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x8000, 0xfffd, 0x17ff, 0x0000, 0x0000, 0xe000, 0x003e, 0x0084, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x07ff, + 0x0000, 0x0000, 0xc000, 0x001e, 0x0040, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x17ff, 0x0000, 0x0000, 0xc000, + 0x200e, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xe000, 0xfffe, 0x0fff, 0x0000, 0x0000, 0xc000, 0x000f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x0fff, + 0x0000, 0x0000, 0xc000, 0x07e7, 0x0801, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x0fff, 0x0000, 0x0000, 0xc000, + 0x1ff7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x6000, 0xfffa, 0x0fff, 0x0000, 0x0000, 0xc000, 0x7e3b, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xfffa, 0x5fff, + 0x0000, 0x0000, 0x8000, 0xf00f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfffa, 0x0fff, 0x0000, 0x0000, 0x8000, + 0xc007, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfffa, 0x1fff, 0x0000, 0x0000, 0x9000, 0x1003, 0x0013, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1000, 0xfffa, 0x1fff, + 0x0000, 0x0000, 0x8000, 0x000b, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, 0x1fff, 0x0000, 0x0000, 0x8000, + 0x0001, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x8000, 0xfff0, 0x9fff, 0x0000, 0x0000, 0xa000, 0x0001, 0x002a, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x1fff, + 0x0000, 0x0000, 0x0000, 0x0005, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x1fff, 0x0000, 0x0000, 0x0000, + 0x0005, 0x0010, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffe0, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0001, 0x0040, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0x1fff, + 0x0000, 0x0000, 0x4000, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffc4, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffc0, 0x3fff, 0x0001, 0x0000, 0x0000, 0x0003, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe88, 0x3fff, + 0x0000, 0x0000, 0x0000, 0x0100, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfd80, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfb00, 0x3fff, 0x0000, 0x0000, 0x8000, 0x41ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfa00, 0x3fff, + 0x0000, 0x0000, 0xc800, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0x7fff, 0x0000, 0x0000, 0xe000, + 0x1fff, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xe000, 0x7fff, 0x0004, 0x0000, 0xf000, 0x7fff, 0x0010, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe080, 0xffff, + 0x0000, 0x0000, 0xf000, 0xffff, 0x0040, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xc900, 0xffff, 0x2000, 0x0000, 0xf000, + 0xffff, 0x0083, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8800, 0xffff, 0x0001, 0x0000, 0xf800, 0xffff, 0x0007, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, + 0x0001, 0x0002, 0xfc80, 0xffff, 0x041f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x2000, 0xffff, 0x0003, 0x0004, 0xfc00, + 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x7200, 0xffff, 0x1f87, 0x0000, 0xf440, 0xffff, 0x107f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7000, 0xfffe, + 0x3fc7, 0x0020, 0xf800, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x7800, 0xfffc, 0xffe3, 0x0001, 0xfc00, + 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x7c00, 0xfffc, 0xffe3, 0x0003, 0xfe00, 0xffff, 0x03ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7c00, 0xfffc, + 0xffe3, 0x000f, 0xff08, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x7e00, 0xfffa, 0xfff1, 0x083f, 0xffc0, + 0xffff, 0x07ff, 0x0010, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x7f00, 0xfffa, 0xfff9, 0x007f, 0xfff0, 0xffff, 0x07ff, 0x0040, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7f84, 0xfff2, + 0xfff8, 0x01ff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x7fc0, 0xfff6, 0xfffc, 0xc7ff, 0xffff, + 0xffff, 0x0fff, 0x0060, 0x0000, 0x2000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x7fe0, 0xfffe, 0xfffc, 0x1fff, 0xffff, 0xffff, 0x0fff, 0x00e0, + 0x0000, 0x0100, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7ff0, 0x7fee, + 0xfffe, 0xffff, 0xfffc, 0xffff, 0x0fff, 0x07c0, 0x0000, 0x0000, 0x0020, + 0x0000, 0x0000, 0x0000, 0x0000, 0x7ff8, 0x3fee, 0xfffe, 0xffff, 0xfff3, + 0xffff, 0x9fff, 0x07c0, 0x0000, 0xf020, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x7ffc, 0x3fde, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, 0x3fc1, + 0x0002, 0xff00, 0x0021, 0x0000, 0x0000, 0x0000, 0x0000, 0x7ffe, 0x3fff, + 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0x7ec0, 0x0000, 0x3f88, 0x0084, + 0x0000, 0x0000, 0x0000, 0x8000, 0x3fff, 0x9fff, 0xffff, 0xffff, 0xffff, + 0xffff, 0x7fff, 0x8006, 0x0001, 0x6080, 0x003e, 0x0000, 0x0000, 0x0000, + 0xc080, 0xbfff, 0x9fff, 0xffff, 0xffff, 0xffff, 0xbfff, 0x7fff, 0x00fe, + 0x0003, 0xbff0, 0x003f, 0x0000, 0x0000, 0x0000, 0xf000, 0x9fff, 0xdffd, + 0xffff, 0xffff, 0xffff, 0xe7ff, 0xffff, 0x1ffc, 0x0004, 0xdff8, 0x001f, + 0x0000, 0x0000, 0x0000, 0xfc00, 0x9fff, 0xcffd, 0xffff, 0xffff, 0xffff, + 0xf9ff, 0xffff, 0xfff9, 0x1019, 0xe7f8, 0x004f, 0x0000, 0x0000, 0x0000, + 0xfe04, 0xdfff, 0xeffe, 0xffff, 0xffff, 0xffff, 0xfcff, 0xffff, 0x0fd3, + 0x0070, 0xf7fc, 0x002f, 0x0000, 0x0000, 0x0000, 0xff80, 0x4fff, 0xf7fe, + 0xffff, 0xffff, 0xffff, 0xfe3f, 0xffff, 0xffe7, 0xf0fc, 0xfbfd, 0x0033, + 0x0000, 0x0000, 0x4000, 0xffe0, 0x6fff, 0xf7fe, 0xffff, 0xffff, 0xffff, + 0xfd9f, 0xffff, 0xff9f, 0xfffb, 0xfbfd, 0x003b, 0x0000, 0x0000, 0x0000, + 0xfff8, 0x27ff, 0xf3ff, 0xffff, 0xffff, 0xffff, 0xf9cf, 0xffff, 0xffbf, + 0xffe3, 0xfbfe, 0x001c, 0x0000, 0x0000, 0x0140, 0xffff, 0x13ff, 0xf9ff, + 0xffff, 0xffff, 0xffff, 0xe7e3, 0xffff, 0xffff, 0xffcf, 0x7bfe, 0x009f, + 0x0000, 0x0000, 0xe000, 0xffff, 0x8bff, 0xfdff, 0xffff, 0xffff, 0xffff, + 0xc7f1, 0xffff, 0xffff, 0x7f1f, 0x97fe, 0x000f, 0x0000, 0x8000, 0xff78, + 0xffff, 0x91ff, 0xfeff, 0xffff, 0xffff, 0xffff, 0xcff8, 0xffff, 0xffff, + 0xf87f, 0x30c3, 0x000f, 0x0000, 0x0000, 0xc1fc, 0xffff, 0xd9ff, 0xfe03, + 0xffff, 0xffff, 0x7fff, 0x9ff8, 0xffff, 0xffff, 0xe07f, 0xffbf, 0x0006, + 0x0000, 0x0000, 0x007e, 0xffff, 0xccff, 0xfc00, 0xffff, 0xffff, 0x7fff, + 0x9ffc, 0xffff, 0xffff, 0xc1ff, 0xffff, 0x0001, 0x0000, 0x9000, 0x3e1f, + 0xfffc, 0x07ff, 0xf0f8, 0xffff, 0xfff7, 0x3fff, 0x3ffe, 0xffff, 0xffff, + 0x01ff, 0xfffc, 0x0000, 0x0000, 0x8000, 0xff8f, 0xfff8, 0x07ff, 0xe1ff, + 0xffff, 0xffff, 0x1fff, 0x3ffe, 0xffff, 0xffff, 0x03ff, 0x7ff0, 0x0000, + 0x0000, 0xc000, 0xffe3, 0xffe3, 0xe07f, 0xc7ff, 0xffff, 0xfffb, 0x0fff, + 0x3dfb, 0xffff, 0xffff, 0x8fdf, 0x0000, 0x0000, 0x0000, 0xe000, 0xfff3, + 0xffc7, 0xf80f, 0x8fff, 0xffff, 0xfffb, 0x8fff, 0x33cf, 0xfffe, 0xffff, + 0x8fbf, 0x0000, 0x0004, 0x0000, 0xf000, 0xeff9, 0x0b1f, 0xffc0, 0x3fff, + 0xffff, 0xfffb, 0x07ff, 0x081c, 0xfffe, 0xffff, 0x18ff, 0x0001, 0x0001, + 0x0000, 0xf900, 0x9ffe, 0x007f, 0xfffe, 0x7fff, 0xfffe, 0xfffb, 0x07ff, + 0x0030, 0xfffe, 0xffff, 0x03ff, 0x2080, 0x0000, 0x0000, 0x7c80, 0x3ffe, + 0xfcff, 0xffff, 0x7fff, 0xfffe, 0xfffb, 0x03ff, 0x0001, 0xfffe, 0xffff, + 0x0f81, 0x0001, 0x0000, 0x0000, 0x7c00, 0x3801, 0xf3fc, 0xffff, 0xffff, + 0xfffc, 0xfffb, 0x03ff, 0x4020, 0xfffe, 0xffff, 0xe03f, 0x0002, 0x0000, + 0x0000, 0x3e00, 0x03f8, 0xcff9, 0xf77f, 0xffff, 0xfff9, 0xfffb, 0x11ff, + 0x0000, 0xffdf, 0xfffc, 0x00ff, 0x0002, 0x0000, 0x0000, 0x8f00, 0xbfff, + 0x3fe3, 0xfb7c, 0x3fff, 0xfffa, 0xfffb, 0x01ff, 0x4000, 0xf07e, 0x0003, + 0x1000, 0x0001, 0x0000, 0x0000, 0xc380, 0xffff, 0xffcf, 0xff82, 0xbfe5, + 0xfff1, 0xfff3, 0x00ff, 0x4000, 0x00fc, 0x0000, 0x0400, 0x0000, 0x0000, + 0x0000, 0xf3c0, 0xffff, 0xff8f, 0xfd9f, 0xbfde, 0xfff7, 0xfffb, 0x08ff, + 0x8000, 0x43e0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfce0, 0xffff, + 0xff00, 0xbcbf, 0xdfbf, 0xffc7, 0xfff3, 0x00ff, 0x0000, 0x0000, 0xa0d0, + 0x0006, 0x0000, 0x0000, 0x0000, 0xfe70, 0x1fff, 0xfe7c, 0xbc3f, 0xdfff, + 0x7fcf, 0xfff3, 0x047f, 0x0000, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xff3c, 0xc0ff, 0xf8fe, 0xddff, 0xde7f, 0x7f1f, 0xfff3, 0x003f, + 0x0000, 0x0480, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xff9e, 0xf81f, + 0xf1fe, 0xec07, 0xdeff, 0x7f3f, 0xfff3, 0x003f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x7fcd, 0x0ec0, 0x47fe, 0xe5e0, 0xdeff, + 0xfe3f, 0xfff2, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xc000, 0x07ee, 0x77f8, 0x0ffe, 0xf5fc, 0xddff, 0xfe7f, 0xfff6, 0x003f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x6000, 0x0007, 0x79ff, + 0x0ffe, 0xf0ff, 0xddff, 0xfcff, 0xfff4, 0x001f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xbe17, 0x7f7f, 0xe7ff, 0xf8ff, 0xd9ff, + 0xfcff, 0xfff4, 0x011f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xc000, 0xdf3b, 0x7f7f, 0x11ff, 0xf8fe, 0xdbff, 0xf8ff, 0xffe4, 0x001f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe200, 0xefbf, 0x3fbf, + 0xb1ff, 0xfcff, 0xd3ff, 0xf9ff, 0xffed, 0x001f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf800, 0xf79f, 0xbfdf, 0xcfff, 0xfcff, 0xd7ff, + 0xf1ff, 0xffe9, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfc40, 0xf79f, 0xdfdf, 0xe3ff, 0xfcff, 0xd7ff, 0xf3ff, 0xffe9, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff90, 0xfb9f, 0xcfe7, + 0xf800, 0xfeff, 0xd7ff, 0xf3ff, 0xffc3, 0x000f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfc00, 0xfd85, 0x07f3, 0xfc00, 0xfc7f, 0xafff, + 0xe3ff, 0xffd3, 0x0083, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0010, 0xfd80, 0x03f9, 0xde00, 0xfc7f, 0xafff, 0xe7ff, 0x7fc3, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0080, 0x7e80, 0x00fc, + 0xe700, 0xfc7f, 0xafff, 0xe7ff, 0x07c3, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x2000, 0x1e80, 0x203e, 0xf1c0, 0xfc7f, 0x8fff, + 0xe7ff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0300, 0x001e, 0xfc60, 0xf87f, 0x8fff, 0xe7ff, 0x0003, 0x0010, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0100, + 0xfe08, 0xf8ff, 0x4fff, 0xcfff, 0xc003, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe4, 0xe0ff, 0x4fff, + 0xcfff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff2, 0xc1ff, 0x4fff, 0xcfff, 0x0187, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0040, 0x0000, + 0xfffc, 0x83ff, 0x0fff, 0xcfff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xfffc, 0x0fff, 0x1ffe, + 0xcffe, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x1fff, 0x0ffc, 0xc7f8, 0x0007, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, + 0xf80f, 0x7fff, 0xcfe0, 0xe381, 0x004f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xffc0, 0xffff, 0xcf80, + 0xe007, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff00, 0xf7ff, 0xe007, 0xc00f, 0x0007, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0200, + 0xc000, 0xfc2f, 0xe01f, 0x303f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xf83d, + 0x003f, 0x0100, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0020, 0x8000, 0xff81, 0x0201, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x1fe0, 0x2040, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0001, + 0x0008, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x2000, 0x0000, 0x0002, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x4000, 0x2000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0202, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000 +}; -#define llama_mask_width 240 +#define llama_mask_width 240 #define llama_mask_height 260 static const unsigned short llama_mask_bits[] = { - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0e00, 0xfc00, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0f80, 0xfc00, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0f80, 0xfc00, 0x00ff, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0fc0, 0xfc00, 0x03ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x1fe0, 0xf800, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1ff0, 0xf000, 0x3fff, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x1ff0, 0xe000, 0x7fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x1ff8, 0xe000, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1ffc, 0xc000, 0xffff, - 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x1ffc, 0x8000, 0xffff, 0x0007, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x1ffc, 0x8000, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3ffe, 0x0000, 0xffff, - 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x3ffe, 0x0000, 0xfffe, 0x007f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x3fff, 0x0000, 0xfffc, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fff, 0x0000, 0xfff8, - 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x3fff, 0x0000, 0xfff0, 0x03ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x3fff, 0x0000, 0xffe0, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fff, 0x0000, 0xffc0, - 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x7fff, 0x0000, 0xff80, 0x1fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x7fff, 0x0000, 0xff80, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7fff, 0x0000, 0xff00, - 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffff, 0x0000, 0xfe00, 0x7fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xffff, 0x0000, 0xfc20, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0x0001, 0xf830, - 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x00c1, 0xf030, 0x7fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfffc, 0x6f81, 0xe030, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xff83, 0xe071, - 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0xff07, 0xc073, 0x7fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xffe0, 0xfdff, 0xc077, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xc07f, - 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xc0ff, 0x3fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfe00, 0xffff, 0xc0ff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x80ff, - 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x81ff, 0x1fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfc00, 0xffff, 0x81ff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x83ff, - 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0x83ff, 0x0fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0x07ff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0x0fff, - 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0x0fff, 0x07ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfff0, 0xffff, 0x1fff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0x3fff, - 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0x7fff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfff8, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, - 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff7c, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fbc, 0xfff0, 0xffff, - 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0f9e, 0xfff9, 0xffff, 0x003f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x878e, 0xfffb, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc186, 0xffff, 0xffff, - 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe082, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf002, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, - 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfe00, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, - 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xffc0, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, - 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfffc, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, - 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfff8, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, - 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfffe, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, - 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, - 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, - 0xffff, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, - 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, - 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, - 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, - 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, - 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, - 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, - 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, - 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x03ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, - 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, - 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, - 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, - 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, - 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, - 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, - 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, - 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, - 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, - 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffcf, 0xffff, - 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xffff, 0xffff, 0xffe3, 0xffff, 0xffff, 0xffff, 0x0001, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, - 0xffff, 0xffe1, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xfff1, 0xffff, - 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfff8, 0xffff, 0xfff8, 0xffff, 0xffff, 0xffff, 0x0003, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, - 0xffff, 0xfff8, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0x7fff, 0xfffc, 0xffff, - 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff80, 0x7fff, 0xfffe, 0xffff, 0xffff, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, - 0x3fff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xbfff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x000d, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf800, 0x9fff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0009, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, - 0xcfff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0013, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xefff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe7ff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf3fe, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf1f8, 0xffff, 0xffff, - 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, - 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, - 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, - 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, - 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0xffff, 0xffff, 0x7fff, 0x8000, 0x0001, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, - 0xffff, 0x3fff, 0x8000, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x07ff, 0xc000, - 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0xffff, 0xffff, 0x03ff, 0xe000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, - 0xffff, 0x003f, 0xe000, 0x3fc0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x0003, 0xf000, - 0xfff0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0xffff, 0x1fff, 0x0000, 0xf000, 0xfff8, 0x0001, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, - 0x07ff, 0x0000, 0xf000, 0xfff8, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x00ff, 0x0000, 0xf800, - 0xfffc, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0xfffe, 0x0007, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0x3fff, - 0x0000, 0x0000, 0xf800, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, - 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0xffff, 0x007f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfd80, 0xffff, 0x3fff, - 0x0000, 0x0000, 0xf800, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfd00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, - 0x03ff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfc00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0x01ff, 0x03fc, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x3fff, - 0x0000, 0x0000, 0xf800, 0x007f, 0x07c0, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, - 0x3fff, 0x0f00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfc00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0xffff, 0x0c00, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x3fff, - 0x0000, 0x0000, 0xf000, 0xffff, 0x0801, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf000, - 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf800, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf000, 0xffff, 0x0003, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x7fff, - 0x0000, 0x0000, 0xf000, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x7fff, 0x0000, 0x0000, 0xf000, - 0xfc3f, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf800, 0xffff, 0x7fff, 0x0000, 0x0000, 0xf000, 0xf00f, 0x001f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x7fff, - 0x0000, 0x0000, 0xe000, 0xc00f, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0x7fff, 0x0000, 0x0000, 0xe000, - 0x0007, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x8000, 0xffff, 0xffff, 0x0000, 0x0000, 0xe000, 0x0007, 0x003e, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, - 0x0000, 0x0000, 0xc000, 0x0007, 0x0038, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0x0000, 0x0000, 0xc000, - 0x0007, 0x0030, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfffe, 0xffff, 0x0000, 0x0000, 0xc000, 0x0007, 0x0060, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, - 0x0000, 0x0000, 0xc000, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0x0000, 0x0000, 0x8000, - 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfff8, 0xffff, 0x0001, 0x0000, 0x8000, 0x000f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, - 0x0001, 0x0000, 0x8000, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0x0001, 0x0000, 0xe000, - 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffe0, 0xffff, 0x0001, 0x0000, 0xf000, 0x7fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, - 0x0003, 0x0000, 0xf800, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0x0003, 0x0000, 0xfc00, - 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffc0, 0xffff, 0x0007, 0x0000, 0xfc00, 0xffff, 0x001f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, - 0x0fc7, 0x0000, 0xfe00, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x3fe7, 0x0000, 0xff00, - 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfc00, 0xffff, 0x7fe7, 0x0000, 0xff00, 0xffff, 0x01ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, - 0xffff, 0x0003, 0xff80, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0x0007, 0xff80, - 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0x000f, 0xffc0, 0xffff, 0x1fff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, - 0xffff, 0x003f, 0xffc0, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0x007f, 0xffc0, - 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0x00ff, 0xffc0, 0xffff, 0x3fff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, - 0xffff, 0x01ff, 0xfff8, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0x7fff, 0x001c, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x007c, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x01fc, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0x7fff, 0x07fc, 0x0000, 0x2000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0ff8, - 0x0000, 0xff00, 0x0007, 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x3ff8, 0x0000, 0xffc0, 0x003f, - 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xfff8, 0x0000, 0xffe0, 0x003f, 0x0000, 0x0000, 0x0000, - 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xfff1, - 0x0003, 0xfff0, 0x003f, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0xfff8, 0x00ff, - 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0x001f, 0xfff8, 0x00ff, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0x003f, 0xfffc, 0x00ff, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0xfffc, 0x00ff, - 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xf1ff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, - 0xfffc, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, - 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0xe000, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0xff7c, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, - 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x8000, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x007f, 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, - 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xf000, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x001f, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0xfc00, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xfffe, 0x0007, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xfff1, 0x0001, - 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0x3f81, 0x0000, 0x0000, 0xff80, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0xf9ff, 0xffff, 0xffff, - 0xffff, 0x0001, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x1fff, 0xc1e0, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, - 0x8000, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0xffe0, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, 0xc000, 0xffff, 0xffff, - 0xffff, 0x0001, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x0fff, 0xc000, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, - 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, - 0x8000, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0xfffe, 0xe0f0, - 0x0007, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x07ff, 0x0000, 0x7ffc, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x03ff, - 0x0000, 0x0f80, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, - 0xffe7, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x3fff, 0xfff0, 0xffff, 0xffff, - 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffe0, 0x03ff, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0x01ff, - 0xfff8, 0xffff, 0xffff, 0xffff, 0xffef, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x8fe0, 0x007f, 0xfffc, 0xffff, 0xffff, - 0xffff, 0x00df, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x87e0, 0x003f, 0xfffe, 0xffff, 0xffff, 0xffff, 0x019f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8040, 0x0007, - 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, - 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, - 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, - 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1e00, - 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0200, 0xfffc, 0xffff, 0xffff, - 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffff, 0xffff, 0xe07f, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, - 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xbf00, 0xffff, 0x0003, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xc000, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x03fe, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000 }; + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0e00, 0xfc00, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0f80, 0xfc00, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0f80, 0xfc00, 0x00ff, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0fc0, 0xfc00, 0x03ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x1fe0, 0xf800, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1ff0, 0xf000, 0x3fff, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x1ff0, 0xe000, 0x7fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x1ff8, 0xe000, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1ffc, 0xc000, 0xffff, + 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x1ffc, 0x8000, 0xffff, 0x0007, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x1ffc, 0x8000, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3ffe, 0x0000, 0xffff, + 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x3ffe, 0x0000, 0xfffe, 0x007f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x3fff, 0x0000, 0xfffc, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fff, 0x0000, 0xfff8, + 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x3fff, 0x0000, 0xfff0, 0x03ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x3fff, 0x0000, 0xffe0, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fff, 0x0000, 0xffc0, + 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x7fff, 0x0000, 0xff80, 0x1fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x7fff, 0x0000, 0xff80, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7fff, 0x0000, 0xff00, + 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffff, 0x0000, 0xfe00, 0x7fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xffff, 0x0000, 0xfc20, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0x0001, 0xf830, + 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x00c1, 0xf030, 0x7fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfffc, 0x6f81, 0xe030, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xff83, 0xe071, + 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0xff07, 0xc073, 0x7fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xffe0, 0xfdff, 0xc077, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xc07f, + 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xc0ff, 0x3fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfe00, 0xffff, 0xc0ff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x80ff, + 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x81ff, 0x1fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfc00, 0xffff, 0x81ff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x83ff, + 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0x83ff, 0x0fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0x07ff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0x0fff, + 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0x0fff, 0x07ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfff0, 0xffff, 0x1fff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0x3fff, + 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0x7fff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfff8, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, + 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff7c, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fbc, 0xfff0, 0xffff, + 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0f9e, 0xfff9, 0xffff, 0x003f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x878e, 0xfffb, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc186, 0xffff, 0xffff, + 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe082, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf002, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, + 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfe00, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, + 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xffc0, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, + 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfffc, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, + 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfff8, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, + 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfffe, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, + 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, + 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, + 0xffff, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, + 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, + 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, + 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, + 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, + 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, + 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, + 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, + 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x03ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, + 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, + 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, + 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, + 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, + 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, + 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, + 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, + 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, + 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, + 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffcf, 0xffff, + 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xffff, 0xffff, 0xffe3, 0xffff, 0xffff, 0xffff, 0x0001, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, + 0xffff, 0xffe1, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xfff1, 0xffff, + 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfff8, 0xffff, 0xfff8, 0xffff, 0xffff, 0xffff, 0x0003, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, + 0xffff, 0xfff8, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0x7fff, 0xfffc, 0xffff, + 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff80, 0x7fff, 0xfffe, 0xffff, 0xffff, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, + 0x3fff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xbfff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x000d, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf800, 0x9fff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0009, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, + 0xcfff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0013, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xefff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe7ff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf3fe, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf1f8, 0xffff, 0xffff, + 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, + 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, + 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, + 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, + 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0xffff, 0xffff, 0x7fff, 0x8000, 0x0001, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, + 0xffff, 0x3fff, 0x8000, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x07ff, 0xc000, + 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0xffff, 0xffff, 0x03ff, 0xe000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, + 0xffff, 0x003f, 0xe000, 0x3fc0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x0003, 0xf000, + 0xfff0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0xffff, 0x1fff, 0x0000, 0xf000, 0xfff8, 0x0001, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, + 0x07ff, 0x0000, 0xf000, 0xfff8, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x00ff, 0x0000, 0xf800, + 0xfffc, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0xfffe, 0x0007, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0x3fff, + 0x0000, 0x0000, 0xf800, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, + 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0xffff, 0x007f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfd80, 0xffff, 0x3fff, + 0x0000, 0x0000, 0xf800, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfd00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, + 0x03ff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfc00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0x01ff, 0x03fc, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x3fff, + 0x0000, 0x0000, 0xf800, 0x007f, 0x07c0, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, + 0x3fff, 0x0f00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfc00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0xffff, 0x0c00, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x3fff, + 0x0000, 0x0000, 0xf000, 0xffff, 0x0801, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf000, + 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf800, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf000, 0xffff, 0x0003, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x7fff, + 0x0000, 0x0000, 0xf000, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x7fff, 0x0000, 0x0000, 0xf000, + 0xfc3f, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf800, 0xffff, 0x7fff, 0x0000, 0x0000, 0xf000, 0xf00f, 0x001f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x7fff, + 0x0000, 0x0000, 0xe000, 0xc00f, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0x7fff, 0x0000, 0x0000, 0xe000, + 0x0007, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x8000, 0xffff, 0xffff, 0x0000, 0x0000, 0xe000, 0x0007, 0x003e, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, + 0x0000, 0x0000, 0xc000, 0x0007, 0x0038, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0x0000, 0x0000, 0xc000, + 0x0007, 0x0030, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfffe, 0xffff, 0x0000, 0x0000, 0xc000, 0x0007, 0x0060, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, + 0x0000, 0x0000, 0xc000, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0x0000, 0x0000, 0x8000, + 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfff8, 0xffff, 0x0001, 0x0000, 0x8000, 0x000f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, + 0x0001, 0x0000, 0x8000, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0x0001, 0x0000, 0xe000, + 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffe0, 0xffff, 0x0001, 0x0000, 0xf000, 0x7fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, + 0x0003, 0x0000, 0xf800, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0x0003, 0x0000, 0xfc00, + 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffc0, 0xffff, 0x0007, 0x0000, 0xfc00, 0xffff, 0x001f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, + 0x0fc7, 0x0000, 0xfe00, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x3fe7, 0x0000, 0xff00, + 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfc00, 0xffff, 0x7fe7, 0x0000, 0xff00, 0xffff, 0x01ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, + 0xffff, 0x0003, 0xff80, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0x0007, 0xff80, + 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0x000f, 0xffc0, 0xffff, 0x1fff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, + 0xffff, 0x003f, 0xffc0, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0x007f, 0xffc0, + 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0x00ff, 0xffc0, 0xffff, 0x3fff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, + 0xffff, 0x01ff, 0xfff8, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0x7fff, 0x001c, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x007c, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x01fc, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0x7fff, 0x07fc, 0x0000, 0x2000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0ff8, + 0x0000, 0xff00, 0x0007, 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x3ff8, 0x0000, 0xffc0, 0x003f, + 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xfff8, 0x0000, 0xffe0, 0x003f, 0x0000, 0x0000, 0x0000, + 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xfff1, + 0x0003, 0xfff0, 0x003f, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0xfff8, 0x00ff, + 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0x001f, 0xfff8, 0x00ff, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0x003f, 0xfffc, 0x00ff, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0xfffc, 0x00ff, + 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xf1ff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, + 0xfffc, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, + 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0xe000, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0xff7c, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, + 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x8000, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x007f, 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, + 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xf000, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x001f, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0xfc00, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xfffe, 0x0007, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xfff1, 0x0001, + 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0x3f81, 0x0000, 0x0000, 0xff80, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0xf9ff, 0xffff, 0xffff, + 0xffff, 0x0001, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x1fff, 0xc1e0, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, + 0x8000, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0xffe0, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, 0xc000, 0xffff, 0xffff, + 0xffff, 0x0001, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x0fff, 0xc000, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, + 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, + 0x8000, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0xfffe, 0xe0f0, + 0x0007, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x07ff, 0x0000, 0x7ffc, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x03ff, + 0x0000, 0x0f80, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, + 0xffe7, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x3fff, 0xfff0, 0xffff, 0xffff, + 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffe0, 0x03ff, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0x01ff, + 0xfff8, 0xffff, 0xffff, 0xffff, 0xffef, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x8fe0, 0x007f, 0xfffc, 0xffff, 0xffff, + 0xffff, 0x00df, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x87e0, 0x003f, 0xfffe, 0xffff, 0xffff, 0xffff, 0x019f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8040, 0x0007, + 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, + 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, + 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, + 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1e00, + 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0200, 0xfffc, 0xffff, 0xffff, + 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffff, 0xffff, 0xe07f, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, + 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xbf00, 0xffff, 0x0003, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xc000, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x03fe, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000 +}; #endif /* SPLASH_H_ */ diff --git a/flight/targets/boards/osd/firmware/osd.c b/flight/targets/boards/osd/firmware/osd.c index 499d1d8ca..047bbe01c 100644 --- a/flight/targets/boards/osd/firmware/osd.c +++ b/flight/targets/boards/osd/firmware/osd.c @@ -1,4 +1,3 @@ - /** ****************************************************************************** * @@ -27,7 +26,7 @@ // ***************************************************************************** -//#define USE_WATCHDOG // comment this out if you don't want to use the watchdog +// #define USE_WATCHDOG // comment this out if you don't want to use the watchdog // ***************************************************************************** @@ -35,21 +34,21 @@ #include /* Task Priorities */ -#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) /* Global Variables */ /* Prototype of PIOS_Board_Init() function */ extern void PIOS_Board_Init(void); extern void Stack_Change(void); -static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change"))); +static void Stack_Change_Weak() __attribute__((weakref("Stack_Change"))); /* Function Prototypes */ static void initTask(void *parameters); /* Local Variables */ -#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority -#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive +#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority +#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive static xTaskHandle initTaskHandle; // ***************************************************************************** @@ -59,49 +58,49 @@ static xTaskHandle initTaskHandle; // Local Variables #if defined(USE_WATCHDOG) - volatile uint16_t watchdog_timer; - uint16_t watchdog_delay; +volatile uint16_t watchdog_timer; +uint16_t watchdog_delay; #endif // ***************************************************************************** #if defined(USE_WATCHDOG) - void processWatchdog(void) - { - // random32 = UpdateCRC32(random32, IWDG->SR); +void processWatchdog(void) +{ + // random32 = UpdateCRC32(random32, IWDG->SR); - if (watchdog_timer < watchdog_delay) - return; + if (watchdog_timer < watchdog_delay) { + return; + } - // the watchdog needs resetting + // the watchdog needs resetting - watchdog_timer = 0; + watchdog_timer = 0; - watchdog_Clear(); - } + watchdog_Clear(); +} - void enableWatchdog(void) - { // enable a watchdog - watchdog_timer = 0; - watchdog_delay = watchdog_Init(1000); // 1 second watchdog timeout - } +void enableWatchdog(void) +{ // enable a watchdog + watchdog_timer = 0; + watchdog_delay = watchdog_Init(1000); // 1 second watchdog timeout +} -#endif +#endif /* if defined(USE_WATCHDOG) */ // ***************************************************************************** void sequenceLEDs(void) { - for (int i = 0; i < 2; i++) - { - //USB_LED_ON; + for (int i = 0; i < 2; i++) { + // USB_LED_ON; PIOS_DELAY_WaitmS(100); - //USB_LED_OFF; + // USB_LED_OFF; PIOS_DELAY_WaitmS(100); #if defined(USE_WATCHDOG) - processWatchdog(); // process the watchdog + processWatchdog(); // process the watchdog #endif } } @@ -111,109 +110,103 @@ void sequenceLEDs(void) void processReset(void) { - if (RCC_GetFlagStatus(RCC_FLAG_IWDGRST) != RESET) - { // Independant Watchdog Reset - + if (RCC_GetFlagStatus(RCC_FLAG_IWDGRST) != RESET) { // Independant Watchdog Reset #if defined(PIOS_COM_DEBUG_CONSOLE) - DEBUG_PRINTF(0, "\r\nINDEPENDANT WATCHDOG CAUSED A RESET\r\n"); + DEBUG_PRINTF(0, "\r\nINDEPENDANT WATCHDOG CAUSED A RESET\r\n"); #endif // all led's ON - //USB_LED_ON; + // USB_LED_ON; - PIOS_DELAY_WaitmS(500); // delay a bit + PIOS_DELAY_WaitmS(500); // delay a bit // all led's OFF - //USB_LED_OFF; - - + // USB_LED_OFF; } /* - if (RCC_GetFlagStatus(RCC_FLAG_WWDGRST) != RESET) - { // Window Watchdog Reset + if (RCC_GetFlagStatus(RCC_FLAG_WWDGRST) != RESET) + { // Window Watchdog Reset - DEBUG_PRINTF(0, "\r\nWINDOW WATCHDOG CAUSED A REBOOT\r\n"); + DEBUG_PRINTF(0, "\r\nWINDOW WATCHDOG CAUSED A REBOOT\r\n"); - // all led's ON - USB_LED_ON; - LINK_LED_ON; - RX_LED_ON; - TX_LED_ON; + // all led's ON + USB_LED_ON; + LINK_LED_ON; + RX_LED_ON; + TX_LED_ON; - PIOS_DELAY_WaitmS(500); // delay a bit + PIOS_DELAY_WaitmS(500); // delay a bit - // all led's OFF - USB_LED_OFF; - LINK_LED_OFF; - RX_LED_OFF; - TX_LED_OFF; - } -*/ - if (RCC_GetFlagStatus(RCC_FLAG_PORRST) != RESET) - { // Power-On Reset + // all led's OFF + USB_LED_OFF; + LINK_LED_OFF; + RX_LED_OFF; + TX_LED_OFF; + } + */ + if (RCC_GetFlagStatus(RCC_FLAG_PORRST) != RESET) { // Power-On Reset #if defined(PIOS_COM_DEBUG) - DEBUG_PRINTF(0, "\r\nPOWER-ON-RESET\r\n"); + DEBUG_PRINTF(0, "\r\nPOWER-ON-RESET\r\n"); #endif } - if (RCC_GetFlagStatus(RCC_FLAG_SFTRST) != RESET) - { // Software Reset + if (RCC_GetFlagStatus(RCC_FLAG_SFTRST) != RESET) { // Software Reset #if defined(PIOS_COM_DEBUG) - DEBUG_PRINTF(0, "\r\nSOFTWARE RESET\r\n"); + DEBUG_PRINTF(0, "\r\nSOFTWARE RESET\r\n"); #endif } - if (RCC_GetFlagStatus(RCC_FLAG_LPWRRST) != RESET) - { // Low-Power Reset + if (RCC_GetFlagStatus(RCC_FLAG_LPWRRST) != RESET) { // Low-Power Reset #if defined(PIOS_COM_DEBUG) - DEBUG_PRINTF(0, "\r\nLOW POWER RESET\r\n"); + DEBUG_PRINTF(0, "\r\nLOW POWER RESET\r\n"); #endif } - if (RCC_GetFlagStatus(RCC_FLAG_PINRST) != RESET) - { // Pin Reset + if (RCC_GetFlagStatus(RCC_FLAG_PINRST) != RESET) { // Pin Reset #if defined(PIOS_COM_DEBUG) - DEBUG_PRINTF("0, \r\nPIN RESET\r\n"); + DEBUG_PRINTF("0, \r\nPIN RESET\r\n"); #endif } // Clear reset flags - //RCC_ClearFlag(); + // RCC_ClearFlag(); } int main() { - int result; + int result; + // ************* // init various variables // ************* - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ - vPortInitialiseBlocks(); + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + vPortInitialiseBlocks(); - // Bring up System using CMSIS functions, enables the LEDs. - PIOS_SYS_Init(); + // Bring up System using CMSIS functions, enables the LEDs. + PIOS_SYS_Init(); - /* For Revolution we use a FreeRTOS task to bring up the system so we can */ - /* always rely on FreeRTOS primitive */ - result = xTaskCreate(initTask, (const signed char *)"init", - INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, - &initTaskHandle); - PIOS_Assert(result == pdPASS); + /* For Revolution we use a FreeRTOS task to bring up the system so we can */ + /* always rely on FreeRTOS primitive */ + result = xTaskCreate(initTask, (const signed char *)"init", + INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, + &initTaskHandle); + PIOS_Assert(result == pdPASS); - /* Start the FreeRTOS scheduler which should never returns.*/ - vTaskStartScheduler(); + /* Start the FreeRTOS scheduler which should never returns.*/ + vTaskStartScheduler(); - /* If all is well we will never reach here as the scheduler will now be running. */ - /* Do some indication to user that something bad just happened */ - PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ - for(;;) { \ - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ - PIOS_DELAY_WaitmS(100); \ - }; + /* If all is well we will never reach here as the scheduler will now be running. */ + /* Do some indication to user that something bad just happened */ + PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ + for (;;) { \ + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ + PIOS_DELAY_WaitmS(100); \ + } + ; - return 0; + return 0; } /** @@ -221,17 +214,16 @@ int main() * * Runs board and module initialisation, then terminates. */ -void -initTask(__attribute__((unused)) void *parameters) +void initTask(__attribute__((unused)) void *parameters) { - /* board driver init */ - PIOS_Board_Init(); + /* board driver init */ + PIOS_Board_Init(); - /* Initialize modules */ - MODULE_INITIALISE_ALL; + /* Initialize modules */ + MODULE_INITIALISE_ALL; - /* terminate this task */ - vTaskDelete(NULL); + /* terminate this task */ + vTaskDelete(NULL); } // ***************************************************************************** diff --git a/flight/targets/boards/osd/firmware/pios_board.c b/flight/targets/boards/osd/firmware/pios_board.c index a241c27e9..ea9b6e954 100644 --- a/flight/targets/boards/osd/firmware/pios_board.c +++ b/flight/targets/boards/osd/firmware/pios_board.c @@ -42,7 +42,7 @@ #include "../board_hw_defs.c" /* Private macro -------------------------------------------------------------*/ -#define countof(a) (sizeof(a) / sizeof(*(a))) +#define countof(a) (sizeof(a) / sizeof(*(a))) /* Private variables ---------------------------------------------------------*/ @@ -51,54 +51,53 @@ void PIOS_ADC_DMC_irq_handler(void); void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); struct pios_adc_cfg pios_adc_cfg = { - .adc_dev = ADC1, - .dma = { - .irq = { - .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), - .init = { - .NVIC_IRQChannel = DMA2_Stream4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA2_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR - }, - } - }, - .half_flag = DMA_IT_HTIF4, - .full_flag = DMA_IT_TCIF4, - + .adc_dev = ADC1, + .dma = { + .irq = { + .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), + .init = { + .NVIC_IRQChannel = DMA2_Stream4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR + }, + } + }, + .half_flag = DMA_IT_HTIF4, + .full_flag = DMA_IT_TCIF4, }; void PIOS_ADC_DMC_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_ADC_DMA_Handler(); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_ADC_DMA_Handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_ADC) */ static void Clock(uint32_t spektrum_id); -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 128 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 128 +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 128 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 128 -#define PIOS_COM_AUX_RX_BUF_LEN 512 -#define PIOS_COM_AUX_TX_BUF_LEN 512 +#define PIOS_COM_AUX_RX_BUF_LEN 512 +#define PIOS_COM_AUX_TX_BUF_LEN 512 -#define PIOS_COM_GPS_RX_BUF_LEN 32 +#define PIOS_COM_GPS_RX_BUF_LEN 32 #define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 uint32_t pios_com_aux_id; uint32_t pios_com_gps_id; @@ -106,388 +105,389 @@ uint32_t pios_com_telem_usb_id; uint32_t pios_com_telem_rf_id; /** - * TIM3 is triggered by the HSYNC signal into its ETR line and will divide the + * TIM3 is triggered by the HSYNC signal into its ETR line and will divide the * APB1_CLOCK to generate a pixel clock that is used by the SPI CLK lines. * TIM4 will be synced to it and will divide by that times the pixel width to * fire an IRQ when the last pixel of the line has been output. Then the timer will - * be rearmed and wait for the next HSYNC signal. + * be rearmed and wait for the next HSYNC signal. * The critical timing detail is that the task be _DISABLED_ at the end of the line * before an extra pixel is clocked out * or we will need to configure the DMA task per line */ #include "pios_tim_priv.h" -#define NTSC_PX_CLOCK 6797088 -#define PAL_PX_CLOCK 6750130 -#define PX_PERIOD ((PIOS_PERIPHERAL_APB1_CLOCK / NTSC_PX_CLOCK) + 1) -#define LINE_PERIOD PX_PERIOD * GRAPHICS_WIDTH +#define NTSC_PX_CLOCK 6797088 +#define PAL_PX_CLOCK 6750130 +#define PX_PERIOD ((PIOS_PERIPHERAL_APB1_CLOCK / NTSC_PX_CLOCK) + 1) +#define LINE_PERIOD PX_PERIOD * GRAPHICS_WIDTH static const TIM_TimeBaseInitTypeDef tim_4_time_base = { - .TIM_Prescaler = 0, //PIOS_PERIPHERAL_APB1_CLOCK, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = LINE_PERIOD - 1, - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = 0, // PIOS_PERIPHERAL_APB1_CLOCK, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = LINE_PERIOD - 1, + .TIM_RepetitionCounter = 0x0000, }; static const struct pios_tim_clock_cfg pios_tim4_cfg = { - .timer = TIM4, - .time_base_init = &tim_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - } + .timer = TIM4, + .time_base_init = &tim_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + } }; -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + // Delay system + PIOS_DELAY_Init(); - // Delay system - PIOS_DELAY_Init(); - - PIOS_LED_Init(&pios_led_cfg); + PIOS_LED_Init(&pios_led_cfg); #if defined(PIOS_INCLUDE_SPI) - /* Set up the SPI interface to the SD card */ - if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) { - PIOS_Assert(0); - } + /* Set up the SPI interface to the SD card */ + if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) { + PIOS_Assert(0); + } #if defined(PIOS_INCLUDE_SDCARD) - /* Enable and mount the SDCard */ - PIOS_SDCARD_Init(pios_spi_sdcard_id); - PIOS_SDCARD_MountFS(0); + /* Enable and mount the SDCard */ + PIOS_SDCARD_Init(pios_spi_sdcard_id); + PIOS_SDCARD_MountFS(0); #endif #endif /* PIOS_INCLUDE_SPI */ #ifdef PIOS_INCLUDE_FLASH_SECTOR_SETTINGS -uintptr_t flash_id; -uintptr_t fs_id; -PIOS_Flash_Internal_Init(&flash_id, &flash_internal_cfg); -PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id); + uintptr_t flash_id; + uintptr_t fs_id; + PIOS_Flash_Internal_Init(&flash_id, &flash_internal_cfg); + PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id); #elif !defined(PIOS_USE_SETTINGS_ON_SDCARD) #error No setting storage specified. (define PIOS_USE_SETTINGS_ON_SDCARD or INCLUDE_FLASH_SECTOR_SETTINGS) #endif - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); - HwSettingsInitialize(); + HwSettingsInitialize(); #ifdef PIOS_INCLUDE_WDG - /* Initialize watchdog as early as possible to catch faults during init */ - PIOS_WDG_Init(); + /* Initialize watchdog as early as possible to catch faults during init */ + PIOS_WDG_Init(); #endif /* PIOS_INCLUDE_WDG */ - /* Initialize the alarms library */ - AlarmsInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); - /* IAP System Setup */ - PIOS_IAP_Init(); - uint16_t boot_count = PIOS_IAP_ReadBootCount(); - if (boot_count < 3) { - PIOS_IAP_WriteBootCount(++boot_count); - AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); - } else { - /* Too many failed boot attempts, force hwsettings to defaults */ - HwSettingsSetDefaults(HwSettingsHandle(), 0); - AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); - } + /* IAP System Setup */ + PIOS_IAP_Init(); + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } #if defined(PIOS_INCLUDE_RTC) - /* Initialize the real-time clock and its associated tick */ - PIOS_RTC_Init(&pios_rtc_main_cfg); - if (!PIOS_RTC_RegisterTickCallback(Clock, 0)) { - PIOS_DEBUG_Assert(0); - } + /* Initialize the real-time clock and its associated tick */ + PIOS_RTC_Init(&pios_rtc_main_cfg); + if (!PIOS_RTC_RegisterTickCallback(Clock, 0)) { + PIOS_DEBUG_Assert(0); + } #endif #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; + /* Flags to determine if various USB interfaces are advertised */ + bool usb_hid_present = false; + bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; + usb_cdc_present = true; #else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; #endif - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_CDC) - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); + uint8_t hwsettings_usb_vcpport; + /* Configure the USB VCP port */ + HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } + if (!usb_cdc_present) { + /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ + hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; + } - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: + switch (hwsettings_usb_vcpport) { + case HWSETTINGS_USB_VCPPORT_DISABLED: + break; + case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: + { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ + { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, + tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + } +#endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); + /* Configure the usb HID port */ + uint8_t hwsettings_usb_hidport; + HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } + if (!usb_hid_present) { + /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ + hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; + } - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: + switch (hwsettings_usb_hidport) { + case HWSETTINGS_USB_HIDPORT_DISABLED: + break; + case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } + { + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + } -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ - if (usb_hid_present || usb_cdc_present) { - PIOS_USBHOOK_Activate(); - } -#endif /* PIOS_INCLUDE_USB */ + if (usb_hid_present || usb_cdc_present) { + PIOS_USBHOOK_Activate(); + } +#endif /* PIOS_INCLUDE_USB */ #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_GPS) - uint32_t pios_usart_gps_id; - if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { - PIOS_Assert(0); - } + uint32_t pios_usart_gps_id; + if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { + PIOS_Assert(0); + } - uint8_t *gps_rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(gps_rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, - gps_rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); - } + uint8_t *gps_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(gps_rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, + gps_rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } -#endif /* PIOS_INCLUDE_GPS */ +#endif /* PIOS_INCLUDE_GPS */ #if defined(PIOS_INCLUDE_COM_AUX) - { - uint32_t pios_usart_aux_id; + { + uint32_t pios_usart_aux_id; - if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) { - PIOS_DEBUG_Assert(0); - } + if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) { + PIOS_DEBUG_Assert(0); + } - uint8_t *aux_rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_RX_BUF_LEN); - uint8_t *aux_tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_TX_BUF_LEN); - PIOS_Assert(aux_rx_buffer); - PIOS_Assert(aux_tx_buffer); + uint8_t *aux_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_AUX_RX_BUF_LEN); + uint8_t *aux_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_AUX_TX_BUF_LEN); + PIOS_Assert(aux_rx_buffer); + PIOS_Assert(aux_tx_buffer); - if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id, - aux_rx_buffer, PIOS_COM_AUX_RX_BUF_LEN, - aux_tx_buffer, PIOS_COM_AUX_TX_BUF_LEN)) { - PIOS_DEBUG_Assert(0); - } - } + if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id, + aux_rx_buffer, PIOS_COM_AUX_RX_BUF_LEN, + aux_tx_buffer, PIOS_COM_AUX_TX_BUF_LEN)) { + PIOS_DEBUG_Assert(0); + } + } #else - pios_com_aux_id = 0; -#endif /* PIOS_INCLUDE_COM_AUX */ + pios_com_aux_id = 0; +#endif /* PIOS_INCLUDE_COM_AUX */ #if defined(PIOS_INCLUDE_COM_TELEM) - { /* Eventually add switch for this port function */ - uint32_t pios_usart_telem_rf_id; - if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_main_cfg)) { - PIOS_Assert(0); - } + { /* Eventually add switch for this port function */ + uint32_t pios_usart_telem_rf_id; + if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_main_cfg)) { + PIOS_Assert(0); + } - uint8_t *telem_rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t *telem_tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(telem_rx_buffer); - PIOS_Assert(telem_tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id, - telem_rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - telem_tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + uint8_t *telem_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *telem_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(telem_rx_buffer); + PIOS_Assert(telem_tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id, + telem_rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + telem_tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } #else - pios_com_telem_rf_id = 0; -#endif /* PIOS_INCLUDE_COM_TELEM */ + pios_com_telem_rf_id = 0; +#endif /* PIOS_INCLUDE_COM_TELEM */ -#endif /* PIOS_INCLUDE_COM */ +#endif /* PIOS_INCLUDE_COM */ - /* Configure FlexiPort */ + /* Configure FlexiPort */ /* uint8_t hwsettings_rv_flexiport; - HwSettingsRV_FlexiPortGet(&hwsettings_rv_flexiport); + HwSettingsRV_FlexiPortGet(&hwsettings_rv_flexiport); - switch (hwsettings_rv_flexiport) { - case HWSETTINGS_RV_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_RV_FLEXIPORT_I2C:*/ + switch (hwsettings_rv_flexiport) { + case HWSETTINGS_RV_FLEXIPORT_DISABLED: + break; + case HWSETTINGS_RV_FLEXIPORT_I2C:*/ #if defined(PIOS_INCLUDE_I2C) - { - if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_I2C */ + { + if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_I2C */ /* break; - case HWSETTINGS_RV_FLEXIPORT_DSM2: - case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: - case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: - { - enum pios_dsm_proto proto; - switch (hwsettings_rv_flexiport) { - case HWSETTINGS_RV_FLEXIPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } - //TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, - &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hwsettings_DSMxBind); - } - break; - case HWSETTINGS_RV_FLEXIPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - case HWSETTINGS_RV_FLEXIPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - }*/ - /* hwsettings_rv_flexiport */ + case HWSETTINGS_RV_FLEXIPORT_DSM2: + case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: + case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: + { + enum pios_dsm_proto proto; + switch (hwsettings_rv_flexiport) { + case HWSETTINGS_RV_FLEXIPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } + //TODO: Define the various Channelgroup for Revo dsm inputs and handle here + PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, + &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hwsettings_DSMxBind); + } + break; + case HWSETTINGS_RV_FLEXIPORT_COMAUX: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); + break; + case HWSETTINGS_RV_FLEXIPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + }*/ + /* hwsettings_rv_flexiport */ #if defined(PIOS_INCLUDE_WAVE) - PIOS_WavPlay_Init(&pios_dac_cfg); + PIOS_WavPlay_Init(&pios_dac_cfg); #endif - // ADC system + // ADC system #if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); + PIOS_ADC_Init(&pios_adc_cfg); #endif #if defined(PIOS_INCLUDE_VIDEO) - PIOS_TIM_InitClock(&tim_8_cfg); - PIOS_Servo_Init(&pios_servo_cfg); - // Start the pixel and line clock counter - //PIOS_TIM_InitClock(&pios_tim4_cfg); - PIOS_Video_Init(&pios_video_cfg); + PIOS_TIM_InitClock(&tim_8_cfg); + PIOS_Servo_Init(&pios_servo_cfg); + // Start the pixel and line clock counter + // PIOS_TIM_InitClock(&pios_tim4_cfg); + PIOS_Video_Init(&pios_video_cfg); #endif } -uint16_t supv_timer=0; +uint16_t supv_timer = 0; -static void Clock(__attribute__((unused)) uint32_t spektrum_id) { - /* 125hz */ - ++supv_timer; - if(supv_timer >= 625) { - supv_timer = 0; - timex.sec++; - } - if (timex.sec >= 60) { - timex.sec = 0; - timex.min++; - } - if (timex.min >= 60) { - timex.min = 0; - timex.hour++; - } - if (timex.hour >= 99) { - timex.hour = 0; - } +static void Clock(__attribute__((unused)) uint32_t spektrum_id) +{ + /* 125hz */ + ++supv_timer; + if (supv_timer >= 625) { + supv_timer = 0; + timex.sec++; + } + if (timex.sec >= 60) { + timex.sec = 0; + timex.min++; + } + if (timex.min >= 60) { + timex.min = 0; + timex.hour++; + } + if (timex.hour >= 99) { + timex.hour = 0; + } } diff --git a/flight/targets/boards/osd/pios_board.h b/flight/targets/boards/osd/pios_board.h index c4d1e34b5..3a96a11d8 100644 --- a/flight/targets/boards/osd/pios_board.h +++ b/flight/targets/boards/osd/pios_board.h @@ -32,170 +32,170 @@ // Timers and Channels Used /* - Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 - ------+------------+------------+------------+------------ - TIM1 | DELAY | - TIM2 | | PPM Output | PPM Input | - TIM3 | TIMER INTERRUPT | - TIM4 | STOPWATCH | - ------+------------+------------+------------+------------ + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+------------+------------+------------+------------ + TIM1 | DELAY | + TIM2 | | PPM Output | PPM Input | + TIM3 | TIMER INTERRUPT | + TIM4 | STOPWATCH | + ------+------------+------------+------------+------------ */ -//------------------------ +// ------------------------ // DMA Channels Used -//------------------------ -/* Channel 1 - */ -/* Channel 2 - */ -/* Channel 3 - */ -/* Channel 4 - */ -/* Channel 5 - */ -/* Channel 6 - */ -/* Channel 7 - */ -/* Channel 8 - */ -/* Channel 9 - */ -/* Channel 10 - */ -/* Channel 11 - */ -/* Channel 12 - */ +// ------------------------ +/* Channel 1 - */ +/* Channel 2 - */ +/* Channel 3 - */ +/* Channel 4 - */ +/* Channel 5 - */ +/* Channel 6 - */ +/* Channel 7 - */ +/* Channel 8 - */ +/* Channel 9 - */ +/* Channel 10 - */ +/* Channel 11 - */ +/* Channel 12 - */ -//------------------------ +// ------------------------ // BOOTLOADER_SETTINGS -//------------------------ -#define BOARD_READABLE true -#define BOARD_WRITABLE true -#define MAX_DEL_RETRYS 3 +// ------------------------ +#define BOARD_READABLE true +#define BOARD_WRITABLE true +#define MAX_DEL_RETRYS 3 -//------------------------- +// ------------------------- // System Settings // // See also System_stm32f4xx.c -//------------------------- -//These macros are deprecated -//please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below -//#define PIOS_MASTER_CLOCK -//#define PIOS_PERIPHERAL_CLOCK -//#define PIOS_PERIPHERAL_CLOCK +// ------------------------- +// These macros are deprecated +// please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below +// #define PIOS_MASTER_CLOCK +// #define PIOS_PERIPHERAL_CLOCK +// #define PIOS_PERIPHERAL_CLOCK -#define PIOS_SYSCLK 168000000 -// Peripherals that belongs to APB1 are: -// DAC |PWR |CAN1,2 -// I2C1,2,3 |UART4,5 |USART3,2 -// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 -// I2S2Ext |IWDG |WWDG -// RTC/BKP reg +#define PIOS_SYSCLK 168000000 +// Peripherals that belongs to APB1 are: +// DAC |PWR |CAN1,2 +// I2C1,2,3 |UART4,5 |USART3,2 +// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 +// I2S2Ext |IWDG |WWDG +// RTC/BKP reg // TIM2,3,4,5,6,7,12,13,14 // Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) // Default APB1 Prescaler = 4 -#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) +#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) -// Peripherals belonging to APB2 -// SDIO |EXTI |SYSCFG |SPI1 -// ADC1,2,3 -// USART1,6 -// TIM1,8,9,10,11 +// Peripherals belonging to APB2 +// SDIO |EXTI |SYSCFG |SPI1 +// ADC1,2,3 +// USART1,6 +// TIM1,8,9,10,11 // // Default APB2 Prescaler = 2 // -#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK +#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK -//------------------------ +// ------------------------ // TELEMETRY -//------------------------ -#define TELEM_QUEUE_SIZE 20 -#define PIOS_TELEM_STACK_SIZE 624 +// ------------------------ +#define TELEM_QUEUE_SIZE 20 +#define PIOS_TELEM_STACK_SIZE 624 // ***************************************************************** // Interrupt Priorities -#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS -#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS -#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... -#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... -//------------------------ +// ------------------------ // WATCHDOG_SETTINGS -//------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -#define PIOS_WDG_REGISTER RTC_BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_OSDGEN 0x0010 +// ------------------------ +#define PIOS_WATCHDOG_TIMEOUT 250 +#define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_OSDGEN 0x0010 // ***************************************************************** // PIOS_LED -#define PIOS_LED_HEARTBEAT 0 -#define PIOS_LED_ALARM 1 +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_LED_ALARM 1 // ***************************************************************** // Delay Timer -//#define PIOS_DELAY_TIMER TIM2 -//#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE) -#define PIOS_DELAY_TIMER TIM1 -#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE) +// #define PIOS_DELAY_TIMER TIM2 +// #define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE) +#define PIOS_DELAY_TIMER TIM1 +#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE) // ***************************************************************** // Timer interrupt /*#define TIMER_INT_TIMER TIM3 - #define TIMER_INT_FUNC TIM3_IRQHandler - #define TIMER_INT_PRIORITY 2 + #define TIMER_INT_FUNC TIM3_IRQHandler + #define TIMER_INT_PRIORITY 2 - // ***************************************************************** - // Stop watch timer + // ***************************************************************** + // Stop watch timer - #define STOPWATCH_TIMER TIM4*/ +#define STOPWATCH_TIMER TIM4*/ -//------------------------ +// ------------------------ // PIOS_SPI // See also pios_board.c -//------------------------ -#define PIOS_SPI_MAX_DEVS 1 +// ------------------------ +#define PIOS_SPI_MAX_DEVS 1 extern uint32_t pios_spi_port_id; -#define PIOS_SPI_PORT (pios_spi_port_id) +#define PIOS_SPI_PORT (pios_spi_port_id) -//------------------------- +// ------------------------- // PIOS_USART // // See also pios_board.c -//------------------------- -#define PIOS_USART_MAX_DEVS 5 +// ------------------------- +#define PIOS_USART_MAX_DEVS 5 -//------------------------- +// ------------------------- // PIOS_COM // // See also pios_board.c -//------------------------- -#define PIOS_COM_MAX_DEVS 5 +// ------------------------- +#define PIOS_COM_MAX_DEVS 5 extern uint32_t pios_com_telem_rf_id; extern uint32_t pios_com_gps_id; extern uint32_t pios_com_aux_id; extern uint32_t pios_com_telem_usb_id; -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_DEBUG (PIOS_COM_AUX) -#define PIOS_COM_OSD (pios_com_aux_id) +#define PIOS_COM_AUX (pios_com_aux_id) +#define PIOS_COM_GPS (pios_com_gps_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_DEBUG (PIOS_COM_AUX) +#define PIOS_COM_OSD (pios_com_aux_id) -//extern uint32_t pios_com_serial_id; -//#define PIOS_COM_SERIAL (pios_com_serial_id) -//#define PIOS_COM_DEBUG PIOS_COM_SERIAL // uncomment this to send debug info out the serial port +// extern uint32_t pios_com_serial_id; +// #define PIOS_COM_SERIAL (pios_com_serial_id) +// #define PIOS_COM_DEBUG PIOS_COM_SERIAL // uncomment this to send debug info out the serial port -//extern uint32_t pios_com_gps_id; -//#define PIOS_COM_GPS (pios_com_gps_id) +// extern uint32_t pios_com_gps_id; +// #define PIOS_COM_GPS (pios_com_gps_id) #if defined(PIOS_INCLUDE_USB_HID) extern uint32_t pios_com_telem_usb_id; -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) #endif // ***************************************************************** // ADC -//------------------------- +// ------------------------- // ADC // PIOS_ADC_PinGet(0) = Current // PIOS_ADC_PinGet(1) = Voltage @@ -204,82 +204,82 @@ extern uint32_t pios_com_telem_usb_id; // PIOS_ADC_PinGet(4) = Video // PIOS_ADC_PinGet(5) = RSSI // PIOS_ADC_PinGet(6) = VREF -//------------------------- +// ------------------------- #define PIOS_DMA_PIN_CONFIG \ -{ \ -{GPIOC, GPIO_Pin_0, ADC_Channel_10}, \ -{GPIOC, GPIO_Pin_1, ADC_Channel_11}, \ -{GPIOC, GPIO_Pin_2, ADC_Channel_12}, \ -{NULL, 0, ADC_Channel_TempSensor}, /* Temperature sensor */\ -{GPIOC, GPIO_Pin_3, ADC_Channel_13}, \ -{GPIOA, GPIO_Pin_7, ADC_Channel_7}, \ -{NULL, 0, ADC_Channel_Vrefint} /* Voltage reference */\ -} + { \ + { GPIOC, GPIO_Pin_0, ADC_Channel_10 }, \ + { GPIOC, GPIO_Pin_1, ADC_Channel_11 }, \ + { GPIOC, GPIO_Pin_2, ADC_Channel_12 }, \ + { NULL, 0, ADC_Channel_TempSensor }, /* Temperature sensor */ \ + { GPIOC, GPIO_Pin_3, ADC_Channel_13 }, \ + { GPIOA, GPIO_Pin_7, ADC_Channel_7 }, \ + { NULL, 0, ADC_Channel_Vrefint } /* Voltage reference */ \ + } /* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */ /* which is annoying because this then determines the rate at which we generate buffer turnover events */ /* the objective here is to get enough buffer space to support 100Hz averaging rate */ -#define PIOS_ADC_NUM_CHANNELS 7 +#define PIOS_ADC_NUM_CHANNELS 7 #define PIOS_ADC_MAX_OVERSAMPLING 10 -#define PIOS_ADC_USE_ADC2 0 -#define PIOS_ADC_USE_TEMP_SENSOR 1 +#define PIOS_ADC_USE_ADC2 0 +#define PIOS_ADC_USE_TEMP_SENSOR 1 // ***************************************************************** // USB #if defined(PIOS_INCLUDE_USB_HID) -#define PIOS_USB_ENABLED 1 -#define PIOS_USB_DETECT_GPIO_PORT GPIO_IN_2_PORT -#define PIOS_USB_DETECT_GPIO_PIN GPIO_IN_2_PIN -#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4 -#define PIOS_IRQ_USB_PRIORITY 8 -#define PIOS_USB_RX_BUFFER_SIZE 512 -#define PIOS_USB_TX_BUFFER_SIZE 512 +#define PIOS_USB_ENABLED 1 +#define PIOS_USB_DETECT_GPIO_PORT GPIO_IN_2_PORT +#define PIOS_USB_DETECT_GPIO_PIN GPIO_IN_2_PIN +#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4 +#define PIOS_IRQ_USB_PRIORITY 8 +#define PIOS_USB_RX_BUFFER_SIZE 512 +#define PIOS_USB_TX_BUFFER_SIZE 512 #endif // ***************************************************************** -//-------------------------- +// -------------------------- // Timer controller settings -//-------------------------- -#define PIOS_TIM_MAX_DEVS 6 +// -------------------------- +#define PIOS_TIM_MAX_DEVS 6 -//------------------------- +// ------------------------- // USB -//------------------------- -#define PIOS_USB_MAX_DEVS 1 -#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ -#define PIOS_USB_HID_MAX_DEVS 1 +// ------------------------- +#define PIOS_USB_MAX_DEVS 1 +#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ +#define PIOS_USB_HID_MAX_DEVS 1 -//------------------------ +// ------------------------ // PIOS_I2C // See also pios_board.c -//------------------------ -#define PIOS_I2C_MAX_DEVS 1 +// ------------------------ +#define PIOS_I2C_MAX_DEVS 1 extern uint32_t pios_i2c_flexiport_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexiport_adapter_id) -#define PIOS_I2C_ESC_ADAPTER (pios_i2c_flexiport_adapter_id) -#define PIOS_I2C_BMP085_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_ESC_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_BMP085_ADAPTER (pios_i2c_flexiport_adapter_id) -//------------------------ +// ------------------------ // PIOS_BMP085 -//------------------------ -#define PIOS_BMP085_OVERSAMPLING 3 +// ------------------------ +#define PIOS_BMP085_OVERSAMPLING 3 /** * glue macros for file IO * STM32 uses DOSFS for file IO */ -#define PIOS_FOPEN_READ(filename,file) DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, DFS_READ, PIOS_SDCARD_Sector, &file) != DFS_OK +#define PIOS_FOPEN_READ(filename, file) DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, DFS_READ, PIOS_SDCARD_Sector, &file) != DFS_OK -#define PIOS_FOPEN_WRITE(filename,file) DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, DFS_WRITE, PIOS_SDCARD_Sector, &file) != DFS_OK +#define PIOS_FOPEN_WRITE(filename, file) DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, DFS_WRITE, PIOS_SDCARD_Sector, &file) != DFS_OK -#define PIOS_FREAD(file,bufferadr,length,resultadr) DFS_ReadFile(file, PIOS_SDCARD_Sector, (uint8_t*)bufferadr, resultadr, length) != DFS_OK +#define PIOS_FREAD(file, bufferadr, length, resultadr) DFS_ReadFile(file, PIOS_SDCARD_Sector, (uint8_t *)bufferadr, resultadr, length) != DFS_OK -#define PIOS_FWRITE(file,bufferadr,length,resultadr) DFS_WriteFile(file, PIOS_SDCARD_Sector, (uint8_t*)bufferadr, resultadr, length) +#define PIOS_FWRITE(file, bufferadr, length, resultadr) DFS_WriteFile(file, PIOS_SDCARD_Sector, (uint8_t *)bufferadr, resultadr, length) -#define PIOS_FCLOSE(file) DFS_Close(&file) +#define PIOS_FCLOSE(file) DFS_Close(&file) -#define PIOS_FUNLINK(filename) DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, PIOS_SDCARD_Sector) +#define PIOS_FUNLINK(filename) DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, PIOS_SDCARD_Sector) #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/osd/pios_usb_board_data.c b/flight/targets/boards/osd/pios_usb_board_data.c index 6e5e087cf..8d0ad46cf 100644 --- a/flight/targets/boards/osd/pios_usb_board_data.c +++ b/flight/targets/boards/osd/pios_usb_board_data.c @@ -28,64 +28,65 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ -#include /* PIOS_SYS_SerialNumberGet */ -#include /* PIOS_USBHOOK_* */ -#include /* PIOS_USB_UTIL_AsciiToUtf8 */ +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ static const uint8_t usb_product_id[8] = { - sizeof(usb_product_id), - USB_DESC_TYPE_STRING, - 'O', 0, - 'S', 0, - 'D', 0, + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'O', 0, + 'S', 0, + 'D', 0, }; -static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = { - sizeof(usb_serial_number), - USB_DESC_TYPE_STRING, +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, }; static const struct usb_string_langid usb_lang_id = { - .bLength = sizeof(usb_lang_id), - .bDescriptorType = USB_DESC_TYPE_STRING, - .bLangID = htousbs(USB_LANGID_ENGLISH_US), + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), }; static const uint8_t usb_vendor_id[28] = { - sizeof(usb_vendor_id), - USB_DESC_TYPE_STRING, - 'o', 0, - 'p', 0, - 'e', 0, - 'n', 0, - 'p', 0, - 'i', 0, - 'l', 0, - 'o', 0, - 't', 0, - '.', 0, - 'o', 0, - 'r', 0, - 'g', 0 + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 }; int32_t PIOS_USB_BOARD_DATA_Init(void) { - /* Load device serial number into serial number string */ - uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - PIOS_SYS_SerialNumberGet((char *)sn); + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ - uint8_t * utf8 = &(usb_serial_number[2]); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1); + PIOS_SYS_SerialNumberGet((char *)sn); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); - return 0; + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; } diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index 3f2778838..511a3c3d2 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -9,19 +9,19 @@ * @{ * @brief Defines board specific static initializers for hardware for the Revolution board. *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -29,317 +29,321 @@ #include static const struct pios_led pios_leds[] = { - [PIOS_LED_HEARTBEAT] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_ALARM] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_ALARM] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, #ifdef PIOS_RFM22B_DEBUG_ON_TELEM - [PIOS_LED_D1] = { - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_D2] = { - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_D3] = { - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_D4] = { - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, -#endif + [PIOS_LED_D1] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D2] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D3] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D4] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, +#endif /* ifdef PIOS_RFM22B_DEBUG_ON_TELEM */ }; static const struct pios_led_cfg pios_led_cfg = { - .leds = pios_leds, - .num_leds = NELEMENTS(pios_leds), + .leds = pios_leds, + .num_leds = NELEMENTS(pios_leds), }; static const struct pios_led pios_leds_v2[] = { - [PIOS_LED_HEARTBEAT] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_ALARM] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_ALARM] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, #ifdef PIOS_RFM22B_DEBUG_ON_TELEM - [PIOS_LED_D1] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_D2] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_D3] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_D4] = { - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, -#endif + [PIOS_LED_D1] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D2] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D3] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D4] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, +#endif /* ifdef PIOS_RFM22B_DEBUG_ON_TELEM */ }; static const struct pios_led_cfg pios_led_v2_cfg = { - .leds = pios_leds_v2, - .num_leds = NELEMENTS(pios_leds_v2), + .leds = pios_leds_v2, + .num_leds = NELEMENTS(pios_leds_v2), }; -const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (uint32_t board_revision) +const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(uint32_t board_revision) { - switch(board_revision) { - case 2: - return &pios_led_cfg; - break; - case 3: - return &pios_led_v2_cfg; - break; - default: - PIOS_DEBUG_Assert(0); - } - return NULL; + switch (board_revision) { + case 2: + return &pios_led_cfg; + + break; + case 3: + return &pios_led_v2_cfg; + + break; + default: + PIOS_DEBUG_Assert(0); + } + return NULL; } -#endif /* PIOS_INCLUDE_LED */ +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_SPI) #include #if defined(PIOS_OVERO_SPI) -/* SPI2 Interface +/* SPI2 Interface * - Used for Flexi/IO/Overo communications - 3: PB12 = SPI2 NSS, CAN2 RX - 4: PB13 = SPI2 SCK, CAN2 TX, USART3 CTS - 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS - 6: PB15 = SPI2 MOSI, TIM12 CH2 + 3: PB12 = SPI2 NSS, CAN2 RX + 4: PB13 = SPI2 SCK, CAN2 TX, USART3 CTS + 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS + 6: PB15 = SPI2 MOSI, TIM12 CH2 */ #include void PIOS_OVERO_irq_handler(void); void DMA1_Stream7_IRQHandler(void) __attribute__((alias("PIOS_OVERO_irq_handler"))); static const struct pios_overo_cfg pios_overo_cfg = { - .regs = SPI2, - .remap = GPIO_AF_SPI2, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Hard, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, - }, - .use_crc = false, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case TX) - .flags = (DMA_IT_TCIF7), - .init = { - .NVIC_IRQChannel = DMA1_Stream7_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .regs = SPI2, + .remap = GPIO_AF_SPI2, + .init = { + .SPI_Mode = SPI_Mode_Slave, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Hard, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + }, + .use_crc = false, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case TX) + .flags = (DMA_IT_TCIF7), + .init = { + .NVIC_IRQChannel = DMA1_Stream7_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, - .rx = { - .channel = DMA1_Stream0, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_Medium, - //TODO: Enable FIFO - .DMA_FIFOMode = DMA_FIFOMode_Disable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_Single, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA1_Stream7, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI2->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_Single, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .slave_count = 1, - .ssel = { { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - } }, + .rx = { + .channel = DMA1_Stream0, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_Medium, + // TODO: Enable FIFO + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + .tx = { + .channel = DMA1_Stream7, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, }; uint32_t pios_overo_id = 0; void PIOS_OVERO_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_OVERO_DMA_irq_handler(pios_overo_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_OVERO_DMA_irq_handler(pios_overo_id); } #endif /* PIOS_OVERO_SPI */ -/* +/* * SPI1 Interface * Used for MPU6000 gyro and accelerometer */ @@ -347,116 +351,118 @@ void PIOS_SPI_gyro_irq_handler(void); void DMA2_Stream0_IRQHandler(void) __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); void DMA2_Stream3_IRQHandler(void) __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); static const struct pios_spi_cfg pios_spi_gyro_cfg = { - .regs = SPI1, - .remap = GPIO_AF_SPI1, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, - }, - .use_crc = false, - .dma = { - .irq = { - .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), - .init = { - .NVIC_IRQChannel = DMA2_Stream0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA2_Stream0, - .init = { + .regs = SPI1, + .remap = GPIO_AF_SPI1, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, + }, + .use_crc = false, + .dma = { + .irq = { + .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), + .init = { + .NVIC_IRQChannel = DMA2_Stream0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA2_Stream0, + .init = { .DMA_Channel = DMA_Channel_3, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, /* .DMA_FIFOThreshold */ .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA2_Stream3, - .init = { + }, + }, + .tx = { + .channel = DMA2_Stream3, + .init = { .DMA_Channel = DMA_Channel_3, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_FIFOMode = DMA_FIFOMode_Disable, /* .DMA_FIFOThreshold */ .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .miso = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .mosi = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .slave_count = 1, - .ssel = { { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - } } - } + }, + }, + }, + .sclk = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .miso = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .mosi = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + } + } + } }; static uint32_t pios_spi_gyro_id; void PIOS_SPI_gyro_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); } @@ -468,128 +474,130 @@ void PIOS_SPI_telem_flash_irq_handler(void); void DMA1_Stream0_IRQHandler(void) __attribute__((alias("PIOS_SPI_telem_flash_irq_handler"))); void DMA1_Stream5_IRQHandler(void) __attribute__((alias("PIOS_SPI_telem_flash_irq_handler"))); static const struct pios_spi_cfg pios_spi_telem_flash_cfg = { - .regs = SPI3, - .remap = GPIO_AF_SPI3, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_Low, - .SPI_CPHA = SPI_CPHA_1Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, - }, - .use_crc = false, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case RX) - .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), - .init = { - .NVIC_IRQChannel = DMA1_Stream0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Stream0, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - //TODO: Enable FIFO - .DMA_FIFOMode = DMA_FIFOMode_Disable, + .regs = SPI3, + .remap = GPIO_AF_SPI3, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_Low, + .SPI_CPHA = SPI_CPHA_1Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, + }, + .use_crc = false, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case RX) + .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), + .init = { + .NVIC_IRQChannel = DMA1_Stream0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Stream0, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + // TODO: Enable FIFO + .DMA_FIFOMode = DMA_FIFOMode_Disable, .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA1_Stream5, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, + }, + }, + .tx = { + .channel = DMA1_Stream5, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .miso = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .mosi = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .slave_count = 2, - .ssel = { - { // RFM22b - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - } }, - { // Flash - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - } }, - }, + }, + }, + }, + .sclk = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .miso = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .mosi = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .slave_count = 2, + .ssel = { + { // RFM22b + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + } + }, + { // Flash + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + } + }, + }, }; uint32_t pios_spi_telem_flash_id; void PIOS_SPI_telem_flash_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_telem_flash_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_telem_flash_id); } @@ -597,65 +605,67 @@ void PIOS_SPI_telem_flash_irq_handler(void) #include static const struct pios_exti_cfg pios_exti_rfm22b_cfg __exti_config = { - .vector = PIOS_RFM22_EXT_Int, - .line = EXTI_Line2, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line2, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Falling, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_RFM22_EXT_Int, + .line = EXTI_Line2, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line2, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Falling, + .EXTI_LineCmd = ENABLE, + }, + }, }; const struct pios_rfm22b_cfg pios_rfm22b_rm1_cfg = { - .spi_cfg = &pios_spi_telem_flash_cfg, - .exti_cfg = &pios_exti_rfm22b_cfg, - .RFXtalCap = 0x7f, - .slave_num = 0, - .gpio_direction = GPIO0_RX_GPIO1_TX, + .spi_cfg = &pios_spi_telem_flash_cfg, + .exti_cfg = &pios_exti_rfm22b_cfg, + .RFXtalCap = 0x7f, + .slave_num = 0, + .gpio_direction = GPIO0_RX_GPIO1_TX, }; const struct pios_rfm22b_cfg pios_rfm22b_rm2_cfg = { - .spi_cfg = &pios_spi_telem_flash_cfg, - .exti_cfg = &pios_exti_rfm22b_cfg, - .RFXtalCap = 0x7f, - .slave_num = 0, - .gpio_direction = GPIO0_TX_GPIO1_RX, + .spi_cfg = &pios_spi_telem_flash_cfg, + .exti_cfg = &pios_exti_rfm22b_cfg, + .RFXtalCap = 0x7f, + .slave_num = 0, + .gpio_direction = GPIO0_TX_GPIO1_RX, }; -const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (uint32_t board_revision) +const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22Cfg(uint32_t board_revision) { - switch(board_revision) { - case 2: - return &pios_rfm22b_rm1_cfg; - break; - case 3: - return &pios_rfm22b_rm2_cfg; - break; - default: - PIOS_DEBUG_Assert(0); - } - return NULL; + switch (board_revision) { + case 2: + return &pios_rfm22b_rm1_cfg; + + break; + case 3: + return &pios_rfm22b_rm2_cfg; + + break; + default: + PIOS_DEBUG_Assert(0); + } + return NULL; } #endif /* PIOS_INCLUDE_RFM22B */ @@ -667,17 +677,17 @@ const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (uint32_t board_re #include "pios_flash_jedec_priv.h" static const struct flashfs_logfs_cfg flashfs_m25p_cfg = { - .fs_magic = 0x99abceef, - .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ - .arena_size = 0x00010000, /* 256 * slot size */ - .slot_size = 0x00000100, /* 256 bytes */ + .fs_magic = 0x99abceef, + .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ + .arena_size = 0x00010000, /* 256 * slot size */ + .slot_size = 0x00000100, /* 256 bytes */ - .start_offset = 0, /* start at the beginning of the chip */ - .sector_size = 0x00010000, /* 64K bytes */ - .page_size = 0x00000100, /* 256 bytes */ + .start_offset = 0, /* start at the beginning of the chip */ + .sector_size = 0x00010000, /* 64K bytes */ + .page_size = 0x00000100, /* 256 bytes */ }; -#endif /* PIOS_INCLUDE_FLASH */ +#endif /* PIOS_INCLUDE_FLASH */ #include @@ -687,44 +697,44 @@ static const struct flashfs_logfs_cfg flashfs_m25p_cfg = { * MAIN USART */ static const struct pios_usart_cfg pios_usart_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART1, + .remap = GPIO_AF_USART1, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_INCLUDE_COM_TELEM */ @@ -732,131 +742,131 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { #include "pios_dsm_priv.h" static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART1, + .remap = GPIO_AF_USART1, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; // Because of the inverter on the main port this will not // work. Notice the mode is set to IN to maintain API // compatibility but protect the pins static const struct pios_dsm_cfg pios_dsm_main_cfg = { - .bind = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, + .bind = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, }; -#endif /* PIOS_INCLUDE_DSM */ +#endif /* PIOS_INCLUDE_DSM */ #include #if defined(PIOS_INCLUDE_SBUS) - /* - * S.Bus USART - */ +/* + * S.Bus USART + */ #include static const struct pios_usart_cfg pios_usart_sbus_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 100000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_Even, - .USART_StopBits = USART_StopBits_2, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, + .regs = USART1, + .remap = GPIO_AF_USART1, + .init = { + .USART_BaudRate = 100000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_Even, + .USART_StopBits = USART_StopBits_2, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, }; -#endif /* PIOS_INCLUDE_SBUS */ +#endif /* PIOS_INCLUDE_SBUS */ // Need this defined regardless to be able to turn it off static const struct pios_sbus_cfg pios_sbus_cfg = { - /* Inverter configuration */ - .inv = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .gpio_inv_enable = Bit_SET, - .gpio_inv_disable = Bit_RESET, - .gpio_clk_func = RCC_AHB1PeriphClockCmd, - .gpio_clk_periph = RCC_AHB1Periph_GPIOC, + /* Inverter configuration */ + .inv = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .gpio_inv_enable = Bit_SET, + .gpio_inv_disable = Bit_RESET, + .gpio_clk_func = RCC_AHB1PeriphClockCmd, + .gpio_clk_periph = RCC_AHB1Periph_GPIOC, }; @@ -865,45 +875,45 @@ static const struct pios_sbus_cfg pios_sbus_cfg = { * FLEXI PORT */ static const struct pios_usart_cfg pios_usart_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART3, + .remap = GPIO_AF_USART3, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_INCLUDE_COM_FLEXI */ @@ -912,144 +922,144 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { #include "pios_dsm_priv.h" static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART3, + .remap = GPIO_AF_USART3, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, + .bind = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, }; -#endif /* PIOS_INCLUDE_DSM */ +#endif /* PIOS_INCLUDE_DSM */ /* * HK OSD */ static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART1, + .remap = GPIO_AF_USART1, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART3, + .remap = GPIO_AF_USART3, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #if defined(PIOS_INCLUDE_COM) @@ -1068,144 +1078,144 @@ static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { void PIOS_I2C_mag_pressure_adapter_ev_irq_handler(void); void PIOS_I2C_mag_pressureadapter_er_irq_handler(void); void I2C1_EV_IRQHandler() - __attribute__ ((alias("PIOS_I2C_mag_pressure_adapter_ev_irq_handler"))); +__attribute__((alias("PIOS_I2C_mag_pressure_adapter_ev_irq_handler"))); void I2C1_ER_IRQHandler() - __attribute__ ((alias("PIOS_I2C_mag_pressure_adapter_er_irq_handler"))); +__attribute__((alias("PIOS_I2C_mag_pressure_adapter_er_irq_handler"))); static const struct pios_i2c_adapter_cfg pios_i2c_mag_pressure_adapter_cfg = { - .regs = I2C1, - .remap = GPIO_AF_I2C1, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, + .regs = I2C1, + .remap = GPIO_AF_I2C1, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_8, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C1_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C1_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; uint32_t pios_i2c_mag_pressure_adapter_id; void PIOS_I2C_mag_pressure_adapter_ev_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_mag_pressure_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_mag_pressure_adapter_id); } void PIOS_I2C_mag_pressure_adapter_er_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_mag_pressure_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_mag_pressure_adapter_id); } void PIOS_I2C_flexiport_adapter_ev_irq_handler(void); void PIOS_I2C_flexiport_adapter_er_irq_handler(void); -void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexiport_adapter_ev_irq_handler"))); -void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexiport_adapter_er_irq_handler"))); +void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_ev_irq_handler"))); +void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_er_irq_handler"))); static const struct pios_i2c_adapter_cfg pios_i2c_flexiport_adapter_cfg = { - .regs = I2C2, - .remap = GPIO_AF_I2C2, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, + .regs = I2C2, + .remap = GPIO_AF_I2C2, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; uint32_t pios_i2c_flexiport_adapter_id; void PIOS_I2C_flexiport_adapter_ev_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexiport_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexiport_adapter_id); } void PIOS_I2C_flexiport_adapter_er_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexiport_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexiport_adapter_id); } @@ -1220,182 +1230,182 @@ void PIOS_I2C_pressure_adapter_er_irq_handler(void); */ #include -void PIOS_RTC_IRQ_Handler (void); -void RTC_WKUP_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler"))); +void PIOS_RTC_IRQ_Handler(void); +void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); static const struct pios_rtc_cfg pios_rtc_main_cfg = { - .clksrc = RCC_RTCCLKSource_HSE_Div8, // Divide 8 Mhz crystal down to 1 - // For some reason it's acting like crystal is 16 Mhz. This clock is then divided - // by another 16 to give a nominal 62.5 khz clock - .prescaler = 100, // Every 100 cycles gives 625 Hz - .irq = { - .init = { - .NVIC_IRQChannel = RTC_WKUP_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .clksrc = RCC_RTCCLKSource_HSE_Div8, // Divide 8 Mhz crystal down to 1 + // For some reason it's acting like crystal is 16 Mhz. This clock is then divided + // by another 16 to give a nominal 62.5 khz clock + .prescaler = 100, // Every 100 cycles gives 625 Hz + .irq = { + .init = { + .NVIC_IRQChannel = RTC_WKUP_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; -void PIOS_RTC_IRQ_Handler (void) +void PIOS_RTC_IRQ_Handler(void) { - PIOS_RTC_irq_handler (); + PIOS_RTC_irq_handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_RTC) */ #include "pios_tim_priv.h" static const TIM_TimeBaseInitTypeDef tim_3_5_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, }; static const TIM_TimeBaseInitTypeDef tim_9_10_11_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, }; static const struct pios_tim_clock_cfg tim_3_cfg = { - .timer = TIM3, - .time_base_init = &tim_3_5_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM3, + .time_base_init = &tim_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_5_cfg = { - .timer = TIM5, - .time_base_init = &tim_3_5_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM5, + .time_base_init = &tim_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_9_cfg = { - .timer = TIM9, - .time_base_init = &tim_9_10_11_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_BRK_TIM9_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM9, + .time_base_init = &tim_9_10_11_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_BRK_TIM9_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_10_cfg = { - .timer = TIM10, - .time_base_init = &tim_9_10_11_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_UP_TIM10_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM10, + .time_base_init = &tim_9_10_11_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_UP_TIM10_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_11_cfg = { - .timer = TIM11, - .time_base_init = &tim_9_10_11_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_TRG_COM_TIM11_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM11, + .time_base_init = &tim_9_10_11_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_TRG_COM_TIM11_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; // Set up timers that only have inputs on APB1 // TIM2,3,4,5,6,7,12,13,14 static const TIM_TimeBaseInitTypeDef tim_apb1_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = 0xFFFF, + .TIM_RepetitionCounter = 0x0000, }; // Set up timers that only have inputs on APB2 // TIM1,8,9,10,11 static const TIM_TimeBaseInitTypeDef tim_apb2_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = 0xFFFF, + .TIM_RepetitionCounter = 0x0000, }; static const struct pios_tim_clock_cfg tim_1_cfg = { - .timer = TIM1, - .time_base_init = &tim_apb2_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM1, + .time_base_init = &tim_apb2_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_4_cfg = { - .timer = TIM4, - .time_base_init = &tim_apb1_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM4, + .time_base_init = &tim_apb1_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_8_cfg = { - .timer = TIM8, - .time_base_init = &tim_apb2_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM8_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM8, + .time_base_init = &tim_apb2_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_12_cfg = { - .timer = TIM12, - .time_base_init = &tim_apb1_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM8_BRK_TIM12_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM12, + .time_base_init = &tim_apb1_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_BRK_TIM12_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; @@ -1405,258 +1415,258 @@ static const struct pios_tim_clock_cfg tim_12_cfg = { */ #include static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource0, - }, - .remap = GPIO_AF_TIM3, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource1, - }, - .remap = GPIO_AF_TIM3, - }, - { - .timer = TIM9, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource3, - }, - .remap = GPIO_AF_TIM9, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource2, - }, - .remap = GPIO_AF_TIM2, - }, - { - .timer = TIM5, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource1, - }, - .remap = GPIO_AF_TIM5, - }, - { - .timer = TIM5, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource0, - }, - .remap = GPIO_AF_TIM5, - }, - // PWM pins on FlexiIO(receiver) port - { - // * 6: PB15 = SPI2 MOSI, TIM12 CH2 + { + .timer = TIM3, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource0, + }, + .remap = GPIO_AF_TIM3, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource1, + }, + .remap = GPIO_AF_TIM3, + }, + { + .timer = TIM9, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource3, + }, + .remap = GPIO_AF_TIM9, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource2, + }, + .remap = GPIO_AF_TIM2, + }, + { + .timer = TIM5, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource1, + }, + .remap = GPIO_AF_TIM5, + }, + { + .timer = TIM5, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource0, + }, + .remap = GPIO_AF_TIM5, + }, + // PWM pins on FlexiIO(receiver) port + { + // * 6: PB15 = SPI2 MOSI, TIM12 CH2 - .timer = TIM12, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource15, - }, - .remap = GPIO_AF_TIM12, - }, - { - // * 7: PC6 = TIM8 CH1, USART6 TX - .timer = TIM8, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource6, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 8: PC7 = TIM8 CH2, USART6 RX - .timer = TIM8, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource7, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 9: PC8 = TIM8 CH3 - .timer = TIM8, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource8, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 10: PC9 = TIM8 CH4 - .timer = TIM8, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource9, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS - .timer = TIM12, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource14, - }, - .remap = GPIO_AF_TIM12, - }, + .timer = TIM12, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource15, + }, + .remap = GPIO_AF_TIM12, + }, + { + // * 7: PC6 = TIM8 CH1, USART6 TX + .timer = TIM8, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource6, + }, + .remap = GPIO_AF_TIM8, + }, + + { + // * 8: PC7 = TIM8 CH2, USART6 RX + .timer = TIM8, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource7, + }, + .remap = GPIO_AF_TIM8, + }, + + { + // * 9: PC8 = TIM8 CH3 + .timer = TIM8, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource8, + }, + .remap = GPIO_AF_TIM8, + }, + + { + // * 10: PC9 = TIM8 CH4 + .timer = TIM8, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource9, + }, + .remap = GPIO_AF_TIM8, + }, + + { + // * 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS + .timer = TIM12, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource14, + }, + .remap = GPIO_AF_TIM12, + }, }; -#define PIOS_SERVOPORT_ALL_PINS_PWMOUT 6 +#define PIOS_SERVOPORT_ALL_PINS_PWMOUT 6 #define PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN_PPM 11 -#define PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN 12 +#define PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN 12 const struct pios_servo_cfg pios_servo_cfg_out = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_all_pins, - .num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT, + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_all_pins, + .num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT, }; // All servo outputs, servo input ch1 ppm, ch2-6 outputs const struct pios_servo_cfg pios_servo_cfg_out_in_ppm = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_all_pins, - .num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN_PPM, + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_all_pins, + .num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN_PPM, }; // All servo outputs, servo inputs ch1-6 Outputs const struct pios_servo_cfg pios_servo_cfg_out_in = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_all_pins, - .num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN, + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_all_pins, + .num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN, }; @@ -1667,127 +1677,127 @@ const struct pios_servo_cfg pios_servo_cfg_out_in = { #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) #include static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { - { - .timer = TIM12, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource14, - }, - .remap = GPIO_AF_TIM12, - }, - { - .timer = TIM12, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource15, - }, - .remap = GPIO_AF_TIM12, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource6, - }, - .remap = GPIO_AF_TIM8, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource7, - }, - .remap = GPIO_AF_TIM8, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource8, - }, - .remap = GPIO_AF_TIM8, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource9, - }, - .remap = GPIO_AF_TIM8, - }, + { + .timer = TIM12, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource14, + }, + .remap = GPIO_AF_TIM12, + }, + { + .timer = TIM12, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource15, + }, + .remap = GPIO_AF_TIM12, + }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource6, + }, + .remap = GPIO_AF_TIM8, + }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource7, + }, + .remap = GPIO_AF_TIM8, + }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource8, + }, + .remap = GPIO_AF_TIM8, + }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource9, + }, + .remap = GPIO_AF_TIM8, + }, }; const struct pios_pwm_cfg pios_pwm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = pios_tim_rcvrport_all_channels, - .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_rcvrport_all_channels, + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), }; -// this configures outputs 2-6 as pwm inputs +// this configures outputs 2-6 as pwm inputs const struct pios_pwm_cfg pios_pwm_ppm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = &pios_tim_rcvrport_all_channels[1], - .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_rcvrport_all_channels[1], + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1, }; -#endif +#endif /* if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) */ /* * PPM Input @@ -1795,23 +1805,23 @@ const struct pios_pwm_cfg pios_pwm_ppm_cfg = { #if defined(PIOS_INCLUDE_PPM) #include static const struct pios_ppm_cfg pios_ppm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - .TIM_Channel = TIM_Channel_1, - }, - /* Use only the first channel for ppm */ - .channels = &pios_tim_rcvrport_all_channels[0], - .num_channels = 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + .TIM_Channel = TIM_Channel_1, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_rcvrport_all_channels[0], + .num_channels = 1, }; -#endif //PPM +#endif // PPM #if defined(PIOS_INCLUDE_GCSRCVR) #include "pios_gcsrcvr_priv.h" -#endif /* PIOS_INCLUDE_GCSRCVR */ +#endif /* PIOS_INCLUDE_GCSRCVR */ #if defined(PIOS_INCLUDE_RCVR) #include "pios_rcvr_priv.h" @@ -1821,60 +1831,62 @@ static const struct pios_ppm_cfg pios_ppm_cfg = { #include "pios_usb_priv.h" static const struct pios_usb_cfg pios_usb_main_rm1_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = OTG_FS_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 3, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_25MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - }, + .irq = { + .init = { + .NVIC_IRQChannel = OTG_FS_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 3, + .NVIC_IRQChannelCmd = ENABLE, }, - .vsense_active_low = false + }, + .vsense = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_25MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + }, + }, + .vsense_active_low = false }; static const struct pios_usb_cfg pios_usb_main_rm2_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = OTG_FS_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 3, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_25MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - }, + .irq = { + .init = { + .NVIC_IRQChannel = OTG_FS_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 3, + .NVIC_IRQChannelCmd = ENABLE, }, - .vsense_active_low = false + }, + .vsense = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_25MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + }, + }, + .vsense_active_low = false }; -const struct pios_usb_cfg * PIOS_BOARD_HW_DEFS_GetUsbCfg (uint32_t board_revision) +const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(uint32_t board_revision) { - switch(board_revision) { - case 2: - return &pios_usb_main_rm1_cfg; - break; - case 3: - return &pios_usb_main_rm2_cfg; - break; - default: - PIOS_DEBUG_Assert(0); - } - return NULL; + switch (board_revision) { + case 2: + return &pios_usb_main_rm1_cfg; + + break; + case 3: + return &pios_usb_main_rm2_cfg; + + break; + default: + PIOS_DEBUG_Assert(0); + } + return NULL; } #include "pios_usb_board_data_priv.h" @@ -1882,7 +1894,7 @@ const struct pios_usb_cfg * PIOS_BOARD_HW_DEFS_GetUsbCfg (uint32_t board_revisio #include "pios_usb_desc_hid_only_priv.h" #include "pios_usbhook.h" -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ #if defined(PIOS_INCLUDE_COM_MSG) @@ -1894,9 +1906,9 @@ const struct pios_usb_cfg * PIOS_BOARD_HW_DEFS_GetUsbCfg (uint32_t board_revisio #include const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, + .data_if = 0, + .data_rx_ep = 1, + .data_tx_ep = 1, }; #endif /* PIOS_INCLUDE_USB_HID */ @@ -1904,11 +1916,11 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { #include const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 1, - .ctrl_tx_ep = 2, + .ctrl_if = 1, + .ctrl_tx_ep = 2, - .data_if = 2, - .data_rx_ep = 3, - .data_tx_ep = 3, + .data_if = 2, + .data_rx_ep = 3, + .data_tx_ep = 3, }; -#endif /* PIOS_INCLUDE_USB_CDC */ +#endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/targets/boards/revolution/bootloader/inc/common.h b/flight/targets/boards/revolution/bootloader/inc/common.h index 9010fddd9..e64b89014 100644 --- a/flight/targets/boards/revolution/bootloader/inc/common.h +++ b/flight/targets/boards/revolution/bootloader/inc/common.h @@ -28,10 +28,10 @@ #ifndef COMMON_H_ #define COMMON_H_ -//#include "board.h" +// #include "board.h" typedef enum { - start, keepgoing, + start, keepgoing, } DownloadAction; /**************************************************/ @@ -39,77 +39,77 @@ typedef enum { /**************************************************/ typedef enum { - DFUidle, //0 - uploading, //1 - wrong_packet_received, //2 - too_many_packets, //3 - too_few_packets, //4 - Last_operation_Success, //5 - downloading, //6 - BLidle, //7 - Last_operation_failed, //8 - uploadingStarting, //9 - outsideDevCapabilities, //10 - CRC_Fail,//11 - failed_jump, -//12 + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + BLidle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, +// 12 } DFUStates; /**************************************************/ /* OP_DFU commands */ /**************************************************/ typedef enum { - Reserved, //0 - Req_Capabilities, //1 - Rep_Capabilities, //2 - EnterDFU, //3 - JumpFW, //4 - Reset, //5 - Abort_Operation, //6 - Upload, //7 - Op_END, //8 - Download_Req, //9 - Download, //10 - Status_Request, //11 - Status_Rep -//12 + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep +// 12 } DFUCommands; typedef enum { - High_Density, Medium_Density + High_Density, Medium_Density } DeviceType; /**************************************************/ /* OP_DFU transfer types */ /**************************************************/ typedef enum { - FW, //0 - Descript -//2 + FW, // 0 + Descript +// 2 } DFUTransfer; /**************************************************/ /* OP_DFU transfer port */ /**************************************************/ typedef enum { - Usb, //0 - Serial -//2 + Usb, // 0 + Serial +// 2 } DFUPort; /**************************************************/ /* OP_DFU programable programable HW types */ /**************************************************/ typedef enum { - Self_flash, //0 - Remote_flash_via_spi -//1 + Self_flash, // 0 + Remote_flash_via_spi +// 1 } DFUProgType; /**************************************************/ /* OP_DFU programable sources */ /**************************************************/ -#define USB 0 -#define SPI 1 +#define USB 0 +#define SPI 1 -#define DownloadDelay 100000 +#define DownloadDelay 100000 -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 +#define MAX_DEL_RETRYS 3 +#define MAX_WRI_RETRYS 3 #endif /* COMMON_H_ */ diff --git a/flight/targets/boards/revolution/bootloader/inc/pios_config.h b/flight/targets/boards/revolution/bootloader/inc/pios_config.h index 86fedc699..d3b203d2e 100644 --- a/flight/targets/boards/revolution/bootloader/inc/pios_config.h +++ b/flight/targets/boards/revolution/bootloader/inc/pios_config.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * - Central compile time config for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/revolution/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/revolution/bootloader/inc/pios_usb_board_data.h index eb00a55be..4e9c7c063 100644 --- a/flight/targets/boards/revolution/bootloader/inc/pios_usb_board_data.h +++ b/flight/targets/boards/revolution/bootloader/inc/pios_usb_board_data.h @@ -33,13 +33,13 @@ #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 2 +#define PIOS_USB_BOARD_EP_NUM 2 -#include /* struct usb_* */ +#include /* struct usb_* */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_BL) -#define PIOS_USB_BOARD_SN_SUFFIX "+BL" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" /* * The bootloader uses a simplified report structure @@ -49,4 +49,4 @@ */ #define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/revolution/bootloader/main.c b/flight/targets/boards/revolution/bootloader/main.c index b1e408c6a..033f14a6c 100644 --- a/flight/targets/boards/revolution/bootloader/main.c +++ b/flight/targets/boards/revolution/bootloader/main.c @@ -8,21 +8,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief This is the file with the main function of the Revolution BootLoader * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,7 +32,7 @@ #include #include #include -#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USBHOOK_* */ #include /* Prototype of PIOS_Board_Init() function */ @@ -61,10 +61,10 @@ uint8_t tempcount = 0; /* Extern variables ----------------------------------------------------------*/ DFUStates DeviceState; -int16_t status = 0; -bool JumpToApp = false; -bool GO_dfu = false; -bool USB_connected = false; +int16_t status = 0; +bool JumpToApp = false; +bool GO_dfu = false; +bool USB_connected = false; bool User_DFU_request = false; static uint8_t mReceive_Buffer[63]; /* Private function prototypes -----------------------------------------------*/ @@ -72,146 +72,160 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); uint8_t processRX(); void jump_to_app(); -int main() { - PIOS_SYS_Init(); - PIOS_Board_Init(); - PIOS_IAP_Init(); +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); - // Make sure the brown out reset value for this chip - // is 2.7 volts - check_bor(); + // Make sure the brown out reset value for this chip + // is 2.7 volts + check_bor(); - USB_connected = PIOS_USB_CheckAvailable(0); + USB_connected = PIOS_USB_CheckAvailable(0); - if (PIOS_IAP_CheckRequest() == true) { - PIOS_DELAY_WaitmS(1000); - User_DFU_request = true; - PIOS_IAP_ClearRequest(); - } + if (PIOS_IAP_CheckRequest() == true) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = true; + PIOS_IAP_ClearRequest(); + } - GO_dfu = (USB_connected == true) || (User_DFU_request == true); + GO_dfu = (USB_connected == true) || (User_DFU_request == true); - if (GO_dfu == true) { - if (User_DFU_request == true) - DeviceState = DFUidle; - else - DeviceState = BLidle; - } else - JumpToApp = true; + if (GO_dfu == true) { + if (User_DFU_request == true) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = true; + } - uint32_t stopwatch = 0; - uint32_t prev_ticks = PIOS_DELAY_GetuS(); - while (true) { - /* Update the stopwatch */ - uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); - prev_ticks += elapsed_ticks; - stopwatch += elapsed_ticks; + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (true) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; - if (JumpToApp == true) - jump_to_app(); + if (JumpToApp == true) { + jump_to_app(); + } - switch (DeviceState) { - case Last_operation_Success: - case uploadingStarting: - case DFUidle: - period1 = 5000; - sweep_steps1 = 100; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case uploading: - period1 = 5000; - sweep_steps1 = 100; - period2 = 2500; - sweep_steps2 = 50; - break; - case downloading: - period1 = 2500; - sweep_steps1 = 50; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case BLidle: - period1 = 0; - PIOS_LED_On(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - default://error - period1 = 5000; - sweep_steps1 = 100; - period2 = 5000; - sweep_steps2 = 100; - } + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } - if (period1 != 0) { - if (LedPWM(period1, sweep_steps1, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_On(PIOS_LED_HEARTBEAT); + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } - if (period2 != 0) { - if (LedPWM(period2, sweep_steps2, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } - if (stopwatch > 50 * 1000 * 1000) - stopwatch = 0; - if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) - JumpToApp = true; + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) { + JumpToApp = true; + } - processRX(); - DataDownload(start); - } + processRX(); + DataDownload(start); + } } -void jump_to_app() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; - PIOS_LED_On(PIOS_LED_HEARTBEAT); - // Look at cm3_vectors struct in startup. In a fw image the first uint32_t contains the address of the top of irqstack - uint32_t fwIrqStackBase = (*(__IO uint32_t*) bdinfo->fw_base) & 0xFFFE0000; - // Check for the two possible irqstack locations (sram or core coupled sram) - if ( fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { - /* Jump to user application */ - FLASH_Lock(); - RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + PIOS_LED_On(PIOS_LED_HEARTBEAT); + // Look at cm3_vectors struct in startup. In a fw image the first uint32_t contains the address of the top of irqstack + uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000; + // Check for the two possible irqstack locations (sram or core coupled sram) + if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { + /* Jump to user application */ + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - PIOS_USBHOOK_Deactivate(); + PIOS_USBHOOK_Deactivate(); - JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); - Jump_To_Application = (pFunction) JumpAddress; - /* Initialize user application's Stack Pointer */ - __set_MSP(*(__IO uint32_t*) bdinfo->fw_base); - Jump_To_Application(); - } else { - DeviceState = failed_jump; - return; - } + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + return; + } } -uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { - uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ - uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ - uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ - if (curr_sweep & 1) { - pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ - } - return ((count % pwm_period) > pwm_duty) ? 1 : 0; + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; } -uint8_t processRX() { - if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { - processComand(mReceive_Buffer); - } - return true; +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return true; } /** @@ -221,14 +235,18 @@ uint8_t processRX() { */ void check_bor() { - uint8_t bor = FLASH_OB_GetBOR(); - if(bor != OB_BOR_LEVEL3) { - FLASH_OB_Unlock(); - FLASH_OB_BORConfig(OB_BOR_LEVEL3); - FLASH_OB_Launch(); - while(FLASH_WaitForLastOperation() == FLASH_BUSY); - FLASH_OB_Lock(); - while(FLASH_WaitForLastOperation() == FLASH_BUSY); - } -} + uint8_t bor = FLASH_OB_GetBOR(); + if (bor != OB_BOR_LEVEL3) { + FLASH_OB_Unlock(); + FLASH_OB_BORConfig(OB_BOR_LEVEL3); + FLASH_OB_Launch(); + while (FLASH_WaitForLastOperation() == FLASH_BUSY) { + ; + } + FLASH_OB_Lock(); + while (FLASH_WaitForLastOperation() == FLASH_BUSY) { + ; + } + } +} diff --git a/flight/targets/boards/revolution/bootloader/pios_board.c b/flight/targets/boards/revolution/bootloader/pios_board.c index 5b2280389..2256f4785 100644 --- a/flight/targets/boards/revolution/bootloader/pios_board.c +++ b/flight/targets/boards/revolution/bootloader/pios_board.c @@ -7,19 +7,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,45 +39,46 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; -void PIOS_Board_Init() { - if (board_init_complete) { - return; - } +void PIOS_Board_Init() +{ + if (board_init_complete) { + return; + } + + /* Delay system */ + PIOS_DELAY_Init(); + + const struct pios_board_info *bdinfo = &pios_board_info_blob; - /* Delay system */ - PIOS_DELAY_Init(); - - const struct pios_board_info * bdinfo = &pios_board_info_blob; - #if defined(PIOS_INCLUDE_LED) - const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); - PIOS_Assert(led_cfg); - PIOS_LED_Init(led_cfg); -#endif /* PIOS_INCLUDE_LED */ + const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Activate the HID-only USB configuration */ - PIOS_USB_DESC_HID_ONLY_Init(); + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { - PIOS_Assert(0); - } -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ - PIOS_USBHOOK_Activate(); + PIOS_USBHOOK_Activate(); -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ - board_init_complete = true; + board_init_complete = true; } diff --git a/flight/targets/boards/revolution/firmware/cm3_fault_handlers.c b/flight/targets/boards/revolution/firmware/cm3_fault_handlers.c index e59e3b79a..e74ffa76a 100644 --- a/flight/targets/boards/revolution/firmware/cm3_fault_handlers.c +++ b/flight/targets/boards/revolution/firmware/cm3_fault_handlers.c @@ -14,34 +14,34 @@ # include #endif -#define FAULT_TRAMPOLINE(_vec) \ -__attribute__((naked, no_instrument_function)) \ -void \ -_vec##_Handler(void) \ -{ \ - __asm( ".syntax unified\n" \ - "MOVS R0, #4 \n" \ - "MOV R1, LR \n" \ - "TST R0, R1 \n" \ - "BEQ 1f \n" \ - "MRS R0, PSP \n" \ - "B " #_vec "_Handler2 \n" \ - "1: \n" \ - "MRS R0, MSP \n" \ - "B " #_vec "_Handler2 \n" \ - ".syntax divided\n"); \ -} \ -struct hack +#define FAULT_TRAMPOLINE(_vec) \ + __attribute__((naked, no_instrument_function)) \ + void \ + _vec##_Handler(void) \ + { \ + __asm(".syntax unified\n" \ + "MOVS R0, #4 \n" \ + "MOV R1, LR \n" \ + "TST R0, R1 \n" \ + "BEQ 1f \n" \ + "MRS R0, PSP \n" \ + "B " #_vec "_Handler2 \n" \ + "1: \n" \ + "MRS R0, MSP \n" \ + "B " #_vec "_Handler2 \n" \ + ".syntax divided\n"); \ + } \ + struct hack struct cm3_frame { - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t pc; - uint32_t psr; + uint32_t r0; + uint32_t r1; + uint32_t r2; + uint32_t r3; + uint32_t r12; + uint32_t lr; + uint32_t pc; + uint32_t psr; }; FAULT_TRAMPOLINE(HardFault); @@ -49,39 +49,42 @@ FAULT_TRAMPOLINE(BusFault); FAULT_TRAMPOLINE(UsageFault); /* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */ -#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) +#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) -void -HardFault_Handler2(struct cm3_frame *frame) +void HardFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nHARD FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(HFSR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nHARD FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(HFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } } -void -BusFault_Handler2(struct cm3_frame *frame) +void BusFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nBUS FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(BFAR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nBUS FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(BFAR)); + dbg_write_char('\n'); + for (;;) { + ; + } } -void -UsageFault_Handler2(struct cm3_frame *frame) +void UsageFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nUSAGE FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nUSAGE FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } } diff --git a/flight/targets/boards/revolution/firmware/dcc_stdio.c b/flight/targets/boards/revolution/firmware/dcc_stdio.c index 102dd70a1..1a522e9a0 100644 --- a/flight/targets/boards/revolution/firmware/dcc_stdio.c +++ b/flight/targets/boards/revolution/firmware/dcc_stdio.c @@ -1,33 +1,33 @@ /*************************************************************************** - * Copyright (C) 2008 by Dominic Rath * - * Dominic.Rath@gmx.de * - * Copyright (C) 2008 by Spencer Oliver * - * spen@spen-soft.co.uk * - * Copyright (C) 2008 by Frederik Kriewtz * - * frederik@kriewitz.eu * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* Copyright (C) 2008 by Frederik Kriewtz * +* frederik@kriewitz.eu * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ #include "inc/dcc_stdio.h" -#define TARGET_REQ_TRACEMSG 0x00 -#define TARGET_REQ_DEBUGMSG_ASCII 0x01 -#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) -#define TARGET_REQ_DEBUGCHAR 0x02 +#define TARGET_REQ_TRACEMSG 0x00 +#define TARGET_REQ_DEBUGMSG_ASCII 0x01 +#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) +#define TARGET_REQ_DEBUGCHAR 0x02 /* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel * DCRDR[7:0] is used by target for status @@ -35,115 +35,115 @@ * DCRDR[23:16] is used for by host for status * DCRDR[31:24] is used for by host for write buffer */ -#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) +#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) -#define BUSY 1 +#define BUSY 1 void dbg_write(unsigned long dcc_data) { - int len = 4; + int len = 4; - while (len--) - { - /* wait for data ready */ - while (NVIC_DBG_DATA_R & BUSY); + while (len--) { + /* wait for data ready */ + while (NVIC_DBG_DATA_R & BUSY) { + ; + } - /* write our data and set write flag - tell host there is data*/ - NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); - dcc_data >>= 8; - } + /* write our data and set write flag - tell host there is data*/ + NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); + dcc_data >>= 8; + } } void dbg_trace_point(unsigned long number) { - dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); + dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); } void dbg_write_u32(const unsigned long *val, long len) { - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); - while (len > 0) - { - dbg_write(*val); + while (len > 0) { + dbg_write(*val); - val++; - len--; - } + val++; + len--; + } } void dbg_write_u16(const unsigned short *val, long len) { - unsigned long dcc_data; + unsigned long dcc_data; - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = val[0] - | ((len > 1) ? val[1] << 16: 0x0000); + while (len > 0) { + dcc_data = val[0] + | ((len > 1) ? val[1] << 16 : 0x0000); - dbg_write(dcc_data); + dbg_write(dcc_data); - val += 2; - len -= 2; - } + val += 2; + len -= 2; + } } void dbg_write_u8(const unsigned char *val, long len) { - unsigned long dcc_data; + unsigned long dcc_data; - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = val[0] - | ((len > 1) ? val[1] << 8 : 0x00) - | ((len > 2) ? val[2] << 16 : 0x00) - | ((len > 3) ? val[3] << 24 : 0x00); + while (len > 0) { + dcc_data = val[0] + | ((len > 1) ? val[1] << 8 : 0x00) + | ((len > 2) ? val[2] << 16 : 0x00) + | ((len > 3) ? val[3] << 24 : 0x00); - dbg_write(dcc_data); + dbg_write(dcc_data); - val += 4; - len -= 4; - } + val += 4; + len -= 4; + } } void dbg_write_str(const char *msg) { - long len; - unsigned long dcc_data; + long len; + unsigned long dcc_data; - for (len = 0; msg[len] && (len < 65536); len++); + for (len = 0; msg[len] && (len < 65536); len++) { + ; + } - dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = msg[0] - | ((len > 1) ? msg[1] << 8 : 0x00) - | ((len > 2) ? msg[2] << 16 : 0x00) - | ((len > 3) ? msg[3] << 24 : 0x00); - dbg_write(dcc_data); + while (len > 0) { + dcc_data = msg[0] + | ((len > 1) ? msg[1] << 8 : 0x00) + | ((len > 2) ? msg[2] << 16 : 0x00) + | ((len > 3) ? msg[3] << 24 : 0x00); + dbg_write(dcc_data); - msg += 4; - len -= 4; - } + msg += 4; + len -= 4; + } } void dbg_write_char(char msg) { - dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); + dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); } void dbg_write_hex32(const unsigned long val) { - static const char hextab[] = { - '0', '1', '2', '3', '4', '5', '6', '7', - '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' - }; + static const char hextab[] = { + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' + }; - for (int shift = 28; shift >= 0; shift -= 4) - dbg_write_char(hextab[(val >> shift) & 0xf]); + for (int shift = 28; shift >= 0; shift -= 4) { + dbg_write_char(hextab[(val >> shift) & 0xf]); + } } diff --git a/flight/targets/boards/revolution/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/revolution/firmware/inc/FreeRTOSConfig.h index 023d20476..62eed5553 100644 --- a/flight/targets/boards/revolution/firmware/inc/FreeRTOSConfig.h +++ b/flight/targets/boards/revolution/firmware/inc/FreeRTOSConfig.h @@ -1,85 +1,84 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ /** - * @addtogroup PIOS PIOS - * @{ - * @addtogroup FreeRTOS FreeRTOS - * @{ - */ + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ /* Notes: We use 5 task priorities */ -#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... -#define configTICK_RATE_HZ ((portTickType )1000) -#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) -#define configMINIMAL_STACK_SIZE ((unsigned short)512) -#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total -#define configMAX_TASK_NAME_LEN (16) +#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total +#define configMAX_TASK_NAME_LEN (16) -#define configUSE_PREEMPTION 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configIDLE_SHOULD_YIELD 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 -#define configUSE_TIMERS 1 -#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ -#define configTIMER_QUEUE_LENGTH 10 -#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE +#define configUSE_TIMERS 1 +#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ + to exclude the API function. */ -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 1 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 1 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 /* Enable run time stats collection */ -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 /* * Once we move to CMSIS2 we can at least use: @@ -88,16 +87,17 @@ NVIC value of 255. */ * * (still nothing for the DWT registers, surprisingly) */ -#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ - do { \ - (*(unsigned long *)0xe000edfc) |= (1<<24); /* DEMCR |= DEMCR_TRCENA */ \ - (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ - } while(0) -#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ /** - * @} - */ + * @} + */ #endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/revolution/firmware/inc/dcc_stdio.h b/flight/targets/boards/revolution/firmware/inc/dcc_stdio.h index dbfd39342..3d7354918 100644 --- a/flight/targets/boards/revolution/firmware/inc/dcc_stdio.h +++ b/flight/targets/boards/revolution/firmware/inc/dcc_stdio.h @@ -1,24 +1,24 @@ /*************************************************************************** - * Copyright (C) 2008 by Dominic Rath * - * Dominic.Rath@gmx.de * - * Copyright (C) 2008 by Spencer Oliver * - * spen@spen-soft.co.uk * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ #ifndef DCC_STDIO_H #define DCC_STDIO_H @@ -33,4 +33,4 @@ void dbg_write_str(const char *msg); void dbg_write_char(char msg); void dbg_write_hex32(const unsigned long val); -#endif /* DCC_STDIO_H */ +#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/revolution/firmware/inc/openpilot.h b/flight/targets/boards/revolution/firmware/inc/openpilot.h index 8c76835cd..3cceef152 100644 --- a/flight/targets/boards/revolution/firmware/inc/openpilot.h +++ b/flight/targets/boards/revolution/firmware/inc/openpilot.h @@ -10,19 +10,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_board_sim.h b/flight/targets/boards/revolution/firmware/inc/pios_board_sim.h index e3625f583..68a32b116 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_board_sim.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_board_sim.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file pios_board.h + * @file pios_board.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -28,25 +28,23 @@ #define PIOS_BOARD_H - - -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -#define PIOS_LED_ALARM 0 +// ------------------------ +#define PIOS_LED_ALARM 0 #define PIOS_LED_HEARTBEAT 1 -#define PIOS_LED_NUM 2 +#define PIOS_LED_NUM 2 -//------------------------- +// ------------------------- // COM // // See also pios_board_posix.c -//------------------------- -//#define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_COM_MAX_DEVS 255 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +// ------------------------- +// #define PIOS_USART_TX_BUFFER_SIZE 256 +#define PIOS_COM_BUFFER_SIZE 1024 +#define PIOS_COM_MAX_DEVS 255 +#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE extern uint32_t pios_com_telem_rf_id; extern uint32_t pios_com_telem_usb_id; @@ -54,13 +52,13 @@ extern uint32_t pios_com_gps_id; extern uint32_t pios_com_aux_id; extern uint32_t pios_com_spectrum_id; -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_GPS (pios_com_gps_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_GPS (pios_com_gps_id) #ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_DEBUG (PIOS_COM_AUX) +#define PIOS_COM_AUX (pios_com_aux_id) +#define PIOS_COM_DEBUG (PIOS_COM_AUX) #endif #define PIOS_GCSRCVR_TIMEOUT_MS 200 @@ -68,18 +66,17 @@ extern uint32_t pios_com_spectrum_id; * glue macros for file IO * STM32 uses DOSFS for file IO */ -#define PIOS_FOPEN_READ(filename,file) (file=fopen((char*)filename,"r"))==NULL +#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL -#define PIOS_FOPEN_WRITE(filename,file) (file=fopen((char*)filename,"w"))==NULL +#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL -#define PIOS_FREAD(file,bufferadr,length,resultadr) (*resultadr=fread((uint8_t*)bufferadr,1,length,*file)) != length +#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length -#define PIOS_FWRITE(file,bufferadr,length,resultadr) *resultadr=fwrite((uint8_t*)bufferadr,1,length,*file) +#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) +#define PIOS_FCLOSE(file) fclose(file) -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char*)filename) +#define PIOS_FUNLINK(file) unlink((char *)filename) #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index abd99b314..ca5b69e8f 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -141,15 +141,15 @@ /* #define PIOS_QUATERNION_STABILIZATION */ /* Performance counters */ -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 /* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 1000 -#define HEAP_LIMIT_CRITICAL 500 -#define IRQSTACK_LIMIT_WARNING 150 -#define IRQSTACK_LIMIT_CRITICAL 80 -#define CPULOAD_LIMIT_WARNING 80 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 1000 +#define HEAP_LIMIT_CRITICAL 500 +#define IRQSTACK_LIMIT_WARNING 150 +#define IRQSTACK_LIMIT_CRITICAL 80 +#define CPULOAD_LIMIT_WARNING 80 +#define CPULOAD_LIMIT_CRITICAL 95 /* Task stack sizes */ /* #define PIOS_ACTUATOR_STACK_SIZE 1020 */ @@ -160,7 +160,7 @@ /* #define PIOS_EVENTDISPATCHER_STACK_SIZE 130 */ /* This can't be too high to stop eventdispatcher thread overflowing */ -#define PIOS_EVENTDISAPTCHER_QUEUE 10 +#define PIOS_EVENTDISAPTCHER_QUEUE 10 /* Revolution series */ #define REVOLUTION diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h b/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h index 763587c02..293516522 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,7 +37,7 @@ #define PIOS_INCLUDE_FREERTOS #define PIOS_INCLUDE_TASK_MONITOR #define PIOS_INCLUDE_COM -//#define PIOS_INCLUDE_GPS +// #define PIOS_INCLUDE_GPS #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_TELEMETRY_RF #define PIOS_INCLUDE_TCP @@ -46,21 +46,21 @@ #define PIOS_INCLUDE_RCVR #define PIOS_INCLUDE_GCSRCVR -#define PIOS_RCVR_MAX_CHANNELS 12 -#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_RCVR_MAX_DEVS 3 /* Defaults for Logging */ -#define LOG_FILENAME "PIOS.LOG" -#define STARTUP_LOG_ENABLED 1 +#define LOG_FILENAME "PIOS.LOG" +#define STARTUP_LOG_ENABLED 1 /* COM Module */ -#define GPS_BAUDRATE 19200 -#define TELEM_BAUDRATE 19200 -#define AUXUART_ENABLED 0 -#define AUXUART_BAUDRATE 19200 +#define GPS_BAUDRATE 19200 +#define TELEM_BAUDRATE 19200 +#define AUXUART_ENABLED 0 +#define AUXUART_BAUDRATE 19200 -#define TELEM_QUEUE_SIZE 20 -#define PIOS_TELEM_STACK_SIZE 2048 +#define TELEM_QUEUE_SIZE 20 +#define PIOS_TELEM_STACK_SIZE 2048 /* Stabilization options */ #define PIOS_QUATERNION_STABILIZATION @@ -68,12 +68,12 @@ /* GPS options */ #define PIOS_GPS_SETS_HOMELOCATION -#define HEAP_LIMIT_WARNING 4000 -#define HEAP_LIMIT_CRITICAL 1000 -#define IRQSTACK_LIMIT_WARNING 150 -#define IRQSTACK_LIMIT_CRITICAL 80 -#define CPULOAD_LIMIT_WARNING 80 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 4000 +#define HEAP_LIMIT_CRITICAL 1000 +#define IRQSTACK_LIMIT_WARNING 150 +#define IRQSTACK_LIMIT_CRITICAL 80 +#define CPULOAD_LIMIT_WARNING 80 +#define CPULOAD_LIMIT_CRITICAL 95 #define REVOLUTION diff --git a/flight/targets/boards/revolution/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/revolution/firmware/inc/pios_usb_board_data.h index b4b42536a..2e607ad2c 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_usb_board_data.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_usb_board_data.h @@ -35,12 +35,12 @@ #define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 4 +#define PIOS_USB_BOARD_EP_NUM 4 -#include /* USB_* macros */ +#include /* USB_* macros */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_FW) -#define PIOS_USB_BOARD_SN_SUFFIX "+FW" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index febdf32d2..d8fbc81a6 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -9,19 +9,19 @@ * @{ * @brief Defines board specific static initializers for hardware for the revomini board. *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -45,12 +45,12 @@ #if defined(PIOS_INCLUDE_RFM22B) // Forward declarations static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, - uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); #endif /** - * Sensor configurations + * Sensor configurations */ #if defined(PIOS_INCLUDE_ADC) @@ -58,77 +58,75 @@ static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, void PIOS_ADC_DMC_irq_handler(void); void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); struct pios_adc_cfg pios_adc_cfg = { - .adc_dev = ADC1, - .dma = { - .irq = { - .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), - .init = { - .NVIC_IRQChannel = DMA2_Stream4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA2_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR - }, - } - }, - .half_flag = DMA_IT_HTIF4, - .full_flag = DMA_IT_TCIF4, - + .adc_dev = ADC1, + .dma = { + .irq = { + .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), + .init = { + .NVIC_IRQChannel = DMA2_Stream4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR + }, + } + }, + .half_flag = DMA_IT_HTIF4, + .full_flag = DMA_IT_TCIF4, }; void PIOS_ADC_DMC_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_ADC_DMA_Handler(); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_ADC_DMA_Handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_ADC) */ #if defined(PIOS_INCLUDE_HMC5883) #include "pios_hmc5883.h" static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { - .vector = PIOS_HMC5883_IRQHandler, - .line = EXTI_Line7, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line7, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_HMC5883_IRQHandler, + .line = EXTI_Line7, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line7, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { - .exti_cfg = &pios_exti_hmc5883_cfg, - .M_ODR = PIOS_HMC5883_ODR_75, - .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, - .Gain = PIOS_HMC5883_GAIN_1_9, - .Mode = PIOS_HMC5883_MODE_CONTINUOUS, - + .exti_cfg = &pios_exti_hmc5883_cfg, + .M_ODR = PIOS_HMC5883_ODR_75, + .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, + .Gain = PIOS_HMC5883_GAIN_1_9, + .Mode = PIOS_HMC5883_MODE_CONTINUOUS, }; #endif /* PIOS_INCLUDE_HMC5883 */ @@ -138,7 +136,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { #if defined(PIOS_INCLUDE_MS5611) #include "pios_ms5611.h" static const struct pios_ms5611_cfg pios_ms5611_cfg = { - .oversampling = MS5611_OSR_512, + .oversampling = MS5611_OSR_512, }; #endif /* PIOS_INCLUDE_MS5611 */ @@ -150,51 +148,51 @@ static const struct pios_ms5611_cfg pios_ms5611_cfg = { #include "pios_mpu6000.h" #include "pios_mpu6000_config.h" static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { - .vector = PIOS_MPU6000_IRQHandler, - .line = EXTI_Line4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line4, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_MPU6000_IRQHandler, + .line = EXTI_Line4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line4, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { - .exti_cfg = &pios_exti_mpu6000_cfg, - .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, - // Clock at 8 khz, downsampled by 12 for 666Hz - .Smpl_rate_div_no_dlp = 11, - // with dlp on output rate is 500Hz - .Smpl_rate_div_dlp = 1, - .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, - .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, - .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, - .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, - .accel_range = PIOS_MPU6000_ACCEL_8G, - .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ, - .orientation = PIOS_MPU6000_TOP_180DEG + .exti_cfg = &pios_exti_mpu6000_cfg, + .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, + // Clock at 8 khz, downsampled by 12 for 666Hz + .Smpl_rate_div_no_dlp = 11, + // with dlp on output rate is 500Hz + .Smpl_rate_div_dlp = 1, + .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, + .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, + .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_TOP_180DEG }; #endif /* PIOS_INCLUDE_MPU6000 */ @@ -204,115 +202,118 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { */ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 -#define PIOS_COM_GPS_RX_BUF_LEN 32 +#define PIOS_COM_GPS_RX_BUF_LEN 32 -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 -#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 -#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512 +#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 +#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512 -#define PIOS_COM_HKOSD_RX_BUF_LEN 22 -#define PIOS_COM_HKOSD_TX_BUF_LEN 22 +#define PIOS_COM_HKOSD_RX_BUF_LEN 22 +#define PIOS_COM_HKOSD_TX_BUF_LEN 22 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) #define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 uint32_t pios_com_debug_id; -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -uint32_t pios_com_gps_id = 0; +uint32_t pios_com_gps_id = 0; uint32_t pios_com_telem_usb_id = 0; -uint32_t pios_com_telem_rf_id = 0; -uint32_t pios_com_bridge_id = 0; -uint32_t pios_com_overo_id = 0; -uint32_t pios_com_hkosd_id = 0; +uint32_t pios_com_telem_rf_id = 0; +uint32_t pios_com_bridge_id = 0; +uint32_t pios_com_overo_id = 0; +uint32_t pios_com_hkosd_id = 0; #if defined(PIOS_INCLUDE_RFM22B) uint32_t pios_rfm22b_id = 0; #endif -/* +/* * Setup a com port based on the passed cfg, driver and buffer sizes. tx size of -1 make the port rx only */ static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, size_t rx_buf_len, size_t tx_buf_len, - const struct pios_com_driver *com_driver, uint32_t *pios_com_id) + const struct pios_com_driver *com_driver, uint32_t *pios_com_id) { - uint32_t pios_usart_id; - if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { - PIOS_Assert(0); - } - - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(rx_buf_len); - PIOS_Assert(rx_buffer); - if(tx_buf_len!= (size_t)-1){ // this is the case for rx/tx ports - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(tx_buf_len); - PIOS_Assert(tx_buffer); - - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - tx_buffer, tx_buf_len)) { - PIOS_Assert(0); - } - } - else{ //rx only port - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - NULL, 0)) { - PIOS_Assert(0); - } - } + uint32_t pios_usart_id; + + if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(rx_buf_len); + PIOS_Assert(rx_buffer); + if (tx_buf_len != (size_t)-1) { // this is the case for rx/tx ports + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(tx_buf_len); + PIOS_Assert(tx_buffer); + + if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, + rx_buffer, rx_buf_len, + tx_buffer, tx_buf_len)) { + PIOS_Assert(0); + } + } else { // rx only port + if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, + rx_buffer, rx_buf_len, + NULL, 0)) { + PIOS_Assert(0); + } + } } -static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, - const struct pios_com_driver *usart_com_driver,enum pios_dsm_proto *proto, - ManualControlSettingsChannelGroupsOptions channelgroup,uint8_t *bind) +static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, + const struct pios_com_driver *usart_com_driver, enum pios_dsm_proto *proto, + ManualControlSettingsChannelGroupsOptions channelgroup, uint8_t *bind) { - uint32_t pios_usart_dsm_id; - if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, - pios_usart_dsm_id, *proto, *bind)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; + uint32_t pios_usart_dsm_id; + + if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_id; + if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, + pios_usart_dsm_id, *proto, *bind)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_rcvr_id; + if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; } static void PIOS_Board_configure_pwm(const struct pios_pwm_cfg *pwm_cfg) { - /* Set up the receiver port. Later this should be optional */ - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, pwm_cfg); + /* Set up the receiver port. Later this should be optional */ + uint32_t pios_pwm_id; - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; + PIOS_PWM_Init(&pios_pwm_id, pwm_cfg); + + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg) { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, ppm_cfg); + uint32_t pios_ppm_id; - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + PIOS_PPM_Init(&pios_ppm_id, ppm_cfg); + + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } /** @@ -323,504 +324,502 @@ static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg) #include -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); + const struct pios_board_info *bdinfo = &pios_board_info_blob; - const struct pios_board_info * bdinfo = &pios_board_info_blob; - #if defined(PIOS_INCLUDE_LED) - const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); - PIOS_Assert(led_cfg); - PIOS_LED_Init(led_cfg); -#endif /* PIOS_INCLUDE_LED */ + const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ - /* Set up the SPI interface to the gyro/acelerometer */ - if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { - PIOS_DEBUG_Assert(0); - } - - /* Set up the SPI interface to the flash and rfm22b */ - if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { - PIOS_DEBUG_Assert(0); - } + /* Set up the SPI interface to the gyro/acelerometer */ + if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { + PIOS_DEBUG_Assert(0); + } + + /* Set up the SPI interface to the flash and rfm22b */ + if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { + PIOS_DEBUG_Assert(0); + } #if defined(PIOS_INCLUDE_FLASH) - /* Connect flash to the appropriate interface and configure it */ - uintptr_t flash_id; - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1)) { - PIOS_DEBUG_Assert(0); - } + /* Connect flash to the appropriate interface and configure it */ + uintptr_t flash_id; + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1)) { + PIOS_DEBUG_Assert(0); + } - uintptr_t fs_id; - if (PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id)) { - PIOS_DEBUG_Assert(0); - } + uintptr_t fs_id; + if (PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } #endif - + #if defined(PIOS_INCLUDE_RTC) - PIOS_RTC_Init(&pios_rtc_main_cfg); + PIOS_RTC_Init(&pios_rtc_main_cfg); #endif - /* IAP System Setup */ - PIOS_IAP_Init(); - // check for safe mode commands from gcs - if(PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && - PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && - PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) - { - PIOS_FLASHFS_Format(fs_id); - PIOS_IAP_WriteBootCmd(0,0); - PIOS_IAP_WriteBootCmd(1,0); - PIOS_IAP_WriteBootCmd(2,0); - } + /* IAP System Setup */ + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + PIOS_FLASHFS_Format(fs_id); + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - - HwSettingsInitialize(); - /* Initialize the alarms library */ - AlarmsInitialize(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } + HwSettingsInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Set up pulse timers */ - PIOS_TIM_InitClock(&tim_1_cfg); - PIOS_TIM_InitClock(&tim_3_cfg); - PIOS_TIM_InitClock(&tim_4_cfg); - PIOS_TIM_InitClock(&tim_5_cfg); - PIOS_TIM_InitClock(&tim_8_cfg); - PIOS_TIM_InitClock(&tim_9_cfg); - PIOS_TIM_InitClock(&tim_10_cfg); - PIOS_TIM_InitClock(&tim_11_cfg); - PIOS_TIM_InitClock(&tim_12_cfg); - - uint16_t boot_count = PIOS_IAP_ReadBootCount(); - if (boot_count < 3) { - PIOS_IAP_WriteBootCount(++boot_count); - AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); - } else { - /* Too many failed boot attempts, force hwsettings to defaults */ - HwSettingsSetDefaults(HwSettingsHandle(), 0); - AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); - } + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + + /* Set up pulse timers */ + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_4_cfg); + PIOS_TIM_InitClock(&tim_5_cfg); + PIOS_TIM_InitClock(&tim_8_cfg); + PIOS_TIM_InitClock(&tim_9_cfg); + PIOS_TIM_InitClock(&tim_10_cfg); + PIOS_TIM_InitClock(&tim_11_cfg); + PIOS_TIM_InitClock(&tim_12_cfg); + + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } - //PIOS_IAP_Init(); + // PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; + /* Flags to determine if various USB interfaces are advertised */ + bool usb_hid_present = false; + bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; + usb_cdc_present = true; #else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; #endif - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); #if defined(PIOS_INCLUDE_USB_CDC) - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); + uint8_t hwsettings_usb_vcpport; + /* Configure the USB VCP port */ + HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } + if (!usb_cdc_present) { + /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ + hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; + } - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: + switch (hwsettings_usb_vcpport) { + case HWSETTINGS_USB_VCPPORT_DISABLED: + break; + case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_telem_usb_id); -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_telem_usb_id); +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) - PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_vcp_id); -#endif /* PIOS_INCLUDE_COM */ - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ + PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_vcp_id); +#endif /* PIOS_INCLUDE_COM */ + break; + } +#endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); + /* Configure the usb HID port */ + uint8_t hwsettings_usb_hidport; + HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } + if (!usb_hid_present) { + /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ + hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; + } - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: + switch (hwsettings_usb_hidport) { + case HWSETTINGS_USB_HIDPORT_DISABLED: + break; + case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } + { + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + } -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ - if (usb_hid_present || usb_cdc_present) { - PIOS_USBHOOK_Activate(); - } -#endif /* PIOS_INCLUDE_USB */ + if (usb_hid_present || usb_cdc_present) { + PIOS_USBHOOK_Activate(); + } +#endif /* PIOS_INCLUDE_USB */ - /* Configure IO ports */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); - - /* Configure main USART port */ - uint8_t hwsettings_mainport; - HwSettingsRM_MainPortGet(&hwsettings_mainport); - switch (hwsettings_mainport) { - case HWSETTINGS_RM_MAINPORT_DISABLED: - break; - case HWSETTINGS_RM_MAINPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RM_MAINPORT_GPS: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); - break; - case HWSETTINGS_RM_MAINPORT_SBUS: + /* Configure IO ports */ + uint8_t hwsettings_DSMxBind; + HwSettingsDSMxBindGet(&hwsettings_DSMxBind); + + /* Configure main USART port */ + uint8_t hwsettings_mainport; + HwSettingsRM_MainPortGet(&hwsettings_mainport); + switch (hwsettings_mainport) { + case HWSETTINGS_RM_MAINPORT_DISABLED: + break; + case HWSETTINGS_RM_MAINPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); + break; + case HWSETTINGS_RM_MAINPORT_GPS: + PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); + break; + case HWSETTINGS_RM_MAINPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) - { - uint32_t pios_usart_sbus_id; - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { - PIOS_Assert(0); - } + { + uint32_t pios_usart_sbus_id; + if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { + PIOS_Assert(0); + } - uint32_t pios_sbus_id; - if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_Assert(0); - } + uint32_t pios_sbus_id; + if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { + PIOS_Assert(0); + } - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; - } + uint32_t pios_sbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; + } #endif - break; - case HWSETTINGS_RM_MAINPORT_DSM2: - case HWSETTINGS_RM_MAINPORT_DSMX10BIT: - case HWSETTINGS_RM_MAINPORT_DSMX11BIT: - { - enum pios_dsm_proto proto; - switch (hwsettings_mainport) { - case HWSETTINGS_RM_MAINPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_RM_MAINPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_RM_MAINPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } + break; + case HWSETTINGS_RM_MAINPORT_DSM2: + case HWSETTINGS_RM_MAINPORT_DSMX10BIT: + case HWSETTINGS_RM_MAINPORT_DSMX11BIT: + { + enum pios_dsm_proto proto; + switch (hwsettings_mainport) { + case HWSETTINGS_RM_MAINPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_RM_MAINPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_RM_MAINPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } - // Force binding to zero on the main port - hwsettings_DSMxBind = 0; + // Force binding to zero on the main port + hwsettings_DSMxBind = 0; - //TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, - &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hwsettings_DSMxBind); - } - break; - case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE: + // TODO: Define the various Channelgroup for Revo dsm inputs and handle here + PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, + &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); + } + break; + case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - break; - case HWSETTINGS_RM_MAINPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RM_MAINPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } /* hwsettings_rm_mainport */ + { + PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); + } +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ + break; + case HWSETTINGS_RM_MAINPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + case HWSETTINGS_RM_MAINPORT_OSDHK: + PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); + break; + } /* hwsettings_rm_mainport */ - if (hwsettings_mainport != HWSETTINGS_RM_MAINPORT_SBUS) { - GPIO_Init(pios_sbus_cfg.inv.gpio, &pios_sbus_cfg.inv.init); - GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); - } + if (hwsettings_mainport != HWSETTINGS_RM_MAINPORT_SBUS) { + GPIO_Init(pios_sbus_cfg.inv.gpio, &pios_sbus_cfg.inv.init); + GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); + } - /* Configure FlexiPort */ - uint8_t hwsettings_flexiport; - HwSettingsRM_FlexiPortGet(&hwsettings_flexiport); - switch (hwsettings_flexiport) { - case HWSETTINGS_RM_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_RM_FLEXIPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RM_FLEXIPORT_I2C: + /* Configure FlexiPort */ + uint8_t hwsettings_flexiport; + HwSettingsRM_FlexiPortGet(&hwsettings_flexiport); + switch (hwsettings_flexiport) { + case HWSETTINGS_RM_FLEXIPORT_DISABLED: + break; + case HWSETTINGS_RM_FLEXIPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); + break; + case HWSETTINGS_RM_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) - { - if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_I2C */ - break; - case HWSETTINGS_RM_FLEXIPORT_GPS: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); - break; - case HWSETTINGS_RM_FLEXIPORT_DSM2: - case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: - case HWSETTINGS_RM_FLEXIPORT_DSMX11BIT: - { - enum pios_dsm_proto proto; - switch (hwsettings_flexiport) { - case HWSETTINGS_RM_FLEXIPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_RM_FLEXIPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } - //TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, - &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT,&hwsettings_DSMxBind); - } - break; - case HWSETTINGS_RM_FLEXIPORT_DEBUGCONSOLE: + { + if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_I2C */ + break; + case HWSETTINGS_RM_FLEXIPORT_GPS: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); + break; + case HWSETTINGS_RM_FLEXIPORT_DSM2: + case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: + case HWSETTINGS_RM_FLEXIPORT_DSMX11BIT: + { + enum pios_dsm_proto proto; + switch (hwsettings_flexiport) { + case HWSETTINGS_RM_FLEXIPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_RM_FLEXIPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } + // TODO: Define the various Channelgroup for Revo dsm inputs and handle here + PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, + &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT, &hwsettings_DSMxBind); + } + break; + case HWSETTINGS_RM_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - break; - case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RM_FLEXIPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } /* hwsettings_rm_flexiport */ + { + PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); + } +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ + break; + case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + case HWSETTINGS_RM_FLEXIPORT_OSDHK: + PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); + break; + } /* hwsettings_rm_flexiport */ - /* Initalize the RFM22B radio COM device. */ + /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) - uint8_t hwsettings_radioport; - HwSettingsRadioPortGet(&hwsettings_radioport); - uint8_t hwsettings_maxrfpower; - HwSettingsMaxRFPowerGet(&hwsettings_maxrfpower); - uint32_t hwsettings_deffreq; - HwSettingsDefaultFrequencyGet(&hwsettings_deffreq); - switch (hwsettings_radioport) { - case HWSETTINGS_RADIOPORT_DISABLED: - break; - case HWSETTINGS_RADIOPORT_TELEMETRY: - { - const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); - if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { - PIOS_Assert(0); - } + uint8_t hwsettings_radioport; + HwSettingsRadioPortGet(&hwsettings_radioport); + uint8_t hwsettings_maxrfpower; + HwSettingsMaxRFPowerGet(&hwsettings_maxrfpower); + uint32_t hwsettings_deffreq; + HwSettingsDefaultFrequencyGet(&hwsettings_deffreq); + switch (hwsettings_radioport) { + case HWSETTINGS_RADIOPORT_DISABLED: + break; + case HWSETTINGS_RADIOPORT_TELEMETRY: + { + const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); + if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { + PIOS_Assert(0); + } - // Set the modem parameters and reinitilize the modem. - PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, hwsettings_deffreq); - switch (hwsettings_maxrfpower) - { - case OPLINKSETTINGS_MAXRFPOWER_125: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); - break; - case OPLINKSETTINGS_MAXRFPOWER_16: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); - break; - case OPLINKSETTINGS_MAXRFPOWER_316: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); - break; - case OPLINKSETTINGS_MAXRFPOWER_63: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); - break; - case OPLINKSETTINGS_MAXRFPOWER_126: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); - break; - case OPLINKSETTINGS_MAXRFPOWER_25: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); - break; - case OPLINKSETTINGS_MAXRFPOWER_50: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); - break; - case OPLINKSETTINGS_MAXRFPOWER_100: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); - break; - default: - // do nothing - break; - } - PIOS_RFM22B_Reinit(pios_rfm22b_id); + // Set the modem parameters and reinitilize the modem. + PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, hwsettings_deffreq); + switch (hwsettings_maxrfpower) { + case OPLINKSETTINGS_MAXRFPOWER_125: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); + break; + case OPLINKSETTINGS_MAXRFPOWER_16: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); + break; + case OPLINKSETTINGS_MAXRFPOWER_316: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); + break; + case OPLINKSETTINGS_MAXRFPOWER_63: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); + break; + case OPLINKSETTINGS_MAXRFPOWER_126: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); + break; + case OPLINKSETTINGS_MAXRFPOWER_25: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); + break; + case OPLINKSETTINGS_MAXRFPOWER_50: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); + break; + case OPLINKSETTINGS_MAXRFPOWER_100: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); + break; + default: + // do nothing + break; + } + PIOS_RFM22B_Reinit(pios_rfm22b_id); #ifdef PIOS_INCLUDE_RFM22B_COM - uint8_t *rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, - rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) - PIOS_Assert(0); + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, + rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } #endif #ifdef PIOS_INCLUDE_RFM22B_RCVR - if (PIOS_RFM22B_RCVR_Init(pios_rfm22b_id) != 0) - PIOS_Assert(0); - uint32_t pios_rfm22b_rcvr_id; - if (PIOS_RCVR_Init(&pios_rfm22b_rcvr_id, &pios_rfm22b_rcvr_driver, pios_rfm22b_id)) - PIOS_Assert(0); - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_rfm22b_rcvr_id; + if (PIOS_RFM22B_RCVR_Init(pios_rfm22b_id) != 0) { + PIOS_Assert(0); + } + uint32_t pios_rfm22b_rcvr_id; + if (PIOS_RCVR_Init(&pios_rfm22b_rcvr_id, &pios_rfm22b_rcvr_driver, pios_rfm22b_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_rfm22b_rcvr_id; #endif - // Set the com port configuration callback. - PIOS_RFM22B_SetComConfigCallback(pios_rfm22b_id, &configureComCallback); + // Set the com port configuration callback. + PIOS_RFM22B_SetComConfigCallback(pios_rfm22b_id, &configureComCallback); - break; - } - } + break; + } + } #endif /* PIOS_INCLUDE_RFM22B */ -#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PWM) - - const struct pios_servo_cfg *pios_servo_cfg; - // default to servo outputs only - pios_servo_cfg = &pios_servo_cfg_out; +#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PWM) + + const struct pios_servo_cfg *pios_servo_cfg; + // default to servo outputs only + pios_servo_cfg = &pios_servo_cfg_out; #endif - - /* Configure the receiver port*/ - uint8_t hwsettings_rcvrport; - HwSettingsRM_RcvrPortGet(&hwsettings_rcvrport); - // - switch (hwsettings_rcvrport){ - case HWSETTINGS_RM_RCVRPORT_DISABLED: - break; - case HWSETTINGS_RM_RCVRPORT_PWM: + + /* Configure the receiver port*/ + uint8_t hwsettings_rcvrport; + HwSettingsRM_RcvrPortGet(&hwsettings_rcvrport); + // + switch (hwsettings_rcvrport) { + case HWSETTINGS_RM_RCVRPORT_DISABLED: + break; + case HWSETTINGS_RM_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) - /* Set up the receiver port. Later this should be optional */ - PIOS_Board_configure_pwm(&pios_pwm_cfg); -#endif /* PIOS_INCLUDE_PWM */ - break; - case HWSETTINGS_RM_RCVRPORT_PPM: - case HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS: - case HWSETTINGS_RM_RCVRPORT_PPMPWM: + /* Set up the receiver port. Later this should be optional */ + PIOS_Board_configure_pwm(&pios_pwm_cfg); +#endif /* PIOS_INCLUDE_PWM */ + break; + case HWSETTINGS_RM_RCVRPORT_PPM: + case HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS: + case HWSETTINGS_RM_RCVRPORT_PPMPWM: #if defined(PIOS_INCLUDE_PPM) - if(hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) - { - // configure servo outputs and the remaining 5 inputs as outputs - pios_servo_cfg = &pios_servo_cfg_out_in_ppm; - } + if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) { + // configure servo outputs and the remaining 5 inputs as outputs + pios_servo_cfg = &pios_servo_cfg_out_in_ppm; + } - PIOS_Board_configure_ppm(&pios_ppm_cfg); + PIOS_Board_configure_ppm(&pios_ppm_cfg); - // enable pwm on the remaining channels - if(hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMPWM) - { - PIOS_Board_configure_pwm(&pios_pwm_ppm_cfg); - } + // enable pwm on the remaining channels + if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMPWM) { + PIOS_Board_configure_pwm(&pios_pwm_ppm_cfg); + } - break; -#endif /* PIOS_INCLUDE_PPM */ - case HWSETTINGS_RM_RCVRPORT_OUTPUTS: - // configure only the servo outputs - pios_servo_cfg = &pios_servo_cfg_out_in; - break; - } + break; +#endif /* PIOS_INCLUDE_PPM */ + case HWSETTINGS_RM_RCVRPORT_OUTPUTS: + // configure only the servo outputs + pios_servo_cfg = &pios_servo_cfg_out_in; + break; + } #if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ + GCSReceiverInitialize(); + uint32_t pios_gcsrcvr_id; + PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_GCSRCVR */ #ifndef PIOS_ENABLE_DEBUG_PINS - // pios_servo_cfg points to the correct configuration based on input port settings - PIOS_Servo_Init(pios_servo_cfg); + // pios_servo_cfg points to the correct configuration based on input port settings + PIOS_Servo_Init(pios_servo_cfg); #else - PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); + PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); #endif - - if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { - PIOS_DEBUG_Assert(0); - } - - PIOS_DELAY_WaitmS(50); + + if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { + PIOS_DEBUG_Assert(0); + } + + PIOS_DELAY_WaitmS(50); #if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); + PIOS_ADC_Init(&pios_adc_cfg); #endif #if defined(PIOS_INCLUDE_HMC5883) - PIOS_HMC5883_Init(&pios_hmc5883_cfg); + PIOS_HMC5883_Init(&pios_hmc5883_cfg); #endif - + #if defined(PIOS_INCLUDE_MS5611) - PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id); + PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id); #endif #if defined(PIOS_INCLUDE_MPU6000) - PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg); - PIOS_MPU6000_CONFIG_Configure(); + PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); + PIOS_MPU6000_CONFIG_Configure(); #endif - } #if defined(PIOS_INCLUDE_RFM22B) @@ -832,12 +831,13 @@ void PIOS_Board_Init(void) { * \param[in] com_speed The com port speed */ static void configureComCallback(__attribute__((unused)) OPLinkSettingsRemoteMainPortOptions main_port, - __attribute__((unused)) OPLinkSettingsRemoteFlexiPortOptions flexi_port, - __attribute__((unused)) OPLinkSettingsRemoteVCPPortOptions vcp_port, - OPLinkSettingsComSpeedOptions com_speed, - uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing) + __attribute__((unused)) OPLinkSettingsRemoteFlexiPortOptions flexi_port, + __attribute__((unused)) OPLinkSettingsRemoteVCPPortOptions vcp_port, + OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing) { uint32_t comBaud = 9600; + switch (com_speed) { case OPLINKSETTINGS_COMSPEED_2400: comBaud = 2400; @@ -869,10 +869,9 @@ static void configureComCallback(__attribute__((unused)) OPLinkSettingsRemoteMai PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, min_frequency, max_frequency, channel_spacing); } -#endif +#endif /* if defined(PIOS_INCLUDE_RFM22B) */ /** * @} * @} */ - diff --git a/flight/targets/boards/revolution/firmware/pios_board_sim.c b/flight/targets/boards/revolution/firmware/pios_board_sim.c index 22cc26d7d..8af566e93 100644 --- a/flight/targets/boards/revolution/firmware/pios_board_sim.c +++ b/flight/targets/boards/revolution/firmware/pios_board_sim.c @@ -7,19 +7,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,30 +39,28 @@ #include #include -void Stack_Change() { -} +void Stack_Change() {} -void Stack_Change_Weak() { -} +void Stack_Change_Weak() {} const struct pios_tcp_cfg pios_tcp_telem_cfg = { - .ip = "0.0.0.0", - .port = 9001, + .ip = "0.0.0.0", + .port = 9001, }; const struct pios_udp_cfg pios_udp_telem_cfg = { - .ip = "0.0.0.0", - .port = 9001, + .ip = "0.0.0.0", + .port = 9001, }; const struct pios_tcp_cfg pios_tcp_gps_cfg = { - .ip = "0.0.0.0", - .port = 9001, + .ip = "0.0.0.0", + .port = 9001, }; const struct pios_tcp_cfg pios_tcp_debug_cfg = { - .ip = "0.0.0.0", - .port = 9002, + .ip = "0.0.0.0", + .port = 9002, }; #ifdef PIOS_COM_AUX @@ -70,42 +68,42 @@ const struct pios_tcp_cfg pios_tcp_debug_cfg = { * AUX USART */ const struct pios_tcp_cfg pios_tcp_aux_cfg = { - .ip = "0.0.0.0", - .port = 9003, + .ip = "0.0.0.0", + .port = 9003, }; #endif #define PIOS_COM_TELEM_RF_RX_BUF_LEN 192 #define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 -#define PIOS_COM_GPS_RX_BUF_LEN 96 +#define PIOS_COM_GPS_RX_BUF_LEN 96 /* * Board specific number of devices. */ /* -struct pios_udp_dev pios_udp_devs[] = { -#define PIOS_UDP_TELEM 0 - { + struct pios_udp_dev pios_udp_devs[] = { + #define PIOS_UDP_TELEM 0 + { .cfg = &pios_udp0_cfg, - }, -#define PIOS_UDP_GPS 1 - { + }, + #define PIOS_UDP_GPS 1 + { .cfg = &pios_udp1_cfg, - }, -#define PIOS_UDP_LOCAL 2 - { + }, + #define PIOS_UDP_LOCAL 2 + { .cfg = &pios_udp2_cfg, - }, -#ifdef PIOS_COM_AUX -#define PIOS_UDP_AUX 3 - { + }, + #ifdef PIOS_COM_AUX + #define PIOS_UDP_AUX 3 + { .cfg = &pios_udp3_cfg, - }, -#endif -}; + }, + #endif + }; -uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs); -*/ + uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs); + */ /* * COM devices */ @@ -129,102 +127,101 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; * initializes all the core systems on this specific hardware * called from System/openpilot.c */ -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + UAVObjectsInitializeAll(); - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - UAVObjectsInitializeAll(); + AccelsInitialize(); + BaroAltitudeInitialize(); + MagnetometerInitialize(); + GPSPositionInitialize(); + GyrosInitialize(); + GyrosBiasInitialize(); - AccelsInitialize(); - BaroAltitudeInitialize(); - MagnetometerInitialize(); - GPSPositionInitialize(); - GyrosInitialize(); - GyrosBiasInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Initialize the alarms library */ - AlarmsInitialize(); + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } - - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1 - { - uint32_t pios_tcp_telem_rf_id; - if (PIOS_TCP_Init(&pios_tcp_telem_rf_id, &pios_tcp_telem_cfg)) { - PIOS_Assert(0); - } + { + uint32_t pios_tcp_telem_rf_id; + if (PIOS_TCP_Init(&pios_tcp_telem_rf_id, &pios_tcp_telem_cfg)) { + PIOS_Assert(0); + } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_tcp_com_driver, pios_tcp_telem_rf_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_tcp_com_driver, pios_tcp_telem_rf_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 0 - { - uint32_t pios_udp_telem_rf_id; - if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) { - PIOS_Assert(0); - } - - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + { + uint32_t pios_udp_telem_rf_id; + if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ #if defined(PIOS_INCLUDE_GPS) - { - uint32_t pios_tcp_gps_id; - if (PIOS_TCP_Init(&pios_tcp_gps_id, &pios_tcp_gps_cfg)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_tcp_com_driver, pios_tcp_gps_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_GPS */ -#endif + { + uint32_t pios_tcp_gps_id; + if (PIOS_TCP_Init(&pios_tcp_gps_id, &pios_tcp_gps_cfg)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_tcp_com_driver, pios_tcp_gps_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_GPS */ +#endif /* if defined(PIOS_INCLUDE_COM) */ #if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ - + GCSReceiverInitialize(); + uint32_t pios_gcsrcvr_id; + PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_GCSRCVR */ } /** diff --git a/flight/targets/boards/revolution/firmware/revolution.c b/flight/targets/boards/revolution/firmware/revolution.c index 02c7f0d01..42b4a995e 100644 --- a/flight/targets/boards/revolution/firmware/revolution.c +++ b/flight/targets/boards/revolution/firmware/revolution.c @@ -9,25 +9,25 @@ * @brief This is where the OP firmware starts. Those files also define the compile-time * options of the firmware. * @{ - * @file openpilot.c + * @file openpilot.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Sets up and runs main OpenPilot tasks. * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,12 +36,12 @@ #include /* Task Priorities */ -#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) /* Global Variables */ /* Local Variables */ -#define INCLUDE_TEST_TASKS 0 +#define INCLUDE_TEST_TASKS 0 #if INCLUDE_TEST_TASKS static uint8_t sdcard_available; #endif @@ -62,11 +62,11 @@ void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value); /* Prototype of PIOS_Board_Init() function */ extern void PIOS_Board_Init(void); extern void Stack_Change(void); -static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change"))); +static void Stack_Change_Weak() __attribute__((weakref("Stack_Change"))); /* Local Variables */ -#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority -#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive +#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority +#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive static xTaskHandle initTaskHandle; /* Function Prototypes */ @@ -76,65 +76,64 @@ static void initTask(void *parameters); extern void InitModules(void); /** -* OpenPilot Main function: -* -* Initialize PiOS
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
-* Start FreeRTOS Scheduler (vTaskStartScheduler)
-* If something goes wrong, blink LED1 and LED2 every 100ms -* -*/ + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler)
+ * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ int main() { - int result; + int result; - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ - vPortInitialiseBlocks(); + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + vPortInitialiseBlocks(); - /* Brings up System using CMSIS functions, enables the LEDs. */ - PIOS_SYS_Init(); - - /* For Revolution we use a FreeRTOS task to bring up the system so we can */ - /* always rely on FreeRTOS primitive */ - result = xTaskCreate(initTask, (const signed char *)"init", - INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, - &initTaskHandle); - PIOS_Assert(result == pdPASS); - - /* Start the FreeRTOS scheduler */ - vTaskStartScheduler(); + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); - /* If all is well we will never reach here as the scheduler will now be running. */ - /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ - PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ - for(;;) { \ - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ - PIOS_DELAY_WaitmS(100); \ - }; + /* For Revolution we use a FreeRTOS task to bring up the system so we can */ + /* always rely on FreeRTOS primitive */ + result = xTaskCreate(initTask, (const signed char *)"init", + INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, + &initTaskHandle); + PIOS_Assert(result == pdPASS); - return 0; + /* Start the FreeRTOS scheduler */ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ + PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ + for (;;) { \ + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ + PIOS_DELAY_WaitmS(100); \ + } + ; + + return 0; } /** * Initialisation task. * * Runs board and module initialisation, then terminates. */ -void -initTask(__attribute__((unused)) void *parameters) +void initTask(__attribute__((unused)) void *parameters) { - /* board driver init */ - PIOS_Board_Init(); - - /* Initialize modules */ - MODULE_INITIALISE_ALL; - - /* terminate this task */ - vTaskDelete(NULL); + /* board driver init */ + PIOS_Board_Init(); + + /* Initialize modules */ + MODULE_INITIALISE_ALL; + + /* terminate this task */ + vTaskDelete(NULL); } /** * @} * @} */ - diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index ef1434754..8bdebb671 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -31,26 +31,26 @@ #include -//------------------------ +// ------------------------ // Timers and Channels Used -//------------------------ +// ------------------------ /* - Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 - ------+-----------+-----------+-----------+---------- - TIM1 | | | | - TIM2 | --------------- PIOS_DELAY ----------------- - TIM3 | | | | - TIM4 | | | | - TIM5 | | | | - TIM6 | | | | - TIM7 | | | | - TIM8 | | | | - ------+-----------+-----------+-----------+---------- + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | | | | + TIM2 | --------------- PIOS_DELAY ----------------- + TIM3 | | | | + TIM4 | | | | + TIM5 | | | | + TIM6 | | | | + TIM7 | | | | + TIM8 | | | | + ------+-----------+-----------+-----------+---------- */ -//------------------------ +// ------------------------ // DMA Channels Used -//------------------------ +// ------------------------ /* Channel 1 - */ /* Channel 2 - SPI1 RX */ /* Channel 3 - SPI1 TX */ @@ -64,243 +64,243 @@ /* Channel 11 - */ /* Channel 12 - */ -//------------------------ +// ------------------------ // BOOTLOADER_SETTINGS -//------------------------ -#define BOARD_READABLE true -#define BOARD_WRITABLE true -#define MAX_DEL_RETRYS 3 +// ------------------------ +#define BOARD_READABLE true +#define BOARD_WRITABLE true +#define MAX_DEL_RETRYS 3 -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -#define PIOS_LED_HEARTBEAT 0 -#define PIOS_LED_ALARM 1 +// ------------------------ +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_LED_ALARM 1 #ifdef PIOS_RFM22B_DEBUG_ON_TELEM -#define PIOS_LED_D1 2 -#define PIOS_LED_D2 3 -#define PIOS_LED_D3 4 -#define PIOS_LED_D4 5 +#define PIOS_LED_D1 2 +#define PIOS_LED_D2 3 +#define PIOS_LED_D3 4 +#define PIOS_LED_D4 5 -#define D1_LED_ON PIOS_LED_On(PIOS_LED_D1) -#define D1_LED_OFF PIOS_LED_Off(PIOS_LED_D1) -#define D1_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D1) +#define D1_LED_ON PIOS_LED_On(PIOS_LED_D1) +#define D1_LED_OFF PIOS_LED_Off(PIOS_LED_D1) +#define D1_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D1) -#define D2_LED_ON PIOS_LED_On(PIOS_LED_D2) -#define D2_LED_OFF PIOS_LED_Off(PIOS_LED_D2) -#define D2_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D2) +#define D2_LED_ON PIOS_LED_On(PIOS_LED_D2) +#define D2_LED_OFF PIOS_LED_Off(PIOS_LED_D2) +#define D2_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D2) -#define D3_LED_ON PIOS_LED_On(PIOS_LED_D3) -#define D3_LED_OFF PIOS_LED_Off(PIOS_LED_D3) -#define D3_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D3) +#define D3_LED_ON PIOS_LED_On(PIOS_LED_D3) +#define D3_LED_OFF PIOS_LED_Off(PIOS_LED_D3) +#define D3_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D3) -#define D4_LED_ON PIOS_LED_On(PIOS_LED_D4) -#define D4_LED_OFF PIOS_LED_Off(PIOS_LED_D4) -#define D4_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D4) +#define D4_LED_ON PIOS_LED_On(PIOS_LED_D4) +#define D4_LED_OFF PIOS_LED_Off(PIOS_LED_D4) +#define D4_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D4) #endif /* PIOS_RFM22B_DEBUG_ON_TELEM */ -//------------------------ +// ------------------------ // PIOS_SPI // See also pios_board.c -//------------------------ -#define PIOS_SPI_MAX_DEVS 3 +// ------------------------ +#define PIOS_SPI_MAX_DEVS 3 -//------------------------ +// ------------------------ // PIOS_WDG -//------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -#define PIOS_WDG_REGISTER RTC_BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_SENSORS 0x0010 +// ------------------------ +#define PIOS_WATCHDOG_TIMEOUT 250 +#define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_SENSORS 0x0010 -//------------------------ +// ------------------------ // PIOS_I2C // See also pios_board.c -//------------------------ -#define PIOS_I2C_MAX_DEVS 3 +// ------------------------ +#define PIOS_I2C_MAX_DEVS 3 extern uint32_t pios_i2c_mag_pressure_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_pressure_adapter_id) +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_pressure_adapter_id) extern uint32_t pios_i2c_flexiport_adapter_id; -#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) -#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) -//------------------------- +// ------------------------- // PIOS_USART // // See also pios_board.c -//------------------------- -#define PIOS_USART_MAX_DEVS 5 +// ------------------------- +#define PIOS_USART_MAX_DEVS 5 -//------------------------- +// ------------------------- // PIOS_COM // // See also pios_board.c -//------------------------- -#define PIOS_COM_MAX_DEVS 4 +// ------------------------- +#define PIOS_COM_MAX_DEVS 4 extern uint32_t pios_com_telem_rf_id; extern uint32_t pios_com_gps_id; extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_bridge_id; extern uint32_t pios_com_vcp_id; extern uint32_t pios_com_hkosd_id; -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_OSDHK (pios_com_hkosd_id) +#define PIOS_COM_GPS (pios_com_gps_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_BRIDGE (pios_com_bridge_id) +#define PIOS_COM_VCP (pios_com_vcp_id) +#define PIOS_COM_OSDHK (pios_com_hkosd_id) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) extern uint32_t pios_com_debug_id; -#define PIOS_COM_DEBUG (pios_com_debug_id) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#define PIOS_COM_DEBUG (pios_com_debug_id) +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #if defined(PIOS_INCLUDE_RFM22B) extern uint32_t pios_rfm22b_id; extern uint32_t pios_spi_telem_flash_id; -#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id) +#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id) #endif /* PIOS_INCLUDE_RFM22B */ -//------------------------- +// ------------------------- // Packet Handler -//------------------------- -#define RS_ECC_NPARITY 4 -#define PIOS_PH_MAX_PACKET 255 -#define PIOS_PH_WIN_SIZE 3 +// ------------------------- +#define RS_ECC_NPARITY 4 +#define PIOS_PH_MAX_PACKET 255 +#define PIOS_PH_WIN_SIZE 3 #define PIOS_PH_MAX_CONNECTIONS 1 extern uint32_t pios_packet_handler; -#define PIOS_PACKET_HANDLER (pios_packet_handler) +#define PIOS_PACKET_HANDLER (pios_packet_handler) -//------------------------ -// TELEMETRY -//------------------------ -#define TELEM_QUEUE_SIZE 80 -#define PIOS_TELEM_STACK_SIZE 624 +// ------------------------ +// TELEMETRY +// ------------------------ +#define TELEM_QUEUE_SIZE 80 +#define PIOS_TELEM_STACK_SIZE 624 -//------------------------- +// ------------------------- // System Settings -// +// // See also System_stm32f4xx.c -//------------------------- -//These macros are deprecated -//please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below -//#define PIOS_MASTER_CLOCK -//#define PIOS_PERIPHERAL_CLOCK -//#define PIOS_PERIPHERAL_CLOCK +// ------------------------- +// These macros are deprecated +// please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below +// #define PIOS_MASTER_CLOCK +// #define PIOS_PERIPHERAL_CLOCK +// #define PIOS_PERIPHERAL_CLOCK -#define PIOS_SYSCLK 168000000 -// Peripherals that belongs to APB1 are: -// DAC |PWR |CAN1,2 -// I2C1,2,3 |UART4,5 |USART3,2 -// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 -// I2S2Ext |IWDG |WWDG -// RTC/BKP reg +#define PIOS_SYSCLK 168000000 +// Peripherals that belongs to APB1 are: +// DAC |PWR |CAN1,2 +// I2C1,2,3 |UART4,5 |USART3,2 +// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 +// I2S2Ext |IWDG |WWDG +// RTC/BKP reg // TIM2,3,4,5,6,7,12,13,14 -// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) -// Default APB1 Prescaler = 4 -#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) +// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) +// Default APB1 Prescaler = 4 +#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) -// Peripherals belonging to APB2 -// SDIO |EXTI |SYSCFG |SPI1 -// ADC1,2,3 -// USART1,6 -// TIM1,8,9,10,11 +// Peripherals belonging to APB2 +// SDIO |EXTI |SYSCFG |SPI1 +// ADC1,2,3 +// USART1,6 +// TIM1,8,9,10,11 // // Default APB2 Prescaler = 2 // -#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK +#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK -//------------------------- +// ------------------------- // Interrupt Priorities -//------------------------- -#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS -#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS -#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... -#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... -//------------------------ +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------ // PIOS_RCVR // See also pios_board.c -//------------------------ +// ------------------------ #define PIOS_RCVR_MAX_DEVS 3 #define PIOS_RCVR_MAX_CHANNELS 12 #define PIOS_GCSRCVR_TIMEOUT_MS 100 #define PIOS_RFM22B_RCVR_TIMEOUT_MS 200 -//------------------------- +// ------------------------- // Receiver PPM input -//------------------------- +// ------------------------- #define PIOS_PPM_MAX_DEVS 1 #define PIOS_PPM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver PWM input -//------------------------- +// ------------------------- #define PIOS_PWM_MAX_DEVS 1 #define PIOS_PWM_NUM_INPUTS 8 -//------------------------- +// ------------------------- // Receiver SPEKTRUM input -//------------------------- +// ------------------------- #define PIOS_SPEKTRUM_MAX_DEVS 2 #define PIOS_SPEKTRUM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver S.Bus input -//------------------------- +// ------------------------- #define PIOS_SBUS_MAX_DEVS 1 -#define PIOS_SBUS_NUM_INPUTS (16+2) +#define PIOS_SBUS_NUM_INPUTS (16 + 2) -//------------------------- +// ------------------------- // Receiver DSM input -//------------------------- -#define PIOS_DSM_MAX_DEVS 2 -#define PIOS_DSM_NUM_INPUTS 12 +// ------------------------- +#define PIOS_DSM_MAX_DEVS 2 +#define PIOS_DSM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Servo outputs -//------------------------- -#define PIOS_SERVO_UPDATE_HZ 50 -#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ -//-------------------------- +// -------------------------- // Timer controller settings -//-------------------------- -#define PIOS_TIM_MAX_DEVS 6 +// -------------------------- +#define PIOS_TIM_MAX_DEVS 6 -//------------------------- +// ------------------------- // ADC // PIOS_ADC_PinGet(0) = Current sensor // PIOS_ADC_PinGet(1) = Voltage sensor // PIOS_ADC_PinGet(4) = VREF // PIOS_ADC_PinGet(5) = Temperature sensor -//------------------------- +// ------------------------- #define PIOS_DMA_PIN_CONFIG \ -{ \ - {GPIOC, GPIO_Pin_1, ADC_Channel_11}, \ - {GPIOC, GPIO_Pin_2, ADC_Channel_12}, \ - {NULL, 0, ADC_Channel_Vrefint}, /* Voltage reference */ \ - {NULL, 0, ADC_Channel_TempSensor}, /* Temperature sensor */ \ -} + { \ + { GPIOC, GPIO_Pin_1, ADC_Channel_11 }, \ + { GPIOC, GPIO_Pin_2, ADC_Channel_12 }, \ + { NULL, 0, ADC_Channel_Vrefint }, /* Voltage reference */ \ + { NULL, 0, ADC_Channel_TempSensor }, /* Temperature sensor */ \ + } /* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */ /* which is annoying because this then determines the rate at which we generate buffer turnover events */ /* the objective here is to get enough buffer space to support 100Hz averaging rate */ -#define PIOS_ADC_NUM_CHANNELS 4 -#define PIOS_ADC_MAX_OVERSAMPLING 2 -#define PIOS_ADC_USE_ADC2 0 -#define PIOS_ADC_VOLTAGE_SCALE 3.30f/4096.0f -#define PIOS_ADC_USE_TEMP_SENSOR 1 +#define PIOS_ADC_NUM_CHANNELS 4 +#define PIOS_ADC_MAX_OVERSAMPLING 2 +#define PIOS_ADC_USE_ADC2 0 +#define PIOS_ADC_VOLTAGE_SCALE 3.30f / 4096.0f +#define PIOS_ADC_USE_TEMP_SENSOR 1 -//------------------------- +// ------------------------- // USB -//------------------------- -#define PIOS_USB_MAX_DEVS 1 -#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ -#define PIOS_USB_HID_MAX_DEVS 1 +// ------------------------- +#define PIOS_USB_MAX_DEVS 1 +#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ +#define PIOS_USB_HID_MAX_DEVS 1 #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/revolution/pios_usb_board_data.c b/flight/targets/boards/revolution/pios_usb_board_data.c index df136bab4..5a097b6bc 100644 --- a/flight/targets/boards/revolution/pios_usb_board_data.c +++ b/flight/targets/boards/revolution/pios_usb_board_data.c @@ -28,71 +28,72 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ -#include /* PIOS_SYS_SerialNumberGet */ -#include /* PIOS_USBHOOK_* */ -#include /* PIOS_USB_UTIL_AsciiToUtf8 */ +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ static const uint8_t usb_product_id[22] = { - sizeof(usb_product_id), - USB_DESC_TYPE_STRING, - 'R', 0, - 'e', 0, - 'v', 0, - 'o', 0, - 'l', 0, - 'u', 0, - 't', 0, - 'i', 0, - 'o', 0, - 'n', 0, + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'R', 0, + 'e', 0, + 'v', 0, + 'o', 0, + 'l', 0, + 'u', 0, + 't', 0, + 'i', 0, + 'o', 0, + 'n', 0, }; -static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = { - sizeof(usb_serial_number), - USB_DESC_TYPE_STRING, +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, }; static const struct usb_string_langid usb_lang_id = { - .bLength = sizeof(usb_lang_id), - .bDescriptorType = USB_DESC_TYPE_STRING, - .bLangID = htousbs(USB_LANGID_ENGLISH_US), + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), }; static const uint8_t usb_vendor_id[28] = { - sizeof(usb_vendor_id), - USB_DESC_TYPE_STRING, - 'o', 0, - 'p', 0, - 'e', 0, - 'n', 0, - 'p', 0, - 'i', 0, - 'l', 0, - 'o', 0, - 't', 0, - '.', 0, - 'o', 0, - 'r', 0, - 'g', 0 + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 }; int32_t PIOS_USB_BOARD_DATA_Init(void) { - /* Load device serial number into serial number string */ - uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - PIOS_SYS_SerialNumberGet((char *)sn); + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ - uint8_t * utf8 = &(usb_serial_number[2]); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1); + PIOS_SYS_SerialNumberGet((char *)sn); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); - return 0; + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; } diff --git a/flight/targets/boards/revoproto/board_hw_defs.c b/flight/targets/boards/revoproto/board_hw_defs.c index b10ffec6f..89a8fe10e 100644 --- a/flight/targets/boards/revoproto/board_hw_defs.c +++ b/flight/targets/boards/revoproto/board_hw_defs.c @@ -9,19 +9,19 @@ * @{ * @brief Defines board specific static initializers for hardware for the Revolution board. *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -29,43 +29,43 @@ #include static const struct pios_led pios_leds[] = { - [PIOS_LED_HEARTBEAT] = { - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_ALARM] = { - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_ALARM] = { + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, }; static const struct pios_led_cfg pios_led_cfg = { - .leds = pios_leds, - .num_leds = NELEMENTS(pios_leds), + .leds = pios_leds, + .num_leds = NELEMENTS(pios_leds), }; -const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (__attribute__((unused)) uint32_t board_revision) +const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision) { - return &pios_led_cfg; + return &pios_led_cfg; } -#endif /* PIOS_INCLUDE_LED */ +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_SPI) #include @@ -76,125 +76,128 @@ void PIOS_SPI_accel_irq_handler(void); void DMA2_Stream0_IRQHandler(void) __attribute__((alias("PIOS_SPI_accel_irq_handler"))); void DMA2_Stream3_IRQHandler(void) __attribute__((alias("PIOS_SPI_accel_irq_handler"))); static const struct pios_spi_cfg pios_spi_accel_cfg = { - .regs = SPI1, - .remap = GPIO_AF_SPI1, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, - }, - .use_crc = false, - .dma = { - .irq = { - .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), - .init = { - .NVIC_IRQChannel = DMA2_Stream0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA2_Stream0, - .init = { + .regs = SPI1, + .remap = GPIO_AF_SPI1, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, + }, + .use_crc = false, + .dma = { + .irq = { + .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), + .init = { + .NVIC_IRQChannel = DMA2_Stream0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA2_Stream0, + .init = { .DMA_Channel = DMA_Channel_3, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, /* .DMA_FIFOThreshold */ .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA2_Stream3, - .init = { + }, + }, + .tx = { + .channel = DMA2_Stream3, + .init = { .DMA_Channel = DMA_Channel_3, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_FIFOMode = DMA_FIFOMode_Disable, /* .DMA_FIFOThreshold */ .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .miso = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .mosi = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .slave_count = 2, - .ssel = { { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - } }, - { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - } }, - } + }, + }, + }, + .sclk = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .miso = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .mosi = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .slave_count = 2, + .ssel = { + { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + } + }, + { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + } + }, + } }; static uint32_t pios_spi_accel_id; void PIOS_SPI_accel_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_accel_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_accel_id); } @@ -205,118 +208,120 @@ void PIOS_SPI_GYRO_irq_handler(void); void DMA1_Stream3_IRQHandler(void) __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); void DMA1_Stream4_IRQHandler(void) __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); static const struct pios_spi_cfg pios_spi_gyro_cfg = { - .regs = SPI2, - .remap = GPIO_AF_SPI2, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, - }, - .use_crc = false, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case RX) - .flags = (DMA_IT_TCIF3 | DMA_IT_TEIF3 | DMA_IT_HTIF3), - .init = { - .NVIC_IRQChannel = DMA1_Stream3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Stream3, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - //TODO: Enable FIFO - .DMA_FIFOMode = DMA_FIFOMode_Disable, + .regs = SPI2, + .remap = GPIO_AF_SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, + }, + .use_crc = false, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case RX) + .flags = (DMA_IT_TCIF3 | DMA_IT_TEIF3 | DMA_IT_HTIF3), + .init = { + .NVIC_IRQChannel = DMA1_Stream3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Stream3, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + // TODO: Enable FIFO + .DMA_FIFOMode = DMA_FIFOMode_Disable, .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA1_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI2->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, + }, + }, + .tx = { + .channel = DMA1_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .slave_count = 1, - .ssel = { { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - } }, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, }; uint32_t pios_spi_gyro_id; void PIOS_SPI_gyro_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); } @@ -328,118 +333,120 @@ void PIOS_SPI_flash_irq_handler(void); void DMA1_Stream0_IRQHandler(void) __attribute__((alias("PIOS_SPI_flash_irq_handler"))); void DMA1_Stream5_IRQHandler(void) __attribute__((alias("PIOS_SPI_flash_irq_handler"))); static const struct pios_spi_cfg pios_spi_flash_cfg = { - .regs = SPI3, - .remap = GPIO_AF_SPI3, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, - }, - .use_crc = false, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case RX) - .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), - .init = { - .NVIC_IRQChannel = DMA1_Stream0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Stream0, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - //TODO: Enable FIFO - .DMA_FIFOMode = DMA_FIFOMode_Disable, + .regs = SPI3, + .remap = GPIO_AF_SPI3, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + }, + .use_crc = false, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case RX) + .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), + .init = { + .NVIC_IRQChannel = DMA1_Stream0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Stream0, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + // TODO: Enable FIFO + .DMA_FIFOMode = DMA_FIFOMode_Disable, .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA1_Stream5, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, + }, + }, + .tx = { + .channel = DMA1_Stream5, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .miso = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .mosi = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .slave_count = 1, - .ssel = { { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - } }, + }, + }, + }, + .sclk = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .miso = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .mosi = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, }; uint32_t pios_spi_flash_id; void PIOS_SPI_flash_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_flash_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_flash_id); } #endif /* PIOS_FLASH_ON_ACCEL */ @@ -450,17 +457,17 @@ void PIOS_SPI_flash_irq_handler(void) #include "pios_flash_jedec_priv.h" static const struct flashfs_logfs_cfg flashfs_m25p_cfg = { - .fs_magic = 0x99abceef, - .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ - .arena_size = 0x00010000, /* 256 * slot size */ - .slot_size = 0x00000100, /* 256 bytes */ + .fs_magic = 0x99abceef, + .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ + .arena_size = 0x00010000, /* 256 * slot size */ + .slot_size = 0x00000100, /* 256 bytes */ - .start_offset = 0, /* start at the beginning of the chip */ - .sector_size = 0x00010000, /* 64K bytes */ - .page_size = 0x00000100, /* 256 bytes */ + .start_offset = 0, /* start at the beginning of the chip */ + .sector_size = 0x00010000, /* 64K bytes */ + .page_size = 0x00000100, /* 256 bytes */ }; -#endif /* PIOS_INCLUDE_FLASH */ +#endif /* PIOS_INCLUDE_FLASH */ #if defined(PIOS_OVERO_SPI) /* SPI3 Interface @@ -470,126 +477,125 @@ static const struct flashfs_logfs_cfg flashfs_m25p_cfg = { void PIOS_OVERO_irq_handler(void); void DMA1_Stream7_IRQHandler(void) __attribute__((alias("PIOS_OVERO_irq_handler"))); static const struct pios_overo_cfg pios_overo_cfg = { - .regs = SPI3, - .remap = GPIO_AF_SPI3, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Hard, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, - }, - .use_crc = false, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case TX) - .flags = (DMA_IT_TCIF7), - .init = { - .NVIC_IRQChannel = DMA1_Stream7_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Stream0, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_Medium, - //TODO: Enable FIFO - .DMA_FIFOMode = DMA_FIFOMode_Disable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_Single, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA1_Stream7, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_Single, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .miso = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .mosi = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .slave_count = 1, - .ssel = { { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - } }, + .regs = SPI3, + .remap = GPIO_AF_SPI3, + .init = { + .SPI_Mode = SPI_Mode_Slave, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Hard, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + }, + .use_crc = false, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case TX) + .flags = (DMA_IT_TCIF7), + .init = { + .NVIC_IRQChannel = DMA1_Stream7_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Stream0, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_Medium, + // TODO: Enable FIFO + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + .tx = { + .channel = DMA1_Stream7, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .sclk = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .miso = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .mosi = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, }; uint32_t pios_overo_id = 0; void PIOS_OVERO_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_OVERO_DMA_irq_handler(pios_overo_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_OVERO_DMA_irq_handler(pios_overo_id); } -#else +#else /* if defined(PIOS_OVERO_SPI) */ #endif /* PIOS_OVERO_SPI */ - - - #include #ifdef PIOS_INCLUDE_COM_TELEM @@ -597,45 +603,45 @@ void PIOS_OVERO_irq_handler(void) * Telemetry on main USART */ static const struct pios_usart_cfg pios_usart_telem_cfg = { - .regs = USART2, - .remap = GPIO_AF_USART2, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART2, + .remap = GPIO_AF_USART2, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_COM_TELEM */ @@ -645,45 +651,45 @@ static const struct pios_usart_cfg pios_usart_telem_cfg = { * GPS USART */ static const struct pios_usart_cfg pios_usart_gps_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART1, + .remap = GPIO_AF_USART1, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_INCLUDE_GPS */ @@ -693,45 +699,45 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = { * AUX USART (UART label on rev2) */ static const struct pios_usart_cfg pios_usart_aux_cfg = { - .regs = USART6, - .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART6, + .remap = GPIO_AF_USART6, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART6_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_COM_AUX */ @@ -741,45 +747,45 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { * AUX USART SBUS ( UART/ S Bus label on rev2) */ static const struct pios_usart_cfg pios_usart_auxsbus_cfg = { - .regs = UART4, - .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = UART4, + .remap = GPIO_AF_UART4, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = UART4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_INCLUDE_COM_AUXSBUS */ @@ -789,45 +795,45 @@ static const struct pios_usart_cfg pios_usart_auxsbus_cfg = { * FLEXI PORT */ static const struct pios_usart_cfg pios_usart_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART3, + .remap = GPIO_AF_USART3, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_INCLUDE_COM_FLEXI */ @@ -839,168 +845,168 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { #include static const struct pios_usart_cfg pios_usart_dsm_aux_cfg = { - .regs = USART6, - .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART6, + .remap = GPIO_AF_USART6, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART6_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; static const struct pios_dsm_cfg pios_dsm_aux_cfg = { - .bind = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, + .bind = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, }; static const struct pios_usart_cfg pios_usart_dsm_auxsbus_cfg = { - .regs = UART4, - .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = UART4, + .remap = GPIO_AF_UART4, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = UART4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; static const struct pios_dsm_cfg pios_dsm_auxsbus_cfg = { - .bind = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, + .bind = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, }; static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART3, + .remap = GPIO_AF_USART3, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, + .bind = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, }; -#endif /* PIOS_INCLUDE_DSM */ +#endif /* PIOS_INCLUDE_DSM */ #if defined(PIOS_INCLUDE_SBUS) /* @@ -1009,148 +1015,148 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { #include static const struct pios_usart_cfg pios_usart_sbus_auxsbus_cfg = { - .regs = UART4, - .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 100000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_Even, - .USART_StopBits = USART_StopBits_2, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, + .regs = UART4, + .remap = GPIO_AF_UART4, + .init = { + .USART_BaudRate = 100000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_Even, + .USART_StopBits = USART_StopBits_2, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = UART4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, }; static const struct pios_sbus_cfg pios_sbus_cfg = { - /* Inverter configuration */ - .inv = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .gpio_inv_enable = Bit_SET, - .gpio_inv_disable = Bit_RESET, - .gpio_clk_func = RCC_AHB1PeriphClockCmd, - .gpio_clk_periph = RCC_AHB1Periph_GPIOC, + /* Inverter configuration */ + .inv = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .gpio_inv_enable = Bit_SET, + .gpio_inv_disable = Bit_RESET, + .gpio_clk_func = RCC_AHB1PeriphClockCmd, + .gpio_clk_periph = RCC_AHB1Periph_GPIOC, }; -#endif /* PIOS_INCLUDE_SBUS */ +#endif /* PIOS_INCLUDE_SBUS */ /* * HK OSD */ static const struct pios_usart_cfg pios_usart_hkosd_auxsbus_cfg = { - .regs = UART4, - .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = UART4, + .remap = GPIO_AF_UART4, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = UART4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; static const struct pios_usart_cfg pios_usart_hkosd_aux_cfg = { - .regs = USART6, - .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART6, + .remap = GPIO_AF_USART6, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART6_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #if defined(PIOS_INCLUDE_COM) @@ -1169,215 +1175,215 @@ static const struct pios_usart_cfg pios_usart_hkosd_aux_cfg = { void PIOS_I2C_mag_adapter_ev_irq_handler(void); void PIOS_I2C_mag_adapter_er_irq_handler(void); void I2C1_EV_IRQHandler() -__attribute__ ((alias("PIOS_I2C_mag_adapter_ev_irq_handler"))); +__attribute__((alias("PIOS_I2C_mag_adapter_ev_irq_handler"))); void I2C1_ER_IRQHandler() -__attribute__ ((alias("PIOS_I2C_mag_adapter_er_irq_handler"))); +__attribute__((alias("PIOS_I2C_mag_adapter_er_irq_handler"))); static const struct pios_i2c_adapter_cfg pios_i2c_mag_adapter_cfg = { - .regs = I2C1, - .remap = GPIO_AF_I2C1, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, + .regs = I2C1, + .remap = GPIO_AF_I2C1, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C1_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C1_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; uint32_t pios_i2c_mag_adapter_id; void PIOS_I2C_mag_adapter_ev_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_mag_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_mag_adapter_id); } void PIOS_I2C_mag_adapter_er_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_mag_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_mag_adapter_id); } void PIOS_I2C_flexiport_adapter_ev_irq_handler(void); void PIOS_I2C_flexiport_adapter_er_irq_handler(void); -void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexiport_adapter_ev_irq_handler"))); -void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexiport_adapter_er_irq_handler"))); +void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_ev_irq_handler"))); +void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_er_irq_handler"))); static const struct pios_i2c_adapter_cfg pios_i2c_flexiport_adapter_cfg = { - .regs = I2C2, - .remap = GPIO_AF_I2C2, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, + .regs = I2C2, + .remap = GPIO_AF_I2C2, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; uint32_t pios_i2c_flexiport_adapter_id; void PIOS_I2C_flexiport_adapter_ev_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexiport_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexiport_adapter_id); } void PIOS_I2C_flexiport_adapter_er_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexiport_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexiport_adapter_id); } void PIOS_I2C_pressure_adapter_ev_irq_handler(void); void PIOS_I2C_pressure_adapter_er_irq_handler(void); -void I2C3_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_pressure_adapter_ev_irq_handler"))); -void I2C3_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_pressure_adapter_er_irq_handler"))); +void I2C3_EV_IRQHandler() __attribute__((alias("PIOS_I2C_pressure_adapter_ev_irq_handler"))); +void I2C3_ER_IRQHandler() __attribute__((alias("PIOS_I2C_pressure_adapter_er_irq_handler"))); static const struct pios_i2c_adapter_cfg pios_i2c_pressure_adapter_cfg = { - .regs = I2C3, - .remap = GPIO_AF_I2C3, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, + .regs = I2C3, + .remap = GPIO_AF_I2C3, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_8, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .sda = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, + }, + }, + .sda = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_9, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C3_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C3_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C3_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C3_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; uint32_t pios_i2c_pressure_adapter_id; void PIOS_I2C_pressure_adapter_ev_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_pressure_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_pressure_adapter_id); } void PIOS_I2C_pressure_adapter_er_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_pressure_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_pressure_adapter_id); } #endif /* PIOS_INCLUDE_I2C */ @@ -1387,193 +1393,192 @@ void PIOS_I2C_pressure_adapter_er_irq_handler(void) */ #include -void PIOS_RTC_IRQ_Handler (void); -void RTC_WKUP_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler"))); +void PIOS_RTC_IRQ_Handler(void); +void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); static const struct pios_rtc_cfg pios_rtc_main_cfg = { - .clksrc = RCC_RTCCLKSource_HSE_Div16, // Divide 8 Mhz crystal down to 1 - // For some reason it's acting like crystal is 16 Mhz. This clock is then divided - // by another 16 to give a nominal 62.5 khz clock - .prescaler = 100, // Every 100 cycles gives 625 Hz - .irq = { - .init = { - .NVIC_IRQChannel = RTC_WKUP_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .clksrc = RCC_RTCCLKSource_HSE_Div16, // Divide 8 Mhz crystal down to 1 + // For some reason it's acting like crystal is 16 Mhz. This clock is then divided + // by another 16 to give a nominal 62.5 khz clock + .prescaler = 100, // Every 100 cycles gives 625 Hz + .irq = { + .init = { + .NVIC_IRQChannel = RTC_WKUP_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; -void PIOS_RTC_IRQ_Handler (void) +void PIOS_RTC_IRQ_Handler(void) { - PIOS_RTC_irq_handler (); + PIOS_RTC_irq_handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_RTC) */ #include "pios_tim_priv.h" static const TIM_TimeBaseInitTypeDef tim_2_3_5_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, }; static const TIM_TimeBaseInitTypeDef tim_9_10_11_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, }; // Set up timers that only have inputs on APB2 static const TIM_TimeBaseInitTypeDef tim_1_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = 0xFFFF, + .TIM_RepetitionCounter = 0x0000, }; static const TIM_TimeBaseInitTypeDef tim_8_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = 0xFFFF, + .TIM_RepetitionCounter = 0x0000, }; // Set up timers that only have inputs on APB1 static const TIM_TimeBaseInitTypeDef tim_4_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = 0xFFFF, + .TIM_RepetitionCounter = 0x0000, }; - static const struct pios_tim_clock_cfg tim_2_cfg = { - .timer = TIM2, - .time_base_init = &tim_2_3_5_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM2, + .time_base_init = &tim_2_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_3_cfg = { - .timer = TIM3, - .time_base_init = &tim_2_3_5_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM3, + .time_base_init = &tim_2_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_5_cfg = { - .timer = TIM5, - .time_base_init = &tim_2_3_5_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM5, + .time_base_init = &tim_2_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_8_cfg = { - .timer = TIM8, - .time_base_init = &tim_8_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM8_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM8, + .time_base_init = &tim_8_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_9_cfg = { - .timer = TIM9, - .time_base_init = &tim_9_10_11_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_BRK_TIM9_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM9, + .time_base_init = &tim_9_10_11_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_BRK_TIM9_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_10_cfg = { - .timer = TIM10, - .time_base_init = &tim_9_10_11_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_UP_TIM10_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM10, + .time_base_init = &tim_9_10_11_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_UP_TIM10_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_11_cfg = { - .timer = TIM11, - .time_base_init = &tim_9_10_11_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_TRG_COM_TIM11_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM11, + .time_base_init = &tim_9_10_11_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_TRG_COM_TIM11_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_1_cfg = { - .timer = TIM1, - .time_base_init = &tim_1_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM1, + .time_base_init = &tim_1_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_4_cfg = { - .timer = TIM4, - .time_base_init = &tim_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM4, + .time_base_init = &tim_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; /** @@ -1581,183 +1586,183 @@ static const struct pios_tim_clock_cfg tim_4_cfg = { */ #include static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - { - .timer = TIM9, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource5, - }, - .remap = GPIO_AF_TIM9, - }, - { - .timer = TIM9, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource6, - }, - .remap = GPIO_AF_TIM9, - }, - { - .timer = TIM11, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource9, - }, - .remap = GPIO_AF_TIM11, - }, - { - .timer = TIM10, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource8, - }, - .remap = GPIO_AF_TIM10, - }, - { - .timer = TIM5, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource2, - }, - .remap = GPIO_AF_TIM5, - }, - { - .timer = TIM5, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource3, - }, - .remap = GPIO_AF_TIM5, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource0, - }, - .remap = GPIO_AF_TIM3, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource1, - }, - .remap = GPIO_AF_TIM3, - }, - // PB3 - TIM2 CH2 LED1 - { - .timer = TIM2, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource3, - }, - .remap = GPIO_AF_TIM2, - }, - // PB4 - TIM3 CH1 LED2 - { - .timer = TIM3, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource4, - }, - .remap = GPIO_AF_TIM3, - }, + { + .timer = TIM9, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource5, + }, + .remap = GPIO_AF_TIM9, + }, + { + .timer = TIM9, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource6, + }, + .remap = GPIO_AF_TIM9, + }, + { + .timer = TIM11, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource9, + }, + .remap = GPIO_AF_TIM11, + }, + { + .timer = TIM10, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource8, + }, + .remap = GPIO_AF_TIM10, + }, + { + .timer = TIM5, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource2, + }, + .remap = GPIO_AF_TIM5, + }, + { + .timer = TIM5, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource3, + }, + .remap = GPIO_AF_TIM5, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource0, + }, + .remap = GPIO_AF_TIM3, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource1, + }, + .remap = GPIO_AF_TIM3, + }, + // PB3 - TIM2 CH2 LED1 + { + .timer = TIM2, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource3, + }, + .remap = GPIO_AF_TIM2, + }, + // PB4 - TIM3 CH1 LED2 + { + .timer = TIM3, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource4, + }, + .remap = GPIO_AF_TIM3, + }, }; const struct pios_servo_cfg pios_servo_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_all_pins, - .num_channels = NELEMENTS(pios_tim_servoport_all_pins), + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_all_pins, + .num_channels = NELEMENTS(pios_tim_servoport_all_pins), }; @@ -1767,147 +1772,147 @@ const struct pios_servo_cfg pios_servo_cfg = { #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) #include static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource15, - }, - .remap = GPIO_AF_TIM4, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource14, - }, - .remap = GPIO_AF_TIM4, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource13, - }, - .remap = GPIO_AF_TIM4, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource12, - }, - .remap = GPIO_AF_TIM4, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource14, - }, - .remap = GPIO_AF_TIM1, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource13, - }, - .remap = GPIO_AF_TIM1, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource11, - }, - .remap = GPIO_AF_TIM1, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource9, - }, - .remap = GPIO_AF_TIM1, - }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource15, + }, + .remap = GPIO_AF_TIM4, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource14, + }, + .remap = GPIO_AF_TIM4, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource13, + }, + .remap = GPIO_AF_TIM4, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource12, + }, + .remap = GPIO_AF_TIM4, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource14, + }, + .remap = GPIO_AF_TIM1, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource13, + }, + .remap = GPIO_AF_TIM1, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource11, + }, + .remap = GPIO_AF_TIM1, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource9, + }, + .remap = GPIO_AF_TIM1, + }, }; const struct pios_pwm_cfg pios_pwm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = pios_tim_rcvrport_all_channels, - .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_rcvrport_all_channels, + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), }; -#endif +#endif /* if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) */ /* * PPM Input @@ -1915,23 +1920,23 @@ const struct pios_pwm_cfg pios_pwm_cfg = { #if defined(PIOS_INCLUDE_PPM) #include static const struct pios_ppm_cfg pios_ppm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - .TIM_Channel = TIM_Channel_2, - }, - /* Use only the first channel for ppm */ - .channels = &pios_tim_rcvrport_all_channels[0], - .num_channels = 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + .TIM_Channel = TIM_Channel_2, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_rcvrport_all_channels[0], + .num_channels = 1, }; -#endif //PPM +#endif // PPM #if defined(PIOS_INCLUDE_GCSRCVR) #include "pios_gcsrcvr_priv.h" -#endif /* PIOS_INCLUDE_GCSRCVR */ +#endif /* PIOS_INCLUDE_GCSRCVR */ #if defined(PIOS_INCLUDE_RCVR) #include "pios_rcvr_priv.h" @@ -1944,67 +1949,67 @@ static const struct pios_ppm_cfg pios_ppm_cfg = { #include static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = { -{ - .timer = TIM8, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_PuPd = GPIO_PuPd_DOWN - }, - .pin_source = GPIO_PinSource8, - }, - .remap = GPIO_AF_TIM8, -}, + { + .timer = TIM8, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_PuPd = GPIO_PuPd_DOWN + }, + .pin_source = GPIO_PinSource8, + }, + .remap = GPIO_AF_TIM8, + }, }; const struct pios_hcsr04_cfg pios_hcsr04_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = pios_tim_hcsr04_port_all_channels, - .num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels), - .trigger = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_hcsr04_port_all_channels, + .num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels), + .trigger = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }; -#endif +#endif /* if defined(PIOS_INCLUDE_HCSR04) */ #if defined(PIOS_INCLUDE_USB) #include "pios_usb_priv.h" static const struct pios_usb_cfg pios_usb_main_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = OTG_FS_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 3, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_25MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - }, + .irq = { + .init = { + .NVIC_IRQChannel = OTG_FS_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 3, + .NVIC_IRQChannelCmd = ENABLE, }, - .vsense_active_low = false + }, + .vsense = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_25MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + }, + }, + .vsense_active_low = false }; #include "pios_usb_board_data_priv.h" @@ -2012,7 +2017,7 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { #include "pios_usb_desc_hid_only_priv.h" #include "pios_usbhook.h" -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ #if defined(PIOS_INCLUDE_COM_MSG) @@ -2024,9 +2029,9 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { #include const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, + .data_if = 0, + .data_rx_ep = 1, + .data_tx_ep = 1, }; #endif /* PIOS_INCLUDE_USB_HID */ @@ -2034,11 +2039,11 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { #include const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 1, - .ctrl_tx_ep = 2, + .ctrl_if = 1, + .ctrl_tx_ep = 2, - .data_if = 2, - .data_rx_ep = 3, - .data_tx_ep = 3, + .data_if = 2, + .data_rx_ep = 3, + .data_tx_ep = 3, }; -#endif /* PIOS_INCLUDE_USB_CDC */ +#endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/targets/boards/revoproto/bootloader/inc/common.h b/flight/targets/boards/revoproto/bootloader/inc/common.h index 9010fddd9..e64b89014 100644 --- a/flight/targets/boards/revoproto/bootloader/inc/common.h +++ b/flight/targets/boards/revoproto/bootloader/inc/common.h @@ -28,10 +28,10 @@ #ifndef COMMON_H_ #define COMMON_H_ -//#include "board.h" +// #include "board.h" typedef enum { - start, keepgoing, + start, keepgoing, } DownloadAction; /**************************************************/ @@ -39,77 +39,77 @@ typedef enum { /**************************************************/ typedef enum { - DFUidle, //0 - uploading, //1 - wrong_packet_received, //2 - too_many_packets, //3 - too_few_packets, //4 - Last_operation_Success, //5 - downloading, //6 - BLidle, //7 - Last_operation_failed, //8 - uploadingStarting, //9 - outsideDevCapabilities, //10 - CRC_Fail,//11 - failed_jump, -//12 + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + BLidle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, +// 12 } DFUStates; /**************************************************/ /* OP_DFU commands */ /**************************************************/ typedef enum { - Reserved, //0 - Req_Capabilities, //1 - Rep_Capabilities, //2 - EnterDFU, //3 - JumpFW, //4 - Reset, //5 - Abort_Operation, //6 - Upload, //7 - Op_END, //8 - Download_Req, //9 - Download, //10 - Status_Request, //11 - Status_Rep -//12 + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep +// 12 } DFUCommands; typedef enum { - High_Density, Medium_Density + High_Density, Medium_Density } DeviceType; /**************************************************/ /* OP_DFU transfer types */ /**************************************************/ typedef enum { - FW, //0 - Descript -//2 + FW, // 0 + Descript +// 2 } DFUTransfer; /**************************************************/ /* OP_DFU transfer port */ /**************************************************/ typedef enum { - Usb, //0 - Serial -//2 + Usb, // 0 + Serial +// 2 } DFUPort; /**************************************************/ /* OP_DFU programable programable HW types */ /**************************************************/ typedef enum { - Self_flash, //0 - Remote_flash_via_spi -//1 + Self_flash, // 0 + Remote_flash_via_spi +// 1 } DFUProgType; /**************************************************/ /* OP_DFU programable sources */ /**************************************************/ -#define USB 0 -#define SPI 1 +#define USB 0 +#define SPI 1 -#define DownloadDelay 100000 +#define DownloadDelay 100000 -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 +#define MAX_DEL_RETRYS 3 +#define MAX_WRI_RETRYS 3 #endif /* COMMON_H_ */ diff --git a/flight/targets/boards/revoproto/bootloader/inc/pios_config.h b/flight/targets/boards/revoproto/bootloader/inc/pios_config.h index 1d121c321..cf7b48ee1 100644 --- a/flight/targets/boards/revoproto/bootloader/inc/pios_config.h +++ b/flight/targets/boards/revoproto/bootloader/inc/pios_config.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * - Central compile time config for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/revoproto/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/revoproto/bootloader/inc/pios_usb_board_data.h index eb00a55be..4e9c7c063 100644 --- a/flight/targets/boards/revoproto/bootloader/inc/pios_usb_board_data.h +++ b/flight/targets/boards/revoproto/bootloader/inc/pios_usb_board_data.h @@ -33,13 +33,13 @@ #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 2 +#define PIOS_USB_BOARD_EP_NUM 2 -#include /* struct usb_* */ +#include /* struct usb_* */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_BL) -#define PIOS_USB_BOARD_SN_SUFFIX "+BL" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" /* * The bootloader uses a simplified report structure @@ -49,4 +49,4 @@ */ #define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/revoproto/bootloader/main.c b/flight/targets/boards/revoproto/bootloader/main.c index 6d1ab5bf9..033f14a6c 100644 --- a/flight/targets/boards/revoproto/bootloader/main.c +++ b/flight/targets/boards/revoproto/bootloader/main.c @@ -8,21 +8,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief This is the file with the main function of the Revolution BootLoader * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,7 +32,7 @@ #include #include #include -#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USBHOOK_* */ #include /* Prototype of PIOS_Board_Init() function */ @@ -61,10 +61,10 @@ uint8_t tempcount = 0; /* Extern variables ----------------------------------------------------------*/ DFUStates DeviceState; -int16_t status = 0; -bool JumpToApp = false; -bool GO_dfu = false; -bool USB_connected = false; +int16_t status = 0; +bool JumpToApp = false; +bool GO_dfu = false; +bool USB_connected = false; bool User_DFU_request = false; static uint8_t mReceive_Buffer[63]; /* Private function prototypes -----------------------------------------------*/ @@ -72,146 +72,160 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); uint8_t processRX(); void jump_to_app(); -int main() { - PIOS_SYS_Init(); - PIOS_Board_Init(); - PIOS_IAP_Init(); +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); - // Make sure the brown out reset value for this chip - // is 2.7 volts - check_bor(); + // Make sure the brown out reset value for this chip + // is 2.7 volts + check_bor(); - USB_connected = PIOS_USB_CheckAvailable(0); + USB_connected = PIOS_USB_CheckAvailable(0); - if (PIOS_IAP_CheckRequest() == true) { - PIOS_DELAY_WaitmS(1000); - User_DFU_request = true; - PIOS_IAP_ClearRequest(); - } + if (PIOS_IAP_CheckRequest() == true) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = true; + PIOS_IAP_ClearRequest(); + } - GO_dfu = (USB_connected == true) || (User_DFU_request == true); + GO_dfu = (USB_connected == true) || (User_DFU_request == true); - if (GO_dfu == true) { - if (User_DFU_request == true) - DeviceState = DFUidle; - else - DeviceState = BLidle; - } else - JumpToApp = true; + if (GO_dfu == true) { + if (User_DFU_request == true) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = true; + } - uint32_t stopwatch = 0; - uint32_t prev_ticks = PIOS_DELAY_GetuS(); - while (true) { - /* Update the stopwatch */ - uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); - prev_ticks += elapsed_ticks; - stopwatch += elapsed_ticks; + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (true) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; - if (JumpToApp == true) - jump_to_app(); + if (JumpToApp == true) { + jump_to_app(); + } - switch (DeviceState) { - case Last_operation_Success: - case uploadingStarting: - case DFUidle: - period1 = 5000; - sweep_steps1 = 100; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case uploading: - period1 = 5000; - sweep_steps1 = 100; - period2 = 2500; - sweep_steps2 = 50; - break; - case downloading: - period1 = 2500; - sweep_steps1 = 50; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case BLidle: - period1 = 0; - PIOS_LED_On(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - default://error - period1 = 5000; - sweep_steps1 = 100; - period2 = 5000; - sweep_steps2 = 100; - } + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } - if (period1 != 0) { - if (LedPWM(period1, sweep_steps1, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_On(PIOS_LED_HEARTBEAT); + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } - if (period2 != 0) { - if (LedPWM(period2, sweep_steps2, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } - if (stopwatch > 50 * 1000 * 1000) - stopwatch = 0; - if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) - JumpToApp = true; + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) { + JumpToApp = true; + } - processRX(); - DataDownload(start); - } + processRX(); + DataDownload(start); + } } -void jump_to_app() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; - PIOS_LED_On(PIOS_LED_HEARTBEAT); + PIOS_LED_On(PIOS_LED_HEARTBEAT); // Look at cm3_vectors struct in startup. In a fw image the first uint32_t contains the address of the top of irqstack - uint32_t fwIrqStackBase = (*(__IO uint32_t*) bdinfo->fw_base) & 0xFFFE0000; + uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000; // Check for the two possible irqstack locations (sram or core coupled sram) - if ( fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { + if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { /* Jump to user application */ - FLASH_Lock(); - RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - PIOS_USBHOOK_Deactivate(); + PIOS_USBHOOK_Deactivate(); - JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); - Jump_To_Application = (pFunction) JumpAddress; - /* Initialize user application's Stack Pointer */ - __set_MSP(*(__IO uint32_t*) bdinfo->fw_base); - Jump_To_Application(); - } else { - DeviceState = failed_jump; - return; - } + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + return; + } } -uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { - uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ - uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ - uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ - if (curr_sweep & 1) { - pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ - } - return ((count % pwm_period) > pwm_duty) ? 1 : 0; + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; } -uint8_t processRX() { - if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { - processComand(mReceive_Buffer); - } - return true; +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return true; } /** @@ -221,14 +235,18 @@ uint8_t processRX() { */ void check_bor() { - uint8_t bor = FLASH_OB_GetBOR(); - if(bor != OB_BOR_LEVEL3) { - FLASH_OB_Unlock(); - FLASH_OB_BORConfig(OB_BOR_LEVEL3); - FLASH_OB_Launch(); - while(FLASH_WaitForLastOperation() == FLASH_BUSY); - FLASH_OB_Lock(); - while(FLASH_WaitForLastOperation() == FLASH_BUSY); - } -} + uint8_t bor = FLASH_OB_GetBOR(); + if (bor != OB_BOR_LEVEL3) { + FLASH_OB_Unlock(); + FLASH_OB_BORConfig(OB_BOR_LEVEL3); + FLASH_OB_Launch(); + while (FLASH_WaitForLastOperation() == FLASH_BUSY) { + ; + } + FLASH_OB_Lock(); + while (FLASH_WaitForLastOperation() == FLASH_BUSY) { + ; + } + } +} diff --git a/flight/targets/boards/revoproto/bootloader/pios_board.c b/flight/targets/boards/revoproto/bootloader/pios_board.c index bca178733..7e9858f18 100644 --- a/flight/targets/boards/revoproto/bootloader/pios_board.c +++ b/flight/targets/boards/revoproto/bootloader/pios_board.c @@ -7,19 +7,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,41 +39,42 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; -void PIOS_Board_Init() { - if (board_init_complete) { - return; - } +void PIOS_Board_Init() +{ + if (board_init_complete) { + return; + } + + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); - #if defined(PIOS_INCLUDE_LED) - PIOS_LED_Init(&pios_led_cfg); -#endif /* PIOS_INCLUDE_LED */ + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Activate the HID-only USB configuration */ - PIOS_USB_DESC_HID_ONLY_Init(); + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { - PIOS_Assert(0); - } -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ - PIOS_USBHOOK_Activate(); + PIOS_USBHOOK_Activate(); -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ - board_init_complete = true; + board_init_complete = true; } diff --git a/flight/targets/boards/revoproto/firmware/cm3_fault_handlers.c b/flight/targets/boards/revoproto/firmware/cm3_fault_handlers.c index e59e3b79a..e74ffa76a 100644 --- a/flight/targets/boards/revoproto/firmware/cm3_fault_handlers.c +++ b/flight/targets/boards/revoproto/firmware/cm3_fault_handlers.c @@ -14,34 +14,34 @@ # include #endif -#define FAULT_TRAMPOLINE(_vec) \ -__attribute__((naked, no_instrument_function)) \ -void \ -_vec##_Handler(void) \ -{ \ - __asm( ".syntax unified\n" \ - "MOVS R0, #4 \n" \ - "MOV R1, LR \n" \ - "TST R0, R1 \n" \ - "BEQ 1f \n" \ - "MRS R0, PSP \n" \ - "B " #_vec "_Handler2 \n" \ - "1: \n" \ - "MRS R0, MSP \n" \ - "B " #_vec "_Handler2 \n" \ - ".syntax divided\n"); \ -} \ -struct hack +#define FAULT_TRAMPOLINE(_vec) \ + __attribute__((naked, no_instrument_function)) \ + void \ + _vec##_Handler(void) \ + { \ + __asm(".syntax unified\n" \ + "MOVS R0, #4 \n" \ + "MOV R1, LR \n" \ + "TST R0, R1 \n" \ + "BEQ 1f \n" \ + "MRS R0, PSP \n" \ + "B " #_vec "_Handler2 \n" \ + "1: \n" \ + "MRS R0, MSP \n" \ + "B " #_vec "_Handler2 \n" \ + ".syntax divided\n"); \ + } \ + struct hack struct cm3_frame { - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t pc; - uint32_t psr; + uint32_t r0; + uint32_t r1; + uint32_t r2; + uint32_t r3; + uint32_t r12; + uint32_t lr; + uint32_t pc; + uint32_t psr; }; FAULT_TRAMPOLINE(HardFault); @@ -49,39 +49,42 @@ FAULT_TRAMPOLINE(BusFault); FAULT_TRAMPOLINE(UsageFault); /* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */ -#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) +#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) -void -HardFault_Handler2(struct cm3_frame *frame) +void HardFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nHARD FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(HFSR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nHARD FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(HFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } } -void -BusFault_Handler2(struct cm3_frame *frame) +void BusFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nBUS FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(BFAR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nBUS FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(BFAR)); + dbg_write_char('\n'); + for (;;) { + ; + } } -void -UsageFault_Handler2(struct cm3_frame *frame) +void UsageFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nUSAGE FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nUSAGE FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } } diff --git a/flight/targets/boards/revoproto/firmware/dcc_stdio.c b/flight/targets/boards/revoproto/firmware/dcc_stdio.c index 102dd70a1..1a522e9a0 100644 --- a/flight/targets/boards/revoproto/firmware/dcc_stdio.c +++ b/flight/targets/boards/revoproto/firmware/dcc_stdio.c @@ -1,33 +1,33 @@ /*************************************************************************** - * Copyright (C) 2008 by Dominic Rath * - * Dominic.Rath@gmx.de * - * Copyright (C) 2008 by Spencer Oliver * - * spen@spen-soft.co.uk * - * Copyright (C) 2008 by Frederik Kriewtz * - * frederik@kriewitz.eu * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* Copyright (C) 2008 by Frederik Kriewtz * +* frederik@kriewitz.eu * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ #include "inc/dcc_stdio.h" -#define TARGET_REQ_TRACEMSG 0x00 -#define TARGET_REQ_DEBUGMSG_ASCII 0x01 -#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) -#define TARGET_REQ_DEBUGCHAR 0x02 +#define TARGET_REQ_TRACEMSG 0x00 +#define TARGET_REQ_DEBUGMSG_ASCII 0x01 +#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) +#define TARGET_REQ_DEBUGCHAR 0x02 /* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel * DCRDR[7:0] is used by target for status @@ -35,115 +35,115 @@ * DCRDR[23:16] is used for by host for status * DCRDR[31:24] is used for by host for write buffer */ -#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) +#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) -#define BUSY 1 +#define BUSY 1 void dbg_write(unsigned long dcc_data) { - int len = 4; + int len = 4; - while (len--) - { - /* wait for data ready */ - while (NVIC_DBG_DATA_R & BUSY); + while (len--) { + /* wait for data ready */ + while (NVIC_DBG_DATA_R & BUSY) { + ; + } - /* write our data and set write flag - tell host there is data*/ - NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); - dcc_data >>= 8; - } + /* write our data and set write flag - tell host there is data*/ + NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); + dcc_data >>= 8; + } } void dbg_trace_point(unsigned long number) { - dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); + dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); } void dbg_write_u32(const unsigned long *val, long len) { - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); - while (len > 0) - { - dbg_write(*val); + while (len > 0) { + dbg_write(*val); - val++; - len--; - } + val++; + len--; + } } void dbg_write_u16(const unsigned short *val, long len) { - unsigned long dcc_data; + unsigned long dcc_data; - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = val[0] - | ((len > 1) ? val[1] << 16: 0x0000); + while (len > 0) { + dcc_data = val[0] + | ((len > 1) ? val[1] << 16 : 0x0000); - dbg_write(dcc_data); + dbg_write(dcc_data); - val += 2; - len -= 2; - } + val += 2; + len -= 2; + } } void dbg_write_u8(const unsigned char *val, long len) { - unsigned long dcc_data; + unsigned long dcc_data; - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = val[0] - | ((len > 1) ? val[1] << 8 : 0x00) - | ((len > 2) ? val[2] << 16 : 0x00) - | ((len > 3) ? val[3] << 24 : 0x00); + while (len > 0) { + dcc_data = val[0] + | ((len > 1) ? val[1] << 8 : 0x00) + | ((len > 2) ? val[2] << 16 : 0x00) + | ((len > 3) ? val[3] << 24 : 0x00); - dbg_write(dcc_data); + dbg_write(dcc_data); - val += 4; - len -= 4; - } + val += 4; + len -= 4; + } } void dbg_write_str(const char *msg) { - long len; - unsigned long dcc_data; + long len; + unsigned long dcc_data; - for (len = 0; msg[len] && (len < 65536); len++); + for (len = 0; msg[len] && (len < 65536); len++) { + ; + } - dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = msg[0] - | ((len > 1) ? msg[1] << 8 : 0x00) - | ((len > 2) ? msg[2] << 16 : 0x00) - | ((len > 3) ? msg[3] << 24 : 0x00); - dbg_write(dcc_data); + while (len > 0) { + dcc_data = msg[0] + | ((len > 1) ? msg[1] << 8 : 0x00) + | ((len > 2) ? msg[2] << 16 : 0x00) + | ((len > 3) ? msg[3] << 24 : 0x00); + dbg_write(dcc_data); - msg += 4; - len -= 4; - } + msg += 4; + len -= 4; + } } void dbg_write_char(char msg) { - dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); + dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); } void dbg_write_hex32(const unsigned long val) { - static const char hextab[] = { - '0', '1', '2', '3', '4', '5', '6', '7', - '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' - }; + static const char hextab[] = { + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' + }; - for (int shift = 28; shift >= 0; shift -= 4) - dbg_write_char(hextab[(val >> shift) & 0xf]); + for (int shift = 28; shift >= 0; shift -= 4) { + dbg_write_char(hextab[(val >> shift) & 0xf]); + } } diff --git a/flight/targets/boards/revoproto/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/revoproto/firmware/inc/FreeRTOSConfig.h index 023d20476..62eed5553 100644 --- a/flight/targets/boards/revoproto/firmware/inc/FreeRTOSConfig.h +++ b/flight/targets/boards/revoproto/firmware/inc/FreeRTOSConfig.h @@ -1,85 +1,84 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ /** - * @addtogroup PIOS PIOS - * @{ - * @addtogroup FreeRTOS FreeRTOS - * @{ - */ + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ /* Notes: We use 5 task priorities */ -#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... -#define configTICK_RATE_HZ ((portTickType )1000) -#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) -#define configMINIMAL_STACK_SIZE ((unsigned short)512) -#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total -#define configMAX_TASK_NAME_LEN (16) +#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total +#define configMAX_TASK_NAME_LEN (16) -#define configUSE_PREEMPTION 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configIDLE_SHOULD_YIELD 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 -#define configUSE_TIMERS 1 -#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ -#define configTIMER_QUEUE_LENGTH 10 -#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE +#define configUSE_TIMERS 1 +#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ + to exclude the API function. */ -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 1 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 1 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 /* Enable run time stats collection */ -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 /* * Once we move to CMSIS2 we can at least use: @@ -88,16 +87,17 @@ NVIC value of 255. */ * * (still nothing for the DWT registers, surprisingly) */ -#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ - do { \ - (*(unsigned long *)0xe000edfc) |= (1<<24); /* DEMCR |= DEMCR_TRCENA */ \ - (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ - } while(0) -#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ /** - * @} - */ + * @} + */ #endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/revoproto/firmware/inc/dcc_stdio.h b/flight/targets/boards/revoproto/firmware/inc/dcc_stdio.h index dbfd39342..3d7354918 100644 --- a/flight/targets/boards/revoproto/firmware/inc/dcc_stdio.h +++ b/flight/targets/boards/revoproto/firmware/inc/dcc_stdio.h @@ -1,24 +1,24 @@ /*************************************************************************** - * Copyright (C) 2008 by Dominic Rath * - * Dominic.Rath@gmx.de * - * Copyright (C) 2008 by Spencer Oliver * - * spen@spen-soft.co.uk * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ #ifndef DCC_STDIO_H #define DCC_STDIO_H @@ -33,4 +33,4 @@ void dbg_write_str(const char *msg); void dbg_write_char(char msg); void dbg_write_hex32(const unsigned long val); -#endif /* DCC_STDIO_H */ +#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/revoproto/firmware/inc/openpilot.h b/flight/targets/boards/revoproto/firmware/inc/openpilot.h index 8c76835cd..3cceef152 100644 --- a/flight/targets/boards/revoproto/firmware/inc/openpilot.h +++ b/flight/targets/boards/revoproto/firmware/inc/openpilot.h @@ -10,19 +10,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_board_sim.h b/flight/targets/boards/revoproto/firmware/inc/pios_board_sim.h index 3c960b03e..d1bcd16bc 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_board_sim.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_board_sim.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file pios_board.h + * @file pios_board.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -28,25 +28,23 @@ #define PIOS_BOARD_H - - -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -#define PIOS_LED_ALARM 0 +// ------------------------ +#define PIOS_LED_ALARM 0 #define PIOS_LED_HEARTBEAT 1 -#define PIOS_LED_NUM 2 +#define PIOS_LED_NUM 2 -//------------------------- +// ------------------------- // COM // // See also pios_board_posix.c -//------------------------- -//#define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_COM_MAX_DEVS 255 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +// ------------------------- +// #define PIOS_USART_TX_BUFFER_SIZE 256 +#define PIOS_COM_BUFFER_SIZE 1024 +#define PIOS_COM_MAX_DEVS 255 +#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE extern uint32_t pios_com_telem_rf_id; extern uint32_t pios_com_telem_usb_id; @@ -54,13 +52,13 @@ extern uint32_t pios_com_gps_id; extern uint32_t pios_com_aux_id; extern uint32_t pios_com_spectrum_id; -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_GPS (pios_com_gps_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_GPS (pios_com_gps_id) #ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_DEBUG (PIOS_COM_AUX +#define PIOS_COM_AUX (pios_com_aux_id) +#define PIOS_COM_DEBUG (PIOS_COM_AUX #endif #define PIOS_GCSRCVR_TIMEOUT_MS 200 @@ -68,18 +66,17 @@ extern uint32_t pios_com_spectrum_id; * glue macros for file IO * STM32 uses DOSFS for file IO */ -#define PIOS_FOPEN_READ(filename,file) (file=fopen((char*)filename,"r"))==NULL +#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL -#define PIOS_FOPEN_WRITE(filename,file) (file=fopen((char*)filename,"w"))==NULL +#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL -#define PIOS_FREAD(file,bufferadr,length,resultadr) (*resultadr=fread((uint8_t*)bufferadr,1,length,*file)) != length +#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length -#define PIOS_FWRITE(file,bufferadr,length,resultadr) *resultadr=fwrite((uint8_t*)bufferadr,1,length,*file) +#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) +#define PIOS_FCLOSE(file) fclose(file) -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char*)filename) +#define PIOS_FUNLINK(file) unlink((char *)filename) #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index d9cd8bff6..a212be9d7 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -141,15 +141,15 @@ /* #define PIOS_QUATERNION_STABILIZATION */ /* Performance counters */ -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 /* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 1000 -#define HEAP_LIMIT_CRITICAL 500 -#define IRQSTACK_LIMIT_WARNING 150 -#define IRQSTACK_LIMIT_CRITICAL 80 -#define CPULOAD_LIMIT_WARNING 80 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 1000 +#define HEAP_LIMIT_CRITICAL 500 +#define IRQSTACK_LIMIT_WARNING 150 +#define IRQSTACK_LIMIT_CRITICAL 80 +#define CPULOAD_LIMIT_WARNING 80 +#define CPULOAD_LIMIT_CRITICAL 95 /* Task stack sizes */ /* #define PIOS_ACTUATOR_STACK_SIZE 1020 */ @@ -160,7 +160,7 @@ /* #define PIOS_EVENTDISPATCHER_STACK_SIZE 130 */ /* This can't be too high to stop eventdispatcher thread overflowing */ -#define PIOS_EVENTDISAPTCHER_QUEUE 10 +#define PIOS_EVENTDISAPTCHER_QUEUE 10 /* Revolution series */ #define REVOLUTION diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h b/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h index 973ec4c72..56470b59b 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,7 +37,7 @@ #define PIOS_INCLUDE_FREERTOS #define PIOS_INCLUDE_TASK_MONITOR #define PIOS_INCLUDE_COM -//#define PIOS_INCLUDE_GPS +// #define PIOS_INCLUDE_GPS #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_TELEMETRY_RF #define PIOS_INCLUDE_TCP @@ -48,21 +48,21 @@ #define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_BL_HELPER -#define PIOS_RCVR_MAX_CHANNELS 12 -#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_RCVR_MAX_DEVS 3 /* Defaults for Logging */ -#define LOG_FILENAME "PIOS.LOG" -#define STARTUP_LOG_ENABLED 1 +#define LOG_FILENAME "PIOS.LOG" +#define STARTUP_LOG_ENABLED 1 /* COM Module */ -#define GPS_BAUDRATE 19200 -#define TELEM_BAUDRATE 19200 -#define AUXUART_ENABLED 0 -#define AUXUART_BAUDRATE 19200 +#define GPS_BAUDRATE 19200 +#define TELEM_BAUDRATE 19200 +#define AUXUART_ENABLED 0 +#define AUXUART_BAUDRATE 19200 -#define TELEM_QUEUE_SIZE 20 -#define PIOS_TELEM_STACK_SIZE 2048 +#define TELEM_QUEUE_SIZE 20 +#define PIOS_TELEM_STACK_SIZE 2048 /* Stabilization options */ #define PIOS_QUATERNION_STABILIZATION @@ -70,12 +70,12 @@ /* GPS options */ #define PIOS_GPS_SETS_HOMELOCATION -#define HEAP_LIMIT_WARNING 4000 -#define HEAP_LIMIT_CRITICAL 1000 -#define IRQSTACK_LIMIT_WARNING 150 -#define IRQSTACK_LIMIT_CRITICAL 80 -#define CPULOAD_LIMIT_WARNING 80 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 4000 +#define HEAP_LIMIT_CRITICAL 1000 +#define IRQSTACK_LIMIT_WARNING 150 +#define IRQSTACK_LIMIT_CRITICAL 80 +#define CPULOAD_LIMIT_WARNING 80 +#define CPULOAD_LIMIT_CRITICAL 95 #define REVOLUTION #define SIM_OSX diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/revoproto/firmware/inc/pios_usb_board_data.h index b4b42536a..2e607ad2c 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_usb_board_data.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_usb_board_data.h @@ -35,12 +35,12 @@ #define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 4 +#define PIOS_USB_BOARD_EP_NUM 4 -#include /* USB_* macros */ +#include /* USB_* macros */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_FW) -#define PIOS_USB_BOARD_SN_SUFFIX "+FW" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index 5c5a5e3b6..b20263c37 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -9,19 +9,19 @@ * @{ * @brief Defines board specific static initializers for hardware for the revolution board. *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,7 +43,7 @@ #include "../board_hw_defs.c" /** - * Sensor configurations + * Sensor configurations */ #if defined(PIOS_INCLUDE_ADC) @@ -51,77 +51,75 @@ void PIOS_ADC_DMC_irq_handler(void); void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); struct pios_adc_cfg pios_adc_cfg = { - .adc_dev = ADC1, - .dma = { - .irq = { - .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), - .init = { - .NVIC_IRQChannel = DMA2_Stream4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA2_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR - }, - } - }, - .half_flag = DMA_IT_HTIF4, - .full_flag = DMA_IT_TCIF4, - + .adc_dev = ADC1, + .dma = { + .irq = { + .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), + .init = { + .NVIC_IRQChannel = DMA2_Stream4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR + }, + } + }, + .half_flag = DMA_IT_HTIF4, + .full_flag = DMA_IT_TCIF4, }; void PIOS_ADC_DMC_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_ADC_DMA_Handler(); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_ADC_DMA_Handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_ADC) */ #if defined(PIOS_INCLUDE_HMC5883) #include "pios_hmc5883.h" static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { - .vector = PIOS_HMC5883_IRQHandler, - .line = EXTI_Line5, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line5, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_HMC5883_IRQHandler, + .line = EXTI_Line5, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line5, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { - .exti_cfg = &pios_exti_hmc5883_cfg, - .M_ODR = PIOS_HMC5883_ODR_75, - .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, - .Gain = PIOS_HMC5883_GAIN_1_9, - .Mode = PIOS_HMC5883_MODE_CONTINUOUS, - + .exti_cfg = &pios_exti_hmc5883_cfg, + .M_ODR = PIOS_HMC5883_ODR_75, + .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, + .Gain = PIOS_HMC5883_GAIN_1_9, + .Mode = PIOS_HMC5883_MODE_CONTINUOUS, }; #endif /* PIOS_INCLUDE_HMC5883 */ @@ -131,7 +129,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { #if defined(PIOS_INCLUDE_MS5611) #include "pios_ms5611.h" static const struct pios_ms5611_cfg pios_ms5611_cfg = { - .oversampling = MS5611_OSR_512, + .oversampling = MS5611_OSR_512, }; #endif /* PIOS_INCLUDE_MS5611 */ @@ -141,39 +139,39 @@ static const struct pios_ms5611_cfg pios_ms5611_cfg = { #if defined(PIOS_INCLUDE_BMA180) #include "pios_bma180.h" static const struct pios_exti_cfg pios_exti_bma180_cfg __exti_config = { - .vector = PIOS_BMA180_IRQHandler, - .line = EXTI_Line4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line4, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_BMA180_IRQHandler, + .line = EXTI_Line4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line4, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_bma180_cfg pios_bma180_cfg = { - .exti_cfg = &pios_exti_bma180_cfg, - .bandwidth = BMA_BW_600HZ, - .range = BMA_RANGE_8G, + .exti_cfg = &pios_exti_bma180_cfg, + .bandwidth = BMA_BW_600HZ, + .range = BMA_RANGE_8G, }; #endif /* PIOS_INCLUDE_BMA180 */ @@ -184,51 +182,51 @@ static const struct pios_bma180_cfg pios_bma180_cfg = { #include "pios_mpu6000.h" #include "pios_mpu6000_config.h" static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { - .vector = PIOS_MPU6000_IRQHandler, - .line = EXTI_Line8, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line8, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_MPU6000_IRQHandler, + .line = EXTI_Line8, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line8, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { - .exti_cfg = &pios_exti_mpu6000_cfg, - .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, - // Clock at 8 khz, downsampled by 12 for 666Hz - .Smpl_rate_div_no_dlp = 11, - // with dlp on output rate is 500Hz - .Smpl_rate_div_dlp = 1, - .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, - .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, - .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, - .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, - .accel_range = PIOS_MPU6000_ACCEL_8G, - .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ, - .orientation = PIOS_MPU6000_TOP_0DEG + .exti_cfg = &pios_exti_mpu6000_cfg, + .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, + // Clock at 8 khz, downsampled by 12 for 666Hz + .Smpl_rate_div_no_dlp = 11, + // with dlp on output rate is 500Hz + .Smpl_rate_div_dlp = 1, + .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, + .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, + .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_TOP_0DEG }; #endif /* PIOS_INCLUDE_MPU6000 */ @@ -238,39 +236,39 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { #if defined(PIOS_INCLUDE_L3GD20) #include "pios_l3gd20.h" static const struct pios_exti_cfg pios_exti_l3gd20_cfg __exti_config = { - .vector = PIOS_L3GD20_IRQHandler, - .line = EXTI_Line8, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line8, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_L3GD20_IRQHandler, + .line = EXTI_Line8, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line8, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_l3gd20_cfg pios_l3gd20_cfg = { - .exti_cfg = &pios_exti_l3gd20_cfg, - .range = PIOS_L3GD20_SCALE_500_DEG, + .exti_cfg = &pios_exti_l3gd20_cfg, + .range = PIOS_L3GD20_SCALE_500_DEG, }; #endif /* PIOS_INCLUDE_L3GD20 */ @@ -280,84 +278,85 @@ static const struct pios_l3gd20_cfg pios_l3gd20_cfg = { */ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 -#define PIOS_COM_GPS_RX_BUF_LEN 32 +#define PIOS_COM_GPS_RX_BUF_LEN 32 #define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 -#define PIOS_COM_AUX_RX_BUF_LEN 512 -#define PIOS_COM_AUX_TX_BUF_LEN 512 +#define PIOS_COM_AUX_RX_BUF_LEN 512 +#define PIOS_COM_AUX_TX_BUF_LEN 512 -#define PIOS_COM_HKOSD_RX_BUF_LEN 22 -#define PIOS_COM_HKOSD_TX_BUF_LEN 22 +#define PIOS_COM_HKOSD_RX_BUF_LEN 22 +#define PIOS_COM_HKOSD_TX_BUF_LEN 22 -uint32_t pios_com_aux_id = 0; -uint32_t pios_com_gps_id = 0; +uint32_t pios_com_aux_id = 0; +uint32_t pios_com_gps_id = 0; uint32_t pios_com_telem_usb_id = 0; -uint32_t pios_com_telem_rf_id = 0; -uint32_t pios_com_bridge_id = 0; -uint32_t pios_com_overo_id = 0; -uint32_t pios_com_hkosd_id = 0; +uint32_t pios_com_telem_rf_id = 0; +uint32_t pios_com_bridge_id = 0; +uint32_t pios_com_overo_id = 0; +uint32_t pios_com_hkosd_id = 0; -/* +/* * Setup a com port based on the passed cfg, driver and buffer sizes. tx size of -1 make the port rx only */ static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, size_t rx_buf_len, size_t tx_buf_len, - const struct pios_com_driver *com_driver, uint32_t *pios_com_id) + const struct pios_com_driver *com_driver, uint32_t *pios_com_id) { - uint32_t pios_usart_id; - if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { - PIOS_Assert(0); - } - - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(rx_buf_len); - PIOS_Assert(rx_buffer); - if(tx_buf_len!= (size_t)-1){ // this is the case for rx/tx ports - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(tx_buf_len); - PIOS_Assert(tx_buffer); - - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - tx_buffer, tx_buf_len)) { - PIOS_Assert(0); - } - } - else{ //rx only port - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - NULL, 0)) { - PIOS_Assert(0); - } - } + uint32_t pios_usart_id; + + if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(rx_buf_len); + PIOS_Assert(rx_buffer); + if (tx_buf_len != (size_t)-1) { // this is the case for rx/tx ports + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(tx_buf_len); + PIOS_Assert(tx_buffer); + + if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, + rx_buffer, rx_buf_len, + tx_buffer, tx_buf_len)) { + PIOS_Assert(0); + } + } else { // rx only port + if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, + rx_buffer, rx_buf_len, + NULL, 0)) { + PIOS_Assert(0); + } + } } -static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, - const struct pios_com_driver *usart_com_driver,enum pios_dsm_proto *proto, - ManualControlSettingsChannelGroupsOptions channelgroup,uint8_t *bind) +static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, + const struct pios_com_driver *usart_com_driver, enum pios_dsm_proto *proto, + ManualControlSettingsChannelGroupsOptions channelgroup, uint8_t *bind) { - uint32_t pios_usart_dsm_id; - if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, - pios_usart_dsm_id, *proto, *bind)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; + uint32_t pios_usart_dsm_id; + + if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_id; + if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, + pios_usart_dsm_id, *proto, *bind)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_rcvr_id; + if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; } /** @@ -368,557 +367,551 @@ static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm #include -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; - const struct pios_board_info * bdinfo = &pios_board_info_blob; - - /* Delay system */ - PIOS_DELAY_Init(); + /* Delay system */ + PIOS_DELAY_Init(); - PIOS_LED_Init(&pios_led_cfg); + PIOS_LED_Init(&pios_led_cfg); - /* Set up the SPI interface to the accelerometer*/ - if (PIOS_SPI_Init(&pios_spi_accel_id, &pios_spi_accel_cfg)) { - PIOS_DEBUG_Assert(0); - } - - /* Set up the SPI interface to the gyro */ - if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { - PIOS_DEBUG_Assert(0); - } + /* Set up the SPI interface to the accelerometer*/ + if (PIOS_SPI_Init(&pios_spi_accel_id, &pios_spi_accel_cfg)) { + PIOS_DEBUG_Assert(0); + } + + /* Set up the SPI interface to the gyro */ + if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { + PIOS_DEBUG_Assert(0); + } #if !defined(PIOS_FLASH_ON_ACCEL) - /* Set up the SPI interface to the flash */ - if (PIOS_SPI_Init(&pios_spi_flash_id, &pios_spi_flash_cfg)) { - PIOS_DEBUG_Assert(0); - } - /* Connect flash to the appropriate interface and configure it */ - uintptr_t flash_id; - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_id, 0)) { - PIOS_DEBUG_Assert(0); - } + /* Set up the SPI interface to the flash */ + if (PIOS_SPI_Init(&pios_spi_flash_id, &pios_spi_flash_cfg)) { + PIOS_DEBUG_Assert(0); + } + /* Connect flash to the appropriate interface and configure it */ + uintptr_t flash_id; + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_id, 0)) { + PIOS_DEBUG_Assert(0); + } #else - /* Connect flash to the appropriate interface and configure it */ - uintptr_t flash_id; - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_accel_id, 1)) { - PIOS_DEBUG_Assert(0); - } + /* Connect flash to the appropriate interface and configure it */ + uintptr_t flash_id; + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_accel_id, 1)) { + PIOS_DEBUG_Assert(0); + } #endif - uintptr_t fs_id; - if (PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id)) { - PIOS_DEBUG_Assert(0); - } + uintptr_t fs_id; + if (PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } #if defined(PIOS_INCLUDE_RTC) - PIOS_RTC_Init(&pios_rtc_main_cfg); + PIOS_RTC_Init(&pios_rtc_main_cfg); #endif - /* IAP System Setup */ - PIOS_IAP_Init(); - // check for safe mode commands from gcs - if(PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && - PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && - PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) - { - PIOS_FLASHFS_Format(fs_id); - PIOS_IAP_WriteBootCmd(0,0); - PIOS_IAP_WriteBootCmd(1,0); - PIOS_IAP_WriteBootCmd(2,0); - } + /* IAP System Setup */ + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + PIOS_FLASHFS_Format(fs_id); + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - - HwSettingsInitialize(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); - /* Initialize the alarms library */ - AlarmsInitialize(); + HwSettingsInitialize(); - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Set up pulse timers */ - PIOS_TIM_InitClock(&tim_1_cfg); - PIOS_TIM_InitClock(&tim_2_cfg); - PIOS_TIM_InitClock(&tim_3_cfg); - PIOS_TIM_InitClock(&tim_4_cfg); - PIOS_TIM_InitClock(&tim_5_cfg); - PIOS_TIM_InitClock(&tim_9_cfg); - PIOS_TIM_InitClock(&tim_10_cfg); - PIOS_TIM_InitClock(&tim_11_cfg); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); - uint16_t boot_count = PIOS_IAP_ReadBootCount(); - if (boot_count < 3) { - PIOS_IAP_WriteBootCount(++boot_count); - AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); - } else { - /* Too many failed boot attempts, force hwsettings to defaults */ - HwSettingsSetDefaults(HwSettingsHandle(), 0); - AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); - } - - - //PIOS_IAP_Init(); + /* Set up pulse timers */ + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_4_cfg); + PIOS_TIM_InitClock(&tim_5_cfg); + PIOS_TIM_InitClock(&tim_9_cfg); + PIOS_TIM_InitClock(&tim_10_cfg); + PIOS_TIM_InitClock(&tim_11_cfg); + + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } + + + // PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; + /* Flags to determine if various USB interfaces are advertised */ + bool usb_hid_present = false; + bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; + usb_cdc_present = true; #else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; #endif - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_CDC) - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); + uint8_t hwsettings_usb_vcpport; + /* Configure the USB VCP port */ + HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } + if (!usb_cdc_present) { + /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ + hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; + } - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: + switch (hwsettings_usb_vcpport) { + case HWSETTINGS_USB_VCPPORT_DISABLED: + break; + case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_telem_usb_id); -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_telem_usb_id); +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) - PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_vcp_id); -#endif /* PIOS_INCLUDE_COM */ - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ + PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_vcp_id); +#endif /* PIOS_INCLUDE_COM */ + break; + } +#endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); + /* Configure the usb HID port */ + uint8_t hwsettings_usb_hidport; + HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } + if (!usb_hid_present) { + /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ + hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; + } - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: + switch (hwsettings_usb_hidport) { + case HWSETTINGS_USB_HIDPORT_DISABLED: + break; + case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } + { + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + } -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ - if (usb_hid_present || usb_cdc_present) { - PIOS_USBHOOK_Activate(); - } -#endif /* PIOS_INCLUDE_USB */ + if (usb_hid_present || usb_cdc_present) { + PIOS_USBHOOK_Activate(); + } +#endif /* PIOS_INCLUDE_USB */ - /* Configure IO ports */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); - - /* Configure Telemetry port */ - uint8_t hwsettings_rv_telemetryport; - HwSettingsRV_TelemetryPortGet(&hwsettings_rv_telemetryport); + /* Configure IO ports */ + uint8_t hwsettings_DSMxBind; + HwSettingsDSMxBindGet(&hwsettings_DSMxBind); - switch (hwsettings_rv_telemetryport){ - case HWSETTINGS_RV_TELEMETRYPORT_DISABLED: - break; - case HWSETTINGS_RV_TELEMETRYPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RV_TELEMETRYPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - case HWSETTINGS_RV_TELEMETRYPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - - } /* hwsettings_rv_telemetryport */ + /* Configure Telemetry port */ + uint8_t hwsettings_rv_telemetryport; + HwSettingsRV_TelemetryPortGet(&hwsettings_rv_telemetryport); - /* Configure GPS port */ - uint8_t hwsettings_rv_gpsport; - HwSettingsRV_GPSPortGet(&hwsettings_rv_gpsport); - switch (hwsettings_rv_gpsport){ - case HWSETTINGS_RV_GPSPORT_DISABLED: - break; - - case HWSETTINGS_RV_GPSPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - - case HWSETTINGS_RV_GPSPORT_GPS: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); - break; - - case HWSETTINGS_RV_GPSPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - - case HWSETTINGS_RV_GPSPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - }/* hwsettings_rv_gpsport */ + switch (hwsettings_rv_telemetryport) { + case HWSETTINGS_RV_TELEMETRYPORT_DISABLED: + break; + case HWSETTINGS_RV_TELEMETRYPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); + break; + case HWSETTINGS_RV_TELEMETRYPORT_COMAUX: + PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); + break; + case HWSETTINGS_RV_TELEMETRYPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + } /* hwsettings_rv_telemetryport */ - /* Configure AUXPort */ - uint8_t hwsettings_rv_auxport; - HwSettingsRV_AuxPortGet(&hwsettings_rv_auxport); + /* Configure GPS port */ + uint8_t hwsettings_rv_gpsport; + HwSettingsRV_GPSPortGet(&hwsettings_rv_gpsport); + switch (hwsettings_rv_gpsport) { + case HWSETTINGS_RV_GPSPORT_DISABLED: + break; - switch (hwsettings_rv_auxport) { - case HWSETTINGS_RV_AUXPORT_DISABLED: - break; - - case HWSETTINGS_RV_AUXPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - - case HWSETTINGS_RV_AUXPORT_DSM2: - case HWSETTINGS_RV_AUXPORT_DSMX10BIT: - case HWSETTINGS_RV_AUXPORT_DSMX11BIT: - { - enum pios_dsm_proto proto; - switch (hwsettings_rv_auxport) { - case HWSETTINGS_RV_AUXPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_RV_AUXPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_RV_AUXPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } - //TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_aux_cfg, &pios_dsm_aux_cfg, - &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hwsettings_DSMxBind); - } - break; - case HWSETTINGS_RV_AUXPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - case HWSETTINGS_RV_AUXPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RV_AUXPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_aux_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; + case HWSETTINGS_RV_GPSPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); + break; - } /* hwsettings_rv_auxport */ - /* Configure AUXSbusPort */ - //TODO: ensure that the serial invertion pin is setted correctly - uint8_t hwsettings_rv_auxsbusport; - HwSettingsRV_AuxSBusPortGet(&hwsettings_rv_auxsbusport); - - switch (hwsettings_rv_auxsbusport) { - case HWSETTINGS_RV_AUXSBUSPORT_DISABLED: - break; - case HWSETTINGS_RV_AUXSBUSPORT_SBUS: + case HWSETTINGS_RV_GPSPORT_GPS: + PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); + break; + + case HWSETTINGS_RV_GPSPORT_COMAUX: + PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); + break; + + case HWSETTINGS_RV_GPSPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + } /* hwsettings_rv_gpsport */ + + /* Configure AUXPort */ + uint8_t hwsettings_rv_auxport; + HwSettingsRV_AuxPortGet(&hwsettings_rv_auxport); + + switch (hwsettings_rv_auxport) { + case HWSETTINGS_RV_AUXPORT_DISABLED: + break; + + case HWSETTINGS_RV_AUXPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); + break; + + case HWSETTINGS_RV_AUXPORT_DSM2: + case HWSETTINGS_RV_AUXPORT_DSMX10BIT: + case HWSETTINGS_RV_AUXPORT_DSMX11BIT: + { + enum pios_dsm_proto proto; + switch (hwsettings_rv_auxport) { + case HWSETTINGS_RV_AUXPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_RV_AUXPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_RV_AUXPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } + // TODO: Define the various Channelgroup for Revo dsm inputs and handle here + PIOS_Board_configure_dsm(&pios_usart_dsm_aux_cfg, &pios_dsm_aux_cfg, + &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); + } + break; + case HWSETTINGS_RV_AUXPORT_COMAUX: + PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); + break; + case HWSETTINGS_RV_AUXPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + case HWSETTINGS_RV_AUXPORT_OSDHK: + PIOS_Board_configure_com(&pios_usart_hkosd_aux_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); + break; + } /* hwsettings_rv_auxport */ + /* Configure AUXSbusPort */ + // TODO: ensure that the serial invertion pin is setted correctly + uint8_t hwsettings_rv_auxsbusport; + HwSettingsRV_AuxSBusPortGet(&hwsettings_rv_auxsbusport); + + switch (hwsettings_rv_auxsbusport) { + case HWSETTINGS_RV_AUXSBUSPORT_DISABLED: + break; + case HWSETTINGS_RV_AUXSBUSPORT_SBUS: #ifdef PIOS_INCLUDE_SBUS - { - uint32_t pios_usart_sbus_id; - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_auxsbus_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_id; - if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; - - } + { + uint32_t pios_usart_sbus_id; + if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_auxsbus_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_sbus_id; + if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { + PIOS_Assert(0); + } + + uint32_t pios_sbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; + } #endif /* PIOS_INCLUDE_SBUS */ - break; + break; - case HWSETTINGS_RV_AUXSBUSPORT_DSM2: - case HWSETTINGS_RV_AUXSBUSPORT_DSMX10BIT: - case HWSETTINGS_RV_AUXSBUSPORT_DSMX11BIT: - { - enum pios_dsm_proto proto; - switch (hwsettings_rv_auxsbusport) { - case HWSETTINGS_RV_AUXSBUSPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_RV_AUXSBUSPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_RV_AUXSBUSPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } - //TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_auxsbus_cfg, &pios_dsm_auxsbus_cfg, - &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hwsettings_DSMxBind); - } - break; - case HWSETTINGS_RV_AUXSBUSPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - case HWSETTINGS_RV_AUXSBUSPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RV_AUXSBUSPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_auxsbus_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } /* hwsettings_rv_auxport */ - - /* Configure FlexiPort */ + case HWSETTINGS_RV_AUXSBUSPORT_DSM2: + case HWSETTINGS_RV_AUXSBUSPORT_DSMX10BIT: + case HWSETTINGS_RV_AUXSBUSPORT_DSMX11BIT: + { + enum pios_dsm_proto proto; + switch (hwsettings_rv_auxsbusport) { + case HWSETTINGS_RV_AUXSBUSPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_RV_AUXSBUSPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_RV_AUXSBUSPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } + // TODO: Define the various Channelgroup for Revo dsm inputs and handle here + PIOS_Board_configure_dsm(&pios_usart_dsm_auxsbus_cfg, &pios_dsm_auxsbus_cfg, + &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); + } + break; + case HWSETTINGS_RV_AUXSBUSPORT_COMAUX: + PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); + break; + case HWSETTINGS_RV_AUXSBUSPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + case HWSETTINGS_RV_AUXSBUSPORT_OSDHK: + PIOS_Board_configure_com(&pios_usart_hkosd_auxsbus_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); + break; + } /* hwsettings_rv_auxport */ - uint8_t hwsettings_rv_flexiport; - HwSettingsRV_FlexiPortGet(&hwsettings_rv_flexiport); - - switch (hwsettings_rv_flexiport) { - case HWSETTINGS_RV_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_RV_FLEXIPORT_I2C: + /* Configure FlexiPort */ + + uint8_t hwsettings_rv_flexiport; + HwSettingsRV_FlexiPortGet(&hwsettings_rv_flexiport); + + switch (hwsettings_rv_flexiport) { + case HWSETTINGS_RV_FLEXIPORT_DISABLED: + break; + case HWSETTINGS_RV_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) - { - if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_I2C */ - break; - - case HWSETTINGS_RV_FLEXIPORT_DSM2: - case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: - case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: - { - enum pios_dsm_proto proto; - switch (hwsettings_rv_flexiport) { - case HWSETTINGS_RV_FLEXIPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } - //TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, - &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hwsettings_DSMxBind); - } - break; - case HWSETTINGS_RV_FLEXIPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - case HWSETTINGS_RV_FLEXIPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - } /* hwsettings_rv_flexiport */ - - - /* Configure the receiver port*/ - uint8_t hwsettings_rcvrport; - HwSettingsRV_RcvrPortGet(&hwsettings_rcvrport); - // - switch (hwsettings_rcvrport){ - case HWSETTINGS_RV_RCVRPORT_DISABLED: - break; - case HWSETTINGS_RV_RCVRPORT_PWM: + { + if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_I2C */ + break; + + case HWSETTINGS_RV_FLEXIPORT_DSM2: + case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: + case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: + { + enum pios_dsm_proto proto; + switch (hwsettings_rv_flexiport) { + case HWSETTINGS_RV_FLEXIPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } + // TODO: Define the various Channelgroup for Revo dsm inputs and handle here + PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, + &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); + } + break; + case HWSETTINGS_RV_FLEXIPORT_COMAUX: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); + break; + case HWSETTINGS_RV_FLEXIPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + } /* hwsettings_rv_flexiport */ + + + /* Configure the receiver port*/ + uint8_t hwsettings_rcvrport; + HwSettingsRV_RcvrPortGet(&hwsettings_rcvrport); + // + switch (hwsettings_rcvrport) { + case HWSETTINGS_RV_RCVRPORT_DISABLED: + break; + case HWSETTINGS_RV_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) - { - /* Set up the receiver port. Later this should be optional */ - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); - - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PWM */ - break; - case HWSETTINGS_RV_RCVRPORT_PPM: - case HWSETTINGS_RV_RCVRPORT_PPMOUTPUTS: + { + /* Set up the receiver port. Later this should be optional */ + uint32_t pios_pwm_id; + PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); + + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PWM */ + break; + case HWSETTINGS_RV_RCVRPORT_PPM: + case HWSETTINGS_RV_RCVRPORT_PPMOUTPUTS: #if defined(PIOS_INCLUDE_PPM) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PPM */ - case HWSETTINGS_RV_RCVRPORT_OUTPUTS: - - break; - } + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PPM */ + case HWSETTINGS_RV_RCVRPORT_OUTPUTS: + + break; + } #if defined(PIOS_OVERO_SPI) - /* Set up the SPI based PIOS_COM interface to the overo */ - { - HwSettingsData hwSettings; - HwSettingsGet(&hwSettings); - if(hwSettings.OptionalModules[HWSETTINGS_OPTIONALMODULES_OVERO] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { - PIOS_DEBUG_Assert(0); - } - const uint32_t PACKET_SIZE = 1024; - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PACKET_SIZE); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PACKET_SIZE); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_overo_id, &pios_overo_com_driver, pios_overo_id, - rx_buffer, PACKET_SIZE, - tx_buffer, PACKET_SIZE)) { - PIOS_Assert(0); - } - } - } + /* Set up the SPI based PIOS_COM interface to the overo */ + { + HwSettingsData hwSettings; + HwSettingsGet(&hwSettings); + if (hwSettings.OptionalModules[HWSETTINGS_OPTIONALMODULES_OVERO] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { + PIOS_DEBUG_Assert(0); + } + const uint32_t PACKET_SIZE = 1024; + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PACKET_SIZE); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PACKET_SIZE); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_overo_id, &pios_overo_com_driver, pios_overo_id, + rx_buffer, PACKET_SIZE, + tx_buffer, PACKET_SIZE)) { + PIOS_Assert(0); + } + } + } -#endif +#endif /* if defined(PIOS_OVERO_SPI) */ #if defined(PIOS_INCLUDE_HCSR04) - { - PIOS_TIM_InitClock(&tim_8_cfg); - uint32_t pios_hcsr04_id; - PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg); - } + { + PIOS_TIM_InitClock(&tim_8_cfg); + uint32_t pios_hcsr04_id; + PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg); + } #endif #if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ - -#ifndef PIOS_ENABLE_DEBUG_PINS - switch (hwsettings_rcvrport) { - case HWSETTINGS_RV_RCVRPORT_DISABLED: - case HWSETTINGS_RV_RCVRPORT_PWM: - case HWSETTINGS_RV_RCVRPORT_PPM: - /* Set up the servo outputs */ - PIOS_Servo_Init(&pios_servo_cfg); - break; - case HWSETTINGS_RV_RCVRPORT_PPMOUTPUTS: - case HWSETTINGS_RV_RCVRPORT_OUTPUTS: - //PIOS_Servo_Init(&pios_servo_rcvr_cfg); - //TODO: Prepare the configurations on board_hw_defs and handle here: - PIOS_Servo_Init(&pios_servo_cfg); - break; - } -#else - PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); -#endif - - if (PIOS_I2C_Init(&pios_i2c_mag_adapter_id, &pios_i2c_mag_adapter_cfg)) { - PIOS_DEBUG_Assert(0); - } + GCSReceiverInitialize(); + uint32_t pios_gcsrcvr_id; + PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_GCSRCVR */ - if (PIOS_I2C_Init(&pios_i2c_pressure_adapter_id, &pios_i2c_pressure_adapter_cfg)) { - PIOS_DEBUG_Assert(0); - } - - PIOS_DELAY_WaitmS(50); +#ifndef PIOS_ENABLE_DEBUG_PINS + switch (hwsettings_rcvrport) { + case HWSETTINGS_RV_RCVRPORT_DISABLED: + case HWSETTINGS_RV_RCVRPORT_PWM: + case HWSETTINGS_RV_RCVRPORT_PPM: + /* Set up the servo outputs */ + PIOS_Servo_Init(&pios_servo_cfg); + break; + case HWSETTINGS_RV_RCVRPORT_PPMOUTPUTS: + case HWSETTINGS_RV_RCVRPORT_OUTPUTS: + // PIOS_Servo_Init(&pios_servo_rcvr_cfg); + // TODO: Prepare the configurations on board_hw_defs and handle here: + PIOS_Servo_Init(&pios_servo_cfg); + break; + } +#else + PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); +#endif + + if (PIOS_I2C_Init(&pios_i2c_mag_adapter_id, &pios_i2c_mag_adapter_cfg)) { + PIOS_DEBUG_Assert(0); + } + + if (PIOS_I2C_Init(&pios_i2c_pressure_adapter_id, &pios_i2c_pressure_adapter_cfg)) { + PIOS_DEBUG_Assert(0); + } + + PIOS_DELAY_WaitmS(50); #if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); + PIOS_ADC_Init(&pios_adc_cfg); #endif #if defined(PIOS_INCLUDE_HMC5883) - PIOS_HMC5883_Init(&pios_hmc5883_cfg); -#endif - -#if defined(PIOS_INCLUDE_MS5611) - PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id); + PIOS_HMC5883_Init(&pios_hmc5883_cfg); #endif - switch(bdinfo->board_rev) { - case 0x01: +#if defined(PIOS_INCLUDE_MS5611) + PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id); +#endif + + switch (bdinfo->board_rev) { + case 0x01: #if defined(PIOS_INCLUDE_L3GD20) - PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg); - PIOS_Assert(PIOS_L3GD20_Test() == 0); + PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg); + PIOS_Assert(PIOS_L3GD20_Test() == 0); #endif #if defined(PIOS_INCLUDE_BMA180) - PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg); - PIOS_Assert(PIOS_BMA180_Test() == 0); + PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg); + PIOS_Assert(PIOS_BMA180_Test() == 0); #endif - break; - case 0x02: + break; + case 0x02: #if defined(PIOS_INCLUDE_MPU6000) - PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg); - PIOS_MPU6000_CONFIG_Configure(); + PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); + PIOS_MPU6000_CONFIG_Configure(); #endif - break; - default: - PIOS_DEBUG_Assert(0); - } - + break; + default: + PIOS_DEBUG_Assert(0); + } } /** * @} * @} */ - diff --git a/flight/targets/boards/revoproto/firmware/pios_board_sim.c b/flight/targets/boards/revoproto/firmware/pios_board_sim.c index 970e69f3f..a73beba28 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board_sim.c +++ b/flight/targets/boards/revoproto/firmware/pios_board_sim.c @@ -7,19 +7,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,30 +39,28 @@ #include #include -void Stack_Change() { -} +void Stack_Change() {} -void Stack_Change_Weak() { -} +void Stack_Change_Weak() {} const struct pios_tcp_cfg pios_tcp_telem_cfg = { - .ip = "0.0.0.0", - .port = 9000, + .ip = "0.0.0.0", + .port = 9000, }; const struct pios_udp_cfg pios_udp_telem_cfg = { - .ip = "0.0.0.0", - .port = 9000, + .ip = "0.0.0.0", + .port = 9000, }; const struct pios_tcp_cfg pios_tcp_gps_cfg = { - .ip = "0.0.0.0", - .port = 9001, + .ip = "0.0.0.0", + .port = 9001, }; const struct pios_tcp_cfg pios_tcp_debug_cfg = { - .ip = "0.0.0.0", - .port = 9002, + .ip = "0.0.0.0", + .port = 9002, }; #ifdef PIOS_COM_AUX @@ -70,42 +68,42 @@ const struct pios_tcp_cfg pios_tcp_debug_cfg = { * AUX USART */ const struct pios_tcp_cfg pios_tcp_aux_cfg = { - .ip = "0.0.0.0", - .port = 9003, + .ip = "0.0.0.0", + .port = 9003, }; #endif #define PIOS_COM_TELEM_RF_RX_BUF_LEN 192 #define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 -#define PIOS_COM_GPS_RX_BUF_LEN 96 +#define PIOS_COM_GPS_RX_BUF_LEN 96 /* * Board specific number of devices. */ /* -struct pios_udp_dev pios_udp_devs[] = { -#define PIOS_UDP_TELEM 0 - { + struct pios_udp_dev pios_udp_devs[] = { + #define PIOS_UDP_TELEM 0 + { .cfg = &pios_udp0_cfg, - }, -#define PIOS_UDP_GPS 1 - { + }, + #define PIOS_UDP_GPS 1 + { .cfg = &pios_udp1_cfg, - }, -#define PIOS_UDP_LOCAL 2 - { + }, + #define PIOS_UDP_LOCAL 2 + { .cfg = &pios_udp2_cfg, - }, -#ifdef PIOS_COM_AUX -#define PIOS_UDP_AUX 3 - { + }, + #ifdef PIOS_COM_AUX + #define PIOS_UDP_AUX 3 + { .cfg = &pios_udp3_cfg, - }, -#endif -}; + }, + #endif + }; -uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs); -*/ + uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs); + */ /* * COM devices */ @@ -129,102 +127,101 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; * initializes all the core systems on this specific hardware * called from System/openpilot.c */ -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + UAVObjectsInitializeAll(); - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - UAVObjectsInitializeAll(); + AccelsInitialize(); + BaroAltitudeInitialize(); + MagnetometerInitialize(); + GPSPositionInitialize(); + GyrosInitialize(); + GyrosBiasInitialize(); - AccelsInitialize(); - BaroAltitudeInitialize(); - MagnetometerInitialize(); - GPSPositionInitialize(); - GyrosInitialize(); - GyrosBiasInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Initialize the alarms library */ - AlarmsInitialize(); + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } - - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1 - { - uint32_t pios_tcp_telem_rf_id; - if (PIOS_TCP_Init(&pios_tcp_telem_rf_id, &pios_tcp_telem_cfg)) { - PIOS_Assert(0); - } + { + uint32_t pios_tcp_telem_rf_id; + if (PIOS_TCP_Init(&pios_tcp_telem_rf_id, &pios_tcp_telem_cfg)) { + PIOS_Assert(0); + } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_tcp_com_driver, pios_tcp_telem_rf_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_tcp_com_driver, pios_tcp_telem_rf_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 0 - { - uint32_t pios_udp_telem_rf_id; - if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) { - PIOS_Assert(0); - } - - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + { + uint32_t pios_udp_telem_rf_id; + if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ #if defined(PIOS_INCLUDE_GPS) - { - uint32_t pios_tcp_gps_id; - if (PIOS_TCP_Init(&pios_tcp_gps_id, &pios_tcp_gps_cfg)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_tcp_com_driver, pios_tcp_gps_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_GPS */ -#endif + { + uint32_t pios_tcp_gps_id; + if (PIOS_TCP_Init(&pios_tcp_gps_id, &pios_tcp_gps_cfg)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_tcp_com_driver, pios_tcp_gps_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_GPS */ +#endif /* if defined(PIOS_INCLUDE_COM) */ #if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ - + GCSReceiverInitialize(); + uint32_t pios_gcsrcvr_id; + PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_GCSRCVR */ } /** diff --git a/flight/targets/boards/revoproto/firmware/revolution.c b/flight/targets/boards/revoproto/firmware/revolution.c index b1f771ddd..860cd34e2 100644 --- a/flight/targets/boards/revoproto/firmware/revolution.c +++ b/flight/targets/boards/revoproto/firmware/revolution.c @@ -9,25 +9,25 @@ * @brief This is where the OP firmware starts. Those files also define the compile-time * options of the firmware. * @{ - * @file openpilot.c + * @file openpilot.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Sets up and runs main OpenPilot tasks. * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,12 +35,12 @@ #include /* Task Priorities */ -#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) /* Global Variables */ /* Local Variables */ -#define INCLUDE_TEST_TASKS 0 +#define INCLUDE_TEST_TASKS 0 #if INCLUDE_TEST_TASKS static uint8_t sdcard_available; #endif @@ -61,11 +61,11 @@ void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value); /* Prototype of PIOS_Board_Init() function */ extern void PIOS_Board_Init(void); extern void Stack_Change(void); -static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change"))); +static void Stack_Change_Weak() __attribute__((weakref("Stack_Change"))); /* Local Variables */ -#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority -#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive +#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority +#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive static xTaskHandle initTaskHandle; /* Function Prototypes */ @@ -75,65 +75,64 @@ static void initTask(void *parameters); extern void InitModules(void); /** -* OpenPilot Main function: -* -* Initialize PiOS
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
-* Start FreeRTOS Scheduler (vTaskStartScheduler)
-* If something goes wrong, blink LED1 and LED2 every 100ms -* -*/ + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler)
+ * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ int main() { - int result; + int result; - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ - vPortInitialiseBlocks(); + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + vPortInitialiseBlocks(); - /* Brings up System using CMSIS functions, enables the LEDs. */ - PIOS_SYS_Init(); - - /* For Revolution we use a FreeRTOS task to bring up the system so we can */ - /* always rely on FreeRTOS primitive */ - result = xTaskCreate(initTask, (const signed char *)"init", - INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, - &initTaskHandle); - PIOS_Assert(result == pdPASS); - - /* Start the FreeRTOS scheduler */ - vTaskStartScheduler(); + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); - /* If all is well we will never reach here as the scheduler will now be running. */ - /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ - PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ - for(;;) { \ - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ - PIOS_DELAY_WaitmS(100); \ - }; + /* For Revolution we use a FreeRTOS task to bring up the system so we can */ + /* always rely on FreeRTOS primitive */ + result = xTaskCreate(initTask, (const signed char *)"init", + INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, + &initTaskHandle); + PIOS_Assert(result == pdPASS); - return 0; + /* Start the FreeRTOS scheduler */ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ + PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ + for (;;) { \ + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ + PIOS_DELAY_WaitmS(100); \ + } + ; + + return 0; } /** * Initialisation task. * * Runs board and module initialisation, then terminates. */ -void -initTask(__attribute__((unused)) void *parameters) +void initTask(__attribute__((unused)) void *parameters) { - /* board driver init */ - PIOS_Board_Init(); - - /* Initialize modules */ - MODULE_INITIALISE_ALL; - - /* terminate this task */ - vTaskDelete(NULL); + /* board driver init */ + PIOS_Board_Init(); + + /* Initialize modules */ + MODULE_INITIALISE_ALL; + + /* terminate this task */ + vTaskDelete(NULL); } /** * @} * @} */ - diff --git a/flight/targets/boards/revoproto/pios_board.h b/flight/targets/boards/revoproto/pios_board.h index 0467cd1fc..7920341eb 100644 --- a/flight/targets/boards/revoproto/pios_board.h +++ b/flight/targets/boards/revoproto/pios_board.h @@ -31,26 +31,26 @@ #include -//------------------------ +// ------------------------ // Timers and Channels Used -//------------------------ +// ------------------------ /* - Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 - ------+-----------+-----------+-----------+---------- - TIM1 | | | | - TIM2 | --------------- PIOS_DELAY ----------------- - TIM3 | | | | - TIM4 | | | | - TIM5 | | | | - TIM6 | | | | - TIM7 | | | | - TIM8 | | | | - ------+-----------+-----------+-----------+---------- + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | | | | + TIM2 | --------------- PIOS_DELAY ----------------- + TIM3 | | | | + TIM4 | | | | + TIM5 | | | | + TIM6 | | | | + TIM7 | | | | + TIM8 | | | | + ------+-----------+-----------+-----------+---------- */ -//------------------------ +// ------------------------ // DMA Channels Used -//------------------------ +// ------------------------ /* Channel 1 - */ /* Channel 2 - SPI1 RX */ /* Channel 3 - SPI1 TX */ @@ -64,61 +64,61 @@ /* Channel 11 - */ /* Channel 12 - */ -//------------------------ +// ------------------------ // BOOTLOADER_SETTINGS -//------------------------ -#define BOARD_READABLE true -#define BOARD_WRITABLE true -#define MAX_DEL_RETRYS 3 +// ------------------------ +#define BOARD_READABLE true +#define BOARD_WRITABLE true +#define MAX_DEL_RETRYS 3 -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -#define PIOS_LED_HEARTBEAT 0 -#define PIOS_LED_ALARM 1 +// ------------------------ +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_LED_ALARM 1 -//------------------------ +// ------------------------ // PIOS_SPI // See also pios_board.c -//------------------------ -#define PIOS_SPI_MAX_DEVS 3 +// ------------------------ +#define PIOS_SPI_MAX_DEVS 3 -//------------------------ +// ------------------------ // PIOS_WDG -//------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -#define PIOS_WDG_REGISTER RTC_BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_SENSORS 0x0010 -#define PIOS_WDG_AUTOTUNE 0x0020 +// ------------------------ +#define PIOS_WATCHDOG_TIMEOUT 250 +#define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_SENSORS 0x0010 +#define PIOS_WDG_AUTOTUNE 0x0020 -//------------------------ +// ------------------------ // PIOS_I2C // See also pios_board.c -//------------------------ -#define PIOS_I2C_MAX_DEVS 3 +// ------------------------ +#define PIOS_I2C_MAX_DEVS 3 extern uint32_t pios_i2c_mag_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_adapter_id) +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_adapter_id) extern uint32_t pios_i2c_flexiport_adapter_id; -#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) -#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) -//------------------------- +// ------------------------- // PIOS_USART // // See also pios_board.c -//------------------------- -#define PIOS_USART_MAX_DEVS 5 +// ------------------------- +#define PIOS_USART_MAX_DEVS 5 -//------------------------- +// ------------------------- // PIOS_COM // // See also pios_board.c -//------------------------- -#define PIOS_COM_MAX_DEVS 4 +// ------------------------- +#define PIOS_COM_MAX_DEVS 4 extern uint32_t pios_com_telem_rf_id; extern uint32_t pios_com_gps_id; extern uint32_t pios_com_aux_id; @@ -126,143 +126,143 @@ extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_bridge_id; extern uint32_t pios_com_vcp_id; extern uint32_t pios_com_hkosd_id; -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_DEBUG PIOS_COM_AUX -#define PIOS_COM_OSDHK (pios_com_hkosd_id) +#define PIOS_COM_AUX (pios_com_aux_id) +#define PIOS_COM_GPS (pios_com_gps_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_BRIDGE (pios_com_bridge_id) +#define PIOS_COM_VCP (pios_com_vcp_id) +#define PIOS_COM_DEBUG PIOS_COM_AUX +#define PIOS_COM_OSDHK (pios_com_hkosd_id) -//------------------------ -// TELEMETRY -//------------------------ -#define TELEM_QUEUE_SIZE 80 -#define PIOS_TELEM_STACK_SIZE 624 +// ------------------------ +// TELEMETRY +// ------------------------ +#define TELEM_QUEUE_SIZE 80 +#define PIOS_TELEM_STACK_SIZE 624 -//------------------------- +// ------------------------- // System Settings -// +// // See also System_stm32f4xx.c -//------------------------- -//These macros are deprecated -//please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below -//#define PIOS_MASTER_CLOCK -//#define PIOS_PERIPHERAL_CLOCK -//#define PIOS_PERIPHERAL_CLOCK +// ------------------------- +// These macros are deprecated +// please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below +// #define PIOS_MASTER_CLOCK +// #define PIOS_PERIPHERAL_CLOCK +// #define PIOS_PERIPHERAL_CLOCK -#define PIOS_SYSCLK 168000000 -// Peripherals that belongs to APB1 are: -// DAC |PWR |CAN1,2 -// I2C1,2,3 |UART4,5 |USART3,2 -// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 -// I2S2Ext |IWDG |WWDG -// RTC/BKP reg +#define PIOS_SYSCLK 168000000 +// Peripherals that belongs to APB1 are: +// DAC |PWR |CAN1,2 +// I2C1,2,3 |UART4,5 |USART3,2 +// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 +// I2S2Ext |IWDG |WWDG +// RTC/BKP reg // TIM2,3,4,5,6,7,12,13,14 -// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) -// Default APB1 Prescaler = 4 -#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) +// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) +// Default APB1 Prescaler = 4 +#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) -// Peripherals belonging to APB2 -// SDIO |EXTI |SYSCFG |SPI1 -// ADC1,2,3 -// USART1,6 -// TIM1,8,9,10,11 +// Peripherals belonging to APB2 +// SDIO |EXTI |SYSCFG |SPI1 +// ADC1,2,3 +// USART1,6 +// TIM1,8,9,10,11 // // Default APB2 Prescaler = 2 // -#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK +#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK -//------------------------- +// ------------------------- // Interrupt Priorities -//------------------------- -#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS -#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS -#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... -#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... -//------------------------ +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------ // PIOS_RCVR // See also pios_board.c -//------------------------ +// ------------------------ #define PIOS_RCVR_MAX_DEVS 3 #define PIOS_RCVR_MAX_CHANNELS 12 #define PIOS_GCSRCVR_TIMEOUT_MS 100 -//------------------------- +// ------------------------- // Receiver PPM input -//------------------------- +// ------------------------- #define PIOS_PPM_MAX_DEVS 1 #define PIOS_PPM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver PWM input -//------------------------- +// ------------------------- #define PIOS_PWM_MAX_DEVS 1 #define PIOS_PWM_NUM_INPUTS 8 -//------------------------- +// ------------------------- // Receiver SPEKTRUM input -//------------------------- +// ------------------------- #define PIOS_SPEKTRUM_MAX_DEVS 2 #define PIOS_SPEKTRUM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver S.Bus input -//------------------------- +// ------------------------- #define PIOS_SBUS_MAX_DEVS 1 -#define PIOS_SBUS_NUM_INPUTS (16+2) +#define PIOS_SBUS_NUM_INPUTS (16 + 2) -//------------------------- +// ------------------------- // Receiver DSM input -//------------------------- +// ------------------------- #define PIOS_DSM_MAX_DEVS 2 #define PIOS_DSM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Servo outputs -//------------------------- -#define PIOS_SERVO_UPDATE_HZ 50 -#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ -//-------------------------- +// -------------------------- // Timer controller settings -//-------------------------- -#define PIOS_TIM_MAX_DEVS 6 +// -------------------------- +#define PIOS_TIM_MAX_DEVS 6 -//------------------------- +// ------------------------- // ADC // PIOS_ADC_PinGet(0) = Current sensor // PIOS_ADC_PinGet(1) = Voltage sensor // PIOS_ADC_PinGet(2) = VREF // PIOS_ADC_PinGet(3) = Temperature sensor // PIOS_ADC_PinGet(4) = Board Power -//------------------------- +// ------------------------- #define PIOS_DMA_PIN_CONFIG \ -{ \ - {GPIOC, GPIO_Pin_0, ADC_Channel_10}, \ - {GPIOC, GPIO_Pin_1, ADC_Channel_11}, \ - {NULL, 0, ADC_Channel_Vrefint}, /* Voltage reference */ \ - {NULL, 0, ADC_Channel_TempSensor}, /* Temperature sensor */ \ - {GPIOC, GPIO_Pin_2, ADC_Channel_12} \ -} + { \ + { GPIOC, GPIO_Pin_0, ADC_Channel_10 }, \ + { GPIOC, GPIO_Pin_1, ADC_Channel_11 }, \ + { NULL, 0, ADC_Channel_Vrefint }, /* Voltage reference */ \ + { NULL, 0, ADC_Channel_TempSensor }, /* Temperature sensor */ \ + { GPIOC, GPIO_Pin_2, ADC_Channel_12 } \ + } /* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */ /* which is annoying because this then determines the rate at which we generate buffer turnover events */ /* the objective here is to get enough buffer space to support 100Hz averaging rate */ -#define PIOS_ADC_NUM_CHANNELS 4 -#define PIOS_ADC_MAX_OVERSAMPLING 2 -#define PIOS_ADC_USE_ADC2 0 -#define PIOS_ADC_VOLTAGE_SCALE 3.30f/4096.0f -#define PIOS_ADC_USE_TEMP_SENSOR 1 +#define PIOS_ADC_NUM_CHANNELS 4 +#define PIOS_ADC_MAX_OVERSAMPLING 2 +#define PIOS_ADC_USE_ADC2 0 +#define PIOS_ADC_VOLTAGE_SCALE 3.30f / 4096.0f +#define PIOS_ADC_USE_TEMP_SENSOR 1 -//------------------------- +// ------------------------- // USB -//------------------------- -#define PIOS_USB_MAX_DEVS 1 -#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ -#define PIOS_USB_HID_MAX_DEVS 1 +// ------------------------- +#define PIOS_USB_MAX_DEVS 1 +#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ +#define PIOS_USB_HID_MAX_DEVS 1 #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/revoproto/pios_usb_board_data.c b/flight/targets/boards/revoproto/pios_usb_board_data.c index df136bab4..5a097b6bc 100644 --- a/flight/targets/boards/revoproto/pios_usb_board_data.c +++ b/flight/targets/boards/revoproto/pios_usb_board_data.c @@ -28,71 +28,72 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ -#include /* PIOS_SYS_SerialNumberGet */ -#include /* PIOS_USBHOOK_* */ -#include /* PIOS_USB_UTIL_AsciiToUtf8 */ +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ static const uint8_t usb_product_id[22] = { - sizeof(usb_product_id), - USB_DESC_TYPE_STRING, - 'R', 0, - 'e', 0, - 'v', 0, - 'o', 0, - 'l', 0, - 'u', 0, - 't', 0, - 'i', 0, - 'o', 0, - 'n', 0, + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'R', 0, + 'e', 0, + 'v', 0, + 'o', 0, + 'l', 0, + 'u', 0, + 't', 0, + 'i', 0, + 'o', 0, + 'n', 0, }; -static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = { - sizeof(usb_serial_number), - USB_DESC_TYPE_STRING, +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, }; static const struct usb_string_langid usb_lang_id = { - .bLength = sizeof(usb_lang_id), - .bDescriptorType = USB_DESC_TYPE_STRING, - .bLangID = htousbs(USB_LANGID_ENGLISH_US), + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), }; static const uint8_t usb_vendor_id[28] = { - sizeof(usb_vendor_id), - USB_DESC_TYPE_STRING, - 'o', 0, - 'p', 0, - 'e', 0, - 'n', 0, - 'p', 0, - 'i', 0, - 'l', 0, - 'o', 0, - 't', 0, - '.', 0, - 'o', 0, - 'r', 0, - 'g', 0 + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 }; int32_t PIOS_USB_BOARD_DATA_Init(void) { - /* Load device serial number into serial number string */ - uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - PIOS_SYS_SerialNumberGet((char *)sn); + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ - uint8_t * utf8 = &(usb_serial_number[2]); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1); + PIOS_SYS_SerialNumberGet((char *)sn); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); - return 0; + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; } diff --git a/flight/targets/boards/simposix/board_hw_defs.c b/flight/targets/boards/simposix/board_hw_defs.c index 0e02eba0b..9971a8d30 100644 --- a/flight/targets/boards/simposix/board_hw_defs.c +++ b/flight/targets/boards/simposix/board_hw_defs.c @@ -11,19 +11,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,8 +36,8 @@ * Telemetry on main USART */ const struct pios_udp_cfg pios_udp_telem_cfg = { - .ip = "0.0.0.0", - .port = 9000, + .ip = "0.0.0.0", + .port = 9000, }; #endif /* PIOS_COM_TELEM */ @@ -46,8 +46,8 @@ const struct pios_udp_cfg pios_udp_telem_cfg = { * GPS USART */ const struct pios_udp_cfg pios_udp_gps_cfg = { - .ip = "0.0.0.0", - .port = 9001, + .ip = "0.0.0.0", + .port = 9001, }; #endif /* PIOS_INCLUDE_GPS */ @@ -57,8 +57,8 @@ const struct pios_udp_cfg pios_udp_gps_cfg = { * AUX USART (UART label on rev2) */ const struct pios_udp_cfg pios_udp_aux_cfg = { - .ip = "0.0.0.0", - .port = 9002, + .ip = "0.0.0.0", + .port = 9002, }; #endif /* PIOS_COM_AUX */ @@ -69,4 +69,3 @@ const struct pios_udp_cfg pios_udp_aux_cfg = { #include #endif /* PIOS_INCLUDE_COM */ - diff --git a/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig.h index c2a334f41..2be6e0658 100644 --- a/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig.h +++ b/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig.h @@ -1,107 +1,103 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ -/* Notes: We use 5 task priorities */ +/* Notes: We use 5 task priorities */ -#ifdef __APPLE__ - #define COND_SIGNALING - #define CHECK_TASK_RESUMES - #define RUNNING_THREAD_MUTEX -// #define TICK_SIGNAL -// #define TICK_SIGWAIT - #define IDLE_SLEEPS +#ifdef __APPLE__ + #define COND_SIGNALING + #define CHECK_TASK_RESUMES + #define RUNNING_THREAD_MUTEX +// #define TICK_SIGNAL +// #define TICK_SIGWAIT + #define IDLE_SLEEPS - #define configUSE_PREEMPTION 1 - #define configIDLE_SHOULD_YIELD 0 + #define configUSE_PREEMPTION 1 + #define configIDLE_SHOULD_YIELD 0 #endif #ifdef __CYGWIN__ - #define COND_SIGNALING - #define CHECK_TASK_RESUMES -// #define RUNNING_THREAD_MUTEX -// #define TICK_SIGNAL - #define TICK_SIGWAIT - #define IDLE_SLEEPS + #define COND_SIGNALING + #define CHECK_TASK_RESUMES +// #define RUNNING_THREAD_MUTEX +// #define TICK_SIGNAL + #define TICK_SIGWAIT + #define IDLE_SLEEPS - #define configUSE_PREEMPTION 0 - #define configIDLE_SHOULD_YIELD 1 + #define configUSE_PREEMPTION 0 + #define configIDLE_SHOULD_YIELD 1 #endif #ifdef __linux__ - #define COND_SIGNALING - #define CHECK_TASK_RESUMES - #define RUNNING_THREAD_MUTEX -// #define TICK_SIGNAL -// #define TICK_SIGWAIT - #define IDLE_SLEEPS + #define COND_SIGNALING + #define CHECK_TASK_RESUMES + #define RUNNING_THREAD_MUTEX +// #define TICK_SIGNAL +// #define TICK_SIGWAIT + #define IDLE_SLEEPS - #define configUSE_PREEMPTION 1 - #define configIDLE_SHOULD_YIELD 0 + #define configUSE_PREEMPTION 1 + #define configIDLE_SHOULD_YIELD 0 #endif -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 ) -#define configTICK_RATE_HZ ( ( portTickType ) 1000 ) -#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) -#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 256 ) -#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 45 * 1024 ) ) -#define configMAX_TASK_NAME_LEN ( 16 ) -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)256) +#define configTOTAL_HEAP_SIZE ((size_t)(45 * 1024)) +#define configMAX_TASK_NAME_LEN (16) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ - -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 0 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 0 - + to exclude the API function. */ +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 0 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 #endif /* FREERTOS_CONFIG_H */ - diff --git a/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig_bk.h b/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig_bk.h index 72d6e288a..3792f665b 100644 --- a/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig_bk.h +++ b/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig_bk.h @@ -1,85 +1,84 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ /** - * @addtogroup PIOS PIOS - * @{ - * @addtogroup FreeRTOS FreeRTOS - * @{ - */ + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ /* Notes: We use 5 task priorities */ -#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... -#define configTICK_RATE_HZ ((portTickType )1000) -#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) -#define configMINIMAL_STACK_SIZE ((unsigned short)512) -#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total -#define configMAX_TASK_NAME_LEN (16) +#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total +#define configMAX_TASK_NAME_LEN (16) -#define configUSE_PREEMPTION 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configIDLE_SHOULD_YIELD 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 -#define configUSE_TIMERS 1 -#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ -#define configTIMER_QUEUE_LENGTH 10 -#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE +#define configUSE_TIMERS 1 +#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -//#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +// #define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ + to exclude the API function. */ -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 1 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 1 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 /* Enable run time stats collection */ -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 /* * Once we move to CMSIS2 we can at least use: @@ -88,16 +87,17 @@ NVIC value of 255. */ * * (still nothing for the DWT registers, surprisingly) */ -#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ - do { \ - (*(unsigned long *)0xe000edfc) |= (1<<24); /* DEMCR |= DEMCR_TRCENA */ \ - (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ - } while(0) -#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ /** - * @} - */ + * @} + */ #endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/simposix/firmware/inc/dcc_stdio.h b/flight/targets/boards/simposix/firmware/inc/dcc_stdio.h index dbfd39342..3d7354918 100644 --- a/flight/targets/boards/simposix/firmware/inc/dcc_stdio.h +++ b/flight/targets/boards/simposix/firmware/inc/dcc_stdio.h @@ -1,24 +1,24 @@ /*************************************************************************** - * Copyright (C) 2008 by Dominic Rath * - * Dominic.Rath@gmx.de * - * Copyright (C) 2008 by Spencer Oliver * - * spen@spen-soft.co.uk * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ #ifndef DCC_STDIO_H #define DCC_STDIO_H @@ -33,4 +33,4 @@ void dbg_write_str(const char *msg); void dbg_write_char(char msg); void dbg_write_hex32(const unsigned long val); -#endif /* DCC_STDIO_H */ +#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/simposix/firmware/inc/openpilot.h b/flight/targets/boards/simposix/firmware/inc/openpilot.h index 8c76835cd..3cceef152 100644 --- a/flight/targets/boards/simposix/firmware/inc/openpilot.h +++ b/flight/targets/boards/simposix/firmware/inc/openpilot.h @@ -10,19 +10,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h index a2ce66ba0..64575f9ca 100644 --- a/flight/targets/boards/simposix/firmware/inc/pios_config.h +++ b/flight/targets/boards/simposix/firmware/inc/pios_config.h @@ -5,28 +5,28 @@ * @addtogroup OpenPilotCore OpenPilot Core * @{ * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * In particular, pios_config.h is where you define which PiOS libraries * and features are included in the firmware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,34 +39,34 @@ #define PIOS_INCLUDE_BL_HELPER /* Enable/Disable PiOS Modules */ -//#define PIOS_INCLUDE_ADC +// #define PIOS_INCLUDE_ADC #define PIOS_INCLUDE_DELAY -//#define PIOS_INCLUDE_I2C +// #define PIOS_INCLUDE_I2C #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_LED #define PIOS_INCLUDE_SDCARD -//#define PIOS_INCLUDE_IAP +// #define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_SERVO #define PIOS_INCLUDE_SPI #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_TASK_MONITOR #define PIOS_INCLUDE_USART -//#define PIOS_INCLUDE_USB +// #define PIOS_INCLUDE_USB #define PIOS_INCLUDE_USB_HID -//#define PIOS_INCLUDE_GPIO +// #define PIOS_INCLUDE_GPIO #define PIOS_INCLUDE_EXTI #define PIOS_INCLUDE_RTC #define PIOS_INCLUDE_WDG #define PIOS_INCLUDE_UDP /* Select the sensors to include */ -//#define PIOS_INCLUDE_BMA180 -//#define PIOS_INCLUDE_HMC5883 -//#define PIOS_INCLUDE_MPU6000 -//#define PIOS_MPU6000_ACCEL -//#define PIOS_INCLUDE_L3GD20 -//#define PIOS_INCLUDE_MS5611 -//#define PIOS_INCLUDE_HCSR04 +// #define PIOS_INCLUDE_BMA180 +// #define PIOS_INCLUDE_HMC5883 +// #define PIOS_INCLUDE_MPU6000 +// #define PIOS_MPU6000_ACCEL +// #define PIOS_INCLUDE_L3GD20 +// #define PIOS_INCLUDE_MS5611 +// #define PIOS_INCLUDE_HCSR04 #define PIOS_FLASH_ON_ACCEL /* true for second revo */ #define FLASH_FREERTOS /* Com systems to include */ @@ -81,34 +81,34 @@ /* Supported receiver interfaces */ #define PIOS_INCLUDE_RCVR #define PIOS_INCLUDE_DSM -//#define PIOS_INCLUDE_SBUS +// #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_PWM -//#define PIOS_INCLUDE_GCSRCVR +// #define PIOS_INCLUDE_GCSRCVR #define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_BL_HELPER #define PIOS_INCLUDE_SETTINGS #define PIOS_INCLUDE_FLASH /* A really shitty setting saving implementation */ -//#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS +// #define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS /* Other Interfaces */ -//#define PIOS_INCLUDE_I2C_ESC +// #define PIOS_INCLUDE_I2C_ESC /* Flags that alter behaviors - mostly to lower resources for CC */ -#define PIOS_INCLUDE_INITCALL /* Include init call structures */ -#define PIOS_TELEM_PRIORITY_QUEUE /* Enable a priority queue in telemetry */ -#define PIOS_QUATERNION_STABILIZATION /* Stabilization options */ -//#define PIOS_GPS_SETS_HOMELOCATION /* GPS options */ +#define PIOS_INCLUDE_INITCALL /* Include init call structures */ +#define PIOS_TELEM_PRIORITY_QUEUE /* Enable a priority queue in telemetry */ +#define PIOS_QUATERNION_STABILIZATION /* Stabilization options */ +// #define PIOS_GPS_SETS_HOMELOCATION /* GPS options */ /* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 4000 -#define HEAP_LIMIT_CRITICAL 1000 -#define IRQSTACK_LIMIT_WARNING 150 -#define IRQSTACK_LIMIT_CRITICAL 80 -#define CPULOAD_LIMIT_WARNING 80 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 4000 +#define HEAP_LIMIT_CRITICAL 1000 +#define IRQSTACK_LIMIT_WARNING 150 +#define IRQSTACK_LIMIT_CRITICAL 80 +#define CPULOAD_LIMIT_WARNING 80 +#define CPULOAD_LIMIT_CRITICAL 95 // This actually needs calibrating #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD (8379692) diff --git a/flight/targets/boards/simposix/firmware/pios_board.c b/flight/targets/boards/simposix/firmware/pios_board.c index ce3ce5ff2..1e8c0db6a 100644 --- a/flight/targets/boards/simposix/firmware/pios_board.c +++ b/flight/targets/boards/simposix/firmware/pios_board.c @@ -11,19 +11,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -45,7 +45,7 @@ #include "../board_hw_defs.c" /** - * Sensor configurations + * Sensor configurations */ /* One slot per selectable receiver group. @@ -54,56 +54,56 @@ */ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 -#define PIOS_COM_GPS_RX_BUF_LEN 32 +#define PIOS_COM_GPS_RX_BUF_LEN 32 #define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 -#define PIOS_COM_AUX_RX_BUF_LEN 512 -#define PIOS_COM_AUX_TX_BUF_LEN 512 +#define PIOS_COM_AUX_RX_BUF_LEN 512 +#define PIOS_COM_AUX_TX_BUF_LEN 512 -uint32_t pios_com_aux_id = 0; -uint32_t pios_com_gps_id = 0; +uint32_t pios_com_aux_id = 0; +uint32_t pios_com_gps_id = 0; uint32_t pios_com_telem_usb_id = 0; -uint32_t pios_com_telem_rf_id = 0; -uint32_t pios_com_bridge_id = 0; +uint32_t pios_com_telem_rf_id = 0; +uint32_t pios_com_bridge_id = 0; -/* +/* * Setup a com port based on the passed cfg, driver and buffer sizes. tx size of -1 make the port rx only */ static void PIOS_Board_configure_com(const struct pios_udp_cfg *usart_port_cfg, size_t rx_buf_len, size_t tx_buf_len, - const struct pios_com_driver *com_driver, uint32_t *pios_com_id) + const struct pios_com_driver *com_driver, uint32_t *pios_com_id) { - uint32_t pios_usart_id; - if (PIOS_UDP_Init(&pios_usart_id, usart_port_cfg)) { - PIOS_Assert(0); - } - - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(rx_buf_len); - PIOS_Assert(rx_buffer); - if(tx_buf_len!= -1){ // this is the case for rx/tx ports - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(tx_buf_len); - PIOS_Assert(tx_buffer); - - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - tx_buffer, tx_buf_len)) { - PIOS_Assert(0); - } - } - else{ //rx only port - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - NULL, 0)) { - PIOS_Assert(0); - } - } + uint32_t pios_usart_id; + + if (PIOS_UDP_Init(&pios_usart_id, usart_port_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(rx_buf_len); + PIOS_Assert(rx_buffer); + if (tx_buf_len != -1) { // this is the case for rx/tx ports + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(tx_buf_len); + PIOS_Assert(tx_buffer); + + if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, + rx_buffer, rx_buf_len, + tx_buffer, tx_buf_len)) { + PIOS_Assert(0); + } + } else { // rx only port + if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, + rx_buffer, rx_buf_len, + NULL, 0)) { + PIOS_Assert(0); + } + } } /** @@ -111,90 +111,87 @@ static void PIOS_Board_configure_com(const struct pios_udp_cfg *usart_port_cfg, * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ -void PIOS_Board_Init(void) { - - /* Delay system */ - PIOS_DELAY_Init(); +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); - HwSettingsInitialize(); - - UAVObjectsInitializeAll(); - - /* Initialize the alarms library */ - AlarmsInitialize(); + HwSettingsInitialize(); - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } + UAVObjectsInitializeAll(); - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Configure IO ports */ - - /* Configure Telemetry port */ - uint8_t hwsettings_rv_telemetryport; - HwSettingsRV_TelemetryPortGet(&hwsettings_rv_telemetryport); + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - switch (hwsettings_rv_telemetryport){ - case HWSETTINGS_RV_TELEMETRYPORT_DISABLED: - break; - case HWSETTINGS_RV_TELEMETRYPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_udp_telem_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RV_TELEMETRYPORT_COMAUX: - PIOS_Board_configure_com(&pios_udp_telem_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); - break; - - } /* hwsettings_rv_telemetryport */ + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); - /* Configure GPS port */ - uint8_t hwsettings_rv_gpsport; - HwSettingsRV_GPSPortGet(&hwsettings_rv_gpsport); - switch (hwsettings_rv_gpsport){ - case HWSETTINGS_RV_GPSPORT_DISABLED: - break; - - case HWSETTINGS_RV_GPSPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); - break; - - case HWSETTINGS_RV_GPSPORT_GPS: - PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_udp_com_driver, &pios_com_gps_id); - break; - - case HWSETTINGS_RV_GPSPORT_COMAUX: - PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); - break; - - }/* hwsettings_rv_gpsport */ + /* Configure IO ports */ - /* Configure AUXPort */ - uint8_t hwsettings_rv_auxport; - HwSettingsRV_AuxPortGet(&hwsettings_rv_auxport); + /* Configure Telemetry port */ + uint8_t hwsettings_rv_telemetryport; + HwSettingsRV_TelemetryPortGet(&hwsettings_rv_telemetryport); - switch (hwsettings_rv_auxport) { - case HWSETTINGS_RV_AUXPORT_DISABLED: - break; - - case HWSETTINGS_RV_AUXPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_udp_aux_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); - break; - - case HWSETTINGS_RV_AUXPORT_COMAUX: - PIOS_Board_configure_com(&pios_udp_aux_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); - break; - break; - } /* hwsettings_rv_auxport */ + switch (hwsettings_rv_telemetryport) { + case HWSETTINGS_RV_TELEMETRYPORT_DISABLED: + break; + case HWSETTINGS_RV_TELEMETRYPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_udp_telem_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); + break; + case HWSETTINGS_RV_TELEMETRYPORT_COMAUX: + PIOS_Board_configure_com(&pios_udp_telem_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); + break; + } /* hwsettings_rv_telemetryport */ + + /* Configure GPS port */ + uint8_t hwsettings_rv_gpsport; + HwSettingsRV_GPSPortGet(&hwsettings_rv_gpsport); + switch (hwsettings_rv_gpsport) { + case HWSETTINGS_RV_GPSPORT_DISABLED: + break; + + case HWSETTINGS_RV_GPSPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); + break; + + case HWSETTINGS_RV_GPSPORT_GPS: + PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_udp_com_driver, &pios_com_gps_id); + break; + + case HWSETTINGS_RV_GPSPORT_COMAUX: + PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); + break; + } /* hwsettings_rv_gpsport */ + + /* Configure AUXPort */ + uint8_t hwsettings_rv_auxport; + HwSettingsRV_AuxPortGet(&hwsettings_rv_auxport); + + switch (hwsettings_rv_auxport) { + case HWSETTINGS_RV_AUXPORT_DISABLED: + break; + + case HWSETTINGS_RV_AUXPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_udp_aux_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); + break; + + case HWSETTINGS_RV_AUXPORT_COMAUX: + PIOS_Board_configure_com(&pios_udp_aux_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); + break; + break; + } /* hwsettings_rv_auxport */ } /** * @} * @} */ - diff --git a/flight/targets/boards/simposix/firmware/simposix.c b/flight/targets/boards/simposix/firmware/simposix.c index 4b6620b9f..3b51a78bd 100644 --- a/flight/targets/boards/simposix/firmware/simposix.c +++ b/flight/targets/boards/simposix/firmware/simposix.c @@ -9,25 +9,25 @@ * @brief This is where the OP firmware starts. Those files also define the compile-time * options of the firmware. * @{ - * @file openpilot.c + * @file openpilot.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Sets up and runs main OpenPilot tasks. * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,12 +36,12 @@ #include /* Task Priorities */ -#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) /* Global Variables */ /* Local Variables */ -#define INCLUDE_TEST_TASKS 0 +#define INCLUDE_TEST_TASKS 0 #if INCLUDE_TEST_TASKS static uint8_t sdcard_available; #endif @@ -63,11 +63,11 @@ void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value); /* Prototype of PIOS_Board_Init() function */ extern void PIOS_Board_Init(void); extern void Stack_Change(void); -static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change"))); +static void Stack_Change_Weak() __attribute__((weakref("Stack_Change"))); /* Local Variables */ -#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority -#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive +#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority +#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive static xTaskHandle initTaskHandle; /* Function Prototypes */ @@ -77,64 +77,63 @@ static void initTask(void *parameters); extern void InitModules(void); /** -* OpenPilot Main function: -* -* Initialize PiOS
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
-* Start FreeRTOS Scheduler (vTaskStartScheduler)
-* If something goes wrong, blink LED1 and LED2 every 100ms -* -*/ + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler)
+ * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ int main() { - int result; + int result; - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ - /* Brings up System using CMSIS functions, enables the LEDs. */ - PIOS_SYS_Init(); - - /* For Revolution we use a FreeRTOS task to bring up the system so we can */ - /* always rely on FreeRTOS primitive */ - result = xTaskCreate(initTask, (const signed char *)"init", - INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, - &initTaskHandle); - PIOS_Assert(result == pdPASS); - - /* Start the FreeRTOS scheduler */ - vTaskStartScheduler(); + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); - /* If all is well we will never reach here as the scheduler will now be running. */ - /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ - PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ - for(;;) { \ - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ - PIOS_DELAY_WaitmS(100); \ - }; + /* For Revolution we use a FreeRTOS task to bring up the system so we can */ + /* always rely on FreeRTOS primitive */ + result = xTaskCreate(initTask, (const signed char *)"init", + INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, + &initTaskHandle); + PIOS_Assert(result == pdPASS); - return 0; + /* Start the FreeRTOS scheduler */ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ + PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ + for (;;) { \ + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ + PIOS_DELAY_WaitmS(100); \ + } + ; + + return 0; } /** * Initialisation task. * * Runs board and module initialisation, then terminates. */ -void -initTask(__attribute__((unused)) void *parameters) +void initTask(__attribute__((unused)) void *parameters) { - /* board driver init */ - PIOS_Board_Init(); - - /* Initialize modules */ - MODULE_INITIALISE_ALL; - - /* terminate this task */ - vTaskDelete(NULL); + /* board driver init */ + PIOS_Board_Init(); + + /* Initialize modules */ + MODULE_INITIALISE_ALL; + + /* terminate this task */ + vTaskDelete(NULL); } /** * @} * @} */ - diff --git a/flight/targets/boards/simposix/pios_board.h b/flight/targets/boards/simposix/pios_board.h index 196124ec6..c0d837f53 100644 --- a/flight/targets/boards/simposix/pios_board.h +++ b/flight/targets/boards/simposix/pios_board.h @@ -32,34 +32,34 @@ /** * glue macros for file IO **/ -#define FILEINFO FILE* -#define PIOS_FOPEN_READ(filename,file) (file=fopen((char*)filename,"r"))==NULL -#define PIOS_FOPEN_WRITE(filename,file) (file=fopen((char*)filename,"w"))==NULL -#define PIOS_FREAD(file,bufferadr,length,resultadr) (*resultadr=fread((uint8_t*)bufferadr,1,length,*file)) != length -#define PIOS_FWRITE(file,bufferadr,length,resultadr) *resultadr=fwrite((uint8_t*)bufferadr,1,length,*file) -#define PIOS_FCLOSE(file) fclose(file) -#define PIOS_FUNLINK(file) unlink((char*)filename) +#define FILEINFO FILE * +#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL +#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL +#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length +#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) +#define PIOS_FCLOSE(file) fclose(file) +#define PIOS_FUNLINK(file) unlink((char *)filename) -//------------------------ +// ------------------------ // Timers and Channels Used -//------------------------ +// ------------------------ /* -Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 -------+-----------+-----------+-----------+---------- -TIM1 | | | | -TIM2 | --------------- PIOS_DELAY ----------------- -TIM3 | | | | -TIM4 | | | | -TIM5 | | | | -TIM6 | | | | -TIM7 | | | | -TIM8 | | | | -------+-----------+-----------+-----------+---------- -*/ + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | | | | + TIM2 | --------------- PIOS_DELAY ----------------- + TIM3 | | | | + TIM4 | | | | + TIM5 | | | | + TIM6 | | | | + TIM7 | | | | + TIM8 | | | | + ------+-----------+-----------+-----------+---------- + */ -//------------------------ +// ------------------------ // DMA Channels Used -//------------------------ +// ------------------------ /* Channel 1 - */ /* Channel 2 - SPI1 RX */ /* Channel 3 - SPI1 TX */ @@ -73,186 +73,186 @@ TIM8 | | | | /* Channel 11 - */ /* Channel 12 - */ -//------------------------ +// ------------------------ // BOOTLOADER_SETTINGS -//------------------------ -//#define BOARD_READABLE true -//#define BOARD_WRITABLE true -//#define MAX_DEL_RETRYS 3 +// ------------------------ +// #define BOARD_READABLE true +// #define BOARD_WRITABLE true +// #define MAX_DEL_RETRYS 3 -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -#define PIOS_LED_NUM 3 -#define PIOS_LED_HEARTBEAT 0 -#define PIOS_LED_ALARM 1 +// ------------------------ +#define PIOS_LED_NUM 3 +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_LED_ALARM 1 -//------------------------ +// ------------------------ // PIOS_SPI // See also pios_board.c -//------------------------ -//#define PIOS_SPI_MAX_DEVS 3 +// ------------------------ +// #define PIOS_SPI_MAX_DEVS 3 -//------------------------ +// ------------------------ // PIOS_WDG -//------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -//#define PIOS_WDG_REGISTER RTC_BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_SENSORS 0x0010 +// ------------------------ +#define PIOS_WATCHDOG_TIMEOUT 250 +// #define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_SENSORS 0x0010 -//------------------------ +// ------------------------ // PIOS_I2C // See also pios_board.c -//------------------------ -//#define PIOS_I2C_MAX_DEVS 3 -//extern uint32_t pios_i2c_mag_adapter_id; -//#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_adapter_id) +// ------------------------ +// #define PIOS_I2C_MAX_DEVS 3 +// extern uint32_t pios_i2c_mag_adapter_id; +// #define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_adapter_id) -//------------------------- +// ------------------------- // PIOS_USART // // See also pios_board.c -//------------------------- -//#define PIOS_USART_MAX_DEVS 5 +// ------------------------- +// #define PIOS_USART_MAX_DEVS 5 -//------------------------- +// ------------------------- // PIOS_COM // // See also pios_board.c -//------------------------- -#define PIOS_COM_MAX_DEVS 25 +// ------------------------- +#define PIOS_COM_MAX_DEVS 25 extern uint32_t pios_com_telem_rf_id; extern uint32_t pios_com_gps_id; extern uint32_t pios_com_aux_id; extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_bridge_id; extern uint32_t pios_com_vcp_id; -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_DEBUG PIOS_COM_AUX +#define PIOS_COM_AUX (pios_com_aux_id) +#define PIOS_COM_GPS (pios_com_gps_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_BRIDGE (pios_com_bridge_id) +#define PIOS_COM_VCP (pios_com_vcp_id) +#define PIOS_COM_DEBUG PIOS_COM_AUX -//------------------------ -// TELEMETRY -//------------------------ -#define TELEM_QUEUE_SIZE 20 -#define PIOS_TELEM_STACK_SIZE 624 +// ------------------------ +// TELEMETRY +// ------------------------ +#define TELEM_QUEUE_SIZE 20 +#define PIOS_TELEM_STACK_SIZE 624 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_UDP_TX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +#define PIOS_COM_BUFFER_SIZE 1024 +#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +#define PIOS_UDP_TX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -//------------------------- +// ------------------------- // System Settings -// +// // See also System_stm32f4xx.c -//------------------------- -//These macros are deprecated -//please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below -//#define PIOS_MASTER_CLOCK -//#define PIOS_PERIPHERAL_CLOCK -//#define PIOS_PERIPHERAL_CLOCK +// ------------------------- +// These macros are deprecated +// please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below +// #define PIOS_MASTER_CLOCK +// #define PIOS_PERIPHERAL_CLOCK +// #define PIOS_PERIPHERAL_CLOCK -#define PIOS_SYSCLK 168000000 -// Peripherals that belongs to APB1 are: -// DAC |PWR |CAN1,2 -// I2C1,2,3 |UART4,5 |USART3,2 -// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 -// I2S2Ext |IWDG |WWDG -// RTC/BKP reg +#define PIOS_SYSCLK 168000000 +// Peripherals that belongs to APB1 are: +// DAC |PWR |CAN1,2 +// I2C1,2,3 |UART4,5 |USART3,2 +// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 +// I2S2Ext |IWDG |WWDG +// RTC/BKP reg // TIM2,3,4,5,6,7,12,13,14 -// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) -// Default APB1 Prescaler = 4 -//#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) +// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) +// Default APB1 Prescaler = 4 +// #define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) -// Peripherals belonging to APB2 -// SDIO |EXTI |SYSCFG |SPI1 -// ADC1,2,3 -// USART1,6 -// TIM1,8,9,10,11 +// Peripherals belonging to APB2 +// SDIO |EXTI |SYSCFG |SPI1 +// ADC1,2,3 +// USART1,6 +// TIM1,8,9,10,11 // // Default APB2 Prescaler = 2 // -//#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK +// #define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK -//------------------------- +// ------------------------- // Interrupt Priorities -//------------------------- -//#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS -//#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS -//#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... -//#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------- +// #define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +// #define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +// #define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +// #define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... -//------------------------ +// ------------------------ // PIOS_RCVR // See also pios_board.c -//------------------------ -#define PIOS_RCVR_MAX_DEVS 3 -#define PIOS_RCVR_MAX_CHANNELS 12 +// ------------------------ +#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 -//------------------------- +// ------------------------- // Receiver PPM input -//------------------------- -//#define PIOS_PPM_MAX_DEVS 1 -//#define PIOS_PPM_NUM_INPUTS 12 +// ------------------------- +// #define PIOS_PPM_MAX_DEVS 1 +// #define PIOS_PPM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver PWM input -//------------------------- -//#define PIOS_PWM_MAX_DEVS 1 -//#define PIOS_PWM_NUM_INPUTS 8 +// ------------------------- +// #define PIOS_PWM_MAX_DEVS 1 +// #define PIOS_PWM_NUM_INPUTS 8 -//------------------------- +// ------------------------- // Receiver SPEKTRUM input -//------------------------- -//#define PIOS_SPEKTRUM_MAX_DEVS 2 -//#define PIOS_SPEKTRUM_NUM_INPUTS 12 +// ------------------------- +// #define PIOS_SPEKTRUM_MAX_DEVS 2 +// #define PIOS_SPEKTRUM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver S.Bus input -//------------------------- -//#define PIOS_SBUS_MAX_DEVS 1 -//#define PIOS_SBUS_NUM_INPUTS (16+2) +// ------------------------- +// #define PIOS_SBUS_MAX_DEVS 1 +// #define PIOS_SBUS_NUM_INPUTS (16+2) -//------------------------- +// ------------------------- // Receiver DSM input -//------------------------- -//#define PIOS_DSM_MAX_DEVS 2 -//#define PIOS_DSM_NUM_INPUTS 12 +// ------------------------- +// #define PIOS_DSM_MAX_DEVS 2 +// #define PIOS_DSM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Servo outputs -//------------------------- -//#define PIOS_SERVO_UPDATE_HZ 50 -//#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +// ------------------------- +// #define PIOS_SERVO_UPDATE_HZ 50 +// #define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ #define PIOS_SERVO_NUM_OUTPUTS 8 -#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS -//-------------------------- +#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS +// -------------------------- // Timer controller settings -//-------------------------- -//#define PIOS_TIM_MAX_DEVS 6 +// -------------------------- +// #define PIOS_TIM_MAX_DEVS 6 -//------------------------- +// ------------------------- // ADC // None. -//------------------------- +// ------------------------- -//------------------------- +// ------------------------- // USB -//------------------------- -//#define PIOS_USB_MAX_DEVS 1 -//#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ -//#define PIOS_USB_HID_MAX_DEVS 1 +// ------------------------- +// #define PIOS_USB_MAX_DEVS 1 +// #define PIOS_USB_ENABLED 1 /* Should remove all references to this */ +// #define PIOS_USB_HID_MAX_DEVS 1 #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/common/bootloader_updater/inc/pios_config.h b/flight/targets/common/bootloader_updater/inc/pios_config.h index 9fbd71501..8f54f402b 100644 --- a/flight/targets/common/bootloader_updater/inc/pios_config.h +++ b/flight/targets/common/bootloader_updater/inc/pios_config.h @@ -2,28 +2,28 @@ ****************************************************************************** * @addtogroup OpenPilotBL OpenPilot BootLoader * @{ - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * In particular, pios_config.h is where you define which PiOS libraries * and features are included in the firmware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/common/bootloader_updater/main.c b/flight/targets/common/bootloader_updater/main.c index 4a55a7aed..385bbdd3d 100644 --- a/flight/targets/common/bootloader_updater/main.c +++ b/flight/targets/common/bootloader_updater/main.c @@ -8,21 +8,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief This is the file with the main function of the OpenPilot BootLoader * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,13 +44,12 @@ void error(int, int); extern uint32_t _binary_start; extern uint32_t _binary_end; extern uint32_t _binary_size; -const uint32_t *embedded_image_start = (uint32_t *) &(_binary_start); -const uint32_t *embedded_image_end = (uint32_t *) &(_binary_end); -const uint32_t embedded_image_size = (uint32_t) &(_binary_size); +const uint32_t *embedded_image_start = (uint32_t *)&(_binary_start); +const uint32_t *embedded_image_end = (uint32_t *)&(_binary_end); +const uint32_t embedded_image_size = (uint32_t)&(_binary_size); int main() { - PIOS_SYS_Init(); PIOS_Board_Init(); PIOS_LED_On(PIOS_LED_HEARTBEAT); @@ -58,9 +57,10 @@ int main() PIOS_LED_Off(PIOS_LED_HEARTBEAT); /// Self overwrite check - uint32_t base_address = SCB ->VTOR; - if ((BL_BANK_BASE + embedded_image_size) > base_address) + uint32_t base_address = SCB->VTOR; + if ((BL_BANK_BASE + embedded_image_size) > base_address) { error(PIOS_LED_HEARTBEAT, 1); + } /// /* @@ -72,10 +72,10 @@ int main() */ /* Calculate how far the board_info_blob is from the beginning of the bootloader */ - uint32_t board_info_blob_offset = (uint32_t) &pios_board_info_blob - (uint32_t) BL_BANK_BASE; + uint32_t board_info_blob_offset = (uint32_t)&pios_board_info_blob - (uint32_t)BL_BANK_BASE; /* Use the same offset into our embedded bootloader image */ - struct pios_board_info *new_board_info_blob = (struct pios_board_info *) ((uint32_t) embedded_image_start + board_info_blob_offset); + struct pios_board_info *new_board_info_blob = (struct pios_board_info *)((uint32_t)embedded_image_start + board_info_blob_offset); /* Compare the two board info blobs to make sure they're for the same HW revision */ if ((pios_board_info_blob.magic != new_board_info_blob->magic) || @@ -92,7 +92,7 @@ int main() fail = (PIOS_BL_HELPER_FLASH_Erase_Bootloader() != 1); - if (fail == true){ + if (fail == true) { error(PIOS_LED_HEARTBEAT, 3); } /// @@ -125,19 +125,18 @@ int main() for (;;) { PIOS_DELAY_WaitmS(1000); } - } void error(int led, int code) { - for (;;) { - PIOS_DELAY_WaitmS(1000); - for (int x = 0; x < code; x++) { - PIOS_LED_On(led); - PIOS_DELAY_WaitmS(200); - PIOS_LED_Off(led); - PIOS_DELAY_WaitmS(1000); - } - PIOS_DELAY_WaitmS(3000); - } + for (;;) { + PIOS_DELAY_WaitmS(1000); + for (int x = 0; x < code; x++) { + PIOS_LED_On(led); + PIOS_DELAY_WaitmS(200); + PIOS_LED_Off(led); + PIOS_DELAY_WaitmS(1000); + } + PIOS_DELAY_WaitmS(3000); + } } diff --git a/flight/targets/common/bootloader_updater/pios_board.c b/flight/targets/common/bootloader_updater/pios_board.c index accc300f9..8ef335e3f 100644 --- a/flight/targets/common/bootloader_updater/pios_board.c +++ b/flight/targets/common/bootloader_updater/pios_board.c @@ -35,19 +35,19 @@ */ #include -void PIOS_Board_Init(void) { - const struct pios_board_info * bdinfo = &pios_board_info_blob; +void PIOS_Board_Init(void) +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; - /* LEDs */ + /* LEDs */ #if defined(PIOS_INCLUDE_LED) - const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); - PIOS_Assert(led_cfg); - PIOS_LED_Init(led_cfg); -#endif /* PIOS_INCLUDE_LED */ + const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ - /* Initialize the PiOS library */ + /* Initialize the PiOS library */ #if defined(PIOS_INCLUDE_GPIO) - PIOS_GPIO_Init(); -#endif /* PIOS_INCLUDE_GPIO */ - + PIOS_GPIO_Init(); +#endif /* PIOS_INCLUDE_GPIO */ } diff --git a/flight/tests/logfs/openpilot.h b/flight/tests/logfs/openpilot.h index 69f7874da..4c2259d19 100644 --- a/flight/tests/logfs/openpilot.h +++ b/flight/tests/logfs/openpilot.h @@ -3,7 +3,9 @@ #include -#define PIOS_Assert(x) if (!(x)) { while (1) ; } +#define PIOS_Assert(x) \ + if (!(x)) { while (1) {; } \ + } #define PIOS_DEBUG_Assert(x) PIOS_Assert(x) #endif /* OPENPILOT_H */ diff --git a/flight/tests/logfs/pios_flash_ut.c b/flight/tests/logfs/pios_flash_ut.c index 32a749783..6662d00f4 100644 --- a/flight/tests/logfs/pios_flash_ut.c +++ b/flight/tests/logfs/pios_flash_ut.c @@ -1,58 +1,58 @@ -#include /* abort */ -#include /* fopen/fread/fwrite/fseek */ -#include /* assert */ -#include /* memset */ +#include /* abort */ +#include /* fopen/fread/fwrite/fseek */ +#include /* assert */ +#include /* memset */ #include #include "pios_flash_ut_priv.h" enum flash_ut_magic { - FLASH_UT_MAGIC = 0x321dabc1, + FLASH_UT_MAGIC = 0x321dabc1, }; struct flash_ut_dev { - enum flash_ut_magic magic; - const struct pios_flash_ut_cfg * cfg; - bool transaction_in_progress; - FILE * flash_file; + enum flash_ut_magic magic; + const struct pios_flash_ut_cfg *cfg; + bool transaction_in_progress; + FILE *flash_file; }; -static struct flash_ut_dev * PIOS_Flash_UT_Alloc(void) +static struct flash_ut_dev *PIOS_Flash_UT_Alloc(void) { - struct flash_ut_dev * flash_dev = malloc(sizeof(struct flash_ut_dev)); + struct flash_ut_dev *flash_dev = malloc(sizeof(struct flash_ut_dev)); - flash_dev->magic = FLASH_UT_MAGIC; + flash_dev->magic = FLASH_UT_MAGIC; - return flash_dev; + return flash_dev; } -int32_t PIOS_Flash_UT_Init(uintptr_t * flash_id, const struct pios_flash_ut_cfg * cfg) +int32_t PIOS_Flash_UT_Init(uintptr_t *flash_id, const struct pios_flash_ut_cfg *cfg) { - /* Check inputs */ - assert(flash_id); - assert(cfg); - assert(cfg->size_of_flash); - assert(cfg->size_of_sector); - assert((cfg->size_of_flash % cfg->size_of_sector) == 0); + /* Check inputs */ + assert(flash_id); + assert(cfg); + assert(cfg->size_of_flash); + assert(cfg->size_of_sector); + assert((cfg->size_of_flash % cfg->size_of_sector) == 0); - struct flash_ut_dev * flash_dev = PIOS_Flash_UT_Alloc(); - assert(flash_dev); + struct flash_ut_dev *flash_dev = PIOS_Flash_UT_Alloc(); + assert(flash_dev); - flash_dev->cfg = cfg; - flash_dev->transaction_in_progress = false; + flash_dev->cfg = cfg; + flash_dev->transaction_in_progress = false; - flash_dev->flash_file = fopen (FLASH_IMAGE_FILE, "rb+"); - if (flash_dev->flash_file == NULL) { - return -1; - } + flash_dev->flash_file = fopen(FLASH_IMAGE_FILE, "rb+"); + if (flash_dev->flash_file == NULL) { + return -1; + } - if (fseek (flash_dev->flash_file, flash_dev->cfg->size_of_flash, SEEK_SET) != 0) { - return -2; - } + if (fseek(flash_dev->flash_file, flash_dev->cfg->size_of_flash, SEEK_SET) != 0) { + return -2; + } - *flash_id = (uintptr_t)flash_dev; + *flash_id = (uintptr_t)flash_dev; - return 0; + return 0; } /********************************** @@ -64,96 +64,95 @@ int32_t PIOS_Flash_UT_Init(uintptr_t * flash_id, const struct pios_flash_ut_cfg static int32_t PIOS_Flash_UT_StartTransaction(uintptr_t flash_id) { - struct flash_ut_dev * flash_dev = (struct flash_ut_dev *)flash_id; + struct flash_ut_dev *flash_dev = (struct flash_ut_dev *)flash_id; - assert(!flash_dev->transaction_in_progress); + assert(!flash_dev->transaction_in_progress); - flash_dev->transaction_in_progress = true; + flash_dev->transaction_in_progress = true; - return 0; + return 0; } static int32_t PIOS_Flash_UT_EndTransaction(uintptr_t flash_id) { - struct flash_ut_dev * flash_dev = (struct flash_ut_dev *)flash_id; + struct flash_ut_dev *flash_dev = (struct flash_ut_dev *)flash_id; - assert(flash_dev->transaction_in_progress); + assert(flash_dev->transaction_in_progress); - flash_dev->transaction_in_progress = false; + flash_dev->transaction_in_progress = false; - return 0; + return 0; } static int32_t PIOS_Flash_UT_EraseSector(uintptr_t flash_id, uint32_t addr) { - struct flash_ut_dev * flash_dev = (struct flash_ut_dev *)flash_id; + struct flash_ut_dev *flash_dev = (struct flash_ut_dev *)flash_id; - assert(flash_dev->transaction_in_progress); + assert(flash_dev->transaction_in_progress); - if (fseek (flash_dev->flash_file, addr, SEEK_SET) != 0) { - assert(0); - } + if (fseek(flash_dev->flash_file, addr, SEEK_SET) != 0) { + assert(0); + } - unsigned char * buf = malloc(flash_dev->cfg->size_of_sector); - assert (buf); - memset((void *)buf, 0xFF, flash_dev->cfg->size_of_sector); + unsigned char *buf = malloc(flash_dev->cfg->size_of_sector); + assert(buf); + memset((void *)buf, 0xFF, flash_dev->cfg->size_of_sector); - size_t s; - s = fwrite (buf, 1, flash_dev->cfg->size_of_sector, flash_dev->flash_file); + size_t s; + s = fwrite(buf, 1, flash_dev->cfg->size_of_sector, flash_dev->flash_file); - assert (s == flash_dev->cfg->size_of_sector); + assert(s == flash_dev->cfg->size_of_sector); - return 0; + return 0; } -static int32_t PIOS_Flash_UT_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len) +static int32_t PIOS_Flash_UT_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) { - /* Check inputs */ - assert(data); + /* Check inputs */ + assert(data); - struct flash_ut_dev * flash_dev = (struct flash_ut_dev *)flash_id; + struct flash_ut_dev *flash_dev = (struct flash_ut_dev *)flash_id; - assert(flash_dev->transaction_in_progress); + assert(flash_dev->transaction_in_progress); - if (fseek (flash_dev->flash_file, addr, SEEK_SET) != 0) { - assert(0); - } + if (fseek(flash_dev->flash_file, addr, SEEK_SET) != 0) { + assert(0); + } - size_t s; - s = fwrite (data, 1, len, flash_dev->flash_file); + size_t s; + s = fwrite(data, 1, len, flash_dev->flash_file); - assert (s == len); + assert(s == len); - return 0; + return 0; } -static int32_t PIOS_Flash_UT_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len) +static int32_t PIOS_Flash_UT_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) { - /* Check inputs */ - assert(data); + /* Check inputs */ + assert(data); - struct flash_ut_dev * flash_dev = (struct flash_ut_dev *)flash_id; + struct flash_ut_dev *flash_dev = (struct flash_ut_dev *)flash_id; - assert(flash_dev->transaction_in_progress); + assert(flash_dev->transaction_in_progress); - if (fseek (flash_dev->flash_file, addr, SEEK_SET) != 0) { - assert(0); - } + if (fseek(flash_dev->flash_file, addr, SEEK_SET) != 0) { + assert(0); + } - size_t s; - s = fread (data, 1, len, flash_dev->flash_file); + size_t s; + s = fread(data, 1, len, flash_dev->flash_file); - assert (s == len); + assert(s == len); - return 0; + return 0; } /* Provide a flash driver to external drivers */ const struct pios_flash_driver pios_ut_flash_driver = { - .start_transaction = PIOS_Flash_UT_StartTransaction, - .end_transaction = PIOS_Flash_UT_EndTransaction, - .erase_sector = PIOS_Flash_UT_EraseSector, - .write_data = PIOS_Flash_UT_WriteData, - .read_data = PIOS_Flash_UT_ReadData, + .start_transaction = PIOS_Flash_UT_StartTransaction, + .end_transaction = PIOS_Flash_UT_EndTransaction, + .erase_sector = PIOS_Flash_UT_EraseSector, + .write_data = PIOS_Flash_UT_WriteData, + .read_data = PIOS_Flash_UT_ReadData, }; - diff --git a/flight/tests/logfs/pios_flash_ut_priv.h b/flight/tests/logfs/pios_flash_ut_priv.h index 90a4aeba9..7f2308944 100644 --- a/flight/tests/logfs/pios_flash_ut_priv.h +++ b/flight/tests/logfs/pios_flash_ut_priv.h @@ -1,11 +1,11 @@ #include struct pios_flash_ut_cfg { - uint32_t size_of_flash; - uint32_t size_of_sector; + uint32_t size_of_flash; + uint32_t size_of_sector; }; -int32_t PIOS_Flash_UT_Init(uintptr_t * flash_id, const struct pios_flash_ut_cfg * cfg); +int32_t PIOS_Flash_UT_Init(uintptr_t *flash_id, const struct pios_flash_ut_cfg *cfg); extern const struct pios_flash_driver pios_ut_flash_driver; diff --git a/flight/tests/logfs/unittest.cpp b/flight/tests/logfs/unittest.cpp index c0c383e76..89f58b5c5 100644 --- a/flight/tests/logfs/unittest.cpp +++ b/flight/tests/logfs/unittest.cpp @@ -1,12 +1,11 @@ #include "gtest/gtest.h" -#include /* printf */ -#include /* abort */ -#include /* memset */ +#include /* printf */ +#include /* abort */ +#include /* memset */ extern "C" { - -#include "pios_flash.h" /* PIOS_FLASH_* API */ +#include "pios_flash.h" /* PIOS_FLASH_* API */ #include "pios_flash_ut_priv.h" extern struct pios_flash_ut_cfg flash_config; @@ -15,245 +14,252 @@ extern struct pios_flash_ut_cfg flash_config; extern struct flashfs_logfs_cfg flashfs_config; -#include "pios_flashfs.h" /* PIOS_FLASHFS_* */ - +#include "pios_flashfs.h" /* PIOS_FLASHFS_* */ } -#define OBJ0_ID 0xAA55AA55 +#define OBJ0_ID 0xAA55AA55 -#define OBJ1_ID 0x12345678 +#define OBJ1_ID 0x12345678 #define OBJ1_SIZE 76 -#define OBJ2_ID 0xABCDEFAB +#define OBJ2_ID 0xABCDEFAB #define OBJ2_SIZE 123 -#define OBJ3_ID 0x99999999 +#define OBJ3_ID 0x99999999 #define OBJ3_SIZE (256 - 12) // leave room for the slot header // To use a test fixture, derive a class from testing::Test. class LogfsTestRaw : public testing::Test { protected: - virtual void SetUp() { - /* create an empty, appropriately sized flash filesystem */ - FILE * theflash = fopen(FLASH_IMAGE_FILE, "wb"); - uint8_t sector[flash_config.size_of_sector]; - memset(sector, 0xFF, sizeof(sector)); - for (uint32_t i = 0; i < flash_config.size_of_flash / flash_config.size_of_sector; i++) { - fwrite(sector, sizeof(sector), 1, theflash); - } - fclose(theflash); + virtual void SetUp() + { + /* create an empty, appropriately sized flash filesystem */ + FILE *theflash = fopen(FLASH_IMAGE_FILE, "wb"); + uint8_t sector[flash_config.size_of_sector]; - /* Set up obj1 */ - for (uint32_t i = 0; i < sizeof(obj1); i++) { - obj1[i] = 0x10 + (i % 10); + memset(sector, 0xFF, sizeof(sector)); + for (uint32_t i = 0; i < flash_config.size_of_flash / flash_config.size_of_sector; i++) { + fwrite(sector, sizeof(sector), 1, theflash); + } + fclose(theflash); + + /* Set up obj1 */ + for (uint32_t i = 0; i < sizeof(obj1); i++) { + obj1[i] = 0x10 + (i % 10); + } + + /* Set up a second version of obj1 with different data */ + for (uint32_t i = 0; i < sizeof(obj1_alt); i++) { + obj1_alt[i] = 0xA0 + (i % 10); + } + + /* Set up obj2 */ + for (uint32_t i = 0; i < sizeof(obj2); i++) { + obj2[i] = 0x20 + (i % 10); + } + + /* Set up obj3 */ + for (uint32_t i = 0; i < sizeof(obj3); i++) { + obj3[i] = 0x30 + (i % 10); + } } - /* Set up a second version of obj1 with different data */ - for (uint32_t i = 0; i < sizeof(obj1_alt); i++) { - obj1_alt[i] = 0xA0 + (i % 10); + virtual void TearDown() + { + unlink("theflash.bin"); } - /* Set up obj2 */ - for (uint32_t i = 0; i < sizeof(obj2); i++) { - obj2[i] = 0x20 + (i % 10); - } - - /* Set up obj3 */ - for (uint32_t i = 0; i < sizeof(obj3); i++) { - obj3[i] = 0x30 + (i % 10); - } - } - - virtual void TearDown() { - unlink("theflash.bin"); - } - - unsigned char obj1[OBJ1_SIZE]; - unsigned char obj1_alt[OBJ1_SIZE]; - unsigned char obj2[OBJ2_SIZE]; - unsigned char obj3[OBJ3_SIZE]; + unsigned char obj1[OBJ1_SIZE]; + unsigned char obj1_alt[OBJ1_SIZE]; + unsigned char obj2[OBJ2_SIZE]; + unsigned char obj3[OBJ3_SIZE]; }; TEST_F(LogfsTestRaw, FlashInit) { - uintptr_t flash_id; - EXPECT_EQ(0, PIOS_Flash_UT_Init(&flash_id, &flash_config)); + uintptr_t flash_id; + + EXPECT_EQ(0, PIOS_Flash_UT_Init(&flash_id, &flash_config)); } TEST_F(LogfsTestRaw, LogfsInit) { - uintptr_t flash_id; - EXPECT_EQ(0, PIOS_Flash_UT_Init(&flash_id, &flash_config)); + uintptr_t flash_id; - uintptr_t fs_id; - EXPECT_EQ(0, PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_config, &pios_ut_flash_driver, flash_id)); + EXPECT_EQ(0, PIOS_Flash_UT_Init(&flash_id, &flash_config)); + + uintptr_t fs_id; + EXPECT_EQ(0, PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_config, &pios_ut_flash_driver, flash_id)); } class LogfsTestCooked : public LogfsTestRaw { protected: - virtual void SetUp() { - /* First, we need to set up the super fixture (LogfsTestRaw) */ - LogfsTestRaw::SetUp(); + virtual void SetUp() + { + /* First, we need to set up the super fixture (LogfsTestRaw) */ + LogfsTestRaw::SetUp(); - /* Init the flash and the flashfs so we don't need to repeat this in every test */ - uintptr_t flash_id; - EXPECT_EQ(0, PIOS_Flash_UT_Init(&flash_id, &flash_config)); - EXPECT_EQ(0, PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_config, &pios_ut_flash_driver, flash_id)); - } + /* Init the flash and the flashfs so we don't need to repeat this in every test */ + uintptr_t flash_id; - uintptr_t fs_id; + EXPECT_EQ(0, PIOS_Flash_UT_Init(&flash_id, &flash_config)); + EXPECT_EQ(0, PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_config, &pios_ut_flash_driver, flash_id)); + } + + uintptr_t fs_id; }; TEST_F(LogfsTestCooked, LogfsFormat) { - EXPECT_EQ(0, PIOS_FLASHFS_Format(fs_id)); + EXPECT_EQ(0, PIOS_FLASHFS_Format(fs_id)); } TEST_F(LogfsTestCooked, WriteOne) { - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); } TEST_F(LogfsTestCooked, WriteVerifyOne) { - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); } TEST_F(LogfsTestCooked, WriteVerifyDeleteVerifyOne) { - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjDelete(fs_id, OBJ1_ID, 0)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjDelete(fs_id, OBJ1_ID, 0)); - EXPECT_EQ(-2, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(-2, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); } TEST_F(LogfsTestCooked, WriteTwoVerifyOneA) { - /* Write obj1 then obj2 */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); + /* Write obj1 then obj2 */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); - /* Read back obj1 */ - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + /* Read back obj1 */ + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); } TEST_F(LogfsTestCooked, WriteTwoVerifyOneB) { - /* Write obj2 then obj1 */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); + /* Write obj2 then obj1 */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); - /* Read back obj1 */ - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + /* Read back obj1 */ + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); } TEST_F(LogfsTestCooked, WriteZeroSize) { - /* Write a zero length object */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ0_ID, 0, NULL, 0)); + /* Write a zero length object */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ0_ID, 0, NULL, 0)); } TEST_F(LogfsTestCooked, WriteVerifyZeroLength) { - /* Write a zero length object */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ0_ID, 0, NULL, 0)); + /* Write a zero length object */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ0_ID, 0, NULL, 0)); - /* Read back a zero length object -- effectively an existence check */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ0_ID, 0, NULL, 0)); + /* Read back a zero length object -- effectively an existence check */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ0_ID, 0, NULL, 0)); } TEST_F(LogfsTestCooked, WriteMaxSize) { - /* Write a zero length object */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ3_ID, 0, obj3, sizeof(obj3))); + /* Write a zero length object */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ3_ID, 0, obj3, sizeof(obj3))); } TEST_F(LogfsTestCooked, ReadNonexistent) { - /* Read back a zero length object -- basically an existence check */ - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(-2, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + /* Read back a zero length object -- basically an existence check */ + unsigned char obj1_check[OBJ1_SIZE]; + + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(-2, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); } TEST_F(LogfsTestCooked, WriteVerifyMultiInstance) { - /* Write instance zero */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); + /* Write instance zero */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); - /* Write a non-zero instance ID */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 123, obj1_alt, sizeof(obj1_alt))); + /* Write a non-zero instance ID */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 123, obj1_alt, sizeof(obj1_alt))); - unsigned char obj1_check[OBJ1_SIZE]; + unsigned char obj1_check[OBJ1_SIZE]; - /* Read back instance 123 */ - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 123, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1_alt, obj1_check, sizeof(obj1_alt))); + /* Read back instance 123 */ + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 123, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1_alt, obj1_check, sizeof(obj1_alt))); - /* Read back instance 0 */ - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + /* Read back instance 0 */ + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); } TEST_F(LogfsTestCooked, FillFilesystemAndGarbageCollect) { - /* Fill up the entire filesystem with multiple instances of obj1 */ - for (uint32_t i = 0; i < (flashfs_config.arena_size / flashfs_config.slot_size) - 1; i++) { - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, i, obj1, sizeof(obj1))); - } + /* Fill up the entire filesystem with multiple instances of obj1 */ + for (uint32_t i = 0; i < (flashfs_config.arena_size / flashfs_config.slot_size) - 1; i++) { + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, i, obj1, sizeof(obj1))); + } - /* Should fail to add a new object since the filesystem is full */ - EXPECT_EQ(-3, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); + /* Should fail to add a new object since the filesystem is full */ + EXPECT_EQ(-3, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); - /* Now save a new version of an existing object which should trigger gc and succeed */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1_alt, sizeof(obj1_alt))); + /* Now save a new version of an existing object which should trigger gc and succeed */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1_alt, sizeof(obj1_alt))); - /* Read back one of the original obj1 instances */ - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 1, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + /* Read back one of the original obj1 instances */ + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 1, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); - /* Read back the new version of obj1 written after gc */ - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1_alt, obj1_check, sizeof(obj1_alt))); + /* Read back the new version of obj1 written after gc */ + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1_alt, obj1_check, sizeof(obj1_alt))); } TEST_F(LogfsTestCooked, WriteManyVerify) { - for (uint32_t i = 0; i < 10000; i++) { - /* Write a collection of objects */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ0_ID, 0, NULL, 0)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 123, obj1_alt, sizeof(obj1_alt))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ3_ID, 0, obj3, sizeof(obj3))); - } + for (uint32_t i = 0; i < 10000; i++) { + /* Write a collection of objects */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ0_ID, 0, NULL, 0)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 123, obj1_alt, sizeof(obj1_alt))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ3_ID, 0, obj3, sizeof(obj3))); + } - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ0_ID, 0, NULL, 0)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ0_ID, 0, NULL, 0)); - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); - unsigned char obj1_alt_check[OBJ1_SIZE]; - memset(obj1_alt_check, 0, sizeof(obj1_alt_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 123, obj1_alt_check, sizeof(obj1_alt_check))); - EXPECT_EQ(0, memcmp(obj1_alt, obj1_alt_check, sizeof(obj1_alt))); + unsigned char obj1_alt_check[OBJ1_SIZE]; + memset(obj1_alt_check, 0, sizeof(obj1_alt_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 123, obj1_alt_check, sizeof(obj1_alt_check))); + EXPECT_EQ(0, memcmp(obj1_alt, obj1_alt_check, sizeof(obj1_alt))); - unsigned char obj2_check[OBJ2_SIZE]; - memset(obj2_check, 0, sizeof(obj2_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ2_ID, 0, obj2_check, sizeof(obj2_check))); - EXPECT_EQ(0, memcmp(obj2, obj2_check, sizeof(obj2))); + unsigned char obj2_check[OBJ2_SIZE]; + memset(obj2_check, 0, sizeof(obj2_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ2_ID, 0, obj2_check, sizeof(obj2_check))); + EXPECT_EQ(0, memcmp(obj2, obj2_check, sizeof(obj2))); - unsigned char obj3_check[OBJ3_SIZE]; - memset(obj3_check, 0, sizeof(obj3_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ3_ID, 0, obj3_check, sizeof(obj3_check))); - EXPECT_EQ(0, memcmp(obj3, obj3_check, sizeof(obj3))); + unsigned char obj3_check[OBJ3_SIZE]; + memset(obj3_check, 0, sizeof(obj3_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ3_ID, 0, obj3_check, sizeof(obj3_check))); + EXPECT_EQ(0, memcmp(obj3, obj3_check, sizeof(obj3))); } diff --git a/flight/tests/logfs/unittest_init.c b/flight/tests/logfs/unittest_init.c index b60570c38..2e68db2fb 100644 --- a/flight/tests/logfs/unittest_init.c +++ b/flight/tests/logfs/unittest_init.c @@ -1,4 +1,4 @@ -/* +/* * These need to be defined in a .c file so that we can use * designated initializer syntax which c++ doesn't support (yet). */ @@ -7,20 +7,19 @@ const struct pios_flash_ut_cfg flash_config = { - .size_of_flash = 0x00200000, - .size_of_sector = 0x00010000, + .size_of_flash = 0x00200000, + .size_of_sector = 0x00010000, }; #include "pios_flashfs_logfs_priv.h" const struct flashfs_logfs_cfg flashfs_config = { - .fs_magic = 0x89abceef, - .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ - .arena_size = 0x00010000, /* 256 * slot size */ - .slot_size = 0x00000100, /* 256 bytes */ + .fs_magic = 0x89abceef, + .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ + .arena_size = 0x00010000, /* 256 * slot size */ + .slot_size = 0x00000100, /* 256 bytes */ - .start_offset = 0, /* start at the beginning of the chip */ - .sector_size = 0x00010000, /* 64K bytes */ - .page_size = 0x00000100, /* 256 bytes */ + .start_offset = 0, /* start at the beginning of the chip */ + .sector_size = 0x00010000, /* 64K bytes */ + .page_size = 0x00000100, /* 256 bytes */ }; - diff --git a/flight/uavobjects/callbackscheduler.c b/flight/uavobjects/callbackscheduler.c index 7a38cb551..f0f666a15 100644 --- a/flight/uavobjects/callbackscheduler.c +++ b/flight/uavobjects/callbackscheduler.c @@ -30,36 +30,35 @@ // Private constants #define STACK_SIZE 128 -#define MAX_SLEEP 1000 +#define MAX_SLEEP 1000 // Private types /** * task information */ struct DelayedCallbackTaskStruct { - DelayedCallbackInfo *callbackQueue[CALLBACK_PRIORITY_LOW + 1]; - DelayedCallbackInfo *queueCursor[CALLBACK_PRIORITY_LOW + 1]; - xTaskHandle callbackSchedulerTaskHandle; - signed char name[3]; - uint32_t stackSize; - DelayedCallbackPriorityTask priorityTask; - xSemaphoreHandle signal; - struct DelayedCallbackTaskStruct *next; + DelayedCallbackInfo *callbackQueue[CALLBACK_PRIORITY_LOW + 1]; + DelayedCallbackInfo *queueCursor[CALLBACK_PRIORITY_LOW + 1]; + xTaskHandle callbackSchedulerTaskHandle; + signed char name[3]; + uint32_t stackSize; + DelayedCallbackPriorityTask priorityTask; + xSemaphoreHandle signal; + struct DelayedCallbackTaskStruct *next; }; /** * callback information */ struct DelayedCallbackInfoStruct { - DelayedCallback cb; - bool volatile waiting; - uint32_t volatile scheduletime; - struct DelayedCallbackTaskStruct *task; - struct DelayedCallbackInfoStruct *next; + DelayedCallback cb; + bool volatile waiting; + uint32_t volatile scheduletime; + struct DelayedCallbackTaskStruct *task; + struct DelayedCallbackInfoStruct *next; }; - // Private variables static struct DelayedCallbackTaskStruct *schedulerTasks; static xSemaphoreHandle mutex; @@ -76,18 +75,18 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa */ int32_t CallbackSchedulerInitialize() { - // Initialize variables - schedulerTasks = NULL; - schedulerStarted = false; + // Initialize variables + schedulerTasks = NULL; + schedulerStarted = false; - // Create mutex - mutex = xSemaphoreCreateRecursiveMutex(); - if (mutex == NULL) { - return -1; - } + // Create mutex + mutex = xSemaphoreCreateRecursiveMutex(); + if (mutex == NULL) { + return -1; + } - // Done - return 0; + // Done + return 0; } /** @@ -102,34 +101,34 @@ int32_t CallbackSchedulerInitialize() */ int32_t CallbackSchedulerStart() { - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // only call once - PIOS_Assert(schedulerStarted == false); + // only call once + PIOS_Assert(schedulerStarted == false); - // start tasks - struct DelayedCallbackTaskStruct *cursor = NULL; - int t = 0; - LL_FOREACH(schedulerTasks, cursor) { - xTaskCreate( - CallbackSchedulerTask, - cursor->name, - cursor->stackSize/4, - cursor, - cursor->priorityTask, - &cursor->callbackSchedulerTaskHandle - ); - if (TASKINFO_RUNNING_CALLBACKSCHEDULER0+t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, cursor->callbackSchedulerTaskHandle); - } - t++; - } + // start tasks + struct DelayedCallbackTaskStruct *cursor = NULL; + int t = 0; + LL_FOREACH(schedulerTasks, cursor) { + xTaskCreate( + CallbackSchedulerTask, + cursor->name, + cursor->stackSize / 4, + cursor, + cursor->priorityTask, + &cursor->callbackSchedulerTaskHandle + ); + if (TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, cursor->callbackSchedulerTaskHandle); + } + t++; + } - schedulerStarted = true; + schedulerStarted = true; - xSemaphoreGiveRecursive(mutex); + xSemaphoreGiveRecursive(mutex); - return 0; + return 0; } /** @@ -145,47 +144,45 @@ int32_t CallbackSchedulerStart() * \return 0: not scheduled, previous schedule takes precedence, 1: new schedule, 2: previous schedule overridden */ int32_t DelayedCallbackSchedule( - DelayedCallbackInfo *cbinfo, - int32_t milliseconds, - DelayedCallbackUpdateMode updatemode - ) + DelayedCallbackInfo *cbinfo, + int32_t milliseconds, + DelayedCallbackUpdateMode updatemode) { - int32_t result = 0; + int32_t result = 0; - PIOS_Assert(cbinfo); + PIOS_Assert(cbinfo); - if (milliseconds <= 0) { - milliseconds = 0; // we can and will not schedule in the past since that ruins the wraparound of uint32_t - } + if (milliseconds <= 0) { + milliseconds = 0; // we can and will not schedule in the past since that ruins the wraparound of uint32_t + } - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - uint32_t new = xTaskGetTickCount() + (milliseconds/portTICK_RATE_MS); - if (!new) { - new = 1; // zero has a special meaning, schedule at time 1 instead - } + uint32_t new = xTaskGetTickCount() + (milliseconds / portTICK_RATE_MS); + if (!new) { + new = 1; // zero has a special meaning, schedule at time 1 instead + } - int32_t diff = new - cbinfo->scheduletime; - if ( (!cbinfo->scheduletime) - || ((updatemode & CALLBACK_UPDATEMODE_SOONER) && diff < 0) - || ((updatemode & CALLBACK_UPDATEMODE_LATER ) && diff > 0) - ) { + int32_t diff = new - cbinfo->scheduletime; + if ((!cbinfo->scheduletime) + || ((updatemode & CALLBACK_UPDATEMODE_SOONER) && diff < 0) + || ((updatemode & CALLBACK_UPDATEMODE_LATER) && diff > 0) + ) { + // the scheduletime may be updated + if (!cbinfo->scheduletime) { + result = 1; + } else { + result = 2; + } + cbinfo->scheduletime = new; - // the scheduletime may be updated - if (!cbinfo->scheduletime) { - result = 1; - } else { - result = 2; - } - cbinfo->scheduletime = new; + // scheduler needs to be notified to adapt sleep times + xSemaphoreGive(cbinfo->task->signal); + } - // scheduler needs to be notified to adapt sleep times - xSemaphoreGive(cbinfo->task->signal); - } + xSemaphoreGiveRecursive(mutex); - xSemaphoreGiveRecursive(mutex); - - return result; + return result; } /** @@ -196,12 +193,12 @@ int32_t DelayedCallbackSchedule( */ int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo) { - PIOS_Assert(cbinfo); + PIOS_Assert(cbinfo); - // no semaphore needed for the callback - cbinfo->waiting = true; - // but the scheduler as a whole needs to be notified - return xSemaphoreGive(cbinfo->task->signal); + // no semaphore needed for the callback + cbinfo->waiting = true; + // but the scheduler as a whole needs to be notified + return xSemaphoreGive(cbinfo->task->signal); } /** @@ -220,12 +217,12 @@ int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo) */ int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken) { - PIOS_Assert(cbinfo); + PIOS_Assert(cbinfo); - // no semaphore needed for the callback - cbinfo->waiting = true; - // but the scheduler as a whole needs to be notified - return xSemaphoreGiveFromISR(cbinfo->task->signal, pxHigherPriorityTaskWoken); + // no semaphore needed for the callback + cbinfo->waiting = true; + // but the scheduler as a whole needs to be notified + return xSemaphoreGiveFromISR(cbinfo->task->signal, pxHigherPriorityTaskWoken); } /** @@ -241,100 +238,98 @@ int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigh * \return CallbackInfo Pointer on success, NULL if failed. */ DelayedCallbackInfo *DelayedCallbackCreate( - DelayedCallback cb, - DelayedCallbackPriority priority, - DelayedCallbackPriorityTask priorityTask, - uint32_t stacksize - ) + DelayedCallback cb, + DelayedCallbackPriority priority, + DelayedCallbackPriorityTask priorityTask, + uint32_t stacksize) { + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // find appropriate scheduler task matching priorityTask + struct DelayedCallbackTaskStruct *task = NULL; + int t = 0; + LL_FOREACH(schedulerTasks, task) { + if (task->priorityTask == priorityTask) { + break; // found + } + t++; + } + // if given priorityTask does not exist, create it + if (!task) { + // allocate memory if possible + task = (struct DelayedCallbackTaskStruct *)pvPortMalloc(sizeof(struct DelayedCallbackTaskStruct)); + if (!task) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } - // find appropriate scheduler task matching priorityTask - struct DelayedCallbackTaskStruct *task = NULL; - int t = 0; - LL_FOREACH(schedulerTasks, task) { - if (task->priorityTask == priorityTask) { - break; // found - } - t++; - } - // if given priorityTask does not exist, create it - if (!task) { - // allocate memory if possible - task = (struct DelayedCallbackTaskStruct*)pvPortMalloc(sizeof(struct DelayedCallbackTaskStruct)); - if (!task) { - xSemaphoreGiveRecursive(mutex); - return NULL; - } + // initialize structure + for (DelayedCallbackPriority p = 0; p <= CALLBACK_PRIORITY_LOW; p++) { + task->callbackQueue[p] = NULL; + task->queueCursor[p] = NULL; + } + task->name[0] = 'C'; + task->name[1] = 'a' + t; + task->name[2] = 0; + task->stackSize = ((STACK_SIZE > stacksize) ? STACK_SIZE : stacksize); + task->priorityTask = priorityTask; + task->next = NULL; - // initialize structure - for (DelayedCallbackPriority p = 0; p <= CALLBACK_PRIORITY_LOW; p++) { - task->callbackQueue[p] = NULL; - task->queueCursor[p] = NULL; - } - task->name[0] = 'C'; - task->name[1] = 'a'+t; - task->name[2] = 0; - task->stackSize = ((STACK_SIZE>stacksize)?STACK_SIZE:stacksize); - task->priorityTask = priorityTask; - task->next = NULL; + // create the signaling semaphore + vSemaphoreCreateBinary(task->signal); + if (!task->signal) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } - // create the signaling semaphore - vSemaphoreCreateBinary( task->signal ); - if (!task->signal) { - xSemaphoreGiveRecursive(mutex); - return NULL; - } + // add to list of scheduler tasks + LL_APPEND(schedulerTasks, task); - // add to list of scheduler tasks - LL_APPEND(schedulerTasks, task); + // Previously registered tasks are spawned when CallbackSchedulerStart() is called. + // Tasks registered afterwards need to spawn upon creation. + if (schedulerStarted) { + xTaskCreate( + CallbackSchedulerTask, + task->name, + task->stackSize / 4, + task, + task->priorityTask, + &task->callbackSchedulerTaskHandle + ); + if (TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, task->callbackSchedulerTaskHandle); + } + } + } - // Previously registered tasks are spawned when CallbackSchedulerStart() is called. - // Tasks registered afterwards need to spawn upon creation. - if (schedulerStarted) { - xTaskCreate( - CallbackSchedulerTask, - task->name, - task->stackSize/4, - task, - task->priorityTask, - &task->callbackSchedulerTaskHandle - ); - if (TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, task->callbackSchedulerTaskHandle); - } - } - } - - if (!schedulerStarted && stacksize > task->stackSize) { - task->stackSize = stacksize; // previous to task initialisation we can still adapt to the maximum needed stack - } + if (!schedulerStarted && stacksize > task->stackSize) { + task->stackSize = stacksize; // previous to task initialisation we can still adapt to the maximum needed stack + } - if (stacksize > task->stackSize) { - xSemaphoreGiveRecursive(mutex); - return NULL; // error - not enough memory - } + if (stacksize > task->stackSize) { + xSemaphoreGiveRecursive(mutex); + return NULL; // error - not enough memory + } - // initialize callback scheduling info - DelayedCallbackInfo *info = (DelayedCallbackInfo*)pvPortMalloc(sizeof(DelayedCallbackInfo)); - if (!info) { - xSemaphoreGiveRecursive(mutex); - return NULL; // error - not enough memory - } - info->next = NULL; - info->waiting = false; - info->scheduletime = 0; - info->task = task; - info->cb = cb; + // initialize callback scheduling info + DelayedCallbackInfo *info = (DelayedCallbackInfo *)pvPortMalloc(sizeof(DelayedCallbackInfo)); + if (!info) { + xSemaphoreGiveRecursive(mutex); + return NULL; // error - not enough memory + } + info->next = NULL; + info->waiting = false; + info->scheduletime = 0; + info->task = task; + info->cb = cb; - // add to scheduling queue - LL_APPEND(task->callbackQueue[priority], info); + // add to scheduling queue + LL_APPEND(task->callbackQueue[priority], info); - xSemaphoreGiveRecursive(mutex); + xSemaphoreGiveRecursive(mutex); - return info; + return info; } /** @@ -345,60 +340,59 @@ DelayedCallbackInfo *DelayedCallbackCreate( */ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallbackPriority priority) { + int32_t result = MAX_SLEEP; + int32_t diff = 0; - int32_t result = MAX_SLEEP; - int32_t diff = 0; + // no such queue + if (priority > CALLBACK_PRIORITY_LOW) { + return result; + } - // no such queue - if (priority > CALLBACK_PRIORITY_LOW) { - return result; - } + // queue is empty, search a lower priority queue + if (task->callbackQueue[priority] == NULL) { + return runNextCallback(task, priority + 1); + } - // queue is empty, search a lower priority queue - if (task->callbackQueue[priority] == NULL) { - return runNextCallback(task, priority + 1); - } - - DelayedCallbackInfo *current = task->queueCursor[priority]; - DelayedCallbackInfo *next; - do { - if (current == NULL) { - next = task->callbackQueue[priority]; // loop around the end of the list - // also attempt to run a callback that has lower priority - // every time the queue is completely traversed - diff = runNextCallback(task, priority + 1); - if (!diff) { - task->queueCursor[priority] = next; // the recursive call has executed a callback - return 0; - } - if (diff < result) { - result = diff; // adjust sleep time - } - } else { - next = current->next; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // access to scheduletime should be mutex protected - if (current->scheduletime) { - diff = current->scheduletime - xTaskGetTickCount(); - if (diff <= 0) { - current->waiting = true; - } else if (diff < result) { - result = diff; // adjust sleep time - } - } - if (current->waiting) { - task->queueCursor[priority] = next; - current->scheduletime = 0; // any schedules are reset - current->waiting = false; // the flag is reset just before execution. - xSemaphoreGiveRecursive(mutex); - current->cb(); // call the callback - return 0; - } - xSemaphoreGiveRecursive(mutex); - } - current = next; - } while (current != task->queueCursor[priority]); - // once the list has been traversed entirely without finding any to be executed task, abort (nothing to do) - return result; + DelayedCallbackInfo *current = task->queueCursor[priority]; + DelayedCallbackInfo *next; + do { + if (current == NULL) { + next = task->callbackQueue[priority]; // loop around the end of the list + // also attempt to run a callback that has lower priority + // every time the queue is completely traversed + diff = runNextCallback(task, priority + 1); + if (!diff) { + task->queueCursor[priority] = next; // the recursive call has executed a callback + return 0; + } + if (diff < result) { + result = diff; // adjust sleep time + } + } else { + next = current->next; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // access to scheduletime should be mutex protected + if (current->scheduletime) { + diff = current->scheduletime - xTaskGetTickCount(); + if (diff <= 0) { + current->waiting = true; + } else if (diff < result) { + result = diff; // adjust sleep time + } + } + if (current->waiting) { + task->queueCursor[priority] = next; + current->scheduletime = 0; // any schedules are reset + current->waiting = false; // the flag is reset just before execution. + xSemaphoreGiveRecursive(mutex); + current->cb(); // call the callback + return 0; + } + xSemaphoreGiveRecursive(mutex); + } + current = next; + } while (current != task->queueCursor[priority]); + // once the list has been traversed entirely without finding any to be executed task, abort (nothing to do) + return result; } /** @@ -407,15 +401,13 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa */ static void CallbackSchedulerTask(void *task) { - uint32_t delay = 0; + uint32_t delay = 0; - while (1) { - - delay = runNextCallback((struct DelayedCallbackTaskStruct*)task, CALLBACK_PRIORITY_CRITICAL); - if (delay) { - // nothing to do but sleep - xSemaphoreTake(((struct DelayedCallbackTaskStruct*)task)->signal, delay); - } - } + while (1) { + delay = runNextCallback((struct DelayedCallbackTaskStruct *)task, CALLBACK_PRIORITY_CRITICAL); + if (delay) { + // nothing to do but sleep + xSemaphoreTake(((struct DelayedCallbackTaskStruct *)task)->signal, delay); + } + } } - diff --git a/flight/uavobjects/eventdispatcher.c b/flight/uavobjects/eventdispatcher.c index 962298980..503e9b1e5 100644 --- a/flight/uavobjects/eventdispatcher.c +++ b/flight/uavobjects/eventdispatcher.c @@ -4,7 +4,7 @@ * @file eventdispatcher.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Event dispatcher, distributes object events as callbacks. Alternative - * to using tasks and queues. All callbacks are invoked from the event task. + * to using tasks and queues. All callbacks are invoked from the event task. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ @@ -30,18 +30,18 @@ // Private constants #if defined(PIOS_EVENTDISAPTCHER_QUEUE) -#define MAX_QUEUE_SIZE PIOS_EVENTDISAPTCHER_QUEUE +#define MAX_QUEUE_SIZE PIOS_EVENTDISAPTCHER_QUEUE #else -#define MAX_QUEUE_SIZE 20 +#define MAX_QUEUE_SIZE 20 #endif #if defined(PIOS_EVENTDISPATCHER_STACK_SIZE) -#define STACK_SIZE PIOS_EVENTDISPATCHER_STACK_SIZE +#define STACK_SIZE PIOS_EVENTDISPATCHER_STACK_SIZE #else -#define STACK_SIZE configMINIMAL_STACK_SIZE +#define STACK_SIZE configMINIMAL_STACK_SIZE #endif /* PIOS_EVENTDISPATCHER_STACK_SIZE */ -#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) #define MAX_UPDATE_PERIOD_MS 1000 // Private types @@ -51,24 +51,24 @@ * Event callback information */ typedef struct { - UAVObjEvent ev; /** The actual event */ - UAVObjEventCallback cb; /** The callback function, or zero if none */ - xQueueHandle queue; /** The queue or zero if none */ + UAVObjEvent ev; /** The actual event */ + UAVObjEventCallback cb; /** The callback function, or zero if none */ + xQueueHandle queue; /** The queue or zero if none */ } EventCallbackInfo; /** * List of object properties that are needed for the periodic updates. */ struct PeriodicObjectListStruct { - EventCallbackInfo evInfo; /** Event callback information */ + EventCallbackInfo evInfo; /** Event callback information */ uint16_t updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */ - int32_t timeToNextUpdateMs; /** Time delay to the next update */ - struct PeriodicObjectListStruct* next; /** Needed by linked list library (utlist.h) */ + int32_t timeToNextUpdateMs; /** Time delay to the next update */ + struct PeriodicObjectListStruct *next; /** Needed by linked list library (utlist.h) */ }; typedef struct PeriodicObjectListStruct PeriodicObjectList; // Private variables -static PeriodicObjectList* mObjList; +static PeriodicObjectList *mObjList; static xQueueHandle mQueue; static xTaskHandle mEventTaskHandle; static xSemaphoreHandle mMutex; @@ -77,8 +77,8 @@ static EventStats mStats; // Private functions static int32_t processPeriodicUpdates(); static void eventTask(); -static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs); -static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs); +static int32_t eventPeriodicCreate(UAVObjEvent *ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs); +static int32_t eventPeriodicUpdate(UAVObjEvent *ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs); static uint16_t randomizePeriod(uint16_t periodMs); @@ -88,34 +88,35 @@ static uint16_t randomizePeriod(uint16_t periodMs); */ int32_t EventDispatcherInitialize() { - // Initialize variables - mObjList = NULL; - memset(&mStats, 0, sizeof(EventStats)); + // Initialize variables + mObjList = NULL; + memset(&mStats, 0, sizeof(EventStats)); - // Create mMutex - mMutex = xSemaphoreCreateRecursiveMutex(); - if (mMutex == NULL) - return -1; + // Create mMutex + mMutex = xSemaphoreCreateRecursiveMutex(); + if (mMutex == NULL) { + return -1; + } - // Create event queue - mQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo)); + // Create event queue + mQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo)); - // Create task - xTaskCreate(eventTask, (signed char*)"Event", STACK_SIZE, NULL, TASK_PRIORITY, &mEventTaskHandle); + // Create task + xTaskCreate(eventTask, (signed char *)"Event", STACK_SIZE, NULL, TASK_PRIORITY, &mEventTaskHandle); - // Done - return 0; + // Done + return 0; } /** * Get the statistics counters * @param[out] statsOut The statistics counters will be copied there */ -void EventGetStats(EventStats* statsOut) +void EventGetStats(EventStats *statsOut) { - xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); - memcpy(statsOut, &mStats, sizeof(EventStats)); - xSemaphoreGiveRecursive(mMutex); + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); + memcpy(statsOut, &mStats, sizeof(EventStats)); + xSemaphoreGiveRecursive(mMutex); } /** @@ -123,9 +124,9 @@ void EventGetStats(EventStats* statsOut) */ void EventClearStats() { - xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); - memset(&mStats, 0, sizeof(EventStats)); - xSemaphoreGiveRecursive(mMutex); + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); + memset(&mStats, 0, sizeof(EventStats)); + xSemaphoreGiveRecursive(mMutex); } /** @@ -135,15 +136,16 @@ void EventClearStats() * \param[in] cb The callback function * \return Success (0), failure (-1) */ -int32_t EventCallbackDispatch(UAVObjEvent* ev, UAVObjEventCallback cb) +int32_t EventCallbackDispatch(UAVObjEvent *ev, UAVObjEventCallback cb) { - EventCallbackInfo evInfo; - // Initialize event callback information - memcpy(&evInfo.ev, ev, sizeof(UAVObjEvent)); - evInfo.cb = cb; - evInfo.queue = 0; - // Push to queue - return xQueueSend(mQueue, &evInfo, 0); // will not block if queue is full + EventCallbackInfo evInfo; + + // Initialize event callback information + memcpy(&evInfo.ev, ev, sizeof(UAVObjEvent)); + evInfo.cb = cb; + evInfo.queue = 0; + // Push to queue + return xQueueSend(mQueue, &evInfo, 0); // will not block if queue is full } /** @@ -153,9 +155,9 @@ int32_t EventCallbackDispatch(UAVObjEvent* ev, UAVObjEventCallback cb) * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, uint16_t periodMs) +int32_t EventPeriodicCallbackCreate(UAVObjEvent *ev, UAVObjEventCallback cb, uint16_t periodMs) { - return eventPeriodicCreate(ev, cb, 0, periodMs); + return eventPeriodicCreate(ev, cb, 0, periodMs); } /** @@ -165,9 +167,9 @@ int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, uin * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, uint16_t periodMs) +int32_t EventPeriodicCallbackUpdate(UAVObjEvent *ev, UAVObjEventCallback cb, uint16_t periodMs) { - return eventPeriodicUpdate(ev, cb, 0, periodMs); + return eventPeriodicUpdate(ev, cb, 0, periodMs); } /** @@ -177,9 +179,9 @@ int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, uin * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, uint16_t periodMs) +int32_t EventPeriodicQueueCreate(UAVObjEvent *ev, xQueueHandle queue, uint16_t periodMs) { - return eventPeriodicCreate(ev, 0, queue, periodMs); + return eventPeriodicCreate(ev, 0, queue, periodMs); } /** @@ -189,9 +191,9 @@ int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, uint16_t p * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, uint16_t periodMs) +int32_t EventPeriodicQueueUpdate(UAVObjEvent *ev, xQueueHandle queue, uint16_t periodMs) { - return eventPeriodicUpdate(ev, 0, queue, periodMs); + return eventPeriodicUpdate(ev, 0, queue, periodMs); } /** @@ -202,39 +204,40 @@ int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, uint16_t p * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs) +static int32_t eventPeriodicCreate(UAVObjEvent *ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs) { - PeriodicObjectList *objEntry; - // Get lock - xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); - // Check that the object is not already connected - LL_FOREACH(mObjList, objEntry) { - if (objEntry->evInfo.cb == cb && - objEntry->evInfo.queue == queue && - objEntry->evInfo.ev.obj == ev->obj && - objEntry->evInfo.ev.instId == ev->instId && - objEntry->evInfo.ev.event == ev->event) { - // Already registered, do nothing - xSemaphoreGiveRecursive(mMutex); - return -1; - } - } + PeriodicObjectList *objEntry; + + // Get lock + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); + // Check that the object is not already connected + LL_FOREACH(mObjList, objEntry) { + if (objEntry->evInfo.cb == cb && + objEntry->evInfo.queue == queue && + objEntry->evInfo.ev.obj == ev->obj && + objEntry->evInfo.ev.instId == ev->instId && + objEntry->evInfo.ev.event == ev->event) { + // Already registered, do nothing + xSemaphoreGiveRecursive(mMutex); + return -1; + } + } // Create handle - objEntry = (PeriodicObjectList*)pvPortMalloc(sizeof(PeriodicObjectList)); - if (objEntry == NULL) { - return -1; - } - objEntry->evInfo.ev.obj = ev->obj; - objEntry->evInfo.ev.instId = ev->instId; - objEntry->evInfo.ev.event = ev->event; - objEntry->evInfo.cb = cb; - objEntry->evInfo.queue = queue; - objEntry->updatePeriodMs = periodMs; + objEntry = (PeriodicObjectList *)pvPortMalloc(sizeof(PeriodicObjectList)); + if (objEntry == NULL) { + return -1; + } + objEntry->evInfo.ev.obj = ev->obj; + objEntry->evInfo.ev.instId = ev->instId; + objEntry->evInfo.ev.event = ev->event; + objEntry->evInfo.cb = cb; + objEntry->evInfo.queue = queue; + objEntry->updatePeriodMs = periodMs; objEntry->timeToNextUpdateMs = randomizePeriod(periodMs); // avoid bunching of updates // Add to list LL_APPEND(mObjList, objEntry); - // Release lock - xSemaphoreGiveRecursive(mMutex); + // Release lock + xSemaphoreGiveRecursive(mMutex); return 0; } @@ -246,28 +249,29 @@ static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs) +static int32_t eventPeriodicUpdate(UAVObjEvent *ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs) { - PeriodicObjectList* objEntry; - // Get lock - xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); - // Find object - LL_FOREACH(mObjList, objEntry) { - if (objEntry->evInfo.cb == cb && - objEntry->evInfo.queue == queue && - objEntry->evInfo.ev.obj == ev->obj && - objEntry->evInfo.ev.instId == ev->instId && - objEntry->evInfo.ev.event == ev->event) { - // Object found, update period - objEntry->updatePeriodMs = periodMs; - objEntry->timeToNextUpdateMs = randomizePeriod(periodMs); // avoid bunching of updates - // Release lock - xSemaphoreGiveRecursive(mMutex); - return 0; - } - } + PeriodicObjectList *objEntry; + + // Get lock + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); + // Find object + LL_FOREACH(mObjList, objEntry) { + if (objEntry->evInfo.cb == cb && + objEntry->evInfo.queue == queue && + objEntry->evInfo.ev.obj == ev->obj && + objEntry->evInfo.ev.instId == ev->instId && + objEntry->evInfo.ev.event == ev->event) { + // Object found, update period + objEntry->updatePeriodMs = periodMs; + objEntry->timeToNextUpdateMs = randomizePeriod(periodMs); // avoid bunching of updates + // Release lock + xSemaphoreGiveRecursive(mMutex); + return 0; + } + } // If this point is reached the object was not found - xSemaphoreGiveRecursive(mMutex); + xSemaphoreGiveRecursive(mMutex); return -1; } @@ -276,38 +280,34 @@ static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue */ static void eventTask() { - uint32_t timeToNextUpdateMs; - uint32_t delayMs; - EventCallbackInfo evInfo; + uint32_t timeToNextUpdateMs; + uint32_t delayMs; + EventCallbackInfo evInfo; - /* Must do this in task context to ensure that TaskMonitor has already finished its init */ - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_EVENTDISPATCHER, mEventTaskHandle); + /* Must do this in task context to ensure that TaskMonitor has already finished its init */ + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_EVENTDISPATCHER, mEventTaskHandle); - // Initialize time - timeToNextUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS; + // Initialize time + timeToNextUpdateMs = xTaskGetTickCount() * portTICK_RATE_MS; - // Loop forever - while (1) - { - // Calculate delay time - delayMs = timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS); + // Loop forever + while (1) { + // Calculate delay time + delayMs = timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS); - // Wait for queue message - if ( xQueueReceive(mQueue, &evInfo, delayMs/portTICK_RATE_MS) == pdTRUE ) - { - // Invoke callback, if one - if ( evInfo.cb != 0) - { - evInfo.cb(&evInfo.ev); // the function is expected to copy the event information - } - } + // Wait for queue message + if (xQueueReceive(mQueue, &evInfo, delayMs / portTICK_RATE_MS) == pdTRUE) { + // Invoke callback, if one + if (evInfo.cb != 0) { + evInfo.cb(&evInfo.ev); // the function is expected to copy the event information + } + } - // Process periodic updates - if ((xTaskGetTickCount()*portTICK_RATE_MS) >= timeToNextUpdateMs ) - { - timeToNextUpdateMs = processPeriodicUpdates(); - } - } + // Process periodic updates + if ((xTaskGetTickCount() * portTICK_RATE_MS) >= timeToNextUpdateMs) { + timeToNextUpdateMs = processPeriodicUpdates(); + } + } } /** @@ -316,43 +316,43 @@ static void eventTask() */ static int32_t processPeriodicUpdates() { - PeriodicObjectList* objEntry; - int32_t timeNow; + PeriodicObjectList *objEntry; + int32_t timeNow; int32_t timeToNextUpdate; int32_t offset; - // Get lock - xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); // Iterate through each object and update its timer, if zero then transmit object. // Also calculate smallest delay to next update. - timeToNextUpdate = xTaskGetTickCount()*portTICK_RATE_MS + MAX_UPDATE_PERIOD_MS; + timeToNextUpdate = xTaskGetTickCount() * portTICK_RATE_MS + MAX_UPDATE_PERIOD_MS; LL_FOREACH(mObjList, objEntry) { // If object is configured for periodic updates if (objEntry->updatePeriodMs > 0) { // Check if time for the next update - timeNow = xTaskGetTickCount()*portTICK_RATE_MS; + timeNow = xTaskGetTickCount() * portTICK_RATE_MS; if (objEntry->timeToNextUpdateMs <= timeNow) { // Reset timer - offset = (timeNow - objEntry->timeToNextUpdateMs) % objEntry->updatePeriodMs; - objEntry->timeToNextUpdateMs = timeNow + objEntry->updatePeriodMs - offset; - // Invoke callback, if one - if (objEntry->evInfo.cb != 0) { - objEntry->evInfo.cb(&objEntry->evInfo.ev); // the function is expected to copy the event information - } - // Push event to queue, if one - if (objEntry->evInfo.queue != 0) { - if (xQueueSend(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0) != pdTRUE ) { // do not block if queue is full - if (objEntry->evInfo.ev.obj != NULL) { - mStats.lastErrorID = UAVObjGetID(objEntry->evInfo.ev.obj); - } - ++mStats.eventErrors; - } - } + offset = (timeNow - objEntry->timeToNextUpdateMs) % objEntry->updatePeriodMs; + objEntry->timeToNextUpdateMs = timeNow + objEntry->updatePeriodMs - offset; + // Invoke callback, if one + if (objEntry->evInfo.cb != 0) { + objEntry->evInfo.cb(&objEntry->evInfo.ev); // the function is expected to copy the event information + } + // Push event to queue, if one + if (objEntry->evInfo.queue != 0) { + if (xQueueSend(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0) != pdTRUE) { // do not block if queue is full + if (objEntry->evInfo.ev.obj != NULL) { + mStats.lastErrorID = UAVObjGetID(objEntry->evInfo.ev.obj); + } + ++mStats.eventErrors; + } + } } // Update minimum delay if (objEntry->timeToNextUpdateMs < timeToNextUpdate) { - timeToNextUpdate = objEntry->timeToNextUpdateMs; + timeToNextUpdate = objEntry->timeToNextUpdateMs; } } } @@ -369,14 +369,16 @@ static int32_t processPeriodicUpdates() */ static uint16_t randomizePeriod(uint16_t periodMs) { - static uint32_t seed = 1; - uint32_t hi, lo; - lo = 16807 * (seed & 0xFFFF); - hi = 16807 * (seed >> 16); - lo += (hi & 0x7FFF) << 16; - lo += hi >> 15; - if (lo > 0x7FFFFFFF) lo -= 0x7FFFFFFF; - seed = lo; - return (uint16_t)( ((float)periodMs * (float)lo) / (float)0x7FFFFFFF ); -} + static uint32_t seed = 1; + uint32_t hi, lo; + lo = 16807 * (seed & 0xFFFF); + hi = 16807 * (seed >> 16); + lo += (hi & 0x7FFF) << 16; + lo += hi >> 15; + if (lo > 0x7FFFFFFF) { + lo -= 0x7FFFFFFF; + } + seed = lo; + return (uint16_t)(((float)periodMs * (float)lo) / (float)0x7FFFFFFF); +} diff --git a/flight/uavobjects/inc/callbackscheduler.h b/flight/uavobjects/inc/callbackscheduler.h index e989d847f..8dfd7b682 100644 --- a/flight/uavobjects/inc/callbackscheduler.h +++ b/flight/uavobjects/inc/callbackscheduler.h @@ -27,11 +27,11 @@ #define CALLBACKSCHEDULER_H // Public types -typedef enum{ - CALLBACK_PRIORITY_CRITICAL = 0, - CALLBACK_PRIORITY_REGULAR = 1, - CALLBACK_PRIORITY_LOW = 2 - } DelayedCallbackPriority; +typedef enum { + CALLBACK_PRIORITY_CRITICAL = 0, + CALLBACK_PRIORITY_REGULAR = 1, + CALLBACK_PRIORITY_LOW = 2 +} DelayedCallbackPriority; // Use the CallbackPriority to define how frequent a callback needs to be // called in relation to others in the same callback scheduling task. // The scheduler will call callbacks waiting for execution with the same @@ -56,12 +56,12 @@ typedef enum{ // a low priority callback can block a critical one from being executed. // Callbacks MUST NOT block execution! -typedef enum{ - CALLBACK_TASK_AUXILIARY = (tskIDLE_PRIORITY + 1), - CALLBACK_TASK_NAVIGATION = (tskIDLE_PRIORITY + 2), - CALLBACK_TASK_FLIGHTCONTROL = (tskIDLE_PRIORITY + 3), - CALLBACK_TASK_DEVICEDRIVER = (tskIDLE_PRIORITY + 4), - } DelayedCallbackPriorityTask; +typedef enum { + CALLBACK_TASK_AUXILIARY = (tskIDLE_PRIORITY + 1), + CALLBACK_TASK_NAVIGATION = (tskIDLE_PRIORITY + 2), + CALLBACK_TASK_FLIGHTCONTROL = (tskIDLE_PRIORITY + 3), + CALLBACK_TASK_DEVICEDRIVER = (tskIDLE_PRIORITY + 4), +} DelayedCallbackPriorityTask; // Use the PriorityTask to define the global importance of callback execution // compared to other processes in the system. // Callbacks dispatched in a higher PriorityTasks will halt the execution of @@ -78,12 +78,12 @@ typedef enum{ // WARNING: Any higher priority task can prevent lower priority code from being // executed! (This does not only apply to callbacks but to all FreeRTOS tasks!) -typedef enum{ - CALLBACK_UPDATEMODE_NONE = 0, - CALLBACK_UPDATEMODE_SOONER = 1, - CALLBACK_UPDATEMODE_LATER = 2, - CALLBACK_UPDATEMODE_OVERRIDE = 3 - } DelayedCallbackUpdateMode; +typedef enum { + CALLBACK_UPDATEMODE_NONE = 0, + CALLBACK_UPDATEMODE_SOONER = 1, + CALLBACK_UPDATEMODE_LATER = 2, + CALLBACK_UPDATEMODE_OVERRIDE = 3 +} DelayedCallbackUpdateMode; // When scheduling a callback for execution at a time in the future, use the // update mode to define what should happen if the callback is already // scheduled. @@ -138,11 +138,10 @@ int32_t CallbackSchedulerStart(); * \return CallbackInfo Pointer on success, NULL if failed. */ DelayedCallbackInfo *DelayedCallbackCreate( - DelayedCallback cb, - DelayedCallbackPriority priority, - DelayedCallbackPriorityTask priorityTask, - uint32_t stacksize - ); + DelayedCallback cb, + DelayedCallbackPriority priority, + DelayedCallbackPriorityTask priorityTask, + uint32_t stacksize); /** * Schedule dispatching a callback at some point in the future. The function returns immediately. @@ -157,10 +156,9 @@ DelayedCallbackInfo *DelayedCallbackCreate( * \return 0: not scheduled, previous schedule takes precedence, 1: new schedule, 2: previous schedule overridden */ int32_t DelayedCallbackSchedule( - DelayedCallbackInfo *cbinfo, - int32_t milliseconds, - DelayedCallbackUpdateMode updatemode - ); + DelayedCallbackInfo *cbinfo, + int32_t milliseconds, + DelayedCallbackUpdateMode updatemode); /** * Dispatch an event by invoking the supplied callback. The function @@ -168,7 +166,7 @@ int32_t DelayedCallbackSchedule( * \param[in] *cbinfo the callback handle * \return Success (-1), failure (0) */ -int32_t DelayedCallbackDispatch( DelayedCallbackInfo *cbinfo ); +int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo); /** * Dispatch an event by invoking the supplied callback. The function @@ -184,6 +182,6 @@ int32_t DelayedCallbackDispatch( DelayedCallbackInfo *cbinfo ); * Check the demo task for your port to find the syntax required. * \return Success (-1), failure (0) */ -int32_t DelayedCallbackDispatchFromISR( DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken ); +int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken); #endif // CALLBACKSCHEDULER_H diff --git a/flight/uavobjects/inc/eventdispatcher.h b/flight/uavobjects/inc/eventdispatcher.h index 258312624..07895fccc 100644 --- a/flight/uavobjects/inc/eventdispatcher.h +++ b/flight/uavobjects/inc/eventdispatcher.h @@ -31,18 +31,18 @@ * Event dispatcher statistics */ typedef struct { - uint32_t lastErrorID; - uint32_t eventErrors; + uint32_t lastErrorID; + uint32_t eventErrors; } EventStats; // Public functions int32_t EventDispatcherInitialize(); -void EventGetStats(EventStats* statsOut); +void EventGetStats(EventStats *statsOut); void EventClearStats(); -int32_t EventCallbackDispatch(UAVObjEvent* ev, UAVObjEventCallback cb); -int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, uint16_t periodMs); -int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, uint16_t periodMs); -int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, uint16_t periodMs); -int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, uint16_t periodMs); +int32_t EventCallbackDispatch(UAVObjEvent *ev, UAVObjEventCallback cb); +int32_t EventPeriodicCallbackCreate(UAVObjEvent *ev, UAVObjEventCallback cb, uint16_t periodMs); +int32_t EventPeriodicCallbackUpdate(UAVObjEvent *ev, UAVObjEventCallback cb, uint16_t periodMs); +int32_t EventPeriodicQueueCreate(UAVObjEvent *ev, xQueueHandle queue, uint16_t periodMs); +int32_t EventPeriodicQueueUpdate(UAVObjEvent *ev, xQueueHandle queue, uint16_t periodMs); #endif // EVENTDISPATCHER_H diff --git a/flight/uavobjects/inc/uavobjectmanager.h b/flight/uavobjects/inc/uavobjectmanager.h index 53183f497..211b7bfc9 100644 --- a/flight/uavobjects/inc/uavobjectmanager.h +++ b/flight/uavobjects/inc/uavobjectmanager.h @@ -1,11 +1,11 @@ /** ****************************************************************************** * @addtogroup UAVObjects OpenPilot UAVObjects - * @{ - * @addtogroup UAV Object Manager + * @{ + * @addtogroup UAV Object Manager * @brief The core UAV Objects functions, most of which are wrappered by * autogenerated defines - * @{ + * @{ * * @file uavobjectmanager.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -32,30 +32,30 @@ #ifndef UAVOBJECTMANAGER_H #define UAVOBJECTMANAGER_H -#define UAVOBJ_ALL_INSTANCES 0xFFFF -#define UAVOBJ_MAX_INSTANCES 1000 +#define UAVOBJ_ALL_INSTANCES 0xFFFF +#define UAVOBJ_MAX_INSTANCES 1000 /* * Shifts and masks used to read/write metadata flags. */ -#define UAVOBJ_ACCESS_SHIFT 0 -#define UAVOBJ_GCS_ACCESS_SHIFT 1 -#define UAVOBJ_TELEMETRY_ACKED_SHIFT 2 -#define UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT 3 -#define UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT 4 +#define UAVOBJ_ACCESS_SHIFT 0 +#define UAVOBJ_GCS_ACCESS_SHIFT 1 +#define UAVOBJ_TELEMETRY_ACKED_SHIFT 2 +#define UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT 3 +#define UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT 4 #define UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT 6 -#define UAVOBJ_UPDATE_MODE_MASK 0x3 +#define UAVOBJ_UPDATE_MODE_MASK 0x3 -typedef void* UAVObjHandle; +typedef void *UAVObjHandle; /** * Object update mode, used by multiple modules (e.g. telemetry and logger) */ typedef enum { - UPDATEMODE_MANUAL = 0, /** Manually update object, by calling the updated() function */ - UPDATEMODE_PERIODIC = 1, /** Automatically update object at periodic intervals */ - UPDATEMODE_ONCHANGE = 2, /** Only update object when its data changes */ - UPDATEMODE_THROTTLED = 3 /** Object is updated on change, but not more often than the interval time */ + UPDATEMODE_MANUAL = 0, /** Manually update object, by calling the updated() function */ + UPDATEMODE_PERIODIC = 1, /** Automatically update object at periodic intervals */ + UPDATEMODE_ONCHANGE = 2, /** Only update object when its data changes */ + UPDATEMODE_THROTTLED = 3 /** Object is updated on change, but not more often than the interval time */ } UAVObjUpdateMode; /** @@ -75,45 +75,45 @@ typedef enum { * 6-7 gcsTelemetryUpdateMode Update mode used by the GCS (UAVObjUpdateMode) */ typedef struct { - uint8_t flags; /** Defines flags for update and logging modes and whether an update should be ACK'd (bits defined above) */ - uint16_t telemetryUpdatePeriod; /** Update period used by the telemetry module (only if telemetry mode is PERIODIC) */ - uint16_t gcsTelemetryUpdatePeriod; /** Update period used by the GCS (only if telemetry mode is PERIODIC) */ - uint16_t loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */ + uint8_t flags; /** Defines flags for update and logging modes and whether an update should be ACK'd (bits defined above) */ + uint16_t telemetryUpdatePeriod; /** Update period used by the telemetry module (only if telemetry mode is PERIODIC) */ + uint16_t gcsTelemetryUpdatePeriod; /** Update period used by the GCS (only if telemetry mode is PERIODIC) */ + uint16_t loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */ } __attribute__((packed)) UAVObjMetadata; /** * Event types generated by the objects. */ typedef enum { - EV_NONE = 0x00, /** No event */ - EV_UNPACKED = 0x01, /** Object data updated by unpacking */ - EV_UPDATED = 0x02, /** Object data updated by changing the data structure */ - EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */ - EV_UPDATED_PERIODIC = 0x08, /** Object update from periodic event */ - EV_UPDATE_REQ = 0x10 /** Request to update object data */ + EV_NONE = 0x00, /** No event */ + EV_UNPACKED = 0x01, /** Object data updated by unpacking */ + EV_UPDATED = 0x02, /** Object data updated by changing the data structure */ + EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */ + EV_UPDATED_PERIODIC = 0x08, /** Object update from periodic event */ + EV_UPDATE_REQ = 0x10 /** Request to update object data */ } UAVObjEventType; /** * Helper macros for event masks */ -#define EV_MASK_ALL 0 +#define EV_MASK_ALL 0 #define EV_MASK_ALL_UPDATES (EV_UNPACKED | EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATED_PERIODIC) /** * Access types */ typedef enum { - ACCESS_READWRITE = 0, - ACCESS_READONLY = 1 + ACCESS_READWRITE = 0, + ACCESS_READONLY = 1 } UAVObjAccessType; /** * Event message, this structure is sent in the event queue each time an event is generated */ typedef struct { - UAVObjHandle obj; - uint16_t instId; - UAVObjEventType event; + UAVObjHandle obj; + uint16_t instId; + UAVObjEventType event; } UAVObjEvent; /** @@ -121,7 +121,7 @@ typedef struct { * will be executed in the event task. The ev parameter should be copied if needed * after the function returns. */ -typedef void (*UAVObjEventCallback)(UAVObjEvent* ev); +typedef void (*UAVObjEventCallback)(UAVObjEvent *ev); /** * Callback used to initialize the object fields to their default values. @@ -132,17 +132,17 @@ typedef void (*UAVObjInitializeCallback)(UAVObjHandle obj_handle, uint16_t instI * Event manager statistics */ typedef struct { - uint32_t eventQueueErrors; - uint32_t eventCallbackErrors; - uint32_t lastCallbackErrorID; - uint32_t lastQueueErrorID; + uint32_t eventQueueErrors; + uint32_t eventCallbackErrors; + uint32_t lastCallbackErrorID; + uint32_t lastQueueErrorID; } UAVObjStats; int32_t UAVObjInitialize(); -void UAVObjGetStats(UAVObjStats* statsOut); +void UAVObjGetStats(UAVObjStats *statsOut); void UAVObjClearStats(); UAVObjHandle UAVObjRegister(uint32_t id, - int32_t isSingleInstance, int32_t isSettings, uint32_t numBytes, UAVObjInitializeCallback initCb); + int32_t isSingleInstance, int32_t isSettings, uint32_t numBytes, UAVObjInitializeCallback initCb); UAVObjHandle UAVObjGetByID(uint32_t id); uint32_t UAVObjGetID(UAVObjHandle obj); uint32_t UAVObjGetNumBytes(UAVObjHandle obj); @@ -152,14 +152,14 @@ uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle, UAVObjInitializeCallback bool UAVObjIsSingleInstance(UAVObjHandle obj); bool UAVObjIsMetaobject(UAVObjHandle obj); bool UAVObjIsSettings(UAVObjHandle obj); -int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t* dataIn); -int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t* dataOut); +int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn); +int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t *dataOut); int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId); int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId); int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId); #if defined(PIOS_INCLUDE_SDCARD) -int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId, FILEINFO* file); -UAVObjHandle UAVObjLoadFromFile(FILEINFO* file); +int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId, FILEINFO *file); +UAVObjHandle UAVObjLoadFromFile(FILEINFO *file); #endif int32_t UAVObjSaveSettings(); int32_t UAVObjLoadSettings(); @@ -167,29 +167,29 @@ int32_t UAVObjDeleteSettings(); int32_t UAVObjSaveMetaobjects(); int32_t UAVObjLoadMetaobjects(); int32_t UAVObjDeleteMetaobjects(); -int32_t UAVObjSetData(UAVObjHandle obj_handle, const void* dataIn); -int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void* dataIn, uint32_t offset, uint32_t size); -int32_t UAVObjGetData(UAVObjHandle obj_handle, void* dataOut); -int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void* dataOut, uint32_t offset, uint32_t size); -int32_t UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId, const void* dataIn); -int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size); -int32_t UAVObjGetInstanceData(UAVObjHandle obj_handle, uint16_t instId, void* dataOut); -int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size); -int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata* dataIn); -int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata* dataOut); -uint8_t UAVObjGetMetadataAccess(const UAVObjMetadata* dataOut); -UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* dataOut); -void UAVObjSetAccess(UAVObjMetadata* dataOut, UAVObjAccessType mode); -UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* dataOut); -void UAVObjSetGcsAccess(UAVObjMetadata* dataOut, UAVObjAccessType mode); -uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata* dataOut); -void UAVObjSetTelemetryAcked( UAVObjMetadata* dataOut, uint8_t val); -uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata* dataOut); -void UAVObjSetGcsTelemetryAcked(UAVObjMetadata* dataOut, uint8_t val); -UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata* dataOut); -void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* dataOut, UAVObjUpdateMode val); -UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* dataOut); -void UAVObjSetTelemetryGcsUpdateMode(UAVObjMetadata* dataOut, UAVObjUpdateMode val); +int32_t UAVObjSetData(UAVObjHandle obj_handle, const void *dataIn); +int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void *dataIn, uint32_t offset, uint32_t size); +int32_t UAVObjGetData(UAVObjHandle obj_handle, void *dataOut); +int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void *dataOut, uint32_t offset, uint32_t size); +int32_t UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId, const void *dataIn); +int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void *dataIn, uint32_t offset, uint32_t size); +int32_t UAVObjGetInstanceData(UAVObjHandle obj_handle, uint16_t instId, void *dataOut); +int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, void *dataOut, uint32_t offset, uint32_t size); +int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata *dataIn); +int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata *dataOut); +uint8_t UAVObjGetMetadataAccess(const UAVObjMetadata *dataOut); +UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata *dataOut); +void UAVObjSetAccess(UAVObjMetadata *dataOut, UAVObjAccessType mode); +UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata *dataOut); +void UAVObjSetGcsAccess(UAVObjMetadata *dataOut, UAVObjAccessType mode); +uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata *dataOut); +void UAVObjSetTelemetryAcked(UAVObjMetadata *dataOut, uint8_t val); +uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata *dataOut); +void UAVObjSetGcsTelemetryAcked(UAVObjMetadata *dataOut, uint8_t val); +UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata *dataOut); +void UAVObjSetTelemetryUpdateMode(UAVObjMetadata *dataOut, UAVObjUpdateMode val); +UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata *dataOut); +void UAVObjSetTelemetryGcsUpdateMode(UAVObjMetadata *dataOut, UAVObjUpdateMode val); int8_t UAVObjReadOnly(UAVObjHandle obj); int32_t UAVObjConnectQueue(UAVObjHandle obj_handle, xQueueHandle queue, uint8_t eventMask); int32_t UAVObjDisconnectQueue(UAVObjHandle obj_handle, xQueueHandle queue); diff --git a/flight/uavobjects/inc/utlist.h b/flight/uavobjects/inc/utlist.h index 40ebbc687..ef833289f 100644 --- a/flight/uavobjects/inc/utlist.h +++ b/flight/uavobjects/inc/utlist.h @@ -1,32 +1,32 @@ /* -Copyright (c) 2007-2009, Troy D. Hanson -All rights reserved. + Copyright (c) 2007-2009, Troy D. Hanson + All rights reserved. -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright + * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. -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. -*/ + 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. + */ #ifndef UTLIST_H #define UTLIST_H #define UTLIST_VERSION 1.8 -/* +/* * This file contains macros to manipulate singly and doubly-linked lists. * * 1. LL_ macros: singly-linked lists. @@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * To use singly-linked lists, your structure must have a "next" pointer. * To use doubly-linked lists, your structure must "prev" and "next" pointers. * Either way, the pointer to the head of the list must be initialized to NULL. - * + * * ----------------.EXAMPLE ------------------------- * struct item { * int id; @@ -62,288 +62,319 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * Unwieldy variable names used here to avoid shadowing passed-in variables. * *****************************************************************************/ #define LL_SORT(list, cmp) \ -do { \ - __typeof__(list) _ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \ - int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ - if (list) { \ - _ls_insize = 1; \ - _ls_looping = 1; \ - while (_ls_looping) { \ - _ls_p = list; \ - _ls_oldhead = list; \ - list = NULL; \ - _ls_tail = NULL; \ - _ls_nmerges = 0; \ - while (_ls_p) { \ - _ls_nmerges++; \ - _ls_q = _ls_p; \ - _ls_psize = 0; \ - for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ - _ls_psize++; \ - _ls_q = _ls_q->next; \ - if (!_ls_q) break; \ - } \ - _ls_qsize = _ls_insize; \ - while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ - if (_ls_psize == 0) { \ - _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ - } else if (_ls_qsize == 0 || !_ls_q) { \ - _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ - } else if (cmp(_ls_p,_ls_q) <= 0) { \ - _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ - } else { \ - _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ - } \ - if (_ls_tail) { \ - _ls_tail->next = _ls_e; \ - } else { \ - list = _ls_e; \ - } \ - _ls_tail = _ls_e; \ - } \ - _ls_p = _ls_q; \ - } \ - _ls_tail->next = NULL; \ - if (_ls_nmerges <= 1) { \ - _ls_looping=0; \ - } \ - _ls_insize *= 2; \ - } \ - } \ -} while (0) + do { \ + __typeof__(list)_ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \ + int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ + if (list) { \ + _ls_insize = 1; \ + _ls_looping = 1; \ + while (_ls_looping) { \ + _ls_p = list; \ + _ls_oldhead = list; \ + list = NULL; \ + _ls_tail = NULL; \ + _ls_nmerges = 0; \ + while (_ls_p) { \ + _ls_nmerges++; \ + _ls_q = _ls_p; \ + _ls_psize = 0; \ + for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ + _ls_psize++; \ + _ls_q = _ls_q->next; \ + if (!_ls_q) { break; } \ + } \ + _ls_qsize = _ls_insize; \ + while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ + if (_ls_psize == 0) { \ + _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ + } \ + else if (_ls_qsize == 0 || !_ls_q) { \ + _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ + } \ + else if (cmp(_ls_p, _ls_q) <= 0) { \ + _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ + } \ + else { \ + _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ + } \ + if (_ls_tail) { \ + _ls_tail->next = _ls_e; \ + } \ + else { \ + list = _ls_e; \ + } \ + _ls_tail = _ls_e; \ + } \ + _ls_p = _ls_q; \ + } \ + _ls_tail->next = NULL; \ + if (_ls_nmerges <= 1) { \ + _ls_looping = 0; \ + } \ + _ls_insize *= 2; \ + } \ + } \ + } \ + while (0) #define DL_SORT(list, cmp) \ -do { \ - __typeof__(list) _ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \ - int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ - if (list) { \ - _ls_insize = 1; \ - _ls_looping = 1; \ - while (_ls_looping) { \ - _ls_p = list; \ - _ls_oldhead = list; \ - list = NULL; \ - _ls_tail = NULL; \ - _ls_nmerges = 0; \ - while (_ls_p) { \ - _ls_nmerges++; \ - _ls_q = _ls_p; \ - _ls_psize = 0; \ - for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ - _ls_psize++; \ - _ls_q = _ls_q->next; \ - if (!_ls_q) break; \ - } \ - _ls_qsize = _ls_insize; \ - while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ - if (_ls_psize == 0) { \ - _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ - } else if (_ls_qsize == 0 || !_ls_q) { \ - _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ - } else if (cmp(_ls_p,_ls_q) <= 0) { \ - _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ - } else { \ - _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ - } \ - if (_ls_tail) { \ - _ls_tail->next = _ls_e; \ - } else { \ - list = _ls_e; \ - } \ - _ls_e->prev = _ls_tail; \ - _ls_tail = _ls_e; \ - } \ - _ls_p = _ls_q; \ - } \ - list->prev = _ls_tail; \ - _ls_tail->next = NULL; \ - if (_ls_nmerges <= 1) { \ - _ls_looping=0; \ - } \ - _ls_insize *= 2; \ - } \ - } \ -} while (0) + do { \ + __typeof__(list)_ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \ + int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ + if (list) { \ + _ls_insize = 1; \ + _ls_looping = 1; \ + while (_ls_looping) { \ + _ls_p = list; \ + _ls_oldhead = list; \ + list = NULL; \ + _ls_tail = NULL; \ + _ls_nmerges = 0; \ + while (_ls_p) { \ + _ls_nmerges++; \ + _ls_q = _ls_p; \ + _ls_psize = 0; \ + for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ + _ls_psize++; \ + _ls_q = _ls_q->next; \ + if (!_ls_q) { break; } \ + } \ + _ls_qsize = _ls_insize; \ + while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ + if (_ls_psize == 0) { \ + _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ + } \ + else if (_ls_qsize == 0 || !_ls_q) { \ + _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ + } \ + else if (cmp(_ls_p, _ls_q) <= 0) { \ + _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ + } \ + else { \ + _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ + } \ + if (_ls_tail) { \ + _ls_tail->next = _ls_e; \ + } \ + else { \ + list = _ls_e; \ + } \ + _ls_e->prev = _ls_tail; \ + _ls_tail = _ls_e; \ + } \ + _ls_p = _ls_q; \ + } \ + list->prev = _ls_tail; \ + _ls_tail->next = NULL; \ + if (_ls_nmerges <= 1) { \ + _ls_looping = 0; \ + } \ + _ls_insize *= 2; \ + } \ + } \ + } \ + while (0) #define CDL_SORT(list, cmp) \ -do { \ - __typeof__(list) _ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \ - int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ - if (list) { \ - _ls_insize = 1; \ - _ls_looping = 1; \ - while (_ls_looping) { \ - _ls_p = list; \ - _ls_oldhead = list; \ - list = NULL; \ - _ls_tail = NULL; \ - _ls_nmerges = 0; \ - while (_ls_p) { \ - _ls_nmerges++; \ - _ls_q = _ls_p; \ - _ls_psize = 0; \ - for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ - _ls_psize++; \ - _ls_q = ((_ls_q->next == _ls_oldhead) ? NULL : _ls_q->next); \ - if (!_ls_q) break; \ - } \ - _ls_qsize = _ls_insize; \ - while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ - if (_ls_psize == 0) { \ - _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ - if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \ - } else if (_ls_qsize == 0 || !_ls_q) { \ - _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ - if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \ - } else if (cmp(_ls_p,_ls_q) <= 0) { \ - _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ - if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \ - } else { \ - _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ - if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \ - } \ - if (_ls_tail) { \ - _ls_tail->next = _ls_e; \ - } else { \ - list = _ls_e; \ - } \ - _ls_e->prev = _ls_tail; \ - _ls_tail = _ls_e; \ - } \ - _ls_p = _ls_q; \ - } \ - list->prev = _ls_tail; \ - _ls_tail->next = list; \ - if (_ls_nmerges <= 1) { \ - _ls_looping=0; \ - } \ - _ls_insize *= 2; \ - } \ - } \ -} while (0) + do { \ + __typeof__(list)_ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \ + int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ + if (list) { \ + _ls_insize = 1; \ + _ls_looping = 1; \ + while (_ls_looping) { \ + _ls_p = list; \ + _ls_oldhead = list; \ + list = NULL; \ + _ls_tail = NULL; \ + _ls_nmerges = 0; \ + while (_ls_p) { \ + _ls_nmerges++; \ + _ls_q = _ls_p; \ + _ls_psize = 0; \ + for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ + _ls_psize++; \ + _ls_q = ((_ls_q->next == _ls_oldhead) ? NULL : _ls_q->next); \ + if (!_ls_q) { break; } \ + } \ + _ls_qsize = _ls_insize; \ + while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ + if (_ls_psize == 0) { \ + _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ + if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \ + } \ + else if (_ls_qsize == 0 || !_ls_q) { \ + _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ + if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \ + } \ + else if (cmp(_ls_p, _ls_q) <= 0) { \ + _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ + if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \ + } \ + else { \ + _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ + if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \ + } \ + if (_ls_tail) { \ + _ls_tail->next = _ls_e; \ + } \ + else { \ + list = _ls_e; \ + } \ + _ls_e->prev = _ls_tail; \ + _ls_tail = _ls_e; \ + } \ + _ls_p = _ls_q; \ + } \ + list->prev = _ls_tail; \ + _ls_tail->next = list; \ + if (_ls_nmerges <= 1) { \ + _ls_looping = 0; \ + } \ + _ls_insize *= 2; \ + } \ + } \ + } \ + while (0) /****************************************************************************** * singly linked list macros (non-circular) * *****************************************************************************/ -#define LL_PREPEND(head,add) \ -do { \ - (add)->next = head; \ - head = add; \ -} while (0) +#define LL_PREPEND(head, add) \ + do { \ + (add)->next = head; \ + head = add; \ + } \ + while (0) -#define LL_APPEND(head,add) \ -do { \ - __typeof__(head) _tmp; \ - (add)->next=NULL; \ - if (head) { \ - _tmp = head; \ - while (_tmp->next) { _tmp = _tmp->next; } \ - _tmp->next=(add); \ - } else { \ - (head)=(add); \ - } \ -} while (0) +#define LL_APPEND(head, add) \ + do { \ + __typeof__(head)_tmp; \ + (add)->next = NULL; \ + if (head) { \ + _tmp = head; \ + while (_tmp->next) { _tmp = _tmp->next; } \ + _tmp->next = (add); \ + } \ + else { \ + (head) = (add); \ + } \ + } \ + while (0) -#define LL_DELETE(head,del) \ -do { \ - __typeof__(head) _tmp; \ - if ((head) == (del)) { \ - (head)=(head)->next; \ - } else { \ - _tmp = head; \ - while (_tmp->next && (_tmp->next != (del))) { \ - _tmp = _tmp->next; \ - } \ - if (_tmp->next) { \ - _tmp->next = ((del)->next); \ - } \ - } \ -} while (0) +#define LL_DELETE(head, del) \ + do { \ + __typeof__(head)_tmp; \ + if ((head) == (del)) { \ + (head) = (head)->next; \ + } \ + else { \ + _tmp = head; \ + while (_tmp->next && (_tmp->next != (del))) { \ + _tmp = _tmp->next; \ + } \ + if (_tmp->next) { \ + _tmp->next = ((del)->next); \ + } \ + } \ + } \ + while (0) -#define LL_FOREACH(head,el) \ - for(el=head;el;el=el->next) +#define LL_FOREACH(head, el) \ + for (el = head; el; el = el->next) /****************************************************************************** * doubly linked list macros (non-circular) * *****************************************************************************/ -#define DL_PREPEND(head,add) \ -do { \ - (add)->next = head; \ - if (head) { \ - (add)->prev = (head)->prev; \ - (head)->prev = (add); \ - } else { \ - (add)->prev = (add); \ - } \ - (head) = (add); \ -} while (0) +#define DL_PREPEND(head, add) \ + do { \ + (add)->next = head; \ + if (head) { \ + (add)->prev = (head)->prev; \ + (head)->prev = (add); \ + } \ + else { \ + (add)->prev = (add); \ + } \ + (head) = (add); \ + } \ + while (0) -#define DL_APPEND(head,add) \ -do { \ - if (head) { \ - (add)->prev = (head)->prev; \ - (head)->prev->next = (add); \ - (head)->prev = (add); \ - (add)->next = NULL; \ - } else { \ - (head)=(add); \ - (head)->prev = (head); \ - (head)->next = NULL; \ - } \ -} while (0); +#define DL_APPEND(head, add) \ + do { \ + if (head) { \ + (add)->prev = (head)->prev; \ + (head)->prev->next = (add); \ + (head)->prev = (add); \ + (add)->next = NULL; \ + } \ + else { \ + (head) = (add); \ + (head)->prev = (head); \ + (head)->next = NULL; \ + } \ + } \ + while (0); -#define DL_DELETE(head,del) \ -do { \ - if ((del)->prev == (del)) { \ - (head)=NULL; \ - } else if ((del)==(head)) { \ - (del)->next->prev = (del)->prev; \ - (head) = (del)->next; \ - } else { \ - (del)->prev->next = (del)->next; \ - if ((del)->next) { \ - (del)->next->prev = (del)->prev; \ - } else { \ - (head)->prev = (del)->prev; \ - } \ - } \ -} while (0); +#define DL_DELETE(head, del) \ + do { \ + if ((del)->prev == (del)) { \ + (head) = NULL; \ + } \ + else if ((del) == (head)) { \ + (del)->next->prev = (del)->prev; \ + (head) = (del)->next; \ + } \ + else { \ + (del)->prev->next = (del)->next; \ + if ((del)->next) { \ + (del)->next->prev = (del)->prev; \ + } \ + else { \ + (head)->prev = (del)->prev; \ + } \ + } \ + } \ + while (0); -#define DL_FOREACH(head,el) \ - for(el=head;el;el=el->next) +#define DL_FOREACH(head, el) \ + for (el = head; el; el = el->next) /****************************************************************************** * circular doubly linked list macros * *****************************************************************************/ -#define CDL_PREPEND(head,add) \ -do { \ - if (head) { \ - (add)->prev = (head)->prev; \ - (add)->next = (head); \ - (head)->prev = (add); \ - (add)->prev->next = (add); \ - } else { \ - (add)->prev = (add); \ - (add)->next = (add); \ - } \ -(head)=(add); \ -} while (0) +#define CDL_PREPEND(head, add) \ + do { \ + if (head) { \ + (add)->prev = (head)->prev; \ + (add)->next = (head); \ + (head)->prev = (add); \ + (add)->prev->next = (add); \ + } \ + else { \ + (add)->prev = (add); \ + (add)->next = (add); \ + } \ + (head) = (add); \ + } \ + while (0) -#define CDL_DELETE(head,del) \ -do { \ - if ( ((head)==(del)) && ((head)->next == (head))) { \ - (head) = 0L; \ - } else { \ - (del)->next->prev = (del)->prev; \ - (del)->prev->next = (del)->next; \ - if ((del) == (head)) (head)=(del)->next; \ - } \ -} while (0); +#define CDL_DELETE(head, del) \ + do { \ + if (((head) == (del)) && ((head)->next == (head))) { \ + (head) = 0L; \ + } \ + else { \ + (del)->next->prev = (del)->prev; \ + (del)->prev->next = (del)->next; \ + if ((del) == (head)) { (head) = (del)->next; } \ + } \ + } \ + while (0); -#define CDL_FOREACH(head,el) \ - for(el=head;el;el= (el->next==head ? 0L : el->next)) +#define CDL_FOREACH(head, el) \ + for (el = head; el; el = (el->next == head ? 0L : el->next)) #endif /* UTLIST_H */ - diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c index d4a22ce10..c09282d95 100644 --- a/flight/uavobjects/uavobjectmanager.c +++ b/flight/uavobjects/uavobjectmanager.c @@ -1,20 +1,20 @@ /** -****************************************************************************** -* @addtogroup UAVObjects OpenPilot UAVObjects -* @{ -* @addtogroup UAV Object Manager -* @brief The core UAV Objects functions, most of which are wrappered by -* autogenerated defines -* @{ -* -* -* @file uavobjectmanager.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. -* @brief Object manager library. This library holds a collection of all objects. -* It can be used by all modules/libraries to find an object reference. -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ + ****************************************************************************** + * @addtogroup UAVObjects OpenPilot UAVObjects + * @{ + * @addtogroup UAV Object Manager + * @brief The core UAV Objects functions, most of which are wrappered by + * autogenerated defines + * @{ + * + * + * @file uavobjectmanager.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Object manager library. This library holds a collection of all objects. + * It can be used by all modules/libraries to find an object reference. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -53,43 +53,43 @@ static long _aslr_offset __attribute__((section("__DATA,_aslr"))); /* Table of UAVO handles */ #if (defined(__MACH__) && defined(__APPLE__)) /* Mach-o format */ -static struct UAVOData ** __start__uavo_handles; -static struct UAVOData ** __stop__uavo_handles; +static struct UAVOData * *__start__uavo_handles; +static struct UAVOData * *__stop__uavo_handles; #else /* ELF format: automagically defined at compile time */ -extern struct UAVOData * __start__uavo_handles[] __attribute__((weak)); -extern struct UAVOData * __stop__uavo_handles[] __attribute__((weak)); +extern struct UAVOData *__start__uavo_handles[] __attribute__((weak)); +extern struct UAVOData *__stop__uavo_handles[] __attribute__((weak)); #endif #define UAVO_LIST_ITERATE(_item) \ -for (struct UAVOData ** _uavo_slot = __start__uavo_handles; \ - _uavo_slot && _uavo_slot < __stop__uavo_handles; \ - _uavo_slot++) { \ - struct UAVOData * _item = *_uavo_slot; \ - if (_item == NULL) continue; + for (struct UAVOData * *_uavo_slot = __start__uavo_handles; \ + _uavo_slot && _uavo_slot < __stop__uavo_handles; \ + _uavo_slot++) { \ + struct UAVOData *_item = *_uavo_slot; \ + if (_item == NULL) { continue; } /** * List of event queues and the eventmask associated with the queue. */ /** opaque type for instances **/ -typedef void* InstanceHandle; +typedef void *InstanceHandle; struct ObjectEventEntry { - struct ObjectEventEntry * next; - xQueueHandle queue; - UAVObjEventCallback cb; - uint8_t eventMask; + struct ObjectEventEntry *next; + xQueueHandle queue; + UAVObjEventCallback cb; + uint8_t eventMask; }; /* - MetaInstance == [UAVOBase [UAVObjMetadata]] - SingleInstance == [UAVOBase [UAVOData [InstanceData]]] - MultiInstance == [UAVOBase [UAVOData [NumInstances [InstanceData0 [next]]]] + MetaInstance == [UAVOBase [UAVObjMetadata]] + SingleInstance == [UAVOBase [UAVOData [InstanceData]]] + MultiInstance == [UAVOBase [UAVOData [NumInstances [InstanceData0 [next]]]] ____________________/ - \-->[InstanceData1 [next]] + \-->[InstanceData1 [next]] _________...________/ - \-->[InstanceDataN [next]] + \-->[InstanceDataN [next]] */ /* @@ -98,111 +98,111 @@ struct ObjectEventEntry { * - The flags determine what type(s) this object */ struct UAVOBase { - /* Let these objects be added to an event queue */ - struct ObjectEventEntry * next_event; + /* Let these objects be added to an event queue */ + struct ObjectEventEntry *next_event; - /* Describe the type of object that follows this header */ - struct UAVOInfo { - bool isMeta : 1; - bool isSingle : 1; - bool isSettings : 1; - } flags; + /* Describe the type of object that follows this header */ + struct UAVOInfo { + bool isMeta : 1; + bool isSingle : 1; + bool isSettings : 1; + } flags; } __attribute__((packed)); /* Augmented type for Meta UAVO */ struct UAVOMeta { - struct UAVOBase base; - UAVObjMetadata instance0; + struct UAVOBase base; + UAVObjMetadata instance0; } __attribute__((packed)); /* Shared data structure for all data-carrying UAVObjects (UAVOSingle and UAVOMulti) */ struct UAVOData { - struct UAVOBase base; - uint32_t id; - /* - * Embed the Meta object as another complete UAVO - * inside the payload for this UAVO. - */ - struct UAVOMeta metaObj; - uint16_t instance_size; + struct UAVOBase base; + uint32_t id; + /* + * Embed the Meta object as another complete UAVO + * inside the payload for this UAVO. + */ + struct UAVOMeta metaObj; + uint16_t instance_size; } __attribute__((packed, aligned(4))); /* Augmented type for Single Instance Data UAVO */ struct UAVOSingle { - struct UAVOData uavo; + struct UAVOData uavo; - uint8_t instance0[]; - /* - * Additional space will be malloc'd here to hold the - * the data for this instance. - */ + uint8_t instance0[]; + /* + * Additional space will be malloc'd here to hold the + * the data for this instance. + */ } __attribute__((packed)); /* Part of a linked list of instances chained off of a multi instance UAVO. */ struct UAVOMultiInst { - struct UAVOMultiInst * next; - uint8_t instance[]; - /* - * Additional space will be malloc'd here to hold the - * the data for this instance. - */ + struct UAVOMultiInst *next; + uint8_t instance[]; + /* + * Additional space will be malloc'd here to hold the + * the data for this instance. + */ } __attribute__((packed)); /* Augmented type for Multi Instance Data UAVO */ struct UAVOMulti { - struct UAVOData uavo; - uint16_t num_instances; - struct UAVOMultiInst instance0 __attribute__((aligned(4))); - /* - * Additional space will be malloc'd here to hold the - * the data for instance 0. - */ + struct UAVOData uavo; + uint16_t num_instances; + struct UAVOMultiInst instance0 __attribute__((aligned(4))); + /* + * Additional space will be malloc'd here to hold the + * the data for instance 0. + */ } __attribute__((packed)); /** all information about a metaobject are hardcoded constants **/ #define MetaNumBytes sizeof(UAVObjMetadata) -#define MetaBaseObjectPtr(obj) ((struct UAVOData *)((obj)-offsetof(struct UAVOData, metaObj))) -#define MetaObjectPtr(obj) ((struct UAVODataMeta*) &((obj)->metaObj)) -#define MetaDataPtr(obj) ((UAVObjMetadata*)&((obj)->instance0)) -#define LinkedMetaDataPtr(obj) ((UAVObjMetadata*)&((obj)->metaObj.instance0)) -#define MetaObjectId(id) ((id)+1) +#define MetaBaseObjectPtr(obj) ((struct UAVOData *)((obj) - offsetof(struct UAVOData, metaObj))) +#define MetaObjectPtr(obj) ((struct UAVODataMeta *)&((obj)->metaObj)) +#define MetaDataPtr(obj) ((UAVObjMetadata *)&((obj)->instance0)) +#define LinkedMetaDataPtr(obj) ((UAVObjMetadata *)&((obj)->metaObj.instance0)) +#define MetaObjectId(id) ((id) + 1) /** all information about instances are dependant on object type **/ -#define ObjSingleInstanceDataOffset(obj) ((void*)(&(( (struct UAVOSingle*)obj )->instance0))) -#define InstanceDataOffset(inst) ((void*)&(( (struct UAVOMultiInst*)inst )->instance)) -#define InstanceData(instance) (void*)instance +#define ObjSingleInstanceDataOffset(obj) ((void *)(&(((struct UAVOSingle *)obj)->instance0))) +#define InstanceDataOffset(inst) ((void *)&(((struct UAVOMultiInst *)inst)->instance)) +#define InstanceData(instance) (void *)instance // Private functions -static int32_t sendEvent(struct UAVOBase * obj, uint16_t instId, - UAVObjEventType event); -static InstanceHandle createInstance(struct UAVOData * obj, uint16_t instId); -static InstanceHandle getInstance(struct UAVOData * obj, uint16_t instId); +static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, + UAVObjEventType event); +static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId); +static InstanceHandle getInstance(struct UAVOData *obj, uint16_t instId); static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, - UAVObjEventCallback cb, uint8_t eventMask); + UAVObjEventCallback cb, uint8_t eventMask); static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue, - UAVObjEventCallback cb); + UAVObjEventCallback cb); #if defined(PIOS_USE_SETTINGS_ON_SDCARD) && defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) #error Both PIOS_USE_SETTINGS_ON_SDCARD and PIOS_INCLUDE_FLASH_SECTOR_SETTINGS. Only one settings storage allowed. #endif #if defined(PIOS_USE_SETTINGS_ON_SDCARD) -static void objectFilename(UAVObjHandle obj_handle, uint8_t * filename); -static void customSPrintf(uint8_t * buffer, uint8_t * format, ...); +static void objectFilename(UAVObjHandle obj_handle, uint8_t *filename); +static void customSPrintf(uint8_t *buffer, uint8_t *format, ...); #endif // Private variables static xSemaphoreHandle mutex; static const UAVObjMetadata defMetadata = { - .flags = (ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT | - ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT | - 1 << UAVOBJ_TELEMETRY_ACKED_SHIFT | - 1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT | - UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT | - UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT), - .telemetryUpdatePeriod = 0, - .gcsTelemetryUpdatePeriod = 0, - .loggingUpdatePeriod = 0, + .flags = (ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT | + ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT | + 1 << UAVOBJ_TELEMETRY_ACKED_SHIFT | + 1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT | + UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT | + UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT), + .telemetryUpdatePeriod = 0, + .gcsTelemetryUpdatePeriod = 0, + .loggingUpdatePeriod = 0, }; static UAVObjStats stats; @@ -214,27 +214,28 @@ static UAVObjStats stats; */ int32_t UAVObjInitialize() { - // Initialize variables - memset(&stats, 0, sizeof(UAVObjStats)); + // Initialize variables + memset(&stats, 0, sizeof(UAVObjStats)); - /* Initialize _uavo_handles start/stop pointers */ - #if (defined(__MACH__) && defined(__APPLE__)) - uint64_t aslr_offset = (uint64_t) & _aslr_offset - getsectbyname("__DATA","_aslr")->addr; - __start__uavo_handles = (struct UAVOData **) (getsectbyname("__DATA","_uavo_handles")->addr + aslr_offset); - __stop__uavo_handles = (struct UAVOData **) ((uint64_t)__start__uavo_handles + getsectbyname("__DATA","_uavo_handles")->size); - #endif + /* Initialize _uavo_handles start/stop pointers */ + #if (defined(__MACH__) && defined(__APPLE__)) + uint64_t aslr_offset = (uint64_t)&_aslr_offset - getsectbyname("__DATA", "_aslr")->addr; + __start__uavo_handles = (struct UAVOData * *)(getsectbyname("__DATA", "_uavo_handles")->addr + aslr_offset); + __stop__uavo_handles = (struct UAVOData * *)((uint64_t)__start__uavo_handles + getsectbyname("__DATA", "_uavo_handles")->size); + #endif - // Initialize the uavo handle table - memset(__start__uavo_handles, 0, - (uintptr_t)__stop__uavo_handles - (uintptr_t)__start__uavo_handles); + // Initialize the uavo handle table + memset(__start__uavo_handles, 0, + (uintptr_t)__stop__uavo_handles - (uintptr_t)__start__uavo_handles); - // Create mutex - mutex = xSemaphoreCreateRecursiveMutex(); - if (mutex == NULL) - return -1; + // Create mutex + mutex = xSemaphoreCreateRecursiveMutex(); + if (mutex == NULL) { + return -1; + } - // Done - return 0; + // Done + return 0; } /***************** @@ -245,11 +246,11 @@ int32_t UAVObjInitialize() * Get the statistics counters * @param[out] statsOut The statistics counters will be copied there */ -void UAVObjGetStats(UAVObjStats * statsOut) +void UAVObjGetStats(UAVObjStats *statsOut) { - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - memcpy(statsOut, &stats, sizeof(UAVObjStats)); - xSemaphoreGiveRecursive(mutex); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + memcpy(statsOut, &stats, sizeof(UAVObjStats)); + xSemaphoreGiveRecursive(mutex); } /** @@ -257,75 +258,80 @@ void UAVObjGetStats(UAVObjStats * statsOut) */ void UAVObjClearStats() { - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - memset(&stats, 0, sizeof(UAVObjStats)); - xSemaphoreGiveRecursive(mutex); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + memset(&stats, 0, sizeof(UAVObjStats)); + xSemaphoreGiveRecursive(mutex); } /************************ * Object Initialization ***********************/ -static void UAVObjInitMetaData (struct UAVOMeta * obj_meta) +static void UAVObjInitMetaData(struct UAVOMeta *obj_meta) { - /* Fill in the common part of the UAVO */ - struct UAVOBase * uavo_base = &(obj_meta->base); - memset(uavo_base, 0, sizeof(*uavo_base)); - uavo_base->flags.isMeta = true; - uavo_base->flags.isSingle = true; - uavo_base->next_event = NULL; + /* Fill in the common part of the UAVO */ + struct UAVOBase *uavo_base = &(obj_meta->base); - /* Clear the instance data carried in the UAVO */ - memset(&(obj_meta->instance0), 0, sizeof(obj_meta->instance0)); + memset(uavo_base, 0, sizeof(*uavo_base)); + uavo_base->flags.isMeta = true; + uavo_base->flags.isSingle = true; + uavo_base->next_event = NULL; + + /* Clear the instance data carried in the UAVO */ + memset(&(obj_meta->instance0), 0, sizeof(obj_meta->instance0)); } -static struct UAVOData * UAVObjAllocSingle(uint32_t num_bytes) +static struct UAVOData *UAVObjAllocSingle(uint32_t num_bytes) { - /* Compute the complete size of the object, including the data for a single embedded instance */ - uint32_t object_size = sizeof(struct UAVOSingle) + num_bytes; + /* Compute the complete size of the object, including the data for a single embedded instance */ + uint32_t object_size = sizeof(struct UAVOSingle) + num_bytes; - /* Allocate the object from the heap */ - struct UAVOSingle * uavo_single = (struct UAVOSingle *) pvPortMalloc(object_size); - if (!uavo_single) - return (NULL); + /* Allocate the object from the heap */ + struct UAVOSingle *uavo_single = (struct UAVOSingle *)pvPortMalloc(object_size); - /* Fill in the common part of the UAVO */ - struct UAVOBase * uavo_base = &(uavo_single->uavo.base); - memset(uavo_base, 0, sizeof(*uavo_base)); - uavo_base->flags.isSingle = true; - uavo_base->next_event = NULL; + if (!uavo_single) { + return NULL; + } - /* Clear the instance data carried in the UAVO */ - memset(&(uavo_single->instance0), 0, num_bytes); + /* Fill in the common part of the UAVO */ + struct UAVOBase *uavo_base = &(uavo_single->uavo.base); + memset(uavo_base, 0, sizeof(*uavo_base)); + uavo_base->flags.isSingle = true; + uavo_base->next_event = NULL; - /* Give back the generic UAVO part */ - return (&(uavo_single->uavo)); + /* Clear the instance data carried in the UAVO */ + memset(&(uavo_single->instance0), 0, num_bytes); + + /* Give back the generic UAVO part */ + return &(uavo_single->uavo); } -static struct UAVOData * UAVObjAllocMulti(uint32_t num_bytes) +static struct UAVOData *UAVObjAllocMulti(uint32_t num_bytes) { - /* Compute the complete size of the object, including the data for a single embedded instance */ - uint32_t object_size = sizeof(struct UAVOMulti) + num_bytes; + /* Compute the complete size of the object, including the data for a single embedded instance */ + uint32_t object_size = sizeof(struct UAVOMulti) + num_bytes; - /* Allocate the object from the heap */ - struct UAVOMulti * uavo_multi = (struct UAVOMulti *) pvPortMalloc(object_size); - if (!uavo_multi) - return (NULL); + /* Allocate the object from the heap */ + struct UAVOMulti *uavo_multi = (struct UAVOMulti *)pvPortMalloc(object_size); - /* Fill in the common part of the UAVO */ - struct UAVOBase * uavo_base = &(uavo_multi->uavo.base); - memset(uavo_base, 0, sizeof(*uavo_base)); - uavo_base->flags.isSingle = false; - uavo_base->next_event = NULL; + if (!uavo_multi) { + return NULL; + } - /* Set up the type-specific part of the UAVO */ - uavo_multi->num_instances = 1; + /* Fill in the common part of the UAVO */ + struct UAVOBase *uavo_base = &(uavo_multi->uavo.base); + memset(uavo_base, 0, sizeof(*uavo_base)); + uavo_base->flags.isSingle = false; + uavo_base->next_event = NULL; - /* Clear the multi instance data carried in the UAVO */ - memset (&(uavo_multi->instance0), 0, sizeof(struct UAVOMultiInst) + num_bytes); + /* Set up the type-specific part of the UAVO */ + uavo_multi->num_instances = 1; - /* Give back the generic UAVO part */ - return (&(uavo_multi->uavo)); + /* Clear the multi instance data carried in the UAVO */ + memset(&(uavo_multi->instance0), 0, sizeof(struct UAVOMultiInst) + num_bytes); + + /* Give back the generic UAVO part */ + return &(uavo_multi->uavo); } /************************** @@ -342,57 +348,61 @@ static struct UAVOData * UAVObjAllocMulti(uint32_t num_bytes) * \return Object handle, or NULL if failure. * \return */ -UAVObjHandle UAVObjRegister(uint32_t id, - int32_t isSingleInstance, int32_t isSettings, - uint32_t num_bytes, - UAVObjInitializeCallback initCb) +UAVObjHandle UAVObjRegister(uint32_t id, + int32_t isSingleInstance, int32_t isSettings, + uint32_t num_bytes, + UAVObjInitializeCallback initCb) { - struct UAVOData * uavo_data = NULL; + struct UAVOData *uavo_data = NULL; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - /* Don't allow duplicate registrations */ - if (UAVObjGetByID(id)) - goto unlock_exit; + /* Don't allow duplicate registrations */ + if (UAVObjGetByID(id)) { + goto unlock_exit; + } - /* Map the various flags to one of the UAVO types we understand */ - if (isSingleInstance) { - uavo_data = UAVObjAllocSingle (num_bytes); - } else { - uavo_data = UAVObjAllocMulti (num_bytes); - } + /* Map the various flags to one of the UAVO types we understand */ + if (isSingleInstance) { + uavo_data = UAVObjAllocSingle(num_bytes); + } else { + uavo_data = UAVObjAllocMulti(num_bytes); + } - if (!uavo_data) - goto unlock_exit; + if (!uavo_data) { + goto unlock_exit; + } - /* Fill in the details about this UAVO */ - uavo_data->id = id; - uavo_data->instance_size = num_bytes; - if (isSettings) { - uavo_data->base.flags.isSettings = true; - } + /* Fill in the details about this UAVO */ + uavo_data->id = id; + uavo_data->instance_size = num_bytes; + if (isSettings) { + uavo_data->base.flags.isSettings = true; + } - /* Initialize the embedded meta UAVO */ - UAVObjInitMetaData (&uavo_data->metaObj); + /* Initialize the embedded meta UAVO */ + UAVObjInitMetaData(&uavo_data->metaObj); - /* Initialize object fields and metadata to default values */ - if (initCb) - initCb((UAVObjHandle) uavo_data, 0); + /* Initialize object fields and metadata to default values */ + if (initCb) { + initCb((UAVObjHandle)uavo_data, 0); + } - /* Always try to load the meta object from flash */ - UAVObjLoad((UAVObjHandle) &(uavo_data->metaObj), 0); + /* Always try to load the meta object from flash */ + UAVObjLoad((UAVObjHandle) & (uavo_data->metaObj), 0); - /* Attempt to load settings object from flash */ - if (uavo_data->base.flags.isSettings) - UAVObjLoad((UAVObjHandle) uavo_data, 0); + /* Attempt to load settings object from flash */ + if (uavo_data->base.flags.isSettings) { + UAVObjLoad((UAVObjHandle)uavo_data, 0); + } - // fire events for outer object and its embedded meta object - UAVObjInstanceUpdated((UAVObjHandle) uavo_data, 0); - UAVObjInstanceUpdated((UAVObjHandle) &(uavo_data->metaObj), 0); + // fire events for outer object and its embedded meta object + UAVObjInstanceUpdated((UAVObjHandle)uavo_data, 0); + UAVObjInstanceUpdated((UAVObjHandle) & (uavo_data->metaObj), 0); unlock_exit: - xSemaphoreGiveRecursive(mutex); - return (UAVObjHandle) uavo_data; + xSemaphoreGiveRecursive(mutex); + return (UAVObjHandle)uavo_data; } /** @@ -402,26 +412,26 @@ unlock_exit: */ UAVObjHandle UAVObjGetByID(uint32_t id) { - UAVObjHandle * found_obj = (UAVObjHandle *) NULL; + UAVObjHandle *found_obj = (UAVObjHandle *)NULL; - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Look for object - UAVO_LIST_ITERATE(tmp_obj) - if (tmp_obj->id == id) { - found_obj = (UAVObjHandle *)tmp_obj; - goto unlock_exit; - } - if (MetaObjectId(tmp_obj->id) == id) { - found_obj = (UAVObjHandle *)&(tmp_obj->metaObj); - goto unlock_exit; - } - } + // Look for object + UAVO_LIST_ITERATE(tmp_obj) + if (tmp_obj->id == id) { + found_obj = (UAVObjHandle *)tmp_obj; + goto unlock_exit; + } + if (MetaObjectId(tmp_obj->id) == id) { + found_obj = (UAVObjHandle *)&(tmp_obj->metaObj); + goto unlock_exit; + } +} unlock_exit: - xSemaphoreGiveRecursive(mutex); - return found_obj; +xSemaphoreGiveRecursive(mutex); +return found_obj; } /** @@ -431,22 +441,22 @@ unlock_exit: */ uint32_t UAVObjGetID(UAVObjHandle obj_handle) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - /* Recover the common object header */ - struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle; + /* Recover the common object header */ + struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle; - if (UAVObjIsMetaobject(obj_handle)) { - /* We have a meta object, find our containing UAVO */ - struct UAVOData * uavo_data = container_of ((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj); + if (UAVObjIsMetaobject(obj_handle)) { + /* We have a meta object, find our containing UAVO */ + struct UAVOData *uavo_data = container_of((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj); - return MetaObjectId (uavo_data->id); - } else { - /* We have a data object, augment our pointer */ - struct UAVOData * uavo_data = (struct UAVOData *) uavo_base; + return MetaObjectId(uavo_data->id); + } else { + /* We have a data object, augment our pointer */ + struct UAVOData *uavo_data = (struct UAVOData *)uavo_base; - return (uavo_data->id); - } + return uavo_data->id; + } } /** @@ -456,23 +466,23 @@ uint32_t UAVObjGetID(UAVObjHandle obj_handle) */ uint32_t UAVObjGetNumBytes(UAVObjHandle obj) { - PIOS_Assert(obj); + PIOS_Assert(obj); - uint32_t instance_size; + uint32_t instance_size; - /* Recover the common object header */ - struct UAVOBase * uavo_base = (struct UAVOBase *) obj; + /* Recover the common object header */ + struct UAVOBase *uavo_base = (struct UAVOBase *)obj; - if (uavo_base->flags.isMeta) { - instance_size = MetaNumBytes; - } else { - /* We have a data object, augment our pointer */ - struct UAVOData * uavo = (struct UAVOData *) uavo_base; + if (uavo_base->flags.isMeta) { + instance_size = MetaNumBytes; + } else { + /* We have a data object, augment our pointer */ + struct UAVOData *uavo = (struct UAVOData *)uavo_base; - instance_size = uavo->instance_size; - } + instance_size = uavo->instance_size; + } - return (instance_size); + return instance_size; } /** @@ -484,22 +494,22 @@ uint32_t UAVObjGetNumBytes(UAVObjHandle obj) */ UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj_handle) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - /* Recover the common object header */ - struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle; + /* Recover the common object header */ + struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle; - if (UAVObjIsMetaobject(obj_handle)) { - /* We have a meta object, find our containing UAVO. */ - struct UAVOData * uavo_data = container_of ((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj); + if (UAVObjIsMetaobject(obj_handle)) { + /* We have a meta object, find our containing UAVO. */ + struct UAVOData *uavo_data = container_of((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj); - return (UAVObjHandle) uavo_data; - } else { - /* We have a data object, augment our pointer */ - struct UAVOData * uavo_data = (struct UAVOData *) uavo_base; + return (UAVObjHandle)uavo_data; + } else { + /* We have a data object, augment our pointer */ + struct UAVOData *uavo_data = (struct UAVOData *)uavo_base; - return (UAVObjHandle) &(uavo_data->metaObj); - } + return (UAVObjHandle) & (uavo_data->metaObj); + } } /** @@ -509,17 +519,17 @@ UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj_handle) */ uint16_t UAVObjGetNumInstances(UAVObjHandle obj_handle) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - if (UAVObjIsSingleInstance(obj_handle)) { - /* Only one instance is allowed */ - return 1; - } else { - /* Multi-instance object. Inspect the object */ - /* Augment our pointer to reflect the proper type */ - struct UAVOMulti * uavo_multi = (struct UAVOMulti *) obj_handle; - return uavo_multi->num_instances; - } + if (UAVObjIsSingleInstance(obj_handle)) { + /* Only one instance is allowed */ + return 1; + } else { + /* Multi-instance object. Inspect the object */ + /* Augment our pointer to reflect the proper type */ + struct UAVOMulti *uavo_multi = (struct UAVOMulti *)obj_handle; + return uavo_multi->num_instances; + } } /** @@ -528,36 +538,36 @@ uint16_t UAVObjGetNumInstances(UAVObjHandle obj_handle) * \return The instance ID or 0 if an error */ uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle, - UAVObjInitializeCallback initCb) + UAVObjInitializeCallback initCb) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - if (UAVObjIsMetaobject(obj_handle)) { - return 0; - } + if (UAVObjIsMetaobject(obj_handle)) { + return 0; + } - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - InstanceHandle instEntry; - uint16_t instId = 0; + InstanceHandle instEntry; + uint16_t instId = 0; - // Create new instance - instId = UAVObjGetNumInstances(obj_handle); - instEntry = createInstance( (struct UAVOData *)obj_handle, instId); - if (instEntry == NULL) { - goto unlock_exit; - } + // Create new instance + instId = UAVObjGetNumInstances(obj_handle); + instEntry = createInstance((struct UAVOData *)obj_handle, instId); + if (instEntry == NULL) { + goto unlock_exit; + } - // Initialize instance data - if (initCb) { - initCb(obj_handle, instId); - } + // Initialize instance data + if (initCb) { + initCb(obj_handle, instId); + } unlock_exit: - xSemaphoreGiveRecursive(mutex); + xSemaphoreGiveRecursive(mutex); - return instId; + return instId; } /** @@ -567,12 +577,12 @@ unlock_exit: */ bool UAVObjIsSingleInstance(UAVObjHandle obj_handle) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - /* Recover the common object header */ - struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle; + /* Recover the common object header */ + struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle; - return uavo_base->flags.isSingle; + return uavo_base->flags.isSingle; } /** @@ -582,12 +592,12 @@ bool UAVObjIsSingleInstance(UAVObjHandle obj_handle) */ bool UAVObjIsMetaobject(UAVObjHandle obj_handle) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - /* Recover the common object header */ - struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle; + /* Recover the common object header */ + struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle; - return uavo_base->flags.isMeta; + return uavo_base->flags.isMeta; } /** @@ -597,12 +607,12 @@ bool UAVObjIsMetaobject(UAVObjHandle obj_handle) */ bool UAVObjIsSettings(UAVObjHandle obj_handle) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - /* Recover the common object header */ - struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle; + /* Recover the common object header */ + struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle; - return uavo_base->flags.isSettings; + return uavo_base->flags.isSettings; } /** @@ -613,48 +623,48 @@ bool UAVObjIsSettings(UAVObjHandle obj_handle) * \return 0 if success or -1 if failure */ int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, - const uint8_t * dataIn) + const uint8_t *dataIn) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - if (UAVObjIsMetaobject(obj_handle)) { - if (instId != 0) { - goto unlock_exit; - } - memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes); - } else { - struct UAVOData *obj; - InstanceHandle instEntry; + if (UAVObjIsMetaobject(obj_handle)) { + if (instId != 0) { + goto unlock_exit; + } + memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes); + } else { + struct UAVOData *obj; + InstanceHandle instEntry; - // Cast handle to object - obj = (struct UAVOData *) obj_handle; + // Cast handle to object + obj = (struct UAVOData *)obj_handle; - // Get the instance - instEntry = getInstance(obj, instId); + // Get the instance + instEntry = getInstance(obj, instId); - // If the instance does not exist create it and any other instances before it - if (instEntry == NULL) { - instEntry = createInstance(obj, instId); - if (instEntry == NULL) { - goto unlock_exit; - } - } - // Set the data - memcpy(InstanceData(instEntry), dataIn, obj->instance_size); - } + // If the instance does not exist create it and any other instances before it + if (instEntry == NULL) { + instEntry = createInstance(obj, instId); + if (instEntry == NULL) { + goto unlock_exit; + } + } + // Set the data + memcpy(InstanceData(instEntry), dataIn, obj->instance_size); + } - // Fire event - sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED); - rc = 0; + // Fire event + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UNPACKED); + rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; + xSemaphoreGiveRecursive(mutex); + return rc; } /** @@ -664,41 +674,41 @@ unlock_exit: * \param[out] dataOut The byte array * \return 0 if success or -1 if failure */ -int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t * dataOut) +int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t *dataOut) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - if (UAVObjIsMetaobject(obj_handle)) { - if (instId != 0) { - goto unlock_exit; - } - memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes); - } else { - struct UAVOData *obj; - InstanceHandle instEntry; + if (UAVObjIsMetaobject(obj_handle)) { + if (instId != 0) { + goto unlock_exit; + } + memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes); + } else { + struct UAVOData *obj; + InstanceHandle instEntry; - // Cast handle to object - obj = (struct UAVOData *) obj_handle; + // Cast handle to object + obj = (struct UAVOData *)obj_handle; - // Get the instance - instEntry = getInstance(obj, instId); - if (instEntry == NULL) { - goto unlock_exit; - } - // Pack data - memcpy(dataOut, InstanceData(instEntry), obj->instance_size); - } + // Get the instance + instEntry = getInstance(obj, instId); + if (instEntry == NULL) { + goto unlock_exit; + } + // Pack data + memcpy(dataOut, InstanceData(instEntry), obj->instance_size); + } - rc = 0; + rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; + xSemaphoreGiveRecursive(mutex); + return rc; } #if defined(PIOS_USE_SETTINGS_ON_SDCARD) @@ -712,69 +722,69 @@ unlock_exit: * @return 0 if success or -1 if failure */ int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId, - FILEINFO * file) + FILEINFO *file) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - uint32_t bytesWritten; - // Check for file system availability - if (PIOS_SDCARD_IsMounted() == 0) { - return -1; - } - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + uint32_t bytesWritten; + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return -1; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - if (UAVObjIsMetaobject(obj_handle)) { - // Get the instance information - if (instId != 0) { - xSemaphoreGiveRecursive(mutex); - return -1; - } - // Write the object ID - uint32_t objId = UAVObjGetID(obj_handle); - PIOS_FWRITE(file, &objId, sizeof(objId), - &bytesWritten); + if (UAVObjIsMetaobject(obj_handle)) { + // Get the instance information + if (instId != 0) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Write the object ID + uint32_t objId = UAVObjGetID(obj_handle); + PIOS_FWRITE(file, &objId, sizeof(objId), + &bytesWritten); - // Write the data and check that the write was successful - PIOS_FWRITE(file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes, - &bytesWritten); - if (bytesWritten != MetaNumBytes) { - xSemaphoreGiveRecursive(mutex); - return -1; - } - } else { - struct UAVOData * uavo; - InstanceHandle instEntry; + // Write the data and check that the write was successful + PIOS_FWRITE(file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes, + &bytesWritten); + if (bytesWritten != MetaNumBytes) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + } else { + struct UAVOData *uavo; + InstanceHandle instEntry; - // Cast to object - uavo = (struct UAVOData *) obj_handle; + // Cast to object + uavo = (struct UAVOData *)obj_handle; - // Get the instance information - instEntry = getInstance(uavo, instId); - if (instEntry == NULL) { - xSemaphoreGiveRecursive(mutex); - return -1; - } - // Write the object ID - PIOS_FWRITE(file, &uavo->id, sizeof(uavo->id), - &bytesWritten); + // Get the instance information + instEntry = getInstance(uavo, instId); + if (instEntry == NULL) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Write the object ID + PIOS_FWRITE(file, &uavo->id, sizeof(uavo->id), + &bytesWritten); - // Write the instance ID - if (!UAVObjIsSingleInstance(obj_handle)) { - PIOS_FWRITE(file, &instId, - sizeof(instId), &bytesWritten); - } - // Write the data and check that the write was successful - PIOS_FWRITE(file, InstanceData(instEntry), uavo->instance_size, - &bytesWritten); - if (bytesWritten != uavo->instance_size) { - xSemaphoreGiveRecursive(mutex); - return -1; - } - } - // Done - xSemaphoreGiveRecursive(mutex); - return 0; + // Write the instance ID + if (!UAVObjIsSingleInstance(obj_handle)) { + PIOS_FWRITE(file, &instId, + sizeof(instId), &bytesWritten); + } + // Write the data and check that the write was successful + PIOS_FWRITE(file, InstanceData(instEntry), uavo->instance_size, + &bytesWritten); + if (bytesWritten != uavo->instance_size) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + // Done + xSemaphoreGiveRecursive(mutex); + return 0; } #endif /* PIOS_USE_SETTINGS_ON_SDCARD */ @@ -790,58 +800,63 @@ int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId, */ int32_t UAVObjSave(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t instId) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); #if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) - if (UAVObjIsMetaobject(obj_handle)) { - if (instId != 0) - return -1; + if (UAVObjIsMetaobject(obj_handle)) { + if (instId != 0) { + return -1; + } - if (PIOS_FLASHFS_ObjSave(0, UAVObjGetID(obj_handle), instId, (uint8_t*) MetaDataPtr((struct UAVOMeta *)obj_handle), UAVObjGetNumBytes(obj_handle)) != 0) - return -1; - } else { - InstanceHandle instEntry = getInstance( (struct UAVOData *)obj_handle, instId); + if (PIOS_FLASHFS_ObjSave(0, UAVObjGetID(obj_handle), instId, (uint8_t *)MetaDataPtr((struct UAVOMeta *)obj_handle), UAVObjGetNumBytes(obj_handle)) != 0) { + return -1; + } + } else { + InstanceHandle instEntry = getInstance((struct UAVOData *)obj_handle, instId); - if (instEntry == NULL) - return -1; + if (instEntry == NULL) { + return -1; + } - if (InstanceData(instEntry) == NULL) - return -1; + if (InstanceData(instEntry) == NULL) { + return -1; + } - if (PIOS_FLASHFS_ObjSave(0, UAVObjGetID(obj_handle), instId, InstanceData(instEntry), UAVObjGetNumBytes(obj_handle)) != 0) - return -1; - } -#endif + if (PIOS_FLASHFS_ObjSave(0, UAVObjGetID(obj_handle), instId, InstanceData(instEntry), UAVObjGetNumBytes(obj_handle)) != 0) { + return -1; + } + } +#endif /* if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) */ #if defined(PIOS_USE_SETTINGS_ON_SDCARD) - FILEINFO file; - uint8_t filename[14]; + FILEINFO file; + uint8_t filename[14]; - // Check for file system availability - if (PIOS_SDCARD_IsMounted() == 0) { - return -1; - } - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return -1; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Get filename - objectFilename(obj_handle, filename); + // Get filename + objectFilename(obj_handle, filename); - // Open file - if (PIOS_FOPEN_WRITE(filename, file)) { - xSemaphoreGiveRecursive(mutex); - return -1; - } - // Append object - if (UAVObjSaveToFile(obj_handle, instId, &file) == -1) { - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); - return -1; - } - // Done, close file and unlock - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); + // Open file + if (PIOS_FOPEN_WRITE(filename, file)) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Append object + if (UAVObjSaveToFile(obj_handle, instId, &file) == -1) { + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Done, close file and unlock + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); #endif /* PIOS_USE_SETTINGS_ON_SDCARD */ - return 0; + return 0; } #if defined(PIOS_USE_SETTINGS_ON_SDCARD) @@ -850,87 +865,85 @@ int32_t UAVObjSave(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t ins * @param[in] file File to read from * @return The handle of the object loaded or NULL if a failure */ -UAVObjHandle UAVObjLoadFromFile(FILEINFO * file) +UAVObjHandle UAVObjLoadFromFile(FILEINFO *file) { - uint32_t bytesRead; - struct UAVOBase *objEntry; - InstanceHandle instEntry; - uint32_t objId; - uint16_t instId; - UAVObjHandle obj_handle; + uint32_t bytesRead; + struct UAVOBase *objEntry; + InstanceHandle instEntry; + uint32_t objId; + uint16_t instId; + UAVObjHandle obj_handle; - // Check for file system availability - if (PIOS_SDCARD_IsMounted() == 0) { - return NULL; - } - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return NULL; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Read the object ID - if (PIOS_FREAD(file, &objId, sizeof(objId), &bytesRead)) { - xSemaphoreGiveRecursive(mutex); - return NULL; - } - // Get the object - obj_handle = UAVObjGetByID(objId); - if (obj_handle == 0) { - xSemaphoreGiveRecursive(mutex); - return NULL; - } - objEntry = (struct UAVOBase *) obj_handle; + // Read the object ID + if (PIOS_FREAD(file, &objId, sizeof(objId), &bytesRead)) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + // Get the object + obj_handle = UAVObjGetByID(objId); + if (obj_handle == 0) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + objEntry = (struct UAVOBase *)obj_handle; - // Get the instance ID - instId = 0; - if (!UAVObjIsSingleInstance(obj_handle)) { - if (PIOS_FREAD - (file, &instId, sizeof(instId), &bytesRead)) { - xSemaphoreGiveRecursive(mutex); - return NULL; - } - } + // Get the instance ID + instId = 0; + if (!UAVObjIsSingleInstance(obj_handle)) { + if (PIOS_FREAD + (file, &instId, sizeof(instId), &bytesRead)) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + } - if (UAVObjIsMetaobject(obj_handle)) { - // If the instance does not exist create it and any other instances before it - if (instId != 0) { - // Error, unlock and return - xSemaphoreGiveRecursive(mutex); - return NULL; - } - // Read the instance data - if (PIOS_FREAD - (file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes, &bytesRead)) { - xSemaphoreGiveRecursive(mutex); - return NULL; - } - } else { + if (UAVObjIsMetaobject(obj_handle)) { + // If the instance does not exist create it and any other instances before it + if (instId != 0) { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return NULL; + } + // Read the instance data + if (PIOS_FREAD + (file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes, &bytesRead)) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + } else { + // Get the instance information + instEntry = getInstance((struct UAVOData *)objEntry, instId); - // Get the instance information - instEntry = getInstance((struct UAVOData *)objEntry, instId); + // If the instance does not exist create it and any other instances before it + if (instEntry == NULL) { + instEntry = createInstance((struct UAVOData *)objEntry, instId); + if (instEntry == NULL) { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return NULL; + } + } + // Read the instance data + if (PIOS_FREAD + (file, InstanceData(instEntry), ((struct UAVOData *)objEntry)->instance_size, &bytesRead)) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + } - // If the instance does not exist create it and any other instances before it - if (instEntry == NULL) { - instEntry = createInstance((struct UAVOData *)objEntry, instId); - if (instEntry == NULL) { - // Error, unlock and return - xSemaphoreGiveRecursive(mutex); - return NULL; - } - } - // Read the instance data - if (PIOS_FREAD - (file, InstanceData(instEntry), ((struct UAVOData *)objEntry)->instance_size, &bytesRead)) { - xSemaphoreGiveRecursive(mutex); - return NULL; - } + // Fire event + sendEvent(objEntry, instId, EV_UNPACKED); - } - - // Fire event - sendEvent(objEntry, instId, EV_UNPACKED); - - // Unlock - xSemaphoreGiveRecursive(mutex); - return obj_handle; + // Unlock + xSemaphoreGiveRecursive(mutex); + return obj_handle; } #endif /* PIOS_USE_SETTINGS_ON_SDCARD */ @@ -944,72 +957,75 @@ UAVObjHandle UAVObjLoadFromFile(FILEINFO * file) */ int32_t UAVObjLoad(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t instId) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); #if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) - if (UAVObjIsMetaobject(obj_handle)) { - if (instId != 0) - return -1; + if (UAVObjIsMetaobject(obj_handle)) { + if (instId != 0) { + return -1; + } - // Fire event on success - if (PIOS_FLASHFS_ObjLoad(0, UAVObjGetID(obj_handle), instId, (uint8_t*) MetaDataPtr((struct UAVOMeta *)obj_handle), UAVObjGetNumBytes(obj_handle)) == 0) - sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED); - else - return -1; - } else { + // Fire event on success + if (PIOS_FLASHFS_ObjLoad(0, UAVObjGetID(obj_handle), instId, (uint8_t *)MetaDataPtr((struct UAVOMeta *)obj_handle), UAVObjGetNumBytes(obj_handle)) == 0) { + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UNPACKED); + } else { + return -1; + } + } else { + InstanceHandle instEntry = getInstance((struct UAVOData *)obj_handle, instId); - InstanceHandle instEntry = getInstance( (struct UAVOData *)obj_handle, instId); + if (instEntry == NULL) { + return -1; + } - if (instEntry == NULL) - return -1; + // Fire event on success + if (PIOS_FLASHFS_ObjLoad(0, UAVObjGetID(obj_handle), instId, InstanceData(instEntry), UAVObjGetNumBytes(obj_handle)) == 0) { + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UNPACKED); + } else { + return -1; + } + } - // Fire event on success - if (PIOS_FLASHFS_ObjLoad(0, UAVObjGetID(obj_handle), instId, InstanceData(instEntry), UAVObjGetNumBytes(obj_handle)) == 0) - sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED); - else - return -1; - } - -#endif +#endif /* if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) */ #if defined(PIOS_USE_SETTINGS_ON_SDCARD) - FILEINFO file; - UAVObjHandle loadedObj; - uint8_t filename[14]; + FILEINFO file; + UAVObjHandle loadedObj; + uint8_t filename[14]; - // Check for file system availability - if (PIOS_SDCARD_IsMounted() == 0) { - return -1; - } - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return -1; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Get filename - objectFilename(obj_handle, filename); + // Get filename + objectFilename(obj_handle, filename); - // Open file - if (PIOS_FOPEN_READ(filename, file)) { - xSemaphoreGiveRecursive(mutex); - return -1; - } - // Load object - loadedObj = UAVObjLoadFromFile(&file); - if (loadedObj == 0) { - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); - return -1; - } - // Check that the IDs match - if (UAVObjGetID(loadedObj) != UAVObjGetID(obj_handle)) { - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); - return -1; - } - // Done, close file and unlock - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); + // Open file + if (PIOS_FOPEN_READ(filename, file)) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Load object + loadedObj = UAVObjLoadFromFile(&file); + if (loadedObj == 0) { + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Check that the IDs match + if (UAVObjGetID(loadedObj) != UAVObjGetID(obj_handle)) { + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Done, close file and unlock + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); #endif /* PIOS_USE_SETTINGS_ON_SDCARD */ - return 0; + return 0; } /** @@ -1020,30 +1036,30 @@ int32_t UAVObjLoad(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t ins */ int32_t UAVObjDelete(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t instId) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); #if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) - PIOS_FLASHFS_ObjDelete(0, UAVObjGetID(obj_handle), instId); + PIOS_FLASHFS_ObjDelete(0, UAVObjGetID(obj_handle), instId); #endif #if defined(PIOS_USE_SETTINGS_ON_SDCARD) - uint8_t filename[14]; + uint8_t filename[14]; - // Check for file system availability - if (PIOS_SDCARD_IsMounted() == 0) { - return -1; - } - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return -1; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Get filename - objectFilename(obj_handle, filename); + // Get filename + objectFilename(obj_handle, filename); - // Delete file - PIOS_FUNLINK(filename); + // Delete file + PIOS_FUNLINK(filename); - // Done - xSemaphoreGiveRecursive(mutex); + // Done + xSemaphoreGiveRecursive(mutex); #endif /* PIOS_USE_SETTINGS_ON_SDCARD */ - return 0; + return 0; } /** @@ -1052,28 +1068,28 @@ int32_t UAVObjDelete(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t i */ int32_t UAVObjSaveSettings() { - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - // Save all settings objects - UAVO_LIST_ITERATE(obj) - // Check if this is a settings object - if (UAVObjIsSettings(obj)) { - // Save object - if (UAVObjSave((UAVObjHandle) obj, 0) == - -1) { - goto unlock_exit; - } - } - } + // Save all settings objects + UAVO_LIST_ITERATE(obj) + // Check if this is a settings object + if (UAVObjIsSettings(obj)) { + // Save object + if (UAVObjSave((UAVObjHandle)obj, 0) == + -1) { + goto unlock_exit; + } + } +} - rc = 0; +rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; +xSemaphoreGiveRecursive(mutex); +return rc; } /** @@ -1082,28 +1098,28 @@ unlock_exit: */ int32_t UAVObjLoadSettings() { - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - // Load all settings objects - UAVO_LIST_ITERATE(obj) - // Check if this is a settings object - if (UAVObjIsSettings(obj)) { - // Load object - if (UAVObjLoad((UAVObjHandle) obj, 0) == - -1) { - goto unlock_exit; - } - } - } + // Load all settings objects + UAVO_LIST_ITERATE(obj) + // Check if this is a settings object + if (UAVObjIsSettings(obj)) { + // Load object + if (UAVObjLoad((UAVObjHandle)obj, 0) == + -1) { + goto unlock_exit; + } + } +} - rc = 0; +rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; +xSemaphoreGiveRecursive(mutex); +return rc; } /** @@ -1112,28 +1128,28 @@ unlock_exit: */ int32_t UAVObjDeleteSettings() { - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - // Save all settings objects - UAVO_LIST_ITERATE(obj) - // Check if this is a settings object - if (UAVObjIsSettings(obj)) { - // Save object - if (UAVObjDelete((UAVObjHandle) obj, 0) - == -1) { - goto unlock_exit; - } - } - } + // Save all settings objects + UAVO_LIST_ITERATE(obj) + // Check if this is a settings object + if (UAVObjIsSettings(obj)) { + // Save object + if (UAVObjDelete((UAVObjHandle)obj, 0) + == -1) { + goto unlock_exit; + } + } +} - rc = 0; +rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; +xSemaphoreGiveRecursive(mutex); +return rc; } /** @@ -1142,25 +1158,25 @@ unlock_exit: */ int32_t UAVObjSaveMetaobjects() { - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - // Save all settings objects - UAVO_LIST_ITERATE(obj) - // Save object - if (UAVObjSave( (UAVObjHandle) MetaObjectPtr(obj), 0) == - -1) { - goto unlock_exit; - } - } + // Save all settings objects + UAVO_LIST_ITERATE(obj) + // Save object + if (UAVObjSave((UAVObjHandle)MetaObjectPtr(obj), 0) == + -1) { + goto unlock_exit; + } +} - rc = 0; +rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; +xSemaphoreGiveRecursive(mutex); +return rc; } /** @@ -1169,25 +1185,25 @@ unlock_exit: */ int32_t UAVObjLoadMetaobjects() { - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - // Load all settings objects - UAVO_LIST_ITERATE(obj) - // Load object - if (UAVObjLoad((UAVObjHandle) MetaObjectPtr(obj), 0) == - -1) { - goto unlock_exit; - } - } + // Load all settings objects + UAVO_LIST_ITERATE(obj) + // Load object + if (UAVObjLoad((UAVObjHandle)MetaObjectPtr(obj), 0) == + -1) { + goto unlock_exit; + } +} - rc = 0; +rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; +xSemaphoreGiveRecursive(mutex); +return rc; } /** @@ -1196,25 +1212,25 @@ unlock_exit: */ int32_t UAVObjDeleteMetaobjects() { - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - // Load all settings objects - UAVO_LIST_ITERATE(obj) - // Load object - if (UAVObjDelete((UAVObjHandle) MetaObjectPtr(obj), 0) - == -1) { - goto unlock_exit; - } - } + // Load all settings objects + UAVO_LIST_ITERATE(obj) + // Load object + if (UAVObjDelete((UAVObjHandle)MetaObjectPtr(obj), 0) + == -1) { + goto unlock_exit; + } +} - rc = 0; +rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; +xSemaphoreGiveRecursive(mutex); +return rc; } /** @@ -1225,7 +1241,7 @@ unlock_exit: */ int32_t UAVObjSetData(UAVObjHandle obj_handle, const void *dataIn) { - return UAVObjSetInstanceData(obj_handle, 0, dataIn); + return UAVObjSetInstanceData(obj_handle, 0, dataIn); } /** @@ -1234,9 +1250,9 @@ int32_t UAVObjSetData(UAVObjHandle obj_handle, const void *dataIn) * \param[in] dataIn The object's data structure * \return 0 if success or -1 if failure */ -int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void* dataIn, uint32_t offset, uint32_t size) +int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void *dataIn, uint32_t offset, uint32_t size) { - return UAVObjSetInstanceDataField(obj_handle, 0, dataIn, offset, size); + return UAVObjSetInstanceDataField(obj_handle, 0, dataIn, offset, size); } /** @@ -1247,7 +1263,7 @@ int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void* dataIn, uint32_t */ int32_t UAVObjGetData(UAVObjHandle obj_handle, void *dataOut) { - return UAVObjGetInstanceData(obj_handle, 0, dataOut); + return UAVObjGetInstanceData(obj_handle, 0, dataOut); } /** @@ -1256,9 +1272,9 @@ int32_t UAVObjGetData(UAVObjHandle obj_handle, void *dataOut) * \param[out] dataOut The object's data structure * \return 0 if success or -1 if failure */ -int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void* dataOut, uint32_t offset, uint32_t size) +int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void *dataOut, uint32_t offset, uint32_t size) { - return UAVObjGetInstanceDataField(obj_handle, 0, dataOut, offset, size); + return UAVObjGetInstanceDataField(obj_handle, 0, dataOut, offset, size); } /** @@ -1269,47 +1285,47 @@ int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void* dataOut, uint32_t offs * \return 0 if success or -1 if failure */ int32_t UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId, - const void *dataIn) + const void *dataIn) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - if (UAVObjIsMetaobject(obj_handle)) { - if (instId != 0) { - goto unlock_exit; - } - memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes); - } else { - struct UAVOData *obj; - InstanceHandle instEntry; + if (UAVObjIsMetaobject(obj_handle)) { + if (instId != 0) { + goto unlock_exit; + } + memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes); + } else { + struct UAVOData *obj; + InstanceHandle instEntry; - // Cast to object info - obj = (struct UAVOData *) obj_handle; + // Cast to object info + obj = (struct UAVOData *)obj_handle; - // Check access level - if (UAVObjReadOnly(obj_handle)) { - goto unlock_exit; - } - // Get instance information - instEntry = getInstance(obj, instId); - if (instEntry == NULL) { - goto unlock_exit; - } - // Set data - memcpy(InstanceData(instEntry), dataIn, obj->instance_size); - } + // Check access level + if (UAVObjReadOnly(obj_handle)) { + goto unlock_exit; + } + // Get instance information + instEntry = getInstance(obj, instId); + if (instEntry == NULL) { + goto unlock_exit; + } + // Set data + memcpy(InstanceData(instEntry), dataIn, obj->instance_size); + } - // Fire event - sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED); - rc = 0; + // Fire event + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED); + rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; + xSemaphoreGiveRecursive(mutex); + return rc; } /** @@ -1319,63 +1335,63 @@ unlock_exit: * \param[in] dataIn The object's data structure * \return 0 if success or -1 if failure */ -int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size) +int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void *dataIn, uint32_t offset, uint32_t size) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - if (UAVObjIsMetaobject(obj_handle)) { - // Get instance information - if (instId != 0) { - goto unlock_exit; - } + if (UAVObjIsMetaobject(obj_handle)) { + // Get instance information + if (instId != 0) { + goto unlock_exit; + } - // Check for overrun - if ((size + offset) > MetaNumBytes) { - goto unlock_exit; - } + // Check for overrun + if ((size + offset) > MetaNumBytes) { + goto unlock_exit; + } - // Set data - memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, dataIn, size); - } else { - struct UAVOData * obj; - InstanceHandle instEntry; + // Set data + memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, dataIn, size); + } else { + struct UAVOData *obj; + InstanceHandle instEntry; - // Cast to object info - obj = (struct UAVOData *)obj_handle; + // Cast to object info + obj = (struct UAVOData *)obj_handle; - // Check access level - if (UAVObjReadOnly(obj_handle)) { - goto unlock_exit; - } + // Check access level + if (UAVObjReadOnly(obj_handle)) { + goto unlock_exit; + } - // Get instance information - instEntry = getInstance(obj, instId); - if (instEntry == NULL) { - goto unlock_exit; - } + // Get instance information + instEntry = getInstance(obj, instId); + if (instEntry == NULL) { + goto unlock_exit; + } - // Check for overrun - if ((size + offset) > obj->instance_size) { - goto unlock_exit; - } + // Check for overrun + if ((size + offset) > obj->instance_size) { + goto unlock_exit; + } - // Set data - memcpy(InstanceData(instEntry) + offset, dataIn, size); - } + // Set data + memcpy(InstanceData(instEntry) + offset, dataIn, size); + } - // Fire event - sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED); - rc = 0; + // Fire event + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED); + rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; + xSemaphoreGiveRecursive(mutex); + return rc; } /** @@ -1386,43 +1402,43 @@ unlock_exit: * \return 0 if success or -1 if failure */ int32_t UAVObjGetInstanceData(UAVObjHandle obj_handle, uint16_t instId, - void *dataOut) + void *dataOut) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - if (UAVObjIsMetaobject(obj_handle)) { - // Get instance information - if (instId != 0) { - goto unlock_exit; - } - // Set data - memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes); - } else { - struct UAVOData *obj; - InstanceHandle instEntry; + if (UAVObjIsMetaobject(obj_handle)) { + // Get instance information + if (instId != 0) { + goto unlock_exit; + } + // Set data + memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes); + } else { + struct UAVOData *obj; + InstanceHandle instEntry; - // Cast to object info - obj = (struct UAVOData *) obj_handle; + // Cast to object info + obj = (struct UAVOData *)obj_handle; - // Get instance information - instEntry = getInstance(obj, instId); - if (instEntry == NULL) { - goto unlock_exit; - } - // Set data - memcpy(dataOut, InstanceData(instEntry), obj->instance_size); - } + // Get instance information + instEntry = getInstance(obj, instId); + if (instEntry == NULL) { + goto unlock_exit; + } + // Set data + memcpy(dataOut, InstanceData(instEntry), obj->instance_size); + } - rc = 0; + rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; + xSemaphoreGiveRecursive(mutex); + return rc; } /** @@ -1432,55 +1448,55 @@ unlock_exit: * \param[out] dataOut The object's data structure * \return 0 if success or -1 if failure */ -int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size) +int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, void *dataOut, uint32_t offset, uint32_t size) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - if (UAVObjIsMetaobject(obj_handle)) { - // Get instance information - if (instId != 0) { - goto unlock_exit; - } + if (UAVObjIsMetaobject(obj_handle)) { + // Get instance information + if (instId != 0) { + goto unlock_exit; + } - // Check for overrun - if ((size + offset) > MetaNumBytes) { - goto unlock_exit; - } + // Check for overrun + if ((size + offset) > MetaNumBytes) { + goto unlock_exit; + } - // Set data - memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, size); - } else { - struct UAVOData * obj; - InstanceHandle instEntry; + // Set data + memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, size); + } else { + struct UAVOData *obj; + InstanceHandle instEntry; - // Cast to object info - obj = (struct UAVOData *)obj_handle; + // Cast to object info + obj = (struct UAVOData *)obj_handle; - // Get instance information - instEntry = getInstance(obj, instId); - if (instEntry == NULL) { - goto unlock_exit; - } + // Get instance information + instEntry = getInstance(obj, instId); + if (instEntry == NULL) { + goto unlock_exit; + } - // Check for overrun - if ((size + offset) > obj->instance_size) { - goto unlock_exit; - } - - // Set data - memcpy(dataOut, InstanceData(instEntry) + offset, size); - } + // Check for overrun + if ((size + offset) > obj->instance_size) { + goto unlock_exit; + } - rc = 0; + // Set data + memcpy(dataOut, InstanceData(instEntry) + offset, size); + } + + rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; + xSemaphoreGiveRecursive(mutex); + return rc; } /** @@ -1489,21 +1505,21 @@ unlock_exit: * \param[in] dataIn The object's metadata structure * \return 0 if success or -1 if failure */ -int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata * dataIn) +int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata *dataIn) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Set metadata (metadata of metaobjects can not be modified) - if (UAVObjIsMetaobject(obj_handle)) { - return -1; - } + // Set metadata (metadata of metaobjects can not be modified) + if (UAVObjIsMetaobject(obj_handle)) { + return -1; + } - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - UAVObjSetData((UAVObjHandle) MetaObjectPtr((struct UAVOData *)obj_handle), dataIn); + UAVObjSetData((UAVObjHandle)MetaObjectPtr((struct UAVOData *)obj_handle), dataIn); - xSemaphoreGiveRecursive(mutex); - return 0; + xSemaphoreGiveRecursive(mutex); + return 0; } /** @@ -1512,24 +1528,24 @@ int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata * dataIn * \param[out] dataOut The object's metadata structure * \return 0 if success or -1 if failure */ -int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata * dataOut) +int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata *dataOut) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Get metadata - if (UAVObjIsMetaobject(obj_handle)) { - memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata)); - } else { - UAVObjGetData((UAVObjHandle) MetaObjectPtr( (struct UAVOData *)obj_handle ), - dataOut); - } + // Get metadata + if (UAVObjIsMetaobject(obj_handle)) { + memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata)); + } else { + UAVObjGetData((UAVObjHandle)MetaObjectPtr((struct UAVOData *)obj_handle), + dataOut); + } - // Unlock - xSemaphoreGiveRecursive(mutex); - return 0; + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; } /******************************* @@ -1541,10 +1557,10 @@ int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata * dataOut) * \param[in] metadata The metadata object * \return the access type */ -UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* metadata) +UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata *metadata) { - PIOS_Assert(metadata); - return (metadata->flags >> UAVOBJ_ACCESS_SHIFT) & 1; + PIOS_Assert(metadata); + return (metadata->flags >> UAVOBJ_ACCESS_SHIFT) & 1; } /** @@ -1552,10 +1568,10 @@ UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* metadata) * \param[in] metadata The metadata object * \param[in] mode The access mode */ -void UAVObjSetAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) +void UAVObjSetAccess(UAVObjMetadata *metadata, UAVObjAccessType mode) { - PIOS_Assert(metadata); - SET_BITS(metadata->flags, UAVOBJ_ACCESS_SHIFT, mode, 1); + PIOS_Assert(metadata); + SET_BITS(metadata->flags, UAVOBJ_ACCESS_SHIFT, mode, 1); } /** @@ -1563,10 +1579,10 @@ void UAVObjSetAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) * \param[in] metadata The metadata object * \return the GCS access type */ -UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* metadata) +UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata *metadata) { - PIOS_Assert(metadata); - return (metadata->flags >> UAVOBJ_GCS_ACCESS_SHIFT) & 1; + PIOS_Assert(metadata); + return (metadata->flags >> UAVOBJ_GCS_ACCESS_SHIFT) & 1; } /** @@ -1574,9 +1590,10 @@ UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* metadata) * \param[in] metadata The metadata object * \param[in] mode The access mode */ -void UAVObjSetGcsAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) { - PIOS_Assert(metadata); - SET_BITS(metadata->flags, UAVOBJ_GCS_ACCESS_SHIFT, mode, 1); +void UAVObjSetGcsAccess(UAVObjMetadata *metadata, UAVObjAccessType mode) +{ + PIOS_Assert(metadata); + SET_BITS(metadata->flags, UAVOBJ_GCS_ACCESS_SHIFT, mode, 1); } /** @@ -1584,9 +1601,10 @@ void UAVObjSetGcsAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) { * \param[in] metadata The metadata object * \return the telemetry acked boolean */ -uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata* metadata) { - PIOS_Assert(metadata); - return (metadata->flags >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 1; +uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata *metadata) +{ + PIOS_Assert(metadata); + return (metadata->flags >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 1; } /** @@ -1594,9 +1612,10 @@ uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata* metadata) { * \param[in] metadata The metadata object * \param[in] val The telemetry acked boolean */ -void UAVObjSetTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) { - PIOS_Assert(metadata); - SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_ACKED_SHIFT, val, 1); +void UAVObjSetTelemetryAcked(UAVObjMetadata *metadata, uint8_t val) +{ + PIOS_Assert(metadata); + SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_ACKED_SHIFT, val, 1); } /** @@ -1604,9 +1623,10 @@ void UAVObjSetTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) { * \param[in] metadata The metadata object * \return the telemetry acked boolean */ -uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata* metadata) { - PIOS_Assert(metadata); - return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 1; +uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata *metadata) +{ + PIOS_Assert(metadata); + return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 1; } /** @@ -1614,9 +1634,10 @@ uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata* metadata) { * \param[in] metadata The metadata object * \param[in] val The GCS telemetry acked boolean */ -void UAVObjSetGcsTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) { - PIOS_Assert(metadata); - SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, val, 1); +void UAVObjSetGcsTelemetryAcked(UAVObjMetadata *metadata, uint8_t val) +{ + PIOS_Assert(metadata); + SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, val, 1); } /** @@ -1624,9 +1645,10 @@ void UAVObjSetGcsTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) { * \param[in] metadata The metadata object * \return the telemetry update mode */ -UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata* metadata) { - PIOS_Assert(metadata); - return (metadata->flags >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK; +UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata *metadata) +{ + PIOS_Assert(metadata); + return (metadata->flags >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK; } /** @@ -1634,9 +1656,10 @@ UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata* metadata) { * \param[in] metadata The metadata object * \param[in] val The telemetry update mode */ -void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val) { - PIOS_Assert(metadata); - SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); +void UAVObjSetTelemetryUpdateMode(UAVObjMetadata *metadata, UAVObjUpdateMode val) +{ + PIOS_Assert(metadata); + SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); } /** @@ -1644,9 +1667,10 @@ void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val * \param[in] metadata The metadata object * \return the GCS telemetry update mode */ -UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* metadata) { - PIOS_Assert(metadata); - return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK; +UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata *metadata) +{ + PIOS_Assert(metadata); + return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK; } /** @@ -1654,27 +1678,28 @@ UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* metadata) * \param[in] metadata The metadata object * \param[in] val The GCS telemetry update mode */ -void UAVObjSetGcsTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val) { - PIOS_Assert(metadata); - SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); +void UAVObjSetGcsTelemetryUpdateMode(UAVObjMetadata *metadata, UAVObjUpdateMode val) +{ + PIOS_Assert(metadata); + SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); } /** * Check if an object is read only * \param[in] obj The object handle - * \return - * \arg 0 if not read only + * \return + * \arg 0 if not read only * \arg 1 if read only * \arg -1 if unable to get meta data */ int8_t UAVObjReadOnly(UAVObjHandle obj_handle) { - PIOS_Assert(obj_handle); - if (!UAVObjIsMetaobject(obj_handle)) { - return UAVObjGetAccess(LinkedMetaDataPtr( (struct UAVOData *)obj_handle)) == ACCESS_READONLY; - } - return -1; + PIOS_Assert(obj_handle); + if (!UAVObjIsMetaobject(obj_handle)) { + return UAVObjGetAccess(LinkedMetaDataPtr((struct UAVOData *)obj_handle)) == ACCESS_READONLY; + } + return -1; } /** @@ -1686,15 +1711,15 @@ int8_t UAVObjReadOnly(UAVObjHandle obj_handle) * \return 0 if success or -1 if failure */ int32_t UAVObjConnectQueue(UAVObjHandle obj_handle, xQueueHandle queue, - uint8_t eventMask) + uint8_t eventMask) { - PIOS_Assert(obj_handle); - PIOS_Assert(queue); - int32_t res; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - res = connectObj(obj_handle, queue, 0, eventMask); - xSemaphoreGiveRecursive(mutex); - return res; + PIOS_Assert(obj_handle); + PIOS_Assert(queue); + int32_t res; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + res = connectObj(obj_handle, queue, 0, eventMask); + xSemaphoreGiveRecursive(mutex); + return res; } /** @@ -1705,13 +1730,13 @@ int32_t UAVObjConnectQueue(UAVObjHandle obj_handle, xQueueHandle queue, */ int32_t UAVObjDisconnectQueue(UAVObjHandle obj_handle, xQueueHandle queue) { - PIOS_Assert(obj_handle); - PIOS_Assert(queue); - int32_t res; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - res = disconnectObj(obj_handle, queue, 0); - xSemaphoreGiveRecursive(mutex); - return res; + PIOS_Assert(obj_handle); + PIOS_Assert(queue); + int32_t res; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + res = disconnectObj(obj_handle, queue, 0); + xSemaphoreGiveRecursive(mutex); + return res; } /** @@ -1723,14 +1748,14 @@ int32_t UAVObjDisconnectQueue(UAVObjHandle obj_handle, xQueueHandle queue) * \return 0 if success or -1 if failure */ int32_t UAVObjConnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb, - uint8_t eventMask) + uint8_t eventMask) { - PIOS_Assert(obj_handle); - int32_t res; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - res = connectObj(obj_handle, 0, cb, eventMask); - xSemaphoreGiveRecursive(mutex); - return res; + PIOS_Assert(obj_handle); + int32_t res; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + res = connectObj(obj_handle, 0, cb, eventMask); + xSemaphoreGiveRecursive(mutex); + return res; } /** @@ -1741,12 +1766,12 @@ int32_t UAVObjConnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb, */ int32_t UAVObjDisconnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb) { - PIOS_Assert(obj_handle); - int32_t res; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - res = disconnectObj(obj_handle, 0, cb); - xSemaphoreGiveRecursive(mutex); - return res; + PIOS_Assert(obj_handle); + int32_t res; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + res = disconnectObj(obj_handle, 0, cb); + xSemaphoreGiveRecursive(mutex); + return res; } /** @@ -1756,7 +1781,7 @@ int32_t UAVObjDisconnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb */ void UAVObjRequestUpdate(UAVObjHandle obj_handle) { - UAVObjRequestInstanceUpdate(obj_handle, UAVOBJ_ALL_INSTANCES); + UAVObjRequestInstanceUpdate(obj_handle, UAVOBJ_ALL_INSTANCES); } /** @@ -1767,10 +1792,10 @@ void UAVObjRequestUpdate(UAVObjHandle obj_handle) */ void UAVObjRequestInstanceUpdate(UAVObjHandle obj_handle, uint16_t instId) { - PIOS_Assert(obj_handle); - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - sendEvent((struct UAVOBase *) obj_handle, instId, EV_UPDATE_REQ); - xSemaphoreGiveRecursive(mutex); + PIOS_Assert(obj_handle); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATE_REQ); + xSemaphoreGiveRecursive(mutex); } /** @@ -1779,7 +1804,7 @@ void UAVObjRequestInstanceUpdate(UAVObjHandle obj_handle, uint16_t instId) */ void UAVObjUpdated(UAVObjHandle obj_handle) { - UAVObjInstanceUpdated(obj_handle, UAVOBJ_ALL_INSTANCES); + UAVObjInstanceUpdated(obj_handle, UAVOBJ_ALL_INSTANCES); } /** @@ -1789,10 +1814,10 @@ void UAVObjUpdated(UAVObjHandle obj_handle) */ void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId) { - PIOS_Assert(obj_handle); - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - sendEvent((struct UAVOBase *) obj_handle, instId, EV_UPDATED_MANUAL); - xSemaphoreGiveRecursive(mutex); + PIOS_Assert(obj_handle); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED_MANUAL); + xSemaphoreGiveRecursive(mutex); } /** @@ -1800,154 +1825,158 @@ void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId) * \param iterator This function will be called once for each object, * the object will be passed as a parameter */ -void UAVObjIterate(void (*iterator) (UAVObjHandle obj)) +void UAVObjIterate(void (*iterator)(UAVObjHandle obj)) { - PIOS_Assert(iterator); + PIOS_Assert(iterator); - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Iterate through the list and invoke iterator for each object - UAVO_LIST_ITERATE(obj) - (*iterator) ((UAVObjHandle) obj); - (*iterator) ((UAVObjHandle) &obj->metaObj); - } + // Iterate through the list and invoke iterator for each object + UAVO_LIST_ITERATE (obj) + (*iterator)((UAVObjHandle)obj); + (*iterator)((UAVObjHandle) &obj->metaObj); +} - // Release lock - xSemaphoreGiveRecursive(mutex); +// Release lock +xSemaphoreGiveRecursive(mutex); } /** * Send a triggered event to all event queues registered on the object. */ -static int32_t sendEvent(struct UAVOBase * obj, uint16_t instId, - UAVObjEventType triggered_event) +static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, + UAVObjEventType triggered_event) { - /* Set up the message that will be sent to all registered listeners */ - UAVObjEvent msg = { - .obj = (UAVObjHandle) obj, - .event = triggered_event, - .instId = instId, - }; + /* Set up the message that will be sent to all registered listeners */ + UAVObjEvent msg = { + .obj = (UAVObjHandle)obj, + .event = triggered_event, + .instId = instId, + }; - // Go through each object and push the event message in the queue (if event is activated for the queue) - struct ObjectEventEntry *event; - LL_FOREACH(obj->next_event, event) { - if (event->eventMask == 0 - || (event->eventMask & triggered_event) != 0) { - // Send to queue if a valid queue is registered - if (event->queue) { - // will not block - if (xQueueSend(event->queue, &msg, 0) != pdTRUE) { - stats.lastQueueErrorID = UAVObjGetID(obj); - ++stats.eventQueueErrors; - } - } + // Go through each object and push the event message in the queue (if event is activated for the queue) + struct ObjectEventEntry *event; - // Invoke callback (from event task) if a valid one is registered - if (event->cb) { - // invoke callback from the event task, will not block - if (EventCallbackDispatch(&msg, event->cb) != pdTRUE) { - ++stats.eventCallbackErrors; - stats.lastCallbackErrorID = UAVObjGetID(obj); - } - } - } - } + LL_FOREACH(obj->next_event, event) { + if (event->eventMask == 0 + || (event->eventMask & triggered_event) != 0) { + // Send to queue if a valid queue is registered + if (event->queue) { + // will not block + if (xQueueSend(event->queue, &msg, 0) != pdTRUE) { + stats.lastQueueErrorID = UAVObjGetID(obj); + ++stats.eventQueueErrors; + } + } - return 0; + // Invoke callback (from event task) if a valid one is registered + if (event->cb) { + // invoke callback from the event task, will not block + if (EventCallbackDispatch(&msg, event->cb) != pdTRUE) { + ++stats.eventCallbackErrors; + stats.lastCallbackErrorID = UAVObjGetID(obj); + } + } + } + } + + return 0; } /** * Create a new object instance, return the instance info or NULL if failure. */ -static InstanceHandle createInstance(struct UAVOData * obj, uint16_t instId) +static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId) { - struct UAVOMultiInst *instEntry; + struct UAVOMultiInst *instEntry; - /* Don't allow more than one instance for single instance objects */ - if (UAVObjIsSingleInstance(&(obj->base))) { - PIOS_Assert(0); - return NULL; - } + /* Don't allow more than one instance for single instance objects */ + if (UAVObjIsSingleInstance(&(obj->base))) { + PIOS_Assert(0); + return NULL; + } - /* Don't create more than the allowed number of instances */ - if (instId >= UAVOBJ_MAX_INSTANCES) { - return NULL; - } + /* Don't create more than the allowed number of instances */ + if (instId >= UAVOBJ_MAX_INSTANCES) { + return NULL; + } - /* Don't allow duplicate instances */ - if (instId < UAVObjGetNumInstances(&(obj->base))) { - return NULL; - } + /* Don't allow duplicate instances */ + if (instId < UAVObjGetNumInstances(&(obj->base))) { + return NULL; + } - // Create any missing instances (all instance IDs must be sequential) - for (uint16_t n = UAVObjGetNumInstances(&(obj->base)); n < instId; ++n) { - if (createInstance(obj, n) == NULL) { - return NULL; - } - } + // Create any missing instances (all instance IDs must be sequential) + for (uint16_t n = UAVObjGetNumInstances(&(obj->base)); n < instId; ++n) { + if (createInstance(obj, n) == NULL) { + return NULL; + } + } - /* Create the actual instance */ - uint32_t size = sizeof(struct UAVOMultiInst) + obj->instance_size; - instEntry = (struct UAVOMultiInst *) pvPortMalloc(size); - if (!instEntry) - return NULL; - memset(instEntry, 0, size); - LL_APPEND(( (struct UAVOMulti*)obj )->instance0.next, instEntry); + /* Create the actual instance */ + uint32_t size = sizeof(struct UAVOMultiInst) + obj->instance_size; + instEntry = (struct UAVOMultiInst *)pvPortMalloc(size); + if (!instEntry) { + return NULL; + } + memset(instEntry, 0, size); + LL_APPEND(((struct UAVOMulti *)obj)->instance0.next, instEntry); - ( (struct UAVOMulti*)obj )->num_instances++; + ((struct UAVOMulti *)obj)->num_instances++; - // Fire event - UAVObjInstanceUpdated((UAVObjHandle) obj, instId); + // Fire event + UAVObjInstanceUpdated((UAVObjHandle)obj, instId); - // Done - return InstanceDataOffset(instEntry); + // Done + return InstanceDataOffset(instEntry); } /** * Get the instance information or NULL if the instance does not exist */ -static InstanceHandle getInstance(struct UAVOData * obj, uint16_t instId) +static InstanceHandle getInstance(struct UAVOData *obj, uint16_t instId) { - if (UAVObjIsMetaobject(&obj->base)) { - /* Metadata Instance */ + if (UAVObjIsMetaobject(&obj->base)) { + /* Metadata Instance */ - if (instId != 0) - return NULL; + if (instId != 0) { + return NULL; + } - /* Augment our pointer to reflect the proper type */ - struct UAVOMeta * uavo_meta = (struct UAVOMeta *) obj; - return (&(uavo_meta->instance0)); + /* Augment our pointer to reflect the proper type */ + struct UAVOMeta *uavo_meta = (struct UAVOMeta *)obj; + return &(uavo_meta->instance0); + } else if (UAVObjIsSingleInstance(&(obj->base))) { + /* Single Instance */ - } else if (UAVObjIsSingleInstance(&(obj->base))) { - /* Single Instance */ + if (instId != 0) { + return NULL; + } - if (instId != 0) - return NULL; + /* Augment our pointer to reflect the proper type */ + struct UAVOSingle *uavo_single = (struct UAVOSingle *)obj; + return &(uavo_single->instance0); + } else { + /* Multi Instance */ + /* Augment our pointer to reflect the proper type */ + struct UAVOMulti *uavo_multi = (struct UAVOMulti *)obj; + if (instId >= uavo_multi->num_instances) { + return NULL; + } - /* Augment our pointer to reflect the proper type */ - struct UAVOSingle * uavo_single = (struct UAVOSingle *) obj; - return (&(uavo_single->instance0)); - } else { - /* Multi Instance */ - /* Augment our pointer to reflect the proper type */ - struct UAVOMulti * uavo_multi = (struct UAVOMulti *) obj; - if (instId >= uavo_multi->num_instances) - return NULL; - - // Look for specified instance ID - uint16_t instance = 0; - struct UAVOMultiInst *instEntry; - LL_FOREACH(&(uavo_multi->instance0), instEntry) { - if (instance++ == instId) { - /* Found it */ - return &(instEntry->instance); - } - } - /* Instance was not found */ - return NULL; - } + // Look for specified instance ID + uint16_t instance = 0; + struct UAVOMultiInst *instEntry; + LL_FOREACH(&(uavo_multi->instance0), instEntry) { + if (instance++ == instId) { + /* Found it */ + return &(instEntry->instance); + } + } + /* Instance was not found */ + return NULL; + } } /** @@ -1959,33 +1988,33 @@ static InstanceHandle getInstance(struct UAVOData * obj, uint16_t instId) * \return 0 if success or -1 if failure */ static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, - UAVObjEventCallback cb, uint8_t eventMask) + UAVObjEventCallback cb, uint8_t eventMask) { - struct ObjectEventEntry *event; - struct UAVOBase *obj; + struct ObjectEventEntry *event; + struct UAVOBase *obj; - // Check that the queue is not already connected, if it is simply update event mask - obj = (struct UAVOBase *) obj_handle; - LL_FOREACH(obj->next_event, event) { - if (event->queue == queue && event->cb == cb) { - // Already connected, update event mask and return - event->eventMask = eventMask; - return 0; - } - } + // Check that the queue is not already connected, if it is simply update event mask + obj = (struct UAVOBase *)obj_handle; + LL_FOREACH(obj->next_event, event) { + if (event->queue == queue && event->cb == cb) { + // Already connected, update event mask and return + event->eventMask = eventMask; + return 0; + } + } - // Add queue to list - event = (struct ObjectEventEntry *) pvPortMalloc(sizeof(struct ObjectEventEntry)); - if (event == NULL) { - return -1; - } - event->queue = queue; - event->cb = cb; - event->eventMask = eventMask; - LL_APPEND(obj->next_event, event); + // Add queue to list + event = (struct ObjectEventEntry *)pvPortMalloc(sizeof(struct ObjectEventEntry)); + if (event == NULL) { + return -1; + } + event->queue = queue; + event->cb = cb; + event->eventMask = eventMask; + LL_APPEND(obj->next_event, event); - // Done - return 0; + // Done + return 0; } /** @@ -1996,42 +2025,43 @@ static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, * \return 0 if success or -1 if failure */ static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue, - UAVObjEventCallback cb) + UAVObjEventCallback cb) { - struct ObjectEventEntry *event; - struct UAVOBase *obj; + struct ObjectEventEntry *event; + struct UAVOBase *obj; - // Find queue and remove it - obj = (struct UAVOBase *) obj_handle; - LL_FOREACH(obj->next_event, event) { - if ((event->queue == queue - && event->cb == cb)) { - LL_DELETE(obj->next_event, event); - vPortFree(event); - return 0; - } - } + // Find queue and remove it + obj = (struct UAVOBase *)obj_handle; + LL_FOREACH(obj->next_event, event) { + if ((event->queue == queue + && event->cb == cb)) { + LL_DELETE(obj->next_event, event); + vPortFree(event); + return 0; + } + } - // If this point is reached the queue was not found - return -1; + // If this point is reached the queue was not found + return -1; } #if defined(PIOS_USE_SETTINGS_ON_SDCARD) /** * Wrapper for the sprintf function */ -static void customSPrintf(uint8_t * buffer, uint8_t * format, ...) +static void customSPrintf(uint8_t *buffer, uint8_t *format, ...) { - va_list args; - va_start(args, format); - vsprintf((char *)buffer, (char *)format, args); + va_list args; + + va_start(args, format); + vsprintf((char *)buffer, (char *)format, args); } /** * Get an 8 character (plus extension) filename for the object. */ -static void objectFilename(UAVObjHandle obj_handle, uint8_t * filename) +static void objectFilename(UAVObjHandle obj_handle, uint8_t *filename) { - customSPrintf(filename, (uint8_t *) "%X.obj", UAVObjGetID(obj_handle)); + customSPrintf(filename, (uint8_t *)"%X.obj", UAVObjGetID(obj_handle)); } #endif /* PIOS_USE_SETTINGS_ON_SDCARD */ diff --git a/flight/uavtalk/inc/uavtalk.h b/flight/uavtalk/inc/uavtalk.h index a11979a95..cfbeef471 100644 --- a/flight/uavtalk/inc/uavtalk.h +++ b/flight/uavtalk/inc/uavtalk.h @@ -30,7 +30,7 @@ #define UAVTALK_H // Public types -typedef int32_t (*UAVTalkOutputStream)(uint8_t* data, int32_t length); +typedef int32_t (*UAVTalkOutputStream)(uint8_t *data, int32_t length); typedef struct { uint32_t txBytes; @@ -43,9 +43,9 @@ typedef struct { uint32_t rxErrors; } UAVTalkStats; -typedef void* UAVTalkConnection; +typedef void *UAVTalkConnection; -typedef enum {UAVTALK_STATE_ERROR=0, UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_TIMESTAMP, UAVTALK_STATE_DATA, UAVTALK_STATE_CS, UAVTALK_STATE_COMPLETE} UAVTalkRxState; +typedef enum { UAVTALK_STATE_ERROR = 0, UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_TIMESTAMP, UAVTALK_STATE_DATA, UAVTALK_STATE_CS, UAVTALK_STATE_COMPLETE } UAVTalkRxState; // Public functions UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream); diff --git a/flight/uavtalk/inc/uavtalk_priv.h b/flight/uavtalk/inc/uavtalk_priv.h index 84174e90a..310f9e351 100644 --- a/flight/uavtalk/inc/uavtalk_priv.h +++ b/flight/uavtalk/inc/uavtalk_priv.h @@ -33,40 +33,40 @@ // Private types and constants typedef struct { - uint8_t sync; - uint8_t type; - uint16_t size; - uint32_t objId; + uint8_t sync; + uint8_t type; + uint16_t size; + uint32_t objId; } uavtalk_min_header; -#define UAVTALK_MIN_HEADER_LENGTH sizeof(uavtalk_min_header) +#define UAVTALK_MIN_HEADER_LENGTH sizeof(uavtalk_min_header) typedef struct { - uint8_t sync; - uint8_t type; - uint16_t size; - uint32_t objId; - uint16_t instId; - uint16_t timestamp; + uint8_t sync; + uint8_t type; + uint16_t size; + uint32_t objId; + uint16_t instId; + uint16_t timestamp; } uavtalk_max_header; -#define UAVTALK_MAX_HEADER_LENGTH sizeof(uavtalk_max_header) +#define UAVTALK_MAX_HEADER_LENGTH sizeof(uavtalk_max_header) typedef uint8_t uavtalk_checksum; -#define UAVTALK_CHECKSUM_LENGTH sizeof(uavtalk_checksum) -#define UAVTALK_MAX_PAYLOAD_LENGTH (UAVOBJECTS_LARGEST + 1) -#define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH -#define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH +#define UAVTALK_CHECKSUM_LENGTH sizeof(uavtalk_checksum) +#define UAVTALK_MAX_PAYLOAD_LENGTH (UAVOBJECTS_LARGEST + 1) +#define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH +#define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH typedef struct { UAVObjHandle obj; uint8_t type; - uint16_t packet_size; - uint32_t objId; - uint16_t instId; - uint32_t length; - uint8_t instanceLength; - uint8_t timestampLength; - uint8_t cs; - uint16_t timestamp; + uint16_t packet_size; + uint32_t objId; + uint16_t instId; + uint32_t length; + uint8_t instanceLength; + uint8_t timestampLength; + uint8_t cs; + uint16_t timestamp; uint32_t rxCount; UAVTalkRxState state; uint16_t rxPacketLength; @@ -75,39 +75,39 @@ typedef struct { typedef struct { uint8_t canari; UAVTalkOutputStream outStream; - xSemaphoreHandle lock; - xSemaphoreHandle transLock; - xSemaphoreHandle respSema; + xSemaphoreHandle lock; + xSemaphoreHandle transLock; + xSemaphoreHandle respSema; UAVObjHandle respObj; - uint16_t respInstId; + uint16_t respInstId; UAVTalkStats stats; UAVTalkInputProcessor iproc; - uint8_t *rxBuffer; - uint32_t txSize; - uint8_t *txBuffer; + uint8_t *rxBuffer; + uint32_t txSize; + uint8_t *txBuffer; } UAVTalkConnectionData; -#define UAVTALK_CANARI 0xCA +#define UAVTALK_CANARI 0xCA #define UAVTALK_WAITFOREVER -1 #define UAVTALK_NOWAIT 0 -#define UAVTALK_SYNC_VAL 0x3C -#define UAVTALK_TYPE_MASK 0x78 -#define UAVTALK_TYPE_VER 0x20 -#define UAVTALK_TIMESTAMPED 0x80 -#define UAVTALK_TYPE_OBJ (UAVTALK_TYPE_VER | 0x00) -#define UAVTALK_TYPE_OBJ_REQ (UAVTALK_TYPE_VER | 0x01) -#define UAVTALK_TYPE_OBJ_ACK (UAVTALK_TYPE_VER | 0x02) -#define UAVTALK_TYPE_ACK (UAVTALK_TYPE_VER | 0x03) -#define UAVTALK_TYPE_NACK (UAVTALK_TYPE_VER | 0x04) -#define UAVTALK_TYPE_OBJ_TS (UAVTALK_TIMESTAMPED | UAVTALK_TYPE_OBJ) -#define UAVTALK_TYPE_OBJ_ACK_TS (UAVTALK_TIMESTAMPED | UAVTALK_TYPE_OBJ_ACK) +#define UAVTALK_SYNC_VAL 0x3C +#define UAVTALK_TYPE_MASK 0x78 +#define UAVTALK_TYPE_VER 0x20 +#define UAVTALK_TIMESTAMPED 0x80 +#define UAVTALK_TYPE_OBJ (UAVTALK_TYPE_VER | 0x00) +#define UAVTALK_TYPE_OBJ_REQ (UAVTALK_TYPE_VER | 0x01) +#define UAVTALK_TYPE_OBJ_ACK (UAVTALK_TYPE_VER | 0x02) +#define UAVTALK_TYPE_ACK (UAVTALK_TYPE_VER | 0x03) +#define UAVTALK_TYPE_NACK (UAVTALK_TYPE_VER | 0x04) +#define UAVTALK_TYPE_OBJ_TS (UAVTALK_TIMESTAMPED | UAVTALK_TYPE_OBJ) +#define UAVTALK_TYPE_OBJ_ACK_TS (UAVTALK_TIMESTAMPED | UAVTALK_TYPE_OBJ_ACK) -//macros -#define CHECKCONHANDLE(handle,variable,failcommand) \ - variable = (UAVTalkConnectionData*) handle; \ - if (variable == NULL || variable->canari != UAVTALK_CANARI) { \ - failcommand; \ - } +// macros +#define CHECKCONHANDLE(handle, variable, failcommand) \ + variable = (UAVTalkConnectionData *)handle; \ + if (variable == NULL || variable->canari != UAVTALK_CANARI) { \ + failcommand; \ + } #endif // UAVTALK__PRIV_H /** diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index eb060b735..bf19512cc 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -8,8 +8,8 @@ * @file uavtalk.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief UAVTalk library, implements to telemetry protocol. See the wiki for more details. - * This library should not be called directly by the application, it is only used by the - * Telemetry module. + * This library should not be called directly by the application, it is only used by the + * Telemetry module. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ @@ -38,7 +38,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type); static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type); static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId); -static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length); +static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data, int32_t length); static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId); /** @@ -50,24 +50,31 @@ static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint1 */ UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream) { - // allocate object - UAVTalkConnectionData * connection = pvPortMalloc(sizeof(UAVTalkConnectionData)); - if (!connection) return 0; - connection->canari = UAVTALK_CANARI; - connection->iproc.rxPacketLength = 0; - connection->iproc.state = UAVTALK_STATE_SYNC; - connection->outStream = outputStream; - connection->lock = xSemaphoreCreateRecursiveMutex(); - connection->transLock = xSemaphoreCreateRecursiveMutex(); - // allocate buffers - connection->rxBuffer = pvPortMalloc(UAVTALK_MAX_PACKET_LENGTH); - if (!connection->rxBuffer) return 0; - connection->txBuffer = pvPortMalloc(UAVTALK_MAX_PACKET_LENGTH); - if (!connection->txBuffer) return 0; - vSemaphoreCreateBinary(connection->respSema); - xSemaphoreTake(connection->respSema, 0); // reset to zero - UAVTalkResetStats( (UAVTalkConnection) connection ); - return (UAVTalkConnection) connection; + // allocate object + UAVTalkConnectionData *connection = pvPortMalloc(sizeof(UAVTalkConnectionData)); + + if (!connection) { + return 0; + } + connection->canari = UAVTALK_CANARI; + connection->iproc.rxPacketLength = 0; + connection->iproc.state = UAVTALK_STATE_SYNC; + connection->outStream = outputStream; + connection->lock = xSemaphoreCreateRecursiveMutex(); + connection->transLock = xSemaphoreCreateRecursiveMutex(); + // allocate buffers + connection->rxBuffer = pvPortMalloc(UAVTALK_MAX_PACKET_LENGTH); + if (!connection->rxBuffer) { + return 0; + } + connection->txBuffer = pvPortMalloc(UAVTALK_MAX_PACKET_LENGTH); + if (!connection->txBuffer) { + return 0; + } + vSemaphoreCreateBinary(connection->respSema); + xSemaphoreTake(connection->respSema, 0); // reset to zero + UAVTalkResetStats((UAVTalkConnection)connection); + return (UAVTalkConnection)connection; } /** @@ -79,21 +86,20 @@ UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream) */ int32_t UAVTalkSetOutputStream(UAVTalkConnection connectionHandle, UAVTalkOutputStream outputStream) { + UAVTalkConnectionData *connection; - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); + CHECKCONHANDLE(connectionHandle, connection, return -1); - // Lock - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - - // set output stream - connection->outStream = outputStream; - - // Release lock - xSemaphoreGiveRecursive(connection->lock); + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - return 0; + // set output stream + connection->outStream = outputStream; + // Release lock + xSemaphoreGiveRecursive(connection->lock); + + return 0; } /** @@ -103,9 +109,10 @@ int32_t UAVTalkSetOutputStream(UAVTalkConnection connectionHandle, UAVTalkOutput */ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connectionHandle) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return NULL); - return connection->outStream; + UAVTalkConnectionData *connection; + + CHECKCONHANDLE(connectionHandle, connection, return NULL); + return connection->outStream; } /** @@ -113,19 +120,20 @@ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connectionHandle) * \param[in] connection UAVTalkConnection to be used * @param[out] statsOut Statistics counters */ -void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats* statsOut) +void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return ); + UAVTalkConnectionData *connection; - // Lock - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - - // Copy stats - memcpy(statsOut, &connection->stats, sizeof(UAVTalkStats)); - - // Release lock - xSemaphoreGiveRecursive(connection->lock); + CHECKCONHANDLE(connectionHandle, connection, return ); + + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + + // Copy stats + memcpy(statsOut, &connection->stats, sizeof(UAVTalkStats)); + + // Release lock + xSemaphoreGiveRecursive(connection->lock); } /** @@ -134,17 +142,18 @@ void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats* statsOut) */ void UAVTalkResetStats(UAVTalkConnection connectionHandle) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return); + UAVTalkConnectionData *connection; - // Lock - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - - // Clear stats - memset(&connection->stats, 0, sizeof(UAVTalkStats)); - - // Release lock - xSemaphoreGiveRecursive(connection->lock); + CHECKCONHANDLE(connectionHandle, connection, return ); + + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + + // Clear stats + memset(&connection->stats, 0, sizeof(UAVTalkStats)); + + // Release lock + xSemaphoreGiveRecursive(connection->lock); } /** @@ -152,11 +161,12 @@ void UAVTalkResetStats(UAVTalkConnection connectionHandle) */ void UAVTalkGetLastTimestamp(UAVTalkConnection connectionHandle, uint16_t *timestamp) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return); + UAVTalkConnectionData *connection; - UAVTalkInputProcessor *iproc = &connection->iproc; - *timestamp = iproc->timestamp; + CHECKCONHANDLE(connectionHandle, connection, return ); + + UAVTalkInputProcessor *iproc = &connection->iproc; + *timestamp = iproc->timestamp; } @@ -172,9 +182,10 @@ void UAVTalkGetLastTimestamp(UAVTalkConnection connectionHandle, uint16_t *times */ int32_t UAVTalkSendObjectRequest(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, int32_t timeout) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ, timeout); + UAVTalkConnectionData *connection; + + CHECKCONHANDLE(connectionHandle, connection, return -1); + return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ, timeout); } /** @@ -189,17 +200,15 @@ int32_t UAVTalkSendObjectRequest(UAVTalkConnection connectionHandle, UAVObjHandl */ int32_t UAVTalkSendObject(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); - // Send object - if (acked == 1) - { - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK, timeoutMs); - } - else - { - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ, timeoutMs); - } + UAVTalkConnectionData *connection; + + CHECKCONHANDLE(connectionHandle, connection, return -1); + // Send object + if (acked == 1) { + return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK, timeoutMs); + } else { + return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ, timeoutMs); + } } /** @@ -214,17 +223,15 @@ int32_t UAVTalkSendObject(UAVTalkConnection connectionHandle, UAVObjHandle obj, */ int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); - // Send object - if (acked == 1) - { - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK_TS, timeoutMs); - } - else - { - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_TS, timeoutMs); - } + UAVTalkConnectionData *connection; + + CHECKCONHANDLE(connectionHandle, connection, return -1); + // Send object + if (acked == 1) { + return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK_TS, timeoutMs); + } else { + return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_TS, timeoutMs); + } } /** @@ -233,57 +240,49 @@ int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjH * \param[in] obj Object * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] type Transaction type - * UAVTALK_TYPE_OBJ: send object, - * UAVTALK_TYPE_OBJ_REQ: request object update - * UAVTALK_TYPE_OBJ_ACK: send object with an ack + * UAVTALK_TYPE_OBJ: send object, + * UAVTALK_TYPE_OBJ_REQ: request object update + * UAVTALK_TYPE_OBJ_ACK: send object with an ack * \return 0 Success * \return -1 Failure */ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs) { - int32_t respReceived; - - // Send object depending on if a response is needed - if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS || type == UAVTALK_TYPE_OBJ_REQ) - { - // Get transaction lock (will block if a transaction is pending) - xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY); - // Send object - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - connection->respObj = obj; - connection->respInstId = instId; - sendObject(connection, obj, instId, type); - xSemaphoreGiveRecursive(connection->lock); - // Wait for response (or timeout) - respReceived = xSemaphoreTake(connection->respSema, timeoutMs/portTICK_RATE_MS); - // Check if a response was received - if (respReceived == pdFALSE) - { - // Cancel transaction - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - xSemaphoreTake(connection->respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema) - connection->respObj = 0; - xSemaphoreGiveRecursive(connection->lock); - xSemaphoreGiveRecursive(connection->transLock); - return -1; - } - else - { - xSemaphoreGiveRecursive(connection->transLock); - return 0; - } - } - else if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS) - { - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - sendObject(connection, obj, instId, type); - xSemaphoreGiveRecursive(connection->lock); - return 0; - } - else - { - return -1; - } + int32_t respReceived; + + // Send object depending on if a response is needed + if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS || type == UAVTALK_TYPE_OBJ_REQ) { + // Get transaction lock (will block if a transaction is pending) + xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY); + // Send object + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + connection->respObj = obj; + connection->respInstId = instId; + sendObject(connection, obj, instId, type); + xSemaphoreGiveRecursive(connection->lock); + // Wait for response (or timeout) + respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS); + // Check if a response was received + if (respReceived == pdFALSE) { + // Cancel transaction + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + xSemaphoreTake(connection->respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema) + connection->respObj = 0; + xSemaphoreGiveRecursive(connection->lock); + xSemaphoreGiveRecursive(connection->transLock); + return -1; + } else { + xSemaphoreGiveRecursive(connection->transLock); + return 0; + } + } else if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS) { + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + sendObject(connection, obj, instId, type); + xSemaphoreGiveRecursive(connection->lock); + return 0; + } else { + return -1; + } } /** @@ -294,246 +293,236 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle */ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t rxbyte) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); + UAVTalkConnectionData *connection; - UAVTalkInputProcessor *iproc = &connection->iproc; - ++connection->stats.rxBytes; + CHECKCONHANDLE(connectionHandle, connection, return -1); - if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) - iproc->state = UAVTALK_STATE_SYNC; - - if (iproc->rxPacketLength < 0xffff) - iproc->rxPacketLength++; // update packet byte count - - // Receive state machine - switch (iproc->state) - { - case UAVTALK_STATE_SYNC: - if (rxbyte != UAVTALK_SYNC_VAL) - break; - - // Initialize and update the CRC - iproc->cs = PIOS_CRC_updateByte(0, rxbyte); - - iproc->rxPacketLength = 1; - - iproc->state = UAVTALK_STATE_TYPE; - break; - - case UAVTALK_STATE_TYPE: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) - { - iproc->state = UAVTALK_STATE_ERROR; - break; - } - - iproc->type = rxbyte; - - iproc->packet_size = 0; - - iproc->state = UAVTALK_STATE_SIZE; - iproc->rxCount = 0; - break; - - case UAVTALK_STATE_SIZE: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - if (iproc->rxCount == 0) - { - iproc->packet_size += rxbyte; - iproc->rxCount++; - break; - } - - iproc->packet_size += rxbyte << 8; - - if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) - { // incorrect packet size - iproc->state = UAVTALK_STATE_ERROR; - break; - } - - iproc->rxCount = 0; - iproc->objId = 0; - iproc->state = UAVTALK_STATE_OBJID; - break; - - case UAVTALK_STATE_OBJID: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - iproc->objId += rxbyte << (8*(iproc->rxCount++)); + UAVTalkInputProcessor *iproc = &connection->iproc; + ++connection->stats.rxBytes; - if (iproc->rxCount < 4) - break; - - // Search for object. - iproc->obj = UAVObjGetByID(iproc->objId); - - // Determine data length - if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) - { - iproc->length = 0; - iproc->instanceLength = 0; - } - else - { - if (iproc->obj) - { - iproc->length = UAVObjGetNumBytes(iproc->obj); - iproc->instanceLength = (UAVObjIsSingleInstance(iproc->obj) ? 0 : 2); - iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0; - } - else - { - // We don't know if it's a multi-instance object, so just assume it's 0. - iproc->instanceLength = 0; - iproc->length = iproc->packet_size - iproc->rxPacketLength; - } - } - - // Check length and determine next state - if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) - { - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; - break; - } - - // Check the lengths match - if ((iproc->rxPacketLength + iproc->instanceLength + iproc->timestampLength + iproc->length) != iproc->packet_size) - { // packet error - mismatched packet size - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; - break; - } - - iproc->instId = 0; - if (iproc->type == UAVTALK_TYPE_NACK) - { - // If this is a NACK, we skip to Checksum - iproc->state = UAVTALK_STATE_CS; - } - // Check if this is a single instance object (i.e. if the instance ID field is coming next) - else if ((iproc->obj != 0) && !UAVObjIsSingleInstance(iproc->obj)) - { - iproc->state = UAVTALK_STATE_INSTID; - } - // Check if this is a single instance and has a timestamp in it - else if ((iproc->obj != 0) && (iproc->type & UAVTALK_TIMESTAMPED)) - { - iproc->timestamp = 0; - iproc->state = UAVTALK_STATE_TIMESTAMP; - } - else - { - // If there is a payload get it, otherwise receive checksum - if (iproc->length > 0) - iproc->state = UAVTALK_STATE_DATA; - else - iproc->state = UAVTALK_STATE_CS; - } - iproc->rxCount = 0; - - break; - - case UAVTALK_STATE_INSTID: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - iproc->instId += rxbyte << (8*(iproc->rxCount++)); + if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) { + iproc->state = UAVTALK_STATE_SYNC; + } - if (iproc->rxCount < 2) - break; - - iproc->rxCount = 0; - - // If there is a timestamp, get it - if ((iproc->length > 0) && (iproc->type & UAVTALK_TIMESTAMPED)) - { - iproc->timestamp = 0; - iproc->state = UAVTALK_STATE_TIMESTAMP; - } - // If there is a payload get it, otherwise receive checksum - else if (iproc->length > 0) - iproc->state = UAVTALK_STATE_DATA; - else - iproc->state = UAVTALK_STATE_CS; - - break; + if (iproc->rxPacketLength < 0xffff) { + iproc->rxPacketLength++; // update packet byte count + } + // Receive state machine + switch (iproc->state) { + case UAVTALK_STATE_SYNC: + if (rxbyte != UAVTALK_SYNC_VAL) { + break; + } - case UAVTALK_STATE_TIMESTAMP: - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + // Initialize and update the CRC + iproc->cs = PIOS_CRC_updateByte(0, rxbyte); - iproc->timestamp += rxbyte << (8*(iproc->rxCount++)); + iproc->rxPacketLength = 1; - if (iproc->rxCount < 2) - break; + iproc->state = UAVTALK_STATE_TYPE; + break; - iproc->rxCount = 0; + case UAVTALK_STATE_TYPE: - // If there is a payload get it, otherwise receive checksum - if (iproc->length > 0) - iproc->state = UAVTALK_STATE_DATA; - else - iproc->state = UAVTALK_STATE_CS; - break; + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - case UAVTALK_STATE_DATA: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - connection->rxBuffer[iproc->rxCount++] = rxbyte; - if (iproc->rxCount < iproc->length) - break; - - iproc->state = UAVTALK_STATE_CS; - iproc->rxCount = 0; - break; - - case UAVTALK_STATE_CS: - - // the CRC byte - if (rxbyte != iproc->cs) - { // packet error - faulty CRC - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; - break; - } - - if (iproc->rxPacketLength != (iproc->packet_size + 1)) - { // packet error - mismatched packet size - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; - break; - } + if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) { + iproc->state = UAVTALK_STATE_ERROR; + break; + } - connection->stats.rxObjectBytes += iproc->length; - connection->stats.rxObjects++; + iproc->type = rxbyte; - iproc->state = UAVTALK_STATE_COMPLETE; - break; - - default: - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; - } - - // Done - return iproc->state; + iproc->packet_size = 0; + + iproc->state = UAVTALK_STATE_SIZE; + iproc->rxCount = 0; + break; + + case UAVTALK_STATE_SIZE: + + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + + if (iproc->rxCount == 0) { + iproc->packet_size += rxbyte; + iproc->rxCount++; + break; + } + + iproc->packet_size += rxbyte << 8; + + if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) { // incorrect packet size + iproc->state = UAVTALK_STATE_ERROR; + break; + } + + iproc->rxCount = 0; + iproc->objId = 0; + iproc->state = UAVTALK_STATE_OBJID; + break; + + case UAVTALK_STATE_OBJID: + + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + + iproc->objId += rxbyte << (8 * (iproc->rxCount++)); + + if (iproc->rxCount < 4) { + break; + } + + // Search for object. + iproc->obj = UAVObjGetByID(iproc->objId); + + // Determine data length + if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) { + iproc->length = 0; + iproc->instanceLength = 0; + } else { + if (iproc->obj) { + iproc->length = UAVObjGetNumBytes(iproc->obj); + iproc->instanceLength = (UAVObjIsSingleInstance(iproc->obj) ? 0 : 2); + iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0; + } else { + // We don't know if it's a multi-instance object, so just assume it's 0. + iproc->instanceLength = 0; + iproc->length = iproc->packet_size - iproc->rxPacketLength; + } + } + + // Check length and determine next state + if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) { + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + break; + } + + // Check the lengths match + if ((iproc->rxPacketLength + iproc->instanceLength + iproc->timestampLength + iproc->length) != iproc->packet_size) { // packet error - mismatched packet size + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + break; + } + + iproc->instId = 0; + if (iproc->type == UAVTALK_TYPE_NACK) { + // If this is a NACK, we skip to Checksum + iproc->state = UAVTALK_STATE_CS; + } + // Check if this is a single instance object (i.e. if the instance ID field is coming next) + else if ((iproc->obj != 0) && !UAVObjIsSingleInstance(iproc->obj)) { + iproc->state = UAVTALK_STATE_INSTID; + } + // Check if this is a single instance and has a timestamp in it + else if ((iproc->obj != 0) && (iproc->type & UAVTALK_TIMESTAMPED)) { + iproc->timestamp = 0; + iproc->state = UAVTALK_STATE_TIMESTAMP; + } else { + // If there is a payload get it, otherwise receive checksum + if (iproc->length > 0) { + iproc->state = UAVTALK_STATE_DATA; + } else { + iproc->state = UAVTALK_STATE_CS; + } + } + iproc->rxCount = 0; + + break; + + case UAVTALK_STATE_INSTID: + + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + + iproc->instId += rxbyte << (8 * (iproc->rxCount++)); + + if (iproc->rxCount < 2) { + break; + } + + iproc->rxCount = 0; + + // If there is a timestamp, get it + if ((iproc->length > 0) && (iproc->type & UAVTALK_TIMESTAMPED)) { + iproc->timestamp = 0; + iproc->state = UAVTALK_STATE_TIMESTAMP; + } + // If there is a payload get it, otherwise receive checksum + else if (iproc->length > 0) { + iproc->state = UAVTALK_STATE_DATA; + } else { + iproc->state = UAVTALK_STATE_CS; + } + + break; + + case UAVTALK_STATE_TIMESTAMP: + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + + iproc->timestamp += rxbyte << (8 * (iproc->rxCount++)); + + if (iproc->rxCount < 2) { + break; + } + + iproc->rxCount = 0; + + // If there is a payload get it, otherwise receive checksum + if (iproc->length > 0) { + iproc->state = UAVTALK_STATE_DATA; + } else { + iproc->state = UAVTALK_STATE_CS; + } + break; + + case UAVTALK_STATE_DATA: + + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + + connection->rxBuffer[iproc->rxCount++] = rxbyte; + if (iproc->rxCount < iproc->length) { + break; + } + + iproc->state = UAVTALK_STATE_CS; + iproc->rxCount = 0; + break; + + case UAVTALK_STATE_CS: + + // the CRC byte + if (rxbyte != iproc->cs) { // packet error - faulty CRC + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + break; + } + + if (iproc->rxPacketLength != (iproc->packet_size + 1)) { // packet error - mismatched packet size + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + break; + } + + connection->stats.rxObjectBytes += iproc->length; + connection->stats.rxObjects++; + + iproc->state = UAVTALK_STATE_COMPLETE; + break; + + default: + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + } + + // Done + return iproc->state; } - + /** * Process an byte from the telemetry stream. * \param[in] connection UAVTalkConnection to be used @@ -542,20 +531,19 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle */ UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) { - UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte); + UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte); - if (state == UAVTALK_STATE_COMPLETE) - { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); - UAVTalkInputProcessor *iproc = &connection->iproc; + if (state == UAVTALK_STATE_COMPLETE) { + UAVTalkConnectionData *connection; + CHECKCONHANDLE(connectionHandle, connection, return -1); + UAVTalkInputProcessor *iproc = &connection->iproc; - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length); - xSemaphoreGiveRecursive(connection->lock); - } + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length); + xSemaphoreGiveRecursive(connection->lock); + } - return state; + return state; } /** @@ -570,59 +558,59 @@ UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uin */ UAVTalkRxState UAVTalkRelayInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) { - UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte); + UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte); - if (state == UAVTALK_STATE_COMPLETE) - { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); - UAVTalkInputProcessor *iproc = &connection->iproc; + if (state == UAVTALK_STATE_COMPLETE) { + UAVTalkConnectionData *connection; + CHECKCONHANDLE(connectionHandle, connection, return -1); + UAVTalkInputProcessor *iproc = &connection->iproc; - if (!connection->outStream) return -1; + if (!connection->outStream) { + return -1; + } - // Setup type and object id fields - connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte - connection->txBuffer[1] = iproc->type; - // data length inserted here below - connection->txBuffer[4] = (uint8_t)(iproc->objId & 0xFF); - connection->txBuffer[5] = (uint8_t)((iproc->objId >> 8) & 0xFF); - connection->txBuffer[6] = (uint8_t)((iproc->objId >> 16) & 0xFF); - connection->txBuffer[7] = (uint8_t)((iproc->objId >> 24) & 0xFF); - - // Setup instance ID if one is required - int32_t dataOffset = 8; - if (iproc->instanceLength > 0) - { - connection->txBuffer[8] = (uint8_t)(iproc->instId & 0xFF); - connection->txBuffer[9] = (uint8_t)((iproc->instId >> 8) & 0xFF); - dataOffset = 10; - } + // Setup type and object id fields + connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte + connection->txBuffer[1] = iproc->type; + // data length inserted here below + connection->txBuffer[4] = (uint8_t)(iproc->objId & 0xFF); + connection->txBuffer[5] = (uint8_t)((iproc->objId >> 8) & 0xFF); + connection->txBuffer[6] = (uint8_t)((iproc->objId >> 16) & 0xFF); + connection->txBuffer[7] = (uint8_t)((iproc->objId >> 24) & 0xFF); - // Add timestamp when the transaction type is appropriate - if (iproc->type & UAVTALK_TIMESTAMPED) - { - portTickType time = xTaskGetTickCount(); - connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); - connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); - dataOffset += 2; - } - - // Copy data (if any) - memcpy(&connection->txBuffer[dataOffset], connection->rxBuffer, iproc->length); - - // Store the packet length - connection->txBuffer[2] = (uint8_t)((dataOffset + iproc->length) & 0xFF); - connection->txBuffer[3] = (uint8_t)(((dataOffset + iproc->length) >> 8) & 0xFF); - - // Copy the checksum - connection->txBuffer[dataOffset + iproc->length] = iproc->cs; + // Setup instance ID if one is required + int32_t dataOffset = 8; + if (iproc->instanceLength > 0) { + connection->txBuffer[8] = (uint8_t)(iproc->instId & 0xFF); + connection->txBuffer[9] = (uint8_t)((iproc->instId >> 8) & 0xFF); + dataOffset = 10; + } - // Send the buffer. - if (UAVTalkSendBuf(connectionHandle, connection->txBuffer, iproc->rxPacketLength) < 0) - return UAVTALK_STATE_ERROR; - } + // Add timestamp when the transaction type is appropriate + if (iproc->type & UAVTALK_TIMESTAMPED) { + portTickType time = xTaskGetTickCount(); + connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); + connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); + dataOffset += 2; + } - return state; + // Copy data (if any) + memcpy(&connection->txBuffer[dataOffset], connection->rxBuffer, iproc->length); + + // Store the packet length + connection->txBuffer[2] = (uint8_t)((dataOffset + iproc->length) & 0xFF); + connection->txBuffer[3] = (uint8_t)(((dataOffset + iproc->length) >> 8) & 0xFF); + + // Copy the checksum + connection->txBuffer[dataOffset + iproc->length] = iproc->cs; + + // Send the buffer. + if (UAVTalkSendBuf(connectionHandle, connection->txBuffer, iproc->rxPacketLength) < 0) { + return UAVTALK_STATE_ERROR; + } + } + + return state; } /** @@ -634,18 +622,19 @@ UAVTalkRxState UAVTalkRelayInputStream(UAVTalkConnection connectionHandle, uint8 */ int32_t UAVTalkSendAck(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); + UAVTalkConnectionData *connection; - // Lock - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + CHECKCONHANDLE(connectionHandle, connection, return -1); - int32_t ret = sendObject(connection, obj, instId, UAVTALK_TYPE_ACK); - - // Release lock - xSemaphoreGiveRecursive(connection->lock); + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - return ret; + int32_t ret = sendObject(connection, obj, instId, UAVTALK_TYPE_ACK); + + // Release lock + xSemaphoreGiveRecursive(connection->lock); + + return ret; } /** @@ -657,18 +646,19 @@ int32_t UAVTalkSendAck(UAVTalkConnection connectionHandle, UAVObjHandle obj, uin */ int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); + UAVTalkConnectionData *connection; - // Lock - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + CHECKCONHANDLE(connectionHandle, connection, return -1); - int32_t ret = sendNack(connection, objId); - - // Release lock - xSemaphoreGiveRecursive(connection->lock); + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - return ret; + int32_t ret = sendNack(connection, objId); + + // Release lock + xSemaphoreGiveRecursive(connection->lock); + + return ret; } /** @@ -682,25 +672,27 @@ int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId) */ int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_t len) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection, return -1); + UAVTalkConnectionData *connection; - // Lock - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + CHECKCONHANDLE(connectionHandle, connection, return -1); - // Output the buffer - int32_t rc = (*connection->outStream)(buf, len); + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - // Update stats - connection->stats.txBytes += len; + // Output the buffer + int32_t rc = (*connection->outStream)(buf, len); - // Release lock - xSemaphoreGiveRecursive(connection->lock); + // Update stats + connection->stats.txBytes += len; - // Done - if (rc != len) - return -1; - return 0; + // Release lock + xSemaphoreGiveRecursive(connection->lock); + + // Done + if (rc != len) { + return -1; + } + return 0; } /** @@ -715,84 +707,73 @@ int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_ * \return -1 Failure */ static int32_t receiveObject(UAVTalkConnectionData *connection, - uint8_t type, - uint32_t objId, - uint16_t instId, - uint8_t* data, - __attribute__((unused))int32_t length) + uint8_t type, + uint32_t objId, + uint16_t instId, + uint8_t *data, + __attribute__((unused)) int32_t length) { - UAVObjHandle obj; - int32_t ret = 0; + UAVObjHandle obj; + int32_t ret = 0; - // Get the handle to the Object. Will be zero - // if object does not exist. - obj = UAVObjGetByID(objId); - - // Process message type - switch (type) { - case UAVTALK_TYPE_OBJ: - case UAVTALK_TYPE_OBJ_TS: - // All instances, not allowed for OBJ messages - if (obj && (instId != UAVOBJ_ALL_INSTANCES)) - { - // Unpack object, if the instance does not exist it will be created! - UAVObjUnpack(obj, instId, data); - // Check if an ack is pending - updateAck(connection, obj, instId); - } - else - { - ret = -1; - } - break; - case UAVTALK_TYPE_OBJ_ACK: - case UAVTALK_TYPE_OBJ_ACK_TS: - // All instances, not allowed for OBJ_ACK messages - if (obj && (instId != UAVOBJ_ALL_INSTANCES)) - { - // Unpack object, if the instance does not exist it will be created! - if ( UAVObjUnpack(obj, instId, data) == 0 ) - { - // Transmit ACK - sendObject(connection, obj, instId, UAVTALK_TYPE_ACK); - } - else - { - ret = -1; - } - } - else - { - ret = -1; - } - break; - case UAVTALK_TYPE_OBJ_REQ: - // Send requested object if message is of type OBJ_REQ - if (obj == 0) - sendNack(connection, objId); - else - sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ); - break; - case UAVTALK_TYPE_NACK: - // Do nothing on flight side, let it time out. - break; - case UAVTALK_TYPE_ACK: - // All instances, not allowed for ACK messages - if (obj && (instId != UAVOBJ_ALL_INSTANCES)) - { - // Check if an ack is pending - updateAck(connection, obj, instId); - } - else - { - ret = -1; - } - break; - default: - ret = -1; - } - // Done - return ret; + // Get the handle to the Object. Will be zero + // if object does not exist. + obj = UAVObjGetByID(objId); + + // Process message type + switch (type) { + case UAVTALK_TYPE_OBJ: + case UAVTALK_TYPE_OBJ_TS: + // All instances, not allowed for OBJ messages + if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { + // Unpack object, if the instance does not exist it will be created! + UAVObjUnpack(obj, instId, data); + // Check if an ack is pending + updateAck(connection, obj, instId); + } else { + ret = -1; + } + break; + case UAVTALK_TYPE_OBJ_ACK: + case UAVTALK_TYPE_OBJ_ACK_TS: + // All instances, not allowed for OBJ_ACK messages + if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { + // Unpack object, if the instance does not exist it will be created! + if (UAVObjUnpack(obj, instId, data) == 0) { + // Transmit ACK + sendObject(connection, obj, instId, UAVTALK_TYPE_ACK); + } else { + ret = -1; + } + } else { + ret = -1; + } + break; + case UAVTALK_TYPE_OBJ_REQ: + // Send requested object if message is of type OBJ_REQ + if (obj == 0) { + sendNack(connection, objId); + } else { + sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ); + } + break; + case UAVTALK_TYPE_NACK: + // Do nothing on flight side, let it time out. + break; + case UAVTALK_TYPE_ACK: + // All instances, not allowed for ACK messages + if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { + // Check if an ack is pending + updateAck(connection, obj, instId); + } else { + ret = -1; + } + break; + default: + ret = -1; + } + // Done + return ret; } /** @@ -803,11 +784,10 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, */ static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId) { - if (connection->respObj == obj && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES)) - { - xSemaphoreGive(connection->respSema); - connection->respObj = 0; - } + if (connection->respObj == obj && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES)) { + xSemaphoreGive(connection->respSema); + connection->respObj = 0; + } } /** @@ -821,53 +801,38 @@ static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint1 */ static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type) { - uint32_t numInst; - uint32_t n; - - // If all instances are requested and this is a single instance object, force instance ID to zero - if ( instId == UAVOBJ_ALL_INSTANCES && UAVObjIsSingleInstance(obj) ) - { - instId = 0; - } - - // Process message type - if ( type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS || type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS ) - { - if (instId == UAVOBJ_ALL_INSTANCES) - { - // Get number of instances - numInst = UAVObjGetNumInstances(obj); - // Send all instances - for (n = 0; n < numInst; ++n) - { - sendSingleObject(connection, obj, n, type); - } - return 0; - } - else - { - return sendSingleObject(connection, obj, instId, type); - } - } - else if (type == UAVTALK_TYPE_OBJ_REQ) - { - return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ); - } - else if (type == UAVTALK_TYPE_ACK) - { - if ( instId != UAVOBJ_ALL_INSTANCES ) - { - return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_ACK); - } - else - { - return -1; - } - } - else - { - return -1; - } + uint32_t numInst; + uint32_t n; + + // If all instances are requested and this is a single instance object, force instance ID to zero + if (instId == UAVOBJ_ALL_INSTANCES && UAVObjIsSingleInstance(obj)) { + instId = 0; + } + + // Process message type + if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS || type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS) { + if (instId == UAVOBJ_ALL_INSTANCES) { + // Get number of instances + numInst = UAVObjGetNumInstances(obj); + // Send all instances + for (n = 0; n < numInst; ++n) { + sendSingleObject(connection, obj, n, type); + } + return 0; + } else { + return sendSingleObject(connection, obj, instId, type); + } + } else if (type == UAVTALK_TYPE_OBJ_REQ) { + return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ); + } else if (type == UAVTALK_TYPE_ACK) { + if (instId != UAVOBJ_ALL_INSTANCES) { + return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_ACK); + } else { + return -1; + } + } else { + return -1; + } } /** @@ -881,87 +846,79 @@ static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, u */ static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type) { - int32_t length; - int32_t dataOffset; - uint32_t objId; + int32_t length; + int32_t dataOffset; + uint32_t objId; - if (!connection->outStream) return -1; + if (!connection->outStream) { + return -1; + } - // Setup type and object id fields - objId = UAVObjGetID(obj); - connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte - connection->txBuffer[1] = type; - // data length inserted here below - connection->txBuffer[4] = (uint8_t)(objId & 0xFF); - connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); - connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); - connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); - - // Setup instance ID if one is required - if (UAVObjIsSingleInstance(obj)) - { - dataOffset = 8; - } - else - { - connection->txBuffer[8] = (uint8_t)(instId & 0xFF); - connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF); - dataOffset = 10; - } + // Setup type and object id fields + objId = UAVObjGetID(obj); + connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte + connection->txBuffer[1] = type; + // data length inserted here below + connection->txBuffer[4] = (uint8_t)(objId & 0xFF); + connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); + connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); + connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); - // Add timestamp when the transaction type is appropriate - if (type & UAVTALK_TIMESTAMPED) - { - portTickType time = xTaskGetTickCount(); - connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); - connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); - dataOffset += 2; - } - - // Determine data length - if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK) - { - length = 0; - } - else - { - length = UAVObjGetNumBytes(obj); - } - - // Check length - if (length >= UAVTALK_MAX_PAYLOAD_LENGTH) - { - return -1; - } - - // Copy data (if any) - if (length > 0) - { - if ( UAVObjPack(obj, instId, &connection->txBuffer[dataOffset]) < 0 ) - { - return -1; - } - } - - // Store the packet length - connection->txBuffer[2] = (uint8_t)((dataOffset+length) & 0xFF); - connection->txBuffer[3] = (uint8_t)(((dataOffset+length) >> 8) & 0xFF); - - // Calculate checksum - connection->txBuffer[dataOffset+length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset+length); + // Setup instance ID if one is required + if (UAVObjIsSingleInstance(obj)) { + dataOffset = 8; + } else { + connection->txBuffer[8] = (uint8_t)(instId & 0xFF); + connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF); + dataOffset = 10; + } - uint16_t tx_msg_len = dataOffset+length+UAVTALK_CHECKSUM_LENGTH; - int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len); + // Add timestamp when the transaction type is appropriate + if (type & UAVTALK_TIMESTAMPED) { + portTickType time = xTaskGetTickCount(); + connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); + connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); + dataOffset += 2; + } - if (rc == tx_msg_len) { - // Update stats - ++connection->stats.txObjects; - connection->stats.txBytes += tx_msg_len; - connection->stats.txObjectBytes += length; - } - - // Done - return 0; + // Determine data length + if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK) { + length = 0; + } else { + length = UAVObjGetNumBytes(obj); + } + + // Check length + if (length >= UAVTALK_MAX_PAYLOAD_LENGTH) { + return -1; + } + + // Copy data (if any) + if (length > 0) { + if (UAVObjPack(obj, instId, &connection->txBuffer[dataOffset]) < 0) { + return -1; + } + } + + // Store the packet length + connection->txBuffer[2] = (uint8_t)((dataOffset + length) & 0xFF); + connection->txBuffer[3] = (uint8_t)(((dataOffset + length) >> 8) & 0xFF); + + // Calculate checksum + connection->txBuffer[dataOffset + length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset + length); + + uint16_t tx_msg_len = dataOffset + length + UAVTALK_CHECKSUM_LENGTH; + int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len); + + if (rc == tx_msg_len) { + // Update stats + ++connection->stats.txObjects; + connection->stats.txBytes += tx_msg_len; + connection->stats.txObjectBytes += length; + } + + // Done + return 0; } /** @@ -973,37 +930,39 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle */ static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId) { - int32_t dataOffset; + int32_t dataOffset; - if (!connection->outStream) return -1; + if (!connection->outStream) { + return -1; + } - connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte - connection->txBuffer[1] = UAVTALK_TYPE_NACK; - // data length inserted here below - connection->txBuffer[4] = (uint8_t)(objId & 0xFF); - connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); - connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); - connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); + connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte + connection->txBuffer[1] = UAVTALK_TYPE_NACK; + // data length inserted here below + connection->txBuffer[4] = (uint8_t)(objId & 0xFF); + connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); + connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); + connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); - dataOffset = 8; + dataOffset = 8; - // Store the packet length - connection->txBuffer[2] = (uint8_t)((dataOffset) & 0xFF); - connection->txBuffer[3] = (uint8_t)(((dataOffset) >> 8) & 0xFF); + // Store the packet length + connection->txBuffer[2] = (uint8_t)((dataOffset) & 0xFF); + connection->txBuffer[3] = (uint8_t)(((dataOffset) >> 8) & 0xFF); - // Calculate checksum - connection->txBuffer[dataOffset] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset); + // Calculate checksum + connection->txBuffer[dataOffset] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset); - uint16_t tx_msg_len = dataOffset+UAVTALK_CHECKSUM_LENGTH; - int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len); + uint16_t tx_msg_len = dataOffset + UAVTALK_CHECKSUM_LENGTH; + int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len); - if (rc == tx_msg_len) { - // Update stats - connection->stats.txBytes += tx_msg_len; - } + if (rc == tx_msg_len) { + // Update stats + connection->stats.txBytes += tx_msg_len; + } - // Done - return 0; + // Done + return 0; } /** diff --git a/ground/openpilotgcs/src/app/gcssplashscreen.cpp b/ground/openpilotgcs/src/app/gcssplashscreen.cpp index 568f2c683..84a0c4245 100644 --- a/ground/openpilotgcs/src/app/gcssplashscreen.cpp +++ b/ground/openpilotgcs/src/app/gcssplashscreen.cpp @@ -35,6 +35,7 @@ GCSSplashScreen::GCSSplashScreen() : { QString revision; QString year; + #ifdef GCS_REVISION revision = GCS_REVISION; #else @@ -48,7 +49,7 @@ GCSSplashScreen::GCSSplashScreen() : #endif setWindowFlags(windowFlags()); - m_pixmap = new QPixmap(":/app/splash.png"); + m_pixmap = new QPixmap(":/app/splash.png"); m_painter = new QPainter(m_pixmap); m_painter->setPen(Qt::lightGray); @@ -58,19 +59,19 @@ GCSSplashScreen::GCSSplashScreen() : QString(" 2010-") + year + QString(tr(" The OpenPilot Project - All Rights Reserved"))); - m_painter->drawText(406, 173, 310, 100, Qt::TextWordWrap|Qt::AlignTop|Qt::AlignLeft, + m_painter->drawText(406, 173, 310, 100, Qt::TextWordWrap | Qt::AlignTop | Qt::AlignLeft, QString(tr("GCS Revision - ")) + revision); setPixmap(*m_pixmap); } GCSSplashScreen::~GCSSplashScreen() -{ -} +{} void GCSSplashScreen::drawMessageText(const QString &message) { QPixmap pix(*m_pixmap); QPainter progressPainter(&pix); + progressPainter.setPen(Qt::lightGray); QFont font("Tahoma", 13); progressPainter.setFont(font); @@ -79,7 +80,8 @@ void GCSSplashScreen::drawMessageText(const QString &message) } void GCSSplashScreen::showPluginLoadingProgress(ExtensionSystem::PluginSpec *pluginSpec) -{ +{ QString message(tr("Loading ") + pluginSpec->name() + " plugin..."); + drawMessageText(message); } diff --git a/ground/openpilotgcs/src/app/gcssplashscreen.h b/ground/openpilotgcs/src/app/gcssplashscreen.h index 6c04e2367..bcf1ee9ad 100644 --- a/ground/openpilotgcs/src/app/gcssplashscreen.h +++ b/ground/openpilotgcs/src/app/gcssplashscreen.h @@ -35,22 +35,23 @@ #include "../gcs_version_info.h" -class GCSSplashScreen : public QSplashScreen -{ +class GCSSplashScreen : public QSplashScreen { Q_OBJECT public: explicit GCSSplashScreen(); ~GCSSplashScreen(); - + public slots: void showPluginLoadingProgress(ExtensionSystem::PluginSpec *pluginSpec); - void showProgressMessage(const QString &message) { drawMessageText(message); } + void showProgressMessage(const QString &message) + { + drawMessageText(message); + } private: QPixmap *m_pixmap; QPainter *m_painter; void drawMessageText(const QString &message); - }; #endif // GCSSPLASHSCREEN_H diff --git a/ground/openpilotgcs/src/app/main.cpp b/ground/openpilotgcs/src/app/main.cpp index a9183f0af..f58f46077 100644 --- a/ground/openpilotgcs/src/app/main.cpp +++ b/ground/openpilotgcs/src/app/main.cpp @@ -4,76 +4,76 @@ * @file main.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ /* - The GCS locale is set to the system locale by default unless the "hidden" setting General/Locale has a value. - The user can not change General/Locale from the Options dialog. + The GCS locale is set to the system locale by default unless the "hidden" setting General/Locale has a value. + The user can not change General/Locale from the Options dialog. - The GCS language will default to the GCS locale unless the General/OverrideLanguage has a value. - The user can change General/OverrideLanguage to any available language from the Options dialog. + The GCS language will default to the GCS locale unless the General/OverrideLanguage has a value. + The user can change General/OverrideLanguage to any available language from the Options dialog. - Both General/Locale and General/OverrideLanguage can be set from the command line or through the the factory defaults file. + Both General/Locale and General/OverrideLanguage can be set from the command line or through the the factory defaults file. - The -D option is used to permanently set a user setting. + The -D option is used to permanently set a user setting. - The -reset switch will clear all the user settings and will trigger a reload of the factory defaults. + The -reset switch will clear all the user settings and will trigger a reload of the factory defaults. - You can combine it with the -config-file= command line argument to quickly switch between multiple settings files. + You can combine it with the -config-file= command line argument to quickly switch between multiple settings files. - [code] - openpilotgcs -reset -config-file ./MyOpenPilotGCS.xml - [/code] + [code] + openpilotgcs -reset -config-file ./MyOpenPilotGCS.xml + [/code] - Relative paths are relative to /share/openpilotgcs/default_configurations/ + Relative paths are relative to /share/openpilotgcs/default_configurations/ - The specified file will be used to load the factory defaults from but only when the user settings are empty. - If the user settings are not empty the file will not be used. - This switch is useful on the 1st run when the user settings are empty or in combination with -reset. + The specified file will be used to load the factory defaults from but only when the user settings are empty. + If the user settings are not empty the file will not be used. + This switch is useful on the 1st run when the user settings are empty or in combination with -reset. - Quickly switch configurations + Quickly switch configurations - [code] - openpilotgcs -reset -config-file - [/code] + [code] + openpilotgcs -reset -config-file + [/code] - Configuring GCS from installer + Configuring GCS from installer - The -D option is used to permanently set a user setting. + The -D option is used to permanently set a user setting. - If the user chooses to start GCS at the end of the installer: + If the user chooses to start GCS at the end of the installer: - [code] - openpilotgcs -D General/OverrideLanguage=de - [/code] + [code] + openpilotgcs -D General/OverrideLanguage=de + [/code] - If the user chooses not to start GCS at the end of the installer, you still need to configure GCS. - In that case you can use -exit-after-config + If the user chooses not to start GCS at the end of the installer, you still need to configure GCS. + In that case you can use -exit-after-config - [code] - openpilotgcs -D General/OverrideLanguage=de -exit-after-config - [/code] + [code] + openpilotgcs -D General/OverrideLanguage=de -exit-after-config + [/code] */ @@ -102,307 +102,314 @@ #include namespace { +typedef QList PluginSpecSet; +typedef QMap AppOptions; +typedef QMap AppOptionValues; - typedef QList PluginSpecSet; - typedef QMap AppOptions; - typedef QMap AppOptionValues; +const int OptionIndent = 4; +const int DescriptionIndent = 24; - const int OptionIndent = 4; - const int DescriptionIndent = 24; +const QLatin1String APP_NAME("OpenPilot GCS"); - const QLatin1String APP_NAME("OpenPilot GCS"); +const QLatin1String CORE_PLUGIN_NAME("Core"); - const QLatin1String CORE_PLUGIN_NAME("Core"); - - const QLatin1String SETTINGS_ORG_NAME("OpenPilot"); - const QLatin1String SETTINGS_APP_NAME("OpenPilotGCS_config"); +const QLatin1String SETTINGS_ORG_NAME("OpenPilot"); +const QLatin1String SETTINGS_APP_NAME("OpenPilotGCS_config"); #ifdef Q_OS_MAC - const QLatin1String SHARE_PATH("/../Resources"); +const QLatin1String SHARE_PATH("/../Resources"); #else - const QLatin1String SHARE_PATH("/../share/openpilotgcs"); +const QLatin1String SHARE_PATH("/../share/openpilotgcs"); #endif - const char *DEFAULT_CONFIG_FILENAME = "OpenPilotGCS.xml"; +const char *DEFAULT_CONFIG_FILENAME = "OpenPilotGCS.xml"; - const char *fixedOptionsC = " [OPTION]... [FILE]...\n" - "Options:\n" - " -help Display this help\n" - " -version Display application version\n" - " -no-splash Don't display splash screen\n" - " -client Attempt to connect to already running instance\n" - " -D = Permanently set a user setting, e.g: -D General/OverrideLanguage=de\n" - " -reset Reset user settings to factory defaults.\n" - " -config-file Specify alternate factory defaults settings file (used with -reset)\n" - " -exit-after-config Exit after manipulating configuration settings\n"; +const char *fixedOptionsC = " [OPTION]... [FILE]...\n" + "Options:\n" + " -help Display this help\n" + " -version Display application version\n" + " -no-splash Don't display splash screen\n" + " -client Attempt to connect to already running instance\n" + " -D = Permanently set a user setting, e.g: -D General/OverrideLanguage=de\n" + " -reset Reset user settings to factory defaults.\n" + " -config-file Specify alternate factory defaults settings file (used with -reset)\n" + " -exit-after-config Exit after manipulating configuration settings\n"; - const QLatin1String HELP1_OPTION("-h"); - const QLatin1String HELP2_OPTION("-help"); - const QLatin1String HELP3_OPTION("/h"); - const QLatin1String HELP4_OPTION("--help"); - const QLatin1String VERSION_OPTION("-version"); - const QLatin1String NO_SPLASH_OPTION("-no-splash"); - const QLatin1String CLIENT_OPTION("-client"); - const QLatin1String CONFIG_OPTION("-D"); - const QLatin1String RESET_OPTION("-reset"); - const QLatin1String CONFIG_FILE_OPTION("-config-file"); - const QLatin1String EXIT_AFTER_CONFIG_OPTION("-exit-after-config"); +const QLatin1String HELP1_OPTION("-h"); +const QLatin1String HELP2_OPTION("-help"); +const QLatin1String HELP3_OPTION("/h"); +const QLatin1String HELP4_OPTION("--help"); +const QLatin1String VERSION_OPTION("-version"); +const QLatin1String NO_SPLASH_OPTION("-no-splash"); +const QLatin1String CLIENT_OPTION("-client"); +const QLatin1String CONFIG_OPTION("-D"); +const QLatin1String RESET_OPTION("-reset"); +const QLatin1String CONFIG_FILE_OPTION("-config-file"); +const QLatin1String EXIT_AFTER_CONFIG_OPTION("-exit-after-config"); - // Helpers for displaying messages. Note that there is no console on Windows. - void displayHelpText(QString t) - { +// Helpers for displaying messages. Note that there is no console on Windows. +void displayHelpText(QString t) +{ #ifdef Q_OS_WIN - // No console on Windows. (???) - // TODO there is a console on windows and popups are not always desired - t.replace(QLatin1Char('&'), QLatin1String("&")); - t.replace(QLatin1Char('<'), QLatin1String("<")); - t.replace(QLatin1Char('>'), QLatin1String(">")); - t.insert(0, QLatin1String("
"));
-        t.append(QLatin1String("
")); - QMessageBox::information(0, APP_NAME, t); + // No console on Windows. (???) + // TODO there is a console on windows and popups are not always desired + t.replace(QLatin1Char('&'), QLatin1String("&")); + t.replace(QLatin1Char('<'), QLatin1String("<")); + t.replace(QLatin1Char('>'), QLatin1String(">")); + t.insert(0, QLatin1String("
"));
+    t.append(QLatin1String("
")); + QMessageBox::information(0, APP_NAME, t); #else - qWarning("%s", qPrintable(t)); + qWarning("%s", qPrintable(t)); #endif - } +} - void displayError(const QString &t) - { +void displayError(const QString &t) +{ #ifdef Q_OS_WIN - // No console on Windows. (???) - // TODO there is a console on windows and popups are not always desired - QMessageBox::critical(0, APP_NAME, t); + // No console on Windows. (???) + // TODO there is a console on windows and popups are not always desired + QMessageBox::critical(0, APP_NAME, t); #else - qCritical("%s", qPrintable(t)); + qCritical("%s", qPrintable(t)); #endif - } +} - void printVersion(const ExtensionSystem::PluginSpec *corePlugin, const ExtensionSystem::PluginManager &pm) - { - QString version; - QTextStream str(&version); - str << '\n' << APP_NAME << ' ' << corePlugin->version() << " based on Qt " << qVersion() << "\n\n"; - pm.formatPluginVersions(str); - str << '\n' << corePlugin->copyright() << '\n'; - displayHelpText(version); - } +void printVersion(const ExtensionSystem::PluginSpec *corePlugin, const ExtensionSystem::PluginManager &pm) +{ + QString version; + QTextStream str(&version); - void printHelp(const QString &appExecName, const ExtensionSystem::PluginManager &pm) - { - QString help; - QTextStream str(&help); - str << "Usage: " << appExecName << fixedOptionsC; - ExtensionSystem::PluginManager::formatOptions(str, OptionIndent, DescriptionIndent); - pm.formatPluginOptions(str, OptionIndent, DescriptionIndent); - displayHelpText(help); - } + str << '\n' << APP_NAME << ' ' << corePlugin->version() << " based on Qt " << qVersion() << "\n\n"; + pm.formatPluginVersions(str); + str << '\n' << corePlugin->copyright() << '\n'; + displayHelpText(version); +} - inline QString msgCoreLoadFailure(const QString &reason) - { - return QCoreApplication::translate("Application", "Failed to load core plug-in, reason is: %1").arg(reason); - } +void printHelp(const QString &appExecName, const ExtensionSystem::PluginManager &pm) +{ + QString help; + QTextStream str(&help); - inline QString msgSendArgumentFailed() - { - return QCoreApplication::translate("Application", - "Unable to send command line arguments to the already running instance. It appears to be not responding."); - } + str << "Usage: " << appExecName << fixedOptionsC; + ExtensionSystem::PluginManager::formatOptions(str, OptionIndent, DescriptionIndent); + pm.formatPluginOptions(str, OptionIndent, DescriptionIndent); + displayHelpText(help); +} - // Prepare a remote argument: If it is a relative file, add the current directory - // since the the central instance might be running in a different directory. - inline QString prepareRemoteArgument(const QString &arg) - { - QFileInfo fi(arg); - if (!fi.exists()) { - return arg; - } - if (fi.isRelative()) { - return fi.absoluteFilePath(); - } +inline QString msgCoreLoadFailure(const QString &reason) +{ + return QCoreApplication::translate("Application", "Failed to load core plug-in, reason is: %1").arg(reason); +} + +inline QString msgSendArgumentFailed() +{ + return QCoreApplication::translate("Application", + "Unable to send command line arguments to the already running instance. It appears to be not responding."); +} + +// Prepare a remote argument: If it is a relative file, add the current directory +// since the the central instance might be running in a different directory. +inline QString prepareRemoteArgument(const QString &arg) +{ + QFileInfo fi(arg); + + if (!fi.exists()) { return arg; } + if (fi.isRelative()) { + return fi.absoluteFilePath(); + } + return arg; +} - // Send the arguments to an already running instance of application - bool sendArguments(SharedTools::QtSingleApplication &app, const QStringList &arguments) - { - if (!arguments.empty()) { - // Send off arguments - const QStringList::const_iterator acend = arguments.constEnd(); - for (QStringList::const_iterator it = arguments.constBegin(); it != acend; ++it) { - if (!app.sendMessage(prepareRemoteArgument(*it))) { - displayError(msgSendArgumentFailed()); - return false; - } +// Send the arguments to an already running instance of application +bool sendArguments(SharedTools::QtSingleApplication &app, const QStringList &arguments) +{ + if (!arguments.empty()) { + // Send off arguments + const QStringList::const_iterator acend = arguments.constEnd(); + for (QStringList::const_iterator it = arguments.constBegin(); it != acend; ++it) { + if (!app.sendMessage(prepareRemoteArgument(*it))) { + displayError(msgSendArgumentFailed()); + return false; } } - // Special empty argument means: Show and raise (the slot just needs to be triggered) - if (!app.sendMessage(QString())) { - displayError(msgSendArgumentFailed()); - return false; - } - return true; } + // Special empty argument means: Show and raise (the slot just needs to be triggered) + if (!app.sendMessage(QString())) { + displayError(msgSendArgumentFailed()); + return false; + } + return true; +} - void systemInit() - { +void systemInit() +{ #ifdef Q_OS_MAC - // increase the number of file that can be opened in application - struct rlimit rl; - getrlimit(RLIMIT_NOFILE, &rl); - rl.rlim_cur = rl.rlim_max; - setrlimit(RLIMIT_NOFILE, &rl); + // increase the number of file that can be opened in application + struct rlimit rl; + getrlimit(RLIMIT_NOFILE, &rl); + rl.rlim_cur = rl.rlim_max; + setrlimit(RLIMIT_NOFILE, &rl); #endif #ifdef Q_OS_LINUX - QApplication::setAttribute(Qt::AA_X11InitThreads, true); + QApplication::setAttribute(Qt::AA_X11InitThreads, true); #endif +} + +inline QStringList getPluginPaths() +{ + QStringList rc; + // Figure out root: Up one from 'bin' + QDir rootDir = QApplication::applicationDirPath(); + + rootDir.cdUp(); + const QString rootDirPath = rootDir.canonicalPath(); + // 1) "plugins" (Win/Linux) + QString pluginPath = rootDirPath; + pluginPath += QLatin1Char('/'); + pluginPath += QLatin1String(GCS_LIBRARY_BASENAME); + pluginPath += QLatin1Char('/'); + pluginPath += QLatin1String("openpilotgcs"); + pluginPath += QLatin1Char('/'); + pluginPath += QLatin1String("plugins"); + rc.push_back(pluginPath); + // 2) "PlugIns" (OS X) + pluginPath = rootDirPath; + pluginPath += QLatin1Char('/'); + pluginPath += QLatin1String("Plugins"); + rc.push_back(pluginPath); + return rc; +} + +AppOptions options() +{ + AppOptions appOptions; + + appOptions.insert(HELP1_OPTION, false); + appOptions.insert(HELP2_OPTION, false); + appOptions.insert(HELP3_OPTION, false); + appOptions.insert(HELP4_OPTION, false); + appOptions.insert(VERSION_OPTION, false); + appOptions.insert(NO_SPLASH_OPTION, false); + appOptions.insert(CLIENT_OPTION, false); + appOptions.insert(CONFIG_OPTION, true); + appOptions.insert(RESET_OPTION, false); + appOptions.insert(CONFIG_FILE_OPTION, true); + appOptions.insert(EXIT_AFTER_CONFIG_OPTION, false); + return appOptions; +} + +AppOptionValues parseCommandLine(SharedTools::QtSingleApplication &app, + ExtensionSystem::PluginManager &pluginManager, QString &errorMessage) +{ + AppOptionValues appOptionValues; + const QStringList arguments = app.arguments(); + + if (arguments.size() > 1) { + AppOptions appOptions = options(); + pluginManager.parseOptions(arguments, appOptions, &appOptionValues, &errorMessage); + } + return appOptionValues; +} + +void loadFactoryDefaults(QSettings &settings, AppOptionValues &appOptionValues) +{ + QDir directory(QCoreApplication::applicationDirPath() + SHARE_PATH + QString("/default_configurations")); + + qDebug() << "Looking for factory defaults configuration files in:" << directory.absolutePath(); + + QString fileName; + + // check if command line option -config-file contains a file name + QString commandLine = appOptionValues.value(CONFIG_FILE_OPTION); + if (!commandLine.isEmpty()) { + QFileInfo fi(commandLine); + if (fi.isRelative()) { + // file name specified on command line has a relative path + commandLine = directory.absolutePath() + QDir::separator() + commandLine; + } + if (QFile::exists(commandLine)) { + fileName = commandLine; + qDebug() << "Configuration file" << fileName << "specified on command line will be loaded."; + } else { + qWarning() << "Configuration file" << commandLine << "specified on command line does not exist."; + } } - inline QStringList getPluginPaths() - { - QStringList rc; - // Figure out root: Up one from 'bin' - QDir rootDir = QApplication::applicationDirPath(); - rootDir.cdUp(); - const QString rootDirPath = rootDir.canonicalPath(); - // 1) "plugins" (Win/Linux) - QString pluginPath = rootDirPath; - pluginPath += QLatin1Char('/'); - pluginPath += QLatin1String(GCS_LIBRARY_BASENAME); - pluginPath += QLatin1Char('/'); - pluginPath += QLatin1String("openpilotgcs"); - pluginPath += QLatin1Char('/'); - pluginPath += QLatin1String("plugins"); - rc.push_back(pluginPath); - // 2) "PlugIns" (OS X) - pluginPath = rootDirPath; - pluginPath += QLatin1Char('/'); - pluginPath += QLatin1String("Plugins"); - rc.push_back(pluginPath); - return rc; + if (fileName.isEmpty()) { + // check default file + if (QFile::exists(directory.absolutePath() + QDir::separator() + DEFAULT_CONFIG_FILENAME)) { + // use default file name + fileName = directory.absolutePath() + QDir::separator() + DEFAULT_CONFIG_FILENAME; + qDebug() << "Default configuration file" << fileName << "will be loaded."; + } else { + qWarning() << "No default configuration file found in" << directory.absolutePath(); + } } - AppOptions options() - { - AppOptions appOptions; - appOptions.insert(HELP1_OPTION, false); - appOptions.insert(HELP2_OPTION, false); - appOptions.insert(HELP3_OPTION, false); - appOptions.insert(HELP4_OPTION, false); - appOptions.insert(VERSION_OPTION, false); - appOptions.insert(NO_SPLASH_OPTION, false); - appOptions.insert(CLIENT_OPTION, false); - appOptions.insert(CONFIG_OPTION, true); - appOptions.insert(RESET_OPTION, false); - appOptions.insert(CONFIG_FILE_OPTION, true); - appOptions.insert(EXIT_AFTER_CONFIG_OPTION, false); - return appOptions; + if (fileName.isEmpty()) { + // TODO should we exit violently? + qCritical() << "No default configuration file found!"; + return; } - AppOptionValues parseCommandLine(SharedTools::QtSingleApplication &app, - ExtensionSystem::PluginManager &pluginManager, QString &errorMessage) - { - AppOptionValues appOptionValues; - const QStringList arguments = app.arguments(); - if (arguments.size() > 1) { - AppOptions appOptions = options(); - pluginManager.parseOptions(arguments, appOptions, &appOptionValues, &errorMessage); - } - return appOptionValues; + // create settings from file + QSettings qs(fileName, XmlConfig::XmlSettingsFormat); + + // transfer loaded settings to application settings + QStringList keys = qs.allKeys(); + foreach(QString key, keys) { + settings.setValue(key, qs.value(key)); } - void loadFactoryDefaults(QSettings &settings, AppOptionValues &appOptionValues) - { - QDir directory(QCoreApplication::applicationDirPath() + SHARE_PATH + QString("/default_configurations")); - qDebug() << "Looking for factory defaults configuration files in:" << directory.absolutePath(); + qDebug() << "Configuration file" << fileName << "was loaded."; +} - QString fileName; +void overrideSettings(QSettings &settings, int argc, char * *argv) +{ + // Options like -D My/setting=test + QRegExp rx("([^=]+)=(.*)"); - // check if command line option -config-file contains a file name - QString commandLine = appOptionValues.value(CONFIG_FILE_OPTION); - if (!commandLine.isEmpty()) { - QFileInfo fi(commandLine); - if (fi.isRelative()) { - // file name specified on command line has a relative path - commandLine = directory.absolutePath() + QDir::separator() + commandLine; - } - if (QFile::exists(commandLine)) { - fileName = commandLine; - qDebug() << "Configuration file" << fileName << "specified on command line will be loaded."; - } else { - qWarning() << "Configuration file" << commandLine << "specified on command line does not exist."; - } - } - - if (fileName.isEmpty()) { - // check default file - if (QFile::exists(directory.absolutePath() + QDir::separator() + DEFAULT_CONFIG_FILENAME)) { - // use default file name - fileName = directory.absolutePath() + QDir::separator() + DEFAULT_CONFIG_FILENAME; - qDebug() << "Default configuration file" << fileName << "will be loaded."; - } else { - qWarning() << "No default configuration file found in" << directory.absolutePath(); - } - } - - if (fileName.isEmpty()) { - // TODO should we exit violently? - qCritical() << "No default configuration file found!"; - return; - } - - // create settings from file - QSettings qs(fileName, XmlConfig::XmlSettingsFormat); - - // transfer loaded settings to application settings - QStringList keys = qs.allKeys(); - foreach(QString key, keys) { - settings.setValue(key, qs.value(key)); - } - - qDebug() << "Configuration file" << fileName << "was loaded."; - } - - void overrideSettings(QSettings &settings, int argc, char **argv) - { - // Options like -D My/setting=test - QRegExp rx("([^=]+)=(.*)"); - - for (int i = 0; i < argc; ++i) { - if (CONFIG_OPTION == QString(argv[i])) { - if (rx.indexIn(argv[++i]) > -1) { - QString key = rx.cap(1); - QString value = rx.cap(2); - qDebug() << "User setting" << key << "set to value" << value; - settings.setValue(key, value); - } - } - } - - settings.sync(); - } - - void loadTranslators(QString language, QTranslator &translator, QTranslator &qtTranslator) - { - const QString &creatorTrPath = QCoreApplication::applicationDirPath() + SHARE_PATH - + QLatin1String("/translations"); - if (translator.load(QLatin1String("openpilotgcs_") + language, creatorTrPath)) { - const QString &qtTrPath = QLibraryInfo::location(QLibraryInfo::TranslationsPath); - const QString &qtTrFile = QLatin1String("qt_") + language; - // Binary installer puts Qt tr files into creatorTrPath - if (qtTranslator.load(qtTrFile, qtTrPath) || qtTranslator.load(qtTrFile, creatorTrPath)) { - QCoreApplication::installTranslator(&translator); - QCoreApplication::installTranslator(&qtTranslator); - } else { - // unload() - translator.load(QString()); + for (int i = 0; i < argc; ++i) { + if (CONFIG_OPTION == QString(argv[i])) { + if (rx.indexIn(argv[++i]) > -1) { + QString key = rx.cap(1); + QString value = rx.cap(2); + qDebug() << "User setting" << key << "set to value" << value; + settings.setValue(key, value); } } } -} // namespace anonymous + settings.sync(); +} -int main(int argc, char **argv) +void loadTranslators(QString language, QTranslator &translator, QTranslator &qtTranslator) +{ + const QString &creatorTrPath = QCoreApplication::applicationDirPath() + SHARE_PATH + + QLatin1String("/translations"); + + if (translator.load(QLatin1String("openpilotgcs_") + language, creatorTrPath)) { + const QString &qtTrPath = QLibraryInfo::location(QLibraryInfo::TranslationsPath); + const QString &qtTrFile = QLatin1String("qt_") + language; + // Binary installer puts Qt tr files into creatorTrPath + if (qtTranslator.load(qtTrFile, qtTrPath) || qtTranslator.load(qtTrFile, creatorTrPath)) { + QCoreApplication::installTranslator(&translator); + QCoreApplication::installTranslator(&qtTranslator); + } else { + // unload() + translator.load(QString()); + } + } +} +} // namespace anonymous + +int main(int argc, char * *argv) { QElapsedTimer timer; + timer.start(); // low level init @@ -492,23 +499,23 @@ int main(int argc, char **argv) splash->showProgressMessage(QObject::tr("Application starting...")); splash->show(); // connect to track progress of plugin manager - QObject::connect(&pluginManager, SIGNAL(pluginAboutToBeLoaded(ExtensionSystem::PluginSpec*)), splash, - SLOT(showPluginLoadingProgress(ExtensionSystem::PluginSpec*))); + QObject::connect(&pluginManager, SIGNAL(pluginAboutToBeLoaded(ExtensionSystem::PluginSpec *)), splash, + SLOT(showPluginLoadingProgress(ExtensionSystem::PluginSpec *))); } // find and load core plugin const PluginSpecSet plugins = pluginManager.plugins(); ExtensionSystem::PluginSpec *coreplugin = 0; - foreach (ExtensionSystem::PluginSpec *spec, plugins) { + foreach(ExtensionSystem::PluginSpec * spec, plugins) { if (spec->name() == CORE_PLUGIN_NAME) { coreplugin = spec; break; } } if (!coreplugin) { - QString nativePaths = QDir::toNativeSeparators(getPluginPaths().join(QLatin1String(","))); + QString nativePaths = QDir::toNativeSeparators(getPluginPaths().join(QLatin1String(","))); const QString reason = QCoreApplication::translate("Application", "Could not find 'Core.pluginspec' in %1").arg( - nativePaths); + nativePaths); displayError(msgCoreLoadFailure(reason)); return 1; } @@ -522,7 +529,7 @@ int main(int argc, char **argv) return 0; } if (appOptionValues.contains(HELP1_OPTION) || appOptionValues.contains(HELP2_OPTION) - || appOptionValues.contains(HELP3_OPTION) || appOptionValues.contains(HELP4_OPTION)) { + || appOptionValues.contains(HELP3_OPTION) || appOptionValues.contains(HELP4_OPTION)) { printHelp(QFileInfo(app.applicationFilePath()).baseName(), pluginManager); return 0; } @@ -541,15 +548,15 @@ int main(int argc, char **argv) { QStringList errors; - foreach (ExtensionSystem::PluginSpec *p, pluginManager.plugins()) { + foreach(ExtensionSystem::PluginSpec * p, pluginManager.plugins()) { if (p->hasError()) { errors.append(p->errorString()); } } if (!errors.isEmpty()) { QMessageBox::warning(0, - QCoreApplication::translate("Application", "OpenPilot GCS - Plugin loader messages"), - errors.join(QString::fromLatin1("\n\n"))); + QCoreApplication::translate("Application", "OpenPilot GCS - Plugin loader messages"), + errors.join(QString::fromLatin1("\n\n"))); } } diff --git a/ground/openpilotgcs/src/experimental/HIDTest/main.cpp b/ground/openpilotgcs/src/experimental/HIDTest/main.cpp index e4c1b0cb9..6734ae38b 100644 --- a/ground/openpilotgcs/src/experimental/HIDTest/main.cpp +++ b/ground/openpilotgcs/src/experimental/HIDTest/main.cpp @@ -5,40 +5,40 @@ #define BUF_LEN 64 class MyThread : public QThread { +public: - public: - - void run() - { - qDebug() << "Hello"; - pjrc_rawhid hidHandle; - int numDevices = hidHandle.open(1,0x20a0,0x4117,0,0); //0xff9c,0x0001); - if( numDevices == 0 ) - numDevices = hidHandle.open(1,0x0483,0,0,0); - - qDebug() << numDevices << " device(s) opened"; - - //hidHandle.mytest(0); - - char buf[BUF_LEN]; - buf[0] = 2; - buf[1] = 0; - int result = hidHandle.send(0,buf, BUF_LEN, 500); - - qDebug() << result << " bytes sent"; - - int received = hidHandle.receive(0, buf, BUF_LEN, 3500); - - qDebug("%u bytes received. First value %x second %x", received,buf[0], buf[1]); + void run() + { + qDebug() << "Hello"; + pjrc_rawhid hidHandle; + int numDevices = hidHandle.open(1, 0x20a0, 0x4117, 0, 0); // 0xff9c,0x0001); + if (numDevices == 0) { + numDevices = hidHandle.open(1, 0x0483, 0, 0, 0); } - }; + qDebug() << numDevices << " device(s) opened"; + + // hidHandle.mytest(0); + + char buf[BUF_LEN]; + buf[0] = 2; + buf[1] = 0; + int result = hidHandle.send(0, buf, BUF_LEN, 500); + + qDebug() << result << " bytes sent"; + + int received = hidHandle.receive(0, buf, BUF_LEN, 3500); + + qDebug("%u bytes received. First value %x second %x", received, buf[0], buf[1]); + } +}; int main(int argc, char *argv[]) { QCoreApplication a(argc, argv); MyThread b; + b.start(); return a.exec(); diff --git a/ground/openpilotgcs/src/experimental/PowerLog6S/main.cpp b/ground/openpilotgcs/src/experimental/PowerLog6S/main.cpp index a95611270..5dbf9c4d8 100644 --- a/ground/openpilotgcs/src/experimental/PowerLog6S/main.cpp +++ b/ground/openpilotgcs/src/experimental/PowerLog6S/main.cpp @@ -31,197 +31,189 @@ using namespace std; -typedef unsigned long ULONG; // 4 Bytes +typedef unsigned long ULONG; // 4 Bytes typedef short SHORT; -typedef unsigned short USHORT; // 2 Bytes -typedef unsigned char BYTE; // 1 Byte -typedef unsigned short WORD; // 2 Bytes -typedef unsigned long DWORD; // 4 Bytes +typedef unsigned short USHORT; // 2 Bytes +typedef unsigned char BYTE; // 1 Byte +typedef unsigned short WORD; // 2 Bytes +typedef unsigned long DWORD; // 4 Bytes class MyParser : public QObject { + struct POWERLOG_HID_PACK { + BYTE Len; + BYTE Type; + DWORD Interval; + BYTE LogState; + SHORT Current; + USHORT Volt; + DWORD Cap; + SHORT Cell[6]; + USHORT RPM; + SHORT Temp[4]; + USHORT Period; + USHORT Pulse; + }; -struct POWERLOG_HID_PACK -{ - BYTE Len; - BYTE Type; - DWORD Interval; - BYTE LogState; - SHORT Current; - USHORT Volt; - DWORD Cap; - SHORT Cell[6]; - USHORT RPM; - SHORT Temp[4]; - USHORT Period; - USHORT Pulse; -}; - -enum -{ - TYPE_DATA_ONLINE = 0x10, - TYPE_DATA_OFFLINE = 0x11, - TYPE_ORDER = 0x20, -}; + enum { + TYPE_DATA_ONLINE = 0x10, + TYPE_DATA_OFFLINE = 0x11, + TYPE_ORDER = 0x20, + }; - public: +public: - void start() - { - qDebug() << "Connect a Junsi PowerLog 6S and watch the logging output"; - pjrc_rawhid hidHandle; - int numDevices = hidHandle.open(1, 0x0483,0x5750,0,0); //0xff9c,0x0001); - if( numDevices == 0 ) - numDevices = hidHandle.open(1,0x0483,0,0,0); - - qDebug() << numDevices << " device(s) opened"; - - //hidHandle.mytest(0); - - char buf[BUF_LEN]; - buf[0] = 2; - buf[1] = 0; - - cout << "Interval,Current,Volt,Cap,Cell1,Cell2,Cell3,Cell4,Cell5,Cell6,RPM,Temp1,Temp2,Temp3,Temp4,Period,Pulse" << endl; - - while (int received = hidHandle.receive(0, buf, BUF_LEN, 3500) ) { - ShowInf(buf); - } + void start() + { + qDebug() << "Connect a Junsi PowerLog 6S and watch the logging output"; + pjrc_rawhid hidHandle; + int numDevices = hidHandle.open(1, 0x0483, 0x5750, 0, 0); // 0xff9c,0x0001); + if (numDevices == 0) { + numDevices = hidHandle.open(1, 0x0483, 0, 0, 0); } + qDebug() << numDevices << " device(s) opened"; - void ShowInf(char *pBuf) - { - // qDebug() << "--------------------"; - POWERLOG_HID_PACK Inf; - int i; - int Count; - - Count=0; - Inf.Len = pBuf[Count]; - Count += sizeof(Inf.Len); - - Inf.Type = pBuf[Count]; - Count += sizeof(Inf.Type); + // hidHandle.mytest(0); - Inf.Interval = *((DWORD *)&pBuf[Count]); - printf("%ld,",Inf.Interval); + char buf[BUF_LEN]; + buf[0] = 2; + buf[1] = 0; - Count += sizeof(Inf.Interval); + cout << "Interval,Current,Volt,Cap,Cell1,Cell2,Cell3,Cell4,Cell5,Cell6,RPM,Temp1,Temp2,Temp3,Temp4,Period,Pulse" << endl; - Inf.LogState = pBuf[Count]; - Count += sizeof(Inf.LogState); - - if(((Inf.Type == TYPE_DATA_ONLINE)||(Inf.Type == TYPE_DATA_OFFLINE)) && (Inf.Len == 0x29))//0x27 - { - Inf.Current = *((SHORT *)&pBuf[Count]); - Count += sizeof(Inf.Current); - GetShowValue(QString("Current:"),Inf.Current,5,2); - - Inf.Volt = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.Volt); - GetShowValue(QString("Voltage:"),Inf.Volt,5,2); - - Inf.Cap = *((DWORD *)&pBuf[Count]); - Count += sizeof(Inf.Cap); - GetShowValue(QString("Cap:"),Inf.Cap,6,0); - - for(i=0;i<6;i++) - { - Inf.Cell[i] = *((SHORT *)&pBuf[Count]); - Count += sizeof(Inf.Cell[i]); - } - GetShowValue(QString("Cell 1:"),Inf.Cell[0],5,3); - GetShowValue(QString("Cell 2:"),Inf.Cell[1],5,3); - GetShowValue(QString("Cell 3:"),Inf.Cell[2],5,3); - GetShowValue(QString("Cell 4:"),Inf.Cell[3],5,3); - GetShowValue(QString("Cell 5:"),Inf.Cell[4],5,3); - GetShowValue(QString("Cell 6:"),Inf.Cell[5],5,3); - - Inf.RPM = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.RPM); - GetShowValue(QString("RPM:"),Inf.RPM,6,0); + while (int received = hidHandle.receive(0, buf, BUF_LEN, 3500)) { + ShowInf(buf); + } + } - for(i=0;i<4;i++) - { - Inf.Temp[i] = *((SHORT *)&pBuf[Count]); - Count += sizeof(Inf.Temp[i]); - - } - GetShowValue(QString("Int Temp1:"),Inf.Temp[0],4,1); - if(Inf.Temp[1]==0x7fff) - QString txtExtTemp1 = QString("NULL"); - else - GetShowValue(QString("Ext temp1:"),Inf.Temp[1],4,1); - if(Inf.Temp[2]==0x7fff) - QString txtExtTemp2 = QString("NULL"); - else - GetShowValue(QString("Ext temp2:"),Inf.Temp[2],4,1); - if(Inf.Temp[3]==0x7fff) - QString txtExtTemp3 = QString("NULL"); - else - GetShowValue(QString("Ext temp3:"),Inf.Temp[3],4,1); + void ShowInf(char *pBuf) + { + // qDebug() << "--------------------"; + POWERLOG_HID_PACK Inf; + int i; + int Count; - Inf.Period = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.Period); - GetShowValue(QString("Period:"),Inf.Period,6,0); + Count = 0; + Inf.Len = pBuf[Count]; + Count += sizeof(Inf.Len); - Inf.Pulse = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.Pulse); - GetShowValue(QString("Pulse:"),Inf.Pulse,6,0); + Inf.Type = pBuf[Count]; + Count += sizeof(Inf.Type); - printf("\n"); - } + Inf.Interval = *((DWORD *)&pBuf[Count]); + printf("%ld,", Inf.Interval); -} + Count += sizeof(Inf.Interval); -void GetShowValue(QString label,DWORD Value,WORD Len,WORD Dot) -{ - //cout << label .toStdString() << " "; + Inf.LogState = pBuf[Count]; + Count += sizeof(Inf.LogState); - if(Value<0) - { - Value = -Value; - if(Dot==1) - printf("-%ld.%01lu",Value/10,Value%10); - else if(Dot==2) - printf("-%ld.%02lu",Value/100,Value%100); - else if(Dot==3) - printf("-%ld.%03lu",Value/1000,Value%1000); - else if(Dot==4) - printf("-%ld.%04lu",Value/10000,Value%10000); - else - printf("-%ld",Value); - } - else - { - if(Dot==1) - printf("%ld.%01lu",Value/10,Value%10); - else if(Dot==2) - printf("%ld.%02lu",Value/100,Value%100); - else if(Dot==3) - printf("%ld.%03lu",Value/1000,Value%1000); - else if(Dot==4) - printf("%ld.%04lu",Value/10000,Value%10000); - else - printf("%ld",Value); - } - printf(","); + if (((Inf.Type == TYPE_DATA_ONLINE) || (Inf.Type == TYPE_DATA_OFFLINE)) && (Inf.Len == 0x29)) { // 0x27 + Inf.Current = *((SHORT *)&pBuf[Count]); + Count += sizeof(Inf.Current); + GetShowValue(QString("Current:"), Inf.Current, 5, 2); -} + Inf.Volt = *((USHORT *)&pBuf[Count]); + Count += sizeof(Inf.Volt); + GetShowValue(QString("Voltage:"), Inf.Volt, 5, 2); + + Inf.Cap = *((DWORD *)&pBuf[Count]); + Count += sizeof(Inf.Cap); + GetShowValue(QString("Cap:"), Inf.Cap, 6, 0); + + for (i = 0; i < 6; i++) { + Inf.Cell[i] = *((SHORT *)&pBuf[Count]); + Count += sizeof(Inf.Cell[i]); + } + GetShowValue(QString("Cell 1:"), Inf.Cell[0], 5, 3); + GetShowValue(QString("Cell 2:"), Inf.Cell[1], 5, 3); + GetShowValue(QString("Cell 3:"), Inf.Cell[2], 5, 3); + GetShowValue(QString("Cell 4:"), Inf.Cell[3], 5, 3); + GetShowValue(QString("Cell 5:"), Inf.Cell[4], 5, 3); + GetShowValue(QString("Cell 6:"), Inf.Cell[5], 5, 3); + + Inf.RPM = *((USHORT *)&pBuf[Count]); + Count += sizeof(Inf.RPM); + GetShowValue(QString("RPM:"), Inf.RPM, 6, 0); + for (i = 0; i < 4; i++) { + Inf.Temp[i] = *((SHORT *)&pBuf[Count]); + Count += sizeof(Inf.Temp[i]); + } + GetShowValue(QString("Int Temp1:"), Inf.Temp[0], 4, 1); + if (Inf.Temp[1] == 0x7fff) { + QString txtExtTemp1 = QString("NULL"); + } else { + GetShowValue(QString("Ext temp1:"), Inf.Temp[1], 4, 1); + } + if (Inf.Temp[2] == 0x7fff) { + QString txtExtTemp2 = QString("NULL"); + } else { + GetShowValue(QString("Ext temp2:"), Inf.Temp[2], 4, 1); + } + if (Inf.Temp[3] == 0x7fff) { + QString txtExtTemp3 = QString("NULL"); + } else { + GetShowValue(QString("Ext temp3:"), Inf.Temp[3], 4, 1); + } - }; + Inf.Period = *((USHORT *)&pBuf[Count]); + Count += sizeof(Inf.Period); + GetShowValue(QString("Period:"), Inf.Period, 6, 0); + + Inf.Pulse = *((USHORT *)&pBuf[Count]); + Count += sizeof(Inf.Pulse); + GetShowValue(QString("Pulse:"), Inf.Pulse, 6, 0); + + printf("\n"); + } + } + + void GetShowValue(QString label, DWORD Value, WORD Len, WORD Dot) + { + // cout << label .toStdString() << " "; + + if (Value < 0) { + Value = -Value; + if (Dot == 1) { + printf("-%ld.%01lu", Value / 10, Value % 10); + } else if (Dot == 2) { + printf("-%ld.%02lu", Value / 100, Value % 100); + } else if (Dot == 3) { + printf("-%ld.%03lu", Value / 1000, Value % 1000); + } else if (Dot == 4) { + printf("-%ld.%04lu", Value / 10000, Value % 10000); + } else { + printf("-%ld", Value); + } + } else { + if (Dot == 1) { + printf("%ld.%01lu", Value / 10, Value % 10); + } else if (Dot == 2) { + printf("%ld.%02lu", Value / 100, Value % 100); + } else if (Dot == 3) { + printf("%ld.%03lu", Value / 1000, Value % 1000); + } else if (Dot == 4) { + printf("%ld.%04lu", Value / 10000, Value % 10000); + } else { + printf("%ld", Value); + } + } + printf(","); + } +}; int main(int argc, char *argv[]) { QCoreApplication a(argc, argv); MyParser b; + b.start(); -// return a.exec(); +// return a.exec(); } diff --git a/ground/openpilotgcs/src/experimental/SerialLogger/main.cpp b/ground/openpilotgcs/src/experimental/SerialLogger/main.cpp index 03c91dfed..1d4e651c9 100644 --- a/ground/openpilotgcs/src/experimental/SerialLogger/main.cpp +++ b/ground/openpilotgcs/src/experimental/SerialLogger/main.cpp @@ -8,53 +8,49 @@ #include -class pollSerialPort : public QThread -{ +class pollSerialPort : public QThread { public: pollSerialPort(QString dn, QString fn) : device(dn), outFile(fn) - { - }; + {}; void run() { QByteArray dat; - //const char framingRaw[16] = {7,9,3,15,193,130,150,10,7,9,3,15,193,130,150,10}; - const char framingRaw[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15}; - QByteArray framing(framingRaw,16); + // const char framingRaw[16] = {7,9,3,15,193,130,150,10,7,9,3,15,193,130,150,10}; + const char framingRaw[16] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 }; + QByteArray framing(framingRaw, 16); PortSettings Settings; - Settings.BaudRate=BAUD115200; - Settings.DataBits=DATA_8; - Settings.Parity=PAR_NONE; - Settings.StopBits=STOP_1; - Settings.FlowControl=FLOW_OFF; - Settings.Timeout_Millisec=500; + + Settings.BaudRate = BAUD115200; + Settings.DataBits = DATA_8; + Settings.Parity = PAR_NONE; + Settings.StopBits = STOP_1; + Settings.FlowControl = FLOW_OFF; + Settings.Timeout_Millisec = 500; QextSerialPort serialPort(device, Settings); serialPort.open(QIODevice::ReadOnly); QFile file(outFile); - if( !file.open( QIODevice::WriteOnly ) ) - { + if (!file.open(QIODevice::WriteOnly)) { qDebug() << "Failed to open file: " << outFile; - return; + return; } - QTextStream ts( &file ); + QTextStream ts(&file); - while(1) - { + while (1) { dat = serialPort.read(500); - if(dat.contains(framing)) - { + if (dat.contains(framing)) { int start = dat.indexOf(framing); - int count = *((int *) (dat.data() + start+16)); + int count = *((int *)(dat.data() + start + 16)); qDebug() << "Found frame start at " << start << " count " << count; - } - else if (dat.size() == 0) + } else if (dat.size() == 0) { qDebug() << "No data"; - else + } else { qDebug() << "No frame start"; + } ts << dat; usleep(100000); } @@ -71,15 +67,17 @@ int main(int argc, char *argv[]) QString device; QString log; - if(argc < 2) + if (argc < 2) { device = "/dev/tty.usbserial-000014FAB"; - else + } else { device = QString(argv[1]); + } - if(argc < 3) + if (argc < 3) { log = "log.dat"; - else + } else { log = QString(argv[2]); + } pollSerialPort thread(device, log); thread.start(); diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/main.cpp b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/main.cpp index 61ca74b04..41891dbf9 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/main.cpp +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/main.cpp @@ -7,69 +7,60 @@ #include "qsspt.h" int main(int argc, char *argv[]) { -#define MAX_PACKET_DATA_LEN 255 -#define MAX_PACKET_BUF_SIZE (1+1+255+2) +#define MAX_PACKET_DATA_LEN 255 +#define MAX_PACKET_BUF_SIZE (1 + 1 + 255 + 2) - uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; - uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; + uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; + uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; - port * info; + port *info; PortSettings settings; - settings.BaudRate=BAUD57600; - settings.DataBits=DATA_8; - settings.FlowControl=FLOW_OFF; - settings.Parity=PAR_NONE; - settings.StopBits=STOP_1; - settings.Timeout_Millisec=5000; + settings.BaudRate = BAUD57600; + settings.DataBits = DATA_8; + settings.FlowControl = FLOW_OFF; + settings.Parity = PAR_NONE; + settings.StopBits = STOP_1; + settings.Timeout_Millisec = 5000; - info=new port(settings,"COM3"); - info->rxBuf = sspRxBuf; - info->rxBufSize = MAX_PACKET_DATA_LEN; - info->txBuf = sspTxBuf; - info->txBufSize = 255; - info->max_retry = 3; - info->timeoutLen = 5000; - //qssp b(info); + info = new port(settings, "COM3"); + info->rxBuf = sspRxBuf; + info->rxBufSize = MAX_PACKET_DATA_LEN; + info->txBuf = sspTxBuf; + info->txBufSize = 255; + info->max_retry = 3; + info->timeoutLen = 5000; + // qssp b(info); qsspt bb(info); uint8_t buf[1000]; QCoreApplication a(argc, argv); - while(!bb.ssp_Synchronise()) - { - qDebug()<<"trying sync"; + while (!bb.ssp_Synchronise()) { + qDebug() << "trying sync"; } - bb.start(); - qDebug()<<"sync complete"; - buf[0]=0; - buf[1]=1; - buf[2]=2; - while(true) - { - if(bb.sendData(buf,63)) - qDebug()<<"send OK"; -// else -// qDebug()<<"send NOK"; -// //bb.ssp_SendData(buf,63); - } - while(true) - { - - - - if(bb.packets_Available()>0) - { - + bb.start(); + qDebug() << "sync complete"; + buf[0] = 0; + buf[1] = 1; + buf[2] = 2; + while (true) { + if (bb.sendData(buf, 63)) { + qDebug() << "send OK"; + } +// else +// qDebug()<<"send NOK"; +////bb.ssp_SendData(buf,63); + } + while (true) { + if (bb.packets_Available() > 0) { bb.read_Packet(buf); - qDebug()<<"receive="<<(int)buf[0]<<(int)buf[1]<<(int)buf[2]; + qDebug() << "receive=" << (int)buf[0] << (int)buf[1] << (int)buf[2]; ++buf[0]; ++buf[1]; ++buf[2]; - //bb.ssp_SendData(buf,63); - bb.sendData(buf,63); + // bb.ssp_SendData(buf,63); + bb.sendData(buf, 63); } - //bb.ssp_ReceiveProcess(); - //bb.ssp_SendProcess(); - - + // bb.ssp_ReceiveProcess(); + // bb.ssp_SendProcess(); } return a.exec(); } diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.cpp b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.cpp index b247a642c..3cc4ed441 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.cpp +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.cpp @@ -1,16 +1,15 @@ #include "port.h" #include "delay.h" -port::port(PortSettings settings,QString name):mstatus(port::closed) +port::port(PortSettings settings, QString name) : mstatus(port::closed) { timer.start(); - sport = new QextSerialPort(name,settings, QextSerialPort::Polling); - if(sport->open(QIODevice::ReadWrite|QIODevice::Unbuffered)) - { - mstatus=port::open; - // sport->setDtr(); + sport = new QextSerialPort(name, settings, QextSerialPort::Polling); + if (sport->open(QIODevice::ReadWrite | QIODevice::Unbuffered)) { + mstatus = port::open; + // sport->setDtr(); + } else { + mstatus = port::error; } - else - mstatus=port::error; } port::portstatus port::status() { @@ -18,21 +17,20 @@ port::portstatus port::status() } int16_t port::pfSerialRead(void) { - char c[1]; - if(sport->bytesAvailable()) - { - sport->read(c,1); - } - else return -1; + + if (sport->bytesAvailable()) { + sport->read(c, 1); + } else { return -1; } return (uint8_t)c[0]; } void port::pfSerialWrite(uint8_t c) { char cc[1]; - cc[0]=c; - sport->write(cc,1); + + cc[0] = c; + sport->write(cc, 1); } uint32_t port::pfGetTime(void) diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.h b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.h index 6ffea1c11..4d3453958 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.h +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.h @@ -6,38 +6,37 @@ #include #include "common.h" -class port -{ +class port { public: - enum portstatus{open,closed,error}; - virtual int16_t pfSerialRead(void); // function to read a character from the serial input stream - virtual void pfSerialWrite( uint8_t ); // function to write a byte to be sent out the serial port + enum portstatus { open, closed, error }; + virtual int16_t pfSerialRead(void); // function to read a character from the serial input stream + virtual void pfSerialWrite(uint8_t); // function to write a byte to be sent out the serial port virtual uint32_t pfGetTime(void); - uint8_t retryCount; // how many times have we tried to transmit the 'send' packet - uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet - uint16_t max_retry; // Maximum number of retrys for a single transmit. - int32_t timeoutLen; // how long to wait for each retry to succeed - int32_t timeout; // current timeout. when 'time' reaches this point we have timed out - uint8_t txSeqNo; // current 'send' packet sequence number - uint16_t rxBufPos; // current buffer position in the receive packet - uint16_t rxBufLen; // number of 'data' bytes in the buffer - uint8_t rxSeqNo; // current 'receive' packet number - uint16_t rxBufSize; // size of the receive buffer. - uint16_t txBufSize; // size of the transmit buffer. - uint8_t *txBuf; // transmit buffer. REquired to store a copy of packet data in case a retry is needed. - uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received. - uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host + uint8_t retryCount; // how many times have we tried to transmit the 'send' packet + uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet + uint16_t max_retry; // Maximum number of retrys for a single transmit. + int32_t timeoutLen; // how long to wait for each retry to succeed + int32_t timeout; // current timeout. when 'time' reaches this point we have timed out + uint8_t txSeqNo; // current 'send' packet sequence number + uint16_t rxBufPos; // current buffer position in the receive packet + uint16_t rxBufLen; // number of 'data' bytes in the buffer + uint8_t rxSeqNo; // current 'receive' packet number + uint16_t rxBufSize; // size of the receive buffer. + uint16_t txBufSize; // size of the transmit buffer. + uint8_t *txBuf; // transmit buffer. REquired to store a copy of packet data in case a retry is needed. + uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received. + uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host // this is required when switching from the application to the bootloader // and vice-versa. This fixes the firwmare download timeout. // when this flag is set to true, the next time we send a packet we will first // send a synchronize packet. - ReceiveState InputState; - decodeState_ DecodeState; - uint16_t SendState; - uint16_t crc; - uint32_t RxError; - uint32_t TxError; - uint16_t flags; - port(PortSettings settings,QString name); + ReceiveState InputState; + decodeState_ DecodeState; + uint16_t SendState; + uint16_t crc; + uint32_t RxError; + uint32_t TxError; + uint16_t flags; + port(PortSettings settings, QString name); portstatus status(); private: portstatus mstatus; diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.cpp b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.cpp index fdb371e35..aadd01330 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.cpp +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.cpp @@ -7,48 +7,47 @@ /** PRIVATE DEFINITIONS **/ -#define SYNC 225 // Sync character used in Serial Protocol -#define ESC 224 // ESC character used in Serial Protocol -#define ESC_SYNC 1 // ESC_SYNC character used in Serial Protocol -#define ACK_BIT 0x80 // Ack bit, bit 7 of sequence number, 1 = Acknowledge, 0 = -// new packet +#define SYNC 225 // Sync character used in Serial Protocol +#define ESC 224 // ESC character used in Serial Protocol +#define ESC_SYNC 1 // ESC_SYNC character used in Serial Protocol +#define ACK_BIT 0x80 // Ack bit, bit 7 of sequence number, 1 = Acknowledge, 0 = +// new packet // packet location definitions. -#define LENGTH 0 -#define SEQNUM 1 -#define DATA 2 +#define LENGTH 0 +#define SEQNUM 1 +#define DATA 2 // Make larger sized integers from smaller sized integers -#define MAKEWORD16( ub, lb ) ((uint16_t)0x0000 | ((uint16_t)(ub) << 8) | (uint16_t)(lb) ) -#define MAKEWORD32( uw, lw ) ((uint32_t)(0x0UL | ((uint32_t)(uw) << 16) | (uint32_t)(lw)) ) -#define MAKEWORD32B( b3, b2, b1, b0 ) ((uint32_t)((uint32_t)(b3)<< 24) | ((uint32_t)(b2)<<16) | ((uint32_t)(b1)<<8) | ((uint32_t)(b0) ) +#define MAKEWORD16(ub, lb) ((uint16_t)0x0000 | ((uint16_t)(ub) << 8) | (uint16_t)(lb)) +#define MAKEWORD32(uw, lw) ((uint32_t)(0x0UL | ((uint32_t)(uw) << 16) | (uint32_t)(lw))) +#define MAKEWORD32B(b3, b2, b1, b0) ((uint32_t)((uint32_t)(b3) << 24) | ((uint32_t)(b2) << 16) | ((uint32_t)(b1) << 8) | ((uint32_t)(b0)) // Used to extract smaller integers from larger sized intergers -#define LOWERBYTE( w ) (uint8_t)((w) & 0x00ff ) -#define UPPERBYTE( w ) (uint8_t)(((w) & 0xff00) >> 8 ) -#define UPPERWORD(lw) (uint16_t)(((lw) & 0xffff0000) >> 16 ) -#define LOWERWORD(lw) (uint16_t)((lw) & 0x0000ffff) +#define LOWERBYTE(w) (uint8_t)((w) & 0x00ff) +#define UPPERBYTE(w) (uint8_t)(((w) & 0xff00) >> 8) +#define UPPERWORD(lw) (uint16_t)(((lw) & 0xffff0000) >> 16) +#define LOWERWORD(lw) (uint16_t)((lw) & 0x0000ffff) // Macros to operate on a target and bitmask. -#define CLEARBIT( a, b ) ((a) = (a) & ~(b)) -#define SETBIT( a, b ) ((a) = (a) | (b) ) -#define TOGGLEBIT(a,b) ((a) = (a) ^ (b) ) +#define CLEARBIT(a, b) ((a) = (a) & ~(b)) +#define SETBIT(a, b) ((a) = (a) | (b)) +#define TOGGLEBIT(a, b) ((a) = (a) ^ (b)) // test bit macros operate using a bit mask. -#define ISBITSET( a, b ) ( ((a) & (b)) == (b) ? TRUE : FALSE ) -#define ISBITCLEAR( a, b) ( (~(a) & (b)) == (b) ? TRUE : FALSE ) - +#define ISBITSET(a, b) (((a) & (b)) == (b) ? TRUE : FALSE) +#define ISBITCLEAR(a, b) ((~(a) & (b)) == (b) ? TRUE : FALSE) /* Flag bit masks...*/ -#define SENT_SYNCH (0x01) -#define ACK_RECEIVED (0x02) -#define ACK_EXPECTED (0x04) +#define SENT_SYNCH (0x01) +#define ACK_RECEIVED (0x02) +#define ACK_EXPECTED (0x04) -#define SSP_AWAITING_ACK 0 -#define SSP_ACKED 1 -#define SSP_IDLE 2 +#define SSP_AWAITING_ACK 0 +#define SSP_ACKED 1 +#define SSP_IDLE 2 /** PRIVATE DATA **/ static const uint16_t CRC_TABLE[] = { @@ -105,27 +104,25 @@ static const uint16_t CRC_TABLE[] = { * Must be called before calling the Send or REceive process functions. */ -void qssp::ssp_Init(const PortConfig_t* const info) +void qssp::ssp_Init(const PortConfig_t *const info) { - - - thisport->maxRetryCount = info->max_retry; - thisport->timeoutLen = info->timeoutLen; + thisport->maxRetryCount = info->max_retry; + thisport->timeoutLen = info->timeoutLen; thisport->txBufSize = info->txBufSize; - thisport->rxBufSize = info->rxBufSize; - thisport->txBuf = info->txBuf; - thisport->rxBuf = info->rxBuf; + thisport->rxBufSize = info->rxBufSize; + thisport->txBuf = info->txBuf; + thisport->rxBuf = info->rxBuf; thisport->retryCount = 0; - thisport->sendSynch = FALSE; //TRUE; - thisport->rxSeqNo = 255; - thisport->txSeqNo = 255; - thisport->SendState = SSP_IDLE; - thisport->InputState =(ReceiveState)0; - thisport->DecodeState =(decodeState_)0; - thisport->TxError =0; - thisport->RxError =0; - thisport->txSeqNo =0; - thisport->rxSeqNo =0; + thisport->sendSynch = FALSE; // TRUE; + thisport->rxSeqNo = 255; + thisport->txSeqNo = 255; + thisport->SendState = SSP_IDLE; + thisport->InputState = (ReceiveState)0; + thisport->DecodeState = (decodeState_)0; + thisport->TxError = 0; + thisport->RxError = 0; + thisport->txSeqNo = 0; + thisport->rxSeqNo = 0; } /*! @@ -139,11 +136,11 @@ void qssp::ssp_Init(const PortConfig_t* const info) * \note * */ -int16_t qssp::ssp_SendProcess( ) +int16_t qssp::ssp_SendProcess() { int16_t value = SSP_TX_WAITING; - if (thisport->SendState == SSP_AWAITING_ACK ) { + if (thisport->SendState == SSP_AWAITING_ACK) { if (sf_CheckTimeout() == TRUE) { if (thisport->retryCount < thisport->maxRetryCount) { // Try again @@ -153,16 +150,17 @@ int16_t qssp::ssp_SendProcess( ) } else { // Give up, # of trys has exceded the limit value = SSP_TX_TIMEOUT; - CLEARBIT( thisport->flags, ACK_RECEIVED); + CLEARBIT(thisport->flags, ACK_RECEIVED); thisport->SendState = SSP_IDLE; - if (debug) - qDebug()<<"Send TimeOut!"; + if (debug) { + qDebug() << "Send TimeOut!"; + } } } else { value = SSP_TX_WAITING; } - } else if( thisport->SendState == SSP_ACKED ) { - SETBIT( thisport->flags, ACK_RECEIVED); + } else if (thisport->SendState == SSP_ACKED) { + SETBIT(thisport->flags, ACK_RECEIVED); value = SSP_TX_ACKED; thisport->SendState = SSP_IDLE; } else { @@ -205,13 +203,13 @@ int16_t qssp::ssp_ReceiveProcess() * */ -int16_t qssp::ssp_ReceiveByte() +int16_t qssp::ssp_ReceiveByte() { int16_t b; int16_t packet_status = SSP_RX_IDLE; b = thisport->pfSerialRead(); - if( b != -1 ) { + if (b != -1) { packet_status = sf_ReceiveState(b); } return packet_status; @@ -228,17 +226,17 @@ int16_t qssp::ssp_ReceiveByte() * \note * */ -uint16_t qssp::ssp_SendDataBlock(uint8_t *data, uint16_t length ) +uint16_t qssp::ssp_SendDataBlock(uint8_t *data, uint16_t length) { - int16_t packet_status = SSP_TX_WAITING; - uint16_t retval = FALSE; + int16_t packet_status = SSP_TX_WAITING; + uint16_t retval = FALSE; - packet_status = ssp_SendData(data, length ); // send the data - while( packet_status == SSP_TX_WAITING ) { // check the status - (void)ssp_ReceiveProcess(); // process any bytes received. - packet_status = ssp_SendProcess(); // check the send status + packet_status = ssp_SendData(data, length); // send the data + while (packet_status == SSP_TX_WAITING) { // check the status + (void)ssp_ReceiveProcess(); // process any bytes received. + packet_status = ssp_SendProcess(); // check the send status } - if( packet_status == SSP_TX_ACKED ) { // figure out what happened to the packet + if (packet_status == SSP_TX_ACKED) { // figure out what happened to the packet retval = TRUE; } else { retval = FALSE; @@ -258,61 +256,62 @@ uint16_t qssp::ssp_SendDataBlock(uint8_t *data, uint16_t length ) * \note * */ -int16_t qssp::ssp_SendData(const uint8_t *data, const uint16_t length ) +int16_t qssp::ssp_SendData(const uint8_t *data, const uint16_t length) { - int16_t value = SSP_TX_WAITING; - if( (length + 2) > thisport->txBufSize ) { + if ((length + 2) > thisport->txBufSize) { // TRYING to send too much data. value = SSP_TX_BUFOVERRUN; - } else if( thisport->SendState == SSP_IDLE ) { + } else if (thisport->SendState == SSP_IDLE) { #ifdef ACTIVE_SYNCH - if( thisport->sendSynch == TRUE ) { + if (thisport->sendSynch == TRUE) { sf_SendSynchPacket(); } #endif #ifdef SYNCH_SEND - if( length == 0 ) { + if (length == 0) { // TODO this method could allow a task/user to start a synchronisation step if a zero is mistakenly passed to this function. - // could add a check for a NULL data pointer, or use some sort of static flag that can only be accessed by a static function - // that must be called before calling this function. + // could add a check for a NULL data pointer, or use some sort of static flag that can only be accessed by a static function + // that must be called before calling this function. // we are attempting to send a synch packet - thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us + thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us SETBIT(thisport->flags, SENT_SYNCH); } else { // we are sending a data packet - CLEARBIT( thisport->txSeqNo, ACK_BIT ); // make sure we are not sending a ACK packet - thisport->txSeqNo++; // update the sequence number. - if( thisport->txSeqNo > 0x7F) { // check for sequence number rollover - thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, + CLEARBIT(thisport->txSeqNo, ACK_BIT); // make sure we are not sending a ACK packet + thisport->txSeqNo++; // update the sequence number. + if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover + thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, // zero is reserviced for synchronization requests } } #else - CLEARBIT( thisport->txSeqNo, ACK_BIT ); // make sure we are not sending a ACK packet - thisport->txSeqNo++; // update the sequence number. - if( thisport->txSeqNo > 0x7F) { // check for sequence number rollover - thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, + CLEARBIT(thisport->txSeqNo, ACK_BIT); // make sure we are not sending a ACK packet + thisport->txSeqNo++; // update the sequence number. + if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover + thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, // zero is reserved for synchronization requests } -#endif - CLEARBIT( thisport->flags, ACK_RECEIVED); - thisport->SendState = SSP_AWAITING_ACK; +#endif // ifdef SYNCH_SEND + CLEARBIT(thisport->flags, ACK_RECEIVED); + thisport->SendState = SSP_AWAITING_ACK; value = SSP_TX_WAITING; - thisport->retryCount = 0; // zero out the retry counter for this transmission - sf_MakePacket( thisport->txBuf, data, length, thisport->txSeqNo ); - sf_SendPacket( ); // punch out the packet to the serial port - sf_SetSendTimeout( ); // do the timeout values - if (debug) - qDebug()<<"Sent DATA PACKET:"<txSeqNo; + thisport->retryCount = 0; // zero out the retry counter for this transmission + sf_MakePacket(thisport->txBuf, data, length, thisport->txSeqNo); + sf_SendPacket(); // punch out the packet to the serial port + sf_SetSendTimeout(); // do the timeout values + if (debug) { + qDebug() << "Sent DATA PACKET:" << thisport->txSeqNo; + } } else { // error we are already sending a packet. Need to wait for the current packet to be acked or timeout. value = SSP_TX_BUSY; - if (debug) - qDebug()<<"Error sending TX was busy"; + if (debug) { + qDebug() << "Error sending TX was busy"; + } } return value; } @@ -326,46 +325,47 @@ int16_t qssp::ssp_SendData(const uint8_t *data, const uint16_t length ) * \note * A. send a packet with a sequence number equal to zero * B. if timed out then: - * send synch packet again - * increment try counter - * if number of tries exceed maximum try limit then exit + * send synch packet again + * increment try counter + * if number of tries exceed maximum try limit then exit * C. goto A */ -uint16_t qssp::ssp_Synchronise( ) +uint16_t qssp::ssp_Synchronise() { - int16_t packet_status; - uint16_t retval = FALSE; + int16_t packet_status; + uint16_t retval = FALSE; #ifndef USE_SENDPACKET_DATA - thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us + thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us SETBIT(thisport->flags, SENT_SYNCH); // TODO - should this be using ssp_SendPacketData()?? - sf_MakePacket( thisport->txBuf, NULL, 0, thisport->txSeqNo ); // construct the packet - sf_SendPacket( ); - sf_SetSendTimeout( ); + sf_MakePacket(thisport->txBuf, NULL, 0, thisport->txSeqNo); // construct the packet + sf_SendPacket(); + sf_SetSendTimeout(); thisport->SendState = SSP_AWAITING_ACK; packet_status = SSP_TX_WAITING; #else - packet_status = ssp_SendData( NULL, 0 ); + packet_status = ssp_SendData(NULL, 0); #endif - while( packet_status == SSP_TX_WAITING ) { // we loop until we time out. - (void)ssp_ReceiveProcess( ); // do the receive process - packet_status = ssp_SendProcess( ); // do the send process + while (packet_status == SSP_TX_WAITING) { // we loop until we time out. + (void)ssp_ReceiveProcess(); // do the receive process + packet_status = ssp_SendProcess(); // do the send process } thisport->sendSynch = FALSE; - switch( packet_status ) { + switch (packet_status) { case SSP_TX_ACKED: retval = TRUE; break; - case SSP_TX_BUSY: // intentional fall through. - case SSP_TX_TIMEOUT: // intentional fall through. + case SSP_TX_BUSY: // intentional fall through. + case SSP_TX_TIMEOUT: // intentional fall through. case SSP_TX_BUFOVERRUN: retval = FALSE; break; default: retval = FALSE; break; - }; + } + ; return retval; } @@ -385,8 +385,8 @@ void qssp::sf_SendPacket() // use the raw serial write function so the SYNC byte does not get 'escaped' thisport->pfSerialWrite(SYNC); - for( uint8_t x = 0; x < packetLen; x++ ) { - sf_write_byte(thisport->txBuf[x] ); + for (uint8_t x = 0; x < packetLen; x++) { + sf_write_byte(thisport->txBuf[x]); } thisport->retryCount++; } @@ -408,26 +408,25 @@ void qssp::sf_SendPacket() * 3. TODO: Should this function return an error if data length to be sent is greater th tx buffer size? * */ -void qssp::sf_MakePacket( uint8_t *txBuf, const uint8_t * pdata, uint16_t length, uint8_t seqNo ) +void qssp::sf_MakePacket(uint8_t *txBuf, const uint8_t *pdata, uint16_t length, uint8_t seqNo) { - uint16_t crc = 0xffff; - uint16_t bufPos = 0; - uint8_t b; + uint16_t crc = 0xffff; + uint16_t bufPos = 0; + uint8_t b; // add 1 for the seq. number txBuf[LENGTH] = length + 1; txBuf[SEQNUM] = seqNo; - crc = sf_crc16( crc, seqNo ); + crc = sf_crc16(crc, seqNo); - length = length + 2; // add two for the length and seqno bytes which are added before the loop. - for( bufPos = 2; bufPos < length; bufPos++ ) { - b = *pdata++; + length = length + 2; // add two for the length and seqno bytes which are added before the loop. + for (bufPos = 2; bufPos < length; bufPos++) { + b = *pdata++; txBuf[bufPos] = b; - crc = sf_crc16( crc, b ); // update CRC value + crc = sf_crc16(crc, b); // update CRC value } txBuf[bufPos++] = LOWERBYTE(crc); txBuf[bufPos] = UPPERBYTE(crc); - } /*! @@ -442,13 +441,14 @@ void qssp::sf_MakePacket( uint8_t *txBuf, const uint8_t * pdata, uint16_t length void qssp::sf_SendAckPacket(uint8_t seqNumber) { - uint8_t AckSeqNumber = SETBIT( seqNumber, ACK_BIT ); + uint8_t AckSeqNumber = SETBIT(seqNumber, ACK_BIT); // create the packet, note we pass AckSequenceNumber directly - sf_MakePacket( thisport->txBuf, NULL, 0, AckSeqNumber ); - sf_SendPacket( ); - if (debug) - qDebug()<<"Sent ACK PACKET:"<txBuf, NULL, 0, AckSeqNumber); + sf_SendPacket(); + if (debug) { + qDebug() << "Sent ACK PACKET:" << seqNumber; + } // we don't set the timeout for an ACK because we don't ACK our ACKs in this protocol } @@ -461,30 +461,30 @@ void qssp::sf_SendAckPacket(uint8_t seqNumber) * \note * */ -void qssp::sf_write_byte(uint8_t c ) +void qssp::sf_write_byte(uint8_t c) { - if( c == SYNC ) { // check for SYNC byte - thisport->pfSerialWrite( ESC ); // since we are not starting a packet we must ESCAPE the SYNCH byte - thisport->pfSerialWrite( ESC_SYNC ); // now send the escaped synch char - } else if( c == ESC ) { // Check for ESC character - thisport->pfSerialWrite( ESC ); // if it is, we need to send it twice - thisport->pfSerialWrite( ESC ); + if (c == SYNC) { // check for SYNC byte + thisport->pfSerialWrite(ESC); // since we are not starting a packet we must ESCAPE the SYNCH byte + thisport->pfSerialWrite(ESC_SYNC); // now send the escaped synch char + } else if (c == ESC) { // Check for ESC character + thisport->pfSerialWrite(ESC); // if it is, we need to send it twice + thisport->pfSerialWrite(ESC); } else { - thisport->pfSerialWrite( c ); // otherwise write the byte to serial port + thisport->pfSerialWrite(c); // otherwise write the byte to serial port } } /************************************************************************************************************ -* -* NAME: uint16_t ssp_crc16( uint16_t crc, uint16_t data ) -* DESCRIPTION: Uses crc_table to calculate new crc -* ARGUMENTS: -* arg1: crc -* arg2: data - byte to calculate into CRC -* RETURN: New crc -* CREATED: 5/8/02 -* -*************************************************************************************************************/ + * + * NAME: uint16_t ssp_crc16( uint16_t crc, uint16_t data ) + * DESCRIPTION: Uses crc_table to calculate new crc + * ARGUMENTS: + * arg1: crc + * arg2: data - byte to calculate into CRC + * RETURN: New crc + * CREATED: 5/8/02 + * + *************************************************************************************************************/ /*! * \brief calculates the new CRC value for 'data' * \param crc = current CRC value @@ -495,9 +495,9 @@ void qssp::sf_write_byte(uint8_t c ) * */ -uint16_t qssp::sf_crc16( uint16_t crc, uint8_t data ) +uint16_t qssp::sf_crc16(uint16_t crc, uint8_t data) { - return (crc >> 8) ^ CRC_TABLE[( crc ^ data ) & 0x00FF ]; + return (crc >> 8) ^ CRC_TABLE[(crc ^ data) & 0x00FF]; } @@ -512,7 +512,8 @@ uint16_t qssp::sf_crc16( uint16_t crc, uint8_t data ) void qssp::sf_SetSendTimeout() { - uint32_t timeout; + uint32_t timeout; + timeout = thisport->pfGetTime() + thisport->timeoutLen; thisport->timeout = timeout; } @@ -526,32 +527,34 @@ void qssp::sf_SetSendTimeout() * \note * */ -uint16_t qssp::sf_CheckTimeout() +uint16_t qssp::sf_CheckTimeout() { - uint16_t retval = FALSE; - uint32_t current_time; + uint16_t retval = FALSE; + uint32_t current_time; current_time = thisport->pfGetTime(); - if( current_time > thisport->timeout ) { + if (current_time > thisport->timeout) { retval = TRUE; } - if(retval) - if (debug) - qDebug()<<"timeout"<timeout; + if (retval) { + if (debug) { + qDebug() << "timeout" << current_time << thisport->timeout; + } + } return retval; } /**************************************************************************** - * NAME: sf_ReceiveState - * DESC: Implements the receive state handling code for escaped and unescaped data - * ARGS: thisport - which port to operate on - * c - incoming byte - * RETURN: - * CREATED: - * NOTES: - * 1. change from using pointer to functions. - ****************************************************************************/ +* NAME: sf_ReceiveState +* DESC: Implements the receive state handling code for escaped and unescaped data +* ARGS: thisport - which port to operate on +* c - incoming byte +* RETURN: +* CREATED: +* NOTES: +* 1. change from using pointer to functions. +****************************************************************************/ /*! * \brief implements the receive state handling code for escaped and unescaped data * \param thisport = which port to use @@ -561,46 +564,46 @@ uint16_t qssp::sf_CheckTimeout() * \note * */ -int16_t qssp::sf_ReceiveState(uint8_t c ) +int16_t qssp::sf_ReceiveState(uint8_t c) { - int16_t retval = SSP_RX_RECEIVING; + int16_t retval = SSP_RX_RECEIVING; - switch( thisport->InputState ) { + switch (thisport->InputState) { case state_unescaped_e: - if( c == SYNC ) { + if (c == SYNC) { thisport->DecodeState = decode_len1_e; - } else if ( c == ESC ) { + } else if (c == ESC) { thisport->InputState = state_escaped_e; } else { retval = sf_DecodeState(c); } - break; // end of unescaped state. - case state_escaped_e: + break; // end of unescaped state. + case state_escaped_e: thisport->InputState = state_unescaped_e; - if( c == SYNC ) { + if (c == SYNC) { thisport->DecodeState = decode_len1_e; - } else if (c == ESC_SYNC ) { + } else if (c == ESC_SYNC) { retval = sf_DecodeState(SYNC); } else { retval = sf_DecodeState(c); } - break; // end of the escaped state. - default: + break; // end of the escaped state. + default: break; } return retval; } /**************************************************************************** - * NAME: sf_DecodeState - * DESC: Implements the receive state finite state machine - * ARGS: thisport - which port to operate on - * c - incoming byte - * RETURN: - * CREATED: - * NOTES: - * 1. change from using pointer to functions. - ****************************************************************************/ +* NAME: sf_DecodeState +* DESC: Implements the receive state finite state machine +* ARGS: thisport - which port to operate on +* c - incoming byte +* RETURN: +* CREATED: +* NOTES: +* 1. change from using pointer to functions. +****************************************************************************/ /*! * \brief implements the receiving decoding state machine @@ -611,19 +614,20 @@ int16_t qssp::sf_ReceiveState(uint8_t c ) * \note * */ -int16_t qssp::sf_DecodeState( uint8_t c ) +int16_t qssp::sf_DecodeState(uint8_t c) { - int16_t retval; - switch( thisport->DecodeState ) { + int16_t retval; + + switch (thisport->DecodeState) { case decode_idle_e: // 'c' is ignored in this state as the only way to leave the idle state is // recognition of the SYNC byte in the sf_ReceiveState function. retval = SSP_RX_IDLE; break; case decode_len1_e: - thisport->rxBuf[LENGTH]= c; + thisport->rxBuf[LENGTH] = c; thisport->rxBufLen = c; - if( thisport->rxBufLen <= thisport->rxBufSize ) { + if (thisport->rxBufLen <= thisport->rxBufSize) { thisport->DecodeState = decode_seqNo_e; retval = SSP_RX_RECEIVING; } else { @@ -631,37 +635,37 @@ int16_t qssp::sf_DecodeState( uint8_t c ) retval = SSP_RX_IDLE; } break; - case decode_seqNo_e: + case decode_seqNo_e: thisport->rxBuf[SEQNUM] = c; thisport->crc = 0xffff; - thisport->rxBufLen--; // subtract 1 for the seq. no. + thisport->rxBufLen--; // subtract 1 for the seq. no. thisport->rxBufPos = 2; - thisport->crc = sf_crc16( thisport->crc, c ); - if( thisport->rxBufLen > 0 ) { + thisport->crc = sf_crc16(thisport->crc, c); + if (thisport->rxBufLen > 0) { thisport->DecodeState = decode_data_e; } else { thisport->DecodeState = decode_crc1_e; } retval = SSP_RX_RECEIVING; break; - case decode_data_e: - thisport->rxBuf[ (thisport->rxBufPos)++] = c; - thisport->crc = sf_crc16( thisport->crc, c ); - if( thisport->rxBufPos == (thisport->rxBufLen+2) ) { + case decode_data_e: + thisport->rxBuf[(thisport->rxBufPos)++] = c; + thisport->crc = sf_crc16(thisport->crc, c); + if (thisport->rxBufPos == (thisport->rxBufLen + 2)) { thisport->DecodeState = decode_crc1_e; } retval = SSP_RX_RECEIVING; break; - case decode_crc1_e: - thisport->crc = sf_crc16( thisport->crc, c ); + case decode_crc1_e: + thisport->crc = sf_crc16(thisport->crc, c); thisport->DecodeState = decode_crc2_e; retval = SSP_RX_RECEIVING; break; - case decode_crc2_e: + case decode_crc2_e: thisport->DecodeState = decode_idle_e; // verify the CRC value for the packet - if( sf_crc16( thisport->crc, c) == 0) { + if (sf_crc16(thisport->crc, c) == 0) { // TODO shouldn't the return value of sf_ReceivePacket() be checked? sf_ReceivePacket(); retval = SSP_RX_COMPLETE; @@ -670,8 +674,8 @@ int16_t qssp::sf_DecodeState( uint8_t c ) retval = SSP_RX_IDLE; } break; - default: - thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet. + default: + thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet. retval = SSP_RX_IDLE; break; } @@ -679,18 +683,18 @@ int16_t qssp::sf_DecodeState( uint8_t c ) } /************************************************************************************************************ -* -* NAME: int16_t sf_ReceivePacket( ) -* DESCRIPTION: Receive one packet, assumed that data is in rec.buff[] -* ARGUMENTS: -* RETURN: 0 . no new packet was received, could be ack or same packet -* 1 . new packet received -* SSP_PACKET_? -* SSP_PACKET_COMPLETE -* SSP_PACKET_ACK -* CREATED: 5/8/02 -* -*************************************************************************************************************/ + * + * NAME: int16_t sf_ReceivePacket( ) + * DESCRIPTION: Receive one packet, assumed that data is in rec.buff[] + * ARGUMENTS: + * RETURN: 0 . no new packet was received, could be ack or same packet + * 1 . new packet received + * SSP_PACKET_? + * SSP_PACKET_COMPLETE + * SSP_PACKET_ACK + * CREATED: 5/8/02 + * + *************************************************************************************************************/ /*! * \brief receive one packet. calls the callback function if needed. * \param thisport = which port to use @@ -706,76 +710,79 @@ int16_t qssp::sf_ReceivePacket() { int16_t value = FALSE; - if( ISBITSET(thisport->rxBuf[SEQNUM], ACK_BIT ) ) { - // Received an ACK packet, need to check if it matches the previous sent packet - if( ( thisport->rxBuf[SEQNUM] & 0x7F) == (thisport->txSeqNo & 0x7f)) { - // It matches the last packet sent by us - SETBIT( thisport->txSeqNo, ACK_BIT ); + if (ISBITSET(thisport->rxBuf[SEQNUM], ACK_BIT)) { + // Received an ACK packet, need to check if it matches the previous sent packet + if ((thisport->rxBuf[SEQNUM] & 0x7F) == (thisport->txSeqNo & 0x7f)) { + // It matches the last packet sent by us + SETBIT(thisport->txSeqNo, ACK_BIT); thisport->SendState = SSP_ACKED; value = FALSE; - if (debug) - qDebug()<<"Received ACK:"<<(thisport->txSeqNo & 0x7F); + if (debug) { + qDebug() << "Received ACK:" << (thisport->txSeqNo & 0x7F); + } } // else ignore the ACK packet } else { - // Received a 'data' packet, figure out what type of packet we received... - if( thisport->rxBuf[SEQNUM] == 0 ) { - if (debug) - qDebug()<<"Received SYNC Request"; + // Received a 'data' packet, figure out what type of packet we received... + if (thisport->rxBuf[SEQNUM] == 0) { + if (debug) { + qDebug() << "Received SYNC Request"; + } // Synchronize sequence number with host #ifdef ACTIVE_SYNCH thisport->sendSynch = TRUE; #endif - sf_SendAckPacket(thisport->rxBuf[SEQNUM] ); - thisport->rxSeqNo = 0; + sf_SendAckPacket(thisport->rxBuf[SEQNUM]); + thisport->rxSeqNo = 0; value = FALSE; - } else if( thisport->rxBuf[SEQNUM] == thisport->rxSeqNo ) { + } else if (thisport->rxBuf[SEQNUM] == thisport->rxSeqNo) { // Already seen this packet, just ack it, don't act on the packet. - sf_SendAckPacket(thisport->rxBuf[SEQNUM] ); + sf_SendAckPacket(thisport->rxBuf[SEQNUM]); value = FALSE; } else { - //New Packet + // New Packet thisport->rxSeqNo = thisport->rxBuf[SEQNUM]; // Let the application do something with the data/packet. // skip the first two bytes (length and seq. no.) in the buffer. - if (debug) - qDebug()<<"Received DATA PACKET seq="<rxSeqNo<<"Data="<<(uint8_t)thisport->rxBuf[2]<<(uint8_t)thisport->rxBuf[3]<<(uint8_t)thisport->rxBuf[4]; - pfCallBack( &(thisport->rxBuf[2]), thisport->rxBufLen); + if (debug) { + qDebug() << "Received DATA PACKET seq=" << thisport->rxSeqNo << "Data=" << (uint8_t)thisport->rxBuf[2] << (uint8_t)thisport->rxBuf[3] << (uint8_t)thisport->rxBuf[4]; + } + pfCallBack(&(thisport->rxBuf[2]), thisport->rxBufLen); // after we send the ACK, it is possible for the host to send a new packet. // Thus the application needs to copy the data and reset the receive buffer // inside of thisport->pfCallBack() - sf_SendAckPacket(thisport->rxBuf[SEQNUM] ); + sf_SendAckPacket(thisport->rxBuf[SEQNUM]); value = TRUE; } } return value; } -qssp::qssp(port * info,bool debug):debug(debug) +qssp::qssp(port *info, bool debug) : debug(debug) { - - thisport=info; - thisport->maxRetryCount = info->max_retry; - thisport->timeoutLen = info->timeoutLen; + thisport = info; + thisport->maxRetryCount = info->max_retry; + thisport->timeoutLen = info->timeoutLen; thisport->txBufSize = info->txBufSize; - thisport->rxBufSize = info->rxBufSize; - thisport->txBuf = info->txBuf; - thisport->rxBuf = info->rxBuf; + thisport->rxBufSize = info->rxBufSize; + thisport->txBuf = info->txBuf; + thisport->rxBuf = info->rxBuf; thisport->retryCount = 0; - thisport->sendSynch = FALSE; //TRUE; - thisport->rxSeqNo = 255; - thisport->txSeqNo = 255; - thisport->SendState = SSP_IDLE; - thisport->InputState =(ReceiveState)0; - thisport->DecodeState =(decodeState_)0; - thisport->TxError =0; - thisport->RxError =0; - thisport->txSeqNo =0; - thisport->rxSeqNo =0; + thisport->sendSynch = FALSE; // TRUE; + thisport->rxSeqNo = 255; + thisport->txSeqNo = 255; + thisport->SendState = SSP_IDLE; + thisport->InputState = (ReceiveState)0; + thisport->DecodeState = (decodeState_)0; + thisport->TxError = 0; + thisport->RxError = 0; + thisport->txSeqNo = 0; + thisport->rxSeqNo = 0; } -void qssp::pfCallBack( uint8_t * buf, uint16_t size) +void qssp::pfCallBack(uint8_t *buf, uint16_t size) { - if (debug) - qDebug()<<"receive callback"<ssp_ReceiveProcess(); - sendstatus=this->ssp_SendProcess(); + while (!endthread) { + receivestatus = this->ssp_ReceiveProcess(); + sendstatus = this->ssp_SendProcess(); sendbufmutex.lock(); - if(datapending && receivestatus==SSP_TX_IDLE) - { - this->ssp_SendData(mbuf,msize); - datapending=false; + if (datapending && receivestatus == SSP_TX_IDLE) { + this->ssp_SendData(mbuf, msize); + datapending = false; } sendbufmutex.unlock(); - if(sendstatus==SSP_TX_ACKED) + if (sendstatus == SSP_TX_ACKED) { sendwait.wakeAll(); + } } - } -bool qsspt::sendData(uint8_t * buf,uint16_t size) +bool qsspt::sendData(uint8_t *buf, uint16_t size) { - if(datapending) + if (datapending) { return false; + } sendbufmutex.lock(); - datapending=true; - mbuf=buf; - msize=size; + datapending = true; + mbuf = buf; + msize = size; sendbufmutex.unlock(); msendwait.lock(); - sendwait.wait(&msendwait,100000); + sendwait.wait(&msendwait, 100000); msendwait.unlock(); return true; } -void qsspt::pfCallBack( uint8_t * buf, uint16_t size) +void qsspt::pfCallBack(uint8_t *buf, uint16_t size) { - if (debug) - qDebug()<<"receive callback"<> 8 ) -#define UPPERWORD(lw) (uint16_t)(((lw) & 0xffff0000) >> 16 ) -#define LOWERWORD(lw) (uint16_t)((lw) & 0x0000ffff) +#define LOWERBYTE(w) (uint8_t)((w) & 0x00ff) +#define UPPERBYTE(w) (uint8_t)(((w) & 0xff00) >> 8) +#define UPPERWORD(lw) (uint16_t)(((lw) & 0xffff0000) >> 16) +#define LOWERWORD(lw) (uint16_t)((lw) & 0x0000ffff) // Macros to operate on a target and bitmask. -#define CLEARBIT( a, b ) ((a) = (a) & ~(b)) -#define SETBIT( a, b ) ((a) = (a) | (b) ) -#define TOGGLEBIT(a,b) ((a) = (a) ^ (b) ) +#define CLEARBIT(a, b) ((a) = (a) & ~(b)) +#define SETBIT(a, b) ((a) = (a) | (b)) +#define TOGGLEBIT(a, b) ((a) = (a) ^ (b)) // test bit macros operate using a bit mask. -#define ISBITSET( a, b ) ( ((a) & (b)) == (b) ? TRUE : FALSE ) -#define ISBITCLEAR( a, b) ( (~(a) & (b)) == (b) ? TRUE : FALSE ) +#define ISBITSET(a, b) (((a) & (b)) == (b) ? TRUE : FALSE) +#define ISBITCLEAR(a, b) ((~(a) & (b)) == (b) ? TRUE : FALSE) /** PRIVATE FUNCTIONS **/ -//static void sf_SendSynchPacket( Port_t *thisport ); -static uint16_t sf_crc16( uint16_t crc, uint8_t data ); -static void sf_write_byte( Port_t *thisport, uint8_t c ); -static void sf_SetSendTimeout( Port_t *thisport ); -static uint16_t sf_CheckTimeout( Port_t *thisport ); -static int16_t sf_DecodeState( Port_t *thisport, uint8_t c ); -static int16_t sf_ReceiveState( Port_t *thisport, uint8_t c ); +// static void sf_SendSynchPacket( Port_t *thisport ); +static uint16_t sf_crc16(uint16_t crc, uint8_t data); +static void sf_write_byte(Port_t *thisport, uint8_t c); +static void sf_SetSendTimeout(Port_t *thisport); +static uint16_t sf_CheckTimeout(Port_t *thisport); +static int16_t sf_DecodeState(Port_t *thisport, uint8_t c); +static int16_t sf_ReceiveState(Port_t *thisport, uint8_t c); -static void sf_SendPacket( Port_t *thisport ); -static void sf_SendAckPacket( Port_t *thisport, uint8_t seqNumber); -static void sf_MakePacket( uint8_t *buf, const uint8_t * pdata, uint16_t length, uint8_t seqNo ); -static int16_t sf_ReceivePacket(Port_t *thisport); +static void sf_SendPacket(Port_t *thisport); +static void sf_SendAckPacket(Port_t *thisport, uint8_t seqNumber); +static void sf_MakePacket(uint8_t *buf, const uint8_t *pdata, uint16_t length, uint8_t seqNo); +static int16_t sf_ReceivePacket(Port_t *thisport); /* Flag bit masks...*/ -#define SENT_SYNCH (0x01) -#define ACK_RECEIVED (0x02) -#define ACK_EXPECTED (0x04) +#define SENT_SYNCH (0x01) +#define ACK_RECEIVED (0x02) +#define ACK_EXPECTED (0x04) -#define SSP_AWAITING_ACK 0 -#define SSP_ACKED 1 -#define SSP_IDLE 2 +#define SSP_AWAITING_ACK 0 +#define SSP_ACKED 1 +#define SSP_IDLE 2 /** PRIVATE DATA **/ static const uint16_t CRC_TABLE[] = { @@ -167,7 +166,7 @@ static const uint16_t CRC_TABLE[] = { 0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41, 0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641, 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040 - }; +}; /** EXTERNAL DATA **/ @@ -188,24 +187,24 @@ static const uint16_t CRC_TABLE[] = { * Must be called before calling the Send or REceive process functions. */ -void ssp_Init( Port_t *thisport, const PortConfig_t* const info) +void ssp_Init(Port_t *thisport, const PortConfig_t *const info) { - thisport->pfCallBack = info->pfCallBack; - thisport->pfSerialRead = info->pfSerialRead; - thisport->pfSerialWrite = info->pfSerialWrite; + thisport->pfCallBack = info->pfCallBack; + thisport->pfSerialRead = info->pfSerialRead; + thisport->pfSerialWrite = info->pfSerialWrite; thisport->pfGetTime = info->pfGetTime; - thisport->maxRetryCount = info->max_retry; - thisport->timeoutLen = info->timeoutLen; - thisport->txBufSize = info->txBufSize; - thisport->rxBufSize = info->rxBufSize; - thisport->txBuf = info->txBuf; - thisport->rxBuf = info->rxBuf; - thisport->retryCount = 0; - thisport->sendSynch = FALSE; //TRUE; - thisport->rxSeqNo = 255; - thisport->txSeqNo = 255; - thisport->SendState = SSP_IDLE; + thisport->maxRetryCount = info->max_retry; + thisport->timeoutLen = info->timeoutLen; + thisport->txBufSize = info->txBufSize; + thisport->rxBufSize = info->rxBufSize; + thisport->txBuf = info->txBuf; + thisport->rxBuf = info->rxBuf; + thisport->retryCount = 0; + thisport->sendSynch = FALSE; // TRUE; + thisport->rxSeqNo = 255; + thisport->txSeqNo = 255; + thisport->SendState = SSP_IDLE; } /*! @@ -219,35 +218,35 @@ void ssp_Init( Port_t *thisport, const PortConfig_t* const info) * \note * */ -int16_t ssp_SendProcess( Port_t *thisport ) +int16_t ssp_SendProcess(Port_t *thisport) { - int16_t value = SSP_TX_WAITING; + int16_t value = SSP_TX_WAITING; - if (thisport->SendState == SSP_AWAITING_ACK ) { - if (sf_CheckTimeout(thisport) == TRUE) { - if (thisport->retryCount < thisport->maxRetryCount) { - // Try again - sf_SendPacket(thisport); - sf_SetSendTimeout(thisport); - value = SSP_TX_WAITING; - } else { - // Give up, # of trys has exceded the limit - value = SSP_TX_TIMEOUT; - CLEARBIT( thisport->flags, ACK_RECEIVED); + if (thisport->SendState == SSP_AWAITING_ACK) { + if (sf_CheckTimeout(thisport) == TRUE) { + if (thisport->retryCount < thisport->maxRetryCount) { + // Try again + sf_SendPacket(thisport); + sf_SetSendTimeout(thisport); + value = SSP_TX_WAITING; + } else { + // Give up, # of trys has exceded the limit + value = SSP_TX_TIMEOUT; + CLEARBIT(thisport->flags, ACK_RECEIVED); thisport->SendState = SSP_IDLE; - } - } else { - value = SSP_TX_WAITING; - } - } else if( thisport->SendState == SSP_ACKED ) { - SETBIT( thisport->flags, ACK_RECEIVED); + } + } else { + value = SSP_TX_WAITING; + } + } else if (thisport->SendState == SSP_ACKED) { + SETBIT(thisport->flags, ACK_RECEIVED); value = SSP_TX_ACKED; thisport->SendState = SSP_IDLE; - } else { - thisport->SendState = SSP_IDLE; + } else { + thisport->SendState = SSP_IDLE; value = SSP_TX_IDLE; } - return value; + return value; } /*! @@ -260,17 +259,17 @@ int16_t ssp_SendProcess( Port_t *thisport ) */ int16_t ssp_ReceiveProcess(Port_t *thisport) { - int16_t b; - int16_t packet_status = SSP_RX_IDLE; + int16_t b; + int16_t packet_status = SSP_RX_IDLE; - do { - b = thisport->pfSerialRead(); // attempt to read a char from the serial buffer - if (b != -1) { - packet_status = sf_ReceiveState(thisport, b); // process the newly received byte in the receive state machine - } - // keep going until either we received a full packet or there are no more bytes to process - } while (packet_status != SSP_RX_COMPLETE && b != -1); - return packet_status; + do { + b = thisport->pfSerialRead(); // attempt to read a char from the serial buffer + if (b != -1) { + packet_status = sf_ReceiveState(thisport, b); // process the newly received byte in the receive state machine + } + // keep going until either we received a full packet or there are no more bytes to process + } while (packet_status != SSP_RX_COMPLETE && b != -1); + return packet_status; } /*! @@ -282,13 +281,13 @@ int16_t ssp_ReceiveProcess(Port_t *thisport) * */ -int16_t ssp_ReceiveByte(Port_t *thisport ) +int16_t ssp_ReceiveByte(Port_t *thisport) { int16_t b; int16_t packet_status = SSP_RX_IDLE; b = thisport->pfSerialRead(); - if( b != -1 ) { + if (b != -1) { packet_status = sf_ReceiveState(thisport, b); } return packet_status; @@ -305,17 +304,17 @@ int16_t ssp_ReceiveByte(Port_t *thisport ) * \note * */ -uint16_t ssp_SendDataBlock( Port_t *thisport, uint8_t *data, uint16_t length ) +uint16_t ssp_SendDataBlock(Port_t *thisport, uint8_t *data, uint16_t length) { - int16_t packet_status = SSP_TX_WAITING; - uint16_t retval = FALSE; + int16_t packet_status = SSP_TX_WAITING; + uint16_t retval = FALSE; - packet_status = ssp_SendData( thisport, data, length ); // send the data - while( packet_status == SSP_TX_WAITING ) { // check the status - (void)ssp_ReceiveProcess( thisport ); // process any bytes received. - packet_status = ssp_SendProcess( thisport ); // check the send status + packet_status = ssp_SendData(thisport, data, length); // send the data + while (packet_status == SSP_TX_WAITING) { // check the status + (void)ssp_ReceiveProcess(thisport); // process any bytes received. + packet_status = ssp_SendProcess(thisport); // check the send status } - if( packet_status == SSP_TX_ACKED ) { // figure out what happened to the packet + if (packet_status == SSP_TX_ACKED) { // figure out what happened to the packet retval = TRUE; } else { retval = FALSE; @@ -335,54 +334,53 @@ uint16_t ssp_SendDataBlock( Port_t *thisport, uint8_t *data, uint16_t length ) * \note * */ -int16_t ssp_SendData( Port_t *thisport, const uint8_t *data, const uint16_t length ) +int16_t ssp_SendData(Port_t *thisport, const uint8_t *data, const uint16_t length) { - int16_t value = SSP_TX_WAITING; - if( (length + 2) > thisport->txBufSize ) { + if ((length + 2) > thisport->txBufSize) { // TRYING to send too much data. value = SSP_TX_BUFOVERRUN; - } else if( thisport->SendState == SSP_IDLE ) { + } else if (thisport->SendState == SSP_IDLE) { #ifdef ACTIVE_SYNCH - if( thisport->sendSynch == TRUE ) { + if (thisport->sendSynch == TRUE) { sf_SendSynchPacket(thisport); } #endif #ifdef SYNCH_SEND - if( length == 0 ) { + if (length == 0) { // TODO this method could allow a task/user to start a synchronisation step if a zero is mistakenly passed to this function. - // could add a check for a NULL data pointer, or use some sort of static flag that can only be accessed by a static function - // that must be called before calling this function. + // could add a check for a NULL data pointer, or use some sort of static flag that can only be accessed by a static function + // that must be called before calling this function. // we are attempting to send a synch packet - thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us + thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us SETBIT(thisport->flags, SENT_SYNCH); } else { // we are sending a data packet - CLEARBIT( thisport->txSeqNo, ACK_BIT ); // make sure we are not sending a ACK packet - thisport->txSeqNo++; // update the sequence number. - if( thisport->txSeqNo > 0x7F) { // check for sequence number rollover - thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, - // zero is reserviced for synchronization requests + CLEARBIT(thisport->txSeqNo, ACK_BIT); // make sure we are not sending a ACK packet + thisport->txSeqNo++; // update the sequence number. + if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover + thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, + // zero is reserviced for synchronization requests } } #else - CLEARBIT( thisport->txSeqNo, ACK_BIT ); // make sure we are not sending a ACK packet - thisport->txSeqNo++; // update the sequence number. - if( thisport->txSeqNo > 0x7F) { // check for sequence number rollover - thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, - // zero is reserved for synchronization requests + CLEARBIT(thisport->txSeqNo, ACK_BIT); // make sure we are not sending a ACK packet + thisport->txSeqNo++; // update the sequence number. + if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover + thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, + // zero is reserved for synchronization requests } -#endif - CLEARBIT( thisport->flags, ACK_RECEIVED); - thisport->SendState = SSP_AWAITING_ACK; +#endif // ifdef SYNCH_SEND + CLEARBIT(thisport->flags, ACK_RECEIVED); + thisport->SendState = SSP_AWAITING_ACK; value = SSP_TX_WAITING; - thisport->retryCount = 0; // zero out the retry counter for this transmission - sf_MakePacket( thisport->txBuf, data, length, thisport->txSeqNo ); - sf_SendPacket( thisport ); // punch out the packet to the serial port - sf_SetSendTimeout( thisport ); // do the timeout values + thisport->retryCount = 0; // zero out the retry counter for this transmission + sf_MakePacket(thisport->txBuf, data, length, thisport->txSeqNo); + sf_SendPacket(thisport); // punch out the packet to the serial port + sf_SetSendTimeout(thisport); // do the timeout values } else { // error we are already sending a packet. Need to wait for the current packet to be acked or timeout. value = SSP_TX_BUSY; @@ -399,46 +397,47 @@ int16_t ssp_SendData( Port_t *thisport, const uint8_t *data, const uint16_t leng * \note * A. send a packet with a sequence number equal to zero * B. if timed out then: - * send synch packet again - * increment try counter - * if number of tries exceed maximum try limit then exit + * send synch packet again + * increment try counter + * if number of tries exceed maximum try limit then exit * C. goto A */ -uint16_t ssp_Synchronise( Port_t *thisport ) +uint16_t ssp_Synchronise(Port_t *thisport) { - int16_t packet_status; - uint16_t retval = FALSE; + int16_t packet_status; + uint16_t retval = FALSE; #ifndef USE_SENDPACKET_DATA - thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us + thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us SETBIT(thisport->flags, SENT_SYNCH); // TODO - should this be using ssp_SendPacketData()?? - sf_MakePacket( thisport->txBuf, NULL, 0, thisport->txSeqNo ); // construct the packet - sf_SendPacket( thisport ); - sf_SetSendTimeout( thisport ); + sf_MakePacket(thisport->txBuf, NULL, 0, thisport->txSeqNo); // construct the packet + sf_SendPacket(thisport); + sf_SetSendTimeout(thisport); thisport->SendState = SSP_AWAITING_ACK; packet_status = SSP_TX_WAITING; #else - packet_status = ssp_SendData( thisport, NULL, 0 ); + packet_status = ssp_SendData(thisport, NULL, 0); #endif - while( packet_status == SSP_TX_WAITING ) { // we loop until we time out. - (void)ssp_ReceiveProcess( thisport ); // do the receive process - packet_status = ssp_SendProcess( thisport ); // do the send process + while (packet_status == SSP_TX_WAITING) { // we loop until we time out. + (void)ssp_ReceiveProcess(thisport); // do the receive process + packet_status = ssp_SendProcess(thisport); // do the send process } thisport->sendSynch = FALSE; - switch( packet_status ) { + switch (packet_status) { case SSP_TX_ACKED: retval = TRUE; break; - case SSP_TX_BUSY: // intentional fall through. - case SSP_TX_TIMEOUT: // intentional fall through. + case SSP_TX_BUSY: // intentional fall through. + case SSP_TX_TIMEOUT: // intentional fall through. case SSP_TX_BUFOVERRUN: retval = FALSE; break; default: retval = FALSE; break; - }; + } + ; return retval; } @@ -452,14 +451,14 @@ uint16_t ssp_Synchronise( Port_t *thisport ) * Packet should be formed through the use of sf_MakePacket before calling this function. */ static void sf_SendPacket(Port_t *thisport) - { - // add 3 to packet data length for: 1 length + 2 CRC (packet overhead) - uint8_t packetLen = thisport->txBuf[LENGTH] + 3; +{ + // add 3 to packet data length for: 1 length + 2 CRC (packet overhead) + uint8_t packetLen = thisport->txBuf[LENGTH] + 3; // use the raw serial write function so the SYNC byte does not get 'escaped' - thisport->pfSerialWrite(SYNC); - for( uint8_t x = 0; x < packetLen; x++ ) { - sf_write_byte(thisport, thisport->txBuf[x] ); + thisport->pfSerialWrite(SYNC); + for (uint8_t x = 0; x < packetLen; x++) { + sf_write_byte(thisport, thisport->txBuf[x]); } thisport->retryCount++; } @@ -481,26 +480,25 @@ static void sf_SendPacket(Port_t *thisport) * 3. TODO: Should this function return an error if data length to be sent is greater th tx buffer size? * */ -void sf_MakePacket( uint8_t *txBuf, const uint8_t * pdata, uint16_t length, uint8_t seqNo ) +void sf_MakePacket(uint8_t *txBuf, const uint8_t *pdata, uint16_t length, uint8_t seqNo) { - uint16_t crc = 0xffff; - uint16_t bufPos = 0; - uint8_t b; + uint16_t crc = 0xffff; + uint16_t bufPos = 0; + uint8_t b; // add 1 for the seq. number txBuf[LENGTH] = length + 1; txBuf[SEQNUM] = seqNo; - crc = sf_crc16( crc, seqNo ); + crc = sf_crc16(crc, seqNo); - length = length + 2; // add two for the length and seqno bytes which are added before the loop. - for( bufPos = 2; bufPos < length; bufPos++ ) { - b = *pdata++; + length = length + 2; // add two for the length and seqno bytes which are added before the loop. + for (bufPos = 2; bufPos < length; bufPos++) { + b = *pdata++; txBuf[bufPos] = b; - crc = sf_crc16( crc, b ); // update CRC value + crc = sf_crc16(crc, b); // update CRC value } txBuf[bufPos++] = LOWERBYTE(crc); txBuf[bufPos] = UPPERBYTE(crc); - } /*! @@ -513,13 +511,13 @@ void sf_MakePacket( uint8_t *txBuf, const uint8_t * pdata, uint16_t length, uint * */ -static void sf_SendAckPacket( Port_t *thisport, uint8_t seqNumber) +static void sf_SendAckPacket(Port_t *thisport, uint8_t seqNumber) { - uint8_t AckSeqNumber = SETBIT( seqNumber, ACK_BIT ); + uint8_t AckSeqNumber = SETBIT(seqNumber, ACK_BIT); // create the packet, note we pass AckSequenceNumber directly - sf_MakePacket( thisport->txBuf, NULL, 0, AckSeqNumber ); - sf_SendPacket( thisport ); + sf_MakePacket(thisport->txBuf, NULL, 0, AckSeqNumber); + sf_SendPacket(thisport); // we don't set the timeout for an ACK because we don't ACK our ACKs in this protocol } @@ -532,30 +530,30 @@ static void sf_SendAckPacket( Port_t *thisport, uint8_t seqNumber) * \note * */ -static void sf_write_byte( Port_t *thisport, uint8_t c ) +static void sf_write_byte(Port_t *thisport, uint8_t c) { - if( c == SYNC ) { // check for SYNC byte - thisport->pfSerialWrite( ESC ); // since we are not starting a packet we must ESCAPE the SYNCH byte - thisport->pfSerialWrite( ESC_SYNC ); // now send the escaped synch char - } else if( c == ESC ) { // Check for ESC character - thisport->pfSerialWrite( ESC ); // if it is, we need to send it twice - thisport->pfSerialWrite( ESC ); - } else { - thisport->pfSerialWrite( c ); // otherwise write the byte to serial port - } + if (c == SYNC) { // check for SYNC byte + thisport->pfSerialWrite(ESC); // since we are not starting a packet we must ESCAPE the SYNCH byte + thisport->pfSerialWrite(ESC_SYNC); // now send the escaped synch char + } else if (c == ESC) { // Check for ESC character + thisport->pfSerialWrite(ESC); // if it is, we need to send it twice + thisport->pfSerialWrite(ESC); + } else { + thisport->pfSerialWrite(c); // otherwise write the byte to serial port + } } /************************************************************************************************************ -* -* NAME: uint16_t ssp_crc16( uint16_t crc, uint16_t data ) -* DESCRIPTION: Uses crc_table to calculate new crc -* ARGUMENTS: -* arg1: crc -* arg2: data - byte to calculate into CRC -* RETURN: New crc -* CREATED: 5/8/02 -* -*************************************************************************************************************/ + * + * NAME: uint16_t ssp_crc16( uint16_t crc, uint16_t data ) + * DESCRIPTION: Uses crc_table to calculate new crc + * ARGUMENTS: + * arg1: crc + * arg2: data - byte to calculate into CRC + * RETURN: New crc + * CREATED: 5/8/02 + * + *************************************************************************************************************/ /*! * \brief calculates the new CRC value for 'data' * \param crc = current CRC value @@ -566,9 +564,9 @@ static void sf_write_byte( Port_t *thisport, uint8_t c ) * */ -static uint16_t sf_crc16( uint16_t crc, uint8_t data ) +static uint16_t sf_crc16(uint16_t crc, uint8_t data) { - return (crc >> 8) ^ CRC_TABLE[( crc ^ data ) & 0x00FF ]; + return (crc >> 8) ^ CRC_TABLE[(crc ^ data) & 0x00FF]; } @@ -581,11 +579,12 @@ static uint16_t sf_crc16( uint16_t crc, uint8_t data ) * */ -static void sf_SetSendTimeout( Port_t *thisport ) +static void sf_SetSendTimeout(Port_t *thisport) { - uint32_t timeout; + uint32_t timeout; + timeout = thisport->pfGetTime() + thisport->timeoutLen; - thisport->timeout = timeout; + thisport->timeout = timeout; } /*! @@ -597,13 +596,13 @@ static void sf_SetSendTimeout( Port_t *thisport ) * \note * */ -static uint16_t sf_CheckTimeout( Port_t *thisport ) +static uint16_t sf_CheckTimeout(Port_t *thisport) { - uint16_t retval = FALSE; - uint32_t current_time; + uint16_t retval = FALSE; + uint32_t current_time; current_time = thisport->pfGetTime(); - if( current_time > thisport->timeout ) { + if (current_time > thisport->timeout) { retval = TRUE; } return retval; @@ -611,15 +610,15 @@ static uint16_t sf_CheckTimeout( Port_t *thisport ) /**************************************************************************** - * NAME: sf_ReceiveState - * DESC: Implements the receive state handling code for escaped and unescaped data - * ARGS: thisport - which port to operate on - * c - incoming byte - * RETURN: - * CREATED: - * NOTES: - * 1. change from using pointer to functions. - ****************************************************************************/ +* NAME: sf_ReceiveState +* DESC: Implements the receive state handling code for escaped and unescaped data +* ARGS: thisport - which port to operate on +* c - incoming byte +* RETURN: +* CREATED: +* NOTES: +* 1. change from using pointer to functions. +****************************************************************************/ /*! * \brief implements the receive state handling code for escaped and unescaped data * \param thisport = which port to use @@ -629,46 +628,46 @@ static uint16_t sf_CheckTimeout( Port_t *thisport ) * \note * */ -static int16_t sf_ReceiveState( Port_t *thisport, uint8_t c ) +static int16_t sf_ReceiveState(Port_t *thisport, uint8_t c) { - int16_t retval = SSP_RX_RECEIVING; + int16_t retval = SSP_RX_RECEIVING; - switch( thisport->InputState ) { - case state_unescaped_e: - if( c == SYNC ) { - thisport->DecodeState = decode_len1_e; - } else if ( c == ESC ) { - thisport->InputState = state_escaped_e; - } else { - retval = sf_DecodeState( thisport, c); - } - break; // end of unescaped state. - case state_escaped_e: - thisport->InputState = state_unescaped_e; - if( c == SYNC ) { - thisport->DecodeState = decode_len1_e; - } else if (c == ESC_SYNC ) { - retval = sf_DecodeState( thisport, SYNC); - } else { - retval = sf_DecodeState( thisport, c); - } - break; // end of the escaped state. - default: - break; - } - return retval; + switch (thisport->InputState) { + case state_unescaped_e: + if (c == SYNC) { + thisport->DecodeState = decode_len1_e; + } else if (c == ESC) { + thisport->InputState = state_escaped_e; + } else { + retval = sf_DecodeState(thisport, c); + } + break; // end of unescaped state. + case state_escaped_e: + thisport->InputState = state_unescaped_e; + if (c == SYNC) { + thisport->DecodeState = decode_len1_e; + } else if (c == ESC_SYNC) { + retval = sf_DecodeState(thisport, SYNC); + } else { + retval = sf_DecodeState(thisport, c); + } + break; // end of the escaped state. + default: + break; + } + return retval; } /**************************************************************************** - * NAME: sf_DecodeState - * DESC: Implements the receive state finite state machine - * ARGS: thisport - which port to operate on - * c - incoming byte - * RETURN: - * CREATED: - * NOTES: - * 1. change from using pointer to functions. - ****************************************************************************/ +* NAME: sf_DecodeState +* DESC: Implements the receive state finite state machine +* ARGS: thisport - which port to operate on +* c - incoming byte +* RETURN: +* CREATED: +* NOTES: +* 1. change from using pointer to functions. +****************************************************************************/ /*! * \brief implements the receiving decoding state machine @@ -679,86 +678,87 @@ static int16_t sf_ReceiveState( Port_t *thisport, uint8_t c ) * \note * */ -static int16_t sf_DecodeState( Port_t *thisport, uint8_t c ) +static int16_t sf_DecodeState(Port_t *thisport, uint8_t c) { - int16_t retval; - switch( thisport->DecodeState ) { - case decode_idle_e: + int16_t retval; + + switch (thisport->DecodeState) { + case decode_idle_e: // 'c' is ignored in this state as the only way to leave the idle state is // recognition of the SYNC byte in the sf_ReceiveState function. retval = SSP_RX_IDLE; - break; - case decode_len1_e: - thisport->rxBuf[LENGTH]= c; - thisport->rxBufLen = c; - if( thisport->rxBufLen <= thisport->rxBufSize ) { - thisport->DecodeState = decode_seqNo_e; - retval = SSP_RX_RECEIVING; - } else { - thisport->DecodeState = decode_idle_e; - retval = SSP_RX_IDLE; - } - break; - case decode_seqNo_e: - thisport->rxBuf[SEQNUM] = c; - thisport->crc = 0xffff; - thisport->rxBufLen--; // subtract 1 for the seq. no. - thisport->rxBufPos = 2; + break; + case decode_len1_e: + thisport->rxBuf[LENGTH] = c; + thisport->rxBufLen = c; + if (thisport->rxBufLen <= thisport->rxBufSize) { + thisport->DecodeState = decode_seqNo_e; + retval = SSP_RX_RECEIVING; + } else { + thisport->DecodeState = decode_idle_e; + retval = SSP_RX_IDLE; + } + break; + case decode_seqNo_e: + thisport->rxBuf[SEQNUM] = c; + thisport->crc = 0xffff; + thisport->rxBufLen--; // subtract 1 for the seq. no. + thisport->rxBufPos = 2; - thisport->crc = sf_crc16( thisport->crc, c ); - if( thisport->rxBufLen > 0 ) { - thisport->DecodeState = decode_data_e; - } else { - thisport->DecodeState = decode_crc1_e; - } - retval = SSP_RX_RECEIVING; - break; - case decode_data_e: - thisport->rxBuf[ (thisport->rxBufPos)++] = c; - thisport->crc = sf_crc16( thisport->crc, c ); - if( thisport->rxBufPos == (thisport->rxBufLen+2) ) { + thisport->crc = sf_crc16(thisport->crc, c); + if (thisport->rxBufLen > 0) { + thisport->DecodeState = decode_data_e; + } else { thisport->DecodeState = decode_crc1_e; - } - retval = SSP_RX_RECEIVING; - break; - case decode_crc1_e: - thisport->crc = sf_crc16( thisport->crc, c ); - thisport->DecodeState = decode_crc2_e; - retval = SSP_RX_RECEIVING; - break; - case decode_crc2_e: - thisport->DecodeState = decode_idle_e; + } + retval = SSP_RX_RECEIVING; + break; + case decode_data_e: + thisport->rxBuf[(thisport->rxBufPos)++] = c; + thisport->crc = sf_crc16(thisport->crc, c); + if (thisport->rxBufPos == (thisport->rxBufLen + 2)) { + thisport->DecodeState = decode_crc1_e; + } + retval = SSP_RX_RECEIVING; + break; + case decode_crc1_e: + thisport->crc = sf_crc16(thisport->crc, c); + thisport->DecodeState = decode_crc2_e; + retval = SSP_RX_RECEIVING; + break; + case decode_crc2_e: + thisport->DecodeState = decode_idle_e; // verify the CRC value for the packet - if( sf_crc16( thisport->crc, c) == 0) { - // TODO shouldn't the return value of sf_ReceivePacket() be checked? - sf_ReceivePacket( thisport ); - retval = SSP_RX_COMPLETE; - } else { - thisport->RxError++; - retval = SSP_RX_IDLE; - } - break; - default: - thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet. - retval = SSP_RX_IDLE; - break; - } - return retval; + if (sf_crc16(thisport->crc, c) == 0) { + // TODO shouldn't the return value of sf_ReceivePacket() be checked? + sf_ReceivePacket(thisport); + retval = SSP_RX_COMPLETE; + } else { + thisport->RxError++; + retval = SSP_RX_IDLE; + } + break; + default: + thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet. + retval = SSP_RX_IDLE; + break; + } + return retval; } /************************************************************************************************************ -* -* NAME: int16_t sf_ReceivePacket( ) -* DESCRIPTION: Receive one packet, assumed that data is in rec.buff[] -* ARGUMENTS: -* RETURN: 0 . no new packet was received, could be ack or same packet -* 1 . new packet received -* SSP_PACKET_? -* SSP_PACKET_COMPLETE -* SSP_PACKET_ACK -* CREATED: 5/8/02 -* -*************************************************************************************************************/ + * + * NAME: int16_t sf_ReceivePacket( ) + * DESCRIPTION: Receive one packet, assumed that data is in rec.buff[] + * ARGUMENTS: + * RETURN: 0 . no new packet was received, could be ack or same packet + * 1 . new packet received + * SSP_PACKET_? + * SSP_PACKET_COMPLETE + * SSP_PACKET_ACK + * CREATED: 5/8/02 + * + *************************************************************************************************************/ /*! * \brief receive one packet. calls the callback function if needed. * \param thisport = which port to use @@ -772,46 +772,45 @@ static int16_t sf_DecodeState( Port_t *thisport, uint8_t c ) static int16_t sf_ReceivePacket(Port_t *thisport) { - int16_t value = FALSE; + int16_t value = FALSE; - if( ISBITSET(thisport->rxBuf[SEQNUM], ACK_BIT ) ) { - // Received an ACK packet, need to check if it matches the previous sent packet - if( ( thisport->rxBuf[SEQNUM] & 0x7F) == (thisport->txSeqNo & 0x7f)) { - // It matches the last packet sent by us - SETBIT( thisport->txSeqNo, ACK_BIT ); - thisport->SendState = SSP_ACKED; - value = FALSE; - } + if (ISBITSET(thisport->rxBuf[SEQNUM], ACK_BIT)) { + // Received an ACK packet, need to check if it matches the previous sent packet + if ((thisport->rxBuf[SEQNUM] & 0x7F) == (thisport->txSeqNo & 0x7f)) { + // It matches the last packet sent by us + SETBIT(thisport->txSeqNo, ACK_BIT); + thisport->SendState = SSP_ACKED; + value = FALSE; + } // else ignore the ACK packet - } else { - // Received a 'data' packet, figure out what type of packet we received... - if( thisport->rxBuf[SEQNUM] == 0 ) { - // Synchronize sequence number with host + } else { + // Received a 'data' packet, figure out what type of packet we received... + if (thisport->rxBuf[SEQNUM] == 0) { + // Synchronize sequence number with host #ifdef ACTIVE_SYNCH thisport->sendSynch = TRUE; #endif - sf_SendAckPacket( thisport, thisport->rxBuf[SEQNUM] ); - thisport->rxSeqNo = 0; - value = FALSE; - } else if( thisport->rxBuf[SEQNUM] == thisport->rxSeqNo ) { - // Already seen this packet, just ack it, don't act on the packet. - sf_SendAckPacket( thisport, thisport->rxBuf[SEQNUM] ); - value = FALSE; - } else { - //New Packet - thisport->rxSeqNo = thisport->rxBuf[SEQNUM]; - // Let the application do something with the data/packet. - if( thisport->pfCallBack != NULL ) { - // skip the first two bytes (length and seq. no.) in the buffer. - thisport->pfCallBack( &(thisport->rxBuf[2]), thisport->rxBufLen); - } - // after we send the ACK, it is possible for the host to send a new packet. - // Thus the application needs to copy the data and reset the receive buffer - // inside of thisport->pfCallBack() - sf_SendAckPacket( thisport, thisport->rxBuf[SEQNUM] ); - value = TRUE; - } - } - return value; + sf_SendAckPacket(thisport, thisport->rxBuf[SEQNUM]); + thisport->rxSeqNo = 0; + value = FALSE; + } else if (thisport->rxBuf[SEQNUM] == thisport->rxSeqNo) { + // Already seen this packet, just ack it, don't act on the packet. + sf_SendAckPacket(thisport, thisport->rxBuf[SEQNUM]); + value = FALSE; + } else { + // New Packet + thisport->rxSeqNo = thisport->rxBuf[SEQNUM]; + // Let the application do something with the data/packet. + if (thisport->pfCallBack != NULL) { + // skip the first two bytes (length and seq. no.) in the buffer. + thisport->pfCallBack(&(thisport->rxBuf[2]), thisport->rxBufLen); + } + // after we send the ACK, it is possible for the host to send a new packet. + // Thus the application needs to copy the data and reset the receive buffer + // inside of thisport->pfCallBack() + sf_SendAckPacket(thisport, thisport->rxBuf[SEQNUM]); + value = TRUE; + } + } + return value; } - diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/ssp.h b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/ssp.h index e269043ea..2a2f4bff1 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/ssp.h +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/ssp.h @@ -1,9 +1,9 @@ /******************************************************************* - * - * NAME: ssp.h - * - * - *******************************************************************/ +* +* NAME: ssp.h +* +* +*******************************************************************/ #ifndef SSP_H #define SSP_H /** INCLUDE FILES **/ @@ -11,111 +11,108 @@ /** LOCAL DEFINITIONS **/ #ifndef TRUE -#define TRUE 1 +#define TRUE 1 #endif #ifndef FALSE -#define FALSE 0 +#define FALSE 0 #endif -#define SSP_TX_IDLE 0 // not expecting a ACK packet (no current transmissions in progress) -#define SSP_TX_WAITING 1 // waiting for a valid ACK to arrive -#define SSP_TX_TIMEOUT 2 // failed to receive a valid ACK in the timeout period, after retrying. -#define SSP_TX_ACKED 3 // valid ACK received before timeout period. -#define SSP_TX_BUFOVERRUN 4 // amount of data to send execeds the transmission buffer sizeof -#define SSP_TX_BUSY 5 // Attempted to start a transmission while a transmission was already in progress. -//#define SSP_TX_FAIL - failure... +#define SSP_TX_IDLE 0 // not expecting a ACK packet (no current transmissions in progress) +#define SSP_TX_WAITING 1 // waiting for a valid ACK to arrive +#define SSP_TX_TIMEOUT 2 // failed to receive a valid ACK in the timeout period, after retrying. +#define SSP_TX_ACKED 3 // valid ACK received before timeout period. +#define SSP_TX_BUFOVERRUN 4 // amount of data to send execeds the transmission buffer sizeof +#define SSP_TX_BUSY 5 // Attempted to start a transmission while a transmission was already in progress. +// #define SSP_TX_FAIL - failure... -#define SSP_RX_IDLE 0 -#define SSP_RX_RECEIVING 1 -#define SSP_RX_COMPLETE 2 +#define SSP_RX_IDLE 0 +#define SSP_RX_RECEIVING 1 +#define SSP_RX_COMPLETE 2 // types of packet that can be received -#define SSP_RX_DATA 5 -#define SSP_RX_ACK 6 -#define SSP_RX_SYNCH 7 +#define SSP_RX_DATA 5 +#define SSP_RX_ACK 6 +#define SSP_RX_SYNCH 7 typedef enum decodeState_ { - decode_len1_e = 0, - decode_seqNo_e, - decode_data_e, - decode_crc1_e, - decode_crc2_e, - decode_idle_e + decode_len1_e = 0, + decode_seqNo_e, + decode_data_e, + decode_crc1_e, + decode_crc2_e, + decode_idle_e } DecodeState_t; typedef enum ReceiveState { - state_escaped_e = 0, - state_unescaped_e + state_escaped_e = 0, + state_unescaped_e } ReceiveState_t; -typedef struct -{ - uint8_t *pbuff; - uint16_t length; - uint16_t crc; - uint8_t seqNo; +typedef struct { + uint8_t *pbuff; + uint16_t length; + uint16_t crc; + uint8_t seqNo; } Packet_t; typedef struct { - - uint8_t *rxBuf; // Buffer used to store rcv data - uint16_t rxBufSize; // rcv buffer size. - uint8_t *txBuf; // Length of data in buffer - uint16_t txBufSize; // CRC for data in Packet buff - uint16_t max_retry; // Maximum number of retrys for a single transmit. - int32_t timeoutLen; // how long to wait for each retry to succeed - void (*pfCallBack)( uint8_t *, uint16_t); // call back function that is called when a full packet has been received - int16_t (*pfSerialRead)(void); // function to call to read a byte from serial hardware - void (*pfSerialWrite)( uint8_t ); // function used to write a byte to serial hardware for transmission - uint32_t (*pfGetTime)(void); // function returns time in number of seconds that has elapsed from a given reference point -} PortConfig_t; + uint8_t *rxBuf; // Buffer used to store rcv data + uint16_t rxBufSize; // rcv buffer size. + uint8_t *txBuf; // Length of data in buffer + uint16_t txBufSize; // CRC for data in Packet buff + uint16_t max_retry; // Maximum number of retrys for a single transmit. + int32_t timeoutLen; // how long to wait for each retry to succeed + void (*pfCallBack)(uint8_t *, uint16_t); // call back function that is called when a full packet has been received + int16_t (*pfSerialRead)(void); // function to call to read a byte from serial hardware + void (*pfSerialWrite)(uint8_t); // function used to write a byte to serial hardware for transmission + uint32_t (*pfGetTime)(void); // function returns time in number of seconds that has elapsed from a given reference point +} PortConfig_t; typedef struct Port_tag { - void (*pfCallBack)( uint8_t *, uint16_t); // call back function that is called when a full packet has been received - int16_t (*pfSerialRead)(void); // function to read a character from the serial input stream - void (*pfSerialWrite)( uint8_t ); // function to write a byte to be sent out the serial port - uint32_t (*pfGetTime)(void); // function returns time in number of seconds that has elapsed from a given reference point - uint8_t retryCount; // how many times have we tried to transmit the 'send' packet - uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet - int32_t timeoutLen; // how long to wait for each retry to succeed - int32_t timeout; // current timeout. when 'time' reaches this point we have timed out - uint8_t txSeqNo; // current 'send' packet sequence number - uint16_t rxBufPos; // current buffer position in the receive packet - uint16_t rxBufLen; // number of 'data' bytes in the buffer - uint8_t rxSeqNo; // current 'receive' packet number - uint16_t rxBufSize; // size of the receive buffer. - uint16_t txBufSize; // size of the transmit buffer. - uint8_t *txBuf; // transmit buffer. REquired to store a copy of packet data in case a retry is needed. - uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received. - uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host - // this is required when switching from the application to the bootloader - // and vice-versa. This fixes the firwmare download timeout. - // when this flag is set to true, the next time we send a packet we will first - // send a synchronize packet. - ReceiveState_t InputState; - DecodeState_t DecodeState; - uint16_t SendState; - uint16_t crc; - uint32_t RxError; - uint32_t TxError; - uint16_t flags; + void (*pfCallBack)(uint8_t *, uint16_t); // call back function that is called when a full packet has been received + int16_t (*pfSerialRead)(void); // function to read a character from the serial input stream + void (*pfSerialWrite)(uint8_t); // function to write a byte to be sent out the serial port + uint32_t (*pfGetTime)(void); // function returns time in number of seconds that has elapsed from a given reference point + uint8_t retryCount; // how many times have we tried to transmit the 'send' packet + uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet + int32_t timeoutLen; // how long to wait for each retry to succeed + int32_t timeout; // current timeout. when 'time' reaches this point we have timed out + uint8_t txSeqNo; // current 'send' packet sequence number + uint16_t rxBufPos; // current buffer position in the receive packet + uint16_t rxBufLen; // number of 'data' bytes in the buffer + uint8_t rxSeqNo; // current 'receive' packet number + uint16_t rxBufSize; // size of the receive buffer. + uint16_t txBufSize; // size of the transmit buffer. + uint8_t *txBuf; // transmit buffer. REquired to store a copy of packet data in case a retry is needed. + uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received. + uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host + // this is required when switching from the application to the bootloader + // and vice-versa. This fixes the firwmare download timeout. + // when this flag is set to true, the next time we send a packet we will first + // send a synchronize packet. + ReceiveState_t InputState; + DecodeState_t DecodeState; + uint16_t SendState; + uint16_t crc; + uint32_t RxError; + uint32_t TxError; + uint16_t flags; } Port_t; - /** Public Data **/ /** PUBLIC FUNCTIONS **/ -int16_t ssp_ReceiveProcess( Port_t *thisport ); -int16_t ssp_SendProcess( Port_t *thisport ); -uint16_t ssp_SendString( Port_t *thisport, char *str ); -int16_t ssp_SendData(Port_t *thisport, const uint8_t * data,const uint16_t length ); -void ssp_Init( Port_t *thisport, const PortConfig_t* const info); -int16_t ssp_ReceiveByte(Port_t *thisport ); -uint16_t ssp_Synchronise( Port_t *thisport ); +int16_t ssp_ReceiveProcess(Port_t *thisport); +int16_t ssp_SendProcess(Port_t *thisport); +uint16_t ssp_SendString(Port_t *thisport, char *str); +int16_t ssp_SendData(Port_t *thisport, const uint8_t *data, const uint16_t length); +void ssp_Init(Port_t *thisport, const PortConfig_t *const info); +int16_t ssp_ReceiveByte(Port_t *thisport); +uint16_t ssp_Synchronise(Port_t *thisport); /** EXTERNAL FUNCTIONS **/ -#endif +#endif // ifndef SSP_H diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.cpp b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.cpp index 3ef75e8ee..2d6335dfe 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.cpp +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.cpp @@ -1,3 +1 @@ - #include "delay.h" - diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.h b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.h index 7119a770d..6862e7a9d 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.h +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.h @@ -2,8 +2,7 @@ #define DELAY_H #include -class delay : public QThread -{ +class delay : public QThread { public: static void msleep(unsigned long msecs) { diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/main.cpp b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/main.cpp index c9c687986..e5c98386b 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/main.cpp +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/main.cpp @@ -11,314 +11,243 @@ int main(int argc, char *argv[]) ///SSP///////////////////////////////// - ///////////////////////////////////////////// QCoreApplication a(argc, argv); -// argc=4; -// argv[1]="-ls"; -// argv[2]="-t"; -// argv[3]="COM3"; - if(argc>1||!PRIVATE) - { - bool use_serial=false; + +// argc=4; +// argv[1]="-ls"; +// argv[2]="-t"; +// argv[3]="COM3"; + if (argc > 1 || !PRIVATE) { + bool use_serial = false; bool verify; - bool debug=false; - bool umodereset=false; + bool debug = false; + bool umodereset = false; OP_DFU::Actions action; QString file; QString serialport; QString description; - int device=-1; + int device = -1; QStringList args; - for(int i=0;i : program hw (requires:-d - optionals:-v,-w) |\n"; - cout<<"| -v : verify (requires:-d) |\n"; - cout<<"| -dn : download firmware to file (requires:-d) |\n"; - // cout<<"| -dd : download discription (requires:-d) |\n"; - cout<<"| -d : (requires: -p) |\n"; - cout<<"| -ca : compares byte by byte current firmware with file|\n"; - cout<<"| -cc : compares CRC of current firmware with file |\n"; - cout<<"| -s : requests status of device |\n"; - cout<<"| -r : resets the device |\n"; - cout<<"| -j : exits bootloader and jumps to user FW |\n"; - cout<<"| -debug : prints debug information |\n"; - cout<<"| -t : uses serial port(requires:-ur) |\n"; - cout<<"| -ur : user mode reset* |\n"; - cout<<"| |\n"; - cout<<"| examples: |\n"; - cout<<"| |\n"; - cout<<"| program and verify device #0 |\n"; - cout<<"| OPUploadTool -p c:/OpenPilot.bin -w \"Openpilot Firmware\" -v -d 0 |\n"; - cout<<"| |\n"; - cout<<"| Perform a quick compare of FW in file with FW in device #1 |\n"; - cout<<"| OPUploadTool -ch c:/OpenPilot2.bin -d 2 |\n"; - cout<<"| |\n"; - cout<<"| *requires valid user space firmwares already running |\n"; - cout<<"|________________________________________________________________________|\n"; + if (args.contains("-debug")) { + debug = true; + } + if (args.contains("-ur")) { + umodereset = true; + } + if (args.contains("-?") || (!PRIVATE && argc == 1)) { + cout << "_________________________________________________________________________\n"; + cout << "| Commands |\n"; + cout << "| |\n"; + cout << "| -ls : lists available devices |\n"; + cout << "| -p : program hw (requires:-d - optionals:-v,-w) |\n"; + cout << "| -v : verify (requires:-d) |\n"; + cout << "| -dn : download firmware to file (requires:-d) |\n"; + // cout<<"| -dd : download discription (requires:-d) |\n"; + cout << "| -d : (requires: -p) |\n"; + cout << "| -ca : compares byte by byte current firmware with file|\n"; + cout << "| -cc : compares CRC of current firmware with file |\n"; + cout << "| -s : requests status of device |\n"; + cout << "| -r : resets the device |\n"; + cout << "| -j : exits bootloader and jumps to user FW |\n"; + cout << "| -debug : prints debug information |\n"; + cout << "| -t : uses serial port(requires:-ur) |\n"; + cout << "| -ur : user mode reset* |\n"; + cout << "| |\n"; + cout << "| examples: |\n"; + cout << "| |\n"; + cout << "| program and verify device #0 |\n"; + cout << "| OPUploadTool -p c:/OpenPilot.bin -w \"Openpilot Firmware\" -v -d 0 |\n"; + cout << "| |\n"; + cout << "| Perform a quick compare of FW in file with FW in device #1 |\n"; + cout << "| OPUploadTool -ch c:/OpenPilot2.bin -d 2 |\n"; + cout << "| |\n"; + cout << "| *requires valid user space firmwares already running |\n"; + cout << "|________________________________________________________________________|\n"; return 0; - - - } - - else if(args.contains(PROGRAMFW)) - { - if(args.contains(VERIFY)) - { - verify=true; + } else if (args.contains(PROGRAMFW)) { + if (args.contains(VERIFY)) { + verify = true; + } else { + verify = false; } - else - verify=false; - if(args.contains(PROGRAMDESC)) - { - if(args.indexOf(PROGRAMDESC)+1dfu.numberOfDevices-1) - { - cout<<"Error:Invalid Device"; + if (device > dfu.numberOfDevices - 1) { + cout << "Error:Invalid Device"; return -1; } -// if(dfu.numberOfDevices==1) -// dfu.use_delay=false; - if(!dfu.enterDFU(device)) - { - cout<<"Error:Could not enter DFU mode\n"; +// if(dfu.numberOfDevices==1) +// dfu.use_delay=false; + if (!dfu.enterDFU(device)) { + cout << "Error:Could not enter DFU mode\n"; return -1; } - if (action==OP_DFU::actionProgram) - { - if(((OP_DFU::device)dfu.devices[device]).Writable==false) - { - cout<<"ERROR device not Writable\n"; + if (action == OP_DFU::actionProgram) { + if (((OP_DFU::device)dfu.devices[device]).Writable == false) { + cout << "ERROR device not Writable\n"; return false; } - OP_DFU::Status retstatus=dfu.UploadFirmware(file.toAscii(),verify, device); - if(retstatus!=OP_DFU::Last_operation_Success) - { - cout<<"Upload failed with code:"< #include -OP_DFU::OP_DFU(bool _debug,bool _use_serial,QString portname,bool umodereset): debug(_debug),use_serial(_use_serial),mready(true) +OP_DFU::OP_DFU(bool _debug, bool _use_serial, QString portname, bool umodereset) : debug(_debug), use_serial(_use_serial), mready(true) { - if(use_serial) - { - cout<<"Connect hw now and press any key"; - // getch(); + if (use_serial) { + cout << "Connect hw now and press any key"; + // getch(); // delay::msleep(2000); PortSettings settings; - settings.BaudRate=BAUD57600; - settings.DataBits=DATA_8; - settings.FlowControl=FLOW_OFF; - settings.Parity=PAR_NONE; - settings.StopBits=STOP_1; - settings.Timeout_Millisec=1000; - info=new port(settings,portname); - info->rxBuf = sspRxBuf; - info->rxBufSize = MAX_PACKET_DATA_LEN; - info->txBuf = sspTxBuf; - info->txBufSize = MAX_PACKET_DATA_LEN; - info->max_retry = 10; - info->timeoutLen = 1000; - if(info->status()!=port::open) - { - cout<<"Could not open serial port\n"; - mready=false; + settings.BaudRate = BAUD57600; + settings.DataBits = DATA_8; + settings.FlowControl = FLOW_OFF; + settings.Parity = PAR_NONE; + settings.StopBits = STOP_1; + settings.Timeout_Millisec = 1000; + info = new port(settings, portname); + info->rxBuf = sspRxBuf; + info->rxBufSize = MAX_PACKET_DATA_LEN; + info->txBuf = sspTxBuf; + info->txBufSize = MAX_PACKET_DATA_LEN; + info->max_retry = 10; + info->timeoutLen = 1000; + if (info->status() != port::open) { + cout << "Could not open serial port\n"; + mready = false; return; } - if(umodereset) + if (umodereset) { sendReset(); - delay::msleep(5000); - serialhandle=new qsspt(info,debug); - - while(serialhandle->ssp_Synchronise()==false) - { - if (debug) - qDebug()<<"SYNC failed, resending"; } - qDebug()<<"SYNC Succeded"; - serialhandle->start(); - // serialhandle->start(); - } - else - { + delay::msleep(5000); + serialhandle = new qsspt(info, debug); - send_delay=10; - use_delay=true; - int numDevices=0; - cout<<"Please connect device now\n"; - int count=0; - while(numDevices==0) - { - cout<<"."; - delay::msleep(500); - numDevices = hidHandle.open(1,0x20a0,0x415A,0,0); //0xff9c,0x0001); - if(numDevices==0) - numDevices = hidHandle.open(1,0x20a0,0x415B,0,0); //0xff9c,0x0001); - if(++count==10) - { - cout<<"\r"; - cout<<" "; - cout<<"\r"; - count=0; + while (serialhandle->ssp_Synchronise() == false) { + if (debug) { + qDebug() << "SYNC failed, resending"; } } - if(debug) + qDebug() << "SYNC Succeded"; + serialhandle->start(); + // serialhandle->start(); + } else { + send_delay = 10; + use_delay = true; + int numDevices = 0; + cout << "Please connect device now\n"; + int count = 0; + while (numDevices == 0) { + cout << "."; + delay::msleep(500); + numDevices = hidHandle.open(1, 0x20a0, 0x415A, 0, 0); // 0xff9c,0x0001); + if (numDevices == 0) { + numDevices = hidHandle.open(1, 0x20a0, 0x415B, 0, 0); // 0xff9c,0x0001); + } + if (++count == 10) { + cout << "\r"; + cout << " "; + cout << "\r"; + count = 0; + } + } + if (debug) { qDebug() << numDevices << " device(s) opened"; - if(umodereset) - { + } + if (umodereset) { sendReset(); - qDebug()<<"before delay"; + qDebug() << "before delay"; delay::msleep(5000); - qDebug()<<"after delay"; - if(hidHandle.open(1,0x20a0,0x4117,0,0)==0) - mready=false; + qDebug() << "after delay"; + if (hidHandle.open(1, 0x20a0, 0x4117, 0, 0) == 0) { + mready = false; + } } } } void OP_DFU::sendReset(void) { - qDebug()<<"Requesting user mode reset"; - char aa[255]= {0x02,0x3E,0x3C,0x20,0x75,0x00,0x20,0x4F,0x67,0x34,0x62,0x04,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};//63 - char ba[255]= {0x02,0x3E,0x3C,0x20,0x75,0x00,0x20,0x4F,0x67,0x34,0xB9,0x08,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; - char ca[255]= {0x02,0x3E,0x3C,0x20,0x75,0x00,0x20,0x4F,0x67,0x34,0x10,0x0D,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; - char bb[255]={0x02,0x3D,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x28,0x8D,0x19,0x00,0x00,0x13}; - char ab[255]={0x02,0x3D,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x47,0x1B,0x19,0x00,0x00,0x76}; - char cb[255]={0x02,0x3D,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x66,0x78,0x1C,0x00,0x00,0x25}; - //125 - if(!use_serial) - { - hidHandle.send(0,aa, 64, 5000); - hidHandle.send(0,ab, 64, 5000); - delay::msleep(600); - hidHandle.send(0,ba, 64, 5000); - hidHandle.send(0,bb, 64, 5000); - delay::msleep(600); - hidHandle.send(0,ca, 64, 5000); - hidHandle.send(0,cb, 64, 5000); - delay::msleep(100); - hidHandle.close(1); -} - else - { + qDebug() << "Requesting user mode reset"; + char aa[255] = { 0x02, 0x3E, 0x3C, 0x20, 0x75, 0x00, 0x20, 0x4F, 0x67, 0x34, 0x62, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; // 63 + char ba[255] = { 0x02, 0x3E, 0x3C, 0x20, 0x75, 0x00, 0x20, 0x4F, 0x67, 0x34, 0xB9, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + char ca[255] = { 0x02, 0x3E, 0x3C, 0x20, 0x75, 0x00, 0x20, 0x4F, 0x67, 0x34, 0x10, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + char bb[255] = { 0x02, 0x3D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x28, 0x8D, 0x19, 0x00, 0x00, 0x13 }; + char ab[255] = { 0x02, 0x3D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x1B, 0x19, 0x00, 0x00, 0x76 }; + char cb[255] = { 0x02, 0x3D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x66, 0x78, 0x1C, 0x00, 0x00, 0x25 }; + // 125 + if (!use_serial) { + hidHandle.send(0, aa, 64, 5000); + hidHandle.send(0, ab, 64, 5000); + delay::msleep(600); + hidHandle.send(0, ba, 64, 5000); + hidHandle.send(0, bb, 64, 5000); + delay::msleep(600); + hidHandle.send(0, ca, 64, 5000); + hidHandle.send(0, cb, 64, 5000); + delay::msleep(100); + hidHandle.close(1); + } else { char a[255]; char b[255]; char c[255]; - memcpy (a,aa+2,62); - memcpy (a+62,ab+2,60); - memcpy (b,ba+2,62); - memcpy (b+62,bb+2,60); - memcpy (c,ca+2,62); - memcpy (c+62,cb+2,60); - for(int x=0;x<123;++x) + memcpy(a, aa + 2, 62); + memcpy(a + 62, ab + 2, 60); + memcpy(b, ba + 2, 62); + memcpy(b + 62, bb + 2, 60); + memcpy(c, ca + 2, 62); + memcpy(c + 62, cb + 2, 60); + for (int x = 0; x < 123; ++x) { info->pfSerialWrite(a[x]); + } delay::msleep(600); - for(int x=0;x<123;++x) + for (int x = 0; x < 123; ++x) { info->pfSerialWrite(b[x]); + } delay::msleep(600); - for(int x=0;x<123;++x) + for (int x = 0; x < 123; ++x) { info->pfSerialWrite(c[x]); + } } } bool OP_DFU::SaveByteArrayToFile(QString const & sfile, const QByteArray &array) { QFile file(sfile); - //QFile file("in.txt"); - if (!file.open(QIODevice::WriteOnly)) - { - if(debug) - qDebug()<<"Cant open file"; + + // QFile file("in.txt"); + if (!file.open(QIODevice::WriteOnly)) { + if (debug) { + qDebug() << "Cant open file"; + } return false; } file.write(array); @@ -144,246 +141,248 @@ bool OP_DFU::SaveByteArrayToFile(QString const & sfile, const QByteArray &array) bool OP_DFU::enterDFU(int const &devNumber) { char buf[BUF_LEN]; - buf[0] =0x02;//reportID - buf[1] = OP_DFU::EnterDFU;//DFU Command - buf[2] = 0;//DFU Count - buf[3] = 0;//DFU Count - buf[4] = 0;//DFU Count - buf[5] = 0;//DFU Count - buf[6] = devNumber;//DFU Data0 - buf[7] = 1;//DFU Data1 - buf[8] = 1;//DFU Data2 - buf[9] = 1;//DFU Data3 + + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::EnterDFU; // DFU Command + buf[2] = 0; // DFU Count + buf[3] = 0; // DFU Count + buf[4] = 0; // DFU Count + buf[5] = 0; // DFU Count + buf[6] = devNumber; // DFU Data0 + buf[7] = 1; // DFU Data1 + buf[8] = 1; // DFU Data2 + buf[9] = 1; // DFU Data3 int result = sendData(buf, BUF_LEN); - if(result<1) + if (result < 1) { return false; - if(debug) + } + if (debug) { qDebug() << result << " bytes sent"; + } return true; } -bool OP_DFU::StartUpload(qint32 const & numberOfBytes, TransferTypes const & type,quint32 crc) +bool OP_DFU::StartUpload(qint32 const & numberOfBytes, TransferTypes const & type, quint32 crc) { int lastPacketCount; - qint32 numberOfPackets=numberOfBytes/4/14; - int pad=(numberOfBytes-numberOfPackets*4*14)/4; - if(pad==0) - { - lastPacketCount=14; - } - else - { + qint32 numberOfPackets = numberOfBytes / 4 / 14; + int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4; + + if (pad == 0) { + lastPacketCount = 14; + } else { ++numberOfPackets; - lastPacketCount=pad; + lastPacketCount = pad; } char buf[BUF_LEN]; - buf[0] =0x02;//reportID - buf[1] = setStartBit(OP_DFU::Upload);//DFU Command - buf[2] = numberOfPackets>>24;//DFU Count - buf[3] = numberOfPackets>>16;//DFU Count - buf[4] = numberOfPackets>>8;//DFU Count - buf[5] = numberOfPackets;//DFU Count - buf[6] = (int)type;//DFU Data0 - buf[7] = lastPacketCount;//DFU Data1 - buf[8] = crc>>24; - buf[9] = crc>>16; - buf[10] = crc>>8; + buf[0] = 0x02; // reportID + buf[1] = setStartBit(OP_DFU::Upload); // DFU Command + buf[2] = numberOfPackets >> 24; // DFU Count + buf[3] = numberOfPackets >> 16; // DFU Count + buf[4] = numberOfPackets >> 8; // DFU Count + buf[5] = numberOfPackets; // DFU Count + buf[6] = (int)type; // DFU Data0 + buf[7] = lastPacketCount; // DFU Data1 + buf[8] = crc >> 24; + buf[9] = crc >> 16; + buf[10] = crc >> 8; buf[11] = crc; - if(debug) - qDebug()<<"Number of packets:"<0) - { + } + if (result > 0) { return true; } return false; } -bool OP_DFU::UploadData(qint32 const & numberOfBytes, QByteArray & data) +bool OP_DFU::UploadData(qint32 const & numberOfBytes, QByteArray & data) { int lastPacketCount; - qint32 numberOfPackets=numberOfBytes/4/14; - int pad=(numberOfBytes-numberOfPackets*4*14)/4; - if(pad==0) - { - lastPacketCount=14; - } - else - { + qint32 numberOfPackets = numberOfBytes / 4 / 14; + int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4; + + if (pad == 0) { + lastPacketCount = 14; + } else { ++numberOfPackets; - lastPacketCount=pad; + lastPacketCount = pad; + } + if (debug) { + qDebug() << "Start Uploading:" << numberOfPackets << "4Bytes"; } - if(debug) - qDebug()<<"Start Uploading:"<>24;//DFU Count - buf[3] = packetcount>>16;//DFU Count - buf[4] = packetcount>>8;//DFU Count - buf[5] = packetcount;//DFU Count - char *pointer=data.data(); - pointer=pointer+4*14*packetcount; - // qDebug()<<"Packet Number="<>24;//DFU Count - buf[3] = numberOfPackets>>16;//DFU Count - buf[4] = numberOfPackets>>8;//DFU Count - buf[5] = numberOfPackets;//DFU Count - buf[6] = (int)type;//DFU Data0 - buf[7] = lastPacketCount;//DFU Data1 - buf[8] = 1;//DFU Data2 - buf[9] = 1;//DFU Data3 + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::Download_Req; // DFU Command + buf[2] = numberOfPackets >> 24; // DFU Count + buf[3] = numberOfPackets >> 16; // DFU Count + buf[4] = numberOfPackets >> 8; // DFU Count + buf[5] = numberOfPackets; // DFU Count + buf[6] = (int)type; // DFU Data0 + buf[7] = lastPacketCount; // DFU Data1 + buf[8] = 1; // DFU Data2 + buf[9] = 1; // DFU Data3 int result = sendData(buf, BUF_LEN); - if(debug) - qDebug() << "StartDownload:"<>(x*2) & 1); - dev.Writable=(bool)(RWFlags>>(x*2+1) & 1); + dev.Readable = (bool)(RWFlags >> (x * 2) & 1); + dev.Writable = (bool)(RWFlags >> (x * 2 + 1) & 1); devices.append(dev); - buf[0] =0x02;//reportID - buf[1] = OP_DFU::Req_Capabilities;//DFU Command + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::Req_Capabilities; // DFU Command buf[2] = 0; buf[3] = 0; buf[4] = 0; buf[5] = 0; - buf[6] = x+1; + buf[6] = x + 1; buf[7] = 0; buf[8] = 0; buf[9] = 0; int result = sendData(buf, BUF_LEN); - result = receiveData(buf,BUF_LEN); - // devices[x].ID=buf[9]; - devices[x].ID=buf[14]; - devices[x].ID=devices[x].ID<<8 | (quint8)buf[15]; - devices[x].BL_Version=buf[7]; - devices[x].SizeOfDesc=buf[8]; + result = receiveData(buf, BUF_LEN); + // devices[x].ID=buf[9]; + devices[x].ID = buf[14]; + devices[x].ID = devices[x].ID << 8 | (quint8)buf[15]; + devices[x].BL_Version = buf[7]; + devices[x].SizeOfDesc = buf[8]; quint32 aux; - aux=(quint8)buf[10]; - aux=aux<<8 |(quint8)buf[11]; - aux=aux<<8 |(quint8)buf[12]; - aux=aux<<8 |(quint8)buf[13]; + aux = (quint8)buf[10]; + aux = aux << 8 | (quint8)buf[11]; + aux = aux << 8 | (quint8)buf[12]; + aux = aux << 8 | (quint8)buf[13]; - devices[x].FW_CRC=aux; + devices[x].FW_CRC = aux; - aux=(quint8)buf[2]; - aux=aux<<8 |(quint8)buf[3]; - aux=aux<<8 |(quint8)buf[4]; - aux=aux<<8 |(quint8)buf[5]; - devices[x].SizeOfCode=aux; + aux = (quint8)buf[2]; + aux = aux << 8 | (quint8)buf[3]; + aux = aux << 8 | (quint8)buf[4]; + aux = aux << 8 | (quint8)buf[5]; + devices[x].SizeOfCode = aux; } - if(debug) - { - qDebug()<<"Found "<0) + } + if (result > 0) { return true; + } return false; } -OP_DFU::Status OP_DFU::UploadFirmware(const QString &sfile, const bool &verify,int device) +OP_DFU::Status OP_DFU::UploadFirmware(const QString &sfile, const bool &verify, int device) { OP_DFU::Status ret; - cout<<"Starting Firmware Uploading...\n"; + + cout << "Starting Firmware Uploading...\n"; QFile file(sfile); - //QFile file("in.txt"); - if (!file.open(QIODevice::ReadOnly)) - { - if(debug) - qDebug()<<"Cant open file"; + // QFile file("in.txt"); + if (!file.open(QIODevice::ReadOnly)) { + if (debug) { + qDebug() << "Cant open file"; + } return OP_DFU::abort; } - QByteArray arr=file.readAll(); + QByteArray arr = file.readAll(); - if(debug) - qDebug()<<"Bytes Loaded="<"); - }else{ - bar.replace(i,1," "); + for (int i = 0; i < 50; i++) { + if (i < (percent / 2)) { + bar.replace(i, 1, "="); + } else if (i == (percent / 2)) { + bar.replace(i, 1, ">"); + } else { + bar.replace(i, 1, " "); } } - std::cout<< "\r"<> 2; // /4 Size passed in as a byte count, assumed to be a multiple of 4 + // Size = Size >> 2; // /4 Size passed in as a byte count, assumed to be a multiple of 4 - while(Size--) - { + while (Size--) { static const quint32 CrcTable[16] = { // Nibble lookup table for 0x04C11DB7 polynomial - 0x00000000,0x04C11DB7,0x09823B6E,0x0D4326D9,0x130476DC,0x17C56B6B,0x1A864DB2,0x1E475005, - 0x2608EDB8,0x22C9F00F,0x2F8AD6D6,0x2B4BCB61,0x350C9B64,0x31CD86D3,0x3C8EA00A,0x384FBDBD }; + 0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005, + 0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61, 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD + }; - Crc = Crc ^ *((quint32 *)Buffer); // Apply all 32-bits + Crc = Crc ^ *((quint32 *)Buffer); // Apply all 32-bits Buffer += 1; // Process 32-bits, 4 at a time, or 8 rounds Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // Assumes 32-bit reg, masking index to 4-bits - Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // 0x04C11DB7 Polynomial used in STM32 + Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // 0x04C11DB7 Polynomial used in STM32 Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; @@ -805,62 +795,56 @@ quint32 OP_DFU::CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer) Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; } - return(Crc); + return Crc; } quint32 OP_DFU::CRCFromQBArray(QByteArray array, quint32 Size) { - int pad=Size-array.length(); - array.append(QByteArray(pad,255)); - quint32 t[Size/4]; - for(int x=0;xsendData((uint8_t*)data+1,size-1)) - { - if (debug) - qDebug()<<"packet sent"<<"data0"<<((uint8_t*)data+1)[0]; + if (!use_serial) { + return hidHandle.send(0, data, size, 5000); + } else { + if (serialhandle->sendData((uint8_t *)data + 1, size - 1)) { + if (debug) { + qDebug() << "packet sent" << "data0" << ((uint8_t *)data + 1)[0]; + } return size; } - if(debug) - qDebug()<<"Serial send OVERRUN"; + if (debug) { + qDebug() << "Serial send OVERRUN"; + } } } -int OP_DFU::receiveData(void * data,int size) +int OP_DFU::receiveData(void *data, int size) { - if(!use_serial) - { - return hidHandle.receive(0,data, size, 10000); - } - else - { + if (!use_serial) { + return hidHandle.receive(0, data, size, 10000); + } else { int x; QTime time; time.start(); - while(true) - { - if(x=serialhandle->read_Packet(((char *) data)+1)!=-1||time.elapsed()>10000) - { - if(time.elapsed()>10000) - qDebug()<<"____timeout"; + while (true) { + if (x = serialhandle->read_Packet(((char *)data) + 1) != -1 || time.elapsed() > 10000) { + if (time.elapsed() > 10000) { + qDebug() << "____timeout"; + } return x; } } diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.h b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.h index f5b82f7e7..e3a0593ec 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.h +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.h @@ -15,61 +15,55 @@ #include "SSP/port.h" #include "SSP/qsspt.h" using namespace std; -#define BUF_LEN 64 +#define BUF_LEN 64 -//Command Line Options -#define DOWNLOAD "-dn" //done -#define DEVICE "-d" //done -//#define DOWNDESCRIPTION "-dd" //done -#define PROGRAMFW "-p" //done -#define PROGRAMDESC "-w" //done -#define VERIFY "-v" //done -#define COMPARECRC "-cc" +// Command Line Options +#define DOWNLOAD "-dn" // done +#define DEVICE "-d" // done +// #define DOWNDESCRIPTION "-dd" //done +#define PROGRAMFW "-p" // done +#define PROGRAMDESC "-w" // done +#define VERIFY "-v" // done +#define COMPARECRC "-cc" #define COMPAREALL "-ca" -#define STATUSREQUEST "-s" //done -#define LISTDEVICES "-ls" //done +#define STATUSREQUEST "-s" // done +#define LISTDEVICES "-ls" // done #define RESET "-r" #define JUMP "-j" #define USE_SERIAL "-t" -#define MAX_PACKET_DATA_LEN 255 -#define MAX_PACKET_BUF_SIZE (1+1+MAX_PACKET_DATA_LEN+2) +#define MAX_PACKET_DATA_LEN 255 +#define MAX_PACKET_BUF_SIZE (1 + 1 + MAX_PACKET_DATA_LEN + 2) -class OP_DFU -{ +class OP_DFU { public: - enum TransferTypes - { + enum TransferTypes { FW, Descript }; - enum CompareType - { + enum CompareType { crccompare, bytetobytecompare }; - enum Status - { - DFUidle,//0 - uploading,//1 - wrong_packet_received,//2 - too_many_packets,//3 - too_few_packets,//4 - Last_operation_Success,//5 - downloading,//6 - idle,//7 - Last_operation_failed,//8 - uploadingStarting,//9 - outsideDevCapabilities,//10 - CRC_Fail,//11 - failed_jump,//12 - abort//13 - + enum Status { + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + idle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, // 12 + abort // 13 }; - enum Actions - { + enum Actions { actionProgram, actionProgramAndVerify, actionDownload, @@ -81,78 +75,80 @@ public: actionJump }; - enum Commands - { - Reserved,//0 - Req_Capabilities,//1 - Rep_Capabilities,//2 - EnterDFU,//3 - JumpFW,//4 - Reset,//5 - Abort_Operation,//6 - Upload,//7 - Op_END,//8 - Download_Req,//9 - Download,//10 - Status_Request,//11 - Status_Rep,//12 - + enum Commands { + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep, // 12 }; - struct device - { - int ID; + struct device { + int ID; quint32 FW_CRC; - int BL_Version; - int SizeOfDesc; + int BL_Version; + int SizeOfDesc; quint32 SizeOfCode; - bool Readable; - bool Writable; + bool Readable; + bool Writable; }; void JumpToApp(); void ResetDevice(void); bool enterDFU(int const &devNumber); - bool StartUpload(qint32 const &numberOfBytes, TransferTypes const & type,quint32 crc); - bool UploadData(qint32 const & numberOfPackets,QByteArray & data); - Status UploadDescription(QString & description); - Status UploadFirmware(const QString &sfile, const bool &verify,int device); + bool StartUpload(qint32 const &numberOfBytes, TransferTypes const & type, quint32 crc); + bool UploadData(qint32 const & numberOfPackets, QByteArray & data); + Status UploadDescription(QString & description); + Status UploadFirmware(const QString &sfile, const bool &verify, int device); Status StatusRequest(); bool EndOperation(); - void printProgBar( int const & percent,QString const& label); + void printProgBar(int const & percent, QString const & label); QString DownloadDescription(int const & numberOfChars); QByteArray StartDownload(qint32 const & numberOfBytes, TransferTypes const & type); - bool SaveByteArrayToFile(QString const & file,QByteArray const &array); - void CopyWords(char * source, char* destination, int count); + bool SaveByteArrayToFile(QString const & file, QByteArray const &array); + void CopyWords(char *source, char *destination, int count); // QByteArray DownloadData(int devNumber,int numberOfPackets); - OP_DFU(bool debug,bool use_serial,QString port,bool umodereset); + OP_DFU(bool debug, bool use_serial, QString port, bool umodereset); void sendReset(void); bool findDevices(); QList devices; int numberOfDevices; - QString StatusToString(OP_DFU::Status const & status); - OP_DFU::Status CompareFirmware(const QString &sfile, const CompareType &type,int device); + QString StatusToString(OP_DFU::Status const & status); + OP_DFU::Status CompareFirmware(const QString &sfile, const CompareType &type, int device); quint32 CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer); quint32 CRCFromQBArray(QByteArray array, quint32 Size); void test(); int send_delay; bool use_delay; - bool ready(){return mready;} + bool ready() + { + return mready; + } void AbortOperation(void); private: bool mready; bool debug; int RWFlags; bool use_serial; - qsspt * serialhandle; - int sendData(void*,int); - int receiveData(void * data,int size); + qsspt *serialhandle; + int sendData(void *, int); + int receiveData(void *data, int size); pjrc_rawhid hidHandle; - int setStartBit(int command){return command|0x20;} - uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; - uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; + int setStartBit(int command) + { + return command | 0x20; + } + uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; + uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; - port * info; + port *info; }; - #endif // OP_DFU_H diff --git a/ground/openpilotgcs/src/experimental/finaltest/main.cpp b/ground/openpilotgcs/src/experimental/finaltest/main.cpp index 6e7efd975..1bd2ce51c 100644 --- a/ground/openpilotgcs/src/experimental/finaltest/main.cpp +++ b/ground/openpilotgcs/src/experimental/finaltest/main.cpp @@ -5,6 +5,7 @@ int main(int argc, char *argv[]) { QApplication a(argc, argv); MainWindow w; + w.show(); return a.exec(); } diff --git a/ground/openpilotgcs/src/experimental/finaltest/mainwindow.cpp b/ground/openpilotgcs/src/experimental/finaltest/mainwindow.cpp index 6cd849d10..039df5ef9 100644 --- a/ground/openpilotgcs/src/experimental/finaltest/mainwindow.cpp +++ b/ground/openpilotgcs/src/experimental/finaltest/mainwindow.cpp @@ -1,28 +1,28 @@ #include "mainwindow.h" #include "ui_mainwindow.h" -//#include "../mapwidget/mapgraphicitem.h" +// #include "../mapwidget/mapgraphicitem.h" MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent), ui(new Ui::MainWindow) { - map=new mapcontrol::OPMapWidget(); + map = new mapcontrol::OPMapWidget(); map->SetShowUAV(true); map->SetShowHome(true); ui->setupUi(this); ui->comboBox->addItems(mapcontrol::Helper::MapTypes()); ui->comboBox->setCurrentIndex(mapcontrol::Helper::MapTypes().indexOf("GoogleHybrid")); - QHBoxLayout *layout=new QHBoxLayout(parent); + QHBoxLayout *layout = new QHBoxLayout(parent); layout->addWidget(map); layout->addWidget(ui->widget); ui->centralWidget->setLayout(layout); - QTimer * timer=new QTimer; + QTimer *timer = new QTimer; timer->setInterval(500); timer->start(); - connect(map,SIGNAL(zoomChanged(double,double,double)),this,SLOT(zoomChanged(double,double,double))); - connect(map,SIGNAL(OnTilesStillToLoad(int)),this,SLOT(ttl(int))); - connect(timer,SIGNAL(timeout()),this,SLOT(time())); - map->WPCreate(internals::PointLatLng(38.42,-9.5),0,"lisbon"); + connect(map, SIGNAL(zoomChanged(double, double, double)), this, SLOT(zoomChanged(double, double, double))); + connect(map, SIGNAL(OnTilesStillToLoad(int)), this, SLOT(ttl(int))); + connect(timer, SIGNAL(timeout()), this, SLOT(time())); + map->WPCreate(internals::PointLatLng(38.42, -9.5), 0, "lisbon"); map->SetMouseWheelZoomType(internals::MouseWheelZoomType::MousePositionAndCenter); } @@ -40,19 +40,19 @@ void MainWindow::ttl(int value) void MainWindow::time() { internals::PointLatLng ll; + ll.SetLat(map->currentMousePosition().Lat()); ll.SetLng(map->currentMousePosition().Lng()); - // map->UAV->SetUAVPos(ll,10); + // map->UAV->SetUAVPos(ll,10); ui->label_2->setText(map->currentMousePosition().ToString()); - QGraphicsItem* itemm=map->itemAt(map->mapFromGlobal(QCursor::pos())); - mapcontrol::WayPointItem* w=qgraphicsitem_cast(itemm); - //if(w) - // qDebug()<Type; - if (itemm->Type==mapcontrol::WayPointItem::Type) - { - int x=itemm->Type; - int xx= x; - QLabel* l=new QLabel(this); + QGraphicsItem *itemm = map->itemAt(map->mapFromGlobal(QCursor::pos())); + mapcontrol::WayPointItem *w = qgraphicsitem_cast(itemm); + // if(w) + // qDebug()<Type; + if (itemm->Type == mapcontrol::WayPointItem::Type) { + int x = itemm->Type; + int xx = x; + QLabel *l = new QLabel(this); l->show(); } } @@ -60,6 +60,7 @@ void MainWindow::time() void MainWindow::changeEvent(QEvent *e) { QMainWindow::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: ui->retranslateUi(this); @@ -71,48 +72,49 @@ void MainWindow::changeEvent(QEvent *e) void MainWindow::on_pushButtonZoomP_clicked() { - double x=map->ZoomTotal(); - double y=ui->doubleSpinBox->value(); - double z=x+y; - map->SetZoom(map->ZoomTotal()+ui->doubleSpinBox->value()); + double x = map->ZoomTotal(); + double y = ui->doubleSpinBox->value(); + double z = x + y; + + map->SetZoom(map->ZoomTotal() + ui->doubleSpinBox->value()); } void MainWindow::on_pushButtonZoomM_clicked() { - map->SetZoom(map->ZoomTotal()-ui->doubleSpinBox->value()); + map->SetZoom(map->ZoomTotal() - ui->doubleSpinBox->value()); } void MainWindow::on_checkBox_clicked(bool checked) { map->SetShowTileGridLines(checked); } -void MainWindow::zoomChanged(double zoomt,double zoom,double zoomdigi) +void MainWindow::zoomChanged(double zoomt, double zoom, double zoomdigi) { - ui->label_5->setText("CurrentZoom="+QString::number(zoomt)+QString::number(zoom)+QString::number(zoomdigi)); + ui->label_5->setText("CurrentZoom=" + QString::number(zoomt) + QString::number(zoom) + QString::number(zoomdigi)); } void MainWindow::on_pushButtonRL_clicked() { map->UAV->DeleteTrail(); - // map->UAV->SetUAVHeading(10); - //map->SetRotate(map->Rotate()-1); - //map->WPCreate(); - // map->WPCreate(internals::PointLatLng(20,20),100); + // map->UAV->SetUAVHeading(10); + // map->SetRotate(map->Rotate()-1); + // map->WPCreate(); + // map->WPCreate(internals::PointLatLng(20,20),100); } void MainWindow::on_pushButtonRC_clicked() { - map->SetRotate(0); -// wp=map->WPInsert(1); + map->SetRotate(0); +// wp=map->WPInsert(1); } void MainWindow::on_pushButtonRR_clicked() { - map->SetRotate(map->Rotate()+1); - // map->WPDeleteAll(); - // map->WPDelete(wp); -// QList list= map->WPSelected(); -// int x=list.at(0)->Number(); + map->SetRotate(map->Rotate() + 1); + // map->WPDeleteAll(); + // map->WPDelete(wp); +// QList list= map->WPSelected(); +// int x=list.at(0)->Number(); } void MainWindow::on_pushButton_clicked() @@ -122,9 +124,9 @@ void MainWindow::on_pushButton_clicked() void MainWindow::on_pushButtonGO_clicked() { - core::GeoCoderStatusCode::Types x=map->SetCurrentPositionByKeywords(ui->lineEdit->text()); - ui->label->setText( mapcontrol::Helper::StrFromGeoCoderStatusCode(x)); + core::GeoCoderStatusCode::Types x = map->SetCurrentPositionByKeywords(ui->lineEdit->text()); + ui->label->setText(mapcontrol::Helper::StrFromGeoCoderStatusCode(x)); } void MainWindow::on_checkBox_2_clicked(bool checked) @@ -134,15 +136,16 @@ void MainWindow::on_checkBox_2_clicked(bool checked) void MainWindow::on_comboBox_currentIndexChanged(QString value) { - if (map->isStarted()) + if (map->isStarted()) { map->SetMapType(mapcontrol::Helper::MapTypeFromString(value)); + } } void MainWindow::on_pushButton_2_clicked() { map->RipMap(); -// QLabel *x=new QLabel(); -// ima=map->X(); -// x->setPixmap(QPixmap::fromImage(ima)); -// x->show(); +// QLabel *x=new QLabel(); +// ima=map->X(); +// x->setPixmap(QPixmap::fromImage(ima)); +// x->show(); } diff --git a/ground/openpilotgcs/src/experimental/finaltest/mainwindow.h b/ground/openpilotgcs/src/experimental/finaltest/mainwindow.h index 9dd7e509a..4a74848b6 100644 --- a/ground/openpilotgcs/src/experimental/finaltest/mainwindow.h +++ b/ground/openpilotgcs/src/experimental/finaltest/mainwindow.h @@ -5,7 +5,7 @@ #include namespace Ui { - class MainWindow; +class MainWindow; } class MainWindow : public QMainWindow { @@ -20,13 +20,13 @@ protected: private: Ui::MainWindow *ui; mapcontrol::OPMapWidget *map; - mapcontrol::WayPointItem* wp; + mapcontrol::WayPointItem *wp; QImage ima; QLabel l; private slots: void on_pushButton_2_clicked(); - void on_comboBox_currentIndexChanged(QString ); + void on_comboBox_currentIndexChanged(QString); void on_checkBox_2_clicked(bool checked); void on_pushButtonGO_clicked(); void on_pushButton_clicked(); @@ -36,7 +36,7 @@ private slots: void on_checkBox_clicked(bool checked); void on_pushButtonZoomM_clicked(); void on_pushButtonZoomP_clicked(); - void zoomChanged(double zoomt,double zoom,double zoomdigi); + void zoomChanged(double zoomt, double zoom, double zoomdigi); void ttl(int value); void time(); }; diff --git a/ground/openpilotgcs/src/experimental/finaltest/ui_mainwindow.h b/ground/openpilotgcs/src/experimental/finaltest/ui_mainwindow.h index 4a65bd5c0..7d141209d 100644 --- a/ground/openpilotgcs/src/experimental/finaltest/ui_mainwindow.h +++ b/ground/openpilotgcs/src/experimental/finaltest/ui_mainwindow.h @@ -32,8 +32,7 @@ QT_BEGIN_NAMESPACE -class Ui_MainWindow -{ +class Ui_MainWindow { public: QWidget *centralWidget; QWidget *widget; @@ -75,8 +74,9 @@ public: void setupUi(QMainWindow *MainWindow) { - if (MainWindow->objectName().isEmpty()) + if (MainWindow->objectName().isEmpty()) { MainWindow->setObjectName(QString::fromUtf8("MainWindow")); + } MainWindow->resize(524, 551); centralWidget = new QWidget(MainWindow); centralWidget->setObjectName(QString::fromUtf8("centralWidget")); @@ -174,7 +174,7 @@ public: verticalLayout_3->addWidget(groupBox_2); - groupBox_3 = new QGroupBox(widget); + groupBox_3 = new QGroupBox(widget); groupBox_3->setObjectName(QString::fromUtf8("groupBox_3")); verticalLayout = new QVBoxLayout(groupBox_3); verticalLayout->setSpacing(6); @@ -183,7 +183,7 @@ public: horizontalLayout_3 = new QHBoxLayout(); horizontalLayout_3->setSpacing(6); horizontalLayout_3->setObjectName(QString::fromUtf8("horizontalLayout_3")); - pushButtonZoomP = new QPushButton(groupBox_3); + pushButtonZoomP = new QPushButton(groupBox_3); pushButtonZoomP->setObjectName(QString::fromUtf8("pushButtonZoomP")); horizontalLayout_3->addWidget(pushButtonZoomP); @@ -260,7 +260,7 @@ public: mainToolBar = new QToolBar(MainWindow); mainToolBar->setObjectName(QString::fromUtf8("mainToolBar")); MainWindow->addToolBar(Qt::TopToolBarArea, mainToolBar); - statusBar = new QStatusBar(MainWindow); + statusBar = new QStatusBar(MainWindow); statusBar->setObjectName(QString::fromUtf8("statusBar")); MainWindow->setStatusBar(statusBar); @@ -292,11 +292,10 @@ public: checkBox_2->setText(QApplication::translate("MainWindow", "UseOpenGL", 0, QApplication::UnicodeUTF8)); pushButton->setText(QApplication::translate("MainWindow", "ReloadMap", 0, QApplication::UnicodeUTF8)); } // retranslateUi - }; namespace Ui { - class MainWindow: public Ui_MainWindow {}; +class MainWindow : public Ui_MainWindow {}; } // namespace Ui QT_END_NAMESPACE diff --git a/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/main.cpp b/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/main.cpp index 256051ba7..1a9701d74 100644 --- a/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/main.cpp +++ b/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/main.cpp @@ -1,38 +1,38 @@ /** -****************************************************************************** -* -* @file main.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup DocumentationHelper -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file main.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup DocumentationHelper + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include #include "mainwindow.h" - + int main(int argc, char *argv[]) { QApplication a(argc, argv); MainWindow w; + w.show(); return a.exec(); } - diff --git a/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.cpp b/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.cpp index 1c0b61203..c312bb0ac 100644 --- a/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.cpp +++ b/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.cpp @@ -1,66 +1,65 @@ /** -****************************************************************************** -* -* @file mainwindow.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup DocumentationHelper -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file mainwindow.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup DocumentationHelper + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "mainwindow.h" #include "ui_mainwindow.h" #include "QFileDialog" MainWindow::MainWindow(QWidget *parent) : - QMainWindow(parent), - ui(new Ui::MainWindow) + QMainWindow(parent), + ui(new Ui::MainWindow) { ui->setupUi(this); - license<<"/**"; - license<<"******************************************************************************"; - license<<"*"; - license<<"* @file %file"; - license<<"* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010."; - license<<"* @brief "; - license<<"* @see The GNU Public License (GPL) Version 3"; - license<<"* @defgroup %defgroup"; - license<<"* @{"; - license<<"* "; - license<<"*****************************************************************************/"; - license<<"/* "; - license<<"* This program is free software; you can redistribute it and/or modify "; - license<<"* it under the terms of the GNU General Public License as published by "; - license<<"* the Free Software Foundation; either version 3 of the License, or "; - license<<"* (at your option) any later version."; - license<<"* "; - license<<"* This program is distributed in the hope that it will be useful, but "; - license<<"* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY "; - license<<"* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License "; - license<<"* for more details."; - license<<"* "; - license<<"* You should have received a copy of the GNU General Public License along "; - license<<"* with this program; if not, write to the Free Software Foundation, Inc., "; - license<<"* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA"; - license<<"*/"; - + license << "/**"; + license << "******************************************************************************"; + license << "*"; + license << "* @file %file"; + license << "* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010."; + license << "* @brief "; + license << "* @see The GNU Public License (GPL) Version 3"; + license << "* @defgroup %defgroup"; + license << "* @{"; + license << "* "; + license << "*****************************************************************************/"; + license << "/* "; + license << "* This program is free software; you can redistribute it and/or modify "; + license << "* it under the terms of the GNU General Public License as published by "; + license << "* the Free Software Foundation; either version 3 of the License, or "; + license << "* (at your option) any later version."; + license << "* "; + license << "* This program is distributed in the hope that it will be useful, but "; + license << "* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY "; + license << "* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License "; + license << "* for more details."; + license << "* "; + license << "* You should have received a copy of the GNU General Public License along "; + license << "* with this program; if not, write to the Free Software Foundation, Inc., "; + license << "* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA"; + license << "*/"; } MainWindow::~MainWindow() @@ -71,6 +70,7 @@ MainWindow::~MainWindow() void MainWindow::changeEvent(QEvent *e) { QMainWindow::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: ui->retranslateUi(this); @@ -82,119 +82,110 @@ void MainWindow::changeEvent(QEvent *e) void MainWindow::on_cpathBT_clicked() { - wpath=QFileDialog::getExistingDirectory(this,"Choose Work Path"); - ui->cpathL->setText("Current Path:"+wpath); + wpath = QFileDialog::getExistingDirectory(this, "Choose Work Path"); + ui->cpathL->setText("Current Path:" + wpath); } void MainWindow::on_goBT_clicked() { QDir myDir(wpath); QStringList filter; - filter<<"*.cpp"<<"*.h"; - QStringList list = myDir.entryList (filter); - foreach(QString str,list) - { - bool go=true; - if(ui->confirmCB->isChecked()) - if(QMessageBox::question(this,"Process File?:",str,QMessageBox::Yes,QMessageBox::No)==QMessageBox::No)go=false; - if(go){ - ui->cfileL->setText("Current File:"+str); - QFile file(wpath+QDir::separator()+str); - if (!file.open(QIODevice::ReadWrite)) { - ui->output->append("Cannot open file "+str+" for writing"); + filter << "*.cpp" << "*.h"; + QStringList list = myDir.entryList(filter); + + foreach(QString str, list) { + bool go = true; + + if (ui->confirmCB->isChecked()) { + if (QMessageBox::question(this, "Process File?:", str, QMessageBox::Yes, QMessageBox::No) == QMessageBox::No) { + go = false; } - else - { - ui->output->append("Processing file "+str); + } + if (go) { + ui->cfileL->setText("Current File:" + str); + QFile file(wpath + QDir::separator() + str); + if (!file.open(QIODevice::ReadWrite)) { + ui->output->append("Cannot open file " + str + " for writing"); + } else { + ui->output->append("Processing file " + str); QTextStream out(&file); QStringList filestr; out.seek(0); - while(!out.atEnd()) filestr<licenseCB->isChecked()) - { - - bool haslicense=false; - foreach(QString str,filestr) - { - if(str.contains("The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010")) - { - haslicense=true; + if (ui->licenseCB->isChecked()) { + bool haslicense = false; + foreach(QString str, filestr) { + if (str.contains("The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010")) { + haslicense = true; ui->output->append("File Already has License Info"); } } - if(!haslicense) - { - bool process=true; - if(ui->confirmCB->isChecked()) - if(QMessageBox::question(this,"Add license Info?:",str,QMessageBox::Yes,QMessageBox::No)==QMessageBox::No) process=false; - if(process) - { + if (!haslicense) { + bool process = true; + if (ui->confirmCB->isChecked()) { + if (QMessageBox::question(this, "Add license Info?:", str, QMessageBox::Yes, QMessageBox::No) == QMessageBox::No) { + process = false; + } + } + if (process) { ui->output->append("Added License info to file"); out.seek(0); - foreach(QString line,license) - { - line.replace("%file",str); - line.replace("%defgroup",ui->defgroup->text()); - out<defgroup->text()); + out << line << "\r\n"; } - foreach(QString str,filestr) out<nameCB->isChecked()) - { + if (ui->nameCB->isChecked()) { filestr.clear(); out.seek(0); - while(!out.atEnd()) filestr<output->append("File Already has Namespace"); } } - if(ui->confirmCB->isChecked() && process) - if(QMessageBox::question(this,"Create Namespace?:",str,QMessageBox::Yes,QMessageBox::No)==QMessageBox::No) process=false; - if(process) - { + if (ui->confirmCB->isChecked() && process) { + if (QMessageBox::question(this, "Create Namespace?:", str, QMessageBox::Yes, QMessageBox::No) == QMessageBox::No) { + process = false; + } + } + if (process) { ui->output->append("Added NameSpace to file"); out.seek(0); - bool initdone=false; - for(int x=0;xnamespa->text()+" {"); - filestr.insert(x," "); - initdone=true; + bool initdone = false; + for (int x = 0; x < filestr.count(); ++x) { + QString line = filestr.at(x); + if (!initdone) { + if (!(line.trimmed().startsWith("#") || line.trimmed().startsWith("/") || line.trimmed().startsWith("*") || line.trimmed().isEmpty())) { + filestr.insert(x, "namespace " + ui->namespa->text() + " {"); + filestr.insert(x, " "); + initdone = true; } - } - else - { - if((QString)str.split(".").at(1)=="cpp") - { + } else { + if ((QString)str.split(".").at(1) == "cpp") { filestr.append("}"); break; - } - else - { - for(int y=filestr.count()-1;y>1;--y) - { - QString s=filestr.at(y); - if(s.contains("#endif")) - { - filestr.insert(y,"}"); + } else { + for (int y = filestr.count() - 1; y > 1; --y) { + QString s = filestr.at(y); + if (s.contains("#endif")) { + filestr.insert(y, "}"); break; } } @@ -202,43 +193,40 @@ void MainWindow::on_goBT_clicked() } } } - foreach(QString str,filestr) out<bockifCB->isChecked()) - { + if (ui->bockifCB->isChecked()) { QString genif; ui->output->append("Creating block ifs"); filestr.clear(); out.seek(0); - while(!out.atEnd()) filestr<textEdit->append(genif); - foreach(QString str,filestr) out<textEdit->append(genif); + } + foreach(QString str, filestr) out << str + "\r\n"; out.flush(); - } file.close(); } - } } } diff --git a/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.h b/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.h index feebc949b..4b482cd80 100644 --- a/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.h +++ b/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file mainwindow.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup DocumentationHelper -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file mainwindow.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup DocumentationHelper + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef MAINWINDOW_H #define MAINWINDOW_H @@ -34,7 +34,7 @@ #include namespace Ui { - class MainWindow; +class MainWindow; } class MainWindow : public QMainWindow { diff --git a/ground/openpilotgcs/src/libs/aggregation/aggregate.cpp b/ground/openpilotgcs/src/libs/aggregation/aggregate.cpp index 81fc794b1..0a98eb5c3 100644 --- a/ground/openpilotgcs/src/libs/aggregation/aggregate.cpp +++ b/ground/openpilotgcs/src/libs/aggregation/aggregate.cpp @@ -4,25 +4,25 @@ * @file aggregate.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,7 +39,7 @@ Components that are bundled to an Aggregate can be "cast" to each other and have a coupled life cycle. See the documentation of Aggregation::Aggregate for details and examples. -*/ + */ /*! \class Aggregation::Aggregate @@ -97,7 +97,7 @@ Aggregation aware code never uses qobject_cast, but always uses Aggregation::query which behaves like a qobject_cast as a fallback. -*/ + */ /*! \fn T *Aggregate::component() @@ -107,7 +107,7 @@ \sa Aggregate::components() \sa Aggregate::add() -*/ + */ /*! \fn QList Aggregate::components() @@ -116,17 +116,17 @@ \sa Aggregate::component() \sa Aggregate::add() -*/ + */ /*! \fn T *Aggregation::query(Aggregate *obj) \internal -*/ + */ /*! \fn QList Aggregation::query_all(Aggregate *obj) \internal -*/ + */ /*! \relates Aggregation::Aggregate @@ -138,7 +138,7 @@ checked, or if it doesn't belong to an Aggregate null is returned. \sa Aggregate::component() -*/ + */ /*! \relates Aggregation::Aggregate @@ -148,7 +148,7 @@ type are returned. Otherwise, \a obj is returned if it is of the requested type. \sa Aggregate::components() -*/ + */ using namespace Aggregation; @@ -156,26 +156,29 @@ using namespace Aggregation; \fn Aggregate *Aggregate::parentAggregate(QObject *obj) Returns the Aggregate object of \a obj if there is one. Otherwise returns 0. -*/ + */ Aggregate *Aggregate::parentAggregate(QObject *obj) { QReadLocker locker(&lock()); + return aggregateMap().value(obj); } QHash &Aggregate::aggregateMap() { static QHash map; + return map; } /*! \fn QReadWriteLock &Aggregate::lock() \internal -*/ + */ QReadWriteLock &Aggregate::lock() { static QReadWriteLock lock; + return lock; } @@ -185,11 +188,12 @@ QReadWriteLock &Aggregate::lock() Creates a new Aggregate with the given \a parent. The \a parent is passed directly passed to the QObject part of the class and is not used beside that. -*/ + */ Aggregate::Aggregate(QObject *parent) : QObject(parent) { QWriteLocker locker(&lock()); + aggregateMap().insert(this, this); } @@ -197,12 +201,13 @@ Aggregate::Aggregate(QObject *parent) \fn Aggregate::~Aggregate() Deleting the aggregate automatically deletes all its components. -*/ + */ Aggregate::~Aggregate() { QWriteLocker locker(&lock()); - foreach (QObject *component, m_components) { - disconnect(component, SIGNAL(destroyed(QObject*)), this, SLOT(deleteSelf(QObject*))); + + foreach(QObject * component, m_components) { + disconnect(component, SIGNAL(destroyed(QObject *)), this, SLOT(deleteSelf(QObject *))); aggregateMap().remove(component); } qDeleteAll(m_components); @@ -226,19 +231,22 @@ void Aggregate::deleteSelf(QObject *obj) Adds the \a component to the aggregate. \sa Aggregate::remove() -*/ + */ void Aggregate::add(QObject *component) { - if (!component) + if (!component) { return; + } QWriteLocker locker(&lock()); Aggregate *parentAggregation = aggregateMap().value(component); - if (parentAggregation == this) + if (parentAggregation == this) { return; - if (parentAggregation) + } + if (parentAggregation) { parentAggregation->remove(component); + } m_components.append(component); - connect(component, SIGNAL(destroyed(QObject*)), this, SLOT(deleteSelf(QObject*))); + connect(component, SIGNAL(destroyed(QObject *)), this, SLOT(deleteSelf(QObject *))); aggregateMap().insert(component, this); } @@ -248,13 +256,14 @@ void Aggregate::add(QObject *component) Removes the \a component from the aggregate. \sa Aggregate::add() -*/ + */ void Aggregate::remove(QObject *component) { - if (!component) + if (!component) { return; + } QWriteLocker locker(&lock()); aggregateMap().remove(component); m_components.removeAll(component); - disconnect(component, SIGNAL(destroyed(QObject*)), this, SLOT(deleteSelf(QObject*))); + disconnect(component, SIGNAL(destroyed(QObject *)), this, SLOT(deleteSelf(QObject *))); } diff --git a/ground/openpilotgcs/src/libs/aggregation/aggregate.h b/ground/openpilotgcs/src/libs/aggregation/aggregate.h index da225977f..d150f2089 100644 --- a/ground/openpilotgcs/src/libs/aggregation/aggregate.h +++ b/ground/openpilotgcs/src/libs/aggregation/aggregate.h @@ -4,25 +4,25 @@ * @file aggregate.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,9 +38,7 @@ #include namespace Aggregation { - -class AGGREGATION_EXPORT Aggregate : public QObject -{ +class AGGREGATION_EXPORT Aggregate : public QObject { Q_OBJECT public: @@ -50,20 +48,23 @@ public: void add(QObject *component); void remove(QObject *component); - template T *component() { + template T *component() + { QReadLocker(&lock()); - foreach (QObject *component, m_components) { - if (T *result = qobject_cast(component)) + foreach(QObject * component, m_components) { + if (T * result = qobject_cast(component)) { return result; + } } return (T *)0; } - template QList components() { + template QList components() + { QReadLocker(&lock()); QList results; - foreach (QObject *component, m_components) { - if (T *result = qobject_cast(component)) { + foreach(QObject * component, m_components) { + if (T * result = qobject_cast(component)) { results << result; } } @@ -85,15 +86,17 @@ private: // get a component via global template function template T *query(Aggregate *obj) { - if (!obj) + if (!obj) { return (T *)0; + } return obj->template component(); } template T *query(QObject *obj) { - if (!obj) + if (!obj) { return (T *)0; + } T *result = qobject_cast(obj); if (!result) { QReadLocker(&lock()); @@ -106,25 +109,27 @@ template T *query(QObject *obj) // get all components of a specific type via template function template QList query_all(Aggregate *obj) { - if (!obj) + if (!obj) { return QList(); + } return obj->template components(); } template QList query_all(QObject *obj) { - if (!obj) + if (!obj) { return QList(); + } QReadLocker(&lock()); Aggregate *parentAggregation = Aggregate::parentAggregate(obj); QList results; - if (parentAggregation) + if (parentAggregation) { results = query_all(parentAggregation); - else if (T *result = qobject_cast(obj)) + } else if (T * result = qobject_cast(obj)) { results.append(result); + } return results; } - } // namespace Aggregation #endif // QAGGREGATION_H diff --git a/ground/openpilotgcs/src/libs/aggregation/aggregation_global.h b/ground/openpilotgcs/src/libs/aggregation/aggregation_global.h index e8bca0029..63f69680e 100644 --- a/ground/openpilotgcs/src/libs/aggregation/aggregation_global.h +++ b/ground/openpilotgcs/src/libs/aggregation/aggregation_global.h @@ -4,25 +4,25 @@ * @file aggregation_global.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/libs/aggregation/examples/text/main.cpp b/ground/openpilotgcs/src/libs/aggregation/examples/text/main.cpp index 525f1575e..c937ef413 100644 --- a/ground/openpilotgcs/src/libs/aggregation/examples/text/main.cpp +++ b/ground/openpilotgcs/src/libs/aggregation/examples/text/main.cpp @@ -4,25 +4,25 @@ * @file main.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -51,6 +51,7 @@ void MyMain::select(int index) IText1 *t1 = Aggregation::query(entry); IText2 *t2 = Aggregation::query(entry); IText3 *t3 = Aggregation::query(entry); + // set the label texts and enable/disable, depending on whether // the respective interface implementations exist ui.text1->setText(t1 ? t1->text() : tr("N/A")); @@ -81,6 +82,7 @@ int main(int argc, char *argv[]) // the second additionally provides an IText2 implementation // adding a component to the aggregation is done by setting the aggregation as the parent of the component Aggregation::Aggregate *obj2 = new Aggregation::Aggregate; + obj2->add(new IComboEntry("Entry with text 2")); obj2->add(new IText2("This is a text for label 2")); diff --git a/ground/openpilotgcs/src/libs/aggregation/examples/text/main.h b/ground/openpilotgcs/src/libs/aggregation/examples/text/main.h index 92e85abae..ae2585641 100644 --- a/ground/openpilotgcs/src/libs/aggregation/examples/text/main.h +++ b/ground/openpilotgcs/src/libs/aggregation/examples/text/main.h @@ -4,25 +4,25 @@ * @file main.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,8 +36,7 @@ #include -class MyMain : public QWidget -{ +class MyMain : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/libs/aggregation/examples/text/myinterfaces.h b/ground/openpilotgcs/src/libs/aggregation/examples/text/myinterfaces.h index 63cce9a79..95a47a3a2 100644 --- a/ground/openpilotgcs/src/libs/aggregation/examples/text/myinterfaces.h +++ b/ground/openpilotgcs/src/libs/aggregation/examples/text/myinterfaces.h @@ -4,25 +4,25 @@ * @file myinterfaces.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,52 +33,60 @@ #include -class IComboEntry : public QObject -{ +class IComboEntry : public QObject { Q_OBJECT public: IComboEntry(QString title) : m_title(title) {} virtual ~IComboEntry() {} - QString title() const { return m_title; } + QString title() const + { + return m_title; + } private: QString m_title; }; -class IText1 : public QObject -{ +class IText1 : public QObject { Q_OBJECT public: IText1(QString text) : m_text(text) {} virtual ~IText1() {} - QString text() const { return m_text; } + QString text() const + { + return m_text; + } private: QString m_text; }; -class IText2 : public QObject -{ +class IText2 : public QObject { Q_OBJECT public: IText2(QString text) : m_text(text) {} - QString text() const { return m_text; } + QString text() const + { + return m_text; + } private: QString m_text; }; -class IText3 : public QObject -{ +class IText3 : public QObject { Q_OBJECT public: IText3(QString text) : m_text(text) {} virtual ~IText3() {} - QString text() const { return m_text; } + QString text() const + { + return m_text; + } private: QString m_text; diff --git a/ground/openpilotgcs/src/libs/extensionsystem/extensionsystem_global.h b/ground/openpilotgcs/src/libs/extensionsystem/extensionsystem_global.h index dbe6b9178..a06821486 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/extensionsystem_global.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/extensionsystem_global.h @@ -4,25 +4,25 @@ * @file extensionsystem_global.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/libs/extensionsystem/iplugin.cpp b/ground/openpilotgcs/src/libs/extensionsystem/iplugin.cpp index 65720561c..f15247ee0 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/iplugin.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/iplugin.cpp @@ -4,25 +4,25 @@ * @file iplugin.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -212,7 +212,7 @@ Plugins have access to the plugin manager (and its object pool) via the PluginManager::instance() method. -*/ + */ /*! \fn bool IPlugin::initialize(const QStringList &arguments, QString *errorString) @@ -224,7 +224,7 @@ the \a errorString should be set to a user-readable message describing the reason. \sa extensionsInitialized() -*/ + */ /*! \fn void IPlugin::extensionsInitialized() @@ -236,7 +236,7 @@ look in the plugin manager's object pool for objects that have been provided by dependent plugins. \sa initialize() -*/ + */ /*! \fn void IPlugin::shutdown() @@ -246,28 +246,28 @@ disconnect from the PluginManager::aboutToRemoveObject() signal if getting the signal (and probably doing lots of stuff to update the internal and visible state) doesn't make sense during shutdown. -*/ + */ using namespace ExtensionSystem; /*! \fn IPlugin::IPlugin() \internal -*/ + */ IPlugin::IPlugin() : d(new Internal::IPluginPrivate()) -{ -} +{} /*! \fn IPlugin::~IPlugin() \internal -*/ + */ IPlugin::~IPlugin() { PluginManager *pm = PluginManager::instance(); - foreach (QObject *obj, d->addedObjectsInReverseOrder) - pm->removeObject(obj); + + foreach(QObject * obj, d->addedObjectsInReverseOrder) + pm->removeObject(obj); qDeleteAll(d->addedObjectsInReverseOrder); d->addedObjectsInReverseOrder.clear(); delete d; @@ -278,7 +278,7 @@ IPlugin::~IPlugin() \fn PluginSpec *IPlugin::pluginSpec() const Returns the PluginSpec corresponding to this plugin. This is not available in the constructor. -*/ + */ PluginSpec *IPlugin::pluginSpec() const { return d->pluginSpec; @@ -288,7 +288,7 @@ PluginSpec *IPlugin::pluginSpec() const \fn void IPlugin::addObject(QObject *obj) Convenience method that registers \a obj in the plugin manager's plugin pool by just calling PluginManager::addObject(). -*/ + */ void IPlugin::addObject(QObject *obj) { PluginManager::instance()->addObject(obj); @@ -303,7 +303,7 @@ void IPlugin::addObject(QObject *obj) removed and deleted in reverse order of registration when the IPlugin instance is destroyed. \sa PluginManager::addObject() -*/ + */ void IPlugin::addAutoReleasedObject(QObject *obj) { d->addedObjectsInReverseOrder.prepend(obj); @@ -314,9 +314,8 @@ void IPlugin::addAutoReleasedObject(QObject *obj) \fn void IPlugin::removeObject(QObject *obj) Convenience method that unregisters \a obj from the plugin manager's plugin pool by just calling PluginManager::removeObject(). -*/ + */ void IPlugin::removeObject(QObject *obj) { PluginManager::instance()->removeObject(obj); } - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/iplugin.h b/ground/openpilotgcs/src/libs/extensionsystem/iplugin.h index 957153645..b713a3b55 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/iplugin.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/iplugin.h @@ -4,25 +4,25 @@ * @file iplugin.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,17 +34,15 @@ #include namespace ExtensionSystem { - namespace Internal { - class IPluginPrivate; - class PluginSpecPrivate; +class IPluginPrivate; +class PluginSpecPrivate; } class PluginManager; class PluginSpec; -class EXTENSIONSYSTEM_EXPORT IPlugin : public QObject -{ +class EXTENSIONSYSTEM_EXPORT IPlugin : public QObject { Q_OBJECT public: @@ -53,7 +51,7 @@ public: virtual bool initialize(const QStringList &arguments, QString *errorString) = 0; virtual void extensionsInitialized() = 0; - virtual void shutdown() { } + virtual void shutdown() {} PluginSpec *pluginSpec() const; @@ -66,7 +64,6 @@ private: friend class Internal::PluginSpecPrivate; }; - } // namespace ExtensionSystem #endif // IPLUGIN_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/iplugin_p.h b/ground/openpilotgcs/src/libs/extensionsystem/iplugin_p.h index 1b8845d07..cc3486fb0 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/iplugin_p.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/iplugin_p.h @@ -4,25 +4,25 @@ * @file iplugin_p.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,20 +34,16 @@ #include namespace ExtensionSystem { - class PluginManager; class PluginSpec; namespace Internal { - -class IPluginPrivate -{ +class IPluginPrivate { public: PluginSpec *pluginSpec; QList addedObjectsInReverseOrder; }; - } // namespace Internal } // namespace ExtensionSystem diff --git a/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.cpp b/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.cpp index 24979c2ca..aa01196bc 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.cpp @@ -4,25 +4,25 @@ * @file optionsparser.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,59 +35,70 @@ using namespace ExtensionSystem::Internal; static const char *END_OF_OPTIONS = "--"; const char *OptionsParser::NO_LOAD_OPTION = "-noload"; -const char *OptionsParser::TEST_OPTION = "-test"; +const char *OptionsParser::TEST_OPTION = "-test"; OptionsParser::OptionsParser(const QStringList &args, - const QMap &appOptions, - QMap *foundAppOptions, - QString *errorString, - PluginManagerPrivate *pmPrivate) + const QMap &appOptions, + QMap *foundAppOptions, + QString *errorString, + PluginManagerPrivate *pmPrivate) : m_args(args), m_appOptions(appOptions), - m_foundAppOptions(foundAppOptions), - m_errorString(errorString), - m_pmPrivate(pmPrivate), - m_it(m_args.constBegin()), - m_end(m_args.constEnd()), - m_isDependencyRefreshNeeded(false), - m_hasError(false) + m_foundAppOptions(foundAppOptions), + m_errorString(errorString), + m_pmPrivate(pmPrivate), + m_it(m_args.constBegin()), + m_end(m_args.constEnd()), + m_isDependencyRefreshNeeded(false), + m_hasError(false) { ++m_it; // jump over program name - if (m_errorString) + if (m_errorString) { m_errorString->clear(); - if (m_foundAppOptions) + } + if (m_foundAppOptions) { m_foundAppOptions->clear(); + } m_pmPrivate->arguments.clear(); } bool OptionsParser::parse() { while (!m_hasError) { - if (!nextToken()) // move forward + if (!nextToken()) { // move forward break; - if (checkForEndOfOptions()) + } + if (checkForEndOfOptions()) { break; - if (checkForNoLoadOption()) + } + if (checkForNoLoadOption()) { continue; - if (checkForTestOption()) + } + if (checkForTestOption()) { continue; - if (checkForAppOption()) + } + if (checkForAppOption()) { continue; - if (checkForPluginOption()) + } + if (checkForPluginOption()) { continue; - if (checkForUnknownOption()) + } + if (checkForUnknownOption()) { break; + } // probably a file or something m_pmPrivate->arguments << m_currentArg; } - if (m_isDependencyRefreshNeeded) + if (m_isDependencyRefreshNeeded) { m_pmPrivate->resolveDependencies(); + } return !m_hasError; } bool OptionsParser::checkForEndOfOptions() { - if (m_currentArg != QLatin1String(END_OF_OPTIONS)) + if (m_currentArg != QLatin1String(END_OF_OPTIONS)) { return false; + } while (nextToken()) { m_pmPrivate->arguments << m_currentArg; } @@ -96,14 +107,16 @@ bool OptionsParser::checkForEndOfOptions() bool OptionsParser::checkForTestOption() { - if (m_currentArg != QLatin1String(TEST_OPTION)) + if (m_currentArg != QLatin1String(TEST_OPTION)) { return false; + } if (nextToken(RequiredToken)) { PluginSpec *spec = m_pmPrivate->pluginByName(m_currentArg); if (!spec) { - if (m_errorString) + if (m_errorString) { *m_errorString = QCoreApplication::translate("PluginManager", "The plugin '%1' does not exist.").arg(m_currentArg); + } m_hasError = true; } else { m_pmPrivate->testSpecs.append(spec); @@ -114,14 +127,16 @@ bool OptionsParser::checkForTestOption() bool OptionsParser::checkForNoLoadOption() { - if (m_currentArg != QLatin1String(NO_LOAD_OPTION)) + if (m_currentArg != QLatin1String(NO_LOAD_OPTION)) { return false; + } if (nextToken(RequiredToken)) { PluginSpec *spec = m_pmPrivate->pluginByName(m_currentArg); if (!spec) { - if (m_errorString) + if (m_errorString) { *m_errorString = QCoreApplication::translate("PluginManager", "The plugin '%1' does not exist.").arg(m_currentArg); + } m_hasError = true; } else { m_pmPrivate->pluginSpecs.removeAll(spec); @@ -134,16 +149,18 @@ bool OptionsParser::checkForNoLoadOption() bool OptionsParser::checkForAppOption() { - if (!m_appOptions.contains(m_currentArg)) + if (!m_appOptions.contains(m_currentArg)) { return false; + } QString option = m_currentArg; QString argument; if (m_appOptions.value(m_currentArg) && nextToken(RequiredToken)) { - //argument required + // argument required argument = m_currentArg; } - if (m_foundAppOptions) + if (m_foundAppOptions) { m_foundAppOptions->insert(option, argument); + } return true; } @@ -151,8 +168,10 @@ bool OptionsParser::checkForPluginOption() { bool requiresParameter; PluginSpec *spec = m_pmPrivate->pluginForOption(m_currentArg, &requiresParameter); - if (!spec) + + if (!spec) { return false; + } spec->addArgument(m_currentArg); if (requiresParameter && nextToken(RequiredToken)) { spec->addArgument(m_currentArg); @@ -162,11 +181,13 @@ bool OptionsParser::checkForPluginOption() bool OptionsParser::checkForUnknownOption() { - if (!m_currentArg.startsWith(QLatin1Char('-'))) + if (!m_currentArg.startsWith(QLatin1Char('-'))) { return false; - if (m_errorString) + } + if (m_errorString) { *m_errorString = QCoreApplication::translate("PluginManager", "Unknown option %1").arg(m_currentArg); + } m_hasError = true; return true; } @@ -176,9 +197,10 @@ bool OptionsParser::nextToken(OptionsParser::TokenType type) if (m_it == m_end) { if (type == OptionsParser::RequiredToken) { m_hasError = true; - if (m_errorString) + if (m_errorString) { *m_errorString = QCoreApplication::translate("PluginManager", "The option %1 requires an argument.").arg(m_currentArg); + } } return false; } diff --git a/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.h b/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.h index 665c0cb71..6132a1ba2 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.h @@ -4,25 +4,25 @@ * @file optionsparser.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,16 +36,14 @@ namespace ExtensionSystem { namespace Internal { - -class OptionsParser -{ +class OptionsParser { public: OptionsParser(const QStringList &args, - const QMap &appOptions, - QMap *foundAppOptions, - QString *errorString, - PluginManagerPrivate *pmPrivate); - + const QMap &appOptions, + QMap *foundAppOptions, + QString *errorString, + PluginManagerPrivate *pmPrivate); + bool parse(); static const char *NO_LOAD_OPTION; @@ -62,13 +60,13 @@ private: enum TokenType { OptionalToken, RequiredToken }; bool nextToken(TokenType type = OptionalToken); - + const QStringList &m_args; const QMap &m_appOptions; QMap *m_foundAppOptions; QString *m_errorString; PluginManagerPrivate *m_pmPrivate; - + // state QString m_currentArg; QStringList::const_iterator m_it; @@ -76,7 +74,6 @@ private: bool m_isDependencyRefreshNeeded; bool m_hasError; }; - } // namespace Internal } // namespace ExtensionSystem diff --git a/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.cpp b/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.cpp index 7bff8e54c..aad82582d 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.cpp @@ -4,25 +4,25 @@ * @file plugindetailsview.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,17 +40,17 @@ uses the plugin manager. \sa ExtensionSystem::PluginView -*/ + */ using namespace ExtensionSystem; /*! \fn PluginDetailsView::PluginDetailsView(QWidget *parent) Constructs a new view with given \a parent widget. -*/ + */ PluginDetailsView::PluginDetailsView(QWidget *parent) - : QWidget(parent), - m_ui(new Internal::Ui::PluginDetailsView()) + : QWidget(parent), + m_ui(new Internal::Ui::PluginDetailsView()) { m_ui->setupUi(this); } @@ -58,7 +58,7 @@ PluginDetailsView::PluginDetailsView(QWidget *parent) /*! \fn PluginDetailsView::~PluginDetailsView() \internal -*/ + */ PluginDetailsView::~PluginDetailsView() { delete m_ui; @@ -68,7 +68,7 @@ PluginDetailsView::~PluginDetailsView() \fn void PluginDetailsView::update(PluginSpec *spec) Reads the given \a spec and displays its values in this PluginDetailsView. -*/ + */ void PluginDetailsView::update(PluginSpec *spec) { m_ui->name->setText(spec->name()); @@ -82,7 +82,7 @@ void PluginDetailsView::update(PluginSpec *spec) m_ui->copyright->setText(spec->copyright()); m_ui->license->setText(spec->license()); QStringList depStrings; - foreach (PluginDependency dep, spec->dependencies()) { + foreach(PluginDependency dep, spec->dependencies()) { depStrings << QString("%1 (%2)").arg(dep.name).arg(dep.version); } m_ui->dependencies->addItems(depStrings); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.h b/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.h index 26e0baf13..1d6161b2c 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.h @@ -4,25 +4,25 @@ * @file plugindetailsview.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,30 +34,27 @@ #include namespace ExtensionSystem { - class PluginSpec; namespace Internal { namespace Ui { - class PluginDetailsView; +class PluginDetailsView; } // namespace Ui } // namespace Internal -class EXTENSIONSYSTEM_EXPORT PluginDetailsView : public QWidget -{ +class EXTENSIONSYSTEM_EXPORT PluginDetailsView : public QWidget { Q_OBJECT public: PluginDetailsView(QWidget *parent = 0); ~PluginDetailsView(); - + void update(PluginSpec *spec); private: Internal::Ui::PluginDetailsView *m_ui; }; - } // namespace ExtensionSystem -#endif +#endif // ifndef PLUGINDETAILSVIEW_H_ diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.cpp b/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.cpp index 3ab6db8e5..df177d361 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.cpp @@ -4,25 +4,25 @@ * @file pluginerrorview.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,17 +40,17 @@ uses the plugin manager. \sa ExtensionSystem::PluginView -*/ + */ using namespace ExtensionSystem; /*! \fn PluginErrorView::PluginErrorView(QWidget *parent) Constructs a new error view with given \a parent widget. -*/ + */ PluginErrorView::PluginErrorView(QWidget *parent) : QWidget(parent), - m_ui(new Internal::Ui::PluginErrorView()) + m_ui(new Internal::Ui::PluginErrorView()) { m_ui->setupUi(this); } @@ -58,7 +58,7 @@ PluginErrorView::PluginErrorView(QWidget *parent) /*! \fn PluginErrorView::~PluginErrorView() \internal -*/ + */ PluginErrorView::~PluginErrorView() { delete m_ui; @@ -68,41 +68,42 @@ PluginErrorView::~PluginErrorView() \fn void PluginErrorView::update(PluginSpec *spec) Reads the given \a spec and displays its state and error information in this PluginErrorView. -*/ + */ void PluginErrorView::update(PluginSpec *spec) { QString text; QString tooltip; + switch (spec->state()) { case PluginSpec::Invalid: - text = tr("Invalid"); + text = tr("Invalid"); tooltip = tr("Description file found, but error on read"); break; case PluginSpec::Read: - text = tr("Read"); + text = tr("Read"); tooltip = tr("Description successfully read"); break; case PluginSpec::Resolved: - text = tr("Resolved"); + text = tr("Resolved"); tooltip = tr("Dependencies are successfully resolved"); break; case PluginSpec::Loaded: - text = tr("Loaded"); + text = tr("Loaded"); tooltip = tr("Library is loaded"); break; case PluginSpec::Initialized: - text = tr("Initialized"); + text = tr("Initialized"); tooltip = tr("Plugin's initialization method succeeded"); break; case PluginSpec::Running: - text = tr("Running"); + text = tr("Running"); tooltip = tr("Plugin successfully loaded and running"); break; case PluginSpec::Stopped: - text = tr("Stopped"); + text = tr("Stopped"); tooltip = tr("Plugin was shut down"); case PluginSpec::Deleted: - text = tr("Deleted"); + text = tr("Deleted"); tooltip = tr("Plugin ended its life cycle and was deleted"); } m_ui->state->setText(text); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.h index 62daf093d..b33aaabf6 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.h @@ -4,25 +4,25 @@ * @file pluginerrorview.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,16 +34,14 @@ #include namespace ExtensionSystem { - class PluginSpec; namespace Internal { namespace Ui { - class PluginErrorView; +class PluginErrorView; } // namespace Ui } // namespace Internal -class EXTENSIONSYSTEM_EXPORT PluginErrorView : public QWidget -{ +class EXTENSIONSYSTEM_EXPORT PluginErrorView : public QWidget { Q_OBJECT public: @@ -55,7 +53,6 @@ public: private: Internal::Ui::PluginErrorView *m_ui; }; - } // namespace ExtensionSystem #endif // PLUGINERRORVIEW_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.cpp b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.cpp index 09730f2d5..8ff664205 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.cpp @@ -4,25 +4,25 @@ * @file pluginmanager.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -52,12 +52,12 @@ enum { debugLeaks = 0 }; The basic extension system contains of the plugin manager and its supporting classes, and the IPlugin interface that must be implemented by plugin providers. -*/ + */ /*! \namespace ExtensionSystem::Internal \internal -*/ + */ /*! \class ExtensionSystem::PluginManager @@ -112,24 +112,24 @@ enum { debugLeaks = 0 }; \endcode \bold Note: The object pool manipulating functions are thread-safe. -*/ + */ /*! \fn void PluginManager::objectAdded(QObject *obj) Signal that \a obj has been added to the object pool. -*/ + */ /*! \fn void PluginManager::aboutToRemoveObject(QObject *obj) Signal that \a obj will be removed from the object pool. -*/ + */ /*! \fn void PluginManager::pluginsChanged() Signal that the list of available plugins has changed. \sa plugins() -*/ + */ /*! \fn T *PluginManager::getObject() const @@ -141,7 +141,7 @@ enum { debugLeaks = 0 }; the object pool, this method will choose an arbitrary one of them. \sa addObject() -*/ + */ /*! \fn QList PluginManager::getObjects() const @@ -151,7 +151,7 @@ enum { debugLeaks = 0 }; determine the type of an object. \sa addObject() -*/ + */ using namespace ExtensionSystem; using namespace ExtensionSystem::Internal; @@ -166,7 +166,7 @@ PluginManager *PluginManager::m_instance = 0; /*! \fn PluginManager *PluginManager::instance() Get the unique plugin manager instance. -*/ + */ PluginManager *PluginManager::instance() { return m_instance; @@ -175,9 +175,9 @@ PluginManager *PluginManager::instance() /*! \fn PluginManager::PluginManager() Create a plugin manager. Should be done only once per application. -*/ + */ PluginManager::PluginManager() - : d(new PluginManagerPrivate(this)),m_allPluginsLoaded(false) + : d(new PluginManagerPrivate(this)), m_allPluginsLoaded(false) { m_instance = this; } @@ -185,7 +185,7 @@ PluginManager::PluginManager() /*! \fn PluginManager::~PluginManager() \internal -*/ + */ PluginManager::~PluginManager() { delete d; @@ -203,7 +203,7 @@ PluginManager::~PluginManager() \sa PluginManager::removeObject() \sa PluginManager::getObject() \sa PluginManager::getObjects() -*/ + */ void PluginManager::addObject(QObject *obj) { d->addObject(obj); @@ -213,7 +213,7 @@ void PluginManager::addObject(QObject *obj) \fn void PluginManager::removeObject(QObject *obj) Emits aboutToRemoveObject() and removes the object \a obj from the object pool. \sa PluginManager::addObject() -*/ + */ void PluginManager::removeObject(QObject *obj) { d->removeObject(obj); @@ -225,7 +225,7 @@ void PluginManager::removeObject(QObject *obj) Usually clients do not need to call this. \sa PluginManager::getObject() \sa PluginManager::getObjects() -*/ + */ QList PluginManager::allObjects() const { return d->allObjects; @@ -239,7 +239,7 @@ QList PluginManager::allObjects() const \sa setPluginPaths() \sa plugins() -*/ + */ void PluginManager::loadPlugins() { return d->loadPlugins(); @@ -250,7 +250,7 @@ void PluginManager::loadPlugins() The list of paths were the plugin manager searches for plugins. \sa setPluginPaths() -*/ + */ QStringList PluginManager::pluginPaths() const { return d->pluginPaths; @@ -264,7 +264,7 @@ QStringList PluginManager::pluginPaths() const \sa pluginPaths() \sa loadPlugins() -*/ + */ void PluginManager::setPluginPaths(const QStringList &paths) { d->setPluginPaths(paths); @@ -276,7 +276,7 @@ void PluginManager::setPluginPaths(const QStringList &paths) The default is "xml". \sa setFileExtension() -*/ + */ QString PluginManager::fileExtension() const { return d->extension; @@ -288,7 +288,7 @@ QString PluginManager::fileExtension() const The default is "xml". At the moment this must be called before setPluginPaths() is called. // ### TODO let this + setPluginPaths read the plugin specs lazyly whenever loadPlugins() or plugins() is called. -*/ + */ void PluginManager::setFileExtension(const QString &extension) { d->extension = extension; @@ -298,7 +298,7 @@ void PluginManager::setFileExtension(const QString &extension) \fn QStringList PluginManager::arguments() const The arguments left over after parsing (Neither startup nor plugin arguments). Typically, this will be the list of files to open. -*/ + */ QStringList PluginManager::arguments() const { return d->arguments; @@ -313,7 +313,7 @@ QStringList PluginManager::arguments() const the plugin specification has a reference to the created plugin instance as well. \sa setPluginPaths() -*/ + */ QList PluginManager::plugins() const { return d->pluginSpecs; @@ -327,30 +327,32 @@ QList PluginManager::plugins() const The caller (the application) may register itself for options via the \a appOptions list, containing pairs of "option string" and a bool that indicates if the option requires an argument. Application options always override any plugin's options. - + \a foundAppOptions is set to pairs of ("option string", "argument") for any application options that were found. The command line options that were not processed can be retrieved via the arguments() method. If an error occurred (like missing argument for an option that requires one), \a errorString contains a descriptive message of the error. - + Returns if there was an error. */ bool PluginManager::parseOptions(const QStringList &args, - const QMap &appOptions, - QMap *foundAppOptions, - QString *errorString) + const QMap &appOptions, + QMap *foundAppOptions, + QString *errorString) { OptionsParser options(args, appOptions, foundAppOptions, errorString, d); + return options.parse(); } - static inline void indent(QTextStream &str, int indent) { const QChar blank = QLatin1Char(' '); - for (int i = 0 ; i < indent; i++) + + for (int i = 0; i < indent; i++) { str << blank; + } } static inline void formatOption(QTextStream &str, @@ -358,6 +360,7 @@ static inline void formatOption(QTextStream &str, int optionIndentation, int descriptionIndentation) { int remainingIndent = descriptionIndentation - optionIndentation - opt.size(); + indent(str, optionIndentation); str << opt; if (!parm.isEmpty()) { @@ -372,7 +375,7 @@ static inline void formatOption(QTextStream &str, \fn static PluginManager::formatOptions(QTextStream &str, int optionIndentation, int descriptionIndentation) Format the startup options of the plugin manager for command line help. -*/ + */ void PluginManager::formatOptions(QTextStream &str, int optionIndentation, int descriptionIndentation) { @@ -385,7 +388,7 @@ void PluginManager::formatOptions(QTextStream &str, int optionIndentation, int d \fn PluginManager::formatPluginOptions(QTextStream &str, int optionIndentation, int descriptionIndentation) const Format the plugin options of the plugin specs for command line help. -*/ + */ void PluginManager::formatPluginOptions(QTextStream &str, int optionIndentation, int descriptionIndentation) const { @@ -395,10 +398,11 @@ void PluginManager::formatPluginOptions(QTextStream &str, int optionIndentation, for (PluginSpecSet::const_iterator pit = d->pluginSpecs.constBegin(); pit != pcend; ++pit) { const PluginArgumentDescriptions pargs = (*pit)->argumentDescriptions(); if (!pargs.empty()) { - str << "\nPlugin: " << (*pit)->name() << '\n'; + str << "\nPlugin: " << (*pit)->name() << '\n'; const PluginArgumentDescriptions::const_iterator acend = pargs.constEnd(); - for (PluginArgumentDescriptions::const_iterator ait =pargs.constBegin(); ait != acend; ++ait) + for (PluginArgumentDescriptions::const_iterator ait = pargs.constBegin(); ait != acend; ++ait) { formatOption(str, ait->name, ait->parameter, ait->description, optionIndentation, descriptionIndentation); + } } } } @@ -407,33 +411,34 @@ void PluginManager::formatPluginOptions(QTextStream &str, int optionIndentation, \fn PluginManager::formatPluginVersions(QTextStream &str) const Format the version of the plugin specs for command line help. -*/ + */ void PluginManager::formatPluginVersions(QTextStream &str) const { const PluginSpecSet::const_iterator cend = d->pluginSpecs.constEnd(); + for (PluginSpecSet::const_iterator it = d->pluginSpecs.constBegin(); it != cend; ++it) { const PluginSpec *ps = *it; - str << " " << ps->name() << ' ' << ps->version() << ' ' << ps->description() << '\n'; + str << " " << ps->name() << ' ' << ps->version() << ' ' << ps->description() << '\n'; } } void PluginManager::startTests() { #ifdef WITH_TESTS - foreach (PluginSpec *pluginSpec, d->testSpecs) { + foreach(PluginSpec * pluginSpec, d->testSpecs) { const QMetaObject *mo = pluginSpec->plugin()->metaObject(); QStringList methods; + methods.append("arg0"); // We only want slots starting with "test" for (int i = mo->methodOffset(); i < mo->methodCount(); ++i) { if (QByteArray(mo->method(i).signature()).startsWith("test")) { QString method = QString::fromLatin1(mo->method(i).signature()); - methods.append(method.left(method.size()-2)); + methods.append(method.left(method.size() - 2)); } } QTest::qExec(pluginSpec->plugin(), methods); - } #endif } @@ -454,7 +459,8 @@ bool PluginManager::runningTests() const QString PluginManager::testDataDirectory() const { QByteArray ba = qgetenv("QTCREATOR_TEST_DIR"); - QString s = QString::fromLocal8Bit(ba.constData(), ba.size()); + QString s = QString::fromLocal8Bit(ba.constData(), ba.size()); + if (s.isEmpty()) { s = GCS_TEST_DIR; s.append("/tests"); @@ -463,12 +469,12 @@ QString PluginManager::testDataDirectory() const return s; } -//============PluginManagerPrivate=========== +// ============PluginManagerPrivate=========== /*! \fn PluginSpec *PluginManagerPrivate::createSpec() \internal -*/ + */ PluginSpec *PluginManagerPrivate::createSpec() { return new PluginSpec(); @@ -477,7 +483,7 @@ PluginSpec *PluginManagerPrivate::createSpec() /*! \fn PluginSpecPrivate *PluginManagerPrivate::privateSpec(PluginSpec *spec) \internal -*/ + */ PluginSpecPrivate *PluginManagerPrivate::privateSpec(PluginSpec *spec) { return spec->d; @@ -486,16 +492,15 @@ PluginSpecPrivate *PluginManagerPrivate::privateSpec(PluginSpec *spec) /*! \fn PluginManagerPrivate::PluginManagerPrivate(PluginManager *pluginManager) \internal -*/ + */ PluginManagerPrivate::PluginManagerPrivate(PluginManager *pluginManager) : extension("xml"), q(pluginManager) -{ -} +{} /*! \fn PluginManagerPrivate::~PluginManagerPrivate() \internal -*/ + */ PluginManagerPrivate::~PluginManagerPrivate() { stopAll(); @@ -508,7 +513,7 @@ PluginManagerPrivate::~PluginManagerPrivate() void PluginManagerPrivate::stopAll() { QList queue = loadQueue(); - foreach (PluginSpec *spec, queue) { + foreach(PluginSpec * spec, queue) { loadPlugin(spec, PluginSpec::Stopped); } QListIterator it(queue); @@ -521,7 +526,7 @@ void PluginManagerPrivate::stopAll() /*! \fn void PluginManagerPrivate::addObject(QObject *obj) \internal -*/ + */ void PluginManagerPrivate::addObject(QObject *obj) { { @@ -535,8 +540,9 @@ void PluginManagerPrivate::addObject(QObject *obj) return; } - if (debugLeaks) + if (debugLeaks) { qDebug() << "PluginManagerPrivate::addObject" << obj << obj->objectName(); + } allObjects.append(obj); } @@ -546,7 +552,7 @@ void PluginManagerPrivate::addObject(QObject *obj) /*! \fn void PluginManagerPrivate::removeObject(QObject *obj) \internal -*/ + */ void PluginManagerPrivate::removeObject(QObject *obj) { if (obj == 0) { @@ -556,11 +562,12 @@ void PluginManagerPrivate::removeObject(QObject *obj) if (!allObjects.contains(obj)) { qWarning() << "PluginManagerPrivate::removeObject(): object not in list:" - << obj << obj->objectName(); + << obj << obj->objectName(); return; } - if (debugLeaks) + if (debugLeaks) { qDebug() << "PluginManagerPrivate::removeObject" << obj << obj->objectName(); + } emit q->aboutToRemoveObject(obj); QWriteLocker lock(&(q->m_lock)); @@ -570,36 +577,36 @@ void PluginManagerPrivate::removeObject(QObject *obj) /*! \fn void PluginManagerPrivate::loadPlugins() \internal -*/ + */ void PluginManagerPrivate::loadPlugins() { QList queue = loadQueue(); - foreach (PluginSpec *spec, queue) { + foreach(PluginSpec * spec, queue) { loadPlugin(spec, PluginSpec::Loaded); } - foreach (PluginSpec *spec, queue) { + foreach(PluginSpec * spec, queue) { loadPlugin(spec, PluginSpec::Initialized); } QListIterator it(queue); it.toBack(); while (it.hasPrevious()) { - PluginSpec* plugin = it.previous(); + PluginSpec *plugin = it.previous(); emit q->pluginAboutToBeLoaded(plugin); loadPlugin(plugin, PluginSpec::Running); } emit q->pluginsChanged(); - q->m_allPluginsLoaded=true; + q->m_allPluginsLoaded = true; emit q->pluginsLoadEnded(); } /*! \fn void PluginManagerPrivate::loadQueue() \internal -*/ + */ QList PluginManagerPrivate::loadQueue() { QList queue; - foreach (PluginSpec *spec, pluginSpecs) { + foreach(PluginSpec * spec, pluginSpecs) { QList circularityCheckQueue; loadQueue(spec, queue, circularityCheckQueue); } @@ -609,20 +616,21 @@ QList PluginManagerPrivate::loadQueue() /*! \fn bool PluginManagerPrivate::loadQueue(PluginSpec *spec, QList &queue, QList &circularityCheckQueue) \internal -*/ + */ bool PluginManagerPrivate::loadQueue(PluginSpec *spec, QList &queue, - QList &circularityCheckQueue) + QList &circularityCheckQueue) { - if (queue.contains(spec)) + if (queue.contains(spec)) { return true; + } // check for circular dependencies if (circularityCheckQueue.contains(spec)) { - spec->d->hasError = true; + spec->d->hasError = true; spec->d->errorString = PluginManager::tr("Circular dependency detected:\n"); int index = circularityCheckQueue.indexOf(spec); for (int i = index; i < circularityCheckQueue.size(); ++i) { spec->d->errorString.append(PluginManager::tr("%1(%2) depends on\n") - .arg(circularityCheckQueue.at(i)->name()).arg(circularityCheckQueue.at(i)->version())); + .arg(circularityCheckQueue.at(i)->name()).arg(circularityCheckQueue.at(i)->version())); } spec->d->errorString.append(PluginManager::tr("%1(%2)").arg(spec->name()).arg(spec->version())); return false; @@ -630,18 +638,18 @@ bool PluginManagerPrivate::loadQueue(PluginSpec *spec, QList &queu circularityCheckQueue.append(spec); // check if we have the dependencies if (spec->state() == PluginSpec::Invalid || spec->state() == PluginSpec::Read) { - spec->d->hasError = true; + spec->d->hasError = true; spec->d->errorString += "\n"; spec->d->errorString += PluginManager::tr("Cannot load plugin because dependencies are not resolved"); return false; } // add dependencies - foreach (PluginSpec *depSpec, spec->dependencySpecs()) { + foreach(PluginSpec * depSpec, spec->dependencySpecs()) { if (!loadQueue(depSpec, queue, circularityCheckQueue)) { - spec->d->hasError = true; + spec->d->hasError = true; spec->d->errorString = PluginManager::tr("Cannot load plugin because dependency failed to load: %1(%2)\nReason: %3") - .arg(depSpec->name()).arg(depSpec->version()).arg(depSpec->errorString()); + .arg(depSpec->name()).arg(depSpec->version()).arg(depSpec->errorString()); return false; } } @@ -653,11 +661,12 @@ bool PluginManagerPrivate::loadQueue(PluginSpec *spec, QList &queu /*! \fn void PluginManagerPrivate::loadPlugin(PluginSpec *spec, PluginSpec::State destState) \internal -*/ + */ void PluginManagerPrivate::loadPlugin(PluginSpec *spec, PluginSpec::State destState) { - if (spec->hasError()) + if (spec->hasError()) { return; + } if (destState == PluginSpec::Running) { spec->d->initializeExtensions(); return; @@ -665,27 +674,28 @@ void PluginManagerPrivate::loadPlugin(PluginSpec *spec, PluginSpec::State destSt spec->d->kill(); return; } - foreach (PluginSpec *depSpec, spec->dependencySpecs()) { + foreach(PluginSpec * depSpec, spec->dependencySpecs()) { if (depSpec->state() != destState) { - spec->d->hasError = true; + spec->d->hasError = true; spec->d->errorString = PluginManager::tr("Cannot load plugin because dependency failed to load: %1(%2)\nReason: %3") - .arg(depSpec->name()).arg(depSpec->version()).arg(depSpec->errorString()); + .arg(depSpec->name()).arg(depSpec->version()).arg(depSpec->errorString()); return; } } - if (destState == PluginSpec::Loaded) + if (destState == PluginSpec::Loaded) { spec->d->loadLibrary(); - else if (destState == PluginSpec::Initialized) + } else if (destState == PluginSpec::Initialized) { spec->d->initializePlugin(); - else if (destState == PluginSpec::Stopped) + } else if (destState == PluginSpec::Stopped) { spec->d->stop(); + } } /*! \fn void PluginManagerPrivate::setPluginPaths(const QStringList &paths) \internal -*/ + */ void PluginManagerPrivate::setPluginPaths(const QStringList &paths) { pluginPaths = paths; @@ -695,7 +705,7 @@ void PluginManagerPrivate::setPluginPaths(const QStringList &paths) /*! \fn void PluginManagerPrivate::readPluginPaths() \internal -*/ + */ void PluginManagerPrivate::readPluginPaths() { qDeleteAll(pluginSpecs); @@ -706,14 +716,15 @@ void PluginManagerPrivate::readPluginPaths() while (!searchPaths.isEmpty()) { const QDir dir(searchPaths.takeFirst()); const QFileInfoList files = dir.entryInfoList(QStringList() << QString("*.%1").arg(extension), QDir::Files); - foreach (const QFileInfo &file, files) - specFiles << file.absoluteFilePath(); - const QFileInfoList dirs = dir.entryInfoList(QDir::Dirs|QDir::NoDotAndDotDot); - foreach (const QFileInfo &subdir, dirs) - searchPaths << subdir.absoluteFilePath(); + foreach(const QFileInfo &file, files) + specFiles << file.absoluteFilePath(); + const QFileInfoList dirs = dir.entryInfoList(QDir::Dirs | QDir::NoDotAndDotDot); + foreach(const QFileInfo &subdir, dirs) + searchPaths << subdir.absoluteFilePath(); } - foreach (const QString &specFile, specFiles) { + foreach(const QString &specFile, specFiles) { PluginSpec *spec = new PluginSpec; + spec->d->read(specFile); pluginSpecs.append(spec); } @@ -725,12 +736,12 @@ void PluginManagerPrivate::readPluginPaths() void PluginManagerPrivate::resolveDependencies() { - foreach (PluginSpec *spec, pluginSpecs) { + foreach(PluginSpec * spec, pluginSpecs) { spec->d->resolveDependencies(pluginSpecs); } } - // Look in argument descriptions of the specs for the option. +// Look in argument descriptions of the specs for the option. PluginSpec *PluginManagerPrivate::pluginForOption(const QString &option, bool *requiresArgument) const { // Look in the plugins for an option @@ -756,9 +767,9 @@ PluginSpec *PluginManagerPrivate::pluginForOption(const QString &option, bool *r PluginSpec *PluginManagerPrivate::pluginByName(const QString &name) const { - foreach (PluginSpec *spec, pluginSpecs) - if (spec->name() == name) - return spec; + foreach(PluginSpec * spec, pluginSpecs) + if (spec->name() == name) { + return spec; + } return 0; } - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.h index c194c20fd..215ae4e0f 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.h @@ -4,25 +4,25 @@ * @file pluginmanager.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,22 +41,23 @@ class QTextStream; QT_END_NAMESPACE namespace ExtensionSystem { - namespace Internal { - class PluginManagerPrivate; +class PluginManagerPrivate; } class IPlugin; class PluginSpec; -class EXTENSIONSYSTEM_EXPORT PluginManager : public QObject -{ +class EXTENSIONSYSTEM_EXPORT PluginManager : public QObject { Q_DISABLE_COPY(PluginManager) Q_OBJECT public: static PluginManager *instance(); - bool allPluginsLoaded(){return m_allPluginsLoaded;} + bool allPluginsLoaded() + { + return m_allPluginsLoaded; + } PluginManager(); virtual ~PluginManager(); @@ -67,24 +68,28 @@ public: template QList getObjects() const { QReadLocker lock(&m_lock); + QList results; QList all = allObjects(); QList result; - foreach (QObject *obj, all) { + foreach(QObject * obj, all) { result = Aggregation::query_all(obj); - if (!result.isEmpty()) + if (!result.isEmpty()) { results += result; + } } return results; } template T *getObject() const { QReadLocker lock(&m_lock); + QList all = allObjects(); T *result = 0; - foreach (QObject *obj, all) { - if ((result = Aggregation::query(obj)) != 0) + foreach(QObject * obj, all) { + if ((result = Aggregation::query(obj)) != 0) { break; + } } return result; } @@ -100,9 +105,9 @@ public: // command line arguments QStringList arguments() const; bool parseOptions(const QStringList &args, - const QMap &appOptions, - QMap *foundAppOptions, - QString *errorString); + const QMap &appOptions, + QMap *foundAppOptions, + QString *errorString); static void formatOptions(QTextStream &str, int optionIndentation, int descriptionIndentation); void formatPluginOptions(QTextStream &str, int optionIndentation, int descriptionIndentation) const; void formatPluginVersions(QTextStream &str) const; @@ -114,7 +119,7 @@ signals: void objectAdded(QObject *obj); void aboutToRemoveObject(QObject *obj); - void pluginAboutToBeLoaded(ExtensionSystem::PluginSpec* pluginSpec); + void pluginAboutToBeLoaded(ExtensionSystem::PluginSpec *pluginSpec); void pluginsChanged(); void pluginsLoadEnded(); private slots: @@ -128,7 +133,6 @@ private: friend class Internal::PluginManagerPrivate; }; - } // namespace ExtensionSystem #endif // EXTENSIONSYSTEM_PLUGINMANAGER_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager_p.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager_p.h index cea427806..29b8e78f0 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager_p.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager_p.h @@ -4,25 +4,25 @@ * @file pluginmanager_p.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,15 +37,12 @@ #include namespace ExtensionSystem { - class PluginManager; namespace Internal { - class PluginSpecPrivate; -class EXTENSIONSYSTEM_EXPORT PluginManagerPrivate -{ +class EXTENSIONSYSTEM_EXPORT PluginManagerPrivate { public: PluginManagerPrivate(PluginManager *pluginManager); virtual ~PluginManagerPrivate(); @@ -81,11 +78,10 @@ private: void readPluginPaths(); bool loadQueue(PluginSpec *spec, - QList &queue, - QList &circularityCheckQueue); + QList &queue, + QList &circularityCheckQueue); void stopAll(); }; - } // namespace Internal } // namespace ExtensionSystem diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.cpp b/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.cpp index f3615595e..1cc6dd8f8 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.cpp @@ -4,25 +4,25 @@ * @file pluginspec.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -50,12 +50,12 @@ #if USE_UNPATCHED_QPLUGINLOADER # include - typedef QT_PREPEND_NAMESPACE(QPluginLoader) PluginLoader; +typedef QT_PREPEND_NAMESPACE (QPluginLoader) PluginLoader; #else # include "patchedpluginloader.cpp" - typedef PatchedPluginLoader PluginLoader; +typedef PatchedPluginLoader PluginLoader; #endif @@ -69,17 +69,17 @@ See also ExtensionSystem::IPlugin for more information about plugin dependencies and version matching. -*/ + */ /*! \variable ExtensionSystem::PluginDependency::name String identifier of the plugin. -*/ + */ /*! \variable ExtensionSystem::PluginDependency::version Version string that a plugin must match to fill this dependency. -*/ + */ /*! \class ExtensionSystem::PluginSpec @@ -90,7 +90,7 @@ goes through its loading process (see PluginSpec::State). If an error occurs, the plugin spec is the place to look for the error details. -*/ + */ /*! \enum ExtensionSystem::PluginSpec::State @@ -120,14 +120,14 @@ The plugin has been shut down, i.e. the plugin's IPlugin::shutdown() method has been called. \value Deleted The plugin instance has been deleted. -*/ + */ using namespace ExtensionSystem; using namespace ExtensionSystem::Internal; /*! \fn bool PluginDependency::operator==(const PluginDependency &other) \internal -*/ + */ bool PluginDependency::operator==(const PluginDependency &other) { return name == other.name && version == other.version; @@ -136,16 +136,15 @@ bool PluginDependency::operator==(const PluginDependency &other) /*! \fn PluginSpec::PluginSpec() \internal -*/ + */ PluginSpec::PluginSpec() : d(new PluginSpecPrivate(this)) -{ -} +{} /*! \fn PluginSpec::~PluginSpec() \internal -*/ + */ PluginSpec::~PluginSpec() { delete d; @@ -155,7 +154,7 @@ PluginSpec::~PluginSpec() /*! \fn QString PluginSpec::name() const The plugin name. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::name() const { return d->name; @@ -164,7 +163,7 @@ QString PluginSpec::name() const /*! \fn QString PluginSpec::version() const The plugin version. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::version() const { return d->version; @@ -173,7 +172,7 @@ QString PluginSpec::version() const /*! \fn QString PluginSpec::compatVersion() const The plugin compatibility version. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::compatVersion() const { return d->compatVersion; @@ -182,7 +181,7 @@ QString PluginSpec::compatVersion() const /*! \fn QString PluginSpec::vendor() const The plugin vendor. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::vendor() const { return d->vendor; @@ -191,7 +190,7 @@ QString PluginSpec::vendor() const /*! \fn QString PluginSpec::copyright() const The plugin copyright. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::copyright() const { return d->copyright; @@ -200,7 +199,7 @@ QString PluginSpec::copyright() const /*! \fn QString PluginSpec::license() const The plugin license. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::license() const { return d->license; @@ -209,7 +208,7 @@ QString PluginSpec::license() const /*! \fn QString PluginSpec::description() const The plugin description. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::description() const { return d->description; @@ -218,7 +217,7 @@ QString PluginSpec::description() const /*! \fn QString PluginSpec::url() const The plugin url where you can find more information about the plugin. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::url() const { return d->url; @@ -227,7 +226,7 @@ QString PluginSpec::url() const /*! \fn QList PluginSpec::dependencies() const The plugin dependencies. This is valid after the PluginSpec::Read state is reached. -*/ + */ QList PluginSpec::dependencies() const { return d->dependencies; @@ -236,7 +235,7 @@ QList PluginSpec::dependencies() const /*! \fn PluginSpec::PluginArgumentDescriptions PluginSpec::argumentDescriptions() const Returns a list of descriptions of command line arguments the plugin processes. -*/ + */ PluginSpec::PluginArgumentDescriptions PluginSpec::argumentDescriptions() const { @@ -247,7 +246,7 @@ PluginSpec::PluginArgumentDescriptions PluginSpec::argumentDescriptions() const \fn QString PluginSpec::location() const The absolute path to the directory containing the plugin xml description file this PluginSpec corresponds to. -*/ + */ QString PluginSpec::location() const { return d->location; @@ -257,7 +256,7 @@ QString PluginSpec::location() const \fn QString PluginSpec::filePath() const The absolute path to the plugin xml description file (including the file name) this PluginSpec corresponds to. -*/ + */ QString PluginSpec::filePath() const { return d->filePath; @@ -266,7 +265,7 @@ QString PluginSpec::filePath() const /*! \fn QStringList PluginSpec::arguments() const Command line arguments specific to that plugin. Set at startup -*/ + */ QStringList PluginSpec::arguments() const { @@ -276,7 +275,7 @@ QStringList PluginSpec::arguments() const /*! \fn void PluginSpec::setArguments(const QStringList &arguments) Set the command line arguments specific to that plugin to \a arguments. -*/ + */ void PluginSpec::setArguments(const QStringList &arguments) { @@ -286,7 +285,7 @@ void PluginSpec::setArguments(const QStringList &arguments) /*! \fn PluginSpec::addArgument(const QString &argument) Adds \a argument to the command line arguments specific to that plugin. -*/ + */ void PluginSpec::addArgument(const QString &argument) { @@ -298,7 +297,7 @@ void PluginSpec::addArgument(const QString &argument) \fn PluginSpec::State PluginSpec::state() const The state in which the plugin currently is. See the description of the PluginSpec::State enum for details. -*/ + */ PluginSpec::State PluginSpec::state() const { return d->state; @@ -307,7 +306,7 @@ PluginSpec::State PluginSpec::state() const /*! \fn bool PluginSpec::hasError() const Returns whether an error occurred while reading/starting the plugin. -*/ + */ bool PluginSpec::hasError() const { return d->hasError; @@ -316,7 +315,7 @@ bool PluginSpec::hasError() const /*! \fn QString PluginSpec::errorString() const Detailed, possibly multi-line, error description in case of an error. -*/ + */ QString PluginSpec::errorString() const { return d->errorString; @@ -328,7 +327,7 @@ QString PluginSpec::errorString() const \a pluginName and \a version. \sa PluginSpec::dependencies() -*/ + */ bool PluginSpec::provides(const QString &pluginName, const QString &version) const { return d->provides(pluginName, version); @@ -338,7 +337,7 @@ bool PluginSpec::provides(const QString &pluginName, const QString &version) con \fn IPlugin *PluginSpec::plugin() const The corresponding IPlugin instance, if the plugin library has already been successfully loaded, i.e. the PluginSpec::Loaded state is reached. -*/ + */ IPlugin *PluginSpec::plugin() const { return d->plugin; @@ -350,70 +349,71 @@ IPlugin *PluginSpec::plugin() const Valid if PluginSpec::Resolved state is reached. \sa PluginSpec::dependencies() -*/ + */ QList PluginSpec::dependencySpecs() const { return d->dependencySpecs; } -//==========PluginSpecPrivate================== +// ==========PluginSpecPrivate================== namespace { - const char * const PLUGIN = "plugin"; - const char * const PLUGIN_NAME = "name"; - const char * const PLUGIN_VERSION = "version"; - const char * const PLUGIN_COMPATVERSION = "compatVersion"; - const char * const VENDOR = "vendor"; - const char * const COPYRIGHT = "copyright"; - const char * const LICENSE = "license"; - const char * const DESCRIPTION = "description"; - const char * const URL = "url"; - const char * const DEPENDENCYLIST = "dependencyList"; - const char * const DEPENDENCY = "dependency"; - const char * const DEPENDENCY_NAME = "name"; - const char * const DEPENDENCY_VERSION = "version"; - const char * const ARGUMENTLIST = "argumentList"; - const char * const ARGUMENT = "argument"; - const char * const ARGUMENT_NAME = "name"; - const char * const ARGUMENT_PARAMETER = "parameter"; +const char *const PLUGIN = "plugin"; +const char *const PLUGIN_NAME = "name"; +const char *const PLUGIN_VERSION = "version"; +const char *const PLUGIN_COMPATVERSION = "compatVersion"; +const char *const VENDOR = "vendor"; +const char *const COPYRIGHT = "copyright"; +const char *const LICENSE = "license"; +const char *const DESCRIPTION = "description"; +const char *const URL = "url"; +const char *const DEPENDENCYLIST = "dependencyList"; +const char *const DEPENDENCY = "dependency"; +const char *const DEPENDENCY_NAME = "name"; +const char *const DEPENDENCY_VERSION = "version"; +const char *const ARGUMENTLIST = "argumentList"; +const char *const ARGUMENT = "argument"; +const char *const ARGUMENT_NAME = "name"; +const char *const ARGUMENT_PARAMETER = "parameter"; } /*! \fn PluginSpecPrivate::PluginSpecPrivate(PluginSpec *spec) \internal -*/ + */ PluginSpecPrivate::PluginSpecPrivate(PluginSpec *spec) : plugin(0), state(PluginSpec::Invalid), hasError(false), q(spec) -{ -} +{} /*! \fn bool PluginSpecPrivate::read(const QString &fileName) \internal -*/ + */ bool PluginSpecPrivate::read(const QString &fileName) { name = version - = compatVersion - = vendor - = copyright - = license - = description - = url - = location - = ""; - state = PluginSpec::Invalid; - hasError = false; + = compatVersion + = vendor + = copyright + = license + = description + = url + = location + = ""; + state = PluginSpec::Invalid; + hasError = false; errorString = ""; dependencies.clear(); QFile file(fileName); - if (!file.exists()) + if (!file.exists()) { return reportError(tr("File does not exist: %1").arg(file.fileName())); - if (!file.open(QIODevice::ReadOnly)) + } + if (!file.open(QIODevice::ReadOnly)) { return reportError(tr("Could not open file for read: %1").arg(file.fileName())); + } QFileInfo fileInfo(file); location = fileInfo.absolutePath(); filePath = fileInfo.absoluteFilePath(); @@ -428,12 +428,13 @@ bool PluginSpecPrivate::read(const QString &fileName) break; } } - if (reader.hasError()) + if (reader.hasError()) { return reportError(tr("Error parsing file %1: %2, at line %3, column %4") - .arg(file.fileName()) - .arg(reader.errorString()) - .arg(reader.lineNumber()) - .arg(reader.columnNumber())); + .arg(file.fileName()) + .arg(reader.errorString()) + .arg(reader.lineNumber()) + .arg(reader.columnNumber())); + } state = PluginSpec::Read; return true; } @@ -441,11 +442,11 @@ bool PluginSpecPrivate::read(const QString &fileName) /*! \fn bool PluginSpecPrivate::reportError(const QString &err) \internal -*/ + */ bool PluginSpecPrivate::reportError(const QString &err) { errorString = err; - hasError = true; + hasError = true; return false; } @@ -477,10 +478,11 @@ static inline QString msgUnexpectedToken() /*! \fn void PluginSpecPrivate::readPluginSpec(QXmlStreamReader &reader) \internal -*/ + */ void PluginSpecPrivate::readPluginSpec(QXmlStreamReader &reader) { QString element = reader.name().toString(); + if (element != QString(PLUGIN)) { reader.raiseError(QCoreApplication::translate("PluginSpec", "Expected element '%1' as top level element").arg(PLUGIN)); return; @@ -511,22 +513,23 @@ void PluginSpecPrivate::readPluginSpec(QXmlStreamReader &reader) switch (reader.tokenType()) { case QXmlStreamReader::StartElement: element = reader.name().toString(); - if (element == VENDOR) + if (element == VENDOR) { vendor = reader.readElementText().trimmed(); - else if (element == COPYRIGHT) + } else if (element == COPYRIGHT) { copyright = reader.readElementText().trimmed(); - else if (element == LICENSE) + } else if (element == LICENSE) { license = reader.readElementText().trimmed(); - else if (element == DESCRIPTION) + } else if (element == DESCRIPTION) { description = reader.readElementText().trimmed(); - else if (element == URL) + } else if (element == URL) { url = reader.readElementText().trimmed(); - else if (element == DEPENDENCYLIST) + } else if (element == DEPENDENCYLIST) { readDependencies(reader); - else if (element == ARGUMENTLIST) + } else if (element == ARGUMENTLIST) { readArgumentDescriptions(reader); - else + } else { reader.raiseError(msgInvalidElement(name)); + } break; case QXmlStreamReader::EndDocument: case QXmlStreamReader::Comment: @@ -543,11 +546,12 @@ void PluginSpecPrivate::readPluginSpec(QXmlStreamReader &reader) /*! \fn void PluginSpecPrivate::readArgumentDescriptions(QXmlStreamReader &reader) \internal -*/ + */ void PluginSpecPrivate::readArgumentDescriptions(QXmlStreamReader &reader) { QString element; + while (!reader.atEnd()) { reader.readNext(); switch (reader.tokenType()) { @@ -564,8 +568,9 @@ void PluginSpecPrivate::readArgumentDescriptions(QXmlStreamReader &reader) break; case QXmlStreamReader::EndElement: element = reader.name().toString(); - if (element == ARGUMENTLIST) + if (element == ARGUMENTLIST) { return; + } reader.raiseError(msgUnexpectedClosing(element)); break; default: @@ -578,29 +583,32 @@ void PluginSpecPrivate::readArgumentDescriptions(QXmlStreamReader &reader) /*! \fn void PluginSpecPrivate::readArgumentDescription(QXmlStreamReader &reader) \internal -*/ + */ void PluginSpecPrivate::readArgumentDescription(QXmlStreamReader &reader) { PluginArgumentDescription arg; + arg.name = reader.attributes().value(ARGUMENT_NAME).toString(); if (arg.name.isEmpty()) { reader.raiseError(msgAttributeMissing(ARGUMENT, ARGUMENT_NAME)); return; } - arg.parameter = reader.attributes().value(ARGUMENT_PARAMETER).toString(); + arg.parameter = reader.attributes().value(ARGUMENT_PARAMETER).toString(); arg.description = reader.readElementText(); - if (reader.tokenType() != QXmlStreamReader::EndElement) + if (reader.tokenType() != QXmlStreamReader::EndElement) { reader.raiseError(msgUnexpectedToken()); + } argumentDescriptions.push_back(arg); } /*! \fn void PluginSpecPrivate::readDependencies(QXmlStreamReader &reader) \internal -*/ + */ void PluginSpecPrivate::readDependencies(QXmlStreamReader &reader) { QString element; + while (!reader.atEnd()) { reader.readNext(); switch (reader.tokenType()) { @@ -617,8 +625,9 @@ void PluginSpecPrivate::readDependencies(QXmlStreamReader &reader) break; case QXmlStreamReader::EndElement: element = reader.name().toString(); - if (element == DEPENDENCYLIST) + if (element == DEPENDENCYLIST) { return; + } reader.raiseError(msgUnexpectedClosing(element)); break; default: @@ -631,10 +640,11 @@ void PluginSpecPrivate::readDependencies(QXmlStreamReader &reader) /*! \fn void PluginSpecPrivate::readDependencyEntry(QXmlStreamReader &reader) \internal -*/ + */ void PluginSpecPrivate::readDependencyEntry(QXmlStreamReader &reader) { PluginDependency dep; + dep.name = reader.attributes().value(DEPENDENCY_NAME).toString(); if (dep.name.isEmpty()) { reader.raiseError(msgAttributeMissing(DEPENDENCY, DEPENDENCY_NAME)); @@ -647,34 +657,37 @@ void PluginSpecPrivate::readDependencyEntry(QXmlStreamReader &reader) } dependencies.append(dep); reader.readNext(); - if (reader.tokenType() != QXmlStreamReader::EndElement) + if (reader.tokenType() != QXmlStreamReader::EndElement) { reader.raiseError(msgUnexpectedToken()); + } } /*! \fn bool PluginSpecPrivate::provides(const QString &pluginName, const QString &pluginVersion) const \internal -*/ + */ bool PluginSpecPrivate::provides(const QString &pluginName, const QString &pluginVersion) const { - if (QString::compare(pluginName, name, Qt::CaseInsensitive) != 0) + if (QString::compare(pluginName, name, Qt::CaseInsensitive) != 0) { return false; + } return (versionCompare(version, pluginVersion) >= 0) && (versionCompare(compatVersion, pluginVersion) <= 0); } /*! \fn QRegExp &PluginSpecPrivate::versionRegExp() \internal -*/ + */ QRegExp &PluginSpecPrivate::versionRegExp() { static QRegExp reg("([0-9]+)(?:[.]([0-9]+))?(?:[.]([0-9]+))?(?:_([0-9]+))?"); + return reg; } /*! \fn bool PluginSpecPrivate::isValidVersion(const QString &version) \internal -*/ + */ bool PluginSpecPrivate::isValidVersion(const QString &version) { return versionRegExp().exactMatch(version); @@ -683,24 +696,29 @@ bool PluginSpecPrivate::isValidVersion(const QString &version) /*! \fn int PluginSpecPrivate::versionCompare(const QString &version1, const QString &version2) \internal -*/ + */ int PluginSpecPrivate::versionCompare(const QString &version1, const QString &version2) { QRegExp reg1 = versionRegExp(); QRegExp reg2 = versionRegExp(); - if (!reg1.exactMatch(version1)) + + if (!reg1.exactMatch(version1)) { return 0; - if (!reg2.exactMatch(version2)) + } + if (!reg2.exactMatch(version2)) { return 0; + } int number1; int number2; for (int i = 0; i < 4; ++i) { - number1 = reg1.cap(i+1).toInt(); - number2 = reg2.cap(i+1).toInt(); - if (number1 < number2) + number1 = reg1.cap(i + 1).toInt(); + number2 = reg2.cap(i + 1).toInt(); + if (number1 < number2) { return -1; - if (number1 > number2) + } + if (number1 > number2) { return 1; + } } return 0; } @@ -708,22 +726,25 @@ int PluginSpecPrivate::versionCompare(const QString &version1, const QString &ve /*! \fn bool PluginSpecPrivate::resolveDependencies(const QList &specs) \internal -*/ + */ bool PluginSpecPrivate::resolveDependencies(const QList &specs) { - if (hasError) + if (hasError) { return false; - if (state == PluginSpec::Resolved) + } + if (state == PluginSpec::Resolved) { state = PluginSpec::Read; // Go back, so we just re-resolve the dependencies. + } if (state != PluginSpec::Read) { errorString = QCoreApplication::translate("PluginSpec", "Resolving dependencies failed because state != Read"); - hasError = true; + hasError = true; return false; } QList resolvedDependencies; - foreach (const PluginDependency &dependency, dependencies) { + foreach(const PluginDependency &dependency, dependencies) { PluginSpec *found = 0; - foreach (PluginSpec *spec, specs) { + + foreach(PluginSpec * spec, specs) { if (spec->provides(dependency.name, dependency.version)) { found = spec; break; @@ -731,16 +752,18 @@ bool PluginSpecPrivate::resolveDependencies(const QList &specs) } if (!found) { hasError = true; - if (!errorString.isEmpty()) + if (!errorString.isEmpty()) { errorString.append("\n"); + } errorString.append(QCoreApplication::translate("PluginSpec", "Could not resolve dependency '%1(%2)'") - .arg(dependency.name).arg(dependency.version)); + .arg(dependency.name).arg(dependency.version)); continue; } resolvedDependencies.append(found); } - if (hasError) + if (hasError) { return false; + } dependencySpecs = resolvedDependencies; state = PluginSpec::Resolved; return true; @@ -749,16 +772,18 @@ bool PluginSpecPrivate::resolveDependencies(const QList &specs) /*! \fn bool PluginSpecPrivate::loadLibrary() \internal -*/ + */ bool PluginSpecPrivate::loadLibrary() { - if (hasError) + if (hasError) { return false; + } if (state != PluginSpec::Resolved) { - if (state == PluginSpec::Loaded) + if (state == PluginSpec::Loaded) { return true; + } errorString = QCoreApplication::translate("PluginSpec", "Loading the library failed because state != Resolved"); - hasError = true; + hasError = true; return false; } #ifdef QT_NO_DEBUG @@ -771,7 +796,7 @@ bool PluginSpecPrivate::loadLibrary() QString libName = QString("%1/lib%2.so").arg(location).arg(name); #endif -#else //Q_NO_DEBUG +#else // Q_NO_DEBUG #ifdef Q_OS_WIN QString libName = QString("%1/%2d.dll").arg(location).arg(name); @@ -785,18 +810,18 @@ bool PluginSpecPrivate::loadLibrary() PluginLoader loader(libName); if (!loader.load()) { - hasError = true; + hasError = true; errorString = libName + QString::fromLatin1(": ") + loader.errorString(); return false; } - IPlugin *pluginObject = qobject_cast(loader.instance()); + IPlugin *pluginObject = qobject_cast(loader.instance()); if (!pluginObject) { - hasError = true; + hasError = true; errorString = QCoreApplication::translate("PluginSpec", "Plugin is not valid (does not derive from IPlugin)"); loader.unload(); return false; } - state = PluginSpec::Loaded; + state = PluginSpec::Loaded; plugin = pluginObject; plugin->d->pluginSpec = q; return true; @@ -805,27 +830,29 @@ bool PluginSpecPrivate::loadLibrary() /*! \fn bool PluginSpecPrivate::initializePlugin() \internal -*/ + */ bool PluginSpecPrivate::initializePlugin() { - if (hasError) + if (hasError) { return false; + } if (state != PluginSpec::Loaded) { - if (state == PluginSpec::Initialized) + if (state == PluginSpec::Initialized) { return true; + } errorString = QCoreApplication::translate("PluginSpec", "Initializing the plugin failed because state != Loaded"); - hasError = true; + hasError = true; return false; } if (!plugin) { errorString = QCoreApplication::translate("PluginSpec", "Internal error: have no plugin instance to initialize"); - hasError = true; + hasError = true; return false; } QString err; if (!plugin->initialize(arguments, &err)) { errorString = QCoreApplication::translate("PluginSpec", "Plugin initialization failed: %1").arg(err); - hasError = true; + hasError = true; return false; } state = PluginSpec::Initialized; @@ -835,21 +862,23 @@ bool PluginSpecPrivate::initializePlugin() /*! \fn bool PluginSpecPrivate::initializeExtensions() \internal -*/ + */ bool PluginSpecPrivate::initializeExtensions() { - if (hasError) + if (hasError) { return false; + } if (state != PluginSpec::Initialized) { - if (state == PluginSpec::Running) + if (state == PluginSpec::Running) { return true; + } errorString = QCoreApplication::translate("PluginSpec", "Cannot perform extensionsInitialized because state != Initialized"); - hasError = true; + hasError = true; return false; } if (!plugin) { errorString = QCoreApplication::translate("PluginSpec", "Internal error: have no plugin instance to perform extensionsInitialized"); - hasError = true; + hasError = true; return false; } plugin->extensionsInitialized(); @@ -860,11 +889,12 @@ bool PluginSpecPrivate::initializeExtensions() /*! \fn bool PluginSpecPrivate::stop() \internal -*/ + */ void PluginSpecPrivate::stop() { - if (!plugin) + if (!plugin) { return; + } plugin->shutdown(); state = PluginSpec::Stopped; } @@ -872,13 +902,13 @@ void PluginSpecPrivate::stop() /*! \fn bool PluginSpecPrivate::kill() \internal -*/ + */ void PluginSpecPrivate::kill() { - if (!plugin) + if (!plugin) { return; + } delete plugin; plugin = 0; - state = PluginSpec::Deleted; + state = PluginSpec::Deleted; } - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.h index f226d6021..846bb4647 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.h @@ -4,25 +4,25 @@ * @file pluginspec.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,32 +39,28 @@ class QStringList; QT_END_NAMESPACE namespace ExtensionSystem { - namespace Internal { - class PluginSpecPrivate; - class PluginManagerPrivate; +class PluginSpecPrivate; +class PluginManagerPrivate; } class IPlugin; -struct EXTENSIONSYSTEM_EXPORT PluginDependency -{ +struct EXTENSIONSYSTEM_EXPORT PluginDependency { QString name; QString version; bool operator==(const PluginDependency &other); }; -struct EXTENSIONSYSTEM_EXPORT PluginArgumentDescription -{ +struct EXTENSIONSYSTEM_EXPORT PluginArgumentDescription { QString name; QString parameter; QString description; }; -class EXTENSIONSYSTEM_EXPORT PluginSpec -{ +class EXTENSIONSYSTEM_EXPORT PluginSpec { public: - enum State { Invalid, Read, Resolved, Loaded, Initialized, Running, Stopped, Deleted}; + enum State { Invalid, Read, Resolved, Loaded, Initialized, Running, Stopped, Deleted }; ~PluginSpec(); @@ -109,8 +105,6 @@ private: Internal::PluginSpecPrivate *d; friend class Internal::PluginManagerPrivate; }; - } // namespace ExtensionSystem #endif // PLUGINSPEC_H - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginspec_p.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginspec_p.h index 912364572..228ed29f2 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginspec_p.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginspec_p.h @@ -4,25 +4,25 @@ * @file pluginspec_p.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,14 +36,11 @@ #include namespace ExtensionSystem { - class IPlugin; class PluginManager; namespace Internal { - -class EXTENSIONSYSTEM_EXPORT PluginSpecPrivate : public QObject -{ +class EXTENSIONSYSTEM_EXPORT PluginSpecPrivate : public QObject { Q_OBJECT public: @@ -95,7 +92,6 @@ private: static QRegExp &versionRegExp(); }; - } // namespace Internal } // namespace ExtensionSystem diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginview.cpp b/ground/openpilotgcs/src/libs/extensionsystem/pluginview.cpp index 4fe0decef..6d5daffc2 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginview.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginview.cpp @@ -4,25 +4,25 @@ * @file pluginview.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -47,33 +47,32 @@ \sa ExtensionSystem::PluginDetailsView \sa ExtensionSystem::PluginErrorView -*/ + */ /*! \fn void PluginView::currentPluginChanged(ExtensionSystem::PluginSpec *spec) The current selection in the plugin list has changed to the plugin corresponding to \a spec. -*/ + */ /*! \fn void PluginView::pluginActivated(ExtensionSystem::PluginSpec *spec) The plugin list entry corresponding to \a spec has been activated, e.g. by a double-click. -*/ + */ using namespace ExtensionSystem; -Q_DECLARE_METATYPE(ExtensionSystem::PluginSpec*) - +Q_DECLARE_METATYPE(ExtensionSystem::PluginSpec *) /*! \fn PluginView::PluginView(PluginManager *manager, QWidget *parent) Constructs a PluginView that gets the list of plugins from the given plugin \a manager with a given \a parent widget. -*/ + */ PluginView::PluginView(PluginManager *manager, QWidget *parent) - : QWidget(parent), - m_ui(new Internal::Ui::PluginView), - p(new Internal::PluginViewPrivate) + : QWidget(parent), + m_ui(new Internal::Ui::PluginView), + p(new Internal::PluginViewPrivate) { m_ui->setupUi(this); QHeaderView *header = m_ui->pluginWidget->header(); @@ -83,17 +82,17 @@ PluginView::PluginView(PluginManager *manager, QWidget *parent) m_ui->pluginWidget->sortItems(1, Qt::AscendingOrder); p->manager = manager; connect(p->manager, SIGNAL(pluginsChanged()), this, SLOT(updateList())); - connect(m_ui->pluginWidget, SIGNAL(currentItemChanged(QTreeWidgetItem*,QTreeWidgetItem*)), - this, SLOT(selectPlugin(QTreeWidgetItem*))); - connect(m_ui->pluginWidget, SIGNAL(itemActivated(QTreeWidgetItem*,int)), - this, SLOT(activatePlugin(QTreeWidgetItem*))); + connect(m_ui->pluginWidget, SIGNAL(currentItemChanged(QTreeWidgetItem *, QTreeWidgetItem *)), + this, SLOT(selectPlugin(QTreeWidgetItem *))); + connect(m_ui->pluginWidget, SIGNAL(itemActivated(QTreeWidgetItem *, int)), + this, SLOT(activatePlugin(QTreeWidgetItem *))); updateList(); } /*! \fn PluginView::~PluginView() \internal -*/ + */ PluginView::~PluginView() { delete p; @@ -103,11 +102,12 @@ PluginView::~PluginView() /*! \fn PluginSpec *PluginView::currentPlugin() const Returns the current selection in the list of plugins. -*/ + */ PluginSpec *PluginView::currentPlugin() const { - if (!m_ui->pluginWidget->currentItem()) + if (!m_ui->pluginWidget->currentItem()) { return 0; + } return m_ui->pluginWidget->currentItem()->data(0, Qt::UserRole).value(); } @@ -115,42 +115,47 @@ void PluginView::updateList() { static QIcon okIcon(":/extensionsystem/images/ok.png"); static QIcon errorIcon(":/extensionsystem/images/error.png"); + QList items; QTreeWidgetItem *currentItem = 0; PluginSpec *currPlugin = currentPlugin(); - foreach (PluginSpec *spec, p->manager->plugins()) { + foreach(PluginSpec * spec, p->manager->plugins()) { QTreeWidgetItem *item = new QTreeWidgetItem(QStringList() - << "" - << spec->name() - << QString("%1 (%2)").arg(spec->version()).arg(spec->compatVersion()) - << spec->vendor() - << QDir::toNativeSeparators(spec->filePath())); + << "" + << spec->name() + << QString("%1 (%2)").arg(spec->version()).arg(spec->compatVersion()) + << spec->vendor() + << QDir::toNativeSeparators(spec->filePath())); + item->setToolTip(4, QDir::toNativeSeparators(spec->filePath())); item->setIcon(0, spec->hasError() ? errorIcon : okIcon); item->setData(0, Qt::UserRole, qVariantFromValue(spec)); items.append(item); - if (currPlugin == spec) + if (currPlugin == spec) { currentItem = item; + } } m_ui->pluginWidget->clear(); - if (!items.isEmpty()) + if (!items.isEmpty()) { m_ui->pluginWidget->addTopLevelItems(items); - if (currentItem) + } + if (currentItem) { m_ui->pluginWidget->setCurrentItem(currentItem); - else if (!items.isEmpty()) + } else if (!items.isEmpty()) { m_ui->pluginWidget->setCurrentItem(items.first()); + } } void PluginView::selectPlugin(QTreeWidgetItem *current) { - if (!current) + if (!current) { emit currentPluginChanged(0); - else + } else { emit currentPluginChanged(current->data(0, Qt::UserRole).value()); + } } void PluginView::activatePlugin(QTreeWidgetItem *item) { emit pluginActivated(item->data(0, Qt::UserRole).value()); } - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginview.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginview.h index b070218a9..f1d49eb6f 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginview.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginview.h @@ -4,25 +4,25 @@ * @file pluginview.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,19 +38,17 @@ class QTreeWidgetItem; QT_END_NAMESPACE namespace ExtensionSystem { - class PluginManager; class PluginSpec; namespace Internal { - class PluginViewPrivate; +class PluginViewPrivate; namespace Ui { - class PluginView; +class PluginView; } // namespace Ui } // namespace Internal -class EXTENSIONSYSTEM_EXPORT PluginView : public QWidget -{ +class EXTENSIONSYSTEM_EXPORT PluginView : public QWidget { Q_OBJECT public: @@ -72,7 +70,6 @@ private: Internal::Ui::PluginView *m_ui; Internal::PluginViewPrivate *p; }; - } // namespae ExtensionSystem #endif // PLUGIN_VIEW_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginview_p.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginview_p.h index c0bbee9c4..d00eedbcf 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginview_p.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginview_p.h @@ -4,25 +4,25 @@ * @file pluginview_p.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,17 +30,13 @@ #define PLUGINVIEW_P_H namespace ExtensionSystem { - class PluginManager; namespace Internal { - -class PluginViewPrivate -{ +class PluginViewPrivate { public: PluginManager *manager; }; - } // namespace Internal } // namespace ExtensionSystem diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.cpp index 6bbc210b5..67188bae9 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.cpp @@ -4,25 +4,25 @@ * @file plugin1.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,8 +33,7 @@ using namespace Plugin1; MyPlugin1::MyPlugin1() -{ -} +{} bool MyPlugin1::initialize(const QStringList &arguments, QString *errorString) { @@ -44,8 +43,6 @@ bool MyPlugin1::initialize(const QStringList &arguments, QString *errorString) } void MyPlugin1::extensionsInitialized() -{ -} +{} Q_EXPORT_PLUGIN(MyPlugin1) - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.h index b765ec016..6d656c3bf 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.h @@ -4,25 +4,25 @@ * @file plugin1.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ #include namespace Plugin1 { - -class MyPlugin1 : public ExtensionSystem::IPlugin -{ +class MyPlugin1 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -45,7 +43,6 @@ public: bool initialize(const QStringList &arguments, QString *errorString); void extensionsInitialized(); }; - } // namespace Plugin1 #endif // PLUGIN1_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.cpp index f2ce38e8f..4bbf5513a 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.cpp @@ -4,25 +4,25 @@ * @file plugin2.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,8 +33,7 @@ using namespace Plugin2; MyPlugin2::MyPlugin2() -{ -} +{} bool MyPlugin2::initialize(const QStringList &, QString *) { @@ -42,8 +41,6 @@ bool MyPlugin2::initialize(const QStringList &, QString *) } void MyPlugin2::extensionsInitialized() -{ -} +{} Q_EXPORT_PLUGIN(MyPlugin2) - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.h index c15e0c458..b91731569 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.h @@ -4,25 +4,25 @@ * @file plugin2.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ #include namespace Plugin2 { - -class MyPlugin2 : public ExtensionSystem::IPlugin -{ +class MyPlugin2 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -45,7 +43,6 @@ public: bool initialize(const QStringList &arguments, QString *errorString); void extensionsInitialized(); }; - } // Plugin2 #endif // PLUGIN2_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.cpp index 2761bd5d2..f74cc322f 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.cpp @@ -4,25 +4,25 @@ * @file plugin3.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "plugin3.h" @@ -32,8 +32,7 @@ using namespace Plugin3; MyPlugin3::MyPlugin3() -{ -} +{} bool MyPlugin3::initialize(const QStringList &, QString *) { @@ -41,8 +40,6 @@ bool MyPlugin3::initialize(const QStringList &, QString *) } void MyPlugin3::extensionsInitialized() -{ -} +{} Q_EXPORT_PLUGIN(MyPlugin3) - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.h index aa40fa1fb..69b2ec6b5 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.h @@ -4,25 +4,25 @@ * @file plugin3.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ #include namespace Plugin3 { - -class MyPlugin3 : public ExtensionSystem::IPlugin -{ +class MyPlugin3 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -45,7 +43,6 @@ public: bool initialize(const QStringList &arguments, QString *errorString); void extensionsInitialized(); }; - } // namespace Plugin3 #endif // PLUGIN3_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.cpp index 8eaf2cab5..8a59a392a 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.cpp @@ -4,25 +4,25 @@ * @file plugin1.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,8 +37,7 @@ using namespace Plugin1; MyPlugin1::MyPlugin1() : initializeCalled(false) -{ -} +{} bool MyPlugin1::initialize(const QStringList & /*arguments*/, QString *errorString) { @@ -49,20 +48,24 @@ bool MyPlugin1::initialize(const QStringList & /*arguments*/, QString *errorStri bool found2 = false; bool found3 = false; - foreach (QObject *object, ExtensionSystem::PluginManager::instance()->allObjects()) { - if (object->objectName() == "MyPlugin2") + foreach(QObject * object, ExtensionSystem::PluginManager::instance()->allObjects()) { + if (object->objectName() == "MyPlugin2") { found2 = true; - else if (object->objectName() == "MyPlugin3") + } else if (object->objectName() == "MyPlugin3") { found3 = true; + } } - if (found2 && found3) + if (found2 && found3) { return true; + } if (errorString) { QString error = "object(s) missing from plugin(s):"; - if (!found2) + if (!found2) { error.append(" plugin2"); - if (!found3) + } + if (!found3) { error.append(" plugin3"); + } *errorString = error; } return false; @@ -70,8 +73,9 @@ bool MyPlugin1::initialize(const QStringList & /*arguments*/, QString *errorStri void MyPlugin1::extensionsInitialized() { - if (!initializeCalled) + if (!initializeCalled) { return; + } // don't do this at home, it's just done here for the test QObject *obj = new QObject; obj->setObjectName("MyPlugin1_running"); @@ -79,4 +83,3 @@ void MyPlugin1::extensionsInitialized() } Q_EXPORT_PLUGIN(MyPlugin1) - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.h index 61b01b3b4..ffe1a8583 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.h @@ -4,25 +4,25 @@ * @file plugin1.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ #include namespace Plugin1 { - -class MyPlugin1 : public ExtensionSystem::IPlugin -{ +class MyPlugin1 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -48,7 +46,6 @@ public: private: bool initializeCalled; }; - } // namespace Plugin1 #endif // PLUGIN1_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.cpp index e50167aeb..59d5da91e 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.cpp @@ -4,25 +4,25 @@ * @file plugin2.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,8 +37,7 @@ using namespace Plugin2; MyPlugin2::MyPlugin2() : initializeCalled(false) -{ -} +{} bool MyPlugin2::initialize(const QStringList &, QString *) { @@ -52,8 +51,9 @@ bool MyPlugin2::initialize(const QStringList &, QString *) void MyPlugin2::extensionsInitialized() { - if (!initializeCalled) + if (!initializeCalled) { return; + } // don't do this at home, it's just done here for the test QObject *obj = new QObject; obj->setObjectName("MyPlugin2_running"); @@ -61,4 +61,3 @@ void MyPlugin2::extensionsInitialized() } Q_EXPORT_PLUGIN(MyPlugin2) - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.h index 262d4ce9e..e7ce45485 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.h @@ -4,25 +4,25 @@ * @file plugin2.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ #include namespace Plugin2 { - -class MyPlugin2 : public ExtensionSystem::IPlugin -{ +class MyPlugin2 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -48,7 +46,6 @@ public: private: bool initializeCalled; }; - } // namespace Plugin2 #endif // PLUGIN2_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.cpp index a55d7e51c..7b38e607b 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.cpp @@ -4,25 +4,25 @@ * @file plugin3.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,8 +37,7 @@ using namespace Plugin3; MyPlugin3::MyPlugin3() : initializeCalled(false) -{ -} +{} bool MyPlugin3::initialize(const QStringList & /*arguments*/, QString *errorString) { @@ -48,21 +47,25 @@ bool MyPlugin3::initialize(const QStringList & /*arguments*/, QString *errorStri addAutoReleasedObject(obj); bool found2 = false; - foreach (QObject *object, ExtensionSystem::PluginManager::instance()->allObjects()) { - if (object->objectName() == "MyPlugin2") + foreach(QObject * object, ExtensionSystem::PluginManager::instance()->allObjects()) { + if (object->objectName() == "MyPlugin2") { found2 = true; + } } - if (found2) + if (found2) { return true; - if (errorString) + } + if (errorString) { *errorString = "object from plugin2 could not be found"; + } return false; } void MyPlugin3::extensionsInitialized() { - if (!initializeCalled) + if (!initializeCalled) { return; + } // don't do this at home, it's just done here for the test QObject *obj = new QObject; obj->setObjectName("MyPlugin3_running"); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.h index f7c1f2cf8..17194f5b3 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.h @@ -4,25 +4,25 @@ * @file plugin3.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PLUGIN3_H @@ -33,9 +33,7 @@ #include namespace Plugin3 { - -class MyPlugin3 : public ExtensionSystem::IPlugin -{ +class MyPlugin3 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -47,7 +45,6 @@ public: private: bool initializeCalled; }; - } // namespace Plugin3 #endif // PLUGIN3_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/tst_pluginmanager.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/tst_pluginmanager.cpp index b81e5892c..515cc3409 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/tst_pluginmanager.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/tst_pluginmanager.cpp @@ -4,25 +4,25 @@ * @file tst_pluginmanager.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,8 +38,7 @@ using namespace ExtensionSystem; class SignalReceiver; -class tst_PluginManager : public QObject -{ +class tst_PluginManager : public QObject { Q_OBJECT private slots: @@ -57,8 +56,7 @@ private: SignalReceiver *m_sr; }; -class SignalReceiver : public QObject -{ +class SignalReceiver : public QObject { Q_OBJECT public: @@ -68,30 +66,36 @@ public: pluginsChangedCount(0), objectAddedObj(0), aboutToRemoveObjectObj(0) - { } + {} int objectAddedCount; int aboutToRemoveObjectCount; int pluginsChangedCount; QObject *objectAddedObj; QObject *aboutToRemoveObjectObj; public slots: - void objectAdded(QObject *obj) { objectAddedCount++; objectAddedObj = obj; } - void aboutToRemoveObject(QObject *obj) { aboutToRemoveObjectCount++; aboutToRemoveObjectObj = obj; } - void pluginsChanged() { pluginsChangedCount++; } + void objectAdded(QObject *obj) + { + objectAddedCount++; objectAddedObj = obj; + } + void aboutToRemoveObject(QObject *obj) + { + aboutToRemoveObjectCount++; aboutToRemoveObjectObj = obj; + } + void pluginsChanged() + { + pluginsChangedCount++; + } }; -class MyClass1 : public QObject -{ +class MyClass1 : public QObject { Q_OBJECT }; -class MyClass2 : public QObject -{ +class MyClass2 : public QObject { Q_OBJECT }; -class MyClass11 : public MyClass1 -{ +class MyClass11 : public MyClass1 { Q_OBJECT }; @@ -99,8 +103,8 @@ void tst_PluginManager::init() { m_pm = new PluginManager; m_sr = new SignalReceiver; - connect(m_pm, SIGNAL(objectAdded(QObject*)), m_sr, SLOT(objectAdded(QObject*))); - connect(m_pm, SIGNAL(aboutToRemoveObject(QObject*)), m_sr, SLOT(aboutToRemoveObject(QObject*))); + connect(m_pm, SIGNAL(objectAdded(QObject *)), m_sr, SLOT(objectAdded(QObject *))); + connect(m_pm, SIGNAL(aboutToRemoveObject(QObject *)), m_sr, SLOT(aboutToRemoveObject(QObject *))); connect(m_pm, SIGNAL(pluginsChanged()), m_sr, SLOT(pluginsChanged())); } @@ -114,6 +118,7 @@ void tst_PluginManager::addRemoveObjects() { QObject *object1 = new QObject; QObject *object2 = new QObject; + QCOMPARE(m_pm->allObjects().size(), 0); m_pm->addObject(object1); QCOMPARE(m_sr->objectAddedCount, 1); @@ -149,15 +154,16 @@ void tst_PluginManager::addRemoveObjects() void tst_PluginManager::getObject() { - MyClass2 *object2 = new MyClass2; + MyClass2 *object2 = new MyClass2; MyClass11 *object11 = new MyClass11; + m_pm->addObject(object2); - QCOMPARE(m_pm->getObject(), (MyClass11*)0); - QCOMPARE(m_pm->getObject(), (MyClass1*)0); + QCOMPARE(m_pm->getObject(), (MyClass11 *)0); + QCOMPARE(m_pm->getObject(), (MyClass1 *)0); QCOMPARE(m_pm->getObject(), object2); m_pm->addObject(object11); QCOMPARE(m_pm->getObject(), object11); - QCOMPARE(m_pm->getObject(), qobject_cast(object11)); + QCOMPARE(m_pm->getObject(), qobject_cast(object11)); QCOMPARE(m_pm->getObject(), object2); m_pm->removeObject(object2); m_pm->removeObject(object11); @@ -167,24 +173,25 @@ void tst_PluginManager::getObject() void tst_PluginManager::getObjects() { - MyClass1 *object1 = new MyClass1; - MyClass2 *object2 = new MyClass2; + MyClass1 *object1 = new MyClass1; + MyClass2 *object2 = new MyClass2; MyClass11 *object11 = new MyClass11; + m_pm->addObject(object2); - QCOMPARE(m_pm->getObjects(), QList()); - QCOMPARE(m_pm->getObjects(), QList()); - QCOMPARE(m_pm->getObjects(), QList() << object2); - QCOMPARE(m_pm->allObjects(), QList() << object2); + QCOMPARE(m_pm->getObjects(), QList()); + QCOMPARE(m_pm->getObjects(), QList()); + QCOMPARE(m_pm->getObjects(), QList() << object2); + QCOMPARE(m_pm->allObjects(), QList() << object2); m_pm->addObject(object11); - QCOMPARE(m_pm->getObjects(), QList() << object11); - QCOMPARE(m_pm->getObjects(), QList() << object11); - QCOMPARE(m_pm->getObjects(), QList() << object2); - QCOMPARE(m_pm->allObjects(), QList() << object2 << object11); + QCOMPARE(m_pm->getObjects(), QList() << object11); + QCOMPARE(m_pm->getObjects(), QList() << object11); + QCOMPARE(m_pm->getObjects(), QList() << object2); + QCOMPARE(m_pm->allObjects(), QList() << object2 << object11); m_pm->addObject(object1); - QCOMPARE(m_pm->getObjects(), QList() << object11); - QCOMPARE(m_pm->getObjects(), QList() << object11 << object1); - QCOMPARE(m_pm->getObjects(), QList() << object2); - QCOMPARE(m_pm->allObjects(), QList() << object2 << object11 << object1); + QCOMPARE(m_pm->getObjects(), QList() << object11); + QCOMPARE(m_pm->getObjects(), QList() << object11 << object1); + QCOMPARE(m_pm->getObjects(), QList() << object2); + QCOMPARE(m_pm->allObjects(), QList() << object2 << object11 << object1); m_pm->removeObject(object2); m_pm->removeObject(object11); m_pm->removeObject(object1); @@ -199,9 +206,10 @@ void tst_PluginManager::plugins() QCOMPARE(m_sr->pluginsChangedCount, 1); QSet plugins = m_pm->plugins(); QCOMPARE(plugins.count(), 3); - foreach (const QString &expected, QStringList() << "helloworld" << "MyPlugin" << "dummyPlugin") { + foreach(const QString &expected, QStringList() << "helloworld" << "MyPlugin" << "dummyPlugin") { bool found = false; - foreach (PluginSpec *spec, plugins) { + + foreach(PluginSpec * spec, plugins) { if (spec->name() == expected) { found = true; break; @@ -215,18 +223,18 @@ void tst_PluginManager::circularPlugins() { m_pm->setPluginPaths(QStringList() << "circularplugins"); m_pm->loadPlugins(); - foreach (PluginSpec *spec, m_pm->plugins()) { + foreach(PluginSpec * spec, m_pm->plugins()) { if (spec->name() == "plugin1") { QVERIFY(spec->hasError()); QCOMPARE(spec->state(), PluginSpec::Resolved); - QCOMPARE(spec->plugin(), (IPlugin*)0); + QCOMPARE(spec->plugin(), (IPlugin *)0); } else if (spec->name() == "plugin2") { QVERIFY(!spec->hasError()); QCOMPARE(spec->state(), PluginSpec::Running); } else if (spec->name() == "plugin3") { QVERIFY(spec->hasError()); QCOMPARE(spec->state(), PluginSpec::Resolved); - QCOMPARE(spec->plugin(), (IPlugin*)0); + QCOMPARE(spec->plugin(), (IPlugin *)0); } } } @@ -236,22 +244,24 @@ void tst_PluginManager::correctPlugins1() m_pm->setFileExtension("spec"); m_pm->setPluginPaths(QStringList() << "correctplugins1"); m_pm->loadPlugins(); - foreach (PluginSpec *spec, m_pm->plugins()) { - if (spec->hasError()) + foreach(PluginSpec * spec, m_pm->plugins()) { + if (spec->hasError()) { qDebug() << spec->errorString(); + } QVERIFY(!spec->hasError()); QCOMPARE(spec->state(), PluginSpec::Running); } bool plugin1running = false; bool plugin2running = false; bool plugin3running = false; - foreach (QObject *obj, m_pm->allObjects()) { - if (obj->objectName() == "MyPlugin1_running") + foreach(QObject * obj, m_pm->allObjects()) { + if (obj->objectName() == "MyPlugin1_running") { plugin1running = true; - else if (obj->objectName() == "MyPlugin2_running") + } else if (obj->objectName() == "MyPlugin2_running") { plugin2running = true; - else if (obj->objectName() == "MyPlugin3_running") + } else if (obj->objectName() == "MyPlugin3_running") { plugin3running = true; + } } QVERIFY(plugin1running); QVERIFY(plugin2running); @@ -261,4 +271,3 @@ void tst_PluginManager::correctPlugins1() QTEST_MAIN(tst_PluginManager) #include "tst_pluginmanager.moc" - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.cpp index 4343c1564..aa28c9726 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.cpp @@ -4,25 +4,25 @@ * @file testplugin.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,8 +34,7 @@ using namespace MyPlugin; MyPluginImpl::MyPluginImpl() : m_isInitialized(false), m_isExtensionsInitialized(false) -{ -} +{} bool MyPluginImpl::initialize(const QStringList &, QString *) { @@ -49,4 +48,3 @@ void MyPluginImpl::extensionsInitialized() } Q_EXPORT_PLUGIN(MyPluginImpl) - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.h index b8824fd6b..1e1f48584 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.h @@ -4,25 +4,25 @@ * @file testplugin.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include namespace MyPlugin { - -class MYPLUGIN_EXPORT MyPluginImpl : public ExtensionSystem::IPlugin -{ +class MYPLUGIN_EXPORT MyPluginImpl : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -46,13 +44,18 @@ public: bool initialize(const QStringList &arguments, QString *errorString); void extensionsInitialized(); - bool isInitialized() { return m_isInitialized; } - bool isExtensionsInitialized() { return m_isExtensionsInitialized; } + bool isInitialized() + { + return m_isInitialized; + } + bool isExtensionsInitialized() + { + return m_isExtensionsInitialized; + } private: bool m_isInitialized; bool m_isExtensionsInitialized; }; - } // namespace #endif // TESTPLUGIN_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin_global.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin_global.h index 08da14ba3..129dd0ecd 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin_global.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin_global.h @@ -4,25 +4,25 @@ * @file testplugin_global.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/tst_pluginspec.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/tst_pluginspec.cpp index 0591217f0..6eed2fa67 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/tst_pluginspec.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/tst_pluginspec.cpp @@ -4,25 +4,25 @@ * @file tst_pluginspec.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,8 +38,7 @@ using namespace ExtensionSystem; -class tst_PluginSpec : public QObject -{ +class tst_PluginSpec : public QObject { Q_OBJECT private slots: @@ -58,6 +57,7 @@ private slots: void tst_PluginSpec::read() { Internal::PluginSpecPrivate spec(0); + QCOMPARE(spec.state, PluginSpec::Invalid); QVERIFY(spec.read("testspecs/spec1.xml")); QCOMPARE(spec.state, PluginSpec::Read); @@ -72,10 +72,10 @@ void tst_PluginSpec::read() QCOMPARE(spec.description, QString("This plugin is just a test.\n it demonstrates the great use of the plugin spec.")); QCOMPARE(spec.url, QString("http://www.trolltech.com")); PluginDependency dep1; - dep1.name = QString("SomeOtherPlugin"); + dep1.name = QString("SomeOtherPlugin"); dep1.version = QString("2.3.0_2"); PluginDependency dep2; - dep2.name = QString("EvenOther"); + dep2.name = QString("EvenOther"); dep2.version = QString("1.0.0"); QCOMPARE(spec.dependencies, QList() << dep1 << dep2); @@ -88,6 +88,7 @@ void tst_PluginSpec::read() void tst_PluginSpec::readError() { Internal::PluginSpecPrivate spec(0); + QCOMPARE(spec.state, PluginSpec::Invalid); QVERIFY(!spec.read("non-existing-file.xml")); QCOMPARE(spec.state, PluginSpec::Invalid); @@ -155,6 +156,7 @@ void tst_PluginSpec::versionCompare() void tst_PluginSpec::provides() { Internal::PluginSpecPrivate spec(0); + QVERIFY(spec.read("testspecs/simplespec.xml")); QVERIFY(!spec.provides("SomeOtherPlugin", "2.2.3_9")); QVERIFY(!spec.provides("MyPlugin", "2.2.3_10")); @@ -180,15 +182,16 @@ void tst_PluginSpec::provides() void tst_PluginSpec::locationAndPath() { Internal::PluginSpecPrivate spec(0); + QVERIFY(spec.read("testspecs/simplespec.xml")); - QCOMPARE(spec.location, QDir::currentPath()+"/testspecs"); - QCOMPARE(spec.filePath, QDir::currentPath()+"/testspecs/simplespec.xml"); + QCOMPARE(spec.location, QDir::currentPath() + "/testspecs"); + QCOMPARE(spec.filePath, QDir::currentPath() + "/testspecs/simplespec.xml"); QVERIFY(spec.read("testdir/../testspecs/simplespec.xml")); - QCOMPARE(spec.location, QDir::currentPath()+"/testspecs"); - QCOMPARE(spec.filePath, QDir::currentPath()+"/testspecs/simplespec.xml"); + QCOMPARE(spec.location, QDir::currentPath() + "/testspecs"); + QCOMPARE(spec.filePath, QDir::currentPath() + "/testspecs/simplespec.xml"); QVERIFY(spec.read("testdir/spec.xml")); - QCOMPARE(spec.location, QDir::currentPath()+"/testdir"); - QCOMPARE(spec.filePath, QDir::currentPath()+"/testdir/spec.xml"); + QCOMPARE(spec.location, QDir::currentPath() + "/testdir"); + QCOMPARE(spec.filePath, QDir::currentPath() + "/testdir/spec.xml"); } void tst_PluginSpec::resolveDependencies() @@ -226,10 +229,11 @@ void tst_PluginSpec::loadLibrary() PluginSpec *ps = Internal::PluginManagerPrivate::createSpec(); Internal::PluginSpecPrivate *spec = Internal::PluginManagerPrivate::privateSpec(ps); PluginManager *manager = new PluginManager(); + QVERIFY(spec->read("testplugin/testplugin.xml")); QVERIFY(spec->resolveDependencies(QSet())); QVERIFY(spec->loadLibrary()); - QVERIFY(qobject_cast(spec->plugin) != 0); + QVERIFY(qobject_cast(spec->plugin) != 0); QCOMPARE(spec->state, PluginSpec::Loaded); QVERIFY(!spec->hasError); QCOMPARE(spec->plugin->pluginSpec(), ps); @@ -241,10 +245,11 @@ void tst_PluginSpec::initializePlugin() { Internal::PluginSpecPrivate spec(0); MyPlugin::MyPluginImpl *impl; + QVERIFY(spec.read("testplugin/testplugin.xml")); QVERIFY(spec.resolveDependencies(QSet())); QVERIFY(spec.loadLibrary()); - impl = qobject_cast(spec.plugin); + impl = qobject_cast(spec.plugin); QVERIFY(impl != 0); QVERIFY(!impl->isInitialized()); QVERIFY(spec.initializePlugin()); @@ -257,10 +262,11 @@ void tst_PluginSpec::initializeExtensions() { Internal::PluginSpecPrivate spec(0); MyPlugin::MyPluginImpl *impl; + QVERIFY(spec.read("testplugin/testplugin.xml")); QVERIFY(spec.resolveDependencies(QSet())); QVERIFY(spec.loadLibrary()); - impl = qobject_cast(spec.plugin); + impl = qobject_cast(spec.plugin); QVERIFY(impl != 0); QVERIFY(spec.initializePlugin()); QVERIFY(spec.initializeExtensions()); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.cpp index eb352547a..6894393b1 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.cpp @@ -4,25 +4,25 @@ * @file plugindialog.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,6 +43,7 @@ PluginDialog::PluginDialog(ExtensionSystem::PluginManager *manager) : m_view(new ExtensionSystem::PluginView(manager, this)) { QVBoxLayout *vl = new QVBoxLayout(this); + vl->setMargin(0); vl->setSpacing(0); vl->addWidget(m_view); @@ -61,10 +62,10 @@ PluginDialog::PluginDialog(ExtensionSystem::PluginManager *manager) resize(650, 300); setWindowTitle(tr("Installed Plugins")); - connect(m_view, SIGNAL(currentPluginChanged(ExtensionSystem::PluginSpec*)), - this, SLOT(updateButtons())); - connect(m_view, SIGNAL(pluginActivated(ExtensionSystem::PluginSpec*)), - this, SLOT(openDetails(ExtensionSystem::PluginSpec*))); + connect(m_view, SIGNAL(currentPluginChanged(ExtensionSystem::PluginSpec *)), + this, SLOT(updateButtons())); + connect(m_view, SIGNAL(pluginActivated(ExtensionSystem::PluginSpec *)), + this, SLOT(openDetails(ExtensionSystem::PluginSpec *))); connect(m_detailsButton, SIGNAL(clicked()), this, SLOT(openDetails())); connect(m_errorDetailsButton, SIGNAL(clicked()), this, SLOT(openErrorDetails())); } @@ -72,6 +73,7 @@ PluginDialog::PluginDialog(ExtensionSystem::PluginManager *manager) void PluginDialog::updateButtons() { ExtensionSystem::PluginSpec *selectedSpec = m_view->currentPlugin(); + if (selectedSpec) { m_detailsButton->setEnabled(true); m_errorDetailsButton->setEnabled(selectedSpec->hasError()); @@ -84,13 +86,14 @@ void PluginDialog::updateButtons() void PluginDialog::openDetails() { - openDetails(m_view->currentPlugin()); + openDetails(m_view->currentPlugin()); } void PluginDialog::openDetails(ExtensionSystem::PluginSpec *spec) { - if (!spec) + if (!spec) { return; + } QDialog dialog(this); dialog.setWindowTitle(tr("Plugin Details of %1").arg(spec->name())); QVBoxLayout *layout = new QVBoxLayout; @@ -109,8 +112,10 @@ void PluginDialog::openDetails(ExtensionSystem::PluginSpec *spec) void PluginDialog::openErrorDetails() { ExtensionSystem::PluginSpec *spec = m_view->currentPlugin(); - if (!spec) + + if (!spec) { return; + } QDialog dialog(this); dialog.setWindowTitle(tr("Plugin Errors of %1").arg(spec->name())); QVBoxLayout *layout = new QVBoxLayout; @@ -131,6 +136,7 @@ int main(int argc, char *argv[]) ExtensionSystem::PluginManager manager; QApplication app(argc, argv); PluginDialog dialog(&manager); + manager.setPluginPaths(QStringList() << "plugins"); manager.loadPlugins(); dialog.show(); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.h b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.h index 3ee89e5ae..77cf5acd3 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.h @@ -4,25 +4,25 @@ * @file plugindialog.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,8 +35,7 @@ #include #include -class PluginDialog : public QWidget -{ +class PluginDialog : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.cpp index 11c8b4ebd..f2932b60b 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.cpp @@ -4,25 +4,25 @@ * @file plugin1.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,8 +37,7 @@ using namespace Plugin1; MyPlugin1::MyPlugin1() : initializeCalled(false) -{ -} +{} bool MyPlugin1::initialize(const QStringList & /*arguments*/, QString *errorString) { @@ -49,20 +48,24 @@ bool MyPlugin1::initialize(const QStringList & /*arguments*/, QString *errorStri bool found2 = false; bool found3 = false; - foreach (QObject *object, ExtensionSystem::PluginManager::instance()->allObjects()) { - if (object->objectName() == "MyPlugin2") + foreach(QObject * object, ExtensionSystem::PluginManager::instance()->allObjects()) { + if (object->objectName() == "MyPlugin2") { found2 = true; - else if (object->objectName() == "MyPlugin3") + } else if (object->objectName() == "MyPlugin3") { found3 = true; + } } - if (found2 && found3) + if (found2 && found3) { return true; + } if (errorString) { QString error = "object(s) missing from plugin(s):"; - if (!found2) + if (!found2) { error.append(" plugin2"); - if (!found3) + } + if (!found3) { error.append(" plugin3"); + } *errorString = error; } return false; @@ -70,8 +73,9 @@ bool MyPlugin1::initialize(const QStringList & /*arguments*/, QString *errorStri void MyPlugin1::extensionsInitialized() { - if (!initializeCalled) + if (!initializeCalled) { return; + } // don't do this at home, it's just done here for the test QObject *obj = new QObject(this); obj->setObjectName("MyPlugin1_running"); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.h b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.h index 085cca8f8..4ad65894f 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.h @@ -4,25 +4,25 @@ * @file plugin1.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include namespace Plugin1 { - -class MyPlugin1 : public ExtensionSystem::IPlugin -{ +class MyPlugin1 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -49,7 +47,6 @@ public: private: bool initializeCalled; }; - } // namespace Plugin1 #endif // PLUGIN1_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.cpp index 52c67cbf9..44409ad83 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.cpp @@ -4,25 +4,25 @@ * @file plugin2.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,8 +37,7 @@ using namespace Plugin2; MyPlugin2::MyPlugin2() : initializeCalled(false) -{ -} +{} bool MyPlugin2::initialize(const QStringList & /*arguments*/, QString *errorString) { @@ -53,8 +52,9 @@ bool MyPlugin2::initialize(const QStringList & /*arguments*/, QString *errorStri void MyPlugin2::extensionsInitialized() { - if (!initializeCalled) + if (!initializeCalled) { return; + } // don't do this at home, it's just done here for the test QObject *obj = new QObject(this); obj->setObjectName("MyPlugin2_running"); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.h b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.h index f524546a3..0a4d6f165 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.h @@ -4,25 +4,25 @@ * @file plugin2.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include namespace Plugin2 { - -class MyPlugin2 : public ExtensionSystem::IPlugin -{ +class MyPlugin2 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -49,7 +47,6 @@ public: private: bool initializeCalled; }; - } // namespace Plugin2 #endif // PLUGIN2_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.cpp index 118b13198..a8b434de7 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.cpp @@ -4,25 +4,25 @@ * @file plugin3.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,8 +37,7 @@ using namespace Plugin3; MyPlugin3::MyPlugin3() : initializeCalled(false) -{ -} +{} bool MyPlugin3::initialize(const QStringList & /*arguments*/, QString *errorString) { @@ -48,21 +47,25 @@ bool MyPlugin3::initialize(const QStringList & /*arguments*/, QString *errorStri addAutoReleasedObject(obj); bool found2 = false; - foreach (QObject *object, ExtensionSystem::PluginManager::instance()->allObjects()) { - if (object->objectName() == "MyPlugin2") + foreach(QObject * object, ExtensionSystem::PluginManager::instance()->allObjects()) { + if (object->objectName() == "MyPlugin2") { found2 = true; + } } - if (found2) + if (found2) { return true; - if (errorString) + } + if (errorString) { *errorString = "object from plugin2 could not be found"; + } return false; } void MyPlugin3::extensionsInitialized() { - if (!initializeCalled) + if (!initializeCalled) { return; + } // don't do this at home, it's just done here for the test QObject *obj = new QObject(this); obj->setObjectName("MyPlugin3_running"); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.h b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.h index cdc7f4df9..3fff58b4d 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.h @@ -4,25 +4,25 @@ * @file plugin3.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include namespace Plugin3 { - -class MyPlugin3 : public ExtensionSystem::IPlugin -{ +class MyPlugin3 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -49,7 +47,6 @@ public: private: bool initializeCalled; }; - } // namespace Plugin3 #endif // PLUGIN3_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.h b/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.h index aae8fd5ff..f7a8361dd 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.h @@ -1,20 +1,17 @@ #ifndef OPMAP_CONTROL_H_ #define OPMAP_CONTROL_H_ #include "src/mapwidget/opmapwidget.h" -namespace mapcontrol -{ - struct customData - { - float velocity; - int mode; - float mode_params[4]; - int condition; - float condition_params[4]; - int command; - int jumpdestination; - int errordestination; - }; - +namespace mapcontrol { +struct customData { + float velocity; + int mode; + float mode_params[4]; + int condition; + float condition_params[4]; + int command; + int jumpdestination; + int errordestination; +}; } Q_DECLARE_METATYPE(mapcontrol::customData) #endif diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/accessmode.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/accessmode.h index a2022e653..673d14444 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/accessmode.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/accessmode.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file accessmode.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file accessmode.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef ACCESSMODE_H #define ACCESSMODE_H @@ -33,54 +33,52 @@ #include #include namespace core { - class AccessMode:public QObject - { - Q_OBJECT - Q_ENUMS(Types) - public: - enum Types - { - /// - /// access only server - /// - ServerOnly, +class AccessMode : public QObject { + Q_OBJECT Q_ENUMS(Types) +public: + enum Types { + /// + /// access only server + /// + ServerOnly, - /// - /// access first server and caches localy - /// - ServerAndCache, - - /// - /// access only cache - /// - CacheOnly - }; - static QString StrByType(Types const& value) - { - QMetaObject metaObject = AccessMode().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - QString s=metaEnum.valueToKey(value); - return s; - } - static Types TypeByStr(QString const& value) - { - QMetaObject metaObject = AccessMode().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - Types s=(Types)metaEnum.keyToValue(value.toLatin1()); - return s; - } - static QStringList TypesList() - { - QStringList ret; - QMetaObject metaObject = AccessMode().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - for(int x=0;x + /// access first server and caches localy + /// + ServerAndCache, + /// + /// access only cache + /// + CacheOnly }; + static QString StrByType(Types const & value) + { + QMetaObject metaObject = AccessMode().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + QString s = metaEnum.valueToKey(value); + + return s; + } + static Types TypeByStr(QString const & value) + { + QMetaObject metaObject = AccessMode().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + Types s = (Types)metaEnum.keyToValue(value.toLatin1()); + + return s; + } + static QStringList TypesList() + { + QStringList ret; + QMetaObject metaObject = AccessMode().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + + for (int x = 0; x < metaEnum.keyCount(); ++x) { + ret.append(metaEnum.key(x)); + } + return ret; + } +}; } #endif // ACCESSMODE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.cpp index a6946eb7d..ca615f94b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.cpp @@ -1,87 +1,82 @@ /** -****************************************************************************** -* -* @file alllayersoftype.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file alllayersoftype.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "alllayersoftype.h" namespace core { - AllLayersOfType::AllLayersOfType() +AllLayersOfType::AllLayersOfType() +{} +QVector AllLayersOfType::GetAllLayersOfType(const MapType::Types &type) +{ + QVector types; { - - } - QVector AllLayersOfType::GetAllLayersOfType(const MapType::Types &type) - { - QVector types; + switch (type) { + case MapType::GoogleHybrid: { - switch(type) - { - case MapType::GoogleHybrid: - { - - types.append(MapType::GoogleSatellite); - types.append(MapType::GoogleLabels); - } - break; - - case MapType::GoogleHybridChina: - { - types.append(MapType::GoogleSatelliteChina); - types.append(MapType::GoogleLabelsChina); - } - break; - - case MapType::GoogleHybridKorea: - { - types.append(MapType::GoogleSatelliteKorea); - types.append(MapType::GoogleLabelsKorea); - } - break; - - case MapType::YahooHybrid: - { - types.append(MapType::YahooSatellite); - types.append(MapType::YahooLabels); - } - break; - - case MapType::ArcGIS_MapsLT_Map_Hybrid: - { - types.append(MapType::ArcGIS_MapsLT_OrtoFoto); - types.append(MapType::ArcGIS_MapsLT_Map_Labels); - } - break; - - default: - { - types.append(type); - } - break; - } + types.append(MapType::GoogleSatellite); + types.append(MapType::GoogleLabels); } + break; - return types; + case MapType::GoogleHybridChina: + { + types.append(MapType::GoogleSatelliteChina); + types.append(MapType::GoogleLabelsChina); + } + break; + case MapType::GoogleHybridKorea: + { + types.append(MapType::GoogleSatelliteKorea); + types.append(MapType::GoogleLabelsKorea); + } + break; + + case MapType::YahooHybrid: + { + types.append(MapType::YahooSatellite); + types.append(MapType::YahooLabels); + } + break; + + case MapType::ArcGIS_MapsLT_Map_Hybrid: + { + types.append(MapType::ArcGIS_MapsLT_OrtoFoto); + types.append(MapType::ArcGIS_MapsLT_Map_Labels); + } + break; + + default: + { + types.append(type); + } + break; + } } + + return types; +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.h index 0c45bc7fe..16ba1b70e 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file alllayersoftype.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file alllayersoftype.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef ALLLAYERSOFTYPE_H #define ALLLAYERSOFTYPE_H @@ -32,12 +32,10 @@ #include namespace core { - class AllLayersOfType - { - public: - AllLayersOfType(); - QVector GetAllLayersOfType(const MapType::Types &type); - }; - +class AllLayersOfType { +public: + AllLayersOfType(); + QVector GetAllLayersOfType(const MapType::Types &type); +}; } #endif // ALLLAYERSOFTYPE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp index e4059af55..47704b63a 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp @@ -1,197 +1,189 @@ /** -****************************************************************************** -* -* @file cache.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file cache.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "cache.h" #include "utils/pathutils.h" #include namespace core { - Cache* Cache::m_pInstance=0; +Cache *Cache::m_pInstance = 0; - Cache* Cache::Instance() - { - if(!m_pInstance) - m_pInstance=new Cache; - return m_pInstance; +Cache *Cache::Instance() +{ + if (!m_pInstance) { + m_pInstance = new Cache; } + return m_pInstance; +} - void Cache::setCacheLocation(const QString& value) - { - cache=value; - routeCache = cache + "RouteCache" + QDir::separator(); - geoCache = cache + "GeocoderCache"+ QDir::separator(); - placemarkCache = cache + "PlacemarkCache" + QDir::separator(); - ImageCache.setGtileCache(value); - } - QString Cache::CacheLocation() - { - return cache; - } - Cache::Cache() - { - if(cache.isNull()|cache.isEmpty()) - { - cache= Utils::PathUtils().GetStoragePath()+"mapscache"+QDir::separator(); - setCacheLocation(cache); - } - } - QString Cache::GetGeocoderFromCache(const QString &urlEnd) - { -#ifdef DEBUG_GetGeocoderFromCache - qDebug()<<"Entered GetGeocoderFromCache"; -#endif - QString ret=QString::null; - QString filename=geoCache+QString(urlEnd)+".geo"; -#ifdef DEBUG_GetGeocoderFromCache - qDebug()<<"GetGeocoderFromCache: Does file exist?:"<>ret; - } - } -#ifdef DEBUG_GetGeocoderFromCache - qDebug()<<"GetGeocoderFromCache:Returning:"<>ret; - } - } -#ifdef DEBUG_CACHE - qDebug()<<"GetPlacemarkFromCache:Returning:"<> ret; + } + } +#ifdef DEBUG_GetGeocoderFromCache + qDebug() << "GetGeocoderFromCache:Returning:" << ret; +#endif + return ret; +} +void Cache::CacheGeocoder(const QString &urlEnd, const QString &content) +{ + QString ret = QString::null; + QString filename = geoCache + QString(urlEnd) + ".geo"; + +#ifdef DEBUG_CACHE + qDebug() << "CacheGeocoder: Filename:" << filename; +#endif // DEBUG_CACHE + QFileInfo File(filename);; + QDir dir = File.absoluteDir(); + QString path = dir.absolutePath(); +#ifdef DEBUG_CACHE + qDebug() << "CacheGeocoder: Path:" << path; +#endif // DEBUG_CACHE + if (!dir.exists()) { +#ifdef DEBUG_CACHE + qDebug() << "CacheGeocoder: Cache path doesn't exist, try to create"; +#endif // DEBUG_CACHE + if (!dir.mkpath(path)) { +#ifdef DEBUG_CACHE + qDebug() << "GetGeocoderFromCache: Could not create path"; +#endif // DEBUG_CACHE + } + } +#ifdef DEBUG_CACHE + qDebug() << "CacheGeocoder: OpenFile:" << filename; +#endif // DEBUG_CACHE + QFile file(filename); + if (file.open(QIODevice::WriteOnly)) { +#ifdef DEBUG_CACHE + qDebug() << "CacheGeocoder: File Opened!!!:" << filename; +#endif // DEBUG_CACHE + QTextStream stream(&file); + stream.setCodec("UTF-8"); + stream << content; + } +} +QString Cache::GetPlacemarkFromCache(const QString &urlEnd) +{ +#ifdef DEBUG_CACHE + qDebug() << "Entered GetPlacemarkFromCache"; +#endif // DEBUG_CACHE + QString ret = QString::null; + QString filename = placemarkCache + QString(urlEnd) + ".plc"; +#ifdef DEBUG_CACHE + qDebug() << "GetPlacemarkFromCache: Does file exist?:" << filename; +#endif // DEBUG_CACHE + QFileInfo File(filename); + if (File.exists()) { +#ifdef DEBUG_CACHE + qDebug() << "GetPlacemarkFromCache:File exists!!"; +#endif // DEBUG_CACHE + QFile file(filename); + if (file.open(QIODevice::ReadOnly)) { + QTextStream stream(&file); + stream.setCodec("UTF-8"); + stream >> ret; + } + } +#ifdef DEBUG_CACHE + qDebug() << "GetPlacemarkFromCache:Returning:" << ret; +#endif // DEBUG_CACHE + return ret; +} +void Cache::CachePlacemark(const QString &urlEnd, const QString &content) +{ + QString ret = QString::null; + QString filename = placemarkCache + QString(urlEnd) + ".plc"; + +#ifdef DEBUG_CACHE + qDebug() << "CachePlacemark: Filename:" << filename; +#endif // DEBUG_CACHE + QFileInfo File(filename);; + QDir dir = File.absoluteDir(); + QString path = dir.absolutePath(); +#ifdef DEBUG_CACHE + qDebug() << "CachePlacemark: Path:" << path; +#endif // DEBUG_CACHE + if (!dir.exists()) { +#ifdef DEBUG_CACHE + qDebug() << "CachePlacemark: Cache path doesn't exist, try to create"; +#endif // DEBUG_CACHE + if (!dir.mkpath(path)) { +#ifdef DEBUG_CACHE + qDebug() << "CachePlacemark: Could not create path"; +#endif // DEBUG_CACHE + } + } +#ifdef DEBUG_CACHE + qDebug() << "CachePlacemark: OpenFile:" << filename; +#endif // DEBUG_CACHE + QFile file(filename); + if (file.open(QIODevice::WriteOnly)) { +#ifdef DEBUG_CACHE + qDebug() << "CachePlacemark: File Opened!!!:" << filename; +#endif // DEBUG_CACHE + QTextStream stream(&file); + stream.setCodec("UTF-8"); + stream << content; + } +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.h index 3cbddf34f..df4e42077 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file cache.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file cache.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef CACHE_H #define CACHE_H @@ -31,32 +31,33 @@ #include "debugheader.h" namespace core { - class Cache +class Cache { +public: + static Cache *Instance(); + + + PureImageCache ImageCache; + QString CacheLocation(); + void setCacheLocation(const QString & value); + void CacheGeocoder(const QString &urlEnd, const QString &content); + QString GetGeocoderFromCache(const QString &urlEnd); + void CachePlacemark(const QString &urlEnd, const QString &content); + QString GetPlacemarkFromCache(const QString &urlEnd); + void CacheRoute(const QString &urlEnd, const QString &content); + QString GetRouteFromCache(const QString &urlEnd); + +private: + Cache(); + Cache(Cache const &) {} + Cache & operator=(Cache const &) { - public: - static Cache* Instance(); - - - PureImageCache ImageCache; - QString CacheLocation(); - void setCacheLocation(const QString& value); - void CacheGeocoder(const QString &urlEnd,const QString &content); - QString GetGeocoderFromCache(const QString &urlEnd); - void CachePlacemark(const QString &urlEnd,const QString &content); - QString GetPlacemarkFromCache(const QString &urlEnd); - void CacheRoute(const QString &urlEnd,const QString &content); - QString GetRouteFromCache(const QString &urlEnd); - - private: - Cache(); - Cache(Cache const&){} - Cache& operator=(Cache const&){ return *this; } - static Cache* m_pInstance; - QString cache; - QString routeCache; - QString geoCache; - QString placemarkCache; - }; - + return *this; + } + static Cache *m_pInstance; + QString cache; + QString routeCache; + QString geoCache; + QString placemarkCache; +}; } #endif // CACHE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.cpp index 7d1f1d661..59cec8169 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.cpp @@ -1,79 +1,79 @@ /** -****************************************************************************** -* -* @file cacheitemqueue.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file cacheitemqueue.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "cacheitemqueue.h" namespace core { - CacheItemQueue::CacheItemQueue(const MapType::Types &Type, const Point &Pos, const QByteArray &Img, const int &Zoom) - { - type=Type; - pos=Pos; - img=Img; - zoom=Zoom; - - } - - QByteArray CacheItemQueue::GetImg() - { - return img; - } - - MapType::Types CacheItemQueue::GetMapType() - { - return type; - } - Point CacheItemQueue::GetPosition() - { - return pos; - } - void CacheItemQueue::SetImg(const QByteArray &value) - { - img=value; - } - void CacheItemQueue::SetMapType(const MapType::Types &value) - { - type=value; - } - void CacheItemQueue::SetPosition(const Point &value) - { - pos=value; - } - - CacheItemQueue& CacheItemQueue::operator =(const CacheItemQueue &cSource) - { - img=cSource.img; - pos=cSource.pos; - type=cSource.type; - zoom=cSource.zoom; - return *this; - } - bool CacheItemQueue::operator ==(const CacheItemQueue &cSource) - { - bool b=(img==cSource.img)&& (pos==cSource.pos) && (type==cSource.type) && (zoom==cSource.zoom); - return b; - } +CacheItemQueue::CacheItemQueue(const MapType::Types &Type, const Point &Pos, const QByteArray &Img, const int &Zoom) +{ + type = Type; + pos = Pos; + img = Img; + zoom = Zoom; +} + +QByteArray CacheItemQueue::GetImg() +{ + return img; +} + +MapType::Types CacheItemQueue::GetMapType() +{ + return type; +} +Point CacheItemQueue::GetPosition() +{ + return pos; +} +void CacheItemQueue::SetImg(const QByteArray &value) +{ + img = value; +} +void CacheItemQueue::SetMapType(const MapType::Types &value) +{ + type = value; +} +void CacheItemQueue::SetPosition(const Point &value) +{ + pos = value; +} + +CacheItemQueue & CacheItemQueue::operator =(const CacheItemQueue &cSource) +{ + img = cSource.img; + pos = cSource.pos; + type = cSource.type; + zoom = cSource.zoom; + return *this; +} +bool CacheItemQueue::operator ==(const CacheItemQueue &cSource) +{ + bool b = (img == cSource.img) && (pos == cSource.pos) && (type == cSource.type) && (zoom == cSource.zoom); + + return b; +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.h index 786fb5d5c..1d610faee 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file cacheitemqueue.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file cacheitemqueue.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef CACHEITEMQUEUE_H #define CACHEITEMQUEUE_H @@ -32,38 +32,41 @@ #include - namespace core { - class CacheItemQueue +class CacheItemQueue { +public: + CacheItemQueue(const MapType::Types &Type, const core::Point &Pos, const QByteArray &Img, const int &Zoom); + CacheItemQueue() {}; + CacheItemQueue(const CacheItemQueue &cSource) { - public: - CacheItemQueue(const MapType::Types &Type,const core::Point &Pos,const QByteArray &Img,const int &Zoom); - CacheItemQueue(){}; - CacheItemQueue(const CacheItemQueue &cSource) - { - img=cSource.img; - pos=cSource.pos; - type=cSource.type; - zoom=cSource.zoom; - } - CacheItemQueue& operator= (const CacheItemQueue &cSource); - bool operator== (const CacheItemQueue &cSource); - void SetMapType(const MapType::Types &value); - void SetPosition(const core::Point &value); - void SetImg(const QByteArray &value); - MapType::Types GetMapType(); - core::Point GetPosition(); - QByteArray GetImg(); - int GetZoom(){return zoom;}; - void SetZoom(const int &value) {zoom=value;}; - private: - - - MapType::Types type; - core::Point pos; - QByteArray img; - int zoom; + img = cSource.img; + pos = cSource.pos; + type = cSource.type; + zoom = cSource.zoom; + } + CacheItemQueue & operator=(const CacheItemQueue &cSource); + bool operator==(const CacheItemQueue &cSource); + void SetMapType(const MapType::Types &value); + void SetPosition(const core::Point &value); + void SetImg(const QByteArray &value); + MapType::Types GetMapType(); + core::Point GetPosition(); + QByteArray GetImg(); + int GetZoom() + { + return zoom; }; + void SetZoom(const int &value) + { + zoom = value; + }; +private: + + MapType::Types type; + core::Point pos; + QByteArray img; + int zoom; +}; } #endif // CACHEITEMQUEUE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/debugheader.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/debugheader.h index 9b410c9c2..d4590fb59 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/debugheader.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/debugheader.h @@ -1,13 +1,13 @@ #ifndef DEBUGHEADER_H #define DEBUGHEADER_H -//#define DEBUG_MEMORY_CACHE -//#define DEBUG_CACHE -//#define DEBUG_GMAPS -//#define DEBUG_PUREIMAGECACHE -//#define DEBUG_TILECACHEQUEUE -//#define DEBUG_URLFACTORY -//#define DEBUG_MEMORY_CACHE -//#define DEBUG_GetGeocoderFromCache +// #define DEBUG_MEMORY_CACHE +// #define DEBUG_CACHE +// #define DEBUG_GMAPS +// #define DEBUG_PUREIMAGECACHE +// #define DEBUG_TILECACHEQUEUE +// #define DEBUG_URLFACTORY +// #define DEBUG_MEMORY_CACHE +// #define DEBUG_GetGeocoderFromCache #endif // DEBUGHEADER_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.cpp index ea2242651..fd4554c7d 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.cpp @@ -1,31 +1,30 @@ /** -****************************************************************************** -* -* @file diagnostics.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file diagnostics.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "diagnostics.h" -diagnostics::diagnostics():networkerrors(0),emptytiles(0),timeouts(0),runningThreads(0),tilesFromMem(0),tilesFromNet(0),tilesFromDB(0) -{ -} +diagnostics::diagnostics() : networkerrors(0), emptytiles(0), timeouts(0), runningThreads(0), tilesFromMem(0), tilesFromNet(0), tilesFromDB(0) +{} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.h index f9c503b97..bad0af01f 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.h @@ -1,46 +1,46 @@ /** -****************************************************************************** -* -* @file diagnostics.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file diagnostics.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef DIAGNOSTICS_H #define DIAGNOSTICS_H #include -struct diagnostics -{ +struct diagnostics { diagnostics(); - int networkerrors; - int emptytiles; - int timeouts; - int runningThreads; - int tilesFromMem; - int tilesFromNet; - int tilesFromDB; + int networkerrors; + int emptytiles; + int timeouts; + int runningThreads; + int tilesFromMem; + int tilesFromNet; + int tilesFromDB; QString toString() { return QString("Network errors:%1\nEmpty Tiles:%2\nTimeOuts:%3\nRunningThreads:%4\nTilesFromMem:%5\nTilesFromNet:%6\nTilesFromDB:%7").arg(networkerrors).arg(emptytiles).arg(timeouts).arg(runningThreads).arg(tilesFromMem).arg(tilesFromNet).arg(tilesFromDB); - ; + + ; } }; diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/geodecoderstatus.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/geodecoderstatus.h index 66fe1e56e..86930ab5b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/geodecoderstatus.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/geodecoderstatus.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file geodecoderstatus.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file geodecoderstatus.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef GEODECODERSTATUS_H #define GEODECODERSTATUS_H @@ -32,100 +32,97 @@ #include #include namespace core { - class GeoCoderStatusCode:public QObject - { - Q_OBJECT - Q_ENUMS(Types) - public: - enum Types - { - /// - /// unknow response - /// - Unknow = -1, +class GeoCoderStatusCode : public QObject { + Q_OBJECT Q_ENUMS(Types) +public: + enum Types { + /// + /// unknow response + /// + Unknow = -1, - /// - /// No errors occurred; the address was successfully parsed and its geocode has been returned. - /// - G_GEO_SUCCESS=200, + /// + /// No errors occurred; the address was successfully parsed and its geocode has been returned. + /// + G_GEO_SUCCESS = 200, - /// - /// A directions request could not be successfully parsed. - /// For example, the request may have been rejected if it contained more than the maximum number of waypoints allowed. - /// - G_GEO_BAD_REQUEST=400, + /// + /// A directions request could not be successfully parsed. + /// For example, the request may have been rejected if it contained more than the maximum number of waypoints allowed. + /// + G_GEO_BAD_REQUEST = 400, - /// - /// A geocoding or directions request could not be successfully processed, yet the exact reason for the failure is not known. - /// - G_GEO_SERVER_ERROR=500, + /// + /// A geocoding or directions request could not be successfully processed, yet the exact reason for the failure is not known. + /// + G_GEO_SERVER_ERROR = 500, - /// - /// The HTTP q parameter was either missing or had no value. - /// For geocoding requests, this means that an empty address was specified as input. For directions requests, this means that no query was specified in the input. - /// - G_GEO_MISSING_QUERY=601, + /// + /// The HTTP q parameter was either missing or had no value. + /// For geocoding requests, this means that an empty address was specified as input. For directions requests, this means that no query was specified in the input. + /// + G_GEO_MISSING_QUERY = 601, - /// - /// Synonym for G_GEO_MISSING_QUERY. - /// - G_GEO_MISSING_ADDRESS=601, + /// + /// Synonym for G_GEO_MISSING_QUERY. + /// + G_GEO_MISSING_ADDRESS = 601, - /// - /// No corresponding geographic location could be found for the specified address. - /// This may be due to the fact that the address is relatively new, or it may be incorrect. - /// - G_GEO_UNKNOWN_ADDRESS=602, + /// + /// No corresponding geographic location could be found for the specified address. + /// This may be due to the fact that the address is relatively new, or it may be incorrect. + /// + G_GEO_UNKNOWN_ADDRESS = 602, - /// - /// The geocode for the given address or the route for the given directions query cannot be returned due to legal or contractual reasons. - /// - G_GEO_UNAVAILABLE_ADDRESS=603, + /// + /// The geocode for the given address or the route for the given directions query cannot be returned due to legal or contractual reasons. + /// + G_GEO_UNAVAILABLE_ADDRESS = 603, - /// - /// The GDirections object could not compute directions between the points mentioned in the query. - /// This is usually because there is no route available between the two points, or because we do not have data for routing in that region. - /// - G_GEO_UNKNOWN_DIRECTIONS=604, + /// + /// The GDirections object could not compute directions between the points mentioned in the query. + /// This is usually because there is no route available between the two points, or because we do not have data for routing in that region. + /// + G_GEO_UNKNOWN_DIRECTIONS = 604, - /// - /// The given key is either invalid or does not match the domain for which it was given. - /// - G_GEO_BAD_KEY=610, - - /// - /// The given key has gone over the requests limit in the 24 hour period or has submitted too many requests in too short a period of time. - /// If you're sending multiple requests in parallel or in a tight loop, use a timer or pause in your code to make sure you don't send the requests too quickly. - /// - G_GEO_TOO_MANY_QUERIES=620 - - }; - static QString StrByType(Types const& value) - { - QMetaObject metaObject = GeoCoderStatusCode().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - QString s=metaEnum.valueToKey(value); - return s; - } - static Types TypeByStr(QString const& value) - { - QMetaObject metaObject = GeoCoderStatusCode().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - Types s=(Types)metaEnum.keyToValue(value.toLatin1()); - return s; - } - static QStringList TypesList() - { - QStringList ret; - QMetaObject metaObject = GeoCoderStatusCode().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - for(int x=0;x + /// The given key is either invalid or does not match the domain for which it was given. + /// + G_GEO_BAD_KEY = 610, + /// + /// The given key has gone over the requests limit in the 24 hour period or has submitted too many requests in too short a period of time. + /// If you're sending multiple requests in parallel or in a tight loop, use a timer or pause in your code to make sure you don't send the requests too quickly. + /// + G_GEO_TOO_MANY_QUERIES = 620 }; + static QString StrByType(Types const & value) + { + QMetaObject metaObject = GeoCoderStatusCode().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + QString s = metaEnum.valueToKey(value); + + return s; + } + static Types TypeByStr(QString const & value) + { + QMetaObject metaObject = GeoCoderStatusCode().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + Types s = (Types)metaEnum.keyToValue(value.toLatin1()); + + return s; + } + static QStringList TypesList() + { + QStringList ret; + QMetaObject metaObject = GeoCoderStatusCode().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + + for (int x = 0; x < metaEnum.keyCount(); ++x) { + ret.append(metaEnum.key(x)); + } + return ret; + } +}; } #endif // GEODECODERSTATUS_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.cpp index b674f4f46..235ba02d1 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.cpp @@ -1,69 +1,68 @@ /** -****************************************************************************** -* -* @file kibertilecache.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file kibertilecache.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "kibertilecache.h" -//TODO add readwrite lock +// TODO add readwrite lock namespace core { - KiberTileCache::KiberTileCache() - { - memoryCacheSize = 0; - _MemoryCacheCapacity = 22; - } - - void KiberTileCache::setMemoryCacheCapacity(const int &value) - { - kiberCacheLock.lockForWrite(); - _MemoryCacheCapacity=value; - kiberCacheLock.unlock(); - } - int KiberTileCache::MemoryCacheCapacity() - { - kiberCacheLock.lockForRead(); - return _MemoryCacheCapacity; - kiberCacheLock.unlock(); - } - - void KiberTileCache::RemoveMemoryOverload() - { - while(MemoryCacheSize()>MemoryCacheCapacity()) - { - if(cachequeue.count()>0 && list.count()>0) - { -#ifdef DEBUG_MEMORY_CACHE - qDebug()<<"Cleaning Memory cache="<<" started with "< MemoryCacheCapacity()) { + if (cachequeue.count() > 0 && list.count() > 0) { +#ifdef DEBUG_MEMORY_CACHE + qDebug() << "Cleaning Memory cache=" << " started with " << cachequeue.count() << " tile " << "ocupying " << memoryCacheSize << " bytes"; +#endif + RawTile first = list.dequeue(); + memoryCacheSize -= cachequeue.value(first).size(); + cachequeue.remove(first); + } + } +#ifdef DEBUG_MEMORY_CACHE + qDebug() << "Cleaning Memory cache=" << " ended with " << cachequeue.count() << " tile " << "ocupying " << memoryCacheSize << " bytes"; +#endif +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.h index 5fe2292de..7102d112a 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file kibertilecache.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file kibertilecache.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef KIBERTILECACHE_H #define KIBERTILECACHE_H @@ -34,25 +34,23 @@ #include #include "debugheader.h" namespace core { - class KiberTileCache +class KiberTileCache { +public: + KiberTileCache(); + + void setMemoryCacheCapacity(const int &value); + int MemoryCacheCapacity(); + double MemoryCacheSize() { - public: - KiberTileCache(); - - void setMemoryCacheCapacity(const int &value); - int MemoryCacheCapacity(); - double MemoryCacheSize(){return memoryCacheSize/1048576.0;} - void RemoveMemoryOverload(); - QReadWriteLock kiberCacheLock; - QHash cachequeue; - QQueue list; - long memoryCacheSize; - private: - int _MemoryCacheCapacity; - - }; - - - + return memoryCacheSize / 1048576.0; + } + void RemoveMemoryOverload(); + QReadWriteLock kiberCacheLock; + QHash cachequeue; + QQueue list; + long memoryCacheSize; +private: + int _MemoryCacheCapacity; +}; } #endif // KIBERTILECACHE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.cpp index 52da98b80..a9856b983 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.cpp @@ -1,100 +1,97 @@ /** -****************************************************************************** -* -* @file languagetype.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file languagetype.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "languagetype.h" - namespace core { - LanguageType::LanguageType() - { - list - <<"ar" - <<"bg" - <<"bn" - <<"ca" - <<"cs" - <<"da" - <<"de" - <<"el" - <<"en" - <<"en-AU" - <<"en-GB" - <<"es" - <<"eu" - <<"fi" - <<"fil" - <<"fr" - <<"gl" - <<"gu" - <<"hi" - <<"hr" - <<"hu" - <<"id" - <<"it" - <<"iw" - <<"ja" - <<"kn" - <<"ko" - <<"lt" - <<"lv" - <<"ml" - <<"mr" - <<"nl" - <<"nn" - <<"no" - <<"or" - <<"pl" - <<"pt" - <<"pt-BR" - <<"pt-PT" - <<"rm" - <<"ro" - <<"ru" - <<"sk" - <<"sl" - <<"sr" - <<"sv" - <<"ta" - <<"te" - <<"th" - <<"tr" - <<"uk" - <<"vi" - <<"zh-CN" - <<"zh-TW"; - - } - QString LanguageType::toShortString(Types type) - { - return list[type]; - } - LanguageType::~LanguageType() - { - list.clear(); - } - +LanguageType::LanguageType() +{ + list + << "ar" + << "bg" + << "bn" + << "ca" + << "cs" + << "da" + << "de" + << "el" + << "en" + << "en-AU" + << "en-GB" + << "es" + << "eu" + << "fi" + << "fil" + << "fr" + << "gl" + << "gu" + << "hi" + << "hr" + << "hu" + << "id" + << "it" + << "iw" + << "ja" + << "kn" + << "ko" + << "lt" + << "lv" + << "ml" + << "mr" + << "nl" + << "nn" + << "no" + << "or" + << "pl" + << "pt" + << "pt-BR" + << "pt-PT" + << "rm" + << "ro" + << "ru" + << "sk" + << "sl" + << "sr" + << "sv" + << "ta" + << "te" + << "th" + << "tr" + << "uk" + << "vi" + << "zh-CN" + << "zh-TW"; +} +QString LanguageType::toShortString(Types type) +{ + return list[type]; +} +LanguageType::~LanguageType() +{ + list.clear(); +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.h index 17cfe3b67..1a6a963e1 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file languagetype.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file languagetype.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef LANGUAGETYPE_H #define LANGUAGETYPE_H @@ -34,100 +34,98 @@ namespace core { - class LanguageType:public QObject - { - Q_OBJECT - Q_ENUMS(Types) - public: - enum Types - { - Arabic, - Bulgarian, - Bengali, - Catalan, - Czech, - Danish, - German, - Greek, - English, - EnglishAustralian, - EnglishGreatBritain, - Spanish, - Basque, - Finnish, - Filipino, - French, - Galician, - Gujarati, - Hindi, - Croatian, - Hungarian, - Indonesian, - Italian, - Hebrew, - Japanese, - Kannada, - Korean, - Lithuanian, - Latvian, - Malayalam, - Marathi, - Dutch, - NorwegianNynorsk, - Norwegian, - Oriya, - Polish, - Portuguese, - PortugueseBrazil, - PortuguesePortugal, - Romansch, - Romanian, - Russian, - Slovak, - Slovenian, - Serbian, - Swedish, - Tamil, - Telugu, - Thai, - Turkish, - Ukrainian, - Vietnamese, - ChineseSimplified, - ChineseTraditional - }; - - static QString StrByType(Types const& value) - { - QMetaObject metaObject = LanguageType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - QString s=metaEnum.valueToKey(value); - return s; - } - static Types TypeByStr(QString const& value) - { - QMetaObject metaObject = LanguageType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - Types s=(Types)metaEnum.keyToValue(value.toLatin1()); - return s; - } - static QStringList TypesList() - { - QStringList ret; - QMetaObject metaObject = LanguageType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - for(int x=0;x @@ -31,99 +31,97 @@ #include namespace core { - class MapType:public QObject - { - Q_OBJECT - Q_ENUMS(Types) - public: - enum Types - { - GoogleMap=1, - GoogleSatellite=4, - GoogleLabels=8, - GoogleTerrain=16, - GoogleHybrid=20, +class MapType : public QObject { + Q_OBJECT Q_ENUMS(Types) +public: + enum Types { + GoogleMap = 1, + GoogleSatellite = 4, + GoogleLabels = 8, + GoogleTerrain = 16, + GoogleHybrid = 20, - GoogleMapChina=22, - GoogleSatelliteChina=24, - GoogleLabelsChina=26, - GoogleTerrainChina=28, - GoogleHybridChina=29, + GoogleMapChina = 22, + GoogleSatelliteChina = 24, + GoogleLabelsChina = 26, + GoogleTerrainChina = 28, + GoogleHybridChina = 29, - OpenStreetMap=32, - OpenStreetOsm=33, - OpenStreetMapSurfer=34, - OpenStreetMapSurferTerrain=35, + OpenStreetMap = 32, + OpenStreetOsm = 33, + OpenStreetMapSurfer = 34, + OpenStreetMapSurferTerrain = 35, - YahooMap=64, - YahooSatellite=128, - YahooLabels=256, - YahooHybrid=333, + YahooMap = 64, + YahooSatellite = 128, + YahooLabels = 256, + YahooHybrid = 333, - BingMap=444, - BingSatellite=555, - BingHybrid=666, + BingMap = 444, + BingSatellite = 555, + BingHybrid = 666, - ArcGIS_Map=777, - ArcGIS_Satellite=788, - ArcGIS_ShadedRelief=799, - ArcGIS_Terrain=811, + ArcGIS_Map = 777, + ArcGIS_Satellite = 788, + ArcGIS_ShadedRelief = 799, + ArcGIS_Terrain = 811, - // use these numbers to clean up old stuff - //ArcGIS_MapsLT_Map_Old= 877, - //ArcGIS_MapsLT_OrtoFoto_Old = 888, - //ArcGIS_MapsLT_Map_Labels_Old = 890, - //ArcGIS_MapsLT_Map_Hybrid_Old = 899, - //ArcGIS_MapsLT_Map=977, - //ArcGIS_MapsLT_OrtoFoto=988, - //ArcGIS_MapsLT_Map_Labels=990, - //ArcGIS_MapsLT_Map_Hybrid=999, - //ArcGIS_MapsLT_Map=978, - //ArcGIS_MapsLT_OrtoFoto=989, - //ArcGIS_MapsLT_Map_Labels=991, - //ArcGIS_MapsLT_Map_Hybrid=998, + // use these numbers to clean up old stuff + // ArcGIS_MapsLT_Map_Old= 877, + // ArcGIS_MapsLT_OrtoFoto_Old = 888, + // ArcGIS_MapsLT_Map_Labels_Old = 890, + // ArcGIS_MapsLT_Map_Hybrid_Old = 899, + // ArcGIS_MapsLT_Map=977, + // ArcGIS_MapsLT_OrtoFoto=988, + // ArcGIS_MapsLT_Map_Labels=990, + // ArcGIS_MapsLT_Map_Hybrid=999, + // ArcGIS_MapsLT_Map=978, + // ArcGIS_MapsLT_OrtoFoto=989, + // ArcGIS_MapsLT_Map_Labels=991, + // ArcGIS_MapsLT_Map_Hybrid=998, - ArcGIS_MapsLT_Map=1000, - ArcGIS_MapsLT_OrtoFoto=1001, - ArcGIS_MapsLT_Map_Labels=1002, - ArcGIS_MapsLT_Map_Hybrid=1003, + ArcGIS_MapsLT_Map = 1000, + ArcGIS_MapsLT_OrtoFoto = 1001, + ArcGIS_MapsLT_Map_Labels = 1002, + ArcGIS_MapsLT_Map_Hybrid = 1003, - PergoTurkeyMap = 2001, - SigPacSpainMap = 3001, + PergoTurkeyMap = 2001, + SigPacSpainMap = 3001, - GoogleMapKorea=4001, - GoogleSatelliteKorea=4002, - GoogleLabelsKorea=4003, - GoogleHybridKorea=4005, + GoogleMapKorea = 4001, + GoogleSatelliteKorea = 4002, + GoogleLabelsKorea = 4003, + GoogleHybridKorea = 4005, - YandexMapRu = 5000 - }; - static QString StrByType(Types const& value) - { - QMetaObject metaObject = MapType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - QString s=metaEnum.valueToKey(value); - return s; - } - static Types TypeByStr(QString const& value) - { - QMetaObject metaObject = MapType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - Types s=(Types)metaEnum.keyToValue(value.toLatin1()); - return s; - } - static QStringList TypesList() - { - QStringList ret; - QMetaObject metaObject = MapType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - for(int x=0;x #include "debugheader.h" namespace core { - class MemoryCache - { - public: - MemoryCache(); - - KiberTileCache TilesInMemory; - QByteArray GetTileFromMemoryCache(const RawTile &tile); - void AddTileToMemoryCache(const RawTile &tile, const QByteArray &pic); - QReadWriteLock kiberCacheLock; - }; - +class MemoryCache { +public: + MemoryCache(); + KiberTileCache TilesInMemory; + QByteArray GetTileFromMemoryCache(const RawTile &tile); + void AddTileToMemoryCache(const RawTile &tile, const QByteArray &pic); + QReadWriteLock kiberCacheLock; +}; } #endif // MEMORYCACHE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp index 86660fd3e..fd249f66d 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp @@ -1,286 +1,270 @@ /** -****************************************************************************** -* -* @file OPMaps.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file OPMaps.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "opmaps.h" namespace core { - OPMaps* OPMaps::m_pInstance=0; +OPMaps *OPMaps::m_pInstance = 0; - OPMaps* OPMaps::Instance() - { - if(!m_pInstance) - m_pInstance=new OPMaps; - return m_pInstance; - } - OPMaps::OPMaps():RetryLoadTile(2),useMemoryCache(true) - { - accessmode=AccessMode::ServerAndCache; - Language=LanguageType::PortuguesePortugal; - LanguageStr=LanguageType().toShortString(Language); - Cache::Instance(); - - } - - - OPMaps::~OPMaps() - { - TileDBcacheQueue.wait(); - } - - - - QByteArray OPMaps::GetImageFrom(const MapType::Types &type,const Point &pos,const int &zoom) - { -#ifdef DEBUG_TIMINGS - QTime time; - time.restart(); -#endif -#ifdef DEBUG_GMAPS - qDebug()<<"Entered GetImageFrom"; -#endif //DEBUG_GMAPS - QByteArray ret; - - if(useMemoryCache) - { -#ifdef DEBUG_GMAPS - qDebug()<<"Try Tile from memory:Size="<ImageCache.GetImageFromCache(type,pos,zoom); - if(!ret.isEmpty()) - { - errorvars.lock(); - ++diag.tilesFromDB; - errorvars.unlock(); -#ifdef DEBUG_GMAPS - qDebug()<<"Tile found in Database"; -#endif //DEBUG_GMAPS - if(useMemoryCache) - { -#ifdef DEBUG_GMAPS - qDebug()<<"Add Tile to memory"; -#endif //DEBUG_GMAPS - AddTileToMemoryCache(RawTile(type,pos,zoom),ret); - } - return ret; - } - } - if(accessmode!=AccessMode::CacheOnly) - { - QEventLoop q; - QNetworkReply *reply; - QNetworkRequest qheader; - QNetworkAccessManager network; - QTimer tT; - tT.setSingleShot(true); - connect(&network, SIGNAL(finished(QNetworkReply*)), - &q, SLOT(quit())); - connect(&tT, SIGNAL(timeout()), &q, SLOT(quit())); - network.setProxy(Proxy); -#ifdef DEBUG_GMAPS - qDebug()<<"Try Tile from the Internet"; -#endif //DEBUG_GMAPS -#ifdef DEBUG_TIMINGS - qDebug()<<"opmaps before make image url"<error()!=QNetworkReply::NoError)) - { - errorvars.lock(); - ++diag.networkerrors; - errorvars.unlock(); - reply->deleteLater(); - return ret; - } - ret=reply->readAll(); - reply->deleteLater();//TODO can't this be global?? - if(ret.isEmpty()) - { -#ifdef DEBUG_GMAPS - qDebug()<<"Invalid Tile"; -#endif //DEBUG_GMAPS - errorvars.lock(); - ++diag.emptytiles; - errorvars.unlock(); - return ret; - } -#ifdef DEBUG_GMAPS - qDebug()<<"Received Tile from the Internet"; -#endif //DEBUG_GMAPS - errorvars.lock(); - ++diag.tilesFromNet; - errorvars.unlock(); - if (useMemoryCache) - { -#ifdef DEBUG_GMAPS - qDebug()<<"Add Tile to memory cache"; -#endif //DEBUG_GMAPS - AddTileToMemoryCache(RawTile(type,pos,zoom),ret); - } - if(accessmode!=AccessMode::ServerOnly) - { -#ifdef DEBUG_GMAPS - qDebug()<<"Add tile to DataBase"; -#endif //DEBUG_GMAPS - CacheItemQueue * item=new CacheItemQueue(type,pos,ret,zoom); - TileDBcacheQueue.EnqueueCacheTask(item); - } - - - } - } -#ifdef DEBUG_GMAPS - qDebug()<<"Entered GetImageFrom"; -#endif //DEBUG_GMAPS - return ret; - } - - bool OPMaps::ExportToGMDB(const QString &file) - { - return Cache::Instance()->ImageCache.ExportMapDataToDB(Cache::Instance()->ImageCache.GtileCache()+QDir::separator()+"Data.qmdb",file); - } - bool OPMaps::ImportFromGMDB(const QString &file) - { - return Cache::Instance()->ImageCache.ExportMapDataToDB(file,Cache::Instance()->ImageCache.GtileCache()+QDir::separator()+"Data.qmdb"); - } - - diagnostics OPMaps::GetDiagnostics() - { - diagnostics i; - errorvars.lock(); - i=diag; - errorvars.unlock(); - return i; +OPMaps *OPMaps::Instance() +{ + if (!m_pInstance) { + m_pInstance = new OPMaps; } + return m_pInstance; +} +OPMaps::OPMaps() : RetryLoadTile(2), useMemoryCache(true) +{ + accessmode = AccessMode::ServerAndCache; + Language = LanguageType::PortuguesePortugal; + LanguageStr = LanguageType().toShortString(Language); + Cache::Instance(); } + +OPMaps::~OPMaps() +{ + TileDBcacheQueue.wait(); +} + + +QByteArray OPMaps::GetImageFrom(const MapType::Types &type, const Point &pos, const int &zoom) +{ +#ifdef DEBUG_TIMINGS + QTime time; + time.restart(); +#endif +#ifdef DEBUG_GMAPS + qDebug() << "Entered GetImageFrom"; +#endif // DEBUG_GMAPS + QByteArray ret; + + if (useMemoryCache) { +#ifdef DEBUG_GMAPS + qDebug() << "Try Tile from memory:Size=" << TilesInMemory.MemoryCacheSize(); +#endif // DEBUG_GMAPS + ret = GetTileFromMemoryCache(RawTile(type, pos, zoom)); + if (!ret.isEmpty()) { + errorvars.lock(); + ++diag.tilesFromMem; + errorvars.unlock(); + } + } + if (ret.isEmpty()) { +#ifdef DEBUG_GMAPS + qDebug() << "Tile not in memory"; +#endif // DEBUG_GMAPS + if (accessmode != (AccessMode::ServerOnly)) { +#ifdef DEBUG_GMAPS + qDebug() << "Try tile from DataBase"; +#endif // DEBUG_GMAPS + ret = Cache::Instance()->ImageCache.GetImageFromCache(type, pos, zoom); + if (!ret.isEmpty()) { + errorvars.lock(); + ++diag.tilesFromDB; + errorvars.unlock(); +#ifdef DEBUG_GMAPS + qDebug() << "Tile found in Database"; +#endif // DEBUG_GMAPS + if (useMemoryCache) { +#ifdef DEBUG_GMAPS + qDebug() << "Add Tile to memory"; +#endif // DEBUG_GMAPS + AddTileToMemoryCache(RawTile(type, pos, zoom), ret); + } + return ret; + } + } + if (accessmode != AccessMode::CacheOnly) { + QEventLoop q; + QNetworkReply *reply; + QNetworkRequest qheader; + QNetworkAccessManager network; + QTimer tT; + tT.setSingleShot(true); + connect(&network, SIGNAL(finished(QNetworkReply *)), + &q, SLOT(quit())); + connect(&tT, SIGNAL(timeout()), &q, SLOT(quit())); + network.setProxy(Proxy); +#ifdef DEBUG_GMAPS + qDebug() << "Try Tile from the Internet"; +#endif // DEBUG_GMAPS +#ifdef DEBUG_TIMINGS + qDebug() << "opmaps before make image url" << time.elapsed(); +#endif + QString url = MakeImageUrl(type, pos, zoom, LanguageStr); +#ifdef DEBUG_TIMINGS + qDebug() << "opmaps after make image url" << time.elapsed(); +#endif // url "http://vec02.maps.yandex.ru/tiles?l=map&v=2.10.2&x=7&y=5&z=3" string + // "http://map3.pergo.com.tr/tile/02/000/000/007/000/000/002.png" + qheader.setUrl(QUrl(url)); + qheader.setRawHeader("User-Agent", UserAgent); + qheader.setRawHeader("Accept", "*/*"); + switch (type) { + case MapType::GoogleMap: + case MapType::GoogleSatellite: + case MapType::GoogleLabels: + case MapType::GoogleTerrain: + case MapType::GoogleHybrid: + { + qheader.setRawHeader("Referrer", "http://maps.google.com/"); + } + break; + + case MapType::GoogleMapChina: + case MapType::GoogleSatelliteChina: + case MapType::GoogleLabelsChina: + case MapType::GoogleTerrainChina: + case MapType::GoogleHybridChina: + { + qheader.setRawHeader("Referrer", "http://ditu.google.cn/"); + } + break; + + case MapType::BingHybrid: + case MapType::BingMap: + case MapType::BingSatellite: + { + qheader.setRawHeader("Referrer", "http://www.bing.com/maps/"); + } + break; + + case MapType::YahooHybrid: + case MapType::YahooLabels: + case MapType::YahooMap: + case MapType::YahooSatellite: + { + qheader.setRawHeader("Referrer", "http://maps.yahoo.com/"); + } + break; + + case MapType::ArcGIS_MapsLT_Map_Labels: + case MapType::ArcGIS_MapsLT_Map: + case MapType::ArcGIS_MapsLT_OrtoFoto: + case MapType::ArcGIS_MapsLT_Map_Hybrid: + { + qheader.setRawHeader("Referrer", "http://www.maps.lt/map_beta/"); + } + break; + + case MapType::OpenStreetMapSurfer: + case MapType::OpenStreetMapSurferTerrain: + { + qheader.setRawHeader("Referrer", "http://www.mapsurfer.net/"); + } + break; + + case MapType::OpenStreetMap: + case MapType::OpenStreetOsm: + { + qheader.setRawHeader("Referrer", "http://www.openstreetmap.org/"); + } + break; + + case MapType::YandexMapRu: + { + qheader.setRawHeader("Referrer", "http://maps.yandex.ru/"); + } + break; + default: + break; + } + reply = network.get(qheader); + tT.start(Timeout); + q.exec(); + + if (!tT.isActive()) { + errorvars.lock(); + ++diag.timeouts; + errorvars.unlock(); + return ret; + } + tT.stop(); + if ((reply->error() != QNetworkReply::NoError)) { + errorvars.lock(); + ++diag.networkerrors; + errorvars.unlock(); + reply->deleteLater(); + return ret; + } + ret = reply->readAll(); + reply->deleteLater(); // TODO can't this be global?? + if (ret.isEmpty()) { +#ifdef DEBUG_GMAPS + qDebug() << "Invalid Tile"; +#endif // DEBUG_GMAPS + errorvars.lock(); + ++diag.emptytiles; + errorvars.unlock(); + return ret; + } +#ifdef DEBUG_GMAPS + qDebug() << "Received Tile from the Internet"; +#endif // DEBUG_GMAPS + errorvars.lock(); + ++diag.tilesFromNet; + errorvars.unlock(); + if (useMemoryCache) { +#ifdef DEBUG_GMAPS + qDebug() << "Add Tile to memory cache"; +#endif // DEBUG_GMAPS + AddTileToMemoryCache(RawTile(type, pos, zoom), ret); + } + if (accessmode != AccessMode::ServerOnly) { +#ifdef DEBUG_GMAPS + qDebug() << "Add tile to DataBase"; +#endif // DEBUG_GMAPS + CacheItemQueue *item = new CacheItemQueue(type, pos, ret, zoom); + TileDBcacheQueue.EnqueueCacheTask(item); + } + } + } +#ifdef DEBUG_GMAPS + qDebug() << "Entered GetImageFrom"; +#endif // DEBUG_GMAPS + return ret; +} + +bool OPMaps::ExportToGMDB(const QString &file) +{ + return Cache::Instance()->ImageCache.ExportMapDataToDB(Cache::Instance()->ImageCache.GtileCache() + QDir::separator() + "Data.qmdb", file); +} +bool OPMaps::ImportFromGMDB(const QString &file) +{ + return Cache::Instance()->ImageCache.ExportMapDataToDB(file, Cache::Instance()->ImageCache.GtileCache() + QDir::separator() + "Data.qmdb"); +} + +diagnostics OPMaps::GetDiagnostics() +{ + diagnostics i; + + errorvars.lock(); + i = diag; + errorvars.unlock(); + return i; +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.h index 81b7daf2c..f5cf9dd43 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file OPMaps.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file OPMaps.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef OPMaps_H #define OPMaps_H @@ -41,54 +41,66 @@ #include "urlfactory.h" #include "diagnostics.h" -//#include "point.h" +// #include "point.h" namespace core { - class OPMaps: public MemoryCache,public AllLayersOfType,public UrlFactory +class OPMaps : public MemoryCache, public AllLayersOfType, public UrlFactory { +public: ~OPMaps(); + + static OPMaps *Instance(); + bool ImportFromGMDB(const QString &file); + bool ExportToGMDB(const QString &file); + /// + /// timeout for map connections + /// + + + QByteArray GetImageFrom(const MapType::Types &type, const core::Point &pos, const int &zoom); + bool UseMemoryCache() { + return useMemoryCache; + } // TODO + void setUseMemoryCache(const bool & value) + { + useMemoryCache = value; + } + void setLanguage(const LanguageType::Types & language) + { + Language = language; + } // TODO + LanguageType::Types GetLanguage() + { + return Language; + } // TODO + AccessMode::Types GetAccessMode() const + { + return accessmode; + } + void setAccessMode(const AccessMode::Types & mode) + { + accessmode = mode; + } + int RetryLoadTile; + diagnostics GetDiagnostics(); - - public: - - ~OPMaps(); - - static OPMaps* Instance(); - bool ImportFromGMDB(const QString &file); - bool ExportToGMDB(const QString &file); - /// - /// timeout for map connections - /// - - - QByteArray GetImageFrom(const MapType::Types &type,const core::Point &pos,const int &zoom); - bool UseMemoryCache(){return useMemoryCache;}//TODO - void setUseMemoryCache(const bool& value){useMemoryCache=value;} - void setLanguage(const LanguageType::Types& language){Language=language;}//TODO - LanguageType::Types GetLanguage(){return Language;}//TODO - AccessMode::Types GetAccessMode()const{return accessmode;} - void setAccessMode(const AccessMode::Types& mode){accessmode=mode;} - int RetryLoadTile; - diagnostics GetDiagnostics(); - - private: - bool useMemoryCache; - LanguageType::Types Language; - AccessMode::Types accessmode; - // PureImageCache ImageCacheLocal;//TODO Criar acesso Get Set - TileCacheQueue TileDBcacheQueue; - OPMaps(); - OPMaps(OPMaps const&){} - OPMaps& operator=(OPMaps const&){ return *this; } - static OPMaps* m_pInstance; - diagnostics diag; - QMutex errorvars; - protected: - // MemoryCache TilesInMemory; - - - - }; - +private: + bool useMemoryCache; + LanguageType::Types Language; + AccessMode::Types accessmode; + // PureImageCache ImageCacheLocal;//TODO Criar acesso Get Set + TileCacheQueue TileDBcacheQueue; + OPMaps(); + OPMaps(OPMaps const &) {} + OPMaps & operator=(OPMaps const &) + { + return *this; + } + static OPMaps *m_pInstance; + diagnostics diag; + QMutex errorvars; +protected: + // MemoryCache TilesInMemory; +}; } #endif // OPMaps_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.cpp index 72d664137..ce94cebfc 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.cpp @@ -1,28 +1,27 @@ /** -****************************************************************************** -* -* @file placemark.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file placemark.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "placemark.h" - diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.h index 7639bc3ca..d4e15d6cc 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file placemark.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file placemark.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef PLACEMARK_H #define PLACEMARK_H @@ -31,24 +31,33 @@ namespace core { - class Placemark +class Placemark { +public: + Placemark(const QString &address) { - public: - Placemark(const QString &address) - { - this->address = address; - } - QString Address(){return address;} - int Accuracy(){return accuracy;} - void SetAddress(const QString &adr){address=adr;} - void SetAccuracy(const int &value){accuracy=value;} - private: + this->address = address; + } + QString Address() + { + return address; + } + int Accuracy() + { + return accuracy; + } + void SetAddress(const QString &adr) + { + address = adr; + } + void SetAccuracy(const int &value) + { + accuracy = value; + } +private: - QString address; - int accuracy; - protected: - - - }; + QString address; + int accuracy; +protected: +}; } #endif // PLACEMARK_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.cpp index c41a66880..81bda5271 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.cpp @@ -1,74 +1,73 @@ /** -****************************************************************************** -* -* @file point.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file point.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "point.h" #include "size.h" namespace core { - Point::Point(int dw) - { - this->x=(short)Point::LOWORD(dw); - this->y=(short)Point::HIWORD(dw); - empty=false; - } - Point::Point(Size sz) - { - this->x=sz.Width(); - this->y=sz.Height(); - empty=false; - } - Point::Point(int x, int y) - { - this->x=x; - this->y=y; - empty=false; - } - Point::Point():x(0),y(0),empty(true) - {} - uint qHash(Point const& point) - { - return point.x^point.y; - } - bool operator==(Point const &lhs,Point const &rhs) - { - return (lhs.x==rhs.x && lhs.y==rhs.y); - } - bool operator!=(Point const &lhs,Point const &rhs) - { - return !(lhs==rhs); - } - int Point::HIWORD(int n) - { - return (n >> 16) & 0xffff; - } - - int Point::LOWORD(int n) - { - return n & 0xffff; - } - Point Point::Empty=Point(); - +Point::Point(int dw) +{ + this->x = (short)Point::LOWORD(dw); + this->y = (short)Point::HIWORD(dw); + empty = false; +} +Point::Point(Size sz) +{ + this->x = sz.Width(); + this->y = sz.Height(); + empty = false; +} +Point::Point(int x, int y) +{ + this->x = x; + this->y = y; + empty = false; +} +Point::Point() : x(0), y(0), empty(true) +{} +uint qHash(Point const & point) +{ + return point.x ^ point.y; +} +bool operator==(Point const &lhs, Point const &rhs) +{ + return lhs.x == rhs.x && lhs.y == rhs.y; +} +bool operator!=(Point const &lhs, Point const &rhs) +{ + return !(lhs == rhs); +} +int Point::HIWORD(int n) +{ + return (n >> 16) & 0xffff; +} + +int Point::LOWORD(int n) +{ + return n & 0xffff; +} +Point Point::Empty = Point(); } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.h index 318922aa2..b23d2a7f7 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file point.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file point.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef OPOINT_H #define OPOINT_H @@ -31,42 +31,59 @@ #include namespace core { - struct Size; - struct Point +struct Size; +struct Point { + friend uint qHash(Point const & point); + friend bool operator==(Point const & lhs, Point const & rhs); + friend bool operator!=(Point const & lhs, Point const & rhs); +public: + + Point(); + Point(int x, int y); + Point(Size sz); + Point(int dw); + bool IsEmpty() { - friend uint qHash(Point const& point); - friend bool operator==(Point const& lhs,Point const& rhs); - friend bool operator!=(Point const& lhs,Point const& rhs); - public: + return empty; + } + int X() const + { + return this->x; + } + int Y() const + { + return this->y; + } + void SetX(const int &value) + { + x = value; empty = false; + } + void SetY(const int &value) + { + y = value; empty = false; + } + QString ToString() const + { + return "{" + QString::number(x) + "," + QString::number(y) + "}"; + } - Point(); - Point(int x,int y); - Point(Size sz); - Point(int dw); - bool IsEmpty(){return empty;} - int X()const{return this->x;} - int Y()const{return this->y;} - void SetX(const int &value){x=value;empty=false;} - void SetY(const int &value){y=value;empty=false;} - QString ToString()const{return "{"+QString::number(x)+","+QString::number(y)+"}";} + static Point Empty; + void Offset(const int &dx, const int &dy) + { + x += dx; + y += dy; + } + void Offset(Point p) + { + Offset(p.x, p.y); + } + static int HIWORD(int n); + static int LOWORD(int n); - static Point Empty; - void Offset(const int &dx,const int &dy) - { - x += dx; - y += dy; - } - void Offset(Point p) - { - Offset(p.x, p.y); - } - static int HIWORD(int n); - static int LOWORD(int n); - - private: - int x; - int y; - bool empty; - }; +private: + int x; + int y; + bool empty; +}; } #endif // POINT_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.cpp index 7e87503c8..a4c913b9e 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.cpp @@ -1,88 +1,87 @@ /** -****************************************************************************** -* -* @file providerstrings.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file providerstrings.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "providerstrings.h" namespace core { - const QString ProviderStrings::levelsForSigPacSpainMap[] = {"0", "1", "2", "3", "4", - "MTNSIGPAC", - "MTN2000", "MTN2000", "MTN2000", "MTN2000", "MTN2000", - "MTN200", "MTN200", "MTN200", - "MTN25", "MTN25", - "ORTOFOTOS","ORTOFOTOS","ORTOFOTOS","ORTOFOTOS"}; +const QString ProviderStrings::levelsForSigPacSpainMap[] = { "0", "1", "2", "3", "4", + "MTNSIGPAC", + "MTN2000", "MTN2000", "MTN2000", "MTN2000", "MTN2000", + "MTN200", "MTN200", "MTN200", + "MTN25", "MTN25", + "ORTOFOTOS", "ORTOFOTOS", "ORTOFOTOS", "ORTOFOTOS" }; - ProviderStrings::ProviderStrings() - { -// VersionGoogleMap = "m@132"; -// VersionGoogleSatellite = "71"; -// VersionGoogleLabels = "h@132"; -// VersionGoogleTerrain = "t@125,r@132"; - // Google version strings - VersionGoogleMap = "m@132"; - VersionGoogleSatellite = "71"; - VersionGoogleLabels = "h@132"; - VersionGoogleTerrain = "t@125,r@132"; - SecGoogleWord = "Galileo"; +ProviderStrings::ProviderStrings() +{ +// VersionGoogleMap = "m@132"; +// VersionGoogleSatellite = "71"; +// VersionGoogleLabels = "h@132"; +// VersionGoogleTerrain = "t@125,r@132"; + // Google version strings + VersionGoogleMap = "m@132"; + VersionGoogleSatellite = "71"; + VersionGoogleLabels = "h@132"; + VersionGoogleTerrain = "t@125,r@132"; + SecGoogleWord = "Galileo"; - // Google (China) version strings - VersionGoogleMapChina = "m@132"; - VersionGoogleSatelliteChina = "s@71"; - VersionGoogleLabelsChina = "h@132"; - VersionGoogleTerrainChina = "t@125,r@132"; + // Google (China) version strings + VersionGoogleMapChina = "m@132"; + VersionGoogleSatelliteChina = "s@71"; + VersionGoogleLabelsChina = "h@132"; + VersionGoogleTerrainChina = "t@125,r@132"; - // Google (Korea) version strings - VersionGoogleMapKorea = "kr1.12"; - VersionGoogleSatelliteKorea = "66"; - VersionGoogleLabelsKorea = "kr1t.12"; + // Google (Korea) version strings + VersionGoogleMapKorea = "kr1.12"; + VersionGoogleSatelliteKorea = "66"; + VersionGoogleLabelsKorea = "kr1t.12"; - /// - /// Google Maps API generated using http://greatmaps.codeplex.com/ - /// from http://code.google.com/intl/en-us/apis/maps/signup.html - /// - GoogleMapsAPIKey = "ABQIAAAAWaQgWiEBF3lW97ifKnAczhRAzBk5Igf8Z5n2W3hNnMT0j2TikxTLtVIGU7hCLLHMAuAMt-BO5UrEWA"; + /// + /// Google Maps API generated using http://greatmaps.codeplex.com/ + /// from http://code.google.com/intl/en-us/apis/maps/signup.html + /// + GoogleMapsAPIKey = "ABQIAAAAWaQgWiEBF3lW97ifKnAczhRAzBk5Igf8Z5n2W3hNnMT0j2TikxTLtVIGU7hCLLHMAuAMt-BO5UrEWA"; - // Yahoo version strings - VersionYahooMap = "4.3"; - VersionYahooSatellite = "1.9"; - VersionYahooLabels = "4.3"; + // Yahoo version strings + VersionYahooMap = "4.3"; + VersionYahooSatellite = "1.9"; + VersionYahooLabels = "4.3"; - // BingMaps - VersionBingMaps = "563"; + // BingMaps + VersionBingMaps = "563"; - // YandexMap - VersionYandexMap = "2.16.0"; - //VersionYandexSatellite = "1.19.0"; - //////////////////// + // YandexMap + VersionYandexMap = "2.16.0"; + // VersionYandexSatellite = "1.19.0"; + //////////////////// - /// - /// Bing Maps Customer Identification, more info here - /// http://msdn.microsoft.com/en-us/library/bb924353.aspx - /// - BingMapsClientToken = ""; - - } + /// + /// Bing Maps Customer Identification, more info here + /// http://msdn.microsoft.com/en-us/library/bb924353.aspx + /// + BingMapsClientToken = ""; +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.h index c24ccd795..1d349e7c2 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file providerstrings.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file providerstrings.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef PROVIDERSTRINGS_H #define PROVIDERSTRINGS_H @@ -31,55 +31,52 @@ namespace core { - class ProviderStrings - { - public: - ProviderStrings(); - static const QString levelsForSigPacSpainMap[]; - QString GoogleMapsAPIKey; - // Google version strings - QString VersionGoogleMap; - QString VersionGoogleSatellite; - QString VersionGoogleLabels; - QString VersionGoogleTerrain; - QString SecGoogleWord; +class ProviderStrings { +public: + ProviderStrings(); + static const QString levelsForSigPacSpainMap[]; + QString GoogleMapsAPIKey; + // Google version strings + QString VersionGoogleMap; + QString VersionGoogleSatellite; + QString VersionGoogleLabels; + QString VersionGoogleTerrain; + QString SecGoogleWord; - // Google (China) version strings - QString VersionGoogleMapChina; - QString VersionGoogleSatelliteChina; - QString VersionGoogleLabelsChina; - QString VersionGoogleTerrainChina; + // Google (China) version strings + QString VersionGoogleMapChina; + QString VersionGoogleSatelliteChina; + QString VersionGoogleLabelsChina; + QString VersionGoogleTerrainChina; - // Google (Korea) version strings - QString VersionGoogleMapKorea; - QString VersionGoogleSatelliteKorea; - QString VersionGoogleLabelsKorea; + // Google (Korea) version strings + QString VersionGoogleMapKorea; + QString VersionGoogleSatelliteKorea; + QString VersionGoogleLabelsKorea; - /// - /// Google Maps API generated using http://greatmaps.codeplex.com/ - /// from http://code.google.com/intl/en-us/apis/maps/signup.html - /// + /// + /// Google Maps API generated using http://greatmaps.codeplex.com/ + /// from http://code.google.com/intl/en-us/apis/maps/signup.html + /// - // Yahoo version strings - QString VersionYahooMap; - QString VersionYahooSatellite; - QString VersionYahooLabels; + // Yahoo version strings + QString VersionYahooMap; + QString VersionYahooSatellite; + QString VersionYahooLabels; - // BingMaps - QString VersionBingMaps; + // BingMaps + QString VersionBingMaps; - // YandexMap - QString VersionYandexMap; + // YandexMap + QString VersionYandexMap; - - /// - /// Bing Maps Customer Identification, more info here - /// http://msdn.microsoft.com/en-us/library/bb924353.aspx - /// - QString BingMapsClientToken; - }; - + /// + /// Bing Maps Customer Identification, more info here + /// http://msdn.microsoft.com/en-us/library/bb924353.aspx + /// + QString BingMapsClientToken; +}; } #endif // PROVIDERSTRINGS_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.cpp index c64f3c111..f6c28e8a6 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.cpp @@ -1,38 +1,35 @@ /** -****************************************************************************** -* -* @file pureimage.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file pureimage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "pureimage.h" - namespace core { PureImageProxy::PureImageProxy() -{ - -} +{} QPixmap PureImageProxy::FromStream(const QByteArray &array) { @@ -40,7 +37,7 @@ QPixmap PureImageProxy::FromStream(const QByteArray &array) } bool PureImageProxy::Save(const QByteArray &array, QPixmap &pic) { - pic=QPixmap::fromImage(QImage::fromData(array)); + pic = QPixmap::fromImage(QImage::fromData(array)); return true; } } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.h index edc6bd85c..8606c40d4 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file pureimage.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file pureimage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef PUREIMAGE_H #define PUREIMAGE_H @@ -32,13 +32,11 @@ namespace core { - class PureImageProxy - { - public: - PureImageProxy(); - static QPixmap FromStream(const QByteArray &array); - static bool Save(const QByteArray &array,QPixmap &pic); - }; - +class PureImageProxy { +public: + PureImageProxy(); + static QPixmap FromStream(const QByteArray &array); + static bool Save(const QByteArray &array, QPixmap &pic); +}; } #endif // PUREIMAGE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.cpp index eb16755ea..3b6a44aac 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.cpp @@ -1,354 +1,331 @@ /** -****************************************************************************** -* -* @file pureimagecache.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file pureimagecache.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "pureimagecache.h" #include #include -//#define DEBUG_PUREIMAGECACHE +// #define DEBUG_PUREIMAGECACHE namespace core { - qlonglong PureImageCache::ConnCounter=0; +qlonglong PureImageCache::ConnCounter = 0; - PureImageCache::PureImageCache() - { +PureImageCache::PureImageCache() +{} +void PureImageCache::setGtileCache(const QString &value) +{ + lock.lockForWrite(); + gtilecache = value; + QDir d; + if (!d.exists(gtilecache)) { + d.mkdir(gtilecache); +#ifdef DEBUG_PUREIMAGECACHE + qDebug() << "Create Cache directory"; +#endif // DEBUG_PUREIMAGECACHE } - - void PureImageCache::setGtileCache(const QString &value) { - lock.lockForWrite(); - gtilecache=value; - QDir d; - if(!d.exists(gtilecache)) - { - d.mkdir(gtilecache); + QString db = gtilecache + "Data.qmdb"; + if (!QFileInfo(db).exists()) { #ifdef DEBUG_PUREIMAGECACHE - qDebug()<<"Create Cache directory"; -#endif //DEBUG_PUREIMAGECACHE + qDebug() << "Try to create EmptyDB"; +#endif // DEBUG_PUREIMAGECACHE + CreateEmptyDB(db); } - { - QString db=gtilecache+"Data.qmdb"; - if(!QFileInfo(db).exists()) - { -#ifdef DEBUG_PUREIMAGECACHE - qDebug()<<"Try to create EmptyDB"; -#endif //DEBUG_PUREIMAGECACHE - CreateEmptyDB(db); - } - } - lock.unlock(); } - QString PureImageCache::GtileCache() - { - return gtilecache; + lock.unlock(); +} +QString PureImageCache::GtileCache() +{ + return gtilecache; +} + + +bool PureImageCache::CreateEmptyDB(const QString &file) +{ +#ifdef DEBUG_PUREIMAGECACHE + qDebug() << "Create database at!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!:" << file; +#endif // DEBUG_PUREIMAGECACHE + QFileInfo File(file); + QDir dir = File.absoluteDir(); + QString path = dir.absolutePath(); + QString filename = File.fileName(); + if (File.exists()) { + QFile(filename).remove(); } + if (!dir.exists()) { +#ifdef DEBUG_PUREIMAGECACHE + qDebug() << "CreateEmptyDB: Cache path doesn't exist, try to create"; +#endif // DEBUG_PUREIMAGECACHE + if (!dir.mkpath(path)) { +#ifdef DEBUG_PUREIMAGECACHE + qDebug() << "CreateEmptyDB: Could not create path"; +#endif // DEBUG_PUREIMAGECACHE + return false; + } + } + QSqlDatabase db; + db = QSqlDatabase::addDatabase("QSQLITE", QLatin1String("CreateConn")); + db.setDatabaseName(file); + if (!db.open()) { +#ifdef DEBUG_PUREIMAGECACHE + qDebug() << "CreateEmptyDB: Unable to create database"; +#endif // DEBUG_PUREIMAGECACHE - bool PureImageCache::CreateEmptyDB(const QString &file) - { + return false; + } + QSqlQuery query(db); + query.exec("CREATE TABLE IF NOT EXISTS Tiles (id INTEGER NOT NULL PRIMARY KEY, X INTEGER NOT NULL, Y INTEGER NOT NULL, Zoom INTEGER NOT NULL, Type INTEGER NOT NULL,Date TEXT)"); + if (query.numRowsAffected() == -1) { #ifdef DEBUG_PUREIMAGECACHE - qDebug()<<"Create database at!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!:"< add; - bool ret=true; - QString dir=gtilecache; - { - QString db=dir+"Data.qmdb"; - ret=QFileInfo(db).exists(); - if(ret) - { - QSqlDatabase cn; - Mcounter.lock(); - qlonglong id=++ConnCounter; - Mcounter.unlock(); - cn = QSqlDatabase::addDatabase("QSQLITE",QString::number(id)); - cn.setDatabaseName(db); - cn.setConnectOptions("QSQLITE_ENABLE_SHARED_CACHE"); - if(cn.open()) - { - { - QSqlQuery query(cn); - query.exec(QString("SELECT id, X, Y, Zoom, Type, Date FROM Tiles")); - while(query.next()) - { - if(QDateTime::fromString(query.value(5).toString()).daysTo(QDateTime::currentDateTime())>days) - add.append(query.value(0).toLongLong()); - } - foreach(long i,add) - { - query.exec(QString("DELETE FROM Tiles WHERE id = %1;").arg(i)); - } - } - - cn.close(); - } - QSqlDatabase::removeDatabase(QString::number(id)); - } - } - } - // PureImageCache::ExportMapDataToDB("C:/Users/Xapo/Documents/mapcontrol/debug/mapscache/data.qmdb","C:/Users/Xapo/Documents/mapcontrol/debug/mapscache/data2.qmdb"); - bool PureImageCache::ExportMapDataToDB(QString sourceFile, QString destFile) - { - bool ret=true; - QList add; - if(!QFileInfo(destFile).exists()) - { -#ifdef DEBUG_PUREIMAGECACHE - qDebug()<<"Try to create EmptyDB"; -#endif //DEBUG_PUREIMAGECACHE - ret=CreateEmptyDB(destFile); - } - if(!ret) return false; - QSqlDatabase ca = QSqlDatabase::addDatabase("QSQLITE","ca"); - ca.setDatabaseName(sourceFile); - - if(ca.open()) - { - QSqlDatabase cb = QSqlDatabase::addDatabase("QSQLITE","cb"); - cb.setDatabaseName(destFile); - if(cb.open()) - { - QSqlQuery queryb(cb); - queryb.exec(QString("ATTACH DATABASE \"%1\" AS Source").arg(sourceFile)); - QSqlQuery querya(ca); - querya.exec("SELECT id, X, Y, Zoom, Type, Date FROM Tiles"); - while(querya.next()) - { - long id=querya.value(0).toLongLong(); - queryb.exec(QString("SELECT id FROM Tiles WHERE X=%1 AND Y=%2 AND Zoom=%3 AND Type=%4;").arg(querya.value(1).toLongLong()).arg(querya.value(2).toLongLong()).arg(querya.value(3).toLongLong()).arg(querya.value(4).toLongLong())); - if(!queryb.next()) - { - add.append(id); - } - - } - long f; - foreach(f,add) - { - queryb.exec(QString("INSERT INTO Tiles(X, Y, Zoom, Type, Date) SELECT X, Y, Zoom, Type, Date FROM Source.Tiles WHERE id=%1").arg(f)); - queryb.exec(QString("INSERT INTO TilesData(id, Tile) Values((SELECT last_insert_rowid()), (SELECT Tile FROM Source.TilesData WHERE id=%1))").arg(f)); - } - add.clear(); - ca.close(); - cb.close(); - - } - else return false; - } - else return false; - QSqlDatabase::removeDatabase("ca"); - QSqlDatabase::removeDatabase("cb"); - return true; - - } - + QSqlDatabase::removeDatabase(QString::number(id)); + lock.unlock(); + return ar; +} +void PureImageCache::deleteOlderTiles(int const & days) +{ + if (gtilecache.isEmpty() | gtilecache.isNull()) { + return; + } + QList add; + bool ret = true; + QString dir = gtilecache; + { + QString db = dir + "Data.qmdb"; + ret = QFileInfo(db).exists(); + if (ret) { + QSqlDatabase cn; + Mcounter.lock(); + qlonglong id = ++ConnCounter; + Mcounter.unlock(); + cn = QSqlDatabase::addDatabase("QSQLITE", QString::number(id)); + cn.setDatabaseName(db); + cn.setConnectOptions("QSQLITE_ENABLE_SHARED_CACHE"); + if (cn.open()) { + { + QSqlQuery query(cn); + query.exec(QString("SELECT id, X, Y, Zoom, Type, Date FROM Tiles")); + while (query.next()) { + if (QDateTime::fromString(query.value(5).toString()).daysTo(QDateTime::currentDateTime()) > days) { + add.append(query.value(0).toLongLong()); + } + } + foreach(long i, add) { + query.exec(QString("DELETE FROM Tiles WHERE id = %1;").arg(i)); + } + } + + cn.close(); + } + QSqlDatabase::removeDatabase(QString::number(id)); + } + } +} +// PureImageCache::ExportMapDataToDB("C:/Users/Xapo/Documents/mapcontrol/debug/mapscache/data.qmdb","C:/Users/Xapo/Documents/mapcontrol/debug/mapscache/data2.qmdb"); +bool PureImageCache::ExportMapDataToDB(QString sourceFile, QString destFile) +{ + bool ret = true; + + QList add; + if (!QFileInfo(destFile).exists()) { +#ifdef DEBUG_PUREIMAGECACHE + qDebug() << "Try to create EmptyDB"; +#endif // DEBUG_PUREIMAGECACHE + ret = CreateEmptyDB(destFile); + } + if (!ret) { + return false; + } + QSqlDatabase ca = QSqlDatabase::addDatabase("QSQLITE", "ca"); + ca.setDatabaseName(sourceFile); + + if (ca.open()) { + QSqlDatabase cb = QSqlDatabase::addDatabase("QSQLITE", "cb"); + cb.setDatabaseName(destFile); + if (cb.open()) { + QSqlQuery queryb(cb); + queryb.exec(QString("ATTACH DATABASE \"%1\" AS Source").arg(sourceFile)); + QSqlQuery querya(ca); + querya.exec("SELECT id, X, Y, Zoom, Type, Date FROM Tiles"); + while (querya.next()) { + long id = querya.value(0).toLongLong(); + queryb.exec(QString("SELECT id FROM Tiles WHERE X=%1 AND Y=%2 AND Zoom=%3 AND Type=%4;").arg(querya.value(1).toLongLong()).arg(querya.value(2).toLongLong()).arg(querya.value(3).toLongLong()).arg(querya.value(4).toLongLong())); + if (!queryb.next()) { + add.append(id); + } + } + long f; + foreach(f, add) { + queryb.exec(QString("INSERT INTO Tiles(X, Y, Zoom, Type, Date) SELECT X, Y, Zoom, Type, Date FROM Source.Tiles WHERE id=%1").arg(f)); + queryb.exec(QString("INSERT INTO TilesData(id, Tile) Values((SELECT last_insert_rowid()), (SELECT Tile FROM Source.TilesData WHERE id=%1))").arg(f)); + } + add.clear(); + ca.close(); + cb.close(); + } else { return false; } + } else { return false; } + QSqlDatabase::removeDatabase("ca"); + QSqlDatabase::removeDatabase("cb"); + return true; +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.h index d92d58090..de324b629 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file pureimagecache.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file pureimagecache.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef PUREIMAGECACHE_H #define PUREIMAGECACHE_H @@ -43,25 +43,21 @@ #include #include namespace core { - class PureImageCache - { - - public: - PureImageCache(); - static bool CreateEmptyDB(const QString &file); - bool PutImageToCache(const QByteArray &tile,const MapType::Types &type,const core::Point &pos, const int &zoom); - QByteArray GetImageFromCache(MapType::Types type, core::Point pos, int zoom); - QString GtileCache(); - void setGtileCache(const QString &value); - static bool ExportMapDataToDB(QString sourceFile, QString destFile); - void deleteOlderTiles(int const& days); - private: - QString gtilecache; - QMutex Mcounter; - QReadWriteLock lock; - static qlonglong ConnCounter; - - }; - +class PureImageCache { +public: + PureImageCache(); + static bool CreateEmptyDB(const QString &file); + bool PutImageToCache(const QByteArray &tile, const MapType::Types &type, const core::Point &pos, const int &zoom); + QByteArray GetImageFromCache(MapType::Types type, core::Point pos, int zoom); + QString GtileCache(); + void setGtileCache(const QString &value); + static bool ExportMapDataToDB(QString sourceFile, QString destFile); + void deleteOlderTiles(int const & days); +private: + QString gtilecache; + QMutex Mcounter; + QReadWriteLock lock; + static qlonglong ConnCounter; +}; } #endif // PUREIMAGECACHE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.cpp index 836d1c6a6..abcf68e04 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.cpp @@ -1,38 +1,38 @@ /** -****************************************************************************** -* -* @file rawtile.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file rawtile.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "rawtile.h" - + namespace core { RawTile::RawTile(const MapType::Types &Type, const Point &Pos, const int &Zoom) { - zoom=Zoom; - type=Type; - pos=Pos; + zoom = Zoom; + type = Type; + pos = Pos; } QString RawTile::ToString() { @@ -52,25 +52,26 @@ int RawTile::Zoom() } void RawTile::setType(const MapType::Types &value) { - type=value; + type = value; } void RawTile::setPos(const Point &value) { - pos=value; + pos = value; } void RawTile::setZoom(const int &value) { - zoom=value; + zoom = value; } -uint qHash(RawTile const& tile) +uint qHash(RawTile const & tile) { // RawTile tile=tilee; - quint64 tmp=(((quint64)(tile.zoom))<<54)+(((quint64)(tile.type))<<36)+(((quint64)(tile.pos.X()))<<18)+(((quint64)(tile.pos.Y()))); - // quint64 tmp5=tmp+tmp2+tmp3+tmp4; + quint64 tmp = (((quint64)(tile.zoom)) << 54) + (((quint64)(tile.type)) << 36) + (((quint64)(tile.pos.X())) << 18) + (((quint64)(tile.pos.Y()))); + + // quint64 tmp5=tmp+tmp2+tmp3+tmp4; return ::qHash(tmp); } -bool operator==(RawTile const &lhs,RawTile const &rhs) +bool operator==(RawTile const &lhs, RawTile const &rhs) { - return (lhs.pos==rhs.pos && lhs.zoom==rhs.zoom && lhs.type==rhs.type); + return lhs.pos == rhs.pos && lhs.zoom == rhs.zoom && lhs.type == rhs.type; } } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.h index ae0796f6c..93cb1c34f 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file rawtile.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file rawtile.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef RAWTILE_H #define RAWTILE_H @@ -33,24 +33,23 @@ #include namespace core { - class RawTile - { - friend uint qHash(RawTile const& tile); - friend bool operator==(RawTile const& lhs,RawTile const& rhs); +class RawTile { + friend uint qHash(RawTile const & tile); + friend bool operator==(RawTile const & lhs, RawTile const & rhs); - public: - RawTile(const MapType::Types &Type,const core::Point &Pos,const int &Zoom); - QString ToString(void); - MapType::Types Type(); - core::Point Pos(); - int Zoom(); - void setType(const MapType::Types &value); - void setPos(const core::Point &value); - void setZoom(const int &value); - private: - MapType::Types type; - core::Point pos; - int zoom; - }; +public: + RawTile(const MapType::Types &Type, const core::Point &Pos, const int &Zoom); + QString ToString(void); + MapType::Types Type(); + core::Point Pos(); + int Zoom(); + void setType(const MapType::Types &value); + void setPos(const core::Point &value); + void setZoom(const int &value); +private: + MapType::Types type; + core::Point pos; + int zoom; +}; } #endif // RAWTILE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.cpp index 3d663cde7..b3988e844 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.cpp @@ -1,33 +1,33 @@ /** -****************************************************************************** -* -* @file size.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file size.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "size.h" - + namespace core { -Size::Size():width(0),height(0) +Size::Size() : width(0), height(0) {} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.h index 6949ae28a..7a97079b9 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file size.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file size.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef SIZE_H #define SIZE_H @@ -32,27 +32,58 @@ #include namespace core { - struct Size +struct Size { + Size(); + Size(Point pt) { - - Size(); - Size(Point pt){width=pt.X(); height=pt.Y();}; - Size(int Width,int Height){width=Width; height=Height;}; - friend uint qHash(Size const& size); - // friend bool operator==(Size const& lhs,Size const& rhs); - Size operator-(const Size &sz1){return Size(width-sz1.width,height-sz1.height);} - Size operator+(const Size &sz1){return Size(sz1.width+width,sz1.height+height);} - - int GetHashCode(){return width^height;} - uint qHash(Size const& /*rect*/){return width^height;} - QString ToString(){return "With="+QString::number(width)+" ,Height="+QString::number(height);} - int Width()const {return width;} - int Height()const {return height;} - void SetWidth(int const& value){width=value;} - void SetHeight(int const& value){height=value;} - private: - int width; - int height; + width = pt.X(); height = pt.Y(); }; + Size(int Width, int Height) + { + width = Width; height = Height; + }; + friend uint qHash(Size const & size); + // friend bool operator==(Size const& lhs,Size const& rhs); + Size operator-(const Size &sz1) + { + return Size(width - sz1.width, height - sz1.height); + } + Size operator+(const Size &sz1) + { + return Size(sz1.width + width, sz1.height + height); + } + + int GetHashCode() + { + return width ^ height; + } + uint qHash(Size const & /*rect*/) + { + return width ^ height; + } + QString ToString() + { + return "With=" + QString::number(width) + " ,Height=" + QString::number(height); + } + int Width() const + { + return width; + } + int Height() const + { + return height; + } + void SetWidth(int const & value) + { + width = value; + } + void SetHeight(int const & value) + { + height = value; + } +private: + int width; + int height; +}; } #endif // SIZE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.cpp index 9843f2f45..a47716517 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.cpp @@ -1,128 +1,112 @@ /** -****************************************************************************** -* -* @file tilecachequeue.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file tilecachequeue.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "tilecachequeue.h" -//#define DEBUG_TILECACHEQUEUE - +// #define DEBUG_TILECACHEQUEUE + namespace core { TileCacheQueue::TileCacheQueue() -{ - -} +{} TileCacheQueue::~TileCacheQueue() { - // QThread::wait(10000); + // QThread::wait(10000); } void TileCacheQueue::EnqueueCacheTask(CacheItemQueue *task) { #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"DB Do I EnqueueCacheTask"<GetPosition().X()<<","<GetPosition().Y(); -#endif //DEBUG_TILECACHEQUEUE - if(!tileCacheQueue.contains(task)) - { + qDebug() << "DB Do I EnqueueCacheTask" << task->GetPosition().X() << "," << task->GetPosition().Y(); +#endif // DEBUG_TILECACHEQUEUE + if (!tileCacheQueue.contains(task)) { #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"EnqueueCacheTask"<GetPosition().X()<<","<GetPosition().Y(); -#endif //DEBUG_TILECACHEQUEUE + qDebug() << "EnqueueCacheTask" << task->GetPosition().X() << "," << task->GetPosition().Y(); +#endif // DEBUG_TILECACHEQUEUE mutex.lock(); tileCacheQueue.enqueue(task); mutex.unlock(); - if(this->isRunning()) - { + if (this->isRunning()) { #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"Wake Thread"; -#endif //DEBUG_TILECACHEQUEUE - //this->start(QThread::NormalPriority); - //waitmutex.lock(); + qDebug() << "Wake Thread"; +#endif // DEBUG_TILECACHEQUEUE + // this->start(QThread::NormalPriority); + // waitmutex.lock(); waitc.wakeAll(); - //waitmutex.unlock(); - } - else - { + // waitmutex.unlock(); + } else { #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"Start Thread"; -#endif //DEBUG_TILECACHEQUEUE + qDebug() << "Start Thread"; +#endif // DEBUG_TILECACHEQUEUE this->start(QThread::NormalPriority); } } - } void TileCacheQueue::run() { #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"Cache Engine Start"; -#endif //DEBUG_TILECACHEQUEUE - while(true) - { + qDebug() << "Cache Engine Start"; +#endif // DEBUG_TILECACHEQUEUE + while (true) { CacheItemQueue *task; #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"Cache"; -#endif //DEBUG_TILECACHEQUEUE - if(tileCacheQueue.count()>0) - { + qDebug() << "Cache"; +#endif // DEBUG_TILECACHEQUEUE + if (tileCacheQueue.count() > 0) { mutex.lock(); - task=tileCacheQueue.dequeue(); + task = tileCacheQueue.dequeue(); mutex.unlock(); #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"Cache engine Put:"<GetPosition().X()<<","<GetPosition().Y(); -#endif //DEBUG_TILECACHEQUEUE - Cache::Instance()->ImageCache.PutImageToCache(task->GetImg(),task->GetMapType(),task->GetPosition(),task->GetZoom()); + qDebug() << "Cache engine Put:" << task->GetPosition().X() << "," << task->GetPosition().Y(); +#endif // DEBUG_TILECACHEQUEUE + Cache::Instance()->ImageCache.PutImageToCache(task->GetImg(), task->GetMapType(), task->GetPosition(), task->GetZoom()); usleep(44); delete task; - } - - else - { - qDebug()<<"Cache engine BEGIN WAIT"; + } else { + qDebug() << "Cache engine BEGIN WAIT"; waitmutex.lock(); - int tout=4000; - if(!waitc.wait(&waitmutex,tout)) - { + int tout = 4000; + if (!waitc.wait(&waitmutex, tout)) { waitmutex.unlock(); #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"Cache Engine TimeOut"; -#endif //DEBUG_TILECACHEQUEUE + qDebug() << "Cache Engine TimeOut"; +#endif // DEBUG_TILECACHEQUEUE mutex.lock(); - if(tileCacheQueue.count()==0) - { + if (tileCacheQueue.count() == 0) { mutex.unlock(); break; } mutex.unlock(); } - qDebug()<<"Cache Engine DID NOT TimeOut"; + qDebug() << "Cache Engine DID NOT TimeOut"; waitmutex.unlock(); } } #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"Cache Engine Stopped"; -#endif //DEBUG_TILECACHEQUEUE + qDebug() << "Cache Engine Stopped"; +#endif // DEBUG_TILECACHEQUEUE } - - } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.h index 2885fcdfe..3ded54303 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file tilecachequeue.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file tilecachequeue.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef TILECACHEQUEUE_H #define TILECACHEQUEUE_H @@ -39,21 +39,20 @@ namespace core { - class TileCacheQueue:public QThread - { - Q_OBJECT - public: - TileCacheQueue(); - ~TileCacheQueue(); - void EnqueueCacheTask(CacheItemQueue *task); +class TileCacheQueue : public QThread { + Q_OBJECT +public: + TileCacheQueue(); + ~TileCacheQueue(); + void EnqueueCacheTask(CacheItemQueue *task); - protected: - QQueue tileCacheQueue; - private: - void run(); - QMutex mutex; - QMutex waitmutex; - QWaitCondition waitc; - }; +protected: + QQueue tileCacheQueue; +private: + void run(); + QMutex mutex; + QMutex waitmutex; + QWaitCondition waitc; +}; } #endif // TILECACHEQUEUE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp index 2ebdeffc5..cd4e4d0d0 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp @@ -1,694 +1,675 @@ /** -****************************************************************************** -* -* @file urlfactory.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file urlfactory.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "urlfactory.h" #include namespace core { +const double UrlFactory::EarthRadiusKm = 6378.137; // WGS-84 - const double UrlFactory::EarthRadiusKm = 6378.137; // WGS-84 +UrlFactory::UrlFactory() +{ + /// + /// timeout for map connections + /// - UrlFactory::UrlFactory() - { - /// - /// timeout for map connections - /// + Proxy.setType(QNetworkProxy::NoProxy); - Proxy.setType(QNetworkProxy::NoProxy); + /// + /// Gets or sets the value of the User-agent HTTP header. + /// + UserAgent = "Mozilla/5.0 (Windows; U; Windows NT 6.0; en-US; rv:1.9.1.7) Gecko/20091221 Firefox/3.5.7"; - /// - /// Gets or sets the value of the User-agent HTTP header. - /// - UserAgent = "Mozilla/5.0 (Windows; U; Windows NT 6.0; en-US; rv:1.9.1.7) Gecko/20091221 Firefox/3.5.7"; + Timeout = 5 * 1000; + CorrectGoogleVersions = true; + isCorrectedGoogleVersions = false; + UseGeocoderCache = true; + UsePlacemarkCache = true; +} +UrlFactory::~UrlFactory() +{} +QString UrlFactory::TileXYToQuadKey(const int &tileX, const int &tileY, const int &levelOfDetail) const +{ + QString quadKey; - Timeout = 5 * 1000; - CorrectGoogleVersions=true; - isCorrectedGoogleVersions = false; - UseGeocoderCache=true; - UsePlacemarkCache=true; - } - UrlFactory::~UrlFactory() - { - } - QString UrlFactory::TileXYToQuadKey(const int &tileX,const int &tileY,const int &levelOfDetail) const - { - QString quadKey; - for(int i = levelOfDetail; i > 0; i--) - { - char digit = '0'; - int mask = 1 << (i - 1); - if((tileX & mask) != 0) - { - digit++; - } - if((tileY & mask) != 0) - { - digit++; - digit++; - } - quadKey.append(digit); + for (int i = levelOfDetail; i > 0; i--) { + char digit = '0'; + int mask = 1 << (i - 1); + if ((tileX & mask) != 0) { + digit++; } - return quadKey; - } - int UrlFactory::GetServerNum(const Point &pos,const int &max) const - { - return (pos.X() + 2 * pos.Y()) % max; - } - void UrlFactory::setIsCorrectGoogleVersions(bool value) - { - isCorrectedGoogleVersions=value; + if ((tileY & mask) != 0) { + digit++; + digit++; + } + quadKey.append(digit); } + return quadKey; +} +int UrlFactory::GetServerNum(const Point &pos, const int &max) const +{ + return (pos.X() + 2 * pos.Y()) % max; +} +void UrlFactory::setIsCorrectGoogleVersions(bool value) +{ + isCorrectedGoogleVersions = value; +} - bool UrlFactory::IsCorrectGoogleVersions() - { - return isCorrectedGoogleVersions; - } +bool UrlFactory::IsCorrectGoogleVersions() +{ + return isCorrectedGoogleVersions; +} - void UrlFactory::TryCorrectGoogleVersions() - { - QMutexLocker locker(&mutex); - if(CorrectGoogleVersions && !IsCorrectGoogleVersions()) - { - QNetworkReply *reply; - QNetworkRequest qheader; - QNetworkAccessManager network; - QEventLoop q; - QTimer tT; - tT.setSingleShot(true); - connect(&network, SIGNAL(finished(QNetworkReply*)), - &q, SLOT(quit())); - connect(&tT, SIGNAL(timeout()), &q, SLOT(quit())); - network.setProxy(Proxy); +void UrlFactory::TryCorrectGoogleVersions() +{ + QMutexLocker locker(&mutex); + + if (CorrectGoogleVersions && !IsCorrectGoogleVersions()) { + QNetworkReply *reply; + QNetworkRequest qheader; + QNetworkAccessManager network; + QEventLoop q; + QTimer tT; + tT.setSingleShot(true); + connect(&network, SIGNAL(finished(QNetworkReply *)), + &q, SLOT(quit())); + connect(&tT, SIGNAL(timeout()), &q, SLOT(quit())); + network.setProxy(Proxy); #ifdef DEBUG_URLFACTORY - qDebug()<<"Correct GoogleVersion"; -#endif //DEBUG_URLFACTORY - setIsCorrectGoogleVersions(true); - QString url = "http://maps.google.com"; + qDebug() << "Correct GoogleVersion"; +#endif // DEBUG_URLFACTORY + setIsCorrectGoogleVersions(true); + QString url = "http://maps.google.com"; - qheader.setUrl(QUrl(url)); - qheader.setRawHeader("User-Agent",UserAgent); - reply=network.get(qheader); - tT.start(Timeout); - q.exec(); - if(!tT.isActive()) - return; - tT.stop(); - if( (reply->error()!=QNetworkReply::NoError)) - { + qheader.setUrl(QUrl(url)); + qheader.setRawHeader("User-Agent", UserAgent); + reply = network.get(qheader); + tT.start(Timeout); + q.exec(); + if (!tT.isActive()) { + return; + } + tT.stop(); + if ((reply->error() != QNetworkReply::NoError)) { #ifdef DEBUG_URLFACTORY - qDebug()<<"Try corrected version withou abort or error:"<errorString(); -#endif //DEBUG_URLFACTORY - return; - } - QString html=QString(reply->readAll()); - QRegExp reg("\"*http://mt0.google.com/vt/lyrs=m@(\\d*)",Qt::CaseInsensitive); - if(reg.indexIn(html)!=-1) - { - QStringList gc=reg.capturedTexts(); - VersionGoogleMap = QString("m@%1").arg(gc[1]); - VersionGoogleMapChina = VersionGoogleMap; + qDebug() << "Try corrected version withou abort or error:" << reply->errorString(); +#endif // DEBUG_URLFACTORY + return; + } + QString html = QString(reply->readAll()); + QRegExp reg("\"*http://mt0.google.com/vt/lyrs=m@(\\d*)", Qt::CaseInsensitive); + if (reg.indexIn(html) != -1) { + QStringList gc = reg.capturedTexts(); + VersionGoogleMap = QString("m@%1").arg(gc[1]); + VersionGoogleMapChina = VersionGoogleMap; #ifdef DEBUG_URLFACTORY - qDebug()<<"TryCorrectGoogleVersions, VersionGoogleMap: "<deleteLater(); - + qDebug() << "TryCorrectGoogleVersions, VersionGoogleMap: " << VersionGoogleMap; +#endif // DEBUG_URLFACTORY } - } - - QString UrlFactory::MakeImageUrl(const MapType::Types &type,const Point &pos,const int &zoom,const QString &language) - { + reg = QRegExp("\"*http://mt0.google.com/vt/lyrs=h@(\\d*)", Qt::CaseInsensitive); + if (reg.indexIn(html) != -1) { + QStringList gc = reg.capturedTexts(); + VersionGoogleLabels = QString("h@%1").arg(gc[1]); + VersionGoogleLabelsChina = VersionGoogleLabels; #ifdef DEBUG_URLFACTORY - qDebug()<<"Entered MakeImageUrl"; -#endif //DEBUG_URLFACTORY - switch(type) - { - case MapType::GoogleMap: - { - QString server = "mt"; - QString request = "vt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - TryCorrectGoogleVersions(); - - return QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleMap).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleSatellite: - { - QString server = "khm"; - QString request = "kh"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - TryCorrectGoogleVersions(); - return QString("http://%1%2.google.com/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatellite).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleLabels: - { - QString server = "mt"; - QString request = "vt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - TryCorrectGoogleVersions(); - - return QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabels).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleTerrain: - { - QString server = "mt"; - QString request = "vt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - TryCorrectGoogleVersions(); - return QString("http://%1%2.google.com/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleTerrain).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleMapChina: - { - QString server = "mt"; - QString request = "vt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - TryCorrectGoogleVersions(); - // http://mt0.google.cn/vt/v=w2.101&hl=zh-CN&gl=cn&x=12&y=6&z=4&s=Ga - - return QString("http://%1%2.google.cn/%3/lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleMapChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleSatelliteChina: - { - QString server = "mt"; - QString request = "vt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - // TryCorrectGoogleVersions(); - // http://khm0.google.cn/kh/v=46&x=12&y=6&z=4&s=Ga - - return QString("http://%1%2.google.cn/%3/lyrs=%4&gl=cn&x=%5%6&y=%7&z=%8&s=%9").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatelliteChina).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleLabelsChina: - { - QString server = "mt"; - QString request = "vt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - TryCorrectGoogleVersions(); - // http://mt0.google.cn/vt/v=w2t.110&hl=zh-CN&gl=cn&x=12&y=6&z=4&s=Ga - - return QString("http://%1%2.google.cn/%3/imgtp=png32&lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabelsChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleTerrainChina: - { - QString server = "mt"; - QString request = "vt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - TryCorrectGoogleVersions(); - // http://mt0.google.cn/vt/v=w2p.110&hl=zh-CN&gl=cn&x=12&y=6&z=4&s=Ga - - return QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleTerrainChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleMapKorea: - { - QString server = "mt"; - QString request = "mt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - - //http://mt3.gmaptiles.co.kr/mt/v=kr1.11&hl=lt&x=109&y=49&z=7&s= - - QString ret = QString("http://%1%2.gmaptiles.co.kr/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleMapKorea).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - return ret; - } - break; - case MapType::GoogleSatelliteKorea: - { - QString server = "khm"; - QString request = "kh"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - - // http://khm1.google.co.kr/kh/v=54&x=109&y=49&z=7&s= - - return QString("http://%1%2.google.co.kr/%3/v=%4&x=%5%6&y=%7&z=%8&s=%9").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatelliteKorea).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleLabelsKorea: - { - QString server = "mt"; - QString request = "mt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - - // http://mt1.gmaptiles.co.kr/mt/v=kr1t.11&hl=lt&x=109&y=50&z=7&s=G - - return QString("http://%1%2.gmaptiles.co.kr/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabelsKorea).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::YahooMap: - { - return QString("http://maps%1.yimg.com/hx/tl?v=%2&.intl=%3&x=%4&y=%5&z=%6&r=1").arg(((GetServerNum(pos, 2)) + 1)).arg(VersionYahooMap).arg(language).arg(pos.X()).arg((((1 << zoom) >> 1) - 1 - pos.Y())).arg((zoom + 1)); - } - - case MapType::YahooSatellite: - { - return QString("http://maps%1.yimg.com/ae/ximg?v=%2&t=a&s=256&.intl=%3&x=%4&y=%5&z=%6&r=1").arg("3").arg(VersionYahooSatellite).arg(language).arg(pos.X()).arg(((1 << zoom) >> 1) - 1 - pos.Y()).arg(zoom + 1); - } - break; - case MapType::YahooLabels: - { - return QString("http://maps%1.yimg.com/hx/tl?v=%2&t=h&.intl=%3&x=%4&y=%5&z=%6&r=1").arg("1").arg(VersionYahooLabels).arg(language).arg(pos.X()).arg(((1 << zoom) >> 1) - 1 - pos.Y()).arg(zoom + 1); - } - break; - case MapType::OpenStreetMap: - { - char letter= "abc"[GetServerNum(pos, 3)]; - return QString("http://%1.tile.openstreetmap.org/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y()); - } - break; - case MapType::OpenStreetOsm: - { - char letter = "abc"[GetServerNum(pos, 3)]; - return QString("http://%1.tah.openstreetmap.org/Tiles/tile/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y()); - } - break; - case MapType::OpenStreetMapSurfer: - { - // http://tiles1.mapsurfer.net/tms_r.ashx?x=37378&y=20826&z=16 - - return QString("http://tiles1.mapsurfer.net/tms_r.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom); - } - break; - case MapType::OpenStreetMapSurferTerrain: - { - // http://tiles2.mapsurfer.net/tms_t.ashx?x=9346&y=5209&z=14 - - return QString("http://tiles2.mapsurfer.net/tms_t.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom); - } - break; - case MapType::BingMap: - { - QString key = TileXYToQuadKey(pos.X(), pos.Y(), zoom); - return QString("http://ecn.t%1.tiles.virtualearth.net/tiles/r%2.png?g=%3&mkt=%4%5").arg(GetServerNum(pos, 4)).arg(key).arg(VersionBingMaps).arg(language).arg(!(BingMapsClientToken.isNull()|BingMapsClientToken.isEmpty()) ? "&token=" + BingMapsClientToken : QString("")); - } - break; - case MapType::BingSatellite: - { - QString key = TileXYToQuadKey(pos.X(), pos.Y(), zoom); - return QString("http://ecn.t%1.tiles.virtualearth.net/tiles/a%2.jpeg?g=%3&mkt=%4%5").arg(GetServerNum(pos, 4)).arg(key).arg(VersionBingMaps).arg(language).arg(!(BingMapsClientToken.isNull()|BingMapsClientToken.isEmpty()) ? "&token=" + BingMapsClientToken : QString("")); - } - break; - case MapType::BingHybrid: - { - QString key = TileXYToQuadKey(pos.X(), pos.Y(), zoom); - return QString("http://ecn.t%1.tiles.virtualearth.net/tiles/h%2.jpeg?g=%3&mkt=%4%5").arg(GetServerNum(pos, 4)).arg(key).arg(VersionBingMaps).arg(language).arg(!(BingMapsClientToken.isNull()|BingMapsClientToken.isEmpty()) ? "&token=" + BingMapsClientToken : QString("")); - } - - case MapType::ArcGIS_Map: - { - // http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_StreetMap_World_2D/MapServer/tile/0/0/0.jpg - - return QString("http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_StreetMap_World_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); - } - break; - case MapType::ArcGIS_Satellite: - { - // http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_Imagery_World_2D/MapServer/tile/1/0/1.jpg - - return QString("http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_Imagery_World_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); - } - break; - case MapType::ArcGIS_ShadedRelief: - { - // http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_ShadedRelief_World_2D/MapServer/tile/1/0/1.jpg - - return QString("http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_ShadedRelief_World_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); - } - break; - case MapType::ArcGIS_Terrain: - { - // http://server.arcgisonline.com/ArcGIS/rest/services/NGS_Topo_US_2D/MapServer/tile/4/3/15 - - return QString("http://server.arcgisonline.com/ArcGIS/rest/services/NGS_Topo_US_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); - } - break; - case MapType::ArcGIS_MapsLT_OrtoFoto: - { - // http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L02/R0000001b/C00000028.jpg - // http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto/MapServer/tile/0/9/13 - // return string.Format("http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L{0:00}/R{1:x8}/C{2:x8}.jpg", zoom, pos.Y(), pos.X()); - // http://dc1.maps.lt/cache/mapslt_ortofoto_512/map/_alllayers/L03/R0000001c/C00000029.jpg - // return string.Format("http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto/MapServer/tile/{0}/{1}/{2}", zoom, pos.Y(), pos.X()); - // http://dc1.maps.lt/cache/mapslt_ortofoto_512/map/_alllayers/L03/R0000001d/C0000002a.jpg - //TODO verificar - return QString("http://dc1.maps.lt/cache/mapslt_ortofoto/map/_alllayers/L%1/R%2/C%3.jpg").arg(zoom,2,10,(QChar)'0').arg(pos.Y(),8,16,(QChar)'0').arg(pos.X(),8,16,(QChar)'0'); - } - break; - case MapType::ArcGIS_MapsLT_Map: - { - // http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L02/R0000001b/C00000028.jpg - // http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto/MapServer/tile/0/9/13 - // return string.Format("http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L{0:00}/R{1:x8}/C{2:x8}.jpg", zoom, pos.Y(), pos.X()); - // http://arcgis.maps.lt/ArcGIS/rest/services/mapslt/MapServer/tile/7/1162/1684.png - // http://dc1.maps.lt/cache/mapslt_512/map/_alllayers/L03/R0000001b/C00000029.png - //TODO verificar - // http://dc1.maps.lt/cache/mapslt/map/_alllayers/L02/R0000001c/C00000029.png - return QString("http://dc1.maps.lt/cache/mapslt/map/_alllayers/L%1/R%2/C%3.png").arg(zoom,2,10,(QChar)'0').arg(pos.Y(),8,16,(QChar)'0').arg(pos.X(),8,16,(QChar)'0'); - } - break; - case MapType::ArcGIS_MapsLT_Map_Labels: - { - //http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto_overlay/MapServer/tile/0/9/13 - //return string.Format("http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto_overlay/MapServer/tile/{0}/{1}/{2}", zoom, pos.Y(), pos.X()); - //http://dc1.maps.lt/cache/mapslt_ortofoto_overlay_512/map/_alllayers/L03/R0000001d/C00000029.png - //TODO verificar - return QString("http://dc1.maps.lt/cache/mapslt_ortofoto_overlay/map/_alllayers/L%1/R%2/C%3.png").arg(zoom,2,10,(QChar)'0').arg(pos.Y(),8,16,(QChar)'0').arg(pos.X(),8,16,(QChar)'0'); - } - break; - case MapType::PergoTurkeyMap: - { - // http://{domain}/{layerName}/{zoomLevel}/{first3LetterOfTileX}/{second3LetterOfTileX}/{third3LetterOfTileX}/{first3LetterOfTileY}/{second3LetterOfTileY}/{third3LetterOfTileXY}.png - - // http://map3.pergo.com.tr/tile/00/000/000/001/000/000/000.png - // That means: Zoom Level: 0 TileX: 1 TileY: 0 - - // http://domain/tile/14/000/019/371/000/011/825.png - // That means: Zoom Level: 14 TileX: 19371 TileY:11825 - - // string x = pos.X().ToString("000000000").Insert(3, "/").Insert(7, "/"); // - 000/000/001 - // string y = pos.Y().ToString("000000000").Insert(3, "/").Insert(7, "/"); // - 000/000/000 - QString x=QString("%1").arg(QString::number(pos.X()),9,(QChar)'0'); - x.insert(3,"/").insert(7,"/"); - QString y=QString("%1").arg(QString::number(pos.Y()),9,(QChar)'0'); - y.insert(3,"/").insert(7,"/"); - //"http://map03.pergo.com.tr/tile/2/000/000/003/000/000/002.png" - return QString("http://map%1.pergo.com.tr/tile/%2/%3/%4.png").arg(GetServerNum(pos, 4)).arg(zoom,2,10,(QChar)'0').arg(x).arg(y); - } - break; - case MapType::SigPacSpainMap: - { - return QString("http://sigpac.mapa.es/kmlserver/raster/%1@3785/%2.%3.%4.img").arg(levelsForSigPacSpainMap[zoom]).arg(zoom).arg(pos.X()).arg((2 << (zoom - 1)) - pos.Y() - 1); - } - break; - - case MapType::YandexMapRu: - { - QString server = "vec"; - - //http://vec01.maps.yandex.ru/tiles?l=map&v=2.10.2&x=1494&y=650&z=11 - - return QString("http://%1").arg(server)+QString("0%2.maps.yandex.ru/tiles?l=map&v=%3&x=%4&y=%5&z=%6").arg(GetServerNum(pos, 4)+1).arg(VersionYandexMap).arg(pos.X()).arg(pos.Y()).arg(zoom); - } - break; - default: - break; + qDebug() << "TryCorrectGoogleVersions, VersionGoogleLabels: " << VersionGoogleLabels; +#endif // DEBUG_URLFACTORY } + reg = QRegExp("\"*http://khm0.google.com/kh/v=(\\d*)", Qt::CaseInsensitive); + if (reg.indexIn(html) != -1) { + QStringList gc = reg.capturedTexts(); + VersionGoogleSatellite = gc[1]; + VersionGoogleSatelliteKorea = VersionGoogleSatellite; + VersionGoogleSatelliteChina = "s@" + VersionGoogleSatellite; - return QString::null; - } - void UrlFactory::GetSecGoogleWords(const Point &pos, QString &sec1, QString &sec2) - { - sec1 = ""; // after &x=... - sec2 = ""; // after &zoom=... - int seclen = ((pos.X() * 3) + pos.Y()) % 8; - sec2 = SecGoogleWord.left(seclen); - if(pos.Y() >= 10000 && pos.Y() < 100000) - { - sec1 = "&s="; + qDebug() << "TryCorrectGoogleVersions, VersionGoogleSatellite: " << VersionGoogleSatellite; } - } - QString UrlFactory::MakeGeocoderUrl(QString keywords) - { - QString key = keywords.replace(' ', '+'); - return QString("http://maps.google.com/maps/geo?q=%1&output=csv&key=%2").arg(key).arg(GoogleMapsAPIKey); - } - QString UrlFactory::MakeReverseGeocoderUrl(internals::PointLatLng &pt,const QString &language) - { - - return QString("http://maps.google.com/maps/geo?hl=%1&ll=%2,%3&output=csv&key=%4").arg(language).arg(QString::number(pt.Lat())).arg(QString::number(pt.Lng())).arg(GoogleMapsAPIKey); - - } - internals::PointLatLng UrlFactory::GetLatLngFromGeodecoder(const QString &keywords, GeoCoderStatusCode::Types &status) - { - return GetLatLngFromGeocoderUrl(MakeGeocoderUrl(keywords),UseGeocoderCache,status); - } - internals::PointLatLng UrlFactory::GetLatLngFromGeocoderUrl(const QString &url, const bool &useCache, GeoCoderStatusCode::Types &status) - { + reg = QRegExp("\"*http://mt0.google.com/vt/lyrs=t@(\\d*),r@(\\d*)", Qt::CaseInsensitive); + if (reg.indexIn(html) != -1) { + QStringList gc = reg.capturedTexts(); + VersionGoogleTerrain = QString("t@%1,r@%2").arg(gc[1]).arg(gc[2]); + VersionGoogleTerrainChina = VersionGoogleTerrain; #ifdef DEBUG_URLFACTORY - qDebug()<<"Entered GetLatLngFromGeocoderUrl:"; -#endif //DEBUG_URLFACTORY - status = GeoCoderStatusCode::Unknow; - internals::PointLatLng ret(0,0); - QString urlEnd = url.mid(url.indexOf("geo?q=")+6); - urlEnd.replace( QRegExp( - "[^" - "A-Z,a-z,0-9," - "\\^,\\&,\\',\\@," - "\\{,\\},\\[,\\]," - "\\,,\\$,\\=,\\!," - "\\-,\\#,\\(,\\)," - "\\%,\\.,\\+,\\~,\\_" - "]"), "_" ); - - QString geo = useCache ? Cache::Instance()->GetGeocoderFromCache(urlEnd) : ""; - - if(geo.isNull()|geo.isEmpty()) - { -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl:Not in cache going internet"; -#endif //DEBUG_URLFACTORY - QNetworkReply *reply; - QNetworkRequest qheader; - QNetworkAccessManager network; - network.setProxy(Proxy); - qheader.setUrl(QUrl(url)); - qheader.setRawHeader("User-Agent",UserAgent); - reply=network.get(qheader); -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl:URL="<isFinished()) || (time.elapsed()>(6*Timeout))) ){QCoreApplication::processEvents(QEventLoop::AllEvents);} -#ifdef DEBUG_URLFACTORY - qDebug()<<"Finished?"<error()<<" abort?"<<(time.elapsed()>Timeout*6); -#endif //DEBUG_URLFACTORY - if( (reply->error()!=QNetworkReply::NoError) | (time.elapsed()>Timeout*6)) - { -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl::Network error"; -#endif //DEBUG_URLFACTORY - return internals::PointLatLng(0,0); - } - { -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl:Reply ok"; -#endif //DEBUG_URLFACTORY - geo=reply->readAll(); - - - // cache geocoding - if(useCache && geo.startsWith("200")) - { - Cache::Instance()->CacheGeocoder(urlEnd, geo); - } - } - reply->deleteLater(); + qDebug() << "TryCorrectGoogleVersions, VersionGoogleTerrain: " << VersionGoogleTerrain; +#endif // DEBUG_URLFACTORY } - - - // parse values - // true : 200,4,56.1451640,22.0681787 - // false: 602,0,0,0 - { - QStringList values = geo.split(','); - if(values.count() == 4) - { - status = (GeoCoderStatusCode::Types) QString(values[0]).toInt(); - if(status == GeoCoderStatusCode::G_GEO_SUCCESS) - { - double lat = QString(values[2]).toDouble(); - double lng = QString(values[3]).toDouble(); - - ret = internals::PointLatLng(lat, lng); -#ifdef DEBUG_URLFACTORY - qDebug()<<"Lat="<GetPlacemarkFromCache(urlEnd) : ""; - - if(reverse.isNull()|reverse.isEmpty()) - { -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl:Not in cache going internet"; -#endif //DEBUG_URLFACTORY - QNetworkReply *reply; - QNetworkRequest qheader; - QNetworkAccessManager network; - network.setProxy(Proxy); - qheader.setUrl(QUrl(url)); - qheader.setRawHeader("User-Agent",UserAgent); - reply=network.get(qheader); -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl:URL="<isFinished()) || (time.elapsed()>(6*Timeout))) ){QCoreApplication::processEvents(QEventLoop::AllEvents);} -#ifdef DEBUG_URLFACTORY - qDebug()<<"Finished?"<error()<<" abort?"<<(time.elapsed()>Timeout*6); -#endif //DEBUG_URLFACTORY - if( (reply->error()!=QNetworkReply::NoError) | (time.elapsed()>Timeout*6)) - { -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl::Network error"; -#endif //DEBUG_URLFACTORY - return ret; - } - { -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl:Reply ok"; -#endif //DEBUG_URLFACTORY - QByteArray a=(reply->readAll()); - QTextCodec *codec = QTextCodec::codecForName("UTF-8"); - reverse = codec->toUnicode(a); -#ifdef DEBUG_URLFACTORY - qDebug()<CachePlacemark(urlEnd, reverse); - } - } - reply->deleteLater(); - } - - - // parse values - // true : 200,4,56.1451640,22.0681787 - // false: 602,0,0,0 - if(reverse.startsWith("200")) - { - QString acc = reverse.left(reverse.indexOf('\"')); - ret = Placemark(reverse.remove(reverse.indexOf('\"'))); - ret.SetAccuracy ((int) (( (QString) acc.split(',')[1]).toInt()) ); - - } - return ret; - } - double UrlFactory::GetDistance(internals::PointLatLng p1, internals::PointLatLng p2) - { - double dLat1InRad = p1.Lat() * (M_PI / 180); - double dLong1InRad = p1.Lng() * (M_PI / 180); - double dLat2InRad = p2.Lat() * (M_PI / 180); - double dLong2InRad = p2.Lng() * (M_PI / 180); - double dLongitude = dLong2InRad - dLong1InRad; - double dLatitude = dLat2InRad - dLat1InRad; - double a = pow(sin(dLatitude / 2), 2) + cos(dLat1InRad) * cos(dLat2InRad) * pow(sin(dLongitude / 2), 2); - double c = 2 * atan2(sqrt(a), sqrt(1 - a)); - double dDistance = EarthRadiusKm * c; - return dDistance; + reply->deleteLater(); } } + +QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, const int &zoom, const QString &language) +{ +#ifdef DEBUG_URLFACTORY + qDebug() << "Entered MakeImageUrl"; +#endif // DEBUG_URLFACTORY + switch (type) { + case MapType::GoogleMap: + { + QString server = "mt"; + QString request = "vt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + TryCorrectGoogleVersions(); + + return QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleMap).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleSatellite: + { + QString server = "khm"; + QString request = "kh"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + TryCorrectGoogleVersions(); + return QString("http://%1%2.google.com/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatellite).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleLabels: + { + QString server = "mt"; + QString request = "vt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + TryCorrectGoogleVersions(); + + return QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabels).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleTerrain: + { + QString server = "mt"; + QString request = "vt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + TryCorrectGoogleVersions(); + return QString("http://%1%2.google.com/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleTerrain).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleMapChina: + { + QString server = "mt"; + QString request = "vt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + TryCorrectGoogleVersions(); + // http://mt0.google.cn/vt/v=w2.101&hl=zh-CN&gl=cn&x=12&y=6&z=4&s=Ga + + return QString("http://%1%2.google.cn/%3/lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleMapChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleSatelliteChina: + { + QString server = "mt"; + QString request = "vt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + // TryCorrectGoogleVersions(); + // http://khm0.google.cn/kh/v=46&x=12&y=6&z=4&s=Ga + + return QString("http://%1%2.google.cn/%3/lyrs=%4&gl=cn&x=%5%6&y=%7&z=%8&s=%9").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatelliteChina).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleLabelsChina: + { + QString server = "mt"; + QString request = "vt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + TryCorrectGoogleVersions(); + // http://mt0.google.cn/vt/v=w2t.110&hl=zh-CN&gl=cn&x=12&y=6&z=4&s=Ga + + return QString("http://%1%2.google.cn/%3/imgtp=png32&lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabelsChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleTerrainChina: + { + QString server = "mt"; + QString request = "vt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + TryCorrectGoogleVersions(); + // http://mt0.google.cn/vt/v=w2p.110&hl=zh-CN&gl=cn&x=12&y=6&z=4&s=Ga + + return QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleTerrainChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleMapKorea: + { + QString server = "mt"; + QString request = "mt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + + // http://mt3.gmaptiles.co.kr/mt/v=kr1.11&hl=lt&x=109&y=49&z=7&s= + + QString ret = QString("http://%1%2.gmaptiles.co.kr/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleMapKorea).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + return ret; + } + break; + case MapType::GoogleSatelliteKorea: + { + QString server = "khm"; + QString request = "kh"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + + // http://khm1.google.co.kr/kh/v=54&x=109&y=49&z=7&s= + + return QString("http://%1%2.google.co.kr/%3/v=%4&x=%5%6&y=%7&z=%8&s=%9").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatelliteKorea).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleLabelsKorea: + { + QString server = "mt"; + QString request = "mt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + + // http://mt1.gmaptiles.co.kr/mt/v=kr1t.11&hl=lt&x=109&y=50&z=7&s=G + + return QString("http://%1%2.gmaptiles.co.kr/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabelsKorea).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::YahooMap: + { + return QString("http://maps%1.yimg.com/hx/tl?v=%2&.intl=%3&x=%4&y=%5&z=%6&r=1").arg(((GetServerNum(pos, 2)) + 1)).arg(VersionYahooMap).arg(language).arg(pos.X()).arg((((1 << zoom) >> 1) - 1 - pos.Y())).arg((zoom + 1)); + } + + case MapType::YahooSatellite: + { + return QString("http://maps%1.yimg.com/ae/ximg?v=%2&t=a&s=256&.intl=%3&x=%4&y=%5&z=%6&r=1").arg("3").arg(VersionYahooSatellite).arg(language).arg(pos.X()).arg(((1 << zoom) >> 1) - 1 - pos.Y()).arg(zoom + 1); + } + break; + case MapType::YahooLabels: + { + return QString("http://maps%1.yimg.com/hx/tl?v=%2&t=h&.intl=%3&x=%4&y=%5&z=%6&r=1").arg("1").arg(VersionYahooLabels).arg(language).arg(pos.X()).arg(((1 << zoom) >> 1) - 1 - pos.Y()).arg(zoom + 1); + } + break; + case MapType::OpenStreetMap: + { + char letter = "abc"[GetServerNum(pos, 3)]; + return QString("http://%1.tile.openstreetmap.org/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y()); + } + break; + case MapType::OpenStreetOsm: + { + char letter = "abc"[GetServerNum(pos, 3)]; + return QString("http://%1.tah.openstreetmap.org/Tiles/tile/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y()); + } + break; + case MapType::OpenStreetMapSurfer: + { + // http://tiles1.mapsurfer.net/tms_r.ashx?x=37378&y=20826&z=16 + + return QString("http://tiles1.mapsurfer.net/tms_r.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom); + } + break; + case MapType::OpenStreetMapSurferTerrain: + { + // http://tiles2.mapsurfer.net/tms_t.ashx?x=9346&y=5209&z=14 + + return QString("http://tiles2.mapsurfer.net/tms_t.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom); + } + break; + case MapType::BingMap: + { + QString key = TileXYToQuadKey(pos.X(), pos.Y(), zoom); + return QString("http://ecn.t%1.tiles.virtualearth.net/tiles/r%2.png?g=%3&mkt=%4%5").arg(GetServerNum(pos, 4)).arg(key).arg(VersionBingMaps).arg(language).arg(!(BingMapsClientToken.isNull() | BingMapsClientToken.isEmpty()) ? "&token=" + BingMapsClientToken : QString("")); + } + break; + case MapType::BingSatellite: + { + QString key = TileXYToQuadKey(pos.X(), pos.Y(), zoom); + return QString("http://ecn.t%1.tiles.virtualearth.net/tiles/a%2.jpeg?g=%3&mkt=%4%5").arg(GetServerNum(pos, 4)).arg(key).arg(VersionBingMaps).arg(language).arg(!(BingMapsClientToken.isNull() | BingMapsClientToken.isEmpty()) ? "&token=" + BingMapsClientToken : QString("")); + } + break; + case MapType::BingHybrid: + { + QString key = TileXYToQuadKey(pos.X(), pos.Y(), zoom); + return QString("http://ecn.t%1.tiles.virtualearth.net/tiles/h%2.jpeg?g=%3&mkt=%4%5").arg(GetServerNum(pos, 4)).arg(key).arg(VersionBingMaps).arg(language).arg(!(BingMapsClientToken.isNull() | BingMapsClientToken.isEmpty()) ? "&token=" + BingMapsClientToken : QString("")); + } + + case MapType::ArcGIS_Map: + { + // http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_StreetMap_World_2D/MapServer/tile/0/0/0.jpg + + return QString("http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_StreetMap_World_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); + } + break; + case MapType::ArcGIS_Satellite: + { + // http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_Imagery_World_2D/MapServer/tile/1/0/1.jpg + + return QString("http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_Imagery_World_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); + } + break; + case MapType::ArcGIS_ShadedRelief: + { + // http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_ShadedRelief_World_2D/MapServer/tile/1/0/1.jpg + + return QString("http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_ShadedRelief_World_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); + } + break; + case MapType::ArcGIS_Terrain: + { + // http://server.arcgisonline.com/ArcGIS/rest/services/NGS_Topo_US_2D/MapServer/tile/4/3/15 + + return QString("http://server.arcgisonline.com/ArcGIS/rest/services/NGS_Topo_US_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); + } + break; + case MapType::ArcGIS_MapsLT_OrtoFoto: + { + // http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L02/R0000001b/C00000028.jpg + // http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto/MapServer/tile/0/9/13 + // return string.Format("http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L{0:00}/R{1:x8}/C{2:x8}.jpg", zoom, pos.Y(), pos.X()); + // http://dc1.maps.lt/cache/mapslt_ortofoto_512/map/_alllayers/L03/R0000001c/C00000029.jpg + // return string.Format("http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto/MapServer/tile/{0}/{1}/{2}", zoom, pos.Y(), pos.X()); + // http://dc1.maps.lt/cache/mapslt_ortofoto_512/map/_alllayers/L03/R0000001d/C0000002a.jpg + // TODO verificar + return QString("http://dc1.maps.lt/cache/mapslt_ortofoto/map/_alllayers/L%1/R%2/C%3.jpg").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0'); + } + break; + case MapType::ArcGIS_MapsLT_Map: + { + // http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L02/R0000001b/C00000028.jpg + // http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto/MapServer/tile/0/9/13 + // return string.Format("http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L{0:00}/R{1:x8}/C{2:x8}.jpg", zoom, pos.Y(), pos.X()); + // http://arcgis.maps.lt/ArcGIS/rest/services/mapslt/MapServer/tile/7/1162/1684.png + // http://dc1.maps.lt/cache/mapslt_512/map/_alllayers/L03/R0000001b/C00000029.png + // TODO verificar + // http://dc1.maps.lt/cache/mapslt/map/_alllayers/L02/R0000001c/C00000029.png + return QString("http://dc1.maps.lt/cache/mapslt/map/_alllayers/L%1/R%2/C%3.png").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0'); + } + break; + case MapType::ArcGIS_MapsLT_Map_Labels: + { + // http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto_overlay/MapServer/tile/0/9/13 + // return string.Format("http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto_overlay/MapServer/tile/{0}/{1}/{2}", zoom, pos.Y(), pos.X()); + // http://dc1.maps.lt/cache/mapslt_ortofoto_overlay_512/map/_alllayers/L03/R0000001d/C00000029.png + // TODO verificar + return QString("http://dc1.maps.lt/cache/mapslt_ortofoto_overlay/map/_alllayers/L%1/R%2/C%3.png").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0'); + } + break; + case MapType::PergoTurkeyMap: + { + // http://{domain}/{layerName}/{zoomLevel}/{first3LetterOfTileX}/{second3LetterOfTileX}/{third3LetterOfTileX}/{first3LetterOfTileY}/{second3LetterOfTileY}/{third3LetterOfTileXY}.png + + // http://map3.pergo.com.tr/tile/00/000/000/001/000/000/000.png + // That means: Zoom Level: 0 TileX: 1 TileY: 0 + + // http://domain/tile/14/000/019/371/000/011/825.png + // That means: Zoom Level: 14 TileX: 19371 TileY:11825 + + // string x = pos.X().ToString("000000000").Insert(3, "/").Insert(7, "/"); // - 000/000/001 + // string y = pos.Y().ToString("000000000").Insert(3, "/").Insert(7, "/"); // - 000/000/000 + QString x = QString("%1").arg(QString::number(pos.X()), 9, (QChar)'0'); + x.insert(3, "/").insert(7, "/"); + QString y = QString("%1").arg(QString::number(pos.Y()), 9, (QChar)'0'); + y.insert(3, "/").insert(7, "/"); + // "http://map03.pergo.com.tr/tile/2/000/000/003/000/000/002.png" + return QString("http://map%1.pergo.com.tr/tile/%2/%3/%4.png").arg(GetServerNum(pos, 4)).arg(zoom, 2, 10, (QChar)'0').arg(x).arg(y); + } + break; + case MapType::SigPacSpainMap: + { + return QString("http://sigpac.mapa.es/kmlserver/raster/%1@3785/%2.%3.%4.img").arg(levelsForSigPacSpainMap[zoom]).arg(zoom).arg(pos.X()).arg((2 << (zoom - 1)) - pos.Y() - 1); + } + break; + + case MapType::YandexMapRu: + { + QString server = "vec"; + + // http://vec01.maps.yandex.ru/tiles?l=map&v=2.10.2&x=1494&y=650&z=11 + + return QString("http://%1").arg(server) + QString("0%2.maps.yandex.ru/tiles?l=map&v=%3&x=%4&y=%5&z=%6").arg(GetServerNum(pos, 4) + 1).arg(VersionYandexMap).arg(pos.X()).arg(pos.Y()).arg(zoom); + } + break; + default: + break; + } + + return QString::null; +} +void UrlFactory::GetSecGoogleWords(const Point &pos, QString &sec1, QString &sec2) +{ + sec1 = ""; // after &x=... + sec2 = ""; // after &zoom=... + int seclen = ((pos.X() * 3) + pos.Y()) % 8; + sec2 = SecGoogleWord.left(seclen); + if (pos.Y() >= 10000 && pos.Y() < 100000) { + sec1 = "&s="; + } +} +QString UrlFactory::MakeGeocoderUrl(QString keywords) +{ + QString key = keywords.replace(' ', '+'); + + return QString("http://maps.google.com/maps/geo?q=%1&output=csv&key=%2").arg(key).arg(GoogleMapsAPIKey); +} +QString UrlFactory::MakeReverseGeocoderUrl(internals::PointLatLng &pt, const QString &language) +{ + return QString("http://maps.google.com/maps/geo?hl=%1&ll=%2,%3&output=csv&key=%4").arg(language).arg(QString::number(pt.Lat())).arg(QString::number(pt.Lng())).arg(GoogleMapsAPIKey); +} +internals::PointLatLng UrlFactory::GetLatLngFromGeodecoder(const QString &keywords, GeoCoderStatusCode::Types &status) +{ + return GetLatLngFromGeocoderUrl(MakeGeocoderUrl(keywords), UseGeocoderCache, status); +} +internals::PointLatLng UrlFactory::GetLatLngFromGeocoderUrl(const QString &url, const bool &useCache, GeoCoderStatusCode::Types &status) +{ +#ifdef DEBUG_URLFACTORY + qDebug() << "Entered GetLatLngFromGeocoderUrl:"; +#endif // DEBUG_URLFACTORY + status = GeoCoderStatusCode::Unknow; + internals::PointLatLng ret(0, 0); + QString urlEnd = url.mid(url.indexOf("geo?q=") + 6); + urlEnd.replace(QRegExp( + "[^" + "A-Z,a-z,0-9," + "\\^,\\&,\\',\\@," + "\\{,\\},\\[,\\]," + "\\,,\\$,\\=,\\!," + "\\-,\\#,\\(,\\)," + "\\%,\\.,\\+,\\~,\\_" + "]"), "_"); + + QString geo = useCache ? Cache::Instance()->GetGeocoderFromCache(urlEnd) : ""; + + if (geo.isNull() | geo.isEmpty()) { +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl:Not in cache going internet"; +#endif // DEBUG_URLFACTORY + QNetworkReply *reply; + QNetworkRequest qheader; + QNetworkAccessManager network; + network.setProxy(Proxy); + qheader.setUrl(QUrl(url)); + qheader.setRawHeader("User-Agent", UserAgent); + reply = network.get(qheader); +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl:URL=" << url; +#endif // DEBUG_URLFACTORY + QTime time; + time.start(); + while ((!(reply->isFinished()) || (time.elapsed() > (6 * Timeout)))) { + QCoreApplication::processEvents(QEventLoop::AllEvents); + } +#ifdef DEBUG_URLFACTORY + qDebug() << "Finished?" << reply->error() << " abort?" << (time.elapsed() > Timeout * 6); +#endif // DEBUG_URLFACTORY + if ((reply->error() != QNetworkReply::NoError) | (time.elapsed() > Timeout * 6)) { +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl::Network error"; +#endif // DEBUG_URLFACTORY + return internals::PointLatLng(0, 0); + } + { +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl:Reply ok"; +#endif // DEBUG_URLFACTORY + geo = reply->readAll(); + + + // cache geocoding + if (useCache && geo.startsWith("200")) { + Cache::Instance()->CacheGeocoder(urlEnd, geo); + } + } + reply->deleteLater(); + } + + + // parse values + // true : 200,4,56.1451640,22.0681787 + // false: 602,0,0,0 + { + QStringList values = geo.split(','); + if (values.count() == 4) { + status = (GeoCoderStatusCode::Types)QString(values[0]).toInt(); + if (status == GeoCoderStatusCode::G_GEO_SUCCESS) { + double lat = QString(values[2]).toDouble(); + double lng = QString(values[3]).toDouble(); + + ret = internals::PointLatLng(lat, lng); +#ifdef DEBUG_URLFACTORY + qDebug() << "Lat=" << lat << " Lng=" << lng; +#endif // DEBUG_URLFACTORY + } + } + } + return ret; +} + +Placemark UrlFactory::GetPlacemarkFromGeocoder(internals::PointLatLng location) +{ + return GetPlacemarkFromReverseGeocoderUrl(MakeReverseGeocoderUrl(location, LanguageStr), UsePlacemarkCache); +} + +Placemark UrlFactory::GetPlacemarkFromReverseGeocoderUrl(const QString &url, const bool &useCache) +{ + Placemark ret(""); + +#ifdef DEBUG_URLFACTORY + qDebug() << "Entered GetPlacemarkFromReverseGeocoderUrl:"; +#endif // DEBUG_URLFACTORY + // status = GeoCoderStatusCode::Unknow; + QString urlEnd = url.right(url.indexOf("geo?hl=")); + urlEnd.replace(QRegExp( + "[^" + "A-Z,a-z,0-9," + "\\^,\\&,\\',\\@," + "\\{,\\},\\[,\\]," + "\\,,\\$,\\=,\\!," + "\\-,\\#,\\(,\\)," + "\\%,\\.,\\+,\\~,\\_" + "]"), "_"); + + QString reverse = useCache ? Cache::Instance()->GetPlacemarkFromCache(urlEnd) : ""; + + if (reverse.isNull() | reverse.isEmpty()) { +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl:Not in cache going internet"; +#endif // DEBUG_URLFACTORY + QNetworkReply *reply; + QNetworkRequest qheader; + QNetworkAccessManager network; + network.setProxy(Proxy); + qheader.setUrl(QUrl(url)); + qheader.setRawHeader("User-Agent", UserAgent); + reply = network.get(qheader); +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl:URL=" << url; +#endif // DEBUG_URLFACTORY + QTime time; + time.start(); + while ((!(reply->isFinished()) || (time.elapsed() > (6 * Timeout)))) { + QCoreApplication::processEvents(QEventLoop::AllEvents); + } +#ifdef DEBUG_URLFACTORY + qDebug() << "Finished?" << reply->error() << " abort?" << (time.elapsed() > Timeout * 6); +#endif // DEBUG_URLFACTORY + if ((reply->error() != QNetworkReply::NoError) | (time.elapsed() > Timeout * 6)) { +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl::Network error"; +#endif // DEBUG_URLFACTORY + return ret; + } + { +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl:Reply ok"; +#endif // DEBUG_URLFACTORY + QByteArray a = (reply->readAll()); + QTextCodec *codec = QTextCodec::codecForName("UTF-8"); + reverse = codec->toUnicode(a); +#ifdef DEBUG_URLFACTORY + qDebug() << reverse; +#endif // DEBUG_URLFACTORY + // cache geocoding + if (useCache && reverse.startsWith("200")) { + Cache::Instance()->CachePlacemark(urlEnd, reverse); + } + } + reply->deleteLater(); + } + + + // parse values + // true : 200,4,56.1451640,22.0681787 + // false: 602,0,0,0 + if (reverse.startsWith("200")) { + QString acc = reverse.left(reverse.indexOf('\"')); + ret = Placemark(reverse.remove(reverse.indexOf('\"'))); + ret.SetAccuracy((int)(((QString)acc.split(',')[1]).toInt())); + } + return ret; +} +double UrlFactory::GetDistance(internals::PointLatLng p1, internals::PointLatLng p2) +{ + double dLat1InRad = p1.Lat() * (M_PI / 180); + double dLong1InRad = p1.Lng() * (M_PI / 180); + double dLat2InRad = p2.Lat() * (M_PI / 180); + double dLong2InRad = p2.Lng() * (M_PI / 180); + double dLongitude = dLong2InRad - dLong1InRad; + double dLatitude = dLat2InRad - dLat1InRad; + double a = pow(sin(dLatitude / 2), 2) + cos(dLat1InRad) * cos(dLat2InRad) * pow(sin(dLongitude / 2), 2); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + double dDistance = EarthRadiusKm * c; + + return dDistance; +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.h index a40947cc2..ace8b52d3 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file urlfactory.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file urlfactory.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef URLFACTORY_H #define URLFACTORY_H @@ -45,44 +45,42 @@ #include "cmath" namespace core { - class UrlFactory: public QObject,public ProviderStrings - { - Q_OBJECT - public: - /// - /// Gets or sets the value of the User-agent HTTP header. - /// - QByteArray UserAgent; - QNetworkProxy Proxy; - UrlFactory(); - ~UrlFactory(); - QString MakeImageUrl(const MapType::Types &type,const core::Point &pos,const int &zoom,const QString &language); - internals::PointLatLng GetLatLngFromGeodecoder(const QString &keywords,GeoCoderStatusCode::Types &status); - Placemark GetPlacemarkFromGeocoder(internals::PointLatLng location); - int Timeout; - private: - void GetSecGoogleWords(const core::Point &pos, QString &sec1, QString &sec2); - int GetServerNum(const core::Point &pos,const int &max) const; - void TryCorrectGoogleVersions(); - bool isCorrectedGoogleVersions; - QString TileXYToQuadKey(const int &tileX,const int &tileY,const int &levelOfDetail) const; - bool CorrectGoogleVersions; - bool UseGeocoderCache; //TODO GetSet - bool UsePlacemarkCache;//TODO GetSet - static const double EarthRadiusKm; - double GetDistance(internals::PointLatLng p1,internals::PointLatLng p2); - QMutex mutex; - - protected: - static short timelapse; - QString LanguageStr; - bool IsCorrectGoogleVersions(); - void setIsCorrectGoogleVersions(bool value); - QString MakeGeocoderUrl(QString keywords); - QString MakeReverseGeocoderUrl(internals::PointLatLng &pt,const QString &language); - internals::PointLatLng GetLatLngFromGeocoderUrl(const QString &url,const bool &useCache, GeoCoderStatusCode::Types &status); - Placemark GetPlacemarkFromReverseGeocoderUrl(const QString &url,const bool &useCache); - }; +class UrlFactory : public QObject, public ProviderStrings { + Q_OBJECT +public: + /// + /// Gets or sets the value of the User-agent HTTP header. + /// + QByteArray UserAgent; + QNetworkProxy Proxy; + UrlFactory(); + ~UrlFactory(); + QString MakeImageUrl(const MapType::Types &type, const core::Point &pos, const int &zoom, const QString &language); + internals::PointLatLng GetLatLngFromGeodecoder(const QString &keywords, GeoCoderStatusCode::Types &status); + Placemark GetPlacemarkFromGeocoder(internals::PointLatLng location); + int Timeout; +private: + void GetSecGoogleWords(const core::Point &pos, QString &sec1, QString &sec2); + int GetServerNum(const core::Point &pos, const int &max) const; + void TryCorrectGoogleVersions(); + bool isCorrectedGoogleVersions; + QString TileXYToQuadKey(const int &tileX, const int &tileY, const int &levelOfDetail) const; + bool CorrectGoogleVersions; + bool UseGeocoderCache; // TODO GetSet + bool UsePlacemarkCache; // TODO GetSet + static const double EarthRadiusKm; + double GetDistance(internals::PointLatLng p1, internals::PointLatLng p2); + QMutex mutex; +protected: + static short timelapse; + QString LanguageStr; + bool IsCorrectGoogleVersions(); + void setIsCorrectGoogleVersions(bool value); + QString MakeGeocoderUrl(QString keywords); + QString MakeReverseGeocoderUrl(internals::PointLatLng &pt, const QString &language); + internals::PointLatLng GetLatLngFromGeocoderUrl(const QString &url, const bool &useCache, GeoCoderStatusCode::Types &status); + Placemark GetPlacemarkFromReverseGeocoderUrl(const QString &url, const bool &useCache); +}; } #endif // URLFACTORY_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/MouseWheelZoomType.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/MouseWheelZoomType.cpp index 17fe3b1cd..efdbd2ba7 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/MouseWheelZoomType.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/MouseWheelZoomType.cpp @@ -1,32 +1,30 @@ /** -****************************************************************************** -* -* @file MouseWheelZoomType.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file MouseWheelZoomType.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "mousewheelzoomtype.h" - -namespace internals { -} +namespace internals {} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/copyrightstrings.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/copyrightstrings.h index 36336bf83..59ac8085f 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/copyrightstrings.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/copyrightstrings.h @@ -1,41 +1,40 @@ /** -****************************************************************************** -* -* @file copyrightstrings.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file copyrightstrings.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef COPYRIGHTSTRINGS_H #define COPYRIGHTSTRINGS_H #include #include - -namespace internals { -static const QString googleCopyright = QString("%1 Google - Map data %1 Tele Atlas, Imagery %1 TerraMetrics").arg(QDate::currentDate().year()); -static const QString openStreetMapCopyright = QString(" OpenStreetMap - Map data %1 OpenStreetMap").arg(QDate::currentDate().year()); -static const QString yahooMapCopyright = QString(" Yahoo! Inc. - Map data & Imagery %1 NAVTEQ").arg(QDate::currentDate().year()); -static const QString virtualEarthCopyright = QString("%1 Microsoft Corporation, %1 NAVTEQ, %1 Image courtesy of NASA").arg(QDate::currentDate().year()); -static const QString arcGisCopyright = QString("%1 ESRI - Map data %1 ArcGIS").arg(QDate::currentDate().year()); +namespace internals { +static const QString googleCopyright = QString("%1 Google - Map data %1 Tele Atlas, Imagery %1 TerraMetrics").arg(QDate::currentDate().year()); +static const QString openStreetMapCopyright = QString(" OpenStreetMap - Map data %1 OpenStreetMap").arg(QDate::currentDate().year()); +static const QString yahooMapCopyright = QString(" Yahoo! Inc. - Map data & Imagery %1 NAVTEQ").arg(QDate::currentDate().year()); +static const QString virtualEarthCopyright = QString("%1 Microsoft Corporation, %1 NAVTEQ, %1 Image courtesy of NASA").arg(QDate::currentDate().year()); +static const QString arcGisCopyright = QString("%1 ESRI - Map data %1 ArcGIS").arg(QDate::currentDate().year()); } #endif // COPYRIGHTSTRINGS_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.cpp index b484f2537..0d81a8bdf 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.cpp @@ -1,180 +1,163 @@ /** -****************************************************************************** -* -* @file core.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file core.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "core.h" #ifdef DEBUG_CORE -qlonglong internals::Core::debugcounter=0; +qlonglong internals::Core::debugcounter = 0; #endif using namespace projections; namespace internals { - Core::Core():MouseWheelZooming(false),currentPosition(0,0),currentPositionPixel(0,0),LastLocationInBounds(-1,-1),sizeOfMapArea(0,0) - ,minOfTiles(0,0),maxOfTiles(0,0),zoom(0),isDragging(false),TooltipTextPadding(10,10),loaderLimit(5),maxzoom(21),started(false),runningThreads(0) - { - mousewheelzoomtype=MouseWheelZoomType::MousePositionAndCenter; - SetProjection(new MercatorProjection()); - this->setAutoDelete(false); - ProcessLoadTaskCallback.setMaxThreadCount(10); - renderOffset=Point(0,0); - dragPoint=Point(0,0); - CanDragMap=true; - tilesToload=0; - OPMaps::Instance(); - } - Core::~Core() - { - ProcessLoadTaskCallback.waitForDone(); - } +Core::Core() : MouseWheelZooming(false), currentPosition(0, 0), currentPositionPixel(0, 0), LastLocationInBounds(-1, -1), sizeOfMapArea(0, 0) + , minOfTiles(0, 0), maxOfTiles(0, 0), zoom(0), isDragging(false), TooltipTextPadding(10, 10), loaderLimit(5), maxzoom(21), started(false), runningThreads(0) +{ + mousewheelzoomtype = MouseWheelZoomType::MousePositionAndCenter; + SetProjection(new MercatorProjection()); + this->setAutoDelete(false); + ProcessLoadTaskCallback.setMaxThreadCount(10); + renderOffset = Point(0, 0); + dragPoint = Point(0, 0); + CanDragMap = true; + tilesToload = 0; + OPMaps::Instance(); +} +Core::~Core() +{ + ProcessLoadTaskCallback.waitForDone(); +} - void Core::run() - { - MrunningThreads.lock(); - ++runningThreads; - MrunningThreads.unlock(); +void Core::run() +{ + MrunningThreads.lock(); + ++runningThreads; + MrunningThreads.unlock(); #ifdef DEBUG_CORE - qlonglong debug; - Mdebug.lock(); - debug=++debugcounter; - Mdebug.unlock(); - qDebug()<<"core:run"<<" ID="<GetPos().ToString()<<" now has "<Overlays.count()<<" overlays"<<" ID="<GetPos().ToString() << " now has " << t->Overlays.count() << " overlays" << " ID=" << debug; +#endif // DEBUG_CORE } Moverlays.unlock(); break; - } - else if(OPMaps::Instance()->RetryLoadTile > 0) - { + } else if (OPMaps::Instance()->RetryLoadTile > 0) { #ifdef DEBUG_CORE - qDebug()<<"ProcessLoadTask: " << task.ToString()<< " -> empty tile, retry " << retry<<" ID="< empty tile, retry " << retry << " ID=" << debug;; +#endif // DEBUG_CORE { QWaitCondition wait; QMutex m; m.lock(); - wait.wait(&m,500); + wait.wait(&m, 500); } } - } - while(++retry < OPMaps::Instance()->RetryLoadTile); + } while (++retry < OPMaps::Instance()->RetryLoadTile); } - if(t->Overlays.count() > 0) - { - Matrix.SetTileAt(task.Pos,t); + if (t->Overlays.count() > 0) { + Matrix.SetTileAt(task.Pos, t); emit OnNeedInvalidation(); #ifdef DEBUG_CORE - qDebug()<<"Core::run add tile "<GetPos().ToString()<<" to matrix index "<Overlays.count()<<" ID="<GetPos().ToString() << " to matrix index " << task.Pos.ToString() << " ID=" << debug; + qDebug() << "Core::run matrix index " << task.Pos.ToString() << " as tile with " << Matrix.TileAt(task.Pos)->Overlays.count() << " ID=" << debug; +#endif // DEBUG_CORE + } else { // emit OnTilesStillToLoad(tilesToload); delete t; @@ -189,8 +172,7 @@ namespace internals { { // last buddy cleans stuff ;} - if(last) - { + if (last) { OPMaps::Instance()->kiberCacheLock.lockForWrite(); OPMaps::Instance()->TilesInMemory.RemoveMemoryOverload(); OPMaps::Instance()->kiberCacheLock.unlock(); @@ -206,505 +188,460 @@ namespace internals { emit OnNeedInvalidation(); - } } - - - } #ifdef DEBUG_CORE - qDebug()<<"loaderLimit release:"+loaderLimit.available()<<" ID="<Type()!="LKS94Projection") - { - SetProjection(new LKS94Projection()); - maxzoom=11; - } - } - break; - - case MapType::PergoTurkeyMap: - { - if(Projection()->Type()!="PlateCarreeProjectionPergo") - { - SetProjection(new PlateCarreeProjectionPergo()); - maxzoom=17; - } - } - break; - - case MapType::YandexMapRu: - { - if(Projection()->Type()!="MercatorProjectionYandex") - { - SetProjection(new MercatorProjectionYandex()); - maxzoom=13; - } - } - break; - - default: - { - if(Projection()->Type()!="MercatorProjection") - { - SetProjection(new MercatorProjection()); - maxzoom=21; - } - } - break; - } - - minOfTiles = Projection()->GetTileMatrixMinXY(Zoom()); - maxOfTiles = Projection()->GetTileMatrixMaxXY(Zoom()); - SetCurrentPositionGPixel(Projection()->FromLatLngToPixel(CurrentPosition(), Zoom())); - - if(started) - { - CancelAsyncTasks(); - OnMapSizeChanged(Width, Height); - GoToCurrentPosition(); - ReloadMap(); - GoToCurrentPosition(); - emit OnMapTypeChanged(value); - - } - } - - } - void Core::StartSystem() - { - if(!started) - { - started = true; - - ReloadMap(); - GoToCurrentPosition(); - } - } - - void Core::UpdateCenterTileXYLocation() - { - PointLatLng center = FromLocalToLatLng(Width/2, Height/2); - Point centerPixel = Projection()->FromLatLngToPixel(center, Zoom()); - centerTileXYLocation = Projection()->FromPixelToTileXY(centerPixel); - } - - void Core::OnMapSizeChanged(int const& width, int const& height) - { - Width = width; - Height = height; - - sizeOfMapArea.SetWidth(1 + (Width/Projection()->TileSize().Width())/2); - sizeOfMapArea.SetHeight(1 + (Height/Projection()->TileSize().Height())/2); - - UpdateCenterTileXYLocation(); - - if(started) - { +void Core::SetZoom(const int &value) +{ + if (!isDragging) { + zoom = value; + minOfTiles = Projection()->GetTileMatrixMinXY(value); + maxOfTiles = Projection()->GetTileMatrixMaxXY(value); + currentPositionPixel = Projection()->FromLatLngToPixel(currentPosition, value); + if (started) { + MtileLoadQueue.lock(); + tileLoadQueue.clear(); + MtileLoadQueue.unlock(); + MtileToload.lock(); + tilesToload = 0; + MtileToload.unlock(); + Matrix.Clear(); + GoToCurrentPositionOnZoom(); UpdateBounds(); + keepInBounds(); + emit OnMapDrag(); + emit OnMapZoomChanged(); + emit OnNeedInvalidation(); + } + } +} +void Core::SetCurrentPosition(const PointLatLng &value) +{ + if (!IsDragging()) { + currentPosition = value; + SetCurrentPositionGPixel(Projection()->FromLatLngToPixel(value, Zoom())); + + if (started) { + GoToCurrentPosition(); + emit OnCurrentPositionChanged(currentPosition); + } + } else { + currentPosition = value; + SetCurrentPositionGPixel(Projection()->FromLatLngToPixel(value, Zoom())); + + if (started) { emit OnCurrentPositionChanged(currentPosition); } } - void Core::OnMapClose() - { - // if(waitOnEmptyTasks != null) - // { - // try - // { - // waitOnEmptyTasks.Set(); - // waitOnEmptyTasks.Close(); - // } - // catch - // { - // } - // } +} +void Core::SetMapType(const MapType::Types &value) +{ + if (value != GetMapType()) { + mapType = value; - CancelAsyncTasks(); - } - GeoCoderStatusCode::Types Core::SetCurrentPositionByKeywords(QString const& keys) - { - GeoCoderStatusCode::Types status = GeoCoderStatusCode::Unknow; - PointLatLng pos = OPMaps::Instance()->GetLatLngFromGeodecoder(keys, status); - if(!pos.IsEmpty() && (status == GeoCoderStatusCode::G_GEO_SUCCESS)) + switch (value) { + case MapType::ArcGIS_Map: + case MapType::ArcGIS_Satellite: + case MapType::ArcGIS_ShadedRelief: + case MapType::ArcGIS_Terrain: { - SetCurrentPosition(pos); - } - - return status; - } - RectLatLng Core::CurrentViewArea() - { - PointLatLng p = Projection()->FromPixelToLatLng(-renderOffset.X(), -renderOffset.Y(), Zoom()); - double rlng = Projection()->FromPixelToLatLng(-renderOffset.X() + Width, -renderOffset.Y(), Zoom()).Lng(); - double blat = Projection()->FromPixelToLatLng(-renderOffset.X(), -renderOffset.Y() + Height, Zoom()).Lat(); - return RectLatLng::FromLTRB(p.Lng(), p.Lat(), rlng, blat); - - } - PointLatLng Core::FromLocalToLatLng(int const& x, int const& y) - { - return Projection()->FromPixelToLatLng(Point(x - renderOffset.X(), y - renderOffset.Y()), Zoom()); - } - - - Point Core::FromLatLngToLocal(PointLatLng const& latlng) - { - Point pLocal = Projection()->FromLatLngToPixel(latlng, Zoom()); - pLocal.Offset(renderOffset); - return pLocal; - } - int Core::GetMaxZoomToFitRect(RectLatLng const& rect) - { - int zoom = 0; - - for(int i = 1; i <= MaxZoom(); i++) - { - Point p1 = Projection()->FromLatLngToPixel(rect.LocationTopLeft(), i); - Point p2 = Projection()->FromLatLngToPixel(rect.Bottom(), rect.Right(), i); - - if(((p2.X() - p1.X()) <= Width+10) && (p2.Y() - p1.Y()) <= Height+10) - { - zoom = i; - } - else - { - break; + if (Projection()->Type() != "PlateCarreeProjection") { + SetProjection(new PlateCarreeProjection()); + maxzoom = 13; } } + break; - return zoom; - } - void Core::BeginDrag(Point const& pt) - { - dragPoint.SetX(pt.X() - renderOffset.X()); - dragPoint.SetY(pt.Y() - renderOffset.Y()); - isDragging = true; - } - void Core::EndDrag() - { - isDragging = false; - emit OnNeedInvalidation(); - - } - void Core::ReloadMap() - { - if(started) + case MapType::ArcGIS_MapsLT_Map_Hybrid: + case MapType::ArcGIS_MapsLT_Map_Labels: + case MapType::ArcGIS_MapsLT_Map: + case MapType::ArcGIS_MapsLT_OrtoFoto: { -#ifdef DEBUG_CORE - qDebug()<<"------------------"; -#endif //DEBUG_CORE - - MtileLoadQueue.lock(); - { - tileLoadQueue.clear(); - } - MtileLoadQueue.unlock(); - MtileToload.lock(); - tilesToload=0; - MtileToload.unlock(); - Matrix.Clear(); - - emit OnNeedInvalidation(); - - } - } - void Core::GoToCurrentPosition() - { - // reset stuff - renderOffset = Point::Empty; - centerTileXYLocationLast = Point::Empty; - dragPoint = Point::Empty; - - // goto location - Drag(Point(-(GetcurrentPositionGPixel().X() - Width/2), -(GetcurrentPositionGPixel().Y() - Height/2))); - } - void Core::GoToCurrentPositionOnZoom() - { - // reset stuff - renderOffset = Point::Empty; - centerTileXYLocationLast = Point::Empty; - dragPoint = Point::Empty; - - // goto location and centering - if(MouseWheelZooming) - { - if(mousewheelzoomtype != MouseWheelZoomType::MousePositionWithoutCenter) - { - Point pt = Point(-(GetcurrentPositionGPixel().X() - Width/2), -(GetcurrentPositionGPixel().Y() - Height/2)); - renderOffset.SetX(pt.X() - dragPoint.X()); - renderOffset.SetY(pt.Y() - dragPoint.Y()); - } - else // without centering - { - renderOffset.SetX(-GetcurrentPositionGPixel().X() - dragPoint.X()); - renderOffset.SetY(-GetcurrentPositionGPixel().Y() - dragPoint.Y()); - renderOffset.Offset(mouseLastZoom); + if (Projection()->Type() != "LKS94Projection") { + SetProjection(new LKS94Projection()); + maxzoom = 11; } } - else // use current map center + break; + + case MapType::PergoTurkeyMap: { - mouseLastZoom = Point::Empty; - - Point pt = Point(-(GetcurrentPositionGPixel().X() - Width/2), -(GetcurrentPositionGPixel().Y() - Height/2)); - renderOffset.SetX(pt.X() - dragPoint.X()); - renderOffset.SetY(pt.Y() - dragPoint.Y()); - } - - UpdateCenterTileXYLocation(); - } - void Core::DragOffset(Point const& offset) - { - renderOffset.Offset(offset); - - UpdateCenterTileXYLocation(); - - if(centerTileXYLocation != centerTileXYLocationLast) - { - centerTileXYLocationLast = centerTileXYLocation; - UpdateBounds(); - } - - { - LastLocationInBounds = CurrentPosition(); - SetCurrentPosition (FromLocalToLatLng((int) Width/2, (int) Height/2)); - } - - emit OnNeedInvalidation(); - emit OnMapDrag(); - } - void Core::Drag(Point const& pt) - { - renderOffset.SetX(pt.X() - dragPoint.X()); - renderOffset.SetY(pt.Y() - dragPoint.Y()); - keepInBounds(); - UpdateCenterTileXYLocation(); - - if(centerTileXYLocation != centerTileXYLocationLast) - { - centerTileXYLocationLast = centerTileXYLocation; - UpdateBounds(); - } - - if(IsDragging()) - { - LastLocationInBounds = CurrentPosition(); - SetCurrentPosition(FromLocalToLatLng((int) Width/2, (int) Height/2)); - } - - emit OnNeedInvalidation(); - - - emit OnMapDrag(); - - } - void Core::CancelAsyncTasks() - { - if(started) - { - ProcessLoadTaskCallback.waitForDone(); - MtileLoadQueue.lock(); - { - tileLoadQueue.clear(); - //tilesToload=0; - } - MtileLoadQueue.unlock(); - MtileToload.lock(); - tilesToload=0; - MtileToload.unlock(); - // ProcessLoadTaskCallback.waitForDone(); - } - } - void Core::UpdateBounds() - { - MtileDrawingList.lock(); - { - FindTilesAround(tileDrawingList); - -#ifdef DEBUG_CORE - qDebug()<<"OnTileLoadStart: " << tileDrawingList.count() << " tiles to load at zoom " << Zoom() << ", time: " << QDateTime::currentDateTime().date(); -#endif //DEBUG_CORE - - emit OnTileLoadStart(); - - - foreach(Point p,tileDrawingList) - { - LoadTask task = LoadTask(p, Zoom()); - { - MtileLoadQueue.lock(); - { - if(!tileLoadQueue.contains(task)) - { - MtileToload.lock(); - ++tilesToload; - MtileToload.unlock(); - tileLoadQueue.enqueue(task); -#ifdef DEBUG_CORE - qDebug()<<"Core::UpdateBounds new Task"<Type() != "PlateCarreeProjectionPergo") { + SetProjection(new PlateCarreeProjectionPergo()); + maxzoom = 17; } } - MtileDrawingList.unlock(); - UpdateGroundResolution(); - } - void Core::FindTilesAround(QList &list) - { - list.clear();; - for(int i = -sizeOfMapArea.Width(); i <= sizeOfMapArea.Width(); i++) + break; + + case MapType::YandexMapRu: { - for(int j = -sizeOfMapArea.Height(); j <= sizeOfMapArea.Height(); j++) - { - Point p = centerTileXYLocation; - p.SetX(p.X() + i); - p.SetY(p.Y() + j); - - //if(p.X < minOfTiles.Width) - //{ - // p.X += (maxOfTiles.Width + 1); - //} - - //if(p.X > maxOfTiles.Width) - //{ - // p.X -= (maxOfTiles.Width + 1); - //} - - if(p.X() >= minOfTiles.Width() && p.Y() >= minOfTiles.Height() && p.X() <= maxOfTiles.Width() && p.Y() <= maxOfTiles.Height()) - { - if(!list.contains(p)) - { - list.append(p); - } - } + if (Projection()->Type() != "MercatorProjectionYandex") { + SetProjection(new MercatorProjectionYandex()); + maxzoom = 13; } } + break; + default: + { + if (Projection()->Type() != "MercatorProjection") { + SetProjection(new MercatorProjection()); + maxzoom = 21; + } + } + break; + } - } - void Core::UpdateGroundResolution() - { - double rez = Projection()->GetGroundResolution(Zoom(), CurrentPosition().Lat()); - pxRes100m = (int) (100.0 / rez); // 100 meters - pxRes1000m = (int) (1000.0 / rez); // 1km - pxRes10km = (int) (10000.0 / rez); // 10km - pxRes100km = (int) (100000.0 / rez); // 100km - pxRes1000km = (int) (1000000.0 / rez); // 1000km - pxRes5000km = (int) (5000000.0 / rez); // 5000km - } - void Core::keepInBounds() - { - if(renderOffset.X()>0) - renderOffset.SetX(0); - if(renderOffset.Y()>0) - renderOffset.SetY(0); - int maxDragY=GetCurrentRegion().Height()-GettileRect().Height()*(maxOfTiles.Height()-minOfTiles.Height()+1); - int maxDragX=GetCurrentRegion().Width()-GettileRect().Width()*(maxOfTiles.Width()-minOfTiles.Width()+1); + minOfTiles = Projection()->GetTileMatrixMinXY(Zoom()); + maxOfTiles = Projection()->GetTileMatrixMaxXY(Zoom()); + SetCurrentPositionGPixel(Projection()->FromLatLngToPixel(CurrentPosition(), Zoom())); - if(maxDragY>renderOffset.Y()) - renderOffset.SetY(maxDragY); - if(maxDragX>renderOffset.X()) - renderOffset.SetX(maxDragX); + if (started) { + CancelAsyncTasks(); + OnMapSizeChanged(Width, Height); + GoToCurrentPosition(); + ReloadMap(); + GoToCurrentPosition(); + emit OnMapTypeChanged(value); + } } } +void Core::StartSystem() +{ + if (!started) { + started = true; + + ReloadMap(); + GoToCurrentPosition(); + } +} + +void Core::UpdateCenterTileXYLocation() +{ + PointLatLng center = FromLocalToLatLng(Width / 2, Height / 2); + Point centerPixel = Projection()->FromLatLngToPixel(center, Zoom()); + + centerTileXYLocation = Projection()->FromPixelToTileXY(centerPixel); +} + +void Core::OnMapSizeChanged(int const & width, int const & height) +{ + Width = width; + Height = height; + + sizeOfMapArea.SetWidth(1 + (Width / Projection()->TileSize().Width()) / 2); + sizeOfMapArea.SetHeight(1 + (Height / Projection()->TileSize().Height()) / 2); + + UpdateCenterTileXYLocation(); + + if (started) { + UpdateBounds(); + + emit OnCurrentPositionChanged(currentPosition); + } +} +void Core::OnMapClose() +{ + // if(waitOnEmptyTasks != null) + // { + // try + // { + // waitOnEmptyTasks.Set(); + // waitOnEmptyTasks.Close(); + // } + // catch + // { + // } + // } + + CancelAsyncTasks(); +} +GeoCoderStatusCode::Types Core::SetCurrentPositionByKeywords(QString const & keys) +{ + GeoCoderStatusCode::Types status = GeoCoderStatusCode::Unknow; + PointLatLng pos = OPMaps::Instance()->GetLatLngFromGeodecoder(keys, status); + + if (!pos.IsEmpty() && (status == GeoCoderStatusCode::G_GEO_SUCCESS)) { + SetCurrentPosition(pos); + } + + return status; +} +RectLatLng Core::CurrentViewArea() +{ + PointLatLng p = Projection()->FromPixelToLatLng(-renderOffset.X(), -renderOffset.Y(), Zoom()); + double rlng = Projection()->FromPixelToLatLng(-renderOffset.X() + Width, -renderOffset.Y(), Zoom()).Lng(); + double blat = Projection()->FromPixelToLatLng(-renderOffset.X(), -renderOffset.Y() + Height, Zoom()).Lat(); + + return RectLatLng::FromLTRB(p.Lng(), p.Lat(), rlng, blat); +} +PointLatLng Core::FromLocalToLatLng(int const & x, int const & y) +{ + return Projection()->FromPixelToLatLng(Point(x - renderOffset.X(), y - renderOffset.Y()), Zoom()); +} + + +Point Core::FromLatLngToLocal(PointLatLng const & latlng) +{ + Point pLocal = Projection()->FromLatLngToPixel(latlng, Zoom()); + + pLocal.Offset(renderOffset); + return pLocal; +} +int Core::GetMaxZoomToFitRect(RectLatLng const & rect) +{ + int zoom = 0; + + for (int i = 1; i <= MaxZoom(); i++) { + Point p1 = Projection()->FromLatLngToPixel(rect.LocationTopLeft(), i); + Point p2 = Projection()->FromLatLngToPixel(rect.Bottom(), rect.Right(), i); + + if (((p2.X() - p1.X()) <= Width + 10) && (p2.Y() - p1.Y()) <= Height + 10) { + zoom = i; + } else { + break; + } + } + + return zoom; +} +void Core::BeginDrag(Point const & pt) +{ + dragPoint.SetX(pt.X() - renderOffset.X()); + dragPoint.SetY(pt.Y() - renderOffset.Y()); + isDragging = true; +} +void Core::EndDrag() +{ + isDragging = false; + emit OnNeedInvalidation(); +} +void Core::ReloadMap() +{ + if (started) { +#ifdef DEBUG_CORE + qDebug() << "------------------"; +#endif // DEBUG_CORE + + MtileLoadQueue.lock(); + { + tileLoadQueue.clear(); + } + MtileLoadQueue.unlock(); + MtileToload.lock(); + tilesToload = 0; + MtileToload.unlock(); + Matrix.Clear(); + + emit OnNeedInvalidation(); + } +} +void Core::GoToCurrentPosition() +{ + // reset stuff + renderOffset = Point::Empty; + centerTileXYLocationLast = Point::Empty; + dragPoint = Point::Empty; + + // goto location + Drag(Point(-(GetcurrentPositionGPixel().X() - Width / 2), -(GetcurrentPositionGPixel().Y() - Height / 2))); +} +void Core::GoToCurrentPositionOnZoom() +{ + // reset stuff + renderOffset = Point::Empty; + centerTileXYLocationLast = Point::Empty; + dragPoint = Point::Empty; + + // goto location and centering + if (MouseWheelZooming) { + if (mousewheelzoomtype != MouseWheelZoomType::MousePositionWithoutCenter) { + Point pt = Point(-(GetcurrentPositionGPixel().X() - Width / 2), -(GetcurrentPositionGPixel().Y() - Height / 2)); + renderOffset.SetX(pt.X() - dragPoint.X()); + renderOffset.SetY(pt.Y() - dragPoint.Y()); + } else { // without centering + renderOffset.SetX(-GetcurrentPositionGPixel().X() - dragPoint.X()); + renderOffset.SetY(-GetcurrentPositionGPixel().Y() - dragPoint.Y()); + renderOffset.Offset(mouseLastZoom); + } + } else { // use current map center + mouseLastZoom = Point::Empty; + + Point pt = Point(-(GetcurrentPositionGPixel().X() - Width / 2), -(GetcurrentPositionGPixel().Y() - Height / 2)); + renderOffset.SetX(pt.X() - dragPoint.X()); + renderOffset.SetY(pt.Y() - dragPoint.Y()); + } + + UpdateCenterTileXYLocation(); +} +void Core::DragOffset(Point const & offset) +{ + renderOffset.Offset(offset); + + UpdateCenterTileXYLocation(); + + if (centerTileXYLocation != centerTileXYLocationLast) { + centerTileXYLocationLast = centerTileXYLocation; + UpdateBounds(); + } + + { + LastLocationInBounds = CurrentPosition(); + SetCurrentPosition(FromLocalToLatLng((int)Width / 2, (int)Height / 2)); + } + + emit OnNeedInvalidation(); + emit OnMapDrag(); +} +void Core::Drag(Point const & pt) +{ + renderOffset.SetX(pt.X() - dragPoint.X()); + renderOffset.SetY(pt.Y() - dragPoint.Y()); + keepInBounds(); + UpdateCenterTileXYLocation(); + + if (centerTileXYLocation != centerTileXYLocationLast) { + centerTileXYLocationLast = centerTileXYLocation; + UpdateBounds(); + } + + if (IsDragging()) { + LastLocationInBounds = CurrentPosition(); + SetCurrentPosition(FromLocalToLatLng((int)Width / 2, (int)Height / 2)); + } + + emit OnNeedInvalidation(); + + + emit OnMapDrag(); +} +void Core::CancelAsyncTasks() +{ + if (started) { + ProcessLoadTaskCallback.waitForDone(); + MtileLoadQueue.lock(); + { + tileLoadQueue.clear(); + // tilesToload=0; + } + MtileLoadQueue.unlock(); + MtileToload.lock(); + tilesToload = 0; + MtileToload.unlock(); + // ProcessLoadTaskCallback.waitForDone(); + } +} +void Core::UpdateBounds() +{ + MtileDrawingList.lock(); + { + FindTilesAround(tileDrawingList); + +#ifdef DEBUG_CORE + qDebug() << "OnTileLoadStart: " << tileDrawingList.count() << " tiles to load at zoom " << Zoom() << ", time: " << QDateTime::currentDateTime().date(); +#endif // DEBUG_CORE + + emit OnTileLoadStart(); + + + foreach(Point p, tileDrawingList) { + LoadTask task = LoadTask(p, Zoom()); + { + MtileLoadQueue.lock(); + { + if (!tileLoadQueue.contains(task)) { + MtileToload.lock(); + ++tilesToload; + MtileToload.unlock(); + tileLoadQueue.enqueue(task); +#ifdef DEBUG_CORE + qDebug() << "Core::UpdateBounds new Task" << task.Pos.ToString(); +#endif // DEBUG_CORE + ProcessLoadTaskCallback.start(this); + } + } + MtileLoadQueue.unlock(); + } + } + } + MtileDrawingList.unlock(); + UpdateGroundResolution(); +} +void Core::FindTilesAround(QList &list) +{ + list.clear();; + for (int i = -sizeOfMapArea.Width(); i <= sizeOfMapArea.Width(); i++) { + for (int j = -sizeOfMapArea.Height(); j <= sizeOfMapArea.Height(); j++) { + Point p = centerTileXYLocation; + p.SetX(p.X() + i); + p.SetY(p.Y() + j); + + // if(p.X < minOfTiles.Width) + // { + // p.X += (maxOfTiles.Width + 1); + // } + + // if(p.X > maxOfTiles.Width) + // { + // p.X -= (maxOfTiles.Width + 1); + // } + + if (p.X() >= minOfTiles.Width() && p.Y() >= minOfTiles.Height() && p.X() <= maxOfTiles.Width() && p.Y() <= maxOfTiles.Height()) { + if (!list.contains(p)) { + list.append(p); + } + } + } + } +} +void Core::UpdateGroundResolution() +{ + double rez = Projection()->GetGroundResolution(Zoom(), CurrentPosition().Lat()); + + pxRes100m = (int)(100.0 / rez); // 100 meters + pxRes1000m = (int)(1000.0 / rez); // 1km + pxRes10km = (int)(10000.0 / rez); // 10km + pxRes100km = (int)(100000.0 / rez); // 100km + pxRes1000km = (int)(1000000.0 / rez); // 1000km + pxRes5000km = (int)(5000000.0 / rez); // 5000km +} +void Core::keepInBounds() +{ + if (renderOffset.X() > 0) { + renderOffset.SetX(0); + } + if (renderOffset.Y() > 0) { + renderOffset.SetY(0); + } + int maxDragY = GetCurrentRegion().Height() - GettileRect().Height() * (maxOfTiles.Height() - minOfTiles.Height() + 1); + int maxDragX = GetCurrentRegion().Width() - GettileRect().Width() * (maxOfTiles.Width() - minOfTiles.Width() + 1); + + if (maxDragY > renderOffset.Y()) { + renderOffset.SetY(maxDragY); + } + if (maxDragX > renderOffset.X()) { + renderOffset.SetX(maxDragX); + } +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.h index dca51d646..9998f0408 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file core.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file core.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef CORE_H #define CORE_H @@ -57,228 +57,340 @@ #include -namespace mapcontrol -{ - class OPMapControl; - class MapGraphicItem; +namespace mapcontrol { +class OPMapControl; +class MapGraphicItem; } namespace internals { +class Core : public QObject, public QRunnable { + Q_OBJECT - class Core:public QObject,public QRunnable + friend class mapcontrol::OPMapControl; + friend class mapcontrol::MapGraphicItem; +public: + Core(); + ~Core(); + void run(); + PointLatLng CurrentPosition() const { - Q_OBJECT + return currentPosition; + } - friend class mapcontrol::OPMapControl; - friend class mapcontrol::MapGraphicItem; - public: - Core(); - ~Core(); - void run(); - PointLatLng CurrentPosition()const{return currentPosition;} + void SetCurrentPosition(const PointLatLng &value); - void SetCurrentPosition(const PointLatLng &value); + core::Point GetcurrentPositionGPixel() + { + return currentPositionPixel; + } + void SetcurrentPositionGPixel(const core::Point &value) + { + currentPositionPixel = value; + } - core::Point GetcurrentPositionGPixel(){return currentPositionPixel;} - void SetcurrentPositionGPixel(const core::Point &value){currentPositionPixel=value;} + core::Point GetrenderOffset() + { + return renderOffset; + } + void SetrenderOffset(const core::Point &value) + { + renderOffset = value; + } - core::Point GetrenderOffset(){return renderOffset;} - void SetrenderOffset(const core::Point &value){renderOffset=value;} + core::Point GetcenterTileXYLocation() + { + return centerTileXYLocation; + } + void SetcenterTileXYLocation(const core::Point &value) + { + centerTileXYLocation = value; + } - core::Point GetcenterTileXYLocation(){return centerTileXYLocation;} - void SetcenterTileXYLocation(const core::Point &value){centerTileXYLocation=value;} + core::Point GetcenterTileXYLocationLast() + { + return centerTileXYLocationLast; + } + void SetcenterTileXYLocationLast(const core::Point &value) + { + centerTileXYLocationLast = value; + } - core::Point GetcenterTileXYLocationLast(){return centerTileXYLocationLast;} - void SetcenterTileXYLocationLast(const core::Point &value){centerTileXYLocationLast=value;} + core::Point GetdragPoint() + { + return dragPoint; + } + void SetdragPoint(const core::Point &value) + { + dragPoint = value; + } - core::Point GetdragPoint(){return dragPoint;} - void SetdragPoint(const core::Point &value){dragPoint=value;} + core::Point GetmouseDown() + { + return mouseDown; + } + void SetmouseDown(const core::Point &value) + { + mouseDown = value; + } - core::Point GetmouseDown(){return mouseDown;} - void SetmouseDown(const core::Point &value){mouseDown=value;} + core::Point GetmouseCurrent() + { + return mouseCurrent; + } + void SetmouseCurrent(const core::Point &value) + { + mouseCurrent = value; + } - core::Point GetmouseCurrent(){return mouseCurrent;} - void SetmouseCurrent(const core::Point &value){mouseCurrent=value;} + core::Point GetmouseLastZoom() + { + return mouseLastZoom; + } + void SetmouseLastZoom(const core::Point &value) + { + mouseLastZoom = value; + } - core::Point GetmouseLastZoom(){return mouseLastZoom;} - void SetmouseLastZoom(const core::Point &value){mouseLastZoom=value;} + MouseWheelZoomType::Types GetMouseWheelZoomType() + { + return mousewheelzoomtype; + } + void SetMouseWheelZoomType(const MouseWheelZoomType::Types &value) + { + mousewheelzoomtype = value; + } - MouseWheelZoomType::Types GetMouseWheelZoomType(){return mousewheelzoomtype;} - void SetMouseWheelZoomType(const MouseWheelZoomType::Types &value){mousewheelzoomtype=value;} + PointLatLng GetLastLocationInBounds() + { + return LastLocationInBounds; + } + void SetLastLocationInBounds(const PointLatLng &value) + { + LastLocationInBounds = value; + } - PointLatLng GetLastLocationInBounds(){return LastLocationInBounds;} - void SetLastLocationInBounds(const PointLatLng &value){LastLocationInBounds=value;} + Size GetsizeOfMapArea() + { + return sizeOfMapArea; + } + void SetsizeOfMapArea(const Size &value) + { + sizeOfMapArea = value; + } - Size GetsizeOfMapArea(){return sizeOfMapArea;} - void SetsizeOfMapArea(const Size &value){sizeOfMapArea=value;} + Size GetminOfTiles() + { + return minOfTiles; + } + void SetminOfTiles(const Size &value) + { + minOfTiles = value; + } - Size GetminOfTiles(){return minOfTiles;} - void SetminOfTiles(const Size &value){minOfTiles=value;} + Size GetmaxOfTiles() + { + return maxOfTiles; + } + void SetmaxOfTiles(const Size &value) + { + maxOfTiles = value; + } - Size GetmaxOfTiles(){return maxOfTiles;} - void SetmaxOfTiles(const Size &value){maxOfTiles=value;} + Rectangle GettileRect() + { + return tileRect; + } + void SettileRect(const Rectangle &value) + { + tileRect = value; + } - Rectangle GettileRect(){return tileRect;} - void SettileRect(const Rectangle &value){tileRect=value;} + core::Point GettilePoint() + { + return tilePoint; + } + void SettilePoint(const core::Point &value) + { + tilePoint = value; + } - core::Point GettilePoint(){return tilePoint;} - void SettilePoint(const core::Point &value){tilePoint=value;} + Rectangle GetCurrentRegion() + { + return CurrentRegion; + } + void SetCurrentRegion(const Rectangle &value) + { + CurrentRegion = value; + } - Rectangle GetCurrentRegion(){return CurrentRegion;} - void SetCurrentRegion(const Rectangle &value){CurrentRegion=value;} + QList tileDrawingList; - QList tileDrawingList; + PureProjection *Projection() + { + return projection; + } + void SetProjection(PureProjection *value) + { + projection = value; + tileRect = Rectangle(core::Point(0, 0), value->TileSize()); + } + bool IsDragging() const + { + return isDragging; + } - PureProjection* Projection() - { - return projection; - } - void SetProjection(PureProjection* value) - { - projection=value; - tileRect=Rectangle(core::Point(0,0),value->TileSize()); - } - bool IsDragging()const{return isDragging;} + int Zoom() const + { + return zoom; + } + void SetZoom(int const & value); - int Zoom()const{return zoom;} - void SetZoom(int const& value); + int MaxZoom() const + { + return maxzoom; + } - int MaxZoom()const{return maxzoom;} + void UpdateBounds(); - void UpdateBounds(); + MapType::Types GetMapType() + { + return mapType; + } + void SetMapType(MapType::Types const & value); - MapType::Types GetMapType(){return mapType;} - void SetMapType(MapType::Types const& value); + void StartSystem(); - void StartSystem(); + void UpdateCenterTileXYLocation(); - void UpdateCenterTileXYLocation(); + void OnMapSizeChanged(int const & width, int const & height); // TODO had as slot - void OnMapSizeChanged(int const& width, int const& height);//TODO had as slot + void OnMapClose(); // TODO had as slot - void OnMapClose();//TODO had as slot + GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const & keys); - GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const& keys); + RectLatLng CurrentViewArea(); - RectLatLng CurrentViewArea(); + PointLatLng FromLocalToLatLng(int const & x, int const & y); - PointLatLng FromLocalToLatLng(int const& x, int const& y); + Point FromLatLngToLocal(PointLatLng const & latlng); - Point FromLatLngToLocal(PointLatLng const& latlng); + int GetMaxZoomToFitRect(RectLatLng const & rect); - int GetMaxZoomToFitRect(RectLatLng const& rect); + void BeginDrag(core::Point const & pt); - void BeginDrag(core::Point const& pt); + void EndDrag(); - void EndDrag(); + void ReloadMap(); - void ReloadMap(); + void GoToCurrentPosition(); - void GoToCurrentPosition(); + bool MouseWheelZooming; - bool MouseWheelZooming; + void DragOffset(core::Point const & offset); - void DragOffset(core::Point const& offset); + void Drag(core::Point const & pt); - void Drag(core::Point const& pt); + void CancelAsyncTasks(); - void CancelAsyncTasks(); + void FindTilesAround(QList &list); - void FindTilesAround(QList &list); + void UpdateGroundResolution(); - void UpdateGroundResolution(); + TileMatrix Matrix; - TileMatrix Matrix; + bool isStarted() + { + return started; + } - bool isStarted(){return started;} + diagnostics GetDiagnostics(); - diagnostics GetDiagnostics(); +signals: + void OnCurrentPositionChanged(internals::PointLatLng point); + void OnTileLoadComplete(); + void OnTilesStillToLoad(int number); + void OnTileLoadStart(); + void OnMapDrag(); + void OnMapZoomChanged(); + void OnMapTypeChanged(MapType::Types type); + void OnEmptyTileError(int zoom, core::Point pos); + void OnNeedInvalidation(); - signals: - void OnCurrentPositionChanged(internals::PointLatLng point); - void OnTileLoadComplete(); - void OnTilesStillToLoad(int number); - void OnTileLoadStart(); - void OnMapDrag(); - void OnMapZoomChanged(); - void OnMapTypeChanged(MapType::Types type); - void OnEmptyTileError(int zoom, core::Point pos); - void OnNeedInvalidation(); +private: - private: + void keepInBounds(); + PointLatLng currentPosition; + core::Point currentPositionPixel; + core::Point renderOffset; + core::Point centerTileXYLocation; + core::Point centerTileXYLocationLast; + core::Point dragPoint; + Rectangle tileRect; + core::Point mouseDown; + bool CanDragMap; + core::Point mouseCurrent; + PointLatLng LastLocationInBounds; + core::Point mouseLastZoom; - void keepInBounds(); - PointLatLng currentPosition; - core::Point currentPositionPixel; - core::Point renderOffset; - core::Point centerTileXYLocation; - core::Point centerTileXYLocationLast; - core::Point dragPoint; - Rectangle tileRect; - core::Point mouseDown; - bool CanDragMap; - core::Point mouseCurrent; - PointLatLng LastLocationInBounds; - core::Point mouseLastZoom; - - MouseWheelZoomType::Types mousewheelzoomtype; + MouseWheelZoomType::Types mousewheelzoomtype; - Size sizeOfMapArea; - Size minOfTiles; - Size maxOfTiles; + Size sizeOfMapArea; + Size minOfTiles; + Size maxOfTiles; - core::Point tilePoint; + core::Point tilePoint; - Rectangle CurrentRegion; + Rectangle CurrentRegion; - QQueue tileLoadQueue; + QQueue tileLoadQueue; - int zoom; + int zoom; - PureProjection* projection; + PureProjection *projection; - bool isDragging; + bool isDragging; - QMutex MtileLoadQueue; + QMutex MtileLoadQueue; - QMutex Moverlays; + QMutex Moverlays; - QMutex MtileDrawingList; + QMutex MtileDrawingList; #ifdef DEBUG_CORE - QMutex Mdebug; - static qlonglong debugcounter; + QMutex Mdebug; + static qlonglong debugcounter; #endif - Size TooltipTextPadding; + Size TooltipTextPadding; - MapType::Types mapType; + MapType::Types mapType; - QSemaphore loaderLimit; + QSemaphore loaderLimit; - QThreadPool ProcessLoadTaskCallback; - QMutex MtileToload; - int tilesToload; + QThreadPool ProcessLoadTaskCallback; + QMutex MtileToload; + int tilesToload; - int maxzoom; - QMutex MrunningThreads; - int runningThreads; - diagnostics diag; + int maxzoom; + QMutex MrunningThreads; + int runningThreads; + diagnostics diag; - protected: - bool started; - - int Width; - int Height; - int pxRes100m; // 100 meters - int pxRes1000m; // 1km - int pxRes10km; // 10km - int pxRes100km; // 100km - int pxRes1000km; // 1000km - int pxRes5000km; // 5000km - void SetCurrentPositionGPixel(core::Point const& value){currentPositionPixel = value;} - void GoToCurrentPositionOnZoom(); - - }; +protected: + bool started; + int Width; + int Height; + int pxRes100m; // 100 meters + int pxRes1000m; // 1km + int pxRes10km; // 10km + int pxRes100km; // 100km + int pxRes1000km; // 1000km + int pxRes5000km; // 5000km + void SetCurrentPositionGPixel(core::Point const & value) + { + currentPositionPixel = value; + } + void GoToCurrentPositionOnZoom(); +}; } #endif // CORE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/debugheader.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/debugheader.h index 50ae028f8..70623712f 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/debugheader.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/debugheader.h @@ -1,8 +1,8 @@ #ifndef DEBUGHEADER_H #define DEBUGHEADER_H -//#define DEBUG_CORE -//#define DEBUG_TILE -//#define DEBUG_TILEMATRIX +// #define DEBUG_CORE +// #define DEBUG_TILE +// #define DEBUG_TILEMATRIX #endif // DEBUGHEADER_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.cpp index 535c7e615..3f0fa7225 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.cpp @@ -1,35 +1,35 @@ /** -****************************************************************************** -* -* @file loadtask.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file loadtask.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "loadtask.h" - + namespace internals { -bool operator==(LoadTask const& lhs,LoadTask const& rhs) +bool operator==(LoadTask const & lhs, LoadTask const & rhs) { - return ((lhs.Pos==rhs.Pos)&&(lhs.Zoom==rhs.Zoom)); + return (lhs.Pos == rhs.Pos) && (lhs.Zoom == rhs.Zoom); } } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.h index 47df78a65..2810c56b5 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file loadtask.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file loadtask.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef LOADTASK_H #define LOADTASK_H @@ -31,35 +31,33 @@ #include "../core/point.h" using namespace core; -namespace internals -{ -struct LoadTask - { - friend bool operator==(LoadTask const& lhs,LoadTask const& rhs); - public: +namespace internals { +struct LoadTask { + friend bool operator==(LoadTask const & lhs, LoadTask const & rhs); +public: core::Point Pos; int Zoom; LoadTask(Point pos, int zoom) - { - Pos = pos; + { + Pos = pos; Zoom = zoom; } LoadTask() { - Pos=core::Point(-1,-1); - Zoom=-1; + Pos = core::Point(-1, -1); + Zoom = -1; } bool HasValue() { - return !(Zoom==-1); + return !(Zoom == -1); } - QString ToString()const - { + QString ToString() const + { return QString::number(Zoom) + " - " + Pos.ToString(); - } - }; + } +}; } #endif // LOADTASK_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/mousewheelzoomtype.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/mousewheelzoomtype.h index d3fe648b9..b947564fb 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/mousewheelzoomtype.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/mousewheelzoomtype.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file mousewheelzoomtype.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file mousewheelzoomtype.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef MOUSEWHEELZOOMTYPE_H #define MOUSEWHEELZOOMTYPE_H #include @@ -32,13 +32,10 @@ #include #include namespace internals { - class MouseWheelZoomType:public QObject - { - Q_OBJECT - Q_ENUMS(Types) - public: - enum Types - { +class MouseWheelZoomType : public QObject { + Q_OBJECT Q_ENUMS(Types) +public: + enum Types { /// /// zooms map to current mouse position and makes it map center /// @@ -55,35 +52,34 @@ namespace internals { /// ViewCenter }; - static QString StrByType(Types const& value) + static QString StrByType(Types const & value) { QMetaObject metaObject = MouseWheelZoomType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - QString s=metaEnum.valueToKey(value); + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + QString s = metaEnum.valueToKey(value); + return s; } - static Types TypeByStr(QString const& value) + static Types TypeByStr(QString const & value) { QMetaObject metaObject = MouseWheelZoomType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - Types s=(Types)metaEnum.keyToValue(value.toLatin1()); + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + Types s = (Types)metaEnum.keyToValue(value.toLatin1()); + return s; } static QStringList TypesList() { QStringList ret; QMetaObject metaObject = MouseWheelZoomType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - for(int x=0;x #include #include "sizelatlng.h" - + namespace internals { -struct PointLatLng -{ - //friend uint qHash(PointLatLng const& point); - friend bool operator==(PointLatLng const& lhs,PointLatLng const& rhs); - friend bool operator!=(PointLatLng const& left, PointLatLng const& right); +struct PointLatLng { + // friend uint qHash(PointLatLng const& point); + friend bool operator==(PointLatLng const & lhs, PointLatLng const & rhs); + friend bool operator!=(PointLatLng const & left, PointLatLng const & right); friend PointLatLng operator+(PointLatLng pt, SizeLatLng sz); friend PointLatLng operator-(PointLatLng pt, SizeLatLng sz); - //TODO Sizelatlng friend PointLatLng operator+(PointLatLng pt, SizeLatLng sz); + // TODO Sizelatlng friend PointLatLng operator+(PointLatLng pt, SizeLatLng sz); - private: +private: double lat; double lng; - bool empty; - public: + bool empty; +public: PointLatLng(); static PointLatLng Empty; - PointLatLng(const double &lat,const double &lng) - { - this->lat = lat; - this->lng = lng; - empty=false; - } + PointLatLng(const double &lat, const double &lng) + { + this->lat = lat; + this->lng = lng; + empty = false; + } - bool IsEmpty() - { - return empty; - } + bool IsEmpty() + { + return empty; + } - double Lat()const - { - return this->lat; - } + double Lat() const + { + return this->lat; + } - void SetLat(const double &value) - { - this->lat = value; - empty=false; - } + void SetLat(const double &value) + { + this->lat = value; + empty = false; + } - double Lng()const - { - return this->lng; - } - void SetLng(const double &value) - { - this->lng = value; - empty=false; - } + double Lng() const + { + return this->lng; + } + void SetLng(const double &value) + { + this->lng = value; + empty = false; + } + static PointLatLng Add(PointLatLng const & pt, SizeLatLng const & sz) + { + return PointLatLng(pt.Lat() - sz.HeightLat(), pt.Lng() + sz.WidthLng()); + } + + static PointLatLng Subtract(PointLatLng const & pt, SizeLatLng const & sz) + { + return PointLatLng(pt.Lat() + sz.HeightLat(), pt.Lng() - sz.WidthLng()); + } + void Offset(PointLatLng const & pos) + { + this->Offset(pos.Lat(), pos.Lng()); + } - static PointLatLng Add(PointLatLng const& pt, SizeLatLng const& sz) - { - return PointLatLng(pt.Lat() - sz.HeightLat(), pt.Lng() + sz.WidthLng()); - } - - static PointLatLng Subtract(PointLatLng const& pt, SizeLatLng const& sz) - { - return PointLatLng(pt.Lat() + sz.HeightLat(), pt.Lng() - sz.WidthLng()); - } + void Offset(double const & lat, double const & lng) + { + this->lng += lng; + this->lat -= lat; + } - void Offset(PointLatLng const& pos) - { - this->Offset(pos.Lat(), pos.Lng()); - } - - void Offset(double const& lat, double const& lng) - { - this->lng += lng; - this->lat -= lat; - } - - - QString ToString()const - { - return QString("{Lat=%1, Lng=%2}").arg(this->lat).arg(this->lng); - } + QString ToString() const + { + return QString("{Lat=%1, Lng=%2}").arg(this->lat).arg(this->lng); + } //// static PointLatLng() //// { //// Empty = new PointLatLng(); //// } - }; +}; // diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.cpp index 0273a5bda..415bbf7f6 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.cpp @@ -1,38 +1,36 @@ /** -****************************************************************************** -* -* @file lks94projection.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file lks94projection.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "lks94projection.h" - namespace projections { -LKS94Projection::LKS94Projection():MinLatitude (53.33 ), MaxLatitude (56.55 ), MinLongitude (20.22 ), -MaxLongitude (27.11 ), orignX (5122000 ), orignY (10000100 ),tileSize(256, 256) -{ -} +LKS94Projection::LKS94Projection() : MinLatitude(53.33), MaxLatitude(56.55), MinLongitude(20.22), + MaxLongitude(27.11), orignX(5122000), orignY(10000100), tileSize(256, 256) +{} Size LKS94Projection::TileSize() const { @@ -44,198 +42,174 @@ double LKS94Projection::Axis() const } double LKS94Projection::Flattening() const { - - return (1.0 / 298.257222101); - + return 1.0 / 298.257222101; } -Point LKS94Projection::FromLatLngToPixel(double lat, double lng, int const& zoom) +Point LKS94Projection::FromLatLngToPixel(double lat, double lng, int const & zoom) { Point ret; - lat = Clip(lat, MinLatitude, MaxLatitude); - lng = Clip(lng, MinLongitude, MaxLongitude); + lat = Clip(lat, MinLatitude, MaxLatitude); + lng = Clip(lng, MinLongitude, MaxLongitude); QVector lks(3); - lks[0]=lng; - lks[1]=lat; - lks = DTM10(lks); - lks = MTD10(lks); - lks = DTM00(lks); + lks[0] = lng; + lks[1] = lat; + lks = DTM10(lks); + lks = MTD10(lks); + lks = DTM00(lks); double res = GetTileMatrixResolution(zoom); - ret.SetX((int) floor((lks[0] + orignX) / res)); - ret.SetY((int) floor((orignY - lks[1]) / res)); + ret.SetX((int)floor((lks[0] + orignX) / res)); + ret.SetY((int)floor((orignY - lks[1]) / res)); return ret; } -internals::PointLatLng LKS94Projection::FromPixelToLatLng(int const& x, int const& y, int const& zoom) +internals::PointLatLng LKS94Projection::FromPixelToLatLng(int const & x, int const & y, int const & zoom) { - internals::PointLatLng ret;// = internals::PointLatLng::Empty; + internals::PointLatLng ret; // = internals::PointLatLng::Empty; double res = GetTileMatrixResolution(zoom); QVector lks(2); - lks[0]=(x * res) - orignX; - lks[1]=-(y * res) + orignY; - lks = MTD11(lks); - lks = DTM10(lks); - lks = MTD10(lks); + lks[0] = (x * res) - orignX; + lks[1] = -(y * res) + orignY; + lks = MTD11(lks); + lks = DTM10(lks); + lks = MTD10(lks); ret.SetLat(Clip(lks[1], MinLatitude, MaxLatitude)); ret.SetLng(Clip(lks[0], MinLongitude, MaxLongitude)); return ret; } -QVector LKS94Projection::DTM10(const QVector & lonlat) +QVector LKS94Projection::DTM10(const QVector & lonlat) { - double es; // Eccentricity squared : (a^2 - b^2)/a^2 - double semiMajor = 6378137.0; // major axis - double semiMinor = 6356752.3142451793; // minor axis - double ab; // Semi_major / semi_minor - double ba; // Semi_minor / semi_major - double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 + double es; // Eccentricity squared : (a^2 - b^2)/a^2 + double semiMajor = 6378137.0; // major axis + double semiMinor = 6356752.3142451793; // minor axis + double ab; // Semi_major / semi_minor + double ba; // Semi_minor / semi_major + double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 - es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); //e^2 + es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); // e^2 ses = (pow(semiMajor, 2) - pow(semiMinor, 2)) / pow(semiMinor, 2); - ba = semiMinor / semiMajor; - ab = semiMajor / semiMinor; + ba = semiMinor / semiMajor; + ab = semiMajor / semiMinor; // ... double lon = DegreesToRadians(lonlat[0]); double lat = DegreesToRadians(lonlat[1]); - double h = lonlat.count() < 3 ? 0 : std::isnan(lonlat[2]) ? 0 : lonlat[2];//TODO NAN - double v = semiMajor / sqrt(1 - es * pow(sin(lat), 2)); - double x = (v + h) * cos(lat) * cos(lon); - double y = (v + h) * cos(lat) * sin(lon); - double z = ((1 - es) * v + h) * sin(lat); + double h = lonlat.count() < 3 ? 0 : std::isnan(lonlat[2]) ? 0 : lonlat[2]; // TODO NAN + double v = semiMajor / sqrt(1 - es * pow(sin(lat), 2)); + double x = (v + h) * cos(lat) * cos(lon); + double y = (v + h) * cos(lat) * sin(lon); + double z = ((1 - es) * v + h) * sin(lat); QVector ret(3); - ret[0]=x; - ret[1]=y; - ret[2]=z; - return ret; + ret[0] = x; + ret[1] = y; + ret[2] = z; + return ret; } -QVector LKS94Projection::MTD10(QVector & pnt) +QVector LKS94Projection::MTD10(QVector & pnt) { QVector ret(3); - const double COS_67P5 = 0.38268343236508977; // cosine of 67.5 degrees - const double AD_C = 1.0026000; // Toms region 1 constant + const double COS_67P5 = 0.38268343236508977; // cosine of 67.5 degrees + const double AD_C = 1.0026000; // Toms region 1 constant - double es; // Eccentricity squared : (a^2 - b^2)/a^2 - double semiMajor = 6378137.0; // major axis - double semiMinor = 6356752.3141403561; // minor axis - double ab; // Semi_major / semi_minor - double ba; // Semi_minor / semi_major - double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 + double es; // Eccentricity squared : (a^2 - b^2)/a^2 + double semiMajor = 6378137.0; // major axis + double semiMinor = 6356752.3141403561; // minor axis + double ab; // Semi_major / semi_minor + double ba; // Semi_minor / semi_major + double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 - es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); //e^2 + es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); // e^2 ses = (pow(semiMajor, 2) - pow(semiMinor, 2)) / pow(semiMinor, 2); - ba = semiMinor / semiMajor; - ab = semiMajor / semiMinor; + ba = semiMinor / semiMajor; + ab = semiMajor / semiMinor; // ... - bool AtPole = false; // is location in polar region - double Z = pnt.count() < 3 ? 0 : std::isnan(pnt[2]) ? 0 : pnt[2];//TODO NaN + bool AtPole = false; // is location in polar region + double Z = pnt.count() < 3 ? 0 : std::isnan(pnt[2]) ? 0 : pnt[2]; // TODO NaN - double lon = 0; - double lat = 0; + double lon = 0; + double lat = 0; double Height = 0; - if(pnt[0] != 0.0) - { + if (pnt[0] != 0.0) { lon = atan2(pnt[1], pnt[0]); - } - else - { - if(pnt[1] > 0) - { + } else { + if (pnt[1] > 0) { lon = M_PI / 2; - } - else - if(pnt[1] < 0) - { + } else if (pnt[1] < 0) { lon = -M_PI * 0.5; - } - else - { + } else { AtPole = true; - lon = 0.0; - if(Z > 0.0) // north pole - { + lon = 0.0; + if (Z > 0.0) { // north pole lat = M_PI * 0.5; - } - else - if(Z < 0.0) // south pole - { + } else if (Z < 0.0) { // south pole lat = -M_PI * 0.5; - } - else // center of earth - { - ret[0]=RadiansToDegrees(lon); - ret[1]=RadiansToDegrees(M_PI * 0.5); - ret[2]=-semiMinor; + } else { // center of earth + ret[0] = RadiansToDegrees(lon); + ret[1] = RadiansToDegrees(M_PI * 0.5); + ret[2] = -semiMinor; return ret; } } } - double W2 = pnt[0] * pnt[0] + pnt[1] * pnt[1]; // Square of distance from Z axis - double W = sqrt(W2); // distance from Z axis - double T0 = Z * AD_C; // initial estimate of vertical component - double S0 = sqrt(T0 * T0 + W2); // initial estimate of horizontal component - double Sin_B0 = T0 / S0; // sin(B0), B0 is estimate of Bowring aux variable - double Cos_B0 = W / S0; // cos(B0) + double W2 = pnt[0] * pnt[0] + pnt[1] * pnt[1]; // Square of distance from Z axis + double W = sqrt(W2); // distance from Z axis + double T0 = Z * AD_C; // initial estimate of vertical component + double S0 = sqrt(T0 * T0 + W2); // initial estimate of horizontal component + double Sin_B0 = T0 / S0; // sin(B0), B0 is estimate of Bowring aux variable + double Cos_B0 = W / S0; // cos(B0) double Sin3_B0 = pow(Sin_B0, 3); - double T1 = Z + semiMinor * ses * Sin3_B0; // corrected estimate of vertical component - double Sum = W - semiMajor * es * Cos_B0 * Cos_B0 * Cos_B0; // numerator of cos(phi1) - double S1 = sqrt(T1 * T1 + Sum * Sum); // corrected estimate of horizontal component - double Sin_p1 = T1 / S1; // sin(phi1), phi1 is estimated latitude - double Cos_p1 = Sum / S1; // cos(phi1) - double Rn = semiMajor / sqrt(1.0 - es * Sin_p1 * Sin_p1); // Earth radius at location - if(Cos_p1 >= COS_67P5) - { + double T1 = Z + semiMinor * ses * Sin3_B0; // corrected estimate of vertical component + double Sum = W - semiMajor * es * Cos_B0 * Cos_B0 * Cos_B0; // numerator of cos(phi1) + double S1 = sqrt(T1 * T1 + Sum * Sum); // corrected estimate of horizontal component + double Sin_p1 = T1 / S1; // sin(phi1), phi1 is estimated latitude + double Cos_p1 = Sum / S1; // cos(phi1) + double Rn = semiMajor / sqrt(1.0 - es * Sin_p1 * Sin_p1); // Earth radius at location + if (Cos_p1 >= COS_67P5) { Height = W / Cos_p1 - Rn; - } - else - if(Cos_p1 <= -COS_67P5) - { + } else if (Cos_p1 <= -COS_67P5) { Height = W / -Cos_p1 - Rn; - } - else - { + } else { Height = Z / Sin_p1 + Rn * (es - 1.0); } - if(!AtPole) - { + if (!AtPole) { lat = atan(Sin_p1 / Cos_p1); } - ret[0]=RadiansToDegrees(lon); - ret[1]=RadiansToDegrees(lat); - ret[2]=Height; + ret[0] = RadiansToDegrees(lon); + ret[1] = RadiansToDegrees(lat); + ret[2] = Height; return ret; } -QVector LKS94Projection::DTM00(QVector & lonlat) +QVector LKS94Projection::DTM00(QVector & lonlat) { - double scaleFactor = 0.9998; // scale factor - double centralMeridian = 0.41887902047863912; // Center qlonglongitude (projection center) */ - double latOrigin = 0.0; // center latitude - double falseNorthing = 0.0; // y offset in meters - double falseEasting = 500000.0; // x offset in meters - double semiMajor = 6378137.0; // major axis - double semiMinor = 6356752.3141403561; // minor axis - double metersPerUnit = 1.0; + double scaleFactor = 0.9998; // scale factor + double centralMeridian = 0.41887902047863912; // Center qlonglongitude (projection center) */ + double latOrigin = 0.0; // center latitude + double falseNorthing = 0.0; // y offset in meters + double falseEasting = 500000.0; // x offset in meters + double semiMajor = 6378137.0; // major axis + double semiMinor = 6356752.3141403561; // minor axis + double metersPerUnit = 1.0; - double e0, e1, e2, e3; // eccentricity constants - double e, es, esp; // eccentricity constants - double ml0; // small value m + double e0, e1, e2, e3; // eccentricity constants + double e, es, esp; // eccentricity constants + double ml0; // small value m - es = 1.0 - pow(semiMinor / semiMajor, 2); - e = sqrt(es); - e0 = e0fn(es); - e1 = e1fn(es); - e2 = e2fn(es); - e3 = e3fn(es); + es = 1.0 - pow(semiMinor / semiMajor, 2); + e = sqrt(es); + e0 = e0fn(es); + e1 = e1fn(es); + e2 = e2fn(es); + e3 = e3fn(es); ml0 = semiMajor * mlfn(e0, e1, e2, e3, latOrigin); esp = es / (1.0 - es); @@ -244,23 +218,23 @@ QVector LKS94Projection::DTM00(QVector & lonlat) double lon = DegreesToRadians(lonlat[0]); double lat = DegreesToRadians(lonlat[1]); - double delta_lon = 0.0; // Delta qlonglongitude (Given qlonglongitude - center) + double delta_lon = 0.0; // Delta qlonglongitude (Given qlonglongitude - center) double sin_phi, cos_phi; // sin and cos value - double al, als; // temporary values - double c, t, tq; // temporary values - double con, n, ml; // cone constant, small m + double al, als; // temporary values + double c, t, tq; // temporary values + double con, n, ml; // cone constant, small m delta_lon = LKS94Projection::AdjustLongitude(lon - centralMeridian); - LKS94Projection::SinCos(lat, sin_phi, cos_phi); + LKS94Projection::SinCos(lat, sin_phi, cos_phi); - al = cos_phi * delta_lon; + al = cos_phi * delta_lon; als = pow(al, 2); - c = pow(cos_phi, 2); - tq = tan(lat); - t = pow(tq, 2); + c = pow(cos_phi, 2); + tq = tan(lat); + t = pow(tq, 2); con = 1.0 - es * pow(sin_phi, 2); - n = semiMajor / sqrt(con); - ml = semiMajor * mlfn(e0, e1, e2, e3, lat); + n = semiMajor / sqrt(con); + ml = semiMajor * mlfn(e0, e1, e2, e3, lat); double x = scaleFactor * n * al * (1.0 + als / 6.0 * (1.0 - t + c + als / 20.0 * (5.0 - 18.0 * t + pow(t, 2) + 72.0 * c - 58.0 * esp))) + falseEasting; @@ -269,176 +243,151 @@ QVector LKS94Projection::DTM00(QVector & lonlat) (5.0 - t + 9.0 * c + 4.0 * pow(c, 2) + als / 30.0 * (61.0 - 58.0 * t + pow(t, 2) + 600.0 * c - 330.0 * esp))))) + falseNorthing; - if(lonlat.count() < 3) - { + if (lonlat.count() < 3) { QVector ret(2); - ret[0]= x / metersPerUnit; - ret[1]= y / metersPerUnit; + ret[0] = x / metersPerUnit; + ret[1] = y / metersPerUnit; return ret; - } - else - { + } else { QVector ret(3); - ret[0]= x / metersPerUnit; - ret[1]= y / metersPerUnit; - ret[2]=lonlat[2]; + ret[0] = x / metersPerUnit; + ret[1] = y / metersPerUnit; + ret[2] = lonlat[2]; return ret; } } -QVector LKS94Projection::DTM01(QVector & lonlat) +QVector LKS94Projection::DTM01(QVector & lonlat) { - double es; // Eccentricity squared : (a^2 - b^2)/a^2 - double semiMajor = 6378137.0; // major axis - double semiMinor = 6356752.3141403561; // minor axis - double ab; // Semi_major / semi_minor - double ba; // Semi_minor / semi_major - double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 + double es; // Eccentricity squared : (a^2 - b^2)/a^2 + double semiMajor = 6378137.0; // major axis + double semiMinor = 6356752.3141403561; // minor axis + double ab; // Semi_major / semi_minor + double ba; // Semi_minor / semi_major + double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 - es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); - ses = (pow(semiMajor, 2) -pow(semiMinor, 2)) / pow(semiMinor, 2); - ba = semiMinor / semiMajor; - ab = semiMajor / semiMinor; + es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); + ses = (pow(semiMajor, 2) - pow(semiMinor, 2)) / pow(semiMinor, 2); + ba = semiMinor / semiMajor; + ab = semiMajor / semiMinor; // ... double lon = DegreesToRadians(lonlat[0]); double lat = DegreesToRadians(lonlat[1]); - double h = lonlat.count() < 3 ? 0 : std::isnan(lonlat[2]) ? 0 : lonlat[2];//TODO NaN - double v = semiMajor / sqrt(1 - es * pow(sin(lat), 2)); - double x = (v + h) * cos(lat) * cos(lon); - double y = (v + h) * cos(lat) * sin(lon); - double z = ((1 - es) * v + h) * sin(lat); + double h = lonlat.count() < 3 ? 0 : std::isnan(lonlat[2]) ? 0 : lonlat[2]; // TODO NaN + double v = semiMajor / sqrt(1 - es * pow(sin(lat), 2)); + double x = (v + h) * cos(lat) * cos(lon); + double y = (v + h) * cos(lat) * sin(lon); + double z = ((1 - es) * v + h) * sin(lat); QVector ret(3); - ret[0]=x; - ret[1]=y; - ret[2]=z; + ret[0] = x; + ret[1] = y; + ret[2] = z; return ret; } -QVector LKS94Projection::MTD01(QVector & pnt) +QVector LKS94Projection::MTD01(QVector & pnt) { const double COS_67P5 = 0.38268343236508977; // cosine of 67.5 degrees - const double AD_C = 1.0026000; // Toms region 1 constant + const double AD_C = 1.0026000; // Toms region 1 constant - double es; // Eccentricity squared : (a^2 - b^2)/a^2 - double semiMajor = 6378137.0; // major axis - double semiMinor = 6356752.3142451793; // minor axis - double ab; // Semi_major / semi_minor - double ba; // Semi_minor / semi_major - double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 + double es; // Eccentricity squared : (a^2 - b^2)/a^2 + double semiMajor = 6378137.0; // major axis + double semiMinor = 6356752.3142451793; // minor axis + double ab; // Semi_major / semi_minor + double ba; // Semi_minor / semi_major + double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 - es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); + es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); ses = (pow(semiMajor, 2) - pow(semiMinor, 2)) / pow(semiMinor, 2); - ba = semiMinor / semiMajor; - ab = semiMajor / semiMinor; + ba = semiMinor / semiMajor; + ab = semiMajor / semiMinor; // ... - bool At_Pole = false; // is location in polar region - double Z = pnt.count() < 3 ? 0 : std::isnan(pnt[2]) ? 0 : pnt[2];//TODO NaN + bool At_Pole = false; // is location in polar region + double Z = pnt.count() < 3 ? 0 : std::isnan(pnt[2]) ? 0 : pnt[2]; // TODO NaN - double lon = 0; - double lat = 0; + double lon = 0; + double lat = 0; double Height = 0; - if(pnt[0] != 0.0) - { + if (pnt[0] != 0.0) { lon = atan2(pnt[1], pnt[0]); - } - else - { - if(pnt[1] > 0) - { + } else { + if (pnt[1] > 0) { lon = M_PI / 2; - } - else - if(pnt[1] < 0) - { + } else if (pnt[1] < 0) { lon = -M_PI * 0.5; - } - else - { + } else { At_Pole = true; - lon = 0.0; - if(Z > 0.0) // north pole - { + lon = 0.0; + if (Z > 0.0) { // north pole lat = M_PI * 0.5; - } - else - if(Z < 0.0) // south pole - { + } else if (Z < 0.0) { // south pole lat = -M_PI * 0.5; - } - else // center of earth - { + } else { // center of earth QVector ret(3); - ret[0]=RadiansToDegrees(lon); - ret[1]=RadiansToDegrees(M_PI * 0.5); - ret[2]=-semiMinor; + ret[0] = RadiansToDegrees(lon); + ret[1] = RadiansToDegrees(M_PI * 0.5); + ret[2] = -semiMinor; return ret; } } } - double W2 = pnt[0] * pnt[0] + pnt[1] * pnt[1]; // Square of distance from Z axis - double W = sqrt(W2); // distance from Z axis - double T0 = Z * AD_C; // initial estimate of vertical component - double S0 = sqrt(T0 * T0 + W2); //initial estimate of horizontal component - double Sin_B0 = T0 / S0; // sin(B0), B0 is estimate of Bowring aux variable - double Cos_B0 = W / S0; // cos(B0) + double W2 = pnt[0] * pnt[0] + pnt[1] * pnt[1]; // Square of distance from Z axis + double W = sqrt(W2); // distance from Z axis + double T0 = Z * AD_C; // initial estimate of vertical component + double S0 = sqrt(T0 * T0 + W2); // initial estimate of horizontal component + double Sin_B0 = T0 / S0; // sin(B0), B0 is estimate of Bowring aux variable + double Cos_B0 = W / S0; // cos(B0) double Sin3_B0 = pow(Sin_B0, 3); - double T1 = Z + semiMinor * ses * Sin3_B0; //corrected estimate of vertical component - double Sum = W - semiMajor * es * Cos_B0 * Cos_B0 * Cos_B0; // numerator of cos(phi1) - double S1 = sqrt(T1 * T1 + Sum * Sum); // corrected estimate of horizontal component - double Sin_p1 = T1 / S1; // sin(phi1), phi1 is estimated latitude - double Cos_p1 = Sum / S1; // cos(phi1) - double Rn = semiMajor / sqrt(1.0 - es * Sin_p1 * Sin_p1); // Earth radius at location + double T1 = Z + semiMinor * ses * Sin3_B0; // corrected estimate of vertical component + double Sum = W - semiMajor * es * Cos_B0 * Cos_B0 * Cos_B0; // numerator of cos(phi1) + double S1 = sqrt(T1 * T1 + Sum * Sum); // corrected estimate of horizontal component + double Sin_p1 = T1 / S1; // sin(phi1), phi1 is estimated latitude + double Cos_p1 = Sum / S1; // cos(phi1) + double Rn = semiMajor / sqrt(1.0 - es * Sin_p1 * Sin_p1); // Earth radius at location - if(Cos_p1 >= COS_67P5) - { + if (Cos_p1 >= COS_67P5) { Height = W / Cos_p1 - Rn; - } - else - if(Cos_p1 <= -COS_67P5) - { + } else if (Cos_p1 <= -COS_67P5) { Height = W / -Cos_p1 - Rn; - } - else - { + } else { Height = Z / Sin_p1 + Rn * (es - 1.0); } - if(!At_Pole) - { + if (!At_Pole) { lat = atan(Sin_p1 / Cos_p1); } QVector ret(3); - ret[0]=RadiansToDegrees(lon); - ret[1]=RadiansToDegrees(lat); - ret[2]=Height; + ret[0] = RadiansToDegrees(lon); + ret[1] = RadiansToDegrees(lat); + ret[2] = Height; return ret; } -QVector LKS94Projection::MTD11(QVector & p) +QVector LKS94Projection::MTD11(QVector & p) { - double scaleFactor = 0.9998; // scale factor - double centralMeridian = 0.41887902047863912; // Center qlonglongitude (projection center) - double latOrigin = 0.0; // center latitude - double falseNorthing = 0.0; // y offset in meters - double falseEasting = 500000.0; // x offset in meters - double semiMajor = 6378137.0; // major axis - double semiMinor = 6356752.3141403561; // minor axis - double metersPerUnit = 1.0; + double scaleFactor = 0.9998; // scale factor + double centralMeridian = 0.41887902047863912; // Center qlonglongitude (projection center) + double latOrigin = 0.0; // center latitude + double falseNorthing = 0.0; // y offset in meters + double falseEasting = 500000.0; // x offset in meters + double semiMajor = 6378137.0; // major axis + double semiMinor = 6356752.3141403561; // minor axis + double metersPerUnit = 1.0; - double e0, e1, e2, e3; // eccentricity constants - double e, es, esp; // eccentricity constants - double ml0; // small value m + double e0, e1, e2, e3; // eccentricity constants + double e, es, esp; // eccentricity constants + double ml0; // small value m - es =(semiMinor * semiMinor) / (semiMajor * semiMajor); - es=1.0-es; - e = sqrt(es); - e0 = e0fn(es); - e1 = e1fn(es); - e2 = e2fn(es); - e3 = e3fn(es); + es = (semiMinor * semiMinor) / (semiMajor * semiMajor); + es = 1.0 - es; + e = sqrt(es); + e0 = e0fn(es); + e1 = e1fn(es); + e2 = e2fn(es); + e3 = e3fn(es); ml0 = semiMajor * mlfn(e0, e1, e2, e3, latOrigin); esp = es / (1.0 - es); @@ -456,30 +405,30 @@ QVector LKS94Projection::MTD11(QVector & p) con = (ml0 + y / scaleFactor) / semiMajor; phi = con; - for(i = 0; ; i++) - { + for (i = 0;; i++) { delta_phi = ((con + e1 * sin(2.0 * phi) - e2 * sin(4.0 * phi) + e3 * sin(6.0 * phi)) / e0) - phi; phi += delta_phi; - if(fabs(delta_phi) <= EPSLoN) + if (fabs(delta_phi) <= EPSLoN) { break; + } - if(i >= max_iter) + if (i >= max_iter) { throw "Latitude failed to converge"; + } } - if(fabs(phi) < HALF_PI) - { - SinCos(phi, sin_phi, cos_phi); + if (fabs(phi) < HALF_PI) { + SinCos(phi, sin_phi, cos_phi); tan_phi = tan(phi); - c = esp * pow(cos_phi, 2); - cs = pow(c, 2); - t = pow(tan_phi, 2); - ts = pow(t, 2); + c = esp * pow(cos_phi, 2); + cs = pow(c, 2); + t = pow(tan_phi, 2); + ts = pow(t, 2); con = 1.0 - es * pow(sin_phi, 2); - n = semiMajor / sqrt(con); - r = n * (1.0 - es) / con; - d = x / (n * scaleFactor); - ds = pow(d, 2); + n = semiMajor / sqrt(con); + r = n * (1.0 - es) / con; + d = x / (n * scaleFactor); + ds = pow(d, 2); double lat = phi - (n * tan_phi * ds / r) * (0.5 - ds / 24.0 * (5.0 + 3.0 * t + 10.0 * c - 4.0 * cs - 9.0 * esp - ds / 30.0 * (61.0 + 90.0 * t + @@ -489,127 +438,115 @@ QVector LKS94Projection::MTD11(QVector & p) c - ds / 20.0 * (5.0 - 2.0 * c + 28.0 * t - 3.0 * cs + 8.0 * esp + 24.0 * ts))) / cos_phi)); - if(p.count() < 3) - { + if (p.count() < 3) { QVector ret(2); - ret[0]= RadiansToDegrees(lon); - ret[1]= RadiansToDegrees(lat); + ret[0] = RadiansToDegrees(lon); + ret[1] = RadiansToDegrees(lat); return ret; - } - else - { + } else { QVector ret(3); - ret[0]= RadiansToDegrees(lon); - ret[1]= RadiansToDegrees(lat); - ret[2]=p[2]; + ret[0] = RadiansToDegrees(lon); + ret[1] = RadiansToDegrees(lat); + ret[2] = p[2]; return ret; - //return new double[] { RadiansToDegrees(lon), RadiansToDegrees(lat), p[2] }; + // return new double[] { RadiansToDegrees(lon), RadiansToDegrees(lat), p[2] }; } - } - else - { - if(p.count() < 3) - { + } else { + if (p.count() < 3) { QVector ret(2); - ret[0]= RadiansToDegrees(HALF_PI * Sign(y)); - ret[1]= RadiansToDegrees(centralMeridian); + ret[0] = RadiansToDegrees(HALF_PI * Sign(y)); + ret[1] = RadiansToDegrees(centralMeridian); return ret; - } - - else - { + } else { QVector ret(3); - ret[0]= RadiansToDegrees(HALF_PI * Sign(y)); - ret[1]= RadiansToDegrees(centralMeridian); - ret[2]=p[2]; + ret[0] = RadiansToDegrees(HALF_PI * Sign(y)); + ret[1] = RadiansToDegrees(centralMeridian); + ret[2] = p[2]; return ret; } - } } -double LKS94Projection::Clip(double const& n, double const& minValue, double const& maxValue) +double LKS94Projection::Clip(double const & n, double const & minValue, double const & maxValue) { return qMin(qMax(n, minValue), maxValue); } -double LKS94Projection::GetTileMatrixResolution(int const& zoom) +double LKS94Projection::GetTileMatrixResolution(int const & zoom) { double ret = 0; - switch(zoom) - { + switch (zoom) { case 0: - { - ret = 1587.50317500635; - } - break; + { + ret = 1587.50317500635; + } + break; case 1: - { - ret = 793.751587503175; - } - break; + { + ret = 793.751587503175; + } + break; case 2: - { - ret = 529.167725002117; - } - break; + { + ret = 529.167725002117; + } + break; case 3: - { - ret = 264.583862501058; - } - break; + { + ret = 264.583862501058; + } + break; case 4: - { - ret = 132.291931250529; - } - break; + { + ret = 132.291931250529; + } + break; case 5: - { - ret = 52.9167725002117; - } - break; + { + ret = 52.9167725002117; + } + break; case 6: - { - ret = 26.4583862501058; - } - break; + { + ret = 26.4583862501058; + } + break; case 7: - { - ret = 13.2291931250529; - } - break; + { + ret = 13.2291931250529; + } + break; case 8: - { - ret = 6.61459656252646; - } - break; + { + ret = 6.61459656252646; + } + break; case 9: - { - ret = 2.64583862501058; - } - break; + { + ret = 2.64583862501058; + } + break; case 10: - { - ret = 1.32291931250529; - } - break; + { + ret = 1.32291931250529; + } + break; case 11: - { - ret = 0.529167725002117; - } - break; - + { + ret = 0.529167725002117; + } + break; } return ret; @@ -617,175 +554,171 @@ double LKS94Projection::GetTileMatrixResolution(int const& zoom) /* * Returns the conversion from pixels to meters */ -double LKS94Projection::GetGroundResolution(int const& zoom, double const& latitude) +double LKS94Projection::GetGroundResolution(int const & zoom, double const & latitude) { Q_UNUSED(zoom); Q_UNUSED(latitude); return GetTileMatrixResolution(zoom); } -Size LKS94Projection::GetTileMatrixMinXY(int const& zoom) +Size LKS94Projection::GetTileMatrixMinXY(int const & zoom) { Size ret; - switch(zoom) - { - + switch (zoom) { case 0: - { - ret = Size(12, 8); - } - break; + { + ret = Size(12, 8); + } + break; case 1: - { - ret = Size(24, 17); - } - break; + { + ret = Size(24, 17); + } + break; case 2: - { - ret = Size(37, 25); - } - break; + { + ret = Size(37, 25); + } + break; case 3: - { - ret = Size(74, 51); - } - break; + { + ret = Size(74, 51); + } + break; case 4: - { - ret = Size(149, 103); - } - break; + { + ret = Size(149, 103); + } + break; case 5: - { - ret = Size(374, 259); - } - break; + { + ret = Size(374, 259); + } + break; case 6: - { - ret = Size(749, 519); - } - break; + { + ret = Size(749, 519); + } + break; case 7: - { - ret = Size(1594, 1100); - } - break; + { + ret = Size(1594, 1100); + } + break; case 8: - { - ret = Size(3188, 2201); - } - break; + { + ret = Size(3188, 2201); + } + break; case 9: - { - ret = Size(7971, 5502); - } - break; + { + ret = Size(7971, 5502); + } + break; case 10: - { - ret = Size(15943, 11005); - } - break; + { + ret = Size(15943, 11005); + } + break; case 11: - { - ret = Size(39858, 27514); - } - break; + { + ret = Size(39858, 27514); + } + break; } return ret; } -Size LKS94Projection::GetTileMatrixMaxXY(int const& zoom) +Size LKS94Projection::GetTileMatrixMaxXY(int const & zoom) { Size ret; - switch(zoom) - { + switch (zoom) { case 0: - { - ret = Size(14, 10); - } - break; + { + ret = Size(14, 10); + } + break; case 1: - { - ret = Size(30, 20); - } - break; + { + ret = Size(30, 20); + } + break; case 2: - { - ret = Size(45, 31); - } - break; + { + ret = Size(45, 31); + } + break; case 3: - { - ret = Size(90, 62); - } - break; + { + ret = Size(90, 62); + } + break; case 4: - { - ret = Size(181, 125); - } - break; + { + ret = Size(181, 125); + } + break; case 5: - { - ret = Size(454, 311); - } - break; + { + ret = Size(454, 311); + } + break; case 6: - { - ret = Size(903, 623); - } - break; + { + ret = Size(903, 623); + } + break; case 7: - { - ret = Size(1718, 1193); - } - break; + { + ret = Size(1718, 1193); + } + break; case 8: - { - ret = Size(3437, 2386); - } - break; + { + ret = Size(3437, 2386); + } + break; case 9: - { - ret = Size(8594, 5966); - } - break; + { + ret = Size(8594, 5966); + } + break; case 10: - { - ret = Size(17189, 11932); - } - break; + { + ret = Size(17189, 11932); + } + break; case 11: - { - ret = Size(42972, 29831); - } - break; + { + ret = Size(42972, 29831); + } + break; } return ret; } - } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h index af230eca7..e7bd633ae 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file lks94projection.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file lks94projection.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef LKS94PROJECTION_H #define LKS94PROJECTION_H #include @@ -32,41 +32,38 @@ namespace projections { -class LKS94Projection:public internals::PureProjection -{ +class LKS94Projection : public internals::PureProjection { public: LKS94Projection(); - double GetTileMatrixResolution(int const& zoom); - virtual QString Type(){return "LKS94Projection";} + double GetTileMatrixResolution(int const & zoom); + virtual QString Type() + { + return "LKS94Projection"; + } virtual Size TileSize() const; virtual double Axis() const; virtual double Flattening() const; - virtual core::Point FromLatLngToPixel(double lat, double lng, int const& zoom); - virtual internals::PointLatLng FromPixelToLatLng(int const& x, int const& y, int const& zoom); - virtual double GetGroundResolution(int const& zoom, double const& latitude); - virtual Size GetTileMatrixMinXY(int const& zoom); - virtual Size GetTileMatrixMaxXY(int const& zoom); + virtual core::Point FromLatLngToPixel(double lat, double lng, int const & zoom); + virtual internals::PointLatLng FromPixelToLatLng(int const & x, int const & y, int const & zoom); + virtual double GetGroundResolution(int const & zoom, double const & latitude); + virtual Size GetTileMatrixMinXY(int const & zoom); + virtual Size GetTileMatrixMaxXY(int const & zoom); private: - const double MinLatitude; - const double MaxLatitude; - const double MinLongitude; - const double MaxLongitude; - const double orignX; - const double orignY; - Size tileSize; - QVector DTM10(const QVector & lonlat); - QVector MTD10(QVector & pnt); - QVector DTM00(QVector & lonlat); - QVector DTM01(QVector & lonlat); - QVector MTD01(QVector & pnt); - QVector MTD11(QVector & p); - double Clip(double const& n, double const& minValue, double const& maxValue); + const double MinLatitude; + const double MaxLatitude; + const double MinLongitude; + const double MaxLongitude; + const double orignX; + const double orignY; + Size tileSize; + QVector DTM10(const QVector & lonlat); + QVector MTD10(QVector & pnt); + QVector DTM00(QVector & lonlat); + QVector DTM01(QVector & lonlat); + QVector MTD01(QVector & pnt); + QVector MTD11(QVector & p); + double Clip(double const & n, double const & minValue, double const & maxValue); }; - } #endif // LKS94PROJECTION_H - - - - diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.cpp index 9f3021672..ad518031c 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.cpp @@ -1,60 +1,59 @@ /** -****************************************************************************** -* -* @file mercatorprojection.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file mercatorprojection.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "mercatorprojection.h" - + namespace projections { -MercatorProjection::MercatorProjection():MinLatitude(-85.05112878), MaxLatitude(85.05112878),MinLongitude(-177), -MaxLongitude(177), tileSize(256, 256) -{ -} +MercatorProjection::MercatorProjection() : MinLatitude(-85.05112878), MaxLatitude(85.05112878), MinLongitude(-177), + MaxLongitude(177), tileSize(256, 256) +{} Point MercatorProjection::FromLatLngToPixel(double lat, double lng, const int &zoom) { - Point ret;// = Point.Empty; + Point ret; // = Point.Empty; lat = Clip(lat, MinLatitude, MaxLatitude); lng = Clip(lng, MinLongitude, MaxLongitude); - double x = (lng + 180) / 360; + double x = (lng + 180) / 360; double sinLatitude = sin(lat * M_PI / 180); - double y = 0.5 - log((1 + sinLatitude) / (1 - sinLatitude)) / (4 * M_PI); + double y = 0.5 - log((1 + sinLatitude) / (1 - sinLatitude)) / (4 * M_PI); - Size s = GetTileMatrixSizePixel(zoom); + Size s = GetTileMatrixSizePixel(zoom); int mapSizeX = s.Width(); int mapSizeY = s.Height(); - ret.SetX((int) Clip(x * mapSizeX + 0.5, 0, mapSizeX - 1)); - ret.SetY((int) Clip(y * mapSizeY + 0.5, 0, mapSizeY - 1)); + ret.SetX((int)Clip(x * mapSizeX + 0.5, 0, mapSizeX - 1)); + ret.SetY((int)Clip(y * mapSizeY + 0.5, 0, mapSizeY - 1)); return ret; } internals::PointLatLng MercatorProjection::FromPixelToLatLng(const int &x, const int &y, const int &zoom) { - internals::PointLatLng ret;// = internals::PointLatLng.Empty; + internals::PointLatLng ret; // = internals::PointLatLng.Empty; Size s = GetTileMatrixSizePixel(zoom); double mapSizeX = s.Width(); @@ -82,13 +81,13 @@ double MercatorProjection::Axis() const } double MercatorProjection::Flattening() const { - return (1.0 / 298.257223563); + return 1.0 / 298.257223563; } Size MercatorProjection::GetTileMatrixMaxXY(const int &zoom) { Q_UNUSED(zoom); int xy = (1 << zoom); - return Size(xy - 1, xy - 1); + return Size(xy - 1, xy - 1); } Size MercatorProjection::GetTileMatrixMinXY(const int &zoom) { diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.h index d36b45c25..06c0b253b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.h @@ -1,55 +1,56 @@ /** -****************************************************************************** -* -* @file mercatorprojection.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file mercatorprojection.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef MERCATORPROJECTION_H #define MERCATORPROJECTION_H #include "../pureprojection.h" namespace projections { - class MercatorProjection:public internals::PureProjection -{ +class MercatorProjection : public internals::PureProjection { public: MercatorProjection(); - virtual QString Type(){return "MercatorProjection";} + virtual QString Type() + { + return "MercatorProjection"; + } virtual Size TileSize() const; virtual double Axis() const; - virtual double Flattening()const; - virtual core::Point FromLatLngToPixel(double lat, double lng, int const& zoom); - virtual internals::PointLatLng FromPixelToLatLng(const int &x,const int &y,const int &zoom); - virtual Size GetTileMatrixMinXY(const int &zoom); - virtual Size GetTileMatrixMaxXY(const int &zoom); + virtual double Flattening() const; + virtual core::Point FromLatLngToPixel(double lat, double lng, int const & zoom); + virtual internals::PointLatLng FromPixelToLatLng(const int &x, const int &y, const int &zoom); + virtual Size GetTileMatrixMinXY(const int &zoom); + virtual Size GetTileMatrixMaxXY(const int &zoom); private: const double MinLatitude; const double MaxLatitude; const double MinLongitude; const double MaxLongitude; - double Clip(double const& n, double const& minValue, double const& maxValue)const; + double Clip(double const & n, double const & minValue, double const & maxValue) const; Size tileSize; }; - } #endif // MERCATORPROJECTION_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.cpp index f2495c1f4..16dd65272 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.cpp @@ -1,38 +1,36 @@ /** -****************************************************************************** -* -* @file mercatorprojectionyandex.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file mercatorprojectionyandex.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "mercatorprojectionyandex.h" - namespace projections { -MercatorProjectionYandex::MercatorProjectionYandex():MinLatitude(-85.05112878), MaxLatitude(85.05112878),MinLongitude(-177), -MaxLongitude(177), RAD_DEG(180 / M_PI),DEG_RAD(M_PI / 180),MathPiDiv4(M_PI / 4),tileSize(256, 256) -{ -} +MercatorProjectionYandex::MercatorProjectionYandex() : MinLatitude(-85.05112878), MaxLatitude(85.05112878), MinLongitude(-177), + MaxLongitude(177), RAD_DEG(180 / M_PI), DEG_RAD(M_PI / 180), MathPiDiv4(M_PI / 4), tileSize(256, 256) +{} Point MercatorProjectionYandex::FromLatLngToPixel(double lat, double lng, const int &zoom) { lat = Clip(lat, MinLatitude, MaxLatitude); @@ -41,44 +39,44 @@ Point MercatorProjectionYandex::FromLatLngToPixel(double lat, double lng, const double rLon = lng * DEG_RAD; // Math.PI / 180; double rLat = lat * DEG_RAD; // Math.PI / 180; - double a = 6378137; - double k = 0.0818191908426; + double a = 6378137; + double k = 0.0818191908426; - double z = tan(MathPiDiv4 + rLat / 2) / pow((tan(MathPiDiv4 + asin(k * sin(rLat)) / 2)), k); - double z1 = pow(2, 23 - zoom); + double z = tan(MathPiDiv4 + rLat / 2) / pow((tan(MathPiDiv4 + asin(k * sin(rLat)) / 2)), k); + double z1 = pow(2, 23 - zoom); - double DX = ((20037508.342789 + a * rLon) * 53.5865938 / z1); - double DY = ((20037508.342789 - a * log(z)) * 53.5865938 / z1); + double DX = ((20037508.342789 + a * rLon) * 53.5865938 / z1); + double DY = ((20037508.342789 - a * log(z)) * 53.5865938 / z1); - Point ret;// = Point.Empty; - ret.SetX((int) DX); - ret.SetY((int) DY); + Point ret; // = Point.Empty; + ret.SetX((int)DX); + ret.SetY((int)DY); return ret; - } internals::PointLatLng MercatorProjectionYandex::FromPixelToLatLng(const int &x, const int &y, const int &zoom) { Size s = GetTileMatrixSizePixel(zoom); - //double mapSizeX = s.Width(); - //double mapSizeY = s.Height(); + // double mapSizeX = s.Width(); + // double mapSizeY = s.Height(); - double a = 6378137; - double c1 = 0.00335655146887969; - double c2 = 0.00000657187271079536; - double c3 = 0.00000001764564338702; - double c4 = 0.00000000005328478445; - double z1 = (23 - zoom); + double a = 6378137; + double c1 = 0.00335655146887969; + double c2 = 0.00000657187271079536; + double c3 = 0.00000001764564338702; + double c4 = 0.00000000005328478445; + double z1 = (23 - zoom); double mercX = (x * pow(2, z1)) / 53.5865938 - 20037508.342789; - double mercY = 20037508.342789 - (y *pow(2, z1)) / 53.5865938; + double mercY = 20037508.342789 - (y * pow(2, z1)) / 53.5865938; - double g = M_PI /2 - 2 *atan(1 / exp(mercY /a)); - double z = g + c1 * sin(2 * g) + c2 * sin(4 * g) + c3 * sin(6 * g) + c4 * sin(8 * g); + double g = M_PI / 2 - 2 * atan(1 / exp(mercY / a)); + double z = g + c1 * sin(2 * g) + c2 * sin(4 * g) + c3 * sin(6 * g) + c4 * sin(8 * g); + + internals::PointLatLng ret; // = internals::PointLatLng.Empty; - internals::PointLatLng ret;// = internals::PointLatLng.Empty; ret.SetLat(z * RAD_DEG); - ret.SetLng (mercX / a * RAD_DEG); + ret.SetLng(mercX / a * RAD_DEG); return ret; } @@ -96,12 +94,13 @@ double MercatorProjectionYandex::Axis() const } double MercatorProjectionYandex::Flattening() const { - return (1.0 / 298.257223563); + return 1.0 / 298.257223563; } Size MercatorProjectionYandex::GetTileMatrixMaxXY(const int &zoom) { int xy = (1 << zoom); - return Size(xy - 1, xy - 1); + + return Size(xy - 1, xy - 1); } Size MercatorProjectionYandex::GetTileMatrixMinXY(const int &zoom) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.h index f7be2c786..425bcfe7a 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.h @@ -1,48 +1,50 @@ /** -****************************************************************************** -* -* @file mercatorprojectionyandex.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file mercatorprojectionyandex.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef MERCATORPROJECTIONYANDEX_H #define MERCATORPROJECTIONYANDEX_H #include "../pureprojection.h" - + namespace projections { - class MercatorProjectionYandex:public internals::PureProjection -{ +class MercatorProjectionYandex : public internals::PureProjection { public: MercatorProjectionYandex(); - virtual QString Type(){return "MercatorProjectionYandex";} + virtual QString Type() + { + return "MercatorProjectionYandex"; + } virtual Size TileSize() const; virtual double Axis() const; - virtual double Flattening()const; - virtual core::Point FromLatLngToPixel(double lat, double lng, int const& zoom); - virtual internals::PointLatLng FromPixelToLatLng(const int &x,const int &y,const int &zoom); - virtual Size GetTileMatrixMinXY(const int &zoom); - virtual Size GetTileMatrixMaxXY(const int &zoom); + virtual double Flattening() const; + virtual core::Point FromLatLngToPixel(double lat, double lng, int const & zoom); + virtual internals::PointLatLng FromPixelToLatLng(const int &x, const int &y, const int &zoom); + virtual Size GetTileMatrixMinXY(const int &zoom); + virtual Size GetTileMatrixMaxXY(const int &zoom); private: const double MinLatitude; const double MaxLatitude; @@ -51,9 +53,8 @@ private: const double RAD_DEG; const double DEG_RAD; const double MathPiDiv4; - double Clip(double const& n, double const& minValue, double const& maxValue)const; + double Clip(double const & n, double const & minValue, double const & maxValue) const; Size tileSize; }; - } #endif // MERCATORPROJECTIONYANDEX_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp index 16b0f0400..30fedd2b7 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp @@ -1,66 +1,63 @@ /** -****************************************************************************** -* -* @file platecarreeprojection.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file platecarreeprojection.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "platecarreeprojection.h" - namespace projections { -PlateCarreeProjection::PlateCarreeProjection():MinLatitude(-85.05112878), MaxLatitude(85.05112878),MinLongitude(-180), -MaxLongitude(180), tileSize(512, 512) -{ -} +PlateCarreeProjection::PlateCarreeProjection() : MinLatitude(-85.05112878), MaxLatitude(85.05112878), MinLongitude(-180), + MaxLongitude(180), tileSize(512, 512) +{} Point PlateCarreeProjection::FromLatLngToPixel(double lat, double lng, const int &zoom) { - Point ret;// = Point.Empty; + Point ret; // = Point.Empty; lat = Clip(lat, MinLatitude, MaxLatitude); lng = Clip(lng, MinLongitude, MaxLongitude); Size s = GetTileMatrixSizePixel(zoom); double mapSizeX = s.Width(); - //double mapSizeY = s.Height(); + // double mapSizeY = s.Height(); - double scale = 360.0 / mapSizeX; + double scale = 360.0 / mapSizeX; - ret.SetY((int) ((90.0 - lat) / scale)); - ret.SetX((int) ((lng + 180.0) / scale)); + ret.SetY((int)((90.0 - lat) / scale)); + ret.SetX((int)((lng + 180.0) / scale)); return ret; - } internals::PointLatLng PlateCarreeProjection::FromPixelToLatLng(const int &x, const int &y, const int &zoom) { - internals::PointLatLng ret;// = internals::PointLatLng.Empty; + internals::PointLatLng ret; // = internals::PointLatLng.Empty; Size s = GetTileMatrixSizePixel(zoom); double mapSizeX = s.Width(); - //double mapSizeY = s.Height(); + // double mapSizeY = s.Height(); - double scale = 360.0 / mapSizeX; + double scale = 360.0 / mapSizeX; ret.SetLat(90 - (y * scale)); ret.SetLng((x * scale) - 180); @@ -81,12 +78,13 @@ double PlateCarreeProjection::Axis() const } double PlateCarreeProjection::Flattening() const { - return (1.0 / 298.257223563); + return 1.0 / 298.257223563; } Size PlateCarreeProjection::GetTileMatrixMaxXY(const int &zoom) { - int y = (int) pow(2, zoom); - return Size((2*y) - 1, y - 1); + int y = (int)pow(2, zoom); + + return Size((2 * y) - 1, y - 1); } Size PlateCarreeProjection::GetTileMatrixMinXY(const int &zoom) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.h index 010cf430a..d65816484 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.h @@ -1,55 +1,57 @@ /** -****************************************************************************** -* -* @file platecarreeprojection.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file platecarreeprojection.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef PLATECARREEPROJECTION_H #define PLATECARREEPROJECTION_H #include "../pureprojection.h" - + namespace projections { -class PlateCarreeProjection:public internals::PureProjection -{ +class PlateCarreeProjection : public internals::PureProjection { public: PlateCarreeProjection(); - virtual QString Type(){return "PlateCarreeProjection";} + virtual QString Type() + { + return "PlateCarreeProjection"; + } virtual Size TileSize() const; virtual double Axis() const; - virtual double Flattening()const; - virtual core::Point FromLatLngToPixel(double lat, double lng, int const& zoom); - virtual internals::PointLatLng FromPixelToLatLng(const int &x,const int &y,const int &zoom); - virtual Size GetTileMatrixMinXY(const int &zoom); - virtual Size GetTileMatrixMaxXY(const int &zoom); + virtual double Flattening() const; + virtual core::Point FromLatLngToPixel(double lat, double lng, int const & zoom); + virtual internals::PointLatLng FromPixelToLatLng(const int &x, const int &y, const int &zoom); + virtual Size GetTileMatrixMinXY(const int &zoom); + virtual Size GetTileMatrixMaxXY(const int &zoom); private: const double MinLatitude; const double MaxLatitude; const double MinLongitude; const double MaxLongitude; - double Clip(double const& n, double const& minValue, double const& maxValue)const; + double Clip(double const & n, double const & minValue, double const & maxValue) const; Size tileSize; }; } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp index e746036c0..3c657aea4 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp @@ -1,64 +1,63 @@ /** -****************************************************************************** -* -* @file platecarreeprojectionpergo.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file platecarreeprojectionpergo.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "platecarreeprojectionpergo.h" - + namespace projections { -PlateCarreeProjectionPergo::PlateCarreeProjectionPergo():MinLatitude(-85.05112878), MaxLatitude(85.05112878),MinLongitude(-180), -MaxLongitude(180), tileSize(256, 256) -{ -} +PlateCarreeProjectionPergo::PlateCarreeProjectionPergo() : MinLatitude(-85.05112878), MaxLatitude(85.05112878), MinLongitude(-180), + MaxLongitude(180), tileSize(256, 256) +{} Point PlateCarreeProjectionPergo::FromLatLngToPixel(double lat, double lng, const int &zoom) { - Point ret;// = Point.Empty; + Point ret; // = Point.Empty; lat = Clip(lat, MinLatitude, MaxLatitude); lng = Clip(lng, MinLongitude, MaxLongitude); Size s = GetTileMatrixSizePixel(zoom); double mapSizeX = s.Width(); - //double mapSizeY = s.Height(); + // double mapSizeY = s.Height(); - double scale = 360.0 / mapSizeX; + double scale = 360.0 / mapSizeX; - ret.SetY((int) ((90.0 - lat) / scale)); - ret.SetX((int) ((lng + 180.0) / scale)); + ret.SetY((int)((90.0 - lat) / scale)); + ret.SetX((int)((lng + 180.0) / scale)); return ret; } internals::PointLatLng PlateCarreeProjectionPergo::FromPixelToLatLng(const int &x, const int &y, const int &zoom) { - internals::PointLatLng ret;// = internals::PointLatLng.Empty; + internals::PointLatLng ret; // = internals::PointLatLng.Empty; Size s = GetTileMatrixSizePixel(zoom); double mapSizeX = s.Width(); - //double mapSizeY = s.Height(); + // double mapSizeY = s.Height(); - double scale = 360.0 / mapSizeX; + double scale = 360.0 / mapSizeX; ret.SetLat(90 - (y * scale)); ret.SetLng((x * scale) - 180); @@ -80,12 +79,13 @@ double PlateCarreeProjectionPergo::Axis() const } double PlateCarreeProjectionPergo::Flattening() const { - return (1.0 / 298.257223563); + return 1.0 / 298.257223563; } Size PlateCarreeProjectionPergo::GetTileMatrixMaxXY(const int &zoom) { - int y = (int) pow(2, zoom); - return Size((2*y) - 1, y - 1); + int y = (int)pow(2, zoom); + + return Size((2 * y) - 1, y - 1); } Size PlateCarreeProjectionPergo::GetTileMatrixMinXY(const int &zoom) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.h index f5f80c316..baa6811b3 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.h @@ -1,55 +1,57 @@ /** -****************************************************************************** -* -* @file platecarreeprojectionpergo.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file platecarreeprojectionpergo.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef PLATECARREEPROJECTIONPERGO_H #define PLATECARREEPROJECTIONPERGO_H #include "../pureprojection.h" - + namespace projections { -class PlateCarreeProjectionPergo:public internals::PureProjection -{ +class PlateCarreeProjectionPergo : public internals::PureProjection { public: PlateCarreeProjectionPergo(); - virtual QString Type(){return "PlateCarreeProjectionPergo";} + virtual QString Type() + { + return "PlateCarreeProjectionPergo"; + } virtual Size TileSize() const; virtual double Axis() const; - virtual double Flattening()const; - virtual core::Point FromLatLngToPixel(double lat, double lng, int const& zoom); - virtual internals::PointLatLng FromPixelToLatLng(const int &x,const int &y,const int &zoom); - virtual Size GetTileMatrixMinXY(const int &zoom); - virtual Size GetTileMatrixMaxXY(const int &zoom); + virtual double Flattening() const; + virtual core::Point FromLatLngToPixel(double lat, double lng, int const & zoom); + virtual internals::PointLatLng FromPixelToLatLng(const int &x, const int &y, const int &zoom); + virtual Size GetTileMatrixMinXY(const int &zoom); + virtual Size GetTileMatrixMaxXY(const int &zoom); private: const double MinLatitude; const double MaxLatitude; const double MinLongitude; const double MaxLongitude; - double Clip(double const& n, double const& minValue, double const& maxValue)const; + double Clip(double const & n, double const & minValue, double const & maxValue) const; Size tileSize; }; } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.cpp index 2eab113a1..6762241d8 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.cpp @@ -1,277 +1,264 @@ /** -****************************************************************************** -* -* @file pureprojection.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file pureprojection.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "pureprojection.h" - - - - namespace internals { - -const double PureProjection::PI = M_PI; +const double PureProjection::PI = M_PI; const double PureProjection::HALF_PI = (M_PI * 0.5); -const double PureProjection::TWO_PI= (M_PI * 2.0); -const double PureProjection::EPSLoN= 1.0e-10; -const double PureProjection::MAX_VAL= 4; -const double PureProjection::MAXLONG= 2147483647; -const double PureProjection::DBLLONG= 4.61168601e18; -const double PureProjection::R2D=180/M_PI; -const double PureProjection::D2R=M_PI/180; - -Point PureProjection::FromLatLngToPixel(const PointLatLng &p,const int &zoom) - { - return FromLatLngToPixel(p.Lat(), p.Lng(), zoom); - } - - - PointLatLng PureProjection::FromPixelToLatLng(const Point &p,const int &zoom) - { - return FromPixelToLatLng(p.X(), p.Y(), zoom); - } - - Point PureProjection::FromPixelToTileXY(const Point &p) - { - return Point((int) (p.X() / TileSize().Width()), (int) (p.Y() / TileSize().Height())); - } - - Point PureProjection::FromTileXYToPixel(const Point &p) - { - return Point((p.X() * TileSize().Width()), (p.Y() * TileSize().Height())); - } - - Size PureProjection::GetTileMatrixSizeXY(const int &zoom) - { - Size sMin = GetTileMatrixMinXY(zoom); - Size sMax = GetTileMatrixMaxXY(zoom); - - return Size(sMax.Width() - sMin.Width() + 1, sMax.Height() - sMin.Height() + 1); - } - int PureProjection::GetTileMatrixItemCount(const int &zoom) - { - Size s = GetTileMatrixSizeXY(zoom); - return (s.Width() * s.Height()); - } - Size PureProjection::GetTileMatrixSizePixel(const int &zoom) - { - Size s = GetTileMatrixSizeXY(zoom); - return Size(s.Width() * TileSize().Width(), s.Height() * TileSize().Height()); - } - QList PureProjection::GetAreaTileList(const RectLatLng &rect,const int &zoom,const int &padding) - { - QList ret; - - Point topLeft = FromPixelToTileXY(FromLatLngToPixel(rect.LocationTopLeft(), zoom)); - Point rightBottom = FromPixelToTileXY(FromLatLngToPixel(rect.Bottom(), rect.Right(), zoom)); - - for(int x = (topLeft.X() - padding); x <= (rightBottom.X() + padding); x++) - { - for(int y = (topLeft.Y() - padding); y <= (rightBottom.Y() + padding); y++) - { - Point p = Point(x, y); - if(!ret.contains(p) && p.X() >= 0 && p.Y() >= 0) - { - ret.append(p); - } - } - } - //ret.TrimExcess(); - - return ret; - } - /* - * Returns the conversion from pixels to meters - */ - double PureProjection::GetGroundResolution(const int &zoom,const double &latitude) - { - return (cos(latitude * (PI / 180)) * 2 * PI * Axis()) / GetTileMatrixSizePixel(zoom).Width(); - } - - double PureProjection::Sign(const double &x) - { - if(x < 0.0) - return (-1); - else - return (1); - } - - double PureProjection::AdjustLongitude(double x) - { - qlonglong count = 0; - while(true) - { - if(qAbs(x) <= PI) - break; - else - if(((qlonglong) qAbs(x / PI)) < 2) - x = x - (Sign(x) * TWO_PI); - - else - if(((qlonglong) qAbs(x / TWO_PI)) < MAXLONG) - { - x = x - (((qlonglong) (x / TWO_PI)) * TWO_PI); - } - else - if(((qlonglong) qAbs(x / (MAXLONG * TWO_PI))) < MAXLONG) - { - x = x - (((qlonglong) (x / (MAXLONG * TWO_PI))) * (TWO_PI * MAXLONG)); - } - else - if(((qlonglong) qAbs(x / (DBLLONG * TWO_PI))) < MAXLONG) - { - x = x - (((qlonglong) (x / (DBLLONG * TWO_PI))) * (TWO_PI * DBLLONG)); - } - else - x = x - (Sign(x) * TWO_PI); - count++; - if(count > MAX_VAL) - break; - } - return (x); - } - - void PureProjection::SinCos(const double &val, double &si, double &co) - { - si = sin(val); - co = cos(val); - } - - double PureProjection::e0fn(const double &x) - { - return (1.0 - 0.25 * x * (1.0 + x / 16.0 * (3.0 + 1.25 * x))); - } - - double PureProjection::e1fn(const double &x) - { - return (0.375 * x * (1.0 + 0.25 * x * (1.0 + 0.46875 * x))); - } - - double PureProjection::e2fn(const double &x) - { - return (0.05859375 * x * x * (1.0 + 0.75 * x)); - } - - double PureProjection::e3fn(const double &x) - { - return (x * x * x * (35.0 / 3072.0)); - } - - double PureProjection::mlfn(const double &e0,const double &e1,const double &e2,const double &e3,const double &phi) - { - return (e0 * phi - e1 * sin(2.0 * phi) + e2 * sin(4.0 * phi) - e3 * sin(6.0 * phi)); - } - - qlonglong PureProjection::GetUTMzone(const double &lon) - { - return ((qlonglong) (((lon + 180.0) / 6.0) + 1.0)); - } - - - void PureProjection::FromGeodeticToCartesian(double Lat,double Lng,double Height, double &X, double &Y, double &Z) - { - Lat = (PI / 180) * Lat; - Lng = (PI / 180) * Lng; - - double B = Axis() * (1.0 - Flattening()); - double ee = 1.0 - (B / Axis()) * (B / Axis()); - double N = (Axis() / sqrt(1.0 - ee * sin(Lat) * sin(Lat))); - - X = (N + Height) * cos(Lat) * cos(Lng); - Y = (N + Height) * cos(Lat) * sin(Lng); - Z = (N * (B / Axis()) * (B / Axis()) + Height) * sin(Lat); - } - void PureProjection::FromCartesianTGeodetic(const double &X,const double &Y,const double &Z, double &Lat, double &Lng) - { - double E = Flattening() * (2.0 - Flattening()); - Lng = atan2(Y, X); - - double P = sqrt(X * X + Y * Y); - double Theta = atan2(Z, (P * (1.0 - Flattening()))); - double st = sin(Theta); - double ct = cos(Theta); - Lat = atan2(Z + E / (1.0 - Flattening()) * Axis() * st * st * st, P - E * Axis() * ct * ct * ct); - - Lat /= (PI / 180); - Lng /= (PI / 180); - } - double PureProjection::courseBetweenLatLng(PointLatLng const& p1,PointLatLng const& p2) - { - - double lon1=p1.Lng()* (M_PI / 180); - double lat1=p1.Lat()* (M_PI / 180); - double lon2=p2.Lng()* (M_PI / 180); - double lat2=p2.Lat()* (M_PI / 180); - - return 2*M_PI-myfmod(atan2(sin(lon1-lon2)*cos(lat2), - cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon1-lon2)), 2*M_PI); - } - - double PureProjection::DistanceBetweenLatLng(PointLatLng const& p1,PointLatLng const& p2) - { - double R = 6371; // km - double lat1=p1.Lat(); - double lat2=p2.Lat(); - double lon1=p1.Lng(); - double lon2=p2.Lng(); - double dLat = (lat2-lat1)* (PI / 180); - double dLon = (lon2-lon1)* (PI / 180); - double a = sin(dLat/2) * sin(dLat/2) + cos(lat1* (PI / 180)) * cos(lat2* (PI / 180)) * sin(dLon/2) * sin(dLon/2); - double c = 2 * atan2(sqrt(a), sqrt(1-a)); - double d = R * c; - return d; - } - - void PureProjection::offSetFromLatLngs(PointLatLng p1,PointLatLng p2,double &distance,double &bearing) - { - distance=DistanceBetweenLatLng(p1,p2)*1000; - bearing=courseBetweenLatLng(p1,p2); - } - - double PureProjection::myfmod(double x,double y) - { - return x - y*floor(x/y); - } - - PointLatLng PureProjection::translate(PointLatLng p1,double distance,double bearing) - { - PointLatLng ret; - double d=distance; - double tc=bearing; - double lat1=p1.Lat()*M_PI/180; - double lon1=p1.Lng()*M_PI/180; - double R=6378137; - double lat2 = asin(sin(lat1)*cos(d/R) + cos(lat1)*sin(d/R)*cos(tc) ); - double lon2 = lon1 + atan2(sin(tc)*sin(d/R)*cos(lat1), - cos(d/R)-sin(lat1)*sin(lat2)); - lat2=lat2*180/M_PI; - lon2=lon2*180/M_PI; - ret.SetLat(lat2); - ret.SetLng(lon2); - return ret; - } +const double PureProjection::TWO_PI = (M_PI * 2.0); +const double PureProjection::EPSLoN = 1.0e-10; +const double PureProjection::MAX_VAL = 4; +const double PureProjection::MAXLONG = 2147483647; +const double PureProjection::DBLLONG = 4.61168601e18; +const double PureProjection::R2D = 180 / M_PI; +const double PureProjection::D2R = M_PI / 180; +Point PureProjection::FromLatLngToPixel(const PointLatLng &p, const int &zoom) +{ + return FromLatLngToPixel(p.Lat(), p.Lng(), zoom); +} + + +PointLatLng PureProjection::FromPixelToLatLng(const Point &p, const int &zoom) +{ + return FromPixelToLatLng(p.X(), p.Y(), zoom); +} + +Point PureProjection::FromPixelToTileXY(const Point &p) +{ + return Point((int)(p.X() / TileSize().Width()), (int)(p.Y() / TileSize().Height())); +} + +Point PureProjection::FromTileXYToPixel(const Point &p) +{ + return Point((p.X() * TileSize().Width()), (p.Y() * TileSize().Height())); +} + +Size PureProjection::GetTileMatrixSizeXY(const int &zoom) +{ + Size sMin = GetTileMatrixMinXY(zoom); + Size sMax = GetTileMatrixMaxXY(zoom); + + return Size(sMax.Width() - sMin.Width() + 1, sMax.Height() - sMin.Height() + 1); +} +int PureProjection::GetTileMatrixItemCount(const int &zoom) +{ + Size s = GetTileMatrixSizeXY(zoom); + + return s.Width() * s.Height(); +} +Size PureProjection::GetTileMatrixSizePixel(const int &zoom) +{ + Size s = GetTileMatrixSizeXY(zoom); + + return Size(s.Width() * TileSize().Width(), s.Height() * TileSize().Height()); +} +QList PureProjection::GetAreaTileList(const RectLatLng &rect, const int &zoom, const int &padding) +{ + QList ret; + + Point topLeft = FromPixelToTileXY(FromLatLngToPixel(rect.LocationTopLeft(), zoom)); + Point rightBottom = FromPixelToTileXY(FromLatLngToPixel(rect.Bottom(), rect.Right(), zoom)); + + for (int x = (topLeft.X() - padding); x <= (rightBottom.X() + padding); x++) { + for (int y = (topLeft.Y() - padding); y <= (rightBottom.Y() + padding); y++) { + Point p = Point(x, y); + if (!ret.contains(p) && p.X() >= 0 && p.Y() >= 0) { + ret.append(p); + } + } + } + // ret.TrimExcess(); + + return ret; +} +/* + * Returns the conversion from pixels to meters + */ +double PureProjection::GetGroundResolution(const int &zoom, const double &latitude) +{ + return (cos(latitude * (PI / 180)) * 2 * PI * Axis()) / GetTileMatrixSizePixel(zoom).Width(); +} + +double PureProjection::Sign(const double &x) +{ + if (x < 0.0) { + return -1; + } else { + return 1; + } +} + +double PureProjection::AdjustLongitude(double x) +{ + qlonglong count = 0; + + while (true) { + if (qAbs(x) <= PI) { + break; + } else if (((qlonglong)qAbs(x / PI)) < 2) { + x = x - (Sign(x) * TWO_PI); + } else if (((qlonglong)qAbs(x / TWO_PI)) < MAXLONG) { + x = x - (((qlonglong)(x / TWO_PI)) * TWO_PI); + } else if (((qlonglong)qAbs(x / (MAXLONG * TWO_PI))) < MAXLONG) { + x = x - (((qlonglong)(x / (MAXLONG * TWO_PI))) * (TWO_PI * MAXLONG)); + } else if (((qlonglong)qAbs(x / (DBLLONG * TWO_PI))) < MAXLONG) { + x = x - (((qlonglong)(x / (DBLLONG * TWO_PI))) * (TWO_PI * DBLLONG)); + } else { + x = x - (Sign(x) * TWO_PI); + } + count++; + if (count > MAX_VAL) { + break; + } + } + return x; +} + +void PureProjection::SinCos(const double &val, double &si, double &co) +{ + si = sin(val); + co = cos(val); +} + +double PureProjection::e0fn(const double &x) +{ + return 1.0 - 0.25 * x * (1.0 + x / 16.0 * (3.0 + 1.25 * x)); +} + +double PureProjection::e1fn(const double &x) +{ + return 0.375 * x * (1.0 + 0.25 * x * (1.0 + 0.46875 * x)); +} + +double PureProjection::e2fn(const double &x) +{ + return 0.05859375 * x * x * (1.0 + 0.75 * x); +} + +double PureProjection::e3fn(const double &x) +{ + return x * x * x * (35.0 / 3072.0); +} + +double PureProjection::mlfn(const double &e0, const double &e1, const double &e2, const double &e3, const double &phi) +{ + return e0 * phi - e1 * sin(2.0 * phi) + e2 * sin(4.0 * phi) - e3 * sin(6.0 * phi); +} + +qlonglong PureProjection::GetUTMzone(const double &lon) +{ + return (qlonglong)(((lon + 180.0) / 6.0) + 1.0); +} + + +void PureProjection::FromGeodeticToCartesian(double Lat, double Lng, double Height, double &X, double &Y, double &Z) +{ + Lat = (PI / 180) * Lat; + Lng = (PI / 180) * Lng; + + double B = Axis() * (1.0 - Flattening()); + double ee = 1.0 - (B / Axis()) * (B / Axis()); + double N = (Axis() / sqrt(1.0 - ee * sin(Lat) * sin(Lat))); + + X = (N + Height) * cos(Lat) * cos(Lng); + Y = (N + Height) * cos(Lat) * sin(Lng); + Z = (N * (B / Axis()) * (B / Axis()) + Height) * sin(Lat); +} +void PureProjection::FromCartesianTGeodetic(const double &X, const double &Y, const double &Z, double &Lat, double &Lng) +{ + double E = Flattening() * (2.0 - Flattening()); + + Lng = atan2(Y, X); + + double P = sqrt(X * X + Y * Y); + double Theta = atan2(Z, (P * (1.0 - Flattening()))); + double st = sin(Theta); + double ct = cos(Theta); + Lat = atan2(Z + E / (1.0 - Flattening()) * Axis() * st * st * st, P - E * Axis() * ct * ct * ct); + + Lat /= (PI / 180); + Lng /= (PI / 180); +} +double PureProjection::courseBetweenLatLng(PointLatLng const & p1, PointLatLng const & p2) +{ + double lon1 = p1.Lng() * (M_PI / 180); + double lat1 = p1.Lat() * (M_PI / 180); + double lon2 = p2.Lng() * (M_PI / 180); + double lat2 = p2.Lat() * (M_PI / 180); + + return 2 * M_PI - myfmod(atan2(sin(lon1 - lon2) * cos(lat2), + cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon1 - lon2)), 2 * M_PI); +} + +double PureProjection::DistanceBetweenLatLng(PointLatLng const & p1, PointLatLng const & p2) +{ + double R = 6371; // km + double lat1 = p1.Lat(); + double lat2 = p2.Lat(); + double lon1 = p1.Lng(); + double lon2 = p2.Lng(); + double dLat = (lat2 - lat1) * (PI / 180); + double dLon = (lon2 - lon1) * (PI / 180); + double a = sin(dLat / 2) * sin(dLat / 2) + cos(lat1 * (PI / 180)) * cos(lat2 * (PI / 180)) * sin(dLon / 2) * sin(dLon / 2); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + double d = R * c; + + return d; +} + +void PureProjection::offSetFromLatLngs(PointLatLng p1, PointLatLng p2, double &distance, double &bearing) +{ + distance = DistanceBetweenLatLng(p1, p2) * 1000; + bearing = courseBetweenLatLng(p1, p2); +} + +double PureProjection::myfmod(double x, double y) +{ + return x - y * floor(x / y); +} + +PointLatLng PureProjection::translate(PointLatLng p1, double distance, double bearing) +{ + PointLatLng ret; + double d = distance; + double tc = bearing; + double lat1 = p1.Lat() * M_PI / 180; + double lon1 = p1.Lng() * M_PI / 180; + double R = 6378137; + double lat2 = asin(sin(lat1) * cos(d / R) + cos(lat1) * sin(d / R) * cos(tc)); + double lon2 = lon1 + atan2(sin(tc) * sin(d / R) * cos(lat1), + cos(d / R) - sin(lat1) * sin(lat2)); + + lat2 = lat2 * 180 / M_PI; + lon2 = lon2 * 180 / M_PI; + ret.SetLat(lat2); + ret.SetLng(lon2); + return ret; +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.h index 6c41db846..7ebc11d1a 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file pureprojection.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file pureprojection.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef PUREPROJECTION_H #define PUREPROJECTION_H @@ -36,56 +36,54 @@ #include using namespace core; -namespace internals -{ - -class PureProjection -{ - - +namespace internals { +class PureProjection { public: - virtual Size TileSize()const=0; + virtual Size TileSize() const = 0; - virtual double Axis()const=0; + virtual double Axis() const = 0; - virtual double Flattening()const=0; + virtual double Flattening() const = 0; - virtual core::Point FromLatLngToPixel(double lat, double lng, int const& zoom)=0; + virtual core::Point FromLatLngToPixel(double lat, double lng, int const & zoom) = 0; - virtual PointLatLng FromPixelToLatLng(const int &x,const int &y,const int &zoom)=0; + virtual PointLatLng FromPixelToLatLng(const int &x, const int &y, const int &zoom) = 0; - virtual QString Type(){return "PureProjection";} - core::Point FromLatLngToPixel(const PointLatLng &p,const int &zoom); + virtual QString Type() + { + return "PureProjection"; + } + core::Point FromLatLngToPixel(const PointLatLng &p, const int &zoom); - PointLatLng FromPixelToLatLng(const Point &p,const int &zoom); + PointLatLng FromPixelToLatLng(const Point &p, const int &zoom); virtual core::Point FromPixelToTileXY(const core::Point &p); virtual core::Point FromTileXYToPixel(const core::Point &p); - virtual Size GetTileMatrixMinXY(const int &zoom)=0; - virtual Size GetTileMatrixMaxXY(const int &zoom)=0; + virtual Size GetTileMatrixMinXY(const int &zoom) = 0; + virtual Size GetTileMatrixMaxXY(const int &zoom) = 0; virtual Size GetTileMatrixSizeXY(const int &zoom); int GetTileMatrixItemCount(const int &zoom); virtual Size GetTileMatrixSizePixel(const int &zoom); - QList GetAreaTileList(const RectLatLng &rect,const int &zoom,const int &padding); - virtual double GetGroundResolution(const int &zoom,const double &latitude); + QList GetAreaTileList(const RectLatLng &rect, const int &zoom, const int &padding); + virtual double GetGroundResolution(const int &zoom, const double &latitude); - double DegreesToRadians(const double °)const + double DegreesToRadians(const double °) const { - return (D2R * deg); + return D2R * deg; } - double RadiansToDegrees(const double &rad)const + double RadiansToDegrees(const double &rad) const { - return (R2D * rad); + return R2D * rad; } - void FromGeodeticToCartesian(double Lat,double Lng,double Height, double &X, double &Y, double &Z); - void FromCartesianTGeodetic(const double &X,const double &Y,const double &Z, double &Lat, double &Lng); - static double DistanceBetweenLatLng(PointLatLng const& p1,PointLatLng const& p2); + void FromGeodeticToCartesian(double Lat, double Lng, double Height, double &X, double &Y, double &Z); + void FromCartesianTGeodetic(const double &X, const double &Y, const double &Z, double &Lat, double &Lng); + static double DistanceBetweenLatLng(PointLatLng const & p1, PointLatLng const & p2); PointLatLng translate(PointLatLng p1, double distance, double bearing); double courseBetweenLatLng(const PointLatLng &p1, const PointLatLng &p2); void offSetFromLatLngs(PointLatLng p1, PointLatLng p2, double &dX, double &dY); protected: - + static const double PI; static const double HALF_PI; static const double TWO_PI; @@ -99,12 +97,12 @@ protected: static double Sign(const double &x); static double AdjustLongitude(double x); - static void SinCos(const double &val, double &sin, double &cos); + static void SinCos(const double &val, double &sin, double &cos); static double e0fn(const double &x); static double e1fn(const double &x); static double e2fn(const double &x); static double e3fn(const double &x); - static double mlfn(const double &e0,const double &e1,const double &e2,const double &e3,const double &phi); + static double mlfn(const double &e0, const double &e1, const double &e2, const double &e3, const double &phi); static qlonglong GetUTMzone(const double &lon); private: double myfmod(double x, double y); diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.cpp index 461a914bb..a9f94a809 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.cpp @@ -1,79 +1,78 @@ /** -****************************************************************************** -* -* @file rectangle.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file rectangle.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "rectangle.h" namespace internals { -Rectangle Rectangle::Empty=Rectangle(); +Rectangle Rectangle::Empty = Rectangle(); Rectangle Rectangle::FromLTRB(int left, int top, int right, int bottom) - { - return Rectangle(left, - top, - right - left, - bottom - top); - } +{ + return Rectangle(left, + top, + right - left, + bottom - top); +} Rectangle Rectangle::Inflate(Rectangle rect, int x, int y) - { - Rectangle r = rect; - r.Inflate(x, y); - return r; - } +{ + Rectangle r = rect; + + r.Inflate(x, y); + return r; +} Rectangle Rectangle::Intersect(Rectangle a, Rectangle b) - { - int x1 = std::max(a.X(), b.X()); - int x2 = std::min(a.X() + a.Width(), b.X() + b.Width()); - int y1 = std::max(a.Y(), b.Y()); - int y2 = std::min(a.Y() + a.Height(), b.Y() + b.Height()); +{ + int x1 = std::max(a.X(), b.X()); + int x2 = std::min(a.X() + a.Width(), b.X() + b.Width()); + int y1 = std::max(a.Y(), b.Y()); + int y2 = std::min(a.Y() + a.Height(), b.Y() + b.Height()); - if(x2 >= x1 - && y2 >= y1) - { - - return Rectangle(x1, y1, x2 - x1, y2 - y1); - } - return Rectangle::Empty; - } -Rectangle Rectangle::Union(const Rectangle &a,const Rectangle &b) - { + if (x2 >= x1 + && y2 >= y1) { + return Rectangle(x1, y1, x2 - x1, y2 - y1); + } + return Rectangle::Empty; +} +Rectangle Rectangle::Union(const Rectangle &a, const Rectangle &b) +{ int x1 = std::min(a.x, b.x); int x2 = std::max(a.x + a.width, b.x + b.width); int y1 = std::min(a.y, b.y); int y2 = std::max(a.y + a.height, b.y + b.height); - return Rectangle(x1, y1, x2 - x1, y2 - y1); - } -bool operator==(Rectangle const& lhs,Rectangle const& rhs) + return Rectangle(x1, y1, x2 - x1, y2 - y1); +} +bool operator==(Rectangle const & lhs, Rectangle const & rhs) { - return (lhs.x == rhs.x && lhs.y == rhs.y && lhs.width == rhs.width && lhs.height == rhs.height); + return lhs.x == rhs.x && lhs.y == rhs.y && lhs.width == rhs.width && lhs.height == rhs.height; +} +uint qHash(Rectangle const & rect) +{ + return (int)((quint32)rect.x ^ + (((quint32)rect.y << 13) | ((quint32)rect.y >> 19)) ^ + (((quint32)rect.width << 26) | ((quint32)rect.width >> 6)) ^ + (((quint32)rect.height << 7) | ((quint32)rect.height >> 25))); } -uint qHash(Rectangle const& rect) - { - return (int) ((quint32) rect.x ^ - (((quint32) rect.y << 13) | ((quint32) rect.y >> 19)) ^ - (((quint32) rect.width << 26) | ((quint32) rect.width >> 6)) ^ - (((quint32) rect.height << 7) | ((quint32) rect.height >> 25))); - } } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.h index bc70ea28c..67d0f636b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.h @@ -1,61 +1,62 @@ /** -****************************************************************************** -* -* @file rectangle.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file rectangle.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef RECTANGLE_H #define RECTANGLE_H -//#include +// #include #include "../core/size.h" #include "math.h" using namespace core; -namespace internals -{ -struct Rectangle -{ - - friend uint qHash(Rectangle const& rect); - friend bool operator==(Rectangle const& lhs,Rectangle const& rhs); +namespace internals { +struct Rectangle { + friend uint qHash(Rectangle const & rect); + friend bool operator==(Rectangle const & lhs, Rectangle const & rhs); public: static Rectangle Empty; static Rectangle FromLTRB(int left, int top, int right, int bottom); - Rectangle(){x=0; y=0; width=0; height=0; }; + Rectangle() + { + x = 0; y = 0; width = 0; height = 0; + }; Rectangle(int x, int y, int width, int height) { - this->x = x; - this->y = y; - this->width = width; + this->x = x; + this->y = y; + this->width = width; this->height = height; } Rectangle(core::Point location, core::Size size) { - this->x = location.X(); - this->y = location.Y(); - this->width = size.Width(); + this->x = location.X(); + this->y = location.Y(); + this->width = size.Width(); this->height = size.Height(); } - core::Point GetLocation() { + core::Point GetLocation() + { return core::Point(x, y); } void SetLocation(const core::Point &value) @@ -63,91 +64,132 @@ public: x = value.X(); y = value.Y(); } - int X(){return x;} - int Y(){return y;} - void SetX(const int &value){x=value;} - void SetY(const int &value){y=value;} - int Width(){return width;} - void SetWidth(const int &value){width=value;} - int Height(){return height;} - void SetHeight(const int &value){height=value;} - int Left(){return x;} - int Top(){return y;} - int Right(){return x+width;} - int Bottom(){return y+height;} - bool IsEmpty(){return (height==0 && width==0 && x==0 && y==0);} + int X() + { + return x; + } + int Y() + { + return y; + } + void SetX(const int &value) + { + x = value; + } + void SetY(const int &value) + { + y = value; + } + int Width() + { + return width; + } + void SetWidth(const int &value) + { + width = value; + } + int Height() + { + return height; + } + void SetHeight(const int &value) + { + height = value; + } + int Left() + { + return x; + } + int Top() + { + return y; + } + int Right() + { + return x + width; + } + int Bottom() + { + return y + height; + } + bool IsEmpty() + { + return height == 0 && width == 0 && x == 0 && y == 0; + } bool operator==(const Rectangle &cSource) { - return (cSource.x == x && cSource.y == y && cSource.width == width && cSource.height == height); + return cSource.x == x && cSource.y == y && cSource.width == width && cSource.height == height; } - bool operator!=(const Rectangle &cSource){return !(*this==cSource);} - bool Contains(const int &x,const int &y) + bool operator!=(const Rectangle &cSource) { - return this->x<=x && xx+this->width && this->y<=y && yy+this->height; + return !(*this == cSource); + } + bool Contains(const int &x, const int &y) + { + return this->x <= x && x < this->x + this->width && this->y <= y && y < this->y + this->height; } bool Contains(const core::Point &pt) { - return Contains(pt.X(),pt.Y()); + return Contains(pt.X(), pt.Y()); } bool Contains(const Rectangle &rect) { return (this->x <= rect.x) && - ((rect.x + rect.width) <= (this->x + this->width)) && - (this->y <= rect.y) && - ((rect.y + rect.height) <= (this->y + this->height)); + ((rect.x + rect.width) <= (this->x + this->width)) && + (this->y <= rect.y) && + ((rect.y + rect.height) <= (this->y + this->height)); } - void Inflate(const int &width,const int &height) - { - this->x -= width; - this->y -= height; - this->width += 2*width; - this->height += 2*height; - } + void Inflate(const int &width, const int &height) + { + this->x -= width; + this->y -= height; + this->width += 2 * width; + this->height += 2 * height; + } void Inflate(Size &size) - { - - Inflate(size.Width(), size.Height()); - } + { + Inflate(size.Width(), size.Height()); + } static Rectangle Inflate(Rectangle rect, int x, int y); void Intersect(const Rectangle &rect) - { + { Rectangle result = Rectangle::Intersect(rect, *this); - this->x = result.X(); - this->y = result.Y(); - this->width = result.Width(); - this->height = result.Height(); - } + this->x = result.X(); + this->y = result.Y(); + this->width = result.Width(); + this->height = result.Height(); + } static Rectangle Intersect(Rectangle a, Rectangle b); bool IntersectsWith(const Rectangle &rect) - { - return (rect.x < this->x + this->width) && + { + return (rect.x < this->x + this->width) && (this->x < (rect.x + rect.width)) && (rect.y < this->y + this->height) && (this->y < rect.y + rect.height); - } - static Rectangle Union(const Rectangle &a,const Rectangle &b); + } + static Rectangle Union(const Rectangle &a, const Rectangle &b); void Offset(const core::Point &pos) { Offset(pos.X(), pos.Y()); } - void Offset(const int &x,const int &y) + void Offset(const int &x, const int &y) { this->x += x; this->y += y; } QString ToString() - { + { return "{X=" + QString::number(x) + ",Y=" + QString::number(y) + - ",Width=" + QString::number(width) + - ",Height=" +QString::number(height) +"}"; - } + ",Width=" + QString::number(width) + + ",Height=" + QString::number(height) + "}"; + } private: int x; int y; diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.cpp index 1d8529623..30221f048 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.cpp @@ -1,48 +1,46 @@ /** -****************************************************************************** -* -* @file rectlatlng.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file rectlatlng.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "rectlatlng.h" - namespace internals { -RectLatLng RectLatLng::Empty=RectLatLng(); -uint qHash(RectLatLng const& rect) +RectLatLng RectLatLng::Empty = RectLatLng(); +uint qHash(RectLatLng const & rect) { - return (int) (((((uint) rect.Lng()) ^ ((((uint) rect.Lat()) << 13) | (((uint) rect.Lat()) >> 0x13))) ^ ((((uint) rect.WidthLng()) << 0x1a) | (((uint) rect.WidthLng()) >> 6))) ^ ((((uint) rect.HeightLat()) << 7) | (((uint) rect.HeightLat()) >> 0x19))); + return (int)(((((uint)rect.Lng()) ^ ((((uint)rect.Lat()) << 13) | (((uint)rect.Lat()) >> 0x13))) ^ ((((uint)rect.WidthLng()) << 0x1a) | (((uint)rect.WidthLng()) >> 6))) ^ ((((uint)rect.HeightLat()) << 7) | (((uint)rect.HeightLat()) >> 0x19))); } -bool operator==(RectLatLng const& left,RectLatLng const& right) +bool operator==(RectLatLng const & left, RectLatLng const & right) { - return ((((left.Lng() == right.Lng()) && (left.Lat() == right.Lat())) && (left.WidthLng() == right.WidthLng())) && (left.HeightLat() == right.HeightLat())); + return (((left.Lng() == right.Lng()) && (left.Lat() == right.Lat())) && (left.WidthLng() == right.WidthLng())) && (left.HeightLat() == right.HeightLat()); } -bool operator!=(RectLatLng const& left,RectLatLng const& right) +bool operator!=(RectLatLng const & left, RectLatLng const & right) { return !(left == right); } - } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.h index 1e3f56fa2..730530f5e 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.h @@ -1,61 +1,60 @@ /** -****************************************************************************** -* -* @file rectlatlng.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file rectlatlng.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef RECTLATLNG_H #define RECTLATLNG_H -//#include "pointlatlng.h" +// #include "pointlatlng.h" #include "../internals/pointlatlng.h" #include "math.h" #include #include "sizelatlng.h" - + namespace internals { -struct RectLatLng -{ +struct RectLatLng { public: static RectLatLng Empty; - friend uint qHash(RectLatLng const& rect); - friend bool operator==(RectLatLng const& left,RectLatLng const& right); - friend bool operator!=(RectLatLng const& left,RectLatLng const& right); - RectLatLng(double const& lat, double const& lng, double const& widthLng, double const& heightLat) + friend uint qHash(RectLatLng const & rect); + friend bool operator==(RectLatLng const & left, RectLatLng const & right); + friend bool operator!=(RectLatLng const & left, RectLatLng const & right); + RectLatLng(double const & lat, double const & lng, double const & widthLng, double const & heightLat) { this->lng = lng; this->lat = lat; this->widthLng = widthLng; this->heightLat = heightLat; - isempty=false; + isempty = false; } - RectLatLng(PointLatLng const& location, SizeLatLng const& size) + RectLatLng(PointLatLng const & location, SizeLatLng const & size) { this->lng = location.Lng(); this->lat = location.Lat(); this->widthLng = size.WidthLng(); this->heightLat = size.HeightLat(); - isempty=false; + isempty = false; } RectLatLng() { @@ -63,27 +62,27 @@ public: this->lat = 0; this->widthLng = 0; this->heightLat = 0; - isempty=true; + isempty = true; } - static RectLatLng FromLTRB(double const& lng, double const& lat, double const& rightLng, double const& bottomLat) + static RectLatLng FromLTRB(double const & lng, double const & lat, double const & rightLng, double const & bottomLat) { return RectLatLng(lat, lng, rightLng - lng, lat - bottomLat); } - PointLatLng LocationTopLeft()const + PointLatLng LocationTopLeft() const { - return PointLatLng(this->lat, this->lng); + return PointLatLng(this->lat, this->lng); } - void SetLocationTopLeft(PointLatLng const& value) + void SetLocationTopLeft(PointLatLng const & value) { this->lng = value.Lng(); this->lat = value.Lat(); - isempty=false; + isempty = false; } PointLatLng LocationRightBottom() { + PointLatLng ret = PointLatLng(this->lat, this->lng); - PointLatLng ret = PointLatLng(this->lat, this->lng); ret.Offset(HeightLat(), WidthLng()); return ret; } @@ -91,174 +90,174 @@ public: { return SizeLatLng(this->HeightLat(), this->WidthLng()); } - void SetSize(SizeLatLng const& value) + void SetSize(SizeLatLng const & value) { - this->widthLng = value.WidthLng(); + this->widthLng = value.WidthLng(); this->heightLat = value.HeightLat(); - isempty=false; + isempty = false; } - double Lng()const + double Lng() const { return this->lng; } - void SetLng(double const& value) + void SetLng(double const & value) { this->lng = value; - isempty=false; + isempty = false; } - double Lat()const + double Lat() const { return this->lat; } - void SetLat(double const& value) + void SetLat(double const & value) { this->lat = value; - isempty=false; + isempty = false; } - double WidthLng()const + double WidthLng() const { return this->widthLng; } - void SetWidthLng(double const& value) + void SetWidthLng(double const & value) { this->widthLng = value; - isempty=false; + isempty = false; } - double HeightLat()const + double HeightLat() const { return this->heightLat; } - void SetHeightLat(double const& value) + void SetHeightLat(double const & value) { this->heightLat = value; - isempty=false; + isempty = false; } - double Left()const + double Left() const { return this->Lng(); } - double Top()const + double Top() const { return this->Lat(); } - double Right()const + double Right() const { - return (this->Lng() + this->WidthLng()); + return this->Lng() + this->WidthLng(); } - double Bottom()const + double Bottom() const { - return (this->Lat() - this->HeightLat()); + return this->Lat() - this->HeightLat(); } - bool IsEmpty()const - { + bool IsEmpty() const + { return isempty; } - bool Contains(double const& lat, double const& lng) + bool Contains(double const & lat, double const & lng) { - return ((((this->Lng() <= lng) && (lng < (this->Lng() + this->WidthLng()))) && (this->Lat() >= lat)) && (lat > (this->Lat() - this->HeightLat()))); + return (((this->Lng() <= lng) && (lng < (this->Lng() + this->WidthLng()))) && (this->Lat() >= lat)) && (lat > (this->Lat() - this->HeightLat())); } - bool Contains(PointLatLng const& pt) + bool Contains(PointLatLng const & pt) { return this->Contains(pt.Lat(), pt.Lng()); } - bool Contains(RectLatLng const& rect) + bool Contains(RectLatLng const & rect) { - return ((((this->Lng() <= rect.Lng()) && ((rect.Lng() + rect.WidthLng()) <= (this->Lng() + this->WidthLng()))) && (this->Lat() >= rect.Lat())) && ((rect.Lat() - rect.HeightLat()) >= (this->Lat() - this->HeightLat()))); + return (((this->Lng() <= rect.Lng()) && ((rect.Lng() + rect.WidthLng()) <= (this->Lng() + this->WidthLng()))) && (this->Lat() >= rect.Lat())) && ((rect.Lat() - rect.HeightLat()) >= (this->Lat() - this->HeightLat())); } - void Inflate(double const& lat, double const& lng) + void Inflate(double const & lat, double const & lng) { this->lng -= lng; this->lat += lat; this->widthLng += (double)2 * lng; - this->heightLat +=(double)2 * lat; + this->heightLat += (double)2 * lat; } - void Inflate(SizeLatLng const& size) + void Inflate(SizeLatLng const & size) { this->Inflate(size.HeightLat(), size.WidthLng()); } - static RectLatLng Inflate(RectLatLng const& rect, double const& lat, double const& lng) + static RectLatLng Inflate(RectLatLng const & rect, double const & lat, double const & lng) { RectLatLng ef = rect; + ef.Inflate(lat, lng); return ef; } - void Intersect(RectLatLng const& rect) + void Intersect(RectLatLng const & rect) { RectLatLng ef = Intersect(rect, *this); + this->lng = ef.Lng(); this->lat = ef.Lat(); this->widthLng = ef.WidthLng(); this->heightLat = ef.HeightLat(); } - static RectLatLng Intersect(RectLatLng const& a, RectLatLng const& b) + static RectLatLng Intersect(RectLatLng const & a, RectLatLng const & b) { - double lng = std::max(a.Lng(), b.Lng()); - double num2 = std::min((double) (a.Lng() + a.WidthLng()), (double) (b.Lng() + b.WidthLng())); + double lng = std::max(a.Lng(), b.Lng()); + double num2 = std::min((double)(a.Lng() + a.WidthLng()), (double)(b.Lng() + b.WidthLng())); - double lat = std::max(a.Lat(), b.Lat()); - double num4 = std::min((double) (a.Lat() + a.HeightLat()), (double) (b.Lat() + b.HeightLat())); + double lat = std::max(a.Lat(), b.Lat()); + double num4 = std::min((double)(a.Lat() + a.HeightLat()), (double)(b.Lat() + b.HeightLat())); - if((num2 >= lng) && (num4 >= lat)) - { + if ((num2 >= lng) && (num4 >= lat)) { return RectLatLng(lng, lat, num2 - lng, num4 - lat); } return Empty; } - bool IntersectsWith(RectLatLng const& rect) - { - return ((((rect.Lng() < (this->Lng() + this->WidthLng())) && (this->Lng() < (rect.Lng() + rect.WidthLng()))) && (rect.Lat() < (this->Lat() + this->HeightLat()))) && (this->Lat() < (rect.Lat() + rect.HeightLat()))); - } + bool IntersectsWith(RectLatLng const & rect) + { + return (((rect.Lng() < (this->Lng() + this->WidthLng())) && (this->Lng() < (rect.Lng() + rect.WidthLng()))) && (rect.Lat() < (this->Lat() + this->HeightLat()))) && (this->Lat() < (rect.Lat() + rect.HeightLat())); + } - static RectLatLng Union(RectLatLng const& a, RectLatLng const& b) - { - double lng = std::min(a.Lng(), b.Lng()); - double num2 = std::max((double) (a.Lng() + a.WidthLng()), (double) (b.Lng() + b.WidthLng())); - double lat = std::min(a.Lat(), b.Lat()); - double num4 = std::max((double) (a.Lat() + a.HeightLat()), (double) (b.Lat() + b.HeightLat())); - return RectLatLng(lng, lat, num2 - lng, num4 - lat); - } - void Offset(PointLatLng const& pos) - { - this->Offset(pos.Lat(), pos.Lng()); - } + static RectLatLng Union(RectLatLng const & a, RectLatLng const & b) + { + double lng = std::min(a.Lng(), b.Lng()); + double num2 = std::max((double)(a.Lng() + a.WidthLng()), (double)(b.Lng() + b.WidthLng())); + double lat = std::min(a.Lat(), b.Lat()); + double num4 = std::max((double)(a.Lat() + a.HeightLat()), (double)(b.Lat() + b.HeightLat())); - void Offset(double const& lat, double const& lng) - { - this->lng += lng; - this->lat -= lat; - } + return RectLatLng(lng, lat, num2 - lng, num4 - lat); + } + void Offset(PointLatLng const & pos) + { + this->Offset(pos.Lat(), pos.Lng()); + } - QString ToString() const - { - return ("{Lat=" + QString::number(this->Lat()) + ",Lng=" + QString::number(this->Lng()) + ",WidthLng=" + QString::number(this->WidthLng()) + ",HeightLat=" + QString::number(this->HeightLat()) + "}"); - } + void Offset(double const & lat, double const & lng) + { + this->lng += lng; + this->lat -= lat; + } + + QString ToString() const + { + return "{Lat=" + QString::number(this->Lat()) + ",Lng=" + QString::number(this->Lng()) + ",WidthLng=" + QString::number(this->WidthLng()) + ",HeightLat=" + QString::number(this->HeightLat()) + "}"; + } private: double lng; double lat; double widthLng; double heightLat; - bool isempty; + bool isempty; }; - } #endif // RECTLATLNG_H - -// static RectLatLng() -// { -// Empty = new RectLatLng(); -// } -// } +// static RectLatLng() +// { +// Empty = new RectLatLng(); +// } +// } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.cpp index f66b7a104..d1163ac6d 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.cpp @@ -1,60 +1,58 @@ /** -****************************************************************************** -* -* @file sizelatlng.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file sizelatlng.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "sizelatlng.h" #include "pointlatlng.h" - -namespace internals { -SizeLatLng::SizeLatLng():heightLat(0),widthLng(0) -{ -} -SizeLatLng::SizeLatLng(PointLatLng const& pt) +namespace internals { +SizeLatLng::SizeLatLng() : heightLat(0), widthLng(0) +{} +SizeLatLng::SizeLatLng(PointLatLng const & pt) { - this->heightLat = pt.Lat(); - this->widthLng = pt.Lng(); + this->heightLat = pt.Lat(); + this->widthLng = pt.Lng(); } -SizeLatLng operator+(SizeLatLng const& sz1, SizeLatLng const& sz2) +SizeLatLng operator+(SizeLatLng const & sz1, SizeLatLng const & sz2) { return SizeLatLng::Add(sz1, sz2); } -SizeLatLng operator-(SizeLatLng const& sz1, SizeLatLng const& sz2) +SizeLatLng operator-(SizeLatLng const & sz1, SizeLatLng const & sz2) { - return SizeLatLng::Subtract(sz1, sz2); + return SizeLatLng::Subtract(sz1, sz2); } -bool operator==(SizeLatLng const& sz1, SizeLatLng const& sz2) +bool operator==(SizeLatLng const & sz1, SizeLatLng const & sz2) { - return ((sz1.WidthLng() == sz2.WidthLng()) && (sz1.HeightLat() == sz2.HeightLat())); + return (sz1.WidthLng() == sz2.WidthLng()) && (sz1.HeightLat() == sz2.HeightLat()); } -bool operator!=(SizeLatLng const& sz1, SizeLatLng const& sz2) +bool operator!=(SizeLatLng const & sz1, SizeLatLng const & sz2) { - return !(sz1 == sz2); + return !(sz1 == sz2); } -SizeLatLng SizeLatLng::Empty=SizeLatLng(); +SizeLatLng SizeLatLng::Empty = SizeLatLng(); } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.h index 7a4463567..e7897cfab 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.h @@ -1,128 +1,127 @@ /** -****************************************************************************** -* -* @file sizelatlng.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file sizelatlng.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef SIZELATLNG_H #define SIZELATLNG_H #include - + namespace internals { struct PointLatLng; -struct SizeLatLng -{ +struct SizeLatLng { public: SizeLatLng(); - static SizeLatLng Empty; + static SizeLatLng Empty; - SizeLatLng(SizeLatLng const& size) + SizeLatLng(SizeLatLng const & size) { - this->widthLng = size.widthLng; - this->heightLat = size.heightLat; + this->widthLng = size.widthLng; + this->heightLat = size.heightLat; } - SizeLatLng(PointLatLng const& pt); + SizeLatLng(PointLatLng const & pt); - SizeLatLng(double const& heightLat, double const& widthLng) + SizeLatLng(double const & heightLat, double const & widthLng) { - this->heightLat = heightLat; - this->widthLng = widthLng; + this->heightLat = heightLat; + this->widthLng = widthLng; } - friend SizeLatLng operator+(SizeLatLng const& sz1, SizeLatLng const& sz2); - friend SizeLatLng operator-(SizeLatLng const& sz1, SizeLatLng const& sz2); - friend bool operator==(SizeLatLng const& sz1, SizeLatLng const& sz2); - friend bool operator!=(SizeLatLng const& sz1, SizeLatLng const& sz2); + friend SizeLatLng operator+(SizeLatLng const & sz1, SizeLatLng const & sz2); + friend SizeLatLng operator-(SizeLatLng const & sz1, SizeLatLng const & sz2); + friend bool operator==(SizeLatLng const & sz1, SizeLatLng const & sz2); + friend bool operator!=(SizeLatLng const & sz1, SizeLatLng const & sz2); -// static explicit operator PointLatLng(SizeLatLng size) -// { -// return new PointLatLng(size.HeightLat(), size.WidthLng()); -// } +// static explicit operator PointLatLng(SizeLatLng size) +// { +// return new PointLatLng(size.HeightLat(), size.WidthLng()); +// } - bool IsEmpty()const - { - return ((this->widthLng == 0) && (this->heightLat == 0)); - } - - double WidthLng()const - { - return this->widthLng; - } - void SetWidthLng(double const& value) - { - this->widthLng = value; - } - - - double HeightLat()const - { - return this->heightLat; - } - void SetHeightLat(double const& value) - { - this->heightLat = value; - } - - static SizeLatLng Add(SizeLatLng const& sz1, SizeLatLng const& sz2) + bool IsEmpty() const { - return SizeLatLng(sz1.HeightLat() + sz2.HeightLat(), sz1.WidthLng() + sz2.WidthLng()); + return (this->widthLng == 0) && (this->heightLat == 0); } - static SizeLatLng Subtract(SizeLatLng const& sz1, SizeLatLng const& sz2) + double WidthLng() const { - return SizeLatLng(sz1.HeightLat() - sz2.HeightLat(), sz1.WidthLng() - sz2.WidthLng()); + return this->widthLng; + } + void SetWidthLng(double const & value) + { + this->widthLng = value; } -// override bool Equals(object obj) -// { -// if(!(obj is SizeLatLng)) -// { -// return false; -// } -// SizeLatLng ef = (SizeLatLng) obj; -// return (((ef.WidthLng == this->WidthLng) && (ef.HeightLat == this->HeightLat)) && ef.GetType().Equals(base.GetType())); -// } -// override int GetHashCode() -// { -// return base.GetHashCode(); -// } - -// PointLatLng ToPointLatLng() -// { -// return (PointLatLng) this; -// } - - QString ToString() + double HeightLat() const { - return ("{WidthLng=" + QString::number(this->widthLng) + ", HeightLng=" + QString::number(this->heightLat) + "}"); + return this->heightLat; + } + void SetHeightLat(double const & value) + { + this->heightLat = value; + } + + static SizeLatLng Add(SizeLatLng const & sz1, SizeLatLng const & sz2) + { + return SizeLatLng(sz1.HeightLat() + sz2.HeightLat(), sz1.WidthLng() + sz2.WidthLng()); + } + + static SizeLatLng Subtract(SizeLatLng const & sz1, SizeLatLng const & sz2) + { + return SizeLatLng(sz1.HeightLat() - sz2.HeightLat(), sz1.WidthLng() - sz2.WidthLng()); + } + +// override bool Equals(object obj) +// { +// if(!(obj is SizeLatLng)) +// { +// return false; +// } +// SizeLatLng ef = (SizeLatLng) obj; +// return (((ef.WidthLng == this->WidthLng) && (ef.HeightLat == this->HeightLat)) && ef.GetType().Equals(base.GetType())); +// } + +// override int GetHashCode() +// { +// return base.GetHashCode(); +// } + +// PointLatLng ToPointLatLng() +// { +// return (PointLatLng) this; +// } + + QString ToString() + { + return "{WidthLng=" + QString::number(this->widthLng) + ", HeightLng=" + QString::number(this->heightLat) + "}"; } @@ -130,7 +129,5 @@ private: double heightLat; double widthLng; }; - } #endif // SIZELATLNG_H - diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.cpp index e63ccc4be..c07ca63b3 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.cpp @@ -1,60 +1,56 @@ /** -****************************************************************************** -* -* @file tile.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file tile.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "tile.h" - + namespace internals { Tile::Tile(int zoom, Point pos) { - this->zoom=zoom; - this->pos=pos; + this->zoom = zoom; + this->pos = pos; } void Tile::Clear() { #ifdef DEBUG_TILE - qDebug()<<"Tile:Clear Overlays"; -#endif //DEBUG_TILE + qDebug() << "Tile:Clear Overlays"; +#endif // DEBUG_TILE mutex.lock(); - foreach(QByteArray img, Overlays) - { + foreach(QByteArray img, Overlays) { img.~QByteArray(); } Overlays.clear(); mutex.unlock(); } -Tile::Tile():zoom(0),pos(0,0) +Tile::Tile() : zoom(0), pos(0, 0) +{} +Tile & Tile::operator =(const Tile &cSource) { - -} -Tile& Tile::operator =(const Tile &cSource) -{ - this->zoom=cSource.zoom; - this->pos=cSource.pos; + this->zoom = cSource.zoom; + this->pos = cSource.pos; return *this; } - } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.h index 0f67ad205..84872ccb8 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file tile.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file tile.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef TILE_H #define TILE_H @@ -34,25 +34,38 @@ #include #include "debugheader.h" using namespace core; -namespace internals -{ -class Tile -{ +namespace internals { +class Tile { public: - Tile(int zoom,core::Point pos); + Tile(int zoom, core::Point pos); Tile(); void Clear(); - int GetZoom(){return zoom;} - core::Point GetPos(){return pos;} - void SetZoom(const int &value){zoom=value;} - void SetPos(const core::Point &value){pos=value;} - Tile& operator= (const Tile &cSource); + int GetZoom() + { + return zoom; + } + core::Point GetPos() + { + return pos; + } + void SetZoom(const int &value) + { + zoom = value; + } + void SetPos(const core::Point &value) + { + pos = value; + } + Tile & operator=(const Tile &cSource); Tile(const Tile &cSource) { - this->zoom=cSource.zoom; - this->pos=cSource.pos; + this->zoom = cSource.zoom; + this->pos = cSource.pos; + } + bool HasValue() + { + return !(zoom == 0); } - bool HasValue(){return !(zoom==0);} QList Overlays; protected: @@ -60,8 +73,6 @@ protected: private: int zoom; core::Point pos; - - }; } #endif // TILE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tilematrix.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tilematrix.cpp index b9ad2b003..8a637d75f 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tilematrix.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tilematrix.cpp @@ -1,148 +1,142 @@ /** -****************************************************************************** -* -* @file tilematrix.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file tilematrix.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "tilematrix.h" - + namespace internals { TileMatrix::TileMatrix() -{ -} +{} void TileMatrix::Clear() { mutex.lock(); - foreach(Tile* t,matrix.values()) - { + foreach(Tile * t, matrix.values()) { delete t; - t=0; + t = 0; } matrix.clear(); mutex.unlock(); } -//void TileMatrix::RebuildToUpperZoom() -//{ -// mutex.lock(); -// QList newtiles; -// foreach(Tile* t,matrix.values()) -// { -// Point point=Point(t->GetPos().X()*2,t->GetPos().Y()*2); -// Tile* tile1=new Tile(t->GetZoom()+1,point); -// Tile* tile2=new Tile(t->GetZoom()+1,Point(point.X()+1,point.Y()+0)); -// Tile* tile3=new Tile(t->GetZoom()+1,Point(point.X()+0,point.Y()+1)); -// Tile* tile4=new Tile(t->GetZoom()+1,Point(point.X()+1,point.Y()+1)); +// void TileMatrix::RebuildToUpperZoom() +// { +// mutex.lock(); +// QList newtiles; +// foreach(Tile* t,matrix.values()) +// { +// Point point=Point(t->GetPos().X()*2,t->GetPos().Y()*2); +// Tile* tile1=new Tile(t->GetZoom()+1,point); +// Tile* tile2=new Tile(t->GetZoom()+1,Point(point.X()+1,point.Y()+0)); +// Tile* tile3=new Tile(t->GetZoom()+1,Point(point.X()+0,point.Y()+1)); +// Tile* tile4=new Tile(t->GetZoom()+1,Point(point.X()+1,point.Y()+1)); // -// foreach(QByteArray arr, t->Overlays) -// { -// QImage ima=QImage::fromData(arr); -// QImage ima1=ima.copy(0,0,ima.width()/2,ima.height()/2); -// QImage ima2=ima.copy(ima.width()/2,0,ima.width()/2,ima.height()/2); -// QImage ima3=ima.copy(0,ima.height()/2,ima.width()/2,ima.height()/2); -// QImage ima4=ima.copy(ima.width()/2,ima.height()/2,ima.width()/2,ima.height()/2); -// QByteArray ba; -// QBuffer buf(&ba); -// ima1.scaled(QSize(ima.width(),ima.height())).save(&buf,"PNG"); -// tile1->Overlays.append(ba); -// QByteArray ba1; -// QBuffer buf1(&ba1); -// ima2.scaled(QSize(ima.width(),ima.height())).save(&buf1,"PNG"); -// tile2->Overlays.append(ba1); -// QByteArray ba2; -// QBuffer buf2(&ba2); -// ima3.scaled(QSize(ima.width(),ima.height())).save(&buf2,"PNG"); -// tile3->Overlays.append(ba2); -// QByteArray ba3; -// QBuffer buf3(&ba3); -// ima4.scaled(QSize(ima.width(),ima.height())).save(&buf3,"PNG"); -// tile4->Overlays.append(ba3); -// newtiles.append(tile1); -// newtiles.append(tile2); -// newtiles.append(tile3); -// newtiles.append(tile4); -// } -// } -// foreach(Tile* t,matrix.values()) -// { -// delete t; -// t=0; -// } -// matrix.clear(); -// foreach(Tile* t,newtiles) -// { -// matrix.insert(t->GetPos(),t); -// } +// foreach(QByteArray arr, t->Overlays) +// { +// QImage ima=QImage::fromData(arr); +// QImage ima1=ima.copy(0,0,ima.width()/2,ima.height()/2); +// QImage ima2=ima.copy(ima.width()/2,0,ima.width()/2,ima.height()/2); +// QImage ima3=ima.copy(0,ima.height()/2,ima.width()/2,ima.height()/2); +// QImage ima4=ima.copy(ima.width()/2,ima.height()/2,ima.width()/2,ima.height()/2); +// QByteArray ba; +// QBuffer buf(&ba); +// ima1.scaled(QSize(ima.width(),ima.height())).save(&buf,"PNG"); +// tile1->Overlays.append(ba); +// QByteArray ba1; +// QBuffer buf1(&ba1); +// ima2.scaled(QSize(ima.width(),ima.height())).save(&buf1,"PNG"); +// tile2->Overlays.append(ba1); +// QByteArray ba2; +// QBuffer buf2(&ba2); +// ima3.scaled(QSize(ima.width(),ima.height())).save(&buf2,"PNG"); +// tile3->Overlays.append(ba2); +// QByteArray ba3; +// QBuffer buf3(&ba3); +// ima4.scaled(QSize(ima.width(),ima.height())).save(&buf3,"PNG"); +// tile4->Overlays.append(ba3); +// newtiles.append(tile1); +// newtiles.append(tile2); +// newtiles.append(tile3); +// newtiles.append(tile4); +// } +// } +// foreach(Tile* t,matrix.values()) +// { +// delete t; +// t=0; +// } +// matrix.clear(); +// foreach(Tile* t,newtiles) +// { +// matrix.insert(t->GetPos(),t); +// } // -// mutex.unlock(); -//} +// mutex.unlock(); +// } void TileMatrix::ClearPointsNotIn(QListlist) { removals.clear(); mutex.lock(); - foreach(Point p, matrix.keys()) - { - if(!list.contains(p)) - { + foreach(Point p, matrix.keys()) { + if (!list.contains(p)) { removals.append(p); } } mutex.unlock(); - foreach(Point p,removals) - { - Tile* t=TileAt(p); - if(t!=0) - { + foreach(Point p, removals) { + Tile *t = TileAt(p); + + if (t != 0) { mutex.lock(); delete t; - t=0; + t = 0; matrix.remove(p); mutex.unlock(); } - } removals.clear(); } -Tile* TileMatrix::TileAt(const Point &p) +Tile *TileMatrix::TileAt(const Point &p) { - #ifdef DEBUG_TILEMATRIX - qDebug()<<"TileMatrix:TileAt:"< namespace internals { -class TileMatrix -{ +class TileMatrix { public: TileMatrix(); void Clear(); void ClearPointsNotIn(QList list); - Tile* TileAt(const core::Point &p); - void SetTileAt(const core::Point &p,Tile* tile); - int count()const{return matrix.count();} - // void RebuildToUpperZoom(); + Tile *TileAt(const core::Point &p); + void SetTileAt(const core::Point &p, Tile *tile); + int count() const + { + return matrix.count(); + } + // void RebuildToUpperZoom(); protected: - QHash matrix; + QHash matrix; QList removals; QMutex mutex; }; - } #endif // TILEMATRIX_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.cpp index 38a855040..28fb01bd6 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.cpp @@ -1,43 +1,42 @@ /** -****************************************************************************** -* -* @file configuration.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A class that centralizes most of the mapcontrol configurations -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file configuration.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A class that centralizes most of the mapcontrol configurations + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "configuration.h" -namespace mapcontrol -{ +namespace mapcontrol { Configuration::Configuration() { - EmptytileBrush = Qt::cyan; - MissingDataFont =QFont ("Times",10,QFont::Bold); - EmptyTileText = "We are sorry, but we don't\nhave imagery at this zoom\nlevel for this region."; + EmptytileBrush = Qt::cyan; + MissingDataFont = QFont("Times", 10, QFont::Bold); + EmptyTileText = "We are sorry, but we don't\nhave imagery at this zoom\nlevel for this region."; EmptyTileBorders = QPen(Qt::white); ScalePen = QPen(Qt::blue); - SelectionPen = QPen(Qt::blue); + SelectionPen = QPen(Qt::blue); DragButton = Qt::LeftButton; } -void Configuration::SetAccessMode(core::AccessMode::Types const& type) +void Configuration::SetAccessMode(core::AccessMode::Types const & type) { core::OPMaps::Instance()->setAccessMode(type); } @@ -45,7 +44,7 @@ core::AccessMode::Types Configuration::AccessMode() { return core::OPMaps::Instance()->GetAccessMode(); } -void Configuration::SetLanguage(core::LanguageType::Types const& type) +void Configuration::SetLanguage(core::LanguageType::Types const & type) { core::OPMaps::Instance()->setLanguage(type); } @@ -53,7 +52,7 @@ core::LanguageType::Types Configuration::Language() { return core::OPMaps::Instance()->GetLanguage(); } -void Configuration::SetUseMemoryCache(bool const& value) +void Configuration::SetUseMemoryCache(bool const & value) { core::OPMaps::Instance()->setUseMemoryCache(value); } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.h index 6c3987654..c1edc793c 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file configuration.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A class that centralizes most of the mapcontrol configurations -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file configuration.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A class that centralizes most of the mapcontrol configurations + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef CONFIGURATION_H #define CONFIGURATION_H @@ -35,152 +35,164 @@ #include "../core/opmaps.h" #include "../core/accessmode.h" #include "../core/cache.h" -namespace mapcontrol -{ - +namespace mapcontrol { /** -* @brief A class that centralizes most of the mapcontrol configurations -* -* @class Configuration configuration.h "configuration.h" -*/ -class Configuration -{ + * @brief A class that centralizes most of the mapcontrol configurations + * + * @class Configuration configuration.h "configuration.h" + */ +class Configuration { public: Configuration(); /** - * @brief Used to draw empty map tiles - * - * @var EmptytileBrush - */ + * @brief Used to draw empty map tiles + * + * @var EmptytileBrush + */ QBrush EmptytileBrush; /** - * @brief Used for empty tiles text - * - * @var EmptyTileText - */ + * @brief Used for empty tiles text + * + * @var EmptyTileText + */ QString EmptyTileText; /** - * @brief Used to draw empty tile borders - * - * @var EmptyTileBorders - */ + * @brief Used to draw empty tile borders + * + * @var EmptyTileBorders + */ QPen EmptyTileBorders; /** - * @brief Used to Draw the maps scale - * - * @var ScalePen - */ + * @brief Used to Draw the maps scale + * + * @var ScalePen + */ QPen ScalePen; /** - * @brief Used to draw selection box - * - * @var SelectionPen - */ + * @brief Used to draw selection box + * + * @var SelectionPen + */ QPen SelectionPen; /** - * @brief - * - * @var MissingDataFont - */ + * @brief + * + * @var MissingDataFont + */ QFont MissingDataFont; /** - * @brief Button used for dragging - * - * @var DragButton - */ + * @brief Button used for dragging + * + * @var DragButton + */ Qt::MouseButton DragButton; /** - * @brief Sets the access mode for the map (cache only, server and cache...) - * - * @param type access mode - */ - void SetAccessMode(core::AccessMode::Types const& type); + * @brief Sets the access mode for the map (cache only, server and cache...) + * + * @param type access mode + */ + void SetAccessMode(core::AccessMode::Types const & type); /** - * @brief Returns the access mode for the map (cache only, server and cache...) - * - * @return core::AccessMode::Types access mode for the map - */ + * @brief Returns the access mode for the map (cache only, server and cache...) + * + * @return core::AccessMode::Types access mode for the map + */ core::AccessMode::Types AccessMode(); /** - * @brief Sets the language used for geocaching - * - * @param type The language to be used - */ - void SetLanguage(core::LanguageType::Types const& type); + * @brief Sets the language used for geocaching + * + * @param type The language to be used + */ + void SetLanguage(core::LanguageType::Types const & type); /** - * @brief Returns the language used for geocaching - * - * @return core::LanguageType::Types - */ + * @brief Returns the language used for geocaching + * + * @return core::LanguageType::Types + */ core::LanguageType::Types Language(); /** - * @brief Used to allow disallow use of memory caching - * - * @param value - * @return - */ - void SetUseMemoryCache(bool const& value); + * @brief Used to allow disallow use of memory caching + * + * @param value + * @return + */ + void SetUseMemoryCache(bool const & value); /** - * @brief Return if memory caching is in use - * - * @return - */ - bool UseMemoryCache(){return core::OPMaps::Instance()->UseMemoryCache();} - - /** - * @brief Returns the currently used memory for tiles - * - * @return - */ - double TileMemoryUsed()const{return core::OPMaps::Instance()->TilesInMemory.MemoryCacheSize();} - - /** - * @brief Sets the size of the memory for tiles - * - * @param value size in Mb to use for tiles - * @return - */ - void SetTileMemorySize(int const& value){core::OPMaps::Instance()->TilesInMemory.setMemoryCacheCapacity(value);} - - /** - * @brief Sets the location for the SQLite Database used for caching and the geocoding cache files - * - * @param dir The path location for the cache file-IMPORTANT Must end with closing slash "/" - */ - void SetCacheLocation(QString const& dir) + * @brief Return if memory caching is in use + * + * @return + */ + bool UseMemoryCache() { - core::Cache::Instance()->setCacheLocation(dir); - + return core::OPMaps::Instance()->UseMemoryCache(); } /** - * @brief Deletes tiles in DataBase older than "days" days - * - * @param days - * @return - */ - void DeleteTilesOlderThan(int const& days){core::Cache::Instance()->ImageCache.deleteOlderTiles(days);} + * @brief Returns the currently used memory for tiles + * + * @return + */ + double TileMemoryUsed() const + { + return core::OPMaps::Instance()->TilesInMemory.MemoryCacheSize(); + } /** - * @brief Exports tiles from one DB to another. Only new tiles are added. - * - * @param sourceDB the source DB - * @param destDB the destination DB. If it doesnt exhist it will be created. - * @return - */ - void ExportMapDataToDB(QString const& sourceDB, QString const& destDB)const{core::PureImageCache::ExportMapDataToDB(sourceDB,destDB);} + * @brief Sets the size of the memory for tiles + * + * @param value size in Mb to use for tiles + * @return + */ + void SetTileMemorySize(int const & value) + { + core::OPMaps::Instance()->TilesInMemory.setMemoryCacheCapacity(value); + } + /** - * @brief Returns the location for the SQLite Database used for caching and the geocoding cache files - * - * @return - */ - QString CacheLocation(){return core::Cache::Instance()->CacheLocation();} + * @brief Sets the location for the SQLite Database used for caching and the geocoding cache files + * + * @param dir The path location for the cache file-IMPORTANT Must end with closing slash "/" + */ + void SetCacheLocation(QString const & dir) + { + core::Cache::Instance()->setCacheLocation(dir); + } + /** + * @brief Deletes tiles in DataBase older than "days" days + * + * @param days + * @return + */ + void DeleteTilesOlderThan(int const & days) + { + core::Cache::Instance()->ImageCache.deleteOlderTiles(days); + } + /** + * @brief Exports tiles from one DB to another. Only new tiles are added. + * + * @param sourceDB the source DB + * @param destDB the destination DB. If it doesnt exhist it will be created. + * @return + */ + void ExportMapDataToDB(QString const & sourceDB, QString const & destDB) const + { + core::PureImageCache::ExportMapDataToDB(sourceDB, destDB); + } + /** + * @brief Returns the location for the SQLite Database used for caching and the geocoding cache files + * + * @return + */ + QString CacheLocation() + { + return core::Cache::Instance()->CacheLocation(); + } }; } #endif // CONFIGURATION_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp index 358ba5ed7..cf6f656e2 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp @@ -1,186 +1,171 @@ /** -****************************************************************************** -* -* @file gpsitem.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a UAV -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file gpsitem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a UAV + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "../internals/pureprojection.h" #include "gpsitem.h" -namespace mapcontrol +namespace mapcontrol { +GPSItem::GPSItem(MapGraphicItem *map, OPMapWidget *parent, QString uavPic) : map(map), mapwidget(parent), showtrail(true), showtrailline(true), trailtime(1), traildistance(2), autosetreached(true) + , autosetdistance(100) { - GPSItem::GPSItem(MapGraphicItem* map,OPMapWidget* parent,QString uavPic):map(map),mapwidget(parent),showtrail(true),showtrailline(true),trailtime(1),traildistance(2),autosetreached(true) - ,autosetdistance(100) - { - pic.load(uavPic); - // Don't scale but trust the image we are given - // pic=pic.scaled(50,33,Qt::IgnoreAspectRatio); - localposition=map->FromLatLngToLocal(mapwidget->CurrentPosition()); - this->setPos(localposition.X(),localposition.Y()); - this->setZValue(4); - trail=new QGraphicsItemGroup(this); - trail->setParentItem(map); - trailLine=new QGraphicsItemGroup(this); - trailLine->setParentItem(map); - this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); - setCacheMode(QGraphicsItem::ItemCoordinateCache); - mapfollowtype=UAVMapFollowType::None; - //trailtype=UAVTrailType::ByDistance; - trailtype=UAVTrailType::ByTimeElapsed; - timer.start(); - connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); - } - GPSItem::~GPSItem() - { + pic.load(uavPic); + // Don't scale but trust the image we are given + // pic=pic.scaled(50,33,Qt::IgnoreAspectRatio); + localposition = map->FromLatLngToLocal(mapwidget->CurrentPosition()); + this->setPos(localposition.X(), localposition.Y()); + this->setZValue(4); + trail = new QGraphicsItemGroup(this); + trail->setParentItem(map); + trailLine = new QGraphicsItemGroup(this); + trailLine->setParentItem(map); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + setCacheMode(QGraphicsItem::ItemCoordinateCache); + mapfollowtype = UAVMapFollowType::None; + // trailtype=UAVTrailType::ByDistance; + trailtype = UAVTrailType::ByTimeElapsed; + timer.start(); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); +} +GPSItem::~GPSItem() +{} +void GPSItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) +{ + Q_UNUSED(option); + Q_UNUSED(widget); + painter->drawPixmap(-pic.width() / 2, -pic.height() / 2, pic); +} +QRectF GPSItem::boundingRect() const +{ + return QRectF(-pic.width() / 2, -pic.height() / 2, pic.width(), pic.height()); +} +void GPSItem::SetUAVPos(const internals::PointLatLng &position, const int &altitude) +{ + if (coord.IsEmpty()) { + lastcoord = coord; } - - void GPSItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) - { - Q_UNUSED(option); - Q_UNUSED(widget); - painter->drawPixmap(-pic.width()/2,-pic.height()/2,pic); - } - QRectF GPSItem::boundingRect()const - { - return QRectF(-pic.width()/2,-pic.height()/2,pic.width(),pic.height()); - } - void GPSItem::SetUAVPos(const internals::PointLatLng &position, const int &altitude) - { - if(coord.IsEmpty()) - lastcoord=coord; - if(coord!=position) - { - - if(trailtype==UAVTrailType::ByTimeElapsed) - { - if(timer.elapsed()>trailtime*1000) - { - TrailItem * ob=new TrailItem(position,altitude,Qt::red,map); - trail->addToGroup(ob); - connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT())); - if(!lasttrailline.IsEmpty()) - { - TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::green,map); - trailLine->addToGroup(obj); - connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot())); - } - lasttrailline=position; - timer.restart(); + if (coord != position) { + if (trailtype == UAVTrailType::ByTimeElapsed) { + if (timer.elapsed() > trailtime * 1000) { + TrailItem *ob = new TrailItem(position, altitude, Qt::red, map); + trail->addToGroup(ob); + connect(this, SIGNAL(setChildPosition()), ob, SLOT(setPosSLOT())); + if (!lasttrailline.IsEmpty()) { + TrailLineItem *obj = new TrailLineItem(lasttrailline, position, Qt::green, map); + trailLine->addToGroup(obj); + connect(this, SIGNAL(setChildLine()), obj, SLOT(setLineSlot())); } - + lasttrailline = position; + timer.restart(); } - else if(trailtype==UAVTrailType::ByDistance) - { - if(qAbs(internals::PureProjection::DistanceBetweenLatLng(lastcoord,position)*1000)>traildistance) - { - TrailItem * ob=new TrailItem(position,altitude,Qt::red,map); - trail->addToGroup(ob); - connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT())); - if(!lasttrailline.IsEmpty()) - { - TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::green,map); - trailLine->addToGroup(obj); - connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot())); - } - lasttrailline=position; - lastcoord=position; + } else if (trailtype == UAVTrailType::ByDistance) { + if (qAbs(internals::PureProjection::DistanceBetweenLatLng(lastcoord, position) * 1000) > traildistance) { + TrailItem *ob = new TrailItem(position, altitude, Qt::red, map); + trail->addToGroup(ob); + connect(this, SIGNAL(setChildPosition()), ob, SLOT(setPosSLOT())); + if (!lasttrailline.IsEmpty()) { + TrailLineItem *obj = new TrailLineItem(lasttrailline, position, Qt::green, map); + trailLine->addToGroup(obj); + connect(this, SIGNAL(setChildLine()), obj, SLOT(setLineSlot())); } + lasttrailline = position; + lastcoord = position; } - coord=position; - this->altitude=altitude; - RefreshPos(); } - } - - /** - * Rotate the UAV Icon on the map, or rotate the map - * depending on the display mode - */ - void GPSItem::SetUAVHeading(const qreal &value) - { - if(mapfollowtype==UAVMapFollowType::CenterAndRotateMap) - { - mapwidget->SetRotate(-value); - } - else { - if (this->rotation() != value) - this->setRotation(value); - } - } - - - int GPSItem::type()const - { - return Type; - } - - - void GPSItem::RefreshPos() - { - localposition=map->FromLatLngToLocal(coord); - this->setPos(localposition.X(),localposition.Y()); - emit setChildPosition(); - emit setChildLine(); - - } - - void GPSItem::setOpacitySlot(qreal opacity) - { - setOpacity(opacity); - } - void GPSItem::SetTrailType(const UAVTrailType::Types &value) - { - trailtype=value; - if(trailtype==UAVTrailType::ByTimeElapsed) - timer.restart(); - } - void GPSItem::SetShowTrail(const bool &value) - { - showtrail=value; - trail->setVisible(value); - - } - void GPSItem::SetShowTrailLine(const bool &value) - { - showtrailline=value; - trailLine->setVisible(value); - } - void GPSItem::DeleteTrail()const - { - foreach(QGraphicsItem* i,trail->childItems()) - delete i; - foreach(QGraphicsItem* i,trailLine->childItems()) - delete i; - } - double GPSItem::Distance3D(const internals::PointLatLng &coord, const int &altitude) - { - return sqrt(pow(internals::PureProjection::DistanceBetweenLatLng(this->coord,coord)*1000,2)+ - pow(this->altitude-altitude,2)); - - } - void GPSItem::SetUavPic(QString UAVPic) - { - pic.load(":/uavs/images/"+UAVPic); + coord = position; + this->altitude = altitude; + RefreshPos(); } } + +/** + * Rotate the UAV Icon on the map, or rotate the map + * depending on the display mode + */ +void GPSItem::SetUAVHeading(const qreal &value) +{ + if (mapfollowtype == UAVMapFollowType::CenterAndRotateMap) { + mapwidget->SetRotate(-value); + } else { + if (this->rotation() != value) { + this->setRotation(value); + } + } +} + + +int GPSItem::type() const +{ + return Type; +} + + +void GPSItem::RefreshPos() +{ + localposition = map->FromLatLngToLocal(coord); + this->setPos(localposition.X(), localposition.Y()); + emit setChildPosition(); + emit setChildLine(); +} + +void GPSItem::setOpacitySlot(qreal opacity) +{ + setOpacity(opacity); +} +void GPSItem::SetTrailType(const UAVTrailType::Types &value) +{ + trailtype = value; + if (trailtype == UAVTrailType::ByTimeElapsed) { + timer.restart(); + } +} +void GPSItem::SetShowTrail(const bool &value) +{ + showtrail = value; + trail->setVisible(value); +} +void GPSItem::SetShowTrailLine(const bool &value) +{ + showtrailline = value; + trailLine->setVisible(value); +} +void GPSItem::DeleteTrail() const +{ + foreach(QGraphicsItem * i, trail->childItems()) + delete i; + foreach(QGraphicsItem * i, trailLine->childItems()) + delete i; +} +double GPSItem::Distance3D(const internals::PointLatLng &coord, const int &altitude) +{ + return sqrt(pow(internals::PureProjection::DistanceBetweenLatLng(this->coord, coord) * 1000, 2) + + pow(this->altitude - altitude, 2)); +} +void GPSItem::SetUavPic(QString UAVPic) +{ + pic.load(":/uavs/images/" + UAVPic); +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.h index c78c734aa..8e760b27e 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file gpsitem.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a WayPoint -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file gpsitem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef GPSITEM_H #define GPSITEM_H @@ -40,190 +40,229 @@ #include "opmapwidget.h" #include "trailitem.h" #include "traillineitem.h" -namespace mapcontrol -{ - class WayPointItem; - class OPMapWidget; +namespace mapcontrol { +class WayPointItem; +class OPMapWidget; +/** + * @brief A QGraphicsItem representing the UAV + * + * @class UAVItem gpsitem.h "mapwidget/gpsitem.h" + */ +class GPSItem : public QObject, public QGraphicsItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) +public: + enum { Type = UserType + 6 }; + GPSItem(MapGraphicItem *map, OPMapWidget *parent, QString uavPic = QString::fromUtf8(":/uavs/images/mapquad.png")); + ~GPSItem(); /** -* @brief A QGraphicsItem representing the UAV -* -* @class UAVItem gpsitem.h "mapwidget/gpsitem.h" -*/ - class GPSItem:public QObject,public QGraphicsItem + * @brief Sets the UAV position + * + * @param position LatLng point + * @param altitude altitude in meters + */ + void SetUAVPos(internals::PointLatLng const & position, int const & altitude); + /** + * @brief Sets the UAV heading + * + * @param value heading angle (north=0deg) + */ + void SetUAVHeading(qreal const & value); + /** + * @brief Returns the UAV position + * + * @return internals::PointLatLng + */ + internals::PointLatLng UAVPos() const { - Q_OBJECT - Q_INTERFACES(QGraphicsItem) - public: - enum { Type = UserType + 6 }; - GPSItem(MapGraphicItem* map,OPMapWidget* parent, QString uavPic=QString::fromUtf8(":/uavs/images/mapquad.png")); - ~GPSItem(); - /** - * @brief Sets the UAV position - * - * @param position LatLng point - * @param altitude altitude in meters - */ - void SetUAVPos(internals::PointLatLng const& position,int const& altitude); - /** - * @brief Sets the UAV heading - * - * @param value heading angle (north=0deg) - */ - void SetUAVHeading(qreal const& value); - /** - * @brief Returns the UAV position - * - * @return internals::PointLatLng - */ - internals::PointLatLng UAVPos()const{return coord;} - /** - * @brief Sets the Map follow type - * - * @param value can be "none"(map doesnt follow UAV), "CenterMap"(map moves to keep UAV centered) or "CenterAndRotateMap"(map moves and rotates to keep UAV centered and straight) - */ - void SetMapFollowType(UAVMapFollowType::Types const& value){mapfollowtype=value;} - /** - * @brief Sets the trail type - * - * @param value can be "NoTrail"(no trail is plotted), "ByTimeElapsed"(a trail point is plotted each TrailTime()) or ByDistance(a trail point is plotted if the distance between the UAV and the last trail point is bigger than TrailDistance()) - */ - void SetTrailType(UAVTrailType::Types const& value); - /** - * @brief Returns the map follow method used - * - * @return UAVMapFollowType::Types - */ - UAVMapFollowType::Types GetMapFollowType()const{return mapfollowtype;} - /** - * @brief Returns the UAV trail type. It can be plotted by time elapsed or distance - * - * @return UAVTrailType::Types - */ - UAVTrailType::Types GetTrailType()const{return trailtype;} + return coord; + } + /** + * @brief Sets the Map follow type + * + * @param value can be "none"(map doesnt follow UAV), "CenterMap"(map moves to keep UAV centered) or "CenterAndRotateMap"(map moves and rotates to keep UAV centered and straight) + */ + void SetMapFollowType(UAVMapFollowType::Types const & value) + { + mapfollowtype = value; + } + /** + * @brief Sets the trail type + * + * @param value can be "NoTrail"(no trail is plotted), "ByTimeElapsed"(a trail point is plotted each TrailTime()) or ByDistance(a trail point is plotted if the distance between the UAV and the last trail point is bigger than TrailDistance()) + */ + void SetTrailType(UAVTrailType::Types const & value); + /** + * @brief Returns the map follow method used + * + * @return UAVMapFollowType::Types + */ + UAVMapFollowType::Types GetMapFollowType() const + { + return mapfollowtype; + } + /** + * @brief Returns the UAV trail type. It can be plotted by time elapsed or distance + * + * @return UAVTrailType::Types + */ + UAVTrailType::Types GetTrailType() const + { + return trailtype; + } - void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, - QWidget *widget); - QRectF boundingRect() const; - /** - * @brief Sets the trail time to be used if TrailType is ByTimeElapsed - * - * @param seconds the UAV trail time elapsed value. If the trail type is time elapsed - * a trail point will be plotted each "value returned" seconds. - */ - void SetTrailTime(int const& seconds){trailtime=seconds;} - /** - * @brief Returns the UAV trail time elapsed value. If the trail type is time elapsed - * a trail point will be plotted each "value returned" seconds. - * - * @return int - */ - int TrailTime()const{return trailtime;} - /** - * @brief Sets the trail distance to be used if TrailType is ByDistance - * - * @param distance the UAV trail plot distance. - * If the trail type is ByDistance a trail dot is plotted if - * the distance between the current UAV position and the last trail point - * is bigger than the returned value - */ - void SetTrailDistance(int const& distance){traildistance=distance;} - /** - * @brief Returns the UAV trail plot distance. - * If the trail type is distance diference a trail dot is plotted if - * the distance between the current UAV position and the last trail point - * is bigger than the returned value - * - * @return int - */ - int TrailDistance()const{return traildistance;} - /** - * @brief Returns true if UAV trail is shown - * - * @return bool - */ - bool ShowTrail()const{return showtrail;} - /** - * @brief Returns true if UAV trail line is shown - * - * @return bool - */ - bool ShowTrailLine()const{return showtrailline;} - /** - * @brief Used to define if the UAV displays a trail - * - * @param value - */ - void SetShowTrail(bool const& value); - /** - * @brief Used to define if the UAV displays a trail line - * - * @param value - */ - void SetShowTrailLine(bool const& value); - /** - * @brief Deletes all the trail points - */ - void DeleteTrail()const; - /** - * @brief Returns true if the UAV automaticaly sets WP reached value (changing its color) - * - * @return bool - */ - bool AutoSetReached()const{return autosetreached;} - /** - * @brief Defines if the UAV can set the WP's "reached" value automaticaly. - * - * @param value - */ - void SetAutoSetReached(bool const& value){autosetreached=value;} - /** - * @brief Returns the 3D distance in meters necessary for the UAV to set WP's to "reached" - * - * @return double - */ - double AutoSetDistance()const{return autosetdistance;} - /** - * @brief Sets the the 3D distance in meters necessary for the UAV to set WP's to "reached" - * - * @param value - */ - void SetAutoSetDistance(double const& value){autosetdistance=value;} + void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, + QWidget *widget); + QRectF boundingRect() const; + /** + * @brief Sets the trail time to be used if TrailType is ByTimeElapsed + * + * @param seconds the UAV trail time elapsed value. If the trail type is time elapsed + * a trail point will be plotted each "value returned" seconds. + */ + void SetTrailTime(int const & seconds) + { + trailtime = seconds; + } + /** + * @brief Returns the UAV trail time elapsed value. If the trail type is time elapsed + * a trail point will be plotted each "value returned" seconds. + * + * @return int + */ + int TrailTime() const + { + return trailtime; + } + /** + * @brief Sets the trail distance to be used if TrailType is ByDistance + * + * @param distance the UAV trail plot distance. + * If the trail type is ByDistance a trail dot is plotted if + * the distance between the current UAV position and the last trail point + * is bigger than the returned value + */ + void SetTrailDistance(int const & distance) + { + traildistance = distance; + } + /** + * @brief Returns the UAV trail plot distance. + * If the trail type is distance diference a trail dot is plotted if + * the distance between the current UAV position and the last trail point + * is bigger than the returned value + * + * @return int + */ + int TrailDistance() const + { + return traildistance; + } + /** + * @brief Returns true if UAV trail is shown + * + * @return bool + */ + bool ShowTrail() const + { + return showtrail; + } + /** + * @brief Returns true if UAV trail line is shown + * + * @return bool + */ + bool ShowTrailLine() const + { + return showtrailline; + } + /** + * @brief Used to define if the UAV displays a trail + * + * @param value + */ + void SetShowTrail(bool const & value); + /** + * @brief Used to define if the UAV displays a trail line + * + * @param value + */ + void SetShowTrailLine(bool const & value); + /** + * @brief Deletes all the trail points + */ + void DeleteTrail() const; + /** + * @brief Returns true if the UAV automaticaly sets WP reached value (changing its color) + * + * @return bool + */ + bool AutoSetReached() const + { + return autosetreached; + } + /** + * @brief Defines if the UAV can set the WP's "reached" value automaticaly. + * + * @param value + */ + void SetAutoSetReached(bool const & value) + { + autosetreached = value; + } + /** + * @brief Returns the 3D distance in meters necessary for the UAV to set WP's to "reached" + * + * @return double + */ + double AutoSetDistance() const + { + return autosetdistance; + } + /** + * @brief Sets the the 3D distance in meters necessary for the UAV to set WP's to "reached" + * + * @param value + */ + void SetAutoSetDistance(double const & value) + { + autosetdistance = value; + } - int type() const; + int type() const; - void SetUavPic(QString UAVPic); - private: - MapGraphicItem* map; + void SetUavPic(QString UAVPic); +private: + MapGraphicItem *map; - int altitude; - UAVMapFollowType::Types mapfollowtype; - UAVTrailType::Types trailtype; - internals::PointLatLng coord; - internals::PointLatLng lastcoord; - QPixmap pic; - core::Point localposition; - OPMapWidget* mapwidget; - QGraphicsItemGroup* trail; - QGraphicsItemGroup * trailLine; - internals::PointLatLng lasttrailline; - QTime timer; - bool showtrail; - bool showtrailline; - int trailtime; - int traildistance; - bool autosetreached; - double Distance3D(internals::PointLatLng const& coord, int const& altitude); - double autosetdistance; - // QRectF rect; + int altitude; + UAVMapFollowType::Types mapfollowtype; + UAVTrailType::Types trailtype; + internals::PointLatLng coord; + internals::PointLatLng lastcoord; + QPixmap pic; + core::Point localposition; + OPMapWidget *mapwidget; + QGraphicsItemGroup *trail; + QGraphicsItemGroup *trailLine; + internals::PointLatLng lasttrailline; + QTime timer; + bool showtrail; + bool showtrailline; + int trailtime; + int traildistance; + bool autosetreached; + double Distance3D(internals::PointLatLng const & coord, int const & altitude); + double autosetdistance; + // QRectF rect; - public slots: - void RefreshPos(); - void setOpacitySlot(qreal opacity); - signals: - void UAVReachedWayPoint(int const& waypointnumber,WayPointItem* waypoint); - void UAVLeftSafetyBouble(internals::PointLatLng const& position); - void setChildPosition(); - void setChildLine(); - }; +public slots: + void RefreshPos(); + void setOpacitySlot(qreal opacity); +signals: + void UAVReachedWayPoint(int const & waypointnumber, WayPointItem *waypoint); + void UAVLeftSafetyBouble(internals::PointLatLng const & position); + void setChildPosition(); + void setChildLine(); +}; } #endif // GPSITEM_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.cpp index fa8bddb48..311ac4b50 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.cpp @@ -1,155 +1,148 @@ /** -****************************************************************************** -* -* @file homeitem.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a trail point -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file homeitem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a trail point + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "homeitem.h" -namespace mapcontrol +namespace mapcontrol { +HomeItem::HomeItem(MapGraphicItem *map, OPMapWidget *parent) : safe(true), map(map), mapwidget(parent), + showsafearea(true), toggleRefresh(true), safearea(1000), altitude(0), isDragging(false) { - HomeItem::HomeItem(MapGraphicItem* map,OPMapWidget* parent):safe(true),map(map),mapwidget(parent), - showsafearea(true),toggleRefresh(true),safearea(1000),altitude(0),isDragging(false) - { - pic.load(QString::fromUtf8(":/markers/images/home2.svg")); - pic=pic.scaled(30,30,Qt::IgnoreAspectRatio); - this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); - this->setFlag(QGraphicsItem::ItemIsMovable,false); - this->setFlag(QGraphicsItem::ItemIsSelectable,true); - localposition=map->FromLatLngToLocal(mapwidget->CurrentPosition()); - this->setPos(localposition.X(),localposition.Y()); - this->setZValue(4); - coord=internals::PointLatLng(50,50); - RefreshToolTip(); - setCacheMode(QGraphicsItem::DeviceCoordinateCache); - connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); - } - - void HomeItem::RefreshToolTip() - { - QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); - - setToolTip(QString("Waypoint: Home\nCoordinate:%1\nAltitude:%2\n").arg(coord_str).arg(QString::number(altitude))); - } - - - void HomeItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) - { - Q_UNUSED(option); - Q_UNUSED(widget); - painter->drawPixmap(-pic.width()/2,-pic.height()/2,pic); - if(showsafearea) - { - if(safe) - painter->setPen(Qt::green); - else - painter->setPen(Qt::red); - painter->drawEllipse(QPointF(0,0),localsafearea,localsafearea); - // painter->drawRect(QRectF(-localsafearea,-localsafearea,localsafearea*2,localsafearea*2)); - } - - } - QRectF HomeItem::boundingRect()const - { - if(pic.width()>localsafearea*2 && !toggleRefresh) - return QRectF(-pic.width()/2,-pic.height()/2,pic.width(),pic.height()); - else - return QRectF(-localsafearea,-localsafearea,localsafearea*2,localsafearea*2); - } - - - int HomeItem::type()const - { - return Type; - } - - void HomeItem::RefreshPos() - { - prepareGeometryChange(); - localposition=map->FromLatLngToLocal(coord); - this->setPos(localposition.X(),localposition.Y()); - if(showsafearea) - localsafearea=safearea/map->Projection()->GetGroundResolution(map->ZoomTotal(),coord.Lat()); - - RefreshToolTip(); - - this->update(); - toggleRefresh=false; - - } - - void HomeItem::setOpacitySlot(qreal opacity) - { - setOpacity(opacity); - } - - void HomeItem::mousePressEvent(QGraphicsSceneMouseEvent *event) - { - if(event->button()==Qt::LeftButton) - { - isDragging=true; - } - QGraphicsItem::mousePressEvent(event); - } - - void HomeItem::mouseReleaseEvent(QGraphicsSceneMouseEvent *event) - { - if(event->button()==Qt::LeftButton) - { - coord=map->FromLocalToLatLng(this->pos().x(),this->pos().y()); - isDragging=false; - - emit homePositionChanged(coord,Altitude()); - } - QGraphicsItem::mouseReleaseEvent(event); - - RefreshToolTip(); - } - void HomeItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent *event) - { - if(event->button()==Qt::LeftButton) - { - emit homedoubleclick(this); - } - } - void HomeItem::mouseMoveEvent(QGraphicsSceneMouseEvent *event) - { - - if(isDragging) - { - coord=map->FromLocalToLatLng(this->pos().x(),this->pos().y()); - emit homePositionChanged(coord,Altitude()); - } - QGraphicsItem::mouseMoveEvent(event); - } - - //Set clickable area as smaller than the bounding rect. - QPainterPath HomeItem::shape() const - { - QPainterPath path; - path.addEllipse(QRectF(-12, -25, 24, 50)); - return path; - } - + pic.load(QString::fromUtf8(":/markers/images/home2.svg")); + pic = pic.scaled(30, 30, Qt::IgnoreAspectRatio); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + this->setFlag(QGraphicsItem::ItemIsMovable, false); + this->setFlag(QGraphicsItem::ItemIsSelectable, true); + localposition = map->FromLatLngToLocal(mapwidget->CurrentPosition()); + this->setPos(localposition.X(), localposition.Y()); + this->setZValue(4); + coord = internals::PointLatLng(50, 50); + RefreshToolTip(); + setCacheMode(QGraphicsItem::DeviceCoordinateCache); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); } +void HomeItem::RefreshToolTip() +{ + QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); + + setToolTip(QString("Waypoint: Home\nCoordinate:%1\nAltitude:%2\n").arg(coord_str).arg(QString::number(altitude))); +} + + +void HomeItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) +{ + Q_UNUSED(option); + Q_UNUSED(widget); + painter->drawPixmap(-pic.width() / 2, -pic.height() / 2, pic); + if (showsafearea) { + if (safe) { + painter->setPen(Qt::green); + } else { + painter->setPen(Qt::red); + } + painter->drawEllipse(QPointF(0, 0), localsafearea, localsafearea); + // painter->drawRect(QRectF(-localsafearea,-localsafearea,localsafearea*2,localsafearea*2)); + } +} +QRectF HomeItem::boundingRect() const +{ + if (pic.width() > localsafearea * 2 && !toggleRefresh) { + return QRectF(-pic.width() / 2, -pic.height() / 2, pic.width(), pic.height()); + } else { + return QRectF(-localsafearea, -localsafearea, localsafearea * 2, localsafearea * 2); + } +} + + +int HomeItem::type() const +{ + return Type; +} + +void HomeItem::RefreshPos() +{ + prepareGeometryChange(); + localposition = map->FromLatLngToLocal(coord); + this->setPos(localposition.X(), localposition.Y()); + if (showsafearea) { + localsafearea = safearea / map->Projection()->GetGroundResolution(map->ZoomTotal(), coord.Lat()); + } + + RefreshToolTip(); + + this->update(); + toggleRefresh = false; +} + +void HomeItem::setOpacitySlot(qreal opacity) +{ + setOpacity(opacity); +} + +void HomeItem::mousePressEvent(QGraphicsSceneMouseEvent *event) +{ + if (event->button() == Qt::LeftButton) { + isDragging = true; + } + QGraphicsItem::mousePressEvent(event); +} + +void HomeItem::mouseReleaseEvent(QGraphicsSceneMouseEvent *event) +{ + if (event->button() == Qt::LeftButton) { + coord = map->FromLocalToLatLng(this->pos().x(), this->pos().y()); + isDragging = false; + + emit homePositionChanged(coord, Altitude()); + } + QGraphicsItem::mouseReleaseEvent(event); + + RefreshToolTip(); +} +void HomeItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent *event) +{ + if (event->button() == Qt::LeftButton) { + emit homedoubleclick(this); + } +} +void HomeItem::mouseMoveEvent(QGraphicsSceneMouseEvent *event) +{ + if (isDragging) { + coord = map->FromLocalToLatLng(this->pos().x(), this->pos().y()); + emit homePositionChanged(coord, Altitude()); + } + QGraphicsItem::mouseMoveEvent(event); +} + +// Set clickable area as smaller than the bounding rect. +QPainterPath HomeItem::shape() const +{ + QPainterPath path; + + path.addEllipse(QRectF(-12, -25, 24, 50)); + return path; +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.h index 2cf34b135..783b30a2c 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file homeitem.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a WayPoint -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file homeitem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef HOMEITEM_H #define HOMEITEM_H @@ -33,56 +33,79 @@ #include "../internals/pointlatlng.h" #include #include "opmapwidget.h" -namespace mapcontrol -{ - - class HomeItem:public QObject,public QGraphicsItem +namespace mapcontrol { +class HomeItem : public QObject, public QGraphicsItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) +public: + enum { Type = UserType + 4 }; + HomeItem(MapGraphicItem *map, OPMapWidget *parent); + void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, + QWidget *widget); + QRectF boundingRect() const; + int type() const; + bool ShowSafeArea() const { - Q_OBJECT - Q_INTERFACES(QGraphicsItem) - public: - enum { Type = UserType + 4 }; - HomeItem(MapGraphicItem* map,OPMapWidget* parent); - void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, - QWidget *widget); - QRectF boundingRect() const; - int type() const; - bool ShowSafeArea()const{return showsafearea;} - void SetShowSafeArea(bool const& value){showsafearea=value;} - void SetToggleRefresh(bool const& value){toggleRefresh=value;} - int SafeArea()const{return safearea;} - void SetSafeArea(int const& value){safearea=value;} - bool safe; - void SetCoord(internals::PointLatLng const& value){coord=value;emit homePositionChanged(value,Altitude());} - internals::PointLatLng Coord()const{return coord;} - void SetAltitude(float const& value){altitude=value;emit homePositionChanged(Coord(),Altitude());} - float Altitude()const{return altitude;} - void RefreshToolTip(); - private: + return showsafearea; + } + void SetShowSafeArea(bool const & value) + { + showsafearea = value; + } + void SetToggleRefresh(bool const & value) + { + toggleRefresh = value; + } + int SafeArea() const + { + return safearea; + } + void SetSafeArea(int const & value) + { + safearea = value; + } + bool safe; + void SetCoord(internals::PointLatLng const & value) + { + coord = value; emit homePositionChanged(value, Altitude()); + } + internals::PointLatLng Coord() const + { + return coord; + } + void SetAltitude(float const & value) + { + altitude = value; emit homePositionChanged(Coord(), Altitude()); + } + float Altitude() const + { + return altitude; + } + void RefreshToolTip(); +private: - MapGraphicItem* map; - OPMapWidget* mapwidget; - QPixmap pic; - core::Point localposition; - internals::PointLatLng coord; - bool showsafearea; - bool toggleRefresh; - int safearea; - int localsafearea; - float altitude; - bool isDragging; - protected: - void mouseMoveEvent ( QGraphicsSceneMouseEvent * event ); - void mousePressEvent ( QGraphicsSceneMouseEvent * event ); - void mouseReleaseEvent ( QGraphicsSceneMouseEvent * event ); - void mouseDoubleClickEvent(QGraphicsSceneMouseEvent *event); - QPainterPath shape() const; - public slots: - void RefreshPos(); - void setOpacitySlot(qreal opacity); - signals: - void homePositionChanged(internals::PointLatLng coord,float); - void homedoubleclick(HomeItem* waypoint); - }; + MapGraphicItem *map; + OPMapWidget *mapwidget; + QPixmap pic; + core::Point localposition; + internals::PointLatLng coord; + bool showsafearea; + bool toggleRefresh; + int safearea; + int localsafearea; + float altitude; + bool isDragging; +protected: + void mouseMoveEvent(QGraphicsSceneMouseEvent *event); + void mousePressEvent(QGraphicsSceneMouseEvent *event); + void mouseReleaseEvent(QGraphicsSceneMouseEvent *event); + void mouseDoubleClickEvent(QGraphicsSceneMouseEvent *event); + QPainterPath shape() const; +public slots: + void RefreshPos(); + void setOpacitySlot(qreal opacity); +signals: + void homePositionChanged(internals::PointLatLng coord, float); + void homedoubleclick(HomeItem *waypoint); +}; } #endif // HOMEITEM_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp index 587303d6d..c4ff05c69 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp @@ -1,563 +1,495 @@ /** -****************************************************************************** -* -* @file mapgraphicitem.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief The main graphicsItem used on the widget, contains the map and map logic -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file mapgraphicitem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief The main graphicsItem used on the widget, contains the map and map logic + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "uavitem.h" #include "gpsitem.h" #include "homeitem.h" #include "mapgraphicitem.h" -namespace mapcontrol +namespace mapcontrol { +MapGraphicItem::MapGraphicItem(internals::Core *core, Configuration *configuration) : core(core), config(configuration), MapRenderTransform(1), maxZoom(17), minZoom(2), zoomReal(0), isSelected(false), rotation(0), zoomDigi(0) { - MapGraphicItem::MapGraphicItem(internals::Core *core, Configuration *configuration):core(core),config(configuration),MapRenderTransform(1), maxZoom(17),minZoom(2),zoomReal(0),isSelected(false),rotation(0),zoomDigi(0) + dragons.load(QString::fromUtf8(":/markers/images/dragons1.jpg")); + showTileGridLines = false; + isMouseOverMarker = false; + maprect = QRectF(0, 0, 1022, 680); + core->SetCurrentRegion(internals::Rectangle(0, 0, maprect.width(), maprect.height())); + core->SetMapType(MapType::GoogleHybrid); + this->SetZoom(2); + this->setFlag(ItemIsFocusable); + connect(core, SIGNAL(OnNeedInvalidation()), this, SLOT(Core_OnNeedInvalidation())); + connect(core, SIGNAL(OnMapDrag()), this, SLOT(childPosRefresh())); + connect(core, SIGNAL(OnMapZoomChanged()), this, SLOT(childPosRefresh())); + setCacheMode(QGraphicsItem::ItemCoordinateCache); +} + +void MapGraphicItem::start() +{ + core->StartSystem(); +} + +void MapGraphicItem::resize(const QRectF &rect) +{ + Q_UNUSED(rect); { - dragons.load(QString::fromUtf8(":/markers/images/dragons1.jpg")); - showTileGridLines=false; - isMouseOverMarker=false; - maprect=QRectF(0,0,1022,680); - core->SetCurrentRegion(internals::Rectangle(0, 0, maprect.width(), maprect.height())); - core->SetMapType(MapType::GoogleHybrid); - this->SetZoom(2); - this->setFlag(ItemIsFocusable); - connect(core,SIGNAL(OnNeedInvalidation()),this,SLOT(Core_OnNeedInvalidation())); - connect(core,SIGNAL(OnMapDrag()),this,SLOT(childPosRefresh())); - connect(core,SIGNAL(OnMapZoomChanged()),this,SLOT(childPosRefresh())); - setCacheMode(QGraphicsItem::ItemCoordinateCache); + this->prepareGeometryChange(); + maprect = boundingBox(scene()->sceneRect(), rotation); + this->setTransform(QTransform().translate(-(maprect.width() - scene()->width()) / 2, -(maprect.height() - scene()->height()) / 2)); + this->setTransformOriginPoint(maprect.center().x(), maprect.center().y()); + this->setRotation(rotation); } - void MapGraphicItem::start() - { - core->StartSystem(); + core->OnMapSizeChanged(maprect.width(), maprect.height()); + core->SetCurrentRegion(internals::Rectangle(0, 0, maprect.width(), maprect.height())); + if (isVisible()) { + core->GoToCurrentPosition(); } +} - void MapGraphicItem::resize(const QRectF &rect) - { - Q_UNUSED(rect); - { - this->prepareGeometryChange(); - maprect=boundingBox(scene()->sceneRect(),rotation); - this->setTransform(QTransform().translate(-(maprect.width()-scene()->width())/2,-(maprect.height()-scene()->height())/2)); - this->setTransformOriginPoint(maprect.center().x(),maprect.center().y()); - this->setRotation(rotation); - } +QRectF MapGraphicItem::boundingRect() const +{ + const int Margin = 1; - core->OnMapSizeChanged(maprect.width(),maprect.height()); - core->SetCurrentRegion(internals::Rectangle(0, 0, maprect.width(), maprect.height())); - if(isVisible()) - { - core->GoToCurrentPosition(); - } - } + return maprect.adjusted(-Margin, -Margin, +Margin, +Margin); +} +void MapGraphicItem::Core_OnNeedInvalidation() +{ + this->update(); + emit childRefreshPosition(); +} +void MapGraphicItem::childPosRefresh() +{ + emit childRefreshPosition(); +} +void MapGraphicItem::setOverlayOpacity(qreal value) +{ + emit childSetOpacity(value); +} +void MapGraphicItem::ConstructLastImage(int const & zoomdiff) +{ + QImage temp; + QSize size = boundingRect().size().toSize(); - QRectF MapGraphicItem::boundingRect() const - { - const int Margin = 1; - return maprect.adjusted(-Margin, -Margin, +Margin, +Margin); - } - void MapGraphicItem::Core_OnNeedInvalidation() - { - this->update(); - emit childRefreshPosition(); - } - void MapGraphicItem::childPosRefresh() - { - emit childRefreshPosition(); - } - void MapGraphicItem::setOverlayOpacity(qreal value) - { - emit childSetOpacity(value); - } - void MapGraphicItem::ConstructLastImage(int const& zoomdiff) - { - QImage temp; - QSize size=boundingRect().size().toSize(); - size.setWidth(size.width()*2*zoomdiff); - size.setHeight(size.height()*2*zoomdiff); - temp=QImage(size,QImage::Format_ARGB32_Premultiplied); - temp.fill(0); - QPainter imagePainter(&temp); - imagePainter.translate(-boundingRect().topLeft()); - imagePainter.scale(2*zoomdiff,2*zoomdiff); - paintImage(&imagePainter); - imagePainter.end(); - lastimagepoint=Point(core->GetrenderOffset().X()*2*zoomdiff,core->GetrenderOffset().Y()*2*zoomdiff); - lastimage=temp; - } - void MapGraphicItem::paintImage(QPainter *painter) - { - - if(MapRenderTransform!=1) - { - QTransform transform; - transform.translate(-((boundingRect().width()*MapRenderTransform)-(boundingRect().width()))/2,-((boundingRect().height()*MapRenderTransform)-(boundingRect().height()))/2); - transform.scale(MapRenderTransform,MapRenderTransform); - painter->setWorldTransform(transform); - { - DrawMap2D(painter); - } - painter->resetTransform(); - } - else + size.setWidth(size.width() * 2 * zoomdiff); + size.setHeight(size.height() * 2 * zoomdiff); + temp = QImage(size, QImage::Format_ARGB32_Premultiplied); + temp.fill(0); + QPainter imagePainter(&temp); + imagePainter.translate(-boundingRect().topLeft()); + imagePainter.scale(2 * zoomdiff, 2 * zoomdiff); + paintImage(&imagePainter); + imagePainter.end(); + lastimagepoint = Point(core->GetrenderOffset().X() * 2 * zoomdiff, core->GetrenderOffset().Y() * 2 * zoomdiff); + lastimage = temp; +} +void MapGraphicItem::paintImage(QPainter *painter) +{ + if (MapRenderTransform != 1) { + QTransform transform; + transform.translate(-((boundingRect().width() * MapRenderTransform) - (boundingRect().width())) / 2, -((boundingRect().height() * MapRenderTransform) - (boundingRect().height())) / 2); + transform.scale(MapRenderTransform, MapRenderTransform); + painter->setWorldTransform(transform); { DrawMap2D(painter); } - //painter->drawRect(maprect); + painter->resetTransform(); + } else { + DrawMap2D(painter); } - void MapGraphicItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) - { - Q_UNUSED(option); - Q_UNUSED(widget); + // painter->drawRect(maprect); +} +void MapGraphicItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) +{ + Q_UNUSED(option); + Q_UNUSED(widget); - if(MapRenderTransform!=1) - { - QTransform transform; - transform.translate(-((boundingRect().width()*MapRenderTransform)-(boundingRect().width()))/2,-((boundingRect().height()*MapRenderTransform)-(boundingRect().height()))/2); - transform.scale(MapRenderTransform,MapRenderTransform); + if (MapRenderTransform != 1) { + QTransform transform; + transform.translate(-((boundingRect().width() * MapRenderTransform) - (boundingRect().width())) / 2, -((boundingRect().height() * MapRenderTransform) - (boundingRect().height())) / 2); + transform.scale(MapRenderTransform, MapRenderTransform); - painter->setWorldTransform(transform); - painter->setRenderHint(QPainter::SmoothPixmapTransform,true); - painter->setRenderHint(QPainter::HighQualityAntialiasing,true); + painter->setWorldTransform(transform); + painter->setRenderHint(QPainter::SmoothPixmapTransform, true); + painter->setRenderHint(QPainter::HighQualityAntialiasing, true); - { - DrawMap2D(painter); - } - painter->resetTransform(); - } - else { DrawMap2D(painter); } + painter->resetTransform(); + } else { + DrawMap2D(painter); } - void MapGraphicItem::mouseMoveEvent(QGraphicsSceneMouseEvent *event) - { - if(core->IsDragging()) - { - if(MapRenderTransform!=1) - { - qreal dx= (event->pos().x()-core->mouseDown.X())/(MapRenderTransform); - qreal dy= (event->pos().y()-core->mouseDown.Y())/(MapRenderTransform); - qreal nx=core->mouseDown.X()+dx; - qreal ny=core->mouseDown.Y()+dy; - core->mouseCurrent.SetX(nx); - core->mouseCurrent.SetY(ny); - } - else - { - core->mouseCurrent.SetX(event->pos().x()); - core->mouseCurrent.SetY(event->pos().y()); - } - { - core->Drag(core->mouseCurrent); - } - +} +void MapGraphicItem::mouseMoveEvent(QGraphicsSceneMouseEvent *event) +{ + if (core->IsDragging()) { + if (MapRenderTransform != 1) { + qreal dx = (event->pos().x() - core->mouseDown.X()) / (MapRenderTransform); + qreal dy = (event->pos().y() - core->mouseDown.Y()) / (MapRenderTransform); + qreal nx = core->mouseDown.X() + dx; + qreal ny = core->mouseDown.Y() + dy; + core->mouseCurrent.SetX(nx); + core->mouseCurrent.SetY(ny); + } else { + core->mouseCurrent.SetX(event->pos().x()); + core->mouseCurrent.SetY(event->pos().y()); } - else if(isSelected && !selectionStart.IsEmpty() && (event->modifiers() == Qt::ControlModifier || event->modifiers() == Qt::ShiftModifier)) { - selectionEnd = FromLocalToLatLng(event->pos().x(), event->pos().y()); - { - internals::PointLatLng p1 = selectionStart; - internals::PointLatLng p2 = selectionEnd; - - double x1 = qMin(p1.Lng(), p2.Lng()); - double y1 = qMax(p1.Lat(), p2.Lat()); - double x2 = qMax(p1.Lng(), p2.Lng()); - double y2 = qMin(p1.Lat(), p2.Lat()); - - SetSelectedArea(internals::RectLatLng(y1, x1, x2 - x1, y1 - y2)); - } + core->Drag(core->mouseCurrent); } - QGraphicsItem::mouseMoveEvent(event); - } - void MapGraphicItem::mousePressEvent(QGraphicsSceneMouseEvent *event) - { - if(!IsMouseOverMarker()) + } else if (isSelected && !selectionStart.IsEmpty() && (event->modifiers() == Qt::ControlModifier || event->modifiers() == Qt::ShiftModifier)) { + selectionEnd = FromLocalToLatLng(event->pos().x(), event->pos().y()); { - if(event->button() == config->DragButton && CanDragMap()&& !((event->modifiers()==Qt::ShiftModifier)||(event->modifiers()==Qt::ControlModifier))) - { - core->mouseDown.SetX(event->pos().x()); - core->mouseDown.SetY(event->pos().y()); + internals::PointLatLng p1 = selectionStart; + internals::PointLatLng p2 = selectionEnd; + double x1 = qMin(p1.Lng(), p2.Lng()); + double y1 = qMax(p1.Lat(), p2.Lat()); + double x2 = qMax(p1.Lng(), p2.Lng()); + double y2 = qMin(p1.Lat(), p2.Lat()); - this->setCursor(Qt::SizeAllCursor); - - core->BeginDrag(core->mouseDown); - this->update(); - - } - else if(!isSelected && ((event->modifiers()==Qt::ControlModifier)||(event->modifiers()==Qt::ShiftModifier))) - { - isSelected = true; - SetSelectedArea (internals::RectLatLng::Empty); - selectionEnd = internals::PointLatLng::Empty; - selectionStart = FromLocalToLatLng(event->pos().x(), event->pos().y()); - } - } - } - void MapGraphicItem::mouseReleaseEvent(QGraphicsSceneMouseEvent *event) - { - if(isSelected) - { - isSelected = false; - } - - if(core->IsDragging()) - { - core->EndDrag(); - - this->setCursor(Qt::ArrowCursor); - if(!BoundsOfMap.IsEmpty() && !BoundsOfMap.Contains(core->CurrentPosition())) - { - if(!core->LastLocationInBounds.IsEmpty()) - { - core->SetCurrentPosition(core->LastLocationInBounds); - } - } - } - else - { - if(!selectionEnd.IsEmpty() && !selectionStart.IsEmpty()) - { - if(!selectedArea.IsEmpty() && event->modifiers() == Qt::ShiftModifier) - { - SetZoomToFitRect(SelectedArea()); - selectedArea=internals::RectLatLng::Empty; - } - } - + SetSelectedArea(internals::RectLatLng(y1, x1, x2 - x1, y1 - y2)); } } - void MapGraphicItem::keyPressEvent(QKeyEvent *event) - { - if(event->modifiers()&(Qt::ShiftModifier|Qt::ControlModifier)) - this->setCursor(Qt::CrossCursor); - if(event->key()==Qt::Key_Escape) - selectedArea=internals::RectLatLng::Empty; - QGraphicsItem::keyPressEvent(event); - } - void MapGraphicItem::keyReleaseEvent(QKeyEvent *event) - { - if((event->modifiers()&(Qt::ShiftModifier|Qt::ControlModifier))==0) - this->setCursor(Qt::ArrowCursor); - } - bool MapGraphicItem::SetZoomToFitRect(internals::RectLatLng const& rect) - { - int maxZoom = core->GetMaxZoomToFitRect(rect); - if(maxZoom > 0) - { - internals::PointLatLng center=internals::PointLatLng(rect.Lat()-(rect.HeightLat()/2), rect.Lng()+(rect.WidthLng()/2)); - core->SetCurrentPosition(center); + QGraphicsItem::mouseMoveEvent(event); +} +void MapGraphicItem::mousePressEvent(QGraphicsSceneMouseEvent *event) +{ + if (!IsMouseOverMarker()) { + if (event->button() == config->DragButton && CanDragMap() && !((event->modifiers() == Qt::ShiftModifier) || (event->modifiers() == Qt::ControlModifier))) { + core->mouseDown.SetX(event->pos().x()); + core->mouseDown.SetY(event->pos().y()); - if(maxZoom > MaxZoom()) - { - maxZoom = MaxZoom(); - } - if((int) Zoom() != maxZoom) - { - SetZoom(maxZoom); - } + this->setCursor(Qt::SizeAllCursor); - return true; - } - return false; - } - - void MapGraphicItem::wheelEvent(QGraphicsSceneWheelEvent *event) - { - - if(!IsMouseOverMarker() && !IsDragging()) - { - if(core->GetmouseLastZoom().X() != event->pos().x() && core->mouseLastZoom.Y() != event->pos().y()) - { - if(GetMouseWheelZoomType() == internals::MouseWheelZoomType::MousePositionAndCenter) - { - core->SetCurrentPosition(FromLocalToLatLng(event->pos().x(), event->pos().y())); - } - else if(GetMouseWheelZoomType() == internals::MouseWheelZoomType::ViewCenter) - { - core->SetCurrentPosition(FromLocalToLatLng((int) maprect.width()/2, (int) maprect.height()/2)); - } - else if(GetMouseWheelZoomType() == internals::MouseWheelZoomType::MousePositionWithoutCenter) - { - core->SetCurrentPosition(FromLocalToLatLng(event->pos().x(), event->pos().y())); - - } - - core->mouseLastZoom.SetX((event->pos().x())); - core->mouseLastZoom.SetY((event->pos().y())); - } - - // set mouse position to map center - if(GetMouseWheelZoomType() != internals::MouseWheelZoomType::MousePositionWithoutCenter) - { - { - // System.Drawing.Point p = PointToScreen(new System.Drawing.Point(Width/2, Height/2)); - // Stuff.SetCursorPos((int) p.X, (int) p.Y); - } - } - - core->MouseWheelZooming = true; - - if(event->delta() > 0) - { - SetZoom(ZoomTotal()+1); - } - else if(event->delta() < 0) - { - SetZoom(ZoomTotal()-1); - } - - core->MouseWheelZooming = false; + core->BeginDrag(core->mouseDown); + this->update(); + } else if (!isSelected && ((event->modifiers() == Qt::ControlModifier) || (event->modifiers() == Qt::ShiftModifier))) { + isSelected = true; + SetSelectedArea(internals::RectLatLng::Empty); + selectionEnd = internals::PointLatLng::Empty; + selectionStart = FromLocalToLatLng(event->pos().x(), event->pos().y()); } } - void MapGraphicItem::DrawMap2D(QPainter *painter) - { - painter->drawImage(this->boundingRect(),dragons.toImage()); - if(!lastimage.isNull()) - painter->drawImage(core->GetrenderOffset().X()-lastimagepoint.X(),core->GetrenderOffset().Y()-lastimagepoint.Y(),lastimage); +} +void MapGraphicItem::mouseReleaseEvent(QGraphicsSceneMouseEvent *event) +{ + if (isSelected) { + isSelected = false; + } - for(int i = -core->GetsizeOfMapArea().Width(); i <= core->GetsizeOfMapArea().Width(); i++) - { - for(int j = -core->GetsizeOfMapArea().Height(); j <= core->GetsizeOfMapArea().Height(); j++) + if (core->IsDragging()) { + core->EndDrag(); + + this->setCursor(Qt::ArrowCursor); + if (!BoundsOfMap.IsEmpty() && !BoundsOfMap.Contains(core->CurrentPosition())) { + if (!core->LastLocationInBounds.IsEmpty()) { + core->SetCurrentPosition(core->LastLocationInBounds); + } + } + } else { + if (!selectionEnd.IsEmpty() && !selectionStart.IsEmpty()) { + if (!selectedArea.IsEmpty() && event->modifiers() == Qt::ShiftModifier) { + SetZoomToFitRect(SelectedArea()); + selectedArea = internals::RectLatLng::Empty; + } + } + } +} +void MapGraphicItem::keyPressEvent(QKeyEvent *event) +{ + if (event->modifiers() & (Qt::ShiftModifier | Qt::ControlModifier)) { + this->setCursor(Qt::CrossCursor); + } + if (event->key() == Qt::Key_Escape) { + selectedArea = internals::RectLatLng::Empty; + } + QGraphicsItem::keyPressEvent(event); +} +void MapGraphicItem::keyReleaseEvent(QKeyEvent *event) +{ + if ((event->modifiers() & (Qt::ShiftModifier | Qt::ControlModifier)) == 0) { + this->setCursor(Qt::ArrowCursor); + } +} +bool MapGraphicItem::SetZoomToFitRect(internals::RectLatLng const & rect) +{ + int maxZoom = core->GetMaxZoomToFitRect(rect); + + if (maxZoom > 0) { + internals::PointLatLng center = internals::PointLatLng(rect.Lat() - (rect.HeightLat() / 2), rect.Lng() + (rect.WidthLng() / 2)); + core->SetCurrentPosition(center); + + if (maxZoom > MaxZoom()) { + maxZoom = MaxZoom(); + } + + if ((int)Zoom() != maxZoom) { + SetZoom(maxZoom); + } + + return true; + } + return false; +} + +void MapGraphicItem::wheelEvent(QGraphicsSceneWheelEvent *event) +{ + if (!IsMouseOverMarker() && !IsDragging()) { + if (core->GetmouseLastZoom().X() != event->pos().x() && core->mouseLastZoom.Y() != event->pos().y()) { + if (GetMouseWheelZoomType() == internals::MouseWheelZoomType::MousePositionAndCenter) { + core->SetCurrentPosition(FromLocalToLatLng(event->pos().x(), event->pos().y())); + } else if (GetMouseWheelZoomType() == internals::MouseWheelZoomType::ViewCenter) { + core->SetCurrentPosition(FromLocalToLatLng((int)maprect.width() / 2, (int)maprect.height() / 2)); + } else if (GetMouseWheelZoomType() == internals::MouseWheelZoomType::MousePositionWithoutCenter) { + core->SetCurrentPosition(FromLocalToLatLng(event->pos().x(), event->pos().y())); + } + + core->mouseLastZoom.SetX((event->pos().x())); + core->mouseLastZoom.SetY((event->pos().y())); + } + + // set mouse position to map center + if (GetMouseWheelZoomType() != internals::MouseWheelZoomType::MousePositionWithoutCenter) { { - core->SettilePoint (core->GetcenterTileXYLocation()); - core->SettilePoint(Point(core->GettilePoint().X()+ i,core->GettilePoint().Y()+j)); - { - internals::Tile* t = core->Matrix.TileAt(core->GettilePoint()); - if(true) - { - core->tileRect.SetX(core->GettilePoint().X()*core->tileRect.Width()); - core->tileRect.SetY(core->GettilePoint().Y()*core->tileRect.Height()); - core->tileRect.Offset(core->GetrenderOffset()); - if(core->GetCurrentRegion().IntersectsWith(core->tileRect)) - { - bool found = false; + // System.Drawing.Point p = PointToScreen(new System.Drawing.Point(Width/2, Height/2)); + // Stuff.SetCursorPos((int) p.X, (int) p.Y); + } + } - // render tile - //lock(t.Overlays) - if(t!=0) - { - foreach(QByteArray img,t->Overlays) - { - if(img.count()!=0) + core->MouseWheelZooming = true; + + if (event->delta() > 0) { + SetZoom(ZoomTotal() + 1); + } else if (event->delta() < 0) { + SetZoom(ZoomTotal() - 1); + } + + core->MouseWheelZooming = false; + } +} +void MapGraphicItem::DrawMap2D(QPainter *painter) +{ + painter->drawImage(this->boundingRect(), dragons.toImage()); + if (!lastimage.isNull()) { + painter->drawImage(core->GetrenderOffset().X() - lastimagepoint.X(), core->GetrenderOffset().Y() - lastimagepoint.Y(), lastimage); + } + + for (int i = -core->GetsizeOfMapArea().Width(); i <= core->GetsizeOfMapArea().Width(); i++) { + for (int j = -core->GetsizeOfMapArea().Height(); j <= core->GetsizeOfMapArea().Height(); j++) { + core->SettilePoint(core->GetcenterTileXYLocation()); + core->SettilePoint(Point(core->GettilePoint().X() + i, core->GettilePoint().Y() + j)); + { + internals::Tile *t = core->Matrix.TileAt(core->GettilePoint()); + if (true) { + core->tileRect.SetX(core->GettilePoint().X() * core->tileRect.Width()); + core->tileRect.SetY(core->GettilePoint().Y() * core->tileRect.Height()); + core->tileRect.Offset(core->GetrenderOffset()); + if (core->GetCurrentRegion().IntersectsWith(core->tileRect)) { + bool found = false; + + // render tile + // lock(t.Overlays) + if (t != 0) { + foreach(QByteArray img, t->Overlays) { + if (img.count() != 0) { + if (!found) { + found = true; + } { - if(!found) - found = true; - { - painter->drawPixmap(core->tileRect.X(),core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height(),PureImageProxy::FromStream(img)); - } + painter->drawPixmap(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height(), PureImageProxy::FromStream(img)); } } } + } - if(showTileGridLines) + if (showTileGridLines) { + painter->setPen(config->EmptyTileBorders); + painter->drawRect(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()); { - painter->setPen(config->EmptyTileBorders); - painter->drawRect(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()); - { - painter->setFont(config->MissingDataFont); - painter->setPen(Qt::red); - painter->drawText(QRectF(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()),Qt::AlignCenter,(core->GettilePoint() == core->GetcenterTileXYLocation()? "CENTER: " :"TILE: ")+core->GettilePoint().ToString()); - } - } - - // add text if tile is missing - if(false) - { - - painter->fillRect(QRectF(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()),config->EmptytileBrush); painter->setFont(config->MissingDataFont); - painter->drawText(QRectF(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()),config->EmptyTileText); - - - - painter->setPen(config->EmptyTileBorders); - painter->drawRect(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()); - - // raise error - - } - if(!SelectedArea().IsEmpty()) - { - core::Point p1 = FromLatLngToLocal(SelectedArea().LocationTopLeft()); - core::Point p2 = FromLatLngToLocal(SelectedArea().LocationRightBottom()); - int x1 = p1.X(); - int y1 = p1.Y(); - int x2 = p2.X(); - int y2 = p2.Y(); - painter->setPen(Qt::black); - painter->setBrush(QBrush(QColor(50,50,100,20))); - painter->drawRect(x1,y1,x2-x1,y2-y1); + painter->setPen(Qt::red); + painter->drawText(QRectF(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()), Qt::AlignCenter, (core->GettilePoint() == core->GetcenterTileXYLocation() ? "CENTER: " : "TILE: ") + core->GettilePoint().ToString()); } } + + // add text if tile is missing + if (false) { + painter->fillRect(QRectF(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()), config->EmptytileBrush); + painter->setFont(config->MissingDataFont); + painter->drawText(QRectF(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()), config->EmptyTileText); + + + painter->setPen(config->EmptyTileBorders); + painter->drawRect(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()); + + // raise error + } + if (!SelectedArea().IsEmpty()) { + core::Point p1 = FromLatLngToLocal(SelectedArea().LocationTopLeft()); + core::Point p2 = FromLatLngToLocal(SelectedArea().LocationRightBottom()); + int x1 = p1.X(); + int y1 = p1.Y(); + int x2 = p2.X(); + int y2 = p2.Y(); + painter->setPen(Qt::black); + painter->setBrush(QBrush(QColor(50, 50, 100, 20))); + painter->drawRect(x1, y1, x2 - x1, y2 - y1); + } } } } } } +} - core::Point MapGraphicItem::FromLatLngToLocal(internals::PointLatLng const& point) - { - core::Point ret = core->FromLatLngToLocal(point); - if(MapRenderTransform!=1) - { - ret.SetX((int) (ret.X() * MapRenderTransform)); - ret.SetY((int) (ret.Y() * MapRenderTransform)); - ret.SetX(ret.X()-((boundingRect().width()*MapRenderTransform)-(boundingRect().width()))/2); - ret.SetY(ret.Y()-((boundingRect().height()*MapRenderTransform)-(boundingRect().height()))/2); +core::Point MapGraphicItem::FromLatLngToLocal(internals::PointLatLng const & point) +{ + core::Point ret = core->FromLatLngToLocal(point); + if (MapRenderTransform != 1) { + ret.SetX((int)(ret.X() * MapRenderTransform)); + ret.SetY((int)(ret.Y() * MapRenderTransform)); + ret.SetX(ret.X() - ((boundingRect().width() * MapRenderTransform) - (boundingRect().width())) / 2); + ret.SetY(ret.Y() - ((boundingRect().height() * MapRenderTransform) - (boundingRect().height())) / 2); + } + return ret; +} +internals::PointLatLng MapGraphicItem::FromLocalToLatLng(int x, int y) +{ + if (MapRenderTransform != 1) { + x = x + ((boundingRect().width() * MapRenderTransform) - (boundingRect().width())) / 2; + y = y + ((boundingRect().height() * MapRenderTransform) - (boundingRect().height())) / 2; + x = (int)(x / MapRenderTransform); + y = (int)(y / MapRenderTransform); + } + return core->FromLocalToLatLng(x, y); +} + +double MapGraphicItem::Zoom() +{ + return zoomReal; +} +double MapGraphicItem::ZoomDigi() +{ + return zoomDigi; +} +double MapGraphicItem::ZoomTotal() +{ + return zoomDigi + zoomReal; +} + +void MapGraphicItem::SetZoom(double const & value) +{ + if (ZoomTotal() != value) { + if (value > MaxZoom()) { + zoomReal = MaxZoom(); + zoomDigi = value - MaxZoom(); + } else if (value < MinZoom()) { + zoomDigi = 0; + zoomReal = MinZoom(); + } else { + zoomDigi = 0; + zoomReal = value; } - return ret; - } - internals::PointLatLng MapGraphicItem::FromLocalToLatLng(int x, int y) - { - if(MapRenderTransform!=1) - { - x=x+((boundingRect().width()*MapRenderTransform)-(boundingRect().width()))/2; - y=y+((boundingRect().height()*MapRenderTransform)-(boundingRect().height()))/2; - - x = (int) (x / MapRenderTransform); - y = (int) (y / MapRenderTransform); - } - return core->FromLocalToLatLng(x, y); - } - - double MapGraphicItem::Zoom() - { - return zoomReal; - } - double MapGraphicItem::ZoomDigi() - { - return zoomDigi; - } - double MapGraphicItem::ZoomTotal() - { - return zoomDigi+zoomReal; - } - - void MapGraphicItem::SetZoom(double const& value) - { - if(ZoomTotal() != value) - { - if(value > MaxZoom()) + double integer; + double remainder = modf(value, &integer); + if (zoomDigi != 0 || remainder != 0) { + float scaleValue = zoomDigi + remainder + 1; { - zoomReal = MaxZoom(); - zoomDigi =value-MaxZoom(); + MapRenderTransform = scaleValue; } - else - if(value < MinZoom()) - { - zoomDigi=0; - zoomReal = MinZoom(); + if (integer > MaxZoom()) { + integer = MaxZoom(); } - else - { - zoomDigi=0; - zoomReal = value; - } - double integer; - double remainder = modf (value , &integer); - if(zoomDigi!=0||remainder != 0) - { - float scaleValue = zoomDigi+remainder + 1; - { - MapRenderTransform = scaleValue; - } - if(integer>MaxZoom()) - integer=MaxZoom(); - SetZoomStep((qint32)(integer)); - // core->GoToCurrentPositionOnZoom(); - this->update(); + SetZoomStep((qint32)(integer)); + // core->GoToCurrentPositionOnZoom(); + this->update(); + } else { + MapRenderTransform = 1; - } - else - { - - MapRenderTransform = 1; - - SetZoomStep ((qint32)(value)); - zoomReal = ZoomStep(); - this->update(); - } + SetZoomStep((qint32)(value)); + zoomReal = ZoomStep(); + this->update(); } } - int MapGraphicItem::ZoomStep()const - { - return core->Zoom(); - } - void MapGraphicItem::SetZoomStep(int const& value) - { - if(value-core->Zoom()>0 && value<= MaxZoom()) - ConstructLastImage(value-core->Zoom()); - else if(value!=MaxZoom()) - lastimage=QImage(); - if(value > MaxZoom()) - { - core->SetZoom(MaxZoom()); - emit zoomChanged(MaxZoom()+ZoomDigi(),Zoom(),ZoomDigi()); - } - else if(value < MinZoom()) - { - core->SetZoom(MinZoom()); - emit zoomChanged(MinZoom()+ZoomDigi(),Zoom(),ZoomDigi()); - } - else - { - core->SetZoom(value); - emit zoomChanged(value+ZoomDigi(),Zoom(),ZoomDigi());; - } - - } - - void MapGraphicItem::Offset(int const& x, int const& y) - { - core->DragOffset(Point(x, y)); - } - void MapGraphicItem::mapRotate(qreal angle) - { - if (rotation != angle) { - rotation=angle; - resize(scene()->sceneRect()); - } - } - QRectF MapGraphicItem::boundingBox(const QRectF &rect, const qreal &angle) - { - QRectF ret(rect); - float c=cos(angle*2*M_PI/360); - float s=sin(angle*2*M_PI/360); - ret.setHeight(rect.height()*fabs(c)+rect.width()*fabs(s)); - ret.setWidth(rect.width()*fabs(c)+rect.height()*fabs(s)); - return ret; - } - QSize MapGraphicItem::sizeHint()const - { - core::Size size=core->projection->GetTileMatrixMaxXY(MinZoom()); - core::Size tilesize=core->projection->TileSize(); - QSize rsize((size.Width()+1)*tilesize.Width(),(size.Height()+1)*tilesize.Height()); - return rsize; - } +} +int MapGraphicItem::ZoomStep() const +{ + return core->Zoom(); +} +void MapGraphicItem::SetZoomStep(int const & value) +{ + if (value - core->Zoom() > 0 && value <= MaxZoom()) { + ConstructLastImage(value - core->Zoom()); + } else if (value != MaxZoom()) { + lastimage = QImage(); + } + if (value > MaxZoom()) { + core->SetZoom(MaxZoom()); + emit zoomChanged(MaxZoom() + ZoomDigi(), Zoom(), ZoomDigi()); + } else if (value < MinZoom()) { + core->SetZoom(MinZoom()); + emit zoomChanged(MinZoom() + ZoomDigi(), Zoom(), ZoomDigi()); + } else { + core->SetZoom(value); + emit zoomChanged(value + ZoomDigi(), Zoom(), ZoomDigi());; + } +} + +void MapGraphicItem::Offset(int const & x, int const & y) +{ + core->DragOffset(Point(x, y)); +} +void MapGraphicItem::mapRotate(qreal angle) +{ + if (rotation != angle) { + rotation = angle; + resize(scene()->sceneRect()); + } +} +QRectF MapGraphicItem::boundingBox(const QRectF &rect, const qreal &angle) +{ + QRectF ret(rect); + float c = cos(angle * 2 * M_PI / 360); + float s = sin(angle * 2 * M_PI / 360); + + ret.setHeight(rect.height() * fabs(c) + rect.width() * fabs(s)); + ret.setWidth(rect.width() * fabs(c) + rect.height() * fabs(s)); + return ret; +} +QSize MapGraphicItem::sizeHint() const +{ + core::Size size = core->projection->GetTileMatrixMaxXY(MinZoom()); + core::Size tilesize = core->projection->TileSize(); + QSize rsize((size.Width() + 1) * tilesize.Width(), (size.Height() + 1) * tilesize.Height()); + + return rsize; +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h index a87450dd1..e1b2c5d88 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h @@ -1,35 +1,35 @@ /** -****************************************************************************** -* -* @file mapgraphicitem.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief The main graphicsItem used on the widget, contains the map and map logic -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file mapgraphicitem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief The main graphicsItem used on the widget, contains the map and map logic + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef MAPGRAPHICITEM_H #define MAPGRAPHICITEM_H #include #include "../internals/core.h" -//#include "../internals/point.h" +// #include "../internals/point.h" #include "../core/diagnostics.h" #include "configuration.h" #include @@ -39,185 +39,228 @@ #include #include #include "waypointitem.h" -//#include "uavitem.h" +// #include "uavitem.h" -namespace mapcontrol -{ - class WayPointItem; - class OPMapWidget; - /** - * @brief The main graphicsItem used on the widget, contains the map and map logic - * - * @class MapGraphicItem mapgraphicitem.h "mapgraphicitem.h" - */ - class MapGraphicItem:public QObject,public QGraphicsItem - { - friend class mapcontrol::OPMapWidget; - Q_OBJECT +namespace mapcontrol { +class WayPointItem; +class OPMapWidget; +/** + * @brief The main graphicsItem used on the widget, contains the map and map logic + * + * @class MapGraphicItem mapgraphicitem.h "mapgraphicitem.h" + */ +class MapGraphicItem : public QObject, public QGraphicsItem { + friend class mapcontrol::OPMapWidget; + Q_OBJECT Q_INTERFACES(QGraphicsItem) - public: +public: - /** - * @brief Contructer - * - * @param core - * @param configuration the configuration to be used - * @return - */ - MapGraphicItem(internals::Core *core,Configuration *configuration); - QRectF boundingRect() const; - void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, - QWidget *widget); + /** + * @brief Contructer + * + * @param core + * @param configuration the configuration to be used + * @return + */ + MapGraphicItem(internals::Core *core, Configuration *configuration); + QRectF boundingRect() const; + void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, + QWidget *widget); - QSize sizeHint()const; - /** - * @brief Convertes LatLong coordinates to local item coordinates - * - * @param point LatLong point to be converted - * @return core::Point Local item point - */ - core::Point FromLatLngToLocal(internals::PointLatLng const& point); - /** - * @brief Converts from local item coordinates to LatLong point - * - * @param x x local coordinate - * @param y y local coordinate - * @return internals::PointLatLng LatLng coordinate - */ - internals::PointLatLng FromLocalToLatLng(int x, int y); - /** - * @brief Returns true if map is being dragged - * - * @return - */ - bool IsDragging()const{return core->IsDragging();} + QSize sizeHint() const; + /** + * @brief Convertes LatLong coordinates to local item coordinates + * + * @param point LatLong point to be converted + * @return core::Point Local item point + */ + core::Point FromLatLngToLocal(internals::PointLatLng const & point); + /** + * @brief Converts from local item coordinates to LatLong point + * + * @param x x local coordinate + * @param y y local coordinate + * @return internals::PointLatLng LatLng coordinate + */ + internals::PointLatLng FromLocalToLatLng(int x, int y); + /** + * @brief Returns true if map is being dragged + * + * @return + */ + bool IsDragging() const + { + return core->IsDragging(); + } - QImage lastimage; - core::Point lastimagepoint; - void paintImage(QPainter* painter); - void ConstructLastImage(int const& zoomdiff); - internals::PureProjection* Projection()const{return core->Projection();} - double Zoom(); - double ZoomDigi(); - double ZoomTotal(); - void setOverlayOpacity(qreal value); - protected: - void mouseMoveEvent ( QGraphicsSceneMouseEvent * event ); - void mousePressEvent ( QGraphicsSceneMouseEvent * event ); - void wheelEvent ( QGraphicsSceneWheelEvent * event ); - void mouseReleaseEvent(QGraphicsSceneMouseEvent *event); - bool IsMouseOverMarker()const{return isMouseOverMarker;} - void keyPressEvent ( QKeyEvent * event ); - void keyReleaseEvent ( QKeyEvent * event ); + QImage lastimage; + core::Point lastimagepoint; + void paintImage(QPainter *painter); + void ConstructLastImage(int const & zoomdiff); + internals::PureProjection *Projection() const + { + return core->Projection(); + } + double Zoom(); + double ZoomDigi(); + double ZoomTotal(); + void setOverlayOpacity(qreal value); +protected: + void mouseMoveEvent(QGraphicsSceneMouseEvent *event); + void mousePressEvent(QGraphicsSceneMouseEvent *event); + void wheelEvent(QGraphicsSceneWheelEvent *event); + void mouseReleaseEvent(QGraphicsSceneMouseEvent *event); + bool IsMouseOverMarker() const + { + return isMouseOverMarker; + } + void keyPressEvent(QKeyEvent *event); + void keyReleaseEvent(QKeyEvent *event); - /** - * @brief Returns current map zoom - * - * @return int Current map zoom - */ - int ZoomStep()const; - /** - * @brief Sets map zoom - * - * @param value zoom value - */ - void SetZoomStep(int const& value); + /** + * @brief Returns current map zoom + * + * @return int Current map zoom + */ + int ZoomStep() const; + /** + * @brief Sets map zoom + * + * @param value zoom value + */ + void SetZoomStep(int const & value); - /** - * @brief Ask Stacey - * - * @param value - */ - void SetShowDragons(bool const& value); - private: - bool showDragons; - bool SetZoomToFitRect(internals::RectLatLng const& rect); - internals::Core *core; - Configuration *config; - bool showTileGridLines; - qreal MapRenderTransform; - void DrawMap2D(QPainter *painter); - /** - * @brief Maximum possible zoom - * - * @var maxZoom - */ - int maxZoom; - /** - * @brief Minimum possible zoom - * - * @var minZoom - */ - int minZoom; - internals::RectLatLng selectedArea; - internals::PointLatLng selectionStart; - internals::PointLatLng selectionEnd; - double zoomReal; - double zoomDigi; - QRectF maprect; - bool isSelected; - bool isMouseOverMarker; - QPixmap dragons; - void SetIsMouseOverMarker(bool const& value){isMouseOverMarker = value;} + /** + * @brief Ask Stacey + * + * @param value + */ + void SetShowDragons(bool const & value); +private: + bool showDragons; + bool SetZoomToFitRect(internals::RectLatLng const & rect); + internals::Core *core; + Configuration *config; + bool showTileGridLines; + qreal MapRenderTransform; + void DrawMap2D(QPainter *painter); + /** + * @brief Maximum possible zoom + * + * @var maxZoom + */ + int maxZoom; + /** + * @brief Minimum possible zoom + * + * @var minZoom + */ + int minZoom; + internals::RectLatLng selectedArea; + internals::PointLatLng selectionStart; + internals::PointLatLng selectionEnd; + double zoomReal; + double zoomDigi; + QRectF maprect; + bool isSelected; + bool isMouseOverMarker; + QPixmap dragons; + void SetIsMouseOverMarker(bool const & value) + { + isMouseOverMarker = value; + } - qreal rotation; - /** - * @brief Creates a rectangle that represents the "view" of the cuurent map, to compensate - * rotation - * - * @param rect original rectangle - * @param angle angle of rotation - * @return QRectF - */ - QRectF boundingBox(QRectF const& rect, qreal const& angle); - /** - * @brief Returns the maximum allowed zoom - * - * @return int - */ - int MaxZoom()const{return core->MaxZoom();} - /** - * @brief Returns the minimum allowed zoom - * - * @return int - */ - int MinZoom()const{return minZoom;} - internals::MouseWheelZoomType::Types GetMouseWheelZoomType(){return core->GetMouseWheelZoomType();} - void SetSelectedArea(internals::RectLatLng const& value){selectedArea = value;this->update();} - internals::RectLatLng SelectedArea()const{return selectedArea;} - internals::RectLatLng BoundsOfMap; - void Offset(int const& x, int const& y); - bool CanDragMap()const{return core->CanDragMap;} - void SetCanDragMap(bool const& value){core->CanDragMap = value;} + qreal rotation; + /** + * @brief Creates a rectangle that represents the "view" of the cuurent map, to compensate + * rotation + * + * @param rect original rectangle + * @param angle angle of rotation + * @return QRectF + */ + QRectF boundingBox(QRectF const & rect, qreal const & angle); + /** + * @brief Returns the maximum allowed zoom + * + * @return int + */ + int MaxZoom() const + { + return core->MaxZoom(); + } + /** + * @brief Returns the minimum allowed zoom + * + * @return int + */ + int MinZoom() const + { + return minZoom; + } + internals::MouseWheelZoomType::Types GetMouseWheelZoomType() + { + return core->GetMouseWheelZoomType(); + } + void SetSelectedArea(internals::RectLatLng const & value) + { + selectedArea = value; this->update(); + } + internals::RectLatLng SelectedArea() const + { + return selectedArea; + } + internals::RectLatLng BoundsOfMap; + void Offset(int const & x, int const & y); + bool CanDragMap() const + { + return core->CanDragMap; + } + void SetCanDragMap(bool const & value) + { + core->CanDragMap = value; + } - void SetZoom(double const& value); - void mapRotate ( qreal angle ); - void start(); - void ReloadMap(){core->ReloadMap();} - GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const& keys){return core->SetCurrentPositionByKeywords(keys);} - MapType::Types GetMapType(){return core->GetMapType();} - void SetMapType(MapType::Types const& value){core->SetMapType(value);} - private slots: - void Core_OnNeedInvalidation(); - void childPosRefresh(); - public slots: - /** - * @brief To be called when the scene size changes - * - * @param rect - */ - void resize ( QRectF const &rect=QRectF() ); - signals: - /** - * @brief Fired when the current zoom is changed - * - * @param zoom - */ - void wpdoubleclicked(WayPointItem * wp); - void zoomChanged(double zoomtotal,double zoomreal,double zoomdigi); - void childRefreshPosition(); - void childSetOpacity(qreal value); - }; + void SetZoom(double const & value); + void mapRotate(qreal angle); + void start(); + void ReloadMap() + { + core->ReloadMap(); + } + GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const & keys) + { + return core->SetCurrentPositionByKeywords(keys); + } + MapType::Types GetMapType() + { + return core->GetMapType(); + } + void SetMapType(MapType::Types const & value) + { + core->SetMapType(value); + } +private slots: + void Core_OnNeedInvalidation(); + void childPosRefresh(); +public slots: + /** + * @brief To be called when the scene size changes + * + * @param rect + */ + void resize(QRectF const &rect = QRectF()); +signals: + /** + * @brief Fired when the current zoom is changed + * + * @param zoom + */ + void wpdoubleclicked(WayPointItem *wp); + void zoomChanged(double zoomtotal, double zoomreal, double zoomdigi); + void childRefreshPosition(); + void childSetOpacity(qreal value); +}; } #endif // MAPGRAPHICITEM_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.cpp index 78256bd35..955818a7b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.cpp @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file mapripform.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief Form to be used with the MapRipper class -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file mapripform.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Form to be used with the MapRipper class + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "mapripform.h" #include "ui_mapripform.h" @@ -33,7 +33,7 @@ MapRipForm::MapRipForm(QWidget *parent) : ui(new Ui::MapRipForm) { ui->setupUi(this); - connect(ui->cancelButton,SIGNAL(clicked()),this,SIGNAL(cancelRequest())); + connect(ui->cancelButton, SIGNAL(clicked()), this, SIGNAL(cancelRequest())); } MapRipForm::~MapRipForm() @@ -44,7 +44,7 @@ void MapRipForm::SetPercentage(const int &perc) { ui->progressBar->setValue(perc); } -void MapRipForm::SetProvider(const QString &prov,const int &zoom) +void MapRipForm::SetProvider(const QString &prov, const int &zoom) { ui->mainlabel->setText(QString("Currently ripping from:%1 at Zoom level %2").arg(prov).arg(zoom)); } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.h index dc0ceb56c..0912acbdc 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.h @@ -1,49 +1,48 @@ /** -****************************************************************************** -* -* @file mapripform.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief Form to be used with the MapRipper class -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file mapripform.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Form to be used with the MapRipper class + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef MAPRIPFORM_H #define MAPRIPFORM_H #include namespace Ui { - class MapRipForm; +class MapRipForm; } -class MapRipForm : public QWidget -{ +class MapRipForm : public QWidget { Q_OBJECT public: explicit MapRipForm(QWidget *parent = 0); ~MapRipForm(); public slots: - void SetPercentage(int const& perc); - void SetProvider(QString const& prov,int const& zoom); - void SetNumberOfTiles(int const& total,int const& actual); + void SetPercentage(int const & perc); + void SetProvider(QString const & prov, int const & zoom); + void SetNumberOfTiles(int const & total, int const & actual); signals: void cancelRequest(); private: diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.cpp index 26778d5cb..59600a21f 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.cpp @@ -1,100 +1,87 @@ /** -****************************************************************************** -* -* @file mapripper.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A class that allows ripping of a selection of the map -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file mapripper.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A class that allows ripping of a selection of the map + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "mapripper.h" -namespace mapcontrol +namespace mapcontrol { +MapRipper::MapRipper(internals::Core *core, const internals::RectLatLng & rect) : sleep(100), cancel(false), progressForm(0), core(core), yesToAll(false) { - -MapRipper::MapRipper(internals::Core * core, const internals::RectLatLng & rect):sleep(100),cancel(false),progressForm(0),core(core),yesToAll(false) - { - if(!rect.IsEmpty()) - { - type=core->GetMapType(); - progressForm=new MapRipForm; - connect(progressForm,SIGNAL(cancelRequest()),this,SLOT(stopFetching())); - area=rect; - zoom=core->Zoom(); - maxzoom=core->MaxZoom(); - points=core->Projection()->GetAreaTileList(area,zoom,0); - this->start(); - cancel=false; - progressForm->show(); - connect(this,SIGNAL(percentageChanged(int)),progressForm,SLOT(SetPercentage(int))); - connect(this,SIGNAL(numberOfTilesChanged(int,int)),progressForm,SLOT(SetNumberOfTiles(int,int))); - connect(this,SIGNAL(providerChanged(QString,int)),progressForm,SLOT(SetProvider(QString,int))); - connect(this,SIGNAL(finished()),this,SLOT(finish())); - emit numberOfTilesChanged(0,0); - } - else + if (!rect.IsEmpty()) { + type = core->GetMapType(); + progressForm = new MapRipForm; + connect(progressForm, SIGNAL(cancelRequest()), this, SLOT(stopFetching())); + area = rect; + zoom = core->Zoom(); + maxzoom = core->MaxZoom(); + points = core->Projection()->GetAreaTileList(area, zoom, 0); + this->start(); + cancel = false; + progressForm->show(); + connect(this, SIGNAL(percentageChanged(int)), progressForm, SLOT(SetPercentage(int))); + connect(this, SIGNAL(numberOfTilesChanged(int, int)), progressForm, SLOT(SetNumberOfTiles(int, int))); + connect(this, SIGNAL(providerChanged(QString, int)), progressForm, SLOT(SetProvider(QString, int))); + connect(this, SIGNAL(finished()), this, SLOT(finish())); + emit numberOfTilesChanged(0, 0); + } else #ifdef Q_OS_DARWIN - QMessageBox::information(new QWidget(),"No valid selection","This pre-caches map data.\n\nPlease first select the area of the map to rip with +Left mouse click"); + { QMessageBox::information(new QWidget(), "No valid selection", "This pre-caches map data.\n\nPlease first select the area of the map to rip with +Left mouse click"); } #else - QMessageBox::information(new QWidget(),"No valid selection","This pre-caches map data.\n\nPlease first select the area of the map to rip with +Left mouse click"); + { QMessageBox::information(new QWidget(), "No valid selection", "This pre-caches map data.\n\nPlease first select the area of the map to rip with +Left mouse click"); } #endif - } +} void MapRipper::finish() { - if(zoomProjection()->GetAreaTileList(area,zoom,0); + points = core->Projection()->GetAreaTileList(area, zoom, 0); this->start(); - } - else if(ret==QMessageBox::YesAll) - { - yesToAll=true; + } else if (ret == QMessageBox::YesAll) { + yesToAll = true; points.clear(); - points=core->Projection()->GetAreaTileList(area,zoom,0); + points = core->Projection()->GetAreaTileList(area, zoom, 0); this->start(); - } - else - { + } else { progressForm->close(); delete progressForm; this->deleteLater(); } - } - else - { - yesToAll=false; + } else { + yesToAll = false; progressForm->close(); delete progressForm; this->deleteLater(); @@ -102,55 +89,53 @@ void MapRipper::finish() } - void MapRipper::run() - { - int countOk = 0; - bool goodtile=false; - // Stuff.Shuffle(ref list); - QVector types = OPMaps::Instance()->GetAllLayersOfType(type); - int all=points.count(); - for(int i = 0; i < all; i++) - { - emit numberOfTilesChanged(all,i+1); - if(cancel) - break; +void MapRipper::run() +{ + int countOk = 0; + bool goodtile = false; - core::Point p = points[i]; - { - //qDebug()<<"offline fetching:"<GetImageFrom(type, p, zoom); - if(img.length()!=0) - { - goodtile=true; - img=NULL; - } - else - goodtile=false; - } - if(goodtile) - { - countOk++; - } - else - { - i--; - QThread::msleep(1000); - continue; + // Stuff.Shuffle(ref list); + QVector types = OPMaps::Instance()->GetAllLayersOfType(type); + int all = points.count(); + for (int i = 0; i < all; i++) { + emit numberOfTilesChanged(all, i + 1); + if (cancel) { + break; + } + + core::Point p = points[i]; + { + // qDebug()<<"offline fetching:"<GetImageFrom(type, p, zoom); + + if (img.length() != 0) { + goodtile = true; + img = NULL; + } else { + goodtile = false; } } - emit percentageChanged((int) ((i+1)*100/all));//, i+1); - // worker.ReportProgress((int) ((i+1)*100/all), i+1); - - QThread::msleep(sleep); + if (goodtile) { + countOk++; + } else { + i--; + QThread::msleep(1000); + continue; + } } - } + emit percentageChanged((int)((i + 1) * 100 / all)); // , i+1); + // worker.ReportProgress((int) ((i+1)*100/all), i+1); - void MapRipper::stopFetching() - { - QMutexLocker locker(&mutex); - cancel=true; + QThread::msleep(sleep); } } + +void MapRipper::stopFetching() +{ + QMutexLocker locker(&mutex); + + cancel = true; +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.h index e6958a1a2..4929a7e93 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file mapripper.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A class that allows ripping of a selection of the map -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file mapripper.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A class that allows ripping of a selection of the map + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef MAPRIPPER_H #define MAPRIPPER_H @@ -32,36 +32,34 @@ #include "mapripform.h" #include #include -namespace mapcontrol -{ - class MapRipper:public QThread - { - Q_OBJECT - public: - MapRipper(internals::Core *,internals::RectLatLng const&); - void run(); - private: - QList points; - int zoom; - core::MapType::Types type; - int sleep; - internals::RectLatLng area; - bool cancel; - MapRipForm * progressForm; - int maxzoom; - internals::Core * core; - bool yesToAll; - QMutex mutex; +namespace mapcontrol { +class MapRipper : public QThread { + Q_OBJECT +public: + MapRipper(internals::Core *, internals::RectLatLng const &); + void run(); +private: + QList points; + int zoom; + core::MapType::Types type; + int sleep; + internals::RectLatLng area; + bool cancel; + MapRipForm *progressForm; + int maxzoom; + internals::Core *core; + bool yesToAll; + QMutex mutex; - signals: - void percentageChanged(int const& perc); - void numberOfTilesChanged(int const& total,int const& actual); - void providerChanged(QString const& prov,int const& zoom); +signals: + void percentageChanged(int const & perc); + void numberOfTilesChanged(int const & total, int const & actual); + void providerChanged(QString const & prov, int const & zoom); - public slots: - void stopFetching(); - void finish(); - }; +public slots: + void stopFetching(); + void finish(); +}; } #endif // MAPRIPPER_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp index ea477adde..f9ede780e 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp @@ -1,553 +1,549 @@ /** -****************************************************************************** -* -* @file opmapwidget.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief The Map Widget, this is the part exposed to the user -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file opmapwidget.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief The Map Widget, this is the part exposed to the user + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "opmapwidget.h" #include #include #include "waypointitem.h" -namespace mapcontrol +namespace mapcontrol { +OPMapWidget::OPMapWidget(QWidget *parent, Configuration *config) : QGraphicsView(parent), configuration(config), UAV(0), GPS(0), Home(0) + , followmouse(true), compass(0), showuav(false), showhome(false), diagTimer(0), diagGraphItem(0), showDiag(false), overlayOpacity(1) { - - OPMapWidget::OPMapWidget(QWidget *parent, Configuration *config):QGraphicsView(parent),configuration(config),UAV(0),GPS(0),Home(0) - ,followmouse(true),compass(0),showuav(false),showhome(false),diagTimer(0),diagGraphItem(0),showDiag(false),overlayOpacity(1) - { - setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); - core=new internals::Core; - map=new MapGraphicItem(core,config); - mscene.addItem(map); - this->setScene(&mscene); - Home=new HomeItem(map,this); - Home->setParentItem(map); - Home->setZValue(-1); - setStyleSheet("QToolTip {font-size:8pt; color:blue;opacity: 223; padding:2px; border-width:2px; border-style:solid; border-color: rgb(170, 170, 127);border-radius:4px }"); - this->adjustSize(); - connect(map,SIGNAL(zoomChanged(double,double,double)),this,SIGNAL(zoomChanged(double,double,double))); - connect(map->core,SIGNAL(OnCurrentPositionChanged(internals::PointLatLng)),this,SIGNAL(OnCurrentPositionChanged(internals::PointLatLng))); - connect(map->core,SIGNAL(OnEmptyTileError(int,core::Point)),this,SIGNAL(OnEmptyTileError(int,core::Point))); - connect(map->core,SIGNAL(OnMapDrag()),this,SIGNAL(OnMapDrag())); - connect(map->core,SIGNAL(OnMapTypeChanged(MapType::Types)),this,SIGNAL(OnMapTypeChanged(MapType::Types))); - connect(map->core,SIGNAL(OnMapZoomChanged()),this,SIGNAL(OnMapZoomChanged())); - connect(map->core,SIGNAL(OnTileLoadComplete()),this,SIGNAL(OnTileLoadComplete())); - connect(map->core,SIGNAL(OnTileLoadStart()),this,SIGNAL(OnTileLoadStart())); - connect(map->core,SIGNAL(OnTilesStillToLoad(int)),this,SIGNAL(OnTilesStillToLoad(int))); - connect(map,SIGNAL(wpdoubleclicked(WayPointItem*)),this,SIGNAL(OnWayPointDoubleClicked(WayPointItem*))); - connect(&mscene,SIGNAL(selectionChanged()),this,SLOT(OnSelectionChanged())); - SetShowDiagnostics(showDiag); - this->setMouseTracking(followmouse); - SetShowCompass(true); - QPixmapCache::setCacheLimit(64*1024); - } - void OPMapWidget::SetShowDiagnostics(bool const& value) - { - showDiag=value; - if(!showDiag) - { - if(diagGraphItem!=0) - { - delete diagGraphItem; - diagGraphItem=0; - } - if(diagTimer!=0) - { - delete diagTimer; - diagTimer=0; - } - - if(GPS!=0) - { - GPS->DeleteTrail(); - delete GPS; - GPS=0; - } - } - else - { - diagTimer=new QTimer(); - connect(diagTimer,SIGNAL(timeout()),this,SLOT(diagRefresh())); - diagTimer->start(500); - if(GPS==0) - { - GPS=new GPSItem(map,this); - GPS->setParentItem(map); - GPS->setOpacity(overlayOpacity); - setOverlayOpacity(overlayOpacity); - } - } - - } - void OPMapWidget::SetUavPic(QString UAVPic) - { - if(UAV!=0) - UAV->SetUavPic(UAVPic); - if(GPS!=0) - GPS->SetUavPic(UAVPic); - } - - WayPointLine * OPMapWidget::WPLineCreate(WayPointItem *from, WayPointItem *to,QColor color) - { - if(!from|!to) - return NULL; - WayPointLine* ret= new WayPointLine(from,to,map,color); - ret->setOpacity(overlayOpacity); - return ret; - } - WayPointLine * OPMapWidget::WPLineCreate(HomeItem *from, WayPointItem *to,QColor color) - { - if(!from|!to) - return NULL; - WayPointLine* ret= new WayPointLine(from,to,map,color); - ret->setOpacity(overlayOpacity); - return ret; - } - WayPointCircle * OPMapWidget::WPCircleCreate(WayPointItem *center, WayPointItem *radius, bool clockwise,QColor color) - { - if(!center|!radius) - return NULL; - WayPointCircle* ret= new WayPointCircle(center,radius,clockwise,map,color); - ret->setOpacity(overlayOpacity); - return ret; - } - - WayPointCircle *OPMapWidget::WPCircleCreate(HomeItem *center, WayPointItem *radius, bool clockwise,QColor color) - { - if(!center|!radius) - return NULL; - WayPointCircle* ret= new WayPointCircle(center,radius,clockwise,map,color); - ret->setOpacity(overlayOpacity); - return ret; - } - void OPMapWidget::SetShowUAV(const bool &value) - { - if(value && UAV==0) - { - UAV=new UAVItem(map,this); - UAV->setParentItem(map); - connect(this,SIGNAL(UAVLeftSafetyBouble(internals::PointLatLng)),UAV,SIGNAL(UAVLeftSafetyBouble(internals::PointLatLng))); - connect(this,SIGNAL(UAVReachedWayPoint(int,WayPointItem*)),UAV,SIGNAL(UAVReachedWayPoint(int,WayPointItem*))); - UAV->setOpacity(overlayOpacity); - } - else if(!value) - { - if(UAV!=0) - { - delete UAV; - UAV=NULL; - } - - } - } - void OPMapWidget::SetShowHome(const bool &value) - { - Home->setVisible(value); - } - - void OPMapWidget::resizeEvent(QResizeEvent *event) - { - if (scene()) - scene()->setSceneRect( - QRect(QPoint(0, 0), event->size())); - QGraphicsView::resizeEvent(event); - if(compass) - compass->setScale(0.1+0.05*(qreal)(event->size().width())/1000*(qreal)(event->size().height())/600); - - } - QSize OPMapWidget::sizeHint() const - { - return map->sizeHint(); - } - void OPMapWidget::showEvent(QShowEvent *event) - { - connect(&mscene,SIGNAL(sceneRectChanged(QRectF)),map,SLOT(resize(QRectF))); - map->start(); - QGraphicsView::showEvent(event); - } - OPMapWidget::~OPMapWidget() - { - if(UAV) - delete UAV; - if(Home) - delete Home; - if(map) - delete map; - if(core) - delete core; - if(configuration) - delete configuration; - foreach(QGraphicsItem* i,this->items()) - { - if(i) - delete i; - } - } - void OPMapWidget::closeEvent(QCloseEvent *event) - { - core->OnMapClose(); - event->accept(); - } - void OPMapWidget::SetUseOpenGL(const bool &value) - { - useOpenGL=value; - if (useOpenGL) - setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); - else - setupViewport(new QWidget()); - update(); - } - internals::PointLatLng OPMapWidget::currentMousePosition() - { - return currentmouseposition; - } - - void OPMapWidget::mouseMoveEvent(QMouseEvent *event) - { - QGraphicsView::mouseMoveEvent(event); - QPointF p=event->posF(); - p=map->mapFromParent(p); - currentmouseposition=map->FromLocalToLatLng(p.x(),p.y()); - } - ////////////////WAYPOINT//////////////////////// - WayPointItem* OPMapWidget::WPCreate() - { - WayPointItem* item=new WayPointItem(this->CurrentPosition(),0,map); - ConnectWP(item); - item->setParentItem(map); - int position=item->Number(); - emit WPCreated(position,item); - return item; - } - WayPointItem* OPMapWidget::magicWPCreate() - { - WayPointItem* item=new WayPointItem(map,true); - item->SetShowNumber(false); - item->setParentItem(map); - return item; - } - void OPMapWidget::WPCreate(WayPointItem* item) - { - ConnectWP(item); - item->setParentItem(map); - int position=item->Number(); - emit WPCreated(position,item); - setOverlayOpacity(overlayOpacity); - } - WayPointItem* OPMapWidget::WPCreate(internals::PointLatLng const& coord,int const& altitude) - { - WayPointItem* item=new WayPointItem(coord,altitude,map); - ConnectWP(item); - item->setParentItem(map); - int position=item->Number(); - emit WPCreated(position,item); - setOverlayOpacity(overlayOpacity); - return item; - } - WayPointItem* OPMapWidget::WPCreate(internals::PointLatLng const& coord,int const& altitude, QString const& description) - { - WayPointItem* item=new WayPointItem(coord,altitude,description,map); - ConnectWP(item); - item->setParentItem(map); - int position=item->Number(); - emit WPCreated(position,item); - setOverlayOpacity(overlayOpacity); - return item; - } - WayPointItem* OPMapWidget::WPCreate(const distBearingAltitude &relativeCoord, const QString &description) - { - WayPointItem* item=new WayPointItem(relativeCoord,description,map); - ConnectWP(item); - item->setParentItem(map); - int position=item->Number(); - emit WPCreated(position,item); - setOverlayOpacity(overlayOpacity); - return item; - } - WayPointItem* OPMapWidget::WPInsert(const int &position) - { - WayPointItem* item=new WayPointItem(this->CurrentPosition(),0,map); - item->SetNumber(position); - ConnectWP(item); - item->setParentItem(map); - emit WPInserted(position,item); - setOverlayOpacity(overlayOpacity); - return item; - } - void OPMapWidget::WPInsert(WayPointItem* item,const int &position) - { - item->SetNumber(position); - ConnectWP(item); - item->setParentItem(map); - emit WPInserted(position,item); - setOverlayOpacity(overlayOpacity); - } - WayPointItem* OPMapWidget::WPInsert(internals::PointLatLng const& coord,int const& altitude,const int &position) - { - WayPointItem* item=new WayPointItem(coord,altitude,map); - item->SetNumber(position); - ConnectWP(item); - item->setParentItem(map); - emit WPInserted(position,item); - setOverlayOpacity(overlayOpacity); - return item; - } - WayPointItem* OPMapWidget::WPInsert(internals::PointLatLng const& coord,int const& altitude, QString const& description,const int &position) - { - internals::PointLatLng mcoord; - bool reloc=false; - if(mcoord==internals::PointLatLng(0,0)) - { - mcoord=CurrentPosition(); - reloc=true; - } - else - mcoord=coord; - WayPointItem* item=new WayPointItem(mcoord,altitude,description,map); - item->SetNumber(position); - ConnectWP(item); - item->setParentItem(map); - emit WPInserted(position,item); - if(reloc) - emit WPValuesChanged(item); - setOverlayOpacity(overlayOpacity); - return item; - } - WayPointItem* OPMapWidget::WPInsert(distBearingAltitude const& relative, QString const& description,const int &position) - { - WayPointItem* item=new WayPointItem(relative,description,map); - item->SetNumber(position); - ConnectWP(item); - item->setParentItem(map); - emit WPInserted(position,item); - setOverlayOpacity(overlayOpacity); - return item; - } - void OPMapWidget::WPDelete(WayPointItem *item) - { - emit WPDeleted(item->Number(),item); - delete item; - } - void OPMapWidget::WPDelete(int number) - { - foreach(QGraphicsItem* i,map->childItems()) - { - WayPointItem* w=qgraphicsitem_cast(i); - if(w) - { - if(w->Number()==number) - { - emit WPDeleted(w->Number(),w); - delete w; - return; - } - } - } - } - WayPointItem * OPMapWidget::WPFind(int number) - { - foreach(QGraphicsItem* i,map->childItems()) - { - WayPointItem* w=qgraphicsitem_cast(i); - if(w) - { - if(w->Number()==number) - { - return w; - } - } - } - return NULL; - } - void OPMapWidget::WPSetVisibleAll(bool value) - { - foreach(QGraphicsItem* i,map->childItems()) - { - WayPointItem* w=qgraphicsitem_cast(i); - if(w) - { - if(w->Number()!=-1) - w->setVisible(value); - } - } - } - void OPMapWidget::WPDeleteAll() - { - foreach(QGraphicsItem* i,map->childItems()) - { - WayPointItem* w=qgraphicsitem_cast(i); - if(w) - { - if(w->Number()!=-1) - { - emit WPDeleted(w->Number(),w); - delete w; - } - } - } - } - bool OPMapWidget::WPPresent() - { - foreach(QGraphicsItem* i,map->childItems()) - { - WayPointItem* w=qgraphicsitem_cast(i); - if(w) - { - if(w->Number()!=-1) - { - return true; - } - } - } - return false; - } - - void OPMapWidget::deleteAllOverlays() - { - foreach(QGraphicsItem* i,map->childItems()) - { - WayPointLine* w=qgraphicsitem_cast(i); - if(w) - w->deleteLater(); - else - { - WayPointCircle* ww=qgraphicsitem_cast(i); - if(ww) - ww->deleteLater(); - } - } - } - QList OPMapWidget::WPSelected() - { - QList list; - foreach(QGraphicsItem* i,mscene.selectedItems()) - { - WayPointItem* w=qgraphicsitem_cast(i); - if(w) - list.append(w); - } - return list; - } - void OPMapWidget::WPRenumber(WayPointItem *item, const int &newnumber) - { - item->SetNumber(newnumber); - } - - void OPMapWidget::ConnectWP(WayPointItem *item) - { - connect(item,SIGNAL(WPNumberChanged(int,int,WayPointItem*)),this,SIGNAL(WPNumberChanged(int,int,WayPointItem*)),Qt::DirectConnection); - connect(item,SIGNAL(WPValuesChanged(WayPointItem*)),this,SIGNAL(WPValuesChanged(WayPointItem*)),Qt::DirectConnection); - connect(item,SIGNAL(localPositionChanged(QPointF,WayPointItem*)),this,SIGNAL(WPLocalPositionChanged(QPointF,WayPointItem*)),Qt::DirectConnection); - connect(item,SIGNAL(manualCoordChange(WayPointItem*)),this,SIGNAL(WPManualCoordChange(WayPointItem*)),Qt::DirectConnection); - connect(this,SIGNAL(WPInserted(int,WayPointItem*)),item,SLOT(WPInserted(int,WayPointItem*)),Qt::DirectConnection); - connect(this,SIGNAL(WPNumberChanged(int,int,WayPointItem*)),item,SLOT(WPRenumbered(int,int,WayPointItem*)),Qt::DirectConnection); - connect(this,SIGNAL(WPDeleted(int,WayPointItem*)),item,SLOT(WPDeleted(int,WayPointItem*)),Qt::DirectConnection); - } - void OPMapWidget::diagRefresh() - { - if(showDiag) - { - if(diagGraphItem==0) - { - diagGraphItem=new QGraphicsTextItem(); - mscene.addItem(diagGraphItem); - diagGraphItem->setPos(10,100); - diagGraphItem->setZValue(3); - diagGraphItem->setFlag(QGraphicsItem::ItemIsMovable,true); - diagGraphItem->setDefaultTextColor(Qt::yellow); - } - diagGraphItem->setPlainText(core->GetDiagnostics().toString()); - } - else - if(diagGraphItem!=0) - { + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); + core = new internals::Core; + map = new MapGraphicItem(core, config); + mscene.addItem(map); + this->setScene(&mscene); + Home = new HomeItem(map, this); + Home->setParentItem(map); + Home->setZValue(-1); + setStyleSheet("QToolTip {font-size:8pt; color:blue;opacity: 223; padding:2px; border-width:2px; border-style:solid; border-color: rgb(170, 170, 127);border-radius:4px }"); + this->adjustSize(); + connect(map, SIGNAL(zoomChanged(double, double, double)), this, SIGNAL(zoomChanged(double, double, double))); + connect(map->core, SIGNAL(OnCurrentPositionChanged(internals::PointLatLng)), this, SIGNAL(OnCurrentPositionChanged(internals::PointLatLng))); + connect(map->core, SIGNAL(OnEmptyTileError(int, core::Point)), this, SIGNAL(OnEmptyTileError(int, core::Point))); + connect(map->core, SIGNAL(OnMapDrag()), this, SIGNAL(OnMapDrag())); + connect(map->core, SIGNAL(OnMapTypeChanged(MapType::Types)), this, SIGNAL(OnMapTypeChanged(MapType::Types))); + connect(map->core, SIGNAL(OnMapZoomChanged()), this, SIGNAL(OnMapZoomChanged())); + connect(map->core, SIGNAL(OnTileLoadComplete()), this, SIGNAL(OnTileLoadComplete())); + connect(map->core, SIGNAL(OnTileLoadStart()), this, SIGNAL(OnTileLoadStart())); + connect(map->core, SIGNAL(OnTilesStillToLoad(int)), this, SIGNAL(OnTilesStillToLoad(int))); + connect(map, SIGNAL(wpdoubleclicked(WayPointItem *)), this, SIGNAL(OnWayPointDoubleClicked(WayPointItem *))); + connect(&mscene, SIGNAL(selectionChanged()), this, SLOT(OnSelectionChanged())); + SetShowDiagnostics(showDiag); + this->setMouseTracking(followmouse); + SetShowCompass(true); + QPixmapCache::setCacheLimit(64 * 1024); +} +void OPMapWidget::SetShowDiagnostics(bool const & value) +{ + showDiag = value; + if (!showDiag) { + if (diagGraphItem != 0) { delete diagGraphItem; - diagGraphItem=0; + diagGraphItem = 0; + } + if (diagTimer != 0) { + delete diagTimer; + diagTimer = 0; } - } - ////////////////////////////////////////////// - void OPMapWidget::SetShowCompass(const bool &value) - { - if(value && !compass) - { - compass=new QGraphicsSvgItem(QString::fromUtf8(":/markers/images/compas.svg")); - compass->setScale(0.1+0.05*(qreal)(this->size().width())/1000*(qreal)(this->size().height())/600); - // compass->setTransformOriginPoint(compass->boundingRect().width(),compass->boundingRect().height()); - compass->setFlag(QGraphicsItem::ItemIsMovable,true); - compass->setFlag(QGraphicsItem::ItemIsSelectable,true); - mscene.addItem(compass); - compass->setTransformOriginPoint(compass->boundingRect().width()/2,compass->boundingRect().height()/2); - compass->setPos(55-compass->boundingRect().width()/2,55-compass->boundingRect().height()/2); - compass->setZValue(3); - compass->setOpacity(0.7); + if (GPS != 0) { + GPS->DeleteTrail(); + delete GPS; + GPS = 0; } - if(!value && compass) - { - delete compass; - compass=0; + } else { + diagTimer = new QTimer(); + connect(diagTimer, SIGNAL(timeout()), this, SLOT(diagRefresh())); + diagTimer->start(500); + if (GPS == 0) { + GPS = new GPSItem(map, this); + GPS->setParentItem(map); + GPS->setOpacity(overlayOpacity); + setOverlayOpacity(overlayOpacity); } } - - void OPMapWidget::setOverlayOpacity(qreal value) - { - map->setOverlayOpacity(value); - overlayOpacity=value; - } - void OPMapWidget::SetRotate(qreal const& value) - { - map->mapRotate(value); - if(compass && (compass->rotation() != value)) { - compass->setRotation(value); - } - } - void OPMapWidget::RipMap() - { - new MapRipper(core,map->SelectedArea()); - } - - void OPMapWidget::setSelectedWP(QListlist) - { - this->scene()->clearSelection(); - foreach(WayPointItem * wp,list) - { - wp->setSelected(true); - } - } - - void OPMapWidget::OnSelectionChanged() - { - QList list; - QList wplist; - list=this->scene()->selectedItems(); - foreach(QGraphicsItem* item,list) - { - WayPointItem * wp=qgraphicsitem_cast(item); - if(wp) - wplist.append(wp); - } - if(wplist.length()>0) - emit selectedWPChanged(wplist); - } +} +void OPMapWidget::SetUavPic(QString UAVPic) +{ + if (UAV != 0) { + UAV->SetUavPic(UAVPic); + } + if (GPS != 0) { + GPS->SetUavPic(UAVPic); + } +} + +WayPointLine *OPMapWidget::WPLineCreate(WayPointItem *from, WayPointItem *to, QColor color) +{ + if (!from | !to) { + return NULL; + } + WayPointLine *ret = new WayPointLine(from, to, map, color); + ret->setOpacity(overlayOpacity); + return ret; +} +WayPointLine *OPMapWidget::WPLineCreate(HomeItem *from, WayPointItem *to, QColor color) +{ + if (!from | !to) { + return NULL; + } + WayPointLine *ret = new WayPointLine(from, to, map, color); + ret->setOpacity(overlayOpacity); + return ret; +} +WayPointCircle *OPMapWidget::WPCircleCreate(WayPointItem *center, WayPointItem *radius, bool clockwise, QColor color) +{ + if (!center | !radius) { + return NULL; + } + WayPointCircle *ret = new WayPointCircle(center, radius, clockwise, map, color); + ret->setOpacity(overlayOpacity); + return ret; +} + +WayPointCircle *OPMapWidget::WPCircleCreate(HomeItem *center, WayPointItem *radius, bool clockwise, QColor color) +{ + if (!center | !radius) { + return NULL; + } + WayPointCircle *ret = new WayPointCircle(center, radius, clockwise, map, color); + ret->setOpacity(overlayOpacity); + return ret; +} +void OPMapWidget::SetShowUAV(const bool &value) +{ + if (value && UAV == 0) { + UAV = new UAVItem(map, this); + UAV->setParentItem(map); + connect(this, SIGNAL(UAVLeftSafetyBouble(internals::PointLatLng)), UAV, SIGNAL(UAVLeftSafetyBouble(internals::PointLatLng))); + connect(this, SIGNAL(UAVReachedWayPoint(int, WayPointItem *)), UAV, SIGNAL(UAVReachedWayPoint(int, WayPointItem *))); + UAV->setOpacity(overlayOpacity); + } else if (!value) { + if (UAV != 0) { + delete UAV; + UAV = NULL; + } + } +} +void OPMapWidget::SetShowHome(const bool &value) +{ + Home->setVisible(value); +} + +void OPMapWidget::resizeEvent(QResizeEvent *event) +{ + if (scene()) { + scene()->setSceneRect( + QRect(QPoint(0, 0), event->size())); + } + QGraphicsView::resizeEvent(event); + if (compass) { + compass->setScale(0.1 + 0.05 * (qreal)(event->size().width()) / 1000 * (qreal)(event->size().height()) / 600); + } +} +QSize OPMapWidget::sizeHint() const +{ + return map->sizeHint(); +} +void OPMapWidget::showEvent(QShowEvent *event) +{ + connect(&mscene, SIGNAL(sceneRectChanged(QRectF)), map, SLOT(resize(QRectF))); + map->start(); + QGraphicsView::showEvent(event); +} +OPMapWidget::~OPMapWidget() +{ + if (UAV) { + delete UAV; + } + if (Home) { + delete Home; + } + if (map) { + delete map; + } + if (core) { + delete core; + } + if (configuration) { + delete configuration; + } + foreach(QGraphicsItem * i, this->items()) { + if (i) { + delete i; + } + } +} +void OPMapWidget::closeEvent(QCloseEvent *event) +{ + core->OnMapClose(); + event->accept(); +} +void OPMapWidget::SetUseOpenGL(const bool &value) +{ + useOpenGL = value; + if (useOpenGL) { + setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); + } else { + setupViewport(new QWidget()); + } + update(); +} +internals::PointLatLng OPMapWidget::currentMousePosition() +{ + return currentmouseposition; +} + +void OPMapWidget::mouseMoveEvent(QMouseEvent *event) +{ + QGraphicsView::mouseMoveEvent(event); + QPointF p = event->posF(); + + p = map->mapFromParent(p); + currentmouseposition = map->FromLocalToLatLng(p.x(), p.y()); +} +////////////////WAYPOINT//////////////////////// +WayPointItem *OPMapWidget::WPCreate() +{ + WayPointItem *item = new WayPointItem(this->CurrentPosition(), 0, map); + + ConnectWP(item); + item->setParentItem(map); + int position = item->Number(); + emit WPCreated(position, item); + return item; +} +WayPointItem *OPMapWidget::magicWPCreate() +{ + WayPointItem *item = new WayPointItem(map, true); + + item->SetShowNumber(false); + item->setParentItem(map); + return item; +} +void OPMapWidget::WPCreate(WayPointItem *item) +{ + ConnectWP(item); + item->setParentItem(map); + int position = item->Number(); + emit WPCreated(position, item); + setOverlayOpacity(overlayOpacity); +} +WayPointItem *OPMapWidget::WPCreate(internals::PointLatLng const & coord, int const & altitude) +{ + WayPointItem *item = new WayPointItem(coord, altitude, map); + + ConnectWP(item); + item->setParentItem(map); + int position = item->Number(); + emit WPCreated(position, item); + setOverlayOpacity(overlayOpacity); + return item; +} +WayPointItem *OPMapWidget::WPCreate(internals::PointLatLng const & coord, int const & altitude, QString const & description) +{ + WayPointItem *item = new WayPointItem(coord, altitude, description, map); + + ConnectWP(item); + item->setParentItem(map); + int position = item->Number(); + emit WPCreated(position, item); + setOverlayOpacity(overlayOpacity); + return item; +} +WayPointItem *OPMapWidget::WPCreate(const distBearingAltitude &relativeCoord, const QString &description) +{ + WayPointItem *item = new WayPointItem(relativeCoord, description, map); + + ConnectWP(item); + item->setParentItem(map); + int position = item->Number(); + emit WPCreated(position, item); + setOverlayOpacity(overlayOpacity); + return item; +} +WayPointItem *OPMapWidget::WPInsert(const int &position) +{ + WayPointItem *item = new WayPointItem(this->CurrentPosition(), 0, map); + + item->SetNumber(position); + ConnectWP(item); + item->setParentItem(map); + emit WPInserted(position, item); + setOverlayOpacity(overlayOpacity); + return item; +} +void OPMapWidget::WPInsert(WayPointItem *item, const int &position) +{ + item->SetNumber(position); + ConnectWP(item); + item->setParentItem(map); + emit WPInserted(position, item); + setOverlayOpacity(overlayOpacity); +} +WayPointItem *OPMapWidget::WPInsert(internals::PointLatLng const & coord, int const & altitude, const int &position) +{ + WayPointItem *item = new WayPointItem(coord, altitude, map); + + item->SetNumber(position); + ConnectWP(item); + item->setParentItem(map); + emit WPInserted(position, item); + setOverlayOpacity(overlayOpacity); + return item; +} +WayPointItem *OPMapWidget::WPInsert(internals::PointLatLng const & coord, int const & altitude, QString const & description, const int &position) +{ + internals::PointLatLng mcoord; + bool reloc = false; + + if (mcoord == internals::PointLatLng(0, 0)) { + mcoord = CurrentPosition(); + reloc = true; + } else { + mcoord = coord; + } + WayPointItem *item = new WayPointItem(mcoord, altitude, description, map); + item->SetNumber(position); + ConnectWP(item); + item->setParentItem(map); + emit WPInserted(position, item); + if (reloc) { + emit WPValuesChanged(item); + } + setOverlayOpacity(overlayOpacity); + return item; +} +WayPointItem *OPMapWidget::WPInsert(distBearingAltitude const & relative, QString const & description, const int &position) +{ + WayPointItem *item = new WayPointItem(relative, description, map); + + item->SetNumber(position); + ConnectWP(item); + item->setParentItem(map); + emit WPInserted(position, item); + setOverlayOpacity(overlayOpacity); + return item; +} +void OPMapWidget::WPDelete(WayPointItem *item) +{ + emit WPDeleted(item->Number(), item); + + delete item; +} +void OPMapWidget::WPDelete(int number) +{ + foreach(QGraphicsItem * i, map->childItems()) { + WayPointItem *w = qgraphicsitem_cast(i); + + if (w) { + if (w->Number() == number) { + emit WPDeleted(w->Number(), w); + delete w; + return; + } + } + } +} +WayPointItem *OPMapWidget::WPFind(int number) +{ + foreach(QGraphicsItem * i, map->childItems()) { + WayPointItem *w = qgraphicsitem_cast(i); + + if (w) { + if (w->Number() == number) { + return w; + } + } + } + return NULL; +} +void OPMapWidget::WPSetVisibleAll(bool value) +{ + foreach(QGraphicsItem * i, map->childItems()) { + WayPointItem *w = qgraphicsitem_cast(i); + + if (w) { + if (w->Number() != -1) { + w->setVisible(value); + } + } + } +} +void OPMapWidget::WPDeleteAll() +{ + foreach(QGraphicsItem * i, map->childItems()) { + WayPointItem *w = qgraphicsitem_cast(i); + + if (w) { + if (w->Number() != -1) { + emit WPDeleted(w->Number(), w); + delete w; + } + } + } +} +bool OPMapWidget::WPPresent() +{ + foreach(QGraphicsItem * i, map->childItems()) { + WayPointItem *w = qgraphicsitem_cast(i); + + if (w) { + if (w->Number() != -1) { + return true; + } + } + } + return false; +} + +void OPMapWidget::deleteAllOverlays() +{ + foreach(QGraphicsItem * i, map->childItems()) { + WayPointLine *w = qgraphicsitem_cast(i); + + if (w) { + w->deleteLater(); + } else { + WayPointCircle *ww = qgraphicsitem_cast(i); + if (ww) { + ww->deleteLater(); + } + } + } +} +QList OPMapWidget::WPSelected() +{ + QList list; + foreach(QGraphicsItem * i, mscene.selectedItems()) { + WayPointItem *w = qgraphicsitem_cast(i); + + if (w) { + list.append(w); + } + } + return list; +} +void OPMapWidget::WPRenumber(WayPointItem *item, const int &newnumber) +{ + item->SetNumber(newnumber); +} + +void OPMapWidget::ConnectWP(WayPointItem *item) +{ + connect(item, SIGNAL(WPNumberChanged(int, int, WayPointItem *)), this, SIGNAL(WPNumberChanged(int, int, WayPointItem *)), Qt::DirectConnection); + connect(item, SIGNAL(WPValuesChanged(WayPointItem *)), this, SIGNAL(WPValuesChanged(WayPointItem *)), Qt::DirectConnection); + connect(item, SIGNAL(localPositionChanged(QPointF, WayPointItem *)), this, SIGNAL(WPLocalPositionChanged(QPointF, WayPointItem *)), Qt::DirectConnection); + connect(item, SIGNAL(manualCoordChange(WayPointItem *)), this, SIGNAL(WPManualCoordChange(WayPointItem *)), Qt::DirectConnection); + connect(this, SIGNAL(WPInserted(int, WayPointItem *)), item, SLOT(WPInserted(int, WayPointItem *)), Qt::DirectConnection); + connect(this, SIGNAL(WPNumberChanged(int, int, WayPointItem *)), item, SLOT(WPRenumbered(int, int, WayPointItem *)), Qt::DirectConnection); + connect(this, SIGNAL(WPDeleted(int, WayPointItem *)), item, SLOT(WPDeleted(int, WayPointItem *)), Qt::DirectConnection); +} +void OPMapWidget::diagRefresh() +{ + if (showDiag) { + if (diagGraphItem == 0) { + diagGraphItem = new QGraphicsTextItem(); + mscene.addItem(diagGraphItem); + diagGraphItem->setPos(10, 100); + diagGraphItem->setZValue(3); + diagGraphItem->setFlag(QGraphicsItem::ItemIsMovable, true); + diagGraphItem->setDefaultTextColor(Qt::yellow); + } + diagGraphItem->setPlainText(core->GetDiagnostics().toString()); + } else if (diagGraphItem != 0) { + delete diagGraphItem; + diagGraphItem = 0; + } +} + +////////////////////////////////////////////// +void OPMapWidget::SetShowCompass(const bool &value) +{ + if (value && !compass) { + compass = new QGraphicsSvgItem(QString::fromUtf8(":/markers/images/compas.svg")); + compass->setScale(0.1 + 0.05 * (qreal)(this->size().width()) / 1000 * (qreal)(this->size().height()) / 600); + // compass->setTransformOriginPoint(compass->boundingRect().width(),compass->boundingRect().height()); + compass->setFlag(QGraphicsItem::ItemIsMovable, true); + compass->setFlag(QGraphicsItem::ItemIsSelectable, true); + mscene.addItem(compass); + compass->setTransformOriginPoint(compass->boundingRect().width() / 2, compass->boundingRect().height() / 2); + compass->setPos(55 - compass->boundingRect().width() / 2, 55 - compass->boundingRect().height() / 2); + compass->setZValue(3); + compass->setOpacity(0.7); + } + if (!value && compass) { + delete compass; + compass = 0; + } +} + +void OPMapWidget::setOverlayOpacity(qreal value) +{ + map->setOverlayOpacity(value); + overlayOpacity = value; +} +void OPMapWidget::SetRotate(qreal const & value) +{ + map->mapRotate(value); + if (compass && (compass->rotation() != value)) { + compass->setRotation(value); + } +} +void OPMapWidget::RipMap() +{ + new MapRipper(core, map->SelectedArea()); +} + +void OPMapWidget::setSelectedWP(QListlist) +{ + this->scene()->clearSelection(); + foreach(WayPointItem * wp, list) { + wp->setSelected(true); + } +} + +void OPMapWidget::OnSelectionChanged() +{ + QList list; + QList wplist; + list = this->scene()->selectedItems(); + foreach(QGraphicsItem * item, list) { + WayPointItem *wp = qgraphicsitem_cast(item); + + if (wp) { + wplist.append(wp); + } + } + if (wplist.length() > 0) { + emit selectedWPChanged(wplist); + } +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h index 020f3831b..f427a560c 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file opmapwidget.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief The Map Widget, this is the part exposed to the user -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file opmapwidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief The Map Widget, this is the part exposed to the user + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef OPMAPWIDGET_H #define OPMAPWIDGET_H @@ -44,473 +44,618 @@ #include "waypointline.h" #include "waypointcircle.h" #include "waypointitem.h" -namespace mapcontrol -{ - class UAVItem; - class GPSItem; - class HomeItem; +namespace mapcontrol { +class UAVItem; +class GPSItem; +class HomeItem; +/** + * @brief Collection of static functions to help dealing with various enums used + * Contains functions for enumToString conversio, StringToEnum, QStringList of enum values... + * + * @class Helper opmapwidget.h "opmapwidget.h" + */ +class Helper { +public: /** - * @brief Collection of static functions to help dealing with various enums used - * Contains functions for enumToString conversio, StringToEnum, QStringList of enum values... - * - * @class Helper opmapwidget.h "opmapwidget.h" - */ - class Helper + * @brief Converts from String to Type + * + * @param value String to convert + * @return + */ + static MapType::Types MapTypeFromString(QString const & value) { - public: - /** - * @brief Converts from String to Type - * - * @param value String to convert - * @return - */ - static MapType::Types MapTypeFromString(QString const& value){return MapType::TypeByStr(value);} - /** - * @brief Converts from Type to String - */ - static QString StrFromMapType(MapType::Types const& value){return MapType::StrByType(value);} - /** - * @brief Returns QStringList with string representing all the enum values - */ - static QStringList MapTypes(){return MapType::TypesList();} - - /** - * @brief Converts from String to Type - */ - static GeoCoderStatusCode::Types GeoCoderStatusCodeFromString(QString const& value){return GeoCoderStatusCode::TypeByStr(value);} - /** - * @brief Converts from Type to String - */ - static QString StrFromGeoCoderStatusCode(GeoCoderStatusCode::Types const& value){return GeoCoderStatusCode::StrByType(value);} - /** - * @brief Returns QStringList with string representing all the enum values - */ - static QStringList GeoCoderTypes(){return GeoCoderStatusCode::TypesList();} - - /** - * @brief Converts from String to Type - */ - static internals::MouseWheelZoomType::Types MouseWheelZoomTypeFromString(QString const& value){return internals::MouseWheelZoomType::TypeByStr(value);} - /** - * @brief Converts from Type to String - */ - static QString StrFromMouseWheelZoomType(internals::MouseWheelZoomType::Types const& value){return internals::MouseWheelZoomType::StrByType(value);} - /** - * @brief Returns QStringList with string representing all the enum values - */ - static QStringList MouseWheelZoomTypes(){return internals::MouseWheelZoomType::TypesList();} - /** - * @brief Converts from String to Type - */ - static core::LanguageType::Types LanguageTypeFromString(QString const& value){return core::LanguageType::TypeByStr(value);} - /** - * @brief Converts from Type to String - */ - static QString StrFromLanguageType(core::LanguageType::Types const& value){return core::LanguageType::StrByType(value);} - /** - * @brief Returns QStringList with string representing all the enum values - */ - static QStringList LanguageTypes(){return core::LanguageType::TypesList();} - /** - * @brief Converts from String to Type - */ - static core::AccessMode::Types AccessModeFromString(QString const& value){return core::AccessMode::TypeByStr(value);} - /** - * @brief Converts from Type to String - */ - static QString StrFromAccessMode(core::AccessMode::Types const& value){return core::AccessMode::StrByType(value);} - /** - * @brief Returns QStringList with string representing all the enum values - */ - static QStringList AccessModeTypes(){return core::AccessMode::TypesList();} - - /** - * @brief Converts from String to Type - */ - static UAVMapFollowType::Types UAVMapFollowFromString(QString const& value){return UAVMapFollowType::TypeByStr(value);} - /** - * @brief Converts from Type to String - */ - static QString StrFromUAVMapFollow(UAVMapFollowType::Types const& value){return UAVMapFollowType::StrByType(value);} - /** - * @brief Returns QStringList with string representing all the enum values - */ - static QStringList UAVMapFollowTypes(){return UAVMapFollowType::TypesList();} - /** - * @brief Converts from String to Type - */ - static UAVTrailType::Types UAVTrailTypeFromString(QString const& value){return UAVTrailType::TypeByStr(value);} - /** - * @brief Converts from Type to String - */ - static QString StrFromUAVTrailType(UAVTrailType::Types const& value){return UAVTrailType::StrByType(value);} - /** - * @brief Returns QStringList with string representing all the enum values - */ - static QStringList UAVTrailTypes(){return UAVTrailType::TypesList();} - }; - - class OPMapWidget:public QGraphicsView + return MapType::TypeByStr(value); + } + /** + * @brief Converts from Type to String + */ + static QString StrFromMapType(MapType::Types const & value) { - Q_OBJECT + return MapType::StrByType(value); + } + /** + * @brief Returns QStringList with string representing all the enum values + */ + static QStringList MapTypes() + { + return MapType::TypesList(); + } - // Q_PROPERTY(int MaxZoom READ MaxZoom WRITE SetMaxZoom) - Q_PROPERTY(int MinZoom READ MinZoom WRITE SetMinZoom) - Q_PROPERTY(bool ShowTileGridLines READ ShowTileGridLines WRITE SetShowTileGridLines) - Q_PROPERTY(double Zoom READ ZoomTotal WRITE SetZoom) - Q_PROPERTY(qreal Rotate READ Rotate WRITE SetRotate) - Q_ENUMS(internals::MouseWheelZoomType::Types) - Q_ENUMS(internals::GeoCoderStatusCode::Types) + /** + * @brief Converts from String to Type + */ + static GeoCoderStatusCode::Types GeoCoderStatusCodeFromString(QString const & value) + { + return GeoCoderStatusCode::TypeByStr(value); + } + /** + * @brief Converts from Type to String + */ + static QString StrFromGeoCoderStatusCode(GeoCoderStatusCode::Types const & value) + { + return GeoCoderStatusCode::StrByType(value); + } + /** + * @brief Returns QStringList with string representing all the enum values + */ + static QStringList GeoCoderTypes() + { + return GeoCoderStatusCode::TypesList(); + } - public: - QSize sizeHint() const; - /** - * @brief Constructor - * - * @param parent parent widget - * @param config pointer to configuration classed to be used - * @return - */ - OPMapWidget(QWidget *parent=0,Configuration *config=new Configuration); - ~OPMapWidget(); + /** + * @brief Converts from String to Type + */ + static internals::MouseWheelZoomType::Types MouseWheelZoomTypeFromString(QString const & value) + { + return internals::MouseWheelZoomType::TypeByStr(value); + } + /** + * @brief Converts from Type to String + */ + static QString StrFromMouseWheelZoomType(internals::MouseWheelZoomType::Types const & value) + { + return internals::MouseWheelZoomType::StrByType(value); + } + /** + * @brief Returns QStringList with string representing all the enum values + */ + static QStringList MouseWheelZoomTypes() + { + return internals::MouseWheelZoomType::TypesList(); + } + /** + * @brief Converts from String to Type + */ + static core::LanguageType::Types LanguageTypeFromString(QString const & value) + { + return core::LanguageType::TypeByStr(value); + } + /** + * @brief Converts from Type to String + */ + static QString StrFromLanguageType(core::LanguageType::Types const & value) + { + return core::LanguageType::StrByType(value); + } + /** + * @brief Returns QStringList with string representing all the enum values + */ + static QStringList LanguageTypes() + { + return core::LanguageType::TypesList(); + } + /** + * @brief Converts from String to Type + */ + static core::AccessMode::Types AccessModeFromString(QString const & value) + { + return core::AccessMode::TypeByStr(value); + } + /** + * @brief Converts from Type to String + */ + static QString StrFromAccessMode(core::AccessMode::Types const & value) + { + return core::AccessMode::StrByType(value); + } + /** + * @brief Returns QStringList with string representing all the enum values + */ + static QStringList AccessModeTypes() + { + return core::AccessMode::TypesList(); + } - /** - * @brief Returns true if map is showing gridlines - * - * @return bool - */ - bool ShowTileGridLines()const {return map->showTileGridLines;} + /** + * @brief Converts from String to Type + */ + static UAVMapFollowType::Types UAVMapFollowFromString(QString const & value) + { + return UAVMapFollowType::TypeByStr(value); + } + /** + * @brief Converts from Type to String + */ + static QString StrFromUAVMapFollow(UAVMapFollowType::Types const & value) + { + return UAVMapFollowType::StrByType(value); + } + /** + * @brief Returns QStringList with string representing all the enum values + */ + static QStringList UAVMapFollowTypes() + { + return UAVMapFollowType::TypesList(); + } + /** + * @brief Converts from String to Type + */ + static UAVTrailType::Types UAVTrailTypeFromString(QString const & value) + { + return UAVTrailType::TypeByStr(value); + } + /** + * @brief Converts from Type to String + */ + static QString StrFromUAVTrailType(UAVTrailType::Types const & value) + { + return UAVTrailType::StrByType(value); + } + /** + * @brief Returns QStringList with string representing all the enum values + */ + static QStringList UAVTrailTypes() + { + return UAVTrailType::TypesList(); + } +}; - /** - * @brief Defines if map is to show gridlines - * - * @param value - * @return - */ - void SetShowTileGridLines(bool const& value){map->showTileGridLines=value;map->update();} +class OPMapWidget : public QGraphicsView { + Q_OBJECT + // Q_PROPERTY(int MaxZoom READ MaxZoom WRITE SetMaxZoom) + Q_PROPERTY(int MinZoom READ MinZoom WRITE SetMinZoom) + Q_PROPERTY(bool ShowTileGridLines READ ShowTileGridLines WRITE SetShowTileGridLines) + Q_PROPERTY(double Zoom READ ZoomTotal WRITE SetZoom) + Q_PROPERTY(qreal Rotate READ Rotate WRITE SetRotate) + Q_ENUMS(internals::MouseWheelZoomType::Types) + Q_ENUMS(internals::GeoCoderStatusCode::Types) - /** - * @brief Returns the maximum zoom for the map - * - */ - int MaxZoom()const{return map->MaxZoom();} +public: + QSize sizeHint() const; + /** + * @brief Constructor + * + * @param parent parent widget + * @param config pointer to configuration classed to be used + * @return + */ + OPMapWidget(QWidget *parent = 0, Configuration *config = new Configuration); + ~OPMapWidget(); - // void SetMaxZoom(int const& value){map->maxZoom = value;} + /** + * @brief Returns true if map is showing gridlines + * + * @return bool + */ + bool ShowTileGridLines() const + { + return map->showTileGridLines; + } - /** - * @brief - * - */ - int MinZoom()const{return map->minZoom;} - /** - * @brief - * - * @param value - */ - void SetMinZoom(int const& value){map->minZoom = value;} + /** + * @brief Defines if map is to show gridlines + * + * @param value + * @return + */ + void SetShowTileGridLines(bool const & value) + { + map->showTileGridLines = value; map->update(); + } - internals::MouseWheelZoomType::Types GetMouseWheelZoomType(){return map->core->GetMouseWheelZoomType();} - void SetMouseWheelZoomType(internals::MouseWheelZoomType::Types const& value){map->core->SetMouseWheelZoomType(value);} - // void SetMouseWheelZoomTypeByStr(const QString &value){map->core->SetMouseWheelZoomType(internals::MouseWheelZoomType::TypeByStr(value));} - // QString GetMouseWheelZoomTypeStr(){return map->GetMouseWheelZoomTypeStr();} + /** + * @brief Returns the maximum zoom for the map + * + */ + int MaxZoom() const + { + return map->MaxZoom(); + } - internals::RectLatLng SelectedArea()const{return map->selectedArea;} - void SetSelectedArea(internals::RectLatLng const& value){ map->selectedArea = value;this->update();} + // void SetMaxZoom(int const& value){map->maxZoom = value;} - bool CanDragMap()const{return map->CanDragMap();} - void SetCanDragMap(bool const& value){map->SetCanDragMap(value);} + /** + * @brief + * + */ + int MinZoom() const + { + return map->minZoom; + } + /** + * @brief + * + * @param value + */ + void SetMinZoom(int const & value) + { + map->minZoom = value; + } - internals::PointLatLng CurrentPosition()const{return map->core->CurrentPosition();} - void SetCurrentPosition(internals::PointLatLng const& value){map->core->SetCurrentPosition(value);} + internals::MouseWheelZoomType::Types GetMouseWheelZoomType() + { + return map->core->GetMouseWheelZoomType(); + } + void SetMouseWheelZoomType(internals::MouseWheelZoomType::Types const & value) + { + map->core->SetMouseWheelZoomType(value); + } + // void SetMouseWheelZoomTypeByStr(const QString &value){map->core->SetMouseWheelZoomType(internals::MouseWheelZoomType::TypeByStr(value));} + // QString GetMouseWheelZoomTypeStr(){return map->GetMouseWheelZoomTypeStr();} - double ZoomReal(){return map->Zoom();} - double ZoomDigi(){return map->ZoomDigi();} - double ZoomTotal(){return map->ZoomTotal();} - void SetZoom(double const& value){map->SetZoom(value);} + internals::RectLatLng SelectedArea() const + { + return map->selectedArea; + } + void SetSelectedArea(internals::RectLatLng const & value) + { + map->selectedArea = value; this->update(); + } - qreal Rotate(){return map->rotation;} - void SetRotate(qreal const& value); + bool CanDragMap() const + { + return map->CanDragMap(); + } + void SetCanDragMap(bool const & value) + { + map->SetCanDragMap(value); + } - void ReloadMap(){map->ReloadMap(); map->resize();} + internals::PointLatLng CurrentPosition() const + { + return map->core->CurrentPosition(); + } + void SetCurrentPosition(internals::PointLatLng const & value) + { + map->core->SetCurrentPosition(value); + } - GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const& keys){return map->SetCurrentPositionByKeywords(keys);} + double ZoomReal() + { + return map->Zoom(); + } + double ZoomDigi() + { + return map->ZoomDigi(); + } + double ZoomTotal() + { + return map->ZoomTotal(); + } + void SetZoom(double const & value) + { + map->SetZoom(value); + } - bool UseOpenGL(){return useOpenGL;} - void SetUseOpenGL(bool const& value); + qreal Rotate() + { + return map->rotation; + } + void SetRotate(qreal const & value); - MapType::Types GetMapType(){return map->core->GetMapType();} - void SetMapType(MapType::Types const& value){map->lastimage=QImage(); map->core->SetMapType(value);} + void ReloadMap() + { + map->ReloadMap(); map->resize(); + } - bool isStarted(){return map->core->isStarted();} + GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const & keys) + { + return map->SetCurrentPositionByKeywords(keys); + } - Configuration* configuration; + bool UseOpenGL() + { + return useOpenGL; + } + void SetUseOpenGL(bool const & value); - internals::PointLatLng currentMousePosition(); + MapType::Types GetMapType() + { + return map->core->GetMapType(); + } + void SetMapType(MapType::Types const & value) + { + map->lastimage = QImage(); map->core->SetMapType(value); + } - void SetFollowMouse(bool const& value){followmouse=value;this->setMouseTracking(followmouse);} - bool FollowMouse(){return followmouse;} + bool isStarted() + { + return map->core->isStarted(); + } - internals::PointLatLng GetFromLocalToLatLng(QPointF p) {return map->FromLocalToLatLng(p.x(),p.y());} + Configuration *configuration; - /** - * @brief Creates a new WayPoint on the center of the map - * - * @return WayPointItem a pointer to the WayPoint created - */ - WayPointItem* WPCreate(); - /** - * @brief Creates a new WayPoint - * - * @param item the WayPoint to create - */ - void WPCreate(WayPointItem* item); - /** - * @brief Creates a new WayPoint - * - * @param coord the coordinates in LatLng of the WayPoint - * @param altitude the Altitude of the WayPoint - * @return WayPointItem a pointer to the WayPoint created - */ - WayPointItem* WPCreate(internals::PointLatLng const& coord,int const& altitude); - /** - * @brief Creates a new WayPoint - * - * @param coord the coordinates in LatLng of the WayPoint - * @param altitude the Altitude of the WayPoint - * @param description the description of the WayPoint - * @return WayPointItem a pointer to the WayPoint created - */ - WayPointItem* WPCreate(internals::PointLatLng const& coord,int const& altitude, QString const& description); - /** - * @brief Creates a new WayPoint - * - * @param coord the offset in meters to the home position - * @param altitude the Altitude of the WayPoint - * @param description the description of the WayPoint - * @return WayPointItem a pointer to the WayPoint created - */ - WayPointItem *WPCreate(const distBearingAltitude &relativeCoord, const QString &description); - /** - * @brief Inserts a new WayPoint on the specified position - * - * @param position index of the WayPoint - * @return WayPointItem a pointer to the WayPoint created - */ - WayPointItem* WPInsert(int const& position); - /** - * @brief Inserts a new WayPoint on the specified position - * - * @param item the WayPoint to Insert - * @param position index of the WayPoint - */ - void WPInsert(WayPointItem* item,int const& position); - /** - * @brief Inserts a new WayPoint on the specified position - * - * @param coord the coordinates in LatLng of the WayPoint - * @param altitude the Altitude of the WayPoint - * @param position index of the WayPoint - * @return WayPointItem a pointer to the WayPoint Inserted - */ - WayPointItem* WPInsert(internals::PointLatLng const& coord,int const& altitude,int const& position); - /** - * @brief Inserts a new WayPoint on the specified position - * - * @param coord the coordinates in LatLng of the WayPoint - * @param altitude the Altitude of the WayPoint - * @param description the description of the WayPoint - * @param position index of the WayPoint - * @return WayPointItem a pointer to the WayPoint Inserted - */ - WayPointItem* WPInsert(internals::PointLatLng const& coord,int const& altitude, QString const& description,int const& position); - WayPointItem *WPInsert(const distBearingAltitude &relative, const QString &description, const int &position); + internals::PointLatLng currentMousePosition(); - /** - * @brief Deletes the WayPoint - * - * @param item the WayPoint to delete - */ - void WPDelete(WayPointItem* item); - /** - * @brief deletes all WayPoints - * - */ - void WPDeleteAll(); - /** - * @brief Returns the currently selected WayPoints - * - * @return @return QList - */ - QList WPSelected(); + void SetFollowMouse(bool const & value) + { + followmouse = value; this->setMouseTracking(followmouse); + } + bool FollowMouse() + { + return followmouse; + } - /** - * @brief Renumbers the WayPoint and all others as needed - * - * @param item the WayPoint to renumber - * @param newnumber the WayPoint's new number - */ - void WPRenumber(WayPointItem* item,int const& newnumber); + internals::PointLatLng GetFromLocalToLatLng(QPointF p) + { + return map->FromLocalToLatLng(p.x(), p.y()); + } - void SetShowCompass(bool const& value); + /** + * @brief Creates a new WayPoint on the center of the map + * + * @return WayPointItem a pointer to the WayPoint created + */ + WayPointItem *WPCreate(); + /** + * @brief Creates a new WayPoint + * + * @param item the WayPoint to create + */ + void WPCreate(WayPointItem *item); + /** + * @brief Creates a new WayPoint + * + * @param coord the coordinates in LatLng of the WayPoint + * @param altitude the Altitude of the WayPoint + * @return WayPointItem a pointer to the WayPoint created + */ + WayPointItem *WPCreate(internals::PointLatLng const & coord, int const & altitude); + /** + * @brief Creates a new WayPoint + * + * @param coord the coordinates in LatLng of the WayPoint + * @param altitude the Altitude of the WayPoint + * @param description the description of the WayPoint + * @return WayPointItem a pointer to the WayPoint created + */ + WayPointItem *WPCreate(internals::PointLatLng const & coord, int const & altitude, QString const & description); + /** + * @brief Creates a new WayPoint + * + * @param coord the offset in meters to the home position + * @param altitude the Altitude of the WayPoint + * @param description the description of the WayPoint + * @return WayPointItem a pointer to the WayPoint created + */ + WayPointItem *WPCreate(const distBearingAltitude &relativeCoord, const QString &description); + /** + * @brief Inserts a new WayPoint on the specified position + * + * @param position index of the WayPoint + * @return WayPointItem a pointer to the WayPoint created + */ + WayPointItem *WPInsert(int const & position); + /** + * @brief Inserts a new WayPoint on the specified position + * + * @param item the WayPoint to Insert + * @param position index of the WayPoint + */ + void WPInsert(WayPointItem *item, int const & position); + /** + * @brief Inserts a new WayPoint on the specified position + * + * @param coord the coordinates in LatLng of the WayPoint + * @param altitude the Altitude of the WayPoint + * @param position index of the WayPoint + * @return WayPointItem a pointer to the WayPoint Inserted + */ + WayPointItem *WPInsert(internals::PointLatLng const & coord, int const & altitude, int const & position); + /** + * @brief Inserts a new WayPoint on the specified position + * + * @param coord the coordinates in LatLng of the WayPoint + * @param altitude the Altitude of the WayPoint + * @param description the description of the WayPoint + * @param position index of the WayPoint + * @return WayPointItem a pointer to the WayPoint Inserted + */ + WayPointItem *WPInsert(internals::PointLatLng const & coord, int const & altitude, QString const & description, int const & position); + WayPointItem *WPInsert(const distBearingAltitude &relative, const QString &description, const int &position); - void setOverlayOpacity(qreal value); + /** + * @brief Deletes the WayPoint + * + * @param item the WayPoint to delete + */ + void WPDelete(WayPointItem *item); + /** + * @brief deletes all WayPoints + * + */ + void WPDeleteAll(); + /** + * @brief Returns the currently selected WayPoints + * + * @return @return QList + */ + QList WPSelected(); - UAVItem* UAV; - GPSItem* GPS; - HomeItem* Home; - void SetShowUAV(bool const& value); - bool ShowUAV()const{return showuav;} - void SetShowHome(bool const& value); - bool ShowHome()const{return showhome;} - void SetShowDiagnostics(bool const& value); - void SetUavPic(QString UAVPic); - WayPointLine * WPLineCreate(WayPointItem *from,WayPointItem *to, QColor color); - WayPointLine * WPLineCreate(HomeItem *from,WayPointItem *to, QColor color); - WayPointCircle *WPCircleCreate(WayPointItem *center, WayPointItem *radius,bool clockwise,QColor color); - WayPointCircle *WPCircleCreate(HomeItem *center, WayPointItem *radius,bool clockwise,QColor color); - void deleteAllOverlays(); - void WPSetVisibleAll(bool value); - WayPointItem *magicWPCreate(); - bool WPPresent(); - void WPDelete(int number); - WayPointItem *WPFind(int number); - void setSelectedWP(QList list); - private: - internals::Core *core; - MapGraphicItem *map; - QGraphicsScene mscene; - bool useOpenGL; - GeoCoderStatusCode x; - MapType y; - core::AccessMode xx; - internals::PointLatLng currentmouseposition; - bool followmouse; - void ConnectWP(WayPointItem* item); - QGraphicsSvgItem *compass; - bool showuav; - bool showhome; - QTimer * diagTimer; - QGraphicsTextItem * diagGraphItem; - bool showDiag; - qreal overlayOpacity; - private slots: - void diagRefresh(); - // WayPointItem* item;//apagar - protected: - void resizeEvent(QResizeEvent *event); - void showEvent ( QShowEvent * event ); - void closeEvent(QCloseEvent *event); - void mouseMoveEvent ( QMouseEvent * event ); - // private slots: - signals: - void zoomChanged(double zoomt,double zoom, double zoomd); - /** - * @brief fires when one of the WayPoints numbers changes (not fired if due to a auto-renumbering) - * - * @param oldnumber WayPoint old number - * @param newnumber WayPoint new number - * @param waypoint a pointer to the WayPoint that was renumbered - */ - void WPNumberChanged(int const& oldnumber,int const& newnumber,WayPointItem* waypoint); - /** - * @brief Fired when the description, altitude or coordinates of a WayPoint changed - * - * @param waypoint a pointer to the WayPoint - */ - void WPValuesChanged(WayPointItem* waypoint); - /** - * @brief Fires when a new WayPoint is inserted - * - * @param number new WayPoint number - * @param waypoint WayPoint inserted - */ - void WPReached(WayPointItem* waypoint); + /** + * @brief Renumbers the WayPoint and all others as needed + * + * @param item the WayPoint to renumber + * @param newnumber the WayPoint's new number + */ + void WPRenumber(WayPointItem *item, int const & newnumber); - void WPCreated(int const& number,WayPointItem* waypoint); + void SetShowCompass(bool const & value); - /** - * @brief Fires when a new WayPoint is inserted - * - * @param number new WayPoint number - * @param waypoint WayPoint inserted - */ - void WPInserted(int const& number,WayPointItem* waypoint); - /** - * @brief Fires When a WayPoint is deleted - * - * @param number number of the deleted WayPoint - */ - void WPDeleted(int const& number,WayPointItem* waypoint); + void setOverlayOpacity(qreal value); - void WPLocalPositionChanged(QPointF,WayPointItem*); - void WPManualCoordChange(WayPointItem*); - /** - * @brief Fires When a WayPoint is Reached - * - * @param number number of the Reached WayPoint - */ - void UAVReachedWayPoint(int const& waypointnumber,WayPointItem* waypoint); - /** - * @brief Fires When the UAV lives the safety bouble - * - * @param position the position of the UAV - */ - void UAVLeftSafetyBouble(internals::PointLatLng const& position); + UAVItem *UAV; + GPSItem *GPS; + HomeItem *Home; + void SetShowUAV(bool const & value); + bool ShowUAV() const + { + return showuav; + } + void SetShowHome(bool const & value); + bool ShowHome() const + { + return showhome; + } + void SetShowDiagnostics(bool const & value); + void SetUavPic(QString UAVPic); + WayPointLine *WPLineCreate(WayPointItem *from, WayPointItem *to, QColor color); + WayPointLine *WPLineCreate(HomeItem *from, WayPointItem *to, QColor color); + WayPointCircle *WPCircleCreate(WayPointItem *center, WayPointItem *radius, bool clockwise, QColor color); + WayPointCircle *WPCircleCreate(HomeItem *center, WayPointItem *radius, bool clockwise, QColor color); + void deleteAllOverlays(); + void WPSetVisibleAll(bool value); + WayPointItem *magicWPCreate(); + bool WPPresent(); + void WPDelete(int number); + WayPointItem *WPFind(int number); + void setSelectedWP(QList list); +private: + internals::Core *core; + MapGraphicItem *map; + QGraphicsScene mscene; + bool useOpenGL; + GeoCoderStatusCode x; + MapType y; + core::AccessMode xx; + internals::PointLatLng currentmouseposition; + bool followmouse; + void ConnectWP(WayPointItem *item); + QGraphicsSvgItem *compass; + bool showuav; + bool showhome; + QTimer *diagTimer; + QGraphicsTextItem *diagGraphItem; + bool showDiag; + qreal overlayOpacity; +private slots: + void diagRefresh(); + // WayPointItem* item;//apagar +protected: + void resizeEvent(QResizeEvent *event); + void showEvent(QShowEvent *event); + void closeEvent(QCloseEvent *event); + void mouseMoveEvent(QMouseEvent *event); + // private slots: +signals: + void zoomChanged(double zoomt, double zoom, double zoomd); + /** + * @brief fires when one of the WayPoints numbers changes (not fired if due to a auto-renumbering) + * + * @param oldnumber WayPoint old number + * @param newnumber WayPoint new number + * @param waypoint a pointer to the WayPoint that was renumbered + */ + void WPNumberChanged(int const & oldnumber, int const & newnumber, WayPointItem *waypoint); + /** + * @brief Fired when the description, altitude or coordinates of a WayPoint changed + * + * @param waypoint a pointer to the WayPoint + */ + void WPValuesChanged(WayPointItem *waypoint); + /** + * @brief Fires when a new WayPoint is inserted + * + * @param number new WayPoint number + * @param waypoint WayPoint inserted + */ + void WPReached(WayPointItem *waypoint); - /** - * @brief Fires when map position changes - * - * @param point the point in LatLng of the new center of the map - */ - void OnCurrentPositionChanged(internals::PointLatLng point); - /** - * @brief Fires when there are no more tiles to load - * - */ - void OnTileLoadComplete(); - /** - * @brief Fires when tiles loading begins - * - */ - void OnTileLoadStart(); - /** - * @brief Fires when the map is dragged - * - */ - void OnMapDrag(); - /** - * @brief Fires when map zoom changes - * - */ - void OnMapZoomChanged(); - /** - * @brief Fires when map type changes - * - * @param type The maps new type - */ - void OnMapTypeChanged(MapType::Types type); - /** - * @brief Fires when an error ocurred while loading a tile - * - * @param zoom tile zoom - * @param pos tile position - */ - void OnEmptyTileError(int zoom, core::Point pos); - /** - * @brief Fires when the number of tiles in the load queue changes - * - * @param number the number of tiles still in the queue - */ - void OnTilesStillToLoad(int number); - void OnWayPointDoubleClicked(WayPointItem * waypoint); - void selectedWPChanged(QList); - public slots: - /** - * @brief Ripps the current selection to the DB - */ - void RipMap(); - void OnSelectionChanged(); + void WPCreated(int const & number, WayPointItem *waypoint); - }; + /** + * @brief Fires when a new WayPoint is inserted + * + * @param number new WayPoint number + * @param waypoint WayPoint inserted + */ + void WPInserted(int const & number, WayPointItem *waypoint); + /** + * @brief Fires When a WayPoint is deleted + * + * @param number number of the deleted WayPoint + */ + void WPDeleted(int const & number, WayPointItem *waypoint); + + void WPLocalPositionChanged(QPointF, WayPointItem *); + void WPManualCoordChange(WayPointItem *); + /** + * @brief Fires When a WayPoint is Reached + * + * @param number number of the Reached WayPoint + */ + void UAVReachedWayPoint(int const & waypointnumber, WayPointItem *waypoint); + /** + * @brief Fires When the UAV lives the safety bouble + * + * @param position the position of the UAV + */ + void UAVLeftSafetyBouble(internals::PointLatLng const & position); + + /** + * @brief Fires when map position changes + * + * @param point the point in LatLng of the new center of the map + */ + void OnCurrentPositionChanged(internals::PointLatLng point); + /** + * @brief Fires when there are no more tiles to load + * + */ + void OnTileLoadComplete(); + /** + * @brief Fires when tiles loading begins + * + */ + void OnTileLoadStart(); + /** + * @brief Fires when the map is dragged + * + */ + void OnMapDrag(); + /** + * @brief Fires when map zoom changes + * + */ + void OnMapZoomChanged(); + /** + * @brief Fires when map type changes + * + * @param type The maps new type + */ + void OnMapTypeChanged(MapType::Types type); + /** + * @brief Fires when an error ocurred while loading a tile + * + * @param zoom tile zoom + * @param pos tile position + */ + void OnEmptyTileError(int zoom, core::Point pos); + /** + * @brief Fires when the number of tiles in the load queue changes + * + * @param number the number of tiles still in the queue + */ + void OnTilesStillToLoad(int number); + void OnWayPointDoubleClicked(WayPointItem *waypoint); + void selectedWPChanged(QList); +public slots: + /** + * @brief Ripps the current selection to the DB + */ + void RipMap(); + void OnSelectionChanged(); +}; } #endif // OPMAPWIDGET_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.cpp index 69b2dbbd2..866cfc842 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.cpp @@ -1,61 +1,61 @@ /** -****************************************************************************** -* -* @file trailitem.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a trail point -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file trailitem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a trail point + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "trailitem.h" #include -namespace mapcontrol +namespace mapcontrol { +TrailItem::TrailItem(internals::PointLatLng const & coord, int const & altitude, QBrush color, MapGraphicItem *map) : QGraphicsItem(map), coord(coord), m_brush(color), m_map(map) { -TrailItem::TrailItem(internals::PointLatLng const& coord,int const& altitude, QBrush color, MapGraphicItem *map):QGraphicsItem(map),coord(coord),m_brush(color),m_map(map) - { - QDateTime time=QDateTime::currentDateTime(); - QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); - setToolTip(QString(tr("Position:")+"%1\n"+tr("Altitude:")+"%2\n"+tr("Time:")+"%3").arg(coord_str).arg(QString::number(altitude)).arg(time.toString())); - } + QDateTime time = QDateTime::currentDateTime(); + QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); - void TrailItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) - { - Q_UNUSED(option); - Q_UNUSED(widget); - - painter->setBrush(m_brush); - painter->drawEllipse(-2,-2,4,4); - } - QRectF TrailItem::boundingRect()const - { - return QRectF(-2,-2,4,4); - } - - - int TrailItem::type()const - { - return Type; - } - - void TrailItem::setPosSLOT() - { - setPos(m_map->FromLatLngToLocal(this->coord).X(),m_map->FromLatLngToLocal(this->coord).Y()); - } + setToolTip(QString(tr("Position:") + "%1\n" + tr("Altitude:") + "%2\n" + tr("Time:") + "%3").arg(coord_str).arg(QString::number(altitude)).arg(time.toString())); +} + +void TrailItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) +{ + Q_UNUSED(option); + Q_UNUSED(widget); + + painter->setBrush(m_brush); + painter->drawEllipse(-2, -2, 4, 4); +} +QRectF TrailItem::boundingRect() const +{ + return QRectF(-2, -2, 4, 4); +} + + +int TrailItem::type() const +{ + return Type; +} + +void TrailItem::setPosSLOT() +{ + setPos(m_map->FromLatLngToLocal(this->coord).X(), m_map->FromLatLngToLocal(this->coord).Y()); +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.h index bfc58b97d..e3a8a4b23 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file trailitem.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a WayPoint -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file trailitem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef TRAILITEM_H #define TRAILITEM_H @@ -34,30 +34,23 @@ #include #include "mapgraphicitem.h" -namespace mapcontrol -{ - - class TrailItem:public QObject,public QGraphicsItem - { - Q_OBJECT - Q_INTERFACES(QGraphicsItem) - public: - enum { Type = UserType + 3 }; - TrailItem(internals::PointLatLng const& coord,int const& altitude, QBrush color,MapGraphicItem * map); - void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, - QWidget *widget); - QRectF boundingRect() const; - int type() const; - internals::PointLatLng coord; - private: - QBrush m_brush; - MapGraphicItem * m_map; - public slots: - void setPosSLOT(); - signals: - - }; +namespace mapcontrol { +class TrailItem : public QObject, public QGraphicsItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) +public: + enum { Type = UserType + 3 }; + TrailItem(internals::PointLatLng const & coord, int const & altitude, QBrush color, MapGraphicItem *map); + void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, + QWidget *widget); + QRectF boundingRect() const; + int type() const; + internals::PointLatLng coord; +private: + QBrush m_brush; + MapGraphicItem *m_map; +public slots: + void setPosSLOT(); +signals: +}; } #endif // TRAILITEM_H - - diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.cpp index 7ad63109e..0e5a9dcd9 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.cpp @@ -1,48 +1,48 @@ /** -****************************************************************************** -* -* @file trailitem.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a trail point -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file trailitem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a trail point + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "traillineitem.h" -namespace mapcontrol +namespace mapcontrol { +TrailLineItem::TrailLineItem(internals::PointLatLng const & coord1, internals::PointLatLng const & coord2, QBrush color, MapGraphicItem *map) : QGraphicsLineItem(map), coord1(coord1), coord2(coord2), m_brush(color), m_map(map) { -TrailLineItem::TrailLineItem(internals::PointLatLng const& coord1,internals::PointLatLng const& coord2, QBrush color, MapGraphicItem * map):QGraphicsLineItem(map),coord1(coord1),coord2(coord2),m_brush(color),m_map(map) - { - QPen pen; - pen.setBrush(m_brush); - pen.setWidth(1); - this->setPen(pen); - } + QPen pen; - int TrailLineItem::type()const - { - return Type; - } - - void TrailLineItem::setLineSlot() - { - setLine(m_map->FromLatLngToLocal(this->coord1).X(),m_map->FromLatLngToLocal(this->coord1).Y(),m_map->FromLatLngToLocal(this->coord2).X(),m_map->FromLatLngToLocal(this->coord2).Y()); - } + pen.setBrush(m_brush); + pen.setWidth(1); + this->setPen(pen); +} + +int TrailLineItem::type() const +{ + return Type; +} + +void TrailLineItem::setLineSlot() +{ + setLine(m_map->FromLatLngToLocal(this->coord1).X(), m_map->FromLatLngToLocal(this->coord1).Y(), m_map->FromLatLngToLocal(this->coord2).X(), m_map->FromLatLngToLocal(this->coord2).Y()); +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.h index db195108f..c61412014 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file traillineitem.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a WayPoint -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file traillineitem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef TAILLINEITEM_H #define TAILLINEITEM_H @@ -34,27 +34,22 @@ #include #include "mapgraphicitem.h" -namespace mapcontrol -{ - - class TrailLineItem:public QObject,public QGraphicsLineItem - { - Q_OBJECT - Q_INTERFACES(QGraphicsItem) - public: - enum { Type = UserType + 7 }; - TrailLineItem(internals::PointLatLng const& coord1,internals::PointLatLng const& coord2, QBrush color,MapGraphicItem * map); - int type() const; - internals::PointLatLng coord1; - internals::PointLatLng coord2; - private: - QBrush m_brush; - MapGraphicItem * m_map; - public slots: - void setLineSlot(); - signals: - - }; +namespace mapcontrol { +class TrailLineItem : public QObject, public QGraphicsLineItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) +public: + enum { Type = UserType + 7 }; + TrailLineItem(internals::PointLatLng const & coord1, internals::PointLatLng const & coord2, QBrush color, MapGraphicItem *map); + int type() const; + internals::PointLatLng coord1; + internals::PointLatLng coord2; +private: + QBrush m_brush; + MapGraphicItem *m_map; +public slots: + void setLineSlot(); +signals: +}; } #endif // TAILLINEITEM_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp index 6fc3d237b..58190c735 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp @@ -1,452 +1,430 @@ /** -****************************************************************************** -* -* @file uavitem.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a UAV -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file uavitem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a UAV + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "../internals/pureprojection.h" #include "uavitem.h" #include -namespace mapcontrol +namespace mapcontrol { +double UAVItem::groundspeed_mps_filt = 0; + +UAVItem::UAVItem(MapGraphicItem *map, OPMapWidget *parent, QString uavPic) : map(map), mapwidget(parent), showtrail(true), showtrailline(true), trailtime(5), traildistance(50), autosetreached(true) + , autosetdistance(100), altitude(0), showUAVInfo(false) { + pic.load(uavPic); + this->setFlag(QGraphicsItem::ItemIsMovable, false); + this->setFlag(QGraphicsItem::ItemIsSelectable, false); + localposition = map->FromLatLngToLocal(mapwidget->CurrentPosition()); + this->setPos(localposition.X(), localposition.Y()); + this->setZValue(4); + trail = new QGraphicsItemGroup(this); + trail->setParentItem(map); + trailLine = new QGraphicsItemGroup(this); + trailLine->setParentItem(map); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + setCacheMode(QGraphicsItem::ItemCoordinateCache); + mapfollowtype = UAVMapFollowType::None; + trailtype = UAVTrailType::ByDistance; + timer.start(); + generateArrowhead(); + double pixels2meters = map->Projection()->GetGroundResolution(map->ZoomTotal(), coord.Lat()); + meters2pixels = 1.0 / pixels2meters; + setCacheMode(QGraphicsItem::DeviceCoordinateCache); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); + connect(map, SIGNAL(zoomChanged(double, double, double)), this, SLOT(zoomChangedSlot())); +} +UAVItem::~UAVItem() +{} - double UAVItem::groundspeed_mps_filt = 0; +void UAVItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) +{ + Q_UNUSED(option); + Q_UNUSED(widget); - UAVItem::UAVItem(MapGraphicItem* map,OPMapWidget* parent,QString uavPic):map(map),mapwidget(parent),showtrail(true),showtrailline(true),trailtime(5),traildistance(50),autosetreached(true) - ,autosetdistance(100),altitude(0),showUAVInfo(false) - { - pic.load(uavPic); - this->setFlag(QGraphicsItem::ItemIsMovable,false); - this->setFlag(QGraphicsItem::ItemIsSelectable,false); - localposition=map->FromLatLngToLocal(mapwidget->CurrentPosition()); - this->setPos(localposition.X(),localposition.Y()); - this->setZValue(4); - trail=new QGraphicsItemGroup(this); - trail->setParentItem(map); - trailLine=new QGraphicsItemGroup(this); - trailLine->setParentItem(map); - this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); - setCacheMode(QGraphicsItem::ItemCoordinateCache); - mapfollowtype=UAVMapFollowType::None; - trailtype=UAVTrailType::ByDistance; - timer.start(); - generateArrowhead(); - double pixels2meters = map->Projection()->GetGroundResolution(map->ZoomTotal(),coord.Lat()); - meters2pixels=1.0 / pixels2meters; - setCacheMode(QGraphicsItem::DeviceCoordinateCache); - connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); - connect(map,SIGNAL(zoomChanged(double,double,double)),this,SLOT(zoomChangedSlot())); - } - UAVItem::~UAVItem() - { + // Draw plane + painter->drawPixmap(-pic.width() / 2, -pic.height() / 2, pic); + // Return if UAV Info context menu is turned off + if (!showUAVInfo) { + return; } - void UAVItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) - { - Q_UNUSED(option); - Q_UNUSED(widget); + QPen myPen; - //Draw plane - painter->drawPixmap(-pic.width()/2,-pic.height()/2,pic); + // Turn on anti-aliasing so the fonts don't look terrible + painter->setRenderHint(QPainter::Antialiasing, true); - //Return if UAV Info context menu is turned off - if(!showUAVInfo){ - return; - } + // Set pen attributes + QColor myColor(Qt::red); + myPen.setWidth(3); + myPen.setColor(myColor); + painter->setPen(myPen); + painter->drawPolygon(arrowHead); + painter->setPen(myPen); + painter->drawLine(arrowShaft); - QPen myPen; + // Set trend arc's color + myPen.setColor(Qt::magenta); + painter->setPen(myPen); - //Turn on anti-aliasing so the fonts don't look terrible - painter->setRenderHint(QPainter::Antialiasing, true); - - //Set pen attributes - QColor myColor(Qt::red); - myPen.setWidth(3); - myPen.setColor(myColor); - painter->setPen(myPen); - painter->drawPolygon(arrowHead); - painter->setPen(myPen); - painter->drawLine(arrowShaft); - - //Set trend arc's color - myPen.setColor(Qt::magenta); - painter->setPen(myPen); - - if (trendSpanAngle > 0){ - QRectF rect(0, -trendRadius, trendRadius*2, trendRadius*2); - painter->drawArc(rect, 180*16, -trendSpanAngle*16); - } - else{ - QRectF rect(-2*trendRadius, -trendRadius, trendRadius*2, trendRadius*2); - painter->drawArc(rect, 0*16, -trendSpanAngle*16); - } - - //*********** Create time rings - if(groundspeed_mps_filt > 0){ //Don't clutter the display with rings that are only one pixel wide - myPen.setWidth(2); - - myPen.setColor(QColor(0, 0, 0, 100)); - painter->setPen(myPen); - painter->drawEllipse(QPointF(0,0),precalcRings,precalcRings); - - myPen.setColor(QColor(0, 0, 0, 110)); - painter->setPen(myPen); - painter->drawEllipse(QPointF(0,0),precalcRings*2,precalcRings*2); - - myPen.setColor(QColor(0, 0, 0, 120)); - painter->setPen(myPen); - painter->drawEllipse(QPointF(0,0),precalcRings*4,precalcRings*4); - } - //Rotate the text back to vertical - qreal rot=this->rotation(); - painter->rotate(-1*rot); - - myPen.setWidth(1); - myPen.setColor(Qt::white); - painter->setBrush(Qt::white); - painter->setPen(myPen); - painter->drawPath(textPath); + if (trendSpanAngle > 0) { + QRectF rect(0, -trendRadius, trendRadius * 2, trendRadius * 2); + painter->drawArc(rect, 180 * 16, -trendSpanAngle * 16); + } else { + QRectF rect(-2 * trendRadius, -trendRadius, trendRadius * 2, trendRadius * 2); + painter->drawArc(rect, 0 * 16, -trendSpanAngle * 16); } - void UAVItem::updateTextOverlay() - { - QPainterPath temp; - if(!showUAVInfo) - { - temp.swap(textPath); - return; - } + // *********** Create time rings + if (groundspeed_mps_filt > 0) { // Don't clutter the display with rings that are only one pixel wide + myPen.setWidth(2); - QFont borderfont( "Arial", 14, QFont::Normal, false ); + myPen.setColor(QColor(0, 0, 0, 100)); + painter->setPen(myPen); + painter->drawEllipse(QPointF(0, 0), precalcRings, precalcRings); - //Top left corner of text - int textAnchorX = 20; - int textAnchorY = 20; + myPen.setColor(QColor(0, 0, 0, 110)); + painter->setPen(myPen); + painter->drawEllipse(QPointF(0, 0), precalcRings * 2, precalcRings * 2); - QString uavoInfoStrLine1, uavoInfoStrLine2; - QString uavoInfoStrLine3, uavoInfoStrLine4; - QString uavoInfoStrLine5; + myPen.setColor(QColor(0, 0, 0, 120)); + painter->setPen(myPen); + painter->drawEllipse(QPointF(0, 0), precalcRings * 4, precalcRings * 4); + } + // Rotate the text back to vertical + qreal rot = this->rotation(); + painter->rotate(-1 * rot); - uavoInfoStrLine1.append(QString("CAS: %1 kph").arg(CAS_mps * 3.6)); - uavoInfoStrLine2.append(QString("Groundspeed: %1 kph").arg(groundspeed_kph, 0, 'f',1)); - uavoInfoStrLine3.append(QString("Lat-Lon: %1, %2").arg(coord.Lat(), 0, 'f',7).arg(coord.Lng(), 0, 'f',7)); - uavoInfoStrLine4.append(QString("North-East: %1 m, %2 m").arg(NED[0], 0, 'f',1).arg(NED[1], 0, 'f',1)); - uavoInfoStrLine5.append(QString("Altitude: %1 m").arg(-NED[2], 0, 'f',1)); - temp.addText(textAnchorX, textAnchorY+16*0, borderfont, uavoInfoStrLine1); - temp.addText(textAnchorX, textAnchorY+16*1, borderfont, uavoInfoStrLine2); - temp.addText(textAnchorX, textAnchorY+16*2, borderfont, uavoInfoStrLine3); - temp.addText(textAnchorX, textAnchorY+16*3, borderfont, uavoInfoStrLine4); - temp.addText(textAnchorX, textAnchorY+16*4, borderfont, uavoInfoStrLine5); + myPen.setWidth(1); + myPen.setColor(Qt::white); + painter->setBrush(Qt::white); + painter->setPen(myPen); + painter->drawPath(textPath); +} - //Add text for time rings. - if(groundspeed_mps > 0){ - //Always add the left side... - temp.addText(-(groundspeed_mps_filt*ringTime*1*meters2pixels+10), 0, borderfont, QString("%1 s").arg(ringTime,0,'f',0)); - temp.addText(-(groundspeed_mps_filt*ringTime*2*meters2pixels+10), 0, borderfont, QString("%1 s").arg(ringTime*2,0,'f',0)); - temp.addText(-(groundspeed_mps_filt*ringTime*4*meters2pixels+10), 0, borderfont, QString("%1 s").arg(ringTime*4,0,'f',0)); - //... and add the right side, only if it doesn't interfere with the uav info text - if(groundspeed_mps_filt*ringTime*4*meters2pixels > 200){ - if(groundspeed_mps_filt*ringTime*2*meters2pixels > 200){ - if(groundspeed_mps_filt*ringTime*1*meters2pixels > 200){ - temp.addText(groundspeed_mps_filt*ringTime*1*meters2pixels-8, 0, borderfont, QString("%1 s").arg(ringTime,0,'f',0)); - } - temp.addText(groundspeed_mps_filt*ringTime*2*meters2pixels-8, 0, borderfont, QString("%1 s").arg(ringTime*2,0,'f',0)); - } - temp.addText(groundspeed_mps_filt*ringTime*4*meters2pixels-8, 0, borderfont, QString("%1 s").arg(ringTime*4,0,'f',0)); - } - } +void UAVItem::updateTextOverlay() +{ + QPainterPath temp; + + if (!showUAVInfo) { temp.swap(textPath); + return; } - QRectF UAVItem::boundingRect()const - { - if(showUAVInfo){ - if (boundingRectSize < 220){ - //In case the bounding rectangle isn't big enough to get the whole of the UAV Info graphic - return QRectF(-boundingRectSize,-80,boundingRectSize+220,180); - } - else{ - return QRectF(-boundingRectSize,-boundingRectSize,2*boundingRectSize,2*boundingRectSize); - } - } - else{ - return QRectF(-pic.width()/2,-pic.height()/2,pic.width(),pic.height()); - } - } + QFont borderfont("Arial", 14, QFont::Normal, false); - void UAVItem::SetNED(double NED[3]){ - this->NED[0] = NED[0]; - this->NED[1] = NED[1]; - this->NED[2] = NED[2]; - } + // Top left corner of text + int textAnchorX = 20; + int textAnchorY = 20; - void UAVItem::SetYawRate(double yawRate_dps){ - this->yawRate_dps=yawRate_dps; + QString uavoInfoStrLine1, uavoInfoStrLine2; + QString uavoInfoStrLine3, uavoInfoStrLine4; + QString uavoInfoStrLine5; - if (fabs(this->yawRate_dps) < 5e-1){ //This number is really the smallest we can go. Any smaller, and it might have problems if we forecast a shorter distance into the future - this->yawRate_dps=5e-1; - } + uavoInfoStrLine1.append(QString("CAS: %1 kph").arg(CAS_mps * 3.6)); + uavoInfoStrLine2.append(QString("Groundspeed: %1 kph").arg(groundspeed_kph, 0, 'f', 1)); + uavoInfoStrLine3.append(QString("Lat-Lon: %1, %2").arg(coord.Lat(), 0, 'f', 7).arg(coord.Lng(), 0, 'f', 7)); + uavoInfoStrLine4.append(QString("North-East: %1 m, %2 m").arg(NED[0], 0, 'f', 1).arg(NED[1], 0, 'f', 1)); + uavoInfoStrLine5.append(QString("Altitude: %1 m").arg(-NED[2], 0, 'f', 1)); + temp.addText(textAnchorX, textAnchorY + 16 * 0, borderfont, uavoInfoStrLine1); + temp.addText(textAnchorX, textAnchorY + 16 * 1, borderfont, uavoInfoStrLine2); + temp.addText(textAnchorX, textAnchorY + 16 * 2, borderfont, uavoInfoStrLine3); + temp.addText(textAnchorX, textAnchorY + 16 * 3, borderfont, uavoInfoStrLine4); + temp.addText(textAnchorX, textAnchorY + 16 * 4, borderfont, uavoInfoStrLine5); - //*********** Create trend arc - trendSpanAngle = this->yawRate_dps * 5; //Forecast 5 seconds into the future - - //Calculate radius in [m], and then convert to pixels in local frame (not the same frame as is displayed on the map widget) - trendRadius=fabs(groundspeed_mps/(this->yawRate_dps*M_PI/180))*meters2pixels; - } - - void UAVItem::SetCAS(double CAS_mps){ - this->CAS_mps=CAS_mps; - } - - void UAVItem::SetGroundspeed(double vNED[3], int m_maxUpdateRate_ms){ - this->vNED[0] = vNED[0]; - this->vNED[1] = vNED[1]; - this->vNED[2] = vNED[2]; - groundspeed_kph=sqrt(vNED[0]*vNED[0] + vNED[1]*vNED[1] + vNED[2]*vNED[2])*3.6; - groundspeed_mps=groundspeed_kph/3.6; - //On the first pass, set the filtered speed to the reported speed. - static bool firstGroundspeed=true; - if (firstGroundspeed){ - groundspeed_mps_filt=groundspeed_kph/3.6; - firstGroundspeed=false; - } - else{ - int riseTime_ms=1000; - double alpha= m_maxUpdateRate_ms/(double)(m_maxUpdateRate_ms+riseTime_ms); - groundspeed_mps_filt= alpha*groundspeed_mps_filt + (1-alpha)*(groundspeed_kph/3.6); - } - ringTime=10*pow(2,17-map->ZoomTotal()); //Basic ring is 10 seconds wide at zoom level 17 - precalcRings=groundspeed_mps_filt*ringTime*meters2pixels; - boundingRectSize=groundspeed_mps_filt*ringTime*4*meters2pixels+20; - prepareGeometryChange(); - } - - - void UAVItem::SetUAVPos(const internals::PointLatLng &position, const int &altitude) - { - if(coord.IsEmpty()) - lastcoord=coord; - if(coord!=position) - { - - if(trailtype==UAVTrailType::ByTimeElapsed) - { - if(timer.elapsed()>trailtime*1000) - { - TrailItem * ob=new TrailItem(position,altitude,Qt::green,map); - trail->addToGroup(ob); - connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT())); - if(!lasttrailline.IsEmpty()) - { - TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::red,map); - trailLine->addToGroup(obj); - connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot())); - } - lasttrailline=position; - timer.restart(); + // Add text for time rings. + if (groundspeed_mps > 0) { + // Always add the left side... + temp.addText(-(groundspeed_mps_filt * ringTime * 1 * meters2pixels + 10), 0, borderfont, QString("%1 s").arg(ringTime, 0, 'f', 0)); + temp.addText(-(groundspeed_mps_filt * ringTime * 2 * meters2pixels + 10), 0, borderfont, QString("%1 s").arg(ringTime * 2, 0, 'f', 0)); + temp.addText(-(groundspeed_mps_filt * ringTime * 4 * meters2pixels + 10), 0, borderfont, QString("%1 s").arg(ringTime * 4, 0, 'f', 0)); + // ... and add the right side, only if it doesn't interfere with the uav info text + if (groundspeed_mps_filt * ringTime * 4 * meters2pixels > 200) { + if (groundspeed_mps_filt * ringTime * 2 * meters2pixels > 200) { + if (groundspeed_mps_filt * ringTime * 1 * meters2pixels > 200) { + temp.addText(groundspeed_mps_filt * ringTime * 1 * meters2pixels - 8, 0, borderfont, QString("%1 s").arg(ringTime, 0, 'f', 0)); } - - } - else if(trailtype==UAVTrailType::ByDistance) - { - if(qAbs(internals::PureProjection::DistanceBetweenLatLng(lastcoord,position)*1000)>traildistance) - { - TrailItem * ob=new TrailItem(position,altitude,Qt::green,map); - trail->addToGroup(ob); - connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT())); - if(!lasttrailline.IsEmpty()) - { - TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::red,map); - trailLine->addToGroup(obj); - connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot())); - } - lasttrailline=position; - lastcoord=position; - } - } - coord=position; - this->altitude=altitude; - RefreshPos(); - if(mapfollowtype==UAVMapFollowType::CenterAndRotateMap||mapfollowtype==UAVMapFollowType::CenterMap) - { - mapwidget->SetCurrentPosition(coord); - } - if(autosetreached) - { - foreach(QGraphicsItem* i,map->childItems()) - { - WayPointItem* wp=qgraphicsitem_cast(i); - if(wp) - { - if(Distance3D(wp->Coord(),wp->Altitude())SetReached(true); - emit UAVReachedWayPoint(wp->Number(),wp); - } - } - } - } - if(mapwidget->Home!=0) - { - //verify if the UAV is inside the safety bouble - if(Distance3D(mapwidget->Home->Coord(),mapwidget->Home->Altitude())>mapwidget->Home->SafeArea()) - { - if(mapwidget->Home->safe!=false) - { - mapwidget->Home->safe=false; - mapwidget->Home->update(); - emit UAVLeftSafetyBouble(this->coord); - } - } - else - { - if(mapwidget->Home->safe!=true) - { - mapwidget->Home->safe=true; - mapwidget->Home->update(); - } - } - + temp.addText(groundspeed_mps_filt * ringTime * 2 * meters2pixels - 8, 0, borderfont, QString("%1 s").arg(ringTime * 2, 0, 'f', 0)); } + temp.addText(groundspeed_mps_filt * ringTime * 4 * meters2pixels - 8, 0, borderfont, QString("%1 s").arg(ringTime * 4, 0, 'f', 0)); } } + temp.swap(textPath); +} - /** - * Rotate the UAV Icon on the map, or rotate the map - * depending on the display mode - */ - void UAVItem::SetUAVHeading(const qreal &value) - { - if(mapfollowtype==UAVMapFollowType::CenterAndRotateMap) - { - mapwidget->SetRotate(-value); +QRectF UAVItem::boundingRect() const +{ + if (showUAVInfo) { + if (boundingRectSize < 220) { + // In case the bounding rectangle isn't big enough to get the whole of the UAV Info graphic + return QRectF(-boundingRectSize, -80, boundingRectSize + 220, 180); + } else { + return QRectF(-boundingRectSize, -boundingRectSize, 2 * boundingRectSize, 2 * boundingRectSize); } - else { - if (this->rotation() != value) - this->setRotation(value); - } - } - - - int UAVItem::type()const - { - return Type; - } - - - void UAVItem::RefreshPos() - { - localposition=map->FromLatLngToLocal(coord); - this->setPos(localposition.X(),localposition.Y()); - emit setChildPosition(); - emit setChildLine(); - updateTextOverlay(); - } - - void UAVItem::setOpacitySlot(qreal opacity) - { - this->setOpacity(opacity); - } - - void UAVItem::zoomChangedSlot() - { - double pixels2meters = map->Projection()->GetGroundResolution(map->ZoomTotal(),coord.Lat()); - meters2pixels=1.0 / pixels2meters; - boundingRectSize=groundspeed_mps_filt*ringTime*4*meters2pixels+20; - prepareGeometryChange(); - updateTextOverlay(); - update(); - } - void UAVItem::SetTrailType(const UAVTrailType::Types &value) - { - trailtype=value; - if(trailtype==UAVTrailType::ByTimeElapsed) - timer.restart(); - } - void UAVItem::SetShowTrail(const bool &value) - { - showtrail=value; - trail->setVisible(value); - } - void UAVItem::SetShowTrailLine(const bool &value) - { - showtrailline=value; - trailLine->setVisible(value); - } - - void UAVItem::DeleteTrail()const - { - foreach(QGraphicsItem* i,trail->childItems()) - delete i; - foreach(QGraphicsItem* i,trailLine->childItems()) - delete i; - } - double UAVItem::Distance3D(const internals::PointLatLng &coord, const int &altitude) - { - return sqrt(pow(internals::PureProjection::DistanceBetweenLatLng(this->coord,coord)*1000,2)+ - pow(this->altitude-altitude,2)); - } - void UAVItem::SetUavPic(QString UAVPic) - { - pic.load(":/uavs/images/"+UAVPic); - } - - void UAVItem::SetShowUAVInfo(bool const& value) - { - showUAVInfo=value; - showJustChanged=true; - update(); - } - - void UAVItem::generateArrowhead(){ - qreal arrowSize = 10; - - //Create line from (0,0), to (1,1). Later, we'll scale and rotate it - arrowShaft=QLineF(0,0,1.0,1.0); - - //Set the starting point to (0,0) - arrowShaft.setP1(QPointF(0,0)); - - //Set angle and length - arrowShaft.setLength(60.0); - arrowShaft.setAngle(90.0); - - //Form arrowhead - double angle = ::acos(arrowShaft.dx() / arrowShaft.length()); - if (arrowShaft.dy() <= 0) - angle = (M_PI * 2) - angle; - - QPointF arrowP1 = arrowShaft.pointAt(1) + QPointF(sin(angle + M_PI / 3) * arrowSize, - cos(angle + M_PI / 3) * arrowSize); - QPointF arrowP2 = arrowShaft.pointAt(1) + QPointF(sin(angle + M_PI - M_PI / 3) * arrowSize, - cos(angle + M_PI - M_PI / 3) * arrowSize); - - //Assemble arrowhead - arrowHead.clear(); - arrowHead << arrowShaft.pointAt(1) << arrowP1 << arrowP2; - + } else { + return QRectF(-pic.width() / 2, -pic.height() / 2, pic.width(), pic.height()); } } + +void UAVItem::SetNED(double NED[3]) +{ + this->NED[0] = NED[0]; + this->NED[1] = NED[1]; + this->NED[2] = NED[2]; +} + +void UAVItem::SetYawRate(double yawRate_dps) +{ + this->yawRate_dps = yawRate_dps; + + if (fabs(this->yawRate_dps) < 5e-1) { // This number is really the smallest we can go. Any smaller, and it might have problems if we forecast a shorter distance into the future + this->yawRate_dps = 5e-1; + } + + // *********** Create trend arc + trendSpanAngle = this->yawRate_dps * 5; // Forecast 5 seconds into the future + + // Calculate radius in [m], and then convert to pixels in local frame (not the same frame as is displayed on the map widget) + trendRadius = fabs(groundspeed_mps / (this->yawRate_dps * M_PI / 180)) * meters2pixels; +} + +void UAVItem::SetCAS(double CAS_mps) +{ + this->CAS_mps = CAS_mps; +} + +void UAVItem::SetGroundspeed(double vNED[3], int m_maxUpdateRate_ms) +{ + this->vNED[0] = vNED[0]; + this->vNED[1] = vNED[1]; + this->vNED[2] = vNED[2]; + groundspeed_kph = sqrt(vNED[0] * vNED[0] + vNED[1] * vNED[1] + vNED[2] * vNED[2]) * 3.6; + groundspeed_mps = groundspeed_kph / 3.6; + // On the first pass, set the filtered speed to the reported speed. + static bool firstGroundspeed = true; + if (firstGroundspeed) { + groundspeed_mps_filt = groundspeed_kph / 3.6; + firstGroundspeed = false; + } else { + int riseTime_ms = 1000; + double alpha = m_maxUpdateRate_ms / (double)(m_maxUpdateRate_ms + riseTime_ms); + groundspeed_mps_filt = alpha * groundspeed_mps_filt + (1 - alpha) * (groundspeed_kph / 3.6); + } + ringTime = 10 * pow(2, 17 - map->ZoomTotal()); // Basic ring is 10 seconds wide at zoom level 17 + precalcRings = groundspeed_mps_filt * ringTime * meters2pixels; + boundingRectSize = groundspeed_mps_filt * ringTime * 4 * meters2pixels + 20; + prepareGeometryChange(); +} + + +void UAVItem::SetUAVPos(const internals::PointLatLng &position, const int &altitude) +{ + if (coord.IsEmpty()) { + lastcoord = coord; + } + if (coord != position) { + if (trailtype == UAVTrailType::ByTimeElapsed) { + if (timer.elapsed() > trailtime * 1000) { + TrailItem *ob = new TrailItem(position, altitude, Qt::green, map); + trail->addToGroup(ob); + connect(this, SIGNAL(setChildPosition()), ob, SLOT(setPosSLOT())); + if (!lasttrailline.IsEmpty()) { + TrailLineItem *obj = new TrailLineItem(lasttrailline, position, Qt::red, map); + trailLine->addToGroup(obj); + connect(this, SIGNAL(setChildLine()), obj, SLOT(setLineSlot())); + } + lasttrailline = position; + timer.restart(); + } + } else if (trailtype == UAVTrailType::ByDistance) { + if (qAbs(internals::PureProjection::DistanceBetweenLatLng(lastcoord, position) * 1000) > traildistance) { + TrailItem *ob = new TrailItem(position, altitude, Qt::green, map); + trail->addToGroup(ob); + connect(this, SIGNAL(setChildPosition()), ob, SLOT(setPosSLOT())); + if (!lasttrailline.IsEmpty()) { + TrailLineItem *obj = new TrailLineItem(lasttrailline, position, Qt::red, map); + trailLine->addToGroup(obj); + connect(this, SIGNAL(setChildLine()), obj, SLOT(setLineSlot())); + } + lasttrailline = position; + lastcoord = position; + } + } + coord = position; + this->altitude = altitude; + RefreshPos(); + if (mapfollowtype == UAVMapFollowType::CenterAndRotateMap || mapfollowtype == UAVMapFollowType::CenterMap) { + mapwidget->SetCurrentPosition(coord); + } + if (autosetreached) { + foreach(QGraphicsItem * i, map->childItems()) { + WayPointItem *wp = qgraphicsitem_cast(i); + + if (wp) { + if (Distance3D(wp->Coord(), wp->Altitude()) < autosetdistance) { + wp->SetReached(true); + emit UAVReachedWayPoint(wp->Number(), wp); + } + } + } + } + if (mapwidget->Home != 0) { + // verify if the UAV is inside the safety bouble + if (Distance3D(mapwidget->Home->Coord(), mapwidget->Home->Altitude()) > mapwidget->Home->SafeArea()) { + if (mapwidget->Home->safe != false) { + mapwidget->Home->safe = false; + mapwidget->Home->update(); + emit UAVLeftSafetyBouble(this->coord); + } + } else { + if (mapwidget->Home->safe != true) { + mapwidget->Home->safe = true; + mapwidget->Home->update(); + } + } + } + } +} + +/** + * Rotate the UAV Icon on the map, or rotate the map + * depending on the display mode + */ +void UAVItem::SetUAVHeading(const qreal &value) +{ + if (mapfollowtype == UAVMapFollowType::CenterAndRotateMap) { + mapwidget->SetRotate(-value); + } else { + if (this->rotation() != value) { + this->setRotation(value); + } + } +} + + +int UAVItem::type() const +{ + return Type; +} + + +void UAVItem::RefreshPos() +{ + localposition = map->FromLatLngToLocal(coord); + this->setPos(localposition.X(), localposition.Y()); + emit setChildPosition(); + emit setChildLine(); + updateTextOverlay(); +} + +void UAVItem::setOpacitySlot(qreal opacity) +{ + this->setOpacity(opacity); +} + +void UAVItem::zoomChangedSlot() +{ + double pixels2meters = map->Projection()->GetGroundResolution(map->ZoomTotal(), coord.Lat()); + + meters2pixels = 1.0 / pixels2meters; + boundingRectSize = groundspeed_mps_filt * ringTime * 4 * meters2pixels + 20; + prepareGeometryChange(); + updateTextOverlay(); + update(); +} +void UAVItem::SetTrailType(const UAVTrailType::Types &value) +{ + trailtype = value; + if (trailtype == UAVTrailType::ByTimeElapsed) { + timer.restart(); + } +} +void UAVItem::SetShowTrail(const bool &value) +{ + showtrail = value; + trail->setVisible(value); +} +void UAVItem::SetShowTrailLine(const bool &value) +{ + showtrailline = value; + trailLine->setVisible(value); +} + +void UAVItem::DeleteTrail() const +{ + foreach(QGraphicsItem * i, trail->childItems()) + delete i; + foreach(QGraphicsItem * i, trailLine->childItems()) + delete i; +} +double UAVItem::Distance3D(const internals::PointLatLng &coord, const int &altitude) +{ + return sqrt(pow(internals::PureProjection::DistanceBetweenLatLng(this->coord, coord) * 1000, 2) + + pow(this->altitude - altitude, 2)); +} +void UAVItem::SetUavPic(QString UAVPic) +{ + pic.load(":/uavs/images/" + UAVPic); +} + +void UAVItem::SetShowUAVInfo(bool const & value) +{ + showUAVInfo = value; + showJustChanged = true; + update(); +} + +void UAVItem::generateArrowhead() +{ + qreal arrowSize = 10; + + // Create line from (0,0), to (1,1). Later, we'll scale and rotate it + arrowShaft = QLineF(0, 0, 1.0, 1.0); + + // Set the starting point to (0,0) + arrowShaft.setP1(QPointF(0, 0)); + + // Set angle and length + arrowShaft.setLength(60.0); + arrowShaft.setAngle(90.0); + + // Form arrowhead + double angle = ::acos(arrowShaft.dx() / arrowShaft.length()); + if (arrowShaft.dy() <= 0) { + angle = (M_PI * 2) - angle; + } + + QPointF arrowP1 = arrowShaft.pointAt(1) + QPointF(sin(angle + M_PI / 3) * arrowSize, + cos(angle + M_PI / 3) * arrowSize); + QPointF arrowP2 = arrowShaft.pointAt(1) + QPointF(sin(angle + M_PI - M_PI / 3) * arrowSize, + cos(angle + M_PI - M_PI / 3) * arrowSize); + + // Assemble arrowhead + arrowHead.clear(); + arrowHead << arrowShaft.pointAt(1) << arrowP1 << arrowP2; +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.h index 472563f37..e137ceac1 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file uavitem.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a WayPoint -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file uavitem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef UAVITEM_H #define UAVITEM_H @@ -40,238 +40,277 @@ #include "opmapwidget.h" #include "trailitem.h" #include "traillineitem.h" -namespace mapcontrol -{ - class WayPointItem; - class OPMapWidget; +namespace mapcontrol { +class WayPointItem; +class OPMapWidget; +/** + * @brief A QGraphicsItem representing the UAV + * + * @class UAVItem uavitem.h "mapwidget/uavitem.h" + */ +class UAVItem : public QObject, public QGraphicsItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) +public: + enum { Type = UserType + 2 }; + UAVItem(MapGraphicItem *map, OPMapWidget *parent, QString uavPic = QString::fromUtf8(":/uavs/images/mapquad.png")); + ~UAVItem(); + /** -* @brief A QGraphicsItem representing the UAV -* -* @class UAVItem uavitem.h "mapwidget/uavitem.h" -*/ - class UAVItem:public QObject,public QGraphicsItem + * @brief Sets the UAV NED position + * + * @param NED + */ + void SetNED(double NED[3]); + /** + * @brief Sets the UAV groundspeed + * + * @param NED + */ + void SetGroundspeed(double vNED[3], int m_maxUpdateRate); + /** + * @brief Sets the UAV Calibrated Airspeed + * + * @param NED + */ + void SetCAS(double CAS); + /** + * @brief Sets the UAV yaw rate + * + * @param NED + */ + void SetYawRate(double yawRate_dps); + /** + * @brief Sets the UAV position + * + * @param position LatLng point + * @param altitude altitude in meters + */ + void SetUAVPos(internals::PointLatLng const & position, int const & altitude); + /** + * @brief Sets the UAV heading + * + * @param value heading angle (north=0deg) + */ + void SetUAVHeading(qreal const & value); + /** + * @brief Returns the UAV position + * + * @return internals::PointLatLng + */ + internals::PointLatLng UAVPos() const { - Q_OBJECT - Q_INTERFACES(QGraphicsItem) - public: - enum { Type = UserType + 2 }; - UAVItem(MapGraphicItem* map,OPMapWidget* parent, QString uavPic=QString::fromUtf8(":/uavs/images/mapquad.png")); - ~UAVItem(); + return coord; + } + /** + * @brief Sets the Map follow type + * + * @param value can be "none"(map doesnt follow UAV), "CenterMap"(map moves to keep UAV centered) or "CenterAndRotateMap"(map moves and rotates to keep UAV centered and straight) + */ + void SetMapFollowType(UAVMapFollowType::Types const & value) + { + mapfollowtype = value; + } + /** + * @brief Sets the trail type + * + * @param value can be "NoTrail"(no trail is plotted), "ByTimeElapsed"(a trail point is plotted each TrailTime()) or ByDistance(a trail point is plotted if the distance between the UAV and the last trail point is bigger than TrailDistance()) + */ + void SetTrailType(UAVTrailType::Types const & value); + /** + * @brief Returns the map follow method used + * + * @return UAVMapFollowType::Types + */ + UAVMapFollowType::Types GetMapFollowType() const + { + return mapfollowtype; + } + /** + * @brief Returns the UAV trail type. It can be plotted by time elapsed or distance + * + * @return UAVTrailType::Types + */ + UAVTrailType::Types GetTrailType() const + { + return trailtype; + } - /** - * @brief Sets the UAV NED position - * - * @param NED - */ - void SetNED(double NED[3]); - /** - * @brief Sets the UAV groundspeed - * - * @param NED - */ - void SetGroundspeed(double vNED[3], int m_maxUpdateRate); - /** - * @brief Sets the UAV Calibrated Airspeed - * - * @param NED - */ - void SetCAS(double CAS); - /** - * @brief Sets the UAV yaw rate - * - * @param NED - */ - void SetYawRate(double yawRate_dps); - /** - * @brief Sets the UAV position - * - * @param position LatLng point - * @param altitude altitude in meters - */ - void SetUAVPos(internals::PointLatLng const& position,int const& altitude); - /** - * @brief Sets the UAV heading - * - * @param value heading angle (north=0deg) - */ - void SetUAVHeading(qreal const& value); - /** - * @brief Returns the UAV position - * - * @return internals::PointLatLng - */ - internals::PointLatLng UAVPos()const{return coord;} - /** - * @brief Sets the Map follow type - * - * @param value can be "none"(map doesnt follow UAV), "CenterMap"(map moves to keep UAV centered) or "CenterAndRotateMap"(map moves and rotates to keep UAV centered and straight) - */ - void SetMapFollowType(UAVMapFollowType::Types const& value){mapfollowtype=value;} - /** - * @brief Sets the trail type - * - * @param value can be "NoTrail"(no trail is plotted), "ByTimeElapsed"(a trail point is plotted each TrailTime()) or ByDistance(a trail point is plotted if the distance between the UAV and the last trail point is bigger than TrailDistance()) - */ - void SetTrailType(UAVTrailType::Types const& value); - /** - * @brief Returns the map follow method used - * - * @return UAVMapFollowType::Types - */ - UAVMapFollowType::Types GetMapFollowType()const{return mapfollowtype;} - /** - * @brief Returns the UAV trail type. It can be plotted by time elapsed or distance - * - * @return UAVTrailType::Types - */ - UAVTrailType::Types GetTrailType()const{return trailtype;} + void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, + QWidget *widget); + QRectF boundingRect() const; + /** + * @brief Sets the trail time to be used if TrailType is ByTimeElapsed + * + * @param seconds the UAV trail time elapsed value. If the trail type is time elapsed + * a trail point will be plotted each "value returned" seconds. + */ + void SetTrailTime(int const & seconds) + { + trailtime = seconds; + } + /** + * @brief Returns the UAV trail time elapsed value. If the trail type is time elapsed + * a trail point will be plotted each "value returned" seconds. + * + * @return int + */ + int TrailTime() const + { + return trailtime; + } + /** + * @brief Sets the trail distance to be used if TrailType is ByDistance + * + * @param distance the UAV trail plot distance. + * If the trail type is ByDistance a trail dot is plotted if + * the distance between the current UAV position and the last trail point + * is bigger than the returned value + */ + void SetTrailDistance(int const & distance) + { + traildistance = distance; + } + /** + * @brief Returns the UAV trail plot distance. + * If the trail type is distance diference a trail dot is plotted if + * the distance between the current UAV position and the last trail point + * is bigger than the returned value + * + * @return int + */ + int TrailDistance() const + { + return traildistance; + } + /** + * @brief Returns true if UAV trail is shown + * + * @return bool + */ + bool ShowTrail() const + { + return showtrail; + } + /** + * @brief Returns true if UAV trail line is shown + * + * @return bool + */ + bool ShowTrailLine() const + { + return showtrailline; + } + /** + * @brief Used to define if the UAV displays a trail + * + * @param value + */ + void SetShowTrail(bool const & value); + /** + * @brief Used to define if the UAV displays a trail line + * + * @param value + */ + void SetShowTrailLine(bool const & value); + /** + * @brief Deletes all the trail points + */ + void DeleteTrail() const; + /** + * @brief Returns true if the UAV automaticaly sets WP reached value (changing its color) + * + * @return bool + */ + bool AutoSetReached() const + { + return autosetreached; + } + /** + * @brief Defines if the UAV can set the WP's "reached" value automaticaly. + * + * @param value + */ + void SetAutoSetReached(bool const & value) + { + autosetreached = value; + } + /** + * @brief Returns the 3D distance in meters necessary for the UAV to set WP's to "reached" + * + * @return double + */ + double AutoSetDistance() const + { + return autosetdistance; + } + /** + * @brief Sets the the 3D distance in meters necessary for the UAV to set WP's to "reached" + * + * @param value + */ + void SetAutoSetDistance(double const & value) + { + autosetdistance = value; + } - void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, - QWidget *widget); - QRectF boundingRect() const; - /** - * @brief Sets the trail time to be used if TrailType is ByTimeElapsed - * - * @param seconds the UAV trail time elapsed value. If the trail type is time elapsed - * a trail point will be plotted each "value returned" seconds. - */ - void SetTrailTime(int const& seconds){trailtime=seconds;} - /** - * @brief Returns the UAV trail time elapsed value. If the trail type is time elapsed - * a trail point will be plotted each "value returned" seconds. - * - * @return int - */ - int TrailTime()const{return trailtime;} - /** - * @brief Sets the trail distance to be used if TrailType is ByDistance - * - * @param distance the UAV trail plot distance. - * If the trail type is ByDistance a trail dot is plotted if - * the distance between the current UAV position and the last trail point - * is bigger than the returned value - */ - void SetTrailDistance(int const& distance){traildistance=distance;} - /** - * @brief Returns the UAV trail plot distance. - * If the trail type is distance diference a trail dot is plotted if - * the distance between the current UAV position and the last trail point - * is bigger than the returned value - * - * @return int - */ - int TrailDistance()const{return traildistance;} - /** - * @brief Returns true if UAV trail is shown - * - * @return bool - */ - bool ShowTrail()const{return showtrail;} - /** - * @brief Returns true if UAV trail line is shown - * - * @return bool - */ - bool ShowTrailLine()const{return showtrailline;} - /** - * @brief Used to define if the UAV displays a trail - * - * @param value - */ - void SetShowTrail(bool const& value); - /** - * @brief Used to define if the UAV displays a trail line - * - * @param value - */ - void SetShowTrailLine(bool const& value); - /** - * @brief Deletes all the trail points - */ - void DeleteTrail()const; - /** - * @brief Returns true if the UAV automaticaly sets WP reached value (changing its color) - * - * @return bool - */ - bool AutoSetReached()const{return autosetreached;} - /** - * @brief Defines if the UAV can set the WP's "reached" value automaticaly. - * - * @param value - */ - void SetAutoSetReached(bool const& value){autosetreached=value;} - /** - * @brief Returns the 3D distance in meters necessary for the UAV to set WP's to "reached" - * - * @return double - */ - double AutoSetDistance()const{return autosetdistance;} - /** - * @brief Sets the the 3D distance in meters necessary for the UAV to set WP's to "reached" - * - * @param value - */ - void SetAutoSetDistance(double const& value){autosetdistance=value;} + int type() const; - int type() const; + void SetUavPic(QString UAVPic); + void SetShowUAVInfo(bool const & value); + void updateTextOverlay(); +private: + void generateArrowhead(); + MapGraphicItem *map; + OPMapWidget *mapwidget; + QPolygonF arrowHead; + QLineF arrowShaft; + int altitude; + UAVMapFollowType::Types mapfollowtype; + UAVTrailType::Types trailtype; + internals::PointLatLng coord; + internals::PointLatLng lastcoord; + double NED[3]; + double vNED[3]; + double CAS_mps; + double groundspeed_kph; + double groundspeed_mps; + double yawRate_dps; + double trendRadius; + double trendSpanAngle; + float meters2pixels; + double precalcRings; + double ringTime; + QPixmap pic; + core::Point localposition; + QGraphicsItemGroup *trail; + QGraphicsItemGroup *trailLine; + internals::PointLatLng lasttrailline; + QTime timer; + bool showtrail; + bool showtrailline; + int trailtime; + int traildistance; + bool autosetreached; + double Distance3D(internals::PointLatLng const & coord, int const & altitude); + double autosetdistance; + bool showUAVInfo; + static double groundspeed_mps_filt; + float boundingRectSize; + bool showJustChanged; - void SetUavPic(QString UAVPic); - void SetShowUAVInfo(bool const& value); - void updateTextOverlay(); - private: - void generateArrowhead(); - MapGraphicItem* map; - OPMapWidget* mapwidget; - QPolygonF arrowHead; - QLineF arrowShaft; - int altitude; - UAVMapFollowType::Types mapfollowtype; - UAVTrailType::Types trailtype; - internals::PointLatLng coord; - internals::PointLatLng lastcoord; - double NED[3]; - double vNED[3]; - double CAS_mps; - double groundspeed_kph; - double groundspeed_mps; - double yawRate_dps; - double trendRadius; - double trendSpanAngle; - float meters2pixels; - double precalcRings; - double ringTime; - QPixmap pic; - core::Point localposition; - QGraphicsItemGroup* trail; - QGraphicsItemGroup * trailLine; - internals::PointLatLng lasttrailline; - QTime timer; - bool showtrail; - bool showtrailline; - int trailtime; - int traildistance; - bool autosetreached; - double Distance3D(internals::PointLatLng const& coord, int const& altitude); - double autosetdistance; - bool showUAVInfo; - static double groundspeed_mps_filt; - float boundingRectSize; - bool showJustChanged; + bool refreshPaint_flag; - bool refreshPaint_flag; + QPainterPath textPath; - QPainterPath textPath; - - public slots: - void RefreshPos(); - void setOpacitySlot(qreal opacity); - void zoomChangedSlot(); - signals: - void UAVReachedWayPoint(int const& waypointnumber,WayPointItem* waypoint); - void UAVLeftSafetyBouble(internals::PointLatLng const& position); - void setChildPosition(); - void setChildLine(); - }; +public slots: + void RefreshPos(); + void setOpacitySlot(qreal opacity); + void zoomChangedSlot(); +signals: + void UAVReachedWayPoint(int const & waypointnumber, WayPointItem *waypoint); + void UAVLeftSafetyBouble(internals::PointLatLng const & position); + void setChildPosition(); + void setChildLine(); +}; } #endif // UAVITEM_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavmapfollowtype.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavmapfollowtype.h index b4c870e87..9f7c06279 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavmapfollowtype.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavmapfollowtype.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file uavmapfollowtype.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief An enum representing the various map follow modes -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file uavmapfollowtype.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief An enum representing the various map follow modes + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef UAVMAPFOLLOWTYPE_H #define UAVMAPFOLLOWTYPE_H @@ -32,55 +32,53 @@ #include #include namespace mapcontrol { - class UAVMapFollowType:public QObject - { - Q_OBJECT - Q_ENUMS(Types) - public: - enum Types - { - /// - /// only centers the map on the UAV - /// - CenterMap, +class UAVMapFollowType : public QObject { + Q_OBJECT Q_ENUMS(Types) +public: + enum Types { + /// + /// only centers the map on the UAV + /// + CenterMap, - /// - /// centers and rotates map on the UAV - /// - CenterAndRotateMap, - - /// - /// map is not connected to UAV position or heading - /// - None - }; - static QString StrByType(Types const& value) - { - QMetaObject metaObject = UAVMapFollowType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - QString s=metaEnum.valueToKey(value); - return s; - } - static Types TypeByStr(QString const& value) - { - QMetaObject metaObject = UAVMapFollowType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - Types s=(Types)metaEnum.keyToValue(value.toLatin1()); - return s; - } - static QStringList TypesList() - { - QStringList ret; - QMetaObject metaObject = UAVMapFollowType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - for(int x=0;x + /// centers and rotates map on the UAV + /// + CenterAndRotateMap, + /// + /// map is not connected to UAV position or heading + /// + None }; + static QString StrByType(Types const & value) + { + QMetaObject metaObject = UAVMapFollowType().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + QString s = metaEnum.valueToKey(value); + + return s; + } + static Types TypeByStr(QString const & value) + { + QMetaObject metaObject = UAVMapFollowType().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + Types s = (Types)metaEnum.keyToValue(value.toLatin1()); + + return s; + } + static QStringList TypesList() + { + QStringList ret; + QMetaObject metaObject = UAVMapFollowType().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + + for (int x = 0; x < metaEnum.keyCount(); ++x) { + ret.append(metaEnum.key(x)); + } + return ret; + } +}; } #endif // UAVMAPFOLLOWTYPE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavtrailtype.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavtrailtype.h index 2a17d4b61..3dc5a18bf 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavtrailtype.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavtrailtype.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file uavtrailtype.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief An enum representing the uav trailing modes -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file uavtrailtype.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief An enum representing the uav trailing modes + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef UAVTRAILTYPE_H #define UAVTRAILTYPE_H @@ -32,59 +32,57 @@ #include #include namespace mapcontrol { - class UAVTrailType:public QObject - { - Q_OBJECT - Q_ENUMS(Types) - public: - enum Types - { - /** - * @brief UAV does not plot a trail - * - * @var NoTrail - */ - NoTrail, - /** - * @brief UAV plots a trail point every 'x' seconds - * - * @var ByTimeElapsed - */ - ByTimeElapsed, - /** - * @brief UAV plots a trail point every 'x' meters (ground distance) - * - * @var ByDistance - */ - ByDistance - }; - static QString StrByType(Types const& value) - { - QMetaObject metaObject = UAVTrailType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - QString s=metaEnum.valueToKey(value); - return s; - } - static Types TypeByStr(QString const& value) - { - QMetaObject metaObject = UAVTrailType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - Types s=(Types)metaEnum.keyToValue(value.toLatin1()); - return s; - } - static QStringList TypesList() - { - QStringList ret; - QMetaObject metaObject = UAVTrailType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - for(int x=0;x #include "homeitem.h" -namespace mapcontrol +namespace mapcontrol { +WayPointCircle::WayPointCircle(WayPointItem *center, WayPointItem *radius, bool clockwise, MapGraphicItem *map, QColor color) : my_center(center), + my_radius(radius), my_map(map), QGraphicsEllipseItem(map), myColor(color), myClockWise(clockwise) { -WayPointCircle::WayPointCircle(WayPointItem *center, WayPointItem *radius,bool clockwise, MapGraphicItem *map,QColor color):my_center(center), - my_radius(radius),my_map(map),QGraphicsEllipseItem(map),myColor(color),myClockWise(clockwise) -{ - connect(center,SIGNAL(localPositionChanged(QPointF,WayPointItem*)),this,SLOT(refreshLocations())); - connect(radius,SIGNAL(localPositionChanged(QPointF,WayPointItem*)),this,SLOT(refreshLocations())); - connect(center,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); - connect(radius,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); + connect(center, SIGNAL(localPositionChanged(QPointF, WayPointItem *)), this, SLOT(refreshLocations())); + connect(radius, SIGNAL(localPositionChanged(QPointF, WayPointItem *)), this, SLOT(refreshLocations())); + connect(center, SIGNAL(aboutToBeDeleted(WayPointItem *)), this, SLOT(waypointdeleted())); + connect(radius, SIGNAL(aboutToBeDeleted(WayPointItem *)), this, SLOT(waypointdeleted())); refreshLocations(); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); - + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); } -WayPointCircle::WayPointCircle(HomeItem *radius, WayPointItem *center, bool clockwise, MapGraphicItem *map, QColor color):my_center(center), - my_radius(radius),my_map(map),QGraphicsEllipseItem(map),myColor(color),myClockWise(clockwise) +WayPointCircle::WayPointCircle(HomeItem *radius, WayPointItem *center, bool clockwise, MapGraphicItem *map, QColor color) : my_center(center), + my_radius(radius), my_map(map), QGraphicsEllipseItem(map), myColor(color), myClockWise(clockwise) { - connect(radius,SIGNAL(homePositionChanged(internals::PointLatLng,float)),this,SLOT(refreshLocations())); - connect(center,SIGNAL(localPositionChanged(QPointF)),this,SLOT(refreshLocations())); - connect(center,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); + connect(radius, SIGNAL(homePositionChanged(internals::PointLatLng, float)), this, SLOT(refreshLocations())); + connect(center, SIGNAL(localPositionChanged(QPointF)), this, SLOT(refreshLocations())); + connect(center, SIGNAL(aboutToBeDeleted(WayPointItem *)), this, SLOT(waypointdeleted())); refreshLocations(); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); } int WayPointCircle::type() const @@ -65,44 +63,44 @@ void WayPointCircle::paint(QPainter *painter, const QStyleOptionGraphicsItem *op QPointF p1; QPointF p2; - p1=QPointF(line.p1().x(),line.p1().y()+line.length()); - p2=QPointF(line.p1().x(),line.p1().y()-line.length()); + p1 = QPointF(line.p1().x(), line.p1().y() + line.length()); + p2 = QPointF(line.p1().x(), line.p1().y() - line.length()); QPen myPen = pen(); myPen.setColor(myColor); qreal arrowSize = 10; painter->setPen(myPen); - QBrush brush=painter->brush(); + QBrush brush = painter->brush(); painter->setBrush(myColor); - double angle =0; - if(!myClockWise) - angle+=M_PI; + double angle = 0; + if (!myClockWise) { + angle += M_PI; + } - QPointF arrowP1 = p1 + QPointF(sin(angle + M_PI / 3) * arrowSize, - cos(angle + M_PI / 3) * arrowSize); - QPointF arrowP2 = p1 + QPointF(sin(angle + M_PI - M_PI / 3) * arrowSize, - cos(angle + M_PI - M_PI / 3) * arrowSize); + QPointF arrowP1 = p1 + QPointF(sin(angle + M_PI / 3) * arrowSize, + cos(angle + M_PI / 3) * arrowSize); + QPointF arrowP2 = p1 + QPointF(sin(angle + M_PI - M_PI / 3) * arrowSize, + cos(angle + M_PI - M_PI / 3) * arrowSize); - QPointF arrowP21 = p2 + QPointF(sin(angle + M_PI + M_PI / 3) * arrowSize, - cos(angle + M_PI + M_PI / 3) * arrowSize); - QPointF arrowP22 = p2 + QPointF(sin(angle + M_PI + M_PI - M_PI / 3) * arrowSize, - cos(angle + M_PI + M_PI - M_PI / 3) * arrowSize); - - arrowHead.clear(); - arrowHead << p1 << arrowP1 << arrowP2; - painter->drawPolygon(arrowHead); - arrowHead.clear(); - arrowHead << p2 << arrowP21 << arrowP22; - painter->drawPolygon(arrowHead); - painter->translate(-line.length(),-line.length()); - painter->setBrush(brush); - painter->drawEllipse(this->rect()); + QPointF arrowP21 = p2 + QPointF(sin(angle + M_PI + M_PI / 3) * arrowSize, + cos(angle + M_PI + M_PI / 3) * arrowSize); + QPointF arrowP22 = p2 + QPointF(sin(angle + M_PI + M_PI - M_PI / 3) * arrowSize, + cos(angle + M_PI + M_PI - M_PI / 3) * arrowSize); + arrowHead.clear(); + arrowHead << p1 << arrowP1 << arrowP2; + painter->drawPolygon(arrowHead); + arrowHead.clear(); + arrowHead << p2 << arrowP21 << arrowP22; + painter->drawPolygon(arrowHead); + painter->translate(-line.length(), -line.length()); + painter->setBrush(brush); + painter->drawEllipse(this->rect()); } void WayPointCircle::refreshLocations() { - line=QLineF(my_center->pos(),my_radius->pos()); - this->setRect(my_center->pos().x(),my_center->pos().y(),2*line.length(),2*line.length()); + line = QLineF(my_center->pos(), my_radius->pos()); + this->setRect(my_center->pos().x(), my_center->pos().y(), 2 * line.length(), 2 * line.length()); this->update(); } @@ -115,5 +113,4 @@ void WayPointCircle::setOpacitySlot(qreal opacity) { setOpacity(opacity); } - } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.h index db839819d..ec6ac5a13 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file waypointcircle.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a circle connecting 2 waypoints -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file waypointcircle.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a circle connecting 2 waypoints + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef WAYPOINTCIRCLE_H #define WAYPOINTCIRCLE_H #include @@ -35,24 +35,22 @@ #include #include -namespace mapcontrol -{ - -class WayPointCircle:public QObject,public QGraphicsEllipseItem -{ - Q_OBJECT - Q_INTERFACES(QGraphicsItem) +namespace mapcontrol { +class WayPointCircle : public QObject, public QGraphicsEllipseItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) public: enum { Type = UserType + 9 }; - WayPointCircle(WayPointItem * center, WayPointItem * radius,bool clockwise,MapGraphicItem * map,QColor color=Qt::green); - WayPointCircle(HomeItem * center, WayPointItem * radius,bool clockwise,MapGraphicItem * map,QColor color=Qt::green); + WayPointCircle(WayPointItem *center, WayPointItem *radius, bool clockwise, MapGraphicItem *map, QColor color = Qt::green); + WayPointCircle(HomeItem *center, WayPointItem *radius, bool clockwise, MapGraphicItem *map, QColor color = Qt::green); int type() const; void setColor(const QColor &color) - { myColor = color; } + { + myColor = color; + } private: - QGraphicsItem * my_center; - QGraphicsItem * my_radius; - MapGraphicItem * my_map; + QGraphicsItem *my_center; + QGraphicsItem *my_radius; + MapGraphicItem *my_map; QPolygonF arrowHead; QColor myColor; bool myClockWise; diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp index 09e6c4d12..59c2ecb25 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp @@ -1,505 +1,485 @@ /** -****************************************************************************** -* -* @file waypointitem.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a WayPoint -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file waypointitem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "waypointitem.h" #include "homeitem.h" -namespace mapcontrol +namespace mapcontrol { +WayPointItem::WayPointItem(const internals::PointLatLng &coord, int const & altitude, MapGraphicItem *map, wptype type) : coord(coord), reached(false), description(""), shownumber(true), isDragging(false), altitude(altitude), map(map), myType(type) { -WayPointItem::WayPointItem(const internals::PointLatLng &coord,int const& altitude, MapGraphicItem *map,wptype type):coord(coord),reached(false),description(""),shownumber(true),isDragging(false),altitude(altitude),map(map),myType(type) - { - text=0; - numberI=0; - isMagic=false; - picture.load(QString::fromUtf8(":/markers/images/marker.png")); - number=WayPointItem::snumber; - ++WayPointItem::snumber; - this->setFlag(QGraphicsItem::ItemIsMovable,true); - this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); - this->setFlag(QGraphicsItem::ItemIsSelectable,true); - SetShowNumber(shownumber); - RefreshToolTip(); - RefreshPos(); - myHome=NULL; - QList list=map->childItems(); - foreach(QGraphicsItem * obj,list) - { - HomeItem* h=qgraphicsitem_cast (obj); - if(h) - myHome=h; - } - - if(myHome) - { - map->Projection()->offSetFromLatLngs(myHome->Coord(),coord,relativeCoord.distance,relativeCoord.bearing); - relativeCoord.altitudeRelative=Altitude()-myHome->Altitude(); - connect(myHome,SIGNAL(homePositionChanged(internals::PointLatLng,float)),this,SLOT(onHomePositionChanged(internals::PointLatLng,float))); - } - connect(this,SIGNAL(waypointdoubleclick(WayPointItem*)),map,SIGNAL(wpdoubleclicked(WayPointItem*))); - emit manualCoordChange(this); - connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); -} - -WayPointItem::WayPointItem(MapGraphicItem *map, bool magicwaypoint):reached(false),description(""),shownumber(true),isDragging(false),altitude(0),map(map) -{ - relativeCoord.bearing=0; - relativeCoord.distance=0; - relativeCoord.altitudeRelative=0; - myType=relative; - if(magicwaypoint) - { - isMagic=true; - picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker3.png")); - number=-1; - } - else - { - isMagic=false; - number=WayPointItem::snumber; - ++WayPointItem::snumber; - } - text=0; - numberI=0; - this->setFlag(QGraphicsItem::ItemIsMovable,true); - this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); - this->setFlag(QGraphicsItem::ItemIsSelectable,true); + text = 0; + numberI = 0; + isMagic = false; + picture.load(QString::fromUtf8(":/markers/images/marker.png")); + number = WayPointItem::snumber; + ++WayPointItem::snumber; + this->setFlag(QGraphicsItem::ItemIsMovable, true); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + this->setFlag(QGraphicsItem::ItemIsSelectable, true); SetShowNumber(shownumber); RefreshToolTip(); RefreshPos(); - myHome=NULL; - QList list=map->childItems(); - foreach(QGraphicsItem * obj,list) - { - HomeItem* h=qgraphicsitem_cast (obj); - if(h) - myHome=h; + myHome = NULL; + QList list = map->childItems(); + foreach(QGraphicsItem * obj, list) { + HomeItem *h = qgraphicsitem_cast (obj); + + if (h) { + myHome = h; + } } - if(myHome) - { - coord=map->Projection()->translate(myHome->Coord(),relativeCoord.distance,relativeCoord.bearing); - SetAltitude(myHome->Altitude()+relativeCoord.altitudeRelative); - connect(myHome,SIGNAL(homePositionChanged(internals::PointLatLng,float)),this,SLOT(onHomePositionChanged(internals::PointLatLng,float))); + if (myHome) { + map->Projection()->offSetFromLatLngs(myHome->Coord(), coord, relativeCoord.distance, relativeCoord.bearing); + relativeCoord.altitudeRelative = Altitude() - myHome->Altitude(); + connect(myHome, SIGNAL(homePositionChanged(internals::PointLatLng, float)), this, SLOT(onHomePositionChanged(internals::PointLatLng, float))); } - connect(this,SIGNAL(waypointdoubleclick(WayPointItem*)),map,SIGNAL(wpdoubleclicked(WayPointItem*))); + connect(this, SIGNAL(waypointdoubleclick(WayPointItem *)), map, SIGNAL(wpdoubleclicked(WayPointItem *))); emit manualCoordChange(this); - connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); } - WayPointItem::WayPointItem(const internals::PointLatLng &coord,int const& altitude, const QString &description, MapGraphicItem *map,wptype type):coord(coord),reached(false),description(description),shownumber(true),isDragging(false),altitude(altitude),map(map),myType(type) - { - text=0; - numberI=0; - isMagic=false; - picture.load(QString::fromUtf8(":/markers/images/marker.png")); - number=WayPointItem::snumber; + +WayPointItem::WayPointItem(MapGraphicItem *map, bool magicwaypoint) : reached(false), description(""), shownumber(true), isDragging(false), altitude(0), map(map) +{ + relativeCoord.bearing = 0; + relativeCoord.distance = 0; + relativeCoord.altitudeRelative = 0; + myType = relative; + if (magicwaypoint) { + isMagic = true; + picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker3.png")); + number = -1; + } else { + isMagic = false; + number = WayPointItem::snumber; ++WayPointItem::snumber; - this->setFlag(QGraphicsItem::ItemIsMovable,true); - this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); - this->setFlag(QGraphicsItem::ItemIsSelectable,true); - SetShowNumber(shownumber); - RefreshToolTip(); - RefreshPos(); - myHome=NULL; - QList list=map->childItems(); - foreach(QGraphicsItem * obj,list) - { - HomeItem* h=qgraphicsitem_cast (obj); - if(h) - myHome=h; - } - if(myHome) - { - map->Projection()->offSetFromLatLngs(myHome->Coord(),coord,relativeCoord.distance,relativeCoord.bearing); - relativeCoord.altitudeRelative=Altitude()-myHome->Altitude(); - connect(myHome,SIGNAL(homePositionChanged(internals::PointLatLng,float)),this,SLOT(onHomePositionChanged(internals::PointLatLng,float))); - } - connect(this,SIGNAL(waypointdoubleclick(WayPointItem*)),map,SIGNAL(wpdoubleclicked(WayPointItem*))); - emit manualCoordChange(this); - connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); } + text = 0; + numberI = 0; + this->setFlag(QGraphicsItem::ItemIsMovable, true); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + this->setFlag(QGraphicsItem::ItemIsSelectable, true); + SetShowNumber(shownumber); + RefreshToolTip(); + RefreshPos(); + myHome = NULL; + QList list = map->childItems(); + foreach(QGraphicsItem * obj, list) { + HomeItem *h = qgraphicsitem_cast (obj); - WayPointItem::WayPointItem(const distBearingAltitude &relativeCoordenate, const QString &description, MapGraphicItem *map):relativeCoord(relativeCoordenate),reached(false),description(description),shownumber(true),isDragging(false),map(map) - { - myHome=NULL; - QList list=map->childItems(); - foreach(QGraphicsItem * obj,list) - { - HomeItem* h=qgraphicsitem_cast (obj); - if(h) - myHome=h; - } - if(myHome) - { - connect(myHome,SIGNAL(homePositionChanged(internals::PointLatLng,float)),this,SLOT(onHomePositionChanged(internals::PointLatLng,float))); - coord=map->Projection()->translate(myHome->Coord(),relativeCoord.distance,relativeCoord.bearing); - SetAltitude(myHome->Altitude()+relativeCoord.altitudeRelative); - } - myType=relative; - text=0; - numberI=0; - isMagic=false; - picture.load(QString::fromUtf8(":/markers/images/marker.png")); - number=WayPointItem::snumber; - ++WayPointItem::snumber; - this->setFlag(QGraphicsItem::ItemIsMovable,true); - this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); - this->setFlag(QGraphicsItem::ItemIsSelectable,true); - SetShowNumber(shownumber); - RefreshToolTip(); - RefreshPos(); - connect(this,SIGNAL(waypointdoubleclick(WayPointItem*)),map,SIGNAL(wpdoubleclicked(WayPointItem*))); - emit manualCoordChange(this); - connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); - } - - void WayPointItem::setWPType(wptype type) - { - myType=type; - emit WPValuesChanged(this); - RefreshPos(); - RefreshToolTip(); - this->update(); - } - - QRectF WayPointItem::boundingRect() const - { - return QRectF(-picture.width()/2,-picture.height(),picture.width(),picture.height()); - } - void WayPointItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) - { - Q_UNUSED(option); - Q_UNUSED(widget); - painter->drawPixmap(-picture.width()/2,-picture.height(),picture); - painter->setPen(Qt::green); - if(this->isSelected()) - painter->drawRect(QRectF(-picture.width()/2,-picture.height(),picture.width()-1,picture.height()-1)); - } - void WayPointItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent *event) - { - if(event->button()==Qt::LeftButton) - { - emit waypointdoubleclick(this); + if (h) { + myHome = h; } } - void WayPointItem::mousePressEvent(QGraphicsSceneMouseEvent *event) - { - if(event->button()==Qt::LeftButton) - { - text=new QGraphicsSimpleTextItem(this); - textBG=new QGraphicsRectItem(this); + if (myHome) { + coord = map->Projection()->translate(myHome->Coord(), relativeCoord.distance, relativeCoord.bearing); + SetAltitude(myHome->Altitude() + relativeCoord.altitudeRelative); + connect(myHome, SIGNAL(homePositionChanged(internals::PointLatLng, float)), this, SLOT(onHomePositionChanged(internals::PointLatLng, float))); + } + connect(this, SIGNAL(waypointdoubleclick(WayPointItem *)), map, SIGNAL(wpdoubleclicked(WayPointItem *))); + emit manualCoordChange(this); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); +} +WayPointItem::WayPointItem(const internals::PointLatLng &coord, int const & altitude, const QString &description, MapGraphicItem *map, wptype type) : coord(coord), reached(false), description(description), shownumber(true), isDragging(false), altitude(altitude), map(map), myType(type) +{ + text = 0; + numberI = 0; + isMagic = false; + picture.load(QString::fromUtf8(":/markers/images/marker.png")); + number = WayPointItem::snumber; + ++WayPointItem::snumber; + this->setFlag(QGraphicsItem::ItemIsMovable, true); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + this->setFlag(QGraphicsItem::ItemIsSelectable, true); + SetShowNumber(shownumber); + RefreshToolTip(); + RefreshPos(); + myHome = NULL; + QList list = map->childItems(); + foreach(QGraphicsItem * obj, list) { + HomeItem *h = qgraphicsitem_cast (obj); + + if (h) { + myHome = h; + } + } + if (myHome) { + map->Projection()->offSetFromLatLngs(myHome->Coord(), coord, relativeCoord.distance, relativeCoord.bearing); + relativeCoord.altitudeRelative = Altitude() - myHome->Altitude(); + connect(myHome, SIGNAL(homePositionChanged(internals::PointLatLng, float)), this, SLOT(onHomePositionChanged(internals::PointLatLng, float))); + } + connect(this, SIGNAL(waypointdoubleclick(WayPointItem *)), map, SIGNAL(wpdoubleclicked(WayPointItem *))); + emit manualCoordChange(this); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); +} + +WayPointItem::WayPointItem(const distBearingAltitude &relativeCoordenate, const QString &description, MapGraphicItem *map) : relativeCoord(relativeCoordenate), reached(false), description(description), shownumber(true), isDragging(false), map(map) +{ + myHome = NULL; + QList list = map->childItems(); + foreach(QGraphicsItem * obj, list) { + HomeItem *h = qgraphicsitem_cast (obj); + + if (h) { + myHome = h; + } + } + if (myHome) { + connect(myHome, SIGNAL(homePositionChanged(internals::PointLatLng, float)), this, SLOT(onHomePositionChanged(internals::PointLatLng, float))); + coord = map->Projection()->translate(myHome->Coord(), relativeCoord.distance, relativeCoord.bearing); + SetAltitude(myHome->Altitude() + relativeCoord.altitudeRelative); + } + myType = relative; + text = 0; + numberI = 0; + isMagic = false; + picture.load(QString::fromUtf8(":/markers/images/marker.png")); + number = WayPointItem::snumber; + ++WayPointItem::snumber; + this->setFlag(QGraphicsItem::ItemIsMovable, true); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + this->setFlag(QGraphicsItem::ItemIsSelectable, true); + SetShowNumber(shownumber); + RefreshToolTip(); + RefreshPos(); + connect(this, SIGNAL(waypointdoubleclick(WayPointItem *)), map, SIGNAL(wpdoubleclicked(WayPointItem *))); + emit manualCoordChange(this); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); +} + +void WayPointItem::setWPType(wptype type) +{ + myType = type; + emit WPValuesChanged(this); + RefreshPos(); + RefreshToolTip(); + this->update(); +} + +QRectF WayPointItem::boundingRect() const +{ + return QRectF(-picture.width() / 2, -picture.height(), picture.width(), picture.height()); +} +void WayPointItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) +{ + Q_UNUSED(option); + Q_UNUSED(widget); + painter->drawPixmap(-picture.width() / 2, -picture.height(), picture); + painter->setPen(Qt::green); + if (this->isSelected()) { + painter->drawRect(QRectF(-picture.width() / 2, -picture.height(), picture.width() - 1, picture.height() - 1)); + } +} +void WayPointItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent *event) +{ + if (event->button() == Qt::LeftButton) { + emit waypointdoubleclick(this); + } +} + +void WayPointItem::mousePressEvent(QGraphicsSceneMouseEvent *event) +{ + if (event->button() == Qt::LeftButton) { + text = new QGraphicsSimpleTextItem(this); + textBG = new QGraphicsRectItem(this); textBG->setBrush(Qt::yellow); text->setPen(QPen(Qt::red)); - text->setPos(10,-picture.height()); - textBG->setPos(10,-picture.height()); + text->setPos(10, -picture.height()); + textBG->setPos(10, -picture.height()); text->setZValue(3); RefreshToolTip(); - isDragging=true; + isDragging = true; } - QGraphicsItem::mousePressEvent(event); - } - void WayPointItem::mouseReleaseEvent(QGraphicsSceneMouseEvent *event) - { - if(event->button()==Qt::LeftButton) - { - if(text) - { - delete text; - text=NULL; - } - if(textBG) - { - delete textBG; - textBG=NULL; - } - - isDragging=false; - RefreshToolTip(); - emit manualCoordChange(this); - emit localPositionChanged(this->pos(),this); - emit WPValuesChanged(this); - } - QGraphicsItem::mouseReleaseEvent(event); - } - void WayPointItem::mouseMoveEvent(QGraphicsSceneMouseEvent *event) - { - - if(isDragging) - { - coord=map->FromLocalToLatLng(this->pos().x(),this->pos().y()); - QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); - if(myHome) - { - map->Projection()->offSetFromLatLngs(myHome->Coord(),coord,relativeCoord.distance,relativeCoord.bearing); - } - QString relativeCoord_str = QString::number(relativeCoord.distance) + "m " + QString::number(relativeCoord.bearing*180/M_PI)+"deg"; - text->setText(coord_str+"\n"+relativeCoord_str); - textBG->setRect(text->boundingRect()); - emit localPositionChanged(this->pos(),this); - emit WPValuesChanged(this); - } - QGraphicsItem::mouseMoveEvent(event); - } - void WayPointItem::SetAltitude(const float &value) - { - if(altitude==value) - return; - altitude=value; - if(myHome) - { - relativeCoord.altitudeRelative=altitude-myHome->Altitude(); - } - RefreshToolTip(); - emit WPValuesChanged(this); - this->update(); - } - - void WayPointItem::setRelativeCoord(distBearingAltitude value) - { - if(qAbs(value.distance-relativeCoord.distance)<0.1 - && qAbs(value.bearing-relativeCoord.bearing)<0.01 && value.altitudeRelative==relativeCoord.altitudeRelative) - return; - relativeCoord=value; - if(myHome) - { - SetCoord(map->Projection()->translate(myHome->Coord(),relativeCoord.distance,relativeCoord.bearing)); - SetAltitude(myHome->Altitude()+relativeCoord.altitudeRelative); - } - RefreshPos(); - RefreshToolTip(); - emit WPValuesChanged(this); - this->update(); - } - - void WayPointItem::SetCoord(const internals::PointLatLng &value) - { - qDebug()<<"1 SetCoord("<Projection()->offSetFromLatLngs(myHome->Coord(),Coord(),back.distance,back.bearing); - if(qAbs(back.bearing-relativeCoord.bearing)>0.01 || qAbs(back.distance-relativeCoord.distance)>0.1) - { - qDebug()<<"4 setCoord-relative coordinates where also updated"; - relativeCoord=back; - } - emit WPValuesChanged(this); - RefreshPos(); - RefreshToolTip(); - this->update(); - qDebug()<<"5 setCoord EXIT"; - } - void WayPointItem::SetDescription(const QString &value) - { - if(description==value) - return; - description=value; - RefreshToolTip(); - emit WPValuesChanged(this); - this->update(); - } - void WayPointItem::SetNumber(const int &value) - { - int oldnumber=number; - number=value; - RefreshToolTip(); - numberI->setText(QString::number(numberAdjusted())); - numberIBG->setRect(numberI->boundingRect().adjusted(-2,0,1,0)); - this->update(); - emit WPNumberChanged(oldnumber,value,this); - } - void WayPointItem::SetReached(const bool &value) - { - reached=value; - emit WPValuesChanged(this); - if(value) - picture.load(QString::fromUtf8(":/markers/images/bigMarkerGreen.png")); - else - { - if(!isMagic) - { - if(this->flags() & QGraphicsItem::ItemIsMovable==QGraphicsItem::ItemIsMovable) - picture.load(QString::fromUtf8(":/markers/images/marker.png")); - else - picture.load(QString::fromUtf8(":/markers/images/waypoint_marker2.png")); - } - else - { - picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker3.png")); - } - } - this->update(); - - } - void WayPointItem::SetShowNumber(const bool &value) - { - shownumber=value; - if((numberI==0) && value) - { - numberI=new QGraphicsSimpleTextItem(this); - numberIBG=new QGraphicsRectItem(this); - numberIBG->setBrush(Qt::white); - numberIBG->setOpacity(0.5); - numberI->setZValue(3); - numberI->setPen(QPen(Qt::blue)); - numberI->setPos(0,-13-picture.height()); - numberIBG->setPos(0,-13-picture.height()); - numberI->setText(QString::number(numberAdjusted())); - numberIBG->setRect(numberI->boundingRect().adjusted(-2,0,1,0)); - } - else if (!value && numberI) - { - delete numberI; - delete numberIBG; - } - this->update(); - } - void WayPointItem::WPDeleted(const int &onumber,WayPointItem *waypoint) - { - Q_UNUSED(waypoint); - int n=number; - if(number>onumber) SetNumber(--n); - } - void WayPointItem::WPInserted(const int &onumber, WayPointItem *waypoint) - { - if(Number()==-1) - return; - - if(waypoint!=this) - { - if(onumber<=number) SetNumber(++number); - } - } - - void WayPointItem::onHomePositionChanged(internals::PointLatLng homepos, float homeAltitude) - { - if(myType==relative) - { - coord=map->Projection()->translate(homepos,relativeCoord.distance,relativeCoord.bearing); - SetAltitude(relativeCoord.altitudeRelative+homeAltitude); - emit WPValuesChanged(this); - RefreshPos(); - RefreshToolTip(); - this->update(); - } - else - { - if(myHome) - { - map->Projection()->offSetFromLatLngs(myHome->Coord(),coord,relativeCoord.distance,relativeCoord.bearing); - relativeCoord.altitudeRelative=Altitude()-homeAltitude; - } - emit WPValuesChanged(this); - } - } - - void WayPointItem::WPRenumbered(const int &oldnumber, const int &newnumber, WayPointItem *waypoint) - { - if (waypoint!=this) - { - if(((oldnumber>number) && (newnumber<=number))) - { - SetNumber(++number); - } - else if (((oldnumbernumber))) - { - SetNumber(--number); - } - else if (newnumber==number) - { - SetNumber(++number); - } - } - } - int WayPointItem::type() const - { - // Enable the use of qgraphicsitem_cast with this item. - return Type; - } - - WayPointItem::~WayPointItem() - { - emit aboutToBeDeleted(this); - --WayPointItem::snumber; - } - void WayPointItem::RefreshPos() - { - core::Point point=map->FromLatLngToLocal(coord); - this->setPos(point.X(),point.Y()); - emit localPositionChanged(this->pos(),this); - } - - void WayPointItem::setOpacitySlot(qreal opacity) - { - setOpacity(opacity); - } - void WayPointItem::RefreshToolTip() - { - QString type_str; - if(myType==relative) - type_str="Relative"; - else - type_str="Absolute"; - QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); - QString relativeCoord_str = " Distance:" + QString::number(relativeCoord.distance) + " Bearing:" + QString::number(relativeCoord.bearing*180/M_PI); - QString relativeAltitude_str=QString::number(relativeCoord.altitudeRelative); - if(Number()!=-1) - setToolTip(QString("WayPoint Number:%1\nDescription:%2\nCoordinate:%4\nFrom Home:%5\nRelative altitude:%6\nAltitude:%7\nType:%8\n%9").arg(QString::number(numberAdjusted())).arg(description).arg(coord_str).arg(relativeCoord_str).arg(relativeAltitude_str).arg(QString::number(altitude)).arg(type_str).arg(myCustomString)); - else - setToolTip(QString("Magic WayPoint\nCoordinate:%1\nFrom Home:%2\nAltitude:%3\nType:%4\n%5").arg(coord_str).arg(relativeCoord_str).arg(QString::number(altitude)).arg(type_str).arg(myCustomString)); - } - - void WayPointItem::setFlag(QGraphicsItem::GraphicsItemFlag flag, bool enabled) - { - if(isMagic) - { - QGraphicsItem::setFlag(flag,enabled); - return; - } - else if(flag==QGraphicsItem::ItemIsMovable) - { - if(enabled) - picture.load(QString::fromUtf8(":/markers/images/marker.png")); - else - picture.load(QString::fromUtf8(":/markers/images/waypoint_marker2.png")); - } - QGraphicsItem::setFlag(flag,enabled); - } - - int WayPointItem::snumber=0; + QGraphicsItem::mousePressEvent(event); +} +void WayPointItem::mouseReleaseEvent(QGraphicsSceneMouseEvent *event) +{ + if (event->button() == Qt::LeftButton) { + if (text) { + delete text; + text = NULL; + } + if (textBG) { + delete textBG; + textBG = NULL; + } + + isDragging = false; + RefreshToolTip(); + emit manualCoordChange(this); + emit localPositionChanged(this->pos(), this); + emit WPValuesChanged(this); + } + QGraphicsItem::mouseReleaseEvent(event); +} +void WayPointItem::mouseMoveEvent(QGraphicsSceneMouseEvent *event) +{ + if (isDragging) { + coord = map->FromLocalToLatLng(this->pos().x(), this->pos().y()); + QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); + if (myHome) { + map->Projection()->offSetFromLatLngs(myHome->Coord(), coord, relativeCoord.distance, relativeCoord.bearing); + } + QString relativeCoord_str = QString::number(relativeCoord.distance) + "m " + QString::number(relativeCoord.bearing * 180 / M_PI) + "deg"; + text->setText(coord_str + "\n" + relativeCoord_str); + textBG->setRect(text->boundingRect()); + emit localPositionChanged(this->pos(), this); + emit WPValuesChanged(this); + } + QGraphicsItem::mouseMoveEvent(event); +} +void WayPointItem::SetAltitude(const float &value) +{ + if (altitude == value) { + return; + } + altitude = value; + if (myHome) { + relativeCoord.altitudeRelative = altitude - myHome->Altitude(); + } + RefreshToolTip(); + emit WPValuesChanged(this); + this->update(); +} + +void WayPointItem::setRelativeCoord(distBearingAltitude value) +{ + if (qAbs(value.distance - relativeCoord.distance) < 0.1 + && qAbs(value.bearing - relativeCoord.bearing) < 0.01 && value.altitudeRelative == relativeCoord.altitudeRelative) { + return; + } + relativeCoord = value; + if (myHome) { + SetCoord(map->Projection()->translate(myHome->Coord(), relativeCoord.distance, relativeCoord.bearing)); + SetAltitude(myHome->Altitude() + relativeCoord.altitudeRelative); + } + RefreshPos(); + RefreshToolTip(); + emit WPValuesChanged(this); + this->update(); +} + +void WayPointItem::SetCoord(const internals::PointLatLng &value) +{ + qDebug() << "1 SetCoord(" << value.Lat() << "," << value.Lng() << ")" << "OLD:" << Coord().Lat() << "," << Coord().Lng(); + if (qAbs(Coord().Lat() - value.Lat()) < 0.0001 && qAbs(Coord().Lng() - value.Lng()) < 0.0001) { + qDebug() << "2 SetCoord nothing changed returning"; + return; + } + qDebug() << "3 setCoord there were changes"; + coord = value; + distBearingAltitude back = relativeCoord; + if (myHome) { + map->Projection()->offSetFromLatLngs(myHome->Coord(), Coord(), back.distance, back.bearing); + } + if (qAbs(back.bearing - relativeCoord.bearing) > 0.01 || qAbs(back.distance - relativeCoord.distance) > 0.1) { + qDebug() << "4 setCoord-relative coordinates where also updated"; + relativeCoord = back; + } + emit WPValuesChanged(this); + RefreshPos(); + RefreshToolTip(); + this->update(); + qDebug() << "5 setCoord EXIT"; +} +void WayPointItem::SetDescription(const QString &value) +{ + if (description == value) { + return; + } + description = value; + RefreshToolTip(); + emit WPValuesChanged(this); + this->update(); +} +void WayPointItem::SetNumber(const int &value) +{ + int oldnumber = number; + + number = value; + RefreshToolTip(); + numberI->setText(QString::number(numberAdjusted())); + numberIBG->setRect(numberI->boundingRect().adjusted(-2, 0, 1, 0)); + this->update(); + emit WPNumberChanged(oldnumber, value, this); +} +void WayPointItem::SetReached(const bool &value) +{ + reached = value; + emit WPValuesChanged(this); + if (value) { + picture.load(QString::fromUtf8(":/markers/images/bigMarkerGreen.png")); + } else { + if (!isMagic) { + if (this->flags() & QGraphicsItem::ItemIsMovable == QGraphicsItem::ItemIsMovable) { + picture.load(QString::fromUtf8(":/markers/images/marker.png")); + } else { + picture.load(QString::fromUtf8(":/markers/images/waypoint_marker2.png")); + } + } else { + picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker3.png")); + } + } + this->update(); +} +void WayPointItem::SetShowNumber(const bool &value) +{ + shownumber = value; + if ((numberI == 0) && value) { + numberI = new QGraphicsSimpleTextItem(this); + numberIBG = new QGraphicsRectItem(this); + numberIBG->setBrush(Qt::white); + numberIBG->setOpacity(0.5); + numberI->setZValue(3); + numberI->setPen(QPen(Qt::blue)); + numberI->setPos(0, -13 - picture.height()); + numberIBG->setPos(0, -13 - picture.height()); + numberI->setText(QString::number(numberAdjusted())); + numberIBG->setRect(numberI->boundingRect().adjusted(-2, 0, 1, 0)); + } else if (!value && numberI) { + delete numberI; + delete numberIBG; + } + this->update(); +} +void WayPointItem::WPDeleted(const int &onumber, WayPointItem *waypoint) +{ + Q_UNUSED(waypoint); + int n = number; + if (number > onumber) { + SetNumber(--n); + } +} +void WayPointItem::WPInserted(const int &onumber, WayPointItem *waypoint) +{ + if (Number() == -1) { + return; + } + + if (waypoint != this) { + if (onumber <= number) { + SetNumber(++number); + } + } +} + +void WayPointItem::onHomePositionChanged(internals::PointLatLng homepos, float homeAltitude) +{ + if (myType == relative) { + coord = map->Projection()->translate(homepos, relativeCoord.distance, relativeCoord.bearing); + SetAltitude(relativeCoord.altitudeRelative + homeAltitude); + emit WPValuesChanged(this); + RefreshPos(); + RefreshToolTip(); + this->update(); + } else { + if (myHome) { + map->Projection()->offSetFromLatLngs(myHome->Coord(), coord, relativeCoord.distance, relativeCoord.bearing); + relativeCoord.altitudeRelative = Altitude() - homeAltitude; + } + emit WPValuesChanged(this); + } +} + +void WayPointItem::WPRenumbered(const int &oldnumber, const int &newnumber, WayPointItem *waypoint) +{ + if (waypoint != this) { + if (((oldnumber > number) && (newnumber <= number))) { + SetNumber(++number); + } else if (((oldnumber < number) && (newnumber > number))) { + SetNumber(--number); + } else if (newnumber == number) { + SetNumber(++number); + } + } +} +int WayPointItem::type() const +{ + // Enable the use of qgraphicsitem_cast with this item. + return Type; +} + +WayPointItem::~WayPointItem() +{ + emit aboutToBeDeleted(this); + + --WayPointItem::snumber; +} +void WayPointItem::RefreshPos() +{ + core::Point point = map->FromLatLngToLocal(coord); + + this->setPos(point.X(), point.Y()); + emit localPositionChanged(this->pos(), this); +} + +void WayPointItem::setOpacitySlot(qreal opacity) +{ + setOpacity(opacity); +} +void WayPointItem::RefreshToolTip() +{ + QString type_str; + + if (myType == relative) { + type_str = "Relative"; + } else { + type_str = "Absolute"; + } + QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); + QString relativeCoord_str = " Distance:" + QString::number(relativeCoord.distance) + " Bearing:" + QString::number(relativeCoord.bearing * 180 / M_PI); + QString relativeAltitude_str = QString::number(relativeCoord.altitudeRelative); + if (Number() != -1) { + setToolTip(QString("WayPoint Number:%1\nDescription:%2\nCoordinate:%4\nFrom Home:%5\nRelative altitude:%6\nAltitude:%7\nType:%8\n%9").arg(QString::number(numberAdjusted())).arg(description).arg(coord_str).arg(relativeCoord_str).arg(relativeAltitude_str).arg(QString::number(altitude)).arg(type_str).arg(myCustomString)); + } else { + setToolTip(QString("Magic WayPoint\nCoordinate:%1\nFrom Home:%2\nAltitude:%3\nType:%4\n%5").arg(coord_str).arg(relativeCoord_str).arg(QString::number(altitude)).arg(type_str).arg(myCustomString)); + } +} + +void WayPointItem::setFlag(QGraphicsItem::GraphicsItemFlag flag, bool enabled) +{ + if (isMagic) { + QGraphicsItem::setFlag(flag, enabled); + return; + } else if (flag == QGraphicsItem::ItemIsMovable) { + if (enabled) { + picture.load(QString::fromUtf8(":/markers/images/marker.png")); + } else { + picture.load(QString::fromUtf8(":/markers/images/waypoint_marker2.png")); + } + } + QGraphicsItem::setFlag(flag, enabled); +} + +int WayPointItem::snumber = 0; } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h index d974a78fc..e97c76255 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file waypointitem.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a WayPoint -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file waypointitem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef WAYPOINTITEM_H #define WAYPOINTITEM_H @@ -35,208 +35,243 @@ #include #include -namespace mapcontrol -{ -struct distBearingAltitude -{ +namespace mapcontrol { +struct distBearingAltitude { double distance; double bearing; - float altitudeRelative; - double bearingToDegrees(){return bearing*180/M_PI;} - void setBearingFromDegrees(double degrees){bearing=degrees*M_PI/180;} + float altitudeRelative; + double bearingToDegrees() + { + return bearing * 180 / M_PI; + } + void setBearingFromDegrees(double degrees) + { + bearing = degrees * M_PI / 180; + } }; class HomeItem; /** -* @brief A QGraphicsItem representing a WayPoint -* -* @class WayPointItem waypointitem.h "waypointitem.h" -*/ -class WayPointItem:public QObject,public QGraphicsItem -{ - Q_OBJECT - Q_INTERFACES(QGraphicsItem) + * @brief A QGraphicsItem representing a WayPoint + * + * @class WayPointItem waypointitem.h "waypointitem.h" + */ +class WayPointItem : public QObject, public QGraphicsItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) public: enum { Type = UserType + 1 }; - enum wptype {absolute,relative}; + enum wptype { absolute, relative }; /** - * @brief Constructer - * - * @param coord coordinates in LatLng of the Waypoint - * @param altitude altitude of the WayPoint - * @param map pointer to map to use - * @return - */ - WayPointItem(internals::PointLatLng const& coord,int const& altitude,MapGraphicItem* map,wptype type=absolute); - WayPointItem(MapGraphicItem* map,bool magicwaypoint); + * @brief Constructer + * + * @param coord coordinates in LatLng of the Waypoint + * @param altitude altitude of the WayPoint + * @param map pointer to map to use + * @return + */ + WayPointItem(internals::PointLatLng const & coord, int const & altitude, MapGraphicItem *map, wptype type = absolute); + WayPointItem(MapGraphicItem *map, bool magicwaypoint); /** - * @brief Constructer - * - * @param coord coordinates in LatLng of the WayPoint - * @param altitude altitude of the WayPoint - * @param description description fo the WayPoint - * @param map pointer to map to use - * @return - */ - WayPointItem(internals::PointLatLng const& coord,int const& altitude,QString const& description,MapGraphicItem* map,wptype type=absolute); - WayPointItem(distBearingAltitude const& relativeCoord,QString const& description,MapGraphicItem* map); + * @brief Constructer + * + * @param coord coordinates in LatLng of the WayPoint + * @param altitude altitude of the WayPoint + * @param description description fo the WayPoint + * @param map pointer to map to use + * @return + */ + WayPointItem(internals::PointLatLng const & coord, int const & altitude, QString const & description, MapGraphicItem *map, wptype type = absolute); + WayPointItem(distBearingAltitude const & relativeCoord, QString const & description, MapGraphicItem *map); /** - * @brief Returns the WayPoint description - * - * @return QString - */ - QString Description(){return description;} + * @brief Returns the WayPoint description + * + * @return QString + */ + QString Description() + { + return description; + } /** - * @brief Sets the WayPoint description - * - * @param value - */ - void SetDescription(QString const& value); + * @brief Sets the WayPoint description + * + * @param value + */ + void SetDescription(QString const & value); /** - * @brief Returns true if WayPoint is Reached - * - * @return bool - */ - bool Reached(){return reached;} + * @brief Returns true if WayPoint is Reached + * + * @return bool + */ + bool Reached() + { + return reached; + } /** - * @brief Sets if WayPoint is Reached - * - * @param value - */ - void SetReached(bool const& value); + * @brief Sets if WayPoint is Reached + * + * @param value + */ + void SetReached(bool const & value); /** - * @brief Returns the WayPoint number - * - */ - int Number(){return number;} - int numberAdjusted(){return number+1;} + * @brief Returns the WayPoint number + * + */ + int Number() + { + return number; + } + int numberAdjusted() + { + return number + 1; + } /** - * @brief Sets WayPoint number - * - * @param value - */ - void SetNumber(int const& value); + * @brief Sets WayPoint number + * + * @param value + */ + void SetNumber(int const & value); /** - * @brief Returns WayPoint LatLng coordinate - * - */ - internals::PointLatLng Coord(){return coord;} + * @brief Returns WayPoint LatLng coordinate + * + */ + internals::PointLatLng Coord() + { + return coord; + } /** - * @brief Sets WayPoint LatLng coordinate - * - * @param value - */ - void SetCoord(internals::PointLatLng const& value); + * @brief Sets WayPoint LatLng coordinate + * + * @param value + */ + void SetCoord(internals::PointLatLng const & value); /** - * @brief Used if WayPoint number is to be drawn on screen - * - */ - bool ShowNumber(){return shownumber;} + * @brief Used if WayPoint number is to be drawn on screen + * + */ + bool ShowNumber() + { + return shownumber; + } /** - * @brief Used to set if WayPoint number is to be drawn on screen - * - * @param value - */ - void SetShowNumber(bool const& value); + * @brief Used to set if WayPoint number is to be drawn on screen + * + * @param value + */ + void SetShowNumber(bool const & value); /** - * @brief Returns the WayPoint altitude - * - * @return int - */ - float Altitude(){return altitude;} + * @brief Returns the WayPoint altitude + * + * @return int + */ + float Altitude() + { + return altitude; + } /** - * @brief Sets the WayPoint Altitude - * - * @param value - */ + * @brief Sets the WayPoint Altitude + * + * @param value + */ void SetAltitude(const float &value); void setRelativeCoord(distBearingAltitude value); - distBearingAltitude getRelativeCoord(){return relativeCoord;} + distBearingAltitude getRelativeCoord() + { + return relativeCoord; + } int type() const; QRectF boundingRect() const; void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, - QWidget *widget); + QWidget *widget); void RefreshToolTip(); QPixmap picture; - QString customString(){return myCustomString;} - void setCustomString(QString arg){myCustomString=arg;} + QString customString() + { + return myCustomString; + } + void setCustomString(QString arg) + { + myCustomString = arg; + } void setFlag(GraphicsItemFlag flag, bool enabled); -~WayPointItem(); + ~WayPointItem(); static int snumber; void setWPType(wptype type); - wptype WPType(){return myType;} + wptype WPType() + { + return myType; + } protected: - void mouseMoveEvent ( QGraphicsSceneMouseEvent * event ); - void mousePressEvent ( QGraphicsSceneMouseEvent * event ); - void mouseReleaseEvent ( QGraphicsSceneMouseEvent * event ); + void mouseMoveEvent(QGraphicsSceneMouseEvent *event); + void mousePressEvent(QGraphicsSceneMouseEvent *event); + void mouseReleaseEvent(QGraphicsSceneMouseEvent *event); void mouseDoubleClickEvent(QGraphicsSceneMouseEvent *event); private: - internals::PointLatLng coord;//coordinates of this WayPoint + internals::PointLatLng coord; // coordinates of this WayPoint distBearingAltitude relativeCoord; bool reached; QString description; bool shownumber; bool isDragging; float altitude; - MapGraphicItem* map; + MapGraphicItem *map; int number; bool isMagic; - QGraphicsSimpleTextItem* text; - QGraphicsRectItem* textBG; - QGraphicsSimpleTextItem* numberI; - QGraphicsRectItem* numberIBG; + QGraphicsSimpleTextItem *text; + QGraphicsRectItem *textBG; + QGraphicsSimpleTextItem *numberI; + QGraphicsRectItem *numberIBG; QTransform transf; - HomeItem * myHome; + HomeItem *myHome; wptype myType; QString myCustomString; public slots: /** - * @brief Called when a WayPoint is deleted - * - * @param number number of the WayPoint that was deleted - */ - void WPDeleted(int const& number,WayPointItem *waypoint); + * @brief Called when a WayPoint is deleted + * + * @param number number of the WayPoint that was deleted + */ + void WPDeleted(int const & number, WayPointItem *waypoint); /** - * @brief Called when a WayPoint is renumbered - * - * @param oldnumber the old WayPoint number - * @param newnumber the new WayPoint number - * @param waypoint a pointer to the WayPoint renumbered - */ - void WPRenumbered(int const& oldnumber,int const& newnumber,WayPointItem* waypoint); + * @brief Called when a WayPoint is renumbered + * + * @param oldnumber the old WayPoint number + * @param newnumber the new WayPoint number + * @param waypoint a pointer to the WayPoint renumbered + */ + void WPRenumbered(int const & oldnumber, int const & newnumber, WayPointItem *waypoint); /** - * @brief Called when a WayPoint is inserted - * - * @param number the number of the WayPoint - * @param waypoint a pointer to the WayPoint inserted - */ - void WPInserted(int const& number,WayPointItem* waypoint); + * @brief Called when a WayPoint is inserted + * + * @param number the number of the WayPoint + * @param waypoint a pointer to the WayPoint inserted + */ + void WPInserted(int const & number, WayPointItem *waypoint); - void onHomePositionChanged(internals::PointLatLng,float altitude); + void onHomePositionChanged(internals::PointLatLng, float altitude); void RefreshPos(); void setOpacitySlot(qreal opacity); signals: /** - * @brief fires when this WayPoint number changes (not fired if due to a auto-renumbering) - * - * @param oldnumber this WayPoint old number - * @param newnumber this WayPoint new number - * @param waypoint a pointer to this WayPoint - */ - void WPNumberChanged(int const& oldnumber,int const& newnumber,WayPointItem* waypoint); + * @brief fires when this WayPoint number changes (not fired if due to a auto-renumbering) + * + * @param oldnumber this WayPoint old number + * @param newnumber this WayPoint new number + * @param waypoint a pointer to this WayPoint + */ + void WPNumberChanged(int const & oldnumber, int const & newnumber, WayPointItem *waypoint); /** - * @brief Fired when the description, altitude or coordinates change - * - * @param waypoint a pointer to this WayPoint - */ + * @brief Fired when the description, altitude or coordinates change + * + * @param waypoint a pointer to this WayPoint + */ - void WPValuesChanged(WayPointItem* waypoint); - void waypointdoubleclick(WayPointItem* waypoint); - void localPositionChanged(QPointF point,WayPointItem* waypoint); + void WPValuesChanged(WayPointItem *waypoint); + void waypointdoubleclick(WayPointItem *waypoint); + void localPositionChanged(QPointF point, WayPointItem *waypoint); void manualCoordChange(WayPointItem *); void aboutToBeDeleted(WayPointItem *); }; diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.cpp index ad245cbc1..eb750a1e6 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.cpp @@ -1,66 +1,67 @@ /** -****************************************************************************** -* -* @file waypointline.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a line connecting 2 waypoints -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file waypointline.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a line connecting 2 waypoints + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "waypointline.h" #include #include "homeitem.h" -namespace mapcontrol +namespace mapcontrol { +WayPointLine::WayPointLine(WayPointItem *from, WayPointItem *to, MapGraphicItem *map, QColor color) : source(from), + destination(to), my_map(map), QGraphicsLineItem(map), myColor(color) { -WayPointLine::WayPointLine(WayPointItem *from, WayPointItem *to, MapGraphicItem *map,QColor color):source(from), - destination(to),my_map(map),QGraphicsLineItem(map),myColor(color) -{ - this->setLine(to->pos().x(),to->pos().y(),from->pos().x(),from->pos().y()); - connect(from,SIGNAL(localPositionChanged(QPointF,WayPointItem*)),this,SLOT(refreshLocations())); - connect(to,SIGNAL(localPositionChanged(QPointF,WayPointItem*)),this,SLOT(refreshLocations())); - connect(from,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); - connect(to,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); - if(myColor==Qt::green) + this->setLine(to->pos().x(), to->pos().y(), from->pos().x(), from->pos().y()); + connect(from, SIGNAL(localPositionChanged(QPointF, WayPointItem *)), this, SLOT(refreshLocations())); + connect(to, SIGNAL(localPositionChanged(QPointF, WayPointItem *)), this, SLOT(refreshLocations())); + connect(from, SIGNAL(aboutToBeDeleted(WayPointItem *)), this, SLOT(waypointdeleted())); + connect(to, SIGNAL(aboutToBeDeleted(WayPointItem *)), this, SLOT(waypointdeleted())); + if (myColor == Qt::green) { this->setZValue(10); - else if(myColor==Qt::yellow) + } else if (myColor == Qt::yellow) { this->setZValue(9); - else if(myColor==Qt::red) + } else if (myColor == Qt::red) { this->setZValue(8); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); + } + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); } -WayPointLine::WayPointLine(HomeItem *from, WayPointItem *to, MapGraphicItem *map, QColor color):source(from), - destination(to),my_map(map),QGraphicsLineItem(map),myColor(color) +WayPointLine::WayPointLine(HomeItem *from, WayPointItem *to, MapGraphicItem *map, QColor color) : source(from), + destination(to), my_map(map), QGraphicsLineItem(map), myColor(color) { - this->setLine(to->pos().x(),to->pos().y(),from->pos().x(),from->pos().y()); - connect(from,SIGNAL(homePositionChanged(internals::PointLatLng,float)),this,SLOT(refreshLocations())); - connect(to,SIGNAL(localPositionChanged(QPointF,WayPointItem*)),this,SLOT(refreshLocations())); - connect(to,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); - if(myColor==Qt::green) + this->setLine(to->pos().x(), to->pos().y(), from->pos().x(), from->pos().y()); + connect(from, SIGNAL(homePositionChanged(internals::PointLatLng, float)), this, SLOT(refreshLocations())); + connect(to, SIGNAL(localPositionChanged(QPointF, WayPointItem *)), this, SLOT(refreshLocations())); + connect(to, SIGNAL(aboutToBeDeleted(WayPointItem *)), this, SLOT(waypointdeleted())); + if (myColor == Qt::green) { this->setZValue(10); - else if(myColor==Qt::yellow) + } else if (myColor == Qt::yellow) { this->setZValue(9); - else if(myColor==Qt::red) + } else if (myColor == Qt::red) { this->setZValue(8); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); + } + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); } int WayPointLine::type() const { @@ -70,6 +71,7 @@ int WayPointLine::type() const QPainterPath WayPointLine::shape() const { QPainterPath path = QGraphicsLineItem::shape(); + path.addPolygon(arrowHead); return path; } @@ -85,30 +87,31 @@ void WayPointLine::paint(QPainter *painter, const QStyleOptionGraphicsItem *opti painter->setBrush(myColor); double angle = ::acos(line().dx() / line().length()); - if (line().dy() >= 0) + if (line().dy() >= 0) { angle = (M_PI * 2) - angle; + } - QPointF arrowP1 = line().pointAt(0.5) + QPointF(sin(angle + M_PI / 3) * arrowSize, - cos(angle + M_PI / 3) * arrowSize); - QPointF arrowP2 = line().pointAt(0.5) + QPointF(sin(angle + M_PI - M_PI / 3) * arrowSize, - cos(angle + M_PI - M_PI / 3) * arrowSize); - arrowHead.clear(); - arrowHead << line().pointAt(0.5) << arrowP1 << arrowP2; - painter->drawPolygon(arrowHead); - if(myColor==Qt::red) - myPen.setWidth(3); - else if(myColor==Qt::yellow) - myPen.setWidth(2); - else if(myColor==Qt::green) - myPen.setWidth(1); - painter->setPen(myPen); - painter->drawLine(line()); - + QPointF arrowP1 = line().pointAt(0.5) + QPointF(sin(angle + M_PI / 3) * arrowSize, + cos(angle + M_PI / 3) * arrowSize); + QPointF arrowP2 = line().pointAt(0.5) + QPointF(sin(angle + M_PI - M_PI / 3) * arrowSize, + cos(angle + M_PI - M_PI / 3) * arrowSize); + arrowHead.clear(); + arrowHead << line().pointAt(0.5) << arrowP1 << arrowP2; + painter->drawPolygon(arrowHead); + if (myColor == Qt::red) { + myPen.setWidth(3); + } else if (myColor == Qt::yellow) { + myPen.setWidth(2); + } else if (myColor == Qt::green) { + myPen.setWidth(1); + } + painter->setPen(myPen); + painter->drawLine(line()); } void WayPointLine::refreshLocations() { - this->setLine(destination->pos().x(),destination->pos().y(),source->pos().x(),source->pos().y()); + this->setLine(destination->pos().x(), destination->pos().y(), source->pos().x(), source->pos().y()); } void WayPointLine::waypointdeleted() @@ -120,5 +123,4 @@ void WayPointLine::setOpacitySlot(qreal opacity) { setOpacity(opacity); } - } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.h index 6b6bc4133..cc1cc3b93 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file waypointline.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a line connecting 2 waypoints -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file waypointline.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a line connecting 2 waypoints + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef WAYPOINTLINE_H #define WAYPOINTLINE_H #include @@ -35,24 +35,23 @@ #include #include -namespace mapcontrol -{ -class WayPointLine:public QObject,public QGraphicsLineItem -{ - Q_OBJECT - Q_INTERFACES(QGraphicsItem) +namespace mapcontrol { +class WayPointLine : public QObject, public QGraphicsLineItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) public: enum { Type = UserType + 8 }; - WayPointLine(WayPointItem * from, WayPointItem * to,MapGraphicItem * map,QColor color=Qt::green); - WayPointLine(HomeItem * from, WayPointItem * to,MapGraphicItem * map,QColor color=Qt::green); + WayPointLine(WayPointItem *from, WayPointItem *to, MapGraphicItem *map, QColor color = Qt::green); + WayPointLine(HomeItem *from, WayPointItem *to, MapGraphicItem *map, QColor color = Qt::green); int type() const; QPainterPath shape() const; void setColor(const QColor &color) - { myColor = color; } + { + myColor = color; + } private: - QGraphicsItem * source; - QGraphicsItem * destination; - MapGraphicItem * my_map; + QGraphicsItem *source; + QGraphicsItem *destination; + MapGraphicItem *my_map; QPolygonF arrowHead; QColor myColor; protected: diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/posix_qextserialport.cpp b/ground/openpilotgcs/src/libs/qextserialport/src/posix_qextserialport.cpp index c7f923cd5..e672122c5 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/posix_qextserialport.cpp +++ b/ground/openpilotgcs/src/libs/qextserialport/src/posix_qextserialport.cpp @@ -1,5 +1,3 @@ - - #include #include #include "qextserialport.h" @@ -13,699 +11,691 @@ void QextSerialPort::platformSpecificInit() } /*! -Standard destructor. -*/ + Standard destructor. + */ void QextSerialPort::platformSpecificDestruct() {} /*! -Sets the baud rate of the serial port. Note that not all rates are applicable on -all platforms. The following table shows translations of the various baud rate -constants on Windows(including NT/2000) and POSIX platforms. Speeds marked with an * -are speeds that are usable on both Windows and POSIX. + Sets the baud rate of the serial port. Note that not all rates are applicable on + all platforms. The following table shows translations of the various baud rate + constants on Windows(including NT/2000) and POSIX platforms. Speeds marked with an * + are speeds that are usable on both Windows and POSIX. -\note -BAUD76800 may not be supported on all POSIX systems. SGI/IRIX systems do not support -BAUD1800. + \note + BAUD76800 may not be supported on all POSIX systems. SGI/IRIX systems do not support + BAUD1800. -\verbatim + \verbatim - RATE Windows Speed POSIX Speed - ----------- ------------- ----------- + RATE Windows Speed POSIX Speed + ----------- ------------- ----------- BAUD50 110 50 BAUD75 110 75 - *BAUD110 110 110 + * BAUD110 110 110 BAUD134 110 134.5 BAUD150 110 150 BAUD200 110 200 - *BAUD300 300 300 - *BAUD600 600 600 - *BAUD1200 1200 1200 + * BAUD300 300 300 + * BAUD600 600 600 + * BAUD1200 1200 1200 BAUD1800 1200 1800 - *BAUD2400 2400 2400 - *BAUD4800 4800 4800 - *BAUD9600 9600 9600 + * BAUD2400 2400 2400 + * BAUD4800 4800 4800 + * BAUD9600 9600 9600 BAUD14400 14400 9600 - *BAUD19200 19200 19200 - *BAUD38400 38400 38400 + * BAUD19200 19200 19200 + * BAUD38400 38400 38400 BAUD56000 56000 38400 - *BAUD57600 57600 57600 + * BAUD57600 57600 57600 BAUD76800 57600 76800 - *BAUD115200 115200 115200 + * BAUD115200 115200 115200 BAUD128000 128000 115200 BAUD256000 256000 115200 -\endverbatim -*/ + \endverbatim + */ void QextSerialPort::setBaudRate(BaudRateType baudRate) { QMutexLocker lock(mutex); - if (Settings.BaudRate!=baudRate) { + + if (Settings.BaudRate != baudRate) { switch (baudRate) { - case BAUD14400: - Settings.BaudRate=BAUD9600; - break; + case BAUD14400: + Settings.BaudRate = BAUD9600; + break; - case BAUD56000: - Settings.BaudRate=BAUD38400; - break; + case BAUD56000: + Settings.BaudRate = BAUD38400; + break; - case BAUD76800: + case BAUD76800: #ifndef B76800 - Settings.BaudRate=BAUD57600; + Settings.BaudRate = BAUD57600; #else - Settings.BaudRate=baudRate; + Settings.BaudRate = baudRate; #endif - break; + break; - case BAUD128000: - case BAUD256000: - Settings.BaudRate=BAUD115200; - break; + case BAUD128000: + case BAUD256000: + Settings.BaudRate = BAUD115200; + break; - default: - Settings.BaudRate=baudRate; - break; + default: + Settings.BaudRate = baudRate; + break; } } if (isOpen()) { switch (baudRate) { - - /*50 baud*/ - case BAUD50: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 50 baud operation."); + /*50 baud*/ + case BAUD50: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 50 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B50; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B50; #else - cfsetispeed(&Posix_CommConfig, B50); - cfsetospeed(&Posix_CommConfig, B50); + cfsetispeed(&Posix_CommConfig, B50); + cfsetospeed(&Posix_CommConfig, B50); #endif - break; + break; - /*75 baud*/ - case BAUD75: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 75 baud operation."); + /*75 baud*/ + case BAUD75: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 75 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B75; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B75; #else - cfsetispeed(&Posix_CommConfig, B75); - cfsetospeed(&Posix_CommConfig, B75); + cfsetispeed(&Posix_CommConfig, B75); + cfsetospeed(&Posix_CommConfig, B75); #endif - break; + break; - /*110 baud*/ - case BAUD110: + /*110 baud*/ + case BAUD110: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B110; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B110; #else - cfsetispeed(&Posix_CommConfig, B110); - cfsetospeed(&Posix_CommConfig, B110); + cfsetispeed(&Posix_CommConfig, B110); + cfsetospeed(&Posix_CommConfig, B110); #endif - break; + break; - /*134.5 baud*/ - case BAUD134: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 134.5 baud operation."); + /*134.5 baud*/ + case BAUD134: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 134.5 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B134; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B134; #else - cfsetispeed(&Posix_CommConfig, B134); - cfsetospeed(&Posix_CommConfig, B134); + cfsetispeed(&Posix_CommConfig, B134); + cfsetospeed(&Posix_CommConfig, B134); #endif - break; + break; - /*150 baud*/ - case BAUD150: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 150 baud operation."); + /*150 baud*/ + case BAUD150: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 150 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B150; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B150; #else - cfsetispeed(&Posix_CommConfig, B150); - cfsetospeed(&Posix_CommConfig, B150); + cfsetispeed(&Posix_CommConfig, B150); + cfsetospeed(&Posix_CommConfig, B150); #endif - break; + break; - /*200 baud*/ - case BAUD200: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 200 baud operation."); + /*200 baud*/ + case BAUD200: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 200 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B200; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B200; #else - cfsetispeed(&Posix_CommConfig, B200); - cfsetospeed(&Posix_CommConfig, B200); + cfsetispeed(&Posix_CommConfig, B200); + cfsetospeed(&Posix_CommConfig, B200); #endif - break; + break; - /*300 baud*/ - case BAUD300: + /*300 baud*/ + case BAUD300: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B300; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B300; #else - cfsetispeed(&Posix_CommConfig, B300); - cfsetospeed(&Posix_CommConfig, B300); + cfsetispeed(&Posix_CommConfig, B300); + cfsetospeed(&Posix_CommConfig, B300); #endif - break; + break; - /*600 baud*/ - case BAUD600: + /*600 baud*/ + case BAUD600: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B600; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B600; #else - cfsetispeed(&Posix_CommConfig, B600); - cfsetospeed(&Posix_CommConfig, B600); + cfsetispeed(&Posix_CommConfig, B600); + cfsetospeed(&Posix_CommConfig, B600); #endif - break; + break; - /*1200 baud*/ - case BAUD1200: + /*1200 baud*/ + case BAUD1200: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B1200; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B1200; #else - cfsetispeed(&Posix_CommConfig, B1200); - cfsetospeed(&Posix_CommConfig, B1200); + cfsetispeed(&Posix_CommConfig, B1200); + cfsetospeed(&Posix_CommConfig, B1200); #endif - break; + break; - /*1800 baud*/ - case BAUD1800: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows and IRIX do not support 1800 baud operation."); + /*1800 baud*/ + case BAUD1800: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows and IRIX do not support 1800 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B1800; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B1800; #else - cfsetispeed(&Posix_CommConfig, B1800); - cfsetospeed(&Posix_CommConfig, B1800); + cfsetispeed(&Posix_CommConfig, B1800); + cfsetospeed(&Posix_CommConfig, B1800); #endif - break; + break; - /*2400 baud*/ - case BAUD2400: + /*2400 baud*/ + case BAUD2400: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B2400; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B2400; #else - cfsetispeed(&Posix_CommConfig, B2400); - cfsetospeed(&Posix_CommConfig, B2400); + cfsetispeed(&Posix_CommConfig, B2400); + cfsetospeed(&Posix_CommConfig, B2400); #endif - break; + break; - /*4800 baud*/ - case BAUD4800: + /*4800 baud*/ + case BAUD4800: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B4800; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B4800; #else - cfsetispeed(&Posix_CommConfig, B4800); - cfsetospeed(&Posix_CommConfig, B4800); + cfsetispeed(&Posix_CommConfig, B4800); + cfsetospeed(&Posix_CommConfig, B4800); #endif - break; + break; - /*9600 baud*/ - case BAUD9600: + /*9600 baud*/ + case BAUD9600: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B9600; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B9600; #else - cfsetispeed(&Posix_CommConfig, B9600); - cfsetospeed(&Posix_CommConfig, B9600); + cfsetispeed(&Posix_CommConfig, B9600); + cfsetospeed(&Posix_CommConfig, B9600); #endif - break; + break; - /*14400 baud*/ - case BAUD14400: - TTY_WARNING("QextSerialPort: POSIX does not support 14400 baud operation. Switching to 9600 baud."); + /*14400 baud*/ + case BAUD14400: + TTY_WARNING("QextSerialPort: POSIX does not support 14400 baud operation. Switching to 9600 baud."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B9600; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B9600; #else - cfsetispeed(&Posix_CommConfig, B9600); - cfsetospeed(&Posix_CommConfig, B9600); + cfsetispeed(&Posix_CommConfig, B9600); + cfsetospeed(&Posix_CommConfig, B9600); #endif - break; + break; - /*19200 baud*/ - case BAUD19200: + /*19200 baud*/ + case BAUD19200: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B19200; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B19200; #else - cfsetispeed(&Posix_CommConfig, B19200); - cfsetospeed(&Posix_CommConfig, B19200); + cfsetispeed(&Posix_CommConfig, B19200); + cfsetospeed(&Posix_CommConfig, B19200); #endif - break; + break; - /*38400 baud*/ - case BAUD38400: + /*38400 baud*/ + case BAUD38400: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B38400; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B38400; #else - cfsetispeed(&Posix_CommConfig, B38400); - cfsetospeed(&Posix_CommConfig, B38400); + cfsetispeed(&Posix_CommConfig, B38400); + cfsetospeed(&Posix_CommConfig, B38400); #endif - break; + break; - /*56000 baud*/ - case BAUD56000: - TTY_WARNING("QextSerialPort: POSIX does not support 56000 baud operation. Switching to 38400 baud."); + /*56000 baud*/ + case BAUD56000: + TTY_WARNING("QextSerialPort: POSIX does not support 56000 baud operation. Switching to 38400 baud."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B38400; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B38400; #else - cfsetispeed(&Posix_CommConfig, B38400); - cfsetospeed(&Posix_CommConfig, B38400); + cfsetispeed(&Posix_CommConfig, B38400); + cfsetospeed(&Posix_CommConfig, B38400); #endif - break; + break; - /*57600 baud*/ - case BAUD57600: + /*57600 baud*/ + case BAUD57600: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B57600; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B57600; #else - cfsetispeed(&Posix_CommConfig, B57600); - cfsetospeed(&Posix_CommConfig, B57600); + cfsetispeed(&Posix_CommConfig, B57600); + cfsetospeed(&Posix_CommConfig, B57600); #endif - break; + break; - /*76800 baud*/ - case BAUD76800: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows and some POSIX systems do not support 76800 baud operation."); + /*76800 baud*/ + case BAUD76800: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows and some POSIX systems do not support 76800 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag &= (~CBAUD); #ifdef B76800 - Posix_CommConfig.c_cflag|=B76800; + Posix_CommConfig.c_cflag |= B76800; #else - TTY_WARNING("QextSerialPort: QextSerialPort was compiled without 76800 baud support. Switching to 57600 baud."); - Posix_CommConfig.c_cflag|=B57600; -#endif //B76800 -#else //CBAUD + TTY_WARNING("QextSerialPort: QextSerialPort was compiled without 76800 baud support. Switching to 57600 baud."); + Posix_CommConfig.c_cflag |= B57600; +#endif // B76800 +#else // CBAUD #ifdef B76800 - cfsetispeed(&Posix_CommConfig, B76800); - cfsetospeed(&Posix_CommConfig, B76800); + cfsetispeed(&Posix_CommConfig, B76800); + cfsetospeed(&Posix_CommConfig, B76800); #else - TTY_WARNING("QextSerialPort: QextSerialPort was compiled without 76800 baud support. Switching to 57600 baud."); - cfsetispeed(&Posix_CommConfig, B57600); - cfsetospeed(&Posix_CommConfig, B57600); -#endif //B76800 -#endif //CBAUD - break; + TTY_WARNING("QextSerialPort: QextSerialPort was compiled without 76800 baud support. Switching to 57600 baud."); + cfsetispeed(&Posix_CommConfig, B57600); + cfsetospeed(&Posix_CommConfig, B57600); +#endif // B76800 +#endif // CBAUD + break; - /*115200 baud*/ - case BAUD115200: + /*115200 baud*/ + case BAUD115200: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B115200; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B115200; #else - cfsetispeed(&Posix_CommConfig, B115200); - cfsetospeed(&Posix_CommConfig, B115200); + cfsetispeed(&Posix_CommConfig, B115200); + cfsetospeed(&Posix_CommConfig, B115200); #endif - break; + break; - /*128000 baud*/ - case BAUD128000: - TTY_WARNING("QextSerialPort: POSIX does not support 128000 baud operation. Switching to 115200 baud."); + /*128000 baud*/ + case BAUD128000: + TTY_WARNING("QextSerialPort: POSIX does not support 128000 baud operation. Switching to 115200 baud."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B115200; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B115200; #else - cfsetispeed(&Posix_CommConfig, B115200); - cfsetospeed(&Posix_CommConfig, B115200); + cfsetispeed(&Posix_CommConfig, B115200); + cfsetospeed(&Posix_CommConfig, B115200); #endif - break; + break; - /*256000 baud*/ - case BAUD256000: - TTY_WARNING("QextSerialPort: POSIX does not support 256000 baud operation. Switching to 115200 baud."); + /*256000 baud*/ + case BAUD256000: + TTY_WARNING("QextSerialPort: POSIX does not support 256000 baud operation. Switching to 115200 baud."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B115200; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B115200; #else - cfsetispeed(&Posix_CommConfig, B115200); - cfsetospeed(&Posix_CommConfig, B115200); + cfsetispeed(&Posix_CommConfig, B115200); + cfsetospeed(&Posix_CommConfig, B115200); #endif - break; + break; } tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); } } /*! -Sets the number of data bits used by the serial port. Possible values of dataBits are: -\verbatim + Sets the number of data bits used by the serial port. Possible values of dataBits are: + \verbatim DATA_5 5 data bits DATA_6 6 data bits DATA_7 7 data bits DATA_8 8 data bits -\endverbatim + \endverbatim -\note -This function is subject to the following restrictions: -\par + \note + This function is subject to the following restrictions: + \par 5 data bits cannot be used with 2 stop bits. -\par + \par 8 data bits cannot be used with space parity on POSIX systems. -*/ + */ void QextSerialPort::setDataBits(DataBitsType dataBits) { QMutexLocker lock(mutex); - if (Settings.DataBits!=dataBits) { - if ((Settings.StopBits==STOP_2 && dataBits==DATA_5) || - (Settings.StopBits==STOP_1_5 && dataBits!=DATA_5) || - (Settings.Parity==PAR_SPACE && dataBits==DATA_8)) { - } - else { - Settings.DataBits=dataBits; + + if (Settings.DataBits != dataBits) { + if ((Settings.StopBits == STOP_2 && dataBits == DATA_5) || + (Settings.StopBits == STOP_1_5 && dataBits != DATA_5) || + (Settings.Parity == PAR_SPACE && dataBits == DATA_8)) {} else { + Settings.DataBits = dataBits; } } if (isOpen()) { - switch(dataBits) { + switch (dataBits) { + /*5 data bits*/ + case DATA_5: + if (Settings.StopBits == STOP_2) { + TTY_WARNING("QextSerialPort: 5 Data bits cannot be used with 2 stop bits."); + } else { + Settings.DataBits = dataBits; + Posix_CommConfig.c_cflag &= (~CSIZE); + Posix_CommConfig.c_cflag |= CS5; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + } + break; - /*5 data bits*/ - case DATA_5: - if (Settings.StopBits==STOP_2) { - TTY_WARNING("QextSerialPort: 5 Data bits cannot be used with 2 stop bits."); - } - else { - Settings.DataBits=dataBits; - Posix_CommConfig.c_cflag&=(~CSIZE); - Posix_CommConfig.c_cflag|=CS5; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; + /*6 data bits*/ + case DATA_6: + if (Settings.StopBits == STOP_1_5) { + TTY_WARNING("QextSerialPort: 6 Data bits cannot be used with 1.5 stop bits."); + } else { + Settings.DataBits = dataBits; + Posix_CommConfig.c_cflag &= (~CSIZE); + Posix_CommConfig.c_cflag |= CS6; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + } + break; - /*6 data bits*/ - case DATA_6: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("QextSerialPort: 6 Data bits cannot be used with 1.5 stop bits."); - } - else { - Settings.DataBits=dataBits; - Posix_CommConfig.c_cflag&=(~CSIZE); - Posix_CommConfig.c_cflag|=CS6; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; + /*7 data bits*/ + case DATA_7: + if (Settings.StopBits == STOP_1_5) { + TTY_WARNING("QextSerialPort: 7 Data bits cannot be used with 1.5 stop bits."); + } else { + Settings.DataBits = dataBits; + Posix_CommConfig.c_cflag &= (~CSIZE); + Posix_CommConfig.c_cflag |= CS7; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + } + break; - /*7 data bits*/ - case DATA_7: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("QextSerialPort: 7 Data bits cannot be used with 1.5 stop bits."); - } - else { - Settings.DataBits=dataBits; - Posix_CommConfig.c_cflag&=(~CSIZE); - Posix_CommConfig.c_cflag|=CS7; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; - - /*8 data bits*/ - case DATA_8: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("QextSerialPort: 8 Data bits cannot be used with 1.5 stop bits."); - } - else { - Settings.DataBits=dataBits; - Posix_CommConfig.c_cflag&=(~CSIZE); - Posix_CommConfig.c_cflag|=CS8; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; + /*8 data bits*/ + case DATA_8: + if (Settings.StopBits == STOP_1_5) { + TTY_WARNING("QextSerialPort: 8 Data bits cannot be used with 1.5 stop bits."); + } else { + Settings.DataBits = dataBits; + Posix_CommConfig.c_cflag &= (~CSIZE); + Posix_CommConfig.c_cflag |= CS8; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + } + break; } } } /*! -Sets the parity associated with the serial port. The possible values of parity are: -\verbatim + Sets the parity associated with the serial port. The possible values of parity are: + \verbatim PAR_SPACE Space Parity PAR_MARK Mark Parity PAR_NONE No Parity PAR_EVEN Even Parity PAR_ODD Odd Parity -\endverbatim + \endverbatim -\note -This function is subject to the following limitations: -\par -POSIX systems do not support mark parity. -\par -POSIX systems support space parity only if tricked into doing so, and only with + \note + This function is subject to the following limitations: + \par + POSIX systems do not support mark parity. + \par + POSIX systems support space parity only if tricked into doing so, and only with fewer than 8 data bits. Use space parity very carefully with POSIX systems. -*/ + */ void QextSerialPort::setParity(ParityType parity) { QMutexLocker lock(mutex); - if (Settings.Parity!=parity) { - if (parity==PAR_MARK || (parity==PAR_SPACE && Settings.DataBits==DATA_8)) { - } - else { - Settings.Parity=parity; + + if (Settings.Parity != parity) { + if (parity == PAR_MARK || (parity == PAR_SPACE && Settings.DataBits == DATA_8)) {} else { + Settings.Parity = parity; } } if (isOpen()) { switch (parity) { + /*space parity*/ + case PAR_SPACE: + if (Settings.DataBits == DATA_8) { + TTY_PORTABILITY_WARNING("QextSerialPort: Space parity is only supported in POSIX with 7 or fewer data bits"); + } else { + /*space parity not directly supported - add an extra data bit to simulate it*/ + Posix_CommConfig.c_cflag &= ~(PARENB | CSIZE); + switch (Settings.DataBits) { + case DATA_5: + Settings.DataBits = DATA_6; + Posix_CommConfig.c_cflag |= CS6; + break; - /*space parity*/ - case PAR_SPACE: - if (Settings.DataBits==DATA_8) { - TTY_PORTABILITY_WARNING("QextSerialPort: Space parity is only supported in POSIX with 7 or fewer data bits"); + case DATA_6: + Settings.DataBits = DATA_7; + Posix_CommConfig.c_cflag |= CS7; + break; + + case DATA_7: + Settings.DataBits = DATA_8; + Posix_CommConfig.c_cflag |= CS8; + break; + + case DATA_8: + break; } - else { - - /*space parity not directly supported - add an extra data bit to simulate it*/ - Posix_CommConfig.c_cflag&=~(PARENB|CSIZE); - switch(Settings.DataBits) { - case DATA_5: - Settings.DataBits=DATA_6; - Posix_CommConfig.c_cflag|=CS6; - break; - - case DATA_6: - Settings.DataBits=DATA_7; - Posix_CommConfig.c_cflag|=CS7; - break; - - case DATA_7: - Settings.DataBits=DATA_8; - Posix_CommConfig.c_cflag|=CS8; - break; - - case DATA_8: - break; - } - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; - - /*mark parity - WINDOWS ONLY*/ - case PAR_MARK: - TTY_WARNING("QextSerialPort: Mark parity is not supported by POSIX."); - break; - - /*no parity*/ - case PAR_NONE: - Posix_CommConfig.c_cflag&=(~PARENB); tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + } + break; - /*even parity*/ - case PAR_EVEN: - Posix_CommConfig.c_cflag&=(~PARODD); - Posix_CommConfig.c_cflag|=PARENB; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + /*mark parity - WINDOWS ONLY*/ + case PAR_MARK: + TTY_WARNING("QextSerialPort: Mark parity is not supported by POSIX."); + break; - /*odd parity*/ - case PAR_ODD: - Posix_CommConfig.c_cflag|=(PARENB|PARODD); - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + /*no parity*/ + case PAR_NONE: + Posix_CommConfig.c_cflag &= (~PARENB); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; + + /*even parity*/ + case PAR_EVEN: + Posix_CommConfig.c_cflag &= (~PARODD); + Posix_CommConfig.c_cflag |= PARENB; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; + + /*odd parity*/ + case PAR_ODD: + Posix_CommConfig.c_cflag |= (PARENB | PARODD); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; } } } /*! -Sets the number of stop bits used by the serial port. Possible values of stopBits are: -\verbatim + Sets the number of stop bits used by the serial port. Possible values of stopBits are: + \verbatim STOP_1 1 stop bit STOP_1_5 1.5 stop bits STOP_2 2 stop bits -\endverbatim -\note -This function is subject to the following restrictions: -\par + \endverbatim + \note + This function is subject to the following restrictions: + \par 2 stop bits cannot be used with 5 data bits. -\par + \par POSIX does not support 1.5 stop bits. -*/ + */ void QextSerialPort::setStopBits(StopBitsType stopBits) { QMutexLocker lock(mutex); - if (Settings.StopBits!=stopBits) { - if ((Settings.DataBits==DATA_5 && stopBits==STOP_2) || stopBits==STOP_1_5) {} - else { - Settings.StopBits=stopBits; + + if (Settings.StopBits != stopBits) { + if ((Settings.DataBits == DATA_5 && stopBits == STOP_2) || stopBits == STOP_1_5) {} else { + Settings.StopBits = stopBits; } } if (isOpen()) { switch (stopBits) { + /*one stop bit*/ + case STOP_1: + Settings.StopBits = stopBits; + Posix_CommConfig.c_cflag &= (~CSTOPB); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; - /*one stop bit*/ - case STOP_1: - Settings.StopBits=stopBits; - Posix_CommConfig.c_cflag&=(~CSTOPB); + /*1.5 stop bits*/ + case STOP_1_5: + TTY_WARNING("QextSerialPort: 1.5 stop bit operation is not supported by POSIX."); + break; + + /*two stop bits*/ + case STOP_2: + if (Settings.DataBits == DATA_5) { + TTY_WARNING("QextSerialPort: 2 stop bits cannot be used with 5 data bits"); + } else { + Settings.StopBits = stopBits; + Posix_CommConfig.c_cflag |= CSTOPB; tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; - - /*1.5 stop bits*/ - case STOP_1_5: - TTY_WARNING("QextSerialPort: 1.5 stop bit operation is not supported by POSIX."); - break; - - /*two stop bits*/ - case STOP_2: - if (Settings.DataBits==DATA_5) { - TTY_WARNING("QextSerialPort: 2 stop bits cannot be used with 5 data bits"); - } - else { - Settings.StopBits=stopBits; - Posix_CommConfig.c_cflag|=CSTOPB; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; + } + break; } } } /*! -Sets the flow control used by the port. Possible values of flow are: -\verbatim + Sets the flow control used by the port. Possible values of flow are: + \verbatim FLOW_OFF No flow control FLOW_HARDWARE Hardware (RTS/CTS) flow control FLOW_XONXOFF Software (XON/XOFF) flow control -\endverbatim -\note -FLOW_HARDWARE may not be supported on all versions of UNIX. In cases where it is -unsupported, FLOW_HARDWARE is the same as FLOW_OFF. + \endverbatim + \note + FLOW_HARDWARE may not be supported on all versions of UNIX. In cases where it is + unsupported, FLOW_HARDWARE is the same as FLOW_OFF. -*/ + */ void QextSerialPort::setFlowControl(FlowType flow) { QMutexLocker lock(mutex); - if (Settings.FlowControl!=flow) { - Settings.FlowControl=flow; + + if (Settings.FlowControl != flow) { + Settings.FlowControl = flow; } if (isOpen()) { - switch(flow) { + switch (flow) { + /*no flow control*/ + case FLOW_OFF: + Posix_CommConfig.c_cflag &= (~CRTSCTS); + Posix_CommConfig.c_iflag &= (~(IXON | IXOFF | IXANY)); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; - /*no flow control*/ - case FLOW_OFF: - Posix_CommConfig.c_cflag&=(~CRTSCTS); - Posix_CommConfig.c_iflag&=(~(IXON|IXOFF|IXANY)); - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + /*software (XON/XOFF) flow control*/ + case FLOW_XONXOFF: + Posix_CommConfig.c_cflag &= (~CRTSCTS); + Posix_CommConfig.c_iflag |= (IXON | IXOFF | IXANY); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; - /*software (XON/XOFF) flow control*/ - case FLOW_XONXOFF: - Posix_CommConfig.c_cflag&=(~CRTSCTS); - Posix_CommConfig.c_iflag|=(IXON|IXOFF|IXANY); - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; - - case FLOW_HARDWARE: - Posix_CommConfig.c_cflag|=CRTSCTS; - Posix_CommConfig.c_iflag&=(~(IXON|IXOFF|IXANY)); - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + case FLOW_HARDWARE: + Posix_CommConfig.c_cflag |= CRTSCTS; + Posix_CommConfig.c_iflag &= (~(IXON | IXOFF | IXANY)); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; } } } /*! -Sets the read and write timeouts for the port to millisec milliseconds. -Note that this is a per-character timeout, i.e. the port will wait this long for each -individual character, not for the whole read operation. This timeout also applies to the -bytesWaiting() function. + Sets the read and write timeouts for the port to millisec milliseconds. + Note that this is a per-character timeout, i.e. the port will wait this long for each + individual character, not for the whole read operation. This timeout also applies to the + bytesWaiting() function. -\note -POSIX does not support millisecond-level control for I/O timeout values. Any -timeout set using this function will be set to the next lowest tenth of a second for -the purposes of detecting read or write timeouts. For example a timeout of 550 milliseconds -will be seen by the class as a timeout of 500 milliseconds for the purposes of reading and -writing the port. However millisecond-level control is allowed by the select() system call, -so for example a 550-millisecond timeout will be seen as 550 milliseconds on POSIX systems for -the purpose of detecting available bytes in the read buffer. + \note + POSIX does not support millisecond-level control for I/O timeout values. Any + timeout set using this function will be set to the next lowest tenth of a second for + the purposes of detecting read or write timeouts. For example a timeout of 550 milliseconds + will be seen by the class as a timeout of 500 milliseconds for the purposes of reading and + writing the port. However millisecond-level control is allowed by the select() system call, + so for example a 550-millisecond timeout will be seen as 550 milliseconds on POSIX systems for + the purpose of detecting available bytes in the read buffer. -*/ + */ void QextSerialPort::setTimeout(long millisec) { QMutexLocker lock(mutex); - Settings.Timeout_Millisec = millisec; - Posix_Copy_Timeout.tv_sec = millisec / 1000; + + Settings.Timeout_Millisec = millisec; + Posix_Copy_Timeout.tv_sec = millisec / 1000; Posix_Copy_Timeout.tv_usec = millisec % 1000; if (isOpen()) { - if (millisec == -1) + if (millisec == -1) { fcntl(fd, F_SETFL, O_NDELAY); - else - //O_SYNC should enable blocking ::write() - //however this seems not working on Linux 2.6.21 (works on OpenBSD 4.2) + } else { + // O_SYNC should enable blocking ::write() + // however this seems not working on Linux 2.6.21 (works on OpenBSD 4.2) fcntl(fd, F_SETFL, O_SYNC); - tcgetattr(fd, & Posix_CommConfig); - Posix_CommConfig.c_cc[VTIME] = millisec/100; - tcsetattr(fd, TCSAFLUSH, & Posix_CommConfig); + } + tcgetattr(fd, &Posix_CommConfig); + Posix_CommConfig.c_cc[VTIME] = millisec / 100; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); } } /*! -Opens the serial port associated to this class. -This function has no effect if the port associated with the class is already open. -The port is also configured to the current settings, as stored in the Settings structure. -*/ + Opens the serial port associated to this class. + This function has no effect if the port associated with the class is already open. + The port is also configured to the current settings, as stored in the Settings structure. + */ bool QextSerialPort::open(OpenMode mode) { QMutexLocker lock(mutex); - if (mode == QIODevice::NotOpen) + + if (mode == QIODevice::NotOpen) { return isOpen(); + } if (!isOpen()) { qDebug() << "trying to open file" << port.toAscii(); - //note: linux 2.6.21 seems to ignore O_NDELAY flag - if ((fd = ::open(port.toAscii() ,O_RDWR | O_NOCTTY | O_NDELAY)) != -1) { + // note: linux 2.6.21 seems to ignore O_NDELAY flag + if ((fd = ::open(port.toAscii(), O_RDWR | O_NOCTTY | O_NDELAY)) != -1) { qDebug("file opened succesfully"); - setOpenMode(mode); // Flag the port as opened - tcgetattr(fd, &old_termios); // Save the old termios + setOpenMode(mode); // Flag the port as opened + tcgetattr(fd, &old_termios); // Save the old termios Posix_CommConfig = old_termios; // Make a working copy - cfmakeraw(&Posix_CommConfig); // Enable raw access + cfmakeraw(&Posix_CommConfig); // Enable raw access /*set up other port settings*/ - Posix_CommConfig.c_cflag|=CREAD|CLOCAL; - Posix_CommConfig.c_lflag&=(~(ICANON|ECHO|ECHOE|ECHOK|ECHONL|ISIG)); - Posix_CommConfig.c_iflag&=(~(INPCK|IGNPAR|PARMRK|ISTRIP|ICRNL|IXANY)); - Posix_CommConfig.c_oflag&=(~OPOST); - Posix_CommConfig.c_cc[VMIN]= 0; -#ifdef _POSIX_VDISABLE // Is a disable character available on this system? + Posix_CommConfig.c_cflag |= CREAD | CLOCAL; + Posix_CommConfig.c_lflag &= (~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG)); + Posix_CommConfig.c_iflag &= (~(INPCK | IGNPAR | PARMRK | ISTRIP | ICRNL | IXANY)); + Posix_CommConfig.c_oflag &= (~OPOST); + Posix_CommConfig.c_cc[VMIN] = 0; +#ifdef _POSIX_VDISABLE // Is a disable character available on this system? // Some systems allow for per-device disable-characters, so get the - // proper value for the configured device + // proper value for the configured device const long vdisable = fpathconf(fd, _PC_VDISABLE); - Posix_CommConfig.c_cc[VINTR] = vdisable; - Posix_CommConfig.c_cc[VQUIT] = vdisable; + Posix_CommConfig.c_cc[VINTR] = vdisable; + Posix_CommConfig.c_cc[VQUIT] = vdisable; Posix_CommConfig.c_cc[VSTART] = vdisable; - Posix_CommConfig.c_cc[VSTOP] = vdisable; - Posix_CommConfig.c_cc[VSUSP] = vdisable; -#endif //_POSIX_VDISABLE + Posix_CommConfig.c_cc[VSTOP] = vdisable; + Posix_CommConfig.c_cc[VSUSP] = vdisable; +#endif // _POSIX_VDISABLE setBaudRate(Settings.BaudRate); setDataBits(Settings.DataBits); setParity(Settings.Parity); @@ -727,24 +717,24 @@ bool QextSerialPort::open(OpenMode mode) } /*! -Closes a serial port. This function has no effect if the serial port associated with the class -is not currently open. -*/ + Closes a serial port. This function has no effect if the serial port associated with the class + is not currently open. + */ void QextSerialPort::close() { QMutexLocker lock(mutex); - if( isOpen() ) - { + + if (isOpen()) { // Force a flush and then restore the original termios flush(); // Using both TCSAFLUSH and TCSANOW here discards any pending input - tcsetattr(fd, TCSAFLUSH | TCSANOW, &old_termios); // Restore termios + tcsetattr(fd, TCSAFLUSH | TCSANOW, &old_termios); // Restore termios // Be a good QIODevice and call QIODevice::close() before POSIX close() - // so the aboutToClose() signal is emitted at the proper time - QIODevice::close(); // Flag the device as closed + // so the aboutToClose() signal is emitted at the proper time + QIODevice::close(); // Flag the device as closed // QIODevice::close() doesn't actually close the port, so do that here ::close(fd); - if(readNotifier) { + if (readNotifier) { delete readNotifier; readNotifier = 0; } @@ -752,42 +742,46 @@ void QextSerialPort::close() } /*! -Flushes all pending I/O to the serial port. This function has no effect if the serial port -associated with the class is not currently open. -*/ + Flushes all pending I/O to the serial port. This function has no effect if the serial port + associated with the class is not currently open. + */ void QextSerialPort::flush() { QMutexLocker lock(mutex); - if (isOpen()) + + if (isOpen()) { tcflush(fd, TCIOFLUSH); + } } /*! -This function will return the number of bytes waiting in the receive queue of the serial port. -It is included primarily to provide a complete QIODevice interface, and will not record errors -in the lastErr member (because it is const). This function is also not thread-safe - in -multithreading situations, use QextSerialPort::bytesWaiting() instead. -*/ + This function will return the number of bytes waiting in the receive queue of the serial port. + It is included primarily to provide a complete QIODevice interface, and will not record errors + in the lastErr member (because it is const). This function is also not thread-safe - in + multithreading situations, use QextSerialPort::bytesWaiting() instead. + */ qint64 QextSerialPort::size() const { int numBytes; - if (ioctl(fd, FIONREAD, &numBytes)<0) { + + if (ioctl(fd, FIONREAD, &numBytes) < 0) { numBytes = 0; } return (qint64)numBytes; } /*! -Returns the number of bytes waiting in the port's receive queue. This function will return 0 if -the port is not currently open, or -1 on error. -*/ + Returns the number of bytes waiting in the port's receive queue. This function will return 0 if + the port is not currently open, or -1 on error. + */ qint64 QextSerialPort::bytesAvailable() const { QMutexLocker lock(mutex); + if (isOpen()) { int bytesQueued; if (ioctl(fd, FIONREAD, &bytesQueued) == -1) { - return (qint64)-1; + return (qint64) - 1; } return bytesQueued + QIODevice::bytesAvailable(); } @@ -795,10 +789,10 @@ qint64 QextSerialPort::bytesAvailable() const } /*! -This function is included to implement the full QIODevice interface, and currently has no -purpose within this class. This function is meaningless on an unbuffered device and currently -only prints a warning message to that effect. -*/ + This function is included to implement the full QIODevice interface, and currently has no + purpose within this class. This function is meaningless on an unbuffered device and currently + only prints a warning message to that effect. + */ void QextSerialPort::ungetChar(char) { /*meaningless on unbuffered sequential device - return error and print a warning*/ @@ -806,154 +800,159 @@ void QextSerialPort::ungetChar(char) } /*! -Translates a system-specific error code to a QextSerialPort error code. Used internally. -*/ + Translates a system-specific error code to a QextSerialPort error code. Used internally. + */ void QextSerialPort::translateError(ulong error) { switch (error) { - case EBADF: - case ENOTTY: - lastErr=E_INVALID_FD; - break; + case EBADF: + case ENOTTY: + lastErr = E_INVALID_FD; + break; - case EINTR: - lastErr=E_CAUGHT_NON_BLOCKED_SIGNAL; - break; + case EINTR: + lastErr = E_CAUGHT_NON_BLOCKED_SIGNAL; + break; - case ENOMEM: - lastErr=E_NO_MEMORY; - break; + case ENOMEM: + lastErr = E_NO_MEMORY; + break; } } /*! -Sets DTR line to the requested state (high by default). This function will have no effect if -the port associated with the class is not currently open. -*/ + Sets DTR line to the requested state (high by default). This function will have no effect if + the port associated with the class is not currently open. + */ void QextSerialPort::setDtr(bool set) { QMutexLocker lock(mutex); + if (isOpen()) { int status; ioctl(fd, TIOCMGET, &status); if (set) { - status|=TIOCM_DTR; - } - else { - status&=~TIOCM_DTR; + status |= TIOCM_DTR; + } else { + status &= ~TIOCM_DTR; } ioctl(fd, TIOCMSET, &status); } } /*! -Sets RTS line to the requested state (high by default). This function will have no effect if -the port associated with the class is not currently open. -*/ + Sets RTS line to the requested state (high by default). This function will have no effect if + the port associated with the class is not currently open. + */ void QextSerialPort::setRts(bool set) { QMutexLocker lock(mutex); + if (isOpen()) { int status; ioctl(fd, TIOCMGET, &status); if (set) { - status|=TIOCM_RTS; - } - else { - status&=~TIOCM_RTS; + status |= TIOCM_RTS; + } else { + status &= ~TIOCM_RTS; } ioctl(fd, TIOCMSET, &status); } } /*! -Returns the line status as stored by the port function. This function will retrieve the states -of the following lines: DCD, CTS, DSR, and RI. On POSIX systems, the following additional lines -can be monitored: DTR, RTS, Secondary TXD, and Secondary RXD. The value returned is an unsigned -long with specific bits indicating which lines are high. The following constants should be used -to examine the states of individual lines: + Returns the line status as stored by the port function. This function will retrieve the states + of the following lines: DCD, CTS, DSR, and RI. On POSIX systems, the following additional lines + can be monitored: DTR, RTS, Secondary TXD, and Secondary RXD. The value returned is an unsigned + long with specific bits indicating which lines are high. The following constants should be used + to examine the states of individual lines: -\verbatim -Mask Line ------- ---- -LS_CTS CTS -LS_DSR DSR -LS_DCD DCD -LS_RI RI -LS_RTS RTS (POSIX only) -LS_DTR DTR (POSIX only) -LS_ST Secondary TXD (POSIX only) -LS_SR Secondary RXD (POSIX only) -\endverbatim + \verbatim + Mask Line + ------ ---- + LS_CTS CTS + LS_DSR DSR + LS_DCD DCD + LS_RI RI + LS_RTS RTS (POSIX only) + LS_DTR DTR (POSIX only) + LS_ST Secondary TXD (POSIX only) + LS_SR Secondary RXD (POSIX only) + \endverbatim -This function will return 0 if the port associated with the class is not currently open. -*/ + This function will return 0 if the port associated with the class is not currently open. + */ unsigned long QextSerialPort::lineStatus() { - unsigned long Status=0, Temp=0; + unsigned long Status = 0, Temp = 0; QMutexLocker lock(mutex); + if (isOpen()) { ioctl(fd, TIOCMGET, &Temp); - if (Temp&TIOCM_CTS) { - Status|=LS_CTS; + if (Temp & TIOCM_CTS) { + Status |= LS_CTS; } - if (Temp&TIOCM_DSR) { - Status|=LS_DSR; + if (Temp & TIOCM_DSR) { + Status |= LS_DSR; } - if (Temp&TIOCM_RI) { - Status|=LS_RI; + if (Temp & TIOCM_RI) { + Status |= LS_RI; } - if (Temp&TIOCM_CD) { - Status|=LS_DCD; + if (Temp & TIOCM_CD) { + Status |= LS_DCD; } - if (Temp&TIOCM_DTR) { - Status|=LS_DTR; + if (Temp & TIOCM_DTR) { + Status |= LS_DTR; } - if (Temp&TIOCM_RTS) { - Status|=LS_RTS; + if (Temp & TIOCM_RTS) { + Status |= LS_RTS; } - if (Temp&TIOCM_ST) { - Status|=LS_ST; + if (Temp & TIOCM_ST) { + Status |= LS_ST; } - if (Temp&TIOCM_SR) { - Status|=LS_SR; + if (Temp & TIOCM_SR) { + Status |= LS_SR; } } return Status; } /*! -Reads a block of data from the serial port. This function will read at most maxSize bytes from -the serial port and place them in the buffer pointed to by data. Return value is the number of -bytes actually read, or -1 on error. + Reads a block of data from the serial port. This function will read at most maxSize bytes from + the serial port and place them in the buffer pointed to by data. Return value is the number of + bytes actually read, or -1 on error. -\warning before calling this function ensure that serial port associated with this class -is currently open (use isOpen() function to check if port is open). -*/ -qint64 QextSerialPort::readData(char * data, qint64 maxSize) + \warning before calling this function ensure that serial port associated with this class + is currently open (use isOpen() function to check if port is open). + */ +qint64 QextSerialPort::readData(char *data, qint64 maxSize) { QMutexLocker lock(mutex); int retVal = ::read(fd, data, maxSize); - if (retVal == -1) + + if (retVal == -1) { lastErr = E_READ_FAILED; + } return retVal; } /*! -Writes a block of data to the serial port. This function will write maxSize bytes -from the buffer pointed to by data to the serial port. Return value is the number -of bytes actually written, or -1 on error. + Writes a block of data to the serial port. This function will write maxSize bytes + from the buffer pointed to by data to the serial port. Return value is the number + of bytes actually written, or -1 on error. -\warning before calling this function ensure that serial port associated with this class -is currently open (use isOpen() function to check if port is open). -*/ -qint64 QextSerialPort::writeData(const char * data, qint64 maxSize) + \warning before calling this function ensure that serial port associated with this class + is currently open (use isOpen() function to check if port is open). + */ +qint64 QextSerialPort::writeData(const char *data, qint64 maxSize) { QMutexLocker lock(mutex); int retVal = ::write(fd, data, maxSize); - if (retVal == -1) - lastErr = E_WRITE_FAILED; + + if (retVal == -1) { + lastErr = E_WRITE_FAILED; + } return (qint64)retVal; } diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator.h b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator.h index de173715c..cf9143ca9 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator.h +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator.h @@ -27,12 +27,12 @@ * Structure containing port information. */ struct QextPortInfo { - QString portName; ///< Port name. - QString physName; ///< Physical name. + QString portName; ///< Port name. + QString physName; ///< Physical name. QString friendName; ///< Friendly name. - QString enumName; ///< Enumerator name. - int vendorID; ///< Vendor ID. - int productID; ///< Product ID + QString enumName; ///< Enumerator name. + int vendorID; ///< Vendor ID. + int productID; ///< Product ID }; #ifdef Q_OS_WIN @@ -40,160 +40,159 @@ struct QextPortInfo { #include class QextSerialEnumerator; -class QextSerialRegistrationWidget : public QWidget -{ +class QextSerialRegistrationWidget : public QWidget { Q_OBJECT - public: - QextSerialRegistrationWidget( QextSerialEnumerator* qese ) { - this->qese = qese; - } - ~QextSerialRegistrationWidget( ) { } +public: + QextSerialRegistrationWidget(QextSerialEnumerator *qese) + { + this->qese = qese; + } + ~QextSerialRegistrationWidget() {} - protected: - QextSerialEnumerator* qese; - bool winEvent( MSG* message, long* result ); +protected: + QextSerialEnumerator *qese; + bool winEvent(MSG *message, long *result); }; #endif // QT_GUI_LIB #endif // Q_OS_WIN /*! - Provides list of ports available in the system. + Provides list of ports available in the system. - \section Usage - To poll the system for a list of connected devices, simply use getPorts(). Each - QextPortInfo structure will populated with information about the corresponding device. + \section Usage + To poll the system for a list of connected devices, simply use getPorts(). Each + QextPortInfo structure will populated with information about the corresponding device. - \b Example - \code - QList ports = QextSerialEnumerator::getPorts(); - foreach( QextPortInfo port, ports ) { + \b Example + \code + QList ports = QextSerialEnumerator::getPorts(); + foreach( QextPortInfo port, ports ) { // inspect port... - } - \endcode + } + \endcode - To enable event-driven notification of device connection events, first call - setUpNotifications() and then connect to the deviceDiscovered() and deviceRemoved() - signals. Event-driven behavior is currently available only on Windows and OS X. + To enable event-driven notification of device connection events, first call + setUpNotifications() and then connect to the deviceDiscovered() and deviceRemoved() + signals. Event-driven behavior is currently available only on Windows and OS X. - \b Example - \code - QextSerialEnumerator* enumerator = new QextSerialEnumerator(); - connect(enumerator, SIGNAL(deviceDiscovered(const QextPortInfo &)), + \b Example + \code + QextSerialEnumerator* enumerator = new QextSerialEnumerator(); + connect(enumerator, SIGNAL(deviceDiscovered(const QextPortInfo &)), myClass, SLOT(onDeviceDiscovered(const QextPortInfo &))); - connect(enumerator, SIGNAL(deviceRemoved(const QextPortInfo &)), + connect(enumerator, SIGNAL(deviceRemoved(const QextPortInfo &)), myClass, SLOT(onDeviceRemoved(const QextPortInfo &))); - \endcode + \endcode - \section Credits - Windows implementation is based on Zach Gorman's work from - The Code Project (http://www.codeproject.com/system/setupdi.asp). + \section Credits + Windows implementation is based on Zach Gorman's work from + The Code Project (http://www.codeproject.com/system/setupdi.asp). - OS X implementation, see - http://developer.apple.com/documentation/DeviceDrivers/Conceptual/AccessingHardware/AH_Finding_Devices/chapter_4_section_2.html + OS X implementation, see + http://developer.apple.com/documentation/DeviceDrivers/Conceptual/AccessingHardware/AH_Finding_Devices/chapter_4_section_2.html - \author Michal Policht, Liam Staskawicz -*/ -class QEXTSERIALPORT_EXPORT QextSerialEnumerator : public QObject -{ -Q_OBJECT - public: - QextSerialEnumerator( ); - ~QextSerialEnumerator( ); + \author Michal Policht, Liam Staskawicz + */ +class QEXTSERIALPORT_EXPORT QextSerialEnumerator : public QObject { + Q_OBJECT +public: + QextSerialEnumerator(); + ~QextSerialEnumerator(); #ifdef Q_OS_WIN - LRESULT onDeviceChangeWin( WPARAM wParam, LPARAM lParam ); - private: - /*! - * Get value of specified property from the registry. - * \param key handle to an open key. - * \param property property name. - * \return property value. - */ - static QString getRegKeyValue(HKEY key, LPCTSTR property); + LRESULT onDeviceChangeWin(WPARAM wParam, LPARAM lParam); +private: + /*! + * Get value of specified property from the registry. + * \param key handle to an open key. + * \param property property name. + * \return property value. + */ + static QString getRegKeyValue(HKEY key, LPCTSTR property); - /*! - * Get specific property from registry. - * \param devInfo pointer to the device information set that contains the interface - * and its underlying device. Returned by SetupDiGetClassDevs() function. - * \param devData pointer to an SP_DEVINFO_DATA structure that defines the device instance. - * this is returned by SetupDiGetDeviceInterfaceDetail() function. - * \param property registry property. One of defined SPDRP_* constants. - * \return property string. - */ - static QString getDeviceProperty(HDEVINFO devInfo, PSP_DEVINFO_DATA devData, DWORD property); + /*! + * Get specific property from registry. + * \param devInfo pointer to the device information set that contains the interface + * and its underlying device. Returned by SetupDiGetClassDevs() function. + * \param devData pointer to an SP_DEVINFO_DATA structure that defines the device instance. + * this is returned by SetupDiGetDeviceInterfaceDetail() function. + * \param property registry property. One of defined SPDRP_* constants. + * \return property string. + */ + static QString getDeviceProperty(HDEVINFO devInfo, PSP_DEVINFO_DATA devData, DWORD property); - /*! - * Search for serial ports using setupapi. - * \param infoList list with result. - */ - static void setupAPIScan(QList & infoList); - void setUpNotificationWin( ); - static bool getDeviceDetailsWin( QextPortInfo* portInfo, HDEVINFO devInfo, - PSP_DEVINFO_DATA devData, WPARAM wParam = DBT_DEVICEARRIVAL ); - static void enumerateDevicesWin( const GUID & guidDev, QList* infoList ); - bool matchAndDispatchChangedDevice(const QString & deviceID, const GUID & guid, WPARAM wParam); + /*! + * Search for serial ports using setupapi. + * \param infoList list with result. + */ + static void setupAPIScan(QList & infoList); + void setUpNotificationWin(); + static bool getDeviceDetailsWin(QextPortInfo *portInfo, HDEVINFO devInfo, + PSP_DEVINFO_DATA devData, WPARAM wParam = DBT_DEVICEARRIVAL); + static void enumerateDevicesWin(const GUID & guidDev, QList *infoList); + bool matchAndDispatchChangedDevice(const QString & deviceID, const GUID & guid, WPARAM wParam); #ifdef QT_GUI_LIB - QextSerialRegistrationWidget* notificationWidget; + QextSerialRegistrationWidget *notificationWidget; #endif #endif /*Q_OS_WIN*/ #ifdef Q_OS_UNIX #ifdef Q_OS_MAC - private: - /*! - * Search for serial ports using IOKit. - * \param infoList list with result. - */ - static void scanPortsOSX(QList & infoList); - static void iterateServicesOSX(io_object_t service, QList & infoList); - static bool getServiceDetailsOSX( io_object_t service, QextPortInfo* portInfo ); +private: + /*! + * Search for serial ports using IOKit. + * \param infoList list with result. + */ + static void scanPortsOSX(QList & infoList); + static void iterateServicesOSX(io_object_t service, QList & infoList); + static bool getServiceDetailsOSX(io_object_t service, QextPortInfo *portInfo); - void setUpNotificationOSX( ); - void onDeviceDiscoveredOSX( io_object_t service ); - void onDeviceTerminatedOSX( io_object_t service ); - friend void deviceDiscoveredCallbackOSX( void *ctxt, io_iterator_t serialPortIterator ); - friend void deviceTerminatedCallbackOSX( void *ctxt, io_iterator_t serialPortIterator ); + void setUpNotificationOSX(); + void onDeviceDiscoveredOSX(io_object_t service); + void onDeviceTerminatedOSX(io_object_t service); + friend void deviceDiscoveredCallbackOSX(void *ctxt, io_iterator_t serialPortIterator); + friend void deviceTerminatedCallbackOSX(void *ctxt, io_iterator_t serialPortIterator); - IONotificationPortRef notificationPortRef; + IONotificationPortRef notificationPortRef; #else // Q_OS_MAC - private: - /*! - * Search for serial ports on unix. - * \param infoList list with result. - */ - static void scanPortsNix(QList & infoList); +private: + /*! + * Search for serial ports on unix. + * \param infoList list with result. + */ + static void scanPortsNix(QList & infoList); #endif // Q_OS_MAC #endif /* Q_OS_UNIX */ - public: - /*! - Get list of ports. - \return list of ports currently available in the system. - */ - static QList getPorts(); - /*! - Enable event-driven notifications of board discovery/removal. - */ - void setUpNotifications( ); +public: + /*! + Get list of ports. + \return list of ports currently available in the system. + */ + static QList getPorts(); + /*! + Enable event-driven notifications of board discovery/removal. + */ + void setUpNotifications(); - signals: - /*! - A new device has been connected to the system. +signals: + /*! + A new device has been connected to the system. - setUpNotifications() must be called first to enable event-driven device notifications. - Currently only implemented on Windows and OS X. - \param info The device that has been discovered. - */ - void deviceDiscovered( const QextPortInfo & info ); - /*! - A device has been disconnected from the system. + setUpNotifications() must be called first to enable event-driven device notifications. + Currently only implemented on Windows and OS X. + \param info The device that has been discovered. + */ + void deviceDiscovered(const QextPortInfo & info); + /*! + A device has been disconnected from the system. - setUpNotifications() must be called first to enable event-driven device notifications. - Currently only implemented on Windows and OS X. - \param info The device that was disconnected. - */ - void deviceRemoved( const QextPortInfo & info ); + setUpNotifications() must be called first to enable event-driven device notifications. + Currently only implemented on Windows and OS X. + \param info The device that was disconnected. + */ + void deviceRemoved(const QextPortInfo & info); }; #endif /*_QEXTSERIALENUMERATOR_H_*/ diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_osx.cpp b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_osx.cpp index 9cf05e69c..65f67ba4e 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_osx.cpp +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_osx.cpp @@ -1,6 +1,3 @@ - - - #include "qextserialenumerator.h" #include #include @@ -9,33 +6,35 @@ #include #include -QextSerialEnumerator::QextSerialEnumerator( ) +QextSerialEnumerator::QextSerialEnumerator() { - if( !QMetaType::isRegistered( QMetaType::type("QextPortInfo") ) ) + if (!QMetaType::isRegistered(QMetaType::type("QextPortInfo"))) { qRegisterMetaType("QextPortInfo"); + } } -QextSerialEnumerator::~QextSerialEnumerator( ) +QextSerialEnumerator::~QextSerialEnumerator() { - IONotificationPortDestroy( notificationPortRef ); + IONotificationPortDestroy(notificationPortRef); } // static QList QextSerialEnumerator::getPorts() -{ QList infoList; +{ + QList infoList; io_iterator_t serialPortIterator = 0; kern_return_t kernResult = KERN_FAILURE; CFMutableDictionaryRef matchingDictionary; // first try to get any serialbsd devices, then try any USBCDC devices - if( !(matchingDictionary = IOServiceMatching(kIOSerialBSDServiceValue) ) ) { + if (!(matchingDictionary = IOServiceMatching(kIOSerialBSDServiceValue))) { qWarning("IOServiceMatching returned a NULL dictionary."); return infoList; } CFDictionaryAddValue(matchingDictionary, CFSTR(kIOSerialBSDTypeKey), CFSTR(kIOSerialBSDAllTypes)); // then create the iterator with all the matching devices - if( IOServiceGetMatchingServices(kIOMasterPortDefault, matchingDictionary, &serialPortIterator) != KERN_SUCCESS ) { + if (IOServiceGetMatchingServices(kIOMasterPortDefault, matchingDictionary, &serialPortIterator) != KERN_SUCCESS) { qCritical() << "IOServiceGetMatchingServices failed, returned" << kernResult; return infoList; } @@ -50,23 +49,24 @@ void QextSerialEnumerator::iterateServicesOSX(io_object_t service, QListportName = ioPathName; - if( bsdPathAsCFString ) - { + if (bsdPathAsCFString) { char path[MAXPATHLEN]; - if( CFStringGetCString((CFStringRef)bsdPathAsCFString, path, - PATH_MAX, kCFStringEncodingUTF8) ) { - portInfo->physName = path; + if (CFStringGetCString((CFStringRef)bsdPathAsCFString, path, + PATH_MAX, kCFStringEncodingUTF8)) { + portInfo->physName = path; portInfo->friendName = path; } CFRelease(bsdPathAsCFString); } - if(productNameAsCFString) - { + if (productNameAsCFString) { char productName[MAXPATHLEN]; - if( CFStringGetCString((CFStringRef)productNameAsCFString, productName, - PATH_MAX, kCFStringEncodingUTF8) ) + if (CFStringGetCString((CFStringRef)productNameAsCFString, productName, + PATH_MAX, kCFStringEncodingUTF8)) { portInfo->friendName = productName; + } CFRelease(productNameAsCFString); } - if(vendorIdAsCFNumber) - { + if (vendorIdAsCFNumber) { SInt32 vID; - if(CFNumberGetValue((CFNumberRef)vendorIdAsCFNumber, kCFNumberSInt32Type, &vID)) + if (CFNumberGetValue((CFNumberRef)vendorIdAsCFNumber, kCFNumberSInt32Type, &vID)) { portInfo->vendorID = vID; + } CFRelease(vendorIdAsCFNumber); } - if(productIdAsCFNumber) - { + if (productIdAsCFNumber) { SInt32 pID; - if(CFNumberGetValue((CFNumberRef)productIdAsCFNumber, kCFNumberSInt32Type, &pID)) + if (CFNumberGetValue((CFNumberRef)productIdAsCFNumber, kCFNumberSInt32Type, &pID)) { portInfo->productID = pID; + } CFRelease(productIdAsCFNumber); } IOObjectRelease(service); @@ -140,58 +139,66 @@ bool QextSerialEnumerator::getServiceDetailsOSX( io_object_t service, QextPortIn } // IOKit callbacks registered via setupNotifications() -void deviceDiscoveredCallbackOSX( void *ctxt, io_iterator_t serialPortIterator ); -void deviceTerminatedCallbackOSX( void *ctxt, io_iterator_t serialPortIterator ); +void deviceDiscoveredCallbackOSX(void *ctxt, io_iterator_t serialPortIterator); +void deviceTerminatedCallbackOSX(void *ctxt, io_iterator_t serialPortIterator); -void deviceDiscoveredCallbackOSX( void *ctxt, io_iterator_t serialPortIterator ) +void deviceDiscoveredCallbackOSX(void *ctxt, io_iterator_t serialPortIterator) { - QextSerialEnumerator* qese = (QextSerialEnumerator*)ctxt; + QextSerialEnumerator *qese = (QextSerialEnumerator *)ctxt; io_object_t serialService; - while ((serialService = IOIteratorNext(serialPortIterator))) + + while ((serialService = IOIteratorNext(serialPortIterator))) { qese->onDeviceDiscoveredOSX(serialService); + } } -void deviceTerminatedCallbackOSX( void *ctxt, io_iterator_t serialPortIterator ) +void deviceTerminatedCallbackOSX(void *ctxt, io_iterator_t serialPortIterator) { - QextSerialEnumerator* qese = (QextSerialEnumerator*)ctxt; + QextSerialEnumerator *qese = (QextSerialEnumerator *)ctxt; io_object_t serialService; - while ((serialService = IOIteratorNext(serialPortIterator))) + + while ((serialService = IOIteratorNext(serialPortIterator))) { qese->onDeviceTerminatedOSX(serialService); + } } /* - A device has been discovered via IOKit. - Create a QextPortInfo if possible, and emit the signal indicating that we've found it. -*/ -void QextSerialEnumerator::onDeviceDiscoveredOSX( io_object_t service ) + A device has been discovered via IOKit. + Create a QextPortInfo if possible, and emit the signal indicating that we've found it. + */ +void QextSerialEnumerator::onDeviceDiscoveredOSX(io_object_t service) { QextPortInfo info; - info.vendorID = 0; + + info.vendorID = 0; info.productID = 0; - if( getServiceDetailsOSX( service, &info ) ) - emit deviceDiscovered( info ); + if (getServiceDetailsOSX(service, &info)) { + emit deviceDiscovered(info); + } } /* - Notification via IOKit that a device has been removed. - Create a QextPortInfo if possible, and emit the signal indicating that it's gone. -*/ -void QextSerialEnumerator::onDeviceTerminatedOSX( io_object_t service ) + Notification via IOKit that a device has been removed. + Create a QextPortInfo if possible, and emit the signal indicating that it's gone. + */ +void QextSerialEnumerator::onDeviceTerminatedOSX(io_object_t service) { QextPortInfo info; - info.vendorID = 0; + + info.vendorID = 0; info.productID = 0; - if( getServiceDetailsOSX( service, &info ) ) - emit deviceRemoved( info ); + if (getServiceDetailsOSX(service, &info)) { + emit deviceRemoved(info); + } } /* - Create matching dictionaries for the devices we want to get notifications for, - and add them to the current run loop. Invoke the callbacks that will be responding - to these notifications once to arm them, and discover any devices that - are currently connected at the time notifications are setup. -*/ -void QextSerialEnumerator::setUpNotifications( ) + Create matching dictionaries for the devices we want to get notifications for, + and add them to the current run loop. Invoke the callbacks that will be responding + to these notifications once to arm them, and discover any devices that + are currently connected at the time notifications are setup. + */ +void QextSerialEnumerator::setUpNotifications() { kern_return_t kernResult; mach_port_t masterPort; @@ -207,22 +214,23 @@ void QextSerialEnumerator::setUpNotifications( ) } classesToMatch = IOServiceMatching(kIOSerialBSDServiceValue); - if (classesToMatch == NULL) + if (classesToMatch == NULL) { qDebug("IOServiceMatching returned a NULL dictionary."); - else + } else { CFDictionarySetValue(classesToMatch, CFSTR(kIOSerialBSDTypeKey), CFSTR(kIOSerialBSDAllTypes)); + } - if( !(cdcClassesToMatch = IOServiceNameMatching("AppleUSBCDC") ) ) { + if (!(cdcClassesToMatch = IOServiceNameMatching("AppleUSBCDC"))) { qWarning("couldn't create cdc matching dict"); return; } // Retain an additional reference since each call to IOServiceAddMatchingNotification consumes one. - classesToMatch = (CFMutableDictionaryRef) CFRetain(classesToMatch); - cdcClassesToMatch = (CFMutableDictionaryRef) CFRetain(cdcClassesToMatch); + classesToMatch = (CFMutableDictionaryRef)CFRetain(classesToMatch); + cdcClassesToMatch = (CFMutableDictionaryRef)CFRetain(cdcClassesToMatch); notificationPortRef = IONotificationPortCreate(masterPort); - if(notificationPortRef == NULL) { + if (notificationPortRef == NULL) { qDebug("IONotificationPortCreate return a NULL IONotificationPortRef."); return; } @@ -243,7 +251,7 @@ void QextSerialEnumerator::setUpNotifications( ) } // arm the callback, and grab any devices that are already connected - deviceDiscoveredCallbackOSX( this, portIterator ); + deviceDiscoveredCallbackOSX(this, portIterator); kernResult = IOServiceAddMatchingNotification(notificationPortRef, kIOMatchedNotification, cdcClassesToMatch, deviceDiscoveredCallbackOSX, this, &portIterator); @@ -253,7 +261,7 @@ void QextSerialEnumerator::setUpNotifications( ) } // arm the callback, and grab any devices that are already connected - deviceDiscoveredCallbackOSX( this, portIterator ); + deviceDiscoveredCallbackOSX(this, portIterator); kernResult = IOServiceAddMatchingNotification(notificationPortRef, kIOTerminatedNotification, classesToMatch, deviceTerminatedCallbackOSX, this, &portIterator); @@ -263,7 +271,7 @@ void QextSerialEnumerator::setUpNotifications( ) } // arm the callback, and clear any devices that are terminated - deviceTerminatedCallbackOSX( this, portIterator ); + deviceTerminatedCallbackOSX(this, portIterator); kernResult = IOServiceAddMatchingNotification(notificationPortRef, kIOTerminatedNotification, cdcClassesToMatch, deviceTerminatedCallbackOSX, this, &portIterator); @@ -273,6 +281,5 @@ void QextSerialEnumerator::setUpNotifications( ) } // arm the callback, and clear any devices that are terminated - deviceTerminatedCallbackOSX( this, portIterator ); + deviceTerminatedCallbackOSX(this, portIterator); } - diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_unix.cpp b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_unix.cpp index fd5af0d25..f60127279 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_unix.cpp +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_unix.cpp @@ -1,21 +1,18 @@ - - - #include "qextserialenumerator.h" #include #include #include #include -QextSerialEnumerator::QextSerialEnumerator( ) +QextSerialEnumerator::QextSerialEnumerator() { - if( !QMetaType::isRegistered( QMetaType::type("QextPortInfo") ) ) + if (!QMetaType::isRegistered(QMetaType::type("QextPortInfo"))) { qRegisterMetaType("QextPortInfo"); + } } -QextSerialEnumerator::~QextSerialEnumerator( ) -{ -} +QextSerialEnumerator::~QextSerialEnumerator() +{} QList QextSerialEnumerator::getPorts() { @@ -32,7 +29,7 @@ QList QextSerialEnumerator::getPorts() bool ok; QString current = portNameList.at(i); // remove the ttyS part, and check, if the other part is a number - current.remove(0,4).toInt(&ok, 10); + current.remove(0, 4).toInt(&ok, 10); if (!ok) { portNameList.removeAt(i); i--; @@ -46,33 +43,31 @@ QList QextSerialEnumerator::getPorts() portNamePrefixes << "ttyACM*" << "ttyUSB*" << "rfcomm*"; portNameList.append(dir.entryList(portNamePrefixes, (QDir::System | QDir::Files), QDir::Name)); - foreach (QString str , portNameList) { + foreach(QString str, portNameList) { QextPortInfo inf; - inf.physName = "/dev/"+str; + + inf.physName = "/dev/" + str; inf.portName = str; if (str.contains("ttyS")) { - inf.friendName = "Serial port "+str.remove(0, 4); + inf.friendName = "Serial port " + str.remove(0, 4); + } else if (str.contains("ttyUSB")) { + inf.friendName = "USB-serial adapter " + str.remove(0, 6); + } else if (str.contains("rfcomm")) { + inf.friendName = "Bluetooth-serial adapter " + str.remove(0, 6); + } else if (str.contains("ttyACM")) { + inf.friendName = "USB VCP adapter " + str.remove(0, 6); } - else if (str.contains("ttyUSB")) { - inf.friendName = "USB-serial adapter "+str.remove(0, 6); - } - else if (str.contains("rfcomm")) { - inf.friendName = "Bluetooth-serial adapter "+str.remove(0, 6); - } - else if (str.contains("ttyACM")) { - inf.friendName = "USB VCP adapter "+str.remove(0, 6); - } inf.enumName = "/dev"; // is there a more helpful name for this? infoList.append(inf); } -#else +#else // ifdef Q_OS_LINUX qCritical("Enumeration for POSIX systems (except Linux) is not implemented yet."); -#endif +#endif // ifdef Q_OS_LINUX return infoList; } -void QextSerialEnumerator::setUpNotifications( ) +void QextSerialEnumerator::setUpNotifications() { qCritical("Notifications for *Nix/FreeBSD are not implemented yet"); } diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_win.cpp b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_win.cpp index 6026b5458..550b91a40 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_win.cpp +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_win.cpp @@ -1,6 +1,3 @@ - - - #include "qextserialenumerator.h" #include #include @@ -10,68 +7,72 @@ #include "qextserialport.h" #include -QextSerialEnumerator::QextSerialEnumerator( ) +QextSerialEnumerator::QextSerialEnumerator() { - if( !QMetaType::isRegistered( QMetaType::type("QextPortInfo") ) ) + if (!QMetaType::isRegistered(QMetaType::type("QextPortInfo"))) { qRegisterMetaType("QextPortInfo"); + } #if (defined QT_GUI_LIB) notificationWidget = 0; #endif // Q_OS_WIN } -QextSerialEnumerator::~QextSerialEnumerator( ) +QextSerialEnumerator::~QextSerialEnumerator() { #if (defined QT_GUI_LIB) - if( notificationWidget ) + if (notificationWidget) { delete notificationWidget; + } #endif } - // see http://msdn.microsoft.com/en-us/library/ms791134.aspx for list of GUID classes #ifndef GUID_DEVCLASS_PORTS - DEFINE_GUID(GUID_DEVCLASS_PORTS, 0x4D36E978, 0xE325, 0x11CE, 0xBF, 0xC1, 0x08, 0x00, 0x2B, 0xE1, 0x03, 0x18 ); +DEFINE_GUID(GUID_DEVCLASS_PORTS, 0x4D36E978, 0xE325, 0x11CE, 0xBF, 0xC1, 0x08, 0x00, 0x2B, 0xE1, 0x03, 0x18); #endif /* Gordon Schumacher's macros for TCHAR -> QString conversions and vice versa */ #ifdef UNICODE - #define QStringToTCHAR(x) (wchar_t*) x.utf16() - #define PQStringToTCHAR(x) (wchar_t*) x->utf16() - #define TCHARToQString(x) QString::fromUtf16((ushort*)(x)) - #define TCHARToQStringN(x,y) QString::fromUtf16((ushort*)(x),(y)) + #define QStringToTCHAR(x) (wchar_t *)x.utf16() + #define PQStringToTCHAR(x) (wchar_t *)x->utf16() + #define TCHARToQString(x) QString::fromUtf16((ushort *)(x)) + #define TCHARToQStringN(x, y) QString::fromUtf16((ushort *)(x), (y)) #else #define QStringToTCHAR(x) x.local8Bit().constData() #define PQStringToTCHAR(x) x->local8Bit().constData() #define TCHARToQString(x) QString::fromLocal8Bit((x)) - #define TCHARToQStringN(x,y) QString::fromLocal8Bit((x),(y)) + #define TCHARToQStringN(x, y) QString::fromLocal8Bit((x), (y)) #endif /*UNICODE*/ -//static +// static QString QextSerialEnumerator::getRegKeyValue(HKEY key, LPCTSTR property) { DWORD size = 0; DWORD type; - RegQueryValueEx(key, property, NULL, NULL, NULL, & size); - BYTE* buff = new BYTE[size]; + + RegQueryValueEx(key, property, NULL, NULL, NULL, &size); + BYTE *buff = new BYTE[size]; QString result; - if( RegQueryValueEx(key, property, NULL, &type, buff, & size) == ERROR_SUCCESS ) + if (RegQueryValueEx(key, property, NULL, &type, buff, &size) == ERROR_SUCCESS) { result = TCHARToQString(buff); + } RegCloseKey(key); - delete [] buff; + delete[] buff; return result; } -//static +// static QString QextSerialEnumerator::getDeviceProperty(HDEVINFO devInfo, PSP_DEVINFO_DATA devData, DWORD property) { DWORD buffSize = 0; - SetupDiGetDeviceRegistryProperty(devInfo, devData, property, NULL, NULL, 0, & buffSize); - BYTE* buff = new BYTE[buffSize]; + + SetupDiGetDeviceRegistryProperty(devInfo, devData, property, NULL, NULL, 0, &buffSize); + BYTE *buff = new BYTE[buffSize]; SetupDiGetDeviceRegistryProperty(devInfo, devData, property, NULL, buff, buffSize, NULL); QString result = TCHARToQString(buff); - delete [] buff; + delete[] buff; return result; } @@ -82,18 +83,17 @@ QList QextSerialEnumerator::getPorts() return ports; } -void QextSerialEnumerator::enumerateDevicesWin( const GUID & guid, QList* infoList ) +void QextSerialEnumerator::enumerateDevicesWin(const GUID & guid, QList *infoList) { HDEVINFO devInfo; - if( (devInfo = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT)) != INVALID_HANDLE_VALUE) - { + + if ((devInfo = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT)) != INVALID_HANDLE_VALUE) { SP_DEVINFO_DATA devInfoData; devInfoData.cbSize = sizeof(SP_DEVINFO_DATA); - for(int i = 0; SetupDiEnumDeviceInfo(devInfo, i, &devInfoData); i++) - { + for (int i = 0; SetupDiEnumDeviceInfo(devInfo, i, &devInfoData); i++) { QextPortInfo info; info.productID = info.vendorID = 0; - getDeviceDetailsWin( &info, devInfo, &devInfoData ); + getDeviceDetailsWin(&info, devInfo, &devInfoData); infoList->append(info); } SetupDiDestroyDeviceInfoList(devInfo); @@ -101,10 +101,10 @@ void QextSerialEnumerator::enumerateDevicesWin( const GUID & guid, QListmessage == WM_DEVICECHANGE ) { - qese->onDeviceChangeWin( message->wParam, message->lParam ); + if (message->message == WM_DEVICECHANGE) { + qese->onDeviceChangeWin(message->wParam, message->lParam); *result = 1; return true; } @@ -112,11 +112,12 @@ bool QextSerialRegistrationWidget::winEvent( MSG* message, long* result ) } #endif -void QextSerialEnumerator::setUpNotifications( ) +void QextSerialEnumerator::setUpNotifications() { #ifdef QT_GUI_LIB - if(notificationWidget) + if (notificationWidget) { return; + } notificationWidget = new QextSerialRegistrationWidget(this); DEV_BROADCAST_DEVICEINTERFACE dbh; @@ -124,26 +125,25 @@ void QextSerialEnumerator::setUpNotifications( ) dbh.dbcc_size = sizeof(dbh); dbh.dbcc_devicetype = DBT_DEVTYP_DEVICEINTERFACE; CopyMemory(&dbh.dbcc_classguid, &GUID_DEVCLASS_PORTS, sizeof(GUID)); - if( RegisterDeviceNotification( notificationWidget->winId( ), &dbh, DEVICE_NOTIFY_WINDOW_HANDLE ) == NULL) + if (RegisterDeviceNotification(notificationWidget->winId(), &dbh, DEVICE_NOTIFY_WINDOW_HANDLE) == NULL) { qWarning() << "RegisterDeviceNotification failed:" << GetLastError(); + } // setting up notifications doesn't tell us about devices already connected // so get those manually - foreach( QextPortInfo port, getPorts() ) - emit deviceDiscovered( port ); + foreach(QextPortInfo port, getPorts()) + emit deviceDiscovered(port); #else qWarning("QextSerialEnumerator: GUI not enabled - can't register for device notifications."); #endif // QT_GUI_LIB } -LRESULT QextSerialEnumerator::onDeviceChangeWin( WPARAM wParam, LPARAM lParam ) +LRESULT QextSerialEnumerator::onDeviceChangeWin(WPARAM wParam, LPARAM lParam) { - if ( DBT_DEVICEARRIVAL == wParam || DBT_DEVICEREMOVECOMPLETE == wParam ) - { + if (DBT_DEVICEARRIVAL == wParam || DBT_DEVICEREMOVECOMPLETE == wParam) { PDEV_BROADCAST_HDR pHdr = (PDEV_BROADCAST_HDR)lParam; - if( pHdr->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE ) - { + if (pHdr->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE) { PDEV_BROADCAST_DEVICEINTERFACE pDevInf = (PDEV_BROADCAST_DEVICEINTERFACE)pHdr; - // delimiters are different across APIs...change to backslash. ugh. + // delimiters are different across APIs...change to backslash. ugh. QString deviceID = TCHARToQString(pDevInf->dbcc_name).toUpper().replace("#", "\\"); matchAndDispatchChangedDevice(deviceID, GUID_DEVCLASS_PORTS, wParam); @@ -157,25 +157,24 @@ bool QextSerialEnumerator::matchAndDispatchChangedDevice(const QString & deviceI bool rv = false; DWORD dwFlag = (DBT_DEVICEARRIVAL == wParam) ? DIGCF_PRESENT : DIGCF_ALLCLASSES; HDEVINFO devInfo; - if( (devInfo = SetupDiGetClassDevs(&guid,NULL,NULL,dwFlag)) != INVALID_HANDLE_VALUE ) - { + + if ((devInfo = SetupDiGetClassDevs(&guid, NULL, NULL, dwFlag)) != INVALID_HANDLE_VALUE) { SP_DEVINFO_DATA spDevInfoData; spDevInfoData.cbSize = sizeof(SP_DEVINFO_DATA); - for(int i=0; SetupDiEnumDeviceInfo(devInfo, i, &spDevInfoData); i++) - { - DWORD nSize=0 ; + for (int i = 0; SetupDiEnumDeviceInfo(devInfo, i, &spDevInfoData); i++) { + DWORD nSize = 0; TCHAR buf[MAX_PATH]; - if ( SetupDiGetDeviceInstanceId(devInfo, &spDevInfoData, buf, MAX_PATH, &nSize) && - deviceID.contains(TCHARToQString(buf))) // we found a match - { + if (SetupDiGetDeviceInstanceId(devInfo, &spDevInfoData, buf, MAX_PATH, &nSize) && + deviceID.contains(TCHARToQString(buf))) { // we found a match rv = true; QextPortInfo info; info.productID = info.vendorID = 0; - getDeviceDetailsWin( &info, devInfo, &spDevInfoData, wParam ); - if( wParam == DBT_DEVICEARRIVAL ) + getDeviceDetailsWin(&info, devInfo, &spDevInfoData, wParam); + if (wParam == DBT_DEVICEARRIVAL) { emit deviceDiscovered(info); - else if( wParam == DBT_DEVICEREMOVECOMPLETE ) + } else if (wParam == DBT_DEVICEREMOVECOMPLETE) { emit deviceRemoved(info); + } break; } } @@ -184,23 +183,22 @@ bool QextSerialEnumerator::matchAndDispatchChangedDevice(const QString & deviceI return rv; } -bool QextSerialEnumerator::getDeviceDetailsWin( QextPortInfo* portInfo, HDEVINFO devInfo, PSP_DEVINFO_DATA devData, WPARAM wParam ) +bool QextSerialEnumerator::getDeviceDetailsWin(QextPortInfo *portInfo, HDEVINFO devInfo, PSP_DEVINFO_DATA devData, WPARAM wParam) { portInfo->friendName = getDeviceProperty(devInfo, devData, SPDRP_FRIENDLYNAME); - if( wParam == DBT_DEVICEARRIVAL) + if (wParam == DBT_DEVICEARRIVAL) { portInfo->physName = getDeviceProperty(devInfo, devData, SPDRP_PHYSICAL_DEVICE_OBJECT_NAME); + } portInfo->enumName = getDeviceProperty(devInfo, devData, SPDRP_ENUMERATOR_NAME); QString hardwareIDs = getDeviceProperty(devInfo, devData, SPDRP_HARDWAREID); HKEY devKey = SetupDiOpenDevRegKey(devInfo, devData, DICS_FLAG_GLOBAL, 0, DIREG_DEV, KEY_READ); - portInfo->portName = QextSerialPort::fullPortNameWin( getRegKeyValue(devKey, TEXT("PortName")) ); + portInfo->portName = QextSerialPort::fullPortNameWin(getRegKeyValue(devKey, TEXT("PortName"))); QRegExp idRx("VID_(\\w+)&PID_(\\w+)"); - if( hardwareIDs.toUpper().contains(idRx) ) - { + if (hardwareIDs.toUpper().contains(idRx)) { bool dummy; - portInfo->vendorID = idRx.cap(1).toInt(&dummy, 16); + portInfo->vendorID = idRx.cap(1).toInt(&dummy, 16); portInfo->productID = idRx.cap(2).toInt(&dummy, 16); - //qDebug() << "got vid:" << vid << "pid:" << pid; + // qDebug() << "got vid:" << vid << "pid:" << pid; } return true; } - diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.cpp b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.cpp index ccf359839..c13b7e466 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.cpp +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.cpp @@ -1,35 +1,32 @@ - - #include #include "qextserialport.h" /*! -Default constructor. Note that the name of the device used by a QextSerialPort constructed with -this constructor will be determined by #defined constants, or lack thereof - the default behavior -is the same as _TTY_LINUX_. Possible naming conventions and their associated constants are: + Default constructor. Note that the name of the device used by a QextSerialPort constructed with + this constructor will be determined by #defined constants, or lack thereof - the default behavior + is the same as _TTY_LINUX_. Possible naming conventions and their associated constants are: -\verbatim + \verbatim -Constant Used By Naming Convention ----------- ------------- ------------------------ -Q_OS_WIN Windows COM1, COM2 -_TTY_IRIX_ SGI/IRIX /dev/ttyf1, /dev/ttyf2 -_TTY_HPUX_ HP-UX /dev/tty1p0, /dev/tty2p0 -_TTY_SUN_ SunOS/Solaris /dev/ttya, /dev/ttyb -_TTY_DIGITAL_ Digital UNIX /dev/tty01, /dev/tty02 -_TTY_FREEBSD_ FreeBSD /dev/ttyd0, /dev/ttyd1 -_TTY_OPENBSD_ OpenBSD /dev/tty00, /dev/tty01 -_TTY_LINUX_ Linux /dev/ttyS0, /dev/ttyS1 - Linux /dev/ttyS0, /dev/ttyS1 -\endverbatim + Constant Used By Naming Convention + ---------- ------------- ------------------------ + Q_OS_WIN Windows COM1, COM2 + _TTY_IRIX_ SGI/IRIX /dev/ttyf1, /dev/ttyf2 + _TTY_HPUX_ HP-UX /dev/tty1p0, /dev/tty2p0 + _TTY_SUN_ SunOS/Solaris /dev/ttya, /dev/ttyb + _TTY_DIGITAL_ Digital UNIX /dev/tty01, /dev/tty02 + _TTY_FREEBSD_ FreeBSD /dev/ttyd0, /dev/ttyd1 + _TTY_OPENBSD_ OpenBSD /dev/tty00, /dev/tty01 + _TTY_LINUX_ Linux /dev/ttyS0, /dev/ttyS1 + Linux /dev/ttyS0, /dev/ttyS1 + \endverbatim -This constructor assigns the device name to the name of the first port on the specified system. -See the other constructors if you need to open a different port. -*/ + This constructor assigns the device name to the name of the first port on the specified system. + See the other constructors if you need to open a different port. + */ QextSerialPort::QextSerialPort(QextSerialPort::QueryMode mode) - : QIODevice() + : QIODevice() { - #ifdef Q_OS_WIN setPortName("COM1"); @@ -51,9 +48,9 @@ QextSerialPort::QextSerialPort(QextSerialPort::QueryMode mode) #elif defined(_TTY_OPENBSD_) setPortName("/dev/tty00"); -#else +#else // ifdef Q_OS_WIN setPortName("/dev/ttyS0"); -#endif +#endif // ifdef Q_OS_WIN construct(); setQueryMode(mode); @@ -61,10 +58,10 @@ QextSerialPort::QextSerialPort(QextSerialPort::QueryMode mode) } /*! -Constructs a serial port attached to the port specified by name. -name is the name of the device, which is windowsystem-specific, -e.g."COM1" or "/dev/ttyS0". -*/ + Constructs a serial port attached to the port specified by name. + name is the name of the device, which is windowsystem-specific, + e.g."COM1" or "/dev/ttyS0". + */ QextSerialPort::QextSerialPort(const QString & name, QextSerialPort::QueryMode mode) : QIODevice() { @@ -75,9 +72,9 @@ QextSerialPort::QextSerialPort(const QString & name, QextSerialPort::QueryMode m } /*! -Constructs a port with default name and specified settings. -*/ -QextSerialPort::QextSerialPort(const PortSettings& settings, QextSerialPort::QueryMode mode) + Constructs a port with default name and specified settings. + */ +QextSerialPort::QextSerialPort(const PortSettings & settings, QextSerialPort::QueryMode mode) : QIODevice() { construct(); @@ -92,9 +89,9 @@ QextSerialPort::QextSerialPort(const PortSettings& settings, QextSerialPort::Que } /*! -Constructs a port with specified name and settings. -*/ -QextSerialPort::QextSerialPort(const QString & name, const PortSettings& settings, QextSerialPort::QueryMode mode) + Constructs a port with specified name and settings. + */ +QextSerialPort::QextSerialPort(const QString & name, const PortSettings & settings, QextSerialPort::QueryMode mode) : QIODevice() { construct(); @@ -110,19 +107,19 @@ QextSerialPort::QextSerialPort(const QString & name, const PortSettings& setting } /*! -Common constructor function for setting up default port settings. -(115200 Baud, 8N1, Hardware flow control where supported, otherwise no flow control, and 0 ms timeout). -*/ + Common constructor function for setting up default port settings. + (115200 Baud, 8N1, Hardware flow control where supported, otherwise no flow control, and 0 ms timeout). + */ void QextSerialPort::construct() { lastErr = E_NO_ERROR; - Settings.BaudRate=BAUD115200; - Settings.DataBits=DATA_8; - Settings.Parity=PAR_NONE; - Settings.StopBits=STOP_1; - Settings.FlowControl=FLOW_HARDWARE; - Settings.Timeout_Millisec=500; - mutex = new QMutex( QMutex::Recursive ); + Settings.BaudRate = BAUD115200; + Settings.DataBits = DATA_8; + Settings.Parity = PAR_NONE; + Settings.StopBits = STOP_1; + Settings.FlowControl = FLOW_HARDWARE; + Settings.Timeout_Millisec = 500; + mutex = new QMutex(QMutex::Recursive); setOpenMode(QIODevice::NotOpen); } @@ -132,86 +129,87 @@ void QextSerialPort::setQueryMode(QueryMode mechanism) } /*! -Sets the name of the device associated with the object, e.g. "COM1", or "/dev/ttyS0". -*/ + Sets the name of the device associated with the object, e.g. "COM1", or "/dev/ttyS0". + */ void QextSerialPort::setPortName(const QString & name) { #ifdef Q_OS_WIN - port = fullPortNameWin( name ); + port = fullPortNameWin(name); #else port = name; #endif } /*! -Returns the name set by setPortName(). -*/ + Returns the name set by setPortName(). + */ QString QextSerialPort::portName() const { return port; } /*! - Reads all available data from the device, and returns it as a QByteArray. - This function has no way of reporting errors; returning an empty QByteArray() - can mean either that no data was currently available for reading, or that an error occurred. -*/ + Reads all available data from the device, and returns it as a QByteArray. + This function has no way of reporting errors; returning an empty QByteArray() + can mean either that no data was currently available for reading, or that an error occurred. + */ QByteArray QextSerialPort::readAll() { int avail = this->bytesAvailable(); + return (avail > 0) ? this->read(avail) : QByteArray(); } /*! -Returns the baud rate of the serial port. For a list of possible return values see -the definition of the enum BaudRateType. -*/ + Returns the baud rate of the serial port. For a list of possible return values see + the definition of the enum BaudRateType. + */ BaudRateType QextSerialPort::baudRate(void) const { return Settings.BaudRate; } /*! -Returns the number of data bits used by the port. For a list of possible values returned by -this function, see the definition of the enum DataBitsType. -*/ + Returns the number of data bits used by the port. For a list of possible values returned by + this function, see the definition of the enum DataBitsType. + */ DataBitsType QextSerialPort::dataBits() const { return Settings.DataBits; } /*! -Returns the type of parity used by the port. For a list of possible values returned by -this function, see the definition of the enum ParityType. -*/ + Returns the type of parity used by the port. For a list of possible values returned by + this function, see the definition of the enum ParityType. + */ ParityType QextSerialPort::parity() const { return Settings.Parity; } /*! -Returns the number of stop bits used by the port. For a list of possible return values, see -the definition of the enum StopBitsType. -*/ + Returns the number of stop bits used by the port. For a list of possible return values, see + the definition of the enum StopBitsType. + */ StopBitsType QextSerialPort::stopBits() const { return Settings.StopBits; } /*! -Returns the type of flow control used by the port. For a list of possible values returned -by this function, see the definition of the enum FlowType. -*/ + Returns the type of flow control used by the port. For a list of possible values returned + by this function, see the definition of the enum FlowType. + */ FlowType QextSerialPort::flowControl() const { return Settings.FlowControl; } /*! -Returns true if device is sequential, otherwise returns false. Serial port is sequential device -so this function always returns true. Check QIODevice::isSequential() documentation for more -information. -*/ + Returns true if device is sequential, otherwise returns false. Serial port is sequential device + so this function always returns true. Check QIODevice::isSequential() documentation for more + information. + */ bool QextSerialPort::isSequential() const { return true; @@ -219,31 +217,46 @@ bool QextSerialPort::isSequential() const QString QextSerialPort::errorString() { - switch(lastErr) - { - case E_NO_ERROR: return "No Error has occurred"; - case E_INVALID_FD: return "Invalid file descriptor (port was not opened correctly)"; - case E_NO_MEMORY: return "Unable to allocate memory tables (POSIX)"; - case E_CAUGHT_NON_BLOCKED_SIGNAL: return "Caught a non-blocked signal (POSIX)"; - case E_PORT_TIMEOUT: return "Operation timed out (POSIX)"; - case E_INVALID_DEVICE: return "The file opened by the port is not a valid device"; - case E_BREAK_CONDITION: return "The port detected a break condition"; - case E_FRAMING_ERROR: return "The port detected a framing error (usually caused by incorrect baud rate settings)"; - case E_IO_ERROR: return "There was an I/O error while communicating with the port"; - case E_BUFFER_OVERRUN: return "Character buffer overrun"; - case E_RECEIVE_OVERFLOW: return "Receive buffer overflow"; - case E_RECEIVE_PARITY_ERROR: return "The port detected a parity error in the received data"; - case E_TRANSMIT_OVERFLOW: return "Transmit buffer overflow"; - case E_READ_FAILED: return "General read operation failure"; - case E_WRITE_FAILED: return "General write operation failure"; - case E_FILE_NOT_FOUND: return "The "+this->portName()+" file doesn't exists"; - default: return QString("Unknown error: %1").arg(lastErr); + switch (lastErr) { + case E_NO_ERROR: return "No Error has occurred"; + + case E_INVALID_FD: return "Invalid file descriptor (port was not opened correctly)"; + + case E_NO_MEMORY: return "Unable to allocate memory tables (POSIX)"; + + case E_CAUGHT_NON_BLOCKED_SIGNAL: return "Caught a non-blocked signal (POSIX)"; + + case E_PORT_TIMEOUT: return "Operation timed out (POSIX)"; + + case E_INVALID_DEVICE: return "The file opened by the port is not a valid device"; + + case E_BREAK_CONDITION: return "The port detected a break condition"; + + case E_FRAMING_ERROR: return "The port detected a framing error (usually caused by incorrect baud rate settings)"; + + case E_IO_ERROR: return "There was an I/O error while communicating with the port"; + + case E_BUFFER_OVERRUN: return "Character buffer overrun"; + + case E_RECEIVE_OVERFLOW: return "Receive buffer overflow"; + + case E_RECEIVE_PARITY_ERROR: return "The port detected a parity error in the received data"; + + case E_TRANSMIT_OVERFLOW: return "Transmit buffer overflow"; + + case E_READ_FAILED: return "General read operation failure"; + + case E_WRITE_FAILED: return "General write operation failure"; + + case E_FILE_NOT_FOUND: return "The " + this->portName() + " file doesn't exists"; + + default: return QString("Unknown error: %1").arg(lastErr); } } /*! -Standard destructor. -*/ + Standard destructor. + */ QextSerialPort::~QextSerialPort() { if (isOpen()) { diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.h b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.h index 8d3ac29a6..57d3a6931 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.h +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.h @@ -1,4 +1,3 @@ - #ifndef _QEXTSERIALPORT_H_ #define _QEXTSERIALPORT_H_ @@ -18,31 +17,31 @@ #ifdef _TTY_NOWARN_ #define TTY_WARNING(s) #else -#define TTY_WARNING(s) qWarning(s) +#define TTY_WARNING(s) qWarning(s) #endif /*_TTY_NOWARN_*/ /*line status constants*/ -#define LS_CTS 0x01 -#define LS_DSR 0x02 -#define LS_DCD 0x04 -#define LS_RI 0x08 -#define LS_RTS 0x10 -#define LS_DTR 0x20 -#define LS_ST 0x40 -#define LS_SR 0x80 +#define LS_CTS 0x01 +#define LS_DSR 0x02 +#define LS_DCD 0x04 +#define LS_RI 0x08 +#define LS_RTS 0x10 +#define LS_DTR 0x20 +#define LS_ST 0x40 +#define LS_SR 0x80 /*error constants*/ -#define E_NO_ERROR 0 -#define E_INVALID_FD 1 -#define E_NO_MEMORY 2 -#define E_CAUGHT_NON_BLOCKED_SIGNAL 3 -#define E_PORT_TIMEOUT 4 -#define E_INVALID_DEVICE 5 -#define E_BREAK_CONDITION 6 -#define E_FRAMING_ERROR 7 -#define E_IO_ERROR 8 -#define E_BUFFER_OVERRUN 9 +#define E_NO_ERROR 0 +#define E_INVALID_FD 1 +#define E_NO_MEMORY 2 +#define E_CAUGHT_NON_BLOCKED_SIGNAL 3 +#define E_PORT_TIMEOUT 4 +#define E_INVALID_DEVICE 5 +#define E_BREAK_CONDITION 6 +#define E_FRAMING_ERROR 7 +#define E_IO_ERROR 8 +#define E_BUFFER_OVERRUN 9 #define E_RECEIVE_OVERFLOW 10 #define E_RECEIVE_PARITY_ERROR 11 #define E_TRANSMIT_OVERFLOW 12 @@ -50,61 +49,56 @@ #define E_WRITE_FAILED 14 #define E_FILE_NOT_FOUND 15 -enum BaudRateType -{ - BAUD50, //POSIX ONLY - BAUD75, //POSIX ONLY +enum BaudRateType { + BAUD50, // POSIX ONLY + BAUD75, // POSIX ONLY BAUD110, - BAUD134, //POSIX ONLY - BAUD150, //POSIX ONLY - BAUD200, //POSIX ONLY + BAUD134, // POSIX ONLY + BAUD150, // POSIX ONLY + BAUD200, // POSIX ONLY BAUD300, BAUD600, BAUD1200, - BAUD1800, //POSIX ONLY + BAUD1800, // POSIX ONLY BAUD2400, BAUD4800, BAUD9600, - BAUD14400, //WINDOWS ONLY + BAUD14400, // WINDOWS ONLY BAUD19200, BAUD38400, - BAUD56000, //WINDOWS ONLY + BAUD56000, // WINDOWS ONLY BAUD57600, - BAUD76800, //POSIX ONLY + BAUD76800, // POSIX ONLY BAUD115200, - BAUD128000, //WINDOWS ONLY - BAUD230400, //WINDOWS ONLY - BAUD256000, //WINDOWS ONLY - BAUD460800, //WINDOWS ONLY - BAUD921600 //WINDOWS ONLY + BAUD128000, // WINDOWS ONLY + BAUD230400, // WINDOWS ONLY + BAUD256000, // WINDOWS ONLY + BAUD460800, // WINDOWS ONLY + BAUD921600 // WINDOWS ONLY }; -enum DataBitsType -{ +enum DataBitsType { DATA_5, DATA_6, DATA_7, DATA_8 }; -enum ParityType -{ +enum ParityType { PAR_NONE, PAR_ODD, PAR_EVEN, - PAR_MARK, //WINDOWS ONLY + PAR_MARK, // WINDOWS ONLY PAR_SPACE }; -enum StopBitsType -{ +enum StopBitsType { STOP_1, - STOP_1_5, //WINDOWS ONLY + STOP_1_5, // WINDOWS ONLY STOP_2 }; -enum FlowType -{ +enum FlowType { FLOW_OFF, FLOW_HARDWARE, FLOW_XONXOFF @@ -113,13 +107,12 @@ enum FlowType /** * structure to contain port settings */ -struct PortSettings -{ +struct PortSettings { BaudRateType BaudRate; DataBitsType DataBits; - ParityType Parity; + ParityType Parity; StopBitsType StopBits; - FlowType FlowControl; + FlowType FlowControl; long Timeout_Millisec; }; @@ -143,23 +136,23 @@ struct PortSettings #endif /*! -Encapsulates a serial port on both POSIX and Windows systems. + Encapsulates a serial port on both POSIX and Windows systems. -\note -Be sure to check the full list of members, as QIODevice provides quite a lot of -functionality for QextSerialPort. + \note + Be sure to check the full list of members, as QIODevice provides quite a lot of + functionality for QextSerialPort. -\section Usage -QextSerialPort offers both a polling and event driven API. Event driven is typically easier -to use, since you never have to worry about checking for new data. + \section Usage + QextSerialPort offers both a polling and event driven API. Event driven is typically easier + to use, since you never have to worry about checking for new data. -\b Example -\code -QextSerialPort* port = new QextSerialPort("COM1", QextSerialPort::EventDriven); -connect(port, SIGNAL(readyRead()), myClass, SLOT(onDataAvailable())); -port->open(); + \b Example + \code + QextSerialPort* port = new QextSerialPort("COM1", QextSerialPort::EventDriven); + connect(port, SIGNAL(readyRead()), myClass, SLOT(onDataAvailable())); + port->open(); -void MyClass::onDataAvailable() { + void MyClass::onDataAvailable() { int avail = port->bytesAvailable(); if( avail > 0 ) { QByteArray usbdata; @@ -169,172 +162,173 @@ void MyClass::onDataAvailable() { processNewData(usbdata); } } -} -\endcode + } + \endcode -\section Compatibility -The user will be notified of errors and possible portability conflicts at run-time -by default - this behavior can be turned off by defining _TTY_NOWARN_ -(to turn off all warnings) or _TTY_NOWARN_PORT_ (to turn off portability warnings) in the project. + \section Compatibility + The user will be notified of errors and possible portability conflicts at run-time + by default - this behavior can be turned off by defining _TTY_NOWARN_ + (to turn off all warnings) or _TTY_NOWARN_PORT_ (to turn off portability warnings) in the project. -On Windows NT/2000/XP this class uses Win32 serial port functions by default. The user may -select POSIX behavior under NT, 2000, or XP ONLY by defining Q_OS_UNIX in the project. -No guarantees are made as to the quality of POSIX support under NT/2000 however. + On Windows NT/2000/XP this class uses Win32 serial port functions by default. The user may + select POSIX behavior under NT, 2000, or XP ONLY by defining Q_OS_UNIX in the project. + No guarantees are made as to the quality of POSIX support under NT/2000 however. -\author Stefan Sander, Michal Policht, Brandon Fosdick, Liam Staskawicz -*/ -class QEXTSERIALPORT_EXPORT QextSerialPort: public QIODevice -{ + \author Stefan Sander, Michal Policht, Brandon Fosdick, Liam Staskawicz + */ +class QEXTSERIALPORT_EXPORT QextSerialPort : public QIODevice { Q_OBJECT - public: - enum QueryMode { - Polling, - EventDriven - }; +public: + enum QueryMode { + Polling, + EventDriven + }; - QextSerialPort(QueryMode mode = EventDriven); - QextSerialPort(const QString & name, QueryMode mode = EventDriven); - QextSerialPort(PortSettings const& s, QueryMode mode = EventDriven); - QextSerialPort(const QString & name, PortSettings const& s, QueryMode mode = EventDriven); - ~QextSerialPort(); + QextSerialPort(QueryMode mode = EventDriven); + QextSerialPort(const QString & name, QueryMode mode = EventDriven); + QextSerialPort(PortSettings const & s, QueryMode mode = EventDriven); + QextSerialPort(const QString & name, PortSettings const & s, QueryMode mode = EventDriven); + ~QextSerialPort(); - void setPortName(const QString & name); - QString portName() const; + void setPortName(const QString & name); + QString portName() const; - /**! - * Get query mode. - * \return query mode. - */ - inline QueryMode queryMode() const { return _queryMode; } + /**! + * Get query mode. + * \return query mode. + */ + inline QueryMode queryMode() const + { + return _queryMode; + } - /*! - * Set desired serial communication handling style. You may choose from polling - * or event driven approach. This function does nothing when port is open; to - * apply changes port must be reopened. - * - * In event driven approach read() and write() functions are acting - * asynchronously. They return immediately and the operation is performed in - * the background, so they doesn't freeze the calling thread. - * To determine when operation is finished, QextSerialPort runs separate thread - * and monitors serial port events. Whenever the event occurs, adequate signal - * is emitted. - * - * When polling is set, read() and write() are acting synchronously. Signals are - * not working in this mode and some functions may not be available. The advantage - * of polling is that it generates less overhead due to lack of signals emissions - * and it doesn't start separate thread to monitor events. - * - * Generally event driven approach is more capable and friendly, although some - * applications may need as low overhead as possible and then polling comes. - * - * \param mode query mode. - */ - void setQueryMode(QueryMode mode); + /*! + * Set desired serial communication handling style. You may choose from polling + * or event driven approach. This function does nothing when port is open; to + * apply changes port must be reopened. + * + * In event driven approach read() and write() functions are acting + * asynchronously. They return immediately and the operation is performed in + * the background, so they doesn't freeze the calling thread. + * To determine when operation is finished, QextSerialPort runs separate thread + * and monitors serial port events. Whenever the event occurs, adequate signal + * is emitted. + * + * When polling is set, read() and write() are acting synchronously. Signals are + * not working in this mode and some functions may not be available. The advantage + * of polling is that it generates less overhead due to lack of signals emissions + * and it doesn't start separate thread to monitor events. + * + * Generally event driven approach is more capable and friendly, although some + * applications may need as low overhead as possible and then polling comes. + * + * \param mode query mode. + */ + void setQueryMode(QueryMode mode); - void setBaudRate(BaudRateType); - BaudRateType baudRate() const; + void setBaudRate(BaudRateType); + BaudRateType baudRate() const; - void setDataBits(DataBitsType); - DataBitsType dataBits() const; + void setDataBits(DataBitsType); + DataBitsType dataBits() const; - void setParity(ParityType); - ParityType parity() const; + void setParity(ParityType); + ParityType parity() const; - void setStopBits(StopBitsType); - StopBitsType stopBits() const; + void setStopBits(StopBitsType); + StopBitsType stopBits() const; - void setFlowControl(FlowType); - FlowType flowControl() const; + void setFlowControl(FlowType); + FlowType flowControl() const; - void setTimeout(long); + void setTimeout(long); - bool open(OpenMode mode); - bool isSequential() const; - void close(); - void flush(); + bool open(OpenMode mode); + bool isSequential() const; + void close(); + void flush(); - qint64 size() const; - qint64 bytesAvailable() const; - QByteArray readAll(); + qint64 size() const; + qint64 bytesAvailable() const; + QByteArray readAll(); - void ungetChar(char c); + void ungetChar(char c); - ulong lastError() const; - void translateError(ulong error); + ulong lastError() const; + void translateError(ulong error); - void setDtr(bool set=true); - void setRts(bool set=true); - ulong lineStatus(); - QString errorString(); + void setDtr(bool set = true); + void setRts(bool set = true); + ulong lineStatus(); + QString errorString(); #ifdef Q_OS_WIN - virtual bool waitForReadyRead(int msecs); ///< @todo implement. - virtual qint64 bytesToWrite() const; - static QString fullPortNameWin(const QString & name); + virtual bool waitForReadyRead(int msecs); ///< @todo implement. + virtual qint64 bytesToWrite() const; + static QString fullPortNameWin(const QString & name); #endif - protected: - QMutex* mutex; - QString port; - PortSettings Settings; - ulong lastErr; - QueryMode _queryMode; +protected: + QMutex *mutex; + QString port; + PortSettings Settings; + ulong lastErr; + QueryMode _queryMode; - // platform specific members + // platform specific members #ifdef Q_OS_UNIX - int fd; - QSocketNotifier *readNotifier; - struct termios Posix_CommConfig; - struct termios old_termios; - struct timeval Posix_Timeout; - struct timeval Posix_Copy_Timeout; + int fd; + QSocketNotifier *readNotifier; + struct termios Posix_CommConfig; + struct termios old_termios; + struct timeval Posix_Timeout; + struct timeval Posix_Copy_Timeout; #elif (defined Q_OS_WIN) - HANDLE Win_Handle; - OVERLAPPED overlap; - COMMCONFIG Win_CommConfig; - COMMTIMEOUTS Win_CommTimeouts; - QWinEventNotifier *winEventNotifier; - DWORD eventMask; - QList pendingWrites; - QReadWriteLock* bytesToWriteLock; - qint64 _bytesToWrite; + HANDLE Win_Handle; + OVERLAPPED overlap; + COMMCONFIG Win_CommConfig; + COMMTIMEOUTS Win_CommTimeouts; + QWinEventNotifier *winEventNotifier; + DWORD eventMask; + QList pendingWrites; + QReadWriteLock *bytesToWriteLock; + qint64 _bytesToWrite; #endif - void construct(); // common construction - void platformSpecificDestruct(); - void platformSpecificInit(); - qint64 readData(char * data, qint64 maxSize); - qint64 writeData(const char * data, qint64 maxSize); + void construct(); // common construction + void platformSpecificDestruct(); + void platformSpecificInit(); + qint64 readData(char *data, qint64 maxSize); + qint64 writeData(const char *data, qint64 maxSize); #ifdef Q_OS_WIN - private slots: - void onWinEvent(HANDLE h); - void triggerTxEmpty(); +private slots: + void onWinEvent(HANDLE h); + void triggerTxEmpty(); private: - QTimer fakeTxEmpty; + QTimer fakeTxEmpty; #endif - private: - Q_DISABLE_COPY(QextSerialPort) +private: + Q_DISABLE_COPY(QextSerialPort) - signals: -// /** -// * This signal is emitted whenever port settings are updated. -// * \param valid \p true if settings are valid, \p false otherwise. -// * -// * @todo implement. -// */ -// // void validSettings(bool valid); - - /*! - * This signal is emitted whenever dsr line has changed its state. You may - * use this signal to check if device is connected. - * \param status \p true when DSR signal is on, \p false otherwise. - * - * \see lineStatus(). - */ - void dsrChanged(bool status); +signals: +///** +// * This signal is emitted whenever port settings are updated. +// * \param valid \p true if settings are valid, \p false otherwise. +// * +// * @todo implement. +// */ +//// void validSettings(bool valid); + /*! + * This signal is emitted whenever dsr line has changed its state. You may + * use this signal to check if device is connected. + * \param status \p true when DSR signal is on, \p false otherwise. + * + * \see lineStatus(). + */ + void dsrChanged(bool status); }; -#endif +#endif // ifndef _QEXTSERIALPORT_H_ diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport_global.h b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport_global.h index a2e65346b..c8210867b 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport_global.h +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport_global.h @@ -1,5 +1,3 @@ - - #ifndef QEXTSERIALPORT_GLOBAL_H #define QEXTSERIALPORT_GLOBAL_H @@ -12,4 +10,3 @@ #endif #endif // QEXTSERIALPORT_GLOBAL_H - diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/win_qextserialport.cpp b/ground/openpilotgcs/src/libs/qextserialport/src/win_qextserialport.cpp index 1fe0ff3ba..d44cbf03c 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/win_qextserialport.cpp +++ b/ground/openpilotgcs/src/libs/qextserialport/src/win_qextserialport.cpp @@ -1,30 +1,29 @@ - - #include #include #include #include "qextserialport.h" #if (defined Q_OS_WIN) - #define CBR_230400 230400 - #define CBR_460800 460800 - #define CBR_921600 921600 + #define CBR_230400 230400 + #define CBR_460800 460800 + #define CBR_921600 921600 #endif void QextSerialPort::platformSpecificInit() { - Win_Handle=INVALID_HANDLE_VALUE; + Win_Handle = INVALID_HANDLE_VALUE; ZeroMemory(&overlap, sizeof(OVERLAPPED)); - overlap.hEvent = CreateEvent(NULL, true, false, NULL); + overlap.hEvent = CreateEvent(NULL, true, false, NULL); winEventNotifier = 0; bytesToWriteLock = new QReadWriteLock; - _bytesToWrite = 0; + _bytesToWrite = 0; } /*! -Standard destructor. -*/ -void QextSerialPort::platformSpecificDestruct() { + Standard destructor. + */ +void QextSerialPort::platformSpecificDestruct() +{ CloseHandle(overlap.hEvent); delete bytesToWriteLock; } @@ -33,45 +32,51 @@ QString QextSerialPort::fullPortNameWin(const QString & name) { QRegExp rx("^COM(\\d+)"); QString fullName(name); - if(fullName.contains(rx)) { + + if (fullName.contains(rx)) { int portnum = rx.cap(1).toInt(); - if(portnum > 9) // COM ports greater than 9 need \\.\ prepended + if (portnum > 9) { // COM ports greater than 9 need \\.\ prepended fullName.prepend("\\\\.\\"); + } } return fullName; } /*! -Opens a serial port. Note that this function does not specify which device to open. If you need -to open a device by name, see QextSerialPort::open(const char*). This function has no effect -if the port associated with the class is already open. The port is also configured to the current -settings, as stored in the Settings structure. -*/ -bool QextSerialPort::open(OpenMode mode) { - unsigned long confSize = sizeof(COMMCONFIG); + Opens a serial port. Note that this function does not specify which device to open. If you need + to open a device by name, see QextSerialPort::open(const char*). This function has no effect + if the port associated with the class is already open. The port is also configured to the current + settings, as stored in the Settings structure. + */ +bool QextSerialPort::open(OpenMode mode) +{ + unsigned long confSize = sizeof(COMMCONFIG); + Win_CommConfig.dwSize = confSize; DWORD dwFlagsAndAttributes = 0; - if (queryMode() == QextSerialPort::EventDriven) + if (queryMode() == QextSerialPort::EventDriven) { dwFlagsAndAttributes += FILE_FLAG_OVERLAPPED; + } QMutexLocker lock(mutex); - if (mode == QIODevice::NotOpen) + if (mode == QIODevice::NotOpen) { return isOpen(); + } if (!isOpen()) { /*open the port*/ - Win_Handle=CreateFileA(port.toAscii(), GENERIC_READ|GENERIC_WRITE, - 0, NULL, OPEN_EXISTING, dwFlagsAndAttributes, NULL); - if (Win_Handle!=INVALID_HANDLE_VALUE) { + Win_Handle = CreateFileA(port.toAscii(), GENERIC_READ | GENERIC_WRITE, + 0, NULL, OPEN_EXISTING, dwFlagsAndAttributes, NULL); + if (Win_Handle != INVALID_HANDLE_VALUE) { QIODevice::open(mode); /*configure port settings*/ GetCommConfig(Win_Handle, &Win_CommConfig, &confSize); GetCommState(Win_Handle, &(Win_CommConfig.dcb)); /*set up parameters*/ - Win_CommConfig.dcb.fBinary=TRUE; - Win_CommConfig.dcb.fInX=FALSE; - Win_CommConfig.dcb.fOutX=FALSE; - Win_CommConfig.dcb.fAbortOnError=FALSE; - Win_CommConfig.dcb.fNull=FALSE; + Win_CommConfig.dcb.fBinary = TRUE; + Win_CommConfig.dcb.fInX = FALSE; + Win_CommConfig.dcb.fOutX = FALSE; + Win_CommConfig.dcb.fAbortOnError = FALSE; + Win_CommConfig.dcb.fNull = FALSE; setBaudRate(Settings.BaudRate); setDataBits(Settings.DataBits); setStopBits(Settings.StopBits); @@ -80,24 +85,23 @@ bool QextSerialPort::open(OpenMode mode) { setTimeout(Settings.Timeout_Millisec); SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - //init event driven approach + // init event driven approach if (queryMode() == QextSerialPort::EventDriven) { Win_CommTimeouts.ReadIntervalTimeout = MAXDWORD; - Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; - Win_CommTimeouts.ReadTotalTimeoutConstant = 0; + Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; + Win_CommTimeouts.ReadTotalTimeoutConstant = 0; Win_CommTimeouts.WriteTotalTimeoutMultiplier = 0; - Win_CommTimeouts.WriteTotalTimeoutConstant = 0; + Win_CommTimeouts.WriteTotalTimeoutConstant = 0; SetCommTimeouts(Win_Handle, &Win_CommTimeouts); - if (!SetCommMask( Win_Handle, EV_TXEMPTY | EV_RXCHAR | EV_DSR)) { + if (!SetCommMask(Win_Handle, EV_TXEMPTY | EV_RXCHAR | EV_DSR)) { qWarning() << "failed to set Comm Mask. Error code:", GetLastError(); return false; } winEventNotifier = new QWinEventNotifier(overlap.hEvent, this); connect(winEventNotifier, SIGNAL(activated(HANDLE)), this, SLOT(onWinEvent(HANDLE))); - connect(&fakeTxEmpty,SIGNAL(timeout()),this,SLOT(triggerTxEmpty())); + connect(&fakeTxEmpty, SIGNAL(timeout()), this, SLOT(triggerTxEmpty())); fakeTxEmpty.start(10000); WaitCommEvent(Win_Handle, &eventMask, &overlap); - } } } else { @@ -107,26 +111,29 @@ bool QextSerialPort::open(OpenMode mode) { } /*! -Closes a serial port. This function has no effect if the serial port associated with the class -is not currently open. -*/ + Closes a serial port. This function has no effect if the serial port associated with the class + is not currently open. + */ void QextSerialPort::close() { QMutexLocker lock(mutex); + fakeTxEmpty.stop(); - disconnect(&fakeTxEmpty,SIGNAL(timeout()),this,SLOT(triggerTxEmpty())); + disconnect(&fakeTxEmpty, SIGNAL(timeout()), this, SLOT(triggerTxEmpty())); if (isOpen()) { - flush(); + flush(); QIODevice::close(); // mark ourselves as closed CancelIo(Win_Handle); - if (CloseHandle(Win_Handle)) + if (CloseHandle(Win_Handle)) { Win_Handle = INVALID_HANDLE_VALUE; - if (winEventNotifier) + } + if (winEventNotifier) { winEventNotifier->deleteLater(); + } _bytesToWrite = 0; - foreach(OVERLAPPED* o, pendingWrites) { + foreach(OVERLAPPED * o, pendingWrites) { CloseHandle(o->hEvent); delete o; } @@ -135,667 +142,668 @@ void QextSerialPort::close() } /*! -Flushes all pending I/O to the serial port. This function has no effect if the serial port -associated with the class is not currently open. -*/ -void QextSerialPort::flush() { + Flushes all pending I/O to the serial port. This function has no effect if the serial port + associated with the class is not currently open. + */ +void QextSerialPort::flush() +{ QMutexLocker lock(mutex); + if (isOpen()) { FlushFileBuffers(Win_Handle); } } /*! -This function will return the number of bytes waiting in the receive queue of the serial port. -It is included primarily to provide a complete QIODevice interface, and will not record errors -in the lastErr member (because it is const). This function is also not thread-safe - in -multithreading situations, use QextSerialPort::bytesAvailable() instead. -*/ -qint64 QextSerialPort::size() const { + This function will return the number of bytes waiting in the receive queue of the serial port. + It is included primarily to provide a complete QIODevice interface, and will not record errors + in the lastErr member (because it is const). This function is also not thread-safe - in + multithreading situations, use QextSerialPort::bytesAvailable() instead. + */ +qint64 QextSerialPort::size() const +{ int availBytes; COMSTAT Win_ComStat; - DWORD Win_ErrorMask=0; + DWORD Win_ErrorMask = 0; + ClearCommError(Win_Handle, &Win_ErrorMask, &Win_ComStat); availBytes = Win_ComStat.cbInQue; return (qint64)availBytes; } /*! -Returns the number of bytes waiting in the port's receive queue. This function will return 0 if -the port is not currently open, or -1 on error. -*/ -qint64 QextSerialPort::bytesAvailable() const { + Returns the number of bytes waiting in the port's receive queue. This function will return 0 if + the port is not currently open, or -1 on error. + */ +qint64 QextSerialPort::bytesAvailable() const +{ QMutexLocker lock(mutex); + if (isOpen()) { DWORD Errors; COMSTAT Status; if (ClearCommError(Win_Handle, &Errors, &Status)) { return Status.cbInQue + QIODevice::bytesAvailable(); } - return (qint64)-1; + return (qint64) - 1; } return 0; } /*! -Translates a system-specific error code to a QextSerialPort error code. Used internally. -*/ -void QextSerialPort::translateError(ulong error) { - if (error&CE_BREAK) { - lastErr=E_BREAK_CONDITION; - } - else if (error&CE_FRAME) { - lastErr=E_FRAMING_ERROR; - } - else if (error&CE_IOE) { - lastErr=E_IO_ERROR; - } - else if (error&CE_MODE) { - lastErr=E_INVALID_FD; - } - else if (error&CE_OVERRUN) { - lastErr=E_BUFFER_OVERRUN; - } - else if (error&CE_RXPARITY) { - lastErr=E_RECEIVE_PARITY_ERROR; - } - else if (error&CE_RXOVER) { - lastErr=E_RECEIVE_OVERFLOW; - } - else if (error&CE_TXFULL) { - lastErr=E_TRANSMIT_OVERFLOW; + Translates a system-specific error code to a QextSerialPort error code. Used internally. + */ +void QextSerialPort::translateError(ulong error) +{ + if (error & CE_BREAK) { + lastErr = E_BREAK_CONDITION; + } else if (error & CE_FRAME) { + lastErr = E_FRAMING_ERROR; + } else if (error & CE_IOE) { + lastErr = E_IO_ERROR; + } else if (error & CE_MODE) { + lastErr = E_INVALID_FD; + } else if (error & CE_OVERRUN) { + lastErr = E_BUFFER_OVERRUN; + } else if (error & CE_RXPARITY) { + lastErr = E_RECEIVE_PARITY_ERROR; + } else if (error & CE_RXOVER) { + lastErr = E_RECEIVE_OVERFLOW; + } else if (error & CE_TXFULL) { + lastErr = E_TRANSMIT_OVERFLOW; } } /*! -Reads a block of data from the serial port. This function will read at most maxlen bytes from -the serial port and place them in the buffer pointed to by data. Return value is the number of -bytes actually read, or -1 on error. + Reads a block of data from the serial port. This function will read at most maxlen bytes from + the serial port and place them in the buffer pointed to by data. Return value is the number of + bytes actually read, or -1 on error. -\warning before calling this function ensure that serial port associated with this class -is currently open (use isOpen() function to check if port is open). -*/ + \warning before calling this function ensure that serial port associated with this class + is currently open (use isOpen() function to check if port is open). + */ qint64 QextSerialPort::readData(char *data, qint64 maxSize) { DWORD retVal; QMutexLocker lock(mutex); + retVal = 0; if (queryMode() == QextSerialPort::EventDriven) { OVERLAPPED overlapRead; ZeroMemory(&overlapRead, sizeof(OVERLAPPED)); - if (!ReadFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, & overlapRead)) { - if (GetLastError() == ERROR_IO_PENDING) - GetOverlappedResult(Win_Handle, & overlapRead, & retVal, true); - else { + if (!ReadFile(Win_Handle, (void *)data, (DWORD)maxSize, &retVal, &overlapRead)) { + if (GetLastError() == ERROR_IO_PENDING) { + GetOverlappedResult(Win_Handle, &overlapRead, &retVal, true); + } else { lastErr = E_READ_FAILED; - retVal = (DWORD)-1; + retVal = (DWORD)-1; } } - } else if (!ReadFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, NULL)) { + } else if (!ReadFile(Win_Handle, (void *)data, (DWORD)maxSize, &retVal, NULL)) { lastErr = E_READ_FAILED; - retVal = (DWORD)-1; + retVal = (DWORD)-1; } return (qint64)retVal; } /*! -Writes a block of data to the serial port. This function will write len bytes -from the buffer pointed to by data to the serial port. Return value is the number -of bytes actually written, or -1 on error. + Writes a block of data to the serial port. This function will write len bytes + from the buffer pointed to by data to the serial port. Return value is the number + of bytes actually written, or -1 on error. -\warning before calling this function ensure that serial port associated with this class -is currently open (use isOpen() function to check if port is open). -*/ + \warning before calling this function ensure that serial port associated with this class + is currently open (use isOpen() function to check if port is open). + */ qint64 QextSerialPort::writeData(const char *data, qint64 maxSize) { - QMutexLocker lock( mutex ); + QMutexLocker lock(mutex); DWORD retVal = 0; + if (queryMode() == QextSerialPort::EventDriven) { - OVERLAPPED* newOverlapWrite = new OVERLAPPED; + OVERLAPPED *newOverlapWrite = new OVERLAPPED; ZeroMemory(newOverlapWrite, sizeof(OVERLAPPED)); newOverlapWrite->hEvent = CreateEvent(NULL, true, false, NULL); - if (WriteFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, newOverlapWrite)) { + if (WriteFile(Win_Handle, (void *)data, (DWORD)maxSize, &retVal, newOverlapWrite)) { CloseHandle(newOverlapWrite->hEvent); delete newOverlapWrite; - } - else if (GetLastError() == ERROR_IO_PENDING) { + } else if (GetLastError() == ERROR_IO_PENDING) { // writing asynchronously...not an error QWriteLocker writelocker(bytesToWriteLock); _bytesToWrite += maxSize; pendingWrites.append(newOverlapWrite); - } - else { + } else { qDebug() << "serialport write error:" << GetLastError(); lastErr = E_WRITE_FAILED; - retVal = (DWORD)-1; - if(!CancelIo(newOverlapWrite->hEvent)) + retVal = (DWORD)-1; + if (!CancelIo(newOverlapWrite->hEvent)) { qDebug() << "serialport: couldn't cancel IO"; - if(!CloseHandle(newOverlapWrite->hEvent)) + } + if (!CloseHandle(newOverlapWrite->hEvent)) { qDebug() << "serialport: couldn't close OVERLAPPED handle"; + } delete newOverlapWrite; } - } else if (!WriteFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, NULL)) { + } else if (!WriteFile(Win_Handle, (void *)data, (DWORD)maxSize, &retVal, NULL)) { lastErr = E_WRITE_FAILED; - retVal = (DWORD)-1; + retVal = (DWORD)-1; } return (qint64)retVal; } /*! -This function is included to implement the full QIODevice interface, and currently has no -purpose within this class. This function is meaningless on an unbuffered device and currently -only prints a warning message to that effect. -*/ -void QextSerialPort::ungetChar(char c) { - + This function is included to implement the full QIODevice interface, and currently has no + purpose within this class. This function is meaningless on an unbuffered device and currently + only prints a warning message to that effect. + */ +void QextSerialPort::ungetChar(char c) +{ /*meaningless on unbuffered sequential device - return error and print a warning*/ TTY_WARNING("QextSerialPort: ungetChar() called on an unbuffered sequential device - operation is meaningless"); } /*! -Sets the flow control used by the port. Possible values of flow are: -\verbatim + Sets the flow control used by the port. Possible values of flow are: + \verbatim FLOW_OFF No flow control FLOW_HARDWARE Hardware (RTS/CTS) flow control FLOW_XONXOFF Software (XON/XOFF) flow control -\endverbatim -*/ -void QextSerialPort::setFlowControl(FlowType flow) { + \endverbatim + */ +void QextSerialPort::setFlowControl(FlowType flow) +{ QMutexLocker lock(mutex); - if (Settings.FlowControl!=flow) { - Settings.FlowControl=flow; + + if (Settings.FlowControl != flow) { + Settings.FlowControl = flow; } if (isOpen()) { - switch(flow) { + switch (flow) { + /*no flow control*/ + case FLOW_OFF: + Win_CommConfig.dcb.fOutxCtsFlow = FALSE; + Win_CommConfig.dcb.fRtsControl = RTS_CONTROL_DISABLE; + Win_CommConfig.dcb.fInX = FALSE; + Win_CommConfig.dcb.fOutX = FALSE; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; - /*no flow control*/ - case FLOW_OFF: - Win_CommConfig.dcb.fOutxCtsFlow=FALSE; - Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_DISABLE; - Win_CommConfig.dcb.fInX=FALSE; - Win_CommConfig.dcb.fOutX=FALSE; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; + /*software (XON/XOFF) flow control*/ + case FLOW_XONXOFF: + Win_CommConfig.dcb.fOutxCtsFlow = FALSE; + Win_CommConfig.dcb.fRtsControl = RTS_CONTROL_DISABLE; + Win_CommConfig.dcb.fInX = TRUE; + Win_CommConfig.dcb.fOutX = TRUE; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; - /*software (XON/XOFF) flow control*/ - case FLOW_XONXOFF: - Win_CommConfig.dcb.fOutxCtsFlow=FALSE; - Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_DISABLE; - Win_CommConfig.dcb.fInX=TRUE; - Win_CommConfig.dcb.fOutX=TRUE; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; - - case FLOW_HARDWARE: - Win_CommConfig.dcb.fOutxCtsFlow=TRUE; - Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_HANDSHAKE; - Win_CommConfig.dcb.fInX=FALSE; - Win_CommConfig.dcb.fOutX=FALSE; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; + case FLOW_HARDWARE: + Win_CommConfig.dcb.fOutxCtsFlow = TRUE; + Win_CommConfig.dcb.fRtsControl = RTS_CONTROL_HANDSHAKE; + Win_CommConfig.dcb.fInX = FALSE; + Win_CommConfig.dcb.fOutX = FALSE; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; } } } /*! -Sets the parity associated with the serial port. The possible values of parity are: -\verbatim + Sets the parity associated with the serial port. The possible values of parity are: + \verbatim PAR_SPACE Space Parity PAR_MARK Mark Parity PAR_NONE No Parity PAR_EVEN Even Parity PAR_ODD Odd Parity -\endverbatim -*/ -void QextSerialPort::setParity(ParityType parity) { + \endverbatim + */ +void QextSerialPort::setParity(ParityType parity) +{ QMutexLocker lock(mutex); - if (Settings.Parity!=parity) { - Settings.Parity=parity; + + if (Settings.Parity != parity) { + Settings.Parity = parity; } if (isOpen()) { - Win_CommConfig.dcb.Parity=(unsigned char)parity; + Win_CommConfig.dcb.Parity = (unsigned char)parity; switch (parity) { + /*space parity*/ + case PAR_SPACE: + if (Settings.DataBits == DATA_8) { + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Space parity with 8 data bits is not supported by POSIX systems."); + } + Win_CommConfig.dcb.fParity = TRUE; + break; - /*space parity*/ - case PAR_SPACE: - if (Settings.DataBits==DATA_8) { - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Space parity with 8 data bits is not supported by POSIX systems."); - } - Win_CommConfig.dcb.fParity=TRUE; - break; + /*mark parity - WINDOWS ONLY*/ + case PAR_MARK: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Mark parity is not supported by POSIX systems"); + Win_CommConfig.dcb.fParity = TRUE; + break; - /*mark parity - WINDOWS ONLY*/ - case PAR_MARK: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Mark parity is not supported by POSIX systems"); - Win_CommConfig.dcb.fParity=TRUE; - break; + /*no parity*/ + case PAR_NONE: + Win_CommConfig.dcb.fParity = FALSE; + break; - /*no parity*/ - case PAR_NONE: - Win_CommConfig.dcb.fParity=FALSE; - break; + /*even parity*/ + case PAR_EVEN: + Win_CommConfig.dcb.fParity = TRUE; + break; - /*even parity*/ - case PAR_EVEN: - Win_CommConfig.dcb.fParity=TRUE; - break; - - /*odd parity*/ - case PAR_ODD: - Win_CommConfig.dcb.fParity=TRUE; - break; + /*odd parity*/ + case PAR_ODD: + Win_CommConfig.dcb.fParity = TRUE; + break; } SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); } } /*! -Sets the number of data bits used by the serial port. Possible values of dataBits are: -\verbatim + Sets the number of data bits used by the serial port. Possible values of dataBits are: + \verbatim DATA_5 5 data bits DATA_6 6 data bits DATA_7 7 data bits DATA_8 8 data bits -\endverbatim + \endverbatim -\note -This function is subject to the following restrictions: -\par + \note + This function is subject to the following restrictions: + \par 5 data bits cannot be used with 2 stop bits. -\par + \par 1.5 stop bits can only be used with 5 data bits. -\par + \par 8 data bits cannot be used with space parity on POSIX systems. -*/ -void QextSerialPort::setDataBits(DataBitsType dataBits) { + */ +void QextSerialPort::setDataBits(DataBitsType dataBits) +{ QMutexLocker lock(mutex); - if (Settings.DataBits!=dataBits) { - if ((Settings.StopBits==STOP_2 && dataBits==DATA_5) || - (Settings.StopBits==STOP_1_5 && dataBits!=DATA_5)) { - } - else { - Settings.DataBits=dataBits; + + if (Settings.DataBits != dataBits) { + if ((Settings.StopBits == STOP_2 && dataBits == DATA_5) || + (Settings.StopBits == STOP_1_5 && dataBits != DATA_5)) {} else { + Settings.DataBits = dataBits; } } if (isOpen()) { - switch(dataBits) { + switch (dataBits) { + /*5 data bits*/ + case DATA_5: + if (Settings.StopBits == STOP_2) { + TTY_WARNING("QextSerialPort: 5 Data bits cannot be used with 2 stop bits."); + } else { + Win_CommConfig.dcb.ByteSize = 5; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; - /*5 data bits*/ - case DATA_5: - if (Settings.StopBits==STOP_2) { - TTY_WARNING("QextSerialPort: 5 Data bits cannot be used with 2 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=5; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; + /*6 data bits*/ + case DATA_6: + if (Settings.StopBits == STOP_1_5) { + TTY_WARNING("QextSerialPort: 6 Data bits cannot be used with 1.5 stop bits."); + } else { + Win_CommConfig.dcb.ByteSize = 6; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; - /*6 data bits*/ - case DATA_6: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("QextSerialPort: 6 Data bits cannot be used with 1.5 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=6; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; + /*7 data bits*/ + case DATA_7: + if (Settings.StopBits == STOP_1_5) { + TTY_WARNING("QextSerialPort: 7 Data bits cannot be used with 1.5 stop bits."); + } else { + Win_CommConfig.dcb.ByteSize = 7; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; - /*7 data bits*/ - case DATA_7: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("QextSerialPort: 7 Data bits cannot be used with 1.5 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=7; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; - - /*8 data bits*/ - case DATA_8: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("QextSerialPort: 8 Data bits cannot be used with 1.5 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=8; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; + /*8 data bits*/ + case DATA_8: + if (Settings.StopBits == STOP_1_5) { + TTY_WARNING("QextSerialPort: 8 Data bits cannot be used with 1.5 stop bits."); + } else { + Win_CommConfig.dcb.ByteSize = 8; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; } } } /*! -Sets the number of stop bits used by the serial port. Possible values of stopBits are: -\verbatim + Sets the number of stop bits used by the serial port. Possible values of stopBits are: + \verbatim STOP_1 1 stop bit STOP_1_5 1.5 stop bits STOP_2 2 stop bits -\endverbatim + \endverbatim -\note -This function is subject to the following restrictions: -\par + \note + This function is subject to the following restrictions: + \par 2 stop bits cannot be used with 5 data bits. -\par + \par 1.5 stop bits cannot be used with 6 or more data bits. -\par + \par POSIX does not support 1.5 stop bits. -*/ -void QextSerialPort::setStopBits(StopBitsType stopBits) { + */ +void QextSerialPort::setStopBits(StopBitsType stopBits) +{ QMutexLocker lock(mutex); - if (Settings.StopBits!=stopBits) { - if ((Settings.DataBits==DATA_5 && stopBits==STOP_2) || - (stopBits==STOP_1_5 && Settings.DataBits!=DATA_5)) { - } - else { - Settings.StopBits=stopBits; + + if (Settings.StopBits != stopBits) { + if ((Settings.DataBits == DATA_5 && stopBits == STOP_2) || + (stopBits == STOP_1_5 && Settings.DataBits != DATA_5)) {} else { + Settings.StopBits = stopBits; } } if (isOpen()) { switch (stopBits) { + /*one stop bit*/ + case STOP_1: + Win_CommConfig.dcb.StopBits = ONESTOPBIT; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; - /*one stop bit*/ - case STOP_1: - Win_CommConfig.dcb.StopBits=ONESTOPBIT; + /*1.5 stop bits*/ + case STOP_1_5: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: 1.5 stop bit operation is not supported by POSIX."); + if (Settings.DataBits != DATA_5) { + TTY_WARNING("QextSerialPort: 1.5 stop bits can only be used with 5 data bits"); + } else { + Win_CommConfig.dcb.StopBits = ONE5STOPBITS; SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; + } + break; - /*1.5 stop bits*/ - case STOP_1_5: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: 1.5 stop bit operation is not supported by POSIX."); - if (Settings.DataBits!=DATA_5) { - TTY_WARNING("QextSerialPort: 1.5 stop bits can only be used with 5 data bits"); - } - else { - Win_CommConfig.dcb.StopBits=ONE5STOPBITS; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; - - /*two stop bits*/ - case STOP_2: - if (Settings.DataBits==DATA_5) { - TTY_WARNING("QextSerialPort: 2 stop bits cannot be used with 5 data bits"); - } - else { - Win_CommConfig.dcb.StopBits=TWOSTOPBITS; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; + /*two stop bits*/ + case STOP_2: + if (Settings.DataBits == DATA_5) { + TTY_WARNING("QextSerialPort: 2 stop bits cannot be used with 5 data bits"); + } else { + Win_CommConfig.dcb.StopBits = TWOSTOPBITS; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; } } } /*! -Sets the baud rate of the serial port. Note that not all rates are applicable on -all platforms. The following table shows translations of the various baud rate -constants on Windows(including NT/2000) and POSIX platforms. Speeds marked with an * -are speeds that are usable on both Windows and POSIX. -\verbatim + Sets the baud rate of the serial port. Note that not all rates are applicable on + all platforms. The following table shows translations of the various baud rate + constants on Windows(including NT/2000) and POSIX platforms. Speeds marked with an * + are speeds that are usable on both Windows and POSIX. + \verbatim - RATE Windows Speed POSIX Speed - ----------- ------------- ----------- + RATE Windows Speed POSIX Speed + ----------- ------------- ----------- BAUD50 110 50 BAUD75 110 75 - *BAUD110 110 110 + * BAUD110 110 110 BAUD134 110 134.5 BAUD150 110 150 BAUD200 110 200 - *BAUD300 300 300 - *BAUD600 600 600 - *BAUD1200 1200 1200 + * BAUD300 300 300 + * BAUD600 600 600 + * BAUD1200 1200 1200 BAUD1800 1200 1800 - *BAUD2400 2400 2400 - *BAUD4800 4800 4800 - *BAUD9600 9600 9600 + * BAUD2400 2400 2400 + * BAUD4800 4800 4800 + * BAUD9600 9600 9600 BAUD14400 14400 9600 - *BAUD19200 19200 19200 - *BAUD38400 38400 38400 + * BAUD19200 19200 19200 + * BAUD38400 38400 38400 BAUD56000 56000 38400 - *BAUD57600 57600 57600 + * BAUD57600 57600 57600 BAUD76800 57600 76800 - *BAUD115200 115200 115200 + * BAUD115200 115200 115200 BAUD128000 128000 115200 BAUD256000 256000 115200 -\endverbatim -*/ -void QextSerialPort::setBaudRate(BaudRateType baudRate) { + \endverbatim + */ +void QextSerialPort::setBaudRate(BaudRateType baudRate) +{ QMutexLocker lock(mutex); - if (Settings.BaudRate!=baudRate) { + + if (Settings.BaudRate != baudRate) { switch (baudRate) { - case BAUD50: - case BAUD75: - case BAUD134: - case BAUD150: - case BAUD200: - Settings.BaudRate=BAUD110; - break; + case BAUD50: + case BAUD75: + case BAUD134: + case BAUD150: + case BAUD200: + Settings.BaudRate = BAUD110; + break; - case BAUD1800: - Settings.BaudRate=BAUD1200; - break; + case BAUD1800: + Settings.BaudRate = BAUD1200; + break; - case BAUD76800: - Settings.BaudRate=BAUD57600; - break; + case BAUD76800: + Settings.BaudRate = BAUD57600; + break; - default: - Settings.BaudRate=baudRate; - break; + default: + Settings.BaudRate = baudRate; + break; } } if (isOpen()) { switch (baudRate) { + /*50 baud*/ + case BAUD50: + TTY_WARNING("QextSerialPort: Windows does not support 50 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate = CBR_110; + break; - /*50 baud*/ - case BAUD50: - TTY_WARNING("QextSerialPort: Windows does not support 50 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + /*75 baud*/ + case BAUD75: + TTY_WARNING("QextSerialPort: Windows does not support 75 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate = CBR_110; + break; - /*75 baud*/ - case BAUD75: - TTY_WARNING("QextSerialPort: Windows does not support 75 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + /*110 baud*/ + case BAUD110: + Win_CommConfig.dcb.BaudRate = CBR_110; + break; - /*110 baud*/ - case BAUD110: - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + /*134.5 baud*/ + case BAUD134: + TTY_WARNING("QextSerialPort: Windows does not support 134.5 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate = CBR_110; + break; - /*134.5 baud*/ - case BAUD134: - TTY_WARNING("QextSerialPort: Windows does not support 134.5 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + /*150 baud*/ + case BAUD150: + TTY_WARNING("QextSerialPort: Windows does not support 150 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate = CBR_110; + break; - /*150 baud*/ - case BAUD150: - TTY_WARNING("QextSerialPort: Windows does not support 150 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + /*200 baud*/ + case BAUD200: + TTY_WARNING("QextSerialPort: Windows does not support 200 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate = CBR_110; + break; - /*200 baud*/ - case BAUD200: - TTY_WARNING("QextSerialPort: Windows does not support 200 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + /*300 baud*/ + case BAUD300: + Win_CommConfig.dcb.BaudRate = CBR_300; + break; - /*300 baud*/ - case BAUD300: - Win_CommConfig.dcb.BaudRate=CBR_300; - break; + /*600 baud*/ + case BAUD600: + Win_CommConfig.dcb.BaudRate = CBR_600; + break; - /*600 baud*/ - case BAUD600: - Win_CommConfig.dcb.BaudRate=CBR_600; - break; + /*1200 baud*/ + case BAUD1200: + Win_CommConfig.dcb.BaudRate = CBR_1200; + break; - /*1200 baud*/ - case BAUD1200: - Win_CommConfig.dcb.BaudRate=CBR_1200; - break; + /*1800 baud*/ + case BAUD1800: + TTY_WARNING("QextSerialPort: Windows does not support 1800 baud operation. Switching to 1200 baud."); + Win_CommConfig.dcb.BaudRate = CBR_1200; + break; - /*1800 baud*/ - case BAUD1800: - TTY_WARNING("QextSerialPort: Windows does not support 1800 baud operation. Switching to 1200 baud."); - Win_CommConfig.dcb.BaudRate=CBR_1200; - break; + /*2400 baud*/ + case BAUD2400: + Win_CommConfig.dcb.BaudRate = CBR_2400; + break; - /*2400 baud*/ - case BAUD2400: - Win_CommConfig.dcb.BaudRate=CBR_2400; - break; + /*4800 baud*/ + case BAUD4800: + Win_CommConfig.dcb.BaudRate = CBR_4800; + break; - /*4800 baud*/ - case BAUD4800: - Win_CommConfig.dcb.BaudRate=CBR_4800; - break; + /*9600 baud*/ + case BAUD9600: + Win_CommConfig.dcb.BaudRate = CBR_9600; + break; - /*9600 baud*/ - case BAUD9600: - Win_CommConfig.dcb.BaudRate=CBR_9600; - break; + /*14400 baud*/ + case BAUD14400: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 14400 baud operation."); + Win_CommConfig.dcb.BaudRate = CBR_14400; + break; - /*14400 baud*/ - case BAUD14400: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 14400 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_14400; - break; + /*19200 baud*/ + case BAUD19200: + Win_CommConfig.dcb.BaudRate = CBR_19200; + break; - /*19200 baud*/ - case BAUD19200: - Win_CommConfig.dcb.BaudRate=CBR_19200; - break; + /*38400 baud*/ + case BAUD38400: + Win_CommConfig.dcb.BaudRate = CBR_38400; + break; - /*38400 baud*/ - case BAUD38400: - Win_CommConfig.dcb.BaudRate=CBR_38400; - break; + /*56000 baud*/ + case BAUD56000: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 56000 baud operation."); + Win_CommConfig.dcb.BaudRate = CBR_56000; + break; - /*56000 baud*/ - case BAUD56000: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 56000 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_56000; - break; + /*57600 baud*/ + case BAUD57600: + Win_CommConfig.dcb.BaudRate = CBR_57600; + break; - /*57600 baud*/ - case BAUD57600: - Win_CommConfig.dcb.BaudRate=CBR_57600; - break; + /*76800 baud*/ + case BAUD76800: + TTY_WARNING("QextSerialPort: Windows does not support 76800 baud operation. Switching to 57600 baud."); + Win_CommConfig.dcb.BaudRate = CBR_57600; + break; - /*76800 baud*/ - case BAUD76800: - TTY_WARNING("QextSerialPort: Windows does not support 76800 baud operation. Switching to 57600 baud."); - Win_CommConfig.dcb.BaudRate=CBR_57600; - break; + /*115200 baud*/ + case BAUD115200: + Win_CommConfig.dcb.BaudRate = CBR_115200; + break; - /*115200 baud*/ - case BAUD115200: - Win_CommConfig.dcb.BaudRate=CBR_115200; - break; + /*128000 baud*/ + case BAUD128000: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 128000 baud operation."); + Win_CommConfig.dcb.BaudRate = CBR_128000; + break; - /*128000 baud*/ - case BAUD128000: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 128000 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_128000; - break; + /*230400 baud*/ + case BAUD230400: + Win_CommConfig.dcb.BaudRate = CBR_230400; + break; - /*230400 baud*/ - case BAUD230400: - Win_CommConfig.dcb.BaudRate=CBR_230400; - break; + /*256000 baud*/ + case BAUD256000: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 256000 baud operation."); + Win_CommConfig.dcb.BaudRate = CBR_256000; + break; - /*256000 baud*/ - case BAUD256000: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 256000 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_256000; - break; + /*460800 baud*/ + case BAUD460800: + Win_CommConfig.dcb.BaudRate = CBR_460800; + break; - /*460800 baud*/ - case BAUD460800: - Win_CommConfig.dcb.BaudRate=CBR_460800; - break; - - /*921600 baud*/ - case BAUD921600: - Win_CommConfig.dcb.BaudRate=CBR_921600; - break; - } + /*921600 baud*/ + case BAUD921600: + Win_CommConfig.dcb.BaudRate = CBR_921600; + break; + } SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); } } /*! -Sets DTR line to the requested state (high by default). This function will have no effect if -the port associated with the class is not currently open. -*/ -void QextSerialPort::setDtr(bool set) { + Sets DTR line to the requested state (high by default). This function will have no effect if + the port associated with the class is not currently open. + */ +void QextSerialPort::setDtr(bool set) +{ QMutexLocker lock(mutex); + if (isOpen()) { if (set) { EscapeCommFunction(Win_Handle, SETDTR); - } - else { + } else { EscapeCommFunction(Win_Handle, CLRDTR); } } } /*! -Sets RTS line to the requested state (high by default). This function will have no effect if -the port associated with the class is not currently open. -*/ -void QextSerialPort::setRts(bool set) { + Sets RTS line to the requested state (high by default). This function will have no effect if + the port associated with the class is not currently open. + */ +void QextSerialPort::setRts(bool set) +{ QMutexLocker lock(mutex); + if (isOpen()) { if (set) { EscapeCommFunction(Win_Handle, SETRTS); - } - else { + } else { EscapeCommFunction(Win_Handle, CLRRTS); } } } /*! -Returns the line status as stored by the port function. This function will retrieve the states -of the following lines: DCD, CTS, DSR, and RI. On POSIX systems, the following additional lines -can be monitored: DTR, RTS, Secondary TXD, and Secondary RXD. The value returned is an unsigned -long with specific bits indicating which lines are high. The following constants should be used -to examine the states of individual lines: + Returns the line status as stored by the port function. This function will retrieve the states + of the following lines: DCD, CTS, DSR, and RI. On POSIX systems, the following additional lines + can be monitored: DTR, RTS, Secondary TXD, and Secondary RXD. The value returned is an unsigned + long with specific bits indicating which lines are high. The following constants should be used + to examine the states of individual lines: -\verbatim -Mask Line ------- ---- -LS_CTS CTS -LS_DSR DSR -LS_DCD DCD -LS_RI RI -\endverbatim + \verbatim + Mask Line + ------ ---- + LS_CTS CTS + LS_DSR DSR + LS_DCD DCD + LS_RI RI + \endverbatim -This function will return 0 if the port associated with the class is not currently open. -*/ -ulong QextSerialPort::lineStatus(void) { - unsigned long Status=0, Temp=0; + This function will return 0 if the port associated with the class is not currently open. + */ +ulong QextSerialPort::lineStatus(void) +{ + unsigned long Status = 0, Temp = 0; QMutexLocker lock(mutex); + if (isOpen()) { GetCommModemStatus(Win_Handle, &Temp); - if (Temp&MS_CTS_ON) { - Status|=LS_CTS; + if (Temp & MS_CTS_ON) { + Status |= LS_CTS; } - if (Temp&MS_DSR_ON) { - Status|=LS_DSR; + if (Temp & MS_DSR_ON) { + Status |= LS_DSR; } - if (Temp&MS_RING_ON) { - Status|=LS_RI; + if (Temp & MS_RING_ON) { + Status |= LS_RI; } - if (Temp&MS_RLSD_ON) { - Status|=LS_DCD; + if (Temp & MS_RLSD_ON) { + Status |= LS_DCD; } } return Status; @@ -803,41 +811,45 @@ ulong QextSerialPort::lineStatus(void) { bool QextSerialPort::waitForReadyRead(int msecs) { - //@todo implement + // @todo implement return false; } qint64 QextSerialPort::bytesToWrite() const { QReadLocker rl(bytesToWriteLock); + return _bytesToWrite; } /* - Triggered when there's activity on our HANDLE. -*/ + Triggered when there's activity on our HANDLE. + */ void QextSerialPort::onWinEvent(HANDLE h) { QMutexLocker lock(mutex); - if(h == overlap.hEvent) { + + if (h == overlap.hEvent) { if (eventMask & EV_RXCHAR) { - if (sender() != this && bytesAvailable() > 0) + if (sender() != this && bytesAvailable() > 0) { emit readyRead(); + } } if (eventMask & EV_TXEMPTY) { /* - A write completed. Run through the list of OVERLAPPED writes, and if - they completed successfully, take them off the list and delete them. - Otherwise, leave them on there so they can finish. - */ + A write completed. Run through the list of OVERLAPPED writes, and if + they completed successfully, take them off the list and delete them. + Otherwise, leave them on there so they can finish. + */ qint64 totalBytesWritten = 0; - QList overlapsToDelete; - foreach(OVERLAPPED* o, pendingWrites) { + QList overlapsToDelete; + foreach(OVERLAPPED * o, pendingWrites) { DWORD numBytes = 0; - if (GetOverlappedResult(Win_Handle, o, & numBytes, false)) { + + if (GetOverlappedResult(Win_Handle, o, &numBytes, false)) { overlapsToDelete.append(o); totalBytesWritten += numBytes; - } else if( GetLastError() != ERROR_IO_INCOMPLETE ) { + } else if (GetLastError() != ERROR_IO_INCOMPLETE) { overlapsToDelete.append(o); qWarning() << "CommEvent overlapped write error:" << GetLastError(); } @@ -849,32 +861,36 @@ void QextSerialPort::onWinEvent(HANDLE h) _bytesToWrite = 0; } - foreach(OVERLAPPED* o, overlapsToDelete) { + foreach(OVERLAPPED * o, overlapsToDelete) { OVERLAPPED *toDelete = pendingWrites.takeAt(pendingWrites.indexOf(o)); + CloseHandle(toDelete->hEvent); delete toDelete; } } if (eventMask & EV_DSR) { - if (lineStatus() & LS_DSR) + if (lineStatus() & LS_DSR) { emit dsrChanged(true); - else + } else { emit dsrChanged(false); + } } } WaitCommEvent(Win_Handle, &eventMask, &overlap); } /*! -Sets the read and write timeouts for the port to millisec milliseconds. -Setting 0 indicates that timeouts are not used for read nor write operations; -however read() and write() functions will still block. Set -1 to provide -non-blocking behaviour (read() and write() will return immediately). + Sets the read and write timeouts for the port to millisec milliseconds. + Setting 0 indicates that timeouts are not used for read nor write operations; + however read() and write() functions will still block. Set -1 to provide + non-blocking behaviour (read() and write() will return immediately). -\note this function does nothing in event driven mode. -*/ -void QextSerialPort::setTimeout(long millisec) { + \note this function does nothing in event driven mode. + */ +void QextSerialPort::setTimeout(long millisec) +{ QMutexLocker lock(mutex); + Settings.Timeout_Millisec = millisec; if (millisec == -1) { @@ -884,28 +900,29 @@ void QextSerialPort::setTimeout(long millisec) { Win_CommTimeouts.ReadIntervalTimeout = millisec; Win_CommTimeouts.ReadTotalTimeoutConstant = millisec; } - Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; + Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; Win_CommTimeouts.WriteTotalTimeoutMultiplier = millisec; - Win_CommTimeouts.WriteTotalTimeoutConstant = 0; - if (queryMode() != QextSerialPort::EventDriven) + Win_CommTimeouts.WriteTotalTimeoutConstant = 0; + if (queryMode() != QextSerialPort::EventDriven) { SetCommTimeouts(Win_Handle, &Win_CommTimeouts); + } } /*! -emulates the EV_TXEMPTY system event not present on some BT interfaces -*/ + emulates the EV_TXEMPTY system event not present on some BT interfaces + */ void QextSerialPort::triggerTxEmpty() { - if (bytesToWrite()>500) - { + if (bytesToWrite() > 500) { QMutexLocker lock(mutex); qint64 totalBytesWritten = 0; - QList overlapsToDelete; - foreach(OVERLAPPED* o, pendingWrites) { + QList overlapsToDelete; + foreach(OVERLAPPED * o, pendingWrites) { DWORD numBytes = 0; - if (GetOverlappedResult(Win_Handle, o, & numBytes, false)) { + + if (GetOverlappedResult(Win_Handle, o, &numBytes, false)) { overlapsToDelete.append(o); totalBytesWritten += numBytes; - } else if( GetLastError() != ERROR_IO_INCOMPLETE ) { + } else if (GetLastError() != ERROR_IO_INCOMPLETE) { overlapsToDelete.append(o); qWarning() << "CommEvent overlapped write error:" << GetLastError(); } @@ -914,14 +931,14 @@ void QextSerialPort::triggerTxEmpty() if (totalBytesWritten > 0) { QWriteLocker writelocker(bytesToWriteLock); _bytesToWrite = 0; - //qDebug()<<"zeroed bytesToWrite"; + // qDebug()<<"zeroed bytesToWrite"; } - //qDebug()<<"overlapsToDelete"<hEvent); delete toDelete; } } - } diff --git a/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.cpp b/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.cpp index dda8b3cbb..aba3b968d 100644 --- a/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.cpp +++ b/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.cpp @@ -5,39 +5,42 @@ #include -//#define QSPINBOX_QSBDEBUG +// #define QSPINBOX_QSBDEBUG #ifdef QSPINBOX_QSBDEBUG # define QSBDEBUG qDebug #else -# define QSBDEBUG if (false) qDebug +# define QSBDEBUG \ + if (false) \ + qDebug #endif -QScienceSpinBox::QScienceSpinBox(QWidget * parent) +QScienceSpinBox::QScienceSpinBox(QWidget *parent) : QDoubleSpinBox(parent) { - initLocalValues(parent); - setDecimals(8); - QDoubleSpinBox::setDecimals(1000); - - // set Range to maximum possible values - double doubleMax = std::numeric_limits::max(); - setRange(-doubleMax, doubleMax); + initLocalValues(parent); + setDecimals(8); + QDoubleSpinBox::setDecimals(1000); - v = new QDoubleValidator(this); - v->setDecimals(1000); // (standard anyway) - v->setNotation(QDoubleValidator::ScientificNotation); - this->lineEdit()->setValidator(v); + // set Range to maximum possible values + double doubleMax = std::numeric_limits::max(); + setRange(-doubleMax, doubleMax); + + v = new QDoubleValidator(this); + v->setDecimals(1000); // (standard anyway) + v->setNotation(QDoubleValidator::ScientificNotation); + this->lineEdit()->setValidator(v); } void QScienceSpinBox::initLocalValues(QWidget *parent) { const QString str = (parent ? parent->locale() : QLocale()).toString(4567.1); + if (str.size() == 6) { delimiter = str.at(4); - thousand = QChar((ushort)0); + thousand = QChar((ushort)0); } else if (str.size() == 7) { - thousand = str.at(1); + thousand = str.at(1); delimiter = str.at(5); } Q_ASSERT(!delimiter.isNull()); @@ -45,33 +48,34 @@ void QScienceSpinBox::initLocalValues(QWidget *parent) int QScienceSpinBox::decimals() const { - return dispDecimals; + return dispDecimals; } void QScienceSpinBox::setDecimals(int value) { - dispDecimals = value; + dispDecimals = value; } // overwritten virtual function of QAbstractSpinBox void QScienceSpinBox::stepBy(int steps) { - if (steps < 0) - stepDown(); - else - stepUp(); + if (steps < 0) { + stepDown(); + } else { + stepUp(); + } } void QScienceSpinBox::stepDown() { - QSBDEBUG() << "stepDown()"; - setValue(value()/10.0); + QSBDEBUG() << "stepDown()"; + setValue(value() / 10.0); } void QScienceSpinBox::stepUp() { - QSBDEBUG() << "stepUp()"; - setValue(value()*10.0); + QSBDEBUG() << "stepUp()"; + setValue(value() * 10.0); } /*! @@ -79,11 +83,11 @@ void QScienceSpinBox::stepUp() */ QString QScienceSpinBox::textFromValue(double value) const { + // convert to string -> Using exponetial display with internal decimals + QString str = locale().toString(value, 'e', dispDecimals); - // convert to string -> Using exponetial display with internal decimals - QString str = locale().toString(value, 'e', dispDecimals); // remove thousand sign - if (qAbs(value) >= 1000.0) { + if (qAbs(value) >= 1000.0) { str.remove(thousand); } return str; @@ -94,13 +98,15 @@ double QScienceSpinBox::valueFromText(const QString &text) const QString copy = text; int pos = this->lineEdit()->cursorPosition(); QValidator::State state = QValidator::Acceptable; + return validateAndInterpret(copy, pos, state).toDouble(); } // this function is never used...? double QScienceSpinBox::round(double value) const { - const QString strDbl = locale().toString(value, 'g', dispDecimals); + const QString strDbl = locale().toString(value, 'g', dispDecimals); + return locale().toDouble(strDbl); } @@ -108,6 +114,7 @@ double QScienceSpinBox::round(double value) const QValidator::State QScienceSpinBox::validate(QString &text, int &pos) const { QValidator::State state; + validateAndInterpret(text, pos, state); return state; } @@ -122,57 +129,58 @@ void QScienceSpinBox::fixup(QString &input) const bool QScienceSpinBox::isIntermediateValue(const QString &str) const { QSBDEBUG() << "input is" << str << minimum() << maximum(); - qint64 dec = 1; - - for (int i=0; i < decimals(); ++i) + qint64 dec = 1; + + for (int i = 0; i < decimals(); ++i) { dec *= 10; + } const QLatin1Char dot('.'); /*! - * determine minimum possible values on left and right of Decimal-char - */ - // I know QString::number() uses CLocale so I use dot - const QString minstr = QString::number(minimum(), 'f', QDoubleSpinBox::decimals()); + * determine minimum possible values on left and right of Decimal-char + */ + // I know QString::number() uses CLocale so I use dot + const QString minstr = QString::number(minimum(), 'f', QDoubleSpinBox::decimals()); qint64 min_left = minstr.left(minstr.indexOf(dot)).toLongLong(); - qint64 min_right = minstr.mid(minstr.indexOf(dot) + 1).toLongLong(); + qint64 min_right = minstr.mid(minstr.indexOf(dot) + 1).toLongLong(); - const QString maxstr = QString::number(maximum(), 'f', QDoubleSpinBox::decimals()); + const QString maxstr = QString::number(maximum(), 'f', QDoubleSpinBox::decimals()); qint64 max_left = maxstr.left(maxstr.indexOf(dot)).toLongLong(); - qint64 max_right = maxstr.mid(maxstr.indexOf(dot) + 1).toLongLong(); - - /*! - * determine left and right long values (left and right of delimiter) - */ - const int dotindex = str.indexOf(delimiter); - const bool negative = maximum() < 0; - qint64 left = 0, right = 0; - bool doleft = true; + qint64 max_right = maxstr.mid(maxstr.indexOf(dot) + 1).toLongLong(); + + /*! + * determine left and right long values (left and right of delimiter) + */ + const int dotindex = str.indexOf(delimiter); + const bool negative = maximum() < 0; + qint64 left = 0, right = 0; + bool doleft = true; bool doright = true; - // no separator -> everthing in left + // no separator -> everthing in left if (dotindex == -1) { - left = str.toLongLong(); + left = str.toLongLong(); doright = false; } - // separator on left or contains '+' - else if (dotindex == 0 || (dotindex == 1 && str.at(0) == QLatin1Char('+'))) { + // separator on left or contains '+' + else if (dotindex == 0 || (dotindex == 1 && str.at(0) == QLatin1Char('+'))) { // '+' at negative max - if (negative) { + if (negative) { QSBDEBUG() << __FILE__ << __LINE__ << "returns false"; return false; } doleft = false; - right = str.mid(dotindex + 1).toLongLong(); + right = str.mid(dotindex + 1).toLongLong(); } - // contains '-' - else if (dotindex == 1 && str.at(0) == QLatin1Char('-')) { - // '-' at positiv max + // contains '-' + else if (dotindex == 1 && str.at(0) == QLatin1Char('-')) { + // '-' at positiv max if (!negative) { QSBDEBUG() << __FILE__ << __LINE__ << "returns false"; return false; } doleft = false; - right = str.mid(dotindex + 1).toLongLong(); + right = str.mid(dotindex + 1).toLongLong(); } else { left = str.left(dotindex).toLongLong(); if (dotindex == str.size() - 1) { // nothing right of Separator @@ -181,11 +189,10 @@ bool QScienceSpinBox::isIntermediateValue(const QString &str) const right = str.mid(dotindex + 1).toLongLong(); } } - // left > 0, with max < 0 and no '-' - if ((left >= 0 && max_left < 0 && !str.startsWith(QLatin1Char('-'))) - // left > 0, with min > 0 - || (left < 0 && min_left >= 0)) - { + // left > 0, with max < 0 and no '-' + if ((left >= 0 && max_left < 0 && !str.startsWith(QLatin1Char('-'))) + // left > 0, with min > 0 + || (left < 0 && min_left >= 0)) { QSBDEBUG("returns false"); return false; } @@ -221,8 +228,8 @@ bool QScienceSpinBox::isIntermediateValue(const QString &str) const if (match != max_left) { max_right = negative ? 0 : dec; } - qint64 tmpl = negative ? max_right : min_right; - qint64 tmpr = negative ? min_right : max_right; + qint64 tmpl = negative ? max_right : min_right; + qint64 tmpr = negative ? min_right : max_right; const bool ret = isIntermediateValueHelper(right, tmpl, tmpr); QSBDEBUG() << __FILE__ << __LINE__ << "returns" << ret; return ret; @@ -236,19 +243,19 @@ bool QScienceSpinBox::isIntermediateValue(const QString &str) const \internal Multi purpose function that parses input, sets state to the appropriate state and returns the value it will be interpreted as. -*/ + */ // reimplemented function, copied from QDoubleSpinBoxPrivate::validateAndInterpret QVariant QScienceSpinBox::validateAndInterpret( - QString &input, - int &pos, + QString &input, + int &pos, QValidator::State &state) const { - /*! return 'cachedText' if - * input = cachedText, or input Empty + /*! return 'cachedText' if + * input = cachedText, or input Empty */ - static QString cachedText; - static QValidator::State cachedState; + static QString cachedText; + static QValidator::State cachedState; static QVariant cachedValue; if (cachedText == input && !input.isEmpty()) { @@ -260,133 +267,129 @@ QVariant QScienceSpinBox::validateAndInterpret( const double max = maximum(); const double min = minimum(); - // removes prefix & suffix - QString copy = stripped(input, &pos); + // removes prefix & suffix + QString copy = stripped(input, &pos); QSBDEBUG() << "input" << input << "copy" << copy; - int len = copy.size(); + int len = copy.size(); double num = min; - const bool plus = max >= 0; + const bool plus = max >= 0; const bool minus = min <= 0; - // Test possible 'Intermediate' reasons - switch (len) - { - case 0: - // Length 0 is always 'Intermediate', except for min=max - if (max != min) { - state = QValidator::Intermediate; - } else { - state = QValidator::Invalid; - } - goto end; - case 1: - // if only char is '+' or '-' - if (copy.at(0) == delimiter - || (plus && copy.at(0) == QLatin1Char('+')) - || (minus && copy.at(0) == QLatin1Char('-'))) { - state = QValidator::Intermediate; - goto end; - } - break; - case 2: - // if only chars are '+' or '-' followed by Comma seperator (delimiter) - if (copy.at(1) == delimiter - && ((plus && copy.at(0) == QLatin1Char('+')) || (minus && copy.at(0) == QLatin1Char('-')))) { - state = QValidator::Intermediate; - goto end; - } - break; - default: break; + // Test possible 'Intermediate' reasons + switch (len) { + case 0: + // Length 0 is always 'Intermediate', except for min=max + if (max != min) { + state = QValidator::Intermediate; + } else { + state = QValidator::Invalid; + } + goto end; + case 1: + // if only char is '+' or '-' + if (copy.at(0) == delimiter + || (plus && copy.at(0) == QLatin1Char('+')) + || (minus && copy.at(0) == QLatin1Char('-'))) { + state = QValidator::Intermediate; + goto end; + } + break; + case 2: + // if only chars are '+' or '-' followed by Comma seperator (delimiter) + if (copy.at(1) == delimiter + && ((plus && copy.at(0) == QLatin1Char('+')) || (minus && copy.at(0) == QLatin1Char('-')))) { + state = QValidator::Intermediate; + goto end; + } + break; + default: break; } // end switch // First char must not be thousand-char - if (copy.at(0) == thousand) - { - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to Invalid"; + if (copy.at(0) == thousand) { + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to Invalid"; state = QValidator::Invalid; goto end; } - // Test possible 'Invalid' reasons - else if (len > 1) - { + // Test possible 'Invalid' reasons + else if (len > 1) { const int dec = copy.indexOf(delimiter); // position of delimiter // if decimal separator (delimiter) exists - if (dec != -1) { - // not two delimiters after one other (meaning something like ',,') + if (dec != -1) { + // not two delimiters after one other (meaning something like ',,') if (dec + 1 < copy.size() && copy.at(dec + 1) == delimiter && pos == dec + 1) { copy.remove(dec + 1, 1); // typing a delimiter when you are on the delimiter - } // should be treated as typing right arrow - // too many decimal points - if (copy.size() - dec > QDoubleSpinBox::decimals() + 1) { - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to Invalid"; + } // should be treated as typing right arrow + // too many decimal points + if (copy.size() - dec > QDoubleSpinBox::decimals() + 1) { + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to Invalid"; state = QValidator::Invalid; goto end; } - // after decimal separator no thousand char - for (int i=dec + 1; i 1) + } // end if (len > 1) - // block of remaining test before 'end' mark - { + // block of remaining test before 'end' mark + { bool ok = false; - bool notAcceptable = false; + bool notAcceptable = false; - // convert 'copy' to double, and check if that was 'ok' + // convert 'copy' to double, and check if that was 'ok' QLocale loc(locale()); num = loc.toDouble(copy, &ok); QSBDEBUG() << __FILE__ << __LINE__ << loc << copy << num << ok; - - // conversion to double did fail + + // conversion to double did fail if (!ok) { - // maybe thousand char was responsable - if (thousand.isPrint()) - { - // if no thousand sign is possible, then - // something else is responable -> Invalid + // maybe thousand char was responsable + if (thousand.isPrint()) { + // if no thousand sign is possible, then + // something else is responable -> Invalid if (max < 1000 && min > -1000 && copy.contains(thousand)) { state = QValidator::Invalid; - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to Invalid"; + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to Invalid"; goto end; } - // two thousand-chars after one other are not valid + // two thousand-chars after one other are not valid const int len = copy.size(); - for (int i=0; i Invalid + // if conversion still not valid, then reason unknown -> Invalid if (!ok) { state = QValidator::Invalid; - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to Invalid"; + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to Invalid"; goto end; } notAcceptable = true; // -> state = Intermediate } // endif: (thousand.isPrint()) } - // no thousand sign, but still invalid for unknown reason + // no thousand sign, but still invalid for unknown reason if (!ok) { state = QValidator::Invalid; - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to Invalid"; - } - // number valid and within valid range - else if (num >= min && num <= max) { - if (notAcceptable) { - state = QValidator::Intermediate; // conversion to num initially failed - } else { - state = QValidator::Acceptable; - } - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to " + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to Invalid"; + } + // number valid and within valid range + else if (num >= min && num <= max) { + if (notAcceptable) { + state = QValidator::Intermediate; // conversion to num initially failed + } else { + state = QValidator::Acceptable; + } + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to " << (state == QValidator::Intermediate ? "Intermediate" : "Acceptable"); - } - // when max and min is the same the only non-Invalid input is max (or min) - else if (max == min) { + } + // when max and min is the same the only non-Invalid input is max (or min) + else if (max == min) { state = QValidator::Invalid; - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to Invalid"; + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to Invalid"; } else { - // value out of valid range (coves only special cases) + // value out of valid range (coves only special cases) if ((num >= 0 && num > max) || (num < 0 && num < min)) { state = QValidator::Invalid; - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to Invalid"; + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to Invalid"; } else { - // invalid range, further test with 'isIntermediateValue' - if (isIntermediateValue(copy)) { - state = QValidator::Intermediate; - } else { - state = QValidator::Invalid; - } - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to " + // invalid range, further test with 'isIntermediateValue' + if (isIntermediateValue(copy)) { + state = QValidator::Intermediate; + } else { + state = QValidator::Invalid; + } + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to " << (state == QValidator::Intermediate ? "Intermediate" : "Acceptable"); } } @@ -443,15 +446,15 @@ QVariant QScienceSpinBox::validateAndInterpret( end: // if something went wrong, set num to something valid - if (state != QValidator::Acceptable) { + if (state != QValidator::Acceptable) { num = max > 0 ? min : max; } - // save (private) cache values - cachedText = prefix() + copy + suffix(); + // save (private) cache values + cachedText = prefix() + copy + suffix(); cachedState = state; cachedValue = QVariant(num); - // return resulting valid num + // return resulting valid num return QVariant(num); } @@ -459,35 +462,37 @@ end: /*! \internal Strips any prefix/suffix from \a text. -*/ + */ // reimplemented function, copied from QAbstractSpinBoxPrivate::stripped QString QScienceSpinBox::stripped(const QString &t, int *pos) const { QString text = t; - QString prefixtext = prefix(); - QString suffixtext = suffix(); - + QString prefixtext = prefix(); + QString suffixtext = suffix(); + if (specialValueText().size() == 0 || text != specialValueText()) { - int from = 0; - int size = text.size(); + int from = 0; + int size = text.size(); bool changed = false; if (prefixtext.size() && text.startsWith(prefixtext)) { - from += prefixtext.size(); - size -= from; + from += prefixtext.size(); + size -= from; changed = true; } if (suffixtext.size() && text.endsWith(suffixtext)) { - size -= suffixtext.size(); + size -= suffixtext.size(); changed = true; } - if (changed) + if (changed) { text = text.mid(from, size); + } } const int s = text.size(); text = text.trimmed(); - if (pos) + if (pos) { (*pos) -= (s - text.size()); + } return text; } @@ -497,12 +502,13 @@ bool QScienceSpinBox::isIntermediateValueHelper(qint64 num, qint64 min, qint64 m QSBDEBUG("%lld %lld %lld", num, min, max); if (num >= min && num <= max) { - if (match) + if (match) { *match = num; + } QSBDEBUG("returns true 0"); return true; } - qint64 tmp = num; + qint64 tmp = num; int numDigits = 0; int digits[10]; @@ -511,7 +517,7 @@ bool QScienceSpinBox::isIntermediateValueHelper(qint64 num, qint64 min, qint64 m digits[0] = 0; } else { tmp = qAbs(num); - for (int i=0; tmp > 0; ++i) { + for (int i = 0; tmp > 0; ++i) { digits[numDigits++] = tmp % 10; tmp /= 10; } @@ -519,22 +525,24 @@ bool QScienceSpinBox::isIntermediateValueHelper(qint64 num, qint64 min, qint64 m int failures = 0; qint64 number; - for (number=max; number>=min; --number) { + for (number = max; number >= min; --number) { tmp = qAbs(number); - for (int i=0; tmp > 0;) { + for (int i = 0; tmp > 0;) { if (digits[i] == (tmp % 10)) { if (++i == numDigits) { - if (match) + if (match) { *match = number; + } QSBDEBUG("returns true 1"); return true; } } tmp /= 10; } - if (failures++ == 500000) { //upper bound - if (match) + if (failures++ == 500000) { // upper bound + if (match) { *match = num; + } QSBDEBUG("returns true 2"); return true; } diff --git a/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.h b/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.h index faecd0df1..99b849e66 100644 --- a/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.h +++ b/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.h @@ -12,39 +12,37 @@ #include -class QScienceSpinBox : public QDoubleSpinBox -{ -Q_OBJECT +class QScienceSpinBox : public QDoubleSpinBox { + Q_OBJECT public: - QScienceSpinBox(QWidget * parent = 0); + QScienceSpinBox(QWidget *parent = 0); - int decimals() const; - void setDecimals(int value); + int decimals() const; + void setDecimals(int value); - QString textFromValue ( double value ) const; - double valueFromText ( const QString & text ) const; + QString textFromValue(double value) const; + double valueFromText(const QString & text) const; static bool isIntermediateValueHelper(qint64 num, qint64 minimum, qint64 maximum, qint64 *match = 0); private: - int dispDecimals; + int dispDecimals; QChar delimiter, thousand; - QDoubleValidator * v; + QDoubleValidator *v; private: - void initLocalValues(QWidget *parent); + void initLocalValues(QWidget *parent); bool isIntermediateValue(const QString &str) const; QVariant validateAndInterpret(QString &input, int &pos, QValidator::State &state) const; - QValidator::State validate(QString &text, int &pos) const; - void fixup(QString &input) const; - QString stripped(const QString &t, int *pos) const; - double round(double value) const; - void stepBy(int steps); + QValidator::State validate(QString &text, int &pos) const; + void fixup(QString &input) const; + QString stripped(const QString &t, int *pos) const; + double round(double value) const; + void stepBy(int steps); public slots: - void stepDown(); - void stepUp(); - + void stepDown(); + void stepUp(); }; -#endif +#endif // ifndef __QScienceSpinBox_H__ diff --git a/ground/openpilotgcs/src/libs/qtconcurrent/multitask.h b/ground/openpilotgcs/src/libs/qtconcurrent/multitask.h index 18a748c0e..8e59ada83 100644 --- a/ground/openpilotgcs/src/libs/qtconcurrent/multitask.h +++ b/ground/openpilotgcs/src/libs/qtconcurrent/multitask.h @@ -4,25 +4,25 @@ * @file multitask.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,27 +44,24 @@ QT_BEGIN_NAMESPACE namespace QtConcurrent { - -class QTCONCURRENT_EXPORT MultiTaskBase : public QObject, public QRunnable -{ +class QTCONCURRENT_EXPORT MultiTaskBase : public QObject, public QRunnable { Q_OBJECT protected slots: - virtual void cancelSelf() = 0; + virtual void cancelSelf() = 0; virtual void setFinished() = 0; virtual void setProgressRange(int min, int max) = 0; virtual void setProgressValue(int value) = 0; - virtual void setProgressText(QString value) = 0; + virtual void setProgressText(QString value) = 0; }; template -class MultiTask : public MultiTaskBase -{ +class MultiTask : public MultiTaskBase { public: - MultiTask(void (Class::*fn)(QFutureInterface &), const QList &objects) + MultiTask(void(Class::*fn)(QFutureInterface &), const QList &objects) : fn(fn), objects(objects) { - maxProgress = 100*objects.size(); + maxProgress = 100 * objects.size(); } QFuture future() @@ -76,13 +73,14 @@ public: void run() { QThreadPool::globalInstance()->releaseThread(); + futureInterface.setProgressRange(0, maxProgress); - foreach (Class *object, objects) { + foreach(Class * object, objects) { QFutureWatcher *watcher = new QFutureWatcher(); watchers.insert(object, watcher); finished.insert(watcher, false); connect(watcher, SIGNAL(finished()), this, SLOT(setFinished())); - connect(watcher, SIGNAL(progressRangeChanged(int,int)), this, SLOT(setProgressRange(int,int))); + connect(watcher, SIGNAL(progressRangeChanged(int, int)), this, SLOT(setProgressRange(int, int))); connect(watcher, SIGNAL(progressValueChanged(int)), this, SLOT(setProgressValue(int))); connect(watcher, SIGNAL(progressTextChanged(QString)), this, SLOT(setProgressText(QString))); watcher->setFuture(QtConcurrent::run(fn, object)); @@ -101,26 +99,28 @@ public: protected: void cancelSelf() { - foreach (QFutureWatcher *watcher, watchers) - watcher->future().cancel(); + foreach(QFutureWatcher *watcher, watchers) + watcher->future().cancel(); } void setFinished() { updateProgress(); QFutureWatcher *watcher = static_cast *>(sender()); - if (finished.contains(watcher)) + if (finished.contains(watcher)) { finished[watcher] = true; + } bool allFinished = true; const QList finishedValues = finished.values(); - foreach (bool isFinished, finishedValues) { + foreach(bool isFinished, finishedValues) { if (!isFinished) { allFinished = false; break; } } - if (allFinished) + if (allFinished) { loop->quit(); + } } void setProgressRange(int min, int max) @@ -146,12 +146,14 @@ private: { int progressSum = 0; const QList *> watchersValues = watchers.values(); - foreach (QFutureWatcher *watcher, watchersValues) { + + foreach(QFutureWatcher *watcher, watchersValues) { if (watcher->progressMinimum() == watcher->progressMaximum()) { - if (watcher->future().isFinished() && !watcher->future().isCanceled()) + if (watcher->future().isFinished() && !watcher->future().isCanceled()) { progressSum += 100; + } } else { - progressSum += 100*(watcher->progressValue()-watcher->progressMinimum())/(watcher->progressMaximum()-watcher->progressMinimum()); + progressSum += 100 * (watcher->progressValue() - watcher->progressMinimum()) / (watcher->progressMaximum() - watcher->progressMinimum()); } } futureInterface.setProgressValue(progressSum); @@ -161,9 +163,11 @@ private: { QString text; const QList *> watchersValues = watchers.values(); - foreach (QFutureWatcher *watcher, watchersValues) { - if (!watcher->progressText().isEmpty()) + + foreach(QFutureWatcher *watcher, watchersValues) { + if (!watcher->progressText().isEmpty()) { text += watcher->progressText() + "\n"; + } } text = text.trimmed(); futureInterface.setProgressValueAndText(futureInterface.progressValue(), text); @@ -188,7 +192,6 @@ QFuture run(void (Class::*fn)(QFutureInterface &), const QList &o QThreadPool::globalInstance()->start(task, priority); return future; } - } // namespace QtConcurrent QT_END_NAMESPACE diff --git a/ground/openpilotgcs/src/libs/qtconcurrent/qtconcurrent_global.h b/ground/openpilotgcs/src/libs/qtconcurrent/qtconcurrent_global.h index ba8c071f3..f1c048b57 100644 --- a/ground/openpilotgcs/src/libs/qtconcurrent/qtconcurrent_global.h +++ b/ground/openpilotgcs/src/libs/qtconcurrent/qtconcurrent_global.h @@ -4,25 +4,25 @@ * @file qtconcurrent_global.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/libs/qtconcurrent/runextensions.h b/ground/openpilotgcs/src/libs/qtconcurrent/runextensions.h index 96f5c7dab..01c8cd533 100644 --- a/ground/openpilotgcs/src/libs/qtconcurrent/runextensions.h +++ b/ground/openpilotgcs/src/libs/qtconcurrent/runextensions.h @@ -4,25 +4,25 @@ * @file runextensions.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,13 +36,11 @@ QT_BEGIN_NAMESPACE namespace QtConcurrent { - -template -class StoredInterfaceFunctionCall0 : public QRunnable -{ +template +class StoredInterfaceFunctionCall0 : public QRunnable { public: - StoredInterfaceFunctionCall0(void (fn)(QFutureInterface &)) - : fn(fn) { } + StoredInterfaceFunctionCall0(void(fn)(QFutureInterface &)) + : fn(fn) {} QFuture start() { @@ -60,14 +58,12 @@ public: private: QFutureInterface futureInterface; FunctionPointer fn; - }; -template -class StoredInterfaceMemberFunctionCall0 : public QRunnable -{ +template +class StoredInterfaceMemberFunctionCall0 : public QRunnable { public: - StoredInterfaceMemberFunctionCall0(void (Class::*fn)(QFutureInterface &), Class *object) - : fn(fn), object(object) { } + StoredInterfaceMemberFunctionCall0(void(Class::*fn)(QFutureInterface &), Class *object) + : fn(fn), object(object) {} QFuture start() { @@ -86,15 +82,13 @@ private: QFutureInterface futureInterface; FunctionPointer fn; Class *object; - }; -template -class StoredInterfaceFunctionCall1 : public QRunnable -{ +template +class StoredInterfaceFunctionCall1 : public QRunnable { public: - StoredInterfaceFunctionCall1(void (fn)(QFutureInterface &, Arg1), Arg1 arg1) - : fn(fn), arg1(arg1) { } + StoredInterfaceFunctionCall1(void(fn)(QFutureInterface &, Arg1), Arg1 arg1) + : fn(fn), arg1(arg1) {} QFuture start() { @@ -114,12 +108,11 @@ private: FunctionPointer fn; Arg1 arg1; }; -template -class StoredInterfaceMemberFunctionCall1 : public QRunnable -{ +template +class StoredInterfaceMemberFunctionCall1 : public QRunnable { public: - StoredInterfaceMemberFunctionCall1(void (Class::*fn)(QFutureInterface &, Arg1), Class *object, Arg1 arg1) - : fn(fn), object(object), arg1(arg1) { } + StoredInterfaceMemberFunctionCall1(void(Class::*fn)(QFutureInterface &, Arg1), Class *object, Arg1 arg1) + : fn(fn), object(object), arg1(arg1) {} QFuture start() { @@ -141,12 +134,11 @@ private: Arg1 arg1; }; -template -class StoredInterfaceFunctionCall2 : public QRunnable -{ +template +class StoredInterfaceFunctionCall2 : public QRunnable { public: - StoredInterfaceFunctionCall2(void (fn)(QFutureInterface &, Arg1, Arg2), Arg1 arg1, Arg2 arg2) - : fn(fn), arg1(arg1), arg2(arg2) { } + StoredInterfaceFunctionCall2(void(fn)(QFutureInterface &, Arg1, Arg2), Arg1 arg1, Arg2 arg2) + : fn(fn), arg1(arg1), arg2(arg2) {} QFuture start() { @@ -166,12 +158,11 @@ private: FunctionPointer fn; Arg1 arg1; Arg2 arg2; }; -template -class StoredInterfaceMemberFunctionCall2 : public QRunnable -{ +template +class StoredInterfaceMemberFunctionCall2 : public QRunnable { public: - StoredInterfaceMemberFunctionCall2(void (Class::*fn)(QFutureInterface &, Arg1, Arg2), Class *object, Arg1 arg1, Arg2 arg2) - : fn(fn), object(object), arg1(arg1), arg2(arg2) { } + StoredInterfaceMemberFunctionCall2(void(Class::*fn)(QFutureInterface &, Arg1, Arg2), Class *object, Arg1 arg1, Arg2 arg2) + : fn(fn), object(object), arg1(arg1), arg2(arg2) {} QFuture start() { @@ -193,12 +184,11 @@ private: Arg1 arg1; Arg2 arg2; }; -template -class StoredInterfaceFunctionCall3 : public QRunnable -{ +template +class StoredInterfaceFunctionCall3 : public QRunnable { public: - StoredInterfaceFunctionCall3(void (fn)(QFutureInterface &, Arg1, Arg2, Arg3), Arg1 arg1, Arg2 arg2, Arg3 arg3) - : fn(fn), arg1(arg1), arg2(arg2), arg3(arg3) { } + StoredInterfaceFunctionCall3(void(fn)(QFutureInterface &, Arg1, Arg2, Arg3), Arg1 arg1, Arg2 arg2, Arg3 arg3) + : fn(fn), arg1(arg1), arg2(arg2), arg3(arg3) {} QFuture start() { @@ -218,12 +208,11 @@ private: FunctionPointer fn; Arg1 arg1; Arg2 arg2; Arg3 arg3; }; -template -class StoredInterfaceMemberFunctionCall3 : public QRunnable -{ +template +class StoredInterfaceMemberFunctionCall3 : public QRunnable { public: - StoredInterfaceMemberFunctionCall3(void (Class::*fn)(QFutureInterface &, Arg1, Arg2, Arg3), Class *object, Arg1 arg1, Arg2 arg2, Arg3 arg3) - : fn(fn), object(object), arg1(arg1), arg2(arg2), arg3(arg3) { } + StoredInterfaceMemberFunctionCall3(void(Class::*fn)(QFutureInterface &, Arg1, Arg2, Arg3), Class *object, Arg1 arg1, Arg2 arg2, Arg3 arg3) + : fn(fn), object(object), arg1(arg1), arg2(arg2), arg3(arg3) {} QFuture start() { @@ -245,12 +234,11 @@ private: Arg1 arg1; Arg2 arg2; Arg3 arg3; }; -template -class StoredInterfaceFunctionCall4 : public QRunnable -{ +template +class StoredInterfaceFunctionCall4 : public QRunnable { public: - StoredInterfaceFunctionCall4(void (fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4), Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4) - : fn(fn), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4) { } + StoredInterfaceFunctionCall4(void(fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4), Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4) + : fn(fn), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4) {} QFuture start() { @@ -270,12 +258,11 @@ private: FunctionPointer fn; Arg1 arg1; Arg2 arg2; Arg3 arg3; Arg4 arg4; }; -template -class StoredInterfaceMemberFunctionCall4 : public QRunnable -{ +template +class StoredInterfaceMemberFunctionCall4 : public QRunnable { public: - StoredInterfaceMemberFunctionCall4(void (Class::*fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4), Class *object, Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4) - : fn(fn), object(object), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4) { } + StoredInterfaceMemberFunctionCall4(void(Class::*fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4), Class *object, Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4) + : fn(fn), object(object), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4) {} QFuture start() { @@ -297,12 +284,11 @@ private: Arg1 arg1; Arg2 arg2; Arg3 arg3; Arg4 arg4; }; -template -class StoredInterfaceFunctionCall5 : public QRunnable -{ +template +class StoredInterfaceFunctionCall5 : public QRunnable { public: - StoredInterfaceFunctionCall5(void (fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4, Arg5), Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4, Arg5 arg5) - : fn(fn), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4), arg5(arg5) { } + StoredInterfaceFunctionCall5(void(fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4, Arg5), Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4, Arg5 arg5) + : fn(fn), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4), arg5(arg5) {} QFuture start() { @@ -322,12 +308,11 @@ private: FunctionPointer fn; Arg1 arg1; Arg2 arg2; Arg3 arg3; Arg4 arg4; Arg5 arg5; }; -template -class StoredInterfaceMemberFunctionCall5 : public QRunnable -{ +template +class StoredInterfaceMemberFunctionCall5 : public QRunnable { public: - StoredInterfaceMemberFunctionCall5(void (Class::*fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4, Arg5), Class *object, Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4, Arg5 arg5) - : fn(fn), object(object), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4), arg5(arg5) { } + StoredInterfaceMemberFunctionCall5(void(Class::*fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4, Arg5), Class *object, Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4, Arg5 arg5) + : fn(fn), object(object), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4), arg5(arg5) {} QFuture start() { @@ -383,9 +368,8 @@ QFuture run(void (*functionPointer)(QFutureInterface &, Arg1, Arg2, Arg3, template QFuture run(void (Class::*fn)(QFutureInterface &), Class *object) { - return (new StoredInterfaceMemberFunctionCall0 &), Class>(fn, object))->start(); + return (new StoredInterfaceMemberFunctionCall0 &), Class>(fn, object))->start(); } - } // namespace QtConcurrent QT_END_NAMESPACE diff --git a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.cpp b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.cpp index 53a36ccf9..a2fb03ed5 100644 --- a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.cpp +++ b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.cpp @@ -24,166 +24,158 @@ /**********************************************************************/ SDLGamepad::SDLGamepad() { - buttons = -1; - axes = -1; - index = -1; - loop = false; - tick = MIN_RATE; - gamepad = 0; + buttons = -1; + axes = -1; + index = -1; + loop = false; + tick = MIN_RATE; + gamepad = 0; } /**********************************************************************/ SDLGamepad::~SDLGamepad() { - loop = false; + loop = false; - if(gamepad) - SDL_JoystickClose(gamepad); + if (gamepad) { + SDL_JoystickClose(gamepad); + } - SDL_Quit(); + SDL_Quit(); } /**********************************************************************/ bool SDLGamepad::init() { - if(SDL_Init(SDL_INIT_JOYSTICK) < 0) - return false; + if (SDL_Init(SDL_INIT_JOYSTICK) < 0) { + return false; + } - if(SDL_NumJoysticks() > 0) - { - emit gamepads(SDL_NumJoysticks()); + if (SDL_NumJoysticks() > 0) { + emit gamepads(SDL_NumJoysticks()); - if(!setGamepad(0)) - return false; + if (!setGamepad(0)) { + return false; + } - for(qint8 i = 0; i < buttons; i++) - buttonStates.append(0); - } - else - { - return false; - } + for (qint8 i = 0; i < buttons; i++) { + buttonStates.append(0); + } + } else { + return false; + } - loop = true; - return true; + loop = true; + return true; } /**********************************************************************/ void SDLGamepad::run() { - while(loop) - { - updateAxes(); - updateButtons(); - msleep(tick); - } + while (loop) { + updateAxes(); + updateButtons(); + msleep(tick); + } } /**********************************************************************/ bool SDLGamepad::setGamepad(qint16 index) { - if (index != this->index) - { - if(SDL_JoystickOpened(this->index)) - SDL_JoystickClose(gamepad); + if (index != this->index) { + if (SDL_JoystickOpened(this->index)) { + SDL_JoystickClose(gamepad); + } - gamepad = SDL_JoystickOpen(index); + gamepad = SDL_JoystickOpen(index); - if(gamepad) - { - buttons = SDL_JoystickNumButtons(gamepad); - axes = SDL_JoystickNumAxes(gamepad); + if (gamepad) { + buttons = SDL_JoystickNumButtons(gamepad); + axes = SDL_JoystickNumAxes(gamepad); - if (axes >= 4) { - this->index = index; - return true; - } - else - { - buttons = -1; - axes = -1; - this->index = -1; - qCritical("Gamepad has less than 4 axes"); - } + if (axes >= 4) { + this->index = index; + return true; + } else { + buttons = -1; + axes = -1; + this->index = -1; + qCritical("Gamepad has less than 4 axes"); + } + } else { + buttons = -1; + axes = -1; + this->index = -1; + qCritical("Unable to open Gamepad!"); + } } - else - { - buttons = -1; - axes = -1; - this->index = -1; - qCritical("Unable to open Gamepad!"); - } - } - return false; + return false; } /**********************************************************************/ void SDLGamepad::setTickRate(qint16 ms) { - tick = ms; + tick = ms; } /**********************************************************************/ void SDLGamepad::updateAxes() { - if(gamepad) - { - QListInt16 values; - SDL_JoystickUpdate(); + if (gamepad) { + QListInt16 values; + SDL_JoystickUpdate(); - for(qint8 i = 0; i < axes; i++) - { - qint16 value = SDL_JoystickGetAxis(gamepad, i); + for (qint8 i = 0; i < axes; i++) { + qint16 value = SDL_JoystickGetAxis(gamepad, i); - if(value > -NULL_RANGE && value < NULL_RANGE) - value = 0; + if (value > -NULL_RANGE && value < NULL_RANGE) { + value = 0; + } - values.append(value); + values.append(value); + } + + emit axesValues(values); } - - emit axesValues(values); - } } /**********************************************************************/ void SDLGamepad::updateButtons() { - if(gamepad) - { - SDL_JoystickUpdate(); + if (gamepad) { + SDL_JoystickUpdate(); - for(qint8 i = 0; i < buttons; i++) - { - qint16 state = SDL_JoystickGetButton(gamepad, i); + for (qint8 i = 0; i < buttons; i++) { + qint16 state = SDL_JoystickGetButton(gamepad, i); - if(buttonStates.at(i) != state) - { - if(state > 0) - emit buttonState((ButtonNumber)i, true); - else - emit buttonState((ButtonNumber)i, false); + if (buttonStates.at(i) != state) { + if (state > 0) { + emit buttonState((ButtonNumber)i, true); + } else { + emit buttonState((ButtonNumber)i, false); + } - buttonStates.replace(i, state); - } + buttonStates.replace(i, state); + } + } } - } } /**********************************************************************/ void SDLGamepad::quit() { - loop = false; + loop = false; } /**********************************************************************/ qint16 SDLGamepad::getAxes() { - return axes; + return axes; } /**********************************************************************/ qint16 SDLGamepad::getButtons() { - return buttons; + return buttons; } diff --git a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.h b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.h index 033f8ff96..217f9465f 100644 --- a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.h +++ b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.h @@ -49,7 +49,7 @@ * * @see SDLGamepad::setTickRate() */ -#define MIN_RATE 10 +#define MIN_RATE 10 /** * Axis enumeration. @@ -58,17 +58,16 @@ * dealing with axes numbers. Up to 10 axes are supported. That should * be more than enough for every gamepad out there. */ -enum AxisNumber -{ - AXIS_1, - AXIS_2, - AXIS_3, - AXIS_4, - AXIS_5, - AXIS_6, - AXIS_7, - AXIS_8, - AXIS_9 +enum AxisNumber { + AXIS_1, + AXIS_2, + AXIS_3, + AXIS_4, + AXIS_5, + AXIS_6, + AXIS_7, + AXIS_8, + AXIS_9 }; /** @@ -81,28 +80,27 @@ enum AxisNumber * * @see SDLGamepad::buttonState() */ -enum ButtonNumber -{ - BUTTON_01, - BUTTON_02, - BUTTON_03, - BUTTON_04, - BUTTON_05, - BUTTON_06, - BUTTON_07, - BUTTON_08, - BUTTON_09, - BUTTON_10, - BUTTON_11, - BUTTON_12, - BUTTON_13, - BUTTON_14, - BUTTON_15, - BUTTON_16, - BUTTON_17, - BUTTON_18, - BUTTON_19, - BUTTON_20 +enum ButtonNumber { + BUTTON_01, + BUTTON_02, + BUTTON_03, + BUTTON_04, + BUTTON_05, + BUTTON_06, + BUTTON_07, + BUTTON_08, + BUTTON_09, + BUTTON_10, + BUTTON_11, + BUTTON_12, + BUTTON_13, + BUTTON_14, + BUTTON_15, + BUTTON_16, + BUTTON_17, + BUTTON_18, + BUTTON_19, + BUTTON_20 }; /** @@ -129,11 +127,10 @@ typedef QList QListInt16; * @version 1.0 * @date 2009 */ -class SDLGAMEPADSHARED_EXPORT SDLGamepad : public QThread -{ - Q_OBJECT +class SDLGAMEPADSHARED_EXPORT SDLGamepad : public QThread { + Q_OBJECT - public: +public: /** * Class constructor. @@ -182,7 +179,7 @@ class SDLGAMEPADSHARED_EXPORT SDLGamepad : public QThread */ qint16 getButtons(); - public slots: +public slots: /** * Init the SDL system and set up gamepad. @@ -227,7 +224,7 @@ class SDLGAMEPADSHARED_EXPORT SDLGamepad : public QThread */ bool setGamepad(qint16 index); - private: +private: /** * Variable to control thread. @@ -312,7 +309,7 @@ class SDLGAMEPADSHARED_EXPORT SDLGamepad : public QThread */ QList buttonStates; - signals: +signals: /** * A signal that emitts the number of gamepads present in SDL. diff --git a/ground/openpilotgcs/src/libs/utils/abstractprocess.h b/ground/openpilotgcs/src/libs/utils/abstractprocess.h index 68f6a45d1..bfbb7e557 100644 --- a/ground/openpilotgcs/src/libs/utils/abstractprocess.h +++ b/ground/openpilotgcs/src/libs/utils/abstractprocess.h @@ -4,25 +4,25 @@ * @file abstractprocess.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,27 +34,37 @@ #include namespace Utils { - -class QTCREATOR_UTILS_EXPORT AbstractProcess -{ +class QTCREATOR_UTILS_EXPORT AbstractProcess { public: AbstractProcess() {} virtual ~AbstractProcess() {} - QString workingDirectory() const { return m_workingDir; } - void setWorkingDirectory(const QString &dir) { m_workingDir = dir; } + QString workingDirectory() const + { + return m_workingDir; + } + void setWorkingDirectory(const QString &dir) + { + m_workingDir = dir; + } - QStringList environment() const { return m_environment; } - void setEnvironment(const QStringList &env) { m_environment = env; } + QStringList environment() const + { + return m_environment; + } + void setEnvironment(const QStringList &env) + { + m_environment = env; + } virtual bool start(const QString &program, const QStringList &args) = 0; virtual void stop() = 0; virtual bool isRunning() const = 0; virtual qint64 applicationPID() const = 0; - virtual int exitCode() const = 0; + virtual int exitCode() const = 0; -//signals: +// signals: virtual void processError(const QString &error) = 0; #ifdef Q_OS_WIN @@ -71,8 +81,6 @@ private: QString m_workingDir; QStringList m_environment; }; - -} //namespace Utils +} // namespace Utils #endif // ABSTRACTPROCESS_H - diff --git a/ground/openpilotgcs/src/libs/utils/abstractprocess_win.cpp b/ground/openpilotgcs/src/libs/utils/abstractprocess_win.cpp index 861ca061f..ed60465e3 100644 --- a/ground/openpilotgcs/src/libs/utils/abstractprocess_win.cpp +++ b/ground/openpilotgcs/src/libs/utils/abstractprocess_win.cpp @@ -4,25 +4,25 @@ * @file abstractprocess_win.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,22 +30,24 @@ #include -namespace Utils { - +namespace Utils { QStringList AbstractProcess::fixWinEnvironment(const QStringList &env) { QStringList envStrings = env; + // add PATH if necessary (for DLL loading) - if (envStrings.filter(QRegExp(QLatin1String("^PATH="),Qt::CaseInsensitive)).isEmpty()) { + if (envStrings.filter(QRegExp(QLatin1String("^PATH="), Qt::CaseInsensitive)).isEmpty()) { QByteArray path = qgetenv("PATH"); - if (!path.isEmpty()) + if (!path.isEmpty()) { envStrings.prepend(QString(QLatin1String("PATH=%1")).arg(QString::fromLocal8Bit(path))); + } } // add systemroot if needed - if (envStrings.filter(QRegExp(QLatin1String("^SystemRoot="),Qt::CaseInsensitive)).isEmpty()) { + if (envStrings.filter(QRegExp(QLatin1String("^SystemRoot="), Qt::CaseInsensitive)).isEmpty()) { QByteArray systemRoot = qgetenv("SystemRoot"); - if (!systemRoot.isEmpty()) + if (!systemRoot.isEmpty()) { envStrings.prepend(QString(QLatin1String("SystemRoot=%1")).arg(QString::fromLocal8Bit(systemRoot))); + } } return envStrings; } @@ -54,9 +56,10 @@ QString AbstractProcess::createWinCommandline(const QString &program, const QStr { const QChar doubleQuote = QLatin1Char('"'); const QChar blank = QLatin1Char(' '); - const QChar backSlash = QLatin1Char('\\'); + const QChar backSlash = QLatin1Char('\\'); + + QString programName = program; - QString programName = program; if (!programName.startsWith(doubleQuote) && !programName.endsWith(doubleQuote) && programName.contains(blank)) { programName.insert(0, doubleQuote); programName.append(doubleQuote); @@ -64,8 +67,9 @@ QString AbstractProcess::createWinCommandline(const QString &program, const QStr // add the prgram as the first arrg ... it works better programName.replace(QLatin1Char('/'), backSlash); QString cmdLine = programName; - if (args.empty()) + if (args.empty()) { return cmdLine; + } cmdLine += blank; for (int i = 0; i < args.size(); ++i) { @@ -99,8 +103,10 @@ QByteArray AbstractProcess::createWinEnvironment(const QStringList &env) { QByteArray envlist; int pos = 0; - foreach (const QString &tmp, env) { + + foreach(const QString &tmp, env) { const uint tmpSize = sizeof(TCHAR) * (tmp.length() + 1); + envlist.resize(envlist.size() + tmpSize); memcpy(envlist.data() + pos, tmp.utf16(), tmpSize); pos += tmpSize; @@ -110,5 +116,4 @@ QByteArray AbstractProcess::createWinEnvironment(const QStringList &env) envlist[pos++] = 0; return envlist; } - -} //namespace Utils +} // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.cpp b/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.cpp index fd3c85c5c..894cccc0b 100644 --- a/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.cpp +++ b/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.cpp @@ -4,25 +4,25 @@ * @file basevalidatinglineedit.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,17 +33,16 @@ enum { debug = 0 }; namespace Utils { - struct BaseValidatingLineEditPrivate { explicit BaseValidatingLineEditPrivate(const QWidget *w); const QColor m_okTextColor; - QColor m_errorTextColor; + QColor m_errorTextColor; BaseValidatingLineEdit::State m_state; QString m_errorMessage; QString m_initialText; - bool m_firstChange; + bool m_firstChange; }; BaseValidatingLineEditPrivate::BaseValidatingLineEditPrivate(const QWidget *w) : @@ -51,8 +50,7 @@ BaseValidatingLineEditPrivate::BaseValidatingLineEditPrivate(const QWidget *w) : m_errorTextColor(Qt::red), m_state(BaseValidatingLineEdit::Invalid), m_firstChange(true) -{ -} +{} BaseValidatingLineEdit::BaseValidatingLineEdit(QWidget *parent) : QLineEdit(parent), @@ -87,9 +85,9 @@ QColor BaseValidatingLineEdit::errorColor() const return m_bd->m_errorTextColor; } -void BaseValidatingLineEdit::setErrorColor(const QColor &c) +void BaseValidatingLineEdit::setErrorColor(const QColor &c) { - m_bd->m_errorTextColor = c; + m_bd->m_errorTextColor = c; } QColor BaseValidatingLineEdit::textColor(const QWidget *w) @@ -100,6 +98,7 @@ QColor BaseValidatingLineEdit::textColor(const QWidget *w) void BaseValidatingLineEdit::setTextColor(QWidget *w, const QColor &c) { QPalette palette = w->palette(); + palette.setColor(QPalette::Active, QPalette::Text, c); w->setPalette(palette); } @@ -125,11 +124,12 @@ void BaseValidatingLineEdit::slotChanged(const QString &t) // Are we displaying the initial text? const bool isDisplayingInitialText = !m_bd->m_initialText.isEmpty() && t == m_bd->m_initialText; const State newState = isDisplayingInitialText ? - DisplayingInitialText : - (validate(t, &m_bd->m_errorMessage) ? Valid : Invalid); + DisplayingInitialText : + (validate(t, &m_bd->m_errorMessage) ? Valid : Invalid); setToolTip(m_bd->m_errorMessage); - if (debug) - qDebug() << Q_FUNC_INFO << t << "State" << m_bd->m_state << "->" << newState << m_bd->m_errorMessage; + if (debug) { + qDebug() << Q_FUNC_INFO << t << "State" << m_bd->m_state << "->" << newState << m_bd->m_errorMessage; + } // Changed..figure out if valid changed. DisplayingInitialText is not valid, // but should not show error color. Also trigger on the first change. if (newState != m_bd->m_state || m_bd->m_firstChange) { @@ -146,13 +146,13 @@ void BaseValidatingLineEdit::slotChanged(const QString &t) void BaseValidatingLineEdit::slotReturnPressed() { - if (isValid()) + if (isValid()) { emit validReturnPressed(); + } } void BaseValidatingLineEdit::triggerChanged() { slotChanged(text()); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.h b/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.h index 2e66a7b58..9d4cd67b7 100644 --- a/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.h +++ b/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.h @@ -4,25 +4,25 @@ * @file basevalidatinglineedit.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,6 @@ #include namespace Utils { - struct BaseValidatingLineEditPrivate; /** @@ -48,10 +47,8 @@ struct BaseValidatingLineEditPrivate; * "". This results in state 'DisplayingInitialText', which * is not valid, but is not marked red. */ -class QTCREATOR_UTILS_EXPORT BaseValidatingLineEdit : public QLineEdit -{ - Q_OBJECT - Q_DISABLE_COPY(BaseValidatingLineEdit) +class QTCREATOR_UTILS_EXPORT BaseValidatingLineEdit : public QLineEdit { + Q_OBJECT Q_DISABLE_COPY(BaseValidatingLineEdit) Q_PROPERTY(QString initialText READ initialText WRITE setInitialText DESIGNABLE true) Q_PROPERTY(QColor errorColor READ errorColor WRITE setErrorColor DESIGNABLE true) @@ -70,7 +67,7 @@ public: void setInitialText(const QString &); QColor errorColor() const; - void setErrorColor(const QColor &); + void setErrorColor(const QColor &); // Trigger an update (after changing settings) void triggerChanged(); @@ -95,7 +92,6 @@ protected slots: private: BaseValidatingLineEditPrivate *m_bd; }; - } // namespace Utils #endif // BASEVALIDATINGLINEEDIT_H diff --git a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp index 2edef8ddf..6cf8d7643 100644 --- a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp +++ b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp @@ -30,7 +30,7 @@ #define GL_CLAMP_TO_EDGE 0x812F #endif -CachedSvgItem::CachedSvgItem(QGraphicsItem * parent) : +CachedSvgItem::CachedSvgItem(QGraphicsItem *parent) : QGraphicsSvgItem(parent), m_context(0), m_texture(0), @@ -39,7 +39,7 @@ CachedSvgItem::CachedSvgItem(QGraphicsItem * parent) : setCacheMode(NoCache); } -CachedSvgItem::CachedSvgItem(const QString & fileName, QGraphicsItem * parent): +CachedSvgItem::CachedSvgItem(const QString & fileName, QGraphicsItem *parent) : QGraphicsSvgItem(fileName, parent), m_context(0), m_texture(0), @@ -58,46 +58,47 @@ CachedSvgItem::~CachedSvgItem() void CachedSvgItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) { - if (painter->paintEngine()->type() != QPaintEngine::OpenGL && - painter->paintEngine()->type() != QPaintEngine::OpenGL2) { - //Fallback to direct painting + painter->paintEngine()->type() != QPaintEngine::OpenGL2) { + // Fallback to direct painting QGraphicsSvgItem::paint(painter, option, widget); return; } QRectF br = boundingRect(); - QTransform transform = painter->worldTransform(); - qreal sceneScale = transform.map(QLineF(0,0,1,0)).length(); + QTransform transform = painter->worldTransform(); + qreal sceneScale = transform.map(QLineF(0, 0, 1, 0)).length(); bool stencilTestEnabled = glIsEnabled(GL_STENCIL_TEST); bool scissorTestEnabled = glIsEnabled(GL_SCISSOR_TEST); painter->beginNativePainting(); - if (stencilTestEnabled) + if (stencilTestEnabled) { glEnable(GL_STENCIL_TEST); - if (scissorTestEnabled) + } + if (scissorTestEnabled) { glEnable(GL_SCISSOR_TEST); + } bool dirty = false; if (!m_texture) { glGenTextures(1, &m_texture); - m_context = const_cast(QGLContext::currentContext()); + m_context = const_cast(QGLContext::currentContext()); - dirty = true; + dirty = true; } if (!qFuzzyCompare(sceneScale, m_scale)) { m_scale = sceneScale; - dirty = true; + dirty = true; } - int textureWidth = (int(br.width()*m_scale) + 3) & ~3; - int textureHeight = (int(br.height()*m_scale) + 3) & ~3; + int textureWidth = (int(br.width() * m_scale) + 3) & ~3; + int textureHeight = (int(br.height() * m_scale) + 3) & ~3; if (dirty) { - //qDebug() << "re-render image"; + // qDebug() << "re-render image"; QImage img(textureWidth, textureHeight, QImage::Format_ARGB32); { @@ -117,15 +118,15 @@ void CachedSvgItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *opt glBindTexture(GL_TEXTURE_2D, m_texture); glTexImage2D( - GL_TEXTURE_2D, - 0, - GL_RGBA, - textureWidth, - textureHeight, - 0, - GL_RGBA, - GL_UNSIGNED_BYTE, - img.bits()); + GL_TEXTURE_2D, + 0, + GL_RGBA, + textureWidth, + textureHeight, + 0, + GL_RGBA, + GL_UNSIGNED_BYTE, + img.bits()); glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); @@ -143,15 +144,15 @@ void CachedSvgItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *opt glBindTexture(GL_TEXTURE_2D, m_texture); - //texture may be slightly large than svn image, ensure only used area is rendered - qreal tw = br.width()*m_scale/textureWidth; - qreal th = br.height()*m_scale/textureHeight; + // texture may be slightly large than svn image, ensure only used area is rendered + qreal tw = br.width() * m_scale / textureWidth; + qreal th = br.height() * m_scale / textureHeight; glBegin(GL_QUADS); - glTexCoord2d(0, 0 ); glVertex3d(br.left(), br.top(), -1); - glTexCoord2d(tw, 0 ); glVertex3d(br.right(), br.top(), -1); + glTexCoord2d(0, 0); glVertex3d(br.left(), br.top(), -1); + glTexCoord2d(tw, 0); glVertex3d(br.right(), br.top(), -1); glTexCoord2d(tw, th); glVertex3d(br.right(), br.bottom(), -1); - glTexCoord2d(0, th); glVertex3d(br.left(), br.bottom(), -1); + glTexCoord2d(0, th); glVertex3d(br.left(), br.bottom(), -1); glEnd(); glDisable(GL_TEXTURE_2D); diff --git a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h index 747ef391c..de88d5281 100644 --- a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h +++ b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h @@ -32,15 +32,14 @@ class QGLContext; -//Cache Svg item as GL Texture. -//Texture is regenerated each time item is scaled -//but it's reused during rotation, unlike DeviceCoordinateCache mode -class QTCREATOR_UTILS_EXPORT CachedSvgItem: public QGraphicsSvgItem -{ +// Cache Svg item as GL Texture. +// Texture is regenerated each time item is scaled +// but it's reused during rotation, unlike DeviceCoordinateCache mode +class QTCREATOR_UTILS_EXPORT CachedSvgItem : public QGraphicsSvgItem { Q_OBJECT public: - CachedSvgItem(QGraphicsItem * parent = 0); - CachedSvgItem(const QString & fileName, QGraphicsItem * parent = 0); + CachedSvgItem(QGraphicsItem *parent = 0); + CachedSvgItem(const QString & fileName, QGraphicsItem *parent = 0); ~CachedSvgItem(); void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget); @@ -51,4 +50,4 @@ private: qreal m_scale; }; -#endif +#endif // ifndef CACHEDSVGITEM_H diff --git a/ground/openpilotgcs/src/libs/utils/checkablemessagebox.cpp b/ground/openpilotgcs/src/libs/utils/checkablemessagebox.cpp index d176d7ee6..83c344cbc 100644 --- a/ground/openpilotgcs/src/libs/utils/checkablemessagebox.cpp +++ b/ground/openpilotgcs/src/libs/utils/checkablemessagebox.cpp @@ -4,25 +4,25 @@ * @file checkablemessagebox.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,7 +33,6 @@ #include namespace Utils { - struct CheckableMessageBoxPrivate { CheckableMessageBoxPrivate() : clickedButton(0) {} @@ -51,7 +50,7 @@ CheckableMessageBox::CheckableMessageBox(QWidget *parent) : m_d->ui.pixmapLabel->setVisible(false); connect(m_d->ui.buttonBox, SIGNAL(accepted()), this, SLOT(accept())); connect(m_d->ui.buttonBox, SIGNAL(rejected()), this, SLOT(reject())); - connect(m_d->ui.buttonBox, SIGNAL(clicked(QAbstractButton*)), this, SLOT(slotClicked(QAbstractButton*))); + connect(m_d->ui.buttonBox, SIGNAL(clicked(QAbstractButton *)), this, SLOT(slotClicked(QAbstractButton *))); } CheckableMessageBox::~CheckableMessageBox() @@ -71,8 +70,9 @@ QAbstractButton *CheckableMessageBox::clickedButton() const QDialogButtonBox::StandardButton CheckableMessageBox::clickedStandardButton() const { - if (m_d->clickedButton) + if (m_d->clickedButton) { return m_d->ui.buttonBox->standardButton(m_d->clickedButton); + } return QDialogButtonBox::NoButton; } @@ -88,8 +88,9 @@ void CheckableMessageBox::setText(const QString &t) QPixmap CheckableMessageBox::iconPixmap() const { - if (const QPixmap *p = m_d->ui.pixmapLabel->pixmap()) + if (const QPixmap * p = m_d->ui.pixmapLabel->pixmap()) { return QPixmap(*p); + } return QPixmap(); } @@ -131,31 +132,33 @@ void CheckableMessageBox::setStandardButtons(QDialogButtonBox::StandardButtons s QDialogButtonBox::StandardButton CheckableMessageBox::defaultButton() const { - foreach (QAbstractButton *b, m_d->ui.buttonBox->buttons()) - if (QPushButton *pb = qobject_cast(b)) - if (pb->isDefault()) - return m_d->ui.buttonBox->standardButton(pb); + foreach(QAbstractButton * b, m_d->ui.buttonBox->buttons()) + if (QPushButton * pb = qobject_cast(b)) { + if (pb->isDefault()) { + return m_d->ui.buttonBox->standardButton(pb); + } + } return QDialogButtonBox::NoButton; } void CheckableMessageBox::setDefaultButton(QDialogButtonBox::StandardButton s) { - if (QPushButton *b = m_d->ui.buttonBox->button(s)) { + if (QPushButton * b = m_d->ui.buttonBox->button(s)) { b->setDefault(true); b->setFocus(); } } -QDialogButtonBox::StandardButton - CheckableMessageBox::question(QWidget *parent, - const QString &title, - const QString &question, - const QString &checkBoxText, - bool *checkBoxSetting, - QDialogButtonBox::StandardButtons buttons, - QDialogButtonBox::StandardButton defaultButton) +QDialogButtonBox::StandardButton CheckableMessageBox::question(QWidget *parent, + const QString &title, + const QString &question, + const QString &checkBoxText, + bool *checkBoxSetting, + QDialogButtonBox::StandardButtons buttons, + QDialogButtonBox::StandardButton defaultButton) { CheckableMessageBox mb(parent); + mb.setWindowTitle(title); mb.setIconPixmap(QMessageBox::standardIcon(QMessageBox::Question)); mb.setText(question); @@ -172,5 +175,4 @@ QMessageBox::StandardButton CheckableMessageBox::dialogButtonBoxToMessageBoxButt { return static_cast(int(db)); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/checkablemessagebox.h b/ground/openpilotgcs/src/libs/utils/checkablemessagebox.h index f9af481dd..d1565fe80 100644 --- a/ground/openpilotgcs/src/libs/utils/checkablemessagebox.h +++ b/ground/openpilotgcs/src/libs/utils/checkablemessagebox.h @@ -4,25 +4,25 @@ * @file checkablemessagebox.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,17 +36,14 @@ #include namespace Utils { - struct CheckableMessageBoxPrivate; /* A messagebox suitable for questions with a * "Do not ask me again" checkbox. Emulates the QMessageBox API with * static conveniences. */ -class QTCREATOR_UTILS_EXPORT CheckableMessageBox : public QDialog -{ - Q_OBJECT - Q_PROPERTY(QString text READ text WRITE setText) +class QTCREATOR_UTILS_EXPORT CheckableMessageBox : public QDialog { + Q_OBJECT Q_PROPERTY(QString text READ text WRITE setText) Q_PROPERTY(QPixmap iconPixmap READ iconPixmap WRITE setIconPixmap) Q_PROPERTY(bool isChecked READ isChecked WRITE setChecked) Q_PROPERTY(QString checkBoxText READ checkBoxText WRITE setCheckBoxText) @@ -56,14 +53,13 @@ public: explicit CheckableMessageBox(QWidget *parent); virtual ~CheckableMessageBox(); - static QDialogButtonBox::StandardButton - question(QWidget *parent, - const QString &title, - const QString &question, - const QString &checkBoxText, - bool *checkBoxSetting, - QDialogButtonBox::StandardButtons buttons = QDialogButtonBox::Yes|QDialogButtonBox::No, - QDialogButtonBox::StandardButton defaultButton = QDialogButtonBox::No); + static QDialogButtonBox::StandardButton question(QWidget *parent, + const QString &title, + const QString &question, + const QString &checkBoxText, + bool *checkBoxSetting, + QDialogButtonBox::StandardButtons buttons = QDialogButtonBox::Yes | QDialogButtonBox::No, + QDialogButtonBox::StandardButton defaultButton = QDialogButtonBox::No); QString text() const; void setText(const QString &); @@ -74,30 +70,29 @@ public: QString checkBoxText() const; void setCheckBoxText(const QString &); - QDialogButtonBox::StandardButtons standardButtons() const; - void setStandardButtons(QDialogButtonBox::StandardButtons s); + QDialogButtonBox::StandardButtons standardButtons() const; + void setStandardButtons(QDialogButtonBox::StandardButtons s); - QDialogButtonBox::StandardButton defaultButton() const; - void setDefaultButton(QDialogButtonBox::StandardButton s); + QDialogButtonBox::StandardButton defaultButton() const; + void setDefaultButton(QDialogButtonBox::StandardButton s); // see static QMessageBox::standardPixmap() QPixmap iconPixmap() const; - void setIconPixmap (const QPixmap &p); + void setIconPixmap(const QPixmap &p); - // Query the result - QAbstractButton *clickedButton() const; - QDialogButtonBox::StandardButton clickedStandardButton() const; + // Query the result + QAbstractButton *clickedButton() const; + QDialogButtonBox::StandardButton clickedStandardButton() const; - // Conversion convenience - static QMessageBox::StandardButton dialogButtonBoxToMessageBoxButton(QDialogButtonBox::StandardButton); + // Conversion convenience + static QMessageBox::StandardButton dialogButtonBoxToMessageBoxButton(QDialogButtonBox::StandardButton); private slots: - void slotClicked(QAbstractButton *b); + void slotClicked(QAbstractButton *b); private: - CheckableMessageBoxPrivate *m_d; + CheckableMessageBoxPrivate *m_d; }; - } // namespace Utils #endif // CHECKABLEMESSAGEBOX_H diff --git a/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.cpp b/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.cpp index 1f23c9be5..d1a1e7d9d 100644 --- a/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.cpp +++ b/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.cpp @@ -4,25 +4,25 @@ * @file classnamevalidatinglineedit.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,6 @@ #include namespace Utils { - struct ClassNameValidatingLineEditPrivate { ClassNameValidatingLineEditPrivate(); @@ -45,21 +44,20 @@ struct ClassNameValidatingLineEditPrivate { }; // Match something like "Namespace1::Namespace2::ClassName". -ClassNameValidatingLineEditPrivate:: ClassNameValidatingLineEditPrivate() : +ClassNameValidatingLineEditPrivate::ClassNameValidatingLineEditPrivate() : m_nameRegexp(QLatin1String("[a-zA-Z_][a-zA-Z0-9_]*(::[a-zA-Z_][a-zA-Z0-9_]*)*")), m_namespaceDelimiter(QLatin1String("::")), m_namespacesEnabled(false), m_lowerCaseFileName(true) { - QTC_ASSERT(m_nameRegexp.isValid(), return); + QTC_ASSERT(m_nameRegexp.isValid(), return ); } // --------------------- ClassNameValidatingLineEdit ClassNameValidatingLineEdit::ClassNameValidatingLineEdit(QWidget *parent) : Utils::BaseValidatingLineEdit(parent), m_d(new ClassNameValidatingLineEditPrivate) -{ -} +{} ClassNameValidatingLineEdit::~ClassNameValidatingLineEdit() { @@ -79,16 +77,19 @@ void ClassNameValidatingLineEdit::setNamespacesEnabled(bool b) bool ClassNameValidatingLineEdit::validate(const QString &value, QString *errorMessage) const { if (!m_d->m_namespacesEnabled && value.contains(QLatin1Char(':'))) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The class name must not contain namespace delimiters."); + } return false; } else if (value.isEmpty()) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("Please enter a class name."); + } return false; } else if (!m_d->m_nameRegexp.exactMatch(value)) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The class name contains invalid characters."); + } return false; } return true; @@ -97,13 +98,15 @@ bool ClassNameValidatingLineEdit::validate(const QString &value, QString *errorM void ClassNameValidatingLineEdit::slotChanged(const QString &t) { Utils::BaseValidatingLineEdit::slotChanged(t); + if (isValid()) { // Suggest file names, strip namespaces QString fileName = m_d->m_lowerCaseFileName ? t.toLower() : t; if (m_d->m_namespacesEnabled) { const int namespaceIndex = fileName.lastIndexOf(m_d->m_namespaceDelimiter); - if (namespaceIndex != -1) + if (namespaceIndex != -1) { fileName.remove(0, namespaceIndex + m_d->m_namespaceDelimiter.size()); + } } emit updateFileName(fileName); } @@ -114,6 +117,7 @@ QString ClassNameValidatingLineEdit::createClassName(const QString &name) // Remove spaces and convert the adjacent characters to uppercase QString className = name; QRegExp spaceMatcher(QLatin1String(" +(\\w)"), Qt::CaseSensitive, QRegExp::RegExp2); + QTC_ASSERT(spaceMatcher.isValid(), /**/); int pos; while ((pos = spaceMatcher.indexIn(className)) != -1) { @@ -144,5 +148,4 @@ void ClassNameValidatingLineEdit::setLowerCaseFileName(bool v) { m_d->m_lowerCaseFileName = v; } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.h b/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.h index 83890f31e..e656c6079 100644 --- a/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.h +++ b/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.h @@ -4,25 +4,25 @@ * @file classnamevalidatinglineedit.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,15 +33,13 @@ #include "basevalidatinglineedit.h" namespace Utils { - struct ClassNameValidatingLineEditPrivate; /* A Line edit that validates a C++ class name and emits a signal * to derive suggested file names from it. */ class QTCREATOR_UTILS_EXPORT ClassNameValidatingLineEdit - : public Utils::BaseValidatingLineEdit -{ + : public Utils::BaseValidatingLineEdit { Q_DISABLE_COPY(ClassNameValidatingLineEdit) Q_PROPERTY(bool namespacesEnabled READ namespacesEnabled WRITE setNamespacesEnabled DESIGNABLE true) Q_PROPERTY(bool lowerCaseFileName READ lowerCaseFileName WRITE setLowerCaseFileName) @@ -72,7 +70,6 @@ protected: private: ClassNameValidatingLineEditPrivate *m_d; }; - } // namespace Utils #endif // CLASSNAMEVALIDATINGLINEEDIT_H diff --git a/ground/openpilotgcs/src/libs/utils/codegeneration.cpp b/ground/openpilotgcs/src/libs/utils/codegeneration.cpp index a8b93c6f2..db9e640ea 100644 --- a/ground/openpilotgcs/src/libs/utils/codegeneration.cpp +++ b/ground/openpilotgcs/src/libs/utils/codegeneration.cpp @@ -4,25 +4,25 @@ * @file codegeneration.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,20 +33,20 @@ #include namespace Utils { - QTCREATOR_UTILS_EXPORT QString fileNameToCppIdentifier(const QString &s) { QString rc; - const int len = s.size(); - const QChar underscore = QLatin1Char('_'); - const QChar dot = QLatin1Char('.'); + const int len = s.size(); + const QChar underscore = QLatin1Char('_'); + const QChar dot = QLatin1Char('.'); for (int i = 0; i < len; i++) { const QChar c = s.at(i); - if (c == underscore || c.isLetterOrNumber()) + if (c == underscore || c.isLetterOrNumber()) { rc += c; - else if (c == dot) + } else if (c == dot) { rc += underscore; + } } return rc; } @@ -55,6 +55,7 @@ QTCREATOR_UTILS_EXPORT QString headerGuard(const QString &file) { const QFileInfo fi(file); QString rc = fileNameToCppIdentifier(fi.completeBaseName()).toUpper(); + rc += QLatin1Char('_'); rc += fileNameToCppIdentifier(fi.suffix()).toUpper(); return rc; @@ -64,9 +65,10 @@ QTCREATOR_UTILS_EXPORT void writeIncludeFileDirective(const QString &file, bool globalInclude, QTextStream &str) { - const QChar opening = globalInclude ? QLatin1Char('<') : QLatin1Char('"'); - const QChar closing = globalInclude ? QLatin1Char('>') : QLatin1Char('"'); - str << QLatin1String("#include ") << opening << file << closing << QLatin1Char('\n'); + const QChar opening = globalInclude ? QLatin1Char('<') : QLatin1Char('"'); + const QChar closing = globalInclude ? QLatin1Char('>') : QLatin1Char('"'); + + str << QLatin1String("#include ") << opening << file << closing << QLatin1Char('\n'); } QTCREATOR_UTILS_EXPORT @@ -75,6 +77,7 @@ QString writeOpeningNameSpaces(const QStringList &l, const QString &indent, { const int count = l.size(); QString rc; + if (count) { str << '\n'; for (int i = 0; i < count; i++) { @@ -89,13 +92,14 @@ QTCREATOR_UTILS_EXPORT void writeClosingNameSpaces(const QStringList &l, const QString &indent, QTextStream &str) { - if (!l.empty()) + if (!l.empty()) { str << '\n'; + } for (int i = l.size() - 1; i >= 0; i--) { - if (i) + if (i) { str << QString(indent.size() * i, QLatin1Char(' ')); - str << "} // namespace " << l.at(i) << '\n'; + } + str << "} // namespace " << l.at(i) << '\n'; } } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/codegeneration.h b/ground/openpilotgcs/src/libs/utils/codegeneration.h index 80debe585..bba0f8b3a 100644 --- a/ground/openpilotgcs/src/libs/utils/codegeneration.h +++ b/ground/openpilotgcs/src/libs/utils/codegeneration.h @@ -4,25 +4,25 @@ * @file codegeneration.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,7 +38,6 @@ class QStringList; QT_END_NAMESPACE namespace Utils { - // Convert a file name to a Cpp identifier (stripping invalid characters // or replacing them by an underscore). QTCREATOR_UTILS_EXPORT QString fileNameToCppIdentifier(const QString &s); @@ -62,7 +61,6 @@ QTCREATOR_UTILS_EXPORT void writeClosingNameSpaces(const QStringList &namespaces, const QString &indent, QTextStream &str); - } // namespace Utils #endif // CODEGENERATION_H diff --git a/ground/openpilotgcs/src/libs/utils/consoleprocess.cpp b/ground/openpilotgcs/src/libs/utils/consoleprocess.cpp index 596e45977..44bc23fbd 100644 --- a/ground/openpilotgcs/src/libs/utils/consoleprocess.cpp +++ b/ground/openpilotgcs/src/libs/utils/consoleprocess.cpp @@ -4,40 +4,41 @@ * @file consoleprocess.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "consoleprocess.h" namespace Utils { - QString ConsoleProcess::modeOption(Mode m) { switch (m) { - case Debug: + case Debug: return QLatin1String("debug"); - case Suspend: + + case Suspend: return QLatin1String("suspend"); - case Run: + + case Run: break; } return QLatin1String("run"); @@ -50,8 +51,8 @@ QString ConsoleProcess::msgCommChannelFailed(const QString &error) QString ConsoleProcess::msgPromptToClose() { - //! Showed in a terminal which might have - //! a different character set on Windows. + // ! Showed in a terminal which might have + // ! a different character set on Windows. return tr("Press to close this window..."); } @@ -79,5 +80,4 @@ QString ConsoleProcess::msgCannotExecute(const QString & p, const QString &why) { return tr("Cannot execute '%1': %2").arg(p, why); } - } diff --git a/ground/openpilotgcs/src/libs/utils/consoleprocess.h b/ground/openpilotgcs/src/libs/utils/consoleprocess.h index 389a14c38..79322f23e 100644 --- a/ground/openpilotgcs/src/libs/utils/consoleprocess.h +++ b/ground/openpilotgcs/src/libs/utils/consoleprocess.h @@ -4,25 +4,25 @@ * @file consoleprocess.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -51,9 +51,7 @@ class QTemporaryFile; QT_END_NAMESPACE namespace Utils { - -class QTCREATOR_UTILS_EXPORT ConsoleProcess : public QObject, public AbstractProcess -{ +class QTCREATOR_UTILS_EXPORT ConsoleProcess : public QObject, public AbstractProcess { Q_OBJECT public: @@ -64,16 +62,34 @@ public: bool start(const QString &program, const QStringList &args); void stop(); - void setMode(Mode m) { m_mode = m; } - Mode mode() const { return m_mode; } + void setMode(Mode m) + { + m_mode = m; + } + Mode mode() const + { + return m_mode; + } bool isRunning() const; // This reflects the state of the console+stub - qint64 applicationPID() const { return m_appPid; } - int exitCode() const { return m_appCode; } // This will be the signal number if exitStatus == CrashExit - QProcess::ExitStatus exitStatus() const { return m_appStatus; } + qint64 applicationPID() const + { + return m_appPid; + } + int exitCode() const + { + return m_appCode; + } // This will be the signal number if exitStatus == CrashExit + QProcess::ExitStatus exitStatus() const + { + return m_appStatus; + } #ifdef Q_OS_UNIX - void setSettings(QSettings *settings) { m_settings = settings; } + void setSettings(QSettings *settings) + { + m_settings = settings; + } static QString defaultTerminalEmulator(); static QString terminalEmulator(const QSettings *settings); static void setTerminalEmulator(QSettings *settings, const QString &term); @@ -132,9 +148,7 @@ private: QByteArray m_stubServerDir; QSettings *m_settings; #endif - }; +} // namespace Utils -} //namespace Utils - -#endif +#endif // ifndef CONSOLEPROCESS_H diff --git a/ground/openpilotgcs/src/libs/utils/consoleprocess_unix.cpp b/ground/openpilotgcs/src/libs/utils/consoleprocess_unix.cpp index 6a66d6d78..fb7165046 100644 --- a/ground/openpilotgcs/src/libs/utils/consoleprocess_unix.cpp +++ b/ground/openpilotgcs/src/libs/utils/consoleprocess_unix.cpp @@ -4,25 +4,25 @@ * @file consoleprocess_unix.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -64,8 +64,9 @@ ConsoleProcess::~ConsoleProcess() bool ConsoleProcess::start(const QString &program, const QStringList &args) { - if (isRunning()) + if (isRunning()) { return false; + } const QString err = stubServerListen(); if (!err.isEmpty()) { @@ -82,7 +83,7 @@ bool ConsoleProcess::start(const QString &program, const QStringList &args) m_tempFile = 0; return false; } - foreach (const QString &var, environment()) { + foreach(const QString &var, environment()) { m_tempFile->write(var.toLocal8Bit()); m_tempFile->write("", 1); } @@ -92,16 +93,16 @@ bool ConsoleProcess::start(const QString &program, const QStringList &args) QStringList xtermArgs = terminalEmulator(m_settings).split(QLatin1Char(' ')); // FIXME: quoting xtermArgs #ifdef Q_OS_MAC - << (QCoreApplication::applicationDirPath() + QLatin1String("/../Resources/qtcreator_process_stub")) -#else - << (QCoreApplication::applicationDirPath() + QLatin1String("/qtcreator_process_stub")) -#endif - << modeOption(m_mode) - << m_stubServer.fullServerName() - << msgPromptToClose() - << workingDirectory() - << (m_tempFile ? m_tempFile->fileName() : 0) - << program << args; + << (QCoreApplication::applicationDirPath() + QLatin1String("/../Resources/qtcreator_process_stub")) + #else + << (QCoreApplication::applicationDirPath() + QLatin1String("/qtcreator_process_stub")) + #endif + << modeOption(m_mode) + << m_stubServer.fullServerName() + << msgPromptToClose() + << workingDirectory() + << (m_tempFile ? m_tempFile->fileName() : 0) + << program << args; QString xterm = xtermArgs.takeFirst(); m_process.start(xterm, xtermArgs); @@ -119,13 +120,15 @@ bool ConsoleProcess::start(const QString &program, const QStringList &args) void ConsoleProcess::stop() { - if (!isRunning()) + if (!isRunning()) { return; + } stubServerShutdown(); m_appPid = 0; m_process.terminate(); - if (!m_process.waitForFinished(1000)) + if (!m_process.waitForFinished(1000)) { m_process.kill(); + } m_process.waitForFinished(); } @@ -139,21 +142,25 @@ QString ConsoleProcess::stubServerListen() // We need to put the socket in a private directory, as some systems simply do not // check the file permissions of sockets. QString stubFifoDir; + forever { { QTemporaryFile tf; - if (!tf.open()) + if (!tf.open()) { return msgCannotCreateTempFile(tf.errorString()); + } stubFifoDir = QFile::encodeName(tf.fileName()); } // By now the temp file was deleted again m_stubServerDir = QFile::encodeName(stubFifoDir); - if (!::mkdir(m_stubServerDir.constData(), 0700)) + if (!::mkdir(m_stubServerDir.constData(), 0700)) { break; - if (errno != EEXIST) + } + if (errno != EEXIST) { return msgCannotCreateTempDir(stubFifoDir, QString::fromLocal8Bit(strerror(errno))); + } } - const QString stubServer = stubFifoDir + "/stub-socket"; + const QString stubServer = stubFifoDir + "/stub-socket"; if (!m_stubServer.listen(stubServer)) { ::rmdir(m_stubServerDir.constData()); return tr("Cannot create socket '%1': %2").arg(stubServer, m_stubServer.errorString()); @@ -196,17 +203,17 @@ void ConsoleProcess::readStubOutput() delete m_tempFile; m_tempFile = 0; - m_appPid = out.mid(4).toInt(); + m_appPid = out.mid(4).toInt(); emit processStarted(); } else if (out.startsWith("exit ")) { m_appStatus = QProcess::NormalExit; - m_appCode = out.mid(5).toInt(); - m_appPid = 0; + m_appCode = out.mid(5).toInt(); + m_appPid = 0; emit processStopped(); } else if (out.startsWith("crash ")) { m_appStatus = QProcess::CrashExit; - m_appCode = out.mid(6).toInt(); - m_appPid = 0; + m_appCode = out.mid(6).toInt(); + m_appPid = 0; emit processStopped(); } else { emit processError(msgUnexpectedOutput()); @@ -219,15 +226,16 @@ void ConsoleProcess::readStubOutput() void ConsoleProcess::stubExited() { // The stub exit might get noticed before we read the error status. - if (m_stubSocket && m_stubSocket->state() == QLocalSocket::ConnectedState) + if (m_stubSocket && m_stubSocket->state() == QLocalSocket::ConnectedState) { m_stubSocket->waitForDisconnected(); + } stubServerShutdown(); delete m_tempFile; m_tempFile = 0; if (m_appPid) { m_appStatus = QProcess::CrashExit; - m_appCode = -1; - m_appPid = 0; + m_appCode = -1; + m_appPid = 0; emit processStopped(); // Maybe it actually did not, but keep state consistent } emit wrapperStopped(); @@ -236,19 +244,23 @@ void ConsoleProcess::stubExited() QString ConsoleProcess::defaultTerminalEmulator() { // FIXME: enable this once runInTerminal works nicely -#if 0 //def Q_OS_MAC +#if 0 // def Q_OS_MAC return QDir::cleanPath(QCoreApplication::applicationDirPath() + QLatin1String("/../Resources/runInTerminal.command")); + #else return QLatin1String("xterm"); + #endif } QString ConsoleProcess::terminalEmulator(const QSettings *settings) { const QString dflt = defaultTerminalEmulator() + QLatin1String(" -e"); - if (!settings) + + if (!settings) { return dflt; + } return settings->value(QLatin1String("General/TerminalEmulator"), dflt).toString(); } diff --git a/ground/openpilotgcs/src/libs/utils/consoleprocess_win.cpp b/ground/openpilotgcs/src/libs/utils/consoleprocess_win.cpp index 15eec8361..a4a6bfb9d 100644 --- a/ground/openpilotgcs/src/libs/utils/consoleprocess_win.cpp +++ b/ground/openpilotgcs/src/libs/utils/consoleprocess_win.cpp @@ -4,25 +4,25 @@ * @file consoleprocess_win.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -62,8 +62,9 @@ ConsoleProcess::~ConsoleProcess() bool ConsoleProcess::start(const QString &program, const QStringList &args) { - if (isRunning()) + if (isRunning()) { return false; + } const QString err = stubServerListen(); if (!err.isEmpty()) { @@ -83,8 +84,8 @@ bool ConsoleProcess::start(const QString &program, const QStringList &args) QTextStream out(m_tempFile); out.setCodec("UTF-16LE"); out.setGenerateByteOrderMark(false); - foreach (const QString &var, fixWinEnvironment(environment())) - out << var << QChar(0); + foreach(const QString &var, fixWinEnvironment(environment())) + out << var << QChar(0); out << QChar(0); } @@ -96,8 +97,9 @@ bool ConsoleProcess::start(const QString &program, const QStringList &args) ZeroMemory(m_pid, sizeof(PROCESS_INFORMATION)); QString workDir = QDir::toNativeSeparators(workingDirectory()); - if (!workDir.isEmpty() && !workDir.endsWith('\\')) + if (!workDir.isEmpty() && !workDir.endsWith('\\')) { workDir.append('\\'); + } QStringList stubArgs; stubArgs << modeOption(m_mode) @@ -108,9 +110,9 @@ bool ConsoleProcess::start(const QString &program, const QStringList &args) << msgPromptToClose(); const QString cmdLine = createWinCommandline( - QCoreApplication::applicationDirPath() + QLatin1String("/qtcreator_process_stub.exe"), stubArgs); + QCoreApplication::applicationDirPath() + QLatin1String("/qtcreator_process_stub.exe"), stubArgs); - bool success = CreateProcessW(0, (WCHAR*)cmdLine.utf16(), + bool success = CreateProcessW(0, (WCHAR *)cmdLine.utf16(), 0, 0, FALSE, CREATE_NEW_CONSOLE, 0, 0, &si, m_pid); @@ -153,8 +155,9 @@ QString ConsoleProcess::stubServerListen() { if (m_stubServer.listen(QString::fromLatin1("creator-%1-%2") .arg(QCoreApplication::applicationPid()) - .arg(rand()))) + .arg(rand()))) { return QString(); + } return m_stubServer.errorString(); } @@ -162,8 +165,9 @@ void ConsoleProcess::stubServerShutdown() { delete m_stubSocket; m_stubSocket = 0; - if (m_stubServer.isListening()) + if (m_stubServer.isListening()) { m_stubServer.close(); + } } void ConsoleProcess::stubConnectionAvailable() @@ -184,12 +188,12 @@ void ConsoleProcess::readStubOutput() } else if (out.startsWith("pid ")) { // Wil not need it any more delete m_tempFile; - m_tempFile = 0; + m_tempFile = 0; - m_appPid = out.mid(4).toInt(); + m_appPid = out.mid(4).toInt(); m_hInferior = OpenProcess( - SYNCHRONIZE | PROCESS_QUERY_INFORMATION | PROCESS_TERMINATE, - FALSE, m_appPid); + SYNCHRONIZE | PROCESS_QUERY_INFORMATION | PROCESS_TERMINATE, + FALSE, m_appPid); if (m_hInferior == NULL) { emit processError(tr("Cannot obtain a handle to the inferior: %1") .arg(winErrorMessage(GetLastError()))); @@ -213,19 +217,20 @@ void ConsoleProcess::cleanupInferior() inferiorFinishedNotifier = 0; CloseHandle(m_hInferior); m_hInferior = NULL; - m_appPid = 0; + m_appPid = 0; } void ConsoleProcess::inferiorExited() { DWORD chldStatus; - if (!GetExitCodeProcess(m_hInferior, &chldStatus)) + if (!GetExitCodeProcess(m_hInferior, &chldStatus)) { emit processError(tr("Cannot obtain exit status from inferior: %1") .arg(winErrorMessage(GetLastError()))); + } cleanupInferior(); m_appStatus = QProcess::NormalExit; - m_appCode = chldStatus; + m_appCode = chldStatus; emit processStopped(); } @@ -245,16 +250,16 @@ void ConsoleProcess::cleanupStub() void ConsoleProcess::stubExited() { // The stub exit might get noticed before we read the pid for the kill. - if (m_stubSocket && m_stubSocket->state() == QLocalSocket::ConnectedState) + if (m_stubSocket && m_stubSocket->state() == QLocalSocket::ConnectedState) { m_stubSocket->waitForDisconnected(); + } cleanupStub(); if (m_hInferior != NULL) { TerminateProcess(m_hInferior, (unsigned)-1); cleanupInferior(); m_appStatus = QProcess::CrashExit; - m_appCode = -1; + m_appCode = -1; emit processStopped(); } emit wrapperStopped(); } - diff --git a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp index bf5b987e8..4de1f311b 100644 --- a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp +++ b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp @@ -32,138 +32,139 @@ #include #include -#define RAD2DEG (180.0/M_PI) -#define DEG2RAD (M_PI/180.0) +#define RAD2DEG (180.0 / M_PI) +#define DEG2RAD (M_PI / 180.0) namespace Utils { - CoordinateConversions::CoordinateConversions() -{ - -} +{} /** - * Get rotation matrix from ECEF to NED for that LLA - * @param[in] LLA Longitude latitude altitude for this location - * @param[out] Rne[3][3] Rotation matrix - */ -void CoordinateConversions::RneFromLLA(double LLA[3], double Rne[3][3]){ + * Get rotation matrix from ECEF to NED for that LLA + * @param[in] LLA Longitude latitude altitude for this location + * @param[out] Rne[3][3] Rotation matrix + */ +void CoordinateConversions::RneFromLLA(double LLA[3], double Rne[3][3]) +{ float sinLat, sinLon, cosLat, cosLon; - sinLat=(float)sin(DEG2RAD*LLA[0]); - sinLon=(float)sin(DEG2RAD*LLA[1]); - cosLat=(float)cos(DEG2RAD*LLA[0]); - cosLon=(float)cos(DEG2RAD*LLA[1]); + sinLat = (float)sin(DEG2RAD * LLA[0]); + sinLon = (float)sin(DEG2RAD * LLA[1]); + cosLat = (float)cos(DEG2RAD * LLA[0]); + cosLon = (float)cos(DEG2RAD * LLA[1]); - Rne[0][0] = -sinLat*cosLon; Rne[0][1] = -sinLat*sinLon; Rne[0][2] = cosLat; - Rne[1][0] = -sinLon; Rne[1][1] = cosLon; Rne[1][2] = 0; - Rne[2][0] = -cosLat*cosLon; Rne[2][1] = -cosLat*sinLon; Rne[2][2] = -sinLat; + Rne[0][0] = -sinLat * cosLon; Rne[0][1] = -sinLat * sinLon; Rne[0][2] = cosLat; + Rne[1][0] = -sinLon; Rne[1][1] = cosLon; Rne[1][2] = 0; + Rne[2][0] = -cosLat * cosLon; Rne[2][1] = -cosLat * sinLon; Rne[2][2] = -sinLat; } /** - * Convert from LLA coordinates to ECEF coordinates - * @param[in] LLA[3] latitude longitude alititude coordinates in - * @param[out] ECEF[3] location in ECEF coordinates - */ -void CoordinateConversions::LLA2ECEF(double LLA[3], double ECEF[3]){ - const double a = 6378137.0; // Equatorial Radius - const double e = 8.1819190842622e-2; // Eccentricity - double sinLat, sinLon, cosLat, cosLon; - double N; + * Convert from LLA coordinates to ECEF coordinates + * @param[in] LLA[3] latitude longitude alititude coordinates in + * @param[out] ECEF[3] location in ECEF coordinates + */ +void CoordinateConversions::LLA2ECEF(double LLA[3], double ECEF[3]) +{ + const double a = 6378137.0; // Equatorial Radius + const double e = 8.1819190842622e-2; // Eccentricity + double sinLat, sinLon, cosLat, cosLon; + double N; - sinLat=sin(DEG2RAD*LLA[0]); - sinLon=sin(DEG2RAD*LLA[1]); - cosLat=cos(DEG2RAD*LLA[0]); - cosLon=cos(DEG2RAD*LLA[1]); + sinLat = sin(DEG2RAD * LLA[0]); + sinLon = sin(DEG2RAD * LLA[1]); + cosLat = cos(DEG2RAD * LLA[0]); + cosLon = cos(DEG2RAD * LLA[1]); - N = a / sqrt(1.0 - e*e*sinLat*sinLat); //prime vertical radius of curvature + N = a / sqrt(1.0 - e * e * sinLat * sinLat); // prime vertical radius of curvature - ECEF[0] = (N+LLA[2])*cosLat*cosLon; - ECEF[1] = (N+LLA[2])*cosLat*sinLon; - ECEF[2] = ((1-e*e)*N + LLA[2]) * sinLat; + ECEF[0] = (N + LLA[2]) * cosLat * cosLon; + ECEF[1] = (N + LLA[2]) * cosLat * sinLon; + ECEF[2] = ((1 - e * e) * N + LLA[2]) * sinLat; } /** - * Convert from ECEF coordinates to LLA coordinates - * @param[in] ECEF[3] location in ECEF coordinates - * @param[out] LLA[3] latitude longitude alititude coordinates - */ + * Convert from ECEF coordinates to LLA coordinates + * @param[in] ECEF[3] location in ECEF coordinates + * @param[out] LLA[3] latitude longitude alititude coordinates + */ int CoordinateConversions::ECEF2LLA(double ECEF[3], double LLA[3]) { - const double a = 6378137.0; // Equatorial Radius - const double e = 8.1819190842622e-2; // Eccentricity - double x=ECEF[0], y=ECEF[1], z=ECEF[2]; + const double a = 6378137.0; // Equatorial Radius + const double e = 8.1819190842622e-2; // Eccentricity + double x = ECEF[0], y = ECEF[1], z = ECEF[2]; double Lat, N, NplusH, delta, esLat; uint16_t iter; - LLA[1] = RAD2DEG*atan2(y,x); + LLA[1] = RAD2DEG * atan2(y, x); N = a; NplusH = N; - delta = 1; - Lat = 1; - iter=0; + delta = 1; + Lat = 1; + iter = 0; - while (((delta > 1.0e-14)||(delta < -1.0e-14)) && (iter < 100)) - { - delta = Lat - atan(z / (sqrt(x*x + y*y)*(1-(N*e*e/NplusH)))); - Lat = Lat-delta; - esLat = e*sin(Lat); - N = a / sqrt(1 - esLat*esLat); - NplusH = sqrt(x*x + y*y)/cos(Lat); - iter += 1; + while (((delta > 1.0e-14) || (delta < -1.0e-14)) && (iter < 100)) { + delta = Lat - atan(z / (sqrt(x * x + y * y) * (1 - (N * e * e / NplusH)))); + Lat = Lat - delta; + esLat = e * sin(Lat); + N = a / sqrt(1 - esLat * esLat); + NplusH = sqrt(x * x + y * y) / cos(Lat); + iter += 1; } - LLA[0] = RAD2DEG*Lat; + LLA[0] = RAD2DEG * Lat; LLA[2] = NplusH - N; - if (iter==500) return (0); - else return (1); + if (iter == 500) { + return 0; + } else { return 1; } } /** - * Get the current location in Longitude, Latitude Altitude (above WSG-84 ellipsoid) - * @param[in] BaseECEF the ECEF of the home location (in m) - * @param[in] NED the offset from the home location (in m) - * @param[out] position three element double for position in decimal degrees and altitude in meters - * @returns - * @arg 0 success - * @arg -1 for failure - */ + * Get the current location in Longitude, Latitude Altitude (above WSG-84 ellipsoid) + * @param[in] BaseECEF the ECEF of the home location (in m) + * @param[in] NED the offset from the home location (in m) + * @param[out] position three element double for position in decimal degrees and altitude in meters + * @returns + * @arg 0 success + * @arg -1 for failure + */ int CoordinateConversions::NED2LLA_HomeECEF(double BaseECEFm[3], double NED[3], double position[3]) { int i; // stored value is in cm, convert to m double BaseLLA[3]; double ECEF[3]; - double Rne [3][3]; + double Rne[3][3]; // Get LLA address to compute conversion matrix ECEF2LLA(BaseECEFm, BaseLLA); RneFromLLA(BaseLLA, Rne); /* P = ECEF + Rne' * NED */ - for(i = 0; i < 3; i++) - ECEF[i] = BaseECEFm[i] + Rne[0][i]*NED[0] + Rne[1][i]*NED[1] + Rne[2][i]*NED[2]; + for (i = 0; i < 3; i++) { + ECEF[i] = BaseECEFm[i] + Rne[0][i] * NED[0] + Rne[1][i] * NED[1] + Rne[2][i] * NED[2]; + } - ECEF2LLA(ECEF,position); + ECEF2LLA(ECEF, position); return 0; } /** - * Get the current location in Longitude, Latitude, Altitude (above WSG-84 ellipsoid) - * @param[in] homeLLA the latitude, longitude, and altitude of the home location (in [m]) - * @param[in] NED the offset from the home location (in [m]) - * @param[out] position three element double for position in decimal degrees and altitude in meters - * @returns - * @arg 0 success - * @arg -1 for failure - */ + * Get the current location in Longitude, Latitude, Altitude (above WSG-84 ellipsoid) + * @param[in] homeLLA the latitude, longitude, and altitude of the home location (in [m]) + * @param[in] NED the offset from the home location (in [m]) + * @param[out] position three element double for position in decimal degrees and altitude in meters + * @returns + * @arg 0 success + * @arg -1 for failure + */ int CoordinateConversions::NED2LLA_HomeLLA(double homeLLA[3], double NED[3], double position[3]) { double T[3]; - T[0] = homeLLA[2]+6.378137E6f * M_PI / 180.0; - T[1] = cosf(homeLLA[0] * M_PI / 180.0)*(homeLLA[2]+6.378137E6f) * M_PI / 180.0; + + T[0] = homeLLA[2] + 6.378137E6f * M_PI / 180.0; + T[1] = cosf(homeLLA[0] * M_PI / 180.0) * (homeLLA[2] + 6.378137E6f) * M_PI / 180.0; T[2] = -1.0f; position[0] = homeLLA[0] + NED[0] / T[0]; @@ -175,108 +176,105 @@ int CoordinateConversions::NED2LLA_HomeLLA(double homeLLA[3], double NED[3], dou void CoordinateConversions::LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]) { - double ECEF[3]; - float diff[3]; + double ECEF[3]; + float diff[3]; - LLA2ECEF(LLA, ECEF); + LLA2ECEF(LLA, ECEF); - diff[0] = (float)(ECEF[0] - BaseECEF[0]); - diff[1] = (float)(ECEF[1] - BaseECEF[1]); - diff[2] = (float)(ECEF[2] - BaseECEF[2]); + diff[0] = (float)(ECEF[0] - BaseECEF[0]); + diff[1] = (float)(ECEF[1] - BaseECEF[1]); + diff[2] = (float)(ECEF[2] - BaseECEF[2]); - NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; - NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; - NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; + NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; + NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; + NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; } // ****** find roll, pitch, yaw from quaternion ******** void CoordinateConversions::Quaternion2RPY(const float q[4], float rpy[3]) { - float R13, R11, R12, R23, R33; - float q0s = q[0] * q[0]; - float q1s = q[1] * q[1]; - float q2s = q[2] * q[2]; - float q3s = q[3] * q[3]; + float R13, R11, R12, R23, R33; + float q0s = q[0] * q[0]; + float q1s = q[1] * q[1]; + float q2s = q[2] * q[2]; + float q3s = q[3] * q[3]; - R13 = 2 * (q[1] * q[3] - q[0] * q[2]); - R11 = q0s + q1s - q2s - q3s; - R12 = 2 * (q[1] * q[2] + q[0] * q[3]); - R23 = 2 * (q[2] * q[3] + q[0] * q[1]); - R33 = q0s - q1s - q2s + q3s; + R13 = 2 * (q[1] * q[3] - q[0] * q[2]); + R11 = q0s + q1s - q2s - q3s; + R12 = 2 * (q[1] * q[2] + q[0] * q[3]); + R23 = 2 * (q[2] * q[3] + q[0] * q[1]); + R33 = q0s - q1s - q2s + q3s; - rpy[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 - rpy[2] = RAD2DEG * atan2f(R12, R11); - rpy[0] = RAD2DEG * atan2f(R23, R33); + rpy[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 + rpy[2] = RAD2DEG * atan2f(R12, R11); + rpy[0] = RAD2DEG * atan2f(R23, R33); - //TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2 + // TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2 } // ****** find quaternion from roll, pitch, yaw ******** void CoordinateConversions::RPY2Quaternion(const float rpy[3], float q[4]) { - float phi, theta, psi; - float cphi, sphi, ctheta, stheta, cpsi, spsi; + float phi, theta, psi; + float cphi, sphi, ctheta, stheta, cpsi, spsi; - phi = DEG2RAD * rpy[0] / 2; - theta = DEG2RAD * rpy[1] / 2; - psi = DEG2RAD * rpy[2] / 2; - cphi = cosf(phi); - sphi = sinf(phi); - ctheta = cosf(theta); - stheta = sinf(theta); - cpsi = cosf(psi); - spsi = sinf(psi); + phi = DEG2RAD * rpy[0] / 2; + theta = DEG2RAD * rpy[1] / 2; + psi = DEG2RAD * rpy[2] / 2; + cphi = cosf(phi); + sphi = sinf(phi); + ctheta = cosf(theta); + stheta = sinf(theta); + cpsi = cosf(psi); + spsi = sinf(psi); - q[0] = cphi * ctheta * cpsi + sphi * stheta * spsi; - q[1] = sphi * ctheta * cpsi - cphi * stheta * spsi; - q[2] = cphi * stheta * cpsi + sphi * ctheta * spsi; - q[3] = cphi * ctheta * spsi - sphi * stheta * cpsi; + q[0] = cphi * ctheta * cpsi + sphi * stheta * spsi; + q[1] = sphi * ctheta * cpsi - cphi * stheta * spsi; + q[2] = cphi * stheta * cpsi + sphi * ctheta * spsi; + q[3] = cphi * ctheta * spsi - sphi * stheta * cpsi; - if (q[0] < 0) { // q0 always positive for uniqueness - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; - } + if (q[0] < 0) { // q0 always positive for uniqueness + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } } -//** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** void CoordinateConversions::Quaternion2R(const float q[4], float Rbe[3][3]) { + float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3]; - float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3]; - - Rbe[0][0] = q0s + q1s - q2s - q3s; - Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]); - Rbe[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]); - Rbe[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]); - Rbe[1][1] = q0s - q1s + q2s - q3s; - Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]); - Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]); - Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]); - Rbe[2][2] = q0s - q1s - q2s + q3s; + Rbe[0][0] = q0s + q1s - q2s - q3s; + Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]); + Rbe[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]); + Rbe[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]); + Rbe[1][1] = q0s - q1s + q2s - q3s; + Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]); + Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]); + Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]); + Rbe[2][2] = q0s - q1s - q2s + q3s; } -//** Find quaternion vector from a rotation matrix, Rbe, a matrix which rotates a vector from earth frame to body frame ** +// ** Find quaternion vector from a rotation matrix, Rbe, a matrix which rotates a vector from earth frame to body frame ** void CoordinateConversions::R2Quaternion(float const Rbe[3][3], float q[4]) { qreal w, x, y, z; // w always >= 0 - w = sqrt(std::max(0.0, 1.0 + Rbe[0][0] + Rbe[1][1] + Rbe[2][2])) / 2.0; - x = sqrt(std::max(0.0, 1.0 + Rbe[0][0] - Rbe[1][1] - Rbe[2][2])) / 2.0; - y = sqrt(std::max(0.0, 1.0 - Rbe[0][0] + Rbe[1][1] - Rbe[2][2])) / 2.0; - z = sqrt(std::max(0.0, 1.0 - Rbe[0][0] - Rbe[1][1] + Rbe[2][2])) / 2.0; + w = sqrt(std::max(0.0, 1.0 + Rbe[0][0] + Rbe[1][1] + Rbe[2][2])) / 2.0; + x = sqrt(std::max(0.0, 1.0 + Rbe[0][0] - Rbe[1][1] - Rbe[2][2])) / 2.0; + y = sqrt(std::max(0.0, 1.0 - Rbe[0][0] + Rbe[1][1] - Rbe[2][2])) / 2.0; + z = sqrt(std::max(0.0, 1.0 - Rbe[0][0] - Rbe[1][1] + Rbe[2][2])) / 2.0; - x = copysign(x, (Rbe[1][2] - Rbe[2][1])); - y = copysign(y, (Rbe[2][0] - Rbe[0][2])); - z = copysign(z, (Rbe[0][1] - Rbe[1][0])); + x = copysign(x, (Rbe[1][2] - Rbe[2][1])); + y = copysign(y, (Rbe[2][0] - Rbe[0][2])); + z = copysign(z, (Rbe[0][1] - Rbe[1][0])); - q[0]=w; - q[1]=x; - q[2]=y; - q[3]=z; + q[0] = w; + q[1] = x; + q[2] = y; + q[3] = z; } - - } diff --git a/ground/openpilotgcs/src/libs/utils/coordinateconversions.h b/ground/openpilotgcs/src/libs/utils/coordinateconversions.h index 5f515c147..de6ac7568 100644 --- a/ground/openpilotgcs/src/libs/utils/coordinateconversions.h +++ b/ground/openpilotgcs/src/libs/utils/coordinateconversions.h @@ -36,9 +36,7 @@ #include "math.h" namespace Utils { - -class QTCREATOR_UTILS_EXPORT CoordinateConversions -{ +class QTCREATOR_UTILS_EXPORT CoordinateConversions { public: CoordinateConversions(); int NED2LLA_HomeECEF(double BaseECEFcm[3], double NED[3], double position[3]); @@ -52,7 +50,6 @@ public: void Quaternion2R(const float q[4], float Rbe[3][3]); void R2Quaternion(float const Rbe[3][3], float q[4]); }; - } #endif /* COORDINATECONVERSIONS_H */ diff --git a/ground/openpilotgcs/src/libs/utils/detailsbutton.cpp b/ground/openpilotgcs/src/libs/utils/detailsbutton.cpp index 0424e2bb0..edccfcb8f 100644 --- a/ground/openpilotgcs/src/libs/utils/detailsbutton.cpp +++ b/ground/openpilotgcs/src/libs/utils/detailsbutton.cpp @@ -4,25 +4,25 @@ * @file detailsbutton.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/libs/utils/detailsbutton.h b/ground/openpilotgcs/src/libs/utils/detailsbutton.h index c4b5d9bd9..efe36a8d3 100644 --- a/ground/openpilotgcs/src/libs/utils/detailsbutton.h +++ b/ground/openpilotgcs/src/libs/utils/detailsbutton.h @@ -4,25 +4,25 @@ * @file detailsbutton.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,7 +35,6 @@ #include "utils_global.h" namespace Utils { - class QTCREATOR_UTILS_EXPORT DetailsButton #ifdef Q_OS_MAC : public QPushButton @@ -45,7 +44,7 @@ class QTCREATOR_UTILS_EXPORT DetailsButton { Q_OBJECT public: - DetailsButton(QWidget *parent=0); + DetailsButton(QWidget *parent = 0); bool isToggled(); public slots: void onClicked(); diff --git a/ground/openpilotgcs/src/libs/utils/detailswidget.cpp b/ground/openpilotgcs/src/libs/utils/detailswidget.cpp index 0f8a4ad18..2de40e972 100644 --- a/ground/openpilotgcs/src/libs/utils/detailswidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/detailswidget.cpp @@ -4,25 +4,25 @@ * @file detailswidget.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,12 +39,11 @@ using namespace Utils; DetailsWidget::DetailsWidget(QWidget *parent) : QWidget(parent), - m_summaryLabel(new QLabel(this)), - m_detailsButton(new DetailsButton(this)), - m_widget(0), - m_toolWidget(0), - m_grid(new QGridLayout(this)) - + m_summaryLabel(new QLabel(this)), + m_detailsButton(new DetailsButton(this)), + m_widget(0), + m_toolWidget(0), + m_grid(new QGridLayout(this)) { m_grid->setContentsMargins(4, 3, 4, 3); @@ -65,26 +64,25 @@ DetailsWidget::DetailsWidget(QWidget *parent) } DetailsWidget::~DetailsWidget() -{ - -} +{} void DetailsWidget::paintEvent(QPaintEvent *paintEvent) { - //TL--> ___________ <-- TR - // | | - //ML-> ______________| <--MM | <--MR - // | | - //BL-> |_________________________| <-- BR + // TL--> ___________ <-- TR + // | | + // ML-> ______________| <--MM | <--MR + // | | + // BL-> |_________________________| <-- BR QWidget::paintEvent(paintEvent); - if (!m_detailsButton->isToggled()) + if (!m_detailsButton->isToggled()) { return; + } const QRect detailsGeometry = m_detailsButton->geometry(); - const QRect widgetGeometry = m_widget ? m_widget->geometry() : QRect(x(), y() + height(), width(), 0); + const QRect widgetGeometry = m_widget ? m_widget->geometry() : QRect(x(), y() + height(), width(), 0); QPoint tl(detailsGeometry.topLeft()); tl += QPoint(-3, -3); @@ -114,8 +112,10 @@ void DetailsWidget::paintEvent(QPaintEvent *paintEvent) void DetailsWidget::detailsButtonClicked() { bool visible = m_detailsButton->isToggled(); - if (m_widget) + + if (m_widget) { m_widget->setVisible(visible); + } m_dummyWidget->setVisible(visible); fixUpLayout(); } @@ -137,8 +137,9 @@ bool DetailsWidget::expanded() const void DetailsWidget::setExpanded(bool v) { - if (expanded() != v) + if (expanded() != v) { m_detailsButton->animateClick(); + } } QWidget *DetailsWidget::widget() const @@ -148,8 +149,9 @@ QWidget *DetailsWidget::widget() const void DetailsWidget::setWidget(QWidget *widget) { - if (m_widget == widget) + if (m_widget == widget) { return; + } if (m_widget) { m_grid->removeWidget(m_widget); m_widget = 0; @@ -165,8 +167,9 @@ void DetailsWidget::setWidget(QWidget *widget) void DetailsWidget::setToolWidget(QWidget *widget) { - if (m_toolWidget == widget) + if (m_toolWidget == widget) { return; + } if (m_toolWidget) { m_grid->removeWidget(m_toolWidget); m_toolWidget = 0; @@ -184,16 +187,17 @@ QWidget *DetailsWidget::toolWidget() const void DetailsWidget::fixUpLayout() { - if (!m_widget) + if (!m_widget) { return; + } QWidget *parent = m_widget; QStack widgets; - while((parent = parent->parentWidget()) && parent && parent->layout()) { + while ((parent = parent->parentWidget()) && parent && parent->layout()) { widgets.push(parent); parent->layout()->update(); } - while(!widgets.isEmpty()) { + while (!widgets.isEmpty()) { widgets.pop()->layout()->activate(); } } diff --git a/ground/openpilotgcs/src/libs/utils/detailswidget.h b/ground/openpilotgcs/src/libs/utils/detailswidget.h index c5d6345e1..71ea2f23e 100644 --- a/ground/openpilotgcs/src/libs/utils/detailswidget.h +++ b/ground/openpilotgcs/src/libs/utils/detailswidget.h @@ -4,25 +4,25 @@ * @file detailswidget.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,10 +41,8 @@ QT_END_NAMESPACE namespace Utils { class DetailsButton; -class QTCREATOR_UTILS_EXPORT DetailsWidget : public QWidget -{ - Q_OBJECT - Q_PROPERTY(QString summaryText READ summaryText WRITE setSummaryText DESIGNABLE true) +class QTCREATOR_UTILS_EXPORT DetailsWidget : public QWidget { + Q_OBJECT Q_PROPERTY(QString summaryText READ summaryText WRITE setSummaryText DESIGNABLE true) Q_PROPERTY(bool expanded READ expanded WRITE setExpanded DESIGNABLE true) public: DetailsWidget(QWidget *parent = 0); diff --git a/ground/openpilotgcs/src/libs/utils/fancylineedit.cpp b/ground/openpilotgcs/src/libs/utils/fancylineedit.cpp index 83d92fa88..a0c7f40c6 100644 --- a/ground/openpilotgcs/src/libs/utils/fancylineedit.cpp +++ b/ground/openpilotgcs/src/libs/utils/fancylineedit.cpp @@ -4,25 +4,25 @@ * @file fancylineedit.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,7 +39,6 @@ enum { margin = 6 }; namespace Utils { - static inline QString sideToStyleSheetString(FancyLineEdit::Side side) { return side == FancyLineEdit::Left ? QLatin1String("left") : QLatin1String("right"); @@ -50,6 +49,7 @@ static inline QString sideToStyleSheetString(FancyLineEdit::Side side) static QString labelStyleSheet(FancyLineEdit::Side side) { QString rc = QLatin1String("QLabel { margin-"); + rc += sideToStyleSheetString(side); rc += QLatin1String(": "); rc += QString::number(margin); @@ -58,7 +58,7 @@ static QString labelStyleSheet(FancyLineEdit::Side side) } // --------- FancyLineEditPrivate as QObject with label -// event filter +// event filter class FancyLineEditPrivate : public QObject { public: @@ -92,16 +92,17 @@ FancyLineEditPrivate::FancyLineEditPrivate(QLineEdit *parent) : m_useLayoutDirection(false), m_menuTabFocusTrigger(false), m_showingHintText(false) -{ -} +{} bool FancyLineEditPrivate::eventFilter(QObject *obj, QEvent *event) { - if (!m_menu || obj != m_menuLabel) + if (!m_menu || obj != m_menuLabel) { return QObject::eventFilter(obj, event); + } switch (event->type()) { - case QEvent::MouseButtonPress: { + case QEvent::MouseButtonPress: + { const QMouseEvent *me = static_cast(event); m_menu->exec(me->globalPos()); return true; @@ -130,8 +131,7 @@ FancyLineEdit::FancyLineEdit(QWidget *parent) : } FancyLineEdit::~FancyLineEdit() -{ -} +{} // Position the menu label left or right according to size. // Called when switching side and from resizeEvent. @@ -139,11 +139,11 @@ void FancyLineEdit::positionMenuLabel() { switch (side()) { case Left: - m_d->m_menuLabel->setGeometry(0, 0, m_d->m_pixmap.width()+margin, height()); + m_d->m_menuLabel->setGeometry(0, 0, m_d->m_pixmap.width() + margin, height()); break; case Right: m_d->m_menuLabel->setGeometry(width() - m_d->m_pixmap.width() - margin, 0, - m_d->m_pixmap.width()+margin, height()); + m_d->m_pixmap.width() + margin, height()); break; } } @@ -154,12 +154,14 @@ void FancyLineEdit::updateStyleSheet(Side side) // respective side and set color according to whether we are showing the // hint text QString sheet = QLatin1String("QLineEdit{ padding-"); + sheet += sideToStyleSheetString(side); sheet += QLatin1String(": "); sheet += QString::number(m_d->m_pixmap.width() + margin); sheet += QLatin1Char(';'); - if (m_d->m_showingHintText) + if (m_d->m_showingHintText) { sheet += QLatin1String(" color: #BBBBBB;"); + } sheet += QLatin1Char('}'); setStyleSheet(sheet); } @@ -190,9 +192,10 @@ void FancyLineEdit::setSide(Side side) FancyLineEdit::Side FancyLineEdit::side() const { - if (m_d->m_useLayoutDirection) + if (m_d->m_useLayoutDirection) { return qApp->layoutDirection() == Qt::LeftToRight ? Left : Right; - return m_d->m_side; + } + return m_d->m_side; } void FancyLineEdit::resizeEvent(QResizeEvent *) @@ -213,12 +216,12 @@ QPixmap FancyLineEdit::pixmap() const void FancyLineEdit::setMenu(QMenu *menu) { - m_d->m_menu = menu; + m_d->m_menu = menu; } QMenu *FancyLineEdit::menu() const { - return m_d->m_menu; + return m_d->m_menu; } bool FancyLineEdit::useLayoutDirection() const @@ -243,8 +246,9 @@ bool FancyLineEdit::hasMenuTabFocusTrigger() const void FancyLineEdit::setMenuTabFocusTrigger(bool v) { - if (m_d->m_menuTabFocusTrigger == v) + if (m_d->m_menuTabFocusTrigger == v) { return; + } m_d->m_menuTabFocusTrigger = v; m_d->m_menuLabel->setFocusPolicy(v ? Qt::TabFocus : Qt::NoFocus); @@ -258,12 +262,14 @@ QString FancyLineEdit::hintText() const void FancyLineEdit::setHintText(const QString &ht) { // Updating magic to make the property work in Designer. - if (ht == m_d->m_hintText) + if (ht == m_d->m_hintText) { return; + } hideHintText(); m_d->m_hintText = ht; - if (!hasFocus() && !ht.isEmpty()) + if (!hasFocus() && !ht.isEmpty()) { showHintText(); + } } void FancyLineEdit::showHintText() @@ -307,5 +313,4 @@ QString FancyLineEdit::typedText() const { return m_d->m_showingHintText ? QString() : text(); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/fancylineedit.h b/ground/openpilotgcs/src/libs/utils/fancylineedit.h index f25c8507e..09eceabe7 100644 --- a/ground/openpilotgcs/src/libs/utils/fancylineedit.h +++ b/ground/openpilotgcs/src/libs/utils/fancylineedit.h @@ -4,25 +4,25 @@ * @file fancylineedit.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,6 @@ #include namespace Utils { - class FancyLineEditPrivate; /* A line edit with an embedded pixmap on one side that is connected to @@ -44,19 +43,17 @@ class FancyLineEditPrivate; * text if isShowingHintText() returns true (that is, does not contain * valid user input). */ -class QTCREATOR_UTILS_EXPORT FancyLineEdit : public QLineEdit -{ +class QTCREATOR_UTILS_EXPORT FancyLineEdit : public QLineEdit { Q_DISABLE_COPY(FancyLineEdit) - Q_OBJECT - Q_ENUMS(Side) + Q_OBJECT Q_ENUMS(Side) Q_PROPERTY(QPixmap pixmap READ pixmap WRITE setPixmap DESIGNABLE true) Q_PROPERTY(Side side READ side WRITE setSide DESIGNABLE isSideStored STORED isSideStored) Q_PROPERTY(bool useLayoutDirection READ useLayoutDirection WRITE setUseLayoutDirection DESIGNABLE true) - Q_PROPERTY(bool menuTabFocusTrigger READ hasMenuTabFocusTrigger WRITE setMenuTabFocusTrigger DESIGNABLE true) + Q_PROPERTY(bool menuTabFocusTrigger READ hasMenuTabFocusTrigger WRITE setMenuTabFocusTrigger DESIGNABLE true) Q_PROPERTY(QString hintText READ hintText WRITE setHintText DESIGNABLE true) public: - enum Side {Left, Right}; + enum Side { Left, Right }; explicit FancyLineEdit(QWidget *parent = 0); ~FancyLineEdit(); @@ -103,7 +100,6 @@ private: FancyLineEditPrivate *m_d; }; - } // namespace Utils #endif // FANCYLINEEDIT_H diff --git a/ground/openpilotgcs/src/libs/utils/fancymainwindow.cpp b/ground/openpilotgcs/src/libs/utils/fancymainwindow.cpp index 870904334..ea03c92c8 100644 --- a/ground/openpilotgcs/src/libs/utils/fancymainwindow.cpp +++ b/ground/openpilotgcs/src/libs/utils/fancymainwindow.cpp @@ -4,25 +4,25 @@ * @file fancymainwindow.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,16 +40,16 @@ FancyMainWindow::FancyMainWindow(QWidget *parent) : QMainWindow(parent), m_locked(true), m_handleDockVisibilityChanges(true) -{ -} +{} QDockWidget *FancyMainWindow::addDockForWidget(QWidget *widget) { QDockWidget *dockWidget = new QDockWidget(widget->windowTitle(), this); + dockWidget->setObjectName(widget->windowTitle()); dockWidget->setWidget(widget); connect(dockWidget->toggleViewAction(), SIGNAL(triggered()), - this, SLOT(onDockActionTriggered()), Qt::QueuedConnection); + this, SLOT(onDockActionTriggered()), Qt::QueuedConnection); connect(dockWidget, SIGNAL(visibilityChanged(bool)), this, SLOT(onDockVisibilityChange(bool))); connect(dockWidget, SIGNAL(topLevelChanged(bool)), @@ -63,12 +63,13 @@ QDockWidget *FancyMainWindow::addDockForWidget(QWidget *widget) void FancyMainWindow::updateDockWidget(QDockWidget *dockWidget) { const QDockWidget::DockWidgetFeatures features = - (m_locked) ? QDockWidget::DockWidgetClosable | QDockWidget::DockWidgetFloatable - : QDockWidget::DockWidgetMovable | QDockWidget::DockWidgetClosable | QDockWidget::DockWidgetFloatable; + (m_locked) ? QDockWidget::DockWidgetClosable | QDockWidget::DockWidgetFloatable + : QDockWidget::DockWidgetMovable | QDockWidget::DockWidgetClosable | QDockWidget::DockWidgetFloatable; QWidget *titleBarWidget = dockWidget->titleBarWidget(); - if (m_locked && !titleBarWidget && !dockWidget->isFloating()) + + if (m_locked && !titleBarWidget && !dockWidget->isFloating()) { titleBarWidget = new QWidget(dockWidget); - else if ((!m_locked || dockWidget->isFloating()) && titleBarWidget) { + } else if ((!m_locked || dockWidget->isFloating()) && titleBarWidget) { delete titleBarWidget; titleBarWidget = 0; } @@ -79,16 +80,19 @@ void FancyMainWindow::updateDockWidget(QDockWidget *dockWidget) void FancyMainWindow::onDockActionTriggered() { QDockWidget *dw = qobject_cast(sender()->parent()); + if (dw) { - if (dw->isVisible()) + if (dw->isVisible()) { dw->raise(); + } } } void FancyMainWindow::onDockVisibilityChange(bool visible) { - if (!m_handleDockVisibilityChanges) + if (!m_handleDockVisibilityChanges) { return; + } QDockWidget *dockWidget = qobject_cast(sender()); int index = m_dockWidgets.indexOf(dockWidget); m_dockWidgetActiveState[index] = visible; @@ -96,15 +100,16 @@ void FancyMainWindow::onDockVisibilityChange(bool visible) void FancyMainWindow::onTopLevelChanged() { - updateDockWidget(qobject_cast(sender())); + updateDockWidget(qobject_cast(sender())); } void FancyMainWindow::setTrackingEnabled(bool enabled) { if (enabled) { m_handleDockVisibilityChanges = true; - for (int i = 0; i < m_dockWidgets.size(); ++i) + for (int i = 0; i < m_dockWidgets.size(); ++i) { m_dockWidgetActiveState[i] = m_dockWidgets[i]->isVisible(); + } } else { m_handleDockVisibilityChanges = false; } @@ -113,7 +118,7 @@ void FancyMainWindow::setTrackingEnabled(bool enabled) void FancyMainWindow::setLocked(bool locked) { m_locked = locked; - foreach (QDockWidget *dockWidget, m_dockWidgets) { + foreach(QDockWidget * dockWidget, m_dockWidgets) { updateDockWidget(dockWidget); } } @@ -139,8 +144,9 @@ void FancyMainWindow::handleVisibilityChanged(bool visible) dockWidget->setVisible(visible && m_dockWidgetActiveState.at(i)); } } - if (visible) + if (visible) { m_handleDockVisibilityChanges = true; + } } void FancyMainWindow::saveSettings(QSettings *settings) const @@ -156,7 +162,7 @@ void FancyMainWindow::saveSettings(QSettings *settings) const void FancyMainWindow::restoreSettings(QSettings *settings) { QHash hash; - foreach (const QString &key, settings->childKeys()) { + foreach(const QString &key, settings->childKeys()) { hash.insert(key, settings->value(key)); } restoreSettings(hash); @@ -165,11 +171,11 @@ void FancyMainWindow::restoreSettings(QSettings *settings) QHash FancyMainWindow::saveSettings() const { QHash settings; - settings["State"] = saveState(); + settings["State"] = saveState(); settings["Locked"] = m_locked; for (int i = 0; i < m_dockWidgetActiveState.count(); ++i) { settings[m_dockWidgets.at(i)->objectName()] = - m_dockWidgetActiveState.at(i); + m_dockWidgetActiveState.at(i); } return settings; } @@ -177,8 +183,10 @@ QHash FancyMainWindow::saveSettings() const void FancyMainWindow::restoreSettings(const QHash &settings) { QByteArray ba = settings.value("State", QByteArray()).toByteArray(); - if (!ba.isEmpty()) + + if (!ba.isEmpty()) { restoreState(ba); + } m_locked = settings.value("Locked", true).toBool(); for (int i = 0; i < m_dockWidgetActiveState.count(); ++i) { m_dockWidgetActiveState[i] = settings.value(m_dockWidgets.at(i)->objectName(), false).toBool(); diff --git a/ground/openpilotgcs/src/libs/utils/fancymainwindow.h b/ground/openpilotgcs/src/libs/utils/fancymainwindow.h index fb8e03e33..0d2abafeb 100644 --- a/ground/openpilotgcs/src/libs/utils/fancymainwindow.h +++ b/ground/openpilotgcs/src/libs/utils/fancymainwindow.h @@ -4,25 +4,25 @@ * @file fancymainwindow.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,19 +41,23 @@ class QSettings; QT_END_NAMESPACE namespace Utils { - -class QTCREATOR_UTILS_EXPORT FancyMainWindow : public QMainWindow -{ +class QTCREATOR_UTILS_EXPORT FancyMainWindow : public QMainWindow { Q_OBJECT public: FancyMainWindow(QWidget *parent = 0); QDockWidget *addDockForWidget(QWidget *widget); - QList dockWidgets() const { return m_dockWidgets; } + QList dockWidgets() const + { + return m_dockWidgets; + } void setTrackingEnabled(bool enabled); - bool isLocked() const { return m_locked; } + bool isLocked() const + { + return m_locked; + } void saveSettings(QSettings *settings) const; void restoreSettings(QSettings *settings); @@ -79,9 +83,8 @@ private: QList m_dockWidgets; QList m_dockWidgetActiveState; bool m_locked; - bool m_handleDockVisibilityChanges; //todo + bool m_handleDockVisibilityChanges; // todo }; - } // namespace Utils #endif // FANCYMAINWINDOW_H diff --git a/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.cpp b/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.cpp index 2111f0afe..3f53447f4 100644 --- a/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.cpp +++ b/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.cpp @@ -4,25 +4,25 @@ * @file filenamevalidatinglineedit.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,16 +33,16 @@ #include namespace Utils { - #define WINDOWS_DEVICES "CON|AUX|PRN|COM1|COM2|LPT1|LPT2|NUL" // Naming a file like a device name will break on Windows, even if it is // "com1.txt". Since we are cross-platform, we generally disallow such file -// names. +// names. static const QRegExp &windowsDeviceNoSubDirPattern() { static const QRegExp rc(QLatin1String(WINDOWS_DEVICES), - Qt::CaseInsensitive); + Qt::CaseInsensitive); + QTC_ASSERT(rc.isValid(), return rc); return rc; } @@ -50,6 +50,7 @@ static const QRegExp &windowsDeviceNoSubDirPattern() static const QRegExp &windowsDeviceSubDirPattern() { static const QRegExp rc(QLatin1String(".*[/\\\\](" WINDOWS_DEVICES ")"), Qt::CaseInsensitive); + QTC_ASSERT(rc.isValid(), return rc); return rc; } @@ -59,8 +60,7 @@ FileNameValidatingLineEdit::FileNameValidatingLineEdit(QWidget *parent) : BaseValidatingLineEdit(parent), m_allowDirectories(false), m_unused(0) -{ -} +{} bool FileNameValidatingLineEdit::allowDirectories() const { @@ -83,51 +83,56 @@ void FileNameValidatingLineEdit::setAllowDirectories(bool v) static const char *notAllowedCharsSubDir = "?:&*\"|#%<> "; static const char *notAllowedCharsNoSubDir = "?:&*\"|#%<> "SLASHES; -static const char *notAllowedSubStrings[] = {".."}; +static const char *notAllowedSubStrings[] = { ".." }; bool FileNameValidatingLineEdit::validateFileName(const QString &name, bool allowDirectories, QString *errorMessage /* = 0*/) { if (name.isEmpty()) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The name must not be empty"); + } return false; } // Characters const char *notAllowedChars = allowDirectories ? notAllowedCharsSubDir : notAllowedCharsNoSubDir; - for (const char *c = notAllowedChars; *c; c++) + for (const char *c = notAllowedChars; *c; c++) { if (name.contains(QLatin1Char(*c))) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The name must not contain any of the characters '%1'.").arg(QLatin1String(notAllowedChars)); + } return false; } + } // Substrings - const int notAllowedSubStringCount = sizeof(notAllowedSubStrings)/sizeof(const char *); + const int notAllowedSubStringCount = sizeof(notAllowedSubStrings) / sizeof(const char *); for (int s = 0; s < notAllowedSubStringCount; s++) { const QLatin1String notAllowedSubString(notAllowedSubStrings[s]); if (name.contains(notAllowedSubString)) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The name must not contain '%1'.").arg(QString(notAllowedSubString)); + } return false; } } // Windows devices bool matchesWinDevice = windowsDeviceNoSubDirPattern().exactMatch(name); - if (!matchesWinDevice && allowDirectories) + if (!matchesWinDevice && allowDirectories) { matchesWinDevice = windowsDeviceSubDirPattern().exactMatch(name); + } if (matchesWinDevice) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The name must not match that of a MS Windows device. (%1)."). arg(windowsDeviceNoSubDirPattern().pattern().replace(QLatin1Char('|'), QLatin1Char(','))); + } return false; } return true; } -bool FileNameValidatingLineEdit::validate(const QString &value, QString *errorMessage) const +bool FileNameValidatingLineEdit::validate(const QString &value, QString *errorMessage) const { return validateFileName(value, m_allowDirectories, errorMessage); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.h b/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.h index 7036ac332..81e0436d7 100644 --- a/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.h +++ b/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.h @@ -4,25 +4,25 @@ * @file filenamevalidatinglineedit.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,15 +32,12 @@ #include "basevalidatinglineedit.h" namespace Utils { - /** * A control that let's the user choose a file name, based on a QLineEdit. Has * some validation logic for embedding into QWizardPage. */ -class QTCREATOR_UTILS_EXPORT FileNameValidatingLineEdit : public BaseValidatingLineEdit -{ - Q_OBJECT - Q_DISABLE_COPY(FileNameValidatingLineEdit) +class QTCREATOR_UTILS_EXPORT FileNameValidatingLineEdit : public BaseValidatingLineEdit { + Q_OBJECT Q_DISABLE_COPY(FileNameValidatingLineEdit) Q_PROPERTY(bool allowDirectories READ allowDirectories WRITE setAllowDirectories) public: explicit FileNameValidatingLineEdit(QWidget *parent = 0); @@ -63,7 +60,6 @@ private: bool m_allowDirectories; void *m_unused; }; - } // namespace Utils #endif // FILENAMEVALIDATINGLINEEDIT_H diff --git a/ground/openpilotgcs/src/libs/utils/filesearch.cpp b/ground/openpilotgcs/src/libs/utils/filesearch.cpp index b2d5a647e..c728aeb78 100644 --- a/ground/openpilotgcs/src/libs/utils/filesearch.cpp +++ b/ground/openpilotgcs/src/libs/utils/filesearch.cpp @@ -4,25 +4,25 @@ * @file filesearch.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -46,7 +46,7 @@ static inline QString msgCanceled(const QString &searchTerm, int numMatches, int return QCoreApplication::translate("Utils::FileSearch", "%1: canceled. %n occurrences found in %2 files.", 0, QCoreApplication::CodecForTr, numMatches). - arg(searchTerm).arg(numFilesSearched); + arg(searchTerm).arg(numFilesSearched); } static inline QString msgFound(const QString &searchTerm, int numMatches, int numFilesSearched) @@ -54,7 +54,7 @@ static inline QString msgFound(const QString &searchTerm, int numMatches, int nu return QCoreApplication::translate("Utils::FileSearch", "%1: %n occurrences found in %2 files.", 0, QCoreApplication::CodecForTr, numMatches). - arg(searchTerm).arg(numFilesSearched); + arg(searchTerm).arg(numFilesSearched); } static inline QString msgFound(const QString &searchTerm, int numMatches, int numFilesSearched, int filesSize) @@ -62,11 +62,10 @@ static inline QString msgFound(const QString &searchTerm, int numMatches, int nu return QCoreApplication::translate("Utils::FileSearch", "%1: %n occurrences found in %2 of %3 files.", 0, QCoreApplication::CodecForTr, numMatches). - arg(searchTerm).arg(numFilesSearched).arg(filesSize); + arg(searchTerm).arg(numFilesSearched).arg(filesSize); } namespace { - void runFileSearch(QFutureInterface &future, QString searchTerm, QStringList files, @@ -78,25 +77,26 @@ void runFileSearch(QFutureInterface &future, int numMatches = 0; bool caseInsensitive = !(flags & QTextDocument::FindCaseSensitively); - bool wholeWord = (flags & QTextDocument::FindWholeWords); + bool wholeWord = (flags & QTextDocument::FindWholeWords); - QByteArray sa = searchTerm.toUtf8(); - int scMaxIndex = sa.length()-1; - const char *sc = sa.constData(); + QByteArray sa = searchTerm.toUtf8(); + int scMaxIndex = sa.length() - 1; + const char *sc = sa.constData(); - QByteArray sal = searchTerm.toLower().toUtf8(); + QByteArray sal = searchTerm.toLower().toUtf8(); const char *scl = sal.constData(); - QByteArray sau = searchTerm.toUpper().toUtf8(); + QByteArray sau = searchTerm.toUpper().toUtf8(); const char *scu = sau.constData(); - int chunkSize = qMax(100000, sa.length()); + int chunkSize = qMax(100000, sa.length()); QFile file; QBuffer buffer; - foreach (QString s, files) { - if (future.isPaused()) + foreach(QString s, files) { + if (future.isPaused()) { future.waitForResume(); + } if (future.isCanceled()) { future.setProgressValueAndText(numFilesSearched, msgCanceled(searchTerm, numMatches, numFilesSearched)); break; @@ -109,54 +109,55 @@ void runFileSearch(QFutureInterface &future, file.setFileName(s); device = &file; } - if (!device->open(QIODevice::ReadOnly)) + if (!device->open(QIODevice::ReadOnly)) { continue; + } int lineNr = 1; const char *startOfLastLine = NULL; bool firstChunk = true; while (!device->atEnd()) { - if (!firstChunk) - device->seek(device->pos()-sa.length()+1); + if (!firstChunk) { + device->seek(device->pos() - sa.length() + 1); + } const QByteArray chunk = device->read(chunkSize); - const char *chunkPtr = chunk.constData(); + const char *chunkPtr = chunk.constData(); startOfLastLine = chunkPtr; - for (const char *regionPtr = chunkPtr; regionPtr < chunkPtr + chunk.length()-scMaxIndex; ++regionPtr) { + for (const char *regionPtr = chunkPtr; regionPtr < chunkPtr + chunk.length() - scMaxIndex; ++regionPtr) { const char *regionEnd = regionPtr + scMaxIndex; if (*regionPtr == '\n') { startOfLastLine = regionPtr + 1; ++lineNr; - } - else if ( - // case sensitive - (!caseInsensitive && *regionPtr == sc[0] && *regionEnd == sc[scMaxIndex]) - || - // case insensitive - (caseInsensitive && (*regionPtr == scl[0] || *regionPtr == scu[0]) - && (*regionEnd == scl[scMaxIndex] || *regionEnd == scu[scMaxIndex])) - ) { - const char *afterRegion = regionEnd + 1; + } else if ( + // case sensitive + (!caseInsensitive && *regionPtr == sc[0] && *regionEnd == sc[scMaxIndex]) + || + // case insensitive + (caseInsensitive && (*regionPtr == scl[0] || *regionPtr == scu[0]) + && (*regionEnd == scl[scMaxIndex] || *regionEnd == scu[scMaxIndex])) + ) { + const char *afterRegion = regionEnd + 1; const char *beforeRegion = regionPtr - 1; bool equal = true; if (wholeWord && - ( isalnum(*beforeRegion) - || (*beforeRegion == '_') - || isalnum(*afterRegion) - || (*afterRegion == '_'))) { + (isalnum(*beforeRegion) + || (*beforeRegion == '_') + || isalnum(*afterRegion) + || (*afterRegion == '_'))) { equal = false; } int regionIndex = 1; for (const char *regionCursor = regionPtr + 1; regionCursor < regionEnd; ++regionCursor, ++regionIndex) { - if ( // case sensitive - (!caseInsensitive && equal && *regionCursor != sc[regionIndex]) - || - // case insensitive - (caseInsensitive && equal && *regionCursor != sc[regionIndex] && *regionCursor != scl[regionIndex] && *regionCursor != scu[regionIndex]) - ) { - equal = false; + if ( // case sensitive + (!caseInsensitive && equal && *regionCursor != sc[regionIndex]) + || + // case insensitive + (caseInsensitive && equal && *regionCursor != sc[regionIndex] && *regionCursor != scl[regionIndex] && *regionCursor != scu[regionIndex]) + ) { + equal = false; } } if (equal) { @@ -166,10 +167,11 @@ void runFileSearch(QFutureInterface &future, res.reserve(256); int i = 0; int n = 0; - while (startOfLastLine[i] != '\n' && startOfLastLine[i] != '\r' && i < textLength && n++ < 256) + while (startOfLastLine[i] != '\n' && startOfLastLine[i] != '\r' && i < textLength && n++ < 256) { res.append(startOfLastLine[i++]); + } future.reportResult(FileSearchResult(s, lineNr, QString(res), - regionPtr - startOfLastLine, sa.length())); + regionPtr - startOfLastLine, sa.length())); ++numMatches; } } @@ -181,30 +183,33 @@ void runFileSearch(QFutureInterface &future, future.setProgressValueAndText(numFilesSearched, msgFound(searchTerm, numMatches, numFilesSearched, files.size())); device->close(); } - if (!future.isCanceled()) + if (!future.isCanceled()) { future.setProgressValueAndText(numFilesSearched, msgFound(searchTerm, numMatches, numFilesSearched)); + } } void runFileSearchRegExp(QFutureInterface &future, - QString searchTerm, - QStringList files, - QTextDocument::FindFlags flags, - QMap fileToContentsMap) + QString searchTerm, + QStringList files, + QTextDocument::FindFlags flags, + QMap fileToContentsMap) { future.setProgressRange(0, files.size()); int numFilesSearched = 0; int numMatches = 0; - if (flags & QTextDocument::FindWholeWords) + if (flags & QTextDocument::FindWholeWords) { searchTerm = QString::fromLatin1("\\b%1\\b").arg(searchTerm); + } const Qt::CaseSensitivity caseSensitivity = (flags & QTextDocument::FindCaseSensitively) ? Qt::CaseSensitive : Qt::CaseInsensitive; const QRegExp expression(searchTerm, caseSensitivity); QFile file; QString str; QTextStream stream; - foreach (const QString &s, files) { - if (future.isPaused()) + foreach(const QString &s, files) { + if (future.isPaused()) { future.waitForResume(); + } if (future.isCanceled()) { future.setProgressValueAndText(numFilesSearched, msgCanceled(searchTerm, numMatches, numFilesSearched)); break; @@ -216,8 +221,9 @@ void runFileSearchRegExp(QFutureInterface &future, stream.setString(&str); } else { file.setFileName(s); - if (!file.open(QIODevice::ReadOnly)) + if (!file.open(QIODevice::ReadOnly)) { continue; + } needsToCloseFile = true; stream.setDevice(&file); } @@ -228,33 +234,34 @@ void runFileSearchRegExp(QFutureInterface &future, int pos = 0; while ((pos = expression.indexIn(line, pos)) != -1) { future.reportResult(FileSearchResult(s, lineNr, line, - pos, expression.matchedLength())); + pos, expression.matchedLength())); pos += expression.matchedLength(); } ++lineNr; } ++numFilesSearched; future.setProgressValueAndText(numFilesSearched, msgFound(searchTerm, numMatches, numFilesSearched, files.size())); - if (needsToCloseFile) + if (needsToCloseFile) { file.close(); + } } - if (!future.isCanceled()) + if (!future.isCanceled()) { future.setProgressValueAndText(numFilesSearched, msgFound(searchTerm, numMatches, numFilesSearched)); + } } - } // namespace QFuture Utils::findInFiles(const QString &searchTerm, const QStringList &files, - QTextDocument::FindFlags flags, QMap fileToContentsMap) + QTextDocument::FindFlags flags, QMap fileToContentsMap) { return QtConcurrent::run > - (runFileSearch, searchTerm, files, flags, fileToContentsMap); + (runFileSearch, searchTerm, files, flags, fileToContentsMap); } QFuture Utils::findInFilesRegExp(const QString &searchTerm, const QStringList &files, - QTextDocument::FindFlags flags, QMap fileToContentsMap) + QTextDocument::FindFlags flags, QMap fileToContentsMap) { return QtConcurrent::run > - (runFileSearchRegExp, searchTerm, files, flags, fileToContentsMap); + (runFileSearchRegExp, searchTerm, files, flags, fileToContentsMap); } diff --git a/ground/openpilotgcs/src/libs/utils/filesearch.h b/ground/openpilotgcs/src/libs/utils/filesearch.h index c7a046b0f..09802336e 100644 --- a/ground/openpilotgcs/src/libs/utils/filesearch.h +++ b/ground/openpilotgcs/src/libs/utils/filesearch.h @@ -4,25 +4,25 @@ * @file filesearch.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,15 +37,12 @@ #include namespace Utils { - -class QTCREATOR_UTILS_EXPORT FileSearchResult -{ +class QTCREATOR_UTILS_EXPORT FileSearchResult { public: FileSearchResult() {} FileSearchResult(QString fileName, int lineNumber, QString matchingLine, int matchStart, int matchLength) - : fileName(fileName), lineNumber(lineNumber), matchingLine(matchingLine), matchStart(matchStart), matchLength(matchLength) - { - } + : fileName(fileName), lineNumber(lineNumber), matchingLine(matchingLine), matchStart(matchStart), matchLength(matchLength) + {} QString fileName; int lineNumber; QString matchingLine; @@ -54,11 +51,10 @@ public: }; QTCREATOR_UTILS_EXPORT QFuture findInFiles(const QString &searchTerm, const QStringList &files, - QTextDocument::FindFlags flags, QMap fileToContentsMap = QMap()); + QTextDocument::FindFlags flags, QMap fileToContentsMap = QMap()); QTCREATOR_UTILS_EXPORT QFuture findInFilesRegExp(const QString &searchTerm, const QStringList &files, - QTextDocument::FindFlags flags, QMap fileToContentsMap = QMap()); - + QTextDocument::FindFlags flags, QMap fileToContentsMap = QMap()); } // namespace Utils #endif // FILESEARCH_H diff --git a/ground/openpilotgcs/src/libs/utils/filewizarddialog.cpp b/ground/openpilotgcs/src/libs/utils/filewizarddialog.cpp index 615b3b0ca..552e212fd 100644 --- a/ground/openpilotgcs/src/libs/utils/filewizarddialog.cpp +++ b/ground/openpilotgcs/src/libs/utils/filewizarddialog.cpp @@ -4,25 +4,25 @@ * @file filewizarddialog.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,7 +32,6 @@ #include namespace Utils { - FileWizardDialog::FileWizardDialog(QWidget *parent) : QWizard(parent), m_filePage(new FileWizardPage) @@ -58,12 +57,10 @@ QString FileWizardDialog::path() const void FileWizardDialog::setPath(const QString &path) { m_filePage->setPath(path); - } void FileWizardDialog::setName(const QString &name) { m_filePage->setName(name); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/filewizarddialog.h b/ground/openpilotgcs/src/libs/utils/filewizarddialog.h index f68e203ce..5e10a2aa2 100644 --- a/ground/openpilotgcs/src/libs/utils/filewizarddialog.h +++ b/ground/openpilotgcs/src/libs/utils/filewizarddialog.h @@ -4,25 +4,25 @@ * @file filewizarddialog.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,17 +34,15 @@ #include namespace Utils { - class FileWizardPage; /* Standard wizard for a single file letting the user choose name and path. Custom pages can be added via Core::IWizardExtension. -*/ + */ class QTCREATOR_UTILS_EXPORT FileWizardDialog : public QWizard { - Q_OBJECT - Q_DISABLE_COPY(FileWizardDialog) + Q_OBJECT Q_DISABLE_COPY(FileWizardDialog) public: explicit FileWizardDialog(QWidget *parent = 0); @@ -58,7 +56,6 @@ public slots: private: FileWizardPage *m_filePage; }; - } // namespace Utils #endif // FILEWIZARDDIALOG_H diff --git a/ground/openpilotgcs/src/libs/utils/filewizardpage.cpp b/ground/openpilotgcs/src/libs/utils/filewizardpage.cpp index 17f16e065..f25fb3d4b 100644 --- a/ground/openpilotgcs/src/libs/utils/filewizardpage.cpp +++ b/ground/openpilotgcs/src/libs/utils/filewizardpage.cpp @@ -4,25 +4,25 @@ * @file filewizardpage.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,9 +30,7 @@ #include "ui_filewizardpage.h" namespace Utils { - -struct FileWizardPagePrivate -{ +struct FileWizardPagePrivate { FileWizardPagePrivate(); Ui::WizardPage m_ui; bool m_complete; @@ -40,8 +38,7 @@ struct FileWizardPagePrivate FileWizardPagePrivate::FileWizardPagePrivate() : m_complete(false) -{ -} +{} FileWizardPage::FileWizardPage(QWidget *parent) : QWizardPage(parent), @@ -83,6 +80,7 @@ void FileWizardPage::setName(const QString &name) void FileWizardPage::changeEvent(QEvent *e) { QWizardPage::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: m_d->m_ui.retranslateUi(this); @@ -110,6 +108,7 @@ void FileWizardPage::setPathLabel(const QString &label) void FileWizardPage::slotValidChanged() { const bool newComplete = m_d->m_ui.pathChooser->isValid() && m_d->m_ui.nameLineEdit->isValid(); + if (newComplete != m_d->m_complete) { m_d->m_complete = newComplete; emit completeChanged(); @@ -118,13 +117,13 @@ void FileWizardPage::slotValidChanged() void FileWizardPage::slotActivated() { - if (m_d->m_complete) + if (m_d->m_complete) { emit activated(); + } } bool FileWizardPage::validateBaseName(const QString &name, QString *errorMessage /* = 0*/) { return FileNameValidatingLineEdit::validateFileName(name, false, errorMessage); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/filewizardpage.h b/ground/openpilotgcs/src/libs/utils/filewizardpage.h index 88d8b01e1..7fa559711 100644 --- a/ground/openpilotgcs/src/libs/utils/filewizardpage.h +++ b/ground/openpilotgcs/src/libs/utils/filewizardpage.h @@ -4,25 +4,25 @@ * @file filewizardpage.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,6 @@ #include namespace Utils { - struct FileWizardPagePrivate; /** @@ -44,10 +43,8 @@ struct FileWizardPagePrivate; * The name and path labels can be changed. By default they are simply "Name:" * and "Path:". */ -class QTCREATOR_UTILS_EXPORT FileWizardPage : public QWizardPage -{ - Q_OBJECT - Q_DISABLE_COPY(FileWizardPage) +class QTCREATOR_UTILS_EXPORT FileWizardPage : public QWizardPage { + Q_OBJECT Q_DISABLE_COPY(FileWizardPage) Q_PROPERTY(QString path READ path WRITE setPath DESIGNABLE true) Q_PROPERTY(QString name READ name WRITE setName DESIGNABLE true) public: @@ -83,7 +80,6 @@ protected: private: FileWizardPagePrivate *m_d; }; - } // namespace Utils #endif // FILEWIZARDPAGE_H diff --git a/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp b/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp index 67a32b0e8..44bf8c079 100644 --- a/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp +++ b/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp @@ -35,40 +35,45 @@ #include "worldmagmodel.h" namespace Utils { +HomeLocationUtil::HomeLocationUtil() +{} - HomeLocationUtil::HomeLocationUtil() - { +/** + * @brief Get local magnetic field + * @param[in] LLA The longitude-latitude-altitude coordinate to compute the magnetic field at + * @param[out] Be The resulting magnetic field at that location and time in [mGau](?) + * @returns 0 if successful, negative otherwise. + */ +int HomeLocationUtil::getDetails(double LLA[3], double Be[3]) +{ + // ************* + // check input parms + + double latitude = LLA[0]; + double longitude = LLA[1]; + double altitude = LLA[2]; + + if (latitude != latitude) { + return -1; // prevent nan error } - - /** - * @brief Get local magnetic field - * @param[in] LLA The longitude-latitude-altitude coordinate to compute the magnetic field at - * @param[out] Be The resulting magnetic field at that location and time in [mGau](?) - * @returns 0 if successful, negative otherwise. - */ - int HomeLocationUtil::getDetails(double LLA[3], double Be[3]) - { - // ************* - // check input parms - - double latitude = LLA[0]; - double longitude = LLA[1]; - double altitude = LLA[2]; - - if (latitude != latitude) return -1; // prevent nan error - if (longitude != longitude) return -2; // prevent nan error - if (altitude != altitude) return -3; // prevent nan error - - if (latitude < -90 || latitude > 90) return -4; // range checking - if (longitude < -180 || longitude > 180) return -5; // range checking - - QDateTime dt = QDateTime::currentDateTime().toUTC(); - - // Fetch world magnetic model - int result = WorldMagModel().GetMagVector(LLA, dt.date().month(), dt.date().day(), dt.date().year(), Be); - Q_ASSERT(result == 0); - - return result; + if (longitude != longitude) { + return -2; // prevent nan error } + if (altitude != altitude) { + return -3; // prevent nan error + } + if (latitude < -90 || latitude > 90) { + return -4; // range checking + } + if (longitude < -180 || longitude > 180) { + return -5; // range checking + } + QDateTime dt = QDateTime::currentDateTime().toUTC(); + // Fetch world magnetic model + int result = WorldMagModel().GetMagVector(LLA, dt.date().month(), dt.date().day(), dt.date().year(), Be); + Q_ASSERT(result == 0); + + return result; +} } diff --git a/ground/openpilotgcs/src/libs/utils/homelocationutil.h b/ground/openpilotgcs/src/libs/utils/homelocationutil.h index 3688787ee..8dde442d2 100644 --- a/ground/openpilotgcs/src/libs/utils/homelocationutil.h +++ b/ground/openpilotgcs/src/libs/utils/homelocationutil.h @@ -34,18 +34,14 @@ // ****************************** namespace Utils { +class QTCREATOR_UTILS_EXPORT HomeLocationUtil { +public: + HomeLocationUtil(); - class QTCREATOR_UTILS_EXPORT HomeLocationUtil - { - public: - HomeLocationUtil(); - - int getDetails(double LLA[3], double Be[3]); - - private: - - }; + int getDetails(double LLA[3], double Be[3]); +private: +}; } // ****************************** diff --git a/ground/openpilotgcs/src/libs/utils/iwelcomepage.cpp b/ground/openpilotgcs/src/libs/utils/iwelcomepage.cpp index c6cac187f..8654a8bc3 100644 --- a/ground/openpilotgcs/src/libs/utils/iwelcomepage.cpp +++ b/ground/openpilotgcs/src/libs/utils/iwelcomepage.cpp @@ -4,25 +4,25 @@ * @file iwelcomepage.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,11 +31,7 @@ using namespace Utils; IWelcomePage::IWelcomePage() -{ - -} +{} IWelcomePage::~IWelcomePage() -{ - -} +{} diff --git a/ground/openpilotgcs/src/libs/utils/iwelcomepage.h b/ground/openpilotgcs/src/libs/utils/iwelcomepage.h index 34d910bbb..9439c1a9a 100644 --- a/ground/openpilotgcs/src/libs/utils/iwelcomepage.h +++ b/ground/openpilotgcs/src/libs/utils/iwelcomepage.h @@ -4,25 +4,25 @@ * @file iwelcomepage.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,11 +35,9 @@ #include namespace Utils { - class IWelcomePagePrivate; -class QTCREATOR_UTILS_EXPORT IWelcomePage : public QObject -{ +class QTCREATOR_UTILS_EXPORT IWelcomePage : public QObject { Q_OBJECT public: @@ -48,13 +46,15 @@ public: virtual QWidget *page() = 0; virtual QString title() const = 0; - virtual int priority() const { return 0; } + virtual int priority() const + { + return 0; + } private: // not used atm IWelcomePagePrivate *m_d; }; - } #endif // IWELCOMEPAGE_H diff --git a/ground/openpilotgcs/src/libs/utils/linecolumnlabel.cpp b/ground/openpilotgcs/src/libs/utils/linecolumnlabel.cpp index 1c59b1a0d..4992b17fa 100644 --- a/ground/openpilotgcs/src/libs/utils/linecolumnlabel.cpp +++ b/ground/openpilotgcs/src/libs/utils/linecolumnlabel.cpp @@ -4,44 +4,42 @@ * @file linecolumnlabel.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "linecolumnlabel.h" namespace Utils { - LineColumnLabel::LineColumnLabel(QWidget *parent) - : QLabel(parent), m_unused(0) -{ -} + : QLabel(parent), m_unused(0) +{} LineColumnLabel::~LineColumnLabel() -{ -} +{} void LineColumnLabel::setText(const QString &text, const QString &maxText) { QLabel::setText(text); + m_maxText = maxText; } QSize LineColumnLabel::sizeHint() const @@ -56,7 +54,6 @@ QString LineColumnLabel::maxText() const void LineColumnLabel::setMaxText(const QString &maxText) { - m_maxText = maxText; + m_maxText = maxText; } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/linecolumnlabel.h b/ground/openpilotgcs/src/libs/utils/linecolumnlabel.h index 836598da2..10ff22117 100644 --- a/ground/openpilotgcs/src/libs/utils/linecolumnlabel.h +++ b/ground/openpilotgcs/src/libs/utils/linecolumnlabel.h @@ -4,25 +4,25 @@ * @file linecolumnlabel.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,15 +33,12 @@ #include namespace Utils { - /* A label suitable for displaying cursor positions, etc. with a fixed * with derived from a sample text. */ -class QTCREATOR_UTILS_EXPORT LineColumnLabel : public QLabel -{ +class QTCREATOR_UTILS_EXPORT LineColumnLabel : public QLabel { Q_DISABLE_COPY(LineColumnLabel) - Q_OBJECT - Q_PROPERTY(QString maxText READ maxText WRITE setMaxText DESIGNABLE true) + Q_OBJECT Q_PROPERTY(QString maxText READ maxText WRITE setMaxText DESIGNABLE true) public: explicit LineColumnLabel(QWidget *parent = 0); @@ -57,7 +54,6 @@ private: QString m_maxText; void *m_unused; }; - } // namespace Utils #endif // LINECOLUMNLABEL_H diff --git a/ground/openpilotgcs/src/libs/utils/listutils.h b/ground/openpilotgcs/src/libs/utils/listutils.h index b3c079c02..a842ecd09 100644 --- a/ground/openpilotgcs/src/libs/utils/listutils.h +++ b/ground/openpilotgcs/src/libs/utils/listutils.h @@ -4,25 +4,25 @@ * @file listutils.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,17 +32,15 @@ #include namespace Utils { - template QList qwConvertList(const QList &list) { QList convertedList; - foreach (T2 listEntry, list) { + foreach(T2 listEntry, list) { convertedList << qobject_cast(listEntry); } return convertedList; } - } // namespace Utils #endif // LISTUTILS_H diff --git a/ground/openpilotgcs/src/libs/utils/mylistwidget.cpp b/ground/openpilotgcs/src/libs/utils/mylistwidget.cpp index f5fbfb639..f8a6de992 100644 --- a/ground/openpilotgcs/src/libs/utils/mylistwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/mylistwidget.cpp @@ -30,9 +30,10 @@ QStyleOptionViewItem MyListWidget::viewOptions() const { QStyleOptionViewItem option = QListWidget::viewOptions(); + if (m_iconAbove) { option.decorationPosition = QStyleOptionViewItem::Top; - option.displayAlignment = Qt::AlignCenter; + option.displayAlignment = Qt::AlignCenter; } return option; } diff --git a/ground/openpilotgcs/src/libs/utils/mylistwidget.h b/ground/openpilotgcs/src/libs/utils/mylistwidget.h index 176063681..7463db68f 100644 --- a/ground/openpilotgcs/src/libs/utils/mylistwidget.h +++ b/ground/openpilotgcs/src/libs/utils/mylistwidget.h @@ -38,12 +38,14 @@ * to place the icon above the label in ListMode. This is achieved * the easiest by subclassing QListWidget and overriding viewOptions(). */ -class QTCREATOR_UTILS_EXPORT MyListWidget : public QListWidget -{ +class QTCREATOR_UTILS_EXPORT MyListWidget : public QListWidget { Q_OBJECT public: MyListWidget(QWidget *parent) : QListWidget(parent), m_iconAbove(false) {} - void setIconAbove(bool iconAbove) { m_iconAbove = iconAbove; } + void setIconAbove(bool iconAbove) + { + m_iconAbove = iconAbove; + } protected: QStyleOptionViewItem viewOptions() const; private: diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp index a9610c0e5..20f45b08e 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp @@ -33,10 +33,10 @@ MyTabbedStackWidget::MyTabbedStackWidget(QWidget *parent, bool isVertical, bool iconAbove) : QWidget(parent), - m_vertical(isVertical), - m_iconAbove(iconAbove) + m_vertical(isVertical), + m_iconAbove(iconAbove) { - m_listWidget = new MyListWidget(this); + m_listWidget = new MyListWidget(this); m_listWidget->setIconAbove(m_iconAbove); m_stackWidget = new QStackedWidget(); m_stackWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); @@ -69,7 +69,7 @@ MyTabbedStackWidget::MyTabbedStackWidget(QWidget *parent, bool isVertical, bool m_stackWidget->setContentsMargins(0, 0, 0, 0); setLayout(toplevelLayout); - connect(m_listWidget, SIGNAL(currentRowChanged(int)), this, SLOT(showWidget(int)),Qt::QueuedConnection); + connect(m_listWidget, SIGNAL(currentRowChanged(int)), this, SLOT(showWidget(int)), Qt::QueuedConnection); } void MyTabbedStackWidget::insertTab(const int index, QWidget *tab, const QIcon &icon, const QString &label) @@ -83,7 +83,8 @@ void MyTabbedStackWidget::insertTab(const int index, QWidget *tab, const QIcon & void MyTabbedStackWidget::removeTab(int index) { - QWidget * wid=m_stackWidget->widget(index); + QWidget *wid = m_stackWidget->widget(index); + m_stackWidget->removeWidget(wid); delete wid; QListWidgetItem *item = m_listWidget->item(index); @@ -93,7 +94,7 @@ void MyTabbedStackWidget::removeTab(int index) void MyTabbedStackWidget::setWidgetsEnabled(bool enabled) { - for(int i = 0; i < m_stackWidget->count(); i++) { + for (int i = 0; i < m_stackWidget->count(); i++) { m_stackWidget->widget(i)->setEnabled(enabled); } } @@ -110,18 +111,16 @@ void MyTabbedStackWidget::setCurrentIndex(int index) void MyTabbedStackWidget::showWidget(int index) { - if(m_stackWidget->currentIndex()==index) + if (m_stackWidget->currentIndex() == index) { return; - bool proceed=false; - emit currentAboutToShow(index,&proceed); - if(proceed) - { + } + bool proceed = false; + emit currentAboutToShow(index, &proceed); + if (proceed) { m_stackWidget->setCurrentIndex(index); emit currentChanged(index); - } - else - { - m_listWidget->setCurrentRow(m_stackWidget->currentIndex(),QItemSelectionModel::ClearAndSelect); + } else { + m_listWidget->setCurrentRow(m_stackWidget->currentIndex(), QItemSelectionModel::ClearAndSelect); } } @@ -131,4 +130,3 @@ void MyTabbedStackWidget::insertCornerWidget(int index, QWidget *widget) widget->hide(); } - diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h index 7307673b1..6862ee426 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h @@ -36,8 +36,7 @@ * MyTabbedStackWidget is a MyListWidget combined with a QStackedWidget, * similar in function to QTabWidget. */ -class QTCREATOR_UTILS_EXPORT MyTabbedStackWidget : public QWidget -{ +class QTCREATOR_UTILS_EXPORT MyTabbedStackWidget : public QWidget { Q_OBJECT public: @@ -45,19 +44,31 @@ public: void insertTab(int index, QWidget *tab, const QIcon &icon, const QString &label); void removeTab(int index); - void setIconSize(int size) { m_listWidget->setIconSize(QSize(size, size)); } + void setIconSize(int size) + { + m_listWidget->setIconSize(QSize(size, size)); + } void setWidgetsEnabled(bool enabled); int currentIndex() const; void insertCornerWidget(int index, QWidget *widget); - int cornerWidgetCount() { return m_cornerWidgetCount; } - QWidget * currentWidget(){return m_stackWidget->currentWidget();} - QWidget * getWidget(int index) {return m_stackWidget->widget(index);} + int cornerWidgetCount() + { + return m_cornerWidgetCount; + } + QWidget *currentWidget() + { + return m_stackWidget->currentWidget(); + } + QWidget *getWidget(int index) + { + return m_stackWidget->widget(index); + } signals: - void currentAboutToShow(int index,bool * proceed); + void currentAboutToShow(int index, bool *proceed); void currentChanged(int index); public slots: diff --git a/ground/openpilotgcs/src/libs/utils/mytabwidget.cpp b/ground/openpilotgcs/src/libs/utils/mytabwidget.cpp index 2a1a7bee4..330312777 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/mytabwidget.cpp @@ -32,12 +32,14 @@ MyTabWidget::MyTabWidget(QWidget *parent) : QTabWidget(parent) { QTabBar *tabBar = QTabWidget::tabBar(); - connect(tabBar, SIGNAL(tabMoved(int, int)), this, SLOT(myTabMoved(int,int))); + + connect(tabBar, SIGNAL(tabMoved(int, int)), this, SLOT(myTabMoved(int, int))); } void MyTabWidget::moveTab(int from, int to) { QTabBar *tabBar = QTabWidget::tabBar(); + tabBar->moveTab(from, to); } diff --git a/ground/openpilotgcs/src/libs/utils/mytabwidget.h b/ground/openpilotgcs/src/libs/utils/mytabwidget.h index 8f941110b..6110c13c6 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabwidget.h +++ b/ground/openpilotgcs/src/libs/utils/mytabwidget.h @@ -37,8 +37,7 @@ * tabMoved(int, int) which QTabBar has but for some reason is * not made available from QTabWidget. */ -class QTCREATOR_UTILS_EXPORT MyTabWidget : public QTabWidget -{ +class QTCREATOR_UTILS_EXPORT MyTabWidget : public QTabWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/libs/utils/newclasswidget.cpp b/ground/openpilotgcs/src/libs/utils/newclasswidget.cpp index 61d7c52a4..844dbd68c 100644 --- a/ground/openpilotgcs/src/libs/utils/newclasswidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/newclasswidget.cpp @@ -4,25 +4,25 @@ * @file newclasswidget.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,7 +41,6 @@ enum { debugNewClassWidget = 0 }; namespace Utils { - struct NewClassWidgetPrivate { NewClassWidgetPrivate(); @@ -49,18 +48,18 @@ struct NewClassWidgetPrivate { QString m_headerExtension; QString m_sourceExtension; QString m_formExtension; - bool m_valid; - bool m_classEdited; + bool m_valid; + bool m_classEdited; // Store the "visible" values to prevent the READ accessors from being // fooled by a temporarily hidden widget - bool m_baseClassInputVisible; - bool m_formInputVisible; - bool m_pathInputVisible; - bool m_qobjectCheckBoxVisible; - bool m_formInputCheckable; + bool m_baseClassInputVisible; + bool m_formInputVisible; + bool m_pathInputVisible; + bool m_qobjectCheckBoxVisible; + bool m_formInputCheckable; }; -NewClassWidgetPrivate:: NewClassWidgetPrivate() : +NewClassWidgetPrivate::NewClassWidgetPrivate() : m_headerExtension(QLatin1String("h")), m_sourceExtension(QLatin1String("cpp")), m_formExtension(QLatin1String("ui")), @@ -72,8 +71,7 @@ NewClassWidgetPrivate:: NewClassWidgetPrivate() : m_qobjectCheckBoxVisible(false), m_formInputCheckable(false) -{ -} +{} // --------------------- NewClassWidget NewClassWidget::NewClassWidget(QWidget *parent) : @@ -114,7 +112,7 @@ NewClassWidget::NewClassWidget(QWidget *parent) : connect(m_d->m_ui.formFileLineEdit, SIGNAL(validReturnPressed()), this, SLOT(slotActivated())); connect(m_d->m_ui.pathChooser, SIGNAL(returnPressed()), - this, SLOT(slotActivated())); + this, SLOT(slotActivated())); connect(m_d->m_ui.generateFormCheckBox, SIGNAL(stateChanged(int)), this, SLOT(slotFormInputChecked())); @@ -131,17 +129,20 @@ NewClassWidget::~NewClassWidget() void NewClassWidget::classNameEdited() { - if (debugNewClassWidget) + if (debugNewClassWidget) { qDebug() << Q_FUNC_INFO << m_d->m_headerExtension << m_d->m_sourceExtension; + } m_d->m_classEdited = true; } void NewClassWidget::suggestClassNameFromBase() { - if (debugNewClassWidget) + if (debugNewClassWidget) { qDebug() << Q_FUNC_INFO << m_d->m_headerExtension << m_d->m_sourceExtension; - if (m_d->m_classEdited) + } + if (m_d->m_classEdited) { return; + } // Suggest a class unless edited ("QMainWindow"->"MainWindow") QString base = baseClassName(); if (base.startsWith(QLatin1Char('Q'))) { @@ -154,8 +155,10 @@ QStringList NewClassWidget::baseClassChoices() const { QStringList rc; const int count = m_d->m_ui.baseClassComboBox->count(); - for (int i = 0; i < count; i++) + + for (int i = 0; i < count; i++) { rc.push_back(m_d->m_ui.baseClassComboBox->itemText(i)); + } return rc; } @@ -179,12 +182,12 @@ void NewClassWidget::setBaseClassEditable(bool editable) bool NewClassWidget::isBaseClassInputVisible() const { - return m_d->m_baseClassInputVisible; + return m_d->m_baseClassInputVisible; } bool NewClassWidget::isBaseClassEditable() const { - return m_d->m_ui.baseClassComboBox->isEditable(); + return m_d->m_ui.baseClassComboBox->isEditable(); } void NewClassWidget::setFormInputVisible(bool visible) @@ -206,8 +209,9 @@ void NewClassWidget::setFormInputCheckable(bool checkable) void NewClassWidget::setFormInputCheckable(bool checkable, bool force) { - if (!force && checkable == m_d->m_formInputCheckable) + if (!force && checkable == m_d->m_formInputCheckable) { return; + } m_d->m_formInputCheckable = checkable; m_d->m_ui.generateFormLabel->setVisible(checkable); m_d->m_ui.generateFormCheckBox->setVisible(checkable); @@ -231,6 +235,7 @@ bool NewClassWidget::formInputChecked() const void NewClassWidget::slotFormInputChecked() { const bool checked = formInputChecked(); + m_d->m_ui.formLabel->setEnabled(checked); m_d->m_ui.formFileLineEdit->setEnabled(checked); } @@ -249,8 +254,9 @@ bool NewClassWidget::isPathInputVisible() const void NewClassWidget::setClassName(const QString &suggestedName) { - if (debugNewClassWidget) + if (debugNewClassWidget) { qDebug() << Q_FUNC_INFO << suggestedName << m_d->m_headerExtension << m_d->m_sourceExtension; + } m_d->m_ui.classLineEdit->setText(ClassNameValidatingLineEdit::createClassName(suggestedName)); } @@ -267,6 +273,7 @@ QString NewClassWidget::baseClassName() const void NewClassWidget::setBaseClassName(const QString &c) { const int index = m_d->m_ui.baseClassComboBox->findText(c); + if (index != -1) { m_d->m_ui.baseClassComboBox->setCurrentIndex(index); suggestClassNameFromBase(); @@ -295,12 +302,12 @@ QString NewClassWidget::path() const void NewClassWidget::setPath(const QString &path) { - m_d->m_ui.pathChooser->setPath(path); + m_d->m_ui.pathChooser->setPath(path); } bool NewClassWidget::namespacesEnabled() const { - return m_d->m_ui.classLineEdit->namespacesEnabled(); + return m_d->m_ui.classLineEdit->namespacesEnabled(); } void NewClassWidget::setNamespacesEnabled(bool b) @@ -315,8 +322,9 @@ QString NewClassWidget::sourceExtension() const void NewClassWidget::setSourceExtension(const QString &e) { - if (debugNewClassWidget) + if (debugNewClassWidget) { qDebug() << Q_FUNC_INFO << e; + } m_d->m_sourceExtension = fixSuffix(e); } @@ -327,8 +335,9 @@ QString NewClassWidget::headerExtension() const void NewClassWidget::setHeaderExtension(const QString &e) { - if (debugNewClassWidget) + if (debugNewClassWidget) { qDebug() << Q_FUNC_INFO << e; + } m_d->m_headerExtension = fixSuffix(e); } @@ -339,8 +348,9 @@ QString NewClassWidget::formExtension() const void NewClassWidget::setFormExtension(const QString &e) { - if (debugNewClassWidget) + if (debugNewClassWidget) { qDebug() << Q_FUNC_INFO << e; + } m_d->m_formExtension = fixSuffix(e); } @@ -393,6 +403,7 @@ void NewClassWidget::setClassTypeComboVisible(bool v) void NewClassWidget::slotValidChanged() { const bool newValid = isValid(); + if (newValid != m_d->m_valid) { m_d->m_valid = newValid; emit validChanged(); @@ -402,8 +413,9 @@ void NewClassWidget::slotValidChanged() bool NewClassWidget::isValid(QString *error) const { if (!m_d->m_ui.classLineEdit->isValid()) { - if (error) + if (error) { *error = m_d->m_ui.classLineEdit->errorMessage(); + } return false; } @@ -412,36 +424,41 @@ bool NewClassWidget::isValid(QString *error) const QRegExp classNameValidator(QLatin1String("[a-zA-Z_][a-zA-Z0-9_]*(::[a-zA-Z_][a-zA-Z0-9_]*)*")); const QString baseClass = m_d->m_ui.baseClassComboBox->currentText().trimmed(); if (!baseClass.isEmpty() && !classNameValidator.exactMatch(baseClass)) { - if (error) + if (error) { *error = tr("Invalid base class name"); + } return false; } } if (!m_d->m_ui.headerFileLineEdit->isValid()) { - if (error) + if (error) { *error = tr("Invalid header file name: '%1'").arg(m_d->m_ui.headerFileLineEdit->errorMessage()); + } return false; } if (!m_d->m_ui.sourceFileLineEdit->isValid()) { - if (error) + if (error) { *error = tr("Invalid source file name: '%1'").arg(m_d->m_ui.sourceFileLineEdit->errorMessage()); + } return false; } if (isFormInputVisible()) { if (!m_d->m_ui.formFileLineEdit->isValid()) { - if (error) + if (error) { *error = tr("Invalid form file name: '%1'").arg(m_d->m_ui.formFileLineEdit->errorMessage()); + } return false; } } if (isPathInputVisible()) { if (!m_d->m_ui.pathChooser->isValid()) { - if (error) - *error = m_d->m_ui.pathChooser->errorMessage(); + if (error) { + *error = m_d->m_ui.pathChooser->errorMessage(); + } return false; } } @@ -455,8 +472,9 @@ void NewClassWidget::triggerUpdateFileNames() void NewClassWidget::slotUpdateFileNames(const QString &baseName) { - if (debugNewClassWidget) + if (debugNewClassWidget) { qDebug() << Q_FUNC_INFO << baseName << m_d->m_headerExtension << m_d->m_sourceExtension; + } const QChar dot = QLatin1Char('.'); m_d->m_ui.sourceFileLineEdit->setText(baseName + dot + m_d->m_sourceExtension); m_d->m_ui.headerFileLineEdit->setText(baseName + dot + m_d->m_headerExtension); @@ -465,15 +483,18 @@ void NewClassWidget::slotUpdateFileNames(const QString &baseName) void NewClassWidget::slotActivated() { - if (m_d->m_valid) + if (m_d->m_valid) { emit activated(); + } } QString NewClassWidget::fixSuffix(const QString &suffix) { QString s = suffix; - if (s.startsWith(QLatin1Char('.'))) + + if (s.startsWith(QLatin1Char('.'))) { s.remove(0, 1); + } return s; } @@ -481,8 +502,10 @@ QString NewClassWidget::fixSuffix(const QString &suffix) static QString ensureSuffix(QString f, const QString &extension) { const QChar dot = QLatin1Char('.'); - if (f.contains(dot)) + + if (f.contains(dot)) { return f; + } f += dot; f += extension; return f; @@ -491,8 +514,9 @@ static QString ensureSuffix(QString f, const QString &extension) // If a non-empty name was passed, expand to directory and suffix static QString expandFileName(const QDir &dir, const QString name, const QString &extension) { - if (name.isEmpty()) + if (name.isEmpty()) { return QString(); + } return dir.absoluteFilePath(ensureSuffix(name, extension)); } @@ -500,11 +524,12 @@ QStringList NewClassWidget::files() const { QStringList rc; const QDir dir = QDir(path()); + rc.push_back(expandFileName(dir, headerFileName(), headerExtension())); rc.push_back(expandFileName(dir, sourceFileName(), sourceExtension())); - if (isFormInputVisible()) + if (isFormInputVisible()) { rc.push_back(expandFileName(dir, formFileName(), formExtension())); + } return rc; } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/newclasswidget.h b/ground/openpilotgcs/src/libs/utils/newclasswidget.h index 6b36553d1..eff073719 100644 --- a/ground/openpilotgcs/src/libs/utils/newclasswidget.h +++ b/ground/openpilotgcs/src/libs/utils/newclasswidget.h @@ -4,25 +4,25 @@ * @file newclasswidget.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,7 +38,6 @@ class QStringList; QT_END_NAMESPACE namespace Utils { - struct NewClassWidgetPrivate; /** @@ -47,11 +46,9 @@ struct NewClassWidgetPrivate; * names for header, source and form files. Has some smart logic to derive * the file names from the class name. */ -class QTCREATOR_UTILS_EXPORT NewClassWidget : public QWidget -{ +class QTCREATOR_UTILS_EXPORT NewClassWidget : public QWidget { Q_DISABLE_COPY(NewClassWidget) - Q_OBJECT - Q_PROPERTY(bool namespacesEnabled READ namespacesEnabled WRITE setNamespacesEnabled DESIGNABLE true) + Q_OBJECT Q_PROPERTY(bool namespacesEnabled READ namespacesEnabled WRITE setNamespacesEnabled DESIGNABLE true) Q_PROPERTY(bool baseClassInputVisible READ isBaseClassInputVisible WRITE setBaseClassInputVisible DESIGNABLE true) Q_PROPERTY(bool baseClassEditable READ isBaseClassEditable WRITE setBaseClassEditable DESIGNABLE false) Q_PROPERTY(bool formInputVisible READ isFormInputVisible WRITE setFormInputVisible DESIGNABLE true) @@ -161,7 +158,6 @@ private: QString fixSuffix(const QString &suffix); NewClassWidgetPrivate *m_d; }; - } // namespace Utils #endif // NEWCLASSWIDGET_H diff --git a/ground/openpilotgcs/src/libs/utils/parameteraction.cpp b/ground/openpilotgcs/src/libs/utils/parameteraction.cpp index 88a145c6b..74e5cde8d 100644 --- a/ground/openpilotgcs/src/libs/utils/parameteraction.cpp +++ b/ground/openpilotgcs/src/libs/utils/parameteraction.cpp @@ -4,42 +4,40 @@ * @file parameteraction.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "parameteraction.h" namespace Utils { - ParameterAction::ParameterAction(const QString &emptyText, - const QString ¶meterText, - EnablingMode mode, - QObject* parent) : + const QString ¶meterText, + EnablingMode mode, + QObject *parent) : QAction(emptyText, parent), m_emptyText(emptyText), m_parameterText(parameterText), m_enablingMode(mode) -{ -} +{} QString ParameterAction::emptyText() const { @@ -74,13 +72,14 @@ void ParameterAction::setEnablingMode(EnablingMode m) void ParameterAction::setParameter(const QString &p) { const bool enabled = !p.isEmpty(); + if (enabled) { setText(m_parameterText.arg(p)); } else { setText(m_emptyText); } - if (m_enablingMode == EnabledWithParameter) + if (m_enablingMode == EnabledWithParameter) { setEnabled(enabled); + } } - } diff --git a/ground/openpilotgcs/src/libs/utils/parameteraction.h b/ground/openpilotgcs/src/libs/utils/parameteraction.h index 599c53a5f..5c36f84d9 100644 --- a/ground/openpilotgcs/src/libs/utils/parameteraction.h +++ b/ground/openpilotgcs/src/libs/utils/parameteraction.h @@ -4,25 +4,25 @@ * @file parameteraction.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,6 @@ #include namespace Utils { - /* ParameterAction: Intended for actions that act on a 'current', * string-type parameter (typically file name) and have 2 states: * 1) displaying "Do XX" (empty text) @@ -44,8 +43,7 @@ namespace Utils { * The text passed in should already be translated; parameterText * should contain a %1 where the parameter is to be inserted. */ -class QTCREATOR_UTILS_EXPORT ParameterAction : public QAction -{ +class QTCREATOR_UTILS_EXPORT ParameterAction : public QAction { Q_ENUMS(EnablingMode) Q_PROPERTY(QString emptyText READ emptyText WRITE setEmptyText) Q_PROPERTY(QString parameterText READ parameterText WRITE setParameterText) @@ -57,7 +55,7 @@ public: explicit ParameterAction(const QString &emptyText, const QString ¶meterText, EnablingMode em = AlwaysEnabled, - QObject* parent = 0); + QObject *parent = 0); QString emptyText() const; void setEmptyText(const QString &); @@ -76,7 +74,6 @@ private: QString m_parameterText; EnablingMode m_enablingMode; }; - } #endif // PARAMETERACTION_H diff --git a/ground/openpilotgcs/src/libs/utils/pathchooser.cpp b/ground/openpilotgcs/src/libs/utils/pathchooser.cpp index 426b8b845..e166c66da 100644 --- a/ground/openpilotgcs/src/libs/utils/pathchooser.cpp +++ b/ground/openpilotgcs/src/libs/utils/pathchooser.cpp @@ -4,25 +4,25 @@ * @file pathchooser.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,18 +43,16 @@ #include #include -/*static*/ const char * const Utils::PathChooser::browseButtonLabel = +/*static*/ const char *const Utils::PathChooser::browseButtonLabel = #ifdef Q_WS_MAC - QT_TRANSLATE_NOOP("Utils::PathChooser", "Choose..."); + QT_TRANSLATE_NOOP("Utils::PathChooser", "Choose..."); #else - QT_TRANSLATE_NOOP("Utils::PathChooser", "Browse..."); + QT_TRANSLATE_NOOP("Utils::PathChooser", "Browse..."); #endif namespace Utils { - // ------------------ PathValidatingLineEdit -class PathValidatingLineEdit : public BaseValidatingLineEdit -{ +class PathValidatingLineEdit : public BaseValidatingLineEdit { public: explicit PathValidatingLineEdit(PathChooser *chooser, QWidget *parent = 0); @@ -69,7 +67,7 @@ PathValidatingLineEdit::PathValidatingLineEdit(PathChooser *chooser, QWidget *pa BaseValidatingLineEdit(parent), m_chooser(chooser) { - QTC_ASSERT(chooser, return); + QTC_ASSERT(chooser, return ); } bool PathValidatingLineEdit::validate(const QString &value, QString *errorMessage) const @@ -78,30 +76,27 @@ bool PathValidatingLineEdit::validate(const QString &value, QString *errorMessag } // ------------------ PathChooserPrivate -struct PathChooserPrivate -{ +struct PathChooserPrivate { PathChooserPrivate(PathChooser *chooser); QHBoxLayout *m_hLayout; PathValidatingLineEdit *m_lineEdit; PathChooser::Kind m_acceptingKind; - QString m_dialogTitleOverride; - QString m_dialogFilter; - QString m_initialBrowsePathOverride; + QString m_dialogTitleOverride; + QString m_dialogFilter; + QString m_initialBrowsePathOverride; }; PathChooserPrivate::PathChooserPrivate(PathChooser *chooser) : m_hLayout(new QHBoxLayout), m_lineEdit(new PathValidatingLineEdit(chooser)), m_acceptingKind(PathChooser::Directory) -{ -} +{} PathChooser::PathChooser(QWidget *parent) : QWidget(parent), m_d(new PathChooserPrivate(this)) { - m_d->m_hLayout->setContentsMargins(0, 0, 0, 0); connect(m_d->m_lineEdit, SIGNAL(validReturnPressed()), this, SIGNAL(returnPressed())); @@ -139,7 +134,7 @@ void PathChooser::addButton(const QString &text, QObject *receiver, const char * QAbstractButton *PathChooser::buttonAtIndex(int index) const { - return findChildren().at(index); + return findChildren().at(index); } QString PathChooser::path() const @@ -157,15 +152,19 @@ void PathChooser::slotBrowse() emit beforeBrowsing(); QString predefined = path(); + if ((predefined.isEmpty() || !QFileInfo(predefined).isDir()) - && !m_d->m_initialBrowsePathOverride.isNull()) { + && !m_d->m_initialBrowsePathOverride.isNull()) { predefined = m_d->m_initialBrowsePathOverride; - if (!QFileInfo(predefined).isDir()) + + if (!QFileInfo(predefined).isDir()) { predefined.clear(); + } } - if (predefined.startsWith(":")) + if (predefined.startsWith(":")) { predefined.clear(); + } // Prompt for a file/dir QString dialogTitle; @@ -173,14 +172,14 @@ void PathChooser::slotBrowse() switch (m_d->m_acceptingKind) { case PathChooser::Directory: newPath = QFileDialog::getExistingDirectory(this, - makeDialogTitle(tr("Choose a directory")), predefined); + makeDialogTitle(tr("Choose a directory")), predefined); break; case PathChooser::File: // fall through case PathChooser::Command: newPath = QFileDialog::getOpenFileName(this, - makeDialogTitle(tr("Choose a file")), predefined, - m_d->m_dialogFilter); + makeDialogTitle(tr("Choose a file")), predefined, + m_d->m_dialogFilter); break; default: @@ -190,8 +189,9 @@ void PathChooser::slotBrowse() // Delete trailing slashes unless it is "/"|"\\", only if (!newPath.isEmpty()) { newPath = QDir::toNativeSeparators(newPath); - if (newPath.size() > 1 && newPath.endsWith(QDir::separator())) + if (newPath.size() > 1 && newPath.endsWith(QDir::separator())) { newPath.truncate(newPath.size() - 1); + } setPath(newPath); } @@ -211,8 +211,9 @@ QString PathChooser::errorMessage() const bool PathChooser::validatePath(const QString &path, QString *errorMessage) { if (path.isEmpty()) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The path must not be empty."); + } return false; } @@ -224,8 +225,9 @@ bool PathChooser::validatePath(const QString &path, QString *errorMessage) case PathChooser::Directory: // fall through case PathChooser::File: if (!fi.exists()) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The path '%1' does not exist.").arg(path); + } return false; } break; @@ -239,16 +241,18 @@ bool PathChooser::validatePath(const QString &path, QString *errorMessage) switch (m_d->m_acceptingKind) { case PathChooser::Directory: if (!isDir) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The path '%1' is not a directory.").arg(path); + } return false; } break; case PathChooser::File: if (isDir) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The path '%1' is not a file.").arg(path); + } return false; } break; @@ -277,8 +281,10 @@ QString PathChooser::homePath() // does not let people actually display the contents of their home // directory. Alternatively, create a QtCreator-specific directory? return QDesktopServices::storageLocation(QDesktopServices::DocumentsLocation); + #else return QDir::homePath(); + #endif } @@ -319,10 +325,10 @@ void PathChooser::setInitialBrowsePathBackup(const QString &path) QString PathChooser::makeDialogTitle(const QString &title) { - if (m_d->m_dialogTitleOverride.isNull()) + if (m_d->m_dialogTitleOverride.isNull()) { return title; - else + } else { return m_d->m_dialogTitleOverride; + } } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/pathchooser.h b/ground/openpilotgcs/src/libs/utils/pathchooser.h index 54a85ed34..3da12a7a0 100644 --- a/ground/openpilotgcs/src/libs/utils/pathchooser.h +++ b/ground/openpilotgcs/src/libs/utils/pathchooser.h @@ -4,25 +4,25 @@ * @file pathchooser.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,24 +35,22 @@ #include namespace Utils { - struct PathChooserPrivate; /** * A control that let's the user choose a path, consisting of a QLineEdit and * a "Browse" button. Has some validation logic for embedding into QWizardPage. */ -class QTCREATOR_UTILS_EXPORT PathChooser : public QWidget -{ +class QTCREATOR_UTILS_EXPORT PathChooser : public QWidget { Q_DISABLE_COPY(PathChooser) Q_OBJECT - Q_ENUMS(Kind) + Q_ENUMS(Kind) Q_PROPERTY(QString path READ path WRITE setPath DESIGNABLE true) Q_PROPERTY(QString promptDialogTitle READ promptDialogTitle WRITE setPromptDialogTitle DESIGNABLE true) Q_PROPERTY(Kind expectedKind READ expectedKind WRITE setExpectedKind DESIGNABLE true) public: - static const char * const browseButtonLabel; + static const char *const browseButtonLabel; explicit PathChooser(QWidget *parent = 0); virtual ~PathChooser(); @@ -114,7 +112,6 @@ private slots: private: PathChooserPrivate *m_d; }; - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/pathlisteditor.cpp b/ground/openpilotgcs/src/libs/utils/pathlisteditor.cpp index 9f1459727..944dd6195 100644 --- a/ground/openpilotgcs/src/libs/utils/pathlisteditor.cpp +++ b/ground/openpilotgcs/src/libs/utils/pathlisteditor.cpp @@ -4,25 +4,25 @@ * @file pathlisteditor.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -46,7 +46,6 @@ #include namespace Utils { - // ------------ PathListPlainTextEdit: // Replaces the platform separator ';',':' by '\n' // when inserting, allowing for pasting in paths @@ -56,7 +55,7 @@ class PathListPlainTextEdit : public QPlainTextEdit { public: explicit PathListPlainTextEdit(QWidget *parent = 0); protected: - virtual void insertFromMimeData (const QMimeData *source); + virtual void insertFromMimeData(const QMimeData *source); }; PathListPlainTextEdit::PathListPlainTextEdit(QWidget *parent) : @@ -85,22 +84,22 @@ void PathListPlainTextEdit::insertFromMimeData(const QMimeData *source) struct PathListEditorPrivate { PathListEditorPrivate(); - QHBoxLayout *layout; - QVBoxLayout *buttonLayout; - QToolButton *toolButton; + QHBoxLayout *layout; + QVBoxLayout *buttonLayout; + QToolButton *toolButton; QMenu *buttonMenu; QPlainTextEdit *edit; - QSignalMapper *envVarMapper; + QSignalMapper *envVarMapper; QString fileDialogTitle; }; PathListEditorPrivate::PathListEditorPrivate() : - layout(new QHBoxLayout), - buttonLayout(new QVBoxLayout), - toolButton(new QToolButton), - buttonMenu(new QMenu), - edit(new PathListPlainTextEdit), - envVarMapper(0) + layout(new QHBoxLayout), + buttonLayout(new QVBoxLayout), + toolButton(new QToolButton), + buttonMenu(new QMenu), + edit(new PathListPlainTextEdit), + envVarMapper(0) { layout->setMargin(0); layout->addWidget(edit); @@ -110,8 +109,8 @@ PathListEditorPrivate::PathListEditorPrivate() : } PathListEditor::PathListEditor(QWidget *parent) : - QWidget(parent), - m_d(new PathListEditorPrivate) + QWidget(parent), + m_d(new PathListEditorPrivate) { setLayout(m_d->layout); m_d->toolButton->setPopupMode(QToolButton::MenuButtonPopup); @@ -129,28 +128,32 @@ PathListEditor::~PathListEditor() delete m_d; } -static inline QAction *createAction(QObject *parent, const QString &text, QObject * receiver, const char *slotFunc) +static inline QAction *createAction(QObject *parent, const QString &text, QObject *receiver, const char *slotFunc) { QAction *rc = new QAction(text, parent); QObject::connect(rc, SIGNAL(triggered()), receiver, slotFunc); + return rc; } -QAction *PathListEditor::addAction(const QString &text, QObject * receiver, const char *slotFunc) +QAction *PathListEditor::addAction(const QString &text, QObject *receiver, const char *slotFunc) { QAction *rc = createAction(this, text, receiver, slotFunc); + m_d->buttonMenu->addAction(rc); return rc; } -QAction *PathListEditor::insertAction(int index /* -1 */, const QString &text, QObject * receiver, const char *slotFunc) +QAction *PathListEditor::insertAction(int index /* -1 */, const QString &text, QObject *receiver, const char *slotFunc) { // Find the 'before' action QAction *beforeAction = 0; + if (index >= 0) { - const QList actions = m_d->buttonMenu->actions(); - if (index < actions.size()) + const QList actions = m_d->buttonMenu->actions(); + if (index < actions.size()) { beforeAction = actions.at(index); + } } QAction *rc = createAction(this, text, receiver, slotFunc); if (beforeAction) { @@ -174,13 +177,16 @@ QString PathListEditor::pathListString() const QStringList PathListEditor::pathList() const { const QString text = m_d->edit->toPlainText().trimmed(); - if (text.isEmpty()) + + if (text.isEmpty()) { return QStringList(); + } // trim each line QStringList rc = text.split(QLatin1Char('\n'), QString::SkipEmptyParts); const QStringList::iterator end = rc.end(); - for (QStringList::iterator it = rc.begin(); it != end; ++it) + for (QStringList::iterator it = rc.begin(); it != end; ++it) { *it = it->trimmed(); + } return rc; } @@ -221,15 +227,19 @@ void PathListEditor::clear() void PathListEditor::slotAdd() { const QString dir = QFileDialog::getExistingDirectory(this, m_d->fileDialogTitle); - if (!dir.isEmpty()) + + if (!dir.isEmpty()) { appendPath(QDir::toNativeSeparators(dir)); + } } void PathListEditor::slotInsert() { const QString dir = QFileDialog::getExistingDirectory(this, m_d->fileDialogTitle); - if (!dir.isEmpty()) + + if (!dir.isEmpty()) { insertPathAtCursor(QDir::toNativeSeparators(dir)); + } } QChar PathListEditor::separator() @@ -269,9 +279,10 @@ void PathListEditor::insertPathAtCursor(const QString &path) { // If the cursor is at an empty line or at end(), // just insert. Else insert line before - QTextCursor cursor = m_d->edit->textCursor(); - QTextBlock block = cursor.block(); + QTextCursor cursor = m_d->edit->textCursor(); + QTextBlock block = cursor.block(); const bool needNewLine = !block.text().isEmpty(); + if (needNewLine) { cursor.movePosition(QTextCursor::StartOfLine, QTextCursor::MoveAnchor); cursor.insertBlock(); @@ -287,8 +298,10 @@ void PathListEditor::insertPathAtCursor(const QString &path) void PathListEditor::appendPath(const QString &path) { QString paths = text().trimmed(); - if (!paths.isEmpty()) + + if (!paths.isEmpty()) { paths += QLatin1Char('\n'); + } paths += path; setText(paths); } @@ -297,14 +310,15 @@ void PathListEditor::deletePathAtCursor() { // Delete current line QTextCursor cursor = m_d->edit->textCursor(); + if (cursor.block().isValid()) { cursor.movePosition(QTextCursor::StartOfLine, QTextCursor::MoveAnchor); // Select down or until end of [last] line - if (!cursor.movePosition(QTextCursor::Down, QTextCursor::KeepAnchor)) + if (!cursor.movePosition(QTextCursor::Down, QTextCursor::KeepAnchor)) { cursor.movePosition(QTextCursor::EndOfLine, QTextCursor::KeepAnchor); + } cursor.removeSelectedText(); m_d->edit->setTextCursor(cursor); } } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/pathlisteditor.h b/ground/openpilotgcs/src/libs/utils/pathlisteditor.h index edd3dbbcb..8bdec3c01 100644 --- a/ground/openpilotgcs/src/libs/utils/pathlisteditor.h +++ b/ground/openpilotgcs/src/libs/utils/pathlisteditor.h @@ -4,25 +4,25 @@ * @file pathlisteditor.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,7 +39,6 @@ class QAction; QT_END_NAMESPACE namespace Utils { - struct PathListEditorPrivate; /** @@ -53,11 +52,9 @@ struct PathListEditorPrivate; * by new line characters for convenience. */ -class QTCREATOR_UTILS_EXPORT PathListEditor : public QWidget -{ +class QTCREATOR_UTILS_EXPORT PathListEditor : public QWidget { Q_DISABLE_COPY(PathListEditor) - Q_OBJECT - Q_PROPERTY(QStringList pathList READ pathList WRITE setPathList DESIGNABLE true) + Q_OBJECT Q_PROPERTY(QStringList pathList READ pathList WRITE setPathList DESIGNABLE true) Q_PROPERTY(QString fileDialogTitle READ fileDialogTitle WRITE setFileDialogTitle DESIGNABLE true) public: @@ -83,8 +80,8 @@ public slots: protected: // Index after which to insert further "Add" actions static int lastAddActionIndex(); - QAction *insertAction(int index /* -1 */, const QString &text, QObject * receiver, const char *slotFunc); - QAction *addAction(const QString &text, QObject * receiver, const char *slotFunc); + QAction *insertAction(int index /* -1 */, const QString &text, QObject *receiver, const char *slotFunc); + QAction *addAction(const QString &text, QObject *receiver, const char *slotFunc); QString text() const; void setText(const QString &); @@ -101,7 +98,6 @@ private slots: private: PathListEditorPrivate *m_d; }; - } // namespace Utils #endif // PATHLISTEDITOR_H diff --git a/ground/openpilotgcs/src/libs/utils/pathutils.cpp b/ground/openpilotgcs/src/libs/utils/pathutils.cpp index 3c06cd497..f3cc27344 100644 --- a/ground/openpilotgcs/src/libs/utils/pathutils.cpp +++ b/ground/openpilotgcs/src/libs/utils/pathutils.cpp @@ -32,112 +32,111 @@ namespace Utils { +PathUtils::PathUtils() +{} - PathUtils::PathUtils() - { +/** + Returns the base path of the share directory. - } - - /** - Returns the base path of the share directory. - - Path is in Qt/Unix conventions, separated by "/". - */ + Path is in Qt/Unix conventions, separated by "/". + */ QString PathUtils::GetDataPath() { // This routine works with "/" as the standard: // Figure out root: Up one from 'bin' QDir rootDir = QApplication::applicationDirPath(); + rootDir.cdUp(); const QString rootDirPath = rootDir.canonicalPath(); QString dataPath = rootDirPath; dataPath += QLatin1Char('/'); dataPath += QLatin1String(GCS_DATA_BASENAME); dataPath += QLatin1Char('/'); - return dataPath; + return dataPath; } /** - Cuts the standard data path from the 'path' argument. If path does not start -with the standard data path, then do nothing. + Cuts the standard data path from the 'path' argument. If path does not start + with the standard data path, then do nothing. Always returns a path converted to "/". - */ + */ QString PathUtils::RemoveDataPath(QString path) { // Depending on the platform, we might get either "/" or "\" // so we need to go to the standard ("/") QString goodPath = QDir::fromNativeSeparators(path); + if (goodPath.startsWith(GetDataPath())) { - int i = goodPath.length()- GetDataPath().length(); + int i = goodPath.length() - GetDataPath().length(); return QString("%%DATAPATH%%") + goodPath.right(i); - } else + } else { return goodPath; + } } /** - Inserts the data path (only if the path starts with %%DATAPATH%%) + Inserts the data path (only if the path starts with %%DATAPATH%%) - Returns a "/" or "\" separated path depending on platform conventions. - */ + Returns a "/" or "\" separated path depending on platform conventions. + */ QString PathUtils::InsertDataPath(QString path) { - if (path.startsWith(QString("%%DATAPATH%%"))) - { + if (path.startsWith(QString("%%DATAPATH%%"))) { QString newPath = GetDataPath(); - newPath += path.right(path.length()-12); + newPath += path.right(path.length() - 12); return QDir::toNativeSeparators(newPath); } return QDir::toNativeSeparators(path); } /** - Gets a standard user-writable location for the system - */ + Gets a standard user-writable location for the system + */ QString PathUtils::GetStoragePath() { // This routine works with "/" as the standard: // Work out where the settings are stored on the machine - QSettings set(XmlConfig::XmlSettingsFormat, QSettings::UserScope,QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS_config")); + QSettings set(XmlConfig::XmlSettingsFormat, QSettings::UserScope, QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS_config")); QFileInfo f(set.fileName()); QDir dir(f.absoluteDir()); const QString homeDirPath = dir.canonicalPath(); QString storagePath = homeDirPath; + storagePath += QLatin1Char('/'); // storagePath += QLatin1String("OpenPilot"); // storagePath += QLatin1Char('/'); - return storagePath; + return storagePath; } /** - Removes the standard storage path and replace with a tag - */ + Removes the standard storage path and replace with a tag + */ QString PathUtils::RemoveStoragePath(QString path) { // Depending on the platform, we might get either "/" or "\" // so we need to go to the standard ("/") QString goodPath = QDir::fromNativeSeparators(path); + if (goodPath.startsWith(GetStoragePath())) { - int i = goodPath.length()- GetStoragePath().length(); + int i = goodPath.length() - GetStoragePath().length(); return QString("%%STOREPATH%%") + goodPath.right(i); - } else + } else { return goodPath; + } } /** - Inserts the standard storage path is there is a storage path tag - */ + Inserts the standard storage path is there is a storage path tag + */ QString PathUtils::InsertStoragePath(QString path) { - if (path.startsWith(QString("%%STOREPATH%%"))) - { + if (path.startsWith(QString("%%STOREPATH%%"))) { QString newPath = GetStoragePath(); - newPath += path.right(path.length()-13); + newPath += path.right(path.length() - 13); return QDir::toNativeSeparators(newPath); } return QDir::toNativeSeparators(path); - } - } diff --git a/ground/openpilotgcs/src/libs/utils/pathutils.h b/ground/openpilotgcs/src/libs/utils/pathutils.h index 6cd8b36c3..0a0b82f09 100644 --- a/ground/openpilotgcs/src/libs/utils/pathutils.h +++ b/ground/openpilotgcs/src/libs/utils/pathutils.h @@ -36,9 +36,7 @@ #include namespace Utils { - -class QTCREATOR_UTILS_EXPORT PathUtils -{ +class QTCREATOR_UTILS_EXPORT PathUtils { public: PathUtils(); QString GetDataPath(); @@ -48,9 +46,7 @@ public: QString GetStoragePath(); QString RemoveStoragePath(QString path); QString InsertStoragePath(QString path); - }; - } #endif /* PATHUTILS_H */ diff --git a/ground/openpilotgcs/src/libs/utils/projectintropage.cpp b/ground/openpilotgcs/src/libs/utils/projectintropage.cpp index 93be86a8c..2eafc29d9 100644 --- a/ground/openpilotgcs/src/libs/utils/projectintropage.cpp +++ b/ground/openpilotgcs/src/libs/utils/projectintropage.cpp @@ -4,25 +4,25 @@ * @file projectintropage.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include namespace Utils { - -struct ProjectIntroPagePrivate -{ +struct ProjectIntroPagePrivate { ProjectIntroPagePrivate(); Ui::ProjectIntroPage m_ui; bool m_complete; @@ -47,13 +45,12 @@ struct ProjectIntroPagePrivate const QString m_hintStyleSheet; }; -ProjectIntroPagePrivate:: ProjectIntroPagePrivate() : +ProjectIntroPagePrivate::ProjectIntroPagePrivate() : m_complete(false), m_errorStyleSheet(QLatin1String("background : red;")), m_warningStyleSheet(QLatin1String("background : yellow;")), m_hintStyleSheet() -{ -} +{} ProjectIntroPage::ProjectIntroPage(QWidget *parent) : QWizardPage(parent), @@ -112,6 +109,7 @@ void ProjectIntroPage::setDescription(const QString &description) void ProjectIntroPage::changeEvent(QEvent *e) { QWizardPage::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: m_d->m_ui.retranslateUi(this); @@ -140,6 +138,7 @@ bool ProjectIntroPage::validate() case BaseValidatingLineEdit::Invalid: displayStatusMessage(Error, m_d->m_ui.nameLineEdit->errorMessage()); return false; + case BaseValidatingLineEdit::DisplayingInitialText: break; case BaseValidatingLineEdit::Valid: @@ -169,6 +168,7 @@ bool ProjectIntroPage::validate() void ProjectIntroPage::slotChanged() { const bool newComplete = validate(); + if (newComplete != m_d->m_complete) { m_d->m_complete = newComplete; emit completeChanged(); @@ -177,8 +177,9 @@ void ProjectIntroPage::slotChanged() void ProjectIntroPage::slotActivated() { - if (m_d->m_complete) + if (m_d->m_complete) { emit activated(); + } } bool ProjectIntroPage::validateProjectDirectory(const QString &name, QString *errorMessage) @@ -206,5 +207,4 @@ void ProjectIntroPage::hideStatusLabel() { displayStatusMessage(Hint, QString()); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/projectintropage.h b/ground/openpilotgcs/src/libs/utils/projectintropage.h index eee66c806..03a2495e1 100644 --- a/ground/openpilotgcs/src/libs/utils/projectintropage.h +++ b/ground/openpilotgcs/src/libs/utils/projectintropage.h @@ -4,25 +4,25 @@ * @file projectintropage.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,6 @@ #include namespace Utils { - struct ProjectIntroPagePrivate; /* Standard wizard page for a single file letting the user choose name @@ -50,13 +49,11 @@ struct ProjectIntroPagePrivate; * layout, otherwise, QWizard will squeeze it due to its strange expanding * hacks. */ -class QTCREATOR_UTILS_EXPORT ProjectIntroPage : public QWizardPage -{ - Q_OBJECT - Q_DISABLE_COPY(ProjectIntroPage) +class QTCREATOR_UTILS_EXPORT ProjectIntroPage : public QWizardPage { + Q_OBJECT Q_DISABLE_COPY(ProjectIntroPage) Q_PROPERTY(QString description READ description WRITE setPath DESIGNABLE true) Q_PROPERTY(QString path READ path WRITE setPath DESIGNABLE true) - Q_PROPERTY(QString name READ name WRITE setName DESIGNABLE true) + Q_PROPERTY(QString name READ name WRITE setName DESIGNABLE true) public: explicit ProjectIntroPage(QWidget *parent = 0); virtual ~ProjectIntroPage(); @@ -97,7 +94,6 @@ private: ProjectIntroPagePrivate *m_d; }; - } // namespace Utils #endif // PROJECTINTROPAGE_H diff --git a/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.cpp b/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.cpp index 5819c384d..56f9ebc4a 100644 --- a/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.cpp +++ b/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.cpp @@ -4,25 +4,25 @@ * @file projectnamevalidatinglineedit.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,24 +30,24 @@ #include "filenamevalidatinglineedit.h" namespace Utils { - ProjectNameValidatingLineEdit::ProjectNameValidatingLineEdit(QWidget *parent) - : BaseValidatingLineEdit(parent) -{ -} + : BaseValidatingLineEdit(parent) +{} bool ProjectNameValidatingLineEdit::validateProjectName(const QString &name, QString *errorMessage /* = 0*/) { // Validation is file name + checking for dots - if (!FileNameValidatingLineEdit::validateFileName(name, false, errorMessage)) + if (!FileNameValidatingLineEdit::validateFileName(name, false, errorMessage)) { return false; + } // We don't want dots in the directory name for some legacy Windows // reason. Since we are cross-platform, we generally disallow it. if (name.contains(QLatin1Char('.'))) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The name must not contain the '.'-character."); - return false; + } + return false; } return true; } @@ -56,5 +56,4 @@ bool ProjectNameValidatingLineEdit::validate(const QString &value, QString *erro { return validateProjectName(value, errorMessage); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.h b/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.h index 1e587c50a..0ddde939e 100644 --- a/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.h +++ b/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.h @@ -4,25 +4,25 @@ * @file projectnamevalidatinglineedit.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,11 +32,8 @@ #include "basevalidatinglineedit.h" namespace Utils { - -class QTCREATOR_UTILS_EXPORT ProjectNameValidatingLineEdit : public BaseValidatingLineEdit -{ - Q_OBJECT - Q_DISABLE_COPY(ProjectNameValidatingLineEdit) +class QTCREATOR_UTILS_EXPORT ProjectNameValidatingLineEdit : public BaseValidatingLineEdit { + Q_OBJECT Q_DISABLE_COPY(ProjectNameValidatingLineEdit) public: explicit ProjectNameValidatingLineEdit(QWidget *parent = 0); @@ -46,7 +43,6 @@ public: protected: virtual bool validate(const QString &value, QString *errorMessage) const; }; - } // namespace Utils #endif // PROJECTNAMEVALIDATINGLINEEDIT_H diff --git a/ground/openpilotgcs/src/libs/utils/qtcassert.h b/ground/openpilotgcs/src/libs/utils/qtcassert.h index 01b10345b..e793fb268 100644 --- a/ground/openpilotgcs/src/libs/utils/qtcassert.h +++ b/ground/openpilotgcs/src/libs/utils/qtcassert.h @@ -4,25 +4,25 @@ * @file qtcassert.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,13 +32,13 @@ #include #define QTC_ASSERT_STRINGIFY_INTERNAL(x) #x -#define QTC_ASSERT_STRINGIFY(x) QTC_ASSERT_STRINGIFY_INTERNAL(x) +#define QTC_ASSERT_STRINGIFY(x) QTC_ASSERT_STRINGIFY_INTERNAL(x) // we do not use the 'do {...} while (0)' idiom here to be able to use // 'break' and 'continue' as 'actions'. #define QTC_ASSERT(cond, action) \ - if(cond){}else{qDebug()<<"ASSERTION " #cond " FAILED AT " __FILE__ ":" QTC_ASSERT_STRINGIFY(__LINE__);action;} + if (cond) {} \ + else { qDebug() << "ASSERTION " #cond " FAILED AT " __FILE__ ":" QTC_ASSERT_STRINGIFY(__LINE__); action; } #endif // QTC_ASSERT_H - diff --git a/ground/openpilotgcs/src/libs/utils/qtcolorbutton.cpp b/ground/openpilotgcs/src/libs/utils/qtcolorbutton.cpp index aab8d71c0..d05368ca6 100644 --- a/ground/openpilotgcs/src/libs/utils/qtcolorbutton.cpp +++ b/ground/openpilotgcs/src/libs/utils/qtcolorbutton.cpp @@ -4,25 +4,25 @@ * @file qtcolorbutton.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include namespace Utils { - -class QtColorButtonPrivate -{ +class QtColorButtonPrivate { QtColorButton *q_ptr; Q_DECLARE_PUBLIC(QtColorButton) public: @@ -58,19 +56,23 @@ public: void QtColorButtonPrivate::slotEditColor() { QColor newColor; + if (m_alphaAllowed) { bool ok; const QRgb rgba = QColorDialog::getRgba(m_color.rgba(), &ok, q_ptr); - if (!ok) + if (!ok) { return; + } newColor = QColor::fromRgba(rgba); } else { newColor = QColorDialog::getColor(m_color, q_ptr); - if (!newColor.isValid()) + if (!newColor.isValid()) { return; + } } - if (newColor == q_ptr->color()) + if (newColor == q_ptr->color()) { return; + } q_ptr->setColor(newColor); emit q_ptr->colorChanged(m_color); } @@ -78,8 +80,9 @@ void QtColorButtonPrivate::slotEditColor() QColor QtColorButtonPrivate::shownColor() const { #ifndef QT_NO_DRAGANDDROP - if (m_dragging) + if (m_dragging) { return m_dragColor; + } #endif return m_color; } @@ -93,6 +96,7 @@ QPixmap QtColorButtonPrivate::generatePixmap() const QPixmap pm(2 * pixSize, 2 * pixSize); QPainter pmp(&pm); + pmp.fillRect(0, 0, pixSize, pixSize, Qt::lightGray); pmp.fillRect(pixSize, pixSize, pixSize, pixSize, Qt::lightGray); pmp.fillRect(0, pixSize, pixSize, pixSize, Qt::darkGray); @@ -102,7 +106,7 @@ QPixmap QtColorButtonPrivate::generatePixmap() const QPainter p(&pix); int corr = 1; - QRect r = pix.rect().adjusted(corr, corr, -corr, -corr); + QRect r = pix.rect().adjusted(corr, corr, -corr, -corr); p.setBrushOrigin((r.width() % pixSize + pixSize) / 2 + corr, (r.height() % pixSize + pixSize) / 2 + corr); p.fillRect(r, br); @@ -121,7 +125,7 @@ QtColorButton::QtColorButton(QWidget *parent) { d_ptr = new QtColorButtonPrivate; d_ptr->q_ptr = this; - d_ptr->m_dragging = false; + d_ptr->m_dragging = false; d_ptr->m_backgroundCheckered = true; d_ptr->m_alphaAllowed = true; @@ -138,8 +142,9 @@ QtColorButton::~QtColorButton() void QtColorButton::setColor(const QColor &color) { - if (d_ptr->m_color == color) + if (d_ptr->m_color == color) { return; + } d_ptr->m_color = color; update(); } @@ -151,8 +156,9 @@ QColor QtColorButton::color() const void QtColorButton::setBackgroundCheckered(bool checkered) { - if (d_ptr->m_backgroundCheckered == checkered) + if (d_ptr->m_backgroundCheckered == checkered) { return; + } d_ptr->m_backgroundCheckered = checkered; update(); } @@ -175,8 +181,10 @@ bool QtColorButton::isAlphaAllowed() const void QtColorButton::paintEvent(QPaintEvent *event) { QToolButton::paintEvent(event); - if (!isEnabled()) + + if (!isEnabled()) { return; + } const int pixSize = 10; QBrush br(d_ptr->shownColor()); @@ -197,21 +205,21 @@ void QtColorButton::paintEvent(QPaintEvent *event) p.setBrushOrigin((r.width() % pixSize + pixSize) / 2 + corr, (r.height() % pixSize + pixSize) / 2 + corr); p.fillRect(r, br); - //const int adjX = qRound(r.width() / 4.0); - //const int adjY = qRound(r.height() / 4.0); - //p.fillRect(r.adjusted(adjX, adjY, -adjX, -adjY), - // QColor(d_ptr->shownColor().rgb())); + // const int adjX = qRound(r.width() / 4.0); + // const int adjY = qRound(r.height() / 4.0); + // p.fillRect(r.adjusted(adjX, adjY, -adjX, -adjY), + // QColor(d_ptr->shownColor().rgb())); /* - p.fillRect(r.adjusted(0, r.height() * 3 / 4, 0, 0), + p.fillRect(r.adjusted(0, r.height() * 3 / 4, 0, 0), QColor(d_ptr->shownColor().rgb())); - p.fillRect(r.adjusted(0, 0, 0, -r.height() * 3 / 4), + p.fillRect(r.adjusted(0, 0, 0, -r.height() * 3 / 4), QColor(d_ptr->shownColor().rgb())); - */ + */ /* - const QColor frameColor0(0, 0, 0, qRound(0.2 * (0xFF - d_ptr->shownColor().alpha()))); - p.setPen(frameColor0); - p.drawRect(r.adjusted(adjX, adjY, -adjX - 1, -adjY - 1)); - */ + const QColor frameColor0(0, 0, 0, qRound(0.2 * (0xFF - d_ptr->shownColor().alpha()))); + p.setPen(frameColor0); + p.drawRect(r.adjusted(adjX, adjY, -adjX - 1, -adjY - 1)); + */ const QColor frameColor1(0, 0, 0, 26); p.setPen(frameColor1); @@ -224,8 +232,9 @@ void QtColorButton::paintEvent(QPaintEvent *event) void QtColorButton::mousePressEvent(QMouseEvent *event) { #ifndef QT_NO_DRAGANDDROP - if (event->button() == Qt::LeftButton) + if (event->button() == Qt::LeftButton) { d_ptr->m_dragStart = event->pos(); + } #endif QToolButton::mousePressEvent(event); } @@ -234,7 +243,7 @@ void QtColorButton::mouseMoveEvent(QMouseEvent *event) { #ifndef QT_NO_DRAGANDDROP if (event->buttons() & Qt::LeftButton && - (d_ptr->m_dragStart - event->pos()).manhattanLength() > QApplication::startDragDistance()) { + (d_ptr->m_dragStart - event->pos()).manhattanLength() > QApplication::startDragDistance()) { QMimeData *mime = new QMimeData; mime->setColorData(color()); QDrag *drg = new QDrag(this); @@ -253,12 +262,14 @@ void QtColorButton::mouseMoveEvent(QMouseEvent *event) void QtColorButton::dragEnterEvent(QDragEnterEvent *event) { const QMimeData *mime = event->mimeData(); - if (!mime->hasColor()) + + if (!mime->hasColor()) { return; + } event->accept(); d_ptr->m_dragColor = qvariant_cast(mime->colorData()); - d_ptr->m_dragging = true; + d_ptr->m_dragging = true; update(); } @@ -273,13 +284,13 @@ void QtColorButton::dropEvent(QDropEvent *event) { event->accept(); d_ptr->m_dragging = false; - if (d_ptr->m_dragColor == color()) + if (d_ptr->m_dragColor == color()) { return; + } setColor(d_ptr->m_dragColor); emit colorChanged(color()); } -#endif - +#endif // ifndef QT_NO_DRAGANDDROP } // namespace Utils #include "moc_qtcolorbutton.cpp" diff --git a/ground/openpilotgcs/src/libs/utils/qtcolorbutton.h b/ground/openpilotgcs/src/libs/utils/qtcolorbutton.h index 6dd5f5068..8773bcd59 100644 --- a/ground/openpilotgcs/src/libs/utils/qtcolorbutton.h +++ b/ground/openpilotgcs/src/libs/utils/qtcolorbutton.h @@ -4,25 +4,25 @@ * @file qtcolorbutton.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,11 +34,8 @@ #include namespace Utils { - -class QTCREATOR_UTILS_EXPORT QtColorButton : public QToolButton -{ - Q_OBJECT - Q_PROPERTY(bool backgroundCheckered READ isBackgroundCheckered WRITE setBackgroundCheckered) +class QTCREATOR_UTILS_EXPORT QtColorButton : public QToolButton { + Q_OBJECT Q_PROPERTY(bool backgroundCheckered READ isBackgroundCheckered WRITE setBackgroundCheckered) Q_PROPERTY(bool alphaAllowed READ isAlphaAllowed WRITE setAlphaAllowed) public: QtColorButton(QWidget *parent = 0); @@ -72,7 +69,6 @@ private: Q_DISABLE_COPY(QtColorButton) Q_PRIVATE_SLOT(d_ptr, void slotEditColor()) }; - } // namespace Utils #endif // QTCOLORBUTTON_H diff --git a/ground/openpilotgcs/src/libs/utils/qwineventnotifier_p.h b/ground/openpilotgcs/src/libs/utils/qwineventnotifier_p.h index 459cd6730..1db97c6bc 100644 --- a/ground/openpilotgcs/src/libs/utils/qwineventnotifier_p.h +++ b/ground/openpilotgcs/src/libs/utils/qwineventnotifier_p.h @@ -43,8 +43,8 @@ #define QWINEVENTNOTIFIER_P_H // -// W A R N I N G -// ------------- +// W A R N I N G +// ------------- // // This file is not part of the Qt API. It exists for the convenience // of other Qt classes. This header file may change from version to @@ -58,10 +58,9 @@ QT_BEGIN_NAMESPACE -class Q_CORE_EXPORT QWinEventNotifier : public QObject -{ +class Q_CORE_EXPORT QWinEventNotifier : public QObject { Q_OBJECT - Q_DECLARE_PRIVATE(QObject) + Q_DECLARE_PRIVATE(QObject) public: explicit QWinEventNotifier(QObject *parent = 0); @@ -80,7 +79,7 @@ Q_SIGNALS: void activated(HANDLE hEvent); protected: - bool event(QEvent * e); + bool event(QEvent *e); private: Q_DISABLE_COPY(QWinEventNotifier) diff --git a/ground/openpilotgcs/src/libs/utils/reloadpromptutils.cpp b/ground/openpilotgcs/src/libs/utils/reloadpromptutils.cpp index 80b237d6b..c7532f7c6 100644 --- a/ground/openpilotgcs/src/libs/utils/reloadpromptutils.cpp +++ b/ground/openpilotgcs/src/libs/utils/reloadpromptutils.cpp @@ -4,25 +4,25 @@ * @file reloadpromptutils.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,34 +34,35 @@ using namespace Utils; -QTCREATOR_UTILS_EXPORT Utils::ReloadPromptAnswer - Utils::reloadPrompt(const QString &fileName, bool modified, QWidget *parent) +QTCREATOR_UTILS_EXPORT Utils::ReloadPromptAnswer Utils::reloadPrompt(const QString &fileName, bool modified, QWidget *parent) { - const QString title = QCoreApplication::translate("Utils::reloadPrompt", "File Changed"); QString msg; - if (modified) + if (modified) { msg = QCoreApplication::translate("Utils::reloadPrompt", "The unsaved file %1 has been changed outside Qt Creator. Do you want to reload it and discard your changes?").arg(QDir::toNativeSeparators(fileName)); - else + } else { msg = QCoreApplication::translate("Utils::reloadPrompt", "The file %1 has changed outside Qt Creator. Do you want to reload it?").arg(QDir::toNativeSeparators(fileName)); + } return reloadPrompt(title, msg, parent); } -QTCREATOR_UTILS_EXPORT Utils::ReloadPromptAnswer - Utils::reloadPrompt(const QString &title, const QString &prompt, QWidget *parent) +QTCREATOR_UTILS_EXPORT Utils::ReloadPromptAnswer Utils::reloadPrompt(const QString &title, const QString &prompt, QWidget *parent) { switch (QMessageBox::question(parent, title, prompt, - QMessageBox::Yes|QMessageBox::YesToAll|QMessageBox::No|QMessageBox::NoToAll, + QMessageBox::Yes | QMessageBox::YesToAll | QMessageBox::No | QMessageBox::NoToAll, QMessageBox::YesToAll)) { case QMessageBox::Yes: - return ReloadCurrent; + return ReloadCurrent; + case QMessageBox::YesToAll: return ReloadAll; + case QMessageBox::No: return ReloadSkipCurrent; + default: break; } diff --git a/ground/openpilotgcs/src/libs/utils/reloadpromptutils.h b/ground/openpilotgcs/src/libs/utils/reloadpromptutils.h index 4c6e328d4..e5bd2f54c 100644 --- a/ground/openpilotgcs/src/libs/utils/reloadpromptutils.h +++ b/ground/openpilotgcs/src/libs/utils/reloadpromptutils.h @@ -4,25 +4,25 @@ * @file reloadpromptutils.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,12 +37,10 @@ class QWidget; QT_END_NAMESPACE namespace Utils { - enum ReloadPromptAnswer { ReloadCurrent, ReloadAll, ReloadSkipCurrent, ReloadNone }; QTCREATOR_UTILS_EXPORT ReloadPromptAnswer reloadPrompt(const QString &fileName, bool modified, QWidget *parent); QTCREATOR_UTILS_EXPORT ReloadPromptAnswer reloadPrompt(const QString &title, const QString &prompt, QWidget *parent); - } // namespace Utils #endif // RELOADPROMPTUTILS_H diff --git a/ground/openpilotgcs/src/libs/utils/savedaction.cpp b/ground/openpilotgcs/src/libs/utils/savedaction.cpp index b3b49fc54..b0e551896 100644 --- a/ground/openpilotgcs/src/libs/utils/savedaction.cpp +++ b/ground/openpilotgcs/src/libs/utils/savedaction.cpp @@ -4,25 +4,25 @@ * @file savedaction.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -54,16 +54,16 @@ using namespace Utils; /*! \class Utils::SavedAction - + \brief The SavedAction class is a helper class for actions with persistent state. \ingroup utils -*/ + */ SavedAction::SavedAction(QObject *parent) - : QAction(parent) + : QAction(parent) { m_widget = 0; connect(this, SIGNAL(triggered(bool)), this, SLOT(actionTriggered(bool))); @@ -74,7 +74,7 @@ SavedAction::SavedAction(QObject *parent) Returns the current value of the object. \sa setValue() -*/ + */ QVariant SavedAction::value() const { return m_value; @@ -86,16 +86,19 @@ QVariant SavedAction::value() const \a doemit is true, the \c valueChanged() signal will be emitted. \sa value() -*/ + */ void SavedAction::setValue(const QVariant &value, bool doemit) { - if (value == m_value) + if (value == m_value) { return; + } m_value = value; - if (this->isCheckable()) + if (this->isCheckable()) { this->setChecked(m_value.toBool()); - if (doemit) + } + if (doemit) { emit valueChanged(m_value); + } } @@ -104,7 +107,7 @@ void SavedAction::setValue(const QVariant &value, bool doemit) in the settings. \sa setDefaultValue() -*/ + */ QVariant SavedAction::defaultValue() const { return m_defaultValue; @@ -116,7 +119,7 @@ QVariant SavedAction::defaultValue() const in the settings. \sa defaultValue() -*/ + */ void SavedAction::setDefaultValue(const QVariant &value) { m_defaultValue = value; @@ -127,7 +130,7 @@ void SavedAction::setDefaultValue(const QVariant &value) Returns the key to be used when accessing the settings. \sa settingsKey() -*/ + */ QString SavedAction::settingsKey() const { return m_settingsKey; @@ -138,7 +141,7 @@ QString SavedAction::settingsKey() const Sets the key to be used when accessing the settings. \sa settingsKey() -*/ + */ void SavedAction::setSettingsKey(const QString &key) { m_settingsKey = key; @@ -149,10 +152,10 @@ void SavedAction::setSettingsKey(const QString &key) Sets the key and group to be used when accessing the settings. \sa settingsKey() -*/ + */ void SavedAction::setSettingsKey(const QString &group, const QString &key) { - m_settingsKey = key; + m_settingsKey = key; m_settingsGroup = group; } @@ -161,7 +164,7 @@ void SavedAction::setSettingsKey(const QString &group, const QString &key) Sets the key to be used when accessing the settings. \sa settingsKey() -*/ + */ QString SavedAction::settingsGroup() const { return m_settingsGroup; @@ -171,7 +174,7 @@ QString SavedAction::settingsGroup() const Sets the group to be used when accessing the settings. \sa settingsGroup() -*/ + */ void SavedAction::setSettingsGroup(const QString &group) { m_settingsGroup = group; @@ -190,9 +193,9 @@ void SavedAction::setTextPattern(const QString &value) QString SavedAction::toString() const { return QLatin1String("value: ") + m_value.toString() - + QLatin1String(" defaultvalue: ") + m_defaultValue.toString() - + QLatin1String(" settingskey: ") + m_settingsGroup - + '/' + m_settingsKey; + + QLatin1String(" defaultvalue: ") + m_defaultValue.toString() + + QLatin1String(" settingskey: ") + m_settingsGroup + + '/' + m_settingsKey; } /*! @@ -210,14 +213,15 @@ QString SavedAction::toString() const with the "%1" placeholder removed will be used. \sa textPattern(), setTextPattern() -*/ + */ QAction *SavedAction::updatedAction(const QString &text0) { QString text = text0; bool enabled = true; + if (!m_textPattern.isEmpty()) { if (text.isEmpty()) { - text = m_textPattern; + text = m_textPattern; text.remove("\"%1\""); text.remove("%1"); enabled = false; @@ -232,94 +236,98 @@ QAction *SavedAction::updatedAction(const QString &text0) } /* - Uses \c settingsGroup() and \c settingsKey() to restore the + Uses \c settingsGroup() and \c settingsKey() to restore the item from \a settings, \sa settingsKey(), settingsGroup(), writeSettings() -*/ + */ void SavedAction::readSettings(QSettings *settings) { - if (m_settingsGroup.isEmpty() || m_settingsKey.isEmpty()) + if (m_settingsGroup.isEmpty() || m_settingsKey.isEmpty()) { return; + } settings->beginGroup(m_settingsGroup); QVariant var = settings->value(m_settingsKey, m_defaultValue); // work around old ini files containing @Invalid() entries - if (isCheckable() && !var.isValid()) + if (isCheckable() && !var.isValid()) { var = false; + } setValue(var); - //qDebug() << "READING: " << var.isValid() << m_settingsKey << " -> " << m_value - // << " (default: " << m_defaultValue << ")" << var; + // qDebug() << "READING: " << var.isValid() << m_settingsKey << " -> " << m_value + // << " (default: " << m_defaultValue << ")" << var; settings->endGroup(); } /* - Uses \c settingsGroup() and \c settingsKey() to write the + Uses \c settingsGroup() and \c settingsKey() to write the item to \a settings, \sa settingsKey(), settingsGroup(), readSettings() -*/ + */ void SavedAction::writeSettings(QSettings *settings) { - if (m_settingsGroup.isEmpty() || m_settingsKey.isEmpty()) + if (m_settingsGroup.isEmpty() || m_settingsKey.isEmpty()) { return; + } settings->beginGroup(m_settingsGroup); settings->setValue(m_settingsKey, m_value); - //qDebug() << "WRITING: " << m_settingsKey << " -> " << toString(); + // qDebug() << "WRITING: " << m_settingsKey << " -> " << toString(); settings->endGroup(); } - + /* A \c SavedAction can be connected to a widget, typically a checkbox, radiobutton, or a lineedit in some configuration dialog. - The widget will retrieve its contents from the SavedAction's + The widget will retrieve its contents from the SavedAction's value, and - depending on the \a ApplyMode - either write changes back immediately, or when \s SavedAction::apply() is called explicitly. \sa apply(), disconnectWidget() -*/ + */ void SavedAction::connectWidget(QWidget *widget, ApplyMode applyMode) { QTC_ASSERT(!m_widget, - qDebug() << "ALREADY CONNECTED: " << widget << m_widget << toString(); return); - m_widget = widget; + qDebug() << "ALREADY CONNECTED: " << widget << m_widget << toString(); return ); + m_widget = widget; m_applyMode = applyMode; - - if (QAbstractButton *button = qobject_cast(widget)) { + + if (QAbstractButton * button = qobject_cast(widget)) { if (button->isCheckable()) { button->setChecked(m_value.toBool()); connect(button, SIGNAL(clicked(bool)), - this, SLOT(checkableButtonClicked(bool))); + this, SLOT(checkableButtonClicked(bool))); } else { connect(button, SIGNAL(clicked()), - this, SLOT(uncheckableButtonClicked())); + this, SLOT(uncheckableButtonClicked())); } - } else if (QSpinBox *spinBox = qobject_cast(widget)) { + } else if (QSpinBox * spinBox = qobject_cast(widget)) { spinBox->setValue(m_value.toInt()); - //qDebug() << "SETTING VALUE" << spinBox->value(); + + // qDebug() << "SETTING VALUE" << spinBox->value(); connect(spinBox, SIGNAL(valueChanged(int)), - this, SLOT(spinBoxValueChanged(int))); + this, SLOT(spinBoxValueChanged(int))); connect(spinBox, SIGNAL(valueChanged(QString)), - this, SLOT(spinBoxValueChanged(QString))); - } else if (QDoubleSpinBox *doubleSpinBox = qobject_cast(widget)) { + this, SLOT(spinBoxValueChanged(QString))); + } else if (QDoubleSpinBox * doubleSpinBox = qobject_cast(widget)) { doubleSpinBox->setValue(m_value.toDouble()); - //qDebug() << "SETTING VALUE" << doubleSpinBox->value(); + // qDebug() << "SETTING VALUE" << doubleSpinBox->value(); connect(doubleSpinBox, SIGNAL(valueChanged(double)), - this, SLOT(doubleSpinBoxValueChanged(double))); + this, SLOT(doubleSpinBoxValueChanged(double))); connect(doubleSpinBox, SIGNAL(valueChanged(QString)), - this, SLOT(doubleSpinBoxValueChanged(QString))); - } else if (QLineEdit *lineEdit = qobject_cast(widget)) { + this, SLOT(doubleSpinBoxValueChanged(QString))); + } else if (QLineEdit * lineEdit = qobject_cast(widget)) { lineEdit->setText(m_value.toString()); - //qDebug() << "SETTING TEXT" << lineEdit->text(); + // qDebug() << "SETTING TEXT" << lineEdit->text(); connect(lineEdit, SIGNAL(editingFinished()), - this, SLOT(lineEditEditingFinished())); - } else if (PathChooser *pathChooser = qobject_cast(widget)) { + this, SLOT(lineEditEditingFinished())); + } else if (PathChooser * pathChooser = qobject_cast(widget)) { pathChooser->setPath(m_value.toString()); connect(pathChooser, SIGNAL(editingFinished()), - this, SLOT(pathChooserEditingFinished())); + this, SLOT(pathChooserEditingFinished())); connect(pathChooser, SIGNAL(browsingFinished()), - this, SLOT(pathChooserEditingFinished())); + this, SLOT(pathChooserEditingFinished())); } else { qDebug() << "Cannot connect widget " << widget << toString(); } @@ -329,7 +337,7 @@ void SavedAction::connectWidget(QWidget *widget, ApplyMode applyMode) Disconnects the \c SavedAction from a widget. \sa apply(), connectWidget() -*/ + */ void SavedAction::disconnectWidget() { m_widget = 0; @@ -337,94 +345,113 @@ void SavedAction::disconnectWidget() void SavedAction::apply(QSettings *s) { - if (QAbstractButton *button = qobject_cast(m_widget)) + if (QAbstractButton * button = qobject_cast(m_widget)) { setValue(button->isChecked()); - else if (QLineEdit *lineEdit = qobject_cast(m_widget)) + } else if (QLineEdit * lineEdit = qobject_cast(m_widget)) { setValue(lineEdit->text()); - else if (QSpinBox *spinBox = qobject_cast(m_widget)) + } else if (QSpinBox * spinBox = qobject_cast(m_widget)) { setValue(spinBox->value()); - else if (QDoubleSpinBox *doubleSpinBox = qobject_cast(m_widget)) + } else if (QDoubleSpinBox * doubleSpinBox = qobject_cast(m_widget)) { setValue(doubleSpinBox->value()); - else if (PathChooser *pathChooser = qobject_cast(m_widget)) + } else if (PathChooser * pathChooser = qobject_cast(m_widget)) { setValue(pathChooser->path()); - if (s) - writeSettings(s); + } + if (s) { + writeSettings(s); + } } void SavedAction::uncheckableButtonClicked() { QAbstractButton *button = qobject_cast(sender()); - QTC_ASSERT(button, return); - //qDebug() << "UNCHECKABLE BUTTON: " << sender(); + + QTC_ASSERT(button, return ); + // qDebug() << "UNCHECKABLE BUTTON: " << sender(); QAction::trigger(); } void SavedAction::checkableButtonClicked(bool) { QAbstractButton *button = qobject_cast(sender()); - QTC_ASSERT(button, return); - //qDebug() << "CHECKABLE BUTTON: " << sender(); - if (m_applyMode == ImmediateApply) + + QTC_ASSERT(button, return ); + // qDebug() << "CHECKABLE BUTTON: " << sender(); + if (m_applyMode == ImmediateApply) { setValue(button->isChecked()); + } } void SavedAction::lineEditEditingFinished() { QLineEdit *lineEdit = qobject_cast(sender()); - QTC_ASSERT(lineEdit, return); - if (m_applyMode == ImmediateApply) + + QTC_ASSERT(lineEdit, return ); + if (m_applyMode == ImmediateApply) { setValue(lineEdit->text()); + } } void SavedAction::spinBoxValueChanged(int value) { QSpinBox *spinBox = qobject_cast(sender()); - QTC_ASSERT(spinBox, return); - if (m_applyMode == ImmediateApply) + + QTC_ASSERT(spinBox, return ); + if (m_applyMode == ImmediateApply) { setValue(value); + } } void SavedAction::spinBoxValueChanged(QString value) { QSpinBox *spinBox = qobject_cast(sender()); - QTC_ASSERT(spinBox, return); - if (m_applyMode == ImmediateApply) + + QTC_ASSERT(spinBox, return ); + if (m_applyMode == ImmediateApply) { setValue(value); + } } void SavedAction::doubleSpinBoxValueChanged(double value) { QDoubleSpinBox *doubleSpinBox = qobject_cast(sender()); - QTC_ASSERT(doubleSpinBox, return); - if (m_applyMode == ImmediateApply) + + QTC_ASSERT(doubleSpinBox, return ); + if (m_applyMode == ImmediateApply) { setValue(value); + } } void SavedAction::doubleSpinBoxValueChanged(QString value) { QDoubleSpinBox *doubleSpinBox = qobject_cast(sender()); - QTC_ASSERT(doubleSpinBox, return); - if (m_applyMode == ImmediateApply) + + QTC_ASSERT(doubleSpinBox, return ); + if (m_applyMode == ImmediateApply) { setValue(value); + } } void SavedAction::pathChooserEditingFinished() { PathChooser *pathChooser = qobject_cast(sender()); - QTC_ASSERT(pathChooser, return); - if (m_applyMode == ImmediateApply) + + QTC_ASSERT(pathChooser, return ); + if (m_applyMode == ImmediateApply) { setValue(pathChooser->path()); + } } void SavedAction::actionTriggered(bool) { - if (isCheckable()) + if (isCheckable()) { setValue(isChecked()); + } if (actionGroup() && actionGroup()->isExclusive()) { // FIXME: should be taken care of more directly - foreach (QAction *act, actionGroup()->actions()) - if (SavedAction *dact = qobject_cast(act)) - dact->setValue(bool(act == this)); + foreach(QAction * act, actionGroup()->actions()) + if (SavedAction * dact = qobject_cast(act)) { + dact->setValue(bool(act == this)); + } } } @@ -444,19 +471,19 @@ void SavedAction::trigger(const QVariant &data) void SavedActionSet::insert(SavedAction *action, QWidget *widget) { m_list.append(action); - if (widget) + if (widget) { action->connectWidget(widget); + } } void SavedActionSet::apply(QSettings *settings) { - foreach (SavedAction *action, m_list) - action->apply(settings); + foreach(SavedAction * action, m_list) + action->apply(settings); } void SavedActionSet::finish() { - foreach (SavedAction *action, m_list) - action->disconnectWidget(); + foreach(SavedAction * action, m_list) + action->disconnectWidget(); } - diff --git a/ground/openpilotgcs/src/libs/utils/savedaction.h b/ground/openpilotgcs/src/libs/utils/savedaction.h index a3eaf0c7f..df624c637 100644 --- a/ground/openpilotgcs/src/libs/utils/savedaction.h +++ b/ground/openpilotgcs/src/libs/utils/savedaction.h @@ -4,25 +4,25 @@ * @file savedaction.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,11 +42,9 @@ class QSettings; QT_END_NAMESPACE namespace Utils { - enum ApplyMode { ImmediateApply, DeferedApply }; -class QTCREATOR_UTILS_EXPORT SavedAction : public QAction -{ +class QTCREATOR_UTILS_EXPORT SavedAction : public QAction { Q_OBJECT public: @@ -71,7 +69,7 @@ public: virtual void readSettings(QSettings *settings); Q_SLOT virtual void writeSettings(QSettings *settings); - + virtual void connectWidget(QWidget *widget, ApplyMode applyMode = DeferedApply); virtual void disconnectWidget(); Q_SLOT virtual void apply(QSettings *settings); @@ -105,8 +103,7 @@ private: ApplyMode m_applyMode; }; -class QTCREATOR_UTILS_EXPORT SavedActionSet -{ +class QTCREATOR_UTILS_EXPORT SavedActionSet { public: SavedActionSet() {} ~SavedActionSet() {} @@ -114,12 +111,14 @@ public: void insert(SavedAction *action, QWidget *widget); void apply(QSettings *settings); void finish(); - void clear() { m_list.clear(); } + void clear() + { + m_list.clear(); + } private: QList m_list; }; - } // namespace Utils #endif // SAVED_ACTION_H diff --git a/ground/openpilotgcs/src/libs/utils/settingsutils.cpp b/ground/openpilotgcs/src/libs/utils/settingsutils.cpp index 4f3180254..c10b9ab42 100644 --- a/ground/openpilotgcs/src/libs/utils/settingsutils.cpp +++ b/ground/openpilotgcs/src/libs/utils/settingsutils.cpp @@ -4,25 +4,25 @@ * @file settingsutils.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,18 +31,18 @@ #include namespace Utils { - QTCREATOR_UTILS_EXPORT QString settingsKey(const QString &category) { QString rc(category); const QChar underscore = QLatin1Char('_'); const int size = rc.size(); + for (int i = 0; i < size; i++) { const QChar c = rc.at(i); - if (!c.isLetterOrNumber() && c != underscore) + if (!c.isLetterOrNumber() && c != underscore) { rc[i] = underscore; + } } return rc; } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/settingsutils.h b/ground/openpilotgcs/src/libs/utils/settingsutils.h index 49b93cb69..be85a69e7 100644 --- a/ground/openpilotgcs/src/libs/utils/settingsutils.h +++ b/ground/openpilotgcs/src/libs/utils/settingsutils.h @@ -4,25 +4,25 @@ * @file settingsutils.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,11 +32,9 @@ #include "utils_global.h" namespace Utils { - // Create a usable settings key from a category, // for example Editor|C++ -> Editor_C__ QTCREATOR_UTILS_EXPORT QString settingsKey(const QString &category); - } // namespace Utils #endif // SETTINGSTUTILS_H diff --git a/ground/openpilotgcs/src/libs/utils/styledbar.cpp b/ground/openpilotgcs/src/libs/utils/styledbar.cpp index 63a27b776..7ceb6709b 100644 --- a/ground/openpilotgcs/src/libs/utils/styledbar.cpp +++ b/ground/openpilotgcs/src/libs/utils/styledbar.cpp @@ -4,25 +4,25 @@ * @file styledbar.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -60,7 +60,7 @@ void StyledBar::paintEvent(QPaintEvent *event) Q_UNUSED(event) QPainter painter(this); QStyleOption option; - option.rect = rect(); + option.rect = rect(); option.state = QStyle::State_Horizontal; style()->drawControl(QStyle::CE_ToolBar, &option, &painter, this); } @@ -76,8 +76,8 @@ void StyledSeparator::paintEvent(QPaintEvent *event) Q_UNUSED(event) QPainter painter(this); QStyleOption option; - option.rect = rect(); - option.state = QStyle::State_Horizontal; + option.rect = rect(); + option.state = QStyle::State_Horizontal; option.palette = palette(); style()->drawPrimitive(QStyle::PE_IndicatorToolBarSeparator, &option, &painter, this); } diff --git a/ground/openpilotgcs/src/libs/utils/styledbar.h b/ground/openpilotgcs/src/libs/utils/styledbar.h index a0144d0a9..e564eb1b6 100644 --- a/ground/openpilotgcs/src/libs/utils/styledbar.h +++ b/ground/openpilotgcs/src/libs/utils/styledbar.h @@ -4,25 +4,25 @@ * @file styledbar.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ #include namespace Utils { - -class QTCREATOR_UTILS_EXPORT StyledBar : public QWidget -{ +class QTCREATOR_UTILS_EXPORT StyledBar : public QWidget { Q_OBJECT public: StyledBar(QWidget *parent = 0); @@ -46,14 +44,12 @@ protected: void paintEvent(QPaintEvent *event); }; -class QTCREATOR_UTILS_EXPORT StyledSeparator : public QWidget -{ +class QTCREATOR_UTILS_EXPORT StyledSeparator : public QWidget { public: StyledSeparator(QWidget *parent = 0); protected: void paintEvent(QPaintEvent *event); }; - } // Utils #endif // STYLEDBAR_H diff --git a/ground/openpilotgcs/src/libs/utils/stylehelper.cpp b/ground/openpilotgcs/src/libs/utils/stylehelper.cpp index 879a0b9b3..5b22e39c5 100644 --- a/ground/openpilotgcs/src/libs/utils/stylehelper.cpp +++ b/ground/openpilotgcs/src/libs/utils/stylehelper.cpp @@ -4,25 +4,25 @@ * @file stylehelper.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,24 +39,25 @@ static int clamp(float x) { const int val = x > 255 ? 255 : static_cast(x); + return val < 0 ? 0 : val; } // Clamps float color values within (0, 255) /* -static int range(float x, int min, int max) -{ + static int range(float x, int min, int max) + { int val = x > max ? max : x; return val < min ? min : val; -} -*/ + } + */ namespace Utils { - QColor StyleHelper::mergedColors(const QColor &colorA, const QColor &colorB, int factor) { const int maxFactor = 100; QColor tmp = colorA; + tmp.setRed((tmp.red() * factor) / maxFactor + (colorB.red() * (maxFactor - factor)) / maxFactor); tmp.setGreen((tmp.green() * factor) / maxFactor + (colorB.green() * (maxFactor - factor)) / maxFactor); tmp.setBlue((tmp.blue() * factor) / maxFactor + (colorB.blue() * (maxFactor - factor)) / maxFactor); @@ -67,14 +68,17 @@ qreal StyleHelper::sidebarFontSize() { #if defined(Q_WS_MAC) return 9; + #else return 7.5; + #endif } QPalette StyleHelper::sidebarFontPalette(const QPalette &original) { QPalette palette = original; + palette.setColor(QPalette::Active, QPalette::Text, panelTextColor()); palette.setColor(QPalette::Active, QPalette::WindowText, panelTextColor()); palette.setColor(QPalette::Inactive, QPalette::Text, panelTextColor().darker()); @@ -84,7 +88,7 @@ QPalette StyleHelper::sidebarFontPalette(const QPalette &original) QColor StyleHelper::panelTextColor() { - //qApp->palette().highlightedText().color(); + // qApp->palette().highlightedText().color(); return Qt::white; } @@ -98,6 +102,7 @@ QColor StyleHelper::baseColor() QColor StyleHelper::highlightColor() { QColor result = baseColor(); + result.setHsv(result.hue(), clamp(result.saturation()), clamp(result.value() * 1.16)); @@ -107,6 +112,7 @@ QColor StyleHelper::highlightColor() QColor StyleHelper::shadowColor() { QColor result = baseColor(); + result.setHsv(result.hue(), clamp(result.saturation() * 1.1), clamp(result.value() * 0.70)); @@ -116,6 +122,7 @@ QColor StyleHelper::shadowColor() QColor StyleHelper::borderColor() { QColor result = baseColor(); + result.setHsv(result.hue(), result.saturation(), result.value() / 2); @@ -126,8 +133,8 @@ void StyleHelper::setBaseColor(const QColor &color) { if (color.isValid() && color != m_baseColor) { m_baseColor = color; - foreach (QWidget *w, QApplication::topLevelWidgets()) - w->update(); + foreach(QWidget * w, QApplication::topLevelWidgets()) + w->update(); } } @@ -135,6 +142,7 @@ static void verticalGradientHelper(QPainter *p, const QRect &spanRect, const QRe { QColor base = StyleHelper::baseColor(); QLinearGradient grad(spanRect.topRight(), spanRect.topLeft()); + grad.setColorAt(0, StyleHelper::highlightColor()); grad.setColorAt(0.301, base); grad.setColorAt(1, StyleHelper::shadowColor()); @@ -150,8 +158,8 @@ void StyleHelper::verticalGradient(QPainter *painter, const QRect &spanRect, con if (StyleHelper::usePixmapCache()) { QString key; key.sprintf("mh_vertical %d %d %d %d %d", - spanRect.width(), spanRect.height(), clipRect.width(), - clipRect.height(), StyleHelper::baseColor().rgb());; + spanRect.width(), spanRect.height(), clipRect.width(), + clipRect.height(), StyleHelper::baseColor().rgb());; QPixmap pixmap; if (!QPixmapCache::find(key, pixmap)) { @@ -170,10 +178,11 @@ void StyleHelper::verticalGradient(QPainter *painter, const QRect &spanRect, con } static void horizontalGradientHelper(QPainter *p, const QRect &spanRect, const -QRect &rect) + QRect &rect) { QColor base = StyleHelper::baseColor(); QLinearGradient grad(rect.topLeft(), rect.bottomLeft()); + grad.setColorAt(0, StyleHelper::highlightColor().lighter(120)); if (rect.height() == StyleHelper::navigationWidgetHeight()) { grad.setColorAt(0.4, StyleHelper::highlightColor()); @@ -189,7 +198,6 @@ QRect &rect) shadowGradient.setColorAt(0.7, highlight); shadowGradient.setColorAt(1, QColor(0, 0, 0, 40)); p->fillRect(rect, shadowGradient); - } void StyleHelper::horizontalGradient(QPainter *painter, const QRect &spanRect, const QRect &clipRect) @@ -197,8 +205,8 @@ void StyleHelper::horizontalGradient(QPainter *painter, const QRect &spanRect, c if (StyleHelper::usePixmapCache()) { QString key; key.sprintf("mh_horizontal %d %d %d %d %d", - spanRect.width(), spanRect.height(), clipRect.width(), - clipRect.height(), StyleHelper::baseColor().rgb()); + spanRect.width(), spanRect.height(), clipRect.width(), + clipRect.height(), StyleHelper::baseColor().rgb()); QPixmap pixmap; if (!QPixmapCache::find(key, pixmap)) { @@ -211,7 +219,6 @@ void StyleHelper::horizontalGradient(QPainter *painter, const QRect &spanRect, c } painter->drawPixmap(clipRect.topLeft(), pixmap); - } else { horizontalGradientHelper(painter, spanRect, clipRect); } @@ -221,6 +228,7 @@ static void menuGradientHelper(QPainter *p, const QRect &spanRect, const QRect & { QLinearGradient grad(spanRect.topLeft(), spanRect.bottomLeft()); QColor menuColor = StyleHelper::mergedColors(StyleHelper::baseColor(), QColor(244, 244, 244), 25); + grad.setColorAt(0, menuColor.lighter(112)); grad.setColorAt(1, menuColor); p->fillRect(rect, grad); @@ -231,8 +239,8 @@ void StyleHelper::menuGradient(QPainter *painter, const QRect &spanRect, const Q if (StyleHelper::usePixmapCache()) { QString key; key.sprintf("mh_menu %d %d %d %d %d", - spanRect.width(), spanRect.height(), clipRect.width(), - clipRect.height(), StyleHelper::baseColor().rgb()); + spanRect.width(), spanRect.height(), clipRect.width(), + clipRect.height(), StyleHelper::baseColor().rgb()); QPixmap pixmap; if (!QPixmapCache::find(key, pixmap)) { @@ -249,5 +257,4 @@ void StyleHelper::menuGradient(QPainter *painter, const QRect &spanRect, const Q menuGradientHelper(painter, spanRect, clipRect); } } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/stylehelper.h b/ground/openpilotgcs/src/libs/utils/stylehelper.h index c0973827e..fb8384555 100644 --- a/ground/openpilotgcs/src/libs/utils/stylehelper.h +++ b/ground/openpilotgcs/src/libs/utils/stylehelper.h @@ -4,25 +4,25 @@ * @file stylehelper.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,11 +42,13 @@ QT_END_NAMESPACE // Helper class holding all custom color values namespace Utils { -class QTCREATOR_UTILS_EXPORT StyleHelper -{ +class QTCREATOR_UTILS_EXPORT StyleHelper { public: // Height of the project explorer navigation bar - static int navigationWidgetHeight() { return 24; } + static int navigationWidgetHeight() + { + return 24; + } static qreal sidebarFontSize(); static QPalette sidebarFontPalette(const QPalette &original); @@ -56,7 +58,10 @@ public: static QColor highlightColor(); static QColor shadowColor(); static QColor borderColor(); - static QColor buttonTextColor() { return QColor(0x4c4c4c); } + static QColor buttonTextColor() + { + return QColor(0x4c4c4c); + } static QColor mergedColors(const QColor &colorA, const QColor &colorB, int factor = 50); // Sets the base color and makes sure all top level widgets are updated @@ -68,11 +73,13 @@ public: static void menuGradient(QPainter *painter, const QRect &spanRect, const QRect &clipRect); // Pixmap cache should only be enabled for X11 due to slow gradients - static bool usePixmapCache() { return true; } + static bool usePixmapCache() + { + return true; + } private: static QColor m_baseColor; }; - } // namespace Utils #endif // STYLEHELPER_H diff --git a/ground/openpilotgcs/src/libs/utils/submiteditorwidget.cpp b/ground/openpilotgcs/src/libs/utils/submiteditorwidget.cpp index 44cdb1d90..76131e276 100644 --- a/ground/openpilotgcs/src/libs/utils/submiteditorwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/submiteditorwidget.cpp @@ -4,25 +4,25 @@ * @file submiteditorwidget.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,11 +44,9 @@ enum { debug = 0 }; enum { defaultLineWidth = 72 }; namespace Utils { - // QActionPushButton: A push button tied to an action // (similar to a QToolButton) -class QActionPushButton : public QPushButton -{ +class QActionPushButton : public QPushButton { Q_OBJECT public: explicit QActionPushButton(QAction *a); @@ -58,7 +56,7 @@ private slots: }; QActionPushButton::QActionPushButton(QAction *a) : - QPushButton(a->icon(), a->text()) + QPushButton(a->icon(), a->text()) { connect(a, SIGNAL(changed()), this, SLOT(actionChanged())); connect(this, SIGNAL(clicked()), a, SLOT(trigger())); @@ -67,20 +65,23 @@ QActionPushButton::QActionPushButton(QAction *a) : void QActionPushButton::actionChanged() { - if (const QAction *a = qobject_cast(sender())) + if (const QAction * a = qobject_cast(sender())) { setEnabled(a->isEnabled()); + } } // Helpers to retrieve model data static inline bool listModelChecked(const QAbstractItemModel *model, int row, int column = 0) { const QModelIndex checkableIndex = model->index(row, column, QModelIndex()); + return model->data(checkableIndex, Qt::CheckStateRole).toInt() == Qt::Checked; } static inline QString listModelText(const QAbstractItemModel *model, int row, int column) { const QModelIndex index = model->index(row, column, QModelIndex()); + return model->data(index, Qt::DisplayRole).toString(); } @@ -88,9 +89,12 @@ static inline QString listModelText(const QAbstractItemModel *model, int row, in static bool listModelContainsCheckedItem(const QAbstractItemModel *model) { const int count = model->rowCount(); - for (int i = 0; i < count; i++) - if (listModelChecked(model, i, 0)) + + for (int i = 0; i < count; i++) { + if (listModelChecked(model, i, 0)) { return true; + } + } return false; } @@ -98,19 +102,21 @@ static bool listModelContainsCheckedItem(const QAbstractItemModel *model) QList selectedRows(const QAbstractItemView *view) { const QModelIndexList indexList = view->selectionModel()->selectedRows(0); - if (indexList.empty()) + + if (indexList.empty()) { return QList(); + } QList rc; const QModelIndexList::const_iterator cend = indexList.constEnd(); - for (QModelIndexList::const_iterator it = indexList.constBegin(); it != cend; ++it) + for (QModelIndexList::const_iterator it = indexList.constBegin(); it != cend; ++it) { rc.push_back(it->row()); + } return rc; } // ----------- SubmitEditorWidgetPrivate -struct SubmitEditorWidgetPrivate -{ +struct SubmitEditorWidgetPrivate { // A pair of position/action to extend context menus typedef QPair > AdditionalContextMenuAction; @@ -119,8 +125,8 @@ struct SubmitEditorWidgetPrivate Ui::SubmitEditorWidget m_ui; bool m_filesSelected; bool m_filesChecked; - int m_fileNameColumn; - int m_activatedRow; + int m_fileNameColumn; + int m_activatedRow; QList descriptionEditContextMenuActions; QVBoxLayout *m_fieldLayout; @@ -135,8 +141,7 @@ SubmitEditorWidgetPrivate::SubmitEditorWidgetPrivate() : m_activatedRow(-1), m_fieldLayout(0), m_lineWidth(defaultLineWidth) -{ -} +{} SubmitEditorWidget::SubmitEditorWidget(QWidget *parent) : QWidget(parent), @@ -165,7 +170,7 @@ SubmitEditorWidget::~SubmitEditorWidget() } void SubmitEditorWidget::registerActions(QAction *editorUndoAction, QAction *editorRedoAction, - QAction *submitAction, QAction *diffAction) + QAction *submitAction, QAction *diffAction) { if (editorUndoAction) { editorUndoAction->setEnabled(m_d->m_ui.description->document()->isUndoAvailable()); @@ -181,8 +186,9 @@ void SubmitEditorWidget::registerActions(QAction *editorUndoAction, QAction *edi if (submitAction) { if (debug) { int count = 0; - if (const QAbstractItemModel *model = m_d->m_ui.fileView->model()) + if (const QAbstractItemModel * model = m_d->m_ui.fileView->model()) { count = model->rowCount(); + } qDebug() << Q_FUNC_INFO << submitAction << count << "items" << m_d->m_filesChecked; } submitAction->setEnabled(m_d->m_filesChecked); @@ -190,8 +196,9 @@ void SubmitEditorWidget::registerActions(QAction *editorUndoAction, QAction *edi m_d->m_ui.buttonLayout->addWidget(new QActionPushButton(submitAction)); } if (diffAction) { - if (debug) + if (debug) { qDebug() << diffAction << m_d->m_filesSelected; + } diffAction->setEnabled(m_d->m_filesSelected); connect(this, SIGNAL(fileSelectionChanged(bool)), diffAction, SLOT(setEnabled(bool))); connect(diffAction, SIGNAL(triggered()), this, SLOT(triggerDiffSelected())); @@ -199,7 +206,7 @@ void SubmitEditorWidget::registerActions(QAction *editorUndoAction, QAction *edi } } -void SubmitEditorWidget::unregisterActions(QAction *editorUndoAction, QAction *editorRedoAction, +void SubmitEditorWidget::unregisterActions(QAction *editorUndoAction, QAction *editorRedoAction, QAction *submitAction, QAction *diffAction) { if (editorUndoAction) { @@ -211,12 +218,13 @@ void SubmitEditorWidget::unregisterActions(QAction *editorUndoAction, QAction * disconnect(editorRedoAction, SIGNAL(triggered()), m_d->m_ui.description, SLOT(redo())); } - if (submitAction) + if (submitAction) { disconnect(this, SIGNAL(fileCheckStateChanged(bool)), submitAction, SLOT(setEnabled(bool))); + } if (diffAction) { - disconnect(this, SIGNAL(fileSelectionChanged(bool)), diffAction, SLOT(setEnabled(bool))); - disconnect(diffAction, SIGNAL(triggered()), this, SLOT(triggerDiffSelected())); + disconnect(this, SIGNAL(fileSelectionChanged(bool)), diffAction, SLOT(setEnabled(bool))); + disconnect(diffAction, SIGNAL(triggered()), this, SLOT(triggerDiffSelected())); } } @@ -224,6 +232,7 @@ void SubmitEditorWidget::unregisterActions(QAction *editorUndoAction, QAction * static inline QString trimMessageText(const QString &t) { QString rc = t.trimmed(); + rc += QLatin1Char('\n'); return rc; } @@ -235,7 +244,8 @@ static QString wrappedText(const QTextEdit *e) const QChar newLine = QLatin1Char('\n'); QString rc; QTextCursor cursor(e->document()); - cursor.movePosition(QTextCursor::Start); + + cursor.movePosition(QTextCursor::Start); while (!cursor.atEnd()) { cursor.select(QTextCursor::LineUnderCursor); rc += cursor.selectedText(); @@ -249,9 +259,10 @@ static QString wrappedText(const QTextEdit *e) QString SubmitEditorWidget::descriptionText() const { QString rc = trimMessageText(lineWrap() ? wrappedText(m_d->m_ui.description) : m_d->m_ui.description->toPlainText()); + // append field entries - foreach(const SubmitFieldWidget *fw, m_d->m_fieldWidgets) - rc += fw->fieldValues(); + foreach(const SubmitFieldWidget * fw, m_d->m_fieldWidgets) + rc += fw->fieldValues(); return rc; } @@ -267,11 +278,12 @@ bool SubmitEditorWidget::lineWrap() const void SubmitEditorWidget::setLineWrap(bool v) { - if (debug) + if (debug) { qDebug() << Q_FUNC_INFO << v; + } if (v) { m_d->m_ui.description->setLineWrapColumnOrWidth(m_d->m_lineWidth); - m_d->m_ui.description->setLineWrapMode(QTextEdit::FixedColumnWidth); + m_d->m_ui.description->setLineWrapMode(QTextEdit::FixedColumnWidth); } else { m_d->m_ui.description->setLineWrapMode(QTextEdit::NoWrap); } @@ -284,13 +296,16 @@ int SubmitEditorWidget::lineWrapWidth() const void SubmitEditorWidget::setLineWrapWidth(int v) { - if (debug) + if (debug) { qDebug() << Q_FUNC_INFO << v << lineWrap(); - if (m_d->m_lineWidth == v) + } + if (m_d->m_lineWidth == v) { return; + } m_d->m_lineWidth = v; - if (lineWrap()) + if (lineWrap()) { m_d->m_ui.description->setLineWrapColumnOrWidth(v); + } } int SubmitEditorWidget::fileNameColumn() const @@ -321,17 +336,18 @@ void SubmitEditorWidget::setFileModel(QAbstractItemModel *model) if (model->rowCount()) { const int columnCount = model->columnCount(); - for (int c = 0; c < columnCount; c++) + for (int c = 0; c < columnCount; c++) { m_d->m_ui.fileView->resizeColumnToContents(c); + } } - connect(model, SIGNAL(dataChanged(QModelIndex,QModelIndex)), + connect(model, SIGNAL(dataChanged(QModelIndex, QModelIndex)), this, SLOT(updateSubmitAction())); connect(model, SIGNAL(modelReset()), this, SLOT(updateSubmitAction())); - connect(model, SIGNAL(rowsInserted(QModelIndex,int,int)), + connect(model, SIGNAL(rowsInserted(QModelIndex, int, int)), this, SLOT(updateSubmitAction())); - connect(model, SIGNAL(rowsRemoved(QModelIndex,int,int)), + connect(model, SIGNAL(rowsRemoved(QModelIndex, int, int)), this, SLOT(updateSubmitAction())); connect(m_d->m_ui.fileView->selectionModel(), SIGNAL(selectionChanged(QItemSelection, QItemSelection)), this, SLOT(updateDiffAction())); @@ -346,14 +362,17 @@ QAbstractItemModel *SubmitEditorWidget::fileModel() const QStringList SubmitEditorWidget::selectedFiles() const { const QList selection = selectedRows(m_d->m_ui.fileView); - if (selection.empty()) + + if (selection.empty()) { return QStringList(); + } QStringList rc; const QAbstractItemModel *model = m_d->m_ui.fileView->model(); const int count = selection.size(); - for (int i = 0; i < count; i++) + for (int i = 0; i < count; i++) { rc.push_back(listModelText(model, selection.at(i), fileNameColumn())); + } return rc; } @@ -361,12 +380,16 @@ QStringList SubmitEditorWidget::checkedFiles() const { QStringList rc; const QAbstractItemModel *model = m_d->m_ui.fileView->model(); - if (!model) + + if (!model) { return rc; + } const int count = model->rowCount(); - for (int i = 0; i < count; i++) - if (listModelChecked(model, i, 0)) + for (int i = 0; i < count; i++) { + if (listModelChecked(model, i, 0)) { rc.push_back(listModelText(model, i, fileNameColumn())); + } + } return rc; } @@ -378,8 +401,10 @@ QTextEdit *SubmitEditorWidget::descriptionEdit() const void SubmitEditorWidget::triggerDiffSelected() { const QStringList sel = selectedFiles(); - if (!sel.empty()) + + if (!sel.empty()) { emit diffSelected(sel); + } } void SubmitEditorWidget::diffActivatedDelayed() @@ -406,6 +431,7 @@ void SubmitEditorWidget::updateActions() void SubmitEditorWidget::updateSubmitAction() { const bool newFilesCheckedState = hasCheckedFiles(); + if (m_d->m_filesChecked != newFilesCheckedState) { m_d->m_filesChecked = newFilesCheckedState; emit fileCheckStateChanged(m_d->m_filesChecked); @@ -416,6 +442,7 @@ void SubmitEditorWidget::updateSubmitAction() void SubmitEditorWidget::updateDiffAction() { const bool filesSelected = hasSelection(); + if (m_d->m_filesSelected != filesSelected) { m_d->m_filesSelected = filesSelected; emit fileSelectionChanged(m_d->m_filesSelected); @@ -425,21 +452,24 @@ void SubmitEditorWidget::updateDiffAction() bool SubmitEditorWidget::hasSelection() const { // Not present until model is set - if (const QItemSelectionModel *sm = m_d->m_ui.fileView->selectionModel()) + if (const QItemSelectionModel * sm = m_d->m_ui.fileView->selectionModel()) { return sm->hasSelection(); + } return false; } bool SubmitEditorWidget::hasCheckedFiles() const { - if (const QAbstractItemModel *model = m_d->m_ui.fileView->model()) + if (const QAbstractItemModel * model = m_d->m_ui.fileView->model()) { return listModelContainsCheckedItem(model); + } return false; } void SubmitEditorWidget::changeEvent(QEvent *e) { QWidget::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: m_d->m_ui.retranslateUi(this); @@ -462,7 +492,7 @@ void SubmitEditorWidget::addSubmitFieldWidget(SubmitFieldWidget *f) QHBoxLayout *outerLayout = new QHBoxLayout; outerLayout->addLayout(m_d->m_fieldLayout); outerLayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Ignored)); - QBoxLayout *descrLayout = qobject_cast(m_d->m_ui.descriptionBox->layout()); + QBoxLayout *descrLayout = qobject_cast(m_d->m_ui.descriptionBox->layout()); Q_ASSERT(descrLayout); descrLayout->addLayout(outerLayout); } @@ -487,9 +517,10 @@ void SubmitEditorWidget::insertDescriptionEditContextMenuAction(int pos, QAction void SubmitEditorWidget::editorCustomContextMenuRequested(const QPoint &pos) { - QMenu *menu = m_d->m_ui.description->createStandardContextMenu(); + QMenu *menu = m_d->m_ui.description->createStandardContextMenu(); + // Extend - foreach (const SubmitEditorWidgetPrivate::AdditionalContextMenuAction &a, m_d->descriptionEditContextMenuActions) { + foreach(const SubmitEditorWidgetPrivate::AdditionalContextMenuAction & a, m_d->descriptionEditContextMenuActions) { if (a.second) { if (a.first >= 0) { menu->insertAction(menu->actions().at(a.first), a.second); @@ -501,7 +532,6 @@ void SubmitEditorWidget::editorCustomContextMenuRequested(const QPoint &pos) menu->exec(m_d->m_ui.description->mapToGlobal(pos)); delete menu; } - } // namespace Utils #include "submiteditorwidget.moc" diff --git a/ground/openpilotgcs/src/libs/utils/submiteditorwidget.h b/ground/openpilotgcs/src/libs/utils/submiteditorwidget.h index 969b562ba..61c639b7e 100644 --- a/ground/openpilotgcs/src/libs/utils/submiteditorwidget.h +++ b/ground/openpilotgcs/src/libs/utils/submiteditorwidget.h @@ -4,25 +4,25 @@ * @file submiteditorwidget.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,7 +44,6 @@ class QLineEdit; QT_END_NAMESPACE namespace Utils { - class SubmitFieldWidget; struct SubmitEditorWidgetPrivate; @@ -66,10 +65,8 @@ struct SubmitEditorWidgetPrivate; * Care should be taken to ensure the widget is deleted properly when the * editor closes. */ -class QTCREATOR_UTILS_EXPORT SubmitEditorWidget : public QWidget -{ - Q_OBJECT - Q_DISABLE_COPY(SubmitEditorWidget) +class QTCREATOR_UTILS_EXPORT SubmitEditorWidget : public QWidget { + Q_OBJECT Q_DISABLE_COPY(SubmitEditorWidget) Q_PROPERTY(QString descriptionText READ descriptionText WRITE setDescriptionText DESIGNABLE true) Q_PROPERTY(int fileNameColumn READ fileNameColumn WRITE setFileNameColumn DESIGNABLE false) Q_PROPERTY(QAbstractItemView::SelectionMode fileListSelectionMode READ fileListSelectionMode WRITE setFileListSelectionMode DESIGNABLE true) @@ -79,9 +76,9 @@ public: explicit SubmitEditorWidget(QWidget *parent = 0); virtual ~SubmitEditorWidget(); - void registerActions(QAction *editorUndoAction, QAction *editorRedoAction, + void registerActions(QAction *editorUndoAction, QAction *editorRedoAction, QAction *submitAction = 0, QAction *diffAction = 0); - void unregisterActions(QAction *editorUndoAction, QAction *editorRedoAction, + void unregisterActions(QAction *editorUndoAction, QAction *editorRedoAction, QAction *submitAction = 0, QAction *diffAction = 0); QString descriptionText() const; @@ -140,7 +137,6 @@ private: SubmitEditorWidgetPrivate *m_d; }; - } // namespace Utils #endif // SUBMITEDITORWIDGET_H diff --git a/ground/openpilotgcs/src/libs/utils/submitfieldwidget.cpp b/ground/openpilotgcs/src/libs/utils/submitfieldwidget.cpp index 93b73f195..3e39238b2 100644 --- a/ground/openpilotgcs/src/libs/utils/submitfieldwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/submitfieldwidget.cpp @@ -4,25 +4,25 @@ * @file submitfieldwidget.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -46,22 +46,22 @@ enum { spacing = 2 }; static void inline setComboBlocked(QComboBox *cb, int index) { const bool blocked = cb->blockSignals(true); + cb->setCurrentIndex(index); cb->blockSignals(blocked); } namespace Utils { - // Field/Row entry struct FieldEntry { FieldEntry(); void createGui(const QIcon &removeIcon); void deleteGuiLater(); - QComboBox *combo; + QComboBox *combo; QHBoxLayout *layout; - QLineEdit *lineEdit; - QToolBar *toolBar; + QLineEdit *lineEdit; + QToolBar *toolBar; QToolButton *clearButton; QToolButton *browseButton; int comboIndex; @@ -75,22 +75,21 @@ FieldEntry::FieldEntry() : clearButton(0), browseButton(0), comboIndex(0) -{ -} +{} void FieldEntry::createGui(const QIcon &removeIcon) { - layout = new QHBoxLayout; + layout = new QHBoxLayout; layout->setMargin(0); - layout ->setSpacing(spacing); - combo = new QComboBox; + layout->setSpacing(spacing); + combo = new QComboBox; layout->addWidget(combo); - lineEdit = new QLineEdit; + lineEdit = new QLineEdit; layout->addWidget(lineEdit); - toolBar = new QToolBar; + toolBar = new QToolBar; toolBar->setProperty("_q_custom_style_disabled", QVariant(true)); layout->addWidget(toolBar); - clearButton = new QToolButton; + clearButton = new QToolButton; clearButton->setIcon(removeIcon); toolBar->addWidget(clearButton); browseButton = new QToolButton; @@ -103,7 +102,7 @@ void FieldEntry::deleteGuiLater() clearButton->deleteLater(); browseButton->deleteLater(); toolBar->deleteLater(); - lineEdit->deleteLater(); + lineEdit->deleteLater(); combo->deleteLater(); layout->deleteLater(); } @@ -116,11 +115,11 @@ struct SubmitFieldWidgetPrivate { int findField(const QString &f, int excluded = -1) const; inline QString fieldText(int) const; inline QString fieldValue(int) const; - inline void focusField(int); + inline void focusField(int); - const QIcon removeFieldIcon; - QStringList fields; - QCompleter *completer; + const QIcon removeFieldIcon; + QStringList fields; + QCompleter *completer; bool hasBrowseButton; bool allowDuplicateFields; @@ -129,21 +128,22 @@ struct SubmitFieldWidgetPrivate { }; SubmitFieldWidgetPrivate::SubmitFieldWidgetPrivate() : - removeFieldIcon(QLatin1String(":/utils/images/removesubmitfield.png")), - completer(0), - hasBrowseButton(false), - allowDuplicateFields(false), - layout(0) -{ -} + removeFieldIcon(QLatin1String(":/utils/images/removesubmitfield.png")), + completer(0), + hasBrowseButton(false), + allowDuplicateFields(false), + layout(0) +{} int SubmitFieldWidgetPrivate::findSender(const QObject *o) const { const int count = fieldEntries.size(); + for (int i = 0; i < count; i++) { const FieldEntry &fe = fieldEntries.at(i); - if (fe.combo == o || fe.browseButton == o || fe.clearButton == o || fe.lineEdit == o) + if (fe.combo == o || fe.browseButton == o || fe.clearButton == o || fe.lineEdit == o) { return i; + } } return -1; } @@ -151,9 +151,12 @@ int SubmitFieldWidgetPrivate::findSender(const QObject *o) const int SubmitFieldWidgetPrivate::findField(const QString &ft, int excluded) const { const int count = fieldEntries.size(); - for (int i = 0; i < count; i++) - if (i != excluded && fieldText(i) == ft) + + for (int i = 0; i < count; i++) { + if (i != excluded && fieldText(i) == ft) { return i; + } + } return -1; } @@ -174,8 +177,8 @@ void SubmitFieldWidgetPrivate::focusField(int pos) // SubmitFieldWidget SubmitFieldWidget::SubmitFieldWidget(QWidget *parent) : - QWidget(parent), - m_d(new SubmitFieldWidgetPrivate) + QWidget(parent), + m_d(new SubmitFieldWidgetPrivate) { m_d->layout = new QVBoxLayout; m_d->layout->setMargin(0); @@ -191,12 +194,14 @@ SubmitFieldWidget::~SubmitFieldWidget() void SubmitFieldWidget::setFields(const QStringList & f) { // remove old fields - for (int i = m_d->fieldEntries.size() - 1 ; i >= 0 ; i--) - removeField(i); + for (int i = m_d->fieldEntries.size() - 1; i >= 0; i--) { + removeField(i); + } m_d->fields = f; - if (!f.empty()) + if (!f.empty()) { createField(f.front()); + } } QStringList SubmitFieldWidget::fields() const @@ -211,11 +216,12 @@ bool SubmitFieldWidget::hasBrowseButton() const void SubmitFieldWidget::setHasBrowseButton(bool d) { - if (m_d->hasBrowseButton == d) + if (m_d->hasBrowseButton == d) { return; + } m_d->hasBrowseButton = d; foreach(const FieldEntry &fe, m_d->fieldEntries) - fe.browseButton->setVisible(d); + fe.browseButton->setVisible(d); } bool SubmitFieldWidget::allowDuplicateFields() const @@ -235,11 +241,12 @@ QCompleter *SubmitFieldWidget::completer() const void SubmitFieldWidget::setCompleter(QCompleter *c) { - if (c == m_d->completer) + if (c == m_d->completer) { return; + } m_d->completer = c; foreach(const FieldEntry &fe, m_d->fieldEntries) - fe.lineEdit->setCompleter(c); + fe.lineEdit->setCompleter(c); } QString SubmitFieldWidget::fieldValue(int pos) const @@ -254,12 +261,14 @@ void SubmitFieldWidget::setFieldValue(int pos, const QString &value) QString SubmitFieldWidget::fieldValues() const { - const QChar blank = QLatin1Char(' '); + const QChar blank = QLatin1Char(' '); const QChar newLine = QLatin1Char('\n'); // Format as "RevBy: value\nSigned-Off: value\n" QString rc; + foreach(const FieldEntry &fe, m_d->fieldEntries) { const QString value = fe.lineEdit->text().trimmed(); + if (!value.isEmpty()) { rc += fe.combo->currentText(); rc += blank; @@ -273,6 +282,7 @@ QString SubmitFieldWidget::fieldValues() const void SubmitFieldWidget::createField(const QString &f) { FieldEntry fe; + fe.createGui(m_d->removeFieldIcon); fe.combo->addItems(m_d->fields); if (!f.isEmpty()) { @@ -284,11 +294,13 @@ void SubmitFieldWidget::createField(const QString &f) } connect(fe.browseButton, SIGNAL(clicked()), this, SLOT(slotBrowseButtonClicked())); - if (!m_d->hasBrowseButton) + if (!m_d->hasBrowseButton) { fe.browseButton->setVisible(false); + } - if (m_d->completer) + if (m_d->completer) { fe.lineEdit->setCompleter(m_d->completer); + } connect(fe.combo, SIGNAL(currentIndexChanged(int)), this, SLOT(slotComboIndexChanged(int))); @@ -302,6 +314,7 @@ void SubmitFieldWidget::slotRemove() { // Never remove first entry const int index = m_d->findSender(sender()); + switch (index) { case -1: break; @@ -316,8 +329,9 @@ void SubmitFieldWidget::slotRemove() void SubmitFieldWidget::removeField(int index) { - FieldEntry fe = m_d->fieldEntries.takeAt(index); - QLayoutItem * item = m_d->layout->takeAt(index); + FieldEntry fe = m_d->fieldEntries.takeAt(index); + QLayoutItem *item = m_d->layout->takeAt(index); + fe.deleteGuiLater(); delete item; } @@ -325,10 +339,13 @@ void SubmitFieldWidget::removeField(int index) void SubmitFieldWidget::slotComboIndexChanged(int comboIndex) { const int pos = m_d->findSender(sender()); - if (debug) + + if (debug) { qDebug() << '>' << Q_FUNC_INFO << pos; - if (pos == -1) + } + if (pos == -1) { return; + } // Accept new index or reset combo to previous value? int &previousIndex = m_d->fieldEntries[pos].comboIndex; if (comboIndexChange(pos, comboIndex)) { @@ -336,8 +353,9 @@ void SubmitFieldWidget::slotComboIndexChanged(int comboIndex) } else { setComboBlocked(m_d->fieldEntries.at(pos).combo, previousIndex); } - if (debug) + if (debug) { qDebug() << '<' << Q_FUNC_INFO << pos; + } } // Handle change of a combo. Return "false" if the combo @@ -345,6 +363,7 @@ void SubmitFieldWidget::slotComboIndexChanged(int comboIndex) bool SubmitFieldWidget::comboIndexChange(int pos, int index) { const QString newField = m_d->fieldEntries.at(pos).combo->itemText(index); + // If the field is visible elsewhere: focus the existing one and refuse if (!m_d->allowDuplicateFields) { const int existingFieldIndex = m_d->findField(newField, pos); @@ -354,8 +373,9 @@ bool SubmitFieldWidget::comboIndexChange(int pos, int index) } } // Empty value: just change the field - if (m_d->fieldValue(pos).isEmpty()) + if (m_d->fieldValue(pos).isEmpty()) { return true; + } // Non-empty: Create a new field and reset the triggering combo createField(newField); return false; @@ -366,5 +386,4 @@ void SubmitFieldWidget::slotBrowseButtonClicked() const int pos = m_d->findSender(sender()); emit browseButtonClicked(pos, m_d->fieldText(pos)); } - } diff --git a/ground/openpilotgcs/src/libs/utils/submitfieldwidget.h b/ground/openpilotgcs/src/libs/utils/submitfieldwidget.h index 10154fffd..8bc912c27 100644 --- a/ground/openpilotgcs/src/libs/utils/submitfieldwidget.h +++ b/ground/openpilotgcs/src/libs/utils/submitfieldwidget.h @@ -4,25 +4,25 @@ * @file submitfieldwidget.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,7 +38,6 @@ class QCompleter; QT_END_NAMESPACE namespace Utils { - struct SubmitFieldWidgetPrivate; /* A widget for editing submit message fields like "reviewed-by:", @@ -47,10 +46,8 @@ struct SubmitFieldWidgetPrivate; * When choosing a different field in the combo, a new row is opened if text * has been entered for the current field. Optionally, a "Browse..." button and * completer can be added. */ -class QTCREATOR_UTILS_EXPORT SubmitFieldWidget : public QWidget -{ - Q_OBJECT - Q_PROPERTY(QStringList fields READ fields WRITE setFields DESIGNABLE true) +class QTCREATOR_UTILS_EXPORT SubmitFieldWidget : public QWidget { + Q_OBJECT Q_PROPERTY(QStringList fields READ fields WRITE setFields DESIGNABLE true) Q_PROPERTY(bool hasBrowseButton READ hasBrowseButton WRITE setHasBrowseButton DESIGNABLE true) Q_PROPERTY(bool allowDuplicateFields READ allowDuplicateFields WRITE setAllowDuplicateFields DESIGNABLE true) @@ -59,7 +56,7 @@ public: virtual ~SubmitFieldWidget(); QStringList fields() const; - void setFields(const QStringList&); + void setFields(const QStringList &); bool hasBrowseButton() const; void setHasBrowseButton(bool d); @@ -91,7 +88,6 @@ private: SubmitFieldWidgetPrivate *m_d; }; - } #endif // SUBMITFIELDWIDGET_H diff --git a/ground/openpilotgcs/src/libs/utils/svgimageprovider.cpp b/ground/openpilotgcs/src/libs/utils/svgimageprovider.cpp index 6e454002b..2b9a8c79a 100644 --- a/ground/openpilotgcs/src/libs/utils/svgimageprovider.cpp +++ b/ground/openpilotgcs/src/libs/utils/svgimageprovider.cpp @@ -30,12 +30,11 @@ #include #include -SvgImageProvider::SvgImageProvider(const QString &basePath): +SvgImageProvider::SvgImageProvider(const QString &basePath) : QObject(), QDeclarativeImageProvider(QDeclarativeImageProvider::Image), m_basePath(basePath) -{ -} +{} SvgImageProvider::~SvgImageProvider() { @@ -45,14 +44,16 @@ SvgImageProvider::~SvgImageProvider() QSvgRenderer *SvgImageProvider::loadRenderer(const QString &svgFile) { QSvgRenderer *renderer = m_renderers.value(svgFile); + if (!renderer) { renderer = new QSvgRenderer(svgFile); QString fn = QUrl::fromLocalFile(m_basePath).resolved(svgFile).toLocalFile(); - //convert path to be relative to base - if (!renderer->isValid()) + // convert path to be relative to base + if (!renderer->isValid()) { renderer->load(fn); + } if (!renderer->isValid()) { qWarning() << "Failed to load svg file:" << svgFile << fn; @@ -79,7 +80,7 @@ QSvgRenderer *SvgImageProvider::loadRenderer(const QString &svgFile) Image { source: "image://svg/pfd.svg!world" } -*/ + */ QImage SvgImageProvider::requestImage(const QString &id, QSize *size, const QSize &requestedSize) { QString svgFile = id; @@ -87,15 +88,16 @@ QImage SvgImageProvider::requestImage(const QString &id, QSize *size, const QSiz QString parameters; int separatorPos = id.indexOf('!'); + if (separatorPos != -1) { svgFile = id.left(separatorPos); - element = id.mid(separatorPos+1); + element = id.mid(separatorPos + 1); } int parametersPos = element.indexOf('?'); if (parametersPos != -1) { - parameters = element.mid(parametersPos+1); - element = element.left(parametersPos); + parameters = element.mid(parametersPos + 1); + element = element.left(parametersPos); } int hSlicesCount = 0; @@ -120,31 +122,33 @@ QImage SvgImageProvider::requestImage(const QString &id, QSize *size, const QSiz } } - if (size) + if (size) { *size = QSize(); + } QSvgRenderer *renderer = loadRenderer(svgFile); - if (!renderer) + if (!renderer) { return QImage(); + } - qreal xScale = 1.0; - qreal yScale = 1.0; + qreal xScale = 1.0; + qreal yScale = 1.0; QSize docSize = renderer->defaultSize(); if (!requestedSize.isEmpty()) { if (!element.isEmpty()) { QRectF elementBounds = renderer->boundsOnElement(element); - xScale = qreal(requestedSize.width())/elementBounds.width(); - yScale = qreal(requestedSize.height())/elementBounds.height(); + xScale = qreal(requestedSize.width()) / elementBounds.width(); + yScale = qreal(requestedSize.height()) / elementBounds.height(); } else if (!docSize.isEmpty()) { - xScale = qreal(requestedSize.width())/docSize.width(); - yScale = qreal(requestedSize.height())/docSize.height(); + xScale = qreal(requestedSize.width()) / docSize.width(); + yScale = qreal(requestedSize.height()) / docSize.height(); } } - //keep the aspect ratio - //TODO: how to configure it? as a part of image path? + // keep the aspect ratio + // TODO: how to configure it? as a part of image path? xScale = yScale = qMin(xScale, yScale); if (!element.isEmpty()) { @@ -154,40 +158,41 @@ QImage SvgImageProvider::requestImage(const QString &id, QSize *size, const QSiz } QRectF elementBounds = renderer->boundsOnElement(element); - int elementWidth = qRound(elementBounds.width() * xScale); - int elementHeigh = qRound(elementBounds.height() * yScale); + int elementWidth = qRound(elementBounds.width() * xScale); + int elementHeigh = qRound(elementBounds.height() * yScale); int w = elementWidth; int h = elementHeigh; int x = 0; int y = 0; if (hSlicesCount > 1) { - x = (w*hSlice)/hSlicesCount; - w = (w*(hSlice+1))/hSlicesCount - x; + x = (w * hSlice) / hSlicesCount; + w = (w * (hSlice + 1)) / hSlicesCount - x; } if (vSlicesCount > 1) { - y = (h*(vSlice))/vSlicesCount; - h = (h*(vSlice+1))/vSlicesCount - y; + y = (h * (vSlice)) / vSlicesCount; + h = (h * (vSlice + 1)) / vSlicesCount - y; } - QImage img(w+border*2, h+border*2, QImage::Format_ARGB32_Premultiplied); + QImage img(w + border * 2, h + border * 2, QImage::Format_ARGB32_Premultiplied); img.fill(0); QPainter p(&img); p.setRenderHints(QPainter::TextAntialiasing | QPainter::Antialiasing | QPainter::SmoothPixmapTransform); - p.translate(-x+border,-y+border); + p.translate(-x + border, -y + border); renderer->render(&p, element, QRectF(0, 0, elementWidth, elementHeigh)); - if (size) + if (size) { *size = QSize(w, h); + } - //img.save(element+parameters+".png"); + // img.save(element+parameters+".png"); return img; } else { - //render the whole svg file + // render the whole svg file int w = qRound(docSize.width() * xScale); int h = qRound(docSize.height() * yScale); @@ -201,8 +206,9 @@ QImage SvgImageProvider::requestImage(const QString &id, QSize *size, const QSiz p.scale(xScale, yScale); renderer->render(&p, QRectF(QPointF(), QSizeF(docSize))); - if (size) + if (size) { *size = QSize(w, h); + } return img; } } @@ -213,17 +219,18 @@ QPixmap SvgImageProvider::requestPixmap(const QString &id, QSize *size, const QS } /*! - \fn SvgImageProvider::scaledElementBounds(const QString &svgFile, const QString &element) + \fn SvgImageProvider::scaledElementBounds(const QString &svgFile, const QString &element) - Returns the bound of \a element in logical coordinates, - scalled to the default size of svg document (so the bounds of whole doc would be (0,0,1,1) ). -*/ + Returns the bound of \a element in logical coordinates, + scalled to the default size of svg document (so the bounds of whole doc would be (0,0,1,1) ). + */ QRectF SvgImageProvider::scaledElementBounds(const QString &svgFile, const QString &elementName) { QSvgRenderer *renderer = loadRenderer(svgFile); - if (!renderer) + if (!renderer) { return QRectF(); + } if (!renderer->elementExists(elementName)) { qWarning() << "invalid element:" << elementName << "of" << svgFile; @@ -234,9 +241,9 @@ QRectF SvgImageProvider::scaledElementBounds(const QString &svgFile, const QStri QMatrix matrix = renderer->matrixForElement(elementName); elementBounds = matrix.mapRect(elementBounds); - QSize docSize = renderer->defaultSize(); - return QRectF(elementBounds.x()/docSize.width(), - elementBounds.y()/docSize.height(), - elementBounds.width()/docSize.width(), - elementBounds.height()/docSize.height()); + QSize docSize = renderer->defaultSize(); + return QRectF(elementBounds.x() / docSize.width(), + elementBounds.y() / docSize.height(), + elementBounds.width() / docSize.width(), + elementBounds.height() / docSize.height()); } diff --git a/ground/openpilotgcs/src/libs/utils/svgimageprovider.h b/ground/openpilotgcs/src/libs/utils/svgimageprovider.h index c13c610ff..e24f51e5a 100644 --- a/ground/openpilotgcs/src/libs/utils/svgimageprovider.h +++ b/ground/openpilotgcs/src/libs/utils/svgimageprovider.h @@ -35,23 +35,22 @@ #include "utils_global.h" -class QTCREATOR_UTILS_EXPORT SvgImageProvider : public QObject, public QDeclarativeImageProvider -{ +class QTCREATOR_UTILS_EXPORT SvgImageProvider : public QObject, public QDeclarativeImageProvider { Q_OBJECT public: SvgImageProvider(const QString &basePath); - ~SvgImageProvider(); + ~SvgImageProvider(); QSvgRenderer *loadRenderer(const QString &svgFile); - QImage requestImage(const QString &id, QSize *size, const QSize& requestedSize); - QPixmap requestPixmap(const QString &id, QSize *size, const QSize& requestedSize); + QImage requestImage(const QString &id, QSize *size, const QSize & requestedSize); + QPixmap requestPixmap(const QString &id, QSize *size, const QSize & requestedSize); Q_INVOKABLE QRectF scaledElementBounds(const QString &svgFile, const QString &elementName); private: - QMap m_renderers; + QMap m_renderers; QString m_basePath; }; -#endif +#endif // ifndef SVGIMAGEPROVIDER_H_ diff --git a/ground/openpilotgcs/src/libs/utils/synchronousprocess.cpp b/ground/openpilotgcs/src/libs/utils/synchronousprocess.cpp index 332c60f64..02a9dfff9 100644 --- a/ground/openpilotgcs/src/libs/utils/synchronousprocess.cpp +++ b/ground/openpilotgcs/src/libs/utils/synchronousprocess.cpp @@ -4,25 +4,25 @@ * @file synchronousprocess.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,25 +44,24 @@ enum { debug = 0 }; enum { defaultMaxHangTimerCount = 10 }; namespace Utils { - // ----------- SynchronousProcessResponse SynchronousProcessResponse::SynchronousProcessResponse() : - result(StartFailed), - exitCode(-1) -{ -} + result(StartFailed), + exitCode(-1) +{} void SynchronousProcessResponse::clear() { - result = StartFailed; + result = StartFailed; exitCode = -1; stdOut.clear(); stdErr.clear(); } -QTCREATOR_UTILS_EXPORT QDebug operator<<(QDebug str, const SynchronousProcessResponse& r) +QTCREATOR_UTILS_EXPORT QDebug operator<<(QDebug str, const SynchronousProcessResponse & r) { QDebug nsp = str.nospace(); + nsp << "SynchronousProcessResponse: result=" << r.result << " ex=" << r.exitCode << '\n' << r.stdOut.size() << " bytes stdout, stderr=" << r.stdErr << '\n'; return str; @@ -78,7 +77,7 @@ struct ChannelBuffer { bool firstData; bool bufferedSignalsEnabled; bool firstBuffer; - int bufferPos; + int bufferPos; }; ChannelBuffer::ChannelBuffer() : @@ -86,14 +85,13 @@ ChannelBuffer::ChannelBuffer() : bufferedSignalsEnabled(false), firstBuffer(true), bufferPos(0) -{ -} +{} void ChannelBuffer::clearForRun() { - firstData = true; + firstData = true; firstBuffer = true; - bufferPos = 0; + bufferPos = 0; } /* Check for complete lines read from the device and return them, moving the @@ -103,10 +101,12 @@ QByteArray ChannelBuffer::linesRead() { // Any new lines? const int lastLineIndex = data.lastIndexOf('\n'); - if (lastLineIndex == -1 || lastLineIndex <= bufferPos) + + if (lastLineIndex == -1 || lastLineIndex <= bufferPos) { return QByteArray(); + } const int nextBufferPos = lastLineIndex + 1; - const QByteArray lines = data.mid(bufferPos, nextBufferPos - bufferPos); + const QByteArray lines = data.mid(bufferPos, nextBufferPos - bufferPos); bufferPos = nextBufferPos; return lines; } @@ -114,15 +114,15 @@ QByteArray ChannelBuffer::linesRead() // ----------- SynchronousProcessPrivate struct SynchronousProcessPrivate { SynchronousProcessPrivate(); - void clearForRun(); + void clearForRun(); QTextCodec *m_stdOutCodec; - QProcess m_process; - QTimer m_timer; + QProcess m_process; + QTimer m_timer; QEventLoop m_eventLoop; SynchronousProcessResponse m_result; - int m_hangTimerCount; - int m_maxHangTimerCount; + int m_hangTimerCount; + int m_maxHangTimerCount; bool m_startFailure; ChannelBuffer m_stdOut; @@ -134,8 +134,7 @@ SynchronousProcessPrivate::SynchronousProcessPrivate() : m_hangTimerCount(0), m_maxHangTimerCount(defaultMaxHangTimerCount), m_startFailure(false) -{ -} +{} void SynchronousProcessPrivate::clearForRun() { @@ -152,7 +151,7 @@ SynchronousProcess::SynchronousProcess() : { m_d->m_timer.setInterval(1000); connect(&m_d->m_timer, SIGNAL(timeout()), this, SLOT(slotTimeout())); - connect(&m_d->m_process, SIGNAL(finished(int,QProcess::ExitStatus)), this, SLOT(finished(int,QProcess::ExitStatus))); + connect(&m_d->m_process, SIGNAL(finished(int, QProcess::ExitStatus)), this, SLOT(finished(int, QProcess::ExitStatus))); connect(&m_d->m_process, SIGNAL(error(QProcess::ProcessError)), this, SLOT(error(QProcess::ProcessError))); connect(&m_d->m_process, SIGNAL(readyReadStandardOutput()), this, SLOT(stdOutReady())); @@ -229,7 +228,7 @@ QString SynchronousProcess::workingDirectory() const return m_d->m_process.workingDirectory(); } -QProcess::ProcessChannelMode SynchronousProcess::processChannelMode () const +QProcess::ProcessChannelMode SynchronousProcess::processChannelMode() const { return m_d->m_process.processChannelMode(); } @@ -240,10 +239,11 @@ void SynchronousProcess::setProcessChannelMode(QProcess::ProcessChannelMode m) } SynchronousProcessResponse SynchronousProcess::run(const QString &binary, - const QStringList &args) + const QStringList &args) { - if (debug) + if (debug) { qDebug() << '>' << Q_FUNC_INFO << binary << args; + } m_d->clearForRun(); @@ -267,38 +267,43 @@ SynchronousProcessResponse SynchronousProcess::run(const QString &binary, QApplication::restoreOverrideCursor(); } - if (debug) + if (debug) { qDebug() << '<' << Q_FUNC_INFO << binary << m_d->m_result; - return m_d->m_result; + } + return m_d->m_result; } void SynchronousProcess::slotTimeout() { if (++m_d->m_hangTimerCount > m_d->m_maxHangTimerCount) { - if (debug) + if (debug) { qDebug() << Q_FUNC_INFO << "HANG detected, killing"; + } m_d->m_process.kill(); m_d->m_result.result = SynchronousProcessResponse::Hang; } else { - if (debug) + if (debug) { qDebug() << Q_FUNC_INFO << m_d->m_hangTimerCount; + } } } void SynchronousProcess::finished(int exitCode, QProcess::ExitStatus e) { - if (debug) + if (debug) { qDebug() << Q_FUNC_INFO << exitCode << e; + } m_d->m_hangTimerCount = 0; switch (e) { case QProcess::NormalExit: - m_d->m_result.result = exitCode ? SynchronousProcessResponse::FinishedError : SynchronousProcessResponse::Finished; + m_d->m_result.result = exitCode ? SynchronousProcessResponse::FinishedError : SynchronousProcessResponse::Finished; m_d->m_result.exitCode = exitCode; break; case QProcess::CrashExit: // Was hang detected before and killed? - if (m_d->m_result.result != SynchronousProcessResponse::Hang) + if (m_d->m_result.result != SynchronousProcessResponse::Hang) { m_d->m_result.result = SynchronousProcessResponse::TerminatedAbnormally; + } m_d->m_result.exitCode = -1; break; } @@ -308,11 +313,13 @@ void SynchronousProcess::finished(int exitCode, QProcess::ExitStatus e) void SynchronousProcess::error(QProcess::ProcessError e) { m_d->m_hangTimerCount = 0; - if (debug) + if (debug) { qDebug() << Q_FUNC_INFO << e; + } // Was hang detected before and killed? - if (m_d->m_result.result != SynchronousProcessResponse::Hang) + if (m_d->m_result.result != SynchronousProcessResponse::Hang) { m_d->m_result.result = SynchronousProcessResponse::StartFailed; + } m_d->m_startFailure = true; m_d->m_eventLoop.quit(); } @@ -337,6 +344,7 @@ QString SynchronousProcess::convertStdErr(const QByteArray &ba) QString SynchronousProcess::convertStdOut(const QByteArray &ba) const { QString stdOut = m_d->m_stdOutCodec ? m_d->m_stdOutCodec->toUnicode(ba) : QString::fromLocal8Bit(ba.constData(), ba.size()); + return stdOut.remove(QLatin1Char('\r')); } @@ -344,8 +352,10 @@ void SynchronousProcess::processStdOut(bool emitSignals) { // Handle binary data const QByteArray ba = m_d->m_process.readAllStandardOutput(); - if (debug > 1) + + if (debug > 1) { qDebug() << Q_FUNC_INFO << emitSignals << ba; + } if (!ba.isEmpty()) { m_d->m_stdOut.data += ba; if (emitSignals) { @@ -368,8 +378,10 @@ void SynchronousProcess::processStdErr(bool emitSignals) { // Handle binary data const QByteArray ba = m_d->m_process.readAllStandardError(); - if (debug > 1) + + if (debug > 1) { qDebug() << Q_FUNC_INFO << emitSignals << ba; + } if (!ba.isEmpty()) { m_d->m_stdErr.data += ba; if (emitSignals) { @@ -408,44 +420,52 @@ static QString checkBinary(const QDir &dir, const QString &binary) { // naive UNIX approach const QFileInfo info(dir.filePath(binary)); - if (info.isFile() && info.isExecutable()) + + if (info.isFile() && info.isExecutable()) { return info.absoluteFilePath(); + } // Does the OS have some weird extension concept or does the // binary have a 3 letter extension? - if (pathOS == OS_Unix) + if (pathOS == OS_Unix) { return QString(); + } const int dotIndex = binary.lastIndexOf(QLatin1Char('.')); - if (dotIndex != -1 && dotIndex == binary.size() - 4) - return QString(); + if (dotIndex != -1 && dotIndex == binary.size() - 4) { + return QString(); + } switch (pathOS) { case OS_Unix: break; - case OS_Windows: { - static const char *windowsExtensions[] = {".cmd", ".bat", ".exe", ".com" }; - // Check the Windows extensions using the order - const int windowsExtensionCount = sizeof(windowsExtensions)/sizeof(const char*); - for (int e = 0; e < windowsExtensionCount; e ++) { - const QFileInfo windowsBinary(dir.filePath(binary + QLatin1String(windowsExtensions[e]))); - if (windowsBinary.isFile() && windowsBinary.isExecutable()) - return windowsBinary.absoluteFilePath(); + case OS_Windows: + { + static const char *windowsExtensions[] = { ".cmd", ".bat", ".exe", ".com" }; + // Check the Windows extensions using the order + const int windowsExtensionCount = sizeof(windowsExtensions) / sizeof(const char *); + for (int e = 0; e < windowsExtensionCount; e++) { + const QFileInfo windowsBinary(dir.filePath(binary + QLatin1String(windowsExtensions[e]))); + if (windowsBinary.isFile() && windowsBinary.isExecutable()) { + return windowsBinary.absoluteFilePath(); } } - break; - case OS_Mac: { - // Check for Mac app folders - const QFileInfo appFolder(dir.filePath(binary + QLatin1String(".app"))); - if (appFolder.isDir()) { - QString macBinaryPath = appFolder.absoluteFilePath(); - macBinaryPath += QLatin1String("/Contents/MacOS/"); - macBinaryPath += binary; - const QFileInfo macBinary(macBinaryPath); - if (macBinary.isFile() && macBinary.isExecutable()) - return macBinary.absoluteFilePath(); + } + break; + case OS_Mac: + { + // Check for Mac app folders + const QFileInfo appFolder(dir.filePath(binary + QLatin1String(".app"))); + if (appFolder.isDir()) { + QString macBinaryPath = appFolder.absoluteFilePath(); + macBinaryPath += QLatin1String("/Contents/MacOS/"); + macBinaryPath += binary; + const QFileInfo macBinary(macBinaryPath); + if (macBinary.isFile() && macBinary.isExecutable()) { + return macBinary.absoluteFilePath(); } } - break; + } + break; } return QString(); } @@ -454,25 +474,30 @@ QString SynchronousProcess::locateBinary(const QString &path, const QString &bin { // Absolute file? const QFileInfo absInfo(binary); - if (absInfo.isAbsolute()) + + if (absInfo.isAbsolute()) { return checkBinary(absInfo.dir(), absInfo.fileName()); + } // Windows finds binaries in the current directory if (pathOS == OS_Windows) { const QString currentDirBinary = checkBinary(QDir::current(), binary); - if (!currentDirBinary.isEmpty()) + if (!currentDirBinary.isEmpty()) { return currentDirBinary; + } } const QStringList paths = path.split(pathSeparator()); - if (paths.empty()) + if (paths.empty()) { return QString(); + } const QStringList::const_iterator cend = paths.constEnd(); for (QStringList::const_iterator it = paths.constBegin(); it != cend; ++it) { const QDir dir(*it); const QString rc = checkBinary(dir, binary); - if (!rc.isEmpty()) + if (!rc.isEmpty()) { return rc; + } } return QString(); } @@ -480,14 +505,15 @@ QString SynchronousProcess::locateBinary(const QString &path, const QString &bin QString SynchronousProcess::locateBinary(const QString &binary) { const QByteArray path = qgetenv("PATH"); + return locateBinary(QString::fromLocal8Bit(path), binary); } QChar SynchronousProcess::pathSeparator() { - if (pathOS == OS_Windows) + if (pathOS == OS_Windows) { return QLatin1Char(';'); + } return QLatin1Char(':'); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/synchronousprocess.h b/ground/openpilotgcs/src/libs/utils/synchronousprocess.h index a3cf2df44..db0ea01a1 100644 --- a/ground/openpilotgcs/src/libs/utils/synchronousprocess.h +++ b/ground/openpilotgcs/src/libs/utils/synchronousprocess.h @@ -4,25 +4,25 @@ * @file synchronousprocess.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,12 +42,10 @@ class QByteArray; QT_END_NAMESPACE namespace Utils { - struct SynchronousProcessPrivate; /* Result of SynchronousProcess execution */ -struct QTCREATOR_UTILS_EXPORT SynchronousProcessResponse -{ +struct QTCREATOR_UTILS_EXPORT SynchronousProcessResponse { enum Result { // Finished with return code 0 Finished, @@ -58,13 +56,14 @@ struct QTCREATOR_UTILS_EXPORT SynchronousProcessResponse // Executable could not be started StartFailed, // Hang, no output after time out - Hang }; + Hang + }; SynchronousProcessResponse(); void clear(); - Result result; - int exitCode; + Result result; + int exitCode; QString stdOut; QString stdErr; }; @@ -83,8 +82,7 @@ QTCREATOR_UTILS_EXPORT QDebug operator<<(QDebug str, const SynchronousProcessRes * stdOutBufferedSignalsEnabled()/setStdErrBufferedSignalsEnabled(). * They would typically be used for log windows. */ -class QTCREATOR_UTILS_EXPORT SynchronousProcess : public QObject -{ +class QTCREATOR_UTILS_EXPORT SynchronousProcess : public QObject { Q_OBJECT public: SynchronousProcess(); @@ -97,7 +95,7 @@ public: void setStdOutCodec(QTextCodec *c); QTextCodec *stdOutCodec() const; - QProcess::ProcessChannelMode processChannelMode () const; + QProcess::ProcessChannelMode processChannelMode() const; void setProcessChannelMode(QProcess::ProcessChannelMode m); bool stdOutBufferedSignalsEnabled() const; @@ -142,7 +140,6 @@ private: SynchronousProcessPrivate *m_d; }; - } // namespace Utils -#endif +#endif // ifndef SYNCHRONOUSPROCESS_H diff --git a/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.cpp b/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.cpp index c5dc21a15..6efdd0d48 100644 --- a/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.cpp +++ b/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.cpp @@ -4,25 +4,25 @@ * @file treewidgetcolumnstretcher.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,7 +33,7 @@ using namespace Utils; TreeWidgetColumnStretcher::TreeWidgetColumnStretcher(QTreeWidget *treeWidget, int columnToStretch) - : QObject(treeWidget->header()), m_columnToStretch(columnToStretch) + : QObject(treeWidget->header()), m_columnToStretch(columnToStretch) { parent()->installEventFilter(this); QHideEvent fake; @@ -44,18 +44,20 @@ bool TreeWidgetColumnStretcher::eventFilter(QObject *obj, QEvent *ev) { if (obj == parent()) { if (ev->type() == QEvent::Show) { - QHeaderView *hv = qobject_cast(obj); - for (int i = 0; i < hv->count(); ++i) + QHeaderView *hv = qobject_cast(obj); + for (int i = 0; i < hv->count(); ++i) { hv->setResizeMode(i, QHeaderView::Interactive); + } } else if (ev->type() == QEvent::Hide) { - QHeaderView *hv = qobject_cast(obj); - for (int i = 0; i < hv->count(); ++i) + QHeaderView *hv = qobject_cast(obj); + for (int i = 0; i < hv->count(); ++i) { hv->setResizeMode(i, i == m_columnToStretch ? QHeaderView::Stretch : QHeaderView::ResizeToContents); + } } else if (ev->type() == QEvent::Resize) { - QHeaderView *hv = qobject_cast(obj); + QHeaderView *hv = qobject_cast(obj); if (hv->resizeMode(m_columnToStretch) == QHeaderView::Interactive) { - QResizeEvent *re = static_cast(ev); - int diff = re->size().width() - re->oldSize().width() ; + QResizeEvent *re = static_cast(ev); + int diff = re->size().width() - re->oldSize().width(); hv->resizeSection(m_columnToStretch, qMax(32, hv->sectionSize(1) + diff)); } } diff --git a/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.h b/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.h index 7d6619b2c..da3659c43 100644 --- a/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.h +++ b/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.h @@ -4,25 +4,25 @@ * @file treewidgetcolumnstretcher.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,24 +37,21 @@ class QTreeWidget; QT_END_NAMESPACE namespace Utils { - /* -The class fixes QTreeWidget to resize all columns to contents, except one -stretching column. As opposed to standard QTreeWidget, all columns are -still interactively resizable. + The class fixes QTreeWidget to resize all columns to contents, except one + stretching column. As opposed to standard QTreeWidget, all columns are + still interactively resizable. -*/ + */ -class QTCREATOR_UTILS_EXPORT TreeWidgetColumnStretcher : public QObject -{ +class QTCREATOR_UTILS_EXPORT TreeWidgetColumnStretcher : public QObject { int m_columnToStretch; public: TreeWidgetColumnStretcher(QTreeWidget *treeWidget, int columnToStretch); bool eventFilter(QObject *obj, QEvent *ev); }; - } // namespace Utils #endif // TREEWIDGETCOLUMNSTRETCHER_H diff --git a/ground/openpilotgcs/src/libs/utils/uncommentselection.cpp b/ground/openpilotgcs/src/libs/utils/uncommentselection.cpp index 64f7ddcf9..1c3669f4c 100644 --- a/ground/openpilotgcs/src/libs/utils/uncommentselection.cpp +++ b/ground/openpilotgcs/src/libs/utils/uncommentselection.cpp @@ -4,25 +4,25 @@ * @file uncommentselection.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,24 +36,25 @@ void Utils::unCommentSelection(QPlainTextEdit *edit) { QTextCursor cursor = edit->textCursor(); QTextDocument *doc = cursor.document(); + cursor.beginEditBlock(); - int pos = cursor.position(); + int pos = cursor.position(); int anchor = cursor.anchor(); - int start = qMin(anchor, pos); - int end = qMax(anchor, pos); - bool anchorIsStart = (anchor == start); + int start = qMin(anchor, pos); + int end = qMax(anchor, pos); + bool anchorIsStart = (anchor == start); QTextBlock startBlock = doc->findBlock(start); - QTextBlock endBlock = doc->findBlock(end); + QTextBlock endBlock = doc->findBlock(end); if (end > start && endBlock.position() == end) { --end; endBlock = endBlock.previous(); } - bool doCStyleUncomment = false; - bool doCStyleComment = false; + bool doCStyleUncomment = false; + bool doCStyleComment = false; bool doCppStyleUncomment = false; bool hasSelection = cursor.hasSelection(); @@ -63,15 +64,15 @@ void Utils::unCommentSelection(QPlainTextEdit *edit) int startPos = start - startBlock.position(); bool hasLeadingCharacters = !startText.left(startPos).trimmed().isEmpty(); if ((startPos >= 2 - && startText.at(startPos-2) == QLatin1Char('/') - && startText.at(startPos-1) == QLatin1Char('*'))) { + && startText.at(startPos - 2) == QLatin1Char('/') + && startText.at(startPos - 1) == QLatin1Char('*'))) { startPos -= 2; - start -= 2; + start -= 2; } bool hasSelStart = (startPos < startText.length() - 2 && startText.at(startPos) == QLatin1Char('/') - && startText.at(startPos+1) == QLatin1Char('*')); + && startText.at(startPos + 1) == QLatin1Char('*')); QString endText = endBlock.text(); @@ -79,18 +80,18 @@ void Utils::unCommentSelection(QPlainTextEdit *edit) bool hasTrailingCharacters = !endText.left(endPos).remove(QLatin1String("//")).trimmed().isEmpty() && !endText.mid(endPos).trimmed().isEmpty(); if ((endPos <= endText.length() - 2 - && endText.at(endPos) == QLatin1Char('*') - && endText.at(endPos+1) == QLatin1Char('/'))) { + && endText.at(endPos) == QLatin1Char('*') + && endText.at(endPos + 1) == QLatin1Char('/'))) { endPos += 2; - end += 2; + end += 2; } bool hasSelEnd = (endPos >= 2 - && endText.at(endPos-2) == QLatin1Char('*') - && endText.at(endPos-1) == QLatin1Char('/')); + && endText.at(endPos - 2) == QLatin1Char('*') + && endText.at(endPos - 1) == QLatin1Char('/')); doCStyleUncomment = hasSelStart && hasSelEnd; - doCStyleComment = !doCStyleUncomment && (hasLeadingCharacters || hasTrailingCharacters); + doCStyleComment = !doCStyleUncomment && (hasLeadingCharacters || hasTrailingCharacters); } if (doCStyleUncomment) { @@ -127,8 +128,9 @@ void Utils::unCommentSelection(QPlainTextEdit *edit) cursor.removeSelectedText(); break; } - if (!text.at(i).isSpace()) + if (!text.at(i).isSpace()) { break; + } ++i; } } else { @@ -141,8 +143,9 @@ void Utils::unCommentSelection(QPlainTextEdit *edit) // adjust selection when commenting out if (hasSelection && !doCStyleUncomment && !doCppStyleUncomment) { cursor = edit->textCursor(); - if (!doCStyleComment) + if (!doCStyleComment) { start = startBlock.position(); // move the double slashes into the selection + } int lastSelPos = anchorIsStart ? cursor.position() : cursor.anchor(); if (anchorIsStart) { cursor.setPosition(start); @@ -156,4 +159,3 @@ void Utils::unCommentSelection(QPlainTextEdit *edit) cursor.endEditBlock(); } - diff --git a/ground/openpilotgcs/src/libs/utils/uncommentselection.h b/ground/openpilotgcs/src/libs/utils/uncommentselection.h index 6be0dca08..56c9f376d 100644 --- a/ground/openpilotgcs/src/libs/utils/uncommentselection.h +++ b/ground/openpilotgcs/src/libs/utils/uncommentselection.h @@ -4,25 +4,25 @@ * @file uncommentselection.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,9 +36,7 @@ class QPlainTextEdit; QT_END_NAMESPACE namespace Utils { - QTCREATOR_UTILS_EXPORT void unCommentSelection(QPlainTextEdit *edit); - } // end of namespace Utils #endif // UNCOMMENTSELECTION_H diff --git a/ground/openpilotgcs/src/libs/utils/utils_global.h b/ground/openpilotgcs/src/libs/utils/utils_global.h index 45bbf5b45..dce6149a7 100644 --- a/ground/openpilotgcs/src/libs/utils/utils_global.h +++ b/ground/openpilotgcs/src/libs/utils/utils_global.h @@ -4,25 +4,25 @@ * @file utils_global.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.cpp b/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.cpp index 1cb0d1e73..723316038 100644 --- a/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.cpp @@ -4,25 +4,25 @@ * @file welcomemodetreewidget.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,27 +34,26 @@ #include namespace Utils { - void WelcomeModeLabel::setStyledText(const QString &text) { - QString rc = QLatin1String( - "" - "" - "

" - ""); + QString rc = QLatin1String( + "" + "" + "

" + ""); + rc += text; rc += QLatin1String("


"); setText(rc); } -struct WelcomeModeTreeWidgetPrivate -{ +struct WelcomeModeTreeWidgetPrivate { WelcomeModeTreeWidgetPrivate() {} QIcon bullet; }; WelcomeModeTreeWidget::WelcomeModeTreeWidget(QWidget *parent) : - QTreeWidget(parent), m_d(new WelcomeModeTreeWidgetPrivate) + QTreeWidget(parent), m_d(new WelcomeModeTreeWidgetPrivate) { m_d->bullet = QIcon(QLatin1String(":/welcome/images/list_bullet_arrow.png")); connect(this, SIGNAL(itemClicked(QTreeWidgetItem *, int)), @@ -81,6 +80,7 @@ QSize WelcomeModeTreeWidget::sizeHint() const QTreeWidgetItem *WelcomeModeTreeWidget::addItem(const QString &label, const QString &data, const QString &toolTip) { QTreeWidgetItem *item = new QTreeWidgetItem(this); + item->setIcon(0, m_d->bullet); item->setSizeHint(0, QSize(24, 30)); QLabel *lbl = new QLabel(label); @@ -90,21 +90,22 @@ QTreeWidgetItem *WelcomeModeTreeWidget::addItem(const QString &label, const QStr QBoxLayout *lay = new QVBoxLayout; lay->setContentsMargins(3, 2, 0, 0); lay->addWidget(lbl); - QWidget *wdg = new QWidget; + QWidget *wdg = new QWidget; wdg->setLayout(lay); setItemWidget(item, 1, wdg); item->setData(0, Qt::UserRole, data); - if (!toolTip.isEmpty()) + if (!toolTip.isEmpty()) { wdg->setToolTip(toolTip); + } return item; - } void WelcomeModeTreeWidget::slotAddNewsItem(const QString &title, const QString &description, const QString &link) { - int itemWidth = width()-header()->sectionSize(0); + int itemWidth = width() - header()->sectionSize(0); QFont f = font(); - QString elidedText = QFontMetrics(f).elidedText(description, Qt::ElideRight, itemWidth); + QString elidedText = QFontMetrics(f).elidedText(description, Qt::ElideRight, itemWidth); + f.setBold(true); QString elidedTitle = QFontMetrics(f).elidedText(title, Qt::ElideRight, itemWidth); QString data = QString::fromLatin1("%1
%2").arg(elidedTitle).arg(elidedText); @@ -115,5 +116,4 @@ void WelcomeModeTreeWidget::slotItemClicked(QTreeWidgetItem *item) { emit activated(item->data(0, Qt::UserRole).toString()); } - } diff --git a/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.h b/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.h index d97cfd0c1..67374c469 100644 --- a/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.h +++ b/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.h @@ -4,25 +4,25 @@ * @file welcomemodetreewidget.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,12 +35,10 @@ #include namespace Utils { - struct WelcomeModeTreeWidgetPrivate; struct WelcomeModeLabelPrivate; -class QTCREATOR_UTILS_EXPORT WelcomeModeLabel : public QLabel -{ +class QTCREATOR_UTILS_EXPORT WelcomeModeLabel : public QLabel { Q_OBJECT public: WelcomeModeLabel(QWidget *parent) : QLabel(parent) {}; @@ -48,14 +46,13 @@ public: WelcomeModeLabelPrivate *m_d; }; -class QTCREATOR_UTILS_EXPORT WelcomeModeTreeWidget : public QTreeWidget -{ +class QTCREATOR_UTILS_EXPORT WelcomeModeTreeWidget : public QTreeWidget { Q_OBJECT public: WelcomeModeTreeWidget(QWidget *parent = 0); ~WelcomeModeTreeWidget(); - QTreeWidgetItem *addItem(const QString &label, const QString &data,const QString &toolTip = QString::null); + QTreeWidgetItem *addItem(const QString &label, const QString &data, const QString &toolTip = QString::null); public slots: void slotAddNewsItem(const QString &title, const QString &description, const QString &link); @@ -73,7 +70,6 @@ private slots: private: WelcomeModeTreeWidgetPrivate *m_d; }; - } #endif // WELCOMEMODETREEWIDGET_H diff --git a/ground/openpilotgcs/src/libs/utils/winutils.cpp b/ground/openpilotgcs/src/libs/utils/winutils.cpp index ae80f26ef..67dba1bb6 100644 --- a/ground/openpilotgcs/src/libs/utils/winutils.cpp +++ b/ground/openpilotgcs/src/libs/utils/winutils.cpp @@ -4,25 +4,25 @@ * @file winutils.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,15 +36,15 @@ #include namespace Utils { - QTCREATOR_UTILS_EXPORT QString winErrorMessage(unsigned long error) { - QString rc = QString::fromLatin1("#%1: ").arg(error); + QString rc = QString::fromLatin1("#%1: ").arg(error); ushort *lpMsgBuf; const int len = FormatMessage( - FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, - NULL, error, 0, (LPTSTR)&lpMsgBuf, 0, NULL); + FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, + NULL, error, 0, (LPTSTR)&lpMsgBuf, 0, NULL); + if (len) { rc = QString::fromUtf16(lpMsgBuf, len); LocalFree(lpMsgBuf); @@ -59,9 +59,9 @@ QTCREATOR_UTILS_EXPORT QString winGetDLLVersion(WinDLLVersionType t, QString *errorMessage) { // Resolve required symbols from the version.dll - typedef DWORD (APIENTRY *GetFileVersionInfoSizeProtoType)(LPCTSTR, LPDWORD); - typedef BOOL (APIENTRY *GetFileVersionInfoWProtoType)(LPCWSTR, DWORD, DWORD, LPVOID); - typedef BOOL (APIENTRY *VerQueryValueWProtoType)(const LPVOID, LPWSTR lpSubBlock, LPVOID, PUINT); + typedef DWORD (APIENTRY * GetFileVersionInfoSizeProtoType)(LPCTSTR, LPDWORD); + typedef BOOL (APIENTRY * GetFileVersionInfoWProtoType)(LPCWSTR, DWORD, DWORD, LPVOID); + typedef BOOL (APIENTRY * VerQueryValueWProtoType)(const LPVOID, LPWSTR lpSubBlock, LPVOID, PUINT); const char *versionDLLC = "version.dll"; QLibrary versionLib(QLatin1String(versionDLLC), 0); @@ -81,7 +81,7 @@ QTCREATOR_UTILS_EXPORT QString winGetDLLVersion(WinDLLVersionType t, // Now go ahead, read version info resource DWORD dummy = 0; const LPCTSTR fileName = reinterpret_cast(name.utf16()); // MinGWsy - const DWORD infoSize = (*getFileVersionInfoSizeW)(fileName, &dummy); + const DWORD infoSize = (*getFileVersionInfoSizeW)(fileName, &dummy); if (infoSize == 0) { *errorMessage = QString::fromLatin1("Unable to determine the size of the version information of %1: %2").arg(name, winErrorMessage(GetLastError())); return QString(); @@ -92,12 +92,12 @@ QTCREATOR_UTILS_EXPORT QString winGetDLLVersion(WinDLLVersionType t, *errorMessage = QString::fromLatin1("Unable to determine the version information of %1: %2").arg(name, winErrorMessage(GetLastError())); return QString(); } - VS_FIXEDFILEINFO *versionInfo; + VS_FIXEDFILEINFO *versionInfo; UINT len = 0; if (!(*verQueryValueW)(data, TEXT("\\"), &versionInfo, &len)) { *errorMessage = QString::fromLatin1("Unable to determine version string of %1: %2").arg(name, winErrorMessage(GetLastError())); return QString(); - } + } QString rc; switch (t) { case WinDLLFileVersion: @@ -109,5 +109,4 @@ QTCREATOR_UTILS_EXPORT QString winGetDLLVersion(WinDLLVersionType t, } return rc; } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/winutils.h b/ground/openpilotgcs/src/libs/utils/winutils.h index acd8fe506..7d268e011 100644 --- a/ground/openpilotgcs/src/libs/utils/winutils.h +++ b/ground/openpilotgcs/src/libs/utils/winutils.h @@ -4,25 +4,25 @@ * @file winutils.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,7 +36,6 @@ class QString; QT_END_NAMESPACE namespace Utils { - // Helper to format a Windows error message, taking the // code as returned by the GetLastError()-API. QTCREATOR_UTILS_EXPORT QString winErrorMessage(unsigned long error); diff --git a/ground/openpilotgcs/src/libs/utils/worldmagmodel.cpp b/ground/openpilotgcs/src/libs/utils/worldmagmodel.cpp index d93588684..37a6f356f 100644 --- a/ground/openpilotgcs/src/libs/utils/worldmagmodel.cpp +++ b/ground/openpilotgcs/src/libs/utils/worldmagmodel.cpp @@ -49,1037 +49,1008 @@ #include #include -#define RAD2DEG(rad) ((rad) * (180.0 / M_PI)) -#define DEG2RAD(deg) ((deg) * (M_PI / 180.0)) +#define RAD2DEG(rad) ((rad) * (180.0 / M_PI)) +#define DEG2RAD(deg) ((deg) * (M_PI / 180.0)) // updated coeffs available from http://www.ngdc.noaa.gov/geomag/WMM/wmm_ddownload.shtml const double CoeffFile[91][6] = { - {0, 0, 0, 0, 0, 0}, - {1, 0, -29496.6, 0.0, 11.6, 0.0}, - {1, 1, -1586.3, 4944.4, 16.5, -25.9}, - {2, 0, -2396.6, 0.0, -12.1, 0.0}, - {2, 1, 3026.1, -2707.7, -4.4, -22.5}, - {2, 2, 1668.6, -576.1, 1.9, -11.8}, - {3, 0, 1340.1, 0.0, 0.4, 0.0}, - {3, 1, -2326.2, -160.2, -4.1, 7.3}, - {3, 2, 1231.9, 251.9, -2.9, -3.9}, - {3, 3, 634.0, -536.6, -7.7, -2.6}, - {4, 0, 912.6, 0.0, -1.8, 0.0}, - {4, 1, 808.9, 286.4, 2.3, 1.1}, - {4, 2, 166.7, -211.2, -8.7, 2.7}, - {4, 3, -357.1, 164.3, 4.6, 3.9}, - {4, 4, 89.4, -309.1, -2.1, -0.8}, - {5, 0, -230.9, 0.0, -1.0, 0.0}, - {5, 1, 357.2, 44.6, 0.6, 0.4}, - {5, 2, 200.3, 188.9, -1.8, 1.8}, - {5, 3, -141.1, -118.2, -1.0, 1.2}, - {5, 4, -163.0, 0.0, 0.9, 4.0}, - {5, 5, -7.8, 100.9, 1.0, -0.6}, - {6, 0, 72.8, 0.0, -0.2, 0.0}, - {6, 1, 68.6, -20.8, -0.2, -0.2}, - {6, 2, 76.0, 44.1, -0.1, -2.1}, - {6, 3, -141.4, 61.5, 2.0, -0.4}, - {6, 4, -22.8, -66.3, -1.7, -0.6}, - {6, 5, 13.2, 3.1, -0.3, 0.5}, - {6, 6, -77.9, 55.0, 1.7, 0.9}, - {7, 0, 80.5, 0.0, 0.1, 0.0}, - {7, 1, -75.1, -57.9, -0.1, 0.7}, - {7, 2, -4.7, -21.1, -0.6, 0.3}, - {7, 3, 45.3, 6.5, 1.3, -0.1}, - {7, 4, 13.9, 24.9, 0.4, -0.1}, - {7, 5, 10.4, 7.0, 0.3, -0.8}, - {7, 6, 1.7, -27.7, -0.7, -0.3}, - {7, 7, 4.9, -3.3, 0.6, 0.3}, - {8, 0, 24.4, 0.0, -0.1, 0.0}, - {8, 1, 8.1, 11.0, 0.1, -0.1}, - {8, 2, -14.5, -20.0, -0.6, 0.2}, - {8, 3, -5.6, 11.9, 0.2, 0.4}, - {8, 4, -19.3, -17.4, -0.2, 0.4}, - {8, 5, 11.5, 16.7, 0.3, 0.1}, - {8, 6, 10.9, 7.0, 0.3, -0.1}, - {8, 7, -14.1, -10.8, -0.6, 0.4}, - {8, 8, -3.7, 1.7, 0.2, 0.3}, - {9, 0, 5.4, 0.0, 0.0, 0.0}, - {9, 1, 9.4, -20.5, -0.1, 0.0}, - {9, 2, 3.4, 11.5, 0.0, -0.2}, - {9, 3, -5.2, 12.8, 0.3, 0.0}, - {9, 4, 3.1, -7.2, -0.4, -0.1}, - {9, 5, -12.4, -7.4, -0.3, 0.1}, - {9, 6, -0.7, 8.0, 0.1, 0.0}, - {9, 7, 8.4, 2.1, -0.1, -0.2}, - {9, 8, -8.5, -6.1, -0.4, 0.3}, - {9, 9, -10.1, 7.0, -0.2, 0.2}, - {10, 0, -2.0, 0.0, 0.0, 0.0}, - {10, 1, -6.3, 2.8, 0.0, 0.1}, - {10, 2, 0.9, -0.1, -0.1, -0.1}, - {10, 3, -1.1, 4.7, 0.2, 0.0}, - {10, 4, -0.2, 4.4, 0.0, -0.1}, - {10, 5, 2.5, -7.2, -0.1, -0.1}, - {10, 6, -0.3, -1.0, -0.2, 0.0}, - {10, 7, 2.2, -3.9, 0.0, -0.1}, - {10, 8, 3.1, -2.0, -0.1, -0.2}, - {10, 9, -1.0, -2.0, -0.2, 0.0}, - {10, 10, -2.8, -8.3, -0.2, -0.1}, - {11, 0, 3.0, 0.0, 0.0, 0.0}, - {11, 1, -1.5, 0.2, 0.0, 0.0}, - {11, 2, -2.1, 1.7, 0.0, 0.1}, - {11, 3, 1.7, -0.6, 0.1, 0.0}, - {11, 4, -0.5, -1.8, 0.0, 0.1}, - {11, 5, 0.5, 0.9, 0.0, 0.0}, - {11, 6, -0.8, -0.4, 0.0, 0.1}, - {11, 7, 0.4, -2.5, 0.0, 0.0}, - {11, 8, 1.8, -1.3, 0.0, -0.1}, - {11, 9, 0.1, -2.1, 0.0, -0.1}, - {11, 10, 0.7, -1.9, -0.1, 0.0}, - {11, 11, 3.8, -1.8, 0.0, -0.1}, - {12, 0, -2.2, 0.0, 0.0, 0.0}, - {12, 1, -0.2, -0.9, 0.0, 0.0}, - {12, 2, 0.3, 0.3, 0.1, 0.0}, - {12, 3, 1.0, 2.1, 0.1, 0.0}, - {12, 4, -0.6, -2.5, -0.1, 0.0}, - {12, 5, 0.9, 0.5, 0.0, 0.0}, - {12, 6, -0.1, 0.6, 0.0, 0.1}, - {12, 7, 0.5, 0.0, 0.0, 0.0}, - {12, 8, -0.4, 0.1, 0.0, 0.0}, - {12, 9, -0.4, 0.3, 0.0, 0.0}, - {12, 10, 0.2, -0.9, 0.0, 0.0}, - {12, 11, -0.8, -0.2, -0.1, 0.0}, - {12, 12, 0.0, 0.9, 0.1, 0.0} + { 0, 0, 0, 0, 0, 0 }, + { 1, 0, -29496.6, 0.0, 11.6, 0.0 }, + { 1, 1, -1586.3, 4944.4, 16.5, -25.9 }, + { 2, 0, -2396.6, 0.0, -12.1, 0.0 }, + { 2, 1, 3026.1, -2707.7, -4.4, -22.5 }, + { 2, 2, 1668.6, -576.1, 1.9, -11.8 }, + { 3, 0, 1340.1, 0.0, 0.4, 0.0 }, + { 3, 1, -2326.2, -160.2, -4.1, 7.3 }, + { 3, 2, 1231.9, 251.9, -2.9, -3.9 }, + { 3, 3, 634.0, -536.6, -7.7, -2.6 }, + { 4, 0, 912.6, 0.0, -1.8, 0.0 }, + { 4, 1, 808.9, 286.4, 2.3, 1.1 }, + { 4, 2, 166.7, -211.2, -8.7, 2.7 }, + { 4, 3, -357.1, 164.3, 4.6, 3.9 }, + { 4, 4, 89.4, -309.1, -2.1, -0.8 }, + { 5, 0, -230.9, 0.0, -1.0, 0.0 }, + { 5, 1, 357.2, 44.6, 0.6, 0.4 }, + { 5, 2, 200.3, 188.9, -1.8, 1.8 }, + { 5, 3, -141.1, -118.2, -1.0, 1.2 }, + { 5, 4, -163.0, 0.0, 0.9, 4.0 }, + { 5, 5, -7.8, 100.9, 1.0, -0.6 }, + { 6, 0, 72.8, 0.0, -0.2, 0.0 }, + { 6, 1, 68.6, -20.8, -0.2, -0.2 }, + { 6, 2, 76.0, 44.1, -0.1, -2.1 }, + { 6, 3, -141.4, 61.5, 2.0, -0.4 }, + { 6, 4, -22.8, -66.3, -1.7, -0.6 }, + { 6, 5, 13.2, 3.1, -0.3, 0.5 }, + { 6, 6, -77.9, 55.0, 1.7, 0.9 }, + { 7, 0, 80.5, 0.0, 0.1, 0.0 }, + { 7, 1, -75.1, -57.9, -0.1, 0.7 }, + { 7, 2, -4.7, -21.1, -0.6, 0.3 }, + { 7, 3, 45.3, 6.5, 1.3, -0.1 }, + { 7, 4, 13.9, 24.9, 0.4, -0.1 }, + { 7, 5, 10.4, 7.0, 0.3, -0.8 }, + { 7, 6, 1.7, -27.7, -0.7, -0.3 }, + { 7, 7, 4.9, -3.3, 0.6, 0.3 }, + { 8, 0, 24.4, 0.0, -0.1, 0.0 }, + { 8, 1, 8.1, 11.0, 0.1, -0.1 }, + { 8, 2, -14.5, -20.0, -0.6, 0.2 }, + { 8, 3, -5.6, 11.9, 0.2, 0.4 }, + { 8, 4, -19.3, -17.4, -0.2, 0.4 }, + { 8, 5, 11.5, 16.7, 0.3, 0.1 }, + { 8, 6, 10.9, 7.0, 0.3, -0.1 }, + { 8, 7, -14.1, -10.8, -0.6, 0.4 }, + { 8, 8, -3.7, 1.7, 0.2, 0.3 }, + { 9, 0, 5.4, 0.0, 0.0, 0.0 }, + { 9, 1, 9.4, -20.5, -0.1, 0.0 }, + { 9, 2, 3.4, 11.5, 0.0, -0.2 }, + { 9, 3, -5.2, 12.8, 0.3, 0.0 }, + { 9, 4, 3.1, -7.2, -0.4, -0.1 }, + { 9, 5, -12.4, -7.4, -0.3, 0.1 }, + { 9, 6, -0.7, 8.0, 0.1, 0.0 }, + { 9, 7, 8.4, 2.1, -0.1, -0.2 }, + { 9, 8, -8.5, -6.1, -0.4, 0.3 }, + { 9, 9, -10.1, 7.0, -0.2, 0.2 }, + { 10, 0, -2.0, 0.0, 0.0, 0.0 }, + { 10, 1, -6.3, 2.8, 0.0, 0.1 }, + { 10, 2, 0.9, -0.1, -0.1, -0.1 }, + { 10, 3, -1.1, 4.7, 0.2, 0.0 }, + { 10, 4, -0.2, 4.4, 0.0, -0.1 }, + { 10, 5, 2.5, -7.2, -0.1, -0.1 }, + { 10, 6, -0.3, -1.0, -0.2, 0.0 }, + { 10, 7, 2.2, -3.9, 0.0, -0.1 }, + { 10, 8, 3.1, -2.0, -0.1, -0.2 }, + { 10, 9, -1.0, -2.0, -0.2, 0.0 }, + { 10, 10, -2.8, -8.3, -0.2, -0.1 }, + { 11, 0, 3.0, 0.0, 0.0, 0.0 }, + { 11, 1, -1.5, 0.2, 0.0, 0.0 }, + { 11, 2, -2.1, 1.7, 0.0, 0.1 }, + { 11, 3, 1.7, -0.6, 0.1, 0.0 }, + { 11, 4, -0.5, -1.8, 0.0, 0.1 }, + { 11, 5, 0.5, 0.9, 0.0, 0.0 }, + { 11, 6, -0.8, -0.4, 0.0, 0.1 }, + { 11, 7, 0.4, -2.5, 0.0, 0.0 }, + { 11, 8, 1.8, -1.3, 0.0, -0.1 }, + { 11, 9, 0.1, -2.1, 0.0, -0.1 }, + { 11, 10, 0.7, -1.9, -0.1, 0.0 }, + { 11, 11, 3.8, -1.8, 0.0, -0.1 }, + { 12, 0, -2.2, 0.0, 0.0, 0.0 }, + { 12, 1, -0.2, -0.9, 0.0, 0.0 }, + { 12, 2, 0.3, 0.3, 0.1, 0.0 }, + { 12, 3, 1.0, 2.1, 0.1, 0.0 }, + { 12, 4, -0.6, -2.5, -0.1, 0.0 }, + { 12, 5, 0.9, 0.5, 0.0, 0.0 }, + { 12, 6, -0.1, 0.6, 0.0, 0.1 }, + { 12, 7, 0.5, 0.0, 0.0, 0.0 }, + { 12, 8, -0.4, 0.1, 0.0, 0.0 }, + { 12, 9, -0.4, 0.3, 0.0, 0.0 }, + { 12, 10, 0.2, -0.9, 0.0, 0.0 }, + { 12, 11, -0.8, -0.2, -0.1, 0.0 }, + { 12, 12, 0.0, 0.9, 0.1, 0.0 } }; namespace Utils { +WorldMagModel::WorldMagModel() +{ + Initialize(); +} - WorldMagModel::WorldMagModel() - { - Initialize(); +/** + * @brief + * @param[in] LLA The longitude-latitude-altitude coordinate to compute the magnetic field at + * @param[in] Month + * @param[in] Day + * @param[in] Year + * @param[out] Be The resulting magnetic field at that location and time in [mGau](?) + * @returns 0 if successful, negative otherwise. + */ +int WorldMagModel::GetMagVector(double LLA[3], int Month, int Day, int Year, double Be[3]) +{ + double Lat = LLA[0]; + double Lon = LLA[1]; + double AltEllipsoid = LLA[2] / 1000.0; // convert to km + + // *********** + // range check supplied params + + if (Lat < -90) { + return -1; // error } + if (Lat > 90) { + return -2; // error + } + if (Lon < -180) { + return -3; // error + } + if (Lon > 180) { + return -4; // error + } + // *********** - /** - * @brief - * @param[in] LLA The longitude-latitude-altitude coordinate to compute the magnetic field at - * @param[in] Month - * @param[in] Day - * @param[in] Year - * @param[out] Be The resulting magnetic field at that location and time in [mGau](?) - * @returns 0 if successful, negative otherwise. + WMMtype_CoordSpherical CoordSpherical; + WMMtype_CoordGeodetic CoordGeodetic; + WMMtype_GeoMagneticElements GeoMagneticElements; + + Initialize(); + + CoordGeodetic.lambda = Lon; + CoordGeodetic.phi = Lat; + CoordGeodetic.HeightAboveEllipsoid = AltEllipsoid; + + // Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report + GeodeticToSpherical(&CoordGeodetic, &CoordSpherical); + + if (DateToYear(Month, Day, Year) < 0) { + return -5; // error + } + // Compute the geoMagnetic field elements and their time change + if (Geomag(&CoordSpherical, &CoordGeodetic, &GeoMagneticElements) < 0) { + return -6; // error + } + // set the returned values + Be[0] = GeoMagneticElements.X * 1e-2; + Be[1] = GeoMagneticElements.Y * 1e-2; + Be[2] = GeoMagneticElements.Z * 1e-2; + + // *********** + + return 0; // OK +} + +void WorldMagModel::Initialize() +{ // Sets default values for WMM subroutines. + // UPDATES : Ellip and MagneticModel + + // Sets WGS-84 parameters + Ellip.a = 6378.137; // semi-major axis of the ellipsoid in km + Ellip.b = 6356.7523142; // semi-minor axis of the ellipsoid in km + Ellip.fla = 1 / 298.257223563; // flattening + Ellip.eps = sqrt(1 - (Ellip.b * Ellip.b) / (Ellip.a * Ellip.a)); // first eccentricity + Ellip.epssq = (Ellip.eps * Ellip.eps); // first eccentricity squared + Ellip.re = 6371.2; // Earth's radius in km + + // Sets Magnetic Model parameters + MagneticModel.nMax = WMM_MAX_MODEL_DEGREES; + MagneticModel.nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES; + MagneticModel.SecularVariationUsed = 0; + + // Really, Really needs to be read from a file - out of date in 2015 at latest + MagneticModel.EditionDate = 5.7863328170559505e-307; + MagneticModel.epoch = 2010.0; + sprintf(MagneticModel.ModelName, "WMM-2010"); +} + + +int WorldMagModel::Geomag(WMMtype_CoordSpherical *CoordSpherical, WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_GeoMagneticElements *GeoMagneticElements) +/* + The main subroutine that calls a sequence of WMM sub-functions to calculate the magnetic field elements for a single point. + The function expects the model coefficients and point coordinates as input and returns the magnetic field elements and + their rate of change. Though, this subroutine can be called successively to calculate a time series, profile or grid + of magnetic field, these are better achieved by the subroutine WMM_Grid. + + INPUT: Ellip + CoordSpherical + CoordGeodetic + TimedMagneticModel + + OUTPUT : GeoMagneticElements + */ +{ + WMMtype_MagneticResults MagneticResultsSph; + WMMtype_MagneticResults MagneticResultsGeo; + WMMtype_MagneticResults MagneticResultsSphVar; + WMMtype_MagneticResults MagneticResultsGeoVar; + WMMtype_LegendreFunction LegendreFunction; + WMMtype_SphericalHarmonicVariables SphVariables; + + // Compute Spherical Harmonic variables + ComputeSphericalHarmonicVariables(CoordSpherical, MagneticModel.nMax, &SphVariables); + + // Compute ALF + if (AssociatedLegendreFunction(CoordSpherical, MagneticModel.nMax, &LegendreFunction) < 0) { + return -1; // error + } + // Accumulate the spherical harmonic coefficients + Summation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSph); + + // Sum the Secular Variation Coefficients + SecVarSummation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSphVar); + + // Map the computed Magnetic fields to Geodeitic coordinates + RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSph, &MagneticResultsGeo); + + // Map the secular variation field components to Geodetic coordinates + RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSphVar, &MagneticResultsGeoVar); + + // Calculate the Geomagnetic elements, Equation 18 , WMM Technical report + CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); + + // Calculate the secular variation of each of the Geomagnetic elements + CalculateSecularVariation(&MagneticResultsGeoVar, GeoMagneticElements); + + return 0; // OK +} + +void WorldMagModel::ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_SphericalHarmonicVariables *SphVariables) +{ + /* Computes Spherical variables + Variables computed are (a/r)^(n+2), cos_m(lamda) and sin_m(lambda) for spherical harmonic + summations. (Equations 10-12 in the WMM Technical Report) + INPUT Ellip data structure with the following elements + float a; semi-major axis of the ellipsoid + float b; semi-minor axis of the ellipsoid + float fla; flattening + float epssq; first eccentricity squared + float eps; first eccentricity + float re; mean radius of ellipsoid + CoordSpherical A data structure with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) + nMax integer ( Maxumum degree of spherical harmonic secular model)\ + + OUTPUT SphVariables Pointer to the data structure with the following elements + float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES+1]; [earth_reference_radius_km sph. radius ]^n + float cos_mlambda[WMM_MAX_MODEL_DEGREES+1]; cp(m) - cosine of (mspherical coord. longitude) + float sin_mlambda[WMM_MAX_MODEL_DEGREES+1]; sp(m) - sine of (mspherical coord. longitude) */ - int WorldMagModel::GetMagVector(double LLA[3], int Month, int Day, int Year, double Be[3]) - { - double Lat = LLA[0]; - double Lon = LLA[1]; - double AltEllipsoid = LLA[2]/1000.0; // convert to km + double cos_lambda = cos(DEG2RAD(CoordSpherical->lambda)); + double sin_lambda = sin(DEG2RAD(CoordSpherical->lambda)); - // *********** - // range check supplied params + /* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2) + for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ - if (Lat < -90) return -1; // error - if (Lat > 90) return -2; // error - - if (Lon < -180) return -3; // error - if (Lon > 180) return -4; // error - - // *********** - - WMMtype_CoordSpherical CoordSpherical; - WMMtype_CoordGeodetic CoordGeodetic; - WMMtype_GeoMagneticElements GeoMagneticElements; - - Initialize(); - - CoordGeodetic.lambda = Lon; - CoordGeodetic.phi = Lat; - CoordGeodetic.HeightAboveEllipsoid = AltEllipsoid; - - // Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report - GeodeticToSpherical(&CoordGeodetic, &CoordSpherical); - - if (DateToYear(Month, Day, Year) < 0) - return -5; // error - - // Compute the geoMagnetic field elements and their time change - if (Geomag(&CoordSpherical, &CoordGeodetic, &GeoMagneticElements) < 0) - return -6; // error - - // set the returned values - Be[0] = GeoMagneticElements.X * 1e-2; - Be[1] = GeoMagneticElements.Y * 1e-2; - Be[2] = GeoMagneticElements.Z * 1e-2; - - // *********** - - return 0; // OK + SphVariables->RelativeRadiusPower[0] = (Ellip.re / CoordSpherical->r) * (Ellip.re / CoordSpherical->r); + for (int n = 1; n <= nMax; n++) { + SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n - 1] * (Ellip.re / CoordSpherical->r); } - void WorldMagModel::Initialize() - { // Sets default values for WMM subroutines. - // UPDATES : Ellip and MagneticModel - - // Sets WGS-84 parameters - Ellip.a = 6378.137; // semi-major axis of the ellipsoid in km - Ellip.b = 6356.7523142; // semi-minor axis of the ellipsoid in km - Ellip.fla = 1 / 298.257223563; // flattening - Ellip.eps = sqrt(1 - (Ellip.b * Ellip.b) / (Ellip.a * Ellip.a)); // first eccentricity - Ellip.epssq = (Ellip.eps * Ellip.eps); // first eccentricity squared - Ellip.re = 6371.2; // Earth's radius in km - - // Sets Magnetic Model parameters - MagneticModel.nMax = WMM_MAX_MODEL_DEGREES; - MagneticModel.nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES; - MagneticModel.SecularVariationUsed = 0; - - // Really, Really needs to be read from a file - out of date in 2015 at latest - MagneticModel.EditionDate = 5.7863328170559505e-307; - MagneticModel.epoch = 2010.0; - sprintf(MagneticModel.ModelName, "WMM-2010"); - } - - - int WorldMagModel::Geomag(WMMtype_CoordSpherical *CoordSpherical, WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_GeoMagneticElements *GeoMagneticElements) /* - The main subroutine that calls a sequence of WMM sub-functions to calculate the magnetic field elements for a single point. - The function expects the model coefficients and point coordinates as input and returns the magnetic field elements and - their rate of change. Though, this subroutine can be called successively to calculate a time series, profile or grid - of magnetic field, these are better achieved by the subroutine WMM_Grid. + Compute cos(m*lambda), sin(m*lambda) for m = 0 ... nMax + cos(a + b) = cos(a)*cos(b) - sin(a)*sin(b) + sin(a + b) = cos(a)*sin(b) + sin(a)*cos(b) + */ + SphVariables->cos_mlambda[0] = 1.0; + SphVariables->sin_mlambda[0] = 0.0; - INPUT: Ellip - CoordSpherical - CoordGeodetic - TimedMagneticModel + SphVariables->cos_mlambda[1] = cos_lambda; + SphVariables->sin_mlambda[1] = sin_lambda; + for (int m = 2; m <= nMax; m++) { + SphVariables->cos_mlambda[m] = SphVariables->cos_mlambda[m - 1] * cos_lambda - SphVariables->sin_mlambda[m - 1] * sin_lambda; + SphVariables->sin_mlambda[m] = SphVariables->cos_mlambda[m - 1] * sin_lambda + SphVariables->sin_mlambda[m - 1] * cos_lambda; + } +} - OUTPUT : GeoMagneticElements - */ - { - WMMtype_MagneticResults MagneticResultsSph; - WMMtype_MagneticResults MagneticResultsGeo; - WMMtype_MagneticResults MagneticResultsSphVar; - WMMtype_MagneticResults MagneticResultsGeoVar; - WMMtype_LegendreFunction LegendreFunction; - WMMtype_SphericalHarmonicVariables SphVariables; +int WorldMagModel::AssociatedLegendreFunction(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_LegendreFunction *LegendreFunction) +{ + /* Computes all of the Schmidt-semi normalized associated Legendre + functions up to degree nMax. If nMax <= 16, function WMM_PcupLow is used. + Otherwise WMM_PcupHigh is called. + INPUT CoordSpherical A data structure with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) + nMax integer ( Maxumum degree of spherical harmonic secular model) + LegendreFunction Pointer to data structure with the following elements + float *Pcup; ( pointer to store Legendre Function ) + float *dPcup; ( pointer to store Derivative of Lagendre function ) - // Compute Spherical Harmonic variables - ComputeSphericalHarmonicVariables(CoordSpherical, MagneticModel.nMax, &SphVariables); + OUTPUT LegendreFunction Calculated Legendre variables in the data structure + */ - // Compute ALF - if (AssociatedLegendreFunction(CoordSpherical, MagneticModel.nMax, &LegendreFunction) < 0) + double sin_phi = sin(DEG2RAD(CoordSpherical->phig)); // sin (geocentric latitude) + + if (nMax <= 16 || (1 - fabs(sin_phi)) < 1.0e-10) { /* If nMax is less tha 16 or at the poles */ + PcupLow(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax); + } else { + if (PcupHigh(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0) { return -1; // error - - // Accumulate the spherical harmonic coefficients - Summation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSph); - - // Sum the Secular Variation Coefficients - SecVarSummation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSphVar); - - // Map the computed Magnetic fields to Geodeitic coordinates - RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSph, &MagneticResultsGeo); - - // Map the secular variation field components to Geodetic coordinates - RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSphVar, &MagneticResultsGeoVar); - - // Calculate the Geomagnetic elements, Equation 18 , WMM Technical report - CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); - - // Calculate the secular variation of each of the Geomagnetic elements - CalculateSecularVariation(&MagneticResultsGeoVar, GeoMagneticElements); - - return 0; // OK - } - - void WorldMagModel::ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_SphericalHarmonicVariables *SphVariables) - { - /* Computes Spherical variables - Variables computed are (a/r)^(n+2), cos_m(lamda) and sin_m(lambda) for spherical harmonic - summations. (Equations 10-12 in the WMM Technical Report) - INPUT Ellip data structure with the following elements - float a; semi-major axis of the ellipsoid - float b; semi-minor axis of the ellipsoid - float fla; flattening - float epssq; first eccentricity squared - float eps; first eccentricity - float re; mean radius of ellipsoid - CoordSpherical A data structure with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) - nMax integer ( Maxumum degree of spherical harmonic secular model)\ - - OUTPUT SphVariables Pointer to the data structure with the following elements - float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES+1]; [earth_reference_radius_km sph. radius ]^n - float cos_mlambda[WMM_MAX_MODEL_DEGREES+1]; cp(m) - cosine of (mspherical coord. longitude) - float sin_mlambda[WMM_MAX_MODEL_DEGREES+1]; sp(m) - sine of (mspherical coord. longitude) - */ - double cos_lambda = cos(DEG2RAD(CoordSpherical->lambda)); - double sin_lambda = sin(DEG2RAD(CoordSpherical->lambda)); - - /* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2) - for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ - - SphVariables->RelativeRadiusPower[0] = (Ellip.re / CoordSpherical->r) * (Ellip.re / CoordSpherical->r); - for (int n = 1; n <= nMax; n++) - SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n - 1] * (Ellip.re / CoordSpherical->r); - - /* - Compute cos(m*lambda), sin(m*lambda) for m = 0 ... nMax - cos(a + b) = cos(a)*cos(b) - sin(a)*sin(b) - sin(a + b) = cos(a)*sin(b) + sin(a)*cos(b) - */ - SphVariables->cos_mlambda[0] = 1.0; - SphVariables->sin_mlambda[0] = 0.0; - - SphVariables->cos_mlambda[1] = cos_lambda; - SphVariables->sin_mlambda[1] = sin_lambda; - for (int m = 2; m <= nMax; m++) - { - SphVariables->cos_mlambda[m] = SphVariables->cos_mlambda[m - 1] * cos_lambda - SphVariables->sin_mlambda[m - 1] * sin_lambda; - SphVariables->sin_mlambda[m] = SphVariables->cos_mlambda[m - 1] * sin_lambda + SphVariables->sin_mlambda[m - 1] * cos_lambda; } } - int WorldMagModel::AssociatedLegendreFunction(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_LegendreFunction *LegendreFunction) - { - /* Computes all of the Schmidt-semi normalized associated Legendre - functions up to degree nMax. If nMax <= 16, function WMM_PcupLow is used. - Otherwise WMM_PcupHigh is called. - INPUT CoordSpherical A data structure with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) - nMax integer ( Maxumum degree of spherical harmonic secular model) - LegendreFunction Pointer to data structure with the following elements - float *Pcup; ( pointer to store Legendre Function ) - float *dPcup; ( pointer to store Derivative of Lagendre function ) + return 0; // OK +} - OUTPUT LegendreFunction Calculated Legendre variables in the data structure - */ +void WorldMagModel::Summation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables *SphVariables, + WMMtype_CoordSpherical *CoordSpherical, + WMMtype_MagneticResults *MagneticResults) +{ + /* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using spherical harmonic summation. - double sin_phi = sin(DEG2RAD(CoordSpherical->phig)); // sin (geocentric latitude) + The vector Magnetic field is given by -grad V, where V is Geomagnetic scalar potential + The gradient in spherical coordinates is given by: - if (nMax <= 16 || (1 - fabs(sin_phi)) < 1.0e-10) /* If nMax is less tha 16 or at the poles */ - PcupLow(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax); - else - { - if (PcupHigh(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0) - return -1; // error - } + dV ^ 1 dV ^ 1 dV ^ + grad V = -- r + - -- t + -------- -- p + dr r dt r sin(t) dp - return 0; // OK - } + INPUT : LegendreFunction + MagneticModel + SphVariables + CoordSpherical + OUTPUT : MagneticResults - void WorldMagModel::Summation( WMMtype_LegendreFunction *LegendreFunction, - WMMtype_SphericalHarmonicVariables *SphVariables, - WMMtype_CoordSpherical *CoordSpherical, - WMMtype_MagneticResults *MagneticResults) - { - /* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using spherical harmonic summation. + Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov + */ - The vector Magnetic field is given by -grad V, where V is Geomagnetic scalar potential - The gradient in spherical coordinates is given by: + MagneticResults->Bz = 0.0; + MagneticResults->By = 0.0; + MagneticResults->Bx = 0.0; - dV ^ 1 dV ^ 1 dV ^ - grad V = -- r + - -- t + -------- -- p - dr r dt r sin(t) dp + for (int n = 1; n <= MagneticModel.nMax; n++) { + for (int m = 0; m <= n; m++) { + int index = (n * (n + 1) / 2 + m); - INPUT : LegendreFunction - MagneticModel - SphVariables - CoordSpherical - OUTPUT : MagneticResults - - Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov - */ - - MagneticResults->Bz = 0.0; - MagneticResults->By = 0.0; - MagneticResults->Bx = 0.0; - - for (int n = 1; n <= MagneticModel.nMax; n++) - { - for (int m = 0; m <= n; m++) - { - int index = (n * (n + 1) / 2 + m); - -/* nMax (n+2) n m m m +/* nMax (n+2) n m m m Bz = -SUM (a/r) (n+1) SUM [g cos(m p) + h sin(m p)] P (sin(phi)) - n=1 m=0 n n n */ + n=1 m=0 n n n */ /* Equation 12 in the WMM Technical report. Derivative with respect to radius.*/ - MagneticResults->Bz -= - SphVariables->RelativeRadiusPower[n] * - (get_main_field_coeff_g(index) * - SphVariables->cos_mlambda[m] + get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) - * (double)(n + 1) * LegendreFunction->Pcup[index]; + MagneticResults->Bz -= + SphVariables->RelativeRadiusPower[n] * + (get_main_field_coeff_g(index) * + SphVariables->cos_mlambda[m] + get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) + * (double)(n + 1) * LegendreFunction->Pcup[index]; /* 1 nMax (n+2) n m m m By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (get_main_field_coeff_g(index) * - SphVariables->sin_mlambda[m] - get_main_field_coeff_h(index) * SphVariables->cos_mlambda[m]) - * (double)(m) * LegendreFunction->Pcup[index]; + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (get_main_field_coeff_g(index) * + SphVariables->sin_mlambda[m] - get_main_field_coeff_h(index) * SphVariables->cos_mlambda[m]) + * (double)(m) * LegendreFunction->Pcup[index]; /* nMax (n+2) n m m m Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */ - MagneticResults->Bx -= - SphVariables->RelativeRadiusPower[n] * - (get_main_field_coeff_g(index) * - SphVariables->cos_mlambda[m] + get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) - * LegendreFunction->dPcup[index]; - - } - } - - double cos_phi = cos(DEG2RAD(CoordSpherical->phig)); - if (fabs(cos_phi) > 1.0e-10) - { - MagneticResults->By = MagneticResults->By / cos_phi; - } - else - { - /* Special calculation for component - By - at Geographic poles. - * If the user wants to avoid using this function, please make sure that - * the latitude is not exactly +/-90. An option is to make use the function - * WMM_CheckGeographicPoles. - */ - SummationSpecial(SphVariables, CoordSpherical, MagneticResults); + MagneticResults->Bx -= + SphVariables->RelativeRadiusPower[n] * + (get_main_field_coeff_g(index) * + SphVariables->cos_mlambda[m] + get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) + * LegendreFunction->dPcup[index]; } } - void WorldMagModel::SecVarSummation( WMMtype_LegendreFunction *LegendreFunction, - WMMtype_SphericalHarmonicVariables *SphVariables, - WMMtype_CoordSpherical *CoordSpherical, - WMMtype_MagneticResults *MagneticResults) - { - /*This Function sums the secular variation coefficients to get the secular variation of the Magnetic vector. - INPUT : LegendreFunction - MagneticModel - SphVariables - CoordSpherical - OUTPUT : MagneticResults + double cos_phi = cos(DEG2RAD(CoordSpherical->phig)); + if (fabs(cos_phi) > 1.0e-10) { + MagneticResults->By = MagneticResults->By / cos_phi; + } else { + /* Special calculation for component - By - at Geographic poles. + * If the user wants to avoid using this function, please make sure that + * the latitude is not exactly +/-90. An option is to make use the function + * WMM_CheckGeographicPoles. */ + SummationSpecial(SphVariables, CoordSpherical, MagneticResults); + } +} - MagneticModel.SecularVariationUsed = true; +void WorldMagModel::SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables *SphVariables, + WMMtype_CoordSpherical *CoordSpherical, + WMMtype_MagneticResults *MagneticResults) +{ + /*This Function sums the secular variation coefficients to get the secular variation of the Magnetic vector. + INPUT : LegendreFunction + MagneticModel + SphVariables + CoordSpherical + OUTPUT : MagneticResults + */ - MagneticResults->Bz = 0.0; - MagneticResults->By = 0.0; - MagneticResults->Bx = 0.0; + MagneticModel.SecularVariationUsed = true; - for (int n = 1; n <= MagneticModel.nMaxSecVar; n++) - { - for (int m = 0; m <= n; m++) - { - int index = (n * (n + 1) / 2 + m); + MagneticResults->Bz = 0.0; + MagneticResults->By = 0.0; + MagneticResults->Bx = 0.0; -/* nMax (n+2) n m m m + for (int n = 1; n <= MagneticModel.nMaxSecVar; n++) { + for (int m = 0; m <= n; m++) { + int index = (n * (n + 1) / 2 + m); + +/* nMax (n+2) n m m m Bz = -SUM (a/r) (n+1) SUM [g cos(m p) + h sin(m p)] P (sin(phi)) - n=1 m=0 n n n */ + n=1 m=0 n n n */ /* Derivative with respect to radius.*/ - MagneticResults->Bz -= - SphVariables->RelativeRadiusPower[n] * - (get_secular_var_coeff_g(index) * - SphVariables->cos_mlambda[m] + get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) - * (double)(n + 1) * LegendreFunction->Pcup[index]; + MagneticResults->Bz -= + SphVariables->RelativeRadiusPower[n] * + (get_secular_var_coeff_g(index) * + SphVariables->cos_mlambda[m] + get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) + * (double)(n + 1) * LegendreFunction->Pcup[index]; /* 1 nMax (n+2) n m m m By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (get_secular_var_coeff_g(index) * - SphVariables->sin_mlambda[m] - get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[m]) - * (double)(m) * LegendreFunction->Pcup[index]; + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (get_secular_var_coeff_g(index) * + SphVariables->sin_mlambda[m] - get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[m]) + * (double)(m) * LegendreFunction->Pcup[index]; /* nMax (n+2) n m m m Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Derivative with respect to latitude, divided by radius. */ - MagneticResults->Bx -= - SphVariables->RelativeRadiusPower[n] * - (get_secular_var_coeff_g(index) * - SphVariables->cos_mlambda[m] + get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) - * LegendreFunction->dPcup[index]; - } - } - - double cos_phi = cos(DEG2RAD(CoordSpherical->phig)); - if (fabs(cos_phi) > 1.0e-10) - { - MagneticResults->By = MagneticResults->By / cos_phi; - } - else - { /* Special calculation for component By at Geographic poles */ - SecVarSummationSpecial(SphVariables, CoordSpherical, MagneticResults); + MagneticResults->Bx -= + SphVariables->RelativeRadiusPower[n] * + (get_secular_var_coeff_g(index) * + SphVariables->cos_mlambda[m] + get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) + * LegendreFunction->dPcup[index]; } } - void WorldMagModel::RotateMagneticVector( WMMtype_CoordSpherical *CoordSpherical, - WMMtype_CoordGeodetic *CoordGeodetic, - WMMtype_MagneticResults *MagneticResultsSph, - WMMtype_MagneticResults *MagneticResultsGeo) - { - /* Rotate the Magnetic Vectors to Geodetic Coordinates - Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov - Equation 16, WMM Technical report + double cos_phi = cos(DEG2RAD(CoordSpherical->phig)); + if (fabs(cos_phi) > 1.0e-10) { + MagneticResults->By = MagneticResults->By / cos_phi; + } else { /* Special calculation for component By at Geographic poles */ + SecVarSummationSpecial(SphVariables, CoordSpherical, MagneticResults); + } +} - INPUT : CoordSpherical : Data structure WMMtype_CoordSpherical with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) +void WorldMagModel::RotateMagneticVector(WMMtype_CoordSpherical *CoordSpherical, + WMMtype_CoordGeodetic *CoordGeodetic, + WMMtype_MagneticResults *MagneticResultsSph, + WMMtype_MagneticResults *MagneticResultsGeo) +{ + /* Rotate the Magnetic Vectors to Geodetic Coordinates + Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov + Equation 16, WMM Technical report - CoordGeodetic : Data structure WMMtype_CoordGeodetic with the following elements - float lambda; (longitude) - float phi; ( geodetic latitude) - float HeightAboveEllipsoid; (height above the ellipsoid (HaE) ) - float HeightAboveGeoid;(height above the Geoid ) + INPUT : CoordSpherical : Data structure WMMtype_CoordSpherical with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) - MagneticResultsSph : Data structure WMMtype_MagneticResults with the following elements - float Bx; North - float By; East - float Bz; Down + CoordGeodetic : Data structure WMMtype_CoordGeodetic with the following elements + float lambda; (longitude) + float phi; ( geodetic latitude) + float HeightAboveEllipsoid; (height above the ellipsoid (HaE) ) + float HeightAboveGeoid;(height above the Geoid ) - OUTPUT: MagneticResultsGeo Pointer to the data structure WMMtype_MagneticResults, with the following elements - float Bx; North - float By; East - float Bz; Down - */ + MagneticResultsSph : Data structure WMMtype_MagneticResults with the following elements + float Bx; North + float By; East + float Bz; Down - /* Difference between the spherical and Geodetic latitudes */ - double Psi = DEG2RAD(CoordSpherical->phig - CoordGeodetic->phi); + OUTPUT: MagneticResultsGeo Pointer to the data structure WMMtype_MagneticResults, with the following elements + float Bx; North + float By; East + float Bz; Down + */ - /* Rotate spherical field components to the Geodeitic system */ - MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sin(Psi) + MagneticResultsSph->Bz * cos(Psi); - MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cos(Psi) - MagneticResultsSph->Bz * sin(Psi); - MagneticResultsGeo->By = MagneticResultsSph->By; + /* Difference between the spherical and Geodetic latitudes */ + double Psi = DEG2RAD(CoordSpherical->phig - CoordGeodetic->phi); + + /* Rotate spherical field components to the Geodeitic system */ + MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sin(Psi) + MagneticResultsSph->Bz * cos(Psi); + MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cos(Psi) - MagneticResultsSph->Bz * sin(Psi); + MagneticResultsGeo->By = MagneticResultsSph->By; +} + +void WorldMagModel::CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements) +{ + /* Calculate all the Geomagnetic elements from X,Y and Z components + INPUT MagneticResultsGeo Pointer to data structure with the following elements + float Bx; ( North ) + float By; ( East ) + float Bz; ( Down ) + OUTPUT GeoMagneticElements Pointer to data structure with the following elements + float Decl; (Angle between the magnetic field vector and true north, positive east) + float Incl; Angle between the magnetic field vector and the horizontal plane, positive down + float F; Magnetic Field Strength + float H; Horizontal Magnetic Field Strength + float X; Northern component of the magnetic field vector + float Y; Eastern component of the magnetic field vector + float Z; Downward component of the magnetic field vector + */ + + GeoMagneticElements->X = MagneticResultsGeo->Bx; + GeoMagneticElements->Y = MagneticResultsGeo->By; + GeoMagneticElements->Z = MagneticResultsGeo->Bz; + + GeoMagneticElements->H = sqrt(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By); + GeoMagneticElements->F = sqrt(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz); + GeoMagneticElements->Decl = RAD2DEG(atan2(GeoMagneticElements->Y, GeoMagneticElements->X)); + GeoMagneticElements->Incl = RAD2DEG(atan2(GeoMagneticElements->Z, GeoMagneticElements->H)); +} + +void WorldMagModel::CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements) +{ + /* This takes the Magnetic Variation in x, y, and z and uses it to calculate the secular variation of each of the Geomagnetic elements. + INPUT MagneticVariation Data structure with the following elements + float Bx; ( North ) + float By; ( East ) + float Bz; ( Down ) + OUTPUT MagneticElements Pointer to the data structure with the following elements updated + float Decldot; Yearly Rate of change in declination + float Incldot; Yearly Rate of change in inclination + float Fdot; Yearly rate of change in Magnetic field strength + float Hdot; Yearly rate of change in horizontal field strength + float Xdot; Yearly rate of change in the northern component + float Ydot; Yearly rate of change in the eastern component + float Zdot; Yearly rate of change in the downward component + float GVdot;Yearly rate of chnage in grid variation + */ + + MagneticElements->Xdot = MagneticVariation->Bx; + MagneticElements->Ydot = MagneticVariation->By; + MagneticElements->Zdot = MagneticVariation->Bz; + MagneticElements->Hdot = (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot) / MagneticElements->H; // See equation 19 in the WMM technical report + MagneticElements->Fdot = + (MagneticElements->X * MagneticElements->Xdot + + MagneticElements->Y * MagneticElements->Ydot + MagneticElements->Z * MagneticElements->Zdot) / MagneticElements->F; + MagneticElements->Decldot = + 180.0 / M_PI * (MagneticElements->X * MagneticElements->Ydot - + MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H); + MagneticElements->Incldot = + 180.0 / M_PI * (MagneticElements->H * MagneticElements->Zdot - + MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F); + MagneticElements->GVdot = MagneticElements->Decldot; +} + +int WorldMagModel::PcupHigh(double *Pcup, double *dPcup, double x, int nMax) +{ + /* This function evaluates all of the Schmidt-semi normalized associated Legendre + functions up to degree nMax. The functions are initially scaled by + 10^280 sin^m in order to minimize the effects of underflow at large m + near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299). + Note that this function performs the same operation as WMM_PcupLow. + However this function also can be used for high degree (large nMax) models. + + Calling Parameters: + INPUT + nMax: Maximum spherical harmonic degree to compute. + x: cos(colatitude) or sin(latitude). + + OUTPUT + Pcup: A vector of all associated Legendgre polynomials evaluated at + x up to nMax. The lenght must by greater or equal to (nMax+1)*(nMax+2)/2. + dPcup: Derivative of Pcup(x) with respect to latitude + Notes: + + Adopted from the FORTRAN code written by Mark Wieczorek September 25, 2005. + + Manoj Nair, Nov, 2009 Manoj.C.Nair@Noaa.Gov + + Change from the previous version + The prevous version computes the derivatives as + dP(n,m)(x)/dx, where x = sin(latitude) (or cos(colatitude) ). + However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude. + Hence the derivatives are multiplied by sin(latitude). + Removed the options for CS phase and normalizations. + + Note: In geomagnetism, the derivatives of ALF are usually found with + respect to the colatitudes. Here the derivatives are found with respect + to the latitude. The difference is a sign reversal for the derivative of + the Associated Legendre Functions. + + The derivates can't be computed for latitude = |90| degrees. + */ + double f1[WMM_NUMPCUP]; + double f2[WMM_NUMPCUP]; + double PreSqr[WMM_NUMPCUP]; + int m; + + if (fabs(x) == 1.0) { + // printf("Error in PcupHigh: derivative cannot be calculated at poles\n"); + return -2; } - void WorldMagModel::CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements) - { - /* Calculate all the Geomagnetic elements from X,Y and Z components - INPUT MagneticResultsGeo Pointer to data structure with the following elements - float Bx; ( North ) - float By; ( East ) - float Bz; ( Down ) - OUTPUT GeoMagneticElements Pointer to data structure with the following elements - float Decl; (Angle between the magnetic field vector and true north, positive east) - float Incl; Angle between the magnetic field vector and the horizontal plane, positive down - float F; Magnetic Field Strength - float H; Horizontal Magnetic Field Strength - float X; Northern component of the magnetic field vector - float Y; Eastern component of the magnetic field vector - float Z; Downward component of the magnetic field vector - */ + double scalef = 1.0e-280; - GeoMagneticElements->X = MagneticResultsGeo->Bx; - GeoMagneticElements->Y = MagneticResultsGeo->By; - GeoMagneticElements->Z = MagneticResultsGeo->Bz; - - GeoMagneticElements->H = sqrt(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By); - GeoMagneticElements->F = sqrt(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz); - GeoMagneticElements->Decl = RAD2DEG(atan2(GeoMagneticElements->Y, GeoMagneticElements->X)); - GeoMagneticElements->Incl = RAD2DEG(atan2(GeoMagneticElements->Z, GeoMagneticElements->H)); + for (int n = 0; n <= 2 * nMax + 1; ++n) { + PreSqr[n] = sqrt((double)(n)); } - void WorldMagModel::CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements) - { - /* This takes the Magnetic Variation in x, y, and z and uses it to calculate the secular variation of each of the Geomagnetic elements. - INPUT MagneticVariation Data structure with the following elements - float Bx; ( North ) - float By; ( East ) - float Bz; ( Down ) - OUTPUT MagneticElements Pointer to the data structure with the following elements updated - float Decldot; Yearly Rate of change in declination - float Incldot; Yearly Rate of change in inclination - float Fdot; Yearly rate of change in Magnetic field strength - float Hdot; Yearly rate of change in horizontal field strength - float Xdot; Yearly rate of change in the northern component - float Ydot; Yearly rate of change in the eastern component - float Zdot; Yearly rate of change in the downward component - float GVdot;Yearly rate of chnage in grid variation - */ + int k = 2; - MagneticElements->Xdot = MagneticVariation->Bx; - MagneticElements->Ydot = MagneticVariation->By; - MagneticElements->Zdot = MagneticVariation->Bz; - MagneticElements->Hdot = (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot) / MagneticElements->H; //See equation 19 in the WMM technical report - MagneticElements->Fdot = - (MagneticElements->X * MagneticElements->Xdot + - MagneticElements->Y * MagneticElements->Ydot + MagneticElements->Z * MagneticElements->Zdot) / MagneticElements->F; - MagneticElements->Decldot = - 180.0 / M_PI * (MagneticElements->X * MagneticElements->Ydot - - MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H); - MagneticElements->Incldot = - 180.0 / M_PI * (MagneticElements->H * MagneticElements->Zdot - - MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F); - MagneticElements->GVdot = MagneticElements->Decldot; - } - - int WorldMagModel::PcupHigh(double *Pcup, double *dPcup, double x, int nMax) - { - /* This function evaluates all of the Schmidt-semi normalized associated Legendre - functions up to degree nMax. The functions are initially scaled by - 10^280 sin^m in order to minimize the effects of underflow at large m - near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299). - Note that this function performs the same operation as WMM_PcupLow. - However this function also can be used for high degree (large nMax) models. - - Calling Parameters: - INPUT - nMax: Maximum spherical harmonic degree to compute. - x: cos(colatitude) or sin(latitude). - - OUTPUT - Pcup: A vector of all associated Legendgre polynomials evaluated at - x up to nMax. The lenght must by greater or equal to (nMax+1)*(nMax+2)/2. - dPcup: Derivative of Pcup(x) with respect to latitude - Notes: - - Adopted from the FORTRAN code written by Mark Wieczorek September 25, 2005. - - Manoj Nair, Nov, 2009 Manoj.C.Nair@Noaa.Gov - - Change from the previous version - The prevous version computes the derivatives as - dP(n,m)(x)/dx, where x = sin(latitude) (or cos(colatitude) ). - However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude. - Hence the derivatives are multiplied by sin(latitude). - Removed the options for CS phase and normalizations. - - Note: In geomagnetism, the derivatives of ALF are usually found with - respect to the colatitudes. Here the derivatives are found with respect - to the latitude. The difference is a sign reversal for the derivative of - the Associated Legendre Functions. - - The derivates can't be computed for latitude = |90| degrees. - */ - double f1[WMM_NUMPCUP]; - double f2[WMM_NUMPCUP]; - double PreSqr[WMM_NUMPCUP]; - int m; - - if (fabs(x) == 1.0) - { - // printf("Error in PcupHigh: derivative cannot be calculated at poles\n"); - return -2; + for (int n = 2; n <= nMax; n++) { + k = k + 1; + f1[k] = (double)(2 * n - 1) / n; + f2[k] = (double)(n - 1) / n; + for (int m = 1; m <= n - 2; m++) { + k = k + 1; + f1[k] = (double)(2 * n - 1) / PreSqr[n + m] / PreSqr[n - m]; + f2[k] = PreSqr[n - m - 1] * PreSqr[n + m - 1] / PreSqr[n + m] / PreSqr[n - m]; } + k = k + 2; + } - double scalef = 1.0e-280; + /*z = sin (geocentric latitude) */ + double z = sqrt((1.0 - x) * (1.0 + x)); + double pm2 = 1.0; + Pcup[0] = 1.0; + dPcup[0] = 0.0; + if (nMax == 0) { + return -3; + } + double pm1 = x; + Pcup[1] = pm1; + dPcup[1] = z; + k = 1; - for (int n = 0; n <= 2 * nMax + 1; ++n) - PreSqr[n] = sqrt((double)(n)); + for (int n = 2; n <= nMax; n++) { + k = k + n; + double plm = f1[k] * x * pm1 - f2[k] * pm2; + Pcup[k] = plm; + dPcup[k] = (double)(n) * (pm1 - x * plm) / z; + pm2 = pm1; + pm1 = plm; + } - int k = 2; + double pmm = PreSqr[2] * scalef; + double rescalem = 1.0 / scalef; + int kstart = 0; - for (int n = 2; n <= nMax; n++) - { - k = k + 1; - f1[k] = (double)(2 * n - 1) / n; - f2[k] = (double)(n - 1) / n; - for (int m = 1; m <= n - 2; m++) - { - k = k + 1; - f1[k] = (double)(2 * n - 1) / PreSqr[n + m] / PreSqr[n - m]; - f2[k] = PreSqr[n - m - 1] * PreSqr[n + m - 1] / PreSqr[n + m] / PreSqr[n - m]; - } - k = k + 2; - } + for (m = 1; m <= nMax - 1; ++m) { + rescalem = rescalem * z; - /*z = sin (geocentric latitude) */ - double z = sqrt((1.0 - x) * (1.0 + x)); - double pm2 = 1.0; - Pcup[0] = 1.0; - dPcup[0] = 0.0; - if (nMax == 0) - return -3; - double pm1 = x; - Pcup[1] = pm1; - dPcup[1] = z; - k = 1; - - for (int n = 2; n <= nMax; n++) - { + /* Calculate Pcup(m,m) */ + kstart = kstart + m + 1; + pmm = pmm * PreSqr[2 * m + 1] / PreSqr[2 * m]; + Pcup[kstart] = pmm * rescalem / PreSqr[2 * m + 1]; + dPcup[kstart] = -((double)(m) * x * Pcup[kstart] / z); + pm2 = pmm / PreSqr[2 * m + 1]; + /* Calculate Pcup(m+1,m) */ + k = kstart + m + 1; + pm1 = x * PreSqr[2 * m + 1] * pm2; + Pcup[k] = pm1 * rescalem; + dPcup[k] = ((pm2 * rescalem) * PreSqr[2 * m + 1] - x * (double)(m + 1) * Pcup[k]) / z; + /* Calculate Pcup(n,m) */ + for (int n = m + 2; n <= nMax; ++n) { k = k + n; - double plm = f1[k] * x * pm1 - f2[k] * pm2; - Pcup[k] = plm; - dPcup[k] = (double)(n) * (pm1 - x * plm) / z; + double plm = x * f1[k] * pm1 - f2[k] * pm2; + Pcup[k] = plm * rescalem; + dPcup[k] = (PreSqr[n + m] * PreSqr[n - m] * (pm1 * rescalem) - (double)(n) * x * Pcup[k]) / z; pm2 = pm1; pm1 = plm; } - - double pmm = PreSqr[2] * scalef; - double rescalem = 1.0 / scalef; - int kstart = 0; - - for (m = 1; m <= nMax - 1; ++m) - { - rescalem = rescalem * z; - - /* Calculate Pcup(m,m) */ - kstart = kstart + m + 1; - pmm = pmm * PreSqr[2 * m + 1] / PreSqr[2 * m]; - Pcup[kstart] = pmm * rescalem / PreSqr[2 * m + 1]; - dPcup[kstart] = -((double)(m) * x * Pcup[kstart] / z); - pm2 = pmm / PreSqr[2 * m + 1]; - /* Calculate Pcup(m+1,m) */ - k = kstart + m + 1; - pm1 = x * PreSqr[2 * m + 1] * pm2; - Pcup[k] = pm1 * rescalem; - dPcup[k] = ((pm2 * rescalem) * PreSqr[2 * m + 1] - x * (double)(m + 1) * Pcup[k]) / z; - /* Calculate Pcup(n,m) */ - for (int n = m + 2; n <= nMax; ++n) - { - k = k + n; - double plm = x * f1[k] * pm1 - f2[k] * pm2; - Pcup[k] = plm * rescalem; - dPcup[k] = (PreSqr[n + m] * PreSqr[n - m] * (pm1 * rescalem) - (double)(n) * x * Pcup[k]) / z; - pm2 = pm1; - pm1 = plm; - } - } - - /* Calculate Pcup(nMax,nMax) */ - rescalem = rescalem * z; - kstart = kstart + m + 1; - pmm = pmm / PreSqr[2 * nMax]; - Pcup[kstart] = pmm * rescalem; - dPcup[kstart] = -(double)(nMax) * x * Pcup[kstart] / z; - - // ********* - - return 0; // OK } - void WorldMagModel::PcupLow(double *Pcup, double *dPcup, double x, int nMax) - { - /* This function evaluates all of the Schmidt-semi normalized associated Legendre functions up to degree nMax. + /* Calculate Pcup(nMax,nMax) */ + rescalem = rescalem * z; + kstart = kstart + m + 1; + pmm = pmm / PreSqr[2 * nMax]; + Pcup[kstart] = pmm * rescalem; + dPcup[kstart] = -(double)(nMax) * x * Pcup[kstart] / z; - Calling Parameters: - INPUT - nMax: Maximum spherical harmonic degree to compute. - x: cos(colatitude) or sin(latitude). + // ********* - OUTPUT - Pcup: A vector of all associated Legendgre polynomials evaluated at - x up to nMax. - dPcup: Derivative of Pcup(x) with respect to latitude + return 0; // OK +} - Notes: Overflow may occur if nMax > 20 , especially for high-latitudes. - Use WMM_PcupHigh for large nMax. +void WorldMagModel::PcupLow(double *Pcup, double *dPcup, double x, int nMax) +{ + /* This function evaluates all of the Schmidt-semi normalized associated Legendre functions up to degree nMax. - Writted by Manoj Nair, June, 2009 . Manoj.C.Nair@Noaa.Gov. + Calling Parameters: + INPUT + nMax: Maximum spherical harmonic degree to compute. + x: cos(colatitude) or sin(latitude). - Note: In geomagnetism, the derivatives of ALF are usually found with - respect to the colatitudes. Here the derivatives are found with respect - to the latitude. The difference is a sign reversal for the derivative of - the Associated Legendre Functions. - */ + OUTPUT + Pcup: A vector of all associated Legendgre polynomials evaluated at + x up to nMax. + dPcup: Derivative of Pcup(x) with respect to latitude - double schmidtQuasiNorm[WMM_NUMPCUP]; + Notes: Overflow may occur if nMax > 20 , especially for high-latitudes. + Use WMM_PcupHigh for large nMax. - Pcup[0] = 1.0; - dPcup[0] = 0.0; + Writted by Manoj Nair, June, 2009 . Manoj.C.Nair@Noaa.Gov. - /*sin (geocentric latitude) - sin_phi */ - double z = sqrt((1.0 - x) * (1.0 + x)); + Note: In geomagnetism, the derivatives of ALF are usually found with + respect to the colatitudes. Here the derivatives are found with respect + to the latitude. The difference is a sign reversal for the derivative of + the Associated Legendre Functions. + */ - /* First, Compute the Gauss-normalized associated Legendre functions */ - for (int n = 1; n <= nMax; n++) - { - for (int m = 0; m <= n; m++) - { - int index = (n * (n + 1) / 2 + m); - if (n == m) - { - int index1 = (n - 1) * n / 2 + m - 1; - Pcup[index] = z * Pcup[index1]; - dPcup[index] = z * dPcup[index1] + x * Pcup[index1]; - } - else - if (n == 1 && m == 0) - { - int index1 = (n - 1) * n / 2 + m; - Pcup[index] = x * Pcup[index1]; - dPcup[index] = x * dPcup[index1] - z * Pcup[index1]; - } - else - if (n > 1 && n != m) - { - int index1 = (n - 2) * (n - 1) / 2 + m; - int index2 = (n - 1) * n / 2 + m; - if (m > n - 2) - { - Pcup[index] = x * Pcup[index2]; - dPcup[index] = x * dPcup[index2] - z * Pcup[index2]; - } - else - { - double k = (double)(((n - 1) * (n - 1)) - (m * m)) / (double)((2 * n - 1) * (2 * n - 3)); - Pcup[index] = x * Pcup[index2] - k * Pcup[index1]; - dPcup[index] = x * dPcup[index2] - z * Pcup[index2] - k * dPcup[index1]; - } + double schmidtQuasiNorm[WMM_NUMPCUP]; + + Pcup[0] = 1.0; + dPcup[0] = 0.0; + + /*sin (geocentric latitude) - sin_phi */ + double z = sqrt((1.0 - x) * (1.0 + x)); + + /* First, Compute the Gauss-normalized associated Legendre functions */ + for (int n = 1; n <= nMax; n++) { + for (int m = 0; m <= n; m++) { + int index = (n * (n + 1) / 2 + m); + if (n == m) { + int index1 = (n - 1) * n / 2 + m - 1; + Pcup[index] = z * Pcup[index1]; + dPcup[index] = z * dPcup[index1] + x * Pcup[index1]; + } else if (n == 1 && m == 0) { + int index1 = (n - 1) * n / 2 + m; + Pcup[index] = x * Pcup[index1]; + dPcup[index] = x * dPcup[index1] - z * Pcup[index1]; + } else if (n > 1 && n != m) { + int index1 = (n - 2) * (n - 1) / 2 + m; + int index2 = (n - 1) * n / 2 + m; + if (m > n - 2) { + Pcup[index] = x * Pcup[index2]; + dPcup[index] = x * dPcup[index2] - z * Pcup[index2]; + } else { + double k = (double)(((n - 1) * (n - 1)) - (m * m)) / (double)((2 * n - 1) * (2 * n - 3)); + Pcup[index] = x * Pcup[index2] - k * Pcup[index1]; + dPcup[index] = x * dPcup[index2] - z * Pcup[index2] - k * dPcup[index1]; } } } + } + /*Compute the ration between the Gauss-normalized associated Legendre + functions and the Schmidt quasi-normalized version. This is equivalent to + sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ + + schmidtQuasiNorm[0] = 1.0; + for (int n = 1; n <= nMax; n++) { + int index = (n * (n + 1) / 2); + int index1 = (n - 1) * n / 2; + /* for m = 0 */ + schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (double)(2 * n - 1) / (double)n; + + for (int m = 1; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + index1 = (n * (n + 1) / 2 + m - 1); + schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * sqrt((double)((n - m + 1) * (m == 1 ? 2 : 1)) / (double)(n + m)); + } + } + + /* Converts the Gauss-normalized associated Legendre + functions to the Schmidt quasi-normalized version using pre-computed + relation stored in the variable schmidtQuasiNorm */ + + for (int n = 1; n <= nMax; n++) { + for (int m = 0; m <= n; m++) { + int index = (n * (n + 1) / 2 + m); + Pcup[index] = Pcup[index] * schmidtQuasiNorm[index]; + dPcup[index] = -dPcup[index] * schmidtQuasiNorm[index]; + /* The sign is changed since the new WMM routines use derivative with respect to latitude insted of co-latitude */ + } + } +} + +void WorldMagModel::SummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) +{ + /* Special calculation for the component By at Geographic poles. + Manoj Nair, June, 2009 manoj.c.nair@noaa.gov + INPUT: MagneticModel + SphVariables + CoordSpherical + OUTPUT: MagneticResults + CALLS : none + See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report + */ + + double PcupS[WMM_NUMPCUPS]; + + PcupS[0] = 1; + double schmidtQuasiNorm1 = 1.0; + + MagneticResults->By = 0.0; + double sin_phi = sin(DEG2RAD(CoordSpherical->phig)); + + for (int n = 1; n <= MagneticModel.nMax; n++) { /*Compute the ration between the Gauss-normalized associated Legendre - functions and the Schmidt quasi-normalized version. This is equivalent to - sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ + functions and the Schmidt quasi-normalized version. This is equivalent to + sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ - schmidtQuasiNorm[0] = 1.0; - for (int n = 1; n <= nMax; n++) - { - int index = (n * (n + 1) / 2); - int index1 = (n - 1) * n / 2; - /* for m = 0 */ - schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (double)(2 * n - 1) / (double)n; - - for (int m = 1; m <= n; m++) - { - index = (n * (n + 1) / 2 + m); - index1 = (n * (n + 1) / 2 + m - 1); - schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * sqrt((double)((n - m + 1) * (m == 1 ? 2 : 1)) / (double)(n + m)); - } + int index = (n * (n + 1) / 2 + 1); + double schmidtQuasiNorm2 = schmidtQuasiNorm1 * (double)(2 * n - 1) / (double)n; + double schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((double)(n * 2) / (double)(n + 1)); + schmidtQuasiNorm1 = schmidtQuasiNorm2; + if (n == 1) { + PcupS[n] = PcupS[n - 1]; + } else { + double k = (double)(((n - 1) * (n - 1)) - 1) / (double)((2 * n - 1) * (2 * n - 3)); + PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; } - /* Converts the Gauss-normalized associated Legendre - functions to the Schmidt quasi-normalized version using pre-computed - relation stored in the variable schmidtQuasiNorm */ - - for (int n = 1; n <= nMax; n++) - { - for (int m = 0; m <= n; m++) - { - int index = (n * (n + 1) / 2 + m); - Pcup[index] = Pcup[index] * schmidtQuasiNorm[index]; - dPcup[index] = -dPcup[index] * schmidtQuasiNorm[index]; - /* The sign is changed since the new WMM routines use derivative with respect to latitude insted of co-latitude */ - } - } - } - - void WorldMagModel::SummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) - { - /* Special calculation for the component By at Geographic poles. - Manoj Nair, June, 2009 manoj.c.nair@noaa.gov - INPUT: MagneticModel - SphVariables - CoordSpherical - OUTPUT: MagneticResults - CALLS : none - See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report - */ - - double PcupS[WMM_NUMPCUPS]; - - PcupS[0] = 1; - double schmidtQuasiNorm1 = 1.0; - - MagneticResults->By = 0.0; - double sin_phi = sin(DEG2RAD(CoordSpherical->phig)); - - for (int n = 1; n <= MagneticModel.nMax; n++) - { - /*Compute the ration between the Gauss-normalized associated Legendre - functions and the Schmidt quasi-normalized version. This is equivalent to - sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ - - int index = (n * (n + 1) / 2 + 1); - double schmidtQuasiNorm2 = schmidtQuasiNorm1 * (double)(2 * n - 1) / (double)n; - double schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((double)(n * 2) / (double)(n + 1)); - schmidtQuasiNorm1 = schmidtQuasiNorm2; - if (n == 1) - { - PcupS[n] = PcupS[n - 1]; - } - else - { - double k = (double)(((n - 1) * (n - 1)) - 1) / (double)((2 * n - 1) * (2 * n - 3)); - PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; - } - /* 1 nMax (n+2) n m m m By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (get_main_field_coeff_g(index) * - SphVariables->sin_mlambda[1] - get_main_field_coeff_h(index) * SphVariables->cos_mlambda[1]) - * PcupS[n] * schmidtQuasiNorm3; - } + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (get_main_field_coeff_g(index) * + SphVariables->sin_mlambda[1] - get_main_field_coeff_h(index) * SphVariables->cos_mlambda[1]) + * PcupS[n] * schmidtQuasiNorm3; } +} - void WorldMagModel::SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) - { - /*Special calculation for the secular variation summation at the poles. +void WorldMagModel::SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) +{ + /*Special calculation for the secular variation summation at the poles. - INPUT: MagneticModel - SphVariables - CoordSpherical - OUTPUT: MagneticResults - */ + INPUT: MagneticModel + SphVariables + CoordSpherical + OUTPUT: MagneticResults + */ - double PcupS[WMM_NUMPCUPS]; + double PcupS[WMM_NUMPCUPS]; - PcupS[0] = 1; - double schmidtQuasiNorm1 = 1.0; + PcupS[0] = 1; + double schmidtQuasiNorm1 = 1.0; - MagneticResults->By = 0.0; - double sin_phi = sin(DEG2RAD(CoordSpherical->phig)); + MagneticResults->By = 0.0; + double sin_phi = sin(DEG2RAD(CoordSpherical->phig)); - for (int n = 1; n <= MagneticModel.nMaxSecVar; n++) - { - int index = (n * (n + 1) / 2 + 1); - double schmidtQuasiNorm2 = schmidtQuasiNorm1 * (double)(2 * n - 1) / (double)n; - double schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((double)(n * 2) / (double)(n + 1)); - schmidtQuasiNorm1 = schmidtQuasiNorm2; - if (n == 1) - { - PcupS[n] = PcupS[n - 1]; - } - else - { - double k = (double)(((n - 1) * (n - 1)) - 1) / (double)((2 * n - 1) * (2 * n - 3)); - PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; - } + for (int n = 1; n <= MagneticModel.nMaxSecVar; n++) { + int index = (n * (n + 1) / 2 + 1); + double schmidtQuasiNorm2 = schmidtQuasiNorm1 * (double)(2 * n - 1) / (double)n; + double schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((double)(n * 2) / (double)(n + 1)); + schmidtQuasiNorm1 = schmidtQuasiNorm2; + if (n == 1) { + PcupS[n] = PcupS[n - 1]; + } else { + double k = (double)(((n - 1) * (n - 1)) - 1) / (double)((2 * n - 1) * (2 * n - 3)); + PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; + } /* 1 nMax (n+2) n m m m By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (get_secular_var_coeff_g(index) * - SphVariables->sin_mlambda[1] - get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[1]) - * PcupS[n] * schmidtQuasiNorm3; - } + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (get_secular_var_coeff_g(index) * + SphVariables->sin_mlambda[1] - get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[1]) + * PcupS[n] * schmidtQuasiNorm3; } - - // brief Comput the MainFieldCoeffH accounting for the date - double WorldMagModel::get_main_field_coeff_g(int index) - { - if (index >= WMM_NUMTERMS) - return 0; - - double coeff = CoeffFile[index][2]; - - int a = MagneticModel.nMaxSecVar; - int b = (a * (a + 1) / 2 + a); - for (int n = 1; n <= MagneticModel.nMax; n++) - { - for (int m = 0; m <= n; m++) - { - int sum_index = (n * (n + 1) / 2 + m); - - /* Hacky for now, will solve for which conditions need summing analytically */ - if (sum_index != index) - continue; - - if (index <= b) - coeff += (decimal_date - MagneticModel.epoch) * get_secular_var_coeff_g(sum_index); - } - } - - return coeff; - } - - double WorldMagModel::get_main_field_coeff_h(int index) - { - if (index >= WMM_NUMTERMS) - return 0; - - double coeff = CoeffFile[index][3]; - - int a = MagneticModel.nMaxSecVar; - int b = (a * (a + 1) / 2 + a); - for (int n = 1; n <= MagneticModel.nMax; n++) - { - for (int m = 0; m <= n; m++) - { - int sum_index = (n * (n + 1) / 2 + m); - - /* Hacky for now, will solve for which conditions need summing analytically */ - if (sum_index != index) - continue; - - if (index <= b) - coeff += (decimal_date - MagneticModel.epoch) * get_secular_var_coeff_h(sum_index); - } - } - - return coeff; - } - - double WorldMagModel::get_secular_var_coeff_g(int index) - { - if (index >= WMM_NUMTERMS) - return 0; - - return CoeffFile[index][4]; - } - - double WorldMagModel::get_secular_var_coeff_h(int index) - { - if (index >= WMM_NUMTERMS) - return 0; - - return CoeffFile[index][5]; - } - - int WorldMagModel::DateToYear(int month, int day, int year) - { - // Converts a given calendar date into a decimal year - - int temp = 0; // Total number of days - int MonthDays[13] = { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; - int ExtraDay = 0; - - if ((year % 4 == 0 && year % 100 != 0) || (year % 400 == 0)) - ExtraDay = 1; - MonthDays[2] += ExtraDay; - - /******************Validation********************************/ - - if (month <= 0 || month > 12) - return -1; // error - - if (day <= 0 || day > MonthDays[month]) - return -2; // error - - /****************Calculation of t***************************/ - for (int i = 1; i <= month; i++) - temp += MonthDays[i - 1]; - temp += day; - - decimal_date = year + (temp - 1) / (365.0 + ExtraDay); - - return 0; // OK - } - - void WorldMagModel::GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical) - { - // Converts Geodetic coordinates to Spherical coordinates - // Convert geodetic coordinates, (defined by the WGS-84 - // reference ellipsoid), to Earth Centered Earth Fixed Cartesian - // coordinates, and then to spherical coordinates. - - double CosLat = cos(DEG2RAD(CoordGeodetic->phi)); - double SinLat = sin(DEG2RAD(CoordGeodetic->phi)); - - // compute the local radius of curvature on the WGS-84 reference ellipsoid - double rc = Ellip.a / sqrt(1.0 - Ellip.epssq * SinLat * SinLat); - - // compute ECEF Cartesian coordinates of specified point (for longitude=0) - double xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat; - double zp = (rc * (1.0 - Ellip.epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat; - - // compute spherical radius and angle lambda and phi of specified point - CoordSpherical->r = sqrt(xp * xp + zp * zp); - CoordSpherical->phig = RAD2DEG(asin(zp / CoordSpherical->r)); // geocentric latitude - CoordSpherical->lambda = CoordGeodetic->lambda; // longitude - } - +} + +// brief Comput the MainFieldCoeffH accounting for the date +double WorldMagModel::get_main_field_coeff_g(int index) +{ + if (index >= WMM_NUMTERMS) { + return 0; + } + + double coeff = CoeffFile[index][2]; + + int a = MagneticModel.nMaxSecVar; + int b = (a * (a + 1) / 2 + a); + for (int n = 1; n <= MagneticModel.nMax; n++) { + for (int m = 0; m <= n; m++) { + int sum_index = (n * (n + 1) / 2 + m); + + /* Hacky for now, will solve for which conditions need summing analytically */ + if (sum_index != index) { + continue; + } + + if (index <= b) { + coeff += (decimal_date - MagneticModel.epoch) * get_secular_var_coeff_g(sum_index); + } + } + } + + return coeff; +} + +double WorldMagModel::get_main_field_coeff_h(int index) +{ + if (index >= WMM_NUMTERMS) { + return 0; + } + + double coeff = CoeffFile[index][3]; + + int a = MagneticModel.nMaxSecVar; + int b = (a * (a + 1) / 2 + a); + for (int n = 1; n <= MagneticModel.nMax; n++) { + for (int m = 0; m <= n; m++) { + int sum_index = (n * (n + 1) / 2 + m); + + /* Hacky for now, will solve for which conditions need summing analytically */ + if (sum_index != index) { + continue; + } + + if (index <= b) { + coeff += (decimal_date - MagneticModel.epoch) * get_secular_var_coeff_h(sum_index); + } + } + } + + return coeff; +} + +double WorldMagModel::get_secular_var_coeff_g(int index) +{ + if (index >= WMM_NUMTERMS) { + return 0; + } + + return CoeffFile[index][4]; +} + +double WorldMagModel::get_secular_var_coeff_h(int index) +{ + if (index >= WMM_NUMTERMS) { + return 0; + } + + return CoeffFile[index][5]; +} + +int WorldMagModel::DateToYear(int month, int day, int year) +{ + // Converts a given calendar date into a decimal year + + int temp = 0; // Total number of days + int MonthDays[13] = { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; + int ExtraDay = 0; + + if ((year % 4 == 0 && year % 100 != 0) || (year % 400 == 0)) { + ExtraDay = 1; + } + MonthDays[2] += ExtraDay; + + /******************Validation********************************/ + + if (month <= 0 || month > 12) { + return -1; // error + } + if (day <= 0 || day > MonthDays[month]) { + return -2; // error + } + /****************Calculation of t***************************/ + for (int i = 1; i <= month; i++) { + temp += MonthDays[i - 1]; + } + temp += day; + + decimal_date = year + (temp - 1) / (365.0 + ExtraDay); + + return 0; // OK +} + +void WorldMagModel::GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical) +{ + // Converts Geodetic coordinates to Spherical coordinates + // Convert geodetic coordinates, (defined by the WGS-84 + // reference ellipsoid), to Earth Centered Earth Fixed Cartesian + // coordinates, and then to spherical coordinates. + + double CosLat = cos(DEG2RAD(CoordGeodetic->phi)); + double SinLat = sin(DEG2RAD(CoordGeodetic->phi)); + + // compute the local radius of curvature on the WGS-84 reference ellipsoid + double rc = Ellip.a / sqrt(1.0 - Ellip.epssq * SinLat * SinLat); + + // compute ECEF Cartesian coordinates of specified point (for longitude=0) + double xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat; + double zp = (rc * (1.0 - Ellip.epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat; + + // compute spherical radius and angle lambda and phi of specified point + CoordSpherical->r = sqrt(xp * xp + zp * zp); + CoordSpherical->phig = RAD2DEG(asin(zp / CoordSpherical->r)); // geocentric latitude + CoordSpherical->lambda = CoordGeodetic->lambda; // longitude +} } diff --git a/ground/openpilotgcs/src/libs/utils/worldmagmodel.h b/ground/openpilotgcs/src/libs/utils/worldmagmodel.h index 2d9ca2840..447dc001d 100644 --- a/ground/openpilotgcs/src/libs/utils/worldmagmodel.h +++ b/ground/openpilotgcs/src/libs/utils/worldmagmodel.h @@ -34,147 +34,135 @@ // ****************************** // internal structure definitions -#define WMM_MAX_MODEL_DEGREES 12 -#define WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES 12 -#define WMM_NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES + 1) * (WMM_MAX_MODEL_DEGREES + 2) / 2); -#define WMM_NUMPCUP 92 // NUMTERMS + 1 -#define WMM_NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES + 1 +#define WMM_MAX_MODEL_DEGREES 12 +#define WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES 12 +#define WMM_NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES + 1) * (WMM_MAX_MODEL_DEGREES + 2) / 2); +#define WMM_NUMPCUP 92 // NUMTERMS + 1 +#define WMM_NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES + 1 -typedef struct -{ +typedef struct { double EditionDate; - double epoch; //Base time of Geomagnetic model epoch (yrs) - char ModelName[20]; -// double Main_Field_Coeff_G[WMM_NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) -// double Main_Field_Coeff_H[WMM_NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) -// double Secular_Var_Coeff_G[WMM_NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) -// double Secular_Var_Coeff_H[WMM_NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) - int nMax; // Maximum degree of spherical harmonic model - int nMaxSecVar; // Maxumum degree of spherical harmonic secular model - int SecularVariationUsed; // Whether or not the magnetic secular variation vector will be needed by program + double epoch; // Base time of Geomagnetic model epoch (yrs) + char ModelName[20]; +// double Main_Field_Coeff_G[WMM_NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) +// double Main_Field_Coeff_H[WMM_NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) +// double Secular_Var_Coeff_G[WMM_NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) +// double Secular_Var_Coeff_H[WMM_NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) + int nMax; // Maximum degree of spherical harmonic model + int nMaxSecVar; // Maxumum degree of spherical harmonic secular model + int SecularVariationUsed; // Whether or not the magnetic secular variation vector will be needed by program } WMMtype_MagneticModel; -typedef struct -{ - double a; // semi-major axis of the ellipsoid - double b; // semi-minor axis of the ellipsoid - double fla; // flattening - double epssq; // first eccentricity squared - double eps; // first eccentricity - double re; // mean radius of ellipsoid +typedef struct { + double a; // semi-major axis of the ellipsoid + double b; // semi-minor axis of the ellipsoid + double fla; // flattening + double epssq; // first eccentricity squared + double eps; // first eccentricity + double re; // mean radius of ellipsoid } WMMtype_Ellipsoid; -typedef struct -{ - double lambda; // longitude - double phi; // geodetic latitude - double HeightAboveEllipsoid; // height above the ellipsoid (HaE) +typedef struct { + double lambda; // longitude + double phi; // geodetic latitude + double HeightAboveEllipsoid; // height above the ellipsoid (HaE) } WMMtype_CoordGeodetic; -typedef struct -{ - double lambda; // longitude - double phig; // geocentric latitude - double r; // distance from the center of the ellipsoid +typedef struct { + double lambda; // longitude + double phig; // geocentric latitude + double r; // distance from the center of the ellipsoid } WMMtype_CoordSpherical; -typedef struct -{ - int Year; - int Month; - int Day; +typedef struct { + int Year; + int Month; + int Day; double DecimalYear; } WMMtype_Date; -typedef struct -{ - double Pcup[WMM_NUMPCUP]; // Legendre Function - double dPcup[WMM_NUMPCUP]; // Derivative of Lagendre fn +typedef struct { + double Pcup[WMM_NUMPCUP]; // Legendre Function + double dPcup[WMM_NUMPCUP]; // Derivative of Lagendre fn } WMMtype_LegendreFunction; -typedef struct -{ - double Bx; // North - double By; // East - double Bz; // Down +typedef struct { + double Bx; // North + double By; // East + double Bz; // Down } WMMtype_MagneticResults; -typedef struct -{ - double RelativeRadiusPower[WMM_MAX_MODEL_DEGREES + 1]; // [earth_reference_radius_km / sph. radius ]^n - double cos_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // cp(m) - cosine of (m*spherical coord. longitude - double sin_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // sp(m) - sine of (m*spherical coord. longitude) +typedef struct { + double RelativeRadiusPower[WMM_MAX_MODEL_DEGREES + 1]; // [earth_reference_radius_km / sph. radius ]^n + double cos_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // cp(m) - cosine of (m*spherical coord. longitude + double sin_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // sp(m) - sine of (m*spherical coord. longitude) } WMMtype_SphericalHarmonicVariables; -typedef struct -{ - double Decl; /*1. Angle between the magnetic field vector and true north, positive east */ - double Incl; /*2. Angle between the magnetic field vector and the horizontal plane, positive down */ - double F; /*3. Magnetic Field Strength */ - double H; /*4. Horizontal Magnetic Field Strength */ - double X; /*5. Northern component of the magnetic field vector */ - double Y; /*6. Eastern component of the magnetic field vector */ - double Z; /*7. Downward component of the magnetic field vector */ - double GV; /*8. The Grid Variation */ - double Decldot; /*9. Yearly Rate of change in declination */ - double Incldot; /*10. Yearly Rate of change in inclination */ - double Fdot; /*11. Yearly rate of change in Magnetic field strength */ - double Hdot; /*12. Yearly rate of change in horizontal field strength */ - double Xdot; /*13. Yearly rate of change in the northern component */ - double Ydot; /*14. Yearly rate of change in the eastern component */ - double Zdot; /*15. Yearly rate of change in the downward component */ - double GVdot; /*16. Yearly rate of chnage in grid variation */ +typedef struct { + double Decl; /*1. Angle between the magnetic field vector and true north, positive east */ + double Incl; /*2. Angle between the magnetic field vector and the horizontal plane, positive down */ + double F; /*3. Magnetic Field Strength */ + double H; /*4. Horizontal Magnetic Field Strength */ + double X; /*5. Northern component of the magnetic field vector */ + double Y; /*6. Eastern component of the magnetic field vector */ + double Z; /*7. Downward component of the magnetic field vector */ + double GV; /*8. The Grid Variation */ + double Decldot; /*9. Yearly Rate of change in declination */ + double Incldot; /*10. Yearly Rate of change in inclination */ + double Fdot; /*11. Yearly rate of change in Magnetic field strength */ + double Hdot; /*12. Yearly rate of change in horizontal field strength */ + double Xdot; /*13. Yearly rate of change in the northern component */ + double Ydot; /*14. Yearly rate of change in the eastern component */ + double Zdot; /*15. Yearly rate of change in the downward component */ + double GVdot; /*16. Yearly rate of chnage in grid variation */ } WMMtype_GeoMagneticElements; // ****************************** namespace Utils { +class QTCREATOR_UTILS_EXPORT WorldMagModel { +public: + WorldMagModel(); - class QTCREATOR_UTILS_EXPORT WorldMagModel - { - public: - WorldMagModel(); + int GetMagVector(double LLA[3], int Month, int Day, int Year, double Be[3]); - int GetMagVector(double LLA[3], int Month, int Day, int Year, double Be[3]); +private: + WMMtype_Ellipsoid Ellip; + WMMtype_MagneticModel MagneticModel; - private: - WMMtype_Ellipsoid Ellip; - WMMtype_MagneticModel MagneticModel; - - double decimal_date; - - void Initialize(); - int Geomag(WMMtype_CoordSpherical *CoordSpherical, WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_GeoMagneticElements *GeoMagneticElements); - void ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_SphericalHarmonicVariables *SphVariables); - int AssociatedLegendreFunction(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_LegendreFunction *LegendreFunction); - void Summation( WMMtype_LegendreFunction *LegendreFunction, - WMMtype_SphericalHarmonicVariables *SphVariables, - WMMtype_CoordSpherical *CoordSpherical, - WMMtype_MagneticResults *MagneticResults); - void SecVarSummation( WMMtype_LegendreFunction *LegendreFunction, - WMMtype_SphericalHarmonicVariables *SphVariables, - WMMtype_CoordSpherical *CoordSpherical, - WMMtype_MagneticResults *MagneticResults); - void RotateMagneticVector( WMMtype_CoordSpherical *CoordSpherical, - WMMtype_CoordGeodetic *CoordGeodetic, - WMMtype_MagneticResults *MagneticResultsSph, - WMMtype_MagneticResults *MagneticResultsGeo); - void CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements); - void CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements); - int PcupHigh(double *Pcup, double *dPcup, double x, int nMax); - void PcupLow(double *Pcup, double *dPcup, double x, int nMax); - void SummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); - void SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); - double get_main_field_coeff_g(int index); - double get_main_field_coeff_h(int index); - double get_secular_var_coeff_g(int index); - double get_secular_var_coeff_h(int index); - int DateToYear(int month, int day, int year); - void GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical); - }; + double decimal_date; + void Initialize(); + int Geomag(WMMtype_CoordSpherical *CoordSpherical, WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_GeoMagneticElements *GeoMagneticElements); + void ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_SphericalHarmonicVariables *SphVariables); + int AssociatedLegendreFunction(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_LegendreFunction *LegendreFunction); + void Summation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables *SphVariables, + WMMtype_CoordSpherical *CoordSpherical, + WMMtype_MagneticResults *MagneticResults); + void SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables *SphVariables, + WMMtype_CoordSpherical *CoordSpherical, + WMMtype_MagneticResults *MagneticResults); + void RotateMagneticVector(WMMtype_CoordSpherical *CoordSpherical, + WMMtype_CoordGeodetic *CoordGeodetic, + WMMtype_MagneticResults *MagneticResultsSph, + WMMtype_MagneticResults *MagneticResultsGeo); + void CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements); + void CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements); + int PcupHigh(double *Pcup, double *dPcup, double x, int nMax); + void PcupLow(double *Pcup, double *dPcup, double x, int nMax); + void SummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); + void SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); + double get_main_field_coeff_g(int index); + double get_main_field_coeff_h(int index); + double get_secular_var_coeff_g(int index); + double get_secular_var_coeff_h(int index); + int DateToYear(int month, int day, int year); + void GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical); +}; } // ****************************** -#endif +#endif // ifndef WORLDMAGMODEL_H diff --git a/ground/openpilotgcs/src/libs/utils/xmlconfig.cpp b/ground/openpilotgcs/src/libs/utils/xmlconfig.cpp index 7b4cc9d5a..3170cd6ca 100644 --- a/ground/openpilotgcs/src/libs/utils/xmlconfig.cpp +++ b/ground/openpilotgcs/src/libs/utils/xmlconfig.cpp @@ -45,7 +45,7 @@ QString XmlConfig::rootName = "gcs"; const QSettings::Format XmlConfig::XmlSettingsFormat = - QSettings::registerFormat("xml", XmlConfig::readXmlFile, XmlConfig::writeXmlFile); + QSettings::registerFormat("xml", XmlConfig::readXmlFile, XmlConfig::writeXmlFile); bool XmlConfig::readXmlFile(QIODevice &device, QSettings::SettingsMap &map) @@ -57,7 +57,7 @@ bool XmlConfig::readXmlFile(QIODevice &device, QSettings::SettingsMap &map) int errorColumn; if (!domDoc.setContent(&device, true, &errorStr, &errorLine, - &errorColumn)) { + &errorColumn)) { QString err = QString(tr("GCS config")) + tr("Parse error at line %1, column %2:\n%3") .arg(errorLine) @@ -72,17 +72,17 @@ bool XmlConfig::readXmlFile(QIODevice &device, QSettings::SettingsMap &map) return true; } -void XmlConfig::handleNode(QDomElement* node, QSettings::SettingsMap &map, QString path) +void XmlConfig::handleNode(QDomElement *node, QSettings::SettingsMap &map, QString path) { - if ( !node ){ + if (!node) { return; } - // qDebug() << "XmlConfig::handleNode start"; + // qDebug() << "XmlConfig::handleNode start"; QString nodeName = node->nodeName(); // For arrays, QT will use simple numbers as keys, which is not a valid element in XML. // Therefore we prefixed these. - if ( nodeName.startsWith(NUM_PREFIX) ){ + if (nodeName.startsWith(NUM_PREFIX)) { nodeName.replace(NUM_PREFIX, ""); } // Xml tags are restrictive with allowed characters, @@ -90,42 +90,42 @@ void XmlConfig::handleNode(QDomElement* node, QSettings::SettingsMap &map, QStri nodeName = nodeName.replace("__PCT__", "%"); nodeName = QUrl::fromPercentEncoding(nodeName.toAscii()); - if ( nodeName == XmlConfig::rootName ) + if (nodeName == XmlConfig::rootName) { ; - else if ( path == "" ) + } else if (path == "") { path = nodeName; - else + } else { path += "/" + nodeName; + } -// qDebug() << "Node: " << ": " << path << " Children: " << node->childNodes().length(); - for ( uint i = 0; i < node->childNodes().length(); ++i ){ +// qDebug() << "Node: " << ": " << path << " Children: " << node->childNodes().length(); + for (uint i = 0; i < node->childNodes().length(); ++i) { QDomNode child = node->childNodes().item(i); - if ( child.isElement() ){ - handleNode( static_cast(&child), map, path); - } - else if ( child.isText() ){ -// qDebug() << "Key: " << path << " Value:" << node->text(); + if (child.isElement()) { + handleNode(static_cast(&child), map, path); + } else if (child.isText()) { +// qDebug() << "Key: " << path << " Value:" << node->text(); map.insert(path, stringToVariant(node->text())); - } - else{ + } else { qDebug() << "Child not Element or text!" << child.nodeType(); } } -// qDebug() << "XmlConfig::handleNode end"; +// qDebug() << "XmlConfig::handleNode end"; } bool XmlConfig::writeXmlFile(QIODevice &device, const QSettings::SettingsMap &map) { QDomDocument outDocument; -// qDebug() << "writeXmlFile start"; - outDocument.appendChild( outDocument.createElement(XmlConfig::rootName)); + +// qDebug() << "writeXmlFile start"; + outDocument.appendChild(outDocument.createElement(XmlConfig::rootName)); QMapIterator iter(map); while (iter.hasNext()) { iter.next(); -// qDebug() << "Entry: " << iter.key() << ": " << iter.value().toString() << endl; +// qDebug() << "Entry: " << iter.key() << ": " << iter.value().toString() << endl; QDomNode node = outDocument.firstChild(); - foreach ( QString elem, iter.key().split('/')){ - if ( elem == "" ){ + foreach(QString elem, iter.key().split('/')) { + if (elem == "") { continue; } // Xml tags are restrictive with allowed characters, @@ -134,14 +134,14 @@ bool XmlConfig::writeXmlFile(QIODevice &device, const QSettings::SettingsMap &ma elem = elem.replace("%", "__PCT__"); // For arrays, QT will use simple numbers as keys, which is not a valid element in XML. // Therefore we prefixed these. - if ( elem.startsWith(NUM_PREFIX) ){ + if (elem.startsWith(NUM_PREFIX)) { qWarning() << "ERROR: Settings must not start with " << NUM_PREFIX - << " in: " + iter.key(); + << " in: " + iter.key(); } - if ( QRegExp("[0-9]").exactMatch(elem.left(1)) ){ + if (QRegExp("[0-9]").exactMatch(elem.left(1))) { elem.prepend(NUM_PREFIX); } - if ( node.firstChildElement(elem).isNull() ){ + if (node.firstChildElement(elem).isNull()) { node.appendChild(outDocument.createElement(elem)); } node = node.firstChildElement(elem); @@ -149,18 +149,20 @@ bool XmlConfig::writeXmlFile(QIODevice &device, const QSettings::SettingsMap &ma node.appendChild(outDocument.createTextNode(variantToString(iter.value()))); } device.write(outDocument.toByteArray(2).constData()); -// qDebug() << "Dokument:\n" << outDocument.toByteArray(2).constData(); -// qDebug() << "writeXmlFile end"; +// qDebug() << "Dokument:\n" << outDocument.toByteArray(2).constData(); +// qDebug() << "writeXmlFile end"; return true; } -QSettings::SettingsMap XmlConfig::settingsToMap(QSettings& qs){ +QSettings::SettingsMap XmlConfig::settingsToMap(QSettings & qs) +{ qDebug() << "settingsToMap:---------------"; QSettings::SettingsMap map; QStringList keys = qs.allKeys(); - foreach (QString key, keys) { + foreach(QString key, keys) { QVariant val = qs.value(key); + qDebug() << key << val.toString(); map.insert(key, val); } @@ -173,34 +175,38 @@ QString XmlConfig::variantToString(const QVariant &v) QString result; switch (v.type()) { - case QVariant::Invalid: - result = QLatin1String("@Invalid()"); - break; + case QVariant::Invalid: + result = QLatin1String("@Invalid()"); + break; - case QVariant::ByteArray: { - QByteArray a = v.toByteArray().toBase64(); - result = QLatin1String("@ByteArray("); - result += QString::fromLatin1(a.constData(), a.size()); - result += QLatin1Char(')'); - break; - } + case QVariant::ByteArray: + { + QByteArray a = v.toByteArray().toBase64(); + result = QLatin1String("@ByteArray("); + result += QString::fromLatin1(a.constData(), a.size()); + result += QLatin1Char(')'); + break; + } - case QVariant::String: - case QVariant::LongLong: - case QVariant::ULongLong: - case QVariant::Int: - case QVariant::UInt: - case QVariant::Bool: - case QVariant::Double: - case QVariant::KeySequence: - case QVariant::Color: { - result = v.toString(); - if (result.startsWith(QLatin1Char('@'))) - result.prepend(QLatin1Char('@')); - break; + case QVariant::String: + case QVariant::LongLong: + case QVariant::ULongLong: + case QVariant::Int: + case QVariant::UInt: + case QVariant::Bool: + case QVariant::Double: + case QVariant::KeySequence: + case QVariant::Color: + { + result = v.toString(); + if (result.startsWith(QLatin1Char('@'))) { + result.prepend(QLatin1Char('@')); } + break; + } #ifndef QT_NO_GEOM_VARIANT - case QVariant::Rect: { + case QVariant::Rect: + { QRect r = qvariant_cast(v); result += QLatin1String("@Rect("); result += QString::number(r.x()); @@ -213,7 +219,8 @@ QString XmlConfig::variantToString(const QVariant &v) result += QLatin1Char(')'); break; } - case QVariant::Size: { + case QVariant::Size: + { QSize s = qvariant_cast(v); result += QLatin1String("@Size("); result += QString::number(s.width()); @@ -222,7 +229,8 @@ QString XmlConfig::variantToString(const QVariant &v) result += QLatin1Char(')'); break; } - case QVariant::Point: { + case QVariant::Point: + { QPoint p = qvariant_cast(v); result += QLatin1String("@Point("); result += QString::number(p.x()); @@ -233,7 +241,8 @@ QString XmlConfig::variantToString(const QVariant &v) } #endif // !QT_NO_GEOM_VARIANT - default: { + default: + { #ifndef QT_NO_DATASTREAM QByteArray a; { @@ -242,20 +251,20 @@ QString XmlConfig::variantToString(const QVariant &v) s << v; } - result = QLatin1String("@Variant("); + result = QLatin1String("@Variant("); result += QString::fromLatin1(a.toBase64().constData()); result += QLatin1Char(')'); - // These were being much too noisy!! - //qDebug() << "Variant Type: " << v.type(); - //qDebug()<< "Variant: " << result; + // These were being much too noisy!! + // qDebug() << "Variant Type: " << v.type(); + // qDebug()<< "Variant: " << result; #else Q_ASSERT(!"QSettings: Cannot save custom types without QDataStream support"); #endif break; } -} + } -return result; + return result; } QVariant XmlConfig::stringToVariant(const QString &s) @@ -272,30 +281,34 @@ QVariant XmlConfig::stringToVariant(const QString &s) QVariant result; stream >> result; return result; + #else Q_ASSERT(!"QSettings: Cannot load custom types without QDataStream support"); #endif #ifndef QT_NO_GEOM_VARIANT } else if (s.startsWith(QLatin1String("@Rect("))) { QStringList args = splitArgs(s, 5); - if (args.size() == 4) + if (args.size() == 4) { return QVariant(QRect(args[0].toInt(), args[1].toInt(), args[2].toInt(), args[3].toInt())); + } } else if (s.startsWith(QLatin1String("@Size("))) { QStringList args = splitArgs(s, 5); - if (args.size() == 2) + if (args.size() == 2) { return QVariant(QSize(args[0].toInt(), args[1].toInt())); + } } else if (s.startsWith(QLatin1String("@Point("))) { QStringList args = splitArgs(s, 6); - if (args.size() == 2) + if (args.size() == 2) { return QVariant(QPoint(args[0].toInt(), args[1].toInt())); + } #endif } else if (s == QLatin1String("@Invalid()")) { return QVariant(); } - } - if (s.startsWith(QLatin1String("@@"))) + if (s.startsWith(QLatin1String("@@"))) { return QVariant(s.mid(1)); + } } return QVariant(s); @@ -304,6 +317,7 @@ QVariant XmlConfig::stringToVariant(const QString &s) QStringList XmlConfig::splitArgs(const QString &s, int idx) { int l = s.length(); + Q_ASSERT(l > 0); Q_ASSERT(s.at(idx) == QLatin1Char('(')); Q_ASSERT(s.at(l - 1) == QLatin1Char(')')); diff --git a/ground/openpilotgcs/src/libs/utils/xmlconfig.h b/ground/openpilotgcs/src/libs/utils/xmlconfig.h index 447b2b6d4..98345e38d 100644 --- a/ground/openpilotgcs/src/libs/utils/xmlconfig.h +++ b/ground/openpilotgcs/src/libs/utils/xmlconfig.h @@ -37,9 +37,7 @@ #include #include -class XMLCONFIG_EXPORT XmlConfig : QObject -{ - +class XMLCONFIG_EXPORT XmlConfig : QObject { public: static const QSettings::Format XmlSettingsFormat; @@ -49,8 +47,8 @@ public: private: static QString rootName; - static void handleNode(QDomElement* node, QSettings::SettingsMap &map, QString path = ""); - static QSettings::SettingsMap settingsToMap(QSettings& qs); + static void handleNode(QDomElement *node, QSettings::SettingsMap &map, QString path = ""); + static QSettings::SettingsMap settingsToMap(QSettings & qs); static QString variantToString(const QVariant &v); static QVariant stringToVariant(const QString &s); static QStringList splitArgs(const QString &s, int idx); diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.cpp b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.cpp index 33cc84c6b..7f10d6029 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.cpp @@ -30,61 +30,59 @@ #include "antennatrackgadgetconfiguration.h" AntennaTrackGadget::AntennaTrackGadget(QString classId, AntennaTrackWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget), - connected(FALSE) + IUAVGadget(classId, parent), + m_widget(widget), + connected(FALSE) { - connect(m_widget->connectButton, SIGNAL(clicked(bool)), this,SLOT(onConnect())); - connect(m_widget->disconnectButton, SIGNAL(clicked(bool)), this,SLOT(onDisconnect())); + connect(m_widget->connectButton, SIGNAL(clicked(bool)), this, SLOT(onConnect())); + connect(m_widget->disconnectButton, SIGNAL(clicked(bool)), this, SLOT(onDisconnect())); } AntennaTrackGadget::~AntennaTrackGadget() -{ -} +{} /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void AntennaTrackGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void AntennaTrackGadget::loadConfiguration(IUAVGadgetConfiguration *config) { // Delete the (old)port, this also closes it. - if(port) { + if (port) { delete port; } // Delete the (old)parser, this also disconnects all signals. - if(parser) { + if (parser) { delete parser; } - AntennaTrackGadgetConfiguration *AntennaTrackConfig = qobject_cast< AntennaTrackGadgetConfiguration*>(config); + AntennaTrackGadgetConfiguration *AntennaTrackConfig = qobject_cast< AntennaTrackGadgetConfiguration *>(config); PortSettings portsettings; - portsettings.BaudRate=AntennaTrackConfig->speed(); - portsettings.DataBits=AntennaTrackConfig->dataBits(); - portsettings.FlowControl=AntennaTrackConfig->flow(); - portsettings.Parity=AntennaTrackConfig->parity(); - portsettings.StopBits=AntennaTrackConfig->stopBits(); - portsettings.Timeout_Millisec=AntennaTrackConfig->timeOut(); + portsettings.BaudRate = AntennaTrackConfig->speed(); + portsettings.DataBits = AntennaTrackConfig->dataBits(); + portsettings.FlowControl = AntennaTrackConfig->flow(); + portsettings.Parity = AntennaTrackConfig->parity(); + portsettings.StopBits = AntennaTrackConfig->stopBits(); + portsettings.Timeout_Millisec = AntennaTrackConfig->timeOut(); // In case we find no port, buttons disabled m_widget->connectButton->setEnabled(false); m_widget->disconnectButton->setEnabled(false); QList ports = QextSerialEnumerator::getPorts(); - foreach( QextPortInfo nport, ports ) { - if(nport.friendName == AntennaTrackConfig->port()) - { + foreach(QextPortInfo nport, ports) { + if (nport.friendName == AntennaTrackConfig->port()) { qDebug() << "Using Serial port"; - //parser = new NMEAParser(); + // parser = new NMEAParser(); #ifdef Q_OS_WIN - port=new QextSerialPort(nport.portName,portsettings,QextSerialPort::EventDriven); + port = new QextSerialPort(nport.portName, portsettings, QextSerialPort::EventDriven); #else - port=new QextSerialPort(nport.physName,portsettings,QextSerialPort::EventDriven); + port = new QextSerialPort(nport.physName, portsettings, QextSerialPort::EventDriven); #endif m_widget->setPort(port); m_widget->connectButton->setEnabled(true); @@ -99,32 +97,33 @@ void AntennaTrackGadget::loadConfiguration(IUAVGadgetConfiguration* config) qDebug() << "Using Telemetry parser"; parser = new TelemetryParser(); - connect(parser, SIGNAL(position(double,double,double)), m_widget,SLOT(setPosition(double,double,double))); - connect(parser, SIGNAL(home(double,double,double)), m_widget,SLOT(setHomePosition(double,double,double))); + connect(parser, SIGNAL(position(double, double, double)), m_widget, SLOT(setPosition(double, double, double))); + connect(parser, SIGNAL(home(double, double, double)), m_widget, SLOT(setHomePosition(double, double, double))); connect(parser, SIGNAL(packet(QString)), m_widget, SLOT(dumpPacket(QString))); } -void AntennaTrackGadget::onConnect() { +void AntennaTrackGadget::onConnect() +{ m_widget->textBrowser->append(QString("Connecting to Tracker ...\n")); // TODO: Somehow mark that we're running, and disable connect button while so? if (port) { - qDebug() << "Opening: " << port->portName() << "."; - bool isOpen = port->open(QIODevice::ReadWrite); - qDebug() << "Open: " << isOpen; - if(isOpen) { + qDebug() << "Opening: " << port->portName() << "."; + bool isOpen = port->open(QIODevice::ReadWrite); + qDebug() << "Open: " << isOpen; + if (isOpen) { m_widget->connectButton->setEnabled(false); m_widget->disconnectButton->setEnabled(true); } } else { qDebug() << "Port undefined or invalid."; } - } -void AntennaTrackGadget::onDisconnect() { +void AntennaTrackGadget::onDisconnect() +{ if (port) { - qDebug() << "Closing: " << port->portName() << "."; + qDebug() << "Closing: " << port->portName() << "."; port->close(); m_widget->connectButton->setEnabled(true); m_widget->disconnectButton->setEnabled(false); @@ -133,23 +132,27 @@ void AntennaTrackGadget::onDisconnect() { } } -void AntennaTrackGadget::onDataAvailable() { +void AntennaTrackGadget::onDataAvailable() +{ int avail = port->bytesAvailable(); - if( avail > 0 ) { + + if (avail > 0) { QByteArray serialData; serialData.resize(avail); int bytesRead = port->read(serialData.data(), serialData.size()); - if( bytesRead > 0 ) { + if (bytesRead > 0) { processNewSerialData(serialData); } } } -void AntennaTrackGadget::processNewSerialData(QByteArray serialData) { - int dataLength = serialData.size(); - const char* data = serialData.constData(); +void AntennaTrackGadget::processNewSerialData(QByteArray serialData) +{ + int dataLength = serialData.size(); + const char *data = serialData.constData(); + m_widget->textBrowser->append(QString(serialData)); - for(int pos = 0; pos < dataLength; pos++) { - //parser->processInputStream(data[pos]); + for (int pos = 0; pos < dataLength; pos++) { + // parser->processInputStream(data[pos]); } } diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.h b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.h index 9363abaf2..682bda768 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.h @@ -41,18 +41,20 @@ class AntennaTrackWidget; using namespace Core; -class AntennaTrackGadget : public Core::IUAVGadget -{ +class AntennaTrackGadget : public Core::IUAVGadget { Q_OBJECT public: AntennaTrackGadget(QString classId, AntennaTrackWidget *widget, QWidget *parent = 0); ~AntennaTrackGadget(); - QWidget *widget() { return m_widget; } + QWidget *widget() + { + return m_widget; + } - // void setMode(QString mode); // Either UAVTalk or serial port + // void setMode(QString mode); // Either UAVTalk or serial port - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); public slots: void onConnect(); void onDisconnect(); diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.cpp index f4c8ed23e..e346db875 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.cpp @@ -32,7 +32,7 @@ * Loads a saved configuration or defaults if non exist. * */ -AntennaTrackGadgetConfiguration::AntennaTrackGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +AntennaTrackGadgetConfiguration::AntennaTrackGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_connectionMode("Serial"), m_defaultPort("Unknown"), @@ -43,36 +43,35 @@ AntennaTrackGadgetConfiguration::AntennaTrackGadgetConfiguration(QString classId m_defaultStopBits(STOP_1), m_defaultTimeOut(5000) { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { BaudRateType speed; DataBitsType databits; FlowType flow; ParityType parity; StopBitsType stopbits; - int ispeed = qSettings->value("defaultSpeed").toInt(); - int idatabits = qSettings->value("defaultDataBits").toInt(); - int iflow = qSettings->value("defaultFlow").toInt(); - int iparity = qSettings->value("defaultParity").toInt(); - int istopbits = qSettings->value("defaultStopBits").toInt(); - QString port = qSettings->value("defaultPort").toString(); + int ispeed = qSettings->value("defaultSpeed").toInt(); + int idatabits = qSettings->value("defaultDataBits").toInt(); + int iflow = qSettings->value("defaultFlow").toInt(); + int iparity = qSettings->value("defaultParity").toInt(); + int istopbits = qSettings->value("defaultStopBits").toInt(); + QString port = qSettings->value("defaultPort").toString(); QString conMode = qSettings->value("connectionMode").toString(); - databits = (DataBitsType) idatabits; - flow = (FlowType)iflow; - parity = (ParityType)iparity; + databits = (DataBitsType)idatabits; + flow = (FlowType)iflow; + parity = (ParityType)iparity; stopbits = (StopBitsType)istopbits; - speed = (BaudRateType)ispeed; - m_defaultPort = port; - m_defaultSpeed = speed; + speed = (BaudRateType)ispeed; + m_defaultPort = port; + m_defaultSpeed = speed; m_defaultDataBits = databits; - m_defaultFlow = flow; - m_defaultParity = parity; + m_defaultFlow = flow; + m_defaultParity = parity; m_defaultStopBits = stopbits; - m_connectionMode = conMode; + m_connectionMode = conMode; } - } /** @@ -83,13 +82,13 @@ IUAVGadgetConfiguration *AntennaTrackGadgetConfiguration::clone() { AntennaTrackGadgetConfiguration *m = new AntennaTrackGadgetConfiguration(this->classId()); - m->m_defaultSpeed = m_defaultSpeed; + m->m_defaultSpeed = m_defaultSpeed; m->m_defaultDataBits = m_defaultDataBits; - m->m_defaultFlow = m_defaultFlow; - m->m_defaultParity = m_defaultParity; + m->m_defaultFlow = m_defaultFlow; + m->m_defaultParity = m_defaultParity; m->m_defaultStopBits = m_defaultStopBits; - m->m_defaultPort = m_defaultPort; - m->m_connectionMode = m_connectionMode; + m->m_defaultPort = m_defaultPort; + m->m_connectionMode = m_connectionMode; return m; } @@ -97,13 +96,13 @@ IUAVGadgetConfiguration *AntennaTrackGadgetConfiguration::clone() * Saves a configuration. * */ -void AntennaTrackGadgetConfiguration::saveConfig(QSettings* settings) const { - settings->setValue("defaultSpeed", m_defaultSpeed); - settings->setValue("defaultDataBits", m_defaultDataBits); - settings->setValue("defaultFlow", m_defaultFlow); - settings->setValue("defaultParity", m_defaultParity); - settings->setValue("defaultStopBits", m_defaultStopBits); - settings->setValue("defaultPort", m_defaultPort); - settings->setValue("connectionMode", m_connectionMode); - +void AntennaTrackGadgetConfiguration::saveConfig(QSettings *settings) const +{ + settings->setValue("defaultSpeed", m_defaultSpeed); + settings->setValue("defaultDataBits", m_defaultDataBits); + settings->setValue("defaultFlow", m_defaultFlow); + settings->setValue("defaultParity", m_defaultParity); + settings->setValue("defaultStopBits", m_defaultStopBits); + settings->setValue("defaultPort", m_defaultPort); + settings->setValue("connectionMode", m_connectionMode); } diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.h index 2521607ff..ad7c171f0 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.h @@ -33,46 +33,92 @@ using namespace Core; -class AntennaTrackGadgetConfiguration : public IUAVGadgetConfiguration -{ +class AntennaTrackGadgetConfiguration : public IUAVGadgetConfiguration { Q_OBJECT - public: - explicit AntennaTrackGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); +public: + explicit AntennaTrackGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void setConnectionMode(QString mode) { m_connectionMode = mode; } - QString connectionMode() { return m_connectionMode; } + void setConnectionMode(QString mode) + { + m_connectionMode = mode; + } + QString connectionMode() + { + return m_connectionMode; + } - //set port configuration functions - void setSpeed(BaudRateType speed) {m_defaultSpeed=speed;} - void setDataBits(DataBitsType databits) {m_defaultDataBits=databits;} - void setFlow(FlowType flow) {m_defaultFlow=flow;} - void setParity(ParityType parity) {m_defaultParity=parity;} - void setStopBits(StopBitsType stopbits) {m_defaultStopBits=stopbits;} - void setPort(QString port){m_defaultPort=port;} - void setTimeOut(long timeout){m_defaultTimeOut=timeout;} + // set port configuration functions + void setSpeed(BaudRateType speed) + { + m_defaultSpeed = speed; + } + void setDataBits(DataBitsType databits) + { + m_defaultDataBits = databits; + } + void setFlow(FlowType flow) + { + m_defaultFlow = flow; + } + void setParity(ParityType parity) + { + m_defaultParity = parity; + } + void setStopBits(StopBitsType stopbits) + { + m_defaultStopBits = stopbits; + } + void setPort(QString port) + { + m_defaultPort = port; + } + void setTimeOut(long timeout) + { + m_defaultTimeOut = timeout; + } - //get port configuration functions - QString port(){return m_defaultPort;} - BaudRateType speed() {return m_defaultSpeed;} - FlowType flow() {return m_defaultFlow;} - DataBitsType dataBits() {return m_defaultDataBits;} - StopBitsType stopBits() {return m_defaultStopBits;} - ParityType parity() {return m_defaultParity;} - long timeOut(){return m_defaultTimeOut;} + // get port configuration functions + QString port() + { + return m_defaultPort; + } + BaudRateType speed() + { + return m_defaultSpeed; + } + FlowType flow() + { + return m_defaultFlow; + } + DataBitsType dataBits() + { + return m_defaultDataBits; + } + StopBitsType stopBits() + { + return m_defaultStopBits; + } + ParityType parity() + { + return m_defaultParity; + } + long timeOut() + { + return m_defaultTimeOut; + } - void saveConfig(QSettings* settings) const; - IUAVGadgetConfiguration *clone(); - - private: - QString m_connectionMode; - QString m_defaultPort; - BaudRateType m_defaultSpeed; - DataBitsType m_defaultDataBits; - FlowType m_defaultFlow; - ParityType m_defaultParity; - StopBitsType m_defaultStopBits; - long m_defaultTimeOut; + void saveConfig(QSettings *settings) const; + IUAVGadgetConfiguration *clone(); +private: + QString m_connectionMode; + QString m_defaultPort; + BaudRateType m_defaultSpeed; + DataBitsType m_defaultDataBits; + FlowType m_defaultFlow; + ParityType m_defaultParity; + StopBitsType m_defaultStopBits; + long m_defaultTimeOut; }; #endif // ANTENNATRACKGADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.cpp index 7f369e033..14818b1cc 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.cpp @@ -32,29 +32,27 @@ #include AntennaTrackGadgetFactory::AntennaTrackGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("AntennaTrackGadget"), - tr("Antenna Track Gadget"), - parent) -{ -} + IUAVGadgetFactory(QString("AntennaTrackGadget"), + tr("Antenna Track Gadget"), + parent) +{} AntennaTrackGadgetFactory::~AntennaTrackGadgetFactory() -{ -} +{} -Core::IUAVGadget* AntennaTrackGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *AntennaTrackGadgetFactory::createGadget(QWidget *parent) { - AntennaTrackWidget* gadgetWidget = new AntennaTrackWidget(parent); + AntennaTrackWidget *gadgetWidget = new AntennaTrackWidget(parent); + return new AntennaTrackGadget(QString("AntennaTrackGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *AntennaTrackGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *AntennaTrackGadgetFactory::createConfiguration(QSettings *qSettings) { return new AntennaTrackGadgetConfiguration(QString("AntennaTrackGadget"), qSettings); } IOptionsPage *AntennaTrackGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new AntennaTrackGadgetOptionsPage(qobject_cast(config)); + return new AntennaTrackGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.h b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.h index e3efd07d5..f8e1333ba 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.h @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class AntennaTrackGadgetFactory : public IUAVGadgetFactory -{ +class AntennaTrackGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: AntennaTrackGadgetFactory(QObject *parent = 0); ~AntennaTrackGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.cpp index eb7c37b21..46f6df650 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.cpp @@ -34,135 +34,135 @@ #include AntennaTrackGadgetOptionsPage::AntennaTrackGadgetOptionsPage(AntennaTrackGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) + IOptionsPage(parent), + m_config(config) { -// Taken from the uploader gadget, since we also can use a serial port for this -// Gadget +// Taken from the uploader gadget, since we also can use a serial port for this +// Gadget - //the begining of some ugly code -//diferent OS's have diferent serial port capabilities + // the begining of some ugly code +// diferent OS's have diferent serial port capabilities #ifdef Q_OS_WIN -//load windows port capabilities -BaudRateTypeString - <<"BAUD110" - <<"BAUD300" - <<"BAUD600" - <<"BAUD1200" - <<"BAUD2400" - <<"BAUD4800" - <<"BAUD9600" - <<"BAUD14400" - <<"BAUD19200" - <<"BAUD38400" - <<"BAUD56000" - <<"BAUD57600" - <<"BAUD115200" - <<"BAUD128000" - <<"BAUD256000"; -DataBitsTypeString - <<"DATA_5" - <<"DATA_6" - <<"DATA_7" - <<"DATA_8"; -ParityTypeString - <<"PAR_NONE" - <<"PAR_ODD" - <<"PAR_EVEN" - <<"PAR_MARK" //WINDOWS ONLY - <<"PAR_SPACE"; -StopBitsTypeString - <<"STOP_1" - <<"STOP_1_5" //WINDOWS ONLY - <<"STOP_2"; -#else -//load POSIX port capabilities -BaudRateTypeString +// load windows port capabilities + BaudRateTypeString + << "BAUD110" + << "BAUD300" + << "BAUD600" + << "BAUD1200" + << "BAUD2400" + << "BAUD4800" + << "BAUD9600" + << "BAUD14400" + << "BAUD19200" + << "BAUD38400" + << "BAUD56000" + << "BAUD57600" + << "BAUD115200" + << "BAUD128000" + << "BAUD256000"; + DataBitsTypeString + << "DATA_5" + << "DATA_6" + << "DATA_7" + << "DATA_8"; + ParityTypeString + << "PAR_NONE" + << "PAR_ODD" + << "PAR_EVEN" + << "PAR_MARK" // WINDOWS ONLY + << "PAR_SPACE"; + StopBitsTypeString + << "STOP_1" + << "STOP_1_5" // WINDOWS ONLY + << "STOP_2"; +#else // ifdef Q_OS_WIN +// load POSIX port capabilities + BaudRateTypeString - <<"BAUD50" //POSIX ONLY - <<"BAUD75" //POSIX ONLY - <<"BAUD110" - <<"BAUD134" //POSIX ONLY - <<"BAUD150" //POSIX ONLY - <<"BAUD200" //POSIX ONLY - <<"BAUD300" - <<"BAUD600" - <<"BAUD1200" - <<"BAUD1800" //POSIX ONLY - <<"BAUD2400" - <<"BAUD4800" - <<"BAUD9600" - <<"BAUD19200" - <<"BAUD38400" - <<"BAUD57600" - <<"BAUD76800" //POSIX ONLY - <<"BAUD115200"; -DataBitsTypeString - <<"DATA_5" - <<"DATA_6" - <<"DATA_7" - <<"DATA_8"; -ParityTypeString - <<"PAR_NONE" - <<"PAR_ODD" - <<"PAR_EVEN" - <<"PAR_SPACE"; -StopBitsTypeString - <<"STOP_1" - <<"STOP_2"; -#endif -//load all OS's capabilities -BaudRateTypeStringALL - <<"BAUD50" //POSIX ONLY - <<"BAUD75" //POSIX ONLY - <<"BAUD110" - <<"BAUD134" //POSIX ONLY - <<"BAUD150" //POSIX ONLY - <<"BAUD200" //POSIX ONLY - <<"BAUD300" - <<"BAUD600" - <<"BAUD1200" - <<"BAUD1800" //POSIX ONLY - <<"BAUD2400" - <<"BAUD4800" - <<"BAUD9600" - <<"BAUD14400" - <<"BAUD19200" - <<"BAUD38400" - <<"BAUD56000" - <<"BAUD57600" - <<"BAUD76800" //POSIX ONLY - <<"BAUD115200" - <<"BAUD128000" - <<"BAUD256000"; -DataBitsTypeStringALL - <<"DATA_5" - <<"DATA_6" - <<"DATA_7" - <<"DATA_8"; -ParityTypeStringALL - <<"PAR_NONE" - <<"PAR_ODD" - <<"PAR_EVEN" - <<"PAR_MARK" //WINDOWS ONLY - <<"PAR_SPACE"; -StopBitsTypeStringALL - <<"STOP_1" - <<"STOP_1_5" //WINDOWS ONLY - <<"STOP_2"; + << "BAUD50" // POSIX ONLY + << "BAUD75" // POSIX ONLY + << "BAUD110" + << "BAUD134" // POSIX ONLY + << "BAUD150" // POSIX ONLY + << "BAUD200" // POSIX ONLY + << "BAUD300" + << "BAUD600" + << "BAUD1200" + << "BAUD1800" // POSIX ONLY + << "BAUD2400" + << "BAUD4800" + << "BAUD9600" + << "BAUD19200" + << "BAUD38400" + << "BAUD57600" + << "BAUD76800" // POSIX ONLY + << "BAUD115200"; + DataBitsTypeString + << "DATA_5" + << "DATA_6" + << "DATA_7" + << "DATA_8"; + ParityTypeString + << "PAR_NONE" + << "PAR_ODD" + << "PAR_EVEN" + << "PAR_SPACE"; + StopBitsTypeString + << "STOP_1" + << "STOP_2"; +#endif // ifdef Q_OS_WIN +// load all OS's capabilities + BaudRateTypeStringALL + << "BAUD50" // POSIX ONLY + << "BAUD75" // POSIX ONLY + << "BAUD110" + << "BAUD134" // POSIX ONLY + << "BAUD150" // POSIX ONLY + << "BAUD200" // POSIX ONLY + << "BAUD300" + << "BAUD600" + << "BAUD1200" + << "BAUD1800" // POSIX ONLY + << "BAUD2400" + << "BAUD4800" + << "BAUD9600" + << "BAUD14400" + << "BAUD19200" + << "BAUD38400" + << "BAUD56000" + << "BAUD57600" + << "BAUD76800" // POSIX ONLY + << "BAUD115200" + << "BAUD128000" + << "BAUD256000"; + DataBitsTypeStringALL + << "DATA_5" + << "DATA_6" + << "DATA_7" + << "DATA_8"; + ParityTypeStringALL + << "PAR_NONE" + << "PAR_ODD" + << "PAR_EVEN" + << "PAR_MARK" // WINDOWS ONLY + << "PAR_SPACE"; + StopBitsTypeStringALL + << "STOP_1" + << "STOP_1_5" // WINDOWS ONLY + << "STOP_2"; -FlowTypeString - <<"FLOW_OFF" - <<"FLOW_HARDWARE" - <<"FLOW_XONXOFF"; + FlowTypeString + << "FLOW_OFF" + << "FLOW_HARDWARE" + << "FLOW_XONXOFF"; } -bool sortPorts(QextPortInfo const& s1,QextPortInfo const& s2) +bool sortPorts(QextPortInfo const & s1, QextPortInfo const & s2) { -return s1.portName ports = QextSerialEnumerator::getPorts(); - qSort(ports.begin(), ports.end(),sortPorts); - foreach( QextPortInfo port, ports ) { + qSort(ports.begin(), ports.end(), sortPorts); + foreach(QextPortInfo port, ports) { qDebug() << "Adding port: " << port.friendName << " (" << port.portName << ")"; options_page->portComboBox->addItem(port.friendName, port.friendName); } int portIndex = options_page->portComboBox->findData(m_config->port()); - if(portIndex!=-1){ + if (portIndex != -1) { qDebug() << "createPage(): port is " << m_config->port(); options_page->portComboBox->setCurrentIndex(portIndex); } @@ -188,40 +188,40 @@ QWidget *AntennaTrackGadgetOptionsPage::createPage(QWidget *parent) options_page->portSpeedComboBox->addItems(BaudRateTypeString); int portSpeedIndex = options_page->portSpeedComboBox->findText(BaudRateTypeStringALL.at((int)m_config->speed())); - if(portSpeedIndex != -1){ - options_page->portSpeedComboBox->setCurrentIndex(portSpeedIndex); + if (portSpeedIndex != -1) { + options_page->portSpeedComboBox->setCurrentIndex(portSpeedIndex); } // FLOW CONTROL options_page->flowControlComboBox->addItems(FlowTypeString); int flowControlIndex = options_page->flowControlComboBox->findText(FlowTypeString.at((int)m_config->flow())); - if(flowControlIndex != -1){ - options_page->flowControlComboBox->setCurrentIndex(flowControlIndex); + if (flowControlIndex != -1) { + options_page->flowControlComboBox->setCurrentIndex(flowControlIndex); } // DATABITS options_page->dataBitsComboBox->addItems(DataBitsTypeString); int dataBitsIndex = options_page->dataBitsComboBox->findText(DataBitsTypeStringALL.at((int)m_config->dataBits())); - if(dataBitsIndex != -1){ - options_page->dataBitsComboBox->setCurrentIndex(dataBitsIndex); + if (dataBitsIndex != -1) { + options_page->dataBitsComboBox->setCurrentIndex(dataBitsIndex); } // STOPBITS options_page->stopBitsComboBox->addItems(StopBitsTypeString); int stopBitsIndex = options_page->stopBitsComboBox->findText(StopBitsTypeStringALL.at((int)m_config->stopBits())); - if(stopBitsIndex != -1){ - options_page->stopBitsComboBox->setCurrentIndex(stopBitsIndex); + if (stopBitsIndex != -1) { + options_page->stopBitsComboBox->setCurrentIndex(stopBitsIndex); } // PARITY options_page->parityComboBox->addItems(ParityTypeString); int parityIndex = options_page->parityComboBox->findText(ParityTypeStringALL.at((int)m_config->parity())); - if(parityIndex != -1){ - options_page->parityComboBox->setCurrentIndex(parityIndex); + if (parityIndex != -1) { + options_page->parityComboBox->setCurrentIndex(parityIndex); } // TIMEOUT @@ -231,8 +231,9 @@ QWidget *AntennaTrackGadgetOptionsPage::createPage(QWidget *parent) connectionModes << "Serial"; options_page->connectionMode->addItems(connectionModes); int conMode = options_page->connectionMode->findText(m_config->connectionMode()); - if (conMode != -1) - options_page->connectionMode->setCurrentIndex(conMode); + if (conMode != -1) { + options_page->connectionMode->setCurrentIndex(conMode); + } return optionsPageWidget; @@ -247,6 +248,7 @@ QWidget *AntennaTrackGadgetOptionsPage::createPage(QWidget *parent) void AntennaTrackGadgetOptionsPage::apply() { int portIndex = options_page->portComboBox->currentIndex(); + m_config->setPort(options_page->portComboBox->itemData(portIndex).toString()); qDebug() << "apply(): port is " << m_config->port(); @@ -255,9 +257,8 @@ void AntennaTrackGadgetOptionsPage::apply() m_config->setDataBits((DataBitsType)DataBitsTypeStringALL.indexOf(options_page->dataBitsComboBox->currentText())); m_config->setStopBits((StopBitsType)StopBitsTypeStringALL.indexOf(options_page->stopBitsComboBox->currentText())); m_config->setParity((ParityType)ParityTypeStringALL.indexOf(options_page->parityComboBox->currentText())); - m_config->setTimeOut( options_page->timeoutSpinBox->value()); + m_config->setTimeOut(options_page->timeoutSpinBox->value()); m_config->setConnectionMode(options_page->connectionMode->currentText()); - } void AntennaTrackGadgetOptionsPage::finish() diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.h index 063869b91..07210dc6e 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.h @@ -41,14 +41,13 @@ class IUAVGadgetConfiguration; class AntennaTrackGadgetConfiguration; namespace Ui { - class AntennaTrackGadgetOptionsPage; +class AntennaTrackGadgetOptionsPage; } using namespace Core; -class AntennaTrackGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class AntennaTrackGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit AntennaTrackGadgetOptionsPage(AntennaTrackGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.cpp b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.cpp index f63a17ddf..b1593639a 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.cpp @@ -5,15 +5,18 @@ #include #include -AntennaTrackPlugin::AntennaTrackPlugin() { - // Do nothing - } - -AntennaTrackPlugin::~AntennaTrackPlugin() { +AntennaTrackPlugin::AntennaTrackPlugin() +{ // Do nothing } -bool AntennaTrackPlugin::initialize(const QStringList& args, QString *errMsg) { +AntennaTrackPlugin::~AntennaTrackPlugin() +{ + // Do nothing +} + +bool AntennaTrackPlugin::initialize(const QStringList & args, QString *errMsg) +{ Q_UNUSED(args); Q_UNUSED(errMsg); @@ -23,11 +26,13 @@ bool AntennaTrackPlugin::initialize(const QStringList& args, QString *errMsg) { return true; } -void AntennaTrackPlugin::extensionsInitialized() { +void AntennaTrackPlugin::extensionsInitialized() +{ // Do nothing } -void AntennaTrackPlugin::shutdown() { +void AntennaTrackPlugin::shutdown() +{ // Do nothing } diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.h b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.h index 71cf7392c..17c1f37b2 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.h @@ -5,14 +5,13 @@ class AntennaTrackGadgetFactory; -class AntennaTrackPlugin : public ExtensionSystem::IPlugin -{ +class AntennaTrackPlugin : public ExtensionSystem::IPlugin { public: AntennaTrackPlugin(); ~AntennaTrackPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); private: AntennaTrackGadgetFactory *mf; diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.cpp b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.cpp index 90ad37aac..9306951d2 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.cpp @@ -41,22 +41,21 @@ AntennaTrackWidget::AntennaTrackWidget(QWidget *parent) : QWidget(parent) { setupUi(this); - azimuth_old=0; - elevation_old=0; + azimuth_old = 0; + elevation_old = 0; } AntennaTrackWidget::~AntennaTrackWidget() -{ -} +{} void AntennaTrackWidget::setPort(QPointer portx) { - port=portx; + port = portx; } void AntennaTrackWidget::dumpPacket(const QString &packet) { textBrowser->append(packet); - if(textBrowser->document()->lineCount() > 200) { + if (textBrowser->document()->lineCount() > 200) { QTextCursor tc = textBrowser->textCursor(); tc.movePosition(QTextCursor::Start); tc.movePosition(QTextCursor::Down, QTextCursor::KeepAnchor); @@ -67,63 +66,69 @@ void AntennaTrackWidget::dumpPacket(const QString &packet) void AntennaTrackWidget::setPosition(double lat, double lon, double alt) { - //lat *= 1E-7; - //lon *= 1E-7; + // lat *= 1E-7; + // lon *= 1E-7; double deg = floor(fabs(lat)); - double min = (fabs(lat)-deg)*60; + double min = (fabs(lat) - deg) * 60; QString str1; - str1.sprintf("%.0f%c%.3f' ", deg,0x00b0, min); - if (lat>0) + + str1.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min); + if (lat > 0) { str1.append("N"); - else + } else { str1.append("S"); + } coord_value->setText(str1); deg = floor(fabs(lon)); - min = (fabs(lon)-deg)*60; + min = (fabs(lon) - deg) * 60; QString str2; - str2.sprintf("%.0f%c%.3f' ", deg,0x00b0, min); - if (lon>0) + str2.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min); + if (lon > 0) { str2.append("E"); - else + } else { str2.append("W"); + } coord_value_2->setText(str2); QString str3; str3.sprintf("%.2f m", alt); coord_value_3->setText(str3); - TrackData.Latitude=lat; - TrackData.Longitude=lon; - TrackData.Altitude=alt; + TrackData.Latitude = lat; + TrackData.Longitude = lon; + TrackData.Altitude = alt; calcAntennaPosition(); } void AntennaTrackWidget::setHomePosition(double lat, double lon, double alt) { - //lat *= 1E-7; - //lon *= 1E-7; + // lat *= 1E-7; + // lon *= 1E-7; double deg = floor(fabs(lat)); - double min = (fabs(lat)-deg)*60; + double min = (fabs(lat) - deg) * 60; QString str1; - str1.sprintf("%.0f%c%.3f' ", deg,0x00b0, min); - if (lat>0) + + str1.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min); + if (lat > 0) { str1.append("N"); - else + } else { str1.append("S"); + } speed_value->setText(str1); deg = floor(fabs(lon)); - min = (fabs(lon)-deg)*60; + min = (fabs(lon) - deg) * 60; QString str2; - str2.sprintf("%.0f%c%.3f' ", deg,0x00b0, min); - if (lon>0) + str2.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min); + if (lon > 0) { str2.append("E"); - else + } else { str2.append("W"); + } bear_label->setText(str2); QString str3; str3.sprintf("%.2f m", alt); bear_value->setText(str3); - TrackData.HomeLatitude=lat; - TrackData.HomeLongitude=lon; - TrackData.HomeAltitude=alt; + TrackData.HomeLatitude = lat; + TrackData.HomeLongitude = lon; + TrackData.HomeAltitude = alt; calcAntennaPosition(); } @@ -132,55 +137,57 @@ void AntennaTrackWidget::calcAntennaPosition(void) /** http://www.movable-type.co.uk/scripts/latlong.html **/ double lat1, lat2, lon1, lon2, a, c, d, x, y, brng; double azimuth, elevation; - double gcsAlt=TrackData.HomeAltitude; // Home MSL altitude - double uavAlt=TrackData.Altitude; // UAV MSL altitude - double dAlt=uavAlt-gcsAlt; // Altitude difference + double gcsAlt = TrackData.HomeAltitude; // Home MSL altitude + double uavAlt = TrackData.Altitude; // UAV MSL altitude + double dAlt = uavAlt - gcsAlt; // Altitude difference // Convert to radians - lat1 = TrackData.HomeLatitude*(M_PI/180); // Home lat - lon1 = TrackData.HomeLongitude*(M_PI/180); // Home lon - lat2 = TrackData.Latitude*(M_PI/180); // UAV lat - lon2 = TrackData.Longitude*(M_PI/180); // UAV lon + lat1 = TrackData.HomeLatitude * (M_PI / 180); // Home lat + lon1 = TrackData.HomeLongitude * (M_PI / 180); // Home lon + lat2 = TrackData.Latitude * (M_PI / 180); // UAV lat + lon2 = TrackData.Longitude * (M_PI / 180); // UAV lon // Bearing /** - var y = Math.sin(dLon) * Math.cos(lat2); - var x = Math.cos(lat1)*Math.sin(lat2) - + var y = Math.sin(dLon) * Math.cos(lat2); + var x = Math.cos(lat1)*Math.sin(lat2) - Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon); - var brng = Math.atan2(y, x).toDeg(); - **/ - y = sin(lon2-lon1) * cos(lat2); - x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon2-lon1); - brng = atan2((sin(lon2-lon1)*cos(lat2)),(cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)))*(180/M_PI); - if(brng<0) - brng+=360; + var brng = Math.atan2(y, x).toDeg(); + **/ + y = sin(lon2 - lon1) * cos(lat2); + x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon2 - lon1); + brng = atan2((sin(lon2 - lon1) * cos(lat2)), (cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon2 - lon1))) * (180 / M_PI); + if (brng < 0) { + brng += 360; + } // bearing to stepper azimuth = brng; // Haversine formula for distance /** - var R = 6371; // km - var dLat = (lat2-lat1).toRad(); - var dLon = (lon2-lon1).toRad(); - var a = Math.sin(dLat/2) * Math.sin(dLat/2) + + var R = 6371; // km + var dLat = (lat2-lat1).toRad(); + var dLon = (lon2-lon1).toRad(); + var a = Math.sin(dLat/2) * Math.sin(dLat/2) + Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) * Math.sin(dLon/2) * Math.sin(dLon/2); - var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)); - var d = R * c; - **/ - a = sin((lat2-lat1)/2) * sin((lat2-lat1)/2) + - cos(lat1) * cos(lat2) * - sin((lon2-lon1)/2) * sin((lon2-lon1)/2); - c = 2 * atan2(sqrt(a), sqrt(1-a)); + var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)); + var d = R * c; + **/ + a = sin((lat2 - lat1) / 2) * sin((lat2 - lat1) / 2) + + cos(lat1) * cos(lat2) * + sin((lon2 - lon1) / 2) * sin((lon2 - lon1) / 2); + c = 2 * atan2(sqrt(a), sqrt(1 - a)); d = 6371 * 1000 * c; // Elevation v depends servo direction - if(d!=0) - elevation = 90-(atan(dAlt/d)*(180/M_PI)); - else + if (d != 0) { + elevation = 90 - (atan(dAlt / d) * (180 / M_PI)); + } else { elevation = 0; - //! TODO: sanity check + } + // ! TODO: sanity check QString str3; str3.sprintf("%.0f deg", azimuth); @@ -189,18 +196,17 @@ void AntennaTrackWidget::calcAntennaPosition(void) str3.sprintf("%.0f deg", elevation); elevation_value->setText(str3); - //servo value 2000-4000 - int servo = (int)(2000.0/180*elevation+2000); - int stepper = (int)(400.0/360*(azimuth-azimuth_old)); + // servo value 2000-4000 + int servo = (int)(2000.0 / 180 * elevation + 2000); + int stepper = (int)(400.0 / 360 * (azimuth - azimuth_old)); // send azimuth and elevation to tracker hardware - str3.sprintf("move %d 2000 2000 2000 %d\r", stepper,servo); - if(port->isOpen()) - { - if(azimuth_old!=azimuth || elevation!=elevation_old) + str3.sprintf("move %d 2000 2000 2000 %d\r", stepper, servo); + if (port->isOpen()) { + if (azimuth_old != azimuth || elevation != elevation_old) { port->write(str3.toAscii()); - azimuth_old = azimuth; + } + azimuth_old = azimuth; elevation_old = elevation; } - } diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.h b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.h index 8b1bea216..17f9abe65 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.h @@ -39,37 +39,34 @@ class Ui_AntennaTrackWidget; -typedef struct struct_TrackData -{ - double Latitude; - double Longitude; - double Altitude; - double HomeLatitude; - double HomeLongitude; - double HomeAltitude; +typedef struct struct_TrackData { + double Latitude; + double Longitude; + double Altitude; + double HomeLatitude; + double HomeLongitude; + double HomeAltitude; +} TrackData_t; -}TrackData_t; - -class AntennaTrackWidget : public QWidget, public Ui_AntennaTrackWidget -{ +class AntennaTrackWidget : public QWidget, public Ui_AntennaTrackWidget { Q_OBJECT public: AntennaTrackWidget(QWidget *parent = 0); - ~AntennaTrackWidget(); - TrackData_t TrackData; - void setPort(QPointer portx); + ~AntennaTrackWidget(); + TrackData_t TrackData; + void setPort(QPointer portx); private slots: - void setPosition(double, double, double); - void setHomePosition(double, double, double); - void dumpPacket(const QString &packet); + void setPosition(double, double, double); + void setHomePosition(double, double, double); + void dumpPacket(const QString &packet); private: - void calcAntennaPosition(void); - QGraphicsSvgItem * marker; - QPointer port; - double azimuth_old; - double elevation_old; + void calcAntennaPosition(void); + QGraphicsSvgItem *marker; + QPointer port; + double azimuth_old; + double elevation_old; }; #endif /* ANTENNATRACKWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.cpp b/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.cpp index da5a3bcfd..a13af7376 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.cpp @@ -33,11 +33,11 @@ GPSParser::GPSParser(QObject *parent) : QObject(parent) } GPSParser::~GPSParser() -{ +{} -} - -void GPSParser::processInputStream(char c) { +void GPSParser::processInputStream(char c) { - Q_UNUSED(c)} + { + Q_UNUSED(c) + } } diff --git a/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.h b/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.h index e189e9ceb..db6765e3b 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.h @@ -32,28 +32,26 @@ #include #include -class GPSParser: public QObject -{ +class GPSParser : public QObject { Q_OBJECT -public: - ~GPSParser(); +public: ~GPSParser(); virtual void processInputStream(char c); protected: GPSParser(QObject *parent = 0); signals: - void sv(int); // Satellites in view - void position(double,double,double); // Lat, Lon, Alt - void home(double,double,double); // Lat, Lon, Alt - void datetime(double,double); // Date then time - void speedheading(double,double); - void packet(QString); // Raw NMEA Packet (or just info) - void satellite(int,int,int,int,int); // Index, PRN, Elevation, Azimuth, SNR - void fixmode(QString); // Mode of fix: "Auto", "Manual". - void fixtype(QString); // Type of fix: "NoGPS", "NoFix", "Fix2D", "Fix3D". - void dop(double, double, double); // HDOP, VDOP, PDOP - void fixSVs(QList); // SV's used for fix. + void sv(int); // Satellites in view + void position(double, double, double); // Lat, Lon, Alt + void home(double, double, double); // Lat, Lon, Alt + void datetime(double, double); // Date then time + void speedheading(double, double); + void packet(QString); // Raw NMEA Packet (or just info) + void satellite(int, int, int, int, int); // Index, PRN, Elevation, Azimuth, SNR + void fixmode(QString); // Mode of fix: "Auto", "Manual". + void fixtype(QString); // Type of fix: "NoGPS", "NoFix", "Fix2D", "Fix3D". + void dop(double, double, double); // HDOP, VDOP, PDOP + void fixSVs(QList); // SV's used for fix. }; #endif // GPSPARSER_H diff --git a/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp b/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp index fc72735e2..6bbdef701 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp @@ -39,43 +39,44 @@ TelemetryParser::TelemetryParser(QObject *parent) : GPSParser(parent) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject *gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + UAVDataObject *gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + if (gpsObj != NULL) { - connect(gpsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateGPS(UAVObject*))); + connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGPS(UAVObject *))); } else { qDebug() << "Error: Object is unknown (GPSPosition)."; } - gpsObj = dynamic_cast(objManager->getObject("HomeLocation")); + gpsObj = dynamic_cast(objManager->getObject("HomeLocation")); if (gpsObj != NULL) { - connect(gpsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateHome(UAVObject*))); + connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateHome(UAVObject *))); } else { qDebug() << "Error: Object is unknown (HomeLocation)."; } - } TelemetryParser::~TelemetryParser() +{} + +void TelemetryParser::updateHome(UAVObject *object1) { - -} - -void TelemetryParser::updateHome( UAVObject* object1) { double lat = object1->getField(QString("Latitude"))->getDouble(); double lon = object1->getField(QString("Longitude"))->getDouble(); double alt = object1->getField(QString("Altitude"))->getDouble(); + lat *= 1E-7; lon *= 1E-7; - emit home(lat,lon,alt); + emit home(lat, lon, alt); } -void TelemetryParser::updateGPS( UAVObject* object1) { +void TelemetryParser::updateGPS(UAVObject *object1) +{ double lat = object1->getField(QString("Latitude"))->getDouble(); double lon = object1->getField(QString("Longitude"))->getDouble(); double alt = object1->getField(QString("Altitude"))->getDouble(); + lat *= 1E-7; lon *= 1E-7; - emit position(lat,lon,alt); + emit position(lat, lon, alt); } - diff --git a/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.h b/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.h index c3e37fa77..b054a80d4 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.h @@ -36,17 +36,15 @@ #include "gpsparser.h" -class TelemetryParser: public GPSParser -{ - +class TelemetryParser : public GPSParser { Q_OBJECT - + public: - TelemetryParser(QObject *parent = 0); - ~TelemetryParser(); + TelemetryParser(QObject *parent = 0); + ~TelemetryParser(); public slots: - void updateGPS(UAVObject* object1); - void updateHome(UAVObject* object1); + void updateGPS(UAVObject *object1); + void updateHome(UAVObject *object1); }; #endif // TELEMETRYPARSER_H diff --git a/ground/openpilotgcs/src/plugins/config/alignment-calibration.cpp b/ground/openpilotgcs/src/plugins/config/alignment-calibration.cpp index c6b880a77..fde1c0f4b 100644 --- a/ground/openpilotgcs/src/plugins/config/alignment-calibration.cpp +++ b/ground/openpilotgcs/src/plugins/config/alignment-calibration.cpp @@ -20,32 +20,31 @@ using namespace Eigen; * reference frame. * @param n_samples The number of samples. */ -void -calibration_misalignment(Vector3f& rotationVector, - const Vector3f samples0[], - const Vector3f& reference0, - const Vector3f samples1[], - const Vector3f& reference1, - size_t n_samples) +void calibration_misalignment(Vector3f & rotationVector, + const Vector3f samples0[], + const Vector3f & reference0, + const Vector3f samples1[], + const Vector3f & reference1, + size_t n_samples) { - // Note that this implementation makes the assumption that the angular - // misalignment is small. Something based on QUEST would be needed to - // account for errors larger than a few degrees. - Matrix X(n_samples, 3); - Matrix y(n_samples, 1); + // Note that this implementation makes the assumption that the angular + // misalignment is small. Something based on QUEST would be needed to + // account for errors larger than a few degrees. + Matrix X(n_samples, 3); + Matrix y(n_samples, 1); - AngleAxisd reference(Quaterniond().setFromTwoVectors( - reference0.cast(), reference1.cast())); - for (size_t i = 0; i < n_samples; ++i) { - AngleAxisd observation(Quaterniond().setFromTwoVectors( - samples0[i].cast(), samples1[i].cast())); + AngleAxisd reference(Quaterniond().setFromTwoVectors( + reference0.cast(), reference1.cast())); + for (size_t i = 0; i < n_samples; ++i) { + AngleAxisd observation(Quaterniond().setFromTwoVectors( + samples0[i].cast(), samples1[i].cast())); - X.row(i) = observation.axis(); - y[i] = reference.angle() - observation.angle(); - } + X.row(i) = observation.axis(); + y[i] = reference.angle() - observation.angle(); + } - // Run linear least squares over the result. - Vector3d result; - (X.transpose() * X).ldlt().solve(X.transpose()*y, &result); - rotationVector = result.cast(); + // Run linear least squares over the result. + Vector3d result; + (X.transpose() * X).ldlt().solve(X.transpose() * y, &result); + rotationVector = result.cast(); } diff --git a/ground/openpilotgcs/src/plugins/config/assertions.h b/ground/openpilotgcs/src/plugins/config/assertions.h index 6b8c0c8fe..fc865ec55 100644 --- a/ground/openpilotgcs/src/plugins/config/assertions.h +++ b/ground/openpilotgcs/src/plugins/config/assertions.h @@ -28,51 +28,52 @@ #include template -bool hasNaN(const MatrixBase& expr); +bool hasNaN(const MatrixBase & expr); template -bool hasInf(const MatrixBase& expr); +bool hasInf(const MatrixBase & expr); template -bool perpendicular(const MatrixBase& expl, const MatrixBase& expr); +bool perpendicular(const MatrixBase & expl, const MatrixBase & expr); template -bool hasNaN(const MatrixBase& expr) +bool hasNaN(const MatrixBase & expr) { - for (int j = 0; j != expr.cols(); ++j) { - for (int i = 0; i != expr.rows(); ++i) { - if (std::isnan(expr.coeff(i, j))) - return true; - } - } - return false; + for (int j = 0; j != expr.cols(); ++j) { + for (int i = 0; i != expr.rows(); ++i) { + if (std::isnan(expr.coeff(i, j))) { + return true; + } + } + } + return false; } template -bool hasInf(const MatrixBase& expr) +bool hasInf(const MatrixBase & expr) { - for (int i = 0; i != expr.cols(); ++i) { - for (int j = 0; j != expr.rows(); ++j) { - if (std::isinf(expr.coeff(j, i))) - return true; - } - } - return false; + for (int i = 0; i != expr.cols(); ++i) { + for (int j = 0; j != expr.rows(); ++j) { + if (std::isinf(expr.coeff(j, i))) { + return true; + } + } + } + return false; } template -bool isReal(const MatrixBase& exp) +bool isReal(const MatrixBase & exp) { - return !hasNaN(exp) && ! hasInf(exp); + return !hasNaN(exp) && !hasInf(exp); } template -bool perpendicular(const MatrixBase& lhs, const MatrixBase& rhs) +bool perpendicular(const MatrixBase & lhs, const MatrixBase & rhs) { - // A really weak check for "almost perpendicular". Use it for debugging - // purposes only. - return fabs(rhs.dot(lhs)) < 0.0001; + // A really weak check for "almost perpendicular". Use it for debugging + // purposes only. + return fabs(rhs.dot(lhs)) < 0.0001; } #endif /* ASSERTIONS_HPP_ */ - diff --git a/ground/openpilotgcs/src/plugins/config/calibration.h b/ground/openpilotgcs/src/plugins/config/calibration.h index 8fa40167d..47805d105 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration.h +++ b/ground/openpilotgcs/src/plugins/config/calibration.h @@ -6,49 +6,42 @@ using std::size_t; using namespace Eigen; -void -calibration_misalignment(Vector3f& rotationVector, - const Vector3f samples0[], - const Vector3f& reference0, - const Vector3f samples1[], - const Vector3f& reference1, - size_t n_samples); +void calibration_misalignment(Vector3f & rotationVector, + const Vector3f samples0[], + const Vector3f & reference0, + const Vector3f samples1[], + const Vector3f & reference1, + size_t n_samples); -Vector3f -twostep_bias_only(const Vector3f samples[], - size_t n_samples, - const Vector3f& referenceField, - const float noise); +Vector3f twostep_bias_only(const Vector3f samples[], + size_t n_samples, + const Vector3f & referenceField, + const float noise); -void -twostep_bias_scale(Vector3f& bias, - Vector3f& scale, - const Vector3f samples[], - const size_t n_samples, - const Vector3f& referenceField, - const float noise); +void twostep_bias_scale(Vector3f & bias, + Vector3f & scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f & referenceField, + const float noise); -void -twostep_bias_scale(Vector3f& bias, - Matrix3f& scale, - const Vector3f samples[], - const size_t n_samples, - const Vector3f& referenceField, - const float noise); +void twostep_bias_scale(Vector3f & bias, + Matrix3f & scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f & referenceField, + const float noise); -void -openpilot_bias_scale(Vector3f& bias, - Vector3f& scale, - const Vector3f samples[], - const size_t n_samples, - const Vector3f& referenceField); +void openpilot_bias_scale(Vector3f & bias, + Vector3f & scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f & referenceField); -void -gyroscope_calibration(Vector3f& bias, - Matrix3f& accelSensitivity, - Vector3f gyroSamples[], - Vector3f accelSamples[], - size_t n_samples); +void gyroscope_calibration(Vector3f & bias, + Matrix3f & accelSensitivity, + Vector3f gyroSamples[], + Vector3f accelSamples[], + size_t n_samples); #endif // !defined AHRS_CALIBRATION_HPP - diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp index abab410fd..166d8c9a1 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp @@ -46,13 +46,14 @@ QStringList ConfigCcpmWidget::getChannelDescriptions() { // init a channel_numelem list of channel desc defaults QStringList channelDesc; - for (int i = 0; i < (int) ConfigCcpmWidget::CHANNEL_NUMELEM; i++) { + + for (int i = 0; i < (int)ConfigCcpmWidget::CHANNEL_NUMELEM; i++) { channelDesc.append(QString("-")); } // get the gui config data GUIConfigDataUnion configData = getConfigData(); - heliGUISettingsStruct heli = configData.heli; + heliGUISettingsStruct heli = configData.heli; if (heli.Throttle > 0) { channelDesc[heli.Throttle - 1] = QString("Throttle"); @@ -118,26 +119,26 @@ QStringList ConfigCcpmWidget::getChannelDescriptions() } ConfigCcpmWidget::ConfigCcpmWidget(QWidget *parent) : - VehicleConfig(parent), m_aircraft(new Ui_CcpmConfigWidget()) + VehicleConfig(parent), m_aircraft(new Ui_CcpmConfigWidget()) { m_aircraft->setupUi(this); SwashLvlConfigurationInProgress = 0; SwashLvlState = 0; SwashLvlServoInterlock = 0; - updatingFromHardware = FALSE; - updatingToHardware = FALSE; + updatingFromHardware = FALSE; + updatingToHardware = FALSE; // Initialization of the swashplaye widget m_aircraft->SwashplateImage->setScene(new QGraphicsScene(this)); m_aircraft->SwashLvlSwashplateImage->setScene(m_aircraft->SwashplateImage->scene()); m_aircraft->SwashLvlSwashplateImage->setSceneRect(-50, -50, 500, 500); - //m_aircraft->SwashLvlSwashplateImage->scale(.85,.85); + // m_aircraft->SwashLvlSwashplateImage->scale(.85,.85); - //m_aircraft->SwashplateImage->setSceneRect(SwashplateImg->boundingRect()); + // m_aircraft->SwashplateImage->setSceneRect(SwashplateImg->boundingRect()); m_aircraft->SwashplateImage->setSceneRect(-50, -30, 500, 500); - //m_aircraft->SwashplateImage->scale(.85,.85); + // m_aircraft->SwashplateImage->scale(.85,.85); QSvgRenderer *renderer = new QSvgRenderer(); renderer->load(QString(":/configgadget/images/ccpm_setup.svg")); @@ -146,7 +147,7 @@ ConfigCcpmWidget::ConfigCcpmWidget(QWidget *parent) : SwashplateImg->setSharedRenderer(renderer); SwashplateImg->setElementId("Swashplate"); SwashplateImg->setObjectName("Swashplate"); - //SwashplateImg->setScale(0.75); + // SwashplateImg->setScale(0.75); m_aircraft->SwashplateImage->scene()->addItem(SwashplateImg); QFont serifFont("Times", 24, QFont::Bold); @@ -163,13 +164,13 @@ ConfigCcpmWidget::ConfigCcpmWidget(QWidget *parent) : // creates a default pen QPen pen2; - //pen2.setStyle(Qt::DotLine); + // pen2.setStyle(Qt::DotLine); pen2.setWidth(1); pen2.setBrush(Qt::blue); - //pen2.setCapStyle(Qt::RoundCap); - //pen2.setJoinStyle(Qt::RoundJoin); + // pen2.setCapStyle(Qt::RoundCap); + // pen2.setJoinStyle(Qt::RoundJoin); - //brush.setStyle(Qt::RadialGradientPattern); + // brush.setStyle(Qt::RadialGradientPattern); QList ServoNames; ServoNames << "ServoW" << "ServoX" << "ServoY" << "ServoZ"; @@ -177,7 +178,7 @@ ConfigCcpmWidget::ConfigCcpmWidget(QWidget *parent) : for (int i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { ServoLines[i] = m_aircraft->SwashLvlSwashplateImage->scene()->addLine(0, 0, 100 * i, i * i * 100, pen); - Servos[i] = new QGraphicsSvgItem(); + Servos[i] = new QGraphicsSvgItem(); Servos[i]->setSharedRenderer(renderer); Servos[i]->setElementId(ServoNames.at(i)); m_aircraft->SwashplateImage->scene()->addItem(Servos[i]); @@ -193,12 +194,11 @@ ConfigCcpmWidget::ConfigCcpmWidget(QWidget *parent) : m_aircraft->SwashplateImage->scene()->addItem(ServosTextCircles[i]); m_aircraft->SwashplateImage->scene()->addItem(ServosText[i]); - SwashLvlSpinBoxes[i] = new QSpinBox(m_aircraft->SwashLvlSwashplateImage); // use QGraphicsView + SwashLvlSpinBoxes[i] = new QSpinBox(m_aircraft->SwashLvlSwashplateImage); // use QGraphicsView m_aircraft->SwashLvlSwashplateImage->scene()->addWidget(SwashLvlSpinBoxes[i]); SwashLvlSpinBoxes[i]->setMaximum(10000); SwashLvlSpinBoxes[i]->setMinimum(0); SwashLvlSpinBoxes[i]->setValue(0); - } // initialize our two mixer curves @@ -225,14 +225,14 @@ ConfigCcpmWidget::ConfigCcpmWidget(QWidget *parent) : QStringList Types; Types << QString::fromUtf8("CCPM 2 Servo 90º") << QString::fromUtf8("CCPM 3 Servo 90º") - << QString::fromUtf8("CCPM 4 Servo 90º") << QString::fromUtf8("CCPM 3 Servo 120º") - << QString::fromUtf8("CCPM 3 Servo 140º") << QString::fromUtf8("FP 2 Servo 90º") - << QString::fromUtf8("Coax 2 Servo 90º") << QString::fromUtf8("Custom - User Angles") - << QString::fromUtf8("Custom - Advanced Settings"); + << QString::fromUtf8("CCPM 4 Servo 90º") << QString::fromUtf8("CCPM 3 Servo 120º") + << QString::fromUtf8("CCPM 3 Servo 140º") << QString::fromUtf8("FP 2 Servo 90º") + << QString::fromUtf8("Coax 2 Servo 90º") << QString::fromUtf8("Custom - User Angles") + << QString::fromUtf8("Custom - Advanced Settings"); m_aircraft->ccpmType->addItems(Types); m_aircraft->ccpmType->setCurrentIndex(m_aircraft->ccpmType->count() - 1); - //refreshWidgetsValues(QString("HeliCP")); + // refreshWidgetsValues(QString("HeliCP")); UpdateType(); @@ -253,7 +253,7 @@ ConfigCcpmWidget::ConfigCcpmWidget(QWidget *parent) : connect(m_aircraft->ccpmCollectivespinBox, SIGNAL(valueChanged(int)), this, SLOT(UpdateMixer())); connect(m_aircraft->ccpmType, SIGNAL(currentIndexChanged(int)), this, SLOT(UpdateType())); connect(m_aircraft->ccpmSingleServo, SIGNAL(currentIndexChanged(int)), this, SLOT(UpdateType())); - connect(m_aircraft->TabObject, SIGNAL(currentChanged ( QWidget * )), this, SLOT(UpdateType())); + connect(m_aircraft->TabObject, SIGNAL(currentChanged(QWidget *)), this, SLOT(UpdateType())); connect(m_aircraft->SwashLvlStartButton, SIGNAL(clicked()), this, SLOT(SwashLvlStartButtonPressed())); connect(m_aircraft->SwashLvlNextButton, SIGNAL(clicked()), this, SLOT(SwashLvlNextButtonPressed())); @@ -277,7 +277,8 @@ void ConfigCcpmWidget::setupUI(QString frameType) Q_UNUSED(frameType); } -void ConfigCcpmWidget::registerWidgets(ConfigTaskWidget &parent) { +void ConfigCcpmWidget::registerWidgets(ConfigTaskWidget &parent) +{ parent.addWidget(m_aircraft->ccpmType); parent.addWidget(m_aircraft->ccpmTailChannel); parent.addWidget(m_aircraft->ccpmEngineChannel); @@ -316,8 +317,8 @@ void ConfigCcpmWidget::registerWidgets(ConfigTaskWidget &parent) { void ConfigCcpmWidget::resetActuators(GUIConfigDataUnion *configData) { - configData->heli.Throttle = 0; - configData->heli.Tail = 0; + configData->heli.Throttle = 0; + configData->heli.Tail = 0; configData->heli.ServoIndexW = 0; configData->heli.ServoIndexX = 0; configData->heli.ServoIndexY = 0; @@ -358,14 +359,14 @@ void ConfigCcpmWidget::refreshWidgetsValues(QString frameType) // servo assignments setComboCurrentIndex(m_aircraft->ccpmServoWChannel, config.heli.ServoIndexW); - setComboCurrentIndex( m_aircraft->ccpmServoXChannel,config.heli.ServoIndexX); - setComboCurrentIndex( m_aircraft->ccpmServoYChannel,config.heli.ServoIndexY); - setComboCurrentIndex( m_aircraft->ccpmServoZChannel,config.heli.ServoIndexZ); + setComboCurrentIndex(m_aircraft->ccpmServoXChannel, config.heli.ServoIndexX); + setComboCurrentIndex(m_aircraft->ccpmServoYChannel, config.heli.ServoIndexY); + setComboCurrentIndex(m_aircraft->ccpmServoZChannel, config.heli.ServoIndexZ); // throttle - setComboCurrentIndex( m_aircraft->ccpmEngineChannel, config.heli.Throttle); + setComboCurrentIndex(m_aircraft->ccpmEngineChannel, config.heli.Throttle); // tail - setComboCurrentIndex( m_aircraft->ccpmTailChannel, config.heli.Tail); + setComboCurrentIndex(m_aircraft->ccpmTailChannel, config.heli.Tail); getMixer(); } @@ -386,11 +387,11 @@ void ConfigCcpmWidget::UpdateType() SetUIComponentVisibilities(); - TypeInt = m_aircraft->ccpmType->count() - m_aircraft->ccpmType->currentIndex() - 1; + TypeInt = m_aircraft->ccpmType->count() - m_aircraft->ccpmType->currentIndex() - 1; TypeText = m_aircraft->ccpmType->currentText(); SingleServoIndex = m_aircraft->ccpmSingleServo->currentIndex(); - //set visibility of user settings + // set visibility of user settings m_aircraft->ccpmAdvancedSettingsTable->setEnabled(TypeInt == 0); m_aircraft->ccpmAdvancedSettingsTable->clearFocus(); @@ -418,7 +419,7 @@ void ConfigCcpmWidget::UpdateType() m_aircraft->PitchCurve->setVisible(1); NumServosDefined = 4; - //set values for pre defined heli types + // set values for pre defined heli types if (TypeText.compare(QString::fromUtf8("CCPM 2 Servo 90º"), Qt::CaseInsensitive) == 0) { m_aircraft->ccpmAngleW->setValue(AdjustmentAngle + 0); m_aircraft->ccpmAngleX->setValue(fmod(AdjustmentAngle + 90, 360)); @@ -504,7 +505,7 @@ void ConfigCcpmWidget::UpdateType() NumServosDefined = 2; } - //Set the text of the motor boxes + // Set the text of the motor boxes if (TypeText.compare(QString::fromUtf8("Coax 2 Servo 90º"), Qt::CaseInsensitive) == 0) { m_aircraft->ccpmEngineLabel->setText("CW motor"); m_aircraft->ccpmTailLabel->setText("CCW motor"); @@ -513,7 +514,7 @@ void ConfigCcpmWidget::UpdateType() m_aircraft->ccpmTailLabel->setText("Tail rotor"); } - //set the visibility of the swashplate servo selection boxes + // set the visibility of the swashplate servo selection boxes m_aircraft->ccpmServoWLabel->setVisible(NumServosDefined >= 1); m_aircraft->ccpmServoXLabel->setVisible(NumServosDefined >= 2); m_aircraft->ccpmServoYLabel->setVisible(NumServosDefined >= 3); @@ -523,7 +524,7 @@ void ConfigCcpmWidget::UpdateType() m_aircraft->ccpmServoYChannel->setVisible(NumServosDefined >= 3); m_aircraft->ccpmServoZChannel->setVisible(NumServosDefined >= 4); - //set the visibility of the swashplate angle selection boxes + // set the visibility of the swashplate angle selection boxes m_aircraft->ccpmServoWLabel_2->setVisible(NumServosDefined >= 1); m_aircraft->ccpmServoXLabel_2->setVisible(NumServosDefined >= 2); m_aircraft->ccpmServoYLabel_2->setVisible(NumServosDefined >= 3); @@ -539,109 +540,109 @@ void ConfigCcpmWidget::UpdateType() table->setColumnWidth(i, (table->width() - table->verticalHeader()->width()) / 6); } - //update UI + // update UI ccpmSwashplateUpdate(); } void ConfigCcpmWidget::ccpmSwashplateRedraw() { - double angle[CCPM_MAX_SWASH_SERVOS],CorrectionAngle,x,y,w,h,radius,CenterX,CenterY; - int used[CCPM_MAX_SWASH_SERVOS],defined[CCPM_MAX_SWASH_SERVOS],i; + double angle[CCPM_MAX_SWASH_SERVOS], CorrectionAngle, x, y, w, h, radius, CenterX, CenterY; + int used[CCPM_MAX_SWASH_SERVOS], defined[CCPM_MAX_SWASH_SERVOS], i; QRectF bounds; QRect size; - double scale,xscale,yscale; + double scale, xscale, yscale; - size = m_aircraft->SwashplateImage->rect(); - xscale=size.width(); - yscale=size.height(); - scale=xscale; - if (yscaleSwashplateImage->resetTransform (); - m_aircraft->SwashplateImage->scale(scale,scale); - - size = m_aircraft->SwashLvlSwashplateImage->rect(); - xscale=size.width(); - yscale=size.height(); - scale=xscale; - if (yscaleSwashLvlSwashplateImage->resetTransform (); - m_aircraft->SwashLvlSwashplateImage->scale(scale,scale); - - CorrectionAngle=m_aircraft->ccpmCorrectionAngle->value(); + size = m_aircraft->SwashplateImage->rect(); + xscale = size.width(); + yscale = size.height(); + scale = xscale; + if (yscale < scale) { + scale = yscale; + } + scale /= 460.00; + m_aircraft->SwashplateImage->resetTransform(); + m_aircraft->SwashplateImage->scale(scale, scale); - CenterX=200; - CenterY=200; + size = m_aircraft->SwashLvlSwashplateImage->rect(); + xscale = size.width(); + yscale = size.height(); + scale = xscale; + if (yscale < scale) { + scale = yscale; + } + scale /= 590.00; + m_aircraft->SwashLvlSwashplateImage->resetTransform(); + m_aircraft->SwashLvlSwashplateImage->scale(scale, scale); - bounds=SwashplateImg->boundingRect(); - - SwashplateImg->setPos(CenterX-bounds.width()/2,CenterY-bounds.height()/2); + CorrectionAngle = m_aircraft->ccpmCorrectionAngle->value(); - defined[0]=(m_aircraft->ccpmServoWChannel->isEnabled()); - defined[1]=(m_aircraft->ccpmServoXChannel->isEnabled()); - defined[2]=(m_aircraft->ccpmServoYChannel->isEnabled()); - defined[3]=(m_aircraft->ccpmServoZChannel->isEnabled()); - used[0]=((m_aircraft->ccpmServoWChannel->currentIndex()>0)&&(m_aircraft->ccpmServoWChannel->isEnabled())); - used[1]=((m_aircraft->ccpmServoXChannel->currentIndex()>0)&&(m_aircraft->ccpmServoXChannel->isEnabled())); - used[2]=((m_aircraft->ccpmServoYChannel->currentIndex()>0)&&(m_aircraft->ccpmServoYChannel->isEnabled())); - used[3]=((m_aircraft->ccpmServoZChannel->currentIndex()>0)&&(m_aircraft->ccpmServoZChannel->isEnabled())); - angle[0]=(CorrectionAngle+180+m_aircraft->ccpmAngleW->value())*Pi/180.00; - angle[1]=(CorrectionAngle+180+m_aircraft->ccpmAngleX->value())*Pi/180.00; - angle[2]=(CorrectionAngle+180+m_aircraft->ccpmAngleY->value())*Pi/180.00; - angle[3]=(CorrectionAngle+180+m_aircraft->ccpmAngleZ->value())*Pi/180.00; + CenterX = 200; + CenterY = 200; + + bounds = SwashplateImg->boundingRect(); + + SwashplateImg->setPos(CenterX - bounds.width() / 2, CenterY - bounds.height() / 2); + + defined[0] = (m_aircraft->ccpmServoWChannel->isEnabled()); + defined[1] = (m_aircraft->ccpmServoXChannel->isEnabled()); + defined[2] = (m_aircraft->ccpmServoYChannel->isEnabled()); + defined[3] = (m_aircraft->ccpmServoZChannel->isEnabled()); + used[0] = ((m_aircraft->ccpmServoWChannel->currentIndex() > 0) && (m_aircraft->ccpmServoWChannel->isEnabled())); + used[1] = ((m_aircraft->ccpmServoXChannel->currentIndex() > 0) && (m_aircraft->ccpmServoXChannel->isEnabled())); + used[2] = ((m_aircraft->ccpmServoYChannel->currentIndex() > 0) && (m_aircraft->ccpmServoYChannel->isEnabled())); + used[3] = ((m_aircraft->ccpmServoZChannel->currentIndex() > 0) && (m_aircraft->ccpmServoZChannel->isEnabled())); + angle[0] = (CorrectionAngle + 180 + m_aircraft->ccpmAngleW->value()) * Pi / 180.00; + angle[1] = (CorrectionAngle + 180 + m_aircraft->ccpmAngleX->value()) * Pi / 180.00; + angle[2] = (CorrectionAngle + 180 + m_aircraft->ccpmAngleY->value()) * Pi / 180.00; + angle[3] = (CorrectionAngle + 180 + m_aircraft->ccpmAngleZ->value()) * Pi / 180.00; - for (i=0;isetPos(x, y); - Servos[i]->setVisible(used[i]!=0); + Servos[i]->setVisible(used[i] != 0); + + radius = 150; + bounds = ServosText[i]->boundingRect(); + x = CenterX - (radius * sin(angle[i])) - bounds.width() / 2; + y = CenterY + (radius * cos(angle[i])) - bounds.height() / 2; - radius=150; - bounds=ServosText[i]->boundingRect(); - x=CenterX-(radius*sin(angle[i]))-bounds.width()/2; - y=CenterY+(radius*cos(angle[i]))-bounds.height()/2; - ServosText[i]->setPos(x, y); - ServosText[i]->setVisible(used[i]!=0); - - if (bounds.width()>bounds.height()) - { + ServosText[i]->setVisible(used[i] != 0); + + if (bounds.width() > bounds.height()) { bounds.setHeight(bounds.width()); - } - else - { + } else { bounds.setWidth(bounds.height()); } - x=CenterX-(radius*sin(angle[i]))-bounds.width()/2; - y=CenterY+(radius*cos(angle[i]))-bounds.height()/2; - + x = CenterX - (radius * sin(angle[i])) - bounds.width() / 2; + y = CenterY + (radius * cos(angle[i])) - bounds.height() / 2; + ServosTextCircles[i]->setRect(bounds); ServosTextCircles[i]->setPos(x, y); - ServosTextCircles[i]->setVisible(used[i]!=0); + ServosTextCircles[i]->setVisible(used[i] != 0); - w=SwashLvlSpinBoxes[i]->width()/2; - h=SwashLvlSpinBoxes[i]->height()/2; - radius = (215.00+w+h); - x=CenterX-(radius*sin(angle[i]))-w; - y=CenterY+(radius*cos(angle[i]))-h; - SwashLvlSpinBoxes[i]->move(m_aircraft->SwashLvlSwashplateImage->mapFromScene (x, y)); - SwashLvlSpinBoxes[i]->setVisible(used[i]!=0); + w = SwashLvlSpinBoxes[i]->width() / 2; + h = SwashLvlSpinBoxes[i]->height() / 2; + radius = (215.00 + w + h); + x = CenterX - (radius * sin(angle[i])) - w; + y = CenterY + (radius * cos(angle[i])) - h; + SwashLvlSpinBoxes[i]->move(m_aircraft->SwashLvlSwashplateImage->mapFromScene(x, y)); + SwashLvlSpinBoxes[i]->setVisible(used[i] != 0); - radius=220; - x=CenterX-(radius*sin(angle[i])); - y=CenterY+(radius*cos(angle[i])); - ServoLines[i]->setLine(CenterX,CenterY,x,y); - ServoLines[i]->setVisible(defined[i]!=0); + radius = 220; + x = CenterX - (radius * sin(angle[i])); + y = CenterY + (radius * cos(angle[i])); + ServoLines[i]->setLine(CenterX, CenterY, x, y); + ServoLines[i]->setVisible(defined[i] != 0); } - //m_aircraft->SwashplateImage->centerOn (CenterX, CenterY); + // m_aircraft->SwashplateImage->centerOn (CenterX, CenterY); - //m_aircraft->SwashplateImage->fitInView(SwashplateImg, Qt::KeepAspectRatio); + // m_aircraft->SwashplateImage->fitInView(SwashplateImg, Qt::KeepAspectRatio); } void ConfigCcpmWidget::ccpmSwashplateUpdate() @@ -660,32 +661,33 @@ void ConfigCcpmWidget::UpdateMixer() float ThisAngle[6]; QString Channel; - if (throwConfigError(QString("HeliCP"))) + if (throwConfigError(QString("HeliCP"))) { return; + } GUIConfigDataUnion config = getConfigData(); - useCCPM = !(config.heli.ccpmCollectivePassthroughState || !config.heli.ccpmLinkCyclicState); + useCCPM = !(config.heli.ccpmCollectivePassthroughState || !config.heli.ccpmLinkCyclicState); useCyclic = config.heli.ccpmLinkRollState; - CollectiveConstant = (float) config.heli.SliderValue0 / 100.00; + CollectiveConstant = (float)config.heli.SliderValue0 / 100.00; - if (useCCPM) { //cyclic = 1 - collective + if (useCCPM) { // cyclic = 1 - collective PitchConstant = 1 - CollectiveConstant; - RollConstant = PitchConstant; + RollConstant = PitchConstant; } else { - PitchConstant = (float) config.heli.SliderValue1 / 100.00; + PitchConstant = (float)config.heli.SliderValue1 / 100.00; ; if (useCyclic) { RollConstant = PitchConstant; } else { - RollConstant = (float) config.heli.SliderValue2 / 100.00; + RollConstant = (float)config.heli.SliderValue2 / 100.00; ; } } - if (config.heli.SwashplateType > 0) { //not advanced settings - //get the channel data from the ui + if (config.heli.SwashplateType > 0) { // not advanced settings + // get the channel data from the ui MixerChannelData[0] = m_aircraft->ccpmEngineChannel->currentIndex(); MixerChannelData[1] = m_aircraft->ccpmTailChannel->currentIndex(); MixerChannelData[2] = m_aircraft->ccpmServoWChannel->currentIndex(); @@ -693,13 +695,13 @@ void ConfigCcpmWidget::UpdateMixer() MixerChannelData[4] = m_aircraft->ccpmServoYChannel->currentIndex(); MixerChannelData[5] = m_aircraft->ccpmServoZChannel->currentIndex(); - //get the angle data from the ui - ThisAngle[2] = m_aircraft->ccpmAngleW->value(); - ThisAngle[3] = m_aircraft->ccpmAngleX->value(); - ThisAngle[4] = m_aircraft->ccpmAngleY->value(); - ThisAngle[5] = m_aircraft->ccpmAngleZ->value(); + // get the angle data from the ui + ThisAngle[2] = m_aircraft->ccpmAngleW->value(); + ThisAngle[3] = m_aircraft->ccpmAngleX->value(); + ThisAngle[4] = m_aircraft->ccpmAngleY->value(); + ThisAngle[5] = m_aircraft->ccpmAngleZ->value(); - //get the angle data from the ui + // get the angle data from the ui ThisEnable[2] = m_aircraft->ccpmServoWChannel->isEnabled(); ThisEnable[3] = m_aircraft->ccpmServoXChannel->isEnabled(); ThisEnable[4] = m_aircraft->ccpmServoYChannel->isEnabled(); @@ -710,18 +712,18 @@ void ConfigCcpmWidget::UpdateMixer() ServosText[2]->setPlainText(QString("%1").arg(MixerChannelData[4])); ServosText[3]->setPlainText(QString("%1").arg(MixerChannelData[5])); - //go through the user data and update the mixer matrix + // go through the user data and update the mixer matrix QTableWidget *table = m_aircraft->ccpmAdvancedSettingsTable; for (int i = 0; i < 6; i++) { if ((MixerChannelData[i] > 0) && ((ThisEnable[i]) || (i < 2))) { table->item(i, 0)->setText(QString("%1").arg(MixerChannelData[i])); - //Generate the mixer vector - if (i == 0) { //main motor-engine - table->item(i, 1)->setText(QString("%1").arg(127)); //ThrottleCurve1 - table->item(i, 2)->setText(QString("%1").arg(0)); //ThrottleCurve2 - table->item(i, 3)->setText(QString("%1").arg(0)); //Roll - table->item(i, 4)->setText(QString("%1").arg(0)); //Pitch + // Generate the mixer vector + if (i == 0) { // main motor-engine + table->item(i, 1)->setText(QString("%1").arg(127)); // ThrottleCurve1 + table->item(i, 2)->setText(QString("%1").arg(0)); // ThrottleCurve2 + table->item(i, 3)->setText(QString("%1").arg(0)); // Roll + table->item(i, 4)->setText(QString("%1").arg(0)); // Pitch if (TypeText.compare(QString::fromUtf8("Coax 2 Servo 90º"), Qt::CaseInsensitive) == 0) { // Yaw @@ -730,7 +732,6 @@ void ConfigCcpmWidget::UpdateMixer() // Yaw table->item(i, 5)->setText(QString("%1").arg(0)); } - } if (i == 1) { // tailrotor --or-- counter-clockwise motor @@ -745,38 +746,35 @@ void ConfigCcpmWidget::UpdateMixer() // Yaw table->item(i, 5)->setText(QString("%1").arg(127)); } - //ThrottleCurve2 + // ThrottleCurve2 table->item(i, 2)->setText(QString("%1").arg(0)); // Roll table->item(i, 3)->setText(QString("%1").arg(0)); // Pitch table->item(i, 4)->setText(QString("%1").arg(0)); - } if (i > 1) { // Swashplate - //ThrottleCurve1 + // ThrottleCurve1 table->item(i, 1)->setText(QString("%1").arg(0)); - //ThrottleCurve2 - table->item(i, 2)->setText(QString("%1").arg((int) (127.0 * CollectiveConstant))); + // ThrottleCurve2 + table->item(i, 2)->setText(QString("%1").arg((int)(127.0 * CollectiveConstant))); table->item(i, 3)->setText( - QString("%1").arg( - (int) (127.0 * (RollConstant) - * sin((180 + config.heli.CorrectionAngle + ThisAngle[i]) * Pi / 180.00)))); //Roll + QString("%1").arg( + (int)(127.0 * (RollConstant) + * sin((180 + config.heli.CorrectionAngle + ThisAngle[i]) * Pi / 180.00)))); // Roll table->item(i, 4)->setText( - QString("%1").arg( - (int) (127.0 * (PitchConstant) - * cos((config.heli.CorrectionAngle + ThisAngle[i]) * Pi / 180.00)))); //Pitch + QString("%1").arg( + (int)(127.0 * (PitchConstant) + * cos((config.heli.CorrectionAngle + ThisAngle[i]) * Pi / 180.00)))); // Pitch // Yaw table->item(i, 5)->setText(QString("%1").arg(0)); - } } else { for (int j = 0; j < 6; j++) { table->item(i, j)->setText(QString("-")); } } - } } else { // advanced settings @@ -798,29 +796,30 @@ QString ConfigCcpmWidget::updateConfigObjects() bool useCCPM; bool useCyclic; - if (updatingFromHardware == TRUE) + if (updatingFromHardware == TRUE) { return airframeType; + } updatingFromHardware = TRUE; - //get the user options + // get the user options GUIConfigDataUnion config = getConfigData(); - //swashplate config - config.heli.SwashplateType = m_aircraft->ccpmType->count() - m_aircraft->ccpmType->currentIndex() - 1; + // swashplate config + config.heli.SwashplateType = m_aircraft->ccpmType->count() - m_aircraft->ccpmType->currentIndex() - 1; config.heli.FirstServoIndex = m_aircraft->ccpmSingleServo->currentIndex(); - //ccpm mixing options + // ccpm mixing options config.heli.ccpmCollectivePassthroughState = m_aircraft->ccpmCollectivePassthrough->isChecked(); config.heli.ccpmLinkCyclicState = m_aircraft->ccpmLinkCyclic->isChecked(); - config.heli.ccpmLinkRollState = m_aircraft->ccpmLinkRoll->isChecked(); - useCCPM = !(config.heli.ccpmCollectivePassthroughState || !config.heli.ccpmLinkCyclicState); + config.heli.ccpmLinkRollState = m_aircraft->ccpmLinkRoll->isChecked(); + useCCPM = !(config.heli.ccpmCollectivePassthroughState || !config.heli.ccpmLinkCyclicState); useCyclic = config.heli.ccpmLinkRollState; - //correction angle + // correction angle config.heli.CorrectionAngle = m_aircraft->ccpmCorrectionAngle->value(); - //update sliders + // update sliders if (useCCPM) { config.heli.SliderValue0 = m_aircraft->ccpmCollectiveSlider->value(); } else { @@ -833,15 +832,15 @@ QString ConfigCcpmWidget::updateConfigObjects() } config.heli.SliderValue2 = m_aircraft->ccpmRollScale->value(); - //servo assignments - config.heli.ServoIndexW = m_aircraft->ccpmServoWChannel->currentIndex(); - config.heli.ServoIndexX = m_aircraft->ccpmServoXChannel->currentIndex(); - config.heli.ServoIndexY = m_aircraft->ccpmServoYChannel->currentIndex(); - config.heli.ServoIndexZ = m_aircraft->ccpmServoZChannel->currentIndex(); + // servo assignments + config.heli.ServoIndexW = m_aircraft->ccpmServoWChannel->currentIndex(); + config.heli.ServoIndexX = m_aircraft->ccpmServoXChannel->currentIndex(); + config.heli.ServoIndexY = m_aircraft->ccpmServoYChannel->currentIndex(); + config.heli.ServoIndexZ = m_aircraft->ccpmServoZChannel->currentIndex(); - //throttle - config.heli.Throttle = m_aircraft->ccpmEngineChannel->currentIndex(); - //tail + // throttle + config.heli.Throttle = m_aircraft->ccpmEngineChannel->currentIndex(); + // tail config.heli.Tail = m_aircraft->ccpmTailChannel->currentIndex(); setConfigData(config); @@ -855,22 +854,21 @@ void ConfigCcpmWidget::SetUIComponentVisibilities() m_aircraft->ccpmRevoMixingBox->setVisible(0); m_aircraft->ccpmPitchMixingBox->setVisible( - !m_aircraft->ccpmCollectivePassthrough->isChecked() && m_aircraft->ccpmLinkCyclic->isChecked()); + !m_aircraft->ccpmCollectivePassthrough->isChecked() && m_aircraft->ccpmLinkCyclic->isChecked()); m_aircraft->ccpmCollectiveScalingBox->setVisible( - m_aircraft->ccpmCollectivePassthrough->isChecked() || !m_aircraft->ccpmLinkCyclic->isChecked()); + m_aircraft->ccpmCollectivePassthrough->isChecked() || !m_aircraft->ccpmLinkCyclic->isChecked()); m_aircraft->ccpmLinkCyclic->setVisible(!m_aircraft->ccpmCollectivePassthrough->isChecked()); m_aircraft->ccpmCyclicScalingBox->setVisible( - (m_aircraft->ccpmCollectivePassthrough->isChecked() || !m_aircraft->ccpmLinkCyclic->isChecked()) - && m_aircraft->ccpmLinkRoll->isChecked()); + (m_aircraft->ccpmCollectivePassthrough->isChecked() || !m_aircraft->ccpmLinkCyclic->isChecked()) + && m_aircraft->ccpmLinkRoll->isChecked()); if (!m_aircraft->ccpmCollectivePassthrough->checkState() && m_aircraft->ccpmLinkCyclic->isChecked()) { m_aircraft->ccpmPitchScalingBox->setVisible(0); m_aircraft->ccpmRollScalingBox->setVisible(0); m_aircraft->ccpmLinkRoll->setVisible(0); - } else { m_aircraft->ccpmPitchScalingBox->setVisible(!m_aircraft->ccpmLinkRoll->isChecked()); m_aircraft->ccpmRollScalingBox->setVisible(!m_aircraft->ccpmLinkRoll->isChecked()); @@ -879,16 +877,20 @@ void ConfigCcpmWidget::SetUIComponentVisibilities() } /** - Request the current value of the SystemSettings which holds the ccpm type - */ + Request the current value of the SystemSettings which holds the ccpm type + */ void ConfigCcpmWidget::getMixer() { - if (SwashLvlConfigurationInProgress)return; - if (updatingToHardware)return; + if (SwashLvlConfigurationInProgress) { + return; + } + if (updatingToHardware) { + return; + } updatingFromHardware = TRUE; - - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); QList curveValues; @@ -897,8 +899,7 @@ void ConfigCcpmWidget::getMixer() // is at least one of the curve values != 0? if (isValidThrottleCurve(&curveValues)) { m_aircraft->ThrottleCurve->setCurve(&curveValues); - } - else { + } else { m_aircraft->ThrottleCurve->ResetCurve(); } @@ -906,46 +907,48 @@ void ConfigCcpmWidget::getMixer() // is at least one of the curve values != 0? if (isValidThrottleCurve(&curveValues)) { m_aircraft->PitchCurve->setCurve(&curveValues); - } - else { + } else { m_aircraft->PitchCurve->ResetCurve(); } - updatingFromHardware=FALSE; + updatingFromHardware = FALSE; ccpmSwashplateUpdate(); } /** - Sends the config to the board (ccpm type) - */ + Sends the config to the board (ccpm type) + */ void ConfigCcpmWidget::setMixer() { - int i,j; + int i, j; - if (SwashLvlConfigurationInProgress)return; - if (updatingToHardware == TRUE) return; + if (SwashLvlConfigurationInProgress) { + return; + } + if (updatingToHardware == TRUE) { + return; + } - updatingToHardware=TRUE; + updatingToHardware = TRUE; - MixerSettings * mixerSettings = MixerSettings::GetInstance(getObjectManager()); + MixerSettings *mixerSettings = MixerSettings::GetInstance(getObjectManager()); Q_ASSERT(mixerSettings); MixerSettings::DataFields mixerSettingsData = mixerSettings->getData(); UpdateMixer(); // Set up some helper pointers - qint8 * mixers[8] = {mixerSettingsData.Mixer1Vector, + qint8 *mixers[8] = { mixerSettingsData.Mixer1Vector, mixerSettingsData.Mixer2Vector, mixerSettingsData.Mixer3Vector, mixerSettingsData.Mixer4Vector, mixerSettingsData.Mixer5Vector, mixerSettingsData.Mixer6Vector, mixerSettingsData.Mixer7Vector, - mixerSettingsData.Mixer8Vector - }; + mixerSettingsData.Mixer8Vector }; - quint8 * mixerTypes[8] = { + quint8 *mixerTypes[8] = { &mixerSettingsData.Mixer1Type, &mixerSettingsData.Mixer2Type, &mixerSettingsData.Mixer3Type, @@ -956,90 +959,89 @@ void ConfigCcpmWidget::setMixer() &mixerSettingsData.Mixer8Type }; - //reset all to Disabled - for (i=0; i<8; i++) + // reset all to Disabled + for (i = 0; i < 8; i++) { *mixerTypes[i] = 0; + } - //go through the user data and update the mixer matrix - for (i=0;i<6;i++) - { - if (MixerChannelData[i]>0) - { - //Set the mixer type. If Coax, then first two are motors. Otherwise, only first is motor - if (TypeText.compare(QString::fromUtf8("Coax 2 Servo 90º"), Qt::CaseInsensitive)==0) - { + // go through the user data and update the mixer matrix + for (i = 0; i < 6; i++) { + if (MixerChannelData[i] > 0) { + // Set the mixer type. If Coax, then first two are motors. Otherwise, only first is motor + if (TypeText.compare(QString::fromUtf8("Coax 2 Servo 90º"), Qt::CaseInsensitive) == 0) { *(mixerTypes[MixerChannelData[i] - 1]) = i > 1 ? - MixerSettings::MIXER1TYPE_SERVO : - MixerSettings::MIXER1TYPE_MOTOR; - } - else{ + MixerSettings::MIXER1TYPE_SERVO : + MixerSettings::MIXER1TYPE_MOTOR; + } else { *(mixerTypes[MixerChannelData[i] - 1]) = i > 0 ? - MixerSettings::MIXER1TYPE_SERVO : - MixerSettings::MIXER1TYPE_MOTOR; + MixerSettings::MIXER1TYPE_SERVO : + MixerSettings::MIXER1TYPE_MOTOR; } - //Configure the vector - for (j=0;j<5;j++) - mixers[MixerChannelData[i] - 1][j] = m_aircraft->ccpmAdvancedSettingsTable->item(i,j+1)->text().toInt(); + // Configure the vector + for (j = 0; j < 5; j++) { + mixers[MixerChannelData[i] - 1][j] = m_aircraft->ccpmAdvancedSettingsTable->item(i, j + 1)->text().toInt(); + } } } - //get the user data for the curve into the mixer settings + // get the user data for the curve into the mixer settings QList curve1 = m_aircraft->ThrottleCurve->getCurve(); QList curve2 = m_aircraft->PitchCurve->getCurve(); - for (i=0;i<5;i++) { + for (i = 0; i < 5; i++) { mixerSettingsData.ThrottleCurve1[i] = curve1.at(i); mixerSettingsData.ThrottleCurve2[i] = curve2.at(i); } - - //mapping of collective input to curve 2... - //MixerSettings.Curve2Source = Throttle,Roll,Pitch,Yaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5 - //check if we are using throttle or directly from a channel... - if (m_aircraft->ccpmCollectivePassthrough->isChecked()) + + // mapping of collective input to curve 2... + // MixerSettings.Curve2Source = Throttle,Roll,Pitch,Yaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5 + // check if we are using throttle or directly from a channel... + if (m_aircraft->ccpmCollectivePassthrough->isChecked()) { mixerSettingsData.Curve2Source = MixerSettings::CURVE2SOURCE_COLLECTIVE; - else + } else { mixerSettingsData.Curve2Source = MixerSettings::CURVE2SOURCE_THROTTLE; - + } + mixerSettings->setData(mixerSettingsData); mixerSettings->updated(); - updatingToHardware=FALSE; - + updatingToHardware = FALSE; } /** - Send ccpm type to the board and request saving to SD card - */ + Send ccpm type to the board and request saving to SD card + */ void ConfigCcpmWidget::saveccpmUpdate() { - if (SwashLvlConfigurationInProgress)return; + if (SwashLvlConfigurationInProgress) { + return; + } ShowDisclaimer(0); // Send update so that the latest value is saved - //sendccpmUpdate(); + // sendccpmUpdate(); setMixer(); - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(obj); saveObjectToSD(obj); } -void ConfigCcpmWidget::resizeEvent(QResizeEvent* event) +void ConfigCcpmWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); // Make the custom table columns autostretch: m_aircraft->ccpmAdvancedSettingsTable->resizeColumnsToContents(); - for (int i=0;i<6;i++) { - m_aircraft->ccpmAdvancedSettingsTable->setColumnWidth(i,(m_aircraft->ccpmAdvancedSettingsTable->width()- - m_aircraft->ccpmAdvancedSettingsTable->verticalHeader()->width())/6); + for (int i = 0; i < 6; i++) { + m_aircraft->ccpmAdvancedSettingsTable->setColumnWidth(i, (m_aircraft->ccpmAdvancedSettingsTable->width() - + m_aircraft->ccpmAdvancedSettingsTable->verticalHeader()->width()) / 6); } ccpmSwashplateRedraw(); - } void ConfigCcpmWidget::showEvent(QShowEvent *event) { Q_UNUSED(event) m_aircraft->ccpmAdvancedSettingsTable->resizeColumnsToContents(); - for (int i=0;i<6;i++) { - m_aircraft->ccpmAdvancedSettingsTable->setColumnWidth(i,(m_aircraft->ccpmAdvancedSettingsTable->width()- - m_aircraft->ccpmAdvancedSettingsTable->verticalHeader()->width())/6); + for (int i = 0; i < 6; i++) { + m_aircraft->ccpmAdvancedSettingsTable->setColumnWidth(i, (m_aircraft->ccpmAdvancedSettingsTable->width() - + m_aircraft->ccpmAdvancedSettingsTable->verticalHeader()->width()) / 6); } ccpmSwashplateRedraw(); } @@ -1049,181 +1051,181 @@ void ConfigCcpmWidget::SwashLvlStartButtonPressed() { QMessageBox msgBox; int i; - msgBox.setText("

Swashplate Leveling Routine

"); - msgBox.setInformativeText("You are about to start the Swashplate levelling routine.

This process will start by downloading the current configuration from the GCS to the OP hardware and will adjust your configuration at various stages.

The final state of your system should match the current configuration in the GCS config gadget.

Please ensure all ccpm settings in the GCS are correct before continuing.

If this process is interrupted, then the state of your OP board may not match the GCS configuration.

After completing this process, please check all settings before attempting to fly.

Please disconnect your motor to ensure it will not spin up.


Do you wish to proceed?"); - msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); - msgBox.setDefaultButton(QMessageBox::Cancel); - msgBox.setIcon(QMessageBox::Information); - int ret = msgBox.exec(); - UAVObjectField* MinField; - UAVObjectField* NeutralField; - UAVObjectField* MaxField; - UAVDataObject* obj; - ExtensionSystem::PluginManager *pm; - UAVObjectManager *objManager; + msgBox.setText("

Swashplate Leveling Routine

"); + msgBox.setInformativeText("You are about to start the Swashplate levelling routine.

This process will start by downloading the current configuration from the GCS to the OP hardware and will adjust your configuration at various stages.

The final state of your system should match the current configuration in the GCS config gadget.

Please ensure all ccpm settings in the GCS are correct before continuing.

If this process is interrupted, then the state of your OP board may not match the GCS configuration.

After completing this process, please check all settings before attempting to fly.

Please disconnect your motor to ensure it will not spin up.


Do you wish to proceed?"); + msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); + msgBox.setDefaultButton(QMessageBox::Cancel); + msgBox.setIcon(QMessageBox::Information); + int ret = msgBox.exec(); - switch (ret) { - case QMessageBox::Yes: - // Yes was clicked - SwashLvlState=0; - //remove Flight control of ActuatorCommand - enableSwashplateLevellingControl(true); + UAVObjectField *MinField; + UAVObjectField *NeutralField; + UAVObjectField *MaxField; + UAVDataObject *obj; + ExtensionSystem::PluginManager *pm; + UAVObjectManager *objManager; - m_aircraft->SwashLvlStartButton->setEnabled(false); - m_aircraft->SwashLvlNextButton->setEnabled(true); - m_aircraft->SwashLvlCancelButton->setEnabled(true); - m_aircraft->SwashLvlFinishButton->setEnabled(false); - //clear status check boxes - m_aircraft->SwashLvlStepList->item(0)->setCheckState(Qt::Unchecked); - m_aircraft->SwashLvlStepList->item(1)->setCheckState(Qt::Unchecked); - m_aircraft->SwashLvlStepList->item(2)->setCheckState(Qt::Unchecked); - m_aircraft->SwashLvlStepList->item(3)->setCheckState(Qt::Unchecked); + switch (ret) { + case QMessageBox::Yes: + // Yes was clicked + SwashLvlState = 0; + // remove Flight control of ActuatorCommand + enableSwashplateLevellingControl(true); + + m_aircraft->SwashLvlStartButton->setEnabled(false); + m_aircraft->SwashLvlNextButton->setEnabled(true); + m_aircraft->SwashLvlCancelButton->setEnabled(true); + m_aircraft->SwashLvlFinishButton->setEnabled(false); + // clear status check boxes + m_aircraft->SwashLvlStepList->item(0)->setCheckState(Qt::Unchecked); + m_aircraft->SwashLvlStepList->item(1)->setCheckState(Qt::Unchecked); + m_aircraft->SwashLvlStepList->item(2)->setCheckState(Qt::Unchecked); + m_aircraft->SwashLvlStepList->item(3)->setCheckState(Qt::Unchecked); - //download the current settings to the OP hw - //sendccpmUpdate(); - setMixer(); + // download the current settings to the OP hw + // sendccpmUpdate(); + setMixer(); - //change control mode to gcs control / disarmed - //set throttle to 0 + // change control mode to gcs control / disarmed + // set throttle to 0 - //save off the old ActuatorSettings for the swashplate servos - pm = ExtensionSystem::PluginManager::instance(); - objManager = pm->getObject(); + // save off the old ActuatorSettings for the swashplate servos + pm = ExtensionSystem::PluginManager::instance(); + objManager = pm->getObject(); - // Get the channel assignements: - obj = dynamic_cast(objManager->getObject(QString("ActuatorSettings"))); - Q_ASSERT(obj); - // obj->requestUpdate(); - MinField = obj->getField(QString("ChannelMin")); - NeutralField = obj->getField(QString("ChannelNeutral")); - MaxField = obj->getField(QString("ChannelMax")); + // Get the channel assignements: + obj = dynamic_cast(objManager->getObject(QString("ActuatorSettings"))); + Q_ASSERT(obj); + // obj->requestUpdate(); + MinField = obj->getField(QString("ChannelMin")); + NeutralField = obj->getField(QString("ChannelNeutral")); + MaxField = obj->getField(QString("ChannelMax")); - //channel assignments - oldSwashLvlConfiguration.ServoChannels[0]=m_aircraft->ccpmServoWChannel->currentIndex(); - oldSwashLvlConfiguration.ServoChannels[1]=m_aircraft->ccpmServoXChannel->currentIndex(); - oldSwashLvlConfiguration.ServoChannels[2]=m_aircraft->ccpmServoYChannel->currentIndex(); - oldSwashLvlConfiguration.ServoChannels[3]=m_aircraft->ccpmServoZChannel->currentIndex(); - //if servos are used - oldSwashLvlConfiguration.Used[0]=((m_aircraft->ccpmServoWChannel->currentIndex()>0)&&(m_aircraft->ccpmServoWChannel->isEnabled())); - oldSwashLvlConfiguration.Used[1]=((m_aircraft->ccpmServoXChannel->currentIndex()>0)&&(m_aircraft->ccpmServoXChannel->isEnabled())); - oldSwashLvlConfiguration.Used[2]=((m_aircraft->ccpmServoYChannel->currentIndex()>0)&&(m_aircraft->ccpmServoYChannel->isEnabled())); - oldSwashLvlConfiguration.Used[3]=((m_aircraft->ccpmServoZChannel->currentIndex()>0)&&(m_aircraft->ccpmServoZChannel->isEnabled())); - //min,neutral,max values for the servos - for (i=0;igetValue(oldSwashLvlConfiguration.ServoChannels[i]).toInt(); - oldSwashLvlConfiguration.Neutral[i]=NeutralField->getValue(oldSwashLvlConfiguration.ServoChannels[i]).toInt(); - oldSwashLvlConfiguration.Max[i]=MaxField->getValue(oldSwashLvlConfiguration.ServoChannels[i]).toInt(); - } + // channel assignments + oldSwashLvlConfiguration.ServoChannels[0] = m_aircraft->ccpmServoWChannel->currentIndex(); + oldSwashLvlConfiguration.ServoChannels[1] = m_aircraft->ccpmServoXChannel->currentIndex(); + oldSwashLvlConfiguration.ServoChannels[2] = m_aircraft->ccpmServoYChannel->currentIndex(); + oldSwashLvlConfiguration.ServoChannels[3] = m_aircraft->ccpmServoZChannel->currentIndex(); + // if servos are used + oldSwashLvlConfiguration.Used[0] = ((m_aircraft->ccpmServoWChannel->currentIndex() > 0) && (m_aircraft->ccpmServoWChannel->isEnabled())); + oldSwashLvlConfiguration.Used[1] = ((m_aircraft->ccpmServoXChannel->currentIndex() > 0) && (m_aircraft->ccpmServoXChannel->isEnabled())); + oldSwashLvlConfiguration.Used[2] = ((m_aircraft->ccpmServoYChannel->currentIndex() > 0) && (m_aircraft->ccpmServoYChannel->isEnabled())); + oldSwashLvlConfiguration.Used[3] = ((m_aircraft->ccpmServoZChannel->currentIndex() > 0) && (m_aircraft->ccpmServoZChannel->isEnabled())); + // min,neutral,max values for the servos + for (i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { + oldSwashLvlConfiguration.Min[i] = MinField->getValue(oldSwashLvlConfiguration.ServoChannels[i]).toInt(); + oldSwashLvlConfiguration.Neutral[i] = NeutralField->getValue(oldSwashLvlConfiguration.ServoChannels[i]).toInt(); + oldSwashLvlConfiguration.Max[i] = MaxField->getValue(oldSwashLvlConfiguration.ServoChannels[i]).toInt(); + } - //copy to new Actuator settings. - memcpy((void*)&newSwashLvlConfiguration,(void*)&oldSwashLvlConfiguration,sizeof(SwashplateServoSettingsStruct)); + // copy to new Actuator settings. + memcpy((void *)&newSwashLvlConfiguration, (void *)&oldSwashLvlConfiguration, sizeof(SwashplateServoSettingsStruct)); - //goto the first step - SwashLvlNextButtonPressed(); + // goto the first step + SwashLvlNextButtonPressed(); break; - case QMessageBox::Cancel: - // Cancel was clicked - SwashLvlState=0; - //restore Flight control of ActuatorCommand - enableSwashplateLevellingControl(false); + case QMessageBox::Cancel: + // Cancel was clicked + SwashLvlState = 0; + // restore Flight control of ActuatorCommand + enableSwashplateLevellingControl(false); - m_aircraft->SwashLvlStartButton->setEnabled(true); - m_aircraft->SwashLvlNextButton->setEnabled(false); - m_aircraft->SwashLvlCancelButton->setEnabled(false); - m_aircraft->SwashLvlFinishButton->setEnabled(false); - break; - default: - // should never be reached - break; - } + m_aircraft->SwashLvlStartButton->setEnabled(true); + m_aircraft->SwashLvlNextButton->setEnabled(false); + m_aircraft->SwashLvlCancelButton->setEnabled(false); + m_aircraft->SwashLvlFinishButton->setEnabled(false); + break; + default: + // should never be reached + break; + } } void ConfigCcpmWidget::SwashLvlNextButtonPressed() { - //ShowDisclaimer(2); + // ShowDisclaimer(2); SwashLvlState++; switch (SwashLvlState) { case 0: break; - case 1: //Neutral levelling + case 1: // Neutral levelling m_aircraft->SwashLvlStepList->setCurrentRow(0); - //set spin boxes and swashplate servos to Neutral values + // set spin boxes and swashplate servos to Neutral values setSwashplateLevel(50); - //disable position slider + // disable position slider m_aircraft->SwashLvlPositionSlider->setEnabled(false); m_aircraft->SwashLvlPositionSpinBox->setEnabled(false); - //set position slider to 50% + // set position slider to 50% m_aircraft->SwashLvlPositionSlider->setValue(50); m_aircraft->SwashLvlPositionSpinBox->setValue(50); - //connect spinbox signals to slots and ebnable them + // connect spinbox signals to slots and ebnable them for (int i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { connect(SwashLvlSpinBoxes[i], SIGNAL(valueChanged(int)), this, SLOT(SwashLvlSpinBoxChanged(int))); SwashLvlSpinBoxes[i]->setEnabled(true); } - //issue user instructions + // issue user instructions m_aircraft->SwashLvlStepInstruction->setHtml( - "

Neutral levelling

Using adjustment of:

  • servo horns
  • link lengths and
  • Neutral timing spinboxes to the right

ensure that the swashplate is in the center of desired travel range and is level."); + "

Neutral levelling

Using adjustment of:

  • servo horns
  • link lengths and
  • Neutral timing spinboxes to the right

ensure that the swashplate is in the center of desired travel range and is level."); break; - case 2: //Max levelling - //check Neutral status as complete + case 2: // Max levelling + // check Neutral status as complete m_aircraft->SwashLvlStepList->item(0)->setCheckState(Qt::Checked); m_aircraft->SwashLvlStepList->setCurrentRow(1); - //set spin boxes and swashplate servos to Max values + // set spin boxes and swashplate servos to Max values setSwashplateLevel(100); - //set position slider to 100% + // set position slider to 100% m_aircraft->SwashLvlPositionSlider->setValue(100); m_aircraft->SwashLvlPositionSpinBox->setValue(100); - //issue user instructions + // issue user instructions m_aircraft->SwashLvlStepInstruction->setText( - "

Max levelling

Using adjustment of:

  • Max timing spinboxes to the right ONLY

ensure that the swashplate is at the top of desired travel range and is level."); + "

Max levelling

Using adjustment of:

  • Max timing spinboxes to the right ONLY

ensure that the swashplate is at the top of desired travel range and is level."); break; - case 3: //Min levelling - //check Max status as complete + case 3: // Min levelling + // check Max status as complete m_aircraft->SwashLvlStepList->item(1)->setCheckState(Qt::Checked); m_aircraft->SwashLvlStepList->setCurrentRow(2); - //set spin boxes and swashplate servos to Min values + // set spin boxes and swashplate servos to Min values setSwashplateLevel(0); - //set position slider to 0% + // set position slider to 0% m_aircraft->SwashLvlPositionSlider->setValue(0); m_aircraft->SwashLvlPositionSpinBox->setValue(0); - //issue user instructions + // issue user instructions m_aircraft->SwashLvlStepInstruction->setText( - "

Min levelling

Using adjustment of:

  • Min timing spinboxes to the right ONLY

ensure that the swashplate is at the bottom of desired travel range and is level."); + "

Min levelling

Using adjustment of:

  • Min timing spinboxes to the right ONLY

ensure that the swashplate is at the bottom of desired travel range and is level."); break; - case 4: //levelling verification - //check Min status as complete + case 4: // levelling verification + // check Min status as complete m_aircraft->SwashLvlStepList->item(2)->setCheckState(Qt::Checked); m_aircraft->SwashLvlStepList->setCurrentRow(3); - //enable position slider + // enable position slider m_aircraft->SwashLvlPositionSlider->setEnabled(true); m_aircraft->SwashLvlPositionSpinBox->setEnabled(true); - //make heli respond to slider movement + // make heli respond to slider movement connect(m_aircraft->SwashLvlPositionSlider, SIGNAL(valueChanged(int)), this, SLOT(setSwashplateLevel(int))); - //disable spin boxes + // disable spin boxes for (int i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { SwashLvlSpinBoxes[i]->setEnabled(false); } - //issue user instructions + // issue user instructions m_aircraft->SwashLvlStepInstruction->setText( - "

levelling verification

Adjust the slider to the right over it's full range and observe the swashplate motion. It should remain level over the entire range of travel."); + "

levelling verification

Adjust the slider to the right over it's full range and observe the swashplate motion. It should remain level over the entire range of travel."); break; - case 5: //levelling complete - //check verify status as complete + case 5: // levelling complete + // check verify status as complete m_aircraft->SwashLvlStepList->item(3)->setCheckState(Qt::Checked); - //issue user instructions + // issue user instructions m_aircraft->SwashLvlStepInstruction->setText( - "

levelling complete

Press the Finish button to save these settings to the SD card

Press the cancel button to return to the pre-levelling settings"); - //disable position slider + "

levelling complete

Press the Finish button to save these settings to the SD card

Press the cancel button to return to the pre-levelling settings"); + // disable position slider m_aircraft->SwashLvlPositionSlider->setEnabled(false); m_aircraft->SwashLvlPositionSpinBox->setEnabled(false); - //disconnect levelling slots from signals + // disconnect levelling slots from signals disconnect(m_aircraft->SwashLvlPositionSlider, SIGNAL(valueChanged(int)), this, SLOT(setSwashplateLevel(int))); for (int i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { disconnect(SwashLvlSpinBoxes[i], SIGNAL(valueChanged(int)), this, SLOT(SwashLvlSpinBoxChanged(int))); @@ -1235,11 +1237,11 @@ void ConfigCcpmWidget::SwashLvlNextButtonPressed() m_aircraft->SwashLvlFinishButton->setEnabled(true); default: - //restore collective/cyclic setting - //restore pitch curve - //clear spin boxes - //change control mode to gcs control (OFF) / disarmed - //issue user confirmation + // restore collective/cyclic setting + // restore pitch curve + // clear spin boxes + // change control mode to gcs control (OFF) / disarmed + // issue user confirmation break; } } @@ -1262,15 +1264,15 @@ void ConfigCcpmWidget::SwashLvlCancelButtonPressed() m_aircraft->SwashLvlStepList->item(2)->setCheckState(Qt::Unchecked); m_aircraft->SwashLvlStepList->item(3)->setCheckState(Qt::Unchecked); - //restore old Actuator Settings + // restore old Actuator Settings ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast(objManager->getObject(QString("ActuatorSettings"))); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("ActuatorSettings"))); Q_ASSERT(obj); - //update settings to match our changes. - MinField = obj->getField(QString("ChannelMin")); + // update settings to match our changes. + MinField = obj->getField(QString("ChannelMin")); NeutralField = obj->getField(QString("ChannelNeutral")); - MaxField = obj->getField(QString("ChannelMax")); + MaxField = obj->getField(QString("ChannelMax")); // min,neutral,max values for the servos for (int i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { @@ -1285,7 +1287,7 @@ void ConfigCcpmWidget::SwashLvlCancelButtonPressed() enableSwashplateLevellingControl(false); m_aircraft->SwashLvlStepInstruction->setText( - "

Levelling Cancelled

Previous settings have been restored."); + "

Levelling Cancelled

Previous settings have been restored."); } @@ -1303,13 +1305,13 @@ void ConfigCcpmWidget::SwashLvlFinishButtonPressed() // save new Actuator Settings to memory and SD card ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast(objManager->getObject(QString("ActuatorSettings"))); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("ActuatorSettings"))); Q_ASSERT(obj); // update settings to match our changes. - MinField = obj->getField(QString("ChannelMin")); + MinField = obj->getField(QString("ChannelMin")); NeutralField = obj->getField(QString("ChannelNeutral")); - MaxField = obj->getField(QString("ChannelMax")); + MaxField = obj->getField(QString("ChannelMax")); // min,neutral,max values for the servos for (int i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { @@ -1325,32 +1327,34 @@ void ConfigCcpmWidget::SwashLvlFinishButtonPressed() enableSwashplateLevellingControl(false); m_aircraft->SwashLvlStepInstruction->setText( - "

Levelling Completed

New settings have been saved to the SD card"); + "

Levelling Completed

New settings have been saved to the SD card"); ShowDisclaimer(0); - //ShowDisclaimer(2); + // ShowDisclaimer(2); } int ConfigCcpmWidget::ShowDisclaimer(int messageID) { QMessageBox msgBox; + msgBox.setText("

Warning!!!

"); int ret; switch (messageID) { case 0: // Basic disclaimer msgBox.setInformativeText( - "

This code has many configurations.

Please double check all settings before attempting flight!"); + "

This code has many configurations.

Please double check all settings before attempting flight!"); msgBox.setStandardButtons(QMessageBox::Ok); msgBox.setDefaultButton(QMessageBox::Ok); msgBox.setIcon(QMessageBox::Information); ret = msgBox.exec(); return 0; + break; case 1: // Not Tested disclaimer msgBox.setInformativeText( - "

The CCPM mixer code needs more testing!

Use it at your own risk!

Do you wish to continue?"); + "

The CCPM mixer code needs more testing!

Use it at your own risk!

Do you wish to continue?"); msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); msgBox.setDefaultButton(QMessageBox::Cancel); msgBox.setIcon(QMessageBox::Warning); @@ -1358,6 +1362,7 @@ int ConfigCcpmWidget::ShowDisclaimer(int messageID) switch (ret) { case QMessageBox::Cancel: return -1; + case QMessageBox::Yes: return 0; } @@ -1365,12 +1370,13 @@ int ConfigCcpmWidget::ShowDisclaimer(int messageID) case 2: // DO NOT use msgBox.setInformativeText( - "

The CCPM swashplate levelling code is NOT complete!

DO NOT use it for flight!"); + "

The CCPM swashplate levelling code is NOT complete!

DO NOT use it for flight!"); msgBox.setStandardButtons(QMessageBox::Ok); msgBox.setDefaultButton(QMessageBox::Ok); msgBox.setIcon(QMessageBox::Critical); ret = msgBox.exec(); return 0; + break; default: // should never be reached @@ -1381,23 +1387,24 @@ int ConfigCcpmWidget::ShowDisclaimer(int messageID) /** - Toggles the channel testing mode by making the GCS take over - the ActuatorCommand objects - */ + Toggles the channel testing mode by making the GCS take over + the ActuatorCommand objects + */ void ConfigCcpmWidget::enableSwashplateLevellingControl(bool state) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast(objManager->getObject(QString("ActuatorCommand"))); - UAVObject::Metadata mdata = obj->getMetadata(); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("ActuatorCommand"))); + UAVObject::Metadata mdata = obj->getMetadata(); + if (state) { SwashLvlaccInitialData = mdata; UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); UAVObject::SetGcsTelemetryAcked(mdata, false); UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); - mdata.gcsTelemetryUpdatePeriod = 100; + mdata.gcsTelemetryUpdatePeriod = 100; SwashLvlConfigurationInProgress = 1; m_aircraft->TabObject->setTabEnabled(0, 0); m_aircraft->TabObject->setTabEnabled(2, 0); @@ -1412,15 +1419,14 @@ void ConfigCcpmWidget::enableSwashplateLevellingControl(bool state) m_aircraft->TabObject->setTabEnabled(2, 1); m_aircraft->TabObject->setTabEnabled(3, 1); m_aircraft->ccpmType->setEnabled(1); - } obj->setMetadata(mdata); } /** - Sets the swashplate level to a given value based on current settings for Max, Neutral and Min values. - level ranges -1 to +1 - */ + Sets the swashplate level to a given value based on current settings for Max, Neutral and Min values. + level ranges -1 to +1 + */ void ConfigCcpmWidget::setSwashplateLevel(int percent) { if (percent < 0) { @@ -1433,7 +1439,7 @@ void ConfigCcpmWidget::setSwashplateLevel(int percent) return; // -1; } - double level = ((double) percent / 50.00) - 1.00; + double level = ((double)percent / 50.00) - 1.00; SwashLvlServoInterlock = 1; @@ -1504,12 +1510,10 @@ void ConfigCcpmWidget::SwashLvlSpinBoxChanged(int value) actuatorCommand->setData(actuatorCommandData); actuatorCommand->updated(); - - return; } /** - This function displays text and color formatting in order to help the user understand what channels have not yet been configured. + This function displays text and color formatting in order to help the user understand what channels have not yet been configured. */ bool ConfigCcpmWidget::throwConfigError(QString airframeType) { @@ -1556,7 +1560,6 @@ bool ConfigCcpmWidget::throwConfigError(QString airframeType) error = true; } else { m_aircraft->ccpmTailLabel->setText(QTextEdit(m_aircraft->ccpmTailLabel->text()).toPlainText()); - } return error; diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.h index a0e4fa094..921a6c414 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.h @@ -52,8 +52,7 @@ typedef struct { int Min[CCPM_MAX_SWASH_SERVOS]; } SwashplateServoSettingsStruct; -class ConfigCcpmWidget: public VehicleConfig -{ +class ConfigCcpmWidget : public VehicleConfig { Q_OBJECT public: @@ -101,7 +100,10 @@ private: virtual void resetActuators(GUIConfigDataUnion *configData); int ShowDisclaimer(int messageID); - virtual void enableControls(bool enable) { Q_UNUSED(enable) }; // Not used by this widget + virtual void enableControls(bool enable) + { + Q_UNUSED(enable) + }; // Not used by this widget bool updatingFromHardware; bool updatingToHardware; @@ -122,8 +124,8 @@ private slots: void SwashLvlCancelButtonPressed(); void SwashLvlFinishButtonPressed(); - //void UpdateCCPMOptionsFromUI(); - //void UpdateCCPMUIFromOptions(); + // void UpdateCCPMOptionsFromUI(); + // void UpdateCCPMUIFromOptions(); void SetUIComponentVisibilities(); diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.cpp index 192a20ed1..248ea1889 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.cpp @@ -41,31 +41,32 @@ QStringList ConfigCustomWidget::getChannelDescriptions() { QStringList channelDesc; - for (int i = 0; i < (int) VehicleConfig::CHANNEL_NUMELEM; i++) { + + for (int i = 0; i < (int)VehicleConfig::CHANNEL_NUMELEM; i++) { channelDesc.append(QString("-")); } return channelDesc; } ConfigCustomWidget::ConfigCustomWidget(QWidget *parent) : - VehicleConfig(parent), m_aircraft(new Ui_CustomConfigWidget()) + VehicleConfig(parent), m_aircraft(new Ui_CustomConfigWidget()) { m_aircraft->setupUi(this); // Put combo boxes in line one of the custom mixer table: - UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); - UAVObjectField* field = mixer->getField(QString("Mixer1Type")); + UAVObjectField *field = mixer->getField(QString("Mixer1Type")); QStringList list = field->getOptions(); - for (int i = 0; i < (int) VehicleConfig::CHANNEL_NUMELEM; i++) { - QComboBox* qb = new QComboBox(m_aircraft->customMixerTable); + for (int i = 0; i < (int)VehicleConfig::CHANNEL_NUMELEM; i++) { + QComboBox *qb = new QComboBox(m_aircraft->customMixerTable); qb->addItems(list); m_aircraft->customMixerTable->setCellWidget(0, i, qb); } SpinBoxDelegate *sbd = new SpinBoxDelegate(); - for (int i = 1; i < (int) VehicleConfig::CHANNEL_NUMELEM; i++) { + for (int i = 1; i < (int)VehicleConfig::CHANNEL_NUMELEM; i++) { m_aircraft->customMixerTable->setItemDelegateForRow(i, sbd); } } @@ -80,7 +81,8 @@ void ConfigCustomWidget::setupUI(QString frameType) Q_ASSERT(m_aircraft); } -void ConfigCustomWidget::registerWidgets(ConfigTaskWidget &parent) { +void ConfigCustomWidget::registerWidgets(ConfigTaskWidget &parent) +{ parent.addWidget(m_aircraft->customMixerTable); parent.addWidget(m_aircraft->customThrottle1Curve->getCurveWidget()); parent.addWidget(m_aircraft->customThrottle1Curve); @@ -89,11 +91,10 @@ void ConfigCustomWidget::registerWidgets(ConfigTaskWidget &parent) { } void ConfigCustomWidget::resetActuators(GUIConfigDataUnion *configData) -{ -} +{} /** - Helper function to refresh the UI widget values + Helper function to refresh the UI widget values */ void ConfigCustomWidget::refreshWidgetsValues(QString frameType) { @@ -111,17 +112,16 @@ void ConfigCustomWidget::refreshWidgetsValues(QString frameType) if (isValidThrottleCurve(&curveValues)) { // yes, use the curve we just read from mixersettings m_aircraft->customThrottle1Curve->initCurve(&curveValues); - } - else { + } else { // no, init a straight curve m_aircraft->customThrottle1Curve->initLinearCurve(curveValues.count(), 1.0); } - if (MixerSettings *mxr = qobject_cast(mixer)) { + if (MixerSettings * mxr = qobject_cast(mixer)) { MixerSettings::DataFields mixerSettingsData = mxr->getData(); - if (mixerSettingsData.Curve2Source == MixerSettings::CURVE2SOURCE_THROTTLE) + if (mixerSettingsData.Curve2Source == MixerSettings::CURVE2SOURCE_THROTTLE) { m_aircraft->customThrottle2Curve->setMixerType(MixerCurve::MIXERCURVE_THROTTLE); - else { + } else { m_aircraft->customThrottle2Curve->setMixerType(MixerCurve::MIXERCURVE_PITCH); } } @@ -131,49 +131,49 @@ void ConfigCustomWidget::refreshWidgetsValues(QString frameType) if (isValidThrottleCurve(&curveValues)) { m_aircraft->customThrottle2Curve->initCurve(&curveValues); - } - else { + } else { m_aircraft->customThrottle2Curve->initLinearCurve(curveValues.count(), 1.0, m_aircraft->customThrottle2Curve->getMin()); } // Update the mixer table: for (int channel = 0; channel < m_aircraft->customMixerTable->columnCount(); channel++) { - UAVObjectField* field = mixer->getField(mixerTypes.at(channel)); + UAVObjectField *field = mixer->getField(mixerTypes.at(channel)); if (field) { - QComboBox* q = (QComboBox*) m_aircraft->customMixerTable->cellWidget(0, channel); + QComboBox *q = (QComboBox *)m_aircraft->customMixerTable->cellWidget(0, channel); if (q) { QString s = field->getValue().toString(); setComboCurrentIndex(q, q->findText(s)); } m_aircraft->customMixerTable->item(1, channel)->setText( - QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1))); + QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1))); m_aircraft->customMixerTable->item(2, channel)->setText( - QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2))); + QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2))); m_aircraft->customMixerTable->item(3, channel)->setText( - QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL))); + QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL))); m_aircraft->customMixerTable->item(4, channel)->setText( - QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH))); + QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH))); m_aircraft->customMixerTable->item(5, channel)->setText( - QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW))); + QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW))); } } } /** - Helper function to + Helper function to */ QString ConfigCustomWidget::updateConfigObjectsFromWidgets() { UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + Q_ASSERT(mixer); setThrottleCurve(mixer, VehicleConfig::MIXER_THROTTLECURVE1, m_aircraft->customThrottle1Curve->getCurve()); setThrottleCurve(mixer, VehicleConfig::MIXER_THROTTLECURVE2, m_aircraft->customThrottle2Curve->getCurve()); // Update the table: - for (int channel = 0; channel < (int) VehicleConfig::CHANNEL_NUMELEM; channel++) { - QComboBox* q = (QComboBox*) m_aircraft->customMixerTable->cellWidget(0, channel); + for (int channel = 0; channel < (int)VehicleConfig::CHANNEL_NUMELEM; channel++) { + QComboBox *q = (QComboBox *)m_aircraft->customMixerTable->cellWidget(0, channel); if (q->currentText() == "Disabled") { setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_DISABLED); } else if (q->currentText() == "Motor") { @@ -200,73 +200,73 @@ QString ConfigCustomWidget::updateConfigObjectsFromWidgets() setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_ACCESSORY5); } setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, - m_aircraft->customMixerTable->item(1, channel)->text().toDouble()); + m_aircraft->customMixerTable->item(1, channel)->text().toDouble()); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2, - m_aircraft->customMixerTable->item(2, channel)->text().toDouble()); + m_aircraft->customMixerTable->item(2, channel)->text().toDouble()); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, - m_aircraft->customMixerTable->item(3, channel)->text().toDouble()); + m_aircraft->customMixerTable->item(3, channel)->text().toDouble()); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, - m_aircraft->customMixerTable->item(4, channel)->text().toDouble()); + m_aircraft->customMixerTable->item(4, channel)->text().toDouble()); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, - m_aircraft->customMixerTable->item(5, channel)->text().toDouble()); + m_aircraft->customMixerTable->item(5, channel)->text().toDouble()); } return "Custom"; } /** - This function displays text and color formatting in order to help the user understand what channels have not yet been configured. + This function displays text and color formatting in order to help the user understand what channels have not yet been configured. */ bool ConfigCustomWidget::throwConfigError(int numMotors) -{ +{ return false; } /** - WHAT DOES THIS DO??? + WHAT DOES THIS DO??? */ void ConfigCustomWidget::showEvent(QShowEvent *event) { Q_UNUSED(event); // Make the custom table columns autostretch: m_aircraft->customMixerTable->resizeColumnsToContents(); - int channelCount = (int) VehicleConfig::CHANNEL_NUMELEM; + int channelCount = (int)VehicleConfig::CHANNEL_NUMELEM; for (int i = 0; i < channelCount; i++) { m_aircraft->customMixerTable->setColumnWidth(i, - (m_aircraft->customMixerTable->width() - m_aircraft->customMixerTable->verticalHeader()->width()) - / channelCount); + (m_aircraft->customMixerTable->width() - m_aircraft->customMixerTable->verticalHeader()->width()) + / channelCount); } } /** - Resize the GUI contents when the user changes the window size + Resize the GUI contents when the user changes the window size */ void ConfigCustomWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); // Make the custom table columns autostretch: m_aircraft->customMixerTable->resizeColumnsToContents(); - int channelCount = (int) VehicleConfig::CHANNEL_NUMELEM; + int channelCount = (int)VehicleConfig::CHANNEL_NUMELEM; for (int i = 0; i < channelCount; i++) { m_aircraft->customMixerTable->setColumnWidth(i, - (m_aircraft->customMixerTable->width() - m_aircraft->customMixerTable->verticalHeader()->width()) - / channelCount); + (m_aircraft->customMixerTable->width() - m_aircraft->customMixerTable->verticalHeader()->width()) + / channelCount); } } /** - Helper delegate for the custom mixer editor table. - Taken straight from Qt examples, thanks! -*/ + Helper delegate for the custom mixer editor table. + Taken straight from Qt examples, thanks! + */ SpinBoxDelegate::SpinBoxDelegate(QObject *parent) : - QItemDelegate(parent) -{ -} + QItemDelegate(parent) +{} QWidget *SpinBoxDelegate::createEditor(QWidget *parent, const QStyleOptionViewItem &option, - const QModelIndex &index) const + const QModelIndex &index) const { QSpinBox *editor = new QSpinBox(parent); + editor->setMinimum(-127); editor->setMaximum(127); @@ -277,13 +277,15 @@ void SpinBoxDelegate::setEditorData(QWidget *editor, const QModelIndex &index) c { int value = index.model()->data(index, Qt::EditRole).toInt(); - QSpinBox *spinBox = static_cast(editor); + QSpinBox *spinBox = static_cast(editor); + spinBox->setValue(value); } void SpinBoxDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, const QModelIndex &index) const { - QSpinBox *spinBox = static_cast(editor); + QSpinBox *spinBox = static_cast(editor); + spinBox->interpretText(); int value = spinBox->value(); @@ -291,7 +293,7 @@ void SpinBoxDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, c } void SpinBoxDelegate::updateEditorGeometry(QWidget *editor, const QStyleOptionViewItem &option, - const QModelIndex &index) const + const QModelIndex &index) const { editor->setGeometry(option.rect); } diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.h index 1bf7bbf75..3364522d2 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.h @@ -41,8 +41,7 @@ class Ui_Widget; -class ConfigCustomWidget: public VehicleConfig -{ +class ConfigCustomWidget : public VehicleConfig { Q_OBJECT public: @@ -67,11 +66,9 @@ private: private slots: virtual void setupUI(QString airframeType); virtual bool throwConfigError(int numMotors); - }; -class SpinBoxDelegate : public QItemDelegate -{ +class SpinBoxDelegate : public QItemDelegate { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index da2575637..1e2068f93 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -44,7 +44,8 @@ QStringList ConfigFixedWingWidget::getChannelDescriptions() { // init a channel_numelem list of channel desc defaults QStringList channelDesc; - for (int i = 0; i < (int) ConfigFixedWingWidget::CHANNEL_NUMELEM; i++) { + + for (int i = 0; i < (int)ConfigFixedWingWidget::CHANNEL_NUMELEM; i++) { channelDesc.append(QString("-")); } @@ -76,7 +77,7 @@ QStringList ConfigFixedWingWidget::getChannelDescriptions() } ConfigFixedWingWidget::ConfigFixedWingWidget(QWidget *parent) : - VehicleConfig(parent), m_aircraft(new Ui_FixedWingConfigWidget()) + VehicleConfig(parent), m_aircraft(new Ui_FixedWingConfigWidget()) { m_aircraft->setupUi(this); @@ -98,13 +99,13 @@ ConfigFixedWingWidget::~ConfigFixedWingWidget() } /** - Virtual function to setup the UI + Virtual function to setup the UI */ void ConfigFixedWingWidget::setupUI(QString frameType) { Q_ASSERT(m_aircraft); - if (frameType == "FixedWing" || frameType == "Elevator aileron rudder") { + if (frameType == "FixedWing" || frameType == "Elevator aileron rudder") { setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Elevator aileron rudder")); m_aircraft->fwRudder1ChannelBox->setEnabled(true); m_aircraft->fwRudder2ChannelBox->setEnabled(true); @@ -112,7 +113,7 @@ void ConfigFixedWingWidget::setupUI(QString frameType) m_aircraft->fwElevator2ChannelBox->setEnabled(true); m_aircraft->fwAileron1ChannelBox->setEnabled(true); m_aircraft->fwAileron2ChannelBox->setEnabled(true); - + m_aircraft->fwAileron1Label->setText("Aileron 1"); m_aircraft->fwAileron2Label->setText("Aileron 2"); m_aircraft->fwElevator1Label->setText("Elevator 1"); @@ -120,7 +121,6 @@ void ConfigFixedWingWidget::setupUI(QString frameType) m_aircraft->elevonSlider1->setEnabled(false); m_aircraft->elevonSlider2->setEnabled(false); - } else if (frameType == "FixedWingElevon" || frameType == "Elevon") { setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Elevon")); m_aircraft->fwAileron1Label->setText("Elevon 1"); @@ -137,8 +137,7 @@ void ConfigFixedWingWidget::setupUI(QString frameType) m_aircraft->elevonSlider1->setEnabled(true); m_aircraft->elevonSlider2->setEnabled(true); - - } else if (frameType == "FixedWingVtail" || frameType == "Vtail") { + } else if (frameType == "FixedWingVtail" || frameType == "Vtail") { setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Vtail")); m_aircraft->fwRudder1ChannelBox->setEnabled(false); m_aircraft->fwRudder2ChannelBox->setEnabled(false); @@ -159,7 +158,8 @@ void ConfigFixedWingWidget::setupUI(QString frameType) } } -void ConfigFixedWingWidget::registerWidgets(ConfigTaskWidget &parent) { +void ConfigFixedWingWidget::registerWidgets(ConfigTaskWidget &parent) +{ parent.addWidget(m_aircraft->fixedWingThrottle->getCurveWidget()); parent.addWidget(m_aircraft->fixedWingThrottle); parent.addWidget(m_aircraft->fixedWingType); @@ -177,17 +177,17 @@ void ConfigFixedWingWidget::registerWidgets(ConfigTaskWidget &parent) { void ConfigFixedWingWidget::resetActuators(GUIConfigDataUnion *configData) { - configData->fixedwing.FixedWingPitch1 = 0; - configData->fixedwing.FixedWingPitch2 = 0; - configData->fixedwing.FixedWingRoll1 = 0; - configData->fixedwing.FixedWingRoll2 = 0; - configData->fixedwing.FixedWingYaw1 = 0; - configData->fixedwing.FixedWingYaw2 = 0; + configData->fixedwing.FixedWingPitch1 = 0; + configData->fixedwing.FixedWingPitch2 = 0; + configData->fixedwing.FixedWingRoll1 = 0; + configData->fixedwing.FixedWingRoll2 = 0; + configData->fixedwing.FixedWingYaw1 = 0; + configData->fixedwing.FixedWingYaw2 = 0; configData->fixedwing.FixedWingThrottle = 0; } /** - Virtual function to refresh the UI widget values + Virtual function to refresh the UI widget values */ void ConfigFixedWingWidget::refreshWidgetsValues(QString frameType) { @@ -205,13 +205,12 @@ void ConfigFixedWingWidget::refreshWidgetsValues(QString frameType) if (isValidThrottleCurve(&curveValues)) { // yes, use the curve we just read from mixersettings m_aircraft->fixedWingThrottle->initCurve(&curveValues); - } - else { + } else { // no, init a straight curve m_aircraft->fixedWingThrottle->initLinearCurve(curveValues.count(), 1.0); } - GUIConfigDataUnion config = getConfigData(); + GUIConfigDataUnion config = getConfigData(); fixedGUISettingsStruct fixed = config.fixedwing; // Then retrieve how channels are setup @@ -230,61 +229,61 @@ void ConfigFixedWingWidget::refreshWidgetsValues(QString frameType) if (channel > -1) { // If for some reason the actuators were incoherent, we might fail here, hence the check. m_aircraft->elevonSlider1->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL) * 100); + getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL) * 100); m_aircraft->elevonSlider2->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); + getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); } - } - else if (frameType == "FixedWingVtail") { + } else if (frameType == "FixedWingVtail") { int channel = m_aircraft->fwElevator1ChannelBox->currentIndex() - 1; if (channel > -1) { // If for some reason the actuators were incoherent, we might fail here, hence the check. m_aircraft->elevonSlider1->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW) * 100); + getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW) * 100); m_aircraft->elevonSlider2->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); + getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); } } } /** - Virtual function to update the UI widget objects + Virtual function to update the UI widget objects */ QString ConfigFixedWingWidget::updateConfigObjectsFromWidgets() { - QString airframeType = "FixedWing"; + QString airframeType = "FixedWing"; + + // Save the curve (common to all Fixed wing frames) + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); - // Save the curve (common to all Fixed wing frames) - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); - // Remove Feed Forward, it is pointless on a plane: + // Remove Feed Forward, it is pointless on a plane: setMixerValue(mixer, "FeedForward", 0.0); // Set the throttle curve - setThrottleCurve(mixer,VehicleConfig::MIXER_THROTTLECURVE1, m_aircraft->fixedWingThrottle->getCurve()); + setThrottleCurve(mixer, VehicleConfig::MIXER_THROTTLECURVE1, m_aircraft->fixedWingThrottle->getCurve()); - // All airframe types must start with "FixedWing" - if (m_aircraft->fixedWingType->currentText() == "Elevator aileron rudder" ) { - airframeType = "FixedWing"; - setupFrameFixedWing( airframeType ); - } else if (m_aircraft->fixedWingType->currentText() == "Elevon") { - airframeType = "FixedWingElevon"; - setupFrameElevon( airframeType ); - } else { // "Vtail" - airframeType = "FixedWingVtail"; - setupFrameVtail( airframeType ); - } + // All airframe types must start with "FixedWing" + if (m_aircraft->fixedWingType->currentText() == "Elevator aileron rudder") { + airframeType = "FixedWing"; + setupFrameFixedWing(airframeType); + } else if (m_aircraft->fixedWingType->currentText() == "Elevon") { + airframeType = "FixedWingElevon"; + setupFrameElevon(airframeType); + } else { // "Vtail" + airframeType = "FixedWingVtail"; + setupFrameVtail(airframeType); + } - return airframeType; + return airframeType; } /** - Setup Elevator/Aileron/Rudder airframe. - - If both Aileron channels are set to 'None' (EasyStar), do Pitch/Rudder mixing - - Returns False if impossible to create the mixer. + Setup Elevator/Aileron/Rudder airframe. + + If both Aileron channels are set to 'None' (EasyStar), do Pitch/Rudder mixing + + Returns False if impossible to create the mixer. */ bool ConfigFixedWingWidget::setupFrameFixedWing(QString airframeType) { @@ -298,16 +297,16 @@ bool ConfigFixedWingWidget::setupFrameFixedWing(QString airframeType) GUIConfigDataUnion config = getConfigData(); resetActuators(&config); - config.fixedwing.FixedWingPitch1 = m_aircraft->fwElevator1ChannelBox->currentIndex(); - config.fixedwing.FixedWingPitch2 = m_aircraft->fwElevator2ChannelBox->currentIndex(); - config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); - config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); - config.fixedwing.FixedWingYaw1 = m_aircraft->fwRudder1ChannelBox->currentIndex(); + config.fixedwing.FixedWingPitch1 = m_aircraft->fwElevator1ChannelBox->currentIndex(); + config.fixedwing.FixedWingPitch2 = m_aircraft->fwElevator2ChannelBox->currentIndex(); + config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); + config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); + config.fixedwing.FixedWingYaw1 = m_aircraft->fwRudder1ChannelBox->currentIndex(); config.fixedwing.FixedWingThrottle = m_aircraft->fwEngineChannelBox->currentIndex(); setConfigData(config); - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); @@ -355,7 +354,7 @@ bool ConfigFixedWingWidget::setupFrameFixedWing(QString airframeType) } /** - Setup Elevon + Setup Elevon */ bool ConfigFixedWingWidget::setupFrameElevon(QString airframeType) { @@ -368,15 +367,15 @@ bool ConfigFixedWingWidget::setupFrameElevon(QString airframeType) GUIConfigDataUnion config = getConfigData(); resetActuators(&config); - config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); - config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); - config.fixedwing.FixedWingYaw1 = m_aircraft->fwRudder1ChannelBox->currentIndex(); - config.fixedwing.FixedWingYaw2 = m_aircraft->fwRudder2ChannelBox->currentIndex(); + config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); + config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); + config.fixedwing.FixedWingYaw1 = m_aircraft->fwRudder1ChannelBox->currentIndex(); + config.fixedwing.FixedWingYaw2 = m_aircraft->fwRudder2ChannelBox->currentIndex(); config.fixedwing.FixedWingThrottle = m_aircraft->fwEngineChannelBox->currentIndex(); setConfigData(config); - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); @@ -407,16 +406,16 @@ bool ConfigFixedWingWidget::setupFrameElevon(QString airframeType) channel = m_aircraft->fwAileron1ChannelBox->currentIndex() - 1; if (channel > -1) { setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - value = (double) (m_aircraft->elevonSlider2->value() * 1.27); + value = (double)(m_aircraft->elevonSlider2->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value); - value = (double) (m_aircraft->elevonSlider1->value() * 1.27); + value = (double)(m_aircraft->elevonSlider1->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, value); channel = m_aircraft->fwAileron2ChannelBox->currentIndex() - 1; setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - value = (double) (m_aircraft->elevonSlider2->value() * 1.27); + value = (double)(m_aircraft->elevonSlider2->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value); - value = (double) (m_aircraft->elevonSlider1->value() * 1.27); + value = (double)(m_aircraft->elevonSlider1->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, -value); } @@ -425,28 +424,28 @@ bool ConfigFixedWingWidget::setupFrameElevon(QString airframeType) } /** - Setup VTail + Setup VTail */ bool ConfigFixedWingWidget::setupFrameVtail(QString airframeType) { // Check coherence: - // Show any config errors in GUI + // Show any config errors in GUI if (throwConfigError(airframeType)) { return false; } - + GUIConfigDataUnion config = getConfigData(); resetActuators(&config); - config.fixedwing.FixedWingPitch1 = m_aircraft->fwElevator1ChannelBox->currentIndex(); - config.fixedwing.FixedWingPitch2 = m_aircraft->fwElevator2ChannelBox->currentIndex(); - config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); - config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); + config.fixedwing.FixedWingPitch1 = m_aircraft->fwElevator1ChannelBox->currentIndex(); + config.fixedwing.FixedWingPitch2 = m_aircraft->fwElevator2ChannelBox->currentIndex(); + config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); + config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); config.fixedwing.FixedWingThrottle = m_aircraft->fwEngineChannelBox->currentIndex(); setConfigData(config); - - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); @@ -458,44 +457,44 @@ bool ConfigFixedWingWidget::setupFrameVtail(QString airframeType) // 1. Assign the servo/motor/none for each channel // motor - int channel = m_aircraft->fwEngineChannelBox->currentIndex()-1; - setMixerType(mixer,channel,VehicleConfig::MIXERTYPE_MOTOR); - setMixerVectorValue(mixer,channel,VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127); + int channel = m_aircraft->fwEngineChannelBox->currentIndex() - 1; + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_MOTOR); + setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127); // rudders - channel = m_aircraft->fwRudder1ChannelBox->currentIndex()-1; - setMixerType(mixer,channel,VehicleConfig::MIXERTYPE_SERVO); + channel = m_aircraft->fwRudder1ChannelBox->currentIndex() - 1; + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, 127); - channel = m_aircraft->fwRudder2ChannelBox->currentIndex()-1; - setMixerType(mixer,channel,VehicleConfig::MIXERTYPE_SERVO); + channel = m_aircraft->fwRudder2ChannelBox->currentIndex() - 1; + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -127); // ailerons - channel = m_aircraft->fwAileron1ChannelBox->currentIndex()-1; + channel = m_aircraft->fwAileron1ChannelBox->currentIndex() - 1; if (channel > -1) { - setMixerType(mixer,channel,VehicleConfig::MIXERTYPE_SERVO); + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, 127); - channel = m_aircraft->fwAileron2ChannelBox->currentIndex()-1; - setMixerType(mixer,channel,VehicleConfig::MIXERTYPE_SERVO); + channel = m_aircraft->fwAileron2ChannelBox->currentIndex() - 1; + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, -127); } // vtail - channel = m_aircraft->fwElevator1ChannelBox->currentIndex()-1; + channel = m_aircraft->fwElevator1ChannelBox->currentIndex() - 1; if (channel > -1) { - setMixerType(mixer,channel,VehicleConfig::MIXERTYPE_SERVO); - double value = (double)(m_aircraft->elevonSlider2->value()*1.27); + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); + double value = (double)(m_aircraft->elevonSlider2->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value); - value = (double)(m_aircraft->elevonSlider1->value()*1.27); + value = (double)(m_aircraft->elevonSlider1->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, value); - channel = m_aircraft->fwElevator2ChannelBox->currentIndex()-1; - setMixerType(mixer,channel,VehicleConfig::MIXERTYPE_SERVO); - value = (double)(m_aircraft->elevonSlider2->value()*1.27); + channel = m_aircraft->fwElevator2ChannelBox->currentIndex() - 1; + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); + value = (double)(m_aircraft->elevonSlider2->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value); - value = (double)(m_aircraft->elevonSlider1->value()*1.27); + value = (double)(m_aircraft->elevonSlider1->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -value); } @@ -506,13 +505,14 @@ bool ConfigFixedWingWidget::setupFrameVtail(QString airframeType) void ConfigFixedWingWidget::enableControls(bool enable) { ConfigTaskWidget::enableControls(enable); - if(enable) { + + if (enable) { setupUI(m_aircraft->fixedWingType->currentText()); } } /** - This function displays text and color formatting in order to help the user understand what channels have not yet been configured. + This function displays text and color formatting in order to help the user understand what channels have not yet been configured. */ bool ConfigFixedWingWidget::throwConfigError(QString airframeType) { @@ -520,79 +520,78 @@ bool ConfigFixedWingWidget::throwConfigError(QString airframeType) bool error = false; // Create a red block. All combo boxes are the same size, so any one should do as a model - int size = m_aircraft->fwEngineChannelBox->style()->pixelMetric(QStyle::PM_SmallIconSize); + int size = m_aircraft->fwEngineChannelBox->style()->pixelMetric(QStyle::PM_SmallIconSize); QPixmap pixmap(size, size); + pixmap.fill(QColor("red")); if (airframeType == "FixedWing") { if (m_aircraft->fwEngineChannelBox->currentText() == "None") { - m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if (m_aircraft->fwElevator1ChannelBox->currentText() == "None") { - m_aircraft->fwElevator1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwElevator1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwElevator1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwElevator1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if ((m_aircraft->fwAileron1ChannelBox->currentText() == "None") - && (m_aircraft->fwRudder1ChannelBox->currentText() == "None")) { + && (m_aircraft->fwRudder1ChannelBox->currentText() == "None")) { pixmap.fill(QColor("green")); - m_aircraft->fwAileron1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes - m_aircraft->fwRudder1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwAileron1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + m_aircraft->fwRudder1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwAileron1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - m_aircraft->fwRudder1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwAileron1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->fwRudder1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } } else if (airframeType == "FixedWingElevon") { if (m_aircraft->fwEngineChannelBox->currentText() == "None") { - m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if (m_aircraft->fwAileron1ChannelBox->currentText() == "None") { - m_aircraft->fwAileron1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwAileron1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwAileron1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwAileron1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if (m_aircraft->fwAileron2ChannelBox->currentText() == "None") { - m_aircraft->fwAileron2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwAileron2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwAileron2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwAileron2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } - } else if (airframeType == "FixedWingVtail") { if (m_aircraft->fwEngineChannelBox->currentText() == "None") { - m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if (m_aircraft->fwElevator1ChannelBox->currentText() == "None") { - m_aircraft->fwElevator1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwElevator1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwElevator1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwElevator1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if (m_aircraft->fwElevator2ChannelBox->currentText() == "None") { - m_aircraft->fwElevator2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwElevator2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwElevator2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwElevator2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } - } if (error) { diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h index 3a0d45883..6716334b5 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h @@ -41,8 +41,7 @@ class Ui_Widget; -class ConfigFixedWingWidget: public VehicleConfig -{ +class ConfigFixedWingWidget : public VehicleConfig { Q_OBJECT public: @@ -70,7 +69,6 @@ protected: private slots: virtual void setupUI(QString airframeType); virtual bool throwConfigError(QString airframeType); - }; #endif // CONFIGFIXEDWINGWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp index d37b58695..8941c7e35 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp @@ -45,7 +45,8 @@ QStringList ConfigGroundVehicleWidget::getChannelDescriptions() { // init a channel_numelem list of channel desc defaults QStringList channelDesc; - for (int i = 0; i < (int) ConfigGroundVehicleWidget::CHANNEL_NUMELEM; i++) { + + for (int i = 0; i < (int)ConfigGroundVehicleWidget::CHANNEL_NUMELEM; i++) { channelDesc.append(QString("-")); } @@ -68,7 +69,7 @@ QStringList ConfigGroundVehicleWidget::getChannelDescriptions() } ConfigGroundVehicleWidget::ConfigGroundVehicleWidget(QWidget *parent) : - VehicleConfig(parent), m_aircraft(new Ui_GroundConfigWidget()) + VehicleConfig(parent), m_aircraft(new Ui_GroundConfigWidget()) { m_aircraft->setupUi(this); @@ -90,7 +91,7 @@ ConfigGroundVehicleWidget::~ConfigGroundVehicleWidget() } /** - Virtual function to setup the UI + Virtual function to setup the UI */ void ConfigGroundVehicleWidget::setupUI(QString frameType) { @@ -106,7 +107,7 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType) if (frameType == "GroundVehicleDifferential" || frameType == "Differential (tank)") { // Tank setComboCurrentIndex(m_aircraft->groundVehicleType, - m_aircraft->groundVehicleType->findText("Differential (tank)")); + m_aircraft->groundVehicleType->findText("Differential (tank)")); m_aircraft->gvMotor1ChannelBox->setEnabled(true); m_aircraft->gvMotor2ChannelBox->setEnabled(true); @@ -123,7 +124,6 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType) m_aircraft->gvThrottleCurve1GroupBox->setTitle("Left throttle curve"); m_aircraft->gvThrottleCurve2GroupBox->setTitle("Right throttle curve"); - } else if (frameType == "GroundVehicleMotorcycle" || frameType == "Motorcycle") { // Motorcycle setComboCurrentIndex(m_aircraft->groundVehicleType, m_aircraft->groundVehicleType->findText("Motorcycle")); @@ -161,12 +161,14 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType) void ConfigGroundVehicleWidget::enableControls(bool enable) { ConfigTaskWidget::enableControls(enable); - if(enable) { + + if (enable) { setupUI(m_aircraft->groundVehicleType->currentText()); } } -void ConfigGroundVehicleWidget::registerWidgets(ConfigTaskWidget &parent) { +void ConfigGroundVehicleWidget::registerWidgets(ConfigTaskWidget &parent) +{ parent.addWidget(m_aircraft->groundVehicleThrottle1->getCurveWidget()); parent.addWidget(m_aircraft->groundVehicleThrottle1); parent.addWidget(m_aircraft->groundVehicleThrottle2->getCurveWidget()); @@ -190,7 +192,7 @@ void ConfigGroundVehicleWidget::resetActuators(GUIConfigDataUnion *configData) } /** - Virtual function to refresh the UI widget values + Virtual function to refresh the UI widget values */ void ConfigGroundVehicleWidget::refreshWidgetsValues(QString frameType) { @@ -229,45 +231,45 @@ void ConfigGroundVehicleWidget::refreshWidgetsValues(QString frameType) setComboCurrentIndex(m_aircraft->gvSteering2ChannelBox, config.ground.GroundVehicleSteering2); if (frameType == "GroundVehicleDifferential") { - //CURRENTLY BROKEN UNTIL WE DECIDE HOW DIFFERENTIAL SHOULD BEHAVE + // CURRENTLY BROKEN UNTIL WE DECIDE HOW DIFFERENTIAL SHOULD BEHAVE // If the vehicle type is "differential", restore the slider setting // Find the channel number for Motor1 - //obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); - //Q_ASSERT(obj); + // obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + // Q_ASSERT(obj); int channel = m_aircraft->gvMotor1ChannelBox->currentIndex() - 1; if (channel > -1) { // If for some reason the actuators were incoherent, we might fail here, hence the check. m_aircraft->differentialSteeringSlider1->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL) * 100); + getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL) * 100); m_aircraft->differentialSteeringSlider2->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); + getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); } } else if (frameType == "GroundVehicleMotorcycle") { // CURRENTLY BROKEN UNTIL WE DECIDE HOW MOTORCYCLE SHOULD BEHAVE -// obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); -// Q_ASSERT(obj); -// int chMixerNumber = m_aircraft->gvMotor1ChannelBox->currentIndex()-1; -// if (chMixerNumber >=0) { -// field = obj->getField(mixerVectors.at(chMixerNumber)); -// int ti = field->getElementNames().indexOf("Yaw"); -// m_aircraft->differentialSteeringSlider1->setValue(field->getDouble(ti)*100); +// obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); +// Q_ASSERT(obj); +// int chMixerNumber = m_aircraft->gvMotor1ChannelBox->currentIndex()-1; +// if (chMixerNumber >=0) { +// field = obj->getField(mixerVectors.at(chMixerNumber)); +// int ti = field->getElementNames().indexOf("Yaw"); +// m_aircraft->differentialSteeringSlider1->setValue(field->getDouble(ti)*100); // -// ti = field->getElementNames().indexOf("Pitch"); -// m_aircraft->differentialSteeringSlider2->setValue(field->getDouble(ti)*100); -// } +// ti = field->getElementNames().indexOf("Pitch"); +// m_aircraft->differentialSteeringSlider2->setValue(field->getDouble(ti)*100); +// } } } /** - Virtual function to update the UI widget objects + Virtual function to update the UI widget objects */ QString ConfigGroundVehicleWidget::updateConfigObjectsFromWidgets() { QString airframeType = "GroundVehicleCar"; // Save the curve (common to all ground vehicle frames) - UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); // Remove Feed Forward, it is pointless on a ground vehicle: setMixerValue(mixer, "FeedForward", 0.0); @@ -292,9 +294,9 @@ QString ConfigGroundVehicleWidget::updateConfigObjectsFromWidgets() } /** - Setup balancing ground vehicle. - - Returns False if impossible to create the mixer. + Setup balancing ground vehicle. + + Returns False if impossible to create the mixer. */ bool ConfigGroundVehicleWidget::setupGroundVehicleMotorcycle(QString airframeType) { @@ -313,7 +315,7 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleMotorcycle(QString airframeTyp setConfigData(config); - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); @@ -341,9 +343,9 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleMotorcycle(QString airframeTyp } /** - Setup differentially steered ground vehicle. - - Returns False if impossible to create the mixer. + Setup differentially steered ground vehicle. + + Returns False if impossible to create the mixer. */ bool ConfigGroundVehicleWidget::setupGroundVehicleDifferential(QString airframeType) { @@ -363,7 +365,7 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleDifferential(QString airframeT setConfigData(config); - UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); @@ -386,9 +388,9 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleDifferential(QString airframeT } /** - Setup steerable ground vehicle. - - Returns False if impossible to create the mixer. + Setup steerable ground vehicle. + + Returns False if impossible to create the mixer. */ bool ConfigGroundVehicleWidget::setupGroundVehicleCar(QString airframeType) { @@ -409,7 +411,7 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleCar(QString airframeType) setConfigData(config); - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); @@ -436,7 +438,7 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleCar(QString airframeType) } /** - This function displays text and color formatting in order to help the user understand what channels have not yet been configured. + This function displays text and color formatting in order to help the user understand what channels have not yet been configured. */ bool ConfigGroundVehicleWidget::throwConfigError(QString airframeType) { @@ -444,83 +446,83 @@ bool ConfigGroundVehicleWidget::throwConfigError(QString airframeType) bool error = false; // Create a red block. All combo boxes are the same size, so any one should do as a model - int size = m_aircraft->gvEngineChannelBox->style()->pixelMetric(QStyle::PM_SmallIconSize); + int size = m_aircraft->gvEngineChannelBox->style()->pixelMetric(QStyle::PM_SmallIconSize); QPixmap pixmap(size, size); + pixmap.fill(QColor("red")); - if (airframeType == "GroundVehicleCar") { //Car + if (airframeType == "GroundVehicleCar") { // Car if (m_aircraft->gvMotor1ChannelBox->currentText() == "None" - && m_aircraft->gvMotor2ChannelBox->currentText() == "None") { + && m_aircraft->gvMotor2ChannelBox->currentText() == "None") { pixmap.fill(QColor("green")); - m_aircraft->gvMotor1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes - m_aircraft->gvMotor2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes - //m_aircraft->gvMotor1Label->setText("" + m_aircraft->gvMotor1Label->text() + ""); - //m_aircraft->gvMotor2Label->setText("" + m_aircraft->gvMotor2Label->text() + ""); + m_aircraft->gvMotor1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + m_aircraft->gvMotor2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + // m_aircraft->gvMotor1Label->setText("" + m_aircraft->gvMotor1Label->text() + ""); + // m_aircraft->gvMotor2Label->setText("" + m_aircraft->gvMotor2Label->text() + ""); error = true; - } else { - m_aircraft->gvMotor1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - m_aircraft->gvMotor2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - //QTextEdit* htmlText=new QTextEdit(m_aircraft->gvMotor1Label->text()); // HtmlText is any QString with html tags. - //m_aircraft->gvMotor1Label->setText(htmlText->toPlainText()); - //delete htmlText; + m_aircraft->gvMotor1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->gvMotor2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + // QTextEdit* htmlText=new QTextEdit(m_aircraft->gvMotor1Label->text()); // HtmlText is any QString with html tags. + // m_aircraft->gvMotor1Label->setText(htmlText->toPlainText()); + // delete htmlText; - //htmlText=new QTextEdit(m_aircraft->gvMotor2Label->text()); // HtmlText is any QString with html tags. - //m_aircraft->gvMotor2Label->setText(htmlText->toPlainText()); + // htmlText=new QTextEdit(m_aircraft->gvMotor2Label->text()); // HtmlText is any QString with html tags. + // m_aircraft->gvMotor2Label->setText(htmlText->toPlainText()); } if (m_aircraft->gvSteering1ChannelBox->currentText() == "None" - && m_aircraft->gvSteering2ChannelBox->currentText() == "None") { - m_aircraft->gvSteering1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes - m_aircraft->gvSteering2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes - //m_aircraft->gvStatusLabel->setText("ERROR: check steering channel assignment"); - //m_aircraft->gvSteering1Label->setText("" + m_aircraft->gvSteering1Label->text() + ""); - //m_aircraft->gvSteering2Label->setText("" + m_aircraft->gvSteering2Label->text() + ""); + && m_aircraft->gvSteering2ChannelBox->currentText() == "None") { + m_aircraft->gvSteering1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + m_aircraft->gvSteering2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + // m_aircraft->gvStatusLabel->setText("ERROR: check steering channel assignment"); + // m_aircraft->gvSteering1Label->setText("" + m_aircraft->gvSteering1Label->text() + ""); + // m_aircraft->gvSteering2Label->setText("" + m_aircraft->gvSteering2Label->text() + ""); error = true; } else { - m_aircraft->gvSteering1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - m_aircraft->gvSteering2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - //QTextEdit* htmlText=new QTextEdit(m_aircraft->gvSteering1Label->text()); // HtmlText is any QString with html tags. - //m_aircraft->gvSteering1Label->setText(htmlText->toPlainText()); - //delete htmlText; + m_aircraft->gvSteering1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->gvSteering2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + // QTextEdit* htmlText=new QTextEdit(m_aircraft->gvSteering1Label->text()); // HtmlText is any QString with html tags. + // m_aircraft->gvSteering1Label->setText(htmlText->toPlainText()); + // delete htmlText; - //htmlText=new QTextEdit(m_aircraft->gvSteering2Label->text()); // HtmlText is any QString with html tags. - //m_aircraft->gvSteering2Label->setText(htmlText->toPlainText()); + // htmlText=new QTextEdit(m_aircraft->gvSteering2Label->text()); // HtmlText is any QString with html tags. + // m_aircraft->gvSteering2Label->setText(htmlText->toPlainText()); } - } else if (airframeType == "GroundVehicleDifferential") { //Tank + } else if (airframeType == "GroundVehicleDifferential") { // Tank if (m_aircraft->gvMotor1ChannelBox->currentText() == "None" - || m_aircraft->gvMotor2ChannelBox->currentText() == "None") { - m_aircraft->gvMotor1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes - m_aircraft->gvMotor2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + || m_aircraft->gvMotor2ChannelBox->currentText() == "None") { + m_aircraft->gvMotor1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + m_aircraft->gvMotor2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->gvMotor1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - m_aircraft->gvMotor2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->gvMotor1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->gvMotor2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } // Always reset - m_aircraft->gvSteering1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - m_aircraft->gvSteering2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - } else if (airframeType == "GroundVehicleMotorcycle") { //Motorcycle + m_aircraft->gvSteering1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->gvSteering2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + } else if (airframeType == "GroundVehicleMotorcycle") { // Motorcycle if (m_aircraft->gvMotor1ChannelBox->currentText() == "None" - && m_aircraft->gvMotor2ChannelBox->currentText() == "None") { - m_aircraft->gvMotor2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + && m_aircraft->gvMotor2ChannelBox->currentText() == "None") { + m_aircraft->gvMotor2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->gvMotor2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->gvMotor2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if (m_aircraft->gvSteering1ChannelBox->currentText() == "None" - && m_aircraft->gvSteering2ChannelBox->currentText() == "None") { - m_aircraft->gvSteering1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + && m_aircraft->gvSteering2ChannelBox->currentText() == "None") { + m_aircraft->gvSteering1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->gvSteering1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->gvSteering1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } // Always reset - m_aircraft->gvMotor1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - m_aircraft->gvSteering2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->gvMotor1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->gvSteering2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if (error) { diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.h index 0a7abd858..c17dcea9d 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.h @@ -41,8 +41,7 @@ class Ui_Widget; -class ConfigGroundVehicleWidget: public VehicleConfig -{ +class ConfigGroundVehicleWidget : public VehicleConfig { Q_OBJECT public: @@ -70,7 +69,6 @@ private: private slots: virtual void setupUI(QString airframeType); virtual bool throwConfigError(QString airframeType); - }; #endif // CONFIGGROUNDVEHICLEWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp index a51cf370b..8aaa0c175 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp @@ -47,13 +47,14 @@ QStringList ConfigMultiRotorWidget::getChannelDescriptions() { // init a channel_numelem list of channel desc defaults QStringList channelDesc; - for (int i = 0; i < (int) ConfigMultiRotorWidget::CHANNEL_NUMELEM; i++) { + + for (int i = 0; i < (int)ConfigMultiRotorWidget::CHANNEL_NUMELEM; i++) { channelDesc.append(QString("-")); } // get the gui config data GUIConfigDataUnion configData = getConfigData(); - multiGUISettingsStruct multi = configData.multi; + multiGUISettingsStruct multi = configData.multi; if (multi.VTOLMotorN > 0 && multi.VTOLMotorN <= ConfigMultiRotorWidget::CHANNEL_NUMELEM) { channelDesc[multi.VTOLMotorN - 1] = QString("VTOLMotorN"); @@ -86,7 +87,7 @@ QStringList ConfigMultiRotorWidget::getChannelDescriptions() } ConfigMultiRotorWidget::ConfigMultiRotorWidget(QWidget *parent) : - VehicleConfig(parent), m_aircraft(new Ui_MultiRotorConfigWidget()), invertMotors(false) + VehicleConfig(parent), m_aircraft(new Ui_MultiRotorConfigWidget()), invertMotors(false) { m_aircraft->setupUi(this); @@ -110,13 +111,13 @@ ConfigMultiRotorWidget::ConfigMultiRotorWidget(QWidget *parent) : QStringList multiRotorTypes; multiRotorTypes << "Tricopter Y" << "Quad +" << "Quad X" << "Hexacopter" << "Hexacopter X" << "Hexacopter Y6" - << "Octocopter" << "Octocopter V" << "Octo Coax +" << "Octo Coax X"; + << "Octocopter" << "Octocopter V" << "Octo Coax +" << "Octo Coax X"; m_aircraft->multirotorFrameType->addItems(multiRotorTypes); // Set default model to "Quad X" m_aircraft->multirotorFrameType->setCurrentIndex(m_aircraft->multirotorFrameType->findText("Quad X")); - //setupUI(m_aircraft->multirotorFrameType->currentText()); + // setupUI(m_aircraft->multirotorFrameType->currentText()); connect(m_aircraft->multirotorFrameType, SIGNAL(currentIndexChanged(QString)), this, SLOT(setupUI(QString))); @@ -162,14 +163,14 @@ void ConfigMultiRotorWidget::setupUI(QString frameType) setYawMixLevel(33); } else if (frameType == "HexaX" || frameType == "Hexacopter X") { setComboCurrentIndex(m_aircraft->multirotorFrameType, - m_aircraft->multirotorFrameType->findText("Hexacopter X")); + m_aircraft->multirotorFrameType->findText("Hexacopter X")); m_aircraft->mrRollMixLevel->setValue(33); m_aircraft->mrPitchMixLevel->setValue(50); setYawMixLevel(33); } else if (frameType == "HexaCoax" || frameType == "Hexacopter Y6") { setComboCurrentIndex(m_aircraft->multirotorFrameType, - m_aircraft->multirotorFrameType->findText("Hexacopter Y6")); + m_aircraft->multirotorFrameType->findText("Hexacopter Y6")); m_aircraft->mrRollMixLevel->setValue(100); m_aircraft->mrPitchMixLevel->setValue(50); @@ -182,7 +183,7 @@ void ConfigMultiRotorWidget::setupUI(QString frameType) setYawMixLevel(25); } else if (frameType == "OctoV" || frameType == "Octocopter V") { setComboCurrentIndex(m_aircraft->multirotorFrameType, - m_aircraft->multirotorFrameType->findText("Octocopter V")); + m_aircraft->multirotorFrameType->findText("Octocopter V")); m_aircraft->mrRollMixLevel->setValue(25); m_aircraft->mrPitchMixLevel->setValue(25); @@ -216,7 +217,7 @@ void ConfigMultiRotorWidget::setupEnabledControls(QString frameType) // disable all motor channel boxes for (int i = 1; i <= 8; i++) { // do it manually so we can turn off any error decorations - QComboBox *combobox = qFindChild(this, "multiMotorChannelBox" + QString::number(i)); + QComboBox *combobox = qFindChild(this, "multiMotorChannelBox" + QString::number(i)); if (combobox) { combobox->setEnabled(false); combobox->setItemData(0, 0, Qt::DecorationRole); @@ -247,7 +248,8 @@ void ConfigMultiRotorWidget::setupEnabledControls(QString frameType) } } -void ConfigMultiRotorWidget::registerWidgets(ConfigTaskWidget &parent) { +void ConfigMultiRotorWidget::registerWidgets(ConfigTaskWidget &parent) +{ parent.addWidget(m_aircraft->multiThrottleCurve->getCurveWidget()); parent.addWidget(m_aircraft->multiThrottleCurve); parent.addWidget(m_aircraft->multirotorFrameType); @@ -268,19 +270,19 @@ void ConfigMultiRotorWidget::registerWidgets(ConfigTaskWidget &parent) { void ConfigMultiRotorWidget::resetActuators(GUIConfigDataUnion *configData) { - configData->multi.VTOLMotorN = 0; + configData->multi.VTOLMotorN = 0; configData->multi.VTOLMotorNE = 0; - configData->multi.VTOLMotorE = 0; + configData->multi.VTOLMotorE = 0; configData->multi.VTOLMotorSE = 0; - configData->multi.VTOLMotorS = 0; + configData->multi.VTOLMotorS = 0; configData->multi.VTOLMotorSW = 0; - configData->multi.VTOLMotorW = 0; + configData->multi.VTOLMotorW = 0; configData->multi.VTOLMotorNW = 0; configData->multi.TRIYaw = 0; } /** - Helper function to refresh the UI widget values + Helper function to refresh the UI widget values */ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) { @@ -303,7 +305,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->multiThrottleCurve->initLinearCurve(curveValues.count(), 0.9); } - GUIConfigDataUnion config = getConfigData(); + GUIConfigDataUnion config = getConfigData(); multiGUISettingsStruct multi = config.multi; if (frameType == "QuadP") { @@ -321,11 +323,11 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) double value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH); m_aircraft->mrPitchMixLevel->setValue(qRound(value / 1.27)); - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); } } else if (frameType == "QuadX") { @@ -370,9 +372,9 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); - //change channels + // change channels channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); } } else if (frameType == "HexaX") { @@ -393,11 +395,11 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) double value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH); m_aircraft->mrPitchMixLevel->setValue(qRound(value / 1.27)); - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); } } else if (frameType == "HexaCoax") { @@ -418,10 +420,10 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->mrPitchMixLevel->setValue(qRound(2 * value / 1.27)); channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(qRound(value / 1.27)); - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(qRound(value / 1.27)); } } else if (frameType == "Octo" || frameType == "OctoV" || frameType == "OctoCoaxP") { @@ -447,9 +449,9 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); - //change channels + // change channels channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); } else if (frameType == "OctoV") { double value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH); @@ -458,9 +460,9 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); - //change channels + // change channels channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); } else if (frameType == "OctoCoaxP") { double value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH); @@ -469,9 +471,9 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); - //change channels + // change channels channel = m_aircraft->multiMotorChannelBox3->currentIndex() - 1; - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); } } @@ -515,7 +517,6 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(qRound(value / 1.27)); - } } @@ -523,15 +524,16 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) } /** - Helper function to update the UI widget objects + Helper function to update the UI widget objects */ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() { UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + Q_ASSERT(mixer); // Curve is also common to all quads: - setThrottleCurve(mixer, VehicleConfig::MIXER_THROTTLECURVE1, m_aircraft->multiThrottleCurve->getCurve() ); + setThrottleCurve(mixer, VehicleConfig::MIXER_THROTTLECURVE1, m_aircraft->multiThrottleCurve->getCurve()); QString airframeType; QList motorList; @@ -558,46 +560,43 @@ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() setupMotors(motorList); // Motor 1 to 6, Y6 Layout: - // pitch roll yaw + // pitch roll yaw double mixerMatrix[8][3] = { - { 0.5, 1, -1 }, - { 0.5, 1, 1 }, - { 0.5, -1, -1 }, - { 0.5, -1, 1 }, - { -1, 0, -1 }, - { -1, 0, 1 }, - { 0, 0, 0 }, - { 0, 0, 0 } + { 0.5, 1, -1 }, + { 0.5, 1, 1 }, + { 0.5, -1, -1 }, + { 0.5, -1, 1 }, + { -1, 0, -1 }, + { -1, 0, 1 }, + { 0, 0, 0 }, + { 0, 0, 0 } }; setupMultiRotorMixer(mixerMatrix); m_aircraft->mrStatusLabel->setText("Configuration OK"); - } else if (m_aircraft->multirotorFrameType->currentText() == "Octocopter") { airframeType = "Octo"; // Show any config errors in GUI if (throwConfigError(8)) { return airframeType; - } motorList << "VTOLMotorN" << "VTOLMotorNE" << "VTOLMotorE" << "VTOLMotorSE" << "VTOLMotorS" << "VTOLMotorSW" - << "VTOLMotorW" << "VTOLMotorNW"; + << "VTOLMotorW" << "VTOLMotorNW"; setupMotors(motorList); // Motor 1 to 8: - // pitch roll yaw + // pitch roll yaw double mixerMatrix[8][3] = { - { 1, 0, -1 }, - { 1, -1, 1 }, - { 0, -1, -1 }, - { -1, -1, 1 }, - { -1, 0, -1 }, - { -1, 1, 1 }, - { 0, 1, -1 }, - { 1, 1, 1 } + { 1, 0, -1 }, + { 1, -1, 1 }, + { 0, -1, -1 }, + { -1, -1, 1 }, + { -1, 0, -1 }, + { -1, 1, 1 }, + { 0, 1, -1 }, + { 1, 1, 1 } }; setupMultiRotorMixer(mixerMatrix); m_aircraft->mrStatusLabel->setText("Configuration OK"); - } else if (m_aircraft->multirotorFrameType->currentText() == "Octocopter V") { airframeType = "OctoV"; @@ -606,24 +605,23 @@ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() return airframeType; } motorList << "VTOLMotorN" << "VTOLMotorNE" << "VTOLMotorE" << "VTOLMotorSE" << "VTOLMotorS" << "VTOLMotorSW" - << "VTOLMotorW" << "VTOLMotorNW"; + << "VTOLMotorW" << "VTOLMotorNW"; setupMotors(motorList); // Motor 1 to 8: // IMPORTANT: Assumes evenly spaced engines - // pitch roll yaw + // pitch roll yaw double mixerMatrix[8][3] = { - { 0.33, -1, -1 }, - { 1 , -1, 1 }, - { -1 , -1, -1 }, - { -0.33, -1, 1 }, - { -0.33, 1, -1 }, - { -1 , 1, 1 }, - { 1 , 1, -1 }, - { 0.33, 1, 1 } + { 0.33, -1, -1 }, + { 1, -1, 1 }, + { -1, -1, -1 }, + { -0.33, -1, 1 }, + { -0.33, 1, -1 }, + { -1, 1, 1 }, + { 1, 1, -1 }, + { 0.33, 1, 1 } }; setupMultiRotorMixer(mixerMatrix); m_aircraft->mrStatusLabel->setText("Configuration OK"); - } else if (m_aircraft->multirotorFrameType->currentText() == "Octo Coax +") { airframeType = "OctoCoaxP"; @@ -632,23 +630,22 @@ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() return airframeType; } motorList << "VTOLMotorN" << "VTOLMotorNE" << "VTOLMotorE" << "VTOLMotorSE" << "VTOLMotorS" << "VTOLMotorSW" - << "VTOLMotorW" << "VTOLMotorNW"; + << "VTOLMotorW" << "VTOLMotorNW"; setupMotors(motorList); // Motor 1 to 8: - // pitch roll yaw + // pitch roll yaw double mixerMatrix[8][3] = { - { 1, 0, -1 }, - { 1, 0, 1 }, - { 0, -1, -1 }, - { 0, -1, 1 }, - { -1, 0, -1 }, - { -1, 0, 1 }, - { 0, 1, -1 }, - { 0, 1, 1 } + { 1, 0, -1 }, + { 1, 0, 1 }, + { 0, -1, -1 }, + { 0, -1, 1 }, + { -1, 0, -1 }, + { -1, 0, 1 }, + { 0, 1, -1 }, + { 0, 1, 1 } }; setupMultiRotorMixer(mixerMatrix); m_aircraft->mrStatusLabel->setText("Configuration OK"); - } else if (m_aircraft->multirotorFrameType->currentText() == "Octo Coax X") { airframeType = "OctoCoaxX"; @@ -657,30 +654,28 @@ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() return airframeType; } motorList << "VTOLMotorNW" << "VTOLMotorN" << "VTOLMotorNE" << "VTOLMotorE" << "VTOLMotorSE" << "VTOLMotorS" - << "VTOLMotorSW" << "VTOLMotorW"; + << "VTOLMotorSW" << "VTOLMotorW"; setupMotors(motorList); // Motor 1 to 8: - // pitch roll yaw + // pitch roll yaw double mixerMatrix[8][3] = { - { 1, 1, -1 }, - { 1, 1, 1 }, - { 1, -1, -1 }, - { 1, -1, 1 }, + { 1, 1, -1 }, + { 1, 1, 1 }, + { 1, -1, -1 }, + { 1, -1, 1 }, { -1, -1, -1 }, - { -1, -1, 1 }, - { -1, 1, -1 }, - { -1, 1, 1 } + { -1, -1, 1 }, + { -1, 1, -1 }, + { -1, 1, 1 } }; setupMultiRotorMixer(mixerMatrix); m_aircraft->mrStatusLabel->setText("Configuration OK"); - } else if (m_aircraft->multirotorFrameType->currentText() == "Tricopter Y") { airframeType = "Tri"; // Show any config errors in GUI if (throwConfigError(3)) { return airframeType; - } if (m_aircraft->triYawChannelBox->currentText() == "None") { m_aircraft->mrStatusLabel->setText("Error: Assign a Yaw channel"); @@ -694,16 +689,16 @@ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() setConfigData(config); // Motor 1 to 6, Y6 Layout: - // pitch roll yaw + // pitch roll yaw double mixerMatrix[8][3] = { - { 0.5, 1, 0 }, - { 0.5, -1, 0 }, - { -1, 0, 0 }, - { 0, 0, 0 }, - { 0, 0, 0 }, - { 0, 0, 0 }, - { 0, 0, 0 }, - { 0, 0, 0 } + { 0.5, 1, 0 }, + { 0.5, -1, 0 }, + { -1, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 } }; setupMultiRotorMixer(mixerMatrix); @@ -716,7 +711,6 @@ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() } m_aircraft->mrStatusLabel->setText(tr("Configuration OK")); - } return airframeType; @@ -731,11 +725,12 @@ void ConfigMultiRotorWidget::setYawMixLevel(int value) m_aircraft->mrYawMixLevel->setValue(value); m_aircraft->MultirotorRevMixerCheckBox->setChecked(false); } - } -void ConfigMultiRotorWidget::reverseMultirotorMotor(){ +void ConfigMultiRotorWidget::reverseMultirotorMotor() +{ QString frameType = m_aircraft->multirotorFrameType->currentText(); + updateAirframe(frameType); } @@ -779,11 +774,12 @@ void ConfigMultiRotorWidget::updateAirframe(QString frameType) } /** - Helper function: setupQuadMotor + Helper function: setupQuadMotor */ void ConfigMultiRotorWidget::setupQuadMotor(int channel, double pitch, double roll, double yaw) { - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + Q_ASSERT(mixer); setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_MOTOR); @@ -796,21 +792,22 @@ void ConfigMultiRotorWidget::setupQuadMotor(int channel, double pitch, double ro } /** - Helper function: setup motors. Takes a list of channel names in input. + Helper function: setup motors. Takes a list of channel names in input. */ void ConfigMultiRotorWidget::setupMotors(QList motorList) { - QList mmList; + QList mmList; mmList << m_aircraft->multiMotorChannelBox1 << m_aircraft->multiMotorChannelBox2 - << m_aircraft->multiMotorChannelBox3 << m_aircraft->multiMotorChannelBox4 - << m_aircraft->multiMotorChannelBox5 << m_aircraft->multiMotorChannelBox6 - << m_aircraft->multiMotorChannelBox7 << m_aircraft->multiMotorChannelBox8; + << m_aircraft->multiMotorChannelBox3 << m_aircraft->multiMotorChannelBox4 + << m_aircraft->multiMotorChannelBox5 << m_aircraft->multiMotorChannelBox6 + << m_aircraft->multiMotorChannelBox7 << m_aircraft->multiMotorChannelBox8; GUIConfigDataUnion configData = getConfigData(); resetActuators(&configData); - foreach (QString motor, motorList) { + foreach(QString motor, motorList) { int index = mmList.takeFirst()->currentIndex(); + if (motor == QString("VTOLMotorN")) { configData.multi.VTOLMotorN = index; } else if (motor == QString("VTOLMotorNE")) { @@ -833,7 +830,7 @@ void ConfigMultiRotorWidget::setupMotors(QList motorList) } /** - Set up a Quad-X or Quad-P mixer + Set up a Quad-X or Quad-P mixer */ bool ConfigMultiRotorWidget::setupQuad(bool pLayout) { @@ -854,37 +851,37 @@ bool ConfigMultiRotorWidget::setupQuad(bool pLayout) // Now, setup the mixer: // Motor 1 to 4, X Layout: - // pitch roll yaw - // {0.5 ,0.5 ,-0.5 //Front left motor (CW) - // {0.5 ,-0.5 ,0.5 //Front right motor(CCW) - // {-0.5 ,-0.5 ,-0.5 //rear right motor (CW) - // {-0.5 ,0.5 ,0.5 //Rear left motor (CCW) - double xMixer [8][3] = { - { 1, 1, -1}, - { 1, -1, 1}, - {-1, -1, -1}, - {-1, 1, 1}, - { 0, 0, 0}, - { 0, 0, 0}, - { 0, 0, 0}, - { 0, 0, 0} + // pitch roll yaw + // {0.5 ,0.5 ,-0.5 //Front left motor (CW) + // {0.5 ,-0.5 ,0.5 //Front right motor(CCW) + // {-0.5 ,-0.5 ,-0.5 //rear right motor (CW) + // {-0.5 ,0.5 ,0.5 //Rear left motor (CCW) + double xMixer[8][3] = { + { 1, 1, -1 }, + { 1, -1, 1 }, + { -1, -1, -1 }, + { -1, 1, 1 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 } }; // Motor 1 to 4, P Layout: // pitch roll yaw - // {1 ,0 ,-0.5 //Front motor (CW) - // {0 ,-1 ,0.5 //Right motor(CCW) - // {-1 ,0 ,-0.5 //Rear motor (CW) - // {0 ,1 ,0.5 //Left motor (CCW) - double pMixer [8][3] = { - { 1, 0, -1}, - { 0, -1, 1}, - {-1, 0, -1}, - { 0, 1, 1}, - { 0, 0, 0}, - { 0, 0, 0}, - { 0, 0, 0}, - { 0, 0, 0} + // {1 ,0 ,-0.5 //Front motor (CW) + // {0 ,-1 ,0.5 //Right motor(CCW) + // {-1 ,0 ,-0.5 //Rear motor (CW) + // {0 ,1 ,0.5 //Left motor (CCW) + double pMixer[8][3] = { + { 1, 0, -1 }, + { 0, -1, 1 }, + { -1, 0, -1 }, + { 0, 1, 1 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 } }; if (pLayout) { @@ -897,7 +894,7 @@ bool ConfigMultiRotorWidget::setupQuad(bool pLayout) } /** - Set up a Hexa-X or Hexa-P mixer + Set up a Hexa-X or Hexa-P mixer */ bool ConfigMultiRotorWidget::setupHexa(bool pLayout) { @@ -918,22 +915,22 @@ bool ConfigMultiRotorWidget::setupHexa(bool pLayout) // and set only the relevant channels: // Motor 1 to 6, P Layout: - // pitch roll yaw - // 1 { 0.3 , 0 ,-0.3 // N CW - // 2 { 0.3 ,-0.5 , 0.3 // NE CCW - // 3 {-0.3 ,-0.5 ,-0.3 // SE CW - // 4 {-0.3 , 0 , 0.3 // S CCW - // 5 {-0.3 , 0.5 ,-0.3 // SW CW - // 6 { 0.3 , 0.5 , 0.3 // NW CCW - double pMixer [8][3] = { - { 1, 0, -1}, - { 1, -1, 1}, - {-1, -1, -1}, - {-1, 0, 1}, - {-1, 1, -1}, - { 1, 1, 1}, - { 0, 0, 0}, - { 0, 0, 0} + // pitch roll yaw + // 1 { 0.3 , 0 ,-0.3 // N CW + // 2 { 0.3 ,-0.5 , 0.3 // NE CCW + // 3 {-0.3 ,-0.5 ,-0.3 // SE CW + // 4 {-0.3 , 0 , 0.3 // S CCW + // 5 {-0.3 , 0.5 ,-0.3 // SW CW + // 6 { 0.3 , 0.5 , 0.3 // NW CCW + double pMixer[8][3] = { + { 1, 0, -1 }, + { 1, -1, 1 }, + { -1, -1, -1 }, + { -1, 0, 1 }, + { -1, 1, -1 }, + { 1, 1, 1 }, + { 0, 0, 0 }, + { 0, 0, 0 } }; // Motor 1 to 6, X Layout: @@ -943,15 +940,15 @@ bool ConfigMultiRotorWidget::setupHexa(bool pLayout) // 4 [ -0.5, 0.3, 0.3 ] SW // 5 [ 0 , 0.3, -0.3 ] W // 6 [ 0.5, 0.3, 0.3 ] NW - double xMixer [8][3] = { - { 1, -1, -1}, - { 0, -1, 1}, - { -1, -1, -1}, - { -1, 1, 1}, - { 0, 1, -1}, - { 1, 1, 1}, - { 0, 0, 0}, - { 0, 0, 0} + double xMixer[8][3] = { + { 1, -1, -1 }, + { 0, -1, 1 }, + { -1, -1, -1 }, + { -1, 1, 1 }, + { 0, 1, -1 }, + { 1, 1, 1 }, + { 0, 0, 0 }, + { 0, 0, 0 } }; if (pLayout) { @@ -965,31 +962,32 @@ bool ConfigMultiRotorWidget::setupHexa(bool pLayout) /** - This function sets up the multirotor mixer values. + This function sets up the multirotor mixer values. */ bool ConfigMultiRotorWidget::setupMultiRotorMixer(double mixerFactors[8][3]) { - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); // and enable only the relevant channels: - double pFactor = (double) m_aircraft->mrPitchMixLevel->value() / 100.0; - double rFactor = (double) m_aircraft->mrRollMixLevel->value() / 100.0; + double pFactor = (double)m_aircraft->mrPitchMixLevel->value() / 100.0; + double rFactor = (double)m_aircraft->mrRollMixLevel->value() / 100.0; invertMotors = m_aircraft->MultirotorRevMixerCheckBox->isChecked(); - double yFactor = (invertMotors ? -1.0 : 1.0) * (double) m_aircraft->mrYawMixLevel->value() / 100.0; + double yFactor = (invertMotors ? -1.0 : 1.0) * (double)m_aircraft->mrYawMixLevel->value() / 100.0; - QList mmList; + QList mmList; mmList << m_aircraft->multiMotorChannelBox1 << m_aircraft->multiMotorChannelBox2 - << m_aircraft->multiMotorChannelBox3 << m_aircraft->multiMotorChannelBox4 - << m_aircraft->multiMotorChannelBox5 << m_aircraft->multiMotorChannelBox6 - << m_aircraft->multiMotorChannelBox7 << m_aircraft->multiMotorChannelBox8; + << m_aircraft->multiMotorChannelBox3 << m_aircraft->multiMotorChannelBox4 + << m_aircraft->multiMotorChannelBox5 << m_aircraft->multiMotorChannelBox6 + << m_aircraft->multiMotorChannelBox7 << m_aircraft->multiMotorChannelBox8; for (int i = 0; i < 8; i++) { if (mmList.at(i)->isEnabled()) { int channel = mmList.at(i)->currentIndex() - 1; if (channel > -1) { setupQuadMotor(channel, mixerFactors[i][0] * pFactor, rFactor * mixerFactors[i][1], - yFactor * mixerFactors[i][2]); + yFactor * mixerFactors[i][2]); } } } @@ -997,7 +995,7 @@ bool ConfigMultiRotorWidget::setupMultiRotorMixer(double mixerFactors[8][3]) } /** - This function displays text and color formatting in order to help the user understand what channels have not yet been configured. + This function displays text and color formatting in order to help the user understand what channels have not yet been configured. */ bool ConfigMultiRotorWidget::throwConfigError(int numMotors) { @@ -1006,30 +1004,30 @@ bool ConfigMultiRotorWidget::throwConfigError(int numMotors) // Iterate through all instances of multiMotorChannelBox for (int i = 0; i < numMotors; i++) { - //Fine widgets with text "multiMotorChannelBox.x", where x is an integer - QComboBox *combobox = qFindChild(this, "multiMotorChannelBox" + QString::number(i + 1)); + // Fine widgets with text "multiMotorChannelBox.x", where x is an integer + QComboBox *combobox = qFindChild(this, "multiMotorChannelBox" + QString::number(i + 1)); if (combobox) { if (combobox->currentText() == "None") { int size = combobox->style()->pixelMetric(QStyle::PM_SmallIconSize); QPixmap pixmap(size, size); pixmap.fill(QColor("red")); - combobox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + combobox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - combobox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + combobox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } } } if (error) { m_aircraft->mrStatusLabel->setText( - QString("ERROR: Assign all %1 motor channels").arg(numMotors)); + QString("ERROR: Assign all %1 motor channels").arg(numMotors)); } return error; } /** - WHAT DOES THIS DO??? + WHAT DOES THIS DO??? */ void ConfigMultiRotorWidget::showEvent(QShowEvent *event) { @@ -1041,7 +1039,7 @@ void ConfigMultiRotorWidget::showEvent(QShowEvent *event) } /** - Resize the GUI contents when the user changes the window size + Resize the GUI contents when the user changes the window size */ void ConfigMultiRotorWidget::resizeEvent(QResizeEvent *event) { @@ -1052,7 +1050,8 @@ void ConfigMultiRotorWidget::resizeEvent(QResizeEvent *event) void ConfigMultiRotorWidget::enableControls(bool enable) { ConfigTaskWidget::enableControls(enable); - if(enable) { + + if (enable) { setupEnabledControls(m_aircraft->multirotorFrameType->currentText()); } } diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h index 75c251777..e9c2c2c70 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h @@ -41,8 +41,7 @@ class Ui_Widget; -class ConfigMultiRotorWidget: public VehicleConfig -{ +class ConfigMultiRotorWidget : public VehicleConfig { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp index e70ee0835..21c892aed 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp @@ -1,4 +1,3 @@ - /** ****************************************************************************** * @@ -37,38 +36,39 @@ VehicleConfig::VehicleConfig(QWidget *parent) : ConfigTaskWidget(parent) { // Generate lists of mixerTypeNames, mixerVectorNames, channelNames channelNames << "None"; - for (int i = 0; i < (int) VehicleConfig::CHANNEL_NUMELEM; i++) { + for (int i = 0; i < (int)VehicleConfig::CHANNEL_NUMELEM; i++) { mixerTypes << QString("Mixer%1Type").arg(i + 1); mixerVectors << QString("Mixer%1Vector").arg(i + 1); channelNames << QString("Channel%1").arg(i + 1); } mixerTypeDescriptions << "Disabled" << "Motor" << "Servo" << "CameraRoll" << "CameraPitch" << "CameraYaw" - << "Accessory0" << "Accessory1" << "Accessory2" << "Accessory3" << "Accessory4" << "Accessory5"; + << "Accessory0" << "Accessory1" << "Accessory2" << "Accessory3" << "Accessory4" << "Accessory5"; // This is needed because new style tries to compact things as much as possible in grid // and on OSX the widget sizes of PushButtons is reported incorrectly: // https://bugreports.qt-project.org/browse/QTBUG-14591 - foreach(QPushButton *btn, findChildren()) { + foreach(QPushButton * btn, findChildren()) { btn->setAttribute(Qt::WA_LayoutUsesWidgetRect); } } VehicleConfig::~VehicleConfig() { - // Do nothing + // Do nothing } GUIConfigDataUnion VehicleConfig::getConfigData() { // get an instance of systemsettings SystemSettings *systemSettings = SystemSettings::GetInstance(getUAVObjectManager()); + Q_ASSERT(systemSettings); SystemSettings::DataFields systemSettingsData = systemSettings->getData(); // copy systemsettings -> local configData GUIConfigDataUnion configData; - for (int i = 0; i < (int) SystemSettings::GUICONFIGDATA_NUMELEM; i++) { + for (int i = 0; i < (int)SystemSettings::GUICONFIGDATA_NUMELEM; i++) { configData.UAVObject[i] = systemSettingsData.GUIConfigData[i]; } @@ -95,7 +95,7 @@ void VehicleConfig::setConfigData(GUIConfigDataUnion configData) } // copy parameter configData -> systemsettings - for (int i = 0; i < (int) SystemSettings::GUICONFIGDATA_NUMELEM; i++) { + for (int i = 0; i < (int)SystemSettings::GUICONFIGDATA_NUMELEM; i++) { guiConfig->setValue(configData.UAVObject[i], i); } } @@ -121,9 +121,7 @@ void VehicleConfig::refreshWidgetsValues(UAVObject *o) } void VehicleConfig::updateObjectsFromWidgets() -{ - -} +{} void VehicleConfig::resetActuators(GUIConfigDataUnion *configData) { @@ -131,7 +129,8 @@ void VehicleConfig::resetActuators(GUIConfigDataUnion *configData) } -void VehicleConfig::registerWidgets(ConfigTaskWidget &parent) { +void VehicleConfig::registerWidgets(ConfigTaskWidget &parent) +{ Q_UNUSED(parent); } @@ -141,15 +140,15 @@ void VehicleConfig::registerWidgets(ConfigTaskWidget &parent) { void VehicleConfig::populateChannelComboBoxes() { QList l = findChildren(QRegExp("\\S+ChannelBo\\S+")); - foreach(QComboBox *combobox, l) { + foreach(QComboBox * combobox, l) { combobox->addItems(channelNames); } } /** - Helper function: - Sets the current index on supplied combobox to index - if it is within bounds 0 <= index < combobox.count() + Helper function: + Sets the current index on supplied combobox to index + if it is within bounds 0 <= index < combobox.count() */ void VehicleConfig::setComboCurrentIndex(QComboBox *box, int index) { @@ -160,13 +159,13 @@ void VehicleConfig::setComboCurrentIndex(QComboBox *box, int index) } /** - Helper function: - enables/disables the named comboboxes within supplied owner + Helper function: + enables/disables the named comboboxes within supplied owner */ void VehicleConfig::enableComboBoxes(QWidget *owner, QString boxName, int boxCount, bool enable) { for (int i = 1; i <= boxCount; i++) { - QComboBox* box = qFindChild(owner, QString("%0%1").arg(boxName).arg(i)); + QComboBox *box = qFindChild(owner, QString("%0%1").arg(boxName).arg(i)); if (box) { box->setEnabled(enable); } @@ -224,7 +223,7 @@ void VehicleConfig::resetMixerVector(UAVDataObject *mixer, int channel) // Disable all servo/motor mixers (but keep camera and accessory ones) void VehicleConfig::resetMotorAndServoMixers(UAVDataObject *mixer) { - for (int channel = 0; channel < (int) VehicleConfig::CHANNEL_NUMELEM; channel++) { + for (int channel = 0; channel < (int)VehicleConfig::CHANNEL_NUMELEM; channel++) { QString type = getMixerType(mixer, channel); if ((type == "Disabled") || (type == "Motor") || (type == "Servo")) { setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_DISABLED); @@ -298,7 +297,7 @@ void VehicleConfig::setThrottleCurve(UAVDataObject *mixer, MixerThrottleCurveEle break; } - if (field && (field->getNumElements() == (unsigned int) curve.length())) { + if (field && (field->getNumElements() == (unsigned int)curve.length())) { for (int i = 0; i < curve.length(); i++) { field->setValue(curve.at(i), i); } @@ -335,8 +334,9 @@ bool VehicleConfig::isValidThrottleCurve(QList *curve) if (curve) { for (int i = 0; i < curve->count(); i++) { - if (curve->at(i) != 0) + if (curve->at(i) != 0) { return true; + } } } return false; @@ -346,6 +346,7 @@ double VehicleConfig::getCurveMin(QList *curve) { // TODO initialize to max double double min = 0; + for (int i = 0; i < curve->count(); i++) { min = std::min(min, curve->at(i)); } @@ -356,6 +357,7 @@ double VehicleConfig::getCurveMax(QList *curve) { // TODO initialize to min double double max = 0; + for (int i = 0; i < curve->count(); i++) { max = std::max(max, curve->at(i)); } @@ -363,9 +365,9 @@ double VehicleConfig::getCurveMax(QList *curve) } /** - Reset the contents of a field - */ -void VehicleConfig::resetField(UAVObjectField * field) + Reset the contents of a field + */ +void VehicleConfig::resetField(UAVObjectField *field) { for (unsigned int i = 0; i < field->getNumElements(); i++) { field->setValue(0, i); @@ -380,6 +382,7 @@ UAVObjectManager *VehicleConfig::getUAVObjectManager() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objMngr = pm->getObject(); + Q_ASSERT(objMngr); return objMngr; } diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h index f116d05b9..b767bc081 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h @@ -34,71 +34,70 @@ #include "actuatorcommand.h" typedef struct { - uint VTOLMotorN:4; - uint VTOLMotorS:4; - uint VTOLMotorE:4; - uint VTOLMotorW:4; - uint VTOLMotorNW:4; - uint VTOLMotorNE:4; - uint VTOLMotorSW:4; - uint VTOLMotorSE:4; // 32 bits - uint TRIYaw:4; - quint32 padding:28; // 64 bits + uint VTOLMotorN : 4; + uint VTOLMotorS : 4; + uint VTOLMotorE : 4; + uint VTOLMotorW : 4; + uint VTOLMotorNW : 4; + uint VTOLMotorNE : 4; + uint VTOLMotorSW : 4; + uint VTOLMotorSE : 4; // 32 bits + uint TRIYaw : 4; + quint32 padding : 28; // 64 bits quint32 padding1; quint32 padding2; // 128 bits } __attribute__((packed)) multiGUISettingsStruct; typedef struct { - uint SwashplateType:3; - uint FirstServoIndex:2; - uint CorrectionAngle:9; - uint ccpmCollectivePassthroughState:1; - uint ccpmLinkCyclicState:1; - uint ccpmLinkRollState:1; - uint SliderValue0:7; - uint SliderValue1:7; - uint SliderValue2:7; // 41 bits - uint ServoIndexW:4; - uint ServoIndexX:4; - uint ServoIndexY:4; - uint ServoIndexZ:4; // 57 bits - uint Throttle:4; - uint Tail:4; // 65bits - quint32 padding:31; // 96 bits + uint SwashplateType : 3; + uint FirstServoIndex : 2; + uint CorrectionAngle : 9; + uint ccpmCollectivePassthroughState : 1; + uint ccpmLinkCyclicState : 1; + uint ccpmLinkRollState : 1; + uint SliderValue0 : 7; + uint SliderValue1 : 7; + uint SliderValue2 : 7; // 41 bits + uint ServoIndexW : 4; + uint ServoIndexX : 4; + uint ServoIndexY : 4; + uint ServoIndexZ : 4; // 57 bits + uint Throttle : 4; + uint Tail : 4; // 65bits + quint32 padding : 31; // 96 bits quint32 padding1; // 128 bits } __attribute__((packed)) heliGUISettingsStruct; typedef struct { - uint FixedWingThrottle:4; - uint FixedWingRoll1:4; - uint FixedWingRoll2:4; - uint FixedWingPitch1:4; - uint FixedWingPitch2:4; - uint FixedWingYaw1:4; - uint FixedWingYaw2:4; - uint padding:4; // 32 bits + uint FixedWingThrottle : 4; + uint FixedWingRoll1 : 4; + uint FixedWingRoll2 : 4; + uint FixedWingPitch1 : 4; + uint FixedWingPitch2 : 4; + uint FixedWingYaw1 : 4; + uint FixedWingYaw2 : 4; + uint padding : 4; // 32 bits quint32 padding1; quint32 padding2; quint32 padding3; // 128 bits } __attribute__((packed)) fixedGUISettingsStruct; typedef struct { - uint GroundVehicleThrottle1:4; - uint GroundVehicleThrottle2:4; - uint GroundVehicleSteering1:4; - uint GroundVehicleSteering2:4; - uint padding:16; // 32 bits + uint GroundVehicleThrottle1 : 4; + uint GroundVehicleThrottle2 : 4; + uint GroundVehicleSteering1 : 4; + uint GroundVehicleSteering2 : 4; + uint padding : 16; // 32 bits quint32 padding1; quint32 padding2; quint32 padding3; // 128 bits } __attribute__((packed)) groundGUISettingsStruct; -typedef union -{ +typedef union { uint UAVObject[4]; // 32 bits * 4 - heliGUISettingsStruct heli; // 128 bits - fixedGUISettingsStruct fixedwing; - multiGUISettingsStruct multi; + heliGUISettingsStruct heli; // 128 bits + fixedGUISettingsStruct fixedwing; + multiGUISettingsStruct multi; groundGUISettingsStruct ground; } GUIConfigDataUnion; @@ -110,9 +109,8 @@ class ConfigTaskWidget; * This class derives from ConfigTaskWidget and overrides its the default "binding" mechanism. * It does not use the "dirty" state management directlyand registers its relevant widgets with ConfigTaskWidget to do so. */ -class VehicleConfig: public ConfigTaskWidget -{ -Q_OBJECT +class VehicleConfig : public ConfigTaskWidget { + Q_OBJECT public: @@ -123,27 +121,27 @@ public: /* Enumeration options for field MixerType */ typedef enum { - MIXERTYPE_DISABLED = 0, - MIXERTYPE_MOTOR = 1, - MIXERTYPE_SERVO = 2, - MIXERTYPE_CAMERAROLL = 3, + MIXERTYPE_DISABLED = 0, + MIXERTYPE_MOTOR = 1, + MIXERTYPE_SERVO = 2, + MIXERTYPE_CAMERAROLL = 3, MIXERTYPE_CAMERAPITCH = 4, - MIXERTYPE_CAMERAYAW = 5, - MIXERTYPE_ACCESSORY0 = 6, - MIXERTYPE_ACCESSORY1 = 7, - MIXERTYPE_ACCESSORY2 = 8, - MIXERTYPE_ACCESSORY3 = 9, - MIXERTYPE_ACCESSORY4 = 10, - MIXERTYPE_ACCESSORY5 = 11 + MIXERTYPE_CAMERAYAW = 5, + MIXERTYPE_ACCESSORY0 = 6, + MIXERTYPE_ACCESSORY1 = 7, + MIXERTYPE_ACCESSORY2 = 8, + MIXERTYPE_ACCESSORY3 = 9, + MIXERTYPE_ACCESSORY4 = 10, + MIXERTYPE_ACCESSORY5 = 11 } MixerTypeElem; /* Array element names for field MixerVector */ typedef enum { MIXERVECTOR_THROTTLECURVE1 = 0, MIXERVECTOR_THROTTLECURVE2 = 1, - MIXERVECTOR_ROLL = 2, + MIXERVECTOR_ROLL = 2, MIXERVECTOR_PITCH = 3, - MIXERVECTOR_YAW = 4 + MIXERVECTOR_YAW = 4 } MixerVectorElem; static const quint32 CHANNEL_NUMELEM = ActuatorCommand::CHANNEL_NUMELEM;; @@ -183,7 +181,7 @@ protected: QString getMixerType(UAVDataObject *mixer, int channel); void setMixerType(UAVDataObject *mixer, int channel, MixerTypeElem mixerType); void setThrottleCurve(UAVDataObject *mixer, MixerThrottleCurveElem curveType, QList curve); - void getThrottleCurve(UAVDataObject *mixer, MixerThrottleCurveElem curveType, QList* curve); + void getThrottleCurve(UAVDataObject *mixer, MixerThrottleCurveElem curveType, QList *curve); bool isValidThrottleCurve(QList *curve); double getCurveMin(QList *curve); double getCurveMax(QList *curve); @@ -199,7 +197,6 @@ private: private slots: virtual void setupUI(QString airframeType); - }; #endif // VEHICLECONFIG_H diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp index d6dc18dde..f0762ff51 100644 --- a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp @@ -44,13 +44,14 @@ ConfigCCHWWidget::ConfigCCHWWidget(QWidget *parent) : ConfigTaskWidget(parent) m_telemetry = new Ui_CC_HW_Widget(); m_telemetry->setupUi(this); - ExtensionSystem::PluginManager *pm=ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); - if(!settings->useExpertMode()) + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { m_telemetry->saveTelemetryToRAM->setVisible(false); + } - UAVObjectUtilManager* utilMngr = pm->getObject(); + UAVObjectUtilManager *utilMngr = pm->getObject(); int id = utilMngr->getBoardModel(); switch (id) { @@ -73,16 +74,16 @@ ConfigCCHWWidget::ConfigCCHWWidget(QWidget *parent) : ConfigTaskWidget(parent) m_telemetry->label_2->setPixmap(QPixmap(":/configgadget/images/coptercontrol.svg")); break; } - addApplySaveButtons(m_telemetry->saveTelemetryToRAM,m_telemetry->saveTelemetryToSD); - addUAVObjectToWidgetRelation("HwSettings","CC_FlexiPort",m_telemetry->cbFlexi); - addUAVObjectToWidgetRelation("HwSettings","CC_MainPort",m_telemetry->cbTele); - addUAVObjectToWidgetRelation("HwSettings","CC_RcvrPort",m_telemetry->cbRcvr); - addUAVObjectToWidgetRelation("HwSettings","USB_HIDPort",m_telemetry->cbUsbHid); - addUAVObjectToWidgetRelation("HwSettings","USB_VCPPort",m_telemetry->cbUsbVcp); - addUAVObjectToWidgetRelation("HwSettings","TelemetrySpeed",m_telemetry->telemetrySpeed); - addUAVObjectToWidgetRelation("HwSettings","GPSSpeed",m_telemetry->gpsSpeed); - addUAVObjectToWidgetRelation("HwSettings","ComUsbBridgeSpeed",m_telemetry->comUsbBridgeSpeed); - connect(m_telemetry->cchwHelp,SIGNAL(clicked()),this,SLOT(openHelp())); + addApplySaveButtons(m_telemetry->saveTelemetryToRAM, m_telemetry->saveTelemetryToSD); + addUAVObjectToWidgetRelation("HwSettings", "CC_FlexiPort", m_telemetry->cbFlexi); + addUAVObjectToWidgetRelation("HwSettings", "CC_MainPort", m_telemetry->cbTele); + addUAVObjectToWidgetRelation("HwSettings", "CC_RcvrPort", m_telemetry->cbRcvr); + addUAVObjectToWidgetRelation("HwSettings", "USB_HIDPort", m_telemetry->cbUsbHid); + addUAVObjectToWidgetRelation("HwSettings", "USB_VCPPort", m_telemetry->cbUsbVcp); + addUAVObjectToWidgetRelation("HwSettings", "TelemetrySpeed", m_telemetry->telemetrySpeed); + addUAVObjectToWidgetRelation("HwSettings", "GPSSpeed", m_telemetry->gpsSpeed); + addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_telemetry->comUsbBridgeSpeed); + connect(m_telemetry->cchwHelp, SIGNAL(clicked()), this, SLOT(openHelp())); enableControls(false); populateWidgets(); refreshWidgetsValues(); @@ -95,42 +96,33 @@ ConfigCCHWWidget::~ConfigCCHWWidget() } void ConfigCCHWWidget::refreshValues() -{ -} +{} void ConfigCCHWWidget::widgetsContentsChanged() { ConfigTaskWidget::widgetsContentsChanged(); + if (((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE) && - (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE)) || - ((m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE) && - (m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE)) || - ((m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) && - (m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE))) - { + (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE)) || + ((m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE) && + (m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE)) || + ((m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) && + (m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE))) { enableControls(false); m_telemetry->problems->setText(tr("Warning: you have configured more than one DebugConsole, this currently is not supported")); - } - else if (((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_TELEMETRY) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_TELEMETRY)) || - ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_GPS) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_GPS)) || - ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE)) || - ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_COMBRIDGE) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_COMBRIDGE))) - { + } else if (((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_TELEMETRY) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_TELEMETRY)) || + ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_GPS) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_GPS)) || + ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE)) || + ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_COMBRIDGE) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_COMBRIDGE))) { enableControls(false); m_telemetry->problems->setText(tr("Warning: you have configured both MainPort and FlexiPort for the same function, this currently is not supported")); - } - else if ((m_telemetry->cbUsbHid->currentIndex() == HwSettings::USB_HIDPORT_USBTELEMETRY) && (m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_USBTELEMETRY)) - { + } else if ((m_telemetry->cbUsbHid->currentIndex() == HwSettings::USB_HIDPORT_USBTELEMETRY) && (m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_USBTELEMETRY)) { enableControls(false); m_telemetry->problems->setText(tr("Warning: you have configured both USB HID Port and USB VCP Port for the same function, this currently is not supported")); - } - else if ((m_telemetry->cbUsbHid->currentIndex() != HwSettings::USB_HIDPORT_USBTELEMETRY) && (m_telemetry->cbUsbVcp->currentIndex() != HwSettings::USB_VCPPORT_USBTELEMETRY)) - { + } else if ((m_telemetry->cbUsbHid->currentIndex() != HwSettings::USB_HIDPORT_USBTELEMETRY) && (m_telemetry->cbUsbVcp->currentIndex() != HwSettings::USB_VCPPORT_USBTELEMETRY)) { enableControls(false); m_telemetry->problems->setText(tr("Warning: you have disabled USB Telemetry on both USB HID Port and USB VCP Port, this currently is not supported")); - } - else - { + } else { m_telemetry->problems->setText(""); enableControls(true); } @@ -138,10 +130,10 @@ void ConfigCCHWWidget::widgetsContentsChanged() void ConfigCCHWWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/D4AUAQ", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/D4AUAQ", QUrl::StrictMode)); } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h index 7c4945281..503955f0b 100644 --- a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h @@ -36,8 +36,7 @@ #include #include "smartsavebutton.h" -class ConfigCCHWWidget: public ConfigTaskWidget -{ +class ConfigCCHWWidget : public ConfigTaskWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp index 63de056ad..e395aaecf 100644 --- a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp @@ -1,4 +1,3 @@ - #include "configautotunewidget.h" #include @@ -34,22 +33,25 @@ ConfigAutotuneWidget::ConfigAutotuneWidget(QWidget *parent) : RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager()); Q_ASSERT(relayTuning); - if(relayTuning) - connect(relayTuning, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(recomputeStabilization())); + if (relayTuning) { + connect(relayTuning, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(recomputeStabilization())); + } // Connect the apply button for the stabilization settings connect(m_autotune->useComputedValues, SIGNAL(pressed()), this, SLOT(saveStabilization())); } /** - * Apply the stabilization settings computed - */ + * Apply the stabilization settings computed + */ void ConfigAutotuneWidget::saveStabilization() { StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager()); + Q_ASSERT(stabilizationSettings); - if(!stabilizationSettings) + if (!stabilizationSettings) { return; + } // Make sure to recompute in case the other stab settings changed since // the last time @@ -61,25 +63,29 @@ void ConfigAutotuneWidget::saveStabilization() } /** - * Called whenever the gain ratios or measured values - * are changed - */ + * Called whenever the gain ratios or measured values + * are changed + */ void ConfigAutotuneWidget::recomputeStabilization() { RelayTuningSettings *relayTuningSettings = RelayTuningSettings::GetInstance(getObjectManager()); + Q_ASSERT(relayTuningSettings); - if (!relayTuningSettings) + if (!relayTuningSettings) { return; + } RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager()); Q_ASSERT(relayTuning); - if(!relayTuning) + if (!relayTuning) { return; + } StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager()); Q_ASSERT(stabilizationSettings); - if(!stabilizationSettings) + if (!stabilizationSettings) { return; + } RelayTuning::DataFields relayTuningData = relayTuning->getData(); RelayTuningSettings::DataFields tuningSettingsData = relayTuningSettings->getData(); @@ -94,24 +100,25 @@ void ConfigAutotuneWidget::recomputeStabilization() // For now just run over roll and pitch for (int i = 0; i < 2; i++) { - if (relayTuningData.Period[i] == 0 || relayTuningData.Gain[i] == 0) + if (relayTuningData.Period[i] == 0 || relayTuningData.Gain[i] == 0) { continue; + } - double wu = 1000.0 * 2 * M_PI / relayTuningData.Period[i]; // ultimate freq = output osc freq (rad/s) + double wu = 1000.0 * 2 * M_PI / relayTuningData.Period[i]; // ultimate freq = output osc freq (rad/s) - double wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s) - double zc = wc * zero_ratio_r; // controller zero location (rad/s) - double kpu = 4.0f / M_PI / relayTuningData.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity - double kp = kpu * gain_ratio_r; // proportional gain - double ki = zc * kp; // integral gain + double wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s) + double zc = wc * zero_ratio_r; // controller zero location (rad/s) + double kpu = 4.0f / M_PI / relayTuningData.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity + double kp = kpu * gain_ratio_r; // proportional gain + double ki = zc * kp; // integral gain // Now calculate gains for the next loop out knowing it is the integral of // the inner loop -- the plant is position/velocity = scale*1/s - double wc2 = wc * gain_ratio_p; // crossover of the attitude loop - double kp2 = wc2; // kp of attitude - double ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude + double wc2 = wc * gain_ratio_p; // crossover of the attitude loop + double kp2 = wc2; // kp of attitude + double ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude - switch(i) { + switch (i) { case 0: // Roll stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KP] = kp; @@ -141,9 +148,9 @@ void ConfigAutotuneWidget::recomputeStabilization() void ConfigAutotuneWidget::refreshWidgetsValues(UAVObject *obj) { HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); - if(obj==hwSettings) - { - bool dirtyBack=isDirty(); + + if (obj == hwSettings) { + bool dirtyBack = isDirty(); HwSettings::DataFields hwSettingsData = hwSettings->getData(); m_autotune->enableAutoTune->setChecked( hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_AUTOTUNE] == HwSettings::OPTIONALMODULES_ENABLED); @@ -155,8 +162,9 @@ void ConfigAutotuneWidget::updateObjectsFromWidgets() { HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); HwSettings::DataFields hwSettingsData = hwSettings->getData(); + hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_AUTOTUNE] = - m_autotune->enableAutoTune->isChecked() ? HwSettings::OPTIONALMODULES_ENABLED : HwSettings::OPTIONALMODULES_DISABLED; + m_autotune->enableAutoTune->isChecked() ? HwSettings::OPTIONALMODULES_ENABLED : HwSettings::OPTIONALMODULES_DISABLED; hwSettings->setData(hwSettingsData); ConfigTaskWidget::updateObjectsFromWidgets(); } diff --git a/ground/openpilotgcs/src/plugins/config/configautotunewidget.h b/ground/openpilotgcs/src/plugins/config/configautotunewidget.h index 466335d04..777c24b3f 100644 --- a/ground/openpilotgcs/src/plugins/config/configautotunewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.h @@ -38,8 +38,7 @@ #include #include -class ConfigAutotuneWidget : public ConfigTaskWidget -{ +class ConfigAutotuneWidget : public ConfigTaskWidget { Q_OBJECT public: explicit ConfigAutotuneWidget(QWidget *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp index 4b8e445e5..829a40608 100644 --- a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp @@ -45,15 +45,15 @@ ConfigCameraStabilizationWidget::ConfigCameraStabilizationWidget(QWidget *parent { ui = new Ui_CameraStabilizationWidget(); ui->setupUi(this); - - addApplySaveButtons(ui->camerastabilizationSaveRAM,ui->camerastabilizationSaveSD); - - ExtensionSystem::PluginManager *pm=ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); - if(!settings->useExpertMode()) + + addApplySaveButtons(ui->camerastabilizationSaveRAM, ui->camerastabilizationSaveSD); + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { ui->camerastabilizationSaveRAM->setVisible(false); - - + } + // These widgets don't have direct relation to UAVObjects // and need special processing @@ -61,7 +61,6 @@ ConfigCameraStabilizationWidget::ConfigCameraStabilizationWidget(QWidget *parent ui->rollChannel, ui->pitchChannel, ui->yawChannel, - }; const int NUM_OUTPUTS = sizeof(outputs) / sizeof(outputs[0]); @@ -69,8 +68,9 @@ ConfigCameraStabilizationWidget::ConfigCameraStabilizationWidget(QWidget *parent for (int i = 0; i < NUM_OUTPUTS; i++) { outputs[i]->clear(); outputs[i]->addItem("None"); - for (quint32 j = 0; j < ActuatorCommand::CHANNEL_NUMELEM; j++) - outputs[i]->addItem(QString("Channel %1").arg(j+1)); + for (quint32 j = 0; j < ActuatorCommand::CHANNEL_NUMELEM; j++) { + outputs[i]->addItem(QString("Channel %1").arg(j + 1)); + } } // Load UAVObjects to widget relations from UI file @@ -102,7 +102,7 @@ ConfigCameraStabilizationWidget::ConfigCameraStabilizationWidget(QWidget *parent ConfigCameraStabilizationWidget::~ConfigCameraStabilizationWidget() { - // Do nothing + // Do nothing } /* @@ -155,10 +155,12 @@ void ConfigCameraStabilizationWidget::refreshWidgetsValues(UAVObject *obj) // Default to none if not found. // Then search for any mixer channels set to this outputs[i]->setCurrentIndex(0); - for (int j = 0; j < NUM_MIXERS; j++) + for (int j = 0; j < NUM_MIXERS; j++) { if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i) && - outputs[i]->currentIndex() != (j + 1)) + outputs[i]->currentIndex() != (j + 1)) { outputs[i]->setCurrentIndex(j + 1); + } + } } setDirty(dirty); @@ -176,9 +178,10 @@ void ConfigCameraStabilizationWidget::updateObjectsFromWidgets() // Save state of the module enable checkbox first. // Do not use setData() member on whole object, if possible, since it triggers // unnessesary UAVObect update. - quint8 enableModule = ui->enableCameraStabilization->isChecked() ? - HwSettings::OPTIONALMODULES_ENABLED : HwSettings::OPTIONALMODULES_DISABLED; + quint8 enableModule = ui->enableCameraStabilization->isChecked() ? + HwSettings::OPTIONALMODULES_ENABLED : HwSettings::OPTIONALMODULES_DISABLED; HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); + hwSettings->setOptionalModules(HwSettings::OPTIONALMODULES_CAMERASTAB, enableModule); // Update mixer channels which were mapped to camera outputs in case they are @@ -220,7 +223,7 @@ void ConfigCameraStabilizationWidget::updateObjectsFromWidgets() if ((mixerNum >= 0) && // Short circuit in case of none (*mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_DISABLED) && - (*mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i) ) { + (*mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i)) { // If the mixer channel already mapped to something, it should not be // used for camera output, we reset it to none outputs[i]->setCurrentIndex(0); @@ -230,17 +233,20 @@ void ConfigCameraStabilizationWidget::updateObjectsFromWidgets() widgetUpdated = true; } else { // Make sure no other channels have this output set - for (int j = 0; j < NUM_MIXERS; j++) - if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i)) + for (int j = 0; j < NUM_MIXERS; j++) { + if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i)) { *mixerTypes[j] = MixerSettings::MIXER1TYPE_DISABLED; + } + } // If this channel is assigned to one of the outputs that is not disabled // set it - if ((mixerNum >= 0) && (mixerNum < NUM_MIXERS)) + if ((mixerNum >= 0) && (mixerNum < NUM_MIXERS)) { *mixerTypes[mixerNum] = MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i; + } } } - } while(widgetUpdated); + } while (widgetUpdated); // FIXME: Should not use setData() to prevent double updates. // It should be refactored after the reformatting of MixerSettings UAVObject. @@ -263,11 +269,11 @@ void ConfigCameraStabilizationWidget::defaultRequestedSlot(int group) // But if you want, you could use the dirtyClone() function to get default // values of an object and then use them to set a widget state. // - //HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); - //HwSettings *hwSettingsDefault=(HwSettings*)hwSettings->dirtyClone(); - //HwSettings::DataFields hwSettingsData = hwSettingsDefault->getData(); - //m_camerastabilization->enableCameraStabilization->setChecked( - // hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_CAMERASTAB] == HwSettings::OPTIONALMODULES_ENABLED); + // HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); + // HwSettings *hwSettingsDefault=(HwSettings*)hwSettings->dirtyClone(); + // HwSettings::DataFields hwSettingsData = hwSettingsDefault->getData(); + // m_camerastabilization->enableCameraStabilization->setChecked( + // hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_CAMERASTAB] == HwSettings::OPTIONALMODULES_ENABLED); // For outputs we set them all to none, so don't use any UAVObject to get defaults QComboBox *outputs[] = { diff --git a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h index 2715daa77..8c656f8ff 100644 --- a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h @@ -34,8 +34,7 @@ #include "uavobject.h" #include "camerastabsettings.h" -class ConfigCameraStabilizationWidget: public ConfigTaskWidget -{ +class ConfigCameraStabilizationWidget : public ConfigTaskWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index cd840e93e..79d69382e 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -39,30 +39,31 @@ #include ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) : - ConfigTaskWidget(parent), - ui(new Ui_ccattitude) + ConfigTaskWidget(parent), + ui(new Ui_ccattitude) { ui->setupUi(this); - forceConnectedState(); //dynamic widgets don't recieve the connected signal - connect(ui->zeroBias,SIGNAL(clicked()),this,SLOT(startAccelCalibration())); + forceConnectedState(); // dynamic widgets don't recieve the connected signal + connect(ui->zeroBias, SIGNAL(clicked()), this, SLOT(startAccelCalibration())); - ExtensionSystem::PluginManager *pm=ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); - if(!settings->useExpertMode()) + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { ui->applyButton->setVisible(false); - - addApplySaveButtons(ui->applyButton,ui->saveButton); + } + + addApplySaveButtons(ui->applyButton, ui->saveButton); addUAVObject("AttitudeSettings"); // Connect the help button connect(ui->ccAttitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp())); - addUAVObjectToWidgetRelation("AttitudeSettings","ZeroDuringArming",ui->zeroGyroBiasOnArming); + addUAVObjectToWidgetRelation("AttitudeSettings", "ZeroDuringArming", ui->zeroGyroBiasOnArming); addUAVObjectToWidgetRelation("AttitudeSettings", "AccelTau", ui->accelTauSpinbox); - addUAVObjectToWidgetRelation("AttitudeSettings","BoardRotation",ui->rollBias,AttitudeSettings::BOARDROTATION_ROLL); - addUAVObjectToWidgetRelation("AttitudeSettings","BoardRotation",ui->pitchBias,AttitudeSettings::BOARDROTATION_PITCH); - addUAVObjectToWidgetRelation("AttitudeSettings","BoardRotation",ui->yawBias,AttitudeSettings::BOARDROTATION_YAW); + addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", ui->rollBias, AttitudeSettings::BOARDROTATION_ROLL); + addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", ui->pitchBias, AttitudeSettings::BOARDROTATION_PITCH); + addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", ui->yawBias, AttitudeSettings::BOARDROTATION_YAW); addWidget(ui->zeroBias); refreshWidgetsValues(); } @@ -72,22 +73,22 @@ ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget() delete ui; } -void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { - - if (!timer.isActive()) { +void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj) +{ + if (!timer.isActive()) { // ignore updates that come in after the timer has expired return; } - Accels * accels = Accels::GetInstance(getObjectManager()); - Gyros * gyros = Gyros::GetInstance(getObjectManager()); + Accels *accels = Accels::GetInstance(getObjectManager()); + Gyros *gyros = Gyros::GetInstance(getObjectManager()); // Accumulate samples until we have _at least_ NUM_SENSOR_UPDATES samples // for both gyros and accels. // Note that, at present, we stash the samples and then compute the bias // at the end, even though the mean could be accumulated as we go. - // In future, a better algorithm could be used. - if(obj->getObjID() == Accels::OBJID) { + // In future, a better algorithm could be used. + if (obj->getObjID() == Accels::OBJID) { accelUpdates++; Accels::DataFields accelsData = accels->getData(); x_accum.append(accelsData.x); @@ -99,16 +100,16 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { x_gyro_accum.append(gyrosData.x); y_gyro_accum.append(gyrosData.y); z_gyro_accum.append(gyrosData.z); - } + } // update the progress indicator - ui->zeroBiasProgress->setValue((float) qMin(accelUpdates, gyroUpdates) / NUM_SENSOR_UPDATES * 100); + ui->zeroBiasProgress->setValue((float)qMin(accelUpdates, gyroUpdates) / NUM_SENSOR_UPDATES * 100); // If we have enough samples, then stop sampling and compute the biases if (accelUpdates >= NUM_SENSOR_UPDATES && gyroUpdates >= NUM_SENSOR_UPDATES) { timer.stop(); - disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); - disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); + disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); + disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); float x_bias = listMean(x_accum) / ACCEL_SCALE; float y_bias = listMean(y_accum) / ACCEL_SCALE; @@ -122,12 +123,12 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData(); // We offset the gyro bias by current bias to help precision - attitudeSettingsData.AccelBias[0] += x_bias; - attitudeSettingsData.AccelBias[1] += y_bias; - attitudeSettingsData.AccelBias[2] += z_bias; - attitudeSettingsData.GyroBias[0] = -x_gyro_bias; - attitudeSettingsData.GyroBias[1] = -y_gyro_bias; - attitudeSettingsData.GyroBias[2] = -z_gyro_bias; + attitudeSettingsData.AccelBias[0] += x_bias; + attitudeSettingsData.AccelBias[1] += y_bias; + attitudeSettingsData.AccelBias[2] += z_bias; + attitudeSettingsData.GyroBias[0] = -x_gyro_bias; + attitudeSettingsData.GyroBias[1] = -y_gyro_bias; + attitudeSettingsData.GyroBias[2] = -z_gyro_bias; attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); this->setDirty(true); @@ -137,13 +138,15 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { } } -void ConfigCCAttitudeWidget::timeout() { - UAVDataObject * obj = Accels::GetInstance(getObjectManager()); - disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); - disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); +void ConfigCCAttitudeWidget::timeout() +{ + UAVDataObject *obj = Accels::GetInstance(getObjectManager()); - Accels * accels = Accels::GetInstance(getObjectManager()); - Gyros * gyros = Gyros::GetInstance(getObjectManager()); + disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); + disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); + + Accels *accels = Accels::GetInstance(getObjectManager()); + Gyros *gyros = Gyros::GetInstance(getObjectManager()); accels->setMetadata(initialAccelsMdata); gyros->setMetadata(initialGyrosMdata); @@ -154,17 +157,18 @@ void ConfigCCAttitudeWidget::timeout() { msgBox.exec(); // reset progress indicator - ui->zeroBiasProgress->setValue(0); + ui->zeroBiasProgress->setValue(0); // reenable controls enableControls(true); } -void ConfigCCAttitudeWidget::startAccelCalibration() { +void ConfigCCAttitudeWidget::startAccelCalibration() +{ // disable controls during sampling enableControls(false); accelUpdates = 0; - gyroUpdates = 0; + gyroUpdates = 0; x_accum.clear(); y_accum.clear(); z_accum.clear(); @@ -178,10 +182,10 @@ void ConfigCCAttitudeWidget::startAccelCalibration() { AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); // Set up to receive updates - UAVDataObject * accels = Accels::GetInstance(getObjectManager()); - UAVDataObject * gyros = Gyros::GetInstance(getObjectManager()); - connect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); - connect(gyros,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); + UAVDataObject *accels = Accels::GetInstance(getObjectManager()); + UAVDataObject *gyros = Gyros::GetInstance(getObjectManager()); + connect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); + connect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); // Speed up updates initialAccelsMdata = accels->getMetadata(); @@ -200,13 +204,12 @@ void ConfigCCAttitudeWidget::startAccelCalibration() { timer.setSingleShot(true); timer.start(5000 + (NUM_SENSOR_UPDATES * qMax(accelsMdata.flightTelemetryUpdatePeriod, gyrosMdata.flightTelemetryUpdatePeriod))); - connect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); + connect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); } void ConfigCCAttitudeWidget::openHelp() { - - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/44Cf", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/44Cf", QUrl::StrictMode)); } void ConfigCCAttitudeWidget::setAccelFiltering(bool active) @@ -225,5 +228,6 @@ void ConfigCCAttitudeWidget::enableControls(bool enable) void ConfigCCAttitudeWidget::updateObjectsFromWidgets() { ConfigTaskWidget::updateObjectsFromWidgets(); + ui->zeroBiasProgress->setValue(0); } diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h index b171035d1..c9b835050 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h @@ -37,8 +37,7 @@ class Ui_Widget; -class ConfigCCAttitudeWidget : public ConfigTaskWidget -{ +class ConfigCCAttitudeWidget : public ConfigTaskWidget { Q_OBJECT public: @@ -48,7 +47,7 @@ public: virtual void updateObjectsFromWidgets(); private slots: - void sensorsUpdated(UAVObject * obj); + void sensorsUpdated(UAVObject *obj); void timeout(); void startAccelCalibration(); void openHelp(); @@ -71,7 +70,6 @@ private: static const float ACCEL_SCALE = 0.004f * 9.81f; protected: virtual void enableControls(bool enable); - }; #endif // CCATTITUDEWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configgadget.cpp b/ground/openpilotgcs/src/plugins/config/configgadget.cpp index dc1ca4721..013309e27 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadget.cpp @@ -28,9 +28,8 @@ #include "configgadgetwidget.h" ConfigGadget::ConfigGadget(QString classId, ConfigGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), m_widget(widget) -{ -} + IUAVGadget(classId, parent), m_widget(widget) +{} ConfigGadget::~ConfigGadget() { @@ -39,5 +38,5 @@ ConfigGadget::~ConfigGadget() void ConfigGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - Q_UNUSED(config); + Q_UNUSED(config); } diff --git a/ground/openpilotgcs/src/plugins/config/configgadget.h b/ground/openpilotgcs/src/plugins/config/configgadget.h index 6e617000b..23184b74e 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadget.h +++ b/ground/openpilotgcs/src/plugins/config/configgadget.h @@ -31,7 +31,7 @@ #include "../uavobjectwidgetutils/configtaskwidget.h" class IUAVGadget; -//class QList; +// class QList; class QWidget; class QString; class ConfigGadgetWidget; @@ -39,15 +39,17 @@ class Ui_ConfigGadget; using namespace Core; -class ConfigGadget : public Core::IUAVGadget -{ +class ConfigGadget : public Core::IUAVGadget { Q_OBJECT public: ConfigGadget(QString classId, ConfigGadgetWidget *widget, QWidget *parent = 0); ~ConfigGadget(); - QWidget *widget() { return (QWidget*)m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return (QWidget *)m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: ConfigGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetfactory.cpp index aef634008..a9add1da0 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetfactory.cpp @@ -34,23 +34,21 @@ ConfigGadgetFactory::ConfigGadgetFactory(QObject *parent) : IUAVGadgetFactory(QString("ConfigGadget"), tr("Config"), parent), gadgetWidget(0) -{ -} +{} ConfigGadgetFactory::~ConfigGadgetFactory() -{ -} +{} Core::IUAVGadget *ConfigGadgetFactory::createGadget(QWidget *parent) { gadgetWidget = new ConfigGadgetWidget(parent); // Add Menu entry - Core::ActionManager *am = Core::ICore::instance()->actionManager(); + Core::ActionManager *am = Core::ICore::instance()->actionManager(); Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_TOOLS); Core::Command *cmd = am->registerAction(new QAction(this), "ConfigPlugin.ShowInputWizard", - QList() << Core::Constants::C_GLOBAL_ID); + QList() << Core::Constants::C_GLOBAL_ID); cmd->setDefaultKeySequence(QKeySequence("Ctrl+R")); cmd->action()->setText(tr("Radio Setup Wizard")); diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetfactory.h b/ground/openpilotgcs/src/plugins/config/configgadgetfactory.h index a6bd08de6..936cb41b4 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetfactory.h @@ -38,9 +38,8 @@ class IUAVGadgetFactory; using namespace Core; -class CONFIG_EXPORT ConfigGadgetFactory: public Core::IUAVGadgetFactory -{ - Q_OBJECT +class CONFIG_EXPORT ConfigGadgetFactory : public Core::IUAVGadgetFactory { + Q_OBJECT public: ConfigGadgetFactory(QObject *parent = 0); ~ConfigGadgetFactory(); diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index 5f2f268ba..db3900e69 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -192,7 +192,7 @@ void ConfigGadgetWidget::onAutopilotConnect() // First of all, check what Board type we are talking to, and // if necessary, remove/add tabs in the config gadget: ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectUtilManager *utilMngr = pm->getObject(); + UAVObjectUtilManager *utilMngr = pm->getObject(); if (utilMngr) { int selectedIndex = ftw->currentIndex(); int board = utilMngr->getBoardModel(); diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h index 24209e1cf..3827f7e7e 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h @@ -40,21 +40,20 @@ #include "utils/mytabbedstackwidget.h" #include "../uavobjectwidgetutils/configtaskwidget.h" -class ConfigGadgetWidget: public QWidget -{ +class ConfigGadgetWidget : public QWidget { Q_OBJECT - QTextBrowser* help; + QTextBrowser *help; public: ConfigGadgetWidget(QWidget *parent = 0); ~ConfigGadgetWidget(); - enum widgetTabs {hardware=0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, oplink}; + enum widgetTabs { hardware = 0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, oplink }; void startInputWizard(); public slots: void onAutopilotConnect(); void onAutopilotDisconnect(); - void tabAboutToChange(int i,bool *); + void tabAboutToChange(int i, bool *); void updateOPLinkStatus(UAVObject *object); void onOPLinkDisconnect(); @@ -65,15 +64,15 @@ signals: void oplinkDisconnect(); protected: - void resizeEvent(QResizeEvent * event); - MyTabbedStackWidget *ftw; + void resizeEvent(QResizeEvent *event); + MyTabbedStackWidget *ftw; private: - UAVDataObject* oplinkStatusObj; + UAVDataObject *oplinkStatusObj; - // A timer that timesout the connction to the OPLink. - QTimer *oplinkTimeout; - bool oplinkConnected; + // A timer that timesout the connction to the OPLink. + QTimer *oplinkTimeout; + bool oplinkConnected; }; #endif // CONFIGGADGETWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 505c3a125..646130d9b 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -46,49 +46,49 @@ #define ACCESS_MIN_MOVE -3 #define ACCESS_MAX_MOVE 3 -#define STICK_MIN_MOVE -8 -#define STICK_MAX_MOVE 8 +#define STICK_MIN_MOVE -8 +#define STICK_MAX_MOVE 8 -ConfigInputWidget::ConfigInputWidget(QWidget *parent) : +ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent), wizardStep(wizardNone), // not currently stored in the settings UAVO - transmitterMode(mode2), - transmitterType(acro), + transmitterMode(mode2), + transmitterType(acro), // loop(NULL), skipflag(false) { - manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); - manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); - flightStatusObj = FlightStatus::GetInstance(getObjectManager()); - receiverActivityObj=ReceiverActivity::GetInstance(getObjectManager()); + manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); + manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); + flightStatusObj = FlightStatus::GetInstance(getObjectManager()); + receiverActivityObj = ReceiverActivity::GetInstance(getObjectManager()); ui = new Ui_InputWidget(); ui->setupUi(this); - - addApplySaveButtons(ui->saveRCInputToRAM,ui->saveRCInputToSD); - ExtensionSystem::PluginManager *pm=ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); - if(!settings->useExpertMode()) + addApplySaveButtons(ui->saveRCInputToRAM, ui->saveRCInputToSD); + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { ui->saveRCInputToRAM->setVisible(false); - - addApplySaveButtons(ui->saveRCInputToRAM,ui->saveRCInputToSD); + } - //Generate the rows of buttons in the input channel form GUI - unsigned int index=0; + addApplySaveButtons(ui->saveRCInputToRAM, ui->saveRCInputToSD); + + // Generate the rows of buttons in the input channel form GUI + unsigned int index = 0; unsigned int indexRT = 0; - foreach (QString name, manualSettingsObj->getField("ChannelNumber")->getElementNames()) - { + foreach(QString name, manualSettingsObj->getField("ChannelNumber")->getElementNames()) { Q_ASSERT(index < ManualControlSettings::CHANNELGROUPS_NUMELEM); - inputChannelForm * inpForm=new inputChannelForm(this,index==0); - ui->channelSettings->layout()->addWidget(inpForm); //Add the row to the UI + inputChannelForm *inpForm = new inputChannelForm(this, index == 0); + ui->channelSettings->layout()->addWidget(inpForm); // Add the row to the UI inpForm->setName(name); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelGroups",inpForm->ui->channelGroup,index); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNumber",inpForm->ui->channelNumber,index); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMin",inpForm->ui->channelMin,index); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNeutral",inpForm->ui->channelNeutral,index); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMax",inpForm->ui->channelMax,index); + addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelGroups", inpForm->ui->channelGroup, index); + addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelNumber", inpForm->ui->channelNumber, index); + addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelMin", inpForm->ui->channelMin, index); + addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelNeutral", inpForm->ui->channelNeutral, index); + addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelMax", inpForm->ui->channelMax, index); addWidget(inpForm->ui->channelNumberDropdown); addWidget(inpForm->ui->channelRev); addWidget(inpForm->ui->channelResponseTime); @@ -119,13 +119,13 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : addUAVObjectToWidgetRelation("ManualControlSettings", "Deadband", ui->deadband, 0, 0.01f); - connect(ui->configurationWizard,SIGNAL(clicked()),this,SLOT(goToWizard())); - connect(ui->stackedWidget,SIGNAL(currentChanged(int)),this,SLOT(disableWizardButton(int))); - connect(ui->runCalibration,SIGNAL(toggled(bool)),this, SLOT(simpleCalibration(bool))); + connect(ui->configurationWizard, SIGNAL(clicked()), this, SLOT(goToWizard())); + connect(ui->stackedWidget, SIGNAL(currentChanged(int)), this, SLOT(disableWizardButton(int))); + connect(ui->runCalibration, SIGNAL(toggled(bool)), this, SLOT(simpleCalibration(bool))); - connect(ui->wzNext,SIGNAL(clicked()),this,SLOT(wzNext())); - connect(ui->wzCancel,SIGNAL(clicked()),this,SLOT(wzCancel())); - connect(ui->wzBack,SIGNAL(clicked()),this,SLOT(wzBack())); + connect(ui->wzNext, SIGNAL(clicked()), this, SLOT(wzNext())); + connect(ui->wzCancel, SIGNAL(clicked()), this, SLOT(wzCancel())); + connect(ui->wzBack, SIGNAL(clicked()), this, SLOT(wzBack())); ui->stackedWidget->setCurrentIndex(0); addUAVObjectToWidgetRelation("ManualControlSettings", "FlightModePosition", ui->fmsModePos1, 0, 1, true); @@ -146,10 +146,10 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : addUAVObjectToWidgetRelation("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Yaw, "Yaw", 1, true); addUAVObjectToWidgetRelation("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Yaw, "Yaw", 1, true); - addUAVObjectToWidgetRelation("ManualControlSettings","Arming",ui->armControl); - addUAVObjectToWidgetRelation("ManualControlSettings","ArmedTimeout",ui->armTimeout,0,1000); - connect( ManualControlCommand::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(moveFMSlider())); - connect( ManualControlSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updatePositionSlider())); + addUAVObjectToWidgetRelation("ManualControlSettings", "Arming", ui->armControl); + addUAVObjectToWidgetRelation("ManualControlSettings", "ArmedTimeout", ui->armTimeout, 0, 1000); + connect(ManualControlCommand::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveFMSlider())); + connect(ManualControlSettings::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updatePositionSlider())); addWidget(ui->configurationWizard); addWidget(ui->runCalibration); @@ -164,13 +164,12 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : m_renderer = new QSvgRenderer(); QGraphicsScene *l_scene = ui->graphicsView->scene(); ui->graphicsView->setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); - if (QFile::exists(":/configgadget/images/TX2.svg") && m_renderer->load(QString(":/configgadget/images/TX2.svg")) && m_renderer->isValid()) - { + if (QFile::exists(":/configgadget/images/TX2.svg") && m_renderer->load(QString(":/configgadget/images/TX2.svg")) && m_renderer->isValid()) { l_scene->clear(); // Deletes all items contained in the scene as well. m_txBackground = new QGraphicsSvgItem(); // All other items will be clipped to the shape of the background - m_txBackground->setFlags(QGraphicsItem::ItemClipsChildrenToShape| + m_txBackground->setFlags(QGraphicsItem::ItemClipsChildrenToShape | QGraphicsItem::ItemClipsToShape); m_txBackground->setSharedRenderer(m_renderer); m_txBackground->setElementId("background"); @@ -219,112 +218,109 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : m_txArrows->setElementId("arrows"); m_txArrows->setVisible(false); - QRectF orig=m_renderer->boundsOnElement("ljoy"); + QRectF orig = m_renderer->boundsOnElement("ljoy"); QMatrix Matrix = m_renderer->matrixForElement("ljoy"); - orig=Matrix.mapRect(orig); - m_txLeftStickOrig.translate(orig.x(),orig.y()); - m_txLeftStick->setTransform(m_txLeftStickOrig,false); + orig = Matrix.mapRect(orig); + m_txLeftStickOrig.translate(orig.x(), orig.y()); + m_txLeftStick->setTransform(m_txLeftStickOrig, false); - orig=m_renderer->boundsOnElement("arrows"); + orig = m_renderer->boundsOnElement("arrows"); Matrix = m_renderer->matrixForElement("arrows"); - orig=Matrix.mapRect(orig); - m_txArrowsOrig.translate(orig.x(),orig.y()); - m_txArrows->setTransform(m_txArrowsOrig,false); + orig = Matrix.mapRect(orig); + m_txArrowsOrig.translate(orig.x(), orig.y()); + m_txArrows->setTransform(m_txArrowsOrig, false); - orig=m_renderer->boundsOnElement("body"); + orig = m_renderer->boundsOnElement("body"); Matrix = m_renderer->matrixForElement("body"); - orig=Matrix.mapRect(orig); - m_txMainBodyOrig.translate(orig.x(),orig.y()); - m_txMainBody->setTransform(m_txMainBodyOrig,false); + orig = Matrix.mapRect(orig); + m_txMainBodyOrig.translate(orig.x(), orig.y()); + m_txMainBody->setTransform(m_txMainBodyOrig, false); - orig=m_renderer->boundsOnElement("flightModeCenter"); + orig = m_renderer->boundsOnElement("flightModeCenter"); Matrix = m_renderer->matrixForElement("flightModeCenter"); - orig=Matrix.mapRect(orig); - m_txFlightModeCOrig.translate(orig.x(),orig.y()); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + orig = Matrix.mapRect(orig); + m_txFlightModeCOrig.translate(orig.x(), orig.y()); + m_txFlightMode->setTransform(m_txFlightModeCOrig, false); - orig=m_renderer->boundsOnElement("flightModeLeft"); + orig = m_renderer->boundsOnElement("flightModeLeft"); Matrix = m_renderer->matrixForElement("flightModeLeft"); - orig=Matrix.mapRect(orig); - m_txFlightModeLOrig.translate(orig.x(),orig.y()); - orig=m_renderer->boundsOnElement("flightModeRight"); + orig = Matrix.mapRect(orig); + m_txFlightModeLOrig.translate(orig.x(), orig.y()); + orig = m_renderer->boundsOnElement("flightModeRight"); Matrix = m_renderer->matrixForElement("flightModeRight"); - orig=Matrix.mapRect(orig); - m_txFlightModeROrig.translate(orig.x(),orig.y()); + orig = Matrix.mapRect(orig); + m_txFlightModeROrig.translate(orig.x(), orig.y()); - orig=m_renderer->boundsOnElement("rjoy"); + orig = m_renderer->boundsOnElement("rjoy"); Matrix = m_renderer->matrixForElement("rjoy"); - orig=Matrix.mapRect(orig); - m_txRightStickOrig.translate(orig.x(),orig.y()); - m_txRightStick->setTransform(m_txRightStickOrig,false); + orig = Matrix.mapRect(orig); + m_txRightStickOrig.translate(orig.x(), orig.y()); + m_txRightStick->setTransform(m_txRightStickOrig, false); - orig=m_renderer->boundsOnElement("access0"); + orig = m_renderer->boundsOnElement("access0"); Matrix = m_renderer->matrixForElement("access0"); - orig=Matrix.mapRect(orig); - m_txAccess0Orig.translate(orig.x(),orig.y()); - m_txAccess0->setTransform(m_txAccess0Orig,false); + orig = Matrix.mapRect(orig); + m_txAccess0Orig.translate(orig.x(), orig.y()); + m_txAccess0->setTransform(m_txAccess0Orig, false); - orig=m_renderer->boundsOnElement("access1"); + orig = m_renderer->boundsOnElement("access1"); Matrix = m_renderer->matrixForElement("access1"); - orig=Matrix.mapRect(orig); - m_txAccess1Orig.translate(orig.x(),orig.y()); - m_txAccess1->setTransform(m_txAccess1Orig,false); + orig = Matrix.mapRect(orig); + m_txAccess1Orig.translate(orig.x(), orig.y()); + m_txAccess1->setTransform(m_txAccess1Orig, false); - orig=m_renderer->boundsOnElement("access2"); + orig = m_renderer->boundsOnElement("access2"); Matrix = m_renderer->matrixForElement("access2"); - orig=Matrix.mapRect(orig); - m_txAccess2Orig.translate(orig.x(),orig.y()); - m_txAccess2->setTransform(m_txAccess2Orig,true); + orig = Matrix.mapRect(orig); + m_txAccess2Orig.translate(orig.x(), orig.y()); + m_txAccess2->setTransform(m_txAccess2Orig, true); } - ui->graphicsView->fitInView(m_txMainBody, Qt::KeepAspectRatio ); - animate=new QTimer(this); - connect(animate,SIGNAL(timeout()),this,SLOT(moveTxControls())); + ui->graphicsView->fitInView(m_txMainBody, Qt::KeepAspectRatio); + animate = new QTimer(this); + connect(animate, SIGNAL(timeout()), this, SLOT(moveTxControls())); heliChannelOrder << ManualControlSettings::CHANNELGROUPS_COLLECTIVE << - ManualControlSettings::CHANNELGROUPS_THROTTLE << - ManualControlSettings::CHANNELGROUPS_ROLL << - ManualControlSettings::CHANNELGROUPS_PITCH << - ManualControlSettings::CHANNELGROUPS_YAW << - ManualControlSettings::CHANNELGROUPS_FLIGHTMODE << - ManualControlSettings::CHANNELGROUPS_ACCESSORY0 << - ManualControlSettings::CHANNELGROUPS_ACCESSORY1 << - ManualControlSettings::CHANNELGROUPS_ACCESSORY2; + ManualControlSettings::CHANNELGROUPS_THROTTLE << + ManualControlSettings::CHANNELGROUPS_ROLL << + ManualControlSettings::CHANNELGROUPS_PITCH << + ManualControlSettings::CHANNELGROUPS_YAW << + ManualControlSettings::CHANNELGROUPS_FLIGHTMODE << + ManualControlSettings::CHANNELGROUPS_ACCESSORY0 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY1 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY2; acroChannelOrder << ManualControlSettings::CHANNELGROUPS_THROTTLE << - ManualControlSettings::CHANNELGROUPS_ROLL << - ManualControlSettings::CHANNELGROUPS_PITCH << - ManualControlSettings::CHANNELGROUPS_YAW << - ManualControlSettings::CHANNELGROUPS_FLIGHTMODE << - ManualControlSettings::CHANNELGROUPS_ACCESSORY0 << - ManualControlSettings::CHANNELGROUPS_ACCESSORY1 << - ManualControlSettings::CHANNELGROUPS_ACCESSORY2; + ManualControlSettings::CHANNELGROUPS_ROLL << + ManualControlSettings::CHANNELGROUPS_PITCH << + ManualControlSettings::CHANNELGROUPS_YAW << + ManualControlSettings::CHANNELGROUPS_FLIGHTMODE << + ManualControlSettings::CHANNELGROUPS_ACCESSORY0 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY1 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY2; updateEnableControls(); } void ConfigInputWidget::resetTxControls() { - - m_txLeftStick->setTransform(m_txLeftStickOrig,false); - m_txRightStick->setTransform(m_txRightStickOrig,false); - m_txAccess0->setTransform(m_txAccess0Orig,false); - m_txAccess1->setTransform(m_txAccess1Orig,false); - m_txAccess2->setTransform(m_txAccess2Orig,false); + m_txLeftStick->setTransform(m_txLeftStickOrig, false); + m_txRightStick->setTransform(m_txRightStickOrig, false); + m_txAccess0->setTransform(m_txAccess0Orig, false); + m_txAccess1->setTransform(m_txAccess1Orig, false); + m_txAccess2->setTransform(m_txAccess2Orig, false); m_txFlightMode->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + m_txFlightMode->setTransform(m_txFlightModeCOrig, false); m_txArrows->setVisible(false); } ConfigInputWidget::~ConfigInputWidget() -{ - -} +{} void ConfigInputWidget::enableControls(bool enable) { ConfigTaskWidget::enableControls(enable); - if(enable) { + if (enable) { updatePositionSlider(); } } @@ -332,17 +328,19 @@ void ConfigInputWidget::enableControls(bool enable) void ConfigInputWidget::resizeEvent(QResizeEvent *event) { QWidget::resizeEvent(event); - ui->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio ); + + ui->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio); } void ConfigInputWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/04Cf", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/04Cf", QUrl::StrictMode)); } void ConfigInputWidget::goToWizard() { QMessageBox msgBox; + msgBox.setText(tr("Arming Settings are now set to 'Always Disarmed' for your safety.")); msgBox.setDetailedText(tr("You will have to reconfigure the arming settings manually " "when the wizard is finished. After the last step of the " @@ -352,20 +350,20 @@ void ConfigInputWidget::goToWizard() msgBox.exec(); // Set correct tab visible before starting wizard. - if(ui->tabWidget->currentIndex() != 0) { + if (ui->tabWidget->currentIndex() != 0) { ui->tabWidget->setCurrentIndex(0); } - - // Stash current manual settings data in case the wizard is + + // Stash current manual settings data in case the wizard is // cancelled or the user proceeds far enough into the wizard such // that the UAVO is changed, but then backs out to the start and // chooses a different TX type (which could otherwise result in // unexpected TX channels being enabled) - manualSettingsData=manualSettingsObj->getData(); + manualSettingsData = manualSettingsObj->getData(); previousManualSettingsData = manualSettingsData; - manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; + manualSettingsData.Arming = ManualControlSettings::ARMING_ALWAYSDISARMED; manualSettingsObj->setData(manualSettingsData); - + // start the wizard wizardSetUpStep(wizardWelcome); ui->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio); @@ -373,10 +371,11 @@ void ConfigInputWidget::goToWizard() void ConfigInputWidget::disableWizardButton(int value) { - if(value!=0) + if (value != 0) { ui->groupBox_3->setVisible(false); - else + } else { ui->groupBox_3->setVisible(true); + } } void ConfigInputWidget::wzCancel() @@ -385,9 +384,10 @@ void ConfigInputWidget::wzCancel() manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); ui->stackedWidget->setCurrentIndex(0); - if(wizardStep != wizardNone) + if (wizardStep != wizardNone) { wizardTearDownStep(wizardStep); - wizardStep=wizardNone; + } + wizardStep = wizardNone; ui->stackedWidget->setCurrentIndex(0); // Load settings back from beginning of wizard @@ -398,12 +398,13 @@ void ConfigInputWidget::wzNext() { // In identify sticks mode the next button can indicate // channel advance - if(wizardStep != wizardNone && - wizardStep != wizardIdentifySticks) + if (wizardStep != wizardNone && + wizardStep != wizardIdentifySticks) { wizardTearDownStep(wizardStep); + } // State transitions for next button - switch(wizardStep) { + switch (wizardStep) { case wizardWelcome: wizardSetUpStep(wizardChooseType); break; @@ -415,7 +416,7 @@ void ConfigInputWidget::wzNext() break; case wizardIdentifySticks: nextChannel(); - if(currentChannelNum==-1) { // Gone through all channels + if (currentChannelNum == -1) { // Gone through all channels wizardTearDownStep(wizardIdentifySticks); wizardSetUpStep(wizardIdentifyCenter); } @@ -430,23 +431,22 @@ void ConfigInputWidget::wzNext() wizardSetUpStep(wizardFinish); break; case wizardFinish: - wizardStep=wizardNone; + wizardStep = wizardNone; // Leave setting the throttle neutral until the final Next press, // else the throttle scaling causes the graphical stick movement to not // match the tx stick - manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE]= - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]+ - ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE]- - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); - if((abs(manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE] - - manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE])<100) || - (abs(manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE] - - manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE])<100)) - { - manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]= - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]+ + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE] = + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE] + + ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE] - + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]) * 0.02); + if ((abs(manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE] - + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]) < 100) || + (abs(manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE] - + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]) < 100)) { + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE] = + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE] + (manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE] - - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE])/2; + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]) / 2; } manualSettingsObj->setData(manualSettingsData); // move to Arming Settings tab @@ -460,12 +460,13 @@ void ConfigInputWidget::wzNext() void ConfigInputWidget::wzBack() { - if(wizardStep != wizardNone && - wizardStep != wizardIdentifySticks) + if (wizardStep != wizardNone && + wizardStep != wizardIdentifySticks) { wizardTearDownStep(wizardStep); + } // State transitions for next button - switch(wizardStep) { + switch (wizardStep) { case wizardChooseType: wizardSetUpStep(wizardWelcome); break; @@ -474,7 +475,7 @@ void ConfigInputWidget::wzBack() break; case wizardIdentifySticks: prevChannel(); - if(currentChannelNum == -1) { + if (currentChannelNum == -1) { wizardTearDownStep(wizardIdentifySticks); wizardSetUpStep(wizardChooseMode); } @@ -499,21 +500,21 @@ void ConfigInputWidget::wzBack() void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) { ui->wzText2->clear(); - - switch(step) { + + switch (step) { case wizardWelcome: - foreach(QPointer wd,extraWidgets) - { - if(!wd.isNull()) + foreach(QPointer wd, extraWidgets) { + if (!wd.isNull()) { delete wd; + } } extraWidgets.clear(); ui->graphicsView->setVisible(false); setTxMovement(nothing); ui->wzText->setText(tr("Welcome to the inputs configuration wizard.\n\n" - "Please follow the instructions on the screen and only move your controls when asked to.\n" - "Make sure you already configured your hardware settings on the proper tab and restarted your board.\n\n" - "You can press 'back' at any time to return to the previous screen or press 'Cancel' to quit the wizard.\n")); + "Please follow the instructions on the screen and only move your controls when asked to.\n" + "Make sure you already configured your hardware settings on the proper tab and restarted your board.\n\n" + "You can press 'back' at any time to return to the previous screen or press 'Cancel' to quit the wizard.\n")); ui->stackedWidget->setCurrentIndex(1); ui->wzBack->setEnabled(false); break; @@ -524,12 +525,11 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) setTxMovement(nothing); ui->wzText->setText(tr("Please choose your transmitter type:")); ui->wzBack->setEnabled(true); - QRadioButton * typeAcro=new QRadioButton(tr("Acro: normal transmitter for fixed-wing or quad"),this); - QRadioButton * typeHeli=new QRadioButton(tr("Helicopter: has collective pitch and throttle input"),this); + QRadioButton *typeAcro = new QRadioButton(tr("Acro: normal transmitter for fixed-wing or quad"), this); + QRadioButton *typeHeli = new QRadioButton(tr("Helicopter: has collective pitch and throttle input"), this); if (transmitterType == heli) { typeHeli->setChecked(true); - } - else { + } else { typeAcro->setChecked(true); } ui->wzText2->setText(tr("If selecting the Helicopter option, please engage throttle hold now.")); @@ -540,7 +540,7 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) ui->radioButtonsLayout->layout()->addWidget(typeAcro); ui->radioButtonsLayout->layout()->addWidget(typeHeli); } - break; + break; case wizardChooseMode: { ui->wzBack->setEnabled(true); @@ -551,23 +551,23 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) txMode mode = static_cast(i); if (transmitterType == heli) { switch (mode) { - case mode1: label = tr("Mode 1: Fore/Aft Cyclic and Yaw on the left, Throttle/Collective and Left/Right Cyclic on the right"); break; - case mode2: label = tr("Mode 2: Throttle/Collective and Yaw on the left, Cyclic on the right"); break; - case mode3: label = tr("Mode 3: Cyclic on the left, Throttle/Collective and Yaw on the right"); break; - case mode4: label = tr("Mode 4: Throttle/Collective and Left/Right Cyclic on the left, Fore/Aft Cyclic and Yaw on the right"); break; - default: Q_ASSERT(0); break; - } } - else { + case mode1: label = tr("Mode 1: Fore/Aft Cyclic and Yaw on the left, Throttle/Collective and Left/Right Cyclic on the right"); break; + case mode2: label = tr("Mode 2: Throttle/Collective and Yaw on the left, Cyclic on the right"); break; + case mode3: label = tr("Mode 3: Cyclic on the left, Throttle/Collective and Yaw on the right"); break; + case mode4: label = tr("Mode 4: Throttle/Collective and Left/Right Cyclic on the left, Fore/Aft Cyclic and Yaw on the right"); break; + default: Q_ASSERT(0); break; + } + } else { switch (mode) { - case mode1: label = tr("Mode 1: Elevator and Rudder on the left, Throttle and Ailerons on the right"); break; - case mode2: label = tr("Mode 2: Throttle and Rudder on the left, Elevator and Ailerons on the right"); break; - case mode3: label = tr("Mode 3: Elevator and Ailerons on the left, Throttle and Rudder on the right"); break; - case mode4: label = tr("Mode 4: Throttle and Ailerons on the left, Elevator and Rudder on the right"); break; - default: Q_ASSERT(0); break; - } + case mode1: label = tr("Mode 1: Elevator and Rudder on the left, Throttle and Ailerons on the right"); break; + case mode2: label = tr("Mode 2: Throttle and Rudder on the left, Elevator and Ailerons on the right"); break; + case mode3: label = tr("Mode 3: Elevator and Ailerons on the left, Throttle and Rudder on the right"); break; + case mode4: label = tr("Mode 4: Throttle and Ailerons on the left, Elevator and Rudder on the right"); break; + default: Q_ASSERT(0); break; + } ui->wzText2->setText(tr("For a Quad: Elevator is Pitch, Ailerons are Roll, and Rudder is Yaw.")); } - QRadioButton * modeButton = new QRadioButton(label, this); + QRadioButton *modeButton = new QRadioButton(label, this); if (transmitterMode == mode) { modeButton->setChecked(true); } @@ -575,77 +575,74 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) ui->radioButtonsLayout->layout()->addWidget(modeButton); } } - break; + break; case wizardIdentifySticks: usedChannels.clear(); - currentChannelNum=-1; + currentChannelNum = -1; nextChannel(); - manualSettingsData=manualSettingsObj->getData(); - connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); + manualSettingsData = manualSettingsObj->getData(); + connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyControls())); ui->wzNext->setEnabled(false); break; case wizardIdentifyCenter: setTxMovement(centerAll); ui->wzText->setText(QString(tr("Please center all controls and trims and press Next when ready.\n\n" - "If your FlightMode switch has only two positions, leave it in either position."))); + "If your FlightMode switch has only two positions, leave it in either position."))); break; case wizardIdentifyLimits: { - accessoryDesiredObj0 = AccessoryDesired::GetInstance(getObjectManager(),0); - accessoryDesiredObj1 = AccessoryDesired::GetInstance(getObjectManager(),1); - accessoryDesiredObj2 = AccessoryDesired::GetInstance(getObjectManager(),2); + accessoryDesiredObj0 = AccessoryDesired::GetInstance(getObjectManager(), 0); + accessoryDesiredObj1 = AccessoryDesired::GetInstance(getObjectManager(), 1); + accessoryDesiredObj2 = AccessoryDesired::GetInstance(getObjectManager(), 2); setTxMovement(nothing); ui->wzText->setText(QString(tr("Please move all controls to their maximum extents on both directions.\n\nPress Next when ready."))); fastMdata(); - manualSettingsData=manualSettingsObj->getData(); - for(uint i=0;igetData(); + for (uint i = 0; i < ManualControlSettings::CHANNELMAX_NUMELEM; ++i) { // Preserve the inverted status - if(manualSettingsData.ChannelMin[i] <= manualSettingsData.ChannelMax[i]) { - manualSettingsData.ChannelMin[i]=manualSettingsData.ChannelNeutral[i]; - manualSettingsData.ChannelMax[i]=manualSettingsData.ChannelNeutral[i]; + if (manualSettingsData.ChannelMin[i] <= manualSettingsData.ChannelMax[i]) { + manualSettingsData.ChannelMin[i] = manualSettingsData.ChannelNeutral[i]; + manualSettingsData.ChannelMax[i] = manualSettingsData.ChannelNeutral[i]; } else { // Make this detect as still inverted - manualSettingsData.ChannelMin[i]=manualSettingsData.ChannelNeutral[i] + 1; - manualSettingsData.ChannelMax[i]=manualSettingsData.ChannelNeutral[i]; + manualSettingsData.ChannelMin[i] = manualSettingsData.ChannelNeutral[i] + 1; + manualSettingsData.ChannelMax[i] = manualSettingsData.ChannelNeutral[i]; } } - connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits())); - connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyLimits())); + connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); } - break; + break; case wizardIdentifyInverted: dimOtherControls(true); setTxMovement(nothing); extraWidgets.clear(); - for (int index = 0; index < manualSettingsObj->getField("ChannelMax")->getElementNames().length(); index++) - { + for (int index = 0; index < manualSettingsObj->getField("ChannelMax")->getElementNames().length(); index++) { QString name = manualSettingsObj->getField("ChannelMax")->getElementNames().at(index); - if(!name.contains("Access") && !name.contains("Flight") && - (!name.contains("Collective") || transmitterType == heli)) - { - QCheckBox * cb=new QCheckBox(name,this); + if (!name.contains("Access") && !name.contains("Flight") && + (!name.contains("Collective") || transmitterType == heli)) { + QCheckBox *cb = new QCheckBox(name, this); // Make sure checked status matches current one cb->setChecked(manualSettingsData.ChannelMax[index] < manualSettingsData.ChannelMin[index]); - dynamic_cast(ui->checkBoxesLayout->layout())->addWidget(cb, extraWidgets.size()/4, extraWidgets.size()%4); + dynamic_cast(ui->checkBoxesLayout->layout())->addWidget(cb, extraWidgets.size() / 4, extraWidgets.size() % 4); extraWidgets.append(cb); - connect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); + connect(cb, SIGNAL(toggled(bool)), this, SLOT(invertControls())); } } - connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); ui->wzText->setText(QString(tr("Please check the picture below and correct all the sticks which show an inverted movement. Press Next when ready."))); fastMdata(); break; case wizardFinish: dimOtherControls(false); - connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); ui->wzText->setText(QString(tr("You have completed this wizard, please check below if the picture mimics your sticks movement.\n\n" - "IMPORTANT: These new settings have not been saved to the board yet. After pressing Next you will go to the Arming Settings " - "tab where you can set your desired arming sequence and save the configuration."))); + "IMPORTANT: These new settings have not been saved to the board yet. After pressing Next you will go to the Arming Settings " + "tab where you can set your desired arming sequence and save the configuration."))); fastMdata(); break; default: @@ -656,76 +653,76 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) void ConfigInputWidget::wizardTearDownStep(enum wizardSteps step) { - QRadioButton * mode, * type; + QRadioButton *mode, *type; + Q_ASSERT(step == wizardStep); - switch(step) { + switch (step) { case wizardWelcome: break; - case wizardChooseType: - type=qobject_cast(extraWidgets.at(0)); - if(type->isChecked()) - transmitterType=acro; - else - transmitterType=heli; + case wizardChooseType: + type = qobject_cast(extraWidgets.at(0)); + if (type->isChecked()) { + transmitterType = acro; + } else { + transmitterType = heli; + } delete extraWidgets.at(0); delete extraWidgets.at(1); extraWidgets.clear(); break; - case wizardChooseMode: - for (int i = mode1; i <= mode4; ++i) { - mode=qobject_cast(extraWidgets.first()); - if(mode->isChecked()) { - transmitterMode=static_cast(i); + case wizardChooseMode: + for (int i = mode1; i <= mode4; ++i) { + mode = qobject_cast(extraWidgets.first()); + if (mode->isChecked()) { + transmitterMode = static_cast(i); } delete mode; extraWidgets.removeFirst(); } break; case wizardIdentifySticks: - disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); + disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyControls())); ui->wzNext->setEnabled(true); setTxMovement(nothing); break; case wizardIdentifyCenter: - manualCommandData=manualCommandObj->getData(); - manualSettingsData=manualSettingsObj->getData(); - for(unsigned int i=0;igetData(); + manualSettingsData = manualSettingsObj->getData(); + for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; ++i) { + manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; } manualSettingsObj->setData(manualSettingsData); setTxMovement(nothing); break; case wizardIdentifyLimits: - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits())); - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - disconnect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - disconnect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyLimits())); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + disconnect(flightStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + disconnect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); manualSettingsObj->setData(manualSettingsData); restoreMdata(); setTxMovement(nothing); break; case wizardIdentifyInverted: dimOtherControls(false); - foreach(QWidget * wd,extraWidgets) - { - QCheckBox * cb=qobject_cast(wd); - if(cb) - { - disconnect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); + foreach(QWidget * wd, extraWidgets) { + QCheckBox *cb = qobject_cast(wd); + + if (cb) { + disconnect(cb, SIGNAL(toggled(bool)), this, SLOT(invertControls())); delete cb; } } extraWidgets.clear(); - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); restoreMdata(); break; case wizardFinish: dimOtherControls(false); setTxMovement(nothing); - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - disconnect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - disconnect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + disconnect(flightStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + disconnect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); restoreMdata(); break; default: @@ -734,8 +731,8 @@ void ConfigInputWidget::wizardTearDownStep(enum wizardSteps step) } /** - * Set manual control command to fast updates - */ + * Set manual control command to fast updates + */ void ConfigInputWidget::fastMdata() { manualControlMdata = manualCommandObj->getMetadata(); @@ -746,56 +743,58 @@ void ConfigInputWidget::fastMdata() } /** - * Restore previous update settings for manual control data - */ + * Restore previous update settings for manual control data + */ void ConfigInputWidget::restoreMdata() { manualCommandObj->setMetadata(manualControlMdata); } /** - * Set the display to indicate which channel the person should move - */ + * Set the display to indicate which channel the person should move + */ void ConfigInputWidget::setChannel(int newChan) { - if(newChan == ManualControlSettings::CHANNELGROUPS_COLLECTIVE) + if (newChan == ManualControlSettings::CHANNELGROUPS_COLLECTIVE) { ui->wzText->setText(QString(tr("Please enable throttle hold mode.\n\nMove the Collective Pitch stick."))); - else if (newChan == ManualControlSettings::CHANNELGROUPS_FLIGHTMODE) + } else if (newChan == ManualControlSettings::CHANNELGROUPS_FLIGHTMODE) { ui->wzText->setText(QString(tr("Please toggle the Flight Mode switch.\n\nFor switches you may have to repeat this rapidly."))); - else if((transmitterType == heli) && (newChan == ManualControlSettings::CHANNELGROUPS_THROTTLE)) + } else if ((transmitterType == heli) && (newChan == ManualControlSettings::CHANNELGROUPS_THROTTLE)) { ui->wzText->setText(QString(tr("Please disable throttle hold mode.\n\nMove the Throttle stick."))); - else + } else { ui->wzText->setText(QString(tr("Please move each control one at a time according to the instructions and picture below.\n\n" - "Move the %1 stick.")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan))); + "Move the %1 stick.")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan))); + } - if(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("Accessory") || - manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("FlightMode")) { + if (manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("Accessory") || + manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("FlightMode")) { ui->wzNext->setEnabled(true); ui->wzText->setText(ui->wzText->text() + tr(" Alternatively, click Next to skip this channel.")); - } else + } else { ui->wzNext->setEnabled(false); + } setMoveFromCommand(newChan); currentChannelNum = newChan; - channelDetected = false; + channelDetected = false; } /** - * Unfortunately order of channel should be different in different conditions. Selects - * next channel based on heli or acro mode - */ + * Unfortunately order of channel should be different in different conditions. Selects + * next channel based on heli or acro mode + */ void ConfigInputWidget::nextChannel() { QList order = (transmitterType == heli) ? heliChannelOrder : acroChannelOrder; - if(currentChannelNum == -1) { + if (currentChannelNum == -1) { setChannel(order[0]); return; } for (int i = 0; i < order.length() - 1; i++) { - if(order[i] == currentChannelNum) { - setChannel(order[i+1]); + if (order[i] == currentChannelNum) { + setChannel(order[i + 1]); return; } } @@ -803,24 +802,25 @@ void ConfigInputWidget::nextChannel() } /** - * Unfortunately order of channel should be different in different conditions. Selects - * previous channel based on heli or acro mode - */ + * Unfortunately order of channel should be different in different conditions. Selects + * previous channel based on heli or acro mode + */ void ConfigInputWidget::prevChannel() { QList order = transmitterType == heli ? heliChannelOrder : acroChannelOrder; // No previous from unset channel or next state - if(currentChannelNum == -1) + if (currentChannelNum == -1) { return; + } for (int i = 1; i < order.length(); i++) { - if(order[i] == currentChannelNum) { - if (!usedChannels.isEmpty() && + if (order[i] == currentChannelNum) { + if (!usedChannels.isEmpty() && usedChannels.back().channelIndex == currentChannelNum) { usedChannels.removeLast(); } - setChannel(order[i-1]); + setChannel(order[i - 1]); return; } } @@ -829,38 +829,38 @@ void ConfigInputWidget::prevChannel() void ConfigInputWidget::identifyControls() { - static int debounce=0; + static int debounce = 0; - receiverActivityData=receiverActivityObj->getData(); - if(receiverActivityData.ActiveChannel==255) + receiverActivityData = receiverActivityObj->getData(); + if (receiverActivityData.ActiveChannel == 255) { return; - if(channelDetected) + } + if (channelDetected) { return; - else - { - receiverActivityData=receiverActivityObj->getData(); - currentChannel.group=receiverActivityData.ActiveGroup; - currentChannel.number=receiverActivityData.ActiveChannel; - if(currentChannel==lastChannel) + } else { + receiverActivityData = receiverActivityObj->getData(); + currentChannel.group = receiverActivityData.ActiveGroup; + currentChannel.number = receiverActivityData.ActiveChannel; + if (currentChannel == lastChannel) { ++debounce; - lastChannel.group= currentChannel.group; - lastChannel.number=currentChannel.number; - lastChannel.channelIndex = currentChannelNum; - if(!usedChannels.contains(lastChannel) && debounce>1) - { - channelDetected = true; - debounce=0; - usedChannels.append(lastChannel); - manualSettingsData=manualSettingsObj->getData(); - manualSettingsData.ChannelGroups[currentChannelNum]=currentChannel.group; - manualSettingsData.ChannelNumber[currentChannelNum]=currentChannel.number; - manualSettingsObj->setData(manualSettingsData); } - else + lastChannel.group = currentChannel.group; + lastChannel.number = currentChannel.number; + lastChannel.channelIndex = currentChannelNum; + if (!usedChannels.contains(lastChannel) && debounce > 1) { + channelDetected = true; + debounce = 0; + usedChannels.append(lastChannel); + manualSettingsData = manualSettingsObj->getData(); + manualSettingsData.ChannelGroups[currentChannelNum] = currentChannel.group; + manualSettingsData.ChannelNumber[currentChannelNum] = currentChannel.number; + manualSettingsObj->setData(manualSettingsData); + } else { return; + } } - //m_config->wzText->clear(); + // m_config->wzText->clear(); setTxMovement(nothing); QTimer::singleShot(2500, this, SLOT(wzNext())); @@ -868,21 +868,24 @@ void ConfigInputWidget::identifyControls() void ConfigInputWidget::identifyLimits() { - manualCommandData=manualCommandObj->getData(); - for(uint i=0;igetData(); + for (uint i = 0; i < ManualControlSettings::CHANNELMAX_NUMELEM; ++i) { + if (manualSettingsData.ChannelMin[i] <= manualSettingsData.ChannelMax[i]) { // Non inverted channel - if(manualSettingsData.ChannelMin[i]>manualCommandData.Channel[i]) - manualSettingsData.ChannelMin[i]=manualCommandData.Channel[i]; - if(manualSettingsData.ChannelMax[i] manualCommandData.Channel[i]) { + manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; + } + if (manualSettingsData.ChannelMax[i] < manualCommandData.Channel[i]) { + manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; + } } else { // Inverted channel - if(manualSettingsData.ChannelMax[i]>manualCommandData.Channel[i]) - manualSettingsData.ChannelMax[i]=manualCommandData.Channel[i]; - if(manualSettingsData.ChannelMin[i] manualCommandData.Channel[i]) { + manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; + } + if (manualSettingsData.ChannelMin[i] < manualCommandData.Channel[i]) { + manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; + } } } manualSettingsObj->setData(manualSettingsData); @@ -890,123 +893,122 @@ void ConfigInputWidget::identifyLimits() void ConfigInputWidget::setMoveFromCommand(int command) { // ManualControlSettings::ChannelNumberElem: - // CHANNELNUMBER_ROLL=0, - // CHANNELNUMBER_PITCH=1, - // CHANNELNUMBER_YAW=2, - // CHANNELNUMBER_THROTTLE=3, - // CHANNELNUMBER_FLIGHTMODE=4, - // CHANNELNUMBER_ACCESSORY0=5, - // CHANNELNUMBER_ACCESSORY1=6, - // CHANNELNUMBER_ACCESSORY2=7 - + // CHANNELNUMBER_ROLL=0, + // CHANNELNUMBER_PITCH=1, + // CHANNELNUMBER_YAW=2, + // CHANNELNUMBER_THROTTLE=3, + // CHANNELNUMBER_FLIGHTMODE=4, + // CHANNELNUMBER_ACCESSORY0=5, + // CHANNELNUMBER_ACCESSORY1=6, + // CHANNELNUMBER_ACCESSORY2=7 + txMovements movement; - + switch (command) { - case ManualControlSettings::CHANNELNUMBER_ROLL: - movement = ((transmitterMode == mode3 || transmitterMode == mode4) ? - moveLeftHorizontalStick: moveRightHorizontalStick); - break; - case ManualControlSettings::CHANNELNUMBER_PITCH: - movement = (transmitterMode == mode1 || transmitterMode == mode3) ? - moveLeftVerticalStick: moveRightVerticalStick; - break; - case ManualControlSettings::CHANNELNUMBER_YAW: - movement = ((transmitterMode == mode1 || transmitterMode == mode2) ? - moveLeftHorizontalStick: moveRightHorizontalStick); - break; - case ManualControlSettings::CHANNELNUMBER_THROTTLE: - movement = (transmitterMode == mode2 || transmitterMode == mode4) ? - moveLeftVerticalStick: moveRightVerticalStick; - break; - case ManualControlSettings::CHANNELNUMBER_COLLECTIVE: - movement = (transmitterMode == mode2 || transmitterMode == mode4) ? - moveLeftVerticalStick: moveRightVerticalStick; - break; - case ManualControlSettings::CHANNELNUMBER_FLIGHTMODE: - movement = moveFlightMode; - break; - case ManualControlSettings::CHANNELNUMBER_ACCESSORY0: - movement = moveAccess0; - break; - case ManualControlSettings::CHANNELNUMBER_ACCESSORY1: - movement = moveAccess1; - break; - case ManualControlSettings::CHANNELNUMBER_ACCESSORY2: - movement = moveAccess2; - break; - default: - Q_ASSERT(0); - break; - } + case ManualControlSettings::CHANNELNUMBER_ROLL: + movement = ((transmitterMode == mode3 || transmitterMode == mode4) ? + moveLeftHorizontalStick : moveRightHorizontalStick); + break; + case ManualControlSettings::CHANNELNUMBER_PITCH: + movement = (transmitterMode == mode1 || transmitterMode == mode3) ? + moveLeftVerticalStick : moveRightVerticalStick; + break; + case ManualControlSettings::CHANNELNUMBER_YAW: + movement = ((transmitterMode == mode1 || transmitterMode == mode2) ? + moveLeftHorizontalStick : moveRightHorizontalStick); + break; + case ManualControlSettings::CHANNELNUMBER_THROTTLE: + movement = (transmitterMode == mode2 || transmitterMode == mode4) ? + moveLeftVerticalStick : moveRightVerticalStick; + break; + case ManualControlSettings::CHANNELNUMBER_COLLECTIVE: + movement = (transmitterMode == mode2 || transmitterMode == mode4) ? + moveLeftVerticalStick : moveRightVerticalStick; + break; + case ManualControlSettings::CHANNELNUMBER_FLIGHTMODE: + movement = moveFlightMode; + break; + case ManualControlSettings::CHANNELNUMBER_ACCESSORY0: + movement = moveAccess0; + break; + case ManualControlSettings::CHANNELNUMBER_ACCESSORY1: + movement = moveAccess1; + break; + case ManualControlSettings::CHANNELNUMBER_ACCESSORY2: + movement = moveAccess2; + break; + default: + Q_ASSERT(0); + break; + } setTxMovement(movement); } void ConfigInputWidget::setTxMovement(txMovements movement) { resetTxControls(); - switch(movement) - { + switch (movement) { case moveLeftVerticalStick: - movePos=0; - growing=true; - currentMovement=moveLeftVerticalStick; + movePos = 0; + growing = true; + currentMovement = moveLeftVerticalStick; animate->start(100); break; case moveRightVerticalStick: - movePos=0; - growing=true; - currentMovement=moveRightVerticalStick; + movePos = 0; + growing = true; + currentMovement = moveRightVerticalStick; animate->start(100); break; case moveLeftHorizontalStick: - movePos=0; - growing=true; - currentMovement=moveLeftHorizontalStick; + movePos = 0; + growing = true; + currentMovement = moveLeftHorizontalStick; animate->start(100); break; case moveRightHorizontalStick: - movePos=0; - growing=true; - currentMovement=moveRightHorizontalStick; + movePos = 0; + growing = true; + currentMovement = moveRightHorizontalStick; animate->start(100); break; case moveAccess0: - movePos=0; - growing=true; - currentMovement=moveAccess0; + movePos = 0; + growing = true; + currentMovement = moveAccess0; animate->start(100); break; case moveAccess1: - movePos=0; - growing=true; - currentMovement=moveAccess1; + movePos = 0; + growing = true; + currentMovement = moveAccess1; animate->start(100); break; case moveAccess2: - movePos=0; - growing=true; - currentMovement=moveAccess2; + movePos = 0; + growing = true; + currentMovement = moveAccess2; animate->start(100); break; case moveFlightMode: - movePos=0; - growing=true; - currentMovement=moveFlightMode; + movePos = 0; + growing = true; + currentMovement = moveFlightMode; animate->start(1000); break; case centerAll: - movePos=0; - currentMovement=centerAll; + movePos = 0; + currentMovement = centerAll; animate->start(1000); break; case moveAll: - movePos=0; - growing=true; - currentMovement=moveAll; + movePos = 0; + growing = true; + currentMovement = moveAll; animate->start(50); break; case nothing: - movePos=0; + movePos = 0; animate->stop(); break; default: @@ -1018,179 +1020,155 @@ void ConfigInputWidget::setTxMovement(txMovements movement) void ConfigInputWidget::moveTxControls() { QTransform trans; - QGraphicsItem * item; + QGraphicsItem *item; txMovementType move; int limitMax; int limitMin; - static bool auxFlag=false; - switch(currentMovement) - { + static bool auxFlag = false; + + switch (currentMovement) { case moveLeftVerticalStick: - item=m_txLeftStick; - trans=m_txLeftStickOrig; - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=vertical; + item = m_txLeftStick; + trans = m_txLeftStickOrig; + limitMax = STICK_MAX_MOVE; + limitMin = STICK_MIN_MOVE; + move = vertical; break; case moveRightVerticalStick: - item=m_txRightStick; - trans=m_txRightStickOrig; - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=vertical; + item = m_txRightStick; + trans = m_txRightStickOrig; + limitMax = STICK_MAX_MOVE; + limitMin = STICK_MIN_MOVE; + move = vertical; break; case moveLeftHorizontalStick: - item=m_txLeftStick; - trans=m_txLeftStickOrig; - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=horizontal; + item = m_txLeftStick; + trans = m_txLeftStickOrig; + limitMax = STICK_MAX_MOVE; + limitMin = STICK_MIN_MOVE; + move = horizontal; break; case moveRightHorizontalStick: - item=m_txRightStick; - trans=m_txRightStickOrig; - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=horizontal; + item = m_txRightStick; + trans = m_txRightStickOrig; + limitMax = STICK_MAX_MOVE; + limitMin = STICK_MIN_MOVE; + move = horizontal; break; case moveAccess0: - item=m_txAccess0; - trans=m_txAccess0Orig; - limitMax=ACCESS_MAX_MOVE; - limitMin=ACCESS_MIN_MOVE; - move=horizontal; + item = m_txAccess0; + trans = m_txAccess0Orig; + limitMax = ACCESS_MAX_MOVE; + limitMin = ACCESS_MIN_MOVE; + move = horizontal; break; case moveAccess1: - item=m_txAccess1; - trans=m_txAccess1Orig; - limitMax=ACCESS_MAX_MOVE; - limitMin=ACCESS_MIN_MOVE; - move=horizontal; + item = m_txAccess1; + trans = m_txAccess1Orig; + limitMax = ACCESS_MAX_MOVE; + limitMin = ACCESS_MIN_MOVE; + move = horizontal; break; case moveAccess2: - item=m_txAccess2; - trans=m_txAccess2Orig; - limitMax=ACCESS_MAX_MOVE; - limitMin=ACCESS_MIN_MOVE; - move=horizontal; + item = m_txAccess2; + trans = m_txAccess2Orig; + limitMax = ACCESS_MAX_MOVE; + limitMin = ACCESS_MIN_MOVE; + move = horizontal; break; case moveFlightMode: - item=m_txFlightMode; - move=jump; + item = m_txFlightMode; + move = jump; break; case centerAll: - item=m_txArrows; - move=jump; + item = m_txArrows; + move = jump; break; case moveAll: - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=mix; + limitMax = STICK_MAX_MOVE; + limitMin = STICK_MIN_MOVE; + move = mix; break; default: break; } - if(move==vertical) - item->setTransform(trans.translate(0,movePos*10),false); - else if(move==horizontal) - item->setTransform(trans.translate(movePos*10,0),false); - else if(move==jump) - { - if(item==m_txArrows) - { + if (move == vertical) { + item->setTransform(trans.translate(0, movePos * 10), false); + } else if (move == horizontal) { + item->setTransform(trans.translate(movePos * 10, 0), false); + } else if (move == jump) { + if (item == m_txArrows) { m_txArrows->setVisible(!m_txArrows->isVisible()); - } - else if(item==m_txFlightMode) - { - QGraphicsSvgItem * svg; - svg=(QGraphicsSvgItem *)item; - if (svg) - { - if(svg->elementId()=="flightModeCenter") - { - if(growing) - { + } else if (item == m_txFlightMode) { + QGraphicsSvgItem *svg; + svg = (QGraphicsSvgItem *)item; + if (svg) { + if (svg->elementId() == "flightModeCenter") { + if (growing) { svg->setElementId("flightModeRight"); - m_txFlightMode->setTransform(m_txFlightModeROrig,false); - } - else - { + m_txFlightMode->setTransform(m_txFlightModeROrig, false); + } else { svg->setElementId("flightModeLeft"); - m_txFlightMode->setTransform(m_txFlightModeLOrig,false); + m_txFlightMode->setTransform(m_txFlightModeLOrig, false); } - } - else if(svg->elementId()=="flightModeRight") - { - growing=false; + } else if (svg->elementId() == "flightModeRight") { + growing = false; svg->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); - } - else if(svg->elementId()=="flightModeLeft") - { - growing=true; + m_txFlightMode->setTransform(m_txFlightModeCOrig, false); + } else if (svg->elementId() == "flightModeLeft") { + growing = true; svg->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + m_txFlightMode->setTransform(m_txFlightModeCOrig, false); } } } - } - else if(move==mix) - { - trans=m_txAccess0Orig; - m_txAccess0->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); - trans=m_txAccess1Orig; - m_txAccess1->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); - trans=m_txAccess2Orig; - m_txAccess2->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); + } else if (move == mix) { + trans = m_txAccess0Orig; + m_txAccess0->setTransform(trans.translate(movePos * 10 * ACCESS_MAX_MOVE / STICK_MAX_MOVE, 0), false); + trans = m_txAccess1Orig; + m_txAccess1->setTransform(trans.translate(movePos * 10 * ACCESS_MAX_MOVE / STICK_MAX_MOVE, 0), false); + trans = m_txAccess2Orig; + m_txAccess2->setTransform(trans.translate(movePos * 10 * ACCESS_MAX_MOVE / STICK_MAX_MOVE, 0), false); - if(auxFlag) - { - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(0,movePos*10),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(0,movePos*10),false); - } - else - { - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(movePos*10,0),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(movePos*10,0),false); + if (auxFlag) { + trans = m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(0, movePos * 10), false); + trans = m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(0, movePos * 10), false); + } else { + trans = m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(movePos * 10, 0), false); + trans = m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(movePos * 10, 0), false); } - if(movePos==0) - { + if (movePos == 0) { m_txFlightMode->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); - } - else if(movePos==ACCESS_MAX_MOVE/2) - { + m_txFlightMode->setTransform(m_txFlightModeCOrig, false); + } else if (movePos == ACCESS_MAX_MOVE / 2) { m_txFlightMode->setElementId("flightModeRight"); - m_txFlightMode->setTransform(m_txFlightModeROrig,false); - } - else if(movePos==ACCESS_MIN_MOVE/2) - { + m_txFlightMode->setTransform(m_txFlightModeROrig, false); + } else if (movePos == ACCESS_MIN_MOVE / 2) { m_txFlightMode->setElementId("flightModeLeft"); - m_txFlightMode->setTransform(m_txFlightModeLOrig,false); + m_txFlightMode->setTransform(m_txFlightModeLOrig, false); } } - if(move==horizontal || move==vertical ||move==mix) - { - if(movePos==0 && growing) - auxFlag=!auxFlag; - if(growing) - ++movePos; - else - --movePos; - if(movePos>limitMax) - { - movePos=movePos-2; - growing=false; + if (move == horizontal || move == vertical || move == mix) { + if (movePos == 0 && growing) { + auxFlag = !auxFlag; } - if(movePos limitMax) { + movePos = movePos - 2; + growing = false; + } + if (movePos < limitMin) { + movePos = movePos + 2; + growing = true; } } } @@ -1198,68 +1176,66 @@ void ConfigInputWidget::moveTxControls() void ConfigInputWidget::moveSticks() { QTransform trans; - manualCommandData=manualCommandObj->getData(); - flightStatusData=flightStatusObj->getData(); - accessoryDesiredData0=accessoryDesiredObj0->getData(); - accessoryDesiredData1=accessoryDesiredObj1->getData(); - accessoryDesiredData2=accessoryDesiredObj2->getData(); + + manualCommandData = manualCommandObj->getData(); + flightStatusData = flightStatusObj->getData(); + accessoryDesiredData0 = accessoryDesiredObj0->getData(); + accessoryDesiredData1 = accessoryDesiredObj1->getData(); + accessoryDesiredData2 = accessoryDesiredObj2->getData(); switch (transmitterMode) { - case mode1: - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); - break; - case mode2: - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); - break; - case mode3: - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); - break; - case mode4: - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); - break; - default: - Q_ASSERT(0); - break; + case mode1: + trans = m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw * STICK_MAX_MOVE * 10, manualCommandData.Pitch * STICK_MAX_MOVE * 10), false); + trans = m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(manualCommandData.Roll * STICK_MAX_MOVE * 10, -manualCommandData.Throttle * STICK_MAX_MOVE * 10), false); + break; + case mode2: + trans = m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw * STICK_MAX_MOVE * 10, -manualCommandData.Throttle * STICK_MAX_MOVE * 10), false); + trans = m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(manualCommandData.Roll * STICK_MAX_MOVE * 10, manualCommandData.Pitch * STICK_MAX_MOVE * 10), false); + break; + case mode3: + trans = m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(manualCommandData.Roll * STICK_MAX_MOVE * 10, manualCommandData.Pitch * STICK_MAX_MOVE * 10), false); + trans = m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(manualCommandData.Yaw * STICK_MAX_MOVE * 10, -manualCommandData.Throttle * STICK_MAX_MOVE * 10), false); + break; + case mode4: + trans = m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(manualCommandData.Roll * STICK_MAX_MOVE * 10, -manualCommandData.Throttle * STICK_MAX_MOVE * 10), false); + trans = m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(manualCommandData.Yaw * STICK_MAX_MOVE * 10, manualCommandData.Pitch * STICK_MAX_MOVE * 10), false); + break; + default: + Q_ASSERT(0); + break; } - if(flightStatusData.FlightMode==manualSettingsData.FlightModePosition[0]) - { + if (flightStatusData.FlightMode == manualSettingsData.FlightModePosition[0]) { m_txFlightMode->setElementId("flightModeLeft"); - m_txFlightMode->setTransform(m_txFlightModeLOrig,false); - } - else if (flightStatusData.FlightMode==manualSettingsData.FlightModePosition[1]) - { + m_txFlightMode->setTransform(m_txFlightModeLOrig, false); + } else if (flightStatusData.FlightMode == manualSettingsData.FlightModePosition[1]) { m_txFlightMode->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); - } - else if (flightStatusData.FlightMode==manualSettingsData.FlightModePosition[2]) - { + m_txFlightMode->setTransform(m_txFlightModeCOrig, false); + } else if (flightStatusData.FlightMode == manualSettingsData.FlightModePosition[2]) { m_txFlightMode->setElementId("flightModeRight"); - m_txFlightMode->setTransform(m_txFlightModeROrig,false); + m_txFlightMode->setTransform(m_txFlightModeROrig, false); } - m_txAccess0->setTransform(QTransform(m_txAccess0Orig).translate(accessoryDesiredData0.AccessoryVal*ACCESS_MAX_MOVE*10,0),false); - m_txAccess1->setTransform(QTransform(m_txAccess1Orig).translate(accessoryDesiredData1.AccessoryVal*ACCESS_MAX_MOVE*10,0),false); - m_txAccess2->setTransform(QTransform(m_txAccess2Orig).translate(accessoryDesiredData2.AccessoryVal*ACCESS_MAX_MOVE*10,0),false); + m_txAccess0->setTransform(QTransform(m_txAccess0Orig).translate(accessoryDesiredData0.AccessoryVal * ACCESS_MAX_MOVE * 10, 0), false); + m_txAccess1->setTransform(QTransform(m_txAccess1Orig).translate(accessoryDesiredData1.AccessoryVal * ACCESS_MAX_MOVE * 10, 0), false); + m_txAccess2->setTransform(QTransform(m_txAccess2Orig).translate(accessoryDesiredData2.AccessoryVal * ACCESS_MAX_MOVE * 10, 0), false); } void ConfigInputWidget::dimOtherControls(bool value) { qreal opac; - if(value) - opac=0.1; - else - opac=1; + + if (value) { + opac = 0.1; + } else { + opac = 1; + } m_txAccess0->setOpacity(opac); m_txAccess1->setOpacity(opac); m_txAccess2->setOpacity(opac); @@ -1268,20 +1244,18 @@ void ConfigInputWidget::dimOtherControls(bool value) void ConfigInputWidget::invertControls() { - manualSettingsData=manualSettingsObj->getData(); - foreach(QWidget * wd,extraWidgets) - { - QCheckBox * cb=qobject_cast(wd); - if(cb) - { + manualSettingsData = manualSettingsObj->getData(); + foreach(QWidget * wd, extraWidgets) { + QCheckBox *cb = qobject_cast(wd); + + if (cb) { int index = manualSettingsObj->getField("ChannelNumber")->getElementNames().indexOf(cb->text()); - if((cb->isChecked() && (manualSettingsData.ChannelMax[index]>manualSettingsData.ChannelMin[index])) || - (!cb->isChecked() && (manualSettingsData.ChannelMax[index]isChecked() && (manualSettingsData.ChannelMax[index] > manualSettingsData.ChannelMin[index])) || + (!cb->isChecked() && (manualSettingsData.ChannelMax[index] < manualSettingsData.ChannelMin[index]))) { qint16 aux; - aux=manualSettingsData.ChannelMax[index]; - manualSettingsData.ChannelMax[index]=manualSettingsData.ChannelMin[index]; - manualSettingsData.ChannelMin[index]=aux; + aux = manualSettingsData.ChannelMax[index]; + manualSettingsData.ChannelMax[index] = manualSettingsData.ChannelMin[index]; + manualSettingsData.ChannelMin[index] = aux; } } } @@ -1291,41 +1265,42 @@ void ConfigInputWidget::invertControls() void ConfigInputWidget::moveFMSlider() { ManualControlSettings::DataFields manualSettingsDataPriv = manualSettingsObj->getData(); - ManualControlCommand::DataFields manualCommandDataPriv = manualCommandObj->getData(); + ManualControlCommand::DataFields manualCommandDataPriv = manualCommandObj->getData(); float valueScaled; - int chMin = manualSettingsDataPriv.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; - int chMax = manualSettingsDataPriv.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]; + int chMin = manualSettingsDataPriv.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; + int chMax = manualSettingsDataPriv.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]; int chNeutral = manualSettingsDataPriv.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]; - int value = manualCommandDataPriv.Channel[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; - if ((chMax > chMin && value >= chNeutral) || (chMin > chMax && value <= chNeutral)) - { - if (chMax != chNeutral) + int value = manualCommandDataPriv.Channel[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; + + if ((chMax > chMin && value >= chNeutral) || (chMin > chMax && value <= chNeutral)) { + if (chMax != chNeutral) { valueScaled = (float)(value - chNeutral) / (float)(chMax - chNeutral); - else + } else { valueScaled = 0; - } - else - { - if (chMin != chNeutral) + } + } else { + if (chMin != chNeutral) { valueScaled = (float)(value - chNeutral) / (float)(chNeutral - chMin); - else + } else { valueScaled = 0; + } } // Bound and scale FlightMode from [-1..+1] to [0..1] range - if (valueScaled < -1.0) + if (valueScaled < -1.0) { valueScaled = -1.0; - else - if (valueScaled > 1.0) - valueScaled = 1.0; + } else if (valueScaled > 1.0) { + valueScaled = 1.0; + } // Convert flightMode value into the switch position in the range [0..N-1] // This uses the same optimized computation as flight code to be consistent uint8_t pos = ((int16_t)(valueScaled * 256) + 256) * manualSettingsDataPriv.FlightModeNumber >> 9; - if (pos >= manualSettingsDataPriv.FlightModeNumber) + if (pos >= manualSettingsDataPriv.FlightModeNumber) { pos = manualSettingsDataPriv.FlightModeNumber - 1; + } ui->fmsSlider->setValue(pos); } @@ -1333,66 +1308,67 @@ void ConfigInputWidget::updatePositionSlider() { ManualControlSettings::DataFields manualSettingsDataPriv = manualSettingsObj->getData(); - switch(manualSettingsDataPriv.FlightModeNumber) { + switch (manualSettingsDataPriv.FlightModeNumber) { default: case 6: ui->fmsModePos6->setEnabled(true); - // pass through + // pass through case 5: ui->fmsModePos5->setEnabled(true); - // pass through + // pass through case 4: ui->fmsModePos4->setEnabled(true); - // pass through + // pass through case 3: ui->fmsModePos3->setEnabled(true); - // pass through + // pass through case 2: ui->fmsModePos2->setEnabled(true); - // pass through + // pass through case 1: ui->fmsModePos1->setEnabled(true); - // pass through + // pass through case 0: - break; + break; } - switch(manualSettingsDataPriv.FlightModeNumber) { + switch (manualSettingsDataPriv.FlightModeNumber) { case 0: ui->fmsModePos1->setEnabled(false); - // pass through + // pass through case 1: ui->fmsModePos2->setEnabled(false); - // pass through + // pass through case 2: ui->fmsModePos3->setEnabled(false); - // pass through + // pass through case 3: ui->fmsModePos4->setEnabled(false); - // pass through + // pass through case 4: ui->fmsModePos5->setEnabled(false); - // pass through + // pass through case 5: ui->fmsModePos6->setEnabled(false); - // pass through + // pass through case 6: default: - break; + break; } } void ConfigInputWidget::updateCalibration() { - manualCommandData=manualCommandObj->getData(); - for(uint i=0;imanualCommandData.Channel[i]) || - (reverse[i] && manualSettingsData.ChannelMin[i]manualCommandData.Channel[i])) - manualSettingsData.ChannelMax[i]=manualCommandData.Channel[i]; + manualCommandData = manualCommandObj->getData(); + for (uint i = 0; i < ManualControlSettings::CHANNELMAX_NUMELEM; ++i) { + if ((!reverse[i] && manualSettingsData.ChannelMin[i] > manualCommandData.Channel[i]) || + (reverse[i] && manualSettingsData.ChannelMin[i] < manualCommandData.Channel[i])) { + manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; + } + if ((!reverse[i] && manualSettingsData.ChannelMax[i] < manualCommandData.Channel[i]) || + (reverse[i] && manualSettingsData.ChannelMax[i] > manualCommandData.Channel[i])) { + manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; + } manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; } @@ -1412,46 +1388,47 @@ void ConfigInputWidget::simpleCalibration(bool enable) msgBox.setDefaultButton(QMessageBox::Ok); msgBox.exec(); - manualCommandData = manualCommandObj->getData(); + manualCommandData = manualCommandObj->getData(); - manualSettingsData=manualSettingsObj->getData(); - manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; + manualSettingsData = manualSettingsObj->getData(); + manualSettingsData.Arming = ManualControlSettings::ARMING_ALWAYSDISARMED; manualSettingsObj->setData(manualSettingsData); for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) { reverse[i] = manualSettingsData.ChannelMax[i] < manualSettingsData.ChannelMin[i]; - manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; + manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; - manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; + manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; } fastMdata(); - connect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(updateCalibration())); + connect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(updateCalibration())); } else { ui->configurationWizard->setEnabled(true); - manualCommandData = manualCommandObj->getData(); + manualCommandData = manualCommandObj->getData(); manualSettingsData = manualSettingsObj->getData(); restoreMdata(); - for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) + for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) { manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; + } // Force flight mode neutral to middle manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE] = - (manualSettingsData.ChannelMax[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE] + - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE]) / 2; + (manualSettingsData.ChannelMax[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE] + + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE]) / 2; // Force throttle to be near min - manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE]= - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]+ - ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE]- - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE] = + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE] + + ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE] - + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]) * 0.02); manualSettingsObj->setData(manualSettingsData); - disconnect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(updateCalibration())); + disconnect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(updateCalibration())); } } diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index be8adedb8..18c6b4209 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -49,124 +49,125 @@ class Ui_InputWidget; -class ConfigInputWidget: public ConfigTaskWidget -{ - Q_OBJECT +class ConfigInputWidget : public ConfigTaskWidget { + Q_OBJECT public: - ConfigInputWidget(QWidget *parent = 0); - ~ConfigInputWidget(); - enum wizardSteps{wizardWelcome,wizardChooseMode,wizardChooseType,wizardIdentifySticks,wizardIdentifyCenter,wizardIdentifyLimits,wizardIdentifyInverted,wizardFinish,wizardNone}; - enum txMode{mode1,mode2,mode3,mode4}; - enum txMovements{moveLeftVerticalStick,moveRightVerticalStick,moveLeftHorizontalStick,moveRightHorizontalStick,moveAccess0,moveAccess1,moveAccess2,moveFlightMode,centerAll,moveAll,nothing}; - enum txMovementType{vertical,horizontal,jump,mix}; - enum txType {acro, heli}; - void startInputWizard() { goToWizard(); } - void enableControls(bool enable); + ConfigInputWidget(QWidget *parent = 0); + ~ConfigInputWidget(); + enum wizardSteps { wizardWelcome, wizardChooseMode, wizardChooseType, wizardIdentifySticks, wizardIdentifyCenter, wizardIdentifyLimits, wizardIdentifyInverted, wizardFinish, wizardNone }; + enum txMode { mode1, mode2, mode3, mode4 }; + enum txMovements { moveLeftVerticalStick, moveRightVerticalStick, moveLeftHorizontalStick, moveRightHorizontalStick, moveAccess0, moveAccess1, moveAccess2, moveFlightMode, centerAll, moveAll, nothing }; + enum txMovementType { vertical, horizontal, jump, mix }; + enum txType { acro, heli }; + void startInputWizard() + { + goToWizard(); + } + void enableControls(bool enable); private: - bool growing; - bool reverse[ManualControlSettings::CHANNELNEUTRAL_NUMELEM]; - txMovements currentMovement; - int movePos; - void setTxMovement(txMovements movement); - Ui_InputWidget *ui; - wizardSteps wizardStep; - QList > extraWidgets; - txMode transmitterMode; - txType transmitterType; - struct channelsStruct + bool growing; + bool reverse[ManualControlSettings::CHANNELNEUTRAL_NUMELEM]; + txMovements currentMovement; + int movePos; + void setTxMovement(txMovements movement); + Ui_InputWidget *ui; + wizardSteps wizardStep; + QList > extraWidgets; + txMode transmitterMode; + txType transmitterType; + struct channelsStruct { + bool operator ==(const channelsStruct & rhs) const { - bool operator ==(const channelsStruct& rhs) const - { - return((group==rhs.group) &&(number==rhs.number)); - } - int group; - int number; - int channelIndex; - }lastChannel; - channelsStruct currentChannel; - QList usedChannels; - bool channelDetected; - QEventLoop * loop; - bool skipflag; + return (group == rhs.group) && (number == rhs.number); + } + int group; + int number; + int channelIndex; + } lastChannel; + channelsStruct currentChannel; + QList usedChannels; + bool channelDetected; + QEventLoop *loop; + bool skipflag; - int currentChannelNum; - QList heliChannelOrder; - QList acroChannelOrder; + int currentChannelNum; + QList heliChannelOrder; + QList acroChannelOrder; - ManualControlCommand * manualCommandObj; - ManualControlCommand::DataFields manualCommandData; - FlightStatus * flightStatusObj; - FlightStatus::DataFields flightStatusData; - AccessoryDesired * accessoryDesiredObj0; - AccessoryDesired * accessoryDesiredObj1; - AccessoryDesired * accessoryDesiredObj2; - AccessoryDesired::DataFields accessoryDesiredData0; - AccessoryDesired::DataFields accessoryDesiredData1; - AccessoryDesired::DataFields accessoryDesiredData2; - UAVObject::Metadata manualControlMdata; - ManualControlSettings * manualSettingsObj; - ManualControlSettings::DataFields manualSettingsData; - ManualControlSettings::DataFields previousManualSettingsData; - ReceiverActivity * receiverActivityObj; - ReceiverActivity::DataFields receiverActivityData; + ManualControlCommand *manualCommandObj; + ManualControlCommand::DataFields manualCommandData; + FlightStatus *flightStatusObj; + FlightStatus::DataFields flightStatusData; + AccessoryDesired *accessoryDesiredObj0; + AccessoryDesired *accessoryDesiredObj1; + AccessoryDesired *accessoryDesiredObj2; + AccessoryDesired::DataFields accessoryDesiredData0; + AccessoryDesired::DataFields accessoryDesiredData1; + AccessoryDesired::DataFields accessoryDesiredData2; + UAVObject::Metadata manualControlMdata; + ManualControlSettings *manualSettingsObj; + ManualControlSettings::DataFields manualSettingsData; + ManualControlSettings::DataFields previousManualSettingsData; + ReceiverActivity *receiverActivityObj; + ReceiverActivity::DataFields receiverActivityData; - QSvgRenderer *m_renderer; + QSvgRenderer *m_renderer; - // Background: background - QGraphicsSvgItem *m_txMainBody; - QGraphicsSvgItem *m_txLeftStick; - QGraphicsSvgItem *m_txRightStick; - QGraphicsSvgItem *m_txAccess0; - QGraphicsSvgItem *m_txAccess1; - QGraphicsSvgItem *m_txAccess2; - QGraphicsSvgItem *m_txFlightMode; - QGraphicsSvgItem *m_txBackground; - QGraphicsSvgItem *m_txArrows; - QTransform m_txLeftStickOrig; - QTransform m_txRightStickOrig; - QTransform m_txAccess0Orig; - QTransform m_txAccess1Orig; - QTransform m_txAccess2Orig; - QTransform m_txFlightModeCOrig; - QTransform m_txFlightModeLOrig; - QTransform m_txFlightModeROrig; - QTransform m_txMainBodyOrig; - QTransform m_txArrowsOrig; - QTimer * animate; - void resetTxControls(); - void setMoveFromCommand(int command); + // Background: background + QGraphicsSvgItem *m_txMainBody; + QGraphicsSvgItem *m_txLeftStick; + QGraphicsSvgItem *m_txRightStick; + QGraphicsSvgItem *m_txAccess0; + QGraphicsSvgItem *m_txAccess1; + QGraphicsSvgItem *m_txAccess2; + QGraphicsSvgItem *m_txFlightMode; + QGraphicsSvgItem *m_txBackground; + QGraphicsSvgItem *m_txArrows; + QTransform m_txLeftStickOrig; + QTransform m_txRightStickOrig; + QTransform m_txAccess0Orig; + QTransform m_txAccess1Orig; + QTransform m_txAccess2Orig; + QTransform m_txFlightModeCOrig; + QTransform m_txFlightModeLOrig; + QTransform m_txFlightModeROrig; + QTransform m_txMainBodyOrig; + QTransform m_txArrowsOrig; + QTimer *animate; + void resetTxControls(); + void setMoveFromCommand(int command); - void fastMdata(); - void restoreMdata(); + void fastMdata(); + void restoreMdata(); - void setChannel(int); - void nextChannel(); - void prevChannel(); + void setChannel(int); + void nextChannel(); + void prevChannel(); - void wizardSetUpStep(enum wizardSteps); - void wizardTearDownStep(enum wizardSteps); + void wizardSetUpStep(enum wizardSteps); + void wizardTearDownStep(enum wizardSteps); private slots: - void wzNext(); - void wzBack(); - void wzCancel(); - void goToWizard(); - void disableWizardButton(int); - void openHelp(); - void identifyControls(); - void identifyLimits(); - void moveTxControls(); - void moveSticks(); - void dimOtherControls(bool value); - void moveFMSlider(); - void updatePositionSlider(); - void invertControls(); - void simpleCalibration(bool state); - void updateCalibration(); + void wzNext(); + void wzBack(); + void wzCancel(); + void goToWizard(); + void disableWizardButton(int); + void openHelp(); + void identifyControls(); + void identifyLimits(); + void moveTxControls(); + void moveSticks(); + void dimOtherControls(bool value); + void moveFMSlider(); + void updatePositionSlider(); + void invertControls(); + void simpleCalibration(bool state); + void updateCalibration(); protected: - void resizeEvent(QResizeEvent *event); + void resizeEvent(QResizeEvent *event); }; -#endif +#endif // ifndef CONFIGINPUTWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp index d71918aba..65a9851af 100644 --- a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp @@ -49,18 +49,18 @@ #include #include -ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(parent),wasItMe(false) +ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(parent), wasItMe(false) { ui = new Ui_OutputWidget(); ui->setupUi(this); - + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Core::Internal::GeneralSettings *settings = pm->getObject(); - if(!settings->useExpertMode()) { + if (!settings->useExpertMode()) { ui->saveRCOutputToRAM->setVisible(false); } - UAVSettingsImportExportFactory *importexportplugin = pm->getObject(); + UAVSettingsImportExportFactory *importexportplugin = pm->getObject(); connect(importexportplugin, SIGNAL(importAboutToBegin()), this, SLOT(stopTests())); connect(ui->channelOutTest, SIGNAL(toggled(bool)), this, SLOT(runChannelTests(bool))); @@ -79,7 +79,7 @@ ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(paren for (unsigned int i = 0; i < ActuatorCommand::CHANNEL_NUMELEM; i++) { OutputChannelForm *form = new OutputChannelForm(i, this, i == 0); connect(ui->channelOutTest, SIGNAL(toggled(bool)), form, SLOT(enableChannelTest(bool))); - connect(form, SIGNAL(channelChanged(int,int)), this, SLOT(sendChannelTest(int,int))); + connect(form, SIGNAL(channelChanged(int, int)), this, SLOT(sendChannelTest(int, int))); ui->channelLayout->addWidget(form); addWidget(form->ui.actuatorMin); addWidget(form->ui.actuatorNeutral); @@ -97,14 +97,14 @@ ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(paren addWidget(ui->cb_outputRate1); addWidget(ui->spinningArmed); - disconnect(this, SLOT(refreshWidgetsValues(UAVObject*))); + disconnect(this, SLOT(refreshWidgetsValues(UAVObject *))); UAVObjectManager *objManager = pm->getObject(); - UAVObject* obj = objManager->getObject(QString("ActuatorCommand")); - if(UAVObject::GetGcsTelemetryUpdateMode(obj->getMetadata()) == UAVObject::UPDATEMODE_ONCHANGE) { + UAVObject *obj = objManager->getObject(QString("ActuatorCommand")); + if (UAVObject::GetGcsTelemetryUpdateMode(obj->getMetadata()) == UAVObject::UPDATEMODE_ONCHANGE) { this->setEnabled(false); } - connect(obj,SIGNAL(objectUpdated(UAVObject*)), this, SLOT(disableIfNotMe(UAVObject*))); + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(disableIfNotMe(UAVObject *))); refreshWidgetsValues(); updateEnableControls(); @@ -113,7 +113,8 @@ ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(paren void ConfigOutputWidget::enableControls(bool enable) { ConfigTaskWidget::enableControls(enable); - if(!enable) { + + if (!enable) { ui->channelOutTest->setChecked(false); } ui->channelOutTest->setEnabled(enable); @@ -121,19 +122,19 @@ void ConfigOutputWidget::enableControls(bool enable) ConfigOutputWidget::~ConfigOutputWidget() { - // Do nothing + // Do nothing } /** - Toggles the channel testing mode by making the GCS take over - the ActuatorCommand objects - */ + Toggles the channel testing mode by making the GCS take over + the ActuatorCommand objects + */ void ConfigOutputWidget::runChannelTests(bool state) { - SystemAlarms * systemAlarmsObj = SystemAlarms::GetInstance(getObjectManager()); + SystemAlarms *systemAlarmsObj = SystemAlarms::GetInstance(getObjectManager()); SystemAlarms::DataFields systemAlarms = systemAlarmsObj->getData(); - if(state && systemAlarms.Alarm[SystemAlarms::ALARM_ACTUATOR] != SystemAlarms::ALARM_OK) { + if (state && systemAlarms.Alarm[SystemAlarms::ALARM_ACTUATOR] != SystemAlarms::ALARM_OK) { QMessageBox mbox; mbox.setText(QString(tr("The actuator module is in an error state. This can also occur because there are no inputs. Please fix these before testing outputs."))); mbox.setStandardButtons(QMessageBox::Ok); @@ -147,12 +148,12 @@ void ConfigOutputWidget::runChannelTests(bool state) } // Confirm this is definitely what they want - if(state) { + if (state) { QMessageBox mbox; mbox.setText(QString(tr("This option will start your motors by the amount selected on the sliders regardless of transmitter. It is recommended to remove any blades from motors. Are you sure you want to do this?"))); mbox.setStandardButtons(QMessageBox::Yes | QMessageBox::No); int retval = mbox.exec(); - if(retval != QMessageBox::Yes) { + if (retval != QMessageBox::Yes) { state = false; qDebug() << "Cancelled"; ui->channelOutTest->setChecked(false); @@ -160,7 +161,7 @@ void ConfigOutputWidget::runChannelTests(bool state) } } - ActuatorCommand * obj = ActuatorCommand::GetInstance(getObjectManager()); + ActuatorCommand *obj = ActuatorCommand::GetInstance(getObjectManager()); UAVObject::Metadata mdata = obj->getMetadata(); if (state) { wasItMe = true; @@ -170,21 +171,19 @@ void ConfigOutputWidget::runChannelTests(bool state) UAVObject::SetGcsTelemetryAcked(mdata, false); UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); mdata.gcsTelemetryUpdatePeriod = 100; - } - else { + } else { wasItMe = false; - mdata = accInitialData; // Restore metadata + mdata = accInitialData; // Restore metadata } obj->setMetadata(mdata); obj->updated(); - } -OutputChannelForm* ConfigOutputWidget::getOutputChannelForm(const int index) const +OutputChannelForm *ConfigOutputWidget::getOutputChannelForm(const int index) const { - QList outputChannelForms = findChildren(); - foreach(OutputChannelForm *outputChannelForm, outputChannelForms) { - if(outputChannelForm->index() == index) { + QList outputChannelForms = findChildren(); + foreach(OutputChannelForm * outputChannelForm, outputChannelForms) { + if (outputChannelForm->index() == index) { return outputChannelForm; } } @@ -194,32 +193,33 @@ OutputChannelForm* ConfigOutputWidget::getOutputChannelForm(const int index) con } /** - * Set the label for a channel output assignement - */ + * Set the label for a channel output assignement + */ void ConfigOutputWidget::assignOutputChannel(UAVDataObject *obj, QString str) { - //FIXME: use signal/ slot approach - UAVObjectField* field = obj->getField(str); - QStringList options = field->getOptions(); + // FIXME: use signal/ slot approach + UAVObjectField *field = obj->getField(str); + QStringList options = field->getOptions(); int index = options.indexOf(field->getValue().toString()); OutputChannelForm *outputChannelForm = getOutputChannelForm(index); - if(outputChannelForm) { + + if (outputChannelForm) { outputChannelForm->setAssignment(str); } } /** - Sends the channel value to the UAV to move the servo. - Returns immediately if we are not in testing mode - */ + Sends the channel value to the UAV to move the servo. + Returns immediately if we are not in testing mode + */ void ConfigOutputWidget::sendChannelTest(int index, int value) { if (!ui->channelOutTest->isChecked()) { return; } - if(index < 0 || (unsigned)index >= ActuatorCommand::CHANNEL_NUMELEM) { + if (index < 0 || (unsigned)index >= ActuatorCommand::CHANNEL_NUMELEM) { return; } @@ -231,15 +231,14 @@ void ConfigOutputWidget::sendChannelTest(int index, int value) } - /******************************** - * Output settings - *******************************/ + * Output settings + *******************************/ /** - Request the current config from the board (RC Output) - */ -void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) + Request the current config from the board (RC Output) + */ +void ConfigOutputWidget::refreshWidgetsValues(UAVObject *obj) { Q_UNUSED(obj); @@ -252,8 +251,8 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) QStringList ChannelDesc = ConfigVehicleTypeWidget::getChannelDescriptions(); // Initialize output forms - QList outputChannelForms = findChildren(); - foreach(OutputChannelForm *outputChannelForm, outputChannelForms) { + QList outputChannelForms = findChildren(); + foreach(OutputChannelForm * outputChannelForm, outputChannelForms) { outputChannelForm->setAssignment(ChannelDesc[outputChannelForm->index()]); // init min,max,neutral @@ -261,7 +260,7 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) int maxValue = actuatorSettingsData.ChannelMax[outputChannelForm->index()]; outputChannelForm->minmax(minValue, maxValue); - int neutral = actuatorSettingsData.ChannelNeutral[outputChannelForm->index()]; + int neutral = actuatorSettingsData.ChannelNeutral[outputChannelForm->index()]; outputChannelForm->neutral(neutral); } @@ -269,22 +268,22 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) ui->spinningArmed->setChecked(actuatorSettingsData.MotorsSpinWhileArmed == ActuatorSettings::MOTORSSPINWHILEARMED_TRUE); // Setup output rates for all banks - if(ui->cb_outputRate1->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[0])) == -1) { + if (ui->cb_outputRate1->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[0])) == -1) { ui->cb_outputRate1->addItem(QString::number(actuatorSettingsData.ChannelUpdateFreq[0])); } - if(ui->cb_outputRate2->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[1])) == -1) { + if (ui->cb_outputRate2->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[1])) == -1) { ui->cb_outputRate2->addItem(QString::number(actuatorSettingsData.ChannelUpdateFreq[1])); } - if(ui->cb_outputRate3->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[2]) )== -1) { + if (ui->cb_outputRate3->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[2])) == -1) { ui->cb_outputRate3->addItem(QString::number(actuatorSettingsData.ChannelUpdateFreq[2])); } - if(ui->cb_outputRate4->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[3])) == -1) { + if (ui->cb_outputRate4->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[3])) == -1) { ui->cb_outputRate4->addItem(QString::number(actuatorSettingsData.ChannelUpdateFreq[3])); } - if(ui->cb_outputRate5->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[4])) == -1) { + if (ui->cb_outputRate5->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[4])) == -1) { ui->cb_outputRate5->addItem(QString::number(actuatorSettingsData.ChannelUpdateFreq[4])); } - if(ui->cb_outputRate6->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[5])) == -1) { + if (ui->cb_outputRate6->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[5])) == -1) { ui->cb_outputRate6->addItem(QString::number(actuatorSettingsData.ChannelUpdateFreq[5])); } ui->cb_outputRate1->setCurrentIndex(ui->cb_outputRate1->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[0]))); @@ -311,10 +310,10 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) // Get connected board model ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Q_ASSERT(pm); - UAVObjectUtilManager *utilMngr = pm->getObject(); + UAVObjectUtilManager *utilMngr = pm->getObject(); Q_ASSERT(utilMngr); - if(utilMngr) { + if (utilMngr) { int board = utilMngr->getBoardModel(); // Setup labels and combos for banks according to board type if ((board & 0xff00) == 0x0400) { @@ -327,8 +326,7 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) ui->cb_outputRate2->setEnabled(true); ui->cb_outputRate3->setEnabled(true); ui->cb_outputRate4->setEnabled(true); - } - else if((board & 0xff00) == 0x0900) { + } else if ((board & 0xff00) == 0x0900) { // Revolution family of boards 6 timer banks ui->chBank1->setText("1-2"); ui->chBank2->setText("3"); @@ -346,9 +344,10 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) } // Get Channel ranges: - foreach(OutputChannelForm *outputChannelForm, outputChannelForms) { - int minValue = actuatorSettingsData.ChannelMin[outputChannelForm->index()]; + foreach(OutputChannelForm * outputChannelForm, outputChannelForms) { + int minValue = actuatorSettingsData.ChannelMin[outputChannelForm->index()]; int maxValue = actuatorSettingsData.ChannelMax[outputChannelForm->index()]; + outputChannelForm->minmax(minValue, maxValue); int neutral = actuatorSettingsData.ChannelNeutral[outputChannelForm->index()]; @@ -357,22 +356,23 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) } /** - * Sends the config to the board, without saving to the SD card (RC Output) - */ + * Sends the config to the board, without saving to the SD card (RC Output) + */ void ConfigOutputWidget::updateObjectsFromWidgets() { emit updateObjectsFromWidgetsRequested(); ActuatorSettings *actuatorSettings = ActuatorSettings::GetInstance(getObjectManager()); + Q_ASSERT(actuatorSettings); - if(actuatorSettings) { + if (actuatorSettings) { ActuatorSettings::DataFields actuatorSettingsData = actuatorSettings->getData(); // Set channel ranges - QList outputChannelForms = findChildren(); - foreach(OutputChannelForm *outputChannelForm, outputChannelForms) { - actuatorSettingsData.ChannelMax[outputChannelForm->index()] = outputChannelForm->max(); - actuatorSettingsData.ChannelMin[outputChannelForm->index()] = outputChannelForm->min(); + QList outputChannelForms = findChildren(); + foreach(OutputChannelForm * outputChannelForm, outputChannelForms) { + actuatorSettingsData.ChannelMax[outputChannelForm->index()] = outputChannelForm->max(); + actuatorSettingsData.ChannelMin[outputChannelForm->index()] = outputChannelForm->min(); actuatorSettingsData.ChannelNeutral[outputChannelForm->index()] = outputChannelForm->neutral(); } @@ -385,8 +385,8 @@ void ConfigOutputWidget::updateObjectsFromWidgets() actuatorSettingsData.ChannelUpdateFreq[5] = ui->cb_outputRate6->currentText().toUInt(); actuatorSettingsData.MotorsSpinWhileArmed = ui->spinningArmed->isChecked() ? - ActuatorSettings::MOTORSSPINWHILEARMED_TRUE : - ActuatorSettings::MOTORSSPINWHILEARMED_FALSE; + ActuatorSettings::MOTORSSPINWHILEARMED_TRUE : + ActuatorSettings::MOTORSSPINWHILEARMED_FALSE; // Apply settings actuatorSettings->setData(actuatorSettingsData); @@ -395,7 +395,7 @@ void ConfigOutputWidget::updateObjectsFromWidgets() void ConfigOutputWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/WIGf", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/WIGf", QUrl::StrictMode)); } void ConfigOutputWidget::stopTests() @@ -403,14 +403,13 @@ void ConfigOutputWidget::stopTests() ui->channelOutTest->setChecked(false); } -void ConfigOutputWidget::disableIfNotMe(UAVObject* obj) +void ConfigOutputWidget::disableIfNotMe(UAVObject *obj) { - if(UAVObject::GetGcsTelemetryUpdateMode(obj->getMetadata()) == UAVObject::UPDATEMODE_ONCHANGE) { - if(!wasItMe) { + if (UAVObject::GetGcsTelemetryUpdateMode(obj->getMetadata()) == UAVObject::UPDATEMODE_ONCHANGE) { + if (!wasItMe) { this->setEnabled(false); } - } - else { + } else { this->setEnabled(true); } } diff --git a/ground/openpilotgcs/src/plugins/config/configoutputwidget.h b/ground/openpilotgcs/src/plugins/config/configoutputwidget.h index 1d15c8de5..1c12f64c0 100644 --- a/ground/openpilotgcs/src/plugins/config/configoutputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configoutputwidget.h @@ -40,40 +40,39 @@ class Ui_OutputWidget; class OutputChannelForm; -class ConfigOutputWidget: public ConfigTaskWidget -{ - Q_OBJECT +class ConfigOutputWidget : public ConfigTaskWidget { + Q_OBJECT public: - ConfigOutputWidget(QWidget *parent = 0); - ~ConfigOutputWidget(); + ConfigOutputWidget(QWidget *parent = 0); + ~ConfigOutputWidget(); private: Ui_OutputWidget *ui; - QList sliders; + QList sliders; - void updateChannelInSlider(QSlider *slider, QLabel *min, QLabel *max, QCheckBox *rev, int value); + void updateChannelInSlider(QSlider *slider, QLabel *min, QLabel *max, QCheckBox *rev, int value); - void assignChannel(UAVDataObject *obj, QString str); - void assignOutputChannel(UAVDataObject *obj, QString str); - OutputChannelForm* getOutputChannelForm(const int index) const; - int mccDataRate; + void assignChannel(UAVDataObject *obj, QString str); + void assignOutputChannel(UAVDataObject *obj, QString str); + OutputChannelForm *getOutputChannelForm(const int index) const; + int mccDataRate; - UAVObject::Metadata accInitialData; + UAVObject::Metadata accInitialData; - bool wasItMe; + bool wasItMe; private slots: - void stopTests(); - void disableIfNotMe(UAVObject *obj); - virtual void refreshWidgetsValues(UAVObject * obj=NULL); - void updateObjectsFromWidgets(); - void runChannelTests(bool state); - void sendChannelTest(int index, int value); - void openHelp(); + void stopTests(); + void disableIfNotMe(UAVObject *obj); + virtual void refreshWidgetsValues(UAVObject *obj = NULL); + void updateObjectsFromWidgets(); + void runChannelTests(bool state); + void sendChannelTest(int index, int value); + void openHelp(); protected: - void enableControls(bool enable); + void enableControls(bool enable); }; -#endif +#endif // ifndef CONFIGOUTPUTWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index a0e4c43de..9bbbab0d9 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -1,14 +1,14 @@ /** -****************************************************************************** -* -* @file configtxpidswidget.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup ConfigPlugin Config Plugin -* @{ -* @brief The Configuration Gadget used to configure the PipXtreme -*****************************************************************************/ + ****************************************************************************** + * + * @file configtxpidswidget.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief The Configuration Gadget used to configure the PipXtreme + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -33,148 +33,149 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget(parent) { - m_oplink = new Ui_PipXtremeWidget(); - m_oplink->setupUi(this); + m_oplink = new Ui_PipXtremeWidget(); + m_oplink->setupUi(this); - // Connect to the OPLinkStatus object updates - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - oplinkStatusObj = dynamic_cast(objManager->getObject("OPLinkStatus")); - if (oplinkStatusObj != NULL ) { - connect(oplinkStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateStatus(UAVObject*))); - } else { - qDebug() << "Error: Object is unknown (OPLinkStatus)."; - } + // Connect to the OPLinkStatus object updates + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + oplinkStatusObj = dynamic_cast(objManager->getObject("OPLinkStatus")); + if (oplinkStatusObj != NULL) { + connect(oplinkStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateStatus(UAVObject *))); + } else { + qDebug() << "Error: Object is unknown (OPLinkStatus)."; + } - // Connect to the OPLinkSettings object updates - oplinkSettingsObj = dynamic_cast(objManager->getObject("OPLinkSettings")); - if (oplinkSettingsObj != NULL ) { - connect(oplinkSettingsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateSettings(UAVObject*))); - } else { - qDebug() << "Error: Object is unknown (OPLinkSettings)."; - } - autoLoadWidgets(); - Core::Internal::GeneralSettings *settings = pm->getObject(); - if(!settings->useExpertMode()) - m_oplink->Apply->setVisible(false); - addApplySaveButtons(m_oplink->Apply, m_oplink->Save); + // Connect to the OPLinkSettings object updates + oplinkSettingsObj = dynamic_cast(objManager->getObject("OPLinkSettings")); + if (oplinkSettingsObj != NULL) { + connect(oplinkSettingsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateSettings(UAVObject *))); + } else { + qDebug() << "Error: Object is unknown (OPLinkSettings)."; + } + autoLoadWidgets(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { + m_oplink->Apply->setVisible(false); + } + addApplySaveButtons(m_oplink->Apply, m_oplink->Save); - addUAVObjectToWidgetRelation("OPLinkSettings", "MainPort", m_oplink->MainPort); - addUAVObjectToWidgetRelation("OPLinkSettings", "FlexiPort", m_oplink->FlexiPort); - addUAVObjectToWidgetRelation("OPLinkSettings", "VCPPort", m_oplink->VCPPort); - addUAVObjectToWidgetRelation("OPLinkSettings", "MaxRFPower", m_oplink->MaxRFTxPower); - addUAVObjectToWidgetRelation("OPLinkSettings", "MinFrequency", m_oplink->MinimumFrequency); - addUAVObjectToWidgetRelation("OPLinkSettings", "MaxFrequency", m_oplink->MaximumFrequency); - addUAVObjectToWidgetRelation("OPLinkSettings", "InitFrequency", m_oplink->InitFrequency); - addUAVObjectToWidgetRelation("OPLinkSettings", "ChannelSpacing", m_oplink->StepSize); + addUAVObjectToWidgetRelation("OPLinkSettings", "MainPort", m_oplink->MainPort); + addUAVObjectToWidgetRelation("OPLinkSettings", "FlexiPort", m_oplink->FlexiPort); + addUAVObjectToWidgetRelation("OPLinkSettings", "VCPPort", m_oplink->VCPPort); + addUAVObjectToWidgetRelation("OPLinkSettings", "MaxRFPower", m_oplink->MaxRFTxPower); + addUAVObjectToWidgetRelation("OPLinkSettings", "MinFrequency", m_oplink->MinimumFrequency); + addUAVObjectToWidgetRelation("OPLinkSettings", "MaxFrequency", m_oplink->MaximumFrequency); + addUAVObjectToWidgetRelation("OPLinkSettings", "InitFrequency", m_oplink->InitFrequency); + addUAVObjectToWidgetRelation("OPLinkSettings", "ChannelSpacing", m_oplink->StepSize); - addUAVObjectToWidgetRelation("OPLinkStatus", "DeviceID", m_oplink->DeviceID); - addUAVObjectToWidgetRelation("OPLinkStatus", "RxGood", m_oplink->Good); - addUAVObjectToWidgetRelation("OPLinkStatus", "RxCorrected", m_oplink->Corrected); - addUAVObjectToWidgetRelation("OPLinkStatus", "RxErrors", m_oplink->Errors); - addUAVObjectToWidgetRelation("OPLinkStatus", "RxMissed", m_oplink->Missed); - addUAVObjectToWidgetRelation("OPLinkStatus", "RxFailure", m_oplink->RxFailure); - addUAVObjectToWidgetRelation("OPLinkStatus", "UAVTalkErrors", m_oplink->UAVTalkErrors); - addUAVObjectToWidgetRelation("OPLinkStatus", "TxDropped", m_oplink->Dropped); - addUAVObjectToWidgetRelation("OPLinkStatus", "TxResent", m_oplink->Resent); - addUAVObjectToWidgetRelation("OPLinkStatus", "TxFailure", m_oplink->TxFailure); - addUAVObjectToWidgetRelation("OPLinkStatus", "Resets", m_oplink->Resets); - addUAVObjectToWidgetRelation("OPLinkStatus", "Timeouts", m_oplink->Timeouts); - addUAVObjectToWidgetRelation("OPLinkStatus", "RSSI", m_oplink->RSSI); - addUAVObjectToWidgetRelation("OPLinkStatus", "HeapRemaining", m_oplink->FreeHeap); - addUAVObjectToWidgetRelation("OPLinkStatus", "LinkQuality", m_oplink->LinkQuality); - addUAVObjectToWidgetRelation("OPLinkStatus", "RXSeq", m_oplink->RXSeq); - addUAVObjectToWidgetRelation("OPLinkStatus", "TXSeq", m_oplink->TXSeq); - addUAVObjectToWidgetRelation("OPLinkStatus", "RXRate", m_oplink->RXRate); - addUAVObjectToWidgetRelation("OPLinkStatus", "TXRate", m_oplink->TXRate); + addUAVObjectToWidgetRelation("OPLinkStatus", "DeviceID", m_oplink->DeviceID); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxGood", m_oplink->Good); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxCorrected", m_oplink->Corrected); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxErrors", m_oplink->Errors); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxMissed", m_oplink->Missed); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxFailure", m_oplink->RxFailure); + addUAVObjectToWidgetRelation("OPLinkStatus", "UAVTalkErrors", m_oplink->UAVTalkErrors); + addUAVObjectToWidgetRelation("OPLinkStatus", "TxDropped", m_oplink->Dropped); + addUAVObjectToWidgetRelation("OPLinkStatus", "TxResent", m_oplink->Resent); + addUAVObjectToWidgetRelation("OPLinkStatus", "TxFailure", m_oplink->TxFailure); + addUAVObjectToWidgetRelation("OPLinkStatus", "Resets", m_oplink->Resets); + addUAVObjectToWidgetRelation("OPLinkStatus", "Timeouts", m_oplink->Timeouts); + addUAVObjectToWidgetRelation("OPLinkStatus", "RSSI", m_oplink->RSSI); + addUAVObjectToWidgetRelation("OPLinkStatus", "HeapRemaining", m_oplink->FreeHeap); + addUAVObjectToWidgetRelation("OPLinkStatus", "LinkQuality", m_oplink->LinkQuality); + addUAVObjectToWidgetRelation("OPLinkStatus", "RXSeq", m_oplink->RXSeq); + addUAVObjectToWidgetRelation("OPLinkStatus", "TXSeq", m_oplink->TXSeq); + addUAVObjectToWidgetRelation("OPLinkStatus", "RXRate", m_oplink->RXRate); + addUAVObjectToWidgetRelation("OPLinkStatus", "TXRate", m_oplink->TXRate); - signalMapperAddBinding = new QSignalMapper(this); - signalMapperRemBinding = new QSignalMapper(this); - connect(m_oplink->BindingAdd_1, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_1, (QWidget*)(m_oplink->BindingID_1)); - connect(m_oplink->BindingRemove_1, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_1, (QWidget*)(m_oplink->BindingID_1)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_1, "0"); - connect(m_oplink->BindingAdd_2, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_2, (QWidget*)(m_oplink->BindingID_2)); - connect(m_oplink->BindingRemove_2, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_2, (QWidget*)(m_oplink->BindingID_2)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_2, "1"); - connect(m_oplink->BindingAdd_3, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_3, (QWidget*)(m_oplink->BindingID_3)); - connect(m_oplink->BindingRemove_3, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_3, (QWidget*)(m_oplink->BindingID_3)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_3, "2"); - connect(m_oplink->BindingAdd_4, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_4, (QWidget*)(m_oplink->BindingID_4)); - connect(m_oplink->BindingRemove_4, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_4, (QWidget*)(m_oplink->BindingID_4)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_4, "3"); - connect(m_oplink->BindingAdd_5, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_5, (QWidget*)(m_oplink->BindingID_5)); - connect(m_oplink->BindingRemove_5, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_5, (QWidget*)(m_oplink->BindingID_5)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_5, "4"); - connect(m_oplink->BindingAdd_6, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_6, (QWidget*)(m_oplink->BindingID_6)); - connect(m_oplink->BindingRemove_6, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_6, (QWidget*)(m_oplink->BindingID_6)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_6, "5"); - connect(m_oplink->BindingAdd_7, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_7, (QWidget*)(m_oplink->BindingID_7)); - connect(m_oplink->BindingRemove_7, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_7, (QWidget*)(m_oplink->BindingID_7)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_7, "6"); - connect(m_oplink->BindingAdd_8, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_8, (QWidget*)(m_oplink->BindingID_8)); - connect(m_oplink->BindingRemove_8, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_8, (QWidget*)(m_oplink->BindingID_8)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_8, "7"); - connect(signalMapperAddBinding, SIGNAL(mapped(QWidget *)), this, SLOT(addBinding(QWidget *))); - connect(signalMapperRemBinding, SIGNAL(mapped(QWidget *)), this, SLOT(removeBinding(QWidget *))); + signalMapperAddBinding = new QSignalMapper(this); + signalMapperRemBinding = new QSignalMapper(this); + connect(m_oplink->BindingAdd_1, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_1, (QWidget *)(m_oplink->BindingID_1)); + connect(m_oplink->BindingRemove_1, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_1, (QWidget *)(m_oplink->BindingID_1)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_1, "0"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_1, "0"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_1, "0"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_1, "0"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_1, "0"); + connect(m_oplink->BindingAdd_2, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_2, (QWidget *)(m_oplink->BindingID_2)); + connect(m_oplink->BindingRemove_2, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_2, (QWidget *)(m_oplink->BindingID_2)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_2, "1"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_2, "1"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_2, "1"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_2, "1"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_2, "1"); + connect(m_oplink->BindingAdd_3, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_3, (QWidget *)(m_oplink->BindingID_3)); + connect(m_oplink->BindingRemove_3, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_3, (QWidget *)(m_oplink->BindingID_3)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_3, "2"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_3, "2"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_3, "2"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_3, "2"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_3, "2"); + connect(m_oplink->BindingAdd_4, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_4, (QWidget *)(m_oplink->BindingID_4)); + connect(m_oplink->BindingRemove_4, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_4, (QWidget *)(m_oplink->BindingID_4)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_4, "3"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_4, "3"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_4, "3"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_4, "3"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_4, "3"); + connect(m_oplink->BindingAdd_5, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_5, (QWidget *)(m_oplink->BindingID_5)); + connect(m_oplink->BindingRemove_5, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_5, (QWidget *)(m_oplink->BindingID_5)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_5, "4"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_5, "4"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_5, "4"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_5, "4"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_5, "4"); + connect(m_oplink->BindingAdd_6, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_6, (QWidget *)(m_oplink->BindingID_6)); + connect(m_oplink->BindingRemove_6, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_6, (QWidget *)(m_oplink->BindingID_6)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_6, "5"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_6, "5"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_6, "5"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_6, "5"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_6, "5"); + connect(m_oplink->BindingAdd_7, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_7, (QWidget *)(m_oplink->BindingID_7)); + connect(m_oplink->BindingRemove_7, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_7, (QWidget *)(m_oplink->BindingID_7)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_7, "6"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_7, "6"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_7, "6"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_7, "6"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_7, "6"); + connect(m_oplink->BindingAdd_8, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_8, (QWidget *)(m_oplink->BindingID_8)); + connect(m_oplink->BindingRemove_8, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_8, (QWidget *)(m_oplink->BindingID_8)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_8, "7"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_8, "7"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_8, "7"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_8, "7"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_8, "7"); + connect(signalMapperAddBinding, SIGNAL(mapped(QWidget *)), this, SLOT(addBinding(QWidget *))); + connect(signalMapperRemBinding, SIGNAL(mapped(QWidget *)), this, SLOT(removeBinding(QWidget *))); - //Add scroll bar when necessary - QScrollArea *scroll = new QScrollArea; - scroll->setWidget(m_oplink->frame_3); - scroll->setWidgetResizable(true); - m_oplink->verticalLayout_3->addWidget(scroll); + // Add scroll bar when necessary + QScrollArea *scroll = new QScrollArea; + scroll->setWidget(m_oplink->frame_3); + scroll->setWidgetResizable(true); + m_oplink->verticalLayout_3->addWidget(scroll); - // Request and update of the setting object. - settingsUpdated = false; + // Request and update of the setting object. + settingsUpdated = false; - disableMouseWheelEvents(); + disableMouseWheelEvents(); } ConfigPipXtremeWidget::~ConfigPipXtremeWidget() @@ -184,145 +185,143 @@ ConfigPipXtremeWidget::~ConfigPipXtremeWidget() } /*! - \brief Called by updates to @OPLinkStatus - */ + \brief Called by updates to @OPLinkStatus + */ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) { + // Request and update of the setting object if we haven't received it yet. + if (!settingsUpdated) { + oplinkSettingsObj->requestUpdate(); + } - // Request and update of the setting object if we haven't received it yet. - if (!settingsUpdated) - oplinkSettingsObj->requestUpdate(); + // Update the detected devices. + UAVObjectField *pairIdField = object->getField("PairIDs"); + if (pairIdField) { + quint32 pairid1 = pairIdField->getValue(0).toUInt(); + m_oplink->PairID1->setText(QString::number(pairid1, 16).toUpper()); + m_oplink->PairID1->setEnabled(false); + m_oplink->PairSelect1->setEnabled(pairid1); + quint32 pairid2 = pairIdField->getValue(1).toUInt(); + m_oplink->PairID2->setText(QString::number(pairIdField->getValue(1).toUInt(), 16).toUpper()); + m_oplink->PairID2->setEnabled(false); + m_oplink->PairSelect2->setEnabled(pairid2); + quint32 pairid3 = pairIdField->getValue(2).toUInt(); + m_oplink->PairID3->setText(QString::number(pairIdField->getValue(2).toUInt(), 16).toUpper()); + m_oplink->PairID3->setEnabled(false); + m_oplink->PairSelect3->setEnabled(pairid3); + quint32 pairid4 = pairIdField->getValue(3).toUInt(); + m_oplink->PairID4->setText(QString::number(pairIdField->getValue(3).toUInt(), 16).toUpper()); + m_oplink->PairID4->setEnabled(false); + m_oplink->PairSelect4->setEnabled(pairid4); + } else { + qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; + } + UAVObjectField *pairRssiField = object->getField("PairSignalStrengths"); + if (pairRssiField) { + m_oplink->PairSignalStrengthBar1->setValue(pairRssiField->getValue(0).toInt()); + m_oplink->PairSignalStrengthBar2->setValue(pairRssiField->getValue(1).toInt()); + m_oplink->PairSignalStrengthBar3->setValue(pairRssiField->getValue(2).toInt()); + m_oplink->PairSignalStrengthBar4->setValue(pairRssiField->getValue(3).toInt()); + m_oplink->PairSignalStrengthLabel1->setText(QString("%1dB").arg(pairRssiField->getValue(0).toInt())); + m_oplink->PairSignalStrengthLabel2->setText(QString("%1dB").arg(pairRssiField->getValue(1).toInt())); + m_oplink->PairSignalStrengthLabel3->setText(QString("%1dB").arg(pairRssiField->getValue(2).toInt())); + m_oplink->PairSignalStrengthLabel4->setText(QString("%1dB").arg(pairRssiField->getValue(3).toInt())); + } else { + qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; + } - // Update the detected devices. - UAVObjectField* pairIdField = object->getField("PairIDs"); - if (pairIdField) { - quint32 pairid1 = pairIdField->getValue(0).toUInt(); - m_oplink->PairID1->setText(QString::number(pairid1, 16).toUpper()); - m_oplink->PairID1->setEnabled(false); - m_oplink->PairSelect1->setEnabled(pairid1); - quint32 pairid2 = pairIdField->getValue(1).toUInt(); - m_oplink->PairID2->setText(QString::number(pairIdField->getValue(1).toUInt(), 16).toUpper()); - m_oplink->PairID2->setEnabled(false); - m_oplink->PairSelect2->setEnabled(pairid2); - quint32 pairid3 = pairIdField->getValue(2).toUInt(); - m_oplink->PairID3->setText(QString::number(pairIdField->getValue(2).toUInt(), 16).toUpper()); - m_oplink->PairID3->setEnabled(false); - m_oplink->PairSelect3->setEnabled(pairid3); - quint32 pairid4 = pairIdField->getValue(3).toUInt(); - m_oplink->PairID4->setText(QString::number(pairIdField->getValue(3).toUInt(), 16).toUpper()); - m_oplink->PairID4->setEnabled(false); - m_oplink->PairSelect4->setEnabled(pairid4); - } else { - qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; - } - UAVObjectField* pairRssiField = object->getField("PairSignalStrengths"); - if (pairRssiField) { - m_oplink->PairSignalStrengthBar1->setValue(pairRssiField->getValue(0).toInt()); - m_oplink->PairSignalStrengthBar2->setValue(pairRssiField->getValue(1).toInt()); - m_oplink->PairSignalStrengthBar3->setValue(pairRssiField->getValue(2).toInt()); - m_oplink->PairSignalStrengthBar4->setValue(pairRssiField->getValue(3).toInt()); - m_oplink->PairSignalStrengthLabel1->setText(QString("%1dB").arg(pairRssiField->getValue(0).toInt())); - m_oplink->PairSignalStrengthLabel2->setText(QString("%1dB").arg(pairRssiField->getValue(1).toInt())); - m_oplink->PairSignalStrengthLabel3->setText(QString("%1dB").arg(pairRssiField->getValue(2).toInt())); - m_oplink->PairSignalStrengthLabel4->setText(QString("%1dB").arg(pairRssiField->getValue(3).toInt())); - } else { - qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; - } + // Update the Description field + UAVObjectField *descField = object->getField("Description"); + if (descField) { + /* + * This looks like a binary with a description at the end + * 4 bytes: header: "OpFw" + * 4 bytes: git commit hash (short version of SHA1) + * 4 bytes: Unix timestamp of last git commit + * 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files. + * 26 bytes: commit tag if it is there, otherwise "Unreleased". Zero-padded + * ---- 40 bytes limit --- + * 20 bytes: SHA1 sum of the firmware. + * 40 bytes: free for now. + */ + char buf[OPLinkStatus::DESCRIPTION_NUMELEM]; + for (unsigned int i = 0; i < 26; ++i) { + buf[i] = descField->getValue(i + 14).toChar().toAscii(); + } + buf[26] = '\0'; + QString descstr(buf); + quint32 gitDate = descField->getValue(11).toChar().toAscii() & 0xFF; + for (int i = 1; i < 4; i++) { + gitDate = gitDate << 8; + gitDate += descField->getValue(11 - i).toChar().toAscii() & 0xFF; + } + QString date = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyy-MM-dd HH:mm"); + m_oplink->FirmwareVersion->setText(descstr + " " + date); + } else { + qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; + } - // Update the Description field - UAVObjectField* descField = object->getField("Description"); - if (descField) { - /* - * This looks like a binary with a description at the end - * 4 bytes: header: "OpFw" - * 4 bytes: git commit hash (short version of SHA1) - * 4 bytes: Unix timestamp of last git commit - * 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files. - * 26 bytes: commit tag if it is there, otherwise "Unreleased". Zero-padded - * ---- 40 bytes limit --- - * 20 bytes: SHA1 sum of the firmware. - * 40 bytes: free for now. - */ - char buf[OPLinkStatus::DESCRIPTION_NUMELEM]; - for (unsigned int i = 0; i < 26; ++i) - buf[i] = descField->getValue(i + 14).toChar().toAscii(); - buf[26] = '\0'; - QString descstr(buf); - quint32 gitDate = descField->getValue(11).toChar().toAscii() & 0xFF; - for (int i = 1; i < 4; i++) { - gitDate = gitDate << 8; - gitDate += descField->getValue(11-i).toChar().toAscii() & 0xFF; - } - QString date = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyy-MM-dd HH:mm"); - m_oplink->FirmwareVersion->setText(descstr + " " + date); - } else { - qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; - } + // Update the serial number field + UAVObjectField *serialField = object->getField("CPUSerial"); + if (serialField) { + char buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2 + 1]; + for (unsigned int i = 0; i < OPLinkStatus::CPUSERIAL_NUMELEM; ++i) { + unsigned char val = serialField->getValue(i).toUInt() >> 4; + buf[i * 2] = ((val < 10) ? '0' : '7') + val; + val = serialField->getValue(i).toUInt() & 0xf; + buf[i * 2 + 1] = ((val < 10) ? '0' : '7') + val; + } + buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2] = '\0'; + m_oplink->SerialNumber->setText(buf); + } else { + qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; + } - // Update the serial number field - UAVObjectField* serialField = object->getField("CPUSerial"); - if (serialField) { - char buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2 + 1]; - for (unsigned int i = 0; i < OPLinkStatus::CPUSERIAL_NUMELEM; ++i) - { - unsigned char val = serialField->getValue(i).toUInt() >> 4; - buf[i * 2] = ((val < 10) ? '0' : '7') + val; - val = serialField->getValue(i).toUInt() & 0xf; - buf[i * 2 + 1] = ((val < 10) ? '0' : '7') + val; - } - buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2] = '\0'; - m_oplink->SerialNumber->setText(buf); - } else { - qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; - } - - // Update the link state - UAVObjectField* linkField = object->getField("LinkState"); - if (linkField) { - m_oplink->LinkState->setText(linkField->getValue().toString()); - } else { - qDebug() << "PipXtremeGadgetWidget: Count not read link state field."; - } + // Update the link state + UAVObjectField *linkField = object->getField("LinkState"); + if (linkField) { + m_oplink->LinkState->setText(linkField->getValue().toString()); + } else { + qDebug() << "PipXtremeGadgetWidget: Count not read link state field."; + } } /*! - \brief Called by updates to @OPLinkSettings - */ + \brief Called by updates to @OPLinkSettings + */ void ConfigPipXtremeWidget::updateSettings(UAVObject *object) { - Q_UNUSED(object); + Q_UNUSED(object); - if (!settingsUpdated) - { - settingsUpdated = true; - enableControls(true); - } + if (!settingsUpdated) { + settingsUpdated = true; + enableControls(true); + } } void ConfigPipXtremeWidget::disconnected() { - if (settingsUpdated) - { - settingsUpdated = false; - enableControls(false); - } + if (settingsUpdated) { + settingsUpdated = false; + enableControls(false); + } } void ConfigPipXtremeWidget::addBinding(QWidget *w) { - if(QLineEdit *le = qobject_cast(w)) { - + if (QLineEdit * le = qobject_cast(w)) { // Get the pair ID out of the selection widget quint32 pairID = 0; bool okay; - if (m_oplink->PairSelect1->isChecked()) + if (m_oplink->PairSelect1->isChecked()) { pairID = m_oplink->PairID1->text().toUInt(&okay, 16); - else if (m_oplink->PairSelect2->isChecked()) + } else if (m_oplink->PairSelect2->isChecked()) { pairID = m_oplink->PairID2->text().toUInt(&okay, 16); - else if (m_oplink->PairSelect3->isChecked()) + } else if (m_oplink->PairSelect3->isChecked()) { pairID = m_oplink->PairID3->text().toUInt(&okay, 16); - else if (m_oplink->PairSelect4->isChecked()) + } else if (m_oplink->PairSelect4->isChecked()) { pairID = m_oplink->PairID4->text().toUInt(&okay, 16); + } // Store the ID in the first open slot (or the last slot if all are full). le->setText(QString::number(pairID, 16).toUpper()); @@ -331,7 +330,7 @@ void ConfigPipXtremeWidget::addBinding(QWidget *w) void ConfigPipXtremeWidget::removeBinding(QWidget *w) { - if(QLineEdit *le = qobject_cast(w)) { + if (QLineEdit * le = qobject_cast(w)) { le->setText(QString::number(0, 16).toUpper()); } } @@ -339,4 +338,4 @@ void ConfigPipXtremeWidget::removeBinding(QWidget *w) /** @} @} -*/ + */ diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h index 9a612bf94..304ebc478 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h @@ -1,14 +1,14 @@ /** -****************************************************************************** -* -* @file configpipxtremewidget.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup ConfigPlugin Config Plugin -* @{ -* @brief The Configuration Gadget used to configure PipXtreme -*****************************************************************************/ + ****************************************************************************** + * + * @file configpipxtremewidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief The Configuration Gadget used to configure PipXtreme + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -32,38 +32,37 @@ #include "ui_pipxtreme.h" #include "configtaskwidget.h" -class ConfigPipXtremeWidget : public ConfigTaskWidget -{ - Q_OBJECT +class ConfigPipXtremeWidget : public ConfigTaskWidget { + Q_OBJECT public: - ConfigPipXtremeWidget(QWidget *parent = 0); - ~ConfigPipXtremeWidget(); - + ConfigPipXtremeWidget(QWidget *parent = 0); + ~ConfigPipXtremeWidget(); + public slots: - void updateStatus(UAVObject *object1); - void updateSettings(UAVObject *object1); + void updateStatus(UAVObject *object1); + void updateSettings(UAVObject *object1); private: - Ui_PipXtremeWidget *m_oplink; + Ui_PipXtremeWidget *m_oplink; - // The OPLink status UAVObject - UAVDataObject *oplinkStatusObj; + // The OPLink status UAVObject + UAVDataObject *oplinkStatusObj; - // The OPLink ssettins UAVObject - OPLinkSettings* oplinkSettingsObj; + // The OPLink ssettins UAVObject + OPLinkSettings *oplinkSettingsObj; - // Are the settings current? - bool settingsUpdated; + // Are the settings current? + bool settingsUpdated; - // Signal mappers to add arguments to signals. - QSignalMapper *signalMapperAddBinding; - QSignalMapper *signalMapperRemBinding; + // Signal mappers to add arguments to signals. + QSignalMapper *signalMapperAddBinding; + QSignalMapper *signalMapperRemBinding; private slots: - void disconnected(); - void addBinding(QWidget *w); - void removeBinding(QWidget *w); + void disconnected(); + void addBinding(QWidget *w); + void removeBinding(QWidget *w); }; #endif // CONFIGTXPIDWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configplugin.cpp b/ground/openpilotgcs/src/plugins/config/configplugin.cpp index fd3a30544..21f696abf 100644 --- a/ground/openpilotgcs/src/plugins/config/configplugin.cpp +++ b/ground/openpilotgcs/src/plugins/config/configplugin.cpp @@ -34,59 +34,61 @@ ConfigPlugin::ConfigPlugin() { - // Do nothing + // Do nothing } ConfigPlugin::~ConfigPlugin() { - // Do nothing + // Do nothing } -bool ConfigPlugin::initialize(const QStringList& args, QString *errMsg) +bool ConfigPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - cf = new ConfigGadgetFactory(this); - addAutoReleasedObject(cf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + cf = new ConfigGadgetFactory(this); + addAutoReleasedObject(cf); - // Add Menu entry to erase all settings - Core::ActionManager* am = Core::ICore::instance()->actionManager(); - Core::ActionContainer* ac = am->actionContainer(Core::Constants::M_TOOLS); + // Add Menu entry to erase all settings + Core::ActionManager *am = Core::ICore::instance()->actionManager(); + Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_TOOLS); - // Command to erase all settings from the board - cmd = am->registerAction(new QAction(this), - "ConfigPlugin.EraseAll", - QList() << - Core::Constants::C_GLOBAL_ID); - cmd->action()->setText(tr("Erase all settings from board...")); + // Command to erase all settings from the board + cmd = am->registerAction(new QAction(this), + "ConfigPlugin.EraseAll", + QList() << + Core::Constants::C_GLOBAL_ID); + cmd->action()->setText(tr("Erase all settings from board...")); - ac->menu()->addSeparator(); - ac->appendGroup("Utilities"); - ac->addAction(cmd, "Utilities"); + ac->menu()->addSeparator(); + ac->appendGroup("Utilities"); + ac->addAction(cmd, "Utilities"); - connect(cmd->action(), SIGNAL(triggered(bool)), this, SLOT(eraseAllSettings())); + connect(cmd->action(), SIGNAL(triggered(bool)), this, SLOT(eraseAllSettings())); - // ********************* - // Listen to autopilot connection events - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - TelemetryManager* telMngr = pm->getObject(); - connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); - connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); + // ********************* + // Listen to autopilot connection events + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + TelemetryManager *telMngr = pm->getObject(); + connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); + connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); - // And check whether by any chance we are not already connected - if (telMngr->isConnected()) - onAutopilotConnect(); + // And check whether by any chance we are not already connected + if (telMngr->isConnected()) { + onAutopilotConnect(); + } - return true; + return true; } /** - * @brief Return handle to object manager - */ -UAVObjectManager * ConfigPlugin::getObjectManager() + * @brief Return handle to object manager + */ +UAVObjectManager *ConfigPlugin::getObjectManager() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * objMngr = pm->getObject(); + UAVObjectManager *objMngr = pm->getObject(); + Q_ASSERT(objMngr); return objMngr; } @@ -94,7 +96,6 @@ UAVObjectManager * ConfigPlugin::getObjectManager() void ConfigPlugin::extensionsInitialized() { cmd->action()->setEnabled(false); - } void ConfigPlugin::shutdown() @@ -103,16 +104,16 @@ void ConfigPlugin::shutdown() } /** - * Enable the menu entry when the autopilot connects - */ + * Enable the menu entry when the autopilot connects + */ void ConfigPlugin::onAutopilotConnect() { cmd->action()->setEnabled(true); } /** - * Enable the menu entry when the autopilot connects - */ + * Enable the menu entry when the autopilot connects + */ void ConfigPlugin::onAutopilotDisconnect() { cmd->action()->setEnabled(false); @@ -120,23 +121,25 @@ void ConfigPlugin::onAutopilotDisconnect() /** - * Erase all settings from the board - */ + * Erase all settings from the board + */ void ConfigPlugin::eraseAllSettings() { QMessageBox msgBox; + msgBox.setText(tr("Are you sure you want to erase all board settings?.")); msgBox.setInformativeText(tr("All settings stored in your board flash will be deleted.")); msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); msgBox.setDefaultButton(QMessageBox::Ok); - if (msgBox.exec() != QMessageBox::Ok) - return; + if (msgBox.exec() != QMessageBox::Ok) { + return; + } settingsErased = false; - ObjectPersistence* objper = ObjectPersistence::GetInstance(getObjectManager()); + ObjectPersistence *objper = ObjectPersistence::GetInstance(getObjectManager()); Q_ASSERT(objper); - connect(objper, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(eraseDone(UAVObject *))); + connect(objper, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(eraseDone(UAVObject *))); ObjectPersistence::DataFields data = objper->getData(); data.Operation = ObjectPersistence::OPERATION_FULLERASE; @@ -145,27 +148,27 @@ void ConfigPlugin::eraseAllSettings() // based on UAVO meta data objper->setData(data); objper->updated(); - QTimer::singleShot(FLASH_ERASE_TIMEOUT_MS,this,SLOT(eraseFailed())); - + QTimer::singleShot(FLASH_ERASE_TIMEOUT_MS, this, SLOT(eraseFailed())); } void ConfigPlugin::eraseFailed() { - if (settingsErased) + if (settingsErased) { return; + } - ObjectPersistence* objper = ObjectPersistence::GetInstance(getObjectManager()); + ObjectPersistence *objper = ObjectPersistence::GetInstance(getObjectManager()); ObjectPersistence::DataFields data = objper->getData(); - if(data.Operation == ObjectPersistence::OPERATION_FULLERASE) { + if (data.Operation == ObjectPersistence::OPERATION_FULLERASE) { // First attempt via flash erase failed. Fall back on erase all settings data.Operation = ObjectPersistence::OPERATION_DELETE; data.Selection = ObjectPersistence::SELECTION_ALLSETTINGS; objper->setData(data); objper->updated(); - QTimer::singleShot(FLASH_ERASE_TIMEOUT_MS,this,SLOT(eraseFailed())); + QTimer::singleShot(FLASH_ERASE_TIMEOUT_MS, this, SLOT(eraseFailed())); } else { - disconnect(objper, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(eraseDone(UAVObject *))); + disconnect(objper, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(eraseDone(UAVObject *))); QMessageBox msgBox; msgBox.setText(tr("Error trying to erase settings.")); msgBox.setInformativeText(tr("Power-cycle your board after removing all blades. Settings might be inconsistent.")); @@ -175,18 +178,19 @@ void ConfigPlugin::eraseFailed() } } -void ConfigPlugin::eraseDone(UAVObject * obj) +void ConfigPlugin::eraseDone(UAVObject *obj) { QMessageBox msgBox; - ObjectPersistence* objper = ObjectPersistence::GetInstance(getObjectManager()); + ObjectPersistence *objper = ObjectPersistence::GetInstance(getObjectManager()); ObjectPersistence::DataFields data = objper->getData(); + Q_ASSERT(obj->getInstID() == objper->getInstID()); - if(data.Operation != ObjectPersistence::OPERATION_COMPLETED) { + if (data.Operation != ObjectPersistence::OPERATION_COMPLETED) { return; } - disconnect(objper, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(eraseDone(UAVObject *))); + disconnect(objper, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(eraseDone(UAVObject *))); if (data.Operation == ObjectPersistence::OPERATION_COMPLETED) { settingsErased = true; msgBox.setText(tr("Settings are now erased.")); diff --git a/ground/openpilotgcs/src/plugins/config/configplugin.h b/ground/openpilotgcs/src/plugins/config/configplugin.h index d0d6ec09a..254f2cdc5 100644 --- a/ground/openpilotgcs/src/plugins/config/configplugin.h +++ b/ground/openpilotgcs/src/plugins/config/configplugin.h @@ -40,17 +40,16 @@ class ConfigGadgetFactory; -class ConfigPlugin : public ExtensionSystem::IPlugin -{ +class ConfigPlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: ConfigPlugin(); ~ConfigPlugin(); - UAVObjectManager * getObjectManager(); + UAVObjectManager *getObjectManager(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); private slots: @@ -60,13 +59,12 @@ private slots: void eraseDone(UAVObject *); void eraseFailed(); - private: +private: ConfigGadgetFactory *cf; - Core::Command* cmd; + Core::Command *cmd; bool settingsErased; static const int FLASH_ERASE_TIMEOUT_MS = 45000; - }; #endif // CONFIGPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp index b4416eb93..f04b3b493 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp @@ -41,34 +41,34 @@ ConfigRevoHWWidget::ConfigRevoHWWidget(QWidget *parent) : ConfigTaskWidget(paren ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Core::Internal::GeneralSettings *settings = pm->getObject(); - if(!settings->useExpertMode()) { + if (!settings->useExpertMode()) { m_ui->saveTelemetryToRAM->setEnabled(false); m_ui->saveTelemetryToRAM->setVisible(false); } addApplySaveButtons(m_ui->saveTelemetryToRAM, m_ui->saveTelemetryToSD); - addUAVObjectToWidgetRelation("HwSettings","RM_FlexiPort",m_ui->cbFlexi); - addUAVObjectToWidgetRelation("HwSettings","RM_MainPort",m_ui->cbMain); - addUAVObjectToWidgetRelation("HwSettings","RM_RcvrPort",m_ui->cbRcvr); + addUAVObjectToWidgetRelation("HwSettings", "RM_FlexiPort", m_ui->cbFlexi); + addUAVObjectToWidgetRelation("HwSettings", "RM_MainPort", m_ui->cbMain); + addUAVObjectToWidgetRelation("HwSettings", "RM_RcvrPort", m_ui->cbRcvr); - addUAVObjectToWidgetRelation("HwSettings","USB_HIDPort",m_ui->cbUSBHIDFunction); - addUAVObjectToWidgetRelation("HwSettings","USB_VCPPort",m_ui->cbUSBVCPFunction); - addUAVObjectToWidgetRelation("HwSettings","ComUsbBridgeSpeed",m_ui->cbUSBVCPSpeed); + addUAVObjectToWidgetRelation("HwSettings", "USB_HIDPort", m_ui->cbUSBHIDFunction); + addUAVObjectToWidgetRelation("HwSettings", "USB_VCPPort", m_ui->cbUSBVCPFunction); + addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_ui->cbUSBVCPSpeed); - addUAVObjectToWidgetRelation("HwSettings","TelemetrySpeed",m_ui->cbFlexiTelemSpeed); - addUAVObjectToWidgetRelation("HwSettings","GPSSpeed",m_ui->cbFlexiGPSSpeed); - addUAVObjectToWidgetRelation("HwSettings","ComUsbBridgeSpeed",m_ui->cbFlexiComSpeed); + addUAVObjectToWidgetRelation("HwSettings", "TelemetrySpeed", m_ui->cbFlexiTelemSpeed); + addUAVObjectToWidgetRelation("HwSettings", "GPSSpeed", m_ui->cbFlexiGPSSpeed); + addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_ui->cbFlexiComSpeed); - addUAVObjectToWidgetRelation("HwSettings","TelemetrySpeed",m_ui->cbMainTelemSpeed); - addUAVObjectToWidgetRelation("HwSettings","GPSSpeed",m_ui->cbMainGPSSpeed); - addUAVObjectToWidgetRelation("HwSettings","ComUsbBridgeSpeed",m_ui->cbMainComSpeed); + addUAVObjectToWidgetRelation("HwSettings", "TelemetrySpeed", m_ui->cbMainTelemSpeed); + addUAVObjectToWidgetRelation("HwSettings", "GPSSpeed", m_ui->cbMainGPSSpeed); + addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_ui->cbMainComSpeed); addUAVObjectToWidgetRelation("HwSettings", "RadioPort", m_ui->cbModem); addUAVObjectToWidgetRelation("HwSettings", "MaxRFPower", m_ui->cbTxPower); addUAVObjectToWidgetRelation("HwSettings", "DefaultFrequency", m_ui->leInitFreq); - connect(m_ui->cchwHelp,SIGNAL(clicked()),this,SLOT(openHelp())); + connect(m_ui->cchwHelp, SIGNAL(clicked()), this, SLOT(openHelp())); setupCustomCombos(); enableControls(true); @@ -94,12 +94,12 @@ void ConfigRevoHWWidget::setupCustomCombos() connect(m_ui->cbFlexi, SIGNAL(currentIndexChanged(int)), this, SLOT(flexiPortChanged(int))); connect(m_ui->cbMain, SIGNAL(currentIndexChanged(int)), this, SLOT(mainPortChanged(int))); connect(m_ui->cbModem, SIGNAL(currentIndexChanged(int)), this, SLOT(modemPortChanged(int))); - } void ConfigRevoHWWidget::refreshWidgetsValues(UAVObject *obj) { ConfigTaskWidget::refreshWidgetsValues(obj); + usbVCPPortChanged(0); mainPortChanged(0); flexiPortChanged(0); @@ -115,20 +115,18 @@ void ConfigRevoHWWidget::updateObjectsFromWidgets() // If any port is configured to be GPS port, enable GPS module if it is not enabled. // Otherwise disable GPS module. - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_GPS || m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_GPS) { + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_GPS || m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_GPS) { data.OptionalModules[HwSettings::OPTIONALMODULES_GPS] = HwSettings::OPTIONALMODULES_ENABLED; - } - else { + } else { data.OptionalModules[HwSettings::OPTIONALMODULES_GPS] = HwSettings::OPTIONALMODULES_DISABLED; } // If any port is configured to be ComBridge port, enable UsbComBridge module if it is not enabled. // Otherwise disable UsbComBridge module. - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_COMBRIDGE || m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_COMBRIDGE || - m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_COMBRIDGE) { + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_COMBRIDGE || m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_COMBRIDGE || + m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_COMBRIDGE) { data.OptionalModules[HwSettings::OPTIONALMODULES_COMUSBBRIDGE] = HwSettings::OPTIONALMODULES_ENABLED; - } - else { + } else { data.OptionalModules[HwSettings::OPTIONALMODULES_COMUSBBRIDGE] = HwSettings::OPTIONALMODULES_DISABLED; } @@ -144,31 +142,31 @@ void ConfigRevoHWWidget::usbVCPPortChanged(int index) m_ui->lblUSBVCPSpeed->setVisible(vcpComBridgeEnabled); m_ui->cbUSBVCPSpeed->setVisible(vcpComBridgeEnabled); - if(!vcpComBridgeEnabled && m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_COMBRIDGE) { + if (!vcpComBridgeEnabled && m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_COMBRIDGE) { m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); } m_ui->cbFlexi->model()->setData(m_ui->cbFlexi->model()->index(HwSettings::RM_FLEXIPORT_COMBRIDGE, 0), - !vcpComBridgeEnabled ? QVariant(0) : QVariant(1|32), Qt::UserRole - 1); + !vcpComBridgeEnabled ? QVariant(0) : QVariant(1 | 32), Qt::UserRole - 1); - if(!vcpComBridgeEnabled && m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_COMBRIDGE) { + if (!vcpComBridgeEnabled && m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_COMBRIDGE) { m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); } m_ui->cbMain->model()->setData(m_ui->cbMain->model()->index(HwSettings::RM_MAINPORT_COMBRIDGE, 0), - !vcpComBridgeEnabled ? QVariant(0) : QVariant(1|32), Qt::UserRole - 1); + !vcpComBridgeEnabled ? QVariant(0) : QVariant(1 | 32), Qt::UserRole - 1); - //_DEBUGCONSOLE modes are mutual exclusive - if(m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) { - if(m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_DEBUGCONSOLE) { + // _DEBUGCONSOLE modes are mutual exclusive + if (m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) { + if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_DEBUGCONSOLE) { m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); } - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_DEBUGCONSOLE) { + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_DEBUGCONSOLE) { m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); } } - //_USBTELEMETRY modes are mutual exclusive - if(m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_USBTELEMETRY) { - if(m_ui->cbUSBHIDFunction->currentIndex() == HwSettings::USB_HIDPORT_USBTELEMETRY) { + // _USBTELEMETRY modes are mutual exclusive + if (m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_USBTELEMETRY) { + if (m_ui->cbUSBHIDFunction->currentIndex() == HwSettings::USB_HIDPORT_USBTELEMETRY) { m_ui->cbUSBHIDFunction->setCurrentIndex(HwSettings::USB_HIDPORT_DISABLED); } } @@ -178,9 +176,9 @@ void ConfigRevoHWWidget::usbHIDPortChanged(int index) { Q_UNUSED(index); - //_USBTELEMETRY modes are mutual exclusive - if(m_ui->cbUSBHIDFunction->currentIndex() == HwSettings::USB_HIDPORT_USBTELEMETRY) { - if(m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_USBTELEMETRY) { + // _USBTELEMETRY modes are mutual exclusive + if (m_ui->cbUSBHIDFunction->currentIndex() == HwSettings::USB_HIDPORT_USBTELEMETRY) { + if (m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_USBTELEMETRY) { m_ui->cbUSBVCPFunction->setCurrentIndex(HwSettings::USB_VCPPORT_DISABLED); } } @@ -195,42 +193,41 @@ void ConfigRevoHWWidget::flexiPortChanged(int index) m_ui->cbFlexiComSpeed->setVisible(false); m_ui->lblFlexiSpeed->setVisible(true); - switch(m_ui->cbFlexi->currentIndex()) - { - case HwSettings::RM_FLEXIPORT_TELEMETRY: - m_ui->cbFlexiTelemSpeed->setVisible(true); - if(m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_TELEMETRY) { - m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); - } - if(m_ui->cbModem->currentIndex() == HwSettings::RADIOPORT_TELEMETRY) { - m_ui->cbModem->setCurrentIndex(HwSettings::RADIOPORT_DISABLED); - } - break; - case HwSettings::RM_FLEXIPORT_GPS: - m_ui->cbFlexiGPSSpeed->setVisible(true); - if(m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_GPS) { - m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); - } - break; - case HwSettings::RM_FLEXIPORT_COMBRIDGE: - m_ui->cbFlexiComSpeed->setVisible(true); - if(m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_COMBRIDGE) { - m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); - } - break; - case HwSettings::RM_FLEXIPORT_DEBUGCONSOLE: - m_ui->cbFlexiComSpeed->setVisible(true); - if(m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_DEBUGCONSOLE) { - m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); - } - if(m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) { - m_ui->cbUSBVCPFunction->setCurrentIndex(HwSettings::USB_VCPPORT_DISABLED); - } - break; - default: - m_ui->lblFlexiSpeed->setVisible(false); - break; + switch (m_ui->cbFlexi->currentIndex()) { + case HwSettings::RM_FLEXIPORT_TELEMETRY: + m_ui->cbFlexiTelemSpeed->setVisible(true); + if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_TELEMETRY) { + m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); } + if (m_ui->cbModem->currentIndex() == HwSettings::RADIOPORT_TELEMETRY) { + m_ui->cbModem->setCurrentIndex(HwSettings::RADIOPORT_DISABLED); + } + break; + case HwSettings::RM_FLEXIPORT_GPS: + m_ui->cbFlexiGPSSpeed->setVisible(true); + if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_GPS) { + m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); + } + break; + case HwSettings::RM_FLEXIPORT_COMBRIDGE: + m_ui->cbFlexiComSpeed->setVisible(true); + if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_COMBRIDGE) { + m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); + } + break; + case HwSettings::RM_FLEXIPORT_DEBUGCONSOLE: + m_ui->cbFlexiComSpeed->setVisible(true); + if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_DEBUGCONSOLE) { + m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); + } + if (m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) { + m_ui->cbUSBVCPFunction->setCurrentIndex(HwSettings::USB_VCPPORT_DISABLED); + } + break; + default: + m_ui->lblFlexiSpeed->setVisible(false); + break; + } } void ConfigRevoHWWidget::mainPortChanged(int index) @@ -242,41 +239,40 @@ void ConfigRevoHWWidget::mainPortChanged(int index) m_ui->cbMainComSpeed->setVisible(false); m_ui->lblMainSpeed->setVisible(true); - switch(m_ui->cbMain->currentIndex()) - { - case HwSettings::RM_MAINPORT_TELEMETRY: - m_ui->cbMainTelemSpeed->setVisible(true); - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_TELEMETRY) { - m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); - } - if(m_ui->cbModem->currentIndex() == HwSettings::RADIOPORT_TELEMETRY) { - m_ui->cbModem->setCurrentIndex(HwSettings::RADIOPORT_DISABLED); - } - break; - case HwSettings::RM_MAINPORT_GPS: - m_ui->cbMainGPSSpeed->setVisible(true); - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_GPS) { - m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); - } - break; - case HwSettings::RM_MAINPORT_COMBRIDGE: - m_ui->cbMainComSpeed->setVisible(true); - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_COMBRIDGE) { - m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); - } - break; - case HwSettings::RM_MAINPORT_DEBUGCONSOLE: - m_ui->cbMainComSpeed->setVisible(true); - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_DEBUGCONSOLE) { - m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); - } - if(m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) { - m_ui->cbUSBVCPFunction->setCurrentIndex(HwSettings::USB_VCPPORT_DISABLED); - } - break; - default: - m_ui->lblMainSpeed->setVisible(false); - break; + switch (m_ui->cbMain->currentIndex()) { + case HwSettings::RM_MAINPORT_TELEMETRY: + m_ui->cbMainTelemSpeed->setVisible(true); + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_TELEMETRY) { + m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); + } + if (m_ui->cbModem->currentIndex() == HwSettings::RADIOPORT_TELEMETRY) { + m_ui->cbModem->setCurrentIndex(HwSettings::RADIOPORT_DISABLED); + } + break; + case HwSettings::RM_MAINPORT_GPS: + m_ui->cbMainGPSSpeed->setVisible(true); + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_GPS) { + m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); + } + break; + case HwSettings::RM_MAINPORT_COMBRIDGE: + m_ui->cbMainComSpeed->setVisible(true); + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_COMBRIDGE) { + m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); + } + break; + case HwSettings::RM_MAINPORT_DEBUGCONSOLE: + m_ui->cbMainComSpeed->setVisible(true); + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_DEBUGCONSOLE) { + m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); + } + if (m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) { + m_ui->cbUSBVCPFunction->setCurrentIndex(HwSettings::USB_VCPPORT_DISABLED); + } + break; + default: + m_ui->lblMainSpeed->setVisible(false); + break; } } @@ -284,11 +280,11 @@ void ConfigRevoHWWidget::modemPortChanged(int index) { Q_UNUSED(index); - if(m_ui->cbModem->currentIndex()== HwSettings::RADIOPORT_TELEMETRY) { - if(m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_TELEMETRY) { + if (m_ui->cbModem->currentIndex() == HwSettings::RADIOPORT_TELEMETRY) { + if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_TELEMETRY) { m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); } - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_TELEMETRY) { + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_TELEMETRY) { m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); } m_ui->lblTxPower->setVisible(true); @@ -305,5 +301,5 @@ void ConfigRevoHWWidget::modemPortChanged(int index) void ConfigRevoHWWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/GgDBAQ", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/GgDBAQ", QUrl::StrictMode)); } diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.h b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.h index 7e6ed51f4..bfabea5b4 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.h @@ -36,8 +36,7 @@ #include -class ConfigRevoHWWidget: public ConfigTaskWidget -{ +class ConfigRevoHWWidget : public ConfigTaskWidget { Q_OBJECT public: @@ -49,7 +48,7 @@ private: void setupCustomCombos(); protected slots: - void refreshWidgetsValues(UAVObject * obj = NULL); + void refreshWidgetsValues(UAVObject *obj = NULL); void updateObjectsFromWidgets(); private slots: @@ -59,7 +58,6 @@ private slots: void mainPortChanged(int index); void modemPortChanged(int index); void openHelp(); - }; #endif // CONFIGREVOHWWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index b28693c09..5859a299f 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -837,35 +837,35 @@ void ConfigRevoWidget::computeScaleBias() void ConfigRevoWidget::storeAndClearBoardRotation() { - if(!isBoardRotationStored) { + if (!isBoardRotationStored) { // Store current board rotation isBoardRotationStored = true; AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); Q_ASSERT(attitudeSettings); - AttitudeSettings::DataFields data = attitudeSettings->getData(); - storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW] = data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]; - storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]; + AttitudeSettings::DataFields data = attitudeSettings->getData(); + storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW] = data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]; + storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]; storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH]; // Set board rotation to no rotation - data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = 0; - data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = 0; - data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = 0; + data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = 0; + data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = 0; + data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = 0; attitudeSettings->setData(data); } } void ConfigRevoWidget::recallBoardRotation() { - if(isBoardRotationStored) { + if (isBoardRotationStored) { // Recall current board rotation isBoardRotationStored = false; AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); Q_ASSERT(attitudeSettings); - AttitudeSettings::DataFields data = attitudeSettings->getData(); - data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW]; - data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL]; + AttitudeSettings::DataFields data = attitudeSettings->getData(); + data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW]; + data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL]; data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH]; attitudeSettings->setData(data); } @@ -1016,7 +1016,6 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject *obj) if (mag_accum_x.length() >= NOISE_SAMPLES && gyro_accum_x.length() >= NOISE_SAMPLES && accel_accum_x.length() >= NOISE_SAMPLES) { - // No need to for more updates Magnetometer *mags = Magnetometer::GetInstance(getObjectManager()); Accels *accels = Accels::GetInstance(getObjectManager()); diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h index d728a877b..7f6324779 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h @@ -37,8 +37,7 @@ #include -class ConfigStabilizationWidget: public ConfigTaskWidget -{ +class ConfigStabilizationWidget : public ConfigTaskWidget { Q_OBJECT public: @@ -47,7 +46,7 @@ public: private: Ui_StabilizationWidget *ui; - QTimer * realtimeUpdates; + QTimer *realtimeUpdates; // Milliseconds between automatic 'Instant Updates' static const int AUTOMATIC_UPDATE_RATE = 500; @@ -58,7 +57,7 @@ protected slots: private slots: void realtimeUpdatesSlot(bool value); void linkCheckBoxes(bool value); - void processLinkedWidgets(QWidget*); + void processLinkedWidgets(QWidget *); }; #endif // ConfigStabilizationWidget_H diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp index b279135bb..02902f5c8 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp @@ -35,11 +35,12 @@ ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent) { m_txpid = new Ui_TxPIDWidget(); m_txpid->setupUi(this); - - ExtensionSystem::PluginManager *pm=ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); - if(!settings->useExpertMode()) + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { m_txpid->Apply->setVisible(false); + } autoLoadWidgets(); addApplySaveButtons(m_txpid->Apply, m_txpid->Save); @@ -82,13 +83,14 @@ ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent) ConfigTxPIDWidget::~ConfigTxPIDWidget() { - // Do nothing + // Do nothing } void ConfigTxPIDWidget::refreshValues() { HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); HwSettings::DataFields hwSettingsData = hwSettings->getData(); + m_txpid->TxPIDEnable->setChecked( hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_TXPID] == HwSettings::OPTIONALMODULES_ENABLED); } @@ -97,6 +99,7 @@ void ConfigTxPIDWidget::applySettings() { HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); HwSettings::DataFields hwSettingsData = hwSettings->getData(); + hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_TXPID] = m_txpid->TxPIDEnable->isChecked() ? HwSettings::OPTIONALMODULES_ENABLED : HwSettings::OPTIONALMODULES_DISABLED; hwSettings->setData(hwSettingsData); diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h index 14eb5925d..38d2aa318 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h @@ -30,8 +30,7 @@ #include "ui_txpid.h" #include "configtaskwidget.h" -class ConfigTxPIDWidget : public ConfigTaskWidget -{ +class ConfigTxPIDWidget : public ConfigTaskWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp index af316585a..2e9a5f0fe 100644 --- a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp @@ -45,14 +45,15 @@ #include /** - Static function to get currently assigned channelDescriptions - for all known vehicle types; instantiates the appropriate object - then asks it to supply channel descs - */ + Static function to get currently assigned channelDescriptions + for all known vehicle types; instantiates the appropriate object + then asks it to supply channel descs + */ QStringList ConfigVehicleTypeWidget::getChannelDescriptions() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objMngr = pm->getObject(); + Q_ASSERT(objMngr); // get an instance of systemsettings @@ -104,9 +105,9 @@ ConfigVehicleTypeWidget::ConfigVehicleTypeWidget(QWidget *parent) : ConfigTaskWi { m_aircraft = new Ui_AircraftWidget(); m_aircraft->setupUi(this); - - ExtensionSystem::PluginManager *pm=ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); if (!settings->useExpertMode()) { m_aircraft->saveAircraftToRAM->setVisible(false); } @@ -125,11 +126,11 @@ ConfigVehicleTypeWidget::ConfigVehicleTypeWidget(QWidget *parent) : ConfigTaskWi m_aircraft->aircraftType->addItems(airframeTypes); // Set default vehicle to MultiRotor - //m_aircraft->aircraftType->setCurrentIndex(3); + // m_aircraft->aircraftType->setCurrentIndex(3); - // Connect aircraft type selection dropbox to callback function + // Connect aircraft type selection dropbox to callback function connect(m_aircraft->aircraftType, SIGNAL(currentIndexChanged(int)), this, SLOT(switchAirframeType(int))); - + // Connect the three feed forward test checkboxes connect(m_aircraft->ffTestBox1, SIGNAL(clicked(bool)), this, SLOT(enableFFTest())); connect(m_aircraft->ffTestBox2, SIGNAL(clicked(bool)), this, SLOT(enableFFTest())); @@ -157,25 +158,26 @@ ConfigVehicleTypeWidget::ConfigVehicleTypeWidget(QWidget *parent) : ConfigTaskWi } /** - Destructor + Destructor */ ConfigVehicleTypeWidget::~ConfigVehicleTypeWidget() { - // Do nothing + // Do nothing } void ConfigVehicleTypeWidget::switchAirframeType(int index) { // TODO not safe w/r to translation!!! QString frameCategory = m_aircraft->aircraftType->currentText(); + m_aircraft->airframesWidget->setCurrentWidget(getVehicleConfigWidget(frameCategory)); } /** - Refreshes the current value of the SystemSettings which holds the aircraft type - Note: The default behavior of ConfigTaskWidget is bypassed. - Therefore no automatic synchronization of UAV Objects to UI is done. - */ + Refreshes the current value of the SystemSettings which holds the aircraft type + Note: The default behavior of ConfigTaskWidget is bypassed. + Therefore no automatic synchronization of UAV Objects to UI is done. + */ void ConfigVehicleTypeWidget::refreshWidgetsValues(UAVObject *o) { Q_UNUSED(o); @@ -183,11 +185,11 @@ void ConfigVehicleTypeWidget::refreshWidgetsValues(UAVObject *o) if (!allObjectsUpdated()) { return; } - + bool dirty = isDirty(); - + // Get the Airframe type from the system settings: - UAVDataObject *system = dynamic_cast(getObjectManager()->getObject(QString("SystemSettings"))); + UAVDataObject *system = dynamic_cast(getObjectManager()->getObject(QString("SystemSettings"))); Q_ASSERT(system); UAVObjectField *field = system->getField(QString("AirframeType")); @@ -198,14 +200,14 @@ void ConfigVehicleTypeWidget::refreshWidgetsValues(UAVObject *o) QString frameType = field->getValue().toString(); qDebug() << "ConfigVehicleTypeWidget::refreshWidgetsValues - frame type:" << frameType; - QString category = frameCategory(frameType); + QString category = frameCategory(frameType); setComboCurrentIndex(m_aircraft->aircraftType, m_aircraft->aircraftType->findText(category)); VehicleConfig *vehicleConfig = getVehicleConfigWidget(category); if (vehicleConfig) { vehicleConfig->refreshWidgetsValues(frameType); } - + updateFeedForwardUI(); setDirty(dirty); @@ -214,21 +216,22 @@ void ConfigVehicleTypeWidget::refreshWidgetsValues(UAVObject *o) } /** - Sends the config to the board (airframe type) + Sends the config to the board (airframe type) - We do all the tasks common to all airframes, or family of airframes, and - we call additional methods for specific frames, so that we do not have a code - that is too heavy. + We do all the tasks common to all airframes, or family of airframes, and + we call additional methods for specific frames, so that we do not have a code + that is too heavy. - Note: The default behavior of ConfigTaskWidget is bypassed. - Therefore no automatic synchronization of UI to UAV Objects is done. -*/ + Note: The default behavior of ConfigTaskWidget is bypassed. + Therefore no automatic synchronization of UI to UAV Objects is done. + */ void ConfigVehicleTypeWidget::updateObjectsFromWidgets() { // Airframe type defaults to Custom QString airframeType = "Custom"; - VehicleConfig *vehicleConfig = (VehicleConfig *) m_aircraft->airframesWidget->currentWidget(); + VehicleConfig *vehicleConfig = (VehicleConfig *)m_aircraft->airframesWidget->currentWidget(); + if (vehicleConfig) { airframeType = vehicleConfig->updateConfigObjectsFromWidgets(); } @@ -260,21 +263,22 @@ void ConfigVehicleTypeWidget::updateObjectsFromWidgets() QString ConfigVehicleTypeWidget::frameCategory(QString frameType) { QString category; + if (frameType == "FixedWing" || frameType == "Elevator aileron rudder" || frameType == "FixedWingElevon" - || frameType == "Elevon" || frameType == "FixedWingVtail" || frameType == "Vtail") { + || frameType == "Elevon" || frameType == "FixedWingVtail" || frameType == "Vtail") { category = "Fixed Wing"; } else if (frameType == "Tri" || frameType == "Tricopter Y" || frameType == "QuadX" || frameType == "Quad X" - || frameType == "QuadP" || frameType == "Quad +" || frameType == "Hexa" || frameType == "Hexacopter" - || frameType == "HexaX" || frameType == "Hexacopter X" || frameType == "HexaCoax" - || frameType == "Hexacopter Y6" || frameType == "Octo" || frameType == "Octocopter" || frameType == "OctoV" - || frameType == "Octocopter V" || frameType == "OctoCoaxP" || frameType == "Octo Coax +" - || frameType == "OctoCoaxX" || frameType == "Octo Coax X") { + || frameType == "QuadP" || frameType == "Quad +" || frameType == "Hexa" || frameType == "Hexacopter" + || frameType == "HexaX" || frameType == "Hexacopter X" || frameType == "HexaCoax" + || frameType == "Hexacopter Y6" || frameType == "Octo" || frameType == "Octocopter" || frameType == "OctoV" + || frameType == "Octocopter V" || frameType == "OctoCoaxP" || frameType == "Octo Coax +" + || frameType == "OctoCoaxX" || frameType == "Octo Coax X") { category = "Multirotor"; } else if (frameType == "HeliCP") { category = "Helicopter"; } else if (frameType == "GroundVehicleCar" || frameType == "Turnable (car)" - || frameType == "GroundVehicleDifferential" || frameType == "Differential (tank)" - || frameType == "GroundVehicleMotorcyle" || frameType == "Motorcycle") { + || frameType == "GroundVehicleDifferential" || frameType == "Differential (tank)" + || frameType == "GroundVehicleMotorcyle" || frameType == "Motorcycle") { category = "Ground"; } else { category = "Custom"; @@ -285,6 +289,7 @@ QString ConfigVehicleTypeWidget::frameCategory(QString frameType) VehicleConfig *ConfigVehicleTypeWidget::getVehicleConfigWidget(QString frameCategory) { VehicleConfig *vehiculeConfig; + if (!vehicleIndexMap.contains(frameCategory)) { // create config widget vehiculeConfig = createVehicleConfigWidget(frameCategory); @@ -296,7 +301,7 @@ VehicleConfig *ConfigVehicleTypeWidget::getVehicleConfigWidget(QString frameCate vehicleIndexMap[frameCategory] = index; } int index = vehicleIndexMap.value(frameCategory); - vehiculeConfig = (VehicleConfig *) m_aircraft->airframesWidget->widget(index); + vehiculeConfig = (VehicleConfig *)m_aircraft->airframesWidget->widget(index); return vehiculeConfig; } @@ -318,8 +323,8 @@ VehicleConfig *ConfigVehicleTypeWidget::createVehicleConfigWidget(QString frameC } /** - Enables and runs feed forward testing - */ + Enables and runs feed forward testing + */ void ConfigVehicleTypeWidget::enableFFTest() { // Role: @@ -327,11 +332,11 @@ void ConfigVehicleTypeWidget::enableFFTest() // - Every other timer event: toggle engine from 45% to 55% // - Every other time event: send FF settings to flight FW if (m_aircraft->ffTestBox1->isChecked() && m_aircraft->ffTestBox2->isChecked() - && m_aircraft->ffTestBox3->isChecked()) { + && m_aircraft->ffTestBox3->isChecked()) { if (!ffTuningInProgress) { // Initiate tuning: - UAVDataObject *obj = dynamic_cast(getObjectManager()->getObject( - QString("ManualControlCommand"))); + UAVDataObject *obj = dynamic_cast(getObjectManager()->getObject( + QString("ManualControlCommand"))); UAVObject::Metadata mdata = obj->getMetadata(); accInitialData = mdata; UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY); @@ -340,7 +345,7 @@ void ConfigVehicleTypeWidget::enableFFTest() // Depending on phase, either move actuator or send FF settings: if (ffTuningPhase) { // Send FF settings to the board - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); QPointer vconfig = new VehicleConfig(); @@ -353,9 +358,9 @@ void ConfigVehicleTypeWidget::enableFFTest() mixer->updated(); } else { // Toggle motor state - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject( - QString("ManualControlCommand"))); - double value = obj->getField("Throttle")->getDouble(); + UAVDataObject *obj = dynamic_cast(getObjectManager()->getObject( + QString("ManualControlCommand"))); + double value = obj->getField("Throttle")->getDouble(); double target = (value < 0.5) ? 0.55 : 0.45; obj->getField("Throttle")->setValue(target); obj->updated(); @@ -368,8 +373,8 @@ void ConfigVehicleTypeWidget::enableFFTest() // Disarm! if (ffTuningInProgress) { ffTuningInProgress = false; - UAVDataObject *obj = dynamic_cast(getObjectManager()->getObject( - QString("ManualControlCommand"))); + UAVDataObject *obj = dynamic_cast(getObjectManager()->getObject( + QString("ManualControlCommand"))); UAVObject::Metadata mdata = obj->getMetadata(); mdata = accInitialData; // Restore metadata obj->setMetadata(mdata); @@ -378,13 +383,14 @@ void ConfigVehicleTypeWidget::enableFFTest() } /** - Updates the custom airframe settings based on the current airframe. + Updates the custom airframe settings based on the current airframe. - Note: does NOT ask for an object refresh itself! - */ + Note: does NOT ask for an object refresh itself! + */ void ConfigVehicleTypeWidget::updateFeedForwardUI() { - UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + Q_ASSERT(mixer); QPointer vconfig = new VehicleConfig(); @@ -397,19 +403,19 @@ void ConfigVehicleTypeWidget::updateFeedForwardUI() } /** - Opens the wiki from the user's default browser + Opens the wiki from the user's default browser */ void ConfigVehicleTypeWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/44Cf", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/44Cf", QUrl::StrictMode)); } /** - Helper function: - Sets the current index on supplied combobox to index - if it is within bounds 0 <= index < combobox.count() + Helper function: + Sets the current index on supplied combobox to index + if it is within bounds 0 <= index < combobox.count() */ -void ConfigVehicleTypeWidget::setComboCurrentIndex(QComboBox* box, int index) +void ConfigVehicleTypeWidget::setComboCurrentIndex(QComboBox *box, int index) { if (index >= 0 && index < box->count()) { box->setCurrentIndex(index); diff --git a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.h b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.h index 6184fd6fb..5a065865c 100644 --- a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.h @@ -53,8 +53,7 @@ * TODO consider to call "super" to benefit from default logic... * TODO improve handling of relationship with VehicleConfig derived classes (i.e. ConfigTaskWidget within ConfigTaskWidget) */ -class ConfigVehicleTypeWidget: public ConfigTaskWidget -{ +class ConfigVehicleTypeWidget : public ConfigTaskWidget { Q_OBJECT public: @@ -90,7 +89,6 @@ private slots: void switchAirframeType(int index); void openHelp(); void enableFFTest(); - }; #endif // CONFIGVEHICLETYPEWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/dblspindelegate.cpp b/ground/openpilotgcs/src/plugins/config/dblspindelegate.cpp index 21af3f1d2..8c7eda9b1 100644 --- a/ground/openpilotgcs/src/plugins/config/dblspindelegate.cpp +++ b/ground/openpilotgcs/src/plugins/config/dblspindelegate.cpp @@ -28,45 +28,48 @@ #include "dblspindelegate.h" /** - Helper delegate for the custom mixer editor table. - */ + Helper delegate for the custom mixer editor table. + */ DoubleSpinDelegate::DoubleSpinDelegate(QObject *parent) - : QItemDelegate(parent) - { - m_min = 0.0; - m_max = 1.0; + : QItemDelegate(parent) +{ + m_min = 0.0; + m_max = 1.0; m_decimals = 2; - m_step = 0.01; - } + m_step = 0.01; +} QWidget *DoubleSpinDelegate::createEditor(QWidget *parent, - const QStyleOptionViewItem &/* option */, - const QModelIndex &/* index */) const + const QStyleOptionViewItem & /* option */, + const QModelIndex & /* index */) const { QDoubleSpinBox *editor = new QDoubleSpinBox(parent); + editor->setMinimum(m_min); editor->setMaximum(m_max); editor->setDecimals(m_decimals); editor->setSingleStep(m_step); - connect(editor,SIGNAL(valueChanged(double)), this, SLOT(valueChanged())); + connect(editor, SIGNAL(valueChanged(double)), this, SLOT(valueChanged())); return editor; } void DoubleSpinDelegate::setEditorData(QWidget *editor, - const QModelIndex &index) const + const QModelIndex &index) const { double value = index.model()->data(index, Qt::EditRole).toDouble(); - QDoubleSpinBox *spinBox = static_cast(editor); + QDoubleSpinBox *spinBox = static_cast(editor); + spinBox->setValue(value); } void DoubleSpinDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, - const QModelIndex &index) const + const QModelIndex &index) const { - QDoubleSpinBox *spinBox = static_cast(editor); + QDoubleSpinBox *spinBox = static_cast(editor); + spinBox->interpretText(); double value = spinBox->value(); @@ -74,7 +77,7 @@ void DoubleSpinDelegate::setModelData(QWidget *editor, QAbstractItemModel *model } void DoubleSpinDelegate::updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &/* index */) const + const QStyleOptionViewItem &option, const QModelIndex & /* index */) const { editor->setGeometry(option.rect); } @@ -83,4 +86,3 @@ void DoubleSpinDelegate::valueChanged() { emit ValueChanged(); } - diff --git a/ground/openpilotgcs/src/plugins/config/dblspindelegate.h b/ground/openpilotgcs/src/plugins/config/dblspindelegate.h index 482a07649..0781a7847 100644 --- a/ground/openpilotgcs/src/plugins/config/dblspindelegate.h +++ b/ground/openpilotgcs/src/plugins/config/dblspindelegate.h @@ -34,8 +34,7 @@ namespace Ui { class DoubleSpinDelegate; } -class DoubleSpinDelegate : public QItemDelegate -{ +class DoubleSpinDelegate : public QItemDelegate { Q_OBJECT public: @@ -49,19 +48,34 @@ public: const QModelIndex &index) const; void updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &index) const; + const QStyleOptionViewItem &option, const QModelIndex &index) const; - void setMin(double min) { m_min = min; } - void setMax(double max) { m_max = max; } - void setRange(double min, double max) { m_min = min; m_max = max; } - void setStep(double step) { m_step = step; } - void setDecimals(int decimals) { m_decimals = decimals; } + void setMin(double min) + { + m_min = min; + } + void setMax(double max) + { + m_max = max; + } + void setRange(double min, double max) + { + m_min = min; m_max = max; + } + void setStep(double step) + { + m_step = step; + } + void setDecimals(int decimals) + { + m_decimals = decimals; + } private: - double m_min; - double m_max; - double m_step; - int m_decimals; + double m_min; + double m_max; + double m_step; + int m_decimals; signals: void ValueChanged(); diff --git a/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.cpp index 4705bb02e..e221aebe2 100644 --- a/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.cpp @@ -31,8 +31,8 @@ #include DefaultAttitudeWidget::DefaultAttitudeWidget(QWidget *parent) : - QWidget(parent), - ui(new Ui_defaultattitude) + QWidget(parent), + ui(new Ui_defaultattitude) { ui->setupUi(this); } @@ -41,4 +41,3 @@ DefaultAttitudeWidget::~DefaultAttitudeWidget() { delete ui; } - diff --git a/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.h b/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.h index 8eb5cfa34..a21541d83 100644 --- a/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.h +++ b/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.h @@ -38,8 +38,7 @@ class Ui_Widget; -class DefaultAttitudeWidget : public QWidget -{ +class DefaultAttitudeWidget : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.cpp b/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.cpp index 69eef9117..21b9274e4 100644 --- a/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.cpp @@ -31,8 +31,8 @@ #include DefaultHwSettingsWidget::DefaultHwSettingsWidget(QWidget *parent) : - QWidget(parent), - ui(new Ui_defaulthwsettings) + QWidget(parent), + ui(new Ui_defaulthwsettings) { ui->setupUi(this); } @@ -41,4 +41,3 @@ DefaultHwSettingsWidget::~DefaultHwSettingsWidget() { delete ui; } - diff --git a/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.h b/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.h index 1d498c35a..604a7f019 100644 --- a/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.h +++ b/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.h @@ -38,8 +38,7 @@ class Ui_Widget; -class DefaultHwSettingsWidget : public QWidget -{ +class DefaultHwSettingsWidget : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/fancytabwidget.cpp b/ground/openpilotgcs/src/plugins/config/fancytabwidget.cpp index bb0b65b09..6a5629ec3 100644 --- a/ground/openpilotgcs/src/plugins/config/fancytabwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/fancytabwidget.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,15 +44,15 @@ #include #include -const int FancyTabBar::m_rounding = 22; +const int FancyTabBar::m_rounding = 22; const int FancyTabBar::m_textPadding = 4; FancyTabBar::FancyTabBar(QWidget *parent, bool isVertical) : QWidget(parent) { - verticalTabs = isVertical; + verticalTabs = isVertical; setIconSize(16); - m_hoverIndex = -1; + m_hoverIndex = -1; m_currentIndex = 0; if (isVertical) { setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); @@ -78,11 +78,12 @@ FancyTabBar::~FancyTabBar() QSize FancyTabBar::tabSizeHint(bool minimum) const { QFont boldFont(font()); + boldFont.setPointSizeF(Utils::StyleHelper::sidebarFontSize()); boldFont.setBold(true); QFontMetrics fm(boldFont); - int spacing = 6; - int width = 90 + spacing + 2; + int spacing = 6; + int width = 90 + spacing + 2; int iconHeight = minimum ? 0 : iconSize; return QSize(width, iconHeight + spacing + fm.height()); @@ -93,9 +94,11 @@ void FancyTabBar::paintEvent(QPaintEvent *event) Q_UNUSED(event) QPainter p(this); - for (int i = 0; i < count(); ++i) - if (i != currentIndex()) + for (int i = 0; i < count(); ++i) { + if (i != currentIndex()) { paintTab(&p, i); + } + } // paint active tab last, since it overlaps the neighbors paintTab(&p, currentIndex()); @@ -117,9 +120,9 @@ void FancyTabBar::mouseMoveEvent(QMouseEvent *e) m_hoverControl.stop(); m_hoverIndex = newHover; update(m_hoverRect); - m_hoverRect = QRect(); + m_hoverRect = QRect(); - if (m_hoverIndex >=0) { + if (m_hoverIndex >= 0) { QRect oldHoverRect = m_hoverRect; m_hoverRect = tabRect(m_hoverIndex); m_hoverControl.start(); @@ -133,7 +136,7 @@ bool FancyTabBar::event(QEvent *event) if (m_hoverIndex >= 0 && m_hoverIndex < m_tabs.count()) { QString tt = tabToolTip(m_hoverIndex); if (!tt.isEmpty()) { - QToolTip::showText(static_cast(event)->globalPos(), tt, this); + QToolTip::showText(static_cast(event)->globalPos(), tt, this); return true; } } @@ -150,7 +153,7 @@ void FancyTabBar::updateHover() void FancyTabBar::enterEvent(QEvent *e) { Q_UNUSED(e) - m_hoverRect = QRect(); + m_hoverRect = QRect(); m_hoverIndex = -1; } @@ -161,7 +164,7 @@ void FancyTabBar::leaveEvent(QEvent *e) if (m_hoverIndex >= 0) { m_hoverIndex = -1; update(m_hoverRect); - m_hoverRect = QRect(); + m_hoverRect = QRect(); } } @@ -174,16 +177,20 @@ void FancyTabBar::updateTabNameIcon(int index, const QIcon &icon, const QString QSize FancyTabBar::sizeHint() const { QSize sh = tabSizeHint(); - if (verticalTabs) + + if (verticalTabs) { return QSize(sh.width(), sh.height() * m_tabs.count()); + } return QSize(sh.width() * m_tabs.count(), sh.height()); } QSize FancyTabBar::minimumSizeHint() const { QSize sh = tabSizeHint(true); - if (verticalTabs) + + if (verticalTabs) { return QSize(sh.width(), sh.height() * m_tabs.count()); + } return QSize(sh.width() * m_tabs.count(), sh.height()); } @@ -192,18 +199,18 @@ QRect FancyTabBar::tabRect(int index) const QSize sh = tabSizeHint(); if (verticalTabs) { - if (sh.height() * m_tabs.count() > height()) + if (sh.height() * m_tabs.count() > height()) { sh.setHeight(height() / m_tabs.count()); + } return QRect(0, index * sh.height(), sh.width(), sh.height()); - } - if(sh.width() * m_tabs.count() > width()) + if (sh.width() * m_tabs.count() > width()) { sh.setWidth(width() / m_tabs.count()); + } return QRect(index * sh.width(), 0, sh.width(), sh.height()); - } void FancyTabBar::mousePressEvent(QMouseEvent *e) @@ -221,10 +228,10 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const { painter->save(); - QRect rect = tabRect(tabIndex); + QRect rect = tabRect(tabIndex); bool selected = (tabIndex == m_currentIndex); - bool hover = (tabIndex == m_hoverIndex); + bool hover = (tabIndex == m_hoverIndex); #ifdef Q_WS_MAC hover = false; // Dont hover on Mac @@ -238,13 +245,13 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const } QColor light = QColor(255, 255, 255, 40); - QColor dark = QColor(0, 0, 0, 60); + QColor dark = QColor(0, 0, 0, 60); if (selected) { QLinearGradient selectedGradient(rect.bottomRight(), QPoint(rect.center().x(), rect.top())); selectedGradient.setColorAt(0, Qt::white); selectedGradient.setColorAt(0.3, Qt::white); - selectedGradient.setColorAt(0.7, QColor(210, 210, 220)); //give a blue-ish color + selectedGradient.setColorAt(0.7, QColor(210, 210, 220)); // give a blue-ish color painter->fillRect(rect, selectedGradient); painter->setPen(QColor(200, 200, 200)); @@ -253,8 +260,9 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const painter->drawLine(rect.topRight(), rect.bottomRight()); } else { painter->fillRect(rect, background); - if (hover) + if (hover) { painter->fillRect(rect, hoverColor); + } painter->setPen(QPen(light, 0)); painter->drawLine(rect.topLeft(), rect.bottomLeft()); painter->setPen(QPen(dark, 0)); @@ -269,25 +277,29 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const boldFont.setBold(true); painter->setFont(boldFont); painter->setPen(selected ? Utils::StyleHelper::panelTextColor() : QColor(30, 30, 30, 80)); - int textFlags = Qt::AlignCenter | Qt::AlignBottom | Qt::ElideRight | Qt::TextWordWrap; + int textFlags = Qt::AlignCenter | Qt::AlignBottom | Qt::ElideRight | Qt::TextWordWrap; painter->drawText(tabTextRect, textFlags, tabText); painter->setPen(selected ? QColor(60, 60, 60) : Utils::StyleHelper::panelTextColor()); int textHeight = painter->fontMetrics().boundingRect(QRect(0, 0, width(), height()), Qt::TextWordWrap, tabText).height(); tabIconRect.adjust(0, 4, 0, -textHeight); - int iconSize = qMin(tabIconRect.width(), tabIconRect.height()); - if (iconSize > 4) + int iconSize = qMin(tabIconRect.width(), tabIconRect.height()); + if (iconSize > 4) { style()->drawItemPixmap(painter, tabIconRect, Qt::AlignCenter | Qt::AlignVCenter, tabIcon(tabIndex).pixmap(tabIconRect.size())); + } painter->translate(0, -1); painter->drawText(tabTextRect, textFlags, tabText); painter->restore(); } -void FancyTabBar::setCurrentIndex(int index) { - bool proceed=true; +void FancyTabBar::setCurrentIndex(int index) +{ + bool proceed = true; emit aboutToChange(&proceed); - if(!proceed) + + if (!proceed) { return; + } m_currentIndex = index; update(); emit currentChanged(index); @@ -297,19 +309,19 @@ void FancyTabBar::setCurrentIndex(int index) { // FancyColorButton ////// -class FancyColorButton : public QWidget -{ +class FancyColorButton : public QWidget { public: FancyColorButton(QWidget *parent) - : m_parent(parent) + : m_parent(parent) { setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Preferred); } void mousePressEvent(QMouseEvent *ev) { - if (ev->modifiers() & Qt::ShiftModifier) + if (ev->modifiers() & Qt::ShiftModifier) { Utils::StyleHelper::setBaseColor(QColorDialog::getColor(Utils::StyleHelper::baseColor(), m_parent)); + } } private: QWidget *m_parent; @@ -375,22 +387,24 @@ FancyTabWidget::FancyTabWidget(QWidget *parent, bool isVertical) selectionLayout->addWidget(m_cornerWidgetContainer, 0); m_modesStack = new QStackedLayout; - m_statusBar = new QStatusBar; + m_statusBar = new QStatusBar; m_statusBar->setSizePolicy(QSizePolicy::Ignored, QSizePolicy::Fixed); QVBoxLayout *vlayout = new QVBoxLayout; vlayout->setMargin(0); vlayout->setSpacing(0); vlayout->addLayout(m_modesStack); - if (!isVertical) + if (!isVertical) { vlayout->addWidget(m_selectionWidget); -// vlayout->addWidget(m_statusBar); //status bar is not used for now + } +// vlayout->addWidget(m_statusBar); //status bar is not used for now QHBoxLayout *mainLayout = new QHBoxLayout; mainLayout->setMargin(0); mainLayout->setSpacing(1); - if (isVertical) + if (isVertical) { mainLayout->addWidget(m_selectionWidget); + } mainLayout->addLayout(vlayout); setLayout(mainLayout); @@ -419,6 +433,7 @@ void FancyTabWidget::updateTabNameIcon(int index, const QIcon &icon, const QStri void FancyTabWidget::setBackgroundBrush(const QBrush &brush) { QPalette pal = m_tabBar->palette(); + pal.setBrush(QPalette::Mid, brush); m_tabBar->setPalette(pal); pal = m_cornerWidgetContainer->palette(); @@ -441,6 +456,7 @@ void FancyTabWidget::paintEvent(QPaintEvent *event) void FancyTabWidget::insertCornerWidget(int pos, QWidget *widget) { QHBoxLayout *layout = static_cast(m_cornerWidgetContainer->layout()); + layout->insertWidget(pos, widget); } @@ -472,6 +488,7 @@ void FancyTabWidget::setCurrentIndex(int index) void FancyTabWidget::showWidget(int index) { emit currentAboutToShow(index); + m_modesStack->setCurrentIndex(index); emit currentChanged(index); } @@ -480,7 +497,7 @@ void FancyTabWidget::setTabToolTip(int index, const QString &toolTip) { m_tabBar->setTabToolTip(index, toolTip); } -QWidget * FancyTabWidget::currentWidget() +QWidget *FancyTabWidget::currentWidget() { return m_modesStack->currentWidget(); } diff --git a/ground/openpilotgcs/src/plugins/config/fancytabwidget.h b/ground/openpilotgcs/src/plugins/config/fancytabwidget.h index 73da2aae8..e0bcad74a 100644 --- a/ground/openpilotgcs/src/plugins/config/fancytabwidget.h +++ b/ground/openpilotgcs/src/plugins/config/fancytabwidget.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,18 +40,17 @@ class QStackedLayout; class QStatusBar; QT_END_NAMESPACE - struct FancyTab { - QIcon icon; - QString text; - QString toolTip; - }; +struct FancyTab { + QIcon icon; + QString text; + QString toolTip; +}; -class FancyTabBar : public QWidget -{ +class FancyTabBar : public QWidget { Q_OBJECT public: - FancyTabBar(QWidget *parent = 0, bool isVertical=false); + FancyTabBar(QWidget *parent = 0, bool isVertical = false); ~FancyTabBar(); bool event(QEvent *event); @@ -66,26 +65,50 @@ public: QSize sizeHint() const; QSize minimumSizeHint() const; - void insertTab(int index, const QIcon &icon, const QString &label) { + void insertTab(int index, const QIcon &icon, const QString &label) + { FancyTab tab; + tab.icon = icon; tab.text = label; m_tabs.insert(index, tab); } - void removeTab(int index) { + void removeTab(int index) + { m_tabs.removeAt(index); } void updateTabNameIcon(int index, const QIcon &icon, const QString &label); void setCurrentIndex(int index); - int currentIndex() const { return m_currentIndex; } + int currentIndex() const + { + return m_currentIndex; + } - void setTabToolTip(int index, QString toolTip) { m_tabs[index].toolTip = toolTip; } - QString tabToolTip(int index) const { return m_tabs.at(index).toolTip; } + void setTabToolTip(int index, QString toolTip) + { + m_tabs[index].toolTip = toolTip; + } + QString tabToolTip(int index) const + { + return m_tabs.at(index).toolTip; + } - void setIconSize(int s) { iconSize = s; } - QIcon tabIcon(int index) const {return m_tabs.at(index).icon; } - QString tabText(int index) const { return m_tabs.at(index).text; } - int count() const {return m_tabs.count(); } + void setIconSize(int s) + { + iconSize = s; + } + QIcon tabIcon(int index) const + { + return m_tabs.at(index).icon; + } + QString tabText(int index) const + { + return m_tabs.at(index).text; + } + int count() const + { + return m_tabs.count(); + } QRect tabRect(int index) const; @@ -108,11 +131,9 @@ private: bool verticalTabs; QSize tabSizeHint(bool minimum = false) const; - }; -class FancyTabWidget : public QWidget -{ +class FancyTabWidget : public QWidget { Q_OBJECT public: @@ -126,14 +147,17 @@ public: int cornerWidgetCount() const; void setTabToolTip(int index, const QString &toolTip); void updateTabNameIcon(int index, const QIcon &icon, const QString &label); - void setIconSize(int s) { m_tabBar->setIconSize(s); } + void setIconSize(int s) + { + m_tabBar->setIconSize(s); + } void paintEvent(QPaintEvent *event); int currentIndex() const; QStatusBar *statusBar() const; - QWidget * currentWidget(); + QWidget *currentWidget(); signals: void currentAboutToShow(int index); void currentChanged(int index); diff --git a/ground/openpilotgcs/src/plugins/config/gyro-calibration.cpp b/ground/openpilotgcs/src/plugins/config/gyro-calibration.cpp index 20c12c252..9f35fbcdd 100644 --- a/ground/openpilotgcs/src/plugins/config/gyro-calibration.cpp +++ b/ground/openpilotgcs/src/plugins/config/gyro-calibration.cpp @@ -1,4 +1,3 @@ - #include #include #include @@ -9,79 +8,78 @@ * The measurement equation is * gyro_k = accelSensitivity * \tilde{accel}_k + bias + omega * - * where, omega is the true angular rate (assumed to be zero) - * bias is the sensor bias - * accelSensitivity is the 3x3 matrix of sensitivity scale factors - * \tilde{accel}_k is the calibrated measurement of the accelerometer - * at time k + * where, omega is the true angular rate (assumed to be zero) + * bias is the sensor bias + * accelSensitivity is the 3x3 matrix of sensitivity scale factors + * \tilde{accel}_k is the calibrated measurement of the accelerometer + * at time k * * After calibration, the optimized gyro measurement is given by * \tilde{gyro}_k = gyro_k - bias - accelSensitivity * \tilde{accel}_k */ -void -gyroscope_calibration(Vector3f& bias, - Matrix3f& accelSensitivity, - Vector3f gyroSamples[], - Vector3f accelSamples[], - size_t n_samples) +void gyroscope_calibration(Vector3f & bias, + Matrix3f & accelSensitivity, + Vector3f gyroSamples[], + Vector3f accelSamples[], + size_t n_samples) { - // Assume the following measurement model: - // y = H*x - // where x is the vector of unknowns, and y is the measurement vector. - // the vector of unknowns is - // [a_x a_y a_z b_x] - // The measurement vector is - // [gyro_x] - // and the measurement matrix H is - // [accelSample_x accelSample_y accelSample_z 1] + // Assume the following measurement model: + // y = H*x + // where x is the vector of unknowns, and y is the measurement vector. + // the vector of unknowns is + // [a_x a_y a_z b_x] + // The measurement vector is + // [gyro_x] + // and the measurement matrix H is + // [accelSample_x accelSample_y accelSample_z 1] - // Note that the individual solutions for x are given by - // (H^T \times H)^-1 \times H^T y - // Everything is identical except for y and x. So, transform it - // into block form X = (H^T \times H)^-1 \times H^T Y - // where each column of X contains the partial solution for each - // column of y. + // Note that the individual solutions for x are given by + // (H^T \times H)^-1 \times H^T y + // Everything is identical except for y and x. So, transform it + // into block form X = (H^T \times H)^-1 \times H^T Y + // where each column of X contains the partial solution for each + // column of y. - // Construct the matrix of accelerometer samples. Intermediate results - // are computed in "twice the precision that the source provides and the - // result deserves" by Kahan's thumbrule to prevent numerical problems. - Matrix H(n_samples, 4); - // And the matrix of gyro samples. - Matrix Y(n_samples, 3); - for (unsigned i = 0; i < n_samples; ++i) { - H.row(i) << accelSamples[i].transpose().cast(), 1.0; - Y.row(i) << gyroSamples[i].transpose().cast(); - } + // Construct the matrix of accelerometer samples. Intermediate results + // are computed in "twice the precision that the source provides and the + // result deserves" by Kahan's thumbrule to prevent numerical problems. + Matrix H(n_samples, 4); + // And the matrix of gyro samples. + Matrix Y(n_samples, 3); + for (unsigned i = 0; i < n_samples; ++i) { + H.row(i) << accelSamples[i].transpose().cast(), 1.0; + Y.row(i) << gyroSamples[i].transpose().cast(); + } #if 1 - Matrix result; - // Use the cholesky-based Penrose pseudoinverse method. - (H.transpose() * H).ldlt().solve(H.transpose()*Y, &result); + Matrix result; + // Use the cholesky-based Penrose pseudoinverse method. + (H.transpose() * H).ldlt().solve(H.transpose() * Y, &result); - // Transpose the result and return it to the caller. Cast back to float - // since there really isn't that much accuracy in the result. - bias = result.row(3).transpose().cast(); - accelSensitivity = result.block<3, 3>(0, 0).transpose().cast(); + // Transpose the result and return it to the caller. Cast back to float + // since there really isn't that much accuracy in the result. + bias = result.row(3).transpose().cast(); + accelSensitivity = result.block<3, 3>(0, 0).transpose().cast(); #else - // TODO: Compare this result with a total-least-squares model. - size_t n = 4; - Matrix C; - C << H, Y; - SVD > usv(C); - Matrix V_ab = usv.matrixV().block<4, 3>(0, n); - Matrix V_bb = usv.matrixV().corner(BottomRight, n_samples-4, 3); - // X = -V_ab/V_bb; - // X^T = (A * B^-1)^T - // X^T = (B^-1^T * A^T) - // X^T = (B^T^-1 * A^T) - // V_bb is orthgonal but not orthonormal. QR decomposition - // should be very fast in this case. - Matrix result; - V_bb.transpose().qr().solve(-V_ab.transpose(), &result); + // TODO: Compare this result with a total-least-squares model. + size_t n = 4; + Matrix C; + C << H, Y; + SVD > usv(C); + Matrix V_ab = usv.matrixV().block<4, 3>(0, n); + Matrix V_bb = usv.matrixV().corner(BottomRight, n_samples - 4, 3); + // X = -V_ab/V_bb; + // X^T = (A * B^-1)^T + // X^T = (B^-1^T * A^T) + // X^T = (B^T^-1 * A^T) + // V_bb is orthgonal but not orthonormal. QR decomposition + // should be very fast in this case. + Matrix result; + V_bb.transpose().qr().solve(-V_ab.transpose(), &result); - // Results only deserve single precision. - bias = result.col(3).cast(); - accelSensitivity = result.block<3, 3>(0, 0).cast(); + // Results only deserve single precision. + bias = result.col(3).cast(); + accelSensitivity = result.block<3, 3>(0, 0).cast(); -#endif +#endif // if 1 } diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.h b/ground/openpilotgcs/src/plugins/config/inputchannelform.h index 606ef2d78..ffdb9898b 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.h +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.h @@ -4,15 +4,14 @@ #include #include "configinputwidget.h" namespace Ui { - class inputChannelForm; +class inputChannelForm; } -class inputChannelForm : public ConfigTaskWidget -{ +class inputChannelForm : public ConfigTaskWidget { Q_OBJECT public: - explicit inputChannelForm(QWidget *parent = 0,bool showlegend=false); + explicit inputChannelForm(QWidget *parent = 0, bool showlegend = false); ~inputChannelForm(); friend class ConfigInputWidget; void setName(QString &name); diff --git a/ground/openpilotgcs/src/plugins/config/legacy-calibration.cpp b/ground/openpilotgcs/src/plugins/config/legacy-calibration.cpp index 92e34efb2..efaf3fc06 100644 --- a/ground/openpilotgcs/src/plugins/config/legacy-calibration.cpp +++ b/ground/openpilotgcs/src/plugins/config/legacy-calibration.cpp @@ -34,8 +34,8 @@ * The measurement equation is * B_k = D^-1 * (A_k * H_k - b) * Where B_k is the measurement of the field at time k - * D is the diagonal matrix of scale factors - * A_k is the attitude direction cosine matrix at time k + * D is the diagonal matrix of scale factors + * A_k is the attitude direction cosine matrix at time k * H_k is the reference field at the kth sample * b is the vector of scale factors. * @@ -45,70 +45,67 @@ * @param bias[out] The computed bias of the sensor * @param scale[out] The computed scale factor of the sensor * @param samples An array of sample data points. Only the first 6 are - * used. + * used. * @param n_samples The number of sample data points. Must be at least 6. * @param referenceField The field being measured by the sensor. */ -void -openpilot_bias_scale(Vector3f& bias, - Vector3f& scale, - const Vector3f samples[], - const size_t n_samples, - const Vector3f& referenceField) +void openpilot_bias_scale(Vector3f & bias, + Vector3f & scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f & referenceField) { - // TODO: Add error handling, and return error codes through the return - // value. - if (n_samples < 6) { - bias = Vector3f::Zero(); - scale = Vector3f::Zero(); - return; - } - // mag = (S*x + b)**2 - // mag = (S**2 * x**2 + 2*S*b*x + b*b) - // 0 = S**2 * (x1**2 - x2**2) + 2*S*B*(x1 - x2)) - // Fill in matrix A - - // write six difference-in-magnitude equations of the form - // Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + - // 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0 - // - // or in other words - // 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + - // 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = - // (x1^2-x2^2) - Matrix A; - Matrix f; - for (unsigned i=0; i<5; i++){ - A.row(i)[0] = 2.0 * (samples[i+1].x() - samples[i].x()); - A.row(i)[1] = samples[i+1].y()* samples[i+1].y() - samples[i].y()*samples[i].y(); - A.row(i)[2] = 2.0 * (samples[i+1].y() - samples[i].y()); - A.row(i)[3] = samples[i+1].z()*samples[i+1].z() - samples[i].z()*samples[i].z(); - A.row(i)[4] = 2.0 * (samples[i+1].z() - samples[i].z()); - f[i] = samples[i].x()*samples[i].x() - samples[i+1].x()*samples[i+1].x(); - } - Matrix c; - A.lu().solve(f, &c); + // TODO: Add error handling, and return error codes through the return + // value. + if (n_samples < 6) { + bias = Vector3f::Zero(); + scale = Vector3f::Zero(); + return; + } + // mag = (S*x + b)**2 + // mag = (S**2 * x**2 + 2*S*b*x + b*b) + // 0 = S**2 * (x1**2 - x2**2) + 2*S*B*(x1 - x2)) + // Fill in matrix A - + // write six difference-in-magnitude equations of the form + // Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + + // 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0 + // + // or in other words + // 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + + // 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = + // (x1^2-x2^2) + Matrix A; + Matrix f; + for (unsigned i = 0; i < 5; i++) { + A.row(i)[0] = 2.0 * (samples[i + 1].x() - samples[i].x()); + A.row(i)[1] = samples[i + 1].y() * samples[i + 1].y() - samples[i].y() * samples[i].y(); + A.row(i)[2] = 2.0 * (samples[i + 1].y() - samples[i].y()); + A.row(i)[3] = samples[i + 1].z() * samples[i + 1].z() - samples[i].z() * samples[i].z(); + A.row(i)[4] = 2.0 * (samples[i + 1].z() - samples[i].z()); + f[i] = samples[i].x() * samples[i].x() - samples[i + 1].x() * samples[i + 1].x(); + } + Matrix c; + A.lu().solve(f, &c); - // use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer - float xp = samples[0].x(); + // use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer + float xp = samples[0].x(); float yp = samples[0].y(); - float zp = samples[0].z(); + float zp = samples[0].z(); - float Sx = sqrt(referenceField.squaredNorm() / - (xp*xp + 2*c[0]*xp + c[0]*c[0] + c[1]*yp*yp + 2*c[2]*yp + c[2]*c[2]/c[1] + c[3]*zp*zp + 2*c[4]*zp + c[4]*c[4]/c[3])); + float Sx = sqrt(referenceField.squaredNorm() / + (xp * xp + 2 * c[0] * xp + c[0] * c[0] + c[1] * yp * yp + 2 * c[2] * yp + c[2] * c[2] / c[1] + c[3] * zp * zp + 2 * c[4] * zp + c[4] * c[4] / c[3])); - scale[0] = Sx; - bias[0] = Sx*c[0]; - scale[1] = sqrt(c[1]*Sx*Sx); - bias[1] = c[2]*Sx*Sx/scale[1]; - scale[2] = sqrt(c[3]*Sx*Sx); - bias[2] = c[4]*Sx*Sx/scale[2]; + scale[0] = Sx; + bias[0] = Sx * c[0]; + scale[1] = sqrt(c[1] * Sx * Sx); + bias[1] = c[2] * Sx * Sx / scale[1]; + scale[2] = sqrt(c[3] * Sx * Sx); + bias[2] = c[4] * Sx * Sx / scale[2]; - for (int i = 0; i < 3; ++i) { - // Fixup difference between openpilot measurement model and twostep - // version - bias[i] = -bias[i] / scale[i]; - } + for (int i = 0; i < 3; ++i) { + // Fixup difference between openpilot measurement model and twostep + // version + bias[i] = -bias[i] / scale[i]; + } } - - diff --git a/ground/openpilotgcs/src/plugins/config/mixercurve.h b/ground/openpilotgcs/src/plugins/config/mixercurve.h index 58e0c2f8b..bd4b0acec 100644 --- a/ground/openpilotgcs/src/plugins/config/mixercurve.h +++ b/ground/openpilotgcs/src/plugins/config/mixercurve.h @@ -43,23 +43,22 @@ namespace Ui { class MixerCurve; } -class MixerCurve : public QFrame -{ +class MixerCurve : public QFrame { Q_OBJECT - + public: explicit MixerCurve(QWidget *parent = 0); ~MixerCurve(); /* Enumeration options for ThrottleCurves */ - typedef enum { MIXERCURVE_THROTTLE=0, MIXERCURVE_PITCH=1 } MixerCurveType; + typedef enum { MIXERCURVE_THROTTLE = 0, MIXERCURVE_PITCH = 1 } MixerCurveType; void setMixerType(MixerCurveType curveType); - void initCurve (const QList* points); + void initCurve(const QList *points); QList getCurve(); void initLinearCurve(int numPoints, double maxValue = 1, double minValue = 0); - void setCurve(const QList* points); + void setCurve(const QList *points); void setMin(double value); double getMin(); void setMax(double value); @@ -69,7 +68,10 @@ public: double getCurveStep(); double setRange(double min, double max); - MixerCurveWidget* getCurveWidget() { return m_curve; } + MixerCurveWidget *getCurveWidget() + { + return m_curve; + } signals: @@ -91,12 +93,11 @@ private slots: void UpdateCurveUI(); private: - Ui::MixerCurve* m_mixerUI; - MixerCurveWidget* m_curve; - QTableWidget* m_settings; + Ui::MixerCurve *m_mixerUI; + MixerCurveWidget *m_curve; + QTableWidget *m_settings; MixerCurveType m_curveType; - DoubleSpinDelegate* m_spinDelegate; - + DoubleSpinDelegate *m_spinDelegate; }; #endif // MIXERCURVE_H diff --git a/ground/openpilotgcs/src/plugins/config/outputchannelform.cpp b/ground/openpilotgcs/src/plugins/config/outputchannelform.cpp index f2d039edb..a69a88bf9 100644 --- a/ground/openpilotgcs/src/plugins/config/outputchannelform.cpp +++ b/ground/openpilotgcs/src/plugins/config/outputchannelform.cpp @@ -29,24 +29,26 @@ #include "configoutputwidget.h" OutputChannelForm::OutputChannelForm(const int index, QWidget *parent, const bool showLegend) : - ConfigTaskWidget(parent), - ui(), - m_index(index), - m_inChannelTest(false) + ConfigTaskWidget(parent), + ui(), + m_index(index), + m_inChannelTest(false) { ui.setupUi(this); - if(!showLegend) - { + if (!showLegend) { // Remove legend - QGridLayout *grid_layout = dynamic_cast(layout()); + QGridLayout *grid_layout = dynamic_cast(layout()); Q_ASSERT(grid_layout); - for (int col = 0; col < grid_layout->columnCount(); col++) - { // remove every item in first row + for (int col = 0; col < grid_layout->columnCount(); col++) { // remove every item in first row QLayoutItem *item = grid_layout->itemAtPosition(0, col); - if (!item) continue; + if (!item) { + continue; + } // get widget from layout item QWidget *legend_widget = item->widget(); - if (!legend_widget) continue; + if (!legend_widget) { + continue; + } // delete widget grid_layout->removeWidget(legend_widget); delete legend_widget; @@ -54,7 +56,7 @@ OutputChannelForm::OutputChannelForm(const int index, QWidget *parent, const boo } // The convention for OP is Channel 1 to Channel 10. - ui.actuatorNumber->setText(QString("%1:").arg(m_index+1)); + ui.actuatorNumber->setText(QString("%1:").arg(m_index + 1)); // Register for ActuatorSettings changes: connect(ui.actuatorMin, SIGNAL(editingFinished()), @@ -84,19 +86,18 @@ OutputChannelForm::~OutputChannelForm() */ void OutputChannelForm::enableChannelTest(bool state) { - if (m_inChannelTest == state) return; + if (m_inChannelTest == state) { + return; + } m_inChannelTest = state; - if(m_inChannelTest) - { + if (m_inChannelTest) { // Prevent stupid users from touching the minimum & maximum ranges while // moving the sliders. Thanks Ivan for the tip :) ui.actuatorMin->setEnabled(false); ui.actuatorMax->setEnabled(false); ui.actuatorRev->setEnabled(false); - } - else - { + } else { ui.actuatorMin->setEnabled(true); ui.actuatorMax->setEnabled(true); ui.actuatorRev->setEnabled(true); @@ -111,34 +112,39 @@ void OutputChannelForm::linkToggled(bool state) { Q_UNUSED(state) - if (!m_inChannelTest) - return; // we are not in Test Output mode - + if (!m_inChannelTest) { + return; // we are not in Test Output mode + } // find the minimum slider value for the linked ones - if (!parent()) return; + if (!parent()) { + return; + } int min = 10000; int linked_count = 0; - QList outputChannelForms = parent()->findChildren(); + QList outputChannelForms = parent()->findChildren(); // set the linked channels of the parent widget to the same value - foreach(OutputChannelForm *outputChannelForm, outputChannelForms) - { - if (!outputChannelForm->ui.actuatorLink->checkState()) + foreach(OutputChannelForm * outputChannelForm, outputChannelForms) { + if (!outputChannelForm->ui.actuatorLink->checkState()) { continue; - if (this == outputChannelForm) + } + if (this == outputChannelForm) { continue; + } int value = outputChannelForm->ui.actuatorNeutral->value(); - if(min > value) min = value; + if (min > value) { + min = value; + } linked_count++; } - if (linked_count <= 0) - return; // no linked channels - + if (linked_count <= 0) { + return; // no linked channels + } // set the linked channels to the same value - foreach(OutputChannelForm *outputChannelForm, outputChannelForms) - { - if (!outputChannelForm->ui.actuatorLink->checkState()) + foreach(OutputChannelForm * outputChannelForm, outputChannelForms) { + if (!outputChannelForm->ui.actuatorLink->checkState()) { continue; + } outputChannelForm->ui.actuatorNeutral->setValue(min); } } @@ -167,10 +173,11 @@ void OutputChannelForm::minmax(int minimum, int maximum) ui.actuatorMin->setValue(minimum); ui.actuatorMax->setValue(maximum); setChannelRange(); - if(ui.actuatorMin->value() > ui.actuatorMax->value()) + if (ui.actuatorMin->value() > ui.actuatorMax->value()) { ui.actuatorRev->setChecked(true); - else + } else { ui.actuatorRev->setChecked(false); + } } /** @@ -188,17 +195,18 @@ void OutputChannelForm::setAssignment(const QString &assignment) { ui.actuatorName->setText(assignment); QFontMetrics metrics(ui.actuatorName->font()); - int width=metrics.width(assignment)+1; - foreach(OutputChannelForm * form,parent()->findChildren()) - { - if(form==this) + int width = metrics.width(assignment) + 1; + foreach(OutputChannelForm * form, parent()->findChildren()) { + if (form == this) { continue; - if(form->ui.actuatorName->minimumSize().width()ui.actuatorName->setMinimumSize(width,0); - else - width=form->ui.actuatorName->minimumSize().width(); + } + if (form->ui.actuatorName->minimumSize().width() < width) { + form->ui.actuatorName->setMinimumSize(width, 0); + } else { + width = form->ui.actuatorName->minimumSize().width(); + } } - ui.actuatorName->setMinimumSize(width,0); + ui.actuatorName->setMinimumSize(width, 0); } /** @@ -211,24 +219,23 @@ void OutputChannelForm::setAssignment(const QString &assignment) void OutputChannelForm::setChannelRange() { int oldMini = ui.actuatorNeutral->minimum(); -// int oldMaxi = ui.actuatorNeutral->maximum(); - if (ui.actuatorMin->value() < ui.actuatorMax->value()) - { +// int oldMaxi = ui.actuatorNeutral->maximum(); + + if (ui.actuatorMin->value() < ui.actuatorMax->value()) { ui.actuatorNeutral->setRange(ui.actuatorMin->value(), ui.actuatorMax->value()); ui.actuatorRev->setChecked(false); - } - else - { + } else { ui.actuatorNeutral->setRange(ui.actuatorMax->value(), ui.actuatorMin->value()); ui.actuatorRev->setChecked(true); } - if (ui.actuatorNeutral->value() == oldMini) + if (ui.actuatorNeutral->value() == oldMini) { ui.actuatorNeutral->setValue(ui.actuatorNeutral->minimum()); + } -// if (ui.actuatorNeutral->value() == oldMaxi) -// ui.actuatorNeutral->setValue(ui.actuatorNeutral->maximum()); // this can be dangerous if it happens to be controlling a motor at the time! +// if (ui.actuatorNeutral->value() == oldMaxi) +// ui.actuatorNeutral->setValue(ui.actuatorNeutral->maximum()); // this can be dangerous if it happens to be controlling a motor at the time! } /** @@ -238,10 +245,12 @@ void OutputChannelForm::reverseChannel(bool state) { // Sanity check: if state became true, make sure the Maxvalue was higher than Minvalue // the situations below can happen! - if (state && (ui.actuatorMax->value() < ui.actuatorMin->value())) + if (state && (ui.actuatorMax->value() < ui.actuatorMin->value())) { return; - if (!state && (ui.actuatorMax->value() > ui.actuatorMin->value())) + } + if (!state && (ui.actuatorMax->value() > ui.actuatorMin->value())) { return; + } // Now, swap the min & max values (only on the spinboxes, the slider // does not change! @@ -253,15 +262,12 @@ void OutputChannelForm::reverseChannel(bool state) // This is a trick to force the slider to update its value and // emit the right signal itself, because our sendChannelTest(int) method // relies on the object sender's identity. - if (ui.actuatorNeutral->value() < ui.actuatorNeutral->maximum()) - { - ui.actuatorNeutral->setValue(ui.actuatorNeutral->value()+1); - ui.actuatorNeutral->setValue(ui.actuatorNeutral->value()-1); - } - else - { - ui.actuatorNeutral->setValue(ui.actuatorNeutral->value()-1); - ui.actuatorNeutral->setValue(ui.actuatorNeutral->value()+1); + if (ui.actuatorNeutral->value() < ui.actuatorNeutral->maximum()) { + ui.actuatorNeutral->setValue(ui.actuatorNeutral->value() + 1); + ui.actuatorNeutral->setValue(ui.actuatorNeutral->value() - 1); + } else { + ui.actuatorNeutral->setValue(ui.actuatorNeutral->value() - 1); + ui.actuatorNeutral->setValue(ui.actuatorNeutral->value() + 1); } } @@ -273,39 +279,48 @@ void OutputChannelForm::sendChannelTest(int value) { int in_value = value; - QSlider *ob = (QSlider *)QObject::sender(); - if (!ob) return; + QSlider *ob = (QSlider *)QObject::sender(); - if (ui.actuatorRev->isChecked()) - value = ui.actuatorMin->value() - value + ui.actuatorMax->value(); // the channel is reversed + if (!ob) { + return; + } + if (ui.actuatorRev->isChecked()) { + value = ui.actuatorMin->value() - value + ui.actuatorMax->value(); // the channel is reversed + } // update the label ui.actuatorValue->setText(QString::number(value)); - if (ui.actuatorLink->checkState() && parent()) - { // the channel is linked to other channels - QList outputChannelForms = parent()->findChildren(); + if (ui.actuatorLink->checkState() && parent()) { // the channel is linked to other channels + QList outputChannelForms = parent()->findChildren(); // set the linked channels of the parent widget to the same value - foreach(OutputChannelForm *outputChannelForm, outputChannelForms) - { - if (this == outputChannelForm) continue; - if (!outputChannelForm->ui.actuatorLink->checkState()) continue; + foreach(OutputChannelForm * outputChannelForm, outputChannelForms) { + if (this == outputChannelForm) { + continue; + } + if (!outputChannelForm->ui.actuatorLink->checkState()) { + continue; + } int val = in_value; - if (val < outputChannelForm->ui.actuatorNeutral->minimum()) + if (val < outputChannelForm->ui.actuatorNeutral->minimum()) { val = outputChannelForm->ui.actuatorNeutral->minimum(); - if (val > outputChannelForm->ui.actuatorNeutral->maximum()) + } + if (val > outputChannelForm->ui.actuatorNeutral->maximum()) { val = outputChannelForm->ui.actuatorNeutral->maximum(); + } - if (outputChannelForm->ui.actuatorNeutral->value() == val) continue; + if (outputChannelForm->ui.actuatorNeutral->value() == val) { + continue; + } outputChannelForm->ui.actuatorNeutral->setValue(val); outputChannelForm->ui.actuatorValue->setText(QString::number(val)); } } - if (!m_inChannelTest) - return; // we are not in Test Output mode - + if (!m_inChannelTest) { + return; // we are not in Test Output mode + } emit channelChanged(index(), value); } diff --git a/ground/openpilotgcs/src/plugins/config/outputchannelform.h b/ground/openpilotgcs/src/plugins/config/outputchannelform.h index 434f9517a..a669fc2f8 100644 --- a/ground/openpilotgcs/src/plugins/config/outputchannelform.h +++ b/ground/openpilotgcs/src/plugins/config/outputchannelform.h @@ -31,8 +31,7 @@ #include "ui_outputchannelform.h" #include "configtaskwidget.h" -class OutputChannelForm : public ConfigTaskWidget -{ +class OutputChannelForm : public ConfigTaskWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/twostep.cpp b/ground/openpilotgcs/src/plugins/config/twostep.cpp index fa9e95f58..e1562af29 100644 --- a/ground/openpilotgcs/src/plugins/config/twostep.cpp +++ b/ground/openpilotgcs/src/plugins/config/twostep.cpp @@ -3,7 +3,7 @@ * * @file twostep.cpp * @author Jonathan Brandmeyer - * Copyright (C) 2010. + * Copyright (C) 2010. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup Config plugin @@ -40,12 +40,12 @@ #include #if defined(__APPLE__) || defined(_WIN32) - // Qt bug work around +// Qt bug work around #ifndef isnan - extern "C" int isnan(double); +extern "C" int isnan(double); #endif #ifndef isinf - extern "C" int isinf(double); +extern "C" int isinf(double); #endif #endif @@ -66,31 +66,32 @@ */ namespace { -Vector3f center(const Vector3f samples[], - size_t n_samples) +Vector3f center(const Vector3f samples[], + size_t n_samples) { - Vector3f summation(Vector3f::Zero()); - for (size_t i = 0; i < n_samples; ++i) { - summation += samples[i]; - } - return summation / float(n_samples); + Vector3f summation(Vector3f::Zero()); + + for (size_t i = 0; i < n_samples; ++i) { + summation += samples[i]; + } + return summation / float(n_samples); } -void inv_fisher_information_matrix(Matrix3f& fisherInv, - Matrix3f& fisher, - const Vector3f samples[], - size_t n_samples, - const float noise) +void inv_fisher_information_matrix(Matrix3f & fisherInv, + Matrix3f & fisher, + const Vector3f samples[], + size_t n_samples, + const float noise) { - MatrixXf column_samples(3, n_samples); - for (size_t i = 0; i < n_samples; ++i) { - column_samples.col(i) = samples[i]; - } - fisher = 4 / noise * - column_samples * column_samples.transpose(); - // Compute the inverse by taking advantage of the results symmetricness - fisher.ldlt().solve(Matrix3f::Identity(), &fisherInv); - return; + MatrixXf column_samples(3, n_samples); + + for (size_t i = 0; i < n_samples; ++i) { + column_samples.col(i) = samples[i]; + } + fisher = 4 / noise * + column_samples * column_samples.transpose(); + // Compute the inverse by taking advantage of the results symmetricness + fisher.ldlt().solve(Matrix3f::Identity(), &fisherInv); } // Eq 14 of the original reference. Computes the gradiant of the cost function // with respect to the bias offset. @@ -100,96 +101,95 @@ void inv_fisher_information_matrix(Matrix3f& fisherInv, // @param mu: The trace of the noise matrix (3*noise) // @return the negative gradiant of the cost function with respect to the // current value of the estimate, b -Vector3f -neg_dJdb(const Vector3f samples[], - const float mags[], - size_t n_samples, - const Vector3f& b, - float mu, - float noise) +Vector3f neg_dJdb(const Vector3f samples[], + const float mags[], + size_t n_samples, + const Vector3f & b, + float mu, + float noise) { - Vector3f summation(Vector3f::Zero()); - float b_norm2 = b.squaredNorm(); - - for (size_t i = 0; i < n_samples; ++i) { - summation += (mags[i] - 2*samples[i].dot(b) + b_norm2 - mu) - *2* (samples[i] - b); - } - - return (1.0 / noise)*summation; + Vector3f summation(Vector3f::Zero()); + float b_norm2 = b.squaredNorm(); + + for (size_t i = 0; i < n_samples; ++i) { + summation += (mags[i] - 2 * samples[i].dot(b) + b_norm2 - mu) + * 2 * (samples[i] - b); + } + + return (1.0 / noise) * summation; } } // !namespace (anon) -Vector3f twostep_bias_only(const Vector3f samples[], - size_t n_samples, - const Vector3f& referenceField, - const float noise) +Vector3f twostep_bias_only(const Vector3f samples[], + size_t n_samples, + const Vector3f & referenceField, + const float noise) { - // \tilde{H} - Vector3f centeredSamples[n_samples]; - // z_k - float sampleDeltaMag[n_samples]; - // eq 7 and 8 applied to samples - Vector3f avg = center(samples, n_samples); - float refSquaredNorm = referenceField.squaredNorm(); - float sampleDeltaMagCenter = 0; - for (size_t i = 0; i < n_samples; ++i) - { - // eq 9 applied to samples - centeredSamples[i] = samples[i] - avg; - // eqn 2a - sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; - sampleDeltaMagCenter += sampleDeltaMag[i]; - } - sampleDeltaMagCenter /= n_samples; + // \tilde{H} + Vector3f centeredSamples[n_samples]; + // z_k + float sampleDeltaMag[n_samples]; + // eq 7 and 8 applied to samples + Vector3f avg = center(samples, n_samples); + float refSquaredNorm = referenceField.squaredNorm(); + float sampleDeltaMagCenter = 0; - Matrix3f P_bb; - Matrix3f P_bb_inv; - // Due to eq 12b - inv_fisher_information_matrix(P_bb, P_bb_inv, centeredSamples, n_samples, noise); - // Compute centered magnitudes - float sampleDeltaMagCentered[n_samples]; - for (size_t i = 0; i < n_samples; ++i) { - sampleDeltaMagCentered[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; - } + for (size_t i = 0; i < n_samples; ++i) { + // eq 9 applied to samples + centeredSamples[i] = samples[i] - avg; + // eqn 2a + sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; + sampleDeltaMagCenter += sampleDeltaMag[i]; + } + sampleDeltaMagCenter /= n_samples; - // From eq 12a - Vector3f estimate( Vector3f::Zero()); - for (size_t i = 0; i < n_samples; ++i) { - estimate += sampleDeltaMagCentered[i] * centeredSamples[i]; - } - estimate = P_bb * ((2 / noise) * estimate); + Matrix3f P_bb; + Matrix3f P_bb_inv; + // Due to eq 12b + inv_fisher_information_matrix(P_bb, P_bb_inv, centeredSamples, n_samples, noise); + // Compute centered magnitudes + float sampleDeltaMagCentered[n_samples]; + for (size_t i = 0; i < n_samples; ++i) { + sampleDeltaMagCentered[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; + } - // Newton-Raphson gradient descent to the optimal solution - // eq 14a and 14b - float mu = -3*noise; - for (int i = 0; i < 6; ++i) { - Vector3f neg_gradiant = neg_dJdb(samples, sampleDeltaMag, n_samples, - estimate, mu, noise); - Matrix3f scale = P_bb_inv + 4/noise*(avg - estimate)*(avg - estimate).transpose(); - Vector3f neg_increment; - scale.ldlt().solve(neg_gradiant, &neg_increment); - // Note that the negative has been done twice - estimate += neg_increment; - } - return estimate; + // From eq 12a + Vector3f estimate(Vector3f::Zero()); + for (size_t i = 0; i < n_samples; ++i) { + estimate += sampleDeltaMagCentered[i] * centeredSamples[i]; + } + estimate = P_bb * ((2 / noise) * estimate); + + // Newton-Raphson gradient descent to the optimal solution + // eq 14a and 14b + float mu = -3 * noise; + for (int i = 0; i < 6; ++i) { + Vector3f neg_gradiant = neg_dJdb(samples, sampleDeltaMag, n_samples, + estimate, mu, noise); + Matrix3f scale = P_bb_inv + 4 / noise * (avg - estimate) * (avg - estimate).transpose(); + Vector3f neg_increment; + scale.ldlt().solve(neg_gradiant, &neg_increment); + // Note that the negative has been done twice + estimate += neg_increment; + } + return estimate; } namespace { // Private utility functions for the version of TWOSTEP that computes bias and // scale factor vectors alone. -/** Compute the gradiant of the norm of b with respect to the estimate vector. +/** Compute the gradiant of the norm of b with respect to the estimate vector. */ Matrix -dnormb_dtheta(const Matrix& theta) +dnormb_dtheta(const Matrix & theta) { - return (Matrix() << 2*theta.coeff(0)/(1+theta.coeff(3)), - 2*theta.coeff(1)/(1+theta.coeff(4)), - 2*theta.coeff(2)/(1+theta.coeff(5)), - -pow(theta.coeff(0), 2)/pow(1+theta.coeff(3), 2), - -pow(theta.coeff(1), 2)/pow(1+theta.coeff(4), 2), - -pow(theta.coeff(2), 2)/pow(1+theta.coeff(5), 2)).finished(); + return (Matrix() << 2 * theta.coeff(0) / (1 + theta.coeff(3)), + 2 * theta.coeff(1) / (1 + theta.coeff(4)), + 2 * theta.coeff(2) / (1 + theta.coeff(5)), + -pow(theta.coeff(0), 2) / pow(1 + theta.coeff(3), 2), + -pow(theta.coeff(1), 2) / pow(1 + theta.coeff(4), 2), + -pow(theta.coeff(2), 2) / pow(1 + theta.coeff(5), 2)).finished(); } /** @@ -197,41 +197,40 @@ dnormb_dtheta(const Matrix& theta) * estimate. */ Matrix -dJdb(const Matrix& centerL, - double centerMag, - const Matrix& theta, - const Matrix& dbdtheta, - double mu, - double noise) +dJdb(const Matrix & centerL, + double centerMag, + const Matrix & theta, + const Matrix & dbdtheta, + double mu, + double noise) { - // \frac{\delta}{\delta \theta'} |b(\theta')|^2 - Matrix vectorPart = dbdtheta - centerL; + // \frac{\delta}{\delta \theta'} |b(\theta')|^2 + Matrix vectorPart = dbdtheta - centerL; - // By equation 35 - double normbthetaSquared = 0; - for (unsigned i = 0; i < 3; ++i) { - normbthetaSquared += theta.coeff(i)*theta.coeff(i)/(1+theta.coeff(3+i)); - } - double scalarPart = (centerMag - centerL.dot(theta) + normbthetaSquared - - mu)/noise; - return scalarPart * vectorPart; + // By equation 35 + double normbthetaSquared = 0; + for (unsigned i = 0; i < 3; ++i) { + normbthetaSquared += theta.coeff(i) * theta.coeff(i) / (1 + theta.coeff(3 + i)); + } + double scalarPart = (centerMag - centerL.dot(theta) + normbthetaSquared + - mu) / noise; + return scalarPart * vectorPart; } /** Reconstruct the scale factor and bias offset vectors from the transformed * estimate vector. */ Matrix -theta_to_sane(const Matrix& theta) +theta_to_sane(const Matrix & theta) { - return (Matrix() << - theta.coeff(0) / sqrt(1 + theta.coeff(3)), - theta.coeff(1) / sqrt(1 + theta.coeff(4)), - theta.coeff(2) / sqrt(1 + theta.coeff(5)), - -1 + sqrt(1 + theta.coeff(3)), - -1 + sqrt(1 + theta.coeff(4)), - -1 + sqrt(1 + theta.coeff(5))).finished(); + return (Matrix() << + theta.coeff(0) / sqrt(1 + theta.coeff(3)), + theta.coeff(1) / sqrt(1 + theta.coeff(4)), + theta.coeff(2) / sqrt(1 + theta.coeff(5)), + -1 + sqrt(1 + theta.coeff(3)), + -1 + sqrt(1 + theta.coeff(4)), + -1 + sqrt(1 + theta.coeff(5))).finished(); } - } // !namespace (anon) /** @@ -255,143 +254,142 @@ theta_to_sane(const Matrix& theta) * @param scale[out] The computed scale factor, in the sensor frame * @param samples[in] An array of measurement samples. * @param n_samples The number of samples in the array. - * @param referenceField The magnetic field vector at this location. + * @param referenceField The magnetic field vector at this location. * @param noise The one-sigma squared standard deviation of the observed noise * in the sensor. */ -void twostep_bias_scale(Vector3f& bias, - Vector3f& scale, - const Vector3f samples[], - const size_t n_samples, - const Vector3f& referenceField, - const float noise) +void twostep_bias_scale(Vector3f & bias, + Vector3f & scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f & referenceField, + const float noise) { - // Initial estimate for gradiant descent starts at eq 37a of ref 2. + // Initial estimate for gradiant descent starts at eq 37a of ref 2. - // Define L_k by eq 30 and 28 for k = 1 .. n_samples - Matrix fullSamples(n_samples, 6); - // \hbar{L} by eq 33, simplified by obesrving that the - Matrix centerSample = Matrix::Zero(); - // Define the sample differences z_k by eq 23 a) - double sampleDeltaMag[n_samples]; - // The center value \hbar{z} - double sampleDeltaMagCenter = 0; - double refSquaredNorm = referenceField.squaredNorm(); + // Define L_k by eq 30 and 28 for k = 1 .. n_samples + Matrix fullSamples(n_samples, 6); + // \hbar{L} by eq 33, simplified by obesrving that the + Matrix centerSample = Matrix::Zero(); + // Define the sample differences z_k by eq 23 a) + double sampleDeltaMag[n_samples]; + // The center value \hbar{z} + double sampleDeltaMagCenter = 0; + double refSquaredNorm = referenceField.squaredNorm(); - for (size_t i = 0; i < n_samples; ++i) { - fullSamples.row(i) << 2*samples[i].transpose().cast(), - -(samples[i][0]*samples[i][0]), - -(samples[i][1]*samples[i][1]), - -(samples[i][2]*samples[i][2]); - centerSample += fullSamples.row(i); + for (size_t i = 0; i < n_samples; ++i) { + fullSamples.row(i) << 2 * samples[i].transpose().cast(), + -(samples[i][0] * samples[i][0]), + -(samples[i][1] * samples[i][1]), + -(samples[i][2] * samples[i][2]); + centerSample += fullSamples.row(i); - sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; - sampleDeltaMagCenter += sampleDeltaMag[i]; - } - sampleDeltaMagCenter /= n_samples; - centerSample /= n_samples; + sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; + sampleDeltaMagCenter += sampleDeltaMag[i]; + } + sampleDeltaMagCenter /= n_samples; + centerSample /= n_samples; - // True for all k. - // double mu = -3*noise; - // The center value of mu, \bar{mu} - // double centerMu = mu; - // The center value of mu, \tilde{mu} - // double centeredMu = 0; + // True for all k. + // double mu = -3*noise; + // The center value of mu, \bar{mu} + // double centerMu = mu; + // The center value of mu, \tilde{mu} + // double centeredMu = 0; - // Define \tilde{L}_k for k = 0 .. n_samples - Matrix centeredSamples(n_samples, 6); - // Define \tilde{z}_k for k = 0 .. n_samples - double centeredMags[n_samples]; - // Compute the term under the summation of eq 37a - Matrix estimateSummation = Matrix::Zero(); - for (size_t i = 0; i < n_samples; ++i) { - centeredSamples.row(i) = fullSamples.row(i) - centerSample; - centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; - estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose(); - } - estimateSummation /= noise; // note: paper supplies 1/noise + // Define \tilde{L}_k for k = 0 .. n_samples + Matrix centeredSamples(n_samples, 6); + // Define \tilde{z}_k for k = 0 .. n_samples + double centeredMags[n_samples]; + // Compute the term under the summation of eq 37a + Matrix estimateSummation = Matrix::Zero(); + for (size_t i = 0; i < n_samples; ++i) { + centeredSamples.row(i) = fullSamples.row(i) - centerSample; + centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; + estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose(); + } + estimateSummation /= noise; // note: paper supplies 1/noise - // By eq 37 b). Note, paper supplies 1/noise here - Matrix P_theta_theta_inv = (1.0f/noise)* - centeredSamples.transpose()*centeredSamples; + // By eq 37 b). Note, paper supplies 1/noise here + Matrix P_theta_theta_inv = (1.0f / noise) * + centeredSamples.transpose() * centeredSamples; #ifdef PRINTF_DEBUGGING - SelfAdjointEigenSolver > eig(P_theta_theta_inv); - std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n"; - std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose() - << "\n"; - std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n"; + SelfAdjointEigenSolver > eig(P_theta_theta_inv); + std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n"; + std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose() + << "\n"; + std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n"; #endif - // The Fisher information matrix has a small eigenvalue, with a - // corresponding eigenvector of about [1e-2 1e-2 1e-2 0.55, 0.55, - // 0.55]. This means that there is relatively little information - // about the common gain on the scale factor, which has a - // corresponding effect on the bias, too. The fixup is performed by - // the first iteration of the second stage of TWOSTEP, as documented in - // [1]. - Matrix estimate; - // By eq 37 a), \tilde{Fisher}^-1 - P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate); + // The Fisher information matrix has a small eigenvalue, with a + // corresponding eigenvector of about [1e-2 1e-2 1e-2 0.55, 0.55, + // 0.55]. This means that there is relatively little information + // about the common gain on the scale factor, which has a + // corresponding effect on the bias, too. The fixup is performed by + // the first iteration of the second stage of TWOSTEP, as documented in + // [1]. + Matrix estimate; + // By eq 37 a), \tilde{Fisher}^-1 + P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate); #ifdef PRINTF_DEBUGGING - Matrix P_theta_theta; - P_theta_theta_inv.ldlt().solve(Matrix::Identity(), &P_theta_theta); - SelfAdjointEigenSolver > eig2(P_theta_theta); - std::cout << "eigenvalues: " << eig2.eigenvalues().transpose() << "\n"; - std::cout << "eigenvectors: \n" << eig2.eigenvectors() << "\n"; - std::cout << "estimate summation: \n" << estimateSummation.normalized() - << "\n"; + Matrix P_theta_theta; + P_theta_theta_inv.ldlt().solve(Matrix::Identity(), &P_theta_theta); + SelfAdjointEigenSolver > eig2(P_theta_theta); + std::cout << "eigenvalues: " << eig2.eigenvalues().transpose() << "\n"; + std::cout << "eigenvectors: \n" << eig2.eigenvectors() << "\n"; + std::cout << "estimate summation: \n" << estimateSummation.normalized() + << "\n"; #endif - // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradiant(theta) - // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1} - size_t count = 3; - while (count --> 0) { - // eq 40 - Matrix db_dtheta = dnormb_dtheta(estimate); + // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradiant(theta) + // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1} + size_t count = 3; + while (count-- > 0) { + // eq 40 + Matrix db_dtheta = dnormb_dtheta(estimate); - Matrix dJ_dtheta = dJdb(centerSample, - sampleDeltaMagCenter, - estimate, - db_dtheta, - -3*noise, - noise/n_samples); + Matrix dJ_dtheta = dJdb(centerSample, + sampleDeltaMagCenter, + estimate, + db_dtheta, + -3 * noise, + noise / n_samples); - // Eq 39 (with double-negation on term inside parens) - db_dtheta = centerSample - db_dtheta; - Matrix scale = P_theta_theta_inv + - (double(n_samples)/noise)*db_dtheta.transpose() * db_dtheta; + // Eq 39 (with double-negation on term inside parens) + db_dtheta = centerSample - db_dtheta; + Matrix scale = P_theta_theta_inv + + (double(n_samples) / noise) * db_dtheta.transpose() * db_dtheta; - // eq 14b, mutatis mutandis (grumble, grumble) - Matrix increment; - scale.ldlt().solve(dJ_dtheta, &increment); - estimate -= increment; - } - - // Transform the estimated parameters from [c | e] back into [b | d]. - for (size_t i = 0; i < 3; ++i) { - scale.coeffRef(i) = -1 + sqrt(1 + estimate.coeff(3+i)); - bias.coeffRef(i) = estimate.coeff(i)/sqrt(1 + estimate.coeff(3+i)); - } + // eq 14b, mutatis mutandis (grumble, grumble) + Matrix increment; + scale.ldlt().solve(dJ_dtheta, &increment); + estimate -= increment; + } + + // Transform the estimated parameters from [c | e] back into [b | d]. + for (size_t i = 0; i < 3; ++i) { + scale.coeffRef(i) = -1 + sqrt(1 + estimate.coeff(3 + i)); + bias.coeffRef(i) = estimate.coeff(i) / sqrt(1 + estimate.coeff(3 + i)); + } } namespace { -// Private functions specific to the scale factor and orthogonal calibration +// Private functions specific to the scale factor and orthogonal calibration // version of TWOSTEP -/** +/** * Reconstruct the symmetric E matrix from the last 6 elements of the estimate * vector */ -Matrix3d -E_theta(const Matrix& theta) +Matrix3d E_theta(const Matrix & theta) { - // By equation 49b - return (Matrix3d() << - theta.coeff(3), theta.coeff(6), theta.coeff(7), - theta.coeff(6), theta.coeff(4), theta.coeff(8), - theta.coeff(7), theta.coeff(8), theta.coeff(5)).finished(); + // By equation 49b + return (Matrix3d() << + theta.coeff(3), theta.coeff(6), theta.coeff(7), + theta.coeff(6), theta.coeff(4), theta.coeff(8), + theta.coeff(7), theta.coeff(8), theta.coeff(5)).finished(); } /** @@ -401,22 +399,23 @@ E_theta(const Matrix& theta) * From eq 55 of [2]. */ Matrix -dnormb_dtheta(const Matrix& theta) +dnormb_dtheta(const Matrix & theta) { - // Re-form the matrix E from the vector of theta' - Matrix3d E = E_theta(theta); - // Compute (I + E)^-1 * c - Vector3d IE_inv_c; - E.ldlt().solve(theta.end<3>(), &IE_inv_c); + // Re-form the matrix E from the vector of theta' + Matrix3d E = E_theta(theta); + // Compute (I + E)^-1 * c + Vector3d IE_inv_c; - return (Matrix() << 2*IE_inv_c, - -IE_inv_c.coeff(0)*IE_inv_c.coeff(0), - -IE_inv_c.coeff(1)*IE_inv_c.coeff(1), - -IE_inv_c.coeff(2)*IE_inv_c.coeff(2), + E.ldlt().solve(theta.end<3>(), &IE_inv_c); - -2*IE_inv_c.coeff(0)*IE_inv_c.coeff(1), - -2*IE_inv_c.coeff(0)*IE_inv_c.coeff(2), - -2*IE_inv_c.coeff(1)*IE_inv_c.coeff(2)).finished(); + return (Matrix() << 2 * IE_inv_c, + -IE_inv_c.coeff(0) * IE_inv_c.coeff(0), + -IE_inv_c.coeff(1) * IE_inv_c.coeff(1), + -IE_inv_c.coeff(2) * IE_inv_c.coeff(2), + + -2 * IE_inv_c.coeff(0) * IE_inv_c.coeff(1), + -2 * IE_inv_c.coeff(0) * IE_inv_c.coeff(2), + -2 * IE_inv_c.coeff(1) * IE_inv_c.coeff(2)).finished(); } // The gradient of the cost function with respect to theta, at a particular @@ -427,25 +426,25 @@ dnormb_dtheta(const Matrix& theta) // @return the gradient of the cost function with respect to the // current value of the estimate, theta' Matrix -dJ_dtheta(const Matrix& centerL, - double centerMag, - const Matrix& theta, - const Matrix& dbdtheta, - double mu, - double noise) +dJ_dtheta(const Matrix & centerL, + double centerMag, + const Matrix & theta, + const Matrix & dbdtheta, + double mu, + double noise) { - // \frac{\delta}{\delta \theta'} |b(\theta')|^2 - Matrix vectorPart = dbdtheta - centerL; + // \frac{\delta}{\delta \theta'} |b(\theta')|^2 + Matrix vectorPart = dbdtheta - centerL; - // |b(\theta')|^2 - Matrix3d E = E_theta(theta); - Vector3d rhs; - (Matrix3d::Identity() + E).ldlt().solve(theta.start<3>(), &rhs); - double normbthetaSquared = theta.start<3>().dot(rhs); + // |b(\theta')|^2 + Matrix3d E = E_theta(theta); + Vector3d rhs; + (Matrix3d::Identity() + E).ldlt().solve(theta.start<3>(), &rhs); + double normbthetaSquared = theta.start<3>().dot(rhs); - double scalarPart = (centerMag - centerL.dot(theta) + normbthetaSquared - - mu)/noise; - return scalarPart * vectorPart; + double scalarPart = (centerMag - centerL.dot(theta) + normbthetaSquared + - mu) / noise; + return scalarPart * vectorPart; } } // !namespace (anonymous) @@ -463,7 +462,7 @@ dJ_dtheta(const Matrix& centerL, * \epsilon_k is the noise at the kth sample * * After computing the scale factor and bias matrices, the optimal estimate is - * given by + * given by * \tilde{B}_k = (I_{3x3} + D)B_k - b * * This implementation makes the assumption that \epsilon is a constant white, @@ -474,145 +473,143 @@ dJ_dtheta(const Matrix& centerL, * @param scale[out] The computed scale factor matrix, in the sensor frame. * @param samples[in] An array of measurement samples. * @param n_samples The number of samples in the array. - * @param referenceField The magnetic field vector at this location. + * @param referenceField The magnetic field vector at this location. * @param noise The one-sigma squared standard deviation of the observed noise * in the sensor. */ -void twostep_bias_scale(Vector3f& bias, - Matrix3f& scale, - const Vector3f samples[], - const size_t n_samples, - const Vector3f& referenceField, - const float noise) +void twostep_bias_scale(Vector3f & bias, + Matrix3f & scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f & referenceField, + const float noise) { - // Define L_k by eq 51 for k = 1 .. n_samples - Matrix fullSamples(n_samples, 9); - // \hbar{L} by eq 52, simplified by observing that the common noise term - // makes this a simple average. - Matrix centerSample = Matrix::Zero(); - // Define the sample differences z_k by eq 23 a) - double sampleDeltaMag[n_samples]; - // The center value \hbar{z} - double sampleDeltaMagCenter = 0; - // The squared norm of the reference vector - double refSquaredNorm = referenceField.squaredNorm(); + // Define L_k by eq 51 for k = 1 .. n_samples + Matrix fullSamples(n_samples, 9); + // \hbar{L} by eq 52, simplified by observing that the common noise term + // makes this a simple average. + Matrix centerSample = Matrix::Zero(); + // Define the sample differences z_k by eq 23 a) + double sampleDeltaMag[n_samples]; + // The center value \hbar{z} + double sampleDeltaMagCenter = 0; + // The squared norm of the reference vector + double refSquaredNorm = referenceField.squaredNorm(); - for (size_t i = 0; i < n_samples; ++i) { - fullSamples.row(i) << 2*samples[i].transpose().cast(), - -(samples[i][0]*samples[i][0]), - -(samples[i][1]*samples[i][1]), - -(samples[i][2]*samples[i][2]), - -2*(samples[i][0]*samples[i][1]), - -2*(samples[i][0]*samples[i][2]), - -2*(samples[i][1]*samples[i][2]); + for (size_t i = 0; i < n_samples; ++i) { + fullSamples.row(i) << 2 * samples[i].transpose().cast(), + -(samples[i][0] * samples[i][0]), + -(samples[i][1] * samples[i][1]), + -(samples[i][2] * samples[i][2]), + -2 * (samples[i][0] * samples[i][1]), + -2 * (samples[i][0] * samples[i][2]), + -2 * (samples[i][1] * samples[i][2]); - centerSample += fullSamples.row(i); + centerSample += fullSamples.row(i); - sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; - sampleDeltaMagCenter += sampleDeltaMag[i]; - } - sampleDeltaMagCenter /= n_samples; - centerSample /= n_samples; + sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; + sampleDeltaMagCenter += sampleDeltaMag[i]; + } + sampleDeltaMagCenter /= n_samples; + centerSample /= n_samples; - // Define \tilde{L}_k for k = 0 .. n_samples - Matrix centeredSamples(n_samples, 9); - // Define \tilde{z}_k for k = 0 .. n_samples - double centeredMags[n_samples]; - // Compute the term under the summation of eq 57a - Matrix estimateSummation = Matrix::Zero(); - for (size_t i = 0; i < n_samples; ++i) { - centeredSamples.row(i) = fullSamples.row(i) - centerSample; - centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; - estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose(); - } - estimateSummation /= noise; + // Define \tilde{L}_k for k = 0 .. n_samples + Matrix centeredSamples(n_samples, 9); + // Define \tilde{z}_k for k = 0 .. n_samples + double centeredMags[n_samples]; + // Compute the term under the summation of eq 57a + Matrix estimateSummation = Matrix::Zero(); + for (size_t i = 0; i < n_samples; ++i) { + centeredSamples.row(i) = fullSamples.row(i) - centerSample; + centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; + estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose(); + } + estimateSummation /= noise; - // By eq 57b - Matrix P_theta_theta_inv = (1.0f/noise)* - centeredSamples.transpose()*centeredSamples; + // By eq 57b + Matrix P_theta_theta_inv = (1.0f / noise) * + centeredSamples.transpose() * centeredSamples; #ifdef PRINTF_DEBUGGING - SelfAdjointEigenSolver > eig(P_theta_theta_inv); - std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n"; - std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose() - << "\n"; - std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n"; + SelfAdjointEigenSolver > eig(P_theta_theta_inv); + std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n"; + std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose() + << "\n"; + std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n"; #endif - // The current value of the estimate. Initialized to \tilde{\theta}^* - Matrix estimate; - // By eq 57a - P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate); + // The current value of the estimate. Initialized to \tilde{\theta}^* + Matrix estimate; + // By eq 57a + P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate); - // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradient(theta) - // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1} - size_t count = 0; - double eta = 10000; - while (count++ < 200 && eta > 1e-8) { - static bool warned = false; - if (hasNaN(estimate)) { - std::cout << "WARNING: found NaN at time " << count << "\n"; - warned = true; - } + // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradient(theta) + // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1} + size_t count = 0; + double eta = 10000; + while (count++ < 200 && eta > 1e-8) { + static bool warned = false; + if (hasNaN(estimate)) { + std::cout << "WARNING: found NaN at time " << count << "\n"; + warned = true; + } #if 0 - SelfAdjointEigenSolver eig_E(E_theta(estimate)); - Vector3d S = eig_E.eigenvalues(); - Vector3d W; W << -1 + sqrt(1 + S.coeff(0)), - -1 + sqrt(1 + S.coeff(1)), - -1 + sqrt(1 + S.coeff(2)); - scale = (eig_E.eigenvectors() * W.asDiagonal() * - eig_E.eigenvectors().transpose()) .cast(); + SelfAdjointEigenSolver eig_E(E_theta(estimate)); + Vector3d S = eig_E.eigenvalues(); + Vector3d W; W << -1 + sqrt(1 + S.coeff(0)), + -1 + sqrt(1 + S.coeff(1)), + -1 + sqrt(1 + S.coeff(2)); + scale = (eig_E.eigenvectors() * W.asDiagonal() * + eig_E.eigenvectors().transpose()).cast(); - (Matrix3f::Identity() + scale).ldlt().solve( - estimate.start<3>().cast(), &bias); - std::cout << "\n\nestimated bias: " << bias.transpose() - << "\nestimated scale:\n" << scale; + (Matrix3f::Identity() + scale).ldlt().solve( + estimate.start<3>().cast(), &bias); + std::cout << "\n\nestimated bias: " << bias.transpose() + << "\nestimated scale:\n" << scale; #endif - - Matrix db_dtheta = dnormb_dtheta(estimate); - Matrix dJ_dtheta = ::dJ_dtheta(centerSample, - sampleDeltaMagCenter, - estimate, - db_dtheta, - -3*noise, - noise/n_samples); + Matrix db_dtheta = dnormb_dtheta(estimate); - // Eq 59, with reused storage on db_dtheta - db_dtheta = centerSample - db_dtheta; - Matrix scale = P_theta_theta_inv + - (double(n_samples)/noise)*db_dtheta.transpose() * db_dtheta; + Matrix dJ_dtheta = ::dJ_dtheta(centerSample, + sampleDeltaMagCenter, + estimate, + db_dtheta, + -3 * noise, + noise / n_samples); - // eq 14b, mutatis mutandis (grumble, grumble) - Matrix increment; - scale.ldlt().solve(dJ_dtheta, &increment); - estimate -= increment; - eta = increment.dot(scale * increment); - std::cout << "eta: " << eta << "\n"; - } - std::cout << "terminated at eta = " << eta - << " after " << count << " iterations\n"; - - if (!isnan(eta) && !isinf(eta)) { - // Transform the estimated parameters from [c | E] back into [b | D]. - // See eq 63-65 - SelfAdjointEigenSolver eig_E(E_theta(estimate)); - Vector3d S = eig_E.eigenvalues(); - Vector3d W; W << -1 + sqrt(1 + S.coeff(0)), - -1 + sqrt(1 + S.coeff(1)), - -1 + sqrt(1 + S.coeff(2)); - scale = (eig_E.eigenvectors() * W.asDiagonal() * - eig_E.eigenvectors().transpose()) .cast(); + // Eq 59, with reused storage on db_dtheta + db_dtheta = centerSample - db_dtheta; + Matrix scale = P_theta_theta_inv + + (double(n_samples) / noise) * db_dtheta.transpose() * db_dtheta; - (Matrix3f::Identity() + scale).ldlt().solve( - estimate.start<3>().cast(), &bias); - } - else { - // return nonsense data. The eigensolver can fall ingo - // an infinite loop otherwise. - // TODO: Add error code return - scale = Matrix3f::Ones()*std::numeric_limits::quiet_NaN(); - bias = Vector3f::Zero(); - } + // eq 14b, mutatis mutandis (grumble, grumble) + Matrix increment; + scale.ldlt().solve(dJ_dtheta, &increment); + estimate -= increment; + eta = increment.dot(scale * increment); + std::cout << "eta: " << eta << "\n"; + } + std::cout << "terminated at eta = " << eta + << " after " << count << " iterations\n"; + + if (!isnan(eta) && !isinf(eta)) { + // Transform the estimated parameters from [c | E] back into [b | D]. + // See eq 63-65 + SelfAdjointEigenSolver eig_E(E_theta(estimate)); + Vector3d S = eig_E.eigenvalues(); + Vector3d W; W << -1 + sqrt(1 + S.coeff(0)), + -1 + sqrt(1 + S.coeff(1)), + -1 + sqrt(1 + S.coeff(2)); + scale = (eig_E.eigenvectors() * W.asDiagonal() * + eig_E.eigenvectors().transpose()).cast(); + + (Matrix3f::Identity() + scale).ldlt().solve( + estimate.start<3>().cast(), &bias); + } else { + // return nonsense data. The eigensolver can fall ingo + // an infinite loop otherwise. + // TODO: Add error code return + scale = Matrix3f::Ones() * std::numeric_limits::quiet_NaN(); + bias = Vector3f::Zero(); + } } - diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.cpp b/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.cpp index b3af24ace..645b9f547 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.cpp +++ b/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -29,8 +29,8 @@ #include "texteditloggerengine.h" ConsoleGadget::ConsoleGadget(QString classId, ConsoleGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) + IUAVGadget(classId, parent), + m_widget(widget) { m_logger = new TextEditLoggerEngine(widget); bool suitableName = false; @@ -38,8 +38,9 @@ ConsoleGadget::ConsoleGadget(QString classId, ConsoleGadgetWidget *widget, QWidg QString loggerName; while (!suitableName) { loggerName = "TextEditLogger" + QVariant(i).toString(); - if (!qxtLog->isLoggerEngine(loggerName)) + if (!qxtLog->isLoggerEngine(loggerName)) { suitableName = true; + } ++i; } qxtLog->addLoggerEngine(loggerName, m_logger); diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.h b/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.h index 50c572cf6..8495e4ef5 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.h +++ b/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,16 +38,24 @@ class TextEditLoggerEngine; using namespace Core; -class ConsoleGadget : public Core::IUAVGadget -{ +class ConsoleGadget : public Core::IUAVGadget { Q_OBJECT public: ConsoleGadget(QString classId, ConsoleGadgetWidget *widget, QWidget *parent = 0); ~ConsoleGadget(); - QList context() const { return m_context; } - QWidget *widget() { return m_widget; } - QString contextHelpId() const { return QString(); } + QList context() const + { + return m_context; + } + QWidget *widget() + { + return m_widget; + } + QString contextHelpId() const + { + return QString(); + } private: QWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.cpp b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.cpp index 1989c3e5e..89df10e3b 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,18 +30,17 @@ #include ConsoleGadgetFactory::ConsoleGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("ConsoleGadget"), - tr("Console"), - parent) -{ -} + IUAVGadgetFactory(QString("ConsoleGadget"), + tr("Console"), + parent) +{} ConsoleGadgetFactory::~ConsoleGadgetFactory() +{} + +IUAVGadget *ConsoleGadgetFactory::createGadget(QWidget *parent) { + ConsoleGadgetWidget *gadgetWidget = new ConsoleGadgetWidget(parent); -} - -IUAVGadget* ConsoleGadgetFactory::createGadget(QWidget *parent) { - ConsoleGadgetWidget* gadgetWidget = new ConsoleGadgetWidget(parent); return new ConsoleGadget(QString("ConsoleGadget"), gadgetWidget, parent); } diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.h b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.h index 183c8c17e..9d21946c1 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,8 +37,7 @@ class IUAVGadgetFactory; using namespace Core; -class ConsoleGadgetFactory : public IUAVGadgetFactory -{ +class ConsoleGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: ConsoleGadgetFactory(QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.cpp b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.cpp index 5267131a1..8218ed34e 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,13 +32,12 @@ ConsoleGadgetWidget::ConsoleGadgetWidget(QWidget *parent) : QTextEdit(parent) { - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setReadOnly(true); } ConsoleGadgetWidget::~ConsoleGadgetWidget() { - // Do nothing + // Do nothing } - diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.h b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.h index 65fe3d18a..f72665a47 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,13 +30,12 @@ #include -class ConsoleGadgetWidget : public QTextEdit -{ +class ConsoleGadgetWidget : public QTextEdit { Q_OBJECT public: ConsoleGadgetWidget(QWidget *parent = 0); - ~ConsoleGadgetWidget(); + ~ConsoleGadgetWidget(); private: }; diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.cpp b/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.cpp index 5461b90a8..7e1806cb5 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.cpp +++ b/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,32 +34,31 @@ ConsolePlugin::ConsolePlugin() { - // Do nothing + // Do nothing } ConsolePlugin::~ConsolePlugin() { - // Do nothing + // Do nothing } -bool ConsolePlugin::initialize(const QStringList& args, QString *errMsg) +bool ConsolePlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new ConsoleGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new ConsoleGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void ConsolePlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void ConsolePlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(ConsolePlugin) - diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.h b/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.h index 243de47d0..5e859d0da 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.h +++ b/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class ConsoleGadgetFactory; -class ConsolePlugin : public ExtensionSystem::IPlugin -{ +class ConsolePlugin : public ExtensionSystem::IPlugin { public: - ConsolePlugin(); - ~ConsolePlugin(); + ConsolePlugin(); + ~ConsolePlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - ConsoleGadgetFactory *mf; + ConsoleGadgetFactory *mf; }; #endif /* CONSOLEPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.cpp b/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.cpp index 136260557..df9acadeb 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.cpp +++ b/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.cpp @@ -8,21 +8,21 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -45,17 +45,16 @@ TextEditLoggerEngine::TextEditLoggerEngine(QTextEdit *textEdit) : m_textEdit(tex } TextEditLoggerEngine::~TextEditLoggerEngine() -{ -} +{} void TextEditLoggerEngine::initLoggerEngine() { - return; // Should work out of the box! + // Should work out of the box! } void TextEditLoggerEngine::killLoggerEngine() { - return; // I do nothing. + // I do nothing. } bool TextEditLoggerEngine::isInitialized() const @@ -66,13 +65,15 @@ bool TextEditLoggerEngine::isInitialized() const void TextEditLoggerEngine::setLogLevelEnabled(QxtLogger::LogLevels level, bool enable) { QxtLoggerEngine::setLogLevelsEnabled(level | QXT_REQUIRED_LEVELS, enable); - if (!enable) QxtLoggerEngine::setLogLevelsEnabled(QXT_REQUIRED_LEVELS); + + if (!enable) { + QxtLoggerEngine::setLogLevelsEnabled(QXT_REQUIRED_LEVELS); + } } void TextEditLoggerEngine::writeFormatted(QxtLogger::LogLevel level, const QList &msgs) { - switch (level) - { + switch (level) { case QxtLogger::ErrorLevel: writeToTextEdit("Error", msgs, Qt::red); break; @@ -100,29 +101,31 @@ void TextEditLoggerEngine::writeFormatted(QxtLogger::LogLevel level, const QList } } -void TextEditLoggerEngine::writeToTextEdit(const QString& level, const QList &msgs, QColor color) +void TextEditLoggerEngine::writeToTextEdit(const QString & level, const QList &msgs, QColor color) { /* Message format... [time] [error level] First message..... second message third message - */ - if (msgs.isEmpty()) + */ + if (msgs.isEmpty()) { return; + } QScrollBar *sb = m_textEdit->verticalScrollBar(); - bool scroll = sb->value() == sb->maximum(); + bool scroll = sb->value() == sb->maximum(); QString header = '[' + QTime::currentTime().toString("hh:mm:ss.zzz") + "] [" + level + "] "; QString padding; QString appendText; appendText.append(header); - for (int i = 0; i < header.size(); i++) padding.append(' '); + for (int i = 0; i < header.size(); i++) { + padding.append(' '); + } int count = 0; - Q_FOREACH(const QVariant& out, msgs) - { - if (!out.isNull()) - { - if (count != 0) + Q_FOREACH(const QVariant &out, msgs) { + if (!out.isNull()) { + if (count != 0) { appendText.append(padding); + } appendText.append(out.toString()); } count++; @@ -130,6 +133,7 @@ void TextEditLoggerEngine::writeToTextEdit(const QString& level, const QList%2").arg(color.name()).arg(appendText); m_textEdit->append(appendText); - if (scroll) + if (scroll) { sb->setValue(sb->maximum()); + } } diff --git a/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.h b/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.h index 6f4b4d22a..6ba73b479 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.h +++ b/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.h @@ -8,21 +8,21 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include class QTextEdit; -class TextEditLoggerEngine : public QxtLoggerEngine -{ - +class TextEditLoggerEngine : public QxtLoggerEngine { public: TextEditLoggerEngine(QTextEdit *textEdit); ~TextEditLoggerEngine(); @@ -50,7 +48,7 @@ public: bool isInitialized() const; private: - virtual void writeToTextEdit(const QString& str_level, const QList &msgs, QColor color = QColor(0,0,0)); + virtual void writeToTextEdit(const QString & str_level, const QList &msgs, QColor color = QColor(0, 0, 0)); QTextEdit *m_textEdit; }; diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.cpp b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.cpp index 0755db0fa..bf8742d75 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,7 +38,7 @@ #include #include -Q_DECLARE_METATYPE(Core::Internal::MenuActionContainer*) +Q_DECLARE_METATYPE(Core::Internal::MenuActionContainer *) using namespace Core; using namespace Core::Internal; @@ -64,7 +64,7 @@ using namespace Core::Internal; You can define if the menu represented by this action container should automatically disable or hide whenever it only contains disabled items and submenus by setting the corresponding \l{ActionContainer::setEmptyAction()}{EmptyAction}. -*/ + */ /*! \enum ActionContainer::EmptyAction @@ -76,37 +76,37 @@ using namespace Core::Internal; The menu will be visible but disabled. \value EA_Hide The menu will not be visible until the state of the subitems change. -*/ + */ /*! \fn ActionContainer::setEmptyAction(EmptyAction disableOrHide) Defines if the menu represented by this action container should automatically \a disableOrHide whenever it only contains disabled items and submenus. \sa ActionContainer::EmptyAction -*/ + */ /*! \fn int ActionContainer::id() const \internal -*/ + */ /*! \fn QMenu *ActionContainer::menu() const Returns the QMenu instance that is represented by this action container, or 0 if this action container represents a menu bar. -*/ + */ /*! \fn QMenuBar *ActionContainer::menuBar() const Returns the QMenuBar instance that is represented by this action container, or 0 if this action container represents a menu. -*/ + */ /*! \fn QAction *ActionContainer::insertLocation(const QString &group) const Returns an action representing the \a group, that could be used with \c{QWidget::insertAction}. -*/ + */ /*! \fn void ActionContainer::appendGroup(const QString &identifier) @@ -115,7 +115,7 @@ using namespace Core::Internal; menus directly to these parts. \sa addAction() \sa addMenu() -*/ + */ /*! \fn void ActionContainer::addAction(Core::Command *action, const QString &group) @@ -123,7 +123,7 @@ using namespace Core::Internal; last item of the specified \a group. \sa appendGroup() \sa addMenu() -*/ + */ /*! \fn void ActionContainer::addMenu(Core::ActionContainer *menu, const QString &group) @@ -131,30 +131,28 @@ using namespace Core::Internal; last item of the specified \a group. \sa appendGroup() \sa addAction() -*/ + */ /*! \fn bool ActionContainer::update() \internal -*/ + */ /*! \fn ActionContainer::~ActionContainer() \internal -*/ + */ // ---------- ActionContainerPrivate ------------ /*! \class Core::Internal::ActionContainerPrivate \internal -*/ + */ ActionContainerPrivate::ActionContainerPrivate(int id) : m_data(0), m_id(id) -{ - -} +{} void ActionContainerPrivate::setEmptyAction(EmptyAction ea) { @@ -169,29 +167,34 @@ bool ActionContainerPrivate::hasEmptyAction(EmptyAction ea) const void ActionContainerPrivate::appendGroup(const QString &group) { int gid = UniqueIDManager::instance()->uniqueIdentifier(group); + m_groups << gid; } QAction *ActionContainerPrivate::insertLocation(const QString &group) const { - int grpid = UniqueIDManager::instance()->uniqueIdentifier(group); + int grpid = UniqueIDManager::instance()->uniqueIdentifier(group); int prevKey = 0; - int pos = ((grpid << 16) | 0xFFFF); + int pos = ((grpid << 16) | 0xFFFF); + return beforeAction(pos, &prevKey); } void ActionContainerPrivate::addAction(Command *action, const QString &group) { - if (!canAddAction(action)) + if (!canAddAction(action)) { return; + } - ActionManagerPrivate *am = ActionManagerPrivate::instance(); + ActionManagerPrivate *am = ActionManagerPrivate::instance(); UniqueIDManager *idmanager = UniqueIDManager::instance(); int grpid = idmanager->uniqueIdentifier(Constants::G_DEFAULT_TWO); - if (!group.isEmpty()) + if (!group.isEmpty()) { grpid = idmanager->uniqueIdentifier(group); - if (!m_groups.contains(grpid) && !am->defaultGroups().contains(grpid)) + } + if (!m_groups.contains(grpid) && !am->defaultGroups().contains(grpid)) { qWarning() << "*** addAction(): Unknown group: " << group; + } int pos = ((grpid << 16) | 0xFFFF); addAction(action, pos, true); } @@ -199,16 +202,20 @@ void ActionContainerPrivate::addAction(Command *action, const QString &group) void ActionContainerPrivate::addMenu(ActionContainer *menu, const QString &group) { ActionContainerPrivate *container = static_cast(menu); - if (!container->canBeAddedToMenu()) - return; - ActionManagerPrivate *am = ActionManagerPrivate::instance(); + if (!container->canBeAddedToMenu()) { + return; + } + + ActionManagerPrivate *am = ActionManagerPrivate::instance(); UniqueIDManager *idmanager = UniqueIDManager::instance(); int grpid = idmanager->uniqueIdentifier(Constants::G_DEFAULT_TWO); - if (!group.isEmpty()) + if (!group.isEmpty()) { grpid = idmanager->uniqueIdentifier(group); - if (!m_groups.contains(grpid) && !am->defaultGroups().contains(grpid)) + } + if (!m_groups.contains(grpid) && !am->defaultGroups().contains(grpid)) { qWarning() << "*** addMenu(): Unknown group: " << group; + } int pos = ((grpid << 16) | 0xFFFF); addMenu(menu, pos, true); } @@ -230,12 +237,12 @@ QMenuBar *ActionContainerPrivate::menuBar() const bool ActionContainerPrivate::canAddAction(Command *action) const { - return (action->action() != 0); + return action->action() != 0; } void ActionContainerPrivate::addAction(Command *action, int pos, bool setpos) { - Action *a = static_cast(action); + Action *a = static_cast(action); int prevKey = 0; QAction *ba = beforeAction(pos, &prevKey); @@ -244,7 +251,7 @@ void ActionContainerPrivate::addAction(Command *action, int pos, bool setpos) pos = calcPosition(pos, prevKey); CommandLocation loc; loc.m_container = m_id; - loc.m_position = pos; + loc.m_position = pos; QList locs = a->locations(); locs.append(loc); a->setLocations(locs); @@ -266,7 +273,7 @@ void ActionContainerPrivate::addMenu(ActionContainer *menu, int pos, bool setpos pos = calcPosition(pos, prevKey); CommandLocation loc; loc.m_container = m_id; - loc.m_position = pos; + loc.m_position = pos; mc->setLocation(loc); } @@ -293,14 +300,18 @@ QAction *ActionContainerPrivate::beforeAction(int pos, int *prevKey) const ++i; } - if (baId == -1) + if (baId == -1) { return 0; + } - if (Command *cmd = am->command(baId)) + if (Command * cmd = am->command(baId)) { return cmd->action(); - if (ActionContainer *container = am->actionContainer(baId)) - if (QMenu *menu = container->menu()) + } + if (ActionContainer * container = am->actionContainer(baId)) { + if (QMenu * menu = container->menu()) { return menu->menuAction(); + } + } return 0; } @@ -308,13 +319,16 @@ QAction *ActionContainerPrivate::beforeAction(int pos, int *prevKey) const int ActionContainerPrivate::calcPosition(int pos, int prevKey) const { int grp = (pos & 0xFFFF0000); - if (prevKey == -1) + + if (prevKey == -1) { return grp; + } int prevgrp = (prevKey & 0xFFFF0000); - if (grp != prevgrp) + if (grp != prevgrp) { return grp; + } return grp + (prevKey & 0xFFFF) + 10; } @@ -324,7 +338,7 @@ int ActionContainerPrivate::calcPosition(int pos, int prevKey) const /*! \class Core::Internal::MenuActionContainer \internal -*/ + */ MenuActionContainer::MenuActionContainer(int id) : ActionContainerPrivate(id), m_menu(0) @@ -337,7 +351,7 @@ void MenuActionContainer::setMenu(QMenu *menu) m_menu = menu; QVariant v; - qVariantSetValue(v, this); + qVariantSetValue(v, this); m_menu->menuAction()->setData(v); } @@ -369,14 +383,15 @@ CommandLocation MenuActionContainer::location() const bool MenuActionContainer::update() { - if (hasEmptyAction(EA_None)) + if (hasEmptyAction(EA_None)) { return true; + } bool hasitems = false; - foreach (ActionContainer *container, subContainers()) { + foreach(ActionContainer * container, subContainers()) { if (container == this) { - qWarning() << Q_FUNC_INFO << "container" << (this->menu() ? this->menu()->title() : "") << "contains itself as subcontainer"; + qWarning() << Q_FUNC_INFO << "container" << (this->menu() ? this->menu()->title() : "") << "contains itself as subcontainer"; continue; } if (container->update()) { @@ -385,7 +400,7 @@ bool MenuActionContainer::update() } } if (!hasitems) { - foreach (Command *command, commands()) { + foreach(Command * command, commands()) { if (command->isActive()) { hasitems = true; break; @@ -393,10 +408,11 @@ bool MenuActionContainer::update() } } - if (hasEmptyAction(EA_Hide)) + if (hasEmptyAction(EA_Hide)) { m_menu->setVisible(hasitems); - else if (hasEmptyAction(EA_Disable)) + } else if (hasEmptyAction(EA_Disable)) { m_menu->setEnabled(hasitems); + } return hasitems; } @@ -412,7 +428,7 @@ bool MenuActionContainer::canBeAddedToMenu() const /*! \class Core::Internal::MenuBarActionContainer \internal -*/ + */ MenuBarActionContainer::MenuBarActionContainer(int id) : ActionContainerPrivate(id), m_menuBar(0) @@ -442,22 +458,24 @@ void MenuBarActionContainer::insertMenu(QAction *before, QMenu *menu) bool MenuBarActionContainer::update() { - if (hasEmptyAction(EA_None)) + if (hasEmptyAction(EA_None)) { return true; + } bool hasitems = false; QList actions = m_menuBar->actions(); - for (int i=0; iisVisible()) { hasitems = true; break; } } - if (hasEmptyAction(EA_Hide)) + if (hasEmptyAction(EA_Hide)) { m_menuBar->setVisible(hasitems); - else if (hasEmptyAction(EA_Disable)) + } else if (hasEmptyAction(EA_Disable)) { m_menuBar->setEnabled(hasitems); + } return hasitems; } @@ -466,4 +484,3 @@ bool MenuBarActionContainer::canBeAddedToMenu() const { return false; } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.h b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.h index a8e20272f..c29f269af 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,17 +35,15 @@ #include namespace Core { - class Command; -class ActionContainer : public QObject -{ +class ActionContainer : public QObject { public: enum EmptyAction { - EA_Mask = 0xFF00, - EA_None = 0x0100, - EA_Hide = 0x0200, - EA_Disable = 0x0300 + EA_Mask = 0xFF00, + EA_None = 0x0100, + EA_Hide = 0x0200, + EA_Disable = 0x0300 }; virtual void setEmptyAction(EmptyAction ea) = 0; @@ -57,13 +55,12 @@ public: virtual QAction *insertLocation(const QString &group) const = 0; virtual void appendGroup(const QString &group) = 0; - virtual void addAction(Core::Command *action, const QString &group = QString()) = 0; + virtual void addAction(Core::Command *action, const QString &group = QString()) = 0; virtual void addMenu(Core::ActionContainer *menu, const QString &group = QString()) = 0; virtual bool update() = 0; virtual ~ActionContainer() {} }; - } // namespace Core #endif // ACTIONCONTAINER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer_p.h b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer_p.h index 47cb6f83d..cb906269e 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer_p.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer_p.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,9 +36,7 @@ namespace Core { namespace Internal { - -class ActionContainerPrivate : public Core::ActionContainer -{ +class ActionContainerPrivate : public Core::ActionContainer { public: ActionContainerPrivate(int id); virtual ~ActionContainerPrivate() {} @@ -59,8 +57,14 @@ public: virtual void insertAction(QAction *before, QAction *action) = 0; virtual void insertMenu(QAction *before, QMenu *menu) = 0; - QList commands() const { return m_commands; } - QList subContainers() const { return m_subContainers; } + QList commands() const + { + return m_commands; + } + QList subContainers() const + { + return m_subContainers; + } protected: bool canAddAction(Command *action) const; bool canAddMenu(ActionContainer *menu) const; @@ -81,8 +85,7 @@ private: QList m_commands; }; -class MenuActionContainer : public ActionContainerPrivate -{ +class MenuActionContainer : public ActionContainerPrivate { public: MenuActionContainer(int id); @@ -103,8 +106,7 @@ private: CommandLocation m_location; }; -class MenuBarActionContainer : public ActionContainerPrivate -{ +class MenuBarActionContainer : public ActionContainerPrivate { public: MenuBarActionContainer(int id); @@ -120,7 +122,6 @@ protected: private: QMenuBar *m_menuBar; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.cpp index 844780df5..a86a70f0c 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,7 +42,7 @@ #include namespace { - enum { warnAboutFindFailures = 0 }; +enum { warnAboutFindFailures = 0 }; } /*! @@ -136,7 +136,7 @@ namespace { \sa Core::Command \sa Core::ActionContainer \sa Core::IContext -*/ + */ /*! \fn ActionContainer *ActionManager::createMenu(const QString &id) @@ -147,7 +147,7 @@ namespace { the returned ActionContainer. Add your menu to some other menu or a menu bar via the ActionManager::actionContainer and ActionContainer::addMenu methods. -*/ + */ /*! \fn ActionContainer *ActionManager::createMenuBar(const QString &id) @@ -156,7 +156,7 @@ namespace { Returns a new ActionContainer that you can use to get the QMenuBar instance or to add menus to the menu bar. The ActionManager owns the returned ActionContainer. -*/ + */ /*! \fn Command *ActionManager::registerAction(QAction *action, const QString &id, const QList &context) @@ -167,7 +167,7 @@ namespace { same \a id as long as the \a context is different. In this case a trigger of the actual action is forwarded to the registered QAction for the currently active context. -*/ + */ /*! \fn Command *ActionManager::registerShortcut(QShortcut *shortcut, const QString &id, const QList &context) @@ -178,7 +178,7 @@ namespace { same \a id as long as the \a context is different. In this case a trigger of the actual shortcut is forwarded to the registered QShortcut for the currently active context. -*/ + */ /*! \fn Command *ActionManager::command(const QString &id) const @@ -186,7 +186,7 @@ namespace { under the given string \a id. \sa ActionManager::registerAction() -*/ + */ /*! \fn ActionContainer *ActionManager::actionContainer(const QString &id) const @@ -195,37 +195,37 @@ namespace { \sa ActionManager::createMenu() \sa ActionManager::createMenuBar() -*/ + */ /*! \fn ActionManager::ActionManager(QObject *parent) \internal -*/ + */ /*! \fn ActionManager::~ActionManager() \internal -*/ + */ using namespace Core; using namespace Core::Internal; -ActionManagerPrivate* ActionManagerPrivate::m_instance = 0; +ActionManagerPrivate *ActionManagerPrivate::m_instance = 0; /*! \class ActionManagerPrivate \inheaderfile actionmanager_p.h \internal -*/ + */ ActionManagerPrivate::ActionManagerPrivate(MainWindow *mainWnd) - : ActionManager(mainWnd), + : ActionManager(mainWnd), m_mainWnd(mainWnd) { UniqueIDManager *uidmgr = UniqueIDManager::instance(); + m_defaultGroups << uidmgr->uniqueIdentifier(Constants::G_DEFAULT_ONE); m_defaultGroups << uidmgr->uniqueIdentifier(Constants::G_DEFAULT_TWO); m_defaultGroups << uidmgr->uniqueIdentifier(Constants::G_DEFAULT_THREE); m_instance = this; - } ActionManagerPrivate::~ActionManagerPrivate() @@ -266,19 +266,22 @@ void ActionManagerPrivate::setContext(const QList &context) // and only update commands that are either in old or new contexts m_context = context; const IdCmdMap::const_iterator cmdcend = m_idCmdMap.constEnd(); - for (IdCmdMap::const_iterator it = m_idCmdMap.constBegin(); it != cmdcend; ++it) + for (IdCmdMap::const_iterator it = m_idCmdMap.constBegin(); it != cmdcend; ++it) { it.value()->setCurrentContext(m_context); + } const IdContainerMap::const_iterator acend = m_idContainerMap.constEnd(); - for (IdContainerMap::const_iterator it = m_idContainerMap.constBegin(); it != acend; ++it) + for (IdContainerMap::const_iterator it = m_idContainerMap.constBegin(); it != acend; ++it) { it.value()->update(); + } } bool ActionManagerPrivate::hasContext(QList context) const { - for (int i=0; iuniqueIdentifier(id); const IdContainerMap::const_iterator it = m_idContainerMap.constFind(uid); - if (it != m_idContainerMap.constEnd()) + + if (it != m_idContainerMap.constEnd()) { return it.value(); + } QMenu *m = new QMenu(m_mainWnd); m->setObjectName(id); @@ -305,8 +310,10 @@ ActionContainer *ActionManagerPrivate::createMenuBar(const QString &id) { const int uid = UniqueIDManager::instance()->uniqueIdentifier(id); const IdContainerMap::const_iterator it = m_idContainerMap.constFind(uid); - if (it != m_idContainerMap.constEnd()) + + if (it != m_idContainerMap.constEnd()) { return it.value(); + } QMenuBar *mb = new QMenuBar; // No parent (System menu bar on Mac OS X) mb->setObjectName(id); @@ -323,9 +330,11 @@ Command *ActionManagerPrivate::registerAction(QAction *action, const QString &id { OverrideableAction *a = 0; Command *c = registerOverridableAction(action, id, false); + a = static_cast(c); - if (a) + if (a) { a->addOverrideAction(action, context); + } return a; } @@ -333,7 +342,8 @@ Command *ActionManagerPrivate::registerOverridableAction(QAction *action, const { OverrideableAction *a = 0; const int uid = UniqueIDManager::instance()->uniqueIdentifier(id); - if (CommandPrivate *c = m_idCmdMap.value(uid, 0)) { + + if (CommandPrivate * c = m_idCmdMap.value(uid, 0)) { a = qobject_cast(c); if (!a) { qWarning() << "registerAction: id" << id << "is registered with a different command type."; @@ -366,7 +376,7 @@ Command *ActionManagerPrivate::registerOverridableAction(QAction *action, const m_mainWnd->addAction(baseAction); a->setKeySequence(a->keySequence()); a->setDefaultKeySequence(QKeySequence()); - } else if (checkUnique) { + } else if (checkUnique) { qWarning() << "registerOverridableAction: id" << id << "is already registered."; } @@ -377,7 +387,8 @@ Command *ActionManagerPrivate::registerShortcut(QShortcut *shortcut, const QStri { Shortcut *sc = 0; int uid = UniqueIDManager::instance()->uniqueIdentifier(id); - if (CommandPrivate *c = m_idCmdMap.value(uid, 0)) { + + if (CommandPrivate * c = m_idCmdMap.value(uid, 0)) { sc = qobject_cast(c); if (!sc) { qWarning() << "registerShortcut: id" << id << "is registered with a different command type."; @@ -393,16 +404,18 @@ Command *ActionManagerPrivate::registerShortcut(QShortcut *shortcut, const QStri return sc; } - if (!hasContext(context)) + if (!hasContext(context)) { shortcut->setEnabled(false); + } shortcut->setObjectName(id); shortcut->setParent(m_mainWnd); sc->setShortcut(shortcut); - if (context.isEmpty()) + if (context.isEmpty()) { sc->setContext(QList() << 0); - else + } else { sc->setContext(context); + } sc->setKeySequence(shortcut->key()); sc->setDefaultKeySequence(QKeySequence()); @@ -414,9 +427,11 @@ Command *ActionManagerPrivate::command(const QString &id) const { const int uid = UniqueIDManager::instance()->uniqueIdentifier(id); const IdCmdMap::const_iterator it = m_idCmdMap.constFind(uid); + if (it == m_idCmdMap.constEnd()) { - if (warnAboutFindFailures) + if (warnAboutFindFailures) { qWarning() << "ActionManagerPrivate::command(): failed to find :" << id << '/' << uid; + } return 0; } return it.value(); @@ -426,9 +441,11 @@ ActionContainer *ActionManagerPrivate::actionContainer(const QString &id) const { const int uid = UniqueIDManager::instance()->uniqueIdentifier(id); const IdContainerMap::const_iterator it = m_idContainerMap.constFind(uid); + if (it == m_idContainerMap.constEnd()) { - if (warnAboutFindFailures) + if (warnAboutFindFailures) { qWarning() << "ActionManagerPrivate::actionContainer(): failed to find :" << id << '/' << uid; + } return 0; } return it.value(); @@ -437,9 +454,11 @@ ActionContainer *ActionManagerPrivate::actionContainer(const QString &id) const Command *ActionManagerPrivate::command(int uid) const { const IdCmdMap::const_iterator it = m_idCmdMap.constFind(uid); + if (it == m_idCmdMap.constEnd()) { - if (warnAboutFindFailures) - qWarning() << "ActionManagerPrivate::command(): failed to find :" << UniqueIDManager::instance()->stringForUniqueIdentifier(uid) << '/' << uid; + if (warnAboutFindFailures) { + qWarning() << "ActionManagerPrivate::command(): failed to find :" << UniqueIDManager::instance()->stringForUniqueIdentifier(uid) << '/' << uid; + } return 0; } return it.value(); @@ -448,9 +467,11 @@ Command *ActionManagerPrivate::command(int uid) const ActionContainer *ActionManagerPrivate::actionContainer(int uid) const { const IdContainerMap::const_iterator it = m_idContainerMap.constFind(uid); + if (it == m_idContainerMap.constEnd()) { - if (warnAboutFindFailures) + if (warnAboutFindFailures) { qWarning() << "ActionManagerPrivate::actionContainer(): failed to find :" << UniqueIDManager::instance()->stringForUniqueIdentifier(uid) << uid; + } return 0; } return it.value(); @@ -458,20 +479,22 @@ ActionContainer *ActionManagerPrivate::actionContainer(int uid) const static const char *settingsGroup = "KeyBindings"; static const char *idKey = "ID"; -static const char *sequenceKey = "Keysequence"; +static const char *sequenceKey = "Keysequence"; void ActionManagerPrivate::readSettings(QSettings *settings) { const int shortcuts = settings->beginReadArray(QLatin1String(settingsGroup)); - for (int i=0; isetArrayIndex(i); const QString sid = settings->value(QLatin1String(idKey)).toString(); const QKeySequence key(settings->value(QLatin1String(sequenceKey)).toString()); const int id = UniqueIDManager::instance()->uniqueIdentifier(sid); Command *cmd = command(id); - if (cmd) + if (cmd) { cmd->setKeySequence(key); + } } settings->endArray(); } @@ -483,9 +506,9 @@ void ActionManagerPrivate::saveSettings(QSettings *settings) const IdCmdMap::const_iterator cmdcend = m_idCmdMap.constEnd(); for (IdCmdMap::const_iterator j = m_idCmdMap.constBegin(); j != cmdcend; ++j) { - const int id = j.key(); + const int id = j.key(); CommandPrivate *cmd = j.value(); - QKeySequence key = cmd->keySequence(); + QKeySequence key = cmd->keySequence(); if (key != cmd->defaultKeySequence()) { const QString sid = UniqueIDManager::instance()->stringForUniqueIdentifier(id); settings->setArrayIndex(count); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.h index eae782693..57350188d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,15 +44,13 @@ class QString; QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT ActionManager : public QObject -{ +class CORE_EXPORT ActionManager : public QObject { Q_OBJECT public: ActionManager(QObject *parent = 0) : QObject(parent) {} virtual ~ActionManager() {} - virtual ActionContainer *createMenu(const QString &id) = 0; + virtual ActionContainer *createMenu(const QString &id) = 0; virtual ActionContainer *createMenuBar(const QString &id) = 0; virtual Command *registerAction(QAction *action, const QString &id, const QList &context) = 0; @@ -61,7 +59,6 @@ public: virtual Command *command(const QString &id) const = 0; virtual ActionContainer *actionContainer(const QString &id) const = 0; }; - } // namespace Core #endif // ACTIONMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager_p.h b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager_p.h index 6573f7031..2c2f440bf 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager_p.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager_p.h @@ -10,19 +10,19 @@ * @{ * @brief The Core GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,24 +39,20 @@ QT_BEGIN_NAMESPACE class QSettings; QT_END_NAMESPACE -struct CommandLocation -{ +struct CommandLocation { int m_container; int m_position; }; namespace Core { - class UniqueIDManager; namespace Internal { - class ActionContainerPrivate; class MainWindow; class CommandPrivate; -class ActionManagerPrivate : public Core::ActionManager -{ +class ActionManagerPrivate : public Core::ActionManager { Q_OBJECT public: @@ -79,14 +75,14 @@ public: void readSettings(QSettings *settings); - //ActionManager Interface + // ActionManager Interface ActionContainer *createMenu(const QString &id); ActionContainer *createMenuBar(const QString &id); Command *registerAction(QAction *action, const QString &id, - const QList &context); + const QList &context); Command *registerShortcut(QShortcut *shortcut, const QString &id, - const QList &context); + const QList &context); Core::Command *command(const QString &id) const; Core::ActionContainer *actionContainer(const QString &id) const; @@ -94,9 +90,9 @@ public: private: bool hasContext(QList context) const; Command *registerOverridableAction(QAction *action, const QString &id, - bool checkUnique); + bool checkUnique); - static ActionManagerPrivate* m_instance; + static ActionManagerPrivate *m_instance; QList m_defaultGroups; typedef QHash IdCmdMap; @@ -105,14 +101,13 @@ private: typedef QHash IdContainerMap; IdContainerMap m_idContainerMap; -// typedef QMap GlobalGroupMap; -// GlobalGroupMap m_globalgroups; +// typedef QMap GlobalGroupMap; +// GlobalGroupMap m_globalgroups; // QList m_context; MainWindow *m_mainWnd; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.cpp b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.cpp index 9f0c84c2d..de5379423 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -56,7 +56,7 @@ If there is no active action, the default behavior of the visible action is to be disabled. You can change that behavior to make the visible action hide instead via the Command's \l{Command::CommandAttribute}{attributes}. -*/ + */ /*! \enum Command::CommandAttribute @@ -74,37 +74,37 @@ \value CA_NonConfigureable Flag to indicate that the keyboard shortcut of this Command should not be configurable by the user. -*/ + */ /*! \fn void Command::setDefaultKeySequence(const QKeySequence &key) Set the default keyboard shortcut that can be used to activate this command to \a key. This is used if the user didn't customize the shortcut, or resets the shortcut to the default one. -*/ + */ /*! \fn void Command::defaultKeySequence() const Returns the default keyboard shortcut that can be used to activate this command. \sa setDefaultKeySequence() -*/ + */ /*! \fn void Command::keySequenceChanged() Sent when the keyboard shortcut assigned to this Command changes, e.g. when the user sets it in the keyboard shortcut settings dialog. -*/ + */ /*! \fn QKeySequence Command::keySequence() const Returns the current keyboard shortcut assigned to this Command. \sa defaultKeySequence() -*/ + */ /*! \fn void Command::setKeySequence(const QKeySequence &key) \internal -*/ + */ /*! \fn void Command::setDefaultText(const QString &text) @@ -112,24 +112,24 @@ keyboard shortcut settings dialog. If you don't set this, the current text from the user visible action is taken (which is ok in many cases). -*/ + */ /*! \fn QString Command::defaultText() const Returns the text that is used to present this Command to the user. \sa setDefaultText() -*/ + */ /*! \fn int Command::id() const \internal -*/ + */ /*! \fn QString Command::stringWithAppendedShortcut(const QString &string) const Returns the \a string with an appended representation of the keyboard shortcut that is currently assigned to this Command. -*/ + */ /*! \fn QAction *Command::action() const @@ -142,13 +142,13 @@ tool tip (in addition to the tool tip of the active action) and gets disabled/hidden when there is no active action for the current context. -*/ + */ /*! \fn QShortcut *Command::shortcut() const Returns the shortcut for this Command. If the Command represents an action, it returns null. -*/ + */ /*! \fn void Command::setAttribute(CommandAttribute attribute) @@ -156,14 +156,14 @@ \sa CommandAttribute \sa removeAttribute() \sa hasAttribute() -*/ + */ /*! \fn void Command::removeAttribute(CommandAttribute attribute) Remove the \a attribute from the attributes of this Command. \sa CommandAttribute \sa setAttribute() -*/ + */ /*! \fn bool Command::hasAttribute(CommandAttribute attribute) const @@ -171,30 +171,29 @@ \sa CommandAttribute \sa removeAttribute() \sa setAttribute() -*/ + */ /*! \fn bool Command::isActive() const Returns if the Command has an active action/shortcut for the current context. -*/ + */ /*! \fn Command::~Command() \internal -*/ + */ using namespace Core::Internal; /*! \class CommandPrivate \internal -*/ + */ CommandPrivate::CommandPrivate(int id) : m_attributes(0), m_id(id) -{ -} +{} void CommandPrivate::setDefaultKeySequence(const QKeySequence &key) { @@ -243,13 +242,13 @@ void CommandPrivate::removeAttribute(CommandAttribute attr) bool CommandPrivate::hasAttribute(CommandAttribute attr) const { - return (m_attributes & attr); + return m_attributes & attr; } QString CommandPrivate::stringWithAppendedShortcut(const QString &str) const { return QString("%1 %2").arg(str).arg( - keySequence().toString(QKeySequence::NativeText)); + keySequence().toString(QKeySequence::NativeText)); } // ---------- Shortcut ------------ @@ -257,18 +256,17 @@ QString CommandPrivate::stringWithAppendedShortcut(const QString &str) const /*! \class Shortcut \internal -*/ + */ Shortcut::Shortcut(int id) : CommandPrivate(id), m_shortcut(0) -{ - -} +{} QString Shortcut::name() const { - if (!m_shortcut) + if (!m_shortcut) { return QString(); + } return m_shortcut->whatsThis(); } @@ -295,8 +293,9 @@ QList Shortcut::context() const void Shortcut::setDefaultKeySequence(const QKeySequence &key) { - if (m_shortcut->key().isEmpty()) + if (m_shortcut->key().isEmpty()) { setKeySequence(key); + } CommandPrivate::setDefaultKeySequence(key); } @@ -323,7 +322,7 @@ QString Shortcut::defaultText() const bool Shortcut::setCurrentContext(const QList &context) { - foreach (int ctxt, m_context) { + foreach(int ctxt, m_context) { if (context.contains(ctxt)) { m_shortcut->setEnabled(true); return true; @@ -341,19 +340,18 @@ bool Shortcut::isActive() const // ---------- Action ------------ /*! - \class Action - \internal -*/ + \class Action + \internal + */ Action::Action(int id) : CommandPrivate(id), m_action(0) -{ - -} +{} QString Action::name() const { - if (!m_action) + if (!m_action) { return QString(); + } return m_action->text(); } @@ -384,8 +382,9 @@ QList Action::locations() const void Action::setDefaultKeySequence(const QKeySequence &key) { - if (m_action->shortcut().isEmpty()) + if (m_action->shortcut().isEmpty()) { setKeySequence(key); + } CommandPrivate::setDefaultKeySequence(key); } @@ -398,10 +397,11 @@ void Action::setKeySequence(const QKeySequence &key) void Action::updateToolTipWithKeySequence() { - if (m_action->shortcut().isEmpty()) + if (m_action->shortcut().isEmpty()) { m_action->setToolTip(m_toolTip); - else + } else { m_action->setToolTip(stringWithAppendedShortcut(m_toolTip)); + } } QKeySequence Action::keySequence() const @@ -414,13 +414,12 @@ QKeySequence Action::keySequence() const /*! \class OverrideableAction \internal -*/ + */ OverrideableAction::OverrideableAction(int id) : Action(id), m_currentAction(0), m_active(false), m_contextInitialized(false) -{ -} +{} void OverrideableAction::setAction(QAction *action) { @@ -434,14 +433,15 @@ bool OverrideableAction::setCurrentContext(const QList &context) QAction *oldAction = m_currentAction; m_currentAction = 0; for (int i = 0; i < m_context.size(); ++i) { - if (QAction *a = m_contextActionMap.value(m_context.at(i), 0)) { + if (QAction * a = m_contextActionMap.value(m_context.at(i), 0)) { m_currentAction = a; break; } } - if (m_currentAction == oldAction && m_contextInitialized) + if (m_currentAction == oldAction && m_contextInitialized) { return true; + } m_contextInitialized = true; if (oldAction) { @@ -459,8 +459,9 @@ bool OverrideableAction::setCurrentContext(const QList &context) m_active = true; return true; } - if (hasAttribute(CA_Hide)) + if (hasAttribute(CA_Hide)) { m_action->setVisible(false); + } m_action->setEnabled(false); m_active = false; return false; @@ -471,10 +472,11 @@ void OverrideableAction::addOverrideAction(QAction *action, const QList &co if (context.isEmpty()) { m_contextActionMap.insert(0, action); } else { - for (int i=0; itext()); + } m_contextActionMap.insert(k, action); } } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.h b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.h index d83110127..6f4b394bc 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,32 +41,30 @@ QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT Command : public QObject -{ +class CORE_EXPORT Command : public QObject { Q_OBJECT public: enum CommandAttribute { - CA_Hide = 0x0100, - CA_UpdateText = 0x0200, - CA_UpdateIcon = 0x0400, + CA_Hide = 0x0100, + CA_UpdateText = 0x0200, + CA_UpdateIcon = 0x0400, CA_NonConfigureable = 0x8000, - CA_Mask = 0xFF00 + CA_Mask = 0xFF00 }; virtual void setDefaultKeySequence(const QKeySequence &key) = 0; - virtual QKeySequence defaultKeySequence() const = 0; + virtual QKeySequence defaultKeySequence() const = 0; virtual QKeySequence keySequence() const = 0; virtual void setDefaultText(const QString &text) = 0; virtual QString defaultText() const = 0; virtual int id() const = 0; - virtual QAction *action() const = 0; + virtual QAction *action() const = 0; virtual QShortcut *shortcut() const = 0; - virtual void setAttribute(CommandAttribute attr) = 0; - virtual void removeAttribute(CommandAttribute attr) = 0; + virtual void setAttribute(CommandAttribute attr) = 0; + virtual void removeAttribute(CommandAttribute attr) = 0; virtual bool hasAttribute(CommandAttribute attr) const = 0; virtual bool isActive() const = 0; @@ -80,7 +78,6 @@ public: signals: void keySequenceChanged(); }; - } // namespace Core #endif // COMMAND_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command_p.h b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command_p.h index ce024768f..a2a80fae8 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command_p.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command_p.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,9 +39,7 @@ namespace Core { namespace Internal { - -class CommandPrivate : public Core::Command -{ +class CommandPrivate : public Core::Command { Q_OBJECT public: CommandPrivate(int id); @@ -76,8 +74,7 @@ protected: QString m_defaultText; }; -class Shortcut : public CommandPrivate -{ +class Shortcut : public CommandPrivate { Q_OBJECT public: Shortcut(int id); @@ -105,8 +102,7 @@ private: QString m_defaultText; }; -class Action : public CommandPrivate -{ +class Action : public CommandPrivate { Q_OBJECT public: Action(int id); @@ -125,14 +121,13 @@ public: protected: void updateToolTipWithKeySequence(); - + QAction *m_action; QList m_locations; QString m_toolTip; }; -class OverrideableAction : public Action -{ +class OverrideableAction : public Action { Q_OBJECT public: @@ -153,7 +148,6 @@ private: bool m_active; bool m_contextInitialized; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.cpp b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.cpp index bc7840b01..9554a817b 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,35 +42,36 @@ using namespace Core::Internal; \class CommandsFile \brief The CommandsFile class provides a collection of import and export commands. \inheaderfile commandsfile.h -*/ + */ /*! ... -*/ + */ CommandsFile::CommandsFile(const QString &filename) : m_filename(filename) -{ - -} +{} /*! ... -*/ + */ QMap CommandsFile::importCommands() const { QMap result; QFile file(m_filename); - if (!file.open(QIODevice::ReadOnly)) + if (!file.open(QIODevice::ReadOnly)) { return result; + } QDomDocument doc("KeyboardMappingScheme"); - if (!doc.setContent(&file)) + if (!doc.setContent(&file)) { return result; + } QDomElement root = doc.documentElement(); - if (root.nodeName() != QLatin1String("mapping")) + if (root.nodeName() != QLatin1String("mapping")) { return result; + } QDomElement ks = root.firstChildElement(); for (; !ks.isNull(); ks = ks.nextSiblingElement()) { @@ -78,8 +79,9 @@ QMap CommandsFile::importCommands() const QString id = ks.attribute(QLatin1String("id")); QKeySequence shortcutkey; QDomElement keyelem = ks.firstChildElement("key"); - if (!keyelem.isNull()) + if (!keyelem.isNull()) { shortcutkey = QKeySequence(keyelem.attribute("value")); + } result.insert(id, shortcutkey); } } @@ -90,21 +92,24 @@ QMap CommandsFile::importCommands() const /*! ... -*/ + */ bool CommandsFile::exportCommands(const QList &items) { UniqueIDManager *idmanager = UniqueIDManager::instance(); QFile file(m_filename); - if (!file.open(QIODevice::WriteOnly)) + + if (!file.open(QIODevice::WriteOnly)) { return false; + } QDomDocument doc("KeyboardMappingScheme"); QDomElement root = doc.createElement("mapping"); doc.appendChild(root); - foreach (const ShortcutItem *item, items) { + foreach(const ShortcutItem * item, items) { QDomElement ctag = doc.createElement("shortcut"); + ctag.setAttribute(QLatin1String("id"), idmanager->stringForUniqueIdentifier(item->m_cmd->id())); root.appendChild(ctag); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.h b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.h index bdfeccac2..8ca75b071 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,11 +37,9 @@ namespace Core { namespace Internal { - struct ShortcutItem; - -class CommandsFile : public QObject -{ + +class CommandsFile : public QObject { Q_OBJECT public: @@ -53,8 +51,7 @@ public: private: QString m_filename; }; - } // namespace Internal } // namespace Core -#endif //COMMANDSFILE_H +#endif // COMMANDSFILE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.cpp b/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.cpp index 8bf76b4ea..7b7c12935 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,7 +43,7 @@ #include #include #include - + #include #include #include @@ -62,71 +62,69 @@ AuthorsDialog::AuthorsDialog(QWidget *parent) setWindowIcon(QIcon(":/core/images/openpilot_logo_32.png")); setWindowTitle(tr("About OpenPilot")); setWindowFlags(windowFlags() & ~Qt::WindowContextHelpButtonHint); - // This loads a QML doc containing a Tabbed view - QDeclarativeView *view = new QDeclarativeView(this); - view->setSource(QUrl("qrc:/core/qml/AboutDialog.qml")); + // This loads a QML doc containing a Tabbed view + QDeclarativeView *view = new QDeclarativeView(this); + view->setSource(QUrl("qrc:/core/qml/AboutDialog.qml")); + QString version = QLatin1String(GCS_VERSION_LONG); + version += QDate(2007, 25, 10).toString(Qt::SystemLocaleDate); - QString version = QLatin1String(GCS_VERSION_LONG); - version += QDate(2007, 25, 10).toString(Qt::SystemLocaleDate); - - QString ideRev; + QString ideRev; #ifdef GCS_REVISION - //: This gets conditionally inserted as argument %8 into the description string. - ideRev = tr("From revision %1
").arg(QString::fromLatin1(GCS_REVISION_STR).left(10)); + // : This gets conditionally inserted as argument %8 into the description string. + ideRev = tr("From revision %1
").arg(QString::fromLatin1(GCS_REVISION_STR).left(10)); #endif #ifdef UAVO_HASH - //: This gets conditionally inserted as argument %11 into the description string. - QByteArray uavoHashArray; - QString uavoHash = QString::fromLatin1(Core::Constants::UAVOSHA1_STR); - uavoHash.chop(2); - uavoHash.remove(0, 2); - uavoHash = uavoHash.trimmed(); - bool ok; - foreach(QString str, uavoHash.split(",")) { - uavoHashArray.append(str.toInt(&ok, 16)); - } - QString gcsUavoHashStr; - foreach(char i, uavoHashArray) { - gcsUavoHashStr.append(QString::number(i, 16).right(2)); - } - QString uavoHashStr = gcsUavoHashStr; + // : This gets conditionally inserted as argument %11 into the description string. + QByteArray uavoHashArray; + QString uavoHash = QString::fromLatin1(Core::Constants::UAVOSHA1_STR); + uavoHash.chop(2); + uavoHash.remove(0, 2); + uavoHash = uavoHash.trimmed(); + bool ok; + foreach(QString str, uavoHash.split(",")) { + uavoHashArray.append(str.toInt(&ok, 16)); + } + QString gcsUavoHashStr; + foreach(char i, uavoHashArray) { + gcsUavoHashStr.append(QString::number(i, 16).right(2)); + } + QString uavoHashStr = gcsUavoHashStr; #else - QString uavoHashStr = "N/A"; + QString uavoHashStr = "N/A"; #endif - const QString description = tr( - "

OpenPilot Ground Control Station

" - "GCS Revision: %1
" - "UAVO Hash: %2
" - "
" - "Built from %3
" - "Built on %4 at %5
" - "Based on Qt %6 (%7 bit)
" - "
" - "© %8, 2010-%9. All rights reserved.
" - "
" - "This program is free software; you can redistribute it and/or modify
" - "it under the terms of the GNU General Public License as published by
" - "the Free Software Foundation; either version 3 of the License, or
" - "(at your option) any later version.
" - "
" - "The program is provided AS IS with NO WARRANTY OF ANY KIND, " - "INCLUDING THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A " - "PARTICULAR PURPOSE.
" - ).arg( - QString::fromLatin1(GCS_REVISION_STR).left(60), // %1 - uavoHashStr, // %2 - QLatin1String(GCS_ORIGIN_STR), // $3 - QLatin1String(__DATE__), // %4 - QLatin1String(__TIME__), // %5 - QLatin1String(QT_VERSION_STR), // %6 - QString::number(QSysInfo::WordSize), // %7 - QLatin1String(GCS_AUTHOR), // %8 - QLatin1String(GCS_YEAR_STR) // %9 - ); - // Expose the version description to the QML doc - view->rootContext()->setContextProperty("version", description); - + const QString description = tr( + "

OpenPilot Ground Control Station

" + "GCS Revision: %1
" + "UAVO Hash: %2
" + "
" + "Built from %3
" + "Built on %4 at %5
" + "Based on Qt %6 (%7 bit)
" + "
" + "© %8, 2010-%9. All rights reserved.
" + "
" + "This program is free software; you can redistribute it and/or modify
" + "it under the terms of the GNU General Public License as published by
" + "the Free Software Foundation; either version 3 of the License, or
" + "(at your option) any later version.
" + "
" + "The program is provided AS IS with NO WARRANTY OF ANY KIND, " + "INCLUDING THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A " + "PARTICULAR PURPOSE.
" + ).arg( + QString::fromLatin1(GCS_REVISION_STR).left(60), // %1 + uavoHashStr, // %2 + QLatin1String(GCS_ORIGIN_STR), // $3 + QLatin1String(__DATE__), // %4 + QLatin1String(__TIME__), // %5 + QLatin1String(QT_VERSION_STR), // %6 + QString::number(QSysInfo::WordSize), // %7 + QLatin1String(GCS_AUTHOR), // %8 + QLatin1String(GCS_YEAR_STR) // %9 + ); + // Expose the version description to the QML doc + view->rootContext()->setContextProperty("version", description); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.h b/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.h index f1521bf14..fadac688e 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,14 +33,11 @@ namespace Core { namespace Internal { - -class AuthorsDialog : public QDialog -{ +class AuthorsDialog : public QDialog { Q_OBJECT public: explicit AuthorsDialog(QWidget *parent); }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/basemode.cpp b/ground/openpilotgcs/src/plugins/coreplugin/basemode.cpp index f50e4dfdd..63d84a679 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/basemode.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/basemode.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -59,7 +59,7 @@ using namespace Core; [...] } \endcode -*/ + */ /*! \fn BaseMode::BaseMode(QObject *parent) @@ -68,17 +68,16 @@ using namespace Core; setter functions to give the mode a meaning. \a parent -*/ -BaseMode::BaseMode(QObject *parent): + */ +BaseMode::BaseMode(QObject *parent) : IMode(parent), m_priority(0), m_widget(0) -{ -} +{} /*! \fn BaseMode::~BaseMode() -*/ + */ BaseMode::~BaseMode() { delete m_widget; diff --git a/ground/openpilotgcs/src/plugins/coreplugin/basemode.h b/ground/openpilotgcs/src/plugins/coreplugin/basemode.h index 786c97bed..c2204c7c5 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/basemode.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/basemode.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,10 +37,8 @@ #include namespace Core { - class CORE_EXPORT BaseMode - : public IMode -{ + : public IMode { Q_OBJECT public: @@ -48,29 +46,64 @@ public: ~BaseMode(); // IMode - QString name() const { return m_name; } - QIcon icon() const { return m_icon; } - int priority() const { return m_priority; } - QWidget *widget() { return m_widget; } - const char *uniqueModeName() const { return m_uniqueModeName; } - QList context() const { return m_context; } + QString name() const + { + return m_name; + } + QIcon icon() const + { + return m_icon; + } + int priority() const + { + return m_priority; + } + QWidget *widget() + { + return m_widget; + } + const char *uniqueModeName() const + { + return m_uniqueModeName; + } + QList context() const + { + return m_context; + } - void setName(const QString &name) { m_name = name; } - void setIcon(const QIcon &icon) { m_icon = icon; } - void setPriority(int priority) { m_priority = priority; } - void setWidget(QWidget *widget) { m_widget = widget; } - void setUniqueModeName(const char *uniqueModeName) { m_uniqueModeName = uniqueModeName; } - void setContext(const QList &context) { m_context = context; } + void setName(const QString &name) + { + m_name = name; + } + void setIcon(const QIcon &icon) + { + m_icon = icon; + } + void setPriority(int priority) + { + m_priority = priority; + } + void setWidget(QWidget *widget) + { + m_widget = widget; + } + void setUniqueModeName(const char *uniqueModeName) + { + m_uniqueModeName = uniqueModeName; + } + void setContext(const QList &context) + { + m_context = context; + } private: QString m_name; QIcon m_icon; int m_priority; QWidget *m_widget; - const char * m_uniqueModeName; + const char *m_uniqueModeName; QList m_context; }; - } // namespace Core #endif // BASEMODE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/baseview.cpp b/ground/openpilotgcs/src/plugins/coreplugin/baseview.cpp index 0a87a9476..f659188d5 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/baseview.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/baseview.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,13 +33,12 @@ using namespace Core; BaseView::BaseView(QObject *parent) - : IView(parent), + : IView(parent), m_viewName(""), m_widget(0), m_context(QList()), m_defaultPosition(IView::First) -{ -} +{} BaseView::~BaseView() { @@ -75,6 +74,7 @@ void BaseView::setUniqueViewName(const char *name) QWidget *BaseView::setWidget(QWidget *widget) { QWidget *oldWidget = m_widget; + m_widget = widget; return oldWidget; } @@ -88,4 +88,3 @@ void BaseView::setDefaultPosition(IView::ViewPosition position) { m_defaultPosition = position; } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/baseview.h b/ground/openpilotgcs/src/plugins/coreplugin/baseview.h index 8f54f3c0f..2bbef0a8d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/baseview.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/baseview.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ #include namespace Core { - -class CORE_EXPORT BaseView : public IView -{ +class CORE_EXPORT BaseView : public IView { Q_OBJECT public: @@ -59,7 +57,6 @@ private: QList m_context; IView::ViewPosition m_defaultPosition; }; - } // namespace Core #endif // BASEVIEW_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp index 2580c2dab..db684e4c1 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp @@ -40,7 +40,6 @@ #include namespace Core { - ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow, QTabWidget *modeStack) : QWidget(mainWindow), m_availableDevList(0), @@ -50,7 +49,7 @@ ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow, QTabWidge m_mainWindow(mainWindow) { // monitor widget - m_monitorWidget = new TelemetryMonitorWidget(this); + m_monitorWidget = new TelemetryMonitorWidget(this); // device list m_availableDevList = new QComboBox; @@ -90,31 +89,35 @@ ConnectionManager::~ConnectionManager() { disconnectDevice(); suspendPolling(); - if (m_monitorWidget) + if (m_monitorWidget) { delete m_monitorWidget; + } } void ConnectionManager::init() { - //register to the plugin manager so we can receive - //new connection object from plugins - QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(objectAdded(QObject*)), this, SLOT(objectAdded(QObject*))); - QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(aboutToRemoveObject(QObject*)), this, SLOT(aboutToRemoveObject(QObject*))); + // register to the plugin manager so we can receive + // new connection object from plugins + QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(objectAdded(QObject *)), this, SLOT(objectAdded(QObject *))); + QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(aboutToRemoveObject(QObject *)), this, SLOT(aboutToRemoveObject(QObject *))); } /** -* Method called when the user clicks the "Connect" button -*/ + * Method called when the user clicks the "Connect" button + */ bool ConnectionManager::connectDevice(DevListItem device) { QString deviceName = m_availableDevList->itemData(m_availableDevList->currentIndex(), Qt::ToolTipRole).toString(); DevListItem connection_device = findDevice(deviceName); - if (!connection_device.connection) + + if (!connection_device.connection) { return false; + } QIODevice *io_dev = connection_device.connection->openDevice(connection_device.device.name); - if (!io_dev) + if (!io_dev) { return false; + } io_dev->open(QIODevice::ReadWrite); @@ -142,9 +145,9 @@ bool ConnectionManager::connectDevice(DevListItem device) } /** -* Method called by plugins who want to force a disconnection. -* Used by Uploader gadget for instance. -*/ + * Method called by plugins who want to force a disconnection. + * Used by Uploader gadget for instance. + */ bool ConnectionManager::disconnectDevice() { // tell the monitor widget we're disconnected @@ -160,10 +163,12 @@ bool ConnectionManager::disconnectDevice() // We are connected - disconnect from the device // stop our timers - if(reconnect->isActive()) + if (reconnect->isActive()) { reconnect->stop(); - if(reconnectCheck->isActive()) + } + if (reconnectCheck->isActive()) { reconnectCheck->stop(); + } // signal interested plugins that user is disconnecting his device emit deviceAboutToDisconnect(); @@ -172,7 +177,7 @@ bool ConnectionManager::disconnectDevice() if (m_connectionDevice.connection) { m_connectionDevice.connection->closeDevice(m_connectionDevice.getConName()); } - } catch (...) { // handle exception + } catch(...) { // handle exception qDebug() << "Exception: m_connectionDevice.connection->closeDevice(" << m_connectionDevice.getConName() << ")"; } @@ -187,15 +192,18 @@ bool ConnectionManager::disconnectDevice() } /** -* Slot called when a plugin added an object to the core pool -*/ + * Slot called when a plugin added an object to the core pool + */ void ConnectionManager::objectAdded(QObject *obj) { - //Check if a plugin added a connection object to the pool + // Check if a plugin added a connection object to the pool IConnection *connection = Aggregation::query(obj); - if (!connection) return; - //register devices and populate CB + if (!connection) { + return; + } + + // register devices and populate CB devChanged(connection); // Keep track of the registration to be able to tell plugins @@ -207,19 +215,22 @@ void ConnectionManager::objectAdded(QObject *obj) void ConnectionManager::aboutToRemoveObject(QObject *obj) { - //Check if a plugin added a connection object to the pool + // Check if a plugin added a connection object to the pool IConnection *connection = Aggregation::query(obj); - if (!connection) return; - if (m_connectionDevice.connection && m_connectionDevice.connection == connection) - { // we are currently using the one that is about to be removed + if (!connection) { + return; + } + + if (m_connectionDevice.connection && m_connectionDevice.connection == connection) { // we are currently using the one that is about to be removed disconnectDevice(); m_connectionDevice.connection = NULL; m_ioDev = NULL; } - if (m_connectionsList.contains(connection)) + if (m_connectionsList.contains(connection)) { m_connectionsList.removeAt(m_connectionsList.indexOf(connection)); + } } @@ -230,8 +241,8 @@ void ConnectionManager::onConnectionDestroyed(QObject *obj) } /** -* Slot called when the user selects a device from the combo box -*/ + * Slot called when the user selects a device from the combo box + */ void ConnectionManager::onDeviceSelectionChanged(int index) { if (index >= 0) { @@ -241,8 +252,8 @@ void ConnectionManager::onDeviceSelectionChanged(int index) } /** -* Slot called when the user clicks the connect/disconnect button -*/ + * Slot called when the user clicks the connect/disconnect button + */ void ConnectionManager::onConnectClicked() { // Check if we have a ioDev already created: @@ -253,48 +264,49 @@ void ConnectionManager::onConnectClicked() if (device.connection) { connectDevice(device); } - } - else { + } else { // disconnecting disconnectDevice(); } } /** -* Slot called when the telemetry is connected -*/ + * Slot called when the telemetry is connected + */ void ConnectionManager::telemetryConnected() { qDebug() << "TelemetryMonitor: connected"; - if(reconnectCheck->isActive()) + if (reconnectCheck->isActive()) { reconnectCheck->stop(); + } - //tell the monitor we're connected + // tell the monitor we're connected m_monitorWidget->connect(); } /** -* Slot called when the telemetry is disconnected -*/ + * Slot called when the telemetry is disconnected + */ void ConnectionManager::telemetryDisconnected() { qDebug() << "TelemetryMonitor: disconnected"; - if (m_ioDev){ - if(m_connectionDevice.connection->shortName()=="Serial") { - if(!reconnect->isActive()) + if (m_ioDev) { + if (m_connectionDevice.connection->shortName() == "Serial") { + if (!reconnect->isActive()) { reconnect->start(1000); + } } } - //tell the monitor we're disconnected + // tell the monitor we're disconnected m_monitorWidget->disconnect(); } /** -* Slot called when the telemetry rates are updated -*/ + * Slot called when the telemetry rates are updated + */ void ConnectionManager::telemetryUpdated(double txRate, double rxRate) { m_monitorWidget->updateTelemetry(txRate, rxRate); @@ -302,17 +314,18 @@ void ConnectionManager::telemetryUpdated(double txRate, double rxRate) void ConnectionManager::reconnectSlot() { - qDebug()<<"reconnect"; - if(m_ioDev->isOpen()) + qDebug() << "reconnect"; + if (m_ioDev->isOpen()) { m_ioDev->close(); + } - if(m_ioDev->open(QIODevice::ReadWrite)) { - qDebug()<<"reconnect successfull"; + if (m_ioDev->open(QIODevice::ReadWrite)) { + qDebug() << "reconnect successfull"; reconnect->stop(); reconnectCheck->start(20000); + } else { + qDebug() << "reconnect NOT successfull"; } - else - qDebug()<<"reconnect NOT successfull"; } void ConnectionManager::reconnectCheckSlot() @@ -322,14 +335,14 @@ void ConnectionManager::reconnectCheckSlot() } /** -* Find a device by its displayed (visible on screen) name -*/ + * Find a device by its displayed (visible on screen) name + */ DevListItem ConnectionManager::findDevice(const QString &devName) { - foreach (DevListItem d, m_devList) - { - if (d.getConName() == devName) + foreach(DevListItem d, m_devList) { + if (d.getConName() == devName) { return d; + } } qDebug() << "findDevice: cannot find " << devName << " in device list"; @@ -340,14 +353,13 @@ DevListItem ConnectionManager::findDevice(const QString &devName) } /** -* Tells every connection plugin to stop polling for devices if they -* are doing that. -* -*/ + * Tells every connection plugin to stop polling for devices if they + * are doing that. + * + */ void ConnectionManager::suspendPolling() { - foreach (IConnection *cnx, m_connectionsList) - { + foreach(IConnection * cnx, m_connectionsList) { cnx->suspendPolling(); } @@ -357,13 +369,12 @@ void ConnectionManager::suspendPolling() } /** -* Tells every connection plugin to resume polling for devices if they -* are doing that. -*/ + * Tells every connection plugin to resume polling for devices if they + * are doing that. + */ void ConnectionManager::resumePolling() { - foreach (IConnection *cnx, m_connectionsList) - { + foreach(IConnection * cnx, m_connectionsList) { cnx->resumePolling(); } @@ -385,8 +396,7 @@ void ConnectionManager::updateConnectionList(IConnection *connection) // Go through the list of connections of that type. If they are not in the // available device list then remove them. If they are connected, then // disconnect them. - for (QLinkedList::iterator iter = m_devList.begin(); iter != m_devList.end(); ) - { + for (QLinkedList::iterator iter = m_devList.begin(); iter != m_devList.end();) { if (iter->connection != connection) { ++iter; continue; @@ -401,49 +411,50 @@ void ConnectionManager::updateConnectionList(IConnection *connection) } iter = m_devList.erase(iter); - } else + } else { ++iter; + } } // Add any back to list that don't exist - foreach (IConnection::device dev, availableDev) - { + foreach(IConnection::device dev, availableDev) { bool found = m_devList.contains(DevListItem(connection, dev)); + if (!found) { - registerDevice(connection,dev); + registerDevice(connection, dev); } } } /** -* Register a device from a specific connection plugin -* @param devN contains the connection shortname + device name which is diplayed in the tooltip -* @param disp is the name that is displayed in the dropdown menu -* @param name is the actual device name -*/ + * Register a device from a specific connection plugin + * @param devN contains the connection shortname + device name which is diplayed in the tooltip + * @param disp is the name that is displayed in the dropdown menu + * @param name is the actual device name + */ void ConnectionManager::registerDevice(IConnection *conn, IConnection::device device) { - DevListItem d(conn,device); + DevListItem d(conn, device); + m_devList.append(d); } /** -* A device plugin notified us that its device list has changed -* (eg: user plug/unplug a usb device) -* \param[in] connection Connection type which signaled the update -*/ + * A device plugin notified us that its device list has changed + * (eg: user plug/unplug a usb device) + * \param[in] connection Connection type which signaled the update + */ void ConnectionManager::devChanged(IConnection *connection) { - if(!ExtensionSystem::PluginManager::instance()->allPluginsLoaded()) - { + if (!ExtensionSystem::PluginManager::instance()->allPluginsLoaded()) { connectionBackup.append(connection); - connect(ExtensionSystem::PluginManager::instance(),SIGNAL(pluginsLoadEnded()),this,SLOT(connectionsCallBack()),Qt::UniqueConnection); + connect(ExtensionSystem::PluginManager::instance(), SIGNAL(pluginsLoadEnded()), this, SLOT(connectionsCallBack()), Qt::UniqueConnection); return; } - //clear device list combobox + // clear device list combobox m_availableDevList->clear(); - //remove registered devices of this IConnection from the list + // remove registered devices of this IConnection from the list updateConnectionList(connection); updateConnectionDropdown(); @@ -452,17 +463,18 @@ void ConnectionManager::devChanged(IConnection *connection) emit availableDevicesChanged(m_devList); - //disable connection button if the liNameif (m_availableDevList->count() > 0) - if (m_availableDevList->count() > 0) + // disable connection button if the liNameif (m_availableDevList->count() > 0) + if (m_availableDevList->count() > 0) { m_connectBtn->setEnabled(true); - else + } else { m_connectBtn->setEnabled(false); + } } void ConnectionManager::updateConnectionDropdown() { // add all the list again to the combobox - foreach (DevListItem d, m_devList) { + foreach(DevListItem d, m_devList) { m_availableDevList->addItem(d.getConName()); m_availableDevList->setItemData(m_availableDevList->count() - 1, d.getConName(), Qt::ToolTipRole); if (!m_ioDev && d.getConName().startsWith("USB")) { @@ -491,12 +503,10 @@ void ConnectionManager::updateConnectionDropdown() void Core::ConnectionManager::connectionsCallBack() { - foreach(IConnection * con,connectionBackup) - { + foreach(IConnection * con, connectionBackup) { devChanged(con); } connectionBackup.clear(); - disconnect(ExtensionSystem::PluginManager::instance(),SIGNAL(pluginsLoadEnded()),this,SLOT(connectionsCallBack())); + disconnect(ExtensionSystem::PluginManager::instance(), SIGNAL(pluginsLoadEnded()), this, SLOT(connectionsCallBack())); } - -} //namespace Core +} // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h index 8a8f49eff..71eee425f 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h @@ -48,31 +48,32 @@ class QTabWidget; QT_END_NAMESPACE namespace Core { - - class IConnection; +class IConnection; namespace Internal { - class FancyTabWidget; - class FancyActionBar; - class MainWindow; +class FancyTabWidget; +class FancyActionBar; +class MainWindow; } // namespace Internal -class DevListItem -{ +class DevListItem { public: DevListItem(IConnection *c, IConnection::device d) : - connection(c), device(d) { } + connection(c), device(d) {} - DevListItem() : connection(NULL) { } + DevListItem() : connection(NULL) {} - QString getConName() { - if (connection == NULL) + QString getConName() + { + if (connection == NULL) { return ""; + } return connection->shortName() + ": " + device.displayName; } - bool operator==(const DevListItem &rhs) { + bool operator==(const DevListItem &rhs) + { return connection == rhs.connection && device == rhs.device; } @@ -81,8 +82,7 @@ public: }; -class CORE_EXPORT ConnectionManager : public QWidget -{ +class CORE_EXPORT ConnectionManager : public QWidget { Q_OBJECT public: @@ -91,13 +91,25 @@ public: void init(); - QIODevice* getCurrentConnection() { return m_ioDev; } - DevListItem getCurrentDevice() { return m_connectionDevice; } + QIODevice *getCurrentConnection() + { + return m_ioDev; + } + DevListItem getCurrentDevice() + { + return m_connectionDevice; + } DevListItem findDevice(const QString &devName); - QLinkedList getAvailableDevices() { return m_devList; } + QLinkedList getAvailableDevices() + { + return m_devList; + } - bool isConnected() { return m_ioDev != 0; } + bool isConnected() + { + return m_ioDev != 0; + } bool connectDevice(DevListItem device); bool disconnectDevice(); @@ -129,7 +141,7 @@ private slots: void devChanged(IConnection *connection); void onConnectionDestroyed(QObject *obj); - void connectionsCallBack(); //used to call devChange after all the plugins are loaded + void connectionsCallBack(); // used to call devChange after all the plugins are loaded void reconnectSlot(); void reconnectCheckSlot(); @@ -137,27 +149,25 @@ protected: QComboBox *m_availableDevList; QPushButton *m_connectBtn; QLinkedList m_devList; - QList m_connectionsList; + QList m_connectionsList; - //tx/rx telemetry monitor - TelemetryMonitorWidget* m_monitorWidget; + // tx/rx telemetry monitor + TelemetryMonitorWidget *m_monitorWidget; - //currently connected connection plugin + // currently connected connection plugin DevListItem m_connectionDevice; - //currently connected QIODevice + // currently connected QIODevice QIODevice *m_ioDev; private: - bool connectDevice(); + bool connectDevice(); bool polling; Internal::MainWindow *m_mainWindow; QList connectionBackup; QTimer *reconnect; QTimer *reconnectCheck; - }; - -} //namespace Core +} // namespace Core #endif // CONNECTIONMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/core_global.h b/ground/openpilotgcs/src/plugins/coreplugin/core_global.h index 3c9b668f2..2297ec6e4 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/core_global.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/core_global.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreconstants.h b/ground/openpilotgcs/src/plugins/coreplugin/coreconstants.h index 6373fc28b..799c8c2e2 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreconstants.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreconstants.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,218 +31,217 @@ namespace Core { namespace Constants { - -#define GCS_VERSION_MAJOR 1 -#define GCS_VERSION_MINOR 0 +#define GCS_VERSION_MAJOR 1 +#define GCS_VERSION_MINOR 0 #define GCS_VERSION_RELEASE 0 #define STRINGIFY_INTERNAL(x) #x -#define STRINGIFY(x) STRINGIFY_INTERNAL(x) +#define STRINGIFY(x) STRINGIFY_INTERNAL(x) -#define GCS_VERSION STRINGIFY(GCS_VERSION_MAJOR) \ +#define GCS_VERSION \ + STRINGIFY(GCS_VERSION_MAJOR) \ "." STRINGIFY(GCS_VERSION_MINOR) \ "." STRINGIFY(GCS_VERSION_RELEASE) -const char * const GCS_VERSION_LONG = GCS_VERSION; +const char *const GCS_VERSION_LONG = GCS_VERSION; #ifdef GCS_REVISION -const char * const GCS_REVISION_STR = GCS_REVISION; +const char *const GCS_REVISION_STR = GCS_REVISION; #else -const char * const GCS_REVISION_STR = "N/A"; +const char *const GCS_REVISION_STR = "N/A"; #endif #ifdef GCS_YEAR -const char * const GCS_YEAR_STR = GCS_YEAR; +const char *const GCS_YEAR_STR = GCS_YEAR; #else -const char * const GCS_YEAR_STR = "2013"; +const char *const GCS_YEAR_STR = "2013"; #endif #ifdef GCS_ORIGIN -const char * const GCS_ORIGIN_STR = GCS_ORIGIN; +const char *const GCS_ORIGIN_STR = GCS_ORIGIN; #else -const char * const GCS_ORIGIN_STR = "unknown repository"; +const char *const GCS_ORIGIN_STR = "unknown repository"; #endif #ifdef UAVO_HASH -const char * const UAVOSHA1_STR = STRINGIFY(UAVO_HASH); +const char *const UAVOSHA1_STR = STRINGIFY(UAVO_HASH); #else -const char * const UAVOSHA1_STR = ""; +const char *const UAVOSHA1_STR = ""; #endif -const char * const GCS_AUTHOR = "The OpenPilot Project"; -const char * const GCS_HELP = "http://wiki.openpilot.org"; +const char *const GCS_AUTHOR = "The OpenPilot Project"; +const char *const GCS_HELP = "http://wiki.openpilot.org"; #undef GCS_VERSION #undef STRINGIFY #undef STRINGIFY_INTERNAL -//modes -const char * const MODE_WELCOME = "Welcome"; -const char * const MODE_UAVGADGET = "Mode 1"; -const int P_MODE_WELCOME = 100; -const int P_MODE_UAVGADGET = 90; +// modes +const char *const MODE_WELCOME = "Welcome"; +const char *const MODE_UAVGADGET = "Mode 1"; +const int P_MODE_WELCOME = 100; +const int P_MODE_UAVGADGET = 90; -//menubar -const char * const MENU_BAR = "GCS.MenuBar"; +// menubar +const char *const MENU_BAR = "GCS.MenuBar"; -//menus -const char * const M_FILE = "GCS.Menu.File"; -const char * const M_FILE_OPEN = "GCS.Menu.File.Open"; -const char * const M_FILE_NEW = "GCS.Menu.File.New"; -const char * const M_FILE_RECENTFILES = "GCS.Menu.File.RecentFiles"; -const char * const M_EDIT = "GCS.Menu.Edit"; -const char * const M_EDIT_ADVANCED = "GCS.Menu.Edit.Advanced"; -const char * const M_TOOLS = "GCS.Menu.Tools"; -const char * const M_WINDOW = "GCS.Menu.Window"; -const char * const M_WINDOW_PANES = "GCS.Menu.Window.Panes"; -const char * const M_HELP = "GCS.Menu.Help"; +// menus +const char *const M_FILE = "GCS.Menu.File"; +const char *const M_FILE_OPEN = "GCS.Menu.File.Open"; +const char *const M_FILE_NEW = "GCS.Menu.File.New"; +const char *const M_FILE_RECENTFILES = "GCS.Menu.File.RecentFiles"; +const char *const M_EDIT = "GCS.Menu.Edit"; +const char *const M_EDIT_ADVANCED = "GCS.Menu.Edit.Advanced"; +const char *const M_TOOLS = "GCS.Menu.Tools"; +const char *const M_WINDOW = "GCS.Menu.Window"; +const char *const M_WINDOW_PANES = "GCS.Menu.Window.Panes"; +const char *const M_HELP = "GCS.Menu.Help"; -//contexts -const char * const C_GLOBAL = "Global Context"; -const int C_GLOBAL_ID = 0; -const char * const C_WELCOME_MODE = "Core.WelcomeMode"; -const char * const C_UAVGADGET_MODE = "Core.UAVGadgetMode"; -const char * const C_UAVGADGETMANAGER = "Core.UAVGadgetManager"; -const char * const C_NAVIGATION_PANE = "Core.NavigationPane"; -const char * const C_PROBLEM_PANE = "Core.ProblemPane"; +// contexts +const char *const C_GLOBAL = "Global Context"; +const int C_GLOBAL_ID = 0; +const char *const C_WELCOME_MODE = "Core.WelcomeMode"; +const char *const C_UAVGADGET_MODE = "Core.UAVGadgetMode"; +const char *const C_UAVGADGETMANAGER = "Core.UAVGadgetManager"; +const char *const C_NAVIGATION_PANE = "Core.NavigationPane"; +const char *const C_PROBLEM_PANE = "Core.ProblemPane"; -//default editor kind -const char * const K_DEFAULT_TEXT_EDITOR = QT_TRANSLATE_NOOP("OpenWith::Editors", "Plain Text Editor"); -const char * const K_DEFAULT_BINARY_EDITOR = QT_TRANSLATE_NOOP("OpenWith::Editors", "Binary Editor"); +// default editor kind +const char *const K_DEFAULT_TEXT_EDITOR = QT_TRANSLATE_NOOP("OpenWith::Editors", "Plain Text Editor"); +const char *const K_DEFAULT_BINARY_EDITOR = QT_TRANSLATE_NOOP("OpenWith::Editors", "Binary Editor"); -//actions -const char * const UNDO = "GCS.Undo"; -const char * const REDO = "GCS.Redo"; -const char * const COPY = "GCS.Copy"; -const char * const PASTE = "GCS.Paste"; -const char * const CUT = "GCS.Cut"; -const char * const SELECTALL = "GCS.SelectAll"; +// actions +const char *const UNDO = "GCS.Undo"; +const char *const REDO = "GCS.Redo"; +const char *const COPY = "GCS.Copy"; +const char *const PASTE = "GCS.Paste"; +const char *const CUT = "GCS.Cut"; +const char *const SELECTALL = "GCS.SelectAll"; -const char * const NEW = "GCS.New"; -const char * const OPEN = "GCS.Open"; -const char * const OPEN_WITH = "GCS.OpenWith"; -const char * const REVERTTOSAVED = "GCS.RevertToSaved"; -const char * const SAVE = "GCS.Save"; -const char * const SAVEAS = "GCS.SaveAs"; -const char * const SAVEALL = "GCS.SaveAll"; -const char * const EXIT = "GCS.Exit"; +const char *const NEW = "GCS.New"; +const char *const OPEN = "GCS.Open"; +const char *const OPEN_WITH = "GCS.OpenWith"; +const char *const REVERTTOSAVED = "GCS.RevertToSaved"; +const char *const SAVE = "GCS.Save"; +const char *const SAVEAS = "GCS.SaveAs"; +const char *const SAVEALL = "GCS.SaveAll"; +const char *const EXIT = "GCS.Exit"; -const char * const OPTIONS = "GCS.Options"; -const char * const TOGGLE_SIDEBAR = "GCS.ToggleSidebar"; -const char * const TOGGLE_FULLSCREEN = "GCS.ToggleFullScreen"; +const char *const OPTIONS = "GCS.Options"; +const char *const TOGGLE_SIDEBAR = "GCS.ToggleSidebar"; +const char *const TOGGLE_FULLSCREEN = "GCS.ToggleFullScreen"; -const char * const MINIMIZE_WINDOW = "GCS.MinimizeWindow"; -const char * const ZOOM_WINDOW = "GCS.ZoomWindow"; +const char *const MINIMIZE_WINDOW = "GCS.MinimizeWindow"; +const char *const ZOOM_WINDOW = "GCS.ZoomWindow"; -const char * const SPLIT = "GCS.Split"; -const char * const SPLIT_SIDE_BY_SIDE = "GCS.SplitSideBySide"; -const char * const REMOVE_CURRENT_SPLIT = "GCS.RemoveCurrentSplit"; -const char * const REMOVE_ALL_SPLITS = "GCS.RemoveAllSplits"; -const char * const GOTO_OTHER_SPLIT = "GCS.GotoOtherSplit"; -const char * const SAVEASDEFAULT = "GCS.SaveAsDefaultLayout"; -const char * const RESTOREDEFAULT = "GCS.RestoreDefaultLayout"; -const char * const HIDE_TOOLBARS = "GCS.HideToolbars"; -const char * const CLOSE = "GCS.Close"; -const char * const CLOSEALL = "GCS.CloseAll"; -const char * const CLOSEOTHERS = "GCS.CloseOthers"; -const char * const GOTONEXT = "GCS.GotoNext"; -const char * const GOTOPREV = "GCS.GotoPrevious"; -const char * const GOTONEXTINHISTORY = "GCS.GotoNextInHistory"; -const char * const GOTOPREVINHISTORY = "GCS.GotoPreviousInHistory"; -const char * const GO_BACK = "GCS.GoBack"; -const char * const GO_FORWARD = "GCS.GoForward"; -const char * const GOTOPREVIOUSGROUP = "GCS.GotoPreviousTabGroup"; -const char * const GOTONEXTGROUP = "GCS.GotoNextTabGroup"; -const char * const WINDOWSLIST = "GCS.WindowsList"; -const char * const ABOUT_OPENPILOTGCS = "GCS.AboutOpenPilotGCS"; -const char * const ABOUT_PLUGINS = "GCS.AboutPlugins"; -const char * const ABOUT_AUTHORS = "GCS.AboutAuthors"; -const char * const ABOUT_QT = "GCS.AboutQt"; -const char * const S_RETURNTOEDITOR = "GCS.ReturnToEditor"; -const char * const OPEN_IN_EXTERNAL_EDITOR = "GCS.OpenInExternalEditor"; +const char *const SPLIT = "GCS.Split"; +const char *const SPLIT_SIDE_BY_SIDE = "GCS.SplitSideBySide"; +const char *const REMOVE_CURRENT_SPLIT = "GCS.RemoveCurrentSplit"; +const char *const REMOVE_ALL_SPLITS = "GCS.RemoveAllSplits"; +const char *const GOTO_OTHER_SPLIT = "GCS.GotoOtherSplit"; +const char *const SAVEASDEFAULT = "GCS.SaveAsDefaultLayout"; +const char *const RESTOREDEFAULT = "GCS.RestoreDefaultLayout"; +const char *const HIDE_TOOLBARS = "GCS.HideToolbars"; +const char *const CLOSE = "GCS.Close"; +const char *const CLOSEALL = "GCS.CloseAll"; +const char *const CLOSEOTHERS = "GCS.CloseOthers"; +const char *const GOTONEXT = "GCS.GotoNext"; +const char *const GOTOPREV = "GCS.GotoPrevious"; +const char *const GOTONEXTINHISTORY = "GCS.GotoNextInHistory"; +const char *const GOTOPREVINHISTORY = "GCS.GotoPreviousInHistory"; +const char *const GO_BACK = "GCS.GoBack"; +const char *const GO_FORWARD = "GCS.GoForward"; +const char *const GOTOPREVIOUSGROUP = "GCS.GotoPreviousTabGroup"; +const char *const GOTONEXTGROUP = "GCS.GotoNextTabGroup"; +const char *const WINDOWSLIST = "GCS.WindowsList"; +const char *const ABOUT_OPENPILOTGCS = "GCS.AboutOpenPilotGCS"; +const char *const ABOUT_PLUGINS = "GCS.AboutPlugins"; +const char *const ABOUT_AUTHORS = "GCS.AboutAuthors"; +const char *const ABOUT_QT = "GCS.AboutQt"; +const char *const S_RETURNTOEDITOR = "GCS.ReturnToEditor"; +const char *const OPEN_IN_EXTERNAL_EDITOR = "GCS.OpenInExternalEditor"; // default groups -const char * const G_DEFAULT_ONE = "GCS.Group.Default.One"; -const char * const G_DEFAULT_TWO = "GCS.Group.Default.Two"; -const char * const G_DEFAULT_THREE = "GCS.Group.Default.Three"; +const char *const G_DEFAULT_ONE = "GCS.Group.Default.One"; +const char *const G_DEFAULT_TWO = "GCS.Group.Default.Two"; +const char *const G_DEFAULT_THREE = "GCS.Group.Default.Three"; // main menu bar groups -const char * const G_FILE = "GCS.Group.File"; -const char * const G_EDIT = "GCS.Group.Edit"; -const char * const G_VIEW = "GCS.Group.View"; -const char * const G_TOOLS = "GCS.Group.Tools"; -const char * const G_WINDOW = "GCS.Group.Window"; -const char * const G_HELP = "GCS.Group.Help"; +const char *const G_FILE = "GCS.Group.File"; +const char *const G_EDIT = "GCS.Group.Edit"; +const char *const G_VIEW = "GCS.Group.View"; +const char *const G_TOOLS = "GCS.Group.Tools"; +const char *const G_WINDOW = "GCS.Group.Window"; +const char *const G_HELP = "GCS.Group.Help"; // file menu groups -const char * const G_FILE_NEW = "GCS.Group.File.New"; -const char * const G_FILE_OPEN = "GCS.Group.File.Open"; -const char * const G_FILE_PROJECT = "GCS.Group.File.Project"; -const char * const G_FILE_SAVE = "GCS.Group.File.Save"; -const char * const G_FILE_CLOSE = "GCS.Group.File.Close"; -const char * const G_FILE_OTHER = "GCS.Group.File.Other"; +const char *const G_FILE_NEW = "GCS.Group.File.New"; +const char *const G_FILE_OPEN = "GCS.Group.File.Open"; +const char *const G_FILE_PROJECT = "GCS.Group.File.Project"; +const char *const G_FILE_SAVE = "GCS.Group.File.Save"; +const char *const G_FILE_CLOSE = "GCS.Group.File.Close"; +const char *const G_FILE_OTHER = "GCS.Group.File.Other"; // edit menu groups -const char * const G_EDIT_UNDOREDO = "GCS.Group.Edit.UndoRedo"; -const char * const G_EDIT_COPYPASTE = "GCS.Group.Edit.CopyPaste"; -const char * const G_EDIT_SELECTALL = "GCS.Group.Edit.SelectAll"; -const char * const G_EDIT_ADVANCED = "GCS.Group.Edit.Advanced"; +const char *const G_EDIT_UNDOREDO = "GCS.Group.Edit.UndoRedo"; +const char *const G_EDIT_COPYPASTE = "GCS.Group.Edit.CopyPaste"; +const char *const G_EDIT_SELECTALL = "GCS.Group.Edit.SelectAll"; +const char *const G_EDIT_ADVANCED = "GCS.Group.Edit.Advanced"; -const char * const G_EDIT_FIND = "GCS.Group.Edit.Find"; -const char * const G_EDIT_OTHER = "GCS.Group.Edit.Other"; +const char *const G_EDIT_FIND = "GCS.Group.Edit.Find"; +const char *const G_EDIT_OTHER = "GCS.Group.Edit.Other"; // advanced edit menu groups -const char * const G_EDIT_FORMAT = "GCS.Group.Edit.Format"; -const char * const G_EDIT_COLLAPSING = "GCS.Group.Edit.Collapsing"; -const char * const G_EDIT_BLOCKS = "GCS.Group.Edit.Blocks"; -const char * const G_EDIT_FONT = "GCS.Group.Edit.Font"; -const char * const G_EDIT_EDITOR = "GCS.Group.Edit.Editor"; +const char *const G_EDIT_FORMAT = "GCS.Group.Edit.Format"; +const char *const G_EDIT_COLLAPSING = "GCS.Group.Edit.Collapsing"; +const char *const G_EDIT_BLOCKS = "GCS.Group.Edit.Blocks"; +const char *const G_EDIT_FONT = "GCS.Group.Edit.Font"; +const char *const G_EDIT_EDITOR = "GCS.Group.Edit.Editor"; // window menu groups -const char * const G_WINDOW_SIZE = "GCS.Group.Window.Size"; -const char * const G_WINDOW_PANES = "GCS.Group.Window.Panes"; -const char * const G_WINDOW_SPLIT = "GCS.Group.Window.Split"; -const char * const G_WINDOW_NAVIGATE = "GCS.Group.Window.Navigate"; -const char * const G_WINDOW_OTHER = "GCS.Group.Window.Other"; -const char * const G_WINDOW_HIDE_TOOLBAR = "GCS.Group.Window.Hide"; +const char *const G_WINDOW_SIZE = "GCS.Group.Window.Size"; +const char *const G_WINDOW_PANES = "GCS.Group.Window.Panes"; +const char *const G_WINDOW_SPLIT = "GCS.Group.Window.Split"; +const char *const G_WINDOW_NAVIGATE = "GCS.Group.Window.Navigate"; +const char *const G_WINDOW_OTHER = "GCS.Group.Window.Other"; +const char *const G_WINDOW_HIDE_TOOLBAR = "GCS.Group.Window.Hide"; // help groups (global) -const char * const G_HELP_HELP = "GCS.Group.Help.Help"; -const char * const G_HELP_ABOUT = "GCS.Group.Help.About"; +const char *const G_HELP_HELP = "GCS.Group.Help.Help"; +const char *const G_HELP_ABOUT = "GCS.Group.Help.About"; -const char * const ICON_MINUS = ":/core/images/minus.png"; -const char * const ICON_PLUS = ":/core/images/plus.png"; -const char * const ICON_NEWFILE = ":/core/images/filenew.png"; -const char * const ICON_OPENFILE = ":/core/images/fileopen.png"; -const char * const ICON_SAVEFILE = ":/core/images/filesave.png"; -const char * const ICON_UNDO = ":/core/images/undo.png"; -const char * const ICON_REDO = ":/core/images/redo.png"; -const char * const ICON_COPY = ":/core/images/editcopy.png"; -const char * const ICON_PASTE = ":/core/images/editpaste.png"; -const char * const ICON_CUT = ":/core/images/editcut.png"; -const char * const ICON_NEXT = ":/core/images/next.png"; -const char * const ICON_PREV = ":/core/images/prev.png"; -const char * const ICON_DIR = ":/core/images/dir.png"; -const char * const ICON_CLEAN_PANE = ":/core/images/clean_pane_small.png"; -const char * const ICON_CLEAR = ":/core/images/clear.png"; -const char * const ICON_FIND = ":/core/images/find.png"; -const char * const ICON_FINDNEXT = ":/core/images/findnext.png"; -const char * const ICON_REPLACE = ":/core/images/replace.png"; -const char * const ICON_RESET = ":/core/images/reset.png"; -const char * const ICON_MAGNIFIER = ":/core/images/magnifier.png"; -const char * const ICON_TOGGLE_SIDEBAR = ":/core/images/sidebaricon.png"; -const char * const ICON_PLUGIN = ":/core/images/pluginicon.png"; -const char * const ICON_EXIT = ":/core/images/exiticon.png"; -const char * const ICON_OPTIONS = ":/core/images/optionsicon.png"; -const char * const ICON_HELP = ":/core/images/helpicon.png"; -const char * const ICON_OPENPILOT = ":/core/images/openpiloticon.png"; +const char *const ICON_MINUS = ":/core/images/minus.png"; +const char *const ICON_PLUS = ":/core/images/plus.png"; +const char *const ICON_NEWFILE = ":/core/images/filenew.png"; +const char *const ICON_OPENFILE = ":/core/images/fileopen.png"; +const char *const ICON_SAVEFILE = ":/core/images/filesave.png"; +const char *const ICON_UNDO = ":/core/images/undo.png"; +const char *const ICON_REDO = ":/core/images/redo.png"; +const char *const ICON_COPY = ":/core/images/editcopy.png"; +const char *const ICON_PASTE = ":/core/images/editpaste.png"; +const char *const ICON_CUT = ":/core/images/editcut.png"; +const char *const ICON_NEXT = ":/core/images/next.png"; +const char *const ICON_PREV = ":/core/images/prev.png"; +const char *const ICON_DIR = ":/core/images/dir.png"; +const char *const ICON_CLEAN_PANE = ":/core/images/clean_pane_small.png"; +const char *const ICON_CLEAR = ":/core/images/clear.png"; +const char *const ICON_FIND = ":/core/images/find.png"; +const char *const ICON_FINDNEXT = ":/core/images/findnext.png"; +const char *const ICON_REPLACE = ":/core/images/replace.png"; +const char *const ICON_RESET = ":/core/images/reset.png"; +const char *const ICON_MAGNIFIER = ":/core/images/magnifier.png"; +const char *const ICON_TOGGLE_SIDEBAR = ":/core/images/sidebaricon.png"; +const char *const ICON_PLUGIN = ":/core/images/pluginicon.png"; +const char *const ICON_EXIT = ":/core/images/exiticon.png"; +const char *const ICON_OPTIONS = ":/core/images/optionsicon.png"; +const char *const ICON_HELP = ":/core/images/helpicon.png"; +const char *const ICON_OPENPILOT = ":/core/images/openpiloticon.png"; // wizard kind -const char * const WIZARD_TYPE_FILE = "GCS::WizardType::File"; -const char * const WIZARD_TYPE_CLASS = "GCS::WizardType::Class"; - +const char *const WIZARD_TYPE_FILE = "GCS::WizardType::File"; +const char *const WIZARD_TYPE_CLASS = "GCS::WizardType::Class"; } // namespace Constants } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.cpp b/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.cpp index b45582628..5ee027096 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.cpp @@ -10,19 +10,19 @@ * @{ * @brief The Core GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,10 +33,8 @@ namespace Core { namespace Internal { - // The Core Singleton static CoreImpl *m_instance = 0; - } // namespace Internal } // namespace Core @@ -45,14 +43,14 @@ using namespace Core; using namespace Core::Internal; -ICore* ICore::instance() +ICore *ICore::instance() { return m_instance; } CoreImpl::CoreImpl(MainWindow *mainwindow) { - m_instance = this; + m_instance = this; m_mainwindow = mainwindow; } @@ -184,24 +182,24 @@ void CoreImpl::updateContext() void CoreImpl::openFiles(const QStringList &arguments) { Q_UNUSED(arguments) - //m_mainwindow->openFiles(arguments); + // m_mainwindow->openFiles(arguments); } -void CoreImpl::readMainSettings(QSettings* qs, bool workspaceDiffOnly) +void CoreImpl::readMainSettings(QSettings *qs, bool workspaceDiffOnly) { m_mainwindow->readSettings(qs, workspaceDiffOnly); } -void CoreImpl::saveMainSettings(QSettings* qs) +void CoreImpl::saveMainSettings(QSettings *qs) { m_mainwindow->saveSettings(qs); } -void CoreImpl::readSettings(IConfigurablePlugin* plugin, QSettings* qs) +void CoreImpl::readSettings(IConfigurablePlugin *plugin, QSettings *qs) { m_mainwindow->readSettings(plugin, qs); } -void CoreImpl::saveSettings(IConfigurablePlugin* plugin, QSettings* qs) +void CoreImpl::saveSettings(IConfigurablePlugin *plugin, QSettings *qs) { m_mainwindow->saveSettings(plugin, qs); } @@ -209,4 +207,3 @@ void CoreImpl::deleteSettings() { m_mainwindow->deleteSettings(); } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.h b/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.h index cd0dfd928..671c248a2 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ namespace Core { namespace Internal { - -class CoreImpl : public ICore -{ +class CoreImpl : public ICore { Q_OBJECT public: @@ -64,10 +62,10 @@ public: QSettings *settings(QSettings::Scope scope = QSettings::UserScope) const; SettingsDatabase *settingsDatabase() const; - void readMainSettings(QSettings* qs, bool workspaceDiffOnly); - void saveMainSettings(QSettings* qs); - void readSettings(IConfigurablePlugin* plugin, QSettings* qs = 0 ); - void saveSettings(IConfigurablePlugin* plugin, QSettings* qs = 0 ); + void readMainSettings(QSettings *qs, bool workspaceDiffOnly); + void saveMainSettings(QSettings *qs); + void readSettings(IConfigurablePlugin *plugin, QSettings *qs = 0); + void saveSettings(IConfigurablePlugin *plugin, QSettings *qs = 0); void deleteSettings(); QString resourcePath() const; @@ -92,7 +90,6 @@ private: MainWindow *m_mainwindow; friend class MainWindow; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.cpp b/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.cpp index 511077ee7..fd5a058cf 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,8 +38,7 @@ using namespace Core::Internal; CorePlugin::CorePlugin() : m_mainWindow(new MainWindow) -{ -} +{} CorePlugin::~CorePlugin() { @@ -61,7 +60,7 @@ void CorePlugin::extensionsInitialized() m_mainWindow->extensionsInitialized(); } -void CorePlugin::remoteArgument(const QString& arg) +void CorePlugin::remoteArgument(const QString & arg) { // An empty argument is sent to trigger activation // of the window via QtSingleApplication. It should be @@ -69,7 +68,7 @@ void CorePlugin::remoteArgument(const QString& arg) if (arg.isEmpty()) { m_mainWindow->activateWindow(); } else { - //m_mainWindow->openFiles(QStringList(arg)); + // m_mainWindow->openFiles(QStringList(arg)); } } @@ -78,4 +77,4 @@ void CorePlugin::shutdown() m_mainWindow->shutdown(); } -Q_EXPORT_PLUGIN2(Core,CorePlugin) +Q_EXPORT_PLUGIN2(Core, CorePlugin) diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.h b/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.h index ccd80c69c..8c4431fee 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,11 +33,9 @@ namespace Core { namespace Internal { - class MainWindow; -class CorePlugin : public ExtensionSystem::IPlugin -{ +class CorePlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -49,12 +47,11 @@ public: virtual void shutdown(); public slots: - void remoteArgument(const QString&); + void remoteArgument(const QString &); private: MainWindow *m_mainWindow; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.cpp b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.cpp index 93ad2eb07..91ec41530 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.cpp @@ -11,35 +11,35 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "ioptionspage.h" /*! - \class Core::IOptionsPage - \mainclass - \brief The IOptionsPage is an interface for providing options pages. + \class Core::IOptionsPage + \mainclass + \brief The IOptionsPage is an interface for providing options pages. - Guidelines for implementing: - \list - \o id() is an id used for filtering when calling ICore:: showOptionsDialog() - \o trName() is the (translated) name for display. - \o category() is the category used for filtering when calling ICore:: showOptionsDialog() - \o trCategory() is the translated category - \o apply() is called to store the settings. It should detect if any changes have been + Guidelines for implementing: + \list + \o id() is an id used for filtering when calling ICore:: showOptionsDialog() + \o trName() is the (translated) name for display. + \o category() is the category used for filtering when calling ICore:: showOptionsDialog() + \o trCategory() is the translated category + \o apply() is called to store the settings. It should detect if any changes have been made and store those. - \endlist -*/ + \endlist + */ diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.h b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.h index cd87f4bba..1060d072d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,9 +39,7 @@ class QWidget; QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT IOptionsPage : public QObject -{ +class CORE_EXPORT IOptionsPage : public QObject { Q_OBJECT public: IOptionsPage(QObject *parent = 0) : @@ -49,24 +47,41 @@ public: m_icon(QIcon()) {} virtual ~IOptionsPage() {} - void setIcon(QIcon icon) { m_icon = icon; } - QIcon icon() { return m_icon; } + void setIcon(QIcon icon) + { + m_icon = icon; + } + QIcon icon() + { + return m_icon; + } /* gadget options pages can leave these 4 functions as is, since they are decorated by UAVGadgetOptionsPageDecorator, all other options pages must override these */ - virtual QString id() const { return ""; }; - virtual QString trName() const { return ""; }; - virtual QString category() const { return "DefaultCategory"; }; - virtual QString trCategory() const { return "DefaultCategory"; }; + virtual QString id() const + { + return ""; + }; + virtual QString trName() const + { + return ""; + }; + virtual QString category() const + { + return "DefaultCategory"; + }; + virtual QString trCategory() const + { + return "DefaultCategory"; + }; virtual QWidget *createPage(QWidget *parent) = 0; - virtual void apply() = 0; + virtual void apply() = 0; virtual void finish() = 0; private: QIcon m_icon; }; - } // namespace Core #endif // IOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.cpp b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.cpp index 122927f63..202f3996d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -57,7 +57,7 @@ \endcode \sa Core::BaseFileWizard \sa Core::StandardFileWizard -*/ + */ /*! \enum Core::IWizard::Kind @@ -70,51 +70,51 @@ The wizard creates a new class (e.g. source+header files). \value ProjectWizard The wizard creates a new project. -*/ + */ /*! \fn IWizard::IWizard(QObject *parent) \internal -*/ + */ /*! \fn IWizard::~IWizard() \internal -*/ + */ /*! \fn Kind IWizard::kind() const Returns what kind of objects are created by the wizard. \sa Kind -*/ + */ /*! \fn QIcon IWizard::icon() const Returns an icon to show in the wizard selection dialog. -*/ + */ /*! \fn QString IWizard::description() const Returns a translated description to show when this wizard is selected in the dialog. -*/ + */ /*! \fn QString IWizard::name() const Returns the translated name of the wizard, how it should appear in the dialog. -*/ + */ /*! \fn QString IWizard::category() const Returns a category ID to add the wizard to. -*/ + */ /*! \fn QString IWizard::trCategory() const Returns the translated string of the category, how it should appear in the dialog. -*/ + */ /*! \fn QStringList IWizard::runWizard(const QString &path, QWidget *parent) @@ -124,25 +124,28 @@ created. The wizard should fill this in its path selection elements as a default path. Returns a list of files (absolute paths) that have been created, if any. -*/ + */ using namespace Core; /* A utility to find all wizards supporting a view mode and matching a predicate */ template - QList findWizards(Predicate predicate) +QList findWizards(Predicate predicate) { // Filter all wizards - const QList allWizards = IWizard::allWizards(); - QList rc; - const QList::const_iterator cend = allWizards.constEnd(); - for (QList::const_iterator it = allWizards.constBegin(); it != cend; ++it) - if (predicate(*(*it))) + const QList allWizards = IWizard::allWizards(); + + QList rc; + const QList::const_iterator cend = allWizards.constEnd(); + for (QList::const_iterator it = allWizards.constBegin(); it != cend; ++it) { + if (predicate(*(*it))) { rc.push_back(*it); + } + } return rc; } -QList IWizard::allWizards() +QList IWizard::allWizards() { return ExtensionSystem::PluginManager::instance()->getObjects(); } @@ -152,13 +155,15 @@ QList IWizard::allWizards() class WizardKindPredicate { public: WizardKindPredicate(IWizard::Kind kind) : m_kind(kind) {} - bool operator()(const IWizard &w) const { return w.kind() == m_kind; } + bool operator()(const IWizard &w) const + { + return w.kind() == m_kind; + } private: const IWizard::Kind m_kind; }; -QList IWizard::wizardsOfKind(Kind kind) +QList IWizard::wizardsOfKind(Kind kind) { return findWizards(WizardKindPredicate(kind)); } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.h b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.h index 0ce03b8fc..b46e5b768 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,10 +37,8 @@ class QIcon; QT_END_NAMESPACE namespace Core { - class CORE_EXPORT IWizard - : public QObject -{ + : public QObject { Q_OBJECT public: enum Kind { @@ -52,22 +50,21 @@ public: IWizard(QObject *parent = 0) : QObject(parent) {} virtual ~IWizard() {} - virtual Kind kind() const = 0; - virtual QIcon icon() const = 0; + virtual Kind kind() const = 0; + virtual QIcon icon() const = 0; virtual QString description() const = 0; - virtual QString name() const = 0; + virtual QString name() const = 0; - virtual QString category() const = 0; - virtual QString trCategory() const = 0; + virtual QString category() const = 0; + virtual QString trCategory() const = 0; virtual QStringList runWizard(const QString &path, QWidget *parent) = 0; // Utility to find all registered wizards - static QList allWizards(); + static QList allWizards(); // Utility to find all registered wizards of a certain kind - static QList wizardsOfKind(Kind kind); + static QList wizardsOfKind(Kind kind); }; - } // namespace Core #endif // IWIZARD_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.cpp b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.cpp index fc45184d5..fd772b0bb 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,7 +32,7 @@ #include "icore.h" #include "coreplugin/uavgadgetinstancemanager.h" #include "coreplugin/uavgadgetoptionspagedecorator.h" -//#include "coreimpl.h" +// #include "coreimpl.h" #include #include @@ -41,11 +41,11 @@ #include namespace { - struct PageData { - int index; - QString category; - QString id; - }; +struct PageData { + int index; + QString category; + QString id; +}; } Q_DECLARE_METATYPE(::PageData) @@ -56,23 +56,27 @@ using namespace Core::Internal; // Helpers to sort by category. id bool optionsPageLessThan(const IOptionsPage *p1, const IOptionsPage *p2) { - const UAVGadgetOptionsPageDecorator *gp1 = qobject_cast(p1); - const UAVGadgetOptionsPageDecorator *gp2 = qobject_cast(p2); - if (gp1 && (gp2 == NULL)) + const UAVGadgetOptionsPageDecorator *gp1 = qobject_cast(p1); + const UAVGadgetOptionsPageDecorator *gp2 = qobject_cast(p2); + + if (gp1 && (gp2 == NULL)) { return false; + } - if (gp2 && (gp1 == NULL)) + if (gp2 && (gp1 == NULL)) { return true; + } - if (const int cc = QString::localeAwareCompare(p1->trCategory(), p2->trCategory())) + if (const int cc = QString::localeAwareCompare(p1->trCategory(), p2->trCategory())) { return cc < 0; + } return QString::localeAwareCompare(p1->trName(), p2->trName()) < 0; } -static inline QList sortedOptionsPages() +static inline QList sortedOptionsPages() { - QList rc = ExtensionSystem::PluginManager::instance()->getObjects(); + QList rc = ExtensionSystem::PluginManager::instance()->getObjects(); qStableSort(rc.begin(), rc.end(), optionsPageLessThan); return rc; } @@ -93,15 +97,15 @@ SettingsDialog::SettingsDialog(QWidget *parent, const QString &categoryId, const // restore last displayed category and page // this is done only if no category or page was provided through the constructor QString initialCategory = categoryId; - QString initialPage = pageId; + QString initialPage = pageId; qDebug() << "SettingsDialog constructor initial category:" << initialCategory << ", initial page:" << initialPage; if (initialCategory.isEmpty() && initialPage.isEmpty()) { initialCategory = settings->value("LastPreferenceCategory", QVariant(QString())).toString(); - initialPage = settings->value("LastPreferencePage", QVariant(QString())).toString(); + initialPage = settings->value("LastPreferencePage", QVariant(QString())).toString(); qDebug() << "SettingsDialog settings initial category:" << initialCategory << ", initial page: " << initialPage; } // restore window size - int windowWidth = settings->value("SettingsWindowWidth", 0).toInt(); + int windowWidth = settings->value("SettingsWindowWidth", 0).toInt(); int windowHeight = settings->value("SettingsWindowHeight", 0).toInt(); qDebug() << "SettingsDialog window width :" << windowWidth << ", height:" << windowHeight; if (windowWidth > 0 && windowHeight > 0) { @@ -112,31 +116,32 @@ SettingsDialog::SettingsDialog(QWidget *parent, const QString &categoryId, const buttonBox->button(QDialogButtonBox::Ok)->setDefault(true); connect(buttonBox->button(QDialogButtonBox::Apply), SIGNAL(clicked()), this, SLOT(apply())); - + m_instanceManager = Core::ICore::instance()->uavGadgetInstanceManager(); - - connect(this, SIGNAL(settingsDialogShown(Core::Internal::SettingsDialog*)), m_instanceManager, SLOT(settingsDialogShown(Core::Internal::SettingsDialog*))); + + connect(this, SIGNAL(settingsDialogShown(Core::Internal::SettingsDialog *)), m_instanceManager, SLOT(settingsDialogShown(Core::Internal::SettingsDialog *))); connect(this, SIGNAL(settingsDialogRemoved()), m_instanceManager, SLOT(settingsDialogRemoved())); connect(this, SIGNAL(categoryItemSelected()), this, SLOT(categoryItemSelectedShowChildInstead()), Qt::QueuedConnection); splitter->setCollapsible(0, false); splitter->setCollapsible(1, false); pageTree->header()->setVisible(false); -// pageTree->setIconSize(QSize(24, 24)); +// pageTree->setIconSize(QSize(24, 24)); connect(pageTree, SIGNAL(currentItemChanged(QTreeWidgetItem *, QTreeWidgetItem *)), - this, SLOT(pageSelected())); + this, SLOT(pageSelected())); QMap categories; - QList pages = sortedOptionsPages(); + QList pages = sortedOptionsPages(); int index = 0; bool firstUavGadgetOptionsPageFound = false; QTreeWidgetItem *initialItem = 0; - foreach (IOptionsPage *page, pages) { + foreach(IOptionsPage * page, pages) { PageData pageData; - pageData.index = index; + + pageData.index = index; pageData.category = page->category(); pageData.id = page->id(); @@ -144,14 +149,14 @@ SettingsDialog::SettingsDialog(QWidget *parent, const QString &categoryId, const item->setText(0, page->trName()); item->setData(0, Qt::UserRole, qVariantFromValue(pageData)); - QString trCategories = page->trCategory(); + QString trCategories = page->trCategory(); QString currentCategory = page->category(); QTreeWidgetItem *categoryItem; if (!categories.contains(currentCategory)) { // Above the first gadget option we insert a separator if (!firstUavGadgetOptionsPageFound) { - UAVGadgetOptionsPageDecorator *pd = qobject_cast(page); + UAVGadgetOptionsPageDecorator *pd = qobject_cast(page); if (pd) { firstUavGadgetOptionsPageFound = true; QTreeWidgetItem *separator = new QTreeWidgetItem(pageTree); @@ -168,8 +173,8 @@ SettingsDialog::SettingsDialog(QWidget *parent, const QString &categoryId, const QList *categoryItemList = m_categoryItemsMap.value(currentCategory); if (!categoryItemList) { - categoryItemList = new QList(); - m_categoryItemsMap.insert(currentCategory, categoryItemList); + categoryItemList = new QList(); + m_categoryItemsMap.insert(currentCategory, categoryItemList); } categoryItemList->append(item); @@ -178,7 +183,7 @@ SettingsDialog::SettingsDialog(QWidget *parent, const QString &categoryId, const // creating all option pages upfront is slow, so we create place holder widgets instead // the real option page widget will be created later when the user selects it // the place holder is a QLabel and we assume that no option page will be a QLabel... - QLabel * placeholderWidget = new QLabel(stackedPages); + QLabel *placeholderWidget = new QLabel(stackedPages); stackedPages->addWidget(placeholderWidget); if (page->id() == initialPage && currentCategory == initialCategory) { @@ -191,8 +196,9 @@ SettingsDialog::SettingsDialog(QWidget *parent, const QString &categoryId, const foreach(QString category, m_categoryItemsMap.keys()) { QList *categoryItemList = m_categoryItemsMap.value(category); if (categoryItemList->size() > 1) { - foreach (QTreeWidgetItem *item, *categoryItemList) { + foreach(QTreeWidgetItem * item, *categoryItemList) { QTreeWidgetItem *categoryItem = categories.value(category); + categoryItem->addChild(item); } } @@ -223,7 +229,7 @@ SettingsDialog::~SettingsDialog() } // delete place holders for (int i = 0; i < stackedPages->count(); i++) { - QLabel * widget = dynamic_cast(stackedPages->widget(i)); + QLabel *widget = dynamic_cast(stackedPages->widget(i)); if (widget) { delete widget; } @@ -233,15 +239,17 @@ SettingsDialog::~SettingsDialog() void SettingsDialog::pageSelected() { QTreeWidgetItem *item = pageTree->currentItem(); - if (!item) + + if (!item) { return; + } PageData data = item->data(0, Qt::UserRole).value(); - int index = data.index; + int index = data.index; m_currentCategory = data.category; - m_currentPage = data.id; + m_currentPage = data.id; // check if we are looking at a place holder or not - QWidget *widget = dynamic_cast(stackedPages->widget(index)); + QWidget *widget = dynamic_cast(stackedPages->widget(index)); if (widget) { // place holder found, get rid of it... stackedPages->removeWidget(widget); @@ -261,6 +269,7 @@ void SettingsDialog::pageSelected() void SettingsDialog::categoryItemSelectedShowChildInstead() { QTreeWidgetItem *item = pageTree->currentItem(); + item->setExpanded(true); pageTree->setCurrentItem(item->child(0), 0, QItemSelectionModel::SelectCurrent); } @@ -268,8 +277,9 @@ void SettingsDialog::categoryItemSelectedShowChildInstead() void SettingsDialog::deletePage() { QTreeWidgetItem *item = pageTree->currentItem(); - PageData data = item->data(0, Qt::UserRole).value(); + PageData data = item->data(0, Qt::UserRole).value(); QString category = data.category; + QList *categoryItemList = m_categoryItemsMap.value(category); QTreeWidgetItem *parentItem = item->parent(); parentItem->removeChild(item); @@ -280,10 +290,11 @@ void SettingsDialog::deletePage() pageSelected(); } -void SettingsDialog::insertPage(IOptionsPage* page) +void SettingsDialog::insertPage(IOptionsPage *page) { PageData pageData; - pageData.index = m_pages.count(); + + pageData.index = m_pages.count(); pageData.category = page->category(); pageData.id = page->id(); @@ -296,8 +307,9 @@ void SettingsDialog::insertPage(IOptionsPage* page) break; } } - if (!categoryItem) + if (!categoryItem) { return; + } // If this category has no child right now // we need to add the "default child" @@ -324,6 +336,7 @@ void SettingsDialog::insertPage(IOptionsPage* page) void SettingsDialog::updateText(QString text) { QTreeWidgetItem *item = pageTree->currentItem(); + item->setText(0, text); } @@ -337,9 +350,9 @@ void SettingsDialog::accept() { m_applied = true; for (int i = 0; i < m_pages.size(); i++) { - QWidget * widget = dynamic_cast(stackedPages->widget(i)); + QWidget *widget = dynamic_cast(stackedPages->widget(i)); if (!widget) { - IOptionsPage * page = m_pages.at(i); + IOptionsPage *page = m_pages.at(i); page->apply(); page->finish(); } @@ -350,9 +363,9 @@ void SettingsDialog::accept() void SettingsDialog::reject() { for (int i = 0; i < m_pages.size(); i++) { - QWidget * widget = dynamic_cast(stackedPages->widget(i)); + QWidget *widget = dynamic_cast(stackedPages->widget(i)); if (!widget) { - IOptionsPage * page = m_pages.at(i); + IOptionsPage *page = m_pages.at(i); page->finish(); } } @@ -362,9 +375,9 @@ void SettingsDialog::reject() void SettingsDialog::apply() { for (int i = 0; i < m_pages.size(); i++) { - QWidget * widget = dynamic_cast(stackedPages->widget(i)); + QWidget *widget = dynamic_cast(stackedPages->widget(i)); if (!widget) { - IOptionsPage * page = m_pages.at(i); + IOptionsPage *page = m_pages.at(i); page->apply(); } } @@ -383,6 +396,7 @@ bool SettingsDialog::execDialog() void SettingsDialog::done(int val) { QSettings *settings = ICore::instance()->settings(); + settings->setValue("General/LastPreferenceCategory", m_currentCategory); settings->setValue("General/LastPreferencePage", m_currentPage); settings->setValue("General/SettingsWindowWidth", this->width()); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.h b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.h index 5ce5d424b..ade61b525 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,13 +36,10 @@ #include "coreplugin/dialogs/ioptionspage.h" namespace Core { - class UAVGadgetInstanceManager; namespace Internal { - -class SettingsDialog : public QDialog, public ::Ui::SettingsDialog -{ +class SettingsDialog : public QDialog, public::Ui::SettingsDialog { Q_OBJECT public: @@ -57,7 +54,7 @@ public: void disableApplyOk(bool disable); signals: - void settingsDialogShown(Core::Internal::SettingsDialog*); + void settingsDialogShown(Core::Internal::SettingsDialog *); void settingsDialogRemoved(); void categoryItemSelected(); @@ -72,14 +69,13 @@ private slots: void categoryItemSelectedShowChildInstead(); private: - QList m_pages; + QList m_pages; QMap *> m_categoryItemsMap; UAVGadgetInstanceManager *m_instanceManager; bool m_applied; QString m_currentCategory; QString m_currentPage; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.cpp b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.cpp index fb4bce83c..6566cf9bd 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,19 +44,17 @@ #include #include -Q_DECLARE_METATYPE(Core::Internal::ShortcutItem*) +Q_DECLARE_METATYPE(Core::Internal::ShortcutItem *) using namespace Core; using namespace Core::Internal; ShortcutSettings::ShortcutSettings(QObject *parent) : IOptionsPage(parent) -{ -} +{} ShortcutSettings::~ShortcutSettings() -{ -} +{} // IOptionsPage @@ -84,7 +82,7 @@ QWidget *ShortcutSettings::createPage(QWidget *parent) { m_keyNum = m_key[0] = m_key[1] = m_key[2] = m_key[3] = 0; - m_page = new Ui_ShortcutSettings(); + m_page = new Ui_ShortcutSettings(); QWidget *w = new QWidget(parent); m_page->setupUi(w); @@ -92,15 +90,15 @@ QWidget *ShortcutSettings::createPage(QWidget *parent) m_page->shortcutEdit->installEventFilter(this); connect(m_page->resetButton, SIGNAL(clicked()), - this, SLOT(resetKeySequence())); + this, SLOT(resetKeySequence())); connect(m_page->removeButton, SIGNAL(clicked()), - this, SLOT(removeKeySequence())); + this, SLOT(removeKeySequence())); connect(m_page->exportButton, SIGNAL(clicked()), - this, SLOT(exportAction())); + this, SLOT(exportAction())); connect(m_page->importButton, SIGNAL(clicked()), - this, SLOT(importAction())); + this, SLOT(importAction())); connect(m_page->defaultButton, SIGNAL(clicked()), - this, SLOT(defaultAction())); + this, SLOT(defaultAction())); initialize(); @@ -108,7 +106,7 @@ QWidget *ShortcutSettings::createPage(QWidget *parent) connect(m_page->filterEdit, SIGNAL(textChanged(QString)), this, SLOT(filterChanged(QString))); connect(m_page->commandList, SIGNAL(currentItemChanged(QTreeWidgetItem *, QTreeWidgetItem *)), - this, SLOT(commandChanged(QTreeWidgetItem *))); + this, SLOT(commandChanged(QTreeWidgetItem *))); connect(m_page->shortcutEdit, SIGNAL(textChanged(QString)), this, SLOT(keyChanged())); new Utils::TreeWidgetColumnStretcher(m_page->commandList, 1); @@ -120,8 +118,8 @@ QWidget *ShortcutSettings::createPage(QWidget *parent) void ShortcutSettings::apply() { - foreach (ShortcutItem *item, m_scitems) - item->m_cmd->setKeySequence(item->m_key); + foreach(ShortcutItem * item, m_scitems) + item->m_cmd->setKeySequence(item->m_key); } void ShortcutSettings::finish() @@ -136,16 +134,17 @@ bool ShortcutSettings::eventFilter(QObject *o, QEvent *e) { Q_UNUSED(o) - if ( e->type() == QEvent::KeyPress ) { - QKeyEvent *k = static_cast(e); + if (e->type() == QEvent::KeyPress) { + QKeyEvent *k = static_cast(e); handleKeyEvent(k); return true; } - if ( e->type() == QEvent::Shortcut || - e->type() == QEvent::ShortcutOverride || - e->type() == QEvent::KeyRelease ) + if (e->type() == QEvent::Shortcut || + e->type() == QEvent::ShortcutOverride || + e->type() == QEvent::KeyRelease) { return true; + } return false; } @@ -164,7 +163,7 @@ void ShortcutSettings::commandChanged(QTreeWidgetItem *current) void ShortcutSettings::filterChanged(const QString &f) { - for (int i=0; icommandList->topLevelItemCount(); ++i) { + for (int i = 0; i < m_page->commandList->topLevelItemCount(); ++i) { QTreeWidgetItem *item = m_page->commandList->topLevelItem(i); item->setHidden(filter(f, item)); } @@ -173,6 +172,7 @@ void ShortcutSettings::filterChanged(const QString &f) void ShortcutSettings::keyChanged() { QTreeWidgetItem *current = m_page->commandList->currentItem(); + if (current && current->data(0, Qt::UserRole).isValid()) { ShortcutItem *scitem = qVariantValue(current->data(0, Qt::UserRole)); scitem->m_key = QKeySequence(m_key[0], m_key[1], m_key[2], m_key[3]); @@ -193,11 +193,13 @@ void ShortcutSettings::setKeySequence(const QKeySequence &key) bool ShortcutSettings::filter(const QString &f, const QTreeWidgetItem *item) { if (item->childCount() == 0) { - if (f.isEmpty()) + if (f.isEmpty()) { return false; + } for (int i = 0; i < item->columnCount(); ++i) { - if (item->text(i).contains(f, Qt::CaseInsensitive)) + if (item->text(i).contains(f, Qt::CaseInsensitive)) { return false; + } } return true; } @@ -218,6 +220,7 @@ bool ShortcutSettings::filter(const QString &f, const QTreeWidgetItem *item) void ShortcutSettings::resetKeySequence() { QTreeWidgetItem *current = m_page->commandList->currentItem(); + if (current && current->data(0, Qt::UserRole).isValid()) { ShortcutItem *scitem = qVariantValue(current->data(0, Qt::UserRole)); setKeySequence(scitem->m_cmd->defaultKeySequence()); @@ -235,19 +238,22 @@ void ShortcutSettings::importAction() UniqueIDManager *uidm = UniqueIDManager::instance(); QString fileName = QFileDialog::getOpenFileName(0, tr("Import Keyboard Mapping Scheme"), - ICore::instance()->resourcePath() + "/schemes/", - tr("Keyboard Mapping Scheme (*.kms)")); + ICore::instance()->resourcePath() + "/schemes/", + tr("Keyboard Mapping Scheme (*.kms)")); + if (!fileName.isEmpty()) { CommandsFile cf(fileName); QMap mapping = cf.importCommands(); - foreach (ShortcutItem *item, m_scitems) { + foreach(ShortcutItem * item, m_scitems) { QString sid = uidm->stringForUniqueIdentifier(item->m_cmd->id()); + if (mapping.contains(sid)) { item->m_key = mapping.value(sid); item->m_item->setText(2, item->m_key); - if (item->m_item == m_page->commandList->currentItem()) + if (item->m_item == m_page->commandList->currentItem()) { commandChanged(item->m_item); + } } } } @@ -255,11 +261,12 @@ void ShortcutSettings::importAction() void ShortcutSettings::defaultAction() { - foreach (ShortcutItem *item, m_scitems) { + foreach(ShortcutItem * item, m_scitems) { item->m_key = item->m_cmd->defaultKeySequence(); item->m_item->setText(2, item->m_key); - if (item->m_item == m_page->commandList->currentItem()) + if (item->m_item == m_page->commandList->currentItem()) { commandChanged(item->m_item); + } } } @@ -283,17 +290,19 @@ void ShortcutSettings::initialize() m_am = ActionManagerPrivate::instance(); UniqueIDManager *uidm = UniqueIDManager::instance(); - foreach (Command *c, m_am->commands()) { - if (c->hasAttribute(Command::CA_NonConfigureable)) + foreach(Command * c, m_am->commands()) { + if (c->hasAttribute(Command::CA_NonConfigureable)) { continue; - if (c->action() && c->action()->isSeparator()) + } + if (c->action() && c->action()->isSeparator()) { continue; + } QTreeWidgetItem *item = 0; ShortcutItem *s = new ShortcutItem; m_scitems << s; - item = new QTreeWidgetItem(m_page->commandList); - s->m_cmd = c; + item = new QTreeWidgetItem(m_page->commandList); + s->m_cmd = c; s->m_item = item; item->setText(0, uidm->stringForUniqueIdentifier(c->id())); @@ -316,29 +325,31 @@ void ShortcutSettings::initialize() void ShortcutSettings::handleKeyEvent(QKeyEvent *e) { int nextKey = e->key(); - if ( m_keyNum > 3 || - nextKey == Qt::Key_Control || - nextKey == Qt::Key_Shift || - nextKey == Qt::Key_Meta || - nextKey == Qt::Key_Alt ) - return; + + if (m_keyNum > 3 || + nextKey == Qt::Key_Control || + nextKey == Qt::Key_Shift || + nextKey == Qt::Key_Meta || + nextKey == Qt::Key_Alt) { + return; + } nextKey |= translateModifiers(e->modifiers(), e->text()); switch (m_keyNum) { - case 0: - m_key[0] = nextKey; - break; - case 1: - m_key[1] = nextKey; - break; - case 2: - m_key[2] = nextKey; - break; - case 3: - m_key[3] = nextKey; - break; - default: - break; + case 0: + m_key[0] = nextKey; + break; + case 1: + m_key[1] = nextKey; + break; + case 2: + m_key[2] = nextKey; + break; + case 3: + m_key[3] = nextKey; + break; + default: + break; } m_keyNum++; QKeySequence ks(m_key[0], m_key[1], m_key[2], m_key[3]); @@ -350,18 +361,23 @@ int ShortcutSettings::translateModifiers(Qt::KeyboardModifiers state, const QString &text) { int result = 0; + // The shift modifier only counts when it is not used to type a symbol // that is only reachable using the shift key anyway if ((state & Qt::ShiftModifier) && (text.size() == 0 || !text.at(0).isPrint() || text.at(0).isLetter() - || text.at(0).isSpace())) + || text.at(0).isSpace())) { result |= Qt::SHIFT; - if (state & Qt::ControlModifier) + } + if (state & Qt::ControlModifier) { result |= Qt::CTRL; - if (state & Qt::MetaModifier) + } + if (state & Qt::MetaModifier) { result |= Qt::META; - if (state & Qt::AltModifier) + } + if (state & Qt::AltModifier) { result |= Qt::ALT; + } return result; } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.h b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.h index 762e397fd..7aac4c6d9 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,24 +41,20 @@ class Ui_ShortcutSettings; QT_END_NAMESPACE namespace Core { - class Command; namespace Internal { - class ActionManagerPrivate; class MainWindow; -struct ShortcutItem -{ +struct ShortcutItem { Command *m_cmd; QKeySequence m_key; QTreeWidgetItem *m_item; }; -class ShortcutSettings : public Core::IOptionsPage -{ +class ShortcutSettings : public Core::IOptionsPage { Q_OBJECT public: @@ -101,7 +97,6 @@ private: int m_key[4], m_keyNum; Ui_ShortcutSettings *m_page; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.cpp b/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.cpp index 0b11fee49..6b2fc4179 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,8 +37,7 @@ using namespace Core::Internal; EventFilteringMainWindow::EventFilteringMainWindow() -{ -} +{} #ifdef Q_OS_WIN bool EventFilteringMainWindow::winEvent(MSG *msg, long *result) diff --git a/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.h b/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.h index 16ca657f2..3f0ddef43 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,14 +33,12 @@ namespace Core { namespace Internal { - /*! * This class only exists because we can't include windows.h in mainwindow.cpp * because windows defines an IContext... */ -class EventFilteringMainWindow : public QMainWindow -{ +class EventFilteringMainWindow : public QMainWindow { Q_OBJECT public: EventFilteringMainWindow(); @@ -52,9 +50,7 @@ protected: signals: void deviceChange(); - }; - } // Internal } // Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.cpp b/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.cpp index fea7d26fd..f6ce52c7a 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,15 +38,15 @@ using namespace Core; using namespace Internal; -static const char* const svgIdButtonBase = "ButtonBase"; -static const char* const svgIdButtonNormalBase = "ButtonNormalBase"; -static const char* const svgIdButtonNormalOverlay = "ButtonNormalOverlay"; -static const char* const svgIdButtonPressedBase = "ButtonPressedBase"; -static const char* const svgIdButtonPressedOverlay = "ButtonPressedOverlay"; -static const char* const svgIdButtonDisabledOverlay = "ButtonDisabledOverlay"; -static const char* const svgIdButtonHoverOverlay = "ButtonHoverOverlay"; +static const char *const svgIdButtonBase = "ButtonBase"; +static const char *const svgIdButtonNormalBase = "ButtonNormalBase"; +static const char *const svgIdButtonNormalOverlay = "ButtonNormalOverlay"; +static const char *const svgIdButtonPressedBase = "ButtonPressedBase"; +static const char *const svgIdButtonPressedOverlay = "ButtonPressedOverlay"; +static const char *const svgIdButtonDisabledOverlay = "ButtonDisabledOverlay"; +static const char *const svgIdButtonHoverOverlay = "ButtonHoverOverlay"; -static const char* const elementsSvgIds[] = { +static const char *const elementsSvgIds[] = { svgIdButtonBase, svgIdButtonNormalBase, svgIdButtonNormalOverlay, @@ -59,9 +59,10 @@ static const char* const elementsSvgIds[] = { const QMap &buttonElementsMap() { static QMap result; + if (result.isEmpty()) { QSvgRenderer renderer(QLatin1String(":/fancyactionbar/images/fancytoolbutton.svg")); - for (size_t i = 0; i < sizeof(elementsSvgIds)/sizeof(elementsSvgIds[0]); i++) { + for (size_t i = 0; i < sizeof(elementsSvgIds) / sizeof(elementsSvgIds[0]); i++) { QString elementId(elementsSvgIds[i]); QPicture elementPicture; QPainter elementPainter(&elementPicture); @@ -93,19 +94,20 @@ void FancyToolButton::paintEvent(QPaintEvent *event) p.drawPicture(0, 0, m_buttonElements.value(svgIdButtonBase)); p.drawPicture(0, 0, m_buttonElements.value(isDown() ? svgIdButtonPressedBase : svgIdButtonNormalBase)); #ifndef Q_WS_MAC // Mac UIs usually don't hover - if (underMouse() && isEnabled()) + if (underMouse() && isEnabled()) { p.drawPicture(0, 0, m_buttonElements.value(svgIdButtonHoverOverlay)); + } #endif - if (scale < 1) + if (scale < 1) { p.restore(); + } if (!icon().isNull()) { icon().paint(&p, rect()); } else { const int margin = 4; p.drawText(rect().adjusted(margin, margin, -margin, -margin), Qt::AlignCenter | Qt::TextWordWrap, text()); - } if (scale < 1) { @@ -150,29 +152,31 @@ FancyActionBar::FancyActionBar(QWidget *parent) void FancyActionBar::insertAction(int index, QAction *action, QMenu *menu) { FancyToolButton *toolButton = new FancyToolButton(this); + toolButton->setDefaultAction(action); if (menu) { toolButton->setMenu(menu); toolButton->setPopupMode(QToolButton::DelayedPopup); // execute action also if a context menu item is select - connect(toolButton, SIGNAL(triggered(QAction*)), - this, SLOT(toolButtonContextMenuActionTriggered(QAction*)), + connect(toolButton, SIGNAL(triggered(QAction *)), + this, SLOT(toolButtonContextMenuActionTriggered(QAction *)), Qt::QueuedConnection); } m_actionsLayout->insertWidget(index, toolButton); } /* - This slot is invoked when a context menu action of a tool button is triggered. - In this case we also want to trigger the default action of the button. + This slot is invoked when a context menu action of a tool button is triggered. + In this case we also want to trigger the default action of the button. - This allows the user e.g. to select and run a specific run configuration with one click. - */ -void FancyActionBar::toolButtonContextMenuActionTriggered(QAction* action) + This allows the user e.g. to select and run a specific run configuration with one click. + */ +void FancyActionBar::toolButtonContextMenuActionTriggered(QAction *action) { - if (QToolButton *button = qobject_cast(sender())) { - if (action != button->defaultAction()) + if (QToolButton * button = qobject_cast(sender())) { + if (action != button->defaultAction()) { button->defaultAction()->trigger(); + } } } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.h b/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.h index 1b61edba2..147474ef4 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.h @@ -4,7 +4,7 @@ * @file fancyactionbar.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup CorePlugin Core Plugin @@ -12,18 +12,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,9 +40,7 @@ QT_END_NAMESPACE namespace Core { namespace Internal { - -class FancyToolButton : public QToolButton -{ +class FancyToolButton : public QToolButton { public: FancyToolButton(QWidget *parent = 0); @@ -54,8 +52,7 @@ private: const QMap &m_buttonElements; }; -class FancyActionBar : public QWidget -{ +class FancyActionBar : public QWidget { Q_OBJECT public: @@ -65,11 +62,10 @@ public: void insertAction(int index, QAction *action, QMenu *menu = 0); private slots: - void toolButtonContextMenuActionTriggered(QAction*); + void toolButtonContextMenuActionTriggered(QAction *); private: QVBoxLayout *m_actionsLayout; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.cpp b/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.cpp index 1a4ca10a5..3e82d40c5 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -47,15 +47,15 @@ using namespace Core; using namespace Internal; -const int FancyTabBar::m_rounding = 22; +const int FancyTabBar::m_rounding = 22; const int FancyTabBar::m_textPadding = 4; FancyTabBar::FancyTabBar(QWidget *parent, bool isVertical) : QWidget(parent) { - verticalTabs = isVertical; + verticalTabs = isVertical; setIconSize(16); - m_hoverIndex = -1; + m_hoverIndex = -1; m_currentIndex = 0; if (isVertical) { setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); @@ -81,11 +81,12 @@ FancyTabBar::~FancyTabBar() QSize FancyTabBar::tabSizeHint(bool minimum) const { QFont boldFont(font()); + boldFont.setPointSizeF(Utils::StyleHelper::sidebarFontSize()); boldFont.setBold(true); QFontMetrics fm(boldFont); - int spacing = 6; - int width = 90 + spacing + 2; + int spacing = 6; + int width = 90 + spacing + 2; int iconHeight = minimum ? 0 : iconSize; return QSize(width, iconHeight + spacing + fm.height()); @@ -96,9 +97,11 @@ void FancyTabBar::paintEvent(QPaintEvent *event) Q_UNUSED(event) QPainter p(this); - for (int i = 0; i < count(); ++i) - if (i != currentIndex()) + for (int i = 0; i < count(); ++i) { + if (i != currentIndex()) { paintTab(&p, i); + } + } // paint active tab last, since it overlaps the neighbors paintTab(&p, currentIndex()); @@ -120,9 +123,9 @@ void FancyTabBar::mouseMoveEvent(QMouseEvent *e) m_hoverControl.stop(); m_hoverIndex = newHover; update(m_hoverRect); - m_hoverRect = QRect(); + m_hoverRect = QRect(); - if (m_hoverIndex >=0) { + if (m_hoverIndex >= 0) { QRect oldHoverRect = m_hoverRect; m_hoverRect = tabRect(m_hoverIndex); m_hoverControl.start(); @@ -136,7 +139,7 @@ bool FancyTabBar::event(QEvent *event) if (m_hoverIndex >= 0 && m_hoverIndex < m_tabs.count()) { QString tt = tabToolTip(m_hoverIndex); if (!tt.isEmpty()) { - QToolTip::showText(static_cast(event)->globalPos(), tt, this); + QToolTip::showText(static_cast(event)->globalPos(), tt, this); return true; } } @@ -153,7 +156,7 @@ void FancyTabBar::updateHover() void FancyTabBar::enterEvent(QEvent *e) { Q_UNUSED(e) - m_hoverRect = QRect(); + m_hoverRect = QRect(); m_hoverIndex = -1; } @@ -164,7 +167,7 @@ void FancyTabBar::leaveEvent(QEvent *e) if (m_hoverIndex >= 0) { m_hoverIndex = -1; update(m_hoverRect); - m_hoverRect = QRect(); + m_hoverRect = QRect(); } } @@ -177,16 +180,20 @@ void FancyTabBar::updateTabNameIcon(int index, const QIcon &icon, const QString QSize FancyTabBar::sizeHint() const { QSize sh = tabSizeHint(); - if (verticalTabs) + + if (verticalTabs) { return QSize(sh.width(), sh.height() * m_tabs.count()); + } return QSize(sh.width() * m_tabs.count(), sh.height()); } QSize FancyTabBar::minimumSizeHint() const { QSize sh = tabSizeHint(true); - if (verticalTabs) + + if (verticalTabs) { return QSize(sh.width(), sh.height() * m_tabs.count()); + } return QSize(sh.width() * m_tabs.count(), sh.height()); } @@ -195,18 +202,18 @@ QRect FancyTabBar::tabRect(int index) const QSize sh = tabSizeHint(); if (verticalTabs) { - if (sh.height() * m_tabs.count() > height()) + if (sh.height() * m_tabs.count() > height()) { sh.setHeight(height() / m_tabs.count()); + } return QRect(0, index * sh.height(), sh.width(), sh.height()); - } - if(sh.width() * m_tabs.count() > width()) + if (sh.width() * m_tabs.count() > width()) { sh.setWidth(width() / m_tabs.count()); + } return QRect(index * sh.width(), 0, sh.width(), sh.height()); - } void FancyTabBar::mousePressEvent(QMouseEvent *e) @@ -224,10 +231,10 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const { painter->save(); - QRect rect = tabRect(tabIndex); + QRect rect = tabRect(tabIndex); bool selected = (tabIndex == m_currentIndex); - bool hover = (tabIndex == m_hoverIndex); + bool hover = (tabIndex == m_hoverIndex); #ifdef Q_WS_MAC hover = false; // Dont hover on Mac @@ -241,13 +248,13 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const } QColor light = QColor(255, 255, 255, 40); - QColor dark = QColor(0, 0, 0, 60); + QColor dark = QColor(0, 0, 0, 60); if (selected) { QLinearGradient selectedGradient(rect.bottomRight(), QPoint(rect.center().x(), rect.top())); selectedGradient.setColorAt(0, Qt::white); selectedGradient.setColorAt(0.3, Qt::white); - selectedGradient.setColorAt(0.7, QColor(210, 210, 220)); //give a blue-ish color + selectedGradient.setColorAt(0.7, QColor(210, 210, 220)); // give a blue-ish color painter->fillRect(rect, selectedGradient); painter->setPen(QColor(200, 200, 200)); @@ -256,8 +263,9 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const painter->drawLine(rect.topRight(), rect.bottomRight()); } else { painter->fillRect(rect, background); - if (hover) + if (hover) { painter->fillRect(rect, hoverColor); + } painter->setPen(QPen(light, 0)); painter->drawLine(rect.topLeft(), rect.bottomLeft()); painter->setPen(QPen(dark, 0)); @@ -272,21 +280,23 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const boldFont.setBold(true); painter->setFont(boldFont); painter->setPen(selected ? Utils::StyleHelper::panelTextColor() : QColor(30, 30, 30, 80)); - int textFlags = Qt::AlignCenter | Qt::AlignBottom | Qt::ElideRight | Qt::TextWordWrap; + int textFlags = Qt::AlignCenter | Qt::AlignBottom | Qt::ElideRight | Qt::TextWordWrap; painter->drawText(tabTextRect, textFlags, tabText); painter->setPen(selected ? QColor(60, 60, 60) : Utils::StyleHelper::panelTextColor()); int textHeight = painter->fontMetrics().boundingRect(QRect(0, 0, width(), height()), Qt::TextWordWrap, tabText).height(); tabIconRect.adjust(0, 4, 0, -textHeight); - int iconSize = qMin(tabIconRect.width(), tabIconRect.height()); - if (iconSize > 4) + int iconSize = qMin(tabIconRect.width(), tabIconRect.height()); + if (iconSize > 4) { style()->drawItemPixmap(painter, tabIconRect, Qt::AlignCenter | Qt::AlignVCenter, tabIcon(tabIndex).pixmap(tabIconRect.size())); + } painter->translate(0, -1); painter->drawText(tabTextRect, textFlags, tabText); painter->restore(); } -void FancyTabBar::setCurrentIndex(int index) { +void FancyTabBar::setCurrentIndex(int index) +{ m_currentIndex = index; update(); emit currentChanged(index); @@ -296,19 +306,19 @@ void FancyTabBar::setCurrentIndex(int index) { // FancyColorButton ////// -class FancyColorButton : public QWidget -{ +class FancyColorButton : public QWidget { public: FancyColorButton(QWidget *parent) - : m_parent(parent) + : m_parent(parent) { setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Preferred); } void mousePressEvent(QMouseEvent *ev) { - if (ev->modifiers() & Qt::ShiftModifier) + if (ev->modifiers() & Qt::ShiftModifier) { Utils::StyleHelper::setBaseColor(QColorDialog::getColor(Utils::StyleHelper::baseColor(), m_parent)); + } } private: QWidget *m_parent; @@ -375,22 +385,24 @@ FancyTabWidget::FancyTabWidget(QWidget *parent, bool isVertical) selectionLayout->addWidget(m_cornerWidgetContainer, 0); m_modesStack = new QStackedLayout; - m_statusBar = new QStatusBar; + m_statusBar = new QStatusBar; m_statusBar->setSizePolicy(QSizePolicy::Ignored, QSizePolicy::Fixed); QVBoxLayout *vlayout = new QVBoxLayout; vlayout->setMargin(0); vlayout->setSpacing(0); vlayout->addLayout(m_modesStack); - if (!isVertical) + if (!isVertical) { vlayout->addWidget(m_selectionWidget); -// vlayout->addWidget(m_statusBar); //status bar is not used for now + } +// vlayout->addWidget(m_statusBar); //status bar is not used for now QHBoxLayout *mainLayout = new QHBoxLayout; mainLayout->setMargin(0); mainLayout->setSpacing(1); - if (isVertical) + if (isVertical) { mainLayout->addWidget(m_selectionWidget); + } mainLayout->addLayout(vlayout); setLayout(mainLayout); @@ -419,6 +431,7 @@ void FancyTabWidget::updateTabNameIcon(int index, const QIcon &icon, const QStri void FancyTabWidget::setBackgroundBrush(const QBrush &brush) { QPalette pal = m_tabBar->palette(); + pal.setBrush(QPalette::Mid, brush); m_tabBar->setPalette(pal); pal = m_cornerWidgetContainer->palette(); @@ -441,6 +454,7 @@ void FancyTabWidget::paintEvent(QPaintEvent *event) void FancyTabWidget::insertCornerWidget(int pos, QWidget *widget) { QHBoxLayout *layout = static_cast(m_cornerWidgetContainer->layout()); + layout->insertWidget(pos, widget); } @@ -472,6 +486,7 @@ void FancyTabWidget::setCurrentIndex(int index) void FancyTabWidget::showWidget(int index) { emit currentAboutToShow(index); + m_modesStack->setCurrentIndex(index); emit currentChanged(index); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.h b/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.h index 136d1b28e..d7ef0860e 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,19 +42,17 @@ QT_END_NAMESPACE namespace Core { namespace Internal { +struct FancyTab { + QIcon icon; + QString text; + QString toolTip; +}; - struct FancyTab { - QIcon icon; - QString text; - QString toolTip; - }; - -class FancyTabBar : public QWidget -{ +class FancyTabBar : public QWidget { Q_OBJECT public: - FancyTabBar(QWidget *parent = 0, bool isVertical=false); + FancyTabBar(QWidget *parent = 0, bool isVertical = false); ~FancyTabBar(); bool event(QEvent *event); @@ -69,28 +67,53 @@ public: QSize sizeHint() const; QSize minimumSizeHint() const; - void insertTab(int index, const QIcon &icon, const QString &label) { + void insertTab(int index, const QIcon &icon, const QString &label) + { FancyTab tab; + tab.icon = icon; tab.text = label; m_tabs.insert(index, tab); } - void removeTab(int index) { + void removeTab(int index) + { m_tabs.removeAt(index); - if ( m_currentIndex >= m_tabs.length() ) + if (m_currentIndex >= m_tabs.length()) { m_currentIndex = m_tabs.length() - 1; + } } void updateTabNameIcon(int index, const QIcon &icon, const QString &label); void setCurrentIndex(int index); - int currentIndex() const { return m_currentIndex; } + int currentIndex() const + { + return m_currentIndex; + } - void setTabToolTip(int index, QString toolTip) { m_tabs[index].toolTip = toolTip; } - QString tabToolTip(int index) const { return m_tabs.at(index).toolTip; } + void setTabToolTip(int index, QString toolTip) + { + m_tabs[index].toolTip = toolTip; + } + QString tabToolTip(int index) const + { + return m_tabs.at(index).toolTip; + } - void setIconSize(int s) { iconSize = s; } - QIcon tabIcon(int index) const {return m_tabs.at(index).icon; } - QString tabText(int index) const { return m_tabs.at(index).text; } - int count() const {return m_tabs.count(); } + void setIconSize(int s) + { + iconSize = s; + } + QIcon tabIcon(int index) const + { + return m_tabs.at(index).icon; + } + QString tabText(int index) const + { + return m_tabs.at(index).text; + } + int count() const + { + return m_tabs.count(); + } QRect tabRect(int index) const; @@ -112,11 +135,9 @@ private: bool verticalTabs; QSize tabSizeHint(bool minimum = false) const; - }; -class FancyTabWidget : public QWidget -{ +class FancyTabWidget : public QWidget { Q_OBJECT public: @@ -130,7 +151,10 @@ public: int cornerWidgetCount() const; void setTabToolTip(int index, const QString &toolTip); void updateTabNameIcon(int index, const QIcon &icon, const QString &label); - void setIconSize(int s) { m_tabBar->setIconSize(s); } + void setIconSize(int s) + { + m_tabBar->setIconSize(s); + } void paintEvent(QPaintEvent *event); @@ -154,7 +178,6 @@ private: QWidget *m_selectionWidget; QStatusBar *m_statusBar; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.cpp b/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.cpp index 432c33c7a..c5eea24fe 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,21 +36,20 @@ using namespace Core; /*! - \class FileIconProvider + \class FileIconProvider - Provides icons based on file suffixes. + Provides icons based on file suffixes. - The class is a singleton: It's instance can be accessed via the static instance() method. - Plugins can register custom icons via registerIconSuffix(), and retrieve icons via the icon() - method. - */ + The class is a singleton: It's instance can be accessed via the static instance() method. + Plugins can register custom icons via registerIconSuffix(), and retrieve icons via the icon() + method. + */ FileIconProvider *FileIconProvider::m_instance = 0; FileIconProvider::FileIconProvider() : m_unknownFileIcon(qApp->style()->standardIcon(QStyle::SP_FileIcon)) -{ -} +{} FileIconProvider::~FileIconProvider() { @@ -58,9 +57,9 @@ FileIconProvider::~FileIconProvider() } /*! - Returns the icon associated with the file suffix in fileInfo. If there is none, - the default icon of the operating system is returned. - */ + Returns the icon associated with the file suffix in fileInfo. If there is none, + the default icon of the operating system is returned. + */ QIcon FileIconProvider::icon(const QFileInfo &fileInfo) { const QString suffix = fileInfo.suffix(); @@ -75,8 +74,9 @@ QIcon FileIconProvider::icon(const QFileInfo &fileInfo) // This is incorrect if the OS does not always return the same icon for the // same suffix (Mac OS X), but should speed up the retrieval a lot ... icon = m_systemIconProvider.icon(fileInfo); - if (!suffix.isEmpty()) + if (!suffix.isEmpty()) { registerIconOverlayForSuffix(icon, suffix); + } #else if (fileInfo.isDir()) { icon = m_systemIconProvider.icon(fileInfo); @@ -90,25 +90,27 @@ QIcon FileIconProvider::icon(const QFileInfo &fileInfo) } /*! - Creates a pixmap with baseicon at size and overlays overlayIcon over it. - */ + Creates a pixmap with baseicon at size and overlays overlayIcon over it. + */ QPixmap FileIconProvider::overlayIcon(QStyle::StandardPixmap baseIcon, const QIcon &overlayIcon, const QSize &size) const { QPixmap iconPixmap = qApp->style()->standardIcon(baseIcon).pixmap(size); QPainter painter(&iconPixmap); + painter.drawPixmap(0, 0, overlayIcon.pixmap(size)); painter.end(); return iconPixmap; } /*! - Registers an icon for a given suffix, overlaying the system file icon. - */ + Registers an icon for a given suffix, overlaying the system file icon. + */ void FileIconProvider::registerIconOverlayForSuffix(const QIcon &icon, const QString &suffix) { QPixmap fileIconPixmap = overlayIcon(QStyle::SP_FileIcon, icon, QSize(16, 16)); + // delete old icon, if it exists - QList >::iterator iter = m_cache.begin(); + QList >::iterator iter = m_cache.begin(); for (; iter != m_cache.end(); ++iter) { if ((*iter).first == suffix) { iter = m_cache.erase(iter); @@ -116,32 +118,34 @@ void FileIconProvider::registerIconOverlayForSuffix(const QIcon &icon, const QSt } } - QPair newEntry(suffix, fileIconPixmap); + QPair newEntry(suffix, fileIconPixmap); m_cache.append(newEntry); } /*! - Registers an icon for all the suffixes of a given mime type, overlaying the system file icon. - */ + Registers an icon for all the suffixes of a given mime type, overlaying the system file icon. + */ void FileIconProvider::registerIconOverlayForMimeType(const QIcon &icon, const MimeType &mimeType) { - foreach (const QString &suffix, mimeType.suffixes()) - registerIconOverlayForSuffix(icon, suffix); + foreach(const QString &suffix, mimeType.suffixes()) + registerIconOverlayForSuffix(icon, suffix); } /*! - Returns an icon for the given suffix, or an empty one if none registered. - */ + Returns an icon for the given suffix, or an empty one if none registered. + */ QIcon FileIconProvider::iconForSuffix(const QString &suffix) const { QIcon icon; + #if defined(Q_WS_WIN) || defined(Q_WS_MAC) // On Windows and Mac we use the file system icons Q_UNUSED(suffix) #else - if (suffix.isEmpty()) + if (suffix.isEmpty()) { return icon; + } - QList >::const_iterator iter = m_cache.constBegin(); + QList >::const_iterator iter = m_cache.constBegin(); for (; iter != m_cache.constEnd(); ++iter) { if ((*iter).first == suffix) { icon = (*iter).second; @@ -153,11 +157,12 @@ QIcon FileIconProvider::iconForSuffix(const QString &suffix) const } /*! - Returns the sole instance of FileIconProvider. - */ + Returns the sole instance of FileIconProvider. + */ FileIconProvider *FileIconProvider::instance() { - if (!m_instance) + if (!m_instance) { m_instance = new FileIconProvider; + } return m_instance; } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.h b/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.h index a8ea8d776..0e06b7378 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,13 +38,10 @@ #include namespace Core { - class MimeType; -class CORE_EXPORT FileIconProvider -{ -public: - ~FileIconProvider(); // used to clear the cache +class CORE_EXPORT FileIconProvider { +public: ~FileIconProvider(); // used to clear the cache QIcon icon(const QFileInfo &fileInfo); QPixmap overlayIcon(QStyle::StandardPixmap baseIcon, const QIcon &overlayIcon, const QSize &size) const; @@ -67,7 +64,6 @@ private: FileIconProvider(); static FileIconProvider *m_instance; }; - } // namespace Core #endif // FILEICONPROVIDER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp index 016004bc6..a383264e4 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,15 +43,14 @@ using namespace Utils; using namespace Core::Internal; -GeneralSettings::GeneralSettings(): +GeneralSettings::GeneralSettings() : m_saveSettingsOnExit(true), m_autoConnect(true), m_autoSelect(true), m_useUDPMirror(false), m_useExpertMode(false), m_dialog(0) -{ -} +{} QString GeneralSettings::id() const { @@ -78,7 +77,8 @@ static bool hasQmFilesForLocale(const QString &locale, const QString &creatorTrP static const QString qtTrPath = QLibraryInfo::location(QLibraryInfo::TranslationsPath); const QString trFile = QLatin1String("qt_") + locale + QLatin1String(".qm"); - return QFile::exists(qtTrPath+'/'+trFile) || QFile::exists(creatorTrPath+'/'+trFile); + + return QFile::exists(qtTrPath + '/' + trFile) || QFile::exists(creatorTrPath + '/' + trFile); } void GeneralSettings::fillLanguageBox() const @@ -92,13 +92,14 @@ void GeneralSettings::fillLanguageBox() const m_page->languageBox->setCurrentIndex(m_page->languageBox->count() - 1); } - const QString creatorTrPath = Core::ICore::instance()->resourcePath() + QLatin1String("/translations"); + const QString creatorTrPath = Core::ICore::instance()->resourcePath() + QLatin1String("/translations"); const QStringList languageFiles = QDir(creatorTrPath).entryList(QStringList(QLatin1String("openpilotgcs*.qm"))); foreach(QString languageFile, languageFiles) { int start = languageFile.indexOf(QLatin1Char('_')) + 1; - int end = languageFile.lastIndexOf(QLatin1Char('.')); + int end = languageFile.lastIndexOf(QLatin1Char('.')); const QString locale = languageFile.mid(start, end - start); + // no need to show a language that creator will not load anyway if (hasQmFilesForLocale(locale, creatorTrPath)) { m_page->languageBox->addItem(QLocale::languageToString(QLocale(locale).language()), locale); @@ -134,15 +135,16 @@ QWidget *GeneralSettings::createPage(QWidget *parent) void GeneralSettings::apply() { int currentIndex = m_page->languageBox->currentIndex(); + setLanguage(m_page->languageBox->itemData(currentIndex, Qt::UserRole).toString()); // Apply the new base color if accepted StyleHelper::setBaseColor(m_page->colorButton->color()); m_saveSettingsOnExit = m_page->checkBoxSaveOnExit->isChecked(); - m_useUDPMirror=m_page->cbUseUDPMirror->isChecked(); - m_useExpertMode=m_page->cbExpertMode->isChecked(); - m_autoConnect = m_page->checkAutoConnect->isChecked(); - m_autoSelect = m_page->checkAutoSelect->isChecked(); + m_useUDPMirror = m_page->cbUseUDPMirror->isChecked(); + m_useExpertMode = m_page->cbExpertMode->isChecked(); + m_autoConnect = m_page->checkAutoConnect->isChecked(); + m_autoSelect = m_page->checkAutoSelect->isChecked(); } void GeneralSettings::finish() @@ -153,11 +155,11 @@ void GeneralSettings::finish() void GeneralSettings::readSettings(QSettings *qs) { qs->beginGroup(QLatin1String("General")); - m_language = qs->value(QLatin1String("OverrideLanguage"), QLocale::system().name()).toString(); + m_language = qs->value(QLatin1String("OverrideLanguage"), QLocale::system().name()).toString(); m_saveSettingsOnExit = qs->value(QLatin1String("SaveSettingsOnExit"), m_saveSettingsOnExit).toBool(); - m_autoConnect = qs->value(QLatin1String("AutoConnect"), m_autoConnect).toBool(); - m_autoSelect = qs->value(QLatin1String("AutoSelect"), m_autoSelect).toBool(); - m_useUDPMirror = qs->value(QLatin1String("UDPMirror"), m_useUDPMirror).toBool(); + m_autoConnect = qs->value(QLatin1String("AutoConnect"), m_autoConnect).toBool(); + m_autoSelect = qs->value(QLatin1String("AutoSelect"), m_autoSelect).toBool(); + m_useUDPMirror = qs->value(QLatin1String("UDPMirror"), m_useUDPMirror).toBool(); m_useExpertMode = qs->value(QLatin1String("ExpertMode"), m_useExpertMode).toBool(); qs->endGroup(); } @@ -168,8 +170,7 @@ void GeneralSettings::saveSettings(QSettings *qs) if (m_language.isEmpty()) { qs->remove(QLatin1String("OverrideLanguage")); - } - else { + } else { qs->setValue(QLatin1String("OverrideLanguage"), m_language); } @@ -196,10 +197,10 @@ void GeneralSettings::showHelpForExternalEditor() } #if 0 QMessageBox *mb = new QMessageBox(QMessageBox::Information, - tr("Variables"), - EditorManager::instance()->externalEditorHelpText(), - QMessageBox::Cancel, - m_page->helpExternalEditorButton); + tr("Variables"), + EditorManager::instance()->externalEditorHelpText(), + QMessageBox::Cancel, + m_page->helpExternalEditorButton); mb->setWindowModality(Qt::NonModal); m_dialog = mb; mb->show(); @@ -221,8 +222,8 @@ void GeneralSettings::setLanguage(const QString &locale) { if (m_language != locale) { if (!locale.isEmpty()) { - QMessageBox::information((QWidget*) Core::ICore::instance()->mainWindow(), tr("Restart required"), - tr("The language change will take effect after a restart of the OpenPilot GCS.")); + QMessageBox::information((QWidget *)Core::ICore::instance()->mainWindow(), tr("Restart required"), + tr("The language change will take effect after a restart of the OpenPilot GCS.")); } m_language = locale; } @@ -255,10 +256,9 @@ bool GeneralSettings::useExpertMode() const void GeneralSettings::slotAutoConnect(int value) { - if (value==Qt::Checked) { + if (value == Qt::Checked) { m_page->checkAutoSelect->setEnabled(false); - } - else { + } else { m_page->checkAutoSelect->setEnabled(true); } } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h index dd585c304..f9b909333 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,13 +36,11 @@ namespace Core { namespace Internal { - namespace Ui { - class GeneralSettings; +class GeneralSettings; } -class CORE_EXPORT GeneralSettings : public IOptionsPage -{ +class CORE_EXPORT GeneralSettings : public IOptionsPage { Q_OBJECT public: @@ -52,15 +50,15 @@ public: QString trName() const; QString category() const; QString trCategory() const; - QWidget* createPage(QWidget *parent); + QWidget *createPage(QWidget *parent); void apply(); void finish(); bool saveSettingsOnExit() const; bool autoConnect() const; bool autoSelect() const; bool useUDPMirror() const; - void readSettings(QSettings* qs); - void saveSettings(QSettings* qs); + void readSettings(QSettings *qs); + void saveSettings(QSettings *qs); bool useExpertMode() const; signals: @@ -73,7 +71,7 @@ private slots: private: void fillLanguageBox() const; QString language() const; - void setLanguage(const QString&); + void setLanguage(const QString &); Ui::GeneralSettings *m_page; QString m_language; bool m_saveSettingsOnExit; @@ -83,9 +81,7 @@ private: bool m_useExpertMode; QPointer m_dialog; QList m_codecs; - }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iconfigurableplugin.h b/ground/openpilotgcs/src/plugins/coreplugin/iconfigurableplugin.h index 951530062..2dd4816a0 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iconfigurableplugin.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/iconfigurableplugin.h @@ -9,17 +9,14 @@ #include namespace Core { - -class CORE_EXPORT IConfigurablePlugin : public ExtensionSystem::IPlugin -{ +class CORE_EXPORT IConfigurablePlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: // IConfigurablePlugin(QObject *parent = 0){} virtual ~IConfigurablePlugin() {} - virtual void readConfig( QSettings* qSettings, UAVConfigInfo *configInfo) = 0; - virtual void saveConfig(QSettings* qSettings, Core::UAVConfigInfo *configInfo) = 0; + virtual void readConfig(QSettings *qSettings, UAVConfigInfo *configInfo) = 0; + virtual void saveConfig(QSettings *qSettings, Core::UAVConfigInfo *configInfo) = 0; }; - } // namespace Core #endif // ICONFIGURABLEPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iconnection.cpp b/ground/openpilotgcs/src/plugins/coreplugin/iconnection.cpp index 445591327..f89813214 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iconnection.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/iconnection.cpp @@ -27,4 +27,3 @@ */ #include "iconnection.h" - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iconnection.h b/ground/openpilotgcs/src/plugins/coreplugin/iconnection.h index 6bc35c18e..a5ebbc17d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iconnection.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/iconnection.h @@ -36,46 +36,52 @@ #include "core_global.h" namespace Core { - /** -* An IConnection object define a "type of connection", -* for instance USB, Serial, Network, ... -*/ -class CORE_EXPORT IConnection : public QObject -{ + * An IConnection object define a "type of connection", + * for instance USB, Serial, Network, ... + */ +class CORE_EXPORT IConnection : public QObject { Q_OBJECT public: /** - * Return the list of devices found on the system - */ - struct device - { + * Return the list of devices found on the system + */ + struct device { QString name; QString displayName; - bool operator==(device i){return this->name==i.name;} + bool operator==(device i) + { + return this->name == i.name; + } }; virtual QList availableDevices() = 0; /** - * Open a device, and return a QIODevice interface from it - * It should be a dynamically created object as it will be - * deleted by the connection manager. - */ + * Open a device, and return a QIODevice interface from it + * It should be a dynamically created object as it will be + * deleted by the connection manager. + */ virtual QIODevice *openDevice(const QString &deviceName) = 0; - virtual void closeDevice(const QString &deviceName) { Q_UNUSED(deviceName) }; + virtual void closeDevice(const QString &deviceName) + { + Q_UNUSED(deviceName) + }; /** - * Connection type name "USB HID" - */ + * Connection type name "USB HID" + */ virtual QString connectionName() = 0; /** - * Short name to display in a combo box - */ - virtual QString shortName() {return connectionName();} + * Short name to display in a combo box + */ + virtual QString shortName() + { + return connectionName(); + } /** * Manage whether the plugin is allowed to poll for devices @@ -86,11 +92,10 @@ public: signals: /** - * Available devices list has changed, signal it to connection manager (and whoever wants to know) - */ + * Available devices list has changed, signal it to connection manager (and whoever wants to know) + */ void availableDevChanged(IConnection *); }; - -} //namespace Core +} // namespace Core #endif // ICONNECTION_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/icontext.h b/ground/openpilotgcs/src/plugins/coreplugin/icontext.h index ca03b6902..d1d43e8aa 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/icontext.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/icontext.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,9 +37,7 @@ class QWidget; QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT IContext : public QObject -{ +class CORE_EXPORT IContext : public QObject { Q_OBJECT public: IContext(QObject *parent = 0) : QObject(parent) {} @@ -47,28 +45,34 @@ public: virtual QList context() const = 0; virtual QWidget *widget() = 0; - virtual QString contextHelpId() const { return QString(); } + virtual QString contextHelpId() const + { + return QString(); + } }; -class BaseContext : public Core::IContext -{ +class BaseContext : public Core::IContext { public: BaseContext(QWidget *widget, const QList &context, QObject *parent = 0) : Core::IContext(parent), m_widget(widget), m_context(context) + {} + + QList context() const { + return m_context; } - QList context() const { return m_context; } - - QWidget *widget() { return m_widget; } + QWidget *widget() + { + return m_widget; + } private: QWidget *m_widget; QList m_context; }; - } // namespace Core -#endif //ICONTEXT_H +#endif // ICONTEXT_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/icore.cpp b/ground/openpilotgcs/src/plugins/coreplugin/icore.cpp index 35030426c..8f4717fb2 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/icore.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/icore.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,12 +32,12 @@ \namespace Core \brief The Core namespace contains all classes that make up the Core plugin which constitute the basic functionality of the OpenPilot GCS. -*/ + */ /*! \namespace Core::Internal \internal -*/ + */ /*! \class Core::ICore @@ -49,7 +49,7 @@ from your plugin through \c{Core::instance()}. \mainclass -*/ + */ /*! \fn QStringList ICore::showNewItemDialog(const QString &title, @@ -64,7 +64,7 @@ current file. \sa Core::FileManager -*/ + */ /*! \fn bool ICore::showOptionsDialog(const QString &group = QString(), @@ -73,7 +73,7 @@ \a page in a specified \a group. The arguments refer to the string IDs of the corresponding IOptionsPage. -*/ + */ /*! \fn bool ICore::showWarningWithOptions(const QString &title, const QString &text, @@ -86,7 +86,7 @@ Should be used to display configuration errors and point users to the setting. Returns true if the settings dialog was accepted. -*/ + */ /*! @@ -95,21 +95,21 @@ The action manager is responsible for registration of menus and menu items and keyboard shortcuts. -*/ + */ /*! \fn FileManager *ICore::fileManager() const \brief Returns the application's file manager. The file manager keeps track of files for changes outside the application. -*/ + */ /*! \fn UniqueIDManager *ICore::uniqueIDManager() const \brief Returns the application's id manager. The unique ID manager transforms strings in unique integers and the other way round. -*/ + */ /*! \fn MessageManager *ICore::messageManager() const @@ -117,7 +117,7 @@ The message manager is the interface to the "General" output pane for general application debug messages. -*/ + */ /*! \fn ExtensionSystem::PluginManager *ICore::pluginManager() const @@ -125,7 +125,7 @@ The plugin manager handles the plugin life cycles and manages the common object pool. -*/ + */ /*! \fn EditorManager *ICore::editorManager() const @@ -134,8 +134,7 @@ The editor manager handles all editor related tasks like opening documents, the stack of currently open documents and the currently active document. -*/ - + */ /*! @@ -145,7 +144,7 @@ The variable manager is used to register application wide string variables like \c MY_PROJECT_DIR such that strings like \c{somecommand ${MY_PROJECT_DIR}/sub} can be resolved/expanded from anywhere in the application. -*/ + */ /*! \fn ThreadManager *ICore::threadManager() const @@ -154,7 +153,7 @@ The thread manager is used to manage application wide QThread objects, allowing certain critical objects to synchronize directly within the same real time thread - anywhere in the application. -*/ + */ /*! \fn ModeManager *ICore::modeManager() const @@ -164,14 +163,14 @@ that were added to the plugin manager's object pool as well as their buttons and the tool bar with the round buttons in the lower left corner of the OpenPilot GCS. -*/ + */ /*! \fn MimeDatabase *ICore::mimeDatabase() const \brief Returns the application's mime database. Use the mime database to manage mime types. -*/ + */ /*! \fn QSettings *ICore::settings(QSettings::UserScope scope) const @@ -189,7 +188,7 @@ functionality exists for internal purposes only. \see settingsDatabase() -*/ + */ /*! \fn SettingsDatabase *ICore::settingsDatabase() const @@ -200,7 +199,7 @@ are application wide. \see SettingsDatabase -*/ + */ /*! \fn QString ICore::resourcePath() const @@ -209,14 +208,14 @@ This abstraction is needed to avoid platform-specific code all over the place, since e.g. on Mac the resources are part of the application bundle. -*/ + */ /*! \fn QMainWindow *ICore::mainWindow() const \brief Returns the main application window. For use as dialog parent etc. -*/ + */ /*! \fn IContext *ICore::currentContextObject() const @@ -224,7 +223,7 @@ \sa ICore::addAdditionalContext() \sa ICore::addContextObject() -*/ + */ /*! \fn void ICore::addAdditionalContext(int context) @@ -237,7 +236,7 @@ \sa ICore::removeAdditionalContext() \sa ICore::hasContext() \sa ICore::updateContext() -*/ + */ /*! \fn void ICore::removeAdditionalContext(int context) @@ -249,7 +248,7 @@ \sa ICore::addAdditionalContext() \sa ICore::hasContext() \sa ICore::updateContext() -*/ + */ /*! \fn bool ICore::hasContext(int context) const @@ -257,7 +256,7 @@ \sa ICore::addAdditionalContext() \sa ICore::addContextObject() -*/ + */ /*! \fn void ICore::addContextObject(IContext *context) @@ -269,7 +268,7 @@ \sa ICore::removeContextObject() \sa ICore::addAdditionalContext() \sa ICore::currentContextObject() -*/ + */ /*! \fn void ICore::removeContextObject(IContext *context) @@ -278,8 +277,8 @@ \sa ICore::addContextObject() \sa ICore::addAdditionalContext() \sa ICore::currentContextObject() -} -*/ + } + */ /*! \fn void ICore::updateContext() @@ -287,29 +286,29 @@ \sa ICore::addAdditionalContext() \sa ICore::removeAdditionalContext() -*/ + */ /*! \fn void ICore::openFiles(const QStringList &fileNames) \brief Open all files from a list of \a fileNames like it would be done if they were given to the OpenPilot GCS on the command line, or they were opened via \gui{File|Open}. -*/ + */ /*! \fn ICore::ICore() \internal -*/ + */ /*! \fn ICore::~ICore() \internal -*/ + */ /*! \fn void ICore::coreOpened() \brief Emitted after all plugins have been loaded and the main window shown. -*/ + */ /*! \fn void ICore::saveSettingsRequested() @@ -317,13 +316,13 @@ should be saved to disk. At the moment that happens when the application is closed, and on \gui{Save All}. -*/ + */ /*! \fn void ICore::optionsDialogRequested() \brief Signal that allows plugins to perform actions just before the \gui{Tools|Options} dialog is shown. -*/ + */ /*! \fn void ICore::coreAboutToClose() @@ -332,16 +331,16 @@ The application is guaranteed to shut down after this signal is emitted. It's there as an addition to the usual plugin lifecycle methods, namely IPlugin::shutdown(), just for convenience. -*/ + */ /*! \fn void ICore::contextAboutToChange(Core::IContext *context) \brief Sent just before a new \a context becomes the current context (meaning that its widget got focus). -*/ + */ /*! \fn void ICore::contextChanged(Core::IContext *context) \brief Sent just after a new \a context became the current context (meaning that its widget got focus). -*/ + */ diff --git a/ground/openpilotgcs/src/plugins/coreplugin/icore.h b/ground/openpilotgcs/src/plugins/coreplugin/icore.h index f06479883..a0b67e820 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/icore.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/icore.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,7 +40,6 @@ template class QList; QT_END_NAMESPACE namespace Core { - class ActionManager; class IContext; class IWizard; @@ -56,8 +55,7 @@ class UAVGadgetManager; class UAVGadgetInstanceManager; class IConfigurablePlugin; -class CORE_EXPORT ICore : public QObject -{ +class CORE_EXPORT ICore : public QObject { Q_OBJECT public: @@ -71,16 +69,16 @@ public: QWidget *parent = 0) = 0; virtual bool showWarningWithOptions(const QString &title, const QString &text, - const QString &details = QString(), - const QString &settingsCategory = QString(), - const QString &settingsId = QString(), - QWidget *parent = 0) = 0; + const QString &details = QString(), + const QString &settingsCategory = QString(), + const QString &settingsId = QString(), + QWidget *parent = 0) = 0; - virtual ActionManager *actionManager() const = 0; - virtual UniqueIDManager *uniqueIDManager() const = 0; - virtual MessageManager *messageManager() const = 0; - virtual VariableManager *variableManager() const = 0; - virtual ThreadManager *threadManager() const = 0; + virtual ActionManager *actionManager() const = 0; + virtual UniqueIDManager *uniqueIDManager() const = 0; + virtual MessageManager *messageManager() const = 0; + virtual VariableManager *variableManager() const = 0; + virtual ThreadManager *threadManager() const = 0; virtual ModeManager *modeManager() const = 0; virtual ConnectionManager *connectionManager() const = 0; virtual UAVGadgetInstanceManager *uavGadgetInstanceManager() const = 0; @@ -88,23 +86,23 @@ public: virtual QSettings *settings(QSettings::Scope scope = QSettings::UserScope) const = 0; virtual SettingsDatabase *settingsDatabase() const = 0; - virtual void readMainSettings(QSettings* qs, bool workspaceDiffOnly = false) = 0; - virtual void saveMainSettings(QSettings* qs) = 0; - virtual void readSettings(IConfigurablePlugin* plugin, QSettings* qs = 0) = 0; - virtual void saveSettings(IConfigurablePlugin* plugin, QSettings* qs = 0) = 0; + virtual void readMainSettings(QSettings *qs, bool workspaceDiffOnly = false) = 0; + virtual void saveMainSettings(QSettings *qs) = 0; + virtual void readSettings(IConfigurablePlugin *plugin, QSettings *qs = 0) = 0; + virtual void saveSettings(IConfigurablePlugin *plugin, QSettings *qs = 0) = 0; virtual void deleteSettings() = 0; - virtual QString resourcePath() const = 0; + virtual QString resourcePath() const = 0; virtual QMainWindow *mainWindow() const = 0; // adds and removes additional active contexts, this context is appended to the // currently active contexts. call updateContext after changing - virtual IContext *currentContextObject() const = 0; - virtual void addAdditionalContext(int context) = 0; - virtual void removeAdditionalContext(int context) = 0; + virtual IContext *currentContextObject() const = 0; + virtual void addAdditionalContext(int context) = 0; + virtual void removeAdditionalContext(int context) = 0; virtual bool hasContext(int context) const = 0; - virtual void addContextObject(IContext *context) = 0; + virtual void addContextObject(IContext *context) = 0; virtual void removeContextObject(IContext *context) = 0; virtual void updateContext() = 0; @@ -120,7 +118,6 @@ signals: void contextAboutToChange(Core::IContext *context); void contextChanged(Core::IContext *context); }; - } // namespace Core #endif // ICORE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/icorelistener.h b/ground/openpilotgcs/src/plugins/coreplugin/icorelistener.h index 8f94fca4b..92c203fbb 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/icorelistener.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/icorelistener.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,39 +34,40 @@ namespace Core { /*! - \class Core::ICoreListener + \class Core::ICoreListener - \brief Provides a hook for plugins to veto on certain events emitted from -the core plugin. + \brief Provides a hook for plugins to veto on certain events emitted from + the core plugin. - You implement this interface if you want to prevent certain events from - occurring, e.g. if you want to prevent the closing of the whole application - or to prevent the closing of an editor window under certain conditions. + You implement this interface if you want to prevent certain events from + occurring, e.g. if you want to prevent the closing of the whole application + or to prevent the closing of an editor window under certain conditions. - If e.g. the application window requests a close, then first - ICoreListener::coreAboutToClose() is called (in arbitrary order) on all - registered objects implementing this interface. If one if these calls returns - false, the process is aborted and the event is ignored. If all calls return - true, the corresponding signal is emitted and the event is accepted/performed. + If e.g. the application window requests a close, then first + ICoreListener::coreAboutToClose() is called (in arbitrary order) on all + registered objects implementing this interface. If one if these calls returns + false, the process is aborted and the event is ignored. If all calls return + true, the corresponding signal is emitted and the event is accepted/performed. - Guidelines for implementing: - \list - \o Return false from the implemented method if you want to prevent the event. - \o You need to add your implementing object to the plugin managers objects: + Guidelines for implementing: + \list + \o Return false from the implemented method if you want to prevent the event. + \o You need to add your implementing object to the plugin managers objects: ExtensionSystem::PluginManager::instance()->addObject(yourImplementingObject); - \o Don't forget to remove the object again at deconstruction (e.g. in the destructor of + \o Don't forget to remove the object again at deconstruction (e.g. in the destructor of your plugin). -*/ -class CORE_EXPORT ICoreListener : public QObject -{ + */ +class CORE_EXPORT ICoreListener : public QObject { Q_OBJECT public: ICoreListener(QObject *parent = 0) : QObject(parent) {} virtual ~ICoreListener() {} - virtual bool coreAboutToClose() { return true; } + virtual bool coreAboutToClose() + { + return true; + } }; - } // namespace Core #endif // ICORELISTENER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/imode.h b/ground/openpilotgcs/src/plugins/coreplugin/imode.h index b5c7eeb1b..44d970677 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/imode.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/imode.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,21 +38,18 @@ class QIcon; QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT IMode : public IContext -{ +class CORE_EXPORT IMode : public IContext { Q_OBJECT public: IMode(QObject *parent = 0) : IContext(parent) {} virtual ~IMode() {} virtual QString name() const = 0; - virtual QIcon icon() const = 0; + virtual QIcon icon() const = 0; virtual int priority() const = 0; virtual void setPriority(int priority) = 0; virtual const char *uniqueModeName() const = 0; }; - } // namespace Core #endif // IMODE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/ioutputpane.h b/ground/openpilotgcs/src/plugins/coreplugin/ioutputpane.h index af166f4c2..aeb518b93 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/ioutputpane.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/ioutputpane.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,16 +40,14 @@ class QWidget; QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT IOutputPane : public QObject -{ +class CORE_EXPORT IOutputPane : public QObject { Q_OBJECT public: IOutputPane(QObject *parent = 0) : QObject(parent) {} virtual ~IOutputPane() {} virtual QWidget *outputWidget(QWidget *parent) = 0; - virtual QList toolBarWidgets() const = 0; + virtual QList toolBarWidgets() const = 0; virtual QString name() const = 0; // -1 don't show in statusBar @@ -60,18 +58,18 @@ public: virtual void visibilityChanged(bool visible) = 0; // This function is called to give the outputwindow focus - virtual void setFocus() = 0; + virtual void setFocus() = 0; // Wheter the outputpane has focus - virtual bool hasFocus() = 0; + virtual bool hasFocus() = 0; // Wheter the outputpane can be focused at the moment. // (E.g. the search result window doesn't want to be focussed if the are no results.) - virtual bool canFocus() = 0; + virtual bool canFocus() = 0; virtual bool canNavigate() = 0; - virtual bool canNext() = 0; + virtual bool canNext() = 0; virtual bool canPrevious() = 0; - virtual void goToNext() = 0; - virtual void goToPrev() = 0; + virtual void goToNext() = 0; + virtual void goToPrev() = 0; public slots: void popup() { @@ -108,7 +106,6 @@ signals: void togglePage(bool withFocusIfShown); void navigateStateUpdate(); }; - } // namespace Core #endif // IOUTPUTPANE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.cpp b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.cpp index d91877b54..4863c4a53 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.cpp @@ -25,4 +25,3 @@ * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.h b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.h index 6fbce59a0..98bc2e7ab 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.h @@ -40,42 +40,57 @@ class QComboBox; QT_END_NAMESPACE namespace Core { - class IUAVGadgetConfiguration; -class CORE_EXPORT IUAVGadget : public IContext -{ +class CORE_EXPORT IUAVGadget : public IContext { Q_OBJECT public: IUAVGadget(QString classId, QObject *parent = 0) : - IContext(parent), - m_classId(classId) { } + IContext(parent), + m_classId(classId) {} virtual ~IUAVGadget() {} - QList context() const { return m_context; } - void setContext(QList context) { m_context = context; } + QList context() const + { + return m_context; + } + void setContext(QList context) + { + m_context = context; + } virtual QWidget *widget() = 0; - virtual QComboBox *toolBar() { return 0; } - virtual QString contextHelpId() const { return QString(); } - QString classId() const { return m_classId; } + virtual QComboBox *toolBar() + { + return 0; + } + virtual QString contextHelpId() const + { + return QString(); + } + QString classId() const + { + return m_classId; + } - virtual IUAVGadgetConfiguration *activeConfiguration() { return 0; } - virtual void loadConfiguration(IUAVGadgetConfiguration*) { } - virtual void saveState(QSettings* /*qSettings*/) { } - virtual void restoreState(QByteArray) { } - virtual void restoreState(QSettings* /*qSettings*/) { } + virtual IUAVGadgetConfiguration *activeConfiguration() + { + return 0; + } + virtual void loadConfiguration(IUAVGadgetConfiguration *) {} + virtual void saveState(QSettings * /*qSettings*/) {} + virtual void restoreState(QByteArray) {} + virtual void restoreState(QSettings * /*qSettings*/) {} public slots: - virtual void configurationChanged(IUAVGadgetConfiguration* ) { } - virtual void configurationAdded(IUAVGadgetConfiguration*) { } - virtual void configurationToBeDeleted(IUAVGadgetConfiguration*) { } - virtual void configurationNameChanged(QString, QString) { } + virtual void configurationChanged(IUAVGadgetConfiguration *) {} + virtual void configurationAdded(IUAVGadgetConfiguration *) {} + virtual void configurationToBeDeleted(IUAVGadgetConfiguration *) {} + virtual void configurationNameChanged(QString, QString) {} private slots: private: QString m_classId; QList m_context; }; - } // namespace Core #endif // IUAVGADGET_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.cpp index 5dc7dff9c..0a1e09293 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.cpp @@ -10,18 +10,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,5 +35,4 @@ IUAVGadgetConfiguration::IUAVGadgetConfiguration(QString classId, QObject *paren m_classId(classId), m_name(tr("default")), m_provisionalName(tr("default")) -{ -} +{} diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.h index 38eeecd09..278987b12 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.h @@ -9,19 +9,19 @@ * @{ * @brief The Core GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,24 +34,46 @@ #include namespace Core { - class UAVConfigInfo; -class CORE_EXPORT IUAVGadgetConfiguration : public QObject -{ -Q_OBJECT +class CORE_EXPORT IUAVGadgetConfiguration : public QObject { + Q_OBJECT public: explicit IUAVGadgetConfiguration(QString classId, QObject *parent = 0); - QString classId() { return m_classId; } - QString name() { return m_name; } - void setName(QString name) { m_name = name; } - QString provisionalName() { return m_provisionalName; } - void setProvisionalName(QString name) { m_provisionalName = name; } - bool locked() const { return m_locked; } - void setLocked(bool locked) { m_locked = locked; } + QString classId() + { + return m_classId; + } + QString name() + { + return m_name; + } + void setName(QString name) + { + m_name = name; + } + QString provisionalName() + { + return m_provisionalName; + } + void setProvisionalName(QString name) + { + m_provisionalName = name; + } + bool locked() const + { + return m_locked; + } + void setLocked(bool locked) + { + m_locked = locked; + } - virtual void saveConfig(QSettings* /*settings*/) const {}; - virtual void saveConfig(QSettings* settings, UAVConfigInfo* /*configInfo*/) const { saveConfig(settings); } + virtual void saveConfig(QSettings * /*settings*/) const {}; + virtual void saveConfig(QSettings *settings, UAVConfigInfo * /*configInfo*/) const + { + saveConfig(settings); + } virtual IUAVGadgetConfiguration *clone() = 0; @@ -64,9 +86,7 @@ private: QString m_classId; QString m_name; QString m_provisionalName; - }; - } // namespace Core #endif // IUAVGADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetfactory.h b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetfactory.h index 801343f7c..61f7d803e 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetfactory.h @@ -40,41 +40,65 @@ class QStringList; QT_END_NAMESPACE namespace Core { - class IUAVGadget; class IUAVGadgetConfiguration; class IOptionsPage; -class CORE_EXPORT IUAVGadgetFactory : public QObject -{ +class CORE_EXPORT IUAVGadgetFactory : public QObject { Q_OBJECT public: IUAVGadgetFactory(QString classId, QString name, QObject *parent = 0) : - QObject(parent), - m_classId(classId), - m_name(name), - m_icon(QIcon()), - m_singleConfigurationGadget(false) {} + QObject(parent), + m_classId(classId), + m_name(name), + m_icon(QIcon()), + m_singleConfigurationGadget(false) {} virtual ~IUAVGadgetFactory() {} virtual IUAVGadget *createGadget(QWidget *parent) = 0; - virtual IUAVGadgetConfiguration *createConfiguration(QSettings* /*qSettings*/) { return 0; } - virtual IUAVGadgetConfiguration *createConfiguration(QSettings* qs, UAVConfigInfo */*configInfo*/) { return createConfiguration(qs); } - virtual IOptionsPage *createOptionsPage(IUAVGadgetConfiguration */*config*/) { return 0; } - QString classId() const { return m_classId; } - QString name() const { return m_name; } - QIcon icon() const { return m_icon; } - bool isSingleConfigurationGadget() { return m_singleConfigurationGadget; } + virtual IUAVGadgetConfiguration *createConfiguration(QSettings * /*qSettings*/) + { + return 0; + } + virtual IUAVGadgetConfiguration *createConfiguration(QSettings *qs, UAVConfigInfo * /*configInfo*/) + { + return createConfiguration(qs); + } + virtual IOptionsPage *createOptionsPage(IUAVGadgetConfiguration * /*config*/) + { + return 0; + } + QString classId() const + { + return m_classId; + } + QString name() const + { + return m_name; + } + QIcon icon() const + { + return m_icon; + } + bool isSingleConfigurationGadget() + { + return m_singleConfigurationGadget; + } protected: - void setIcon(QIcon icon) { m_icon = icon; } - void setSingleConfigurationGadgetTrue() { m_singleConfigurationGadget = true; } + void setIcon(QIcon icon) + { + m_icon = icon; + } + void setSingleConfigurationGadgetTrue() + { + m_singleConfigurationGadget = true; + } private: QString m_classId; // unique class id QString m_name; // display name, should also be unique QIcon m_icon; bool m_singleConfigurationGadget; // true if there is exactly one configuration for this gadget }; - } // namespace Core #endif // IUAVGADGETFACTORY_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iversioncontrol.h b/ground/openpilotgcs/src/plugins/coreplugin/iversioncontrol.h index 43379e30b..991ca58a1 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iversioncontrol.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/iversioncontrol.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include namespace Core { - -class CORE_EXPORT IVersionControl : public QObject -{ +class CORE_EXPORT IVersionControl : public QObject { Q_OBJECT public: enum Operation { AddOperation, DeleteOperation, OpenOperation }; @@ -45,7 +43,7 @@ public: IVersionControl(QObject *parent = 0) : QObject(parent) {} virtual ~IVersionControl() {} - virtual QString name() const = 0; + virtual QString name() const = 0; virtual bool isEnabled() const = 0; @@ -84,7 +82,7 @@ public: * * \note The EditorManager calls this for the editors. */ - virtual bool vcsOpen(const QString &fileName) = 0; + virtual bool vcsOpen(const QString &fileName) = 0; /*! * Called after a file has been added to a project If the version control @@ -94,7 +92,7 @@ public: * \note This function should be called from IProject subclasses after * files are added to the project. */ - virtual bool vcsAdd(const QString &filename) = 0; + virtual bool vcsAdd(const QString &filename) = 0; /*! * Called after a file has been removed from the project (if the user @@ -112,7 +110,6 @@ signals: // TODO: ADD A WAY TO DETECT WHETHER A FILE IS MANAGED, e.g // virtual bool sccManaged(const QString &filename) = 0; }; - } // namespace Core #endif // IVERSIONCONTROL_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iview.h b/ground/openpilotgcs/src/plugins/coreplugin/iview.h index d36c297ba..28ffb3381 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iview.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/iview.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,20 +34,17 @@ #include namespace Core { - -class CORE_EXPORT IView : public IContext -{ +class CORE_EXPORT IView : public IContext { Q_OBJECT public: - enum ViewPosition { First=0, Second=1, Third=2 }; + enum ViewPosition { First = 0, Second = 1, Third = 2 }; IView(QObject *parent = 0) : IContext(parent) {} virtual ~IView() {} - virtual const char *uniqueViewName() const = 0; + virtual const char *uniqueViewName() const = 0; virtual ViewPosition defaultPosition() const = 0; }; - } // namespace Core #endif // IVIEW_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp index 86176c9b2..d57393baf 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -104,7 +104,7 @@ MainWindow::MainWindow() : m_settings(new QSettings(XmlConfig::XmlSettingsFormat, QSettings::UserScope, QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS_config"), this)), m_globalSettings(new QSettings(XmlConfig::XmlSettingsFormat, QSettings::SystemScope, - QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS_config"), this)), + QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS_config"), this)), m_settingsDatabase(new SettingsDatabase(QFileInfo(m_settings->fileName()).path(), QLatin1String("OpenPilotGCS_config"), this)), @@ -149,10 +149,11 @@ MainWindow::MainWindow() : // Sometimes we get the standard windows 95 style as a fallback // e.g. if we are running on a KDE4 desktop QByteArray desktopEnvironment = qgetenv("DESKTOP_SESSION"); - if (desktopEnvironment == "kde") + if (desktopEnvironment == "kde") { baseName = QLatin1String("plastique"); - else + } else { baseName = QLatin1String("cleanlooks"); + } } #endif qApp->setStyle(new ManhattanStyle(baseName)); @@ -166,7 +167,7 @@ MainWindow::MainWindow() : registerDefaultActions(); m_modeStack = new MyTabWidget(this); - m_modeStack->setIconSize(QSize(24,24)); + m_modeStack->setIconSize(QSize(24, 24)); m_modeStack->setTabPosition(QTabWidget::South); m_modeStack->setMovable(false); m_modeStack->setMinimumWidth(512); @@ -178,18 +179,18 @@ MainWindow::MainWindow() : m_connectionManager = new ConnectionManager(this, m_modeStack); - m_messageManager = new MessageManager; + m_messageManager = new MessageManager; setCentralWidget(m_modeStack); - connect(QApplication::instance(), SIGNAL(focusChanged(QWidget*,QWidget*)), - this, SLOT(updateFocusWidget(QWidget*,QWidget*))); - connect(m_workspaceSettings, SIGNAL(tabBarSettingsApplied(QTabWidget::TabPosition,bool)), - this, SLOT(applyTabBarSettings(QTabWidget::TabPosition,bool))); - connect(m_modeManager, SIGNAL(newModeOrder(QVector)), m_workspaceSettings, SLOT(newModeOrder(QVector))); + connect(QApplication::instance(), SIGNAL(focusChanged(QWidget *, QWidget *)), + this, SLOT(updateFocusWidget(QWidget *, QWidget *))); + connect(m_workspaceSettings, SIGNAL(tabBarSettingsApplied(QTabWidget::TabPosition, bool)), + this, SLOT(applyTabBarSettings(QTabWidget::TabPosition, bool))); + connect(m_modeManager, SIGNAL(newModeOrder(QVector)), m_workspaceSettings, SLOT(newModeOrder(QVector))); statusBar()->setProperty("p_styled", true); setAcceptDrops(true); - foreach (QString engine, qxtLog->allLoggerEngines()) - qxtLog->removeLoggerEngine(engine); + foreach(QString engine, qxtLog->allLoggerEngines()) + qxtLog->removeLoggerEngine(engine); qxtLog->addLoggerEngine("std", new QxtBasicSTDLoggerEngine()); qxtLog->installAsMessageHandler(); qxtLog->enableAllLogLevels(); @@ -197,21 +198,19 @@ MainWindow::MainWindow() : MainWindow::~MainWindow() { - if (m_connectionManager) // Pip - { - m_connectionManager->disconnectDevice(); - m_connectionManager->suspendPolling(); - } + if (m_connectionManager) { // Pip + m_connectionManager->disconnectDevice(); + m_connectionManager->suspendPolling(); + } - hide(); + hide(); - qxtLog->removeAsMessageHandler(); - foreach (QString engine, qxtLog->allLoggerEngines()) - qxtLog->removeLoggerEngine(engine); + qxtLog->removeAsMessageHandler(); + foreach(QString engine, qxtLog->allLoggerEngines()) + qxtLog->removeLoggerEngine(engine); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); if (m_uavGadgetManagers.count() > 0) { - foreach (UAVGadgetManager *mode, m_uavGadgetManagers) - { + foreach(UAVGadgetManager * mode, m_uavGadgetManagers) { pm->removeObject(mode); delete mode; } @@ -221,11 +220,11 @@ MainWindow::~MainWindow() pm->removeObject(m_generalSettings); pm->removeObject(m_workspaceSettings); delete m_messageManager; - m_messageManager = 0; + m_messageManager = 0; delete m_shortcutSettings; - m_shortcutSettings = 0; + m_shortcutSettings = 0; delete m_generalSettings; - m_generalSettings = 0; + m_generalSettings = 0; delete m_workspaceSettings; m_workspaceSettings = 0; delete m_settings; @@ -235,10 +234,10 @@ MainWindow::~MainWindow() pm->removeObject(m_coreImpl); delete m_coreImpl; - m_coreImpl = 0; + m_coreImpl = 0; delete m_modeManager; - m_modeManager = 0; + m_modeManager = 0; delete m_mimeDatabase; m_mimeDatabase = 0; } @@ -247,7 +246,7 @@ bool MainWindow::init(QString *errorMessage) { Q_UNUSED(errorMessage) - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + ExtensionSystem::PluginManager * pm = ExtensionSystem::PluginManager::instance(); pm->addObject(m_coreImpl); m_modeManager->init(); m_connectionManager->init(); @@ -259,10 +258,8 @@ bool MainWindow::init(QString *errorMessage) return true; } -void MainWindow::modeChanged(Core::IMode */*mode*/) -{ - -} +void MainWindow::modeChanged(Core::IMode * /*mode*/) +{} void MainWindow::extensionsInitialized() { @@ -271,8 +268,8 @@ void MainWindow::extensionsInitialized() qs->beginGroup("General"); m_config_description = qs->value("Description", "none").toString(); - m_config_details = qs->value("Details", "none").toString(); - m_config_stylesheet = qs->value("StyleSheet", "none").toString(); + m_config_details = qs->value("Details", "none").toString(); + m_config_stylesheet = qs->value("StyleSheet", "none").toString(); qDebug() << "Configured style sheet:" << m_config_stylesheet; if (m_config_stylesheet == "wide") { @@ -281,7 +278,7 @@ void MainWindow::extensionsInitialized() } // Load common style sheet - QString style = loadStyleSheet(m_config_stylesheet + ".qss"); + QString style = loadStyleSheet(m_config_stylesheet + ".qss"); // Load and concatenate platform specific style sheet QString fileName = m_config_stylesheet; @@ -292,7 +289,7 @@ void MainWindow::extensionsInitialized() #else fileName += "_windows.qss"; #endif - style += loadStyleSheet(fileName); + style += loadStyleSheet(fileName); // We'll use qApp macro to get the QApplication pointer // and set the style sheet application wide. @@ -314,9 +311,11 @@ void MainWindow::extensionsInitialized() emit m_coreImpl->coreOpened(); } -QString MainWindow::loadStyleSheet(QString fileName) { +QString MainWindow::loadStyleSheet(QString fileName) +{ // Let's use QFile and point to a resource... QDir dir(QCoreApplication::applicationDirPath()); + #ifdef Q_OS_MAC dir.cdUp(); dir.cd("Resources"); @@ -336,8 +335,7 @@ QString MainWindow::loadStyleSheet(QString fileName) { // ...read file to a string. style = textStream.readAll(); file.close(); - } - else { + } else { qDebug() << "Failed to open style sheet file" << file.fileName(); } return style; @@ -353,7 +351,7 @@ void MainWindow::closeEvent(QCloseEvent *event) } const QList listeners = ExtensionSystem::PluginManager::instance()->getObjects(); - foreach (ICoreListener *listener, listeners) { + foreach(ICoreListener * listener, listeners) { if (!listener->coreAboutToClose()) { event->ignore(); return; @@ -372,14 +370,17 @@ void MainWindow::closeEvent(QCloseEvent *event) // Check for desktop file manager file drop events static bool isDesktopFileManagerDrop(const QMimeData *d, QStringList *files = 0) { - if (files) + if (files) { files->clear(); + } // Extract dropped files from Mime data. - if (!d->hasFormat(QLatin1String(uriListMimeFormatC))) + if (!d->hasFormat(QLatin1String(uriListMimeFormatC))) { return false; + } const QList urls = d->urls(); - if (urls.empty()) + if (urls.empty()) { return false; + } // Try to find local files bool hasFiles = false; const QList::const_iterator cend = urls.constEnd(); @@ -409,9 +410,10 @@ void MainWindow::dragEnterEvent(QDragEnterEvent *event) void MainWindow::dropEvent(QDropEvent *event) { QStringList files; + if (isDesktopFileManagerDrop(event->mimeData(), &files)) { event->accept(); - //openFiles(files); + // openFiles(files); } else { event->ignore(); } @@ -424,7 +426,7 @@ IContext *MainWindow::currentContextObject() const QStatusBar *MainWindow::statusBar() const { - return new QStatusBar();// m_modeStack->statusBar(); + return new QStatusBar(); // m_modeStack->statusBar(); } void MainWindow::registerDefaultContainers() @@ -494,6 +496,7 @@ void MainWindow::registerDefaultContainers() static Command *createSeparator(ActionManager *am, QObject *parent, const QString &name, const QList &context) { QAction *tmpaction = new QAction(parent); + tmpaction->setSeparator(true); Command *cmd = am->registerAction(tmpaction, name, context); return cmd; @@ -502,17 +505,18 @@ static Command *createSeparator(ActionManager *am, QObject *parent, const QStrin void MainWindow::registerDefaultActions() { ActionManagerPrivate *am = m_actionManager; - ActionContainer *mfile = am->actionContainer(Constants::M_FILE); - ActionContainer *medit = am->actionContainer(Constants::M_EDIT); - ActionContainer *mtools = am->actionContainer(Constants::M_TOOLS); + ActionContainer *mfile = am->actionContainer(Constants::M_FILE); + ActionContainer *medit = am->actionContainer(Constants::M_EDIT); + ActionContainer *mtools = am->actionContainer(Constants::M_TOOLS); ActionContainer *mwindow = am->actionContainer(Constants::M_WINDOW); - ActionContainer *mhelp = am->actionContainer(Constants::M_HELP); + ActionContainer *mhelp = am->actionContainer(Constants::M_HELP); // File menu separators Command *cmd = createSeparator(am, this, QLatin1String("QtCreator.File.Sep.Save"), m_globalContext); + mfile->addAction(cmd, Constants::G_FILE_SAVE); - cmd = createSeparator(am, this, QLatin1String("QtCreator.File.Sep.Close"), m_globalContext); + cmd = createSeparator(am, this, QLatin1String("QtCreator.File.Sep.Close"), m_globalContext); mfile->addAction(cmd, Constants::G_FILE_CLOSE); cmd = createSeparator(am, this, QLatin1String("QtCreator.File.Sep.Other"), m_globalContext); @@ -547,12 +551,12 @@ void MainWindow::registerDefaultActions() // New File Action /* - m_newAction = new QAction(QIcon(Constants::ICON_NEWFILE), tr("&New File or Project..."), this); - cmd = am->registerAction(m_newAction, Constants::NEW, m_globalContext); - cmd->setDefaultKeySequence(QKeySequence::New); - mfile->addAction(cmd, Constants::G_FILE_NEW); - connect(m_newAction, SIGNAL(triggered()), this, SLOT(newFile())); -*/ + m_newAction = new QAction(QIcon(Constants::ICON_NEWFILE), tr("&New File or Project..."), this); + cmd = am->registerAction(m_newAction, Constants::NEW, m_globalContext); + cmd->setDefaultKeySequence(QKeySequence::New); + mfile->addAction(cmd, Constants::G_FILE_NEW); + connect(m_newAction, SIGNAL(triggered()), this, SLOT(newFile())); + */ // Open Action /* @@ -561,7 +565,7 @@ void MainWindow::registerDefaultActions() cmd->setDefaultKeySequence(QKeySequence::Open); mfile->addAction(cmd, Constants::G_FILE_OPEN); connect(m_openAction, SIGNAL(triggered()), this, SLOT(openFile())); -*/ + */ // Open With Action /* @@ -569,14 +573,14 @@ void MainWindow::registerDefaultActions() cmd = am->registerAction(m_openWithAction, Constants::OPEN_WITH, m_globalContext); mfile->addAction(cmd, Constants::G_FILE_OPEN); connect(m_openWithAction, SIGNAL(triggered()), this, SLOT(openFileWith())); -*/ + */ - // File->Recent Files Menu + // File->Recent Files Menu /* ActionContainer *ac = am->createMenu(Constants::M_FILE_RECENTFILES); mfile->addMenu(ac, Constants::G_FILE_OPEN); ac->menu()->setTitle(tr("Recent Files")); -*/ + */ /* // Save Action QAction *tmpaction = new QAction(QIcon(Constants::ICON_SAVEFILE), tr("&Save"), this); @@ -589,13 +593,13 @@ void MainWindow::registerDefaultActions() // Save As Action tmpaction = new QAction(tr("Save &As..."), this); cmd = am->registerAction(tmpaction, Constants::SAVEAS, m_globalContext); -#ifdef Q_WS_MAC + #ifdef Q_WS_MAC cmd->setDefaultKeySequence(QKeySequence(tr("Ctrl+Shift+S"))); -#endif + #endif cmd->setAttribute(Command::CA_UpdateText); cmd->setDefaultText(tr("Save &As...")); mfile->addAction(cmd, Constants::G_FILE_SAVE); - */ + */ // SaveAll Action m_saveAllAction = new QAction(tr("Save &GCS Default Settings"), this); @@ -702,8 +706,8 @@ void MainWindow::registerDefaultActions() * UavGadgetManager Actions */ const QList uavGadgetManagerContext = - QList() << CoreImpl::instance()->uniqueIDManager()->uniqueIdentifier(Constants::C_UAVGADGETMANAGER); - //Window menu separators + QList() << CoreImpl::instance()->uniqueIDManager()->uniqueIdentifier(Constants::C_UAVGADGETMANAGER); + // Window menu separators QAction *tmpaction1 = new QAction(this); tmpaction1->setSeparator(true); cmd = am->registerAction(tmpaction1, QLatin1String("OpenPilot.Window.Sep.Split"), uavGadgetManagerContext); @@ -715,7 +719,7 @@ void MainWindow::registerDefaultActions() cmd->setDefaultKeySequence(QKeySequence(tr("Ctrl+Shift+F10"))); mwindow->addAction(cmd, Constants::G_WINDOW_HIDE_TOOLBAR); - //Window menu separators + // Window menu separators QAction *tmpaction2 = new QAction(this); tmpaction2->setSeparator(true); cmd = am->registerAction(tmpaction2, QLatin1String("OpenPilot.Window.Sep.Split2"), uavGadgetManagerContext); @@ -752,12 +756,12 @@ void MainWindow::registerDefaultActions() cmd->setDefaultKeySequence(QKeySequence(tr("%1+N").arg(prefix))); mwindow->addAction(cmd, Constants::G_WINDOW_SPLIT); - //Help Action + // Help Action tmpaction = new QAction(QIcon(Constants::ICON_HELP), tr("&Help..."), this); cmd = am->registerAction(tmpaction, Constants::G_HELP_HELP, m_globalContext); mhelp->addAction(cmd, Constants::G_HELP_HELP); tmpaction->setEnabled(true); - connect(tmpaction, SIGNAL(triggered()), this, SLOT(showHelp())); + connect(tmpaction, SIGNAL(triggered()), this, SLOT(showHelp())); // About sep #ifndef Q_WS_MAC // doesn't have the "About" actions in the Help menu @@ -777,9 +781,9 @@ void MainWindow::registerDefaultActions() #ifdef Q_WS_MAC #endif - connect(tmpaction, SIGNAL(triggered()), this, SLOT(aboutOpenPilotGCS())); + connect(tmpaction, SIGNAL(triggered()), this, SLOT(aboutOpenPilotGCS())); - //About Plugins Action + // About Plugins Action tmpaction = new QAction(QIcon(Constants::ICON_PLUGIN), tr("About &Plugins..."), this); cmd = am->registerAction(tmpaction, Constants::ABOUT_PLUGINS, m_globalContext); mhelp->addAction(cmd, Constants::G_HELP_ABOUT); @@ -787,9 +791,9 @@ void MainWindow::registerDefaultActions() #ifdef Q_WS_MAC cmd->action()->setMenuRole(QAction::ApplicationSpecificRole); #endif - connect(tmpaction, SIGNAL(triggered()), this, SLOT(aboutPlugins())); + connect(tmpaction, SIGNAL(triggered()), this, SLOT(aboutPlugins())); - //Credits Action + // Credits Action tmpaction = new QAction(QIcon(Constants::ICON_PLUGIN), tr("About &OpenPilot..."), this); cmd = am->registerAction(tmpaction, Constants::ABOUT_AUTHORS, m_globalContext); mhelp->addAction(cmd, Constants::G_HELP_ABOUT); @@ -797,29 +801,25 @@ void MainWindow::registerDefaultActions() #ifdef Q_WS_MAC cmd->action()->setMenuRole(QAction::ApplicationSpecificRole); #endif - connect(tmpaction, SIGNAL(triggered()), this, SLOT(aboutOpenPilotAuthors())); - - + connect(tmpaction, SIGNAL(triggered()), this, SLOT(aboutOpenPilotAuthors())); } void MainWindow::newFile() -{ -} +{} void MainWindow::openFile() -{ -} +{} /*static QList getNonEditorFileFactories() -{ + { QList tmp; return tmp; -} + } -static IFileFactory *findFileFactory(const QList &fileFactories, + static IFileFactory *findFileFactory(const QList &fileFactories, const MimeDatabase *db, const QFileInfo &fi) -{ + { if (const MimeType mt = db->findByFile(fi)) { const QString type = mt.type(); foreach (IFileFactory *factory, fileFactories) { @@ -828,11 +828,11 @@ static IFileFactory *findFileFactory(const QList &fileFactories, } } return 0; -} + } -// opens either an editor or loads a project -void MainWindow::openFiles(const QStringList &fileNames) -{ + // opens either an editor or loads a project + void MainWindow::openFiles(const QStringList &fileNames) + { QList nonEditorFileFactories = getNonEditorFileFactories(); foreach (const QString &fileName, fileNames) { @@ -844,16 +844,15 @@ void MainWindow::openFiles(const QStringList &fileNames) } } -}*/ + }*/ void MainWindow::setFocusToEditor() -{ - -} +{} bool MainWindow::showOptionsDialog(const QString &category, const QString &page, QWidget *parent) { emit m_coreImpl->optionsDialogRequested(); + if (!parent) { parent = this; } @@ -863,7 +862,9 @@ bool MainWindow::showOptionsDialog(const QString &category, const QString &page, void MainWindow::saveAll() { - if ( m_dontSaveSettings) return; + if (m_dontSaveSettings) { + return; + } emit m_coreImpl->saveSettingsRequested(); saveSettings(); // OpenPilot-specific. @@ -876,23 +877,23 @@ void MainWindow::exit() // since on close we are going to delete everything // so to prevent the deleting of that object we // just append it - QTimer::singleShot(0, this, SLOT(close())); + QTimer::singleShot(0, this, SLOT(close())); } void MainWindow::openFileWith() +{} + +void MainWindow::applyTabBarSettings(QTabWidget::TabPosition pos, bool movable) { - -} - -void MainWindow::applyTabBarSettings(QTabWidget::TabPosition pos, bool movable) { - if (m_modeStack->tabPosition() != pos) + if (m_modeStack->tabPosition() != pos) { m_modeStack->setTabPosition(pos); + } m_modeStack->setMovable(movable); } void MainWindow::showHelp() { - QDesktopServices::openUrl( QUrl(Constants::GCS_HELP, QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl(Constants::GCS_HELP, QUrl::StrictMode)); } ActionManager *MainWindow::actionManager() const @@ -912,20 +913,21 @@ MessageManager *MainWindow::messageManager() const QSettings *MainWindow::settings(QSettings::Scope scope) const { - if (scope == QSettings::UserScope) + if (scope == QSettings::UserScope) { return m_settings; - else + } else { return m_globalSettings; + } } VariableManager *MainWindow::variableManager() const { - return m_variableManager; + return m_variableManager; } ThreadManager *MainWindow::threadManager() const { - return m_threadManager; + return m_threadManager; } ConnectionManager *MainWindow::connectionManager() const @@ -933,7 +935,7 @@ ConnectionManager *MainWindow::connectionManager() const return m_connectionManager; } -QList MainWindow::uavGadgetManagers() const +QList MainWindow::uavGadgetManagers() const { return m_uavGadgetManagers; } @@ -954,7 +956,7 @@ MimeDatabase *MainWindow::mimeDatabase() const return m_mimeDatabase; } -GeneralSettings * MainWindow::generalSettings() const +GeneralSettings *MainWindow::generalSettings() const { return m_generalSettings; } @@ -966,43 +968,51 @@ IContext *MainWindow::contextObject(QWidget *widget) void MainWindow::addContextObject(IContext *context) { - if (!context) + if (!context) { return; + } QWidget *widget = context->widget(); - if (m_contextWidgets.contains(widget)) + if (m_contextWidgets.contains(widget)) { return; + } m_contextWidgets.insert(widget, context); } void MainWindow::removeContextObject(IContext *context) { - if (!context) + if (!context) { return; + } QWidget *widget = context->widget(); - if (!m_contextWidgets.contains(widget)) + if (!m_contextWidgets.contains(widget)) { return; + } m_contextWidgets.remove(widget); - if (m_activeContext == context) + if (m_activeContext == context) { updateContextObject(0); + } } void MainWindow::changeEvent(QEvent *e) { QMainWindow::changeEvent(e); + if (e->type() == QEvent::ActivationChange) { if (isActiveWindow()) { - if (debugMainWindow) + if (debugMainWindow) { qDebug() << "main window activated"; + } emit windowActivated(); } } else if (e->type() == QEvent::WindowStateChange) { #ifdef Q_WS_MAC bool minimized = isMinimized(); - if (debugMainWindow) + if (debugMainWindow) { qDebug() << "main window state changed to minimized=" << minimized; + } m_minimizeAction->setEnabled(!minimized); m_zoomAction->setEnabled(!minimized); #else @@ -1017,8 +1027,9 @@ void MainWindow::updateFocusWidget(QWidget *old, QWidget *now) Q_UNUSED(old) // Prevent changing the context object just because the menu is activated - if (qobject_cast(now)) + if (qobject_cast(now)) { return; + } IContext *newContext = 0; if (focusWidget()) { @@ -1038,16 +1049,18 @@ void MainWindow::updateFocusWidget(QWidget *old, QWidget *now) void MainWindow::updateContextObject(IContext *context) { - if (context == m_activeContext) + if (context == m_activeContext) { return; + } IContext *oldContext = m_activeContext; m_activeContext = context; if (!context || oldContext != m_activeContext) { emit m_coreImpl->contextAboutToChange(context); updateContext(); - if (debugMainWindow) + if (debugMainWindow) { qDebug() << "new context object =" << context << (context ? context->widget() : 0) << (context ? context->widget()->metaObject()->className() : 0); + } emit m_coreImpl->contextChanged(context); } } @@ -1059,8 +1072,8 @@ void MainWindow::resetContext() void MainWindow::shutdown() { - disconnect(QApplication::instance(), SIGNAL(focusChanged(QWidget*,QWidget*)), - this, SLOT(updateFocusWidget(QWidget*,QWidget*))); + disconnect(QApplication::instance(), SIGNAL(focusChanged(QWidget *, QWidget *)), + this, SLOT(updateFocusWidget(QWidget *, QWidget *))); m_activeContext = 0; // We have to remove all the existing gagdets at his point, not @@ -1079,21 +1092,23 @@ void MainWindow::showUavGadgetMenus(bool show, bool hasSplitter) m_gotoOtherSplitAction->setEnabled(show && hasSplitter); } -inline int takeLeastPriorityUavGadgetManager(const QList m_uavGadgetManagers) { +inline int takeLeastPriorityUavGadgetManager(const QList m_uavGadgetManagers) +{ int index = 0; - int prio = m_uavGadgetManagers.at(0)->priority(); + int prio = m_uavGadgetManagers.at(0)->priority(); + for (int i = 0; i < m_uavGadgetManagers.count(); i++) { int prio2 = m_uavGadgetManagers.at(i)->priority(); if (prio2 < prio) { - prio = prio2; + prio = prio2; index = i; } } return index; } -void MainWindow::createWorkspaces(QSettings* qs, bool diffOnly) { - +void MainWindow::createWorkspaces(QSettings *qs, bool diffOnly) +{ ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Core::UAVGadgetManager *uavGadgetManager; @@ -1101,12 +1116,14 @@ void MainWindow::createWorkspaces(QSettings* qs, bool diffOnly) { // If diffOnly is true, we only add/remove the number of workspaces // that has changed, // otherwise a complete reload of workspaces is done - int toRemoveFirst = m_uavGadgetManagers.count(); + int toRemoveFirst = m_uavGadgetManagers.count(); int newWorkspacesNo = m_workspaceSettings->numberOfWorkspaces(); - if (diffOnly && m_uavGadgetManagers.count() > newWorkspacesNo) + + if (diffOnly && m_uavGadgetManagers.count() > newWorkspacesNo) { toRemoveFirst = m_uavGadgetManagers.count() - newWorkspacesNo; - else + } else { toRemoveFirst = 0; + } int removed = 0; @@ -1132,11 +1149,11 @@ void MainWindow::createWorkspaces(QSettings* qs, bool diffOnly) { QElapsedTimer timer; timer.start(); - const QString name = m_workspaceSettings->name(i); + const QString name = m_workspaceSettings->name(i); const QString iconName = m_workspaceSettings->iconName(i); const QString modeName = m_workspaceSettings->modeName(i); uavGadgetManager = new Core::UAVGadgetManager(CoreImpl::instance(), name, QIcon(iconName), 90 - i + 1, modeName, - this); + this); connect(uavGadgetManager, SIGNAL(showUavGadgetMenus(bool, bool)), this, SLOT(showUavGadgetMenus(bool, bool))); @@ -1155,14 +1172,14 @@ void MainWindow::createWorkspaces(QSettings* qs, bool diffOnly) { qDebug() << "MainWindow::createWorkspaces - creating workspaces took" << totalTimer.elapsed() << "ms"; } -static const char *settingsGroup = "MainWindow"; -static const char *geometryKey = "Geometry"; +static const char *settingsGroup = "MainWindow"; +static const char *geometryKey = "Geometry"; static const char *colorKey = "Color"; static const char *maxKey = "Maximized"; -static const char *fullScreenKey = "FullScreen"; +static const char *fullScreenKey = "FullScreen"; static const char *modePriorities = "ModePriorities"; -void MainWindow::readSettings(QSettings* qs, bool workspaceDiffOnly) +void MainWindow::readSettings(QSettings *qs, bool workspaceDiffOnly) { if (!qs) { qs = m_settings; @@ -1201,17 +1218,16 @@ void MainWindow::readSettings(QSettings* qs, bool workspaceDiffOnly) qs->beginGroup(QLatin1String(modePriorities)); QStringList modeNames = qs->childKeys(); QMap map; - foreach (QString modeName, modeNames) { + foreach(QString modeName, modeNames) { map.insert(modeName, qs->value(modeName).toInt()); } m_modeManager->reorderModes(map); qs->endGroup(); - } -void MainWindow::saveSettings(QSettings* qs) +void MainWindow::saveSettings(QSettings *qs) { if (m_dontSaveSettings) { return; @@ -1228,8 +1244,8 @@ void MainWindow::saveSettings(QSettings* qs) qs->setValue(QLatin1String(colorKey), Utils::StyleHelper::baseColor()); if (windowState() & (Qt::WindowMaximized | Qt::WindowFullScreen)) { - qs->setValue(QLatin1String(maxKey), (bool) (windowState() & Qt::WindowMaximized)); - qs->setValue(QLatin1String(fullScreenKey), (bool) (windowState() & Qt::WindowFullScreen)); + qs->setValue(QLatin1String(maxKey), (bool)(windowState() & Qt::WindowMaximized)); + qs->setValue(QLatin1String(fullScreenKey), (bool)(windowState() & Qt::WindowFullScreen)); } else { qs->setValue(QLatin1String(maxKey), false); qs->setValue(QLatin1String(fullScreenKey), false); @@ -1240,49 +1256,48 @@ void MainWindow::saveSettings(QSettings* qs) // Write tab ordering qs->beginGroup(QLatin1String(modePriorities)); - QVector modes = m_modeManager->modes(); - foreach (IMode *mode, modes) { + QVector modes = m_modeManager->modes(); + foreach(IMode * mode, modes) { qs->setValue(QLatin1String(mode->uniqueModeName()), mode->priority()); } qs->endGroup(); - foreach (UAVGadgetManager *manager, m_uavGadgetManagers) { + foreach(UAVGadgetManager * manager, m_uavGadgetManagers) { manager->saveSettings(qs); } m_actionManager->saveSettings(qs); m_generalSettings->saveSettings(qs); qs->beginGroup("General"); - qs->setValue("Description",m_config_description); - qs->setValue("Details",m_config_details); - qs->setValue("StyleSheet",m_config_stylesheet); + qs->setValue("Description", m_config_description); + qs->setValue("Details", m_config_details); + qs->setValue("StyleSheet", m_config_stylesheet); qs->endGroup(); } -void MainWindow::readSettings(IConfigurablePlugin* plugin, QSettings* qs) +void MainWindow::readSettings(IConfigurablePlugin *plugin, QSettings *qs) { if (!qs) { qs = m_settings; } UAVConfigInfo configInfo; - QObject* qo = reinterpret_cast(plugin); + QObject *qo = reinterpret_cast(plugin); QString configName = qo->metaObject()->className(); qs->beginGroup("Plugins"); qs->beginGroup(configName); configInfo.read(qs); - configInfo.setNameOfConfigurable("Plugin-"+configName); + configInfo.setNameOfConfigurable("Plugin-" + configName); qs->beginGroup("data"); plugin->readConfig(qs, &configInfo); qs->endGroup(); qs->endGroup(); qs->endGroup(); - } -void MainWindow::saveSettings(IConfigurablePlugin* plugin, QSettings* qs) +void MainWindow::saveSettings(IConfigurablePlugin *plugin, QSettings *qs) { if (m_dontSaveSettings) { return; @@ -1303,7 +1318,6 @@ void MainWindow::saveSettings(IConfigurablePlugin* plugin, QSettings* qs) configInfo.save(qs); qs->endGroup(); qs->endGroup(); - } void MainWindow::deleteSettings() @@ -1315,21 +1329,25 @@ void MainWindow::deleteSettings() void MainWindow::addAdditionalContext(int context) { - if (context == 0) + if (context == 0) { return; + } - if (!m_additionalContexts.contains(context)) + if (!m_additionalContexts.contains(context)) { m_additionalContexts.prepend(context); + } } void MainWindow::removeAdditionalContext(int context) { - if (context == 0) + if (context == 0) { return; + } int index = m_additionalContexts.indexOf(context); - if (index != -1) + if (index != -1) { m_additionalContexts.removeAt(index); + } } bool MainWindow::hasContext(int context) const @@ -1341,16 +1359,18 @@ void MainWindow::updateContext() { QList contexts; - if (m_activeContext) + if (m_activeContext) { contexts += m_activeContext->context(); + } contexts += m_additionalContexts; QList uniquecontexts; for (int i = 0; i < contexts.size(); ++i) { const int c = contexts.at(i); - if (!uniquecontexts.contains(c)) + if (!uniquecontexts.contains(c)) { uniquecontexts << c; + } } m_actionManager->setContext(uniquecontexts); @@ -1360,6 +1380,7 @@ void MainWindow::aboutToShowRecentFiles() { ActionContainer *aci = m_actionManager->actionContainer(Constants::M_FILE_RECENTFILES); + if (aci) { aci->menu()->clear(); @@ -1371,12 +1392,13 @@ void MainWindow::aboutToShowRecentFiles() void MainWindow::openRecentFile() { - QAction *action = qobject_cast(sender()); - if (!action) + QAction *action = qobject_cast(sender()); + + if (!action) { return; - QString fileName = action->data().toString(); - if (!fileName.isEmpty()) { } + QString fileName = action->data().toString(); + if (!fileName.isEmpty()) {} } void MainWindow::aboutOpenPilotGCS() @@ -1419,22 +1441,24 @@ void MainWindow::destroyAuthorsDialog() void MainWindow::aboutPlugins() { PluginDialog dialog(this); + dialog.exec(); } void MainWindow::setFullScreen(bool on) { - if (bool(windowState() & Qt::WindowFullScreen) == on) + if (bool(windowState() & Qt::WindowFullScreen) == on) { return; + } if (on) { setWindowState(windowState() | Qt::WindowFullScreen); - //statusBar()->hide(); - //menuBar()->hide(); + // statusBar()->hide(); + // menuBar()->hide(); } else { setWindowState(windowState() & ~Qt::WindowFullScreen); - //menuBar()->show(); - //statusBar()->show(); + // menuBar()->show(); + // statusBar()->show(); } } @@ -1448,15 +1472,18 @@ bool MainWindow::showWarningWithOptions(const QString &title, const QString &settingsId, QWidget *parent) { - if (parent == 0) + if (parent == 0) { parent = this; + } QMessageBox msgBox(QMessageBox::Warning, title, text, QMessageBox::Ok, parent); - if (details.isEmpty()) + if (details.isEmpty()) { msgBox.setDetailedText(details); + } QAbstractButton *settingsButton = 0; - if (!settingsId.isEmpty() || !settingsCategory.isEmpty()) + if (!settingsId.isEmpty() || !settingsCategory.isEmpty()) { settingsButton = msgBox.addButton(tr("Settings..."), QMessageBox::AcceptRole); + } msgBox.exec(); if (settingsButton && msgBox.clickedButton() == settingsButton) { return showOptionsDialog(settingsCategory, settingsId); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.h b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.h index 641c8bf6c..a1aa1f508 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,7 +44,6 @@ class MyTabWidget; QT_END_NAMESPACE namespace Core { - class ActionManager; class BaseMode; class BaseView; @@ -67,7 +66,6 @@ class UAVGadgetInstanceManager; namespace Internal { - class ActionManagerPrivate; class CoreImpl; class FancyTabWidget; @@ -77,8 +75,7 @@ class WorkspaceSettings; class VersionDialog; class AuthorsDialog; -class CORE_EXPORT MainWindow : public EventFilteringMainWindow -{ +class CORE_EXPORT MainWindow : public EventFilteringMainWindow { Q_OBJECT public: @@ -93,17 +90,17 @@ public: void addContextObject(IContext *contex); void removeContextObject(IContext *contex); void resetContext(); - void readSettings(QSettings* qs = 0, bool workspaceDiffOnly = false); - void saveSettings(QSettings* qs = 0); - void readSettings(IConfigurablePlugin* plugin, QSettings* qs = 0); - void saveSettings(IConfigurablePlugin* plugin, QSettings* qs = 0); + void readSettings(QSettings *qs = 0, bool workspaceDiffOnly = false); + void saveSettings(QSettings *qs = 0); + void readSettings(IConfigurablePlugin *plugin, QSettings *qs = 0); + void saveSettings(IConfigurablePlugin *plugin, QSettings *qs = 0); void deleteSettings(); void openFiles(const QStringList &fileNames); Core::ActionManager *actionManager() const; Core::UniqueIDManager *uniqueIDManager() const; Core::MessageManager *messageManager() const; - QList uavGadgetManagers() const; + QList uavGadgetManagers() const; UAVGadgetInstanceManager *uavGadgetInstanceManager() const; Core::ConnectionManager *connectionManager() const; Core::VariableManager *variableManager() const; @@ -112,8 +109,11 @@ public: Core::MimeDatabase *mimeDatabase() const; Internal::GeneralSettings *generalSettings() const; QSettings *settings(QSettings::Scope scope) const; - inline SettingsDatabase *settingsDatabase() const { return m_settingsDatabase; } - IContext * currentContextObject() const; + inline SettingsDatabase *settingsDatabase() const + { + return m_settingsDatabase; + } + IContext *currentContextObject() const; QStatusBar *statusBar() const; void addAdditionalContext(int context); void removeAdditionalContext(int context); @@ -169,7 +169,7 @@ private: void updateContextObject(IContext *context); void registerDefaultContainers(); void registerDefaultActions(); - void createWorkspaces(QSettings* qs, bool diffOnly = false); + void createWorkspaces(QSettings *qs, bool diffOnly = false); QString loadStyleSheet(QString name); CoreImpl *m_coreImpl; @@ -185,7 +185,7 @@ private: VariableManager *m_variableManager; ThreadManager *m_threadManager; ModeManager *m_modeManager; - QList m_uavGadgetManagers; + QList m_uavGadgetManagers; UAVGadgetInstanceManager *m_uavGadgetInstanceManager; ConnectionManager *m_connectionManager; MimeDatabase *m_mimeDatabase; @@ -194,7 +194,7 @@ private: VersionDialog *m_versionDialog; AuthorsDialog *m_authorsDialog; - IContext * m_activeContext; + IContext *m_activeContext; QMap m_contextWidgets; @@ -226,9 +226,7 @@ private: QAction *m_minimizeAction; QAction *m_zoomAction; #endif - }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.cpp b/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.cpp index 2d15af167..a1c6a1c73 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.cpp @@ -10,19 +10,19 @@ * @{ * @brief The Core GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -65,10 +65,12 @@ bool styleEnabled(const QWidget *widget) { const QWidget *p = widget; + while (p) { - if (p->property("_q_custom_style_disabled").toBool()) + if (p->property("_q_custom_style_disabled").toBool()) { return false; - p = p->parentWidget(); + } + p = p->parentWidget(); } return true; } @@ -79,21 +81,21 @@ bool panelWidget(const QWidget *widget) const QWidget *p = widget; while (p) { - if (qobject_cast(p) && styleEnabled(p)) + if (qobject_cast(p) && styleEnabled(p)) { return true; - else if (qobject_cast(p) && styleEnabled(p)) + } else if (qobject_cast(p) && styleEnabled(p)) { return true; - else if (qobject_cast(p) && styleEnabled(p)) + } else if (qobject_cast(p) && styleEnabled(p)) { return true; - else if (p->property("panelwidget").toBool()) + } else if (p->property("panelwidget").toBool()) { return true; + } p = p->parentWidget(); } return false; } -class ManhattanStylePrivate -{ +class ManhattanStylePrivate { public: ManhattanStylePrivate(const QString &baseStyleName) { @@ -122,8 +124,7 @@ public: ManhattanStyle::ManhattanStyle(const QString &baseStyleName) : QWindowsStyle(), d(new ManhattanStylePrivate(baseStyleName)) -{ -} +{} ManhattanStyle::~ManhattanStyle() { @@ -142,57 +143,64 @@ void drawCornerImage(const QImage &img, QPainter *painter, QRect rect, int bottom = 0) { QSize size = img.size(); - if (top > 0) { //top - painter->drawImage(QRect(rect.left() + left, rect.top(), rect.width() -right - left, top), img, - QRect(left, 0, size.width() -right - left, top)); - if (left > 0) //top-left + + if (top > 0) { // top + painter->drawImage(QRect(rect.left() + left, rect.top(), rect.width() - right - left, top), img, + QRect(left, 0, size.width() - right - left, top)); + if (left > 0) { // top-left painter->drawImage(QRect(rect.left(), rect.top(), left, top), img, QRect(0, 0, left, top)); - if (right > 0) //top-right + } + if (right > 0) { // top-right painter->drawImage(QRect(rect.left() + rect.width() - right, rect.top(), right, top), img, QRect(size.width() - right, 0, right, top)); + } } - //left - if (left > 0) - painter->drawImage(QRect(rect.left(), rect.top()+top, left, rect.height() - top - bottom), img, + // left + if (left > 0) { + painter->drawImage(QRect(rect.left(), rect.top() + top, left, rect.height() - top - bottom), img, QRect(0, top, left, size.height() - bottom - top)); - //center - painter->drawImage(QRect(rect.left() + left, rect.top()+top, rect.width() -right - left, + } + // center + painter->drawImage(QRect(rect.left() + left, rect.top() + top, rect.width() - right - left, rect.height() - bottom - top), img, - QRect(left, top, size.width() -right -left, + QRect(left, top, size.width() - right - left, size.height() - bottom - top)); - if (right > 0) //right - painter->drawImage(QRect(rect.left() +rect.width() - right, rect.top()+top, right, rect.height() - top - bottom), img, + if (right > 0) { // right + painter->drawImage(QRect(rect.left() + rect.width() - right, rect.top() + top, right, rect.height() - top - bottom), img, QRect(size.width() - right, top, right, size.height() - bottom - top)); - if (bottom > 0) { //bottom - painter->drawImage(QRect(rect.left() +left, rect.top() + rect.height() - bottom, + } + if (bottom > 0) { // bottom + painter->drawImage(QRect(rect.left() + left, rect.top() + rect.height() - bottom, rect.width() - right - left, bottom), img, QRect(left, size.height() - bottom, size.width() - right - left, bottom)); - if (left > 0) //bottom-left - painter->drawImage(QRect(rect.left(), rect.top() + rect.height() - bottom, left, bottom), img, - QRect(0, size.height() - bottom, left, bottom)); - if (right > 0) //bottom-right - painter->drawImage(QRect(rect.left() + rect.width() - right, rect.top() + rect.height() - bottom, right, bottom), img, - QRect(size.width() - right, size.height() - bottom, right, bottom)); + if (left > 0) { // bottom-left + painter->drawImage(QRect(rect.left(), rect.top() + rect.height() - bottom, left, bottom), img, + QRect(0, size.height() - bottom, left, bottom)); + } + if (right > 0) { // bottom-right + painter->drawImage(QRect(rect.left() + rect.width() - right, rect.top() + rect.height() - bottom, right, bottom), img, + QRect(size.width() - right, size.height() - bottom, right, bottom)); + } } } QPixmap ManhattanStyle::generatedIconPixmap(QIcon::Mode iconMode, const QPixmap &pixmap, const QStyleOption *opt) const { QPixmap result; + result = d->style->generatedIconPixmap(iconMode, pixmap, opt); return result; } int ManhattanStyle::layoutSpacingImplementation(QSizePolicy::ControlType control1, - QSizePolicy::ControlType control2, - Qt::Orientation orientation, - const QStyleOption * option , - const QWidget * widget ) const + QSizePolicy::ControlType control2, + Qt::Orientation orientation, + const QStyleOption *option, + const QWidget *widget) const { return d->style->layoutSpacing(control1, control2, orientation, option, widget); - } QSize ManhattanStyle::sizeFromContents(ContentsType type, const QStyleOption *option, @@ -200,16 +208,18 @@ QSize ManhattanStyle::sizeFromContents(ContentsType type, const QStyleOption *op { QSize newSize = d->style->sizeFromContents(type, option, size, widget); - if (type == CT_Splitter && widget && widget->property("minisplitter").toBool()) + if (type == CT_Splitter && widget && widget->property("minisplitter").toBool()) { return QSize(1, 1); - else if (type == CT_ComboBox && panelWidget(widget)) + } else if (type == CT_ComboBox && panelWidget(widget)) { newSize += QSize(14, 0); + } return newSize; } QRect ManhattanStyle::subElementRect(SubElement element, const QStyleOption *option, const QWidget *widget) const { QRect rect; + rect = d->style->subElementRect(element, option, widget); return rect; } @@ -218,35 +228,30 @@ QRect ManhattanStyle::subControlRect(ComplexControl control, const QStyleOptionC SubControl subControl, const QWidget *widget) const { QRect rect; + #ifndef Q_WS_MACX // Not using OSX, size combo dropdown to fit contents - if(control == CC_ComboBox && subControl == SC_ComboBoxListBoxPopup) - { + if (control == CC_ComboBox && subControl == SC_ComboBoxListBoxPopup) { const QStyleOptionComboBox *cb = qstyleoption_cast(option); - const QComboBox* combo = qobject_cast(widget); + const QComboBox *combo = qobject_cast(widget); QRect comboRect = cb->rect; - int newWidth = combo->view()->sizeHintForColumn(0); - if(newWidth > comboRect.width()) - { + int newWidth = combo->view()->sizeHintForColumn(0); + if (newWidth > comboRect.width()) { // Set new rectangle, only width matters, list height is set by // combination of number of combo box items and setMaxVisibleItems rect.setRect(comboRect.x(), comboRect.y(), newWidth, comboRect.height()); rect = visualRect(cb->direction, cb->rect, rect); - } - else - { + } else { rect = d->style->subControlRect(control, option, subControl, widget); } - } - else - { + } else { rect = d->style->subControlRect(control, option, subControl, widget); } #else // Using OSX, use default style behaviour as this already sizes the // combo dropdown to fit rect = d->style->subControlRect(control, option, subControl, widget); -#endif +#endif // ifndef Q_WS_MACX return rect; } @@ -254,6 +259,7 @@ QStyle::SubControl ManhattanStyle::hitTestComplexControl(ComplexControl control, const QPoint &pos, const QWidget *widget) const { SubControl result = QStyle::SC_None; + result = d->style->hitTestComplexControl(control, option, pos, widget); return result; } @@ -261,34 +267,40 @@ QStyle::SubControl ManhattanStyle::hitTestComplexControl(ComplexControl control, int ManhattanStyle::pixelMetric(PixelMetric metric, const QStyleOption *option, const QWidget *widget) const { int retval = 0; + retval = d->style->pixelMetric(metric, option, widget); switch (metric) { case PM_SplitterWidth: - if (widget && widget->property("minisplitter").toBool()) + if (widget && widget->property("minisplitter").toBool()) { retval = 1; + } break; case PM_ToolBarIconSize: - if (panelWidget(widget)) + if (panelWidget(widget)) { retval = 16; + } break; case PM_MenuPanelWidth: case PM_MenuBarHMargin: case PM_MenuBarVMargin: case PM_ToolBarFrameWidth: - if (panelWidget(widget)) + if (panelWidget(widget)) { retval = 1; + } break; case PM_ButtonShiftVertical: case PM_ButtonShiftHorizontal: case PM_MenuBarPanelWidth: case PM_ToolBarItemMargin: case PM_ToolBarItemSpacing: - if (panelWidget(widget)) + if (panelWidget(widget)) { retval = 0; + } break; case PM_DefaultFrameWidth: - if (qobject_cast(widget) && panelWidget(widget)) + if (qobject_cast(widget) && panelWidget(widget)) { return 1; + } break; default: break; @@ -299,6 +311,7 @@ int ManhattanStyle::pixelMetric(PixelMetric metric, const QStyleOption *option, QPalette ManhattanStyle::standardPalette() const { QPalette result; + result = d->style->standardPalette(); return result; } @@ -317,6 +330,7 @@ QPalette panelPalette(const QPalette &oldPalette) { QColor color = Utils::StyleHelper::panelTextColor(); QPalette pal = oldPalette; + pal.setBrush(QPalette::All, QPalette::WindowText, color); pal.setBrush(QPalette::All, QPalette::ButtonText, color); pal.setBrush(QPalette::All, QPalette::Foreground, color); @@ -333,26 +347,25 @@ void ManhattanStyle::polish(QWidget *widget) // OxygenStyle forces a rounded widget mask on toolbars if (d->style->inherits("OxygenStyle")) { - if (qobject_cast(widget)) + if (qobject_cast(widget)) { widget->removeEventFilter(d->style); + } } if (panelWidget(widget)) { widget->setAttribute(Qt::WA_LayoutUsesWidgetRect, true); - if (qobject_cast(widget)) { + if (qobject_cast(widget)) { widget->setAttribute(Qt::WA_Hover); widget->setMaximumHeight(Utils::StyleHelper::navigationWidgetHeight() - 2); - } - else if (qobject_cast(widget)) { + } else if (qobject_cast(widget)) { widget->setAttribute(Qt::WA_Hover); widget->setMaximumHeight(Utils::StyleHelper::navigationWidgetHeight() - 2); - } - else if (qobject_cast(widget)) + } else if (qobject_cast(widget)) { widget->setPalette(panelPalette(widget->palette())); - else if (qobject_cast(widget) || widget->property("panelwidget_singlerow").toBool()) + } else if (qobject_cast(widget) || widget->property("panelwidget_singlerow").toBool()) { widget->setFixedHeight(Utils::StyleHelper::navigationWidgetHeight()); - else if (qobject_cast(widget)) + } else if (qobject_cast(widget)) { widget->setFixedHeight(Utils::StyleHelper::navigationWidgetHeight() + 2); - else if (qobject_cast(widget)) { + } else if (qobject_cast(widget)) { widget->setMaximumHeight(Utils::StyleHelper::navigationWidgetHeight() - 2); widget->setAttribute(Qt::WA_Hover); } @@ -364,12 +377,13 @@ void ManhattanStyle::unpolish(QWidget *widget) d->style->unpolish(widget); if (panelWidget(widget)) { widget->setAttribute(Qt::WA_LayoutUsesWidgetRect, false); - if (qobject_cast(widget)) + if (qobject_cast(widget)) { widget->setAttribute(Qt::WA_Hover, false); - else if (qobject_cast(widget)) + } else if (qobject_cast(widget)) { widget->setAttribute(Qt::WA_Hover, false); - else if (qobject_cast(widget)) + } else if (qobject_cast(widget)) { widget->setAttribute(Qt::WA_Hover, false); + } } } @@ -382,10 +396,12 @@ QIcon ManhattanStyle::standardIconImplementation(StandardPixmap standardIcon, co const QWidget *widget) const { QIcon icon; + switch (standardIcon) { case QStyle::SP_TitleBarCloseButton: case QStyle::SP_ToolBarHorizontalExtensionButton: return QIcon(standardPixmap(standardIcon, option, widget)); + default: icon = d->style->standardIcon(standardIcon, option, widget); } @@ -395,21 +411,24 @@ QIcon ManhattanStyle::standardIconImplementation(StandardPixmap standardIcon, co QPixmap ManhattanStyle::standardPixmap(StandardPixmap standardPixmap, const QStyleOption *opt, const QWidget *widget) const { - if (widget && !panelWidget(widget)) + if (widget && !panelWidget(widget)) { return d->style->standardPixmap(standardPixmap, opt, widget); + } QPixmap pixmap; switch (standardPixmap) { - case QStyle::SP_ToolBarHorizontalExtensionButton: { - static const QPixmap extButton(":/core/images/extension.png"); - pixmap = extButton; - } - break; - case QStyle::SP_TitleBarCloseButton: { - static const QPixmap closeButton(":/core/images/closebutton.png"); - pixmap = closeButton; - } - break; + case QStyle::SP_ToolBarHorizontalExtensionButton: + { + static const QPixmap extButton(":/core/images/extension.png"); + pixmap = extButton; + } + break; + case QStyle::SP_TitleBarCloseButton: + { + static const QPixmap closeButton(":/core/images/closebutton.png"); + pixmap = closeButton; + } + break; default: pixmap = d->style->standardPixmap(standardPixmap, opt, widget); } @@ -421,15 +440,18 @@ int ManhattanStyle::styleHint(StyleHint hint, const QStyleOption *option, const QStyleHintReturn *returnData) const { int ret = d->style->styleHint(hint, option, widget, returnData); + switch (hint) { // Make project explorer alternate rows all the way case QStyle::SH_ItemView_PaintAlternatingRowColorsForEmptyArea: - if (widget && widget->property("AlternateEmpty").toBool()) + if (widget && widget->property("AlternateEmpty").toBool()) { ret = true; + } break; case QStyle::SH_EtchDisabledText: - if (panelWidget(widget)) + if (panelWidget(widget)) { ret = false; + } break; default: break; @@ -440,16 +462,17 @@ int ManhattanStyle::styleHint(StyleHint hint, const QStyleOption *option, const void ManhattanStyle::drawPrimitive(PrimitiveElement element, const QStyleOption *option, QPainter *painter, const QWidget *widget) const { - if (!panelWidget(widget)) + if (!panelWidget(widget)) { return d->style->drawPrimitive(element, option, painter, widget); + } bool animating = (option->state & State_Animating); int state = option->state; - QRect rect = option->rect; + QRect rect = option->rect; QRect oldRect; QRect newRect; if (widget && (element == PE_PanelButtonTool) && !animating) { - QWidget *w = const_cast (widget); + QWidget *w = const_cast (widget); int oldState = w->property("_q_stylestate").toInt(); oldRect = w->property("_q_stylerect").toRect(); newRect = w->rect(); @@ -457,10 +480,9 @@ void ManhattanStyle::drawPrimitive(PrimitiveElement element, const QStyleOption w->setProperty("_q_stylerect", w->rect()); // Determine the animated transition - bool doTransition = ((state & State_On) != (oldState & State_On) || - (state & State_MouseOver) != (oldState & State_MouseOver)); - if (oldRect != newRect) - { + bool doTransition = ((state & State_On) != (oldState & State_On) || + (state & State_MouseOver) != (oldState & State_MouseOver)); + if (oldRect != newRect) { doTransition = false; d->animator.stopAnimation(widget); } @@ -468,9 +490,9 @@ void ManhattanStyle::drawPrimitive(PrimitiveElement element, const QStyleOption if (doTransition) { QImage startImage(option->rect.size(), QImage::Format_ARGB32_Premultiplied); QImage endImage(option->rect.size(), QImage::Format_ARGB32_Premultiplied); - Animation *anim = d->animator.widgetAnimation(widget); + Animation *anim = d->animator.widgetAnimation(widget); QStyleOption opt = *option; - opt.state = (QStyle::State)oldState; + opt.state = (QStyle::State)oldState; opt.state |= (State)State_Animating; startImage.fill(0); Transition *t = new Transition; @@ -490,239 +512,243 @@ void ManhattanStyle::drawPrimitive(PrimitiveElement element, const QStyleOption QPainter endPainter(&endImage); drawPrimitive(element, &endOpt, &endPainter, widget); t->setEndImage(endImage); - if (oldState & State_MouseOver) + if (oldState & State_MouseOver) { t->setDuration(150); - else + } else { t->setDuration(75); + } t->setStartTime(QTime::currentTime()); } } switch (element) { case PE_PanelLineEdit: - { - painter->save(); + { + painter->save(); - // Fill the line edit background - QRect filledRect = option->rect.adjusted(1, 1, -1, -1); - painter->setBrushOrigin(filledRect.topLeft()); - painter->fillRect(filledRect, option->palette.base()); + // Fill the line edit background + QRect filledRect = option->rect.adjusted(1, 1, -1, -1); + painter->setBrushOrigin(filledRect.topLeft()); + painter->fillRect(filledRect, option->palette.base()); - if (option->state & State_Enabled) - drawCornerImage(d->lineeditImage, painter, option->rect, 2, 2, 2, 2); - else - drawCornerImage(d->lineeditImage_disabled, painter, option->rect, 2, 2, 2, 2); - - if (option->state & State_HasFocus || option->state & State_MouseOver) { - QColor hover = Utils::StyleHelper::baseColor(); - if (state & State_HasFocus) - hover.setAlpha(100); - else - hover.setAlpha(50); - - painter->setPen(QPen(hover, 1)); - painter->drawRect(option->rect.adjusted(1, 1, -2 ,-2)); - } - painter->restore(); + if (option->state & State_Enabled) { + drawCornerImage(d->lineeditImage, painter, option->rect, 2, 2, 2, 2); + } else { + drawCornerImage(d->lineeditImage_disabled, painter, option->rect, 2, 2, 2, 2); } - break; + + if (option->state & State_HasFocus || option->state & State_MouseOver) { + QColor hover = Utils::StyleHelper::baseColor(); + if (state & State_HasFocus) { + hover.setAlpha(100); + } else { + hover.setAlpha(50); + } + + painter->setPen(QPen(hover, 1)); + painter->drawRect(option->rect.adjusted(1, 1, -2, -2)); + } + painter->restore(); + } + break; case PE_FrameStatusBarItem: break; - case PE_PanelButtonTool: { - Animation *anim = d->animator.widgetAnimation(widget); - if (!animating && anim) { - anim->paint(painter, option); - } else { - bool pressed = option->state & State_Sunken || option->state & State_On; - QColor shadow(0, 0, 0, 30); - painter->setPen(shadow); - if (pressed) { - QColor shade(0, 0, 0, 40); - painter->fillRect(rect, shade); - painter->drawLine(rect.topLeft() + QPoint(1, 0), rect.topRight() - QPoint(1, 0)); - painter->drawLine(rect.topLeft(), rect.bottomLeft()); - painter->drawLine(rect.topRight(), rect.bottomRight()); - // painter->drawLine(rect.bottomLeft() + QPoint(1, 0), rect.bottomRight() - QPoint(1, 0)); - QColor highlight(255, 255, 255, 30); - painter->setPen(highlight); - } - else if (option->state & State_Enabled && - option->state & State_MouseOver) { - QColor lighter(255, 255, 255, 37); - painter->fillRect(rect, lighter); - } - } + case PE_PanelButtonTool: + { + Animation *anim = d->animator.widgetAnimation(widget); + if (!animating && anim) { + anim->paint(painter, option); + } else { + bool pressed = option->state & State_Sunken || option->state & State_On; + QColor shadow(0, 0, 0, 30); + painter->setPen(shadow); + if (pressed) { + QColor shade(0, 0, 0, 40); + painter->fillRect(rect, shade); + painter->drawLine(rect.topLeft() + QPoint(1, 0), rect.topRight() - QPoint(1, 0)); + painter->drawLine(rect.topLeft(), rect.bottomLeft()); + painter->drawLine(rect.topRight(), rect.bottomRight()); + // painter->drawLine(rect.bottomLeft() + QPoint(1, 0), rect.bottomRight() - QPoint(1, 0)); + QColor highlight(255, 255, 255, 30); + painter->setPen(highlight); + } else if (option->state & State_Enabled && + option->state & State_MouseOver) { + QColor lighter(255, 255, 255, 37); + painter->fillRect(rect, lighter); + } } - break; + } + break; case PE_PanelStatusBar: - { - painter->save(); - QLinearGradient grad(option->rect.topLeft(), QPoint(rect.center().x(), rect.bottom())); - QColor startColor = Utils::StyleHelper::shadowColor().darker(164); - QColor endColor = Utils::StyleHelper::baseColor().darker(130); - grad.setColorAt(0, startColor); - grad.setColorAt(1, endColor); - painter->fillRect(option->rect, grad); - painter->setPen(QColor(255, 255, 255, 60)); - painter->drawLine(rect.topLeft() + QPoint(0,1), - rect.topRight()+ QPoint(0,1)); - painter->setPen(Utils::StyleHelper::borderColor().darker(110)); - painter->drawLine(rect.topLeft(), rect.topRight()); - painter->restore(); - } - break; + { + painter->save(); + QLinearGradient grad(option->rect.topLeft(), QPoint(rect.center().x(), rect.bottom())); + QColor startColor = Utils::StyleHelper::shadowColor().darker(164); + QColor endColor = Utils::StyleHelper::baseColor().darker(130); + grad.setColorAt(0, startColor); + grad.setColorAt(1, endColor); + painter->fillRect(option->rect, grad); + painter->setPen(QColor(255, 255, 255, 60)); + painter->drawLine(rect.topLeft() + QPoint(0, 1), + rect.topRight() + QPoint(0, 1)); + painter->setPen(Utils::StyleHelper::borderColor().darker(110)); + painter->drawLine(rect.topLeft(), rect.topRight()); + painter->restore(); + } + break; case PE_IndicatorToolBarSeparator: - { - QColor separatorColor = Utils::StyleHelper::borderColor(); - separatorColor.setAlpha(100); - painter->setPen(separatorColor); - const int margin = 6; - if (option->state & State_Horizontal) { - const int offset = rect.width()/2; - painter->drawLine(rect.bottomLeft().x() + offset, - rect.bottomLeft().y() - margin, - rect.topLeft().x() + offset, - rect.topLeft().y() + margin); - } else { //Draw vertical separator - const int offset = rect.height()/2; - painter->setPen(QPen(option->palette.background().color().darker(110))); - painter->drawLine(rect.topLeft().x() + margin , - rect.topLeft().y() + offset, - rect.topRight().x() - margin, - rect.topRight().y() + offset); - } + { + QColor separatorColor = Utils::StyleHelper::borderColor(); + separatorColor.setAlpha(100); + painter->setPen(separatorColor); + const int margin = 6; + if (option->state & State_Horizontal) { + const int offset = rect.width() / 2; + painter->drawLine(rect.bottomLeft().x() + offset, + rect.bottomLeft().y() - margin, + rect.topLeft().x() + offset, + rect.topLeft().y() + margin); + } else { // Draw vertical separator + const int offset = rect.height() / 2; + painter->setPen(QPen(option->palette.background().color().darker(110))); + painter->drawLine(rect.topLeft().x() + margin, + rect.topLeft().y() + offset, + rect.topRight().x() - margin, + rect.topRight().y() + offset); } - break; + } + break; case PE_IndicatorToolBarHandle: - { - bool horizontal = option->state & State_Horizontal; - painter->save(); - QPainterPath path; - int x = option->rect.x() + horizontal ? 2 : 6; - int y = option->rect.y() + horizontal ? 6 : 2; - static const int RectHeight = 2; - if (horizontal) { - while (y < option->rect.height() - RectHeight - 6) { - path.moveTo(x, y); - path.addRect(x, y, RectHeight, RectHeight); - y += 6; - } - } else { - while (x < option->rect.width() - RectHeight - 6) { - path.moveTo(x, y); - path.addRect(x, y, RectHeight, RectHeight); - x += 6; - } + { + bool horizontal = option->state & State_Horizontal; + painter->save(); + QPainterPath path; + int x = option->rect.x() + horizontal ? 2 : 6; + int y = option->rect.y() + horizontal ? 6 : 2; + static const int RectHeight = 2; + if (horizontal) { + while (y < option->rect.height() - RectHeight - 6) { + path.moveTo(x, y); + path.addRect(x, y, RectHeight, RectHeight); + y += 6; + } + } else { + while (x < option->rect.width() - RectHeight - 6) { + path.moveTo(x, y); + path.addRect(x, y, RectHeight, RectHeight); + x += 6; } - - painter->setPen(Qt::NoPen); - QColor dark = Utils::StyleHelper::borderColor(); - dark.setAlphaF(0.4); - - QColor light = Utils::StyleHelper::baseColor(); - light.setAlphaF(0.4); - - painter->fillPath(path, light); - painter->save(); - painter->translate(1, 1); - painter->fillPath(path, dark); - painter->restore(); - painter->translate(3, 3); - painter->fillPath(path, light); - painter->translate(1, 1); - painter->fillPath(path, dark); - painter->restore(); } - break; + + painter->setPen(Qt::NoPen); + QColor dark = Utils::StyleHelper::borderColor(); + dark.setAlphaF(0.4); + + QColor light = Utils::StyleHelper::baseColor(); + light.setAlphaF(0.4); + + painter->fillPath(path, light); + painter->save(); + painter->translate(1, 1); + painter->fillPath(path, dark); + painter->restore(); + painter->translate(3, 3); + painter->fillPath(path, light); + painter->translate(1, 1); + painter->fillPath(path, dark); + painter->restore(); + } + break; case PE_IndicatorArrowUp: case PE_IndicatorArrowDown: case PE_IndicatorArrowRight: case PE_IndicatorArrowLeft: - { - // From windowsstyle but modified to enable AA - if (option->rect.width() <= 1 || option->rect.height() <= 1) - break; - - QRect r = option->rect; - int size = qMin(r.height(), r.width()); - QPixmap pixmap; - QString pixmapName; - QTextStream(&pixmapName) << "$qt_ia" << "-" << metaObject()->className() << - "-" << uint(option->state) << "-" << element << "-" << size << - "-" << option->palette.cacheKey(); -// pixmapName.sprintf("%s-%s-%d-%d-%d-%lld", -// "$qt_ia", metaObject()->className(), -// uint(option->state), element, -// size, option->palette.cacheKey()); - if (!QPixmapCache::find(pixmapName, pixmap)) { - int border = size/5; - int sqsize = 2*(size/2); - QImage image(sqsize, sqsize, QImage::Format_ARGB32); - image.fill(Qt::transparent); - QPainter imagePainter(&image); - imagePainter.setRenderHint(QPainter::Antialiasing, true); - imagePainter.translate(0.5, 0.5); - QPolygon a; - switch (element) { - case PE_IndicatorArrowUp: - a.setPoints(3, border, sqsize/2, sqsize/2, border, sqsize - border, sqsize/2); - break; - case PE_IndicatorArrowDown: - a.setPoints(3, border, sqsize/2, sqsize/2, sqsize - border, sqsize - border, sqsize/2); - break; - case PE_IndicatorArrowRight: - a.setPoints(3, sqsize - border, sqsize/2, sqsize/2, border, sqsize/2, sqsize - border); - break; - case PE_IndicatorArrowLeft: - a.setPoints(3, border, sqsize/2, sqsize/2, border, sqsize/2, sqsize - border); - break; - default: - break; - } - - int bsx = 0; - int bsy = 0; - - if (option->state & State_Sunken) { - bsx = pixelMetric(PM_ButtonShiftHorizontal); - bsy = pixelMetric(PM_ButtonShiftVertical); - } - - QRect bounds = a.boundingRect(); - int sx = sqsize / 2 - bounds.center().x() - 1; - int sy = sqsize / 2 - bounds.center().y() - 1; - imagePainter.translate(sx + bsx, sy + bsy); - - if (!(option->state & State_Enabled)) { - QColor foreGround(150, 150, 150, 150); - imagePainter.setBrush(option->palette.mid().color()); - imagePainter.setPen(option->palette.mid().color()); - } else { - QColor shadow(0, 0, 0, 100); - imagePainter.translate(0, 1); - imagePainter.setPen(shadow); - imagePainter.setBrush(shadow); - QColor foreGround(255, 255, 255, 210); - imagePainter.drawPolygon(a); - imagePainter.translate(0, -1); - imagePainter.setPen(foreGround); - imagePainter.setBrush(foreGround); - } - imagePainter.drawPolygon(a); - imagePainter.end(); - pixmap = QPixmap::fromImage(image); - QPixmapCache::insert(pixmapName, pixmap); - } - int xOffset = r.x() + (r.width() - size)/2; - int yOffset = r.y() + (r.height() - size)/2; - painter->drawPixmap(xOffset, yOffset, pixmap); + { + // From windowsstyle but modified to enable AA + if (option->rect.width() <= 1 || option->rect.height() <= 1) { + break; } - break; + + QRect r = option->rect; + int size = qMin(r.height(), r.width()); + QPixmap pixmap; + QString pixmapName; + QTextStream(&pixmapName) << "$qt_ia" << "-" << metaObject()->className() << + "-" << uint(option->state) << "-" << element << "-" << size << + "-" << option->palette.cacheKey(); +// pixmapName.sprintf("%s-%s-%d-%d-%d-%lld", +// "$qt_ia", metaObject()->className(), +// uint(option->state), element, +// size, option->palette.cacheKey()); + if (!QPixmapCache::find(pixmapName, pixmap)) { + int border = size / 5; + int sqsize = 2 * (size / 2); + QImage image(sqsize, sqsize, QImage::Format_ARGB32); + image.fill(Qt::transparent); + QPainter imagePainter(&image); + imagePainter.setRenderHint(QPainter::Antialiasing, true); + imagePainter.translate(0.5, 0.5); + QPolygon a; + switch (element) { + case PE_IndicatorArrowUp: + a.setPoints(3, border, sqsize / 2, sqsize / 2, border, sqsize - border, sqsize / 2); + break; + case PE_IndicatorArrowDown: + a.setPoints(3, border, sqsize / 2, sqsize / 2, sqsize - border, sqsize - border, sqsize / 2); + break; + case PE_IndicatorArrowRight: + a.setPoints(3, sqsize - border, sqsize / 2, sqsize / 2, border, sqsize / 2, sqsize - border); + break; + case PE_IndicatorArrowLeft: + a.setPoints(3, border, sqsize / 2, sqsize / 2, border, sqsize / 2, sqsize - border); + break; + default: + break; + } + + int bsx = 0; + int bsy = 0; + + if (option->state & State_Sunken) { + bsx = pixelMetric(PM_ButtonShiftHorizontal); + bsy = pixelMetric(PM_ButtonShiftVertical); + } + + QRect bounds = a.boundingRect(); + int sx = sqsize / 2 - bounds.center().x() - 1; + int sy = sqsize / 2 - bounds.center().y() - 1; + imagePainter.translate(sx + bsx, sy + bsy); + + if (!(option->state & State_Enabled)) { + QColor foreGround(150, 150, 150, 150); + imagePainter.setBrush(option->palette.mid().color()); + imagePainter.setPen(option->palette.mid().color()); + } else { + QColor shadow(0, 0, 0, 100); + imagePainter.translate(0, 1); + imagePainter.setPen(shadow); + imagePainter.setBrush(shadow); + QColor foreGround(255, 255, 255, 210); + imagePainter.drawPolygon(a); + imagePainter.translate(0, -1); + imagePainter.setPen(foreGround); + imagePainter.setBrush(foreGround); + } + imagePainter.drawPolygon(a); + imagePainter.end(); + pixmap = QPixmap::fromImage(image); + QPixmapCache::insert(pixmapName, pixmap); + } + int xOffset = r.x() + (r.width() - size) / 2; + int yOffset = r.y() + (r.height() - size) / 2; + painter->drawPixmap(xOffset, yOffset, pixmap); + } + break; default: d->style->drawPrimitive(element, option, painter, widget); @@ -733,19 +759,20 @@ void ManhattanStyle::drawPrimitive(PrimitiveElement element, const QStyleOption void ManhattanStyle::drawControl(ControlElement element, const QStyleOption *option, QPainter *painter, const QWidget *widget) const { - if (!panelWidget(widget)) + if (!panelWidget(widget)) { return d->style->drawControl(element, option, painter, widget); + } switch (element) { case CE_MenuBarItem: painter->save(); - if (const QStyleOptionMenuItem *mbi = qstyleoption_cast(option)) { + if (const QStyleOptionMenuItem * mbi = qstyleoption_cast(option)) { QColor highlightOutline = Utils::StyleHelper::borderColor().lighter(120); bool act = mbi->state & State_Selected && mbi->state & State_Sunken; bool dis = !(mbi->state & State_Enabled); Utils::StyleHelper::menuGradient(painter, option->rect, option->rect); QStyleOptionMenuItem item = *mbi; - item.rect = mbi->rect; + item.rect = mbi->rect; QPalette pal = mbi->palette; pal.setBrush(QPalette::ButtonText, dis ? Qt::gray : Qt::black); item.palette = pal; @@ -770,10 +797,11 @@ void ManhattanStyle::drawControl(ControlElement element, const QStyleOption *opt painter->drawPoint(r.topLeft()); painter->drawPoint(r.topRight()); - QPalette pal = mbi->palette; + QPalette pal = mbi->palette; uint alignment = Qt::AlignCenter | Qt::TextShowMnemonic | Qt::TextDontClip | Qt::TextSingleLine; - if (!styleHint(SH_UnderlineShortcut, mbi, widget)) + if (!styleHint(SH_UnderlineShortcut, mbi, widget)) { alignment |= Qt::TextHideMnemonic; + } pal.setBrush(QPalette::Text, dis ? Qt::gray : QColor(0, 0, 0, 60)); drawItemText(painter, item.rect.translated(0, 1), alignment, pal, mbi->state & State_Enabled, mbi->text, QPalette::Text); pal.setBrush(QPalette::Text, dis ? Qt::gray : Qt::white); @@ -784,28 +812,30 @@ void ManhattanStyle::drawControl(ControlElement element, const QStyleOption *opt break; case CE_ComboBoxLabel: - if (const QStyleOptionComboBox *cb = qstyleoption_cast(option)) { + if (const QStyleOptionComboBox * cb = qstyleoption_cast(option)) { if (panelWidget(widget)) { - QRect editRect = subControlRect(CC_ComboBox, cb, SC_ComboBoxEditField, widget); + QRect editRect = subControlRect(CC_ComboBox, cb, SC_ComboBoxEditField, widget); QPalette customPal = cb->palette; if (!cb->currentIcon.isNull()) { QIcon::Mode mode = cb->state & State_Enabled ? QIcon::Normal - : QIcon::Disabled; - QPixmap pixmap = cb->currentIcon.pixmap(cb->iconSize, mode); + : QIcon::Disabled; + QPixmap pixmap = cb->currentIcon.pixmap(cb->iconSize, mode); QRect iconRect(editRect); iconRect.setWidth(cb->iconSize.width() + 4); iconRect = alignedRect(cb->direction, Qt::AlignLeft | Qt::AlignVCenter, iconRect.size(), editRect); - if (cb->editable) + if (cb->editable) { painter->fillRect(iconRect, customPal.brush(QPalette::Base)); + } drawItemPixmap(painter, iconRect, Qt::AlignCenter, pixmap); - if (cb->direction == Qt::RightToLeft) + if (cb->direction == Qt::RightToLeft) { editRect.translate(-4 - cb->iconSize.width(), 0); - else + } else { editRect.translate(cb->iconSize.width() + 4, 0); + } // Reserve some space for the down-arrow editRect.adjust(0, 0, -13, 0); @@ -827,104 +857,108 @@ void ManhattanStyle::drawControl(ControlElement element, const QStyleOption *opt } break; - case CE_SizeGrip: { - painter->save(); - QColor dark = Qt::white; - dark.setAlphaF(0.1); - int x, y, w, h; - option->rect.getRect(&x, &y, &w, &h); - int sw = qMin(h, w); - if (h > w) - painter->translate(0, h - w); - else - painter->translate(w - h, 0); - int sx = x; - int sy = y; - int s = 4; - painter->setPen(dark); - if (option->direction == Qt::RightToLeft) { - sx = x + sw; - for (int i = 0; i < 4; ++i) { - painter->drawLine(x, sy, sx, sw); - sx -= s; - sy += s; - } - } else { - for (int i = 0; i < 4; ++i) { - painter->drawLine(sx, sw, sw, sy); - sx += s; - sy += s; - } + case CE_SizeGrip: + { + painter->save(); + QColor dark = Qt::white; + dark.setAlphaF(0.1); + int x, y, w, h; + option->rect.getRect(&x, &y, &w, &h); + int sw = qMin(h, w); + if (h > w) { + painter->translate(0, h - w); + } else { + painter->translate(w - h, 0); + } + int sx = x; + int sy = y; + int s = 4; + painter->setPen(dark); + if (option->direction == Qt::RightToLeft) { + sx = x + sw; + for (int i = 0; i < 4; ++i) { + painter->drawLine(x, sy, sx, sw); + sx -= s; + sy += s; + } + } else { + for (int i = 0; i < 4; ++i) { + painter->drawLine(sx, sw, sw, sy); + sx += s; + sy += s; } - painter->restore(); } - break; + painter->restore(); + } + break; - case CE_MenuBarEmptyArea: { - Utils::StyleHelper::menuGradient(painter, option->rect, option->rect); - painter->save(); - painter->setPen(Utils::StyleHelper::borderColor()); - painter->drawLine(option->rect.bottomLeft(), option->rect.bottomRight()); - painter->restore(); - } - break; + case CE_MenuBarEmptyArea: + { + Utils::StyleHelper::menuGradient(painter, option->rect, option->rect); + painter->save(); + painter->setPen(Utils::StyleHelper::borderColor()); + painter->drawLine(option->rect.bottomLeft(), option->rect.bottomRight()); + painter->restore(); + } + break; case CE_ToolBar: - { - QString key; - key.sprintf("mh_toolbar %d %d %d", option->rect.width(), option->rect.height(), Utils::StyleHelper::baseColor().rgb());; + { + QString key; + key.sprintf("mh_toolbar %d %d %d", option->rect.width(), option->rect.height(), Utils::StyleHelper::baseColor().rgb());; - QPixmap pixmap; - QPainter *p = painter; - QRect rect = option->rect; - if (Utils::StyleHelper::usePixmapCache() && !QPixmapCache::find(key, pixmap)) { - pixmap = QPixmap(option->rect.size()); - p = new QPainter(&pixmap); - rect = QRect(0, 0, option->rect.width(), option->rect.height()); - } - - bool horizontal = option->state & State_Horizontal; - // Map offset for global window gradient - QPoint offset = widget->window()->mapToGlobal(option->rect.topLeft()) - - widget->mapToGlobal(option->rect.topLeft()); - QRect gradientSpan; - if (widget) { - gradientSpan = QRect(offset, widget->window()->size()); - } - if (horizontal) - Utils::StyleHelper::horizontalGradient(p, gradientSpan, rect); - else - Utils::StyleHelper::verticalGradient(p, gradientSpan, rect); - - painter->setPen(Utils::StyleHelper::borderColor()); - - if (horizontal) { - // Note: This is a hack to determine if the - // toolbar should draw the top or bottom outline - // (needed for the find toolbar for instance) - QColor lighter(255, 255, 255, 40); - if (widget && widget->property("topBorder").toBool()) { - p->drawLine(rect.topLeft(), rect.topRight()); - p->setPen(lighter); - p->drawLine(rect.topLeft() + QPoint(0, 1), rect.topRight() + QPoint(0, 1)); - } else { - p->drawLine(rect.bottomLeft(), rect.bottomRight()); - p->setPen(lighter); - p->drawLine(rect.topLeft(), rect.topRight()); - } - } else { - p->drawLine(rect.topLeft(), rect.bottomLeft()); - p->drawLine(rect.topRight(), rect.bottomRight()); - } - - if (Utils::StyleHelper::usePixmapCache() && !QPixmapCache::find(key, pixmap)) { - painter->drawPixmap(rect.topLeft(), pixmap); - p->end(); - delete p; - QPixmapCache::insert(key, pixmap); - } + QPixmap pixmap; + QPainter *p = painter; + QRect rect = option->rect; + if (Utils::StyleHelper::usePixmapCache() && !QPixmapCache::find(key, pixmap)) { + pixmap = QPixmap(option->rect.size()); + p = new QPainter(&pixmap); + rect = QRect(0, 0, option->rect.width(), option->rect.height()); } - break; + + bool horizontal = option->state & State_Horizontal; + // Map offset for global window gradient + QPoint offset = widget->window()->mapToGlobal(option->rect.topLeft()) - + widget->mapToGlobal(option->rect.topLeft()); + QRect gradientSpan; + if (widget) { + gradientSpan = QRect(offset, widget->window()->size()); + } + if (horizontal) { + Utils::StyleHelper::horizontalGradient(p, gradientSpan, rect); + } else { + Utils::StyleHelper::verticalGradient(p, gradientSpan, rect); + } + + painter->setPen(Utils::StyleHelper::borderColor()); + + if (horizontal) { + // Note: This is a hack to determine if the + // toolbar should draw the top or bottom outline + // (needed for the find toolbar for instance) + QColor lighter(255, 255, 255, 40); + if (widget && widget->property("topBorder").toBool()) { + p->drawLine(rect.topLeft(), rect.topRight()); + p->setPen(lighter); + p->drawLine(rect.topLeft() + QPoint(0, 1), rect.topRight() + QPoint(0, 1)); + } else { + p->drawLine(rect.bottomLeft(), rect.bottomRight()); + p->setPen(lighter); + p->drawLine(rect.topLeft(), rect.topRight()); + } + } else { + p->drawLine(rect.topLeft(), rect.bottomLeft()); + p->drawLine(rect.topRight(), rect.bottomRight()); + } + + if (Utils::StyleHelper::usePixmapCache() && !QPixmapCache::find(key, pixmap)) { + painter->drawPixmap(rect.topLeft(), pixmap); + p->end(); + delete p; + QPixmapCache::insert(key, pixmap); + } + } + break; default: d->style->drawControl(element, option, painter, widget); @@ -935,15 +969,16 @@ void ManhattanStyle::drawControl(ControlElement element, const QStyleOption *opt void ManhattanStyle::drawComplexControl(ComplexControl control, const QStyleOptionComplex *option, QPainter *painter, const QWidget *widget) const { - if (!panelWidget(widget)) - return d->style->drawComplexControl(control, option, painter, widget); + if (!panelWidget(widget)) { + return d->style->drawComplexControl(control, option, painter, widget); + } QRect rect = option->rect; switch (control) { case CC_ToolButton: - if (const QStyleOptionToolButton *toolbutton = qstyleoption_cast(option)) { + if (const QStyleOptionToolButton * toolbutton = qstyleoption_cast(option)) { QRect button, menuarea; - button = subControlRect(control, toolbutton, SC_ToolButton, widget); + button = subControlRect(control, toolbutton, SC_ToolButton, widget); menuarea = subControlRect(control, toolbutton, SC_ToolButtonMenu, widget); State bflags = toolbutton->state; @@ -955,16 +990,18 @@ void ManhattanStyle::drawComplexControl(ComplexControl control, const QStyleOpti State mflags = bflags; if (toolbutton->state & State_Sunken) { - if (toolbutton->activeSubControls & SC_ToolButton) + if (toolbutton->activeSubControls & SC_ToolButton) { bflags |= State_Sunken; - if (toolbutton->activeSubControls & SC_ToolButtonMenu) + } + if (toolbutton->activeSubControls & SC_ToolButtonMenu) { mflags |= State_Sunken; + } } QStyleOption tool(0); tool.palette = toolbutton->palette; if (toolbutton->subControls & SC_ToolButton) { - tool.rect = button; + tool.rect = button; tool.state = bflags; drawPrimitive(PE_PanelButtonTool, &tool, painter, widget); } @@ -973,9 +1010,10 @@ void ManhattanStyle::drawComplexControl(ComplexControl control, const QStyleOpti QStyleOptionFocusRect fr; fr.QStyleOption::operator=(*toolbutton); fr.rect.adjust(3, 3, -3, -3); - if (toolbutton->features & QStyleOptionToolButton::MenuButtonPopup) + if (toolbutton->features & QStyleOptionToolButton::MenuButtonPopup) { fr.rect.adjust(0, 0, -pixelMetric(QStyle::PM_MenuButtonIndicator, toolbutton, widget), 0); + } QPen oldPen = painter->pen(); QColor focusColor = Utils::StyleHelper::panelTextColor(); focusColor.setAlpha(120); @@ -990,12 +1028,12 @@ void ManhattanStyle::drawComplexControl(ComplexControl control, const QStyleOpti QStyleOptionToolButton label = *toolbutton; label.palette = panelPalette(option->palette); int fw = pixelMetric(PM_DefaultFrameWidth, option, widget); - label.rect = button.adjusted(fw, fw, -fw, -fw); + label.rect = button.adjusted(fw, fw, -fw, -fw); drawControl(CE_ToolButtonLabel, &label, painter, widget); if (toolbutton->subControls & SC_ToolButtonMenu) { tool.state = mflags; - tool.rect = menuarea.adjusted(1, 1, -1, -1); + tool.rect = menuarea.adjusted(1, 1, -1, -1); if (mflags & (State_Sunken | State_On | State_Raised)) { painter->setPen(Qt::gray); painter->drawLine(tool.rect.topLeft(), tool.rect.bottomLeft()); @@ -1017,15 +1055,15 @@ void ManhattanStyle::drawComplexControl(ComplexControl control, const QStyleOpti QRect ir = toolbutton->rect.adjusted(1, 1, -1, -1); QStyleOptionToolButton newBtn = *toolbutton; newBtn.palette = panelPalette(option->palette); - newBtn.rect = QRect(ir.right() - arrowSize - 1, - ir.height() - arrowSize - 2, arrowSize, arrowSize); + newBtn.rect = QRect(ir.right() - arrowSize - 1, + ir.height() - arrowSize - 2, arrowSize, arrowSize); drawPrimitive(PE_IndicatorArrowDown, &newBtn, painter, widget); } } break; case CC_ComboBox: - if (const QStyleOptionComboBox *cb = qstyleoption_cast(option)) { + if (const QStyleOptionComboBox * cb = qstyleoption_cast(option)) { painter->save(); bool isEmpty = cb->currentText.isEmpty() && cb->currentIcon.isNull(); bool reverse = option->direction == Qt::RightToLeft; @@ -1043,30 +1081,34 @@ void ManhattanStyle::drawComplexControl(ComplexControl control, const QStyleOpti grad.setColorAt(0.7, QColor(0, 0, 0, 70)); grad.setColorAt(1, QColor(0, 0, 0, 40)); painter->setPen(QPen(grad, 0)); - if (!reverse) - painter->drawLine(rect.topRight() - QPoint(1,0), rect.bottomRight() - QPoint(1,0)); - else + if (!reverse) { + painter->drawLine(rect.topRight() - QPoint(1, 0), rect.bottomRight() - QPoint(1, 0)); + } else { painter->drawLine(rect.topLeft(), rect.bottomLeft()); + } QStyleOption toolbutton = *option; - if (isEmpty) + if (isEmpty) { toolbutton.state &= ~(State_Enabled | State_Sunken); + } painter->save(); painter->setClipRect(toolbutton.rect.adjusted(0, 0, -2, 0)); drawPrimitive(PE_PanelButtonTool, &toolbutton, painter, widget); painter->restore(); // Draw arrow int menuButtonWidth = 12; - int left = !reverse ? rect.right() - menuButtonWidth : rect.left(); + int left = !reverse ? rect.right() - menuButtonWidth : rect.left(); int right = !reverse ? rect.right() : rect.left() + menuButtonWidth; QRect arrowRect((left + right) / 2 + (reverse ? 6 : -6), rect.center().y() - 3, 9, 9); - if (option->state & State_On) + if (option->state & State_On) { arrowRect.translate(d->style->pixelMetric(PM_ButtonShiftHorizontal, option, widget), d->style->pixelMetric(PM_ButtonShiftVertical, option, widget)); + } QStyleOption arrowOpt = *option; arrowOpt.rect = arrowRect; - if (isEmpty) + if (isEmpty) { arrowOpt.state &= ~(State_Enabled | State_Sunken); + } if (styleHint(SH_ComboBox_Popup, option, widget)) { arrowOpt.rect.translate(0, -3); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.h b/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.h index 241e41e84..bb90cf544 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,8 +40,7 @@ QT_END_NAMESPACE class ManhattanStylePrivate; -class CORE_EXPORT ManhattanStyle : public QWindowsStyle -{ +class CORE_EXPORT ManhattanStyle : public QWindowsStyle { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.cpp index d9e2d373f..a2996c8bf 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -47,6 +47,7 @@ MessageManager::MessageManager() MessageManager::~MessageManager() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + if (pm && m_messageOutputWindow) { pm->removeObject(m_messageOutputWindow); delete m_messageOutputWindow; @@ -63,22 +64,25 @@ void MessageManager::init() void MessageManager::showOutputPane() { - if (m_messageOutputWindow) + if (m_messageOutputWindow) { m_messageOutputWindow->popup(false); + } } void MessageManager::displayStatusBarMessage(const QString & /*text*/, int /*ms*/) { // TODO: Currently broken, but noone really notices, so... - //m_mainWindow->statusBar()->showMessage(text, ms); + // m_mainWindow->statusBar()->showMessage(text, ms); } void MessageManager::printToOutputPane(const QString &text, bool bringToForeground) { - if (!m_messageOutputWindow) + if (!m_messageOutputWindow) { return; - if (bringToForeground) + } + if (bringToForeground) { m_messageOutputWindow->popup(false); + } m_messageOutputWindow->append(text); } @@ -91,4 +95,3 @@ void MessageManager::printToOutputPane(const QString &text) { printToOutputPane(text, false); } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.h b/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.h index 60164a07e..1927b70fa 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,13 +33,11 @@ #include namespace Core { - namespace Internal { class MessageOutputWindow; } -class CORE_EXPORT MessageManager : public QObject -{ +class CORE_EXPORT MessageManager : public QObject { Q_OBJECT public: @@ -48,7 +46,10 @@ public: void init(); - static MessageManager *instance() { return m_instance; } + static MessageManager *instance() + { + return m_instance; + } void displayStatusBarMessage(const QString &text, int ms = 0); void showOutputPane(); @@ -63,7 +64,6 @@ private: static MessageManager *m_instance; }; - } // namespace Core #endif // MESSAGEMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.cpp b/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.cpp index a30fbd1c1..d4c4560ea 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -76,8 +76,7 @@ QString MessageOutputWindow::name() const } void MessageOutputWindow::visibilityChanged(bool /*b*/) -{ -} +{} void MessageOutputWindow::append(const QString &text) { @@ -100,14 +99,10 @@ bool MessageOutputWindow::canPrevious() } void MessageOutputWindow::goToNext() -{ - -} +{} void MessageOutputWindow::goToPrev() -{ - -} +{} bool MessageOutputWindow::canNavigate() { diff --git a/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.h b/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.h index f2fb5877d..fb76d3bb3 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,9 +37,7 @@ QT_END_NAMESPACE namespace Core { namespace Internal { - -class MessageOutputWindow : public Core::IOutputPane -{ +class MessageOutputWindow : public Core::IOutputPane { Q_OBJECT public: @@ -47,7 +45,10 @@ public: ~MessageOutputWindow(); QWidget *outputWidget(QWidget *parent); - QList toolBarWidgets() const { return QList(); } + QList toolBarWidgets() const + { + return QList(); + } QString name() const; int priorityInStatusBar() const; @@ -68,7 +69,6 @@ public: private: QTextEdit *m_widget; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.cpp b/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.cpp index 609316b53..b037fc35b 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -49,51 +49,50 @@ enum { debugMimeDB = 0 }; // XML tags in mime files -static const char *mimeInfoTagC = "mime-info"; -static const char *mimeTypeTagC = "mime-type"; -static const char *mimeTypeAttributeC = "type"; -static const char *subClassTagC = "sub-class-of"; -static const char *commentTagC = "comment"; -static const char *globTagC = "glob"; -static const char *aliasTagC = "alias"; -static const char *patternAttributeC = "pattern"; -static const char *localeAttributeC = "xml:lang"; +static const char *mimeInfoTagC = "mime-info"; +static const char *mimeTypeTagC = "mime-type"; +static const char *mimeTypeAttributeC = "type"; +static const char *subClassTagC = "sub-class-of"; +static const char *commentTagC = "comment"; +static const char *globTagC = "glob"; +static const char *aliasTagC = "alias"; +static const char *patternAttributeC = "pattern"; +static const char *localeAttributeC = "xml:lang"; -static const char *magicTagC = "magic"; -static const char *priorityAttributeC = "priority"; -static const char *matchTagC = "match"; -static const char *matchValueAttributeC = "value"; -static const char *matchTypeAttributeC = "type"; -static const char *matchStringTypeValueC = "string"; -static const char *matchOffsetAttributeC = "offset"; +static const char *magicTagC = "magic"; +static const char *priorityAttributeC = "priority"; +static const char *matchTagC = "match"; +static const char *matchValueAttributeC = "value"; +static const char *matchTypeAttributeC = "type"; +static const char *matchStringTypeValueC = "string"; +static const char *matchOffsetAttributeC = "offset"; // Types -static const char *textTypeC = "text/plain"; -static const char *binaryTypeC = "application/octet-stream"; +static const char *textTypeC = "text/plain"; +static const char *binaryTypeC = "application/octet-stream"; // UTF16 byte order marks -static const char bigEndianByteOrderMarkC[] = "\xFE\xFF"; +static const char bigEndianByteOrderMarkC[] = "\xFE\xFF"; static const char littleEndianByteOrderMarkC[] = "\xFF\xFE"; // Fallback priorities, must be low. -enum { BinaryMatchPriority = 1, TextMatchPriority = 2}; +enum { BinaryMatchPriority = 1, TextMatchPriority = 2 }; /* Parse sth like ( being optional): - *\code -?xml version="1.0" encoding="UTF-8"?> - - - + *\code + ?xml version="1.0" encoding="UTF-8"?> + + + Qt QMake Profile - - - *\endcode -*/ + + + *\endcode + */ namespace Core { namespace Internal { - // FileMatchContext: Passed on to the mimetypes from the database // when looking for a file match. It exists to enable reading the file // contents "on demand" (as opposed to each mime type trying to open @@ -107,7 +106,10 @@ public: explicit FileMatchContext(const QFileInfo &fi); - inline QString fileName() const { return m_fileName; } + inline QString fileName() const + { + return m_fileName; + } // Return (cached) first MaxData bytes of file QByteArray data(); @@ -118,7 +120,8 @@ private: // Not read yet DataNotRead, // Available - DataRead }; + DataRead + }; const QFileInfo m_fileInfo; const QString m_fileName; State m_state; @@ -129,8 +132,7 @@ FileMatchContext::FileMatchContext(const QFileInfo &fi) : m_fileInfo(fi), m_fileName(fi.fileName()), m_state(fi.isFile() && fi.isReadable() && fi.size() > 0 ? DataNotRead : NoDataAvailable) -{ -} +{} QByteArray FileMatchContext::data() { @@ -139,7 +141,7 @@ QByteArray FileMatchContext::data() const QString fullName = m_fileInfo.absoluteFilePath(); QFile file(fullName); if (file.open(QIODevice::ReadOnly)) { - m_data = file.read(MaxData); + m_data = file.read(MaxData); m_state = DataRead; } else { qWarning("%s failed to open %s: %s\n", Q_FUNC_INFO, fullName.toUtf8().constData(), file.errorString().toUtf8().constData()); @@ -154,8 +156,14 @@ class BinaryMatcher : public IMagicMatcher { Q_DISABLE_COPY(BinaryMatcher) public: BinaryMatcher() {} - virtual bool matches(const QByteArray & /*data*/) const { return true; } - virtual int priority() const { return BinaryMatchPriority; } + virtual bool matches(const QByteArray & /*data*/) const + { + return true; + } + virtual int priority() const + { + return BinaryMatchPriority; + } }; // A heuristic text file matcher: If the data do not contain any character @@ -165,7 +173,10 @@ class HeuristicTextMagicMatcher : public IMagicMatcher { public: HeuristicTextMagicMatcher() {} virtual bool matches(const QByteArray &data) const; - virtual int priority() const { return TextMatchPriority; } + virtual int priority() const + { + return TextMatchPriority; + } static bool isTextFile(const QByteArray &data); }; @@ -173,12 +184,15 @@ public: bool HeuristicTextMagicMatcher::isTextFile(const QByteArray &data) { const int size = data.size(); + for (int i = 0; i < size; i++) { const char c = data.at(i); - if (c >= 0x01 && c < 0x09) // Sure-fire binary + if (c >= 0x01 && c < 0x09) { // Sure-fire binary return false; - if (c == 0) // Check for UTF16 + } + if (c == 0) { // Check for UTF16 return data.startsWith(bigEndianByteOrderMarkC) || data.startsWith(littleEndianByteOrderMarkC); + } } return true; } @@ -186,11 +200,12 @@ bool HeuristicTextMagicMatcher::isTextFile(const QByteArray &data) bool HeuristicTextMagicMatcher::matches(const QByteArray &data) const { const bool rc = isTextFile(data); - if (debugMimeDB) + + if (debugMimeDB) { qDebug() << Q_FUNC_INFO << " on " << data.size() << " returns " << rc; + } return rc; } - } // namespace Internal // MagicRule @@ -198,18 +213,20 @@ MagicRule::MagicRule(const QByteArray &pattern, int startPos, int endPos) : m_pattern(pattern), m_startPos(startPos), m_endPos(endPos) -{ -} +{} bool MagicRule::matches(const QByteArray &data) const { // Quick check const int dataSize = data.size(); - if ((m_startPos + m_pattern.size()) >= dataSize) + + if ((m_startPos + m_pattern.size()) >= dataSize) { return false; + } // Most common: some string at position 0: - if (m_startPos == 0 && m_startPos == m_endPos) + if (m_startPos == 0 && m_startPos == m_endPos) { return data.startsWith(m_pattern); + } // Range const int index = data.indexOf(m_pattern, m_startPos); return index != -1 && index < m_endPos; @@ -222,9 +239,8 @@ MagicRule *MagicRule::createStringRule(const QString &c, int startPos, int endPo // List matcher MagicRuleMatcher::MagicRuleMatcher() : - m_priority(65535) -{ -} + m_priority(65535) +{} void MagicRuleMatcher::add(const MagicRuleSharedPointer &rule) { @@ -234,9 +250,12 @@ void MagicRuleMatcher::add(const MagicRuleSharedPointer &rule) bool MagicRuleMatcher::matches(const QByteArray &data) const { const MagicRuleList::const_iterator cend = m_list.constEnd(); - for (MagicRuleList::const_iterator it = m_list.constBegin(); it != cend; ++it) - if ( (*it)->matches(data)) + + for (MagicRuleList::const_iterator it = m_list.constBegin(); it != cend; ++it) { + if ((*it)->matches(data)) { return true; + } + } return false; } @@ -253,7 +272,7 @@ void MagicRuleMatcher::setPriority(int p) // ---------- MimeTypeData class MimeTypeData : public QSharedData { public: - typedef QHash LocaleHash; + typedef QHash LocaleHash; void clear(); void debug(QTextStream &str, int indent = 0) const; @@ -287,21 +306,24 @@ void MimeTypeData::clear() void MimeTypeData::debug(QTextStream &str, int indent) const { const QString indentS = QString(indent, QLatin1Char(' ')); - const QString comma = QString(1, QLatin1Char(',')); + const QString comma = QString(1, QLatin1Char(',')); + str << indentS << "Type: " << type; - if (!aliases.empty()) + if (!aliases.empty()) { str << " Aliases: " << aliases.join(comma); + } str << ", magic: " << magicMatchers.size() << '\n'; str << indentS << "Comment: " << comment << '\n'; - if (!subClassesOf.empty()) + if (!subClassesOf.empty()) { str << indentS << "SubClassesOf: " << subClassesOf.join(comma) << '\n'; + } if (!globPatterns.empty()) { str << indentS << "Glob: "; - foreach (const QRegExp &r, globPatterns) - str << r.pattern() << ' '; + foreach(const QRegExp &r, globPatterns) + str << r.pattern() << ' '; str << '\n'; if (!suffixes.empty()) { - str << indentS << "Suffixes: " << suffixes.join(comma) + str << indentS << "Suffixes: " << suffixes.join(comma) << " preferred: " << preferredSuffix << '\n'; } } @@ -311,29 +333,26 @@ void MimeTypeData::debug(QTextStream &str, int indent) const // ---------------- MimeType MimeType::MimeType() : m_d(new MimeTypeData) -{ -} +{} MimeType::MimeType(const MimeType &rhs) : m_d(rhs.m_d) -{ -} +{} MimeType &MimeType::operator=(const MimeType &rhs) { - if (this != &rhs) + if (this != &rhs) { m_d = rhs.m_d; + } return *this; } MimeType::MimeType(const MimeTypeData &d) : m_d(new MimeTypeData(d)) -{ -} +{} MimeType::~MimeType() -{ -} +{} void MimeType::clear() { @@ -380,8 +399,10 @@ static inline QString systemLanguage() { QString name = QLocale::system().name(); const int underScorePos = name.indexOf(QLatin1Char('_')); - if (underScorePos != -1) + + if (underScorePos != -1) { name.truncate(underScorePos); + } return name; } @@ -389,14 +410,16 @@ QString MimeType::localeComment(const QString &localeArg) const { const QString locale = localeArg.isEmpty() ? systemLanguage() : localeArg; const MimeTypeData::LocaleHash::const_iterator it = m_d->localeComments.constFind(locale); - if (it == m_d->localeComments.constEnd()) + + if (it == m_d->localeComments.constEnd()) { return m_d->comment; + } return it.value(); } void MimeType::setLocaleComment(const QString &locale, const QString &comment) { - m_d->localeComments[locale] = comment; + m_d->localeComments[locale] = comment; } QStringList MimeType::aliases() const @@ -406,7 +429,7 @@ QStringList MimeType::aliases() const void MimeType::setAliases(const QStringList &a) { - m_d->aliases = a; + m_d->aliases = a; } QList MimeType::globPatterns() const @@ -450,8 +473,10 @@ bool MimeType::setPreferredSuffix(const QString &s) static QString formatFilterString(const QString &description, const QList &globs) { QString rc; - if (globs.empty()) // Binary files + + if (globs.empty()) { // Binary files return rc; + } { QTextStream str(&rc); str << description; @@ -459,8 +484,9 @@ static QString formatFilterString(const QString &description, const QListglobPatterns) { - if (pattern.exactMatch(c.fileName())) + foreach(QRegExp pattern, m_d->globPatterns) { + if (pattern.exactMatch(c.fileName())) { return GlobMatchPriority; + } } // Nope, try magic matchers on context data - if (m_d->magicMatchers.isEmpty()) + if (m_d->magicMatchers.isEmpty()) { return 0; + } const QByteArray data = c.data(); if (!data.isEmpty()) { - foreach (MimeTypeData::IMagicMatcherSharedPointer matcher, m_d->magicMatchers) { - if (matcher->matches(data)) + foreach(MimeTypeData::IMagicMatcherSharedPointer matcher, m_d->magicMatchers) { + if (matcher->matches(data)) { return matcher->priority(); + } } } return 0; @@ -530,14 +560,14 @@ QDebug operator<<(QDebug d, const MimeType &mt) QTextStream str(&s); mt.m_d->debug(str); } + d << s; return d; } namespace Internal { - -// MimeDatabase helpers: Generic parser for a sequence of . -// Calls abstract handler function process for MimeType it finds. +// MimeDatabase helpers: Generic parser for a sequence of . +// Calls abstract handler function process for MimeType it finds. class BaseMimeTypeParser { Q_DISABLE_COPY(BaseMimeTypeParser) public: @@ -553,23 +583,23 @@ private: void addGlobPattern(const QString &pattern, MimeTypeData *d) const; enum ParseStage { ParseBeginning, - ParseMimeInfo, - ParseMimeType, - ParseComment, - ParseGlobPattern, - ParseSubClass, - ParseAlias, - ParseMagic, - ParseMagicMatchRule, - ParseOtherMimeTypeSubTag, - ParseError }; + ParseMimeInfo, + ParseMimeType, + ParseComment, + ParseGlobPattern, + ParseSubClass, + ParseAlias, + ParseMagic, + ParseMagicMatchRule, + ParseOtherMimeTypeSubTag, + ParseError }; static ParseStage nextStage(ParseStage currentStage, const QStringRef &startElement); const QRegExp m_suffixPattern; }; -BaseMimeTypeParser:: BaseMimeTypeParser() : +BaseMimeTypeParser::BaseMimeTypeParser() : // RE to match a suffix glob pattern: "*.ext" (and not sth like "Makefile" or // "*.log[1-9]" m_suffixPattern(QLatin1String("^\\*\\.[\\w+]+$")) @@ -579,8 +609,9 @@ BaseMimeTypeParser:: BaseMimeTypeParser() : void BaseMimeTypeParser::addGlobPattern(const QString &pattern, MimeTypeData *d) const { - if (pattern.isEmpty()) + if (pattern.isEmpty()) { return; + } // Collect patterns as a QRegExp list and filter out the plain // suffix ones for our suffix list. Use first one as preferred const QRegExp wildCard(pattern, Qt::CaseSensitive, QRegExp::Wildcard); @@ -594,8 +625,9 @@ void BaseMimeTypeParser::addGlobPattern(const QString &pattern, MimeTypeData *d) if (m_suffixPattern.exactMatch(pattern)) { const QString suffix = pattern.right(pattern.size() - 2); d->suffixes.push_back(suffix); - if (d->preferredSuffix.isEmpty()) + if (d->preferredSuffix.isEmpty()) { d->preferredSuffix = suffix; + } } } @@ -603,13 +635,17 @@ BaseMimeTypeParser::ParseStage BaseMimeTypeParser::nextStage(ParseStage currentS { switch (currentStage) { case ParseBeginning: - if (startElement == QLatin1String(mimeInfoTagC)) + if (startElement == QLatin1String(mimeInfoTagC)) { return ParseMimeInfo; - if (startElement == QLatin1String(mimeTypeTagC)) + } + if (startElement == QLatin1String(mimeTypeTagC)) { return ParseMimeType; + } return ParseError; + case ParseMimeInfo: return startElement == QLatin1String(mimeTypeTagC) ? ParseMimeType : ParseError; + case ParseMimeType: case ParseComment: case ParseGlobPattern: @@ -617,22 +653,30 @@ BaseMimeTypeParser::ParseStage BaseMimeTypeParser::nextStage(ParseStage currentS case ParseAlias: case ParseOtherMimeTypeSubTag: case ParseMagicMatchRule: - if (startElement == QLatin1String(mimeTypeTagC)) // Sequence of + if (startElement == QLatin1String(mimeTypeTagC)) { // Sequence of return ParseMimeType; - if (startElement == QLatin1String(commentTagC )) + } + if (startElement == QLatin1String(commentTagC)) { return ParseComment; - if (startElement == QLatin1String(globTagC)) + } + if (startElement == QLatin1String(globTagC)) { return ParseGlobPattern; - if (startElement == QLatin1String(subClassTagC)) + } + if (startElement == QLatin1String(subClassTagC)) { return ParseSubClass; - if (startElement == QLatin1String(aliasTagC)) + } + if (startElement == QLatin1String(aliasTagC)) { return ParseAlias; - if (startElement == QLatin1String(magicTagC)) + } + if (startElement == QLatin1String(magicTagC)) { return ParseMagic; + } return ParseOtherMimeTypeSubTag; + case ParseMagic: - if (startElement == QLatin1String(matchTagC)) + if (startElement == QLatin1String(matchTagC)) { return ParseMagicMatchRule; + } break; case ParseError: break; @@ -644,6 +688,7 @@ BaseMimeTypeParser::ParseStage BaseMimeTypeParser::nextStage(ParseStage currentS static bool parseNumber(const QString &n, int *target, QString *errorMessage) { bool ok; + *target = n.toInt(&ok); if (!ok) { *errorMessage = QString::fromLatin1("Not a number '%1'.").arg(n); @@ -653,13 +698,14 @@ static bool parseNumber(const QString &n, int *target, QString *errorMessage) } // Evaluate a magic match rule like -// -// +// +// static bool addMagicMatchRule(const QXmlStreamAttributes &atts, MagicRuleMatcher *ruleMatcher, QString *errorMessage) { const QString type = atts.value(QLatin1String(matchTypeAttributeC)).toString(); + if (type != QLatin1String(matchStringTypeValueC)) { qWarning("%s: match type %s is not supported.", Q_FUNC_INFO, type.toUtf8().constData()); return true; @@ -671,14 +717,16 @@ static bool addMagicMatchRule(const QXmlStreamAttributes &atts, } // Parse for offset as "1" or "1:10" int startPos, endPos; - const QString offsetS = atts.value(QLatin1String(matchOffsetAttributeC)).toString(); - const int colonIndex = offsetS.indexOf(QLatin1Char(':')); + const QString offsetS = atts.value(QLatin1String(matchOffsetAttributeC)).toString(); + const int colonIndex = offsetS.indexOf(QLatin1Char(':')); const QString startPosS = colonIndex == -1 ? offsetS : offsetS.mid(0, colonIndex); const QString endPosS = colonIndex == -1 ? offsetS : offsetS.mid(colonIndex + 1); - if (!parseNumber(startPosS, &startPos, errorMessage) || !parseNumber(endPosS, &endPos, errorMessage)) + if (!parseNumber(startPosS, &startPos, errorMessage) || !parseNumber(endPosS, &endPos, errorMessage)) { return false; - if (debugMimeDB) + } + if (debugMimeDB) { qDebug() << Q_FUNC_INFO << value << startPos << endPos; + } ruleMatcher->add(QSharedPointer(MagicRule::createStringRule(value, startPos, endPos))); return true; } @@ -690,30 +738,34 @@ bool BaseMimeTypeParser::parse(QIODevice *dev, const QString &fileName, QString QXmlStreamReader reader(dev); ParseStage ps = ParseBeginning; + while (!reader.atEnd()) { switch (reader.readNext()) { case QXmlStreamReader::StartElement: ps = nextStage(ps, reader.name()); switch (ps) { - case ParseMimeType: { // start parsing a type - const QString type = reader.attributes().value(QLatin1String(mimeTypeAttributeC)).toString(); - if (type.isEmpty()) { - reader.raiseError(QString::fromLatin1("Missing 'type'-attribute")); - } else { - data.type = type; - } + case ParseMimeType: // start parsing a type + { const QString type = reader.attributes().value(QLatin1String(mimeTypeAttributeC)).toString(); + if (type.isEmpty()) { + reader.raiseError(QString::fromLatin1("Missing 'type'-attribute")); + } else { + data.type = type; + } } - break; + break; case ParseGlobPattern: addGlobPattern(reader.attributes().value(QLatin1String(patternAttributeC)).toString(), &data); break; - case ParseSubClass: { + case ParseSubClass: + { const QString inheritsFrom = reader.attributes().value(QLatin1String(mimeTypeAttributeC)).toString(); - if (!inheritsFrom.isEmpty()) + if (!inheritsFrom.isEmpty()) { data.subClassesOf.push_back(inheritsFrom); + } } - break; - case ParseComment: { + break; + case ParseComment: + { // comments have locale attributes. We want the default, English one QString locale = reader.attributes().value(QLatin1String(localeAttributeC)).toString(); const QString comment = QCoreApplication::translate("MimeType", reader.readElementText().toAscii()); @@ -723,28 +775,32 @@ bool BaseMimeTypeParser::parse(QIODevice *dev, const QString &fileName, QString data.localeComments.insert(locale, comment); } } - break; - case ParseAlias: { + break; + case ParseAlias: + { const QString alias = reader.attributes().value(QLatin1String(mimeTypeAttributeC)).toString(); - if (!alias.isEmpty()) + if (!alias.isEmpty()) { data.aliases.push_back(alias); + } } - break; - case ParseMagic: { + break; + case ParseMagic: + { int priority = 0; const QString priorityS = reader.attributes().value(QLatin1String(priorityAttributeC)).toString(); if (!priorityS.isEmpty()) { - if (!parseNumber(priorityS, &priority, errorMessage)) + if (!parseNumber(priorityS, &priority, errorMessage)) { return false; - + } } ruleMatcher = new MagicRuleMatcher; ruleMatcher->setPriority(priority); } - break; + break; case ParseMagicMatchRule: - if (!addMagicMatchRule(reader.attributes(), ruleMatcher, errorMessage)) + if (!addMagicMatchRule(reader.attributes(), ruleMatcher, errorMessage)) { return false; + } break; case ParseError: reader.raiseError(QString::fromLatin1("Unexpected element <%1>").arg(reader.name().toString())); @@ -756,8 +812,9 @@ bool BaseMimeTypeParser::parse(QIODevice *dev, const QString &fileName, QString // continue switch QXmlStreamReader::Token... case QXmlStreamReader::EndElement: // Finished element if (reader.name() == QLatin1String(mimeTypeTagC)) { - if (!process(MimeType(data), errorMessage)) + if (!process(MimeType(data), errorMessage)) { return false; + } data.clear(); } else { // Finished a match sequence @@ -779,15 +836,13 @@ bool BaseMimeTypeParser::parse(QIODevice *dev, const QString &fileName, QString } return true; } - } // namespace Internal // MimeMapEntry: Entry of a type map, consisting of type and level. enum { Dangling = 32767 }; -struct MimeMapEntry -{ +struct MimeMapEntry { explicit MimeMapEntry(const MimeType &t = MimeType(), int aLevel = Dangling); MimeType type; int level; // hierachy level @@ -796,8 +851,7 @@ struct MimeMapEntry MimeMapEntry::MimeMapEntry(const MimeType &t, int aLevel) : type(t), level(aLevel) -{ -} +{} /* MimeDatabasePrivate: Requirements for storage: * - Must be robust in case of incomplete hierachies, dangling entries @@ -822,8 +876,7 @@ MimeMapEntry::MimeMapEntry(const MimeType &t, int aLevel) : * to check the most specific types first). Starting a recursion from the * leaves is not suitable since it will hit parent nodes several times. */ -class MimeDatabasePrivate -{ +class MimeDatabasePrivate { Q_DISABLE_COPY(MimeDatabasePrivate) public: MimeDatabasePrivate(); @@ -864,31 +917,35 @@ private: MimeDatabasePrivate::MimeDatabasePrivate() : m_maxLevel(-1) -{ -} +{} namespace Internal { - // Parser that builds MimeDB hierarchy by adding to MimeDatabasePrivate - class MimeTypeParser : public BaseMimeTypeParser { - public: - explicit MimeTypeParser(MimeDatabasePrivate &db) : m_db(db) {} - private: - virtual bool process(const MimeType &t, QString *) { m_db.addMimeType(t); return true; } +// Parser that builds MimeDB hierarchy by adding to MimeDatabasePrivate +class MimeTypeParser : public BaseMimeTypeParser { +public: + explicit MimeTypeParser(MimeDatabasePrivate &db) : m_db(db) {} +private: + virtual bool process(const MimeType &t, QString *) + { + m_db.addMimeType(t); return true; + } - MimeDatabasePrivate &m_db; - }; + MimeDatabasePrivate &m_db; +}; } // namespace Internal bool MimeDatabasePrivate::addMimeTypes(QIODevice *device, const QString &fileName, QString *errorMessage) { Internal::MimeTypeParser parser(*this); + return parser.parse(device, fileName, errorMessage); } bool MimeDatabasePrivate::addMimeTypes(const QString &fileName, QString *errorMessage) { QFile file(fileName); - if (!file.open(QIODevice::ReadOnly|QIODevice::Text)) { + + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { *errorMessage = QString::fromLatin1("Cannot open %1: %2").arg(fileName, file.errorString()); return false; } @@ -902,8 +959,9 @@ bool MimeDatabasePrivate::addMimeTypes(QIODevice *device, QString *errorMessage) bool MimeDatabasePrivate::addMimeType(MimeType mt) { - if (!mt) + if (!mt) { return false; + } const QString type = mt.type(); // Hack: Add a magic text matcher to "text/plain" and the fallback matcher to @@ -911,8 +969,9 @@ bool MimeDatabasePrivate::addMimeType(MimeType mt) if (type == QLatin1String(textTypeC)) { mt.addMagicMatcher(QSharedPointer(new Internal::HeuristicTextMagicMatcher)); } else { - if (type == QLatin1String(binaryTypeC)) - mt.addMagicMatcher(QSharedPointer(new Internal::BinaryMatcher)); + if (type == QLatin1String(binaryTypeC)) { + mt.addMagicMatcher(QSharedPointer(new Internal::BinaryMatcher)); + } } // insert the type. m_typeMimeTypeMap.insert(type, MimeMapEntry(mt)); @@ -922,42 +981,48 @@ bool MimeDatabasePrivate::addMimeType(MimeType mt) const QStringList subClassesOf = mt.subClassesOf(); if (!subClassesOf.empty()) { const QStringList::const_iterator socend = subClassesOf.constEnd(); - for (QStringList::const_iterator soit = subClassesOf.constBegin(); soit != socend; ++soit) + for (QStringList::const_iterator soit = subClassesOf.constBegin(); soit != socend; ++soit) { m_parentChildrenMap.insert(resolveAlias(*soit), type); + } } // register aliasses const QStringList aliases = mt.aliases(); if (!aliases.empty()) { const QStringList::const_iterator cend = aliases.constEnd(); - for (QStringList::const_iterator it = aliases.constBegin(); it != cend; ++it) + for (QStringList::const_iterator it = aliases.constBegin(); it != cend; ++it) { m_aliasMap.insert(*it, type); + } } m_maxLevel = -1; // Mark as dirty return true; } -const QString &MimeDatabasePrivate::resolveAlias(const QString &name) const +const QString &MimeDatabasePrivate::resolveAlias(const QString &name) const { const AliasMap::const_iterator aliasIt = m_aliasMap.constFind(name); + return aliasIt == m_aliasMap.constEnd() ? name : aliasIt.value(); } void MimeDatabasePrivate::raiseLevelRecursion(MimeMapEntry &e, int level) { - if (e.level == Dangling || e.level < level) + if (e.level == Dangling || e.level < level) { e.level = level; - if (m_maxLevel < level) + } + if (m_maxLevel < level) { m_maxLevel = level; + } // At all events recurse over children since nodes might have been // added. const QStringList childTypes = m_parentChildrenMap.values(e.type.type()); - if (childTypes.empty()) + if (childTypes.empty()) { return; + } // look them up in the type->mime type map const int nextLevel = level + 1; const TypeMimeTypeMap::iterator tm_end = m_typeMimeTypeMap.end(); const QStringList::const_iterator cend = childTypes.constEnd(); - for (QStringList::const_iterator it = childTypes.constBegin(); it != cend; ++it) { + for (QStringList::const_iterator it = childTypes.constBegin(); it != cend; ++it) { const TypeMimeTypeMap::iterator tm_it = m_typeMimeTypeMap.find(resolveAlias(*it)); if (tm_it == tm_end) { qWarning("%s: Inconsistent mime hierarchy detected, child %s of %s cannot be found.", @@ -979,17 +1044,19 @@ void MimeDatabasePrivate::determineLevels() // sets of parents/children QSet parentSet, childrenSet; const ParentChildrenMap::const_iterator pcend = m_parentChildrenMap.constEnd(); - for (ParentChildrenMap::const_iterator it = m_parentChildrenMap.constBegin(); it != pcend; ++it) + for (ParentChildrenMap::const_iterator it = m_parentChildrenMap.constBegin(); it != pcend; ++it) { if (m_typeMimeTypeMap.contains(it.key())) { parentSet.insert(it.key()); childrenSet.insert(it.value()); } + } const QSet topLevels = parentSet.subtract(childrenSet); - if (debugMimeDB) + if (debugMimeDB) { qDebug() << Q_FUNC_INFO << "top levels" << topLevels; + } const TypeMimeTypeMap::iterator tm_end = m_typeMimeTypeMap.end(); const QSet::const_iterator tl_cend = topLevels.constEnd(); - for (QSet::const_iterator tl_it = topLevels.constBegin(); tl_it != tl_cend; ++tl_it) { + for (QSet::const_iterator tl_it = topLevels.constBegin(); tl_it != tl_cend; ++tl_it) { const TypeMimeTypeMap::iterator tm_it = m_typeMimeTypeMap.find(resolveAlias(*tl_it)); if (tm_it == tm_end) { qWarning("%s: Inconsistent mime hierarchy detected, top level element %s cannot be found.", @@ -1002,18 +1069,22 @@ void MimeDatabasePrivate::determineLevels() bool MimeDatabasePrivate::setPreferredSuffix(const QString &typeOrAlias, const QString &suffix) { - TypeMimeTypeMap::iterator tit = m_typeMimeTypeMap.find(resolveAlias(typeOrAlias)); - if (tit != m_typeMimeTypeMap.end()) + TypeMimeTypeMap::iterator tit = m_typeMimeTypeMap.find(resolveAlias(typeOrAlias)); + + if (tit != m_typeMimeTypeMap.end()) { return tit.value().type.setPreferredSuffix(suffix); + } return false; } // Returns a mime type or Null one if none found MimeType MimeDatabasePrivate::findByType(const QString &typeOrAlias) const { - const TypeMimeTypeMap::const_iterator tit = m_typeMimeTypeMap.constFind(resolveAlias(typeOrAlias)); - if (tit != m_typeMimeTypeMap.constEnd()) + const TypeMimeTypeMap::const_iterator tit = m_typeMimeTypeMap.constFind(resolveAlias(typeOrAlias)); + + if (tit != m_typeMimeTypeMap.constEnd()) { return tit.value().type; + } return MimeType(); } @@ -1021,8 +1092,10 @@ MimeType MimeDatabasePrivate::findByType(const QString &typeOrAlias) const MimeType MimeDatabasePrivate::findByFile(const QFileInfo &f) const { unsigned priority = 0; - if (debugMimeDB) + + if (debugMimeDB) { qDebug() << '>' << Q_FUNC_INFO << f.fileName(); + } const MimeType rc = findByFile(f, &priority); if (debugMimeDB) { if (rc) { @@ -1051,13 +1124,14 @@ MimeType MimeDatabasePrivate::findByFile(const QFileInfo &f, unsigned *priorityP MimeType rc; Internal::FileMatchContext context(f); const TypeMimeTypeMap::const_iterator cend = m_typeMimeTypeMap.constEnd(); - for (int level = m_maxLevel; level >= 0; level--) - for (TypeMimeTypeMap::const_iterator it = m_typeMimeTypeMap.constBegin(); it != cend; ++it) + for (int level = m_maxLevel; level >= 0; level--) { + for (TypeMimeTypeMap::const_iterator it = m_typeMimeTypeMap.constBegin(); it != cend; ++it) { if (it.value().level == level) { const unsigned priority = it.value().type.matchesFile(context); - if (debugMimeDB > 1) - qDebug() << "pass" << level << it.value().type.type() << " matches " << priority; - if (priority) + if (debugMimeDB > 1) { + qDebug() << "pass" << level << it.value().type.type() << " matches " << priority; + } + if (priority) { if (priority > maxPriority) { rc = it.value().type; maxPriority = priority; @@ -1067,7 +1141,10 @@ MimeType MimeDatabasePrivate::findByFile(const QFileInfo &f, unsigned *priorityP return rc; } } + } } + } + } return rc; } @@ -1076,8 +1153,10 @@ QStringList MimeDatabasePrivate::suffixes() const { QStringList rc; const TypeMimeTypeMap::const_iterator cend = m_typeMimeTypeMap.constEnd(); - for (TypeMimeTypeMap::const_iterator it = m_typeMimeTypeMap.constBegin(); it != cend; ++it) + + for (TypeMimeTypeMap::const_iterator it = m_typeMimeTypeMap.constBegin(); it != cend; ++it) { rc += it.value().type.suffixes(); + } return rc; } @@ -1085,8 +1164,10 @@ QStringList MimeDatabasePrivate::filterStrings() const { QStringList rc; const TypeMimeTypeMap::const_iterator cend = m_typeMimeTypeMap.constEnd(); - for (TypeMimeTypeMap::const_iterator it = m_typeMimeTypeMap.constBegin(); it != cend; ++it) + + for (TypeMimeTypeMap::const_iterator it = m_typeMimeTypeMap.constBegin(); it != cend; ++it) { rc += it.value().type.filterString(); + } return rc; } @@ -1104,8 +1185,7 @@ void MimeDatabasePrivate::debug(QTextStream &str) const // --------------- MimeDatabase MimeDatabase::MimeDatabase() : m_d(new MimeDatabasePrivate) -{ -} +{} MimeDatabase::~MimeDatabase() { @@ -1122,7 +1202,7 @@ MimeType MimeDatabase::findByFile(const QFileInfo &f) const return m_d->findByFile(f); } -bool MimeDatabase::addMimeType(const MimeType &mt) +bool MimeDatabase::addMimeType(const MimeType &mt) { return m_d->addMimeType(mt); } @@ -1149,15 +1229,17 @@ QStringList MimeDatabase::filterStrings() const QString MimeDatabase::preferredSuffixByType(const QString &type) const { - if (const MimeType mt = findByType(type)) + if (const MimeType mt = findByType(type)) { return mt.preferredSuffix(); + } return QString(); } QString MimeDatabase::preferredSuffixByFile(const QFileInfo &f) const { - if (const MimeType mt = findByFile(f)) + if (const MimeType mt = findByFile(f)) { return mt.preferredSuffix(); + } return QString(); } @@ -1173,8 +1255,8 @@ QDebug operator<<(QDebug d, const MimeDatabase &mt) QTextStream str(&s); mt.m_d->debug(str); } + d << s; return d; } - } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.h b/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.h index 9265a25d1..1726852f4 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,18 +43,16 @@ class QFileInfo; QT_END_NAMESPACE namespace Core { - class MimeTypeData; class MimeDatabasePrivate; namespace Internal { - class BaseMimeTypeParser; - class FileMatchContext; +class BaseMimeTypeParser; +class FileMatchContext; } /* Magic (file contents) matcher interface. */ -class CORE_EXPORT IMagicMatcher -{ +class CORE_EXPORT IMagicMatcher { Q_DISABLE_COPY(IMagicMatcher) protected: IMagicMatcher() {} @@ -69,8 +67,7 @@ public: /* Utility class: A standard Magic match rule based on contents. Provides * static factory methods for creation (currently only for "string". This can * be extended to handle "little16"/"big16", etc.). */ -class CORE_EXPORT MagicRule -{ +class CORE_EXPORT MagicRule { Q_DISABLE_COPY(MagicRule) public: explicit MagicRule(const QByteArray &pattern, int startPos, int endPos); @@ -87,8 +84,7 @@ private: /* Utility class: A Magic matcher that checks a number of rules based on * operator "or". It is used for rules parsed from XML files. */ -class CORE_EXPORT MagicRuleMatcher : public IMagicMatcher -{ +class CORE_EXPORT MagicRuleMatcher : public IMagicMatcher { Q_DISABLE_COPY(MagicRuleMatcher) public: typedef QSharedPointer MagicRuleSharedPointer; @@ -115,15 +111,14 @@ private: * Extensions: * - List of suffixes and preferred suffix (derived from glob patterns). */ -class CORE_EXPORT MimeType -{ +class CORE_EXPORT MimeType { public: /* Return value of a glob match, which is higher than magic */ enum { GlobMatchPriority = 101 }; MimeType(); - MimeType(const MimeType&); - MimeType &operator=(const MimeType&); + MimeType(const MimeType &); + MimeType &operator=(const MimeType &); ~MimeType(); void clear(); @@ -156,7 +151,7 @@ public: // Extension over standard mime data QString preferredSuffix() const; - bool setPreferredSuffix(const QString&); + bool setPreferredSuffix(const QString &); // Check for type or one of the aliases bool matchesType(const QString &type) const; @@ -188,8 +183,7 @@ private: * * A good testcase is to run it over '/usr/share/mime/<*>/<*>.xml' on Linux. */ -class CORE_EXPORT MimeDatabase -{ +class CORE_EXPORT MimeDatabase { Q_DISABLE_COPY(MimeDatabase) public: MimeDatabase(); @@ -198,7 +192,7 @@ public: bool addMimeTypes(const QString &fileName, QString *errorMessage); bool addMimeTypes(QIODevice *device, QString *errorMessage); - bool addMimeType(const MimeType &mt); + bool addMimeType(const MimeType &mt); // Returns a mime type or Null one if none found MimeType findByType(const QString &type) const; @@ -220,7 +214,6 @@ public: private: MimeDatabasePrivate *m_d; }; - } // namespace Core #endif // MIMEDATABASE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.cpp b/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.cpp index d75105592..4142a9a21 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,12 +36,10 @@ namespace Core { namespace Internal { - -class MiniSplitterHandle : public QSplitterHandle -{ +class MiniSplitterHandle : public QSplitterHandle { public: MiniSplitterHandle(Qt::Orientation orientation, QSplitter *parent) - : QSplitterHandle(orientation, parent) + : QSplitterHandle(orientation, parent) { setMask(QRegion(contentsRect())); setAttribute(Qt::WA_MouseNoMask, true); @@ -50,7 +48,6 @@ protected: void resizeEvent(QResizeEvent *event); void paintEvent(QPaintEvent *event); }; - } // namespace Internal } // namespace Core @@ -66,10 +63,11 @@ void MiniSplitterHandle::resizeEvent(QResizeEvent *event) // This means that with Qt upgrades it's worthwhile to see if anything changed // in QSplitterHandle::resizeEvent, to see if there's anything important we miss. - if (orientation() == Qt::Horizontal) + if (orientation() == Qt::Horizontal) { setContentsMargins(6, 0, 6, 0); - else + } else { setContentsMargins(0, 6, 0, 6); + } setMask(QRegion(contentsRect())); QWidget::resizeEvent(event); @@ -78,6 +76,7 @@ void MiniSplitterHandle::resizeEvent(QResizeEvent *event) void MiniSplitterHandle::paintEvent(QPaintEvent *event) { QPainter painter(this); + painter.fillRect(event->rect(), Utils::StyleHelper::borderColor()); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.h b/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.h index 94803aacd..b446acbac 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,10 +38,8 @@ class QSplitterHandle; QT_END_NAMESPACE namespace Core { - /*! This is a simple helper-class to obtain mac-style 1-pixel wide splitters */ -class CORE_EXPORT MiniSplitter : public QSplitter -{ +class CORE_EXPORT MiniSplitter : public QSplitter { public: MiniSplitter(QWidget *parent = 0); MiniSplitter(Qt::Orientation orientation); @@ -49,7 +47,6 @@ public: protected: QSplitterHandle *createHandle(); }; - } // namespace Core #endif // MINISPLITTER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp index 6ecd3d5e6..2e67778f1 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -68,18 +68,18 @@ ModeManager::ModeManager(Internal::MainWindow *mainWindow, MyTabWidget *modeStac { m_instance = this; -// connect((m_modeStack), SIGNAL(currentAboutToShow(int)), SLOT(currentTabAboutToChange(int))); +// connect((m_modeStack), SIGNAL(currentAboutToShow(int)), SLOT(currentTabAboutToChange(int))); connect(m_modeStack, SIGNAL(currentChanged(int)), this, SLOT(currentTabChanged(int))); - connect(m_modeStack, SIGNAL(tabMoved(int,int)), this, SLOT(tabMoved(int,int))); + connect(m_modeStack, SIGNAL(tabMoved(int, int)), this, SLOT(tabMoved(int, int))); connect(m_signalMapper, SIGNAL(mapped(QString)), this, SLOT(activateMode(QString))); } void ModeManager::init() { - QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(objectAdded(QObject*)), - this, SLOT(objectAdded(QObject*))); - QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(aboutToRemoveObject(QObject*)), - this, SLOT(aboutToRemoveObject(QObject*))); + QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(objectAdded(QObject *)), + this, SLOT(objectAdded(QObject *))); + QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(aboutToRemoveObject(QObject *)), + this, SLOT(aboutToRemoveObject(QObject *))); } void ModeManager::addWidget(QWidget *widget) @@ -89,48 +89,53 @@ void ModeManager::addWidget(QWidget *widget) // We want the actionbar to stay on the bottom // so m_modeStack->cornerWidgetCount() -1 inserts it at the position immediately above // the actionbar -// m_modeStack->insertCornerWidget(m_modeStack->cornerWidgetCount() -1, widget); +// m_modeStack->insertCornerWidget(m_modeStack->cornerWidgetCount() -1, widget); } IMode *ModeManager::currentMode() const { - if (m_modes.count() > m_modeStack->currentIndex() ) + if (m_modes.count() > m_modeStack->currentIndex()) { return m_modes.at(m_modeStack->currentIndex()); - else + } else { m_modeStack->setCurrentIndex(0); // Fix illegal Index. + } return 0; } int ModeManager::indexOf(const QString &id) const { for (int i = 0; i < m_modes.count(); ++i) { - if (m_modes.at(i)->uniqueModeName() == id) + if (m_modes.at(i)->uniqueModeName() == id) { return i; + } } -// qDebug() << "Warning, no such mode:" << id; +// qDebug() << "Warning, no such mode:" << id; return -1; } IMode *ModeManager::mode(const QString &id) const { const int index = indexOf(id); - if (index >= 0) + + if (index >= 0) { return m_modes.at(index); + } return 0; } void ModeManager::activateMode(const QString &id) { const int index = indexOf(id); - if (index >= 0) + + if (index >= 0) { m_modeStack->setCurrentIndex(index); + } } void ModeManager::activateModeByWorkspaceName(const QString &id) { for (int i = 0; i < m_modes.count(); ++i) { - if (m_modes.at(i)->name() == id) - { + if (m_modes.at(i)->name() == id) { m_modeStack->setCurrentIndex(i); return; } @@ -140,22 +145,25 @@ void ModeManager::activateModeByWorkspaceName(const QString &id) void ModeManager::objectAdded(QObject *obj) { IMode *mode = Aggregation::query(obj); - if (!mode) + + if (!mode) { return; + } m_mainWindow->addContextObject(mode); // Count the number of modes with a higher priority int index = 0; - foreach (const IMode *m, m_modes) - if (m->priority() > mode->priority()) - ++index; + foreach(const IMode * m, m_modes) + if (m->priority() > mode->priority()) { + ++index; + } m_modes.insert(index, mode); m_modeStack->insertTab(index, mode->widget(), mode->icon(), mode->name()); // Register mode shortcut - ActionManager *am = m_mainWindow->actionManager(); + ActionManager *am = m_mainWindow->actionManager(); const QString shortcutId = QLatin1String("GCS.Mode.") + mode->uniqueModeName(); QShortcut *shortcut = new QShortcut(m_mainWindow); shortcut->setWhatsThis(tr("Switch to %1 mode").arg(mode->name())); @@ -170,36 +178,42 @@ void ModeManager::objectAdded(QObject *obj) connect(shortcut, SIGNAL(activated()), m_signalMapper, SLOT(map())); } -void ModeManager::setDefaultKeyshortcuts() { +void ModeManager::setDefaultKeyshortcuts() +{ for (int i = 0; i < m_modeShortcuts.size(); ++i) { Command *currentCmd = m_modeShortcuts.at(i); bool currentlyHasDefaultSequence = (currentCmd->keySequence() == currentCmd->defaultKeySequence()); #ifdef Q_WS_MAC - currentCmd->setDefaultKeySequence(QKeySequence(QString("Meta+%1").arg(i+1))); + currentCmd->setDefaultKeySequence(QKeySequence(QString("Meta+%1").arg(i + 1))); #else - currentCmd->setDefaultKeySequence(QKeySequence(QString("Ctrl+%1").arg(i+1))); + currentCmd->setDefaultKeySequence(QKeySequence(QString("Ctrl+%1").arg(i + 1))); #endif - if (currentlyHasDefaultSequence) + if (currentlyHasDefaultSequence) { currentCmd->setKeySequence(currentCmd->defaultKeySequence()); + } } } void ModeManager::updateModeToolTip() { Command *cmd = qobject_cast(sender()); + if (cmd) { int index = m_modeShortcuts.indexOf(cmd); - if (index != -1) + if (index != -1) { m_modeStack->setTabToolTip(index, cmd->stringWithAppendedShortcut(cmd->shortcut()->whatsThis())); + } } } void ModeManager::updateModeNameIcon(IMode *mode, const QIcon &icon, const QString &label) { int index = indexOf(mode->uniqueModeName()); - if (index < 0) + + if (index < 0) { return; + } m_modeStack->setTabIcon(index, icon); m_modeStack->setTabText(index, label); } @@ -207,8 +221,10 @@ void ModeManager::updateModeNameIcon(IMode *mode, const QIcon &icon, const QStri void ModeManager::aboutToRemoveObject(QObject *obj) { IMode *mode = Aggregation::query(obj); - if (!mode) + + if (!mode) { return; + } const int index = m_modes.indexOf(mode); m_modes.remove(index); @@ -228,25 +244,27 @@ void ModeManager::addAction(Command *command, int priority, QMenu *menu) // Count the number of commands with a higher priority int index = 0; - foreach (int p, m_actions.values()) - if (p > priority) - ++index; + foreach(int p, m_actions.values()) + if (p > priority) { + ++index; + } -// m_actionBar->insertAction(index, command->action(), menu); +// m_actionBar->insertAction(index, command->action(), menu); } void ModeManager::currentTabAboutToChange(int index) { if (index >= 0) { IMode *mode = m_modes.at(index); - if (mode) + if (mode) { emit currentModeAboutToChange(mode); + } } } void ModeManager::currentTabChanged(int index) { -// qDebug() << "Current tab changed " << index; +// qDebug() << "Current tab changed " << index; // Tab index changes to -1 when there is no tab left. if (index >= 0) { IMode *mode = m_modes.at(index); @@ -255,12 +273,12 @@ void ModeManager::currentTabChanged(int index) // they use the editor widget, which is already a context widget so the main window won't // go further up the parent tree to find the mode context. ICore *core = ICore::instance(); - foreach (const int context, m_addedContexts) - core->removeAdditionalContext(context); + foreach(const int context, m_addedContexts) + core->removeAdditionalContext(context); m_addedContexts = mode->context(); - foreach (const int context, m_addedContexts) - core->addAdditionalContext(context); + foreach(const int context, m_addedContexts) + core->addAdditionalContext(context); emit currentModeChanged(mode); core->updateContext(); } @@ -269,6 +287,7 @@ void ModeManager::currentTabChanged(int index) void ModeManager::tabMoved(int from, int to) { IMode *mode = m_modes.at(from); + m_modes.remove(from); m_modes.insert(to, mode); Command *cmd = m_modeShortcuts.at(from); @@ -278,30 +297,30 @@ void ModeManager::tabMoved(int from, int to) // Reprioritize, high priority means show to the left if (!m_isReprioritizing) { for (int i = 0; i < m_modes.count(); ++i) { - m_modes.at(i)->setPriority(100-i); + m_modes.at(i)->setPriority(100 - i); } emit newModeOrder(m_modes); - } + } } void ModeManager::reorderModes(QMap priorities) { - foreach (IMode *mode, m_modes) - mode->setPriority(priorities.value(QString(QLatin1String(mode->uniqueModeName())), mode->priority())); + foreach(IMode * mode, m_modes) + mode->setPriority(priorities.value(QString(QLatin1String(mode->uniqueModeName())), mode->priority())); m_isReprioritizing = true; IMode *current = currentMode(); // Bubble sort - bool swapped = false; + bool swapped = false; do { swapped = false; - for (int i = 0; i < m_modes.count()-1; ++i) { + for (int i = 0; i < m_modes.count() - 1; ++i) { IMode *mode1 = m_modes.at(i); - IMode *mode2 = m_modes.at(i+1); -// qDebug() << "Comparing " << i << " to " << i+1 << " p1 " << mode1->priority() << " p2 " << mode2->priority(); + IMode *mode2 = m_modes.at(i + 1); +// qDebug() << "Comparing " << i << " to " << i+1 << " p1 " << mode1->priority() << " p2 " << mode2->priority(); if (mode2->priority() > mode1->priority()) { - m_modeStack->moveTab(i, i+1); -// qDebug() << "Tab moved from " << i << " to " << i+1; + m_modeStack->moveTab(i, i + 1); +// qDebug() << "Tab moved from " << i << " to " << i+1; swapped = true; } } @@ -315,22 +334,25 @@ void ModeManager::reorderModes(QMap priorities) void ModeManager::setFocusToCurrentMode() { - IMode *mode = currentMode(); - QTC_ASSERT(mode, return); + IMode *mode = currentMode(); + + QTC_ASSERT(mode, return ); QWidget *widget = mode->widget(); if (widget) { QWidget *focusWidget = widget->focusWidget(); - if (focusWidget) + + if (focusWidget) { focusWidget->setFocus(); - else + } else { widget->setFocus(); + } } } void ModeManager::triggerAction(const QString &actionId) { - foreach(Command * command, m_actions.keys()){ - if(command->action()->objectName() == actionId) { + foreach(Command * command, m_actions.keys()) { + if (command->action()->objectName() == actionId) { command->action()->trigger(); break; } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h index a2ea95e8c..6568f5473 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,7 +44,6 @@ class MyTabWidget; QT_END_NAMESPACE namespace Core { - class Command; class IMode; @@ -54,29 +53,34 @@ class FancyActionBar; class MainWindow; } // namespace Internal -class CORE_EXPORT ModeManager : public QObject -{ +class CORE_EXPORT ModeManager : public QObject { Q_OBJECT public: ModeManager(Internal::MainWindow *mainWindow, MyTabWidget *modeStack); void init(); - static ModeManager *instance() { return m_instance; } + static ModeManager *instance() + { + return m_instance; + } - IMode* currentMode() const; - IMode* mode(const QString &id) const; + IMode *currentMode() const; + IMode *mode(const QString &id) const; void addAction(Command *command, int priority, QMenu *menu = 0); void addWidget(QWidget *widget); void updateModeNameIcon(IMode *mode, const QIcon &icon, const QString &label); - QVector modes() const { return m_modes; } + QVector modes() const + { + return m_modes; + } void reorderModes(QMap priorities); signals: void currentModeAboutToChange(Core::IMode *mode); void currentModeChanged(Core::IMode *mode); - void newModeOrder(QVector modes); + void newModeOrder(QVector modes); public slots: void activateMode(const QString &id); @@ -99,15 +103,14 @@ private: static ModeManager *m_instance; Internal::MainWindow *m_mainWindow; MyTabWidget *m_modeStack; - QMap m_actions; - QVector m_modes; - QVector m_modeShortcuts; + QMap m_actions; + QVector m_modes; + QVector m_modeShortcuts; QSignalMapper *m_signalMapper; QList m_addedContexts; QList m_tabOrder; bool m_isReprioritizing; }; - } // namespace Core #endif // MODEMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/outputpane.h b/ground/openpilotgcs/src/plugins/coreplugin/outputpane.h index 4e0b438c0..77e0d3684 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/outputpane.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/outputpane.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,7 +43,6 @@ class QPushButton; QT_END_NAMESPACE namespace Core { - class IMode; class IOutputPane; @@ -53,8 +52,7 @@ class MainWindow; } -class CORE_EXPORT OutputPanePlaceHolder : public QWidget -{ +class CORE_EXPORT OutputPanePlaceHolder : public QWidget { friend class Core::Internal::OutputPaneManager; // needs to set m_visible and thus access m_current Q_OBJECT public: @@ -62,20 +60,21 @@ public: ~OutputPanePlaceHolder(); void setCloseable(bool b); bool closeable(); - static OutputPanePlaceHolder *getCurrent() { return m_current; } + static OutputPanePlaceHolder *getCurrent() + { + return m_current; + } private slots: void currentModeChanged(Core::IMode *); private: Core::IMode *m_mode; bool m_closeable; - static OutputPanePlaceHolder* m_current; + static OutputPanePlaceHolder *m_current; }; namespace Internal { - -class OutputPaneManager : public QWidget -{ +class OutputPaneManager : public QWidget { Q_OBJECT public: @@ -127,7 +126,7 @@ private: QToolButton *m_nextToolButton; QWidget *m_toolBar; - QMap m_pageMap; + QMap m_pageMap; int m_lastIndex; QStackedWidget *m_outputWidgetPane; @@ -136,7 +135,6 @@ private: QMap m_buttons; QMap m_actions; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.cpp b/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.cpp index 49eaa081d..72e32b117 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -45,14 +45,15 @@ using namespace Core::Internal; PluginDialog::PluginDialog(QWidget *parent) : QDialog(parent), - m_view(new ExtensionSystem::PluginView(ExtensionSystem::PluginManager::instance(), this)) + m_view(new ExtensionSystem::PluginView(ExtensionSystem::PluginManager::instance(), this)) { QVBoxLayout *vl = new QVBoxLayout(this); + vl->addWidget(m_view); m_detailsButton = new QPushButton(tr("Details"), this); m_errorDetailsButton = new QPushButton(tr("Error Details"), this); - m_closeButton = new QPushButton(tr("Close"), this); + m_closeButton = new QPushButton(tr("Close"), this); m_detailsButton->setEnabled(false); m_errorDetailsButton->setEnabled(false); m_closeButton->setEnabled(true); @@ -70,10 +71,10 @@ PluginDialog::PluginDialog(QWidget *parent) setWindowTitle(tr("Installed Plugins")); setWindowIcon(QIcon(":/core/images/pluginicon.png")); - connect(m_view, SIGNAL(currentPluginChanged(ExtensionSystem::PluginSpec*)), + connect(m_view, SIGNAL(currentPluginChanged(ExtensionSystem::PluginSpec *)), this, SLOT(updateButtons())); - connect(m_view, SIGNAL(pluginActivated(ExtensionSystem::PluginSpec*)), - this, SLOT(openDetails(ExtensionSystem::PluginSpec*))); + connect(m_view, SIGNAL(pluginActivated(ExtensionSystem::PluginSpec *)), + this, SLOT(openDetails(ExtensionSystem::PluginSpec *))); connect(m_detailsButton, SIGNAL(clicked()), this, SLOT(openDetails())); connect(m_errorDetailsButton, SIGNAL(clicked()), this, SLOT(openErrorDetails())); connect(m_closeButton, SIGNAL(clicked()), this, SLOT(accept())); @@ -83,6 +84,7 @@ PluginDialog::PluginDialog(QWidget *parent) void PluginDialog::updateButtons() { ExtensionSystem::PluginSpec *selectedSpec = m_view->currentPlugin(); + if (selectedSpec) { m_detailsButton->setEnabled(true); m_errorDetailsButton->setEnabled(selectedSpec->hasError()); @@ -100,8 +102,9 @@ void PluginDialog::openDetails() void PluginDialog::openDetails(ExtensionSystem::PluginSpec *spec) { - if (!spec) + if (!spec) { return; + } QDialog dialog(this); dialog.setWindowTitle(tr("Plugin Details of %1").arg(spec->name())); QVBoxLayout *layout = new QVBoxLayout; @@ -120,8 +123,10 @@ void PluginDialog::openDetails(ExtensionSystem::PluginSpec *spec) void PluginDialog::openErrorDetails() { ExtensionSystem::PluginSpec *spec = m_view->currentPlugin(); - if (!spec) + + if (!spec) { return; + } QDialog dialog(this); dialog.setWindowTitle(tr("Plugin Errors of %1").arg(spec->name())); QVBoxLayout *layout = new QVBoxLayout; @@ -136,4 +141,3 @@ void PluginDialog::openErrorDetails() dialog.resize(500, 300); dialog.exec(); } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.h b/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.h index 486347cc5..b2d19e594 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,9 +42,7 @@ class PluginView; namespace Core { namespace Internal { - -class PluginDialog : public QDialog -{ +class PluginDialog : public QDialog { Q_OBJECT public: @@ -63,7 +61,6 @@ private: QPushButton *m_errorDetailsButton; QPushButton *m_closeButton; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/rightpane.cpp b/ground/openpilotgcs/src/plugins/coreplugin/rightpane.cpp index 29b7fc9af..9fe528ae0 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/rightpane.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/rightpane.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,13 +44,13 @@ using namespace Core::Internal; RightPanePlaceHolder *RightPanePlaceHolder::m_current = 0; -RightPanePlaceHolder* RightPanePlaceHolder::current() +RightPanePlaceHolder *RightPanePlaceHolder::current() { return m_current; } RightPanePlaceHolder::RightPanePlaceHolder(Core::IMode *mode, QWidget *parent) - :QWidget(parent), m_mode(mode) + : QWidget(parent), m_mode(mode) { setLayout(new QVBoxLayout); layout()->setMargin(0); @@ -73,14 +73,15 @@ void RightPanePlaceHolder::applyStoredSize(int width) if (splitter) { // A splitter we need to resize the splitter sizes QList sizes = splitter->sizes(); - int index = splitter->indexOf(this); - int diff = width - sizes.at(index); + int index = splitter->indexOf(this); + int diff = width - sizes.at(index); int adjust = sizes.count() > 1 ? (diff / (sizes.count() - 1)) : 0; for (int i = 0; i < sizes.count(); ++i) { - if (i != index) + if (i != index) { sizes[i] -= adjust; + } } - sizes[index]= width; + sizes[index] = width; splitter->setSizes(sizes); } else { QSize s = size(); @@ -152,6 +153,7 @@ RightPaneWidget::~RightPaneWidget() void RightPaneWidget::objectAdded(QObject *obj) { BaseRightPaneWidget *rpw = qobject_cast(obj); + if (rpw) { layout()->addWidget(rpw->widget()); setFocusProxy(rpw->widget()); @@ -161,6 +163,7 @@ void RightPaneWidget::objectAdded(QObject *obj) void RightPaneWidget::aboutToRemoveObject(QObject *obj) { BaseRightPaneWidget *rpw = qobject_cast(obj); + if (rpw) { delete rpw->widget(); } @@ -178,8 +181,9 @@ int RightPaneWidget::storedWidth() void RightPaneWidget::resizeEvent(QResizeEvent *re) { - if (m_width && re->size().width()) + if (m_width && re->size().width()) { m_width = re->size().width(); + } QWidget::resizeEvent(re); } @@ -194,15 +198,16 @@ void RightPaneWidget::readSettings(QSettings *settings) if (settings->contains("RightPane/Visible")) { setShown(settings->value("RightPane/Visible").toBool()); } else { - setShown(false); //TODO set to false + setShown(false); // TODO set to false } if (settings->contains("RightPane/Width")) { m_width = settings->value("RightPane/Width").toInt(); - if (!m_width) + if (!m_width) { m_width = 500; + } } else { - m_width = 500; //pixel + m_width = 500; // pixel } // Apply if (RightPanePlaceHolder::m_current) { @@ -212,8 +217,9 @@ void RightPaneWidget::readSettings(QSettings *settings) void RightPaneWidget::setShown(bool b) { - if (RightPanePlaceHolder::m_current) + if (RightPanePlaceHolder::m_current) { RightPanePlaceHolder::m_current->setVisible(b); + } m_shown = b; } @@ -232,9 +238,7 @@ BaseRightPaneWidget::BaseRightPaneWidget(QWidget *widget) } BaseRightPaneWidget::~BaseRightPaneWidget() -{ - -} +{} QWidget *BaseRightPaneWidget::widget() const { diff --git a/ground/openpilotgcs/src/plugins/coreplugin/rightpane.h b/ground/openpilotgcs/src/plugins/coreplugin/rightpane.h index 24259149e..e88f05dbe 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/rightpane.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/rightpane.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,7 +38,6 @@ class QSettings; QT_END_NAMESPACE namespace Core { - class IMode; class RightPaneWidget; @@ -49,8 +48,7 @@ class RightPaneWidget; // RightPaneWidget::setWidget(QWidget *w) Anyway if a second plugin wants to // show something there, redesign this API -class CORE_EXPORT RightPanePlaceHolder : public QWidget -{ +class CORE_EXPORT RightPanePlaceHolder : public QWidget { friend class Core::RightPaneWidget; Q_OBJECT @@ -65,12 +63,11 @@ private slots: private: void applyStoredSize(int width); Core::IMode *m_mode; - static RightPanePlaceHolder* m_current; + static RightPanePlaceHolder *m_current; }; -class CORE_EXPORT BaseRightPaneWidget : public QObject -{ +class CORE_EXPORT BaseRightPaneWidget : public QObject { Q_OBJECT public: @@ -83,8 +80,7 @@ private: }; -class CORE_EXPORT RightPaneWidget : public QWidget -{ +class CORE_EXPORT RightPaneWidget : public QWidget { Q_OBJECT public: @@ -113,7 +109,6 @@ private: int m_width; static RightPaneWidget *m_instance; }; - } // namespace Core #endif // RIGHTPANE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.cpp b/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.cpp index 3d61f20f0..365c27479 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -49,7 +49,7 @@ rewriting the whole file each time one of the settings change. The SettingsDatabase API mimics that of QSettings. -*/ + */ using namespace Core; using namespace Core::Internal; @@ -58,11 +58,9 @@ enum { debug_settings = 0 }; namespace Core { namespace Internal { - typedef QMap SettingsMap; -class SettingsDatabasePrivate -{ +class SettingsDatabasePrivate { public: QString effectiveGroup() const { @@ -72,8 +70,10 @@ public: QString effectiveKey(const QString &key) const { QString g = effectiveGroup(); - if (!g.isEmpty() && !key.isEmpty()) + + if (!g.isEmpty() && !key.isEmpty()) { g += QLatin1Char('/'); + } g += key; return g; } @@ -85,7 +85,6 @@ public: QSqlDatabase m_db; }; - } // namespace Internal } // namespace Core @@ -99,16 +98,19 @@ SettingsDatabase::SettingsDatabase(const QString &path, // TODO: Don't rely on a path, but determine automatically QDir pathDir(path); - if (!pathDir.exists()) + + if (!pathDir.exists()) { pathDir.mkpath(pathDir.absolutePath()); + } QString fileName = path; - if (!fileName.endsWith(slash)) + if (!fileName.endsWith(slash)) { fileName += slash; + } fileName += application; fileName += QLatin1String(".db"); - d->m_db = QSqlDatabase::addDatabase("QSQLITE", QLatin1String("settings")); + d->m_db = QSqlDatabase::addDatabase("QSQLITE", QLatin1String("settings")); d->m_db.setDatabaseName(fileName); if (!d->m_db.open()) { qWarning().nospace() << "Warning: Failed to open settings database at " << fileName << " (" @@ -119,9 +121,10 @@ SettingsDatabase::SettingsDatabase(const QString &path, query.prepare(QLatin1String("CREATE TABLE IF NOT EXISTS settings (" "key PRIMARY KEY ON CONFLICT REPLACE, " "value)")); - if (!query.exec()) + if (!query.exec()) { qWarning().nospace() << "Warning: Failed to prepare settings database! (" << query.lastError().driverText() << ")"; + } // Retrieve all available keys (values are retrieved lazily) if (query.exec(QLatin1String("SELECT key FROM settings"))) { @@ -147,8 +150,9 @@ void SettingsDatabase::setValue(const QString &key, const QVariant &value) // Add to cache d->m_settings.insert(effectiveKey, value); - if (!d->m_db.isOpen()) + if (!d->m_db.isOpen()) { return; + } // Instant apply (TODO: Delay writing out settings) QSqlQuery query(d->m_db); @@ -157,16 +161,18 @@ void SettingsDatabase::setValue(const QString &key, const QVariant &value) query.addBindValue(value); query.exec(); - if (debug_settings) + if (debug_settings) { qDebug() << "Stored:" << effectiveKey << "=" << value; + } } QVariant SettingsDatabase::value(const QString &key, const QVariant &defaultValue) const { - const QString effectiveKey = d->effectiveKey(key); + const QString effectiveKey = d->effectiveKey(key); QVariant value = defaultValue; SettingsMap::const_iterator i = d->m_settings.constFind(effectiveKey); + if (i != d->m_settings.constEnd() && i.value().isValid()) { value = i.value(); } else if (d->m_db.isOpen()) { @@ -178,8 +184,9 @@ QVariant SettingsDatabase::value(const QString &key, const QVariant &defaultValu if (query.next()) { value = query.value(0); - if (debug_settings) + if (debug_settings) { qDebug() << "Retrieved:" << effectiveKey << "=" << value; + } } // Cache the result @@ -199,18 +206,18 @@ void SettingsDatabase::remove(const QString &key) const QString effectiveKey = d->effectiveKey(key); // Remove keys from the cache - foreach (const QString &k, d->m_settings.keys()) { + foreach(const QString &k, d->m_settings.keys()) { // Either it's an exact match, or it matches up to a / if (k.startsWith(effectiveKey) && (k.length() == effectiveKey.length() - || k.at(effectiveKey.length()) == QLatin1Char('/'))) - { + || k.at(effectiveKey.length()) == QLatin1Char('/'))) { d->m_settings.remove(k); } } - if (!d->m_db.isOpen()) + if (!d->m_db.isOpen()) { return; + } // Delete keys from the database QSqlQuery query(d->m_db); @@ -240,6 +247,7 @@ QStringList SettingsDatabase::childKeys() const QStringList childs; const QString g = group(); + QMapIterator i(d->m_settings); while (i.hasNext()) { const QString &key = i.next().key(); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.h b/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.h index 1ed78dc59..0cbe96946 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,13 +37,11 @@ #include namespace Core { - namespace Internal { class SettingsDatabasePrivate; } -class CORE_EXPORT SettingsDatabase : public QObject -{ +class CORE_EXPORT SettingsDatabase : public QObject { public: SettingsDatabase(const QString &path, const QString &application, QObject *parent = 0); ~SettingsDatabase(); @@ -63,7 +61,6 @@ public: private: Internal::SettingsDatabasePrivate *d; }; - } // namespace Core #endif // SETTINGSDATABASE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/sidebar.cpp b/ground/openpilotgcs/src/plugins/coreplugin/sidebar.cpp index 15068f855..dcfc10ab5 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/sidebar.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/sidebar.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -48,17 +48,19 @@ SideBarItem::~SideBarItem() delete m_widget; } -SideBar::SideBar(QList itemList, - QList defaultVisible) +SideBar::SideBar(QList itemList, + QList defaultVisible) { setOrientation(Qt::Vertical); - foreach (SideBarItem *item, itemList) { + foreach(SideBarItem * item, itemList) { const QString title = item->widget()->windowTitle(); + m_itemMap.insert(title, item); } - foreach (SideBarItem *item, defaultVisible) { - if (!itemList.contains(item)) + foreach(SideBarItem * item, defaultVisible) { + if (!itemList.contains(item)) { continue; + } m_defaultVisible.append(item->widget()->windowTitle()); } @@ -77,7 +79,7 @@ QStringList SideBar::availableItems() const void SideBar::makeItemAvailable(SideBarItem *item) { - QMap::const_iterator it = m_itemMap.constBegin(); + QMap::const_iterator it = m_itemMap.constBegin(); while (it != m_itemMap.constEnd()) { if (it.value() == item) { m_availableItems.append(it.key()); @@ -100,6 +102,7 @@ SideBarItem *SideBar::item(const QString &title) SideBarWidget *SideBar::insertSideBarWidget(int position, const QString &title) { SideBarWidget *item = new SideBarWidget(this, title); + connect(item, SIGNAL(splitMe()), this, SLOT(splitSubWidget())); connect(item, SIGNAL(closeMe()), this, SLOT(closeSubWidget())); connect(item, SIGNAL(currentWidgetChanged()), this, SLOT(updateWidgets())); @@ -119,8 +122,9 @@ void SideBar::removeSideBarWidget(SideBarWidget *widget) void SideBar::splitSubWidget() { - SideBarWidget *original = qobject_cast(sender()); + SideBarWidget *original = qobject_cast(sender()); int pos = indexOf(original) + 1; + insertSideBarWidget(pos); updateWidgets(); } @@ -128,9 +132,10 @@ void SideBar::splitSubWidget() void SideBar::closeSubWidget() { if (m_widgets.count() != 1) { - SideBarWidget *widget = qobject_cast(sender()); - if (!widget) + SideBarWidget *widget = qobject_cast(sender()); + if (!widget) { return; + } removeSideBarWidget(widget); updateWidgets(); } @@ -138,44 +143,48 @@ void SideBar::closeSubWidget() void SideBar::updateWidgets() { - foreach (SideBarWidget *i, m_widgets) - i->updateAvailableItems(); + foreach(SideBarWidget * i, m_widgets) + i->updateAvailableItems(); } void SideBar::saveSettings(QSettings *settings) { QStringList views; - for (int i = 0; i < m_widgets.count(); ++i) + + for (int i = 0; i < m_widgets.count(); ++i) { views.append(m_widgets.at(i)->currentItemTitle()); + } settings->setValue("HelpSideBar/Views", views); - settings->setValue("HelpSideBar/Visible", true);//isVisible()); + settings->setValue("HelpSideBar/Visible", true); // isVisible()); settings->setValue("HelpSideBar/VerticalPosition", saveState()); settings->setValue("HelpSideBar/Width", width()); } void SideBar::readSettings(QSettings *settings) { - foreach (SideBarWidget *widget, m_widgets) - removeSideBarWidget(widget); + foreach(SideBarWidget * widget, m_widgets) + removeSideBarWidget(widget); if (settings->contains("HelpSideBar/Views")) { QStringList views = settings->value("HelpSideBar/Views").toStringList(); if (views.count()) { - foreach (const QString &title, views) - insertSideBarWidget(m_widgets.count(), title); + foreach(const QString &title, views) + insertSideBarWidget(m_widgets.count(), title); } else { insertSideBarWidget(0); } } else { - foreach (const QString &title, m_defaultVisible) - insertSideBarWidget(m_widgets.count(), title); + foreach(const QString &title, m_defaultVisible) + insertSideBarWidget(m_widgets.count(), title); } - if (settings->contains("HelpSideBar/Visible")) + if (settings->contains("HelpSideBar/Visible")) { setVisible(settings->value("HelpSideBar/Visible").toBool()); + } - if (settings->contains("HelpSideBar/VerticalPosition")) + if (settings->contains("HelpSideBar/VerticalPosition")) { restoreState(settings->value("HelpSideBar/VerticalPosition").toByteArray()); + } if (settings->contains("HelpSideBar/Width")) { QSize s = size(); @@ -186,7 +195,7 @@ void SideBar::readSettings(QSettings *settings) void SideBar::activateItem(SideBarItem *item) { - QMap::const_iterator it = m_itemMap.constBegin(); + QMap::const_iterator it = m_itemMap.constBegin(); QString title; while (it != m_itemMap.constEnd()) { if (it.value() == item) { @@ -196,8 +205,9 @@ void SideBar::activateItem(SideBarItem *item) ++it; } - if (title.isEmpty()) + if (title.isEmpty()) { return; + } for (int i = 0; i < m_widgets.count(); ++i) { if (m_widgets.at(i)->currentItemTitle() == title) { @@ -212,18 +222,17 @@ void SideBar::activateItem(SideBarItem *item) item->widget()->setFocus(); } -void SideBar::setShortcutMap(const QMap &shortcutMap) +void SideBar::setShortcutMap(const QMap &shortcutMap) { m_shortcutMap = shortcutMap; } -QMap SideBar::shortcutMap() const +QMap SideBar::shortcutMap() const { return m_shortcutMap; } - SideBarWidget::SideBarWidget(SideBar *sideBar, const QString &title) : m_currentItem(0) , m_sideBar(sideBar) @@ -231,7 +240,7 @@ SideBarWidget::SideBarWidget(SideBar *sideBar, const QString &title) m_comboBox = new ComboBox(this); m_comboBox->setMinimumContentsLength(15); - m_toolbar = new QToolBar(this); + m_toolbar = new QToolBar(this); m_toolbar->setContentsMargins(0, 0, 0, 0); m_toolbar->addWidget(m_comboBox); @@ -263,8 +272,9 @@ SideBarWidget::SideBarWidget(SideBar *sideBar, const QString &title) if (lst.count()) { m_comboBox->addItems(lst); m_comboBox->setCurrentIndex(0); - if (t.isEmpty()) + if (t.isEmpty()) { t = m_comboBox->currentText(); + } } setCurrentItem(t); @@ -273,8 +283,7 @@ SideBarWidget::SideBarWidget(SideBar *sideBar, const QString &title) } SideBarWidget::~SideBarWidget() -{ -} +{} QString SideBarWidget::currentItemTitle() const { @@ -285,38 +294,43 @@ void SideBarWidget::setCurrentItem(const QString &title) { if (!title.isEmpty()) { int idx = m_comboBox->findText(title); - if (idx < 0) + if (idx < 0) { idx = 0; + } bool blocked = m_comboBox->blockSignals(true); m_comboBox->setCurrentIndex(idx); m_comboBox->blockSignals(blocked); } SideBarItem *item = m_sideBar->item(title); - if (!item) + if (!item) { return; + } removeCurrentItem(); m_currentItem = item; layout()->addWidget(m_currentItem->widget()); // Add buttons and remember their actions for later removal - foreach (QToolButton *b, m_currentItem->createToolBarWidgets()) - m_addedToolBarActions.append(m_toolbar->insertWidget(m_splitAction, b)); + foreach(QToolButton * b, m_currentItem->createToolBarWidgets()) + m_addedToolBarActions.append(m_toolbar->insertWidget(m_splitAction, b)); } void SideBarWidget::updateAvailableItems() { - bool blocked = m_comboBox->blockSignals(true); - QString current = m_comboBox->currentText(); + bool blocked = m_comboBox->blockSignals(true); + QString current = m_comboBox->currentText(); + m_comboBox->clear(); QStringList itms = m_sideBar->availableItems(); - if (!current.isEmpty() && !itms.contains(current)) + if (!current.isEmpty() && !itms.contains(current)) { itms.append(current); + } qSort(itms); m_comboBox->addItems(itms); int idx = m_comboBox->findText(current); - if (idx < 0) + if (idx < 0) { idx = 0; + } m_comboBox->setCurrentIndex(idx); m_splitButton->setEnabled(itms.count() > 1); m_comboBox->blockSignals(blocked); @@ -324,8 +338,9 @@ void SideBarWidget::updateAvailableItems() void SideBarWidget::removeCurrentItem() { - if (!m_currentItem) + if (!m_currentItem) { return; + } QWidget *w = m_currentItem->widget(); layout()->removeWidget(w); @@ -347,19 +362,19 @@ void SideBarWidget::setCurrentIndex(int) Core::Command *SideBarWidget::command(const QString &title) const { - const QMap shortcutMap = m_sideBar->shortcutMap(); - QMap::const_iterator r = shortcutMap.find(title); - if (r != shortcutMap.end()) + const QMap shortcutMap = m_sideBar->shortcutMap(); + + QMap::const_iterator r = shortcutMap.find(title); + if (r != shortcutMap.end()) { return r.value(); + } return 0; } - ComboBox::ComboBox(SideBarWidget *sideBarWidget) : m_sideBarWidget(sideBarWidget) -{ -} +{} bool ComboBox::event(QEvent *e) { diff --git a/ground/openpilotgcs/src/plugins/coreplugin/sidebar.h b/ground/openpilotgcs/src/plugins/coreplugin/sidebar.h index 4527ab817..705ed3028 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/sidebar.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/sidebar.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,7 +44,6 @@ class QToolButton; QT_END_NAMESPACE namespace Core { - class Command; namespace Internal { @@ -59,8 +58,7 @@ class ComboBox; * * The SideBarItem takes ownership over the widget. */ -class CORE_EXPORT SideBarItem -{ +class CORE_EXPORT SideBarItem { public: SideBarItem(QWidget *widget) : m_widget(widget) @@ -68,7 +66,10 @@ public: virtual ~SideBarItem(); - QWidget *widget() { return m_widget; } + QWidget *widget() + { + return m_widget; + } /* Should always return a new set of tool buttons. * @@ -85,15 +86,14 @@ private: QWidget *m_widget; }; -class CORE_EXPORT SideBar : public MiniSplitter -{ +class CORE_EXPORT SideBar : public MiniSplitter { Q_OBJECT public: /* * The SideBar takes ownership of the SideBarItems. */ - SideBar(QList widgetList, - QList defaultVisible); + SideBar(QList widgetList, + QList defaultVisible); ~SideBar(); QStringList availableItems() const; @@ -105,8 +105,8 @@ public: void activateItem(SideBarItem *item); - void setShortcutMap(const QMap &shortcutMap); - QMap shortcutMap() const; + void setShortcutMap(const QMap &shortcutMap); + QMap shortcutMap() const; private slots: void splitSubWidget(); @@ -118,18 +118,16 @@ private: const QString &title = QString()); void removeSideBarWidget(Internal::SideBarWidget *widget); - QList m_widgets; + QList m_widgets; - QMap m_itemMap; + QMap m_itemMap; QStringList m_availableItems; QStringList m_defaultVisible; - QMap m_shortcutMap; + QMap m_shortcutMap; }; namespace Internal { - -class SideBarWidget : public QWidget -{ +class SideBarWidget : public QWidget { Q_OBJECT public: SideBarWidget(SideBar *sideBar, const QString &title); @@ -162,8 +160,7 @@ private: QToolButton *m_closeButton; }; -class ComboBox : public QComboBox -{ +class ComboBox : public QComboBox { Q_OBJECT public: @@ -175,7 +172,6 @@ protected: private: SideBarWidget *m_sideBarWidget; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.cpp b/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.cpp index 7d8c27afb..9a7b9c914 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,13 +30,15 @@ #include -Animation * StyleAnimator::widgetAnimation(const QWidget *widget) const +Animation *StyleAnimator::widgetAnimation(const QWidget *widget) const { - if (!widget) + if (!widget) { return 0; - foreach (Animation *a, animations) { - if (a->widget() == widget) + } + foreach(Animation * a, animations) { + if (a->widget() == widget) { return a; + } } return 0; } @@ -49,40 +51,42 @@ void Animation::paint(QPainter *painter, const QStyleOption *option) void Animation::drawBlendedImage(QPainter *painter, QRect rect, float alpha) { - if (m_secondaryImage.isNull() || m_primaryImage.isNull()) + if (m_secondaryImage.isNull() || m_primaryImage.isNull()) { return; + } - if (m_tempImage.isNull()) + if (m_tempImage.isNull()) { m_tempImage = m_secondaryImage; + } - const int a = qRound(alpha*256); - const int ia = 256 - a; - const int sw = m_primaryImage.width(); - const int sh = m_primaryImage.height(); + const int a = qRound(alpha * 256); + const int ia = 256 - a; + const int sw = m_primaryImage.width(); + const int sh = m_primaryImage.height(); const int bpl = m_primaryImage.bytesPerLine(); switch (m_primaryImage.depth()) { case 32: - { - uchar *mixed_data = m_tempImage.bits(); - const uchar *back_data = m_primaryImage.bits(); - const uchar *front_data = m_secondaryImage.bits(); - for (int sy = 0; sy < sh; sy++) { - quint32 *mixed = (quint32*)mixed_data; - const quint32* back = (const quint32*)back_data; - const quint32* front = (const quint32*)front_data; - for (int sx = 0; sx < sw; sx++) { - quint32 bp = back[sx]; - quint32 fp = front[sx]; - mixed[sx] = qRgba ((qRed(bp)*ia + qRed(fp)*a)>>8, - (qGreen(bp)*ia + qGreen(fp)*a)>>8, - (qBlue(bp)*ia + qBlue(fp)*a)>>8, - (qAlpha(bp)*ia + qAlpha(fp)*a)>>8); - } - mixed_data += bpl; - back_data += bpl; - front_data += bpl; + { + uchar *mixed_data = m_tempImage.bits(); + const uchar *back_data = m_primaryImage.bits(); + const uchar *front_data = m_secondaryImage.bits(); + for (int sy = 0; sy < sh; sy++) { + quint32 *mixed = (quint32 *)mixed_data; + const quint32 *back = (const quint32 *)back_data; + const quint32 *front = (const quint32 *)front_data; + for (int sx = 0; sx < sw; sx++) { + quint32 bp = back[sx]; + quint32 fp = front[sx]; + mixed[sx] = qRgba((qRed(bp) * ia + qRed(fp) * a) >> 8, + (qGreen(bp) * ia + qGreen(fp) * a) >> 8, + (qBlue(bp) * ia + qBlue(fp) * a) >> 8, + (qAlpha(bp) * ia + qAlpha(fp) * a) >> 8); } + mixed_data += bpl; + back_data += bpl; + front_data += bpl; } + } default: break; } @@ -92,20 +96,21 @@ void Animation::drawBlendedImage(QPainter *painter, QRect rect, float alpha) void Transition::paint(QPainter *painter, const QStyleOption *option) { float alpha = 1.0; + if (m_duration > 0) { QTime current = QTime::currentTime(); - if (m_startTime > current) + if (m_startTime > current) { m_startTime = current; + } int timeDiff = m_startTime.msecsTo(current); - alpha = timeDiff/(float)m_duration; + alpha = timeDiff / (float)m_duration; if (timeDiff > m_duration) { m_running = false; - alpha = 1.0; + alpha = 1.0; } - } - else { + } else { m_running = false; } drawBlendedImage(painter, option->rect, alpha); @@ -113,16 +118,16 @@ void Transition::paint(QPainter *painter, const QStyleOption *option) void StyleAnimator::timerEvent(QTimerEvent *) { - for (int i = animations.size() - 1 ; i >= 0 ; --i) { - if (animations[i]->widget()) + for (int i = animations.size() - 1; i >= 0; --i) { + if (animations[i]->widget()) { animations[i]->widget()->update(); + } if (!animations[i]->widget() || !animations[i]->widget()->isEnabled() || !animations[i]->widget()->isVisible() || animations[i]->widget()->window()->isMinimized() || - !animations[i]->running()) - { + !animations[i]->running()) { Animation *a = animations.takeAt(i); delete a; } @@ -134,7 +139,7 @@ void StyleAnimator::timerEvent(QTimerEvent *) void StyleAnimator::stopAnimation(const QWidget *w) { - for (int i = animations.size() - 1 ; i >= 0 ; --i) { + for (int i = animations.size() - 1; i >= 0; --i) { if (animations[i]->widget() == w) { Animation *a = animations.takeAt(i); delete a; diff --git a/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.h b/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.h index cc4201822..d5f1027d2 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,24 +36,41 @@ #include #include -/* +/* * This is a set of helper classes to allow for widget animations in * the style. Its mostly taken from Vista style so it should be fully documented * there. * */ - -class Animation -{ -public : - Animation() : m_running(true) { } - virtual ~Animation() { } - QWidget * widget() const { return m_widget; } - bool running() const { return m_running; } - const QTime &startTime() const { return m_startTime; } - void setRunning(bool val) { m_running = val; } - void setWidget(QWidget *widget) { m_widget = widget; } - void setStartTime(const QTime &startTime) { m_startTime = startTime; } + +class Animation { +public: + Animation() : m_running(true) {} + virtual ~Animation() {} + QWidget *widget() const + { + return m_widget; + } + bool running() const + { + return m_running; + } + const QTime &startTime() const + { + return m_startTime; + } + void setRunning(bool val) + { + m_running = val; + } + void setWidget(QWidget *widget) + { + m_widget = widget; + } + void setStartTime(const QTime &startTime) + { + m_startTime = startTime; + } virtual void paint(QPainter *painter, const QStyleOption *option); protected: @@ -67,21 +84,31 @@ protected: }; // Handles state transition animations -class Transition : public Animation -{ -public : +class Transition : public Animation { +public: Transition() : Animation() {} virtual ~Transition() {} - void setDuration(int duration) { m_duration = duration; } - void setStartImage(const QImage &image) { m_primaryImage = image; } - void setEndImage(const QImage &image) { m_secondaryImage = image; } + void setDuration(int duration) + { + m_duration = duration; + } + void setStartImage(const QImage &image) + { + m_primaryImage = image; + } + void setEndImage(const QImage &image) + { + m_secondaryImage = image; + } virtual void paint(QPainter *painter, const QStyleOption *option); - int duration() const { return m_duration; } - int m_duration; //set time in ms to complete a state transition + int duration() const + { + return m_duration; + } + int m_duration; // set time in ms to complete a state transition }; -class StyleAnimator : public QObject -{ +class StyleAnimator : public QObject { Q_OBJECT public: @@ -90,11 +117,11 @@ public: void timerEvent(QTimerEvent *); void startAnimation(Animation *); void stopAnimation(const QWidget *); - Animation* widgetAnimation(const QWidget *) const; - + Animation *widgetAnimation(const QWidget *) const; + private: QBasicTimer animationTimer; - QList animations; + QList animations; }; #endif // ANIMATION_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.cpp b/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.cpp index 0c96d200f..f51266071 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,13 +37,13 @@ using namespace Core::Internal; TabPositionIndicator::TabPositionIndicator() : QWidget(0, Qt::ToolTip) -{ -} +{} void TabPositionIndicator::paintEvent(QPaintEvent *event) { QPainter p(this); QPen pen = p.pen(); + pen.setWidth(2); pen.setColor(palette().color(QPalette::Active, QPalette::LinkVisited)); pen.setStyle(Qt::DotLine); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.h b/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.h index a4223adae..7de3823a2 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,21 +33,21 @@ namespace Core { namespace Internal { - -class TabPositionIndicator : public QWidget -{ +class TabPositionIndicator : public QWidget { Q_OBJECT public: enum { TABPOSITIONINDICATOR_WIDTH = 2 }; TabPositionIndicator(); - int indicatorWidth() { return TABPOSITIONINDICATOR_WIDTH; } + int indicatorWidth() + { + return TABPOSITIONINDICATOR_WIDTH; + } private: void paintEvent(QPaintEvent *event); }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.cpp b/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.cpp index d7f6b052c..268c7f1d4 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.cpp @@ -7,8 +7,8 @@ TelemetryMonitorWidget::TelemetryMonitorWidget(QWidget *parent) : QGraphicsView(parent) { - setMinimumSize(180,100); - setMaximumSize(180,100); + setMinimumSize(180, 100); + setMaximumSize(180, 100); setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); @@ -18,7 +18,7 @@ TelemetryMonitorWidget::TelemetryMonitorWidget(QWidget *parent) : QGraphicsView( setAttribute(Qt::WA_TranslucentBackground); setWindowFlags(Qt::FramelessWindowHint); - QGraphicsScene *scene = new QGraphicsScene(0,0,180,100, this); + QGraphicsScene *scene = new QGraphicsScene(0, 0, 180, 100, this); QSvgRenderer *renderer = new QSvgRenderer(); if (renderer->load(QString(":/core/images/tx-rx.svg"))) { @@ -27,9 +27,9 @@ TelemetryMonitorWidget::TelemetryMonitorWidget(QWidget *parent) : QGraphicsView( graph->setElementId("txrxBackground"); QString name; - QGraphicsSvgItem* pt; + QGraphicsSvgItem *pt; - for (int i=0; ielementExists(name)) { pt = new QGraphicsSvgItem(); @@ -53,13 +53,13 @@ TelemetryMonitorWidget::TelemetryMonitorWidget(QWidget *parent) : QGraphicsView( txSpeed = new QGraphicsTextItem(); txSpeed->setDefaultTextColor(Qt::white); - txSpeed->setFont(QFont("Helvetica",22,2)); + txSpeed->setFont(QFont("Helvetica", 22, 2)); txSpeed->setParentItem(graph); scene->addItem(txSpeed); rxSpeed = new QGraphicsTextItem(); rxSpeed->setDefaultTextColor(Qt::white); - rxSpeed->setFont(QFont("Helvetica",22,2)); + rxSpeed->setFont(QFont("Helvetica", 22, 2)); rxSpeed->setParentItem(graph); scene->addItem(rxSpeed); @@ -68,8 +68,8 @@ TelemetryMonitorWidget::TelemetryMonitorWidget(QWidget *parent) : QGraphicsView( } connected = false; - txValue = 0.0; - rxValue = 0.0; + txValue = 0.0; + rxValue = 0.0; setMin(0.0); setMax(1200.0); @@ -79,34 +79,36 @@ TelemetryMonitorWidget::TelemetryMonitorWidget(QWidget *parent) : QGraphicsView( TelemetryMonitorWidget::~TelemetryMonitorWidget() { - while (!txNodes.isEmpty()) + while (!txNodes.isEmpty()) { delete txNodes.takeFirst(); + } - while (!rxNodes.isEmpty()) + while (!rxNodes.isEmpty()) { delete rxNodes.takeFirst(); + } } void TelemetryMonitorWidget::connect() { connected = true; - //flash the lights + // flash the lights updateTelemetry(maxValue, maxValue); } void TelemetryMonitorWidget::disconnect() { - //flash the lights + // flash the lights updateTelemetry(maxValue, maxValue); connected = false; - updateTelemetry(0.0,0.0); + updateTelemetry(0.0, 0.0); } /*! - \brief Called by the UAVObject which got updated + \brief Called by the UAVObject which got updated - Updates the numeric value and/or the icon if the dial wants this. - */ + Updates the numeric value and/or the icon if the dial wants this. + */ void TelemetryMonitorWidget::updateTelemetry(double txRate, double rxRate) { txValue = txRate; @@ -119,29 +121,30 @@ void TelemetryMonitorWidget::updateTelemetry(double txRate, double rxRate) // this enables smooth movement in moveIndex below void TelemetryMonitorWidget::showTelemetry() { - txIndex = (txValue-minValue)/(maxValue-minValue) * NODE_NUMELEM; - rxIndex = (rxValue-minValue)/(maxValue-minValue) * NODE_NUMELEM; + txIndex = (txValue - minValue) / (maxValue - minValue) * NODE_NUMELEM; + rxIndex = (rxValue - minValue) / (maxValue - minValue) * NODE_NUMELEM; - if (connected) + if (connected) { this->setToolTip(QString("Tx: %0 bytes/sec\nRx: %1 bytes/sec").arg(txValue).arg(rxValue)); - else + } else { this->setToolTip(QString("Disconnected")); + } int i; int nodeMargin = 8; int leftMargin = 60; - QGraphicsItem* node; + QGraphicsItem *node; - for (i=0; i < txNodes.count(); i++) { + for (i = 0; i < txNodes.count(); i++) { node = txNodes.at(i); - node->setPos((i*(node->boundingRect().width() + nodeMargin)) + leftMargin, (node->boundingRect().height()/2) - 2); + node->setPos((i * (node->boundingRect().width() + nodeMargin)) + leftMargin, (node->boundingRect().height() / 2) - 2); node->setVisible(connected && i < txIndex); node->update(); } - for (i=0; i < rxNodes.count(); i++) { + for (i = 0; i < rxNodes.count(); i++) { node = rxNodes.at(i); - node->setPos((i*(node->boundingRect().width() + nodeMargin)) + leftMargin, (node->boundingRect().height()*2) - 2); + node->setPos((i * (node->boundingRect().width() + nodeMargin)) + leftMargin, (node->boundingRect().height() * 2) - 2); node->setVisible(connected && i < rxIndex); node->update(); } @@ -165,11 +168,10 @@ void TelemetryMonitorWidget::showEvent(QShowEvent *event) fitInView(graph, Qt::KeepAspectRatio); } -void TelemetryMonitorWidget::resizeEvent(QResizeEvent* event) +void TelemetryMonitorWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - graph->setPos(0,-130); + graph->setPos(0, -130); fitInView(graph, Qt::KeepAspectRatio); } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.h b/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.h index 803362230..b157d734d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.h @@ -8,23 +8,34 @@ #include #include -class TelemetryMonitorWidget : public QGraphicsView -{ +class TelemetryMonitorWidget : public QGraphicsView { Q_OBJECT public: explicit TelemetryMonitorWidget(QWidget *parent = 0); ~TelemetryMonitorWidget(); - void setMin(double min) { minValue = min;} - double getMin() { return minValue; } - void setMax(double max) { maxValue = max;} - double getMax() { return maxValue; } + void setMin(double min) + { + minValue = min; + } + double getMin() + { + return minValue; + } + void setMax(double max) + { + maxValue = max; + } + double getMax() + { + return maxValue; + } - //number of tx/rx nodes in the graph + // number of tx/rx nodes in the graph static const int NODE_NUMELEM = 7; signals: - + public slots: void connect(); void disconnect(); @@ -37,19 +48,19 @@ protected: void resizeEvent(QResizeEvent *event); private: - QGraphicsSvgItem *graph; - QPointer txSpeed; - QPointer rxSpeed; - QList txNodes; - QList rxNodes; + QGraphicsSvgItem *graph; + QPointer txSpeed; + QPointer rxSpeed; + QList txNodes; + QList rxNodes; - bool connected; - double txIndex; - double txValue; - double rxIndex; - double rxValue; - double minValue; - double maxValue; + bool connected; + double txIndex; + double txValue; + double rxIndex; + double rxValue; + double minValue; + double maxValue; }; #endif // TELEMETRYMONITORWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.cpp index f4f747d90..629675f56 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,8 +35,8 @@ ThreadManager *ThreadManager::m_instance = 0; ThreadManager::ThreadManager(QObject *parent) : QObject(parent) { - m_instance = this; - realTimeThread= new QThread(this); + m_instance = this; + realTimeThread = new QThread(this); realTimeThread->start(QThread::TimeCriticalPriority); } @@ -49,5 +49,5 @@ ThreadManager::~ThreadManager() QThread *ThreadManager::getRealTimeThread() { - return realTimeThread; + return realTimeThread; } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.h index a2164f281..0cda72e8c 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,19 +35,20 @@ #include QT_BEGIN_NAMESPACE -QT_END_NAMESPACE + QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT ThreadManager : public QObject -{ +class CORE_EXPORT ThreadManager : public QObject { Q_OBJECT public: ThreadManager(QObject *parent); ~ThreadManager(); - static ThreadManager* instance() { return m_instance; } + static ThreadManager *instance() + { + return m_instance; + } QThread *getRealTimeThread(); @@ -56,7 +57,6 @@ private: QThread *realTimeThread; static ThreadManager *m_instance; }; - } // namespace Core #endif // THREADMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.cpp index 30b0877f8..be05426f3 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.cpp @@ -102,38 +102,40 @@ Returns true when the import should be done, false otherwise. -*/ + */ #include "uavconfiginfo.h" #include -#define VERSION_DEFAULT "0.0.0" +#define VERSION_DEFAULT "0.0.0" -#define TEXT_MINOR_LOSS_OF_CONFIGURATION tr( \ -" Some of the configured features might not be supported \ +#define TEXT_MINOR_LOSS_OF_CONFIGURATION \ + tr( \ + " Some of the configured features might not be supported \ by your version of the plugin. You might want to upgrade the plugin.") -#define TEXT_MISSING_CONFIGURATION tr( \ -" Some configuration is missing in the imported config and will be replaced \ +#define TEXT_MISSING_CONFIGURATION \ + tr( \ + " Some configuration is missing in the imported config and will be replaced \ by default settings.") -#define TEXT_MAJOR_LOSS_OF_CONFIGURATION tr( \ -" Major features can't be imported \ +#define TEXT_MAJOR_LOSS_OF_CONFIGURATION \ + tr( \ + " Major features can't be imported \ by your version of the plugin. You should upgrade the plugin to import these settings.") -#define TEXT_NOT_COMPATIBLE tr( \ -" The imported settings are not compatible with this plugin and won't be imported!") +#define TEXT_NOT_COMPATIBLE \ + tr( \ + " The imported settings are not compatible with this plugin and won't be imported!") using namespace Core; UAVConfigInfo::UAVConfigInfo(QObject *parent) : - QObject(parent), - m_version(VERSION_DEFAULT), - m_locked(false), - m_nameOfConfigurable("") -{ - -} + QObject(parent), + m_version(VERSION_DEFAULT), + m_locked(false), + m_nameOfConfigurable("") +{} UAVConfigInfo::UAVConfigInfo(QSettings *qs, QObject *parent) : QObject(parent), @@ -143,16 +145,14 @@ UAVConfigInfo::UAVConfigInfo(QSettings *qs, QObject *parent) : } UAVConfigInfo::UAVConfigInfo(UAVConfigVersion version, QString nameOfConfigurable, QObject *parent) : - QObject(parent), - m_version(version), - m_locked(false), - m_nameOfConfigurable(nameOfConfigurable) -{ - -} + QObject(parent), + m_version(version), + m_locked(false), + m_nameOfConfigurable(nameOfConfigurable) +{} UAVConfigInfo::UAVConfigInfo(IUAVGadgetConfiguration *config, QObject *parent) : - QObject(parent) + QObject(parent) { m_locked = config->locked(); m_nameOfConfigurable = config->classId() + "-" + config->name(); @@ -169,21 +169,21 @@ void UAVConfigInfo::save(QSettings *qs) void UAVConfigInfo::read(QSettings *qs) { qs->beginGroup("configInfo"); - m_version = UAVConfigVersion( qs->value("version", VERSION_DEFAULT ).toString()); - m_locked = qs->value("locked", false ).toBool(); + m_version = UAVConfigVersion(qs->value("version", VERSION_DEFAULT).toString()); + m_locked = qs->value("locked", false).toBool(); qs->endGroup(); } bool UAVConfigInfo::askToAbort(int compat, QString message) { QMessageBox msgBox; + msgBox.setInformativeText(tr("Do you want to continue the import?")); msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); int result; - switch(compat){ - + switch (compat) { case FullyCompatible: return false; @@ -215,30 +215,35 @@ bool UAVConfigInfo::askToAbort(int compat, QString message) default: msgBox.setText("INTERNAL ERROR: " + message + tr("Unknown compatibility level: " + compat)); } - if ( result == QMessageBox::Ok ) + if (result == QMessageBox::Ok) { return false; - else + } else { return true; - + } } void UAVConfigInfo::notify(QString message) { QMessageBox msgBox; + msgBox.setText(message); msgBox.exec(); } int UAVConfigInfo::checkCompatibilityWith(UAVConfigVersion programVersion) { - if ( m_version.majorNr != programVersion.majorNr ) + if (m_version.majorNr != programVersion.majorNr) { return NotCompatible; - if ( m_version.minorNr < programVersion.minorNr ) + } + if (m_version.minorNr < programVersion.minorNr) { return MissingConfiguration; - if ( m_version.minorNr > programVersion.minorNr ) + } + if (m_version.minorNr > programVersion.minorNr) { return MajorLossOfConfiguration; - if ( m_version.patchNr > programVersion.patchNr ) + } + if (m_version.patchNr > programVersion.patchNr) { return MinorLossOfConfiguration; + } return FullyCompatible; } @@ -246,31 +251,30 @@ int UAVConfigInfo::checkCompatibilityWith(UAVConfigVersion programVersion) bool UAVConfigInfo::standardVersionHandlingOK(UAVConfigVersion programVersion) { return !askToAbort( - checkCompatibilityWith(programVersion), - "("+m_nameOfConfigurable+")"); + checkCompatibilityWith(programVersion), + "(" + m_nameOfConfigurable + ")"); } UAVConfigVersion::UAVConfigVersion(int majorNum, int minorNum, int patchNum) - :majorNr(majorNum) - ,minorNr(minorNum) - ,patchNr(patchNum) -{ -} + : majorNr(majorNum) + , minorNr(minorNum) + , patchNr(patchNum) +{} UAVConfigVersion::UAVConfigVersion(QString versionString) { int begin; int end = 0; - begin = end; - end = versionString.indexOf(".", begin); - majorNr = versionString.mid(begin, end-begin).toInt(); + begin = end; + end = versionString.indexOf(".", begin); + majorNr = versionString.mid(begin, end - begin).toInt(); - begin = end+1; - end = versionString.indexOf(".", begin); - minorNr = versionString.mid(begin, end-begin).toInt(); + begin = end + 1; + end = versionString.indexOf(".", begin); + minorNr = versionString.mid(begin, end - begin).toInt(); - begin = end+1; + begin = end + 1; patchNr = versionString.mid(begin).toInt(); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.h b/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.h index 314d33459..bae3d4e8d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.h @@ -33,12 +33,10 @@ #include "iuavgadgetconfiguration.h" #include "core_global.h" -namespace Core -{ - +namespace Core { class IUAVGadgetConfiguration; -class CORE_EXPORT UAVConfigVersion{ +class CORE_EXPORT UAVConfigVersion { public: UAVConfigVersion(QString versionString = "0.0.0"); UAVConfigVersion(int major, int minor, int patch); @@ -51,34 +49,57 @@ public: bool operator==(const UAVConfigVersion &other); }; -class CORE_EXPORT UAVConfigInfo : public QObject -{ +class CORE_EXPORT UAVConfigInfo : public QObject { Q_OBJECT public: explicit UAVConfigInfo(QObject *parent = 0); explicit UAVConfigInfo(QSettings *qs, QObject *parent = 0); - explicit UAVConfigInfo(IUAVGadgetConfiguration* config, QObject *parent = 0); + explicit UAVConfigInfo(IUAVGadgetConfiguration *config, QObject *parent = 0); UAVConfigInfo(UAVConfigVersion version, QString nameOfConfigurable, QObject *parent = 0); enum Compatibility { FullyCompatible, MinorLossOfConfiguration, MissingConfiguration, MajorLossOfConfiguration, NotCompatible }; - void setNameOfConfigurable(const QString nameOfConfigurable){m_nameOfConfigurable = nameOfConfigurable;} + void setNameOfConfigurable(const QString nameOfConfigurable) + { + m_nameOfConfigurable = nameOfConfigurable; + } void save(QSettings *qs); void read(QSettings *qs); - void setVersion(int major, int minor, int patch){m_version = UAVConfigVersion(major, minor, patch);} - void setVersion(const QString version){m_version = UAVConfigVersion(version);} - void setVersion(const UAVConfigVersion version){m_version = version;} - UAVConfigVersion version(){ return m_version;} - bool locked(){ return m_locked; } - void setLocked(bool locked){ m_locked = locked; } + void setVersion(int major, int minor, int patch) + { + m_version = UAVConfigVersion(major, minor, patch); + } + void setVersion(const QString version) + { + m_version = UAVConfigVersion(version); + } + void setVersion(const UAVConfigVersion version) + { + m_version = version; + } + UAVConfigVersion version() + { + return m_version; + } + bool locked() + { + return m_locked; + } + void setLocked(bool locked) + { + m_locked = locked; + } int checkCompatibilityWith(UAVConfigVersion programVersion); bool askToAbort(int compat, QString message); void notify(QString message); bool standardVersionHandlingOK(UAVConfigVersion programVersion); - bool standardVersionHandlingOK(QString programVersion){ return standardVersionHandlingOK(UAVConfigVersion(programVersion));} + bool standardVersionHandlingOK(QString programVersion) + { + return standardVersionHandlingOK(UAVConfigVersion(programVersion)); + } signals: @@ -88,9 +109,7 @@ private: UAVConfigVersion m_version; bool m_locked; QString m_nameOfConfigurable; - }; - } // namespace Core #endif // UAVCONFIGINFO_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.cpp index 56c02b00a..2fc7c580d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.cpp @@ -9,19 +9,19 @@ * @{ * @brief The Core GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,17 +33,17 @@ using namespace Core; -UAVGadgetDecorator::UAVGadgetDecorator(IUAVGadget *gadget, QList *configurations) : - IUAVGadget(gadget->classId(), gadget->parent()), - m_gadget(gadget), - m_toolbar(new QComboBox), - m_activeConfiguration(0), - m_configurations(configurations) +UAVGadgetDecorator::UAVGadgetDecorator(IUAVGadget *gadget, QList *configurations) : + IUAVGadget(gadget->classId(), gadget->parent()), + m_gadget(gadget), + m_toolbar(new QComboBox), + m_activeConfiguration(0), + m_configurations(configurations) { m_gadget->setParent(this); m_toolbar->setMinimumContentsLength(15); - foreach (IUAVGadgetConfiguration *config, *m_configurations) - m_toolbar->addItem(config->name()); + foreach(IUAVGadgetConfiguration * config, *m_configurations) + m_toolbar->addItem(config->name()); connect(m_toolbar, SIGNAL(activated(int)), this, SLOT(loadConfiguration(int))); updateToolbar(); } @@ -54,8 +54,10 @@ UAVGadgetDecorator::~UAVGadgetDecorator() delete m_toolbar; } -void UAVGadgetDecorator::loadConfiguration(int index) { +void UAVGadgetDecorator::loadConfiguration(int index) +{ IUAVGadgetConfiguration *config = m_configurations->at(index); + loadConfiguration(config); } @@ -65,19 +67,20 @@ void UAVGadgetDecorator::loadConfiguration(IUAVGadgetConfiguration *config) int index = m_toolbar->findText(config->name()); m_toolbar->setCurrentIndex(index); m_gadget->loadConfiguration(config); - } void UAVGadgetDecorator::configurationChanged(IUAVGadgetConfiguration *config) { - if (config == m_activeConfiguration) + if (config == m_activeConfiguration) { loadConfiguration(config); + } } void UAVGadgetDecorator::configurationAdded(IUAVGadgetConfiguration *config) { - if (config->classId() != classId()) + if (config->classId() != classId()) { return; + } m_configurations->append(config); m_toolbar->addItem(config->name()); updateToolbar(); @@ -85,8 +88,9 @@ void UAVGadgetDecorator::configurationAdded(IUAVGadgetConfiguration *config) void UAVGadgetDecorator::configurationToBeDeleted(IUAVGadgetConfiguration *config) { - if (config->classId() != classId()) + if (config->classId() != classId()) { return; + } int index = m_configurations->indexOf(config); if (index >= 0) { m_toolbar->removeItem(index); @@ -97,11 +101,13 @@ void UAVGadgetDecorator::configurationToBeDeleted(IUAVGadgetConfiguration *confi void UAVGadgetDecorator::configurationNameChanged(IUAVGadgetConfiguration *config, QString oldName, QString newName) { - if (config->classId() != classId()) + if (config->classId() != classId()) { return; + } for (int i = 0; i < m_toolbar->count(); ++i) { - if (m_toolbar->itemText(i) == oldName) + if (m_toolbar->itemText(i) == oldName) { m_toolbar->setItemText(i, newName); + } } } @@ -120,7 +126,8 @@ void UAVGadgetDecorator::saveState(QSettings *qSettings) void UAVGadgetDecorator::restoreState(QSettings *qSettings) { QString configName = qSettings->value("activeConfiguration").toString(); - foreach (IUAVGadgetConfiguration *config, *m_configurations) { + + foreach(IUAVGadgetConfiguration * config, *m_configurations) { if (config->name() == configName) { m_activeConfiguration = config; loadConfiguration(config); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.h b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.h index c7dd59509..0a233eefb 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.h @@ -10,18 +10,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,27 +30,34 @@ #include namespace Core { - class IUAVGadgetConfiguration; -class UAVGadgetDecorator : public IUAVGadget -{ -Q_OBJECT +class UAVGadgetDecorator : public IUAVGadget { + Q_OBJECT public: - explicit UAVGadgetDecorator(IUAVGadget *gadget, QList *configurations); + explicit UAVGadgetDecorator(IUAVGadget *gadget, QList *configurations); ~UAVGadgetDecorator(); - QWidget *widget() { return m_gadget->widget(); } - QComboBox *toolBar() { return m_toolbar; } - IUAVGadgetConfiguration *activeConfiguration() { return m_activeConfiguration; } + QWidget *widget() + { + return m_gadget->widget(); + } + QComboBox *toolBar() + { + return m_toolbar; + } + IUAVGadgetConfiguration *activeConfiguration() + { + return m_activeConfiguration; + } void loadConfiguration(IUAVGadgetConfiguration *config); - void saveState(QSettings* qSettings); - void restoreState(QSettings* qSettings); + void saveState(QSettings *qSettings); + void restoreState(QSettings *qSettings); public slots: - void configurationChanged(IUAVGadgetConfiguration* config); - void configurationAdded(IUAVGadgetConfiguration* config); - void configurationToBeDeleted(IUAVGadgetConfiguration* config); - void configurationNameChanged(IUAVGadgetConfiguration* config, QString oldName, QString newName); + void configurationChanged(IUAVGadgetConfiguration *config); + void configurationAdded(IUAVGadgetConfiguration *config); + void configurationToBeDeleted(IUAVGadgetConfiguration *config); + void configurationNameChanged(IUAVGadgetConfiguration *config, QString oldName, QString newName); private slots: void loadConfiguration(int index); private: @@ -58,10 +65,8 @@ private: IUAVGadget *m_gadget; QComboBox *m_toolbar; IUAVGadgetConfiguration *m_activeConfiguration; - QList *m_configurations; - + QList *m_configurations; }; - } // namespace Core #endif // UAVGADGETDECORATOR_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp index a20612c59..1aa28d890 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp @@ -9,19 +9,19 @@ * @{ * @brief The Core GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -50,12 +50,12 @@ UAVGadgetInstanceManager::UAVGadgetInstanceManager(QObject *parent) : QObject(parent) { m_pm = ExtensionSystem::PluginManager::instance(); - QList factories = m_pm->getObjects(); - foreach (IUAVGadgetFactory *f, factories) { + QList factories = m_pm->getObjects(); + foreach(IUAVGadgetFactory * f, factories) { if (!m_factories.contains(f)) { m_factories.append(f); QString classId = f->classId(); - QString name = f->name(); + QString name = f->name(); QIcon icon = f->icon(); m_classIdNameMap.insert(classId, name); m_classIdIconMap.insert(classId, icon); @@ -65,40 +65,37 @@ UAVGadgetInstanceManager::UAVGadgetInstanceManager(QObject *parent) : UAVGadgetInstanceManager::~UAVGadgetInstanceManager() { - foreach (IOptionsPage *page, m_optionsPages) { + foreach(IOptionsPage * page, m_optionsPages) { m_pm->removeObject(page); delete page; } - } void UAVGadgetInstanceManager::readSettings(QSettings *qs) { - while ( !m_configurations.isEmpty() ){ - emit configurationToBeDeleted(m_configurations.takeLast()); + while (!m_configurations.isEmpty()) { + emit configurationToBeDeleted(m_configurations.takeLast()); } qs->beginGroup("UAVGadgetConfigurations"); UAVConfigInfo configInfo(qs); configInfo.setNameOfConfigurable("UAVGadgetConfigurations"); - if ( configInfo.version() == UAVConfigVersion() ){ + if (configInfo.version() == UAVConfigVersion()) { // If version is not set, assume its a old version before readable config (1.0.0). // however compatibility to 1.0.0 is broken. configInfo.setVersion("1.0.0"); } - if ( configInfo.version() == UAVConfigVersion("1.1.0") ){ + if (configInfo.version() == UAVConfigVersion("1.1.0")) { configInfo.notify(tr("Migrating UAVGadgetConfigurations from version 1.1.0 to ") + m_versionUAVGadgetConfigurations.toString()); readConfigs_1_1_0(qs); // this is fully compatible with 1.2.0 - } - else if ( !configInfo.standardVersionHandlingOK(m_versionUAVGadgetConfigurations) ){ + } else if (!configInfo.standardVersionHandlingOK(m_versionUAVGadgetConfigurations)) { // We are in trouble now. User wants us to quit the import, but when he saves // the GCS, his old config will be lost. configInfo.notify( - tr("You might want to save your old config NOW since it might be replaced by broken one when you exit the GCS!") - ); - } - else{ + tr("You might want to save your old config NOW since it might be replaced by broken one when you exit the GCS!") + ); + } else { readConfigs_1_2_0(qs); } @@ -110,32 +107,31 @@ void UAVGadgetInstanceManager::readConfigs_1_2_0(QSettings *qs) { UAVConfigInfo configInfo; - foreach (QString classId, m_classIdNameMap.keys()) - { + foreach(QString classId, m_classIdNameMap.keys()) { IUAVGadgetFactory *f = factory(classId); + qs->beginGroup(classId); QStringList configs = QStringList(); configs = qs->childGroups(); - foreach (QString configName, configs) { - qDebug() << "Loading config: " << classId << "," << configName; + foreach(QString configName, configs) { + qDebug() << "Loading config: " << classId << "," << configName; qs->beginGroup(configName); configInfo.read(qs); - configInfo.setNameOfConfigurable(classId+"-"+configName); + configInfo.setNameOfConfigurable(classId + "-" + configName); qs->beginGroup("data"); IUAVGadgetConfiguration *config = f->createConfiguration(qs, &configInfo); - if (config){ + if (config) { config->setName(configName); config->setProvisionalName(configName); config->setLocked(configInfo.locked()); int idx = indexForConfig(m_configurations, classId, configName); - if ( idx >= 0 ){ + if (idx >= 0) { // We should replace the config, but it might be used, so just // throw it out of the list. The GCS should be reinitialised soon. m_configurations[idx] = config; - } - else{ + } else { m_configurations.append(config); } } @@ -161,31 +157,30 @@ void UAVGadgetInstanceManager::readConfigs_1_1_0(QSettings *qs) { UAVConfigInfo configInfo; - foreach (QString classId, m_classIdNameMap.keys()) - { + foreach(QString classId, m_classIdNameMap.keys()) { IUAVGadgetFactory *f = factory(classId); + qs->beginGroup(classId); QStringList configs = QStringList(); configs = qs->childGroups(); - foreach (QString configName, configs) { - qDebug() << "Loading config: " << classId << "," << configName; + foreach(QString configName, configs) { + qDebug() << "Loading config: " << classId << "," << configName; qs->beginGroup(configName); bool locked = qs->value("config.locked").toBool(); - configInfo.setNameOfConfigurable(classId+"-"+configName); + configInfo.setNameOfConfigurable(classId + "-" + configName); IUAVGadgetConfiguration *config = f->createConfiguration(qs, &configInfo); - if (config){ + if (config) { config->setName(configName); config->setProvisionalName(configName); config->setLocked(locked); int idx = indexForConfig(m_configurations, classId, configName); - if ( idx >= 0 ){ + if (idx >= 0) { // We should replace the config, but it might be used, so just // throw it out of the list. The GCS should be reinitialised soon. m_configurations[idx] = config; - } - else{ + } else { m_configurations.append(config); } } @@ -209,13 +204,13 @@ void UAVGadgetInstanceManager::readConfigs_1_1_0(QSettings *qs) void UAVGadgetInstanceManager::saveSettings(QSettings *qs) { UAVConfigInfo *configInfo; + qs->beginGroup("UAVGadgetConfigurations"); qs->remove(""); // Remove existing configurations configInfo = new UAVConfigInfo(m_versionUAVGadgetConfigurations, "UAVGadgetConfigurations"); configInfo->save(qs); delete configInfo; - foreach (IUAVGadgetConfiguration *config, m_configurations) - { + foreach(IUAVGadgetConfiguration * config, m_configurations) { configInfo = new UAVConfigInfo(config); qs->beginGroup(config->classId()); qs->beginGroup(config->name()); @@ -235,11 +230,11 @@ void UAVGadgetInstanceManager::createOptionsPages() // In case there are pages (import a configuration), remove them. // Maybe they should be deleted as well (memory-leak), // but this might lead to NULL-pointers? - while (!m_optionsPages.isEmpty()){ + while (!m_optionsPages.isEmpty()) { m_pm->removeObject(m_optionsPages.takeLast()); } - QMutableListIterator ite(m_configurations); + QMutableListIterator ite(m_configurations); while (ite.hasNext()) { IUAVGadgetConfiguration *config = ite.next(); IUAVGadgetFactory *f = factory(config->classId()); @@ -249,11 +244,10 @@ void UAVGadgetInstanceManager::createOptionsPages() page->setIcon(f->icon()); m_optionsPages.append(page); m_pm->addObject(page); - } - else { + } else { qWarning() - << "UAVGadgetInstanceManager::createOptionsPages - failed to create options page for configuration " - + config->classId() + ":" + config->name() + ", configuration will be removed."; + << "UAVGadgetInstanceManager::createOptionsPages - failed to create options page for configuration " + + config->classId() + ":" + config->name() + ", configuration will be removed."; // The m_optionsPages list and m_configurations list must be in synch otherwise nasty issues happen later // so if we fail to create an options page we must remove the associated configuration ite.remove(); @@ -265,8 +259,9 @@ void UAVGadgetInstanceManager::createOptionsPages() IUAVGadget *UAVGadgetInstanceManager::createGadget(QString classId, QWidget *parent, bool loadDefaultConfiguration) { IUAVGadgetFactory *f = factory(classId); + if (f) { - QList *configs = configurations(classId); + QList *configs = configurations(classId); IUAVGadget *g = f->createGadget(parent); UAVGadgetDecorator *gadget = new UAVGadgetDecorator(g, configs); if ((loadDefaultConfiguration && configs && configs->count()) > 0) { @@ -274,10 +269,10 @@ IUAVGadget *UAVGadgetInstanceManager::createGadget(QString classId, QWidget *par } m_gadgetInstances.append(gadget); - connect(this, SIGNAL(configurationAdded(IUAVGadgetConfiguration*)), gadget, SLOT(configurationAdded(IUAVGadgetConfiguration*))); - connect(this, SIGNAL(configurationChanged(IUAVGadgetConfiguration*)), gadget, SLOT(configurationChanged(IUAVGadgetConfiguration*))); - connect(this, SIGNAL(configurationNameChanged(IUAVGadgetConfiguration*, QString,QString)), gadget, SLOT(configurationNameChanged(IUAVGadgetConfiguration*, QString,QString))); - connect(this, SIGNAL(configurationToBeDeleted(IUAVGadgetConfiguration*)), gadget, SLOT(configurationToBeDeleted(IUAVGadgetConfiguration*))); + connect(this, SIGNAL(configurationAdded(IUAVGadgetConfiguration *)), gadget, SLOT(configurationAdded(IUAVGadgetConfiguration *))); + connect(this, SIGNAL(configurationChanged(IUAVGadgetConfiguration *)), gadget, SLOT(configurationChanged(IUAVGadgetConfiguration *))); + connect(this, SIGNAL(configurationNameChanged(IUAVGadgetConfiguration *, QString, QString)), gadget, SLOT(configurationNameChanged(IUAVGadgetConfiguration *, QString, QString))); + connect(this, SIGNAL(configurationToBeDeleted(IUAVGadgetConfiguration *)), gadget, SLOT(configurationToBeDeleted(IUAVGadgetConfiguration *))); return gadget; } return 0; @@ -293,15 +288,15 @@ void UAVGadgetInstanceManager::removeGadget(IUAVGadget *gadget) } /** - * Removes all the gadgets. This is called by the core plugin when - * shutting down: this ensures that all registered gadget factory destructors are - * indeed called when the GCS is shutting down. We can't destroy them at the end - * (coreplugin is deleted last), because the gadgets sometimes depend on other - * plugins, like uavobjects... - */ + * Removes all the gadgets. This is called by the core plugin when + * shutting down: this ensures that all registered gadget factory destructors are + * indeed called when the GCS is shutting down. We can't destroy them at the end + * (coreplugin is deleted last), because the gadgets sometimes depend on other + * plugins, like uavobjects... + */ void UAVGadgetInstanceManager::removeAllGadgets() { - foreach( IUAVGadget *gadget, m_gadgetInstances) { + foreach(IUAVGadget * gadget, m_gadgetInstances) { m_gadgetInstances.removeOne(gadget); delete gadget; } @@ -311,20 +306,21 @@ void UAVGadgetInstanceManager::removeAllGadgets() bool UAVGadgetInstanceManager::canDeleteConfiguration(IUAVGadgetConfiguration *config) { // to be able to delete a configuration, no instance must be using it - foreach (IUAVGadget *gadget, m_gadgetInstances) { + foreach(IUAVGadget * gadget, m_gadgetInstances) { if (gadget->activeConfiguration() == config) { - return false; + return false; } } // and it cannot be the only configuration - foreach (IUAVGadgetConfiguration *c, m_configurations) { - if (c != config && c->classId() == config->classId()) + foreach(IUAVGadgetConfiguration * c, m_configurations) { + if (c != config && c->classId() == config->classId()) { return true; + } } return false; } -void UAVGadgetInstanceManager::deleteConfiguration(IUAVGadgetConfiguration *config) +void UAVGadgetInstanceManager::deleteConfiguration(IUAVGadgetConfiguration *config) { m_provisionalDeletes.append(config); if (m_provisionalConfigs.contains(config)) { @@ -340,16 +336,17 @@ void UAVGadgetInstanceManager::deleteConfiguration(IUAVGadgetConfiguration *con configurationNameEdited("", false); } -void UAVGadgetInstanceManager::cloneConfiguration(IUAVGadgetConfiguration *configToClone) +void UAVGadgetInstanceManager::cloneConfiguration(IUAVGadgetConfiguration *configToClone) { QString name = suggestName(configToClone->classId(), configToClone->name()); IUAVGadgetConfiguration *config = configToClone->clone(); + config->setName(name); config->setProvisionalName(name); IUAVGadgetFactory *f = factory(config->classId()); IOptionsPage *p = f->createOptionsPage(config); - IOptionsPage *page = new UAVGadgetOptionsPageDecorator(p, config); + IOptionsPage *page = new UAVGadgetOptionsPageDecorator(p, config); page->setIcon(f->icon()); m_provisionalConfigs.append(config); m_provisionalOptionsPages.append(page); @@ -364,12 +361,13 @@ QString UAVGadgetInstanceManager::suggestName(QString classId, QString name) QString last = name.split(" ").last(); bool ok; int num = last.toInt(&ok); - int i = 1; + int i = 1; + if (ok) { QStringList n = name.split(" "); n.removeLast(); name = n.join(" "); - i = num+1; + i = num + 1; } do { suggestedName = name + " " + QString::number(i); @@ -391,7 +389,7 @@ void UAVGadgetInstanceManager::applyChanges(IUAVGadgetConfiguration *config) m_takenNames[config->classId()].removeAt(j); m_pm->removeObject(m_optionsPages.at(i)); m_configurations.removeAt(i); - m_optionsPages.removeAt(i);//TODO delete + m_optionsPages.removeAt(i); // TODO delete } return; } @@ -416,33 +414,39 @@ void UAVGadgetInstanceManager::applyChanges(IUAVGadgetConfiguration *config) void UAVGadgetInstanceManager::configurationNameEdited(QString text, bool hasText) { bool disable = false; - foreach (IUAVGadgetConfiguration *c, m_configurations) { - foreach (IUAVGadgetConfiguration *d, m_configurations) { - if (c != d && c->classId() == d->classId() && c->provisionalName() == d->provisionalName()) + + foreach(IUAVGadgetConfiguration * c, m_configurations) { + foreach(IUAVGadgetConfiguration * d, m_configurations) { + if (c != d && c->classId() == d->classId() && c->provisionalName() == d->provisionalName()) { disable = true; + } } - foreach (IUAVGadgetConfiguration *d, m_provisionalConfigs) { - if (c != d && c->classId() == d->classId() && c->provisionalName() == d->provisionalName()) + foreach(IUAVGadgetConfiguration * d, m_provisionalConfigs) { + if (c != d && c->classId() == d->classId() && c->provisionalName() == d->provisionalName()) { disable = true; + } } } - foreach (IUAVGadgetConfiguration *c, m_provisionalConfigs) { - foreach (IUAVGadgetConfiguration *d, m_provisionalConfigs) { - if (c != d && c->classId() == d->classId() && c->provisionalName() == d->provisionalName()) + foreach(IUAVGadgetConfiguration * c, m_provisionalConfigs) { + foreach(IUAVGadgetConfiguration * d, m_provisionalConfigs) { + if (c != d && c->classId() == d->classId() && c->provisionalName() == d->provisionalName()) { disable = true; + } } } - if (hasText && text == "") + if (hasText && text == "") { disable = true; + } m_settingsDialog->disableApplyOk(disable); - if (hasText) + if (hasText) { m_settingsDialog->updateText(text); + } } -void UAVGadgetInstanceManager::settingsDialogShown(Core::Internal::SettingsDialog* settingsDialog) +void UAVGadgetInstanceManager::settingsDialogShown(Core::Internal::SettingsDialog *settingsDialog) { - foreach (QString classId, classIds()) - m_takenNames.insert(classId, configurationNames(classId)); + foreach(QString classId, classIds()) + m_takenNames.insert(classId, configurationNames(classId)); m_settingsDialog = settingsDialog; } @@ -451,18 +455,20 @@ void UAVGadgetInstanceManager::settingsDialogRemoved() m_takenNames.clear(); m_provisionalConfigs.clear(); m_provisionalDeletes.clear(); - m_provisionalOptionsPages.clear(); //TODO delete - foreach (IUAVGadgetConfiguration *config, m_configurations) - config->setProvisionalName(config->name()); + m_provisionalOptionsPages.clear(); // TODO delete + foreach(IUAVGadgetConfiguration * config, m_configurations) + config->setProvisionalName(config->name()); m_settingsDialog = 0; } QStringList UAVGadgetInstanceManager::configurationNames(QString classId) const { QStringList names; - foreach (IUAVGadgetConfiguration *config, m_configurations) { - if (config->classId() == classId) + + foreach(IUAVGadgetConfiguration * config, m_configurations) { + if (config->classId() == classId) { names.append(config->name()); + } } return names; } @@ -479,30 +485,33 @@ QIcon UAVGadgetInstanceManager::gadgetIcon(QString classId) const IUAVGadgetFactory *UAVGadgetInstanceManager::factory(QString classId) const { - foreach (IUAVGadgetFactory *f, m_factories) { - if (f->classId() == classId) + foreach(IUAVGadgetFactory * f, m_factories) { + if (f->classId() == classId) { return f; + } } return 0; } -QList *UAVGadgetInstanceManager::configurations(QString classId) const +QList *UAVGadgetInstanceManager::configurations(QString classId) const { - QList *configs = new QList; - foreach (IUAVGadgetConfiguration *config, m_configurations) { - if (config->classId() == classId) + QList *configs = new QList; + foreach(IUAVGadgetConfiguration * config, m_configurations) { + if (config->classId() == classId) { configs->append(config); + } } return configs; } -int UAVGadgetInstanceManager::indexForConfig(QList configurations, - QString classId, QString configName) +int UAVGadgetInstanceManager::indexForConfig(QList configurations, + QString classId, QString configName) { - for ( int i=0; iclassId() == classId - && configurations.at(i)->name() == configName ) + for (int i = 0; i < configurations.length(); ++i) { + if (configurations.at(i)->classId() == classId + && configurations.at(i)->name() == configName) { return i; + } } return -1; } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.h b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.h index a67953fc1..23ee1075a 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.h @@ -9,19 +9,19 @@ * @{ * @brief The Core GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,12 +37,10 @@ #include "uavconfiginfo.h" namespace ExtensionSystem { - class PluginManager; +class PluginManager; } -namespace Core -{ - +namespace Core { namespace Internal { class SettingsDialog; } @@ -52,10 +50,8 @@ class IUAVGadgetConfiguration; class IOptionsPage; class IUAVGadgetFactory; -class CORE_EXPORT UAVGadgetInstanceManager : public QObject -{ - -Q_OBJECT +class CORE_EXPORT UAVGadgetInstanceManager : public QObject { + Q_OBJECT public: explicit UAVGadgetInstanceManager(QObject *parent = 0); ~UAVGadgetInstanceManager(); @@ -69,44 +65,46 @@ public: void cloneConfiguration(IUAVGadgetConfiguration *config); void applyChanges(IUAVGadgetConfiguration *config); void configurationNameEdited(QString text, bool hasText = true); - QStringList classIds() const { return m_classIdNameMap.keys(); } + QStringList classIds() const + { + return m_classIdNameMap.keys(); + } QStringList configurationNames(QString classId) const; QString gadgetName(QString classId) const; QIcon gadgetIcon(QString classId) const; signals: - void configurationChanged(IUAVGadgetConfiguration* config); - void configurationAdded(IUAVGadgetConfiguration* config); - void configurationToBeDeleted(IUAVGadgetConfiguration* config); - void configurationNameChanged(IUAVGadgetConfiguration* config, QString oldName, QString newName); + void configurationChanged(IUAVGadgetConfiguration *config); + void configurationAdded(IUAVGadgetConfiguration *config); + void configurationToBeDeleted(IUAVGadgetConfiguration *config); + void configurationNameChanged(IUAVGadgetConfiguration *config, QString oldName, QString newName); public slots: - void settingsDialogShown(Core::Internal::SettingsDialog* settingsDialog); + void settingsDialogShown(Core::Internal::SettingsDialog *settingsDialog); void settingsDialogRemoved(); private: IUAVGadgetFactory *factory(QString classId) const; void createOptionsPages(); - QList *configurations(QString classId) const; + QList *configurations(QString classId) const; QString suggestName(QString classId, QString name); - QList m_gadgetInstances; - QList m_factories; - QList m_configurations; - QList m_optionsPages; + QList m_gadgetInstances; + QList m_factories; + QList m_configurations; + QList m_optionsPages; QMap m_classIdNameMap; QMap m_classIdIconMap; QMap m_takenNames; - QList m_provisionalConfigs; - QList m_provisionalDeletes; - QList m_provisionalOptionsPages; + QList m_provisionalConfigs; + QList m_provisionalDeletes; + QList m_provisionalOptionsPages; Core::Internal::SettingsDialog *m_settingsDialog; ExtensionSystem::PluginManager *m_pm; - int indexForConfig(QList configurations, + int indexForConfig(QList configurations, QString classId, QString configName); void readConfigs_1_1_0(QSettings *qs); void readConfigs_1_2_0(QSettings *qs); }; - } // namespace Core #endif // UAVGADGETINSTANCEMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.cpp index 502f75eef..e9868b4a3 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.cpp @@ -43,21 +43,21 @@ using namespace Core; using namespace Core::Internal; SplitterOrView::SplitterOrView(Core::UAVGadgetManager *uavGadgetManager, Core::IUAVGadget *uavGadget) : - m_uavGadgetManager(uavGadgetManager), - m_splitter(0) + m_uavGadgetManager(uavGadgetManager), + m_splitter(0) { - m_view = new UAVGadgetView(m_uavGadgetManager, uavGadget, this); + m_view = new UAVGadgetView(m_uavGadgetManager, uavGadget, this); m_layout = new QStackedLayout(this); m_layout->addWidget(m_view); } SplitterOrView::~SplitterOrView() { - if(m_view) { + if (m_view) { delete m_view; m_view = 0; } - if(m_splitter) { + if (m_splitter) { delete m_splitter; m_splitter = 0; } @@ -65,8 +65,9 @@ SplitterOrView::~SplitterOrView() void SplitterOrView::mousePressEvent(QMouseEvent *e) { - if (e->button() != Qt::LeftButton) + if (e->button() != Qt::LeftButton) { return; + } if (gadget()) { setFocus(Qt::MouseFocusReason); m_uavGadgetManager->setCurrentGadget(this->gadget()); @@ -80,9 +81,11 @@ SplitterOrView *SplitterOrView::findFirstView() { if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) - if (SplitterOrView *result = splitterOrView->findFirstView()) + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { + if (SplitterOrView * result = splitterOrView->findFirstView()) { return result; + } + } } return 0; } @@ -94,13 +97,16 @@ SplitterOrView *SplitterOrView::findFirstView() */ SplitterOrView *SplitterOrView::findView(Core::IUAVGadget *uavGadget) { - if (!uavGadget || hasGadget(uavGadget)) + if (!uavGadget || hasGadget(uavGadget)) { return this; + } if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) - if (SplitterOrView *result = splitterOrView->findView(uavGadget)) + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { + if (SplitterOrView * result = splitterOrView->findView(uavGadget)) { return result; + } + } } } return 0; @@ -111,13 +117,16 @@ SplitterOrView *SplitterOrView::findView(Core::IUAVGadget *uavGadget) */ SplitterOrView *SplitterOrView::findView(UAVGadgetView *view) { - if (view == m_view) + if (view == m_view) { return this; + } if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) - if (SplitterOrView *result = splitterOrView->findView(view)) + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { + if (SplitterOrView * result = splitterOrView->findView(view)) { return result; + } + } } } return 0; @@ -131,11 +140,13 @@ SplitterOrView *SplitterOrView::findSplitter(Core::IUAVGadget *uavGadget) { if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) { - if (splitterOrView->hasGadget(uavGadget)) + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { + if (splitterOrView->hasGadget(uavGadget)) { return this; - if (SplitterOrView *result = splitterOrView->findSplitter(uavGadget)) + } + if (SplitterOrView * result = splitterOrView->findSplitter(uavGadget)) { return result; + } } } } @@ -150,11 +161,13 @@ SplitterOrView *SplitterOrView::findSplitter(SplitterOrView *child) { if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) { - if (splitterOrView == child) + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { + if (splitterOrView == child) { return this; - if (SplitterOrView *result = splitterOrView->findSplitter(child)) + } + if (SplitterOrView * result = splitterOrView->findSplitter(child)) { return result; + } } } } @@ -168,6 +181,7 @@ SplitterOrView *SplitterOrView::findSplitter(SplitterOrView *child) SplitterOrView *SplitterOrView::findNextView(SplitterOrView *view) { bool found = false; + return findNextView_helper(view, &found); } @@ -184,9 +198,10 @@ SplitterOrView *SplitterOrView::findNextView_helper(SplitterOrView *view, bool * if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) { - if (SplitterOrView *result = splitterOrView->findNextView_helper(view, found)) + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { + if (SplitterOrView * result = splitterOrView->findNextView_helper(view, found)) { return result; + } } } } @@ -195,16 +210,19 @@ SplitterOrView *SplitterOrView::findNextView_helper(SplitterOrView *view, bool * QSize SplitterOrView::minimumSizeHint() const { - if (m_splitter) + if (m_splitter) { return m_splitter->minimumSizeHint(); + } return QSize(64, 64); } QSplitter *SplitterOrView::takeSplitter() { QSplitter *oldSplitter = m_splitter; - if (m_splitter) + + if (m_splitter) { m_layout->removeWidget(m_splitter); + } m_splitter = 0; return oldSplitter; } @@ -212,21 +230,24 @@ QSplitter *SplitterOrView::takeSplitter() UAVGadgetView *SplitterOrView::takeView() { UAVGadgetView *oldView = m_view; - if (m_view) + + if (m_view) { m_layout->removeWidget(m_view); + } m_view = 0; return oldView; } -QList SplitterOrView::gadgets() +QList SplitterOrView::gadgets() { - QList g; - if (hasGadget()) + QList g; + if (hasGadget()) { g.append(gadget()); + } if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) { - QList result = splitterOrView->gadgets(); + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { + QList result = splitterOrView->gadgets(); g.append(result); } } @@ -240,7 +261,7 @@ void SplitterOrView::split(Qt::Orientation orientation) Q_ASSERT(!m_splitter); m_splitter = new MiniSplitter(this); m_splitter->setOrientation(orientation); - connect(m_splitter, SIGNAL(splitterMoved(int,int)), this, SLOT(onSplitterMoved(int,int))); + connect(m_splitter, SIGNAL(splitterMoved(int, int)), this, SLOT(onSplitterMoved(int, int))); m_layout->addWidget(m_splitter); Core::IUAVGadget *ourGadget = m_view->gadget(); @@ -263,7 +284,8 @@ void SplitterOrView::split(Qt::Orientation orientation) } } -void SplitterOrView::onSplitterMoved( int pos, int index ) { +void SplitterOrView::onSplitterMoved(int pos, int index) +{ Q_UNUSED(pos); Q_UNUSED(index); // Update when the splitter is actually moved. @@ -280,7 +302,7 @@ void SplitterOrView::unsplitAll(IUAVGadget *currentGadget) delete m_splitter; m_splitter = 0; - m_view = new UAVGadgetView(m_uavGadgetManager, currentGadget, this); + m_view = new UAVGadgetView(m_uavGadgetManager, currentGadget, this); m_layout->addWidget(m_view); } @@ -291,7 +313,7 @@ void SplitterOrView::unsplitAll_helper() } if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) { + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { splitterOrView->unsplitAll_helper(); } } @@ -300,10 +322,11 @@ void SplitterOrView::unsplitAll_helper() void SplitterOrView::unsplit() { - if (!m_splitter) + if (!m_splitter) { return; + } Q_ASSERT(m_splitter->count() == 1); - SplitterOrView *childSplitterOrView = qobject_cast(m_splitter->widget(0)); + SplitterOrView *childSplitterOrView = qobject_cast(m_splitter->widget(0)); QSplitter *oldSplitter = m_splitter; m_splitter = 0; @@ -316,7 +339,7 @@ void SplitterOrView::unsplit() UAVGadgetView *childView = childSplitterOrView->view(); Q_ASSERT(childView); if (m_view) { - if (IUAVGadget *e = childView->gadget()) { + if (IUAVGadget * e = childView->gadget()) { childView->removeGadget(); m_view->setGadget(e); } @@ -331,43 +354,45 @@ void SplitterOrView::unsplit() m_uavGadgetManager->setCurrentGadget(findFirstView()->gadget()); } -void SplitterOrView::saveState(QSettings* qSettings) const { +void SplitterOrView::saveState(QSettings *qSettings) const +{ if (m_splitter) { qSettings->setValue("type", "splitter"); qSettings->setValue("splitterOrientation", (qint32)m_splitter->orientation()); QList sizesQVariant; - foreach (int value, m_sizes) { + foreach(int value, m_sizes) { sizesQVariant.append(value); } qSettings->setValue("splitterSizes", sizesQVariant); qSettings->beginGroup("side0"); - static_cast(m_splitter->widget(0))->saveState(qSettings); + static_cast(m_splitter->widget(0))->saveState(qSettings); qSettings->endGroup(); qSettings->beginGroup("side1"); - static_cast(m_splitter->widget(1))->saveState(qSettings); + static_cast(m_splitter->widget(1))->saveState(qSettings); qSettings->endGroup(); } else if (gadget()) { m_view->saveState(qSettings); } } -void SplitterOrView::restoreState(QSettings* qSettings) +void SplitterOrView::restoreState(QSettings *qSettings) { QString mode = qSettings->value("type").toString(); + if (mode == "splitter") { qint32 orientation = qSettings->value("splitterOrientation").toInt(); QList sizesQVariant = qSettings->value("splitterSizes").toList(); m_sizes.clear(); - foreach (QVariant value, sizesQVariant) { + foreach(QVariant value, sizesQVariant) { m_sizes.append(value.toInt()); } split((Qt::Orientation)orientation); m_splitter->setSizes(m_sizes); qSettings->beginGroup("side0"); - static_cast(m_splitter->widget(0))->restoreState(qSettings); + static_cast(m_splitter->widget(0))->restoreState(qSettings); qSettings->endGroup(); qSettings->beginGroup("side1"); - static_cast(m_splitter->widget(1))->restoreState(qSettings); + static_cast(m_splitter->widget(1))->restoreState(qSettings); qSettings->endGroup(); } else if (mode == "uavGadget") { m_view->restoreState(qSettings); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.h b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.h index 02e4e253a..a3a782d33 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.h @@ -35,11 +35,8 @@ #include "uavgadgetview.h" namespace Core { - namespace Internal { - -class SplitterOrView : public QWidget -{ +class SplitterOrView : public QWidget { Q_OBJECT public: SplitterOrView(UAVGadgetManager *uavGadgetManager, Core::IUAVGadget *uavGadget = 0); @@ -48,20 +45,41 @@ public: void split(Qt::Orientation orientation); void unsplit(); - inline bool isView() const { return m_view != 0; } + inline bool isView() const + { + return m_view != 0; + } - inline bool isSplitter() const { return m_splitter != 0; } - inline Core::IUAVGadget *gadget() const { return m_view ? m_view->gadget() : 0; } - inline bool hasGadget(Core::IUAVGadget *uavGadget) const { return m_view && m_view->hasGadget(uavGadget); } - inline bool hasGadget() const { return m_view && (m_view->gadget() != 0); } - inline UAVGadgetView *view() const { return m_view; } - inline QSplitter *splitter() const { return m_splitter; } - QList gadgets(); + inline bool isSplitter() const + { + return m_splitter != 0; + } + inline Core::IUAVGadget *gadget() const + { + return m_view ? m_view->gadget() : 0; + } + inline bool hasGadget(Core::IUAVGadget *uavGadget) const + { + return m_view && m_view->hasGadget(uavGadget); + } + inline bool hasGadget() const + { + return m_view && (m_view->gadget() != 0); + } + inline UAVGadgetView *view() const + { + return m_view; + } + inline QSplitter *splitter() const + { + return m_splitter; + } + QList gadgets(); QSplitter *takeSplitter(); UAVGadgetView *takeView(); - void saveState(QSettings*) const; - void restoreState(QSettings*); + void saveState(QSettings *) const; + void restoreState(QSettings *); SplitterOrView *findView(Core::IUAVGadget *uavGadget); SplitterOrView *findView(UAVGadgetView *view); @@ -71,18 +89,21 @@ public: SplitterOrView *findNextView(SplitterOrView *view); - QSize sizeHint() const { return minimumSizeHint(); } + QSize sizeHint() const + { + return minimumSizeHint(); + } QSize minimumSizeHint() const; void unsplitAll(IUAVGadget *currentGadget); protected: -// void paintEvent(QPaintEvent *); +// void paintEvent(QPaintEvent *); void mousePressEvent(QMouseEvent *e); private slots: // Called when the user moves the splitter, and updates our m_sizes. - void onSplitterMoved( int pos, int index ); + void onSplitterMoved(int pos, int index); private: void unsplitAll_helper(); @@ -103,8 +124,6 @@ private: // The splitter sizes. We keep our own copy of these, since after loading they can't realiably be retrieved. QList m_sizes; }; - - } } #endif // SPLITTERORVIEW_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.cpp index 1033de615..6da5940e7 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.cpp @@ -63,20 +63,20 @@ #include #include -Q_DECLARE_METATYPE(Core::IUAVGadget*) +Q_DECLARE_METATYPE(Core::IUAVGadget *) using namespace Core; using namespace Core::Internal; using namespace Utils; -enum { debugUAVGadgetManager=0 }; +enum { debugUAVGadgetManager = 0 }; static inline ExtensionSystem::PluginManager *pluginManager() { return ExtensionSystem::PluginManager::instance(); } -//===================UAVGadgetManager===================== +// ===================UAVGadgetManager===================== UAVGadgetManager::UAVGadgetManager(ICore *core, QString name, QIcon icon, int priority, QString uniqueName, QWidget *parent) : m_showToolbars(true), @@ -88,24 +88,24 @@ UAVGadgetManager::UAVGadgetManager(ICore *core, QString name, QIcon icon, int pr m_priority(priority), m_widget(new QWidget(parent)) { - // checking that the mode name is unique gives harmless // warnings on the console output ModeManager *modeManager = ModeManager::instance(); + if (!modeManager->mode(uniqueName)) { m_uniqueName = uniqueName; } else { // this shouldn't happen m_uniqueName = uniqueName + QString::number(quint64(this)); } - m_uniqueNameBA = m_uniqueName.toLatin1(); + m_uniqueNameBA = m_uniqueName.toLatin1(); m_uniqueModeName = m_uniqueNameBA.data(); connect(m_core, SIGNAL(contextAboutToChange(Core::IContext *)), this, SLOT(handleContextChange(Core::IContext *))); - connect(modeManager, SIGNAL(currentModeChanged(Core::IMode*)), - this, SLOT(modeChanged(Core::IMode*))); + connect(modeManager, SIGNAL(currentModeChanged(Core::IMode *)), + this, SLOT(modeChanged(Core::IMode *))); // other setup m_splitterOrView = new SplitterOrView(this, 0); @@ -125,20 +125,21 @@ UAVGadgetManager::UAVGadgetManager(ICore *core, QString name, QIcon icon, int pr } UAVGadgetManager::~UAVGadgetManager() -{ -} +{} QList UAVGadgetManager::context() const { static QList contexts = QList() << - UniqueIDManager::instance()->uniqueIdentifier(Constants::C_UAVGADGETMANAGER); + UniqueIDManager::instance()->uniqueIdentifier(Constants::C_UAVGADGETMANAGER); + return contexts; } void UAVGadgetManager::modeChanged(Core::IMode *mode) { - if (mode != this) + if (mode != this) { return; + } m_currentGadget->widget()->setFocus(); showToolbars(toolbarsShown()); @@ -152,26 +153,31 @@ void UAVGadgetManager::init() void UAVGadgetManager::handleContextChange(Core::IContext *context) { -// if (debugUAVGadgetManager) -// qDebug() << Q_FUNC_INFO << context; - IUAVGadget *uavGadget = context ? qobject_cast(context) : 0; - if (uavGadget) +// if (debugUAVGadgetManager) +// qDebug() << Q_FUNC_INFO << context; + IUAVGadget *uavGadget = context ? qobject_cast(context) : 0; + + if (uavGadget) { setCurrentGadget(uavGadget); + } } void UAVGadgetManager::setCurrentGadget(IUAVGadget *uavGadget) { - if (m_currentGadget == uavGadget) + if (m_currentGadget == uavGadget) { return; + } SplitterOrView *oldView = currentSplitterOrView(); m_currentGadget = uavGadget; - SplitterOrView *view = currentSplitterOrView(); + SplitterOrView *view = currentSplitterOrView(); if (oldView != view) { - if (oldView) + if (oldView) { oldView->update(); - if (view) + } + if (view) { view->update(); + } } uavGadget->widget()->setFocus(); emit currentGadgetChanged(uavGadget); @@ -182,8 +188,9 @@ void UAVGadgetManager::setCurrentGadget(IUAVGadget *uavGadget) */ Core::Internal::SplitterOrView *UAVGadgetManager::currentSplitterOrView() const { - if (!m_splitterOrView) // this is only for startup + if (!m_splitterOrView) { // this is only for startup return 0; + } SplitterOrView *view = m_currentGadget ? m_splitterOrView->findView(m_currentGadget) : 0; @@ -197,33 +204,36 @@ IUAVGadget *UAVGadgetManager::currentGadget() const void UAVGadgetManager::emptyView(Core::Internal::UAVGadgetView *view) { - if (!view) + if (!view) { return; + } IUAVGadget *uavGadget = view->gadget(); -// emit uavGadgetAboutToClose(uavGadget); +// emit uavGadgetAboutToClose(uavGadget); removeGadget(uavGadget); view->removeGadget(); -// emit uavGadgetsClosed(uavGadgets); +// emit uavGadgetsClosed(uavGadgets); } void UAVGadgetManager::closeView(Core::Internal::UAVGadgetView *view) { - if (!view) + if (!view) { return; + } SplitterOrView *splitterOrView = m_splitterOrView->findView(view); Q_ASSERT(splitterOrView); Q_ASSERT(splitterOrView->view() == view); - if (splitterOrView == m_splitterOrView) + if (splitterOrView == m_splitterOrView) { return; + } IUAVGadget *gadget = view->gadget(); emptyView(view); UAVGadgetInstanceManager *im = ICore::instance()->uavGadgetInstanceManager(); im->removeGadget(gadget); - SplitterOrView *splitter = m_splitterOrView->findSplitter(splitterOrView); + SplitterOrView *splitter = m_splitterOrView->findSplitter(splitterOrView); Q_ASSERT(splitterOrView->hasGadget() == false); Q_ASSERT(splitter->isSplitter() == true); splitterOrView->hide(); @@ -233,36 +243,41 @@ void UAVGadgetManager::closeView(Core::Internal::UAVGadgetView *view) SplitterOrView *newCurrent = splitter->findFirstView(); Q_ASSERT(newCurrent); - if (newCurrent) + if (newCurrent) { setCurrentGadget(newCurrent->gadget()); + } } void UAVGadgetManager::addGadgetToContext(IUAVGadget *gadget) { - if (!gadget) + if (!gadget) { return; + } m_core->addContextObject(gadget); -// emit uavGadgetOpened(uavGadget); +// emit uavGadgetOpened(uavGadget); } void UAVGadgetManager::removeGadget(IUAVGadget *gadget) { - if (!gadget) + if (!gadget) { return; - m_core->removeContextObject(qobject_cast(gadget)); + } + m_core->removeContextObject(qobject_cast(gadget)); } void UAVGadgetManager::ensureUAVGadgetManagerVisible() { - if (!m_widget->isVisible()) + if (!m_widget->isVisible()) { m_core->modeManager()->activateMode(this->uniqueModeName()); + } } void UAVGadgetManager::showToolbars(bool show) { - if (m_core->modeManager()->currentMode() != this) + if (m_core->modeManager()->currentMode() != this) { return; + } m_showToolbars = show; SplitterOrView *next = m_splitterOrView->findFirstView(); @@ -276,25 +291,27 @@ void UAVGadgetManager::showToolbars(bool show) void UAVGadgetManager::updateUavGadgetMenus() { - if (m_core->modeManager()->currentMode() != this) + if (m_core->modeManager()->currentMode() != this) { return; - if (!m_splitterOrView) // this is only for startup + } + if (!m_splitterOrView) { // this is only for startup return; + } // Splitting is only possible when the toolbars are shown bool hasSplitter = m_splitterOrView->isSplitter(); emit showUavGadgetMenus(m_showToolbars, hasSplitter); } -void UAVGadgetManager::saveState(QSettings* qSettings) const +void UAVGadgetManager::saveState(QSettings *qSettings) const { - qSettings->setValue("version","UAVGadgetManagerV1"); - qSettings->setValue("showToolbars",m_showToolbars); + qSettings->setValue("version", "UAVGadgetManagerV1"); + qSettings->setValue("showToolbars", m_showToolbars); qSettings->beginGroup("splitter"); m_splitterOrView->saveState(qSettings); qSettings->endGroup(); } -bool UAVGadgetManager::restoreState(QSettings* qSettings) +bool UAVGadgetManager::restoreState(QSettings *qSettings) { removeAllSplits(); @@ -338,12 +355,13 @@ void UAVGadgetManager::saveSettings(QSettings *qs) void UAVGadgetManager::readSettings(QSettings *qs) { QString uavGadgetManagerRootKey = "UAVGadgetManager"; - if(!qs->childGroups().contains(uavGadgetManagerRootKey)) { + + if (!qs->childGroups().contains(uavGadgetManagerRootKey)) { return; } qs->beginGroup(uavGadgetManagerRootKey); - if(!qs->childGroups().contains(uniqueModeName())) { + if (!qs->childGroups().contains(uniqueModeName())) { qs->endGroup(); return; } @@ -359,16 +377,17 @@ void UAVGadgetManager::readSettings(QSettings *qs) void UAVGadgetManager::split(Qt::Orientation orientation) { - if (m_core->modeManager()->currentMode() != this) + if (m_core->modeManager()->currentMode() != this) { return; + } IUAVGadget *uavGadget = m_currentGadget; Q_ASSERT(uavGadget); - SplitterOrView *view = currentSplitterOrView(); + SplitterOrView *view = currentSplitterOrView(); Q_ASSERT(view); view->split(orientation); - SplitterOrView *sor = m_splitterOrView->findView(uavGadget); + SplitterOrView *sor = m_splitterOrView->findView(uavGadget); SplitterOrView *next = m_splitterOrView->findNextView(sor); setCurrentGadget(next->gadget()); updateUavGadgetMenus(); @@ -386,12 +405,14 @@ void UAVGadgetManager::splitSideBySide() void UAVGadgetManager::removeCurrentSplit() { - if (m_core->modeManager()->currentMode() != this) + if (m_core->modeManager()->currentMode() != this) { return; + } SplitterOrView *viewToClose = currentSplitterOrView(); - if (viewToClose == m_splitterOrView) + if (viewToClose == m_splitterOrView) { return; + } closeView(viewToClose->view()); updateUavGadgetMenus(); } @@ -399,17 +420,19 @@ void UAVGadgetManager::removeCurrentSplit() // Removes all gadgets and splits in the workspace, except the current/active gadget. void UAVGadgetManager::removeAllSplits() { - if (m_core->modeManager()->currentMode() != this) + if (m_core->modeManager()->currentMode() != this) { return; + } - if (!m_splitterOrView->isSplitter()) + if (!m_splitterOrView->isSplitter()) { return; + } // Use a QPointer, just in case we accidently delete the gadget we want to keep. QPointer currentGadget = m_currentGadget; Q_ASSERT(currentGadget); - QList gadgets = m_splitterOrView->gadgets(); + QList gadgets = m_splitterOrView->gadgets(); Q_ASSERT(gadgets.count(currentGadget) == 1); gadgets.removeOne(currentGadget); @@ -422,7 +445,7 @@ void UAVGadgetManager::removeAllSplits() // Remove all other gadgets from the instance manager. UAVGadgetInstanceManager *im = ICore::instance()->uavGadgetInstanceManager(); - foreach (IUAVGadget *g, gadgets) { + foreach(IUAVGadget * g, gadgets) { im->removeGadget(g); } updateUavGadgetMenus(); @@ -430,14 +453,16 @@ void UAVGadgetManager::removeAllSplits() void UAVGadgetManager::gotoOtherSplit() { - if (m_core->modeManager()->currentMode() != this) + if (m_core->modeManager()->currentMode() != this) { return; + } if (m_splitterOrView->isSplitter()) { SplitterOrView *currentView = currentSplitterOrView(); SplitterOrView *view = m_splitterOrView->findNextView(currentView); - if (!view) + if (!view) { view = m_splitterOrView->findFirstView(); + } if (view) { setCurrentGadget(view->gadget()); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.h index ff2c76267..547c85e58 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.h @@ -45,20 +45,16 @@ class QModelIndex; QT_END_NAMESPACE namespace Core { - class IContext; class ICore; class IUAVGadget; namespace Internal { - class UAVGadgetView; class SplitterOrView; - } // namespace Internal -class CORE_EXPORT UAVGadgetManager : public IMode -{ +class CORE_EXPORT UAVGadgetManager : public IMode { Q_OBJECT public: @@ -67,26 +63,47 @@ public: virtual ~UAVGadgetManager(); // IMode - QString name() const { return m_name; } - QIcon icon() const { return m_icon; } - int priority() const { return m_priority; } - void setPriority(int priority) { m_priority = priority; } - const char *uniqueModeName() const { return m_uniqueModeName; } + QString name() const + { + return m_name; + } + QIcon icon() const + { + return m_icon; + } + int priority() const + { + return m_priority; + } + void setPriority(int priority) + { + m_priority = priority; + } + const char *uniqueModeName() const + { + return m_uniqueModeName; + } QList context() const; void init(); - QWidget *widget() { return m_widget; } + QWidget *widget() + { + return m_widget; + } void ensureUAVGadgetManagerVisible(); IUAVGadget *currentGadget() const; - void saveState(QSettings*) const; - bool restoreState(QSettings* qSettings); + void saveState(QSettings *) const; + bool restoreState(QSettings *qSettings); - void saveSettings(QSettings* qs); - void readSettings(QSettings* qs); - bool toolbarsShown() { return m_showToolbars; } + void saveSettings(QSettings *qs); + void readSettings(QSettings *qs); + bool toolbarsShown() + { + return m_showToolbars; + } signals: void currentGadgetChanged(IUAVGadget *gadget); @@ -126,13 +143,12 @@ private: int m_priority; QString m_uniqueName; QByteArray m_uniqueNameBA; - const char* m_uniqueModeName; + const char *m_uniqueModeName; QWidget *m_widget; friend class Core::Internal::SplitterOrView; friend class Core::Internal::UAVGadgetView; }; - } // namespace Core #endif // UAVGADGETMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.cpp index ad56261a6..79f177943 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.cpp @@ -62,17 +62,16 @@ using namespace Core; using namespace Core::Internal; UAVGadgetView::UAVGadgetView(Core::UAVGadgetManager *uavGadgetManager, IUAVGadget *uavGadget, QWidget *parent) : - QWidget(parent), - m_uavGadgetManager(uavGadgetManager), - m_uavGadget(uavGadget), - m_toolBar(new QWidget(this)), - m_defaultToolBar(new QComboBox(this)), - m_uavGadgetList(new QComboBox(this)), - m_closeButton(new QToolButton(this)), - m_defaultIndex(0), - m_activeLabel(new QLabel) + QWidget(parent), + m_uavGadgetManager(uavGadgetManager), + m_uavGadget(uavGadget), + m_toolBar(new QWidget(this)), + m_defaultToolBar(new QComboBox(this)), + m_uavGadgetList(new QComboBox(this)), + m_closeButton(new QToolButton(this)), + m_defaultIndex(0), + m_activeLabel(new QLabel) { - tl = new QVBoxLayout(this); tl->setSpacing(0); tl->setMargin(0); @@ -82,24 +81,22 @@ UAVGadgetView::UAVGadgetView(Core::UAVGadgetManager *uavGadgetManager, IUAVGadge m_uavGadgetList->setMaxVisibleItems(40); m_uavGadgetList->setContextMenuPolicy(Qt::CustomContextMenu); UAVGadgetInstanceManager *im = ICore::instance()->uavGadgetInstanceManager(); - QStringList sl = im->classIds(); + QStringList sl = im->classIds(); int index = 0; bool startFromOne = false; - foreach(QString classId, sl) - { + foreach(QString classId, sl) { if (classId == QString("EmptyGadget")) { m_defaultIndex = 0; - startFromOne = true; + startFromOne = true; m_uavGadgetList->insertItem(0, im->gadgetName(classId), classId); m_uavGadgetList->setItemIcon(0, im->gadgetIcon(classId)); m_uavGadgetList->insertSeparator(1); } else { - int i = startFromOne ? 1 : 0; - for ( ; i < m_uavGadgetList->count(); i++) - { - if (QString::localeAwareCompare(m_uavGadgetList->itemText(i), im->gadgetName(classId)) > 0) + for (; i < m_uavGadgetList->count(); i++) { + if (QString::localeAwareCompare(m_uavGadgetList->itemText(i), im->gadgetName(classId)) > 0) { break; + } } m_uavGadgetList->insertItem(i, im->gadgetName(classId), classId); m_uavGadgetList->setItemIcon(i, im->gadgetIcon(classId)); @@ -141,7 +138,7 @@ UAVGadgetView::UAVGadgetView(Core::UAVGadgetManager *uavGadgetManager, IUAVGadge connect(m_uavGadgetList, SIGNAL(activated(int)), this, SLOT(listSelectionActivated(int))); connect(m_closeButton, SIGNAL(clicked()), this, SLOT(closeView()), Qt::QueuedConnection); - connect(m_uavGadgetManager, SIGNAL(currentGadgetChanged(IUAVGadget*)), this, SLOT(currentGadgetChanged(IUAVGadget*))); + connect(m_uavGadgetManager, SIGNAL(currentGadgetChanged(IUAVGadget *)), this, SLOT(currentGadgetChanged(IUAVGadget *))); } if (m_uavGadget) { setGadget(m_uavGadget); @@ -157,7 +154,7 @@ UAVGadgetView::~UAVGadgetView() bool UAVGadgetView::hasGadget(IUAVGadget *uavGadget) const { - return (m_uavGadget == uavGadget); + return m_uavGadget == uavGadget; } void UAVGadgetView::showToolbar(bool show) @@ -172,8 +169,9 @@ void UAVGadgetView::closeView() void UAVGadgetView::removeGadget() { - if (!m_uavGadget) + if (!m_uavGadget) { return; + } tl->removeWidget(m_uavGadget->widget()); m_uavGadget->setParent(0); @@ -214,15 +212,19 @@ void UAVGadgetView::setGadget(IUAVGadget *uavGadget) void UAVGadgetView::updateToolBar() { - if (!m_uavGadget) + if (!m_uavGadget) { return; + } QComboBox *toolBar = m_uavGadget->toolBar(); - if (!toolBar) + if (!toolBar) { toolBar = m_defaultToolBar; - if (m_activeToolBar == toolBar) + } + if (m_activeToolBar == toolBar) { return; - if (toolBar->count() == 0) + } + if (toolBar->count() == 0) { toolBar->hide(); + } m_toolBar->layout()->addWidget(toolBar); m_activeToolBar->setVisible(false); m_activeToolBar = toolBar; @@ -230,17 +232,19 @@ void UAVGadgetView::updateToolBar() void UAVGadgetView::listSelectionActivated(int index) { - if (index < 0 || index >= m_uavGadgetList->count()) + if (index < 0 || index >= m_uavGadgetList->count()) { index = m_defaultIndex; + } QString classId = m_uavGadgetList->itemData(index).toString(); - if (m_uavGadget && (m_uavGadget->classId() == classId)) + if (m_uavGadget && (m_uavGadget->classId() == classId)) { return; + } UAVGadgetInstanceManager *im = ICore::instance()->uavGadgetInstanceManager(); IUAVGadget *gadget = im->createGadget(classId, this); - IUAVGadget *gadgetToRemove = m_uavGadget; + IUAVGadget *gadgetToRemove = m_uavGadget; setGadget(gadget); m_uavGadgetManager->setCurrentGadget(gadget); im->removeGadget(gadgetToRemove); @@ -256,7 +260,7 @@ void UAVGadgetView::currentGadgetChanged(IUAVGadget *gadget) m_activeLabel->setVisible(m_uavGadget == gadget); } -void UAVGadgetView::saveState(QSettings* qSettings) +void UAVGadgetView::saveState(QSettings *qSettings) { qSettings->setValue("type", "uavGadget"); qSettings->setValue("classId", gadget()->classId()); @@ -265,10 +269,11 @@ void UAVGadgetView::saveState(QSettings* qSettings) qSettings->endGroup(); } -void UAVGadgetView::restoreState(QSettings* qSettings) +void UAVGadgetView::restoreState(QSettings *qSettings) { QString classId = qSettings->value("classId").toString(); int index = indexOfClassId(classId); + if (index < 0) { index = m_defaultIndex; } @@ -276,13 +281,12 @@ void UAVGadgetView::restoreState(QSettings* qSettings) IUAVGadget *newGadget; UAVGadgetInstanceManager *im = ICore::instance()->uavGadgetInstanceManager(); - if(qSettings->childGroups().contains("gadget")) { + if (qSettings->childGroups().contains("gadget")) { newGadget = im->createGadget(classId, this, false); qSettings->beginGroup("gadget"); newGadget->restoreState(qSettings); qSettings->endGroup(); - } - else { + } else { newGadget = im->createGadget(classId, this); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.h b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.h index dfd52adcf..37e7362c2 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.h @@ -52,15 +52,11 @@ class StyledBar; } namespace Core { - class IUAVGadget; class UAVGadgetManager; namespace Internal { - - -class UAVGadgetView : public QWidget -{ +class UAVGadgetView : public QWidget { Q_OBJECT public: @@ -101,7 +97,6 @@ private: int m_defaultIndex; QLabel *m_activeLabel; }; - } } #endif // UAVGADGETVIEW_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.cpp index caa2fa1f5..a2a956c99 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.cpp @@ -10,18 +10,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -58,8 +58,9 @@ QWidget *UAVGadgetOptionsPageDecorator::createPage(QWidget *parent) m_page->lockCheckBox->hide(); m_page->nameLineEdit->setDisabled(true); } - if (!m_instanceManager->canDeleteConfiguration(m_config)) + if (!m_instanceManager->canDeleteConfiguration(m_config)) { m_page->deleteButton->setDisabled(true); + } m_page->lockCheckBox->hide(); // m_page->nameLineEdit->setText(m_id); @@ -106,4 +107,3 @@ void UAVGadgetOptionsPageDecorator::textEdited(QString name) m_config->setProvisionalName(name); m_instanceManager->configurationNameEdited(name); } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.h b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.h index e71bfc5db..f82244a0d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.h @@ -10,18 +10,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,20 +35,30 @@ class Ui_TopOptionsPage; namespace Core { - class IUAVGadgetConfiguration; class UAVGadgetInstanceManager; -class CORE_EXPORT UAVGadgetOptionsPageDecorator : public Core::IOptionsPage -{ -Q_OBJECT +class CORE_EXPORT UAVGadgetOptionsPageDecorator : public Core::IOptionsPage { + Q_OBJECT public: explicit UAVGadgetOptionsPageDecorator(IOptionsPage *page, IUAVGadgetConfiguration *config, bool isSingleConfigurationGadget = false, QObject *parent = 0); - QString id() const { return m_id; } - QString trName() const { return m_id; } - QString category() const { return m_category; } - QString trCategory() const { return m_categoryTr; } + QString id() const + { + return m_id; + } + QString trName() const + { + return m_id; + } + QString category() const + { + return m_category; + } + QString trCategory() const + { + return m_categoryTr; + } QWidget *createPage(QWidget *parent); void apply(); @@ -74,7 +84,6 @@ private: Ui_TopOptionsPage *m_page; }; - } // namespace Core #endif // UAVGADGETOPTIONSPAGEDECORATOR_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.cpp index d609d6e09..f544bebcc 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,7 +31,7 @@ using namespace Core; -UniqueIDManager* UniqueIDManager::m_instance = 0; +UniqueIDManager *UniqueIDManager::m_instance = 0; UniqueIDManager::UniqueIDManager() { @@ -51,8 +51,9 @@ bool UniqueIDManager::hasUniqueIdentifier(const QString &id) const int UniqueIDManager::uniqueIdentifier(const QString &id) { - if (hasUniqueIdentifier(id)) + if (hasUniqueIdentifier(id)) { return m_uniqueIdentifiers.value(id); + } int uid = m_uniqueIdentifiers.count() + 1; m_uniqueIdentifiers.insert(id, uid); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.h index d86f2483f..2b0738a85 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,14 +35,15 @@ #include namespace Core { - -class CORE_EXPORT UniqueIDManager -{ +class CORE_EXPORT UniqueIDManager { public: UniqueIDManager(); ~UniqueIDManager(); - static UniqueIDManager *instance() { return m_instance; } + static UniqueIDManager *instance() + { + return m_instance; + } bool hasUniqueIdentifier(const QString &id) const; int uniqueIdentifier(const QString &id); @@ -52,7 +53,6 @@ private: QHash m_uniqueIdentifiers; static UniqueIDManager *m_instance; }; - } // namespace Core #endif // UNIQUEIDMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.cpp index 821c7850c..2d62e4150 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -52,7 +52,7 @@ void VariableManager::insert(const QString &variable, const QString &value) void VariableManager::insertFileInfo(const QString &tag, const QFileInfo &file) { insert(tag, file.filePath()); - insert(tag + QLatin1String(":absoluteFilePath"), file.absoluteFilePath()); + insert(tag + QLatin1String(":absoluteFilePath"), file.absoluteFilePath()); insert(tag + QLatin1String(":absolutePath"), file.absolutePath()); insert(tag + QLatin1String(":baseName"), file.baseName()); insert(tag + QLatin1String(":canonicalPath"), file.canonicalPath()); @@ -100,6 +100,7 @@ bool VariableManager::remove(const QString &variable) QString VariableManager::resolve(const QString &stringWithVariables) const { QString result = stringWithVariables; + QMapIterator i(m_map); while (i.hasNext()) { i.next(); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.h b/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.h index 81ce4192c..27d028985 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,16 +40,17 @@ class QFileInfo; QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT VariableManager : public QObject -{ +class CORE_EXPORT VariableManager : public QObject { Q_OBJECT public: VariableManager(QObject *parent); ~VariableManager(); - static VariableManager* instance() { return m_instance; } + static VariableManager *instance() + { + return m_instance; + } void insert(const QString &variable, const QString &value); void insertFileInfo(const QString &tag, const QFileInfo &file); @@ -64,7 +65,6 @@ private: QMap m_map; static VariableManager *m_instance; }; - } // namespace Core #endif // VARIABLEMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.cpp b/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.cpp index 01cf29feb..27129b34c 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -60,28 +60,26 @@ VersionDialog::VersionDialog(QWidget *parent) layout->setSizeConstraint(QLayout::SetFixedSize); #ifdef UAVO_HASH - //: This gets conditionally inserted as argument %11 into the description string. - QByteArray uavoHashArray; - QString uavoHash = QString::fromLatin1(Core::Constants::UAVOSHA1_STR); - uavoHash.chop(2); - uavoHash.remove(0,2); - uavoHash=uavoHash.trimmed(); - bool ok; - foreach(QString str,uavoHash.split(",")) - { - uavoHashArray.append(str.toInt(&ok,16)); - } - QString gcsUavoHashStr; - foreach(char i, uavoHashArray) - { - gcsUavoHashStr.append(QString::number(i,16).right(2)); - } - QString uavoHashStr = gcsUavoHashStr; + // : This gets conditionally inserted as argument %11 into the description string. + QByteArray uavoHashArray; + QString uavoHash = QString::fromLatin1(Core::Constants::UAVOSHA1_STR); + uavoHash.chop(2); + uavoHash.remove(0, 2); + uavoHash = uavoHash.trimmed(); + bool ok; + foreach(QString str, uavoHash.split(",")) { + uavoHashArray.append(str.toInt(&ok, 16)); + } + QString gcsUavoHashStr; + foreach(char i, uavoHashArray) { + gcsUavoHashStr.append(QString::number(i, 16).right(2)); + } + QString uavoHashStr = gcsUavoHashStr; #else QString uavoHashStr = "N/A"; #endif - const QString description = tr( + const QString description = tr( "

OpenPilot Ground Control Station

" "GCS Revision: %1
" "UAVO Hash: %2
" @@ -100,17 +98,17 @@ VersionDialog::VersionDialog(QWidget *parent) "The program is provided AS IS with NO WARRANTY OF ANY KIND, " "INCLUDING THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A " "PARTICULAR PURPOSE." - ).arg( + ).arg( QString::fromLatin1(GCS_REVISION_STR).left(60), // %1 - uavoHashStr, // %2 - QLatin1String(GCS_ORIGIN_STR), // $3 - QLatin1String(__DATE__), // %4 - QLatin1String(__TIME__), // %5 - QLatin1String(QT_VERSION_STR), // %6 - QString::number(QSysInfo::WordSize), // %7 - QLatin1String(GCS_AUTHOR), // %8 - QLatin1String(GCS_YEAR_STR) // %9 - ); + uavoHashStr, // %2 + QLatin1String(GCS_ORIGIN_STR), // $3 + QLatin1String(__DATE__), // %4 + QLatin1String(__TIME__), // %5 + QLatin1String(QT_VERSION_STR), // %6 + QString::number(QSysInfo::WordSize), // %7 + QLatin1String(GCS_AUTHOR), // %8 + QLatin1String(GCS_YEAR_STR) // %9 + ); QLabel *copyRightLabel = new QLabel(description); copyRightLabel->setWordWrap(true); @@ -118,14 +116,14 @@ VersionDialog::VersionDialog(QWidget *parent) copyRightLabel->setTextInteractionFlags(Qt::TextBrowserInteraction); QDialogButtonBox *buttonBox = new QDialogButtonBox(QDialogButtonBox::Close); - QPushButton *closeButton = buttonBox->button(QDialogButtonBox::Close); + QPushButton *closeButton = buttonBox->button(QDialogButtonBox::Close); QTC_ASSERT(closeButton, /**/); buttonBox->addButton(closeButton, QDialogButtonBox::ButtonRole(QDialogButtonBox::RejectRole | QDialogButtonBox::AcceptRole)); - connect(buttonBox , SIGNAL(rejected()), this, SLOT(reject())); + connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject())); QLabel *logoLabel = new QLabel; logoLabel->setPixmap(QPixmap(QLatin1String(":/core/images/openpilot_logo_128.png"))); - layout->addWidget(logoLabel , 0, 0, 1, 1); + layout->addWidget(logoLabel, 0, 0, 1, 1); layout->addWidget(copyRightLabel, 0, 1, 4, 4); layout->addWidget(buttonBox, 4, 0, 1, 5); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.h b/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.h index fa5a8a89c..16ffe3d14 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,14 +35,11 @@ namespace Core { namespace Internal { - -class VersionDialog : public QDialog -{ +class VersionDialog : public QDialog { Q_OBJECT public: explicit VersionDialog(QWidget *parent); }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp index 1f3b12910..62279fd1a 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp @@ -10,18 +10,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,12 +41,10 @@ const int WorkspaceSettings::MAX_WORKSPACES = 10; WorkspaceSettings::WorkspaceSettings(QObject *parent) : IOptionsPage(parent) -{ -} +{} WorkspaceSettings::~WorkspaceSettings() -{ -} +{} // IOptionsPage @@ -95,14 +93,15 @@ QWidget *WorkspaceSettings::createPage(QWidget *parent) m_currentIndex = 0; selectWorkspace(m_currentIndex); - if (0 <= m_tabBarPlacementIndex && m_tabBarPlacementIndex < m_page->comboBoxTabBarPlacement->count()) + if (0 <= m_tabBarPlacementIndex && m_tabBarPlacementIndex < m_page->comboBoxTabBarPlacement->count()) { m_page->comboBoxTabBarPlacement->setCurrentIndex(m_tabBarPlacementIndex); + } m_page->checkBoxAllowTabMovement->setChecked(m_allowTabBarMovement); return w; } -void WorkspaceSettings::readSettings(QSettings* qs) +void WorkspaceSettings::readSettings(QSettings *qs) { m_names.clear(); m_iconNames.clear(); @@ -112,31 +111,31 @@ void WorkspaceSettings::readSettings(QSettings* qs) m_numberOfWorkspaces = qs->value(QLatin1String("NumberOfWorkspaces"), 2).toInt(); m_previousNumberOfWorkspaces = m_numberOfWorkspaces; for (int i = 1; i <= MAX_WORKSPACES; ++i) { - QString numberString = QString::number(i); - QString defaultName = "Workspace" + numberString; + QString numberString = QString::number(i); + QString defaultName = "Workspace" + numberString; QString defaultIconName = "Icon" + numberString; - QString name = qs->value(defaultName, defaultName).toString(); + QString name = qs->value(defaultName, defaultName).toString(); QString iconName = qs->value(defaultIconName, ":/core/images/openpilot_logo_64.png").toString(); m_names.append(name); m_iconNames.append(iconName); - m_modeNames.append(QString("Mode")+ QString::number(i)); + m_modeNames.append(QString("Mode") + QString::number(i)); } m_tabBarPlacementIndex = qs->value(QLatin1String("TabBarPlacementIndex"), 1).toInt(); // 1 == "Bottom" - m_allowTabBarMovement = qs->value(QLatin1String("AllowTabBarMovement"), false).toBool(); + m_allowTabBarMovement = qs->value(QLatin1String("AllowTabBarMovement"), false).toBool(); qs->endGroup(); QTabWidget::TabPosition pos = m_tabBarPlacementIndex == 0 ? QTabWidget::North : QTabWidget::South; emit tabBarSettingsApplied(pos, m_allowTabBarMovement); } -void WorkspaceSettings::saveSettings(QSettings* qs) +void WorkspaceSettings::saveSettings(QSettings *qs) { qs->beginGroup(QLatin1String("Workspace")); qs->setValue(QLatin1String("NumberOfWorkspaces"), m_numberOfWorkspaces); for (int i = 0; i < MAX_WORKSPACES; ++i) { - QString mode = QString("Mode")+ QString::number(i+1); + QString mode = QString("Mode") + QString::number(i + 1); int j = m_modeNames.indexOf(mode); - QString numberString = QString::number(i+1); - QString defaultName = "Workspace" + numberString; + QString numberString = QString::number(i + 1); + QString defaultName = "Workspace" + numberString; QString defaultIconName = "Icon" + numberString; qs->setValue(defaultName, m_names.at(j)); qs->setValue(defaultIconName, m_iconNames.at(j)); @@ -157,16 +156,16 @@ void WorkspaceSettings::apply() m_previousNumberOfWorkspaces = m_numberOfWorkspaces; } - ModeManager* modeManager = Core::ICore::instance()->modeManager(); + ModeManager *modeManager = Core::ICore::instance()->modeManager(); for (int i = 0; i < MAX_WORKSPACES; ++i) { IMode *baseMode = modeManager->mode(modeName(i)); - Core::UAVGadgetManager *mode = qobject_cast(baseMode); + Core::UAVGadgetManager *mode = qobject_cast(baseMode); if (mode) { modeManager->updateModeNameIcon(mode, QIcon(iconName(i)), name(i)); } } m_tabBarPlacementIndex = m_page->comboBoxTabBarPlacement->currentIndex(); - m_allowTabBarMovement = m_page->checkBoxAllowTabMovement->isChecked(); + m_allowTabBarMovement = m_page->checkBoxAllowTabMovement->isChecked(); QTabWidget::TabPosition pos = m_tabBarPlacementIndex == 0 ? QTabWidget::North : QTabWidget::South; emit tabBarSettingsApplied(pos, m_allowTabBarMovement); } @@ -185,6 +184,7 @@ void WorkspaceSettings::textEdited(QString name) void WorkspaceSettings::iconChanged() { QString iconName = m_page->iconPathChooser->path(); + m_page->workspaceComboBox->setItemIcon(m_currentIndex, QIcon(iconName)); } @@ -196,8 +196,8 @@ void WorkspaceSettings::numberOfWorkspacesChanged(int value) for (int i = count; i < value; ++i) { m_page->workspaceComboBox->addItem(QIcon(m_iconNames.at(i)), m_names.at(i)); } - } else if (value < count){ - for (int i = count-1; i >= value; --i) { + } else if (value < count) { + for (int i = count - 1; i >= value; --i) { m_page->workspaceComboBox->removeItem(i); } } @@ -213,19 +213,19 @@ void WorkspaceSettings::selectWorkspace(int index, bool store) m_page->workspaceComboBox->setItemText(m_currentIndex, m_names.at(m_currentIndex)); } - // display current workspace + // display current workspace QString iconName = m_iconNames.at(index); m_page->iconPathChooser->setPath(iconName); m_page->nameEdit->setText(m_names.at(index)); m_currentIndex = index; } -void WorkspaceSettings::newModeOrder(QVector modes) +void WorkspaceSettings::newModeOrder(QVector modes) { QList priorities; QStringList modeNames; for (int i = 0; i < modes.count(); ++i) { - Core::UAVGadgetManager *mode = qobject_cast(modes.at(i)); + Core::UAVGadgetManager *mode = qobject_cast(modes.at(i)); if (mode) { priorities.append(mode->priority()); modeNames.append(mode->uniqueModeName()); @@ -235,12 +235,12 @@ void WorkspaceSettings::newModeOrder(QVector modes) bool swapped = false; do { swapped = false; - for (int i = 0; i < m_names.count()-1; ++i) { - int j = i+1; + for (int i = 0; i < m_names.count() - 1; ++i) { + int j = i + 1; int p = modeNames.indexOf(m_modeNames.at(i)); int q = modeNames.indexOf(m_modeNames.at(j)); - bool nonShowingMode = (p == -1 && q >=0); - bool pqBothFound = (p >= 0 && q >= 0); + bool nonShowingMode = (p == -1 && q >= 0); + bool pqBothFound = (p >= 0 && q >= 0); if (nonShowingMode || (pqBothFound && (priorities.at(q) > priorities.at(p)))) { m_names.swap(i, j); m_iconNames.swap(i, j); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h index 6ff92cad9..37d2897ae 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h @@ -10,18 +10,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,19 +36,16 @@ class QSettings; namespace Core { - - class ModeManager; - class IMode; +class ModeManager; +class IMode; namespace Internal { - namespace Ui { - class WorkspaceSettings; +class WorkspaceSettings; } -class WorkspaceSettings : public IOptionsPage -{ -Q_OBJECT +class WorkspaceSettings : public IOptionsPage { + Q_OBJECT public: WorkspaceSettings(QObject *parent = 0); ~WorkspaceSettings(); @@ -62,12 +59,24 @@ public: QWidget *createPage(QWidget *parent); void apply(); void finish(); - void readSettings(QSettings* qs); - void saveSettings(QSettings* qs); - int numberOfWorkspaces() const { return m_numberOfWorkspaces;} - QString iconName(int i) const { return m_iconNames.at(i);} - QString name(int i) const { return m_names.at(i);} - QString modeName(int i) const { return m_modeNames.at(i);} + void readSettings(QSettings *qs); + void saveSettings(QSettings *qs); + int numberOfWorkspaces() const + { + return m_numberOfWorkspaces; + } + QString iconName(int i) const + { + return m_iconNames.at(i); + } + QString name(int i) const + { + return m_names.at(i); + } + QString modeName(int i) const + { + return m_modeNames.at(i); + } signals: void tabBarSettingsApplied(QTabWidget::TabPosition pos, bool movable); @@ -77,7 +86,7 @@ public slots: void numberOfWorkspacesChanged(int value); void textEdited(QString string); void iconChanged(); - void newModeOrder(QVector modes); + void newModeOrder(QVector modes); private: Ui::WorkspaceSettings *m_page; @@ -91,7 +100,6 @@ private: bool m_allowTabBarMovement; static const int MAX_WORKSPACES; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debugengine.cpp b/ground/openpilotgcs/src/plugins/debuggadget/debugengine.cpp index 48ec18525..39a0a8f70 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debugengine.cpp +++ b/ground/openpilotgcs/src/plugins/debuggadget/debugengine.cpp @@ -1,13 +1,12 @@ #include "debugengine.h" debugengine::debugengine() -{ -} +{} void debugengine::writeToStdErr(const QString &level, const QList &msgs) { - emit dbgMsgError(level,msgs); + emit dbgMsgError(level, msgs); } void debugengine::writeToStdOut(const QString &level, const QList &msgs) { - emit dbgMsg(level,msgs); + emit dbgMsg(level, msgs); } diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debugengine.h b/ground/openpilotgcs/src/plugins/debuggadget/debugengine.h index 5452f7590..1871a6611 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debugengine.h +++ b/ground/openpilotgcs/src/plugins/debuggadget/debugengine.h @@ -2,17 +2,16 @@ #define DEBUGENGINE_H #include "qxtbasicstdloggerengine.h" #include -class debugengine:public QObject,public QxtBasicSTDLoggerEngine -{ +class debugengine : public QObject, public QxtBasicSTDLoggerEngine { Q_OBJECT public: debugengine(); protected: - void writeToStdErr ( const QString & level, const QList & msgs ); - void writeToStdOut ( const QString & level, const QList & msgs ); + void writeToStdErr(const QString & level, const QList & msgs); + void writeToStdOut(const QString & level, const QList & msgs); signals: - void dbgMsgError( const QString & level, const QList & msgs ); - void dbgMsg( const QString & level, const QList & msgs ); + void dbgMsgError(const QString & level, const QList & msgs); + void dbgMsg(const QString & level, const QList & msgs); }; #endif // DEBUGENGINE_H diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.cpp b/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.cpp index 05c147c03..ea5bcb0af 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.cpp +++ b/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -28,10 +28,9 @@ #include "debuggadgetwidget.h" DebugGadget::DebugGadget(QString classId, DebugGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} DebugGadget::~DebugGadget() { diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.h b/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.h index 68773df53..ca70511f5 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.h +++ b/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,26 +33,34 @@ namespace Core { class IUAVGadget; } -//class QWidget; -//class QString; +// class QWidget; +// class QString; class DebugGadgetWidget; using namespace Core; -class DebugGadget : public Core::IUAVGadget -{ +class DebugGadget : public Core::IUAVGadget { Q_OBJECT public: DebugGadget(QString classId, DebugGadgetWidget *widget, QWidget *parent = 0); ~DebugGadget(); - QList context() const { return m_context; } - QWidget *widget() { return m_widget; } - QString contextHelpId() const { return QString(); } + QList context() const + { + return m_context; + } + QWidget *widget() + { + return m_widget; + } + QString contextHelpId() const + { + return QString(); + } private: QWidget *m_widget; - QList m_context; + QList m_context; }; diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.cpp b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.cpp index f7d925b39..fcacb7a28 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,18 +30,17 @@ #include DebugGadgetFactory::DebugGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("DebugGadget"), - tr("DebugGadget"), - parent) -{ -} + IUAVGadgetFactory(QString("DebugGadget"), + tr("DebugGadget"), + parent) +{} DebugGadgetFactory::~DebugGadgetFactory() +{} + +IUAVGadget *DebugGadgetFactory::createGadget(QWidget *parent) { + DebugGadgetWidget *gadgetWidget = new DebugGadgetWidget(parent); -} - -IUAVGadget* DebugGadgetFactory::createGadget(QWidget *parent) { - DebugGadgetWidget* gadgetWidget = new DebugGadgetWidget(parent); return new DebugGadget(QString("DebugGadget"), gadgetWidget, parent); } diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.h b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.h index 186fca5f6..aea8a33ed 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,8 +37,7 @@ class IUAVGadgetFactory; using namespace Core; -class DebugGadgetFactory : public IUAVGadgetFactory -{ +class DebugGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: DebugGadgetFactory(QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.cpp b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.cpp index 34043c806..100a1c4f9 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -43,11 +43,11 @@ DebugGadgetWidget::DebugGadgetWidget(QWidget *parent) : QLabel(parent) { m_config = new Ui_Form(); m_config->setupUi(this); - debugengine * de=new debugengine(); + debugengine *de = new debugengine(); QxtLogger::getInstance()->addLoggerEngine("debugplugin", de); - connect(de,SIGNAL(dbgMsg(QString,QList)),this,SLOT(dbgMsg(QString,QList))); - connect(de,SIGNAL(dbgMsgError(QString,QList)),this,SLOT(dbgMsgError(QString,QList))); - connect(m_config->pushButton,SIGNAL(clicked()),this,SLOT(saveLog())); + connect(de, SIGNAL(dbgMsg(QString, QList)), this, SLOT(dbgMsg(QString, QList))); + connect(de, SIGNAL(dbgMsgError(QString, QList)), this, SLOT(dbgMsgError(QString, QList))); + connect(m_config->pushButton, SIGNAL(clicked()), this, SLOT(saveLog())); } DebugGadgetWidget::~DebugGadgetWidget() @@ -59,7 +59,7 @@ void DebugGadgetWidget::dbgMsg(const QString &level, const QList &msgs { m_config->plainTextEdit->setTextColor(Qt::red); - m_config->plainTextEdit->append(QString("%2[%0]%1").arg(level).arg(msgs[0].toString()).arg(QTime::currentTime().toString())); + m_config->plainTextEdit->append(QString("%2[%0]%1").arg(level).arg(msgs[0].toString()).arg(QTime::currentTime().toString())); QScrollBar *sb = m_config->plainTextEdit->verticalScrollBar(); sb->setValue(sb->maximum()); @@ -70,7 +70,7 @@ void DebugGadgetWidget::dbgMsgError(const QString &level, const QList m_config->plainTextEdit->setTextColor(Qt::black); - m_config->plainTextEdit->append(QString("%2[%0]%1").arg(level).arg(msgs[0].toString()).arg(QTime::currentTime().toString())); + m_config->plainTextEdit->append(QString("%2[%0]%1").arg(level).arg(msgs[0].toString()).arg(QTime::currentTime().toString())); QScrollBar *sb = m_config->plainTextEdit->verticalScrollBar(); sb->setValue(sb->maximum()); @@ -78,13 +78,14 @@ void DebugGadgetWidget::dbgMsgError(const QString &level, const QList void DebugGadgetWidget::saveLog() { QString fileName = QFileDialog::getSaveFileName(0, tr("Save log File As"), ""); + if (fileName.isEmpty()) { return; } QFile file(fileName); if (file.open(QIODevice::WriteOnly) && - (file.write(m_config->plainTextEdit->toHtml().toAscii()) != -1)) { + (file.write(m_config->plainTextEdit->toHtml().toAscii()) != -1)) { file.close(); } else { QMessageBox::critical(0, diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.h b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.h index ed71f73c2..37826df95 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,8 +30,7 @@ #include #include "ui_debug.h" -class DebugGadgetWidget : public QLabel -{ +class DebugGadgetWidget : public QLabel { Q_OBJECT public: @@ -41,9 +40,9 @@ public: private: Ui_Form *m_config; private slots: - void saveLog(); - void dbgMsgError( const QString & level, const QList & msgs ); - void dbgMsg( const QString & level, const QList & msgs ); + void saveLog(); + void dbgMsgError(const QString & level, const QList & msgs); + void dbgMsg(const QString & level, const QList & msgs); }; #endif /* DEBUGGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.cpp b/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.cpp index f83395ef7..149fa57b3 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.cpp +++ b/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,32 +34,31 @@ DebugPlugin::DebugPlugin() { - // Do nothing + // Do nothing } DebugPlugin::~DebugPlugin() { - // Do nothing + // Do nothing } -bool DebugPlugin::initialize(const QStringList& args, QString *errMsg) +bool DebugPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new DebugGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new DebugGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void DebugPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void DebugPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(DebugPlugin) - diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.h b/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.h index 9cdbefd02..83115c1c9 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.h +++ b/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class DebugGadgetFactory; -class DebugPlugin : public ExtensionSystem::IPlugin -{ +class DebugPlugin : public ExtensionSystem::IPlugin { public: - DebugPlugin(); - ~DebugPlugin(); + DebugPlugin(); + ~DebugPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - DebugGadgetFactory *mf; + DebugGadgetFactory *mf; }; #endif /* DEBUGPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadget.cpp b/ground/openpilotgcs/src/plugins/dial/dialgadget.cpp index ea944348e..46c46d507 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadget.cpp +++ b/ground/openpilotgcs/src/plugins/dial/dialgadget.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -31,10 +31,9 @@ #include "dialgadgetconfiguration.h" DialGadget::DialGadget(QString classId, DialGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} DialGadget::~DialGadget() { @@ -42,20 +41,21 @@ DialGadget::~DialGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void DialGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void DialGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - DialGadgetConfiguration *m = qobject_cast(config); + DialGadgetConfiguration *m = qobject_cast(config); + m_widget->setDialFile(m->dialFile(), m->dialBackground(), m->dialForeground(), m->dialNeedle1(), - m->dialNeedle2(),m->dialNeedle3(),m->getN1Move(), m->getN2Move(), + m->dialNeedle2(), m->dialNeedle3(), m->getN1Move(), m->getN2Move(), m->getN3Move()); - m_widget->enableOpenGL(m->useOpenGL()); - m_widget->enableSmoothUpdates(m->getBeSmooth()); + m_widget->enableOpenGL(m->useOpenGL()); + m_widget->enableSmoothUpdates(m->getBeSmooth()); m_widget->setN1Min(m->getN1Min()); m_widget->setN1Max(m->getN1Max()); @@ -67,8 +67,8 @@ void DialGadget::loadConfiguration(IUAVGadgetConfiguration* config) m_widget->setN3Max(m->getN3Max()); m_widget->setN3Factor(m->getN3Factor()); m_widget->setDialFont(m->getFont()); - m_widget->connectNeedles(m->getN1DataObject(),m->getN1ObjField(), - m->getN2DataObject(),m->getN2ObjField(), - m->getN3DataObject(),m->getN3ObjField() + m_widget->connectNeedles(m->getN1DataObject(), m->getN1ObjField(), + m->getN2DataObject(), m->getN2ObjField(), + m->getN3DataObject(), m->getN3ObjField() ); } diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadget.h b/ground/openpilotgcs/src/plugins/dial/dialgadget.h index 6bd91cc7b..2ed4d566b 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadget.h +++ b/ground/openpilotgcs/src/plugins/dial/dialgadget.h @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -39,15 +39,17 @@ class DialGadgetWidget; using namespace Core; -class DialGadget : public Core::IUAVGadget -{ +class DialGadget : public Core::IUAVGadget { Q_OBJECT public: DialGadget(QString classId, DialGadgetWidget *widget, QWidget *parent = 0); ~DialGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: DialGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.cpp index e8c3a8443..d85e64089 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,10 +33,10 @@ * Loads a saved configuration or defaults if non exist. * */ -DialGadgetConfiguration::DialGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +DialGadgetConfiguration::DialGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_defaultDial("Unknown"), - dialBackgroundID("background"), + dialBackgroundID("background"), dialForegroundID("foreground"), dialNeedleID1("needle"), dialNeedleID2("needle2"), @@ -52,42 +52,42 @@ DialGadgetConfiguration::DialGadgetConfiguration(QString classId, QSettings* qSe needle3Factor(1), needle1Move("Rotate"), needle2Move("Rotate"), - needle3Move("Rotate"), - useOpenGLFlag(false), - beSmooth(true) + needle3Move("Rotate"), + useOpenGLFlag(false), + beSmooth(true) { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { QString dialFile = qSettings->value("dialFile").toString(); - m_defaultDial=Utils::PathUtils().InsertDataPath(dialFile); - dialBackgroundID = qSettings->value("dialBackgroundID").toString(); - dialForegroundID = qSettings->value("dialForegroundID").toString(); - dialNeedleID1 = qSettings->value("dialNeedleID1").toString(); - dialNeedleID2 = qSettings->value("dialNeedleID2").toString(); - dialNeedleID3 = qSettings->value("dialNeedleID3").toString(); - needle1MinValue = qSettings->value("needle1MinValue").toDouble(); - needle1MaxValue = qSettings->value("needle1MaxValue").toDouble(); - needle2MinValue = qSettings->value("needle2MinValue").toDouble(); - needle2MaxValue = qSettings->value("needle2MaxValue").toDouble(); - needle3MinValue = qSettings->value("needle3MinValue").toDouble(); - needle3MaxValue = qSettings->value("needle3MaxValue").toDouble(); - needle1DataObject = qSettings->value("needle1DataObject").toString(); + m_defaultDial = Utils::PathUtils().InsertDataPath(dialFile); + dialBackgroundID = qSettings->value("dialBackgroundID").toString(); + dialForegroundID = qSettings->value("dialForegroundID").toString(); + dialNeedleID1 = qSettings->value("dialNeedleID1").toString(); + dialNeedleID2 = qSettings->value("dialNeedleID2").toString(); + dialNeedleID3 = qSettings->value("dialNeedleID3").toString(); + needle1MinValue = qSettings->value("needle1MinValue").toDouble(); + needle1MaxValue = qSettings->value("needle1MaxValue").toDouble(); + needle2MinValue = qSettings->value("needle2MinValue").toDouble(); + needle2MaxValue = qSettings->value("needle2MaxValue").toDouble(); + needle3MinValue = qSettings->value("needle3MinValue").toDouble(); + needle3MaxValue = qSettings->value("needle3MaxValue").toDouble(); + needle1DataObject = qSettings->value("needle1DataObject").toString(); needle1ObjectField = qSettings->value("needle1ObjectField").toString(); - needle2DataObject = qSettings->value("needle2DataObject").toString(); + needle2DataObject = qSettings->value("needle2DataObject").toString(); needle2ObjectField = qSettings->value("needle2ObjectField").toString(); - needle3DataObject = qSettings->value("needle3DataObject").toString(); + needle3DataObject = qSettings->value("needle3DataObject").toString(); needle3ObjectField = qSettings->value("needle3ObjectField").toString(); - needle1Factor = qSettings->value("needle1Factor").toDouble(); - needle2Factor = qSettings->value("needle2Factor").toDouble(); - needle3Factor = qSettings->value("needle3Factor").toDouble(); - needle1Move = qSettings->value("needle1Move").toString(); - needle2Move = qSettings->value("needle2Move").toString(); - needle3Move = qSettings->value("needle3Move").toString(); + needle1Factor = qSettings->value("needle1Factor").toDouble(); + needle2Factor = qSettings->value("needle2Factor").toDouble(); + needle3Factor = qSettings->value("needle3Factor").toDouble(); + needle1Move = qSettings->value("needle1Move").toString(); + needle2Move = qSettings->value("needle2Move").toString(); + needle3Move = qSettings->value("needle3Move").toString(); font = qSettings->value("font").toString(); - useOpenGLFlag = qSettings->value("useOpenGLFlag").toBool(); - beSmooth = qSettings->value("beSmooth").toBool(); - } + useOpenGLFlag = qSettings->value("useOpenGLFlag").toBool(); + beSmooth = qSettings->value("beSmooth").toBool(); + } } /** @@ -97,7 +97,8 @@ DialGadgetConfiguration::DialGadgetConfiguration(QString classId, QSettings* qSe IUAVGadgetConfiguration *DialGadgetConfiguration::clone() { DialGadgetConfiguration *m = new DialGadgetConfiguration(this->classId()); - m->m_defaultDial=m_defaultDial; + + m->m_defaultDial = m_defaultDial; m->setDialBackgroundID(dialBackgroundID); m->setDialForegroundID(dialForegroundID); m->setDialNeedleID1(dialNeedleID1); @@ -122,8 +123,8 @@ IUAVGadgetConfiguration *DialGadgetConfiguration::clone() m->setN2Move(needle2Move); m->setN3Move(needle3Move); m->setFont(font); - m->useOpenGLFlag = useOpenGLFlag; - m->beSmooth = beSmooth; + m->useOpenGLFlag = useOpenGLFlag; + m->beSmooth = beSmooth; return m; } @@ -132,11 +133,13 @@ IUAVGadgetConfiguration *DialGadgetConfiguration::clone() * Saves a configuration. * */ -void DialGadgetConfiguration::saveConfig(QSettings* settings) const { +void DialGadgetConfiguration::saveConfig(QSettings *settings) const +{ QString dialFile = Utils::PathUtils().RemoveDataPath(m_defaultDial); + settings->setValue("dialFile", dialFile); - settings->setValue("dialBackgroundID", dialBackgroundID); + settings->setValue("dialBackgroundID", dialBackgroundID); settings->setValue("dialForegroundID", dialForegroundID); settings->setValue("dialNeedleID1", dialNeedleID1); @@ -167,6 +170,6 @@ void DialGadgetConfiguration::saveConfig(QSettings* settings) const { settings->setValue("font", font); - settings->setValue("useOpenGLFlag", useOpenGLFlag); - settings->setValue("beSmooth", beSmooth); + settings->setValue("useOpenGLFlag", useOpenGLFlag); + settings->setValue("beSmooth", beSmooth); } diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.h index 79a3de02a..6929debf9 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.h @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,83 +35,244 @@ using namespace Core; /* Despite its name, this is actually a generic analog dial supporting up to two rotating "needle" indicators. - */ -class DialGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT + */ +class DialGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit DialGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit DialGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - //set dial configuration functions - void setDialFile(QString dialFile){m_defaultDial=dialFile;} - void setDialBackgroundID(QString elementID) { dialBackgroundID = elementID;} - void setDialForegroundID(QString elementID) { dialForegroundID = elementID;} - void setDialNeedleID1(QString elementID) { dialNeedleID1 = elementID;} - void setDialNeedleID2(QString elementID) { dialNeedleID2 = elementID;} - void setDialNeedleID3(QString elementID) { dialNeedleID3 = elementID;} - void setN1Min(double val) { needle1MinValue = val;} - void setN2Min(double val) { needle2MinValue = val;} - void setN3Min(double val) { needle3MinValue = val;} - void setN1Max(double val) { needle1MaxValue = val;} - void setN2Max(double val) { needle2MaxValue = val;} - void setN3Max(double val) { needle3MaxValue = val;} - void setN1Factor(double val) { needle1Factor = val;} - void setN2Factor(double val) { needle2Factor = val;} - void setN3Factor(double val) { needle3Factor = val;} - void setN1DataObject(QString text) {needle1DataObject = text; } - void setN2DataObject(QString text){ needle2DataObject = text; } - void setN3DataObject(QString text){ needle3DataObject = text; } - void setN1ObjField(QString text) { needle1ObjectField = text; } - void setN2ObjField(QString text) { needle2ObjectField = text; } - void setN3ObjField(QString text) { needle3ObjectField = text; } - void setN1Move( QString move) { needle1Move = move; } - void setN2Move( QString move) { needle2Move = move; } - void setN3Move( QString move) { needle3Move = move; } - void setFont(QString text) { font = text; } - void setUseOpenGL(bool flag) { useOpenGLFlag = flag; } - void setBeSmooth(bool flag) { beSmooth = flag;} + // set dial configuration functions + void setDialFile(QString dialFile) + { + m_defaultDial = dialFile; + } + void setDialBackgroundID(QString elementID) + { + dialBackgroundID = elementID; + } + void setDialForegroundID(QString elementID) + { + dialForegroundID = elementID; + } + void setDialNeedleID1(QString elementID) + { + dialNeedleID1 = elementID; + } + void setDialNeedleID2(QString elementID) + { + dialNeedleID2 = elementID; + } + void setDialNeedleID3(QString elementID) + { + dialNeedleID3 = elementID; + } + void setN1Min(double val) + { + needle1MinValue = val; + } + void setN2Min(double val) + { + needle2MinValue = val; + } + void setN3Min(double val) + { + needle3MinValue = val; + } + void setN1Max(double val) + { + needle1MaxValue = val; + } + void setN2Max(double val) + { + needle2MaxValue = val; + } + void setN3Max(double val) + { + needle3MaxValue = val; + } + void setN1Factor(double val) + { + needle1Factor = val; + } + void setN2Factor(double val) + { + needle2Factor = val; + } + void setN3Factor(double val) + { + needle3Factor = val; + } + void setN1DataObject(QString text) + { + needle1DataObject = text; + } + void setN2DataObject(QString text) + { + needle2DataObject = text; + } + void setN3DataObject(QString text) + { + needle3DataObject = text; + } + void setN1ObjField(QString text) + { + needle1ObjectField = text; + } + void setN2ObjField(QString text) + { + needle2ObjectField = text; + } + void setN3ObjField(QString text) + { + needle3ObjectField = text; + } + void setN1Move(QString move) + { + needle1Move = move; + } + void setN2Move(QString move) + { + needle2Move = move; + } + void setN3Move(QString move) + { + needle3Move = move; + } + void setFont(QString text) + { + font = text; + } + void setUseOpenGL(bool flag) + { + useOpenGLFlag = flag; + } + void setBeSmooth(bool flag) + { + beSmooth = flag; + } - //get dial configuration functions - QString dialFile() {return m_defaultDial;} - QString dialBackground() {return dialBackgroundID;} - QString dialForeground() {return dialForegroundID;} - QString dialNeedle1() {return dialNeedleID1;} - QString dialNeedle2() {return dialNeedleID2;} - QString dialNeedle3() {return dialNeedleID3;} - double getN1Min() { return needle1MinValue;} - double getN2Min() { return needle2MinValue;} - double getN3Min() { return needle3MinValue;} - double getN1Max() { return needle1MaxValue;} - double getN2Max() { return needle2MaxValue;} - double getN3Max() { return needle3MaxValue;} - double getN1Factor() { return needle1Factor;} - double getN2Factor() { return needle2Factor;} - double getN3Factor() { return needle3Factor;} - QString getN1DataObject() { return needle1DataObject; } - QString getN2DataObject() { return needle2DataObject; } - QString getN3DataObject() { return needle3DataObject; } - QString getN1ObjField() { return needle1ObjectField; } - QString getN2ObjField() { return needle2ObjectField; } - QString getN3ObjField() { return needle3ObjectField; } - QString getN1Move() { return needle1Move; } - QString getN2Move() { return needle2Move; } - QString getN3Move() { return needle3Move; } - QString getFont() { return font;} - bool useOpenGL() { return useOpenGLFlag; } - bool getBeSmooth() { return beSmooth; } + // get dial configuration functions + QString dialFile() + { + return m_defaultDial; + } + QString dialBackground() + { + return dialBackgroundID; + } + QString dialForeground() + { + return dialForegroundID; + } + QString dialNeedle1() + { + return dialNeedleID1; + } + QString dialNeedle2() + { + return dialNeedleID2; + } + QString dialNeedle3() + { + return dialNeedleID3; + } + double getN1Min() + { + return needle1MinValue; + } + double getN2Min() + { + return needle2MinValue; + } + double getN3Min() + { + return needle3MinValue; + } + double getN1Max() + { + return needle1MaxValue; + } + double getN2Max() + { + return needle2MaxValue; + } + double getN3Max() + { + return needle3MaxValue; + } + double getN1Factor() + { + return needle1Factor; + } + double getN2Factor() + { + return needle2Factor; + } + double getN3Factor() + { + return needle3Factor; + } + QString getN1DataObject() + { + return needle1DataObject; + } + QString getN2DataObject() + { + return needle2DataObject; + } + QString getN3DataObject() + { + return needle3DataObject; + } + QString getN1ObjField() + { + return needle1ObjectField; + } + QString getN2ObjField() + { + return needle2ObjectField; + } + QString getN3ObjField() + { + return needle3ObjectField; + } + QString getN1Move() + { + return needle1Move; + } + QString getN2Move() + { + return needle2Move; + } + QString getN3Move() + { + return needle3Move; + } + QString getFont() + { + return font; + } + bool useOpenGL() + { + return useOpenGLFlag; + } + bool getBeSmooth() + { + return beSmooth; + } - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: QString m_defaultDial; // The name of the dial's SVG source file QString dialBackgroundID; // SVG elementID of the background QString dialForegroundID; // ... of the foreground - QString dialNeedleID1; // ... and the first needle - QString dialNeedleID2; // ... and the second - QString dialNeedleID3; // ... and the third + QString dialNeedleID1; // ... and the first needle + QString dialNeedleID2; // ... and the second + QString dialNeedleID3; // ... and the third // Note: MinValue not used at the moment! double needle1MinValue; // Value corresponding to a 0 degree angle; @@ -139,8 +300,8 @@ private: QString needle2Move; QString needle3Move; - bool useOpenGLFlag; - bool beSmooth; + bool useOpenGLFlag; + bool beSmooth; }; #endif // DIALGADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.cpp index b4336cd58..3c07f11e5 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,29 +33,27 @@ #include DialGadgetFactory::DialGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("DialGadget"), - tr("Analog Dial"), - parent) -{ -} + IUAVGadgetFactory(QString("DialGadget"), + tr("Analog Dial"), + parent) +{} DialGadgetFactory::~DialGadgetFactory() -{ -} +{} -Core::IUAVGadget* DialGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *DialGadgetFactory::createGadget(QWidget *parent) { - DialGadgetWidget* gadgetWidget = new DialGadgetWidget(parent); + DialGadgetWidget *gadgetWidget = new DialGadgetWidget(parent); + return new DialGadget(QString("DialGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *DialGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *DialGadgetFactory::createConfiguration(QSettings *qSettings) { return new DialGadgetConfiguration(QString("DialGadget"), qSettings); } IOptionsPage *DialGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new DialGadgetOptionsPage(qobject_cast(config)); + return new DialGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.h b/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.h index 17aed6262..e4c51c4c8 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.h @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,15 +38,14 @@ class IUAVGadgetFactory; using namespace Core; -class DialGadgetFactory : public IUAVGadgetFactory -{ +class DialGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: DialGadgetFactory(QObject *parent = 0); ~DialGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.cpp index 1afd69305..292629f69 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -39,28 +39,26 @@ #include DialGadgetOptionsPage::DialGadgetOptionsPage(DialGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *DialGadgetOptionsPage::createPage(QWidget *parent) { - Q_UNUSED(parent); options_page = new Ui::DialGadgetOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget; - //main layout + // main layout options_page->setupUi(optionsPageWidget); // Fills the combo boxes for the UAVObjects ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - QList< QList > objList = objManager->getDataObjects(); - foreach (QList list, objList) { - foreach (UAVDataObject* obj, list) { + QList< QList > objList = objManager->getDataObjects(); + foreach(QList list, objList) { + foreach(UAVDataObject * obj, list) { options_page->uavObject1->addItem(obj->getName()); options_page->uavObject2->addItem(obj->getName()); options_page->uavObject3->addItem(obj->getName()); @@ -104,41 +102,41 @@ QWidget *DialGadgetOptionsPage::createPage(QWidget *parent) options_page->moveNeedle2->setCurrentIndex(options_page->moveNeedle2->findText(m_config->getN2Move())); options_page->moveNeedle3->setCurrentIndex(options_page->moveNeedle3->findText(m_config->getN3Move())); - options_page->useOpenGL->setChecked(m_config->useOpenGL()); - options_page->smoothUpdates->setChecked(m_config->getBeSmooth()); + options_page->useOpenGL->setChecked(m_config->useOpenGL()); + options_page->smoothUpdates->setChecked(m_config->getBeSmooth()); - //select saved UAV Object field values - if(options_page->uavObject1->findText(m_config->getN1DataObject())!=-1){ + // select saved UAV Object field values + if (options_page->uavObject1->findText(m_config->getN1DataObject()) != -1) { options_page->uavObject1->setCurrentIndex(options_page->uavObject1->findText(m_config->getN1DataObject())); // Now load the object field values - 1st check that the object saved in the config still exists - UAVDataObject* obj = dynamic_cast( objManager->getObject(m_config->getN1DataObject()) ); - if (obj != NULL ) { - on_uavObject1_currentIndexChanged(m_config->getN1DataObject()); - // And set the highlighed value from the settings: - options_page->objectField1->setCurrentIndex(options_page->objectField1->findText(m_config->getN1ObjField())); + UAVDataObject *obj = dynamic_cast(objManager->getObject(m_config->getN1DataObject())); + if (obj != NULL) { + on_uavObject1_currentIndexChanged(m_config->getN1DataObject()); + // And set the highlighed value from the settings: + options_page->objectField1->setCurrentIndex(options_page->objectField1->findText(m_config->getN1ObjField())); } } connect(options_page->uavObject1, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_uavObject1_currentIndexChanged(QString))); - if(options_page->uavObject2->findText(m_config->getN2DataObject())!=-1){ + if (options_page->uavObject2->findText(m_config->getN2DataObject()) != -1) { options_page->uavObject2->setCurrentIndex(options_page->uavObject2->findText(m_config->getN2DataObject())); // Now load the object field values: - UAVDataObject* obj = dynamic_cast( objManager->getObject(m_config->getN2DataObject())); - if (obj != NULL ) { + UAVDataObject *obj = dynamic_cast(objManager->getObject(m_config->getN2DataObject())); + if (obj != NULL) { on_uavObject2_currentIndexChanged(m_config->getN2DataObject()); options_page->objectField2->setCurrentIndex(options_page->objectField2->findText(m_config->getN2ObjField())); } } connect(options_page->uavObject2, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_uavObject2_currentIndexChanged(QString))); - if(options_page->uavObject3->findText(m_config->getN3DataObject())!=-1){ + if (options_page->uavObject3->findText(m_config->getN3DataObject()) != -1) { options_page->uavObject3->setCurrentIndex(options_page->uavObject3->findText(m_config->getN3DataObject())); // Now load the object field values: - UAVDataObject* obj = dynamic_cast( objManager->getObject(m_config->getN3DataObject())); - if (obj != NULL ) { + UAVDataObject *obj = dynamic_cast(objManager->getObject(m_config->getN3DataObject())); + if (obj != NULL) { on_uavObject3_currentIndexChanged(m_config->getN3DataObject()); - options_page->objectField3->setCurrentIndex(options_page->objectField3->findText(m_config->getN3ObjField())); + options_page->objectField3->setCurrentIndex(options_page->objectField3->findText(m_config->getN3ObjField())); } } connect(options_page->uavObject3, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_uavObject3_currentIndexChanged(QString))); @@ -157,7 +155,7 @@ QWidget *DialGadgetOptionsPage::createPage(QWidget *parent) void DialGadgetOptionsPage::apply() { m_config->setDialFile(options_page->svgSourceFile->path()); - m_config->setDialBackgroundID(options_page->backgroundID->text()); + m_config->setDialBackgroundID(options_page->backgroundID->text()); m_config->setDialForegroundID(options_page->foregroundID->text()); m_config->setDialNeedleID1(options_page->needle1ID->text()); m_config->setDialNeedleID2(options_page->needle2ID->text()); @@ -181,8 +179,8 @@ void DialGadgetOptionsPage::apply() m_config->setN2Move(options_page->moveNeedle2->currentText()); m_config->setN3Move(options_page->moveNeedle3->currentText()); m_config->setFont(font.toString()); - m_config->setUseOpenGL(options_page->useOpenGL->checkState()); - m_config->setBeSmooth(options_page->smoothUpdates->checkState()); + m_config->setUseOpenGL(options_page->useOpenGL->checkState()); + m_config->setBeSmooth(options_page->smoothUpdates->checkState()); } /** @@ -192,87 +190,86 @@ void DialGadgetOptionsPage::apply() void DialGadgetOptionsPage::on_fontPicker_clicked() { bool ok; - font = QFontDialog::getFont(&ok, QFont("Arial", 12), qobject_cast(this)); + + font = QFontDialog::getFont(&ok, QFont("Arial", 12), qobject_cast(this)); } /* - Fills in the field1 combo box when value is changed in the - object1 field -*/ -void DialGadgetOptionsPage::on_uavObject1_currentIndexChanged(QString val) { + Fills in the field1 combo box when value is changed in the + object1 field + */ +void DialGadgetOptionsPage::on_uavObject1_currentIndexChanged(QString val) +{ options_page->objectField1->clear(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(val) ); - QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { - if(field->getType() == UAVObjectField::STRING || field->getType() == UAVObjectField::ENUM ) + UAVDataObject *obj = dynamic_cast(objManager->getObject(val)); + QList fieldList = obj->getFields(); + foreach(UAVObjectField * field, fieldList) { + if (field->getType() == UAVObjectField::STRING || field->getType() == UAVObjectField::ENUM) { continue; - if(field->getElementNames().count() > 1) - { - foreach(QString elemName , field->getElementNames()) - { + } + if (field->getElementNames().count() > 1) { + foreach(QString elemName, field->getElementNames()) { options_page->objectField1->addItem(field->getName() + "-" + elemName); } - } - else + } else { options_page->objectField1->addItem(field->getName()); + } } } /* - Fills in the field2 combo box when value is changed in the - object2 field -*/ -void DialGadgetOptionsPage::on_uavObject2_currentIndexChanged(QString val) { + Fills in the field2 combo box when value is changed in the + object2 field + */ +void DialGadgetOptionsPage::on_uavObject2_currentIndexChanged(QString val) +{ options_page->objectField2->clear(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(val) ); - QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { - if(field->getType() == UAVObjectField::STRING || field->getType() == UAVObjectField::ENUM ) + UAVDataObject *obj = dynamic_cast(objManager->getObject(val)); + QList fieldList = obj->getFields(); + foreach(UAVObjectField * field, fieldList) { + if (field->getType() == UAVObjectField::STRING || field->getType() == UAVObjectField::ENUM) { continue; - if(field->getElementNames().count() > 1) - { - foreach(QString elemName , field->getElementNames()) - { + } + if (field->getElementNames().count() > 1) { + foreach(QString elemName, field->getElementNames()) { options_page->objectField2->addItem(field->getName() + "-" + elemName); } - } - else + } else { options_page->objectField2->addItem(field->getName()); + } } } /* - Fills in the field3 combo box when value is changed in the - object3 field -*/ -void DialGadgetOptionsPage::on_uavObject3_currentIndexChanged(QString val) { + Fills in the field3 combo box when value is changed in the + object3 field + */ +void DialGadgetOptionsPage::on_uavObject3_currentIndexChanged(QString val) +{ options_page->objectField3->clear(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(val) ); - QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { - if(field->getType() == UAVObjectField::STRING || field->getType() == UAVObjectField::ENUM ) + UAVDataObject *obj = dynamic_cast(objManager->getObject(val)); + QList fieldList = obj->getFields(); + foreach(UAVObjectField * field, fieldList) { + if (field->getType() == UAVObjectField::STRING || field->getType() == UAVObjectField::ENUM) { continue; - if(field->getElementNames().count() > 1) - { - foreach(QString elemName , field->getElementNames()) - { + } + if (field->getElementNames().count() > 1) { + foreach(QString elemName, field->getElementNames()) { options_page->objectField3->addItem(field->getName() + "-" + elemName); } - } - else + } else { options_page->objectField3->addItem(field->getName()); + } } } void DialGadgetOptionsPage::finish() -{ - -} +{} diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.h index 49d0d329d..1b87226f3 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.h @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -43,14 +43,13 @@ class IUAVGadgetConfiguration; class DialGadgetConfiguration; namespace Ui { - class DialGadgetOptionsPage; +class DialGadgetOptionsPage; } using namespace Core; -class DialGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class DialGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit DialGadgetOptionsPage(DialGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.cpp index cc9b05b5c..fd31187f3 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,16 +37,16 @@ DialGadgetWidget::DialGadgetWidget(QWidget *parent) : QGraphicsView(parent) // TODO: create a proper "needle" object instead of hardcoding all this // which is ugly (but easy). - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setScene(new QGraphicsScene(this)); setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); m_renderer = new QSvgRenderer(); - obj1 = NULL; - obj2 = NULL; - obj3 = NULL; + obj1 = NULL; + obj2 = NULL; + obj3 = NULL; m_text1 = NULL; m_text2 = NULL; m_text3 = NULL; // Should be initialized to NULL otherwise the setFont method @@ -56,8 +56,8 @@ DialGadgetWidget::DialGadgetWidget(QWidget *parent) : QGraphicsView(parent) needle2Target = 0; needle3Target = 0; -// beSmooth = true; - beSmooth = false; +// beSmooth = true; + beSmooth = false; // This timer mechanism makes needles rotate smoothly connect(&dialTimer, SIGNAL(timeout()), this, SLOT(rotateNeedles())); @@ -65,52 +65,54 @@ DialGadgetWidget::DialGadgetWidget(QWidget *parent) : QGraphicsView(parent) DialGadgetWidget::~DialGadgetWidget() { - // Do nothing + // Do nothing } /*! - \brief Enables/Disables OpenGL - */ + \brief Enables/Disables OpenGL + */ void DialGadgetWidget::enableOpenGL(bool flag) { - if (flag) - setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); - else - setViewport(new QWidget); + if (flag) { + setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); + } else { + setViewport(new QWidget); + } } /*! - \brief Connects the widget to the relevant UAVObjects - */ + \brief Connects the widget to the relevant UAVObjects + */ void DialGadgetWidget::connectNeedles(QString object1, QString nfield1, - QString object2, QString nfield2, - QString object3, QString nfield3) { - if (obj1 != NULL) - disconnect(obj1,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updateNeedle1(UAVObject*))); - if (obj2 != NULL) - disconnect(obj2,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updateNeedle2(UAVObject*))); - if (obj3 != NULL) - disconnect(obj3,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updateNeedle3(UAVObject*))); + QString object2, QString nfield2, + QString object3, QString nfield3) +{ + if (obj1 != NULL) { + disconnect(obj1, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateNeedle1(UAVObject *))); + } + if (obj2 != NULL) { + disconnect(obj2, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateNeedle2(UAVObject *))); + } + if (obj3 != NULL) { + disconnect(obj3, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateNeedle3(UAVObject *))); + } ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); // Check validity of arguments first, reject empty args and unknown fields. if (!(object1.isEmpty() || nfield1.isEmpty())) { - obj1 = dynamic_cast( objManager->getObject(object1) ); - if (obj1 != NULL ) { + obj1 = dynamic_cast(objManager->getObject(object1)); + if (obj1 != NULL) { // qDebug() << "Connected Object 1 (" << object1 << ")."; - connect(obj1, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateNeedle1(UAVObject*))); - if(nfield1.contains("-")) - { + connect(obj1, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateNeedle1(UAVObject *))); + if (nfield1.contains("-")) { QStringList fieldSubfield = nfield1.split("-", QString::SkipEmptyParts); - field1 = fieldSubfield.at(0); - subfield1 = fieldSubfield.at(1); + field1 = fieldSubfield.at(0); + subfield1 = fieldSubfield.at(1); haveSubField1 = true; - } - else - { - field1= nfield1; + } else { + field1 = nfield1; haveSubField1 = false; } } else { @@ -120,20 +122,17 @@ void DialGadgetWidget::connectNeedles(QString object1, QString nfield1, // And do the same for the second needle. if (!(object2.isEmpty() || nfield2.isEmpty())) { - obj2 = dynamic_cast( objManager->getObject(object2) ); - if (obj2 != NULL ) { + obj2 = dynamic_cast(objManager->getObject(object2)); + if (obj2 != NULL) { // qDebug() << "Connected Object 2 (" << object2 << ")."; - connect(obj2, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateNeedle2(UAVObject*))); - if(nfield2.contains("-")) - { + connect(obj2, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateNeedle2(UAVObject *))); + if (nfield2.contains("-")) { QStringList fieldSubfield = nfield2.split("-", QString::SkipEmptyParts); - field2 = fieldSubfield.at(0); - subfield2 = fieldSubfield.at(1); + field2 = fieldSubfield.at(0); + subfield2 = fieldSubfield.at(1); haveSubField2 = true; - } - else - { - field2= nfield2; + } else { + field2 = nfield2; haveSubField2 = false; } } else { @@ -143,20 +142,17 @@ void DialGadgetWidget::connectNeedles(QString object1, QString nfield1, // And do the same for the third needle. if (!(object3.isEmpty() || nfield3.isEmpty())) { - obj3 = dynamic_cast( objManager->getObject(object3) ); - if (obj3 != NULL ) { + obj3 = dynamic_cast(objManager->getObject(object3)); + if (obj3 != NULL) { // qDebug() << "Connected Object 3 (" << object3 << ")."; - connect(obj3, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateNeedle3(UAVObject*))); - if(nfield3.contains("-")) - { + connect(obj3, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateNeedle3(UAVObject *))); + if (nfield3.contains("-")) { QStringList fieldSubfield = nfield3.split("-", QString::SkipEmptyParts); - field3 = fieldSubfield.at(0); - subfield3 = fieldSubfield.at(1); + field3 = fieldSubfield.at(0); + subfield3 = fieldSubfield.at(1); haveSubField3 = true; - } - else - { - field3= nfield3; + } else { + field3 = nfield3; haveSubField3 = false; } } else { @@ -166,18 +162,21 @@ void DialGadgetWidget::connectNeedles(QString object1, QString nfield1, } /*! - \brief Called by the UAVObject which got updated - */ -void DialGadgetWidget::updateNeedle1(UAVObject *object1) { + \brief Called by the UAVObject which got updated + */ +void DialGadgetWidget::updateNeedle1(UAVObject *object1) +{ // Double check that the field exists: double value; - UAVObjectField* field = object1->getField(field1); + UAVObjectField *field = object1->getField(field1); + if (field) { - if(haveSubField1){ + if (haveSubField1) { int indexOfSubField = field->getElementNames().indexOf(QRegExp(subfield1, Qt::CaseSensitive, QRegExp::FixedString)); value = field->getDouble(indexOfSubField); - } else + } else { value = field->getDouble(); + } if (value != value) { qDebug() << "Dial widget: encountered NaN !!"; return; @@ -189,17 +188,20 @@ void DialGadgetWidget::updateNeedle1(UAVObject *object1) { } /*! - \brief Called by the UAVObject which got updated - */ -void DialGadgetWidget::updateNeedle2(UAVObject *object2) { + \brief Called by the UAVObject which got updated + */ +void DialGadgetWidget::updateNeedle2(UAVObject *object2) +{ double value; - UAVObjectField* field = object2->getField(field2); + UAVObjectField *field = object2->getField(field2); + if (field) { - if(haveSubField2){ + if (haveSubField2) { int indexOfSubField = field->getElementNames().indexOf(QRegExp(subfield2, Qt::CaseSensitive, QRegExp::FixedString)); value = field->getDouble(indexOfSubField); - } else + } else { value = field->getDouble(); + } if (value != value) { qDebug() << "Dial widget: encountered NaN !!"; return; @@ -211,17 +213,20 @@ void DialGadgetWidget::updateNeedle2(UAVObject *object2) { } /*! - \brief Called by the UAVObject which got updated - */ -void DialGadgetWidget::updateNeedle3(UAVObject *object3) { + \brief Called by the UAVObject which got updated + */ +void DialGadgetWidget::updateNeedle3(UAVObject *object3) +{ double value; - UAVObjectField* field = object3->getField(field3); + UAVObjectField *field = object3->getField(field3); + if (field) { - if(haveSubField3){ + if (haveSubField3) { int indexOfSubField = field->getElementNames().indexOf(QRegExp(subfield3, Qt::CaseSensitive, QRegExp::FixedString)); value = field->getDouble(indexOfSubField); - } else + } else { value = field->getDouble(); + } if (value != value) { qDebug() << "Dial widget: encountered NaN !!"; return; @@ -233,211 +238,207 @@ void DialGadgetWidget::updateNeedle3(UAVObject *object3) { } /* - Initializes the dial file, and does all the one-time calculations for - display later. This is the method which really initializes the dial. - */ + Initializes the dial file, and does all the one-time calculations for + display later. This is the method which really initializes the dial. + */ void DialGadgetWidget::setDialFile(QString dfn, QString bg, QString fg, QString n1, QString n2, - QString n3, QString n1Move, QString n2Move, QString n3Move) + QString n3, QString n1Move, QString n2Move, QString n3Move) { - fgenabled = false; - n2enabled = false; - n3enabled = false; - QGraphicsScene *l_scene = scene(); - setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); - if (QFile::exists(dfn) && m_renderer->load(dfn) && m_renderer->isValid()) - { - l_scene->clear(); // This also deletes all items contained in the scene. - m_background = new QGraphicsSvgItem(); - // All other items will be clipped to the shape of the background - m_background->setFlags(QGraphicsItem::ItemClipsChildrenToShape| - QGraphicsItem::ItemClipsToShape); - m_foreground = new QGraphicsSvgItem(); - m_needle1 = new QGraphicsSvgItem(); - m_needle2 = new QGraphicsSvgItem(); - m_needle3 = new QGraphicsSvgItem(); - m_needle1->setParentItem(m_background); - m_needle2->setParentItem(m_background); - m_needle3->setParentItem(m_background); - m_foreground->setParentItem(m_background); + fgenabled = false; + n2enabled = false; + n3enabled = false; + QGraphicsScene *l_scene = scene(); + setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); + if (QFile::exists(dfn) && m_renderer->load(dfn) && m_renderer->isValid()) { + l_scene->clear(); // This also deletes all items contained in the scene. + m_background = new QGraphicsSvgItem(); + // All other items will be clipped to the shape of the background + m_background->setFlags(QGraphicsItem::ItemClipsChildrenToShape | + QGraphicsItem::ItemClipsToShape); + m_foreground = new QGraphicsSvgItem(); + m_needle1 = new QGraphicsSvgItem(); + m_needle2 = new QGraphicsSvgItem(); + m_needle3 = new QGraphicsSvgItem(); + m_needle1->setParentItem(m_background); + m_needle2->setParentItem(m_background); + m_needle3->setParentItem(m_background); + m_foreground->setParentItem(m_background); - // We assume the dial contains at least the background - // and needle1 - m_background->setSharedRenderer(m_renderer); - m_background->setElementId(bg); - l_scene->addItem(m_background); + // We assume the dial contains at least the background + // and needle1 + m_background->setSharedRenderer(m_renderer); + m_background->setElementId(bg); + l_scene->addItem(m_background); - m_needle1->setSharedRenderer(m_renderer); - m_needle1->setElementId(n1); - // Note: no need to add the item explicitely because it - // is done automatically since it's a child item of the - // background. - //l_scene->addItem(m_needle1); + m_needle1->setSharedRenderer(m_renderer); + m_needle1->setElementId(n1); + // Note: no need to add the item explicitely because it + // is done automatically since it's a child item of the + // background. + // l_scene->addItem(m_needle1); - // The dial gadget allows Needle1 and Needle2 to be - // the same element, for combined movement. Needle3 - // is always independent. - if (n1 == n2) { - m_needle2 = m_needle1; - n2enabled = true; - } else { - if (m_renderer->elementExists(n2)) { - m_needle2->setSharedRenderer(m_renderer); - m_needle2->setElementId(n2); - //l_scene->addItem(m_needle2); - n2enabled = true; + // The dial gadget allows Needle1 and Needle2 to be + // the same element, for combined movement. Needle3 + // is always independent. + if (n1 == n2) { + m_needle2 = m_needle1; + n2enabled = true; + } else { + if (m_renderer->elementExists(n2)) { + m_needle2->setSharedRenderer(m_renderer); + m_needle2->setElementId(n2); + // l_scene->addItem(m_needle2); + n2enabled = true; + } } - } - if (m_renderer->elementExists(n3)) { - m_needle3->setSharedRenderer(m_renderer); - m_needle3->setElementId(n3); - //l_scene->addItem(m_needle3); - n3enabled = true; - } + if (m_renderer->elementExists(n3)) { + m_needle3->setSharedRenderer(m_renderer); + m_needle3->setElementId(n3); + // l_scene->addItem(m_needle3); + n3enabled = true; + } - if (m_renderer->elementExists(fg)) { - m_foreground->setSharedRenderer(m_renderer); - m_foreground->setElementId(fg); - // Center it on the scene + if (m_renderer->elementExists(fg)) { + m_foreground->setSharedRenderer(m_renderer); + m_foreground->setElementId(fg); + // Center it on the scene + QRectF rectB = m_background->boundingRect(); + QRectF rectF = m_foreground->boundingRect(); + m_foreground->setPos(rectB.width() / 2 - rectF.width() / 2, rectB.height() / 2 - rectF.height() / 2); + // l_scene->addItem(m_foreground); + fgenabled = true; + } + + rotateN1 = false; + horizN1 = false; + vertN1 = false; + rotateN2 = false; + horizN2 = false; + vertN2 = false; + rotateN3 = false; + horizN3 = false; + vertN3 = false; + + // Now setup the rotation/translation settings: + // this is UGLY UGLY UGLY, sorry... + if (n1Move.contains("Rotate")) { + rotateN1 = true; + } else if (n1Move.contains("Horizontal")) { + horizN1 = true; + } else if (n1Move.contains("Vertical")) { + vertN1 = true; + } + + if (n2Move.contains("Rotate")) { + rotateN2 = true; + } else if (n2Move.contains("Horizontal")) { + horizN2 = true; + } else if (n2Move.contains("Vertical")) { + vertN2 = true; + } + + if (n3Move.contains("Rotate")) { + rotateN3 = true; + } else if (n3Move.contains("Horizontal")) { + horizN3 = true; + } else if (n3Move.contains("Vertical")) { + vertN3 = true; + } + + l_scene->setSceneRect(m_background->boundingRect()); + + // Now Initialize the center for all transforms of the dial needles to the + // center of the background: + // - Move the center of the needle to the center of the background. QRectF rectB = m_background->boundingRect(); - QRectF rectF = m_foreground->boundingRect(); - m_foreground->setPos(rectB.width()/2-rectF.width()/2,rectB.height()/2-rectF.height()/2); - //l_scene->addItem(m_foreground); - fgenabled = true; - } + QRectF rectN = m_needle1->boundingRect(); + m_needle1->setPos(rectB.width() / 2 - rectN.width() / 2, rectB.height() / 2 - rectN.height() / 2); + // - Put the transform origin point of the needle at its center. + m_needle1->setTransformOriginPoint(rectN.width() / 2, rectN.height() / 2); - rotateN1 = false; - horizN1 = false; - vertN1 = false; - rotateN2 = false; - horizN2 = false; - vertN2 = false; - rotateN3 = false; - horizN3 = false; - vertN3 = false; + // Check whether the dial also wants display the numeric value: + if (m_renderer->elementExists(n1 + "-text")) { + QMatrix textMatrix = m_renderer->matrixForElement(n1 + "-text"); + qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement(n1 + "-text")).x(); + qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement(n1 + "-text")).y(); + QTransform matrix; + matrix.translate(startX, startY); + m_text1 = new QGraphicsTextItem("0.00"); + m_text1->setDefaultTextColor(QColor("White")); + m_text1->setTransform(matrix, false); + l_scene->addItem(m_text1); + } else { + m_text1 = NULL; + } - // Now setup the rotation/translation settings: - // this is UGLY UGLY UGLY, sorry... - if (n1Move.contains("Rotate")) { - rotateN1 = true; - } else if (n1Move.contains("Horizontal")) { - horizN1 = true; - } else if (n1Move.contains("Vertical")) { - vertN1 = true; - } - if (n2Move.contains("Rotate")) { - rotateN2 = true; - } else if (n2Move.contains("Horizontal")) { - horizN2 = true; - } else if (n2Move.contains("Vertical")) { - vertN2 = true; - } + if ((n1 != n2) && n2enabled) { + // Only do it for needle2 if it is not the same as n1 + rectN = m_needle2->boundingRect(); + m_needle2->setPos(rectB.width() / 2 - rectN.width() / 2, rectB.height() / 2 - rectN.height() / 2); + m_needle2->setTransformOriginPoint(rectN.width() / 2, rectN.height() / 2); + // Check whether the dial also wants display the numeric value: + if (m_renderer->elementExists(n2 + "-text")) { + QMatrix textMatrix = m_renderer->matrixForElement(n2 + "-text"); + qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement(n2 + "-text")).x(); + qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement(n2 + "-text")).y(); + QTransform matrix; + matrix.translate(startX, startY); + m_text2 = new QGraphicsTextItem("0.00"); + m_text2->setDefaultTextColor(QColor("White")); + m_text2->setTransform(matrix, false); + l_scene->addItem(m_text2); + } else { + m_text2 = NULL; + } + } + if (n3enabled) { + rectN = m_needle3->boundingRect(); + m_needle3->setPos(rectB.width() / 2 - rectN.width() / 2, rectB.height() / 2 - rectN.height() / 2); + m_needle3->setTransformOriginPoint(rectN.width() / 2, rectN.height() / 2); + // Check whether the dial also wants display the numeric value: + if (m_renderer->elementExists(n3 + "-text")) { + QMatrix textMatrix = m_renderer->matrixForElement(n3 + "-text"); + qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement(n3 + "-text")).x(); + qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement(n3 + "-text")).y(); + QTransform matrix; + matrix.translate(startX, startY); + m_text3 = new QGraphicsTextItem("0.00"); + m_text3->setDefaultTextColor(QColor("White")); + m_text3->setTransform(matrix, false); + l_scene->addItem(m_text3); + } else { + m_text3 = NULL; + } + } - if (n3Move.contains("Rotate")) { - rotateN3 = true; - } else if (n3Move.contains("Horizontal")) { - horizN3 = true; - } else if (n3Move.contains("Vertical")) { - vertN3 = true; - } - - l_scene->setSceneRect(m_background->boundingRect()); - - // Now Initialize the center for all transforms of the dial needles to the - // center of the background: - // - Move the center of the needle to the center of the background. - QRectF rectB = m_background->boundingRect(); - QRectF rectN = m_needle1->boundingRect(); - m_needle1->setPos(rectB.width()/2-rectN.width()/2,rectB.height()/2-rectN.height()/2); - // - Put the transform origin point of the needle at its center. - m_needle1->setTransformOriginPoint(rectN.width()/2,rectN.height()/2); - - // Check whether the dial also wants display the numeric value: - if (m_renderer->elementExists(n1+"-text")) { - QMatrix textMatrix = m_renderer->matrixForElement(n1+"-text"); - qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement(n1+"-text")).x(); - qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement(n1+"-text")).y(); - QTransform matrix; - matrix.translate(startX,startY); - m_text1 = new QGraphicsTextItem("0.00"); - m_text1->setDefaultTextColor(QColor("White")); - m_text1->setTransform(matrix,false); - l_scene->addItem(m_text1); + // Last: we just loaded the dial file which is by default positioned on a "zero" value + // of the needles, so we have to reset the needle values too upon dial file loading, otherwise + // we would end up with an offset whenever we change a dial file and the needle value + // is not zero at that time. + needle1Value = 0; + needle2Value = 0; + needle3Value = 0; + if (!dialTimer.isActive()) { + dialTimer.start(); + } + dialError = false; } else { - m_text1 = NULL; + qDebug() << "no file: display default background."; + m_renderer->load(QString(":/dial/images/empty.svg")); + l_scene->clear(); // This also deletes all items contained in the scene. + m_background = new QGraphicsSvgItem(); + m_background->setSharedRenderer(m_renderer); + l_scene->addItem(m_background); + m_text1 = NULL; + m_text2 = NULL; + m_text3 = NULL; + m_needle1 = NULL; + m_needle2 = NULL; + m_needle3 = NULL; + dialError = true; } - - - if ((n1 != n2) && n2enabled) { - // Only do it for needle2 if it is not the same as n1 - rectN = m_needle2->boundingRect(); - m_needle2->setPos(rectB.width()/2-rectN.width()/2,rectB.height()/2-rectN.height()/2); - m_needle2->setTransformOriginPoint(rectN.width()/2,rectN.height()/2); - // Check whether the dial also wants display the numeric value: - if (m_renderer->elementExists(n2+"-text")) { - QMatrix textMatrix = m_renderer->matrixForElement(n2+"-text"); - qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement(n2+"-text")).x(); - qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement(n2+"-text")).y(); - QTransform matrix; - matrix.translate(startX,startY); - m_text2 = new QGraphicsTextItem("0.00"); - m_text2->setDefaultTextColor(QColor("White")); - m_text2->setTransform(matrix,false); - l_scene->addItem(m_text2); - } else { - m_text2 = NULL; - } - - } - if (n3enabled) { - rectN = m_needle3->boundingRect(); - m_needle3->setPos(rectB.width()/2-rectN.width()/2,rectB.height()/2-rectN.height()/2); - m_needle3->setTransformOriginPoint(rectN.width()/2,rectN.height()/2); - // Check whether the dial also wants display the numeric value: - if (m_renderer->elementExists(n3+"-text")) { - QMatrix textMatrix = m_renderer->matrixForElement(n3+"-text"); - qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement(n3+"-text")).x(); - qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement(n3+"-text")).y(); - QTransform matrix; - matrix.translate(startX,startY); - m_text3 = new QGraphicsTextItem("0.00"); - m_text3->setDefaultTextColor(QColor("White")); - m_text3->setTransform(matrix,false); - l_scene->addItem(m_text3); - } else { - m_text3 = NULL; - } - - } - - // Last: we just loaded the dial file which is by default positioned on a "zero" value - // of the needles, so we have to reset the needle values too upon dial file loading, otherwise - // we would end up with an offset whenever we change a dial file and the needle value - // is not zero at that time. - needle1Value = 0; - needle2Value = 0; - needle3Value = 0; - if (!dialTimer.isActive()) - dialTimer.start(); - dialError = false; - } - else - { - qDebug()<<"no file: display default background."; - m_renderer->load(QString(":/dial/images/empty.svg")); - l_scene->clear(); // This also deletes all items contained in the scene. - m_background = new QGraphicsSvgItem(); - m_background->setSharedRenderer(m_renderer); - l_scene->addItem(m_background); - m_text1 = NULL; - m_text2 = NULL; - m_text3 = NULL; - m_needle1 = NULL; - m_needle2 = NULL; - m_needle3 = NULL; - dialError = true; - } } void DialGadgetWidget::paint() @@ -448,11 +449,11 @@ void DialGadgetWidget::paint() void DialGadgetWidget::paintEvent(QPaintEvent *event) { // Skip painting until the dial file is loaded - if (! m_renderer->isValid()) { - qDebug()<<"Dial file not loaded, not rendering"; + if (!m_renderer->isValid()) { + qDebug() << "Dial file not loaded, not rendering"; return; } - QGraphicsView::paintEvent(event); + QGraphicsView::paintEvent(event); } // This event enables the dial to be dynamically resized @@ -461,12 +462,13 @@ void DialGadgetWidget::paintEvent(QPaintEvent *event) void DialGadgetWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - fitInView(m_background, Qt::KeepAspectRatio ); + fitInView(m_background, Qt::KeepAspectRatio); } void DialGadgetWidget::setDialFont(QString fontProps) { - QFont font = QFont("Arial",12); + QFont font = QFont("Arial", 12); + font.fromString(fontProps); if (m_text1) { m_text1->setFont(font); @@ -476,60 +478,65 @@ void DialGadgetWidget::setDialFont(QString fontProps) // Converts the value into an angle: // this enables smooth rotation in rotateNeedles below -void DialGadgetWidget::setNeedle1(double value) { +void DialGadgetWidget::setNeedle1(double value) +{ if (rotateN1) { - needle1Target = 360*(value*n1Factor)/(n1MaxValue-n1MinValue); + needle1Target = 360 * (value * n1Factor) / (n1MaxValue - n1MinValue); } if (horizN1) { - needle1Target = (value*n1Factor)/(n1MaxValue-n1MinValue); + needle1Target = (value * n1Factor) / (n1MaxValue - n1MinValue); } if (vertN1) { - needle1Target = (value*n1Factor)/(n1MaxValue-n1MinValue); + needle1Target = (value * n1Factor) / (n1MaxValue - n1MinValue); } - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); + } if (m_text1) { QString s; - s.sprintf("%.2f",value*n1Factor); + s.sprintf("%.2f", value * n1Factor); m_text1->setPlainText(s); } } -void DialGadgetWidget::setNeedle2(double value) { +void DialGadgetWidget::setNeedle2(double value) +{ if (rotateN2) { - needle2Target = 360*(value*n2Factor)/(n2MaxValue-n2MinValue); + needle2Target = 360 * (value * n2Factor) / (n2MaxValue - n2MinValue); } if (horizN2) { - needle2Target = (value*n2Factor)/(n2MaxValue-n2MinValue); + needle2Target = (value * n2Factor) / (n2MaxValue - n2MinValue); } if (vertN2) { - needle2Target = (value*n2Factor)/(n2MaxValue-n2MinValue); + needle2Target = (value * n2Factor) / (n2MaxValue - n2MinValue); } - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); + } if (m_text2) { QString s; - s.sprintf("%.2f",value*n2Factor); + s.sprintf("%.2f", value * n2Factor); m_text2->setPlainText(s); } - } -void DialGadgetWidget::setNeedle3(double value) { +void DialGadgetWidget::setNeedle3(double value) +{ if (rotateN3) { - needle3Target = 360*(value*n3Factor)/(n3MaxValue-n3MinValue); + needle3Target = 360 * (value * n3Factor) / (n3MaxValue - n3MinValue); } if (horizN3) { - needle3Target = (value*n3Factor)/(n3MaxValue-n3MinValue); + needle3Target = (value * n3Factor) / (n3MaxValue - n3MinValue); } if (vertN3) { - needle3Target = (value*n3Factor)/(n3MaxValue-n3MinValue); + needle3Target = (value * n3Factor) / (n3MaxValue - n3MinValue); } - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); + } if (m_text3) { QString s; - s.sprintf("%.2f",value*n3Factor); + s.sprintf("%.2f", value * n3Factor); m_text3->setPlainText(s); } } @@ -551,30 +558,30 @@ void DialGadgetWidget::rotateNeedles() int dialRun = 3; if (n2enabled) { double needle2Diff; - if (abs((needle2Value-needle2Target)*10) > 5 && beSmooth) { - needle2Diff =(needle2Target - needle2Value)/5; + if (abs((needle2Value - needle2Target) * 10) > 5 && beSmooth) { + needle2Diff = (needle2Target - needle2Value) / 5; } else { needle2Diff = needle2Target - needle2Value; dialRun--; } if (rotateN2) { - m_needle2->setRotation(m_needle2->rotation()+needle2Diff); + m_needle2->setRotation(m_needle2->rotation() + needle2Diff); } else { - QPointF opd = QPointF(0,0); - if (horizN2) { - opd = QPointF(needle2Diff,0); - } - if (vertN2) { - opd = QPointF(0,needle2Diff); - } - m_needle2->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), true); - // Since we have moved the needle, we need to move - // the transform origin point the opposite way - // so that it keeps rotating from the same point. - // (this is only useful if needle1 and needle2 are the - // same object, for combined movement such as attitude indicator). - QPointF oop = m_needle2->transformOriginPoint(); - m_needle2->setTransformOriginPoint(oop.x()-opd.x(),oop.y()-opd.y()); + QPointF opd = QPointF(0, 0); + if (horizN2) { + opd = QPointF(needle2Diff, 0); + } + if (vertN2) { + opd = QPointF(0, needle2Diff); + } + m_needle2->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), true); + // Since we have moved the needle, we need to move + // the transform origin point the opposite way + // so that it keeps rotating from the same point. + // (this is only useful if needle1 and needle2 are the + // same object, for combined movement such as attitude indicator). + QPointF oop = m_needle2->transformOriginPoint(); + m_needle2->setTransformOriginPoint(oop.x() - opd.x(), oop.y() - opd.y()); } needle2Value += needle2Diff; } else { @@ -583,56 +590,58 @@ void DialGadgetWidget::rotateNeedles() // We assume that needle1 always exists! double needle1Diff; - if ((abs((needle1Value-needle1Target)*10) > 5) && beSmooth) { - needle1Diff = (needle1Target - needle1Value)/5; + if ((abs((needle1Value - needle1Target) * 10) > 5) && beSmooth) { + needle1Diff = (needle1Target - needle1Value) / 5; } else { needle1Diff = needle1Target - needle1Value; dialRun--; } if (rotateN1) { - m_needle1->setRotation(m_needle1->rotation()+needle1Diff); + m_needle1->setRotation(m_needle1->rotation() + needle1Diff); } else { - QPointF opd = QPointF(0,0); + QPointF opd = QPointF(0, 0); if (horizN1) { - opd = QPointF(needle1Diff,0); + opd = QPointF(needle1Diff, 0); } if (vertN1) { - opd = QPointF(0,needle1Diff); + opd = QPointF(0, needle1Diff); } - m_needle1->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), true); + m_needle1->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), true); QPointF oop = m_needle1->transformOriginPoint(); - m_needle1->setTransformOriginPoint((oop.x()-opd.x()),(oop.y()-opd.y())); + m_needle1->setTransformOriginPoint((oop.x() - opd.x()), (oop.y() - opd.y())); } needle1Value += needle1Diff; - if (n3enabled) { - double needle3Diff; - if ((abs((needle3Value-needle3Target)*10) > 5) && beSmooth) { - needle3Diff = (needle3Target - needle3Value)/5; - } else { - needle3Diff = needle3Target - needle3Value; - dialRun--; - } - if (rotateN3) { - m_needle3->setRotation(m_needle3->rotation()+needle3Diff); - } else { - QPointF opd = QPointF(0,0); + if (n3enabled) { + double needle3Diff; + if ((abs((needle3Value - needle3Target) * 10) > 5) && beSmooth) { + needle3Diff = (needle3Target - needle3Value) / 5; + } else { + needle3Diff = needle3Target - needle3Value; + dialRun--; + } + if (rotateN3) { + m_needle3->setRotation(m_needle3->rotation() + needle3Diff); + } else { + QPointF opd = QPointF(0, 0); if (horizN3) { - opd = QPointF(needle3Diff,0); + opd = QPointF(needle3Diff, 0); } - if (vertN3) { - opd = QPointF(0,needle3Diff); - } - m_needle3->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), true); - QPointF oop = m_needle3->transformOriginPoint(); - m_needle3->setTransformOriginPoint((oop.x()-opd.x()),(oop.y()-opd.y())); - } - needle3Value += needle3Diff; + if (vertN3) { + opd = QPointF(0, needle3Diff); + } + m_needle3->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), true); + QPointF oop = m_needle3->transformOriginPoint(); + m_needle3->setTransformOriginPoint((oop.x() - opd.x()), (oop.y() - opd.y())); + } + needle3Value += needle3Diff; } else { dialRun--; } // Now check: if dialRun is now zero, we should // just stop the timer since all needles have finished moving - if (!dialRun) dialTimer.stop(); + if (!dialRun) { + dialTimer.stop(); + } } diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.h b/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.h index 3fbc0415c..4bfddd747 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.h @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -40,114 +40,143 @@ #include #include -class DialGadgetWidget : public QGraphicsView -{ +class DialGadgetWidget : public QGraphicsView { Q_OBJECT public: DialGadgetWidget(QWidget *parent = 0); - ~DialGadgetWidget(); - void enableOpenGL(bool flag); - void enableSmoothUpdates(bool flag) { beSmooth = flag; } - void setDialFile(QString dfn, QString bg, QString fg, QString n1, QString n2, QString n3, - QString n1Move, QString n2Move, QString n3Move); - void paint(); + ~DialGadgetWidget(); + void enableOpenGL(bool flag); + void enableSmoothUpdates(bool flag) + { + beSmooth = flag; + } + void setDialFile(QString dfn, QString bg, QString fg, QString n1, QString n2, QString n3, + QString n1Move, QString n2Move, QString n3Move); + void paint(); // setNeedle1 and setNeedle2 use a timer to simulate // needle inertia - void setNeedle1(double value); - void setNeedle2(double value); - void setNeedle3(double value); - void setN1Min(double value) {n1MinValue = value;} - void setN1Max(double value) {n1MaxValue = value;} - void setN1Factor(double value) {n1Factor = value;} - void setN2Min(double value) {n2MinValue = value;} - void setN2Max(double value) {n2MaxValue = value;} - void setN2Factor(double value) {n2Factor = value;} - void setN3Min(double value) {n3MinValue = value;} - void setN3Max(double value) {n3MaxValue = value;} - void setN3Factor(double value) {n3Factor = value;} - // Sets up needle/UAVObject connections: - void connectNeedles(QString object1, QString field1, - QString object2, QString field2, - QString object3, QString field3); - void setDialFont(QString fontProps); + void setNeedle1(double value); + void setNeedle2(double value); + void setNeedle3(double value); + void setN1Min(double value) + { + n1MinValue = value; + } + void setN1Max(double value) + { + n1MaxValue = value; + } + void setN1Factor(double value) + { + n1Factor = value; + } + void setN2Min(double value) + { + n2MinValue = value; + } + void setN2Max(double value) + { + n2MaxValue = value; + } + void setN2Factor(double value) + { + n2Factor = value; + } + void setN3Min(double value) + { + n3MinValue = value; + } + void setN3Max(double value) + { + n3MaxValue = value; + } + void setN3Factor(double value) + { + n3Factor = value; + } + // Sets up needle/UAVObject connections: + void connectNeedles(QString object1, QString field1, + QString object2, QString field2, + QString object3, QString field3); + void setDialFont(QString fontProps); public slots: - void updateNeedle1(UAVObject *object1); // Called by the UAVObject - void updateNeedle2(UAVObject *object2); // Called by the UAVObject - void updateNeedle3(UAVObject *object3); // Called by the UAVObject + void updateNeedle1(UAVObject *object1); // Called by the UAVObject + void updateNeedle2(UAVObject *object2); // Called by the UAVObject + void updateNeedle3(UAVObject *object3); // Called by the UAVObject protected: - void paintEvent(QPaintEvent *event); - void resizeEvent(QResizeEvent *event); + void paintEvent(QPaintEvent *event); + void resizeEvent(QResizeEvent *event); private slots: - void rotateNeedles(); + void rotateNeedles(); private: - QSvgRenderer *m_renderer; - QGraphicsSvgItem *m_background; - QGraphicsSvgItem *m_foreground; - QGraphicsSvgItem *m_needle1; - QGraphicsSvgItem *m_needle2; - QGraphicsSvgItem *m_needle3; - QGraphicsTextItem *m_text1; - QGraphicsTextItem *m_text2; - QGraphicsTextItem *m_text3; + QSvgRenderer *m_renderer; + QGraphicsSvgItem *m_background; + QGraphicsSvgItem *m_foreground; + QGraphicsSvgItem *m_needle1; + QGraphicsSvgItem *m_needle2; + QGraphicsSvgItem *m_needle3; + QGraphicsTextItem *m_text1; + QGraphicsTextItem *m_text2; + QGraphicsTextItem *m_text3; - bool n3enabled; - bool n2enabled; // Simple flag to skip rendering if the - bool fgenabled; // layer does not exist. - bool dialError ; + bool n3enabled; + bool n2enabled; // Simple flag to skip rendering if the + bool fgenabled; // layer does not exist. + bool dialError; - // Settings concerning move of the dials - bool rotateN1; - bool rotateN2; - bool rotateN3; - bool horizN1; - bool horizN2; - bool horizN3; - bool vertN1; - bool vertN2; - bool vertN3; + // Settings concerning move of the dials + bool rotateN1; + bool rotateN2; + bool rotateN3; + bool horizN1; + bool horizN2; + bool horizN3; + bool vertN1; + bool vertN2; + bool vertN3; - double n1MinValue; - double n1MaxValue; - double n1Factor; - double n2MinValue; - double n2MaxValue; - double n2Factor; - double n3MinValue; - double n3MaxValue; - double n3Factor; + double n1MinValue; + double n1MaxValue; + double n1Factor; + double n2MinValue; + double n2MaxValue; + double n2Factor; + double n3MinValue; + double n3MaxValue; + double n3Factor; - // The Value and target variables - // are expressed in degrees - double needle1Target; - double needle1Value; - double needle2Target; - double needle2Value; - double needle3Target; - double needle3Value; + // The Value and target variables + // are expressed in degrees + double needle1Target; + double needle1Value; + double needle2Target; + double needle2Value; + double needle3Target; + double needle3Value; - // Name of the fields to read when an update is received: - UAVDataObject* obj1; - UAVDataObject* obj2; - UAVDataObject* obj3; - QString field1; - QString subfield1; - bool haveSubField1; - QString field2; - QString subfield2; - bool haveSubField2; - QString field3; - QString subfield3; - bool haveSubField3; + // Name of the fields to read when an update is received: + UAVDataObject *obj1; + UAVDataObject *obj2; + UAVDataObject *obj3; + QString field1; + QString subfield1; + bool haveSubField1; + QString field2; + QString subfield2; + bool haveSubField2; + QString field3; + QString subfield3; + bool haveSubField3; - // Rotation timer - QTimer dialTimer; + // Rotation timer + QTimer dialTimer; - bool beSmooth; + bool beSmooth; }; #endif /* DIALGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/dial/dialplugin.cpp b/ground/openpilotgcs/src/plugins/dial/dialplugin.cpp index c56b0f52d..4fe6d3533 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialplugin.cpp +++ b/ground/openpilotgcs/src/plugins/dial/dialplugin.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -36,32 +36,31 @@ DialPlugin::DialPlugin() { - // Do nothing + // Do nothing } DialPlugin::~DialPlugin() { - // Do nothing + // Do nothing } -bool DialPlugin::initialize(const QStringList& args, QString *errMsg) +bool DialPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new DialGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new DialGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void DialPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void DialPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(DialPlugin) - diff --git a/ground/openpilotgcs/src/plugins/dial/dialplugin.h b/ground/openpilotgcs/src/plugins/dial/dialplugin.h index 2014818b2..8d5231571 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialplugin.h +++ b/ground/openpilotgcs/src/plugins/dial/dialplugin.h @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,16 +33,15 @@ class DialGadgetFactory; -class DialPlugin : public ExtensionSystem::IPlugin -{ +class DialPlugin : public ExtensionSystem::IPlugin { public: - DialPlugin(); - ~DialPlugin(); + DialPlugin(); + ~DialPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - DialGadgetFactory *mf; + DialGadgetFactory *mf; }; #endif /* DIALPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/donothing/donothingplugin.cpp b/ground/openpilotgcs/src/plugins/donothing/donothingplugin.cpp index f748c6390..56172c451 100644 --- a/ground/openpilotgcs/src/plugins/donothing/donothingplugin.cpp +++ b/ground/openpilotgcs/src/plugins/donothing/donothingplugin.cpp @@ -7,52 +7,52 @@ * @{ * @addtogroup DoNothingPlugin Do Nothing Plugin * @{ - * @brief A minimal example plugin + * @brief A minimal example plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "donothingplugin.h" -#include -#include +#include "donothingplugin.h" +#include +#include -DoNothingPlugin::DoNothingPlugin() -{ - // Do nothing -} +DoNothingPlugin::DoNothingPlugin() +{ + // Do nothing +} -DoNothingPlugin::~DoNothingPlugin() -{ - // Do nothing -} +DoNothingPlugin::~DoNothingPlugin() +{ + // Do nothing +} -bool DoNothingPlugin::initialize(const QStringList& args, QString *errMsg) -{ - Q_UNUSED(args); - Q_UNUSED(errMsg); +bool DoNothingPlugin::initialize(const QStringList & args, QString *errMsg) +{ + Q_UNUSED(args); + Q_UNUSED(errMsg); - return true; -} + return true; +} -void DoNothingPlugin::extensionsInitialized() -{ - // Do nothing -} +void DoNothingPlugin::extensionsInitialized() +{ + // Do nothing +} -void DoNothingPlugin::shutdown() -{ - // Do nothing +void DoNothingPlugin::shutdown() +{ + // Do nothing } Q_EXPORT_PLUGIN(DoNothingPlugin) diff --git a/ground/openpilotgcs/src/plugins/donothing/donothingplugin.h b/ground/openpilotgcs/src/plugins/donothing/donothingplugin.h index 5505a6c1a..60ff00d89 100644 --- a/ground/openpilotgcs/src/plugins/donothing/donothingplugin.h +++ b/ground/openpilotgcs/src/plugins/donothing/donothingplugin.h @@ -7,37 +7,36 @@ * @{ * @addtogroup DoNothingPlugin Do Nothing Plugin * @{ - * @brief A minimal example plugin + * @brief A minimal example plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef DONOTHINGPLUGIN_H -#define DONOTHINGPLUGIN_H +#ifndef DONOTHINGPLUGIN_H +#define DONOTHINGPLUGIN_H -#include +#include -class DoNothingPlugin : public ExtensionSystem::IPlugin -{ -public: - DoNothingPlugin(); - ~DoNothingPlugin(); +class DoNothingPlugin : public ExtensionSystem::IPlugin { +public: + DoNothingPlugin(); + ~DoNothingPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); -}; + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); +}; -#endif // DONOTHINGPLUGIN_H \ No newline at end of file +#endif // DONOTHINGPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.cpp b/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.cpp index 8146d433b..5bbfdca2d 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.cpp +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -28,10 +28,9 @@ #include "emptygadgetwidget.h" EmptyGadget::EmptyGadget(QString classId, EmptyGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} EmptyGadget::~EmptyGadget() { diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.h b/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.h index a5b5dabb5..a9a5c0a26 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.h +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,26 +33,34 @@ namespace Core { class IUAVGadget; } -//class QWidget; -//class QString; +// class QWidget; +// class QString; class EmptyGadgetWidget; using namespace Core; -class EmptyGadget : public Core::IUAVGadget -{ +class EmptyGadget : public Core::IUAVGadget { Q_OBJECT public: EmptyGadget(QString classId, EmptyGadgetWidget *widget, QWidget *parent = 0); ~EmptyGadget(); - QList context() const { return m_context; } - QWidget *widget() { return m_widget; } - QString contextHelpId() const { return QString(); } + QList context() const + { + return m_context; + } + QWidget *widget() + { + return m_widget; + } + QString contextHelpId() const + { + return QString(); + } private: - QWidget *m_widget; - QList m_context; + QWidget *m_widget; + QList m_context; }; diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.cpp b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.cpp index 9f8db2bc9..8a5e6e1f7 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,18 +30,17 @@ #include EmptyGadgetFactory::EmptyGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("EmptyGadget"), - tr("Choose Gadget..."), - parent) -{ -} + IUAVGadgetFactory(QString("EmptyGadget"), + tr("Choose Gadget..."), + parent) +{} EmptyGadgetFactory::~EmptyGadgetFactory() +{} + +IUAVGadget *EmptyGadgetFactory::createGadget(QWidget *parent) { + EmptyGadgetWidget *gadgetWidget = new EmptyGadgetWidget(parent); -} - -IUAVGadget* EmptyGadgetFactory::createGadget(QWidget *parent) { - EmptyGadgetWidget* gadgetWidget = new EmptyGadgetWidget(parent); return new EmptyGadget(QString("EmptyGadget"), gadgetWidget, parent); } diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.h b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.h index 561d33fc6..84bd95138 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,8 +37,7 @@ class IUAVGadgetFactory; using namespace Core; -class EmptyGadgetFactory : public IUAVGadgetFactory -{ +class EmptyGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: EmptyGadgetFactory(QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.cpp b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.cpp index 48ff18dea..8df5f0601 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,7 +35,7 @@ EmptyGadgetWidget::EmptyGadgetWidget(QWidget *parent) : QLabel(parent) { - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); this-> setText(tr("Choose a gadget to display in this view.\n") + @@ -45,6 +45,5 @@ EmptyGadgetWidget::EmptyGadgetWidget(QWidget *parent) : QLabel(parent) EmptyGadgetWidget::~EmptyGadgetWidget() { - // Do nothing + // Do nothing } - diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.h b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.h index 7ee0bd326..737846899 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,13 +30,12 @@ #include -class EmptyGadgetWidget : public QLabel -{ +class EmptyGadgetWidget : public QLabel { Q_OBJECT public: EmptyGadgetWidget(QWidget *parent = 0); - ~EmptyGadgetWidget(); + ~EmptyGadgetWidget(); private: }; diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.cpp b/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.cpp index 63a00964f..23f6f8038 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.cpp +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,32 +34,31 @@ EmptyPlugin::EmptyPlugin() { - // Do nothing + // Do nothing } EmptyPlugin::~EmptyPlugin() { - // Do nothing + // Do nothing } -bool EmptyPlugin::initialize(const QStringList& args, QString *errMsg) +bool EmptyPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new EmptyGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new EmptyGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void EmptyPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void EmptyPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(EmptyPlugin) - diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.h b/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.h index 71aea3285..4baa5e1d8 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.h +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class EmptyGadgetFactory; -class EmptyPlugin : public ExtensionSystem::IPlugin -{ +class EmptyPlugin : public ExtensionSystem::IPlugin { public: - EmptyPlugin(); - ~EmptyPlugin(); + EmptyPlugin(); + ~EmptyPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - EmptyGadgetFactory *mf; + EmptyGadgetFactory *mf; }; #endif /* EMPTYPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp index b8cea0d8c..ce2629bba 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp @@ -35,25 +35,24 @@ #define JOYSTICK_UPDATE_RATE 50 GCSControlGadget::GCSControlGadget(QString classId, GCSControlGadgetWidget *widget, QWidget *parent, QObject *plugin) : - IUAVGadget(classId, parent), - m_widget(widget) + IUAVGadget(classId, parent), + m_widget(widget) { - connect(getManualControlCommand(),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(manualControlCommandUpdated(UAVObject*))); - connect(widget,SIGNAL(sticksChanged(double,double,double,double)),this,SLOT(sticksChangedLocally(double,double,double,double))); - connect(this,SIGNAL(sticksChangedRemotely(double,double,double,double)),widget,SLOT(updateSticks(double,double,double,double))); + connect(getManualControlCommand(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(manualControlCommandUpdated(UAVObject *))); + connect(widget, SIGNAL(sticksChanged(double, double, double, double)), this, SLOT(sticksChangedLocally(double, double, double, double))); + connect(this, SIGNAL(sticksChangedRemotely(double, double, double, double)), widget, SLOT(updateSticks(double, double, double, double))); manualControlCommandUpdated(getManualControlCommand()); control_sock = new QUdpSocket(this); - connect(control_sock,SIGNAL(readyRead()),this,SLOT(readUDPCommand())); + connect(control_sock, SIGNAL(readyRead()), this, SLOT(readUDPCommand())); joystickTime.start(); - GCSControlPlugin *pl = dynamic_cast(plugin); - connect(pl->sdlGamepad,SIGNAL(gamepads(quint8)),this,SLOT(gamepads(quint8))); - connect(pl->sdlGamepad,SIGNAL(buttonState(ButtonNumber,bool)),this,SLOT(buttonState(ButtonNumber,bool))); - connect(pl->sdlGamepad,SIGNAL(axesValues(QListInt16)),this,SLOT(axesValues(QListInt16))); - + GCSControlPlugin *pl = dynamic_cast(plugin); + connect(pl->sdlGamepad, SIGNAL(gamepads(quint8)), this, SLOT(gamepads(quint8))); + connect(pl->sdlGamepad, SIGNAL(buttonState(ButtonNumber, bool)), this, SLOT(buttonState(ButtonNumber, bool))); + connect(pl->sdlGamepad, SIGNAL(axesValues(QListInt16)), this, SLOT(axesValues(QListInt16))); } GCSControlGadget::~GCSControlGadget() @@ -61,47 +60,48 @@ GCSControlGadget::~GCSControlGadget() delete m_widget; } -void GCSControlGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void GCSControlGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - GCSControlGadgetConfiguration *GCSControlConfig = qobject_cast< GCSControlGadgetConfiguration*>(config); + GCSControlGadgetConfiguration *GCSControlConfig = qobject_cast< GCSControlGadgetConfiguration *>(config); QList ql = GCSControlConfig->getChannelsMapping(); - rollChannel = ql.at(0); - pitchChannel = ql.at(1); - yawChannel = ql.at(2); + rollChannel = ql.at(0); + pitchChannel = ql.at(1); + yawChannel = ql.at(2); throttleChannel = ql.at(3); - // if(control_sock->isOpen()) - // control_sock->close(); - control_sock->bind(GCSControlConfig->getUDPControlHost(), GCSControlConfig->getUDPControlPort(),QUdpSocket::ShareAddress); + // if(control_sock->isOpen()) + // control_sock->close(); + control_sock->bind(GCSControlConfig->getUDPControlHost(), GCSControlConfig->getUDPControlPort(), QUdpSocket::ShareAddress); controlsMode = GCSControlConfig->getControlsMode(); int i; - for (i=0;i<8;i++) - { - buttonSettings[i].ActionID=GCSControlConfig->getbuttonSettings(i).ActionID; - buttonSettings[i].FunctionID=GCSControlConfig->getbuttonSettings(i).FunctionID; - buttonSettings[i].Amount=GCSControlConfig->getbuttonSettings(i).Amount; - buttonSettings[i].Amount=GCSControlConfig->getbuttonSettings(i).Amount; - channelReverse[i]=GCSControlConfig->getChannelsReverse().at(i); + for (i = 0; i < 8; i++) { + buttonSettings[i].ActionID = GCSControlConfig->getbuttonSettings(i).ActionID; + buttonSettings[i].FunctionID = GCSControlConfig->getbuttonSettings(i).FunctionID; + buttonSettings[i].Amount = GCSControlConfig->getbuttonSettings(i).Amount; + buttonSettings[i].Amount = GCSControlConfig->getbuttonSettings(i).Amount; + channelReverse[i] = GCSControlConfig->getChannelsReverse().at(i); } - } -ManualControlCommand* GCSControlGadget::getManualControlCommand() { +ManualControlCommand *GCSControlGadget::getManualControlCommand() +{ ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - return dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); + + return dynamic_cast(objManager->getObject(QString("ManualControlCommand"))); } -void GCSControlGadget::manualControlCommandUpdated(UAVObject * obj) { - double roll = obj->getField("Roll")->getDouble(); - double pitch = obj->getField("Pitch")->getDouble(); - double yaw = obj->getField("Yaw")->getDouble(); +void GCSControlGadget::manualControlCommandUpdated(UAVObject *obj) +{ + double roll = obj->getField("Roll")->getDouble(); + double pitch = obj->getField("Pitch")->getDouble(); + double yaw = obj->getField("Yaw")->getDouble(); double throttle = obj->getField("Throttle")->getDouble(); - // necessary against having the wrong joystick profile chosen, which shows weird values + // necessary against having the wrong joystick profile chosen, which shows weird values if (throttle > -1.0 && throttle <= 1.0) { // convert ManualControlCommand.Throttle range (0..1) to the widget's throttle stick range (-1..+1) throttle = -1.0 + (throttle * 2.0); @@ -118,31 +118,32 @@ void GCSControlGadget::manualControlCommandUpdated(UAVObject * obj) { switch (controlsMode) { case 1: // Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle - emit sticksChangedRemotely(yaw,-pitch,roll,throttle); + emit sticksChangedRemotely(yaw, -pitch, roll, throttle); break; case 2: // Mode 2: LeftX = Yaw, LeftY = Throttle, RightX = Roll, RightY = Pitch - emit sticksChangedRemotely(yaw,throttle,roll,-pitch); + emit sticksChangedRemotely(yaw, throttle, roll, -pitch); break; case 3: // Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle - emit sticksChangedRemotely(roll,-pitch,yaw,throttle); + emit sticksChangedRemotely(roll, -pitch, yaw, throttle); break; case 4: // Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch; - emit sticksChangedRemotely(roll,throttle,yaw,-pitch); + emit sticksChangedRemotely(roll, throttle, yaw, -pitch); break; } } /** - Update the manual commands - maps depending on mode - */ -void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double rightX, double rightY) { - ManualControlCommand * obj = getManualControlCommand(); - double oldRoll = obj->getField("Roll")->getDouble(); - double oldPitch = obj->getField("Pitch")->getDouble(); - double oldYaw = obj->getField("Yaw")->getDouble(); + Update the manual commands - maps depending on mode + */ +void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double rightX, double rightY) +{ + ManualControlCommand *obj = getManualControlCommand(); + double oldRoll = obj->getField("Roll")->getDouble(); + double oldPitch = obj->getField("Pitch")->getDouble(); + double oldYaw = obj->getField("Yaw")->getDouble(); double oldThrottle = obj->getField("Throttle")->getDouble(); double newRoll; @@ -154,67 +155,83 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r switch (controlsMode) { case 1: // Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle - newRoll = rightX; - newPitch = -leftY; - newYaw = leftX; + newRoll = rightX; + newPitch = -leftY; + newYaw = leftX; newThrottle = rightY; break; case 2: // Mode 2: LeftX = Yaw, LeftY = Throttle, RightX = Roll, RightY = Pitch - newRoll = rightX; - newPitch = -rightY; - newYaw = leftX; + newRoll = rightX; + newPitch = -rightY; + newYaw = leftX; newThrottle = leftY; break; case 3: // Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle - newRoll = leftX; - newPitch = -leftY; - newYaw = rightX; + newRoll = leftX; + newPitch = -leftY; + newYaw = rightX; newThrottle = rightY; break; case 4: // Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch; - newRoll = leftX; - newPitch = -rightY; - newYaw = rightX; + newRoll = leftX; + newPitch = -rightY; + newYaw = rightX; newThrottle = leftY; break; } - //check if buttons have control over this axis... if so don't update it - int buttonRollControl=0; - int buttonPitchControl=0; - int buttonYawControl=0; - int buttonThrottleControl=0; - for (int i=0;i<8;i++) - { - if ((buttonSettings[i].FunctionID==1)&&((buttonSettings[i].ActionID==1)||(buttonSettings[i].ActionID==2)))buttonRollControl=1; - if ((buttonSettings[i].FunctionID==2)&&((buttonSettings[i].ActionID==1)||(buttonSettings[i].ActionID==2)))buttonPitchControl=1; - if ((buttonSettings[i].FunctionID==3)&&((buttonSettings[i].ActionID==1)||(buttonSettings[i].ActionID==2)))buttonYawControl=1; - if ((buttonSettings[i].FunctionID==4)&&((buttonSettings[i].ActionID==1)||(buttonSettings[i].ActionID==2)))buttonThrottleControl=1; + // check if buttons have control over this axis... if so don't update it + int buttonRollControl = 0; + int buttonPitchControl = 0; + int buttonYawControl = 0; + int buttonThrottleControl = 0; + for (int i = 0; i < 8; i++) { + if ((buttonSettings[i].FunctionID == 1) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) { + buttonRollControl = 1; + } + if ((buttonSettings[i].FunctionID == 2) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) { + buttonPitchControl = 1; + } + if ((buttonSettings[i].FunctionID == 3) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) { + buttonYawControl = 1; + } + if ((buttonSettings[i].FunctionID == 4) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) { + buttonThrottleControl = 1; + } } - //if we are not in local gcs control mode, ignore the joystick input - if (((GCSControlGadgetWidget *)m_widget)->getGCSControl()==false || ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) + // if we are not in local gcs control mode, ignore the joystick input + if (((GCSControlGadgetWidget *)m_widget)->getGCSControl() == false || ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) { return; + } - //if (newThrottle != oldThrottle) { - // convert widget's throttle stick range (-1..+1) to ManualControlCommand.Throttle range (0..1) - newThrottle = (newThrottle + 1.0) / 2.0; - - // safety value to stop the motors from spinning at 0% throttle - if (newThrottle <= 0.01 ) { - newThrottle = -1; + // if (newThrottle != oldThrottle) { + // convert widget's throttle stick range (-1..+1) to ManualControlCommand.Throttle range (0..1) + newThrottle = (newThrottle + 1.0) / 2.0; + + // safety value to stop the motors from spinning at 0% throttle + if (newThrottle <= 0.01) { + newThrottle = -1; + } + // } + + + if ((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) { + if (buttonRollControl == 0) { + obj->getField("Roll")->setDouble(newRoll); + } + if (buttonPitchControl == 0) { + obj->getField("Pitch")->setDouble(newPitch); + } + if (buttonYawControl == 0) { + obj->getField("Yaw")->setDouble(newYaw); + } + if (buttonThrottleControl == 0) { + obj->getField("Throttle")->setDouble(newThrottle); } - //} - - - if((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) { - if (buttonRollControl==0)obj->getField("Roll")->setDouble(newRoll); - if (buttonPitchControl==0)obj->getField("Pitch")->setDouble(newPitch); - if (buttonYawControl==0)obj->getField("Yaw")->setDouble(newYaw); - if (buttonThrottleControl==0)obj->getField("Throttle")->setDouble(newThrottle); obj->updated(); } } @@ -223,179 +240,168 @@ void GCSControlGadget::gamepads(quint8 count) { Q_UNUSED(count); -// sdlGamepad.setGamepad(0); -// sdlGamepad.setTickRate(JOYSTICK_UPDATE_RATE); +// sdlGamepad.setGamepad(0); +// sdlGamepad.setTickRate(JOYSTICK_UPDATE_RATE); } void GCSControlGadget::readUDPCommand() { double pitch, yaw, roll, throttle; + while (control_sock->hasPendingDatagrams()) { QByteArray datagram; datagram.resize(control_sock->pendingDatagramSize()); control_sock->readDatagram(datagram.data(), datagram.size()); QDataStream readData(datagram); bool badPack = false; - int state = 0; - while(!readData.atEnd() && !badPack) - { + int state = 0; + while (!readData.atEnd() && !badPack) { double buffer; readData >> buffer; - switch(state) - { + switch (state) { case 0: - if(buffer == 42){ + if (buffer == 42) { state = 1; - }else{ - state = 0; + } else { + state = 0; badPack = true; } break; case 1: - pitch = buffer; - state = 2; + pitch = buffer; + state = 2; break; case 2: - yaw = buffer; - state = 3; + yaw = buffer; + state = 3; break; case 3: - roll = buffer; - state = 4; + roll = buffer; + state = 4; break; case 4: throttle = buffer; - state = 5; + state = 5; break; case 5: - if(buffer != 36 || !readData.atEnd()) - badPack=true; + if (buffer != 36 || !readData.atEnd()) { + badPack = true; + } break; } - } - if(!badPack && ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) - { - ManualControlCommand * obj = getManualControlCommand(); - bool update = false; + if (!badPack && ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) { + ManualControlCommand *obj = getManualControlCommand(); + bool update = false; - if(pitch != obj->getField("Pitch")->getDouble()){ - obj->getField("Pitch")->setDouble(constrain(pitch)); - update = true; - } - if(yaw != obj->getField("Yaw")->getDouble()){ - obj->getField("Yaw")->setDouble(constrain(yaw)); - update = true; - } - if(roll != obj->getField("Roll")->getDouble()){ - obj->getField("Roll")->setDouble(constrain(roll)); - update = true; - } - if(throttle != obj->getField("Throttle")->getDouble()){ + if (pitch != obj->getField("Pitch")->getDouble()) { + obj->getField("Pitch")->setDouble(constrain(pitch)); + update = true; + } + if (yaw != obj->getField("Yaw")->getDouble()) { + obj->getField("Yaw")->setDouble(constrain(yaw)); + update = true; + } + if (roll != obj->getField("Roll")->getDouble()) { + obj->getField("Roll")->setDouble(constrain(roll)); + update = true; + } + if (throttle != obj->getField("Throttle")->getDouble()) { obj->getField("Throttle")->setDouble(constrain(throttle)); update = true; - } - if(update) - obj->updated(); + } + if (update) { + obj->updated(); + } } } qDebug() << "Pitch: " << pitch << " Yaw: " << yaw << " Roll: " << roll << " Throttle: " << throttle; - - } double GCSControlGadget::constrain(double value) { - if(value < -1) + if (value < -1) { return -1; - if(value > 1) + } + if (value > 1) { return 1; + } return value; } void GCSControlGadget::buttonState(ButtonNumber number, bool pressed) { - if ((buttonSettings[number].ActionID>0)&&(buttonSettings[number].FunctionID>0)&&(pressed)) - {//this button is configured + if ((buttonSettings[number].ActionID > 0) && (buttonSettings[number].FunctionID > 0) && (pressed)) { // this button is configured ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("ManualControlCommand"))); bool currentCGSControl = ((GCSControlGadgetWidget *)m_widget)->getGCSControl(); bool currentUDPControl = ((GCSControlGadgetWidget *)m_widget)->getUDPControl(); - switch (buttonSettings[number].ActionID) - { - case 1://increase - if (currentCGSControl) - { - switch (buttonSettings[number].FunctionID) - { - case 1://Roll - obj->getField("Roll")->setValue(bound(obj->getField("Roll")->getValue().toDouble()+buttonSettings[number].Amount)); + switch (buttonSettings[number].ActionID) { + case 1: // increase + if (currentCGSControl) { + switch (buttonSettings[number].FunctionID) { + case 1: // Roll + obj->getField("Roll")->setValue(bound(obj->getField("Roll")->getValue().toDouble() + buttonSettings[number].Amount)); break; - case 2://Pitch - obj->getField("Pitch")->setValue(bound(obj->getField("Pitch")->getValue().toDouble()+buttonSettings[number].Amount)); - break; - case 3://Yaw - obj->getField("Yaw")->setValue(wrap(obj->getField("Yaw")->getValue().toDouble()+buttonSettings[number].Amount)); + case 2: // Pitch + obj->getField("Pitch")->setValue(bound(obj->getField("Pitch")->getValue().toDouble() + buttonSettings[number].Amount)); break; - case 4://Throttle - obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble()+buttonSettings[number].Amount)); + case 3: // Yaw + obj->getField("Yaw")->setValue(wrap(obj->getField("Yaw")->getValue().toDouble() + buttonSettings[number].Amount)); + break; + case 4: // Throttle + obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble() + buttonSettings[number].Amount)); break; } } break; - case 2://decrease - if (currentCGSControl) - { - switch (buttonSettings[number].FunctionID) - { - case 1://Roll - obj->getField("Roll")->setValue(bound(obj->getField("Roll")->getValue().toDouble()-buttonSettings[number].Amount)); + case 2: // decrease + if (currentCGSControl) { + switch (buttonSettings[number].FunctionID) { + case 1: // Roll + obj->getField("Roll")->setValue(bound(obj->getField("Roll")->getValue().toDouble() - buttonSettings[number].Amount)); break; - case 2://Pitch - obj->getField("Pitch")->setValue(bound(obj->getField("Pitch")->getValue().toDouble()-buttonSettings[number].Amount)); - break; - case 3://Yaw - obj->getField("Yaw")->setValue(wrap(obj->getField("Yaw")->getValue().toDouble()-buttonSettings[number].Amount)); + case 2: // Pitch + obj->getField("Pitch")->setValue(bound(obj->getField("Pitch")->getValue().toDouble() - buttonSettings[number].Amount)); break; - case 4://Throttle - obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble()-buttonSettings[number].Amount)); + case 3: // Yaw + obj->getField("Yaw")->setValue(wrap(obj->getField("Yaw")->getValue().toDouble() - buttonSettings[number].Amount)); + break; + case 4: // Throttle + obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble() - buttonSettings[number].Amount)); break; } } break; - case 3://toggle - switch (buttonSettings[number].FunctionID) - { - case 1://Armed - if (currentCGSControl) - { - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); + case 3: // toggle + switch (buttonSettings[number].FunctionID) { + case 1: // Armed + if (currentCGSControl) { + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("FlightStatus"))); - if(obj->getField("Armed")->getValue().toString().compare("Armed")==0) - { - obj->getField("Armed")->setValue("Disarmed"); - } - else - { - obj->getField("Armed")->setValue("Armed"); - } + if (obj->getField("Armed")->getValue().toString().compare("Armed") == 0) { + obj->getField("Armed")->setValue("Disarmed"); + } else { + obj->getField("Armed")->setValue("Armed"); } + } break; - case 2://GCS Control - //Toggle the GCS Control checkbox, its built in signalling will handle the update to OP + case 2: // GCS Control + // Toggle the GCS Control checkbox, its built in signalling will handle the update to OP ((GCSControlGadgetWidget *)m_widget)->setGCSControl(!currentCGSControl); - break; - case 3: //UDP Control - if(currentCGSControl) - ((GCSControlGadgetWidget *)m_widget)->setUDPControl(!currentUDPControl); + break; + case 3: // UDP Control + if (currentCGSControl) { + ((GCSControlGadgetWidget *)m_widget)->setUDPControl(!currentUDPControl); + } - break; + break; } break; @@ -403,16 +409,17 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed) obj->updated(); } - //buttonSettings[number].ActionID NIDT - //buttonSettings[number].FunctionID -RPYTAC - //buttonSettings[number].Amount + // buttonSettings[number].ActionID NIDT + // buttonSettings[number].FunctionID -RPYTAC + // buttonSettings[number].Amount } void GCSControlGadget::axesValues(QListInt16 values) { int chMax = values.length(); + if (rollChannel >= chMax || pitchChannel >= chMax || - yawChannel >= chMax || throttleChannel >= chMax ) { + yawChannel >= chMax || throttleChannel >= chMax) { qDebug() << "GCSControl: configuration is inconsistent with current joystick! Aborting update."; return; } @@ -421,15 +428,31 @@ void GCSControlGadget::axesValues(QListInt16 values) double pValue = (pitchChannel > -1) ? values[pitchChannel] : 0; double yValue = (yawChannel > -1) ? values[yawChannel] : 0; double tValue = (throttleChannel > -1) ? values[throttleChannel] : 0; - double max = 32767; + double max = 32767; - if (rollChannel > -1) if(channelReverse[rollChannel]==true)rValue = -rValue; - if (pitchChannel > -1) if(channelReverse[pitchChannel]==true)pValue = -pValue; - if (yawChannel > -1) if(channelReverse[yawChannel]==true)yValue = -yValue; - if (throttleChannel > -1) if(channelReverse[throttleChannel]==true)tValue = -tValue; + if (rollChannel > -1) { + if (channelReverse[rollChannel] == true) { + rValue = -rValue; + } + } + if (pitchChannel > -1) { + if (channelReverse[pitchChannel] == true) { + pValue = -pValue; + } + } + if (yawChannel > -1) { + if (channelReverse[yawChannel] == true) { + yValue = -yValue; + } + } + if (throttleChannel > -1) { + if (channelReverse[throttleChannel] == true) { + tValue = -tValue; + } + } - if(joystickTime.elapsed() > JOYSTICK_UPDATE_RATE) { + if (joystickTime.elapsed() > JOYSTICK_UPDATE_RATE) { joystickTime.restart(); // Remap RPYT to left X/Y and right X/Y depending on mode // Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle @@ -438,16 +461,16 @@ void GCSControlGadget::axesValues(QListInt16 values) // Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch; switch (controlsMode) { case 1: - sticksChangedLocally(yValue/max,-pValue/max,rValue/max,-tValue/max); + sticksChangedLocally(yValue / max, -pValue / max, rValue / max, -tValue / max); break; case 2: - sticksChangedLocally(yValue/max,-tValue/max,rValue/max,-pValue/max); + sticksChangedLocally(yValue / max, -tValue / max, rValue / max, -pValue / max); break; case 3: - sticksChangedLocally(rValue/max,-pValue/max,yValue/max,-tValue/max); + sticksChangedLocally(rValue / max, -pValue / max, yValue / max, -tValue / max); break; case 4: - sticksChangedLocally(rValue/max,-tValue/max,yValue/max,-pValue/max); + sticksChangedLocally(rValue / max, -tValue / max, yValue / max, -pValue / max); break; } } @@ -456,14 +479,22 @@ void GCSControlGadget::axesValues(QListInt16 values) double GCSControlGadget::bound(double input) { - if (input > 1.0)return 1.0; - if (input <-1.0)return -1.0; + if (input > 1.0) { + return 1.0; + } + if (input < -1.0) { + return -1.0; + } return input; } double GCSControlGadget::wrap(double input) { - while (input > 1.0)input -= 2.0; - while (input <-1.0)input += 2.0; + while (input > 1.0) { + input -= 2.0; + } + while (input < -1.0) { + input += 2.0; + } return input; } diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h index 14808275c..2a25bcef4 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h @@ -41,27 +41,35 @@ namespace Core { class IUAVGadget; } -//class QWidget; -//class QString; +// class QWidget; +// class QString; class GCSControlGadgetWidget; using namespace Core; -class GCSControlGadget : public Core::IUAVGadget -{ +class GCSControlGadget : public Core::IUAVGadget { Q_OBJECT public: - GCSControlGadget(QString classId, GCSControlGadgetWidget *widget, QWidget *parent = 0, QObject *plugin=0); + GCSControlGadget(QString classId, GCSControlGadgetWidget *widget, QWidget *parent = 0, QObject *plugin = 0); ~GCSControlGadget(); - QList context() const { return m_context; } - QWidget *widget() { return m_widget; } - QString contextHelpId() const { return QString(); } + QList context() const + { + return m_context; + } + QWidget *widget() + { + return m_widget; + } + QString contextHelpId() const + { + return QString(); + } - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); private: - ManualControlCommand* getManualControlCommand(); + ManualControlCommand *getManualControlCommand(); double constrain(double value); QTime joystickTime; QWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp index 517ee06c8..866b43867 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp @@ -31,7 +31,7 @@ * Loads a saved configuration or defaults if non exist. * */ -GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), rollChannel(-1), pitchChannel(-1), @@ -39,34 +39,32 @@ GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QS throttleChannel(-1) { int i; - for (i=0;i<8;i++) - { - buttonSettings[i].ActionID=0; - buttonSettings[i].FunctionID=0; - buttonSettings[i].Amount=0; + + for (i = 0; i < 8; i++) { + buttonSettings[i].ActionID = 0; + buttonSettings[i].FunctionID = 0; + buttonSettings[i].Amount = 0; channelReverse[i] = 0; } - //if a saved configuration exists load it - if(qSettings != 0) { - controlsMode = qSettings->value("controlsMode").toInt(); - rollChannel = qSettings->value("rollChannel").toInt(); - pitchChannel = qSettings->value("pitchChannel").toInt(); - yawChannel = qSettings->value("yawChannel").toInt(); + // if a saved configuration exists load it + if (qSettings != 0) { + controlsMode = qSettings->value("controlsMode").toInt(); + rollChannel = qSettings->value("rollChannel").toInt(); + pitchChannel = qSettings->value("pitchChannel").toInt(); + yawChannel = qSettings->value("yawChannel").toInt(); throttleChannel = qSettings->value("throttleChannel").toInt(); udp_port = qSettings->value("controlPortUDP").toUInt(); udp_host = QHostAddress(qSettings->value("controlHostUDP").toString()); int i; - for (i=0;i<8;i++) - { - buttonSettings[i].ActionID = qSettings->value(QString().sprintf("button%dAction",i)).toInt(); - buttonSettings[i].FunctionID = qSettings->value(QString().sprintf("button%dFunction",i)).toInt(); - buttonSettings[i].Amount = qSettings->value(QString().sprintf("button%dAmount",i)).toDouble(); - channelReverse[i] = qSettings->value(QString().sprintf("channel%dReverse",i)).toBool(); + for (i = 0; i < 8; i++) { + buttonSettings[i].ActionID = qSettings->value(QString().sprintf("button%dAction", i)).toInt(); + buttonSettings[i].FunctionID = qSettings->value(QString().sprintf("button%dFunction", i)).toInt(); + buttonSettings[i].Amount = qSettings->value(QString().sprintf("button%dAmount", i)).toDouble(); + channelReverse[i] = qSettings->value(QString().sprintf("channel%dReverse", i)).toBool(); } } - } void GCSControlGadgetConfiguration::setUDPControlSettings(int port, QString host) @@ -84,10 +82,11 @@ QHostAddress GCSControlGadgetConfiguration::getUDPControlHost() return udp_host; } -void GCSControlGadgetConfiguration::setRPYTchannels(int roll, int pitch, int yaw, int throttle) { - rollChannel = roll; - pitchChannel = pitch; - yawChannel = yaw; +void GCSControlGadgetConfiguration::setRPYTchannels(int roll, int pitch, int yaw, int throttle) +{ + rollChannel = roll; + pitchChannel = pitch; + yawChannel = yaw; throttleChannel = throttle; } @@ -101,7 +100,9 @@ QList GCSControlGadgetConfiguration::getChannelsReverse() { QList ql; int i; - for (i=0;i<8;i++)ql << channelReverse[i]; + for (i = 0; i < 8; i++) { + ql << channelReverse[i]; + } return ql; } @@ -114,21 +115,20 @@ IUAVGadgetConfiguration *GCSControlGadgetConfiguration::clone() { GCSControlGadgetConfiguration *m = new GCSControlGadgetConfiguration(this->classId()); - m->controlsMode = controlsMode; - m->rollChannel = rollChannel; - m->pitchChannel = pitchChannel; - m->yawChannel = yawChannel; + m->controlsMode = controlsMode; + m->rollChannel = rollChannel; + m->pitchChannel = pitchChannel; + m->yawChannel = yawChannel; m->throttleChannel = throttleChannel; m->udp_host = udp_host; m->udp_port = udp_port; int i; - for (i=0;i<8;i++) - { - m->buttonSettings[i].ActionID = buttonSettings[i].ActionID; + for (i = 0; i < 8; i++) { + m->buttonSettings[i].ActionID = buttonSettings[i].ActionID; m->buttonSettings[i].FunctionID = buttonSettings[i].FunctionID; - m->buttonSettings[i].Amount = buttonSettings[i].Amount; + m->buttonSettings[i].Amount = buttonSettings[i].Amount; m->channelReverse[i] = channelReverse[i]; } @@ -140,23 +140,22 @@ IUAVGadgetConfiguration *GCSControlGadgetConfiguration::clone() * Saves a configuration. * */ -void GCSControlGadgetConfiguration::saveConfig(QSettings* settings) const { +void GCSControlGadgetConfiguration::saveConfig(QSettings *settings) const +{ settings->setValue("controlsMode", controlsMode); settings->setValue("rollChannel", rollChannel); settings->setValue("pitchChannel", pitchChannel); settings->setValue("yawChannel", yawChannel); settings->setValue("throttleChannel", throttleChannel); - settings->setValue("controlPortUDP",QString::number(udp_port)); - settings->setValue("controlHostUDP",udp_host.toString()); + settings->setValue("controlPortUDP", QString::number(udp_port)); + settings->setValue("controlHostUDP", udp_host.toString()); int i; - for (i=0;i<8;i++) - { - settings->setValue(QString().sprintf("button%dAction",i), buttonSettings[i].ActionID); - settings->setValue(QString().sprintf("button%dFunction",i), buttonSettings[i].FunctionID); - settings->setValue(QString().sprintf("button%dAmount",i), buttonSettings[i].Amount); - settings->setValue(QString().sprintf("channel%dReverse",i), channelReverse[i]); + for (i = 0; i < 8; i++) { + settings->setValue(QString().sprintf("button%dAction", i), buttonSettings[i].ActionID); + settings->setValue(QString().sprintf("button%dFunction", i), buttonSettings[i].FunctionID); + settings->setValue(QString().sprintf("button%dAmount", i), buttonSettings[i].Amount); + settings->setValue(QString().sprintf("channel%dReverse", i), channelReverse[i]); } - } diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h index 7a963f7bc..66602ac35 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h @@ -31,60 +31,77 @@ #include #include -typedef struct{ - int ActionID; - int FunctionID; - double Amount; -}buttonSettingsStruct; +typedef struct { + int ActionID; + int FunctionID; + double Amount; +} buttonSettingsStruct; -typedef struct{ +typedef struct { int port; QHostAddress address; -}portSettingsStruct; +} portSettingsStruct; using namespace Core; - -class GCSControlGadgetConfiguration : public IUAVGadgetConfiguration -{ +class GCSControlGadgetConfiguration : public IUAVGadgetConfiguration { Q_OBJECT - public: - explicit GCSControlGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); +public: + explicit GCSControlGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void setControlsMode(int mode) { controlsMode = mode; } + void setControlsMode(int mode) + { + controlsMode = mode; + } void setRPYTchannels(int roll, int pitch, int yaw, int throttle); void setUDPControlSettings(int port, QString host); int getUDPControlPort(); QHostAddress getUDPControlHost(); - int getControlsMode() { return controlsMode; } + int getControlsMode() + { + return controlsMode; + } QList getChannelsMapping(); QList getChannelsReverse(); - buttonSettingsStruct getbuttonSettings(int i){return buttonSettings[i];} - void setbuttonSettingsAction(int i, int ActionID ){buttonSettings[i].ActionID=ActionID;return;} - void setbuttonSettingsFunction(int i, int FunctionID ){buttonSettings[i].FunctionID=FunctionID;return;} - void setbuttonSettingsAmount(int i, double Amount ){buttonSettings[i].Amount=Amount;return;} - void setChannelReverse(int i, bool Reverse ){channelReverse[i]=Reverse;return;} + buttonSettingsStruct getbuttonSettings(int i) + { + return buttonSettings[i]; + } + void setbuttonSettingsAction(int i, int ActionID) + { + buttonSettings[i].ActionID = ActionID; + } + void setbuttonSettingsFunction(int i, int FunctionID) + { + buttonSettings[i].FunctionID = FunctionID; + } + void setbuttonSettingsAmount(int i, double Amount) + { + buttonSettings[i].Amount = Amount; + } + void setChannelReverse(int i, bool Reverse) + { + channelReverse[i] = Reverse; + } - void saveConfig(QSettings* settings) const; - IUAVGadgetConfiguration *clone(); - - private: - int controlsMode; // Mode1 to Mode4 - // Joystick mappings for roll/pitch/yaw/throttle: - int rollChannel; - int pitchChannel; - int yawChannel; - int throttleChannel; - buttonSettingsStruct buttonSettings[8]; - bool channelReverse[8]; - int udp_port; - QHostAddress udp_host; - + void saveConfig(QSettings *settings) const; + IUAVGadgetConfiguration *clone(); +private: + int controlsMode; // Mode1 to Mode4 + // Joystick mappings for roll/pitch/yaw/throttle: + int rollChannel; + int pitchChannel; + int yawChannel; + int throttleChannel; + buttonSettingsStruct buttonSettings[8]; + bool channelReverse[8]; + int udp_port; + QHostAddress udp_host; }; #endif // GCSCONTROLGADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.cpp index f8135f4ed..bef6c191f 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.cpp @@ -30,30 +30,27 @@ #include GCSControlGadgetFactory::GCSControlGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("GCSControlGadget"), - tr("Controller"), - parent) -{ -} + IUAVGadgetFactory(QString("GCSControlGadget"), + tr("Controller"), + parent) +{} GCSControlGadgetFactory::~GCSControlGadgetFactory() +{} + +IUAVGadget *GCSControlGadgetFactory::createGadget(QWidget *parent) { + GCSControlGadgetWidget *gadgetWidget = new GCSControlGadgetWidget(parent); -} - -IUAVGadget* GCSControlGadgetFactory::createGadget(QWidget *parent) { - GCSControlGadgetWidget* gadgetWidget = new GCSControlGadgetWidget(parent); return new GCSControlGadget(QString("GCSControlGadget"), gadgetWidget, parent, this->parent()); } -IUAVGadgetConfiguration *GCSControlGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *GCSControlGadgetFactory::createConfiguration(QSettings *qSettings) { return new GCSControlGadgetConfiguration(QString("GCSControlGadget"), qSettings); } IOptionsPage *GCSControlGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new GCSControlGadgetOptionsPage(qobject_cast(config), this->parent()); + return new GCSControlGadgetOptionsPage(qobject_cast(config), this->parent()); } - - diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.h index a30e22cc8..0a13fd2f1 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.h @@ -39,17 +39,15 @@ class IUAVGadgetFactory; using namespace Core; -class GCSControlGadgetFactory : public IUAVGadgetFactory -{ +class GCSControlGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: GCSControlGadgetFactory(QObject *parent = 0); ~GCSControlGadgetFactory(); IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); - }; #endif // GCSControlGADGETFACTORY_H_ diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp index 3ca50313e..45316ce05 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp @@ -34,39 +34,32 @@ #include GCSControlGadgetOptionsPage::GCSControlGadgetOptionsPage(GCSControlGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) + IOptionsPage(parent), + m_config(config) { options_page = NULL; - sdlGamepad = dynamic_cast(parent)->sdlGamepad; - - - + sdlGamepad = dynamic_cast(parent)->sdlGamepad; } GCSControlGadgetOptionsPage::~GCSControlGadgetOptionsPage() -{ - -} +{} void GCSControlGadgetOptionsPage::buttonState(ButtonNumber number, bool pressed) { if (options_page) { - QList rbList; + QList rbList; rbList << options_page->buttonInput0 << - options_page->buttonInput1 << options_page->buttonInput2 << - options_page->buttonInput3 << options_page->buttonInput4 << - options_page->buttonInput5 << options_page->buttonInput6 << - options_page->buttonInput7; + options_page->buttonInput1 << options_page->buttonInput2 << + options_page->buttonInput3 << options_page->buttonInput4 << + options_page->buttonInput5 << options_page->buttonInput6 << + options_page->buttonInput7; - if (number<8) // We only support 8 buttons - { + if (number < 8) { // We only support 8 buttons rbList.at(number)->setChecked(pressed); } } - } void GCSControlGadgetOptionsPage::gamepads(quint8 count) @@ -74,37 +67,42 @@ void GCSControlGadgetOptionsPage::gamepads(quint8 count) Q_UNUSED(count); /*options_page->AvailableControllerList->clear(); - for (int i=0;iAvailableControllerList->addItem(QString().sprintf("%d",i));//SDL_JoystickName(i)); - }*/ - + }*/ } void GCSControlGadgetOptionsPage::axesValues(QListInt16 values) { if (options_page) { - QList pbList; + QList pbList; pbList << options_page->joyCh0 << - options_page->joyCh1 << options_page->joyCh2 << - options_page->joyCh3 << options_page->joyCh4 << - options_page->joyCh5 << options_page->joyCh6 << - options_page->joyCh7; - int i=0; - foreach (qint16 value, values) { - if (i>7) break; // We only support 7 channels - if (chRevList.at(i)->isChecked()==1)value = 65535 - value; - if (pbList.at(i)->minimum() > value) - pbList.at(i)->setMinimum(value); - if (pbList.at(i)->maximum() < value) - pbList.at(i)->setMaximum(value); + options_page->joyCh1 << options_page->joyCh2 << + options_page->joyCh3 << options_page->joyCh4 << + options_page->joyCh5 << options_page->joyCh6 << + options_page->joyCh7; + int i = 0; + foreach(qint16 value, values) { + if (i > 7) { + break; // We only support 7 channels + } + if (chRevList.at(i)->isChecked() == 1) { + value = 65535 - value; + } + if (pbList.at(i)->minimum() > value) { + pbList.at(i)->setMinimum(value); + } + if (pbList.at(i)->maximum() < value) { + pbList.at(i)->setMaximum(value); + } pbList.at(i++)->setValue(value); } } } -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *GCSControlGadgetOptionsPage::createPage(QWidget *parent) { Q_UNUSED(parent); @@ -115,106 +113,105 @@ QWidget *GCSControlGadgetOptionsPage::createPage(QWidget *parent) options_page->setupUi(optionsPageWidget); - - //QList chList; + // QList chList; chList.clear(); chList << options_page->channel0 << options_page->channel1 << - options_page->channel2 << options_page->channel3 << - options_page->channel4 << options_page->channel5 << - options_page->channel6 << options_page->channel7; + options_page->channel2 << options_page->channel3 << + options_page->channel4 << options_page->channel5 << + options_page->channel6 << options_page->channel7; QStringList chOptions; chOptions << "None" << "Roll" << "Pitch" << "Yaw" << "Throttle"; - foreach (QComboBox* qb, chList) { + foreach(QComboBox * qb, chList) { qb->addItems(chOptions); } - //QList chRevList; + // QList chRevList; chRevList.clear(); chRevList << options_page->revCheckBox_1 << options_page->revCheckBox_2 << - options_page->revCheckBox_3 << options_page->revCheckBox_4 << - options_page->revCheckBox_5 << options_page->revCheckBox_6 << - options_page->revCheckBox_7 << options_page->revCheckBox_8; + options_page->revCheckBox_3 << options_page->revCheckBox_4 << + options_page->revCheckBox_5 << options_page->revCheckBox_6 << + options_page->revCheckBox_7 << options_page->revCheckBox_8; - //QList buttonFunctionList; + // QList buttonFunctionList; buttonFunctionList.clear(); buttonFunctionList << options_page->buttonFunction0 << options_page->buttonFunction1 << - options_page->buttonFunction2 << options_page->buttonFunction3 << - options_page->buttonFunction4 << options_page->buttonFunction5 << - options_page->buttonFunction6 << options_page->buttonFunction7; + options_page->buttonFunction2 << options_page->buttonFunction3 << + options_page->buttonFunction4 << options_page->buttonFunction5 << + options_page->buttonFunction6 << options_page->buttonFunction7; QStringList buttonOptions; - buttonOptions <<"-" << "Roll" << "Pitch" << "Yaw" << "Throttle" << "Armed" << "GCS Control"; //added UDP control to action list - foreach (QComboBox* qb, buttonFunctionList) { + buttonOptions << "-" << "Roll" << "Pitch" << "Yaw" << "Throttle" << "Armed" << "GCS Control"; // added UDP control to action list + foreach(QComboBox * qb, buttonFunctionList) { qb->addItems(buttonOptions); } - //QList buttonActionList; + // QList buttonActionList; buttonActionList.clear(); buttonActionList << options_page->buttonAction0 << options_page->buttonAction1 << - options_page->buttonAction2 << options_page->buttonAction3 << - options_page->buttonAction4 << options_page->buttonAction5 << - options_page->buttonAction6 << options_page->buttonAction7; + options_page->buttonAction2 << options_page->buttonAction3 << + options_page->buttonAction4 << options_page->buttonAction5 << + options_page->buttonAction6 << options_page->buttonAction7; QStringList buttonActionOptions; buttonActionOptions << "Does nothing" << "Increases" << "Decreases" << "Toggles"; - foreach (QComboBox* qb, buttonActionList) { + foreach(QComboBox * qb, buttonActionList) { qb->addItems(buttonActionOptions); } - //QList buttonValueList; + // QList buttonValueList; buttonValueList.clear(); buttonValueList << options_page->buttonAmount0 << options_page->buttonAmount1 << - options_page->buttonAmount2 << options_page->buttonAmount3 << - options_page->buttonAmount4 << options_page->buttonAmount5 << - options_page->buttonAmount6 << options_page->buttonAmount7; - //QList buttonLabelList; + options_page->buttonAmount2 << options_page->buttonAmount3 << + options_page->buttonAmount4 << options_page->buttonAmount5 << + options_page->buttonAmount6 << options_page->buttonAmount7; + // QList buttonLabelList; buttonLabelList.clear(); buttonLabelList << options_page->buttonLabel0 << options_page->buttonLabel1 << - options_page->buttonLabel2 << options_page->buttonLabel3 << - options_page->buttonLabel4 << options_page->buttonLabel5 << - options_page->buttonLabel6 << options_page->buttonLabel7; + options_page->buttonLabel2 << options_page->buttonLabel3 << + options_page->buttonLabel4 << options_page->buttonLabel5 << + options_page->buttonLabel6 << options_page->buttonLabel7; - for (i=0;i<8;i++) - { + for (i = 0; i < 8; i++) { buttonActionList.at(i)->setCurrentIndex(m_config->getbuttonSettings(i).ActionID); buttonFunctionList.at(i)->setCurrentIndex(m_config->getbuttonSettings(i).FunctionID); buttonValueList.at(i)->setValue(m_config->getbuttonSettings(i).Amount); - connect(buttonFunctionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonFunction())); - //connect(buttonActionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonActions[i]())); + connect(buttonFunctionList.at(i), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonFunction())); + // connect(buttonActionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonActions[i]())); updateButtonAction(i); buttonFunctionList.at(i)->setCurrentIndex(m_config->getbuttonSettings(i).FunctionID); } - connect(buttonActionList.at(0),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_0())); - connect(buttonActionList.at(1),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_1())); - connect(buttonActionList.at(2),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_2())); - connect(buttonActionList.at(3),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_3())); - connect(buttonActionList.at(4),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_4())); - connect(buttonActionList.at(5),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_5())); - connect(buttonActionList.at(6),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_6())); - connect(buttonActionList.at(7),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_7())); + connect(buttonActionList.at(0), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_0())); + connect(buttonActionList.at(1), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_1())); + connect(buttonActionList.at(2), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_2())); + connect(buttonActionList.at(3), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_3())); + connect(buttonActionList.at(4), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_4())); + connect(buttonActionList.at(5), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_5())); + connect(buttonActionList.at(6), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_6())); + connect(buttonActionList.at(7), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_7())); - //updateButtonFunction(); + // updateButtonFunction(); options_page->udp_host->setText(m_config->getUDPControlHost().toString()); options_page->udp_port->setText(QString::number(m_config->getUDPControlPort())); // Controls mode are from 1 to 4. - if (m_config->getControlsMode()>0 && m_config->getControlsMode() < 5) - options_page->controlsMode->setCurrentIndex(m_config->getControlsMode()-1); - else + if (m_config->getControlsMode() > 0 && m_config->getControlsMode() < 5) { + options_page->controlsMode->setCurrentIndex(m_config->getControlsMode() - 1); + } else { qDebug() << "GCSControl: Invalid control modes setting! Did you edit by hand?"; + } QList ql = m_config->getChannelsMapping(); - for (int i=0; i<4; i++) { - if (ql.at(i) > -1) - chList.at(ql.at(i))->setCurrentIndex(i+1); + for (int i = 0; i < 4; i++) { + if (ql.at(i) > -1) { + chList.at(ql.at(i))->setCurrentIndex(i + 1); + } } QList qlChRev = m_config->getChannelsReverse(); - for (i=0; i<8; i++) - { + for (i = 0; i < 8; i++) { chRevList.at(i)->setChecked(qlChRev.at(i));; } - connect(sdlGamepad,SIGNAL(axesValues(QListInt16)),this,SLOT(axesValues(QListInt16))); - connect(sdlGamepad,SIGNAL(buttonState(ButtonNumber,bool)),this,SLOT(buttonState(ButtonNumber,bool))); - connect(sdlGamepad,SIGNAL(gamepads(quint8)),this,SLOT(gamepads(quint8))); + connect(sdlGamepad, SIGNAL(axesValues(QListInt16)), this, SLOT(axesValues(QListInt16))); + connect(sdlGamepad, SIGNAL(buttonState(ButtonNumber, bool)), this, SLOT(buttonState(ButtonNumber, bool))); + connect(sdlGamepad, SIGNAL(gamepads(quint8)), this, SLOT(gamepads(quint8))); return optionsPageWidget; } @@ -227,66 +224,63 @@ QWidget *GCSControlGadgetOptionsPage::createPage(QWidget *parent) */ void GCSControlGadgetOptionsPage::apply() { - m_config->setControlsMode(options_page->controlsMode->currentIndex()+1); - /*QList chList; - chList << options_page->channel0 << options_page->channel1 << - options_page->channel2 << options_page->channel3 << - options_page->channel4 << options_page->channel5 << - options_page->channel6 << options_page->channel7; - QList buttonFunctionList; - buttonFunctionList << options_page->buttonFunction0 << options_page->buttonFunction1 << - options_page->buttonFunction2 << options_page->buttonFunction3 << - options_page->buttonFunction4 << options_page->buttonFunction5 << - options_page->buttonFunction6 << options_page->buttonFunction7; - QList buttonActionList; - buttonActionList << options_page->buttonAction0 << options_page->buttonAction1 << - options_page->buttonAction2 << options_page->buttonAction3 << - options_page->buttonAction4 << options_page->buttonAction5 << - options_page->buttonAction6 << options_page->buttonAction7; - QList buttonValueList; - buttonValueList << options_page->buttonAmount0 << options_page->buttonAmount1 << - options_page->buttonAmount2 << options_page->buttonAmount3 << - options_page->buttonAmount4 << options_page->buttonAmount5 << - options_page->buttonAmount6 << options_page->buttonAmount7; -*/ + m_config->setControlsMode(options_page->controlsMode->currentIndex() + 1); + /*QList chList; + chList << options_page->channel0 << options_page->channel1 << + options_page->channel2 << options_page->channel3 << + options_page->channel4 << options_page->channel5 << + options_page->channel6 << options_page->channel7; + QList buttonFunctionList; + buttonFunctionList << options_page->buttonFunction0 << options_page->buttonFunction1 << + options_page->buttonFunction2 << options_page->buttonFunction3 << + options_page->buttonFunction4 << options_page->buttonFunction5 << + options_page->buttonFunction6 << options_page->buttonFunction7; + QList buttonActionList; + buttonActionList << options_page->buttonAction0 << options_page->buttonAction1 << + options_page->buttonAction2 << options_page->buttonAction3 << + options_page->buttonAction4 << options_page->buttonAction5 << + options_page->buttonAction6 << options_page->buttonAction7; + QList buttonValueList; + buttonValueList << options_page->buttonAmount0 << options_page->buttonAmount1 << + options_page->buttonAmount2 << options_page->buttonAmount3 << + options_page->buttonAmount4 << options_page->buttonAmount5 << + options_page->buttonAmount6 << options_page->buttonAmount7; + */ - int roll=-1 , pitch=-1, yaw=-1, throttle=-1; - for (int i=0; icurrentIndex()) { - case 1: - roll = i; - break; - case 2: - pitch =i; - break; - case 3: - yaw = i; - break; - case 4: - throttle = i; - break; - } - } - m_config->setRPYTchannels(roll,pitch,yaw,throttle); + int roll = -1, pitch = -1, yaw = -1, throttle = -1; + for (int i = 0; i < chList.length(); i++) { + switch (chList.at(i)->currentIndex()) { + case 1: + roll = i; + break; + case 2: + pitch = i; + break; + case 3: + yaw = i; + break; + case 4: + throttle = i; + break; + } + } + m_config->setRPYTchannels(roll, pitch, yaw, throttle); - m_config->setUDPControlSettings(options_page->udp_port->text().toInt(),options_page->udp_host->text()); - - - int j; - for (j=0;j<8;j++) - { - m_config->setbuttonSettingsAction(j,buttonActionList.at(j)->currentIndex()); - m_config->setbuttonSettingsFunction(j,buttonFunctionList.at(j)->currentIndex()); - m_config->setbuttonSettingsAmount(j,buttonValueList.at(j)->value()); - m_config->setChannelReverse(j,chRevList.at(j)->isChecked()); - } + m_config->setUDPControlSettings(options_page->udp_port->text().toInt(), options_page->udp_host->text()); + int j; + for (j = 0; j < 8; j++) { + m_config->setbuttonSettingsAction(j, buttonActionList.at(j)->currentIndex()); + m_config->setbuttonSettingsFunction(j, buttonFunctionList.at(j)->currentIndex()); + m_config->setbuttonSettingsAmount(j, buttonValueList.at(j)->value()); + m_config->setChannelReverse(j, chRevList.at(j)->isChecked()); + } } void GCSControlGadgetOptionsPage::finish() { - disconnect(sdlGamepad,0,this,0); + disconnect(sdlGamepad, 0, this, 0); delete options_page; options_page = NULL; } @@ -295,113 +289,98 @@ void GCSControlGadgetOptionsPage::finish() void GCSControlGadgetOptionsPage::updateButtonFunction() { int i; + /*QList buttonFunctionList; - buttonFunctionList << options_page->buttonFunction0 << options_page->buttonFunction1 << + buttonFunctionList << options_page->buttonFunction0 << options_page->buttonFunction1 << options_page->buttonFunction2 << options_page->buttonFunction3 << options_page->buttonFunction4 << options_page->buttonFunction5 << options_page->buttonFunction6 << options_page->buttonFunction7; - QList buttonActionList; - buttonActionList << options_page->buttonAction0 << options_page->buttonAction1 << + QList buttonActionList; + buttonActionList << options_page->buttonAction0 << options_page->buttonAction1 << options_page->buttonAction2 << options_page->buttonAction3 << options_page->buttonAction4 << options_page->buttonAction5 << options_page->buttonAction6 << options_page->buttonAction7; - QList buttonValueList; - buttonValueList << options_page->buttonAmount0 << options_page->buttonAmount1 << + QList buttonValueList; + buttonValueList << options_page->buttonAmount0 << options_page->buttonAmount1 << options_page->buttonAmount2 << options_page->buttonAmount3 << options_page->buttonAmount4 << options_page->buttonAmount5 << options_page->buttonAmount6 << options_page->buttonAmount7; - QList buttonLabelList; - buttonLabelList << options_page->buttonLabel0 << options_page->buttonLabel1 << + QList buttonLabelList; + buttonLabelList << options_page->buttonLabel0 << options_page->buttonLabel1 << options_page->buttonLabel2 << options_page->buttonLabel3 << options_page->buttonLabel4 << options_page->buttonLabel5 << options_page->buttonLabel6 << options_page->buttonLabel7; -*/ - for (i=0;i<8;i++) - { - if (buttonActionList.at(i)->currentText().compare("Does nothing")==0) - { + */ + for (i = 0; i < 8; i++) { + if (buttonActionList.at(i)->currentText().compare("Does nothing") == 0) { buttonFunctionList.at(i)->setVisible(0); buttonLabelList.at(i)->setVisible(0); buttonValueList.at(i)->setVisible(0); - } - else - if (buttonActionList.at(i)->currentText().compare("Toggles")==0) - { + } else if (buttonActionList.at(i)->currentText().compare("Toggles") == 0) { buttonFunctionList.at(i)->setVisible(1); buttonLabelList.at(i)->setVisible(0); buttonValueList.at(i)->setVisible(0); - } - else - { + } else { buttonFunctionList.at(i)->setVisible(1); buttonLabelList.at(i)->setVisible(1); buttonValueList.at(i)->setVisible(1); } } - - } void GCSControlGadgetOptionsPage::updateButtonAction(int controlID) { int i; QStringList buttonOptions; + /*QList buttonFunctionList; - buttonFunctionList << options_page->buttonFunction0 << options_page->buttonFunction1 << + buttonFunctionList << options_page->buttonFunction0 << options_page->buttonFunction1 << options_page->buttonFunction2 << options_page->buttonFunction3 << options_page->buttonFunction4 << options_page->buttonFunction5 << options_page->buttonFunction6 << options_page->buttonFunction7; - QList buttonActionList; - buttonActionList << options_page->buttonAction0 << options_page->buttonAction1 << + QList buttonActionList; + buttonActionList << options_page->buttonAction0 << options_page->buttonAction1 << options_page->buttonAction2 << options_page->buttonAction3 << options_page->buttonAction4 << options_page->buttonAction5 << options_page->buttonAction6 << options_page->buttonAction7; - QList buttonValueList; - buttonValueList << options_page->buttonAmount0 << options_page->buttonAmount1 << + QList buttonValueList; + buttonValueList << options_page->buttonAmount0 << options_page->buttonAmount1 << options_page->buttonAmount2 << options_page->buttonAmount3 << options_page->buttonAmount4 << options_page->buttonAmount5 << options_page->buttonAmount6 << options_page->buttonAmount7; - QList buttonLabelList; - buttonLabelList << options_page->buttonLabel0 << options_page->buttonLabel1 << + QList buttonLabelList; + buttonLabelList << options_page->buttonLabel0 << options_page->buttonLabel1 << options_page->buttonLabel2 << options_page->buttonLabel3 << options_page->buttonLabel4 << options_page->buttonLabel5 << options_page->buttonLabel6 << options_page->buttonLabel7; -*/ - //for (i=0;i<8;i++) - i=controlID; + */ + // for (i=0;i<8;i++) + i = controlID; { - if (buttonActionList.at(i)->currentText().compare("Does nothing")==0) - { + if (buttonActionList.at(i)->currentText().compare("Does nothing") == 0) { buttonFunctionList.at(i)->setVisible(0); buttonLabelList.at(i)->setVisible(0); buttonValueList.at(i)->setVisible(0); - } - else - if (buttonActionList.at(i)->currentText().compare("Toggles")==0) - { - disconnect(buttonFunctionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonFunction())); - buttonOptions <<"-" << "Armed" << "GCS Control" << "UDP Control"; + } else if (buttonActionList.at(i)->currentText().compare("Toggles") == 0) { + disconnect(buttonFunctionList.at(i), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonFunction())); + buttonOptions << "-" << "Armed" << "GCS Control" << "UDP Control"; buttonFunctionList.at(i)->clear(); - buttonFunctionList.at(i)->insertItems(-1,buttonOptions); + buttonFunctionList.at(i)->insertItems(-1, buttonOptions); buttonFunctionList.at(i)->setVisible(1); buttonLabelList.at(i)->setVisible(0); buttonValueList.at(i)->setVisible(0); - connect(buttonFunctionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonFunction())); - } - else - { - disconnect(buttonFunctionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonFunction())); - buttonOptions <<"-" << "Roll" << "Pitch" << "Yaw" << "Throttle" ; + connect(buttonFunctionList.at(i), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonFunction())); + } else { + disconnect(buttonFunctionList.at(i), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonFunction())); + buttonOptions << "-" << "Roll" << "Pitch" << "Yaw" << "Throttle"; buttonFunctionList.at(i)->clear(); buttonFunctionList.at(i)->addItems(buttonOptions); buttonFunctionList.at(i)->setVisible(1); buttonLabelList.at(i)->setVisible(1); buttonValueList.at(i)->setVisible(1); - connect(buttonFunctionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonFunction())); + connect(buttonFunctionList.at(i), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonFunction())); } } - - } diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.h index 3d526cd0c..5eb5bf4dd 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.h @@ -46,14 +46,13 @@ class IUAVGadgetConfiguration; class GCSControlGadgetConfiguration; namespace Ui { - class GCSControlGadgetOptionsPage; +class GCSControlGadgetOptionsPage; } using namespace Core; -class GCSControlGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class GCSControlGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit GCSControlGadgetOptionsPage(GCSControlGadgetConfiguration *config, QObject *parent = 0); ~GCSControlGadgetOptionsPage(); @@ -67,12 +66,12 @@ private: GCSControlGadgetConfiguration *m_config; SDLGamepad *sdlGamepad; - QList chList; - QList chRevList; - QList buttonFunctionList; - QList buttonActionList; - QList buttonValueList; - QList buttonLabelList; + QList chList; + QList chRevList; + QList buttonFunctionList; + QList buttonActionList; + QList buttonValueList; + QList buttonLabelList; protected slots: // signals from joystick @@ -81,14 +80,38 @@ protected slots: void axesValues(QListInt16 values); void updateButtonFunction(); void updateButtonAction(int controlID); - void updateButtonAction_0(void){updateButtonAction(0);}; - void updateButtonAction_1(void){updateButtonAction(1);}; - void updateButtonAction_2(void){updateButtonAction(2);}; - void updateButtonAction_3(void){updateButtonAction(3);}; - void updateButtonAction_4(void){updateButtonAction(4);}; - void updateButtonAction_5(void){updateButtonAction(5);}; - void updateButtonAction_6(void){updateButtonAction(6);}; - void updateButtonAction_7(void){updateButtonAction(7);}; + void updateButtonAction_0(void) + { + updateButtonAction(0); + }; + void updateButtonAction_1(void) + { + updateButtonAction(1); + }; + void updateButtonAction_2(void) + { + updateButtonAction(2); + }; + void updateButtonAction_3(void) + { + updateButtonAction(3); + }; + void updateButtonAction_4(void) + { + updateButtonAction(4); + }; + void updateButtonAction_5(void) + { + updateButtonAction(5); + }; + void updateButtonAction_6(void) + { + updateButtonAction(6); + }; + void updateButtonAction_7(void) + { + updateButtonAction(7); + }; }; #endif // GCSCONTROLGADGETOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp index bbafda25e..d5cef0f0d 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp @@ -48,77 +48,80 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent) ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); - UAVObject::Metadata mdata = obj->getMetadata(); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("ManualControlCommand"))); + UAVObject::Metadata mdata = obj->getMetadata(); m_gcscontrol->checkBoxGcsControl->setChecked(UAVObject::GetFlightAccess(mdata) == UAVObject::ACCESS_READONLY); // Set up the drop down box for the flightmode - UAVDataObject* flightStatus = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); + UAVDataObject *flightStatus = dynamic_cast(objManager->getObject(QString("FlightStatus"))); m_gcscontrol->comboBoxFlightMode->addItems(flightStatus->getField("FlightMode")->getOptions()); // Set up slots and signals for joysticks - connect(m_gcscontrol->widgetLeftStick,SIGNAL(positionClicked(double,double)),this,SLOT(leftStickClicked(double,double))); - connect(m_gcscontrol->widgetRightStick,SIGNAL(positionClicked(double,double)),this,SLOT(rightStickClicked(double,double))); + connect(m_gcscontrol->widgetLeftStick, SIGNAL(positionClicked(double, double)), this, SLOT(leftStickClicked(double, double))); + connect(m_gcscontrol->widgetRightStick, SIGNAL(positionClicked(double, double)), this, SLOT(rightStickClicked(double, double))); // Connect misc controls connect(m_gcscontrol->checkBoxGcsControl, SIGNAL(stateChanged(int)), this, SLOT(toggleControl(int))); connect(m_gcscontrol->checkBoxArmed, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int))); connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(selectFlightMode(int))); - connect(m_gcscontrol->checkBoxUDPControl, SIGNAL(stateChanged(int)),this,SLOT(toggleUDPControl(int))); //UDP control checkbox + connect(m_gcscontrol->checkBoxUDPControl, SIGNAL(stateChanged(int)), this, SLOT(toggleUDPControl(int))); // UDP control checkbox // Connect object updated event from UAVObject to also update check boxes and dropdown - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(mccChanged(UAVObject*))); + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(mccChanged(UAVObject *))); - leftX = 0; - leftY = 0; + leftX = 0; + leftY = 0; rightX = 0; rightY = 0; // No point enabling OpenGL for the joysticks, and causes // issues on some computers: -// m_gcscontrol->widgetLeftStick->enableOpenGL(true); -// m_gcscontrol->widgetRightStick->enableOpenGL(true); +// m_gcscontrol->widgetLeftStick->enableOpenGL(true); +// m_gcscontrol->widgetRightStick->enableOpenGL(true); } GCSControlGadgetWidget::~GCSControlGadgetWidget() { - // Do nothing + // Do nothing } -void GCSControlGadgetWidget::updateSticks(double nleftX, double nleftY, double nrightX, double nrightY) { - leftX = nleftX; - leftY = nleftY; +void GCSControlGadgetWidget::updateSticks(double nleftX, double nleftY, double nrightX, double nrightY) +{ + leftX = nleftX; + leftY = nleftY; rightX = nrightX; rightY = nrightY; - m_gcscontrol->widgetLeftStick->changePosition(leftX,leftY); - m_gcscontrol->widgetRightStick->changePosition(rightX,rightY); + m_gcscontrol->widgetLeftStick->changePosition(leftX, leftY); + m_gcscontrol->widgetRightStick->changePosition(rightX, rightY); } -void GCSControlGadgetWidget::leftStickClicked(double X, double Y) { +void GCSControlGadgetWidget::leftStickClicked(double X, double Y) +{ leftX = X; leftY = Y; - emit sticksChanged(leftX,leftY,rightX,rightY); + emit sticksChanged(leftX, leftY, rightX, rightY); } -void GCSControlGadgetWidget::rightStickClicked(double X, double Y) { +void GCSControlGadgetWidget::rightStickClicked(double X, double Y) +{ rightX = X; rightY = Y; - emit sticksChanged(leftX,leftY,rightX,rightY); + emit sticksChanged(leftX, leftY, rightX, rightY); } /*! - \brief Called when the gcs control is toggled and enabled or disables flight write access to manual control command - */ + \brief Called when the gcs control is toggled and enabled or disables flight write access to manual control command + */ void GCSControlGadgetWidget::toggleControl(int state) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("ManualControlCommand"))); - UAVObject::Metadata mdata = obj->getMetadata(); - if (state) - { + UAVObject::Metadata mdata = obj->getMetadata(); + + if (state) { mccInitialData = mdata; UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); @@ -126,9 +129,7 @@ void GCSControlGadgetWidget::toggleControl(int state) UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); mdata.gcsTelemetryUpdatePeriod = 100; m_gcscontrol->checkBoxUDPControl->setEnabled(true); - } - else - { + } else { mdata = mccInitialData; toggleUDPControl(false); m_gcscontrol->checkBoxUDPControl->setEnabled(false); @@ -140,44 +141,46 @@ void GCSControlGadgetWidget::toggleArmed(int state) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); - if(state) + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("FlightStatus"))); + + if (state) { obj->getField("Armed")->setValue("Armed"); - else + } else { obj->getField("Armed")->setValue("Disarmed"); + } obj->updated(); } -void GCSControlGadgetWidget::mccChanged(UAVObject * obj) +void GCSControlGadgetWidget::mccChanged(UAVObject *obj) { Q_UNUSED(obj); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* flightStatus = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); + UAVDataObject *flightStatus = dynamic_cast(objManager->getObject(QString("FlightStatus"))); m_gcscontrol->comboBoxFlightMode->setCurrentIndex(m_gcscontrol->comboBoxFlightMode->findText(flightStatus->getField("FlightMode")->getValue().toString())); m_gcscontrol->checkBoxArmed->setChecked(flightStatus->getField("Armed")->getValue() == "Armed"); } void GCSControlGadgetWidget::toggleUDPControl(int state) { - if(state) - { + if (state) { setUDPControl(true); - }else{ + } else { setUDPControl(false); } } /*! - \brief Called when the flight mode drop down is changed and sets the ManualControlCommand->FlightMode accordingly - */ + \brief Called when the flight mode drop down is changed and sets the ManualControlCommand->FlightMode accordingly + */ void GCSControlGadgetWidget::selectFlightMode(int state) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); - UAVObjectField * field = obj->getField("FlightMode"); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("FlightStatus"))); + UAVObjectField *field = obj->getField("FlightMode"); + field->setValue(field->getOptions()[state]); obj->updated(); } @@ -203,7 +206,6 @@ bool GCSControlGadgetWidget::getUDPControl(void) /** - * @} - * @} - */ - + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h index 05c3964cd..16f794d4c 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GCSControlGadgetPlugin GCSControl Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,8 +35,7 @@ class Ui_GCSControl; -class GCSControlGadgetWidget : public QLabel -{ +class GCSControlGadgetWidget : public QLabel { Q_OBJECT public: @@ -68,7 +67,7 @@ protected slots: private: Ui_GCSControl *m_gcscontrol; UAVObject::Metadata mccInitialData; - double leftX,leftY,rightX,rightY; + double leftX, leftY, rightX, rightY; }; #endif /* GCSControlGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.cpp index af866df17..3f8c8db41 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.cpp @@ -34,42 +34,42 @@ GCSControlPlugin::GCSControlPlugin() { - // Do nothing + // Do nothing } GCSControlPlugin::~GCSControlPlugin() { - // Do nothing + // Do nothing } -bool GCSControlPlugin::initialize(const QStringList& args, QString *errMsg) +bool GCSControlPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); + Q_UNUSED(args); + Q_UNUSED(errMsg); sdlGamepad = new SDLGamepad(); - if(sdlGamepad->init()) { - sdlGamepad->start(); - qRegisterMetaType("QListInt16"); - qRegisterMetaType("ButtonNumber"); - } - mf = new GCSControlGadgetFactory(this); - addAutoReleasedObject(mf); + if (sdlGamepad->init()) { + sdlGamepad->start(); + qRegisterMetaType("QListInt16"); + qRegisterMetaType("ButtonNumber"); + } + mf = new GCSControlGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void GCSControlPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void GCSControlPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(GCSControlPlugin) /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.h index e115e3546..4ab32503f 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.h @@ -33,19 +33,17 @@ class GCSControlGadgetFactory; -class GCSControlPlugin : public ExtensionSystem::IPlugin -{ +class GCSControlPlugin : public ExtensionSystem::IPlugin { public: GCSControlPlugin(); - ~GCSControlPlugin(); + ~GCSControlPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); - SDLGamepad *sdlGamepad; + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); + SDLGamepad *sdlGamepad; private: - GCSControlGadgetFactory *mf; - + GCSControlGadgetFactory *mf; }; #endif /* GCSControlPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcsonctrolgadgetwidget.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcsonctrolgadgetwidget.h index 7ee0bd326..737846899 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcsonctrolgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcsonctrolgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,13 +30,12 @@ #include -class EmptyGadgetWidget : public QLabel -{ +class EmptyGadgetWidget : public QLabel { Q_OBJECT public: EmptyGadgetWidget(QWidget *parent = 0); - ~EmptyGadgetWidget(); + ~EmptyGadgetWidget(); private: }; diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.cpp index 8a0fb1360..861e38c96 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.cpp @@ -33,8 +33,8 @@ #include /** - * @brief Constructor for JoystickControl widget. Sets up the image of a joystick - */ + * @brief Constructor for JoystickControl widget. Sets up the image of a joystick + */ JoystickControl::JoystickControl(QWidget *parent) : QGraphicsView(parent) { setMinimumSize(64, 64); @@ -61,7 +61,7 @@ JoystickControl::JoystickControl(QWidget *parent) : QGraphicsView(parent) m_joystickArea->setPos( (m_background->boundingRect().width() - m_joystickArea->boundingRect().width()) * 0.5, (m_background->boundingRect().height() - m_joystickArea->boundingRect().height()) * 0.5 - ); + ); m_joystickArea->setVisible(false); QGraphicsScene *l_scene = scene(); @@ -80,50 +80,61 @@ JoystickControl::~JoystickControl() } /** - * @brief Enables/Disables OpenGL - */ + * @brief Enables/Disables OpenGL + */ void JoystickControl::enableOpenGL(bool flag) { - if (flag) + if (flag) { setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); - else + } else { setViewport(new QWidget); + } } /** - * @brief Update the displayed position based on an MCC update - */ + * @brief Update the displayed position based on an MCC update + */ void JoystickControl::changePosition(double x, double y) { QRectF areaSize = m_joystickArea->boundingRect(); QPointF point( ((1.0 + x) * areaSize.width() - m_joystickEnd->boundingRect().width()) * 0.5, ((1.0 - y) * areaSize.height() - m_joystickEnd->boundingRect().height()) * 0.5 - ); + ); + m_joystickEnd->setPos(m_joystickArea->mapToScene(point)); } /** - * @brief Redirect mouse move events to control position - */ + * @brief Redirect mouse move events to control position + */ void JoystickControl::mouseMoveEvent(QMouseEvent *event) { - QPointF point = m_joystickArea->mapFromScene(mapToScene(event->pos())); + QPointF point = m_joystickArea->mapFromScene(mapToScene(event->pos())); QSizeF areaSize = m_joystickArea->boundingRect().size(); - double y = - (point.y() / areaSize.height() - 0.5) * 2.0; + double y = -(point.y() / areaSize.height() - 0.5) * 2.0; double x = (point.x() / areaSize.width() - 0.5) * 2.0; - if (y < -1.0) y = -1.0; - if (y > 1.0) y = 1.0; - if (x < -1.0) x = -1.0; - if (x > 1.0) x = 1.0; + + if (y < -1.0) { + y = -1.0; + } + if (y > 1.0) { + y = 1.0; + } + if (x < -1.0) { + x = -1.0; + } + if (x > 1.0) { + x = 1.0; + } emit positionClicked(x, y); } /** - * @brief Redirect mouse move clicks to control position - */ + * @brief Redirect mouse move clicks to control position + */ void JoystickControl::mousePressEvent(QMouseEvent *event) { if (event->button() == Qt::LeftButton) { @@ -153,6 +164,6 @@ void JoystickControl::resizeEvent(QResizeEvent *event) } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.h b/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.h index 9c6ac59d0..6bd3bb8d0 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.h @@ -35,11 +35,10 @@ #include "manualcontrolcommand.h" namespace Ui { - class JoystickControl; +class JoystickControl; } -class JoystickControl : public QGraphicsView -{ +class JoystickControl : public QGraphicsView { Q_OBJECT public: @@ -61,15 +60,15 @@ signals: void positionClicked(double x, double y); private: - QSvgRenderer *m_renderer; - QGraphicsSvgItem *m_background; - QGraphicsSvgItem *m_joystickArea; - QGraphicsSvgItem *m_joystickEnd; + QSvgRenderer *m_renderer; + QGraphicsSvgItem *m_background; + QGraphicsSvgItem *m_joystickArea; + QGraphicsSvgItem *m_joystickEnd; }; #endif // JOYSTICKCONTROL_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.cpp index 08d93c157..032fec211 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.cpp @@ -4,8 +4,8 @@ * @file buffer.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief see below - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and * objects. * @see The GNU Public License (GPL) Version 3 * @@ -28,7 +28,7 @@ */ /*! \file buffer.c \brief Multipurpose byte buffer structure and methods. */ -//***************************************************************************** +// ***************************************************************************** // // File Name : 'buffer.c' // Title : Multipurpose byte buffer structure and methods @@ -40,9 +40,9 @@ // Editor Tabs : 4 // // This code is distributed under the GNU Public License -// which can be found at http://www.gnu.org/licenses/gpl.txt +// which can be found at http://www.gnu.org/licenses/gpl.txt // -//***************************************************************************** +// ***************************************************************************** #include "buffer.h" @@ -50,90 +50,81 @@ // initialization -void bufferInit(cBuffer* buffer, unsigned char *start, unsigned short size) +void bufferInit(cBuffer *buffer, unsigned char *start, unsigned short size) { - // set start pointer of the buffer - buffer->dataptr = start; - buffer->size = size; - // initialize index and length - buffer->dataindex = 0; - buffer->datalength = 0; + // set start pointer of the buffer + buffer->dataptr = start; + buffer->size = size; + // initialize index and length + buffer->dataindex = 0; + buffer->datalength = 0; } // access routines -unsigned char bufferGetFromFront(cBuffer* buffer) +unsigned char bufferGetFromFront(cBuffer *buffer) { - unsigned char data = 0; + unsigned char data = 0; - // check to see if there's data in the buffer - if(buffer->datalength) - { - // get the first character from buffer - data = buffer->dataptr[buffer->dataindex]; - // move index down and decrement length - buffer->dataindex++; - if(buffer->dataindex >= buffer->size) - { - buffer->dataindex %= buffer->size; - } - buffer->datalength--; - } - // return - return data; + // check to see if there's data in the buffer + if (buffer->datalength) { + // get the first character from buffer + data = buffer->dataptr[buffer->dataindex]; + // move index down and decrement length + buffer->dataindex++; + if (buffer->dataindex >= buffer->size) { + buffer->dataindex %= buffer->size; + } + buffer->datalength--; + } + // return + return data; } -void bufferDumpFromFront(cBuffer* buffer, unsigned short numbytes) +void bufferDumpFromFront(cBuffer *buffer, unsigned short numbytes) { - // dump numbytes from the front of the buffer - // are we dumping less than the entire buffer? - if(numbytes < buffer->datalength) - { - // move index down by numbytes and decrement length by numbytes - buffer->dataindex += numbytes; - if(buffer->dataindex >= buffer->size) - { - buffer->dataindex %= buffer->size; - } - buffer->datalength -= numbytes; - } - else - { - // flush the whole buffer - buffer->datalength = 0; - } + // dump numbytes from the front of the buffer + // are we dumping less than the entire buffer? + if (numbytes < buffer->datalength) { + // move index down by numbytes and decrement length by numbytes + buffer->dataindex += numbytes; + if (buffer->dataindex >= buffer->size) { + buffer->dataindex %= buffer->size; + } + buffer->datalength -= numbytes; + } else { + // flush the whole buffer + buffer->datalength = 0; + } } -unsigned char bufferGetAtIndex(cBuffer* buffer, unsigned short index) +unsigned char bufferGetAtIndex(cBuffer *buffer, unsigned short index) { - // return character at index in buffer - return buffer->dataptr[(buffer->dataindex+index)%(buffer->size)]; + // return character at index in buffer + return buffer->dataptr[(buffer->dataindex + index) % (buffer->size)]; } -unsigned char bufferAddToEnd(cBuffer* buffer, unsigned char data) +unsigned char bufferAddToEnd(cBuffer *buffer, unsigned char data) { - // make sure the buffer has room - if(buffer->datalength < buffer->size) - { - // save data byte at end of buffer - buffer->dataptr[(buffer->dataindex + buffer->datalength) % buffer->size] = data; - // increment the length - buffer->datalength++; - // return success - return -1; - } - else return 0; + // make sure the buffer has room + if (buffer->datalength < buffer->size) { + // save data byte at end of buffer + buffer->dataptr[(buffer->dataindex + buffer->datalength) % buffer->size] = data; + // increment the length + buffer->datalength++; + // return success + return -1; + } else { return 0; } } -unsigned char bufferIsNotFull(cBuffer* buffer) +unsigned char bufferIsNotFull(cBuffer *buffer) { - // check to see if the buffer has room - // return true if there is room - return (buffer->datalength < buffer->size); + // check to see if the buffer has room + // return true if there is room + return buffer->datalength < buffer->size; } -void bufferFlush(cBuffer* buffer) +void bufferFlush(cBuffer *buffer) { - // flush contents of the buffer - buffer->datalength = 0; + // flush contents of the buffer + buffer->datalength = 0; } - diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.h b/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.h index 513b1b241..cb495f15d 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.h @@ -4,15 +4,15 @@ * @file buffer.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief see below - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and * objects. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ /*! \file buffer.h \brief Multipurpose byte buffer structure and methods. */ -//***************************************************************************** +// ***************************************************************************** // // File Name : 'buffer.h' // Title : Multipurpose byte buffer structure and methods @@ -40,48 +40,47 @@ /// maximum size.  This buffer is used in many places in the avrlib code. // // This code is distributed under the GNU Public License -// which can be found at http://www.gnu.org/licenses/gpl.txt +// which can be found at http://www.gnu.org/licenses/gpl.txt // -//***************************************************************************** -//@{ +// ***************************************************************************** +// @{ #ifndef BUFFER_HPP #define BUFFER_HPP // structure/typdefs -//! cBuffer structure -typedef struct struct_cBuffer -{ - unsigned char *dataptr; ///< the physical memory address where the buffer is stored - unsigned short size; ///< the allocated size of the buffer - unsigned short datalength; ///< the length of the data currently in the buffer - unsigned short dataindex; ///< the index into the buffer where the data starts +// ! cBuffer structure +typedef struct struct_cBuffer { + unsigned char *dataptr; ///< the physical memory address where the buffer is stored + unsigned short size; ///< the allocated size of the buffer + unsigned short datalength; ///< the length of the data currently in the buffer + unsigned short dataindex; ///< the index into the buffer where the data starts } cBuffer; // function prototypes -//! initialize a buffer to start at a given address and have given size -void bufferInit(cBuffer* buffer, unsigned char *start, unsigned short size); +// ! initialize a buffer to start at a given address and have given size +void bufferInit(cBuffer *buffer, unsigned char *start, unsigned short size); -//! get the first byte from the front of the buffer -unsigned char bufferGetFromFront(cBuffer* buffer); +// ! get the first byte from the front of the buffer +unsigned char bufferGetFromFront(cBuffer *buffer); -//! dump (discard) the first numbytes from the front of the buffer -void bufferDumpFromFront(cBuffer* buffer, unsigned short numbytes); +// ! dump (discard) the first numbytes from the front of the buffer +void bufferDumpFromFront(cBuffer *buffer, unsigned short numbytes); -//! get a byte at the specified index in the buffer (kind of like array access) +// ! get a byte at the specified index in the buffer (kind of like array access) // ** note: this does not remove the byte that was read from the buffer -unsigned char bufferGetAtIndex(cBuffer* buffer, unsigned short index); +unsigned char bufferGetAtIndex(cBuffer *buffer, unsigned short index); -//! add a byte to the end of the buffer -unsigned char bufferAddToEnd(cBuffer* buffer, unsigned char data); +// ! add a byte to the end of the buffer +unsigned char bufferAddToEnd(cBuffer *buffer, unsigned char data); -//! check if the buffer is full/not full (returns non-zero value if not full) -unsigned char bufferIsNotFull(cBuffer* buffer); +// ! check if the buffer is full/not full (returns non-zero value if not full) +unsigned char bufferIsNotFull(cBuffer *buffer); -//! flush (clear) the contents of the buffer -void bufferFlush(cBuffer* buffer); +// ! flush (clear) the contents of the buffer +void bufferFlush(cBuffer *buffer); -#endif -//@} +#endif // ifndef BUFFER_HPP +// @} diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp index e70f2ed72..2bf058844 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp @@ -35,7 +35,6 @@ */ GpsConstellationWidget::GpsConstellationWidget(QWidget *parent) : QGraphicsView(parent) { - // Create a layout, add a QGraphicsView and put the SVG inside. // The constellation widget looks like this: // |--------------------| @@ -64,7 +63,7 @@ GpsConstellationWidget::GpsConstellationWidget(QWidget *parent) : QGraphicsView( setScene(scene); // Now create 'maxSatellites' satellite icons which we will move around on the map: - for (int i=0; i < MAX_SATTELITES;i++) { + for (int i = 0; i < MAX_SATTELITES; i++) { satellites[i][0] = 0; satellites[i][1] = 0; satellites[i][2] = 0; @@ -75,7 +74,7 @@ GpsConstellationWidget::GpsConstellationWidget(QWidget *parent) : QGraphicsView( satIcons[i]->setElementId("sat-notSeen"); satIcons[i]->hide(); - satTexts[i] = new QGraphicsSimpleTextItem("##",satIcons[i]); + satTexts[i] = new QGraphicsSimpleTextItem("##", satIcons[i]); satTexts[i]->setBrush(QColor("Black")); satTexts[i]->setFont(QFont("Courier")); } @@ -86,8 +85,8 @@ GpsConstellationWidget::~GpsConstellationWidget() delete scene; scene = 0; - //delete renderer; - //renderer = 0; + // delete renderer; + // renderer = 0; } void GpsConstellationWidget::showEvent(QShowEvent *event) @@ -98,13 +97,12 @@ void GpsConstellationWidget::showEvent(QShowEvent *event) // the result is usually a ahrsbargraph that is way too small. fitInView(world, Qt::KeepAspectRatio); // Scale, can't use fitInView since that doesn't work until we're shown. - // qreal factor = height()/world->boundingRect().height(); -// world->setScale(factor); - // setSceneRect(world->boundingRect()); - + // qreal factor = height()/world->boundingRect().height(); +// world->setScale(factor); +// setSceneRect(world->boundingRect()); } -void GpsConstellationWidget::resizeEvent(QResizeEvent* event) +void GpsConstellationWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); fitInView(world, Qt::KeepAspectRatio); @@ -124,10 +122,10 @@ void GpsConstellationWidget::updateSat(int index, int prn, int elevation, int az satellites[index][3] = snr; if (prn && elevation >= 0) { - QPointF opd = polarToCoord(elevation,azimuth); + QPointF opd = polarToCoord(elevation, azimuth); opd += QPointF(-satIcons[index]->boundingRect().center().x(), -satIcons[index]->boundingRect().center().y()); - satIcons[index]->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false); + satIcons[index]->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), false); if (snr) { satIcons[index]->setElementId("satellite"); } else { @@ -135,30 +133,29 @@ void GpsConstellationWidget::updateSat(int index, int prn, int elevation, int az } satIcons[index]->show(); - QRectF iconRect = satIcons[index]->boundingRect(); + QRectF iconRect = satIcons[index]->boundingRect(); QString prnString = QString().number(prn); - if(prnString.length() == 1) { + if (prnString.length() == 1) { prnString = "0" + prnString; } satTexts[index]->setText(prnString); QRectF textRect = satTexts[index]->boundingRect(); QTransform matrix; - qreal scale = 0.70 * (iconRect.width() / textRect.width()); - matrix.translate(iconRect.width()/2, iconRect.height()/2); - matrix.scale(scale,scale); - matrix.translate(-textRect.width()/2,-textRect.height()/2); - satTexts[index]->setTransform(matrix,false); + qreal scale = 0.70 * (iconRect.width() / textRect.width()); + matrix.translate(iconRect.width() / 2, iconRect.height() / 2); + matrix.scale(scale, scale); + matrix.translate(-textRect.width() / 2, -textRect.height() / 2); + satTexts[index]->setTransform(matrix, false); } else { satIcons[index]->hide(); } - } /** - Converts the elevation/azimuth to X/Y coordinates on the map + Converts the elevation/azimuth to X/Y coordinates on the map - */ + */ QPointF GpsConstellationWidget::polarToCoord(int elevation, int azimuth) { double x; @@ -167,18 +164,17 @@ QPointF GpsConstellationWidget::polarToCoord(int elevation, int azimuth) double rad_azimuth; - rad_elevation = M_PI*elevation/180; - rad_azimuth = M_PI*azimuth/180; + rad_elevation = M_PI * elevation / 180; + rad_azimuth = M_PI * azimuth / 180; - x = cos(rad_elevation)*sin(rad_azimuth); - y = -cos(rad_elevation)*cos(rad_azimuth); + x = cos(rad_elevation) * sin(rad_azimuth); + y = -cos(rad_elevation) * cos(rad_azimuth); - x = world->boundingRect().width()/2 * x; - y = world->boundingRect().height()/2 * y; + x = world->boundingRect().width() / 2 * x; + y = world->boundingRect().height() / 2 * y; x = (world->boundingRect().width() / 2) + x; y = (world->boundingRect().height() / 2) + y; - return QPointF(x,y); - + return QPointF(x, y); } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.h index 6fc6e8543..b8b1424a5 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.h @@ -33,35 +33,32 @@ #include -class GpsConstellationWidget : public QGraphicsView -{ +class GpsConstellationWidget : public QGraphicsView { Q_OBJECT public: - explicit GpsConstellationWidget(QWidget *parent = 0); - ~GpsConstellationWidget(); + explicit GpsConstellationWidget(QWidget *parent = 0); + ~GpsConstellationWidget(); public slots: - void updateSat(int index, int prn, int elevation, int azimuth, int snr); + void updateSat(int index, int prn, int elevation, int azimuth, int snr); private slots: private: - static const int MAX_SATTELITES = 16; - int satellites[MAX_SATTELITES][4]; - QGraphicsScene *scene; - QSvgRenderer *renderer; - QGraphicsSvgItem* world; - QGraphicsSvgItem* satIcons[MAX_SATTELITES]; - QGraphicsSimpleTextItem* satTexts[MAX_SATTELITES]; + static const int MAX_SATTELITES = 16; + int satellites[MAX_SATTELITES][4]; + QGraphicsScene *scene; + QSvgRenderer *renderer; + QGraphicsSvgItem *world; + QGraphicsSvgItem *satIcons[MAX_SATTELITES]; + QGraphicsSimpleTextItem *satTexts[MAX_SATTELITES]; - QPointF polarToCoord(int elevation, int azimuth); + QPointF polarToCoord(int elevation, int azimuth); protected: void showEvent(QShowEvent *event); void resizeEvent(QResizeEvent *event); - - }; #endif /* GPSCONSTELLATIONWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.cpp index 7d8f6eb9e..75167ebf7 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,12 +30,12 @@ #include "gpsdisplaygadgetconfiguration.h" GpsDisplayGadget::GpsDisplayGadget(QString classId, GpsDisplayWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget), - connected(FALSE) + IUAVGadget(classId, parent), + m_widget(widget), + connected(FALSE) { - connect(m_widget->connectButton, SIGNAL(clicked(bool)), this,SLOT(onConnect())); - connect(m_widget->disconnectButton, SIGNAL(clicked(bool)), this,SLOT(onDisconnect())); + connect(m_widget->connectButton, SIGNAL(clicked(bool)), this, SLOT(onConnect())); + connect(m_widget->disconnectButton, SIGNAL(clicked(bool)), this, SLOT(onDisconnect())); } GpsDisplayGadget::~GpsDisplayGadget() @@ -44,49 +44,48 @@ GpsDisplayGadget::~GpsDisplayGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void GpsDisplayGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void GpsDisplayGadget::loadConfiguration(IUAVGadgetConfiguration *config) { // Delete the (old)port, this also closes it. - if(port) { + if (port) { delete port; } // Delete the (old)parser, this also disconnects all signals. - if(parser) { + if (parser) { delete parser; } - GpsDisplayGadgetConfiguration *gpsDisplayConfig = qobject_cast< GpsDisplayGadgetConfiguration*>(config); + GpsDisplayGadgetConfiguration *gpsDisplayConfig = qobject_cast< GpsDisplayGadgetConfiguration *>(config); if (gpsDisplayConfig->connectionMode() == "Serial") { PortSettings portsettings; - portsettings.BaudRate=gpsDisplayConfig->speed(); - portsettings.DataBits=gpsDisplayConfig->dataBits(); - portsettings.FlowControl=gpsDisplayConfig->flow(); - portsettings.Parity=gpsDisplayConfig->parity(); - portsettings.StopBits=gpsDisplayConfig->stopBits(); - portsettings.Timeout_Millisec=gpsDisplayConfig->timeOut(); + portsettings.BaudRate = gpsDisplayConfig->speed(); + portsettings.DataBits = gpsDisplayConfig->dataBits(); + portsettings.FlowControl = gpsDisplayConfig->flow(); + portsettings.Parity = gpsDisplayConfig->parity(); + portsettings.StopBits = gpsDisplayConfig->stopBits(); + portsettings.Timeout_Millisec = gpsDisplayConfig->timeOut(); // In case we find no port, buttons disabled m_widget->connectButton->setEnabled(false); m_widget->disconnectButton->setEnabled(false); QList ports = QextSerialEnumerator::getPorts(); - foreach( QextPortInfo nport, ports ) { - if(nport.friendName == gpsDisplayConfig->port()) - { + foreach(QextPortInfo nport, ports) { + if (nport.friendName == gpsDisplayConfig->port()) { qDebug() << "Using Serial parser"; parser = new NMEAParser(); #ifdef Q_OS_WIN - port=new QextSerialPort(nport.portName,portsettings,QextSerialPort::EventDriven); + port = new QextSerialPort(nport.portName, portsettings, QextSerialPort::EventDriven); #else - port=new QextSerialPort(nport.physName,portsettings,QextSerialPort::EventDriven); + port = new QextSerialPort(nport.physName, portsettings, QextSerialPort::EventDriven); #endif m_widget->connectButton->setEnabled(true); m_widget->disconnectButton->setEnabled(false); @@ -104,44 +103,45 @@ void GpsDisplayGadget::loadConfiguration(IUAVGadgetConfiguration* config) m_widget->connectButton->setHidden(true); m_widget->dataStreamGroupBox->setHidden(true); } else if (gpsDisplayConfig->connectionMode() == "Network") { - // Not implemented for now... + // Not implemented for now... m_widget->connectButton->setEnabled(false); m_widget->disconnectButton->setEnabled(false); m_widget->dataStreamGroupBox->setHidden(false); } - connect(parser, SIGNAL(sv(int)), m_widget,SLOT(setSVs(int))); - connect(parser, SIGNAL(position(double,double,double)), m_widget,SLOT(setPosition(double,double,double))); - connect(parser, SIGNAL(speedheading(double,double)), m_widget,SLOT(setSpeedHeading(double,double))); - connect(parser, SIGNAL(datetime(double,double)), m_widget,SLOT(setDateTime(double,double))); + connect(parser, SIGNAL(sv(int)), m_widget, SLOT(setSVs(int))); + connect(parser, SIGNAL(position(double, double, double)), m_widget, SLOT(setPosition(double, double, double))); + connect(parser, SIGNAL(speedheading(double, double)), m_widget, SLOT(setSpeedHeading(double, double))); + connect(parser, SIGNAL(datetime(double, double)), m_widget, SLOT(setDateTime(double, double))); connect(parser, SIGNAL(packet(QString)), m_widget, SLOT(dumpPacket(QString))); - connect(parser, SIGNAL(satellite(int,int,int,int,int)), m_widget->gpsSky, SLOT(updateSat(int,int,int,int,int))); - connect(parser, SIGNAL(satellite(int,int,int,int,int)), m_widget->gpsSnrWidget, SLOT(updateSat(int,int,int,int,int))); + connect(parser, SIGNAL(satellite(int, int, int, int, int)), m_widget->gpsSky, SLOT(updateSat(int, int, int, int, int))); + connect(parser, SIGNAL(satellite(int, int, int, int, int)), m_widget->gpsSnrWidget, SLOT(updateSat(int, int, int, int, int))); connect(parser, SIGNAL(fixtype(QString)), m_widget, SLOT(setFixType(QString))); - connect(parser, SIGNAL(dop(double,double,double)), m_widget, SLOT(setDOP(double,double,double))); + connect(parser, SIGNAL(dop(double, double, double)), m_widget, SLOT(setDOP(double, double, double))); } -void GpsDisplayGadget::onConnect() { +void GpsDisplayGadget::onConnect() +{ m_widget->textBrowser->append(QString("Connecting to GPS ...\n")); // TODO: Somehow mark that we're running, and disable connect button while so? if (port) { - qDebug() << "Opening: " << port->portName() << "."; - bool isOpen = port->open(QIODevice::ReadWrite); - qDebug() << "Open: " << isOpen; - if(isOpen) { + qDebug() << "Opening: " << port->portName() << "."; + bool isOpen = port->open(QIODevice::ReadWrite); + qDebug() << "Open: " << isOpen; + if (isOpen) { m_widget->connectButton->setEnabled(false); m_widget->disconnectButton->setEnabled(true); } } else { qDebug() << "Port undefined or invalid."; } - } -void GpsDisplayGadget::onDisconnect() { +void GpsDisplayGadget::onDisconnect() +{ if (port) { - qDebug() << "Closing: " << port->portName() << "."; + qDebug() << "Closing: " << port->portName() << "."; port->close(); m_widget->connectButton->setEnabled(true); m_widget->disconnectButton->setEnabled(false); @@ -150,23 +150,26 @@ void GpsDisplayGadget::onDisconnect() { } } -void GpsDisplayGadget::onDataAvailable() { +void GpsDisplayGadget::onDataAvailable() +{ int avail = port->bytesAvailable(); - if( avail > 0 ) { + + if (avail > 0) { QByteArray serialData; serialData.resize(avail); int bytesRead = port->read(serialData.data(), serialData.size()); - if( bytesRead > 0 ) { + if (bytesRead > 0) { processNewSerialData(serialData); } } } -void GpsDisplayGadget::processNewSerialData(QByteArray serialData) { - int dataLength = serialData.size(); - const char* data = serialData.constData(); +void GpsDisplayGadget::processNewSerialData(QByteArray serialData) +{ + int dataLength = serialData.size(); + const char *data = serialData.constData(); - for(int pos = 0; pos < dataLength; pos++) { + for (int pos = 0; pos < dataLength; pos++) { parser->processInputStream(data[pos]); } } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.h index 69d5607da..c0dd90b64 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -42,18 +42,20 @@ class GpsDisplayWidget; using namespace Core; -class GpsDisplayGadget : public Core::IUAVGadget -{ +class GpsDisplayGadget : public Core::IUAVGadget { Q_OBJECT public: GpsDisplayGadget(QString classId, GpsDisplayWidget *widget, QWidget *parent = 0); ~GpsDisplayGadget(); - QWidget *widget() { return m_widget; } + QWidget *widget() + { + return m_widget; + } - // void setMode(QString mode); // Either UAVTalk or serial port + // void setMode(QString mode); // Either UAVTalk or serial port - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); public slots: void onConnect(); void onDisconnect(); diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.cpp index 404aa36a3..d1baaba25 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,7 +32,7 @@ * Loads a saved configuration or defaults if non exist. * */ -GpsDisplayGadgetConfiguration::GpsDisplayGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +GpsDisplayGadgetConfiguration::GpsDisplayGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_connectionMode("Serial"), m_defaultPort("Unknown"), @@ -43,36 +43,35 @@ GpsDisplayGadgetConfiguration::GpsDisplayGadgetConfiguration(QString classId, QS m_defaultStopBits(STOP_1), m_defaultTimeOut(5000) { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { BaudRateType speed; DataBitsType databits; FlowType flow; ParityType parity; StopBitsType stopbits; - int ispeed = qSettings->value("defaultSpeed").toInt(); - int idatabits = qSettings->value("defaultDataBits").toInt(); - int iflow = qSettings->value("defaultFlow").toInt(); - int iparity = qSettings->value("defaultParity").toInt(); - int istopbits = qSettings->value("defaultStopBits").toInt(); - QString port = qSettings->value("defaultPort").toString(); + int ispeed = qSettings->value("defaultSpeed").toInt(); + int idatabits = qSettings->value("defaultDataBits").toInt(); + int iflow = qSettings->value("defaultFlow").toInt(); + int iparity = qSettings->value("defaultParity").toInt(); + int istopbits = qSettings->value("defaultStopBits").toInt(); + QString port = qSettings->value("defaultPort").toString(); QString conMode = qSettings->value("connectionMode").toString(); - databits = (DataBitsType) idatabits; - flow = (FlowType)iflow; - parity = (ParityType)iparity; + databits = (DataBitsType)idatabits; + flow = (FlowType)iflow; + parity = (ParityType)iparity; stopbits = (StopBitsType)istopbits; - speed = (BaudRateType)ispeed; - m_defaultPort = port; - m_defaultSpeed = speed; + speed = (BaudRateType)ispeed; + m_defaultPort = port; + m_defaultSpeed = speed; m_defaultDataBits = databits; - m_defaultFlow = flow; - m_defaultParity = parity; + m_defaultFlow = flow; + m_defaultParity = parity; m_defaultStopBits = stopbits; - m_connectionMode = conMode; + m_connectionMode = conMode; } - } /** @@ -83,13 +82,13 @@ IUAVGadgetConfiguration *GpsDisplayGadgetConfiguration::clone() { GpsDisplayGadgetConfiguration *m = new GpsDisplayGadgetConfiguration(this->classId()); - m->m_defaultSpeed = m_defaultSpeed; + m->m_defaultSpeed = m_defaultSpeed; m->m_defaultDataBits = m_defaultDataBits; - m->m_defaultFlow = m_defaultFlow; - m->m_defaultParity = m_defaultParity; + m->m_defaultFlow = m_defaultFlow; + m->m_defaultParity = m_defaultParity; m->m_defaultStopBits = m_defaultStopBits; - m->m_defaultPort = m_defaultPort; - m->m_connectionMode = m_connectionMode; + m->m_defaultPort = m_defaultPort; + m->m_connectionMode = m_connectionMode; return m; } @@ -97,13 +96,13 @@ IUAVGadgetConfiguration *GpsDisplayGadgetConfiguration::clone() * Saves a configuration. * */ -void GpsDisplayGadgetConfiguration::saveConfig(QSettings* settings) const { - settings->setValue("defaultSpeed", m_defaultSpeed); - settings->setValue("defaultDataBits", m_defaultDataBits); - settings->setValue("defaultFlow", m_defaultFlow); - settings->setValue("defaultParity", m_defaultParity); - settings->setValue("defaultStopBits", m_defaultStopBits); - settings->setValue("defaultPort", m_defaultPort); - settings->setValue("connectionMode", m_connectionMode); - +void GpsDisplayGadgetConfiguration::saveConfig(QSettings *settings) const +{ + settings->setValue("defaultSpeed", m_defaultSpeed); + settings->setValue("defaultDataBits", m_defaultDataBits); + settings->setValue("defaultFlow", m_defaultFlow); + settings->setValue("defaultParity", m_defaultParity); + settings->setValue("defaultStopBits", m_defaultStopBits); + settings->setValue("defaultPort", m_defaultPort); + settings->setValue("connectionMode", m_connectionMode); } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.h index 54fede7e8..3fdd94b64 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,46 +33,92 @@ using namespace Core; -class GpsDisplayGadgetConfiguration : public IUAVGadgetConfiguration -{ +class GpsDisplayGadgetConfiguration : public IUAVGadgetConfiguration { Q_OBJECT - public: - explicit GpsDisplayGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); +public: + explicit GpsDisplayGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void setConnectionMode(QString mode) { m_connectionMode = mode; } - QString connectionMode() { return m_connectionMode; } + void setConnectionMode(QString mode) + { + m_connectionMode = mode; + } + QString connectionMode() + { + return m_connectionMode; + } - //set port configuration functions - void setSpeed(BaudRateType speed) {m_defaultSpeed=speed;} - void setDataBits(DataBitsType databits) {m_defaultDataBits=databits;} - void setFlow(FlowType flow) {m_defaultFlow=flow;} - void setParity(ParityType parity) {m_defaultParity=parity;} - void setStopBits(StopBitsType stopbits) {m_defaultStopBits=stopbits;} - void setPort(QString port){m_defaultPort=port;} - void setTimeOut(long timeout){m_defaultTimeOut=timeout;} + // set port configuration functions + void setSpeed(BaudRateType speed) + { + m_defaultSpeed = speed; + } + void setDataBits(DataBitsType databits) + { + m_defaultDataBits = databits; + } + void setFlow(FlowType flow) + { + m_defaultFlow = flow; + } + void setParity(ParityType parity) + { + m_defaultParity = parity; + } + void setStopBits(StopBitsType stopbits) + { + m_defaultStopBits = stopbits; + } + void setPort(QString port) + { + m_defaultPort = port; + } + void setTimeOut(long timeout) + { + m_defaultTimeOut = timeout; + } - //get port configuration functions - QString port(){return m_defaultPort;} - BaudRateType speed() {return m_defaultSpeed;} - FlowType flow() {return m_defaultFlow;} - DataBitsType dataBits() {return m_defaultDataBits;} - StopBitsType stopBits() {return m_defaultStopBits;} - ParityType parity() {return m_defaultParity;} - long timeOut(){return m_defaultTimeOut;} + // get port configuration functions + QString port() + { + return m_defaultPort; + } + BaudRateType speed() + { + return m_defaultSpeed; + } + FlowType flow() + { + return m_defaultFlow; + } + DataBitsType dataBits() + { + return m_defaultDataBits; + } + StopBitsType stopBits() + { + return m_defaultStopBits; + } + ParityType parity() + { + return m_defaultParity; + } + long timeOut() + { + return m_defaultTimeOut; + } - void saveConfig(QSettings* settings) const; - IUAVGadgetConfiguration *clone(); - - private: - QString m_connectionMode; - QString m_defaultPort; - BaudRateType m_defaultSpeed; - DataBitsType m_defaultDataBits; - FlowType m_defaultFlow; - ParityType m_defaultParity; - StopBitsType m_defaultStopBits; - long m_defaultTimeOut; + void saveConfig(QSettings *settings) const; + IUAVGadgetConfiguration *clone(); +private: + QString m_connectionMode; + QString m_defaultPort; + BaudRateType m_defaultSpeed; + DataBitsType m_defaultDataBits; + FlowType m_defaultFlow; + ParityType m_defaultParity; + StopBitsType m_defaultStopBits; + long m_defaultTimeOut; }; #endif // GPSDISPLAYGADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.cpp index bedf86c19..2b97864d9 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,29 +32,27 @@ #include GpsDisplayGadgetFactory::GpsDisplayGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("GpsDisplayGadget"), - tr("GPS Display"), - parent) -{ -} + IUAVGadgetFactory(QString("GpsDisplayGadget"), + tr("GPS Display"), + parent) +{} GpsDisplayGadgetFactory::~GpsDisplayGadgetFactory() -{ -} +{} -Core::IUAVGadget* GpsDisplayGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *GpsDisplayGadgetFactory::createGadget(QWidget *parent) { - GpsDisplayWidget* gadgetWidget = new GpsDisplayWidget(parent); + GpsDisplayWidget *gadgetWidget = new GpsDisplayWidget(parent); + return new GpsDisplayGadget(QString("GpsDisplayGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *GpsDisplayGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *GpsDisplayGadgetFactory::createConfiguration(QSettings *qSettings) { return new GpsDisplayGadgetConfiguration(QString("GpsDisplayGadget"), qSettings); } IOptionsPage *GpsDisplayGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new GpsDisplayGadgetOptionsPage(qobject_cast(config)); + return new GpsDisplayGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.h index 54ce96d55..99bb20e50 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class GpsDisplayGadgetFactory : public IUAVGadgetFactory -{ +class GpsDisplayGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: GpsDisplayGadgetFactory(QObject *parent = 0); ~GpsDisplayGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.cpp index 4ec271c57..be8646636 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,135 +34,135 @@ #include GpsDisplayGadgetOptionsPage::GpsDisplayGadgetOptionsPage(GpsDisplayGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) + IOptionsPage(parent), + m_config(config) { -// Taken from the uploader gadget, since we also can use a serial port for this -// Gadget +// Taken from the uploader gadget, since we also can use a serial port for this +// Gadget - //the begining of some ugly code -//diferent OS's have diferent serial port capabilities + // the begining of some ugly code +// diferent OS's have diferent serial port capabilities #ifdef Q_OS_WIN -//load windows port capabilities -BaudRateTypeString - <<"BAUD110" - <<"BAUD300" - <<"BAUD600" - <<"BAUD1200" - <<"BAUD2400" - <<"BAUD4800" - <<"BAUD9600" - <<"BAUD14400" - <<"BAUD19200" - <<"BAUD38400" - <<"BAUD56000" - <<"BAUD57600" - <<"BAUD115200" - <<"BAUD128000" - <<"BAUD256000"; -DataBitsTypeString - <<"DATA_5" - <<"DATA_6" - <<"DATA_7" - <<"DATA_8"; -ParityTypeString - <<"PAR_NONE" - <<"PAR_ODD" - <<"PAR_EVEN" - <<"PAR_MARK" //WINDOWS ONLY - <<"PAR_SPACE"; -StopBitsTypeString - <<"STOP_1" - <<"STOP_1_5" //WINDOWS ONLY - <<"STOP_2"; -#else -//load POSIX port capabilities -BaudRateTypeString +// load windows port capabilities + BaudRateTypeString + << "BAUD110" + << "BAUD300" + << "BAUD600" + << "BAUD1200" + << "BAUD2400" + << "BAUD4800" + << "BAUD9600" + << "BAUD14400" + << "BAUD19200" + << "BAUD38400" + << "BAUD56000" + << "BAUD57600" + << "BAUD115200" + << "BAUD128000" + << "BAUD256000"; + DataBitsTypeString + << "DATA_5" + << "DATA_6" + << "DATA_7" + << "DATA_8"; + ParityTypeString + << "PAR_NONE" + << "PAR_ODD" + << "PAR_EVEN" + << "PAR_MARK" // WINDOWS ONLY + << "PAR_SPACE"; + StopBitsTypeString + << "STOP_1" + << "STOP_1_5" // WINDOWS ONLY + << "STOP_2"; +#else // ifdef Q_OS_WIN +// load POSIX port capabilities + BaudRateTypeString - <<"BAUD50" //POSIX ONLY - <<"BAUD75" //POSIX ONLY - <<"BAUD110" - <<"BAUD134" //POSIX ONLY - <<"BAUD150" //POSIX ONLY - <<"BAUD200" //POSIX ONLY - <<"BAUD300" - <<"BAUD600" - <<"BAUD1200" - <<"BAUD1800" //POSIX ONLY - <<"BAUD2400" - <<"BAUD4800" - <<"BAUD9600" - <<"BAUD19200" - <<"BAUD38400" - <<"BAUD57600" - <<"BAUD76800" //POSIX ONLY - <<"BAUD115200"; -DataBitsTypeString - <<"DATA_5" - <<"DATA_6" - <<"DATA_7" - <<"DATA_8"; -ParityTypeString - <<"PAR_NONE" - <<"PAR_ODD" - <<"PAR_EVEN" - <<"PAR_SPACE"; -StopBitsTypeString - <<"STOP_1" - <<"STOP_2"; -#endif -//load all OS's capabilities -BaudRateTypeStringALL - <<"BAUD50" //POSIX ONLY - <<"BAUD75" //POSIX ONLY - <<"BAUD110" - <<"BAUD134" //POSIX ONLY - <<"BAUD150" //POSIX ONLY - <<"BAUD200" //POSIX ONLY - <<"BAUD300" - <<"BAUD600" - <<"BAUD1200" - <<"BAUD1800" //POSIX ONLY - <<"BAUD2400" - <<"BAUD4800" - <<"BAUD9600" - <<"BAUD14400" - <<"BAUD19200" - <<"BAUD38400" - <<"BAUD56000" - <<"BAUD57600" - <<"BAUD76800" //POSIX ONLY - <<"BAUD115200" - <<"BAUD128000" - <<"BAUD256000"; -DataBitsTypeStringALL - <<"DATA_5" - <<"DATA_6" - <<"DATA_7" - <<"DATA_8"; -ParityTypeStringALL - <<"PAR_NONE" - <<"PAR_ODD" - <<"PAR_EVEN" - <<"PAR_MARK" //WINDOWS ONLY - <<"PAR_SPACE"; -StopBitsTypeStringALL - <<"STOP_1" - <<"STOP_1_5" //WINDOWS ONLY - <<"STOP_2"; + << "BAUD50" // POSIX ONLY + << "BAUD75" // POSIX ONLY + << "BAUD110" + << "BAUD134" // POSIX ONLY + << "BAUD150" // POSIX ONLY + << "BAUD200" // POSIX ONLY + << "BAUD300" + << "BAUD600" + << "BAUD1200" + << "BAUD1800" // POSIX ONLY + << "BAUD2400" + << "BAUD4800" + << "BAUD9600" + << "BAUD19200" + << "BAUD38400" + << "BAUD57600" + << "BAUD76800" // POSIX ONLY + << "BAUD115200"; + DataBitsTypeString + << "DATA_5" + << "DATA_6" + << "DATA_7" + << "DATA_8"; + ParityTypeString + << "PAR_NONE" + << "PAR_ODD" + << "PAR_EVEN" + << "PAR_SPACE"; + StopBitsTypeString + << "STOP_1" + << "STOP_2"; +#endif // ifdef Q_OS_WIN +// load all OS's capabilities + BaudRateTypeStringALL + << "BAUD50" // POSIX ONLY + << "BAUD75" // POSIX ONLY + << "BAUD110" + << "BAUD134" // POSIX ONLY + << "BAUD150" // POSIX ONLY + << "BAUD200" // POSIX ONLY + << "BAUD300" + << "BAUD600" + << "BAUD1200" + << "BAUD1800" // POSIX ONLY + << "BAUD2400" + << "BAUD4800" + << "BAUD9600" + << "BAUD14400" + << "BAUD19200" + << "BAUD38400" + << "BAUD56000" + << "BAUD57600" + << "BAUD76800" // POSIX ONLY + << "BAUD115200" + << "BAUD128000" + << "BAUD256000"; + DataBitsTypeStringALL + << "DATA_5" + << "DATA_6" + << "DATA_7" + << "DATA_8"; + ParityTypeStringALL + << "PAR_NONE" + << "PAR_ODD" + << "PAR_EVEN" + << "PAR_MARK" // WINDOWS ONLY + << "PAR_SPACE"; + StopBitsTypeStringALL + << "STOP_1" + << "STOP_1_5" // WINDOWS ONLY + << "STOP_2"; -FlowTypeString - <<"FLOW_OFF" - <<"FLOW_HARDWARE" - <<"FLOW_XONXOFF"; + FlowTypeString + << "FLOW_OFF" + << "FLOW_HARDWARE" + << "FLOW_XONXOFF"; } -bool sortPorts(QextPortInfo const& s1,QextPortInfo const& s2) +bool sortPorts(QextPortInfo const & s1, QextPortInfo const & s2) { -return s1.portName ports = QextSerialEnumerator::getPorts(); - qSort(ports.begin(), ports.end(),sortPorts); - foreach( QextPortInfo port, ports ) { + qSort(ports.begin(), ports.end(), sortPorts); + foreach(QextPortInfo port, ports) { qDebug() << "Adding port: " << port.friendName << " (" << port.portName << ")"; options_page->portComboBox->addItem(port.friendName, port.friendName); } int portIndex = options_page->portComboBox->findData(m_config->port()); - if(portIndex!=-1){ + if (portIndex != -1) { qDebug() << "createPage(): port is " << m_config->port(); options_page->portComboBox->setCurrentIndex(portIndex); } @@ -190,40 +190,40 @@ QWidget *GpsDisplayGadgetOptionsPage::createPage(QWidget *parent) options_page->portSpeedComboBox->addItems(BaudRateTypeString); int portSpeedIndex = options_page->portSpeedComboBox->findText(BaudRateTypeStringALL.at((int)m_config->speed())); - if(portSpeedIndex != -1){ - options_page->portSpeedComboBox->setCurrentIndex(portSpeedIndex); + if (portSpeedIndex != -1) { + options_page->portSpeedComboBox->setCurrentIndex(portSpeedIndex); } // FLOW CONTROL options_page->flowControlComboBox->addItems(FlowTypeString); int flowControlIndex = options_page->flowControlComboBox->findText(FlowTypeString.at((int)m_config->flow())); - if(flowControlIndex != -1){ - options_page->flowControlComboBox->setCurrentIndex(flowControlIndex); + if (flowControlIndex != -1) { + options_page->flowControlComboBox->setCurrentIndex(flowControlIndex); } // DATABITS options_page->dataBitsComboBox->addItems(DataBitsTypeString); int dataBitsIndex = options_page->dataBitsComboBox->findText(DataBitsTypeStringALL.at((int)m_config->dataBits())); - if(dataBitsIndex != -1){ - options_page->dataBitsComboBox->setCurrentIndex(dataBitsIndex); + if (dataBitsIndex != -1) { + options_page->dataBitsComboBox->setCurrentIndex(dataBitsIndex); } // STOPBITS options_page->stopBitsComboBox->addItems(StopBitsTypeString); int stopBitsIndex = options_page->stopBitsComboBox->findText(StopBitsTypeStringALL.at((int)m_config->stopBits())); - if(stopBitsIndex != -1){ - options_page->stopBitsComboBox->setCurrentIndex(stopBitsIndex); + if (stopBitsIndex != -1) { + options_page->stopBitsComboBox->setCurrentIndex(stopBitsIndex); } // PARITY options_page->parityComboBox->addItems(ParityTypeString); int parityIndex = options_page->parityComboBox->findText(ParityTypeStringALL.at((int)m_config->parity())); - if(parityIndex != -1){ - options_page->parityComboBox->setCurrentIndex(parityIndex); + if (parityIndex != -1) { + options_page->parityComboBox->setCurrentIndex(parityIndex); } // TIMEOUT @@ -233,8 +233,9 @@ QWidget *GpsDisplayGadgetOptionsPage::createPage(QWidget *parent) connectionModes << "Serial" << "Network" << "Telemetry"; options_page->connectionMode->addItems(connectionModes); int conMode = options_page->connectionMode->findText(m_config->connectionMode()); - if (conMode != -1) - options_page->connectionMode->setCurrentIndex(conMode); + if (conMode != -1) { + options_page->connectionMode->setCurrentIndex(conMode); + } return optionsPageWidget; @@ -249,6 +250,7 @@ QWidget *GpsDisplayGadgetOptionsPage::createPage(QWidget *parent) void GpsDisplayGadgetOptionsPage::apply() { int portIndex = options_page->portComboBox->currentIndex(); + m_config->setPort(options_page->portComboBox->itemData(portIndex).toString()); qDebug() << "apply(): port is " << m_config->port(); @@ -257,9 +259,8 @@ void GpsDisplayGadgetOptionsPage::apply() m_config->setDataBits((DataBitsType)DataBitsTypeStringALL.indexOf(options_page->dataBitsComboBox->currentText())); m_config->setStopBits((StopBitsType)StopBitsTypeStringALL.indexOf(options_page->stopBitsComboBox->currentText())); m_config->setParity((ParityType)ParityTypeStringALL.indexOf(options_page->parityComboBox->currentText())); - m_config->setTimeOut( options_page->timeoutSpinBox->value()); + m_config->setTimeOut(options_page->timeoutSpinBox->value()); m_config->setConnectionMode(options_page->connectionMode->currentText()); - } void GpsDisplayGadgetOptionsPage::finish() diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.h index b98af560a..a2fe2861f 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -41,14 +41,13 @@ class IUAVGadgetConfiguration; class GpsDisplayGadgetConfiguration; namespace Ui { - class GpsDisplayGadgetOptionsPage; +class GpsDisplayGadgetOptionsPage; } using namespace Core; -class GpsDisplayGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class GpsDisplayGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit GpsDisplayGadgetOptionsPage(GpsDisplayGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.cpp index 7f43e8fbb..0e2909bce 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,31 +35,31 @@ GpsDisplayPlugin::GpsDisplayPlugin() { - // Do nothing + // Do nothing } GpsDisplayPlugin::~GpsDisplayPlugin() { - // Do nothing + // Do nothing } -bool GpsDisplayPlugin::initialize(const QStringList& args, QString *errMsg) +bool GpsDisplayPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new GpsDisplayGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new GpsDisplayGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void GpsDisplayPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void GpsDisplayPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(GpsDisplayPlugin) diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.h index f1e9a4e90..f867c552d 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class GpsDisplayGadgetFactory; -class GpsDisplayPlugin : public ExtensionSystem::IPlugin -{ +class GpsDisplayPlugin : public ExtensionSystem::IPlugin { public: - GpsDisplayPlugin(); - ~GpsDisplayPlugin(); + GpsDisplayPlugin(); + ~GpsDisplayPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - GpsDisplayGadgetFactory *mf; + GpsDisplayGadgetFactory *mf; }; #endif /* GPSDISPLAYPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.cpp index bdc2684b0..67f30080a 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -41,52 +41,53 @@ GpsDisplayWidget::GpsDisplayWidget(QWidget *parent) : QWidget(parent) { setupUi(this); - //Not elegant, just load the image for now + // Not elegant, just load the image for now QGraphicsScene *fescene = new QGraphicsScene(this); - QPixmap earthpix( ":/gpsgadget/images/flatEarth.png" ); - fescene->addPixmap( earthpix ); + QPixmap earthpix(":/gpsgadget/images/flatEarth.png"); + fescene->addPixmap(earthpix); flatEarth->setScene(fescene); marker = new QGraphicsSvgItem(); QSvgRenderer *renderer = new QSvgRenderer(); renderer->load(QString(":/gpsgadget/images/marker.svg")); marker->setSharedRenderer(renderer); fescene->addItem(marker); - double scale = earthpix.width()/(marker->boundingRect().width()*20); + double scale = earthpix.width() / (marker->boundingRect().width() * 20); marker->setScale(scale); } GpsDisplayWidget::~GpsDisplayWidget() -{ -} +{} void GpsDisplayWidget::setSpeedHeading(double speed, double heading) { QString str; - speed_value->setText(str.sprintf("%.02f m/s",speed)); - bear_value->setText(str.sprintf("%.02f deg",heading)); + + speed_value->setText(str.sprintf("%.02f m/s", speed)); + bear_value->setText(str.sprintf("%.02f deg", heading)); } void GpsDisplayWidget::setDateTime(double date, double time) { - QString dstring1,dstring2; - dstring1.sprintf("%06.0f",date); - dstring1.insert(dstring1.length()-2,"."); - dstring1.insert(dstring1.length()-5,"."); - dstring2.sprintf("%06.0f",time); - dstring2.insert(dstring2.length()-2,":"); - dstring2.insert(dstring2.length()-5,":"); + QString dstring1, dstring2; + + dstring1.sprintf("%06.0f", date); + dstring1.insert(dstring1.length() - 2, "."); + dstring1.insert(dstring1.length() - 5, "."); + dstring2.sprintf("%06.0f", time); + dstring2.insert(dstring2.length() - 2, ":"); + dstring2.insert(dstring2.length() - 5, ":"); time_value->setText(dstring1 + " " + dstring2 + " GMT"); } void GpsDisplayWidget::setFixType(const QString &fixtype) { - if(fixtype =="NoGPS") { + if (fixtype == "NoGPS") { fix_value->setText("No GPS"); } else if (fixtype == "NoFix") { fix_value->setText("Fix not available"); } else if (fixtype == "Fix2D") { fix_value->setText("2D"); - } else if (fixtype =="Fix3D") { + } else if (fixtype == "Fix3D") { fix_value->setText("3D"); } else { fix_value->setText("Unknown"); @@ -96,7 +97,7 @@ void GpsDisplayWidget::setFixType(const QString &fixtype) void GpsDisplayWidget::dumpPacket(const QString &packet) { textBrowser->append(packet); - if(textBrowser->document()->lineCount() > 200) { + if (textBrowser->document()->lineCount() > 200) { QTextCursor tc = textBrowser->textCursor(); tc.movePosition(QTextCursor::Start); tc.movePosition(QTextCursor::Down, QTextCursor::KeepAnchor); @@ -108,6 +109,7 @@ void GpsDisplayWidget::dumpPacket(const QString &packet) void GpsDisplayWidget::setSVs(int sv) { QString temp; + temp.append(QString::number(sv)); status_value->setText(temp); status_value->adjustSize(); @@ -115,44 +117,45 @@ void GpsDisplayWidget::setSVs(int sv) void GpsDisplayWidget::setDOP(double hdop, double vdop, double pdop) { - QString str; + str.sprintf("%.2f / %.2f / %.2f", hdop, vdop, pdop); dop_value->setText(str); - } void GpsDisplayWidget::setPosition(double lat, double lon, double alt) { - //lat *= 1E-7; - //lon *= 1E-7; + // lat *= 1E-7; + // lon *= 1E-7; double deg = floor(fabs(lat)); - double min = (fabs(lat)-deg)*60; + double min = (fabs(lat) - deg) * 60; QString str1; - str1.sprintf("%.0f%c%.3f' ", deg,0x00b0, min); - if (lat>0) + + str1.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min); + if (lat > 0) { str1.append("N"); - else + } else { str1.append("S"); + } coord_value->setText(str1); deg = floor(fabs(lon)); - min = (fabs(lon)-deg)*60; + min = (fabs(lon) - deg) * 60; QString str2; - str2.sprintf("%.0f%c%.3f' ", deg,0x00b0, min); - if (lon>0) + str2.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min); + if (lon > 0) { str2.append("E"); - else + } else { str2.append("W"); + } coord_value_2->setText(str2); QString str3; str3.sprintf("%.2f m", alt); coord_value_3->setText(str3); // Now place the marker: - double wscale = flatEarth->sceneRect().width()/360; - double hscale = flatEarth->sceneRect().height()/180; - QPointF opd = QPointF((lon+180)*wscale-marker->boundingRect().width()*marker->scale()/2, - (90-lat)*hscale-marker->boundingRect().height()*marker->scale()/2); - marker->setTransform(QTransform::fromTranslate( opd.x(), opd.y()) , false); - + double wscale = flatEarth->sceneRect().width() / 360; + double hscale = flatEarth->sceneRect().height() / 180; + QPointF opd = QPointF((lon + 180) * wscale - marker->boundingRect().width() * marker->scale() / 2, + (90 - lat) * hscale - marker->boundingRect().height() * marker->scale() / 2); + marker->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), false); } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.h index 50873791e..a7be00809 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,25 +38,24 @@ class Ui_GpsDisplayWidget; -class GpsDisplayWidget : public QWidget, public Ui_GpsDisplayWidget -{ +class GpsDisplayWidget : public QWidget, public Ui_GpsDisplayWidget { Q_OBJECT public: GpsDisplayWidget(QWidget *parent = 0); - ~GpsDisplayWidget(); + ~GpsDisplayWidget(); private slots: - void setSVs(int); - void setPosition(double, double, double); - void setDateTime(double, double); - void setSpeedHeading(double, double); - void dumpPacket(const QString &packet); - void setFixType(const QString &fixtype); - void setDOP(double hdop, double vdop, double pdop); + void setSVs(int); + void setPosition(double, double, double); + void setDateTime(double, double); + void setSpeedHeading(double, double); + void dumpPacket(const QString &packet); + void setFixType(const QString &fixtype); + void setDOP(double hdop, double vdop, double pdop); private: - GpsConstellationWidget * gpsConstellation; - QGraphicsSvgItem * marker; + GpsConstellationWidget *gpsConstellation; + QGraphicsSvgItem *marker; }; #endif /* GPSDISPLAYWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.cpp index 8f1717f45..53060e5eb 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.cpp @@ -33,11 +33,11 @@ GPSParser::GPSParser(QObject *parent) : QObject(parent) } GPSParser::~GPSParser() -{ +{} -} - -void GPSParser::processInputStream(char c) { +void GPSParser::processInputStream(char c) { - Q_UNUSED(c)} + { + Q_UNUSED(c) + } } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.h index a980f689a..ae0479ccc 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.h @@ -32,27 +32,25 @@ #include #include -class GPSParser: public QObject -{ +class GPSParser : public QObject { Q_OBJECT -public: - ~GPSParser(); +public: ~GPSParser(); virtual void processInputStream(char c); protected: GPSParser(QObject *parent = 0); signals: - void sv(int); // Satellites in view - void position(double,double,double); // Lat, Lon, Alt - void datetime(double,double); // Date then time - void speedheading(double,double); - void packet(QString); // Raw NMEA Packet (or just info) - void satellite(int,int,int,int,int); // Index, PRN, Elevation, Azimuth, SNR - void fixmode(QString); // Mode of fix: "Auto", "Manual". - void fixtype(QString); // Type of fix: "NoGPS", "NoFix", "Fix2D", "Fix3D". - void dop(double, double, double); // HDOP, VDOP, PDOP - void fixSVs(QList); // SV's used for fix. + void sv(int); // Satellites in view + void position(double, double, double); // Lat, Lon, Alt + void datetime(double, double); // Date then time + void speedheading(double, double); + void packet(QString); // Raw NMEA Packet (or just info) + void satellite(int, int, int, int, int); // Index, PRN, Elevation, Azimuth, SNR + void fixmode(QString); // Mode of fix: "Auto", "Manual". + void fixtype(QString); // Type of fix: "NoGPS", "NoFix", "Fix2D", "Fix3D". + void dop(double, double, double); // HDOP, VDOP, PDOP + void fixSVs(QList); // SV's used for fix. }; #endif // GPSPARSER_H diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.cpp index 3edc51ee3..157a461d5 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.cpp @@ -1,13 +1,13 @@ #include "gpssnrwidget.h" GpsSnrWidget::GpsSnrWidget(QWidget *parent) : - QGraphicsView(parent) { - + QGraphicsView(parent) +{ scene = new QGraphicsScene(this); setScene(scene); // Now create 'maxSatellites' satellite icons which we will move around on the map: - for (int i=0; i < MAX_SATTELITES;i++) { + for (int i = 0; i < MAX_SATTELITES; i++) { satellites[i][0] = 0; satellites[i][1] = 0; satellites[i][2] = 0; @@ -18,40 +18,42 @@ GpsSnrWidget::GpsSnrWidget(QWidget *parent) : scene->addItem(boxes[i]); boxes[i]->hide(); - satTexts[i] = new QGraphicsSimpleTextItem("##",boxes[i]); + satTexts[i] = new QGraphicsSimpleTextItem("##", boxes[i]); satTexts[i]->setBrush(QColor("Black")); satTexts[i]->setFont(QFont("Courier")); - satSNRs[i] = new QGraphicsSimpleTextItem("##",boxes[i]); + satSNRs[i] = new QGraphicsSimpleTextItem("##", boxes[i]); satSNRs[i]->setBrush(QColor("Black")); satSNRs[i]->setFont(QFont("Courier")); - } - } -GpsSnrWidget::~GpsSnrWidget() { +GpsSnrWidget::~GpsSnrWidget() +{ delete scene; scene = 0; } -void GpsSnrWidget::showEvent(QShowEvent *event) { +void GpsSnrWidget::showEvent(QShowEvent *event) +{ Q_UNUSED(event) - scene->setSceneRect(0,0, this->viewport()->width(), this->viewport()->height()); - for(int index = 0 ;index < MAX_SATTELITES ; index++) { + scene->setSceneRect(0, 0, this->viewport()->width(), this->viewport()->height()); + for (int index = 0; index < MAX_SATTELITES; index++) { drawSat(index); } } -void GpsSnrWidget::resizeEvent(QResizeEvent* event) { +void GpsSnrWidget::resizeEvent(QResizeEvent *event) +{ Q_UNUSED(event); - scene->setSceneRect(0,0, this->viewport()->width(), this->viewport()->height()); - for(int index = 0 ;index < MAX_SATTELITES ; index++) { + scene->setSceneRect(0, 0, this->viewport()->width(), this->viewport()->height()); + for (int index = 0; index < MAX_SATTELITES; index++) { drawSat(index); } } -void GpsSnrWidget::updateSat(int index, int prn, int elevation, int azimuth, int snr) { +void GpsSnrWidget::updateSat(int index, int prn, int elevation, int azimuth, int snr) +{ if (index >= MAX_SATTELITES) { // A bit of error checking never hurts. return; @@ -66,7 +68,8 @@ void GpsSnrWidget::updateSat(int index, int prn, int elevation, int azimuth, int drawSat(index); } -void GpsSnrWidget::drawSat(int index) { +void GpsSnrWidget::drawSat(int index) +{ if (index >= MAX_SATTELITES) { // A bit of error checking never hurts. return; @@ -85,10 +88,10 @@ void GpsSnrWidget::drawSat(int index) { // Casting to int rounds down, which is what I want. // Minus 2 to allow a pixel of white left and right. - int availableWidth = (int)((scene->width()-2) / MAX_SATTELITES); + int availableWidth = (int)((scene->width() - 2) / MAX_SATTELITES); // 2 pixels, one on each side. - qreal width = availableWidth - 2; + qreal width = availableWidth - 2; // SNR = 1-99 (0 is special).. qreal height = int((scene->height() / 99) * snr + 0.5); // 1 for showing a pixel of white to the left. @@ -96,38 +99,37 @@ void GpsSnrWidget::drawSat(int index) { // Rember, 0 is at the top. qreal y = scene->height() - height; // Compensate for the extra pixel for the border. - boxes[index]->setRect(0,0,width-1,height-1); - boxes[index]->setPos(x,y); + boxes[index]->setRect(0, 0, width - 1, height - 1); + boxes[index]->setPos(x, y); - QRectF boxRect = boxes[index]->boundingRect(); + QRectF boxRect = boxes[index]->boundingRect(); QString prnString = QString().number(prn); - if(prnString.length() == 1) { + if (prnString.length() == 1) { prnString = "0" + prnString; } satTexts[index]->setText(prnString); QRectF textRect = satTexts[index]->boundingRect(); QTransform matrix; - qreal scale = 0.85 * (boxRect.width() / textRect.width()); - matrix.translate( boxRect.width()/2, boxRect.height()); - matrix.scale(scale,scale); - matrix.translate(-textRect.width()/2,-textRect.height()); - satTexts[index]->setTransform(matrix,false); + qreal scale = 0.85 * (boxRect.width() / textRect.width()); + matrix.translate(boxRect.width() / 2, boxRect.height()); + matrix.scale(scale, scale); + matrix.translate(-textRect.width() / 2, -textRect.height()); + satTexts[index]->setTransform(matrix, false); QString snrString = QString().number(snr); - if (snrString.length() ==1) { // Will probably never happen! + if (snrString.length() == 1) { // Will probably never happen! snrString = "0" + snrString; } satSNRs[index]->setText(snrString); textRect = satSNRs[index]->boundingRect(); matrix.reset(); - scale = 0.85 * (boxRect.width() / textRect.width()); - matrix.translate( boxRect.width()/2,0); - matrix.scale(scale,scale); - matrix.translate(-textRect.width()/2,-textRect.height()); - satSNRs[index]->setTransform(matrix,false); - + scale = 0.85 * (boxRect.width() / textRect.width()); + matrix.translate(boxRect.width() / 2, 0); + matrix.scale(scale, scale); + matrix.translate(-textRect.width() / 2, -textRect.height()); + satSNRs[index]->setTransform(matrix, false); } else { boxes[index]->hide(); } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.h index 7b7f9517c..6b8d2e7b8 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.h @@ -4,8 +4,7 @@ #include #include -class GpsSnrWidget : public QGraphicsView -{ +class GpsSnrWidget : public QGraphicsView { Q_OBJECT public: explicit GpsSnrWidget(QWidget *parent = 0); @@ -21,15 +20,14 @@ private: int satellites[MAX_SATTELITES][4]; QGraphicsScene *scene; QGraphicsRectItem *boxes[MAX_SATTELITES]; - QGraphicsSimpleTextItem* satTexts[MAX_SATTELITES]; - QGraphicsSimpleTextItem* satSNRs[MAX_SATTELITES]; + QGraphicsSimpleTextItem *satTexts[MAX_SATTELITES]; + QGraphicsSimpleTextItem *satSNRs[MAX_SATTELITES]; void drawSat(int index); protected: void showEvent(QShowEvent *event); void resizeEvent(QResizeEvent *event); - }; #endif // GPSSNRWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp index 735316496..af0193e7c 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp @@ -36,59 +36,56 @@ #include // Message Codes -#define NMEA_NODATA 0 // No data. Packet not available, bad, or not decoded -#define NMEA_GPGGA 1 // Global Positioning System Fix Data -#define NMEA_GPVTG 2 // Course over ground and ground speed -#define NMEA_GPGLL 3 // Geographic position - latitude/longitude -#define NMEA_GPGSV 4 // GPS satellites in view -#define NMEA_GPGSA 5 // GPS DOP and active satellites -#define NMEA_GPRMC 6 // Recommended minimum specific GPS data -#define NMEA_GPZDA 7 // Time and Date -#define NMEA_UNKNOWN 0xFF// Packet received but not known +#define NMEA_NODATA 0 // No data. Packet not available, bad, or not decoded +#define NMEA_GPGGA 1 // Global Positioning System Fix Data +#define NMEA_GPVTG 2 // Course over ground and ground speed +#define NMEA_GPGLL 3 // Geographic position - latitude/longitude +#define NMEA_GPGSV 4 // GPS satellites in view +#define NMEA_GPGSA 5 // GPS DOP and active satellites +#define NMEA_GPRMC 6 // Recommended minimum specific GPS data +#define NMEA_GPZDA 7 // Time and Date +#define NMEA_UNKNOWN 0xFF // Packet received but not known #define GPS_TIMEOUT_MS 500 // Debugging -//#define GPSDEBUG -//#define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages +// #define GPSDEBUG +// #define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages #ifdef GPSDEBUG - #define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages - #define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages - #define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages - #define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages - #define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages - #define NMEA_DEBUG_ZDA ///< define to enable debug of ZDA messages + #define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages + #define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages + #define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages + #define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages + #define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages + #define NMEA_DEBUG_ZDA ///< define to enable debug of ZDA messages #endif /** * Initialize the parser */ -NMEAParser::NMEAParser(QObject *parent):GPSParser(parent) +NMEAParser::NMEAParser(QObject *parent) : GPSParser(parent) { bufferInit(&gpsRxBuffer, (unsigned char *)gpsRxData, 512); - gpsRxOverflow=0; + gpsRxOverflow = 0; } NMEAParser::~NMEAParser() -{ - -} +{} /** * Called each time there are data in the input buffer */ void NMEAParser::processInputStream(char c) { - if( !bufferAddToEnd(&gpsRxBuffer, c) ) - { - // no space in buffer - // count overflow - gpsRxOverflow++; - return; - } - nmeaProcess(&gpsRxBuffer); + if (!bufferAddToEnd(&gpsRxBuffer, c)) { + // no space in buffer + // count overflow + gpsRxOverflow++; + return; + } + nmeaProcess(&gpsRxBuffer); } @@ -98,48 +95,39 @@ void NMEAParser::processInputStream(char c) * \return 0 checksum not valid * \return 1 checksum valid */ -char NMEAParser::nmeaChecksum(char* gps_buffer) +char NMEAParser::nmeaChecksum(char *gps_buffer) { - char checksum=0; - char checksum_received=0; + char checksum = 0; + char checksum_received = 0; - for(int x=0; xdatalength) - { - // look for a start of NMEA packet - if(bufferGetAtIndex(rxBuffer,0) == '$') - { - // found start - startFlag = TRUE; - // when start is found, we leave it intact in the receive buffer - // in case the full NMEA string is not completely received. The - // start will be detected in the next nmeaProcess iteration. + // process the receive buffer + // go through buffer looking for packets + while (rxBuffer->datalength) { + // look for a start of NMEA packet + if (bufferGetAtIndex(rxBuffer, 0) == '$') { + // found start + startFlag = TRUE; + // when start is found, we leave it intact in the receive buffer + // in case the full NMEA string is not completely received. The + // start will be detected in the next nmeaProcess iteration. - // done looking for start - break; - } - else + // done looking for start + break; + } else { + bufferGetFromFront(rxBuffer); + } + } + + // if we detected a start, look for end of packet + if (startFlag) { + for (i = 1; i < (rxBuffer->datalength) - 1; i++) { + // check for end of NMEA packet + if ((bufferGetAtIndex(rxBuffer, i) == '\r') && (bufferGetAtIndex(rxBuffer, i + 1) == '\n')) { + // have a packet end + // dump initial '$' + bufferGetFromFront(rxBuffer); + // copy packet to NmeaPacket + for (j = 0; j < (i - 1); j++) { + // although NMEA strings should be 80 characters or less, + // receive buffer errors can generate erroneous packets. + // Protect against packet buffer overflow + if (j < (NMEA_BUFFERSIZE - 1)) { + NmeaPacket[j] = bufferGetFromFront(rxBuffer); + } else { bufferGetFromFront(rxBuffer); - } - - // if we detected a start, look for end of packet - if(startFlag) - { - for(i=1; i<(rxBuffer->datalength)-1; i++) - { - // check for end of NMEA packet - if((bufferGetAtIndex(rxBuffer,i) == '\r') && (bufferGetAtIndex(rxBuffer,i+1) == '\n')) - { - // have a packet end - // dump initial '$' - bufferGetFromFront(rxBuffer); - // copy packet to NmeaPacket - for(j=0; j<(i-1); j++) - { - // although NMEA strings should be 80 characters or less, - // receive buffer errors can generate erroneous packets. - // Protect against packet buffer overflow - if(j<(NMEA_BUFFERSIZE-1)) - NmeaPacket[j] = bufferGetFromFront(rxBuffer); - else - bufferGetFromFront(rxBuffer); - } - // null terminate it - if (j<(NMEA_BUFFERSIZE-1)) { - NmeaPacket[j] = 0; - } else { - NmeaPacket[NMEA_BUFFERSIZE-1] = 0; - } - // dump from rxBuffer - bufferGetFromFront(rxBuffer); - bufferGetFromFront(rxBuffer); - //DEBUG + } + } + // null terminate it + if (j < (NMEA_BUFFERSIZE - 1)) { + NmeaPacket[j] = 0; + } else { + NmeaPacket[NMEA_BUFFERSIZE - 1] = 0; + } + // dump from rxBuffer + bufferGetFromFront(rxBuffer); + bufferGetFromFront(rxBuffer); + // DEBUG #ifdef NMEA_DEBUG_PKT - qDebug() << NmeaPacket; + qDebug() << NmeaPacket; #endif - emit packet(QString(NmeaPacket)); - // found a packet - // done with this processing session - foundpacket = NMEA_UNKNOWN; - break; - } - } + emit packet(QString(NmeaPacket)); + // found a packet + // done with this processing session + foundpacket = NMEA_UNKNOWN; + break; + } } + } - if(foundpacket) - { - // check message type and process appropriately - if(!strncmp(NmeaPacket, "GPGGA", 5)) - { - // process packet of this type - nmeaProcessGPGGA(NmeaPacket); - // report packet type - foundpacket = NMEA_GPGGA; - } - else if(!strncmp(NmeaPacket, "GPVTG", 5)) - { - // process packet of this type - nmeaProcessGPVTG(NmeaPacket); - // report packet type - foundpacket = NMEA_GPVTG; - } - else if(!strncmp(NmeaPacket, "GPGSA", 5)) - { - // process packet of this type - nmeaProcessGPGSA(NmeaPacket); - // report packet type - foundpacket = NMEA_GPGSA; - } - else if(!strncmp(NmeaPacket, "GPRMC", 5)) - { - // process packet of this type - nmeaProcessGPRMC(NmeaPacket); - // report packet type - foundpacket = NMEA_GPRMC; - } - else if(!strncmp(NmeaPacket, "GPGSV", 5)) - { - // Process packet of this type - nmeaProcessGPGSV(NmeaPacket); - // rerpot packet type - foundpacket = NMEA_GPGSV; - } - else if(!strncmp(NmeaPacket, "GPZDA", 5)) - { - // Process packet of this type - nmeaProcessGPZDA(NmeaPacket); - // rerpot packet type - foundpacket = NMEA_GPZDA; - } + if (foundpacket) { + // check message type and process appropriately + if (!strncmp(NmeaPacket, "GPGGA", 5)) { + // process packet of this type + nmeaProcessGPGGA(NmeaPacket); + // report packet type + foundpacket = NMEA_GPGGA; + } else if (!strncmp(NmeaPacket, "GPVTG", 5)) { + // process packet of this type + nmeaProcessGPVTG(NmeaPacket); + // report packet type + foundpacket = NMEA_GPVTG; + } else if (!strncmp(NmeaPacket, "GPGSA", 5)) { + // process packet of this type + nmeaProcessGPGSA(NmeaPacket); + // report packet type + foundpacket = NMEA_GPGSA; + } else if (!strncmp(NmeaPacket, "GPRMC", 5)) { + // process packet of this type + nmeaProcessGPRMC(NmeaPacket); + // report packet type + foundpacket = NMEA_GPRMC; + } else if (!strncmp(NmeaPacket, "GPGSV", 5)) { + // Process packet of this type + nmeaProcessGPGSV(NmeaPacket); + // rerpot packet type + foundpacket = NMEA_GPGSV; + } else if (!strncmp(NmeaPacket, "GPZDA", 5)) { + // Process packet of this type + nmeaProcessGPZDA(NmeaPacket); + // rerpot packet type + foundpacket = NMEA_GPZDA; } - else if(rxBuffer->datalength >= rxBuffer->size) - { - // if we found no packet, and the buffer is full - // we're logjammed, flush entire buffer - bufferFlush(rxBuffer); - } - return foundpacket; + } else if (rxBuffer->datalength >= rxBuffer->size) { + // if we found no packet, and the buffer is full + // we're logjammed, flush entire buffer + bufferFlush(rxBuffer); + } + return foundpacket; } /** - * Processes NMEA GSV sentences (satellites in view) - * \param[in] Buffer for parsed nmea GSV sentence - */ + * Processes NMEA GSV sentences (satellites in view) + * \param[in] Buffer for parsed nmea GSV sentence + */ void NMEAParser::nmeaProcessGPGSV(char *packet) { - // start parsing just after "GPGSV," // attempt to reject empty packets right away - if(packet[6]==',' && packet[7]==',') - return; + if (packet[6] == ',' && packet[7] == ',') { + return; + } - if(!nmeaChecksum(packet)) - { - // checksum not valid - return; + if (!nmeaChecksum(packet)) { + // checksum not valid + return; } nmeaTerminateAtChecksum(packet); - QString nmeaString( packet ); + QString nmeaString(packet); QStringList tokenslist = nmeaString.split(","); @@ -302,21 +270,21 @@ void NMEAParser::nmeaProcessGPGSV(char *packet) const int sentence_total = tokenslist.at(1).toInt(); // Number of sentences for full data const int sentence_index = tokenslist.at(2).toInt(); // sentence x of y - int sats = (tokenslist.size() - 4) /4; - for(int sat = 0; sat < sats; sat++) { - int base = 4+sat*4; - const int id = tokenslist.at(base+0).toInt(); // Satellite PRN number - const int elv = tokenslist.at(base+1).toInt(); // Elevation, degrees - const int azimuth = tokenslist.at(base+2).toInt(); // Azimuth, degrees - const int sig = tokenslist.at(base+3).toInt(); // SNR - higher is better - const int index = (sentence_index-1) * 4 + sat; + int sats = (tokenslist.size() - 4) / 4; + for (int sat = 0; sat < sats; sat++) { + int base = 4 + sat * 4; + const int id = tokenslist.at(base + 0).toInt(); // Satellite PRN number + const int elv = tokenslist.at(base + 1).toInt(); // Elevation, degrees + const int azimuth = tokenslist.at(base + 2).toInt(); // Azimuth, degrees + const int sig = tokenslist.at(base + 3).toInt(); // SNR - higher is better + const int index = (sentence_index - 1) * 4 + sat; emit satellite(index, id, elv, azimuth, sig); } - if(sentence_index == sentence_total) { + if (sentence_index == sentence_total) { // Last sentence - int total_sats = (sentence_index-1) * 4 + sats; - for(int emptySatIndex = total_sats; emptySatIndex < 16; emptySatIndex++) { + int total_sats = (sentence_index - 1) * 4 + sats; + for (int emptySatIndex = total_sats; emptySatIndex < 16; emptySatIndex++) { // Wipe the rest. emit satellite(emptySatIndex, 0, 0, 0, 0); } @@ -327,76 +295,79 @@ void NMEAParser::nmeaProcessGPGSV(char *packet) * Prosesses NMEA GPGGA sentences * \param[in] Buffer for parsed nmea GPGGA sentence */ -void NMEAParser::nmeaProcessGPGGA(char* packet) +void NMEAParser::nmeaProcessGPGGA(char *packet) { - // start parsing just after "GPGGA," - // attempt to reject empty packets right away - if(packet[6]==',' && packet[7]==',') - return; + // start parsing just after "GPGGA," + // attempt to reject empty packets right away + if (packet[6] == ',' && packet[7] == ',') { + return; + } - if(!nmeaChecksum(packet)) - { - // checksum not valid - return; - } - nmeaTerminateAtChecksum(packet); + if (!nmeaChecksum(packet)) { + // checksum not valid + return; + } + nmeaTerminateAtChecksum(packet); - QString nmeaString( packet ); - QStringList tokenslist = nmeaString.split(","); - GpsData.GPStime = tokenslist.at(1).toDouble(); - GpsData.Latitude = tokenslist.at(2).toDouble(); - int deg = (int)GpsData.Latitude/100; - double min = ((GpsData.Latitude)-(deg*100))/60.0; - GpsData.Latitude=deg+min; - // next field: N/S indicator - // correct latitute for N/S - if(tokenslist.at(3).contains("S")) GpsData.Latitude = -GpsData.Latitude; + QString nmeaString(packet); + QStringList tokenslist = nmeaString.split(","); + GpsData.GPStime = tokenslist.at(1).toDouble(); + GpsData.Latitude = tokenslist.at(2).toDouble(); + int deg = (int)GpsData.Latitude / 100; + double min = ((GpsData.Latitude) - (deg * 100)) / 60.0; + GpsData.Latitude = deg + min; + // next field: N/S indicator + // correct latitute for N/S + if (tokenslist.at(3).contains("S")) { + GpsData.Latitude = -GpsData.Latitude; + } - GpsData.Longitude = tokenslist.at(4).toDouble(); - deg = (int)GpsData.Longitude/100; - min = ((GpsData.Longitude)-(deg*100))/60.0; - GpsData.Longitude=deg+min; - // next field: E/W indicator - // correct latitute for E/W - if(tokenslist.at(5).contains("W")) GpsData.Longitude = -GpsData.Longitude; + GpsData.Longitude = tokenslist.at(4).toDouble(); + deg = (int)GpsData.Longitude / 100; + min = ((GpsData.Longitude) - (deg * 100)) / 60.0; + GpsData.Longitude = deg + min; + // next field: E/W indicator + // correct latitute for E/W + if (tokenslist.at(5).contains("W")) { + GpsData.Longitude = -GpsData.Longitude; + } - GpsData.SV = tokenslist.at(7).toInt(); - - GpsData.Altitude = tokenslist.at(9).toDouble(); - GpsData.GeoidSeparation = tokenslist.at(11).toDouble(); - emit position(GpsData.Latitude,GpsData.Longitude,GpsData.Altitude); - emit sv(GpsData.SV); - emit datetime(GpsData.GPSdate,GpsData.GPStime); + GpsData.SV = tokenslist.at(7).toInt(); + GpsData.Altitude = tokenslist.at(9).toDouble(); + GpsData.GeoidSeparation = tokenslist.at(11).toDouble(); + emit position(GpsData.Latitude, GpsData.Longitude, GpsData.Altitude); + emit sv(GpsData.SV); + emit datetime(GpsData.GPSdate, GpsData.GPStime); } /** * Prosesses NMEA GPRMC sentences * \param[in] Buffer for parsed nmea GPRMC sentence */ -void NMEAParser::nmeaProcessGPRMC(char* packet) +void NMEAParser::nmeaProcessGPRMC(char *packet) { - // start parsing just after "GPRMC," - // attempt to reject empty packets right away - if(packet[6]==',' && packet[7]==',') - return; + // start parsing just after "GPRMC," + // attempt to reject empty packets right away + if (packet[6] == ',' && packet[7] == ',') { + return; + } - if(!nmeaChecksum(packet)) - { - // checksum not valid - return; - } - nmeaTerminateAtChecksum(packet); + if (!nmeaChecksum(packet)) { + // checksum not valid + return; + } + nmeaTerminateAtChecksum(packet); - QString nmeaString( packet ); - QStringList tokenslist = nmeaString.split(","); - GpsData.GPStime = tokenslist.at(1).toDouble(); - GpsData.Groundspeed = tokenslist.at(7).toDouble(); - GpsData.Groundspeed = GpsData.Groundspeed*0.51444; - GpsData.Heading = tokenslist.at(8).toDouble(); - GpsData.GPSdate = tokenslist.at(9).toDouble(); - emit datetime(GpsData.GPSdate,GpsData.GPStime); - emit speedheading(GpsData.Groundspeed,GpsData.Heading); + QString nmeaString(packet); + QStringList tokenslist = nmeaString.split(","); + GpsData.GPStime = tokenslist.at(1).toDouble(); + GpsData.Groundspeed = tokenslist.at(7).toDouble(); + GpsData.Groundspeed = GpsData.Groundspeed * 0.51444; + GpsData.Heading = tokenslist.at(8).toDouble(); + GpsData.GPSdate = tokenslist.at(9).toDouble(); + emit datetime(GpsData.GPSdate, GpsData.GPStime); + emit speedheading(GpsData.Groundspeed, GpsData.Heading); } @@ -404,111 +375,113 @@ void NMEAParser::nmeaProcessGPRMC(char* packet) * Prosesses NMEA GPVTG sentences * \param[in] Buffer for parsed nmea GPVTG sentence */ -void NMEAParser::nmeaProcessGPVTG(char* packet) +void NMEAParser::nmeaProcessGPVTG(char *packet) { - // start parsing just after "GPVTG," - // attempt to reject empty packets right away - if(packet[6]==',' && packet[7]==',') - return; + // start parsing just after "GPVTG," + // attempt to reject empty packets right away + if (packet[6] == ',' && packet[7] == ',') { + return; + } - if(!nmeaChecksum(packet)) - { - // checksum not valid - return; - } - nmeaTerminateAtChecksum(packet); + if (!nmeaChecksum(packet)) { + // checksum not valid + return; + } + nmeaTerminateAtChecksum(packet); - QString nmeaString( packet ); - QStringList tokenslist = nmeaString.split(","); + QString nmeaString(packet); + QStringList tokenslist = nmeaString.split(","); - GpsData.Heading = tokenslist.at(1).toDouble(); - GpsData.Groundspeed = tokenslist.at(7).toDouble(); - GpsData.Groundspeed = GpsData.Groundspeed/3.6; - emit speedheading(GpsData.Groundspeed,GpsData.Heading); + GpsData.Heading = tokenslist.at(1).toDouble(); + GpsData.Groundspeed = tokenslist.at(7).toDouble(); + GpsData.Groundspeed = GpsData.Groundspeed / 3.6; + emit speedheading(GpsData.Groundspeed, GpsData.Heading); } /** * Prosesses NMEA GPGSA sentences * \param[in] Buffer for parsed nmea GPGSA sentence */ -void NMEAParser::nmeaProcessGPGSA(char* packet) +void NMEAParser::nmeaProcessGPGSA(char *packet) { - // start parsing just after "GPGSA," - // attempt to reject empty packets right away - if(packet[6]==',' && packet[7]==',') - return; + // start parsing just after "GPGSA," + // attempt to reject empty packets right away + if (packet[6] == ',' && packet[7] == ',') { + return; + } - if(!nmeaChecksum(packet)) { - // checksum not valid - return; + if (!nmeaChecksum(packet)) { + // checksum not valid + return; + } + nmeaTerminateAtChecksum(packet); + + QString nmeaString(packet); + QStringList tokenslist = nmeaString.split(","); + + // M=Manual, forced to operate in 2D or 3D + // A=Automatic, 3D/2D + QString fixmodeValue = tokenslist.at(1); + if (fixmodeValue == "A") { + emit fixmode(QString("Auto")); + } else if (fixmodeValue == "B") { + emit fixmode(QString("Manual")); + } + + // Mode: 1=Fix not available, 2=2D, 3=3D + int fixtypeValue = tokenslist.at(2).toInt(); + if (fixtypeValue == 1) { + emit fixtype(QString("NoFix")); + } else if (fixtypeValue == 2) { + emit fixtype(QString("Fix2D")); + } else if (fixtypeValue == 3) { + emit fixtype(QString("Fix3D")); + } + + // 3-14 = IDs of SVs used in position fix (null for unused fields) + QList svList; + for (int pos = 0; pos < 12; pos++) { + QString sv = tokenslist.at(3 + pos); + if (!sv.isEmpty()) { + svList.append(sv.toInt()); } - nmeaTerminateAtChecksum(packet); + } + emit fixSVs(svList); - QString nmeaString( packet ); - QStringList tokenslist = nmeaString.split(","); - - // M=Manual, forced to operate in 2D or 3D - // A=Automatic, 3D/2D - QString fixmodeValue = tokenslist.at(1); - if (fixmodeValue == "A") { - emit fixmode(QString("Auto")); - } else if (fixmodeValue == "B") { - emit fixmode(QString("Manual")); - } - - // Mode: 1=Fix not available, 2=2D, 3=3D - int fixtypeValue = tokenslist.at(2).toInt(); - if (fixtypeValue == 1) { - emit fixtype(QString("NoFix")); - } else if (fixtypeValue == 2) { - emit fixtype(QString("Fix2D")); - } else if (fixtypeValue == 3) { - emit fixtype(QString("Fix3D")); - } - - // 3-14 = IDs of SVs used in position fix (null for unused fields) - QList svList; - for(int pos = 0; pos < 12;pos ++) { - QString sv = tokenslist.at(3+pos); - if(!sv.isEmpty()) { - svList.append(sv.toInt()); - } - } - emit fixSVs(svList); - - // 15 = PDOP - // 16 = HDOP - // 17 = VDOP - GpsData.PDOP = tokenslist.at(15).toDouble(); - GpsData.HDOP = tokenslist.at(16).toDouble(); - GpsData.VDOP = tokenslist.at(17).toDouble(); - emit dop(GpsData.HDOP, GpsData.VDOP, GpsData.PDOP); + // 15 = PDOP + // 16 = HDOP + // 17 = VDOP + GpsData.PDOP = tokenslist.at(15).toDouble(); + GpsData.HDOP = tokenslist.at(16).toDouble(); + GpsData.VDOP = tokenslist.at(17).toDouble(); + emit dop(GpsData.HDOP, GpsData.VDOP, GpsData.PDOP); } /** * Prosesses NMEA GPZDA sentences * \param[in] Buffer for parsed nmea GPZDA sentence */ -void NMEAParser::nmeaProcessGPZDA(char* packet) +void NMEAParser::nmeaProcessGPZDA(char *packet) { - // start parsing just after "GPZDA," - // attempt to reject empty packets right away - if(packet[6]==',' && packet[7]==',') - return; + // start parsing just after "GPZDA," + // attempt to reject empty packets right away + if (packet[6] == ',' && packet[7] == ',') { + return; + } - if(!nmeaChecksum(packet)) { - // checksum not valid - return; - } - nmeaTerminateAtChecksum(packet); + if (!nmeaChecksum(packet)) { + // checksum not valid + return; + } + nmeaTerminateAtChecksum(packet); - QString nmeaString( packet ); - QStringList tokenslist = nmeaString.split(","); + QString nmeaString(packet); + QStringList tokenslist = nmeaString.split(","); - GpsData.GPStime = tokenslist.at(1).toDouble(); - int day = tokenslist.at(2).toInt(); - int month = tokenslist.at(3).toInt(); - int year = tokenslist.at(4).toInt(); - GpsData.GPSdate = day*10000+month*100+(year-2000); - emit datetime(GpsData.GPSdate,GpsData.GPStime); + GpsData.GPStime = tokenslist.at(1).toDouble(); + int day = tokenslist.at(2).toInt(); + int month = tokenslist.at(3).toInt(); + int year = tokenslist.at(4).toInt(); + GpsData.GPSdate = day * 10000 + month * 100 + (year - 2000); + emit datetime(GpsData.GPSdate, GpsData.GPStime); } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.h b/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.h index ad4f29ab6..158704333 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.h @@ -35,52 +35,48 @@ #include "gpsparser.h" // constants/macros/typdefs -#define NMEA_BUFFERSIZE 128 +#define NMEA_BUFFERSIZE 128 -typedef struct struct_GpsData -{ - double Latitude; - double Longitude; - double Altitude; - double Groundspeed; - double Heading; - int SV; - int Status; - double PDOP; - double HDOP; - double VDOP; - double GeoidSeparation; - double GPStime; - double GPSdate; +typedef struct struct_GpsData { + double Latitude; + double Longitude; + double Altitude; + double Groundspeed; + double Heading; + int SV; + int Status; + double PDOP; + double HDOP; + double VDOP; + double GeoidSeparation; + double GPStime; + double GPSdate; +} GpsData_t; -}GpsData_t; - -class NMEAParser: public GPSParser -{ +class NMEAParser : public GPSParser { Q_OBJECT public: - NMEAParser(QObject *parent = 0); - ~NMEAParser(); - void processInputStream(char c); - char* nmeaGetPacketBuffer(void); - char nmeaChecksum(char* gps_buffer); - void nmeaTerminateAtChecksum(char* gps_buffer); - uint8_t nmeaProcess(cBuffer* rxBuffer); - void nmeaProcessGPGGA(char* packet); - void nmeaProcessGPRMC(char* packet); - void nmeaProcessGPVTG(char* packet); - void nmeaProcessGPGSA(char* packet); - void nmeaProcessGPGSV(char* packet); - void nmeaProcessGPZDA(char* packet); - GpsData_t GpsData; - cBuffer gpsRxBuffer; - char gpsRxData[512]; - char NmeaPacket[NMEA_BUFFERSIZE]; - uint32_t numUpdates; - uint32_t numErrors; - int32_t gpsRxOverflow; - + NMEAParser(QObject *parent = 0); + ~NMEAParser(); + void processInputStream(char c); + char *nmeaGetPacketBuffer(void); + char nmeaChecksum(char *gps_buffer); + void nmeaTerminateAtChecksum(char *gps_buffer); + uint8_t nmeaProcess(cBuffer *rxBuffer); + void nmeaProcessGPGGA(char *packet); + void nmeaProcessGPRMC(char *packet); + void nmeaProcessGPVTG(char *packet); + void nmeaProcessGPGSA(char *packet); + void nmeaProcessGPGSV(char *packet); + void nmeaProcessGPZDA(char *packet); + GpsData_t GpsData; + cBuffer gpsRxBuffer; + char gpsRxData[512]; + char NmeaPacket[NMEA_BUFFERSIZE]; + uint32_t numUpdates; + uint32_t numErrors; + int32_t gpsRxOverflow; }; #endif // NMEAPARSER_H diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp index b647977c0..70fc4bc1a 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp @@ -39,47 +39,47 @@ TelemetryParser::TelemetryParser(QObject *parent) : GPSParser(parent) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject *gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + UAVDataObject *gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + if (gpsObj != NULL) { - connect(gpsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateGPS(UAVObject*))); + connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGPS(UAVObject *))); } else { qDebug() << "Error: Object is unknown (GPSPosition)."; } - gpsObj = dynamic_cast(objManager->getObject("GPSTime")); + gpsObj = dynamic_cast(objManager->getObject("GPSTime")); if (gpsObj != NULL) { - connect(gpsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateTime(UAVObject*))); + connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateTime(UAVObject *))); } else { qDebug() << "Error: Object is unknown (GPSTime)."; } - gpsObj = dynamic_cast(objManager->getObject("GPSSatellites")); + gpsObj = dynamic_cast(objManager->getObject("GPSSatellites")); if (gpsObj != NULL) { - connect(gpsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateSats(UAVObject*))); + connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateSats(UAVObject *))); } - } TelemetryParser::~TelemetryParser() +{} + + +void TelemetryParser::updateGPS(UAVObject *object1) { - -} - - -void TelemetryParser::updateGPS( UAVObject* object1) { - UAVObjectField* field = object1->getField(QString("Satellites")); + UAVObjectField *field = object1->getField(QString("Satellites")); emit sv(field->getValue().toInt()); double lat = object1->getField(QString("Latitude"))->getDouble(); double lon = object1->getField(QString("Longitude"))->getDouble(); double alt = object1->getField(QString("Altitude"))->getDouble(); + lat *= 1E-7; lon *= 1E-7; - emit position(lat,lon,alt); + emit position(lat, lon, alt); - double hdg = object1->getField(QString("Heading"))->getDouble(); - double spd = object1->getField(QString("Groundspeed"))->getDouble(); - emit speedheading(spd,hdg); + double hdg = object1->getField(QString("Heading"))->getDouble(); + double spd = object1->getField(QString("Groundspeed"))->getDouble(); + emit speedheading(spd, hdg); QString fix = object1->getField(QString("Status"))->getValue().toString(); emit fixtype(fix); @@ -87,39 +87,38 @@ void TelemetryParser::updateGPS( UAVObject* object1) { double hdop = object1->getField(QString("HDOP"))->getDouble(); double vdop = object1->getField(QString("VDOP"))->getDouble(); double pdop = object1->getField(QString("PDOP"))->getDouble(); - emit dop(hdop,vdop,pdop); - - + emit dop(hdop, vdop, pdop); } -void TelemetryParser::updateTime( UAVObject* object1) { - double hour = object1->getField(QString("Hour"))->getDouble(); +void TelemetryParser::updateTime(UAVObject *object1) +{ + double hour = object1->getField(QString("Hour"))->getDouble(); double minute = object1->getField(QString("Minute"))->getDouble(); double second = object1->getField(QString("Second"))->getDouble(); - double time = second + minute*100 + hour*10000; - double year = object1->getField(QString("Year"))->getDouble(); - double month = object1->getField(QString("Month"))->getDouble(); - double day = object1->getField(QString("Day"))->getDouble(); - double date = day + month * 100 + year * 10000; - emit datetime(date,time); + double time = second + minute * 100 + hour * 10000; + double year = object1->getField(QString("Year"))->getDouble(); + double month = object1->getField(QString("Month"))->getDouble(); + double day = object1->getField(QString("Day"))->getDouble(); + double date = day + month * 100 + year * 10000; + emit datetime(date, time); } /** - Updates the satellite constellation. + Updates the satellite constellation. - Not really optimized for now, we should only send updates for the stas - which have changed instead of updating everything... That said, Qt is supposed - to be able to optimize redraws anyway. - */ -void TelemetryParser::updateSats( UAVObject* object1) { - UAVObjectField* prn = object1->getField(QString("PRN")); - UAVObjectField* elevation = object1->getField(QString("Elevation")); - UAVObjectField* azimuth = object1->getField(QString("Azimuth")); - UAVObjectField* snr = object1->getField(QString("SNR")); + Not really optimized for now, we should only send updates for the stas + which have changed instead of updating everything... That said, Qt is supposed + to be able to optimize redraws anyway. + */ +void TelemetryParser::updateSats(UAVObject *object1) +{ + UAVObjectField *prn = object1->getField(QString("PRN")); + UAVObjectField *elevation = object1->getField(QString("Elevation")); + UAVObjectField *azimuth = object1->getField(QString("Azimuth")); + UAVObjectField *snr = object1->getField(QString("SNR")); - for (unsigned int i=0;i< prn->getNumElements();i++) { - emit satellite(i,prn->getValue(i).toInt(),elevation->getValue(i).toInt(), + for (unsigned int i = 0; i < prn->getNumElements(); i++) { + emit satellite(i, prn->getValue(i).toInt(), elevation->getValue(i).toInt(), azimuth->getValue(i).toInt(), snr->getValue(i).toInt()); } - } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.h b/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.h index 67ae6ea17..4b9f26517 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.h @@ -36,20 +36,17 @@ #include "gpsparser.h" -class TelemetryParser: public GPSParser -{ - +class TelemetryParser : public GPSParser { Q_OBJECT - + public: - TelemetryParser(QObject *parent = 0); - ~TelemetryParser(); + TelemetryParser(QObject *parent = 0); + ~TelemetryParser(); public slots: - void updateGPS(UAVObject* object1); - void updateTime(UAVObject* object1); - void updateSats(UAVObject* object1); - + void updateGPS(UAVObject *object1); + void updateTime(UAVObject *object1); + void updateSats(UAVObject *object1); }; #endif // TELEMETRYPARSER_H diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/aerosimrcdatastruct.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/aerosimrcdatastruct.h index 697695bc2..402192979 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/aerosimrcdatastruct.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/aerosimrcdatastruct.h @@ -31,175 +31,171 @@ #include const quint8 AEROSIMRC_MAX_CHANNELS = 39; -const quint16 DBG_BUFFER_MAX_SIZE = 4096; +const quint16 DBG_BUFFER_MAX_SIZE = 4096; #define MAX_DLL_USER_MENU_ITEMS 16 #define OBSOLETE_MIT_COMMAND (1 << 0) #define OBSOLETE_MIT_CHECKBOX (1 << 1) #define OBSOLETE_MIT_SEPARATOR (1 << 7) -#define PACK_STRUCT __attribute__((packed)) +#define PACK_STRUCT __attribute__((packed)) -struct simToPlugin -{ - quint16 structSize; - float simTimeStep; - float chSimTX[AEROSIMRC_MAX_CHANNELS]; - float chSimRX[AEROSIMRC_MAX_CHANNELS]; - uchar *OSDVideoBuf; - quint32 simMenuStatus; - float initPosX; - float initPosY; - float initPosZ; - float initHeading; - float initPitch; - float initRoll; - float wpHomeX; - float wpHomeY; - float wpHomeLat; - float wpHomeLong; +struct simToPlugin { + quint16 structSize; + float simTimeStep; + float chSimTX[AEROSIMRC_MAX_CHANNELS]; + float chSimRX[AEROSIMRC_MAX_CHANNELS]; + uchar *OSDVideoBuf; + quint32 simMenuStatus; + float initPosX; + float initPosY; + float initPosZ; + float initHeading; + float initPitch; + float initRoll; + float wpHomeX; + float wpHomeY; + float wpHomeLat; + float wpHomeLong; const char *wpHomeDesc; // (m, deg, string) - float wpAX; - float wpAY; - float wpALat; - float wpALong; - const char *wpADesc; // (m, deg, string) - float wpBX; - float wpBY; - float wpBLat; - float wpBLong; - const char *wpBDesc; // (m, deg, string) - float wpCX; - float wpCY; - float wpCLat; - float wpCLong; - const char *wpCDesc; // (m, deg, string) - float wpDX; - float wpDY; - float wpDLat; - float wpDLong; - const char *wpDDesc; // (m, deg, string) - float posX; - float posY; - float posZ; - float velX; - float velY; - float velZ; - float angVelX; - float angVelY; - float angVelZ; - float accelX; - float accelY; - float accelZ; - qreal latitude; - qreal longitude; - float AGL; - float heading; - float pitch; - float roll; - float windVelX; - float windVelY; - float windVelZ; - float eng1RPM; - float eng2RPM; - float eng3RPM; - float eng4RPM; - float voltage; // V - float current; // A - float consumedCharge; // Ah - float capacity; // Ah - float fuelConsumed; // l - float fuelTankCapacity; // l + float wpAX; + float wpAY; + float wpALat; + float wpALong; + const char *wpADesc; // (m, deg, string) + float wpBX; + float wpBY; + float wpBLat; + float wpBLong; + const char *wpBDesc; // (m, deg, string) + float wpCX; + float wpCY; + float wpCLat; + float wpCLong; + const char *wpCDesc; // (m, deg, string) + float wpDX; + float wpDY; + float wpDLat; + float wpDLong; + const char *wpDDesc; // (m, deg, string) + float posX; + float posY; + float posZ; + float velX; + float velY; + float velZ; + float angVelX; + float angVelY; + float angVelZ; + float accelX; + float accelY; + float accelZ; + qreal latitude; + qreal longitude; + float AGL; + float heading; + float pitch; + float roll; + float windVelX; + float windVelY; + float windVelZ; + float eng1RPM; + float eng2RPM; + float eng3RPM; + float eng4RPM; + float voltage; // V + float current; // A + float consumedCharge; // Ah + float capacity; // Ah + float fuelConsumed; // l + float fuelTankCapacity; // l // ver 3.83 - qint16 screenW; - qint16 screenH; + qint16 screenW; + qint16 screenH; // Model Orientation Matrix (X=Right, Y=Front, Z=Up) - float axisXx; - float axisXy; - float axisXz; - float axisYx; - float axisYy; - float axisYz; - float axisZx; - float axisZy; - float axisZz; + float axisXx; + float axisXy; + float axisXz; + float axisYx; + float axisYy; + float axisYz; + float axisZx; + float axisZy; + float axisZz; // Model data in body frame coordinates (X=Right, Y=Front, Z=Up) - float velXm; // m/s Model velocity in body coordinates - float velYm; - float velZm; - float angVelXm; // rad/s Model angular velocity in body coordinates - float angVelYm; - float angVelZm; - float accelXm; // m/s/s Model acceleration in body coordinates - float accelYm; - float accelZm; + float velXm; // m/s Model velocity in body coordinates + float velYm; + float velZm; + float angVelXm; // rad/s Model angular velocity in body coordinates + float angVelYm; + float angVelZm; + float accelXm; // m/s/s Model acceleration in body coordinates + float accelYm; + float accelZm; // ver 3.90 - quint32 OSDVideoBufSize; -} PACK_STRUCT ; // normal - 592, packed - 582 OK (3.81) - // normal - ???, packed - 658 OK (3.83) - // normal - ???, packed - 662 OK (3.90) + quint32 OSDVideoBufSize; +} PACK_STRUCT; // normal - 592, packed - 582 OK (3.81) + // normal - ???, packed - 658 OK (3.83) + // normal - ???, packed - 662 OK (3.90) -struct pluginToSim -{ - quint16 structSize; +struct pluginToSim { + quint16 structSize; const char *dbgInfoText; - uchar chOverTX[AEROSIMRC_MAX_CHANNELS]; - float chNewTX[AEROSIMRC_MAX_CHANNELS]; - uchar chOverRX[AEROSIMRC_MAX_CHANNELS]; - float chNewRX[AEROSIMRC_MAX_CHANNELS]; - float newPosX; // m - float newPosY; - float newPosZ; - float newVelX; // m/s - float newVelY; - float newVelZ; - float newAngVelX; // rad/s - float newAngVelY; - float newAngVelZ; - float newHeading; // rad - float newPitch; - float newRoll; + uchar chOverTX[AEROSIMRC_MAX_CHANNELS]; + float chNewTX[AEROSIMRC_MAX_CHANNELS]; + uchar chOverRX[AEROSIMRC_MAX_CHANNELS]; + float chNewRX[AEROSIMRC_MAX_CHANNELS]; + float newPosX; // m + float newPosY; + float newPosZ; + float newVelX; // m/s + float newVelY; + float newVelZ; + float newAngVelX; // rad/s + float newAngVelY; + float newAngVelZ; + float newHeading; // rad + float newPitch; + float newRoll; quint32 modelOverrideFlags; quint32 newMenuStatus; - quint8 isOSDShow; - quint8 isOSDChanged; + quint8 isOSDShow; + quint8 isOSDChanged; quint16 OSDWindow_DX; quint16 OSDWindow_DY; - float OSDScale; - float newWindVelX; - float newWindVelY; - float newWindVelZ; - float newEng1RPM; - float newEng2RPM; - float newEng3RPM; - float newEng4RPM; - float newVoltage; - float newCurrent; - float newConsumedCharge; - float newFuelConsumed; - quint8 modelCrashInhibit; + float OSDScale; + float newWindVelX; + float newWindVelY; + float newWindVelZ; + float newEng1RPM; + float newEng2RPM; + float newEng3RPM; + float newEng4RPM; + float newVoltage; + float newCurrent; + float newConsumedCharge; + float newFuelConsumed; + quint8 modelCrashInhibit; // ver 3.83 - qint16 newScreenW; // Simulator window position and size on screen - qint16 newScreenH; - qint16 newScreenX; - qint16 newScreenY; -} PACK_STRUCT ; // normal 516, packed 507 OK (3.81) - // normal ???, packed 515 OK (3.83 & 3.90) + qint16 newScreenW; // Simulator window position and size on screen + qint16 newScreenH; + qint16 newScreenX; + qint16 newScreenY; +} PACK_STRUCT; // normal 516, packed 507 OK (3.81) + // normal ???, packed 515 OK (3.83 & 3.90) -struct TPluginMenuItem -{ +struct TPluginMenuItem { quint32 OBSOLETE_eType; - char *OBSOLETE_strName; -} PACK_STRUCT ; + char *OBSOLETE_strName; +} PACK_STRUCT; -struct pluginInit -{ - quint32 nStructSize; - char *OBSOLETE_strMenuTitle; +struct pluginInit { + quint32 nStructSize; + char *OBSOLETE_strMenuTitle; TPluginMenuItem OBSOLETE_atMenuItem[MAX_DLL_USER_MENU_ITEMS]; const char *strPluginFolder; const char *strOutputFolder; -} PACK_STRUCT ; // normal - 144, packed - 144 OK (3.81 & 3.83 & 3.90) +} PACK_STRUCT; // normal - 144, packed - 144 OK (3.81 & 3.83 & 3.90) #undef PACK_STRUCT diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/enums.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/enums.h index 31abb1e0a..bfcd874ad 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/enums.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/enums.h @@ -30,21 +30,20 @@ // Custom Menu Item masks enum MenuMasks { - MenuEnable = (1 << 0), - MenuTx = (1 << 1), - MenuRx = (1 << 2), - MenuScreen = (1 << 3), - MenuNextWpt = (1 << 4), - MenuCmdReset = (1 << 5), - MenuLedBlue = (1 << 6), - MenuLedGreen = (1 << 7), - MenuFMode1 = (1 << 8), - MenuFMode2 = (1 << 9), - MenuFMode3 = (1 << 10) + MenuEnable = (1 << 0), + MenuTx = (1 << 1), + MenuRx = (1 << 2), + MenuScreen = (1 << 3), + MenuNextWpt = (1 << 4), + MenuCmdReset = (1 << 5), + MenuLedBlue = (1 << 6), + MenuLedGreen = (1 << 7), + MenuFMode1 = (1 << 8), + MenuFMode2 = (1 << 9), + MenuFMode3 = (1 << 10) }; -enum EOverrideFlags -{ +enum EOverrideFlags { OVR_POS = (1 << 0), OVR_VEL = (1 << 1), OVR_ANG_VEL = (1 << 2), @@ -53,7 +52,7 @@ enum EOverrideFlags OVR_ENGINE_RPM = (1 << 5), // Override RPM of all Engines or Motors OVR_BAT_VOLT = (1 << 6), // Override motor Battery Voltage OVR_BAT_AMP = (1 << 7), // Override motor Battery current - OVR_BAT_AH_CONSUMED = (1 << 8), // Override motor Battery AmpsHour consumed + OVR_BAT_AH_CONSUMED = (1 << 8), // Override motor Battery AmpsHour consumed OVR_FUEL_CONSUMED = (1 << 9) // Override Fuel consumed (gas & jet engines) }; diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.cpp index 18ba2c15d..8aac55d6b 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.cpp @@ -45,12 +45,12 @@ UdpReceiver *rcvr; const float RAD2DEG = (float)(180.0 / M_PI); const float DEG2RAD = (float)(M_PI / 180.0); -//extern "C" int __stdcall DllMain(HINSTANCE hinstDLL, DWORD fdwReason, LPVOID lpvReserved) -extern "C" int __stdcall DllMain(void*, quint32 fdwReason, void*) +// extern "C" int __stdcall DllMain(HINSTANCE hinstDLL, DWORD fdwReason, LPVOID lpvReserved) +extern "C" int __stdcall DllMain(void *, quint32 fdwReason, void *) { switch (fdwReason) { case 0: -// qDebug() << hinstDLL << "DLL_PROCESS_DETACH " << lpvReserved; +// qDebug() << hinstDLL << "DLL_PROCESS_DETACH " << lpvReserved; // free resources here rcvr->stop(); rcvr->wait(500); @@ -59,13 +59,13 @@ extern "C" int __stdcall DllMain(void*, quint32 fdwReason, void*) qDebug("------"); break; case 1: -// qDebug() << hinstDLL << " DLL_PROCESS_ATTACH " << lpvReserved; +// qDebug() << hinstDLL << " DLL_PROCESS_ATTACH " << lpvReserved; break; case 2: -// qDebug() << hinstDLL << "DLL_THREAD_ATTACH " << lpvReserved; +// qDebug() << hinstDLL << "DLL_THREAD_ATTACH " << lpvReserved; break; case 3: -// qDebug() << hinstDLL << "DLL_THREAD_DETACH " << lpvReserved; +// qDebug() << hinstDLL << "DLL_THREAD_DETACH " << lpvReserved; break; } return true; @@ -115,28 +115,28 @@ SIM_DLL_EXPORT void AeroSIMRC_Plugin_Init(pluginInit *p) qDebug() << "AeroSIMRC_Plugin_Init done"; } -//----------------------------------------------------------------------------- +// ----------------------------------------------------------------------------- -void Run_Command_Reset(/*const simToPlugin *stp, - pluginToSim *pts*/) +void Run_Command_Reset( /*const simToPlugin *stp, + pluginToSim *pts*/) { // Print some debug info, although it will only be seen during one frame debugInfo.append("\nRESET"); } void Run_Command_WindowSizeAndPos(const simToPlugin *stp, - pluginToSim *pts) + pluginToSim *pts) { static quint8 snSequence = 0; quint8 idx = snSequence * 4; - if (snSequence >= videoModes.at(0)) { // set fullscreen + if (snSequence >= videoModes.at(0)) { // set fullscreen pts->newScreenX = 0; pts->newScreenY = 0; pts->newScreenW = stp->screenW; pts->newScreenH = stp->screenH; snSequence = 0; - } else { // set video mode from config + } else { // set video mode from config pts->newScreenX = videoModes.at(idx + 1); pts->newScreenY = videoModes.at(idx + 2); pts->newScreenW = videoModes.at(idx + 3); @@ -146,11 +146,11 @@ void Run_Command_WindowSizeAndPos(const simToPlugin *stp, } void Run_Command_MoveToNextWaypoint(const simToPlugin *stp, - pluginToSim *pts) + pluginToSim *pts) { static quint8 snSequence = 0; - switch(snSequence) { + switch (snSequence) { case 0: pts->newPosX = stp->wpAX; pts->newPosY = stp->wpAY; @@ -179,16 +179,17 @@ void Run_Command_MoveToNextWaypoint(const simToPlugin *stp, default: qFatal("Run_Command_MoveToNextWaypoint switch error"); } - pts->modelOverrideFlags = 0; + pts->modelOverrideFlags = 0; pts->modelOverrideFlags |= OVR_POS; snSequence++; - if(snSequence > 4) + if (snSequence > 4) { snSequence = 0; + } } void Run_BlinkLEDs(const simToPlugin *stp, - pluginToSim *pts) + pluginToSim *pts) { if ((stp->simMenuStatus & MenuEnable) != 0) { pts->newMenuStatus |= MenuLedGreen; @@ -198,14 +199,15 @@ void Run_BlinkLEDs(const simToPlugin *stp, rcvr->getFlighStatus(armed, mode); debugInfo.append(QString("armed: %1, mode: %2\n").arg(armed).arg(mode)); - if (armed == 0) // disarm + if (armed == 0) { // disarm timeout = 1000; - else if (armed == 1) // arming + } else if (armed == 1) { // arming timeout = 40; - else if (armed == 2) // armed + } else if (armed == 2) { // armed timeout = 100; - else // unknown + } else { // unknown timeout = 2000; + } if (ledTimer.elapsed() > timeout) { ledTimer.restart(); pts->newMenuStatus ^= MenuLedBlue; @@ -235,7 +237,7 @@ void Run_BlinkLEDs(const simToPlugin *stp, pts->newMenuStatus &= ~MenuFMode3; pts->newMenuStatus &= ~MenuFMode2; pts->newMenuStatus |= MenuFMode1; - } else /*(mode == 0)*/ { + } else { /*(mode == 0)*/ pts->newMenuStatus &= ~MenuFMode3; pts->newMenuStatus &= ~MenuFMode2; pts->newMenuStatus &= ~MenuFMode1; @@ -246,112 +248,112 @@ void Run_BlinkLEDs(const simToPlugin *stp, } void InfoText(const simToPlugin *stp, - pluginToSim *pts) + pluginToSim *pts) { debugInfo.append( - QString( - "Plugin Folder = %1\n" - "Output Folder = %2\n" - "nStructSize = %3 " - "fIntegrationTimeStep = %4\n" - "\n" - "Aileron TX = %5 RX = %6 RCMD TX = %7 RX = %8\n" - "Elevator TX = %9 RX = %10 RCMD TX = %11 RX = %12\n" - "Throttle TX = %13 RX = %14 RCMD TX = %15 RX = %16\n" - "Rudder TX = %17 RX = %18 RCMD TX = %19 RX = %20\n" - "Channel5 TX = %21 RX = %22 RCMD TX = %23 RX = %24\n" - "Channel6 TX = %25 RX = %26 RCMD TX = %27 RX = %28\n" - "Channel7 TX = %29 RX = %30 RCMD TX = %31 RX = %32\n" - "PluginCh1 TX = %33 RX = %34 RCMD TX = %35 RX = %36\n" - "PluginCh2 TX = %37 RX = %38 RCMD TX = %39 RX = %40\n" - "FPVCamPan TX = %41 RX = %42 RCMD TX = %43 RX = %44\n" - "FPVCamTil TX = %45 RX = %46 RCMD TX = %47 RX = %48\n" - "\n" - "MenuItems = %49\n" - // Model data - "\n" - "fPosX,Y,Z = (%50, %51, %52)\n" - "fVelX,Y,Z = (%53, %54, %55)\n" - "fAngVelX,Y,Z = (%56, %57, %58)\n" - "fAccelX,Y,Z = (%59, %60, %61)\n" - "\n" - "Lat, Long = %62, %63\n" - "fHeightAboveTerrain = %64\n" - "\n" - "fHeading = %65 fPitch = %66 fRoll = %67\n" - ) - .arg(pluginFolder) - .arg(outputFolder) - .arg(stp->structSize) - .arg(1.0 / stp->simTimeStep, 4, 'f', 1) - .arg(stp->chSimTX[Ch1Aileron], 5, 'f', 2) - .arg(stp->chSimRX[Ch1Aileron], 5, 'f', 2) - .arg(pts->chNewTX[Ch1Aileron], 5, 'f', 2) - .arg(pts->chNewRX[Ch1Aileron], 5, 'f', 2) - .arg(stp->chSimTX[Ch2Elevator], 5, 'f', 2) - .arg(stp->chSimRX[Ch2Elevator], 5, 'f', 2) - .arg(pts->chNewTX[Ch2Elevator], 5, 'f', 2) - .arg(pts->chNewRX[Ch2Elevator], 5, 'f', 2) - .arg(stp->chSimTX[Ch3Throttle], 5, 'f', 2) - .arg(stp->chSimRX[Ch3Throttle], 5, 'f', 2) - .arg(pts->chNewTX[Ch3Throttle], 5, 'f', 2) - .arg(pts->chNewRX[Ch3Throttle], 5, 'f', 2) - .arg(stp->chSimTX[Ch4Rudder], 5, 'f', 2) - .arg(stp->chSimRX[Ch4Rudder], 5, 'f', 2) - .arg(pts->chNewTX[Ch4Rudder], 5, 'f', 2) - .arg(pts->chNewRX[Ch4Rudder], 5, 'f', 2) - .arg(stp->chSimTX[Ch5], 5, 'f', 2) - .arg(stp->chSimRX[Ch5], 5, 'f', 2) - .arg(pts->chNewTX[Ch5], 5, 'f', 2) - .arg(pts->chNewRX[Ch5], 5, 'f', 2) - .arg(stp->chSimTX[Ch6], 5, 'f', 2) - .arg(stp->chSimRX[Ch6], 5, 'f', 2) - .arg(pts->chNewTX[Ch6], 5, 'f', 2) - .arg(pts->chNewRX[Ch6], 5, 'f', 2) - .arg(stp->chSimTX[Ch7], 5, 'f', 2) - .arg(stp->chSimRX[Ch7], 5, 'f', 2) - .arg(pts->chNewTX[Ch7], 5, 'f', 2) - .arg(pts->chNewRX[Ch7], 5, 'f', 2) - .arg(stp->chSimTX[Ch23Plugin1], 5, 'f', 2) - .arg(stp->chSimRX[Ch23Plugin1], 5, 'f', 2) - .arg(pts->chNewTX[Ch23Plugin1], 5, 'f', 2) - .arg(pts->chNewRX[Ch23Plugin1], 5, 'f', 2) - .arg(stp->chSimTX[Ch24Plugin2], 5, 'f', 2) - .arg(stp->chSimRX[Ch24Plugin2], 5, 'f', 2) - .arg(pts->chNewTX[Ch24Plugin2], 5, 'f', 2) - .arg(pts->chNewRX[Ch24Plugin2], 5, 'f', 2) - .arg(stp->chSimTX[Ch12FPVCamPan], 5, 'f', 2) - .arg(stp->chSimRX[Ch12FPVCamPan], 5, 'f', 2) - .arg(pts->chNewTX[Ch12FPVCamPan], 5, 'f', 2) - .arg(pts->chNewRX[Ch12FPVCamPan], 5, 'f', 2) - .arg(stp->chSimTX[Ch13FPVCamTilt], 5, 'f', 2) - .arg(stp->chSimRX[Ch13FPVCamTilt], 5, 'f', 2) - .arg(pts->chNewTX[Ch13FPVCamTilt], 5, 'f', 2) - .arg(pts->chNewRX[Ch13FPVCamTilt], 5, 'f', 2) - .arg(stp->simMenuStatus) - .arg(stp->posX, 5, 'f', 2) - .arg(stp->posY, 5, 'f', 2) - .arg(stp->posZ, 5, 'f', 2) - .arg(stp->velX, 5, 'f', 2) - .arg(stp->velY, 5, 'f', 2) - .arg(stp->velZ, 5, 'f', 2) - .arg(stp->angVelXm, 5, 'f', 2) - .arg(stp->angVelYm, 5, 'f', 2) - .arg(stp->angVelZm, 5, 'f', 2) - .arg(stp->accelXm, 5, 'f', 2) - .arg(stp->accelYm, 5, 'f', 2) - .arg(stp->accelZm, 5, 'f', 2) - .arg(stp->latitude, 5, 'f', 2) - .arg(stp->longitude, 5, 'f', 2) - .arg(stp->AGL, 5, 'f', 2) - .arg(stp->heading*RAD2DEG, 5, 'f', 2) - .arg(stp->pitch*RAD2DEG, 5, 'f', 2) - .arg(stp->roll*RAD2DEG, 5, 'f', 2) - ); + QString( + "Plugin Folder = %1\n" + "Output Folder = %2\n" + "nStructSize = %3 " + "fIntegrationTimeStep = %4\n" + "\n" + "Aileron TX = %5 RX = %6 RCMD TX = %7 RX = %8\n" + "Elevator TX = %9 RX = %10 RCMD TX = %11 RX = %12\n" + "Throttle TX = %13 RX = %14 RCMD TX = %15 RX = %16\n" + "Rudder TX = %17 RX = %18 RCMD TX = %19 RX = %20\n" + "Channel5 TX = %21 RX = %22 RCMD TX = %23 RX = %24\n" + "Channel6 TX = %25 RX = %26 RCMD TX = %27 RX = %28\n" + "Channel7 TX = %29 RX = %30 RCMD TX = %31 RX = %32\n" + "PluginCh1 TX = %33 RX = %34 RCMD TX = %35 RX = %36\n" + "PluginCh2 TX = %37 RX = %38 RCMD TX = %39 RX = %40\n" + "FPVCamPan TX = %41 RX = %42 RCMD TX = %43 RX = %44\n" + "FPVCamTil TX = %45 RX = %46 RCMD TX = %47 RX = %48\n" + "\n" + "MenuItems = %49\n" + // Model data + "\n" + "fPosX,Y,Z = (%50, %51, %52)\n" + "fVelX,Y,Z = (%53, %54, %55)\n" + "fAngVelX,Y,Z = (%56, %57, %58)\n" + "fAccelX,Y,Z = (%59, %60, %61)\n" + "\n" + "Lat, Long = %62, %63\n" + "fHeightAboveTerrain = %64\n" + "\n" + "fHeading = %65 fPitch = %66 fRoll = %67\n" + ) + .arg(pluginFolder) + .arg(outputFolder) + .arg(stp->structSize) + .arg(1.0 / stp->simTimeStep, 4, 'f', 1) + .arg(stp->chSimTX[Ch1Aileron], 5, 'f', 2) + .arg(stp->chSimRX[Ch1Aileron], 5, 'f', 2) + .arg(pts->chNewTX[Ch1Aileron], 5, 'f', 2) + .arg(pts->chNewRX[Ch1Aileron], 5, 'f', 2) + .arg(stp->chSimTX[Ch2Elevator], 5, 'f', 2) + .arg(stp->chSimRX[Ch2Elevator], 5, 'f', 2) + .arg(pts->chNewTX[Ch2Elevator], 5, 'f', 2) + .arg(pts->chNewRX[Ch2Elevator], 5, 'f', 2) + .arg(stp->chSimTX[Ch3Throttle], 5, 'f', 2) + .arg(stp->chSimRX[Ch3Throttle], 5, 'f', 2) + .arg(pts->chNewTX[Ch3Throttle], 5, 'f', 2) + .arg(pts->chNewRX[Ch3Throttle], 5, 'f', 2) + .arg(stp->chSimTX[Ch4Rudder], 5, 'f', 2) + .arg(stp->chSimRX[Ch4Rudder], 5, 'f', 2) + .arg(pts->chNewTX[Ch4Rudder], 5, 'f', 2) + .arg(pts->chNewRX[Ch4Rudder], 5, 'f', 2) + .arg(stp->chSimTX[Ch5], 5, 'f', 2) + .arg(stp->chSimRX[Ch5], 5, 'f', 2) + .arg(pts->chNewTX[Ch5], 5, 'f', 2) + .arg(pts->chNewRX[Ch5], 5, 'f', 2) + .arg(stp->chSimTX[Ch6], 5, 'f', 2) + .arg(stp->chSimRX[Ch6], 5, 'f', 2) + .arg(pts->chNewTX[Ch6], 5, 'f', 2) + .arg(pts->chNewRX[Ch6], 5, 'f', 2) + .arg(stp->chSimTX[Ch7], 5, 'f', 2) + .arg(stp->chSimRX[Ch7], 5, 'f', 2) + .arg(pts->chNewTX[Ch7], 5, 'f', 2) + .arg(pts->chNewRX[Ch7], 5, 'f', 2) + .arg(stp->chSimTX[Ch23Plugin1], 5, 'f', 2) + .arg(stp->chSimRX[Ch23Plugin1], 5, 'f', 2) + .arg(pts->chNewTX[Ch23Plugin1], 5, 'f', 2) + .arg(pts->chNewRX[Ch23Plugin1], 5, 'f', 2) + .arg(stp->chSimTX[Ch24Plugin2], 5, 'f', 2) + .arg(stp->chSimRX[Ch24Plugin2], 5, 'f', 2) + .arg(pts->chNewTX[Ch24Plugin2], 5, 'f', 2) + .arg(pts->chNewRX[Ch24Plugin2], 5, 'f', 2) + .arg(stp->chSimTX[Ch12FPVCamPan], 5, 'f', 2) + .arg(stp->chSimRX[Ch12FPVCamPan], 5, 'f', 2) + .arg(pts->chNewTX[Ch12FPVCamPan], 5, 'f', 2) + .arg(pts->chNewRX[Ch12FPVCamPan], 5, 'f', 2) + .arg(stp->chSimTX[Ch13FPVCamTilt], 5, 'f', 2) + .arg(stp->chSimRX[Ch13FPVCamTilt], 5, 'f', 2) + .arg(pts->chNewTX[Ch13FPVCamTilt], 5, 'f', 2) + .arg(pts->chNewRX[Ch13FPVCamTilt], 5, 'f', 2) + .arg(stp->simMenuStatus) + .arg(stp->posX, 5, 'f', 2) + .arg(stp->posY, 5, 'f', 2) + .arg(stp->posZ, 5, 'f', 2) + .arg(stp->velX, 5, 'f', 2) + .arg(stp->velY, 5, 'f', 2) + .arg(stp->velZ, 5, 'f', 2) + .arg(stp->angVelXm, 5, 'f', 2) + .arg(stp->angVelYm, 5, 'f', 2) + .arg(stp->angVelZm, 5, 'f', 2) + .arg(stp->accelXm, 5, 'f', 2) + .arg(stp->accelYm, 5, 'f', 2) + .arg(stp->accelZm, 5, 'f', 2) + .arg(stp->latitude, 5, 'f', 2) + .arg(stp->longitude, 5, 'f', 2) + .arg(stp->AGL, 5, 'f', 2) + .arg(stp->heading * RAD2DEG, 5, 'f', 2) + .arg(stp->pitch * RAD2DEG, 5, 'f', 2) + .arg(stp->roll * RAD2DEG, 5, 'f', 2) + ); } SIM_DLL_EXPORT void AeroSIMRC_Plugin_Run(const simToPlugin *stp, - pluginToSim *pts) + pluginToSim *pts) { debugInfo = "---\n"; // By default do not change the Menu Items of type CheckBox @@ -365,7 +367,7 @@ SIM_DLL_EXPORT void AeroSIMRC_Plugin_Run(const simToPlugin *stp, bool isNextWp = (stp->simMenuStatus & MenuNextWpt) != 0; // Run commands if (isReset) { - Run_Command_Reset(/*stp, pts*/); + Run_Command_Reset( /*stp, pts*/); } else if (isScreen) { Run_Command_WindowSizeAndPos(stp, pts); } else if (isNextWp) { @@ -373,10 +375,12 @@ SIM_DLL_EXPORT void AeroSIMRC_Plugin_Run(const simToPlugin *stp, } else { Run_BlinkLEDs(stp, pts); if (isEnable) { - if (isTxON) + if (isTxON) { sndr->sendDatagram(stp); - if (isRxON) + } + if (isRxON) { rcvr->setChannels(pts); + } } // network lag diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.h index a590c8a24..be94f15ff 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.h @@ -38,16 +38,13 @@ SIM_DLL_EXPORT void AeroSIMRC_Plugin_ReportStructSizes( quint32 *sizeSimToPlugin, quint32 *sizePluginToSim, - quint32 *sizePluginInit -); + quint32 *sizePluginInit); SIM_DLL_EXPORT void AeroSIMRC_Plugin_Init( - pluginInit *p -); + pluginInit *p); SIM_DLL_EXPORT void AeroSIMRC_Plugin_Run( const simToPlugin *stp, - pluginToSim *pts -); + pluginToSim *pts); #endif // PLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/qdebughandler.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/qdebughandler.cpp index bb9617511..4e859c60c 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/qdebughandler.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/qdebughandler.cpp @@ -59,6 +59,7 @@ void myQDebugHandler(QtMsgType type, const char *msg) ts << time.currentTime().toString("hh:mm:ss.zzz") << " " << txt << endl; - if (type == QtFatalMsg) + if (type == QtFatalMsg) { abort(); + } } diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.cpp index 6c19f91dc..f2cac7caf 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.cpp @@ -30,20 +30,22 @@ Settings::Settings(QString settingsPath, QObject *parent) : QObject(parent) { - settings = new QSettings(settingsPath + "/cc_plugin.ini", QSettings::IniFormat); - // default settings - sendToHost = "127.0.0.1"; - sendToPort = 40100; - listenOnHost = "127.0.0.1"; - listenOnPort = 40200; - channels.reserve(60); - for (quint8 i = 0; i < 10; ++i) - inputMap << 255; - for (quint8 i = 0; i < 8; ++i) - outputMap << 255; - sendToRX = true; - takeFromTX = true; - videoModes << 1 << 50 << 50 << 800 << 600; + settings = new QSettings(settingsPath + "/cc_plugin.ini", QSettings::IniFormat); + // default settings + sendToHost = "127.0.0.1"; + sendToPort = 40100; + listenOnHost = "127.0.0.1"; + listenOnPort = 40200; + channels.reserve(60); + for (quint8 i = 0; i < 10; ++i) { + inputMap << 255; + } + for (quint8 i = 0; i < 8; ++i) { + outputMap << 255; + } + sendToRX = true; + takeFromTX = true; + videoModes << 1 << 50 << 50 << 800 << 600; } void Settings::read() @@ -51,20 +53,20 @@ void Settings::read() // network listenOnHost = settings->value("listen_on_host", listenOnHost).toString(); listenOnPort = settings->value("listen_on_port", listenOnPort).toInt(); - sendToHost = settings->value("send_to_host", sendToHost).toString(); - sendToPort = settings->value("send_to_port", sendToPort).toInt(); + sendToHost = settings->value("send_to_host", sendToHost).toString(); + sendToPort = settings->value("send_to_port", sendToPort).toInt(); QString allChannels = settings->value("all_channels").toString(); QString chan; int i = 0; - foreach (chan, allChannels.split(" ")) - channels.insert(chan, i++); + foreach(chan, allChannels.split(" ")) + channels.insert(chan, i++); // inputs QString num = ""; QString map = ""; for (quint8 i = 0; i < 10; ++i) { - num = QString::number(i+1); + num = QString::number(i + 1); map = settings->value("Input/cc_channel_" + num).toString(); inputMap[i] = channels.value(map, inputMap.at(i)); } @@ -74,7 +76,7 @@ void Settings::read() // outputs for (quint8 i = 0; i < 8; ++i) { - num = QString::number(i+1); + num = QString::number(i + 1); map = settings->value("Output/sim_channel_" + num).toString(); outputMap[i] = channels.value(map, outputMap.at(i)); } @@ -88,11 +90,11 @@ void Settings::read() videoModes.clear(); videoModes << resolutionNum; for (quint8 i = 0; i < resolutionNum; ++i) { - num = QString::number(i+1); + num = QString::number(i + 1); QString modes = settings->value("Video/resolution_" + num, "0, 0, 640, 480").toString(); QString mode; - foreach (mode, modes.split(" ")) - videoModes << mode.toInt(); + foreach(mode, modes.split(" ")) + videoModes << mode.toInt(); } } } diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.h index 5bc39135d..b10359a78 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.h @@ -35,20 +35,46 @@ #include #include -class Settings : public QObject -{ +class Settings : public QObject { public: explicit Settings(QString settingsPath, QObject *parent = 0); void read(); - QString remoteHost() { return sendToHost; } - quint16 remotePort() { return sendToPort; } - QString localHost() { return listenOnHost; } - quint16 localPort() { return listenOnPort; } - QList getInputMap() { return inputMap; } - QList getOutputMap() { return outputMap; } - bool isToRX() { return sendToRX; } - bool isFromTX() { return takeFromTX; } - QList getVideoModes() { return videoModes; } + QString remoteHost() + { + return sendToHost; + } + quint16 remotePort() + { + return sendToPort; + } + QString localHost() + { + return listenOnHost; + } + quint16 localPort() + { + return listenOnPort; + } + QList getInputMap() + { + return inputMap; + } + QList getOutputMap() + { + return outputMap; + } + bool isToRX() + { + return sendToRX; + } + bool isFromTX() + { + return takeFromTX; + } + QList getVideoModes() + { + return videoModes; + } private: QHash channels; diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp index 44151b661..c15d34cea 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp @@ -35,32 +35,35 @@ UdpSender::UdpSender(const QList map, { qDebug() << this << "UdpSender::UdpSender thread:" << thread(); outSocket = NULL; - for (int i = 0; i < 8; ++i) + for (int i = 0; i < 8; ++i) { channels << 0.0; - channelsMap = map; - takeFromTX = isTX; + } + channelsMap = map; + takeFromTX = isTX; packetsSended = 0; } UdpSender::~UdpSender() { - qDebug() << this << "UdpSender::~UdpSender"; - if (outSocket) + qDebug() << this << "UdpSender::~UdpSender"; + if (outSocket) { delete outSocket; + } } // public void UdpSender::init(const QString &remoteHost, quint16 remotePort) { qDebug() << this << "UdpSender::init"; - outHost = remoteHost; - outPort = remotePort; + outHost = remoteHost; + outPort = remotePort; outSocket = new QUdpSocket(); } void UdpSender::sendDatagram(const simToPlugin *stp) { QByteArray data; + data.resize(188); QDataStream out(&data, QIODevice::WriteOnly); out.setFloatingPointPrecision(QDataStream::SinglePrecision); @@ -70,24 +73,24 @@ void UdpSender::sendDatagram(const simToPlugin *stp) // simulation step out << stp->simTimeStep; // home location - out << stp->initPosX << stp->initPosY << stp->initPosZ; - out << stp->wpHomeX << stp->wpHomeY << stp->wpHomeLat << stp->wpHomeLong; + out << stp->initPosX << stp->initPosY << stp->initPosZ; + out << stp->wpHomeX << stp->wpHomeY << stp->wpHomeLat << stp->wpHomeLong; // position - out << stp->posX << stp->posY << stp->posZ; + out << stp->posX << stp->posY << stp->posZ; // velocity (world) - out << stp->velX << stp->velY << stp->velZ; + out << stp->velX << stp->velY << stp->velZ; // angular velocity (model) - out << stp->angVelXm << stp->angVelYm << stp->angVelZm; + out << stp->angVelXm << stp->angVelYm << stp->angVelZm; // acceleration (model) - out << stp->accelXm << stp->accelYm << stp->accelZm; + out << stp->accelXm << stp->accelYm << stp->accelZm; // coordinates - out << stp->latitude << stp->longitude; + out << stp->latitude << stp->longitude; // sonar out << stp->AGL; // attitude - out << stp->heading << stp->pitch << stp->roll; + out << stp->heading << stp->pitch << stp->roll; // electric - out << stp->voltage << stp->current << stp->consumedCharge; + out << stp->voltage << stp->current << stp->consumedCharge; // matrix out << stp->axisXx << stp->axisXy << stp->axisXz; out << stp->axisYx << stp->axisYy << stp->axisYz; @@ -95,12 +98,13 @@ void UdpSender::sendDatagram(const simToPlugin *stp) // channels for (int i = 0; i < 8; ++i) { quint8 mapTo = channelsMap.at(i); - if (mapTo == 255) // unused channel + if (mapTo == 255) { // unused channel out << 0.0; - else if (takeFromTX) // use values from simulators transmitter + } else if (takeFromTX) { // use values from simulators transmitter out << stp->chSimTX[mapTo]; - else // direct use values from ESC/motors/ailerons/etc + } else { // direct use values from ESC/motors/ailerons/etc out << stp->chSimRX[mapTo]; + } } // packet counter @@ -110,7 +114,7 @@ void UdpSender::sendDatagram(const simToPlugin *stp) ++packetsSended; } -//----------------------------------------------------------------------------- +// ----------------------------------------------------------------------------- UdpReceiver::UdpReceiver(const QList map, bool isRX, @@ -119,12 +123,13 @@ UdpReceiver::UdpReceiver(const QList map, { qDebug() << this << "UdpReceiver::UdpReceiver thread:" << thread(); - stopped = false; + stopped = false; inSocket = NULL; - for (int i = 0; i < 10; ++i) + for (int i = 0; i < 10; ++i) { channels << -1.0; - channelsMap = map; - sendToRX = isRX; + } + channelsMap = map; + sendToRX = isRX; armed = 0; mode = 0; packetsRecived = 1; @@ -132,9 +137,10 @@ UdpReceiver::UdpReceiver(const QList map, UdpReceiver::~UdpReceiver() { - qDebug() << this << "UdpReceiver::~UdpReceiver"; - if (inSocket) + qDebug() << this << "UdpReceiver::~UdpReceiver"; + if (inSocket) { delete inSocket; + } } // public @@ -151,8 +157,9 @@ void UdpReceiver::init(const QString &localHost, quint16 localPort) void UdpReceiver::run() { qDebug() << this << "UdpReceiver::run start"; - while (!stopped) + while (!stopped) { onReadyRead(); + } qDebug() << this << "UdpReceiver::run ended"; } @@ -172,11 +179,11 @@ void UdpReceiver::setChannels(pluginToSim *pts) float channelValue = qBound(-1.0f, channels.at(i), 1.0f); if (sendToRX) { // direct connect to ESC/motors/ailerons/etc - pts->chNewRX[mapTo] = channelValue; + pts->chNewRX[mapTo] = channelValue; pts->chOverRX[mapTo] = true; } else { // replace simulators transmitter - pts->chNewTX[mapTo] = channelValue; + pts->chNewTX[mapTo] = channelValue; pts->chOverTX[mapTo] = true; } } @@ -194,8 +201,9 @@ void UdpReceiver::getFlighStatus(quint8 &arm, quint8 &mod) // private void UdpReceiver::onReadyRead() { - if (!inSocket->waitForReadyRead(8)) // 1/60fps ~= 16.7ms, 1/120fps ~= 8.3ms + if (!inSocket->waitForReadyRead(8)) { // 1/60fps ~= 16.7ms, 1/120fps ~= 8.3ms return; + } // TODO: add failsafe // if a command not recieved then slowly return all channel to neutral // @@ -212,15 +220,18 @@ void UdpReceiver::onReadyRead() void UdpReceiver::processDatagram(QByteArray &datagram) { QDataStream stream(datagram); + stream.setFloatingPointPrecision(QDataStream::SinglePrecision); // check magic header quint32 magic; stream >> magic; - if (magic != 0x52434D44) // "RCMD" + if (magic != 0x52434D44) { // "RCMD" return; + } // read channels - for (int i = 0; i < 10; ++i) + for (int i = 0; i < 10; ++i) { stream >> channels[i]; + } // read flight mode stream >> armed >> mode; // read counter diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.h index ead07f046..d3365b955 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.h @@ -36,15 +36,17 @@ #include #include "aerosimrcdatastruct.h" -class UdpSender : public QObject -{ -// Q_OBJECT +class UdpSender : public QObject { +// Q_OBJECT public: explicit UdpSender(const QList map, bool isTX, QObject *parent = 0); ~UdpSender(); void init(const QString &remoteHost, quint16 remotePort); void sendDatagram(const simToPlugin *stp); - quint32 pcks() { return packetsSended; } + quint32 pcks() + { + return packetsSended; + } private: QUdpSocket *outSocket; @@ -57,9 +59,8 @@ private: }; -class UdpReceiver : public QThread -{ -// Q_OBJECT +class UdpReceiver : public QThread { +// Q_OBJECT public: explicit UdpReceiver(const QList map, bool isRX, QObject *parent = 0); ~UdpReceiver(); @@ -69,9 +70,18 @@ public: // function getChannels for other threads void setChannels(pluginToSim *pts); void getFlighStatus(quint8 &arm, quint8 &mod); - quint8 getArmed() { return armed; } - quint8 getMode() { return mode; } - quint32 pcks() { return packetsRecived; } + quint8 getArmed() + { + return armed; + } + quint8 getMode() + { + return mode; + } + quint32 pcks() + { + return packetsRecived; + } private: volatile bool stopped; diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestmain.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestmain.cpp index 744e4bf25..252906baa 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestmain.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestmain.cpp @@ -32,6 +32,7 @@ int main(int argc, char *argv[]) { QApplication a(argc, argv); Widget w; + w.show(); return a.exec(); diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.cpp index 9ab45e45a..47920a9f3 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.cpp @@ -34,8 +34,8 @@ Widget::Widget(QWidget *parent) : { ui->setupUi(this); - inSocket = NULL; - outSocket = NULL; + inSocket = NULL; + outSocket = NULL; screenTimeout.start(); packetCounter = 0; @@ -45,10 +45,10 @@ Widget::Widget(QWidget *parent) : Widget::~Widget() { - if(outSocket) { + if (outSocket) { delete outSocket; } - if(inSocket) { + if (inSocket) { delete inSocket; } delete ui; @@ -80,7 +80,7 @@ void Widget::on_btReciveStart_clicked() void Widget::on_btReciveStop_clicked() { - if(inSocket) { + if (inSocket) { delete inSocket; inSocket = NULL; ui->listWidget->addItem("unbind ok"); @@ -98,8 +98,8 @@ void Widget::on_btTransmitStart_clicked() on_btTransmitStop_clicked(); outSocket = new QUdpSocket(); - outHost = ui->simHost->text(); - outPort = ui->simPort->text().toInt(); + outHost = ui->simHost->text(); + outPort = ui->simPort->text().toInt(); ui->listWidget->addItem("transmit started"); ui->btTransmitStop->setEnabled(1); @@ -110,7 +110,7 @@ void Widget::on_btTransmitStart_clicked() void Widget::on_btTransmitStop_clicked() { - if(outSocket) { + if (outSocket) { delete outSocket; outSocket = NULL; ui->listWidget->addItem("transmit stopped"); @@ -135,8 +135,9 @@ void Widget::readDatagram() Q_UNUSED(datagramSize); processDatagram(datagram); - if (ui->autoAnswer->isChecked()) + if (ui->autoAnswer->isChecked()) { sendDatagram(); + } } } @@ -146,24 +147,25 @@ void Widget::processDatagram(const QByteArray &data) { QByteArray buf = data; QDataStream stream(&buf, QIODevice::ReadOnly); + stream.setFloatingPointPrecision(QDataStream::SinglePrecision); // check magic header quint32 magic; stream >> magic; - if (magic == 0x4153494D) { // "AERO" - float timeStep, - homeX, homeY, homeZ, - WpHX, WpHY, WpLat, WpLon, - posX, posY, posZ, - velX, velY, velZ, - angX, angY, angZ, - accX, accY, accZ, - lat, lon, alt, - head, pitch, roll, - volt, curr, - rx, ry, rz, fx, fy, fz, ux, uy, uz, - chAil, chEle, chThr, chRud, chPlg1, chPlg2, chFpv1, chFpv2; + if (magic == 0x4153494D) { // "AERO" + float timeStep, + homeX, homeY, homeZ, + WpHX, WpHY, WpLat, WpLon, + posX, posY, posZ, + velX, velY, velZ, + angX, angY, angZ, + accX, accY, accZ, + lat, lon, alt, + head, pitch, roll, + volt, curr, + rx, ry, rz, fx, fy, fz, ux, uy, uz, + chAil, chEle, chThr, chRud, chPlg1, chPlg2, chFpv1, chFpv2; stream >> timeStep; stream >> homeX >> homeY >> homeZ; @@ -179,59 +181,61 @@ void Widget::processDatagram(const QByteArray &data) stream >> chAil >> chEle >> chThr >> chRud >> chPlg1 >> chPlg2 >> chFpv1 >> chFpv2; stream >> packetCounter; - if(ui->tabWidget->currentIndex() != 0) + if (ui->tabWidget->currentIndex() != 0) { return; + } - if (screenTimeout.elapsed() < 200) + if (screenTimeout.elapsed() < 200) { return; + } ui->listWidget->clear(); /* - ui->listWidget->addItem("time step (s)"); - ui->listWidget->addItem(QString("%1") + ui->listWidget->addItem("time step (s)"); + ui->listWidget->addItem(QString("%1") .arg(timeStep); - ui->listWidget->addItem("home location (m)"); - ui->listWidget->addItem(QString("%1, %2, %3") + ui->listWidget->addItem("home location (m)"); + ui->listWidget->addItem(QString("%1, %2, %3") .arg(homeX, 7, 'f', 4) .arg(homeY, 7, 'f', 4) .arg(homeZ, 7, 'f', 4)); - ui->listWidget->addItem("home waypoint"); - ui->listWidget->addItem(QString("%1, %2, %3, %4") + ui->listWidget->addItem("home waypoint"); + ui->listWidget->addItem(QString("%1, %2, %3, %4") .arg(WpHX, 7, 'f', 4) .arg(WpHY, 7, 'f', 4) .arg(WpLat, 7, 'f', 4) .arg(WpLon, 7, 'f', 4)); - ui->listWidget->addItem("model position (m)"); - ui->listWidget->addItem(QString("%1, %2, %3") + ui->listWidget->addItem("model position (m)"); + ui->listWidget->addItem(QString("%1, %2, %3") .arg(posX, 7, 'f', 4) .arg(posY, 7, 'f', 4) .arg(posZ, 7, 'f', 4)); - ui->listWidget->addItem("model velocity (m/s)"); - ui->listWidget->addItem(QString("%1, %2, %3") + ui->listWidget->addItem("model velocity (m/s)"); + ui->listWidget->addItem(QString("%1, %2, %3") .arg(velX, 7, 'f', 4) .arg(velY, 7, 'f', 4) .arg(velZ, 7, 'f', 4)); - ui->listWidget->addItem("model angular velocity (rad/s)"); - ui->listWidget->addItem(QString("%1, %2, %3") + ui->listWidget->addItem("model angular velocity (rad/s)"); + ui->listWidget->addItem(QString("%1, %2, %3") .arg(angX, 7, 'f', 4) .arg(angY, 7, 'f', 4) .arg(angZ, 7, 'f', 4)); - ui->listWidget->addItem("model acceleration (m/s/s)"); - ui->listWidget->addItem(QString("%1, %2, %3") + ui->listWidget->addItem("model acceleration (m/s/s)"); + ui->listWidget->addItem(QString("%1, %2, %3") .arg(accX, 7, 'f', 4) .arg(accY, 7, 'f', 4) .arg(accZ, 7, 'f', 4)); - ui->listWidget->addItem("model coordinates (deg, deg, m)"); - ui->listWidget->addItem(QString("%1, %2, %3") + ui->listWidget->addItem("model coordinates (deg, deg, m)"); + ui->listWidget->addItem(QString("%1, %2, %3") .arg(lat, 7, 'f', 4) .arg(lon, 7, 'f', 4) .arg(alt, 7, 'f', 4)); - ui->listWidget->addItem("model electrics"); - ui->listWidget->addItem(QString("%1V, %2A") + ui->listWidget->addItem("model electrics"); + ui->listWidget->addItem(QString("%1V, %2A") .arg(volt, 7, 'f', 4) .arg(curr, 7, 'f', 4)); - ui->listWidget->addItem("channels"); - ui->listWidget->addItem(QString("%1 %2 %3 %4 %5 %6 %7 %8") + ui->listWidget->addItem("channels"); + ui->listWidget->addItem(QString("%1 %2 %3 %4 %5 %6 %7 %8") .arg(chAil, 6, 'f', 3) .arg(chEle, 6, 'f', 3) .arg(chThr, 6, 'f', 3) @@ -240,18 +244,18 @@ void Widget::processDatagram(const QByteArray &data) .arg(chPlg2, 6, 'f', 3) .arg(chFpv1, 6, 'f', 3) .arg(chFpv2, 6, 'f', 3)); - ui->listWidget->addItem("datagram size (bytes), packet counter"); - ui->listWidget->addItem(QString("%1 %2") + ui->listWidget->addItem("datagram size (bytes), packet counter"); + ui->listWidget->addItem(QString("%1 %2") .arg(data.size()) .arg(packetCounter)); -*/ + */ // matrix calculation start // model matrix - QMatrix4x4 m = QMatrix4x4( fy, fx, -fz, 0.0, - ry, rx, -rz, 0.0, - -uy, -ux, uz, 0.0, - 0.0, 0.0, 0.0, 1.0); + QMatrix4x4 m = QMatrix4x4(fy, fx, -fz, 0.0, + ry, rx, -rz, 0.0, + -uy, -ux, uz, 0.0, + 0.0, 0.0, 0.0, 1.0); m.optimize(); // world matrix @@ -301,9 +305,9 @@ void Widget::processDatagram(const QByteArray &data) .arg(q.scalar(), 7, 'f', 4)); ui->listWidget->addItem("model attitude (deg)"); ui->listWidget->addItem(QString("%1, %2, %3") - .arg(roll*RAD2DEG, 7, 'f', 4) - .arg(pitch*RAD2DEG, 7, 'f', 4) - .arg(head*RAD2DEG, 7, 'f', 4)); + .arg(roll * RAD2DEG, 7, 'f', 4) + .arg(pitch * RAD2DEG, 7, 'f', 4) + .arg(head * RAD2DEG, 7, 'f', 4)); ui->listWidget->addItem("CC attitude calculated (deg)"); ui->listWidget->addItem(QString("%1, %2, %3") .arg(rpy.x(), 7, 'f', 4) @@ -311,16 +315,16 @@ void Widget::processDatagram(const QByteArray &data) .arg(rpy.z(), 7, 'f', 4)); screenTimeout.restart(); - } else if (magic == 0x52434D44) { // "RCMD" qreal ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8, ch9, ch10; stream >> ch1 >> ch2 >> ch3 >> ch4 >> ch5 >> ch6 >> ch7 >> ch8 >> ch9 >> ch10; quint8 armed, mode; stream >> armed >> mode; - if(ui->tabWidget->currentIndex() == 0) { - if (screenTimeout.elapsed() < 200) + if (ui->tabWidget->currentIndex() == 0) { + if (screenTimeout.elapsed() < 200) { return; + } ui->listWidget->clear(); ui->listWidget->addItem("channels"); ui->listWidget->addItem("CH1: " + QString::number(ch1)); @@ -344,10 +348,11 @@ void Widget::processDatagram(const QByteArray &data) void Widget::sendDatagram() { - if(!outSocket) + if (!outSocket) { return; + } - float ch[10]; // = {0,0,0,0,0,0,0,0,0,0}; + float ch[10]; // = {0,0,0,0,0,0,0,0,0,0}; quint8 armed; quint8 fmode; const float coeff = 1.0 / 512.0; @@ -430,30 +435,31 @@ void Widget::asQuat2RPY(const QQuaternion &q, QVector3D &rpy) qreal pitch; qreal yaw; - const qreal d2 = 2.0; + const qreal d2 = 2.0; const qreal qss = q.scalar() * q.scalar(); const qreal qxx = q.x() * q.x(); const qreal qyy = q.y() * q.y(); const qreal qzz = q.z() * q.z(); qreal test = -d2 * (q.x() * q.z() - q.scalar() * q.y()); + if (qFabs(test) > 0.998) { // ~86.3°, gimbal lock qreal R10 = d2 * (q.x() * q.y() - q.scalar() * q.z()); qreal R11 = qss - qxx + qyy - qzz; - roll = 0.0; + roll = 0.0; pitch = copysign(M_PI_2, test); - yaw = qAtan2(-R10, R11); + yaw = qAtan2(-R10, R11); } else { qreal R12 = d2 * (q.y() * q.z() + q.scalar() * q.x()); qreal R22 = qss - qxx - qyy + qzz; qreal R01 = d2 * (q.x() * q.y() + q.scalar() * q.z()); qreal R00 = qss + qxx - qyy - qzz; - roll = qAtan2(R12, R22); - pitch = qAsin(test); - yaw = qAtan2(R01, R00); + roll = qAtan2(R12, R22); + pitch = qAsin(test); + yaw = qAtan2(R01, R00); } rpy.setX(RAD2DEG * roll); rpy.setY(RAD2DEG * pitch); @@ -477,15 +483,15 @@ void Widget::asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy) yaw = qAtan2(m(0, 1), m(0, 0)); } - rpy.setX(roll * RAD2DEG); + rpy.setX(roll * RAD2DEG); rpy.setY(pitch * RAD2DEG); - rpy.setZ(yaw * RAD2DEG); + rpy.setZ(yaw * RAD2DEG); } /* // not used -void Widget::ccRPY2Quat(const QVector3D &rpy, QQuaternion &q) -{ + void Widget::ccRPY2Quat(const QVector3D &rpy, QQuaternion &q) + { float phi, theta, psi; float cphi, sphi, ctheta, stheta, cpsi, spsi; @@ -510,10 +516,10 @@ void Widget::ccRPY2Quat(const QVector3D &rpy, QQuaternion &q) q.setY(-q.y()); q.setZ(-q.z()); } -} + } -void Widget::ccQuat2Matrix(const QQuaternion &q, QMatrix4x4 &m) -{ + void Widget::ccQuat2Matrix(const QQuaternion &q, QMatrix4x4 &m) + { float q0s = q.scalar() * q.scalar(); float q1s = q.x() * q.x(); float q2s = q.y() * q.y(); @@ -533,5 +539,5 @@ void Widget::ccQuat2Matrix(const QQuaternion &q, QMatrix4x4 &m) m10, m11, m12, 0, m20, m21, m22, 0, 0, 0, 0, 1); -} -*/ + } + */ diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.h index 500f35482..115985552 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.h @@ -38,14 +38,13 @@ #include namespace Ui { - class Widget; +class Widget; } -const float RAD2DEG = (float)(180.0/M_PI); -const float DEG2RAD = (float)(M_PI/180.0); +const float RAD2DEG = (float)(180.0 / M_PI); +const float DEG2RAD = (float)(M_PI / 180.0); -class Widget : public QWidget -{ +class Widget : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp index 09f96baa5..d829bd937 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp @@ -37,12 +37,12 @@ AeroSimRCSimulator::AeroSimRCSimulator(const SimulatorSettings ¶ms) } AeroSimRCSimulator::~AeroSimRCSimulator() -{ -} +{} bool AeroSimRCSimulator::setupProcess() { QMutexLocker locker(&lock); + return true; } @@ -60,6 +60,7 @@ void AeroSimRCSimulator::transmitUpdate() { // read actuator output ActuatorCommand::DataFields actCmdData; + actCmdData = actCommand->getData(); float channels[10]; for (int i = 0; i < 10; ++i) { @@ -73,34 +74,34 @@ void AeroSimRCSimulator::transmitUpdate() } ActuatorDesired::DataFields actData; - FlightStatus::DataFields flightStatusData = flightStatus->getData(); + FlightStatus::DataFields flightStatusData = flightStatus->getData(); ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData(); - float roll = -1; - float pitch = -1; - float yaw = -1; + float roll = -1; + float pitch = -1; + float yaw = -1; float throttle = -1; if (flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) { // Read joystick input if (flightStatusData.Armed == FlightStatus::ARMED_ARMED) { // Note: Pitch sign is reversed in FG ? - roll = manCtrlData.Roll; - pitch = -manCtrlData.Pitch; - yaw = manCtrlData.Yaw; + roll = manCtrlData.Roll; + pitch = -manCtrlData.Pitch; + yaw = manCtrlData.Yaw; throttle = manCtrlData.Throttle; } } else { // Read ActuatorDesired from autopilot - actData = actDesired->getData(); + actData = actDesired->getData(); - roll = actData.Roll; - pitch = -actData.Pitch; - yaw = actData.Yaw; - throttle = (actData.Throttle*2.0)-1.0; + roll = actData.Roll; + pitch = -actData.Pitch; + yaw = actData.Yaw; + throttle = (actData.Throttle * 2.0) - 1.0; } channels[0] = roll; channels[1] = pitch; - if(throttle < -1) { + if (throttle < -1) { throttle = -1; } channels[2] = throttle; @@ -110,19 +111,19 @@ void AeroSimRCSimulator::transmitUpdate() quint8 armed; quint8 mode; armed = flightStatusData.Armed; - mode = flightStatusData.FlightMode; + mode = flightStatusData.FlightMode; QByteArray data; // 50 - current size of values, 4(quint32) + 10*4(float) + 2(quint8) + 4(quint32) data.resize(50); QDataStream stream(&data, QIODevice::WriteOnly); stream.setFloatingPointPrecision(QDataStream::SinglePrecision); - stream << quint32(0x52434D44); // magic header, "RCMD" + stream << quint32(0x52434D44); // magic header, "RCMD" for (int i = 0; i < 10; ++i) { - stream << channels[i]; // channels + stream << channels[i]; // channels } - stream << armed << mode; // flight status - stream << udpCounterASrecv; // packet counter + stream << armed << mode; // flight status + stream << udpCounterASrecv; // packet counter if (outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { qDebug() << "write failed: " << outSocket->errorString(); @@ -154,25 +155,25 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data) // check magic header quint32 magic; stream >> magic; - if (magic != 0x4153494D) { // "AERO" + if (magic != 0x4153494D) { // "AERO" qDebug() << "wrong magic: " << magic << ", correct: " << quint32(0x4153494D); return; } #define AEROSIM_RCCHANNEL_NUMELEM 8 - float delT, - homeX, homeY, homeZ, - WpHX, WpHY, WpLat, WpLon, - posX, posY, posZ, // world - velX, velY, velZ, // world - angX, angY, angZ, // model - accX, accY, accZ; // model - qreal lat, lon; - float agl, // world - yaw, pitch, roll, // model - volt, curr, cons, - rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix - ch[AEROSIM_RCCHANNEL_NUMELEM]; + float delT, + homeX, homeY, homeZ, + WpHX, WpHY, WpLat, WpLon, + posX, posY, posZ, // world + velX, velY, velZ, // world + angX, angY, angZ, // model + accX, accY, accZ; // model + qreal lat, lon; + float agl, // world + yaw, pitch, roll, // model + volt, curr, cons, + rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix + ch[AEROSIM_RCCHANNEL_NUMELEM]; stream >> delT; stream >> homeX >> homeY >> homeZ; @@ -195,65 +196,65 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data) out.delT = delT; /*************************************************************************************/ - for (int i=0; i< AEROSIM_RCCHANNEL_NUMELEM; i++) { - out.rc_channel[i]=ch[i]; //Elements in rc_channel are between -1 and 1 + for (int i = 0; i < AEROSIM_RCCHANNEL_NUMELEM; i++) { + out.rc_channel[i] = ch[i]; // Elements in rc_channel are between -1 and 1 } /**********************************************************************************************/ QMatrix4x4 mat; - mat = QMatrix4x4( fy, fx, -fz, 0.0, // model matrix - ry, rx, -rz, 0.0, // (X,Y,Z) -> (+Y,+X,-Z) - -uy, -ux, uz, 0.0, - 0.0, 0.0, 0.0, 1.0); + mat = QMatrix4x4(fy, fx, -fz, 0.0, // model matrix + ry, rx, -rz, 0.0, // (X,Y,Z) -> (+Y,+X,-Z) + -uy, -ux, uz, 0.0, + 0.0, 0.0, 0.0, 1.0); mat.optimize(); - QQuaternion quat; // model quat + QQuaternion quat; // model quat asMatrix2Quat(mat, quat); /*************************************************************************************/ // rotate gravity - QVector3D acc = QVector3D(accY, accX, -accZ); // accel (X,Y,Z) -> (+Y,+X,-Z) + QVector3D acc = QVector3D(accY, accX, -accZ); // accel (X,Y,Z) -> (+Y,+X,-Z) QVector3D gee = QVector3D(0.0, 0.0, -GEE); QQuaternion qWorld = quat.conjugate(); - gee = qWorld.rotatedVector(gee); + gee = qWorld.rotatedVector(gee); acc += gee; - out.rollRate = angY * RAD2DEG; // gyros (X,Y,Z) -> (+Y,+X,-Z) + out.rollRate = angY * RAD2DEG; // gyros (X,Y,Z) -> (+Y,+X,-Z) out.pitchRate = angX * RAD2DEG; - out.yawRate = angZ * -RAD2DEG; + out.yawRate = angZ * -RAD2DEG; out.accX = acc.x(); out.accY = acc.y(); out.accZ = acc.z(); /*************************************************************************************/ - QVector3D rpy; // model roll, pitch, yaw + QVector3D rpy; // model roll, pitch, yaw asMatrix2RPY(mat, rpy); - out.roll = rpy.x(); - out.pitch = rpy.y(); + out.roll = rpy.x(); + out.pitch = rpy.y(); out.heading = rpy.z(); /**********************************************************************************************/ - out.altitude = posZ; + out.altitude = posZ; out.agl = posZ; - out.heading = yaw * RAD2DEG; - out.latitude = lat * 10e6; - out.longitude = lon * 10e6; + out.heading = yaw * RAD2DEG; + out.latitude = lat * 10e6; + out.longitude = lon * 10e6; out.groundspeed = qSqrt(velX * velX + velY * velY); /**********************************************************************************************/ - out.dstN = posY; - out.dstE = posX; - out.dstD = -posZ; + out.dstN = posY; + out.dstE = posX; + out.dstD = -posZ; - out.velNorth = velY; - out.velEast = velX; - out.velDown = -velZ; + out.velNorth = velY; + out.velEast = velX; + out.velDown = -velZ; - out.voltage = volt; - out.current = curr; + out.voltage = volt; + out.current = curr; out.consumption = cons * 1000.0; updateUAVOs(out); @@ -306,7 +307,7 @@ void AeroSimRCSimulator::asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy) yaw = qAtan2(m(0, 1), m(0, 0)); } - rpy.setX(roll * RAD2DEG); + rpy.setX(roll * RAD2DEG); rpy.setY(pitch * RAD2DEG); - rpy.setZ(yaw * RAD2DEG); + rpy.setZ(yaw * RAD2DEG); } diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.h index b265b9463..1d9441991 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.h @@ -34,8 +34,7 @@ #include #include "simulator.h" -class AeroSimRCSimulator: public Simulator -{ +class AeroSimRCSimulator : public Simulator { Q_OBJECT public: @@ -43,13 +42,13 @@ public: ~AeroSimRCSimulator(); bool setupProcess(); - void setupUdpPorts(const QString& host, int inPort, int outPort); + void setupUdpPorts(const QString & host, int inPort, int outPort); private slots: void transmitUpdate(); private: - quint32 udpCounterASrecv; //keeps track of udp packets received by ASim + quint32 udpCounterASrecv; // keeps track of udp packets received by ASim void processUpdate(const QByteArray &data); @@ -57,14 +56,13 @@ private: void asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy); }; -class AeroSimRCSimulatorCreator : public SimulatorCreator -{ +class AeroSimRCSimulatorCreator : public SimulatorCreator { public: AeroSimRCSimulatorCreator(const QString &classId, const QString &description) - : SimulatorCreator (classId, description) + : SimulatorCreator(classId, description) {} - Simulator* createSimulator(const SimulatorSettings ¶ms) + Simulator *createSimulator(const SimulatorSettings ¶ms) { return new AeroSimRCSimulator(params); } diff --git a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp index 77fd8ecba..790265ea2 100644 --- a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp @@ -31,25 +31,25 @@ #include "coreplugin/threadmanager.h" #ifndef M_PI -#define M_PI 3.14159265358979323846 +#define M_PI 3.14159265358979323846 #endif -//FGSimulator::FGSimulator(QString hostAddr, int outPort, int inPort, bool manual, QString binPath, QString dataPath) : -// Simulator(hostAddr, outPort, inPort, manual, binPath, dataPath), -// fgProcess(NULL) -//{ -// // Note: Only tested on windows 7 -//#if defined(Q_WS_WIN) -// cmdShell = QString("c:/windows/system32/cmd.exe"); -//#else -// cmdShell = QString("bash"); -//#endif -//} +// FGSimulator::FGSimulator(QString hostAddr, int outPort, int inPort, bool manual, QString binPath, QString dataPath) : +// Simulator(hostAddr, outPort, inPort, manual, binPath, dataPath), +// fgProcess(NULL) +// { +//// Note: Only tested on windows 7 +// #if defined(Q_WS_WIN) +// cmdShell = QString("c:/windows/system32/cmd.exe"); +// #else +// cmdShell = QString("bash"); +// #endif +// } -FGSimulator::FGSimulator(const SimulatorSettings& params) : +FGSimulator::FGSimulator(const SimulatorSettings & params) : Simulator(params) { - udpCounterFGrecv = 0; + udpCounterFGrecv = 0; udpCounterGCSsend = 0; } @@ -58,14 +58,15 @@ FGSimulator::~FGSimulator() disconnect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead())); } -void FGSimulator::setupUdpPorts(const QString& host, int inPort, int outPort) +void FGSimulator::setupUdpPorts(const QString & host, int inPort, int outPort) { Q_UNUSED(outPort); - if(inSocket->bind(QHostAddress(host), inPort)) + if (inSocket->bind(QHostAddress(host), inPort)) { emit processOutput("Successfully bound to address " + host + " on port " + QString::number(inPort) + "\n"); - else + } else { emit processOutput("Cannot bind to address " + host + " on port " + QString::number(inPort) + "\n"); + } } bool FGSimulator::setupProcess() @@ -75,17 +76,18 @@ bool FGSimulator::setupProcess() // Copy FlightGear generic protocol configuration file to the FG protocol directory // NOTE: Not working on Windows 7, if FG is installed in the "Program Files", // likelly due to permissions. The file should be manually copied to data/Protocol/opfgprotocol.xml - // QFile xmlFile(":/flightgear/genericprotocol/opfgprotocol.xml"); - // xmlFile.open(QIODevice::ReadOnly | QIODevice::Text); - // QString xml = xmlFile.readAll(); - // xmlFile.close(); - // QFile xmlFileOut(pathData + "/Protocol/opfgprotocol.xml"); - // xmlFileOut.open(QIODevice::WriteOnly | QIODevice::Text); - // xmlFileOut.write(xml.toAscii()); - // xmlFileOut.close(); + // QFile xmlFile(":/flightgear/genericprotocol/opfgprotocol.xml"); + // xmlFile.open(QIODevice::ReadOnly | QIODevice::Text); + // QString xml = xmlFile.readAll(); + // xmlFile.close(); + // QFile xmlFileOut(pathData + "/Protocol/opfgprotocol.xml"); + // xmlFileOut.open(QIODevice::WriteOnly | QIODevice::Text); + // xmlFileOut.write(xml.toAscii()); + // xmlFileOut.close(); Qt::HANDLE mainThread = QThread::currentThreadId(); - qDebug() << "setupProcess Thread: "<< mainThread; + + qDebug() << "setupProcess Thread: " << mainThread; simProcess = new QProcess(); simProcess->setReadChannelMode(QProcess::MergedChannels); @@ -99,8 +101,7 @@ bool FGSimulator::setupProcess() // Start shell (Note: Could not start FG directly on Windows, only through terminal!) simProcess->start(cmdShell); - if (simProcess->waitForStarted() == false) - { + if (simProcess->waitForStarted() == false) { emit processOutput("Error:" + simProcess->errorString()); return false; } @@ -117,19 +118,15 @@ bool FGSimulator::setupProcess() "--vc=100 " + "--log-level=alert " + "--generic=socket,out,20," + settings.hostAddress + "," + QString::number(settings.inPort) + ",udp,opfgprotocol"); - if(settings.manualControlEnabled) // <--[BCH] What does this do? Why does it depend on ManualControl? - { + if (settings.manualControlEnabled) { // <--[BCH] What does this do? Why does it depend on ManualControl? args.append(" --generic=socket,in,400," + settings.remoteAddress + "," + QString::number(settings.outPort) + ",udp,opfgprotocol"); } // Start FlightGear - only if checkbox is selected in HITL options page - if (settings.startSim) - { + if (settings.startSim) { QString cmd("\"" + settings.binPath + "\" " + args + "\n"); simProcess->write(cmd.toAscii()); - } - else - { + } else { emit processOutput("Start Flightgear from the command line with the following arguments: \n\n" + args + "\n\n" + "You can optionally run Flightgear from a networked computer.\n" + "Make sure the computer running Flightgear can can ping your local interface adapter. ie." + settings.hostAddress + "\n" @@ -145,8 +142,8 @@ void FGSimulator::processReadyRead() { QByteArray bytes = simProcess->readAllStandardOutput(); QString str(bytes); - if ( !str.contains("Error reading data") ) // ignore error - { + + if (!str.contains("Error reading data")) { // ignore error emit processOutput(str); } } @@ -154,140 +151,133 @@ void FGSimulator::processReadyRead() void FGSimulator::transmitUpdate() { ActuatorDesired::DataFields actData; - FlightStatus::DataFields flightStatusData = flightStatus->getData(); + FlightStatus::DataFields flightStatusData = flightStatus->getData(); ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData(); float ailerons = -1; float elevator = -1; - float rudder = -1; + float rudder = -1; float throttle = -1; - if(flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) - { + if (flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) { // Read joystick input - if(flightStatusData.Armed == FlightStatus::ARMED_ARMED) - { + if (flightStatusData.Armed == FlightStatus::ARMED_ARMED) { // Note: Pitch sign is reversed in FG ? ailerons = manCtrlData.Roll; elevator = -manCtrlData.Pitch; - rudder = manCtrlData.Yaw; + rudder = manCtrlData.Yaw; throttle = manCtrlData.Throttle; } - } - else - { + } else { // Read ActuatorDesired from autopilot - actData = actDesired->getData(); + actData = actDesired->getData(); ailerons = actData.Roll; elevator = -actData.Pitch; - rudder = actData.Yaw; + rudder = actData.Yaw; throttle = actData.Throttle; } int allowableDifference = 10; - //qDebug() << "UDP sent:" << udpCounterGCSsend << " - UDP Received:" << udpCounterFGrecv; + // qDebug() << "UDP sent:" << udpCounterGCSsend << " - UDP Received:" << udpCounterFGrecv; - if(udpCounterFGrecv == udpCounterGCSsend) + if (udpCounterFGrecv == udpCounterGCSsend) { udpCounterGCSsend = 0; - - if((udpCounterGCSsend < allowableDifference) || (udpCounterFGrecv==0) ) //FG udp queue is not delayed - { + } + + if ((udpCounterGCSsend < allowableDifference) || (udpCounterFGrecv == 0)) { // FG udp queue is not delayed udpCounterGCSsend++; // Send update to FlightGear QString cmd; cmd = QString("%1,%2,%3,%4,%5\n") - .arg(ailerons) //ailerons - .arg(elevator) //elevator - .arg(rudder) //rudder - .arg(throttle) //throttle - .arg(udpCounterGCSsend); //UDP packet counter delay + .arg(ailerons) // ailerons + .arg(elevator) // elevator + .arg(rudder) // rudder + .arg(throttle) // throttle + .arg(udpCounterGCSsend); // UDP packet counter delay QByteArray data = cmd.toAscii(); - if(outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) - { + if (outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { emit processOutput("Error sending UDP packet to FG: " + outSocket->errorString() + "\n"); } - } - else - { + } else { // don't send new packet. Flightgear cannot process UDP fast enough. // V1.9.1 reads udp packets at set frequency and will get delayed if packets are sent too fast // V2.0 does not currently work with --generic-protocol } - - if(settings.manualControlEnabled) - { - actData.Roll = ailerons; - actData.Pitch = -elevator; - actData.Yaw = rudder; + + if (settings.manualControlEnabled) { + actData.Roll = ailerons; + actData.Pitch = -elevator; + actData.Yaw = rudder; actData.Throttle = throttle; - //actData.NumLongUpdates = (float)udpCounterFGrecv; - //actData.UpdateTime = (float)udpCounterGCSsend; + // actData.NumLongUpdates = (float)udpCounterFGrecv; + // actData.UpdateTime = (float)udpCounterGCSsend; actDesired->setData(actData); } } -void FGSimulator::processUpdate(const QByteArray& inp) +void FGSimulator::processUpdate(const QByteArray & inp) { - //TODO: this does not use the FLIGHT_PARAM structure, it should! + // TODO: this does not use the FLIGHT_PARAM structure, it should! // Split QString data(inp); QStringList fields = data.split(","); // Get xRate (deg/s) - // float xRate = fields[0].toFloat() * 180.0/M_PI; + // float xRate = fields[0].toFloat() * 180.0/M_PI; // Get yRate (deg/s) - // float yRate = fields[1].toFloat() * 180.0/M_PI; + // float yRate = fields[1].toFloat() * 180.0/M_PI; // Get zRate (deg/s) - // float zRate = fields[2].toFloat() * 180.0/M_PI; + // float zRate = fields[2].toFloat() * 180.0/M_PI; // Get xAccel (m/s^2) - float xAccel = fields[3].toFloat() * FT2M; + float xAccel = fields[3].toFloat() * FT2M; // Get yAccel (m/s^2) - float yAccel = fields[4].toFloat() * FT2M; + float yAccel = fields[4].toFloat() * FT2M; // Get xAccel (m/s^2) - float zAccel = fields[5].toFloat() * FT2M; + float zAccel = fields[5].toFloat() * FT2M; // Get pitch (deg) - float pitch = fields[6].toFloat(); + float pitch = fields[6].toFloat(); // Get pitchRate (deg/s) float pitchRate = fields[7].toFloat(); // Get roll (deg) - float roll = fields[8].toFloat(); + float roll = fields[8].toFloat(); // Get rollRate (deg/s) float rollRate = fields[9].toFloat(); // Get yaw (deg) - float yaw = fields[10].toFloat(); + float yaw = fields[10].toFloat(); // Get yawRate (deg/s) - float yawRate = fields[11].toFloat(); + float yawRate = fields[11].toFloat(); // Get latitude (deg) - float latitude = fields[12].toFloat(); + float latitude = fields[12].toFloat(); // Get longitude (deg) - float longitude = fields[13].toFloat(); + float longitude = fields[13].toFloat(); // Get heading (deg) - float heading = fields[14].toFloat(); + float heading = fields[14].toFloat(); // Get altitude (m) float altitude_msl = fields[15].toFloat() * FT2M; // Get altitudeAGL (m) float altitude_agl = fields[16].toFloat() * FT2M; // Get groundspeed (m/s) - float groundspeed = fields[17].toFloat() * KT2MPS; + float groundspeed = fields[17].toFloat() * KT2MPS; // Get airspeed (m/s) - float airspeed = fields[18].toFloat() * KT2MPS; + float airspeed = fields[18].toFloat() * KT2MPS; // Get temperature (degC) - float temperature = fields[19].toFloat(); + float temperature = fields[19].toFloat(); // Get pressure (kpa) - float pressure = fields[20].toFloat() * INHG2KPA; + float pressure = fields[20].toFloat() * INHG2KPA; // Get VelocityActual Down (cm/s) - float velocityActualDown = - fields[21].toFloat() * FPS2CMPS; + float velocityActualDown = -fields[21].toFloat() * FPS2CMPS; // Get VelocityActual East (cm/s) - float velocityActualEast = fields[22].toFloat() * FPS2CMPS; + float velocityActualEast = fields[22].toFloat() * FPS2CMPS; // Get VelocityActual Down (cm/s) float velocityActualNorth = fields[23].toFloat() * FPS2CMPS; // Get UDP packets received by FG int n = fields[24].toInt(); + udpCounterFGrecv = n; /////// @@ -299,18 +289,18 @@ void FGSimulator::processUpdate(const QByteArray& inp) float NED[3]; // convert from cm back to meters - double LLA[3] = {latitude, longitude, altitude_msl}; + double LLA[3] = { latitude, longitude, altitude_msl }; double ECEF[3]; double RNE[9]; - Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE); - Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF); - Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float (*)[3]) RNE, NED); + Utils::CoordinateConversions().RneFromLLA(LLA, (double(*)[3])RNE); + Utils::CoordinateConversions().LLA2ECEF(LLA, ECEF); + Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float(*)[3])RNE, NED); // Update GPS Position objects - out.latitude = latitude * 1e7; - out.longitude = longitude * 1e7; - out.altitude = altitude_msl; + out.latitude = latitude * 1e7; + out.longitude = longitude * 1e7; + out.altitude = altitude_msl; out.agl = altitude_agl; out.groundspeed = groundspeed; @@ -319,32 +309,31 @@ void FGSimulator::processUpdate(const QByteArray& inp) // Update BaroAltitude object out.temperature = temperature; - out.pressure = pressure; + out.pressure = pressure; // Update attActual object - out.roll = roll; //roll; - out.pitch = pitch; // pitch - out.heading = yaw; // yaw + out.roll = roll; // roll; + out.pitch = pitch; // pitch + out.heading = yaw; // yaw - out.dstN= NED[0]; - out.dstE= NED[1]; - out.dstD= NED[2]; + out.dstN = NED[0]; + out.dstE = NED[1]; + out.dstD = NED[2]; // Update VelocityActual.{North,East,Down} - out.velNorth = velocityActualNorth; - out.velEast = velocityActualEast; - out.velDown = velocityActualDown; + out.velNorth = velocityActualNorth; + out.velEast = velocityActualEast; + out.velDown = velocityActualDown; - //Update gyroscope sensor data - out.rollRate = rollRate; + // Update gyroscope sensor data + out.rollRate = rollRate; out.pitchRate = pitchRate; - out.yawRate = yawRate; + out.yawRate = yawRate; - //Update accelerometer sensor data - out.accX = xAccel; - out.accY = yAccel; - out.accZ = -zAccel; + // Update accelerometer sensor data + out.accX = xAccel; + out.accY = yAccel; + out.accZ = -zAccel; updateUAVOs(out); } - diff --git a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.h b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.h index 9405389e6..be5e48539 100644 --- a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.h @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -31,40 +31,37 @@ #include #include "simulator.h" -class FGSimulator: public Simulator -{ +class FGSimulator : public Simulator { Q_OBJECT public: - FGSimulator(const SimulatorSettings& params); - ~FGSimulator(); + FGSimulator(const SimulatorSettings & params); + ~FGSimulator(); - bool setupProcess(); - void setupUdpPorts(const QString& host, int inPort, int outPort); + bool setupProcess(); + void setupUdpPorts(const QString & host, int inPort, int outPort); private slots: void transmitUpdate(); - void processReadyRead(); + void processReadyRead(); private: - int udpCounterGCSsend; //keeps track of udp packets sent to FG - int udpCounterFGrecv; //keeps track of udp packets received by FG + int udpCounterGCSsend; // keeps track of udp packets sent to FG + int udpCounterFGrecv; // keeps track of udp packets received by FG - void processUpdate(const QByteArray& data); + void processUpdate(const QByteArray & data); }; -class FGSimulatorCreator : public SimulatorCreator -{ +class FGSimulatorCreator : public SimulatorCreator { public: - FGSimulatorCreator(const QString& classId, const QString& description) - : SimulatorCreator (classId,description) - {} - - Simulator* createSimulator(const SimulatorSettings& params) - { - return new FGSimulator(params); - } + FGSimulatorCreator(const QString & classId, const QString & description) + : SimulatorCreator(classId, description) + {} + Simulator *createSimulator(const SimulatorSettings & params) + { + return new FGSimulator(params); + } }; #endif // FGSIMULATOR_H diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp index 3055ec1df..244c27b6c 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp @@ -7,125 +7,124 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "hitlconfiguration.h" -HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +HITLConfiguration::HITLConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent) { - - //Default settings values - settings.simulatorId = ""; - settings.binPath = ""; - settings.dataPath = ""; - settings.manualControlEnabled = true; - settings.startSim = false; - settings.addNoise = false; - settings.hostAddress = "127.0.0.1"; - settings.remoteAddress = "127.0.0.1"; - settings.outPort = 0; + // Default settings values + settings.simulatorId = ""; + settings.binPath = ""; + settings.dataPath = ""; + settings.manualControlEnabled = true; + settings.startSim = false; + settings.addNoise = false; + settings.hostAddress = "127.0.0.1"; + settings.remoteAddress = "127.0.0.1"; + settings.outPort = 0; settings.inPort = 0; - settings.latitude = ""; - settings.longitude = ""; + settings.latitude = ""; + settings.longitude = ""; - settings.attRawEnabled = false; - settings.attRawRate = 20; + settings.attRawEnabled = false; + settings.attRawRate = 20; - settings.attActualEnabled = true; - settings.attActHW = false; - settings.attActSim = true; - settings.attActCalc = false; + settings.attActualEnabled = true; + settings.attActHW = false; + settings.attActSim = true; + settings.attActCalc = false; - settings.gpsPositionEnabled = false; - settings.gpsPosRate = 100; + settings.gpsPositionEnabled = false; + settings.gpsPosRate = 100; - settings.groundTruthEnabled = false; - settings.groundTruthRate = 100; + settings.groundTruthEnabled = false; + settings.groundTruthRate = 100; - settings.inputCommand = false; - settings.gcsReceiverEnabled = false; - settings.manualControlEnabled= false; - settings.minOutputPeriod = 100; + settings.inputCommand = false; + settings.gcsReceiverEnabled = false; + settings.manualControlEnabled = false; + settings.minOutputPeriod = 100; - settings.airspeedActualEnabled= false; - settings.airspeedActualRate = 100; + settings.airspeedActualEnabled = false; + settings.airspeedActualRate = 100; // if a saved configuration exists load it, and overwrite defaults if (qSettings != 0) { + settings.simulatorId = qSettings->value("simulatorId").toString(); + settings.binPath = qSettings->value("binPath").toString(); + settings.dataPath = qSettings->value("dataPath").toString(); - settings.simulatorId = qSettings->value("simulatorId").toString(); - settings.binPath = qSettings->value("binPath").toString(); - settings.dataPath = qSettings->value("dataPath").toString(); + settings.hostAddress = qSettings->value("hostAddress").toString(); + settings.remoteAddress = qSettings->value("remoteAddress").toString(); + settings.outPort = qSettings->value("outPort").toInt(); + settings.inPort = qSettings->value("inPort").toInt(); - settings.hostAddress = qSettings->value("hostAddress").toString(); - settings.remoteAddress = qSettings->value("remoteAddress").toString(); - settings.outPort = qSettings->value("outPort").toInt(); - settings.inPort = qSettings->value("inPort").toInt(); + settings.latitude = qSettings->value("latitude").toString(); + settings.longitude = qSettings->value("longitude").toString(); + settings.startSim = qSettings->value("startSim").toBool(); + settings.addNoise = qSettings->value("noiseCheckBox").toBool(); - settings.latitude = qSettings->value("latitude").toString(); - settings.longitude = qSettings->value("longitude").toString(); - settings.startSim = qSettings->value("startSim").toBool(); - settings.addNoise = qSettings->value("noiseCheckBox").toBool(); + settings.gcsReceiverEnabled = qSettings->value("gcsReceiverEnabled").toBool(); + settings.manualControlEnabled = qSettings->value("manualControlEnabled").toBool(); - settings.gcsReceiverEnabled = qSettings->value("gcsReceiverEnabled").toBool(); - settings.manualControlEnabled= qSettings->value("manualControlEnabled").toBool(); + settings.attRawEnabled = qSettings->value("attRawEnabled").toBool(); + settings.attRawRate = qSettings->value("attRawRate").toInt(); - settings.attRawEnabled = qSettings->value("attRawEnabled").toBool(); - settings.attRawRate = qSettings->value("attRawRate").toInt(); + settings.attActualEnabled = qSettings->value("attActualEnabled").toBool(); + settings.attActHW = qSettings->value("attActHW").toBool(); + settings.attActSim = qSettings->value("attActSim").toBool(); + settings.attActCalc = qSettings->value("attActCalc").toBool(); - settings.attActualEnabled = qSettings->value("attActualEnabled").toBool(); - settings.attActHW = qSettings->value("attActHW").toBool(); - settings.attActSim = qSettings->value("attActSim").toBool(); - settings.attActCalc = qSettings->value("attActCalc").toBool(); + settings.baroAltitudeEnabled = qSettings->value("baroAltitudeEnabled").toBool(); + settings.baroAltRate = qSettings->value("baroAltRate").toInt(); - settings.baroAltitudeEnabled = qSettings->value("baroAltitudeEnabled").toBool(); - settings.baroAltRate = qSettings->value("baroAltRate").toInt(); + settings.gpsPositionEnabled = qSettings->value("gpsPositionEnabled").toBool(); + settings.gpsPosRate = qSettings->value("gpsPosRate").toInt(); - settings.gpsPositionEnabled = qSettings->value("gpsPositionEnabled").toBool(); - settings.gpsPosRate = qSettings->value("gpsPosRate").toInt(); + settings.groundTruthEnabled = qSettings->value("groundTruthEnabled").toBool(); + settings.groundTruthRate = qSettings->value("groundTruthRate").toInt(); - settings.groundTruthEnabled = qSettings->value("groundTruthEnabled").toBool(); - settings.groundTruthRate = qSettings->value("groundTruthRate").toInt(); + settings.inputCommand = qSettings->value("inputCommand").toBool(); + settings.minOutputPeriod = qSettings->value("minOutputPeriod").toInt(); - settings.inputCommand = qSettings->value("inputCommand").toBool(); - settings.minOutputPeriod = qSettings->value("minOutputPeriod").toInt(); - - settings.airspeedActualEnabled=qSettings->value("airspeedActualEnabled").toBool(); - settings.airspeedActualRate = qSettings->value("airspeedActualRate").toInt(); + settings.airspeedActualEnabled = qSettings->value("airspeedActualEnabled").toBool(); + settings.airspeedActualRate = qSettings->value("airspeedActualRate").toInt(); } } IUAVGadgetConfiguration *HITLConfiguration::clone() { - HITLConfiguration *m = new HITLConfiguration(this->classId()); + HITLConfiguration *m = new HITLConfiguration(this->classId()); - m->settings = settings; + m->settings = settings; return m; } - /** - * Saves a configuration. - * - */ -void HITLConfiguration::saveConfig(QSettings* qSettings) const { +/** + * Saves a configuration. + * + */ +void HITLConfiguration::saveConfig(QSettings *qSettings) const +{ qSettings->setValue("simulatorId", settings.simulatorId); qSettings->setValue("binPath", settings.binPath); qSettings->setValue("dataPath", settings.dataPath); @@ -161,4 +160,3 @@ void HITLConfiguration::saveConfig(QSettings* qSettings) const { qSettings->setValue("airspeedActualEnabled", settings.airspeedActualEnabled); qSettings->setValue("airspeedActualRate", settings.airspeedActualRate); } - diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.h b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.h index 70d2c90f2..1b434c9ff 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.h @@ -7,21 +7,21 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,27 +36,29 @@ using namespace Core; -class HITLConfiguration : public IUAVGadgetConfiguration -{ - - Q_OBJECT - - Q_PROPERTY(SimulatorSettings settings READ Settings WRITE setSimulatorSettings) +class HITLConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT Q_PROPERTY(SimulatorSettings settings READ Settings WRITE setSimulatorSettings) public: - explicit HITLConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit HITLConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); - SimulatorSettings Settings() const { return settings; } + SimulatorSettings Settings() const + { + return settings; + } public slots: - void setSimulatorSettings (const SimulatorSettings& params ) { settings = params; } + void setSimulatorSettings(const SimulatorSettings & params) + { + settings = params; + } private: - SimulatorSettings settings; + SimulatorSettings settings; }; #endif // HITLCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlfactory.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlfactory.cpp index 248b1f21c..4d7e7d833 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlfactory.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,30 +32,25 @@ #include HITLFactory::HITLFactory(QObject *parent) - : IUAVGadgetFactory(QString("HITL"), tr("HITL Simulation"), parent) -{ - -} + : IUAVGadgetFactory(QString("HITL"), tr("HITL Simulation"), parent) +{} HITLFactory::~HITLFactory() +{} + +Core::IUAVGadget *HITLFactory::createGadget(QWidget *parent) { + HITLWidget *gadgetWidget = new HITLWidget(parent); + + return new HITLGadget(QString("HITL"), gadgetWidget, parent); } -Core::IUAVGadget* HITLFactory::createGadget(QWidget *parent) -{ - - - HITLWidget* gadgetWidget = new HITLWidget(parent); - return new HITLGadget(QString("HITL"), gadgetWidget, parent); -} - -IUAVGadgetConfiguration *HITLFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *HITLFactory::createConfiguration(QSettings *qSettings) { return new HITLConfiguration(QString("HITL"), qSettings); } IOptionsPage *HITLFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new HITLOptionsPage(qobject_cast(config)); + return new HITLOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlfactory.h b/ground/openpilotgcs/src/plugins/hitl/hitlfactory.h index b761a1014..16da8ae80 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlfactory.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class HITLFactory : public Core::IUAVGadgetFactory -{ +class HITLFactory : public Core::IUAVGadgetFactory { Q_OBJECT public: HITLFactory(QObject *parent = 0); ~HITLFactory(); - IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadget *createGadget(QWidget *parent); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlgadget.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlgadget.cpp index a05b5ce71..76da7afb4 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlgadget.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlgadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,10 +30,10 @@ #include "simulator.h" HITLGadget::HITLGadget(QString classId, HITLWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) + IUAVGadget(classId, parent), + m_widget(widget) { - connect(this,SIGNAL(changeConfiguration(void)),m_widget,SLOT(stopButtonClicked(void))); + connect(this, SIGNAL(changeConfiguration(void)), m_widget, SLOT(stopButtonClicked(void))); } HITLGadget::~HITLGadget() @@ -41,11 +41,11 @@ HITLGadget::~HITLGadget() delete m_widget; } -void HITLGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void HITLGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - HITLConfiguration *m = qobject_cast(config); + HITLConfiguration *m = qobject_cast(config); // IL2 <-- Is this still necessary? [KDS] - emit changeConfiguration(); - m_widget->setSettingParameters(m->Settings()); -} + emit changeConfiguration(); + m_widget->setSettingParameters(m->Settings()); +} diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlgadget.h b/ground/openpilotgcs/src/plugins/hitl/hitlgadget.h index 78add1e1d..dc7221862 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlgadget.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlgadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,22 +38,24 @@ class Simulator; using namespace Core; -class HITLGadget : public Core::IUAVGadget -{ +class HITLGadget : public Core::IUAVGadget { Q_OBJECT public: - HITLGadget(QString classId, HITLWidget *widget, QWidget *parent = 0); - ~HITLGadget(); + HITLGadget(QString classId, HITLWidget *widget, QWidget *parent = 0); + ~HITLGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); signals: - void changeConfiguration(); + void changeConfiguration(); private: - HITLWidget* m_widget; - Simulator* simulator; + HITLWidget *m_widget; + Simulator *simulator; }; diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp index f24a88268..637707c44 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp @@ -35,44 +35,44 @@ HitlNoiseGeneration::HitlNoiseGeneration() HitlNoiseGeneration::~HitlNoiseGeneration() +{} + +Noise HitlNoiseGeneration::getNoise() { -} - -Noise HitlNoiseGeneration::getNoise(){ return noise; } -Noise HitlNoiseGeneration::generateNoise(){ +Noise HitlNoiseGeneration::generateNoise() +{ + noise.accelData.x = 0; + noise.accelData.y = 0; + noise.accelData.z = 0; - noise.accelData.x=0; - noise.accelData.y=0; - noise.accelData.z=0; + noise.gpsPosData.Latitude = 0; + noise.gpsPosData.Longitude = 0; + noise.gpsPosData.Groundspeed = 0; + noise.gpsPosData.Heading = 0; + noise.gpsPosData.Altitude = 0; - noise.gpsPosData.Latitude=0; - noise.gpsPosData.Longitude=0; - noise.gpsPosData.Groundspeed=0; - noise.gpsPosData.Heading=0; - noise.gpsPosData.Altitude=0; + noise.gpsVelData.North = 0; + noise.gpsVelData.East = 0; + noise.gpsVelData.Down = 0; - noise.gpsVelData.North=0; - noise.gpsVelData.East=0; - noise.gpsVelData.Down=0; + noise.baroAltData.Altitude = 0; - noise.baroAltData.Altitude=0; + noise.attActualData.Roll = 0; + noise.attActualData.Pitch = 0; + noise.attActualData.Yaw = 0; - noise.attActualData.Roll=0; - noise.attActualData.Pitch=0; - noise.attActualData.Yaw=0; + noise.gyroData.x = 0; + noise.gyroData.y = 0; + noise.gyroData.z = 0; - noise.gyroData.x=0; - noise.gyroData.y=0; - noise.gyroData.z=0; + noise.accelData.x = 0; + noise.accelData.y = 0; + noise.accelData.z = 0; - noise.accelData.x=0; - noise.accelData.y=0; - noise.accelData.z=0; - - noise.airspeedActual.CalibratedAirspeed=0; + noise.airspeedActual.CalibratedAirspeed = 0; return noise; } diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h index cdf34b318..ead9ba9dd 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h @@ -28,30 +28,29 @@ #ifndef HITLNOISEGENERATION_H #define HITLNOISEGENERATION_H -//#include -//#include +// #include +// #include #include "xplanesimulator.h" #include "hitlnoisegeneration.h" #include "extensionsystem/pluginmanager.h" #include #include -struct Noise{ +struct Noise { Accels::DataFields accelData; AttitudeActual::DataFields attActualData; - BaroAltitude::DataFields baroAltData; + BaroAltitude::DataFields baroAltData; AirspeedActual::DataFields airspeedActual; - GPSPosition::DataFields gpsPosData; - GPSVelocity::DataFields gpsVelData; + GPSPosition::DataFields gpsPosData; + GPSVelocity::DataFields gpsVelData; Gyros::DataFields gyroData; - HomeLocation::DataFields homeData; + HomeLocation::DataFields homeData; PositionActual::DataFields positionActualData; VelocityActual::DataFields velocityActualData; }; -class HitlNoiseGeneration -{ -// Q_OBJECT +class HitlNoiseGeneration { +// Q_OBJECT public: HitlNoiseGeneration(); ~HitlNoiseGeneration(); diff --git a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp index f26fa09c7..c85b2fb11 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp @@ -7,21 +7,21 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,12 +36,10 @@ #include - HITLOptionsPage::HITLOptionsPage(HITLConfiguration *conf, QObject *parent) : IOptionsPage(parent), - config(conf) -{ -} + config(conf) +{} QWidget *HITLOptionsPage::createPage(QWidget *parent) { @@ -49,31 +47,31 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent) // Create page m_optionsPage = new Ui::HITLOptionsPage(); - QWidget* optionsPageWidget = new QWidget; + QWidget *optionsPageWidget = new QWidget; m_optionsPage->setupUi(optionsPageWidget); - int index = 0; - foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) - { - m_optionsPage->chooseFlightSimulator->insertItem(index++, creator->Description(),creator->ClassId()); - } + int index = 0; + foreach(SimulatorCreator * creator, HITLPlugin::typeSimulators) { + m_optionsPage->chooseFlightSimulator->insertItem(index++, creator->Description(), creator->ClassId()); + } - //QString classId = widget->listSimulators->itemData(0).toString(); - //SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(classId); + // QString classId = widget->listSimulators->itemData(0).toString(); + // SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(classId); - //QWidget* embedPage = creator->createOptionsPage(); - //m_optionsPage->verticalLayout->addWidget(embedPage); + // QWidget* embedPage = creator->createOptionsPage(); + // m_optionsPage->verticalLayout->addWidget(embedPage); - m_optionsPage->executablePath->setExpectedKind(Utils::PathChooser::File); - m_optionsPage->executablePath->setPromptDialogTitle(tr("Choose flight simulator executable")); - m_optionsPage->dataPath->setExpectedKind(Utils::PathChooser::Directory); - m_optionsPage->dataPath->setPromptDialogTitle(tr("Choose flight simulator data directory")); + m_optionsPage->executablePath->setExpectedKind(Utils::PathChooser::File); + m_optionsPage->executablePath->setPromptDialogTitle(tr("Choose flight simulator executable")); + m_optionsPage->dataPath->setExpectedKind(Utils::PathChooser::Directory); + m_optionsPage->dataPath->setPromptDialogTitle(tr("Choose flight simulator data directory")); // Restore the contents from the settings: - foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) - { + foreach(SimulatorCreator * creator, HITLPlugin::typeSimulators) { QString id = config->Settings().simulatorId; - if(creator->ClassId() == id) + + if (creator->ClassId() == id) { m_optionsPage->chooseFlightSimulator->setCurrentIndex(HITLPlugin::typeSimulators.indexOf(creator)); + } } m_optionsPage->executablePath->setPath(config->Settings().binPath); @@ -98,11 +96,10 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent) m_optionsPage->attRawCheckbox->setChecked(config->Settings().attRawEnabled); - m_optionsPage->attRawRateSpinbox->setValue(config->Settings().attRawRate); m_optionsPage->gpsPosRateSpinbox->setValue(config->Settings().gpsPosRate); m_optionsPage->groundTruthRateSpinbox->setValue(config->Settings().groundTruthRate); -// m_optionsPage->attActualRate->setValue(config->Settings().attActualRate); +// m_optionsPage->attActualRate->setValue(config->Settings().attActualRate); m_optionsPage->baroAltitudeCheckbox->setChecked(config->Settings().baroAltitudeEnabled); m_optionsPage->baroAltRateSpinbox->setValue(config->Settings().baroAltRate); @@ -124,48 +121,48 @@ void HITLOptionsPage::apply() SimulatorSettings settings; int i = m_optionsPage->chooseFlightSimulator->currentIndex(); - settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); - settings.binPath = m_optionsPage->executablePath->path(); - settings.dataPath = m_optionsPage->dataPath->path(); - settings.startSim = m_optionsPage->startSim->isChecked(); - settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); - settings.hostAddress = m_optionsPage->hostAddress->text(); - settings.remoteAddress = m_optionsPage->remoteAddress->text(); + settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); + settings.binPath = m_optionsPage->executablePath->path(); + settings.dataPath = m_optionsPage->dataPath->path(); + settings.startSim = m_optionsPage->startSim->isChecked(); + settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); + settings.hostAddress = m_optionsPage->hostAddress->text(); + settings.remoteAddress = m_optionsPage->remoteAddress->text(); settings.inPort = m_optionsPage->inputPort->text().toInt(); - settings.outPort = m_optionsPage->outputPort->text().toInt(); - settings.longitude = m_optionsPage->longitude->text(); - settings.latitude = m_optionsPage->latitude->text(); + settings.outPort = m_optionsPage->outputPort->text().toInt(); + settings.longitude = m_optionsPage->longitude->text(); + settings.latitude = m_optionsPage->latitude->text(); - settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); + settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); - settings.attRawEnabled = m_optionsPage->attRawCheckbox->isChecked(); - settings.attRawRate = m_optionsPage->attRawRateSpinbox->value(); + settings.attRawEnabled = m_optionsPage->attRawCheckbox->isChecked(); + settings.attRawRate = m_optionsPage->attRawRateSpinbox->value(); - settings.attActualEnabled = m_optionsPage->attActualCheckbox->isChecked(); + settings.attActualEnabled = m_optionsPage->attActualCheckbox->isChecked(); - settings.gpsPositionEnabled = m_optionsPage->gpsPositionCheckbox->isChecked(); - settings.gpsPosRate = m_optionsPage->gpsPosRateSpinbox->value(); + settings.gpsPositionEnabled = m_optionsPage->gpsPositionCheckbox->isChecked(); + settings.gpsPosRate = m_optionsPage->gpsPosRateSpinbox->value(); - settings.groundTruthEnabled = m_optionsPage->groundTruthCheckbox->isChecked(); - settings.groundTruthRate = m_optionsPage->groundTruthRateSpinbox->value(); + settings.groundTruthEnabled = m_optionsPage->groundTruthCheckbox->isChecked(); + settings.groundTruthRate = m_optionsPage->groundTruthRateSpinbox->value(); - settings.baroAltitudeEnabled = m_optionsPage->baroAltitudeCheckbox->isChecked(); - settings.baroAltRate = m_optionsPage->baroAltRateSpinbox->value(); + settings.baroAltitudeEnabled = m_optionsPage->baroAltitudeCheckbox->isChecked(); + settings.baroAltRate = m_optionsPage->baroAltRateSpinbox->value(); - settings.minOutputPeriod = m_optionsPage->minOutputPeriodSpinbox->value(); + settings.minOutputPeriod = m_optionsPage->minOutputPeriodSpinbox->value(); - settings.manualControlEnabled = m_optionsPage->manualControlRadioButton->isChecked(); - settings.gcsReceiverEnabled = m_optionsPage->gcsReceiverRadioButton->isChecked(); + settings.manualControlEnabled = m_optionsPage->manualControlRadioButton->isChecked(); + settings.gcsReceiverEnabled = m_optionsPage->gcsReceiverRadioButton->isChecked(); - settings.attActHW = m_optionsPage->attActHW->isChecked(); - settings.attActSim = m_optionsPage->attActSim->isChecked(); - settings.attActCalc = m_optionsPage->attActCalc->isChecked(); + settings.attActHW = m_optionsPage->attActHW->isChecked(); + settings.attActSim = m_optionsPage->attActSim->isChecked(); + settings.attActCalc = m_optionsPage->attActCalc->isChecked(); - settings.airspeedActualEnabled=m_optionsPage->airspeedActualCheckbox->isChecked(); - settings.airspeedActualRate=m_optionsPage->airspeedRateSpinbox->value(); + settings.airspeedActualEnabled = m_optionsPage->airspeedActualCheckbox->isChecked(); + settings.airspeedActualRate = m_optionsPage->airspeedRateSpinbox->value(); - //Write settings to file + // Write settings to file config->setSimulatorSettings(settings); } diff --git a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.h b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.h index a6749cc12..8a34d419b 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.h @@ -7,21 +7,21 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,27 +39,28 @@ class HITLConfiguration; using namespace Core; namespace Ui { - class HITLOptionsPage; +class HITLOptionsPage; } -class HITLOptionsPage : public IOptionsPage -{ -Q_OBJECT +class HITLOptionsPage : public IOptionsPage { + Q_OBJECT public: - explicit HITLOptionsPage(HITLConfiguration *conf, QObject *parent = 0); + explicit HITLOptionsPage(HITLConfiguration *conf, QObject *parent = 0); - QWidget *createPage(QWidget *parent); + QWidget *createPage(QWidget *parent); void apply(); void finish(); - bool isDecorated() const { return true;} + bool isDecorated() const + { + return true; + } signals: private slots: private: - HITLConfiguration* config; - Ui::HITLOptionsPage* m_optionsPage; - + HITLConfiguration *config; + Ui::HITLOptionsPage *m_optionsPage; }; #endif // HITLOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp index 351c62610..a1bd18e8a 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,42 +34,41 @@ #include "il2simulator.h" #include "xplanesimulator.h" -QList HITLPlugin::typeSimulators; +QList HITLPlugin::typeSimulators; HITLPlugin::HITLPlugin() { - // Do nothing + // Do nothing } HITLPlugin::~HITLPlugin() { - // Do nothing + // Do nothing } -bool HITLPlugin::initialize(const QStringList& args, QString *errMsg) +bool HITLPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new HITLFactory(this); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new HITLFactory(this); - addAutoReleasedObject(mf); + addAutoReleasedObject(mf); - addSimulator(new AeroSimRCSimulatorCreator("ASimRC", "AeroSimRC")); - addSimulator(new FGSimulatorCreator("FG","FlightGear")); - addSimulator(new IL2SimulatorCreator("IL2","IL2")); - addSimulator(new XplaneSimulatorCreator("X-Plane","X-Plane")); + addSimulator(new AeroSimRCSimulatorCreator("ASimRC", "AeroSimRC")); + addSimulator(new FGSimulatorCreator("FG", "FlightGear")); + addSimulator(new IL2SimulatorCreator("IL2", "IL2")); + addSimulator(new XplaneSimulatorCreator("X-Plane", "X-Plane")); - return true; + return true; } void HITLPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void HITLPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(HITLPlugin) - diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlplugin.h b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.h index 5639c3be4..cb7254edb 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlplugin.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,37 +35,34 @@ class HITLFactory; -class HITLPlugin : public ExtensionSystem::IPlugin -{ +class HITLPlugin : public ExtensionSystem::IPlugin { public: HITLPlugin(); - ~HITLPlugin(); + ~HITLPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); - static void addSimulator(SimulatorCreator* creator) - { - HITLPlugin::typeSimulators.append(creator); - } + static void addSimulator(SimulatorCreator *creator) + { + HITLPlugin::typeSimulators.append(creator); + } - static SimulatorCreator* getSimulatorCreator(const QString classId) - { - foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) - { - if(classId == creator->ClassId()) - return creator; - } - return 0; - } + static SimulatorCreator *getSimulatorCreator(const QString classId) + { + foreach(SimulatorCreator * creator, HITLPlugin::typeSimulators) { + if (classId == creator->ClassId()) { + return creator; + } + } + return 0; + } - static QList typeSimulators; + static QList typeSimulators; private: - HITLFactory *mf; - - + HITLFactory *mf; }; #endif /* HITLPLUGIN_H */ diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlwidget.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlwidget.cpp index f29035f39..42e449b7c 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlwidget.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlwidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -43,139 +43,129 @@ QStringList Simulator::instances; HITLWidget::HITLWidget(QWidget *parent) - : QWidget(parent), - simulator(0) + : QWidget(parent), + simulator(0) { - widget = new Ui_HITLWidget(); - widget->setupUi(this); - widget->startButton->setEnabled(true); - widget->stopButton->setEnabled(false); + widget = new Ui_HITLWidget(); + widget->setupUi(this); + widget->startButton->setEnabled(true); + widget->stopButton->setEnabled(false); - greenColor = "rgb(35, 221, 35)"; //Change the green color in order to make it a bit more vibrant - strStyleEnable = QString("QFrame{background-color: %1; color: white}").arg(greenColor); + greenColor = "rgb(35, 221, 35)"; // Change the green color in order to make it a bit more vibrant + strStyleEnable = QString("QFrame{background-color: %1; color: white}").arg(greenColor); strStyleDisable = "QFrame{background-color: red; color: white}"; strAutopilotDisconnected = " Autopilot OFF "; strSimulatorDisconnected = " Simulator OFF "; - strAutopilotConnected = " Autopilot ON "; + strAutopilotConnected = " Autopilot ON "; - widget->apLabel->setText(strAutopilotDisconnected); - widget->simLabel->setText(strSimulatorDisconnected); + widget->apLabel->setText(strAutopilotDisconnected); + widget->simLabel->setText(strSimulatorDisconnected); - connect(widget->startButton, SIGNAL(clicked()), this, SLOT(startButtonClicked())); - connect(widget->stopButton, SIGNAL(clicked()), this, SLOT(stopButtonClicked())); - connect(widget->buttonClearLog, SIGNAL(clicked()), this, SLOT(buttonClearLogClicked())); + connect(widget->startButton, SIGNAL(clicked()), this, SLOT(startButtonClicked())); + connect(widget->stopButton, SIGNAL(clicked()), this, SLOT(stopButtonClicked())); + connect(widget->buttonClearLog, SIGNAL(clicked()), this, SLOT(buttonClearLogClicked())); } HITLWidget::~HITLWidget() { - delete widget; + delete widget; } void HITLWidget::startButtonClicked() { - QThread* mainThread = QThread::currentThread(); - qDebug() << "Main Thread: "<< mainThread; + QThread *mainThread = QThread::currentThread(); - //Allow only one instance per simulator - if(Simulator::Instances().indexOf(settings.simulatorId) != -1) - { - widget->textBrowser->append(settings.simulatorId + " alreary started!"); - return; - } + qDebug() << "Main Thread: " << mainThread; - if(!HITLPlugin::typeSimulators.size()) - { - qxtLog->info("There is no registered simulators, add through HITLPlugin::addSimulator"); - return; - } + // Allow only one instance per simulator + if (Simulator::Instances().indexOf(settings.simulatorId) != -1) { + widget->textBrowser->append(settings.simulatorId + " alreary started!"); + return; + } - // Stop running process if one is active - if(simulator) - { - QMetaObject::invokeMethod(simulator, "onDeleteSimulator",Qt::QueuedConnection); - simulator = NULL; - } + if (!HITLPlugin::typeSimulators.size()) { + qxtLog->info("There is no registered simulators, add through HITLPlugin::addSimulator"); + return; + } - if(settings.hostAddress == "" || settings.inPort == 0) - { - widget->textBrowser->append("Before start, set UDP parameters in options page!"); - return; - } + // Stop running process if one is active + if (simulator) { + QMetaObject::invokeMethod(simulator, "onDeleteSimulator", Qt::QueuedConnection); + simulator = NULL; + } - SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(settings.simulatorId); + if (settings.hostAddress == "" || settings.inPort == 0) { + widget->textBrowser->append("Before start, set UDP parameters in options page!"); + return; + } + + SimulatorCreator *creator = HITLPlugin::getSimulatorCreator(settings.simulatorId); simulator = creator->createSimulator(settings); // move to thread <--[BCH] - simulator->setName(creator->Description()); - simulator->setSimulatorId(creator->ClassId()); + simulator->setName(creator->Description()); + simulator->setSimulatorId(creator->ClassId()); connect(simulator, SIGNAL(processOutput(QString)), this, SLOT(onProcessOutput(QString))); - // Setup process - widget->textBrowser->append(QString("[%1] Starting %2... ").arg(QTime::currentTime().toString("hh:mm:ss")).arg(creator->Description())); - qxtLog->info("HITL: Starting " + creator->Description()); + // Setup process + widget->textBrowser->append(QString("[%1] Starting %2... ").arg(QTime::currentTime().toString("hh:mm:ss")).arg(creator->Description())); + qxtLog->info("HITL: Starting " + creator->Description()); - // Start bridge - bool ret = QMetaObject::invokeMethod(simulator, "setupProcess",Qt::QueuedConnection); - if(ret) - { - Simulator::setInstance(settings.simulatorId); + // Start bridge + bool ret = QMetaObject::invokeMethod(simulator, "setupProcess", Qt::QueuedConnection); + if (ret) { + Simulator::setInstance(settings.simulatorId); - connect(this,SIGNAL(deleteSimulator()),simulator, SLOT(onDeleteSimulator()),Qt::QueuedConnection); + connect(this, SIGNAL(deleteSimulator()), simulator, SLOT(onDeleteSimulator()), Qt::QueuedConnection); - widget->startButton->setEnabled(false); - widget->stopButton->setEnabled(true); - qxtLog->info("HITL: Starting bridge, initializing flight simulator and Autopilot connections"); + widget->startButton->setEnabled(false); + widget->stopButton->setEnabled(true); + qxtLog->info("HITL: Starting bridge, initializing flight simulator and Autopilot connections"); - connect(simulator, SIGNAL(autopilotConnected()), this, SLOT(onAutopilotConnect()),Qt::QueuedConnection); - connect(simulator, SIGNAL(autopilotDisconnected()), this, SLOT(onAutopilotDisconnect()),Qt::QueuedConnection); - connect(simulator, SIGNAL(simulatorConnected()), this, SLOT(onSimulatorConnect()),Qt::QueuedConnection); - connect(simulator, SIGNAL(simulatorDisconnected()), this, SLOT(onSimulatorDisconnect()),Qt::QueuedConnection); + connect(simulator, SIGNAL(autopilotConnected()), this, SLOT(onAutopilotConnect()), Qt::QueuedConnection); + connect(simulator, SIGNAL(autopilotDisconnected()), this, SLOT(onAutopilotDisconnect()), Qt::QueuedConnection); + connect(simulator, SIGNAL(simulatorConnected()), this, SLOT(onSimulatorConnect()), Qt::QueuedConnection); + connect(simulator, SIGNAL(simulatorDisconnected()), this, SLOT(onSimulatorDisconnect()), Qt::QueuedConnection); - // Initialize connection status - if ( simulator->isAutopilotConnected() ) - { - onAutopilotConnect(); - } - else - { - onAutopilotDisconnect(); - } + // Initialize connection status + if (simulator->isAutopilotConnected()) { + onAutopilotConnect(); + } else { + onAutopilotDisconnect(); + } - if ( simulator->isSimulatorConnected() ) - { - onSimulatorConnect(); - } - else - { - onSimulatorDisconnect(); - } - } + if (simulator->isSimulatorConnected()) { + onSimulatorConnect(); + } else { + onSimulatorDisconnect(); + } + } } void HITLWidget::stopButtonClicked() { - if(simulator) - widget->textBrowser->append(QString("[%1] Terminate %2 ").arg(QTime::currentTime().toString("hh:mm:ss")).arg(simulator->Name())); + if (simulator) { + widget->textBrowser->append(QString("[%1] Terminate %2 ").arg(QTime::currentTime().toString("hh:mm:ss")).arg(simulator->Name())); + } - widget->startButton->setEnabled(true); - widget->stopButton->setEnabled(false); + widget->startButton->setEnabled(true); + widget->stopButton->setEnabled(false); widget->apLabel->setStyleSheet(QString::fromUtf8("QFrame{background-color: transparent; color: white}")); widget->simLabel->setStyleSheet(QString::fromUtf8("QFrame{background-color: transparent; color: white}")); widget->apLabel->setText(strAutopilotDisconnected); - widget->simLabel->setText(strSimulatorDisconnected); - if(simulator) - { - QMetaObject::invokeMethod(simulator, "onDeleteSimulator",Qt::QueuedConnection); - simulator = NULL; - } + widget->simLabel->setText(strSimulatorDisconnected); + if (simulator) { + QMetaObject::invokeMethod(simulator, "onDeleteSimulator", Qt::QueuedConnection); + simulator = NULL; + } } void HITLWidget::buttonClearLogClicked() { - widget->textBrowser->clear(); + widget->textBrowser->clear(); } void HITLWidget::onProcessOutput(QString text) @@ -187,26 +177,26 @@ void HITLWidget::onAutopilotConnect() { widget->apLabel->setStyleSheet(strStyleEnable); widget->apLabel->setText(strAutopilotConnected); - qxtLog->info("HITL: Autopilot connected, initializing for HITL simulation"); + qxtLog->info("HITL: Autopilot connected, initializing for HITL simulation"); } void HITLWidget::onAutopilotDisconnect() { widget->apLabel->setStyleSheet(strStyleDisable); - widget->apLabel->setText(strAutopilotDisconnected); - qxtLog->info(strAutopilotDisconnected); + widget->apLabel->setText(strAutopilotDisconnected); + qxtLog->info(strAutopilotDisconnected); } void HITLWidget::onSimulatorConnect() { widget->simLabel->setStyleSheet(strStyleEnable); - widget->simLabel->setText(" " + simulator->Name() +" connected "); - qxtLog->info(QString("HITL: %1 connected").arg(simulator->Name())); + widget->simLabel->setText(" " + simulator->Name() + " connected "); + qxtLog->info(QString("HITL: %1 connected").arg(simulator->Name())); } void HITLWidget::onSimulatorDisconnect() { widget->simLabel->setStyleSheet(strStyleDisable); - widget->simLabel->setText(" " + simulator->Name() +" disconnected "); - qxtLog->info(QString("HITL: %1 disconnected").arg(simulator->Name())); + widget->simLabel->setText(" " + simulator->Name() + " disconnected "); + qxtLog->info(QString("HITL: %1 disconnected").arg(simulator->Name())); } diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlwidget.h b/ground/openpilotgcs/src/plugins/hitl/hitlwidget.h index 0a1f1c881..5a746c430 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlwidget.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,37 +34,39 @@ class Ui_HITLWidget; -class HITLWidget : public QWidget -{ +class HITLWidget : public QWidget { Q_OBJECT public: HITLWidget(QWidget *parent = 0); ~HITLWidget(); - void setSettingParameters(const SimulatorSettings& params) {settings = params;} + void setSettingParameters(const SimulatorSettings & params) + { + settings = params; + } signals: - void deleteSimulator(); + void deleteSimulator(); private slots: void startButtonClicked(); void stopButtonClicked(); - void buttonClearLogClicked(); + void buttonClearLogClicked(); void onProcessOutput(QString text); void onAutopilotConnect(); void onAutopilotDisconnect(); - void onSimulatorConnect(); - void onSimulatorDisconnect(); + void onSimulatorConnect(); + void onSimulatorDisconnect(); private: - Ui_HITLWidget* widget; - Simulator* simulator; - SimulatorSettings settings; + Ui_HITLWidget *widget; + Simulator *simulator; + SimulatorSettings settings; - QString greenColor; - QString strAutopilotDisconnected; - QString strSimulatorDisconnected; - QString strAutopilotConnected; + QString greenColor; + QString strAutopilotDisconnected; + QString strSimulatorDisconnected; + QString strAutopilotConnected; QString strStyleEnable; QString strStyleDisable; }; diff --git a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp index f1defa953..c4936d33d 100644 --- a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp @@ -68,34 +68,34 @@ #include #include -const float IL2Simulator::FT2M = 12*.254; -const float IL2Simulator::KT2MPS = 0.514444444; -const float IL2Simulator::MPS2KMH = 3.6; -const float IL2Simulator::KMH2MPS = (1.0/3.6); +const float IL2Simulator::FT2M = 12 * .254; +const float IL2Simulator::KT2MPS = 0.514444444; +const float IL2Simulator::MPS2KMH = 3.6; +const float IL2Simulator::KMH2MPS = (1.0 / 3.6); const float IL2Simulator::INHG2KPA = 3.386; -const float IL2Simulator::RAD2DEG = (180.0/M_PI); -const float IL2Simulator::DEG2RAD = (M_PI/180.0); -const float IL2Simulator::NM2DEG = 60.*1852.; // 60 miles per degree times 1852 meters per mile -const float IL2Simulator::DEG2NM = (1.0/(60.*1852.)); +const float IL2Simulator::RAD2DEG = (180.0 / M_PI); +const float IL2Simulator::DEG2RAD = (M_PI / 180.0); +const float IL2Simulator::NM2DEG = 60. * 1852.; // 60 miles per degree times 1852 meters per mile +const float IL2Simulator::DEG2NM = (1.0 / (60. * 1852.)); -IL2Simulator::IL2Simulator(const SimulatorSettings& params) : +IL2Simulator::IL2Simulator(const SimulatorSettings & params) : Simulator(params) { - airParameters=getAirParameters(); + airParameters = getAirParameters(); } IL2Simulator::~IL2Simulator() -{ -} +{} -void IL2Simulator::setupUdpPorts(const QString& host, int inPort, int outPort) +void IL2Simulator::setupUdpPorts(const QString & host, int inPort, int outPort) { Q_UNUSED(outPort); - inSocket->connectToHost(host,inPort); // IL2 - if(!inSocket->waitForConnected()) + inSocket->connectToHost(host, inPort); // IL2 + if (!inSocket->waitForConnected()) { qxtLog->error(Name() + " cann't connect to UDP Port: " + QString::number(inPort)); + } } void IL2Simulator::transmitUpdate() @@ -104,59 +104,60 @@ void IL2Simulator::transmitUpdate() ActuatorDesired::DataFields actData = actDesired->getData(); float ailerons = actData.Roll; float elevator = actData.Pitch; - float rudder = actData.Yaw; - float throttle = actData.Throttle*2-1.0; + float rudder = actData.Yaw; + float throttle = actData.Throttle * 2 - 1.0; // Send update to Il2 QString cmd; - cmd=QString("R/30/32/40/42/46/48/81\\%1/85\\%2/87\\%3/89\\%4/") - .arg(throttle) - .arg(ailerons) - .arg(elevator) - .arg(rudder); + + cmd = QString("R/30/32/40/42/46/48/81\\%1/85\\%2/87\\%3/89\\%4/") + .arg(throttle) + .arg(ailerons) + .arg(elevator) + .arg(rudder); QByteArray data = cmd.toAscii(); - //outSocket->write(data); - inSocket->write(data); // for IL2 must send to the same port as input!!!!!!!!!!!!! + // outSocket->write(data); + inSocket->write(data); // for IL2 must send to the same port as input!!!!!!!!!!!!! } /** * process data string from flight simulator */ -void IL2Simulator::processUpdate(const QByteArray& inp) +void IL2Simulator::processUpdate(const QByteArray & inp) { // save old flight data to calculate delta's later - old=current; + old = current; QString data(inp); // Split QStringList fields = data.split("/"); // split up response string int t; - for (t=0; t=2) { + if (values.length() >= 2) { int id = values[0].toInt(); float value = values[1].toFloat(); switch (id) { case 30: - current.cas=value * KMH2MPS; + current.cas = value * KMH2MPS; break; case 32: - current.dZ=value; + current.dZ = value; break; case 40: - current.Z=value; + current.Z = value; break; case 42: - current.azimuth=value; + current.azimuth = value; break; case 46: - current.roll=-value; + current.roll = -value; break; case 48: - current.pitch=value; + current.pitch = value; break; } } @@ -164,50 +165,64 @@ void IL2Simulator::processUpdate(const QByteArray& inp) // measure time current.dT = ((float)time->restart()) / 1000.0; - if (current.dT<0.001) current.dT=0.001; - current.T = old.T+current.dT; - current.i = old.i+1; - if (current.i==1) { - old.dRoll=0; - old.dPitch=0; - old.dAzimuth=0; - old.ddX=0; - old.ddX=0; - old.ddX=0; + if (current.dT < 0.001) { + current.dT = 0.001; + } + current.T = old.T + current.dT; + current.i = old.i + 1; + if (current.i == 1) { + old.dRoll = 0; + old.dPitch = 0; + old.dAzimuth = 0; + old.ddX = 0; + old.ddX = 0; + old.ddX = 0; } // calculate TAS from alt and CAS - float gravity =9.805; + float gravity = 9.805; current.tas = cas2tas(current.cas, current.Z, airParameters, gravity); // assume the plane actually flies straight and no wind // groundspeed is horizontal vector of TAS - current.groundspeed = current.tas*cos(current.pitch*DEG2RAD); + current.groundspeed = current.tas * cos(current.pitch * DEG2RAD); // x and y vector components - current.dX = current.groundspeed*sin(current.azimuth*DEG2RAD); - current.dY = current.groundspeed*cos(current.azimuth*DEG2RAD); + current.dX = current.groundspeed * sin(current.azimuth * DEG2RAD); + current.dY = current.groundspeed * cos(current.azimuth * DEG2RAD); // simple IMS - integration over time the easy way... - current.X = old.X + (current.dX*current.dT); - current.Y = old.Y + (current.dY*current.dT); + current.X = old.X + (current.dX * current.dT); + current.Y = old.Y + (current.dY * current.dT); // accelerations (filtered) - if (isnan(old.ddX) || isinf(old.ddX)) old.ddX=0; - if (isnan(old.ddY) || isinf(old.ddY)) old.ddY=0; - if (isnan(old.ddZ) || isinf(old.ddZ)) old.ddZ=0; + if (isnan(old.ddX) || isinf(old.ddX)) { + old.ddX = 0; + } + if (isnan(old.ddY) || isinf(old.ddY)) { + old.ddY = 0; + } + if (isnan(old.ddZ) || isinf(old.ddZ)) { + old.ddZ = 0; + } #define SPEED_FILTER 10 - current.ddX = ((current.dX-old.dX)/current.dT + SPEED_FILTER * (old.ddX)) / (SPEED_FILTER+1); - current.ddY = ((current.dY-old.dY)/current.dT + SPEED_FILTER * (old.ddY)) / (SPEED_FILTER+1); - current.ddZ = ((current.dZ-old.dZ)/current.dT + SPEED_FILTER * (old.ddZ)) / (SPEED_FILTER+1); + current.ddX = ((current.dX - old.dX) / current.dT + SPEED_FILTER * (old.ddX)) / (SPEED_FILTER + 1); + current.ddY = ((current.dY - old.dY) / current.dT + SPEED_FILTER * (old.ddY)) / (SPEED_FILTER + 1); + current.ddZ = ((current.dZ - old.dZ) / current.dT + SPEED_FILTER * (old.ddZ)) / (SPEED_FILTER + 1); -#define TURN_FILTER 10 +#define TURN_FILTER 10 // turn speeds (filtered) - if (isnan(old.dAzimuth) || isinf(old.dAzimuth)) old.dAzimuth=0; - if (isnan(old.dPitch) || isinf(old.dPitch)) old.dPitch=0; - if (isnan(old.dRoll) || isinf(old.dRoll)) old.dRoll=0; - current.dAzimuth = (angleDifference(current.azimuth,old.azimuth)/current.dT + TURN_FILTER * (old.dAzimuth)) / (TURN_FILTER+1); - current.dPitch = (angleDifference(current.pitch,old.pitch)/current.dT + TURN_FILTER * (old.dPitch)) / (TURN_FILTER+1); - current.dRoll = (angleDifference(current.roll,old.roll)/current.dT + TURN_FILTER * (old.dRoll)) / (TURN_FILTER+1); + if (isnan(old.dAzimuth) || isinf(old.dAzimuth)) { + old.dAzimuth = 0; + } + if (isnan(old.dPitch) || isinf(old.dPitch)) { + old.dPitch = 0; + } + if (isnan(old.dRoll) || isinf(old.dRoll)) { + old.dRoll = 0; + } + current.dAzimuth = (angleDifference(current.azimuth, old.azimuth) / current.dT + TURN_FILTER * (old.dAzimuth)) / (TURN_FILTER + 1); + current.dPitch = (angleDifference(current.pitch, old.pitch) / current.dT + TURN_FILTER * (old.dPitch)) / (TURN_FILTER + 1); + current.dRoll = (angleDifference(current.roll, old.roll) / current.dT + TURN_FILTER * (old.dRoll)) / (TURN_FILTER + 1); /////// // Output formatting @@ -219,68 +234,68 @@ void IL2Simulator::processUpdate(const QByteArray& inp) float Rbe[3][3]; float rpy[3]; float quat[4]; - rpy[0]=current.roll; - rpy[1]=current.pitch; - rpy[2]=current.azimuth; - Utils::CoordinateConversions().RPY2Quaternion(rpy,quat); - Utils::CoordinateConversions().Quaternion2R(quat,Rbe); + rpy[0] = current.roll; + rpy[1] = current.pitch; + rpy[2] = current.azimuth; + Utils::CoordinateConversions().RPY2Quaternion(rpy, quat); + Utils::CoordinateConversions().Quaternion2R(quat, Rbe); // Update GPS Position objects double HomeLLA[3]; double LLA[3]; double NED[3]; - HomeLLA[0]=settings.latitude.toFloat(); - HomeLLA[1]=settings.longitude.toFloat(); - HomeLLA[2]=0; - NED[0] = current.Y; - NED[1] = current.X; - NED[2] = -current.Z; - Utils::CoordinateConversions().NED2LLA_HomeLLA(HomeLLA,NED,LLA); - out.latitude = LLA[0] * 1e7; - out.longitude = LLA[1] * 1e7; - out.groundspeed = current.groundspeed; + HomeLLA[0] = settings.latitude.toFloat(); + HomeLLA[1] = settings.longitude.toFloat(); + HomeLLA[2] = 0; + NED[0] = current.Y; + NED[1] = current.X; + NED[2] = -current.Z; + Utils::CoordinateConversions().NED2LLA_HomeLLA(HomeLLA, NED, LLA); + out.latitude = LLA[0] * 1e7; + out.longitude = LLA[1] * 1e7; + out.groundspeed = current.groundspeed; out.calibratedAirspeed = current.cas; - out.trueAirspeed=cas2tas(current.cas, current.Z, airParameters, gravity); + out.trueAirspeed = cas2tas(current.cas, current.Z, airParameters, gravity); - out.dstN=current.Y; - out.dstE=current.X; - out.dstD=-current.Z; + out.dstN = current.Y; + out.dstE = current.X; + out.dstD = -current.Z; // Update BaroAltitude object - out.altitude = current.Z; - out.agl = current.Z; - out.temperature = airParameters.groundTemp + (current.Z * airParameters.tempLapseRate) - 273.0; - out.pressure = airPressureFromAltitude(current.Z, airParameters, gravity) ; // kpa + out.altitude = current.Z; + out.agl = current.Z; + out.temperature = airParameters.groundTemp + (current.Z * airParameters.tempLapseRate) - 273.0; + out.pressure = airPressureFromAltitude(current.Z, airParameters, gravity); // kpa // Update attActual object - out.roll = current.roll; //roll; - out.pitch = current.pitch; // pitch + out.roll = current.roll; // roll; + out.pitch = current.pitch; // pitch out.heading = current.azimuth; // yaw // Update VelocityActual.{North,East,Down} out.velNorth = current.dY; - out.velEast = current.dX; - out.velDown = -current.dZ; + out.velEast = current.dX; + out.velDown = -current.dZ; // rotate turn rates and accelerations into body frame // (note: rotation deltas are NOT in NED frame but in RPY - manual conversion!) - out.rollRate = current.dRoll; + out.rollRate = current.dRoll; out.pitchRate = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth; - out.yawRate = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch; + out.yawRate = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch; - //Update accelerometer sensor data - out.accX = current.ddX*Rbe[0][0] - +current.ddY*Rbe[0][1] - +(current.ddZ+GEE)*Rbe[0][2]; - out.accY = current.ddX*Rbe[1][0] - +current.ddY*Rbe[1][1] - +(current.ddZ+GEE)*Rbe[1][2]; - out.accZ = - (current.ddX*Rbe[2][0] - +current.ddY*Rbe[2][1] - +(current.ddZ+GEE)*Rbe[2][2]); + // Update accelerometer sensor data + out.accX = current.ddX * Rbe[0][0] + + current.ddY * Rbe[0][1] + + (current.ddZ + GEE) * Rbe[0][2]; + out.accY = current.ddX * Rbe[1][0] + + current.ddY * Rbe[1][1] + + (current.ddZ + GEE) * Rbe[1][2]; + out.accZ = -(current.ddX * Rbe[2][0] + + current.ddY * Rbe[2][1] + + (current.ddZ + GEE) * Rbe[2][2]); updateUAVOs(out); } @@ -290,8 +305,13 @@ void IL2Simulator::processUpdate(const QByteArray& inp) */ float IL2Simulator::angleDifference(float a, float b) { - float d=a-b; - if (d>180) d-=360; - if (d<-180)d+=360; + float d = a - b; + + if (d > 180) { + d -= 360; + } + if (d < -180) { + d += 360; + } return d; } diff --git a/ground/openpilotgcs/src/plugins/hitl/il2simulator.h b/ground/openpilotgcs/src/plugins/hitl/il2simulator.h index 62823f9fc..e50702150 100644 --- a/ground/openpilotgcs/src/plugins/hitl/il2simulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/il2simulator.h @@ -31,17 +31,16 @@ #include #include -class IL2Simulator: public Simulator -{ - Q_OBJECT +class IL2Simulator : public Simulator { + Q_OBJECT public: - IL2Simulator(const SimulatorSettings& params); - ~IL2Simulator(); + IL2Simulator(const SimulatorSettings & params); + ~IL2Simulator(); - void setupUdpPorts(const QString& host, int inPort, int outPort); + void setupUdpPorts(const QString & host, int inPort, int outPort); private slots: - void transmitUpdate(); + void transmitUpdate(); private: static const float FT2M; @@ -54,23 +53,22 @@ private: static const float NM2DEG; static const float DEG2NM; - void processUpdate(const QByteArray& data); - float angleDifference(float a,float b); + void processUpdate(const QByteArray & data); + float angleDifference(float a, float b); AirParameters airParameters; }; -class IL2SimulatorCreator : public SimulatorCreator -{ +class IL2SimulatorCreator : public SimulatorCreator { public: - IL2SimulatorCreator(const QString& classId, const QString& description) - : SimulatorCreator (classId,description) - {} + IL2SimulatorCreator(const QString & classId, const QString & description) + : SimulatorCreator(classId, description) + {} - Simulator* createSimulator(const SimulatorSettings& params) - { - return new IL2Simulator(params); - } + Simulator *createSimulator(const SimulatorSettings & params) + { + return new IL2Simulator(params); + } }; #endif // IL2SIMULATOR_H diff --git a/ground/openpilotgcs/src/plugins/hitl/isimulator.h b/ground/openpilotgcs/src/plugins/hitl/isimulator.h index 183a030d7..842e69a8f 100644 --- a/ground/openpilotgcs/src/plugins/hitl/isimulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/isimulator.h @@ -15,59 +15,58 @@ #include "positionactual.h" #include "gcstelemetrystats.h" -class Simulator: public QObject -{ - Q_OBJECT +class Simulator : public QObject { + Q_OBJECT public: - //static ISimulator* Instance(); -//protected: - Simulator(); - ~ISimulator(); + // static ISimulator* Instance(); +// protected: + Simulator(); + ~ISimulator(); - bool isAutopilotConnected(); - bool isFGConnected(); + bool isAutopilotConnected(); + bool isFGConnected(); signals: - void myStart(); - void autopilotConnected(); - void autopilotDisconnected(); - void fgConnected(); - void fgDisconnected(); + void myStart(); + void autopilotConnected(); + void autopilotDisconnected(); + void fgConnected(); + void fgDisconnected(); private slots: - void onStart(); - void transmitUpdate(); - void receiveUpdate(); - void onAutopilotConnect(); - void onAutopilotDisconnect(); - void onFGConnectionTimeout(); - void telStatsUpdated(UAVObject* obj); + void onStart(); + void transmitUpdate(); + void receiveUpdate(); + void onAutopilotConnect(); + void onAutopilotDisconnect(); + void onFGConnectionTimeout(); + void telStatsUpdated(UAVObject *obj); private: - //static ISimulator* _instance; + // static ISimulator* _instance; - QUdpSocket* inSocket; - QUdpSocket* outSocket; - ActuatorDesired* actDesired; - AltitudeActual* altActual; - VelocityActual* velActual; - AttitudeActual* attActual; - PositionActual* posActual; - GCSTelemetryStats* telStats; - QHostAddress fgHost; - int inPort; - int outPort; - int updatePeriod; - QTimer* txTimer; - QTimer* fgTimer; - bool autopilotConnectionStatus; - bool fgConnectionStatus; - int fgTimeout; + QUdpSocket *inSocket; + QUdpSocket *outSocket; + ActuatorDesired *actDesired; + AltitudeActual *altActual; + VelocityActual *velActual; + AttitudeActual *attActual; + PositionActual *posActual; + GCSTelemetryStats *telStats; + QHostAddress fgHost; + int inPort; + int outPort; + int updatePeriod; + QTimer *txTimer; + QTimer *fgTimer; + bool autopilotConnectionStatus; + bool fgConnectionStatus; + int fgTimeout; - void processUpdate(QString& data); - void setupOutputObject(UAVObject* obj, int updatePeriod); - void setupInputObject(UAVObject* obj, int updatePeriod); - void setupObjects(); + void processUpdate(QString & data); + void setupOutputObject(UAVObject *obj, int updatePeriod); + void setupInputObject(UAVObject *obj, int updatePeriod); + void setupObjects(); }; diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp index 01dbfa136..f3d594d36 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp @@ -35,234 +35,228 @@ volatile bool Simulator::isStarted = false; -const float Simulator::GEE = 9.81; -const float Simulator::FT2M = 0.3048; -const float Simulator::KT2MPS = 0.514444444; +const float Simulator::GEE = 9.81; +const float Simulator::FT2M = 0.3048; +const float Simulator::KT2MPS = 0.514444444; const float Simulator::INHG2KPA = 3.386; const float Simulator::FPS2CMPS = 30.48; -const float Simulator::DEG2RAD = (M_PI/180.0); -const float Simulator::RAD2DEG = (180.0/M_PI); +const float Simulator::DEG2RAD = (M_PI / 180.0); +const float Simulator::RAD2DEG = (180.0 / M_PI); -Simulator::Simulator(const SimulatorSettings& params) : - simProcess(NULL), - time(NULL), - inSocket(NULL), - outSocket(NULL), - settings(params), - updatePeriod(50), - simTimeout(8000), - autopilotConnectionStatus(false), - simConnectionStatus(false), - txTimer(NULL), - simTimer(NULL), - name("") +Simulator::Simulator(const SimulatorSettings & params) : + simProcess(NULL), + time(NULL), + inSocket(NULL), + outSocket(NULL), + settings(params), + updatePeriod(50), + simTimeout(8000), + autopilotConnectionStatus(false), + simConnectionStatus(false), + txTimer(NULL), + simTimer(NULL), + name("") { - // move to thread - moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread()); - connect(this, SIGNAL(myStart()), this, SLOT(onStart()),Qt::QueuedConnection); - emit myStart(); + // move to thread + moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread()); + connect(this, SIGNAL(myStart()), this, SLOT(onStart()), Qt::QueuedConnection); + emit myStart(); - QTime currentTime=QTime::currentTime(); - gpsPosTime = currentTime; - groundTruthTime = currentTime; - gcsRcvrTime = currentTime; - attRawTime = currentTime; - baroAltTime = currentTime; + QTime currentTime = QTime::currentTime(); + gpsPosTime = currentTime; + groundTruthTime = currentTime; + gcsRcvrTime = currentTime; + attRawTime = currentTime; + baroAltTime = currentTime; battTime = currentTime; - airspeedActualTime=currentTime; + airspeedActualTime = currentTime; - //Define standard atmospheric constants - airParameters.univGasConstant=8.31447; //[J/(mol·K)] - airParameters.dryAirConstant=287.058; //[J/(kg*K)] - airParameters.groundDensity=1.225; //[kg/m^3] - airParameters.groundTemp=15+273.15; //[K] - airParameters.tempLapseRate=0.0065; //[deg/m] - airParameters.M=0.0289644; //[kg/mol] - airParameters.relativeHumidity=20; //[%] - airParameters.seaLevelPress=101.325; //[kPa] + // Define standard atmospheric constants + airParameters.univGasConstant = 8.31447; // [J/(mol·K)] + airParameters.dryAirConstant = 287.058; // [J/(kg*K)] + airParameters.groundDensity = 1.225; // [kg/m^3] + airParameters.groundTemp = 15 + 273.15; // [K] + airParameters.tempLapseRate = 0.0065; // [deg/m] + airParameters.M = 0.0289644; // [kg/mol] + airParameters.relativeHumidity = 20; // [%] + airParameters.seaLevelPress = 101.325; // [kPa] } Simulator::~Simulator() { - if(inSocket) - { - delete inSocket; - inSocket = NULL; - } + if (inSocket) { + delete inSocket; + inSocket = NULL; + } - if(outSocket) - { - delete outSocket; - outSocket = NULL; - } + if (outSocket) { + delete outSocket; + outSocket = NULL; + } - if(txTimer) - { - delete txTimer; - txTimer = NULL; - } + if (txTimer) { + delete txTimer; + txTimer = NULL; + } - if(simTimer) - { - delete simTimer; - simTimer = NULL; - } - // NOTE: Does not currently work, may need to send control+c to through the terminal - if (simProcess != NULL) - { - //connect(simProcess,SIGNAL(finished(int, QProcess::ExitStatus)),this,SLOT(onFinished(int, QProcess::ExitStatus))); + if (simTimer) { + delete simTimer; + simTimer = NULL; + } + // NOTE: Does not currently work, may need to send control+c to through the terminal + if (simProcess != NULL) { + // connect(simProcess,SIGNAL(finished(int, QProcess::ExitStatus)),this,SLOT(onFinished(int, QProcess::ExitStatus))); - simProcess->disconnect(); - if(simProcess->state() == QProcess::Running) - simProcess->kill(); - //if(simProcess->waitForFinished()) - //emit deleteSimProcess(); - delete simProcess; - simProcess = NULL; - } + simProcess->disconnect(); + if (simProcess->state() == QProcess::Running) { + simProcess->kill(); + } + // if(simProcess->waitForFinished()) + // emit deleteSimProcess(); + delete simProcess; + simProcess = NULL; + } } void Simulator::onDeleteSimulator(void) { - // [1] - Simulator::setStarted(false); - // [2] - Simulator::Instances().removeOne(simulatorId); + // [1] + Simulator::setStarted(false); + // [2] + Simulator::Instances().removeOne(simulatorId); - disconnect(this); - delete this; + disconnect(this); + delete this; } void Simulator::onStart() { QMutexLocker locker(&lock); - QThread* mainThread = QThread::currentThread(); + QThread *mainThread = QThread::currentThread(); - qDebug() << "Simulator Thread: "<< mainThread; + qDebug() << "Simulator Thread: " << mainThread; // Get required UAVObjects - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager* objManager = pm->getObject(); - actDesired = ActuatorDesired::GetInstance(objManager); - actCommand = ActuatorCommand::GetInstance(objManager); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + actDesired = ActuatorDesired::GetInstance(objManager); + actCommand = ActuatorCommand::GetInstance(objManager); manCtrlCommand = ManualControlCommand::GetInstance(objManager); - gcsReceiver = GCSReceiver::GetInstance(objManager); - flightStatus = FlightStatus::GetInstance(objManager); - posHome = HomeLocation::GetInstance(objManager); - velActual = VelocityActual::GetInstance(objManager); - posActual = PositionActual::GetInstance(objManager); - baroAlt = BaroAltitude::GetInstance(objManager); - flightBatt = FlightBatteryState::GetInstance(objManager); + gcsReceiver = GCSReceiver::GetInstance(objManager); + flightStatus = FlightStatus::GetInstance(objManager); + posHome = HomeLocation::GetInstance(objManager); + velActual = VelocityActual::GetInstance(objManager); + posActual = PositionActual::GetInstance(objManager); + baroAlt = BaroAltitude::GetInstance(objManager); + flightBatt = FlightBatteryState::GetInstance(objManager); airspeedActual = AirspeedActual::GetInstance(objManager); - attActual = AttitudeActual::GetInstance(objManager); - attSettings = AttitudeSettings::GetInstance(objManager); - accels = Accels::GetInstance(objManager); - gyros = Gyros::GetInstance(objManager); - gpsPos = GPSPosition::GetInstance(objManager); - gpsVel = GPSVelocity::GetInstance(objManager); - telStats = GCSTelemetryStats::GetInstance(objManager); + attActual = AttitudeActual::GetInstance(objManager); + attSettings = AttitudeSettings::GetInstance(objManager); + accels = Accels::GetInstance(objManager); + gyros = Gyros::GetInstance(objManager); + gpsPos = GPSPosition::GetInstance(objManager); + gpsVel = GPSVelocity::GetInstance(objManager); + telStats = GCSTelemetryStats::GetInstance(objManager); groundTruth = GroundTruth::GetInstance(objManager); // Listen to autopilot connection events - TelemetryManager* telMngr = pm->getObject(); + TelemetryManager *telMngr = pm->getObject(); connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); - //connect(telStats, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(telStatsUpdated(UAVObject*))); + // connect(telStats, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(telStatsUpdated(UAVObject*))); // If already connect setup autopilot GCSTelemetryStats::DataFields stats = telStats->getData(); - if ( stats.Status == GCSTelemetryStats::STATUS_CONNECTED ) + if (stats.Status == GCSTelemetryStats::STATUS_CONNECTED) { onAutopilotConnect(); + } - inSocket = new QUdpSocket(); + inSocket = new QUdpSocket(); outSocket = new QUdpSocket(); - setupUdpPorts(settings.hostAddress,settings.inPort,settings.outPort); + setupUdpPorts(settings.hostAddress, settings.inPort, settings.outPort); - emit processOutput("\nLocal interface: " + settings.hostAddress + "\n" + \ - "Remote interface: " + settings.remoteAddress + "\n" + \ - "inputPort: " + QString::number(settings.inPort) + "\n" + \ - "outputPort: " + QString::number(settings.outPort) + "\n"); + emit processOutput("\nLocal interface: " + settings.hostAddress + "\n" + \ + "Remote interface: " + settings.remoteAddress + "\n" + \ + "inputPort: " + QString::number(settings.inPort) + "\n" + \ + "outputPort: " + QString::number(settings.outPort) + "\n"); - qxtLog->info("\nLocal interface: " + settings.hostAddress + "\n" + \ - "Remote interface: " + settings.remoteAddress + "\n" + \ - "inputPort: " + QString::number(settings.inPort) + "\n" + \ - "outputPort: " + QString::number(settings.outPort) + "\n"); + qxtLog->info("\nLocal interface: " + settings.hostAddress + "\n" + \ + "Remote interface: " + settings.remoteAddress + "\n" + \ + "inputPort: " + QString::number(settings.inPort) + "\n" + \ + "outputPort: " + QString::number(settings.outPort) + "\n"); -// if(!inSocket->waitForConnected(5000)) -// emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.inPort)); -// outSocket->connectToHost(settings.hostAddress,settings.outPort); // FG -// if(!outSocket->waitForConnected(5000)) -// emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.outPort)); +// if(!inSocket->waitForConnected(5000)) +// emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.inPort)); +// outSocket->connectToHost(settings.hostAddress,settings.outPort); // FG +// if(!outSocket->waitForConnected(5000)) +// emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.outPort)); - connect(inSocket, SIGNAL(readyRead()), this, SLOT(receiveUpdate()),Qt::DirectConnection); + connect(inSocket, SIGNAL(readyRead()), this, SLOT(receiveUpdate()), Qt::DirectConnection); - // Setup transmit timer - txTimer = new QTimer(); - connect(txTimer, SIGNAL(timeout()), this, SLOT(transmitUpdate()),Qt::DirectConnection); - txTimer->setInterval(updatePeriod); - txTimer->start(); - // Setup simulator connection timer - simTimer = new QTimer(); - connect(simTimer, SIGNAL(timeout()), this, SLOT(onSimulatorConnectionTimeout()),Qt::DirectConnection); - simTimer->setInterval(simTimeout); - simTimer->start(); - - // setup time - time = new QTime(); - time->start(); - current.T=0; - current.i=0; + // Setup transmit timer + txTimer = new QTimer(); + connect(txTimer, SIGNAL(timeout()), this, SLOT(transmitUpdate()), Qt::DirectConnection); + txTimer->setInterval(updatePeriod); + txTimer->start(); + // Setup simulator connection timer + simTimer = new QTimer(); + connect(simTimer, SIGNAL(timeout()), this, SLOT(onSimulatorConnectionTimeout()), Qt::DirectConnection); + simTimer->setInterval(simTimeout); + simTimer->start(); + // setup time + time = new QTime(); + time->start(); + current.T = 0; + current.i = 0; } void Simulator::receiveUpdate() { - // Update connection timer and status - simTimer->setInterval(simTimeout); - simTimer->stop(); - simTimer->start(); - if ( !simConnectionStatus ) - { - simConnectionStatus = true; - emit simulatorConnected(); - } + // Update connection timer and status + simTimer->setInterval(simTimeout); + simTimer->stop(); + simTimer->start(); + if (!simConnectionStatus) { + simConnectionStatus = true; + emit simulatorConnected(); + } - // Process data - while(inSocket->hasPendingDatagrams()) { - // Receive datagram - QByteArray datagram; - datagram.resize(inSocket->pendingDatagramSize()); - QHostAddress sender; - quint16 senderPort; - inSocket->readDatagram(datagram.data(), datagram.size(), - &sender, &senderPort); - //QString datastr(datagram); - // Process incomming data - processUpdate(datagram); - } + // Process data + while (inSocket->hasPendingDatagrams()) { + // Receive datagram + QByteArray datagram; + datagram.resize(inSocket->pendingDatagramSize()); + QHostAddress sender; + quint16 senderPort; + inSocket->readDatagram(datagram.data(), datagram.size(), + &sender, &senderPort); + // QString datastr(datagram); + // Process incomming data + processUpdate(datagram); + } } void Simulator::setupObjects() { - if (settings.gcsReceiverEnabled) { - setupInputObject(actCommand, settings.minOutputPeriod); //Input to the simulator + setupInputObject(actCommand, settings.minOutputPeriod); // Input to the simulator setupOutputObject(gcsReceiver, settings.minOutputPeriod); } else if (settings.manualControlEnabled) { - setupInputObject(actDesired, settings.minOutputPeriod); //Input to the simulator + setupInputObject(actDesired, settings.minOutputPeriod); // Input to the simulator } - setupOutputObject(posHome, 10000); //Hardcoded? Bleh. + setupOutputObject(posHome, 10000); // Hardcoded? Bleh. - if (settings.gpsPositionEnabled){ + if (settings.gpsPositionEnabled) { setupOutputObject(gpsPos, settings.gpsPosRate); setupOutputObject(gpsVel, settings.gpsPosRate); } - if (settings.groundTruthEnabled){ + if (settings.groundTruthEnabled) { setupOutputObject(posActual, settings.groundTruthRate); setupOutputObject(velActual, settings.groundTruthRate); } @@ -272,31 +266,31 @@ void Simulator::setupObjects() setupOutputObject(gyros, settings.attRawRate); } - if (settings.attActualEnabled && settings.attActHW) { + if (settings.attActualEnabled && settings.attActHW) { setupOutputObject(accels, settings.attRawRate); setupOutputObject(gyros, settings.attRawRate); } - if (settings.attActualEnabled && !settings.attActHW) - setupOutputObject(attActual, 20); //Hardcoded? Bleh. - else - setupWatchedObject(attActual, 100); //Hardcoded? Bleh. - - if(settings.airspeedActualEnabled) + if (settings.attActualEnabled && !settings.attActHW) { + setupOutputObject(attActual, 20); // Hardcoded? Bleh. + } else { + setupWatchedObject(attActual, 100); // Hardcoded? Bleh. + } + if (settings.airspeedActualEnabled) { setupOutputObject(airspeedActual, settings.airspeedActualRate); + } - if(settings.baroAltitudeEnabled) - { + if (settings.baroAltitudeEnabled) { setupOutputObject(baroAlt, settings.baroAltRate); setupOutputObject(flightBatt, settings.baroAltRate); } - } -void Simulator::setupInputObject(UAVObject* obj, quint32 updatePeriod) +void Simulator::setupInputObject(UAVObject *obj, quint32 updatePeriod) { UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READONLY); @@ -317,6 +311,7 @@ void Simulator::setupInputObject(UAVObject* obj, quint32 updatePeriod) void Simulator::setupWatchedObject(UAVObject *obj, quint32 updatePeriod) { UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READONLY); @@ -333,99 +328,95 @@ void Simulator::setupWatchedObject(UAVObject *obj, quint32 updatePeriod) } -void Simulator::setupOutputObject(UAVObject* obj, quint32 updatePeriod) +void Simulator::setupOutputObject(UAVObject *obj, quint32 updatePeriod) { - UAVObject::Metadata mdata; - mdata = obj->getDefaultMetadata(); + UAVObject::Metadata mdata; + + mdata = obj->getDefaultMetadata(); UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READWRITE); UAVObject::SetGcsTelemetryAcked(mdata, false); - UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); + UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.gcsTelemetryUpdatePeriod = updatePeriod; UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY); - UAVObject::SetFlightTelemetryUpdateMode(mdata,UAVObject::UPDATEMODE_MANUAL); + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL); obj->setMetadata(mdata); } void Simulator::onAutopilotConnect() { - autopilotConnectionStatus = true; - setupObjects(); - emit autopilotConnected(); + autopilotConnectionStatus = true; + setupObjects(); + emit autopilotConnected(); } void Simulator::onAutopilotDisconnect() { - autopilotConnectionStatus = false; - emit autopilotDisconnected(); + autopilotConnectionStatus = false; + emit autopilotDisconnected(); } void Simulator::onSimulatorConnectionTimeout() { - if ( simConnectionStatus ) - { - simConnectionStatus = false; - emit simulatorDisconnected(); - } + if (simConnectionStatus) { + simConnectionStatus = false; + emit simulatorDisconnected(); + } } -void Simulator::telStatsUpdated(UAVObject* obj) +void Simulator::telStatsUpdated(UAVObject *obj) { Q_UNUSED(obj); GCSTelemetryStats::DataFields stats = telStats->getData(); - if ( !autopilotConnectionStatus && stats.Status == GCSTelemetryStats::STATUS_CONNECTED ) - { - onAutopilotConnect(); - } - else if ( autopilotConnectionStatus && stats.Status != GCSTelemetryStats::STATUS_CONNECTED ) - { - onAutopilotDisconnect(); - } + if (!autopilotConnectionStatus && stats.Status == GCSTelemetryStats::STATUS_CONNECTED) { + onAutopilotConnect(); + } else if (autopilotConnectionStatus && stats.Status != GCSTelemetryStats::STATUS_CONNECTED) { + onAutopilotDisconnect(); + } } -void Simulator::resetInitialHomePosition(){ - once=false; +void Simulator::resetInitialHomePosition() +{ + once = false; } -void Simulator::updateUAVOs(Output2Hardware out){ - +void Simulator::updateUAVOs(Output2Hardware out) +{ QTime currentTime = QTime::currentTime(); Noise noise; HitlNoiseGeneration noiseSource; - if(settings.addNoise){ + if (settings.addNoise) { noise = noiseSource.generateNoise(); - } - else{ + } else { memset(&noise, 0, sizeof(Noise)); } /*******************************/ HomeLocation::DataFields homeData = posHome->getData(); - if(!once) - { + if (!once) { // Upon startup, we reset the HomeLocation object to // the plane's location: memset(&homeData, 0, sizeof(HomeLocation::DataFields)); // Update homelocation - homeData.Latitude = out.latitude; //Already in *10^7 integer format - homeData.Longitude = out.longitude; //Already in *10^7 integer format - homeData.Altitude = out.agl; + homeData.Latitude = out.latitude; // Already in *10^7 integer format + homeData.Longitude = out.longitude; // Already in *10^7 integer format + homeData.Altitude = out.agl; double LLA[3]; - LLA[0]=out.latitude; - LLA[1]=out.longitude; - LLA[2]=out.altitude; + LLA[0] = out.latitude; + LLA[1] = out.longitude; + LLA[2] = out.altitude; - homeData.Be[0]=0; - homeData.Be[1]=0; - homeData.Be[2]=0; + homeData.Be[0] = 0; + homeData.Be[1] = 0; + homeData.Be[2] = 0; posHome->setData(homeData); posHome->updated(); @@ -434,40 +425,40 @@ void Simulator::updateUAVOs(Output2Hardware out){ initE = out.dstE; initD = out.dstD; - once=true; + once = true; } /*******************************/ - //Copy everything to the ground truth object. GroundTruth is Noise-free. + // Copy everything to the ground truth object. GroundTruth is Noise-free. GroundTruth::DataFields groundTruthData; groundTruthData = groundTruth->getData(); - groundTruthData.AccelerationXYZ[0]=out.accX; - groundTruthData.AccelerationXYZ[1]=out.accY; - groundTruthData.AccelerationXYZ[2]=out.accZ; + groundTruthData.AccelerationXYZ[0] = out.accX; + groundTruthData.AccelerationXYZ[1] = out.accY; + groundTruthData.AccelerationXYZ[2] = out.accZ; - groundTruthData.AngularRates[0]=out.rollRate; - groundTruthData.AngularRates[1]=out.pitchRate; - groundTruthData.AngularRates[2]=out.yawRate; + groundTruthData.AngularRates[0] = out.rollRate; + groundTruthData.AngularRates[1] = out.pitchRate; + groundTruthData.AngularRates[2] = out.yawRate; - groundTruthData.CalibratedAirspeed=out.calibratedAirspeed; - groundTruthData.TrueAirspeed=out.trueAirspeed; - groundTruthData.AngleOfAttack=out.angleOfAttack; - groundTruthData.AngleOfSlip=out.angleOfSlip; + groundTruthData.CalibratedAirspeed = out.calibratedAirspeed; + groundTruthData.TrueAirspeed = out.trueAirspeed; + groundTruthData.AngleOfAttack = out.angleOfAttack; + groundTruthData.AngleOfSlip = out.angleOfSlip; - groundTruthData.PositionNED[0]=out.dstN-initN; - groundTruthData.PositionNED[1]=out.dstE-initD; - groundTruthData.PositionNED[2]=out.dstD-initD; + groundTruthData.PositionNED[0] = out.dstN - initN; + groundTruthData.PositionNED[1] = out.dstE - initD; + groundTruthData.PositionNED[2] = out.dstD - initD; - groundTruthData.VelocityNED[0]=out.velNorth; - groundTruthData.VelocityNED[1]=out.velEast; - groundTruthData.VelocityNED[2]=out.velDown; + groundTruthData.VelocityNED[0] = out.velNorth; + groundTruthData.VelocityNED[1] = out.velEast; + groundTruthData.VelocityNED[2] = out.velDown; - groundTruthData.RPY[0]=out.roll; - groundTruthData.RPY[0]=out.pitch; - groundTruthData.RPY[0]=out.heading; + groundTruthData.RPY[0] = out.roll; + groundTruthData.RPY[0] = out.pitch; + groundTruthData.RPY[0] = out.heading; - //Set UAVO + // Set UAVO groundTruth->setData(groundTruthData); /*******************************/ @@ -480,33 +471,33 @@ void Simulator::updateUAVOs(Output2Hardware out){ /*****************************************/ } else if (settings.attActSim) { // take all data from simulator - attActualData.Roll = out.roll + noise.attActualData.Roll; //roll; - attActualData.Pitch = out.pitch + noise.attActualData.Pitch; // pitch - attActualData.Yaw = out.heading + noise.attActualData.Yaw; // Yaw + attActualData.Roll = out.roll + noise.attActualData.Roll; // roll; + attActualData.Pitch = out.pitch + noise.attActualData.Pitch; // pitch + attActualData.Yaw = out.heading + noise.attActualData.Yaw; // Yaw float rpy[3]; float quat[4]; rpy[0] = attActualData.Roll; rpy[1] = attActualData.Pitch; rpy[2] = attActualData.Yaw; - Utils::CoordinateConversions().RPY2Quaternion(rpy,quat); + Utils::CoordinateConversions().RPY2Quaternion(rpy, quat); attActualData.q1 = quat[0]; attActualData.q2 = quat[1]; attActualData.q3 = quat[2]; attActualData.q4 = quat[3]; - //Set UAVO + // Set UAVO attActual->setData(attActualData); /*****************************************/ } else if (settings.attActCalc) { // calculate RPY with code from Attitude module - static float q[4] = {1, 0, 0, 0}; + static float q[4] = { 1, 0, 0, 0 }; static float gyro_correct_int2 = 0; float dT = out.delT; AttitudeSettings::DataFields attSettData = attSettings->getData(); - float accelKp = attSettData.AccelKp * 0.1666666666666667; - float accelKi = attSettData.AccelKp * 0.1666666666666667; + float accelKp = attSettData.AccelKp * 0.1666666666666667; + float accelKi = attSettData.AccelKp * 0.1666666666666667; float yawBiasRate = attSettData.YawBiasRate; // calibrate sensors on arming @@ -515,8 +506,8 @@ void Simulator::updateUAVOs(Output2Hardware out){ accelKi = 0.9; } - float gyro[3] = {out.rollRate, out.pitchRate, out.yawRate}; - float attRawAcc[3] = {out.accX, out.accY, out.accZ}; + float gyro[3] = { out.rollRate, out.pitchRate, out.yawRate }; + float attRawAcc[3] = { out.accX, out.accY, out.accZ }; // code from Attitude module begin /////////////////////////////// float *accels = attRawAcc; @@ -526,13 +517,13 @@ void Simulator::updateUAVOs(Output2Hardware out){ // Rotate gravity to body frame and cross with accels grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); - grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); + grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); // CrossProduct { - accel_err[0] = accels[1]*grot[2] - grot[1]*accels[2]; - accel_err[1] = grot[0]*accels[2] - accels[0]*grot[2]; - accel_err[2] = accels[0]*grot[1] - grot[0]*accels[1]; + accel_err[0] = accels[1] * grot[2] - grot[1] * accels[2]; + accel_err[1] = grot[0] * accels[2] - accels[0] * grot[2]; + accel_err[2] = accels[0] * grot[1] - grot[0] * accels[1]; } // Account for accel magnitude @@ -558,12 +549,12 @@ void Simulator::updateUAVOs(Output2Hardware out){ qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2; // Take a time step - q[0] += qdot[0]; - q[1] += qdot[1]; - q[2] += qdot[2]; - q[3] += qdot[3]; + q[0] += qdot[0]; + q[1] += qdot[1]; + q[2] += qdot[2]; + q[3] += qdot[3]; - if(q[0] < 0) { + if (q[0] < 0) { q[0] = -q[0]; q[1] = -q[1]; q[2] = -q[2]; @@ -579,7 +570,7 @@ void Simulator::updateUAVOs(Output2Hardware out){ // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN - if((fabs(qmag) < 1e-3) || (qmag != qmag)) { + if ((fabs(qmag) < 1e-3) || (qmag != qmag)) { q[0] = 1; q[1] = 0; q[2] = 0; @@ -590,19 +581,19 @@ void Simulator::updateUAVOs(Output2Hardware out){ // Quaternion2RPY { float q0s, q1s, q2s, q3s; - q0s = q[0] * q[0]; - q1s = q[1] * q[1]; - q2s = q[2] * q[2]; - q3s = q[3] * q[3]; + q0s = q[0] * q[0]; + q1s = q[1] * q[1]; + q2s = q[2] * q[2]; + q3s = q[3] * q[3]; float R13, R11, R12, R23, R33; - R13 = 2 * (q[1] * q[3] - q[0] * q[2]); - R11 = q0s + q1s - q2s - q3s; - R12 = 2 * (q[1] * q[2] + q[0] * q[3]); - R23 = 2 * (q[2] * q[3] + q[0] * q[1]); - R33 = q0s - q1s - q2s + q3s; + R13 = 2 * (q[1] * q[3] - q[0] * q[2]); + R11 = q0s + q1s - q2s - q3s; + R12 = 2 * (q[1] * q[2] + q[0] * q[3]); + R23 = 2 * (q[2] * q[3] + q[0] * q[1]); + R33 = q0s - q1s - q2s + q3s; - rpy2[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 + rpy2[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 rpy2[2] = RAD2DEG * atan2f(R12, R11); rpy2[0] = RAD2DEG * atan2f(R23, R33); } @@ -610,12 +601,12 @@ void Simulator::updateUAVOs(Output2Hardware out){ attActualData.Roll = rpy2[0]; attActualData.Pitch = rpy2[1]; attActualData.Yaw = rpy2[2]; - attActualData.q1 = q[0]; - attActualData.q2 = q[1]; - attActualData.q3 = q[2]; - attActualData.q4 = q[3]; + attActualData.q1 = q[0]; + attActualData.q2 = q[1]; + attActualData.q3 = q[2]; + attActualData.q4 = q[3]; - //Set UAVO + // Set UAVO attActual->setData(attActualData); /*****************************************/ } @@ -626,14 +617,13 @@ void Simulator::updateUAVOs(Output2Hardware out){ GCSReceiver::DataFields gcsRcvrData; memset(&gcsRcvrData, 0, sizeof(GCSReceiver::DataFields)); - for (quint16 i = 0; i < GCSReceiver::CHANNEL_NUMELEM; i++){ - gcsRcvrData.Channel[i] = 1500 + (out.rc_channel[i]*500); //Elements in rc_channel are between -1 and 1 + for (quint16 i = 0; i < GCSReceiver::CHANNEL_NUMELEM; i++) { + gcsRcvrData.Channel[i] = 1500 + (out.rc_channel[i] * 500); // Elements in rc_channel are between -1 and 1 } gcsReceiver->setData(gcsRcvrData); - gcsRcvrTime=gcsRcvrTime.addMSecs(settings.minOutputPeriod); - + gcsRcvrTime = gcsRcvrTime.addMSecs(settings.minOutputPeriod); } } @@ -641,19 +631,19 @@ void Simulator::updateUAVOs(Output2Hardware out){ /*******************************/ if (settings.gpsPositionEnabled) { if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) { - qDebug()<< " GPS time:" << gpsPosTime << ", currentTime: " << currentTime << ", difference: " << gpsPosTime.msecsTo(currentTime); + qDebug() << " GPS time:" << gpsPosTime << ", currentTime: " << currentTime << ", difference: " << gpsPosTime.msecsTo(currentTime); // Update GPS Position objects GPSPosition::DataFields gpsPosData; memset(&gpsPosData, 0, sizeof(GPSPosition::DataFields)); - gpsPosData.Altitude = out.altitude + noise.gpsPosData.Altitude; - gpsPosData.Heading = out.heading + noise.gpsPosData.Heading; - gpsPosData.Groundspeed = out.groundspeed + noise.gpsPosData.Groundspeed; - gpsPosData.Latitude = out.latitude + noise.gpsPosData.Latitude; //Already in *10^7 integer format - gpsPosData.Longitude = out.longitude + noise.gpsPosData.Longitude; //Already in *10^7 integer format + gpsPosData.Altitude = out.altitude + noise.gpsPosData.Altitude; + gpsPosData.Heading = out.heading + noise.gpsPosData.Heading; + gpsPosData.Groundspeed = out.groundspeed + noise.gpsPosData.Groundspeed; + gpsPosData.Latitude = out.latitude + noise.gpsPosData.Latitude; // Already in *10^7 integer format + gpsPosData.Longitude = out.longitude + noise.gpsPosData.Longitude; // Already in *10^7 integer format gpsPosData.GeoidSeparation = 0.0; gpsPosData.PDOP = 3.0; - gpsPosData.VDOP = gpsPosData.PDOP*1.5; - gpsPosData.Satellites = 10; + gpsPosData.VDOP = gpsPosData.PDOP * 1.5; + gpsPosData.Satellites = 10; gpsPosData.Status = GPSPosition::STATUS_FIX3D; gpsPos->setData(gpsPosData); @@ -662,12 +652,12 @@ void Simulator::updateUAVOs(Output2Hardware out){ GPSVelocity::DataFields gpsVelData; memset(&gpsVelData, 0, sizeof(GPSVelocity::DataFields)); gpsVelData.North = out.velNorth + noise.gpsVelData.North; - gpsVelData.East = out.velEast + noise.gpsVelData.East; - gpsVelData.Down = out.velDown + noise.gpsVelData.Down; + gpsVelData.East = out.velEast + noise.gpsVelData.East; + gpsVelData.Down = out.velDown + noise.gpsVelData.Down; gpsVel->setData(gpsVelData); - gpsPosTime=gpsPosTime.addMSecs(settings.gpsPosRate); + gpsPosTime = gpsPosTime.addMSecs(settings.gpsPosRate); } } @@ -678,87 +668,87 @@ void Simulator::updateUAVOs(Output2Hardware out){ VelocityActual::DataFields velocityActualData; memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields)); velocityActualData.North = out.velNorth + noise.velocityActualData.North; - velocityActualData.East = out.velEast + noise.velocityActualData.East; - velocityActualData.Down = out.velDown + noise.velocityActualData.Down; + velocityActualData.East = out.velEast + noise.velocityActualData.East; + velocityActualData.Down = out.velDown + noise.velocityActualData.Down; velActual->setData(velocityActualData); // Update PositionActual.{Nort,East,Down} PositionActual::DataFields positionActualData; memset(&positionActualData, 0, sizeof(PositionActual::DataFields)); - positionActualData.North = (out.dstN-initN) + noise.positionActualData.North; - positionActualData.East = (out.dstE-initE) + noise.positionActualData.East; - positionActualData.Down = (out.dstD/*-initD*/) + noise.positionActualData.Down; + positionActualData.North = (out.dstN - initN) + noise.positionActualData.North; + positionActualData.East = (out.dstE - initE) + noise.positionActualData.East; + positionActualData.Down = (out.dstD /*-initD*/) + noise.positionActualData.Down; posActual->setData(positionActualData); - groundTruthTime=groundTruthTime.addMSecs(settings.groundTruthRate); + groundTruthTime = groundTruthTime.addMSecs(settings.groundTruthRate); } } -// /*******************************/ -// if (settings.sonarAltitude) { -// static QTime sonarAltTime = currentTime; -// if (sonarAltTime.msecsTo(currentTime) >= settings.sonarAltRate) { -// SonarAltitude::DataFields sonarAltData; -// sonarAltData = sonarAlt->getData(); +///*******************************/ +// if (settings.sonarAltitude) { +// static QTime sonarAltTime = currentTime; +// if (sonarAltTime.msecsTo(currentTime) >= settings.sonarAltRate) { +// SonarAltitude::DataFields sonarAltData; +// sonarAltData = sonarAlt->getData(); -// float sAlt = settings.sonarMaxAlt; -// // 0.35 rad ~= 20 degree -// if ((agl < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) { -// float x = agl * qTan(roll); -// float y = agl * qTan(pitch); -// float h = qSqrt(x*x + y*y + agl*agl); -// sAlt = qMin(h, sAlt); -// } +// float sAlt = settings.sonarMaxAlt; +//// 0.35 rad ~= 20 degree +// if ((agl < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) { +// float x = agl * qTan(roll); +// float y = agl * qTan(pitch); +// float h = qSqrt(x*x + y*y + agl*agl); +// sAlt = qMin(h, sAlt); +// } -// sonarAltData.Altitude = sAlt; -// sonarAlt->setData(sonarAltData); -// sonarAltTime = currentTime; -// } -// } +// sonarAltData.Altitude = sAlt; +// sonarAlt->setData(sonarAltData); +// sonarAltTime = currentTime; +// } +// } /*******************************/ // Update BaroAltitude object - if (settings.baroAltitudeEnabled){ + if (settings.baroAltitudeEnabled) { if (baroAltTime.msecsTo(currentTime) >= settings.baroAltRate) { - BaroAltitude::DataFields baroAltData; - memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields)); - baroAltData.Altitude = out.altitude + noise.baroAltData.Altitude; - baroAltData.Temperature = out.temperature + noise.baroAltData.Temperature; - baroAltData.Pressure = out.pressure + noise.baroAltData.Pressure; - baroAlt->setData(baroAltData); + BaroAltitude::DataFields baroAltData; + memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields)); + baroAltData.Altitude = out.altitude + noise.baroAltData.Altitude; + baroAltData.Temperature = out.temperature + noise.baroAltData.Temperature; + baroAltData.Pressure = out.pressure + noise.baroAltData.Pressure; + baroAlt->setData(baroAltData); - baroAltTime=baroAltTime.addMSecs(settings.baroAltRate); + baroAltTime = baroAltTime.addMSecs(settings.baroAltRate); } } /*******************************/ // Update FlightBatteryState object - if (settings.baroAltitudeEnabled){ + if (settings.baroAltitudeEnabled) { if (battTime.msecsTo(currentTime) >= settings.baroAltRate) { - FlightBatteryState::DataFields batteryData; - memset(&batteryData, 0, sizeof(FlightBatteryState::DataFields)); - batteryData.Voltage = out.voltage; - batteryData.Current = out.current; - batteryData.ConsumedEnergy = out.consumption; - flightBatt->setData(batteryData); + FlightBatteryState::DataFields batteryData; + memset(&batteryData, 0, sizeof(FlightBatteryState::DataFields)); + batteryData.Voltage = out.voltage; + batteryData.Current = out.current; + batteryData.ConsumedEnergy = out.consumption; + flightBatt->setData(batteryData); - battTime=battTime.addMSecs(settings.baroAltRate); + battTime = battTime.addMSecs(settings.baroAltRate); } } /*******************************/ // Update AirspeedActual object - if (settings.airspeedActualEnabled){ + if (settings.airspeedActualEnabled) { if (airspeedActualTime.msecsTo(currentTime) >= settings.airspeedActualRate) { - AirspeedActual::DataFields airspeedActualData; - memset(&airspeedActualData, 0, sizeof(AirspeedActual::DataFields)); - airspeedActualData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedActual.CalibratedAirspeed; - airspeedActualData.TrueAirspeed = out.trueAirspeed + noise.airspeedActual.TrueAirspeed; - //airspeedActualData.alpha=out.angleOfAttack; // to be implemented - //airspeedActualData.beta=out.angleOfSlip; - airspeedActual->setData(airspeedActualData); + AirspeedActual::DataFields airspeedActualData; + memset(&airspeedActualData, 0, sizeof(AirspeedActual::DataFields)); + airspeedActualData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedActual.CalibratedAirspeed; + airspeedActualData.TrueAirspeed = out.trueAirspeed + noise.airspeedActual.TrueAirspeed; + // airspeedActualData.alpha=out.angleOfAttack; // to be implemented + // airspeedActualData.beta=out.angleOfSlip; + airspeedActual->setData(airspeedActualData); - airspeedActualTime=airspeedActualTime.addMSecs(settings.airspeedActualRate); + airspeedActualTime = airspeedActualTime.addMSecs(settings.airspeedActualRate); } } @@ -766,7 +756,7 @@ void Simulator::updateUAVOs(Output2Hardware out){ // Update raw attitude sensors if (settings.attRawEnabled) { if (attRawTime.msecsTo(currentTime) >= settings.attRawRate) { - //Update gyroscope sensor data + // Update gyroscope sensor data Gyros::DataFields gyroData; memset(&gyroData, 0, sizeof(Gyros::DataFields)); gyroData.x = out.rollRate + noise.gyroData.x; @@ -774,7 +764,7 @@ void Simulator::updateUAVOs(Output2Hardware out){ gyroData.z = out.yawRate + noise.gyroData.z; gyros->setData(gyroData); - //Update accelerometer sensor data + // Update accelerometer sensor data Accels::DataFields accelData; memset(&accelData, 0, sizeof(Accels::DataFields)); accelData.x = out.accX + noise.accelData.x; @@ -782,7 +772,7 @@ void Simulator::updateUAVOs(Output2Hardware out){ accelData.z = out.accZ + noise.accelData.z; accels->setData(accelData); - attRawTime=attRawTime.addMSecs(settings.attRawRate); + attRawTime = attRawTime.addMSecs(settings.attRawRate); } } } @@ -790,9 +780,10 @@ void Simulator::updateUAVOs(Output2Hardware out){ /** * calculate air density from altitude. http://en.wikipedia.org/wiki/Density_of_air */ -float Simulator::airDensityFromAltitude(float alt, AirParameters air, float gravity) { - float p= airPressureFromAltitude(alt, air, gravity); - float rho=p*air.M/(air.univGasConstant*(air.groundTemp-air.tempLapseRate*alt)); +float Simulator::airDensityFromAltitude(float alt, AirParameters air, float gravity) +{ + float p = airPressureFromAltitude(alt, air, gravity); + float rho = p * air.M / (air.univGasConstant * (air.groundTemp - air.tempLapseRate * alt)); return rho; } @@ -804,8 +795,9 @@ float Simulator::airDensityFromAltitude(float alt, AirParameters air, float grav * @param gravity * @return */ -float Simulator::airPressureFromAltitude(float alt, AirParameters air, float gravity) { - return air.seaLevelPress* pow(1 - air.tempLapseRate * alt /air.groundTemp, gravity * air.M/(air.univGasConstant*air.tempLapseRate)); +float Simulator::airPressureFromAltitude(float alt, AirParameters air, float gravity) +{ + return air.seaLevelPress * pow(1 - air.tempLapseRate * alt / air.groundTemp, gravity * air.M / (air.univGasConstant * air.tempLapseRate)); } /** @@ -816,10 +808,11 @@ float Simulator::airPressureFromAltitude(float alt, AirParameters air, float gra * @param gravity * @return True airspeed */ -float Simulator::cas2tas(float CAS, float alt, AirParameters air, float gravity) { - float rho=airDensityFromAltitude(alt, air, gravity); +float Simulator::cas2tas(float CAS, float alt, AirParameters air, float gravity) +{ + float rho = airDensityFromAltitude(alt, air, gravity); - return (CAS * sqrt(air.groundDensity/rho)); + return CAS * sqrt(air.groundDensity / rho); } /** @@ -830,17 +823,19 @@ float Simulator::cas2tas(float CAS, float alt, AirParameters air, float gravity) * @param gravity * @return Calibrated airspeed */ -float Simulator::tas2cas(float TAS, float alt, AirParameters air, float gravity) { - float rho=airDensityFromAltitude(alt, air, gravity); +float Simulator::tas2cas(float TAS, float alt, AirParameters air, float gravity) +{ + float rho = airDensityFromAltitude(alt, air, gravity); - return (TAS / sqrt(air.groundDensity/rho)); + return TAS / sqrt(air.groundDensity / rho); } /** * @brief Simulator::getAirParameters get air parameters * @return airParameters */ -AirParameters Simulator::getAirParameters(){ +AirParameters Simulator::getAirParameters() +{ return airParameters; } @@ -848,6 +843,7 @@ AirParameters Simulator::getAirParameters(){ * @brief Simulator::setAirParameters set air parameters * @param airParameters */ -void Simulator::setAirParameters(AirParameters airParameters){ - this->airParameters=airParameters; +void Simulator::setAirParameters(AirParameters airParameters) +{ + this->airParameters = airParameters; } diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.h b/ground/openpilotgcs/src/plugins/hitl/simulator.h index c4795142c..e9136a031 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.h @@ -65,15 +65,14 @@ * just imagine this was a class without methods and all public properties */ typedef struct _FLIGHT_PARAM { - // time float T; float dT; unsigned int i; // speeds - float cas; //Calibrated airspeed - float tas; //True airspeed + float cas; // Calibrated airspeed + float tas; // True airspeed float groundspeed; // position (absolute) @@ -91,104 +90,100 @@ typedef struct _FLIGHT_PARAM { float ddY; float ddZ; - //angle + // angle float azimuth; float pitch; float roll; - //rotation speed + // rotation speed float dAzimuth; float dPitch; float dRoll; - } FLIGHT_PARAM; -struct AirParameters -{ +struct AirParameters { float groundDensity; float groundTemp; float seaLevelPress; float tempLapseRate; float univGasConstant; float dryAirConstant; - float relativeHumidity; //[%] - float M; //Molar mass + float relativeHumidity; // [%] + float M; // Molar mass }; -typedef struct _CONNECTION -{ +typedef struct _CONNECTION { QString simulatorId; QString binPath; QString dataPath; QString hostAddress; QString remoteAddress; - int outPort; - int inPort; - bool startSim; - bool addNoise; + int outPort; + int inPort; + bool startSim; + bool addNoise; QString latitude; QString longitude; -// bool homeLocation; +// bool homeLocation; - bool attRawEnabled; - quint8 attRawRate; + bool attRawEnabled; + quint8 attRawRate; - bool attActualEnabled; - bool attActHW; - bool attActSim; - bool attActCalc; + bool attActualEnabled; + bool attActHW; + bool attActSim; + bool attActCalc; - bool baroAltitudeEnabled; + bool baroAltitudeEnabled; quint16 baroAltRate; - bool groundTruthEnabled; + bool groundTruthEnabled; quint16 groundTruthRate; - bool gpsPositionEnabled; + bool gpsPositionEnabled; quint16 gpsPosRate; - bool inputCommand; - bool gcsReceiverEnabled; - bool manualControlEnabled; + bool inputCommand; + bool gcsReceiverEnabled; + bool manualControlEnabled; quint16 minOutputPeriod; - bool airspeedActualEnabled; + bool airspeedActualEnabled; quint16 airspeedActualRate; - } SimulatorSettings; -struct Output2Hardware{ +struct Output2Hardware { float latitude; float longitude; float altitude; - float agl; //[m] + float agl; // [m] float heading; - float groundspeed; //[m/s] - float calibratedAirspeed; //[m/s] - float trueAirspeed; //[m/s] + float groundspeed; // [m/s] + float calibratedAirspeed; // [m/s] + float trueAirspeed; // [m/s] float angleOfAttack; float angleOfSlip; float roll; float pitch; float pressure; float temperature; - float velNorth; //[m/s] - float velEast; //[m/s] - float velDown; //[m/s] - float dstN; //[m] - float dstE; //[m] - float dstD; //[m] - float accX; //[m/s^2] - float accY; //[m/s^2] - float accZ; //[m/s^2] - float rollRate; //[deg/s] - float pitchRate; //[deg/s] - float yawRate; //[deg/s] - float delT; //[s] + float velNorth; // [m/s] + float velEast; // [m/s] + float velDown; // [m/s] + float dstN; // [m] + float dstE; // [m] + float dstD; // [m] + float accX; // [m/s^2] + float accY; // [m/s^2] + float accZ; // [m/s^2] + float rollRate; // [deg/s] + float pitchRate; // [deg/s] + float yawRate; // [deg/s] + float delT; // [s] - float rc_channel[GCSReceiver::CHANNEL_NUMELEM]; //Elements in rc_channel are between -1 and 1 + float rc_channel[GCSReceiver::CHANNEL_NUMELEM]; // Elements in rc_channel are between -1 and 1 float rollDesired; @@ -201,33 +196,50 @@ struct Output2Hardware{ float consumption; }; -//struct Output2Simulator{ -// float roll; -// float pitch; -// float yaw; -// float throttle; +// struct Output2Simulator{ +// float roll; +// float pitch; +// float yaw; +// float throttle; -// float ailerons; -// float rudder; -// float elevator; -// float motor; -//}; +// float ailerons; +// float rudder; +// float elevator; +// float motor; +// }; -class Simulator : public QObject -{ +class Simulator : public QObject { Q_OBJECT public: - Simulator(const SimulatorSettings& params); + Simulator(const SimulatorSettings & params); virtual ~Simulator(); - bool isAutopilotConnected() const { return autopilotConnectionStatus; } - bool isSimulatorConnected() const { return simConnectionStatus; } - QString Name() const { return name; } - void setName(QString str) { name = str; } + bool isAutopilotConnected() const + { + return autopilotConnectionStatus; + } + bool isSimulatorConnected() const + { + return simConnectionStatus; + } + QString Name() const + { + return name; + } + void setName(QString str) + { + name = str; + } - QString SimulatorId() const { return simulatorId; } - void setSimulatorId(QString str) { simulatorId = str; } + QString SimulatorId() const + { + return simulatorId; + } + void setSimulatorId(QString str) + { + simulatorId = str; + } float airDensityFromAltitude(float alt, AirParameters air, float gravity); float airPressureFromAltitude(float alt, AirParameters air, float gravity); @@ -235,13 +247,28 @@ public: float tas2cas(float TAS, float alt, AirParameters air, float gravity); - static bool IsStarted() { return isStarted; } - static void setStarted(bool val) { isStarted = val; } - static QStringList& Instances() { return Simulator::instances; } - static void setInstance(const QString& str) { Simulator::instances.append(str); } + static bool IsStarted() + { + return isStarted; + } + static void setStarted(bool val) + { + isStarted = val; + } + static QStringList & Instances() + { + return Simulator::instances; + } + static void setInstance(const QString & str) + { + Simulator::instances.append(str); + } virtual void stopProcess() {} - virtual void setupUdpPorts(const QString& host, int inPort, int outPort) { Q_UNUSED(host) Q_UNUSED(inPort) Q_UNUSED(outPort)} + virtual void setupUdpPorts(const QString & host, int inPort, int outPort) + { + Q_UNUSED(host) Q_UNUSED(inPort) Q_UNUSED(outPort) + } void resetInitialHomePosition(); void updateUAVOs(Output2Hardware out); @@ -258,19 +285,22 @@ signals: void deleteSimProcess(); void myStart(); public slots: - Q_INVOKABLE virtual bool setupProcess() { return true;} + Q_INVOKABLE virtual bool setupProcess() + { + return true; + } private slots: void onStart(); - //void transmitUpdate(); + // void transmitUpdate(); void receiveUpdate(); void onAutopilotConnect(); void onAutopilotDisconnect(); void onSimulatorConnectionTimeout(); - void telStatsUpdated(UAVObject* obj); + void telStatsUpdated(UAVObject *obj); Q_INVOKABLE void onDeleteSimulator(void); virtual void transmitUpdate() = 0; - virtual void processUpdate(const QByteArray& data) = 0; + virtual void processUpdate(const QByteArray & data) = 0; protected: static const float GEE; @@ -281,30 +311,30 @@ protected: static const float DEG2RAD; static const float RAD2DEG; - QProcess* simProcess; - QTime* time; - QUdpSocket* inSocket;//(new QUdpSocket()); - QUdpSocket* outSocket; + QProcess *simProcess; + QTime *time; + QUdpSocket *inSocket; // (new QUdpSocket()); + QUdpSocket *outSocket; - ActuatorCommand* actCommand; - ActuatorDesired* actDesired; - ManualControlCommand* manCtrlCommand; - FlightStatus* flightStatus; - FlightBatteryState* flightBatt; - BaroAltitude* baroAlt; - AirspeedActual* airspeedActual; - AttitudeActual* attActual; - AttitudeSettings* attSettings; - VelocityActual* velActual; - GPSPosition* gpsPos; - GPSVelocity* gpsVel; - PositionActual* posActual; - HomeLocation* posHome; - Accels* accels; - Gyros* gyros; - GCSTelemetryStats* telStats; - GCSReceiver* gcsReceiver; - GroundTruth* groundTruth; + ActuatorCommand *actCommand; + ActuatorDesired *actDesired; + ManualControlCommand *manCtrlCommand; + FlightStatus *flightStatus; + FlightBatteryState *flightBatt; + BaroAltitude *baroAlt; + AirspeedActual *airspeedActual; + AttitudeActual *attActual; + AttitudeSettings *attSettings; + VelocityActual *velActual; + GPSPosition *gpsPos; + GPSVelocity *gpsVel; + PositionActual *posActual; + HomeLocation *posHome; + Accels *accels; + Gyros *gyros; + GCSTelemetryStats *telStats; + GCSReceiver *gcsReceiver; + GroundTruth *groundTruth; SimulatorSettings settings; @@ -322,8 +352,8 @@ private: int simTimeout; volatile bool autopilotConnectionStatus; volatile bool simConnectionStatus; - QTimer* txTimer; - QTimer* simTimer; + QTimer *txTimer; + QTimer *simTimer; QTime attRawTime; QTime gpsPosTime; @@ -337,9 +367,9 @@ private: QString simulatorId; volatile static bool isStarted; static QStringList instances; - //QList > requiredUAVObjects; - void setupOutputObject(UAVObject* obj, quint32 updatePeriod); - void setupInputObject(UAVObject* obj, quint32 updatePeriod); + // QList > requiredUAVObjects; + void setupOutputObject(UAVObject *obj, quint32 updatePeriod); + void setupInputObject(UAVObject *obj, quint32 updatePeriod); void setupWatchedObject(UAVObject *obj, quint32 updatePeriod); void setupObjects(); @@ -347,9 +377,7 @@ private: }; - -class SimulatorCreator -{ +class SimulatorCreator { public: SimulatorCreator(QString id, QString descr) : classId(id), @@ -357,10 +385,16 @@ public: {} virtual ~SimulatorCreator() {} - QString ClassId() const {return classId;} - QString Description() const {return description;} + QString ClassId() const + { + return classId; + } + QString Description() const + { + return description; + } - virtual Simulator* createSimulator(const SimulatorSettings& params) = 0; + virtual Simulator *createSimulator(const SimulatorSettings & params) = 0; private: QString classId; diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp index e79f8f45f..f26dc3229 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp @@ -63,9 +63,9 @@ #include #include -void TraceBuf(const char* buf,int len); +void TraceBuf(const char *buf, int len); -XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) : +XplaneSimulator::XplaneSimulator(const SimulatorSettings & params) : Simulator(params) { resetInitialHomePosition(); @@ -73,25 +73,23 @@ XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) : XplaneSimulator::~XplaneSimulator() -{ -} +{} -void XplaneSimulator::setupUdpPorts(const QString& host, int inPort, int outPort) +void XplaneSimulator::setupUdpPorts(const QString & host, int inPort, int outPort) { Q_UNUSED(outPort); inSocket->bind(QHostAddress(host), inPort); - //outSocket->bind(QHostAddress(host), outPort); + // outSocket->bind(QHostAddress(host), outPort); resetInitialHomePosition(); - } bool XplaneSimulator::setupProcess() { emit processOutput(QString("Please start X-Plane manually, and make sure it is setup to output its ") + "data to host " + settings.hostAddress + " UDP port " + QString::number(settings.inPort)); - return true; + return true; } /** @@ -100,198 +98,190 @@ bool XplaneSimulator::setupProcess() void XplaneSimulator::transmitUpdate() { if (settings.manualControlEnabled) { - //Read ActuatorDesired from autopilot + // Read ActuatorDesired from autopilot ActuatorDesired::DataFields actData = actDesired->getData(); float ailerons = actData.Roll; float elevator = actData.Pitch; - float rudder = actData.Yaw; - float throttle = actData.Throttle > 0? actData.Throttle : 0; - float none = -999; - //quint32 none = *((quint32*)&tmp); // get float as 4 bytes - + float rudder = actData.Yaw; + float throttle = actData.Throttle > 0 ? actData.Throttle : 0; + float none = -999; + // quint32 none = *((quint32*)&tmp); // get float as 4 bytes + quint32 code; QByteArray buf; - QDataStream stream(&buf,QIODevice::ReadWrite); - + QDataStream stream(&buf, QIODevice::ReadWrite); + // !!! LAN byte order - Big Endian #if Q_BYTE_ORDER == Q_LITTLE_ENDIAN stream.setByteOrder(QDataStream::LittleEndian); #endif - + // 11th data settings (flight con: ail/elv/rud) buf.clear(); code = 11; - //quint8 header[] = "DATA"; + // quint8 header[] = "DATA"; /* - stream << *((quint32*)header) << + stream << *((quint32*)header) << (quint8)0x30 << code << - *((quint32*)&elevator) << - *((quint32*)&ailerons) << - *((quint32*)&rudder) << + *((quint32*)&elevator) << + *((quint32*)&ailerons) << + *((quint32*)&rudder) << none << - *((quint32*)&ailerons) << + *((quint32*)&ailerons) << none << none << none; - */ + */ buf.append("DATA0"); - buf.append(reinterpret_cast(&code), sizeof(code)); - buf.append(reinterpret_cast(&elevator), sizeof(elevator)); - buf.append(reinterpret_cast(&ailerons), sizeof(ailerons)); - buf.append(reinterpret_cast(&rudder), sizeof(rudder)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&rudder), sizeof(rudder)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); -// TraceBuf(buf.data(),41); - - if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) - { + buf.append(reinterpret_cast(&code), sizeof(code)); + buf.append(reinterpret_cast(&elevator), sizeof(elevator)); + buf.append(reinterpret_cast(&ailerons), sizeof(ailerons)); + buf.append(reinterpret_cast(&rudder), sizeof(rudder)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&rudder), sizeof(rudder)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); +// TraceBuf(buf.data(),41); + + if (outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); } - //outSocket->write(buf); - + // outSocket->write(buf); + // 25th data settings (throttle command) buf.clear(); code = 25; - //stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none - // << none << none << none << none << none; + // stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none + // << none << none << none << none << none; buf.append("DATA0"); - buf.append(reinterpret_cast(&code), sizeof(code)); - buf.append(reinterpret_cast(&throttle), sizeof(throttle)); - buf.append(reinterpret_cast(&throttle), sizeof(throttle)); - buf.append(reinterpret_cast(&throttle), sizeof(throttle)); - buf.append(reinterpret_cast(&throttle), sizeof(throttle)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - - if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) - { + buf.append(reinterpret_cast(&code), sizeof(code)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + + if (outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); } - - //outSocket->write(buf); - - - - /** !!! this settings was given from ardupilot X-Plane.pl, I comment them - but if it needed comment should be removed !!! - - // 8th data settings (joystick 1 ail/elv/rud) - stream << "DATA0" << quint32(11) << elevator << ailerons << rudder - << float(-999) << float(-999) << float(-999) << float(-999) << float(-999); - outSocket->write(buf); - */ - } + // outSocket->write(buf); + + + /** !!! this settings was given from ardupilot X-Plane.pl, I comment them + but if it needed comment should be removed !!! + + // 8th data settings (joystick 1 ail/elv/rud) + stream << "DATA0" << quint32(11) << elevator << ailerons << rudder + << float(-999) << float(-999) << float(-999) << float(-999) << float(-999); + outSocket->write(buf); + */ + } } /** * process data string from X-Plane simulator */ -void XplaneSimulator::processUpdate(const QByteArray& dataBuf) +void XplaneSimulator::processUpdate(const QByteArray & dataBuf) { - float altitude_msl = 0; - float altitude_agl = 0; - float latitude = 0; - float longitude = 0; - float airspeed_keas = 0; + float altitude_msl = 0; + float altitude_agl = 0; + float latitude = 0; + float longitude = 0; + float airspeed_keas = 0; float groundspeed_ktgs = 0; - float pitch = 0; - float roll = 0; - float heading = 0; - float pressure = 0; - float temperature = 0; - float velX = 0; - float velY = 0; - float velZ = 0; - float dstX = 0; - float dstY = 0; - float dstZ = 0; - float accX = 0; - float accY = 0; - float accZ = 0; - float rollRate_rad=0; - float pitchRate_rad=0; - float yawRate_rad=0; + float pitch = 0; + float roll = 0; + float heading = 0; + float pressure = 0; + float temperature = 0; + float velX = 0; + float velY = 0; + float velZ = 0; + float dstX = 0; + float dstY = 0; + float dstZ = 0; + float accX = 0; + float accY = 0; + float accZ = 0; + float rollRate_rad = 0; + float pitchRate_rad = 0; + float yawRate_rad = 0; - // QString str; - QByteArray& buf = const_cast(dataBuf); + // QString str; + QByteArray & buf = const_cast(dataBuf); QString data(buf); - if(data.left(4) == "DATA") // check type of packet - { - buf.remove(0,5); - if(dataBuf.size() % 36) - { - qxtLog->info("incorrect length of UDP packet: ",buf); + if (data.left(4) == "DATA") { // check type of packet + buf.remove(0, 5); + if (dataBuf.size() % 36) { + qxtLog->info("incorrect length of UDP packet: ", buf); return; // incorrect length of struct } // check correctness of data length, length must be multiple of (id_size+8*float_size)=4+8*4=36 int channelCounter = dataBuf.size() / 36; - do - { - switch(buf[0]) // switch by id - { + do { + switch (buf[0]) { // switch by id case XplaneSimulator::LatitudeLongitudeAltitude: - latitude = *((float*)(buf.data()+4*1)); - longitude = *((float*)(buf.data()+4*2)); - altitude_msl = *((float*)(buf.data()+4*3))* FT2M; - altitude_agl = *((float*)(buf.data()+4*4))* FT2M; + latitude = *((float *)(buf.data() + 4 * 1)); + longitude = *((float *)(buf.data() + 4 * 2)); + altitude_msl = *((float *)(buf.data() + 4 * 3)) * FT2M; + altitude_agl = *((float *)(buf.data() + 4 * 4)) * FT2M; break; case XplaneSimulator::Speed: - airspeed_keas = *((float*)(buf.data()+4*2)); - groundspeed_ktgs = *((float*)(buf.data()+4*4)); + airspeed_keas = *((float *)(buf.data() + 4 * 2)); + groundspeed_ktgs = *((float *)(buf.data() + 4 * 4)); break; case XplaneSimulator::PitchRollHeading: - pitch = *((float*)(buf.data()+4*1)); - roll = *((float*)(buf.data()+4*2)); - heading = *((float*)(buf.data()+4*3)); + pitch = *((float *)(buf.data() + 4 * 1)); + roll = *((float *)(buf.data() + 4 * 2)); + heading = *((float *)(buf.data() + 4 * 3)); break; - /* - case XplaneSimulator::SystemPressures: - pressure = *((float*)(buf.data()+4*1)); - break; - */ + /* + case XplaneSimulator::SystemPressures: + pressure = *((float*)(buf.data()+4*1)); + break; + */ case XplaneSimulator::AtmosphereWeather: - pressure = *((float*)(buf.data()+4*1)) * INHG2KPA; - temperature = *((float*)(buf.data()+4*2)); + pressure = *((float *)(buf.data() + 4 * 1)) * INHG2KPA; + temperature = *((float *)(buf.data() + 4 * 2)); break; case XplaneSimulator::LocVelDistTraveled: - dstX = *((float*)(buf.data()+4*1)); - dstY = - *((float*)(buf.data()+4*3)); - dstZ = *((float*)(buf.data()+4*2)); - velX = *((float*)(buf.data()+4*4)); - velY = - *((float*)(buf.data()+4*6)); - velZ = *((float*)(buf.data()+4*5)); + dstX = *((float *)(buf.data() + 4 * 1)); + dstY = -*((float *)(buf.data() + 4 * 3)); + dstZ = *((float *)(buf.data() + 4 * 2)); + velX = *((float *)(buf.data() + 4 * 4)); + velY = -*((float *)(buf.data() + 4 * 6)); + velZ = *((float *)(buf.data() + 4 * 5)); break; - case XplaneSimulator::AngularVelocities: //In [rad/s] - pitchRate_rad = *((float*)(buf.data()+4*1)); - rollRate_rad = *((float*)(buf.data()+4*2)); - yawRate_rad = *((float*)(buf.data()+4*3)); - break; + case XplaneSimulator::AngularVelocities: // In [rad/s] + pitchRate_rad = *((float *)(buf.data() + 4 * 1)); + rollRate_rad = *((float *)(buf.data() + 4 * 2)); + yawRate_rad = *((float *)(buf.data() + 4 * 3)); + break; case XplaneSimulator::Gload: - accX = *((float*)(buf.data()+4*6)) * GEE; - accY = *((float*)(buf.data()+4*7)) * GEE; - accZ = *((float*)(buf.data()+4*5)) * GEE; + accX = *((float *)(buf.data() + 4 * 6)) * GEE; + accY = *((float *)(buf.data() + 4 * 7)) * GEE; + accZ = *((float *)(buf.data() + 4 * 5)) * GEE; break; default: break; } channelCounter--; - buf.remove(0,36); + buf.remove(0, 36); } while (channelCounter); @@ -302,73 +292,70 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf) memset(&out, 0, sizeof(Output2Hardware)); // Update GPS Position objects - out.latitude = latitude * 1e7; - out.longitude = longitude * 1e7; - out.altitude = altitude_msl; + out.latitude = latitude * 1e7; + out.longitude = longitude * 1e7; + out.altitude = altitude_msl; out.agl = altitude_agl; - out.groundspeed = groundspeed_ktgs*1.15*1.6089/3.6; //Convert from [kts] to [m/s] + out.groundspeed = groundspeed_ktgs * 1.15 * 1.6089 / 3.6; // Convert from [kts] to [m/s] - out.calibratedAirspeed = airspeed_keas*1.15*1.6089/3.6; //Convert from [kts] to [m/s] + out.calibratedAirspeed = airspeed_keas * 1.15 * 1.6089 / 3.6; // Convert from [kts] to [m/s] // Update BaroAltitude object out.temperature = temperature; - out.pressure = pressure; + out.pressure = pressure; // Update attActual object - out.roll = roll; //roll; - out.pitch = pitch; // pitch - out.heading = heading; // yaw + out.roll = roll; // roll; + out.pitch = pitch; // pitch + out.heading = heading; // yaw - out.dstN=dstY; - out.dstE=dstX; - out.dstD=-dstZ; + out.dstN = dstY; + out.dstE = dstX; + out.dstD = -dstZ; // Update VelocityActual.{North,East,Down} - out.velNorth = velY; - out.velEast = velX; - out.velDown = -velZ; + out.velNorth = velY; + out.velEast = velX; + out.velDown = -velZ; - //Update gyroscope sensor data - out.rollRate = rollRate_rad; + // Update gyroscope sensor data + out.rollRate = rollRate_rad; out.pitchRate = pitchRate_rad; - out.yawRate = yawRate_rad; + out.yawRate = yawRate_rad; - //Update accelerometer sensor data - out.accX = accX; - out.accY = accY; - out.accZ = -accZ; + // Update accelerometer sensor data + out.accX = accX; + out.accY = accY; + out.accZ = -accZ; updateUAVOs(out); } // issue manual update - //attActual->updated(); - //altActual->updated(); - //posActual->updated(); - + // attActual->updated(); + // altActual->updated(); + // posActual->updated(); } -void TraceBuf(const char* buf,int len) +void TraceBuf(const char *buf, int len) { - QString str; - bool reminder=true; - for(int i=0; i < len; i++) - { - if(!(i%16)) - { - if(i>0) - { - qDebug() << str; - str.clear(); - reminder=false; - } - reminder=true; - } - str+=QString(" 0x%1").arg((quint8)buf[i],2,16,QLatin1Char('0')); - } + QString str; + bool reminder = true; - if(reminder){ + for (int i = 0; i < len; i++) { + if (!(i % 16)) { + if (i > 0) { + qDebug() << str; + str.clear(); + reminder = false; + } + reminder = true; + } + str += QString(" 0x%1").arg((quint8)buf[i], 2, 16, QLatin1Char('0')); + } + + if (reminder) { qDebug() << str; } } diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h index 0aaa42bd2..b1c33f36f 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h @@ -31,61 +31,58 @@ #include #include -class XplaneSimulator: public Simulator -{ - Q_OBJECT +class XplaneSimulator : public Simulator { + Q_OBJECT public: - XplaneSimulator(const SimulatorSettings& params); - ~XplaneSimulator(); - bool setupProcess(); + XplaneSimulator(const SimulatorSettings & params); + ~XplaneSimulator(); + bool setupProcess(); - void setupUdpPorts(const QString& host, int inPort, int outPort); + void setupUdpPorts(const QString & host, int inPort, int outPort); private slots: - void transmitUpdate(); + void transmitUpdate(); private: - enum XplaneOutputData //***WARNING***: Elements in this enum are in a precise order, do - { // not change. Cf. http://www.nuclearprojects.com/xplane/info.shtml - FramRate, - Times, - SimStats, - Speed, - Gload, - AtmosphereWeather, - AtmosphereAircraft, - SystemPressures, - Joystick1, - Joystick2, - ArtStab, - FlightCon, - WingSweep, - Trim, - Brakes, - AngularMoments, - AngularAccelerations, + enum XplaneOutputData // ***WARNING***: Elements in this enum are in a precise order, do + { // not change. Cf. http://www.nuclearprojects.com/xplane/info.shtml + FramRate, + Times, + SimStats, + Speed, + Gload, + AtmosphereWeather, + AtmosphereAircraft, + SystemPressures, + Joystick1, + Joystick2, + ArtStab, + FlightCon, + WingSweep, + Trim, + Brakes, + AngularMoments, + AngularAccelerations, AngularVelocities, PitchRollHeading, - AoA, + AoA, LatitudeLongitudeAltitude, - LocVelDistTraveled - }; - - void processUpdate(const QByteArray& data); + LocVelDistTraveled + }; + void processUpdate(const QByteArray & data); }; -class XplaneSimulatorCreator : public SimulatorCreator -{ +class XplaneSimulatorCreator : public SimulatorCreator { public: - XplaneSimulatorCreator(const QString& classId, const QString& description) - : SimulatorCreator (classId,description) - {} + XplaneSimulatorCreator(const QString & classId, const QString & description) + : SimulatorCreator(classId, description) + {} - Simulator* createSimulator(const SimulatorSettings& params) - { - return new XplaneSimulator(params); - } + Simulator *createSimulator(const SimulatorSettings & params) + { + return new XplaneSimulator(params); + } }; #endif // XPLANESIMULATOR_H diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportdialog.cpp b/ground/openpilotgcs/src/plugins/importexport/importexportdialog.cpp index df38653f8..952ea6e35 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportdialog.cpp +++ b/ground/openpilotgcs/src/plugins/importexport/importexportdialog.cpp @@ -8,7 +8,7 @@ ImportExportDialog::ImportExportDialog(QWidget *parent) : ui->setupUi(this); setWindowTitle(tr("Import Export Settings")); - connect( ui->widget, SIGNAL(done()), this, SLOT(close())); + connect(ui->widget, SIGNAL(done()), this, SLOT(close())); } ImportExportDialog::~ImportExportDialog() @@ -19,6 +19,7 @@ ImportExportDialog::~ImportExportDialog() void ImportExportDialog::changeEvent(QEvent *e) { QDialog::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: ui->retranslateUi(this); diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportdialog.h b/ground/openpilotgcs/src/plugins/importexport/importexportdialog.h index 008d588d1..b897673ca 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportdialog.h +++ b/ground/openpilotgcs/src/plugins/importexport/importexportdialog.h @@ -29,11 +29,10 @@ #include namespace Ui { - class ImportExportDialog; +class ImportExportDialog; } -class ImportExportDialog : public QDialog -{ +class ImportExportDialog : public QDialog { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportgadget.cpp b/ground/openpilotgcs/src/plugins/importexport/importexportgadget.cpp index 41a9b1616..e3080d8b8 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportgadget.cpp +++ b/ground/openpilotgcs/src/plugins/importexport/importexportgadget.cpp @@ -32,10 +32,9 @@ #include "importexportgadgetconfiguration.h" ImportExportGadget::ImportExportGadget(QString classId, ImportExportGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} ImportExportGadget::~ImportExportGadget() { @@ -43,14 +42,14 @@ ImportExportGadget::~ImportExportGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void ImportExportGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void ImportExportGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - m_widget->loadConfiguration(qobject_cast(config)); + m_widget->loadConfiguration(qobject_cast(config)); } /** * @} diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportgadget.h b/ground/openpilotgcs/src/plugins/importexport/importexportgadget.h index 13d7a7db6..f01084994 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportgadget.h +++ b/ground/openpilotgcs/src/plugins/importexport/importexportgadget.h @@ -37,17 +37,17 @@ class ImportExportGadgetWidget; using namespace Core; -class IMPORTEXPORT_EXPORT ImportExportGadget : public Core::IUAVGadget -{ +class IMPORTEXPORT_EXPORT ImportExportGadget : public Core::IUAVGadget { Q_OBJECT public: ImportExportGadget(QString classId, ImportExportGadgetWidget *widget, QWidget *parent = 0); ~ImportExportGadget(); - QWidget *widget() { + QWidget *widget() + { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); private: ImportExportGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.cpp index 35d4696a0..f7a2b3179 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.cpp @@ -43,13 +43,13 @@ #include ImportExportGadgetWidget::ImportExportGadgetWidget(QWidget *parent) : - QWidget(parent), - ui(new Ui::ImportExportGadgetWidget) + QWidget(parent), + ui(new Ui::ImportExportGadgetWidget) { setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); ui->setupUi(this); - filename = ""; + filename = ""; } ImportExportGadgetWidget::~ImportExportGadgetWidget() @@ -60,6 +60,7 @@ ImportExportGadgetWidget::~ImportExportGadgetWidget() void ImportExportGadgetWidget::changeEvent(QEvent *e) { QWidget::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: ui->retranslateUi(this); @@ -71,26 +72,28 @@ void ImportExportGadgetWidget::changeEvent(QEvent *e) void ImportExportGadgetWidget::on_exportButton_clicked() { - QString file = filename; - QString filter = tr("GCS Settings file (*.xml)"); - file = QFileDialog::getSaveFileName(this, tr("Save GCS Settings too file .."), QFileInfo(file).absoluteFilePath(), filter).trimmed(); - if (file.isEmpty()) { - return; - } + QString file = filename; + QString filter = tr("GCS Settings file (*.xml)"); - // Add a "XML" extension to the file in case it does not exist: - if (!file.toLower().endsWith(".xml")) + file = QFileDialog::getSaveFileName(this, tr("Save GCS Settings too file .."), QFileInfo(file).absoluteFilePath(), filter).trimmed(); + if (file.isEmpty()) { + return; + } + + // Add a "XML" extension to the file in case it does not exist: + if (!file.toLower().endsWith(".xml")) { file.append(".xml"); + } - filename = file; + filename = file; qDebug() << "Export pressed! Write to file " << QFileInfo(file).absoluteFilePath(); QMessageBox msgBox; QDir dir = QFileInfo(file).absoluteDir(); - if (! dir.exists()) { + if (!dir.exists()) { msgBox.setText(tr("Can't write file ") + QFileInfo(file).absoluteFilePath() - + " since directory "+ dir.absolutePath() + " doesn't exist!"); + + " since directory " + dir.absolutePath() + " doesn't exist!"); msgBox.exec(); return; } @@ -99,28 +102,27 @@ void ImportExportGadgetWidget::on_exportButton_clicked() msgBox.setText(tr("The settings have been exported to ") + QFileInfo(file).absoluteFilePath()); msgBox.exec(); emit done(); - } -QList ImportExportGadgetWidget::getConfigurables() +QList ImportExportGadgetWidget::getConfigurables() { - QList configurables; + QList configurables; QList specs = ExtensionSystem::PluginManager::instance()->plugins(); - foreach ( ExtensionSystem::PluginSpec* spec, specs ){ - if ( Core::IConfigurablePlugin* plugin = dynamic_cast(spec->plugin()) ){ - qDebug()<< "Configurable: " << plugin->metaObject()->className(); + foreach(ExtensionSystem::PluginSpec * spec, specs) { + if (Core::IConfigurablePlugin * plugin = dynamic_cast(spec->plugin())) { + qDebug() << "Configurable: " << plugin->metaObject()->className(); configurables.append(plugin); } } return configurables; } -void ImportExportGadgetWidget::exportConfiguration(const QString& fileName) +void ImportExportGadgetWidget::exportConfiguration(const QString & fileName) { - bool doGeneral = ui->checkBoxGeneral->isChecked(); + bool doGeneral = ui->checkBoxGeneral->isChecked(); bool doAllGadgets = ui->checkBoxAllGadgets->isChecked(); - bool doPlugins = ui->checkBoxPlugins->isChecked(); + bool doPlugins = ui->checkBoxPlugins->isChecked(); QSettings::Format format = XmlConfig::XmlSettingsFormat; QSettings qs(fileName, format); @@ -131,9 +133,9 @@ void ImportExportGadgetWidget::exportConfiguration(const QString& fileName) if (doAllGadgets) { Core::ICore::instance()->uavGadgetInstanceManager()->saveSettings(&qs); } - if ( doPlugins ){ - foreach ( Core::IConfigurablePlugin *plugin, getConfigurables()){ - Core::ICore::instance()->saveSettings(plugin,&qs); + if (doPlugins) { + foreach(Core::IConfigurablePlugin * plugin, getConfigurables()) { + Core::ICore::instance()->saveSettings(plugin, &qs); } } @@ -141,26 +143,27 @@ void ImportExportGadgetWidget::exportConfiguration(const QString& fileName) } -void ImportExportGadgetWidget::writeError(const QString& msg) const +void ImportExportGadgetWidget::writeError(const QString & msg) const { qWarning() << "ERROR: " << msg; } void ImportExportGadgetWidget::on_importButton_clicked() { - QString file = filename; - QString filter = tr("GCS Settings file (*.xml)"); - file = QFileDialog::getOpenFileName(this, tr("Load GCS Settings from file .."), QFileInfo(file).absoluteFilePath(), filter).trimmed(); - if (file.isEmpty()) { - return; - } + QString file = filename; + QString filter = tr("GCS Settings file (*.xml)"); - filename = file; + file = QFileDialog::getOpenFileName(this, tr("Load GCS Settings from file .."), QFileInfo(file).absoluteFilePath(), filter).trimmed(); + if (file.isEmpty()) { + return; + } + + filename = file; qDebug() << "Import pressed! Read from file " << QFileInfo(file).absoluteFilePath(); QMessageBox msgBox; - if (! QFileInfo(file).isReadable()) { + if (!QFileInfo(file).isReadable()) { msgBox.setText(tr("Can't read file ") + QFileInfo(file).absoluteFilePath()); msgBox.exec(); return; @@ -174,23 +177,23 @@ void ImportExportGadgetWidget::on_importButton_clicked() emit done(); } -void ImportExportGadgetWidget::importConfiguration(const QString& fileName) +void ImportExportGadgetWidget::importConfiguration(const QString & fileName) { - bool doGeneral = ui->checkBoxGeneral->isChecked(); + bool doGeneral = ui->checkBoxGeneral->isChecked(); bool doAllGadgets = ui->checkBoxAllGadgets->isChecked(); - bool doPlugins = ui->checkBoxPlugins->isChecked(); + bool doPlugins = ui->checkBoxPlugins->isChecked(); QSettings qs(fileName, XmlConfig::XmlSettingsFormat); - if ( doAllGadgets ) { + if (doAllGadgets) { Core::ICore::instance()->uavGadgetInstanceManager()->readSettings(&qs); } - if ( doGeneral ) { + if (doGeneral) { Core::ICore::instance()->readMainSettings(&qs); } - if ( doPlugins ){ - foreach ( Core::IConfigurablePlugin *plugin, getConfigurables()){ - Core::ICore::instance()->readSettings(plugin,&qs); + if (doPlugins) { + foreach(Core::IConfigurablePlugin * plugin, getConfigurables()) { + Core::ICore::instance()->readSettings(plugin, &qs); } } @@ -206,15 +209,15 @@ void ImportExportGadgetWidget::on_helpButton_clicked() void ImportExportGadgetWidget::on_resetButton_clicked() { QMessageBox msgBox; + msgBox.setText(tr("All your settings will be deleted!")); msgBox.setInformativeText(tr("You must restart the GCS in order to activate the changes.")); msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); msgBox.setDefaultButton(QMessageBox::Ok); - if ( msgBox.exec() == QMessageBox::Ok ){ + if (msgBox.exec() == QMessageBox::Ok) { qDebug() << "Reset requested!"; Core::ICore::instance()->deleteSettings(); - } - else{ + } else { qDebug() << "Reset canceled!"; return; } @@ -225,4 +228,3 @@ void ImportExportGadgetWidget::on_resetButton_clicked() * @} * @} */ - diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.h b/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.h index bb6d46aa3..4c8b6d54f 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.h @@ -16,13 +16,11 @@ #include "importexport_global.h" #include -namespace Ui -{ +namespace Ui { class ImportExportGadgetWidget; } -class IMPORTEXPORT_EXPORT ImportExportGadgetWidget : public QWidget -{ +class IMPORTEXPORT_EXPORT ImportExportGadgetWidget : public QWidget { Q_OBJECT public: ImportExportGadgetWidget(QWidget *parent = 0); @@ -36,15 +34,15 @@ protected: private: Ui::ImportExportGadgetWidget *ui; - void writeError(const QString&) const; - void exportConfiguration(const QString& fileName); - void importConfiguration(const QString& fileName); - QList getConfigurables(); + void writeError(const QString &) const; + void exportConfiguration(const QString & fileName); + void importConfiguration(const QString & fileName); + QList getConfigurables(); - QString filename; + QString filename; private slots: - void on_resetButton_clicked(); + void on_resetButton_clicked(); void on_helpButton_clicked(); void on_importButton_clicked(); void on_exportButton_clicked(); diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportplugin.cpp b/ground/openpilotgcs/src/plugins/importexport/importexportplugin.cpp index d7fa31a54..7877844de 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportplugin.cpp +++ b/ground/openpilotgcs/src/plugins/importexport/importexportplugin.cpp @@ -50,25 +50,25 @@ ImportExportPlugin::~ImportExportPlugin() // Do nothing } -bool ImportExportPlugin::initialize(const QStringList& args, QString *errMsg) +bool ImportExportPlugin::initialize(const QStringList & args, QString *errMsg) { Q_UNUSED(args); Q_UNUSED(errMsg); // Add Menu entry - Core::ActionManager* am = Core::ICore::instance()->actionManager(); - Core::ActionContainer* ac = am->actionContainer(Core::Constants::M_FILE); + Core::ActionManager *am = Core::ICore::instance()->actionManager(); + Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_FILE); - Core::Command* cmd = am->registerAction(new QAction(this), + Core::Command *cmd = am->registerAction(new QAction(this), "ImportExportPlugin.ImportExport", QList() << Core::Constants::C_GLOBAL_ID); cmd->setDefaultKeySequence(QKeySequence("Ctrl+S")); cmd->action()->setText(tr("GCS Settings Import/Export...")); -// ac->menu()->addSeparator(); -// ac->appendGroup("ImportExport"); -// ac->addAction(cmd, "ImportExport"); +// ac->menu()->addSeparator(); +// ac->appendGroup("ImportExport"); +// ac->addAction(cmd, "ImportExport"); ac->addAction(cmd, Core::Constants::G_FILE_SAVE); diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportplugin.h b/ground/openpilotgcs/src/plugins/importexport/importexportplugin.h index 7070ce56c..92d8e0c6b 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportplugin.h +++ b/ground/openpilotgcs/src/plugins/importexport/importexportplugin.h @@ -30,8 +30,7 @@ #include #include "importexport_global.h" -class IMPORTEXPORT_EXPORT ImportExportPlugin : public ExtensionSystem::IPlugin -{ +class IMPORTEXPORT_EXPORT ImportExportPlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -39,13 +38,12 @@ public: ~ImportExportPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); private: private slots: void importExport(); - }; #endif /* IMPORTEXPORTPLUGIN_H_ */ /** diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnection_internal.h b/ground/openpilotgcs/src/plugins/ipconnection/ipconnection_internal.h index c0eddc508..b7c2f3992 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnection_internal.h +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnection_internal.h @@ -3,16 +3,15 @@ #include "ipconnectionplugin.h" -//Simple class for creating & destroying a socket in the real-time thread -//Needed because sockets need to be created in the same thread that they're used -class IPConnection : public QObject -{ +// Simple class for creating & destroying a socket in the real-time thread +// Needed because sockets need to be created in the same thread that they're used +class IPConnection : public QObject { Q_OBJECT public: IPConnection(IPconnectionConnection *connection); - //virtual ~IPConnection(); + // virtual ~IPConnection(); public slots: diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.cpp b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.cpp index 537965577..14c9eccd0 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.cpp @@ -28,7 +28,7 @@ #include "ipconnectionconfiguration.h" #include -IPconnectionConfiguration::IPconnectionConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +IPconnectionConfiguration::IPconnectionConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_HostName("127.0.0.1"), m_Port(1000), @@ -40,15 +40,15 @@ IPconnectionConfiguration::IPconnectionConfiguration(QString classId, QSettings* } IPconnectionConfiguration::~IPconnectionConfiguration() -{ -} +{} IUAVGadgetConfiguration *IPconnectionConfiguration::clone() { IPconnectionConfiguration *m = new IPconnectionConfiguration(this->classId()); - m->m_Port = m_Port; + + m->m_Port = m_Port; m->m_HostName = m_HostName; - m->m_UseTCP = m_UseTCP; + m->m_UseTCP = m_UseTCP; return m; } @@ -56,23 +56,24 @@ IUAVGadgetConfiguration *IPconnectionConfiguration::clone() * Saves a configuration. * */ -void IPconnectionConfiguration::saveConfig(QSettings* qSettings) const { - qSettings->setValue("port", m_Port); - qSettings->setValue("hostName", m_HostName); - qSettings->setValue("useTCP", m_UseTCP); +void IPconnectionConfiguration::saveConfig(QSettings *qSettings) const +{ + qSettings->setValue("port", m_Port); + qSettings->setValue("hostName", m_HostName); + qSettings->setValue("useTCP", m_UseTCP); } void IPconnectionConfiguration::savesettings() const { settings->beginGroup(QLatin1String("IPconnection")); - settings->beginWriteArray("Current"); - settings->setArrayIndex(0); - settings->setValue(QLatin1String("HostName"), m_HostName); - settings->setValue(QLatin1String("Port"), m_Port); - settings->setValue(QLatin1String("UseTCP"), m_UseTCP); - settings->endArray(); - settings->endGroup(); + settings->beginWriteArray("Current"); + settings->setArrayIndex(0); + settings->setValue(QLatin1String("HostName"), m_HostName); + settings->setValue(QLatin1String("Port"), m_Port); + settings->setValue(QLatin1String("UseTCP"), m_UseTCP); + settings->endArray(); + settings->endGroup(); } @@ -80,14 +81,11 @@ void IPconnectionConfiguration::restoresettings() { settings->beginGroup(QLatin1String("IPconnection")); - settings->beginReadArray("Current"); - settings->setArrayIndex(0); - m_HostName = (settings->value(QLatin1String("HostName"), tr("")).toString()); - m_Port = (settings->value(QLatin1String("Port"), tr("")).toInt()); - m_UseTCP = (settings->value(QLatin1String("UseTCP"), tr("")).toInt()); - settings->endArray(); - settings->endGroup(); - - + settings->beginReadArray("Current"); + settings->setArrayIndex(0); + m_HostName = (settings->value(QLatin1String("HostName"), tr("")).toString()); + m_Port = (settings->value(QLatin1String("Port"), tr("")).toInt()); + m_UseTCP = (settings->value(QLatin1String("UseTCP"), tr("")).toInt()); + settings->endArray(); + settings->endGroup(); } - diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.h b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.h index 4286b91ca..63b70c5b4 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.h +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.h @@ -34,41 +34,55 @@ using namespace Core; -class IPconnectionConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT -Q_PROPERTY(QString HostName READ HostName WRITE setHostName) -Q_PROPERTY(int Port READ Port WRITE setPort) -Q_PROPERTY(int UseTCP READ UseTCP WRITE setUseTCP) +class IPconnectionConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT Q_PROPERTY(QString HostName READ HostName WRITE setHostName) + Q_PROPERTY(int Port READ Port WRITE setPort) + Q_PROPERTY(int UseTCP READ UseTCP WRITE setUseTCP) public: - explicit IPconnectionConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit IPconnectionConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); virtual ~IPconnectionConfiguration(); - void saveConfig(QSettings* settings) const; - //void savesettings(QSettings* settings) const; - //void restoresettings(QSettings* settings); + void saveConfig(QSettings *settings) const; + // void savesettings(QSettings* settings) const; + // void restoresettings(QSettings* settings); void savesettings() const; void restoresettings(); - IUAVGadgetConfiguration *clone(); + IUAVGadgetConfiguration *clone(); - QString HostName() const { return m_HostName; } - int Port() const { return m_Port; } - int UseTCP() const { return m_UseTCP; } + QString HostName() const + { + return m_HostName; + } + int Port() const + { + return m_Port; + } + int UseTCP() const + { + return m_UseTCP; + } public slots: - void setHostName(QString HostName) { m_HostName = HostName; } - void setPort(int Port) { m_Port = Port; } - void setUseTCP(int UseTCP) { m_UseTCP = UseTCP; } + void setHostName(QString HostName) + { + m_HostName = HostName; + } + void setPort(int Port) + { + m_Port = Port; + } + void setUseTCP(int UseTCP) + { + m_UseTCP = UseTCP; + } private: QString m_HostName; int m_Port; int m_UseTCP; - QSettings* settings; - - + QSettings *settings; }; #endif // IPconnectionCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.cpp b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.cpp index a3dc4b5bc..af274a67c 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.cpp @@ -41,23 +41,19 @@ IPconnectionOptionsPage::IPconnectionOptionsPage(IPconnectionConfiguration *config, QObject *parent) : IOptionsPage(parent), m_config(config) -{ - -} +{} IPconnectionOptionsPage::~IPconnectionOptionsPage() -{ -} +{} QWidget *IPconnectionOptionsPage::createPage(QWidget *parent) { - m_page = new Ui::IPconnectionOptionsPage(); QWidget *w = new QWidget(parent); m_page->setupUi(w); m_page->Port->setValue(m_config->Port()); m_page->HostName->setText(m_config->HostName()); - m_page->UseTCP->setChecked(m_config->UseTCP()?true:false); - m_page->UseUDP->setChecked(m_config->UseTCP()?false:true); + m_page->UseTCP->setChecked(m_config->UseTCP() ? true : false); + m_page->UseUDP->setChecked(m_config->UseTCP() ? false : true); return w; } @@ -66,17 +62,13 @@ void IPconnectionOptionsPage::apply() { m_config->setPort(m_page->Port->value()); m_config->setHostName(m_page->HostName->text()); - m_config->setUseTCP(m_page->UseTCP->isChecked()?1:0); + m_config->setUseTCP(m_page->UseTCP->isChecked() ? 1 : 0); m_config->savesettings(); emit availableDevChanged(); - - - } void IPconnectionOptionsPage::finish() { delete m_page; } - diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.h b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.h index e750159db..d0110339c 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.h +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.h @@ -33,26 +33,37 @@ class IPconnectionConfiguration; namespace Core { - class IUAVGadgetConfiguration; +class IUAVGadgetConfiguration; } namespace Ui { - class IPconnectionOptionsPage; +class IPconnectionOptionsPage; } using namespace Core; -class IPconnectionOptionsPage : public IOptionsPage -{ -Q_OBJECT +class IPconnectionOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit IPconnectionOptionsPage(IPconnectionConfiguration *config, QObject *parent = 0); virtual ~IPconnectionOptionsPage(); - QString id() const { return QLatin1String("settings"); } - QString trName() const { return tr("settings"); } - QString category() const { return "IP Network Telemetry"; }; - QString trCategory() const { return "IP Network Telemetry"; }; + QString id() const + { + return QLatin1String("settings"); + } + QString trName() const + { + return tr("settings"); + } + QString category() const + { + return "IP Network Telemetry"; + }; + QString trCategory() const + { + return "IP Network Telemetry"; + }; QWidget *createPage(QWidget *parent); void apply(); @@ -65,7 +76,6 @@ public slots: private: IPconnectionConfiguration *m_config; Ui::IPconnectionOptionsPage *m_page; - }; #endif // IPconnectionOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp index c4d95ec4b..7ee944071 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp @@ -25,7 +25,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -//The core of this plugin has been directly copied from the serial plugin and converted to work over a TCP link instead of a direct serial link +// The core of this plugin has been directly copied from the serial plugin and converted to work over a TCP link instead of a direct serial link #include "ipconnectionplugin.h" @@ -46,11 +46,11 @@ #include -//Communication between IPconnectionConnection::OpenDevice() and IPConnection::onOpenDevice() +// Communication between IPconnectionConnection::OpenDevice() and IPConnection::onOpenDevice() QString errorMsg; QWaitCondition openDeviceWait; QWaitCondition closeDeviceWait; -//QReadWriteLock dummyLock; +// QReadWriteLock dummyLock; QMutex ipConMutex; QAbstractSocket *ret; @@ -58,16 +58,16 @@ IPConnection::IPConnection(IPconnectionConnection *connection) : QObject() { moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread()); - QObject::connect(connection, SIGNAL(CreateSocket(QString,int,bool)), - this, SLOT(onOpenDevice(QString,int,bool))); - QObject::connect(connection, SIGNAL(CloseSocket(QAbstractSocket*)), - this, SLOT(onCloseDevice(QAbstractSocket*))); + QObject::connect(connection, SIGNAL(CreateSocket(QString, int, bool)), + this, SLOT(onOpenDevice(QString, int, bool))); + QObject::connect(connection, SIGNAL(CloseSocket(QAbstractSocket *)), + this, SLOT(onCloseDevice(QAbstractSocket *))); } /*IPConnection::~IPConnection() -{ + { -}*/ + }*/ void IPConnection::onOpenDevice(QString HostName, int Port, bool UseTCP) { @@ -81,27 +81,25 @@ void IPConnection::onOpenDevice(QString HostName, int Port, bool UseTCP) ipSocket = new QUdpSocket(this); } - //do sanity check on hostname and port... - if((HostName.length()==0)||(Port<1)){ + // do sanity check on hostname and port... + if ((HostName.length() == 0) || (Port < 1)) { errorMsg = "Please configure Host and Port options before opening the connection"; - - } - else { - //try to connect... + } else { + // try to connect... ipSocket->connectToHost(HostName, Port); - //in blocking mode so we wait for the connection to succeed + // in blocking mode so we wait for the connection to succeed if (ipSocket->waitForConnected(Timeout)) { ret = ipSocket; openDeviceWait.wakeAll(); ipConMutex.unlock(); return; } - //tell user something went wrong - errorMsg = ipSocket->errorString (); + // tell user something went wrong + errorMsg = ipSocket->errorString(); } /* BUGBUG TODO - returning null here leads to segfault because some caller still calls disconnect without checking our return value properly - * someone needs to debug this, I got lost in the calling chain.*/ + * someone needs to debug this, I got lost in the calling chain.*/ ret = NULL; openDeviceWait.wakeAll(); ipConMutex.unlock(); @@ -110,28 +108,29 @@ void IPConnection::onOpenDevice(QString HostName, int Port, bool UseTCP) void IPConnection::onCloseDevice(QAbstractSocket *ipSocket) { ipConMutex.lock(); - ipSocket->close (); - delete(ipSocket); + ipSocket->close(); + delete (ipSocket); closeDeviceWait.wakeAll(); ipConMutex.unlock(); } -IPConnection * connection = 0; +IPConnection *connection = 0; IPconnectionConnection::IPconnectionConnection() { ipSocket = NULL; - //create all our objects + // create all our objects m_config = new IPconnectionConfiguration("IP Network Telemetry", NULL, this); m_config->restoresettings(); - m_optionspage = new IPconnectionOptionsPage(m_config,this); + m_optionspage = new IPconnectionOptionsPage(m_config, this); - if(!connection) + if (!connection) { connection = new IPConnection(this); + } - //just signal whenever we have a device event... + // just signal whenever we have a device event... QMainWindow *mw = Core::ICore::instance()->mainWindow(); QObject::connect(mw, SIGNAL(deviceChange()), this, SLOT(onEnumerationChanged())); @@ -140,35 +139,34 @@ IPconnectionConnection::IPconnectionConnection() } IPconnectionConnection::~IPconnectionConnection() -{//clean up out resources... - if (ipSocket){ - ipSocket->close (); - delete(ipSocket); +{ // clean up out resources... + if (ipSocket) { + ipSocket->close(); + delete (ipSocket); } - if(connection) - { + if (connection) { delete connection; connection = NULL; } } void IPconnectionConnection::onEnumerationChanged() -{//no change from serial plugin +{ // no change from serial plugin emit availableDevChanged(this); } - QList IPconnectionConnection::availableDevices() { QList list; device d; - if (m_config->HostName().length()>1) - d.displayName=(const QString )m_config->HostName(); - else - d.displayName="Unconfigured"; - d.name=(const QString )m_config->HostName(); - //we only have one "device" as defined by the configuration m_config + if (m_config->HostName().length() > 1) { + d.displayName = (const QString)m_config->HostName(); + } else { + d.displayName = "Unconfigured"; + } + d.name = (const QString)m_config->HostName(); + // we only have one "device" as defined by the configuration m_config list.append(d); return list; @@ -181,13 +179,13 @@ QIODevice *IPconnectionConnection::openDevice(const QString &) bool UseTCP; QMessageBox msgBox; - //get the configuration info + // get the configuration info HostName = m_config->HostName(); - Port = m_config->Port(); - UseTCP = m_config->UseTCP(); + Port = m_config->Port(); + UseTCP = m_config->UseTCP(); - if (ipSocket){ - //Andrew: close any existing socket... this should never occur + if (ipSocket) { + // Andrew: close any existing socket... this should never occur ipConMutex.lock(); emit CloseSocket(ipSocket); closeDeviceWait.wait(&ipConMutex); @@ -200,9 +198,8 @@ QIODevice *IPconnectionConnection::openDevice(const QString &) openDeviceWait.wait(&ipConMutex); ipConMutex.unlock(); ipSocket = ret; - if(ipSocket == NULL) - { - msgBox.setText((const QString )errorMsg); + if (ipSocket == NULL) { + msgBox.setText((const QString)errorMsg); msgBox.exec(); } return ipSocket; @@ -210,7 +207,7 @@ QIODevice *IPconnectionConnection::openDevice(const QString &) void IPconnectionConnection::closeDevice(const QString &) { - if (ipSocket){ + if (ipSocket) { ipConMutex.lock(); emit CloseSocket(ipSocket); closeDeviceWait.wait(&ipConMutex); @@ -221,12 +218,12 @@ void IPconnectionConnection::closeDevice(const QString &) QString IPconnectionConnection::connectionName() -{//updated from serial plugin +{ // updated from serial plugin return QString("Network telemetry port"); } QString IPconnectionConnection::shortName() -{//updated from serial plugin +{ // updated from serial plugin if (m_config->UseTCP()) { return QString("TCP"); } else { @@ -236,11 +233,11 @@ QString IPconnectionConnection::shortName() IPconnectionPlugin::IPconnectionPlugin() -{//no change from serial plugin +{ // no change from serial plugin } IPconnectionPlugin::~IPconnectionPlugin() -{//manually remove the options page object +{ // manually remove the options page object removeObject(m_connection->Optionspage()); } @@ -254,9 +251,9 @@ bool IPconnectionPlugin::initialize(const QStringList &arguments, QString *error Q_UNUSED(arguments); Q_UNUSED(errorString); m_connection = new IPconnectionConnection(); - //must manage this registration of child object ourselves - //if we use an autorelease here it causes the GCS to crash - //as it is deleting objects as the app closes... + // must manage this registration of child object ourselves + // if we use an autorelease here it causes the GCS to crash + // as it is deleting objects as the app closes... addObject(m_connection->Optionspage()); return true; diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.h b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.h index 75500555d..7a1bea5bb 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.h +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.h @@ -33,7 +33,7 @@ #include "ipconnectionconfiguration.h" #include "coreplugin/iconnection.h" #include -//#include +// #include class QAbstractSocket; @@ -42,13 +42,12 @@ class QUdpSocket; class IConnection; /** -* Define a connection via the IConnection interface -* Plugin will add a instance of this class to the pool, -* so the connection manager can use it. -*/ + * Define a connection via the IConnection interface + * Plugin will add a instance of this class to the pool, + * so the connection manager can use it. + */ class IPconnection_EXPORT IPconnectionConnection - : public Core::IConnection -{ + : public Core::IConnection { Q_OBJECT public: IPconnectionConnection(); @@ -61,30 +60,33 @@ public: virtual QString connectionName(); virtual QString shortName(); - IPconnectionConfiguration * Config() const { return m_config; } - IPconnectionOptionsPage * Optionspage() const { return m_optionspage; } - + IPconnectionConfiguration *Config() const + { + return m_config; + } + IPconnectionOptionsPage *Optionspage() const + { + return m_optionspage; + } protected slots: void onEnumerationChanged(); -signals: //For the benefit of IPConnection +signals: // For the benefit of IPConnection void CreateSocket(QString HostName, int Port, bool UseTCP); void CloseSocket(QAbstractSocket *socket); private: - QAbstractSocket *ipSocket; - IPconnectionConfiguration *m_config; - IPconnectionOptionsPage *m_optionspage; - //QSettings* settings; - + QAbstractSocket *ipSocket; + IPconnectionConfiguration *m_config; + IPconnectionOptionsPage *m_optionspage; + // QSettings* settings; }; class IPconnection_EXPORT IPconnectionPlugin - : public ExtensionSystem::IPlugin -{ + : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -96,7 +98,6 @@ public: private: IPconnectionConnection *m_connection; - }; diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.cpp b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.cpp index be5a3356f..dc2e21c47 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.cpp +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,10 +30,9 @@ #include "lineardialgadgetconfiguration.h" LineardialGadget::LineardialGadget(QString classId, LineardialGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} LineardialGadget::~LineardialGadget() { @@ -41,22 +40,23 @@ LineardialGadget::~LineardialGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void LineardialGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void LineardialGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - LineardialGadgetConfiguration *m = qobject_cast(config); + LineardialGadgetConfiguration *m = qobject_cast(config); + m_widget->setFactor(m->getFactor()); m_widget->setDecimalPlaces(m->getDecimalPlaces()); - m_widget->setRange(m->getMin(),m->getMax()); + m_widget->setRange(m->getMin(), m->getMax()); m_widget->setGreenRange(m->getGreenMin(), m->getGreenMax()); m_widget->setYellowRange(m->getYellowMin(), m->getYellowMax()); m_widget->setRedRange(m->getRedMin(), m->getRedMax()); m_widget->setDialFile(m->getDialFile()); // Triggers widget repaint m_widget->setDialFont(m->getFont()); m_widget->connectInput(m->getSourceDataObject(), m->getSourceObjectField()); - m_widget->enableOpenGL(m->useOpenGL()); + m_widget->enableOpenGL(m->useOpenGL()); } diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.h b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.h index 16d280576..4f1311f42 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.h +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,15 +38,17 @@ class LineardialGadgetWidget; using namespace Core; -class LineardialGadget : public Core::IUAVGadget -{ +class LineardialGadget : public Core::IUAVGadget { Q_OBJECT public: LineardialGadget(QString classId, LineardialGadgetWidget *widget, QWidget *parent = 0); ~LineardialGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: LineardialGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.cpp index e622b29f4..1bfc5a434 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.cpp @@ -32,7 +32,7 @@ * Loads a saved configuration or defaults if non exist. * */ -LineardialGadgetConfiguration::LineardialGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +LineardialGadgetConfiguration::LineardialGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), dialFile("Unknown"), sourceDataObject("Unknown"), @@ -46,28 +46,28 @@ LineardialGadgetConfiguration::LineardialGadgetConfiguration(QString classId, QS greenMin(66), greenMax(100), factor(1.00), - decimalPlaces(0), - useOpenGLFlag(false) + decimalPlaces(0), + useOpenGLFlag(false) { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { QString dFile = qSettings->value("dFile").toString(); - dialFile = Utils::PathUtils().InsertDataPath(dFile); - sourceDataObject = qSettings->value("sourceDataObject").toString(); + dialFile = Utils::PathUtils().InsertDataPath(dFile); + sourceDataObject = qSettings->value("sourceDataObject").toString(); sourceObjectField = qSettings->value("sourceObjectField").toString(); - minValue = qSettings->value("minValue").toDouble(); - maxValue = qSettings->value("maxValue").toDouble(); - redMin = qSettings->value("redMin").toDouble(); - redMax = qSettings->value("redMax").toDouble(); - yellowMin = qSettings->value("yellowMin").toDouble(); - yellowMax = qSettings->value("yellowMax").toDouble(); - greenMin = qSettings->value("greenMin").toDouble(); - greenMax = qSettings->value("greenMax").toDouble(); + minValue = qSettings->value("minValue").toDouble(); + maxValue = qSettings->value("maxValue").toDouble(); + redMin = qSettings->value("redMin").toDouble(); + redMax = qSettings->value("redMax").toDouble(); + yellowMin = qSettings->value("yellowMin").toDouble(); + yellowMax = qSettings->value("yellowMax").toDouble(); + greenMin = qSettings->value("greenMin").toDouble(); + greenMax = qSettings->value("greenMax").toDouble(); font = qSettings->value("font").toString(); - decimalPlaces = qSettings->value("decimalPlaces").toInt(); - factor = qSettings->value("factor").toDouble(); - useOpenGLFlag = qSettings->value("useOpenGLFlag").toBool(); - } + decimalPlaces = qSettings->value("decimalPlaces").toInt(); + factor = qSettings->value("factor").toDouble(); + useOpenGLFlag = qSettings->value("useOpenGLFlag").toBool(); + } } /** @@ -77,21 +77,22 @@ LineardialGadgetConfiguration::LineardialGadgetConfiguration(QString classId, QS IUAVGadgetConfiguration *LineardialGadgetConfiguration::clone() { LineardialGadgetConfiguration *m = new LineardialGadgetConfiguration(this->classId()); - m->dialFile=dialFile; - m->sourceDataObject = sourceDataObject; + + m->dialFile = dialFile; + m->sourceDataObject = sourceDataObject; m->sourceObjectField = sourceObjectField; - m->minValue = minValue; - m->maxValue = maxValue; - m->redMin = redMin; - m->redMax = redMax; - m->yellowMin = yellowMin; - m->yellowMax = yellowMax; - m->greenMin = greenMin; - m->greenMax = greenMax; + m->minValue = minValue; + m->maxValue = maxValue; + m->redMin = redMin; + m->redMax = redMax; + m->yellowMin = yellowMin; + m->yellowMax = yellowMax; + m->greenMin = greenMin; + m->greenMax = greenMax; m->font = font; - m->decimalPlaces = decimalPlaces; - m->factor = factor; - m->useOpenGLFlag = useOpenGLFlag; + m->decimalPlaces = decimalPlaces; + m->factor = factor; + m->useOpenGLFlag = useOpenGLFlag; return m; } @@ -100,21 +101,23 @@ IUAVGadgetConfiguration *LineardialGadgetConfiguration::clone() * Saves a configuration. * */ -void LineardialGadgetConfiguration::saveConfig(QSettings* qSettings) const { - QString dFile = Utils::PathUtils().RemoveDataPath(dialFile); - qSettings->setValue("dFile", dFile); - qSettings->setValue("sourceDataObject", sourceDataObject); - qSettings->setValue("sourceObjectField", sourceObjectField); - qSettings->setValue("minValue", minValue); - qSettings->setValue("maxValue", maxValue); - qSettings->setValue("redMin", redMin); - qSettings->setValue("redMax", redMax); - qSettings->setValue("yellowMin", yellowMin); - qSettings->setValue("yellowMax", yellowMax); - qSettings->setValue("greenMin", greenMin); - qSettings->setValue("greenMax", greenMax); - qSettings->setValue("font", font); - qSettings->setValue("decimalPlaces", decimalPlaces); - qSettings->setValue("factor", factor); - qSettings->setValue("useOpenGLFlag", useOpenGLFlag); +void LineardialGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ + QString dFile = Utils::PathUtils().RemoveDataPath(dialFile); + + qSettings->setValue("dFile", dFile); + qSettings->setValue("sourceDataObject", sourceDataObject); + qSettings->setValue("sourceObjectField", sourceObjectField); + qSettings->setValue("minValue", minValue); + qSettings->setValue("maxValue", maxValue); + qSettings->setValue("redMin", redMin); + qSettings->setValue("redMax", redMax); + qSettings->setValue("yellowMin", yellowMin); + qSettings->setValue("yellowMax", yellowMax); + qSettings->setValue("greenMin", greenMin); + qSettings->setValue("greenMax", greenMax); + qSettings->setValue("font", font); + qSettings->setValue("decimalPlaces", decimalPlaces); + qSettings->setValue("factor", factor); + qSettings->setValue("useOpenGLFlag", useOpenGLFlag); } diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.h index d8991c0c1..3a85146a1 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.h @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,48 +34,125 @@ using namespace Core; /* This is a generic bargraph dial supporting one indicator. - */ -class LineardialGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT + */ +class LineardialGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit LineardialGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit LineardialGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - //set dial configuration functions - void setDialFile(QString filename){dialFile=filename;} - void setRange(double min, double max) { minValue = min; maxValue = max;} - void setGreenRange(double min, double max) { greenMin = min; greenMax = max;} - void setYellowRange(double min, double max) { yellowMin = min; yellowMax = max;} - void setRedRange(double min, double max) { redMin = min; redMax = max;} + // set dial configuration functions + void setDialFile(QString filename) + { + dialFile = filename; + } + void setRange(double min, double max) + { + minValue = min; maxValue = max; + } + void setGreenRange(double min, double max) + { + greenMin = min; greenMax = max; + } + void setYellowRange(double min, double max) + { + yellowMin = min; yellowMax = max; + } + void setRedRange(double min, double max) + { + redMin = min; redMax = max; + } - void setFont(QString text) { font = text; } + void setFont(QString text) + { + font = text; + } - void setFactor(double val) { factor = val; } - void setDecimalPlaces (int val) { decimalPlaces = val; } + void setFactor(double val) + { + factor = val; + } + void setDecimalPlaces(int val) + { + decimalPlaces = val; + } - void setSourceDataObject(QString text) {sourceDataObject = text; } - void setSourceObjField(QString text) { sourceObjectField = text; } + void setSourceDataObject(QString text) + { + sourceDataObject = text; + } + void setSourceObjField(QString text) + { + sourceObjectField = text; + } - void setUseOpenGL(bool flag) { useOpenGLFlag = flag; } + void setUseOpenGL(bool flag) + { + useOpenGLFlag = flag; + } - //get dial configuration functions - QString getDialFile() {return dialFile;} - double getMin() { return minValue;} - double getMax() { return maxValue;} - double getGreenMin(){ return greenMin;} - double getGreenMax(){ return greenMax;} - double getYellowMin(){ return yellowMin;} - double getYellowMax(){ return yellowMax;} - double getRedMin(){ return redMin;} - double getRedMax(){ return redMax;} - QString getSourceDataObject() { return sourceDataObject;} - QString getSourceObjectField() { return sourceObjectField;} - QString getFont() { return font;} - int getDecimalPlaces() { return decimalPlaces; } - double getFactor() { return factor; } - bool useOpenGL() { return useOpenGLFlag; } + // get dial configuration functions + QString getDialFile() + { + return dialFile; + } + double getMin() + { + return minValue; + } + double getMax() + { + return maxValue; + } + double getGreenMin() + { + return greenMin; + } + double getGreenMax() + { + return greenMax; + } + double getYellowMin() + { + return yellowMin; + } + double getYellowMax() + { + return yellowMax; + } + double getRedMin() + { + return redMin; + } + double getRedMax() + { + return redMax; + } + QString getSourceDataObject() + { + return sourceDataObject; + } + QString getSourceObjectField() + { + return sourceObjectField; + } + QString getFont() + { + return font; + } + int getDecimalPlaces() + { + return decimalPlaces; + } + double getFactor() + { + return factor; + } + bool useOpenGL() + { + return useOpenGLFlag; + } - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: @@ -101,7 +178,7 @@ private: double greenMax; double factor; - bool useOpenGLFlag; + bool useOpenGLFlag; int decimalPlaces; }; diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.cpp index 0fc4e2616..4d2e27827 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,29 +32,27 @@ #include LineardialGadgetFactory::LineardialGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("LineardialGadget"), - tr("Bargraph Dial"), - parent) -{ -} + IUAVGadgetFactory(QString("LineardialGadget"), + tr("Bargraph Dial"), + parent) +{} LineardialGadgetFactory::~LineardialGadgetFactory() -{ -} +{} -Core::IUAVGadget* LineardialGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *LineardialGadgetFactory::createGadget(QWidget *parent) { - LineardialGadgetWidget* gadgetWidget = new LineardialGadgetWidget(parent); + LineardialGadgetWidget *gadgetWidget = new LineardialGadgetWidget(parent); + return new LineardialGadget(QString("LineardialGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *LineardialGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *LineardialGadgetFactory::createConfiguration(QSettings *qSettings) { return new LineardialGadgetConfiguration(QString("LineardialGadget"), qSettings); } IOptionsPage *LineardialGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new LineardialGadgetOptionsPage(qobject_cast(config)); + return new LineardialGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.h b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.h index e8040d68d..7eeecb307 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class LineardialGadgetFactory : public IUAVGadgetFactory -{ +class LineardialGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: LineardialGadgetFactory(QObject *parent = 0); ~LineardialGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.cpp index ccd34af51..ae257508a 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,21 +38,20 @@ #include LineardialGadgetOptionsPage::LineardialGadgetOptionsPage(LineardialGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) + IOptionsPage(parent), + m_config(config) { font = QFont("Arial", 12); // Default in case nothing exists yet. } -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *LineardialGadgetOptionsPage::createPage(QWidget *parent) { - Q_UNUSED(parent); options_page = new Ui::LineardialGadgetOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget; - //main layout + // main layout options_page->setupUi(optionsPageWidget); @@ -75,27 +74,27 @@ QWidget *LineardialGadgetOptionsPage::createPage(QWidget *parent) options_page->redMax->setValue(m_config->getRedMax()); options_page->factor->setValue(m_config->getFactor()); options_page->decPlaces->setValue(m_config->getDecimalPlaces()); - font.fromString(m_config->getFont()); - options_page->useOpenGL->setChecked(m_config->useOpenGL()); + font.fromString(m_config->getFont()); + options_page->useOpenGL->setChecked(m_config->useOpenGL()); // Fills the combo boxes for the UAVObjects ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - QList< QList > objList = objManager->getDataObjects(); - foreach (QList list, objList) { - foreach (UAVDataObject* obj, list) { + QList< QList > objList = objManager->getDataObjects(); + foreach(QList list, objList) { + foreach(UAVDataObject * obj, list) { options_page->objectName->addItem(obj->getName()); } } - //select saved UAV Object field values - if(options_page->objectName->findText(m_config->getSourceDataObject())!=-1){ + // select saved UAV Object field values + if (options_page->objectName->findText(m_config->getSourceDataObject()) != -1) { options_page->objectName->setCurrentIndex(options_page->objectName->findText(m_config->getSourceDataObject())); // Now load the object field values: - UAVDataObject* obj = dynamic_cast( objManager->getObject(m_config->getSourceDataObject()) ); - if (obj != NULL ) { - on_objectName_currentIndexChanged(m_config->getSourceDataObject()); - // And set the highlighed value from the settings: - options_page->objectField->setCurrentIndex(options_page->objectField->findText(m_config->getSourceObjectField())); + UAVDataObject *obj = dynamic_cast(objManager->getObject(m_config->getSourceDataObject())); + if (obj != NULL) { + on_objectName_currentIndexChanged(m_config->getSourceDataObject()); + // And set the highlighed value from the settings: + options_page->objectField->setCurrentIndex(options_page->objectField->findText(m_config->getSourceObjectField())); } } @@ -122,7 +121,6 @@ void LineardialGadgetOptionsPage::on_rangeMin_valueChanged(double val) options_page->greenMax->setMinimum(val); options_page->yellowMax->setMinimum(val); options_page->redMax->setMinimum(val); - } /** @@ -155,16 +153,16 @@ void LineardialGadgetOptionsPage::apply() m_config->setDialFile(options_page->svgSourceFile->path()); double rangeMin = options_page->minValue->value(); double rangeMax = options_page->maxValue->value(); - m_config->setRange(rangeMin,rangeMax); - m_config->setGreenRange(options_page->greenMin->value(),options_page->greenMax->value()); - m_config->setYellowRange(options_page->yellowMin->value(),options_page->yellowMax->value()); - m_config->setRedRange(options_page->redMin->value(),options_page->redMax->value()); + m_config->setRange(rangeMin, rangeMax); + m_config->setGreenRange(options_page->greenMin->value(), options_page->greenMax->value()); + m_config->setYellowRange(options_page->yellowMin->value(), options_page->yellowMax->value()); + m_config->setRedRange(options_page->redMin->value(), options_page->redMax->value()); m_config->setSourceDataObject(options_page->objectName->currentText()); m_config->setSourceObjField(options_page->objectField->currentText()); m_config->setFont(font.toString()); m_config->setDecimalPlaces(options_page->decPlaces->value()); m_config->setFactor(options_page->factor->value()); - m_config->setUseOpenGL(options_page->useOpenGL->checkState()); + m_config->setUseOpenGL(options_page->useOpenGL->checkState()); } /** @@ -174,34 +172,32 @@ void LineardialGadgetOptionsPage::apply() void LineardialGadgetOptionsPage::on_fontPicker_clicked() { bool ok; - font = QFontDialog::getFont(&ok, font , qobject_cast(this)); + + font = QFontDialog::getFont(&ok, font, qobject_cast(this)); } /* - Fills in the field1 combo box when value is changed in the - object1 field -*/ -void LineardialGadgetOptionsPage::on_objectName_currentIndexChanged(QString val) { + Fills in the field1 combo box when value is changed in the + object1 field + */ +void LineardialGadgetOptionsPage::on_objectName_currentIndexChanged(QString val) +{ options_page->objectField->clear(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(val) ); - QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { - if(field->getElementNames().count() > 1) - { - foreach(QString elemName , field->getElementNames()) - { + UAVDataObject *obj = dynamic_cast(objManager->getObject(val)); + QList fieldList = obj->getFields(); + foreach(UAVObjectField * field, fieldList) { + if (field->getElementNames().count() > 1) { + foreach(QString elemName, field->getElementNames()) { options_page->objectField->addItem(field->getName() + "-" + elemName); } - } - else + } else { options_page->objectField->addItem(field->getName()); + } } } void LineardialGadgetOptionsPage::finish() -{ - -} +{} diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.h index a9939de2d..de79c2255 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.h @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -41,14 +41,13 @@ class IUAVGadgetConfiguration; class LineardialGadgetConfiguration; namespace Ui { - class LineardialGadgetOptionsPage; +class LineardialGadgetOptionsPage; } using namespace Core; -class LineardialGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class LineardialGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit LineardialGadgetOptionsPage(LineardialGadgetConfiguration *config, QObject *parent = 0); @@ -66,7 +65,6 @@ private slots: void on_objectName_currentIndexChanged(QString val); void on_rangeMin_valueChanged(double val); void on_rangeMax_valueChanged(double val); - }; #endif // LINEARDIALGADGETOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.cpp index a108877a8..d4ee357bb 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.cpp @@ -33,52 +33,53 @@ LineardialGadgetWidget::LineardialGadgetWidget(QWidget *parent) : QGraphicsView(parent) { - setMinimumSize(32,32); + setMinimumSize(32, 32); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setScene(new QGraphicsScene(this)); setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); - m_renderer = new QSvgRenderer(); + m_renderer = new QSvgRenderer(); verticalDial = false; paint(); obj1 = NULL; - fieldName = NULL; - fieldValue = NULL; + fieldName = NULL; + fieldValue = NULL; indexTarget = 0; - indexValue = 0; + indexValue = 0; places = 0; factor = 1; // This timer mechanism makes the index rotate smoothly connect(&dialTimer, SIGNAL(timeout()), this, SLOT(moveIndex())); dialTimer.start(30); - } LineardialGadgetWidget::~LineardialGadgetWidget() { - // Do nothing + // Do nothing } /*! - \brief Enables/Disables OpenGL - */ + \brief Enables/Disables OpenGL + */ void LineardialGadgetWidget::enableOpenGL(bool flag) { - if (flag) - setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); - else - setViewport(new QWidget); + if (flag) { + setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); + } else { + setViewport(new QWidget); + } } /*! - \brief Connects the widget to the relevant UAVObjects - */ -void LineardialGadgetWidget::connectInput(QString object1, QString nfield1) { - - if (obj1 != NULL) - disconnect(obj1,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updateIndex(UAVObject*))); + \brief Connects the widget to the relevant UAVObjects + */ +void LineardialGadgetWidget::connectInput(QString object1, QString nfield1) +{ + if (obj1 != NULL) { + disconnect(obj1, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateIndex(UAVObject *))); + } ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); @@ -86,25 +87,22 @@ void LineardialGadgetWidget::connectInput(QString object1, QString nfield1) { // Check validity of arguments first, reject empty args and unknown fields. if (!(object1.isEmpty() || nfield1.isEmpty())) { - obj1 = dynamic_cast( objManager->getObject(object1) ); - if (obj1 != NULL ) { - connect(obj1, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateIndex(UAVObject*))); - if(nfield1.contains("-")) - { + obj1 = dynamic_cast(objManager->getObject(object1)); + if (obj1 != NULL) { + connect(obj1, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateIndex(UAVObject *))); + if (nfield1.contains("-")) { QStringList fieldSubfield = nfield1.split("-", QString::SkipEmptyParts); - field1 = fieldSubfield.at(0); - subfield1 = fieldSubfield.at(1); + field1 = fieldSubfield.at(0); + subfield1 = fieldSubfield.at(1); haveSubField1 = true; - } - else - { - field1= nfield1; + } else { + field1 = nfield1; haveSubField1 = false; } - if (fieldName) + if (fieldName) { fieldName->setPlainText(nfield1); + } updateIndex(obj1); - } else { qDebug() << "Error: Object is unknown (" << object1 << ") this should not happen."; } @@ -112,24 +110,27 @@ void LineardialGadgetWidget::connectInput(QString object1, QString nfield1) { } /*! - \brief Called by the UAVObject which got updated + \brief Called by the UAVObject which got updated - Updates the numeric value and/or the icon if the dial wants this. - */ -void LineardialGadgetWidget::updateIndex(UAVObject *object1) { + Updates the numeric value and/or the icon if the dial wants this. + */ +void LineardialGadgetWidget::updateIndex(UAVObject *object1) +{ // Double check that the field exists: - UAVObjectField* field = object1->getField(field1); + UAVObjectField *field = object1->getField(field1); + if (field) { QString s; if (field->isNumeric()) { double v; - if(haveSubField1){ + if (haveSubField1) { int indexOfSubField = field->getElementNames().indexOf(QRegExp(subfield1, Qt::CaseSensitive, QRegExp::FixedString)); - v = field->getDouble(indexOfSubField)*factor; - }else - v = field->getDouble()*factor; + v = field->getDouble(indexOfSubField) * factor; + } else { + v = field->getDouble() * factor; + } setIndex(v); - s.sprintf("%.*f",places,v); + s.sprintf("%.*f", places, v); } if (field->isText()) { s = field->getValue().toString(); @@ -144,191 +145,192 @@ void LineardialGadgetWidget::updateIndex(UAVObject *object1) { } } - if (fieldValue) + if (fieldValue) { fieldValue->setPlainText(s); + } - if (index && !dialTimer.isActive()) + if (index && !dialTimer.isActive()) { dialTimer.start(); + } } else { qDebug() << "Wrong field, maybe an issue with object disconnection ?"; } } /*! - \brief Setup dial using its master SVG template. + \brief Setup dial using its master SVG template. - Should only be called after the min/max ranges have been set. + Should only be called after the min/max ranges have been set. - */ + */ void LineardialGadgetWidget::setDialFile(QString dfn) { QGraphicsScene *l_scene = scene(); + setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); - if (QFile::exists(dfn) && m_renderer->load(dfn) && m_renderer->isValid() ) - { - l_scene->clear(); // Beware: clear also deletes all objects - // which are currently in the scene - background = new QGraphicsSvgItem(); - background->setSharedRenderer(m_renderer); - background->setElementId("background"); - background->setFlags(QGraphicsItem::ItemClipsChildrenToShape| - QGraphicsItem::ItemClipsToShape); - l_scene->addItem(background); + if (QFile::exists(dfn) && m_renderer->load(dfn) && m_renderer->isValid()) { + l_scene->clear(); // Beware: clear also deletes all objects + // which are currently in the scene + background = new QGraphicsSvgItem(); + background->setSharedRenderer(m_renderer); + background->setElementId("background"); + background->setFlags(QGraphicsItem::ItemClipsChildrenToShape | + QGraphicsItem::ItemClipsToShape); + l_scene->addItem(background); - // The red/yellow/green zones are optional, we just - // test on the presence of "red" - if (m_renderer->elementExists("red")) { - // Order is important: red, then yellow then green - // overlayed on top of each other - red = new QGraphicsSvgItem(); - red->setSharedRenderer(m_renderer); - red->setElementId("red"); - red->setParentItem(background); - yellow = new QGraphicsSvgItem(); - yellow->setSharedRenderer(m_renderer); - yellow->setElementId("yellow"); - yellow->setParentItem(background); - green = new QGraphicsSvgItem(); - green->setSharedRenderer(m_renderer); - green->setElementId("green"); - green->setParentItem(background); - // In order to properly render the Green/Yellow/Red graphs, we need to find out - // the starting location of the bargraph rendering area: - QMatrix textMatrix = m_renderer->matrixForElement("bargraph"); - qreal bgX = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).x(); - qreal bgY = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).y(); - bargraphSize = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).width(); - // Detect if the bargraph is vertical or horizontal. - qreal bargraphHeight = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).height(); - if (bargraphHeight > bargraphSize) { - verticalDial = true; - bargraphSize = bargraphHeight; - } else { - verticalDial = false; - } - // Now adjust the red/yellow/green zones: - double range = maxValue-minValue; + // The red/yellow/green zones are optional, we just + // test on the presence of "red" + if (m_renderer->elementExists("red")) { + // Order is important: red, then yellow then green + // overlayed on top of each other + red = new QGraphicsSvgItem(); + red->setSharedRenderer(m_renderer); + red->setElementId("red"); + red->setParentItem(background); + yellow = new QGraphicsSvgItem(); + yellow->setSharedRenderer(m_renderer); + yellow->setElementId("yellow"); + yellow->setParentItem(background); + green = new QGraphicsSvgItem(); + green->setSharedRenderer(m_renderer); + green->setElementId("green"); + green->setParentItem(background); + // In order to properly render the Green/Yellow/Red graphs, we need to find out + // the starting location of the bargraph rendering area: + QMatrix textMatrix = m_renderer->matrixForElement("bargraph"); + qreal bgX = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).x(); + qreal bgY = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).y(); + bargraphSize = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).width(); + // Detect if the bargraph is vertical or horizontal. + qreal bargraphHeight = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).height(); + if (bargraphHeight > bargraphSize) { + verticalDial = true; + bargraphSize = bargraphHeight; + } else { + verticalDial = false; + } + // Now adjust the red/yellow/green zones: + double range = maxValue - minValue; - green->resetTransform(); - double greenScale = (greenMax-greenMin)/range; - double greenStart = verticalDial ? (maxValue-greenMax)/range*green->boundingRect().height() : - (greenMin-minValue)/range*green->boundingRect().width(); - QTransform matrix; - matrix.reset(); - if (verticalDial) { - matrix.scale(1,greenScale); - matrix.translate(bgX,(greenStart+bgY)/greenScale); - } else { - matrix.scale(greenScale,1); - matrix.translate((greenStart+bgX)/greenScale,bgY); - } - green->setTransform(matrix,false); + green->resetTransform(); + double greenScale = (greenMax - greenMin) / range; + double greenStart = verticalDial ? (maxValue - greenMax) / range * green->boundingRect().height() : + (greenMin - minValue) / range * green->boundingRect().width(); + QTransform matrix; + matrix.reset(); + if (verticalDial) { + matrix.scale(1, greenScale); + matrix.translate(bgX, (greenStart + bgY) / greenScale); + } else { + matrix.scale(greenScale, 1); + matrix.translate((greenStart + bgX) / greenScale, bgY); + } + green->setTransform(matrix, false); - yellow->resetTransform(); - double yellowScale = (yellowMax-yellowMin)/range; - double yellowStart = verticalDial ? (maxValue-yellowMax)/range*yellow->boundingRect().height() : - (yellowMin-minValue)/range*yellow->boundingRect().width(); - matrix.reset(); - if (verticalDial) { - matrix.scale(1,yellowScale); - matrix.translate(bgX,(yellowStart+bgY)/yellowScale); - } else { - matrix.scale(yellowScale,1); - matrix.translate((yellowStart+bgX)/yellowScale,bgY); - } - yellow->setTransform(matrix,false); + yellow->resetTransform(); + double yellowScale = (yellowMax - yellowMin) / range; + double yellowStart = verticalDial ? (maxValue - yellowMax) / range * yellow->boundingRect().height() : + (yellowMin - minValue) / range * yellow->boundingRect().width(); + matrix.reset(); + if (verticalDial) { + matrix.scale(1, yellowScale); + matrix.translate(bgX, (yellowStart + bgY) / yellowScale); + } else { + matrix.scale(yellowScale, 1); + matrix.translate((yellowStart + bgX) / yellowScale, bgY); + } + yellow->setTransform(matrix, false); - red->resetTransform(); - double redScale = (redMax-redMin)/range; - double redStart = verticalDial ? (maxValue-redMax)/range*red->boundingRect().height() : - (redMin-minValue)/range*red->boundingRect().width(); - matrix.reset(); - if (verticalDial) { - matrix.scale(1,redScale); - matrix.translate(bgX,(redStart+bgY)/redScale); - } else { - matrix.scale(redScale,1); - matrix.translate((redStart+bgX)/redScale,bgY); - } - red->setTransform(matrix,false); - - } else { - red = NULL; + red->resetTransform(); + double redScale = (redMax - redMin) / range; + double redStart = verticalDial ? (maxValue - redMax) / range * red->boundingRect().height() : + (redMin - minValue) / range * red->boundingRect().width(); + matrix.reset(); + if (verticalDial) { + matrix.scale(1, redScale); + matrix.translate(bgX, (redStart + bgY) / redScale); + } else { + matrix.scale(redScale, 1); + matrix.translate((redStart + bgX) / redScale, bgY); + } + red->setTransform(matrix, false); + } else { + red = NULL; yellow = NULL; - green = NULL; - } + green = NULL; + } - // Check whether the dial wants to display a moving index: - if (m_renderer->elementExists("needle")) { - QMatrix textMatrix = m_renderer->matrixForElement("needle"); - QRectF nRect = textMatrix.mapRect(m_renderer->boundsOnElement("needle")); - startX = nRect.x(); - startY = nRect.y(); - QTransform matrix; - matrix.translate(startX,startY); - index = new QGraphicsSvgItem(); - index->setSharedRenderer(m_renderer); - index->setElementId("needle"); - index->setTransform(matrix,false); - index->setParentItem(background); - } else { - index = NULL; - } + // Check whether the dial wants to display a moving index: + if (m_renderer->elementExists("needle")) { + QMatrix textMatrix = m_renderer->matrixForElement("needle"); + QRectF nRect = textMatrix.mapRect(m_renderer->boundsOnElement("needle")); + startX = nRect.x(); + startY = nRect.y(); + QTransform matrix; + matrix.translate(startX, startY); + index = new QGraphicsSvgItem(); + index->setSharedRenderer(m_renderer); + index->setElementId("needle"); + index->setTransform(matrix, false); + index->setParentItem(background); + } else { + index = NULL; + } - // Check whether the dial wants display its field name: - if (m_renderer->elementExists("field")) { - QMatrix textMatrix = m_renderer->matrixForElement("field"); - QRectF rect = textMatrix.mapRect(m_renderer->boundsOnElement("field")); - qreal startX = rect.x(); - qreal startY = rect.y(); - qreal elHeight = rect.height(); - QTransform matrix; - matrix.translate(startX,startY-elHeight/2); - fieldName = new QGraphicsTextItem("field"); - fieldName->setFont(QFont("Arial",(int)elHeight)); - fieldName->setDefaultTextColor(QColor("White")); - fieldName->setTransform(matrix,false); - fieldName->setParentItem(background); - } else { - fieldName = NULL; - } + // Check whether the dial wants display its field name: + if (m_renderer->elementExists("field")) { + QMatrix textMatrix = m_renderer->matrixForElement("field"); + QRectF rect = textMatrix.mapRect(m_renderer->boundsOnElement("field")); + qreal startX = rect.x(); + qreal startY = rect.y(); + qreal elHeight = rect.height(); + QTransform matrix; + matrix.translate(startX, startY - elHeight / 2); + fieldName = new QGraphicsTextItem("field"); + fieldName->setFont(QFont("Arial", (int)elHeight)); + fieldName->setDefaultTextColor(QColor("White")); + fieldName->setTransform(matrix, false); + fieldName->setParentItem(background); + } else { + fieldName = NULL; + } - // Check whether the dial wants display the numeric value: - if (m_renderer->elementExists("value")) { - QMatrix textMatrix = m_renderer->matrixForElement("value"); - QRectF nRect = textMatrix.mapRect(m_renderer->boundsOnElement("value")); - qreal startX = nRect.x(); - qreal startY = nRect.y(); - qreal elHeight = nRect.height(); - QTransform matrix; - matrix.translate(startX,startY-elHeight/2); - fieldValue = new QGraphicsTextItem("0.00"); - fieldValue->setFont(QFont("Arial",(int)elHeight)); - fieldValue->setDefaultTextColor(QColor("White")); - fieldValue->setTransform(matrix,false); - fieldValue->setParentItem(background); - } else { - fieldValue = NULL; - } + // Check whether the dial wants display the numeric value: + if (m_renderer->elementExists("value")) { + QMatrix textMatrix = m_renderer->matrixForElement("value"); + QRectF nRect = textMatrix.mapRect(m_renderer->boundsOnElement("value")); + qreal startX = nRect.x(); + qreal startY = nRect.y(); + qreal elHeight = nRect.height(); + QTransform matrix; + matrix.translate(startX, startY - elHeight / 2); + fieldValue = new QGraphicsTextItem("0.00"); + fieldValue->setFont(QFont("Arial", (int)elHeight)); + fieldValue->setDefaultTextColor(QColor("White")); + fieldValue->setTransform(matrix, false); + fieldValue->setParentItem(background); + } else { + fieldValue = NULL; + } - // Check whether the dial wants to display the value as a - // symbol (only works for text values): - if (m_renderer->elementExists("symbol")) { - QMatrix textMatrix = m_renderer->matrixForElement("symbol"); - qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement("symbol")).x(); - qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement("symbol")).y(); - QTransform matrix; - matrix.translate(startX,startY); - fieldSymbol = new QGraphicsSvgItem(); - fieldSymbol->setElementId("symbol"); - fieldSymbol->setSharedRenderer(m_renderer); - fieldSymbol->setTransform(matrix,false); - fieldSymbol->setParentItem(background); - } else { - fieldSymbol = NULL; - } + // Check whether the dial wants to display the value as a + // symbol (only works for text values): + if (m_renderer->elementExists("symbol")) { + QMatrix textMatrix = m_renderer->matrixForElement("symbol"); + qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement("symbol")).x(); + qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement("symbol")).y(); + QTransform matrix; + matrix.translate(startX, startY); + fieldSymbol = new QGraphicsSvgItem(); + fieldSymbol->setElementId("symbol"); + fieldSymbol->setSharedRenderer(m_renderer); + fieldSymbol->setTransform(matrix, false); + fieldSymbol->setParentItem(background); + } else { + fieldSymbol = NULL; + } - if (m_renderer->elementExists("foreground")) { + if (m_renderer->elementExists("foreground")) { foreground = new QGraphicsSvgItem(); foreground->setSharedRenderer(m_renderer); foreground->setElementId("foreground"); @@ -338,26 +340,25 @@ void LineardialGadgetWidget::setDialFile(QString dfn) fgenabled = false; } - l_scene->setSceneRect(background->boundingRect()); + l_scene->setSceneRect(background->boundingRect()); - // Reset the current index value: - indexValue = 0; - if (!dialTimer.isActive() && index) - dialTimer.start(); - } - else - { - qDebug() << "no file "; - m_renderer->load(QString(":/lineardial/images/empty.svg")); - l_scene->clear(); // This also deletes all items contained in the scene. - background = new QGraphicsSvgItem(); - background->setSharedRenderer(m_renderer); - l_scene->addItem(background); - fieldName = NULL; - fieldValue = NULL; - fieldSymbol = NULL; - index = NULL; - } + // Reset the current index value: + indexValue = 0; + if (!dialTimer.isActive() && index) { + dialTimer.start(); + } + } else { + qDebug() << "no file "; + m_renderer->load(QString(":/lineardial/images/empty.svg")); + l_scene->clear(); // This also deletes all items contained in the scene. + background = new QGraphicsSvgItem(); + background->setSharedRenderer(m_renderer); + l_scene->addItem(background); + fieldName = NULL; + fieldValue = NULL; + fieldSymbol = NULL; + index = NULL; + } } @@ -365,7 +366,8 @@ void LineardialGadgetWidget::setDialFont(QString fontProps) { // Note: a bit of juggling to preserve the automatic // font size which was calculated upon dial initialization. - QFont font = QFont("Arial",12); + QFont font = QFont("Arial", 12); + font.fromString(fontProps); if (fieldName) { int fieldSize = fieldName->font().pointSize(); @@ -373,10 +375,10 @@ void LineardialGadgetWidget::setDialFont(QString fontProps) fieldName->setFont(font); } if (fieldValue) { - int fieldSize = fieldValue->font().pointSize(); - font.setPointSize(fieldSize); - fieldValue->setFont(font); - } + int fieldSize = fieldValue->font().pointSize(); + font.setPointSize(fieldSize); + fieldValue->setFont(font); + } } @@ -388,11 +390,11 @@ void LineardialGadgetWidget::paint() void LineardialGadgetWidget::paintEvent(QPaintEvent *event) { // Skip painting until the dial file is loaded - if (! m_renderer->isValid()) { - qDebug()<<"Dial file not loaded, not rendering"; + if (!m_renderer->isValid()) { + qDebug() << "Dial file not loaded, not rendering"; return; } - QGraphicsView::paintEvent(event); + QGraphicsView::paintEvent(event); } // This event enables the dial to be dynamically resized @@ -401,16 +403,17 @@ void LineardialGadgetWidget::paintEvent(QPaintEvent *event) void LineardialGadgetWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - fitInView(background, Qt::KeepAspectRatio ); + fitInView(background, Qt::KeepAspectRatio); } // Converts the value into an percentage: // this enables smooth movement in moveIndex below -void LineardialGadgetWidget::setIndex(double value) { +void LineardialGadgetWidget::setIndex(double value) +{ if (verticalDial) { - indexTarget = 100*(maxValue-value)/(maxValue-minValue); + indexTarget = 100 * (maxValue - value) / (maxValue - minValue); } else { - indexTarget = 100*(value-minValue)/(maxValue-minValue); + indexTarget = 100 * (value - minValue) / (maxValue - minValue); } } @@ -423,21 +426,21 @@ void LineardialGadgetWidget::moveIndex() dialTimer.stop(); return; } - if ((abs((indexValue-indexTarget)*10) > 3)) { - indexValue += (indexTarget - indexValue)/5; + if ((abs((indexValue - indexTarget) * 10) > 3)) { + indexValue += (indexTarget - indexValue) / 5; } else { indexValue = indexTarget; dialTimer.stop(); } QTransform matrix; index->resetTransform(); - qreal trans = indexValue*bargraphSize/100; + qreal trans = indexValue * bargraphSize / 100; if (verticalDial) { - matrix.translate(startX,trans+startY); + matrix.translate(startX, trans + startY); } else { - matrix.translate(trans+startX,startY); + matrix.translate(trans + startX, startY); } - index->setTransform(matrix,false); - + index->setTransform(matrix, false); + update(); } diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.h b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.h index 9e8030e0d..12f476208 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -39,86 +39,102 @@ #include #include -class LineardialGadgetWidget : public QGraphicsView -{ +class LineardialGadgetWidget : public QGraphicsView { Q_OBJECT public: LineardialGadgetWidget(QWidget *parent = 0); - ~LineardialGadgetWidget(); - void enableOpenGL(bool flag); - void setDialFile(QString dfn); - void paint(); - void setRange(double min, double max) { minValue=min; maxValue=max;} - void setGreenRange(double min, double max) {greenMin=min; greenMax=max;} - void setYellowRange(double min, double max) {yellowMin=min; yellowMax=max;} - void setRedRange(double min, double max) {redMin=min; redMax=max;} - void connectInput(QString obj, QString field); - void setIndex(double val); - void setDialFont(QString fontProps); - void setFactor (double val) { factor = val;} - void setDecimalPlaces(int val) { places = val;} + ~LineardialGadgetWidget(); + void enableOpenGL(bool flag); + void setDialFile(QString dfn); + void paint(); + void setRange(double min, double max) + { + minValue = min; maxValue = max; + } + void setGreenRange(double min, double max) + { + greenMin = min; greenMax = max; + } + void setYellowRange(double min, double max) + { + yellowMin = min; yellowMax = max; + } + void setRedRange(double min, double max) + { + redMin = min; redMax = max; + } + void connectInput(QString obj, QString field); + void setIndex(double val); + void setDialFont(QString fontProps); + void setFactor(double val) + { + factor = val; + } + void setDecimalPlaces(int val) + { + places = val; + } public slots: void updateIndex(UAVObject *object1); protected: - void paintEvent(QPaintEvent *event); - void resizeEvent(QResizeEvent *event); + void paintEvent(QPaintEvent *event); + void resizeEvent(QResizeEvent *event); private: private slots: - void moveIndex(); + void moveIndex(); private: - QSvgRenderer *m_renderer; - QGraphicsSvgItem *background; - QGraphicsSvgItem *foreground; - QGraphicsSvgItem *index; - QGraphicsSvgItem *green; - QGraphicsSvgItem *yellow; - QGraphicsSvgItem *red; - QGraphicsSvgItem *fieldSymbol; + QSvgRenderer *m_renderer; + QGraphicsSvgItem *background; + QGraphicsSvgItem *foreground; + QGraphicsSvgItem *index; + QGraphicsSvgItem *green; + QGraphicsSvgItem *yellow; + QGraphicsSvgItem *red; + QGraphicsSvgItem *fieldSymbol; - QGraphicsTextItem *fieldName; - QGraphicsTextItem *fieldValue; + QGraphicsTextItem *fieldName; + QGraphicsTextItem *fieldValue; - // Simple flag to skip rendering if the - bool fgenabled; // layer does not exist. - bool verticalDial; // True if the dials scales vertically. + // Simple flag to skip rendering if the + bool fgenabled; // layer does not exist. + bool verticalDial; // True if the dials scales vertically. - qreal startX; // Where we should draw the bargraph - qreal startY; // green/yellow/red zones. - qreal bargraphSize; - qreal indexHeight; - qreal indexWidth; + qreal startX; // Where we should draw the bargraph + qreal startY; // green/yellow/red zones. + qreal bargraphSize; + qreal indexHeight; + qreal indexWidth; - double minValue; - double maxValue; - double greenMin; - double greenMax; - double yellowMin; - double yellowMax; - double redMin; - double redMax; - double factor; - int places; + double minValue; + double maxValue; + double greenMin; + double greenMax; + double yellowMin; + double yellowMax; + double redMin; + double redMax; + double factor; + int places; - // The Value and target variables - // are expressed in degrees - double indexTarget; - double indexValue; + // The Value and target variables + // are expressed in degrees + double indexTarget; + double indexValue; - // Rotation timer - QTimer dialTimer; - - // Name of the fields to read when an update is received: - UAVDataObject* obj1; - QString field1; - QString subfield1; - bool haveSubField1; + // Rotation timer + QTimer dialTimer; + // Name of the fields to read when an update is received: + UAVDataObject *obj1; + QString field1; + QString subfield1; + bool haveSubField1; }; #endif /* LINEARDIALGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.cpp b/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.cpp index 8441d9b99..451be06fc 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.cpp +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,32 +35,31 @@ LineardialPlugin::LineardialPlugin() { - // Do nothing + // Do nothing } LineardialPlugin::~LineardialPlugin() { - // Do nothing + // Do nothing } -bool LineardialPlugin::initialize(const QStringList& args, QString *errMsg) +bool LineardialPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new LineardialGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new LineardialGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void LineardialPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void LineardialPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(LineardialPlugin) - diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.h b/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.h index 6ababda8e..820444c48 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.h +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class LineardialGadgetFactory; -class LineardialPlugin : public ExtensionSystem::IPlugin -{ +class LineardialPlugin : public ExtensionSystem::IPlugin { public: - LineardialPlugin(); - ~LineardialPlugin(); + LineardialPlugin(); + ~LineardialPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - LineardialGadgetFactory *mf; + LineardialGadgetFactory *mf; }; #endif /* LINEARDIALPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/logging/logfile.cpp b/ground/openpilotgcs/src/plugins/logging/logfile.cpp index 7a74d6367..ce0dd4146 100644 --- a/ground/openpilotgcs/src/plugins/logging/logfile.cpp +++ b/ground/openpilotgcs/src/plugins/logging/logfile.cpp @@ -13,8 +13,8 @@ LogFile::LogFile(QObject *parent) : * we want to save the logfile, we open in WriteOnly. In case we * want to read the logfile, we open in ReadOnly. */ -bool LogFile::open(OpenMode mode) { - +bool LogFile::open(OpenMode mode) +{ // start a timer for playback myTime.restart(); if (file.isOpen()) { @@ -25,8 +25,7 @@ bool LogFile::open(OpenMode mode) { return true; } - if(file.open(mode) == FALSE) - { + if (file.open(mode) == FALSE) { qDebug() << "Unable to open " << file.fileName() << " for logging"; return false; } @@ -46,33 +45,39 @@ void LogFile::close() { emit aboutToClose(); - if (timer.isActive()) + if (timer.isActive()) { timer.stop(); + } file.close(); QIODevice::close(); } -qint64 LogFile::writeData(const char * data, qint64 dataSize) { - if (!file.isWritable()) +qint64 LogFile::writeData(const char *data, qint64 dataSize) +{ + if (!file.isWritable()) { return dataSize; + } quint32 timeStamp = myTime.elapsed(); - file.write((char *) &timeStamp,sizeof(timeStamp)); - file.write((char *) &dataSize, sizeof(dataSize)); + file.write((char *)&timeStamp, sizeof(timeStamp)); + file.write((char *)&dataSize, sizeof(dataSize)); qint64 written = file.write(data, dataSize); - if(written != -1) + if (written != -1) { emit bytesWritten(written); + } return dataSize; } -qint64 LogFile::readData(char * data, qint64 maxSize) { +qint64 LogFile::readData(char *data, qint64 maxSize) +{ QMutexLocker locker(&mutex); - qint64 toRead = qMin(maxSize,(qint64)dataBuffer.size()); - memcpy(data,dataBuffer.data(),toRead); - dataBuffer.remove(0,toRead); + qint64 toRead = qMin(maxSize, (qint64)dataBuffer.size()); + + memcpy(data, dataBuffer.data(), toRead); + dataBuffer.remove(0, toRead); return toRead; } @@ -85,28 +90,26 @@ void LogFile::timerFired() { qint64 dataSize; - if(file.bytesAvailable() > 4) - { - - int time; - time = myTime.elapsed(); + if (file.bytesAvailable() > 4) { + int time; + time = myTime.elapsed(); // TODO: going back in time will be a problem - while ((lastPlayed + ((time - timeOffset)* playbackSpeed) > lastTimeStamp)) { - lastPlayed += ((time - timeOffset)* playbackSpeed); - if(file.bytesAvailable() < 4) { + while ((lastPlayed + ((time - timeOffset) * playbackSpeed) > lastTimeStamp)) { + lastPlayed += ((time - timeOffset) * playbackSpeed); + if (file.bytesAvailable() < 4) { stopReplay(); return; } - file.read((char *) &dataSize, sizeof(dataSize)); + file.read((char *)&dataSize, sizeof(dataSize)); - if (dataSize<1 || dataSize>(1024*1024)) { - qDebug() << "Error: Logfile corrupted! Unlikely packet size: " << dataSize << "\n"; - stopReplay(); - return; - } - if(file.bytesAvailable() < dataSize) { + if (dataSize < 1 || dataSize > (1024 * 1024)) { + qDebug() << "Error: Logfile corrupted! Unlikely packet size: " << dataSize << "\n"; + stopReplay(); + return; + } + if (file.bytesAvailable() < dataSize) { stopReplay(); return; } @@ -116,45 +119,45 @@ void LogFile::timerFired() mutex.unlock(); emit readyRead(); - if(file.bytesAvailable() < 4) { + if (file.bytesAvailable() < 4) { stopReplay(); return; } - int save=lastTimeStamp; - file.read((char *) &lastTimeStamp,sizeof(lastTimeStamp)); - // some validity checks - if (lastTimeStamp (60*60*1000)) { // gap of more than 60 minutes) - qDebug() << "Error: Logfile corrupted! Unlikely timestamp " << lastTimeStamp << " after "<< save << "\n"; - stopReplay(); - return; + int save = lastTimeStamp; + file.read((char *)&lastTimeStamp, sizeof(lastTimeStamp)); + // some validity checks + if (lastTimeStamp < save // logfile goies back in time + || (lastTimeStamp - save) > (60 * 60 * 1000)) { // gap of more than 60 minutes) + qDebug() << "Error: Logfile corrupted! Unlikely timestamp " << lastTimeStamp << " after " << save << "\n"; + stopReplay(); + return; } timeOffset = time; time = myTime.elapsed(); - } } else { stopReplay(); } - } -bool LogFile::startReplay() { +bool LogFile::startReplay() +{ dataBuffer.clear(); myTime.restart(); - timeOffset = 0; - lastPlayed = 0; + timeOffset = 0; + lastPlayed = 0; playbackSpeed = 1; - file.read((char *) &lastTimeStamp,sizeof(lastTimeStamp)); + file.read((char *)&lastTimeStamp, sizeof(lastTimeStamp)); timer.setInterval(10); timer.start(); emit replayStarted(); return true; } -bool LogFile::stopReplay() { +bool LogFile::stopReplay() +{ close(); emit replayFinished(); return true; @@ -170,4 +173,3 @@ void LogFile::resumeReplay() timeOffset = myTime.elapsed(); timer.start(); } - diff --git a/ground/openpilotgcs/src/plugins/logging/logfile.h b/ground/openpilotgcs/src/plugins/logging/logfile.h index a07e1832a..9bd588fd5 100644 --- a/ground/openpilotgcs/src/plugins/logging/logfile.h +++ b/ground/openpilotgcs/src/plugins/logging/logfile.h @@ -10,24 +10,32 @@ #include "uavobjectmanager.h" #include -class LogFile : public QIODevice -{ +class LogFile : public QIODevice { Q_OBJECT public: explicit LogFile(QObject *parent = 0); qint64 bytesAvailable() const; - qint64 bytesToWrite() { return file.bytesToWrite(); }; + qint64 bytesToWrite() + { + return file.bytesToWrite(); + }; bool open(OpenMode mode); - void setFileName(QString name) { file.setFileName(name); }; + void setFileName(QString name) + { + file.setFileName(name); + }; void close(); - qint64 writeData(const char * data, qint64 dataSize); - qint64 readData(char * data, qint64 maxlen); + qint64 writeData(const char *data, qint64 dataSize); + qint64 readData(char *data, qint64 maxlen); bool startReplay(); bool stopReplay(); public slots: - void setReplaySpeed(double val) { playbackSpeed = val; qDebug() << playbackSpeed; }; + void setReplaySpeed(double val) + { + playbackSpeed = val; qDebug() << playbackSpeed; + }; void pauseReplay(); void resumeReplay(); diff --git a/ground/openpilotgcs/src/plugins/logging/logginggadget.cpp b/ground/openpilotgcs/src/plugins/logging/logginggadget.cpp index c067fc52d..a4fbc8d7d 100644 --- a/ground/openpilotgcs/src/plugins/logging/logginggadget.cpp +++ b/ground/openpilotgcs/src/plugins/logging/logginggadget.cpp @@ -32,16 +32,14 @@ #include "uavobject.h" LoggingGadget::LoggingGadget(QString classId, LoggingGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} LoggingGadget::~LoggingGadget() { delete m_widget; } -void LoggingGadget::loadConfiguration(IUAVGadgetConfiguration* config) -{ -} +void LoggingGadget::loadConfiguration(IUAVGadgetConfiguration *config) +{} diff --git a/ground/openpilotgcs/src/plugins/logging/logginggadget.h b/ground/openpilotgcs/src/plugins/logging/logginggadget.h index 2805c7afa..a5dcc8ca9 100644 --- a/ground/openpilotgcs/src/plugins/logging/logginggadget.h +++ b/ground/openpilotgcs/src/plugins/logging/logginggadget.h @@ -33,27 +33,35 @@ namespace Core { class IUAVGadget; } -//class QWidget; -//class QString; +// class QWidget; +// class QString; class LoggingGadgetWidget; using namespace Core; -class LoggingGadget : public Core::IUAVGadget -{ +class LoggingGadget : public Core::IUAVGadget { Q_OBJECT public: LoggingGadget(QString classId, LoggingGadgetWidget *widget, QWidget *parent = 0); ~LoggingGadget(); - QList context() const { return m_context; } - QWidget *widget() { return m_widget; } - QString contextHelpId() const { return QString(); } + QList context() const + { + return m_context; + } + QWidget *widget() + { + return m_widget; + } + QString contextHelpId() const + { + return QString(); + } - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); private: - QWidget *m_widget; - QList m_context; + QWidget *m_widget; + QList m_context; }; diff --git a/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.cpp b/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.cpp index 5874a8cb1..37501187f 100644 --- a/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.cpp @@ -30,20 +30,20 @@ #include LoggingGadgetFactory::LoggingGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("LoggingGadget"), - tr("Logging"), - parent) + IUAVGadgetFactory(QString("LoggingGadget"), + tr("Logging"), + parent) { - loggingPlugin = (LoggingPlugin *) parent; + loggingPlugin = (LoggingPlugin *)parent; } LoggingGadgetFactory::~LoggingGadgetFactory() +{} + +IUAVGadget *LoggingGadgetFactory::createGadget(QWidget *parent) { + LoggingGadgetWidget *gadgetWidget = new LoggingGadgetWidget(parent); -} - -IUAVGadget* LoggingGadgetFactory::createGadget(QWidget *parent) { - LoggingGadgetWidget* gadgetWidget = new LoggingGadgetWidget(parent); gadgetWidget->setPlugin(loggingPlugin); return new LoggingGadget(QString("LoggingGadget"), gadgetWidget, parent); } diff --git a/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.h b/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.h index 05c020f17..15129025d 100644 --- a/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.h @@ -38,18 +38,20 @@ class IUAVGadgetFactory; using namespace Core; class LoggingPlugin; -class LoggingGadgetFactory : public IUAVGadgetFactory -{ +class LoggingGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: LoggingGadgetFactory(QObject *parent = 0); ~LoggingGadgetFactory(); - void setPlugin(LoggingPlugin * p) { loggingPlugin = p; }; + void setPlugin(LoggingPlugin *p) + { + loggingPlugin = p; + }; IUAVGadget *createGadget(QWidget *parent); private: - LoggingPlugin * loggingPlugin; + LoggingPlugin *loggingPlugin; }; #endif // LoggingGADGETFACTORY_H_ diff --git a/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.cpp b/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.cpp index f40213148..d5788322f 100644 --- a/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.cpp @@ -42,23 +42,22 @@ LoggingGadgetWidget::LoggingGadgetWidget(QWidget *parent) : QLabel(parent) ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); scpPlugin = pm->getObject(); - } LoggingGadgetWidget::~LoggingGadgetWidget() { - // Do nothing + // Do nothing } -void LoggingGadgetWidget::setPlugin(LoggingPlugin * p) +void LoggingGadgetWidget::setPlugin(LoggingPlugin *p) { loggingPlugin = p; - connect(p,SIGNAL(stateChanged(QString)),this,SLOT(stateChanged(QString))); - connect(m_logging->playButton,SIGNAL(clicked()),p->getLogfile(),SLOT(resumeReplay())); + connect(p, SIGNAL(stateChanged(QString)), this, SLOT(stateChanged(QString))); + connect(m_logging->playButton, SIGNAL(clicked()), p->getLogfile(), SLOT(resumeReplay())); connect(m_logging->playButton, SIGNAL(clicked()), scpPlugin, SLOT(startPlotting())); - connect(m_logging->pauseButton,SIGNAL(clicked()),p->getLogfile(),SLOT(pauseReplay())); + connect(m_logging->pauseButton, SIGNAL(clicked()), p->getLogfile(), SLOT(pauseReplay())); connect(m_logging->pauseButton, SIGNAL(clicked()), scpPlugin, SLOT(stopPlotting())); - connect(m_logging->playbackSpeed,SIGNAL(valueChanged(double)),p->getLogfile(),SLOT(setReplaySpeed(double))); + connect(m_logging->playbackSpeed, SIGNAL(valueChanged(double)), p->getLogfile(), SLOT(setReplaySpeed(double))); void pauseReplay(); void resumeReplay(); } @@ -70,6 +69,6 @@ void LoggingGadgetWidget::stateChanged(QString status) } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.h b/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.h index bee7028f3..a2a166b3c 100644 --- a/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GCSControlGadgetPlugin GCSControl Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,14 +37,13 @@ class Ui_Logging; class LoggingPlugin; -class LoggingGadgetWidget : public QLabel -{ +class LoggingGadgetWidget : public QLabel { Q_OBJECT public: LoggingGadgetWidget(QWidget *parent = 0); ~LoggingGadgetWidget(); - void setPlugin(LoggingPlugin * p); + void setPlugin(LoggingPlugin *p); protected slots: void stateChanged(QString status); @@ -55,10 +54,8 @@ signals: private: Ui_Logging *m_logging; - LoggingPlugin * loggingPlugin; - ScopeGadgetFactory * scpPlugin; - - + LoggingPlugin *loggingPlugin; + ScopeGadgetFactory *scpPlugin; }; #endif /* LoggingGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp b/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp index 6333c4e98..cbf2dfe9d 100644 --- a/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp +++ b/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp @@ -45,35 +45,32 @@ LoggingConnection::LoggingConnection() -{ - -} +{} LoggingConnection::~LoggingConnection() -{ -} +{} void LoggingConnection::onEnumerationChanged() { - emit availableDevChanged(this); + emit availableDevChanged(this); } QList LoggingConnection::availableDevices() { QList list; device d; - d.displayName="Logfile replay..."; - d.name="Logfile replay..."; - list <getObject(); uavTalk = new UAVTalk(&logFile, objManager); - connect(parent,SIGNAL(stopLoggingSignal()),this,SLOT(stopLogging())); + connect(parent, SIGNAL(stopLoggingSignal()), this, SLOT(stopLogging())); return true; }; /** - * Logs an object update to the file. Data format is the - * timestamp as a 32 bit uint counting ms from start of - * file writing (flight time will be embedded in stream), - * then object packet size, then the packed UAVObject. - */ -void LoggingThread::objectUpdated(UAVObject * obj) + * Logs an object update to the file. Data format is the + * timestamp as a 32 bit uint counting ms from start of + * file writing (flight time will be embedded in stream), + * then object packet size, then the packed UAVObject. + */ +void LoggingThread::objectUpdated(UAVObject *obj) { QWriteLocker locker(&lock); - if(!uavTalk->sendObject(obj,false,false) ) + + if (!uavTalk->sendObject(obj, false, false)) { qDebug() << "Error logging " << obj->getName(); + } }; /** - * Connect signals from all the objects updates to the write routine then - * run event loop - */ + * Connect signals from all the objects updates to the write routine then + * run event loop + */ void LoggingThread::run() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - QList< QList > list; + QList< QList > list; list = objManager->getObjects(); - QList< QList >::const_iterator i; - QList::const_iterator j; + QList< QList >::const_iterator i; + QList::const_iterator j; int objects = 0; - for (i = list.constBegin(); i != list.constEnd(); ++i) - { - for (j = (*i).constBegin(); j != (*i).constEnd(); ++j) - { - connect(*j, SIGNAL(objectUpdated(UAVObject*)), (LoggingThread*) this, SLOT(objectUpdated(UAVObject*))); + for (i = list.constBegin(); i != list.constEnd(); ++i) { + for (j = (*i).constBegin(); j != (*i).constEnd(); ++j) { + connect(*j, SIGNAL(objectUpdated(UAVObject *)), (LoggingThread *)this, SLOT(objectUpdated(UAVObject *))); objects++; - //qDebug() << "Detected " << j[0]; + // qDebug() << "Detected " << j[0]; } } - GCSTelemetryStats* gcsStatsObj = GCSTelemetryStats::GetInstance(objManager); + GCSTelemetryStats *gcsStatsObj = GCSTelemetryStats::GetInstance(objManager); GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); - if ( gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED ) - { + if (gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED) { qDebug() << "Logging: connected already, ask for all settings"; retrieveSettings(); } else { @@ -193,8 +185,8 @@ void LoggingThread::run() /** - * Pass this command to the correct thread then close the file - */ + * Pass this command to the correct thread then close the file + */ void LoggingThread::stopLogging() { QWriteLocker locker(&lock); @@ -203,16 +195,14 @@ void LoggingThread::stopLogging() ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - QList< QList > list; + QList< QList > list; list = objManager->getObjects(); - QList< QList >::const_iterator i; - QList::const_iterator j; + QList< QList >::const_iterator i; + QList::const_iterator j; - for (i = list.constBegin(); i != list.constEnd(); ++i) - { - for (j = (*i).constBegin(); j != (*i).constEnd(); ++j) - { - disconnect(*j, SIGNAL(objectUpdated(UAVObject*)), (LoggingThread*) this, SLOT(objectUpdated(UAVObject*))); + for (i = list.constBegin(); i != list.constEnd(); ++i) { + for (j = (*i).constBegin(); j != (*i).constEnd(); ++j) { + disconnect(*j, SIGNAL(objectUpdated(UAVObject *)), (LoggingThread *)this, SLOT(objectUpdated(UAVObject *))); } } @@ -230,20 +220,18 @@ void LoggingThread::retrieveSettings() queue.clear(); // Get all objects, add metaobjects, settings and data objects with OnChange update mode to the queue // Get UAVObjectManager instance - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objMngr = pm->getObject(); - QList< QList > objs = objMngr->getDataObjects(); - for (int n = 0; n < objs.length(); ++n) - { - UAVDataObject* obj = objs[n][0]; - if ( obj->isSettings() ) - { - queue.enqueue(obj); - } + QList< QList > objs = objMngr->getDataObjects(); + for (int n = 0; n < objs.length(); ++n) { + UAVDataObject *obj = objs[n][0]; + if (obj->isSettings()) { + queue.enqueue(obj); } + } // Start retrieving qDebug() << tr("Logging: retrieve settings objects from the autopilot (%1 objects)") - .arg( queue.length()); + .arg(queue.length()); retrieveNextObject(); } @@ -254,15 +242,14 @@ void LoggingThread::retrieveSettings() void LoggingThread::retrieveNextObject() { // If queue is empty return - if ( queue.isEmpty() ) - { + if (queue.isEmpty()) { qDebug() << "Logging: Object retrieval completed"; return; } // Get next object from the queue - UAVObject* obj = queue.dequeue(); + UAVObject *obj = queue.dequeue(); // Connect to object - connect(obj, SIGNAL(transactionCompleted(UAVObject*,bool)), this, SLOT(transactionCompleted(UAVObject*,bool))); + connect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool))); // Request update obj->requestUpdate(); } @@ -270,30 +257,26 @@ void LoggingThread::retrieveNextObject() /** * Called by the retrieved object when a transaction is completed. */ -void LoggingThread::transactionCompleted(UAVObject* obj, bool success) +void LoggingThread::transactionCompleted(UAVObject *obj, bool success) { Q_UNUSED(success); // Disconnect from sending object obj->disconnect(this); // Process next object if telemetry is still available // Get stats objects - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - GCSTelemetryStats* gcsStatsObj = GCSTelemetryStats::GetInstance(objManager); + GCSTelemetryStats *gcsStatsObj = GCSTelemetryStats::GetInstance(objManager); GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); - if ( gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED ) - { + if (gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED) { retrieveNextObject(); - } - else - { + } else { qDebug() << "Logging: Object retrieval has been cancelled"; queue.clear(); } } - /**************************************************************** Logging plugin ********************************/ @@ -306,17 +289,18 @@ LoggingPlugin::LoggingPlugin() : state(IDLE) LoggingPlugin::~LoggingPlugin() { - if (loggingThread) + if (loggingThread) { delete loggingThread; + } // Don't delete it, the plugin manager will do it: - //delete logConnection; + // delete logConnection; } /** - * Add Logging plugin to File menu - */ -bool LoggingPlugin::initialize(const QStringList& args, QString *errMsg) + * Add Logging plugin to File menu + */ +bool LoggingPlugin::initialize(const QStringList & args, QString *errMsg) { Q_UNUSED(args); Q_UNUSED(errMsg); @@ -325,14 +309,14 @@ bool LoggingPlugin::initialize(const QStringList& args, QString *errMsg) // Add Menu entry - Core::ActionManager* am = Core::ICore::instance()->actionManager(); - Core::ActionContainer* ac = am->actionContainer(Core::Constants::M_TOOLS); + Core::ActionManager *am = Core::ICore::instance()->actionManager(); + Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_TOOLS); // Command to start logging cmd = am->registerAction(new QAction(this), - "LoggingPlugin.Logging", - QList() << - Core::Constants::C_GLOBAL_ID); + "LoggingPlugin.Logging", + QList() << + Core::Constants::C_GLOBAL_ID); cmd->setDefaultKeySequence(QKeySequence("Ctrl+L")); cmd->action()->setText("Start logging..."); @@ -347,33 +331,29 @@ bool LoggingPlugin::initialize(const QStringList& args, QString *errMsg) addAutoReleasedObject(mf); // Map signal from end of replay to replay stopped - connect(getLogfile(),SIGNAL(replayFinished()), this, SLOT(replayStopped())); - connect(getLogfile(),SIGNAL(replayStarted()), this, SLOT(replayStarted())); + connect(getLogfile(), SIGNAL(replayFinished()), this, SLOT(replayStopped())); + connect(getLogfile(), SIGNAL(replayStarted()), this, SLOT(replayStarted())); return true; } /** - * The action that is triggered by the menu item which opens the - * file and begins logging if successful - */ + * The action that is triggered by the menu item which opens the + * file and begins logging if successful + */ void LoggingPlugin::toggleLogging() { - if(state == IDLE) - { - + if (state == IDLE) { QString fileName = QFileDialog::getSaveFileName(NULL, tr("Start Log"), - tr("OP-%0.opl").arg(QDateTime::currentDateTime().toString("yyyy-MM-dd_hh-mm-ss")), - tr("OpenPilot Log (*.opl)")); - if (fileName.isEmpty()) + tr("OP-%0.opl").arg(QDateTime::currentDateTime().toString("yyyy-MM-dd_hh-mm-ss")), + tr("OpenPilot Log (*.opl)")); + if (fileName.isEmpty()) { return; + } startLogging(fileName); cmd->action()->setText(tr("Stop logging")); - - } - else if(state == LOGGING) - { + } else if (state == LOGGING) { stopLogging(); cmd->action()->setText(tr("Start logging...")); } @@ -381,18 +361,18 @@ void LoggingPlugin::toggleLogging() /** - * Starts the logging thread to a certain file - */ + * Starts the logging thread to a certain file + */ void LoggingPlugin::startLogging(QString file) { qDebug() << "Logging to " << file; // We have to delete the previous logging thread if is was still there! - if (loggingThread) + if (loggingThread) { delete loggingThread; + } loggingThread = new LoggingThread(); - if(loggingThread->openFile(file,this)) - { - connect(loggingThread,SIGNAL(finished()),this,SLOT(loggingStopped())); + if (loggingThread->openFile(file, this)) { + connect(loggingThread, SIGNAL(finished()), this, SLOT(loggingStopped())); state = LOGGING; loggingThread->start(); emit stateChanged("LOGGING"); @@ -404,24 +384,25 @@ void LoggingPlugin::startLogging(QString file) } /** - * Send the stop logging signal to the LoggingThread - */ + * Send the stop logging signal to the LoggingThread + */ void LoggingPlugin::stopLogging() { emit stopLoggingSignal(); - disconnect( this,SIGNAL(stopLoggingSignal()),0,0); + disconnect(this, SIGNAL(stopLoggingSignal()), 0, 0); } /** - * Receive the logging stopped signal from the LoggingThread - * and change status to not logging - */ + * Receive the logging stopped signal from the LoggingThread + * and change status to not logging + */ void LoggingPlugin::loggingStopped() { - if(state == LOGGING) + if (state == LOGGING) { state = IDLE; + } emit stateChanged("IDLE"); @@ -430,8 +411,8 @@ void LoggingPlugin::loggingStopped() } /** - * Received the replay stopped signal from the LogFile - */ + * Received the replay stopped signal from the LogFile + */ void LoggingPlugin::replayStopped() { state = IDLE; @@ -439,8 +420,8 @@ void LoggingPlugin::replayStopped() } /** - * Received the replay started signal from the LogFile - */ + * Received the replay started signal from the LogFile + */ void LoggingPlugin::replayStarted() { state = REPLAY; @@ -448,8 +429,6 @@ void LoggingPlugin::replayStarted() } - - void LoggingPlugin::extensionsInitialized() { addAutoReleasedObject(logConnection); diff --git a/ground/openpilotgcs/src/plugins/logging/loggingplugin.h b/ground/openpilotgcs/src/plugins/logging/loggingplugin.h index 1ecf35b9b..64abda4fc 100644 --- a/ground/openpilotgcs/src/plugins/logging/loggingplugin.h +++ b/ground/openpilotgcs/src/plugins/logging/loggingplugin.h @@ -45,13 +45,12 @@ class LoggingPlugin; class LoggingGadgetFactory; /** -* Define a connection via the IConnection interface -* Plugin will add a instance of this class to the pool, -* so the connection manager can use it. -*/ + * Define a connection via the IConnection interface + * Plugin will add a instance of this class to the pool, + * so the connection manager can use it. + */ class LoggingConnection - : public Core::IConnection -{ + : public Core::IConnection { Q_OBJECT public: LoggingConnection(); @@ -64,8 +63,14 @@ public: virtual QString connectionName(); virtual QString shortName(); - bool deviceOpened() {return m_deviceOpened;} - LogFile* getLogfile() { return &logFile;} + bool deviceOpened() + { + return m_deviceOpened; + } + LogFile *getLogfile() + { + return &logFile; + } private: @@ -81,16 +86,14 @@ protected: }; - -class LoggingThread : public QThread -{ -Q_OBJECT +class LoggingThread : public QThread { + Q_OBJECT public: - bool openFile(QString file, LoggingPlugin * parent); + bool openFile(QString file, LoggingPlugin *parent); private slots: - void objectUpdated(UAVObject * obj); - void transactionCompleted(UAVObject* obj, bool success); + void objectUpdated(UAVObject *obj); + void transactionCompleted(UAVObject *obj, bool success); public slots: void stopLogging(); @@ -99,18 +102,16 @@ protected: void run(); QReadWriteLock lock; LogFile logFile; - UAVTalk * uavTalk; + UAVTalk *uavTalk; private: - QQueue queue; + QQueue queue; void retrieveSettings(); void retrieveNextObject(); - }; -class LoggingPlugin : public ExtensionSystem::IPlugin -{ +class LoggingPlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -118,11 +119,17 @@ public: ~LoggingPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); - LoggingConnection* getLogConnection() { return logConnection; }; - LogFile* getLogfile() { return logConnection->getLogfile();} + LoggingConnection *getLogConnection() + { + return logConnection; + }; + LogFile *getLogfile() + { + return logConnection->getLogfile(); + } void setLogMenuTitle(QString str); @@ -133,11 +140,11 @@ signals: protected: - enum {IDLE, LOGGING, REPLAY} state; - LoggingThread * loggingThread; + enum { IDLE, LOGGING, REPLAY } state; + LoggingThread *loggingThread; // These are used for replay, logging in its own thread - LoggingConnection* logConnection; + LoggingConnection *logConnection; private slots: void toggleLogging(); @@ -149,8 +156,7 @@ private slots: private: LoggingGadgetFactory *mf; - Core::Command* cmd; - + Core::Command *cmd; }; #endif /* LoggingPLUGIN_H_ */ /** diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.cpp b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.cpp index f1c330fec..f4c6f0f84 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.cpp +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.cpp @@ -33,17 +33,16 @@ #include MagicWaypointGadget::MagicWaypointGadget(QString classId, MagicWaypointGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} MagicWaypointGadget::~MagicWaypointGadget() { delete m_widget; } -void MagicWaypointGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void MagicWaypointGadget::loadConfiguration(IUAVGadgetConfiguration *config) { Q_UNUSED(config); } diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.h b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.h index 16f432d3b..120b0c2ac 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.h +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.h @@ -33,27 +33,35 @@ namespace Core { class IUAVGadget; } -//class QWidget; -//class QString; +// class QWidget; +// class QString; class MagicWaypointGadgetWidget; using namespace Core; -class MagicWaypointGadget : public Core::IUAVGadget -{ +class MagicWaypointGadget : public Core::IUAVGadget { Q_OBJECT public: MagicWaypointGadget(QString classId, MagicWaypointGadgetWidget *widget, QWidget *parent = 0); ~MagicWaypointGadget(); - QList context() const { return m_context; } - QWidget *widget() { return m_widget; } - QString contextHelpId() const { return QString(); } + QList context() const + { + return m_context; + } + QWidget *widget() + { + return m_widget; + } + QString contextHelpId() const + { + return QString(); + } - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); private: - QWidget *m_widget; - QList m_context; + QWidget *m_widget; + QList m_context; }; diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.cpp index fc0e52362..b69ca17f0 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.cpp @@ -31,18 +31,17 @@ #include MagicWaypointGadgetFactory::MagicWaypointGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("MagicWaypointGadget"), - tr("Magic Waypoint"), - parent) -{ -} + IUAVGadgetFactory(QString("MagicWaypointGadget"), + tr("Magic Waypoint"), + parent) +{} MagicWaypointGadgetFactory::~MagicWaypointGadgetFactory() +{} + +IUAVGadget *MagicWaypointGadgetFactory::createGadget(QWidget *parent) { + MagicWaypointGadgetWidget *gadgetWidget = new MagicWaypointGadgetWidget(parent); -} - -IUAVGadget* MagicWaypointGadgetFactory::createGadget(QWidget *parent) { - MagicWaypointGadgetWidget* gadgetWidget = new MagicWaypointGadgetWidget(parent); return new MagicWaypointGadget(QString("MagicWaypointGadget"), gadgetWidget, parent); } diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.h b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.h index 92d65edd1..bc542b887 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.h @@ -37,8 +37,7 @@ class IUAVGadgetFactory; using namespace Core; -class MagicWaypointGadgetFactory : public IUAVGadgetFactory -{ +class MagicWaypointGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: MagicWaypointGadgetFactory(QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp index cc2e99418..bd7d9e585 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp @@ -47,13 +47,13 @@ MagicWaypointGadgetWidget::MagicWaypointGadgetWidget(QWidget *parent) : QLabel(p m_magicwaypoint->setupUi(this); // Connect object updated event from UAVObject to also update check boxes - connect(getPathDesired(), SIGNAL(objectUpdated(UAVObject*)), this, SLOT(positionObjectChanged(UAVObject*))); - connect(getPositionActual(), SIGNAL(objectUpdated(UAVObject*)), this, SLOT(positionObjectChanged(UAVObject*))); + connect(getPathDesired(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(positionObjectChanged(UAVObject *))); + connect(getPositionActual(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(positionObjectChanged(UAVObject *))); // Connect updates from the position widget to this widget - connect(m_magicwaypoint->widgetPosition, SIGNAL(positionClicked(double,double)), this, SLOT(positionSelected(double,double))); - connect(this, SIGNAL(positionActualObjectChanged(double,double)), m_magicwaypoint->widgetPosition, SLOT(updateActualIndicator(double,double))); - connect(this, SIGNAL(positionDesiredObjectChanged(double,double)), m_magicwaypoint->widgetPosition, SLOT(updateDesiredIndicator(double,double))); + connect(m_magicwaypoint->widgetPosition, SIGNAL(positionClicked(double, double)), this, SLOT(positionSelected(double, double))); + connect(this, SIGNAL(positionActualObjectChanged(double, double)), m_magicwaypoint->widgetPosition, SLOT(updateActualIndicator(double, double))); + connect(this, SIGNAL(positionDesiredObjectChanged(double, double)), m_magicwaypoint->widgetPosition, SLOT(updateDesiredIndicator(double, double))); // Catch changes in scale for visualization connect(m_magicwaypoint->horizontalSliderScale, SIGNAL(valueChanged(int)), this, SLOT(scaleChanged(int))); @@ -64,45 +64,48 @@ MagicWaypointGadgetWidget::MagicWaypointGadgetWidget(QWidget *parent) : QLabel(p MagicWaypointGadgetWidget::~MagicWaypointGadgetWidget() { - // Do nothing + // Do nothing } /*! - \brief Returns the @ref PositionDesired UAVObject - */ -PathDesired* MagicWaypointGadgetWidget::getPathDesired() + \brief Returns the @ref PositionDesired UAVObject + */ +PathDesired *MagicWaypointGadgetWidget::getPathDesired() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - PathDesired* obj = PathDesired::GetInstance(objManager); + PathDesired *obj = PathDesired::GetInstance(objManager); + Q_ASSERT(obj != NULL); // Save crashes later return obj; } /*! - \brief Returns the @ref PositionActual UAVObject - */ -PositionActual* MagicWaypointGadgetWidget::getPositionActual() + \brief Returns the @ref PositionActual UAVObject + */ +PositionActual *MagicWaypointGadgetWidget::getPositionActual() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); PositionActual *obj = PositionActual::GetInstance(objManager); + Q_ASSERT(obj != NULL); return obj; } /** - * Detect changes in scale and update visualization (does not change position) - */ -void MagicWaypointGadgetWidget::scaleChanged(int scale) { + * Detect changes in scale and update visualization (does not change position) + */ +void MagicWaypointGadgetWidget::scaleChanged(int scale) +{ Q_UNUSED(scale); pathDesiredChanged(getPathDesired()); positionActualChanged(getPositionActual()); } /** - * Emit a position changed signal when @ref PositionActual object is changed - */ + * Emit a position changed signal when @ref PositionActual object is changed + */ void MagicWaypointGadgetWidget::positionActualChanged(UAVObject *) { PositionActual::DataFields positionActual = getPositionActual()->getData(); @@ -113,8 +116,8 @@ void MagicWaypointGadgetWidget::positionActualChanged(UAVObject *) } /** - * Emit a position changed signal when @ref PathDesired is changed - */ + * Emit a position changed signal when @ref PathDesired is changed + */ void MagicWaypointGadgetWidget::pathDesiredChanged(UAVObject *) { PathDesired::DataFields pathDesired = getPathDesired()->getData(); @@ -125,19 +128,21 @@ void MagicWaypointGadgetWidget::pathDesiredChanged(UAVObject *) } /** - * Slot called by visualization when a new @ref PathDesired is requested - */ -void MagicWaypointGadgetWidget::positionSelected(double north, double east) { + * Slot called by visualization when a new @ref PathDesired is requested + */ +void MagicWaypointGadgetWidget::positionSelected(double north, double east) +{ double scale = m_magicwaypoint->horizontalSliderScale->value(); PathDesired::DataFields pathDesired = getPathDesired()->getData(); + pathDesired.End[PathDesired::END_NORTH] = north * scale; - pathDesired.End[PathDesired::END_EAST] = east * scale; + pathDesired.End[PathDesired::END_EAST] = east * scale; pathDesired.Mode = PathDesired::MODE_FLYENDPOINT; getPathDesired()->setData(pathDesired); } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h index 59d55197b..49e8f1251 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GCSControlGadgetPlugin GCSControl Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,8 +34,7 @@ class Ui_MagicWaypoint; -class MagicWaypointGadgetWidget : public QLabel -{ +class MagicWaypointGadgetWidget : public QLabel { Q_OBJECT public: @@ -53,9 +52,9 @@ protected slots: void positionSelected(double north, double east); private: - PathDesired * getPathDesired(); - PositionActual * getPositionActual(); - Ui_MagicWaypoint * m_magicwaypoint; + PathDesired *getPathDesired(); + PositionActual *getPositionActual(); + Ui_MagicWaypoint *m_magicwaypoint; }; #endif /* MagicWaypointGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.cpp b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.cpp index 2dd8025e0..06ffe24dd 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.cpp +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.cpp @@ -34,36 +34,36 @@ MagicWaypointPlugin::MagicWaypointPlugin() { - // Do nothing + // Do nothing } MagicWaypointPlugin::~MagicWaypointPlugin() { - // Do nothing + // Do nothing } -bool MagicWaypointPlugin::initialize(const QStringList& args, QString *errMsg) +bool MagicWaypointPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new MagicWaypointGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new MagicWaypointGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void MagicWaypointPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void MagicWaypointPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(MagicWaypointPlugin) /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.h b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.h index 030786707..636c0c3f3 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.h +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.h @@ -32,16 +32,15 @@ class MagicWaypointGadgetFactory; -class MagicWaypointPlugin : public ExtensionSystem::IPlugin -{ +class MagicWaypointPlugin : public ExtensionSystem::IPlugin { public: MagicWaypointPlugin(); - ~MagicWaypointPlugin(); + ~MagicWaypointPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - MagicWaypointGadgetFactory *mf; + MagicWaypointGadgetFactory *mf; }; #endif /* GCSControlPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.cpp b/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.cpp index 26172ce4a..65fd77069 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.cpp +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.cpp @@ -38,12 +38,12 @@ #include /** - * @brief Constructor for JoystickControl widget. Sets up the image of a joystick - */ + * @brief Constructor for JoystickControl widget. Sets up the image of a joystick + */ PositionField::PositionField(QWidget *parent) : QGraphicsView(parent) { - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setScene(new QGraphicsScene(this)); setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); @@ -59,12 +59,12 @@ PositionField::PositionField(QWidget *parent) : m_positiondesired = new QGraphicsSvgItem(); m_positiondesired->setSharedRenderer(m_renderer); m_positiondesired->setElementId(QString("desiredPosition")); - m_positiondesired->setPos(0,0); + m_positiondesired->setPos(0, 0); m_positionactual = new QGraphicsSvgItem(); m_positionactual->setSharedRenderer(m_renderer); m_positionactual->setElementId(QString("actualPosition")); - m_positionactual->setPos(0,0); + m_positionactual->setPos(0, 0); QGraphicsScene *l_scene = scene(); l_scene->clear(); // This also deletes all items contained in the scene. @@ -75,20 +75,18 @@ PositionField::PositionField(QWidget *parent) : } PositionField::~PositionField() -{ - -} +{} /** - * @brief Update aircraft position on image (values go from -1 to 1) - */ + * @brief Update aircraft position on image (values go from -1 to 1) + */ void PositionField::updateDesiredIndicator(double north, double east) { QRectF sceneSize = scene()->sceneRect(); m_positiondesired->setPos( - (east+1)/2*sceneSize.width() - m_positiondesired->boundingRect().width() / 2, - (-north+1)/2*sceneSize.height() - m_positiondesired->boundingRect().height() / 2); + (east + 1) / 2 * sceneSize.width() - m_positiondesired->boundingRect().width() / 2, + (-north + 1) / 2 * sceneSize.height() - m_positiondesired->boundingRect().height() / 2); } void PositionField::updateActualIndicator(double north, double east) @@ -96,29 +94,29 @@ void PositionField::updateActualIndicator(double north, double east) QRectF sceneSize = scene()->sceneRect(); m_positionactual->setPos( - (east+1)/2*sceneSize.width() - m_positionactual->boundingRect().width() / 2, - (-north+1)/2*sceneSize.height() - m_positionactual->boundingRect().height() / 2); + (east + 1) / 2 * sceneSize.width() - m_positionactual->boundingRect().width() / 2, + (-north + 1) / 2 * sceneSize.height() - m_positionactual->boundingRect().height() / 2); } /** - * @brief Redirect mouse move events to control position - */ + * @brief Redirect mouse move events to control position + */ void PositionField::mouseMoveEvent(QMouseEvent *event) { - QPointF point = mapToScene(event->pos()); + QPointF point = mapToScene(event->pos()); QRectF sceneSize = scene()->sceneRect(); - double north = - (point.y() / sceneSize.height() - .5) * 2; + double north = -(point.y() / sceneSize.height() - .5) * 2; double east = (point.x() / sceneSize.width() - .5) * 2; emit positionClicked(north, east); } /** - * @brief Redirect mouse move clicks to control position - */ + * @brief Redirect mouse move clicks to control position + */ void PositionField::mousePressEvent(QMouseEvent *event) { - if( event->button() == Qt::LeftButton ) { + if (event->button() == Qt::LeftButton) { mouseMoveEvent(event); } } @@ -131,8 +129,8 @@ void PositionField::paint() void PositionField::paintEvent(QPaintEvent *event) { // Skip painting until the dial file is loaded - if (! m_renderer->isValid()) { - qDebug()<<"Image file not loaded, not rendering"; + if (!m_renderer->isValid()) { + qDebug() << "Image file not loaded, not rendering"; } QGraphicsView::paintEvent(event); @@ -141,11 +139,11 @@ void PositionField::paintEvent(QPaintEvent *event) void PositionField::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - fitInView(m_background, Qt::IgnoreAspectRatio ); + fitInView(m_background, Qt::IgnoreAspectRatio); } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.h b/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.h index 3a8b17cbf..0c5ce7834 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.h +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.h @@ -35,11 +35,10 @@ #include namespace Ui { - class PositionField; +class PositionField; } -class PositionField : public QGraphicsView -{ +class PositionField : public QGraphicsView { Q_OBJECT public: @@ -70,6 +69,6 @@ private: #endif // POSITIONFIELD_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.cpp index 899345b13..b84a6b5af 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -29,22 +29,21 @@ #include "modelviewgadgetconfiguration.h" ModelViewGadget::ModelViewGadget(QString classId, ModelViewGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} ModelViewGadget::~ModelViewGadget() { delete m_widget; } -void ModelViewGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void ModelViewGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - ModelViewGadgetConfiguration *m = qobject_cast(config); + ModelViewGadgetConfiguration *m = qobject_cast(config); + m_widget->setAcFilename(m->acFilename()); m_widget->setBgFilename(m->bgFilename()); m_widget->setVboEnable(m->vboEnabled()); m_widget->reloadScene(); } - diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.h b/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.h index 2c6734cfd..7ca4282aa 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.h +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,15 +38,17 @@ class ModelViewGadgetWidget; using namespace Core; -class ModelViewGadget : public Core::IUAVGadget -{ +class ModelViewGadget : public Core::IUAVGadget { Q_OBJECT public: ModelViewGadget(QString classId, ModelViewGadgetWidget *widget, QWidget *parent = 0); ~ModelViewGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: ModelViewGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.cpp index 50416d4b4..dd886d17f 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.cpp @@ -7,38 +7,38 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "modelviewgadgetconfiguration.h" #include "utils/pathutils.h" -ModelViewGadgetConfiguration::ModelViewGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +ModelViewGadgetConfiguration::ModelViewGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_acFilename("../share/openpilotgcs/models/planes/Easystar/EasyStar.3ds"), m_bgFilename(""), m_enableVbo(false) { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { QString modelFile = qSettings->value("acFilename").toString(); - QString bgFile = qSettings->value("bgFilename").toString(); - m_enableVbo = qSettings->value("enableVbo").toBool(); + QString bgFile = qSettings->value("bgFilename").toString(); + m_enableVbo = qSettings->value("enableVbo").toBool(); m_acFilename = Utils::PathUtils().InsertDataPath(modelFile); m_bgFilename = Utils::PathUtils().InsertDataPath(bgFile); } @@ -47,9 +47,10 @@ ModelViewGadgetConfiguration::ModelViewGadgetConfiguration(QString classId, QSet IUAVGadgetConfiguration *ModelViewGadgetConfiguration::clone() { ModelViewGadgetConfiguration *mv = new ModelViewGadgetConfiguration(this->classId()); + mv->m_acFilename = m_acFilename; mv->m_bgFilename = m_bgFilename; - mv->m_enableVbo = m_enableVbo; + mv->m_enableVbo = m_enableVbo; return mv; } @@ -57,8 +58,9 @@ IUAVGadgetConfiguration *ModelViewGadgetConfiguration::clone() * Saves a configuration. * */ -void ModelViewGadgetConfiguration::saveConfig(QSettings* qSettings) const { - qSettings->setValue("acFilename", Utils::PathUtils().RemoveDataPath(m_acFilename)); - qSettings->setValue("bgFilename", Utils::PathUtils().RemoveDataPath(m_bgFilename)); - qSettings->setValue("enableVbo", m_enableVbo); +void ModelViewGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ + qSettings->setValue("acFilename", Utils::PathUtils().RemoveDataPath(m_acFilename)); + qSettings->setValue("bgFilename", Utils::PathUtils().RemoveDataPath(m_bgFilename)); + qSettings->setValue("enableVbo", m_enableVbo); } diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.h index a43684487..ce3b0a939 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.h @@ -7,21 +7,21 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,20 +32,37 @@ using namespace Core; -class ModelViewGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT +class ModelViewGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit ModelViewGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit ModelViewGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); - QString acFilename() {return m_acFilename;} - void setAcFilename(QString acFile) { m_acFilename = acFile; } - QString bgFilename() { return m_bgFilename; } - void setBgFilename(QString bgFile) { m_bgFilename = bgFile; } - bool vboEnabled() { return m_enableVbo; } - void setVboEnabled(bool vboEnable) { m_enableVbo = vboEnable; } + QString acFilename() + { + return m_acFilename; + } + void setAcFilename(QString acFile) + { + m_acFilename = acFile; + } + QString bgFilename() + { + return m_bgFilename; + } + void setBgFilename(QString bgFile) + { + m_bgFilename = bgFile; + } + bool vboEnabled() + { + return m_enableVbo; + } + void setVboEnabled(bool vboEnable) + { + m_enableVbo = vboEnable; + } signals: public slots: @@ -53,7 +70,7 @@ public slots: private: QString m_acFilename; QString m_bgFilename; - bool m_enableVbo; // Vertex buffer objects, a few GPUs crash if enabled + bool m_enableVbo; // Vertex buffer objects, a few GPUs crash if enabled }; #endif // MODELVIEWGADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.cpp index 4c1f4ef52..bad149297 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,27 +33,25 @@ #include ModelViewGadgetFactory::ModelViewGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("ModelViewGadget"), tr("ModelView"), parent) -{ -} + IUAVGadgetFactory(QString("ModelViewGadget"), tr("ModelView"), parent) +{} ModelViewGadgetFactory::~ModelViewGadgetFactory() +{} + +Core::IUAVGadget *ModelViewGadgetFactory::createGadget(QWidget *parent) { + ModelViewGadgetWidget *gadgetWidget = new ModelViewGadgetWidget(parent); + + return new ModelViewGadget(QString("ModelViewGadget"), gadgetWidget, parent); } -Core::IUAVGadget* ModelViewGadgetFactory::createGadget(QWidget *parent) -{ - ModelViewGadgetWidget* gadgetWidget = new ModelViewGadgetWidget(parent); - return new ModelViewGadget(QString("ModelViewGadget"), gadgetWidget, parent); -} - -IUAVGadgetConfiguration *ModelViewGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *ModelViewGadgetFactory::createConfiguration(QSettings *qSettings) { return new ModelViewGadgetConfiguration(QString("ModelViewGadget"), qSettings); } IOptionsPage *ModelViewGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new ModelViewGadgetOptionsPage(qobject_cast(config)); + return new ModelViewGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.h b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.h index ff3ec52c2..32267d845 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class ModelViewGadgetFactory : public Core::IUAVGadgetFactory -{ +class ModelViewGadgetFactory : public Core::IUAVGadgetFactory { Q_OBJECT public: ModelViewGadgetFactory(QObject *parent = 0); ~ModelViewGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.cpp index 005e73242..2822555a4 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.cpp @@ -7,21 +7,21 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,8 +34,7 @@ ModelViewGadgetOptionsPage::ModelViewGadgetOptionsPage(ModelViewGadgetConfiguration *config, QObject *parent) : IOptionsPage(parent), m_config(config) -{ -} +{} QWidget *ModelViewGadgetOptionsPage::createPage(QWidget *parent) { @@ -70,4 +69,3 @@ void ModelViewGadgetOptionsPage::finish() { delete m_page; } - diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.h index 04e604f1a..e5875583f 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.h @@ -7,21 +7,21 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,23 +38,34 @@ class ModelViewGadgetConfiguration; class QFileDialog; namespace Core { - class IUAVGadgetConfiguration; +class IUAVGadgetConfiguration; } namespace Ui { - class ModelViewOptionsPage; +class ModelViewOptionsPage; } using namespace Core; -class ModelViewGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class ModelViewGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit ModelViewGadgetOptionsPage(ModelViewGadgetConfiguration *config, QObject *parent = 0); - QString id() const { return ""; } - QString trName() const { return ""; } - QString category() const { return ""; } - QString trCategory() const { return ""; } + QString id() const + { + return ""; + } + QString trName() const + { + return ""; + } + QString category() const + { + return ""; + } + QString trCategory() const + { + return ""; + } QWidget *createPage(QWidget *parent); void apply(); diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp index 562be089c..11047c7b5 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,67 +35,61 @@ #include -ModelViewGadgetWidget::ModelViewGadgetWidget(QWidget *parent) -: QGLWidget(new GLC_Context(QGLFormat(QGL::SampleBuffers)),parent) -, m_Light() -, m_World() -, m_GlView(this) -, m_MoverController() -, m_ModelBoundingBox() -, m_MotionTimer() -, acFilename() -, bgFilename() -, vboEnable(false) +ModelViewGadgetWidget::ModelViewGadgetWidget(QWidget *parent) + : QGLWidget(new GLC_Context(QGLFormat(QGL::SampleBuffers)), parent) + , m_Light() + , m_World() + , m_GlView(this) + , m_MoverController() + , m_ModelBoundingBox() + , m_MotionTimer() + , acFilename() + , bgFilename() + , vboEnable(false) { - setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); CreateScene(); QColor repColor; repColor.setRgbF(1.0, 0.11372, 0.11372, 0.0); - m_MoverController= GLC_Factory::instance()->createDefaultMoverController(repColor, &m_GlView); + m_MoverController = GLC_Factory::instance()->createDefaultMoverController(repColor, &m_GlView); // Get required UAVObjects - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager* objManager = pm->getObject(); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); attActual = AttitudeActual::GetInstance(objManager); connect(&m_MotionTimer, SIGNAL(timeout()), this, SLOT(updateAttitude())); } ModelViewGadgetWidget::~ModelViewGadgetWidget() -{ - -} +{} void ModelViewGadgetWidget::setAcFilename(QString acf) { - if(QFile::exists(acf)) + if (QFile::exists(acf)) { acFilename = acf; - else - { - acFilename= acf= ":/modelview/models/warning_sign.obj"; + } else { + acFilename = acf = ":/modelview/models/warning_sign.obj"; m_GlView.cameraHandle()->setFrontView(); // set to front camera to see/read the warning sign } } void ModelViewGadgetWidget::setBgFilename(QString bgf) { - if (QFile::exists(bgFilename)) + if (QFile::exists(bgFilename)) { bgFilename = bgf; - else - { - qDebug() << "file " << bgf << " doesn't exists"; - bgFilename= ":/modelview/models/black.jpg"; // will put a black background if there's no background + } else { + qDebug() << "file " << bgf << " doesn't exists"; + bgFilename = ":/modelview/models/black.jpg"; // will put a black background if there's no background } - } void ModelViewGadgetWidget::setVboEnable(bool eVbo) { - vboEnable = eVbo; - m_World.collection()->setVboUsage(vboEnable); + vboEnable = eVbo; + m_World.collection()->setVboUsage(vboEnable); } //// Public funcitons //// @@ -127,32 +121,30 @@ void ModelViewGadgetWidget::initializeGL() void ModelViewGadgetWidget::paintGL() { - try - { - // Clear screen - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + try { + // Clear screen + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - // OpenGL error handler - { - GLenum error= glGetError(); - if (error != GL_NO_ERROR) - { - GLC_OpenGlException OpenGlException("ModelViewGadgetWidget::paintGL() ", error); - throw(OpenGlException); - } - } + // OpenGL error handler + { + GLenum error = glGetError(); + if (error != GL_NO_ERROR) { + GLC_OpenGlException OpenGlException("ModelViewGadgetWidget::paintGL() ", error); + throw(OpenGlException); + } + } - // Load identity matrix - glLoadIdentity(); + // Load identity matrix + glLoadIdentity(); - // Enable antialiasing - glEnable(GL_MULTISAMPLE); + // Enable antialiasing + glEnable(GL_MULTISAMPLE); // Calculate camera depth of view m_GlView.setDistMinAndMax(m_World.boundingBox()); // define view matrix - m_Light.glExecute(); + m_Light.glExecute(); m_GlView.glExecuteCam(); // Display the collection of GLC_Object @@ -161,169 +153,155 @@ void ModelViewGadgetWidget::paintGL() // Display UI Info (orbit circle) m_MoverController.drawActiveMoverRep(); + } catch(GLC_Exception &e) { + qDebug() << e.what(); } - catch (GLC_Exception &e) - { - qDebug() << e.what(); - } - - } void ModelViewGadgetWidget::resizeGL(int width, int height) { - m_GlView.setWinGLSize(width, height); // Compute window aspect ratio - // OpenGL error handler - { - GLenum error= glGetError(); - if (error != GL_NO_ERROR) - { - GLC_OpenGlException OpenGlException("ModelViewGadgetWidget::resizeGL() ", error); - throw(OpenGlException); - } - } - + m_GlView.setWinGLSize(width, height); // Compute window aspect ratio + // OpenGL error handler + { + GLenum error = glGetError(); + if (error != GL_NO_ERROR) { + GLC_OpenGlException OpenGlException("ModelViewGadgetWidget::resizeGL() ", error); + throw(OpenGlException); + } + } } // Create GLC_Object to display void ModelViewGadgetWidget::CreateScene() { // put a black background if the 3D model is invalid or if the background image is also invalid - if (acFilename == ":/modelview/models/warning_sign.obj" or !QFile::exists(bgFilename)) - bgFilename= ":/modelview/models/black.jpg"; - - - try - { - m_GlView.loadBackGroundImage(bgFilename); + if (acFilename == ":/modelview/models/warning_sign.obj" or !QFile::exists(bgFilename)) { + bgFilename = ":/modelview/models/black.jpg"; } - catch(GLC_Exception e) - { + + + try { + m_GlView.loadBackGroundImage(bgFilename); + } catch(GLC_Exception e) { qDebug("ModelView: background image file loading failed."); } - try - { - if(QFile::exists(acFilename)) - { + try { + if (QFile::exists(acFilename)) { QFile aircraft(acFilename); - m_World= GLC_Factory::instance()->createWorldFromFile(aircraft); - m_ModelBoundingBox= m_World.boundingBox(); + m_World = GLC_Factory::instance()->createWorldFromFile(aircraft); + m_ModelBoundingBox = m_World.boundingBox(); m_GlView.reframe(m_ModelBoundingBox); // center 3D model in the scene } else { - qDebug("ModelView: aircraft file not found."); + qDebug("ModelView: aircraft file not found."); } - } - catch(GLC_Exception e) - { + } catch(GLC_Exception e) { qDebug("ModelView: aircraft file loading failed."); } } -void ModelViewGadgetWidget::wheelEvent(QWheelEvent * e) +void ModelViewGadgetWidget::wheelEvent(QWheelEvent *e) { - double delta = m_GlView.cameraHandle()->distEyeTarget() - (e->delta()/4) ; - m_GlView.cameraHandle()->setDistEyeTarget(delta); - m_GlView.setDistMinAndMax(m_World.boundingBox()); + double delta = m_GlView.cameraHandle()->distEyeTarget() - (e->delta() / 4); + + m_GlView.cameraHandle()->setDistEyeTarget(delta); + m_GlView.setDistMinAndMax(m_World.boundingBox()); } void ModelViewGadgetWidget::mousePressEvent(QMouseEvent *e) { - GLC_UserInput userInput(e->x(), e->y()); - if (m_MoverController.hasActiveMover()) return; + GLC_UserInput userInput(e->x(), e->y()); - switch (e->button()) - { - case (Qt::LeftButton): - m_MotionTimer.stop(); - m_MoverController.setActiveMover(GLC_MoverController::TurnTable, userInput); - updateGL(); - break; - case (Qt::RightButton): - printf("VBO enabled: %s, VBO supported: %s, VBO used: %s\n", - vboEnable ? "yes" : "no", - GLC_State::vboSupported() ? "yes" : "no", - GLC_State::vboUsed() ? "yes" : "no"); - printf("Renderer - %s \n", (char*)glGetString(GL_RENDERER)); - printf("Extensions - %s\n", (char*)glGetString(GL_EXTENSIONS)); - break; - default: - break; - } -} + if (m_MoverController.hasActiveMover()) { + return; + } -void ModelViewGadgetWidget::mouseMoveEvent(QMouseEvent * e) -{ - GLC_UserInput userInput(e->x(), e->y()); - if (not m_MoverController.hasActiveMover()) return; - m_MoverController.move(userInput); - m_GlView.setDistMinAndMax(m_World.boundingBox()); + switch (e->button()) { + case (Qt::LeftButton): + m_MotionTimer.stop(); + m_MoverController.setActiveMover(GLC_MoverController::TurnTable, userInput); updateGL(); + break; + case (Qt::RightButton): + printf("VBO enabled: %s, VBO supported: %s, VBO used: %s\n", + vboEnable ? "yes" : "no", + GLC_State::vboSupported() ? "yes" : "no", + GLC_State::vboUsed() ? "yes" : "no"); + printf("Renderer - %s \n", (char *)glGetString(GL_RENDERER)); + printf("Extensions - %s\n", (char *)glGetString(GL_EXTENSIONS)); + break; + default: + break; + } } -void ModelViewGadgetWidget::mouseReleaseEvent(QMouseEvent*) +void ModelViewGadgetWidget::mouseMoveEvent(QMouseEvent *e) { - if (not m_MoverController.hasActiveMover()) return; - m_MoverController.setNoMover(); - m_MotionTimer.start(); - updateGL(); + GLC_UserInput userInput(e->x(), e->y()); + + if (not m_MoverController.hasActiveMover()) { + return; + } + m_MoverController.move(userInput); + m_GlView.setDistMinAndMax(m_World.boundingBox()); + updateGL(); } -void ModelViewGadgetWidget::keyPressEvent(QKeyEvent * e) // switch between camera +void ModelViewGadgetWidget::mouseReleaseEvent(QMouseEvent *) { - if (e->key() == Qt::Key_1) - { + if (not m_MoverController.hasActiveMover()) { + return; + } + m_MoverController.setNoMover(); + m_MotionTimer.start(); + updateGL(); +} + +void ModelViewGadgetWidget::keyPressEvent(QKeyEvent *e) // switch between camera +{ + if (e->key() == Qt::Key_1) { m_GlView.cameraHandle()->setIsoView(); updateGL(); } - if (e->key() == Qt::Key_2) - { + if (e->key() == Qt::Key_2) { m_GlView.cameraHandle()->setFrontView(); updateGL(); } - if (e->key() == Qt::Key_3) - { + if (e->key() == Qt::Key_3) { m_GlView.cameraHandle()->setIsoView(); - m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS,glc::toRadian(90)); + m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS, glc::toRadian(90)); updateGL(); } - if (e->key() == Qt::Key_4) - { + if (e->key() == Qt::Key_4) { m_GlView.cameraHandle()->setLeftView(); updateGL(); } - if (e->key() == Qt::Key_5) - { + if (e->key() == Qt::Key_5) { m_GlView.cameraHandle()->setTopView(); - m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS,glc::toRadian(180)); + m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS, glc::toRadian(180)); updateGL(); } - if (e->key() == Qt::Key_6) - { + if (e->key() == Qt::Key_6) { m_GlView.cameraHandle()->setRightView();; updateGL(); } - if (e->key() == Qt::Key_7) - { + if (e->key() == Qt::Key_7) { m_GlView.cameraHandle()->setIsoView(); - m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS,glc::toRadian(-90)); + m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS, glc::toRadian(-90)); updateGL(); } - if (e->key() == Qt::Key_8) - { + if (e->key() == Qt::Key_8) { m_GlView.cameraHandle()->setRearView();; updateGL(); } - if (e->key() == Qt::Key_9) - { + if (e->key() == Qt::Key_9) { m_GlView.cameraHandle()->setIsoView(); - m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS,glc::toRadian(180)); + m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS, glc::toRadian(180)); updateGL(); } - if (e->key() == Qt::Key_0) - { + if (e->key() == Qt::Key_0) { m_GlView.cameraHandle()->setBottomView(); - m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS,glc::toRadian(180)); + m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS, glc::toRadian(180)); updateGL(); } } @@ -334,32 +312,34 @@ void ModelViewGadgetWidget::keyPressEvent(QKeyEvent * e) // switch between camer void ModelViewGadgetWidget::updateAttitude() { AttitudeActual::DataFields data = attActual->getData(); // get attitude data - GLC_StructOccurence* rootObject= m_World.rootOccurence(); // get the full 3D model - double x= data.q3; - double y= data.q2; - double z= data.q4; - double w= data.q1; - if (w == 0.0) + GLC_StructOccurence *rootObject = m_World.rootOccurence(); // get the full 3D model + double x = data.q3; + double y = data.q2; + double z = data.q4; + double w = data.q1; + + if (w == 0.0) { w = 1.0; + } // create and gives the product of 2 4x4 matrices to get the rotation of the 3D model's matrix QMatrix4x4 m1; - m1.setRow(0, QVector4D(w,z,-y,x)); - m1.setRow(1, QVector4D(-z,w,x,y)); - m1.setRow(2, QVector4D(y,-x,w,z)); - m1.setRow(3, QVector4D(-x,-y,-z,w)); + m1.setRow(0, QVector4D(w, z, -y, x)); + m1.setRow(1, QVector4D(-z, w, x, y)); + m1.setRow(2, QVector4D(y, -x, w, z)); + m1.setRow(3, QVector4D(-x, -y, -z, w)); QMatrix4x4 m2; - m2.setRow(0, QVector4D(w,z,-y,-x)); - m2.setRow(1, QVector4D(-z,w,x,-y)); - m2.setRow(2, QVector4D(y,-x,w,-z)); - m2.setRow(3, QVector4D(x,y,z,w)); - QMatrix4x4 m0= m1 * m2; + m2.setRow(0, QVector4D(w, z, -y, -x)); + m2.setRow(1, QVector4D(-z, w, x, -y)); + m2.setRow(2, QVector4D(y, -x, w, -z)); + m2.setRow(3, QVector4D(x, y, z, w)); + QMatrix4x4 m0 = m1 * m2; // convert QMatrix4x4 to GLC_Matrix4x4 GLC_Matrix4x4 rootObjectRotation; { - double* newMatrixData = rootObjectRotation.setData(); - double* oldMatrixData = (double*) m0.data(); - for (int i=0; i<16; i++){ - newMatrixData[i]= oldMatrixData[i]; + double *newMatrixData = rootObjectRotation.setData(); + double *oldMatrixData = (double *)m0.data(); + for (int i = 0; i < 16; i++) { + newMatrixData[i] = oldMatrixData[i]; } } // sets and updates the 3D model's matrix diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h index 7ab6f19b2..353aeba7a 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -42,34 +42,32 @@ #include "attitudeactual.h" - -class ModelViewGadgetWidget : public QGLWidget -{ +class ModelViewGadgetWidget : public QGLWidget { Q_OBJECT public: ModelViewGadgetWidget(QWidget *parent = 0); - ~ModelViewGadgetWidget(); - void setAcFilename(QString acf); + ~ModelViewGadgetWidget(); + void setAcFilename(QString acf); - void setBgFilename(QString bgf); - void setVboEnable(bool eVbo); - void reloadScene(); - void updateAttitude(int value); + void setBgFilename(QString bgf); + void setVboEnable(bool eVbo); + void reloadScene(); + void updateAttitude(int value); private: - void initializeGL(); - void paintGL(); - void resizeGL(int width, int height); - // Create GLC_Object to display - void CreateScene(); + void initializeGL(); + void paintGL(); + void resizeGL(int width, int height); + // Create GLC_Object to display + void CreateScene(); - //Mouse events - void mousePressEvent(QMouseEvent * e); - void mouseMoveEvent(QMouseEvent * e); - void mouseReleaseEvent(QMouseEvent * e); - void wheelEvent(QWheelEvent * e); - void keyPressEvent(QKeyEvent * e); + // Mouse events + void mousePressEvent(QMouseEvent *e); + void mouseMoveEvent(QMouseEvent *e); + void mouseReleaseEvent(QMouseEvent *e); + void wheelEvent(QWheelEvent *e); + void keyPressEvent(QKeyEvent *e); ////////////////////////////////////////////////////////////////////// // Private slots Functions @@ -78,20 +76,20 @@ private slots: void updateAttitude(); private: - GLC_Factory* m_pFactory; + GLC_Factory *m_pFactory; GLC_Light m_Light; GLC_World m_World; GLC_Viewport m_GlView; GLC_MoverController m_MoverController; GLC_BoundingBox m_ModelBoundingBox; - //! The timer used for motion + // ! The timer used for motion QTimer m_MotionTimer; QString acFilename; QString bgFilename; bool vboEnable; - AttitudeActual* attActual; + AttitudeActual *attActual; }; #endif /* MODELVIEWGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.cpp index de4831d51..12540de27 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,32 +34,31 @@ ModelViewPlugin::ModelViewPlugin() { - // Do nothing + // Do nothing } ModelViewPlugin::~ModelViewPlugin() { - // Do nothing + // Do nothing } -bool ModelViewPlugin::initialize(const QStringList& args, QString *errMsg) +bool ModelViewPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mvf = new ModelViewGadgetFactory(this); - addAutoReleasedObject(mvf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mvf = new ModelViewGadgetFactory(this); + addAutoReleasedObject(mvf); - return true; + return true; } void ModelViewPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void ModelViewPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(ModelViewPlugin) - diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.h b/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.h index f0c9bca8b..74cbc6447 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.h +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class ModelViewGadgetFactory; -class ModelViewPlugin : public ExtensionSystem::IPlugin -{ +class ModelViewPlugin : public ExtensionSystem::IPlugin { public: - ModelViewPlugin(); - ~ModelViewPlugin(); + ModelViewPlugin(); + ~ModelViewPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - ModelViewGadgetFactory *mvf; + ModelViewGadgetFactory *mvf; }; #endif /* MODELVIEWPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/notify/notificationitem.cpp b/ground/openpilotgcs/src/plugins/notify/notificationitem.cpp index a54e7ff89..b3d627838 100644 --- a/ground/openpilotgcs/src/plugins/notify/notificationitem.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notificationitem.cpp @@ -25,7 +25,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -//Qt headers +// Qt headers #include #include @@ -40,9 +40,6 @@ #include "notifylogging.h" - - - QStringList NotificationItem::sayOrderValues; QStringList NotificationItem::retryValues; @@ -69,47 +66,44 @@ NotificationItem::NotificationItem(QObject *parent) , _expireTimeout(eDefaultTimeout) , _mute(false) { - NotificationItem::sayOrderValues.clear(); - NotificationItem::sayOrderValues.insert(never,QString(tr("Never"))); - NotificationItem::sayOrderValues.insert(beforeFirst,QString(tr("Before first"))); - NotificationItem::sayOrderValues.insert(beforeSecond,QString(tr("Before second"))); - NotificationItem::sayOrderValues.insert(afterSecond,QString(tr("After second"))); + NotificationItem::sayOrderValues.insert(never, QString(tr("Never"))); + NotificationItem::sayOrderValues.insert(beforeFirst, QString(tr("Before first"))); + NotificationItem::sayOrderValues.insert(beforeSecond, QString(tr("Before second"))); + NotificationItem::sayOrderValues.insert(afterSecond, QString(tr("After second"))); NotificationItem::retryValues.clear(); - NotificationItem::retryValues.insert(repeatOnce,QString(tr("Repeat Once"))); - NotificationItem::retryValues.insert(repeatOncePerUpdate,QString(tr("Repeat Once per update"))); - NotificationItem::retryValues.insert(repeatInstantly,QString(tr("Repeat Instantly"))); - NotificationItem::retryValues.insert(repeat10seconds,QString(tr("Repeat 10 seconds"))); - NotificationItem::retryValues.insert(repeat30seconds,QString(tr("Repeat 30 seconds"))); - NotificationItem::retryValues.insert(repeat1minute,QString(tr("Repeat 1 minute"))); - + NotificationItem::retryValues.insert(repeatOnce, QString(tr("Repeat Once"))); + NotificationItem::retryValues.insert(repeatOncePerUpdate, QString(tr("Repeat Once per update"))); + NotificationItem::retryValues.insert(repeatInstantly, QString(tr("Repeat Instantly"))); + NotificationItem::retryValues.insert(repeat10seconds, QString(tr("Repeat 10 seconds"))); + NotificationItem::retryValues.insert(repeat30seconds, QString(tr("Repeat 30 seconds"))); + NotificationItem::retryValues.insert(repeat1minute, QString(tr("Repeat 1 minute"))); } -void NotificationItem::copyTo(NotificationItem* that) const +void NotificationItem::copyTo(NotificationItem *that) const { - that->isNowPlaying = isNowPlaying; - that->_isPlayed = _isPlayed; + that->isNowPlaying = isNowPlaying; + that->_isPlayed = _isPlayed; that->_soundCollectionPath = _soundCollectionPath; - that->_currentLanguage = _currentLanguage; + that->_currentLanguage = _currentLanguage; that->_soundCollectionPath = _soundCollectionPath; - that->_dataObject = _dataObject; - that->_objectField = _objectField; - that->_condition = _condition; - that->_sound1 = _sound1; - that->_sound2 = _sound2; - that->_sound3 = _sound3; - that->_sayOrder = _sayOrder; - that->_singleValue = _singleValue; - that->_valueRange2 = _valueRange2; - that->_repeatValue = _repeatValue; + that->_dataObject = _dataObject; + that->_objectField = _objectField; + that->_condition = _condition; + that->_sound1 = _sound1; + that->_sound2 = _sound2; + that->_sound3 = _sound3; + that->_sayOrder = _sayOrder; + that->_singleValue = _singleValue; + that->_valueRange2 = _valueRange2; + that->_repeatValue = _repeatValue; that->_expireTimeout = _expireTimeout; that->_mute = _mute; - } -void NotificationItem::saveState(QSettings* settings) const +void NotificationItem::saveState(QSettings *settings) const { settings->setValue("SoundCollectionPath", Utils::PathUtils().RemoveDataPath(getSoundCollectionPath())); settings->setValue(QLatin1String("CurrentLanguage"), getCurrentLanguage()); @@ -127,9 +121,9 @@ void NotificationItem::saveState(QSettings* settings) const settings->setValue(QLatin1String("Mute"), mute()); } -void NotificationItem::restoreState(QSettings* settings) +void NotificationItem::restoreState(QSettings *settings) { - //settings = Core::ICore::instance()->settings(); + // settings = Core::ICore::instance()->settings(); setSoundCollectionPath(Utils::PathUtils().InsertDataPath(settings->value(QLatin1String("SoundCollectionPath"), tr("")).toString())); setCurrentLanguage(settings->value(QLatin1String("CurrentLanguage"), tr("")).toString()); setDataObject(settings->value(QLatin1String("DataObject"), tr("")).toString()); @@ -147,14 +141,14 @@ void NotificationItem::restoreState(QSettings* settings) setMute(settings->value(QLatin1String("Mute"), tr("")).toInt()); } -void NotificationItem::serialize(QDataStream& stream) +void NotificationItem::serialize(QDataStream & stream) { stream << this->_soundCollectionPath; stream << this->_currentLanguage; stream << this->_dataObject; stream << this->_objectField; stream << this->_condition; - qNotifyDebug()<<"getOptionsPageValues seriaize"<<_condition; + qNotifyDebug() << "getOptionsPageValues seriaize" << _condition; stream << this->_sound1; stream << this->_sound2; stream << this->_sound3; @@ -166,7 +160,7 @@ void NotificationItem::serialize(QDataStream& stream) stream << this->_mute; } -void NotificationItem::deserialize(QDataStream& stream) +void NotificationItem::deserialize(QDataStream & stream) { stream >> this->_soundCollectionPath; stream >> this->_currentLanguage; @@ -190,16 +184,18 @@ void NotificationItem::startTimer(int msec) _timer = new QTimer(this); _timer->setInterval(msec); } - if (!_timer->isActive()) + if (!_timer->isActive()) { _timer->start(); + } } void NotificationItem::restartTimer() { if (!_timer) { - if (!_timer->isActive()) + if (!_timer->isActive()) { _timer->start(); + } } } @@ -207,8 +203,9 @@ void NotificationItem::restartTimer() void NotificationItem::stopTimer() { if (_timer) { - if (_timer->isActive()) + if (_timer->isActive()) { _timer->stop(); + } } } @@ -232,8 +229,9 @@ void NotificationItem::startExpireTimer() void NotificationItem::stopExpireTimer() { if (_expireTimer) { - if (_expireTimer) + if (_expireTimer) { _expireTimer->stop(); + } } } @@ -248,7 +246,7 @@ void NotificationItem::disposeExpireTimer() int getValuePosition(QString sayOrder) { - return NotificationItem::sayOrderValues.indexOf(sayOrder)-1; + return NotificationItem::sayOrderValues.indexOf(sayOrder) - 1; } QString NotificationItem::checkSoundExists(QString fileName) @@ -257,67 +255,71 @@ QString NotificationItem::checkSoundExists(QString fileName) QString filePath = QDir::toNativeSeparators(getSoundCollectionPath() + "/" + getCurrentLanguage() + "/" + name); - if(QFile::exists(filePath)) + + if (QFile::exists(filePath)) { return filePath; - else { + } else { filePath = QDir::toNativeSeparators(getSoundCollectionPath() + "/default/" + name); - if(!QFile::exists(filePath)) + if (!QFile::exists(filePath)) { filePath.clear(); + } } return filePath; } QStringList valueToSoundList(QString value) { - qNotifyDebug()<<"notificationItem valueToSoundList input param"< number < 1 digitWavs.append(numberParts.at(1)); } else { @@ -327,20 +329,22 @@ QStringList valueToSoundList(QString value) digitWavs.append(numberParts.at(1).right(1)); } } - qNotifyDebug()<<"notificationItem valueToSoundList return value"<getType()) { - if(!field->getOptions().contains(value.toString())) + if (!field->getOptions().contains(value.toString())) { return QString(); + } str = value.toString(); } else { str = QString("%L1").arg(value.toDouble()); @@ -351,62 +355,68 @@ QString stringFromValue(QVariant value, UAVObjectField* field) QString NotificationItem::toString() { QString str; - UAVObjectField* field = getUAVObjectField(); + UAVObjectField *field = getUAVObjectField(); QString value = stringFromValue(singleValue(), field); - int pos = getSayOrder()-1; + int pos = getSayOrder() - 1; QStringList lst; + lst.append(getSoundCaption(getSound1())); lst.append(getSoundCaption(getSound2())); lst.append(getSoundCaption(getSound3())); QStringList valueSounds = valueToSoundList(value); bool missed = false; foreach(QString sound, valueSounds) { - if(checkSoundExists(sound).isEmpty()) { - missed = true; - break; + if (checkSoundExists(sound).isEmpty()) { + missed = true; + break; } } // if not "Never" case - if(-1 != pos) { - if(missed) + if (-1 != pos) { + if (missed) { lst.insert(pos, "[missed]" + value); - else + } else { lst.insert(pos, value); + } } str = lst.join(" "); return str; } -QStringList& NotificationItem::toSoundList() +QStringList & NotificationItem::toSoundList() { // tips: // check of *.wav files exist needed for playing phonon queues; // if phonon player don't find next file in queue, it buzz - UAVObjectField* field = getUAVObjectField(); + UAVObjectField *field = getUAVObjectField(); QString value = stringFromValue(singleValue(), field); // generate queue of sound files to play _messageSequence.clear(); - int pos = getSayOrder()-1; + int pos = getSayOrder() - 1; QStringList lst; - if(!getSound1().isEmpty()) + if (!getSound1().isEmpty()) { lst.append(getSound1()); - if(!getSound2().isEmpty()) + } + if (!getSound2().isEmpty()) { lst.append(getSound2()); - if(!getSound3().isEmpty()) + } + if (!getSound3().isEmpty()) { lst.append(getSound3()); + } // if not "Never" case - if(-1 != pos) { + if (-1 != pos) { QStringList valueSounds = valueToSoundList(value); foreach(QString sound, valueSounds) - lst.insert(pos++, sound); + lst.insert(pos++, sound); } foreach(QString sound, lst) { QString path = checkSoundExists(sound); + if (!path.isEmpty()) { _messageSequence.append(path); } else { @@ -419,17 +429,21 @@ QStringList& NotificationItem::toSoundList() QString NotificationItem::getSoundCaption(QString fileName) { - if(fileName.isEmpty()) return QString(); - if(checkSoundExists(fileName).isEmpty()) { + if (fileName.isEmpty()) { + return QString(); + } + if (checkSoundExists(fileName).isEmpty()) { return QString("[missed]") + fileName; } return fileName; } -UAVObjectField* NotificationItem::getUAVObjectField() { +UAVObjectField *NotificationItem::getUAVObjectField() +{ return getUAVObject()->getField(getObjectField()); } -UAVDataObject* NotificationItem::getUAVObject() { - return dynamic_cast((ExtensionSystem::PluginManager::instance()->getObject())->getObject(getDataObject())); +UAVDataObject *NotificationItem::getUAVObject() +{ + return dynamic_cast((ExtensionSystem::PluginManager::instance()->getObject())->getObject(getDataObject())); } diff --git a/ground/openpilotgcs/src/plugins/notify/notificationitem.h b/ground/openpilotgcs/src/plugins/notify/notificationitem.h index 1475fd64f..c90b9c894 100644 --- a/ground/openpilotgcs/src/plugins/notify/notificationitem.h +++ b/ground/openpilotgcs/src/plugins/notify/notificationitem.h @@ -37,112 +37,195 @@ using namespace Core; #define DECLARE_SOUND(number) \ - QString getSound##number() const { return _sound##number; } \ - void setSound##number(QString text) { _sound##number = text; } \ + QString getSound##number() const { return _sound##number; } \ + void setSound##number(QString text) { _sound##number = text; } \ class UAVDataObject; class UAVObjectField; -class NotificationItem : public QObject -{ +class NotificationItem : public QObject { Q_OBJECT public: enum { eDefaultTimeout = 15 }; // in sec - enum {never,beforeFirst,beforeSecond,afterSecond}; - enum {repeatOncePerUpdate,repeatOnce,repeatInstantly,repeat10seconds, - repeat30seconds,repeat1minute}; + enum { never, beforeFirst, beforeSecond, afterSecond }; + enum { repeatOncePerUpdate, repeatOnce, repeatInstantly, repeat10seconds, + repeat30seconds, repeat1minute }; explicit NotificationItem(QObject *parent = 0); - void copyTo(NotificationItem*) const; + void copyTo(NotificationItem *) const; DECLARE_SOUND(1) DECLARE_SOUND(2) DECLARE_SOUND(3) - bool getCurrentUpdatePlayed() const {return _currentUpdatePlayed;} - void setCurrentUpdatePlayed(bool value){_currentUpdatePlayed=value;} + bool getCurrentUpdatePlayed() const + { + return _currentUpdatePlayed; + } + void setCurrentUpdatePlayed(bool value) + { + _currentUpdatePlayed = value; + } - int getCondition() const { return _condition; } - void setCondition(int value) { _condition = value; } + int getCondition() const + { + return _condition; + } + void setCondition(int value) + { + _condition = value; + } - int getSayOrder() const { return _sayOrder; } - void setSayOrder(int text) { _sayOrder = text; } + int getSayOrder() const + { + return _sayOrder; + } + void setSayOrder(int text) + { + _sayOrder = text; + } - QVariant singleValue() const { return _singleValue; } - void setSingleValue(QVariant value) { _singleValue = value; } + QVariant singleValue() const + { + return _singleValue; + } + void setSingleValue(QVariant value) + { + _singleValue = value; + } - double valueRange2() const { return _valueRange2; } - void setValueRange2(double value) { _valueRange2 = value; } + double valueRange2() const + { + return _valueRange2; + } + void setValueRange2(double value) + { + _valueRange2 = value; + } - QString getDataObject() const { return _dataObject; } - void setDataObject(QString text) { _dataObject = text; } + QString getDataObject() const + { + return _dataObject; + } + void setDataObject(QString text) + { + _dataObject = text; + } - QString getObjectField() const { return _objectField; } - void setObjectField(QString text) { _objectField = text; } + QString getObjectField() const + { + return _objectField; + } + void setObjectField(QString text) + { + _objectField = text; + } - QString getSoundCollectionPath() const { return _soundCollectionPath; } - void setSoundCollectionPath(QString path) { _soundCollectionPath = path; } + QString getSoundCollectionPath() const + { + return _soundCollectionPath; + } + void setSoundCollectionPath(QString path) + { + _soundCollectionPath = path; + } - QString getCurrentLanguage() const { return _currentLanguage; } - void setCurrentLanguage(QString text) { _currentLanguage = text; } + QString getCurrentLanguage() const + { + return _currentLanguage; + } + void setCurrentLanguage(QString text) + { + _currentLanguage = text; + } - QStringList getMessageSequence() const { return _messageSequence; } - void setMessageSequence(QStringList sequence) { _messageSequence = sequence; } + QStringList getMessageSequence() const + { + return _messageSequence; + } + void setMessageSequence(QStringList sequence) + { + _messageSequence = sequence; + } - int retryValue() const { return _repeatValue; } - void setRetryValue(int value) { _repeatValue = value; } + int retryValue() const + { + return _repeatValue; + } + void setRetryValue(int value) + { + _repeatValue = value; + } - int lifetime() const { return _expireTimeout; } - void setLifetime(int value) { _expireTimeout = value; } + int lifetime() const + { + return _expireTimeout; + } + void setLifetime(int value) + { + _expireTimeout = value; + } - bool mute() const { return _mute; } - void setMute(bool value) { _mute = value; } + bool mute() const + { + return _mute; + } + void setMute(bool value) + { + _mute = value; + } - void saveState(QSettings* settings) const; - void restoreState(QSettings* settings); + void saveState(QSettings *settings) const; + void restoreState(QSettings *settings); - UAVDataObject* getUAVObject(void); - UAVObjectField* getUAVObjectField(void); + UAVDataObject *getUAVObject(void); + UAVObjectField *getUAVObjectField(void); - void serialize(QDataStream& stream); - void deserialize(QDataStream& stream); + void serialize(QDataStream & stream); + void deserialize(QDataStream & stream); /** - * Convert notification item fields in single string, - * to show in table for example - * - * @return string which describe notification - */ + * Convert notification item fields in single string, + * to show in table for example + * + * @return string which describe notification + */ QString toString(); /** - * Generate list of sound files needed to play notification - * - * @return success - reference to non-empty _messageSequence; - * error - if one of sounds doesn't exist returns - * reference to empty _messageSequence; - */ - QStringList& toSoundList(); + * Generate list of sound files needed to play notification + * + * @return success - reference to non-empty _messageSequence; + * error - if one of sounds doesn't exist returns + * reference to empty _messageSequence; + */ + QStringList & toSoundList(); /** - * Returns sound caption name, needed to create string representation of notification. - * - * @return success - string == , if sound file exists - * error - string == [missind], if sound file doesn't exist - */ + * Returns sound caption name, needed to create string representation of notification. + * + * @return success - string == , if sound file exists + * error - string == [missind], if sound file doesn't exist + */ QString getSoundCaption(QString fileName); - QTimer* getTimer() const { return _timer; } + QTimer *getTimer() const + { + return _timer; + } void startTimer(int value); void restartTimer(); void stopTimer(); void disposeTimer(); - QTimer* getExpireTimer() const { return _expireTimer; } + QTimer *getExpireTimer() const + { + return _expireTimer; + } void startExpireTimer(); void stopExpireTimer(); @@ -161,57 +244,57 @@ private: bool _currentUpdatePlayed; - QTimer* _timer; + QTimer *_timer; - //! time from putting notification in queue till moment when notification became out-of-date - //! NOTE: each notification has it lifetime, this time setups individually for each notification - //! according to its priority - QTimer* _expireTimer; + // ! time from putting notification in queue till moment when notification became out-of-date + // ! NOTE: each notification has it lifetime, this time setups individually for each notification + // ! according to its priority + QTimer *_expireTimer; - //! list of wav files from which notification consists + // ! list of wav files from which notification consists QStringList _messageSequence; - //! path to folder with sound files + // ! path to folder with sound files QString _soundCollectionPath; - //! language in what notifications will be spelled + // ! language in what notifications will be spelled QString _currentLanguage; - //! one UAV object per one notification + // ! one UAV object per one notification QString _dataObject; - //! one field value change can be assigned to one notification + // ! one field value change can be assigned to one notification QString _objectField; - //! fire condition for UAV field value (lower, greater, in range) + // ! fire condition for UAV field value (lower, greater, in range) int _condition; - //! possible sounds(at least one required to play notification) + // ! possible sounds(at least one required to play notification) QString _sound1; QString _sound2; QString _sound3; - //! order in what sounds 1-3 will be played + // ! order in what sounds 1-3 will be played int _sayOrder; - //! one-side range, value(numeric or ENUM type) maybe lower, greater or in range + // ! one-side range, value(numeric or ENUM type) maybe lower, greater or in range QVariant _singleValue; - //! both-side range, value should be inside the range - //double _valueRange1; + // ! both-side range, value should be inside the range + // double _valueRange1; double _valueRange2; - //! how often or what periodicaly notification should be played + // ! how often or what periodicaly notification should be played int _repeatValue; - //! time after event occured till notification became invalid - //! and will be removed from list + // ! time after event occured till notification became invalid + // ! and will be removed from list int _expireTimeout; - //! enables/disables playing of current notification + // ! enables/disables playing of current notification bool _mute; }; -Q_DECLARE_METATYPE(NotificationItem*) +Q_DECLARE_METATYPE(NotificationItem *) #endif // NotificationItem_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.cpp b/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.cpp index 1db47049b..c3da268cb 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.cpp @@ -31,28 +31,27 @@ #include "notifylogging.h" #include "notificationitem.h" -NotifyItemDelegate::NotifyItemDelegate(QObject* parent) +NotifyItemDelegate::NotifyItemDelegate(QObject *parent) : QItemDelegate(parent) , _parent(parent) -{ -} +{} -QWidget *NotifyItemDelegate::createEditor(QWidget* parent, const QStyleOptionViewItem& /*none*/, - const QModelIndex& index) const +QWidget *NotifyItemDelegate::createEditor(QWidget *parent, const QStyleOptionViewItem & /*none*/, + const QModelIndex & index) const { if (eRepeatValue == index.column()) { - QComboBox* editor = new QComboBox(parent); + QComboBox *editor = new QComboBox(parent); editor->clear(); editor->addItems(NotificationItem::retryValues); return editor; } else { if (eExpireTimer == index.column()) { - QSpinBox* editor = new QSpinBox(parent); + QSpinBox *editor = new QSpinBox(parent); connect(editor, SIGNAL(editingFinished()), this, SLOT(commitAndCloseEditor())); return editor; } else { if (eTurnOn == index.column()) { - QCheckBox* editor = new QCheckBox(parent); + QCheckBox *editor = new QCheckBox(parent); connect(editor, SIGNAL(editingFinished()), this, SLOT(commitAndCloseEditor())); return editor; } @@ -64,26 +63,24 @@ QWidget *NotifyItemDelegate::createEditor(QWidget* parent, const QStyleOptionVie void NotifyItemDelegate::commitAndCloseEditor() { - QLineEdit* editor = qobject_cast(sender()); + QLineEdit *editor = qobject_cast(sender()); + if (editor) { emit commitData(editor); emit closeEditor(editor); } else { - QComboBox* editor = qobject_cast(sender()); - if (editor) - { + QComboBox *editor = qobject_cast(sender()); + if (editor) { emit commitData(editor); emit closeEditor(editor); } else { - QSpinBox* editor = qobject_cast(sender()); - if (editor) - { + QSpinBox *editor = qobject_cast(sender()); + if (editor) { emit commitData(editor); emit closeEditor(editor); } else { - QCheckBox* editor = qobject_cast(sender()); - if (editor) - { + QCheckBox *editor = qobject_cast(sender()); + if (editor) { emit commitData(editor); emit closeEditor(editor); } @@ -94,21 +91,23 @@ void NotifyItemDelegate::commitAndCloseEditor() void NotifyItemDelegate::setEditorData(QWidget *editor, const QModelIndex &index) const { - QLineEdit* edit = qobject_cast(editor); + QLineEdit *edit = qobject_cast(editor); + if (edit) { edit->setText(index.model()->data(index, Qt::EditRole).toString()); } else { - QComboBox* repeatEditor = qobject_cast(editor); - if (repeatEditor) + QComboBox *repeatEditor = qobject_cast(editor); + if (repeatEditor) { repeatEditor->setCurrentIndex(repeatEditor->findText(index.model()->data(index, Qt::EditRole).toString())); - else { - QSpinBox* expireEditor = qobject_cast(editor); - if (expireEditor) + } else { + QSpinBox *expireEditor = qobject_cast(editor); + if (expireEditor) { expireEditor->setValue(index.model()->data(index, Qt::EditRole).toInt()); - else { - QCheckBox* enablePlayEditor = qobject_cast(editor); - if (enablePlayEditor) + } else { + QCheckBox *enablePlayEditor = qobject_cast(editor); + if (enablePlayEditor) { enablePlayEditor->setChecked(index.model()->data(index, Qt::EditRole).toBool()); + } } } } @@ -117,18 +116,19 @@ void NotifyItemDelegate::setEditorData(QWidget *editor, const QModelIndex &index void NotifyItemDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, const QModelIndex &index) const { QLineEdit *edit = qobject_cast(editor); + if (edit) { model->setData(index, edit->text()); } else { - QComboBox * repeatEditor = qobject_cast(editor); + QComboBox *repeatEditor = qobject_cast(editor); if (repeatEditor) { model->setData(index, repeatEditor->currentText()); } else { - QSpinBox * expireEditor = qobject_cast(editor); + QSpinBox *expireEditor = qobject_cast(editor); if (expireEditor) { model->setData(index, expireEditor->value(), Qt::EditRole); } else { - QCheckBox* enablePlayEditor = qobject_cast(editor); + QCheckBox *enablePlayEditor = qobject_cast(editor); if (enablePlayEditor) { model->setData(index, enablePlayEditor->isChecked(), Qt::EditRole); } @@ -137,20 +137,22 @@ void NotifyItemDelegate::setModelData(QWidget *editor, QAbstractItemModel *model } } -void NotifyItemDelegate::selectRow(const QString& text) +void NotifyItemDelegate::selectRow(const QString & text) { - QComboBox* combo = qobject_cast(sender()); - QTableWidget* table = new QTableWidget; - table = (QTableWidget*)(combo->parent()); + QComboBox *combo = qobject_cast(sender()); + QTableWidget *table = new QTableWidget; + + table = (QTableWidget *)(combo->parent()); qNotifyDebug() << table->columnCount(); qNotifyDebug() << table->rowCount(); qNotifyDebug() << table->currentRow(); } -QSize NotifyItemDelegate::sizeHint(const QStyleOptionViewItem& option, const QModelIndex& index) const +QSize NotifyItemDelegate::sizeHint(const QStyleOptionViewItem & option, const QModelIndex & index) const { QSize s = QItemDelegate::sizeHint(option, index); + s.setHeight(10); return s; } diff --git a/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.h b/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.h index cdebd4928..904cc2d29 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.h +++ b/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.h @@ -32,25 +32,24 @@ #include -class NotifyItemDelegate : public QItemDelegate -{ +class NotifyItemDelegate : public QItemDelegate { Q_OBJECT public: NotifyItemDelegate(QObject *parent = 0); QWidget *createEditor(QWidget *parent, const QStyleOptionViewItem &, - const QModelIndex &index) const; + const QModelIndex &index) const; void setEditorData(QWidget *editor, const QModelIndex &index) const; void setModelData(QWidget *editor, QAbstractItemModel *model, - const QModelIndex &index) const; - QSize sizeHint ( const QStyleOptionViewItem & option, const QModelIndex & index ) const; + const QModelIndex &index) const; + QSize sizeHint(const QStyleOptionViewItem & option, const QModelIndex & index) const; private slots: void selectRow(const QString & text); void commitAndCloseEditor(); private: - QObject* _parent; + QObject *_parent; }; #endif // NOTIFYITEMDELEGATE_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifylogging.h b/ground/openpilotgcs/src/plugins/notify/notifylogging.h index 43e54f3b7..9fb58906f 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifylogging.h +++ b/ground/openpilotgcs/src/plugins/notify/notifylogging.h @@ -41,6 +41,8 @@ QDebug qNotifyDebug(); QNoDebug qNotifyDebug(); #endif -#define qNotifyDebug_if(test) if(test) qNotifyDebug() +#define qNotifyDebug_if(test) \ + if (test) \ + qNotifyDebug() #endif // NOTIFYLOGGING_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp b/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp index 19f38b626..72920e549 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp @@ -3,25 +3,25 @@ * * @file notifyplugin.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 * @defgroup notifyplugin * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,7 +42,7 @@ static const QString VERSION = "1.0.0"; -//#define DEBUG_NOTIFIES +// #define DEBUG_NOTIFIES SoundNotifyPlugin::SoundNotifyPlugin() @@ -53,33 +53,36 @@ SoundNotifyPlugin::SoundNotifyPlugin() SoundNotifyPlugin::~SoundNotifyPlugin() { Core::ICore::instance()->saveSettings(this); - if (phonon.mo != NULL) + + if (phonon.mo != NULL) { delete phonon.mo; + } } -bool SoundNotifyPlugin::initialize(const QStringList& args, QString *errMsg) -{ - Q_UNUSED(args); - Q_UNUSED(errMsg); +bool SoundNotifyPlugin::initialize(const QStringList & args, QString *errMsg) +{ + Q_UNUSED(args); + Q_UNUSED(errMsg); - mop = new NotifyPluginOptionsPage(this); - addAutoReleasedObject(mop); + mop = new NotifyPluginOptionsPage(this); + addAutoReleasedObject(mop); - return true; -} + return true; +} void SoundNotifyPlugin::extensionsInitialized() -{ +{ Core::ICore::instance()->readSettings(this); - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); - connect(pm, SIGNAL(objectAdded(QObject*)), this, SLOT(onTelemetryManagerAdded(QObject*))); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + + connect(pm, SIGNAL(objectAdded(QObject *)), this, SLOT(onTelemetryManagerAdded(QObject *))); _toRemoveNotifications.clear(); connectNotifications(); -} - -void SoundNotifyPlugin::saveConfig( QSettings* settings, UAVConfigInfo *configInfo){ +} +void SoundNotifyPlugin::saveConfig(QSettings *settings, UAVConfigInfo *configInfo) +{ configInfo->setVersion(VERSION); settings->beginWriteArray("Current"); @@ -98,13 +101,12 @@ void SoundNotifyPlugin::saveConfig( QSettings* settings, UAVConfigInfo *configIn } settings->endArray(); settings->setValue(QLatin1String("EnableSound"), enableSound); - } -void SoundNotifyPlugin::readConfig( QSettings* settings, UAVConfigInfo * /* configInfo */) +void SoundNotifyPlugin::readConfig(QSettings *settings, UAVConfigInfo * /* configInfo */) { // Just for migration to the new format. - //Q_ASSERT(configInfo->version() == UAVConfigVersion()); + // Q_ASSERT(configInfo->version() == UAVConfigVersion()); settings->beginReadArray("Current"); settings->setArrayIndex(0); @@ -115,24 +117,25 @@ void SoundNotifyPlugin::readConfig( QSettings* settings, UAVConfigInfo * /* conf int size = settings->beginReadArray("listNotifies"); for (int i = 0; i < size; ++i) { settings->setArrayIndex(i); - NotificationItem* notification = new NotificationItem; + NotificationItem *notification = new NotificationItem; notification->restoreState(settings); _notificationList.append(notification); } settings->endArray(); - setEnableSound(settings->value(QLatin1String("EnableSound"),0).toBool()); + setEnableSound(settings->value(QLatin1String("EnableSound"), 0).toBool()); } -void SoundNotifyPlugin::onTelemetryManagerAdded(QObject* obj) +void SoundNotifyPlugin::onTelemetryManagerAdded(QObject *obj) { telMngr = qobject_cast(obj); - if (telMngr) + if (telMngr) { connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); + } } void SoundNotifyPlugin::shutdown() -{ - // Do nothing +{ + // Do nothing } void SoundNotifyPlugin::onAutopilotDisconnect() @@ -146,8 +149,8 @@ void SoundNotifyPlugin::onAutopilotDisconnect() */ void SoundNotifyPlugin::resetNotification(void) { - //first, reject empty args and unknown fields. - foreach(NotificationItem* ntf, _notificationList) { + // first, reject empty args and unknown fields. + foreach(NotificationItem * ntf, _notificationList) { ntf->disposeTimer(); disconnect(ntf->getTimer(), SIGNAL(timeout()), this, SLOT(on_timerRepeated_Notification())); ntf->disposeExpireTimer(); @@ -159,12 +162,12 @@ void SoundNotifyPlugin::resetNotification(void) update list of notifies; will be perform on OK or APPLY press of option page */ -void SoundNotifyPlugin::updateNotificationList(QList list) +void SoundNotifyPlugin::updateNotificationList(QList list) { _toRemoveNotifications.clear(); resetNotification(); _notificationList.clear(); - _notificationList=list; + _notificationList = list; connectNotifications(); Core::ICore::instance()->saveSettings(this); @@ -172,16 +175,19 @@ void SoundNotifyPlugin::updateNotificationList(QList list) void SoundNotifyPlugin::connectNotifications() { - foreach(UAVDataObject* obj,lstNotifiedUAVObjects) { - if (obj != NULL) - disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(on_arrived_Notification(UAVObject*))); + foreach(UAVDataObject * obj, lstNotifiedUAVObjects) { + if (obj != NULL) { + disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(on_arrived_Notification(UAVObject *))); + } } if (phonon.mo != NULL) { delete phonon.mo; phonon.mo = NULL; } - if (!enableSound) return; + if (!enableSound) { + return; + } ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); @@ -191,23 +197,27 @@ void SoundNotifyPlugin::connectNotifications() _notificationList.append(_toRemoveNotifications); _toRemoveNotifications.clear(); - //first, reject empty args and unknown fields. - foreach(NotificationItem* notify, _notificationList) { - notify->_isPlayed = false; - notify->isNowPlaying=false; + // first, reject empty args and unknown fields. + foreach(NotificationItem * notify, _notificationList) { + notify->_isPlayed = false; + notify->isNowPlaying = false; - if(notify->mute()) continue; + if (notify->mute()) { + continue; + } // check is all sounds presented for notification, // if not - we must not subscribe to it at all - if(notify->toSoundList().isEmpty()) continue; + if (notify->toSoundList().isEmpty()) { + continue; + } - UAVDataObject* obj = dynamic_cast( objManager->getObject(notify->getDataObject()) ); - if (obj != NULL ) { + UAVDataObject *obj = dynamic_cast(objManager->getObject(notify->getDataObject())); + if (obj != NULL) { if (!lstNotifiedUAVObjects.contains(obj)) { lstNotifiedUAVObjects.append(obj); - connect(obj, SIGNAL(objectUpdated(UAVObject*)), - this, SLOT(on_arrived_Notification(UAVObject*)), + connect(obj, SIGNAL(objectUpdated(UAVObject *)), + this, SLOT(on_arrived_Notification(UAVObject *)), Qt::QueuedConnection); } } else { @@ -215,64 +225,71 @@ void SoundNotifyPlugin::connectNotifications() } } - if (_notificationList.isEmpty()) return; + if (_notificationList.isEmpty()) { + return; + } // set notification message to current event phonon.mo = Phonon::createPlayer(Phonon::NotificationCategory); phonon.mo->clearQueue(); phonon.firstPlay = true; QList audioOutputDevices = - Phonon::BackendCapabilities::availableAudioOutputDevices(); + Phonon::BackendCapabilities::availableAudioOutputDevices(); foreach(Phonon::AudioOutputDevice dev, audioOutputDevices) { qNotifyDebug() << "Notify: Audio Output device: " << dev.name() << " - " << dev.description(); } - connect(phonon.mo, SIGNAL(stateChanged(Phonon::State,Phonon::State)), - this, SLOT(stateChanged(Phonon::State,Phonon::State))); + connect(phonon.mo, SIGNAL(stateChanged(Phonon::State, Phonon::State)), + this, SLOT(stateChanged(Phonon::State, Phonon::State))); } void SoundNotifyPlugin::on_arrived_Notification(UAVObject *object) { - foreach(NotificationItem* ntf, _notificationList) { - if (object->getName() != ntf->getDataObject()) + foreach(NotificationItem * ntf, _notificationList) { + if (object->getName() != ntf->getDataObject()) { continue; + } // skip duplicate notifications - if (_nowPlayingNotification == ntf) + if (_nowPlayingNotification == ntf) { continue; + } // skip periodical notifications // this condition accepts: // 1. Periodical notifications played firstly; - // NOTE: At first time it will be played, then it played only by timer, - // when conditions became false firstStart flag has been cleared and - // notification can be accepted again; + // NOTE: At first time it will be played, then it played only by timer, + // when conditions became false firstStart flag has been cleared and + // notification can be accepted again; // 2. Once time notifications, they removed immediately after first playing; // 3. Instant notifications(played one by one without interval); if (ntf->retryValue() != NotificationItem::repeatInstantly && ntf->retryValue() != NotificationItem::repeatOncePerUpdate && - ntf->retryValue() != NotificationItem::repeatOnce && ntf->_isPlayed) - continue; + ntf->retryValue() != NotificationItem::repeatOnce && ntf->_isPlayed) { + continue; + } qNotifyDebug() << QString("new notification: | %1 | %2 | val1: %3 | val2: %4") - .arg(ntf->getDataObject()) - .arg(ntf->getObjectField()) - .arg(ntf->singleValue().toString()) - .arg(ntf->valueRange2()); + .arg(ntf->getDataObject()) + .arg(ntf->getObjectField()) + .arg(ntf->singleValue().toString()) + .arg(ntf->valueRange2()); checkNotificationRule(ntf, object); } - connect(object, SIGNAL(objectUpdated(UAVObject*)), - this, SLOT(on_arrived_Notification(UAVObject*)), Qt::UniqueConnection); + connect(object, SIGNAL(objectUpdated(UAVObject *)), + this, SLOT(on_arrived_Notification(UAVObject *)), Qt::UniqueConnection); } void SoundNotifyPlugin::on_timerRepeated_Notification() { - NotificationItem* notification = static_cast(sender()->parent()); - if (!notification) + NotificationItem *notification = static_cast(sender()->parent()); + + if (!notification) { return; + } // skip duplicate notifications // WARNING: generally we shoudn't ever trap here - // this means, that timer fires to early and notification overlap itself + // this means, that timer fires to early and notification overlap itself if (_nowPlayingNotification == notification) { qNotifyDebug() << "WARN: on_timerRepeated - notification was skipped!"; notification->restartTimer(); @@ -280,29 +297,32 @@ void SoundNotifyPlugin::on_timerRepeated_Notification() } qNotifyDebug() << QString("repeatTimer: %1% | %2 | %3").arg(notification->getDataObject()) - .arg(notification->getObjectField()) - .arg(notification->toString()); + .arg(notification->getObjectField()) + .arg(notification->toString()); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVObject* object = objManager->getObject(notification->getDataObject()); - if (object) - checkNotificationRule(notification,object); + UAVObject *object = objManager->getObject(notification->getDataObject()); + if (object) { + checkNotificationRule(notification, object); + } } void SoundNotifyPlugin::on_expiredTimer_Notification() { // fire expire timer - NotificationItem* notification = static_cast(sender()->parent()); - if(!notification) + NotificationItem *notification = static_cast(sender()->parent()); + + if (!notification) { return; + } notification->stopExpireTimer(); if (!_pendingNotifications.isEmpty()) { qNotifyDebug() << QString("expireTimer: %1% | %2 | %3").arg(notification->getDataObject()) - .arg(notification->getObjectField()) - .arg(notification->toString()); + .arg(notification->getObjectField()) + .arg(notification->toString()); _pendingNotifications.removeOne(notification); } @@ -312,19 +332,19 @@ void SoundNotifyPlugin::stateChanged(Phonon::State newstate, Phonon::State oldst { Q_UNUSED(oldstate) - //qNotifyDebug() << "File length (ms): " << phonon.mo->totalTime(); + // qNotifyDebug() << "File length (ms): " << phonon.mo->totalTime(); #ifndef Q_OS_WIN // This is a hack to force Linux to wait until the end of the // wav file before moving to the next in the queue. // I wish I did not have to go through a #define, but I did not // manage to make this work on both platforms any other way! - if (phonon.mo->totalTime()>0) + if (phonon.mo->totalTime() > 0) { phonon.mo->setTransitionTime(phonon.mo->totalTime()); + } #endif - if ((newstate == Phonon::PausedState) || - (newstate == Phonon::StoppedState)) - { + if ((newstate == Phonon::PausedState) || + (newstate == Phonon::StoppedState)) { qNotifyDebug() << "New State: " << QVariant(newstate).toString(); // assignment to NULL needed to detect that palying is finished @@ -332,16 +352,15 @@ void SoundNotifyPlugin::stateChanged(Phonon::State newstate, Phonon::State oldst // that notification has not overlap with itself _nowPlayingNotification = NULL; - if (!_pendingNotifications.isEmpty()) - { - NotificationItem* notification = _pendingNotifications.takeFirst(); + if (!_pendingNotifications.isEmpty()) { + NotificationItem *notification = _pendingNotifications.takeFirst(); qNotifyDebug_if(notification) << "play audioFree - " << notification->toString(); playNotification(notification); - qNotifyDebug()<<"end playNotification"; + qNotifyDebug() << "end playNotification"; } } else { - if (newstate == Phonon::ErrorState) { - if (phonon.mo->errorType()==0) { + if (newstate == Phonon::ErrorState) { + if (phonon.mo->errorType() == 0) { qDebug() << "Phonon::ErrorState: ErrorType = " << phonon.mo->errorType(); phonon.mo->clearQueue(); } @@ -351,10 +370,9 @@ void SoundNotifyPlugin::stateChanged(Phonon::State newstate, Phonon::State oldst bool checkRange(QString fieldValue, QString enumValue, QStringList /* values */, int direction) { - bool ret = false; - switch(direction) - { + + switch (direction) { case NotifyPluginOptionsPage::equal: ret = !QString::compare(enumValue, fieldValue, Qt::CaseInsensitive) ? true : false; break; @@ -362,16 +380,17 @@ bool checkRange(QString fieldValue, QString enumValue, QStringList /* values */, default: ret = true; break; - }; + } + ; return ret; } bool checkRange(double fieldValue, double min, double max, int direction) { bool ret = false; - //Q_ASSERT(min < max); - switch(direction) - { + + // Q_ASSERT(min < max); + switch (direction) { case NotifyPluginOptionsPage::equal: ret = (fieldValue == min); break; @@ -387,44 +406,48 @@ bool checkRange(double fieldValue, double min, double max, int direction) default: ret = (fieldValue > min) && (fieldValue < max); break; - }; + } + ; return ret; } -void SoundNotifyPlugin::checkNotificationRule(NotificationItem* notification, UAVObject* object) +void SoundNotifyPlugin::checkNotificationRule(NotificationItem *notification, UAVObject *object) { - if(notification->getDataObject()!=object->getName() || object->getField(notification->getObjectField())==NULL) + if (notification->getDataObject() != object->getName() || object->getField(notification->getObjectField()) == NULL) { return; - bool condition=false; + } + bool condition = false; - if (notification->mute()) + if (notification->mute()) { return; + } - int direction = notification->getCondition(); - QString fieldName = notification->getObjectField(); - UAVObjectField* field = object->getField(fieldName); + int direction = notification->getCondition(); + QString fieldName = notification->getObjectField(); + UAVObjectField *field = object->getField(fieldName); - if (field->getName().isEmpty()) + if (field->getName().isEmpty()) { return; + } QVariant value = field->getValue(); - if(UAVObjectField::ENUM == field->getType()) { - qNotifyDebug()<<"Check range ENUM"<singleValue().toString()<<"|"<getOptions()<<"|"<< - direction<singleValue().toString(), - field->getOptions(), - direction);; + if (UAVObjectField::ENUM == field->getType()) { + qNotifyDebug() << "Check range ENUM" << value.toString() << "|" << notification->singleValue().toString() << "|" << field->getOptions() << "|" << + direction << checkRange(value.toString(), + notification->singleValue().toString(), + field->getOptions(), + direction);; condition = checkRange(value.toString(), notification->singleValue().toString(), field->getOptions(), direction); } else { - qNotifyDebug()<<"Check range VAL"<singleValue().toString()<<"|"<getOptions()<<"|"<< - direction<singleValue().toDouble(), - notification->valueRange2(), - direction); + qNotifyDebug() << "Check range VAL" << value.toString() << "|" << notification->singleValue().toString() << "|" << field->getOptions() << "|" << + direction << checkRange(value.toDouble(), + notification->singleValue().toDouble(), + notification->valueRange2(), + direction); condition = checkRange(value.toDouble(), notification->singleValue().toDouble(), notification->valueRange2(), @@ -439,18 +462,19 @@ void SoundNotifyPlugin::checkNotificationRule(NotificationItem* notification, UA notification->setCurrentUpdatePlayed(false); return; } - if(notification->retryValue() == NotificationItem::repeatOncePerUpdate && notification->getCurrentUpdatePlayed()) + if (notification->retryValue() == NotificationItem::repeatOncePerUpdate && notification->getCurrentUpdatePlayed()) { return; + } if (!playNotification(notification)) { if (!_pendingNotifications.contains(notification) - && (_nowPlayingNotification != notification)) { + && (_nowPlayingNotification != notification)) { notification->stopTimer(); qNotifyDebug() << "add to pending list - " << notification->toString(); // if audio is busy, start expiration timer - //ms = (notification->getExpiredTimeout()[in sec])*1000 - //QxtTimer::singleShot(notification->getExpireTimeout()*1000, this, SLOT(expirationTimerHandler(NotificationItem*)), qVariantFromValue(notification)); + // ms = (notification->getExpiredTimeout()[in sec])*1000 + // QxtTimer::singleShot(notification->getExpireTimeout()*1000, this, SLOT(expirationTimerHandler(NotificationItem*)), qVariantFromValue(notification)); _pendingNotifications.append(notification); notification->startExpireTimer(); connect(notification->getExpireTimer(), SIGNAL(timeout()), @@ -459,34 +483,34 @@ void SoundNotifyPlugin::checkNotificationRule(NotificationItem* notification, UA } } -bool SoundNotifyPlugin::playNotification(NotificationItem* notification) +bool SoundNotifyPlugin::playNotification(NotificationItem *notification) { - if(!notification) + if (!notification) { return false; + } // Check: race condition, if phonon.mo got deleted don't go further - if (phonon.mo == NULL) + if (phonon.mo == NULL) { return false; + } - //qNotifyDebug() << "Phonon State: " << phonon.mo->state(); + // qNotifyDebug() << "Phonon State: " << phonon.mo->state(); - if ((phonon.mo->state()==Phonon::PausedState) - || (phonon.mo->state()==Phonon::StoppedState) - || phonon.firstPlay) - { + if ((phonon.mo->state() == Phonon::PausedState) + || (phonon.mo->state() == Phonon::StoppedState) + || phonon.firstPlay) { _nowPlayingNotification = notification; notification->stopExpireTimer(); if (notification->retryValue() == NotificationItem::repeatOnce) { _toRemoveNotifications.append(_notificationList.takeAt(_notificationList.indexOf(notification))); - } - else if(notification->retryValue() == NotificationItem::repeatOncePerUpdate) + } else if (notification->retryValue() == NotificationItem::repeatOncePerUpdate) { notification->setCurrentUpdatePlayed(true); - else { + } else { if (notification->retryValue() != NotificationItem::repeatInstantly) { QRegExp rxlen("(\\d+)"); QString value; - int timer_value=0; + int timer_value = 0; int pos = rxlen.indexIn(NotificationItem::retryValues.at(notification->retryValue())); if (pos > -1) { value = rxlen.cap(1); // "189" @@ -509,18 +533,18 @@ bool SoundNotifyPlugin::playNotification(NotificationItem* notification) } phonon.mo->clear(); qNotifyDebug() << "play: " << notification->toString(); - foreach (QString item, notification->toSoundList()) { + foreach(QString item, notification->toSoundList()) { Phonon::MediaSource *ms = new Phonon::MediaSource(item); + ms->setAutoDelete(true); phonon.mo->enqueue(*ms); } - qNotifyDebug()<<"begin play"; + qNotifyDebug() << "begin play"; phonon.mo->play(); - qNotifyDebug()<<"end play"; + qNotifyDebug() << "end play"; phonon.firstPlay = false; // On Linux, you sometimes have to nudge Phonon to play 1 time before // the state is not "Loading" anymore. return true; - } return false; diff --git a/ground/openpilotgcs/src/plugins/notify/notifyplugin.h b/ground/openpilotgcs/src/plugins/notify/notifyplugin.h index c9027bf82..d54126f29 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyplugin.h +++ b/ground/openpilotgcs/src/plugins/notify/notifyplugin.h @@ -3,31 +3,31 @@ * * @file notifyplugin.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 * @defgroup notifyplugin * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef SOUNDNOTIFYPLUGIN_H #define SOUNDNOTIFYPLUGIN_H -#include +#include #include #include "uavtalk/telemetrymanager.h" #include "uavobjectmanager.h" @@ -43,44 +43,55 @@ class NotifyPluginOptionsPage; typedef struct { - Phonon::MediaObject* mo; - NotificationItem* notify; - bool firstPlay; + Phonon::MediaObject *mo; + NotificationItem *notify; + bool firstPlay; } PhononObject, *pPhononObject; -class SoundNotifyPlugin : public Core::IConfigurablePlugin -{ +class SoundNotifyPlugin : public Core::IConfigurablePlugin { Q_OBJECT public: SoundNotifyPlugin(); ~SoundNotifyPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void readConfig( QSettings* qSettings, Core::UAVConfigInfo *configInfo); - void saveConfig( QSettings* qSettings, Core::UAVConfigInfo *configInfo); + bool initialize(const QStringList & arguments, QString *errorString); + void readConfig(QSettings *qSettings, Core::UAVConfigInfo *configInfo); + void saveConfig(QSettings *qSettings, Core::UAVConfigInfo *configInfo); void shutdown(); - QList getListNotifications() { return _notificationList; } - NotificationItem* getCurrentNotification(){ return ¤tNotification;} + QList getListNotifications() + { + return _notificationList; + } + NotificationItem *getCurrentNotification() + { + return ¤tNotification; + } - bool getEnableSound() const { return enableSound; } - void setEnableSound(bool value) {enableSound = value; } + bool getEnableSound() const + { + return enableSound; + } + void setEnableSound(bool value) + { + enableSound = value; + } private: Q_DISABLE_COPY(SoundNotifyPlugin) - bool playNotification(NotificationItem* notification); - void checkNotificationRule(NotificationItem* notification, UAVObject* object); + bool playNotification(NotificationItem *notification); + void checkNotificationRule(NotificationItem *notification, UAVObject *object); private slots: - void onTelemetryManagerAdded(QObject* obj); + void onTelemetryManagerAdded(QObject *obj); void onAutopilotDisconnect(); void connectNotifications(); - void updateNotificationList(QList list); + void updateNotificationList(QList list); void resetNotification(void); void on_arrived_Notification(UAVObject *object); void on_timerRepeated_Notification(void); @@ -90,17 +101,17 @@ private slots: private: bool enableSound; - QList lstNotifiedUAVObjects; - QList _notificationList; - QList _pendingNotifications; - QList _toRemoveNotifications; + QList lstNotifiedUAVObjects; + QList _notificationList; + QList _pendingNotifications; + QList _toRemoveNotifications; NotificationItem currentNotification; - NotificationItem* _nowPlayingNotification; + NotificationItem *_nowPlayingNotification; PhononObject phonon; - NotifyPluginOptionsPage* mop; - TelemetryManager* telMngr; -}; + NotifyPluginOptionsPage *mop; + TelemetryManager *telMngr; +}; #endif // SOUNDNOTIFYPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.cpp b/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.cpp index 81b365741..454fafe94 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.cpp @@ -25,27 +25,25 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "notifypluginfactory.h" -//#include "notifypluginwidget.h" +// #include "notifypluginwidget.h" #include "notifyplugin.h" #include "notifypluginconfiguration.h" #include "notifypluginoptionspage.h" #include NotifyPluginFactory::NotifyPluginFactory(QObject *parent) : - IUAVGadgetFactory(QString("Notify Plugin"), - tr("Notify Plugin"), - parent) -{ -} + IUAVGadgetFactory(QString("Notify Plugin"), + tr("Notify Plugin"), + parent) +{} NotifyPluginFactory::~NotifyPluginFactory() -{ -} +{} -Core::IUAVGadget* NotifyPluginFactory::createGadget(QWidget *parent) +Core::IUAVGadget *NotifyPluginFactory::createGadget(QWidget *parent) { - // NotifyPluginWidget* gadgetWidget = new NotifyPluginWidget(parent); - return (Core::IUAVGadget*)0;//new NotifyPlugin(QString("NotifyPlugin"), gadgetWidget, parent); + // NotifyPluginWidget* gadgetWidget = new NotifyPluginWidget(parent); + return (Core::IUAVGadget *)0; // new NotifyPlugin(QString("NotifyPlugin"), gadgetWidget, parent); } IUAVGadgetConfiguration *NotifyPluginFactory::createConfiguration(const QByteArray &state) @@ -55,7 +53,5 @@ IUAVGadgetConfiguration *NotifyPluginFactory::createConfiguration(const QByteArr IOptionsPage *NotifyPluginFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new NotifyPluginOptionsPage(qobject_cast(config)); + return new NotifyPluginOptionsPage(qobject_cast(config)); } - - diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.h b/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.h index 8784d3943..a14d930e8 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.h +++ b/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.h @@ -38,8 +38,7 @@ class IUAVGadgetFactory; using namespace Core; -class NotifyPluginFactory : public IUAVGadgetFactory -{ +class NotifyPluginFactory : public IUAVGadgetFactory { Q_OBJECT public: NotifyPluginFactory(QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/notify/notifyplugingadget.h b/ground/openpilotgcs/src/plugins/notify/notifyplugingadget.h index f6aa2f912..ec780ad54 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyplugingadget.h +++ b/ground/openpilotgcs/src/plugins/notify/notifyplugingadget.h @@ -29,27 +29,26 @@ #define NOTIFYPLUGINGADGET_H #include -//#include "NotifyPlugingadgetwidget.h" +// #include "NotifyPlugingadgetwidget.h" class IUAVGadget; class QWidget; class QString; -//class NotifyPluginGadgetWidget; +// class NotifyPluginGadgetWidget; using namespace Core; -class NotifyPluginGadget : public Core::IUAVGadget -{ +class NotifyPluginGadget : public Core::IUAVGadget { Q_OBJECT public: NotifyPluginGadget(QString classId, NotifyPluginGadgetWidget *widget, QWidget *parent = 0); ~NotifyPluginGadget(); - // QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + // QWidget *widget() { return m_widget; } + void loadConfiguration(IUAVGadgetConfiguration *config); private: -// NotifyPluginGadgetWidget *m_widget; +// NotifyPluginGadgetWidget *m_widget; }; diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp index 38df0eb52..7ef248ad2 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp @@ -52,7 +52,7 @@ QStringList NotifyPluginOptionsPage::conditionValues; NotifyPluginOptionsPage::NotifyPluginOptionsPage(QObject *parent) : IOptionsPage(parent) , _objManager(*ExtensionSystem::PluginManager::instance()->getObject()) - , _owner(qobject_cast(parent)) + , _owner(qobject_cast(parent)) , _dynamicFieldCondition(NULL) , _dynamicFieldWidget(NULL) , _dynamicFieldType(-1) @@ -60,11 +60,10 @@ NotifyPluginOptionsPage::NotifyPluginOptionsPage(QObject *parent) , _form(NULL) , _selectedNotification(NULL) { - NotifyPluginOptionsPage::conditionValues.insert(equal,tr("Equal to")); - NotifyPluginOptionsPage::conditionValues.insert(bigger,tr("Large than")); - NotifyPluginOptionsPage::conditionValues.insert(smaller,tr("Lower than")); - NotifyPluginOptionsPage::conditionValues.insert(inrange,tr("In range")); - + NotifyPluginOptionsPage::conditionValues.insert(equal, tr("Equal to")); + NotifyPluginOptionsPage::conditionValues.insert(bigger, tr("Large than")); + NotifyPluginOptionsPage::conditionValues.insert(smaller, tr("Lower than")); + NotifyPluginOptionsPage::conditionValues.insert(inrange, tr("In range")); } NotifyPluginOptionsPage::~NotifyPluginOptionsPage() @@ -73,27 +72,27 @@ NotifyPluginOptionsPage::~NotifyPluginOptionsPage() QWidget *NotifyPluginOptionsPage::createPage(QWidget * /* parent */) { _optionsPage.reset(new Ui::NotifyPluginOptionsPage()); - //main widget - QWidget* optionsPageWidget = new QWidget; - _dynamicFieldWidget = NULL; + // main widget + QWidget *optionsPageWidget = new QWidget; + _dynamicFieldWidget = NULL; _dynamicFieldCondition = NULL; resetFieldType(); - //save ref to form, needed for binding dynamic fields in future + // save ref to form, needed for binding dynamic fields in future _form = optionsPageWidget; - //main layout + // main layout _optionsPage->setupUi(optionsPageWidget); _optionsPage->SoundDirectoryPathChooser->setExpectedKind(Utils::PathChooser::Directory); _optionsPage->SoundDirectoryPathChooser->setPromptDialogTitle(tr("Choose sound collection directory")); - connect(_optionsPage->SoundDirectoryPathChooser, SIGNAL(changed(const QString&)), - this, SLOT(on_clicked_buttonSoundFolder(const QString&))); - connect(_optionsPage->SoundCollectionList, SIGNAL(currentIndexChanged (int)), + connect(_optionsPage->SoundDirectoryPathChooser, SIGNAL(changed(const QString &)), + this, SLOT(on_clicked_buttonSoundFolder(const QString &))); + connect(_optionsPage->SoundCollectionList, SIGNAL(currentIndexChanged(int)), this, SLOT(on_changedIndex_soundLanguage(int))); - connect(this, SIGNAL(updateNotifications(QList)), - _owner, SLOT(updateNotificationList(QList))); - //connect(this, SIGNAL(resetNotification()),owner, SLOT(resetNotification())); + connect(this, SIGNAL(updateNotifications(QList)), + _owner, SLOT(updateNotificationList(QList))); + // connect(this, SIGNAL(resetNotification()),owner, SLOT(resetNotification())); _privListNotifications = _owner->getListNotifications(); @@ -110,7 +109,7 @@ QWidget *NotifyPluginOptionsPage::createPage(QWidget * /* parent */) int curr_row = _privListNotifications.indexOf(_selectedNotification); _notifyRulesSelection->setCurrentIndex(_notifyRulesModel->index(curr_row, 0, QModelIndex()), - QItemSelectionModel::ClearAndSelect | QItemSelectionModel::Rows); + QItemSelectionModel::ClearAndSelect | QItemSelectionModel::Rows); return optionsPageWidget; } @@ -127,8 +126,8 @@ void NotifyPluginOptionsPage::finish() disconnect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_changedIndex_UAVField(QString))); - disconnect(_testSound.data(),SIGNAL(stateChanged(Phonon::State,Phonon::State)), - this,SLOT(on_changed_playButtonText(Phonon::State,Phonon::State))); + disconnect(_testSound.data(), SIGNAL(stateChanged(Phonon::State, Phonon::State)), + this, SLOT(on_changed_playButtonText(Phonon::State, Phonon::State))); if (_testSound) { _testSound->stop(); _testSound->clear(); @@ -157,8 +156,8 @@ void NotifyPluginOptionsPage::initButtons() void NotifyPluginOptionsPage::initPhononPlayer() { _testSound.reset(Phonon::createPlayer(Phonon::NotificationCategory)); - connect(_testSound.data(),SIGNAL(stateChanged(Phonon::State,Phonon::State)), - this,SLOT(on_changed_playButtonText(Phonon::State,Phonon::State))); + connect(_testSound.data(), SIGNAL(stateChanged(Phonon::State, Phonon::State)), + this, SLOT(on_changed_playButtonText(Phonon::State, Phonon::State))); connect(_testSound.data(), SIGNAL(finished(void)), this, SLOT(on_FinishedPlaying(void))); } @@ -169,8 +168,8 @@ void NotifyPluginOptionsPage::initRulesTable() _notifyRulesModel.reset(new NotifyTableModel(_privListNotifications)); _notifyRulesSelection = new QItemSelectionModel(_notifyRulesModel.data()); - connect(_notifyRulesSelection, SIGNAL(selectionChanged ( const QItemSelection &, const QItemSelection & )), - this, SLOT(on_changedSelection_notifyTable( const QItemSelection & , const QItemSelection & ))); + connect(_notifyRulesSelection, SIGNAL(selectionChanged(const QItemSelection &, const QItemSelection &)), + this, SLOT(on_changedSelection_notifyTable(const QItemSelection &, const QItemSelection &))); connect(this, SIGNAL(entryUpdated(int)), _notifyRulesModel.data(), SLOT(entryUpdated(int))); @@ -179,26 +178,26 @@ void NotifyPluginOptionsPage::initRulesTable() _optionsPage->notifyRulesView->setItemDelegate(new NotifyItemDelegate(this)); _optionsPage->notifyRulesView->resizeRowsToContents(); - _optionsPage->notifyRulesView->setColumnWidth(eMessageName,200); - _optionsPage->notifyRulesView->setColumnWidth(eRepeatValue,120); - _optionsPage->notifyRulesView->setColumnWidth(eExpireTimer,100); - _optionsPage->notifyRulesView->setColumnWidth(eTurnOn,60); + _optionsPage->notifyRulesView->setColumnWidth(eMessageName, 200); + _optionsPage->notifyRulesView->setColumnWidth(eRepeatValue, 120); + _optionsPage->notifyRulesView->setColumnWidth(eExpireTimer, 100); + _optionsPage->notifyRulesView->setColumnWidth(eTurnOn, 60); _optionsPage->notifyRulesView->setDragEnabled(true); _optionsPage->notifyRulesView->setAcceptDrops(true); _optionsPage->notifyRulesView->setDropIndicatorShown(true); _optionsPage->notifyRulesView->setDragDropMode(QAbstractItemView::InternalMove); } -UAVObjectField* NotifyPluginOptionsPage::getObjectFieldFromSelected() +UAVObjectField *NotifyPluginOptionsPage::getObjectFieldFromSelected() { return (_currUAVObject) ? _currUAVObject->getField(_selectedNotification->getObjectField()) : NULL; } -void NotifyPluginOptionsPage::setSelectedNotification(NotificationItem* ntf) +void NotifyPluginOptionsPage::setSelectedNotification(NotificationItem *ntf) { _selectedNotification = ntf; - _currUAVObject = dynamic_cast(_objManager.getObject(_selectedNotification->getDataObject())); - if(!_currUAVObject) { + _currUAVObject = dynamic_cast(_objManager.getObject(_selectedNotification->getDataObject())); + if (!_currUAVObject) { qNotifyDebug() << "no such UAVObject: " << _selectedNotification->getDataObject(); } } @@ -209,12 +208,13 @@ void NotifyPluginOptionsPage::addDynamicFieldLayout() // thus it doesn't use in this field directly QSizePolicy labelSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + labelSizePolicy.setHorizontalStretch(0); labelSizePolicy.setVerticalStretch(0); -// labelSizePolicy.setHeightForWidth(UAVObject->sizePolicy().hasHeightForWidth()); +// labelSizePolicy.setHeightForWidth(UAVObject->sizePolicy().hasHeightForWidth()); - QLabel* labelSayOrder = new QLabel("Say order ", _form); + QLabel *labelSayOrder = new QLabel("Say order ", _form); labelSayOrder->setSizePolicy(labelSizePolicy); _optionsPage->dynamicValueLayout->addWidget(labelSayOrder); @@ -222,24 +222,24 @@ void NotifyPluginOptionsPage::addDynamicFieldLayout() _optionsPage->dynamicValueLayout->addWidget(_sayOrder); _sayOrder->addItems(NotificationItem::sayOrderValues); - QLabel* labelValueIs = new QLabel("Value is ", _form); + QLabel *labelValueIs = new QLabel("Value is ", _form); labelValueIs->setSizePolicy(labelSizePolicy); _optionsPage->dynamicValueLayout->addWidget(labelValueIs); _dynamicFieldCondition = new QComboBox(_form); _optionsPage->dynamicValueLayout->addWidget(_dynamicFieldCondition); - UAVObjectField* field = getObjectFieldFromSelected(); + UAVObjectField *field = getObjectFieldFromSelected(); addDynamicField(field); } -void NotifyPluginOptionsPage::addDynamicField(UAVObjectField* objField) +void NotifyPluginOptionsPage::addDynamicField(UAVObjectField *objField) { - if(!objField) { + if (!objField) { qNotifyDebug() << "addDynamicField | input objField == NULL"; return; } if (objField->getType() == _dynamicFieldType) { - if (QComboBox* fieldValue = dynamic_cast(_dynamicFieldWidget)) { + if (QComboBox * fieldValue = dynamic_cast(_dynamicFieldWidget)) { fieldValue->clear(); QStringList enumValues(objField->getOptions()); fieldValue->addItems(enumValues); @@ -256,20 +256,21 @@ void NotifyPluginOptionsPage::addDynamicField(UAVObjectField* objField) _dynamicFieldCondition->removeItem(smaller); _dynamicFieldCondition->removeItem(bigger); } - int cond=_selectedNotification->getCondition(); - if(cond<0) + int cond = _selectedNotification->getCondition(); + if (cond < 0) { return; + } _dynamicFieldCondition->setCurrentIndex(_dynamicFieldCondition->findText(NotifyPluginOptionsPage::conditionValues.at(cond))); - connect(_dynamicFieldCondition, SIGNAL(currentIndexChanged(QString)), - this, SLOT(on_changedIndex_rangeValue(QString))); + connect(_dynamicFieldCondition, SIGNAL(currentIndexChanged(QString)), + this, SLOT(on_changedIndex_rangeValue(QString))); addDynamicFieldWidget(objField); } -void NotifyPluginOptionsPage::addDynamicFieldWidget(UAVObjectField* objField) +void NotifyPluginOptionsPage::addDynamicFieldWidget(UAVObjectField *objField) { - if(!objField) { + if (!objField) { qNotifyDebug() << "objField == NULL!"; return; } @@ -286,49 +287,50 @@ void NotifyPluginOptionsPage::addDynamicFieldWidget(UAVObjectField* objField) sizePolicy.setVerticalStretch(0); _dynamicFieldType = objField->getType(); - switch(_dynamicFieldType) - { + switch (_dynamicFieldType) { case UAVObjectField::ENUM: - { - _dynamicFieldWidget = new QComboBox(_form); - QStringList enumValues(objField->getOptions()); - (dynamic_cast(_dynamicFieldWidget))->addItems(enumValues); - } - break; + { + _dynamicFieldWidget = new QComboBox(_form); + QStringList enumValues(objField->getOptions()); + (dynamic_cast(_dynamicFieldWidget))->addItems(enumValues); + } + break; default: Q_ASSERT(_dynamicFieldCondition); if (NotifyPluginOptionsPage::conditionValues.indexOf(_dynamicFieldCondition->currentText()) == NotifyPluginOptionsPage::inrange) { _dynamicFieldWidget = new QLineEdit(_form); - (static_cast(_dynamicFieldWidget))->setInputMask("#999.99 : #999.99;"); - (static_cast(_dynamicFieldWidget))->setText("0000000000"); - (static_cast(_dynamicFieldWidget))->setCursorPosition(0); + (static_cast(_dynamicFieldWidget))->setInputMask("#999.99 : #999.99;"); + (static_cast(_dynamicFieldWidget))->setText("0000000000"); + (static_cast(_dynamicFieldWidget))->setCursorPosition(0); } else { _dynamicFieldWidget = new QDoubleSpinBox(_form); - (dynamic_cast(_dynamicFieldWidget))->setRange(-999.99, 999.99); + (dynamic_cast(_dynamicFieldWidget))->setRange(-999.99, 999.99); } break; - }; + } + ; enum { eDynamicFieldWidth = 100 }; _dynamicFieldWidget->setSizePolicy(sizePolicy); _dynamicFieldWidget->setFixedWidth(eDynamicFieldWidth); _optionsPage->dynamicValueLayout->addWidget(_dynamicFieldWidget); } -void NotifyPluginOptionsPage::setDynamicFieldValue(NotificationItem* notification) +void NotifyPluginOptionsPage::setDynamicFieldValue(NotificationItem *notification) { - if (QDoubleSpinBox* seValue = dynamic_cast(_dynamicFieldWidget)) + if (QDoubleSpinBox * seValue = dynamic_cast(_dynamicFieldWidget)) { seValue->setValue(notification->singleValue().toDouble()); - else { - if (QComboBox* cbValue = dynamic_cast(_dynamicFieldWidget)) { + } else { + if (QComboBox * cbValue = dynamic_cast(_dynamicFieldWidget)) { int idx = cbValue->findText(notification->singleValue().toString()); - if(-1 != idx) + if (-1 != idx) { cbValue->setCurrentIndex(idx); + } } else { - if (QLineEdit* rangeValue = dynamic_cast(_dynamicFieldWidget)) { + if (QLineEdit * rangeValue = dynamic_cast(_dynamicFieldWidget)) { QString str = QString("%1%2").arg(notification->singleValue().toDouble(), 5, 'f', 2, '0') - .arg(notification->valueRange2(), 5, 'f', 2, '0'); + .arg(notification->valueRange2(), 5, 'f', 2, '0'); rangeValue->setText(str); } else { qNotifyDebug() << "NotifyPluginOptionsPage::setDynamicValueField | unknown _fieldValue: " << _dynamicFieldWidget; @@ -342,7 +344,7 @@ void NotifyPluginOptionsPage::resetFieldType() _dynamicFieldType = -1; } -void NotifyPluginOptionsPage::getOptionsPageValues(NotificationItem* notification) +void NotifyPluginOptionsPage::getOptionsPageValues(NotificationItem *notification) { Q_ASSERT(notification); notification->setSoundCollectionPath(_optionsPage->SoundDirectoryPathChooser->path()); @@ -354,13 +356,13 @@ void NotifyPluginOptionsPage::getOptionsPageValues(NotificationItem* notificatio notification->setSound3(_optionsPage->Sound3->currentText()); notification->setSayOrder(_sayOrder->currentIndex()); notification->setCondition(NotifyPluginOptionsPage::conditionValues.indexOf(_dynamicFieldCondition->currentText())); - if (QDoubleSpinBox* spinValue = dynamic_cast(_dynamicFieldWidget)) + if (QDoubleSpinBox * spinValue = dynamic_cast(_dynamicFieldWidget)) { notification->setSingleValue(spinValue->value()); - else { - if (QComboBox* comboBoxValue = dynamic_cast(_dynamicFieldWidget)) + } else { + if (QComboBox * comboBoxValue = dynamic_cast(_dynamicFieldWidget)) { notification->setSingleValue(comboBoxValue->currentText()); - else { - if (QLineEdit* rangeValue = dynamic_cast(_dynamicFieldWidget)) { + } else { + if (QLineEdit * rangeValue = dynamic_cast(_dynamicFieldWidget)) { QString str = rangeValue->text(); QStringList range = str.split(':'); notification->setSingleValue(range.at(0).toDouble()); @@ -370,13 +372,13 @@ void NotifyPluginOptionsPage::getOptionsPageValues(NotificationItem* notificatio } } -void NotifyPluginOptionsPage::updateConfigView(NotificationItem* notification) +void NotifyPluginOptionsPage::updateConfigView(NotificationItem *notification) { Q_ASSERT(notification); disconnect(_optionsPage->UAVObject, SIGNAL(currentIndexChanged(QString)), - this, SLOT(on_changedIndex_UAVObject(QString))); + this, SLOT(on_changedIndex_UAVObject(QString))); disconnect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), - this, SLOT(on_changedIndex_UAVField(QString))); + this, SLOT(on_changedIndex_UAVField(QString))); QString path = notification->getSoundCollectionPath(); if (path.isEmpty()) { @@ -392,9 +394,9 @@ void NotifyPluginOptionsPage::updateConfigView(NotificationItem* notification) } // Fills the combo boxes for the UAVObjects - QList< QList > objList = _objManager.getDataObjects(); - foreach (QList list, objList) { - foreach (UAVDataObject* obj, list) { + QList< QList > objList = _objManager.getDataObjects(); + foreach(QList list, objList) { + foreach(UAVDataObject * obj, list) { _optionsPage->UAVObject->addItem(obj->getName()); } } @@ -404,10 +406,10 @@ void NotifyPluginOptionsPage::updateConfigView(NotificationItem* notification) } _optionsPage->UAVObjectField->clear(); - if(_currUAVObject) { - QList fieldList = _currUAVObject->getFields(); - foreach (UAVObjectField* field, fieldList) - _optionsPage->UAVObjectField->addItem(field->getName()); + if (_currUAVObject) { + QList fieldList = _currUAVObject->getFields(); + foreach(UAVObjectField * field, fieldList) + _optionsPage->UAVObjectField->addItem(field->getName()); } if (-1 != _optionsPage->UAVObjectField->findText(notification->getObjectField())) { @@ -437,9 +439,10 @@ void NotifyPluginOptionsPage::updateConfigView(NotificationItem* notification) _optionsPage->SoundCollectionList->setCurrentIndex(_optionsPage->SoundCollectionList->findText("default")); _optionsPage->Sound3->setCurrentIndex(_optionsPage->Sound3->findText(notification->getSound3())); } - int cond=notification->getCondition(); - if(cond<0) + int cond = notification->getCondition(); + if (cond < 0) { return; + } _dynamicFieldCondition->setCurrentIndex(_dynamicFieldCondition->findText(NotifyPluginOptionsPage::conditionValues.at(cond))); _sayOrder->setCurrentIndex(notification->getSayOrder()); @@ -450,13 +453,12 @@ void NotifyPluginOptionsPage::updateConfigView(NotificationItem* notification) this, SLOT(on_changedIndex_UAVObject(QString))); connect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_changedIndex_UAVField(QString))); - } void NotifyPluginOptionsPage::on_changedIndex_rangeValue(QString /* rangeStr */) { Q_ASSERT(_dynamicFieldWidget); - UAVObjectField* field = getObjectFieldFromSelected(); + UAVObjectField *field = getObjectFieldFromSelected(); Q_ASSERT(!!field); addDynamicFieldWidget(field); setDynamicFieldValue(_selectedNotification); @@ -467,22 +469,21 @@ void NotifyPluginOptionsPage::on_changedIndex_UAVField(QString field) resetFieldType(); Q_ASSERT(_currUAVObject); addDynamicField(_currUAVObject->getField(field)); - } void NotifyPluginOptionsPage::on_changedIndex_UAVObject(QString val) { resetFieldType(); - _currUAVObject = dynamic_cast( _objManager.getObject(val) ); - if(!_currUAVObject) { + _currUAVObject = dynamic_cast(_objManager.getObject(val)); + if (!_currUAVObject) { qNotifyDebug() << "on_UAVObject_indexChanged | no such UAVOBject"; return; } - QList fieldList = _currUAVObject->getFields(); + QList fieldList = _currUAVObject->getFields(); disconnect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_changedIndex_UAVField(QString))); _optionsPage->UAVObjectField->clear(); - foreach (UAVObjectField* field, fieldList) { - _optionsPage->UAVObjectField->addItem(field->getName()); + foreach(UAVObjectField * field, fieldList) { + _optionsPage->UAVObjectField->addItem(field->getName()); } connect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_changedIndex_UAVField(QString))); _selectedNotification->setObjectField(fieldList.at(0)->getName()); @@ -494,7 +495,7 @@ void NotifyPluginOptionsPage::on_changedIndex_soundLanguage(int index) { _optionsPage->SoundCollectionList->setCurrentIndex(index); QString collectionPath = _optionsPage->SoundDirectoryPathChooser->path() - + QDir::toNativeSeparators("/" + _optionsPage->SoundCollectionList->currentText()); + + QDir::toNativeSeparators("/" + _optionsPage->SoundCollectionList->currentText()); QDir dirPath(collectionPath); QStringList filters; @@ -514,29 +515,30 @@ void NotifyPluginOptionsPage::on_changedIndex_soundLanguage(int index) } -void NotifyPluginOptionsPage::on_changed_playButtonText(Phonon::State newstate, Phonon::State /* oldstate */) +void NotifyPluginOptionsPage::on_changed_playButtonText(Phonon::State newstate, Phonon::State /* oldstate */) { - //Q_ASSERT(Phonon::ErrorState != newstate); + // Q_ASSERT(Phonon::ErrorState != newstate); - if (newstate == Phonon::PausedState || newstate == Phonon::StoppedState) { + if (newstate == Phonon::PausedState || newstate == Phonon::StoppedState) { _optionsPage->buttonPlayNotification->setText("Play"); _optionsPage->buttonPlayNotification->setIcon(QPixmap(":/notify/images/play.png")); } else { - if (newstate == Phonon::PlayingState) { + if (newstate == Phonon::PlayingState) { _optionsPage->buttonPlayNotification->setText("Stop"); _optionsPage->buttonPlayNotification->setIcon(QPixmap(":/notify/images/stop.png")); } } } -void NotifyPluginOptionsPage::on_changedSelection_notifyTable(const QItemSelection & selected, const QItemSelection & /* deselected */ ) +void NotifyPluginOptionsPage::on_changedSelection_notifyTable(const QItemSelection & selected, const QItemSelection & /* deselected */) { bool select = false; + _testSound->stop(); if (selected.indexes().size()) { select = true; setSelectedNotification(_privListNotifications.at(selected.indexes().at(0).row())); - UAVObjectField* field = getObjectFieldFromSelected(); + UAVObjectField *field = getObjectFieldFromSelected(); addDynamicField(field); updateConfigView(_selectedNotification); } @@ -546,17 +548,19 @@ void NotifyPluginOptionsPage::on_changedSelection_notifyTable(const QItemSelecti _optionsPage->buttonPlayNotification->setEnabled(select); } -void NotifyPluginOptionsPage::on_clicked_buttonSoundFolder(const QString& path) +void NotifyPluginOptionsPage::on_clicked_buttonSoundFolder(const QString & path) { QDir dirPath(path); QStringList listDirCollections = dirPath.entryList(QDir::AllDirs | QDir::NoDotAndDotDot); + _optionsPage->SoundCollectionList->clear(); _optionsPage->SoundCollectionList->addItems(listDirCollections); } void NotifyPluginOptionsPage::on_clicked_buttonTestSoundNotification() { - NotificationItem* notification = NULL; + NotificationItem *notification = NULL; + qNotifyDebug() << "on_buttonTestSoundNotification_clicked"; Q_ASSERT(-1 != _notifyRulesSelection->currentIndex().row()); _testSound->clearQueue(); @@ -575,11 +579,11 @@ void NotifyPluginOptionsPage::on_clicked_buttonTestSoundNotification() void NotifyPluginOptionsPage::on_clicked_buttonAddNotification() { - NotificationItem* notification = new NotificationItem; + NotificationItem *notification = new NotificationItem; if (_optionsPage->SoundDirectoryPathChooser->path().isEmpty()) { - QPalette textPalette=_optionsPage->SoundDirectoryPathChooser->palette(); - textPalette.setColor(QPalette::Normal,QPalette::Text, Qt::red); + QPalette textPalette = _optionsPage->SoundDirectoryPathChooser->palette(); + textPalette.setColor(QPalette::Normal, QPalette::Text, Qt::red); _optionsPage->SoundDirectoryPathChooser->setPalette(textPalette); _optionsPage->SoundDirectoryPathChooser->setPath("please select sound collection folder"); delete notification; @@ -587,8 +591,8 @@ void NotifyPluginOptionsPage::on_clicked_buttonAddNotification() } getOptionsPageValues(notification); - if ( ((!_optionsPage->Sound2->currentText().isEmpty()) && (_sayOrder->currentText()=="After second")) - || ((!_optionsPage->Sound3->currentText().isEmpty()) && (_sayOrder->currentText()=="After third")) ) { + if (((!_optionsPage->Sound2->currentText().isEmpty()) && (_sayOrder->currentText() == "After second")) + || ((!_optionsPage->Sound3->currentText().isEmpty()) && (_sayOrder->currentText() == "After third"))) { delete notification; return; } else { @@ -596,8 +600,8 @@ void NotifyPluginOptionsPage::on_clicked_buttonAddNotification() } _notifyRulesModel->entryAdded(notification); - _notifyRulesSelection->setCurrentIndex(_notifyRulesModel->index(_privListNotifications.size()-1,0,QModelIndex()), - QItemSelectionModel::ClearAndSelect | QItemSelectionModel::Rows); + _notifyRulesSelection->setCurrentIndex(_notifyRulesModel->index(_privListNotifications.size() - 1, 0, QModelIndex()), + QItemSelectionModel::ClearAndSelect | QItemSelectionModel::Rows); } void NotifyPluginOptionsPage::on_clicked_buttonDeleteNotification() @@ -605,8 +609,7 @@ void NotifyPluginOptionsPage::on_clicked_buttonDeleteNotification() _notifyRulesModel->removeRow(_notifyRulesSelection->currentIndex().row()); if (!_notifyRulesModel->rowCount() && (_notifyRulesSelection->currentIndex().row() > 0 - && _notifyRulesSelection->currentIndex().row() < _notifyRulesModel->rowCount()) ) - { + && _notifyRulesSelection->currentIndex().row() < _notifyRulesModel->rowCount())) { _optionsPage->buttonDelete->setEnabled(false); _optionsPage->buttonModify->setEnabled(false); _optionsPage->buttonPlayNotification->setEnabled(false); @@ -615,26 +618,27 @@ void NotifyPluginOptionsPage::on_clicked_buttonDeleteNotification() void NotifyPluginOptionsPage::on_clicked_buttonModifyNotification() { - NotificationItem* notification = new NotificationItem; + NotificationItem *notification = new NotificationItem; + getOptionsPageValues(notification); notification->setRetryValue(_privListNotifications.at(_notifyRulesSelection->currentIndex().row())->retryValue()); notification->setLifetime(_privListNotifications.at(_notifyRulesSelection->currentIndex().row())->lifetime()); notification->setMute(_privListNotifications.at(_notifyRulesSelection->currentIndex().row())->mute()); - _privListNotifications.replace(_notifyRulesSelection->currentIndex().row(),notification); + _privListNotifications.replace(_notifyRulesSelection->currentIndex().row(), notification); entryUpdated(_notifyRulesSelection->currentIndex().row()); } -void NotifyPluginOptionsPage::on_FinishedPlaying() +void NotifyPluginOptionsPage::on_FinishedPlaying() { _testSound->clear(); } void NotifyPluginOptionsPage::on_toggled_checkEnableSound(bool state) { - bool state1 = 1^state; + bool state1 = 1 ^ state; - QList listOutputs = _testSound->outputPaths(); - Phonon::AudioOutput * audioOutput = (Phonon::AudioOutput*)listOutputs.last().sink(); + QList listOutputs = _testSound->outputPaths(); + Phonon::AudioOutput *audioOutput = (Phonon::AudioOutput *)listOutputs.last().sink(); audioOutput->setMuted(state1); } diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.h b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.h index fb3c2885d..adeadde9a 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.h +++ b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.h @@ -50,23 +50,34 @@ class NotificationItem; class SoundNotifyPlugin; namespace Ui { - class NotifyPluginOptionsPage; +class NotifyPluginOptionsPage; }; using namespace Core; -class NotifyPluginOptionsPage : public IOptionsPage -{ +class NotifyPluginOptionsPage : public IOptionsPage { Q_OBJECT public: - enum {equal,bigger,smaller,inrange}; + enum { equal, bigger, smaller, inrange }; explicit NotifyPluginOptionsPage(QObject *parent = 0); ~NotifyPluginOptionsPage(); - QString id() const { return QLatin1String("settings"); } - QString trName() const { return tr("settings"); } - QString category() const { return QLatin1String("Notify Plugin");} - QString trCategory() const { return tr("Notify Plugin");} + QString id() const + { + return QLatin1String("settings"); + } + QString trName() const + { + return tr("settings"); + } + QString category() const + { + return QLatin1String("Notify Plugin"); + } + QString trCategory() const + { + return tr("Notify Plugin"); + } QWidget *createPage(QWidget *parent); void apply(); @@ -75,7 +86,7 @@ public: static QStringList conditionValues; signals: - void updateNotifications(QList list); + void updateNotifications(QList list); void entryUpdated(int index); private slots: @@ -88,10 +99,10 @@ private slots: * We can use continuous selection, to select simultaneously * multiple rows to move them(using drag & drop) inside table ranges. */ - void on_changedSelection_notifyTable( const QItemSelection & selected, const QItemSelection & deselected ); + void on_changedSelection_notifyTable(const QItemSelection & selected, const QItemSelection & deselected); void on_changedIndex_soundLanguage(int index); - void on_clicked_buttonSoundFolder(const QString& path); + void on_clicked_buttonSoundFolder(const QString & path); void on_changedIndex_UAVObject(QString val); void on_changedIndex_UAVField(QString val); void on_changed_playButtonText(Phonon::State newstate, Phonon::State oldstate); @@ -115,30 +126,30 @@ private: void initPhononPlayer(); void initRulesTable(); - void setSelectedNotification(NotificationItem* ntf); + void setSelectedNotification(NotificationItem *ntf); void resetValueRange(); void resetFieldType(); - void updateConfigView(NotificationItem* notification); - void getOptionsPageValues(NotificationItem* notification); - UAVObjectField* getObjectFieldFromPage(); - UAVObjectField* getObjectFieldFromSelected(); + void updateConfigView(NotificationItem *notification); + void getOptionsPageValues(NotificationItem *notification); + UAVObjectField *getObjectFieldFromPage(); + UAVObjectField *getObjectFieldFromSelected(); void addDynamicFieldLayout(); - void addDynamicField(UAVObjectField* objField); - void addDynamicFieldWidget(UAVObjectField* objField); - void setDynamicFieldValue(NotificationItem* notification); + void addDynamicField(UAVObjectField *objField); + void addDynamicFieldWidget(UAVObjectField *objField); + void setDynamicFieldValue(NotificationItem *notification); private: - UAVObjectManager& _objManager; - SoundNotifyPlugin* _owner; + UAVObjectManager & _objManager; + SoundNotifyPlugin *_owner; - //! Media object uses to test sound playing + // ! Media object uses to test sound playing QScopedPointer _testSound; QScopedPointer _notifyRulesModel; - QItemSelectionModel* _notifyRulesSelection; + QItemSelectionModel *_notifyRulesSelection; /** * Local copy of notification list, which owned by notify plugin. @@ -148,40 +159,39 @@ private: * we don't have additional cost for that, copy will created * only after modification of private notify list. */ - QList _privListNotifications; + QList _privListNotifications; QScopedPointer _optionsPage; - //! Widget to convinient selection of condition for field value (equal, lower, greater) - QComboBox* _dynamicFieldCondition; + // ! Widget to convinient selection of condition for field value (equal, lower, greater) + QComboBox *_dynamicFieldCondition; - //! Represents edit widget for dynamic UAVObjectfield, - //! can be spinbox - for numerics, combobox - enums, or - //! lineedit - for numerics with range constraints - QWidget* _dynamicFieldWidget; + // ! Represents edit widget for dynamic UAVObjectfield, + // ! can be spinbox - for numerics, combobox - enums, or + // ! lineedit - for numerics with range constraints + QWidget *_dynamicFieldWidget; - //! Type of UAVObjectField - numeric or ENUM, - //! this variable needs to correctly set appropriate dynamic UI element (_dynamicFieldWidget) - //! NOTE: ocassionaly it should be invalidated (= -1) to reset _dynamicFieldWidget + // ! Type of UAVObjectField - numeric or ENUM, + // ! this variable needs to correctly set appropriate dynamic UI element (_dynamicFieldWidget) + // ! NOTE: ocassionaly it should be invalidated (= -1) to reset _dynamicFieldWidget int _dynamicFieldType; - //! Widget to convinient selection of position of - //! between sounds[1..3] - QComboBox* _sayOrder; + // ! Widget to convinient selection of position of + // ! between sounds[1..3] + QComboBox *_sayOrder; - //! Actualy reference to optionsPageWidget, - //! we MUST hold it beyond the scope of createPage func - //! to have possibility change dynamic parts of options page layout in future - QWidget* _form; + // ! Actualy reference to optionsPageWidget, + // ! we MUST hold it beyond the scope of createPage func + // ! to have possibility change dynamic parts of options page layout in future + QWidget *_form; - //! Currently selected notification, all controls filled accroding to it. - //! On options page startup, always points to first row. - NotificationItem* _selectedNotification; - - //! Retrieved from UAVObjectManager by name from _selectedNotification, - //! if UAVObjectManager doesn't have such object, this field will be NULL - UAVDataObject* _currUAVObject; + // ! Currently selected notification, all controls filled accroding to it. + // ! On options page startup, always points to first row. + NotificationItem *_selectedNotification; + // ! Retrieved from UAVObjectManager by name from _selectedNotification, + // ! if UAVObjectManager doesn't have such object, this field will be NULL + UAVDataObject *_currUAVObject; }; #endif // NOTIFYPLUGINOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifytablemodel.cpp b/ground/openpilotgcs/src/plugins/notify/notifytablemodel.cpp index 0242214bd..8871f2352 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifytablemodel.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifytablemodel.cpp @@ -30,19 +30,19 @@ #include #include -const char* mime_type_notify_table = "openpilot/notify_plugin_table"; +const char *mime_type_notify_table = "openpilot/notify_plugin_table"; -NotifyTableModel::NotifyTableModel(QList& parentList, QObject* parent) - : QAbstractTableModel(parent) - , _list(parentList) +NotifyTableModel::NotifyTableModel(QList & parentList, QObject *parent) + : QAbstractTableModel(parent) + , _list(parentList) { - _headerStrings << "Name" << "Repeats" << "Lifetime,sec" << "Mute"; - connect(this, SIGNAL(dragRows(int, int)), this, SLOT(dropRows(int, int))); + _headerStrings << "Name" << "Repeats" << "Lifetime,sec" << "Mute"; + connect(this, SIGNAL(dragRows(int, int)), this, SLOT(dropRows(int, int))); } bool NotifyTableModel::setData(const QModelIndex &index, - const QVariant &value, int role) + const QVariant &value, int role) { if (index.isValid() && role == Qt::DisplayRole) { if (eMessageName == index.column()) { @@ -51,14 +51,15 @@ bool NotifyTableModel::setData(const QModelIndex &index, } } if (index.isValid() && role == Qt::EditRole) { - if (eRepeatValue == index.column()) - _list.at(index.row())->setRetryValue(NotificationItem::retryValues.indexOf(value.toString())); - else { - if (eExpireTimer == index.column()) + if (eRepeatValue == index.column()) { + _list.at(index.row())->setRetryValue(NotificationItem::retryValues.indexOf(value.toString())); + } else { + if (eExpireTimer == index.column()) { _list.at(index.row())->setLifetime(value.toInt()); - else { - if (eTurnOn == index.column()) + } else { + if (eTurnOn == index.column()) { _list.at(index.row())->setMute(value.toBool()); + } } } emit dataChanged(index, index); @@ -74,18 +75,17 @@ QVariant NotifyTableModel::data(const QModelIndex &index, int role) const return QVariant(); } - if (index.row() >= _list.size()) + if (index.row() >= _list.size()) { return QVariant(); + } - if (role == Qt::DisplayRole || role == Qt::EditRole) - { - switch(index.column()) - { + if (role == Qt::DisplayRole || role == Qt::EditRole) { + switch (index.column()) { case eMessageName: return _list.at(index.row())->toString(); case eRepeatValue: - return (NotificationItem::retryValues.at(_list.at(index.row())->retryValue())); + return NotificationItem::retryValues.at(_list.at(index.row())->retryValue()); case eExpireTimer: return _list.at(index.row())->lifetime(); @@ -96,11 +96,9 @@ QVariant NotifyTableModel::data(const QModelIndex &index, int role) const default: return QVariant(); } - } - else - { - if (Qt::SizeHintRole == role){ - return QVariant(10); + } else { + if (Qt::SizeHintRole == role) { + return QVariant(10); } } return QVariant(); @@ -108,24 +106,26 @@ QVariant NotifyTableModel::data(const QModelIndex &index, int role) const QVariant NotifyTableModel::headerData(int section, Qt::Orientation orientation, int role) const { - if (role != Qt::DisplayRole) + if (role != Qt::DisplayRole) { return QVariant(); + } - if (orientation == Qt::Horizontal) + if (orientation == Qt::Horizontal) { return _headerStrings.at(section); - else - if (orientation == Qt::Vertical) - return QString("%1").arg(section); + } else if (orientation == Qt::Vertical) { + return QString("%1").arg(section); + } return QVariant(); } -bool NotifyTableModel::insertRows(int position, int rows, const QModelIndex& index) +bool NotifyTableModel::insertRows(int position, int rows, const QModelIndex & index) { Q_UNUSED(index); - if (-1 == position || -1 == rows) + if (-1 == position || -1 == rows) { return false; + } beginInsertRows(QModelIndex(), position, position + rows - 1); @@ -137,12 +137,13 @@ bool NotifyTableModel::insertRows(int position, int rows, const QModelIndex& ind return true; } -bool NotifyTableModel::removeRows(int position, int rows, const QModelIndex& index) +bool NotifyTableModel::removeRows(int position, int rows, const QModelIndex & index) { Q_UNUSED(index); - if ((-1 == position) || (-1 == rows) ) + if ((-1 == position) || (-1 == rows)) { return false; + } beginRemoveRows(QModelIndex(), position, position + rows - 1); @@ -160,10 +161,10 @@ void NotifyTableModel::entryUpdated(int offset) emit dataChanged(idx, idx); } -void NotifyTableModel::entryAdded(NotificationItem* item) +void NotifyTableModel::entryAdded(NotificationItem *item) { insertRows(rowCount(), 1, QModelIndex()); - NotificationItem* tmp = _list.at(rowCount() - 1); + NotificationItem *tmp = _list.at(rowCount() - 1); _list.replace(rowCount() - 1, item); delete tmp; entryUpdated(rowCount() - 1); @@ -177,77 +178,87 @@ Qt::DropActions NotifyTableModel::supportedDropActions() const QStringList NotifyTableModel::mimeTypes() const { QStringList types; + types << mime_type_notify_table; return types; } -bool NotifyTableModel::dropMimeData( const QMimeData * data, Qt::DropAction action, int row, - int column, const QModelIndex& parent) +bool NotifyTableModel::dropMimeData(const QMimeData *data, Qt::DropAction action, int row, + int column, const QModelIndex & parent) { - if (action == Qt::IgnoreAction) + if (action == Qt::IgnoreAction) { return true; + } - if (!data->hasFormat(mime_type_notify_table)) + if (!data->hasFormat(mime_type_notify_table)) { return false; + } int beginRow = -1; - if (row != -1) + if (row != -1) { beginRow = row; - else { - if (parent.isValid()) + } else { + if (parent.isValid()) { beginRow = parent.row(); - else + } else { beginRow = rowCount(QModelIndex()); + } } - if (-1 == beginRow) + if (-1 == beginRow) { return false; + } QByteArray encodedData = data->data(mime_type_notify_table); QDataStream stream(&encodedData, QIODevice::ReadOnly); int rows = beginRow; // read next item from input MIME and drop into the table line by line - while(!stream.atEnd()) { + while (!stream.atEnd()) { quintptr ptr; stream >> ptr; - NotificationItem* item = reinterpret_cast(ptr); + NotificationItem *item = reinterpret_cast(ptr); int dragged = _list.indexOf(item); // we can drag item from top rows to bottom (DOWN_DIRECTION), // or from bottom rows to top rows (UP_DIRECTION) enum { UP_DIRECTION, DOWN_DIRECTION }; int direction = (dragged < rows) ? DOWN_DIRECTION : (dragged += 1, UP_DIRECTION); // check drop bounds - if (dragged < 0 || ((dragged + 1) >= _list.size() && direction == DOWN_DIRECTION) || dragged == rows) { + if (dragged < 0 || ((dragged + 1) >= _list.size() && direction == DOWN_DIRECTION) || dragged == rows) { qNotifyDebug() << "no such item"; continue; } // addiional check in case dropping of multiple rows - if(rows + direction > _list.size()) continue; + if (rows + direction > _list.size()) { + continue; + } bool success = insertRows(rows + direction, 1, QModelIndex()); Q_ASSERT(success); _list.replace(rows + direction, item); success = removeRows(dragged, 1, QModelIndex()); Q_ASSERT(success); - if (direction == UP_DIRECTION) + if (direction == UP_DIRECTION) { ++rows; - }; + } + } + ; - QModelIndex idxTopLeft = index(beginRow, 0, QModelIndex()); + QModelIndex idxTopLeft = index(beginRow, 0, QModelIndex()); QModelIndex idxBotRight = index(beginRow, columnCount(QModelIndex()), QModelIndex()); emit dataChanged(idxTopLeft, idxBotRight); return true; } -QMimeData* NotifyTableModel::mimeData(const QModelIndexList& indexes) const +QMimeData *NotifyTableModel::mimeData(const QModelIndexList & indexes) const { - QMimeData* mimeData = new QMimeData(); + QMimeData *mimeData = new QMimeData(); QByteArray encodedData; QDataStream stream(&encodedData, QIODevice::WriteOnly); int rows = 0; - foreach (const QModelIndex& index, indexes) { + + foreach(const QModelIndex &index, indexes) { if (!index.column()) { quintptr item = reinterpret_cast(_list.at(index.row())); stream << item; diff --git a/ground/openpilotgcs/src/plugins/notify/notifytablemodel.h b/ground/openpilotgcs/src/plugins/notify/notifytablemodel.h index 0b4c75b93..01974baab 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifytablemodel.h +++ b/ground/openpilotgcs/src/plugins/notify/notifytablemodel.h @@ -35,37 +35,37 @@ enum ColumnNames { eMessageName, eRepeatValue, eExpireTimer, eTurnOn }; -class NotifyTableModel : public QAbstractTableModel -{ +class NotifyTableModel : public QAbstractTableModel { Q_OBJECT - enum {eColumnCount = 4 }; + enum { eColumnCount = 4 }; public: - NotifyTableModel(QList& parentList, QObject* parent = 0); - int rowCount(const QModelIndex& parent = QModelIndex()) const + NotifyTableModel(QList & parentList, QObject *parent = 0); + int rowCount(const QModelIndex & parent = QModelIndex()) const { return _list.count(); } - int columnCount(const QModelIndex &/*parent*/) const + int columnCount(const QModelIndex & /*parent*/) const { return eColumnCount; } Qt::ItemFlags flags(const QModelIndex &index) const { - if (!index.isValid()) + if (!index.isValid()) { return Qt::ItemIsEnabled | Qt::ItemIsDropEnabled; + } return QAbstractItemModel::flags(index) | Qt::ItemIsEditable | Qt::ItemIsDragEnabled | Qt::ItemIsDropEnabled; } QStringList mimeTypes() const; Qt::DropActions supportedDropActions() const; - bool dropMimeData( const QMimeData * data, Qt::DropAction action, int row, - int column, const QModelIndex& parent); - QMimeData* mimeData(const QModelIndexList &indexes) const; + bool dropMimeData(const QMimeData *data, Qt::DropAction action, int row, + int column, const QModelIndex & parent); + QMimeData *mimeData(const QModelIndexList &indexes) const; bool setData(const QModelIndex &index, const QVariant &value, int role); @@ -73,7 +73,7 @@ public: QVariant headerData(int section, Qt::Orientation orientation, int role) const; bool insertRows(int position, int rows, const QModelIndex &index); bool removeRows(int position, int rows, const QModelIndex &index); - void entryAdded(NotificationItem* item); + void entryAdded(NotificationItem *item); signals: void dragRows(int position, int count); @@ -83,7 +83,7 @@ private slots: void dropRows(int position, int count) const; private: - QList& _list; + QList & _list; QStringList _headerStrings; }; diff --git a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp index 4ca229866..633199aa5 100644 --- a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp @@ -28,392 +28,452 @@ #include "flightdatamodel.h" #include #include -flightDataModel::flightDataModel(QObject *parent):QAbstractTableModel(parent) -{ +flightDataModel::flightDataModel(QObject *parent) : QAbstractTableModel(parent) +{} -} - -int flightDataModel::rowCount(const QModelIndex &/*parent*/) const +int flightDataModel::rowCount(const QModelIndex & /*parent*/) const { return dataStorage.length(); } int flightDataModel::columnCount(const QModelIndex &parent) const { - if (parent.isValid()) + if (parent.isValid()) { return 0; + } return 23; } QVariant flightDataModel::data(const QModelIndex &index, int role) const { - if (role == Qt::DisplayRole||role==Qt::EditRole) - { - int rowNumber=index.row(); - int columnNumber=index.column(); - if(rowNumber>dataStorage.length()-1 || rowNumber<0) + if (role == Qt::DisplayRole || role == Qt::EditRole) { + int rowNumber = index.row(); + int columnNumber = index.column(); + if (rowNumber > dataStorage.length() - 1 || rowNumber < 0) { return QVariant::Invalid; - pathPlanData * myRow=dataStorage.at(rowNumber); - QVariant ret=getColumnByIndex(myRow,columnNumber); + } + pathPlanData *myRow = dataStorage.at(rowNumber); + QVariant ret = getColumnByIndex(myRow, columnNumber); return ret; } /* - else if (role == Qt::BackgroundRole) { - // WaypointActive::DataFields waypointActive = waypointActiveObj->getData(); + else if (role == Qt::BackgroundRole) { + // WaypointActive::DataFields waypointActive = waypointActiveObj->getData(); if(index.row() == waypointActive.Index) { return QBrush(Qt::lightGray); } else return QVariant::Invalid; - }*/ + }*/ else { return QVariant::Invalid; } } -bool flightDataModel::setColumnByIndex(pathPlanData *row,const int index,const QVariant value) +bool flightDataModel::setColumnByIndex(pathPlanData *row, const int index, const QVariant value) { - switch(index) - { + switch (index) { case WPDESCRITPTION: - row->wpDescritption=value.toString(); + row->wpDescritption = value.toString(); return true; + break; case LATPOSITION: - row->latPosition=value.toDouble(); + row->latPosition = value.toDouble(); return true; + break; case LNGPOSITION: - row->lngPosition=value.toDouble(); + row->lngPosition = value.toDouble(); return true; + break; case DISRELATIVE: - row->disRelative=value.toDouble(); + row->disRelative = value.toDouble(); return true; + break; case BEARELATIVE: - row->beaRelative=value.toDouble(); + row->beaRelative = value.toDouble(); return true; + break; case ALTITUDERELATIVE: - row->altitudeRelative=value.toFloat(); + row->altitudeRelative = value.toFloat(); return true; + break; case ISRELATIVE: - row->isRelative=value.toBool(); + row->isRelative = value.toBool(); return true; + break; case ALTITUDE: - row->altitude=value.toDouble(); + row->altitude = value.toDouble(); return true; + break; case VELOCITY: - row->velocity=value.toFloat(); + row->velocity = value.toFloat(); return true; + break; case MODE: - row->mode=value.toInt(); + row->mode = value.toInt(); return true; + break; case MODE_PARAMS0: - row->mode_params[0]=value.toFloat(); + row->mode_params[0] = value.toFloat(); return true; + break; case MODE_PARAMS1: - row->mode_params[1]=value.toFloat(); + row->mode_params[1] = value.toFloat(); return true; + break; case MODE_PARAMS2: - row->mode_params[2]=value.toFloat(); + row->mode_params[2] = value.toFloat(); return true; + break; case MODE_PARAMS3: - row->mode_params[3]=value.toFloat(); + row->mode_params[3] = value.toFloat(); return true; + break; case CONDITION: - row->condition=value.toInt(); + row->condition = value.toInt(); return true; + break; case CONDITION_PARAMS0: - row->condition_params[0]=value.toFloat(); + row->condition_params[0] = value.toFloat(); return true; + break; case CONDITION_PARAMS1: - row->condition_params[1]=value.toFloat(); + row->condition_params[1] = value.toFloat(); return true; + break; case CONDITION_PARAMS2: - row->condition_params[2]=value.toFloat(); + row->condition_params[2] = value.toFloat(); return true; + break; case CONDITION_PARAMS3: - row->condition_params[3]=value.toFloat(); + row->condition_params[3] = value.toFloat(); return true; + break; case COMMAND: - row->command=value.toInt(); + row->command = value.toInt(); break; case JUMPDESTINATION: - row->jumpdestination=value.toInt(); + row->jumpdestination = value.toInt(); return true; + break; case ERRORDESTINATION: - row->errordestination=value.toInt(); + row->errordestination = value.toInt(); return true; + break; case LOCKED: - row->locked=value.toBool(); + row->locked = value.toBool(); return true; + break; default: return false; } return false; } -QVariant flightDataModel::getColumnByIndex(const pathPlanData *row,const int index) const +QVariant flightDataModel::getColumnByIndex(const pathPlanData *row, const int index) const { - switch(index) - { + switch (index) { case WPDESCRITPTION: return row->wpDescritption; + break; case LATPOSITION: return row->latPosition; + break; case LNGPOSITION: return row->lngPosition; + break; case DISRELATIVE: return row->disRelative; + break; case BEARELATIVE: return row->beaRelative; + break; case ALTITUDERELATIVE: return row->altitudeRelative; + break; case ISRELATIVE: return row->isRelative; + break; case ALTITUDE: return row->altitude; + break; case VELOCITY: return row->velocity; + break; case MODE: return row->mode; + break; case MODE_PARAMS0: return row->mode_params[0]; + break; case MODE_PARAMS1: return row->mode_params[1]; + break; case MODE_PARAMS2: return row->mode_params[2]; + break; case MODE_PARAMS3: return row->mode_params[3]; + break; case CONDITION: return row->condition; + break; case CONDITION_PARAMS0: return row->condition_params[0]; + break; case CONDITION_PARAMS1: return row->condition_params[1]; + break; case CONDITION_PARAMS2: return row->condition_params[2]; + break; case CONDITION_PARAMS3: return row->condition_params[3]; + break; case COMMAND: return row->command; + break; case JUMPDESTINATION: return row->jumpdestination; + break; case ERRORDESTINATION: return row->errordestination; + break; case LOCKED: return row->locked; } } QVariant flightDataModel::headerData(int section, Qt::Orientation orientation, int role) const - { - if (role == Qt::DisplayRole) - { - if(orientation==Qt::Vertical) - { - return QString::number(section+1); - } - else if (orientation == Qt::Horizontal) { - switch (section) - { - case WPDESCRITPTION: - return QString("Description"); - break; - case LATPOSITION: - return QString("Latitude"); - break; - case LNGPOSITION: - return QString("Longitude"); - break; - case DISRELATIVE: - return QString("Distance to home"); - break; - case BEARELATIVE: - return QString("Bearing from home"); - break; - case ALTITUDERELATIVE: - return QString("Altitude above home"); - break; - case ISRELATIVE: - return QString("Relative to home"); - break; - case ALTITUDE: - return QString("Altitude"); - break; - case VELOCITY: - return QString("Velocity"); - break; - case MODE: - return QString("Mode"); - break; - case MODE_PARAMS0: - return QString("Mode parameter 0"); - break; - case MODE_PARAMS1: - return QString("Mode parameter 1"); - break; - case MODE_PARAMS2: - return QString("Mode parameter 2"); - break; - case MODE_PARAMS3: - return QString("Mode parameter 3"); - break; - case CONDITION: - return QString("Condition"); - break; - case CONDITION_PARAMS0: - return QString("Condition parameter 0"); - break; - case CONDITION_PARAMS1: - return QString("Condition parameter 1"); - break; - case CONDITION_PARAMS2: - return QString("Condition parameter 2"); - break; - case CONDITION_PARAMS3: - return QString("Condition parameter 3"); - break; - case COMMAND: - return QString("Command"); - break; - case JUMPDESTINATION: - return QString("Jump Destination"); - break; - case ERRORDESTINATION: - return QString("Error Destination"); - break; - case LOCKED: - return QString("Locked"); - break; - default: - return QString(); - break; - } - } - } - else - return QAbstractTableModel::headerData(section, orientation, role); +{ + if (role == Qt::DisplayRole) { + if (orientation == Qt::Vertical) { + return QString::number(section + 1); + } else if (orientation == Qt::Horizontal) { + switch (section) { + case WPDESCRITPTION: + return QString("Description"); + + break; + case LATPOSITION: + return QString("Latitude"); + + break; + case LNGPOSITION: + return QString("Longitude"); + + break; + case DISRELATIVE: + return QString("Distance to home"); + + break; + case BEARELATIVE: + return QString("Bearing from home"); + + break; + case ALTITUDERELATIVE: + return QString("Altitude above home"); + + break; + case ISRELATIVE: + return QString("Relative to home"); + + break; + case ALTITUDE: + return QString("Altitude"); + + break; + case VELOCITY: + return QString("Velocity"); + + break; + case MODE: + return QString("Mode"); + + break; + case MODE_PARAMS0: + return QString("Mode parameter 0"); + + break; + case MODE_PARAMS1: + return QString("Mode parameter 1"); + + break; + case MODE_PARAMS2: + return QString("Mode parameter 2"); + + break; + case MODE_PARAMS3: + return QString("Mode parameter 3"); + + break; + case CONDITION: + return QString("Condition"); + + break; + case CONDITION_PARAMS0: + return QString("Condition parameter 0"); + + break; + case CONDITION_PARAMS1: + return QString("Condition parameter 1"); + + break; + case CONDITION_PARAMS2: + return QString("Condition parameter 2"); + + break; + case CONDITION_PARAMS3: + return QString("Condition parameter 3"); + + break; + case COMMAND: + return QString("Command"); + + break; + case JUMPDESTINATION: + return QString("Jump Destination"); + + break; + case ERRORDESTINATION: + return QString("Error Destination"); + + break; + case LOCKED: + return QString("Locked"); + + break; + default: + return QString(); + + break; + } + } + } else { + return QAbstractTableModel::headerData(section, orientation, role); + } } bool flightDataModel::setData(const QModelIndex &index, const QVariant &value, int role) { - if (role == Qt::EditRole) - { - int columnIndex=index.column(); - int rowIndex=index.row(); - if(rowIndex>dataStorage.length()-1) + if (role == Qt::EditRole) { + int columnIndex = index.column(); + int rowIndex = index.row(); + if (rowIndex > dataStorage.length() - 1) { return false; - pathPlanData * myRow=dataStorage.at(rowIndex); - setColumnByIndex(myRow,columnIndex,value); - emit dataChanged(index,index); + } + pathPlanData *myRow = dataStorage.at(rowIndex); + setColumnByIndex(myRow, columnIndex, value); + emit dataChanged(index, index); } return true; } Qt::ItemFlags flightDataModel::flags(const QModelIndex & /*index*/) const - { - return Qt::ItemIsSelectable | Qt::ItemIsEditable | Qt::ItemIsEnabled ; +{ + return Qt::ItemIsSelectable | Qt::ItemIsEditable | Qt::ItemIsEnabled; } -bool flightDataModel::insertRows(int row, int count, const QModelIndex &/*parent*/) +bool flightDataModel::insertRows(int row, int count, const QModelIndex & /*parent*/) { - pathPlanData * data; - beginInsertRows(QModelIndex(),row,row+count-1); - for(int x=0; xlatPosition=0; - data->lngPosition=0; - data->disRelative=0; - data->beaRelative=0; - data->altitudeRelative=0; - data->isRelative=true; - data->altitude=0; - data->velocity=0; - data->mode=1; - data->mode_params[0]=0; - data->mode_params[1]=0; - data->mode_params[2]=0; - data->mode_params[3]=0; - data->condition=3; - data->condition_params[0]=0; - data->condition_params[1]=0; - data->condition_params[2]=0; - data->condition_params[3]=0; - data->command=0; - data->jumpdestination=0; - data->errordestination=0; - data->locked=false; - if(rowCount()>0) - { - data->altitude=this->data(this->index(rowCount()-1,ALTITUDE)).toDouble(); - data->altitudeRelative=this->data(this->index(rowCount()-1,ALTITUDERELATIVE)).toDouble(); - data->isRelative=this->data(this->index(rowCount()-1,ISRELATIVE)).toBool(); - data->velocity=this->data(this->index(rowCount()-1,VELOCITY)).toFloat(); - data->mode=this->data(this->index(rowCount()-1,MODE)).toInt(); - data->mode_params[0]=this->data(this->index(rowCount()-1,MODE_PARAMS0)).toFloat(); - data->mode_params[1]=this->data(this->index(rowCount()-1,MODE_PARAMS1)).toFloat(); - data->mode_params[2]=this->data(this->index(rowCount()-1,MODE_PARAMS2)).toFloat(); - data->mode_params[3]=this->data(this->index(rowCount()-1,MODE_PARAMS3)).toFloat(); - data->condition=this->data(this->index(rowCount()-1,CONDITION)).toInt(); - data->condition_params[0]=this->data(this->index(rowCount()-1,CONDITION_PARAMS0)).toFloat(); - data->condition_params[1]=this->data(this->index(rowCount()-1,CONDITION_PARAMS1)).toFloat(); - data->condition_params[2]=this->data(this->index(rowCount()-1,CONDITION_PARAMS2)).toFloat(); - data->condition_params[3]=this->data(this->index(rowCount()-1,CONDITION_PARAMS3)).toFloat(); - data->command=this->data(this->index(rowCount()-1,COMMAND)).toInt(); - data->errordestination=this->data(this->index(rowCount()-1,ERRORDESTINATION)).toInt(); + pathPlanData *data; + + beginInsertRows(QModelIndex(), row, row + count - 1); + for (int x = 0; x < count; ++x) { + data = new pathPlanData; + data->latPosition = 0; + data->lngPosition = 0; + data->disRelative = 0; + data->beaRelative = 0; + data->altitudeRelative = 0; + data->isRelative = true; + data->altitude = 0; + data->velocity = 0; + data->mode = 1; + data->mode_params[0] = 0; + data->mode_params[1] = 0; + data->mode_params[2] = 0; + data->mode_params[3] = 0; + data->condition = 3; + data->condition_params[0] = 0; + data->condition_params[1] = 0; + data->condition_params[2] = 0; + data->condition_params[3] = 0; + data->command = 0; + data->jumpdestination = 0; + data->errordestination = 0; + data->locked = false; + if (rowCount() > 0) { + data->altitude = this->data(this->index(rowCount() - 1, ALTITUDE)).toDouble(); + data->altitudeRelative = this->data(this->index(rowCount() - 1, ALTITUDERELATIVE)).toDouble(); + data->isRelative = this->data(this->index(rowCount() - 1, ISRELATIVE)).toBool(); + data->velocity = this->data(this->index(rowCount() - 1, VELOCITY)).toFloat(); + data->mode = this->data(this->index(rowCount() - 1, MODE)).toInt(); + data->mode_params[0] = this->data(this->index(rowCount() - 1, MODE_PARAMS0)).toFloat(); + data->mode_params[1] = this->data(this->index(rowCount() - 1, MODE_PARAMS1)).toFloat(); + data->mode_params[2] = this->data(this->index(rowCount() - 1, MODE_PARAMS2)).toFloat(); + data->mode_params[3] = this->data(this->index(rowCount() - 1, MODE_PARAMS3)).toFloat(); + data->condition = this->data(this->index(rowCount() - 1, CONDITION)).toInt(); + data->condition_params[0] = this->data(this->index(rowCount() - 1, CONDITION_PARAMS0)).toFloat(); + data->condition_params[1] = this->data(this->index(rowCount() - 1, CONDITION_PARAMS1)).toFloat(); + data->condition_params[2] = this->data(this->index(rowCount() - 1, CONDITION_PARAMS2)).toFloat(); + data->condition_params[3] = this->data(this->index(rowCount() - 1, CONDITION_PARAMS3)).toFloat(); + data->command = this->data(this->index(rowCount() - 1, COMMAND)).toInt(); + data->errordestination = this->data(this->index(rowCount() - 1, ERRORDESTINATION)).toInt(); } - dataStorage.insert(row,data); + dataStorage.insert(row, data); } endInsertRows(); } -bool flightDataModel::removeRows(int row, int count, const QModelIndex &/*parent*/) +bool flightDataModel::removeRows(int row, int count, const QModelIndex & /*parent*/) { - if(row<0) + if (row < 0) { return false; - beginRemoveRows(QModelIndex(),row,row+count-1); - for(int x=0; xwpDescritption); - field.setAttribute("name","description"); + QDomElement field = doc.createElement("field"); + field.setAttribute("value", obj->wpDescritption); + field.setAttribute("name", "description"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->latPosition); - field.setAttribute("name","latitude"); + field = doc.createElement("field"); + field.setAttribute("value", obj->latPosition); + field.setAttribute("name", "latitude"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->lngPosition); - field.setAttribute("name","longitude"); + field = doc.createElement("field"); + field.setAttribute("value", obj->lngPosition); + field.setAttribute("name", "longitude"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->disRelative); - field.setAttribute("name","distance_to_home"); + field = doc.createElement("field"); + field.setAttribute("value", obj->disRelative); + field.setAttribute("name", "distance_to_home"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->beaRelative); - field.setAttribute("name","bearing_from_home"); + field = doc.createElement("field"); + field.setAttribute("value", obj->beaRelative); + field.setAttribute("name", "bearing_from_home"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->altitudeRelative); - field.setAttribute("name","altitude_above_home"); + field = doc.createElement("field"); + field.setAttribute("value", obj->altitudeRelative); + field.setAttribute("name", "altitude_above_home"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->isRelative); - field.setAttribute("name","is_relative_to_home"); + field = doc.createElement("field"); + field.setAttribute("value", obj->isRelative); + field.setAttribute("name", "is_relative_to_home"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->altitude); - field.setAttribute("name","altitude"); + field = doc.createElement("field"); + field.setAttribute("value", obj->altitude); + field.setAttribute("name", "altitude"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->velocity); - field.setAttribute("name","velocity"); + field = doc.createElement("field"); + field.setAttribute("value", obj->velocity); + field.setAttribute("name", "velocity"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->mode); - field.setAttribute("name","mode"); + field = doc.createElement("field"); + field.setAttribute("value", obj->mode); + field.setAttribute("name", "mode"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->mode_params[0]); - field.setAttribute("name","mode_param0"); + field = doc.createElement("field"); + field.setAttribute("value", obj->mode_params[0]); + field.setAttribute("name", "mode_param0"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->mode_params[1]); - field.setAttribute("name","mode_param1"); + field = doc.createElement("field"); + field.setAttribute("value", obj->mode_params[1]); + field.setAttribute("name", "mode_param1"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->mode_params[2]); - field.setAttribute("name","mode_param2"); + field = doc.createElement("field"); + field.setAttribute("value", obj->mode_params[2]); + field.setAttribute("name", "mode_param2"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->mode_params[3]); - field.setAttribute("name","mode_param3"); + field = doc.createElement("field"); + field.setAttribute("value", obj->mode_params[3]); + field.setAttribute("name", "mode_param3"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->condition); - field.setAttribute("name","condition"); + field = doc.createElement("field"); + field.setAttribute("value", obj->condition); + field.setAttribute("name", "condition"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->condition_params[0]); - field.setAttribute("name","condition_param0"); + field = doc.createElement("field"); + field.setAttribute("value", obj->condition_params[0]); + field.setAttribute("name", "condition_param0"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->condition_params[1]); - field.setAttribute("name","condition_param1"); + field = doc.createElement("field"); + field.setAttribute("value", obj->condition_params[1]); + field.setAttribute("name", "condition_param1"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->condition_params[2]); - field.setAttribute("name","condition_param2"); + field = doc.createElement("field"); + field.setAttribute("value", obj->condition_params[2]); + field.setAttribute("name", "condition_param2"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->condition_params[3]); - field.setAttribute("name","condition_param3"); + field = doc.createElement("field"); + field.setAttribute("value", obj->condition_params[3]); + field.setAttribute("name", "condition_param3"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->command); - field.setAttribute("name","command"); + field = doc.createElement("field"); + field.setAttribute("value", obj->command); + field.setAttribute("name", "command"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->jumpdestination); - field.setAttribute("name","jumpdestination"); + field = doc.createElement("field"); + field.setAttribute("value", obj->jumpdestination); + field.setAttribute("name", "jumpdestination"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->errordestination); - field.setAttribute("name","errordestination"); + field = doc.createElement("field"); + field.setAttribute("value", obj->errordestination); + field.setAttribute("name", "errordestination"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->locked); - field.setAttribute("name","is_locked"); + field = doc.createElement("field"); + field.setAttribute("value", obj->locked); + field.setAttribute("name", "is_locked"); waypoint.appendChild(field); - } file.write(doc.toString().toAscii()); file.close(); @@ -562,14 +619,14 @@ bool flightDataModel::writeToFile(QString fileName) } void flightDataModel::readFromFile(QString fileName) { - //TODO warning message - removeRows(0,rowCount()); + // TODO warning message + removeRows(0, rowCount()); QFile file(fileName); file.open(QIODevice::ReadOnly); QDomDocument doc("PathPlan"); - QByteArray array=file.readAll(); + QByteArray array = file.readAll(); QString error; - if (!doc.setContent(array,&error)) { + if (!doc.setContent(array, &error)) { QMessageBox msgBox; msgBox.setText(tr("File Parsing Failed.")); msgBox.setInformativeText(QString(tr("This file is not a correct XML file:%0")).arg(error)); @@ -590,73 +647,72 @@ void flightDataModel::readFromFile(QString fileName) return; } - pathPlanData * data=NULL; + pathPlanData *data = NULL; QDomNode node = root.firstChild(); while (!node.isNull()) { QDomElement e = node.toElement(); if (e.tagName() == "waypoint") { - QDomNode fieldNode=e.firstChild(); - data=new pathPlanData; + QDomNode fieldNode = e.firstChild(); + data = new pathPlanData; while (!fieldNode.isNull()) { QDomElement field = fieldNode.toElement(); if (field.tagName() == "field") { - if(field.attribute("name")=="altitude") - data->altitude=field.attribute("value").toDouble(); - else if(field.attribute("name")=="description") - data->wpDescritption=field.attribute("value"); - else if(field.attribute("name")=="latitude") - data->latPosition=field.attribute("value").toDouble(); - else if(field.attribute("name")=="longitude") - data->lngPosition=field.attribute("value").toDouble(); - else if(field.attribute("name")=="distance_to_home") - data->disRelative=field.attribute("value").toDouble(); - else if(field.attribute("name")=="bearing_from_home") - data->beaRelative=field.attribute("value").toDouble(); - else if(field.attribute("name")=="altitude_above_home") - data->altitudeRelative=field.attribute("value").toFloat(); - else if(field.attribute("name")=="is_relative_to_home") - data->isRelative=field.attribute("value").toInt(); - else if(field.attribute("name")=="altitude") - data->altitude=field.attribute("value").toDouble(); - else if(field.attribute("name")=="velocity") - data->velocity=field.attribute("value").toFloat(); - else if(field.attribute("name")=="mode") - data->mode=field.attribute("value").toInt(); - else if(field.attribute("name")=="mode_param0") - data->mode_params[0]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="mode_param1") - data->mode_params[1]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="mode_param2") - data->mode_params[2]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="mode_param3") - data->mode_params[3]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="condition") - data->condition=field.attribute("value").toDouble(); - else if(field.attribute("name")=="condition_param0") - data->condition_params[0]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="condition_param1") - data->condition_params[1]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="condition_param2") - data->condition_params[2]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="condition_param3") - data->condition_params[3]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="command") - data->command=field.attribute("value").toInt(); - else if(field.attribute("name")=="jumpdestination") - data->jumpdestination=field.attribute("value").toInt(); - else if(field.attribute("name")=="errordestination") - data->errordestination=field.attribute("value").toInt(); - else if(field.attribute("name")=="is_locked") - data->locked=field.attribute("value").toInt(); - + if (field.attribute("name") == "altitude") { + data->altitude = field.attribute("value").toDouble(); + } else if (field.attribute("name") == "description") { + data->wpDescritption = field.attribute("value"); + } else if (field.attribute("name") == "latitude") { + data->latPosition = field.attribute("value").toDouble(); + } else if (field.attribute("name") == "longitude") { + data->lngPosition = field.attribute("value").toDouble(); + } else if (field.attribute("name") == "distance_to_home") { + data->disRelative = field.attribute("value").toDouble(); + } else if (field.attribute("name") == "bearing_from_home") { + data->beaRelative = field.attribute("value").toDouble(); + } else if (field.attribute("name") == "altitude_above_home") { + data->altitudeRelative = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "is_relative_to_home") { + data->isRelative = field.attribute("value").toInt(); + } else if (field.attribute("name") == "altitude") { + data->altitude = field.attribute("value").toDouble(); + } else if (field.attribute("name") == "velocity") { + data->velocity = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "mode") { + data->mode = field.attribute("value").toInt(); + } else if (field.attribute("name") == "mode_param0") { + data->mode_params[0] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "mode_param1") { + data->mode_params[1] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "mode_param2") { + data->mode_params[2] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "mode_param3") { + data->mode_params[3] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "condition") { + data->condition = field.attribute("value").toDouble(); + } else if (field.attribute("name") == "condition_param0") { + data->condition_params[0] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "condition_param1") { + data->condition_params[1] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "condition_param2") { + data->condition_params[2] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "condition_param3") { + data->condition_params[3] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "command") { + data->command = field.attribute("value").toInt(); + } else if (field.attribute("name") == "jumpdestination") { + data->jumpdestination = field.attribute("value").toInt(); + } else if (field.attribute("name") == "errordestination") { + data->errordestination = field.attribute("value").toInt(); + } else if (field.attribute("name") == "is_locked") { + data->locked = field.attribute("value").toInt(); + } } - fieldNode=fieldNode.nextSibling(); + fieldNode = fieldNode.nextSibling(); } - beginInsertRows(QModelIndex(),dataStorage.length(),dataStorage.length()); - dataStorage.append(data); - endInsertRows(); + beginInsertRows(QModelIndex(), dataStorage.length(), dataStorage.length()); + dataStorage.append(data); + endInsertRows(); } - node=node.nextSibling(); + node = node.nextSibling(); } } - diff --git a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.h b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.h index 3043c607c..3dcdf337c 100644 --- a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.h +++ b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.h @@ -29,48 +29,45 @@ #include #include "opmapcontrol/opmapcontrol.h" -struct pathPlanData -{ +struct pathPlanData { QString wpDescritption; - double latPosition; - double lngPosition; - double disRelative; - double beaRelative; - double altitudeRelative; - bool isRelative; - double altitude; - float velocity; - int mode; - float mode_params[4]; - int condition; - float condition_params[4]; - int command; - int jumpdestination; - int errordestination; - bool locked; + double latPosition; + double lngPosition; + double disRelative; + double beaRelative; + double altitudeRelative; + bool isRelative; + double altitude; + float velocity; + int mode; + float mode_params[4]; + int condition; + float condition_params[4]; + int command; + int jumpdestination; + int errordestination; + bool locked; }; -class flightDataModel:public QAbstractTableModel -{ +class flightDataModel : public QAbstractTableModel { Q_OBJECT public: - enum pathPlanDataEnum - { - WPDESCRITPTION,LATPOSITION,LNGPOSITION,DISRELATIVE,BEARELATIVE,ALTITUDERELATIVE,ISRELATIVE,ALTITUDE, - VELOCITY,MODE,MODE_PARAMS0,MODE_PARAMS1,MODE_PARAMS2,MODE_PARAMS3, - CONDITION,CONDITION_PARAMS0,CONDITION_PARAMS1,CONDITION_PARAMS2,CONDITION_PARAMS3, - COMMAND,JUMPDESTINATION,ERRORDESTINATION,LOCKED + enum pathPlanDataEnum { + WPDESCRITPTION, LATPOSITION, LNGPOSITION, DISRELATIVE, BEARELATIVE, ALTITUDERELATIVE, ISRELATIVE, ALTITUDE, + VELOCITY, MODE, MODE_PARAMS0, MODE_PARAMS1, MODE_PARAMS2, MODE_PARAMS3, + CONDITION, CONDITION_PARAMS0, CONDITION_PARAMS1, CONDITION_PARAMS2, CONDITION_PARAMS3, + COMMAND, JUMPDESTINATION, ERRORDESTINATION, LOCKED }; flightDataModel(QObject *parent); - int rowCount(const QModelIndex &parent = QModelIndex()) const ; + int rowCount(const QModelIndex &parent = QModelIndex()) const; int columnCount(const QModelIndex &parent = QModelIndex()) const; QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const; QVariant headerData(int section, Qt::Orientation orientation, int role) const; bool setData(const QModelIndex & index, const QVariant & value, int role = Qt::EditRole); - Qt::ItemFlags flags(const QModelIndex & index) const ; - bool insertRows ( int row, int count, const QModelIndex & parent = QModelIndex() ); - bool removeRows ( int row, int count, const QModelIndex & parent = QModelIndex() ); + Qt::ItemFlags flags(const QModelIndex & index) const; + bool insertRows(int row, int count, const QModelIndex & parent = QModelIndex()); + bool removeRows(int row, int count, const QModelIndex & parent = QModelIndex()); bool writeToFile(QString filename); void readFromFile(QString fileName); private: diff --git a/ground/openpilotgcs/src/plugins/opmap/homeeditor.cpp b/ground/openpilotgcs/src/plugins/opmap/homeeditor.cpp index 47b5466f0..299dc3b12 100644 --- a/ground/openpilotgcs/src/plugins/opmap/homeeditor.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/homeeditor.cpp @@ -32,13 +32,12 @@ homeEditor::homeEditor(HomeItem *home, QWidget *parent) : ui(new Ui::homeEditor), myhome(home) { - if(!home) - { + if (!home) { deleteLater(); return; } ui->setupUi(this); - this->setAttribute(Qt::WA_DeleteOnClose,true); + this->setAttribute(Qt::WA_DeleteOnClose, true); ui->altitude->setValue(home->Altitude()); ui->latitude->setValue(home->Coord().Lat()); ui->longitude->setValue(home->Coord().Lng()); @@ -52,7 +51,7 @@ homeEditor::~homeEditor() void homeEditor::on_buttonBox_accepted() { - myhome->SetCoord(internals::PointLatLng(ui->latitude->value(),ui->longitude->value())); + myhome->SetCoord(internals::PointLatLng(ui->latitude->value(), ui->longitude->value())); myhome->SetAltitude(ui->altitude->value()); } diff --git a/ground/openpilotgcs/src/plugins/opmap/homeeditor.h b/ground/openpilotgcs/src/plugins/opmap/homeeditor.h index e0404e6b9..89905357d 100644 --- a/ground/openpilotgcs/src/plugins/opmap/homeeditor.h +++ b/ground/openpilotgcs/src/plugins/opmap/homeeditor.h @@ -36,14 +36,13 @@ namespace Ui { class homeEditor; } -class homeEditor : public QDialog -{ +class homeEditor : public QDialog { Q_OBJECT - + public: - explicit homeEditor(HomeItem * home,QWidget *parent = 0); + explicit homeEditor(HomeItem *home, QWidget *parent = 0); ~homeEditor(); - + private slots: void on_buttonBox_accepted(); @@ -51,7 +50,7 @@ private slots: private: Ui::homeEditor *ui; - HomeItem * myhome; + HomeItem *myhome; }; #endif // HOMEEDITOR_H diff --git a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp index a802d69f8..2dac1b0ac 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp @@ -27,45 +27,48 @@ #include "modelmapproxy.h" -modelMapProxy::modelMapProxy(QObject *parent,OPMapWidget *map,flightDataModel * model,QItemSelectionModel * selectionModel):QObject(parent),myMap(map),model(model),selection(selectionModel) +modelMapProxy::modelMapProxy(QObject *parent, OPMapWidget *map, flightDataModel *model, QItemSelectionModel *selectionModel) : QObject(parent), myMap(map), model(model), selection(selectionModel) { - connect(model,SIGNAL(rowsInserted(const QModelIndex&,int,int)),this,SLOT(rowsInserted(const QModelIndex&,int,int))); - connect(model,SIGNAL(rowsRemoved(const QModelIndex&,int,int)),this,SLOT(rowsRemoved(const QModelIndex&,int,int))); - connect(selection,SIGNAL(currentRowChanged(QModelIndex,QModelIndex)),this,SLOT(currentRowChanged(QModelIndex,QModelIndex))); - connect(model,SIGNAL(dataChanged(QModelIndex,QModelIndex)),this,SLOT(dataChanged(QModelIndex,QModelIndex))); - connect(myMap,SIGNAL(selectedWPChanged(QList)),this,SLOT(selectedWPChanged(QList))); - connect(myMap,SIGNAL(WPValuesChanged(WayPointItem*)),this,SLOT(WPValuesChanged(WayPointItem*))); + connect(model, SIGNAL(rowsInserted(const QModelIndex &, int, int)), this, SLOT(rowsInserted(const QModelIndex &, int, int))); + connect(model, SIGNAL(rowsRemoved(const QModelIndex &, int, int)), this, SLOT(rowsRemoved(const QModelIndex &, int, int))); + connect(selection, SIGNAL(currentRowChanged(QModelIndex, QModelIndex)), this, SLOT(currentRowChanged(QModelIndex, QModelIndex))); + connect(model, SIGNAL(dataChanged(QModelIndex, QModelIndex)), this, SLOT(dataChanged(QModelIndex, QModelIndex))); + connect(myMap, SIGNAL(selectedWPChanged(QList)), this, SLOT(selectedWPChanged(QList))); + connect(myMap, SIGNAL(WPValuesChanged(WayPointItem *)), this, SLOT(WPValuesChanged(WayPointItem *))); } -void modelMapProxy::WPValuesChanged(WayPointItem * wp) +void modelMapProxy::WPValuesChanged(WayPointItem *wp) { QModelIndex index; - index=model->index(wp->Number(),flightDataModel::LATPOSITION); - if(!index.isValid()) + + index = model->index(wp->Number(), flightDataModel::LATPOSITION); + if (!index.isValid()) { return; - model->setData(index,wp->Coord().Lat(),Qt::EditRole); - index=model->index(wp->Number(),flightDataModel::LNGPOSITION); - model->setData(index,wp->Coord().Lng(),Qt::EditRole); + } + model->setData(index, wp->Coord().Lat(), Qt::EditRole); + index = model->index(wp->Number(), flightDataModel::LNGPOSITION); + model->setData(index, wp->Coord().Lng(), Qt::EditRole); - index=model->index(wp->Number(),flightDataModel::ALTITUDE); - model->setData(index,wp->Altitude(),Qt::EditRole); + index = model->index(wp->Number(), flightDataModel::ALTITUDE); + model->setData(index, wp->Altitude(), Qt::EditRole); - index=model->index(wp->Number(),flightDataModel::DISRELATIVE); - model->setData(index,wp->getRelativeCoord().distance,Qt::EditRole); - index=model->index(wp->Number(),flightDataModel::BEARELATIVE); - model->setData(index,wp->getRelativeCoord().bearingToDegrees(),Qt::EditRole); - index=model->index(wp->Number(),flightDataModel::ALTITUDERELATIVE); - model->setData(index,wp->getRelativeCoord().altitudeRelative,Qt::EditRole); + index = model->index(wp->Number(), flightDataModel::DISRELATIVE); + model->setData(index, wp->getRelativeCoord().distance, Qt::EditRole); + index = model->index(wp->Number(), flightDataModel::BEARELATIVE); + model->setData(index, wp->getRelativeCoord().bearingToDegrees(), Qt::EditRole); + index = model->index(wp->Number(), flightDataModel::ALTITUDERELATIVE); + model->setData(index, wp->getRelativeCoord().altitudeRelative, Qt::EditRole); } void modelMapProxy::currentRowChanged(QModelIndex current, QModelIndex previous) { Q_UNUSED(previous); - QList list; - WayPointItem * wp=findWayPointNumber(current.row()); - if(!wp) + QList list; + WayPointItem *wp = findWayPointNumber(current.row()); + if (!wp) { return; + } list.append(wp); myMap->setSelectedWP(list); } @@ -73,132 +76,132 @@ void modelMapProxy::currentRowChanged(QModelIndex current, QModelIndex previous) void modelMapProxy::selectedWPChanged(QList list) { selection->clearSelection(); - foreach(WayPointItem * wp,list) - { - QModelIndex index=model->index(wp->Number(),0); - selection->setCurrentIndex(index,QItemSelectionModel::Select | QItemSelectionModel::Rows); + foreach(WayPointItem * wp, list) { + QModelIndex index = model->index(wp->Number(), 0); + + selection->setCurrentIndex(index, QItemSelectionModel::Select | QItemSelectionModel::Rows); } } modelMapProxy::overlayType modelMapProxy::overlayTranslate(int type) { - switch(type) - { + switch (type) { case MapDataDelegate::MODE_FLYENDPOINT: case MapDataDelegate::MODE_FLYVECTOR: case MapDataDelegate::MODE_DRIVEENDPOINT: case MapDataDelegate::MODE_DRIVEVECTOR: - return OVERLAY_LINE; + return OVERLAY_LINE; + break; case MapDataDelegate::MODE_FLYCIRCLERIGHT: case MapDataDelegate::MODE_DRIVECIRCLERIGHT: return OVERLAY_CIRCLE_RIGHT; - break; + + break; case MapDataDelegate::MODE_FLYCIRCLELEFT: case MapDataDelegate::MODE_DRIVECIRCLELEFT: return OVERLAY_CIRCLE_LEFT; + break; default: break; } } -void modelMapProxy::createOverlay(WayPointItem *from, WayPointItem *to, modelMapProxy::overlayType type,QColor color) +void modelMapProxy::createOverlay(WayPointItem *from, WayPointItem *to, modelMapProxy::overlayType type, QColor color) { - if(from==NULL || to==NULL || from==to) + if (from == NULL || to == NULL || from == to) { return; - switch(type) - { + } + switch (type) { case OVERLAY_LINE: - myMap->WPLineCreate(from,to,color); + myMap->WPLineCreate(from, to, color); break; case OVERLAY_CIRCLE_RIGHT: - myMap->WPCircleCreate(to,from,true,color); + myMap->WPCircleCreate(to, from, true, color); break; case OVERLAY_CIRCLE_LEFT: - myMap->WPCircleCreate(to,from,false,color); + myMap->WPCircleCreate(to, from, false, color); break; default: break; - } } -void modelMapProxy::createOverlay(WayPointItem *from, HomeItem *to, modelMapProxy::overlayType type,QColor color) +void modelMapProxy::createOverlay(WayPointItem *from, HomeItem *to, modelMapProxy::overlayType type, QColor color) { - if(from==NULL || to==NULL) + if (from == NULL || to == NULL) { return; - switch(type) - { + } + switch (type) { case OVERLAY_LINE: - myMap->WPLineCreate(to,from,color); + myMap->WPLineCreate(to, from, color); break; case OVERLAY_CIRCLE_RIGHT: - myMap->WPCircleCreate(to,from,true,color); + myMap->WPCircleCreate(to, from, true, color); break; case OVERLAY_CIRCLE_LEFT: - myMap->WPCircleCreate(to,from,false,color); + myMap->WPCircleCreate(to, from, false, color); break; default: break; - } } void modelMapProxy::refreshOverlays() { myMap->deleteAllOverlays(); - if(model->rowCount()<1) + if (model->rowCount() < 1) { return; - WayPointItem * wp_current=NULL; - WayPointItem * wp_next=NULL; + } + WayPointItem *wp_current = NULL; + WayPointItem *wp_next = NULL; int wp_jump; int wp_error; overlayType wp_next_overlay; overlayType wp_jump_overlay; overlayType wp_error_overlay; - wp_current=findWayPointNumber(0); - overlayType wp_current_overlay=overlayTranslate(model->data(model->index(0,flightDataModel::MODE)).toInt()); - createOverlay(wp_current,myMap->Home,wp_current_overlay,Qt::green); - for(int x=0;xrowCount();++x) - { - wp_current=findWayPointNumber(x); - wp_jump=model->data(model->index(x,flightDataModel::JUMPDESTINATION)).toInt()-1; - wp_error=model->data(model->index(x,flightDataModel::ERRORDESTINATION)).toInt()-1; - wp_next_overlay=overlayTranslate(model->data(model->index(x+1,flightDataModel::MODE)).toInt()); - wp_jump_overlay=overlayTranslate(model->data(model->index(wp_jump,flightDataModel::MODE)).toInt()); - wp_error_overlay=overlayTranslate(model->data(model->index(wp_error,flightDataModel::MODE)).toInt()); - createOverlay(wp_current,findWayPointNumber(wp_error),wp_error_overlay,Qt::red); - switch(model->data(model->index(x,flightDataModel::COMMAND)).toInt()) - { + wp_current = findWayPointNumber(0); + overlayType wp_current_overlay = overlayTranslate(model->data(model->index(0, flightDataModel::MODE)).toInt()); + createOverlay(wp_current, myMap->Home, wp_current_overlay, Qt::green); + for (int x = 0; x < model->rowCount(); ++x) { + wp_current = findWayPointNumber(x); + wp_jump = model->data(model->index(x, flightDataModel::JUMPDESTINATION)).toInt() - 1; + wp_error = model->data(model->index(x, flightDataModel::ERRORDESTINATION)).toInt() - 1; + wp_next_overlay = overlayTranslate(model->data(model->index(x + 1, flightDataModel::MODE)).toInt()); + wp_jump_overlay = overlayTranslate(model->data(model->index(wp_jump, flightDataModel::MODE)).toInt()); + wp_error_overlay = overlayTranslate(model->data(model->index(wp_error, flightDataModel::MODE)).toInt()); + createOverlay(wp_current, findWayPointNumber(wp_error), wp_error_overlay, Qt::red); + switch (model->data(model->index(x, flightDataModel::COMMAND)).toInt()) { case MapDataDelegate::COMMAND_ONCONDITIONNEXTWAYPOINT: - wp_next=findWayPointNumber(x+1); - createOverlay(wp_current,wp_next,wp_next_overlay,Qt::green); + wp_next = findWayPointNumber(x + 1); + createOverlay(wp_current, wp_next, wp_next_overlay, Qt::green); break; case MapDataDelegate::COMMAND_ONCONDITIONJUMPWAYPOINT: - wp_next=findWayPointNumber(wp_jump); - createOverlay(wp_current,wp_next,wp_jump_overlay,Qt::green); + wp_next = findWayPointNumber(wp_jump); + createOverlay(wp_current, wp_next, wp_jump_overlay, Qt::green); break; case MapDataDelegate::COMMAND_ONNOTCONDITIONJUMPWAYPOINT: - wp_next=findWayPointNumber(wp_jump); - createOverlay(wp_current,wp_next,wp_jump_overlay,Qt::yellow); + wp_next = findWayPointNumber(wp_jump); + createOverlay(wp_current, wp_next, wp_jump_overlay, Qt::yellow); break; case MapDataDelegate::COMMAND_ONNOTCONDITIONNEXTWAYPOINT: - wp_next=findWayPointNumber(x+1); - createOverlay(wp_current,wp_next,wp_next_overlay,Qt::yellow); + wp_next = findWayPointNumber(x + 1); + createOverlay(wp_current, wp_next, wp_next_overlay, Qt::yellow); break; case MapDataDelegate::COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: - wp_next=findWayPointNumber(wp_jump); - createOverlay(wp_current,wp_next,wp_jump_overlay,Qt::green); - wp_next=findWayPointNumber(x+1); - createOverlay(wp_current,wp_next,wp_next_overlay,Qt::green); + wp_next = findWayPointNumber(wp_jump); + createOverlay(wp_current, wp_next, wp_jump_overlay, Qt::green); + wp_next = findWayPointNumber(x + 1); + createOverlay(wp_current, wp_next, wp_next_overlay, Qt::green); break; } } } -WayPointItem * modelMapProxy::findWayPointNumber(int number) +WayPointItem *modelMapProxy::findWayPointNumber(int number) { - if(number<0) + if (number < 0) { return NULL; + } return myMap->WPFind(number); } @@ -206,8 +209,7 @@ void modelMapProxy::rowsRemoved(const QModelIndex &parent, int first, int last) { Q_UNUSED(parent); - for(int x=last;x>first-1;x--) - { + for (int x = last; x > first - 1; x--) { myMap->WPDelete(x); } refreshOverlays(); @@ -217,76 +219,77 @@ void modelMapProxy::dataChanged(const QModelIndex &topLeft, const QModelIndex &b { Q_UNUSED(bottomRight); - WayPointItem * item=findWayPointNumber(topLeft.row()); - if(!item) + WayPointItem *item = findWayPointNumber(topLeft.row()); + if (!item) { return; + } internals::PointLatLng latlng; - int x=topLeft.row(); + int x = topLeft.row(); distBearingAltitude distBearing; double altitude; bool relative; QModelIndex index; QString desc; - switch(topLeft.column()) - { + switch (topLeft.column()) { case flightDataModel::COMMAND: case flightDataModel::CONDITION: case flightDataModel::JUMPDESTINATION: case flightDataModel::ERRORDESTINATION: case flightDataModel::MODE: refreshOverlays(); - break; + break; case flightDataModel::WPDESCRITPTION: - index=model->index(x,flightDataModel::WPDESCRITPTION); - desc=index.data(Qt::DisplayRole).toString(); + index = model->index(x, flightDataModel::WPDESCRITPTION); + desc = index.data(Qt::DisplayRole).toString(); item->SetDescription(desc); break; case flightDataModel::LATPOSITION: - latlng=item->Coord(); - index=model->index(x,flightDataModel::LATPOSITION); + latlng = item->Coord(); + index = model->index(x, flightDataModel::LATPOSITION); latlng.SetLat(index.data(Qt::DisplayRole).toDouble()); item->SetCoord(latlng); break; case flightDataModel::LNGPOSITION: - latlng=item->Coord(); - index=model->index(x,flightDataModel::LNGPOSITION); + latlng = item->Coord(); + index = model->index(x, flightDataModel::LNGPOSITION); latlng.SetLng(index.data(Qt::DisplayRole).toDouble()); item->SetCoord(latlng); break; case flightDataModel::BEARELATIVE: - distBearing=item->getRelativeCoord(); - index=model->index(x,flightDataModel::BEARELATIVE); + distBearing = item->getRelativeCoord(); + index = model->index(x, flightDataModel::BEARELATIVE); distBearing.setBearingFromDegrees(index.data(Qt::DisplayRole).toDouble()); item->setRelativeCoord(distBearing); break; case flightDataModel::DISRELATIVE: - distBearing=item->getRelativeCoord(); - index=model->index(x,flightDataModel::DISRELATIVE); - distBearing.distance=index.data(Qt::DisplayRole).toDouble(); + distBearing = item->getRelativeCoord(); + index = model->index(x, flightDataModel::DISRELATIVE); + distBearing.distance = index.data(Qt::DisplayRole).toDouble(); item->setRelativeCoord(distBearing); break; case flightDataModel::ALTITUDERELATIVE: - distBearing=item->getRelativeCoord(); - index=model->index(x,flightDataModel::ALTITUDERELATIVE); - distBearing.altitudeRelative=index.data(Qt::DisplayRole).toFloat(); + distBearing = item->getRelativeCoord(); + index = model->index(x, flightDataModel::ALTITUDERELATIVE); + distBearing.altitudeRelative = index.data(Qt::DisplayRole).toFloat(); item->setRelativeCoord(distBearing); break; case flightDataModel::ISRELATIVE: - index=model->index(x,flightDataModel::ISRELATIVE); - relative=index.data(Qt::DisplayRole).toBool(); - if(relative) + index = model->index(x, flightDataModel::ISRELATIVE); + relative = index.data(Qt::DisplayRole).toBool(); + if (relative) { item->setWPType(mapcontrol::WayPointItem::relative); - else + } else { item->setWPType(mapcontrol::WayPointItem::absolute); + } break; case flightDataModel::ALTITUDE: - index=model->index(x,flightDataModel::ALTITUDE); - altitude=index.data(Qt::DisplayRole).toDouble(); + index = model->index(x, flightDataModel::ALTITUDE); + altitude = index.data(Qt::DisplayRole).toDouble(); item->SetAltitude(altitude); break; case flightDataModel::LOCKED: - index=model->index(x,flightDataModel::LOCKED); - item->setFlag(QGraphicsItem::ItemIsMovable,!index.data(Qt::DisplayRole).toBool()); + index = model->index(x, flightDataModel::LOCKED); + item->setFlag(QGraphicsItem::ItemIsMovable, !index.data(Qt::DisplayRole).toBool()); break; } } @@ -294,55 +297,55 @@ void modelMapProxy::dataChanged(const QModelIndex &topLeft, const QModelIndex &b void modelMapProxy::rowsInserted(const QModelIndex &parent, int first, int last) { Q_UNUSED(parent); - for(int x=first;xindex(x,flightDataModel::WPDESCRITPTION); - QString desc=index.data(Qt::DisplayRole).toString(); - index=model->index(x,flightDataModel::LATPOSITION); + index = model->index(x, flightDataModel::WPDESCRITPTION); + QString desc = index.data(Qt::DisplayRole).toString(); + index = model->index(x, flightDataModel::LATPOSITION); latlng.SetLat(index.data(Qt::DisplayRole).toDouble()); - index=model->index(x,flightDataModel::LNGPOSITION); + index = model->index(x, flightDataModel::LNGPOSITION); latlng.SetLng(index.data(Qt::DisplayRole).toDouble()); - index=model->index(x,flightDataModel::DISRELATIVE); - distBearing.distance=index.data(Qt::DisplayRole).toDouble(); - index=model->index(x,flightDataModel::BEARELATIVE); + index = model->index(x, flightDataModel::DISRELATIVE); + distBearing.distance = index.data(Qt::DisplayRole).toDouble(); + index = model->index(x, flightDataModel::BEARELATIVE); distBearing.setBearingFromDegrees(index.data(Qt::DisplayRole).toDouble()); - index=model->index(x,flightDataModel::ALTITUDERELATIVE); - distBearing.altitudeRelative=index.data(Qt::DisplayRole).toFloat(); - index=model->index(x,flightDataModel::ISRELATIVE); - relative=index.data(Qt::DisplayRole).toBool(); - index=model->index(x,flightDataModel::ALTITUDE); - altitude=index.data(Qt::DisplayRole).toDouble(); - if(relative) - item=myMap->WPInsert(distBearing,desc,x); - else - item=myMap->WPInsert(latlng,altitude,desc,x); + index = model->index(x, flightDataModel::ALTITUDERELATIVE); + distBearing.altitudeRelative = index.data(Qt::DisplayRole).toFloat(); + index = model->index(x, flightDataModel::ISRELATIVE); + relative = index.data(Qt::DisplayRole).toBool(); + index = model->index(x, flightDataModel::ALTITUDE); + altitude = index.data(Qt::DisplayRole).toDouble(); + if (relative) { + item = myMap->WPInsert(distBearing, desc, x); + } else { + item = myMap->WPInsert(latlng, altitude, desc, x); + } } refreshOverlays(); } void modelMapProxy::deleteWayPoint(int number) { - model->removeRow(number,QModelIndex()); + model->removeRow(number, QModelIndex()); } void modelMapProxy::createWayPoint(internals::PointLatLng coord) { - model->insertRow(model->rowCount(),QModelIndex()); - QModelIndex index=model->index(model->rowCount()-1,flightDataModel::LATPOSITION,QModelIndex()); - model->setData(index,coord.Lat(),Qt::EditRole); - index=model->index(model->rowCount()-1,flightDataModel::LNGPOSITION,QModelIndex()); - model->setData(index,coord.Lng(),Qt::EditRole); - index=model->index(model->rowCount()-1,flightDataModel::JUMPDESTINATION,QModelIndex()); - model->setData(index,1,Qt::EditRole); - index=model->index(model->rowCount()-1,flightDataModel::ERRORDESTINATION,QModelIndex()); - model->setData(index,1,Qt::EditRole); + model->insertRow(model->rowCount(), QModelIndex()); + QModelIndex index = model->index(model->rowCount() - 1, flightDataModel::LATPOSITION, QModelIndex()); + model->setData(index, coord.Lat(), Qt::EditRole); + index = model->index(model->rowCount() - 1, flightDataModel::LNGPOSITION, QModelIndex()); + model->setData(index, coord.Lng(), Qt::EditRole); + index = model->index(model->rowCount() - 1, flightDataModel::JUMPDESTINATION, QModelIndex()); + model->setData(index, 1, Qt::EditRole); + index = model->index(model->rowCount() - 1, flightDataModel::ERRORDESTINATION, QModelIndex()); + model->setData(index, 1, Qt::EditRole); } void modelMapProxy::deleteAll() { - model->removeRows(0,model->rowCount(),QModelIndex()); + model->removeRows(0, model->rowCount(), QModelIndex()); } diff --git a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.h b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.h index 29c61c9b1..04269c9ba 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.h @@ -38,31 +38,30 @@ using namespace mapcontrol; -class modelMapProxy:public QObject -{ - typedef enum {OVERLAY_LINE,OVERLAY_CIRCLE_RIGHT,OVERLAY_CIRCLE_LEFT} overlayType; +class modelMapProxy : public QObject { + typedef enum { OVERLAY_LINE, OVERLAY_CIRCLE_RIGHT, OVERLAY_CIRCLE_LEFT } overlayType; Q_OBJECT public: - explicit modelMapProxy(QObject *parent,OPMapWidget * map,flightDataModel * model,QItemSelectionModel * selectionModel); + explicit modelMapProxy(QObject *parent, OPMapWidget *map, flightDataModel *model, QItemSelectionModel *selectionModel); WayPointItem *findWayPointNumber(int number); void createWayPoint(internals::PointLatLng coord); void deleteWayPoint(int number); void deleteAll(); private slots: - void dataChanged ( const QModelIndex & topLeft, const QModelIndex & bottomRight ); - void rowsInserted ( const QModelIndex & parent, int first, int last ); - void rowsRemoved ( const QModelIndex & parent, int first, int last ); + void dataChanged(const QModelIndex & topLeft, const QModelIndex & bottomRight); + void rowsInserted(const QModelIndex & parent, int first, int last); + void rowsRemoved(const QModelIndex & parent, int first, int last); void WPValuesChanged(WayPointItem *wp); - void currentRowChanged(QModelIndex,QModelIndex); - void selectedWPChanged(QList); + void currentRowChanged(QModelIndex, QModelIndex); + void selectedWPChanged(QList); private: overlayType overlayTranslate(int type); - void createOverlay(WayPointItem * from,WayPointItem * to,overlayType type,QColor color); + void createOverlay(WayPointItem *from, WayPointItem *to, overlayType type, QColor color); void createOverlay(WayPointItem *from, HomeItem *to, modelMapProxy::overlayType type, QColor color); - OPMapWidget * myMap; - flightDataModel * model; + OPMapWidget *myMap; + flightDataModel *model; void refreshOverlays(); - QItemSelectionModel * selection; + QItemSelectionModel *selection; }; #endif // MODELMAPPROXY_H diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index 192f5b932..bc8c580ed 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -27,93 +27,92 @@ #include "modeluavoproxy.h" #include "extensionsystem/pluginmanager.h" #include -modelUavoProxy::modelUavoProxy(QObject *parent,flightDataModel * model):QObject(parent),myModel(model) +modelUavoProxy::modelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm != NULL); - objManager = pm->getObject(); + objManager = pm->getObject(); Q_ASSERT(objManager != NULL); - waypointObj = Waypoint::GetInstance(objManager); + waypointObj = Waypoint::GetInstance(objManager); Q_ASSERT(waypointObj != NULL); - pathactionObj=PathAction::GetInstance(objManager); + pathactionObj = PathAction::GetInstance(objManager); Q_ASSERT(pathactionObj != NULL); } void modelUavoProxy::modelToObjects() { - PathAction * act=NULL; - Waypoint * wp=NULL; + PathAction *act = NULL; + Waypoint *wp = NULL; QModelIndex index; double distance; double bearing; double altitude; - int lastaction=-1; - for(int x=0;xrowCount();++x) - { - int instances=objManager->getNumInstances(waypointObj->getObjID()); - if(x>instances-1) - { - wp=new Waypoint; - wp->initialize(x,wp->getMetaObject()); + int lastaction = -1; + + for (int x = 0; x < myModel->rowCount(); ++x) { + int instances = objManager->getNumInstances(waypointObj->getObjID()); + if (x > instances - 1) { + wp = new Waypoint; + wp->initialize(x, wp->getMetaObject()); objManager->registerObject(wp); + } else { + wp = Waypoint::GetInstance(objManager, x); } - else - { - wp=Waypoint::GetInstance(objManager,x); - } - act=new PathAction; + act = new PathAction; Q_ASSERT(act); Q_ASSERT(wp); Waypoint::DataFields waypoint = wp->getData(); PathAction::DataFields action = act->getData(); ///Waypoint object data - index=myModel->index(x,flightDataModel::DISRELATIVE); - distance=myModel->data(index).toDouble(); - index=myModel->index(x,flightDataModel::BEARELATIVE); - bearing=myModel->data(index).toDouble(); - index=myModel->index(x,flightDataModel::ALTITUDERELATIVE); - altitude=myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::VELOCITY); - waypoint.Velocity=myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::DISRELATIVE); + distance = myModel->data(index).toDouble(); + index = myModel->index(x, flightDataModel::BEARELATIVE); + bearing = myModel->data(index).toDouble(); + index = myModel->index(x, flightDataModel::ALTITUDERELATIVE); + altitude = myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::VELOCITY); + waypoint.Velocity = myModel->data(index).toFloat(); - waypoint.Position[Waypoint::POSITION_NORTH]=distance*cos(bearing/180*M_PI); - waypoint.Position[Waypoint::POSITION_EAST]=distance*sin(bearing/180*M_PI); - waypoint.Position[Waypoint::POSITION_DOWN]=(-1.0f)*altitude; + waypoint.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / 180 * M_PI); + waypoint.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / 180 * M_PI); + waypoint.Position[Waypoint::POSITION_DOWN] = (-1.0f) * altitude; ///PathAction object data - index=myModel->index(x,flightDataModel::MODE); - action.Mode=myModel->data(index).toInt(); - index=myModel->index(x,flightDataModel::MODE_PARAMS0); - action.ModeParameters[0]=myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::MODE_PARAMS1); - action.ModeParameters[1]=myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::MODE_PARAMS2); - action.ModeParameters[2]=myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::MODE_PARAMS3); - action.ModeParameters[3]=myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::MODE); + action.Mode = myModel->data(index).toInt(); + index = myModel->index(x, flightDataModel::MODE_PARAMS0); + action.ModeParameters[0] = myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::MODE_PARAMS1); + action.ModeParameters[1] = myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::MODE_PARAMS2); + action.ModeParameters[2] = myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::MODE_PARAMS3); + action.ModeParameters[3] = myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::CONDITION); - action.EndCondition=myModel->data(index).toInt(); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS0); - action.ConditionParameters[0]=myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS1); - action.ConditionParameters[1]=myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS2); - action.ConditionParameters[2]=myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS3); - action.ConditionParameters[3]=myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::CONDITION); + action.EndCondition = myModel->data(index).toInt(); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS0); + action.ConditionParameters[0] = myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS1); + action.ConditionParameters[1] = myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS2); + action.ConditionParameters[2] = myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS3); + action.ConditionParameters[3] = myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::COMMAND); - action.Command=myModel->data(index).toInt(); - index=myModel->index(x,flightDataModel::JUMPDESTINATION); - action.JumpDestination=myModel->data(index).toInt()-1; - index=myModel->index(x,flightDataModel::ERRORDESTINATION); - action.ErrorDestination=myModel->data(index).toInt()-1; + index = myModel->index(x, flightDataModel::COMMAND); + action.Command = myModel->data(index).toInt(); + index = myModel->index(x, flightDataModel::JUMPDESTINATION); + action.JumpDestination = myModel->data(index).toInt() - 1; + index = myModel->index(x, flightDataModel::ERRORDESTINATION); + action.ErrorDestination = myModel->data(index).toInt() - 1; - int actionNumber=addAction(act,action,lastaction); - if(actionNumber>lastaction) - lastaction=actionNumber; - waypoint.Action=actionNumber; + int actionNumber = addAction(act, action, lastaction); + if (actionNumber > lastaction) { + lastaction = actionNumber; + } + waypoint.Action = actionNumber; wp->setData(waypoint); wp->updated(); } @@ -121,129 +120,128 @@ void modelUavoProxy::modelToObjects() void modelUavoProxy::objectsToModel() { - Waypoint * wp; + Waypoint *wp; Waypoint::DataFields wpfields; - PathAction * action; + PathAction *action; QModelIndex index; double distance; double bearing; PathAction::DataFields actionfields; - myModel->removeRows(0,myModel->rowCount()); - for(int x=0;xgetNumInstances(waypointObj->getObjID());++x) - { - wp=Waypoint::GetInstance(objManager,x); + myModel->removeRows(0, myModel->rowCount()); + for (int x = 0; x < objManager->getNumInstances(waypointObj->getObjID()); ++x) { + wp = Waypoint::GetInstance(objManager, x); Q_ASSERT(wp); - if(!wp) + if (!wp) { continue; - wpfields=wp->getData(); + } + wpfields = wp->getData(); myModel->insertRow(x); - index=myModel->index(x,flightDataModel::VELOCITY); - myModel->setData(index,wpfields.Velocity); - distance=sqrt(wpfields.Position[Waypoint::POSITION_NORTH]*wpfields.Position[Waypoint::POSITION_NORTH]+ - wpfields.Position[Waypoint::POSITION_EAST]*wpfields.Position[Waypoint::POSITION_EAST]); - bearing=atan2(wpfields.Position[Waypoint::POSITION_EAST],wpfields.Position[Waypoint::POSITION_NORTH])*180/M_PI; + index = myModel->index(x, flightDataModel::VELOCITY); + myModel->setData(index, wpfields.Velocity); + distance = sqrt(wpfields.Position[Waypoint::POSITION_NORTH] * wpfields.Position[Waypoint::POSITION_NORTH] + + wpfields.Position[Waypoint::POSITION_EAST] * wpfields.Position[Waypoint::POSITION_EAST]); + bearing = atan2(wpfields.Position[Waypoint::POSITION_EAST], wpfields.Position[Waypoint::POSITION_NORTH]) * 180 / M_PI; - if(bearing!=bearing) - bearing=0; - index=myModel->index(x,flightDataModel::DISRELATIVE); - myModel->setData(index,distance); - index=myModel->index(x,flightDataModel::BEARELATIVE); - myModel->setData(index,bearing); - index=myModel->index(x,flightDataModel::ALTITUDERELATIVE); - myModel->setData(index,(-1.0f)*wpfields.Position[Waypoint::POSITION_DOWN]); + if (bearing != bearing) { + bearing = 0; + } + index = myModel->index(x, flightDataModel::DISRELATIVE); + myModel->setData(index, distance); + index = myModel->index(x, flightDataModel::BEARELATIVE); + myModel->setData(index, bearing); + index = myModel->index(x, flightDataModel::ALTITUDERELATIVE); + myModel->setData(index, (-1.0f) * wpfields.Position[Waypoint::POSITION_DOWN]); - action=PathAction::GetInstance(objManager,wpfields.Action); + action = PathAction::GetInstance(objManager, wpfields.Action); Q_ASSERT(action); - if(!action) + if (!action) { continue; - actionfields=action->getData(); + } + actionfields = action->getData(); - index=myModel->index(x,flightDataModel::ISRELATIVE); - myModel->setData(index,true); + index = myModel->index(x, flightDataModel::ISRELATIVE); + myModel->setData(index, true); - index=myModel->index(x,flightDataModel::COMMAND); - myModel->setData(index,actionfields.Command); + index = myModel->index(x, flightDataModel::COMMAND); + myModel->setData(index, actionfields.Command); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS0); - myModel->setData(index,actionfields.ConditionParameters[0]); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS1); - myModel->setData(index,actionfields.ConditionParameters[1]); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS2); - myModel->setData(index,actionfields.ConditionParameters[2]); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS3); - myModel->setData(index,actionfields.ConditionParameters[3]); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS0); + myModel->setData(index, actionfields.ConditionParameters[0]); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS1); + myModel->setData(index, actionfields.ConditionParameters[1]); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS2); + myModel->setData(index, actionfields.ConditionParameters[2]); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS3); + myModel->setData(index, actionfields.ConditionParameters[3]); - index=myModel->index(x,flightDataModel::CONDITION); - myModel->setData(index,actionfields.EndCondition); + index = myModel->index(x, flightDataModel::CONDITION); + myModel->setData(index, actionfields.EndCondition); - index=myModel->index(x,flightDataModel::ERRORDESTINATION); - myModel->setData(index,actionfields.ErrorDestination+1); + index = myModel->index(x, flightDataModel::ERRORDESTINATION); + myModel->setData(index, actionfields.ErrorDestination + 1); - index=myModel->index(x,flightDataModel::JUMPDESTINATION); - myModel->setData(index,actionfields.JumpDestination+1); + index = myModel->index(x, flightDataModel::JUMPDESTINATION); + myModel->setData(index, actionfields.JumpDestination + 1); - index=myModel->index(x,flightDataModel::MODE); - myModel->setData(index,actionfields.Mode); + index = myModel->index(x, flightDataModel::MODE); + myModel->setData(index, actionfields.Mode); - index=myModel->index(x,flightDataModel::MODE_PARAMS0); - myModel->setData(index,actionfields.ModeParameters[0]); - index=myModel->index(x,flightDataModel::MODE_PARAMS1); - myModel->setData(index,actionfields.ModeParameters[1]); - index=myModel->index(x,flightDataModel::MODE_PARAMS2); - myModel->setData(index,actionfields.ModeParameters[2]); - index=myModel->index(x,flightDataModel::MODE_PARAMS3); - myModel->setData(index,actionfields.ModeParameters[3]); + index = myModel->index(x, flightDataModel::MODE_PARAMS0); + myModel->setData(index, actionfields.ModeParameters[0]); + index = myModel->index(x, flightDataModel::MODE_PARAMS1); + myModel->setData(index, actionfields.ModeParameters[1]); + index = myModel->index(x, flightDataModel::MODE_PARAMS2); + myModel->setData(index, actionfields.ModeParameters[2]); + index = myModel->index(x, flightDataModel::MODE_PARAMS3); + myModel->setData(index, actionfields.ModeParameters[3]); } } -int modelUavoProxy::addAction(PathAction * actionObj,PathAction::DataFields actionFields,int lastaction) +int modelUavoProxy::addAction(PathAction *actionObj, PathAction::DataFields actionFields, int lastaction) { - //check if a similar action already exhists - int instances=objManager->getNumInstances(pathactionObj->getObjID()); - for(int x=0;xgetNumInstances(pathactionObj->getObjID()); + + for (int x = 0; x < lastaction + 1; ++x) { + PathAction *action = PathAction::GetInstance(objManager, x); Q_ASSERT(action); - if(!action) + if (!action) { continue; - PathAction::DataFields fields=action->getData(); - if(fields.Command==actionFields.Command - && fields.ConditionParameters[0]==actionFields.ConditionParameters[0] - && fields.ConditionParameters[1]==actionFields.ConditionParameters[1] - && fields.ConditionParameters[2]==actionFields.ConditionParameters[2] - &&fields.EndCondition==actionFields.EndCondition - &&fields.ErrorDestination==actionFields.ErrorDestination - &&fields.JumpDestination==actionFields.JumpDestination - &&fields.Mode==actionFields.Mode - &&fields.ModeParameters[0]==actionFields.ModeParameters[0] - &&fields.ModeParameters[1]==actionFields.ModeParameters[1] - &&fields.ModeParameters[2]==actionFields.ModeParameters[2]) - { - qDebug()<<"ModelUAVProxy:"<<"found similar action instance:"<getData(); + if (fields.Command == actionFields.Command + && fields.ConditionParameters[0] == actionFields.ConditionParameters[0] + && fields.ConditionParameters[1] == actionFields.ConditionParameters[1] + && fields.ConditionParameters[2] == actionFields.ConditionParameters[2] + && fields.EndCondition == actionFields.EndCondition + && fields.ErrorDestination == actionFields.ErrorDestination + && fields.JumpDestination == actionFields.JumpDestination + && fields.Mode == actionFields.Mode + && fields.ModeParameters[0] == actionFields.ModeParameters[0] + && fields.ModeParameters[1] == actionFields.ModeParameters[1] + && fields.ModeParameters[2] == actionFields.ModeParameters[2]) { + qDebug() << "ModelUAVProxy:" << "found similar action instance:" << x; actionObj->deleteLater(); return x; } } - //if we get here it means no similar action was found, we have to create it - if(instancesinitialize(instances,actionObj->getMetaObject()); + // if we get here it means no similar action was found, we have to create it + if (instances < lastaction + 2) { + actionObj->initialize(instances, actionObj->getMetaObject()); objManager->registerObject(actionObj); actionObj->setData(actionFields); actionObj->updated(); - qDebug()<<"ModelUAVProxy:"<<"created new action instance:"<setData(actionFields); action->updated(); actionObj->deleteLater(); - qDebug()<<"ModelUAVProxy:"<<"reused action instance:"<setupUi(this); - connect(ui->checkBoxLocked,SIGNAL(toggled(bool)),this,SLOT(enableEditWidgets(bool))); - connect(ui->cbMode,SIGNAL(currentIndexChanged(int)),this,SLOT(setupModeWidgets())); - connect(ui->cbCondition,SIGNAL(currentIndexChanged(int)),this,SLOT(setupConditionWidgets())); - connect(ui->pushButtonCancel,SIGNAL(clicked()),this,SLOT(pushButtonCancel_clicked())); - MapDataDelegate::loadComboBox(ui->cbMode,flightDataModel::MODE); - MapDataDelegate::loadComboBox(ui->cbCondition,flightDataModel::CONDITION); - MapDataDelegate::loadComboBox(ui->cbCommand,flightDataModel::COMMAND); + connect(ui->checkBoxLocked, SIGNAL(toggled(bool)), this, SLOT(enableEditWidgets(bool))); + connect(ui->cbMode, SIGNAL(currentIndexChanged(int)), this, SLOT(setupModeWidgets())); + connect(ui->cbCondition, SIGNAL(currentIndexChanged(int)), this, SLOT(setupConditionWidgets())); + connect(ui->pushButtonCancel, SIGNAL(clicked()), this, SLOT(pushButtonCancel_clicked())); + MapDataDelegate::loadComboBox(ui->cbMode, flightDataModel::MODE); + MapDataDelegate::loadComboBox(ui->cbCondition, flightDataModel::CONDITION); + MapDataDelegate::loadComboBox(ui->cbCommand, flightDataModel::COMMAND); mapper = new QDataWidgetMapper(this); mapper->setItemDelegate(new MapDataDelegate(this)); - connect(mapper,SIGNAL(currentIndexChanged(int)),this,SLOT(currentIndexChanged(int))); + connect(mapper, SIGNAL(currentIndexChanged(int)), this, SLOT(currentIndexChanged(int))); mapper->setModel(model); mapper->setSubmitPolicy(QDataWidgetMapper::AutoSubmit); - mapper->addMapping(ui->checkBoxLocked,flightDataModel::LOCKED); - mapper->addMapping(ui->doubleSpinBoxLatitude,flightDataModel::LATPOSITION); - mapper->addMapping(ui->doubleSpinBoxLongitude,flightDataModel::LNGPOSITION); - mapper->addMapping(ui->doubleSpinBoxAltitude,flightDataModel::ALTITUDE); - mapper->addMapping(ui->lineEditDescription,flightDataModel::WPDESCRITPTION); - mapper->addMapping(ui->checkBoxRelative,flightDataModel::ISRELATIVE); - mapper->addMapping(ui->doubleSpinBoxBearing,flightDataModel::BEARELATIVE); - mapper->addMapping(ui->doubleSpinBoxVelocity,flightDataModel::VELOCITY); - mapper->addMapping(ui->doubleSpinBoxDistance,flightDataModel::DISRELATIVE); - mapper->addMapping(ui->doubleSpinBoxRelativeAltitude,flightDataModel::ALTITUDERELATIVE); - mapper->addMapping(ui->cbMode,flightDataModel::MODE); - mapper->addMapping(ui->dsb_modeParam1,flightDataModel::MODE_PARAMS0); - mapper->addMapping(ui->dsb_modeParam2,flightDataModel::MODE_PARAMS1); - mapper->addMapping(ui->dsb_modeParam3,flightDataModel::MODE_PARAMS2); - mapper->addMapping(ui->dsb_modeParam4,flightDataModel::MODE_PARAMS3); + mapper->addMapping(ui->checkBoxLocked, flightDataModel::LOCKED); + mapper->addMapping(ui->doubleSpinBoxLatitude, flightDataModel::LATPOSITION); + mapper->addMapping(ui->doubleSpinBoxLongitude, flightDataModel::LNGPOSITION); + mapper->addMapping(ui->doubleSpinBoxAltitude, flightDataModel::ALTITUDE); + mapper->addMapping(ui->lineEditDescription, flightDataModel::WPDESCRITPTION); + mapper->addMapping(ui->checkBoxRelative, flightDataModel::ISRELATIVE); + mapper->addMapping(ui->doubleSpinBoxBearing, flightDataModel::BEARELATIVE); + mapper->addMapping(ui->doubleSpinBoxVelocity, flightDataModel::VELOCITY); + mapper->addMapping(ui->doubleSpinBoxDistance, flightDataModel::DISRELATIVE); + mapper->addMapping(ui->doubleSpinBoxRelativeAltitude, flightDataModel::ALTITUDERELATIVE); + mapper->addMapping(ui->cbMode, flightDataModel::MODE); + mapper->addMapping(ui->dsb_modeParam1, flightDataModel::MODE_PARAMS0); + mapper->addMapping(ui->dsb_modeParam2, flightDataModel::MODE_PARAMS1); + mapper->addMapping(ui->dsb_modeParam3, flightDataModel::MODE_PARAMS2); + mapper->addMapping(ui->dsb_modeParam4, flightDataModel::MODE_PARAMS3); - mapper->addMapping(ui->cbCondition,flightDataModel::CONDITION); - mapper->addMapping(ui->dsb_condParam1,flightDataModel::CONDITION_PARAMS0); - mapper->addMapping(ui->dsb_condParam2,flightDataModel::CONDITION_PARAMS1); - mapper->addMapping(ui->dsb_condParam3,flightDataModel::CONDITION_PARAMS2); - mapper->addMapping(ui->dsb_condParam4,flightDataModel::CONDITION_PARAMS0); + mapper->addMapping(ui->cbCondition, flightDataModel::CONDITION); + mapper->addMapping(ui->dsb_condParam1, flightDataModel::CONDITION_PARAMS0); + mapper->addMapping(ui->dsb_condParam2, flightDataModel::CONDITION_PARAMS1); + mapper->addMapping(ui->dsb_condParam3, flightDataModel::CONDITION_PARAMS2); + mapper->addMapping(ui->dsb_condParam4, flightDataModel::CONDITION_PARAMS0); - mapper->addMapping(ui->cbCommand,flightDataModel::COMMAND); - mapper->addMapping(ui->sbJump,flightDataModel::JUMPDESTINATION); - mapper->addMapping(ui->sbError,flightDataModel::ERRORDESTINATION); - connect(itemSelection,SIGNAL(currentRowChanged(QModelIndex,QModelIndex)),this,SLOT(currentRowChanged(QModelIndex,QModelIndex))); + mapper->addMapping(ui->cbCommand, flightDataModel::COMMAND); + mapper->addMapping(ui->sbJump, flightDataModel::JUMPDESTINATION); + mapper->addMapping(ui->sbError, flightDataModel::ERRORDESTINATION); + connect(itemSelection, SIGNAL(currentRowChanged(QModelIndex, QModelIndex)), this, SLOT(currentRowChanged(QModelIndex, QModelIndex))); } void opmap_edit_waypoint_dialog::currentIndexChanged(int index) { - ui->lbNumber->setText(QString::number(index+1)); - QModelIndex idx=mapper->model()->index(index,0); - if(index==itemSelection->currentIndex().row()) + ui->lbNumber->setText(QString::number(index + 1)); + QModelIndex idx = mapper->model()->index(index, 0); + if (index == itemSelection->currentIndex().row()) { return; + } itemSelection->clear(); - itemSelection->setCurrentIndex(idx,QItemSelectionModel::Select | QItemSelectionModel::Rows); + itemSelection->setCurrentIndex(idx, QItemSelectionModel::Select | QItemSelectionModel::Rows); } opmap_edit_waypoint_dialog::~opmap_edit_waypoint_dialog() @@ -100,9 +101,9 @@ void opmap_edit_waypoint_dialog::on_pushButtonOK_clicked() void opmap_edit_waypoint_dialog::setupModeWidgets() { - MapDataDelegate::ModeOptions mode=(MapDataDelegate::ModeOptions)ui->cbMode->itemData(ui->cbMode->currentIndex()).toInt(); - switch(mode) - { + MapDataDelegate::ModeOptions mode = (MapDataDelegate::ModeOptions)ui->cbMode->itemData(ui->cbMode->currentIndex()).toInt(); + + switch (mode) { case MapDataDelegate::MODE_FLYENDPOINT: case MapDataDelegate::MODE_FLYVECTOR: case MapDataDelegate::MODE_FLYCIRCLERIGHT: @@ -146,14 +147,14 @@ void opmap_edit_waypoint_dialog::setupModeWidgets() ui->dsb_modeParam2->setVisible(true); ui->dsb_modeParam3->setVisible(false); ui->dsb_modeParam4->setVisible(false); - break; + break; } } void opmap_edit_waypoint_dialog::setupConditionWidgets() { - MapDataDelegate::EndConditionOptions mode=(MapDataDelegate::EndConditionOptions)ui->cbCondition->itemData(ui->cbCondition->currentIndex()).toInt(); - switch(mode) - { + MapDataDelegate::EndConditionOptions mode = (MapDataDelegate::EndConditionOptions)ui->cbCondition->itemData(ui->cbCondition->currentIndex()).toInt(); + + switch (mode) { case MapDataDelegate::ENDCONDITION_NONE: case MapDataDelegate::ENDCONDITION_IMMEDIATE: case MapDataDelegate::ENDCONDITION_PYTHONSCRIPT: @@ -187,7 +188,7 @@ void opmap_edit_waypoint_dialog::setupConditionWidgets() ui->dsb_condParam3->setVisible(false); ui->dsb_condParam4->setVisible(false); ui->condParam1->setText("Distance(m)"); - ui->condParam2->setText("Flag(0=2D,1=3D)");//FIXME + ui->condParam2->setText("Flag(0=2D,1=3D)"); // FIXME break; case MapDataDelegate::ENDCONDITION_LEGREMAINING: ui->condParam1->setVisible(true); @@ -258,13 +259,18 @@ void opmap_edit_waypoint_dialog::pushButtonCancel_clicked() } void opmap_edit_waypoint_dialog::editWaypoint(mapcontrol::WayPointItem *waypoint_item) { - if (!waypoint_item) return; - if(!isVisible()) + if (!waypoint_item) { + return; + } + if (!isVisible()) { show(); - if(isMinimized()) + } + if (isMinimized()) { showNormal(); - if(!isActiveWindow()) + } + if (!isActiveWindow()) { activateWindow(); + } raise(); setFocus(Qt::OtherFocusReason); mapper->setCurrentIndex(waypoint_item->Number()); @@ -282,24 +288,29 @@ void opmap_edit_waypoint_dialog::on_pushButton_2_clicked() void opmap_edit_waypoint_dialog::enableEditWidgets(bool value) { - QWidget * w; - foreach(QWidget * obj,this->findChildren()) - { - w=qobject_cast(obj); - if(w) + QWidget *w; + + foreach(QWidget * obj, this->findChildren()) { + w = qobject_cast(obj); + if (w) { w->setEnabled(!value); - w=qobject_cast(obj); - if(w) + } + w = qobject_cast(obj); + if (w) { w->setEnabled(!value); - w=qobject_cast(obj); - if(w) + } + w = qobject_cast(obj); + if (w) { w->setEnabled(!value); - w=qobject_cast(obj); - if(w && w!=ui->checkBoxLocked) + } + w = qobject_cast(obj); + if (w && w != ui->checkBoxLocked) { w->setEnabled(!value); - w=qobject_cast(obj); - if(w) + } + w = qobject_cast(obj); + if (w) { w->setEnabled(!value); + } } } diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.h b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.h index 435871dbc..46dadab97 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,29 +33,28 @@ #include "opmapcontrol/opmapcontrol.h" #include "flightdatamodel.h" namespace Ui { - class opmap_edit_waypoint_dialog; +class opmap_edit_waypoint_dialog; } using namespace mapcontrol; -class opmap_edit_waypoint_dialog : public QWidget -{ +class opmap_edit_waypoint_dialog : public QWidget { Q_OBJECT public: - opmap_edit_waypoint_dialog(QWidget *parent,QAbstractItemModel * model,QItemSelectionModel * selection); + opmap_edit_waypoint_dialog(QWidget *parent, QAbstractItemModel *model, QItemSelectionModel *selection); ~opmap_edit_waypoint_dialog(); /** - * @brief public functions - * - * @param - */ + * @brief public functions + * + * @param + */ void editWaypoint(mapcontrol::WayPointItem *waypoint_item); private: Ui::opmap_edit_waypoint_dialog *ui; QDataWidgetMapper *mapper; - QAbstractItemModel * model; - QItemSelectionModel * itemSelection; + QAbstractItemModel *model; + QItemSelectionModel *itemSelection; private slots: private slots: @@ -67,7 +66,7 @@ private slots: void on_pushButton_clicked(); void on_pushButton_2_clicked(); void enableEditWidgets(bool); - void currentRowChanged(QModelIndex,QModelIndex); + void currentRowChanged(QModelIndex, QModelIndex); }; #endif // OPMAP_EDIT_WAYPOINT_DIALOG_H diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.cpp index bece61bad..32285454a 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.h b/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.h index afcd3ab23..9992ab7ef 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -31,11 +31,10 @@ #include namespace Ui { - class opmap_statusbar_widget; +class opmap_statusbar_widget; } -class opmap_statusbar_widget : public QWidget -{ +class opmap_statusbar_widget : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.cpp index e3b19afec..83ba9c099 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.h b/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.h index bcf5055ed..077651666 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -31,11 +31,10 @@ #include namespace Ui { - class opmap_zoom_slider_widget; +class opmap_zoom_slider_widget; } -class opmap_zoom_slider_widget : public QWidget -{ +class opmap_zoom_slider_widget : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp index a0a92cdc8..6facd9ef9 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -28,21 +28,20 @@ #include "opmapgadgetwidget.h" OPMapGadget::OPMapGadget(QString classId, OPMapGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget),m_config(NULL) + IUAVGadget(classId, parent), + m_widget(widget), m_config(NULL) { - connect(m_widget,SIGNAL(defaultLocationAndZoomChanged(double,double,double)),this,SLOT(saveDefaultLocation(double,double,double))); - connect(m_widget,SIGNAL(overlayOpacityChanged(qreal)),this,SLOT(saveOpacity(qreal))); + connect(m_widget, SIGNAL(defaultLocationAndZoomChanged(double, double, double)), this, SLOT(saveDefaultLocation(double, double, double))); + connect(m_widget, SIGNAL(overlayOpacityChanged(qreal)), this, SLOT(saveOpacity(qreal))); } OPMapGadget::~OPMapGadget() { delete m_widget; } -void OPMapGadget::saveDefaultLocation(double lng,double lat,double zoom) +void OPMapGadget::saveDefaultLocation(double lng, double lat, double zoom) { - if(m_config) - { + if (m_config) { m_config->setLatitude(lat); m_config->setLongitude(lng); m_config->setZoom(zoom); @@ -52,14 +51,13 @@ void OPMapGadget::saveDefaultLocation(double lng,double lat,double zoom) void OPMapGadget::saveOpacity(qreal value) { - if(m_config) - { + if (m_config) { m_config->setOpacity(value); } } void OPMapGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - m_config = qobject_cast(config); + m_config = qobject_cast(config); m_widget->setMapProvider(m_config->mapProvider()); m_widget->setUseOpenGL(m_config->useOpenGL()); m_widget->setShowTileGridLines(m_config->showTileGridLines()); @@ -72,4 +70,3 @@ void OPMapGadget::loadConfiguration(IUAVGadgetConfiguration *config) m_widget->setHomePosition(QPointF(m_config->longitude(), m_config->latitude())); m_widget->setOverlayOpacity(m_config->opacity()); } - diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadget.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadget.h index 250994e9b..bbc4f4ae9 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -39,15 +39,17 @@ class OPMapGadgetWidget; using namespace Core; -class OPMapGadget : public Core::IUAVGadget -{ +class OPMapGadget : public Core::IUAVGadget { Q_OBJECT public: OPMapGadget(QString classId, OPMapGadgetWidget *widget, QWidget *parent = 0); ~OPMapGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* m_config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *m_config); private: OPMapGadgetWidget *m_widget; OPMapGadgetConfiguration *m_config; diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp index 325cb1721..922d41e16 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -29,7 +29,7 @@ #include "utils/pathutils.h" #include -OPMapGadgetConfiguration::OPMapGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +OPMapGadgetConfiguration::OPMapGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_mapProvider("GoogleHybrid"), m_defaultZoom(2), @@ -40,98 +40,105 @@ OPMapGadgetConfiguration::OPMapGadgetConfiguration(QString classId, QSettings* m_accessMode("ServerAndCache"), m_useMemoryCache(true), m_cacheLocation(Utils::PathUtils().GetStoragePath() + "mapscache" + QDir::separator()), - m_uavSymbol(QString::fromUtf8(":/uavs/images/mapquad.png")), - m_maxUpdateRate(2000), // ms + m_uavSymbol(QString::fromUtf8(":/uavs/images/mapquad.png")), + m_maxUpdateRate(2000), // ms m_settings(qSettings), m_opacity(1) { - - //if a saved configuration exists load it - if (qSettings != 0) { - - QString mapProvider = qSettings->value("mapProvider").toString(); + // if a saved configuration exists load it + if (qSettings != 0) { + QString mapProvider = qSettings->value("mapProvider").toString(); int zoom = qSettings->value("defaultZoom").toInt(); - double latitude= qSettings->value("defaultLatitude").toDouble(); - double longitude= qSettings->value("defaultLongitude").toDouble(); - bool useOpenGL= qSettings->value("useOpenGL").toBool(); - bool showTileGridLines= qSettings->value("showTileGridLines").toBool(); - QString accessMode= qSettings->value("accessMode").toString(); - bool useMemoryCache= qSettings->value("useMemoryCache").toBool(); - QString cacheLocation= qSettings->value("cacheLocation").toString(); - QString uavSymbol=qSettings->value("uavSymbol").toString(); - int max_update_rate = qSettings->value("maxUpdateRate").toInt(); + double latitude = qSettings->value("defaultLatitude").toDouble(); + double longitude = qSettings->value("defaultLongitude").toDouble(); + bool useOpenGL = qSettings->value("useOpenGL").toBool(); + bool showTileGridLines = qSettings->value("showTileGridLines").toBool(); + QString accessMode = qSettings->value("accessMode").toString(); + bool useMemoryCache = qSettings->value("useMemoryCache").toBool(); + QString cacheLocation = qSettings->value("cacheLocation").toString(); + QString uavSymbol = qSettings->value("uavSymbol").toString(); + int max_update_rate = qSettings->value("maxUpdateRate").toInt(); - m_opacity=qSettings->value("overlayOpacity",1).toReal(); + m_opacity = qSettings->value("overlayOpacity", 1).toReal(); - if (!mapProvider.isEmpty()) m_mapProvider = mapProvider; - m_defaultZoom = zoom; - m_defaultLatitude = latitude; - m_defaultLongitude = longitude; + if (!mapProvider.isEmpty()) { + m_mapProvider = mapProvider; + } + m_defaultZoom = zoom; + m_defaultLatitude = latitude; + m_defaultLongitude = longitude; m_useOpenGL = useOpenGL; m_showTileGridLines = showTileGridLines; - m_uavSymbol = uavSymbol; + m_uavSymbol = uavSymbol; - m_maxUpdateRate = max_update_rate; - if (m_maxUpdateRate < 100 || m_maxUpdateRate > 5000) - m_maxUpdateRate = 2000; + m_maxUpdateRate = max_update_rate; + if (m_maxUpdateRate < 100 || m_maxUpdateRate > 5000) { + m_maxUpdateRate = 2000; + } - if (!accessMode.isEmpty()) - m_accessMode = accessMode; + if (!accessMode.isEmpty()) { + m_accessMode = accessMode; + } m_useMemoryCache = useMemoryCache; - if (!cacheLocation.isEmpty()) - m_cacheLocation = Utils::PathUtils().InsertStoragePath(cacheLocation); + if (!cacheLocation.isEmpty()) { + m_cacheLocation = Utils::PathUtils().InsertStoragePath(cacheLocation); + } } } -IUAVGadgetConfiguration * OPMapGadgetConfiguration::clone() +IUAVGadgetConfiguration *OPMapGadgetConfiguration::clone() { OPMapGadgetConfiguration *m = new OPMapGadgetConfiguration(this->classId()); - m->m_mapProvider = m_mapProvider; - m->m_defaultZoom = m_defaultZoom; - m->m_defaultLatitude = m_defaultLatitude; - m->m_defaultLongitude = m_defaultLongitude; + m->m_mapProvider = m_mapProvider; + m->m_defaultZoom = m_defaultZoom; + m->m_defaultLatitude = m_defaultLatitude; + m->m_defaultLongitude = m_defaultLongitude; m->m_useOpenGL = m_useOpenGL; m->m_showTileGridLines = m_showTileGridLines; m->m_accessMode = m_accessMode; - m->m_useMemoryCache = m_useMemoryCache; - m->m_cacheLocation = m_cacheLocation; - m->m_uavSymbol = m_uavSymbol; - m->m_maxUpdateRate = m_maxUpdateRate; - m->m_opacity=m_opacity; + m->m_useMemoryCache = m_useMemoryCache; + m->m_cacheLocation = m_cacheLocation; + m->m_uavSymbol = m_uavSymbol; + m->m_maxUpdateRate = m_maxUpdateRate; + m->m_opacity = m_opacity; return m; } -void OPMapGadgetConfiguration::saveConfig() const { - if(!m_settings) +void OPMapGadgetConfiguration::saveConfig() const +{ + if (!m_settings) { return; - m_settings->setValue("mapProvider", m_mapProvider); - m_settings->setValue("defaultZoom", m_defaultZoom); - m_settings->setValue("defaultLatitude", m_defaultLatitude); - m_settings->setValue("defaultLongitude", m_defaultLongitude); - m_settings->setValue("useOpenGL", m_useOpenGL); - m_settings->setValue("showTileGridLines", m_showTileGridLines); - m_settings->setValue("accessMode", m_accessMode); - m_settings->setValue("useMemoryCache", m_useMemoryCache); - m_settings->setValue("uavSymbol", m_uavSymbol); - m_settings->setValue("cacheLocation", Utils::PathUtils().RemoveStoragePath(m_cacheLocation)); - m_settings->setValue("maxUpdateRate", m_maxUpdateRate); - m_settings->setValue("overlayOpacity",m_opacity); + } + m_settings->setValue("mapProvider", m_mapProvider); + m_settings->setValue("defaultZoom", m_defaultZoom); + m_settings->setValue("defaultLatitude", m_defaultLatitude); + m_settings->setValue("defaultLongitude", m_defaultLongitude); + m_settings->setValue("useOpenGL", m_useOpenGL); + m_settings->setValue("showTileGridLines", m_showTileGridLines); + m_settings->setValue("accessMode", m_accessMode); + m_settings->setValue("useMemoryCache", m_useMemoryCache); + m_settings->setValue("uavSymbol", m_uavSymbol); + m_settings->setValue("cacheLocation", Utils::PathUtils().RemoveStoragePath(m_cacheLocation)); + m_settings->setValue("maxUpdateRate", m_maxUpdateRate); + m_settings->setValue("overlayOpacity", m_opacity); } -void OPMapGadgetConfiguration::saveConfig(QSettings* qSettings) const { - qSettings->setValue("mapProvider", m_mapProvider); - qSettings->setValue("defaultZoom", m_defaultZoom); - qSettings->setValue("defaultLatitude", m_defaultLatitude); - qSettings->setValue("defaultLongitude", m_defaultLongitude); - qSettings->setValue("useOpenGL", m_useOpenGL); - qSettings->setValue("showTileGridLines", m_showTileGridLines); - qSettings->setValue("accessMode", m_accessMode); - qSettings->setValue("useMemoryCache", m_useMemoryCache); - qSettings->setValue("uavSymbol", m_uavSymbol); - qSettings->setValue("cacheLocation", Utils::PathUtils().RemoveStoragePath(m_cacheLocation)); - qSettings->setValue("maxUpdateRate", m_maxUpdateRate); - qSettings->setValue("overlayOpacity",m_opacity); +void OPMapGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ + qSettings->setValue("mapProvider", m_mapProvider); + qSettings->setValue("defaultZoom", m_defaultZoom); + qSettings->setValue("defaultLatitude", m_defaultLatitude); + qSettings->setValue("defaultLongitude", m_defaultLongitude); + qSettings->setValue("useOpenGL", m_useOpenGL); + qSettings->setValue("showTileGridLines", m_showTileGridLines); + qSettings->setValue("accessMode", m_accessMode); + qSettings->setValue("useMemoryCache", m_useMemoryCache); + qSettings->setValue("uavSymbol", m_uavSymbol); + qSettings->setValue("cacheLocation", Utils::PathUtils().RemoveStoragePath(m_cacheLocation)); + qSettings->setValue("maxUpdateRate", m_maxUpdateRate); + qSettings->setValue("overlayOpacity", m_opacity); } -void OPMapGadgetConfiguration::setCacheLocation(QString cacheLocation){ +void OPMapGadgetConfiguration::setCacheLocation(QString cacheLocation) +{ m_cacheLocation = cacheLocation; } diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.h index 22b3e749e..19462b894 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,55 +33,121 @@ using namespace Core; -class OPMapGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT - -Q_PROPERTY(QString mapProvider READ mapProvider WRITE setMapProvider) -Q_PROPERTY(int zoommo READ zoom WRITE setZoom) -Q_PROPERTY(double latitude READ latitude WRITE setLatitude) -Q_PROPERTY(double longitude READ longitude WRITE setLongitude) -Q_PROPERTY(bool useOpenGL READ useOpenGL WRITE setUseOpenGL) -Q_PROPERTY(bool showTileGridLines READ showTileGridLines WRITE setShowTileGridLines) -Q_PROPERTY(QString accessMode READ accessMode WRITE setAccessMode) -Q_PROPERTY(bool useMemoryCache READ useMemoryCache WRITE setUseMemoryCache) -Q_PROPERTY(QString cacheLocation READ cacheLocation WRITE setCacheLocation) -Q_PROPERTY(QString uavSymbol READ uavSymbol WRITE setUavSymbol) -Q_PROPERTY(int maxUpdateRate READ maxUpdateRate WRITE setMaxUpdateRate) -Q_PROPERTY(qreal overlayOpacity READ opacity WRITE setOpacity) +class OPMapGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT Q_PROPERTY(QString mapProvider READ mapProvider WRITE setMapProvider) + Q_PROPERTY(int zoommo READ zoom WRITE setZoom) + Q_PROPERTY(double latitude READ latitude WRITE setLatitude) + Q_PROPERTY(double longitude READ longitude WRITE setLongitude) + Q_PROPERTY(bool useOpenGL READ useOpenGL WRITE setUseOpenGL) + Q_PROPERTY(bool showTileGridLines READ showTileGridLines WRITE setShowTileGridLines) + Q_PROPERTY(QString accessMode READ accessMode WRITE setAccessMode) + Q_PROPERTY(bool useMemoryCache READ useMemoryCache WRITE setUseMemoryCache) + Q_PROPERTY(QString cacheLocation READ cacheLocation WRITE setCacheLocation) + Q_PROPERTY(QString uavSymbol READ uavSymbol WRITE setUavSymbol) + Q_PROPERTY(int maxUpdateRate READ maxUpdateRate WRITE setMaxUpdateRate) + Q_PROPERTY(qreal overlayOpacity READ opacity WRITE setOpacity) public: - explicit OPMapGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit OPMapGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); - QString mapProvider() const { return m_mapProvider; } - int zoom() const { return m_defaultZoom; } - double latitude() const { return m_defaultLatitude; } - double longitude() const { return m_defaultLongitude; } - bool useOpenGL() const { return m_useOpenGL; } - bool showTileGridLines() const { return m_showTileGridLines; } - QString accessMode() const { return m_accessMode; } - bool useMemoryCache() const { return m_useMemoryCache; } - QString cacheLocation() const { return m_cacheLocation; } - QString uavSymbol() const { return m_uavSymbol; } - int maxUpdateRate() const { return m_maxUpdateRate; } - qreal opacity() const { return m_opacity; } + QString mapProvider() const + { + return m_mapProvider; + } + int zoom() const + { + return m_defaultZoom; + } + double latitude() const + { + return m_defaultLatitude; + } + double longitude() const + { + return m_defaultLongitude; + } + bool useOpenGL() const + { + return m_useOpenGL; + } + bool showTileGridLines() const + { + return m_showTileGridLines; + } + QString accessMode() const + { + return m_accessMode; + } + bool useMemoryCache() const + { + return m_useMemoryCache; + } + QString cacheLocation() const + { + return m_cacheLocation; + } + QString uavSymbol() const + { + return m_uavSymbol; + } + int maxUpdateRate() const + { + return m_maxUpdateRate; + } + qreal opacity() const + { + return m_opacity; + } void saveConfig() const; public slots: - void setMapProvider(QString provider) { m_mapProvider = provider; } - void setZoom(int zoom) { m_defaultZoom = zoom; } - void setLatitude(double latitude) { m_defaultLatitude = latitude; } - void setOpacity(qreal value) { m_opacity = value; } - void setLongitude(double longitude) { m_defaultLongitude = longitude; } - void setUseOpenGL(bool useOpenGL) { m_useOpenGL = useOpenGL; } - void setShowTileGridLines(bool showTileGridLines) { m_showTileGridLines = showTileGridLines; } - void setAccessMode(QString accessMode) { m_accessMode = accessMode; } - void setUseMemoryCache(bool useMemoryCache) { m_useMemoryCache = useMemoryCache; } + void setMapProvider(QString provider) + { + m_mapProvider = provider; + } + void setZoom(int zoom) + { + m_defaultZoom = zoom; + } + void setLatitude(double latitude) + { + m_defaultLatitude = latitude; + } + void setOpacity(qreal value) + { + m_opacity = value; + } + void setLongitude(double longitude) + { + m_defaultLongitude = longitude; + } + void setUseOpenGL(bool useOpenGL) + { + m_useOpenGL = useOpenGL; + } + void setShowTileGridLines(bool showTileGridLines) + { + m_showTileGridLines = showTileGridLines; + } + void setAccessMode(QString accessMode) + { + m_accessMode = accessMode; + } + void setUseMemoryCache(bool useMemoryCache) + { + m_useMemoryCache = useMemoryCache; + } void setCacheLocation(QString cacheLocation); - void setUavSymbol(QString symbol){m_uavSymbol=symbol;} - void setMaxUpdateRate(int update_rate){m_maxUpdateRate = update_rate;} + void setUavSymbol(QString symbol) + { + m_uavSymbol = symbol; + } + void setMaxUpdateRate(int update_rate) + { + m_maxUpdateRate = update_rate; + } private: QString m_mapProvider; @@ -94,8 +160,8 @@ private: bool m_useMemoryCache; QString m_cacheLocation; QString m_uavSymbol; - int m_maxUpdateRate; - QSettings * m_settings; + int m_maxUpdateRate; + QSettings *m_settings; qreal m_opacity; }; diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.cpp index fb4b73efa..111cef2cd 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,27 +32,25 @@ #include OPMapGadgetFactory::OPMapGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("OPMapGadget"), tr("OPMap"), parent) -{ -} + IUAVGadgetFactory(QString("OPMapGadget"), tr("OPMap"), parent) +{} OPMapGadgetFactory::~OPMapGadgetFactory() -{ -} +{} -Core::IUAVGadget * OPMapGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *OPMapGadgetFactory::createGadget(QWidget *parent) { OPMapGadgetWidget *gadgetWidget = new OPMapGadgetWidget(parent); + return new OPMapGadget(QString("OPMapGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *OPMapGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *OPMapGadgetFactory::createConfiguration(QSettings *qSettings) { return new OPMapGadgetConfiguration(QString("OPMapGadget"), qSettings); } -IOptionsPage * OPMapGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) +IOptionsPage *OPMapGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new OPMapGadgetOptionsPage(qobject_cast(config)); + return new OPMapGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.h index 138efcb90..908bd1f4d 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -31,14 +31,13 @@ #include namespace Core { - class IUAVGadget; - class IUAVGadgetFactory; +class IUAVGadget; +class IUAVGadgetFactory; } using namespace Core; -class OPMapGadgetFactory : public Core::IUAVGadgetFactory -{ +class OPMapGadgetFactory : public Core::IUAVGadgetFactory { Q_OBJECT public: @@ -46,8 +45,8 @@ public: ~OPMapGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; -#endif +#endif // ifndef OPAMP_GADGETFACTORY_H_ diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.cpp index acf343a46..91a71e6ea 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -44,14 +44,13 @@ OPMapGadgetOptionsPage::OPMapGadgetOptionsPage(OPMapGadgetConfiguration *config, QObject *parent) : IOptionsPage(parent), m_config(config) -{ -} +{} QWidget *OPMapGadgetOptionsPage::createPage(QWidget *parent) { - int index; + int index; - m_page = new Ui::OPMapGadgetOptionsPage(); + m_page = new Ui::OPMapGadgetOptionsPage(); QWidget *w = new QWidget(parent); m_page->setupUi(w); @@ -63,22 +62,22 @@ QWidget *OPMapGadgetOptionsPage::createPage(QWidget *parent) m_page->accessModeComboBox->clear(); m_page->accessModeComboBox->addItems(mapcontrol::Helper::AccessModeTypes()); - index = m_page->providerComboBox->findText(m_config->mapProvider()); - index = (index >= 0) ? index : 0; - m_page->providerComboBox->setCurrentIndex(index); + index = m_page->providerComboBox->findText(m_config->mapProvider()); + index = (index >= 0) ? index : 0; + m_page->providerComboBox->setCurrentIndex(index); - // populate the map max update rate combobox - m_page->maxUpdateRateComboBox->clear(); - m_page->maxUpdateRateComboBox->addItem("100ms", 100); - m_page->maxUpdateRateComboBox->addItem("200ms", 200); - m_page->maxUpdateRateComboBox->addItem("500ms", 500); - m_page->maxUpdateRateComboBox->addItem("1 sec", 1000); - m_page->maxUpdateRateComboBox->addItem("2 sec", 2000); - m_page->maxUpdateRateComboBox->addItem("5 sec", 5000); + // populate the map max update rate combobox + m_page->maxUpdateRateComboBox->clear(); + m_page->maxUpdateRateComboBox->addItem("100ms", 100); + m_page->maxUpdateRateComboBox->addItem("200ms", 200); + m_page->maxUpdateRateComboBox->addItem("500ms", 500); + m_page->maxUpdateRateComboBox->addItem("1 sec", 1000); + m_page->maxUpdateRateComboBox->addItem("2 sec", 2000); + m_page->maxUpdateRateComboBox->addItem("5 sec", 5000); - index = m_page->maxUpdateRateComboBox->findData(m_config->maxUpdateRate()); - index = (index >= 0) ? index : 4; - m_page->maxUpdateRateComboBox->setCurrentIndex(index); + index = m_page->maxUpdateRateComboBox->findData(m_config->maxUpdateRate()); + index = (index >= 0) ? index : 4; + m_page->maxUpdateRateComboBox->setCurrentIndex(index); m_page->zoomSpinBox->setValue(m_config->zoom()); m_page->latitudeSpinBox->setValue(m_config->latitude()); @@ -98,16 +97,14 @@ QWidget *OPMapGadgetOptionsPage::createPage(QWidget *parent) m_page->lineEditCacheLocation->setPath(m_config->cacheLocation()); QDir dir(":/uavs/images/"); - QStringList list=dir.entryList(); - foreach(QString i,list) - { - QIcon icon(QPixmap(":/uavs/images/"+i)); - m_page->uavSymbolComboBox->addItem(icon,QString(),i); + QStringList list = dir.entryList(); + foreach(QString i, list) { + QIcon icon(QPixmap(":/uavs/images/" + i)); + + m_page->uavSymbolComboBox->addItem(icon, QString(), i); } - for(int x=0;xuavSymbolComboBox->count();++x) - { - if(m_page->uavSymbolComboBox->itemData(x).toString()==m_config->uavSymbol()) - { + for (int x = 0; x < m_page->uavSymbolComboBox->count(); ++x) { + if (m_page->uavSymbolComboBox->itemData(x).toString() == m_config->uavSymbol()) { m_page->uavSymbolComboBox->setCurrentIndex(x); } } @@ -120,12 +117,12 @@ QWidget *OPMapGadgetOptionsPage::createPage(QWidget *parent) void OPMapGadgetOptionsPage::on_pushButtonCacheDefaults_clicked() { int index = m_page->accessModeComboBox->findText("ServerAndCache"); + index = (index >= 0) ? index : 0; m_page->accessModeComboBox->setCurrentIndex(index); m_page->checkBoxUseMemoryCache->setChecked(true); m_page->lineEditCacheLocation->setPath(Utils::PathUtils().GetStoragePath() + "mapscache" + QDir::separator()); - } void OPMapGadgetOptionsPage::apply() @@ -140,7 +137,7 @@ void OPMapGadgetOptionsPage::apply() m_config->setUseMemoryCache(m_page->checkBoxUseMemoryCache->isChecked()); m_config->setCacheLocation(m_page->lineEditCacheLocation->path()); m_config->setUavSymbol(m_page->uavSymbolComboBox->itemData(m_page->uavSymbolComboBox->currentIndex()).toString()); - m_config->setMaxUpdateRate(m_page->maxUpdateRateComboBox->itemData(m_page->maxUpdateRateComboBox->currentIndex()).toInt()); + m_config->setMaxUpdateRate(m_page->maxUpdateRateComboBox->itemData(m_page->maxUpdateRateComboBox->currentIndex()).toInt()); } void OPMapGadgetOptionsPage::finish() diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.h index 1f421cd03..2ad5c551b 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,18 +33,17 @@ class OPMapGadgetConfiguration; namespace Core { - class IUAVGadgetConfiguration; +class IUAVGadgetConfiguration; } namespace Ui { - class OPMapGadgetOptionsPage; +class OPMapGadgetOptionsPage; } using namespace Core; -class OPMapGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class OPMapGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit OPMapGadgetOptionsPage(OPMapGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 223343ec4..c7d193f24 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -61,26 +61,26 @@ // ************************************************************************************* -#define deg_to_rad ((double)M_PI / 180.0) -#define rad_to_deg (180.0 / (double)M_PI) +#define deg_to_rad ((double)M_PI / 180.0) +#define rad_to_deg (180.0 / (double)M_PI) -#define earth_mean_radius 6371 // kilometers +#define earth_mean_radius 6371 // kilometers -#define max_digital_zoom 3 // maximum allowed digital zoom level +#define max_digital_zoom 3 // maximum allowed digital zoom level -const int safe_area_radius_list[] = {5, 10, 20, 50, 100, 200, 500, 1000, 2000, 5000}; // meters +const int safe_area_radius_list[] = { 5, 10, 20, 50, 100, 200, 500, 1000, 2000, 5000 }; // meters -const int uav_trail_time_list[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; // seconds +const int uav_trail_time_list[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 }; // seconds -const int uav_trail_distance_list[] = {1, 2, 5, 10, 20, 50, 100, 200, 500}; // meters +const int uav_trail_distance_list[] = { 1, 2, 5, 10, 20, 50, 100, 200, 500 }; // meters -const int max_update_rate_list[] = {100, 200, 500, 1000, 2000, 5000}; // milliseconds +const int max_update_rate_list[] = { 100, 200, 500, 1000, 2000, 5000 }; // milliseconds // ************************************************************************************* // ************************************************************************************* -// NOTE: go back to SVN REV 2137 and earlier to get back to experimental waypoint support. +// NOTE: go back to SVN REV 2137 and earlier to get back to experimental waypoint support. // ************************************************************************************* @@ -90,54 +90,53 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) // ************** m_widget = NULL; - m_map = NULL; + m_map = NULL; findPlaceCompleter = NULL; - m_mouse_waypoint = NULL; + m_mouse_waypoint = NULL; - pm = NULL; - obm = NULL; - obum = NULL; + pm = NULL; + obm = NULL; + obum = NULL; - m_prev_tile_number = 0; + m_prev_tile_number = 0; - m_min_zoom = m_max_zoom = 0; + m_min_zoom = m_max_zoom = 0; m_map_mode = Normal_MapMode; - m_maxUpdateRate = max_update_rate_list[4]; // 2 seconds //SHOULDN'T THIS BE LOADED FROM THE USER PREFERENCES? + m_maxUpdateRate = max_update_rate_list[4]; // 2 seconds //SHOULDN'T THIS BE LOADED FROM THE USER PREFERENCES? - m_telemetry_connected = false; + m_telemetry_connected = false; - m_context_menu_lat_lon = m_mouse_lat_lon = internals::PointLatLng(0, 0); + m_context_menu_lat_lon = m_mouse_lat_lon = internals::PointLatLng(0, 0); setMouseTracking(true); - pm = ExtensionSystem::PluginManager::instance(); - if (pm) - { - obm = pm->getObject(); - obum = pm->getObject(); - } + pm = ExtensionSystem::PluginManager::instance(); + if (pm) { + obm = pm->getObject(); + obum = pm->getObject(); + } - // ************** + // ************** // get current location - double latitude = 0; + double latitude = 0; double longitude = 0; - double altitude = 0; + double altitude = 0; - // current position - getUAVPosition(latitude, longitude, altitude); + // current position + getUAVPosition(latitude, longitude, altitude); internals::PointLatLng pos_lat_lon = internals::PointLatLng(latitude, longitude); // ************** // default home position - m_home_position.coord = pos_lat_lon; - m_home_position.altitude = altitude; - m_home_position.locked = false; + m_home_position.coord = pos_lat_lon; + m_home_position.altitude = altitude; + m_home_position.locked = false; // ************** // create the widget that holds the user controls and the map @@ -148,39 +147,39 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) // ************** // create the central map widget - m_map = new mapcontrol::OPMapWidget(); // create the map object + m_map = new mapcontrol::OPMapWidget(); // create the map object - m_map->setFrameStyle(QFrame::NoFrame); // no border frame + m_map->setFrameStyle(QFrame::NoFrame); // no border frame m_map->setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); // tile background - m_map->configuration->DragButton = Qt::LeftButton; // use the left mouse button for map dragging + m_map->configuration->DragButton = Qt::LeftButton; // use the left mouse button for map dragging - m_widget->horizontalSliderZoom->setMinimum(m_map->MinZoom()); // - m_widget->horizontalSliderZoom->setMaximum(m_map->MaxZoom() + max_digital_zoom); // + m_widget->horizontalSliderZoom->setMinimum(m_map->MinZoom()); // + m_widget->horizontalSliderZoom->setMaximum(m_map->MaxZoom() + max_digital_zoom); // - m_min_zoom = m_widget->horizontalSliderZoom->minimum(); // minimum zoom we can accept - m_max_zoom = m_widget->horizontalSliderZoom->maximum(); // maximum zoom we can accept + m_min_zoom = m_widget->horizontalSliderZoom->minimum(); // minimum zoom we can accept + m_max_zoom = m_widget->horizontalSliderZoom->maximum(); // maximum zoom we can accept - m_map->SetMouseWheelZoomType(internals::MouseWheelZoomType::MousePositionWithoutCenter); // set how the mouse wheel zoom functions - m_map->SetFollowMouse(true); // we want a contiuous mouse position reading + m_map->SetMouseWheelZoomType(internals::MouseWheelZoomType::MousePositionWithoutCenter); // set how the mouse wheel zoom functions + m_map->SetFollowMouse(true); // we want a contiuous mouse position reading - m_map->SetShowHome(true); // display the HOME position on the map - m_map->SetShowUAV(true); // display the UAV position on the map + m_map->SetShowHome(true); // display the HOME position on the map + m_map->SetShowUAV(true); // display the UAV position on the map - m_map->Home->SetSafeArea(safe_area_radius_list[0]); // set radius (meters) //SHOULDN'T THE DEFAULT BE USER DEFINED? - m_map->Home->SetShowSafeArea(true); // show the safe area //SHOULDN'T THE DEFAULT BE USER DEFINED? + m_map->Home->SetSafeArea(safe_area_radius_list[0]); // set radius (meters) //SHOULDN'T THE DEFAULT BE USER DEFINED? + m_map->Home->SetShowSafeArea(true); // show the safe area //SHOULDN'T THE DEFAULT BE USER DEFINED? m_map->Home->SetToggleRefresh(true); - if(m_map->Home) - connect(m_map->Home,SIGNAL(homedoubleclick(HomeItem*)),this,SLOT(onHomeDoubleClick(HomeItem*))); - m_map->UAV->SetTrailTime(uav_trail_time_list[0]); // seconds - m_map->UAV->SetTrailDistance(uav_trail_distance_list[1]); // meters + if (m_map->Home) { + connect(m_map->Home, SIGNAL(homedoubleclick(HomeItem *)), this, SLOT(onHomeDoubleClick(HomeItem *))); + } + m_map->UAV->SetTrailTime(uav_trail_time_list[0]); // seconds + m_map->UAV->SetTrailDistance(uav_trail_distance_list[1]); // meters m_map->UAV->SetTrailType(UAVTrailType::ByTimeElapsed); - if(m_map->GPS) - { - m_map->GPS->SetTrailTime(uav_trail_time_list[0]); // seconds - m_map->GPS->SetTrailDistance(uav_trail_distance_list[1]); // meters + if (m_map->GPS) { + m_map->GPS->SetTrailTime(uav_trail_time_list[0]); // seconds + m_map->GPS->SetTrailDistance(uav_trail_distance_list[1]); // meters m_map->GPS->SetTrailType(UAVTrailType::ByTimeElapsed); } @@ -194,9 +193,9 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) layout->addWidget(m_map); m_widget->mapWidget->setLayout(layout); - m_widget->toolButtonMagicWaypointMapMode->setChecked(false); - m_widget->toolButtonNormalMapMode->setChecked(true); - hideMagicWaypointControls(); + m_widget->toolButtonMagicWaypointMapMode->setChecked(false); + m_widget->toolButtonNormalMapMode->setChecked(true); + hideMagicWaypointControls(); m_widget->labelUAVPos->setText("---"); m_widget->labelMapPos->setText("---"); @@ -205,30 +204,31 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) m_widget->progressBarMap->setMaximum(1); - connect(m_map, SIGNAL(zoomChanged(double, double, double)), this, SLOT(zoomChanged(double, double, double))); // map zoom change signals - connect(m_map, SIGNAL(OnCurrentPositionChanged(internals::PointLatLng)), this, SLOT(OnCurrentPositionChanged(internals::PointLatLng))); // map poisition change signals - connect(m_map, SIGNAL(OnTileLoadComplete()), this, SLOT(OnTileLoadComplete())); // tile loading stop signals - connect(m_map, SIGNAL(OnTileLoadStart()), this, SLOT(OnTileLoadStart())); // tile loading start signals - connect(m_map, SIGNAL(OnTilesStillToLoad(int)), this, SLOT(OnTilesStillToLoad(int))); // tile loading signals - connect(m_map,SIGNAL(OnWayPointDoubleClicked(WayPointItem*)),this,SLOT(wpDoubleClickEvent(WayPointItem*))); - m_map->SetCurrentPosition(m_home_position.coord); // set the map position - m_map->Home->SetCoord(m_home_position.coord); // set the HOME position - m_map->UAV->SetUAVPos(m_home_position.coord, 0.0); // set the UAV position + connect(m_map, SIGNAL(zoomChanged(double, double, double)), this, SLOT(zoomChanged(double, double, double))); // map zoom change signals + connect(m_map, SIGNAL(OnCurrentPositionChanged(internals::PointLatLng)), this, SLOT(OnCurrentPositionChanged(internals::PointLatLng))); // map poisition change signals + connect(m_map, SIGNAL(OnTileLoadComplete()), this, SLOT(OnTileLoadComplete())); // tile loading stop signals + connect(m_map, SIGNAL(OnTileLoadStart()), this, SLOT(OnTileLoadStart())); // tile loading start signals + connect(m_map, SIGNAL(OnTilesStillToLoad(int)), this, SLOT(OnTilesStillToLoad(int))); // tile loading signals + connect(m_map, SIGNAL(OnWayPointDoubleClicked(WayPointItem *)), this, SLOT(wpDoubleClickEvent(WayPointItem *))); + m_map->SetCurrentPosition(m_home_position.coord); // set the map position + m_map->Home->SetCoord(m_home_position.coord); // set the HOME position + m_map->UAV->SetUAVPos(m_home_position.coord, 0.0); // set the UAV position m_map->UAV->update(); - if(m_map->GPS) - m_map->GPS->SetUAVPos(m_home_position.coord, 0.0); // set the GPS position + if (m_map->GPS) { + m_map->GPS->SetUAVPos(m_home_position.coord, 0.0); // set the GPS position + } #ifdef USE_PATHPLANNER - model=new flightDataModel(this); - table=new pathPlanner(); - selectionModel=new QItemSelectionModel(model); - mapProxy=new modelMapProxy(this,m_map,model,selectionModel); - table->setModel(model,selectionModel); - waypoint_edit_dialog=new opmap_edit_waypoint_dialog(this,model,selectionModel); - UAVProxy=new modelUavoProxy(this,model); - connect(table,SIGNAL(sendPathPlanToUAV()),UAVProxy,SLOT(modelToObjects())); - connect(table,SIGNAL(receivePathPlanFromUAV()),UAVProxy,SLOT(objectsToModel())); + model = new flightDataModel(this); + table = new pathPlanner(); + selectionModel = new QItemSelectionModel(model); + mapProxy = new modelMapProxy(this, m_map, model, selectionModel); + table->setModel(model, selectionModel); + waypoint_edit_dialog = new opmap_edit_waypoint_dialog(this, model, selectionModel); + UAVProxy = new modelUavoProxy(this, model); + connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(modelToObjects())); + connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(objectsToModel())); #endif - magicWayPoint=m_map->magicWPCreate(); + magicWayPoint = m_map->magicWPCreate(); magicWayPoint->setVisible(false); m_map->setOverlayOpacity(0.5); @@ -241,22 +241,18 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) // ************** // connect to the UAVObject updates we require to become a bit aware of our environment: - if (pm) - { + if (pm) { // Register for Home Location state changes - if (obm) - { - UAVDataObject *obj = dynamic_cast(obm->getObject(QString("HomeLocation"))); - if (obj) - { - connect(obj, SIGNAL(objectUpdated(UAVObject *)), this , SLOT(homePositionUpdated(UAVObject *))); + if (obm) { + UAVDataObject *obj = dynamic_cast(obm->getObject(QString("HomeLocation"))); + if (obj) { + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(homePositionUpdated(UAVObject *))); } } // Listen to telemetry connection events TelemetryManager *telMngr = pm->getObject(); - if (telMngr) - { + if (telMngr) { connect(telMngr, SIGNAL(connected()), this, SLOT(onTelemetryConnect())); connect(telMngr, SIGNAL(disconnected()), this, SLOT(onTelemetryDisconnect())); } @@ -266,13 +262,13 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) // create the desired timers m_updateTimer = new QTimer(); - m_updateTimer->setInterval(m_maxUpdateRate); + m_updateTimer->setInterval(m_maxUpdateRate); connect(m_updateTimer, SIGNAL(timeout()), this, SLOT(updatePosition())); m_updateTimer->start(); m_statusUpdateTimer = new QTimer(); - m_statusUpdateTimer->setInterval(200); - connect(m_statusUpdateTimer, SIGNAL(timeout()), this, SLOT(updateMousePos())); + m_statusUpdateTimer->setInterval(200); + connect(m_statusUpdateTimer, SIGNAL(timeout()), this, SLOT(updateMousePos())); m_statusUpdateTimer->start(); // ************** @@ -282,30 +278,34 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) // destructor OPMapGadgetWidget::~OPMapGadgetWidget() { - if (m_map) - { - disconnect(m_map, 0, 0, 0); - m_map->SetShowHome(false); // doing this appears to stop the map lib crashing on exit - m_map->SetShowUAV(false); // " " - } + if (m_map) { + disconnect(m_map, 0, 0, 0); + m_map->SetShowHome(false); // doing this appears to stop the map lib crashing on exit + m_map->SetShowUAV(false); // " " + } - if (m_map) - { - delete m_map; - m_map = NULL; - } - if(!model.isNull()) + if (m_map) { + delete m_map; + m_map = NULL; + } + if (!model.isNull()) { delete model; - if(!table.isNull()) + } + if (!table.isNull()) { delete table; - if(!selectionModel.isNull()) + } + if (!selectionModel.isNull()) { delete selectionModel; - if(!mapProxy.isNull()) + } + if (!mapProxy.isNull()) { delete mapProxy; - if(!waypoint_edit_dialog.isNull()) + } + if (!waypoint_edit_dialog.isNull()) { delete waypoint_edit_dialog; - if(!UAVProxy.isNull()) + } + if (!UAVProxy.isNull()) { delete UAVProxy; + } } // ************************************************************************************* @@ -318,39 +318,35 @@ void OPMapGadgetWidget::resizeEvent(QResizeEvent *event) void OPMapGadgetWidget::mouseMoveEvent(QMouseEvent *event) { - if (m_widget && m_map) - { - } + if (m_widget && m_map) {} - if (event->buttons() & Qt::LeftButton) - { - } + if (event->buttons() & Qt::LeftButton) {} QWidget::mouseMoveEvent(event); } -void OPMapGadgetWidget::wpDoubleClickEvent(WayPointItem *wp) +void OPMapGadgetWidget::wpDoubleClickEvent(WayPointItem *wp) { - m_mouse_waypoint = wp; - onEditWayPointAct_triggered(); + m_mouse_waypoint = wp; + onEditWayPointAct_triggered(); } void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) -{ // the user has right clicked on the map - create the pop-up context menu and display it - +{ // the user has right clicked on the map - create the pop-up context menu and display it QString s; - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - if (event->reason() != QContextMenuEvent::Mouse) - return; // not a mouse click event - + if (event->reason() != QContextMenuEvent::Mouse) { + return; // not a mouse click event + } // current mouse position QPoint p = m_map->mapFromGlobal(event->globalPos()); m_context_menu_lat_lon = m_map->GetFromLocalToLatLng(p); - if (!m_map->contentsRect().contains(p)) - return; // the mouse click was not on the map - + if (!m_map->contentsRect().contains(p)) { + return; // the mouse click was not on the map + } // show the mouse position s = QString::number(m_context_menu_lat_lon.Lat(), 'f', 7) + " " + QString::number(m_context_menu_lat_lon.Lng(), 'f', 7); m_widget->labelMousePos->setText(s); @@ -361,8 +357,9 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) // find out if the waypoint is locked (or not) bool waypoint_locked = false; - if (m_mouse_waypoint) + if (m_mouse_waypoint) { waypoint_locked = (m_mouse_waypoint->flags() & QGraphicsItem::ItemIsMovable) == 0; + } // **************** // Dynamically create the popup menu @@ -375,28 +372,31 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) contextMenu.addSeparator(); QMenu maxUpdateRateSubMenu(tr("&Max Update Rate ") + "(" + QString::number(m_maxUpdateRate) + " ms)", this); - for (int i = 0; i < maxUpdateRateAct.count(); i++) + for (int i = 0; i < maxUpdateRateAct.count(); i++) { maxUpdateRateSubMenu.addAction(maxUpdateRateAct.at(i)); + } contextMenu.addMenu(&maxUpdateRateSubMenu); contextMenu.addSeparator(); - switch (m_map_mode) - { - case Normal_MapMode: s = tr(" (Normal)"); break; - case MagicWaypoint_MapMode: s = tr(" (Magic Waypoint)"); break; - default: s = tr(" (Unknown)"); break; + switch (m_map_mode) { + case Normal_MapMode: s = tr(" (Normal)"); break; + case MagicWaypoint_MapMode: s = tr(" (Magic Waypoint)"); break; + default: s = tr(" (Unknown)"); break; } - for (int i = 0; i < mapModeAct.count(); i++) - { // set the menu to checked (or not) + for (int i = 0; i < mapModeAct.count(); i++) { // set the menu to checked (or not) QAction *act = mapModeAct.at(i); - if (!act) continue; - if (act->data().toInt() == (int)m_map_mode) + if (!act) { + continue; + } + if (act->data().toInt() == (int)m_map_mode) { act->setChecked(true); + } } QMenu mapModeSubMenu(tr("Map mode") + s, this); - for (int i = 0; i < mapModeAct.count(); i++) + for (int i = 0; i < mapModeAct.count(); i++) { mapModeSubMenu.addAction(mapModeAct.at(i)); + } contextMenu.addMenu(&mapModeSubMenu); contextMenu.addSeparator(); @@ -412,10 +412,11 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) contextMenu.addSeparator(); QMenu safeArea("Safety Area definitions"); - // menu.addAction(showSafeAreaAct); + // menu.addAction(showSafeAreaAct); QMenu safeAreaSubMenu(tr("Safe Area Radius") + " (" + QString::number(m_map->Home->SafeArea()) + "m)", this); - for (int i = 0; i < safeAreaAct.count(); i++) + for (int i = 0; i < safeAreaAct.count(); i++) { safeAreaSubMenu.addAction(safeAreaAct.at(i)); + } safeArea.addMenu(&safeAreaSubMenu); safeArea.addAction(showSafeAreaAct); contextMenu.addMenu(&safeArea); @@ -434,8 +435,9 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) contextMenu.addAction(zoomOutAct); QMenu zoomSubMenu(tr("&Zoom ") + "(" + QString::number(m_map->ZoomTotal()) + ")", this); - for (int i = 0; i < zoomAct.count(); i++) + for (int i = 0; i < zoomAct.count(); i++) { zoomSubMenu.addAction(zoomAct.at(i)); + } contextMenu.addMenu(&zoomSubMenu); contextMenu.addSeparator(); @@ -454,18 +456,21 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) uav_menu.addSeparator()->setText(tr("UAV Trail")); contextMenu.addMenu(&uav_menu); QMenu uavTrailTypeSubMenu(tr("UAV trail type") + " (" + mapcontrol::Helper::StrFromUAVTrailType(m_map->UAV->GetTrailType()) + ")", this); - for (int i = 0; i < uavTrailTypeAct.count(); i++) + for (int i = 0; i < uavTrailTypeAct.count(); i++) { uavTrailTypeSubMenu.addAction(uavTrailTypeAct.at(i)); + } uav_menu.addMenu(&uavTrailTypeSubMenu); QMenu uavTrailTimeSubMenu(tr("UAV trail time") + " (" + QString::number(m_map->UAV->TrailTime()) + " sec)", this); - for (int i = 0; i < uavTrailTimeAct.count(); i++) + for (int i = 0; i < uavTrailTimeAct.count(); i++) { uavTrailTimeSubMenu.addAction(uavTrailTimeAct.at(i)); + } uav_menu.addMenu(&uavTrailTimeSubMenu); QMenu uavTrailDistanceSubMenu(tr("UAV trail distance") + " (" + QString::number(m_map->UAV->TrailDistance()) + " meters)", this); - for (int i = 0; i < uavTrailDistanceAct.count(); i++) + for (int i = 0; i < uavTrailDistanceAct.count(); i++) { uavTrailDistanceSubMenu.addAction(uavTrailDistanceAct.at(i)); + } uav_menu.addMenu(&uavTrailDistanceSubMenu); uav_menu.addAction(showTrailAct); @@ -485,49 +490,49 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) // ********* #ifdef USE_PATHPLANNER - switch (m_map_mode) - { - case Normal_MapMode: + switch (m_map_mode) { + case Normal_MapMode: // only show the waypoint stuff if not in 'magic waypoint' mode - contextMenu.addSeparator()->setText(tr("Waypoints")); + contextMenu.addSeparator()->setText(tr("Waypoints")); - contextMenu.addAction(wayPointEditorAct); - contextMenu.addAction(addWayPointActFromContextMenu); + contextMenu.addAction(wayPointEditorAct); + contextMenu.addAction(addWayPointActFromContextMenu); - if (m_mouse_waypoint) - { // we have a waypoint under the mouse - contextMenu.addAction(editWayPointAct); + if (m_mouse_waypoint) { // we have a waypoint under the mouse + contextMenu.addAction(editWayPointAct); - lockWayPointAct->setChecked(waypoint_locked); - contextMenu.addAction(lockWayPointAct); + lockWayPointAct->setChecked(waypoint_locked); + contextMenu.addAction(lockWayPointAct); - if (!waypoint_locked) - contextMenu.addAction(deleteWayPointAct); + if (!waypoint_locked) { + contextMenu.addAction(deleteWayPointAct); } + } - if (m_map->WPPresent()) - contextMenu.addAction(clearWayPointsAct); // we have waypoints + if (m_map->WPPresent()) { + contextMenu.addAction(clearWayPointsAct); // we have waypoints + } + break; - break; - - case MagicWaypoint_MapMode: - contextMenu.addSeparator()->setText(tr("Waypoints")); - contextMenu.addAction(homeMagicWaypointAct); - break; + case MagicWaypoint_MapMode: + contextMenu.addSeparator()->setText(tr("Waypoints")); + contextMenu.addAction(homeMagicWaypointAct); + break; } -#endif - // ********* +#endif // ifdef USE_PATHPLANNER + // ********* - QMenu overlaySubMenu(tr("&Overlay Opacity "),this); - for (int i = 0; i < overlayOpacityAct.count(); i++) + QMenu overlaySubMenu(tr("&Overlay Opacity "), this); + for (int i = 0; i < overlayOpacityAct.count(); i++) { overlaySubMenu.addAction(overlayOpacityAct.at(i)); + } contextMenu.addMenu(&overlaySubMenu); contextMenu.addSeparator(); contextMenu.addAction(closeAct2); - contextMenu.exec(event->globalPos()); // popup the menu + contextMenu.exec(event->globalPos()); // popup the menu // **************** } @@ -542,166 +547,164 @@ void OPMapGadgetWidget::closeEvent(QCloseEvent *event) // timer signals /** - Updates the UAV position on the map. It is called at a user-defined frequency, - as set inside the map widget. -*/ + Updates the UAV position on the map. It is called at a user-defined frequency, + as set inside the map widget. + */ void OPMapGadgetWidget::updatePosition() { double uav_latitude, uav_longitude, uav_altitude, uav_yaw; double gps_latitude, gps_longitude, gps_altitude, gps_heading; - internals::PointLatLng uav_pos; - internals::PointLatLng gps_pos; + internals::PointLatLng uav_pos; + internals::PointLatLng gps_pos; - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } QMutexLocker locker(&m_map_mutex); - // ************* - // get the current UAV details + // ************* + // get the current UAV details // get current UAV position - if (!getUAVPosition(uav_latitude, uav_longitude, uav_altitude)) + if (!getUAVPosition(uav_latitude, uav_longitude, uav_altitude)) { return; + } // get current UAV heading - uav_yaw = getUAV_Yaw(); + uav_yaw = getUAV_Yaw(); - uav_pos = internals::PointLatLng(uav_latitude, uav_longitude); + uav_pos = internals::PointLatLng(uav_latitude, uav_longitude); - // ************* + // ************* // get the current GPS position and heading GPSPosition *gpsPositionObj = GPSPosition::GetInstance(obm); Q_ASSERT(gpsPositionObj); GPSPosition::DataFields gpsPositionData = gpsPositionObj->getData(); - gps_heading = gpsPositionData.Heading; - gps_latitude = gpsPositionData.Latitude; + gps_heading = gpsPositionData.Heading; + gps_latitude = gpsPositionData.Latitude; gps_longitude = gpsPositionData.Longitude; - gps_altitude = gpsPositionData.Altitude; + gps_altitude = gpsPositionData.Altitude; - gps_pos = internals::PointLatLng(gps_latitude*1e-7, gps_longitude*1e-7); + gps_pos = internals::PointLatLng(gps_latitude * 1e-7, gps_longitude * 1e-7); - //********************** + // ********************** // get the current position and heading estimates AttitudeActual *attitudeActualObj = AttitudeActual::GetInstance(obm); PositionActual *positionActualObj = PositionActual::GetInstance(obm); VelocityActual *velocityActualObj = VelocityActual::GetInstance(obm); - AirspeedActual *airspeedActualObj = AirspeedActual::GetInstance(obm); + AirspeedActual *airspeedActualObj = AirspeedActual::GetInstance(obm); Gyros *gyrosObj = Gyros::GetInstance(obm); Q_ASSERT(attitudeActualObj); Q_ASSERT(positionActualObj); Q_ASSERT(velocityActualObj); - Q_ASSERT(airspeedActualObj); + Q_ASSERT(airspeedActualObj); Q_ASSERT(gyrosObj); AttitudeActual::DataFields attitudeActualData = attitudeActualObj->getData(); PositionActual::DataFields positionActualData = positionActualObj->getData(); VelocityActual::DataFields velocityActualData = velocityActualObj->getData(); - AirspeedActual::DataFields airspeedActualData = airspeedActualObj->getData(); + AirspeedActual::DataFields airspeedActualData = airspeedActualObj->getData(); Gyros::DataFields gyrosData = gyrosObj->getData(); - double NED[3]={positionActualData.North, positionActualData.East, positionActualData.Down}; - double vNED[3]={velocityActualData.North, velocityActualData.East, velocityActualData.Down}; + double NED[3] = { positionActualData.North, positionActualData.East, positionActualData.Down }; + double vNED[3] = { velocityActualData.North, velocityActualData.East, velocityActualData.Down }; - //Set the position and heading estimates in the painter module + // Set the position and heading estimates in the painter module m_map->UAV->SetNED(NED); - m_map->UAV->SetCAS(airspeedActualData.CalibratedAirspeed); + m_map->UAV->SetCAS(airspeedActualData.CalibratedAirspeed); m_map->UAV->SetGroundspeed(vNED, m_maxUpdateRate); - //Convert angular velocities into a rotationg rate around the world-frame yaw axis. This is found by simply taking the dot product of the angular Euler-rate matrix with the angular rates. - float psiRate_dps=0*gyrosData.z + sin(attitudeActualData.Roll*deg_to_rad)/cos(attitudeActualData.Pitch*deg_to_rad)*gyrosData.y + cos(attitudeActualData.Roll*deg_to_rad)/cos(attitudeActualData.Pitch*deg_to_rad)*gyrosData.z; + // Convert angular velocities into a rotationg rate around the world-frame yaw axis. This is found by simply taking the dot product of the angular Euler-rate matrix with the angular rates. + float psiRate_dps = 0 * gyrosData.z + sin(attitudeActualData.Roll * deg_to_rad) / cos(attitudeActualData.Pitch * deg_to_rad) * gyrosData.y + cos(attitudeActualData.Roll * deg_to_rad) / cos(attitudeActualData.Pitch * deg_to_rad) * gyrosData.z; - //Set the angular rate in the painter module - m_map->UAV->SetYawRate(psiRate_dps); //Not correct, but I'm being lazy right now. + // Set the angular rate in the painter module + m_map->UAV->SetYawRate(psiRate_dps); // Not correct, but I'm being lazy right now. // ************* - // display the UAV position + // display the UAV position QString str = - "lat: " + QString::number(uav_pos.Lat(), 'f', 7) + - " lon: " + QString::number(uav_pos.Lng(), 'f', 7) + - " " + QString::number(uav_yaw, 'f', 1) + "deg" + - " " + QString::number(uav_altitude, 'f', 1) + "m"; -// " " + QString::number(uav_ground_speed_meters_per_second, 'f', 1) + "m/s"; + "lat: " + QString::number(uav_pos.Lat(), 'f', 7) + + " lon: " + QString::number(uav_pos.Lng(), 'f', 7) + + " " + QString::number(uav_yaw, 'f', 1) + "deg" + + " " + QString::number(uav_altitude, 'f', 1) + "m"; +// " " + QString::number(uav_ground_speed_meters_per_second, 'f', 1) + "m/s"; m_widget->labelUAVPos->setText(str); - // ************* - // set the UAV icon position on the map + // ************* + // set the UAV icon position on the map - m_map->UAV->SetUAVPos(uav_pos, uav_altitude); // set the maps UAV position -// qDebug()<<"UAVPOSITION"<UAV->SetUAVHeading(uav_yaw); // set the maps UAV heading + m_map->UAV->SetUAVPos(uav_pos, uav_altitude); // set the maps UAV position +// qDebug()<<"UAVPOSITION"<UAV->SetUAVHeading(uav_yaw); // set the maps UAV heading - // ************* - // set the GPS icon position on the map - if(m_map->GPS) - { + // ************* + // set the GPS icon position on the map + if (m_map->GPS) { m_map->GPS->SetUAVPos(gps_pos, gps_altitude); // set the maps GPS position - m_map->GPS->SetUAVHeading(gps_heading); // set the maps GPS heading + m_map->GPS->SetUAVHeading(gps_heading); // set the maps GPS heading m_map->GPS->update(); } m_map->UAV->updateTextOverlay(); m_map->UAV->update(); - // ************* + // ************* } /** - Update plugin behaviour based on mouse position; Called every few ms by a - timer. + Update plugin behaviour based on mouse position; Called every few ms by a + timer. */ void OPMapGadgetWidget::updateMousePos() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } QMutexLocker locker(&m_map_mutex); QPoint p = m_map->mapFromGlobal(QCursor::pos()); - internals::PointLatLng lat_lon = m_map->GetFromLocalToLatLng(p); // fetch the current lat/lon mouse position - lastLatLngMouse=lat_lon; - if (!m_map->contentsRect().contains(p)) - return; // the mouse is not on the map - -// internals::PointLatLng lat_lon = m_map->currentMousePosition(); // fetch the current lat/lon mouse position + internals::PointLatLng lat_lon = m_map->GetFromLocalToLatLng(p); // fetch the current lat/lon mouse position + lastLatLngMouse = lat_lon; + if (!m_map->contentsRect().contains(p)) { + return; // the mouse is not on the map + } +// internals::PointLatLng lat_lon = m_map->currentMousePosition(); // fetch the current lat/lon mouse position QGraphicsItem *item = m_map->itemAt(p); // find out if we are over the home position - mapcontrol::HomeItem *home = qgraphicsitem_cast(item); + mapcontrol::HomeItem *home = qgraphicsitem_cast(item); // find out if we have a waypoint under the mouse cursor mapcontrol::WayPointItem *wp = qgraphicsitem_cast(item); - if (m_mouse_lat_lon == lat_lon) - return; // the mouse has not moved - - m_mouse_lat_lon = lat_lon; // yes it has! + if (m_mouse_lat_lon == lat_lon) { + return; // the mouse has not moved + } + m_mouse_lat_lon = lat_lon; // yes it has! internals::PointLatLng home_lat_lon = m_map->Home->Coord(); - QString s = QString::number(m_mouse_lat_lon.Lat(), 'f', 7) + " " + QString::number(m_mouse_lat_lon.Lng(), 'f', 7); - if (wp) - { + QString s = QString::number(m_mouse_lat_lon.Lat(), 'f', 7) + " " + QString::number(m_mouse_lat_lon.Lng(), 'f', 7); + if (wp) { s += " wp[" + QString::number(wp->numberAdjusted()) + "]"; double dist = distance(home_lat_lon, wp->Coord()); double bear = bearing(home_lat_lon, wp->Coord()); s += " " + QString::number(dist * 1000, 'f', 1) + "m"; s += " " + QString::number(bear, 'f', 1) + "deg"; - } - else - if (home) - { + } else if (home) { s += " home"; - double dist = distance(home_lat_lon, m_mouse_lat_lon); - double bear = bearing(home_lat_lon, m_mouse_lat_lon); + double dist = distance(home_lat_lon, m_mouse_lat_lon); + double bear = bearing(home_lat_lon, m_mouse_lat_lon); s += " " + QString::number(dist * 1000, 'f', 1) + "m"; s += " " + QString::number(bear, 'f', 1) + "deg"; } @@ -713,75 +716,84 @@ void OPMapGadgetWidget::updateMousePos() /** - Update the Plugin UI to reflect a change in zoom level - */ + Update the Plugin UI to reflect a change in zoom level + */ void OPMapGadgetWidget::zoomChanged(double zoomt, double zoom, double zoomd) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - QString s = "tot:" + QString::number(zoomt, 'f', 1) + " rea:" + QString::number(zoom, 'f', 1) + " dig:" + QString::number(zoomd, 'f', 1); + QString s = "tot:" + QString::number(zoomt, 'f', 1) + " rea:" + QString::number(zoom, 'f', 1) + " dig:" + QString::number(zoomd, 'f', 1); m_widget->labelMapZoom->setText(s); int i_zoom = (int)(zoomt + 0.5); - if (i_zoom < m_min_zoom) i_zoom = m_min_zoom; - else - if (i_zoom > m_max_zoom) i_zoom = m_max_zoom; + if (i_zoom < m_min_zoom) { + i_zoom = m_min_zoom; + } else if (i_zoom > m_max_zoom) { + i_zoom = m_max_zoom; + } - if (m_widget->horizontalSliderZoom->value() != i_zoom) - m_widget->horizontalSliderZoom->setValue(i_zoom); // set the GUI zoom slider position - - int index0_zoom = i_zoom - m_min_zoom; // zoom level starting at index level '0' - if (index0_zoom < zoomAct.count()) - zoomAct.at(index0_zoom)->setChecked(true); // set the right-click context menu zoom level + if (m_widget->horizontalSliderZoom->value() != i_zoom) { + m_widget->horizontalSliderZoom->setValue(i_zoom); // set the GUI zoom slider position + } + int index0_zoom = i_zoom - m_min_zoom; // zoom level starting at index level '0' + if (index0_zoom < zoomAct.count()) { + zoomAct.at(index0_zoom)->setChecked(true); // set the right-click context menu zoom level + } } void OPMapGadgetWidget::OnCurrentPositionChanged(internals::PointLatLng point) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } QString coord_str = QString::number(point.Lat(), 'f', 7) + " " + QString::number(point.Lng(), 'f', 7) + " "; m_widget->labelMapPos->setText(coord_str); } /** - Update the progress bar while there are still tiles to load - */ + Update the progress bar while there are still tiles to load + */ void OPMapGadgetWidget::OnTilesStillToLoad(int number) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - if (m_widget->progressBarMap->maximum() < number) - m_widget->progressBarMap->setMaximum(number); + if (m_widget->progressBarMap->maximum() < number) { + m_widget->progressBarMap->setMaximum(number); + } - m_widget->progressBarMap->setValue(m_widget->progressBarMap->maximum() - number); // update the progress bar + m_widget->progressBarMap->setValue(m_widget->progressBarMap->maximum() - number); // update the progress bar - m_prev_tile_number = number; + m_prev_tile_number = number; } /** - Show the progress bar as soon as the map lib starts downloading - */ + Show the progress bar as soon as the map lib starts downloading + */ void OPMapGadgetWidget::OnTileLoadStart() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_widget->progressBarMap->setVisible(true); } /** - Hide the progress bar once the map lib has finished downloading + Hide the progress bar once the map lib has finished downloading - TODO: somehow this gets called before tile load is actually complete? - */ + TODO: somehow this gets called before tile load is actually complete? + */ void OPMapGadgetWidget::OnTileLoadComplete() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_widget->progressBarMap->setVisible(false); } @@ -789,25 +801,29 @@ void OPMapGadgetWidget::OnTileLoadComplete() void OPMapGadgetWidget::on_toolButtonZoomP_clicked() { QMutexLocker locker(&m_map_mutex); + zoomIn(); } void OPMapGadgetWidget::on_toolButtonZoomM_clicked() { QMutexLocker locker(&m_map_mutex); + zoomOut(); } void OPMapGadgetWidget::on_toolButtonMapHome_clicked() { QMutexLocker locker(&m_map_mutex); + goHome(); } void OPMapGadgetWidget::on_toolButtonMapUAV_clicked() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } QMutexLocker locker(&m_map_mutex); @@ -816,16 +832,18 @@ void OPMapGadgetWidget::on_toolButtonMapUAV_clicked() void OPMapGadgetWidget::on_toolButtonMapUAVheading_clicked() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } followUAVheadingAct->toggle(); } void OPMapGadgetWidget::on_horizontalSliderZoom_sliderMoved(int position) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } QMutexLocker locker(&m_map_mutex); @@ -858,103 +876,119 @@ void OPMapGadgetWidget::on_toolButtonMoveToWP_clicked() void OPMapGadgetWidget::onTelemetryConnect() { - m_telemetry_connected = true; + m_telemetry_connected = true; - if (!obum) return; + if (!obum) { + return; + } - bool set; - double LLA[3]; + bool set; + double LLA[3]; - // *********************** - // fetch the home location + // *********************** + // fetch the home location - if (obum->getHomeLocation(set, LLA) < 0) - return; // error + if (obum->getHomeLocation(set, LLA) < 0) { + return; // error + } + setHome(internals::PointLatLng(LLA[0], LLA[1]), LLA[2]); - setHome(internals::PointLatLng(LLA[0], LLA[1]),LLA[2]); - - if (m_map) - { - if(m_map->UAV->GetMapFollowType()!=UAVMapFollowType::None) - m_map->SetCurrentPosition(m_home_position.coord); // set the map position + if (m_map) { + if (m_map->UAV->GetMapFollowType() != UAVMapFollowType::None) { + m_map->SetCurrentPosition(m_home_position.coord); // set the map position + } } // *********************** } void OPMapGadgetWidget::onTelemetryDisconnect() { - m_telemetry_connected = false; + m_telemetry_connected = false; } // Updates the Home position icon whenever the HomePosition object is updated void OPMapGadgetWidget::homePositionUpdated(UAVObject *hp) { Q_UNUSED(hp); - if (!obum) return; + if (!obum) { + return; + } bool set; double LLA[3]; - if (obum->getHomeLocation(set, LLA) < 0) - return; // error - setHome(internals::PointLatLng(LLA[0], LLA[1]),LLA[2]); - + if (obum->getHomeLocation(set, LLA) < 0) { + return; // error + } + setHome(internals::PointLatLng(LLA[0], LLA[1]), LLA[2]); } // ************************************************************************************* // public functions /** - Sets the home position on the map widget - */ + Sets the home position on the map widget + */ void OPMapGadgetWidget::setHome(QPointF pos) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - double latitude = pos.x(); + double latitude = pos.x(); double longitude = pos.y(); - if (latitude > 90) latitude = 90; - else - if (latitude < -90) latitude = -90; + if (latitude > 90) { + latitude = 90; + } else if (latitude < -90) { + latitude = -90; + } - if (longitude != longitude) longitude = 0; // nan detection - else - if (longitude > 180) longitude = 180; - else - if (longitude < -180) longitude = -180; + if (longitude != longitude) { + longitude = 0; // nan detection + } else if (longitude > 180) { + longitude = 180; + } else if (longitude < -180) { + longitude = -180; + } - setHome(internals::PointLatLng(latitude, longitude),0); + setHome(internals::PointLatLng(latitude, longitude), 0); } /** - Sets the home position on the map widget - */ + Sets the home position on the map widget + */ void OPMapGadgetWidget::setHome(internals::PointLatLng pos_lat_lon, double altitude) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - if (pos_lat_lon.Lat() != pos_lat_lon.Lat() || pos_lat_lon.Lng() != pos_lat_lon.Lng()) - return;; // nan prevention + if (pos_lat_lon.Lat() != pos_lat_lon.Lat() || pos_lat_lon.Lng() != pos_lat_lon.Lng()) { + return; + } + ; // nan prevention - double latitude = pos_lat_lon.Lat(); + double latitude = pos_lat_lon.Lat(); double longitude = pos_lat_lon.Lng(); - if (latitude != latitude) latitude = 0; // nan detection - else - if (latitude > 90) latitude = 90; - else - if (latitude < -90) latitude = -90; + if (latitude != latitude) { + latitude = 0; // nan detection + } else if (latitude > 90) { + latitude = 90; + } else if (latitude < -90) { + latitude = -90; + } - if (longitude != longitude) longitude = 0; // nan detection - else - if (longitude > 180) longitude = 180; - else - if (longitude < -180) longitude = -180; + if (longitude != longitude) { + longitude = 0; // nan detection + } else if (longitude > 180) { + longitude = 180; + } else if (longitude < -180) { + longitude = -180; + } // ********* - m_home_position.coord = internals::PointLatLng(latitude, longitude); + m_home_position.coord = internals::PointLatLng(latitude, longitude); m_home_position.altitude = altitude; m_map->Home->SetCoord(m_home_position.coord); @@ -967,78 +1001,92 @@ void OPMapGadgetWidget::setHome(internals::PointLatLng pos_lat_lon, double altit /** - Centers the map over the home position - */ + Centers the map over the home position + */ void OPMapGadgetWidget::goHome() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } followUAVpositionAct->setChecked(false); - internals::PointLatLng home_pos = m_home_position.coord; // get the home location - m_map->SetCurrentPosition(home_pos); // center the map onto the home location + internals::PointLatLng home_pos = m_home_position.coord; // get the home location + m_map->SetCurrentPosition(home_pos); // center the map onto the home location } void OPMapGadgetWidget::zoomIn() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } int zoom = m_map->ZoomTotal() + 1; - if (zoom < m_min_zoom) zoom = m_min_zoom; - else - if (zoom > m_max_zoom) zoom = m_max_zoom; + if (zoom < m_min_zoom) { + zoom = m_min_zoom; + } else if (zoom > m_max_zoom) { + zoom = m_max_zoom; + } m_map->SetZoom(zoom); } void OPMapGadgetWidget::zoomOut() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } int zoom = m_map->ZoomTotal() - 1; - if (zoom < m_min_zoom) zoom = m_min_zoom; - else - if (zoom > m_max_zoom) zoom = m_max_zoom; + if (zoom < m_min_zoom) { + zoom = m_min_zoom; + } else if (zoom > m_max_zoom) { + zoom = m_max_zoom; + } m_map->SetZoom(zoom); } void OPMapGadgetWidget::setMaxUpdateRate(int update_rate) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - int list_size = sizeof(max_update_rate_list) / sizeof(max_update_rate_list[0]); - int min_rate = max_update_rate_list[0]; - int max_rate = max_update_rate_list[list_size - 1]; + int list_size = sizeof(max_update_rate_list) / sizeof(max_update_rate_list[0]); + int min_rate = max_update_rate_list[0]; + int max_rate = max_update_rate_list[list_size - 1]; - if (update_rate < min_rate) update_rate = min_rate; - else - if (update_rate > max_rate) update_rate = max_rate; + if (update_rate < min_rate) { + update_rate = min_rate; + } else if (update_rate > max_rate) { + update_rate = max_rate; + } - m_maxUpdateRate = update_rate; + m_maxUpdateRate = update_rate; - if (m_updateTimer) - m_updateTimer->setInterval(m_maxUpdateRate); + if (m_updateTimer) { + m_updateTimer->setInterval(m_maxUpdateRate); + } -// if (m_statusUpdateTimer) -// m_statusUpdateTimer->setInterval(m_maxUpdateRate); +// if (m_statusUpdateTimer) +// m_statusUpdateTimer->setInterval(m_maxUpdateRate); } void OPMapGadgetWidget::setZoom(int zoom) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - if (zoom < m_min_zoom) zoom = m_min_zoom; - else - if (zoom > m_max_zoom) zoom = m_max_zoom; + if (zoom < m_min_zoom) { + zoom = m_min_zoom; + } else if (zoom > m_max_zoom) { + zoom = m_max_zoom; + } internals::MouseWheelZoomType::Types zoom_type = m_map->GetMouseWheelZoomType(); m_map->SetMouseWheelZoomType(internals::MouseWheelZoomType::ViewCenter); @@ -1049,167 +1097,188 @@ void OPMapGadgetWidget::setZoom(int zoom) } void OPMapGadgetWidget::setOverlayOpacity(qreal value) { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } m_map->setOverlayOpacity(value); - overlayOpacityAct.at(value*10)->setChecked(true); + overlayOpacityAct.at(value * 10)->setChecked(true); } void OPMapGadgetWidget::setHomePosition(QPointF pos) { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - double latitude = pos.y(); + double latitude = pos.y(); double longitude = pos.x(); - if (latitude != latitude || longitude != longitude) + if (latitude != latitude || longitude != longitude) { return; // nan prevention + } + if (latitude > 90) { + latitude = 90; + } else if (latitude < -90) { + latitude = -90; + } - if (latitude > 90) latitude = 90; - else - if (latitude < -90) latitude = -90; - - if (longitude > 180) longitude = 180; - else - if (longitude < -180) longitude = -180; + if (longitude > 180) { + longitude = 180; + } else if (longitude < -180) { + longitude = -180; + } m_map->Home->SetCoord(internals::PointLatLng(latitude, longitude)); } void OPMapGadgetWidget::setPosition(QPointF pos) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - double latitude = pos.y(); + double latitude = pos.y(); double longitude = pos.x(); - if (latitude != latitude || longitude != longitude) + if (latitude != latitude || longitude != longitude) { return; // nan prevention + } + if (latitude > 90) { + latitude = 90; + } else if (latitude < -90) { + latitude = -90; + } - if (latitude > 90) latitude = 90; - else - if (latitude < -90) latitude = -90; - - if (longitude > 180) longitude = 180; - else - if (longitude < -180) longitude = -180; + if (longitude > 180) { + longitude = 180; + } else if (longitude < -180) { + longitude = -180; + } m_map->SetCurrentPosition(internals::PointLatLng(latitude, longitude)); } void OPMapGadgetWidget::setMapProvider(QString provider) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->SetMapType(mapcontrol::Helper::MapTypeFromString(provider)); } void OPMapGadgetWidget::setAccessMode(QString accessMode) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->configuration->SetAccessMode(mapcontrol::Helper::AccessModeFromString(accessMode)); } void OPMapGadgetWidget::setUseOpenGL(bool useOpenGL) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->SetUseOpenGL(useOpenGL); } void OPMapGadgetWidget::setShowTileGridLines(bool showTileGridLines) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->SetShowTileGridLines(showTileGridLines); } void OPMapGadgetWidget::setUseMemoryCache(bool useMemoryCache) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->configuration->SetUseMemoryCache(useMemoryCache); } void OPMapGadgetWidget::setCacheLocation(QString cacheLocation) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - cacheLocation = cacheLocation.simplified(); // remove any surrounding spaces + cacheLocation = cacheLocation.simplified(); // remove any surrounding spaces - if (cacheLocation.isEmpty()) return; + if (cacheLocation.isEmpty()) { + return; + } - if (!cacheLocation.endsWith(QDir::separator())) cacheLocation += QDir::separator(); + if (!cacheLocation.endsWith(QDir::separator())) { + cacheLocation += QDir::separator(); + } QDir dir; - if (!dir.exists(cacheLocation)) - if (!dir.mkpath(cacheLocation)) + if (!dir.exists(cacheLocation)) { + if (!dir.mkpath(cacheLocation)) { return; + } + } m_map->configuration->SetCacheLocation(cacheLocation); } void OPMapGadgetWidget::setMapMode(opMapModeType mode) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - if (mode != Normal_MapMode && mode != MagicWaypoint_MapMode) - mode = Normal_MapMode; // fix error - - if (m_map_mode == mode) - { // no change in map mode - switch (mode) - { // make sure the UI buttons are set correctly - case Normal_MapMode: - m_widget->toolButtonMagicWaypointMapMode->setChecked(false); - m_widget->toolButtonNormalMapMode->setChecked(true); - break; - case MagicWaypoint_MapMode: - m_widget->toolButtonNormalMapMode->setChecked(false); - m_widget->toolButtonMagicWaypointMapMode->setChecked(true); - break; + if (mode != Normal_MapMode && mode != MagicWaypoint_MapMode) { + mode = Normal_MapMode; // fix error + } + if (m_map_mode == mode) { // no change in map mode + switch (mode) { // make sure the UI buttons are set correctly + case Normal_MapMode: + m_widget->toolButtonMagicWaypointMapMode->setChecked(false); + m_widget->toolButtonNormalMapMode->setChecked(true); + break; + case MagicWaypoint_MapMode: + m_widget->toolButtonNormalMapMode->setChecked(false); + m_widget->toolButtonMagicWaypointMapMode->setChecked(true); + break; } return; } - switch (mode) - { - case Normal_MapMode: - m_map_mode = Normal_MapMode; + switch (mode) { + case Normal_MapMode: + m_map_mode = Normal_MapMode; - m_widget->toolButtonMagicWaypointMapMode->setChecked(false); - m_widget->toolButtonNormalMapMode->setChecked(true); + m_widget->toolButtonMagicWaypointMapMode->setChecked(false); + m_widget->toolButtonNormalMapMode->setChecked(true); - hideMagicWaypointControls(); + hideMagicWaypointControls(); - magicWayPoint->setVisible(false); - m_map->WPSetVisibleAll(true); + magicWayPoint->setVisible(false); + m_map->WPSetVisibleAll(true); - break; + break; - case MagicWaypoint_MapMode: - m_map_mode = MagicWaypoint_MapMode; + case MagicWaypoint_MapMode: + m_map_mode = MagicWaypoint_MapMode; - m_widget->toolButtonNormalMapMode->setChecked(false); - m_widget->toolButtonMagicWaypointMapMode->setChecked(true); + m_widget->toolButtonNormalMapMode->setChecked(false); + m_widget->toolButtonMagicWaypointMapMode->setChecked(true); - showMagicWaypointControls(); + showMagicWaypointControls(); - // delete the normal waypoints from the map + // delete the normal waypoints from the map - m_map->WPSetVisibleAll(false); - magicWayPoint->setVisible(true); + m_map->WPSetVisibleAll(false); + magicWayPoint->setVisible(true); - break; + break; } } @@ -1218,10 +1287,11 @@ void OPMapGadgetWidget::setMapMode(opMapModeType mode) void OPMapGadgetWidget::createActions() { - int list_size; + int list_size; - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } // *********************** // create menu actions @@ -1306,7 +1376,7 @@ void OPMapGadgetWidget::createActions() setHomeAct = new QAction(tr("Set the home location"), this); setHomeAct->setStatusTip(tr("Set the home location to where you clicked")); #if !defined(allow_manual_home_location_move) - setHomeAct->setEnabled(false); + setHomeAct->setEnabled(false); #endif connect(setHomeAct, SIGNAL(triggered()), this, SLOT(onSetHomeAct_triggered())); @@ -1335,8 +1405,8 @@ void OPMapGadgetWidget::createActions() connect(followUAVheadingAct, SIGNAL(toggled(bool)), this, SLOT(onFollowUAVheadingAct_toggled(bool))); /* - TODO: Waypoint support is disabled for v1.0 - */ + TODO: Waypoint support is disabled for v1.0 + */ #ifdef USE_PATHPLANNER wayPointEditorAct = new QAction(tr("&Waypoint editor"), this); @@ -1375,15 +1445,14 @@ void OPMapGadgetWidget::createActions() clearWayPointsAct->setShortcut(tr("Ctrl+C")); clearWayPointsAct->setStatusTip(tr("Clear waypoints")); connect(clearWayPointsAct, SIGNAL(triggered()), this, SLOT(onClearWayPointsAct_triggered())); -#endif +#endif // ifdef USE_PATHPLANNER overlayOpacityActGroup = new QActionGroup(this); connect(overlayOpacityActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onOverlayOpacityActGroup_triggered(QAction *))); overlayOpacityAct.clear(); - for (int i = 0; i <= 10; i++) - { - QAction *overlayAct = new QAction(QString::number(i*10), overlayOpacityActGroup); + for (int i = 0; i <= 10; i++) { + QAction *overlayAct = new QAction(QString::number(i * 10), overlayOpacityActGroup); overlayAct->setCheckable(true); - overlayAct->setData(i*10); + overlayAct->setData(i * 10); overlayOpacityAct.append(overlayAct); } @@ -1413,30 +1482,28 @@ void OPMapGadgetWidget::createActions() zoomActGroup = new QActionGroup(this); connect(zoomActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onZoomActGroup_triggered(QAction *))); zoomAct.clear(); - for (int i = m_min_zoom; i <= m_max_zoom; i++) - { + for (int i = m_min_zoom; i <= m_max_zoom; i++) { QAction *zoom_act = new QAction(QString::number(i), zoomActGroup); zoom_act->setCheckable(true); zoom_act->setData(i); zoomAct.append(zoom_act); } - maxUpdateRateActGroup = new QActionGroup(this); - connect(maxUpdateRateActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onMaxUpdateRateActGroup_triggered(QAction *))); - maxUpdateRateAct.clear(); - list_size = sizeof(max_update_rate_list) / sizeof(max_update_rate_list[0]); - for (int i = 0; i < list_size; i++) - { - QAction *maxUpdateRate_act; - int j = max_update_rate_list[i]; - maxUpdateRate_act = new QAction(QString::number(j), maxUpdateRateActGroup); - maxUpdateRate_act->setCheckable(true); - maxUpdateRate_act->setData(j); - maxUpdateRate_act->setChecked(j == m_maxUpdateRate); - maxUpdateRateAct.append(maxUpdateRate_act); - } + maxUpdateRateActGroup = new QActionGroup(this); + connect(maxUpdateRateActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onMaxUpdateRateActGroup_triggered(QAction *))); + maxUpdateRateAct.clear(); + list_size = sizeof(max_update_rate_list) / sizeof(max_update_rate_list[0]); + for (int i = 0; i < list_size; i++) { + QAction *maxUpdateRate_act; + int j = max_update_rate_list[i]; + maxUpdateRate_act = new QAction(QString::number(j), maxUpdateRateActGroup); + maxUpdateRate_act->setCheckable(true); + maxUpdateRate_act->setData(j); + maxUpdateRate_act->setChecked(j == m_maxUpdateRate); + maxUpdateRateAct.append(maxUpdateRate_act); + } - // ***** + // ***** // safe area showSafeAreaAct = new QAction(tr("Show Safe Area"), this); @@ -1448,9 +1515,8 @@ void OPMapGadgetWidget::createActions() safeAreaActGroup = new QActionGroup(this); connect(safeAreaActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onSafeAreaActGroup_triggered(QAction *))); safeAreaAct.clear(); - list_size = sizeof(safe_area_radius_list) / sizeof(safe_area_radius_list[0]); - for (int i = 0; i < list_size; i++) - { + list_size = sizeof(safe_area_radius_list) / sizeof(safe_area_radius_list[0]); + for (int i = 0; i < list_size; i++) { int safeArea = safe_area_radius_list[i]; QAction *safeArea_act = new QAction(QString::number(safeArea) + "m", safeAreaActGroup); safeArea_act->setCheckable(true); @@ -1466,8 +1532,7 @@ void OPMapGadgetWidget::createActions() connect(uavTrailTypeActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onUAVTrailTypeActGroup_triggered(QAction *))); uavTrailTypeAct.clear(); QStringList uav_trail_type_list = mapcontrol::Helper::UAVTrailTypes(); - for (int i = 0; i < uav_trail_type_list.count(); i++) - { + for (int i = 0; i < uav_trail_type_list.count(); i++) { mapcontrol::UAVTrailType::Types uav_trail_type = mapcontrol::Helper::UAVTrailTypeFromString(uav_trail_type_list[i]); QAction *uavTrailType_act = new QAction(mapcontrol::Helper::StrFromUAVTrailType(uav_trail_type), uavTrailTypeActGroup); uavTrailType_act->setCheckable(true); @@ -1495,9 +1560,8 @@ void OPMapGadgetWidget::createActions() uavTrailTimeActGroup = new QActionGroup(this); connect(uavTrailTimeActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onUAVTrailTimeActGroup_triggered(QAction *))); uavTrailTimeAct.clear(); - list_size = sizeof(uav_trail_time_list) / sizeof(uav_trail_time_list[0]); - for (int i = 0; i < list_size; i++) - { + list_size = sizeof(uav_trail_time_list) / sizeof(uav_trail_time_list[0]); + for (int i = 0; i < list_size; i++) { int uav_trail_time = uav_trail_time_list[i]; QAction *uavTrailTime_act = new QAction(QString::number(uav_trail_time) + " sec", uavTrailTimeActGroup); uavTrailTime_act->setCheckable(true); @@ -1509,9 +1573,8 @@ void OPMapGadgetWidget::createActions() uavTrailDistanceActGroup = new QActionGroup(this); connect(uavTrailDistanceActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onUAVTrailDistanceActGroup_triggered(QAction *))); uavTrailDistanceAct.clear(); - list_size = sizeof(uav_trail_distance_list) / sizeof(uav_trail_distance_list[0]); - for (int i = 0; i < list_size; i++) - { + list_size = sizeof(uav_trail_distance_list) / sizeof(uav_trail_distance_list[0]); + for (int i = 0; i < list_size; i++) { int uav_trail_distance = uav_trail_distance_list[i]; QAction *uavTrailDistance_act = new QAction(QString::number(uav_trail_distance) + " meters", uavTrailDistanceActGroup); uavTrailDistance_act->setCheckable(true); @@ -1523,8 +1586,9 @@ void OPMapGadgetWidget::createActions() void OPMapGadgetWidget::onReloadAct_triggered() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->ReloadMap(); } @@ -1537,88 +1601,102 @@ void OPMapGadgetWidget::onRipAct_triggered() void OPMapGadgetWidget::onCopyMouseLatLonToClipAct_triggered() { QClipboard *clipboard = QApplication::clipboard(); - clipboard->setText(QString::number(m_context_menu_lat_lon.Lat(), 'f', 7) + ", " + QString::number(m_context_menu_lat_lon.Lng(), 'f', 7), QClipboard::Clipboard); + + clipboard->setText(QString::number(m_context_menu_lat_lon.Lat(), 'f', 7) + ", " + QString::number(m_context_menu_lat_lon.Lng(), 'f', 7), QClipboard::Clipboard); } void OPMapGadgetWidget::onCopyMouseLatToClipAct_triggered() { QClipboard *clipboard = QApplication::clipboard(); - clipboard->setText(QString::number(m_context_menu_lat_lon.Lat(), 'f', 7), QClipboard::Clipboard); + + clipboard->setText(QString::number(m_context_menu_lat_lon.Lat(), 'f', 7), QClipboard::Clipboard); } void OPMapGadgetWidget::onCopyMouseLonToClipAct_triggered() { QClipboard *clipboard = QApplication::clipboard(); - clipboard->setText(QString::number(m_context_menu_lat_lon.Lng(), 'f', 7), QClipboard::Clipboard); + + clipboard->setText(QString::number(m_context_menu_lat_lon.Lng(), 'f', 7), QClipboard::Clipboard); } void OPMapGadgetWidget::onShowCompassAct_toggled(bool show) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->SetShowCompass(show); } void OPMapGadgetWidget::onShowDiagnostics_toggled(bool show) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->SetShowDiagnostics(show); } void OPMapGadgetWidget::onShowUAVInfo_toggled(bool show) { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } m_map->UAV->SetShowUAVInfo(show); } void OPMapGadgetWidget::onShowHomeAct_toggled(bool show) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->Home->setVisible(show); } void OPMapGadgetWidget::onShowUAVAct_toggled(bool show) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->UAV->setVisible(show); - if(m_map->GPS) + if (m_map->GPS) { m_map->GPS->setVisible(show); + } } void OPMapGadgetWidget::onShowTrailAct_toggled(bool show) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->UAV->SetShowTrail(show); - if(m_map->GPS) + if (m_map->GPS) { m_map->GPS->SetShowTrail(show); + } } void OPMapGadgetWidget::onShowTrailLineAct_toggled(bool show) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->UAV->SetShowTrailLine(show); - if(m_map->GPS) + if (m_map->GPS) { m_map->GPS->SetShowTrailLine(show); + } } void OPMapGadgetWidget::onMapModeActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) + if (!m_widget || !m_map || !action) { return; + } opMapModeType mode = (opMapModeType)action->data().toInt(); @@ -1637,101 +1715,113 @@ void OPMapGadgetWidget::onGoZoomOutAct_triggered() void OPMapGadgetWidget::onZoomActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) - return; + if (!m_widget || !m_map || !action) { + return; + } setZoom(action->data().toInt()); } void OPMapGadgetWidget::onMaxUpdateRateActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) - return; + if (!m_widget || !m_map || !action) { + return; + } setMaxUpdateRate(action->data().toInt()); } void OPMapGadgetWidget::onChangeDefaultLocalAndZoom() { - emit defaultLocationAndZoomChanged(m_map->CurrentPosition().Lng(),m_map->CurrentPosition().Lat(),m_map->ZoomTotal()); + emit defaultLocationAndZoomChanged(m_map->CurrentPosition().Lng(), m_map->CurrentPosition().Lat(), m_map->ZoomTotal()); } void OPMapGadgetWidget::onGoMouseClickAct_triggered() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - m_map->SetCurrentPosition(m_map->currentMousePosition()); // center the map onto the mouse position + m_map->SetCurrentPosition(m_map->currentMousePosition()); // center the map onto the mouse position } void OPMapGadgetWidget::onSetHomeAct_triggered() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - float altitude=0; + float altitude = 0; bool ok; - //Get desired HomeLocation altitude from dialog box. - //TODO: Populate box with altitude already in HomeLocation UAVO + // Get desired HomeLocation altitude from dialog box. + // TODO: Populate box with altitude already in HomeLocation UAVO altitude = QInputDialog::getDouble(this, tr("Set home altitude"), - tr("In [m], referenced to WGS84:"), altitude, -100, 100000, 2, &ok); + tr("In [m], referenced to WGS84:"), altitude, -100, 100000, 2, &ok); setHome(m_context_menu_lat_lon, altitude); - setHomeLocationObject(); // update the HomeLocation UAVObject + setHomeLocationObject(); // update the HomeLocation UAVObject } void OPMapGadgetWidget::onGoHomeAct_triggered() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } goHome(); } void OPMapGadgetWidget::onGoUAVAct_triggered() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } double latitude; double longitude; double altitude; - if (getUAVPosition(latitude, longitude, altitude)) // get current UAV position - { - internals::PointLatLng uav_pos = internals::PointLatLng(latitude, longitude); // current UAV position - internals::PointLatLng map_pos = m_map->CurrentPosition(); // current MAP position - if (map_pos != uav_pos) m_map->SetCurrentPosition(uav_pos); // center the map onto the UAV + if (getUAVPosition(latitude, longitude, altitude)) { // get current UAV position + internals::PointLatLng uav_pos = internals::PointLatLng(latitude, longitude); // current UAV position + internals::PointLatLng map_pos = m_map->CurrentPosition(); // current MAP position + if (map_pos != uav_pos) { + m_map->SetCurrentPosition(uav_pos); // center the map onto the UAV + } } } void OPMapGadgetWidget::onFollowUAVpositionAct_toggled(bool checked) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - if (m_widget->toolButtonMapUAV->isChecked() != checked) + if (m_widget->toolButtonMapUAV->isChecked() != checked) { m_widget->toolButtonMapUAV->setChecked(checked); + } setMapFollowingMode(); } void OPMapGadgetWidget::onFollowUAVheadingAct_toggled(bool checked) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - if (m_widget->toolButtonMapUAVheading->isChecked() != checked) + if (m_widget->toolButtonMapUAVheading->isChecked() != checked) { m_widget->toolButtonMapUAVheading->setChecked(checked); + } setMapFollowingMode(); } void OPMapGadgetWidget::onUAVTrailTypeActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) - return; + if (!m_widget || !m_map || !action) { + return; + } int trail_type_idx = action->data().toInt(); @@ -1743,18 +1833,21 @@ void OPMapGadgetWidget::onUAVTrailTypeActGroup_triggered(QAction *action) void OPMapGadgetWidget::onClearUAVtrailAct_triggered() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->UAV->DeleteTrail(); - if(m_map->GPS) + if (m_map->GPS) { m_map->GPS->DeleteTrail(); + } } void OPMapGadgetWidget::onUAVTrailTimeActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) - return; + if (!m_widget || !m_map || !action) { + return; + } int trail_time = (double)action->data().toInt(); @@ -1763,8 +1856,9 @@ void OPMapGadgetWidget::onUAVTrailTimeActGroup_triggered(QAction *action) void OPMapGadgetWidget::onUAVTrailDistanceActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) - return; + if (!m_widget || !m_map || !action) { + return; + } int trail_distance = action->data().toInt(); @@ -1786,33 +1880,38 @@ void OPMapGadgetWidget::onAddWayPointAct_triggeredFromThis() void OPMapGadgetWidget::onAddWayPointAct_triggered(internals::PointLatLng coord) { - if (!m_widget || !m_map) - return; - - if (m_map_mode != Normal_MapMode) + if (!m_widget || !m_map) { return; + } + + if (m_map_mode != Normal_MapMode) { + return; + } mapProxy->createWayPoint(coord); } /** - * Called when the user asks to edit a waypoint from the map - * - * TODO: should open an interface to edit waypoint properties, or - * propagate the signal to a specific WP plugin (tbd). - **/ + * Called when the user asks to edit a waypoint from the map + * + * TODO: should open an interface to edit waypoint properties, or + * propagate the signal to a specific WP plugin (tbd). + **/ void OPMapGadgetWidget::onEditWayPointAct_triggered() { - if (!m_widget || !m_map) - return; - - if (m_map_mode != Normal_MapMode) + if (!m_widget || !m_map) { return; + } - if (!m_mouse_waypoint) + if (m_map_mode != Normal_MapMode) { return; + } + + if (!m_mouse_waypoint) { + return; + } waypoint_edit_dialog->editWaypoint(m_mouse_waypoint); m_mouse_waypoint = NULL; @@ -1820,24 +1919,27 @@ void OPMapGadgetWidget::onEditWayPointAct_triggered() /** - * TODO: unused for v1.0 - */ + * TODO: unused for v1.0 + */ void OPMapGadgetWidget::onLockWayPointAct_triggered() { - if (!m_widget || !m_map || !m_mouse_waypoint) + if (!m_widget || !m_map || !m_mouse_waypoint) { return; + } - if (m_map_mode != Normal_MapMode) + if (m_map_mode != Normal_MapMode) { return; + } bool locked = (m_mouse_waypoint->flags() & QGraphicsItem::ItemIsMovable) == 0; m_mouse_waypoint->setFlag(QGraphicsItem::ItemIsMovable, locked); - if (!locked) + if (!locked) { m_mouse_waypoint->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker2.png")); - else + } else { m_mouse_waypoint->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker1.png")); + } m_mouse_waypoint->update(); m_mouse_waypoint = NULL; @@ -1845,42 +1947,45 @@ void OPMapGadgetWidget::onLockWayPointAct_triggered() void OPMapGadgetWidget::onDeleteWayPointAct_triggered() { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - if (m_map_mode != Normal_MapMode) + if (m_map_mode != Normal_MapMode) { return; + } - if (!m_mouse_waypoint) + if (!m_mouse_waypoint) { return; + } mapProxy->deleteWayPoint(m_mouse_waypoint->Number()); } void OPMapGadgetWidget::onClearWayPointsAct_triggered() { - - //First, ask to ensure this is what the user wants to do + // First, ask to ensure this is what the user wants to do QMessageBox msgBox; + msgBox.setText(tr("Are you sure you want to clear waypoints?")); msgBox.setInformativeText(tr("All associated data will be lost.")); msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No); int ret = msgBox.exec(); - if (ret == QMessageBox::No) - { + if (ret == QMessageBox::No) { return; } - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - if (m_map_mode != Normal_MapMode) + if (m_map_mode != Normal_MapMode) { return; + } mapProxy->deleteAll(); - - } +} void OPMapGadgetWidget::onHomeMagicWaypointAct_triggered() @@ -1891,22 +1996,24 @@ void OPMapGadgetWidget::onHomeMagicWaypointAct_triggered() void OPMapGadgetWidget::onShowSafeAreaAct_toggled(bool show) { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - m_map->Home->SetShowSafeArea(show); // show the safe area + m_map->Home->SetShowSafeArea(show); // show the safe area m_map->Home->SetToggleRefresh(true); m_map->Home->RefreshPos(); } void OPMapGadgetWidget::onSafeAreaActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) + if (!m_widget || !m_map || !action) { return; + } int radius = action->data().toInt(); - m_map->Home->SetSafeArea(radius); // set the radius (meters) + m_map->Home->SetSafeArea(radius); // set the radius (meters) m_map->Home->RefreshPos(); // move the magic waypoint if need be to keep it within the safe area around the home position @@ -1914,15 +2021,17 @@ void OPMapGadgetWidget::onSafeAreaActGroup_triggered(QAction *action) } /** -* move the magic waypoint to the home position -**/ + * move the magic waypoint to the home position + **/ void OPMapGadgetWidget::homeMagicWaypoint() { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - if (m_map_mode != MagicWaypoint_MapMode) + if (m_map_mode != MagicWaypoint_MapMode) { return; + } magicWayPoint->SetCoord(m_home_position.coord); } @@ -1932,11 +2041,13 @@ void OPMapGadgetWidget::homeMagicWaypoint() void OPMapGadgetWidget::moveToMagicWaypointPosition() { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - if (m_map_mode != MagicWaypoint_MapMode) + if (m_map_mode != MagicWaypoint_MapMode) { return; + } } // ************************************************************************************* @@ -1955,9 +2066,9 @@ void OPMapGadgetWidget::showMagicWaypointControls() m_widget->toolButtonHomeWaypoint->setVisible(true); #if defined(allow_manual_home_location_move) - m_widget->toolButtonMoveToWP->setVisible(true); + m_widget->toolButtonMoveToWP->setVisible(true); #else - m_widget->toolButtonMoveToWP->setVisible(false); + m_widget->toolButtonMoveToWP->setVisible(false); #endif } @@ -1966,7 +2077,6 @@ void OPMapGadgetWidget::showMagicWaypointControls() void OPMapGadgetWidget::keepMagicWaypointWithInSafeArea() { - // calcute the bearing and distance from the home position to the magic waypoint double dist = distance(m_home_position.coord, magicWayPoint->Coord()); double bear = bearing(m_home_position.coord, magicWayPoint->Coord()); @@ -1974,14 +2084,16 @@ void OPMapGadgetWidget::keepMagicWaypointWithInSafeArea() // get the maximum safe distance - in kilometers double boundry_dist = (double)m_map->Home->SafeArea() / 1000; - if (dist > boundry_dist) dist = boundry_dist; + if (dist > boundry_dist) { + dist = boundry_dist; + } // move the magic waypoint; - if (m_map_mode == MagicWaypoint_MapMode) - { // move the on-screen waypoint - if (magicWayPoint) + if (m_map_mode == MagicWaypoint_MapMode) { // move the on-screen waypoint + if (magicWayPoint) { magicWayPoint->SetCoord(destPoint(m_home_position.coord, bear, dist)); + } } } @@ -1996,7 +2108,7 @@ double OPMapGadgetWidget::distance(internals::PointLatLng from, internals::Point double lat2 = to.Lat() * deg_to_rad; double lon2 = to.Lng() * deg_to_rad; - return (acos(sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(lon2 - lon1)) * earth_mean_radius); + return acos(sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(lon2 - lon1)) * earth_mean_radius; // *********************** } @@ -2012,16 +2124,20 @@ double OPMapGadgetWidget::bearing(internals::PointLatLng from, internals::PointL double lat2 = to.Lat() * deg_to_rad; double lon2 = to.Lng() * deg_to_rad; -// double delta_lat = lat2 - lat1; +// double delta_lat = lat2 - lat1; double delta_lon = lon2 - lon1; - double y = sin(delta_lon) * cos(lat2); - double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(delta_lon); + double y = sin(delta_lon) * cos(lat2); + double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(delta_lon); double bear = atan2(y, x) * rad_to_deg; bear += 360; - while (bear < 0) bear += 360; - while (bear >= 360) bear -= 360; + while (bear < 0) { + bear += 360; + } + while (bear >= 360) { + bear -= 360; + } return bear; } @@ -2036,7 +2152,7 @@ internals::PointLatLng OPMapGadgetWidget::destPoint(internals::PointLatLng sourc bear *= deg_to_rad; - double ad = dist / earth_mean_radius; + double ad = dist / earth_mean_radius; double lat2 = asin(sin(lat1) * cos(ad) + cos(lat1) * sin(ad) * cos(bear)); double lon2 = lon1 + atan2(sin(bear) * sin(ad) * cos(lat1), cos(ad) - sin(lat1) * sin(lat2)); @@ -2072,52 +2188,68 @@ bool OPMapGadgetWidget::getUAVPosition(double &latitude, double &longitude, doub Utils::CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA); - latitude = LLA[0]; + latitude = LLA[0]; longitude = LLA[1]; - altitude = LLA[2]; + altitude = LLA[2]; - if (latitude != latitude) latitude = 0; // nan detection - else if (latitude > 90) latitude = 90; - else if (latitude < -90) latitude = -90; + if (latitude != latitude) { + latitude = 0; // nan detection + } else if (latitude > 90) { + latitude = 90; + } else if (latitude < -90) { + latitude = -90; + } - if (longitude != longitude) longitude = 0; // nan detection - else if (longitude > 180) longitude = 180; - else if (longitude < -180) longitude = -180; - - if (altitude != altitude) altitude = 0; // nan detection + if (longitude != longitude) { + longitude = 0; // nan detection + } else if (longitude > 180) { + longitude = 180; + } else if (longitude < -180) { + longitude = -180; + } + if (altitude != altitude) { + altitude = 0; // nan detection + } return true; } double OPMapGadgetWidget::getUAV_Yaw() { - if (!obm) - return 0; + if (!obm) { + return 0; + } - UAVObject *obj = dynamic_cast(obm->getObject(QString("AttitudeActual"))); - double yaw = obj->getField(QString("Yaw"))->getDouble(); + UAVObject *obj = dynamic_cast(obm->getObject(QString("AttitudeActual"))); + double yaw = obj->getField(QString("Yaw"))->getDouble(); - if (yaw != yaw) yaw = 0; // nan detection + if (yaw != yaw) { + yaw = 0; // nan detection + } + while (yaw < 0) { + yaw += 360; + } + while (yaw >= 360) { + yaw -= 360; + } - while (yaw < 0) yaw += 360; - while (yaw >= 360) yaw -= 360; - - return yaw; + return yaw; } bool OPMapGadgetWidget::getGPSPosition(double &latitude, double &longitude, double &altitude) { - double LLA[3]; + double LLA[3]; - if (!obum) - return false; + if (!obum) { + return false; + } - if (obum->getGPSPosition(LLA) < 0) - return false; // error - - latitude = LLA[0]; - longitude = LLA[1]; - altitude = LLA[2]; + if (obum->getGPSPosition(LLA) < 0) { + return false; // error + } + latitude = LLA[0]; + longitude = LLA[1]; + altitude = LLA[2]; return true; } @@ -2126,25 +2258,20 @@ bool OPMapGadgetWidget::getGPSPosition(double &latitude, double &longitude, doub void OPMapGadgetWidget::setMapFollowingMode() { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - if (!followUAVpositionAct->isChecked()) - { + if (!followUAVpositionAct->isChecked()) { m_map->UAV->SetMapFollowType(UAVMapFollowType::None); - m_map->SetRotate(0); // reset map rotation to 0deg - } - else - if (!followUAVheadingAct->isChecked()) - { + m_map->SetRotate(0); // reset map rotation to 0deg + } else if (!followUAVheadingAct->isChecked()) { m_map->UAV->SetMapFollowType(UAVMapFollowType::CenterMap); - m_map->SetRotate(0); // reset map rotation to 0deg - } - else - { - m_map->UAV->SetMapFollowType(UAVMapFollowType::CenterMap); // the map library won't let you reset the uav rotation if it's already in rotate mode + m_map->SetRotate(0); // reset map rotation to 0deg + } else { + m_map->UAV->SetMapFollowType(UAVMapFollowType::CenterMap); // the map library won't let you reset the uav rotation if it's already in rotate mode - m_map->UAV->SetUAVHeading(0); // reset the UAV heading to 0deg + m_map->UAV->SetUAVHeading(0); // reset the UAV heading to 0deg m_map->UAV->SetMapFollowType(UAVMapFollowType::CenterAndRotateMap); } } @@ -2154,11 +2281,12 @@ void OPMapGadgetWidget::setMapFollowingMode() bool OPMapGadgetWidget::setHomeLocationObject() { - if (!obum) - return false; + if (!obum) { + return false; + } - double LLA[3] = {m_home_position.coord.Lat(), m_home_position.coord.Lng(), m_home_position.altitude}; - return (obum->setHomeLocation(LLA, true) >= 0); + double LLA[3] = { m_home_position.coord.Lat(), m_home_position.coord.Lng(), m_home_position.altitude }; + return obum->setHomeLocation(LLA, true) >= 0; } // ************************************************************************************* @@ -2172,32 +2300,31 @@ void OPMapGadgetWidget::on_tbFind_clicked() { QPalette pal = m_widget->leFind->palette(); - int result=m_map->SetCurrentPositionByKeywords(m_widget->leFind->text()); - if(result==core::GeoCoderStatusCode::G_GEO_SUCCESS) - { - pal.setColor( m_widget->leFind->backgroundRole(), Qt::green); + int result = m_map->SetCurrentPositionByKeywords(m_widget->leFind->text()); + + if (result == core::GeoCoderStatusCode::G_GEO_SUCCESS) { + pal.setColor(m_widget->leFind->backgroundRole(), Qt::green); m_widget->leFind->setPalette(pal); m_map->SetZoom(12); - } - else - { - pal.setColor( m_widget->leFind->backgroundRole(), Qt::red); + } else { + pal.setColor(m_widget->leFind->backgroundRole(), Qt::red); m_widget->leFind->setPalette(pal); } } void OPMapGadgetWidget::onHomeDoubleClick(HomeItem *) { - new homeEditor(m_map->Home,this); + new homeEditor(m_map->Home, this); } void OPMapGadgetWidget::onOverlayOpacityActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) + if (!m_widget || !m_map || !action) { return; + } - m_map->setOverlayOpacity(action->data().toReal()/100); - emit overlayOpacityChanged(action->data().toReal()/100); + m_map->setOverlayOpacity(action->data().toReal() / 100); + emit overlayOpacityChanged(action->data().toReal() / 100); } void OPMapGadgetWidget::on_leFind_returnPressed() diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h index 78a2313fb..7de84fce2 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -64,17 +64,15 @@ // ****************************************************** -namespace Ui -{ - class OPMap_Widget; +namespace Ui { +class OPMap_Widget; } using namespace mapcontrol; // ****************************************************** -typedef struct t_home -{ +typedef struct t_home { internals::PointLatLng coord; double altitude; bool locked; @@ -83,23 +81,22 @@ typedef struct t_home // ****************************************************** enum opMapModeType { Normal_MapMode = 0, - MagicWaypoint_MapMode = 1}; + MagicWaypoint_MapMode = 1 }; // ****************************************************** -class OPMapGadgetWidget : public QWidget -{ +class OPMapGadgetWidget : public QWidget { Q_OBJECT public: OPMapGadgetWidget(QWidget *parent = 0); - ~OPMapGadgetWidget(); + ~OPMapGadgetWidget(); /** - * @brief public functions - * - * @param - */ + * @brief public functions + * + * @param + */ void setHome(QPointF pos); void setHome(internals::PointLatLng pos_lat_lon, double altitude); void goHome(); @@ -112,13 +109,13 @@ public: void setUseMemoryCache(bool useMemoryCache); void setCacheLocation(QString cacheLocation); void setMapMode(opMapModeType mode); - void SetUavPic(QString UAVPic); + void SetUavPic(QString UAVPic); void setMaxUpdateRate(int update_rate); void setHomePosition(QPointF pos); void setOverlayOpacity(qreal value); bool getGPSPosition(double &latitude, double &longitude, double &altitude); signals: - void defaultLocationAndZoomChanged(double lng,double lat,double zoom); + void defaultLocationAndZoomChanged(double lng, double lat, double zoom); void overlayOpacityChanged(qreal); public slots: @@ -141,10 +138,10 @@ private slots: void zoomOut(); /** - * @brief signals received from the various map plug-in widget user controls - * - * Some are currently disabled for the v1.0 plugin version. - */ + * @brief signals received from the various map plug-in widget user controls + * + * Some are currently disabled for the v1.0 plugin version. + */ void on_toolButtonZoomM_clicked(); void on_toolButtonZoomP_clicked(); void on_toolButtonMapHome_clicked(); @@ -157,17 +154,17 @@ private slots: void on_toolButtonMoveToWP_clicked(); /** - * @brief signals received from the map object - */ - void zoomChanged(double zoomt,double zoom, double zoomd); + * @brief signals received from the map object + */ + void zoomChanged(double zoomt, double zoom, double zoomd); void OnCurrentPositionChanged(internals::PointLatLng point); void OnTileLoadComplete(); void OnTileLoadStart(); void OnTilesStillToLoad(int number); /** - * @brief mouse right click context menu signals - */ + * @brief mouse right click context menu signals + */ void onReloadAct_triggered(); void onRipAct_triggered(); void onCopyMouseLatLonToClipAct_triggered(); @@ -207,43 +204,43 @@ private slots: void onClearUAVtrailAct_triggered(); void onUAVTrailTimeActGroup_triggered(QAction *action); void onUAVTrailDistanceActGroup_triggered(QAction *action); - void onMaxUpdateRateActGroup_triggered(QAction *action); + void onMaxUpdateRateActGroup_triggered(QAction *action); void onChangeDefaultLocalAndZoom(); void on_tbFind_clicked(); - void onHomeDoubleClick(HomeItem*); + void onHomeDoubleClick(HomeItem *); void onOverlayOpacityActGroup_triggered(QAction *action); void on_leFind_returnPressed(); private: - int m_min_zoom; - int m_max_zoom; - double m_heading; // uav heading - internals::PointLatLng m_mouse_lat_lon; - internals::PointLatLng m_context_menu_lat_lon; - int m_prev_tile_number; + int m_min_zoom; + int m_max_zoom; + double m_heading; // uav heading + internals::PointLatLng m_mouse_lat_lon; + internals::PointLatLng m_context_menu_lat_lon; + int m_prev_tile_number; opMapModeType m_map_mode; - int m_maxUpdateRate; - t_home m_home_position; - QStringList findPlaceWordList; + int m_maxUpdateRate; + t_home m_home_position; + QStringList findPlaceWordList; QCompleter *findPlaceCompleter; QTimer *m_updateTimer; QTimer *m_statusUpdateTimer; Ui::OPMap_Widget *m_widget; mapcontrol::OPMapWidget *m_map; - ExtensionSystem::PluginManager *pm; - UAVObjectManager *obm; - UAVObjectUtilManager *obum; + ExtensionSystem::PluginManager *pm; + UAVObjectManager *obm; + UAVObjectUtilManager *obum; QPointer waypoint_edit_dialog; QStandardItemModel wayPoint_treeView_model; mapcontrol::WayPointItem *m_mouse_waypoint; QPointer UAVProxy; QMutex m_map_mutex; - bool m_telemetry_connected; + bool m_telemetry_connected; QAction *closeAct1; QAction *closeAct2; QAction *reloadAct; QAction *ripAct; - QAction *copyMouseLatLonToClipAct; + QAction *copyMouseLatLonToClipAct; QAction *copyMouseLatToClipAct; QAction *copyMouseLonToClipAct; QAction *showCompassAct; @@ -293,11 +290,11 @@ private: QList zoomAct; QList overlayOpacityAct; - QActionGroup *maxUpdateRateActGroup; - QList maxUpdateRateAct; + QActionGroup *maxUpdateRateActGroup; + QList maxUpdateRateAct; void createActions(); - void homeMagicWaypoint(); + void homeMagicWaypoint(); void moveToMagicWaypointPosition(); void hideMagicWaypointControls(); void showMagicWaypointControls(); @@ -307,15 +304,15 @@ private: double bearing(internals::PointLatLng from, internals::PointLatLng to); internals::PointLatLng destPoint(internals::PointLatLng source, double bear, double dist); - bool getUAVPosition(double &latitude, double &longitude, double &altitude); + bool getUAVPosition(double &latitude, double &longitude, double &altitude); double getUAV_Yaw(); void setMapFollowingMode(); - bool setHomeLocationObject(); + bool setHomeLocationObject(); QMenu contextMenu; internals::PointLatLng lastLatLngMouse; - WayPointItem * magicWayPoint; + WayPointItem *magicWayPoint; QPointer model; QPointer table; diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapplugin.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapplugin.cpp index df155f0ac..38fe1b3a9 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapplugin.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,32 +32,32 @@ OPMapPlugin::OPMapPlugin() { - // Do nothing + // Do nothing } OPMapPlugin::~OPMapPlugin() { - // Do nothing + // Do nothing } -bool OPMapPlugin::initialize(const QStringList& args, QString *errMsg) +bool OPMapPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); + Q_UNUSED(args); + Q_UNUSED(errMsg); - mf = new OPMapGadgetFactory(this); - addAutoReleasedObject(mf); + mf = new OPMapGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void OPMapPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void OPMapPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(OPMapPlugin) diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapplugin.h b/ground/openpilotgcs/src/plugins/opmap/opmapplugin.h index 07c41b533..263e4bcd6 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapplugin.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class OPMapGadgetFactory; -class OPMapPlugin : public ExtensionSystem::IPlugin -{ +class OPMapPlugin : public ExtensionSystem::IPlugin { public: OPMapPlugin(); ~OPMapPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - OPMapGadgetFactory *mf; + OPMapGadgetFactory *mf; }; #endif /* OPMAP_PLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp b/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp index a7c9425a9..80bba9ce6 100644 --- a/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp @@ -39,35 +39,38 @@ PathCompiler::PathCompiler(QObject *parent) : HomeLocation *homeLocation = NULL; /* To catch new waypoint UAVOs */ - connect(getObjectManager(), SIGNAL(newInstance(UAVObject*)), this, SLOT(doNewInstance(UAVObject*))); + connect(getObjectManager(), SIGNAL(newInstance(UAVObject *)), this, SLOT(doNewInstance(UAVObject *))); /* Connect the object updates */ int numWaypoints = getObjectManager()->getNumInstances(Waypoint::OBJID); for (int i = 0; i < numWaypoints; i++) { Waypoint *waypoint = Waypoint::GetInstance(getObjectManager(), i); Q_ASSERT(waypoint); - if(waypoint) - connect(waypoint, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doUpdateFromUAV(UAVObject*))); + if (waypoint) { + connect(waypoint, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doUpdateFromUAV(UAVObject *))); + } } homeLocation = HomeLocation::GetInstance(getObjectManager()); Q_ASSERT(homeLocation); - if(homeLocation) - connect(homeLocation, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doUpdateFromUAV(UAVObject*))); + if (homeLocation) { + connect(homeLocation, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doUpdateFromUAV(UAVObject *))); + } } /** - * Helper method to get the uavobjectamanger - */ -UAVObjectManager * PathCompiler::getObjectManager() + * Helper method to get the uavobjectamanger + */ +UAVObjectManager *PathCompiler::getObjectManager() { ExtensionSystem::PluginManager *pm = NULL; UAVObjectManager *objMngr = NULL; pm = ExtensionSystem::PluginManager::instance(); Q_ASSERT(pm); - if(pm) + if (pm) { objMngr = pm->getObject(); + } Q_ASSERT(objMngr); return objMngr; @@ -96,31 +99,33 @@ int PathCompiler::loadPath(QString filename) } /** - * Called whenever a new object instance is created so we can check - * if it's a waypoint and if so connect to it - * @param [in] obj The point to the object being created - */ -void PathCompiler::doNewInstance(UAVObject* obj) + * Called whenever a new object instance is created so we can check + * if it's a waypoint and if so connect to it + * @param [in] obj The point to the object being created + */ +void PathCompiler::doNewInstance(UAVObject *obj) { Q_ASSERT(obj); - if (!obj) + if (!obj) { return; + } - if (obj->getObjID() == Waypoint::OBJID) - connect(obj, SIGNAL(objectUpdated(UAVObject*)),this,SLOT(doUpdateFromUAV(UAVObject*))); + if (obj->getObjID() == Waypoint::OBJID) { + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doUpdateFromUAV(UAVObject *))); + } } /** - * add a waypoint - * @param waypoint the new waypoint to add - * @param position which position to insert it to, defaults to end - */ + * add a waypoint + * @param waypoint the new waypoint to add + * @param position which position to insert it to, defaults to end + */ void PathCompiler::doAddWaypoint(waypoint newWaypointInternal, int /*position*/) { /* TODO: If a waypoint is inserted not at the end shift them all by one and */ /* add the data there */ - UAVObjectManager *objManager = getObjectManager(); + UAVObjectManager *objManager = getObjectManager(); // Format the data from the map into a UAVO Waypoint::DataFields newWaypoint = InternalToUavo(newWaypointInternal); @@ -129,14 +134,16 @@ void PathCompiler::doAddWaypoint(waypoint newWaypointInternal, int /*position*/) // should add the waypoint immediately after that one int numWaypoints = objManager->getNumInstances(Waypoint::OBJID); int i; + for (i = 0; i < numWaypoints; i++) { Waypoint *waypoint = Waypoint::GetInstance(objManager, i); Q_ASSERT(waypoint); - if(waypoint == NULL) + if (waypoint == NULL) { return; + } Waypoint::DataFields waypointData = waypoint->getData(); - if(waypointData.Action == Waypoint::ACTION_STOP) { + if (waypointData.Action == Waypoint::ACTION_STOP) { waypointData.Action = Waypoint::ACTION_PATHTONEXT; waypoint->setData(waypointData); waypoint->updated(); @@ -152,7 +159,7 @@ void PathCompiler::doAddWaypoint(waypoint newWaypointInternal, int /*position*/) if (waypoint) { // Register a new waypoint instance quint32 newInstId = objManager->getNumInstances(waypoint->getObjID()); - waypoint->initialize(newInstId,waypoint->getMetaObject()); + waypoint->initialize(newInstId, waypoint->getMetaObject()); objManager->registerObject(waypoint); // Set the data in the new object @@ -170,14 +177,16 @@ void PathCompiler::doAddWaypoint(waypoint newWaypointInternal, int /*position*/) } /** - * Update waypoint - */ + * Update waypoint + */ void PathCompiler::doUpdateWaypoints(PathCompiler::waypoint changedWaypoint, int position) { int numWaypoints = getObjectManager()->getNumInstances(Waypoint::OBJID); + Q_ASSERT(position < numWaypoints); - if (position >= numWaypoints) + if (position >= numWaypoints) { return; + } Waypoint *waypointInst = Waypoint::GetInstance(getObjectManager(), position); Q_ASSERT(waypointInst); @@ -185,7 +194,7 @@ void PathCompiler::doUpdateWaypoints(PathCompiler::waypoint changedWaypoint, int // Mirror over the updated position. We don't just use the changedWaypoint // because things like action might need to be preserved Waypoint::DataFields changedWaypointUAVO = InternalToUavo(changedWaypoint); - Waypoint::DataFields oldWaypointUAVO = waypointInst->getData(); + Waypoint::DataFields oldWaypointUAVO = waypointInst->getData(); oldWaypointUAVO.Position[0] = changedWaypointUAVO.Position[0]; oldWaypointUAVO.Position[1] = changedWaypointUAVO.Position[1]; // Don't take the altitude from the map for now @@ -195,9 +204,9 @@ void PathCompiler::doUpdateWaypoints(PathCompiler::waypoint changedWaypoint, int } /** - * Delete a waypoint - * @param index which waypoint to delete - */ + * Delete a waypoint + * @param index which waypoint to delete + */ void PathCompiler::doDelWaypoint(int index) { // This method is awkward because there is no support @@ -207,20 +216,23 @@ void PathCompiler::doDelWaypoint(int index) UAVObjectManager *objManager = getObjectManager(); Waypoint *waypoint = Waypoint::GetInstance(objManager); + Q_ASSERT(waypoint); - if (!waypoint) + if (!waypoint) { return; + } int numWaypoints = objManager->getNumInstances(waypoint->getObjID()); for (int i = index; i < numWaypoints - 1; i++) { Waypoint *waypointDest = Waypoint::GetInstance(objManager, i); Q_ASSERT(waypointDest); - Waypoint *waypointSrc = Waypoint::GetInstance(objManager, i + 1); + Waypoint *waypointSrc = Waypoint::GetInstance(objManager, i + 1); Q_ASSERT(waypointSrc); - if (!waypointDest || !waypointSrc) + if (!waypointDest || !waypointSrc) { return; + } // Copy the data down an index Waypoint::DataFields waypoint = waypointSrc->getData(); @@ -243,14 +255,16 @@ void PathCompiler::doDelWaypoint(int index) } /** - * Delete all the waypoints - */ + * Delete all the waypoints + */ void PathCompiler::doDelAllWaypoints() { Waypoint *waypointObj = Waypoint::GetInstance(getObjectManager(), 0); + Q_ASSERT(waypointObj); - if (waypointObj == NULL) + if (waypointObj == NULL) { return; + } int numWaypoints = getObjectManager()->getNumInstances(waypointObj->getObjID()); for (int i = 0; i < numWaypoints; i++) { @@ -267,30 +281,34 @@ void PathCompiler::doDelAllWaypoints() } /** - * When the UAV waypoints change trigger the pathcompiler to - * get the latest version and then update the visualization - */ + * When the UAV waypoints change trigger the pathcompiler to + * get the latest version and then update the visualization + */ void PathCompiler::doUpdateFromUAV(UAVObject *obj) { UAVObjectManager *objManager = getObjectManager(); - if (!objManager) + + if (!objManager) { return; + } Waypoint *waypointObj = Waypoint::GetInstance(getObjectManager()); Q_ASSERT(waypointObj); - if (waypointObj == NULL) + if (waypointObj == NULL) { return; + } /* Get all the waypoints from the UAVO and create a representation for the visualization */ QList waypoints; waypoints.clear(); int numWaypoints = objManager->getNumInstances(waypointObj->getObjID()); - bool stopped = false; + bool stopped = false; for (int i = 0; i < numWaypoints && !stopped; i++) { Waypoint *waypoint = Waypoint::GetInstance(objManager, i); Q_ASSERT(waypoint); - if(waypoint == NULL) + if (waypoint == NULL) { return; + } Waypoint::DataFields waypointData = waypoint->getData(); waypoints.append(UavoToInternal(waypointData)); @@ -308,10 +326,10 @@ void PathCompiler::doUpdateFromUAV(UAVObject *obj) } /** - * Convert a UAVO waypoint to the local structure - * @param uavo The UAVO data representation - * @return The waypoint structure for visualization - */ + * Convert a UAVO waypoint to the local structure + * @param uavo The UAVO data representation + * @return The waypoint structure for visualization + */ struct PathCompiler::waypoint PathCompiler::UavoToInternal(Waypoint::DataFields uavo) { double homeLLA[3]; @@ -320,30 +338,32 @@ struct PathCompiler::waypoint PathCompiler::UavoToInternal(Waypoint::DataFields waypoint internalWaypoint; HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); + Q_ASSERT(homeLocation); - if (homeLocation == NULL) + if (homeLocation == NULL) { return internalWaypoint; + } HomeLocation::DataFields homeLocationData = homeLocation->getData(); homeLLA[0] = homeLocationData.Latitude / 10e6; homeLLA[1] = homeLocationData.Longitude / 10e6; homeLLA[2] = homeLocationData.Altitude; - NED[0] = uavo.Position[Waypoint::POSITION_NORTH]; - NED[1] = uavo.Position[Waypoint::POSITION_EAST]; - NED[2] = uavo.Position[Waypoint::POSITION_DOWN]; + NED[0] = uavo.Position[Waypoint::POSITION_NORTH]; + NED[1] = uavo.Position[Waypoint::POSITION_EAST]; + NED[2] = uavo.Position[Waypoint::POSITION_DOWN]; Utils::CoordinateConversions().GetLLA(homeLLA, NED, LLA); - internalWaypoint.latitude = LLA[0]; + internalWaypoint.latitude = LLA[0]; internalWaypoint.longitude = LLA[1]; - internalWaypoint.altitude = LLA[2]; + internalWaypoint.altitude = LLA[2]; return internalWaypoint; } /** - * Convert a UAVO waypoint to the local structure - * @param internal The internal structure type - * @returns The waypoint UAVO data structure - */ + * Convert a UAVO waypoint to the local structure + * @param internal The internal structure type + * @returns The waypoint UAVO data structure + */ Waypoint::DataFields PathCompiler::InternalToUavo(waypoint internal) { Waypoint::DataFields uavo; @@ -353,31 +373,32 @@ Waypoint::DataFields PathCompiler::InternalToUavo(waypoint internal) double NED[3]; HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); + Q_ASSERT(homeLocation); - if (homeLocation == NULL) + if (homeLocation == NULL) { return uavo; + } HomeLocation::DataFields homeLocationData = homeLocation->getData(); homeLLA[0] = homeLocationData.Latitude / 10e6; homeLLA[1] = homeLocationData.Longitude / 10e6; homeLLA[2] = homeLocationData.Altitude; // TODO: Give the point a concept of altitude - LLA[0] = internal.latitude; - LLA[1] = internal.longitude; - LLA[2] = internal.altitude; + LLA[0] = internal.latitude; + LLA[1] = internal.longitude; + LLA[2] = internal.altitude; Utils::CoordinateConversions().GetNED(homeLLA, LLA, NED); uavo.Position[Waypoint::POSITION_NORTH] = NED[0]; - uavo.Position[Waypoint::POSITION_EAST] = NED[1]; - uavo.Position[Waypoint::POSITION_DOWN] = NED[2]; + uavo.Position[Waypoint::POSITION_EAST] = NED[1]; + uavo.Position[Waypoint::POSITION_DOWN] = NED[2]; uavo.Action = Waypoint::ACTION_PATHTONEXT; uavo.Velocity[Waypoint::VELOCITY_NORTH] = 5; - uavo.Velocity[Waypoint::VELOCITY_EAST] = 0; - uavo.Velocity[Waypoint::VELOCITY_DOWN] = 0; + uavo.Velocity[Waypoint::VELOCITY_EAST] = 0; + uavo.Velocity[Waypoint::VELOCITY_DOWN] = 0; return uavo; } - diff --git a/ground/openpilotgcs/src/plugins/opmap/pathcompiler.h b/ground/openpilotgcs/src/plugins/opmap/pathcompiler.h index ed721794c..655df9297 100644 --- a/ground/openpilotgcs/src/plugins/opmap/pathcompiler.h +++ b/ground/openpilotgcs/src/plugins/opmap/pathcompiler.h @@ -36,41 +36,41 @@ // factory? static variables? /** - * This class is a two way adapter between a visualization of a path and the - * UAVObject representation on the flight controller. It also can support multiple - * ways of converting a path from what the user clicked to the underlying representation - * to achieve the desired end flight trajectory - * - * So the chain of data for the map lib is: - * FC <-> PathCompiler <-> OPMapGadget <-> OPMapLib - * - * The goal is that PathCompiler be as state free as is possible. Eventually for more - * complicated path compilation this will probably not be achievable. That means it - * should not cache a copy of waypoints locally if that can be avoided (i.e. it should - * refer directly to what is stored on the FC). - * - * For the visualization to have the ability to manipulate the path though it needs to - * be able to map unambiguously from the graphical items to the internal waypoints. It - * must cache a lookup from the graphical item to the index from this tool. - */ -class PathCompiler : public QObject -{ + * This class is a two way adapter between a visualization of a path and the + * UAVObject representation on the flight controller. It also can support multiple + * ways of converting a path from what the user clicked to the underlying representation + * to achieve the desired end flight trajectory + * + * So the chain of data for the map lib is: + * FC <-> PathCompiler <-> OPMapGadget <-> OPMapLib + * + * The goal is that PathCompiler be as state free as is possible. Eventually for more + * complicated path compilation this will probably not be achievable. That means it + * should not cache a copy of waypoints locally if that can be avoided (i.e. it should + * refer directly to what is stored on the FC). + * + * For the visualization to have the ability to manipulate the path though it needs to + * be able to map unambiguously from the graphical items to the internal waypoints. It + * must cache a lookup from the graphical item to the index from this tool. + */ +class PathCompiler : public QObject { Q_OBJECT public: explicit PathCompiler(QObject *parent = 0); - //! This method opens a dialog (if filename is null) and saves the path + // ! This method opens a dialog (if filename is null) and saves the path int savePath(QString filename = NULL); - //! This method opens a dialog (if filename is null) and loads the path + // ! This method opens a dialog (if filename is null) and loads the path int loadPath(QString filename = NULL); - //! Waypoint representation that is exchanged between visualization + // ! Waypoint representation that is exchanged between visualization class waypoint { - public: +public: waypoint() {} - const bool operator==(const waypoint other) { + const bool operator==(const waypoint other) + { return (other.latitude == latitude) && (other.longitude == longitude); } @@ -80,68 +80,68 @@ public: }; private: - //! Helper method to get uavobject manager - UAVObjectManager * getObjectManager(); + // ! Helper method to get uavobject manager + UAVObjectManager *getObjectManager(); - //! Convert a UAVO waypoint to the local structure + // ! Convert a UAVO waypoint to the local structure struct PathCompiler::waypoint UavoToInternal(Waypoint::DataFields); - //! Convert a UAVO waypoint to the local structure + // ! Convert a UAVO waypoint to the local structure Waypoint::DataFields InternalToUavo(waypoint); QList previousWaypoints; signals: /** - * Indicates something changed the waypoints and the map should - * update the display - */ + * Indicates something changed the waypoints and the map should + * update the display + */ void visualizationChanged(QList); public slots: /** - * These are slots that the visualization can call to manipulate the path. - * It is an important design detail that the visualiation _not_ attempt to maintain - * the list of waypoints itself. This starts the slippery of moving the path logic - * into the view. - */ + * These are slots that the visualization can call to manipulate the path. + * It is an important design detail that the visualiation _not_ attempt to maintain + * the list of waypoints itself. This starts the slippery of moving the path logic + * into the view. + */ /** - * Called when new instances are registered - */ - void doNewInstance(UAVObject*); + * Called when new instances are registered + */ + void doNewInstance(UAVObject *); /** - * add a waypoint - * @param waypoint the new waypoint to add - * @param position which position to insert it to, defaults to end - */ + * add a waypoint + * @param waypoint the new waypoint to add + * @param position which position to insert it to, defaults to end + */ void doAddWaypoint(waypoint, int position = -1); /** - * Update waypoint - */ + * Update waypoint + */ void doUpdateWaypoints(PathCompiler::waypoint, int position); /** - * Delete a waypoint - * @param index which waypoint to delete - */ + * Delete a waypoint + * @param index which waypoint to delete + */ void doDelWaypoint(int index); /** - * Delete all the waypoints - */ + * Delete all the waypoints + */ void doDelAllWaypoints(); public slots: /** - * These are slots that the UAV can call to update the path. - */ + * These are slots that the UAV can call to update the path. + */ /** - * When the UAV waypoints change trigger the pathcompiler to - * get the latest version and then update the visualization - */ + * When the UAV waypoints change trigger the pathcompiler to + * get the latest version and then update the visualization + */ void doUpdateFromUAV(UAVObject *); }; diff --git a/ground/openpilotgcs/src/plugins/opmap/pathplanner.cpp b/ground/openpilotgcs/src/plugins/opmap/pathplanner.cpp index aed5eed6e..dd02a6258 100644 --- a/ground/openpilotgcs/src/plugins/opmap/pathplanner.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/pathplanner.cpp @@ -33,7 +33,7 @@ pathPlanner::pathPlanner(QWidget *parent) : QWidget(parent), - ui(new Ui::pathPlannerUI),wid(NULL),myModel(NULL) + ui(new Ui::pathPlannerUI), wid(NULL), myModel(NULL) { ui->setupUi(this); } @@ -41,31 +41,31 @@ pathPlanner::pathPlanner(QWidget *parent) : pathPlanner::~pathPlanner() { delete ui; - if(wid) + if (wid) { delete wid; + } } -void pathPlanner::setModel(flightDataModel *model,QItemSelectionModel *selection) +void pathPlanner::setModel(flightDataModel *model, QItemSelectionModel *selection) { - myModel=model; + myModel = model; ui->tableView->setModel(model); ui->tableView->setSelectionModel(selection); ui->tableView->setSelectionBehavior(QAbstractItemView::SelectRows); ui->tableView->setItemDelegate(new MapDataDelegate(this)); - connect(model,SIGNAL(rowsInserted(const QModelIndex&,int,int)),this,SLOT(rowsInserted(const QModelIndex&,int,int))); - wid=new opmap_edit_waypoint_dialog(NULL,model,selection); + connect(model, SIGNAL(rowsInserted(const QModelIndex &, int, int)), this, SLOT(rowsInserted(const QModelIndex &, int, int))); + wid = new opmap_edit_waypoint_dialog(NULL, model, selection); ui->tableView->resizeColumnsToContents(); } -void pathPlanner::rowsInserted ( const QModelIndex & parent, int start, int end ) +void pathPlanner::rowsInserted(const QModelIndex & parent, int start, int end) { Q_UNUSED(parent); - for(int x=start;xtableView->model()->index(x,flightDataModel::MODE); + for (int x = start; x < end + 1; x++) { + QModelIndex index = ui->tableView->model()->index(x, flightDataModel::MODE); ui->tableView->openPersistentEditor(index); - index=ui->tableView->model()->index(x,flightDataModel::CONDITION); + index = ui->tableView->model()->index(x, flightDataModel::CONDITION); ui->tableView->openPersistentEditor(index); - index=ui->tableView->model()->index(x,flightDataModel::COMMAND); + index = ui->tableView->model()->index(x, flightDataModel::COMMAND); ui->tableView->openPersistentEditor(index); ui->tableView->size().setHeight(10); } @@ -88,24 +88,27 @@ void pathPlanner::on_tbInsert_clicked() void pathPlanner::on_tbReadFromFile_clicked() { - if(!myModel) + if (!myModel) { return; + } QString fileName = QFileDialog::getOpenFileName(this, tr("Open File")); myModel->readFromFile(fileName); } void pathPlanner::on_tbSaveToFile_clicked() { - if(!myModel) + if (!myModel) { return; + } QString fileName = QFileDialog::getSaveFileName(this, tr("Save File")); myModel->writeToFile(fileName); } void pathPlanner::on_tbDetails_clicked() { - if(wid) - wid->show(); + if (wid) { + wid->show(); + } } void pathPlanner::on_tbSendToUAV_clicked() diff --git a/ground/openpilotgcs/src/plugins/opmap/pathplanner.h b/ground/openpilotgcs/src/plugins/opmap/pathplanner.h index 274e0ae67..10ec86785 100644 --- a/ground/openpilotgcs/src/plugins/opmap/pathplanner.h +++ b/ground/openpilotgcs/src/plugins/opmap/pathplanner.h @@ -35,38 +35,37 @@ namespace Ui { class pathPlannerUI; } -class pathPlanner : public QWidget -{ +class pathPlanner : public QWidget { Q_OBJECT - + public: explicit pathPlanner(QWidget *parent = 0); ~pathPlanner(); - - void setModel(flightDataModel *model,QItemSelectionModel *selection); + + void setModel(flightDataModel *model, QItemSelectionModel *selection); private slots: - void rowsInserted ( const QModelIndex & parent, int start, int end ); + void rowsInserted(const QModelIndex & parent, int start, int end); - void on_tbAdd_clicked(); + void on_tbAdd_clicked(); - void on_tbDelete_clicked(); + void on_tbDelete_clicked(); - void on_tbInsert_clicked(); + void on_tbInsert_clicked(); - void on_tbReadFromFile_clicked(); + void on_tbReadFromFile_clicked(); - void on_tbSaveToFile_clicked(); + void on_tbSaveToFile_clicked(); - void on_tbDetails_clicked(); + void on_tbDetails_clicked(); - void on_tbSendToUAV_clicked(); + void on_tbSendToUAV_clicked(); - void on_tbFetchFromUAV_clicked(); + void on_tbFetchFromUAV_clicked(); private: Ui::pathPlannerUI *ui; - opmap_edit_waypoint_dialog * wid; - flightDataModel * myModel; + opmap_edit_waypoint_dialog *wid; + flightDataModel *myModel; signals: void sendPathPlanToUAV(); void receivePathPlanFromUAV(); diff --git a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp index fd44550a6..a6cd1b642 100644 --- a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp @@ -29,31 +29,35 @@ #include #include QWidget *MapDataDelegate::createEditor(QWidget *parent, - const QStyleOptionViewItem & option, - const QModelIndex & index) const + const QStyleOptionViewItem & option, + const QModelIndex & index) const { - int column=index.column(); - QComboBox * box; - switch(column) - { + int column = index.column(); + QComboBox *box; + + switch (column) { case flightDataModel::MODE: - box=new QComboBox(parent); - MapDataDelegate::loadComboBox(box,flightDataModel::MODE); + box = new QComboBox(parent); + MapDataDelegate::loadComboBox(box, flightDataModel::MODE); return box; + break; case flightDataModel::CONDITION: - box=new QComboBox(parent); - MapDataDelegate::loadComboBox(box,flightDataModel::CONDITION); + box = new QComboBox(parent); + MapDataDelegate::loadComboBox(box, flightDataModel::CONDITION); return box; + break; case flightDataModel::COMMAND: - box=new QComboBox(parent); - MapDataDelegate::loadComboBox(box,flightDataModel::COMMAND); + box = new QComboBox(parent); + MapDataDelegate::loadComboBox(box, flightDataModel::COMMAND); return box; + break; default: - return QItemDelegate::createEditor(parent,option,index); + return QItemDelegate::createEditor(parent, option, index); + break; } @@ -62,84 +66,84 @@ QWidget *MapDataDelegate::createEditor(QWidget *parent, } void MapDataDelegate::setEditorData(QWidget *editor, - const QModelIndex &index) const + const QModelIndex &index) const { - if(!index.isValid()) + if (!index.isValid()) { return; - QString className=editor->metaObject()->className(); + } + QString className = editor->metaObject()->className(); if (className.contains("QComboBox")) { int value = index.model()->data(index, Qt::EditRole).toInt(); - QComboBox *comboBox = static_cast(editor); - int x=comboBox->findData(value); - qDebug()<<"VALUE="<(editor); + int x = comboBox->findData(value); + qDebug() << "VALUE=" << x; comboBox->setCurrentIndex(x); - } - else + } else { QItemDelegate::setEditorData(editor, index); + } } void MapDataDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, - const QModelIndex &index) const + const QModelIndex &index) const { - QString className=editor->metaObject()->className(); + QString className = editor->metaObject()->className(); + if (className.contains("QComboBox")) { - QComboBox *comboBox = static_cast(editor); + QComboBox *comboBox = static_cast(editor); int value = comboBox->itemData(comboBox->currentIndex()).toInt(); model->setData(index, value, Qt::EditRole); + } else { + QItemDelegate::setModelData(editor, model, index); } - else - QItemDelegate::setModelData(editor,model,index); } void MapDataDelegate::updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &/* index */) const + const QStyleOptionViewItem &option, const QModelIndex & /* index */) const { editor->setGeometry(option.rect); } void MapDataDelegate::loadComboBox(QComboBox *combo, flightDataModel::pathPlanDataEnum type) { - switch(type) - { + switch (type) { case flightDataModel::MODE: - combo->addItem("Fly Direct",MODE_FLYENDPOINT); - combo->addItem("Fly Vector",MODE_FLYVECTOR); - combo->addItem("Fly Circle Right",MODE_FLYCIRCLERIGHT); - combo->addItem("Fly Circle Left",MODE_FLYCIRCLELEFT); + combo->addItem("Fly Direct", MODE_FLYENDPOINT); + combo->addItem("Fly Vector", MODE_FLYVECTOR); + combo->addItem("Fly Circle Right", MODE_FLYCIRCLERIGHT); + combo->addItem("Fly Circle Left", MODE_FLYCIRCLELEFT); - combo->addItem("Drive Direct",MODE_DRIVEENDPOINT); - combo->addItem("Drive Vector",MODE_DRIVEVECTOR); - combo->addItem("Drive Circle Right",MODE_DRIVECIRCLELEFT); - combo->addItem("Drive Circle Left",MODE_DRIVECIRCLERIGHT); + combo->addItem("Drive Direct", MODE_DRIVEENDPOINT); + combo->addItem("Drive Vector", MODE_DRIVEVECTOR); + combo->addItem("Drive Circle Right", MODE_DRIVECIRCLELEFT); + combo->addItem("Drive Circle Left", MODE_DRIVECIRCLERIGHT); - combo->addItem("Fixed Attitude",MODE_FIXEDATTITUDE); - combo->addItem("Set Accessory",MODE_SETACCESSORY); - combo->addItem("Disarm Alarm",MODE_DISARMALARM); + combo->addItem("Fixed Attitude", MODE_FIXEDATTITUDE); + combo->addItem("Set Accessory", MODE_SETACCESSORY); + combo->addItem("Disarm Alarm", MODE_DISARMALARM); break; case flightDataModel::CONDITION: - combo->addItem("None",ENDCONDITION_NONE); - combo->addItem("Timeout",ENDCONDITION_TIMEOUT); - combo->addItem("Distance to tgt",ENDCONDITION_DISTANCETOTARGET); - combo->addItem("Leg remaining",ENDCONDITION_LEGREMAINING); - combo->addItem("Below Error",ENDCONDITION_BELOWERROR); - combo->addItem("Above Altitude",ENDCONDITION_ABOVEALTITUDE); - combo->addItem("Above Speed",ENDCONDITION_ABOVESPEED); - combo->addItem("Pointing towards next",ENDCONDITION_POINTINGTOWARDSNEXT); - combo->addItem("Python script",ENDCONDITION_PYTHONSCRIPT); - combo->addItem("Immediate",ENDCONDITION_IMMEDIATE); + combo->addItem("None", ENDCONDITION_NONE); + combo->addItem("Timeout", ENDCONDITION_TIMEOUT); + combo->addItem("Distance to tgt", ENDCONDITION_DISTANCETOTARGET); + combo->addItem("Leg remaining", ENDCONDITION_LEGREMAINING); + combo->addItem("Below Error", ENDCONDITION_BELOWERROR); + combo->addItem("Above Altitude", ENDCONDITION_ABOVEALTITUDE); + combo->addItem("Above Speed", ENDCONDITION_ABOVESPEED); + combo->addItem("Pointing towards next", ENDCONDITION_POINTINGTOWARDSNEXT); + combo->addItem("Python script", ENDCONDITION_PYTHONSCRIPT); + combo->addItem("Immediate", ENDCONDITION_IMMEDIATE); break; case flightDataModel::COMMAND: - combo->addItem("On conditon next wp",COMMAND_ONCONDITIONNEXTWAYPOINT); - combo->addItem("On NOT conditon next wp",COMMAND_ONNOTCONDITIONNEXTWAYPOINT); - combo->addItem("On conditon jump wp",COMMAND_ONCONDITIONJUMPWAYPOINT); - combo->addItem("On NOT conditon jump wp",COMMAND_ONNOTCONDITIONJUMPWAYPOINT); - combo->addItem("On conditon jump wp else next wp",COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT); + combo->addItem("On conditon next wp", COMMAND_ONCONDITIONNEXTWAYPOINT); + combo->addItem("On NOT conditon next wp", COMMAND_ONNOTCONDITIONNEXTWAYPOINT); + combo->addItem("On conditon jump wp", COMMAND_ONCONDITIONJUMPWAYPOINT); + combo->addItem("On NOT conditon jump wp", COMMAND_ONNOTCONDITIONJUMPWAYPOINT); + combo->addItem("On conditon jump wp else next wp", COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT); break; default: break; } } -MapDataDelegate::MapDataDelegate(QObject *parent):QItemDelegate(parent) -{ -} +MapDataDelegate::MapDataDelegate(QObject *parent) : QItemDelegate(parent) +{} diff --git a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h index 53aac4887..2b824c979 100644 --- a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h +++ b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h @@ -32,34 +32,33 @@ #include "flightdatamodel.h" -class MapDataDelegate : public QItemDelegate - { - Q_OBJECT +class MapDataDelegate : public QItemDelegate { + Q_OBJECT - public: - typedef enum { MODE_FLYENDPOINT=0, MODE_FLYVECTOR=1, MODE_FLYCIRCLERIGHT=2, MODE_FLYCIRCLELEFT=3, - MODE_DRIVEENDPOINT=4, MODE_DRIVEVECTOR=5, MODE_DRIVECIRCLELEFT=6, MODE_DRIVECIRCLERIGHT=7, - MODE_FIXEDATTITUDE=8, MODE_SETACCESSORY=9, MODE_DISARMALARM=10 } ModeOptions; - typedef enum { ENDCONDITION_NONE=0, ENDCONDITION_TIMEOUT=1, ENDCONDITION_DISTANCETOTARGET=2, - ENDCONDITION_LEGREMAINING=3, ENDCONDITION_BELOWERROR=4, ENDCONDITION_ABOVEALTITUDE=5, - ENDCONDITION_ABOVESPEED=6, ENDCONDITION_POINTINGTOWARDSNEXT=7, ENDCONDITION_PYTHONSCRIPT=8, - ENDCONDITION_IMMEDIATE=9 } EndConditionOptions; - typedef enum { COMMAND_ONCONDITIONNEXTWAYPOINT=0, COMMAND_ONNOTCONDITIONNEXTWAYPOINT=1, - COMMAND_ONCONDITIONJUMPWAYPOINT=2, COMMAND_ONNOTCONDITIONJUMPWAYPOINT=3, - COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT=4 } CommandOptions; +public: + typedef enum { MODE_FLYENDPOINT = 0, MODE_FLYVECTOR = 1, MODE_FLYCIRCLERIGHT = 2, MODE_FLYCIRCLELEFT = 3, + MODE_DRIVEENDPOINT = 4, MODE_DRIVEVECTOR = 5, MODE_DRIVECIRCLELEFT = 6, MODE_DRIVECIRCLERIGHT = 7, + MODE_FIXEDATTITUDE = 8, MODE_SETACCESSORY = 9, MODE_DISARMALARM = 10 } ModeOptions; + typedef enum { ENDCONDITION_NONE = 0, ENDCONDITION_TIMEOUT = 1, ENDCONDITION_DISTANCETOTARGET = 2, + ENDCONDITION_LEGREMAINING = 3, ENDCONDITION_BELOWERROR = 4, ENDCONDITION_ABOVEALTITUDE = 5, + ENDCONDITION_ABOVESPEED = 6, ENDCONDITION_POINTINGTOWARDSNEXT = 7, ENDCONDITION_PYTHONSCRIPT = 8, + ENDCONDITION_IMMEDIATE = 9 } EndConditionOptions; + typedef enum { COMMAND_ONCONDITIONNEXTWAYPOINT = 0, COMMAND_ONNOTCONDITIONNEXTWAYPOINT = 1, + COMMAND_ONCONDITIONJUMPWAYPOINT = 2, COMMAND_ONNOTCONDITIONJUMPWAYPOINT = 3, + COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT = 4 } CommandOptions; - MapDataDelegate(QObject *parent = 0); + MapDataDelegate(QObject *parent = 0); - QWidget *createEditor(QWidget *parent, const QStyleOptionViewItem &option, - const QModelIndex &index) const; + QWidget *createEditor(QWidget *parent, const QStyleOptionViewItem &option, + const QModelIndex &index) const; - void setEditorData(QWidget *editor, const QModelIndex &index) const; - void setModelData(QWidget *editor, QAbstractItemModel *model, - const QModelIndex &index) const; + void setEditorData(QWidget *editor, const QModelIndex &index) const; + void setModelData(QWidget *editor, QAbstractItemModel *model, + const QModelIndex &index) const; - void updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &index) const; - static void loadComboBox(QComboBox * combo,flightDataModel::pathPlanDataEnum type); - }; + void updateEditorGeometry(QWidget *editor, + const QStyleOptionViewItem &option, const QModelIndex &index) const; + static void loadComboBox(QComboBox *combo, flightDataModel::pathPlanDataEnum type); +}; #endif // WIDGETDELEGATES_H diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.cpp index fbfb0d613..ccff62dd8 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.cpp @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgearthviewgadget.cpp -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgearthviewgadget.cpp + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -28,10 +28,9 @@ #include "osgearthviewgadgetconfiguration.h" OsgEarthviewGadget::OsgEarthviewGadget(QString classId, OsgEarthviewWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} OsgEarthviewGadget::~OsgEarthviewGadget() { @@ -39,12 +38,12 @@ OsgEarthviewGadget::~OsgEarthviewGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void OsgEarthviewGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void OsgEarthviewGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - OsgEarthviewGadgetConfiguration *m = qobject_cast(config); + OsgEarthviewGadgetConfiguration *m = qobject_cast(config); } diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.h b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.h index d5b8234c6..a02925c86 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.h +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.h @@ -1,13 +1,13 @@ /******************************************************************************** -* -* @file osgearthviewgadget.h -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * + * @file osgearthviewgadget.h + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -37,15 +37,17 @@ class OsgEarthviewWidget; using namespace Core; -class OsgEarthviewGadget : public Core::IUAVGadget -{ +class OsgEarthviewGadget : public Core::IUAVGadget { Q_OBJECT public: OsgEarthviewGadget(QString classId, OsgEarthviewWidget *widget, QWidget *parent = 0); ~OsgEarthviewGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: OsgEarthviewWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.cpp index 50126677e..5a3955ee2 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.cpp @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgearthviewgadgetconfiguration.cpp -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgearthviewgadgetconfiguration.cpp + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -30,13 +30,12 @@ * Loads a saved configuration or defaults if non exist. * */ -OsgEarthviewGadgetConfiguration::OsgEarthviewGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +OsgEarthviewGadgetConfiguration::OsgEarthviewGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent) { Q_UNUSED(qSettings); - //if a saved configuration exists load it - if(qSettings != 0) { - } + // if a saved configuration exists load it + if (qSettings != 0) {} } /** @@ -46,6 +45,7 @@ OsgEarthviewGadgetConfiguration::OsgEarthviewGadgetConfiguration(QString classId IUAVGadgetConfiguration *OsgEarthviewGadgetConfiguration::clone() { OsgEarthviewGadgetConfiguration *m = new OsgEarthviewGadgetConfiguration(this->classId()); + return m; } @@ -53,5 +53,4 @@ IUAVGadgetConfiguration *OsgEarthviewGadgetConfiguration::clone() * Saves a configuration. * */ -void OsgEarthviewGadgetConfiguration::saveConfig(QSettings* qSettings) const { -} +void OsgEarthviewGadgetConfiguration::saveConfig(QSettings *qSettings) const {} diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.h index 9ac03c3fa..02d2560d6 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.h @@ -1,13 +1,13 @@ /******************************************************************************** -* -* @file osgearthviewgadgetconfiguration.h -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * + * @file osgearthviewgadgetconfiguration.h + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -31,13 +31,12 @@ using namespace Core; -class OsgEarthviewGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT +class OsgEarthviewGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit OsgEarthviewGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit OsgEarthviewGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.cpp index 245455ca0..d01a2d5ab 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.cpp @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgearthviewgadgetfactory.cpp -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgearthviewgadgetfactory.cpp + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -30,29 +30,27 @@ #include OsgEarthviewGadgetFactory::OsgEarthviewGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("OsgEarthviewGadget"), - tr("Osg Earth View"), - parent) -{ -} + IUAVGadgetFactory(QString("OsgEarthviewGadget"), + tr("Osg Earth View"), + parent) +{} OsgEarthviewGadgetFactory::~OsgEarthviewGadgetFactory() -{ -} +{} -Core::IUAVGadget* OsgEarthviewGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *OsgEarthviewGadgetFactory::createGadget(QWidget *parent) { - OsgEarthviewWidget* gadgetWidget = new OsgEarthviewWidget(parent); + OsgEarthviewWidget *gadgetWidget = new OsgEarthviewWidget(parent); + return new OsgEarthviewGadget(QString("OsgEarthviewGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *OsgEarthviewGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *OsgEarthviewGadgetFactory::createConfiguration(QSettings *qSettings) { return new OsgEarthviewGadgetConfiguration(QString("OsgEarthviewGadget"), qSettings); } IOptionsPage *OsgEarthviewGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new OsgEarthviewGadgetOptionsPage(qobject_cast(config)); + return new OsgEarthviewGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.h b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.h index 5b6381c36..9557c8570 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OsgEarthview Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class OsgEarthviewGadgetFactory : public IUAVGadgetFactory -{ +class OsgEarthviewGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: OsgEarthviewGadgetFactory(QObject *parent = 0); ~OsgEarthviewGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.cpp index 181005f47..21d424e03 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.cpp @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgearthviewgadgetoptions.cpp -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgearthviewgadgetoptions.cpp + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -36,19 +36,17 @@ #include OsgEarthviewGadgetOptionsPage::OsgEarthviewGadgetOptionsPage(OsgEarthviewGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *OsgEarthviewGadgetOptionsPage::createPage(QWidget *parent) { - options_page = new Ui::OsgEarthviewGadgetOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget; - //main layout + // main layout options_page->setupUi(optionsPageWidget); return optionsPageWidget; @@ -61,11 +59,8 @@ QWidget *OsgEarthviewGadgetOptionsPage::createPage(QWidget *parent) * */ void OsgEarthviewGadgetOptionsPage::apply() -{ -} - +{} void OsgEarthviewGadgetOptionsPage::finish() -{ -} +{} diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.h index 856f5607d..7a77b391d 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -40,14 +40,13 @@ class IUAVGadgetConfiguration; class OsgEarthviewGadgetConfiguration; namespace Ui { - class OsgEarthviewGadgetOptionsPage; +class OsgEarthviewGadgetOptionsPage; } using namespace Core; -class OsgEarthviewGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class OsgEarthviewGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit OsgEarthviewGadgetOptionsPage(OsgEarthviewGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.cpp index 4f7fd7408..ea357544f 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.cpp @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgearthviewplugin.cpp -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgearthviewplugin.cpp + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -35,34 +35,33 @@ OsgEarthviewPlugin::OsgEarthviewPlugin() { - // Do nothing + // Do nothing } OsgEarthviewPlugin::~OsgEarthviewPlugin() { - // Do nothing + // Do nothing } -bool OsgEarthviewPlugin::initialize(const QStringList& args, QString *errMsg) +bool OsgEarthviewPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new OsgEarthviewGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new OsgEarthviewGadgetFactory(this); + addAutoReleasedObject(mf); - osgQt::initQtWindowingSystem(); + osgQt::initQtWindowingSystem(); - return true; + return true; } void OsgEarthviewPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void OsgEarthviewPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(OsgEarthviewPlugin) - diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.h b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.h index ced4dc089..5dafd81ef 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.h +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class OsgEarthviewGadgetFactory; -class OsgEarthviewPlugin : public ExtensionSystem::IPlugin -{ +class OsgEarthviewPlugin : public ExtensionSystem::IPlugin { public: - OsgEarthviewPlugin(); - ~OsgEarthviewPlugin(); + OsgEarthviewPlugin(); + ~OsgEarthviewPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - OsgEarthviewGadgetFactory *mf; + OsgEarthviewGadgetFactory *mf; }; #endif /* PFDPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp index 4d0b2cb80..c96ab3f77 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgearthviewwidget.cpp -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin Widget -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgearthviewwidget.cpp + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin Widget + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -101,26 +101,21 @@ using namespace Utils; OsgEarthviewWidget::OsgEarthviewWidget(QWidget *parent) : QWidget(parent) { - m_widget = new Ui_OsgEarthview(); m_widget->setupUi(this); /*viewWidget = new OsgViewerWidget(this); - viewWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + viewWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - setLayout(new QVBoxLayout()); - layout()->addWidget(viewWidget);*/ + setLayout(new QVBoxLayout()); + layout()->addWidget(viewWidget);*/ } OsgEarthviewWidget::~OsgEarthviewWidget() -{ -} +{} -void OsgEarthviewWidget::paintEvent( QPaintEvent* event ) -{ -} +void OsgEarthviewWidget::paintEvent(QPaintEvent *event) +{} void OsgEarthviewWidget::resizeEvent(QResizeEvent *event) -{ -} - +{} diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.h b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.h index be7545e29..d0eac2652 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.h +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.h @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgearthviewwidget.h -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgearthviewwidget.h + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -83,19 +83,18 @@ using namespace osgEarth::Annotation; class Ui_OsgEarthview; -class OsgEarthviewWidget : public QWidget -{ +class OsgEarthviewWidget : public QWidget { Q_OBJECT public: OsgEarthviewWidget(QWidget *parent = 0); - ~OsgEarthviewWidget(); + ~OsgEarthviewWidget(); public slots: protected: /* Protected methods */ - void paintEvent(QPaintEvent *event); - void resizeEvent(QResizeEvent *event); + void paintEvent(QPaintEvent *event); + void resizeEvent(QResizeEvent *event); OsgViewerWidget *viewWidget; Ui_OsgEarthview *m_widget; diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp index 89619f99b..2b2d11b73 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgviewerwidget.cpp -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin Widget -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgviewerwidget.cpp + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin Widget + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -108,19 +108,18 @@ OsgViewerWidget::OsgViewerWidget(QWidget *parent) : QWidget(parent) setThreadingModel(osgViewer::ViewerBase::CullThreadPerCameraDrawThreadPerContext); setAttribute(Qt::WA_PaintOnScreen, true); - osg::Group* root = new osg::Group; - osg::Node* earth = osgDB::readNodeFile("/Users/Cotton/Programming/osg/osgearth/tests/boston.earth"); - mapNode = osgEarth::MapNode::findMapNode( earth ); - if (!mapNode) - { - qDebug() <<"Uhoh"; + osg::Group *root = new osg::Group; + osg::Node *earth = osgDB::readNodeFile("/Users/Cotton/Programming/osg/osgearth/tests/boston.earth"); + mapNode = osgEarth::MapNode::findMapNode(earth); + if (!mapNode) { + qDebug() << "Uhoh"; } root->addChild(earth); - osg::Node* airplane = createAirplane(); + osg::Node *airplane = createAirplane(); uavPos = new osgEarth::Util::ObjectLocatorNode(mapNode->getMap()); - uavPos->getLocator()->setPosition( osg::Vec3d(-71.100549, 42.349273, 150) ); + uavPos->getLocator()->setPosition(osg::Vec3d(-71.100549, 42.349273, 150)); uavPos->addChild(airplane); root->addChild(uavPos); @@ -128,7 +127,7 @@ OsgViewerWidget::OsgViewerWidget(QWidget *parent) : QWidget(parent) osgUtil::Optimizer optimizer; optimizer.optimize(root); - QWidget* viewWidget = createViewWidget( createCamera(0,0,200,200,"Earth",false), root); + QWidget *viewWidget = createViewWidget(createCamera(0, 0, 200, 200, "Earth", false), root); viewWidget->show(); setLayout(new QVBoxLayout(this)); @@ -136,84 +135,86 @@ OsgViewerWidget::OsgViewerWidget(QWidget *parent) : QWidget(parent) layout()->addWidget(viewWidget); - connect( &_timer, SIGNAL(timeout()), this, SLOT(update()) ); - _timer.start( 10 ); + connect(&_timer, SIGNAL(timeout()), this, SLOT(update())); + _timer.start(10); } OsgViewerWidget::~OsgViewerWidget() +{} + +QWidget *OsgViewerWidget::createViewWidget(osg::Camera *camera, osg::Node *scene) { -} + osgViewer::View *view = new osgViewer::View; -QWidget* OsgViewerWidget::createViewWidget( osg::Camera* camera, osg::Node* scene ) -{ - osgViewer::View* view = new osgViewer::View; - view->setCamera( camera ); + view->setCamera(camera); - addView( view ); + addView(view); - view->setSceneData( scene ); - view->addEventHandler( new osgViewer::StatsHandler ); - view->getDatabasePager()->setDoPreCompile( true ); + view->setSceneData(scene); + view->addEventHandler(new osgViewer::StatsHandler); + view->getDatabasePager()->setDoPreCompile(true); manip = new EarthManipulator(); - view->setCameraManipulator( manip ); + view->setCameraManipulator(manip); -// osgGA::NodeTrackerManipulator *camTracker = new osgGA::NodeTrackerManipulator(); -// camTracker->setTrackNode(uavPos); -// camTracker->setMinimumDistance(0.0001f); -// camTracker->setDistance(0.001f); -// camTracker->setTrackerMode(osgGA::NodeTrackerManipulator::NODE_CENTER); -// view->setCameraManipulator(camTracker); +// osgGA::NodeTrackerManipulator *camTracker = new osgGA::NodeTrackerManipulator(); +// camTracker->setTrackNode(uavPos); +// camTracker->setMinimumDistance(0.0001f); +// camTracker->setDistance(0.001f); +// camTracker->setTrackerMode(osgGA::NodeTrackerManipulator::NODE_CENTER); +// view->setCameraManipulator(camTracker); - Grid* grid = new Grid(); - grid->setControl(0,0,new LabelControl("OpenPilot")); + Grid *grid = new Grid(); + grid->setControl(0, 0, new LabelControl("OpenPilot")); ControlCanvas::get(view, true)->addControl(grid); // zoom to a good startup position - manip->setViewpoint( Viewpoint(-71.100549, 42.349273, 0, 24.261, -21.6, 350.0), 5.0 ); - //manip->setViewpoint( Viewpoint(-71.100549, 42.349273, 0, 24.261, -81.6, 650.0), 5.0 ); - //manip->setHomeViewpoint(Viewpoint("Boston", osg::Vec3d(-71.0763, 42.34425, 0), 24.261, -21.6, 3450.0)); + manip->setViewpoint(Viewpoint(-71.100549, 42.349273, 0, 24.261, -21.6, 350.0), 5.0); + // manip->setViewpoint( Viewpoint(-71.100549, 42.349273, 0, 24.261, -81.6, 650.0), 5.0 ); + // manip->setHomeViewpoint(Viewpoint("Boston", osg::Vec3d(-71.0763, 42.34425, 0), 24.261, -21.6, 3450.0)); manip->setTetherNode(uavPos); - osgQt::GraphicsWindowQt* gw = dynamic_cast( camera->getGraphicsContext() ); + osgQt::GraphicsWindowQt *gw = dynamic_cast(camera->getGraphicsContext()); return gw ? gw->getGLWidget() : NULL; } -osg::Camera* OsgViewerWidget::createCamera( int x, int y, int w, int h, const std::string& name="", bool windowDecoration=false ) +osg::Camera *OsgViewerWidget::createCamera(int x, int y, int w, int h, const std::string & name = "", bool windowDecoration = false) { - osg::DisplaySettings* ds = osg::DisplaySettings::instance().get(); + osg::DisplaySettings *ds = osg::DisplaySettings::instance().get(); osg::ref_ptr traits = new osg::GraphicsContext::Traits; + traits->windowName = name; traits->windowDecoration = windowDecoration; - traits->x = x; - traits->y = y; - traits->width = w; - traits->height = h; + traits->x = x; + traits->y = y; + traits->width = w; + traits->height = h; traits->doubleBuffer = true; - traits->alpha = ds->getMinimumNumAlphaBits(); + traits->alpha = ds->getMinimumNumAlphaBits(); traits->stencil = ds->getMinimumNumStencilBits(); traits->sampleBuffers = ds->getMultiSamples(); traits->samples = ds->getNumMultiSamples(); osg::ref_ptr camera = new osg::Camera; - camera->setGraphicsContext( new osgQt::GraphicsWindowQt(traits.get()) ); + camera->setGraphicsContext(new osgQt::GraphicsWindowQt(traits.get())); - camera->setClearColor( osg::Vec4(0.2, 0.2, 0.6, 1.0) ); - camera->setViewport( new osg::Viewport(0, 0, traits->width, traits->height) ); + camera->setClearColor(osg::Vec4(0.2, 0.2, 0.6, 1.0)); + camera->setViewport(new osg::Viewport(0, 0, traits->width, traits->height)); camera->setProjectionMatrixAsPerspective( - 30.0f, static_cast(traits->width)/static_cast(traits->height), 1.0f, 10000.0f ); + 30.0f, static_cast(traits->width) / static_cast(traits->height), 1.0f, 10000.0f); return camera.release(); } -osg::Node* OsgViewerWidget::createAirplane() +osg::Node *OsgViewerWidget::createAirplane() { - osg::Group* model = new osg::Group; + osg::Group *model = new osg::Group; osg::Node *uav; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm); - UAVObjectManager * objMngr = pm->getObject(); + UAVObjectManager *objMngr = pm->getObject(); Q_ASSERT(objMngr); SystemSettings *systemSettingsObj = SystemSettings::GetInstance(objMngr); @@ -221,56 +222,57 @@ osg::Node* OsgViewerWidget::createAirplane() qDebug() << "Frame type:" << systemSettingsObj->getField("AirframeType")->getValue().toString(); // Get model that matches airframe type - switch(systemSettings.AirframeType) { + switch (systemSettings.AirframeType) { case SystemSettings::AIRFRAMETYPE_FIXEDWING: case SystemSettings::AIRFRAMETYPE_FIXEDWINGELEVON: - case SystemSettings::AIRFRAMETYPE_FIXEDWINGVTAIL: + case SystemSettings::AIRFRAMETYPE_FIXEDWINGVTAIL: uav = osgDB::readNodeFile("/Users/Cotton/Programming/OpenPilot/artwork/3D Model/planes/Easystar/easystar.3ds"); break; default: uav = osgDB::readNodeFile("/Users/Cotton/Programming/OpenPilot/artwork/3D Model/multi/joes_cnc/J14-QT_+.3DS"); } - if(uav) { + if (uav) { uavAttitudeAndScale = new osg::MatrixTransform(); - uavAttitudeAndScale->setMatrix(osg::Matrixd::scale(0.2e0,0.2e0,0.2e0)); + uavAttitudeAndScale->setMatrix(osg::Matrixd::scale(0.2e0, 0.2e0, 0.2e0)); // Apply a rotation so model is NED before any other rotations osg::MatrixTransform *rotateModelNED = new osg::MatrixTransform(); - rotateModelNED->setMatrix(osg::Matrixd::scale(0.05e0,0.05e0,0.05e0) * osg::Matrixd::rotate(M_PI, osg::Vec3d(0,0,1))); - rotateModelNED->addChild( uav ); + rotateModelNED->setMatrix(osg::Matrixd::scale(0.05e0, 0.05e0, 0.05e0) * osg::Matrixd::rotate(M_PI, osg::Vec3d(0, 0, 1))); + rotateModelNED->addChild(uav); - uavAttitudeAndScale->addChild( rotateModelNED ); + uavAttitudeAndScale->addChild(rotateModelNED); model->addChild(uavAttitudeAndScale); - } else + } else { qDebug() << "Bad model file"; + } return model; } -void OsgViewerWidget::paintEvent( QPaintEvent* event ) +void OsgViewerWidget::paintEvent(QPaintEvent *event) { Q_UNUSED(event); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * objMngr = pm->getObject(); + UAVObjectManager *objMngr = pm->getObject(); - PositionActual *positionActualObj = PositionActual::GetInstance(objMngr); + PositionActual *positionActualObj = PositionActual::GetInstance(objMngr); PositionActual::DataFields positionActual = positionActualObj->getData(); - double NED[3] = {positionActual.North, positionActual.East, positionActual.Down}; + double NED[3] = { positionActual.North, positionActual.East, positionActual.Down }; bool positionActualUpdate = true; - if(positionActualUpdate) { + if (positionActualUpdate) { HomeLocation *homeLocationObj = HomeLocation::GetInstance(objMngr); HomeLocation::DataFields homeLocation = homeLocationObj->getData(); - double homeLLA[3] = {homeLocation.Latitude / 10.0e6, homeLocation.Longitude / 10.0e6, homeLocation.Altitude}; + double homeLLA[3] = { homeLocation.Latitude / 10.0e6, homeLocation.Longitude / 10.0e6, homeLocation.Altitude }; double LLA[3]; - CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA); - uavPos->getLocator()->setPosition( osg::Vec3d(LLA[1], LLA[0], LLA[2]) ); // Note this takes longtitude first + CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA); + uavPos->getLocator()->setPosition(osg::Vec3d(LLA[1], LLA[0], LLA[2])); // Note this takes longtitude first } else { GPSPosition *gpsPosObj = GPSPosition::GetInstance(objMngr); GPSPosition::DataFields gpsPos = gpsPosObj->getData(); - uavPos->getLocator()->setPosition( osg::Vec3d(gpsPos.Longitude / 10.0e6, gpsPos.Latitude / 10.0e6, gpsPos.Altitude)); + uavPos->getLocator()->setPosition(osg::Vec3d(gpsPos.Longitude / 10.0e6, gpsPos.Latitude / 10.0e6, gpsPos.Altitude)); } // Set the attitude (reverse the attitude) @@ -281,8 +283,8 @@ void OsgViewerWidget::paintEvent( QPaintEvent* event ) // Have to rotate the axes from OP NED frame to OSG frame (X east, Y north, Z down) double angle; osg::Vec3d axis; - quat.getRotate(angle,axis); - quat.makeRotate(angle, osg::Vec3d(axis[1],axis[0],-axis[2])); + quat.getRotate(angle, axis); + quat.makeRotate(angle, osg::Vec3d(axis[1], axis[0], -axis[2])); osg::Matrixd rot = osg::Matrixd::rotate(quat); uavAttitudeAndScale->setMatrix(rot); diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.h b/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.h index cc7e8894c..03002c32f 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.h +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.h @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgviewerwidget.h -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgviewerwidget.h + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -82,8 +82,7 @@ using namespace osgEarth::Annotation; #include -class OsgViewerWidget : public QWidget, public osgViewer::CompositeViewer -{ +class OsgViewerWidget : public QWidget, public osgViewer::CompositeViewer { Q_OBJECT public: explicit OsgViewerWidget(QWidget *parent = 0); @@ -96,20 +95,20 @@ protected: void paintEvent(QPaintEvent *event); /* Create a osgQt::GraphicsWindowQt to add to the widget */ - QWidget* createViewWidget( osg::Camera* camera, osg::Node* scene ); + QWidget *createViewWidget(osg::Camera *camera, osg::Node *scene); /* Create an osg::Camera which sets up the OSG view */ - osg::Camera* createCamera( int x, int y, int w, int h, const std::string& name, bool windowDecoration ); + osg::Camera *createCamera(int x, int y, int w, int h, const std::string & name, bool windowDecoration); /* Get the model to render */ - osg::Node* createAirplane(); + osg::Node *createAirplane(); private: /* Private variables */ QTimer _timer; - EarthManipulator* manip; - osgEarth::Util::ObjectLocatorNode* uavPos; - osg::MatrixTransform* uavAttitudeAndScale; - osgEarth::MapNode* mapNode; + EarthManipulator *manip; + osgEarth::Util::ObjectLocatorNode *uavPos; + osg::MatrixTransform *uavAttitudeAndScale; + osgEarth::MapNode *mapNode; }; diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.cpp index c0ac4fa01..028dc82c3 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.cpp @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -29,16 +29,15 @@ #include "fieldtreeitem.h" BrowserItemDelegate::BrowserItemDelegate(QObject *parent) : - QStyledItemDelegate(parent) -{ -} + QStyledItemDelegate(parent) +{} QWidget *BrowserItemDelegate::createEditor(QWidget *parent, - const QStyleOptionViewItem & option , - const QModelIndex & index ) const + const QStyleOptionViewItem & option, + const QModelIndex & index) const { Q_UNUSED(option) - FieldTreeItem *item = static_cast(index.internalPointer()); + FieldTreeItem * item = static_cast(index.internalPointer()); QWidget *editor = item->createEditor(parent); Q_ASSERT(editor); return editor; @@ -48,21 +47,23 @@ QWidget *BrowserItemDelegate::createEditor(QWidget *parent, void BrowserItemDelegate::setEditorData(QWidget *editor, const QModelIndex &index) const { - FieldTreeItem *item = static_cast(index.internalPointer()); + FieldTreeItem *item = static_cast(index.internalPointer()); QVariant value = index.model()->data(index, Qt::EditRole); + item->setEditorValue(editor, value); } void BrowserItemDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, const QModelIndex &index) const { - FieldTreeItem *item = static_cast(index.internalPointer()); + FieldTreeItem *item = static_cast(index.internalPointer()); QVariant value = item->getEditorValue(editor); + model->setData(index, value, Qt::EditRole); } void BrowserItemDelegate::updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &/* index */) const + const QStyleOptionViewItem &option, const QModelIndex & /* index */) const { editor->setGeometry(option.rect); } diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.h b/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.h index 9c6a0e983..17952b5db 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,9 +30,8 @@ #include -class BrowserItemDelegate : public QStyledItemDelegate -{ -Q_OBJECT +class BrowserItemDelegate : public QStyledItemDelegate { + Q_OBJECT public: explicit BrowserItemDelegate(QObject *parent = 0); @@ -44,15 +43,14 @@ public: const QModelIndex &index) const; void updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &index) const; + const QStyleOptionViewItem &option, const QModelIndex &index) const; QSize sizeHint(const QStyleOptionViewItem & option, - const QModelIndex &index) const; + const QModelIndex &index) const; signals: public slots: - }; #endif // BROWSERITEMDELEGATE_H diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.cpp index fa3c9e203..2c1407d06 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.cpp @@ -10,20 +10,19 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "fieldtreeitem.h" - diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.h b/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.h index 7d33c431f..27c4566f7 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.h @@ -37,88 +37,102 @@ #include #include -#define QINT8MIN std::numeric_limits::min() -#define QINT8MAX std::numeric_limits::max() -#define QUINTMIN std::numeric_limits::min() -#define QUINT8MAX std::numeric_limits::max() -#define QINT16MIN std::numeric_limits::min() -#define QINT16MAX std::numeric_limits::max() +#define QINT8MIN std::numeric_limits::min() +#define QINT8MAX std::numeric_limits::max() +#define QUINTMIN std::numeric_limits::min() +#define QUINT8MAX std::numeric_limits::max() +#define QINT16MIN std::numeric_limits::min() +#define QINT16MAX std::numeric_limits::max() #define QUINT16MAX std::numeric_limits::max() -#define QINT32MIN std::numeric_limits::min() -#define QINT32MAX std::numeric_limits::max() +#define QINT32MIN std::numeric_limits::min() +#define QINT32MAX std::numeric_limits::max() #define QUINT32MAX std::numeric_limits::max() -//#define USE_SCIENTIFIC_NOTATION +// #define USE_SCIENTIFIC_NOTATION -class FieldTreeItem : public TreeItem -{ -Q_OBJECT +class FieldTreeItem : public TreeItem { + Q_OBJECT public: FieldTreeItem(int index, const QList &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_index(index) { } + TreeItem(data, parent), m_index(index) {} FieldTreeItem(int index, const QVariant &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_index(index) { } - bool isEditable() { return true; } - virtual QWidget *createEditor(QWidget *parent) = 0; + TreeItem(data, parent), m_index(index) {} + bool isEditable() + { + return true; + } + virtual QWidget *createEditor(QWidget *parent) = 0; virtual QVariant getEditorValue(QWidget *editor) = 0; virtual void setEditorValue(QWidget *editor, QVariant value) = 0; - virtual void apply() { } + virtual void apply() {} protected: int m_index; }; -class EnumFieldTreeItem : public FieldTreeItem -{ -Q_OBJECT +class EnumFieldTreeItem : public FieldTreeItem { + Q_OBJECT public: EnumFieldTreeItem(UAVObjectField *field, int index, const QList &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) { } + FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) {} EnumFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) { } - void setData(QVariant value, int column) { + FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) {} + void setData(QVariant value, int column) + { QStringList options = m_field->getOptions(); - QVariant tmpValue = m_field->getValue(m_index); - int tmpValIndex = options.indexOf(tmpValue.toString()); + QVariant tmpValue = m_field->getValue(m_index); + int tmpValIndex = options.indexOf(tmpValue.toString()); TreeItem::setData(value, column); + setChanged(tmpValIndex != value); } - QString enumOptions(int index) { - if((index < 0) || (index >= m_enumOptions.length())) { + QString enumOptions(int index) + { + if ((index < 0) || (index >= m_enumOptions.length())) { return QString("Invalid Value (") + QString().setNum(index) + QString(")"); } return m_enumOptions.at(index); } - void apply() { + void apply() + { int value = data(dataColumn).toInt(); QStringList options = m_field->getOptions(); + m_field->setValue(options[value], m_index); setChanged(false); } - void update() { + void update() + { QStringList options = m_field->getOptions(); QVariant value = m_field->getValue(m_index); - int valIndex = options.indexOf(value.toString()); + int valIndex = options.indexOf(value.toString()); + if (data() != valIndex || changed()) { TreeItem::setData(valIndex); setHighlight(true); } } - QWidget *createEditor(QWidget *parent) { + QWidget *createEditor(QWidget *parent) + { QComboBox *editor = new QComboBox(parent); - foreach (QString option, m_enumOptions) - editor->addItem(option); + + foreach(QString option, m_enumOptions) + editor->addItem(option); return editor; } - QVariant getEditorValue(QWidget *editor) { - QComboBox *comboBox = static_cast(editor); + QVariant getEditorValue(QWidget *editor) + { + QComboBox *comboBox = static_cast(editor); + return comboBox->currentIndex(); } - void setEditorValue(QWidget *editor, QVariant value) { - QComboBox *comboBox = static_cast(editor); + void setEditorValue(QWidget *editor, QVariant value) + { + QComboBox *comboBox = static_cast(editor); + comboBox->setCurrentIndex(value.toInt()); } private: @@ -126,20 +140,22 @@ private: UAVObjectField *m_field; }; -class IntFieldTreeItem : public FieldTreeItem -{ -Q_OBJECT +class IntFieldTreeItem : public FieldTreeItem { + Q_OBJECT public: IntFieldTreeItem(UAVObjectField *field, int index, const QList &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { + FieldTreeItem(index, data, parent), m_field(field) + { setMinMaxValues(); } IntFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { + FieldTreeItem(index, data, parent), m_field(field) + { setMinMaxValues(); } - void setMinMaxValues() { + void setMinMaxValues() + { switch (m_field->getType()) { case UAVObjectField::INT8: m_minValue = QINT8MIN; @@ -171,34 +187,45 @@ public: } } - QWidget *createEditor(QWidget *parent) { + QWidget *createEditor(QWidget *parent) + { QSpinBox *editor = new QSpinBox(parent); + editor->setMinimum(m_minValue); editor->setMaximum(m_maxValue); return editor; } - QVariant getEditorValue(QWidget *editor) { - QSpinBox *spinBox = static_cast(editor); + QVariant getEditorValue(QWidget *editor) + { + QSpinBox *spinBox = static_cast(editor); + spinBox->interpretText(); return spinBox->value(); } - void setEditorValue(QWidget *editor, QVariant value) { - QSpinBox *spinBox = static_cast(editor); + void setEditorValue(QWidget *editor, QVariant value) + { + QSpinBox *spinBox = static_cast(editor); + spinBox->setValue(value.toInt()); } - void setData(QVariant value, int column) { - QVariant old=m_field->getValue(m_index); - TreeItem::setData(value, column); + void setData(QVariant value, int column) + { + QVariant old = m_field->getValue(m_index); + TreeItem::setData(value, column); + setChanged(old != value); } - void apply() { + void apply() + { m_field->setValue(data(dataColumn).toInt(), m_index); setChanged(false); } - void update() { + void update() + { int value = m_field->getValue(m_index).toInt(); + if (data() != value || changed()) { TreeItem::setData(value); setHighlight(true); @@ -211,112 +238,137 @@ private: int m_maxValue; }; -class FloatFieldTreeItem : public FieldTreeItem -{ -Q_OBJECT +class FloatFieldTreeItem : public FieldTreeItem { + Q_OBJECT public: FloatFieldTreeItem(UAVObjectField *field, int index, const QList &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { } + FieldTreeItem(index, data, parent), m_field(field) {} FloatFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { } - void setData(QVariant value, int column) { - QVariant old=m_field->getValue(m_index); + FieldTreeItem(index, data, parent), m_field(field) {} + void setData(QVariant value, int column) + { + QVariant old = m_field->getValue(m_index); TreeItem::setData(value, column); + setChanged(old != value); } - void apply() { + void apply() + { m_field->setValue(data(dataColumn).toDouble(), m_index); setChanged(false); } - void update() { + void update() + { double value = m_field->getValue(m_index).toDouble(); + if (data() != value || changed()) { TreeItem::setData(value); setHighlight(true); } } - QWidget *createEditor(QWidget *parent) { - #ifdef USE_SCIENTIFIC_NOTATION - QScienceSpinBox *editor = new QScienceSpinBox(parent); - editor->setDecimals(6); - #else - QDoubleSpinBox *editor = new QDoubleSpinBox(parent); - editor->setDecimals(8); - #endif + QWidget *createEditor(QWidget *parent) + { + #ifdef USE_SCIENTIFIC_NOTATION + QScienceSpinBox *editor = new QScienceSpinBox(parent); + editor->setDecimals(6); + #else + QDoubleSpinBox *editor = new QDoubleSpinBox(parent); + editor->setDecimals(8); + #endif editor->setMinimum(-std::numeric_limits::max()); editor->setMaximum(std::numeric_limits::max()); return editor; } - QVariant getEditorValue(QWidget *editor) { - #ifdef USE_SCIENTIFIC_NOTATION - QScienceSpinBox *spinBox = static_cast(editor); - #else - QDoubleSpinBox *spinBox = static_cast(editor); - #endif - spinBox->interpretText(); + QVariant getEditorValue(QWidget *editor) + { + #ifdef USE_SCIENTIFIC_NOTATION + QScienceSpinBox *spinBox = static_cast(editor); + #else + QDoubleSpinBox *spinBox = static_cast(editor); + #endif + spinBox->interpretText(); return spinBox->value(); } - void setEditorValue(QWidget *editor, QVariant value) { - #ifdef USE_SCIENTIFIC_NOTATION - QScienceSpinBox *spinBox = static_cast(editor); - #else - QDoubleSpinBox *spinBox = static_cast(editor); - #endif - spinBox->setValue(value.toDouble()); + void setEditorValue(QWidget *editor, QVariant value) + { + #ifdef USE_SCIENTIFIC_NOTATION + QScienceSpinBox *spinBox = static_cast(editor); + #else + QDoubleSpinBox *spinBox = static_cast(editor); + #endif + spinBox->setValue(value.toDouble()); } private: UAVObjectField *m_field; }; -class ActionFieldTreeItem : public FieldTreeItem -{ -Q_OBJECT +class ActionFieldTreeItem : public FieldTreeItem { + Q_OBJECT public: ActionFieldTreeItem(UAVObjectField *field, int index, const QList &data, QStringList *actions, - TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { m_enumOptions=actions; } + TreeItem *parent = 0) : + FieldTreeItem(index, data, parent), m_field(field) + { + m_enumOptions = actions; + } ActionFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, QStringList *actions, - TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { m_enumOptions=actions; } - void setData(QVariant value, int column) { + TreeItem *parent = 0) : + FieldTreeItem(index, data, parent), m_field(field) + { + m_enumOptions = actions; + } + void setData(QVariant value, int column) + { int tmpValIndex = m_field->getValue(m_index).toInt(); TreeItem::setData(value, column); + setChanged(tmpValIndex != value); } - QString enumOptions(int index) { - if((index < 0) || (index >= m_enumOptions->length())) { + QString enumOptions(int index) + { + if ((index < 0) || (index >= m_enumOptions->length())) { return QString("Invalid Value (") + QString().setNum(index) + QString(")"); } return m_enumOptions->at(index); } - void apply() { + void apply() + { int value = data(dataColumn).toInt(); + m_field->setValue(value, m_index); setChanged(false); } - void update() { + void update() + { int valIndex = m_field->getValue(m_index).toInt(); + if (data() != valIndex || changed()) { TreeItem::setData(valIndex); setHighlight(true); } } - QWidget *createEditor(QWidget *parent) { + QWidget *createEditor(QWidget *parent) + { QComboBox *editor = new QComboBox(parent); - foreach (QString option, *m_enumOptions) - editor->addItem(option); + + foreach(QString option, *m_enumOptions) + editor->addItem(option); return editor; } - QVariant getEditorValue(QWidget *editor) { - QComboBox *comboBox = static_cast(editor); + QVariant getEditorValue(QWidget *editor) + { + QComboBox *comboBox = static_cast(editor); + return comboBox->currentIndex(); } - void setEditorValue(QWidget *editor, QVariant value) { - QComboBox *comboBox = static_cast(editor); + void setEditorValue(QWidget *editor, QVariant value) + { + QComboBox *comboBox = static_cast(editor); + comboBox->setCurrentIndex(value.toInt()); } private: diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.cpp index 078c58525..0d78b536c 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.cpp @@ -32,17 +32,16 @@ #include PathActionEditorGadget::PathActionEditorGadget(QString classId, PathActionEditorGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} PathActionEditorGadget::~PathActionEditorGadget() { delete m_widget; } -void PathActionEditorGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void PathActionEditorGadget::loadConfiguration(IUAVGadgetConfiguration *config) { Q_UNUSED(config); } diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.h b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.h index 936019940..caeb66f30 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.h @@ -32,27 +32,35 @@ namespace Core { class IUAVGadget; } -//class QWidget; -//class QString; +// class QWidget; +// class QString; class PathActionEditorGadgetWidget; using namespace Core; -class PathActionEditorGadget : public Core::IUAVGadget -{ +class PathActionEditorGadget : public Core::IUAVGadget { Q_OBJECT public: PathActionEditorGadget(QString classId, PathActionEditorGadgetWidget *widget, QWidget *parent = 0); ~PathActionEditorGadget(); - QList context() const { return m_context; } - QWidget *widget() { return m_widget; } - QString contextHelpId() const { return QString(); } + QList context() const + { + return m_context; + } + QWidget *widget() + { + return m_widget; + } + QString contextHelpId() const + { + return QString(); + } - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); private: - QWidget *m_widget; - QList m_context; + QWidget *m_widget; + QList m_context; }; diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.cpp index c84250e21..b6f48a522 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.cpp @@ -30,18 +30,17 @@ #include PathActionEditorGadgetFactory::PathActionEditorGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("PathActionEditorGadget"), - tr("PathAction Editor"), - parent) -{ -} + IUAVGadgetFactory(QString("PathActionEditorGadget"), + tr("PathAction Editor"), + parent) +{} PathActionEditorGadgetFactory::~PathActionEditorGadgetFactory() +{} + +IUAVGadget *PathActionEditorGadgetFactory::createGadget(QWidget *parent) { + PathActionEditorGadgetWidget *gadgetWidget = new PathActionEditorGadgetWidget(parent); -} - -IUAVGadget* PathActionEditorGadgetFactory::createGadget(QWidget *parent) { - PathActionEditorGadgetWidget* gadgetWidget = new PathActionEditorGadgetWidget(parent); return new PathActionEditorGadget(QString("PathActionEditorGadget"), gadgetWidget, parent); } diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.h b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.h index e78021528..a2295dda7 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.h @@ -36,8 +36,7 @@ class IUAVGadgetFactory; using namespace Core; -class PathActionEditorGadgetFactory : public IUAVGadgetFactory -{ +class PathActionEditorGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: PathActionEditorGadgetFactory(QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.cpp index a1b568988..00f306587 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.cpp @@ -58,7 +58,7 @@ PathActionEditorGadgetWidget::PathActionEditorGadgetWidget(QWidget *parent) : QL Q_ASSERT(objManager != NULL); pathactionObj = PathAction::GetInstance(objManager); Q_ASSERT(pathactionObj != NULL); - waypointObj = Waypoint::GetInstance(objManager); + waypointObj = Waypoint::GetInstance(objManager); Q_ASSERT(waypointObj != NULL); // Connect the signals @@ -70,24 +70,24 @@ PathActionEditorGadgetWidget::PathActionEditorGadgetWidget(QWidget *parent) : QL PathActionEditorGadgetWidget::~PathActionEditorGadgetWidget() { - // Do nothing + // Do nothing } void PathActionEditorGadgetWidget::pathactionChanged(UAVObject *) -{ -} +{} void PathActionEditorGadgetWidget::addPathActionInstance() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm != NULL); UAVObjectManager *objManager = pm->getObject(); Q_ASSERT(objManager != NULL); qDebug() << "Instances before: " << objManager->getNumInstances(pathactionObj->getObjID()); - PathAction *obj = new PathAction(); + PathAction *obj = new PathAction(); quint32 newInstId = objManager->getNumInstances(pathactionObj->getObjID()); - obj->initialize(newInstId,obj->getMetaObject()); + obj->initialize(newInstId, obj->getMetaObject()); objManager->registerObject(obj); qDebug() << "Instances after: " << objManager->getNumInstances(pathactionObj->getObjID()); } @@ -95,19 +95,20 @@ void PathActionEditorGadgetWidget::addPathActionInstance() void PathActionEditorGadgetWidget::addWaypointInstance() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm != NULL); UAVObjectManager *objManager = pm->getObject(); Q_ASSERT(objManager != NULL); qDebug() << "Instances before: " << objManager->getNumInstances(waypointObj->getObjID()); - Waypoint *obj = new Waypoint(); + Waypoint *obj = new Waypoint(); quint32 newInstId = objManager->getNumInstances(waypointObj->getObjID()); - obj->initialize(newInstId,obj->getMetaObject()); + obj->initialize(newInstId, obj->getMetaObject()); objManager->registerObject(obj); qDebug() << "Instances after: " << objManager->getNumInstances(waypointObj->getObjID()); } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.h b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.h index bfbcb6193..f01411861 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.h @@ -36,8 +36,7 @@ class Ui_PathActionEditor; -class PathActionEditorGadgetWidget : public QLabel -{ +class PathActionEditorGadgetWidget : public QLabel { Q_OBJECT public: @@ -52,7 +51,7 @@ protected slots: void addWaypointInstance(); private: - Ui_PathActionEditor * m_pathactioneditor; + Ui_PathActionEditor *m_pathactioneditor; PathActionEditorTreeModel *m_model; PathAction *pathactionObj; Waypoint *waypointObj; diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.cpp index 090397bcc..99c1a611e 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.cpp @@ -33,36 +33,36 @@ PathActionEditorPlugin::PathActionEditorPlugin() { - // Do nothing + // Do nothing } PathActionEditorPlugin::~PathActionEditorPlugin() { - // Do nothing + // Do nothing } -bool PathActionEditorPlugin::initialize(const QStringList& args, QString *errMsg) +bool PathActionEditorPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new PathActionEditorGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new PathActionEditorGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void PathActionEditorPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void PathActionEditorPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(PathActionEditorPlugin) /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.h b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.h index d0102ada7..2a00feda4 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.h @@ -31,16 +31,15 @@ class PathActionEditorGadgetFactory; -class PathActionEditorPlugin : public ExtensionSystem::IPlugin -{ +class PathActionEditorPlugin : public ExtensionSystem::IPlugin { public: PathActionEditorPlugin(); - ~PathActionEditorPlugin(); + ~PathActionEditorPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - PathActionEditorGadgetFactory *mf; + PathActionEditorGadgetFactory *mf; }; #endif /* PathActionEditorPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.cpp index 86b0c8400..629140e1a 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.cpp @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,7 @@ #include "uavobjectfield.h" #include "extensionsystem/pluginmanager.h" #include -//#include +// #include #include #include #include @@ -43,17 +43,18 @@ #include "pathaction.h" PathActionEditorTreeModel::PathActionEditorTreeModel(QObject *parent) : - QAbstractItemModel(parent), - m_recentlyUpdatedColor(QColor(255, 230, 230)), - m_manuallyChangedColor(QColor(230, 230, 255)) + QAbstractItemModel(parent), + m_recentlyUpdatedColor(QColor(255, 230, 230)), + m_manuallyChangedColor(QColor(230, 230, 255)) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + m_objManager = pm->getObject(); - connect(m_objManager, SIGNAL(newInstance(UAVObject*)), this, SLOT(newInstance(UAVObject*))); - connect(m_objManager->getObject("WaypointActive"),SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objUpdated(UAVObject*))); - connect(m_objManager->getObject("PathAction"),SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objUpdated(UAVObject*))); - connect(m_objManager->getObject("Waypoint"),SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objUpdated(UAVObject*))); + connect(m_objManager, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *))); + connect(m_objManager->getObject("WaypointActive"), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(objUpdated(UAVObject *))); + connect(m_objManager->getObject("PathAction"), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(objUpdated(UAVObject *))); + connect(m_objManager->getObject("Waypoint"), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(objUpdated(UAVObject *))); setupModelData(); } @@ -65,7 +66,6 @@ PathActionEditorTreeModel::~PathActionEditorTreeModel() void PathActionEditorTreeModel::setupModelData() { - m_actions = new QStringList(); updateActions(); @@ -76,52 +76,54 @@ void PathActionEditorTreeModel::setupModelData() m_pathactionsTree = new TopTreeItem(tr("PathActions"), m_rootItem); m_rootItem->appendChild(m_pathactionsTree); - m_waypointsTree = new TopTreeItem(tr("Waypoints"), m_rootItem); + m_waypointsTree = new TopTreeItem(tr("Waypoints"), m_rootItem); m_rootItem->appendChild(m_waypointsTree); - connect(m_rootItem, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); - connect(m_pathactionsTree, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); - connect(m_waypointsTree, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(m_rootItem, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); + connect(m_pathactionsTree, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); + connect(m_waypointsTree, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); { - QList list = m_objManager->getObjectInstances("PathAction"); - foreach (UAVObject* obj, list) { - addInstance(obj,m_pathactionsTree); + QList list = m_objManager->getObjectInstances("PathAction"); + foreach(UAVObject * obj, list) { + addInstance(obj, m_pathactionsTree); } } { - QList list = m_objManager->getObjectInstances("Waypoint"); - foreach (UAVObject* obj, list) { - addInstance(obj,m_waypointsTree); + QList list = m_objManager->getObjectInstances("Waypoint"); + foreach(UAVObject * obj, list) { + addInstance(obj, m_waypointsTree); } } } -void PathActionEditorTreeModel::updateActions() { - m_actions->clear(); - QList list = m_objManager->getObjectInstances("PathAction"); - foreach (UAVObject* obj, list) { - QString title; - title.append((QVariant(obj->getInstID()).toString())); - title.append(" "); - title.append((obj->getField("Mode")->getValue().toString())); - title.append(" "); - title.append((obj->getField("Command")->getValue().toString())); - title.append(":"); - title.append((obj->getField("EndCondition")->getValue().toString())); - title.append(" "); - m_actions->append(title); - } +void PathActionEditorTreeModel::updateActions() +{ + m_actions->clear(); + QList list = m_objManager->getObjectInstances("PathAction"); + foreach(UAVObject * obj, list) { + QString title; + + title.append((QVariant(obj->getInstID()).toString())); + title.append(" "); + title.append((obj->getField("Mode")->getValue().toString())); + title.append(" "); + title.append((obj->getField("Command")->getValue().toString())); + title.append(":"); + title.append((obj->getField("EndCondition")->getValue().toString())); + title.append(" "); + m_actions->append(title); + } } void PathActionEditorTreeModel::addInstance(UAVObject *obj, TreeItem *parent) { - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(highlightUpdatedObject(UAVObject*))); + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(highlightUpdatedObject(UAVObject *))); TreeItem *item; QString name = QString::number(obj->getInstID()); item = new InstanceTreeItem(obj, name); - connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(item, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); parent->appendChild(item); - foreach (UAVObjectField *field, obj->getFields()) { + foreach(UAVObjectField * field, obj->getFields()) { if (field->getNumElements() > 1) { addArrayField(field, item); } else { @@ -134,7 +136,8 @@ void PathActionEditorTreeModel::addInstance(UAVObject *obj, TreeItem *parent) void PathActionEditorTreeModel::addArrayField(UAVObjectField *field, TreeItem *parent) { TreeItem *item = new ArrayFieldTreeItem(field->getName()); - connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + + connect(item, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); for (uint i = 0; i < field->getNumElements(); ++i) { addSingleField(i, field, item); } @@ -144,83 +147,90 @@ void PathActionEditorTreeModel::addArrayField(UAVObjectField *field, TreeItem *p void PathActionEditorTreeModel::addSingleField(int index, UAVObjectField *field, TreeItem *parent) { QList data; - if (field->getNumElements() == 1) + if (field->getNumElements() == 1) { data.append(field->getName()); - else - data.append( QString("[%1]").arg((field->getElementNames())[index]) ); + } else { + data.append(QString("[%1]").arg((field->getElementNames())[index])); + } FieldTreeItem *item; UAVObjectField::FieldType type = field->getType(); // hack: list available actions in an enum - if (field->getName().compare("Action")==0 && type==UAVObjectField::UINT8) { - data.append( field->getValue(index).toInt()); - data.append( field->getUnits()); + if (field->getName().compare("Action") == 0 && type == UAVObjectField::UINT8) { + data.append(field->getValue(index).toInt()); + data.append(field->getUnits()); item = new ActionFieldTreeItem(field, index, data, m_actions); } else { - switch (type) { - case UAVObjectField::ENUM: { - QStringList options = field->getOptions(); - QVariant value = field->getValue(); - data.append( options.indexOf(value.toString()) ); - data.append(field->getUnits()); - item = new EnumFieldTreeItem(field, index, data); - break; + switch (type) { + case UAVObjectField::ENUM: + { + QStringList options = field->getOptions(); + QVariant value = field->getValue(); + data.append(options.indexOf(value.toString())); + data.append(field->getUnits()); + item = new EnumFieldTreeItem(field, index, data); + break; + } + case UAVObjectField::INT8: + case UAVObjectField::INT16: + case UAVObjectField::INT32: + case UAVObjectField::UINT8: + case UAVObjectField::UINT16: + case UAVObjectField::UINT32: + data.append(field->getValue(index)); + data.append(field->getUnits()); + item = new IntFieldTreeItem(field, index, data); + break; + case UAVObjectField::FLOAT32: + data.append(field->getValue(index)); + data.append(field->getUnits()); + item = new FloatFieldTreeItem(field, index, data); + break; + default: + Q_ASSERT(false); + } } - case UAVObjectField::INT8: - case UAVObjectField::INT16: - case UAVObjectField::INT32: - case UAVObjectField::UINT8: - case UAVObjectField::UINT16: - case UAVObjectField::UINT32: - data.append(field->getValue(index)); - data.append(field->getUnits()); - item = new IntFieldTreeItem(field, index, data); - break; - case UAVObjectField::FLOAT32: - data.append(field->getValue(index)); - data.append(field->getUnits()); - item = new FloatFieldTreeItem(field, index, data); - break; - default: - Q_ASSERT(false); - } - } - connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(item, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); parent->appendChild(item); } QModelIndex PathActionEditorTreeModel::index(int row, int column, const QModelIndex &parent) - const +const { - if (!hasIndex(row, column, parent)) + if (!hasIndex(row, column, parent)) { return QModelIndex(); + } TreeItem *parentItem; - if (!parent.isValid()) + if (!parent.isValid()) { parentItem = m_rootItem; - else - parentItem = static_cast(parent.internalPointer()); + } else { + parentItem = static_cast(parent.internalPointer()); + } TreeItem *childItem = parentItem->child(row); - if (childItem) + if (childItem) { return createIndex(row, column, childItem); - else + } else { return QModelIndex(); + } } QModelIndex PathActionEditorTreeModel::index(TreeItem *item) { - if (item->parent() == 0) + if (item->parent() == 0) { return QModelIndex(); + } QModelIndex root = index(item->parent()); for (int i = 0; i < rowCount(root); ++i) { QModelIndex childIndex = index(i, 0, root); - TreeItem *child = static_cast(childIndex.internalPointer()); - if (child == item) + TreeItem *child = static_cast(childIndex.internalPointer()); + if (child == item) { return childIndex; + } } Q_ASSERT(false); return QModelIndex(); @@ -228,14 +238,16 @@ QModelIndex PathActionEditorTreeModel::index(TreeItem *item) QModelIndex PathActionEditorTreeModel::parent(const QModelIndex &index) const { - if (!index.isValid()) + if (!index.isValid()) { return QModelIndex(); + } - TreeItem *childItem = static_cast(index.internalPointer()); + TreeItem *childItem = static_cast(index.internalPointer()); TreeItem *parentItem = childItem->parent(); - if (parentItem == m_rootItem) + if (parentItem == m_rootItem) { return QModelIndex(); + } return createIndex(parentItem->row(), 0, parentItem); } @@ -243,72 +255,81 @@ QModelIndex PathActionEditorTreeModel::parent(const QModelIndex &index) const int PathActionEditorTreeModel::rowCount(const QModelIndex &parent) const { TreeItem *parentItem; - if (parent.column() > 0) - return 0; - if (!parent.isValid()) + if (parent.column() > 0) { + return 0; + } + + if (!parent.isValid()) { parentItem = m_rootItem; - else - parentItem = static_cast(parent.internalPointer()); + } else { + parentItem = static_cast(parent.internalPointer()); + } return parentItem->childCount(); } int PathActionEditorTreeModel::columnCount(const QModelIndex &parent) const { - if (parent.isValid()) - return static_cast(parent.internalPointer())->columnCount(); - else + if (parent.isValid()) { + return static_cast(parent.internalPointer())->columnCount(); + } else { return m_rootItem->columnCount(); + } } QVariant PathActionEditorTreeModel::data(const QModelIndex &index, int role) const { - if (!index.isValid()) + if (!index.isValid()) { return QVariant(); + } if (index.column() == TreeItem::dataColumn && role == Qt::EditRole) { - TreeItem *item = static_cast(index.internalPointer()); + TreeItem *item = static_cast(index.internalPointer()); return item->data(index.column()); } -// if (role == Qt::DecorationRole) -// return QIcon(":/core/images/openpilot_logo_128.png"); +// if (role == Qt::DecorationRole) +// return QIcon(":/core/images/openpilot_logo_128.png"); if (role == Qt::ToolTipRole) { - TreeItem *item = static_cast(index.internalPointer()); + TreeItem *item = static_cast(index.internalPointer()); return item->description(); } - TreeItem *item = static_cast(index.internalPointer()); + TreeItem *item = static_cast(index.internalPointer()); if (index.column() == 0 && role == Qt::BackgroundRole) { - ObjectTreeItem *objItem = dynamic_cast(item); - if (objItem && objItem->highlighted()) + ObjectTreeItem *objItem = dynamic_cast(item); + if (objItem && objItem->highlighted()) { return QVariant(m_recentlyUpdatedColor); + } } if (index.column() == TreeItem::dataColumn && role == Qt::BackgroundRole) { - FieldTreeItem *fieldItem = dynamic_cast(item); - if (fieldItem && fieldItem->highlighted()) + FieldTreeItem *fieldItem = dynamic_cast(item); + if (fieldItem && fieldItem->highlighted()) { return QVariant(m_recentlyUpdatedColor); - if (fieldItem && fieldItem->changed()) + } + if (fieldItem && fieldItem->changed()) { return QVariant(m_manuallyChangedColor); + } } - if (role != Qt::DisplayRole) + if (role != Qt::DisplayRole) { return QVariant(); + } if (index.column() == TreeItem::dataColumn) { - EnumFieldTreeItem *fieldItem = dynamic_cast(item); + EnumFieldTreeItem *fieldItem = dynamic_cast(item); if (fieldItem) { int enumIndex = fieldItem->data(index.column()).toInt(); return fieldItem->enumOptions(enumIndex); } else { - ActionFieldTreeItem *afieldItem = dynamic_cast(item); + ActionFieldTreeItem *afieldItem = dynamic_cast(item); if (afieldItem) { int enumIndex = afieldItem->data(index.column()).toInt(); return afieldItem->enumOptions(enumIndex); } - } + } } return item->data(index.column()); @@ -317,7 +338,7 @@ QVariant PathActionEditorTreeModel::data(const QModelIndex &index, int role) con bool PathActionEditorTreeModel::setData(const QModelIndex &index, const QVariant & value, int role) { Q_UNUSED(role) - TreeItem *item = static_cast(index.internalPointer()); + TreeItem * item = static_cast(index.internalPointer()); item->setData(value, index.column()); return true; } @@ -325,23 +346,26 @@ bool PathActionEditorTreeModel::setData(const QModelIndex &index, const QVariant Qt::ItemFlags PathActionEditorTreeModel::flags(const QModelIndex &index) const { - if (!index.isValid()) + if (!index.isValid()) { return 0; + } if (index.column() == TreeItem::dataColumn) { - TreeItem *item = static_cast(index.internalPointer()); - if (item->isEditable()) + TreeItem *item = static_cast(index.internalPointer()); + if (item->isEditable()) { return Qt::ItemIsEnabled | Qt::ItemIsSelectable | Qt::ItemIsEditable; + } } return Qt::ItemIsEnabled | Qt::ItemIsSelectable; } QVariant PathActionEditorTreeModel::headerData(int section, Qt::Orientation orientation, - int role) const + int role) const { - if (orientation == Qt::Horizontal && role == Qt::DisplayRole) + if (orientation == Qt::Horizontal && role == Qt::DisplayRole) { return m_rootItem->data(section); + } return QVariant(); } @@ -349,21 +373,23 @@ QVariant PathActionEditorTreeModel::headerData(int section, Qt::Orientation orie void PathActionEditorTreeModel::updateHighlight(TreeItem *item) { QModelIndex itemIndex = index(item); + Q_ASSERT(itemIndex != QModelIndex()); emit dataChanged(itemIndex, itemIndex.sibling(itemIndex.row(), TreeItem::dataColumn)); // update uavobject if any - go up the tree until there is ObjectTreeItem *objItem = 0; - TreeItem *searchItem = item; + TreeItem *searchItem = item; while (searchItem) { - objItem = dynamic_cast(searchItem); - if (objItem) + objItem = dynamic_cast(searchItem); + if (objItem) { break; + } searchItem = searchItem->parent(); } if (objItem) { - item->apply(); - objItem->apply(); + item->apply(); + objItem->apply(); UAVObject *obj = objItem->object(); Q_ASSERT(obj); obj->updated(); @@ -373,23 +399,22 @@ void PathActionEditorTreeModel::updateHighlight(TreeItem *item) void PathActionEditorTreeModel::highlightUpdatedObject(UAVObject *obj) { Q_ASSERT(obj); - if (obj->getName().compare("Waypoint")==0) { + if (obj->getName().compare("Waypoint") == 0) { m_waypointsTree->update(); - //emit dataChanged(index(m_waypointsTree), index(m_waypointsTree)); - } else if (obj->getName().compare("PathAction")==0) { + // emit dataChanged(index(m_waypointsTree), index(m_waypointsTree)); + } else if (obj->getName().compare("PathAction") == 0) { m_pathactionsTree->update(); - //emit dataChanged(index(m_pathactionsTree), index(m_pathactionsTree)); + // emit dataChanged(index(m_pathactionsTree), index(m_pathactionsTree)); } } void PathActionEditorTreeModel::newInstance(UAVObject *obj) { - - if (obj->getName().compare("Waypoint")==0) { - addInstance(obj,m_waypointsTree); + if (obj->getName().compare("Waypoint") == 0) { + addInstance(obj, m_waypointsTree); m_waypointsTree->update(); - } else if (obj->getName().compare("PathAction")==0) { - addInstance(obj,m_pathactionsTree); + } else if (obj->getName().compare("PathAction") == 0) { + addInstance(obj, m_pathactionsTree); m_pathactionsTree->update(); } updateActions(); @@ -400,8 +425,10 @@ void PathActionEditorTreeModel::objUpdated(UAVObject *obj) { quint16 index = m_objManager->getObject("WaypointActive")->getField("Index")->getValue().toInt(); quint16 action; - foreach (TreeItem *child,m_waypointsTree->treeChildren()) { - ObjectTreeItem *objItem = dynamic_cast(child); + + foreach(TreeItem * child, m_waypointsTree->treeChildren()) { + ObjectTreeItem *objItem = dynamic_cast(child); + if (index == objItem->object()->getInstID()) { child->setActive(true); action = objItem->object()->getField("Action")->getValue().toInt(); @@ -409,8 +436,9 @@ void PathActionEditorTreeModel::objUpdated(UAVObject *obj) child->setActive(false); } } - foreach (TreeItem *child,m_pathactionsTree->treeChildren()) { - ObjectTreeItem *objItem = dynamic_cast(child); + foreach(TreeItem * child, m_pathactionsTree->treeChildren()) { + ObjectTreeItem *objItem = dynamic_cast(child); + if (action == objItem->object()->getInstID()) { child->setActive(true); } else { @@ -420,4 +448,3 @@ void PathActionEditorTreeModel::objUpdated(UAVObject *obj) updateActions(); emit layoutChanged(); } - diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.h b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.h index f34c30a41..30300bacb 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,9 +44,8 @@ class UAVObjectManager; class QSignalMapper; class QTimer; -class PathActionEditorTreeModel : public QAbstractItemModel -{ -Q_OBJECT +class PathActionEditorTreeModel : public QAbstractItemModel { + Q_OBJECT public: explicit PathActionEditorTreeModel(QObject *parent = 0); ~PathActionEditorTreeModel(); @@ -62,8 +61,14 @@ public: int rowCount(const QModelIndex &parent = QModelIndex()) const; int columnCount(const QModelIndex &parent = QModelIndex()) const; - void setRecentlyUpdatedColor(QColor color) { m_recentlyUpdatedColor = color; } - void setManuallyChangedColor(QColor color) { m_manuallyChangedColor = color; } + void setRecentlyUpdatedColor(QColor color) + { + m_recentlyUpdatedColor = color; + } + void setManuallyChangedColor(QColor color) + { + m_manuallyChangedColor = color; + } signals: @@ -73,7 +78,7 @@ public slots: private slots: void highlightUpdatedObject(UAVObject *obj); - void updateHighlight(TreeItem*); + void updateHighlight(TreeItem *); private: QModelIndex index(TreeItem *item); @@ -81,7 +86,7 @@ private: void addSingleField(int index, UAVObjectField *field, TreeItem *parent); void addInstance(UAVObject *obj, TreeItem *parent); - //QString updateMode(quint8 updateMode); + // QString updateMode(quint8 updateMode); void setupModelData(); void updateActions(); diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.cpp index 125545cb5..2fe0b7717 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.cpp @@ -10,37 +10,36 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "treeitem.h" TreeItem::TreeItem(const QList &data, TreeItem *parent) : - QObject(0), - m_data(data), - m_parent(parent), - m_highlight(false), - m_changed(false) -{ -} + QObject(0), + m_data(data), + m_parent(parent), + m_highlight(false), + m_changed(false) +{} TreeItem::TreeItem(const QVariant &data, TreeItem *parent) : - QObject(0), - m_parent(parent), - m_highlight(false), - m_changed(false) + QObject(0), + m_parent(parent), + m_highlight(false), + m_changed(false) { m_data << data << "" << ""; } @@ -73,8 +72,9 @@ int TreeItem::childCount() const } int TreeItem::row() const { - if (m_parent) - return m_parent->m_children.indexOf(const_cast(this)); + if (m_parent) { + return m_parent->m_children.indexOf(const_cast(this)); + } return 0; } @@ -94,29 +94,34 @@ void TreeItem::setData(QVariant value, int column) m_data.replace(column, value); } -void TreeItem::update() { - foreach(TreeItem *child, treeChildren()) - child->update(); +void TreeItem::update() +{ + foreach(TreeItem * child, treeChildren()) + child->update(); } -void TreeItem::apply() { - foreach(TreeItem *child, treeChildren()) - child->apply(); +void TreeItem::apply() +{ + foreach(TreeItem * child, treeChildren()) + child->apply(); } -void TreeItem::setHighlight(bool highlight) { - //m_highlight = highlight; +void TreeItem::setHighlight(bool highlight) +{ + // m_highlight = highlight; m_changed = false; } -void TreeItem::setActive(bool highlight) { +void TreeItem::setActive(bool highlight) +{ m_highlight = highlight; - foreach(TreeItem *child, treeChildren()) - child->setActive(highlight); + foreach(TreeItem * child, treeChildren()) + child->setActive(highlight); } -void TreeItem::removeHighlight() { +void TreeItem::removeHighlight() +{ m_highlight = false; update(); } diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.h b/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.h index 7a39023c5..376b24163 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,9 +36,8 @@ #include -class TreeItem : public QObject -{ -Q_OBJECT +class TreeItem : public QObject { + Q_OBJECT public: TreeItem(const QList &data, TreeItem *parent = 0); TreeItem(const QVariant &data, TreeItem *parent = 0); @@ -48,14 +47,22 @@ public: void insert(int index, TreeItem *child); TreeItem *child(int row); - inline QList treeChildren() const { return m_children; } + inline QList treeChildren() const + { + return m_children; + } int childCount() const; int columnCount() const; QVariant data(int column = 1) const; - QString description() { return m_description; } - void setDescription(QString d) { // Split around 40 characters - int idx = d.indexOf(" ",40); - d.insert(idx,QString("
")); + QString description() + { + return m_description; + } + void setDescription(QString d) // Split around 40 characters + { + int idx = d.indexOf(" ", 40); + + d.insert(idx, QString("
")); d.remove("@Ref", Qt::CaseInsensitive); m_description = d; } @@ -63,27 +70,47 @@ public: // other columns are initialized in constructor virtual void setData(QVariant value, int column = 1); int row() const; - TreeItem *parent() { return m_parent; } - void setParentTree(TreeItem *parent) { m_parent = parent; } - inline virtual bool isEditable() { return false; } + TreeItem *parent() + { + return m_parent; + } + void setParentTree(TreeItem *parent) + { + m_parent = parent; + } + inline virtual bool isEditable() + { + return false; + } virtual void update(); virtual void apply(); - inline bool highlighted() { return m_highlight; } + inline bool highlighted() + { + return m_highlight; + } void setHighlight(bool highlight); void setActive(bool highlight); - inline bool changed() { return m_changed; } - inline void setChanged(bool changed) { m_changed = changed; if(changed) emit updateHighlight(this); } + inline bool changed() + { + return m_changed; + } + inline void setChanged(bool changed) + { + m_changed = changed; if (changed) { + emit updateHighlight(this); + } + } signals: - void updateHighlight(TreeItem*); + void updateHighlight(TreeItem *); private slots: void removeHighlight(); private: - QList m_children; + QList m_children; // m_data contains: [0] property name, [1] value, [2] unit QList m_data; QString m_description; @@ -95,20 +122,30 @@ public: private: }; -class TopTreeItem : public TreeItem -{ -Q_OBJECT +class TopTreeItem : public TreeItem { + Q_OBJECT public: - TopTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) { } - TopTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { } + TopTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) {} + TopTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) {} - QList objIds() { return m_objIds; } - void addObjId(quint32 objId) { m_objIds.append(objId); } - void insertObjId(int index, quint32 objId) { m_objIds.insert(index, objId); } - int nameIndex(QString name) { + QList objIds() + { + return m_objIds; + } + void addObjId(quint32 objId) + { + m_objIds.append(objId); + } + void insertObjId(int index, quint32 objId) + { + m_objIds.insert(index, objId); + } + int nameIndex(QString name) + { for (int i = 0; i < childCount(); ++i) { - if (name < child(i)->data(0).toString()) + if (name < child(i)->data(0).toString()) { return i; + } } return childCount(); } @@ -117,72 +154,97 @@ private: QList m_objIds; }; -class ObjectTreeItem : public TreeItem -{ -Q_OBJECT +class ObjectTreeItem : public TreeItem { + Q_OBJECT public: ObjectTreeItem(const QList &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_obj(0) { } + TreeItem(data, parent), m_obj(0) {} ObjectTreeItem(const QVariant &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_obj(0) { } - void setObject(UAVObject *obj) { m_obj = obj; setDescription(obj->getDescription()); } - inline UAVObject *object() { return m_obj; } + TreeItem(data, parent), m_obj(0) {} + void setObject(UAVObject *obj) + { + m_obj = obj; setDescription(obj->getDescription()); + } + inline UAVObject *object() + { + return m_obj; + } private: UAVObject *m_obj; }; -class MetaObjectTreeItem : public ObjectTreeItem -{ -Q_OBJECT +class MetaObjectTreeItem : public ObjectTreeItem { + Q_OBJECT public: MetaObjectTreeItem(UAVObject *obj, const QList &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { setObject(obj); } + ObjectTreeItem(data, parent) + { + setObject(obj); + } MetaObjectTreeItem(UAVObject *obj, const QVariant &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { setObject(obj); } + ObjectTreeItem(data, parent) + { + setObject(obj); + } }; -class DataObjectTreeItem : public ObjectTreeItem -{ -Q_OBJECT +class DataObjectTreeItem : public ObjectTreeItem { + Q_OBJECT public: DataObjectTreeItem(const QList &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { } + ObjectTreeItem(data, parent) {} DataObjectTreeItem(const QVariant &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { } - virtual void apply() { - foreach(TreeItem *child, treeChildren()) { - MetaObjectTreeItem *metaChild = dynamic_cast(child); - if (!metaChild) + ObjectTreeItem(data, parent) {} + virtual void apply() + { + foreach(TreeItem * child, treeChildren()) { + MetaObjectTreeItem *metaChild = dynamic_cast(child); + + if (!metaChild) { child->apply(); + } } } - virtual void update() { - foreach(TreeItem *child, treeChildren()) { - MetaObjectTreeItem *metaChild = dynamic_cast(child); - if (!metaChild) + virtual void update() + { + foreach(TreeItem * child, treeChildren()) { + MetaObjectTreeItem *metaChild = dynamic_cast(child); + + if (!metaChild) { child->update(); + } } } }; -class InstanceTreeItem : public DataObjectTreeItem -{ -Q_OBJECT +class InstanceTreeItem : public DataObjectTreeItem { + Q_OBJECT public: InstanceTreeItem(UAVObject *obj, const QList &data, TreeItem *parent = 0) : - DataObjectTreeItem(data, parent) { setObject(obj); } + DataObjectTreeItem(data, parent) + { + setObject(obj); + } InstanceTreeItem(UAVObject *obj, const QVariant &data, TreeItem *parent = 0) : - DataObjectTreeItem(data, parent) { setObject(obj); } - virtual void apply() { TreeItem::apply(); } - virtual void update() { TreeItem::update(); } + DataObjectTreeItem(data, parent) + { + setObject(obj); + } + virtual void apply() + { + TreeItem::apply(); + } + virtual void update() + { + TreeItem::update(); + } }; -class ArrayFieldTreeItem : public TreeItem -{ -Q_OBJECT +class ArrayFieldTreeItem : public TreeItem { + Q_OBJECT public: - ArrayFieldTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) { } - ArrayFieldTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { } + ArrayFieldTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) {} + ArrayFieldTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) {} }; #endif // TREEITEM_H diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadget.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdgadget.cpp index aee36b74e..6d91e3c88 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadget.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,10 +30,9 @@ #include "pfdgadgetconfiguration.h" PFDGadget::PFDGadget(QString classId, PFDGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} PFDGadget::~PFDGadget() { @@ -41,14 +40,15 @@ PFDGadget::~PFDGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void PFDGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void PFDGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - PFDGadgetConfiguration *m = qobject_cast(config); + PFDGadgetConfiguration *m = qobject_cast(config); + m_widget->setHqFonts(m->getHqFonts()); m_widget->setDialFile(m->dialFile()); m_widget->enableOpenGL(m->useOpenGL()); diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadget.h b/ground/openpilotgcs/src/plugins/pfd/pfdgadget.h index 46864ee4a..c161aa97f 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadget.h +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,15 +38,17 @@ class PFDGadgetWidget; using namespace Core; -class PFDGadget : public Core::IUAVGadget -{ +class PFDGadget : public Core::IUAVGadget { Q_OBJECT public: PFDGadget(QString classId, PFDGadgetWidget *widget, QWidget *parent = 0); ~PFDGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: PFDGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.cpp index 0dae38c58..f05235409 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,18 +32,18 @@ * Loads a saved configuration or defaults if non exist. * */ -PFDGadgetConfiguration::PFDGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +PFDGadgetConfiguration::PFDGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_defaultDial("Unknown"), beSmooth(true) { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { QString dialFile = qSettings->value("dialFile").toString(); useOpenGLFlag = qSettings->value("useOpenGLFlag").toBool(); hqFonts = qSettings->value("hqFonts").toBool(); beSmooth = qSettings->value("beSmooth").toBool(); - m_defaultDial=Utils::PathUtils().InsertDataPath(dialFile); + m_defaultDial = Utils::PathUtils().InsertDataPath(dialFile); } } @@ -54,9 +54,10 @@ PFDGadgetConfiguration::PFDGadgetConfiguration(QString classId, QSettings* qSett IUAVGadgetConfiguration *PFDGadgetConfiguration::clone() { PFDGadgetConfiguration *m = new PFDGadgetConfiguration(this->classId()); - m->m_defaultDial=m_defaultDial; + + m->m_defaultDial = m_defaultDial; m->useOpenGLFlag = useOpenGLFlag; - m->hqFonts = hqFonts; + m->hqFonts = hqFonts; m->beSmooth = beSmooth; return m; } @@ -65,8 +66,10 @@ IUAVGadgetConfiguration *PFDGadgetConfiguration::clone() * Saves a configuration. * */ -void PFDGadgetConfiguration::saveConfig(QSettings* qSettings) const { +void PFDGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ QString dialFile = Utils::PathUtils().RemoveDataPath(m_defaultDial); + qSettings->setValue("dialFile", dialFile); qSettings->setValue("useOpenGLFlag", useOpenGLFlag); qSettings->setValue("hqFonts", hqFonts); diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.h index 3014a0722..9f3152c63 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,25 +32,48 @@ using namespace Core; -class PFDGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT +class PFDGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit PFDGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit PFDGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - //set dial configuration functions - void setDialFile(QString dialFile){m_defaultDial=dialFile;} - void setUseOpenGL(bool flag) { useOpenGLFlag = flag; } - void setHqFonts(bool flag) { hqFonts = flag; } - void setBeSmooth(bool flag) { beSmooth = flag;} + // set dial configuration functions + void setDialFile(QString dialFile) + { + m_defaultDial = dialFile; + } + void setUseOpenGL(bool flag) + { + useOpenGLFlag = flag; + } + void setHqFonts(bool flag) + { + hqFonts = flag; + } + void setBeSmooth(bool flag) + { + beSmooth = flag; + } - //get dial configuration functions - QString dialFile() {return m_defaultDial;} - bool useOpenGL() { return useOpenGLFlag; } - bool getHqFonts() { return hqFonts; } - bool getBeSmooth() { return beSmooth; } + // get dial configuration functions + QString dialFile() + { + return m_defaultDial; + } + bool useOpenGL() + { + return useOpenGLFlag; + } + bool getHqFonts() + { + return hqFonts; + } + bool getBeSmooth() + { + return beSmooth; + } - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.cpp index 5bc51c775..1e3725189 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,29 +32,27 @@ #include PFDGadgetFactory::PFDGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("PFDGadget"), - tr("Primary Flight Display"), - parent) -{ -} + IUAVGadgetFactory(QString("PFDGadget"), + tr("Primary Flight Display"), + parent) +{} PFDGadgetFactory::~PFDGadgetFactory() -{ -} +{} -Core::IUAVGadget* PFDGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *PFDGadgetFactory::createGadget(QWidget *parent) { - PFDGadgetWidget* gadgetWidget = new PFDGadgetWidget(parent); + PFDGadgetWidget *gadgetWidget = new PFDGadgetWidget(parent); + return new PFDGadget(QString("PFDGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *PFDGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *PFDGadgetFactory::createConfiguration(QSettings *qSettings) { return new PFDGadgetConfiguration(QString("PFDGadget"), qSettings); } IOptionsPage *PFDGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new PFDGadgetOptionsPage(qobject_cast(config)); + return new PFDGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.h b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.h index 8bc97e9a9..be7867494 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class PFDGadgetFactory : public IUAVGadgetFactory -{ +class PFDGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: PFDGadgetFactory(QObject *parent = 0); ~PFDGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.cpp index e8dd3378e..3af712dd4 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,24 +38,22 @@ #include PFDGadgetOptionsPage::PFDGadgetOptionsPage(PFDGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *PFDGadgetOptionsPage::createPage(QWidget *parent) { Q_UNUSED(parent); options_page = new Ui::PFDGadgetOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget; - //main layout + // main layout options_page->setupUi(optionsPageWidget); - // Restore the contents from the settings: options_page->svgSourceFile->setExpectedKind(Utils::PathChooser::File); options_page->svgSourceFile->setPromptDialogFilter(tr("SVG image (*.svg)")); @@ -83,7 +81,5 @@ void PFDGadgetOptionsPage::apply() } - void PFDGadgetOptionsPage::finish() -{ -} +{} diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.h index bf30f19c1..83ea51ded 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -40,14 +40,13 @@ class IUAVGadgetConfiguration; class PFDGadgetConfiguration; namespace Ui { - class PFDGadgetOptionsPage; +class PFDGadgetOptionsPage; } using namespace Core; -class PFDGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class PFDGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit PFDGadgetOptionsPage(PFDGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp index edd07ae44..bbd034193 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -36,36 +36,35 @@ PFDGadgetWidget::PFDGadgetWidget(QWidget *parent) : QGraphicsView(parent) { - - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setScene(new QGraphicsScene(this)); -// setRenderHint(QPainter::SmoothPixmapTransform); +// setRenderHint(QPainter::SmoothPixmapTransform); setViewportUpdateMode(QGraphicsView::FullViewportUpdate); - //setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); - //setRenderHints(QPainter::TextAntialiasing); - //setRenderHints(QPainter::HighQualityAntialiasing); + // setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); + // setRenderHints(QPainter::TextAntialiasing); + // setRenderHints(QPainter::HighQualityAntialiasing); - m_renderer = new QSvgRenderer(); + m_renderer = new QSvgRenderer(); - attitudeObj = NULL; - headingObj = NULL; - gcsBatteryObj = NULL; + attitudeObj = NULL; + headingObj = NULL; + gcsBatteryObj = NULL; gpsObj = NULL; - compassBandWidth = 0; + compassBandWidth = 0; pfdError = true; hqFonts = false; - rollTarget = 0; + rollTarget = 0; rollValue = 0; - pitchTarget = 0; - pitchValue = 0; - headingTarget = 0; - headingValue = 0; + pitchTarget = 0; + pitchValue = 0; + headingTarget = 0; + headingValue = 0; groundspeedTarget = 0; - groundspeedValue = 0; - altitudeTarget = 0; - altitudeValue = 0; + groundspeedValue = 0; + altitudeTarget = 0; + altitudeValue = 0; // This timer mechanism makes needles rotate smoothly connect(&dialTimer, SIGNAL(timeout()), this, SLOT(moveNeedles())); @@ -73,8 +72,6 @@ PFDGadgetWidget::PFDGadgetWidget(QWidget *parent) : QGraphicsView(parent) connect(&skyDialTimer, SIGNAL(timeout()), this, SLOT(moveSky())); skyDialTimer.start(30); - - } PFDGadgetWidget::~PFDGadgetWidget() @@ -85,16 +82,19 @@ PFDGadgetWidget::~PFDGadgetWidget() void PFDGadgetWidget::setToolTipPrivate() { - static qint32 updateRate=0; - UAVObject::Metadata mdata=attitudeObj->getMetadata(); - if(mdata.flightTelemetryUpdatePeriod!=updateRate) - this->setToolTip("Current refresh rate:"+QString::number(mdata.flightTelemetryUpdatePeriod)+" miliseconds"+"\nIf you want to change it please edit the AttitudeActual metadata on the object browser."); + static qint32 updateRate = 0; + UAVObject::Metadata mdata = attitudeObj->getMetadata(); + + if (mdata.flightTelemetryUpdatePeriod != updateRate) { + this->setToolTip("Current refresh rate:" + QString::number(mdata.flightTelemetryUpdatePeriod) + " miliseconds" + "\nIf you want to change it please edit the AttitudeActual metadata on the object browser."); + } } /*! - \brief Enables/Disables OpenGL - */ -void PFDGadgetWidget::enableOpenGL(bool flag) { + \brief Enables/Disables OpenGL + */ +void PFDGadgetWidget::enableOpenGL(bool flag) +{ if (flag) { setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); } else { @@ -103,102 +103,110 @@ void PFDGadgetWidget::enableOpenGL(bool flag) { } /*! - \brief Connects the widget to the relevant UAVObjects + \brief Connects the widget to the relevant UAVObjects - Should only be called after the PFD artwork is loaded. - We want: AttitudeActual, FlightBattery, Location. + Should only be called after the PFD artwork is loaded. + We want: AttitudeActual, FlightBattery, Location. - */ -void PFDGadgetWidget::connectNeedles() { - if (attitudeObj != NULL) - disconnect(attitudeObj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updateAttitude(UAVObject*))); + */ +void PFDGadgetWidget::connectNeedles() +{ + if (attitudeObj != NULL) { + disconnect(attitudeObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateAttitude(UAVObject *))); + } - if (headingObj != NULL) - disconnect(headingObj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updateHeading(UAVObject*))); + if (headingObj != NULL) { + disconnect(headingObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateHeading(UAVObject *))); + } - if (gcsBatteryObj != NULL) - disconnect(gcsBatteryObj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updateBattery(UAVObject*))); + if (gcsBatteryObj != NULL) { + disconnect(gcsBatteryObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateBattery(UAVObject *))); + } - if (gpsObj != NULL) - disconnect(gpsObj,SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateGPS(UAVObject*))); + if (gpsObj != NULL) { + disconnect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGPS(UAVObject *))); + } // Safeguard: if artwork did not load properly, don't go further - if (pfdError) - return; + if (pfdError) { + return; + } ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - airspeedObj = dynamic_cast(objManager->getObject("AirspeedActual")); - if (airspeedObj != NULL ) { - connect(airspeedObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateAirspeed(UAVObject*))); + airspeedObj = dynamic_cast(objManager->getObject("AirspeedActual")); + if (airspeedObj != NULL) { + connect(airspeedObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateAirspeed(UAVObject *))); } else { - qDebug() << "Error: Object is unknown (AirspeedActual)."; + qDebug() << "Error: Object is unknown (AirspeedActual)."; } - groundspeedObj = dynamic_cast(objManager->getObject("VelocityActual")); - if (groundspeedObj != NULL ) { - connect(groundspeedObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateGroundspeed(UAVObject*))); + groundspeedObj = dynamic_cast(objManager->getObject("VelocityActual")); + if (groundspeedObj != NULL) { + connect(groundspeedObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGroundspeed(UAVObject *))); } else { - qDebug() << "Error: Object is unknown (VelocityActual)."; + qDebug() << "Error: Object is unknown (VelocityActual)."; } - altitudeObj = dynamic_cast(objManager->getObject("PositionActual")); - if (altitudeObj != NULL ) { - connect(altitudeObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateAltitude(UAVObject*))); + altitudeObj = dynamic_cast(objManager->getObject("PositionActual")); + if (altitudeObj != NULL) { + connect(altitudeObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateAltitude(UAVObject *))); } else { - qDebug() << "Error: Object is unknown (PositionActual)."; - } - - attitudeObj = dynamic_cast(objManager->getObject("AttitudeActual")); - if (attitudeObj != NULL ) { - connect(attitudeObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateAttitude(UAVObject*))); - } else { - qDebug() << "Error: Object is unknown (AttitudeActual)."; - } - - headingObj = dynamic_cast(objManager->getObject("PositionActual")); - if (headingObj != NULL ) { - connect(headingObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateHeading(UAVObject*))); - } else { qDebug() << "Error: Object is unknown (PositionActual)."; - } - - if (gcsGPSStats) { - gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); - if (gpsObj != NULL) { - connect(gpsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateGPS(UAVObject*))); - } else { - qDebug() << "Error: Object is unknown (GPSPosition)."; - } - } - - if (gcsTelemetryArrow || gcsTelemetryStats ) { - // Only register if the PFD wants link stats/status - gcsTelemetryObj = dynamic_cast(objManager->getObject("GCSTelemetryStats")); - if (gcsTelemetryObj != NULL ) { - connect(gcsTelemetryObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateLinkStatus(UAVObject*))); - } else { - qDebug() << "Error: Object is unknown (GCSTelemetryStats)."; - } } - if (gcsBatteryStats) { // Only register if the PFD wants battery display - gcsBatteryObj = dynamic_cast(objManager->getObject("FlightBatteryState")); - if (gcsBatteryObj != NULL ) { - connect(gcsBatteryObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateBattery(UAVObject*))); - } else { + attitudeObj = dynamic_cast(objManager->getObject("AttitudeActual")); + if (attitudeObj != NULL) { + connect(attitudeObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateAttitude(UAVObject *))); + } else { + qDebug() << "Error: Object is unknown (AttitudeActual)."; + } + + headingObj = dynamic_cast(objManager->getObject("PositionActual")); + if (headingObj != NULL) { + connect(headingObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateHeading(UAVObject *))); + } else { + qDebug() << "Error: Object is unknown (PositionActual)."; + } + + if (gcsGPSStats) { + gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + if (gpsObj != NULL) { + connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGPS(UAVObject *))); + } else { + qDebug() << "Error: Object is unknown (GPSPosition)."; + } + } + + if (gcsTelemetryArrow || gcsTelemetryStats) { + // Only register if the PFD wants link stats/status + gcsTelemetryObj = dynamic_cast(objManager->getObject("GCSTelemetryStats")); + if (gcsTelemetryObj != NULL) { + connect(gcsTelemetryObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateLinkStatus(UAVObject *))); + } else { + qDebug() << "Error: Object is unknown (GCSTelemetryStats)."; + } + } + + if (gcsBatteryStats) { // Only register if the PFD wants battery display + gcsBatteryObj = dynamic_cast(objManager->getObject("FlightBatteryState")); + if (gcsBatteryObj != NULL) { + connect(gcsBatteryObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateBattery(UAVObject *))); + } else { qDebug() << "Error: Object is unknown (FlightBatteryState)."; - } - } + } + } } /*! - \brief Updates the GPS stats - */ -void PFDGadgetWidget::updateGPS(UAVObject *object1) { - UAVObjectField* field = object1->getField(QString("Satellites")); - UAVObjectField* field1 = object1->getField(QString("PDOP")); + \brief Updates the GPS stats + */ +void PFDGadgetWidget::updateGPS(UAVObject *object1) +{ + UAVObjectField *field = object1->getField(QString("Satellites")); + UAVObjectField *field1 = object1->getField(QString("PDOP")); + if (field && field1) { QString s = QString("GPS: ") + field->getValue().toString() + "\nPDP: " + field1->getValue().toString(); @@ -210,59 +218,64 @@ void PFDGadgetWidget::updateGPS(UAVObject *object1) { } /*! - \brief Updates the link stats + \brief Updates the link stats */ -void PFDGadgetWidget::updateLinkStatus(UAVObject *object1) { +void PFDGadgetWidget::updateLinkStatus(UAVObject *object1) +{ // TODO: find a way to avoid updating the graphics if the value - // has not changed since the last update + // has not changed since the last update // Double check that the field exists: - QString st = QString("Status"); + QString st = QString("Status"); QString tdr = QString("TxDataRate"); QString rdr = QString("RxDataRate"); - UAVObjectField* field = object1->getField(st); - UAVObjectField* field2 = object1->getField(tdr); - UAVObjectField* field3 = object1->getField(rdr); + UAVObjectField *field = object1->getField(st); + UAVObjectField *field2 = object1->getField(tdr); + UAVObjectField *field3 = object1->getField(rdr); + if (field && field2 && field3) { - QString s = field->getValue().toString(); + QString s = field->getValue().toString(); if (m_renderer->elementExists("gcstelemetry-" + s) && gcsTelemetryArrow) { - gcsTelemetryArrow->setElementId("gcstelemetry-" + s); - } else { // Safeguard - gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); - } + gcsTelemetryArrow->setElementId("gcstelemetry-" + s); + } else { // Safeguard + gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); + } double v1 = field2->getDouble(); double v2 = field3->getDouble(); - s.sprintf("%.0f/%.0f",v1,v2); - if (gcsTelemetryStats) gcsTelemetryStats->setPlainText(s); + s.sprintf("%.0f/%.0f", v1, v2); + if (gcsTelemetryStats) { + gcsTelemetryStats->setPlainText(s); + } } else { qDebug() << "UpdateLinkStatus: Wrong field, maybe an issue with object disconnection ?"; } } /*! - \brief Reads the updated attitude and computes the new display position + \brief Reads the updated attitude and computes the new display position - Resolution is 1 degree roll & 1/7.5 degree pitch. - */ -void PFDGadgetWidget::updateAttitude(UAVObject *object1) { + Resolution is 1 degree roll & 1/7.5 degree pitch. + */ +void PFDGadgetWidget::updateAttitude(UAVObject *object1) +{ setToolTipPrivate(); - UAVObjectField * rollField = object1->getField(QString("Roll")); - UAVObjectField * yawField = object1->getField(QString("Yaw")); - UAVObjectField * pitchField = object1->getField(QString("Pitch")); - if(rollField && yawField && pitchField) { + UAVObjectField *rollField = object1->getField(QString("Roll")); + UAVObjectField *yawField = object1->getField(QString("Yaw")); + UAVObjectField *pitchField = object1->getField(QString("Pitch")); + if (rollField && yawField && pitchField) { // These factors assume some things about the PFD SVG, namely: // - Roll, Pitch and Heading value in degrees // - Pitch lines are 300px high for a +20/-20 range, which means - // 7.5 pixels per pitch degree. + // 7.5 pixels per pitch degree. // TODO: loosen this constraint and only require a +/- 20 deg range, - // and compute the height from the SVG element. + // and compute the height from the SVG element. // Also: keep the integer value only, to avoid unnecessary redraws - rollTarget = -floor(rollField->getDouble()*10)/10; + rollTarget = -floor(rollField->getDouble() * 10) / 10; if ((rollTarget - rollValue) > 180) { rollValue += 360; } else if (((rollTarget - rollValue) < -180)) { rollValue -= 360; } - pitchTarget = floor(pitchField->getDouble()*7.5); + pitchTarget = floor(pitchField->getDouble() * 7.5); // These factors assume some things about the PFD SVG, namely: // - Heading value in degrees @@ -271,51 +284,55 @@ void PFDGadgetWidget::updateAttitude(UAVObject *object1) { // Corvus Corax: "If you want a smooth transition between two angles, It is usually solved that by substracting // one from another, and if the result is >180 or <-180 I substract (respectively add) 360 degrees // to it. That way you always get the "shorter difference" to turn in." - double fac = compassBandWidth/540; - headingTarget = yawField->getDouble()*(-fac); - if (headingTarget != headingTarget) + double fac = compassBandWidth / 540; + headingTarget = yawField->getDouble() * (-fac); + if (headingTarget != headingTarget) { headingTarget = headingValue; // NaN checking. - if ((headingValue - headingTarget)/fac > 180) { - headingTarget += 360*fac; - } else if (((headingValue - headingTarget)/fac < -180)) { - headingTarget -= 360*fac; } - headingTarget = floor(headingTarget*10)/10; // Avoid stupid redraws + if ((headingValue - headingTarget) / fac > 180) { + headingTarget += 360 * fac; + } else if (((headingValue - headingTarget) / fac < -180)) { + headingTarget -= 360 * fac; + } + headingTarget = floor(headingTarget * 10) / 10; // Avoid stupid redraws - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); // Rearm the dial Timer which might be stopped. - + } } else { qDebug() << "Unable to get one of the fields for attitude update"; } } /*! - \brief Updates the compass reading and speed dial. + \brief Updates the compass reading and speed dial. - This also updates speed & altitude according to PositionActual data. + This also updates speed & altitude according to PositionActual data. Note: the speed dial shows the ground speed at the moment, because there is no airspeed by default. Should become configurable in a future gadget release (TODO) - */ -void PFDGadgetWidget::updateHeading(UAVObject *object) { + */ +void PFDGadgetWidget::updateHeading(UAVObject *object) +{ Q_UNUSED(object); } /*! - \brief Called by updates to @PositionActual to compute groundspeed from velocity - */ -void PFDGadgetWidget::updateGroundspeed(UAVObject *object) { - UAVObjectField* northField = object->getField("North"); - UAVObjectField* eastField = object->getField("East"); + \brief Called by updates to @PositionActual to compute groundspeed from velocity + */ +void PFDGadgetWidget::updateGroundspeed(UAVObject *object) +{ + UAVObjectField *northField = object->getField("North"); + UAVObjectField *eastField = object->getField("East"); + if (northField && eastField) { - double val = floor(sqrt(pow(northField->getDouble(),2) + pow(eastField->getDouble(),2))*10)/10; - groundspeedTarget = 3.6*val*speedScaleHeight/30; + double val = floor(sqrt(pow(northField->getDouble(), 2) + pow(eastField->getDouble(), 2)) * 10) / 10; + groundspeedTarget = 3.6 * val * speedScaleHeight / 30; - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); // Rearm the dial Timer which might be stopped. - + } } else { qDebug() << "UpdateHeading: Wrong field, maybe an issue with object disconnection ?"; } @@ -323,32 +340,36 @@ void PFDGadgetWidget::updateGroundspeed(UAVObject *object) { /*! - \brief Called by updates to @AirspeedActual - */ -void PFDGadgetWidget::updateAirspeed(UAVObject *object) { - UAVObjectField* airspeedField = object->getField("CalibratedAirspeed"); + \brief Called by updates to @AirspeedActual + */ +void PFDGadgetWidget::updateAirspeed(UAVObject *object) +{ + UAVObjectField *airspeedField = object->getField("CalibratedAirspeed"); + if (airspeedField) { airspeedTarget = airspeedField->getDouble(); - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); // Rearm the dial Timer which might be stopped. - + } } else { qDebug() << "UpdateHeading: Wrong field, maybe an issue with object disconnection ?"; } } /*! - \brief Called by the @ref PositionActual updates to show altitude - */ -void PFDGadgetWidget::updateAltitude(UAVObject *object) { - UAVObjectField* downField = object->getField("Down"); + \brief Called by the @ref PositionActual updates to show altitude + */ +void PFDGadgetWidget::updateAltitude(UAVObject *object) +{ + UAVObjectField *downField = object->getField("Down"); + if (downField) { altitudeTarget = -downField->getDouble(); - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); // Rearm the dial Timer which might be stopped. - + } } else { qDebug() << "Unable to get field for altitude update. Obj: " << object->getName(); } @@ -356,22 +377,24 @@ void PFDGadgetWidget::updateAltitude(UAVObject *object) { /*! - \brief Called by the UAVObject which got updated - */ -void PFDGadgetWidget::updateBattery(UAVObject *object1) { + \brief Called by the UAVObject which got updated + */ +void PFDGadgetWidget::updateBattery(UAVObject *object1) +{ // Double check that the field exists: QString voltage = QString("Voltage"); QString current = QString("Current"); - QString energy = QString("ConsumedEnergy"); - UAVObjectField* field = object1->getField(voltage); - UAVObjectField* field2 = object1->getField(current); - UAVObjectField* field3 = object1->getField(energy); + QString energy = QString("ConsumedEnergy"); + UAVObjectField *field = object1->getField(voltage); + UAVObjectField *field2 = object1->getField(current); + UAVObjectField *field3 = object1->getField(energy); + if (field && field2 && field3) { - QString s = QString(); - double v0 = field->getDouble(); + QString s = QString(); + double v0 = field->getDouble(); double v1 = field2->getDouble(); double v2 = field3->getDouble(); - s.sprintf("%.2fV\n%.2fA\n%.0fmAh",v0,v1,v2); + s.sprintf("%.2fV\n%.2fA\n%.0fmAh", v0, v1, v2); if (s != batString) { gcsBatteryStats->setPlainText(s); batString = s; @@ -379,21 +402,20 @@ void PFDGadgetWidget::updateBattery(UAVObject *object1) { } else { qDebug() << "UpdateBattery: Wrong field, maybe an issue with object disconnection ?"; } - } /*! - \brief Sets up the PFD from the SVG master file. + \brief Sets up the PFD from the SVG master file. - Initializes the display, and does all the one-time calculations. - */ + Initializes the display, and does all the one-time calculations. + */ void PFDGadgetWidget::setDialFile(QString dfn) { - QGraphicsScene *l_scene = scene(); - setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); - if (QFile::exists(dfn) && m_renderer->load(dfn) && m_renderer->isValid()) - { + QGraphicsScene *l_scene = scene(); + + setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); + if (QFile::exists(dfn) && m_renderer->load(dfn) && m_renderer->isValid()) { /* The PFD element IDs are fixed, not like with the analog dial. - Background: background - Foreground: foreground (contains all fixed elements, including plane) @@ -416,351 +438,369 @@ void PFDGadgetWidget::setDialFile(QString dfn) - GPS status text: gps-txt - Battery stats: battery-txt */ - l_scene->clear(); // Deletes all items contained in the scene as well. - m_background = new CachedSvgItem(); - // All other items will be clipped to the shape of the background - m_background->setFlags(QGraphicsItem::ItemClipsChildrenToShape| - QGraphicsItem::ItemClipsToShape); - m_background->setSharedRenderer(m_renderer); - m_background->setElementId("background"); - l_scene->addItem(m_background); + l_scene->clear(); // Deletes all items contained in the scene as well. + m_background = new CachedSvgItem(); + // All other items will be clipped to the shape of the background + m_background->setFlags(QGraphicsItem::ItemClipsChildrenToShape | + QGraphicsItem::ItemClipsToShape); + m_background->setSharedRenderer(m_renderer); + m_background->setElementId("background"); + l_scene->addItem(m_background); - m_world = new CachedSvgItem(); - m_world->setParentItem(m_background); - m_world->setSharedRenderer(m_renderer); - m_world->setElementId("world"); - l_scene->addItem(m_world); + m_world = new CachedSvgItem(); + m_world->setParentItem(m_background); + m_world->setSharedRenderer(m_renderer); + m_world->setElementId("world"); + l_scene->addItem(m_world); - // red Roll scale: rollscale - m_rollscale = new CachedSvgItem(); - m_rollscale->setSharedRenderer(m_renderer); - m_rollscale->setElementId("rollscale"); - l_scene->addItem(m_rollscale); + // red Roll scale: rollscale + m_rollscale = new CachedSvgItem(); + m_rollscale->setSharedRenderer(m_renderer); + m_rollscale->setElementId("rollscale"); + l_scene->addItem(m_rollscale); - // Home point: - m_homewaypoint = new CachedSvgItem(); - // Next point: - m_nextwaypoint = new CachedSvgItem(); - // Home point bearing: - m_homepointbearing = new CachedSvgItem(); - // Next point bearing: - m_nextpointbearing = new CachedSvgItem(); + // Home point: + m_homewaypoint = new CachedSvgItem(); + // Next point: + m_nextwaypoint = new CachedSvgItem(); + // Home point bearing: + m_homepointbearing = new CachedSvgItem(); + // Next point bearing: + m_nextpointbearing = new CachedSvgItem(); - QGraphicsSvgItem *m_foreground = new CachedSvgItem(); - m_foreground->setParentItem(m_background); - m_foreground->setSharedRenderer(m_renderer); - m_foreground->setElementId("foreground"); - l_scene->addItem(m_foreground); + QGraphicsSvgItem *m_foreground = new CachedSvgItem(); + m_foreground->setParentItem(m_background); + m_foreground->setSharedRenderer(m_renderer); + m_foreground->setElementId("foreground"); + l_scene->addItem(m_foreground); - //////////////////// - // Compass - //////////////////// - // Get the default location of the Compass: - QMatrix compassMatrix = m_renderer->matrixForElement("compass"); - qreal startX = compassMatrix.mapRect(m_renderer->boundsOnElement("compass")).x(); - qreal startY = compassMatrix.mapRect(m_renderer->boundsOnElement("compass")).y(); - // Then once we have the initial location, we can put it - // into a QGraphicsSvgItem which we will display at the same - // place: we do this so that the heading scale can be clipped to - // the compass dial region. - m_compass = new CachedSvgItem(); - m_compass->setSharedRenderer(m_renderer); - m_compass->setElementId("compass"); - m_compass->setFlags(QGraphicsItem::ItemClipsChildrenToShape| + //////////////////// + // Compass + //////////////////// + // Get the default location of the Compass: + QMatrix compassMatrix = m_renderer->matrixForElement("compass"); + qreal startX = compassMatrix.mapRect(m_renderer->boundsOnElement("compass")).x(); + qreal startY = compassMatrix.mapRect(m_renderer->boundsOnElement("compass")).y(); + // Then once we have the initial location, we can put it + // into a QGraphicsSvgItem which we will display at the same + // place: we do this so that the heading scale can be clipped to + // the compass dial region. + m_compass = new CachedSvgItem(); + m_compass->setSharedRenderer(m_renderer); + m_compass->setElementId("compass"); + m_compass->setFlags(QGraphicsItem::ItemClipsChildrenToShape | + QGraphicsItem::ItemClipsToShape); + l_scene->addItem(m_compass); + QTransform matrix; + matrix.translate(startX, startY); + m_compass->setTransform(matrix, false); + + // Now place the compass scale inside: + m_compassband = new CachedSvgItem(); + m_compassband->setSharedRenderer(m_renderer); + m_compassband->setElementId("compass-band"); + m_compassband->setParentItem(m_compass); + l_scene->addItem(m_compassband); + matrix.reset(); + // Note: the compass band has to be a path, which means all text elements have to be + // converted, ortherwise boundsOnElement does not compute the height correctly + // if the highest element is a text element. This is a Qt Bug as far as I can tell. + + // compass-scale is the while bottom line inside the band: using the band's width + // includes half the width of the letters, which causes errors: + compassBandWidth = m_renderer->boundsOnElement("compass-scale").width(); + + //////////////////// + // Speed + //////////////////// + // Speedometer on the left hand: + // + compassMatrix = m_renderer->matrixForElement("speed-bg"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).y(); + QGraphicsSvgItem *verticalbg = new CachedSvgItem(); + verticalbg->setSharedRenderer(m_renderer); + verticalbg->setElementId("speed-bg"); + verticalbg->setFlags(QGraphicsItem::ItemClipsChildrenToShape | QGraphicsItem::ItemClipsToShape); - l_scene->addItem(m_compass); - QTransform matrix; - matrix.translate(startX,startY); - m_compass->setTransform(matrix,false); + l_scene->addItem(verticalbg); + matrix.reset(); + matrix.translate(startX, startY); + verticalbg->setTransform(matrix, false); - // Now place the compass scale inside: - m_compassband = new CachedSvgItem(); - m_compassband->setSharedRenderer(m_renderer); - m_compassband->setElementId("compass-band"); - m_compassband->setParentItem(m_compass); - l_scene->addItem(m_compassband); - matrix.reset(); - // Note: the compass band has to be a path, which means all text elements have to be - // converted, ortherwise boundsOnElement does not compute the height correctly - // if the highest element is a text element. This is a Qt Bug as far as I can tell. + // Note: speed-scale should contain exactly 6 major ticks + // for 30km/h + m_speedscale = new QGraphicsItemGroup(); + m_speedscale->setParentItem(verticalbg); - // compass-scale is the while bottom line inside the band: using the band's width - // includes half the width of the letters, which causes errors: - compassBandWidth = m_renderer->boundsOnElement("compass-scale").width(); + QGraphicsSvgItem *speedscalelines = new CachedSvgItem(); + speedscalelines->setSharedRenderer(m_renderer); + speedscalelines->setElementId("speed-scale"); + speedScaleHeight = m_renderer->matrixForElement("speed-scale").mapRect( + m_renderer->boundsOnElement("speed-scale")).height(); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).width(); + startX -= m_renderer->matrixForElement("speed-scale").mapRect( + m_renderer->boundsOnElement("speed-scale")).width(); + matrix.reset(); + matrix.translate(startX, 0); + speedscalelines->setTransform(matrix, false); + // Quick way to reposition the item before putting it in the group: + speedscalelines->setParentItem(verticalbg); + m_speedscale->addToGroup(speedscalelines); // (reparents the item) - //////////////////// - // Speed - //////////////////// - // Speedometer on the left hand: - // - compassMatrix = m_renderer->matrixForElement("speed-bg"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).y(); - QGraphicsSvgItem *verticalbg = new CachedSvgItem(); - verticalbg->setSharedRenderer(m_renderer); - verticalbg->setElementId("speed-bg"); - verticalbg->setFlags(QGraphicsItem::ItemClipsChildrenToShape| + // Add the scale text elements: + QGraphicsTextItem *speed0 = new QGraphicsTextItem("0"); + speed0->setDefaultTextColor(QColor("White")); + speed0->setFont(QFont("Arial", (int)speedScaleHeight / 30)); + if (hqFonts) { + speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + matrix.reset(); + matrix.translate(compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).width() / 10, -speedScaleHeight / 30); + speed0->setTransform(matrix, false); + speed0->setParentItem(verticalbg); + m_speedscale->addToGroup(speed0); + for (int i = 0; i < 6; i++) { + speed0 = new QGraphicsTextItem(""); + speed0->setDefaultTextColor(QColor("White")); + speed0->setFont(QFont("Arial", (int)speedScaleHeight / 30)); + speed0->setPlainText(QString().setNum(i * 5 + 1)); + if (hqFonts) { + speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + matrix.translate(0, speedScaleHeight / 6); + speed0->setTransform(matrix, false); + speed0->setParentItem(verticalbg); + m_speedscale->addToGroup(speed0); + } + // Now vertically center the speed scale on the speed background + QRectF rectB = verticalbg->boundingRect(); + QRectF rectN = speedscalelines->boundingRect(); + m_speedscale->setPos(0, rectB.height() / 2 - rectN.height() / 2 - rectN.height() / 6); + + // Isolate the speed window and put it above the speed scale + compassMatrix = m_renderer->matrixForElement("speed-window"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).y(); + qreal speedWindowHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).height(); + QGraphicsSvgItem *speedwindow = new CachedSvgItem(); + speedwindow->setSharedRenderer(m_renderer); + speedwindow->setElementId("speed-window"); + speedwindow->setFlags(QGraphicsItem::ItemClipsChildrenToShape | + QGraphicsItem::ItemClipsToShape); + l_scene->addItem(speedwindow); + matrix.reset(); + matrix.translate(startX, startY); + speedwindow->setTransform(matrix, false); + + // Last create a Text Item at the location of the speed window + // and save it for future updates: + m_speedtext = new QGraphicsTextItem("0000"); + m_speedtext->setDefaultTextColor(QColor("White")); + m_speedtext->setFont(QFont("Arial", (int)speedWindowHeight / 2)); + if (hqFonts) { + m_speedtext->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + m_speedtext->setParentItem(speedwindow); + + //////////////////// + // Altitude + //////////////////// + // Right hand, very similar to speed + compassMatrix = m_renderer->matrixForElement("altitude-bg"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-bg")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-bg")).y(); + verticalbg = new CachedSvgItem(); + verticalbg->setSharedRenderer(m_renderer); + verticalbg->setElementId("altitude-bg"); + verticalbg->setFlags(QGraphicsItem::ItemClipsChildrenToShape | QGraphicsItem::ItemClipsToShape); - l_scene->addItem(verticalbg); - matrix.reset(); - matrix.translate(startX,startY); - verticalbg->setTransform(matrix,false); + l_scene->addItem(verticalbg); + matrix.reset(); + matrix.translate(startX, startY); + verticalbg->setTransform(matrix, false); - // Note: speed-scale should contain exactly 6 major ticks - // for 30km/h - m_speedscale = new QGraphicsItemGroup(); - m_speedscale->setParentItem(verticalbg); + // Note: altitude-scale should contain exactly 6 major ticks + // for 30 meters + m_altitudescale = new QGraphicsItemGroup(); + m_altitudescale->setParentItem(verticalbg); - QGraphicsSvgItem *speedscalelines = new CachedSvgItem(); - speedscalelines->setSharedRenderer(m_renderer); - speedscalelines->setElementId("speed-scale"); - speedScaleHeight = m_renderer->matrixForElement("speed-scale").mapRect( - m_renderer->boundsOnElement("speed-scale")).height(); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).width(); - startX -= m_renderer->matrixForElement("speed-scale").mapRect( - m_renderer->boundsOnElement("speed-scale")).width(); - matrix.reset(); - matrix.translate(startX,0); - speedscalelines->setTransform(matrix,false); - // Quick way to reposition the item before putting it in the group: - speedscalelines->setParentItem(verticalbg); - m_speedscale->addToGroup(speedscalelines); // (reparents the item) + QGraphicsSvgItem *altitudescalelines = new CachedSvgItem(); + altitudescalelines->setSharedRenderer(m_renderer); + altitudescalelines->setElementId("altitude-scale"); + altitudeScaleHeight = m_renderer->matrixForElement("altitude-scale").mapRect( + m_renderer->boundsOnElement("altitude-scale")).height(); + // Quick way to reposition the item before putting it in the group: + altitudescalelines->setParentItem(verticalbg); + m_altitudescale->addToGroup(altitudescalelines); // (reparents the item) - // Add the scale text elements: - QGraphicsTextItem *speed0 = new QGraphicsTextItem("0"); - speed0->setDefaultTextColor(QColor("White")); - speed0->setFont(QFont("Arial",(int) speedScaleHeight/30)); - if (hqFonts) speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - matrix.reset(); - matrix.translate(compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).width()/10,-speedScaleHeight/30); - speed0->setTransform(matrix,false); - speed0->setParentItem(verticalbg); - m_speedscale->addToGroup(speed0); - for (int i=0; i<6;i++) { - speed0 = new QGraphicsTextItem(""); - speed0->setDefaultTextColor(QColor("White")); - speed0->setFont(QFont("Arial",(int) speedScaleHeight/30)); - speed0->setPlainText(QString().setNum(i*5+1)); - if (hqFonts) speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - matrix.translate(0,speedScaleHeight/6); - speed0->setTransform(matrix,false); - speed0->setParentItem(verticalbg); - m_speedscale->addToGroup(speed0); - } - // Now vertically center the speed scale on the speed background - QRectF rectB = verticalbg->boundingRect(); - QRectF rectN = speedscalelines->boundingRect(); - m_speedscale->setPos(0,rectB.height()/2-rectN.height()/2-rectN.height()/6); + // Add the scale text elements: + speed0 = new QGraphicsTextItem("XXXX"); + speed0->setDefaultTextColor(QColor("White")); + speed0->setFont(QFont("Arial", (int)altitudeScaleHeight / 30)); + if (hqFonts) { + speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + matrix.reset(); + matrix.translate(m_renderer->matrixForElement("altitude-scale").mapRect(m_renderer->boundsOnElement("altitude-scale")).width() + + m_renderer->matrixForElement("altitude-bg").mapRect(m_renderer->boundsOnElement("altitude-bg")).width() / 10, -altitudeScaleHeight / 30); + speed0->setTransform(matrix, false); + speed0->setParentItem(verticalbg); + m_altitudescale->addToGroup(speed0); + for (int i = 0; i < 6; i++) { + speed0 = new QGraphicsTextItem("XXXX"); + speed0->setDefaultTextColor(QColor("White")); + speed0->setFont(QFont("Arial", (int)altitudeScaleHeight / 30)); + speed0->setPlainText(QString().setNum(i * 5 + 1)); + if (hqFonts) { + speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + matrix.translate(0, altitudeScaleHeight / 6); + speed0->setTransform(matrix, false); + speed0->setParentItem(verticalbg); + m_altitudescale->addToGroup(speed0); + } + // Now vertically center the speed scale on the speed background + rectB = verticalbg->boundingRect(); + rectN = altitudescalelines->boundingRect(); + m_altitudescale->setPos(0, rectB.height() / 2 - rectN.height() / 2 - rectN.height() / 6); - // Isolate the speed window and put it above the speed scale - compassMatrix = m_renderer->matrixForElement("speed-window"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).y(); - qreal speedWindowHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).height(); - QGraphicsSvgItem *speedwindow = new CachedSvgItem(); - speedwindow->setSharedRenderer(m_renderer); - speedwindow->setElementId("speed-window"); - speedwindow->setFlags(QGraphicsItem::ItemClipsChildrenToShape| - QGraphicsItem::ItemClipsToShape); - l_scene->addItem(speedwindow); - matrix.reset(); - matrix.translate(startX,startY); - speedwindow->setTransform(matrix,false); + // Isolate the Altitude window and put it above the altitude scale + compassMatrix = m_renderer->matrixForElement("altitude-window"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).y(); + qreal altitudeWindowHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).height(); + QGraphicsSvgItem *altitudewindow = new CachedSvgItem(); + altitudewindow->setSharedRenderer(m_renderer); + altitudewindow->setElementId("altitude-window"); + altitudewindow->setFlags(QGraphicsItem::ItemClipsChildrenToShape | + QGraphicsItem::ItemClipsToShape); + l_scene->addItem(altitudewindow); + matrix.reset(); + matrix.translate(startX, startY); + altitudewindow->setTransform(matrix, false); - // Last create a Text Item at the location of the speed window - // and save it for future updates: - m_speedtext = new QGraphicsTextItem("0000"); - m_speedtext->setDefaultTextColor(QColor("White")); - m_speedtext->setFont(QFont("Arial",(int) speedWindowHeight/2)); - if (hqFonts) m_speedtext->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - m_speedtext->setParentItem(speedwindow); + // Last create a Text Item at the location of the speed window + // and save it for future updates: + m_altitudetext = new QGraphicsTextItem("0000"); + m_altitudetext->setDefaultTextColor(QColor("White")); + m_altitudetext->setFont(QFont("Arial", (int)altitudeWindowHeight / 2)); + if (hqFonts) { + m_altitudetext->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + m_altitudetext->setParentItem(altitudewindow); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).width() / 10; + matrix.reset(); + matrix.translate(startX, 0); + m_altitudetext->setTransform(matrix, false); - //////////////////// - // Altitude - //////////////////// - // Right hand, very similar to speed - compassMatrix = m_renderer->matrixForElement("altitude-bg"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-bg")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-bg")).y(); - verticalbg = new CachedSvgItem(); - verticalbg->setSharedRenderer(m_renderer); - verticalbg->setElementId("altitude-bg"); - verticalbg->setFlags(QGraphicsItem::ItemClipsChildrenToShape| - QGraphicsItem::ItemClipsToShape); - l_scene->addItem(verticalbg); - matrix.reset(); - matrix.translate(startX,startY); - verticalbg->setTransform(matrix,false); + //////////////// + // GCS Telemetry Indicator + //////////////// + if (m_renderer->elementExists("gcstelemetry-Disconnected")) { + compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); + gcsTelemetryArrow = new CachedSvgItem(); + gcsTelemetryArrow->setSharedRenderer(m_renderer); + gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); + l_scene->addItem(gcsTelemetryArrow); + matrix.reset(); + matrix.translate(startX, startY); + gcsTelemetryArrow->setTransform(matrix, false); + } else { + gcsTelemetryArrow = NULL; + } - // Note: altitude-scale should contain exactly 6 major ticks - // for 30 meters - m_altitudescale = new QGraphicsItemGroup(); - m_altitudescale->setParentItem(verticalbg); - - QGraphicsSvgItem *altitudescalelines = new CachedSvgItem(); - altitudescalelines->setSharedRenderer(m_renderer); - altitudescalelines->setElementId("altitude-scale"); - altitudeScaleHeight = m_renderer->matrixForElement("altitude-scale").mapRect( - m_renderer->boundsOnElement("altitude-scale")).height(); - // Quick way to reposition the item before putting it in the group: - altitudescalelines->setParentItem(verticalbg); - m_altitudescale->addToGroup(altitudescalelines); // (reparents the item) - - // Add the scale text elements: - speed0 = new QGraphicsTextItem("XXXX"); - speed0->setDefaultTextColor(QColor("White")); - speed0->setFont(QFont("Arial",(int) altitudeScaleHeight/30)); - if (hqFonts) speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - matrix.reset(); - matrix.translate(m_renderer->matrixForElement("altitude-scale").mapRect(m_renderer->boundsOnElement("altitude-scale")).width() - + m_renderer->matrixForElement("altitude-bg").mapRect(m_renderer->boundsOnElement("altitude-bg")).width()/10,-altitudeScaleHeight/30); - speed0->setTransform(matrix,false); - speed0->setParentItem(verticalbg); - m_altitudescale->addToGroup(speed0); - for (int i=0; i<6;i++) { - speed0 = new QGraphicsTextItem("XXXX"); - speed0->setDefaultTextColor(QColor("White")); - speed0->setFont(QFont("Arial",(int) altitudeScaleHeight/30)); - speed0->setPlainText(QString().setNum(i*5+1)); - if (hqFonts) speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - matrix.translate(0,altitudeScaleHeight/6); - speed0->setTransform(matrix,false); - speed0->setParentItem(verticalbg); - m_altitudescale->addToGroup(speed0); - } - // Now vertically center the speed scale on the speed background - rectB = verticalbg->boundingRect(); - rectN = altitudescalelines->boundingRect(); - m_altitudescale->setPos(0,rectB.height()/2-rectN.height()/2-rectN.height()/6); - - // Isolate the Altitude window and put it above the altitude scale - compassMatrix = m_renderer->matrixForElement("altitude-window"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).y(); - qreal altitudeWindowHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).height(); - QGraphicsSvgItem *altitudewindow = new CachedSvgItem(); - altitudewindow->setSharedRenderer(m_renderer); - altitudewindow->setElementId("altitude-window"); - altitudewindow->setFlags(QGraphicsItem::ItemClipsChildrenToShape| - QGraphicsItem::ItemClipsToShape); - l_scene->addItem(altitudewindow); - matrix.reset(); - matrix.translate(startX,startY); - altitudewindow->setTransform(matrix,false); - - // Last create a Text Item at the location of the speed window - // and save it for future updates: - m_altitudetext = new QGraphicsTextItem("0000"); - m_altitudetext->setDefaultTextColor(QColor("White")); - m_altitudetext->setFont(QFont("Arial",(int) altitudeWindowHeight/2)); - if (hqFonts) m_altitudetext->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - m_altitudetext->setParentItem(altitudewindow); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).width()/10; - matrix.reset(); - matrix.translate(startX,0); - m_altitudetext->setTransform(matrix,false); - - //////////////// - // GCS Telemetry Indicator - //////////////// - if (m_renderer->elementExists("gcstelemetry-Disconnected")) { - compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); - gcsTelemetryArrow = new CachedSvgItem(); - gcsTelemetryArrow->setSharedRenderer(m_renderer); - gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); - l_scene->addItem(gcsTelemetryArrow); - matrix.reset(); - matrix.translate(startX,startY); - gcsTelemetryArrow->setTransform(matrix,false); - } else { - gcsTelemetryArrow = NULL; - } - - if (m_renderer->elementExists("linkrate")) { - compassMatrix = m_renderer->matrixForElement("linkrate"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("linkrate")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("linkrate")).y(); - qreal linkRateHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("linkrate")).height(); - gcsTelemetryStats = new QGraphicsTextItem(); - gcsTelemetryStats->setDefaultTextColor(QColor("White")); - gcsTelemetryStats->setFont(QFont("Arial",(int) linkRateHeight)); - if (hqFonts) gcsTelemetryStats->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - l_scene->addItem(gcsTelemetryStats); - matrix.reset(); - matrix.translate(startX,startY-linkRateHeight/2); - gcsTelemetryStats->setTransform(matrix,false); - } else { - gcsTelemetryStats = NULL; - } + if (m_renderer->elementExists("linkrate")) { + compassMatrix = m_renderer->matrixForElement("linkrate"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("linkrate")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("linkrate")).y(); + qreal linkRateHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("linkrate")).height(); + gcsTelemetryStats = new QGraphicsTextItem(); + gcsTelemetryStats->setDefaultTextColor(QColor("White")); + gcsTelemetryStats->setFont(QFont("Arial", (int)linkRateHeight)); + if (hqFonts) { + gcsTelemetryStats->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + l_scene->addItem(gcsTelemetryStats); + matrix.reset(); + matrix.translate(startX, startY - linkRateHeight / 2); + gcsTelemetryStats->setTransform(matrix, false); + } else { + gcsTelemetryStats = NULL; + } - //////////////// - // GCS Battery Indicator - //////////////// - /* (to be used the day I add a green/yellow/red indicator) - compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); - gcsTelemetryArrow = new CachedSvgItem(); - gcsTelemetryArrow->setSharedRenderer(m_renderer); - gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); - l_scene->addItem(gcsTelemetryArrow); - matrix.reset(); - matrix.translate(startX,startY); - gcsTelemetryArrow->setTransform(matrix,false); + //////////////// + // GCS Battery Indicator + //////////////// + /* (to be used the day I add a green/yellow/red indicator) + compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); + gcsTelemetryArrow = new CachedSvgItem(); + gcsTelemetryArrow->setSharedRenderer(m_renderer); + gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); + l_scene->addItem(gcsTelemetryArrow); + matrix.reset(); + matrix.translate(startX,startY); + gcsTelemetryArrow->setTransform(matrix,false); */ - if (m_renderer->elementExists("battery-txt")) { - compassMatrix = m_renderer->matrixForElement("battery-txt"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("battery-txt")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("battery-txt")).y(); - qreal batStatHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("battery-txt")).height(); - gcsBatteryStats = new QGraphicsTextItem("Battery"); - gcsBatteryStats->setDefaultTextColor(QColor("White")); - gcsBatteryStats->setFont(QFont("Arial",(int) batStatHeight)); - if (hqFonts) gcsBatteryStats->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - l_scene->addItem(gcsBatteryStats); - matrix.reset(); - matrix.translate(startX,startY-batStatHeight/2); - gcsBatteryStats->setTransform(matrix,false); - } else { - gcsBatteryStats = NULL; - } + if (m_renderer->elementExists("battery-txt")) { + compassMatrix = m_renderer->matrixForElement("battery-txt"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("battery-txt")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("battery-txt")).y(); + qreal batStatHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("battery-txt")).height(); + gcsBatteryStats = new QGraphicsTextItem("Battery"); + gcsBatteryStats->setDefaultTextColor(QColor("White")); + gcsBatteryStats->setFont(QFont("Arial", (int)batStatHeight)); + if (hqFonts) { + gcsBatteryStats->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + l_scene->addItem(gcsBatteryStats); + matrix.reset(); + matrix.translate(startX, startY - batStatHeight / 2); + gcsBatteryStats->setTransform(matrix, false); + } else { + gcsBatteryStats = NULL; + } - //////////////// - // GCS GPS Indicator - //////////////// - /* (to be used the day I add a green/yellow/red indicator) - compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); - gcsTelemetryArrow = new CachedSvgItem(); - gcsTelemetryArrow->setSharedRenderer(m_renderer); - gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); - l_scene->addItem(gcsTelemetryArrow); - matrix.reset(); - matrix.translate(startX,startY); - gcsTelemetryArrow->setTransform(matrix,false); + //////////////// + // GCS GPS Indicator + //////////////// + /* (to be used the day I add a green/yellow/red indicator) + compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); + gcsTelemetryArrow = new CachedSvgItem(); + gcsTelemetryArrow->setSharedRenderer(m_renderer); + gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); + l_scene->addItem(gcsTelemetryArrow); + matrix.reset(); + matrix.translate(startX,startY); + gcsTelemetryArrow->setTransform(matrix,false); */ - if (m_renderer->elementExists("gps-txt")) { - compassMatrix = m_renderer->matrixForElement("gps-txt"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gps-txt")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gps-txt")).y(); - qreal gpsStatHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("gps-txt")).height(); - gcsGPSStats = new QGraphicsTextItem("GPS"); - gcsGPSStats->setDefaultTextColor(QColor("White")); - gcsGPSStats->setFont(QFont("Arial",(int) gpsStatHeight)); - if (hqFonts) gcsGPSStats->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - l_scene->addItem(gcsGPSStats); - matrix.reset(); - matrix.translate(startX,startY-gpsStatHeight/2); - gcsGPSStats->setTransform(matrix,false); - } else { - gcsGPSStats = NULL; - } + if (m_renderer->elementExists("gps-txt")) { + compassMatrix = m_renderer->matrixForElement("gps-txt"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gps-txt")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gps-txt")).y(); + qreal gpsStatHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("gps-txt")).height(); + gcsGPSStats = new QGraphicsTextItem("GPS"); + gcsGPSStats->setDefaultTextColor(QColor("White")); + gcsGPSStats->setFont(QFont("Arial", (int)gpsStatHeight)); + if (hqFonts) { + gcsGPSStats->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + l_scene->addItem(gcsGPSStats); + matrix.reset(); + matrix.translate(startX, startY - gpsStatHeight / 2); + gcsGPSStats->setTransform(matrix, false); + } else { + gcsGPSStats = NULL; + } l_scene->setSceneRect(m_background->boundingRect()); @@ -774,57 +814,55 @@ void PFDGadgetWidget::setDialFile(QString dfn) // 1) Move the center of the needle to the center of the background. rectB = m_background->boundingRect(); rectN = m_world->boundingRect(); - m_world->setPos(rectB.width()/2-rectN.width()/2,rectB.height()/2-rectN.height()/2); + m_world->setPos(rectB.width() / 2 - rectN.width() / 2, rectB.height() / 2 - rectN.height() / 2); // 2) Put the transform origin point of the needle at its center. - m_world->setTransformOriginPoint(rectN.width()/2,rectN.height()/2); + m_world->setTransformOriginPoint(rectN.width() / 2, rectN.height() / 2); rectN = m_rollscale->boundingRect(); - m_rollscale->setPos(rectB.width()/2-rectN.width()/2,rectB.height()/2-rectN.height()/2); - m_rollscale->setTransformOriginPoint(rectN.width()/2,rectN.height()/2); + m_rollscale->setPos(rectB.width() / 2 - rectN.width() / 2, rectB.height() / 2 - rectN.height() / 2); + m_rollscale->setTransformOriginPoint(rectN.width() / 2, rectN.height() / 2); // Also to the same init for the compass: rectB = m_compass->boundingRect(); rectN = m_compassband->boundingRect(); - m_compassband->setPos(rectB.width()/2-rectN.width()/2,rectB.height()/2-rectN.height()/2); - m_compassband->setTransformOriginPoint(rectN.width()/2,rectN.height()/2); + m_compassband->setPos(rectB.width() / 2 - rectN.width() / 2, rectB.height() / 2 - rectN.height() / 2); + m_compassband->setTransformOriginPoint(rectN.width() / 2, rectN.height() / 2); // Last: we just loaded the dial file which is by default valid for a "zero" value // of the needles, so we have to reset the needles too upon dial file loading, otherwise // we would end up with an offset when we change a dial file and the needle value // is not zero at that time. - rollValue = 0; - pitchValue = 0; - headingValue = 0; + rollValue = 0; + pitchValue = 0; + headingValue = 0; groundspeedValue = 0; - altitudeValue = 0; + altitudeValue = 0; pfdError = false; - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); // Rearm the dial Timer which might be stopped. - } - else - { qDebug()<<"Error on PFD artwork file."; - m_renderer->load(QString(":/pfd/images/pfd-default.svg")); - l_scene->clear(); // This also deletes all items contained in the scene. - m_background = new CachedSvgItem(); - m_background->setSharedRenderer(m_renderer); - l_scene->addItem(m_background); - pfdError = true; - } + } + } else { qDebug() << "Error on PFD artwork file."; + m_renderer->load(QString(":/pfd/images/pfd-default.svg")); + l_scene->clear(); // This also deletes all items contained in the scene. + m_background = new CachedSvgItem(); + m_background->setSharedRenderer(m_renderer); + l_scene->addItem(m_background); + pfdError = true; } } void PFDGadgetWidget::paint() { - // update(); + // update(); } void PFDGadgetWidget::paintEvent(QPaintEvent *event) { // Skip painting until the dial file is loaded - if (! m_renderer->isValid()) { + if (!m_renderer->isValid()) { qDebug() << "Dial file not loaded, not rendering"; return; } - QGraphicsView::paintEvent(event); + QGraphicsView::paintEvent(event); } // This event enables the dial to be dynamically resized @@ -833,24 +871,24 @@ void PFDGadgetWidget::paintEvent(QPaintEvent *event) void PFDGadgetWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - fitInView(m_background, Qt::KeepAspectRatio ); + fitInView(m_background, Qt::KeepAspectRatio); } /*! - \brief Update method for the vertical scales - */ -void PFDGadgetWidget::moveVerticalScales() { + \brief Update method for the vertical scales + */ +void PFDGadgetWidget::moveVerticalScales() {} -} - -void PFDGadgetWidget::moveSky() { +void PFDGadgetWidget::moveSky() +{ int dialCount = 2; // Gets decreased by one for each element - // which has finished moving -// qDebug() << "MoveSky"; + + // which has finished moving +// qDebug() << "MoveSky"; /// TODO: optimize!!! if (pfdError) { - //skyDialTimer.stop(); + // skyDialTimer.stop(); return; } @@ -859,21 +897,22 @@ void PFDGadgetWidget::moveSky() { // the PFD and the whole GCS: for this reason, we check this here. // The strange check below works, it is a workaround because "isnan(double)" // is not supported on every compiler. - if (rollTarget != rollTarget || pitchTarget != pitchTarget) + if (rollTarget != rollTarget || pitchTarget != pitchTarget) { return; + } ////// // Roll ////// if (rollValue != rollTarget) { double rollDiff; - if ((abs((rollValue-rollTarget)*10) > 5) && beSmooth ) { - rollDiff =(rollTarget - rollValue)/2; + if ((abs((rollValue - rollTarget) * 10) > 5) && beSmooth) { + rollDiff = (rollTarget - rollValue) / 2; } else { rollDiff = rollTarget - rollValue; dialCount--; } - m_world->setRotation(m_world->rotation()+rollDiff); - m_rollscale->setRotation(m_rollscale->rotation()+rollDiff); + m_world->setRotation(m_world->rotation() + rollDiff); + m_rollscale->setRotation(m_rollscale->rotation() + rollDiff); rollValue += rollDiff; } else { dialCount--; @@ -884,27 +923,27 @@ void PFDGadgetWidget::moveSky() { ////// if (pitchValue != pitchTarget) { double pitchDiff; - if ((abs((pitchValue-pitchTarget)*10) > 5) && beSmooth ) { - // if (0) { - pitchDiff = (pitchTarget - pitchValue)/2; + if ((abs((pitchValue - pitchTarget) * 10) > 5) && beSmooth) { + // if (0) { + pitchDiff = (pitchTarget - pitchValue) / 2; } else { pitchDiff = pitchTarget - pitchValue; dialCount--; } - QPointF opd = QPointF(0,pitchDiff); - m_world->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), true); + QPointF opd = QPointF(0, pitchDiff); + m_world->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), true); QPointF oop = m_world->transformOriginPoint(); - m_world->setTransformOriginPoint((oop.x()-opd.x()),(oop.y()-opd.y())); + m_world->setTransformOriginPoint((oop.x() - opd.x()), (oop.y() - opd.y())); pitchValue += pitchDiff; } else { dialCount--; } - if (dialCount) + if (dialCount) { scene()->update(sceneRect()); - // if (!dialCount) - // skyDialTimer.stop(); - + } + // if (!dialCount) + // skyDialTimer.stop(); } @@ -915,14 +954,15 @@ void PFDGadgetWidget::moveSky() { void PFDGadgetWidget::moveNeedles() { int dialCount = 3; // Gets decreased by one for each element - // which has finished moving -// qDebug() << "MoveNeedles"; + // which has finished moving + +// qDebug() << "MoveNeedles"; /// TODO: optimize!!! if (pfdError) { - dialTimer.stop(); - return; + dialTimer.stop(); + return; } ////// @@ -934,28 +974,28 @@ void PFDGadgetWidget::moveNeedles() ////// if (headingValue != headingTarget) { double headingDiff; - if ((abs((headingValue-headingTarget)*10) > 5) && beSmooth ) { - headingDiff = (headingTarget - headingValue)/5; + if ((abs((headingValue - headingTarget) * 10) > 5) && beSmooth) { + headingDiff = (headingTarget - headingValue) / 5; } else { - headingDiff = headingTarget-headingValue; + headingDiff = headingTarget - headingValue; dialCount--; } - double threshold = 180*compassBandWidth/540; + double threshold = 180 * compassBandWidth / 540; // Note: rendering can jump oh so very slightly when crossing the 180 degree // boundary, should not impact actual useability of the display. - if ((headingValue+headingDiff)>=threshold) { + if ((headingValue + headingDiff) >= threshold) { // We went over 180°: activate a -360 degree offset - headingDiff -= 2*threshold; - headingTarget -= 2*threshold; - } else if ((headingValue+headingDiff)<-threshold) { + headingDiff -= 2 * threshold; + headingTarget -= 2 * threshold; + } else if ((headingValue + headingDiff) < -threshold) { // We went under -180°: remove the -360 degree offset - headingDiff += 2*threshold; - headingTarget += 2*threshold; + headingDiff += 2 * threshold; + headingTarget += 2 * threshold; } - QPointF opd = QPointF(headingDiff,0); - m_compassband->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), true); + QPointF opd = QPointF(headingDiff, 0); + m_compassband->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), true); headingValue += headingDiff; - } else { + } else { dialCount--; } @@ -963,37 +1003,38 @@ void PFDGadgetWidget::moveNeedles() // Airspeed ////// if (airspeedValue != airspeedTarget) { - if ((abs(airspeedValue-airspeedTarget) > speedScaleHeight/100) && beSmooth ) { - airspeedValue += (airspeedTarget-airspeedValue)/2; + if ((abs(airspeedValue - airspeedTarget) > speedScaleHeight / 100) && beSmooth) { + airspeedValue += (airspeedTarget - airspeedValue) / 2; } else { airspeedValue = airspeedTarget; dialCount--; } - float airspeed_kph=airspeedValue*3.6; - float airspeed_kph_scale = airspeed_kph*speedScaleHeight/30; + float airspeed_kph = airspeedValue * 3.6; + float airspeed_kph_scale = airspeed_kph * speedScaleHeight / 30; - qreal x = m_speedscale->transform().dx(); - //opd = QPointF(x,fmod(airspeed_kph,speedScaleHeight/6)); + qreal x = m_speedscale->transform().dx(); + // opd = QPointF(x,fmod(airspeed_kph,speedScaleHeight/6)); // fmod does rounding errors!! the formula below works better: - QPointF opd = QPointF(x, airspeed_kph_scale-floor(airspeed_kph_scale/speedScaleHeight*6)*speedScaleHeight/6); - m_speedscale->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false); + QPointF opd = QPointF(x, airspeed_kph_scale - floor(airspeed_kph_scale / speedScaleHeight * 6) * speedScaleHeight / 6); + m_speedscale->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), false); double speedText = airspeed_kph; - QString s = QString().sprintf("%.0f",speedText); + QString s = QString().sprintf("%.0f", speedText); m_speedtext->setPlainText(s); // Now update the text elements inside the scale: // (Qt documentation states that the list has the same order // as the item add order in the QGraphicsItemGroup) QList textList = m_speedscale->childItems(); - qreal val = 5*floor(airspeed_kph_scale/speedScaleHeight*6)+20; - foreach( QGraphicsItem * item, textList) { - if (QGraphicsTextItem *text = qgraphicsitem_cast(item)) { - QString s = (val<0) ? QString() : QString().sprintf("%.0f",val); - if (text->toPlainText() == s) + qreal val = 5 * floor(airspeed_kph_scale / speedScaleHeight * 6) + 20; + foreach(QGraphicsItem * item, textList) { + if (QGraphicsTextItem * text = qgraphicsitem_cast(item)) { + QString s = (val < 0) ? QString() : QString().sprintf("%.0f", val); + if (text->toPlainText() == s) { break; // This should happen at element one if is has not changed, indicating - // that it's not necessary to do the rest of the list + } + // that it's not necessary to do the rest of the list text->setPlainText(s); val -= 5; } @@ -1007,68 +1048,70 @@ void PFDGadgetWidget::moveNeedles() ////// if (groundspeedValue != groundspeedTarget) { groundspeedValue = groundspeedTarget; - qreal x = m_speedscale->transform().dx(); - //opd = QPointF(x,fmod(groundspeedValue,speedScaleHeight/6)); - // fmod does rounding errors!! the formula below works better: - QPointF opd = QPointF(x,groundspeedValue-floor(groundspeedValue/speedScaleHeight*6)*speedScaleHeight/6); - m_speedscale->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false); + qreal x = m_speedscale->transform().dx(); + // opd = QPointF(x,fmod(groundspeedValue,speedScaleHeight/6)); + // fmod does rounding errors!! the formula below works better: + QPointF opd = QPointF(x, groundspeedValue - floor(groundspeedValue / speedScaleHeight * 6) * speedScaleHeight / 6); + m_speedscale->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), false); - double speedText = groundspeedValue/speedScaleHeight*30; - QString s = QString().sprintf("%.0f",speedText); - m_speedtext->setPlainText(s); + double speedText = groundspeedValue / speedScaleHeight * 30; + QString s = QString().sprintf("%.0f", speedText); + m_speedtext->setPlainText(s); - // Now update the text elements inside the scale: - // (Qt documentation states that the list has the same order - // as the item add order in the QGraphicsItemGroup) - QList textList = m_speedscale->childItems(); - qreal val = 5*floor(groundspeedValue/speedScaleHeight*6)+20; - foreach( QGraphicsItem * item, textList) { - if (QGraphicsTextItem *text = qgraphicsitem_cast(item)) { - QString s = (val<0) ? QString() : QString().sprintf("%.0f",val); - if (text->toPlainText() == s) - break; // This should happen at element one if is has not changed, indicating - // that it's not necessary to do the rest of the list - text->setPlainText(s); - val -= 5; - } - } + // Now update the text elements inside the scale: + // (Qt documentation states that the list has the same order + // as the item add order in the QGraphicsItemGroup) + QList textList = m_speedscale->childItems(); + qreal val = 5 * floor(groundspeedValue / speedScaleHeight * 6) + 20; + foreach(QGraphicsItem * item, textList) { + if (QGraphicsTextItem * text = qgraphicsitem_cast(item)) { + QString s = (val < 0) ? QString() : QString().sprintf("%.0f", val); + if (text->toPlainText() == s) { + break; // This should happen at element one if is has not changed, indicating + } + // that it's not necessary to do the rest of the list + text->setPlainText(s); + val -= 5; + } + } } ////// // Altitude ////// if (altitudeValue != altitudeTarget) { - if ((abs(altitudeValue-altitudeTarget) > altitudeScaleHeight/100) && beSmooth ) { - altitudeValue += (altitudeTarget-altitudeValue)/2; + if ((abs(altitudeValue - altitudeTarget) > altitudeScaleHeight / 100) && beSmooth) { + altitudeValue += (altitudeTarget - altitudeValue) / 2; } else { altitudeValue = altitudeTarget; dialCount--; } // The altitude scale represents 30 meters - float altitudeValue_scale = floor(altitudeValue*10)/10*altitudeScaleHeight/30; + float altitudeValue_scale = floor(altitudeValue * 10) / 10 * altitudeScaleHeight / 30; - qreal x = m_altitudescale->transform().dx(); - //opd = QPointF(x,fmod(altitudeValue,altitudeScaleHeight/6)); + qreal x = m_altitudescale->transform().dx(); + // opd = QPointF(x,fmod(altitudeValue,altitudeScaleHeight/6)); // fmod does rounding errors!! the formula below works better: - QPointF opd = QPointF(x,altitudeValue_scale-floor(altitudeValue_scale/altitudeScaleHeight*6)*altitudeScaleHeight/6); - m_altitudescale->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false); + QPointF opd = QPointF(x, altitudeValue_scale - floor(altitudeValue_scale / altitudeScaleHeight * 6) * altitudeScaleHeight / 6); + m_altitudescale->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), false); double altitudeText = altitudeValue; - QString s = QString().sprintf("%.0f",altitudeText); + QString s = QString().sprintf("%.0f", altitudeText); m_altitudetext->setPlainText(s); // Now update the text elements inside the scale: // (Qt documentation states that the list has the same order // as the item add order in the QGraphicsItemGroup) QList textList = m_altitudescale->childItems(); - qreal val = 5*floor(altitudeValue_scale/altitudeScaleHeight*6)+20; - foreach( QGraphicsItem * item, textList) { - if (QGraphicsTextItem *text = qgraphicsitem_cast(item)) { - QString s = (val<0) ? QString() : QString().sprintf("%.0f",val); - if (text->toPlainText() == s) + qreal val = 5 * floor(altitudeValue_scale / altitudeScaleHeight * 6) + 20; + foreach(QGraphicsItem * item, textList) { + if (QGraphicsTextItem * text = qgraphicsitem_cast(item)) { + QString s = (val < 0) ? QString() : QString().sprintf("%.0f", val); + if (text->toPlainText() == s) { break; // This should happen at element one if is has not changed, indicating - // that it's not necessary to do the rest of the list + } + // that it's not necessary to do the rest of the list text->setPlainText(s); val -= 5; } @@ -1077,13 +1120,14 @@ void PFDGadgetWidget::moveNeedles() dialCount--; } - if (!dialCount) - dialTimer.stop(); - else - scene()->update(sceneRect()); + if (!dialCount) { + dialTimer.stop(); + } else { + scene()->update(sceneRect()); + } } /** - @} - @} - */ + @} + @} + */ diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.h b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.h index d3ef40ec8..d80b73432 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -39,118 +39,122 @@ #include #include -class PFDGadgetWidget : public QGraphicsView -{ +class PFDGadgetWidget : public QGraphicsView { Q_OBJECT public: PFDGadgetWidget(QWidget *parent = 0); - ~PFDGadgetWidget(); - void setDialFile(QString dfn); - void paint(); - // Sets up needle/UAVObject connections: - void connectNeedles(); - void enableOpenGL(bool flag); - void setHqFonts(bool flag) { hqFonts = flag; } - void enableSmoothUpdates(bool flag) { beSmooth = flag; } + ~PFDGadgetWidget(); + void setDialFile(QString dfn); + void paint(); + // Sets up needle/UAVObject connections: + void connectNeedles(); + void enableOpenGL(bool flag); + void setHqFonts(bool flag) + { + hqFonts = flag; + } + void enableSmoothUpdates(bool flag) + { + beSmooth = flag; + } public slots: - void updateAttitude(UAVObject *object1); - void updateHeading(UAVObject *object1); - void updateGPS(UAVObject *object1); - void updateGroundspeed(UAVObject *object1); - void updateAirspeed(UAVObject *object1); - void updateAltitude(UAVObject *object1); - void updateBattery(UAVObject *object1); - void updateLinkStatus(UAVObject *object1); + void updateAttitude(UAVObject *object1); + void updateHeading(UAVObject *object1); + void updateGPS(UAVObject *object1); + void updateGroundspeed(UAVObject *object1); + void updateAirspeed(UAVObject *object1); + void updateAltitude(UAVObject *object1); + void updateBattery(UAVObject *object1); + void updateLinkStatus(UAVObject *object1); protected: - void paintEvent(QPaintEvent *event); - void resizeEvent(QResizeEvent *event); + void paintEvent(QPaintEvent *event); + void resizeEvent(QResizeEvent *event); private slots: - void moveNeedles(); - void moveVerticalScales(); - void moveSky(); - void setToolTipPrivate(); + void moveNeedles(); + void moveVerticalScales(); + void moveSky(); + void setToolTipPrivate(); private: - QSvgRenderer *m_renderer; + QSvgRenderer *m_renderer; - // Background: background - QGraphicsSvgItem *m_background; - // earth/sky : world - QGraphicsSvgItem *m_world; - // Roll scale: rollscale - QGraphicsSvgItem *m_rollscale; - // Compass dial: - QGraphicsSvgItem *m_compass; - // Compass band: - QGraphicsSvgItem *m_compassband; - // Home point: - QGraphicsSvgItem *m_homewaypoint; - // Next point: - QGraphicsSvgItem *m_nextwaypoint; - // Home point bearing: - QGraphicsSvgItem *m_homepointbearing; - // Next point bearing: - QGraphicsSvgItem *m_nextpointbearing; - // Speed scale: - QGraphicsItemGroup *m_speedscale; - // Speed indicator text: - QGraphicsTextItem *m_speedtext; - // Vertical altitude scale: - QGraphicsItemGroup *m_altitudescale; - // Altitude indicator text: - QGraphicsTextItem *m_altitudetext; - // GCS link status Arrow - QGraphicsSvgItem *gcsTelemetryArrow; - QGraphicsTextItem *gcsTelemetryStats; - QGraphicsTextItem *gcsBatteryStats; - QGraphicsTextItem *gcsGPSStats; + // Background: background + QGraphicsSvgItem *m_background; + // earth/sky : world + QGraphicsSvgItem *m_world; + // Roll scale: rollscale + QGraphicsSvgItem *m_rollscale; + // Compass dial: + QGraphicsSvgItem *m_compass; + // Compass band: + QGraphicsSvgItem *m_compassband; + // Home point: + QGraphicsSvgItem *m_homewaypoint; + // Next point: + QGraphicsSvgItem *m_nextwaypoint; + // Home point bearing: + QGraphicsSvgItem *m_homepointbearing; + // Next point bearing: + QGraphicsSvgItem *m_nextpointbearing; + // Speed scale: + QGraphicsItemGroup *m_speedscale; + // Speed indicator text: + QGraphicsTextItem *m_speedtext; + // Vertical altitude scale: + QGraphicsItemGroup *m_altitudescale; + // Altitude indicator text: + QGraphicsTextItem *m_altitudetext; + // GCS link status Arrow + QGraphicsSvgItem *gcsTelemetryArrow; + QGraphicsTextItem *gcsTelemetryStats; + QGraphicsTextItem *gcsBatteryStats; + QGraphicsTextItem *gcsGPSStats; - // The Value and target variables - // are expressed in degrees - double rollTarget; - double rollValue; - double pitchTarget; - double pitchValue; - double headingTarget; - double headingValue; - double groundspeedTarget; - double groundspeedValue; - double airspeedTarget; - double airspeedValue; - double altitudeTarget; - double altitudeValue; + // The Value and target variables + // are expressed in degrees + double rollTarget; + double rollValue; + double pitchTarget; + double pitchValue; + double headingTarget; + double headingValue; + double groundspeedTarget; + double groundspeedValue; + double airspeedTarget; + double airspeedValue; + double altitudeTarget; + double altitudeValue; - qreal compassBandWidth; - qreal speedScaleHeight; - qreal altitudeScaleHeight; + qreal compassBandWidth; + qreal speedScaleHeight; + qreal altitudeScaleHeight; - // Name of the fields to read when an update is received: - UAVDataObject* airspeedObj; - UAVDataObject* altitudeObj; - UAVDataObject* attitudeObj; - UAVDataObject* groundspeedObj; - UAVDataObject* headingObj; - UAVDataObject* gpsObj; - UAVDataObject* gcsTelemetryObj; - UAVDataObject* gcsBatteryObj; + // Name of the fields to read when an update is received: + UAVDataObject *airspeedObj; + UAVDataObject *altitudeObj; + UAVDataObject *attitudeObj; + UAVDataObject *groundspeedObj; + UAVDataObject *headingObj; + UAVDataObject *gpsObj; + UAVDataObject *gcsTelemetryObj; + UAVDataObject *gcsBatteryObj; - // Rotation timer - QTimer dialTimer; - QTimer skyDialTimer; + // Rotation timer + QTimer dialTimer; + QTimer skyDialTimer; - QString satString; - QString batString; - - // Flag to check for pfd Error - bool pfdError; - // Flag to enable better rendering of fonts in OpenGL - bool hqFonts; - bool beSmooth; + QString satString; + QString batString; + // Flag to check for pfd Error + bool pfdError; + // Flag to enable better rendering of fonts in OpenGL + bool hqFonts; + bool beSmooth; }; #endif /* PFDGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdplugin.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdplugin.cpp index 662c109bf..fce87ea88 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdplugin.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,32 +35,31 @@ PFDPlugin::PFDPlugin() { - // Do nothing + // Do nothing } PFDPlugin::~PFDPlugin() { - // Do nothing + // Do nothing } -bool PFDPlugin::initialize(const QStringList& args, QString *errMsg) +bool PFDPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new PFDGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new PFDGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void PFDPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void PFDPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(PFDPlugin) - diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdplugin.h b/ground/openpilotgcs/src/plugins/pfd/pfdplugin.h index 1a05f6d25..067384793 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdplugin.h +++ b/ground/openpilotgcs/src/plugins/pfd/pfdplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class PFDGadgetFactory; -class PFDPlugin : public ExtensionSystem::IPlugin -{ +class PFDPlugin : public ExtensionSystem::IPlugin { public: - PFDPlugin(); - ~PFDPlugin(); + PFDPlugin(); + ~PFDPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - PFDGadgetFactory *mf; + PFDGadgetFactory *mf; }; #endif /* PFDPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/pfdqml/osgearth.cpp b/ground/openpilotgcs/src/plugins/pfdqml/osgearth.cpp index a14fca75e..6cbe03aed 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/osgearth.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/osgearth.cpp @@ -41,7 +41,7 @@ #include "utils/pathutils.h" -OsgEarthItem::OsgEarthItem(QDeclarativeItem *parent): +OsgEarthItem::OsgEarthItem(QDeclarativeItem *parent) : QDeclarativeItem(parent), m_renderer(0), m_rendererThread(0), @@ -63,8 +63,8 @@ OsgEarthItem::~OsgEarthItem() { if (m_renderer) { m_rendererThread->exit(); - //wait up to 10 seconds for renderer thread to exit - m_rendererThread->wait(10*1000); + // wait up to 10 seconds for renderer thread to exit + m_rendererThread->wait(10 * 1000); delete m_renderer; delete m_rendererThread; @@ -75,9 +75,9 @@ QString OsgEarthItem::resolvedSceneFile() const { QString sceneFile = m_sceneFile; - //try to resolve the relative scene file name: + // try to resolve the relative scene file name: if (!QFileInfo(sceneFile).exists()) { - QDeclarativeView *view = qobject_cast(scene()->views().first()); + QDeclarativeView *view = qobject_cast(scene()->views().first()); if (view) { QUrl baseUrl = view->engine()->baseUrl(); @@ -93,16 +93,16 @@ void OsgEarthItem::geometryChanged(const QRectF &newGeometry, const QRectF &oldG Q_UNUSED(oldGeometry); Q_UNUSED(newGeometry); - //Dynamic gyometry changes are not supported yet, - //terrain is rendered to fixed geompetry and scalled for now + // Dynamic gyometry changes are not supported yet, + // terrain is rendered to fixed geompetry and scalled for now /* - qDebug() << Q_FUNC_INFO << newGeometry; + qDebug() << Q_FUNC_INFO << newGeometry; - int w = qRound(newGeometry.width()); - int h = qRound(newGeometry.height()); + int w = qRound(newGeometry.width()); + int h = qRound(newGeometry.height()); - if (m_currentSize != QSize(w,h) && m_gw.get()) { + if (m_currentSize != QSize(w,h) && m_gw.get()) { m_currentSize = QSize(w,h); m_gw->getEventQueue()->windowResize(0,0,w,h); @@ -111,15 +111,15 @@ void OsgEarthItem::geometryChanged(const QRectF &newGeometry, const QRectF &oldG osg::Camera *camera = m_viewer->getCamera(); camera->setViewport(new osg::Viewport(0,0,w,h)); camera->setProjectionMatrixAsPerspective(m_fieldOfView, qreal(w)/h, 1.0f, 10000.0f); - } - */ + } + */ } void OsgEarthItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *style, QWidget *widget) { Q_UNUSED(painter); Q_UNUSED(style); - QGLWidget *glWidget = qobject_cast(widget); + QGLWidget *glWidget = qobject_cast(widget); if (!m_renderer) { m_renderer = new OsgEarthItemRenderer(this, glWidget); @@ -136,8 +136,9 @@ void OsgEarthItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *styl QGLFramebufferObject *fbo = m_renderer->lastFrame(); - if (glWidget && fbo) + if (glWidget && fbo) { glWidget->drawTexture(boundingRect(), fbo->texture()); + } } void OsgEarthItem::updateView() @@ -182,7 +183,7 @@ void OsgEarthItem::setYaw(qreal arg) void OsgEarthItem::setLatitude(double arg) { - //not sure qFuzzyCompare is accurate enough for geo coordinates + // not sure qFuzzyCompare is accurate enough for geo coordinates if (m_latitude != arg) { m_latitude = arg; emit latitudeChanged(arg); @@ -199,26 +200,26 @@ void OsgEarthItem::setLongitude(double arg) void OsgEarthItem::setAltitude(double arg) { - if (!qFuzzyCompare(m_altitude,arg)) { + if (!qFuzzyCompare(m_altitude, arg)) { m_altitude = arg; emit altitudeChanged(arg); } } -//! Camera vertical field of view in degrees +// ! Camera vertical field of view in degrees void OsgEarthItem::setFieldOfView(qreal arg) { - if (!qFuzzyCompare(m_fieldOfView,arg)) { + if (!qFuzzyCompare(m_fieldOfView, arg)) { m_fieldOfView = arg; emit fieldOfViewChanged(arg); - //it should be a queued call to OsgEarthItemRenderer instead + // it should be a queued call to OsgEarthItemRenderer instead /*if (m_viewer.get()) { m_viewer->getCamera()->setProjectionMatrixAsPerspective( m_fieldOfView, qreal(m_currentSize.width())/m_currentSize.height(), 1.0f, 10000.0f); - }*/ + }*/ updateFrame(); } @@ -239,23 +240,23 @@ OsgEarthItemRenderer::OsgEarthItemRenderer(OsgEarthItem *item, QGLWidget *glWidg m_currentSize(640, 480), m_cameraDirty(false) { - //make a shared gl widget to avoid - //osg rendering to mess with qpainter state - //this runs in the main thread + // make a shared gl widget to avoid + // osg rendering to mess with qpainter state + // this runs in the main thread m_glWidget = new QGLWidget(0, glWidget); m_glWidget.data()->setAttribute(Qt::WA_PaintOutsidePaintEvent); - for (int i=0; imakeCurrent(); - for (int i=0; iresolvedSceneFile(); m_model = osgDB::readNodeFile(sceneFile.toStdString()); - //setup caching + // setup caching osgEarth::MapNode *mapNode = osgEarth::MapNode::findMapNode(m_model.get()); if (!mapNode) { qWarning() << Q_FUNC_INFO << sceneFile << " doesn't look like an osgEarth file"; } - m_gw = new osgViewer::GraphicsWindowEmbedded(0,0,w,h); + m_gw = new osgViewer::GraphicsWindowEmbedded(0, 0, w, h); m_viewer = new osgViewer::Viewer(); m_viewer->setThreadingModel(osgViewer::Viewer::SingleThreaded); @@ -293,26 +294,27 @@ void OsgEarthItemRenderer::initScene() m_viewer->getDatabasePager()->setDoPreCompile(true); osg::Camera *camera = m_viewer->getCamera(); - camera->setViewport(new osg::Viewport(0,0,w,h)); + camera->setViewport(new osg::Viewport(0, 0, w, h)); camera->setGraphicsContext(m_gw); - camera->setClearMask(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); + camera->setClearMask(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // configure the near/far so we don't clip things that are up close camera->setNearFarRatio(0.00002); - camera->setProjectionMatrixAsPerspective(m_item->fieldOfView(), qreal(w)/h, 1.0f, 10000.0f); + camera->setProjectionMatrixAsPerspective(m_item->fieldOfView(), qreal(w) / h, 1.0f, 10000.0f); updateFrame(); } void OsgEarthItemRenderer::updateFrame() { - if (!m_cameraDirty || !m_viewer.get() || m_glWidget.isNull()) + if (!m_cameraDirty || !m_viewer.get() || m_glWidget.isNull()) { return; + } m_glWidget.data()->makeCurrent(); - //To find a camera view matrix, find placer matrixes for two points - //onr at requested coords and another latitude shifted by 0.01 deg + // To find a camera view matrix, find placer matrixes for two points + // onr at requested coords and another latitude shifted by 0.01 deg osgEarth::Util::ObjectPlacer placer(m_viewer->getSceneData()); m_cameraDirty = false; @@ -320,40 +322,40 @@ void OsgEarthItemRenderer::updateFrame() osg::Matrixd positionMatrix; placer.createPlacerMatrix(m_item->latitude(), m_item->longitude(), m_item->altitude(), positionMatrix); osg::Matrixd positionMatrix2; - placer.createPlacerMatrix(m_item->latitude()+0.01, m_item->longitude(), m_item->altitude(), positionMatrix2); + placer.createPlacerMatrix(m_item->latitude() + 0.01, m_item->longitude(), m_item->altitude(), positionMatrix2); osg::Vec3d eye(0.0f, 0.0f, 0.0f); osg::Vec3d viewVector(0.0f, 0.0f, 0.0f); osg::Vec3d upVector(0.0f, 0.0f, 1.0f); eye = positionMatrix.preMult(eye); - upVector = positionMatrix.preMult(upVector); + upVector = positionMatrix.preMult(upVector); upVector.normalize(); - viewVector = positionMatrix2.preMult(viewVector) - eye; + viewVector = positionMatrix2.preMult(viewVector) - eye; viewVector.normalize(); viewVector *= 10.0; - //TODO: clarify the correct rotation order, - //currently assuming yaw, pitch, roll + // TODO: clarify the correct rotation order, + // currently assuming yaw, pitch, roll osg::Quat q; - q.makeRotate(-m_item->yaw()*M_PI/180.0, upVector); - upVector = q * upVector; + q.makeRotate(-m_item->yaw() * M_PI / 180.0, upVector); + upVector = q * upVector; viewVector = q * viewVector; osg::Vec3d side = viewVector ^ upVector; - q.makeRotate(m_item->pitch()*M_PI/180.0, side); - upVector = q * upVector; + q.makeRotate(m_item->pitch() * M_PI / 180.0, side); + upVector = q * upVector; viewVector = q * viewVector; - q.makeRotate(m_item->roll()*M_PI/180.0, viewVector); - upVector = q * upVector; + q.makeRotate(m_item->roll() * M_PI / 180.0, viewVector); + upVector = q * upVector; viewVector = q * viewVector; osg::Vec3d center = eye + viewVector; -// qDebug() << "e " << eye.x() << eye.y() << eye.z(); -// qDebug() << "c " << center.x() << center.y() << center.z(); -// qDebug() << "up" << upVector.x() << upVector.y() << upVector.z(); +// qDebug() << "e " << eye.x() << eye.y() << eye.z(); +// qDebug() << "c " << center.x() << center.y() << center.z(); +// qDebug() << "up" << upVector.x() << upVector.y() << upVector.z(); m_viewer->getCamera()->setViewMatrixAsLookAt(osg::Vec3d(eye.x(), eye.y(), eye.z()), osg::Vec3d(center.x(), center.y(), center.z()), diff --git a/ground/openpilotgcs/src/plugins/pfdqml/osgearth.h b/ground/openpilotgcs/src/plugins/pfdqml/osgearth.h index 03f0543a1..332c27156 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/osgearth.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/osgearth.h @@ -26,10 +26,8 @@ class QGLFramebufferObject; class QGLWidget; class OsgEarthItemRenderer; -class OsgEarthItem : public QDeclarativeItem -{ - Q_OBJECT - Q_DISABLE_COPY(OsgEarthItem) +class OsgEarthItem : public QDeclarativeItem { + Q_OBJECT Q_DISABLE_COPY(OsgEarthItem) Q_PROPERTY(QString sceneFile READ sceneFile WRITE setSceneFile NOTIFY sceneFileChanged) Q_PROPERTY(qreal fieldOfView READ fieldOfView WRITE setFieldOfView NOTIFY fieldOfViewChanged) @@ -46,17 +44,41 @@ public: OsgEarthItem(QDeclarativeItem *parent = 0); ~OsgEarthItem(); - QString sceneFile() const { return m_sceneFile; } + QString sceneFile() const + { + return m_sceneFile; + } QString resolvedSceneFile() const; - qreal fieldOfView() const { return m_fieldOfView; } + qreal fieldOfView() const + { + return m_fieldOfView; + } - qreal roll() const { return m_roll; } - qreal pitch() const { return m_pitch; } - qreal yaw() const { return m_yaw; } + qreal roll() const + { + return m_roll; + } + qreal pitch() const + { + return m_pitch; + } + qreal yaw() const + { + return m_yaw; + } - double latitude() const { return m_latitude; } - double longitude() const { return m_longitude; } - double altitude() const { return m_altitude; } + double latitude() const + { + return m_latitude; + } + double longitude() const + { + return m_longitude; + } + double altitude() const + { + return m_altitude; + } protected: void geometryChanged(const QRectF &newGeometry, const QRectF &oldGeometry); @@ -106,18 +128,19 @@ private: qreal m_fieldOfView; QString m_sceneFile; - }; -class OsgEarthItemRenderer : public QObject -{ +class OsgEarthItemRenderer : public QObject { Q_OBJECT public: OsgEarthItemRenderer(OsgEarthItem *item, QGLWidget *glWidget); ~OsgEarthItemRenderer(); QGLFramebufferObject *lastFrame(); - void markDirty() { m_cameraDirty = true; } + void markDirty() + { + m_cameraDirty = true; + } public slots: void initScene(); @@ -135,7 +158,7 @@ private: osg::ref_ptr m_model; QWeakPointer m_glWidget; - QGLFramebufferObject* m_fbo[FboCount]; + QGLFramebufferObject *m_fbo[FboCount]; int m_lastFboNumber; QSize m_currentSize; @@ -146,4 +169,3 @@ private: QML_DECLARE_TYPE(OsgEarthItem) #endif // OSGEARTH_H - diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.cpp index 770cc9744..265a96497 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.cpp @@ -19,10 +19,9 @@ #include "pfdqmlgadgetconfiguration.h" PfdQmlGadget::PfdQmlGadget(QString classId, PfdQmlGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} PfdQmlGadget::~PfdQmlGadget() { @@ -30,14 +29,15 @@ PfdQmlGadget::~PfdQmlGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void PfdQmlGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void PfdQmlGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - PfdQmlGadgetConfiguration *m = qobject_cast(config); + PfdQmlGadgetConfiguration *m = qobject_cast(config); + m_widget->setOpenGLEnabled(m->openGLEnabled()); m_widget->setQmlFile(m->qmlFile()); m_widget->setEarthFile(m->earthFile()); @@ -47,8 +47,8 @@ void PfdQmlGadget::loadConfiguration(IUAVGadgetConfiguration* config) m_widget->setLongitude(m->longitude()); m_widget->setAltitude(m->altitude()); - //setting OSGEARTH_CACHE_ONLY seems to work the most reliably - //between osgEarth versions I tried + // setting OSGEARTH_CACHE_ONLY seems to work the most reliably + // between osgEarth versions I tried if (m->cacheOnly()) { qputenv("OSGEARTH_CACHE_ONLY", "true"); } else { diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.h b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.h index be6625ab0..b6525007f 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.h @@ -27,15 +27,17 @@ class PfdQmlGadgetWidget; using namespace Core; -class PfdQmlGadget : public Core::IUAVGadget -{ +class PfdQmlGadget : public Core::IUAVGadget { Q_OBJECT public: PfdQmlGadget(QString classId, PfdQmlGadgetWidget *widget, QWidget *parent = 0); ~PfdQmlGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: PfdQmlGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.cpp index ba85992ad..e00e52a1d 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.cpp @@ -33,21 +33,21 @@ PfdQmlGadgetConfiguration::PfdQmlGadgetConfiguration(QString classId, QSettings m_altitude(0), m_cacheOnly(false) { - //if a saved configuration exists load it - if(qSettings != 0) { - m_qmlFile = qSettings->value("qmlFile").toString(); - m_qmlFile=Utils::PathUtils().InsertDataPath(m_qmlFile); + // if a saved configuration exists load it + if (qSettings != 0) { + m_qmlFile = qSettings->value("qmlFile").toString(); + m_qmlFile = Utils::PathUtils().InsertDataPath(m_qmlFile); - m_earthFile = qSettings->value("earthFile").toString(); - m_earthFile=Utils::PathUtils().InsertDataPath(m_earthFile); + m_earthFile = qSettings->value("earthFile").toString(); + m_earthFile = Utils::PathUtils().InsertDataPath(m_earthFile); - m_openGLEnabled = qSettings->value("openGLEnabled", true).toBool(); - m_terrainEnabled = qSettings->value("terrainEnabled").toBool(); + m_openGLEnabled = qSettings->value("openGLEnabled", true).toBool(); + m_terrainEnabled = qSettings->value("terrainEnabled").toBool(); m_actualPositionUsed = qSettings->value("actualPositionUsed").toBool(); - m_latitude = qSettings->value("latitude").toDouble(); - m_longitude = qSettings->value("longitude").toDouble(); - m_altitude = qSettings->value("altitude").toDouble(); - m_cacheOnly = qSettings->value("cacheOnly").toBool(); + m_latitude = qSettings->value("latitude").toDouble(); + m_longitude = qSettings->value("longitude").toDouble(); + m_altitude = qSettings->value("altitude").toDouble(); + m_cacheOnly = qSettings->value("cacheOnly").toBool(); } } @@ -58,15 +58,16 @@ PfdQmlGadgetConfiguration::PfdQmlGadgetConfiguration(QString classId, QSettings IUAVGadgetConfiguration *PfdQmlGadgetConfiguration::clone() { PfdQmlGadgetConfiguration *m = new PfdQmlGadgetConfiguration(this->classId()); - m->m_qmlFile = m_qmlFile; - m->m_openGLEnabled = m_openGLEnabled; - m->m_earthFile = m_earthFile; - m->m_terrainEnabled = m_terrainEnabled; + + m->m_qmlFile = m_qmlFile; + m->m_openGLEnabled = m_openGLEnabled; + m->m_earthFile = m_earthFile; + m->m_terrainEnabled = m_terrainEnabled; m->m_actualPositionUsed = m_actualPositionUsed; - m->m_latitude = m_latitude; - m->m_longitude = m_longitude; - m->m_altitude = m_altitude; - m->m_cacheOnly = m_cacheOnly; + m->m_latitude = m_latitude; + m->m_longitude = m_longitude; + m->m_altitude = m_altitude; + m->m_cacheOnly = m_cacheOnly; return m; } @@ -75,8 +76,10 @@ IUAVGadgetConfiguration *PfdQmlGadgetConfiguration::clone() * Saves a configuration. * */ -void PfdQmlGadgetConfiguration::saveConfig(QSettings* qSettings) const { - QString qmlFile = Utils::PathUtils().RemoveDataPath(m_qmlFile); +void PfdQmlGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ + QString qmlFile = Utils::PathUtils().RemoveDataPath(m_qmlFile); + qSettings->setValue("qmlFile", qmlFile); QString earthFile = Utils::PathUtils().RemoveDataPath(m_earthFile); qSettings->setValue("earthFile", earthFile); diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.h index 3f12f25b1..f09e4a0ad 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.h @@ -21,33 +21,86 @@ using namespace Core; -class PfdQmlGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT +class PfdQmlGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit PfdQmlGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit PfdQmlGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void setQmlFile(const QString &fileName) { m_qmlFile=fileName; } - void setEarthFile(const QString &fileName) { m_earthFile=fileName; } - void setOpenGLEnabled(bool flag) { m_openGLEnabled = flag; } - void setTerrainEnabled(bool flag) { m_terrainEnabled = flag; } - void setActualPositionUsed(bool flag) { m_actualPositionUsed = flag; } - void setLatitude(double value) { m_latitude = value; } - void setLongitude(double value) { m_longitude = value; } - void setAltitude(double value) { m_altitude = value; } - void setCacheOnly(bool flag) { m_cacheOnly = flag; } + void setQmlFile(const QString &fileName) + { + m_qmlFile = fileName; + } + void setEarthFile(const QString &fileName) + { + m_earthFile = fileName; + } + void setOpenGLEnabled(bool flag) + { + m_openGLEnabled = flag; + } + void setTerrainEnabled(bool flag) + { + m_terrainEnabled = flag; + } + void setActualPositionUsed(bool flag) + { + m_actualPositionUsed = flag; + } + void setLatitude(double value) + { + m_latitude = value; + } + void setLongitude(double value) + { + m_longitude = value; + } + void setAltitude(double value) + { + m_altitude = value; + } + void setCacheOnly(bool flag) + { + m_cacheOnly = flag; + } - QString qmlFile() const { return m_qmlFile; } - QString earthFile() const { return m_earthFile; } - bool openGLEnabled() const { return m_openGLEnabled; } - bool terrainEnabled() const { return m_terrainEnabled; } - bool actualPositionUsed() const { return m_actualPositionUsed; } - double latitude() const { return m_latitude; } - double longitude() const { return m_longitude; } - double altitude() const { return m_altitude; } - bool cacheOnly() const { return m_cacheOnly; } + QString qmlFile() const + { + return m_qmlFile; + } + QString earthFile() const + { + return m_earthFile; + } + bool openGLEnabled() const + { + return m_openGLEnabled; + } + bool terrainEnabled() const + { + return m_terrainEnabled; + } + bool actualPositionUsed() const + { + return m_actualPositionUsed; + } + double latitude() const + { + return m_latitude; + } + double longitude() const + { + return m_longitude; + } + double altitude() const + { + return m_altitude; + } + bool cacheOnly() const + { + return m_cacheOnly; + } - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.cpp index c49fa904d..0fa0503aa 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.cpp @@ -21,19 +21,18 @@ #include PfdQmlGadgetFactory::PfdQmlGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("PfdQmlGadget"), - tr("PFD (qml)"), - parent) -{ -} + IUAVGadgetFactory(QString("PfdQmlGadget"), + tr("PFD (qml)"), + parent) +{} PfdQmlGadgetFactory::~PfdQmlGadgetFactory() -{ -} +{} -Core::IUAVGadget* PfdQmlGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *PfdQmlGadgetFactory::createGadget(QWidget *parent) { - PfdQmlGadgetWidget* gadgetWidget = new PfdQmlGadgetWidget(parent); + PfdQmlGadgetWidget *gadgetWidget = new PfdQmlGadgetWidget(parent); + return new PfdQmlGadget(QString("PfdQmlGadget"), gadgetWidget, parent); } @@ -44,6 +43,5 @@ IUAVGadgetConfiguration *PfdQmlGadgetFactory::createConfiguration(QSettings *qSe IOptionsPage *PfdQmlGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new PfdQmlGadgetOptionsPage(qobject_cast(config)); + return new PfdQmlGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.h b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.h index 348afc249..23e4610ed 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.h @@ -26,8 +26,7 @@ class IUAVGadgetFactory; using namespace Core; -class PfdQmlGadgetFactory : public IUAVGadgetFactory -{ +class PfdQmlGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: PfdQmlGadgetFactory(QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.cpp index 2221ec691..0b20b8480 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.cpp @@ -27,18 +27,17 @@ #include PfdQmlGadgetOptionsPage::PfdQmlGadgetOptionsPage(PfdQmlGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *PfdQmlGadgetOptionsPage::createPage(QWidget *parent) { options_page = new Ui::PfdQmlGadgetOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget(parent); - //main layout + // main layout options_page->setupUi(optionsPageWidget); // Restore the contents from the settings: @@ -97,5 +96,4 @@ void PfdQmlGadgetOptionsPage::apply() } void PfdQmlGadgetOptionsPage::finish() -{ -} +{} diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.h index 44711bdb8..a324c0709 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.h @@ -29,14 +29,13 @@ class IUAVGadgetConfiguration; class PfdQmlGadgetConfiguration; namespace Ui { - class PfdQmlGadgetOptionsPage; +class PfdQmlGadgetOptionsPage; } using namespace Core; -class PfdQmlGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class PfdQmlGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit PfdQmlGadgetOptionsPage(PfdQmlGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp index 4cde9f2e8..60fe0539c 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp @@ -41,36 +41,38 @@ PfdQmlGadgetWidget::PfdQmlGadgetWidget(QWidget *parent) : m_longitude(10.158932), m_altitude(2000) { - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setResizeMode(SizeRootObjectToView); - //setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); + // setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); QStringList objectsToExport; objectsToExport << "VelocityActual" << - "PositionActual" << - "AttitudeActual" << - "Accels" << - "VelocityDesired" << - "PositionDesired" << - "AttitudeHoldDesired" << - "GPSPosition" << - "GCSTelemetryStats" << - "FlightBatteryState"; + "PositionActual" << + "AttitudeActual" << + "Accels" << + "VelocityDesired" << + "PositionDesired" << + "AttitudeHoldDesired" << + "GPSPosition" << + "GCSTelemetryStats" << + "FlightBatteryState"; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - foreach (const QString &objectName, objectsToExport) { - UAVObject* object = objManager->getObject(objectName); - if (object) + foreach(const QString &objectName, objectsToExport) { + UAVObject *object = objManager->getObject(objectName); + + if (object) { engine()->rootContext()->setContextProperty(objectName, object); - else + } else { qWarning() << "Failed to load object" << objectName; + } } - //to expose settings values + // to expose settings values engine()->rootContext()->setContextProperty("qmlWidget", this); #ifdef USE_OSG qmlRegisterType("org.OpenPilot", 1, 0, "OsgEarth"); @@ -78,8 +80,7 @@ PfdQmlGadgetWidget::PfdQmlGadgetWidget(QWidget *parent) : } PfdQmlGadgetWidget::~PfdQmlGadgetWidget() -{ -} +{} void PfdQmlGadgetWidget::setQmlFile(QString fn) { @@ -89,7 +90,7 @@ void PfdQmlGadgetWidget::setQmlFile(QString fn) SvgImageProvider *svgProvider = new SvgImageProvider(fn); engine()->addImageProvider("svg", svgProvider); - //it's necessary to allow qml side to query svg element position + // it's necessary to allow qml side to query svg element position engine()->rootContext()->setContextProperty("svgRenderer", svgProvider); engine()->setBaseUrl(QUrl::fromLocalFile(fn)); @@ -112,10 +113,12 @@ void PfdQmlGadgetWidget::setEarthFile(QString arg) void PfdQmlGadgetWidget::setTerrainEnabled(bool arg) { bool wasEnabled = terrainEnabled(); + m_terrainEnabled = arg; - if (wasEnabled != terrainEnabled()) + if (wasEnabled != terrainEnabled()) { emit terrainEnabledChanged(terrainEnabled()); + } } void PfdQmlGadgetWidget::setOpenGLEnabled(bool arg) @@ -124,18 +127,19 @@ void PfdQmlGadgetWidget::setOpenGLEnabled(bool arg) m_openGLEnabled = arg; qDebug() << Q_FUNC_INFO << "Set OPENGL" << m_openGLEnabled; - if (m_openGLEnabled) + if (m_openGLEnabled) { setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); - else + } else { setViewport(new QWidget); + } - //update terrainEnabled status with opengl status chaged + // update terrainEnabled status with opengl status chaged setTerrainEnabled(m_terrainEnabled); } } -//Switch between PositionActual UAVObject position -//and pre-defined latitude/longitude/altitude properties +// Switch between PositionActual UAVObject position +// and pre-defined latitude/longitude/altitude properties void PfdQmlGadgetWidget::setActualPositionUsed(bool arg) { if (m_actualPositionUsed != arg) { @@ -146,7 +150,7 @@ void PfdQmlGadgetWidget::setActualPositionUsed(bool arg) void PfdQmlGadgetWidget::setLatitude(double arg) { - //not sure qFuzzyCompare is accurate enough for geo coordinates + // not sure qFuzzyCompare is accurate enough for geo coordinates if (m_latitude != arg) { m_latitude = arg; emit latitudeChanged(arg); @@ -163,7 +167,7 @@ void PfdQmlGadgetWidget::setLongitude(double arg) void PfdQmlGadgetWidget::setAltitude(double arg) { - if (!qFuzzyCompare(m_altitude,arg)) { + if (!qFuzzyCompare(m_altitude, arg)) { m_altitude = arg; emit altitudeChanged(arg); } diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.h b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.h index 56cc17dd4..77ab49f9d 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.h @@ -20,31 +20,47 @@ #include "pfdqmlgadgetconfiguration.h" #include -class PfdQmlGadgetWidget : public QDeclarativeView -{ - Q_OBJECT - Q_PROPERTY(QString earthFile READ earthFile WRITE setEarthFile NOTIFY earthFileChanged) +class PfdQmlGadgetWidget : public QDeclarativeView { + Q_OBJECT Q_PROPERTY(QString earthFile READ earthFile WRITE setEarthFile NOTIFY earthFileChanged) Q_PROPERTY(bool terrainEnabled READ terrainEnabled WRITE setTerrainEnabled NOTIFY terrainEnabledChanged) Q_PROPERTY(bool actualPositionUsed READ actualPositionUsed WRITE setActualPositionUsed NOTIFY actualPositionUsedChanged) - //pre-defined fallback position + // pre-defined fallback position Q_PROPERTY(double latitude READ latitude WRITE setLatitude NOTIFY latitudeChanged) Q_PROPERTY(double longitude READ longitude WRITE setLongitude NOTIFY longitudeChanged) Q_PROPERTY(double altitude READ altitude WRITE setAltitude NOTIFY altitudeChanged) public: PfdQmlGadgetWidget(QWidget *parent = 0); - ~PfdQmlGadgetWidget(); + ~PfdQmlGadgetWidget(); void setQmlFile(QString fn); - QString earthFile() const { return m_earthFile; } - bool terrainEnabled() const { return m_terrainEnabled && m_openGLEnabled; } + QString earthFile() const + { + return m_earthFile; + } + bool terrainEnabled() const + { + return m_terrainEnabled && m_openGLEnabled; + } - bool actualPositionUsed() const { return m_actualPositionUsed; } - double latitude() const { return m_latitude; } - double longitude() const { return m_longitude; } - double altitude() const { return m_altitude; } + bool actualPositionUsed() const + { + return m_actualPositionUsed; + } + double latitude() const + { + return m_latitude; + } + double longitude() const + { + return m_longitude; + } + double altitude() const + { + return m_altitude; + } public slots: void setEarthFile(QString arg); diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.cpp index e71763638..085ca06f6 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.cpp @@ -24,32 +24,31 @@ PfdQmlPlugin::PfdQmlPlugin() { - // Do nothing + // Do nothing } PfdQmlPlugin::~PfdQmlPlugin() { - // Do nothing + // Do nothing } -bool PfdQmlPlugin::initialize(const QStringList& args, QString *errMsg) +bool PfdQmlPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new PfdQmlGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new PfdQmlGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void PfdQmlPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void PfdQmlPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(PfdQmlPlugin) - diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.h b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.h index 8686cf9c9..344d22128 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.h @@ -21,16 +21,15 @@ class PfdQmlGadgetFactory; -class PfdQmlPlugin : public ExtensionSystem::IPlugin -{ +class PfdQmlPlugin : public ExtensionSystem::IPlugin { public: - PfdQmlPlugin(); - ~PfdQmlPlugin(); + PfdQmlPlugin(); + ~PfdQmlPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList &arguments, QString *errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList &arguments, QString *errorString); + void shutdown(); private: - PfdQmlGadgetFactory *mf; + PfdQmlGadgetFactory *mf; }; #endif /* PFDQMLPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.cpp b/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.cpp index e0124789e..da2080638 100644 --- a/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.cpp +++ b/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.cpp @@ -44,44 +44,43 @@ #include - /** - * Sets the file to use for logging and takes the parent plugin - * to connect to stop logging signal - * @param[in] file File name to write to - * @param[in] parent plugin - */ -bool PowerlogThread::openFile(QString file, PowerlogPlugin * parent) + * Sets the file to use for logging and takes the parent plugin + * to connect to stop logging signal + * @param[in] file File name to write to + * @param[in] parent plugin + */ +bool PowerlogThread::openFile(QString file, PowerlogPlugin *parent) { logFile.setFileName(file); logFile.open(QIODevice::WriteOnly); fileStream.setDevice(&logFile); - connect(parent,SIGNAL(stopLoggingSignal()),this,SLOT(stopLogging())); + connect(parent, SIGNAL(stopLoggingSignal()), this, SLOT(stopLogging())); return true; }; /** - * Get all logs from Powerlog - */ + * Get all logs from Powerlog + */ void PowerlogThread::run() { - - // TODO: pop up a dialog here! qDebug() << "Connect a Junsi PowerLog 6S and watch the logging output"; pjrc_rawhid hidHandle; - int numDevices = hidHandle.open(1, 0x0483,0x5750,0,0); //0xff9c,0x0001); - if( numDevices == 0 ) - numDevices = hidHandle.open(1,0x0483,0,0,0); + int numDevices = hidHandle.open(1, 0x0483, 0x5750, 0, 0); // 0xff9c,0x0001); + if (numDevices == 0) { + numDevices = hidHandle.open(1, 0x0483, 0, 0, 0); + } qDebug() << numDevices << " device(s) opened"; - if (numDevices == 0) + if (numDevices == 0) { return; + } - //hidHandle.mytest(0); + // hidHandle.mytest(0); char buf[BUF_LEN]; buf[0] = 2; @@ -89,119 +88,118 @@ void PowerlogThread::run() fileStream << "Interval,Current,Volt,Cap,Cell1,Cell2,Cell3,Cell4,Cell5,Cell6,RPM,Temp0,Temp1,Temp2,Temp3,Period,Pulse\n"; - while (int received = hidHandle.receive(0, buf, BUF_LEN, 3500) ) { + while (int received = hidHandle.receive(0, buf, BUF_LEN, 3500)) { ShowInf(buf); fileStream.flush(); // Just to be sure... } stopLogging(); - } /** - * Pass this command to the correct thread then close the file - */ + * Pass this command to the correct thread then close the file + */ void PowerlogThread::stopLogging() { QWriteLocker locker(&lock); + fileStream.flush(); logFile.close(); quit(); } /** - * Formats the content of the buffer we just read and write - * to the logfile - */ + * Formats the content of the buffer we just read and write + * to the logfile + */ void PowerlogThread::ShowInf(char *pBuf) { POWERLOG_HID_PACK Inf; int i; int Count; - Count=0; - Inf.Len = pBuf[Count]; - Count += sizeof(Inf.Len); + Count = 0; + Inf.Len = pBuf[Count]; + Count += sizeof(Inf.Len); - Inf.Type = pBuf[Count]; - Count += sizeof(Inf.Type); + Inf.Type = pBuf[Count]; + Count += sizeof(Inf.Type); Inf.Interval = *((DWORD *)&pBuf[Count]); fileStream << QString::number(Inf.Interval) << ","; - Count += sizeof(Inf.Interval); + Count += sizeof(Inf.Interval); Inf.LogState = pBuf[Count]; - Count += sizeof(Inf.LogState); + Count += sizeof(Inf.LogState); - if(((Inf.Type == TYPE_DATA_ONLINE)||(Inf.Type == TYPE_DATA_OFFLINE)) && (Inf.Len == 0x29))//0x27 - { + if (((Inf.Type == TYPE_DATA_ONLINE) || (Inf.Type == TYPE_DATA_OFFLINE)) && (Inf.Len == 0x29)) { // 0x27 Inf.Current = *((SHORT *)&pBuf[Count]); - Count += sizeof(Inf.Current); - GetShowValue(QString("Current"),Inf.Current,5,2); + Count += sizeof(Inf.Current); + GetShowValue(QString("Current"), Inf.Current, 5, 2); Inf.Volt = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.Volt); - GetShowValue(QString("Voltage"),Inf.Volt,5,2); + Count += sizeof(Inf.Volt); + GetShowValue(QString("Voltage"), Inf.Volt, 5, 2); - Inf.Cap = *((DWORD *)&pBuf[Count]); - Count += sizeof(Inf.Cap); - GetShowValue(QString("Cap"),Inf.Cap,6,0); + Inf.Cap = *((DWORD *)&pBuf[Count]); + Count += sizeof(Inf.Cap); + GetShowValue(QString("Cap"), Inf.Cap, 6, 0); - for(i=0;i<6;i++) - { - Inf.Cell[i] = *((SHORT *)&pBuf[Count]); - Count += sizeof(Inf.Cell[i]); + for (i = 0; i < 6; i++) { + Inf.Cell[i] = *((SHORT *)&pBuf[Count]); + Count += sizeof(Inf.Cell[i]); } - GetShowValue(QString("Cell 1"),Inf.Cell[0],5,3); - GetShowValue(QString("Cell 2"),Inf.Cell[1],5,3); - GetShowValue(QString("Cell 3"),Inf.Cell[2],5,3); - GetShowValue(QString("Cell 4"),Inf.Cell[3],5,3); - GetShowValue(QString("Cell 5"),Inf.Cell[4],5,3); - GetShowValue(QString("Cell 6"),Inf.Cell[5],5,3); + GetShowValue(QString("Cell 1"), Inf.Cell[0], 5, 3); + GetShowValue(QString("Cell 2"), Inf.Cell[1], 5, 3); + GetShowValue(QString("Cell 3"), Inf.Cell[2], 5, 3); + GetShowValue(QString("Cell 4"), Inf.Cell[3], 5, 3); + GetShowValue(QString("Cell 5"), Inf.Cell[4], 5, 3); + GetShowValue(QString("Cell 6"), Inf.Cell[5], 5, 3); Inf.RPM = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.RPM); - GetShowValue(QString("RPM"),Inf.RPM,6,0); - - for(i=0;i<4;i++) - { - Inf.Temp[i] = *((SHORT *)&pBuf[Count]); - Count += sizeof(Inf.Temp[i]); + Count += sizeof(Inf.RPM); + GetShowValue(QString("RPM"), Inf.RPM, 6, 0); + for (i = 0; i < 4; i++) { + Inf.Temp[i] = *((SHORT *)&pBuf[Count]); + Count += sizeof(Inf.Temp[i]); } - GetShowValue(QString("Int Temp0"),Inf.Temp[0],4,1); + GetShowValue(QString("Int Temp0"), Inf.Temp[0], 4, 1); - if (Inf.Temp[1]==0x7fff) - fileStream << "0.0,"; - else - GetShowValue(QString("Ext temp1"),Inf.Temp[1],4,1); - - if (Inf.Temp[2]==0x7fff) + if (Inf.Temp[1] == 0x7fff) { fileStream << "0.0,"; - else - GetShowValue(QString("Ext temp2"),Inf.Temp[2],4,1); + } else { + GetShowValue(QString("Ext temp1"), Inf.Temp[1], 4, 1); + } - if (Inf.Temp[3]==0x7fff) + if (Inf.Temp[2] == 0x7fff) { fileStream << "0.0,"; - else - GetShowValue(QString("Ext temp3"),Inf.Temp[3],4,1); + } else { + GetShowValue(QString("Ext temp2"), Inf.Temp[2], 4, 1); + } + + if (Inf.Temp[3] == 0x7fff) { + fileStream << "0.0,"; + } else { + GetShowValue(QString("Ext temp3"), Inf.Temp[3], 4, 1); + } Inf.Period = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.Period); - GetShowValue(QString("Period:"),Inf.Period,6,0); + Count += sizeof(Inf.Period); + GetShowValue(QString("Period:"), Inf.Period, 6, 0); Inf.Pulse = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.Pulse); - GetShowValue(QString("Pulse:"),Inf.Pulse,6,0); + Count += sizeof(Inf.Pulse); + GetShowValue(QString("Pulse:"), Inf.Pulse, 6, 0); fileStream << "\n"; } } /** - * Formats a numeric value - */ + * Formats a numeric value + */ void PowerlogThread::GetShowValue(QString label, DWORD Value, WORD Len, WORD Dot) { QString out; @@ -211,23 +209,21 @@ void PowerlogThread::GetShowValue(QString label, DWORD Value, WORD Len, WORD Dot Value = -Value; } - if(Dot==1) - fileStream << Value/10 << "." << Value%10; // printf("%ld.%01lu",Value/10,Value%10); - else if(Dot==2) - fileStream << Value/100 << "." << Value%100; // printf("%ld.%02lu",Value/100,Value%100); - else if(Dot==3) - fileStream << Value/1000 << "." << Value%1000; // printf("%ld.%03lu",Value/1000,Value%1000); - else if(Dot==4) - fileStream << Value/10000 << "." << Value%10000; // printf("%ld.%04lu",Value/10000,Value%10000); - else - fileStream << Value; // printf("%ld",Value); - + if (Dot == 1) { + fileStream << Value / 10 << "." << Value % 10; // printf("%ld.%01lu",Value/10,Value%10); + } else if (Dot == 2) { + fileStream << Value / 100 << "." << Value % 100; // printf("%ld.%02lu",Value/100,Value%100); + } else if (Dot == 3) { + fileStream << Value / 1000 << "." << Value % 1000; // printf("%ld.%03lu",Value/1000,Value%1000); + } else if (Dot == 4) { + fileStream << Value / 10000 << "." << Value % 10000; // printf("%ld.%04lu",Value/10000,Value%10000); + } else { + fileStream << Value; // printf("%ld",Value); + } fileStream << out << ","; - } - /**************************************************************** Logging plugin ********************************/ @@ -237,33 +233,29 @@ PowerlogPlugin::PowerlogPlugin() : devSerialNumber(""), logging(false), loggingThread(NULL) -{ - -} +{} PowerlogPlugin::~PowerlogPlugin() -{ - -} +{} /** - * Add Powerlog plugin entry to File menu - */ -bool PowerlogPlugin::initialize(const QStringList& args, QString *errMsg) + * Add Powerlog plugin entry to File menu + */ +bool PowerlogPlugin::initialize(const QStringList & args, QString *errMsg) { Q_UNUSED(args); Q_UNUSED(errMsg); // Add Menu entry - Core::ActionManager* am = Core::ICore::instance()->actionManager(); - Core::ActionContainer* ac = am->actionContainer(Core::Constants::M_TOOLS); + Core::ActionManager *am = Core::ICore::instance()->actionManager(); + Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_TOOLS); // Command to start logging cmd = am->registerAction(new QAction(this), - "PowerlogPlugin.Transfer", - QList() << - Core::Constants::C_GLOBAL_ID); + "PowerlogPlugin.Transfer", + QList() << + Core::Constants::C_GLOBAL_ID); cmd->action()->setText("Receive from PowerLog6S..."); ac->menu()->addSeparator(); @@ -275,16 +267,16 @@ bool PowerlogPlugin::initialize(const QStringList& args, QString *errMsg) // At this stage we know that other plugins we depend upon are // initialized, in prticular the USB Monitor is now running: USBMonitor *mon = USBMonitor::instance(); - connect(mon,SIGNAL(deviceDiscovered(USBPortInfo)), this, SLOT(devConnected(USBPortInfo))); - connect(mon,SIGNAL(deviceRemoved(USBPortInfo)), this, SLOT(devRemoved(USBPortInfo))); + connect(mon, SIGNAL(deviceDiscovered(USBPortInfo)), this, SLOT(devConnected(USBPortInfo))); + connect(mon, SIGNAL(deviceRemoved(USBPortInfo)), this, SLOT(devRemoved(USBPortInfo))); return true; } /** - * The action that is triggered by the menu item which opens the - * file and begins log reception if successful - */ + * The action that is triggered by the menu item which opens the + * file and begins log reception if successful + */ void PowerlogPlugin::receiveLog() { if (logging) { @@ -293,29 +285,30 @@ void PowerlogPlugin::receiveLog() cmd->action()->setText("Receive from PowerLog6S..."); } else { QString fileName = QFileDialog::getSaveFileName(NULL, tr("Log filename"), - tr("PowerLog-%0.csv").arg(QDateTime::currentDateTime().toString("yyyy-MM-dd_hh-mm-ss")), - tr("Comma Separated Values (*.csv)")); - if (fileName.isEmpty()) + tr("PowerLog-%0.csv").arg(QDateTime::currentDateTime().toString("yyyy-MM-dd_hh-mm-ss")), + tr("Comma Separated Values (*.csv)")); + if (fileName.isEmpty()) { return; + } loggingThread = new PowerlogThread(); - if (loggingThread->openFile(fileName,this)) { + if (loggingThread->openFile(fileName, this)) { loggingThread->start(); cmd->action()->setText("Stop PowerLog6S reception"); logging = true; } } - } /** - Device connected, check whether it is a powerlog & act accordingly - */ + Device connected, check whether it is a powerlog & act accordingly + */ void PowerlogPlugin::devConnected(USBPortInfo port) { - if (devSerialNumber.length() > 0) + if (devSerialNumber.length() > 0) { return; - if ((port.vendorID == 0x0483) && (port.productID==0x5750)) { + } + if ((port.vendorID == 0x0483) && (port.productID == 0x5750)) { devSerialNumber = port.serialNumber; cmd->action()->setEnabled(true); } @@ -323,18 +316,19 @@ void PowerlogPlugin::devConnected(USBPortInfo port) /** - Device Removed, check whether it is a powerlog & act accordingly. - As when the device is removed, we don't get the info on the device, - we have to list all available remaining devices and check if the serial - number of our device is missing... - */ + Device Removed, check whether it is a powerlog & act accordingly. + As when the device is removed, we don't get the info on the device, + we have to list all available remaining devices and check if the serial + number of our device is missing... + */ void PowerlogPlugin::devRemoved(USBPortInfo port) { bool foundDevice; + QList ports = USBMonitor::instance()->availableDevices(); foreach(USBPortInfo port, ports) { - if ((port.vendorID == 0x0483) && (port.productID==0x5750) && - (devSerialNumber == port.serialNumber)) { + if ((port.vendorID == 0x0483) && (port.productID == 0x5750) && + (devSerialNumber == port.serialNumber)) { foundDevice = true; break; } @@ -343,8 +337,9 @@ void PowerlogPlugin::devRemoved(USBPortInfo port) devSerialNumber = QString(""); cmd->action()->setEnabled(false); // Also stop logging in case we were logging: - if (loggingThread) + if (loggingThread) { loggingThread->stopLogging(); + } } } @@ -354,7 +349,7 @@ void PowerlogPlugin::extensionsInitialized() cmd->action()->setEnabled(false); QList ports = USBMonitor::instance()->availableDevices(); foreach(USBPortInfo port, ports) { - if ((port.vendorID == 0x0483) && (port.productID==0x5750)) { + if ((port.vendorID == 0x0483) && (port.productID == 0x5750)) { devSerialNumber = port.serialNumber; cmd->action()->setEnabled(true); break; diff --git a/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.h b/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.h index bcb971447..517f950de 100644 --- a/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.h +++ b/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.h @@ -41,50 +41,44 @@ using namespace std; -typedef unsigned long ULONG; // 4 Bytes +typedef unsigned long ULONG; // 4 Bytes typedef short SHORT; -typedef unsigned short USHORT; // 2 Bytes -typedef unsigned char BYTE; // 1 Byte -typedef unsigned short WORD; // 2 Bytes -typedef unsigned long DWORD; // 4 Bytes - +typedef unsigned short USHORT; // 2 Bytes +typedef unsigned char BYTE; // 1 Byte +typedef unsigned short WORD; // 2 Bytes +typedef unsigned long DWORD; // 4 Bytes #define BUF_LEN 64 -struct POWERLOG_HID_PACK -{ - BYTE Len; - BYTE Type; - DWORD Interval; - BYTE LogState; - SHORT Current; - USHORT Volt; - DWORD Cap; - SHORT Cell[6]; - USHORT RPM; - SHORT Temp[4]; - USHORT Period; - USHORT Pulse; +struct POWERLOG_HID_PACK { + BYTE Len; + BYTE Type; + DWORD Interval; + BYTE LogState; + SHORT Current; + USHORT Volt; + DWORD Cap; + SHORT Cell[6]; + USHORT RPM; + SHORT Temp[4]; + USHORT Period; + USHORT Pulse; }; -enum -{ - TYPE_DATA_ONLINE = 0x10, - TYPE_DATA_OFFLINE = 0x11, - TYPE_ORDER = 0x20, +enum { + TYPE_DATA_ONLINE = 0x10, + TYPE_DATA_OFFLINE = 0x11, + TYPE_ORDER = 0x20, }; - - class PowerlogPlugin; -class PowerlogThread : public QThread -{ +class PowerlogThread : public QThread { Q_OBJECT public: - bool openFile(QString file, PowerlogPlugin * parent); + bool openFile(QString file, PowerlogPlugin *parent); private slots: @@ -99,12 +93,11 @@ protected: private: void ShowInf(char *pBuf); - void GetShowValue(QString label,DWORD Value,WORD Len,WORD Dot); + void GetShowValue(QString label, DWORD Value, WORD Len, WORD Dot); }; -class PowerlogPlugin : public ExtensionSystem::IPlugin -{ +class PowerlogPlugin : public ExtensionSystem::IPlugin { Q_OBJECT @@ -113,7 +106,7 @@ public: ~PowerlogPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); void setPowerlogMenuTitle(QString str); @@ -130,11 +123,10 @@ private slots: void devRemoved(USBPortInfo); private: - Core::Command* cmd; + Core::Command *cmd; QString devSerialNumber; - PowerlogThread* loggingThread; + PowerlogThread *loggingThread; bool logging; - }; #endif /* POWERLOGPLUGIN_H_ */ /** diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.cpp index 7a65746c7..d5aa1749c 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -31,10 +31,9 @@ #include "qmlviewgadgetconfiguration.h" QmlViewGadget::QmlViewGadget(QString classId, QmlViewGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} QmlViewGadget::~QmlViewGadget() { @@ -42,14 +41,15 @@ QmlViewGadget::~QmlViewGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void QmlViewGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void QmlViewGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - QmlViewGadgetConfiguration *m = qobject_cast(config); + QmlViewGadgetConfiguration *m = qobject_cast(config); + m_widget->setQmlFile(m->dialFile()); m_widget->enableOpenGL(m->useOpenGL()); } diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.h b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.h index 036ce47d0..152c58042 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.h +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,15 +38,17 @@ class QmlViewGadgetWidget; using namespace Core; -class QmlViewGadget : public Core::IUAVGadget -{ +class QmlViewGadget : public Core::IUAVGadget { Q_OBJECT public: QmlViewGadget(QString classId, QmlViewGadgetWidget *widget, QWidget *parent = 0); ~QmlViewGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: QmlViewGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.cpp index 40d096ebb..95183e09b 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,15 +32,15 @@ * Loads a saved configuration or defaults if non exist. * */ -QmlViewGadgetConfiguration::QmlViewGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +QmlViewGadgetConfiguration::QmlViewGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_defaultDial("Unknown") { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { QString dialFile = qSettings->value("dialFile").toString(); useOpenGLFlag = qSettings->value("useOpenGLFlag").toBool(); - m_defaultDial=Utils::PathUtils().InsertDataPath(dialFile); + m_defaultDial = Utils::PathUtils().InsertDataPath(dialFile); } } @@ -51,7 +51,8 @@ QmlViewGadgetConfiguration::QmlViewGadgetConfiguration(QString classId, QSetting IUAVGadgetConfiguration *QmlViewGadgetConfiguration::clone() { QmlViewGadgetConfiguration *m = new QmlViewGadgetConfiguration(this->classId()); - m->m_defaultDial=m_defaultDial; + + m->m_defaultDial = m_defaultDial; m->useOpenGLFlag = useOpenGLFlag; return m; } @@ -60,8 +61,10 @@ IUAVGadgetConfiguration *QmlViewGadgetConfiguration::clone() * Saves a configuration. * */ -void QmlViewGadgetConfiguration::saveConfig(QSettings* qSettings) const { +void QmlViewGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ QString dialFile = Utils::PathUtils().RemoveDataPath(m_defaultDial); + qSettings->setValue("dialFile", dialFile); qSettings->setValue("useOpenGLFlag", useOpenGLFlag); } diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.h index 5e8f4687a..416d5a8db 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,21 +32,32 @@ using namespace Core; -class QmlViewGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT +class QmlViewGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit QmlViewGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit QmlViewGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - //set dial configuration functions - void setDialFile(QString dialFile){m_defaultDial=dialFile;} - void setUseOpenGL(bool flag) { useOpenGLFlag = flag; } + // set dial configuration functions + void setDialFile(QString dialFile) + { + m_defaultDial = dialFile; + } + void setUseOpenGL(bool flag) + { + useOpenGLFlag = flag; + } - //get dial configuration functions - QString dialFile() {return m_defaultDial;} - bool useOpenGL() { return useOpenGLFlag; } + // get dial configuration functions + QString dialFile() + { + return m_defaultDial; + } + bool useOpenGL() + { + return useOpenGLFlag; + } - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.cpp index 6d8f0b7c5..20b7693bb 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,29 +32,27 @@ #include QmlViewGadgetFactory::QmlViewGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("QmlViewGadget"), - tr("QML Viewer, QML"), - parent) -{ -} + IUAVGadgetFactory(QString("QmlViewGadget"), + tr("QML Viewer, QML"), + parent) +{} QmlViewGadgetFactory::~QmlViewGadgetFactory() -{ -} +{} -Core::IUAVGadget* QmlViewGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *QmlViewGadgetFactory::createGadget(QWidget *parent) { - QmlViewGadgetWidget* gadgetWidget = new QmlViewGadgetWidget(parent); + QmlViewGadgetWidget *gadgetWidget = new QmlViewGadgetWidget(parent); + return new QmlViewGadget(QString("QmlViewGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *QmlViewGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *QmlViewGadgetFactory::createConfiguration(QSettings *qSettings) { return new QmlViewGadgetConfiguration(QString("QmlViewGadget"), qSettings); } IOptionsPage *QmlViewGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new QmlViewGadgetOptionsPage(qobject_cast(config)); + return new QmlViewGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.h b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.h index babbf2ae6..759778875 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class QmlViewGadgetFactory : public IUAVGadgetFactory -{ +class QmlViewGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: QmlViewGadgetFactory(QObject *parent = 0); ~QmlViewGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.cpp index 9e0c20a6e..15921007b 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,24 +38,22 @@ #include QmlViewGadgetOptionsPage::QmlViewGadgetOptionsPage(QmlViewGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *QmlViewGadgetOptionsPage::createPage(QWidget *parent) { Q_UNUSED(parent); options_page = new Ui::QmlViewGadgetOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget; - //main layout + // main layout options_page->setupUi(optionsPageWidget); - // Restore the contents from the settings: options_page->svgSourceFile->setExpectedKind(Utils::PathChooser::File); options_page->svgSourceFile->setPromptDialogFilter(tr("QML file (*.qml)")); @@ -79,5 +77,4 @@ void QmlViewGadgetOptionsPage::apply() } void QmlViewGadgetOptionsPage::finish() -{ -} +{} diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.h index 9fd2f335a..6dfd6c48d 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -40,14 +40,13 @@ class IUAVGadgetConfiguration; class QmlViewGadgetConfiguration; namespace Ui { - class QmlViewGadgetOptionsPage; +class QmlViewGadgetOptionsPage; } using namespace Core; -class QmlViewGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class QmlViewGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit QmlViewGadgetOptionsPage(QmlViewGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp index fda9042ef..4bc172fee 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -44,35 +44,36 @@ QmlViewGadgetWidget::QmlViewGadgetWidget(QWidget *parent) : QDeclarativeView(parent) { - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setResizeMode(SizeRootObjectToView); QStringList objectsToExport; objectsToExport << "VelocityActual" << - "PositionActual" << - "AttitudeActual" << - "GPSPosition" << - "GCSTelemetryStats" << - "FlightBatteryState"; + "PositionActual" << + "AttitudeActual" << + "GPSPosition" << + "GCSTelemetryStats" << + "FlightBatteryState"; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - foreach (const QString &objectName, objectsToExport) { - UAVObject* object = objManager->getObject(objectName); - if (object) + foreach(const QString &objectName, objectsToExport) { + UAVObject *object = objManager->getObject(objectName); + + if (object) { engine()->rootContext()->setContextProperty(objectName, object); - else + } else { qWarning() << "Failed to load object" << objectName; + } } engine()->rootContext()->setContextProperty("qmlWidget", this); } QmlViewGadgetWidget::~QmlViewGadgetWidget() -{ -} +{} void QmlViewGadgetWidget::setQmlFile(QString fn) { @@ -82,7 +83,7 @@ void QmlViewGadgetWidget::setQmlFile(QString fn) SvgImageProvider *svgProvider = new SvgImageProvider(fn); engine()->addImageProvider("svg", svgProvider); - //it's necessary to allow qml side to query svg element position + // it's necessary to allow qml side to query svg element position engine()->rootContext()->setContextProperty("svgRenderer", svgProvider); engine()->setBaseUrl(QUrl::fromLocalFile(fn)); @@ -95,9 +96,10 @@ void QmlViewGadgetWidget::setQmlFile(QString fn) } /*! - \brief Enables/Disables OpenGL - */ -void QmlViewGadgetWidget::enableOpenGL(bool flag) { + \brief Enables/Disables OpenGL + */ +void QmlViewGadgetWidget::enableOpenGL(bool flag) +{ if (flag) { setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); } else { diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.h b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.h index 642349911..783cc963e 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -40,21 +40,23 @@ class UAVObject; -class QmlViewGadgetWidget : public QDeclarativeView -{ +class QmlViewGadgetWidget : public QDeclarativeView { Q_OBJECT public: QmlViewGadgetWidget(QWidget *parent = 0); - ~QmlViewGadgetWidget(); - void setQmlFile(QString fn); + ~QmlViewGadgetWidget(); + void setQmlFile(QString fn); - void enableOpenGL(bool flag); - void enableSmoothUpdates(bool flag) { beSmooth = flag; } + void enableOpenGL(bool flag); + void enableSmoothUpdates(bool flag) + { + beSmooth = flag; + } private: - // Flag to enable better rendering of fonts in OpenGL - bool beSmooth; - QString m_fn; + // Flag to enable better rendering of fonts in OpenGL + bool beSmooth; + QString m_fn; }; #endif /* QmlViewGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.cpp index e58a3559f..2cda37fe8 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,32 +35,31 @@ QmlViewPlugin::QmlViewPlugin() { - // Do nothing + // Do nothing } QmlViewPlugin::~QmlViewPlugin() { - // Do nothing + // Do nothing } -bool QmlViewPlugin::initialize(const QStringList& args, QString *errMsg) +bool QmlViewPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new QmlViewGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new QmlViewGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void QmlViewPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void QmlViewPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(QmlViewPlugin) - diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.h b/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.h index 33ad1d9e5..de21c5821 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.h +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class QmlViewGadgetFactory; -class QmlViewPlugin : public ExtensionSystem::IPlugin -{ +class QmlViewPlugin : public ExtensionSystem::IPlugin { public: - QmlViewPlugin(); - ~QmlViewPlugin(); + QmlViewPlugin(); + ~QmlViewPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - QmlViewGadgetFactory *mf; + QmlViewGadgetFactory *mf; }; #endif /* QMLVIEWPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h index f981f6a94..486f88b92 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h @@ -36,14 +36,14 @@ #include #include "rawhid_global.h" -#if defined( Q_OS_MAC) +#if defined(Q_OS_MAC) #include #include #include #elif defined(Q_OS_UNIX) -//#elif defined(Q_OS_LINUX) +// #elif defined(Q_OS_LINUX) #include #include #include @@ -56,16 +56,15 @@ // ************ -#if defined( Q_OS_MAC) +#if defined(Q_OS_MAC) // todo: #elif defined(Q_OS_UNIX) -//#elif defined(Q_OS_LINUX) +// #elif defined(Q_OS_LINUX) typedef struct hid_struct hid_t; -struct hid_struct -{ +struct hid_struct { usb_dev_handle *usb; int open; int iface; @@ -79,21 +78,17 @@ struct hid_struct typedef struct hid_struct hid_t; -struct hid_struct -{ +struct hid_struct { HANDLE handle; - int open; + int open; struct hid_struct *prev; struct hid_struct *next; }; -#endif +#endif // if defined(Q_OS_MAC) - - -class RAWHID_EXPORT pjrc_rawhid: public QObject -{ +class RAWHID_EXPORT pjrc_rawhid : public QObject { Q_OBJECT public: @@ -105,51 +100,51 @@ public: int send(int num, void *buf, int len, int timeout); QString getserial(int num); signals: - void deviceUnplugged(int); + void deviceUnplugged(int); private: -#if defined( Q_OS_MAC) +#if defined(Q_OS_MAC) - // Static callbacks called by the HID system with handles to the PJRC object - static void attach_callback(void *, IOReturn, void *, IOHIDDeviceRef); - static void dettach_callback(void *, IOReturn, void *hid_mgr, IOHIDDeviceRef dev); - static void input_callback(void *, IOReturn, void *, IOHIDReportType, uint32_t, uint8_t *, CFIndex); - static void timeout_callback(CFRunLoopTimerRef, void *); + // Static callbacks called by the HID system with handles to the PJRC object + static void attach_callback(void *, IOReturn, void *, IOHIDDeviceRef); + static void dettach_callback(void *, IOReturn, void *hid_mgr, IOHIDDeviceRef dev); + static void input_callback(void *, IOReturn, void *, IOHIDReportType, uint32_t, uint8_t *, CFIndex); + static void timeout_callback(CFRunLoopTimerRef, void *); - // Non static methods to call into - void attach(IOHIDDeviceRef dev); - void dettach(IOHIDDeviceRef dev); - void input(uint8_t *, CFIndex); + // Non static methods to call into + void attach(IOHIDDeviceRef dev); + void dettach(IOHIDDeviceRef dev); + void input(uint8_t *, CFIndex); - // Platform specific handles for the USB device - IOHIDManagerRef hid_manager; - IOHIDDeviceRef dev; - CFRunLoopRef the_correct_runloop; - CFRunLoopRef received_runloop; + // Platform specific handles for the USB device + IOHIDManagerRef hid_manager; + IOHIDDeviceRef dev; + CFRunLoopRef the_correct_runloop; + CFRunLoopRef received_runloop; - static const int BUFFER_SIZE = 64; - uint8_t buffer[BUFFER_SIZE]; - int attach_count; - int buffer_count; - bool device_open; - bool unplugged; + static const int BUFFER_SIZE = 64; + uint8_t buffer[BUFFER_SIZE]; + int attach_count; + int buffer_count; + bool device_open; + bool unplugged; - QMutex *m_writeMutex; - QMutex *m_readMutex; + QMutex *m_writeMutex; + QMutex *m_readMutex; #elif defined(Q_OS_UNIX) - hid_t *first_hid; + hid_t * first_hid; hid_t *last_hid; void add_hid(hid_t *h); - hid_t * get_hid(int num); + hid_t *get_hid(int num); void free_all_hid(void); void hid_close(hid_t *hid); - int hid_parse_item(uint32_t *val, uint8_t **data, const uint8_t *end); + int hid_parse_item(uint32_t *val, uint8_t * *data, const uint8_t *end); #elif defined(Q_OS_WIN32) - hid_t *first_hid; + hid_t * first_hid; hid_t *last_hid; HANDLE rx_event; HANDLE tx_event; @@ -157,12 +152,12 @@ private: CRITICAL_SECTION tx_mutex; void add_hid(hid_t *h); - hid_t * get_hid(int num); + hid_t *get_hid(int num); void free_all_hid(void); void hid_close(hid_t *hid); void print_win32_err(DWORD err); -#endif +#endif // if defined(Q_OS_MAC) }; -#endif +#endif // ifndef PJRC_RAWHID_H diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp index 80d5deda5..1ca909ccd 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp @@ -54,7 +54,7 @@ pjrc_rawhid::pjrc_rawhid() : device_open(false), hid_manager(NULL), buffer_count(0), unplugged(false) { m_writeMutex = new QMutex(); - m_readMutex = new QMutex(); + m_readMutex = new QMutex(); } pjrc_rawhid::~pjrc_rawhid() @@ -75,14 +75,14 @@ pjrc_rawhid::~pjrc_rawhid() } /** - * @brief open - open 1 or more devices - * @param[in] max maximum number of devices to open - * @param[in] vid Vendor ID, or -1 if any - * @param[in] pid Product ID, or -1 if any - * @param[in] usage_page top level usage page, or -1 if any - * @param[in] usage top level usage number, or -1 if any - * @returns actual number of devices opened - */ + * @brief open - open 1 or more devices + * @param[in] max maximum number of devices to open + * @param[in] vid Vendor ID, or -1 if any + * @param[in] pid Product ID, or -1 if any + * @param[in] usage_page top level usage page, or -1 if any + * @param[in] usage top level usage number, or -1 if any + * @returns actual number of devices opened + */ int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) { CFMutableDictionaryRef dict; @@ -95,9 +95,11 @@ int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) attach_count = 0; // Start the HID Manager - hid_manager = IOHIDManagerCreate(kCFAllocatorDefault, kIOHIDOptionsTypeNone); + hid_manager = IOHIDManagerCreate(kCFAllocatorDefault, kIOHIDOptionsTypeNone); if (hid_manager == NULL || CFGetTypeID(hid_manager) != IOHIDManagerGetTypeID()) { - if (hid_manager) CFRelease(hid_manager); + if (hid_manager) { + CFRelease(hid_manager); + } return 0; } @@ -105,7 +107,9 @@ int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) // Tell the HID Manager what type of devices we want dict = CFDictionaryCreateMutable(kCFAllocatorDefault, 0, &kCFTypeDictionaryKeyCallBacks, &kCFTypeDictionaryValueCallBacks); - if (!dict) return 0; + if (!dict) { + return 0; + } if (vid > 0) { num = CFNumberCreate(kCFAllocatorDefault, kCFNumberIntType, &vid); CFDictionarySetValue(dict, CFSTR(kIOHIDVendorIDKey), num); @@ -149,7 +153,9 @@ int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) } // let it do the callback for all devices - while (CFRunLoopRunInMode(kCFRunLoopDefaultMode, 0, true) == kCFRunLoopRunHandledSource) ; + while (CFRunLoopRunInMode(kCFRunLoopDefaultMode, 0, true) == kCFRunLoopRunHandledSource) { + ; + } // count up how many were added by the callback return attach_count; @@ -166,18 +172,20 @@ int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) int pjrc_rawhid::receive(int, void *buf, int len, int timeout) { QMutexLocker locker(m_readMutex); + Q_UNUSED(locker); - if (!device_open) + if (!device_open) { return -1; + } // Pass information to the callback to stop this run loop and signal if a timeout occurred struct timeout_info info; - info.loopRef = CFRunLoopGetCurrent();; + info.loopRef = CFRunLoopGetCurrent();; info.timed_out = false; CFRunLoopTimerContext context; memset(&context, 0, sizeof(context)); - context.info = &info; + context.info = &info; // Set up the timer for the timeout CFRunLoopTimerRef timer; @@ -187,9 +195,11 @@ int pjrc_rawhid::receive(int, void *buf, int len, int timeout) received_runloop = CFRunLoopGetCurrent(); // Run the CFRunLoop until either a timeout or data is available - while(1) { + while (1) { if (buffer_count != 0) { - if (len > buffer_count) len = buffer_count; + if (len > buffer_count) { + len = buffer_count; + } memcpy(buf, buffer, len); buffer_count = 0; break; @@ -212,14 +222,14 @@ int pjrc_rawhid::receive(int, void *buf, int len, int timeout) * @brief Helper class that will workaround the fact * that the HID send is broken on OSX */ -class Sender : public QThread -{ +class Sender : public QThread { public: - Sender(IOHIDDeviceRef d, uint8_t * b, int l) : - dev(d), buf(b), len(l), result(-1) { } + Sender(IOHIDDeviceRef d, uint8_t *b, int l) : + dev(d), buf(b), len(l), result(-1) {} - void run() { - ret = IOHIDDeviceSetReport(dev, kIOHIDReportTypeOutput, 2, buf, len); + void run() + { + ret = IOHIDDeviceSetReport(dev, kIOHIDReportTypeOutput, 2, buf, len); result = (ret == kIOReturnSuccess) ? len : -1; } @@ -227,7 +237,7 @@ public: IOReturn ret; private: IOHIDDeviceRef dev; - uint8_t * buf; + uint8_t *buf; int len; }; @@ -244,14 +254,15 @@ int pjrc_rawhid::send(int, void *buf, int len, int timeout) // This lock ensures that when closing we don't do it until the // write has terminated (and then the device_open flag is set to false) QMutexLocker locker(m_writeMutex); + Q_UNUSED(locker); - if(!device_open || unplugged) { + if (!device_open || unplugged) { return -1; } - uint8_t *report_buf = (uint8_t *) malloc(len); - memcpy(&report_buf[0], buf,len); + uint8_t *report_buf = (uint8_t *)malloc(len); + memcpy(&report_buf[0], buf, len); QEventLoop el; Sender sender(dev, report_buf, len); @@ -263,29 +274,31 @@ int pjrc_rawhid::send(int, void *buf, int len, int timeout) return sender.result; } -//! Get the serial number for a HID device -QString pjrc_rawhid::getserial(int num) { +// ! Get the serial number for a HID device +QString pjrc_rawhid::getserial(int num) +{ QMutexLocker locker(m_readMutex); + Q_UNUSED(locker); - if (!device_open || unplugged) + if (!device_open || unplugged) { return ""; + } CFTypeRef serialnum = IOHIDDeviceGetProperty(dev, CFSTR(kIOHIDSerialNumberKey)); - if(serialnum && CFGetTypeID(serialnum) == CFStringGetTypeID()) - { - //Note: I'm not sure it will always succeed if encoded as MacRoman but that - //is a superset of UTF8 so I think this is fine - CFStringRef str = static_cast(serialnum); + if (serialnum && CFGetTypeID(serialnum) == CFStringGetTypeID()) { + // Note: I'm not sure it will always succeed if encoded as MacRoman but that + // is a superset of UTF8 so I think this is fine + CFStringRef str = static_cast(serialnum); int length = CFStringGetLength(str); if (length == 0) { return ""; } - char* ptr = (char*)malloc(length+1); - Boolean ret = CFStringGetCString(str, ptr, length+1, kCFStringEncodingMacRoman); + char *ptr = (char *)malloc(length + 1); + Boolean ret = CFStringGetCString(str, ptr, length + 1, kCFStringEncodingMacRoman); QString strResult; if (ret == true) { - strResult = ptr; + strResult = ptr; } free(ptr); return strResult; @@ -294,7 +307,7 @@ QString pjrc_rawhid::getserial(int num) { return QString("Error"); } -//! Close the HID device +// ! Close the HID device void pjrc_rawhid::close(int) { // Make sure any pending locks are done @@ -326,47 +339,56 @@ void pjrc_rawhid::close(int) */ void pjrc_rawhid::input(uint8_t *data, CFIndex len) { - if (!device_open) + if (!device_open) { return; + } - if (len > BUFFER_SIZE) len = BUFFER_SIZE; + if (len > BUFFER_SIZE) { + len = BUFFER_SIZE; + } // Note: packet preprocessing done in OS independent code memcpy(buffer, &data[0], len); buffer_count = len; - if (received_runloop) + if (received_runloop) { CFRunLoopStop(received_runloop); + } } -//! Callback for the HID driver on an input report +// ! Callback for the HID driver on an input report void pjrc_rawhid::input_callback(void *c, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len) { - if (ret != kIOReturnSuccess || len < 1) return; + if (ret != kIOReturnSuccess || len < 1) { + return; + } - pjrc_rawhid *context = (pjrc_rawhid *) c; + pjrc_rawhid *context = (pjrc_rawhid *)c; context->input(data, len); } -//! Timeout used for the +// ! Timeout used for the void pjrc_rawhid::timeout_callback(CFRunLoopTimerRef, void *i) { - struct timeout_info *info = (struct timeout_info *) i; + struct timeout_info *info = (struct timeout_info *)i; + info->timed_out = true; CFRunLoopStop(info->loopRef); } -//! Called on a dettach event +// ! Called on a dettach event void pjrc_rawhid::dettach(IOHIDDeviceRef d) { unplugged = true; - if (d == dev) + if (d == dev) { emit deviceUnplugged(0); + } } -//! Called from the USB system and forwarded to the instance (context) +// ! Called from the USB system and forwarded to the instance (context) void pjrc_rawhid::dettach_callback(void *context, IOReturn, void *, IOHIDDeviceRef dev) { - pjrc_rawhid *p = (pjrc_rawhid*) context; + pjrc_rawhid *p = (pjrc_rawhid *)context; + p->dettach(dev); } @@ -379,7 +401,9 @@ void pjrc_rawhid::attach(IOHIDDeviceRef d) // Store the device handle dev = d; - if (IOHIDDeviceOpen(dev, kIOHIDOptionsTypeNone) != kIOReturnSuccess) return; + if (IOHIDDeviceOpen(dev, kIOHIDOptionsTypeNone) != kIOReturnSuccess) { + return; + } // Disconnect the attach callback since we don't want to automatically reconnect IOHIDManagerRegisterDeviceMatchingCallback(hid_manager, NULL, NULL); IOHIDDeviceScheduleWithRunLoop(dev, the_correct_runloop, kCFRunLoopDefaultMode); @@ -387,13 +411,13 @@ void pjrc_rawhid::attach(IOHIDDeviceRef d) attach_count++; device_open = true; - unplugged = false; + unplugged = false; } -//! Called from the USB system and forwarded to the instance (context) +// ! Called from the USB system and forwarded to the instance (context) void pjrc_rawhid::attach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) { - pjrc_rawhid *p = (pjrc_rawhid*) context; + pjrc_rawhid *p = (pjrc_rawhid *)context; + p->attach(dev); } - diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_unix.cpp b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_unix.cpp index bd44a9144..f8f123c3a 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_unix.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_unix.cpp @@ -43,204 +43,232 @@ pjrc_rawhid::pjrc_rawhid() { first_hid = NULL; - last_hid = NULL; + last_hid = NULL; } pjrc_rawhid::~pjrc_rawhid() -{ -} +{} -// open - open 1 or more devices +// open - open 1 or more devices // -// Inputs: -// max = maximum number of devices to open -// vid = Vendor ID, or -1 if any -// pid = Product ID, or -1 if any -// usage_page = top level usage page, or -1 if any -// usage = top level usage number, or -1 if any -// Output: -// actual number of devices opened +// Inputs: +// max = maximum number of devices to open +// vid = Vendor ID, or -1 if any +// pid = Product ID, or -1 if any +// usage_page = top level usage page, or -1 if any +// usage = top level usage number, or -1 if any +// Output: +// actual number of devices opened // int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) { - struct usb_bus *bus; - struct usb_device *dev; - struct usb_interface *iface; - struct usb_interface_descriptor *desc; - struct usb_endpoint_descriptor *ep; - usb_dev_handle *u; - uint8_t buf[1024], *p; - int i, n, len, tag, ep_in, ep_out, count=0, claimed; - uint32_t val=0, parsed_usage, parsed_usage_page; - hid_t *hid; + struct usb_bus *bus; + struct usb_device *dev; + struct usb_interface *iface; + struct usb_interface_descriptor *desc; + struct usb_endpoint_descriptor *ep; + usb_dev_handle *u; + uint8_t buf[1024], *p; + int i, n, len, tag, ep_in, ep_out, count = 0, claimed; + uint32_t val = 0, parsed_usage, parsed_usage_page; + hid_t *hid; - if (first_hid) free_all_hid(); - //printf("pjrc_rawhid_open, max=%d\n", max); + if (first_hid) { + free_all_hid(); + } + // printf("pjrc_rawhid_open, max=%d\n", max); - if (max < 1) return 0; + if (max < 1) { + return 0; + } - usb_init(); - usb_find_busses(); - usb_find_devices(); + usb_init(); + usb_find_busses(); + usb_find_devices(); - for (bus = usb_get_busses(); bus; bus = bus->next) - { - for (dev = bus->devices; dev; dev = dev->next) - { - if (vid > 0 && dev->descriptor.idVendor != vid) continue; - if (pid > 0 && dev->descriptor.idProduct != pid) continue; - if (!dev->config) continue; - if (dev->config->bNumInterfaces < 1) continue; - printf("device: vid=%04X, pic=%04X, with %d iface", + for (bus = usb_get_busses(); bus; bus = bus->next) { + for (dev = bus->devices; dev; dev = dev->next) { + if (vid > 0 && dev->descriptor.idVendor != vid) { + continue; + } + if (pid > 0 && dev->descriptor.idProduct != pid) { + continue; + } + if (!dev->config) { + continue; + } + if (dev->config->bNumInterfaces < 1) { + continue; + } + printf("device: vid=%04X, pic=%04X, with %d iface", dev->descriptor.idVendor, dev->descriptor.idProduct, dev->config->bNumInterfaces); - iface = dev->config->interface; - u = NULL; - claimed = 0; - for (i=0; iconfig->bNumInterfaces && iface; i++, iface++) - { - desc = iface->altsetting; - if (!desc) continue; + iface = dev->config->interface; + u = NULL; + claimed = 0; + for (i = 0; i < dev->config->bNumInterfaces && iface; i++, iface++) { + desc = iface->altsetting; + if (!desc) { + continue; + } - printf(" type %d, %d, %d", desc->bInterfaceClass, desc->bInterfaceSubClass, desc->bInterfaceProtocol); + printf(" type %d, %d, %d", desc->bInterfaceClass, desc->bInterfaceSubClass, desc->bInterfaceProtocol); - if (desc->bInterfaceClass != 3) continue; - if (desc->bInterfaceSubClass != 0) continue; - if (desc->bInterfaceProtocol != 0) continue; + if (desc->bInterfaceClass != 3) { + continue; + } + if (desc->bInterfaceSubClass != 0) { + continue; + } + if (desc->bInterfaceProtocol != 0) { + continue; + } - ep = desc->endpoint; - ep_in = ep_out = 0; - for (n = 0; n < desc->bNumEndpoints; n++, ep++) - { - if (ep->bEndpointAddress & 0x80) - { - if (!ep_in) ep_in = ep->bEndpointAddress & 0x7F; - qDebug() << " IN endpoint " << ep_in; - } - else - { - if (!ep_out) ep_out = ep->bEndpointAddress; - qDebug() << " OUT endpoint " << ep_out; - } - } - if (!ep_in) continue; + ep = desc->endpoint; + ep_in = ep_out = 0; + for (n = 0; n < desc->bNumEndpoints; n++, ep++) { + if (ep->bEndpointAddress & 0x80) { + if (!ep_in) { + ep_in = ep->bEndpointAddress & 0x7F; + } + qDebug() << " IN endpoint " << ep_in; + } else { + if (!ep_out) { + ep_out = ep->bEndpointAddress; + } + qDebug() << " OUT endpoint " << ep_out; + } + } + if (!ep_in) { + continue; + } - if (!u) - { - u = usb_open(dev); - if (!u) - { - qDebug() << " unable to open device"; - break; - } - } - qDebug() << " hid interface (generic)"; - if (usb_get_driver_np(u, i, (char *)buf, sizeof(buf)) >= 0) - { - printf(" in use by driver \"%s\"", buf); - if (usb_detach_kernel_driver_np(u, i) < 0) - { - printf(" unable to detach from kernel"); - continue; - } - } + if (!u) { + u = usb_open(dev); + if (!u) { + qDebug() << " unable to open device"; + break; + } + } + qDebug() << " hid interface (generic)"; + if (usb_get_driver_np(u, i, (char *)buf, sizeof(buf)) >= 0) { + printf(" in use by driver \"%s\"", buf); + if (usb_detach_kernel_driver_np(u, i) < 0) { + printf(" unable to detach from kernel"); + continue; + } + } - if (usb_claim_interface(u, i) < 0) - { - printf(" unable claim interface %d", i); - continue; - } + if (usb_claim_interface(u, i) < 0) { + printf(" unable claim interface %d", i); + continue; + } - len = usb_control_msg(u, 0x81, 6, 0x2200, i, (char *)buf, sizeof(buf), 250); - printf(" descriptor, len=%d", len); - if (len < 2) - { - usb_release_interface(u, i); - continue; - } + len = usb_control_msg(u, 0x81, 6, 0x2200, i, (char *)buf, sizeof(buf), 250); + printf(" descriptor, len=%d", len); + if (len < 2) { + usb_release_interface(u, i); + continue; + } - p = buf; - parsed_usage_page = parsed_usage = 0; - while ((tag = hid_parse_item(&val, &p, buf + len)) >= 0) - { - printf(" tag: %X, val %X", tag, val); - if (tag == 4) parsed_usage_page = val; - if (tag == 8) parsed_usage = val; - if (parsed_usage_page && parsed_usage) break; - } - if ((!parsed_usage_page) || (!parsed_usage) || + p = buf; + parsed_usage_page = parsed_usage = 0; + while ((tag = hid_parse_item(&val, &p, buf + len)) >= 0) { + printf(" tag: %X, val %X", tag, val); + if (tag == 4) { + parsed_usage_page = val; + } + if (tag == 8) { + parsed_usage = val; + } + if (parsed_usage_page && parsed_usage) { + break; + } + } + if ((!parsed_usage_page) || (!parsed_usage) || (usage_page > 0 && parsed_usage_page != (uint32_t)usage_page) || - (usage > 0 && parsed_usage != (uint32_t)usage)) - { - usb_release_interface(u, i); - continue; - } + (usage > 0 && parsed_usage != (uint32_t)usage)) { + usb_release_interface(u, i); + continue; + } - hid = (struct hid_struct *)malloc(sizeof(struct hid_struct)); - if (!hid) - { - usb_release_interface(u, i); - continue; - } + hid = (struct hid_struct *)malloc(sizeof(struct hid_struct)); + if (!hid) { + usb_release_interface(u, i); + continue; + } - hid->usb = u; - hid->iface = i; - hid->ep_in = ep_in; - hid->ep_out = ep_out; - hid->open = 1; - add_hid(hid); + hid->usb = u; + hid->iface = i; + hid->ep_in = ep_in; + hid->ep_out = ep_out; + hid->open = 1; + add_hid(hid); - claimed++; - count++; - if (count >= max) return count; - } + claimed++; + count++; + if (count >= max) { + return count; + } + } - if (u && !claimed) usb_close(u); - } - } + if (u && !claimed) { + usb_close(u); + } + } + } - return count; + return count; } -// recveive - receive a packet -// Inputs: -// num = device to receive from (zero based) -// buf = buffer to receive packet -// len = buffer's size -// timeout = time to wait, in milliseconds -// Output: -// number of bytes received, or -1 on error +// recveive - receive a packet +// Inputs: +// num = device to receive from (zero based) +// buf = buffer to receive packet +// len = buffer's size +// timeout = time to wait, in milliseconds +// Output: +// number of bytes received, or -1 on error // int pjrc_rawhid::receive(int num, void *buf, int len, int timeout) { - if (!buf) return -1; + if (!buf) { + return -1; + } - hid_t *hid = get_hid(num); - if (!hid || !hid->open) return -1; - - int r = usb_interrupt_read(hid->usb, hid->ep_in, (char *)buf, len, timeout); - if (r >= 0) return r; - if (r == -110) return 0; // timeout + hid_t *hid = get_hid(num); + if (!hid || !hid->open) { + return -1; + } + int r = usb_interrupt_read(hid->usb, hid->ep_in, (char *)buf, len, timeout); + if (r >= 0) { + return r; + } + if (r == -110) { + return 0; // timeout + } return -1; } -// send - send a packet -// Inputs: -// num = device to transmit to (zero based) -// buf = buffer containing packet to send -// len = number of bytes to transmit -// timeout = time to wait, in milliseconds -// Output: -// number of bytes sent, or -1 on error +// send - send a packet +// Inputs: +// num = device to transmit to (zero based) +// buf = buffer containing packet to send +// len = number of bytes to transmit +// timeout = time to wait, in milliseconds +// Output: +// number of bytes sent, or -1 on error // int pjrc_rawhid::send(int num, void *buf, int len, int timeout) { hid_t *hid; hid = get_hid(num); - if (!hid || !hid->open) return -1; + if (!hid || !hid->open) { + return -1; + } if (hid->ep_out) { return usb_interrupt_write(hid->usb, hid->ep_out, (char *)buf, len, timeout); } else { @@ -248,34 +276,38 @@ int pjrc_rawhid::send(int num, void *buf, int len, int timeout) } } -// getserial - get the serialnumber of the device +// getserial - get the serialnumber of the device // -// Inputs: -// num = device to close (zero based) -// buf = buffer to read the serialnumber into -// Output -// number of bytes in found, or -1 on error +// Inputs: +// num = device to close (zero based) +// buf = buffer to read the serialnumber into +// Output +// number of bytes in found, or -1 on error // -QString pjrc_rawhid::getserial(int num) { +QString pjrc_rawhid::getserial(int num) +{ hid_t *hid; char buf[128]; + hid = get_hid(num); - if (!hid || !hid->open) return QString(""); + if (!hid || !hid->open) { + return QString(""); + } int retlen = usb_get_string_simple(hid->usb, 3, buf, 128); - return QString().fromAscii(buf,-1); + return QString().fromAscii(buf, -1); } -// close - close a device +// close - close a device // -// Inputs: -// num = device to close (zero based) -// Output -// (nothing) +// Inputs: +// num = device to close (zero based) +// Output +// (nothing) // void pjrc_rawhid::close(int num) { - hid_close(get_hid(num)); + hid_close(get_hid(num)); } // Chuck Robey wrote a real HID report parser @@ -284,25 +316,31 @@ void pjrc_rawhid::close(int num) // this tiny thing only needs to extract the top-level usage page // and usage, and even then is may not be truly correct, but it does // work with the Teensy Raw HID example. -int pjrc_rawhid::hid_parse_item(uint32_t *val, uint8_t **data, const uint8_t *end) +int pjrc_rawhid::hid_parse_item(uint32_t *val, uint8_t * *data, const uint8_t *end) { const uint8_t *p = *data; uint8_t tag; - int table[4] = {0, 1, 2, 4}; + int table[4] = { 0, 1, 2, 4 }; int len; - if (p >= end) return -1; + if (p >= end) { + return -1; + } if (p[0] == 0xFE) { // long item, HID 1.11, 6.2.2.3, page 27 - if (p + 5 >= end || p + p[1] >= end) return -1; - tag = p[2]; + if (p + 5 >= end || p + p[1] >= end) { + return -1; + } + tag = p[2]; *val = 0; - len = p[1] + 5; + len = p[1] + 5; } else { // short item, HID 1.11, 6.2.2.2, page 26 tag = p[0] & 0xFC; len = table[p[0] & 0x03]; - if (p + len + 1 >= end) return -1; + if (p + len + 1 >= end) { + return -1; + } switch (p[0] & 0x03) { case 3: *val = p[1] | (p[2] << 8) | (p[3] << 16) | (p[4] << 24); break; case 2: *val = p[1] | (p[2] << 8); break; @@ -316,36 +354,40 @@ int pjrc_rawhid::hid_parse_item(uint32_t *val, uint8_t **data, const uint8_t *en void pjrc_rawhid::add_hid(hid_t *h) { - if (!h) return; + if (!h) { + return; + } - if (!first_hid || !last_hid) - { + if (!first_hid || !last_hid) { first_hid = last_hid = h; - h->next = h->prev = NULL; + h->next = h->prev = NULL; return; } last_hid->next = h; - h->prev = last_hid; - h->next = NULL; + h->prev = last_hid; + h->next = NULL; last_hid = h; } -hid_t * pjrc_rawhid::get_hid(int num) +hid_t *pjrc_rawhid::get_hid(int num) { - hid_t *p = NULL; - for (p = first_hid; p && num > 0; p = p->next, num--) ; + hid_t *p = NULL; + + for (p = first_hid; p && num > 0; p = p->next, num--) { + ; + } return p; } void pjrc_rawhid::free_all_hid(void) { - for (hid_t *p = first_hid; p; p = p->next) + for (hid_t *p = first_hid; p; p = p->next) { hid_close(p); + } - hid_t *p = first_hid; - while (p) - { - hid_t *q = p; + hid_t *p = first_hid; + while (p) { + hid_t *q = p; p = p->next; free(q); } @@ -355,20 +397,25 @@ void pjrc_rawhid::free_all_hid(void) void pjrc_rawhid::hid_close(hid_t *hid) { - if (!hid) return; - if (!hid->open) return; - - usb_release_interface(hid->usb, hid->iface); - - int others = 0; - for (hid_t *p = first_hid; p; p = p->next) - { - if (p->open && p->usb == hid->usb) - others++; + if (!hid) { + return; + } + if (!hid->open) { + return; } - if (!others) - usb_close(hid->usb); - hid->usb = NULL; + usb_release_interface(hid->usb, hid->iface); + + int others = 0; + for (hid_t *p = first_hid; p; p = p->next) { + if (p->open && p->usb == hid->usb) { + others++; + } + } + if (!others) { + usb_close(hid->usb); + } + + hid->usb = NULL; hid->open = 0; } diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_win.cpp b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_win.cpp index 4a5421de6..37d4a9075 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_win.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_win.cpp @@ -43,436 +43,441 @@ pjrc_rawhid::pjrc_rawhid() { - first_hid = NULL; - last_hid = NULL; - rx_event = NULL; - tx_event = NULL; + last_hid = NULL; + rx_event = NULL; + tx_event = NULL; } pjrc_rawhid::~pjrc_rawhid() -{ +{} -} - -// open - open 1 or more devices +// open - open 1 or more devices // -// Inputs: -// max = maximum number of devices to open -// vid = Vendor ID, or -1 if any -// pid = Product ID, or -1 if any -// usage_page = top level usage page, or -1 if any -// usage = top level usage number, or -1 if any -// Output: -// actual number of devices opened +// Inputs: +// max = maximum number of devices to open +// vid = Vendor ID, or -1 if any +// pid = Product ID, or -1 if any +// usage_page = top level usage page, or -1 if any +// usage = top level usage number, or -1 if any +// Output: +// actual number of devices opened // int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) { - GUID guid; - HDEVINFO info; - DWORD index=0, reqd_size; - SP_DEVICE_INTERFACE_DATA iface; - SP_DEVICE_INTERFACE_DETAIL_DATA *details; - HIDD_ATTRIBUTES attrib; - PHIDP_PREPARSED_DATA hid_data; - HIDP_CAPS capabilities; - HANDLE h; - BOOL ret; - hid_t *hid; + GUID guid; + HDEVINFO info; + DWORD index = 0, reqd_size; + SP_DEVICE_INTERFACE_DATA iface; + SP_DEVICE_INTERFACE_DETAIL_DATA *details; + HIDD_ATTRIBUTES attrib; + PHIDP_PREPARSED_DATA hid_data; + HIDP_CAPS capabilities; + HANDLE h; + BOOL ret; + hid_t *hid; - int count=0; + int count = 0; - if (first_hid) free_all_hid(); - - if (max < 1) return 0; - - if (!rx_event) - { - rx_event = CreateEvent(NULL, TRUE, TRUE, NULL); - tx_event = CreateEvent(NULL, TRUE, TRUE, NULL); - InitializeCriticalSection(&rx_mutex); - InitializeCriticalSection(&tx_mutex); + if (first_hid) { + free_all_hid(); } - HidD_GetHidGuid(&guid); + if (max < 1) { + return 0; + } - info = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE); - if (info == INVALID_HANDLE_VALUE) return 0; + if (!rx_event) { + rx_event = CreateEvent(NULL, TRUE, TRUE, NULL); + tx_event = CreateEvent(NULL, TRUE, TRUE, NULL); + InitializeCriticalSection(&rx_mutex); + InitializeCriticalSection(&tx_mutex); + } - for (index=0; 1 ;index++) - { - iface.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); - ret = SetupDiEnumDeviceInterfaces(info, NULL, &guid, index, &iface); - if (!ret) return count; + HidD_GetHidGuid(&guid); - SetupDiGetInterfaceDeviceDetail(info, &iface, NULL, 0, &reqd_size, NULL); - details = (SP_DEVICE_INTERFACE_DETAIL_DATA *)malloc(reqd_size); - if (details == NULL) continue; + info = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE); + if (info == INVALID_HANDLE_VALUE) { + return 0; + } - memset(details, 0, reqd_size); - details->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA); - ret = SetupDiGetDeviceInterfaceDetail(info, &iface, details, reqd_size, NULL, NULL); - if (!ret) - { - free(details); - continue; - } - - h = CreateFile(details->DevicePath, GENERIC_READ|GENERIC_WRITE, FILE_SHARE_READ|FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL); - if (h == INVALID_HANDLE_VALUE) - { - DWORD err = GetLastError(); - - // I get ERROR_ACCESS_DENIED with most/all my input devices (mice/trackballs/tablet). - // Let's not log it :) - if (err == ERROR_ACCESS_DENIED) - { - free(details); - continue; - } - - // qDebug wipes the GetLastError() it seems, so do that after print_win32_err(). - print_win32_err(err); - qDebug() << "Problem opening handle, path: " << QString().fromWCharArray(details->DevicePath); - - free(details); - continue; - } - - free(details); - - attrib.Size = sizeof(HIDD_ATTRIBUTES); - ret = HidD_GetAttributes(h, &attrib); - //printf("vid: %4x\n", attrib.VendorID); - if (!ret || (vid > 0 && attrib.VendorID != vid) || - (pid > 0 && attrib.ProductID != pid) || - !HidD_GetPreparsedData(h, &hid_data)) - { - CloseHandle(h); - continue; - } - - if (!HidP_GetCaps(hid_data, &capabilities) || - (usage_page > 0 && capabilities.UsagePage != usage_page) || - (usage > 0 && capabilities.Usage != usage)) - { - HidD_FreePreparsedData(hid_data); - CloseHandle(h); - continue; - } - - HidD_FreePreparsedData(hid_data); - - hid = (struct hid_struct *)malloc(sizeof(struct hid_struct)); - if (!hid) - { - CloseHandle(h); - continue; - } - -// COMMTIMEOUTS CommTimeouts; -// CommTimeouts.ReadIntervalTimeout = 100; // 100ms -// CommTimeouts.ReadTotalTimeoutConstant = 5; // ms -// CommTimeouts.ReadTotalTimeoutMultiplier = 1; // -// CommTimeouts.WriteTotalTimeoutConstant = 5; // ms -// CommTimeouts.WriteTotalTimeoutMultiplier = 1; // -// if (!SetCommTimeouts(h, &CommTimeouts)) -// { -//// DWORD err = GetLastError(); -// -// } - - qDebug("Open: Handle address: %li for num: %i", (long int) h, count); - - hid->handle = h; - add_hid(hid); - - count++; - if (count >= max) return count; + for (index = 0; 1; index++) { + iface.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); + ret = SetupDiEnumDeviceInterfaces(info, NULL, &guid, index, &iface); + if (!ret) { + return count; } - return count; + SetupDiGetInterfaceDeviceDetail(info, &iface, NULL, 0, &reqd_size, NULL); + details = (SP_DEVICE_INTERFACE_DETAIL_DATA *)malloc(reqd_size); + if (details == NULL) { + continue; + } + + memset(details, 0, reqd_size); + details->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA); + ret = SetupDiGetDeviceInterfaceDetail(info, &iface, details, reqd_size, NULL, NULL); + if (!ret) { + free(details); + continue; + } + + h = CreateFile(details->DevicePath, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL); + if (h == INVALID_HANDLE_VALUE) { + DWORD err = GetLastError(); + + // I get ERROR_ACCESS_DENIED with most/all my input devices (mice/trackballs/tablet). + // Let's not log it :) + if (err == ERROR_ACCESS_DENIED) { + free(details); + continue; + } + + // qDebug wipes the GetLastError() it seems, so do that after print_win32_err(). + print_win32_err(err); + qDebug() << "Problem opening handle, path: " << QString().fromWCharArray(details->DevicePath); + + free(details); + continue; + } + + free(details); + + attrib.Size = sizeof(HIDD_ATTRIBUTES); + ret = HidD_GetAttributes(h, &attrib); + // printf("vid: %4x\n", attrib.VendorID); + if (!ret || (vid > 0 && attrib.VendorID != vid) || + (pid > 0 && attrib.ProductID != pid) || + !HidD_GetPreparsedData(h, &hid_data)) { + CloseHandle(h); + continue; + } + + if (!HidP_GetCaps(hid_data, &capabilities) || + (usage_page > 0 && capabilities.UsagePage != usage_page) || + (usage > 0 && capabilities.Usage != usage)) { + HidD_FreePreparsedData(hid_data); + CloseHandle(h); + continue; + } + + HidD_FreePreparsedData(hid_data); + + hid = (struct hid_struct *)malloc(sizeof(struct hid_struct)); + if (!hid) { + CloseHandle(h); + continue; + } + +// COMMTIMEOUTS CommTimeouts; +// CommTimeouts.ReadIntervalTimeout = 100; // 100ms +// CommTimeouts.ReadTotalTimeoutConstant = 5; // ms +// CommTimeouts.ReadTotalTimeoutMultiplier = 1; // +// CommTimeouts.WriteTotalTimeoutConstant = 5; // ms +// CommTimeouts.WriteTotalTimeoutMultiplier = 1; // +// if (!SetCommTimeouts(h, &CommTimeouts)) +// { +//// DWORD err = GetLastError(); +// +// } + + qDebug("Open: Handle address: %li for num: %i", (long int)h, count); + + hid->handle = h; + add_hid(hid); + + count++; + if (count >= max) { + return count; + } + } + + return count; } -// recveive - receive a packet -// Inputs: -// num = device to receive from (zero based) -// buf = buffer to receive packet -// len = buffer's size -// timeout = time to wait, in milliseconds -// Output: -// number of bytes received, or -1 on error +// recveive - receive a packet +// Inputs: +// num = device to receive from (zero based) +// buf = buffer to receive packet +// len = buffer's size +// timeout = time to wait, in milliseconds +// Output: +// number of bytes received, or -1 on error // int pjrc_rawhid::receive(int num, void *buf, int len, int timeout) { - OVERLAPPED ov; - DWORD n; + OVERLAPPED ov; + DWORD n; - hid_t *hid = get_hid(num); - if (!hid) - return -1; - if (!hid->handle) - return -1; + hid_t *hid = get_hid(num); - EnterCriticalSection(&rx_mutex); + if (!hid) { + return -1; + } + if (!hid->handle) { + return -1; + } - ResetEvent(&rx_event); + EnterCriticalSection(&rx_mutex); - memset(&ov, 0, sizeof(ov)); - ov.hEvent = rx_event; + ResetEvent(&rx_event); - if (!ReadFile(hid->handle, buf, len, NULL, &ov)) - { - DWORD err = GetLastError(); + memset(&ov, 0, sizeof(ov)); + ov.hEvent = rx_event; - if (err == ERROR_DEVICE_NOT_CONNECTED) - { // the device has been unplugged - print_win32_err(err); - hid_close(hid); - LeaveCriticalSection(&rx_mutex); - emit deviceUnplugged(num); - return -1; - } + if (!ReadFile(hid->handle, buf, len, NULL, &ov)) { + DWORD err = GetLastError(); - if (err != ERROR_IO_PENDING) - { - print_win32_err(err); - LeaveCriticalSection(&rx_mutex); - return -1; - } - - DWORD r = WaitForSingleObject(rx_event, timeout); - if (r == WAIT_TIMEOUT) - { - CancelIo(hid->handle); - LeaveCriticalSection(&rx_mutex); - return 0; - } - if (r != WAIT_OBJECT_0) - { - DWORD err = GetLastError(); - print_win32_err(err); - LeaveCriticalSection(&rx_mutex); - return -1; - } + if (err == ERROR_DEVICE_NOT_CONNECTED) { // the device has been unplugged + print_win32_err(err); + hid_close(hid); + LeaveCriticalSection(&rx_mutex); + emit deviceUnplugged(num); + return -1; } - if (!GetOverlappedResult(hid->handle, &ov, &n, FALSE)) - { - DWORD err = GetLastError(); - print_win32_err(err); + if (err != ERROR_IO_PENDING) { + print_win32_err(err); + LeaveCriticalSection(&rx_mutex); + return -1; + } - if (err == ERROR_DEVICE_NOT_CONNECTED) - { // the device has been unplugged - hid_close(hid); - LeaveCriticalSection(&rx_mutex); - emit deviceUnplugged(num); - return -1; - } + DWORD r = WaitForSingleObject(rx_event, timeout); + if (r == WAIT_TIMEOUT) { + CancelIo(hid->handle); + LeaveCriticalSection(&rx_mutex); + return 0; + } + if (r != WAIT_OBJECT_0) { + DWORD err = GetLastError(); + print_win32_err(err); + LeaveCriticalSection(&rx_mutex); + return -1; + } + } - LeaveCriticalSection(&rx_mutex); - return -1; + if (!GetOverlappedResult(hid->handle, &ov, &n, FALSE)) { + DWORD err = GetLastError(); + print_win32_err(err); + + if (err == ERROR_DEVICE_NOT_CONNECTED) { // the device has been unplugged + hid_close(hid); + LeaveCriticalSection(&rx_mutex); + emit deviceUnplugged(num); + return -1; } LeaveCriticalSection(&rx_mutex); + return -1; + } - if (n <= 0) return -1; + LeaveCriticalSection(&rx_mutex); -// qDebug("Received %i bytes, first %x, second %x", len, *((char *) buf),*((char *)buf + 1)); + if (n <= 0) { + return -1; + } - if ((int)n > len) n = len; - return n; +// qDebug("Received %i bytes, first %x, second %x", len, *((char *) buf),*((char *)buf + 1)); + + if ((int)n > len) { + n = len; + } + return n; } -// send - send a packet -// Inputs: -// num = device to transmit to (zero based) -// buf = buffer containing packet to send -// len = number of bytes to transmit -// timeout = time to wait, in milliseconds -// Output: -// number of bytes sent, or -1 on error +// send - send a packet +// Inputs: +// num = device to transmit to (zero based) +// buf = buffer containing packet to send +// len = number of bytes to transmit +// timeout = time to wait, in milliseconds +// Output: +// number of bytes sent, or -1 on error // int pjrc_rawhid::send(int num, void *buf, int len, int timeout) { - OVERLAPPED ov; - DWORD n, r; + OVERLAPPED ov; + DWORD n, r; - hid_t *hid = get_hid(num); - if (!hid) - return -1; - if (!hid->handle) - return -1; + hid_t *hid = get_hid(num); -// qDebug("Send: Handle address: %li for num: %i", (long int) hid->handle, num); + if (!hid) { + return -1; + } + if (!hid->handle) { + return -1; + } - EnterCriticalSection(&tx_mutex); +// qDebug("Send: Handle address: %li for num: %i", (long int) hid->handle, num); - ResetEvent(&tx_event); + EnterCriticalSection(&tx_mutex); - memset(&ov, 0, sizeof(ov)); - ov.hEvent = tx_event; + ResetEvent(&tx_event); -// qDebug("Trying to write %u bytes. First %x second %x",len, *((char *) buf), *((char *)buf + 1)); + memset(&ov, 0, sizeof(ov)); + ov.hEvent = tx_event; - if (!WriteFile(hid->handle, buf, len, NULL, &ov)) - { - DWORD err = GetLastError(); +// qDebug("Trying to write %u bytes. First %x second %x",len, *((char *) buf), *((char *)buf + 1)); - if (err == ERROR_DEVICE_NOT_CONNECTED) - { // the device has been unplugged - hid_close(hid); - LeaveCriticalSection(&tx_mutex); - emit deviceUnplugged(num); - return -1; - } + if (!WriteFile(hid->handle, buf, len, NULL, &ov)) { + DWORD err = GetLastError(); - if (err == ERROR_SUCCESS || err == ERROR_IO_PENDING) - { -// qDebug("Waiting for write to finish"); - r = WaitForSingleObject(tx_event, timeout); - if (r == WAIT_TIMEOUT) - { - CancelIo(hid->handle); - LeaveCriticalSection(&tx_mutex); - return 0; - } - if (r != WAIT_OBJECT_0) - { - DWORD err = GetLastError(); - print_win32_err(err); - LeaveCriticalSection(&tx_mutex); - return -1; - } - } - else - { -// qDebug("Error writing to file"); - print_win32_err(err); - LeaveCriticalSection(&tx_mutex); - return -1; - } + if (err == ERROR_DEVICE_NOT_CONNECTED) { // the device has been unplugged + hid_close(hid); + LeaveCriticalSection(&tx_mutex); + emit deviceUnplugged(num); + return -1; } - if (!GetOverlappedResult(hid->handle, &ov, &n, FALSE)) - { + if (err == ERROR_SUCCESS || err == ERROR_IO_PENDING) { +// qDebug("Waiting for write to finish"); + r = WaitForSingleObject(tx_event, timeout); + if (r == WAIT_TIMEOUT) { + CancelIo(hid->handle); + LeaveCriticalSection(&tx_mutex); + return 0; + } + if (r != WAIT_OBJECT_0) { DWORD err = GetLastError(); - - qDebug("Problem getting overlapped result"); print_win32_err(err); - - if (err == ERROR_DEVICE_NOT_CONNECTED) - { // the device has been unplugged - hid_close(hid); - LeaveCriticalSection(&tx_mutex); - emit deviceUnplugged(num); - return -1; - } + LeaveCriticalSection(&tx_mutex); + return -1; + } + } else { +// qDebug("Error writing to file"); + print_win32_err(err); + LeaveCriticalSection(&tx_mutex); + return -1; } + } - LeaveCriticalSection(&tx_mutex); + if (!GetOverlappedResult(hid->handle, &ov, &n, FALSE)) { + DWORD err = GetLastError(); - if (n <= 0) return -1; - return n; + qDebug("Problem getting overlapped result"); + print_win32_err(err); + + if (err == ERROR_DEVICE_NOT_CONNECTED) { // the device has been unplugged + hid_close(hid); + LeaveCriticalSection(&tx_mutex); + emit deviceUnplugged(num); + return -1; + } + } + + LeaveCriticalSection(&tx_mutex); + + if (n <= 0) { + return -1; + } + return n; } QString pjrc_rawhid::getserial(int num) { - hid_t *hid = get_hid(num); - if (!hid) - return ""; - if (!hid->handle) - return ""; + hid_t *hid = get_hid(num); - // Should we do some "critical section" stuff here?? - char temp[126]; - if (!HidD_GetSerialNumberString(hid->handle, temp, sizeof(temp))) - { - DWORD err = GetLastError(); - print_win32_err(err); + if (!hid) { + return ""; + } + if (!hid->handle) { + return ""; + } - if (err == ERROR_DEVICE_NOT_CONNECTED) - { // the device has been unplugged - hid_close(hid); - emit deviceUnplugged(num); - return ""; - } + // Should we do some "critical section" stuff here?? + char temp[126]; + if (!HidD_GetSerialNumberString(hid->handle, temp, sizeof(temp))) { + DWORD err = GetLastError(); + print_win32_err(err); - return QString("Error"); + if (err == ERROR_DEVICE_NOT_CONNECTED) { // the device has been unplugged + hid_close(hid); + emit deviceUnplugged(num); + return ""; } - return QString().fromUtf16((ushort*)temp,-1); + return QString("Error"); + } + + return QString().fromUtf16((ushort *)temp, -1); } -// close - close a device +// close - close a device // -// Inputs: -// num = device to close (zero based) -// Output -// (nothing) +// Inputs: +// num = device to close (zero based) +// Output +// (nothing) // void pjrc_rawhid::close(int num) { - hid_close(get_hid(num)); + hid_close(get_hid(num)); } void pjrc_rawhid::add_hid(hid_t *h) { - if (!h) return; + if (!h) { + return; + } - if (!first_hid || !last_hid) - { - first_hid = last_hid = h; - h->next = h->prev = NULL; - return; - } + if (!first_hid || !last_hid) { + first_hid = last_hid = h; + h->next = h->prev = NULL; + return; + } - last_hid->next = h; - h->prev = last_hid; - h->next = NULL; - last_hid = h; + last_hid->next = h; + h->prev = last_hid; + h->next = NULL; + last_hid = h; } -hid_t * pjrc_rawhid::get_hid(int num) +hid_t *pjrc_rawhid::get_hid(int num) { - hid_t *p; - for (p = first_hid; p && num > 0; p = p->next, num--) ; - return p; + hid_t *p; + + for (p = first_hid; p && num > 0; p = p->next, num--) { + ; + } + return p; } void pjrc_rawhid::free_all_hid(void) { - for (hid_t *p = first_hid; p; p = p->next) - hid_close(p); + for (hid_t *p = first_hid; p; p = p->next) { + hid_close(p); + } - hid_t *p = first_hid; - while (p) - { - hid_t *q = p; - p = p->next; - free(q); - } + hid_t *p = first_hid; + while (p) { + hid_t *q = p; + p = p->next; + free(q); + } - first_hid = last_hid = NULL; + first_hid = last_hid = NULL; } void pjrc_rawhid::hid_close(hid_t *hid) { - if (!hid) return; - if (!hid->handle) return; + if (!hid) { + return; + } + if (!hid->handle) { + return; + } - CloseHandle(hid->handle); - hid->handle = NULL; + CloseHandle(hid->handle); + hid->handle = NULL; } void pjrc_rawhid::print_win32_err(DWORD err) { - char buf[256]; - char temp[256]; + char buf[256]; + char temp[256]; - //FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, err, 0, (WCHAR*)buf, sizeof(buf), NULL); - FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, err, MAKELANGID(LANG_ENGLISH, SUBLANG_DEFAULT), (WCHAR*)buf, sizeof(buf), NULL); - WideCharToMultiByte( CP_ACP, 0, (WCHAR*)buf, sizeof(buf), temp, sizeof(temp), NULL, NULL ); - printf("err %ld: %s\n", err, temp); + // FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, err, 0, (WCHAR*)buf, sizeof(buf), NULL); + FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, err, MAKELANGID(LANG_ENGLISH, SUBLANG_DEFAULT), (WCHAR *)buf, sizeof(buf), NULL); + WideCharToMultiByte(CP_ACP, 0, (WCHAR *)buf, sizeof(buf), temp, sizeof(temp), NULL, NULL); + printf("err %ld: %s\n", err, temp); } - diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp b/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp index 3a1aa896d..3e3d8a4ad 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp @@ -37,22 +37,20 @@ class IConnection; -//timeout value used when we want to return directly without waiting -static const int READ_TIMEOUT = 200; -static const int READ_SIZE = 64; +// timeout value used when we want to return directly without waiting +static const int READ_TIMEOUT = 200; +static const int READ_SIZE = 64; static const int WRITE_TIMEOUT = 1000; -static const int WRITE_SIZE = 64; - +static const int WRITE_SIZE = 64; // ********************************************************************************* /** -* Thread to desynchronize reading from the device -*/ -class RawHIDReadThread : public QThread -{ + * Thread to desynchronize reading from the device + */ +class RawHIDReadThread : public QThread { public: RawHIDReadThread(RawHID *hid); virtual ~RawHIDReadThread(); @@ -64,7 +62,8 @@ public: qint64 getBytesAvailable(); public slots: - void terminate() { + void terminate() + { m_running = false; } @@ -72,7 +71,7 @@ protected: void run(); /** QByteArray might not be the most efficient way to implement - a circular buffer but it's good enough and very simple */ + a circular buffer but it's good enough and very simple */ QByteArray m_readBuffer; /** A mutex to protect read buffer */ @@ -90,10 +89,9 @@ protected: // ********************************************************************************* /** -* This class is nearly the same than RawHIDReadThread but for writing -*/ -class RawHIDWriteThread : public QThread -{ + * This class is nearly the same than RawHIDReadThread but for writing + */ +class RawHIDWriteThread : public QThread { public: RawHIDWriteThread(RawHID *hid); virtual ~RawHIDWriteThread(); @@ -105,7 +103,8 @@ public: qint64 getBytesToWrite(); public slots: - void terminate() { + void terminate() + { m_running = false; } @@ -113,7 +112,7 @@ protected: void run(); /** QByteArray might not be the most efficient way to implement - a circular buffer but it's good enough and very simple */ + a circular buffer but it's good enough and very simple */ QByteArray m_writeBuffer; /** A mutex to protect read buffer */ @@ -144,43 +143,38 @@ RawHIDReadThread::RawHIDReadThread(RawHID *hid) RawHIDReadThread::~RawHIDReadThread() { m_running = false; - //wait for the thread to terminate - if(wait(10000) == false) + // wait for the thread to terminate + if (wait(10000) == false) { qDebug() << "Cannot terminate RawHIDReadThread"; + } } void RawHIDReadThread::run() { m_running = m_hid->openDevice(); - while(m_running) - { - //here we use a temporary buffer so we don't need to lock - //the mutex while we are reading from the device + while (m_running) { + // here we use a temporary buffer so we don't need to lock + // the mutex while we are reading from the device // Want to read in regular chunks that match the packet size the device // is using. In this case it is 64 bytes (the interrupt packet limit) // although it would be nice if the device had a different report to // configure this - char buffer[READ_SIZE] = {0}; + char buffer[READ_SIZE] = { 0 }; int ret = hiddev->receive(hidno, buffer, READ_SIZE, READ_TIMEOUT); - if(ret > 0) //read some data - { + if (ret > 0) { // read some data QMutexLocker lock(&m_readBufMtx); // Note: Preprocess the USB packets in this OS independent code // First byte is report ID, second byte is the number of valid bytes m_readBuffer.append(&buffer[2], buffer[1]); emit m_hid->readyRead(); - } - else if(ret == 0) //nothing read - { - } - else // < 0 => error - { - //TODO! make proper error handling, this only quick hack for unplug freeze - m_running=false; + } else if (ret == 0) { // nothing read + } else { // < 0 => error + // TODO! make proper error handling, this only quick hack for unplug freeze + m_running = false; } } m_hid->closeDevice(); @@ -201,6 +195,7 @@ int RawHIDReadThread::getReadData(char *data, int size) qint64 RawHIDReadThread::getBytesAvailable() { QMutexLocker lock(&m_readBufMtx); + return m_readBuffer.size(); } @@ -209,66 +204,60 @@ RawHIDWriteThread::RawHIDWriteThread(RawHID *hid) hiddev(&hid->dev), hidno(hid->m_deviceNo), m_running(true) -{ -} +{} // ********************************************************************************* RawHIDWriteThread::~RawHIDWriteThread() { m_running = false; - //wait for the thread to terminate - if(wait(10000) == false) + // wait for the thread to terminate + if (wait(10000) == false) { qDebug() << "Cannot terminate RawHIDReadThread"; + } } void RawHIDWriteThread::run() { - while(m_running) - { - char buffer[WRITE_SIZE] = {0}; + while (m_running) { + char buffer[WRITE_SIZE] = { 0 }; m_writeBufMtx.lock(); - int size = qMin(WRITE_SIZE-2, m_writeBuffer.size()); - while(size <= 0) - { - //wait on new data to write condition, the timeout - //enable the thread to shutdown properly + int size = qMin(WRITE_SIZE - 2, m_writeBuffer.size()); + while (size <= 0) { + // wait on new data to write condition, the timeout + // enable the thread to shutdown properly m_newDataToWrite.wait(&m_writeBufMtx, 200); - if(!m_running) + if (!m_running) { return; + } size = m_writeBuffer.size(); } - //NOTE: data size is limited to 2 bytes less than the - //usb packet size (64 bytes for interrupt) to make room - //for the reportID and valid data length - size = qMin(WRITE_SIZE-2, m_writeBuffer.size()); + // NOTE: data size is limited to 2 bytes less than the + // usb packet size (64 bytes for interrupt) to make room + // for the reportID and valid data length + size = qMin(WRITE_SIZE - 2, m_writeBuffer.size()); memcpy(&buffer[2], m_writeBuffer.constData(), size); - buffer[1] = size; //valid data length - buffer[0] = 2; //reportID + buffer[1] = size; // valid data length + buffer[0] = 2; // reportID m_writeBufMtx.unlock(); // must hold lock through the send to know how much was sent int ret = hiddev->send(hidno, buffer, WRITE_SIZE, WRITE_TIMEOUT); - if(ret > 0) - { - //only remove the size actually written to the device + if (ret > 0) { + // only remove the size actually written to the device QMutexLocker lock(&m_writeBufMtx); m_writeBuffer.remove(0, size); emit m_hid->bytesWritten(ret - 2); - } - else if(ret < 0) // < 0 => error - { - //TODO! make proper error handling, this only quick hack for unplug freeze - m_running=false; + } else if (ret < 0) { // < 0 => error + // TODO! make proper error handling, this only quick hack for unplug freeze + m_running = false; qDebug() << "Error writing to device"; - } - else - { + } else { qDebug() << "No data written to device ??"; } } @@ -279,7 +268,7 @@ int RawHIDWriteThread::pushDataToWrite(const char *data, int size) QMutexLocker lock(&m_writeBufMtx); m_writeBuffer.append(data, size); - m_newDataToWrite.wakeOne(); //signal that new data arrived + m_newDataToWrite.wakeOne(); // signal that new data arrived return size; } @@ -293,14 +282,13 @@ qint64 RawHIDWriteThread::getBytesToWrite() // ********************************************************************************* RawHID::RawHID(const QString &deviceName) - :QIODevice(), + : QIODevice(), serialNumber(deviceName), m_deviceNo(-1), m_readThread(NULL), - m_writeThread(NULL), - m_mutex(NULL) + m_writeThread(NULL), + m_mutex(NULL) { - m_mutex = new QMutex(QMutex::Recursive); m_startedMutex = new QMutex(); @@ -323,22 +311,24 @@ RawHID::RawHID(const QString &deviceName) * system code is registered in that thread instead of the calling * thread (usually UI) */ -bool RawHID::openDevice() { +bool RawHID::openDevice() +{ int opened = dev.open(USB_MAX_DEVICES, USBMonitor::idVendor_OpenPilot, -1, USB_USAGE_PAGE, USB_USAGE); - for (int i =0; i< opened; i++) { - if (serialNumber == dev.getserial(i)) + + for (int i = 0; i < opened; i++) { + if (serialNumber == dev.getserial(i)) { m_deviceNo = i; - else + } else { dev.close(i); + } } // Now things are opened or not (from read thread) allow the constructor to complete m_startedMutex->unlock(); - //didn't find the device we are trying to open (shouldnt happen) + // didn't find the device we are trying to open (shouldnt happen) device_open = opened >= 0; - if (opened < 0) - { + if (opened < 0) { return false; } @@ -352,39 +342,47 @@ bool RawHID::openDevice() { * It is uses as a callback from the read thread so that the USB * system code is unregistered from that thread\ */ -bool RawHID::closeDevice() { +bool RawHID::closeDevice() +{ dev.close(m_deviceNo); } RawHID::~RawHID() { // If the read thread exists then the device is open - if (m_readThread) + if (m_readThread) { close(); + } } void RawHID::onDeviceUnplugged(int num) { - if (num != m_deviceNo) - return; + if (num != m_deviceNo) { + return; + } - // the USB device has been unplugged - close(); + // the USB device has been unplugged + close(); } bool RawHID::open(OpenMode mode) { - QMutexLocker locker(m_mutex); + QMutexLocker locker(m_mutex); - if (m_deviceNo < 0) + if (m_deviceNo < 0) { return false; + } QIODevice::open(mode); Q_ASSERT(m_readThread); Q_ASSERT(m_writeThread); - if (m_readThread) m_readThread->start(); - if (m_writeThread) m_writeThread->start(); + if (m_readThread) { + m_readThread->start(); + } + if (m_writeThread) { + m_writeThread->start(); + } return true; } @@ -393,8 +391,7 @@ void RawHID::close() { qDebug() << "RawHID::close()"; emit aboutToClose(); - if (m_writeThread) - { + if (m_writeThread) { qDebug() << "About to terminate write thread"; m_writeThread->terminate(); delete m_writeThread; @@ -403,8 +400,7 @@ void RawHID::close() } - if (m_readThread) - { + if (m_readThread) { qDebug() << "About to terminate read thread"; m_readThread->terminate(); delete m_readThread; // calls wait @@ -424,42 +420,46 @@ bool RawHID::isSequential() const qint64 RawHID::bytesAvailable() const { - QMutexLocker locker(m_mutex); + QMutexLocker locker(m_mutex); - if (!m_readThread) - return -1; + if (!m_readThread) { + return -1; + } - return m_readThread->getBytesAvailable() + QIODevice::bytesAvailable(); + return m_readThread->getBytesAvailable() + QIODevice::bytesAvailable(); } qint64 RawHID::bytesToWrite() const { - QMutexLocker locker(m_mutex); + QMutexLocker locker(m_mutex); - if (!m_writeThread) - return -1; + if (!m_writeThread) { + return -1; + } return m_writeThread->getBytesToWrite() + QIODevice::bytesToWrite(); } qint64 RawHID::readData(char *data, qint64 maxSize) { - QMutexLocker locker(m_mutex); + QMutexLocker locker(m_mutex); - if (!m_readThread || !data) - return -1; + if (!m_readThread || !data) { + return -1; + } - return m_readThread->getReadData(data, maxSize); + return m_readThread->getReadData(data, maxSize); } qint64 RawHID::writeData(const char *data, qint64 maxSize) { - QMutexLocker locker(m_mutex); + QMutexLocker locker(m_mutex); - if (!m_writeThread || !data) - return -1; + if (!m_writeThread || !data) { + return -1; + } - return m_writeThread->pushDataToWrite(data, maxSize); + return m_writeThread->pushDataToWrite(data, maxSize); } // ********************************************************************************* diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhid.h b/ground/openpilotgcs/src/plugins/rawhid/rawhid.h index f3487bf20..72275b4b4 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhid.h +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhid.h @@ -38,17 +38,16 @@ #include "pjrc_rawhid.h" #include "usbmonitor.h" -//helper classes +// helper classes class RawHIDReadThread; class RawHIDWriteThread; /** -* The actual IO device that will be used to communicate -* with the board. -*/ -class RAWHID_EXPORT RawHID : public QIODevice -{ - Q_OBJECT + * The actual IO device that will be used to communicate + * with the board. + */ +class RAWHID_EXPORT RawHID : public QIODevice { + Q_OBJECT friend class RawHIDReadThread; friend class RawHIDWriteThread; @@ -63,10 +62,10 @@ public: virtual bool isSequential() const; signals: - void closed(); + void closed(); public slots: - void onDeviceUnplugged(int num); + void onDeviceUnplugged(int num); protected: virtual qint64 readData(char *data, qint64 maxSize); @@ -74,10 +73,10 @@ protected: virtual qint64 bytesAvailable() const; virtual qint64 bytesToWrite() const; - //! Callback from the read thread to open the device + // ! Callback from the read thread to open the device bool openDevice(); - //! Callback from teh read thread to close the device + // ! Callback from teh read thread to close the device bool closeDevice(); QString serialNumber; @@ -89,7 +88,7 @@ protected: RawHIDReadThread *m_readThread; RawHIDWriteThread *m_writeThread; - QMutex *m_mutex; + QMutex *m_mutex; QMutex *m_startedMutex; }; diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhid_const.h b/ground/openpilotgcs/src/plugins/rawhid/rawhid_const.h index db85c4add..9e58720cf 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhid_const.h +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhid_const.h @@ -28,13 +28,13 @@ #ifndef RAWHID_CONST_H #define RAWHID_CONST_H -static const int USB_MAX_DEVICES = 10; +static const int USB_MAX_DEVICES = 10; -static const int USB_VID = 0x20A0; -static const int USB_PID = 0x4117; +static const int USB_VID = 0x20A0; +static const int USB_PID = 0x4117; -static const int USB_USAGE_PAGE = 0xFF9C; -static const int USB_USAGE = 0x0001; +static const int USB_USAGE_PAGE = 0xFF9C; +static const int USB_USAGE = 0x0001; static const int USB_DEV_SERIAL_LEN = 24; diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhid_global.h b/ground/openpilotgcs/src/plugins/rawhid/rawhid_global.h index 31cb98f1e..84ab9c0e0 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhid_global.h +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhid_global.h @@ -10,18 +10,18 @@ * @brief Impliments a HID USB connection to the flight hardware as a QIODevice *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp b/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp index 24276b981..4dd682758 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp @@ -10,18 +10,18 @@ * @brief Impliments a HID USB connection to the flight hardware as a QIODevice *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,55 +41,57 @@ RawHIDConnection::RawHIDConnection() { - //added by andrew - RawHidHandle = NULL; + // added by andrew + RawHidHandle = NULL; enablePolling = true; - m_usbMonitor = USBMonitor::instance(); + m_usbMonitor = USBMonitor::instance(); connect(m_usbMonitor, SIGNAL(deviceDiscovered(USBPortInfo)), this, SLOT(onDeviceConnected())); connect(m_usbMonitor, SIGNAL(deviceRemoved(USBPortInfo)), this, SLOT(onDeviceDisconnected())); - } RawHIDConnection::~RawHIDConnection() { - if (RawHidHandle) - if (RawHidHandle->isOpen()) - RawHidHandle->close(); + if (RawHidHandle) { + if (RawHidHandle->isOpen()) { + RawHidHandle->close(); + } + } } /** - The USB monitor tells us a new device appeared - */ + The USB monitor tells us a new device appeared + */ void RawHIDConnection::onDeviceConnected() { emit availableDevChanged(this); } /** - The USB monitor tells us a device disappeard - */ + The USB monitor tells us a device disappeard + */ void RawHIDConnection::onDeviceDisconnected() { qDebug() << "onDeviceDisconnected()"; - if (enablePolling) + if (enablePolling) { emit availableDevChanged(this); + } } /** - Returns the list of all currently available devices - */ + Returns the list of all currently available devices + */ QList < Core::IConnection::device> RawHIDConnection::availableDevices() { QList < Core::IConnection::device> devices; - QList portsList = m_usbMonitor->availableDevices(USBMonitor::idVendor_OpenPilot, -1, -1,USBMonitor::Running); + QList portsList = m_usbMonitor->availableDevices(USBMonitor::idVendor_OpenPilot, -1, -1, USBMonitor::Running); // We currently list devices by their serial number device dev; foreach(USBPortInfo prt, portsList) { - dev.name=prt.serialNumber; - dev.displayName=prt.product; + dev.name = prt.serialNumber; + dev.displayName = prt.product; devices.append(dev); } return devices; @@ -97,12 +99,13 @@ QList < Core::IConnection::device> RawHIDConnection::availableDevices() QIODevice *RawHIDConnection::openDevice(const QString &deviceName) { - //added by andrew - if (RawHidHandle) + // added by andrew + if (RawHidHandle) { closeDevice(deviceName); - //end added by andrew + } + // end added by andrew - //return new RawHID(deviceName); + // return new RawHID(deviceName); RawHidHandle = new RawHID(deviceName); return RawHidHandle; } @@ -111,12 +114,11 @@ QIODevice *RawHIDConnection::openDevice(const QString &deviceName) void RawHIDConnection::closeDevice(const QString &deviceName) { Q_UNUSED(deviceName); - if (RawHidHandle) - { + if (RawHidHandle) { qDebug() << "Closing the device here"; RawHidHandle->close(); - delete RawHidHandle; - RawHidHandle = NULL; + delete RawHidHandle; + RawHidHandle = NULL; } } @@ -131,7 +133,7 @@ QString RawHIDConnection::shortName() } /** - Tells the Raw HID plugin to stop polling for USB devices + Tells the Raw HID plugin to stop polling for USB devices */ void RawHIDConnection::suspendPolling() { @@ -139,7 +141,7 @@ void RawHIDConnection::suspendPolling() } /** - Tells the Raw HID plugin to resume polling for USB devices + Tells the Raw HID plugin to resume polling for USB devices */ void RawHIDConnection::resumePolling() { @@ -150,26 +152,25 @@ void RawHIDConnection::resumePolling() RawHIDPlugin::RawHIDPlugin() { - hidConnection = NULL; // Pip + hidConnection = NULL; // Pip } RawHIDPlugin::~RawHIDPlugin() { m_usbMonitor->quit(); m_usbMonitor->wait(500); - } void RawHIDPlugin::extensionsInitialized() { - hidConnection = new RawHIDConnection(); - addAutoReleasedObject(hidConnection); + hidConnection = new RawHIDConnection(); + addAutoReleasedObject(hidConnection); - //temp for test - //addAutoReleasedObject(new RawHIDTestThread); + // temp for test + // addAutoReleasedObject(new RawHIDTestThread); } -bool RawHIDPlugin::initialize(const QStringList & arguments, QString * errorString) +bool RawHIDPlugin::initialize(const QStringList & arguments, QString *errorString) { Q_UNUSED(arguments); Q_UNUSED(errorString); diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.h b/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.h index 324471a14..3b241c6e5 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.h +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.h @@ -10,18 +10,18 @@ * @brief Impliments a HID USB connection to the flight hardware as a QIODevice *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,13 +42,12 @@ class RawHIDConnection; /** -* Define a connection via the IConnection interface -* Plugin will add a instance of this class to the pool, -* so the connection manager can use it. -*/ + * Define a connection via the IConnection interface + * Plugin will add a instance of this class to the pool, + * so the connection manager can use it. + */ class RAWHID_EXPORT RawHIDConnection - : public Core::IConnection -{ + : public Core::IConnection { Q_OBJECT public: RawHIDConnection(); @@ -63,7 +62,10 @@ public: virtual void suspendPolling(); virtual void resumePolling(); - bool deviceOpened() { return (RawHidHandle != NULL); } // Pip + bool deviceOpened() + { + return RawHidHandle != NULL; + } // Pip protected slots: void onDeviceConnected(); @@ -75,13 +77,12 @@ private: protected: QMutex m_enumMutex; - USBMonitor* m_usbMonitor; + USBMonitor *m_usbMonitor; bool m_deviceOpened; }; class RAWHID_EXPORT RawHIDPlugin - : public ExtensionSystem::IPlugin -{ + : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -92,8 +93,7 @@ public: virtual void extensionsInitialized(); private: RawHIDConnection *hidConnection; - USBMonitor* m_usbMonitor; - + USBMonitor *m_usbMonitor; }; #endif // RAWHIDPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor.h b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor.h index fd114042f..9dde7e1ec 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor.h +++ b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor.h @@ -34,7 +34,7 @@ #include // Depending on the OS, we'll need different things: -#if defined( Q_OS_MAC) +#if defined(Q_OS_MAC) #include #include #elif defined(Q_OS_UNIX) @@ -42,23 +42,22 @@ #include #include -#elif defined (Q_OS_WIN32) +#elif defined(Q_OS_WIN32) #ifndef _WIN32_WINNT - #define _WIN32_WINNT 0x0500 + #define _WIN32_WINNT 0x0500 #endif #ifndef _WIN32_WINDOWS #define _WIN32_WINDOWS 0x0500 #endif #ifndef WINVER - #define WINVER 0x0500 + #define WINVER 0x0500 #endif #include #include #include #include #include -#endif - +#endif // if defined(Q_OS_MAC) #ifdef Q_OS_WIN @@ -66,60 +65,59 @@ #include class USBMonitor; -class USBRegistrationWidget : public QWidget -{ +class USBRegistrationWidget : public QWidget { Q_OBJECT public: - USBRegistrationWidget( USBMonitor* qese ) { + USBRegistrationWidget(USBMonitor *qese) + { this->qese = qese; } - ~USBRegistrationWidget( ) {} + ~USBRegistrationWidget() {} protected: - USBMonitor* qese; - bool winEvent( MSG* message, long* result ); + USBMonitor *qese; + bool winEvent(MSG *message, long *result); }; #endif #endif struct USBPortInfo { - //QString friendName; ///< Friendly name. - //QString physName; - //QString enumName; ///< It seems its the only one with meaning + // QString friendName; ///< Friendly name. + // QString physName; + // QString enumName; ///< It seems its the only one with meaning QString serialNumber; // As a string as it can be anything, really... QString manufacturer; QString product; #if defined(Q_OS_WIN32) - QString devicePath; //only has meaning on windows + QString devicePath; // only has meaning on windows #elif defined(Q_OS_MAC) IOHIDDeviceRef dev_handle; #endif int UsagePage; int Usage; - int vendorID; ///< Vendor ID. - int productID; ///< Product ID + int vendorID; ///< Vendor ID. + int productID; ///< Product ID int bcdDevice; }; /** -* A monitoring thread which will wait for device events. -*/ + * A monitoring thread which will wait for device events. + */ -class RAWHID_EXPORT USBMonitor : public QThread -{ +class RAWHID_EXPORT USBMonitor : public QThread { Q_OBJECT public: enum RunState { Bootloader = 0x01, - Running = 0x02 + Running = 0x02 }; enum USBConstants { - idVendor_OpenPilot = 0x20a0, - idProduct_OpenPilot = 0x415a, + idVendor_OpenPilot = 0x20a0, + idProduct_OpenPilot = 0x415a, idProduct_CopterControl = 0x415b, - idProduct_PipXtreme = 0x415c + idProduct_PipXtreme = 0x415c }; static USBMonitor *instance(); @@ -128,48 +126,48 @@ public: ~USBMonitor(); QList availableDevices(); QList availableDevices(int vid, int pid, int boardModel, int runState); - #if defined (Q_OS_WIN32) - LRESULT onDeviceChangeWin( WPARAM wParam, LPARAM lParam ); + #if defined(Q_OS_WIN32) + LRESULT onDeviceChangeWin(WPARAM wParam, LPARAM lParam); #endif signals: /*! - A new device has been connected to the system. + A new device has been connected to the system. - setUpNotifications() must be called first to enable event-driven device notifications. - Currently only implemented on Windows and OS X. - \param info The device that has been discovered. - */ - void deviceDiscovered( const USBPortInfo & info ); + setUpNotifications() must be called first to enable event-driven device notifications. + Currently only implemented on Windows and OS X. + \param info The device that has been discovered. + */ + void deviceDiscovered(const USBPortInfo & info); /*! - A device has been disconnected from the system. + A device has been disconnected from the system. - setUpNotifications() must be called first to enable event-driven device notifications. - Currently only implemented on Windows and OS X. - \param info The device that was disconnected. - */ - void deviceRemoved( const USBPortInfo & info ); + setUpNotifications() must be called first to enable event-driven device notifications. + Currently only implemented on Windows and OS X. + \param info The device that was disconnected. + */ + void deviceRemoved(const USBPortInfo & info); private slots: /** - Callback available for whenever the system that is put in place gets - an event + Callback available for whenever the system that is put in place gets + an event */ void deviceEventReceived(); private: - //! Mutex for modifying the list of available devices - QMutex * listMutex; + // ! Mutex for modifying the list of available devices + QMutex *listMutex; - //! List of known devices maintained by callbacks + // ! List of known devices maintained by callbacks QList knowndevices; Q_DISABLE_COPY(USBMonitor) - static USBMonitor *m_instance; + static USBMonitor * m_instance; // Depending on the OS, we'll need different things: -#if defined( Q_OS_MAC) +#if defined(Q_OS_MAC) static void attach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev); static void detach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev); void addDevice(USBPortInfo info); @@ -180,10 +178,10 @@ private: struct udev_monitor *monitor; QSocketNotifier *monitorNotifier; USBPortInfo makePortInfo(struct udev_device *dev); -#elif defined (Q_OS_WIN32) +#elif defined(Q_OS_WIN32) GUID guid_hid; void setUpNotifications(); - /*! + /*! * Get specific property from registry. * \param devInfo pointer to the device information set that contains the interface * and its underlying device. Returned by SetupDiGetClassDevs() function. @@ -193,13 +191,12 @@ private: * \return property string. */ static QString getDeviceProperty(HDEVINFO devInfo, PSP_DEVINFO_DATA devData, DWORD property); - static int infoFromHandle(const GUID & guid,USBPortInfo & info,HDEVINFO & devInfo,DWORD & index); - static void enumerateDevicesWin( const GUID & guidDev, QList* infoList ); + static int infoFromHandle(const GUID & guid, USBPortInfo & info, HDEVINFO & devInfo, DWORD & index); + static void enumerateDevicesWin(const GUID & guidDev, QList *infoList); bool matchAndDispatchChangedDevice(const QString & deviceID, const GUID & guid, WPARAM wParam); #ifdef QT_GUI_LIB - USBRegistrationWidget* notificationWidget; + USBRegistrationWidget *notificationWidget; #endif -#endif - +#endif // if defined(Q_OS_MAC) }; #endif // USBMONITOR_H diff --git a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_linux.cpp b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_linux.cpp index 6b44cc819..7fc9d77c8 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_linux.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_linux.cpp @@ -26,9 +26,9 @@ */ /* - Need help wihth udev ? + Need help wihth udev ? There is a great tuturial at: http://www.signal11.us/oss/udev/ - */ + */ #include "usbmonitor.h" #include @@ -43,65 +43,62 @@ void printPortInfo(struct udev_device *dev) printf(" Devtype: %s", udev_device_get_devtype(dev)); printf(" Action: %s", udev_device_get_action(dev)); /* From here, we can call get_sysattr_value() for each file - in the device's /sys entry. The strings passed into these - functions (idProduct, idVendor, serial, etc.) correspond - directly to the files in the directory which represents - the USB device. Note that USB strings are Unicode, UCS2 - encoded, but the strings returned from - udev_device_get_sysattr_value() are UTF-8 encoded. */ + in the device's /sys entry. The strings passed into these + functions (idProduct, idVendor, serial, etc.) correspond + directly to the files in the directory which represents + the USB device. Note that USB strings are Unicode, UCS2 + encoded, but the strings returned from + udev_device_get_sysattr_value() are UTF-8 encoded. */ printf(" VID/PID/bcdDevice : %s %s %s", - udev_device_get_sysattr_value(dev,"idVendor"), + udev_device_get_sysattr_value(dev, "idVendor"), udev_device_get_sysattr_value(dev, "idProduct"), - udev_device_get_sysattr_value(dev,"bcdDevice")); + udev_device_get_sysattr_value(dev, "bcdDevice")); printf(" %s - %s", - udev_device_get_sysattr_value(dev,"manufacturer"), - udev_device_get_sysattr_value(dev,"product")); + udev_device_get_sysattr_value(dev, "manufacturer"), + udev_device_get_sysattr_value(dev, "product")); printf(" serial: %s", - udev_device_get_sysattr_value(dev, "serial")); - + udev_device_get_sysattr_value(dev, "serial")); } -void USBMonitor::deviceEventReceived() { - +void USBMonitor::deviceEventReceived() +{ qDebug() << "Device event"; struct udev_device *dev; dev = udev_monitor_receive_device(this->monitor); if (dev) { printf("------- Got Device Event"); - QString action = QString(udev_device_get_action(dev)); + QString action = QString(udev_device_get_action(dev)); QString devtype = QString(udev_device_get_devtype(dev)); if (action == "add" && devtype == "usb_device") { printPortInfo(dev); emit deviceDiscovered(makePortInfo(dev)); - } else if (action == "remove" && devtype == "usb_device"){ + } else if (action == "remove" && devtype == "usb_device") { printPortInfo(dev); emit deviceRemoved(makePortInfo(dev)); } udev_device_unref(dev); - } - else { + } else { printf("No Device from receive_device(). An error occured."); } - } -USBMonitor* USBMonitor::instance() +USBMonitor *USBMonitor::instance() { return m_instance; } -USBMonitor* USBMonitor::m_instance = 0; +USBMonitor *USBMonitor::m_instance = 0; /** - Initialize the udev monitor here - */ -USBMonitor::USBMonitor(QObject *parent): QThread(parent) { - - m_instance = this; + Initialize the udev monitor here + */ +USBMonitor::USBMonitor(QObject *parent) : QThread(parent) +{ + m_instance = this; this->context = udev_new(); @@ -124,8 +121,8 @@ USBMonitor::~USBMonitor() } /** -Returns a list of all currently available devices -*/ + Returns a list of all currently available devices + */ QList USBMonitor::availableDevices() { QList devicesList; @@ -134,56 +131,55 @@ QList USBMonitor::availableDevices() struct udev_device *dev; enumerate = udev_enumerate_new(this->context); - udev_enumerate_add_match_subsystem(enumerate,"usb"); -// udev_enumerate_add_match_sysattr(enumerate, "idVendor", "20a0"); + udev_enumerate_add_match_subsystem(enumerate, "usb"); +// udev_enumerate_add_match_sysattr(enumerate, "idVendor", "20a0"); udev_enumerate_scan_devices(enumerate); devices = udev_enumerate_get_list_entry(enumerate); // Will use the 'native' udev functions to loop: - udev_list_entry_foreach(dev_list_entry,devices) { + udev_list_entry_foreach(dev_list_entry, devices) { const char *path; /* Get the filename of the /sys entry for the device - and create a udev_device object (dev) representing it */ + and create a udev_device object (dev) representing it */ path = udev_list_entry_get_name(dev_list_entry); - dev = udev_device_new_from_syspath(this->context, path); - if (QString(udev_device_get_devtype(dev)) == "usb_device") + dev = udev_device_new_from_syspath(this->context, path); + if (QString(udev_device_get_devtype(dev)) == "usb_device") { devicesList.append(makePortInfo(dev)); + } udev_device_unref(dev); } /* free the enumerator object */ udev_enumerate_unref(enumerate); return devicesList; - } /** - Be a bit more picky and ask only for a specific type of device: + Be a bit more picky and ask only for a specific type of device: On OpenPilot, the bcdDeviceLSB indicates the run state: bootloader or running. bcdDeviceMSB indicates the board model. - */ + */ QList USBMonitor::availableDevices(int vid, int pid, int bcdDeviceMSB, int bcdDeviceLSB) { QList allPorts = availableDevices(); QList thePortsWeWant; - foreach (USBPortInfo port, allPorts) { - if((port.vendorID==vid || vid==-1) && (port.productID==pid || pid==-1) && ((port.bcdDevice>>8)==bcdDeviceMSB || bcdDeviceMSB==-1) && - ( (port.bcdDevice&0x00ff) ==bcdDeviceLSB || bcdDeviceLSB==-1)) + foreach(USBPortInfo port, allPorts) { + if ((port.vendorID == vid || vid == -1) && (port.productID == pid || pid == -1) && ((port.bcdDevice >> 8) == bcdDeviceMSB || bcdDeviceMSB == -1) && + ((port.bcdDevice & 0x00ff) == bcdDeviceLSB || bcdDeviceLSB == -1)) { thePortsWeWant.append(port); + } } return thePortsWeWant; } - - USBPortInfo USBMonitor::makePortInfo(struct udev_device *dev) { USBPortInfo prtInfo; ////////// - // Debug info + // Debug info ////////// #ifdef DEBUG printPortInfo(dev); @@ -191,19 +187,15 @@ USBPortInfo USBMonitor::makePortInfo(struct udev_device *dev) bool ok; - prtInfo.vendorID = QString(udev_device_get_sysattr_value(dev, "idVendor")).toInt(&ok, 16); - prtInfo.productID = QString(udev_device_get_sysattr_value(dev, "idProduct")).toInt(&ok, 16); + prtInfo.vendorID = QString(udev_device_get_sysattr_value(dev, "idVendor")).toInt(&ok, 16); + prtInfo.productID = QString(udev_device_get_sysattr_value(dev, "idProduct")).toInt(&ok, 16); prtInfo.serialNumber = QString(udev_device_get_sysattr_value(dev, "serial")); - prtInfo.manufacturer = QString(udev_device_get_sysattr_value(dev,"manufacturer")); - prtInfo.product = QString(udev_device_get_sysattr_value(dev,"product")); -// prtInfo.UsagePage = QString(udev_device_get_sysattr_value(dev,"")); -// prtInfo.Usage = QString(udev_device_get_sysattr_value(dev,"")); - prtInfo.bcdDevice = QString(udev_device_get_sysattr_value(dev,"bcdDevice")).toInt(&ok, 16); - - - + prtInfo.manufacturer = QString(udev_device_get_sysattr_value(dev, "manufacturer")); + prtInfo.product = QString(udev_device_get_sysattr_value(dev, "product")); +// prtInfo.UsagePage = QString(udev_device_get_sysattr_value(dev,"")); +// prtInfo.Usage = QString(udev_device_get_sysattr_value(dev,"")); + prtInfo.bcdDevice = QString(udev_device_get_sysattr_value(dev, "bcdDevice")).toInt(&ok, 16); return prtInfo; - } diff --git a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp index 2f04b4b37..19aac0a47 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp @@ -36,25 +36,28 @@ #define printf qDebug -//! Local helper functions -static bool HID_GetIntProperty(IOHIDDeviceRef dev, CFStringRef property, int * value); +// ! Local helper functions +static bool HID_GetIntProperty(IOHIDDeviceRef dev, CFStringRef property, int *value); static bool HID_GetStrProperty(IOHIDDeviceRef dev, CFStringRef property, QString & value); /** - Initialize the USB monitor here - */ -USBMonitor::USBMonitor(QObject *parent): QThread(parent) { - hid_manager=NULL; + Initialize the USB monitor here + */ +USBMonitor::USBMonitor(QObject *parent) : QThread(parent) +{ + hid_manager = NULL; IOReturn ret; - m_instance = this; + m_instance = this; - listMutex = new QMutex(); + listMutex = new QMutex(); knowndevices.clear(); hid_manager = IOHIDManagerCreate(kCFAllocatorDefault, kIOHIDOptionsTypeNone); if (hid_manager == NULL || CFGetTypeID(hid_manager) != IOHIDManagerGetTypeID()) { - if (hid_manager) CFRelease(hid_manager); + if (hid_manager) { + CFRelease(hid_manager); + } Q_ASSERT(0); } @@ -78,41 +81,40 @@ USBMonitor::USBMonitor(QObject *parent): QThread(parent) { USBMonitor::~USBMonitor() { - //if(hid_manager != NULL) - // IOHIDManagerUnscheduleFromRunLoop(hid_manager, CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); + // if(hid_manager != NULL) + // IOHIDManagerUnscheduleFromRunLoop(hid_manager, CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); quit(); } -void USBMonitor::deviceEventReceived() { - +void USBMonitor::deviceEventReceived() +{ qDebug() << "Device event"; } -USBMonitor* USBMonitor::instance() +USBMonitor *USBMonitor::instance() { return m_instance; } -USBMonitor* USBMonitor::m_instance = 0; +USBMonitor *USBMonitor::m_instance = 0; -void USBMonitor::removeDevice(IOHIDDeviceRef dev) { - for( int i = 0; i < knowndevices.length(); i++) { +void USBMonitor::removeDevice(IOHIDDeviceRef dev) +{ + for (int i = 0; i < knowndevices.length(); i++) { USBPortInfo port = knowndevices.at(i); - if(port.dev_handle == dev) { + if (port.dev_handle == dev) { QMutexLocker locker(listMutex); knowndevices.removeAt(i); emit deviceRemoved(port); return; } } - - } /** - * @brief Static callback for the USB driver to indicate device removed - */ + * @brief Static callback for the USB driver to indicate device removed + */ void USBMonitor::detach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) { Q_UNUSED(context); @@ -123,8 +125,10 @@ void USBMonitor::detach_callback(void *context, IOReturn r, void *hid_mgr, IOHID instance()->removeDevice(dev); } -void USBMonitor::addDevice(USBPortInfo info) { +void USBMonitor::addDevice(USBPortInfo info) +{ QMutexLocker locker(listMutex); + knowndevices.append(info); emit deviceDiscovered(info); } @@ -144,69 +148,72 @@ void USBMonitor::attach_callback(void *context, IOReturn r, void *hid_mgr, IOHID qDebug() << "USBMonitor: Device attached event"; // Populate the device info structure - got_properties &= HID_GetIntProperty(dev, CFSTR( kIOHIDVendorIDKey ), &deviceInfo.vendorID); - got_properties &= HID_GetIntProperty(dev, CFSTR( kIOHIDProductIDKey ), &deviceInfo.productID); - got_properties &= HID_GetIntProperty(dev, CFSTR( kIOHIDVersionNumberKey ), &deviceInfo.bcdDevice); - got_properties &= HID_GetStrProperty(dev, CFSTR( kIOHIDSerialNumberKey ), deviceInfo.serialNumber); - got_properties &= HID_GetStrProperty(dev, CFSTR( kIOHIDProductKey ), deviceInfo.product); - got_properties &= HID_GetStrProperty(dev, CFSTR( kIOHIDManufacturerKey ), deviceInfo.manufacturer); + got_properties &= HID_GetIntProperty(dev, CFSTR(kIOHIDVendorIDKey), &deviceInfo.vendorID); + got_properties &= HID_GetIntProperty(dev, CFSTR(kIOHIDProductIDKey), &deviceInfo.productID); + got_properties &= HID_GetIntProperty(dev, CFSTR(kIOHIDVersionNumberKey), &deviceInfo.bcdDevice); + got_properties &= HID_GetStrProperty(dev, CFSTR(kIOHIDSerialNumberKey), deviceInfo.serialNumber); + got_properties &= HID_GetStrProperty(dev, CFSTR(kIOHIDProductKey), deviceInfo.product); + got_properties &= HID_GetStrProperty(dev, CFSTR(kIOHIDManufacturerKey), deviceInfo.manufacturer); // TOOD: Eventually want to take array of usages if devices start needing that - got_properties &= HID_GetIntProperty(dev, CFSTR( kIOHIDPrimaryUsageKey ), &deviceInfo.Usage); - got_properties &= HID_GetIntProperty(dev, CFSTR( kIOHIDPrimaryUsagePageKey ), &deviceInfo.UsagePage); + got_properties &= HID_GetIntProperty(dev, CFSTR(kIOHIDPrimaryUsageKey), &deviceInfo.Usage); + got_properties &= HID_GetIntProperty(dev, CFSTR(kIOHIDPrimaryUsagePageKey), &deviceInfo.UsagePage); // Currently only enumerating objects that have the complete list of properties - if(got_properties) { + if (got_properties) { qDebug() << "USBMonitor: Adding device"; instance()->addDevice(deviceInfo); } } /** -Returns a list of all currently available devices -*/ + Returns a list of all currently available devices + */ QList USBMonitor::availableDevices() { - //QMutexLocker locker(listMutex); + // QMutexLocker locker(listMutex); return knowndevices; } /** - * @brief Be a bit more picky and ask only for a specific type of device: - * @param[in] vid VID to screen or -1 to ignore - * @param[in] pid PID to screen or -1 to ignore - * @param[in] bcdDeviceMSB MSB of bcdDevice to screen or -1 to ignore - * @param[in] bcdDeviceLSB LSB of bcdDevice to screen or -1 to ignore - * @return List of USBPortInfo that meet this criterion - * @note - * On OpenPilot, the bcdDeviceLSB indicates the run state: bootloader or running. - * bcdDeviceMSB indicates the board model. - */ + * @brief Be a bit more picky and ask only for a specific type of device: + * @param[in] vid VID to screen or -1 to ignore + * @param[in] pid PID to screen or -1 to ignore + * @param[in] bcdDeviceMSB MSB of bcdDevice to screen or -1 to ignore + * @param[in] bcdDeviceLSB LSB of bcdDevice to screen or -1 to ignore + * @return List of USBPortInfo that meet this criterion + * @note + * On OpenPilot, the bcdDeviceLSB indicates the run state: bootloader or running. + * bcdDeviceMSB indicates the board model. + */ QList USBMonitor::availableDevices(int vid, int pid, int bcdDeviceMSB, int bcdDeviceLSB) { QList allPorts = availableDevices(); QList thePortsWeWant; - foreach (USBPortInfo port, allPorts) { - if((port.vendorID==vid || vid==-1) && (port.productID==pid || pid==-1) && ((port.bcdDevice>>8)==bcdDeviceMSB || bcdDeviceMSB==-1) && - ( (port.bcdDevice&0x00ff) ==bcdDeviceLSB || bcdDeviceLSB==-1)) + foreach(USBPortInfo port, allPorts) { + if ((port.vendorID == vid || vid == -1) && (port.productID == pid || pid == -1) && ((port.bcdDevice >> 8) == bcdDeviceMSB || bcdDeviceMSB == -1) && + ((port.bcdDevice & 0x00ff) == bcdDeviceLSB || bcdDeviceLSB == -1)) { thePortsWeWant.append(port); + } } return thePortsWeWant; } /** - * @brief Helper function get get a HID integer property - * @param[in] dev Device reference - * @param[in] property The property to get (constants defined in IOKIT) - * @param[out] value Pointer to integer to set - * @return True if successful, false otherwise - */ -static bool HID_GetIntProperty(IOHIDDeviceRef dev, CFStringRef property, int * value) { + * @brief Helper function get get a HID integer property + * @param[in] dev Device reference + * @param[in] property The property to get (constants defined in IOKIT) + * @param[out] value Pointer to integer to set + * @return True if successful, false otherwise + */ +static bool HID_GetIntProperty(IOHIDDeviceRef dev, CFStringRef property, int *value) +{ CFTypeRef prop = IOHIDDeviceGetProperty(dev, property); + if (prop) { if (CFNumberGetTypeID() == CFGetTypeID(prop)) { // if a number - CFNumberGetValue((CFNumberRef) prop, kCFNumberSInt32Type, value); + CFNumberGetValue((CFNumberRef)prop, kCFNumberSInt32Type, value); return true; } } @@ -214,18 +221,20 @@ static bool HID_GetIntProperty(IOHIDDeviceRef dev, CFStringRef property, int * v } /** - * @brief Helper function get get a HID string property - * @param[in] dev Device reference - * @param[in] property The property to get (constants defined in IOKIT) - * @param[out] value The QString to set - * @return True if successful, false otherwise - */ -static bool HID_GetStrProperty(IOHIDDeviceRef dev, CFStringRef property, QString & value) { + * @brief Helper function get get a HID string property + * @param[in] dev Device reference + * @param[in] property The property to get (constants defined in IOKIT) + * @param[out] value The QString to set + * @return True if successful, false otherwise + */ +static bool HID_GetStrProperty(IOHIDDeviceRef dev, CFStringRef property, QString & value) +{ CFTypeRef prop = IOHIDDeviceGetProperty(dev, property); + if (prop) { if (CFStringGetTypeID() == CFGetTypeID(prop)) { // if a string char buffer[2550]; - bool success = CFStringGetCString ( (CFStringRef) prop, buffer, sizeof(buffer), kCFStringEncodingMacRoman ); + bool success = CFStringGetCString((CFStringRef)prop, buffer, sizeof(buffer), kCFStringEncodingMacRoman); value = QString(buffer); return success; } diff --git a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_win.cpp b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_win.cpp index aa653179a..04f5c8154 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_win.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_win.cpp @@ -34,105 +34,108 @@ #include #define printf qDebug -void USBMonitor::deviceEventReceived() { - +void USBMonitor::deviceEventReceived() +{ qDebug() << "Device event"; // Dispatch and emit the signals here... - } -USBMonitor* USBMonitor::instance() +USBMonitor *USBMonitor::instance() { return m_instance; } -USBMonitor* USBMonitor::m_instance = 0; +USBMonitor *USBMonitor::m_instance = 0; - -USBMonitor::USBMonitor(QObject *parent): QThread(parent) { +USBMonitor::USBMonitor(QObject *parent) : QThread(parent) +{ HidD_GetHidGuid(&guid_hid); - if( !QMetaType::isRegistered( QMetaType::type("USBPortInfo") ) ) + if (!QMetaType::isRegistered(QMetaType::type("USBPortInfo"))) { qRegisterMetaType("USBPortInfo"); + } #if (defined QT_GUI_LIB) notificationWidget = 0; #endif // Q_OS_WIN setUpNotifications(); - m_instance=this; + m_instance = this; } USBMonitor::~USBMonitor() { #if (defined QT_GUI_LIB) - if( notificationWidget ) + if (notificationWidget) { delete notificationWidget; + } #endif quit(); } /** - Be a bit more picky and ask only for a specific type of device: + Be a bit more picky and ask only for a specific type of device: On OpenPilot, the bcdDeviceLSB indicates the run state: bootloader or running. bcdDeviceMSB indicates the board model. - */ + */ QList USBMonitor::availableDevices(int vid, int pid, int bcdDeviceMSB, int bcdDeviceLSB) { QList allPorts = availableDevices(); - qDebug()<<"USBMonitor::availableDevices list off all ports:"; - qDebug()<<"USBMonitor::availableDevices total ports:"< thePortsWeWant; - qDebug()<<"USBMonitor::availableDevices bcdLSB="< QString conversions and vice versa */ #ifdef UNICODE -#define QStringToTCHAR(x) (wchar_t*) x.utf16() -#define PQStringToTCHAR(x) (wchar_t*) x->utf16() -#define TCHARToQString(x) QString::fromUtf16((ushort*)(x)) -#define TCHARToQStringN(x,y) QString::fromUtf16((ushort*)(x),(y)) +#define QStringToTCHAR(x) (wchar_t *)x.utf16() +#define PQStringToTCHAR(x) (wchar_t *)x->utf16() +#define TCHARToQString(x) QString::fromUtf16((ushort *)(x)) +#define TCHARToQStringN(x, y) QString::fromUtf16((ushort *)(x), (y)) #else #define QStringToTCHAR(x) x.local8Bit().constData() #define PQStringToTCHAR(x) x->local8Bit().constData() #define TCHARToQString(x) QString::fromLocal8Bit((x)) -#define TCHARToQStringN(x,y) QString::fromLocal8Bit((x),(y)) +#define TCHARToQStringN(x, y) QString::fromLocal8Bit((x), (y)) #endif /*UNICODE*/ -void USBMonitor::setUpNotifications( ) +void USBMonitor::setUpNotifications() { #ifdef QT_GUI_LIB - if(notificationWidget) + if (notificationWidget) { return; + } notificationWidget = new USBRegistrationWidget(this); DEV_BROADCAST_DEVICEINTERFACE dbh; @@ -140,36 +143,35 @@ void USBMonitor::setUpNotifications( ) dbh.dbcc_size = sizeof(dbh); dbh.dbcc_devicetype = DBT_DEVTYP_DEVICEINTERFACE; CopyMemory(&dbh.dbcc_classguid, &guid_hid, sizeof(GUID)); - if( RegisterDeviceNotification( notificationWidget->winId( ), &dbh, DEVICE_NOTIFY_WINDOW_HANDLE ) == NULL) + if (RegisterDeviceNotification(notificationWidget->winId(), &dbh, DEVICE_NOTIFY_WINDOW_HANDLE) == NULL) { qWarning() << "RegisterDeviceNotification failed:" << GetLastError(); + } // setting up notifications doesn't tell us about devices already connected // so get those manually - foreach( USBPortInfo port,availableDevices() ) - emit deviceDiscovered( port ); + foreach(USBPortInfo port, availableDevices()) + emit deviceDiscovered(port); #else qWarning("GUI not enabled - can't register for device notifications."); #endif // QT_GUI_LIB } -LRESULT USBMonitor::onDeviceChangeWin( WPARAM wParam, LPARAM lParam ) +LRESULT USBMonitor::onDeviceChangeWin(WPARAM wParam, LPARAM lParam) { - if ( DBT_DEVICEARRIVAL == wParam || DBT_DEVICEREMOVECOMPLETE == wParam ) - { + if (DBT_DEVICEARRIVAL == wParam || DBT_DEVICEREMOVECOMPLETE == wParam) { PDEV_BROADCAST_HDR pHdr = (PDEV_BROADCAST_HDR)lParam; - if( pHdr->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE ) - { + if (pHdr->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE) { PDEV_BROADCAST_DEVICEINTERFACE pDevInf = (PDEV_BROADCAST_DEVICEINTERFACE)pHdr; // delimiters are different across APIs...change to backslash. ugh. QString deviceID = TCHARToQString(pDevInf->dbcc_name).toUpper().replace("#", "\\"); - matchAndDispatchChangedDevice(deviceID,guid_hid, wParam); + matchAndDispatchChangedDevice(deviceID, guid_hid, wParam); } } return 0; } #ifdef QT_GUI_LIB -bool USBRegistrationWidget::winEvent( MSG* message, long* result ) +bool USBRegistrationWidget::winEvent(MSG *message, long *result) { - if ( message->message == WM_DEVICECHANGE ) { - qese->onDeviceChangeWin( message->wParam, message->lParam ); + if (message->message == WM_DEVICECHANGE) { + qese->onDeviceChangeWin(message->wParam, message->lParam); *result = 1; return true; } @@ -178,76 +180,62 @@ bool USBRegistrationWidget::winEvent( MSG* message, long* result ) #endif bool USBMonitor::matchAndDispatchChangedDevice(const QString & deviceID, const GUID & guid, WPARAM wParam) { - qDebug()<<"USB_MONITOR matchAndDispatchChangedDevice deviceID="< USBMonitor::availableDevices() { QList ports; enumerateDevicesWin(guid_hid, &ports); - //qDebug()<<"USBMonitorWin availabledevices="<append(info); - else if (r==0) + } else if (r == 0) { break; + } } SetupDiDestroyDeviceInfoList(devInfo); } } -int USBMonitor::infoFromHandle(const GUID & guid,USBPortInfo & info,HDEVINFO & devInfo,DWORD & index) +int USBMonitor::infoFromHandle(const GUID & guid, USBPortInfo & info, HDEVINFO & devInfo, DWORD & index) { - //qDebug()<<"index0="<DevicePath); + + free(details); + return 2; } - //qDebug()<<"index3="<DevicePath, GENERIC_READ|GENERIC_WRITE, FILE_SHARE_READ|FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL); - - if (h == INVALID_HANDLE_VALUE) - { - DWORD err = GetLastError(); - - // I get ERROR_ACCESS_DENIED with most/all my input devices (mice/trackballs/tablet). - // Let's not log it :) - if (err == ERROR_ACCESS_DENIED) - { - free(details); - return 2; - } - - qDebug() << "Problem opening handle, path: " << QString().fromWCharArray(details->DevicePath); - - free(details); - return 2; - } - //qDebug()<<"index4="<DevicePath).toUpper().replace("#", "\\"); - attrib.Size = sizeof(HIDD_ATTRIBUTES); + // qDebug()<<"DETAILS???"<DevicePath).toUpper().replace("#", "\\"); + attrib.Size = sizeof(HIDD_ATTRIBUTES); ret = HidD_GetAttributes(h, &attrib); - info.vendorID=attrib.VendorID; - info.productID=attrib.ProductID; - info.bcdDevice=attrib.VersionNumber; + info.vendorID = attrib.VendorID; + info.productID = attrib.ProductID; + info.bcdDevice = attrib.VersionNumber; - if (!ret || !HidD_GetPreparsedData(h, &hid_data)) - { - CloseHandle(h); - return 2; + if (!ret || !HidD_GetPreparsedData(h, &hid_data)) { + CloseHandle(h); + return 2; } - //qDebug()<<"index5="<> 8) == m_boardModel || m_boardModel == -1) && + ((port.bcdDevice & 0x00ff) == m_runState || m_runState == -1)) { + qDebug() << "USBSignalFilter emit device discovered"; emit deviceDiscovered(); } } -USBSignalFilter::USBSignalFilter(int vid, int pid, int boardModel, int runState): +USBSignalFilter::USBSignalFilter(int vid, int pid, int boardModel, int runState) : m_vid(vid), m_pid(pid), m_boardModel(boardModel), m_runState(runState) { - connect(USBMonitor::instance(),SIGNAL(deviceDiscovered(USBPortInfo)),this,SLOT(m_deviceDiscovered(USBPortInfo))); + connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), this, SLOT(m_deviceDiscovered(USBPortInfo))); } - - diff --git a/ground/openpilotgcs/src/plugins/rawhid/usbsignalfilter.h b/ground/openpilotgcs/src/plugins/rawhid/usbsignalfilter.h index 6a9310d46..a3eafa131 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/usbsignalfilter.h +++ b/ground/openpilotgcs/src/plugins/rawhid/usbsignalfilter.h @@ -3,8 +3,7 @@ #include #include "usbmonitor.h" -class RAWHID_EXPORT USBSignalFilter : public QObject -{ +class RAWHID_EXPORT USBSignalFilter : public QObject { Q_OBJECT private: int m_vid; diff --git a/ground/openpilotgcs/src/plugins/scope/plotdata.cpp b/ground/openpilotgcs/src/plugins/scope/plotdata.cpp index 8d4a8ff69..673b322d8 100644 --- a/ground/openpilotgcs/src/plugins/scope/plotdata.cpp +++ b/ground/openpilotgcs/src/plugins/scope/plotdata.cpp @@ -31,49 +31,47 @@ #include PlotData::PlotData(QString p_uavObject, QString p_uavField) -{ +{ uavObject = p_uavObject; - if(p_uavField.contains("-")) - { + if (p_uavField.contains("-")) { QStringList fieldSubfield = p_uavField.split("-", QString::SkipEmptyParts); - uavField = fieldSubfield.at(0); - uavSubField = fieldSubfield.at(1); + uavField = fieldSubfield.at(0); + uavSubField = fieldSubfield.at(1); haveSubField = true; - } - else - { - uavField = p_uavField; + } else { + uavField = p_uavField; haveSubField = false; } - xData = new QVector(); - yData = new QVector(); - yDataHistory = new QVector(); + xData = new QVector(); + yData = new QVector(); + yDataHistory = new QVector(); - curve = 0; - scalePower = 0; + curve = 0; + scalePower = 0; meanSamples = 1; - meanSum = 0.0f; -// mathFunction=0; - correctionSum = 0.0f; + meanSum = 0.0f; +// mathFunction=0; + correctionSum = 0.0f; correctionCount = 0; - yMinimum = 0; - yMaximum = 0; + yMinimum = 0; + yMaximum = 0; - m_xWindowSize = 0; + m_xWindowSize = 0; } -double PlotData::valueAsDouble(UAVObject* obj, UAVObjectField* field) +double PlotData::valueAsDouble(UAVObject *obj, UAVObjectField *field) { Q_UNUSED(obj); QVariant value; - if(haveSubField){ + if (haveSubField) { int indexOfSubField = field->getElementNames().indexOf(QRegExp(uavSubField, Qt::CaseSensitive, QRegExp::FixedString)); value = field->getValue(indexOfSubField); - }else + } else { value = field->getValue(); + } // qDebug() << "Data (" << value.typeName() << ") " << value.toString(); @@ -88,63 +86,60 @@ PlotData::~PlotData() } -bool SequentialPlotData::append(UAVObject* obj) +bool SequentialPlotData::append(UAVObject *obj) { if (uavObject == obj->getName()) { - - //Get the field of interest - UAVObjectField* field = obj->getField(uavField); + // Get the field of interest + UAVObjectField *field = obj->getField(uavField); if (field) { - double currentValue = valueAsDouble(obj, field) * pow(10, scalePower); - //Perform scope math, if necessary - if (mathFunction == "Boxcar average" || mathFunction == "Standard deviation"){ - //Put the new value at the front - yDataHistory->append( currentValue ); + // Perform scope math, if necessary + if (mathFunction == "Boxcar average" || mathFunction == "Standard deviation") { + // Put the new value at the front + yDataHistory->append(currentValue); // calculate average value meanSum += currentValue; - if(yDataHistory->size() > meanSamples) { + if (yDataHistory->size() > meanSamples) { meanSum -= yDataHistory->first(); yDataHistory->pop_front(); } // make sure to correct the sum every meanSamples steps to prevent it // from running away due to floating point rounding errors - correctionSum+=currentValue; + correctionSum += currentValue; if (++correctionCount >= meanSamples) { meanSum = correctionSum; correctionSum = 0.0f; correctionCount = 0; } - double boxcarAvg=meanSum/yDataHistory->size(); + double boxcarAvg = meanSum / yDataHistory->size(); - if ( mathFunction == "Standard deviation" ){ - //Calculate square of sample standard deviation, with Bessel's correction - double stdSum=0; - for (int i=0; i < yDataHistory->size(); i++){ - stdSum+= pow(yDataHistory->at(i)- boxcarAvg,2)/(meanSamples-1); + if (mathFunction == "Standard deviation") { + // Calculate square of sample standard deviation, with Bessel's correction + double stdSum = 0; + for (int i = 0; i < yDataHistory->size(); i++) { + stdSum += pow(yDataHistory->at(i) - boxcarAvg, 2) / (meanSamples - 1); } yData->append(sqrt(stdSum)); - } - else { + } else { yData->append(boxcarAvg); } - } - else{ - yData->append( currentValue ); + } else { + yData->append(currentValue); } - if (yData->size() > m_xWindowSize) { //If new data overflows the window, remove old data... + if (yData->size() > m_xWindowSize) { // If new data overflows the window, remove old data... yData->pop_front(); - } else //...otherwise, add a new y point at position xData + } else { // ...otherwise, add a new y point at position xData xData->insert(xData->size(), xData->size()); + } - //notify the gui of changes in the data - //dataChanged(); + // notify the gui of changes in the data + // dataChanged(); return true; } } @@ -152,65 +147,63 @@ bool SequentialPlotData::append(UAVObject* obj) return false; } -bool ChronoPlotData::append(UAVObject* obj) +bool ChronoPlotData::append(UAVObject *obj) { if (uavObject == obj->getName()) { - //Get the field of interest - UAVObjectField* field = obj->getField(uavField); - //qDebug() << "uavObject: " << uavObject << ", uavField: " << uavField; + // Get the field of interest + UAVObjectField *field = obj->getField(uavField); + // qDebug() << "uavObject: " << uavObject << ", uavField: " << uavField; if (field) { - QDateTime NOW = QDateTime::currentDateTime(); //THINK ABOUT REIMPLEMENTING THIS TO SHOW UAVO TIME, NOT SYSTEM TIME + QDateTime NOW = QDateTime::currentDateTime(); // THINK ABOUT REIMPLEMENTING THIS TO SHOW UAVO TIME, NOT SYSTEM TIME double currentValue = valueAsDouble(obj, field) * pow(10, scalePower); - //Perform scope math, if necessary - if (mathFunction == "Boxcar average" || mathFunction == "Standard deviation"){ - //Put the new value at the back - yDataHistory->append( currentValue ); + // Perform scope math, if necessary + if (mathFunction == "Boxcar average" || mathFunction == "Standard deviation") { + // Put the new value at the back + yDataHistory->append(currentValue); // calculate average value meanSum += currentValue; - if(yDataHistory->size() > meanSamples) { + if (yDataHistory->size() > meanSamples) { meanSum -= yDataHistory->first(); yDataHistory->pop_front(); } // make sure to correct the sum every meanSamples steps to prevent it // from running away due to floating point rounding errors - correctionSum+=currentValue; + correctionSum += currentValue; if (++correctionCount >= meanSamples) { meanSum = correctionSum; correctionSum = 0.0f; correctionCount = 0; } - double boxcarAvg=meanSum/yDataHistory->size(); -//qDebug()<size(); i++){ - stdSum+= pow(yDataHistory->at(i)- boxcarAvg,2)/(meanSamples-1); + double boxcarAvg = meanSum / yDataHistory->size(); +// qDebug()<size(); i++) { + stdSum += pow(yDataHistory->at(i) - boxcarAvg, 2) / (meanSamples - 1); } yData->append(sqrt(stdSum)); - } - else { + } else { yData->append(boxcarAvg); } - } - else{ - yData->append( currentValue ); + } else { + yData->append(currentValue); } double valueX = NOW.toTime_t() + NOW.time().msec() / 1000.0; xData->append(valueX); - //qDebug() << "Data " << uavObject << "." << field->getName() << " X,Y:" << valueX << "," << valueY; + // qDebug() << "Data " << uavObject << "." << field->getName() << " X,Y:" << valueX << "," << valueY; - //Remove stale data + // Remove stale data removeStaleData(); - //notify the gui of chages in the data - //dataChanged(); + // notify the gui of chages in the data + // dataChanged(); return true; } } @@ -224,8 +217,9 @@ void ChronoPlotData::removeStaleData() double oldestValue; while (1) { - if (xData->size() == 0) + if (xData->size() == 0) { break; + } newestValue = xData->last(); oldestValue = xData->first(); @@ -233,21 +227,22 @@ void ChronoPlotData::removeStaleData() if (newestValue - oldestValue > m_xWindowSize) { yData->pop_front(); xData->pop_front(); - } else + } else { break; + } } - //qDebug() << "removeStaleData "; + // qDebug() << "removeStaleData "; } void ChronoPlotData::removeStaleDataTimeout() { removeStaleData(); - //dataChanged(); - //qDebug() << "removeStaleDataTimeout"; + // dataChanged(); + // qDebug() << "removeStaleDataTimeout"; } -bool UAVObjectPlotData::append(UAVObject* obj) +bool UAVObjectPlotData::append(UAVObject *obj) { Q_UNUSED(obj); return false; diff --git a/ground/openpilotgcs/src/plugins/scope/plotdata.h b/ground/openpilotgcs/src/plugins/scope/plotdata.h index 358f97aa2..5b17ca917 100644 --- a/ground/openpilotgcs/src/plugins/scope/plotdata.h +++ b/ground/openpilotgcs/src/plugins/scope/plotdata.h @@ -41,8 +41,8 @@ #include /*! -\brief Defines the different type of plots. - */ + \brief Defines the different type of plots. + */ enum PlotType { SequentialPlot, ChronoPlot, @@ -52,10 +52,9 @@ enum PlotType { }; /*! - \brief Base class that keeps the data for each curve in the plot. - */ -class PlotData : public QObject -{ + \brief Base class that keeps the data for each curve in the plot. + */ +class PlotData : public QObject { Q_OBJECT public: @@ -66,7 +65,7 @@ public: QString uavField; QString uavSubField; bool haveSubField; - int scalePower; //This is the power to which each value must be raised + int scalePower; // This is the power to which each value must be raised int meanSamples; double meanSum; QString mathFunction; @@ -75,71 +74,71 @@ public: double yMinimum; double yMaximum; double m_xWindowSize; - QwtPlotCurve* curve; - QVector* xData; - QVector* yData; - QVector* yDataHistory; + QwtPlotCurve *curve; + QVector *xData; + QVector *yData; + QVector *yDataHistory; - virtual bool append(UAVObject* obj) = 0; - virtual PlotType plotType() = 0; + virtual bool append(UAVObject *obj) = 0; + virtual PlotType plotType() = 0; virtual void removeStaleData() = 0; void updatePlotCurveData(); protected: - double valueAsDouble(UAVObject* obj, UAVObjectField* field); + double valueAsDouble(UAVObject *obj, UAVObjectField *field); signals: void dataChanged(); }; /*! - \brief The sequential plot have a fixed size buffer of data. All the curves in one plot - have the same size buffer. - */ -class SequentialPlotData : public PlotData -{ + \brief The sequential plot have a fixed size buffer of data. All the curves in one plot + have the same size buffer. + */ +class SequentialPlotData : public PlotData { Q_OBJECT public: SequentialPlotData(QString uavObject, QString uavField) - : PlotData(uavObject, uavField) {} + : PlotData(uavObject, uavField) {} ~SequentialPlotData() {} /*! - \brief Append new data to the plot - */ - bool append(UAVObject* obj); + \brief Append new data to the plot + */ + bool append(UAVObject *obj); /*! - \brief The type of plot - */ - virtual PlotType plotType() { + \brief The type of plot + */ + virtual PlotType plotType() + { return SequentialPlot; } /*! - \brief Removes the old data from the buffer - */ - virtual void removeStaleData(){} + \brief Removes the old data from the buffer + */ + virtual void removeStaleData() {} }; /*! - \brief The chrono plot have a variable sized buffer of data, where the data is for a specified time period. - */ -class ChronoPlotData : public PlotData -{ + \brief The chrono plot have a variable sized buffer of data, where the data is for a specified time period. + */ +class ChronoPlotData : public PlotData { Q_OBJECT public: ChronoPlotData(QString uavObject, QString uavField) - : PlotData(uavObject, uavField) { + : PlotData(uavObject, uavField) + { scalePower = 1; } - ~ChronoPlotData() { - } + ~ChronoPlotData() {} - bool append(UAVObject* obj); + bool append(UAVObject *obj); - virtual PlotType plotType() { + virtual PlotType plotType() + { return ChronoPlot; } @@ -152,24 +151,24 @@ private slots: }; /*! - \brief UAVObject plot use a fixed size buffer of data, where the horizontal axis values come from - a UAVObject field. - */ -class UAVObjectPlotData : public PlotData -{ + \brief UAVObject plot use a fixed size buffer of data, where the horizontal axis values come from + a UAVObject field. + */ +class UAVObjectPlotData : public PlotData { Q_OBJECT public: UAVObjectPlotData(QString uavObject, QString uavField) - : PlotData(uavObject, uavField) {} + : PlotData(uavObject, uavField) {} ~UAVObjectPlotData() {} - bool append(UAVObject* obj); + bool append(UAVObject *obj); - virtual PlotType plotType() { + virtual PlotType plotType() + { return UAVObjectPlot; - } + } - virtual void removeStaleData(){} + virtual void removeStaleData() {} }; #endif // PLOTDATA_H diff --git a/ground/openpilotgcs/src/plugins/scope/scope_global.h b/ground/openpilotgcs/src/plugins/scope/scope_global.h index 6da462245..648369745 100644 --- a/ground/openpilotgcs/src/plugins/scope/scope_global.h +++ b/ground/openpilotgcs/src/plugins/scope/scope_global.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,4 +38,3 @@ #endif #endif // UAVOBJECTS_GLOBAL_H - diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadget.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadget.cpp index ec7c01113..a403233bd 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadget.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadget.cpp @@ -59,7 +59,8 @@ void ScopeGadget::loadConfiguration(IUAVGadgetConfiguration *config) int mean = plotCurveConfig->yMeanSamples; QString mathFunction = plotCurveConfig->mathFunction; QRgb color = plotCurveConfig->color; - bool antialiased = plotCurveConfig->drawAntialiased; + bool antialiased = plotCurveConfig->drawAntialiased; + widget->addCurvePlot( uavObject, uavField, @@ -71,7 +72,7 @@ void ScopeGadget::loadConfiguration(IUAVGadgetConfiguration *config) Qt::SolidLine, Qt::SquareCap, Qt::BevelJoin), - antialiased + antialiased ); } diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadget.h b/ground/openpilotgcs/src/plugins/scope/scopegadget.h index 9d7c5ef3e..8b06739b6 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadget.h +++ b/ground/openpilotgcs/src/plugins/scope/scopegadget.h @@ -33,29 +33,31 @@ #include "scopegadgetwidget.h" class IUAVGadget; -//class QList; +// class QList; class QWidget; class QString; class ScopeGadgetWidget; using namespace Core; -class ScopeGadget : public Core::IUAVGadget -{ +class ScopeGadget : public Core::IUAVGadget { Q_OBJECT public: ScopeGadget(QString classId, ScopeGadgetWidget *widget, QWidget *parent = 0); ~ScopeGadget(); - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); - QList context() const { + QList context() const + { return m_context; } - QWidget *widget() { + QWidget *widget() + { return m_widget; } - QString contextHelpId() const { + QString contextHelpId() const + { return QString(); } diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.cpp index cc1095cfc..0dd99f35d 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.cpp @@ -55,16 +55,16 @@ ScopeGadgetConfiguration::ScopeGadgetConfiguration(QString classId, QSettings *q qSettings->beginGroup(QString("plotCurve") + QString().number(plotDatasLoadIndex)); PlotCurveConfiguration *plotCurveConf = new PlotCurveConfiguration(); - plotCurveConf->uavObject = qSettings->value("uavObject").toString(); - plotCurveConf->uavField = qSettings->value("uavField").toString(); - plotCurveConf->color = qSettings->value("color").value(); - plotCurveConf->yScalePower = qSettings->value("yScalePower").toInt(); - plotCurveConf->mathFunction = qSettings->value("mathFunction").toString(); - plotCurveConf->yMeanSamples = qSettings->value("yMeanSamples", 1).toInt(); - plotCurveConf->yMeanSamples = qSettings->value("yMeanSamples", 1).toInt(); + plotCurveConf->uavObject = qSettings->value("uavObject").toString(); + plotCurveConf->uavField = qSettings->value("uavField").toString(); + plotCurveConf->color = qSettings->value("color").value(); + plotCurveConf->yScalePower = qSettings->value("yScalePower").toInt(); + plotCurveConf->mathFunction = qSettings->value("mathFunction").toString(); + plotCurveConf->yMeanSamples = qSettings->value("yMeanSamples", 1).toInt(); + plotCurveConf->yMeanSamples = qSettings->value("yMeanSamples", 1).toInt(); plotCurveConf->drawAntialiased = qSettings->value("drawAntialiased", true).toBool(); - plotCurveConf->yMinimum = qSettings->value("yMinimum").toDouble(); - plotCurveConf->yMaximum = qSettings->value("yMaximum").toDouble(); + plotCurveConf->yMinimum = qSettings->value("yMinimum").toDouble(); + plotCurveConf->yMaximum = qSettings->value("yMaximum").toDouble(); m_plotCurveConfigs.append(plotCurveConf); qSettings->endGroup(); @@ -72,7 +72,7 @@ ScopeGadgetConfiguration::ScopeGadgetConfiguration(QString classId, QSettings *q m_loggingEnabled = qSettings->value("LoggingEnabled").toBool(); m_loggingNewFileOnConnect = qSettings->value("LoggingNewFileOnConnect").toBool(); - m_loggingPath = qSettings->value("LoggingPath").toString(); + m_loggingPath = qSettings->value("LoggingPath").toString(); } } @@ -109,15 +109,15 @@ IUAVGadgetConfiguration *ScopeGadgetConfiguration::clone() PlotCurveConfiguration *currentPlotCurveConf = m_plotCurveConfigs.at(plotDatasLoadIndex); PlotCurveConfiguration *newPlotCurveConf = new PlotCurveConfiguration(); - newPlotCurveConf->uavObject = currentPlotCurveConf->uavObject; - newPlotCurveConf->uavField = currentPlotCurveConf->uavField; - newPlotCurveConf->color = currentPlotCurveConf->color; - newPlotCurveConf->yScalePower = currentPlotCurveConf->yScalePower; - newPlotCurveConf->yMeanSamples = currentPlotCurveConf->yMeanSamples; - newPlotCurveConf->mathFunction = currentPlotCurveConf->mathFunction; + newPlotCurveConf->uavObject = currentPlotCurveConf->uavObject; + newPlotCurveConf->uavField = currentPlotCurveConf->uavField; + newPlotCurveConf->color = currentPlotCurveConf->color; + newPlotCurveConf->yScalePower = currentPlotCurveConf->yScalePower; + newPlotCurveConf->yMeanSamples = currentPlotCurveConf->yMeanSamples; + newPlotCurveConf->mathFunction = currentPlotCurveConf->mathFunction; newPlotCurveConf->drawAntialiased = currentPlotCurveConf->drawAntialiased; - newPlotCurveConf->yMinimum = currentPlotCurveConf->yMinimum; - newPlotCurveConf->yMaximum = currentPlotCurveConf->yMaximum; + newPlotCurveConf->yMinimum = currentPlotCurveConf->yMinimum; + newPlotCurveConf->yMaximum = currentPlotCurveConf->yMaximum; m->addPlotCurveConfig(newPlotCurveConf); } diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.cpp index adbdcdd0d..df75fe0a5 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.cpp @@ -32,15 +32,13 @@ #include ScopeGadgetFactory::ScopeGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("ScopeGadget"), - tr("Scope"), - parent) -{ -} + IUAVGadgetFactory(QString("ScopeGadget"), + tr("Scope"), + parent) +{} ScopeGadgetFactory::~ScopeGadgetFactory() -{ -} +{} void ScopeGadgetFactory::stopPlotting() { @@ -53,20 +51,21 @@ void ScopeGadgetFactory::startPlotting() } -Core::IUAVGadget* ScopeGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *ScopeGadgetFactory::createGadget(QWidget *parent) { - ScopeGadgetWidget* gadgetWidget = new ScopeGadgetWidget(parent); - connect(this,SIGNAL(onStartPlotting()), gadgetWidget, SLOT(startPlotting())); - connect(this,SIGNAL(onStopPlotting()), gadgetWidget, SLOT(stopPlotting())); + ScopeGadgetWidget *gadgetWidget = new ScopeGadgetWidget(parent); + + connect(this, SIGNAL(onStartPlotting()), gadgetWidget, SLOT(startPlotting())); + connect(this, SIGNAL(onStopPlotting()), gadgetWidget, SLOT(stopPlotting())); return new ScopeGadget(QString("ScopeGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *ScopeGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *ScopeGadgetFactory::createConfiguration(QSettings *qSettings) { return new ScopeGadgetConfiguration(QString("ScopeGadget"), qSettings); } IOptionsPage *ScopeGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new ScopeGadgetOptionsPage(qobject_cast(config)); + return new ScopeGadgetOptionsPage(qobject_cast(config)); } diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.h b/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.h index 16fd4f431..11a77f2ca 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.h @@ -31,23 +31,21 @@ #include "scope_global.h" #include -namespace Core -{ +namespace Core { class IUAVGadget; class IUAVGadgetFactory; } using namespace Core; -class SCOPE_EXPORT ScopeGadgetFactory : public IUAVGadgetFactory -{ +class SCOPE_EXPORT ScopeGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: ScopeGadgetFactory(QObject *parent = 0); ~ScopeGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); public slots: @@ -57,7 +55,6 @@ public slots: signals: void onStopPlotting(); void onStartPlotting(); - }; #endif // SCOPEGADGETFACTORY_H_ diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.cpp index e0c0a0143..8ba4440b6 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.cpp @@ -103,13 +103,13 @@ QWidget *ScopeGadgetOptionsPage::createPage(QWidget *parent) // add the configured curves foreach(PlotCurveConfiguration * plotData, m_config->plotCurveConfigs()) { - QString uavObject = plotData->uavObject; - QString uavField = plotData->uavField; + QString uavObject = plotData->uavObject; + QString uavField = plotData->uavField; int scale = plotData->yScalePower; int mean = plotData->yMeanSamples; QString mathFunction = plotData->mathFunction; - QVariant varColor = plotData->color; - bool antialiased = plotData->drawAntialiased; + QVariant varColor = plotData->color; + bool antialiased = plotData->drawAntialiased; addPlotCurveConfig(uavObject, uavField, scale, mean, mathFunction, varColor, antialiased); } @@ -335,7 +335,7 @@ void ScopeGadgetOptionsPage::on_btnAddCurve_clicked() QVariant varColor = (int)QColor(options_page->btnColor->text()).rgb(); - bool antialiased = options_page->drawAntialiasedCheckBox->isChecked(); + bool antialiased = options_page->drawAntialiasedCheckBox->isChecked(); // Find an existing plot curve config based on the uavobject and uav field. If it // exists, update it, else add a new one. if (options_page->lstCurves->count() && @@ -363,7 +363,7 @@ void ScopeGadgetOptionsPage::addPlotCurveConfig(QString uavObject, QString uavFi void ScopeGadgetOptionsPage::setCurvePlotProperties(QListWidgetItem *listWidgetItem, QString uavObject, QString uavField, int scale, int mean, QString mathFunction, QVariant varColor, bool antialias) { - bool parseOK = false; + bool parseOK = false; // Set the properties of the newly added list item QRgb rgbColor = (QRgb)varColor.toInt(&parseOK); diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.h b/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.h index da081c5c5..a7e57b65d 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.h @@ -42,25 +42,24 @@ #include /*! - \brief This class is used to render the time values on the horizontal axis for the - ChronoPlot. - */ -class TimeScaleDraw : public QwtScaleDraw -{ + \brief This class is used to render the time values on the horizontal axis for the + ChronoPlot. + */ +class TimeScaleDraw : public QwtScaleDraw { public: - TimeScaleDraw() { - } - virtual QwtText label(double v) const { - uint seconds = (uint)(v); + TimeScaleDraw() {} + virtual QwtText label(double v) const + { + uint seconds = (uint)(v); QDateTime upTime = QDateTime::fromTime_t(seconds); - QTime timePart = upTime.time().addMSecs((v - seconds )* 1000); + QTime timePart = upTime.time().addMSecs((v - seconds) * 1000); + upTime.setTime(timePart); return upTime.toLocalTime().toString("hh:mm:ss"); } }; -class ScopeGadgetWidget : public QwtPlot -{ +class ScopeGadgetWidget : public QwtPlot { Q_OBJECT public: @@ -70,34 +69,58 @@ public: void setupSequentialPlot(); void setupChronoPlot(); void setupUAVObjectPlot(); - PlotType plotType(){return m_plotType;} + PlotType plotType() + { + return m_plotType; + } - void setXWindowSize(double xWindowSize){m_xWindowSize = xWindowSize;} - double xWindowSize(){return m_xWindowSize;} - void setRefreshInterval(double refreshInterval){m_refreshInterval = refreshInterval;} - int refreshInterval(){return m_refreshInterval;} + void setXWindowSize(double xWindowSize) + { + m_xWindowSize = xWindowSize; + } + double xWindowSize() + { + return m_xWindowSize; + } + void setRefreshInterval(double refreshInterval) + { + m_refreshInterval = refreshInterval; + } + int refreshInterval() + { + return m_refreshInterval; + } void addCurvePlot(QString uavObject, QString uavFieldSubField, int scaleOrderFactor = 0, int meanSamples = 1, - QString mathFunction= "None", QPen pen = QPen(Qt::black), bool antialiased = true); + QString mathFunction = "None", QPen pen = QPen(Qt::black), bool antialiased = true); void clearCurvePlots(); int csvLoggingStart(); int csvLoggingStop(); void csvLoggingSetName(QString); - void setLoggingEnabled(bool value){m_csvLoggingEnabled=value;} - void setLoggingNewFileOnConnect(bool value){m_csvLoggingNewFileOnConnect=value;} - void setLoggingPath(QString value){m_csvLoggingPath=value;} + void setLoggingEnabled(bool value) + { + m_csvLoggingEnabled = value; + } + void setLoggingNewFileOnConnect(bool value) + { + m_csvLoggingNewFileOnConnect = value; + } + void setLoggingPath(QString value) + { + m_csvLoggingPath = value; + } protected: - void mousePressEvent(QMouseEvent *e); - void mouseReleaseEvent(QMouseEvent *e); - void mouseDoubleClickEvent(QMouseEvent *e); - void mouseMoveEvent(QMouseEvent *e); - void wheelEvent(QWheelEvent *e); + void mousePressEvent(QMouseEvent *e); + void mouseReleaseEvent(QMouseEvent *e); + void mouseDoubleClickEvent(QMouseEvent *e); + void mouseMoveEvent(QMouseEvent *e); + void wheelEvent(QWheelEvent *e); void showEvent(QShowEvent *e); private slots: - void uavObjectReceived(UAVObject*); + void uavObjectReceived(UAVObject *); void replotNewData(); void showCurve(QwtPlotItem *item, bool on); void startPlotting(); @@ -115,7 +138,7 @@ private: double m_xWindowSize; int m_refreshInterval; QList m_connectedUAVObjects; - QMap m_curvesData; + QMap m_curvesData; QTimer *replotTimer; @@ -136,14 +159,14 @@ private: QString m_csvLoggingBuffer; QFile m_csvLoggingFile; - QMutex mutex; + QMutex mutex; int csvLoggingInsertHeader(); int csvLoggingAddData(); int csvLoggingInsertData(); - void deleteLegend(); - void addLegend(); + void deleteLegend(); + void addLegend(); }; diff --git a/ground/openpilotgcs/src/plugins/scope/scopeplugin.cpp b/ground/openpilotgcs/src/plugins/scope/scopeplugin.cpp index 889c29f3b..9ff600b20 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopeplugin.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopeplugin.cpp @@ -44,7 +44,7 @@ ScopePlugin::~ScopePlugin() } -bool ScopePlugin::initialize(const QStringList& args, QString *errMsg) +bool ScopePlugin::initialize(const QStringList & args, QString *errMsg) { Q_UNUSED(args); Q_UNUSED(errMsg); @@ -64,4 +64,3 @@ void ScopePlugin::shutdown() // Do nothing } Q_EXPORT_PLUGIN(ScopePlugin) - diff --git a/ground/openpilotgcs/src/plugins/scope/scopeplugin.h b/ground/openpilotgcs/src/plugins/scope/scopeplugin.h index 1abadcfee..d51527c30 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopeplugin.h +++ b/ground/openpilotgcs/src/plugins/scope/scopeplugin.h @@ -32,19 +32,16 @@ class ScopeGadgetFactory; -class ScopePlugin : public ExtensionSystem::IPlugin -{ +class ScopePlugin : public ExtensionSystem::IPlugin { public: ScopePlugin(); ~ScopePlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); private: ScopeGadgetFactory *mf; - - }; #endif /* SCOPEPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serial_global.h b/ground/openpilotgcs/src/plugins/serialconnection/serial_global.h index 832676a99..e284778a1 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serial_global.h +++ b/ground/openpilotgcs/src/plugins/serialconnection/serial_global.h @@ -3,7 +3,7 @@ * * @file serial_global.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief + * @brief * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup SerialPlugin Serial Connection Plugin @@ -11,18 +11,18 @@ * @brief Impliments serial connection to the flight hardware for Telemetry *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp b/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp index 1f7a476f0..b56dbb001 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp @@ -10,18 +10,18 @@ * @brief Impliments serial connection to the flight hardware for Telemetry *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,38 +36,34 @@ #include - SerialEnumerationThread::SerialEnumerationThread(SerialConnection *serial) : m_serial(serial), m_running(true) -{ -} +{} SerialEnumerationThread::~SerialEnumerationThread() { m_running = false; - //wait for the thread to terminate - if(wait(2100) == false) + // wait for the thread to terminate + if (wait(2100) == false) { qDebug() << "Cannot terminate SerialEnumerationThread"; + } } void SerialEnumerationThread::run() { QList devices = m_serial->availableDevices(); - while(m_running) - { - if(!m_serial->deviceOpened()) - { + while (m_running) { + if (!m_serial->deviceOpened()) { QList newDev = m_serial->availableDevices(); - if(devices != newDev) - { + if (devices != newDev) { devices = newDev; emit enumerationChanged(); } } - msleep(2000); //update available devices every two seconds (doesn't need more) + msleep(2000); // update available devices every two seconds (doesn't need more) } } @@ -75,45 +71,45 @@ void SerialEnumerationThread::run() SerialConnection::SerialConnection() : enablePolling(true), m_enumerateThread(this) { - serialHandle = NULL; - m_config = new SerialPluginConfiguration("Serial Telemetry", NULL, this); + serialHandle = NULL; + m_config = new SerialPluginConfiguration("Serial Telemetry", NULL, this); m_config->restoresettings(); - m_optionspage = new SerialPluginOptionsPage(m_config,this); + m_optionspage = new SerialPluginOptionsPage(m_config, this); // Experimental: enable polling on all OS'es since there // were reports that autodetect does not work on XP amongst // others. - //#ifdef Q_OS_WIN -// //I'm cheating a little bit here: -// //Knowing if the device enumeration really changed is a bit complicated -// //so I just signal it whenever we have a device event... -// QMainWindow *mw = Core::ICore::instance()->mainWindow(); -// QObject::connect(mw, SIGNAL(deviceChange()), -// this, SLOT(onEnumerationChanged())); -//#else + // #ifdef Q_OS_WIN +////I'm cheating a little bit here: +////Knowing if the device enumeration really changed is a bit complicated +////so I just signal it whenever we have a device event... +// QMainWindow *mw = Core::ICore::instance()->mainWindow(); +// QObject::connect(mw, SIGNAL(deviceChange()), +// this, SLOT(onEnumerationChanged())); +// #else // Other OSes do not send such signals: QObject::connect(&m_enumerateThread, SIGNAL(enumerationChanged()), this, SLOT(onEnumerationChanged())); m_enumerateThread.start(); -//#endif +// #endif } SerialConnection::~SerialConnection() -{ -} +{} void SerialConnection::onEnumerationChanged() { - if (enablePolling) + if (enablePolling) { emit availableDevChanged(this); + } } -bool sortPorts(const QextPortInfo &s1,const QextPortInfo &s2) +bool sortPorts(const QextPortInfo &s1, const QextPortInfo &s2) { - return s1.portName SerialConnection::availableDevices() @@ -123,13 +119,14 @@ QList SerialConnection::availableDevices() if (enablePolling) { QList ports = QextSerialEnumerator::getPorts(); - //sort the list by port number (nice idea from PT_Dreamer :)) - qSort(ports.begin(), ports.end(),sortPorts); - foreach( QextPortInfo port, ports ) { - device d; - d.displayName=port.friendName; - d.name=port.physName; - list.append(d); + // sort the list by port number (nice idea from PT_Dreamer :)) + qSort(ports.begin(), ports.end(), sortPorts); + foreach(QextPortInfo port, ports) { + device d; + + d.displayName = port.friendName; + d.name = port.physName; + list.append(d); } } @@ -138,28 +135,27 @@ QList SerialConnection::availableDevices() QIODevice *SerialConnection::openDevice(const QString &deviceName) { - if (serialHandle){ + if (serialHandle) { closeDevice(deviceName); } QList ports = QextSerialEnumerator::getPorts(); - foreach( QextPortInfo port, ports ) { - if(port.physName == deviceName) - { - //we need to handle port settings here... + foreach(QextPortInfo port, ports) { + if (port.physName == deviceName) { + // we need to handle port settings here... PortSettings set; - set.BaudRate = stringToBaud(m_config->speed()); - qDebug()<<"Serial telemetry running at "<speed(); - set.DataBits = DATA_8; - set.Parity = PAR_NONE; - set.StopBits = STOP_1; + set.BaudRate = stringToBaud(m_config->speed()); + qDebug() << "Serial telemetry running at " << m_config->speed(); + set.DataBits = DATA_8; + set.Parity = PAR_NONE; + set.StopBits = STOP_1; set.FlowControl = FLOW_OFF; set.Timeout_Millisec = 500; #ifdef Q_OS_WIN - serialHandle = new QextSerialPort(port.portName, set); + serialHandle = new QextSerialPort(port.portName, set); #else - serialHandle = new QextSerialPort(port.physName, set); + serialHandle = new QextSerialPort(port.physName, set); #endif - m_deviceOpened = true; + m_deviceOpened = true; return serialHandle; } } @@ -169,10 +165,10 @@ QIODevice *SerialConnection::openDevice(const QString &deviceName) void SerialConnection::closeDevice(const QString &deviceName) { Q_UNUSED(deviceName); - //we have to delete the serial connection we created - if (serialHandle){ + // we have to delete the serial connection we created + if (serialHandle) { serialHandle->deleteLater(); - serialHandle = NULL; + serialHandle = NULL; m_deviceOpened = false; } } @@ -189,7 +185,7 @@ QString SerialConnection::shortName() } /** - Tells the Serial plugin to stop polling for serial devices + Tells the Serial plugin to stop polling for serial devices */ void SerialConnection::suspendPolling() { @@ -197,7 +193,7 @@ void SerialConnection::suspendPolling() } /** - Tells the Serial plugin to resume polling for serial devices + Tells the Serial plugin to resume polling for serial devices */ void SerialConnection::resumePolling() { @@ -205,48 +201,49 @@ void SerialConnection::resumePolling() } BaudRateType SerialConnection::stringToBaud(QString str) -{ - if(str=="1200") +{ + if (str == "1200") { return BAUD1200; - if(str=="1800") + } + if (str == "1800") { return BAUD1800; - else if(str=="2400") + } else if (str == "2400") { return BAUD2400; - else if(str== "4800") + } else if (str == "4800") { return BAUD4800; - else if(str== "9600") + } else if (str == "9600") { return BAUD9600; - else if(str== "14400") + } else if (str == "14400") { return BAUD14400; - else if(str== "19200") + } else if (str == "19200") { return BAUD19200; - else if(str== "38400") + } else if (str == "38400") { return BAUD38400; - else if(str== "56000") + } else if (str == "56000") { return BAUD56000; - else if(str== "57600") + } else if (str == "57600") { return BAUD57600; - else if(str== "76800") + } else if (str == "76800") { return BAUD76800; - else if(str== "115200") + } else if (str == "115200") { return BAUD115200; - else if(str== "128000") + } else if (str == "128000") { return BAUD128000; - else if(str== "230400") + } else if (str == "230400") { return BAUD230400; - else if(str== "256000") + } else if (str == "256000") { return BAUD256000; - else if(str== "460800") + } else if (str == "460800") { return BAUD460800; - else if(str== "921600") + } else if (str == "921600") { return BAUD921600; - else + } else { return BAUD57600; + } } SerialPlugin::SerialPlugin() -{ -} +{} SerialPlugin::~SerialPlugin() { @@ -263,9 +260,9 @@ bool SerialPlugin::initialize(const QStringList &arguments, QString *errorString Q_UNUSED(arguments); Q_UNUSED(errorString); m_connection = new SerialConnection(); - //must manage this registration of child object ourselves - //if we use an autorelease here it causes the GCS to crash - //as it is deleting objects as the app closes... + // must manage this registration of child object ourselves + // if we use an autorelease here it causes the GCS to crash + // as it is deleting objects as the app closes... addObject(m_connection->Optionspage()); return true; } diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.h b/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.h index 388117ead..41fccdf2a 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.h +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.h @@ -10,25 +10,25 @@ * @brief Impliments serial connection to the flight hardware for Telemetry *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef SERIALPLUGIN_H #define SERIALPLUGIN_H -//#include "serial_global.h" +// #include "serial_global.h" #include #include #include "coreplugin/iconnection.h" @@ -42,13 +42,12 @@ class QextSerialEnumerator; class SerialConnection; /** -* Helper thread to check on new serial port connection/disconnection -* Some operating systems do not send device insertion events so -* for those we have to poll -*/ -//class SERIAL_EXPORT SerialEnumerationThread : public QThread -class SerialEnumerationThread : public QThread -{ + * Helper thread to check on new serial port connection/disconnection + * Some operating systems do not send device insertion events so + * for those we have to poll + */ +// class SERIAL_EXPORT SerialEnumerationThread : public QThread +class SerialEnumerationThread : public QThread { Q_OBJECT public: SerialEnumerationThread(SerialConnection *serial); @@ -66,14 +65,13 @@ protected: /** -* Define a connection via the IConnection interface -* Plugin will add a instance of this class to the pool, -* so the connection manager can use it. -*/ -//class SERIAL_EXPORT SerialConnection + * Define a connection via the IConnection interface + * Plugin will add a instance of this class to the pool, + * so the connection manager can use it. + */ +// class SERIAL_EXPORT SerialConnection class SerialConnection - : public Core::IConnection -{ + : public Core::IConnection { Q_OBJECT public: SerialConnection(); @@ -88,13 +86,22 @@ public: virtual void suspendPolling(); virtual void resumePolling(); - bool deviceOpened() {return m_deviceOpened;} - SerialPluginConfiguration * Config() const { return m_config; } - SerialPluginOptionsPage * Optionspage() const { return m_optionspage; } + bool deviceOpened() + { + return m_deviceOpened; + } + SerialPluginConfiguration *Config() const + { + return m_config; + } + SerialPluginOptionsPage *Optionspage() const + { + return m_optionspage; + } private: - QextSerialPort* serialHandle; + QextSerialPort *serialHandle; bool enablePolling; SerialPluginConfiguration *m_config; SerialPluginOptionsPage *m_optionspage; @@ -109,11 +116,9 @@ protected: }; - -//class SERIAL_EXPORT SerialPlugin +// class SERIAL_EXPORT SerialPlugin class SerialPlugin - : public ExtensionSystem::IPlugin -{ + : public ExtensionSystem::IPlugin { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.cpp b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.cpp index 8d8c9bbe6..54c896781 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.cpp @@ -46,8 +46,7 @@ SerialPluginConfiguration::SerialPluginConfiguration(QString classId, QSettings } SerialPluginConfiguration::~SerialPluginConfiguration() -{ -} +{} /** * Clones a configuration. @@ -56,6 +55,7 @@ SerialPluginConfiguration::~SerialPluginConfiguration() IUAVGadgetConfiguration *SerialPluginConfiguration::clone() { SerialPluginConfiguration *m = new SerialPluginConfiguration(this->classId()); + m->m_speed = m_speed; return m; } @@ -64,8 +64,9 @@ IUAVGadgetConfiguration *SerialPluginConfiguration::clone() * Saves a configuration. * */ -void SerialPluginConfiguration::saveConfig(QSettings* settings) const { - settings->setValue("speed", m_speed); +void SerialPluginConfiguration::saveConfig(QSettings *settings) const +{ + settings->setValue("speed", m_speed); } void SerialPluginConfiguration::restoresettings() @@ -75,8 +76,7 @@ void SerialPluginConfiguration::restoresettings() qDebug() << "SerialPluginConfiguration::restoresettings - speed" << str; if (str.isEmpty()) { m_speed = "57600"; - } - else { + } else { m_speed = str; } settings->endGroup(); diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.h b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.h index 41a1e2c5a..8335105ea 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.h +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.h @@ -35,15 +35,17 @@ using namespace Core; /* Despite its name, this is actually a generic analog Serial supporting up to two rotating "needle" indicators. - */ -class SerialPluginConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT + */ +class SerialPluginConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: explicit SerialPluginConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); virtual ~SerialPluginConfiguration(); - QString speed() { return m_speed; } + QString speed() + { + return m_speed; + } void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); void savesettings() const; @@ -54,8 +56,10 @@ private: QSettings *settings; public slots: - void setSpeed(QString speed) { m_speed = speed; } - + void setSpeed(QString speed) + { + m_speed = speed; + } }; #endif // SERIALPLUGINCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.cpp b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.cpp index bd46dbb47..b83f98b9f 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.cpp @@ -10,7 +10,7 @@ * @{ * @brief Impliments serial connection to the flight hardware for Telemetry *****************************************************************************/ - /* +/* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3 of the License, or @@ -32,50 +32,48 @@ #include "extensionsystem/pluginmanager.h" SerialPluginOptionsPage::SerialPluginOptionsPage(SerialPluginConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *SerialPluginOptionsPage::createPage(QWidget *parent) { - Q_UNUSED(parent); options_page = new Ui::SerialPluginOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget; - //main layout + // main layout options_page->setupUi(optionsPageWidget); QStringList allowedSpeeds; - allowedSpeeds<<"1200" -#ifdef Q_OS_UNIX - <<"1800" //POSIX ONLY -#endif - <<"2400" - <<"4800" - <<"9600" -#ifdef Q_OS_WIN - <<"14400" //WINDOWS ONLY -#endif - <<"19200" - <<"38400" -#ifdef Q_OS_WIN - <<"56000" //WINDOWS ONLY -#endif - <<"57600" -#ifdef Q_OS_UNIX - <<"76800" //POSIX ONLY -#endif - <<"115200" -#ifdef Q_OS_WIN - <<"128000" //WINDOWS ONLY - <<"230400" //WINDOWS ONLY - <<"256000" //WINDOWS ONLY - <<"460800" //WINDOWS ONLY - <<"921600" //WINDOWS ONLY -#endif - ; + allowedSpeeds << "1200" + #ifdef Q_OS_UNIX + << "1800" // POSIX ONLY + #endif + << "2400" + << "4800" + << "9600" + #ifdef Q_OS_WIN + << "14400" // WINDOWS ONLY + #endif + << "19200" + << "38400" + #ifdef Q_OS_WIN + << "56000" // WINDOWS ONLY + #endif + << "57600" + #ifdef Q_OS_UNIX + << "76800" // POSIX ONLY + #endif + << "115200" + #ifdef Q_OS_WIN + << "128000" // WINDOWS ONLY + << "230400" // WINDOWS ONLY + << "256000" // WINDOWS ONLY + << "460800" // WINDOWS ONLY + << "921600" // WINDOWS ONLY + #endif + ; options_page->cb_speed->addItems(allowedSpeeds); @@ -96,7 +94,6 @@ void SerialPluginOptionsPage::apply() } - void SerialPluginOptionsPage::finish() { delete options_page; diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.h b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.h index 6ce23b743..1a0475f48 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.h +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.h @@ -10,7 +10,7 @@ * @{ * @brief Impliments serial connection to the flight hardware for Telemetry *****************************************************************************/ - /* +/* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3 of the License, or @@ -42,21 +42,32 @@ class IUAVpluginConfiguration; class SerialPluginConfiguration; namespace Ui { - class SerialPluginOptionsPage; +class SerialPluginOptionsPage; } using namespace Core; -class SerialPluginOptionsPage : public IOptionsPage -{ -Q_OBJECT +class SerialPluginOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit SerialPluginOptionsPage(SerialPluginConfiguration *config, QObject *parent = 0); - QString id() const { return QLatin1String("settings"); } - QString trName() const { return tr("settings"); } - QString category() const { return "Serial Telemetry"; } - QString trCategory() const { return "Serial Telemetry"; } + QString id() const + { + return QLatin1String("settings"); + } + QString trName() const + { + return tr("settings"); + } + QString category() const + { + return "Serial Telemetry"; + } + QString trCategory() const + { + return "Serial Telemetry"; + } QWidget *createPage(QWidget *parent); void apply(); void finish(); @@ -64,8 +75,6 @@ public: private: Ui::SerialPluginOptionsPage *options_page; SerialPluginConfiguration *m_config; - - }; #endif // SERIALpluginOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp index bfba40313..1b3f64804 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp @@ -37,31 +37,29 @@ BiasCalibrationUtil::BiasCalibrationUtil(long measurementCount, long measurementRate) : QObject(), m_isMeasuring(false), m_accelMeasurementCount(measurementCount), m_gyroMeasurementCount(measurementCount), m_accelMeasurementRate(measurementRate), m_gyroMeasurementRate(measurementRate) -{ -} +{} BiasCalibrationUtil::BiasCalibrationUtil(long accelMeasurementCount, long accelMeasurementRate, - long gyroMeasurementCount, long gyroMeasurementRate) : QObject(), + long gyroMeasurementCount, long gyroMeasurementRate) : QObject(), m_isMeasuring(false), m_accelMeasurementCount(accelMeasurementCount), m_gyroMeasurementCount(gyroMeasurementCount), m_accelMeasurementRate(accelMeasurementRate), m_gyroMeasurementRate(gyroMeasurementRate) -{ -} +{} void BiasCalibrationUtil::start() { - if(!m_isMeasuring) { + if (!m_isMeasuring) { startMeasurement(); // Set up timeout timer connect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout())); - //Set timeout to max time of measurement + 10 secs + // Set timeout to max time of measurement + 10 secs m_timeoutTimer.start(std::max(m_accelMeasurementCount * m_accelMeasurementRate, m_gyroMeasurementCount * m_gyroMeasurementRate) + 10000); } } void BiasCalibrationUtil::abort() { - if(m_isMeasuring) { + if (m_isMeasuring) { stopMeasurement(); } } @@ -70,12 +68,12 @@ void BiasCalibrationUtil::gyroMeasurementsUpdated(UAVObject *obj) { Q_UNUSED(obj); - if(m_receivedGyroUpdates < m_gyroMeasurementCount) { + if (m_receivedGyroUpdates < m_gyroMeasurementCount) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * uavObjectManager = pm->getObject(); + UAVObjectManager *uavObjectManager = pm->getObject(); Q_ASSERT(uavObjectManager); - Gyros * gyros = Gyros::GetInstance(uavObjectManager); + Gyros *gyros = Gyros::GetInstance(uavObjectManager); Gyros::DataFields gyrosData = gyros->getData(); m_gyroX += gyrosData.x; @@ -84,9 +82,8 @@ void BiasCalibrationUtil::gyroMeasurementsUpdated(UAVObject *obj) m_receivedGyroUpdates++; emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount); - } - else if (m_receivedAccelUpdates >= m_accelMeasurementCount && - m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { + } else if (m_receivedAccelUpdates >= m_accelMeasurementCount && + m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { stopMeasurement(); } } @@ -95,12 +92,12 @@ void BiasCalibrationUtil::accelMeasurementsUpdated(UAVObject *obj) { Q_UNUSED(obj); - if(m_receivedAccelUpdates < m_accelMeasurementCount) { + if (m_receivedAccelUpdates < m_accelMeasurementCount) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * uavObjectManager = pm->getObject(); + UAVObjectManager *uavObjectManager = pm->getObject(); Q_ASSERT(uavObjectManager); - Accels * accels = Accels::GetInstance(uavObjectManager); + Accels *accels = Accels::GetInstance(uavObjectManager); Accels::DataFields accelsData = accels->getData(); m_accelerometerX += accelsData.x; @@ -109,9 +106,8 @@ void BiasCalibrationUtil::accelMeasurementsUpdated(UAVObject *obj) m_receivedAccelUpdates++; emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount); - } - else if (m_receivedAccelUpdates >= m_accelMeasurementCount && - m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { + } else if (m_receivedAccelUpdates >= m_accelMeasurementCount && + m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { stopMeasurement(); } } @@ -138,7 +134,7 @@ void BiasCalibrationUtil::startMeasurement() m_gyroZ = 0; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * uavObjectManager = pm->getObject(); + UAVObjectManager *uavObjectManager = pm->getObject(); Q_ASSERT(uavObjectManager); // Disable gyro bias correction to see raw data @@ -148,7 +144,7 @@ void BiasCalibrationUtil::startMeasurement() // Set up to receive updates for accels UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager); - connect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelMeasurementsUpdated(UAVObject*))); + connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(accelMeasurementsUpdated(UAVObject *))); // Set update period for accels m_previousAccelMetaData = uavObject->getMetadata(); @@ -159,7 +155,7 @@ void BiasCalibrationUtil::startMeasurement() // Set up to receive updates from gyros uavObject = Gyros::GetInstance(uavObjectManager); - connect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(gyroMeasurementsUpdated(UAVObject*))); + connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroMeasurementsUpdated(UAVObject *))); // Set update period for gyros m_previousGyroMetaData = uavObject->getMetadata(); @@ -175,22 +171,22 @@ void BiasCalibrationUtil::stopMeasurement() m_isMeasuring = false; - //Stop timeout timer + // Stop timeout timer m_timeoutTimer.stop(); disconnect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout())); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * uavObjectManager = pm->getObject(); + UAVObjectManager *uavObjectManager = pm->getObject(); Q_ASSERT(uavObjectManager); // Stop listening for updates from accels UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager); - disconnect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelMeasurementsUpdated(UAVObject*))); + disconnect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(accelMeasurementsUpdated(UAVObject *))); uavObject->setMetadata(m_previousAccelMetaData); // Stop listening for updates from gyros uavObject = Gyros::GetInstance(uavObjectManager); - disconnect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(gyroMeasurementsUpdated(UAVObject*))); + disconnect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroMeasurementsUpdated(UAVObject *))); uavObject->setMetadata(m_previousGyroMetaData); // Enable gyro bias correction again diff --git a/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.h b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.h index 980305eeb..c5e0af4ed 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.h @@ -34,13 +34,12 @@ #include "uavobject.h" #include "vehicleconfigurationsource.h" -class BiasCalibrationUtil : public QObject -{ +class BiasCalibrationUtil : public QObject { Q_OBJECT public: explicit BiasCalibrationUtil(long measurementCount, long measurementRate); explicit BiasCalibrationUtil(long accelMeasurementCount, long accelMeasurementRate, - long gyroMeasurementCount, long gyroMeasurementRate); + long gyroMeasurementCount, long gyroMeasurementRate); signals: void progress(long current, long total); @@ -52,8 +51,8 @@ public slots: void abort(); private slots: - void gyroMeasurementsUpdated(UAVObject * obj); - void accelMeasurementsUpdated(UAVObject * obj); + void gyroMeasurementsUpdated(UAVObject *obj); + void accelMeasurementsUpdated(UAVObject *obj); void timeout(); private: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp index 9f1064c39..4a19eb4cf 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp @@ -31,7 +31,7 @@ #include "connectiondiagram.h" #include "ui_connectiondiagram.h" -ConnectionDiagram::ConnectionDiagram(QWidget *parent, VehicleConfigurationSource* configSource) : +ConnectionDiagram::ConnectionDiagram(QWidget *parent, VehicleConfigurationSource *configSource) : QDialog(parent), ui(new Ui::ConnectionDiagram), m_configSource(configSource), m_background(0) { ui->setupUi(this); @@ -47,12 +47,14 @@ ConnectionDiagram::~ConnectionDiagram() void ConnectionDiagram::resizeEvent(QResizeEvent *event) { QWidget::resizeEvent(event); + ui->connectionDiagram->fitInView(m_background, Qt::KeepAspectRatio); } -void ConnectionDiagram::showEvent(QShowEvent * event) +void ConnectionDiagram::showEvent(QShowEvent *event) { QWidget::showEvent(event); + ui->connectionDiagram->fitInView(m_background, Qt::KeepAspectRatio); } @@ -60,90 +62,85 @@ void ConnectionDiagram::setupGraphicsScene() { m_renderer = new QSvgRenderer(); if (QFile::exists(QString(":/setupwizard/resources/connection-diagrams.svg")) && - m_renderer->load(QString(":/setupwizard/resources/connection-diagrams.svg")) && - m_renderer->isValid()) - { + m_renderer->load(QString(":/setupwizard/resources/connection-diagrams.svg")) && + m_renderer->isValid()) { m_scene = new QGraphicsScene(this); ui->connectionDiagram->setScene(m_scene); - //ui->connectionDiagram->setViewportUpdateMode(QGraphicsView::FullViewportUpdate); + // ui->connectionDiagram->setViewportUpdateMode(QGraphicsView::FullViewportUpdate); m_background = new QGraphicsSvgItem(); m_background->setSharedRenderer(m_renderer); m_background->setElementId("background"); m_background->setOpacity(0); - //m_background->setFlags(QGraphicsItem::ItemClipsToShape); + // m_background->setFlags(QGraphicsItem::ItemClipsToShape); m_background->setZValue(-1); m_scene->addItem(m_background); QList elementsToShow; - switch(m_configSource->getControllerType()) - { - case VehicleConfigurationSource::CONTROLLER_CC: - case VehicleConfigurationSource::CONTROLLER_CC3D: - elementsToShow << "controller-cc"; - break; - case VehicleConfigurationSource::CONTROLLER_REVO: - elementsToShow << "controller-revo"; - break; - case VehicleConfigurationSource::CONTROLLER_OPLINK: - default: - elementsToShow << "controller-cc"; - break; + switch (m_configSource->getControllerType()) { + case VehicleConfigurationSource::CONTROLLER_CC: + case VehicleConfigurationSource::CONTROLLER_CC3D: + elementsToShow << "controller-cc"; + break; + case VehicleConfigurationSource::CONTROLLER_REVO: + elementsToShow << "controller-revo"; + break; + case VehicleConfigurationSource::CONTROLLER_OPLINK: + default: + elementsToShow << "controller-cc"; + break; } - switch (m_configSource->getVehicleType()) - { - case VehicleConfigurationSource::VEHICLE_MULTI: - switch (m_configSource->getVehicleSubType()) - { - case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: - elementsToShow << "tri"; - break; - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: - elementsToShow << "quad-x"; - break; - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: - elementsToShow << "quad-p"; - break; - case VehicleConfigurationSource::MULTI_ROTOR_HEXA: - elementsToShow << "hexa"; - break; - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: - elementsToShow << "hexa-y"; - break; - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: - elementsToShow << "hexa-h"; - break; - default: - break; - } + switch (m_configSource->getVehicleType()) { + case VehicleConfigurationSource::VEHICLE_MULTI: + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: + elementsToShow << "tri"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: + elementsToShow << "quad-x"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: + elementsToShow << "quad-p"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA: + elementsToShow << "hexa"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: + elementsToShow << "hexa-y"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: + elementsToShow << "hexa-h"; break; - case VehicleConfigurationSource::VEHICLE_FIXEDWING: - case VehicleConfigurationSource::VEHICLE_HELI: - case VehicleConfigurationSource::VEHICLE_SURFACE: default: break; + } + break; + case VehicleConfigurationSource::VEHICLE_FIXEDWING: + case VehicleConfigurationSource::VEHICLE_HELI: + case VehicleConfigurationSource::VEHICLE_SURFACE: + default: + break; } - switch (m_configSource->getInputType()) - { - case VehicleConfigurationSource::INPUT_PWM: - elementsToShow << "pwm" ; - break; - case VehicleConfigurationSource::INPUT_PPM: - elementsToShow << "ppm"; - break; - case VehicleConfigurationSource::INPUT_SBUS: - elementsToShow << "sbus"; - break; - case VehicleConfigurationSource::INPUT_DSMX10: - case VehicleConfigurationSource::INPUT_DSMX11: - case VehicleConfigurationSource::INPUT_DSM2: - elementsToShow << "satellite"; - break; - default: - break; + switch (m_configSource->getInputType()) { + case VehicleConfigurationSource::INPUT_PWM: + elementsToShow << "pwm"; + break; + case VehicleConfigurationSource::INPUT_PPM: + elementsToShow << "ppm"; + break; + case VehicleConfigurationSource::INPUT_SBUS: + elementsToShow << "sbus"; + break; + case VehicleConfigurationSource::INPUT_DSMX10: + case VehicleConfigurationSource::INPUT_DSMX11: + case VehicleConfigurationSource::INPUT_DSM2: + elementsToShow << "satellite"; + break; + default: + break; } setupGraphicsSceneItems(elementsToShow); @@ -161,24 +158,23 @@ void ConnectionDiagram::setupGraphicsSceneItems(QList elementsToShow) QRectF backgBounds = m_renderer->boundsOnElement("background"); foreach(QString elementId, elementsToShow) { - if(m_renderer->elementExists(elementId)) { - QGraphicsSvgItem* element = new QGraphicsSvgItem(); + if (m_renderer->elementExists(elementId)) { + QGraphicsSvgItem *element = new QGraphicsSvgItem(); element->setSharedRenderer(m_renderer); element->setElementId(elementId); element->setZValue(z++); element->setOpacity(1.0); QMatrix matrix = m_renderer->matrixForElement(elementId); - QRectF orig = matrix.mapRect(m_renderer->boundsOnElement(elementId)); - element->setPos(orig.x(),orig.y()); + QRectF orig = matrix.mapRect(m_renderer->boundsOnElement(elementId)); + element->setPos(orig.x(), orig.y()); - //QRectF orig = m_renderer->boundsOnElement(elementId); - //element->setPos(orig.x() - backgBounds.x(), orig.y() - backgBounds.y()); + // QRectF orig = m_renderer->boundsOnElement(elementId); + // element->setPos(orig.x() - backgBounds.x(), orig.y() - backgBounds.y()); m_scene->addItem(element); qDebug() << "Adding " << elementId << " to scene at " << element->pos(); - } - else { + } else { qDebug() << "Element with id: " << elementId << " not found."; } } @@ -187,11 +183,12 @@ void ConnectionDiagram::setupGraphicsSceneItems(QList elementsToShow) void ConnectionDiagram::on_saveButton_clicked() { QImage image(2200, 1100, QImage::Format_ARGB32); + image.fill(0); QPainter painter(&image); m_scene->render(&painter); QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "", tr("Images (*.png *.xpm *.jpg)")); - if(!fileName.isEmpty()) { + if (!fileName.isEmpty()) { image.save(fileName); } } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.h b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.h index e03ed6b0e..8d7a0574d 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.h @@ -40,20 +40,19 @@ namespace Ui { class ConnectionDiagram; } -class ConnectionDiagram : public QDialog -{ +class ConnectionDiagram : public QDialog { Q_OBJECT - + public: explicit ConnectionDiagram(QWidget *parent, VehicleConfigurationSource *configSource); ~ConnectionDiagram(); - + private: Ui::ConnectionDiagram *ui; VehicleConfigurationSource *m_configSource; QSvgRenderer *m_renderer; - QGraphicsSvgItem* m_background; + QGraphicsSvgItem *m_background; QGraphicsScene *m_scene; void setupGraphicsScene(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp b/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp index 1e7e48959..e75b95b2b 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp @@ -35,6 +35,7 @@ OutputCalibrationUtil::OutputCalibrationUtil(QObject *parent) : QObject(parent), m_outputChannel(-1), m_safeValue(1000) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + m_uavObjectManager = pm->getObject(); Q_ASSERT(m_uavObjectManager); } @@ -46,31 +47,30 @@ OutputCalibrationUtil::~OutputCalibrationUtil() void OutputCalibrationUtil::startChannelOutput(quint16 channel, quint16 safeValue) { - if(m_outputChannel < 0 && channel < ActuatorCommand::CHANNEL_NUMELEM) - { - //Start output... + if (m_outputChannel < 0 && channel < ActuatorCommand::CHANNEL_NUMELEM) { + // Start output... m_outputChannel = channel; - m_safeValue = safeValue; + m_safeValue = safeValue; qDebug() << "Starting output for channel " << m_outputChannel << "..."; ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager); Q_ASSERT(actuatorCommand); - UAVObject::Metadata metaData = actuatorCommand->getMetadata(); + UAVObject::Metadata metaData = actuatorCommand->getMetadata(); m_savedActuatorCommandMetadata = metaData; - //Store current data for later restore - m_savedActuatorCommandData = actuatorCommand->getData(); + // Store current data for later restore + m_savedActuatorCommandData = actuatorCommand->getData(); - //Enable actuator control from GCS... - //Store current metadata for later restore + // Enable actuator control from GCS... + // Store current metadata for later restore UAVObject::SetFlightAccess(metaData, UAVObject::ACCESS_READONLY); UAVObject::SetFlightTelemetryUpdateMode(metaData, UAVObject::UPDATEMODE_ONCHANGE); UAVObject::SetGcsTelemetryAcked(metaData, false); UAVObject::SetGcsTelemetryUpdateMode(metaData, UAVObject::UPDATEMODE_ONCHANGE); metaData.gcsTelemetryUpdatePeriod = 100; - //Apply changes + // Apply changes actuatorCommand->setMetadata(metaData); actuatorCommand->updated(); @@ -80,17 +80,16 @@ void OutputCalibrationUtil::startChannelOutput(quint16 channel, quint16 safeValu void OutputCalibrationUtil::stopChannelOutput() { - if(m_outputChannel >= 0) - { + if (m_outputChannel >= 0) { qDebug() << "Stopping output for channel " << m_outputChannel << "..."; - //Stop output... + // Stop output... setChannelOutputValue(m_safeValue); qDebug() << "Settings output for channel " << m_outputChannel << " to " << m_safeValue; // Restore metadata to what it was before ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager); Q_ASSERT(actuatorCommand); - //actuatorCommand->setData(m_savedActuatorCommandData); + // actuatorCommand->setData(m_savedActuatorCommandData); actuatorCommand->setMetadata(m_savedActuatorCommandMetadata); actuatorCommand->updated(); @@ -102,9 +101,8 @@ void OutputCalibrationUtil::stopChannelOutput() void OutputCalibrationUtil::setChannelOutputValue(quint16 value) { - if(m_outputChannel >= 0) - { - //Set output value + if (m_outputChannel >= 0) { + // Set output value qDebug() << "Setting output value for channel " << m_outputChannel << " to " << value << "."; ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager); Q_ASSERT(actuatorCommand); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.h b/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.h index dc9fc7fc8..4867e5660 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.h @@ -36,15 +36,14 @@ #include "actuatorcommand.h" -class OutputCalibrationUtil : public QObject -{ +class OutputCalibrationUtil : public QObject { Q_OBJECT public: explicit OutputCalibrationUtil(QObject *parent = 0); ~OutputCalibrationUtil(); - + signals: - + public slots: void startChannelOutput(quint16 channel, quint16 safeValue); void stopChannelOutput(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.cpp index 2e5ed82da..d7ea0e743 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.cpp @@ -27,9 +27,9 @@ #include "abstractwizardpage.h" -AbstractWizardPage::AbstractWizardPage(SetupWizard* wizard, QWidget *parent) : +AbstractWizardPage::AbstractWizardPage(SetupWizard *wizard, QWidget *parent) : QWizardPage(parent) { m_wizard = wizard; - //setFixedSize(600, 400); + // setFixedSize(600, 400); } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.h index 072d619e7..904f1771b 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.h @@ -31,8 +31,7 @@ #include #include "setupwizard.h" -class AbstractWizardPage : public QWizardPage -{ +class AbstractWizardPage : public QWizardPage { Q_OBJECT protected: explicit AbstractWizardPage(SetupWizard *wizard, QWidget *parent = 0); @@ -41,8 +40,10 @@ private: SetupWizard *m_wizard; public: - SetupWizard* getWizard() { return m_wizard; } - + SetupWizard *getWizard() + { + return m_wizard; + } }; #endif // ABSTRACTWIZARDPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp index 6542940f1..6506fde34 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp @@ -13,11 +13,11 @@ AutoUpdatePage::AutoUpdatePage(SetupWizard *wizard, QWidget *parent) : ui->setupUi(this); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Q_ASSERT(pm); - UploaderGadgetFactory *uploader = pm->getObject(); + UploaderGadgetFactory *uploader = pm->getObject(); Q_ASSERT(uploader); connect(ui->startUpdate, SIGNAL(clicked()), this, SLOT(disableButtons())); connect(ui->startUpdate, SIGNAL(clicked()), uploader, SIGNAL(autoUpdate())); - connect(uploader, SIGNAL(autoUpdateSignal(uploader::AutoUpdateStep,QVariant)), this, SLOT(updateStatus(uploader::AutoUpdateStep, QVariant))); + connect(uploader, SIGNAL(autoUpdateSignal(uploader::AutoUpdateStep, QVariant)), this, SLOT(updateStatus(uploader::AutoUpdateStep, QVariant))); } AutoUpdatePage::~AutoUpdatePage() @@ -37,8 +37,7 @@ void AutoUpdatePage::enableButtons(bool enable = false) void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant value) { - switch(status) - { + switch (status) { case uploader::WAITING_DISCONNECT: getWizard()->setWindowFlags(getWizard()->windowFlags() & ~Qt::WindowStaysOnTopHint); disableButtons(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.h index f76873a9f..a8c4a402d 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.h @@ -39,8 +39,7 @@ namespace Ui { class AutoUpdatePage; } -class AutoUpdatePage : public AbstractWizardPage -{ +class AutoUpdatePage : public AbstractWizardPage { Q_OBJECT public: @@ -48,13 +47,15 @@ public: ~AutoUpdatePage(); private slots: - void updateStatus(uploader::AutoUpdateStep ,QVariant); - void disableButtons(){ enableButtons(false); } + void updateStatus(uploader::AutoUpdateStep, QVariant); + void disableButtons() + { + enableButtons(false); + } void enableButtons(bool enable); private: Ui::AutoUpdatePage *ui; - }; #endif // AUTOUPDATEPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.cpp index 3fb5a3ca6..e5b2ca62f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.cpp @@ -33,7 +33,7 @@ BiasCalibrationPage::BiasCalibrationPage(SetupWizard *wizard, QWidget *parent) : AbstractWizardPage(wizard, parent), - ui(new Ui::BiasCalibrationPage), m_calibrationUtil(0) + ui(new Ui::BiasCalibrationPage), m_calibrationUtil(0) { ui->setupUi(this); connect(ui->levelButton, SIGNAL(clicked()), this, SLOT(performCalibration())); @@ -41,7 +41,7 @@ BiasCalibrationPage::BiasCalibrationPage(SetupWizard *wizard, QWidget *parent) : BiasCalibrationPage::~BiasCalibrationPage() { - if(m_calibrationUtil) { + if (m_calibrationUtil) { delete m_calibrationUtil; } delete ui; @@ -69,7 +69,7 @@ void BiasCalibrationPage::enableButtons(bool enable) void BiasCalibrationPage::performCalibration() { - if(!getWizard()->getConnectionManager()->isConnected()) { + if (!getWizard()->getConnectionManager()->isConnected()) { QMessageBox msgBox; msgBox.setText(tr("An OpenPilot controller must be connected to your computer to perform bias " "calculations.\nPlease connect your OpenPilot controller to your computer and try again.")); @@ -83,12 +83,11 @@ void BiasCalibrationPage::performCalibration() ui->progressLabel->setText(QString(tr("Retrieving data..."))); - if(!m_calibrationUtil) - { + if (!m_calibrationUtil) { m_calibrationUtil = new BiasCalibrationUtil(BIAS_CYCLES, BIAS_RATE); } - connect(m_calibrationUtil, SIGNAL(progress(long,long)), this, SLOT(calibrationProgress(long,long))); + connect(m_calibrationUtil, SIGNAL(progress(long, long)), this, SLOT(calibrationProgress(long, long))); connect(m_calibrationUtil, SIGNAL(done(accelGyroBias)), this, SLOT(calibrationDone(accelGyroBias))); connect(m_calibrationUtil, SIGNAL(timeout(QString)), this, SLOT(calibrationTimeout(QString))); @@ -97,10 +96,10 @@ void BiasCalibrationPage::performCalibration() void BiasCalibrationPage::calibrationProgress(long current, long total) { - if(ui->levellinProgressBar->maximum() != (int)total) { + if (ui->levellinProgressBar->maximum() != (int)total) { ui->levellinProgressBar->setMaximum((int)total); } - if(ui->levellinProgressBar->value() != (int)current) { + if (ui->levellinProgressBar->value() != (int)current) { ui->levellinProgressBar->setValue((int)current); } } @@ -125,9 +124,8 @@ void BiasCalibrationPage::calibrationTimeout(QString message) void BiasCalibrationPage::stopCalibration() { - if(m_calibrationUtil) - { - disconnect(m_calibrationUtil, SIGNAL(progress(long,long)), this, SLOT(calibrationProgress(long,long))); + if (m_calibrationUtil) { + disconnect(m_calibrationUtil, SIGNAL(progress(long, long)), this, SLOT(calibrationProgress(long, long))); disconnect(m_calibrationUtil, SIGNAL(done(accelGyroBias)), this, SLOT(calibrationDone(accelGyroBias))); disconnect(m_calibrationUtil, SIGNAL(timeout(QString)), this, SLOT(calibrationTimeout(QString))); ui->progressLabel->setText(QString(tr("Done!"))); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.h index f34c48abe..002edf51f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.h @@ -35,8 +35,7 @@ namespace Ui { class BiasCalibrationPage; } -class BiasCalibrationPage : public AbstractWizardPage -{ +class BiasCalibrationPage : public AbstractWizardPage { Q_OBJECT public: @@ -53,7 +52,7 @@ private slots: private: static const int BIAS_CYCLES = 200; - static const int BIAS_RATE = 50; + static const int BIAS_RATE = 50; Ui::BiasCalibrationPage *ui; BiasCalibrationUtil *m_calibrationUtil; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp index 595928931..5c67612dc 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp @@ -62,11 +62,10 @@ ControllerPage::~ControllerPage() void ControllerPage::initializePage() { - if(anyControllerConnected()) { + if (anyControllerConnected()) { SetupWizard::CONTROLLER_TYPE type = getControllerType(); setControllerType(type); - } - else { + } else { setControllerType(SetupWizard::CONTROLLER_UNKNOWN); } emit completeChanged(); @@ -75,7 +74,7 @@ void ControllerPage::initializePage() bool ControllerPage::isComplete() const { return m_telemtryManager->isConnected() && ui->boardTypeCombo->currentIndex() > 0 && - m_connectionManager->getCurrentDevice().getConName().startsWith("USB:", Qt::CaseInsensitive); + m_connectionManager->getCurrentDevice().getConName().startsWith("USB:", Qt::CaseInsensitive); } bool ControllerPage::validatePage() @@ -92,18 +91,22 @@ bool ControllerPage::anyControllerConnected() SetupWizard::CONTROLLER_TYPE ControllerPage::getControllerType() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectUtilManager* utilMngr = pm->getObject(); + UAVObjectUtilManager *utilMngr = pm->getObject(); int id = utilMngr->getBoardModel(); switch (id) { case 0x0301: return SetupWizard::CONTROLLER_OPLINK; + case 0x0401: return SetupWizard::CONTROLLER_CC; + case 0x0402: return SetupWizard::CONTROLLER_CC3D; + case 0x0903: return SetupWizard::CONTROLLER_REVO; + default: return SetupWizard::CONTROLLER_UNKNOWN; } @@ -118,6 +121,7 @@ void ControllerPage::setupDeviceList() void ControllerPage::setupBoardTypes() { QVariant v(0); + ui->boardTypeCombo->addItem(tr(""), SetupWizard::CONTROLLER_UNKNOWN); ui->boardTypeCombo->addItem(tr("OpenPilot CopterControl"), SetupWizard::CONTROLLER_CC); ui->boardTypeCombo->addItem(tr("OpenPilot CopterControl 3D"), SetupWizard::CONTROLLER_CC3D); @@ -127,8 +131,8 @@ void ControllerPage::setupBoardTypes() void ControllerPage::setControllerType(SetupWizard::CONTROLLER_TYPE type) { - for(int i = 0; i < ui->boardTypeCombo->count(); ++i) { - if(ui->boardTypeCombo->itemData(i) == type) { + for (int i = 0; i < ui->boardTypeCombo->count(); ++i) { + if (ui->boardTypeCombo->itemData(i) == type) { ui->boardTypeCombo->setCurrentIndex(i); break; } @@ -139,7 +143,7 @@ void ControllerPage::devicesChanged(QLinkedList devices) { // Get the selected item before the update if any QString currSelectedDeviceName = ui->deviceCombo->currentIndex() != -1 ? - ui->deviceCombo->itemData(ui->deviceCombo->currentIndex(), Qt::ToolTipRole).toString() : ""; + ui->deviceCombo->itemData(ui->deviceCombo->currentIndex(), Qt::ToolTipRole).toString() : ""; // Clear the box ui->deviceCombo->clear(); @@ -148,36 +152,35 @@ void ControllerPage::devicesChanged(QLinkedList devices) int i = 0; // Loop and fill the combo with items from connectionmanager - foreach (Core::DevListItem deviceItem, devices) - { + foreach(Core::DevListItem deviceItem, devices) { ui->deviceCombo->addItem(deviceItem.getConName()); QString deviceName = (const QString)deviceItem.getConName(); ui->deviceCombo->setItemData(ui->deviceCombo->count() - 1, deviceName, Qt::ToolTipRole); - if(!deviceName.startsWith("USB:", Qt::CaseInsensitive)) { + if (!deviceName.startsWith("USB:", Qt::CaseInsensitive)) { ui->deviceCombo->setItemData(ui->deviceCombo->count() - 1, QVariant(0), Qt::UserRole - 1); } - if(currSelectedDeviceName != "" && currSelectedDeviceName == deviceName) { + if (currSelectedDeviceName != "" && currSelectedDeviceName == deviceName) { indexOfSelectedItem = i; } i++; } // Re select the item that was selected before if any - if(indexOfSelectedItem != -1) { + if (indexOfSelectedItem != -1) { ui->deviceCombo->setCurrentIndex(indexOfSelectedItem); } - //connectionStatusChanged(); + // connectionStatusChanged(); } void ControllerPage::connectionStatusChanged() { - if(m_connectionManager->isConnected()) { + if (m_connectionManager->isConnected()) { ui->deviceCombo->setEnabled(false); ui->connectButton->setText(tr("Disconnect")); ui->boardTypeCombo->setEnabled(false); QString connectedDeviceName = m_connectionManager->getCurrentDevice().getConName(); - for(int i = 0; i < ui->deviceCombo->count(); ++i) { - if(connectedDeviceName == ui->deviceCombo->itemData(i, Qt::ToolTipRole).toString()) { + for (int i = 0; i < ui->deviceCombo->count(); ++i) { + if (connectedDeviceName == ui->deviceCombo->itemData(i, Qt::ToolTipRole).toString()) { ui->deviceCombo->setCurrentIndex(i); break; } @@ -186,8 +189,7 @@ void ControllerPage::connectionStatusChanged() SetupWizard::CONTROLLER_TYPE type = getControllerType(); setControllerType(type); qDebug() << "Connection status changed: Connected, controller type: " << getControllerType(); - } - else { + } else { ui->deviceCombo->setEnabled(true); ui->connectButton->setText(tr("Connect")); ui->boardTypeCombo->setEnabled(false); @@ -200,10 +202,9 @@ void ControllerPage::connectionStatusChanged() void ControllerPage::connectDisconnect() { - if(m_connectionManager->isConnected()) { + if (m_connectionManager->isConnected()) { m_connectionManager->disconnectDevice(); - } - else { + } else { m_connectionManager->connectDevice(m_connectionManager->findDevice(ui->deviceCombo->itemData(ui->deviceCombo->currentIndex(), Qt::ToolTipRole).toString())); } emit completeChanged(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h index 9370f4cd4..5bae29d6b 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h @@ -38,17 +38,16 @@ namespace Ui { class ControllerPage; } -class ControllerPage : public AbstractWizardPage -{ +class ControllerPage : public AbstractWizardPage { Q_OBJECT - + public: explicit ControllerPage(SetupWizard *wizard, QWidget *parent = 0); ~ControllerPage(); void initializePage(); bool isComplete() const; bool validatePage(); - + private: Ui::ControllerPage *ui; bool anyControllerConnected(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.cpp index 34228c401..48b08fb75 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.cpp @@ -48,14 +48,13 @@ EndPage::~EndPage() void EndPage::openInputWizard() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - ConfigGadgetFactory* configGadgetFactory = pm->getObject(); + ConfigGadgetFactory *configGadgetFactory = pm->getObject(); - if(configGadgetFactory) { - //Core::ModeManager::instance()->activateModeByWorkspaceName("Configuration"); + if (configGadgetFactory) { + // Core::ModeManager::instance()->activateModeByWorkspaceName("Configuration"); getWizard()->close(); configGadgetFactory->startInputWizard(); - } - else { + } else { QMessageBox msgBox; msgBox.setText(tr("Unable to open Input Wizard since the Config Plugin is not\nloaded in the current workspace.")); msgBox.setStandardButtons(QMessageBox::Ok); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.h index dfbad7942..92bfc194f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.h @@ -34,14 +34,13 @@ namespace Ui { class EndPage; } -class EndPage : public AbstractWizardPage -{ +class EndPage : public AbstractWizardPage { Q_OBJECT - + public: explicit EndPage(SetupWizard *wizard, QWidget *parent = 0); ~EndPage(); - + private slots: void openInputWizard(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h index 697721a9e..6a99ef2fe 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h @@ -34,14 +34,13 @@ namespace Ui { class FixedWingPage; } -class FixedWingPage : public AbstractWizardPage -{ +class FixedWingPage : public AbstractWizardPage { Q_OBJECT - + public: explicit FixedWingPage(SetupWizard *wizard, QWidget *parent = 0); ~FixedWingPage(); - + private: Ui::FixedWingPage *ui; }; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.h index 2b2b5b894..94727da41 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.h @@ -34,14 +34,13 @@ namespace Ui { class HeliPage; } -class HeliPage : public AbstractWizardPage -{ +class HeliPage : public AbstractWizardPage { Q_OBJECT - + public: explicit HeliPage(SetupWizard *wizard, QWidget *parent = 0); ~HeliPage(); - + private: Ui::HeliPage *ui; }; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp index bfa83ba7e..5515a31dd 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp @@ -33,7 +33,7 @@ #include "hwsettings.h" InputPage::InputPage(SetupWizard *wizard, QWidget *parent) : - AbstractWizardPage(wizard, parent), + AbstractWizardPage(wizard, parent), ui(new Ui::InputPage) { @@ -47,19 +47,15 @@ InputPage::~InputPage() bool InputPage::validatePage() { - if(ui->pwmButton->isChecked()) { + if (ui->pwmButton->isChecked()) { getWizard()->setInputType(SetupWizard::INPUT_PWM); - } - else if(ui->ppmButton->isChecked()) { + } else if (ui->ppmButton->isChecked()) { getWizard()->setInputType(SetupWizard::INPUT_PPM); - } - else if(ui->sbusButton->isChecked()) { + } else if (ui->sbusButton->isChecked()) { getWizard()->setInputType(SetupWizard::INPUT_SBUS); - } - else if(ui->spectrumButton->isChecked()) { + } else if (ui->spectrumButton->isChecked()) { getWizard()->setInputType(SetupWizard::INPUT_DSM2); - } - else { + } else { getWizard()->setInputType(SetupWizard::INPUT_PWM); } getWizard()->setRestartNeeded(getWizard()->isRestartNeeded() || restartNeeded(getWizard()->getInputType())); @@ -71,45 +67,52 @@ bool InputPage::restartNeeded(VehicleConfigurationSource::INPUT_TYPE selectedTyp { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *uavoManager = pm->getObject(); + Q_ASSERT(uavoManager); - HwSettings* hwSettings = HwSettings::GetInstance(uavoManager); + HwSettings *hwSettings = HwSettings::GetInstance(uavoManager); HwSettings::DataFields data = hwSettings->getData(); - switch(getWizard()->getControllerType()) { - case SetupWizard::CONTROLLER_CC: - case SetupWizard::CONTROLLER_CC3D: - { - switch(selectedType) - { - case VehicleConfigurationSource::INPUT_PWM: - return data.CC_RcvrPort != HwSettings::CC_RCVRPORT_PWM; - case VehicleConfigurationSource::INPUT_PPM: - return data.CC_RcvrPort != HwSettings::CC_RCVRPORT_PPM; - case VehicleConfigurationSource::INPUT_SBUS: - return data.CC_MainPort != HwSettings::CC_MAINPORT_SBUS; - case VehicleConfigurationSource::INPUT_DSM2: - // TODO: Handle all of the DSM types ?? Which is most common? - return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM2; - default: return true; - } - break; - } - case SetupWizard::CONTROLLER_REVO: - { - switch(selectedType) - { - case VehicleConfigurationSource::INPUT_PWM: - return data.RM_RcvrPort != HwSettings::CC_RCVRPORT_PWM; - case VehicleConfigurationSource::INPUT_PPM: - return data.RM_RcvrPort != HwSettings::CC_RCVRPORT_PPM; - case VehicleConfigurationSource::INPUT_SBUS: - return data.RM_MainPort != HwSettings::CC_MAINPORT_SBUS; - case VehicleConfigurationSource::INPUT_DSM2: - // TODO: Handle all of the DSM types ?? Which is most common? - return data.RM_MainPort != HwSettings::CC_MAINPORT_DSM2; - default: return true; - } - break; - } + switch (getWizard()->getControllerType()) { + case SetupWizard::CONTROLLER_CC: + case SetupWizard::CONTROLLER_CC3D: + { + switch (selectedType) { + case VehicleConfigurationSource::INPUT_PWM: + return data.CC_RcvrPort != HwSettings::CC_RCVRPORT_PWM; + + case VehicleConfigurationSource::INPUT_PPM: + return data.CC_RcvrPort != HwSettings::CC_RCVRPORT_PPM; + + case VehicleConfigurationSource::INPUT_SBUS: + return data.CC_MainPort != HwSettings::CC_MAINPORT_SBUS; + + case VehicleConfigurationSource::INPUT_DSM2: + // TODO: Handle all of the DSM types ?? Which is most common? + return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM2; + default: return true; + } + break; + } + case SetupWizard::CONTROLLER_REVO: + { + switch (selectedType) { + case VehicleConfigurationSource::INPUT_PWM: + return data.RM_RcvrPort != HwSettings::CC_RCVRPORT_PWM; + + case VehicleConfigurationSource::INPUT_PPM: + return data.RM_RcvrPort != HwSettings::CC_RCVRPORT_PPM; + + case VehicleConfigurationSource::INPUT_SBUS: + return data.RM_MainPort != HwSettings::CC_MAINPORT_SBUS; + + case VehicleConfigurationSource::INPUT_DSM2: + // TODO: Handle all of the DSM types ?? Which is most common? + return data.RM_MainPort != HwSettings::CC_MAINPORT_DSM2; + + default: return true; + } + break; + } + default: return true; } } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.h index dde50b6e6..7b0221527 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.h @@ -34,8 +34,7 @@ namespace Ui { class InputPage; } -class InputPage : public AbstractWizardPage -{ +class InputPage : public AbstractWizardPage { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp index 79ea7131c..1c47894cc 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp @@ -33,7 +33,7 @@ MultiPage::MultiPage(SetupWizard *wizard, QWidget *parent) : AbstractWizardPage(wizard, parent), ui(new Ui::MultiPage) { - ui->setupUi(this); + ui->setupUi(this); QSvgRenderer *renderer = new QSvgRenderer(); renderer->load(QString(":/configgadget/images/multirotor-shapes.svg")); @@ -65,7 +65,8 @@ void MultiPage::initializePage() bool MultiPage::validatePage() { - SetupWizard::VEHICLE_SUB_TYPE type = (SetupWizard::VEHICLE_SUB_TYPE) ui->typeCombo->itemData(ui->typeCombo->currentIndex()).toInt(); + SetupWizard::VEHICLE_SUB_TYPE type = (SetupWizard::VEHICLE_SUB_TYPE)ui->typeCombo->itemData(ui->typeCombo->currentIndex()).toInt(); + getWizard()->setVehicleSubType(type); return true; } @@ -73,7 +74,7 @@ bool MultiPage::validatePage() void MultiPage::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - if(m_multiPic) { + if (m_multiPic) { ui->typeGraphicsView->setSceneRect(m_multiPic->boundingRect()); ui->typeGraphicsView->fitInView(m_multiPic, Qt::KeepAspectRatio); } @@ -108,71 +109,71 @@ void MultiPage::setupMultiTypesCombo() // Fredrik Arvidsson(m_thread) 2012-08-26 Disable Octos until further notice /* - ui->typeCombo->addItem(tr("Octocopter"), SetupWizard::MULTI_ROTOR_OCTO); - m_descriptions << tr("Octocopter"); + ui->typeCombo->addItem(tr("Octocopter"), SetupWizard::MULTI_ROTOR_OCTO); + m_descriptions << tr("Octocopter"); - ui->typeCombo->addItem(tr("Octocopter Coax X"), SetupWizard::MULTI_ROTOR_OCTO_COAX_X); - m_descriptions << tr("Octocopter Coax X"); + ui->typeCombo->addItem(tr("Octocopter Coax X"), SetupWizard::MULTI_ROTOR_OCTO_COAX_X); + m_descriptions << tr("Octocopter Coax X"); - ui->typeCombo->addItem(tr("Octocopter Coax +"), SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS); - m_descriptions << tr("Octocopter Coax +"); + ui->typeCombo->addItem(tr("Octocopter Coax +"), SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS); + m_descriptions << tr("Octocopter Coax +"); - ui->typeCombo->addItem(tr("Octocopter V"), SetupWizard::MULTI_ROTOR_OCTO_V); - m_descriptions << tr("Octocopter V"); - */ + ui->typeCombo->addItem(tr("Octocopter V"), SetupWizard::MULTI_ROTOR_OCTO_V); + m_descriptions << tr("Octocopter V"); + */ } void MultiPage::updateAvailableTypes() { /* - QVariant enable = (getWizard()->getInputType() == SetupWizard::INPUT_PWM) ? QVariant(0) : QVariant(1 | 32); - ui->typeCombo->model()->setData(ui->typeCombo->model()->index(6, 0), enable, Qt::UserRole - 1); - ui->typeCombo->model()->setData(ui->typeCombo->model()->index(7, 0), enable, Qt::UserRole - 1); - ui->typeCombo->model()->setData(ui->typeCombo->model()->index(8, 0), enable, Qt::UserRole - 1); - ui->typeCombo->model()->setData(ui->typeCombo->model()->index(9, 0), enable, Qt::UserRole - 1); - */ + QVariant enable = (getWizard()->getInputType() == SetupWizard::INPUT_PWM) ? QVariant(0) : QVariant(1 | 32); + ui->typeCombo->model()->setData(ui->typeCombo->model()->index(6, 0), enable, Qt::UserRole - 1); + ui->typeCombo->model()->setData(ui->typeCombo->model()->index(7, 0), enable, Qt::UserRole - 1); + ui->typeCombo->model()->setData(ui->typeCombo->model()->index(8, 0), enable, Qt::UserRole - 1); + ui->typeCombo->model()->setData(ui->typeCombo->model()->index(9, 0), enable, Qt::UserRole - 1); + */ } void MultiPage::updateImageAndDescription() { - SetupWizard::VEHICLE_SUB_TYPE type = (SetupWizard::VEHICLE_SUB_TYPE) ui->typeCombo->itemData(ui->typeCombo->currentIndex()).toInt(); - QString elementId = ""; + SetupWizard::VEHICLE_SUB_TYPE type = (SetupWizard::VEHICLE_SUB_TYPE)ui->typeCombo->itemData(ui->typeCombo->currentIndex()).toInt(); + QString elementId = ""; QString description = m_descriptions.at(ui->typeCombo->currentIndex()); - switch(type) - { - case SetupWizard::MULTI_ROTOR_TRI_Y: - elementId = "tri"; - break; - case SetupWizard::MULTI_ROTOR_QUAD_X: - elementId = "quad-x"; - break; - case SetupWizard::MULTI_ROTOR_QUAD_PLUS: - elementId = "quad-plus"; - break; - case SetupWizard::MULTI_ROTOR_HEXA: - elementId = "quad-hexa"; - break; - case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: - elementId = "hexa-coax"; - break; - case SetupWizard::MULTI_ROTOR_HEXA_H: - elementId = "quad-hexa-H"; - break; - case SetupWizard::MULTI_ROTOR_OCTO: - elementId = "quad-octo"; - break; - case SetupWizard::MULTI_ROTOR_OCTO_COAX_X: - elementId = "octo-coax-X"; - break; - case SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS: - elementId = "octo-coax-P"; - break; - case SetupWizard::MULTI_ROTOR_OCTO_V: - elementId = "quad-octo-v"; - break; - default: - elementId = ""; - break; + + switch (type) { + case SetupWizard::MULTI_ROTOR_TRI_Y: + elementId = "tri"; + break; + case SetupWizard::MULTI_ROTOR_QUAD_X: + elementId = "quad-x"; + break; + case SetupWizard::MULTI_ROTOR_QUAD_PLUS: + elementId = "quad-plus"; + break; + case SetupWizard::MULTI_ROTOR_HEXA: + elementId = "quad-hexa"; + break; + case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: + elementId = "hexa-coax"; + break; + case SetupWizard::MULTI_ROTOR_HEXA_H: + elementId = "quad-hexa-H"; + break; + case SetupWizard::MULTI_ROTOR_OCTO: + elementId = "quad-octo"; + break; + case SetupWizard::MULTI_ROTOR_OCTO_COAX_X: + elementId = "octo-coax-X"; + break; + case SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS: + elementId = "octo-coax-P"; + break; + case SetupWizard::MULTI_ROTOR_OCTO_V: + elementId = "quad-octo-v"; + break; + default: + elementId = ""; + break; } m_multiPic->setElementId(elementId); ui->typeGraphicsView->setSceneRect(m_multiPic->boundingRect()); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.h index 5614ea6cb..ced899ebe 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.h @@ -38,10 +38,9 @@ namespace Ui { class MultiPage; } -class MultiPage : public AbstractWizardPage -{ +class MultiPage : public AbstractWizardPage { Q_OBJECT - + public: explicit MultiPage(SetupWizard *wizard, QWidget *parent = 0); ~MultiPage(); @@ -51,7 +50,7 @@ public: protected: void resizeEvent(QResizeEvent *event); - + private: Ui::MultiPage *ui; void setupMultiTypesCombo(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.h index 4f323d9fc..93d4f1231 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.h @@ -34,14 +34,13 @@ namespace Ui { class NotYetImplementedPage; } -class NotYetImplementedPage : public AbstractWizardPage -{ +class NotYetImplementedPage : public AbstractWizardPage { Q_OBJECT - + public: explicit NotYetImplementedPage(SetupWizard *wizard, QWidget *parent = 0); ~NotYetImplementedPage(); - + private: Ui::NotYetImplementedPage *ui; }; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index 70a2a9fc5..c1eebfa1d 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -38,9 +38,8 @@ OutputCalibrationPage::OutputCalibrationPage(SetupWizard *wizard, QWidget *paren m_vehicleRenderer = new QSvgRenderer(); if (QFile::exists(QString(":/setupwizard/resources/multirotor-shapes.svg")) && - m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) && - m_vehicleRenderer->isValid()) - { + m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->isValid()) { m_vehicleScene = new QGraphicsScene(this); ui->vehicleView->setScene(m_vehicleScene); } @@ -48,7 +47,7 @@ OutputCalibrationPage::OutputCalibrationPage(SetupWizard *wizard, QWidget *paren OutputCalibrationPage::~OutputCalibrationPage() { - if(m_calibrationUtil) { + if (m_calibrationUtil) { delete m_calibrationUtil; m_calibrationUtil = 0; } @@ -64,56 +63,55 @@ void OutputCalibrationPage::setupVehicle() m_channelIndex.clear(); m_currentWizardIndex = 0; m_vehicleScene->clear(); - switch(getWizard()->getVehicleSubType()) - { - case SetupWizard::MULTI_ROTOR_TRI_Y: - m_wizardIndexes << 0 << 1 << 1 << 1 << 2 << 3 << 4; - m_vehicleElementIds << "tri" << "tri-frame" << "tri-m1" << "tri-m2" << "tri-m3" << "tri-s1"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 4 << 4; - m_channelIndex << 0 << 0 << 1 << 2 << 3 << 3 << 3; - m_actuatorSettings[3].channelMin = 1500; - m_actuatorSettings[3].channelNeutral = 1500; - m_actuatorSettings[3].channelMax = 1500; - getWizard()->setActuatorSettings(m_actuatorSettings); - break; - case SetupWizard::MULTI_ROTOR_QUAD_X: - m_wizardIndexes << 0 << 1 << 1 << 1 << 1; - m_vehicleElementIds << "quad-x" << "quad-x-frame" << "quad-x-m1" << "quad-x-m2" << "quad-x-m3" << "quad-x-m4"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4; - m_channelIndex << 0 << 0 << 1 << 2 << 3; - break; - case SetupWizard::MULTI_ROTOR_QUAD_PLUS: - m_wizardIndexes << 0 << 1 << 1 << 1 << 1; - m_vehicleElementIds << "quad-p" << "quad-p-frame" << "quad-p-m1" << "quad-p-m2" << "quad-p-m3" << "quad-p-m4"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4; - m_channelIndex << 0 << 0 << 1 << 2 << 3; - break; - case SetupWizard::MULTI_ROTOR_HEXA: - m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; - m_vehicleElementIds << "hexa" << "hexa-frame" << "hexa-m1" << "hexa-m2" << "hexa-m3" << "hexa-m4" << "hexa-m5" << "hexa-m6"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; - m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; - break; - case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: - m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; - m_vehicleElementIds << "hexa-y6" << "hexa-y6-frame" << "hexa-y6-m2" << "hexa-y6-m1" << "hexa-y6-m4" << "hexa-y6-m3" << "hexa-y6-m6" << "hexa-y6-m5"; - m_vehicleHighlightElementIndexes << 0 << 2 << 1 << 4 << 3 << 6 << 5; - m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; - break; - case SetupWizard::MULTI_ROTOR_HEXA_H: - m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; - m_vehicleElementIds << "hexa-h" << "hexa-h-frame" << "hexa-h-m1" << "hexa-h-m2" << "hexa-h-m3" << "hexa-h-m4" << "hexa-h-m5" << "hexa-h-m6"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; - m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; - break; - default: - break; + switch (getWizard()->getVehicleSubType()) { + case SetupWizard::MULTI_ROTOR_TRI_Y: + m_wizardIndexes << 0 << 1 << 1 << 1 << 2 << 3 << 4; + m_vehicleElementIds << "tri" << "tri-frame" << "tri-m1" << "tri-m2" << "tri-m3" << "tri-s1"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 4 << 4; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 3 << 3; + m_actuatorSettings[3].channelMin = 1500; + m_actuatorSettings[3].channelNeutral = 1500; + m_actuatorSettings[3].channelMax = 1500; + getWizard()->setActuatorSettings(m_actuatorSettings); + break; + case SetupWizard::MULTI_ROTOR_QUAD_X: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "quad-x" << "quad-x-frame" << "quad-x-m1" << "quad-x-m2" << "quad-x-m3" << "quad-x-m4"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4; + m_channelIndex << 0 << 0 << 1 << 2 << 3; + break; + case SetupWizard::MULTI_ROTOR_QUAD_PLUS: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "quad-p" << "quad-p-frame" << "quad-p-m1" << "quad-p-m2" << "quad-p-m3" << "quad-p-m4"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4; + m_channelIndex << 0 << 0 << 1 << 2 << 3; + break; + case SetupWizard::MULTI_ROTOR_HEXA: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "hexa" << "hexa-frame" << "hexa-m1" << "hexa-m2" << "hexa-m3" << "hexa-m4" << "hexa-m5" << "hexa-m6"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + break; + case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "hexa-y6" << "hexa-y6-frame" << "hexa-y6-m2" << "hexa-y6-m1" << "hexa-y6-m4" << "hexa-y6-m3" << "hexa-y6-m6" << "hexa-y6-m5"; + m_vehicleHighlightElementIndexes << 0 << 2 << 1 << 4 << 3 << 6 << 5; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + break; + case SetupWizard::MULTI_ROTOR_HEXA_H: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "hexa-h" << "hexa-h-frame" << "hexa-h-m1" << "hexa-h-m2" << "hexa-h-m3" << "hexa-h-m4" << "hexa-h-m5" << "hexa-h-m6"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + break; + default: + break; } VehicleConfigurationHelper helper(getWizard()); helper.setupVehicle(false); - if(m_calibrationUtil) { + if (m_calibrationUtil) { delete m_calibrationUtil; m_calibrationUtil = 0; } @@ -134,8 +132,7 @@ void OutputCalibrationPage::setupVehicleItems() QRectF parentBounds = m_vehicleRenderer->boundsOnElement(m_vehicleElementIds[0]); - for(int i = 1; i < m_vehicleElementIds.size(); i++) - { + for (int i = 1; i < m_vehicleElementIds.size(); i++) { QGraphicsSvgItem *item = new QGraphicsSvgItem(); item->setSharedRenderer(m_vehicleRenderer); item->setElementId(m_vehicleElementIds[i]); @@ -160,9 +157,10 @@ void OutputCalibrationPage::setupVehicleHighlightedPart() { qreal dimOpaque = m_currentWizardIndex == 0 ? 1.0 : 0.3; qreal highlightOpaque = 1.0; - int highlightedIndex = m_vehicleHighlightElementIndexes[m_currentWizardIndex]; - for(int i = 0; i < m_vehicleItems.size(); i++) { - QGraphicsSvgItem* item = m_vehicleItems[i]; + int highlightedIndex = m_vehicleHighlightElementIndexes[m_currentWizardIndex]; + + for (int i = 0; i < m_vehicleItems.size(); i++) { + QGraphicsSvgItem *item = m_vehicleItems[i]; item->setOpacity((highlightedIndex == i) ? highlightOpaque : dimOpaque); } } @@ -180,18 +178,15 @@ void OutputCalibrationPage::setWizardPage() int currentChannel = getCurrentChannel(); qDebug() << "Current channel: " << currentChannel; - if(currentChannel >= 0) { - if(currentPageIndex == 1) { + if (currentChannel >= 0) { + if (currentPageIndex == 1) { ui->motorNeutralSlider->setValue(m_actuatorSettings[currentChannel].channelNeutral); - } - else if(currentPageIndex == 2) { + } else if (currentPageIndex == 2) { ui->servoCenterSlider->setValue(m_actuatorSettings[currentChannel].channelNeutral); - } - else if(currentPageIndex == 3) { + } else if (currentPageIndex == 3) { ui->servoMinAngleSlider->setMaximum(m_actuatorSettings[currentChannel].channelNeutral); ui->servoMinAngleSlider->setValue(m_actuatorSettings[currentChannel].channelMin); - } - else if(currentPageIndex == 4) { + } else if (currentPageIndex == 4) { ui->servoMaxAngleSlider->setMinimum(m_actuatorSettings[currentChannel].channelNeutral); ui->servoMaxAngleSlider->setValue(m_actuatorSettings[currentChannel].channelMax); } @@ -201,7 +196,7 @@ void OutputCalibrationPage::setWizardPage() void OutputCalibrationPage::initializePage() { - if(m_vehicleScene) { + if (m_vehicleScene) { setupVehicle(); startWizard(); } @@ -209,7 +204,7 @@ void OutputCalibrationPage::initializePage() bool OutputCalibrationPage::validatePage() { - if(isFinished()) { + if (isFinished()) { getWizard()->setActuatorSettings(m_actuatorSettings); return true; } else { @@ -222,7 +217,7 @@ bool OutputCalibrationPage::validatePage() void OutputCalibrationPage::showEvent(QShowEvent *event) { Q_UNUSED(event); - if(m_vehicleBoundsItem) { + if (m_vehicleBoundsItem) { ui->vehicleView->setSceneRect(m_vehicleBoundsItem->boundingRect()); ui->vehicleView->fitInView(m_vehicleBoundsItem, Qt::KeepAspectRatio); } @@ -231,7 +226,7 @@ void OutputCalibrationPage::showEvent(QShowEvent *event) void OutputCalibrationPage::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - if(m_vehicleBoundsItem) { + if (m_vehicleBoundsItem) { ui->vehicleView->setSceneRect(m_vehicleBoundsItem->boundingRect()); ui->vehicleView->fitInView(m_vehicleBoundsItem, Qt::KeepAspectRatio); } @@ -239,20 +234,17 @@ void OutputCalibrationPage::resizeEvent(QResizeEvent *event) void OutputCalibrationPage::customBackClicked() { - if(m_currentWizardIndex > 0) - { + if (m_currentWizardIndex > 0) { m_currentWizardIndex--; setWizardPage(); - } - else - { + } else { getWizard()->back(); } } quint16 OutputCalibrationPage::getCurrentChannel() { - return m_channelIndex[m_currentWizardIndex]; + return m_channelIndex[m_currentWizardIndex]; } void OutputCalibrationPage::enableButtons(bool enable) @@ -271,19 +263,18 @@ void OutputCalibrationPage::on_motorNeutralButton_toggled(bool checked) onStartButtonToggle(ui->motorNeutralButton, channel, m_actuatorSettings[channel].channelNeutral, 1000, ui->motorNeutralSlider); } -void OutputCalibrationPage::onStartButtonToggle(QAbstractButton *button, quint16 channel, quint16 value, quint16 safeValue, QSlider *slider) { - if(button->isChecked()) { - if(checkAlarms()) { +void OutputCalibrationPage::onStartButtonToggle(QAbstractButton *button, quint16 channel, quint16 value, quint16 safeValue, QSlider *slider) +{ + if (button->isChecked()) { + if (checkAlarms()) { enableButtons(false); m_calibrationUtil->startChannelOutput(channel, safeValue); slider->setValue(value); m_calibrationUtil->setChannelOutputValue(value); - } - else { + } else { button->setChecked(false); } - } - else { + } else { m_calibrationUtil->stopChannelOutput(); enableButtons(true); } @@ -294,12 +285,13 @@ bool OutputCalibrationPage::checkAlarms() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *uavObjectManager = pm->getObject(); + Q_ASSERT(uavObjectManager); - SystemAlarms * systemAlarms = SystemAlarms::GetInstance(uavObjectManager); + SystemAlarms *systemAlarms = SystemAlarms::GetInstance(uavObjectManager); Q_ASSERT(systemAlarms); SystemAlarms::DataFields data = systemAlarms->getData(); - if(data.Alarm[SystemAlarms::ALARM_ACTUATOR] != SystemAlarms::ALARM_OK) { + if (data.Alarm[SystemAlarms::ALARM_ACTUATOR] != SystemAlarms::ALARM_OK) { QMessageBox mbox(this); mbox.setText(QString(tr("The actuator module is in an error state.\n\n" "Please make sure the correct firmware version is used then " @@ -323,6 +315,7 @@ bool OutputCalibrationPage::checkAlarms() void OutputCalibrationPage::debugLogChannelValues() { quint16 channel = getCurrentChannel(); + qDebug() << "ChannelMin : " << m_actuatorSettings[channel].channelMin; qDebug() << "ChannelNeutral: " << m_actuatorSettings[channel].channelNeutral; qDebug() << "ChannelMax : " << m_actuatorSettings[channel].channelMax; @@ -331,7 +324,7 @@ void OutputCalibrationPage::debugLogChannelValues() void OutputCalibrationPage::on_motorNeutralSlider_valueChanged(int value) { Q_UNUSED(value); - if(ui->motorNeutralButton->isChecked()) { + if (ui->motorNeutralButton->isChecked()) { quint16 value = ui->motorNeutralSlider->value(); m_calibrationUtil->setChannelOutputValue(value); m_actuatorSettings[getCurrentChannel()].channelNeutral = value; @@ -342,7 +335,7 @@ void OutputCalibrationPage::on_motorNeutralSlider_valueChanged(int value) void OutputCalibrationPage::on_servoCenterButton_toggled(bool checked) { ui->servoCenterButton->setText(checked ? tr("Stop") : tr("Start")); - quint16 channel = getCurrentChannel(); + quint16 channel = getCurrentChannel(); quint16 safeValue = m_actuatorSettings[channel].channelNeutral; onStartButtonToggle(ui->servoCenterButton, channel, safeValue, safeValue, ui->servoCenterSlider); } @@ -350,17 +343,17 @@ void OutputCalibrationPage::on_servoCenterButton_toggled(bool checked) void OutputCalibrationPage::on_servoCenterSlider_valueChanged(int position) { Q_UNUSED(position); - if(ui->servoCenterButton->isChecked()) { - quint16 value = ui->servoCenterSlider->value(); + if (ui->servoCenterButton->isChecked()) { + quint16 value = ui->servoCenterSlider->value(); m_calibrationUtil->setChannelOutputValue(value); quint16 channel = getCurrentChannel(); m_actuatorSettings[channel].channelNeutral = value; - //Adjust min and max - if(value < m_actuatorSettings[channel].channelMin) { + // Adjust min and max + if (value < m_actuatorSettings[channel].channelMin) { m_actuatorSettings[channel].channelMin = value; } - if(value > m_actuatorSettings[channel].channelMax) { + if (value > m_actuatorSettings[channel].channelMax) { m_actuatorSettings[channel].channelMax = value; } debugLogChannelValues(); @@ -370,7 +363,7 @@ void OutputCalibrationPage::on_servoCenterSlider_valueChanged(int position) void OutputCalibrationPage::on_servoMinAngleButton_toggled(bool checked) { ui->servoMinAngleButton->setText(checked ? tr("Stop") : tr("Start")); - quint16 channel = getCurrentChannel(); + quint16 channel = getCurrentChannel(); quint16 safeValue = m_actuatorSettings[channel].channelNeutral; onStartButtonToggle(ui->servoMinAngleButton, channel, m_actuatorSettings[channel].channelMin, safeValue, ui->servoMinAngleSlider); } @@ -378,7 +371,7 @@ void OutputCalibrationPage::on_servoMinAngleButton_toggled(bool checked) void OutputCalibrationPage::on_servoMinAngleSlider_valueChanged(int position) { Q_UNUSED(position); - if(ui->servoMinAngleButton->isChecked()) { + if (ui->servoMinAngleButton->isChecked()) { quint16 value = ui->servoMinAngleSlider->value(); m_calibrationUtil->setChannelOutputValue(value); m_actuatorSettings[getCurrentChannel()].channelMin = value; @@ -389,7 +382,7 @@ void OutputCalibrationPage::on_servoMinAngleSlider_valueChanged(int position) void OutputCalibrationPage::on_servoMaxAngleButton_toggled(bool checked) { ui->servoMaxAngleButton->setText(checked ? tr("Stop") : tr("Start")); - quint16 channel = getCurrentChannel(); + quint16 channel = getCurrentChannel(); quint16 safeValue = m_actuatorSettings[channel].channelNeutral; onStartButtonToggle(ui->servoMaxAngleButton, channel, m_actuatorSettings[channel].channelMax, safeValue, ui->servoMaxAngleSlider); } @@ -397,7 +390,7 @@ void OutputCalibrationPage::on_servoMaxAngleButton_toggled(bool checked) void OutputCalibrationPage::on_servoMaxAngleSlider_valueChanged(int position) { Q_UNUSED(position); - if(ui->servoMaxAngleButton->isChecked()) { + if (ui->servoMaxAngleButton->isChecked()) { quint16 value = ui->servoMaxAngleSlider->value(); m_calibrationUtil->setChannelOutputValue(value); m_actuatorSettings[getCurrentChannel()].channelMax = value; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h index 1a3302e53..50791517a 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h @@ -38,22 +38,24 @@ namespace Ui { class OutputCalibrationPage; } -class OutputCalibrationPage : public AbstractWizardPage -{ +class OutputCalibrationPage : public AbstractWizardPage { Q_OBJECT - + public: explicit OutputCalibrationPage(SetupWizard *wizard, QWidget *parent = 0); ~OutputCalibrationPage(); void initializePage(); bool validatePage(); - bool isFinished() { return m_currentWizardIndex >= m_wizardIndexes.size() - 1; } + bool isFinished() + { + return m_currentWizardIndex >= m_wizardIndexes.size() - 1; + } protected: void showEvent(QShowEvent *event); void resizeEvent(QResizeEvent *event); - + public slots: void customBackClicked(); @@ -90,7 +92,7 @@ private: qint16 m_currentWizardIndex; QList m_vehicleElementIds; - QList m_vehicleItems; + QList m_vehicleItems; QList m_vehicleHighlightElementIndexes; QList m_channelIndex; QList m_wizardIndexes; @@ -98,7 +100,6 @@ private: QList m_actuatorSettings; OutputCalibrationUtil *m_calibrationUtil; - }; #endif // OUTPUTCALIBRATIONPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp index d9903af57..b57237f3f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp @@ -30,7 +30,7 @@ #include "setupwizard.h" OutputPage::OutputPage(SetupWizard *wizard, QWidget *parent) : - AbstractWizardPage(wizard, parent), + AbstractWizardPage(wizard, parent), ui(new Ui::OutputPage) { @@ -44,10 +44,9 @@ OutputPage::~OutputPage() bool OutputPage::validatePage() { - if(ui->rapidESCButton->isChecked()) { + if (ui->rapidESCButton->isChecked()) { getWizard()->setESCType(SetupWizard::ESC_RAPID); - } - else { + } else { getWizard()->setESCType(SetupWizard::ESC_LEGACY); } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.h index 3a7130238..40ba78ac3 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.h @@ -34,15 +34,14 @@ namespace Ui { class OutputPage; } -class OutputPage : public AbstractWizardPage -{ +class OutputPage : public AbstractWizardPage { Q_OBJECT - + public: explicit OutputPage(SetupWizard *wizard, QWidget *parent = 0); ~OutputPage(); bool validatePage(); - + private: Ui::OutputPage *ui; }; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.cpp index 43153380b..4ca0c9c74 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.cpp @@ -46,7 +46,7 @@ RebootPage::~RebootPage() void RebootPage::initializePage() { - if(!m_timer.isActive()) { + if (!m_timer.isActive()) { connect(&m_timer, SIGNAL(timeout()), this, SLOT(toggleLabel())); m_timer.setInterval(500); m_timer.setSingleShot(false); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.h index c3d32fca2..786a77253 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.h @@ -34,17 +34,16 @@ namespace Ui { class RebootPage; } -class RebootPage : public AbstractWizardPage -{ +class RebootPage : public AbstractWizardPage { Q_OBJECT - + public: explicit RebootPage(SetupWizard *wizard, QWidget *parent = 0); ~RebootPage(); void initializePage(); bool validatePage(); - + private: Ui::RebootPage *ui; QTimer m_timer; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/revocalibrationpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/revocalibrationpage.h index 9939d59ea..c9134ea8f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/revocalibrationpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/revocalibrationpage.h @@ -34,14 +34,13 @@ namespace Ui { class RevoCalibrationPage; } -class RevoCalibrationPage : public AbstractWizardPage -{ +class RevoCalibrationPage : public AbstractWizardPage { Q_OBJECT - + public: explicit RevoCalibrationPage(SetupWizard *wizard, QWidget *parent = 0); ~RevoCalibrationPage(); - + private: Ui::RevoCalibrationPage *ui; }; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.cpp index 3069d20e6..22084b277 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.cpp @@ -32,7 +32,7 @@ #include "vehicleconfigurationhelper.h" SavePage::SavePage(SetupWizard *wizard, QWidget *parent) : - AbstractWizardPage(wizard, parent), + AbstractWizardPage(wizard, parent), ui(new Ui::SavePage), m_successfulWrite(false) { ui->setupUi(this); @@ -45,7 +45,7 @@ SavePage::~SavePage() } bool SavePage::validatePage() -{ +{ return true; } @@ -56,7 +56,7 @@ bool SavePage::isComplete() const void SavePage::writeToController() { - if(!getWizard()->getConnectionManager()->isConnected()) { + if (!getWizard()->getConnectionManager()->isConnected()) { QMessageBox msgBox; msgBox.setText(tr("An OpenPilot controller must be connected to your computer to save the " "configuration.\nPlease connect your OpenPilot controller to your computer and try again.")); @@ -68,11 +68,11 @@ void SavePage::writeToController() enableButtons(false); VehicleConfigurationHelper helper(getWizard()); - connect(&helper, SIGNAL(saveProgress(int, int, QString)),this, SLOT(saveProgress(int, int, QString))); + connect(&helper, SIGNAL(saveProgress(int, int, QString)), this, SLOT(saveProgress(int, int, QString))); m_successfulWrite = helper.setupVehicle(); - disconnect(&helper, SIGNAL(saveProgress(int, int, QString)),this, SLOT(saveProgress(int, int, QString))); + disconnect(&helper, SIGNAL(saveProgress(int, int, QString)), this, SLOT(saveProgress(int, int, QString))); ui->saveProgressLabel->setText(QString("%2").arg(m_successfulWrite ? "green" : "red", ui->saveProgressLabel->text())); enableButtons(true); @@ -92,13 +92,13 @@ void SavePage::enableButtons(bool enable) void SavePage::saveProgress(int total, int current, QString description) { qDebug() << "Progress " << current << "(" << total << ")"; - if(ui->saveProgressBar->maximum() != total) { + if (ui->saveProgressBar->maximum() != total) { ui->saveProgressBar->setMaximum(total); } - if(ui->saveProgressBar->value() != current) { + if (ui->saveProgressBar->value() != current) { ui->saveProgressBar->setValue(current); } - if(ui->saveProgressLabel->text() != description) { + if (ui->saveProgressLabel->text() != description) { ui->saveProgressLabel->setText(description); } } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.h index d8382df93..9310d837c 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.h @@ -34,8 +34,7 @@ namespace Ui { class SavePage; } -class SavePage : public AbstractWizardPage -{ +class SavePage : public AbstractWizardPage { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.cpp index bc0ee36f1..568da659c 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.cpp @@ -32,7 +32,7 @@ StartPage::StartPage(SetupWizard *wizard, QWidget *parent) : ui(new Ui::StartPage) { ui->setupUi(this); - setFont(QFont("Ubuntu",2)); + setFont(QFont("Ubuntu", 2)); } StartPage::~StartPage() diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.h index 3e9e120cc..da44d920e 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.h @@ -33,14 +33,13 @@ namespace Ui { class StartPage; } -class StartPage : public AbstractWizardPage -{ +class StartPage : public AbstractWizardPage { Q_OBJECT - + public: explicit StartPage(SetupWizard *wizard, QWidget *parent = 0); ~StartPage(); - + private: Ui::StartPage *ui; }; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.cpp index c4846e782..884c94eb9 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.cpp @@ -31,7 +31,7 @@ #include "connectiondiagram.h" SummaryPage::SummaryPage(SetupWizard *wizard, QWidget *parent) : - AbstractWizardPage(wizard, parent), + AbstractWizardPage(wizard, parent), ui(new Ui::SummaryPage) { ui->setupUi(this); @@ -56,5 +56,6 @@ void SummaryPage::initializePage() void SummaryPage::showDiagram() { ConnectionDiagram diagram(this, getWizard()); + diagram.exec(); } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.h index 616f82392..daf5402b1 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.h @@ -34,8 +34,7 @@ namespace Ui { class SummaryPage; } -class SummaryPage : public AbstractWizardPage -{ +class SummaryPage : public AbstractWizardPage { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.h index 593aa1fca..0bef8dc5c 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.h @@ -34,14 +34,13 @@ namespace Ui { class SurfacePage; } -class SurfacePage : public AbstractWizardPage -{ +class SurfacePage : public AbstractWizardPage { Q_OBJECT - + public: explicit SurfacePage(SetupWizard *wizard, QWidget *parent = 0); ~SurfacePage(); - + private: Ui::SurfacePage *ui; }; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp index a616a1194..b7de92bc9 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp @@ -42,19 +42,15 @@ VehiclePage::~VehiclePage() bool VehiclePage::validatePage() { - if(ui->multirotorButton->isChecked()) { + if (ui->multirotorButton->isChecked()) { getWizard()->setVehicleType(SetupWizard::VEHICLE_MULTI); - } - else if(ui->fixedwingButton->isChecked()) { + } else if (ui->fixedwingButton->isChecked()) { getWizard()->setVehicleType(SetupWizard::VEHICLE_FIXEDWING); - } - else if(ui->heliButton->isChecked()) { + } else if (ui->heliButton->isChecked()) { getWizard()->setVehicleType(SetupWizard::VEHICLE_HELI); - } - else if(ui->surfaceButton->isChecked()) { + } else if (ui->surfaceButton->isChecked()) { getWizard()->setVehicleType(SetupWizard::VEHICLE_SURFACE); - } - else { + } else { getWizard()->setVehicleType(SetupWizard::VEHICLE_UNKNOWN); } return true; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h index 5e72a3d21..737f1fc25 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h @@ -34,10 +34,9 @@ namespace Ui { class VehiclePage; } -class VehiclePage : public AbstractWizardPage -{ +class VehiclePage : public AbstractWizardPage { Q_OBJECT - + public: explicit VehiclePage(SetupWizard *wizard, QWidget *parent = 0); ~VehiclePage(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index bd237f875..86b009314 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -56,8 +56,7 @@ SetupWizard::SetupWizard(QWidget *parent) : QWizard(parent), VehicleConfiguratio { setWindowTitle(tr("OpenPilot Setup Wizard")); setOption(QWizard::IndependentPages, false); - for(quint16 i = 0; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) - { + for (quint16 i = 0; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) { m_actuatorSettings << actuatorChannelSettings(); } setWizardStyle(QWizard::ModernStyle); @@ -69,210 +68,218 @@ SetupWizard::SetupWizard(QWidget *parent) : QWizard(parent), VehicleConfiguratio int SetupWizard::nextId() const { switch (currentId()) { - case PAGE_START: - if(canAutoUpdate()) { - return PAGE_UPDATE; - } else { - return PAGE_CONTROLLER; - } - case PAGE_UPDATE: + case PAGE_START: + if (canAutoUpdate()) { + return PAGE_UPDATE; + } else { return PAGE_CONTROLLER; - case PAGE_CONTROLLER: { - switch(getControllerType()) - { - case CONTROLLER_CC: - case CONTROLLER_CC3D: - case CONTROLLER_REVO: - return PAGE_INPUT; - case CONTROLLER_OPLINK: - default: - return PAGE_NOTYETIMPLEMENTED; - } } - case PAGE_VEHICLES: { - switch(getVehicleType()) - { - case VEHICLE_MULTI: - return PAGE_MULTI; - case VEHICLE_FIXEDWING: - return PAGE_FIXEDWING; - case VEHICLE_HELI: - return PAGE_HELI; - case VEHICLE_SURFACE: - return PAGE_SURFACE; - default: - return PAGE_NOTYETIMPLEMENTED; - } - } - case PAGE_MULTI: - return PAGE_OUTPUT; - case PAGE_INPUT: - if(isRestartNeeded()) { - saveHardwareSettings(); - return PAGE_REBOOT; - } - else { - return PAGE_VEHICLES; - } - case PAGE_REBOOT: - return PAGE_VEHICLES; - case PAGE_OUTPUT: - return PAGE_SUMMARY; - case PAGE_BIAS_CALIBRATION: - return PAGE_OUTPUT_CALIBRATION; - //case PAGE_REVO_CALIBRATION: - // return PAGE_OUTPUT_CALIBRATION; - case PAGE_OUTPUT_CALIBRATION: - return PAGE_SAVE; - case PAGE_SUMMARY: - { - switch(getControllerType()) - { - case CONTROLLER_CC: - case CONTROLLER_CC3D: - case CONTROLLER_REVO: - return PAGE_BIAS_CALIBRATION; - default: - return PAGE_NOTYETIMPLEMENTED; - } - } - case PAGE_SAVE: - return PAGE_END; - case PAGE_NOTYETIMPLEMENTED: - return PAGE_END; + case PAGE_UPDATE: + return PAGE_CONTROLLER; + + case PAGE_CONTROLLER: + { + switch (getControllerType()) { + case CONTROLLER_CC: + case CONTROLLER_CC3D: + case CONTROLLER_REVO: + return PAGE_INPUT; + + case CONTROLLER_OPLINK: default: - return -1; + return PAGE_NOTYETIMPLEMENTED; + } + } + case PAGE_VEHICLES: + { + switch (getVehicleType()) { + case VEHICLE_MULTI: + return PAGE_MULTI; + + case VEHICLE_FIXEDWING: + return PAGE_FIXEDWING; + + case VEHICLE_HELI: + return PAGE_HELI; + + case VEHICLE_SURFACE: + return PAGE_SURFACE; + + default: + return PAGE_NOTYETIMPLEMENTED; + } + } + case PAGE_MULTI: + return PAGE_OUTPUT; + + case PAGE_INPUT: + if (isRestartNeeded()) { + saveHardwareSettings(); + return PAGE_REBOOT; + } else { + return PAGE_VEHICLES; + } + case PAGE_REBOOT: + return PAGE_VEHICLES; + + case PAGE_OUTPUT: + return PAGE_SUMMARY; + + case PAGE_BIAS_CALIBRATION: + return PAGE_OUTPUT_CALIBRATION; + + // case PAGE_REVO_CALIBRATION: + // return PAGE_OUTPUT_CALIBRATION; + case PAGE_OUTPUT_CALIBRATION: + return PAGE_SAVE; + + case PAGE_SUMMARY: + { + switch (getControllerType()) { + case CONTROLLER_CC: + case CONTROLLER_CC3D: + case CONTROLLER_REVO: + return PAGE_BIAS_CALIBRATION; + + default: + return PAGE_NOTYETIMPLEMENTED; + } + } + case PAGE_SAVE: + return PAGE_END; + + case PAGE_NOTYETIMPLEMENTED: + return PAGE_END; + + default: + return -1; } } QString SetupWizard::getSummaryText() { QString summary = ""; + summary.append("").append(tr("Controller type: ")).append(""); - switch(getControllerType()) - { - case CONTROLLER_CC: - summary.append(tr("OpenPilot CopterControl")); - break; - case CONTROLLER_CC3D: - summary.append(tr("OpenPilot CopterControl 3D")); - break; - case CONTROLLER_REVO: - summary.append(tr("OpenPilot Revolution")); - break; - case CONTROLLER_OPLINK: - summary.append(tr("OpenPilot OPLink Radio Modem")); - break; - default: - summary.append(tr("Unknown")); - break; + switch (getControllerType()) { + case CONTROLLER_CC: + summary.append(tr("OpenPilot CopterControl")); + break; + case CONTROLLER_CC3D: + summary.append(tr("OpenPilot CopterControl 3D")); + break; + case CONTROLLER_REVO: + summary.append(tr("OpenPilot Revolution")); + break; + case CONTROLLER_OPLINK: + summary.append(tr("OpenPilot OPLink Radio Modem")); + break; + default: + summary.append(tr("Unknown")); + break; } summary.append("
"); summary.append("").append(tr("Vehicle type: ")).append(""); - switch (getVehicleType()) - { - case VEHICLE_MULTI: - summary.append(tr("Multirotor")); - - summary.append("
"); - summary.append("").append(tr("Vehicle sub type: ")).append(""); - switch (getVehicleSubType()) - { - case SetupWizard::MULTI_ROTOR_TRI_Y: - summary.append(tr("Tricopter")); - break; - case SetupWizard::MULTI_ROTOR_QUAD_X: - summary.append(tr("Quadcopter X")); - break; - case SetupWizard::MULTI_ROTOR_QUAD_PLUS: - summary.append(tr("Quadcopter +")); - break; - case SetupWizard::MULTI_ROTOR_HEXA: - summary.append(tr("Hexacopter")); - break; - case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: - summary.append(tr("Hexacopter Coax (Y6)")); - break; - case SetupWizard::MULTI_ROTOR_HEXA_H: - summary.append(tr("Hexacopter X")); - break; - case SetupWizard::MULTI_ROTOR_OCTO: - summary.append(tr("Octocopter")); - break; - case SetupWizard::MULTI_ROTOR_OCTO_COAX_X: - summary.append(tr("Octocopter Coax X")); - break; - case SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS: - summary.append(tr("Octocopter Coax +")); - break; - case SetupWizard::MULTI_ROTOR_OCTO_V: - summary.append(tr("Octocopter V")); - break; - default: - summary.append(tr("Unknown")); - break; - } + switch (getVehicleType()) { + case VEHICLE_MULTI: + summary.append(tr("Multirotor")); + summary.append("
"); + summary.append("").append(tr("Vehicle sub type: ")).append(""); + switch (getVehicleSubType()) { + case SetupWizard::MULTI_ROTOR_TRI_Y: + summary.append(tr("Tricopter")); break; - case VEHICLE_FIXEDWING: - summary.append(tr("Fixed wing")); + case SetupWizard::MULTI_ROTOR_QUAD_X: + summary.append(tr("Quadcopter X")); break; - case VEHICLE_HELI: - summary.append(tr("Helicopter")); + case SetupWizard::MULTI_ROTOR_QUAD_PLUS: + summary.append(tr("Quadcopter +")); break; - case VEHICLE_SURFACE: - summary.append(tr("Surface vehicle")); + case SetupWizard::MULTI_ROTOR_HEXA: + summary.append(tr("Hexacopter")); + break; + case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: + summary.append(tr("Hexacopter Coax (Y6)")); + break; + case SetupWizard::MULTI_ROTOR_HEXA_H: + summary.append(tr("Hexacopter X")); + break; + case SetupWizard::MULTI_ROTOR_OCTO: + summary.append(tr("Octocopter")); + break; + case SetupWizard::MULTI_ROTOR_OCTO_COAX_X: + summary.append(tr("Octocopter Coax X")); + break; + case SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS: + summary.append(tr("Octocopter Coax +")); + break; + case SetupWizard::MULTI_ROTOR_OCTO_V: + summary.append(tr("Octocopter V")); break; default: summary.append(tr("Unknown")); + break; + } + + break; + case VEHICLE_FIXEDWING: + summary.append(tr("Fixed wing")); + break; + case VEHICLE_HELI: + summary.append(tr("Helicopter")); + break; + case VEHICLE_SURFACE: + summary.append(tr("Surface vehicle")); + break; + default: + summary.append(tr("Unknown")); } summary.append("
"); summary.append("").append(tr("Input type: ")).append(""); - switch (getInputType()) - { - case INPUT_PWM: - summary.append(tr("PWM (One cable per channel)")); - break; - case INPUT_PPM: - summary.append(tr("PPM (One cable for all channels)")); - break; - case INPUT_SBUS: - summary.append(tr("Futaba S.Bus")); - break; - case INPUT_DSM2: - summary.append(tr("Spektrum satellite (DSM2)")); - break; - case INPUT_DSMX10: - summary.append(tr("Spektrum satellite (DSMX10BIT)")); - break; - case INPUT_DSMX11: - summary.append(tr("Spektrum satellite (DSMX11BIT)")); - break; - default: - summary.append(tr("Unknown")); + switch (getInputType()) { + case INPUT_PWM: + summary.append(tr("PWM (One cable per channel)")); + break; + case INPUT_PPM: + summary.append(tr("PPM (One cable for all channels)")); + break; + case INPUT_SBUS: + summary.append(tr("Futaba S.Bus")); + break; + case INPUT_DSM2: + summary.append(tr("Spektrum satellite (DSM2)")); + break; + case INPUT_DSMX10: + summary.append(tr("Spektrum satellite (DSMX10BIT)")); + break; + case INPUT_DSMX11: + summary.append(tr("Spektrum satellite (DSMX11BIT)")); + break; + default: + summary.append(tr("Unknown")); } summary.append("
"); summary.append("").append(tr("ESC type: ")).append(""); - switch (getESCType()) - { - case ESC_LEGACY: - summary.append(tr("Legacy ESC (50 Hz)")); - break; - case ESC_RAPID: - summary.append(tr("Rapid ESC (400 Hz)")); - break; - default: - summary.append(tr("Unknown")); + switch (getESCType()) { + case ESC_LEGACY: + summary.append(tr("Legacy ESC (50 Hz)")); + break; + case ESC_RAPID: + summary.append(tr("Rapid ESC (400 Hz)")); + break; + default: + summary.append(tr("Unknown")); } /* - summary.append("
"); - summary.append("").append(tr("Reboot required: ")).append(""); - summary.append(isRestartNeeded() ? tr("Yes") : tr("No")); - */ + summary.append("
"); + summary.append("").append(tr("Reboot required: ")).append(""); + summary.append(isRestartNeeded() ? tr("Yes") : tr("No")); + */ return summary; } @@ -289,7 +296,7 @@ void SetupWizard::createPages() setPage(PAGE_INPUT, new InputPage(this)); setPage(PAGE_OUTPUT, new OutputPage(this)); setPage(PAGE_BIAS_CALIBRATION, new BiasCalibrationPage(this)); - //setPage(PAGE_REVO_CALIBRATION, new RevoCalibrationPage(this)); + // setPage(PAGE_REVO_CALIBRATION, new RevoCalibrationPage(this)); setPage(PAGE_OUTPUT_CALIBRATION, new OutputCalibrationPage(this)); setPage(PAGE_SUMMARY, new SummaryPage(this)); setPage(PAGE_SAVE, new SavePage(this)); @@ -309,10 +316,9 @@ void SetupWizard::createPages() void SetupWizard::customBackClicked() { - if(currentId() == PAGE_OUTPUT_CALIBRATION) { - static_cast(currentPage())->customBackClicked(); - } - else { + if (currentId() == PAGE_OUTPUT_CALIBRATION) { + static_cast(currentPage())->customBackClicked(); + } else { back(); } } @@ -326,14 +332,16 @@ void SetupWizard::pageChanged(int currId) bool SetupWizard::saveHardwareSettings() const { VehicleConfigurationHelper helper(const_cast(this)); + return helper.setupHardwareSettings(); } bool SetupWizard::canAutoUpdate() const { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm); - UploaderGadgetFactory *uploader = pm->getObject(); + UploaderGadgetFactory *uploader = pm->getObject(); Q_ASSERT(uploader); return uploader->isAutoUpdateCapable(); } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h index f0418660c..a908ac989 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h @@ -35,49 +35,115 @@ #include "vehicleconfigurationsource.h" #include "vehicleconfigurationhelper.h" -class SetupWizard : public QWizard, public VehicleConfigurationSource -{ +class SetupWizard : public QWizard, public VehicleConfigurationSource { Q_OBJECT public: SetupWizard(QWidget *parent = 0); int nextId() const; - void setControllerType(SetupWizard::CONTROLLER_TYPE type) { m_controllerType = type; } - SetupWizard::CONTROLLER_TYPE getControllerType() const { return m_controllerType; } + void setControllerType(SetupWizard::CONTROLLER_TYPE type) + { + m_controllerType = type; + } + SetupWizard::CONTROLLER_TYPE getControllerType() const + { + return m_controllerType; + } - void setVehicleType(SetupWizard::VEHICLE_TYPE type) { m_vehicleType = type; } - SetupWizard::VEHICLE_TYPE getVehicleType() const { return m_vehicleType; } + void setVehicleType(SetupWizard::VEHICLE_TYPE type) + { + m_vehicleType = type; + } + SetupWizard::VEHICLE_TYPE getVehicleType() const + { + return m_vehicleType; + } - void setVehicleSubType(SetupWizard::VEHICLE_SUB_TYPE type) { m_vehicleSubType = type; } - SetupWizard::VEHICLE_SUB_TYPE getVehicleSubType() const { return m_vehicleSubType; } + void setVehicleSubType(SetupWizard::VEHICLE_SUB_TYPE type) + { + m_vehicleSubType = type; + } + SetupWizard::VEHICLE_SUB_TYPE getVehicleSubType() const + { + return m_vehicleSubType; + } - void setInputType(SetupWizard::INPUT_TYPE type) { m_inputType = type; } - SetupWizard::INPUT_TYPE getInputType() const { return m_inputType; } + void setInputType(SetupWizard::INPUT_TYPE type) + { + m_inputType = type; + } + SetupWizard::INPUT_TYPE getInputType() const + { + return m_inputType; + } - void setESCType(SetupWizard::ESC_TYPE type) { m_escType = type; } - SetupWizard::ESC_TYPE getESCType() const { return m_escType; } + void setESCType(SetupWizard::ESC_TYPE type) + { + m_escType = type; + } + SetupWizard::ESC_TYPE getESCType() const + { + return m_escType; + } - void setGPSSetting(SetupWizard::GPS_SETTING setting) { m_gpsSetting = setting; } - SetupWizard::GPS_SETTING getGPSSetting() const {return m_gpsSetting;} + void setGPSSetting(SetupWizard::GPS_SETTING setting) + { + m_gpsSetting = setting; + } + SetupWizard::GPS_SETTING getGPSSetting() const + { + return m_gpsSetting; + } - void setRadioSetting(SetupWizard::RADIO_SETTING setting) { m_radioSetting = setting; } - SetupWizard::RADIO_SETTING getRadioSetting() const {return m_radioSetting;} + void setRadioSetting(SetupWizard::RADIO_SETTING setting) + { + m_radioSetting = setting; + } + SetupWizard::RADIO_SETTING getRadioSetting() const + { + return m_radioSetting; + } - void setLevellingBias(accelGyroBias bias) { m_calibrationBias = bias; m_calibrationPerformed = true; } - bool isCalibrationPerformed() const { return m_calibrationPerformed; } - accelGyroBias getCalibrationBias() const { return m_calibrationBias; } + void setLevellingBias(accelGyroBias bias) + { + m_calibrationBias = bias; m_calibrationPerformed = true; + } + bool isCalibrationPerformed() const + { + return m_calibrationPerformed; + } + accelGyroBias getCalibrationBias() const + { + return m_calibrationBias; + } - void setActuatorSettings(QList actuatorSettings) { m_actuatorSettings = actuatorSettings; } - bool isMotorCalibrationPerformed() const { return m_motorCalibrationPerformed; } - QList getActuatorSettings() const { return m_actuatorSettings; } + void setActuatorSettings(QList actuatorSettings) + { + m_actuatorSettings = actuatorSettings; + } + bool isMotorCalibrationPerformed() const + { + return m_motorCalibrationPerformed; + } + QList getActuatorSettings() const + { + return m_actuatorSettings; + } - void setRestartNeeded(bool needed) { m_restartNeeded = needed; } - bool isRestartNeeded() const {return m_restartNeeded; } + void setRestartNeeded(bool needed) + { + m_restartNeeded = needed; + } + bool isRestartNeeded() const + { + return m_restartNeeded; + } QString getSummaryText(); - Core::ConnectionManager* getConnectionManager() { + Core::ConnectionManager *getConnectionManager() + { if (!m_connectionManager) { m_connectionManager = Core::ICore::instance()->connectionManager(); Q_ASSERT(m_connectionManager); @@ -89,10 +155,10 @@ private slots: void customBackClicked(); void pageChanged(int currId); private: - enum {PAGE_START, PAGE_CONTROLLER, PAGE_VEHICLES, PAGE_MULTI, PAGE_FIXEDWING, - PAGE_HELI, PAGE_SURFACE, PAGE_INPUT, PAGE_OUTPUT, PAGE_BIAS_CALIBRATION, - PAGE_REVO_CALIBRATION, PAGE_OUTPUT_CALIBRATION, PAGE_SAVE, PAGE_SUMMARY, - PAGE_NOTYETIMPLEMENTED, PAGE_REBOOT, PAGE_END, PAGE_UPDATE}; + enum { PAGE_START, PAGE_CONTROLLER, PAGE_VEHICLES, PAGE_MULTI, PAGE_FIXEDWING, + PAGE_HELI, PAGE_SURFACE, PAGE_INPUT, PAGE_OUTPUT, PAGE_BIAS_CALIBRATION, + PAGE_REVO_CALIBRATION, PAGE_OUTPUT_CALIBRATION, PAGE_SAVE, PAGE_SUMMARY, + PAGE_NOTYETIMPLEMENTED, PAGE_REBOOT, PAGE_END, PAGE_UPDATE }; void createPages(); bool saveHardwareSettings() const; bool canAutoUpdate() const; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.cpp index defc1e927..2055c8e63 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.cpp @@ -38,23 +38,21 @@ #include SetupWizardPlugin::SetupWizardPlugin() : wizardRunning(false) -{ -} +{} SetupWizardPlugin::~SetupWizardPlugin() -{ -} +{} -bool SetupWizardPlugin::initialize(const QStringList& args, QString *errMsg) +bool SetupWizardPlugin::initialize(const QStringList & args, QString *errMsg) { Q_UNUSED(args); Q_UNUSED(errMsg); // Add Menu entry - Core::ActionManager* am = Core::ICore::instance()->actionManager(); - Core::ActionContainer* ac = am->actionContainer(Core::Constants::M_TOOLS); + Core::ActionManager *am = Core::ICore::instance()->actionManager(); + Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_TOOLS); - Core::Command* cmd = am->registerAction(new QAction(this), + Core::Command *cmd = am->registerAction(new QAction(this), "SetupWizardPlugin.ShowSetupWizard", QList() << Core::Constants::C_GLOBAL_ID); @@ -72,12 +70,10 @@ bool SetupWizardPlugin::initialize(const QStringList& args, QString *errMsg) } void SetupWizardPlugin::extensionsInitialized() -{ -} +{} void SetupWizardPlugin::shutdown() -{ -} +{} void SetupWizardPlugin::showSetupWizard() { @@ -85,7 +81,7 @@ void SetupWizardPlugin::showSetupWizard() wizardRunning = true; SetupWizard *m_wiz = new SetupWizard(); connect(m_wiz, SIGNAL(finished(int)), this, SLOT(wizardTerminated())); - m_wiz->setAttribute( Qt::WA_DeleteOnClose, true ); + m_wiz->setAttribute(Qt::WA_DeleteOnClose, true); m_wiz->setWindowFlags(m_wiz->windowFlags() | Qt::WindowStaysOnTopHint); m_wiz->show(); } @@ -94,7 +90,7 @@ void SetupWizardPlugin::showSetupWizard() void SetupWizardPlugin::wizardTerminated() { wizardRunning = false; - disconnect(this,SLOT(wizardTerminated())); + disconnect(this, SLOT(wizardTerminated())); } Q_EXPORT_PLUGIN(SetupWizardPlugin) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.h b/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.h index b79c0f7c5..f0765d993 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.h @@ -31,23 +31,22 @@ #include #include "setupwizard.h" -class SetupWizardPlugin : public ExtensionSystem::IPlugin -{ +class SetupWizardPlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: - SetupWizardPlugin(); - ~SetupWizardPlugin(); + SetupWizardPlugin(); + ~SetupWizardPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private slots: - void showSetupWizard(); - void wizardTerminated(); + void showSetupWizard(); + void wizardTerminated(); private: - bool wizardRunning; + bool wizardRunning; }; #endif // SETUPWIZARDPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 854a629cc..81d60f3c2 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -37,12 +37,12 @@ #include "revocalibration.h" const qint16 VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE = 50; -const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 400; +const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 400; VehicleConfigurationHelper::VehicleConfigurationHelper(VehicleConfigurationSource *configSource) : m_configSource(configSource), m_uavoManager(0), - m_transactionOK(false), m_transactionTimeout(false), m_currentTransactionObjectID(-1), - m_progress(0) + m_transactionOK(false), m_transactionTimeout(false), m_currentTransactionObjectID(-1), + m_progress(0) { Q_ASSERT(m_configSource); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); @@ -56,7 +56,7 @@ bool VehicleConfigurationHelper::setupVehicle(bool save) clearModifiedObjects(); resetVehicleConfig(); resetGUIData(); - if(!saveChangesToController(save)) { + if (!saveChangesToController(save)) { return false; } @@ -66,7 +66,7 @@ bool VehicleConfigurationHelper::setupVehicle(bool save) applyActuatorConfiguration(); applyFlighModeConfiguration(); - if(save) { + if (save) { applySensorBiasConfiguration(); } @@ -92,13 +92,13 @@ bool VehicleConfigurationHelper::setupHardwareSettings(bool save) void VehicleConfigurationHelper::addModifiedObject(UAVDataObject *object, QString description) { - m_modifiedObjects << new QPair(object, description); + m_modifiedObjects << new QPair(object, description); } void VehicleConfigurationHelper::clearModifiedObjects() { - for(int i = 0; i < m_modifiedObjects.count(); i++) { - QPair *pair = m_modifiedObjects.at(i); + for (int i = 0; i < m_modifiedObjects.count(); i++) { + QPair *pair = m_modifiedObjects.at(i); delete pair; } m_modifiedObjects.clear(); @@ -106,81 +106,79 @@ void VehicleConfigurationHelper::clearModifiedObjects() void VehicleConfigurationHelper::applyHardwareConfiguration() { - HwSettings* hwSettings = HwSettings::GetInstance(m_uavoManager); + HwSettings *hwSettings = HwSettings::GetInstance(m_uavoManager); HwSettings::DataFields data = hwSettings->getData(); - switch(m_configSource->getControllerType()) - { - case VehicleConfigurationSource::CONTROLLER_CC: - case VehicleConfigurationSource::CONTROLLER_CC3D: - // Reset all ports - data.CC_RcvrPort = HwSettings::CC_RCVRPORT_DISABLED; - //Default mainport to be active telemetry link - data.CC_MainPort = HwSettings::CC_MAINPORT_TELEMETRY; + switch (m_configSource->getControllerType()) { + case VehicleConfigurationSource::CONTROLLER_CC: + case VehicleConfigurationSource::CONTROLLER_CC3D: + // Reset all ports + data.CC_RcvrPort = HwSettings::CC_RCVRPORT_DISABLED; - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DISABLED; - switch(m_configSource->getInputType()) - { - case VehicleConfigurationSource::INPUT_PWM: - data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PWM; - break; - case VehicleConfigurationSource::INPUT_PPM: - data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PPM; - break; - case VehicleConfigurationSource::INPUT_SBUS: - // We have to set teletry on flexport since s.bus needs the mainport. - data.CC_MainPort = HwSettings::CC_MAINPORT_SBUS; - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_TELEMETRY; - break; - case VehicleConfigurationSource::INPUT_DSMX10: - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSMX10BIT; - break; - case VehicleConfigurationSource::INPUT_DSMX11: - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSMX11BIT; - break; - case VehicleConfigurationSource::INPUT_DSM2: - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM2; - break; - default: - break; - } + // Default mainport to be active telemetry link + data.CC_MainPort = HwSettings::CC_MAINPORT_TELEMETRY; + + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DISABLED; + switch (m_configSource->getInputType()) { + case VehicleConfigurationSource::INPUT_PWM: + data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PWM; break; - case VehicleConfigurationSource::CONTROLLER_REVO: - // Reset all ports - data.RM_RcvrPort = HwSettings::RM_RCVRPORT_DISABLED; - - //Default mainport to be active telemetry link - data.RM_MainPort = HwSettings::RM_MAINPORT_TELEMETRY; - - data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DISABLED; - switch(m_configSource->getInputType()) - { - case VehicleConfigurationSource::INPUT_PWM: - data.RM_RcvrPort = HwSettings::RM_RCVRPORT_PWM; - break; - case VehicleConfigurationSource::INPUT_PPM: - data.RM_RcvrPort = HwSettings::RM_RCVRPORT_PPM; - break; - case VehicleConfigurationSource::INPUT_SBUS: - // We have to set teletry on flexport since s.bus needs the mainport. - data.RM_MainPort = HwSettings::RM_MAINPORT_SBUS; - data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_TELEMETRY; - break; - case VehicleConfigurationSource::INPUT_DSMX10: - data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSMX10BIT; - break; - case VehicleConfigurationSource::INPUT_DSMX11: - data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSMX11BIT; - break; - case VehicleConfigurationSource::INPUT_DSM2: - data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM2; - break; - default: - break; - } + case VehicleConfigurationSource::INPUT_PPM: + data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PPM; + break; + case VehicleConfigurationSource::INPUT_SBUS: + // We have to set teletry on flexport since s.bus needs the mainport. + data.CC_MainPort = HwSettings::CC_MAINPORT_SBUS; + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_TELEMETRY; + break; + case VehicleConfigurationSource::INPUT_DSMX10: + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSMX10BIT; + break; + case VehicleConfigurationSource::INPUT_DSMX11: + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSMX11BIT; + break; + case VehicleConfigurationSource::INPUT_DSM2: + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM2; break; default: break; + } + break; + case VehicleConfigurationSource::CONTROLLER_REVO: + // Reset all ports + data.RM_RcvrPort = HwSettings::RM_RCVRPORT_DISABLED; + + // Default mainport to be active telemetry link + data.RM_MainPort = HwSettings::RM_MAINPORT_TELEMETRY; + + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DISABLED; + switch (m_configSource->getInputType()) { + case VehicleConfigurationSource::INPUT_PWM: + data.RM_RcvrPort = HwSettings::RM_RCVRPORT_PWM; + break; + case VehicleConfigurationSource::INPUT_PPM: + data.RM_RcvrPort = HwSettings::RM_RCVRPORT_PPM; + break; + case VehicleConfigurationSource::INPUT_SBUS: + // We have to set teletry on flexport since s.bus needs the mainport. + data.RM_MainPort = HwSettings::RM_MAINPORT_SBUS; + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_TELEMETRY; + break; + case VehicleConfigurationSource::INPUT_DSMX10: + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSMX10BIT; + break; + case VehicleConfigurationSource::INPUT_DSMX11: + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSMX11BIT; + break; + case VehicleConfigurationSource::INPUT_DSM2: + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM2; + break; + default: + break; + } + break; + default: + break; } hwSettings->setData(data); addModifiedObject(hwSettings, tr("Writing hardware settings")); @@ -188,127 +186,127 @@ void VehicleConfigurationHelper::applyHardwareConfiguration() void VehicleConfigurationHelper::applyVehicleConfiguration() { - - switch(m_configSource->getVehicleType()) + switch (m_configSource->getVehicleType()) { + case VehicleConfigurationSource::VEHICLE_MULTI: { - case VehicleConfigurationSource::VEHICLE_MULTI: - { - switch(m_configSource->getVehicleSubType()) - { - case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: - setupTriCopter(); - break; - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: - setupQuadCopter(); - break; - case VehicleConfigurationSource::MULTI_ROTOR_HEXA: - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: - setupHexaCopter(); - break; - case VehicleConfigurationSource::MULTI_ROTOR_OCTO: - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: - setupOctoCopter(); - break; - default: - break; - } + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: + setupTriCopter(); break; - } - case VehicleConfigurationSource::VEHICLE_FIXEDWING: - case VehicleConfigurationSource::VEHICLE_HELI: - case VehicleConfigurationSource::VEHICLE_SURFACE: - // TODO: Implement settings for other vehicle types? + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: + setupQuadCopter(); + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA: + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: + setupHexaCopter(); + break; + case VehicleConfigurationSource::MULTI_ROTOR_OCTO: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: + setupOctoCopter(); break; default: break; + } + break; + } + case VehicleConfigurationSource::VEHICLE_FIXEDWING: + case VehicleConfigurationSource::VEHICLE_HELI: + case VehicleConfigurationSource::VEHICLE_SURFACE: + // TODO: Implement settings for other vehicle types? + break; + default: + break; } } void VehicleConfigurationHelper::applyActuatorConfiguration() { - ActuatorSettings* actSettings = ActuatorSettings::GetInstance(m_uavoManager); - switch(m_configSource->getVehicleType()) { - case VehicleConfigurationSource::VEHICLE_MULTI: { - ActuatorSettings::DataFields data = actSettings->getData(); + ActuatorSettings *actSettings = ActuatorSettings::GetInstance(m_uavoManager); - QList actuatorSettings = m_configSource->getActuatorSettings(); - for(quint16 i = 0; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) { - data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM; - data.ChannelAddr[i] = i; - data.ChannelMin[i] = actuatorSettings[i].channelMin; - data.ChannelNeutral[i] = actuatorSettings[i].channelNeutral; - data.ChannelMax[i] = actuatorSettings[i].channelMax; - } + switch (m_configSource->getVehicleType()) { + case VehicleConfigurationSource::VEHICLE_MULTI: + { + ActuatorSettings::DataFields data = actSettings->getData(); - data.MotorsSpinWhileArmed = ActuatorSettings::MOTORSSPINWHILEARMED_FALSE; - - for(quint16 i = 0; i < ActuatorSettings::CHANNELUPDATEFREQ_NUMELEM; i++) { - data.ChannelUpdateFreq[i] = LEGACY_ESC_FREQUENCE; - } - - qint16 updateFrequence = LEGACY_ESC_FREQUENCE; - switch(m_configSource->getESCType()) { - case VehicleConfigurationSource::ESC_LEGACY: - updateFrequence = LEGACY_ESC_FREQUENCE; - break; - case VehicleConfigurationSource::ESC_RAPID: - updateFrequence = RAPID_ESC_FREQUENCE; - break; - default: - break; - } - - switch(m_configSource->getVehicleSubType()) { - case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: - data.ChannelUpdateFreq[0] = updateFrequence; - if(m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) { - data.ChannelUpdateFreq[1] = updateFrequence; - } - break; - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: - data.ChannelUpdateFreq[0] = updateFrequence; - data.ChannelUpdateFreq[1] = updateFrequence; - if(m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) { - data.ChannelUpdateFreq[2] = updateFrequence; - } - break; - case VehicleConfigurationSource::MULTI_ROTOR_HEXA: - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: - case VehicleConfigurationSource::MULTI_ROTOR_OCTO: - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: - data.ChannelUpdateFreq[0] = updateFrequence; - data.ChannelUpdateFreq[1] = updateFrequence; - data.ChannelUpdateFreq[2] = updateFrequence; - data.ChannelUpdateFreq[3] = updateFrequence; - break; - default: - break; - } - actSettings->setData(data); - addModifiedObject(actSettings, tr("Writing actuator settings")); - break; + QList actuatorSettings = m_configSource->getActuatorSettings(); + for (quint16 i = 0; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) { + data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM; + data.ChannelAddr[i] = i; + data.ChannelMin[i] = actuatorSettings[i].channelMin; + data.ChannelNeutral[i] = actuatorSettings[i].channelNeutral; + data.ChannelMax[i] = actuatorSettings[i].channelMax; } - case VehicleConfigurationSource::VEHICLE_FIXEDWING: - case VehicleConfigurationSource::VEHICLE_HELI: - case VehicleConfigurationSource::VEHICLE_SURFACE: - // TODO: Implement settings for other vehicle types? + + data.MotorsSpinWhileArmed = ActuatorSettings::MOTORSSPINWHILEARMED_FALSE; + + for (quint16 i = 0; i < ActuatorSettings::CHANNELUPDATEFREQ_NUMELEM; i++) { + data.ChannelUpdateFreq[i] = LEGACY_ESC_FREQUENCE; + } + + qint16 updateFrequence = LEGACY_ESC_FREQUENCE; + switch (m_configSource->getESCType()) { + case VehicleConfigurationSource::ESC_LEGACY: + updateFrequence = LEGACY_ESC_FREQUENCE; + break; + case VehicleConfigurationSource::ESC_RAPID: + updateFrequence = RAPID_ESC_FREQUENCE; break; default: break; + } + + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: + data.ChannelUpdateFreq[0] = updateFrequence; + if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) { + data.ChannelUpdateFreq[1] = updateFrequence; + } + break; + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: + data.ChannelUpdateFreq[0] = updateFrequence; + data.ChannelUpdateFreq[1] = updateFrequence; + if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) { + data.ChannelUpdateFreq[2] = updateFrequence; + } + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA: + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: + data.ChannelUpdateFreq[0] = updateFrequence; + data.ChannelUpdateFreq[1] = updateFrequence; + data.ChannelUpdateFreq[2] = updateFrequence; + data.ChannelUpdateFreq[3] = updateFrequence; + break; + default: + break; + } + actSettings->setData(data); + addModifiedObject(actSettings, tr("Writing actuator settings")); + break; + } + case VehicleConfigurationSource::VEHICLE_FIXEDWING: + case VehicleConfigurationSource::VEHICLE_HELI: + case VehicleConfigurationSource::VEHICLE_SURFACE: + // TODO: Implement settings for other vehicle types? + break; + default: + break; } } void VehicleConfigurationHelper::applyFlighModeConfiguration() { - ManualControlSettings* controlSettings = ManualControlSettings::GetInstance(m_uavoManager); + ManualControlSettings *controlSettings = ManualControlSettings::GetInstance(m_uavoManager); + Q_ASSERT(controlSettings); ManualControlSettings::DataFields data = controlSettings->getData(); @@ -322,31 +320,30 @@ void VehicleConfigurationHelper::applyFlighModeConfiguration() data.Stabilization3Settings[1] = ManualControlSettings::STABILIZATION3SETTINGS_RATE; data.Stabilization3Settings[2] = ManualControlSettings::STABILIZATION3SETTINGS_RATE; data.FlightModeNumber = 3; - data.FlightModePosition[0] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; - data.FlightModePosition[1] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; - data.FlightModePosition[2] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; - data.FlightModePosition[3] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; - data.FlightModePosition[4] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; - data.FlightModePosition[5] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[0] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[1] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[2] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[3] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[4] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[5] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; controlSettings->setData(data); addModifiedObject(controlSettings, tr("Writing flight mode settings")); } void VehicleConfigurationHelper::applySensorBiasConfiguration() { - if(m_configSource->isCalibrationPerformed()) - { + if (m_configSource->isCalibrationPerformed()) { accelGyroBias bias = m_configSource->getCalibrationBias(); float G = 9.81f; - switch(m_configSource->getControllerType()) { + switch (m_configSource->getControllerType()) { case VehicleConfigurationSource::CONTROLLER_CC: case VehicleConfigurationSource::CONTROLLER_CC3D: { const float ACCELERATION_SCALE = 0.004f * G; const float GYRO_SCALE = 100.0f; - AttitudeSettings* copterControlCalibration = AttitudeSettings::GetInstance(m_uavoManager); + AttitudeSettings *copterControlCalibration = AttitudeSettings::GetInstance(m_uavoManager); Q_ASSERT(copterControlCalibration); AttitudeSettings::DataFields data = copterControlCalibration->getData(); @@ -356,9 +353,9 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration() data.AccelBias[AttitudeSettings::ACCELBIAS_Y] += bias.m_accelerometerYBias / ACCELERATION_SCALE; data.AccelBias[AttitudeSettings::ACCELBIAS_Z] += (bias.m_accelerometerZBias + G) / ACCELERATION_SCALE; - data.GyroBias[AttitudeSettings::GYROBIAS_X] = -(bias.m_gyroXBias * GYRO_SCALE); - data.GyroBias[AttitudeSettings::GYROBIAS_Y] = -(bias.m_gyroYBias * GYRO_SCALE); - data.GyroBias[AttitudeSettings::GYROBIAS_Z] = -(bias.m_gyroZBias * GYRO_SCALE); + data.GyroBias[AttitudeSettings::GYROBIAS_X] = -(bias.m_gyroXBias * GYRO_SCALE); + data.GyroBias[AttitudeSettings::GYROBIAS_Y] = -(bias.m_gyroYBias * GYRO_SCALE); + data.GyroBias[AttitudeSettings::GYROBIAS_Z] = -(bias.m_gyroZBias * GYRO_SCALE); copterControlCalibration->setData(data); addModifiedObject(copterControlCalibration, tr("Writing gyro and accelerometer bias settings")); @@ -366,7 +363,7 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration() } case VehicleConfigurationSource::CONTROLLER_REVO: { - RevoCalibration* revolutionCalibration = RevoCalibration::GetInstance(m_uavoManager); + RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(m_uavoManager); Q_ASSERT(revolutionCalibration); RevoCalibration::DataFields data = revolutionCalibration->getData(); @@ -376,16 +373,16 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration() data.accel_bias[RevoCalibration::ACCEL_BIAS_Y] += bias.m_accelerometerYBias; data.accel_bias[RevoCalibration::ACCEL_BIAS_Z] += bias.m_accelerometerZBias + G; - data.gyro_bias[RevoCalibration::GYRO_BIAS_X] = bias.m_gyroXBias; - data.gyro_bias[RevoCalibration::GYRO_BIAS_Y] = bias.m_gyroYBias; - data.gyro_bias[RevoCalibration::GYRO_BIAS_Z] = bias.m_gyroZBias; + data.gyro_bias[RevoCalibration::GYRO_BIAS_X] = bias.m_gyroXBias; + data.gyro_bias[RevoCalibration::GYRO_BIAS_Y] = bias.m_gyroYBias; + data.gyro_bias[RevoCalibration::GYRO_BIAS_Z] = bias.m_gyroZBias; revolutionCalibration->setData(data); addModifiedObject(revolutionCalibration, tr("Writing gyro and accelerometer bias settings")); break; } default: - //Something went terribly wrong. + // Something went terribly wrong. break; } } @@ -393,7 +390,8 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration() void VehicleConfigurationHelper::applyStabilizationConfiguration() { - StabilizationSettings *stabSettings = StabilizationSettings::GetInstance(m_uavoManager); + StabilizationSettings *stabSettings = StabilizationSettings::GetInstance(m_uavoManager); + Q_ASSERT(stabSettings); StabilizationSettings::DataFields data = stabSettings->getData(); @@ -405,13 +403,14 @@ void VehicleConfigurationHelper::applyStabilizationConfiguration() void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings channels[]) { // Set all mixer data - MixerSettings* mSettings = MixerSettings::GetInstance(m_uavoManager); + MixerSettings *mSettings = MixerSettings::GetInstance(m_uavoManager); + Q_ASSERT(mSettings); // Set Mixer types and values - QString mixerTypePattern = "Mixer%1Type"; + QString mixerTypePattern = "Mixer%1Type"; QString mixerVectorPattern = "Mixer%1Vector"; - for(int i = 0; i < 10; i++) { + for (int i = 0; i < 10; i++) { UAVObjectField *field = mSettings->getField(mixerTypePattern.arg(i + 1)); Q_ASSERT(field); field->setValue(field->getOptions().at(channels[i].type)); @@ -422,18 +421,18 @@ void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings ch field->setValue((channels[i].throttle2 * 127) / 100, 1); field->setValue((channels[i].roll * 127) / 100, 2); field->setValue((channels[i].pitch * 127) / 100, 3); - field->setValue((channels[i].yaw *127) / 100, 4); + field->setValue((channels[i].yaw * 127) / 100, 4); } // Apply updates mSettings->setData(mSettings->getData()); addModifiedObject(mSettings, tr("Writing mixer settings")); - } void VehicleConfigurationHelper::applyMultiGUISettings(SystemSettings::AirframeTypeOptions airframe, GUIConfigDataUnion guiConfig) { - SystemSettings * sSettings = SystemSettings::GetInstance(m_uavoManager); + SystemSettings *sSettings = SystemSettings::GetInstance(m_uavoManager); + Q_ASSERT(sSettings); SystemSettings::DataFields data = sSettings->getData(); data.AirframeType = airframe; @@ -449,40 +448,40 @@ void VehicleConfigurationHelper::applyMultiGUISettings(SystemSettings::AirframeT void VehicleConfigurationHelper::applyManualControlDefaults() { ManualControlSettings *mcSettings = ManualControlSettings::GetInstance(m_uavoManager); + Q_ASSERT(mcSettings); ManualControlSettings::DataFields cData = mcSettings->getData(); ManualControlSettings::ChannelGroupsOptions channelType = ManualControlSettings::CHANNELGROUPS_PWM; - switch(m_configSource->getInputType()) - { - case VehicleConfigurationSource::INPUT_PWM: - channelType = ManualControlSettings::CHANNELGROUPS_PWM; - break; - case VehicleConfigurationSource::INPUT_PPM: - channelType = ManualControlSettings::CHANNELGROUPS_PPM; - break; - case VehicleConfigurationSource::INPUT_SBUS: - channelType = ManualControlSettings::CHANNELGROUPS_SBUS; - break; - case VehicleConfigurationSource::INPUT_DSMX10: - case VehicleConfigurationSource::INPUT_DSMX11: - case VehicleConfigurationSource::INPUT_DSM2: - channelType = ManualControlSettings::CHANNELGROUPS_DSMMAINPORT; - break; - default: - break; + switch (m_configSource->getInputType()) { + case VehicleConfigurationSource::INPUT_PWM: + channelType = ManualControlSettings::CHANNELGROUPS_PWM; + break; + case VehicleConfigurationSource::INPUT_PPM: + channelType = ManualControlSettings::CHANNELGROUPS_PPM; + break; + case VehicleConfigurationSource::INPUT_SBUS: + channelType = ManualControlSettings::CHANNELGROUPS_SBUS; + break; + case VehicleConfigurationSource::INPUT_DSMX10: + case VehicleConfigurationSource::INPUT_DSMX11: + case VehicleConfigurationSource::INPUT_DSM2: + channelType = ManualControlSettings::CHANNELGROUPS_DSMMAINPORT; + break; + default: + break; } - cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_THROTTLE] = channelType; - cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_ROLL] = channelType; + cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_THROTTLE] = channelType; + cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_ROLL] = channelType; cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_YAW] = channelType; - cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_PITCH] = channelType; + cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_PITCH] = channelType; cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_FLIGHTMODE] = channelType; - cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_THROTTLE] = 1; - cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_ROLL] = 2; + cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_THROTTLE] = 1; + cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_ROLL] = 2; cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_YAW] = 3; - cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_PITCH] = 4; + cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_PITCH] = 4; cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_FLIGHTMODE] = 5; mcSettings->setData(cData); @@ -497,7 +496,7 @@ bool VehicleConfigurationHelper::saveChangesToController(bool save) ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Q_ASSERT(pm); - UAVObjectUtilManager* utilMngr = pm->getObject(); + UAVObjectUtilManager *utilMngr = pm->getObject(); Q_ASSERT(utilMngr); QTimer outerTimeoutTimer; @@ -506,48 +505,47 @@ bool VehicleConfigurationHelper::saveChangesToController(bool save) QTimer innerTimeoutTimer; innerTimeoutTimer.setSingleShot(true); - connect(utilMngr, SIGNAL(saveCompleted(int ,bool)), this, SLOT(uAVOTransactionCompleted(int, bool))); + connect(utilMngr, SIGNAL(saveCompleted(int, bool)), this, SLOT(uAVOTransactionCompleted(int, bool))); connect(&innerTimeoutTimer, SIGNAL(timeout()), &m_eventLoop, SLOT(quit())); connect(&outerTimeoutTimer, SIGNAL(timeout()), this, SLOT(saveChangesTimeout())); outerTimeoutTimer.start(OUTER_TIMEOUT); - for(int i = 0; i < m_modifiedObjects.count(); i++) { - QPair *objPair = m_modifiedObjects.at(i); + for (int i = 0; i < m_modifiedObjects.count(); i++) { + QPair *objPair = m_modifiedObjects.at(i); m_transactionOK = false; - UAVDataObject* obj = objPair->first; + UAVDataObject *obj = objPair->first; QString objDescription = objPair->second; - if(UAVObject::GetGcsAccess(obj->getMetadata()) != UAVObject::ACCESS_READONLY && obj->isSettings()) { - + if (UAVObject::GetGcsAccess(obj->getMetadata()) != UAVObject::ACCESS_READONLY && obj->isSettings()) { emit saveProgress(m_modifiedObjects.count() + 1, ++m_progress, objDescription); m_currentTransactionObjectID = obj->getObjID(); - connect(obj, SIGNAL(transactionCompleted(UAVObject* ,bool)), this, SLOT(uAVOTransactionCompleted(UAVObject*, bool))); - while(!m_transactionOK && !m_transactionTimeout) { + connect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(uAVOTransactionCompleted(UAVObject *, bool))); + while (!m_transactionOK && !m_transactionTimeout) { // Allow the transaction to take some time innerTimeoutTimer.start(INNER_TIMEOUT); // Set object updated obj->updated(); - if(!m_transactionOK) { + if (!m_transactionOK) { m_eventLoop.exec(); } innerTimeoutTimer.stop(); } - disconnect(obj, SIGNAL(transactionCompleted(UAVObject* ,bool)), this, SLOT(uAVOTransactionCompleted(UAVObject*, bool))); - if(m_transactionOK) { + disconnect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(uAVOTransactionCompleted(UAVObject *, bool))); + if (m_transactionOK) { qDebug() << "Object " << obj->getName() << " was successfully updated."; - if(save) { + if (save) { m_transactionOK = false; m_currentTransactionObjectID = obj->getObjID(); // Try to save until success or timeout - while(!m_transactionOK && !m_transactionTimeout) { + while (!m_transactionOK && !m_transactionTimeout) { // Allow the transaction to take some time innerTimeoutTimer.start(INNER_TIMEOUT); // Persist object in controller utilMngr->saveObjectToSD(obj); - if(!m_transactionOK) { + if (!m_transactionOK) { m_eventLoop.exec(); } innerTimeoutTimer.stop(); @@ -556,17 +554,15 @@ bool VehicleConfigurationHelper::saveChangesToController(bool save) } } - if(!m_transactionOK) { + if (!m_transactionOK) { qDebug() << "Transaction timed out when trying to save: " << obj->getName(); - } - else { + } else { qDebug() << "Object " << obj->getName() << " was successfully saved."; } - } - else { + } else { qDebug() << "Trying to save a UAVDataObject that is read only or is not a settings object."; } - if(m_transactionTimeout) { + if (m_transactionTimeout) { qDebug() << "Transaction timed out when trying to save " << m_modifiedObjects.count() << " objects."; break; } @@ -584,8 +580,7 @@ bool VehicleConfigurationHelper::saveChangesToController(bool save) void VehicleConfigurationHelper::uAVOTransactionCompleted(int oid, bool success) { - if(oid == m_currentTransactionObjectID) - { + if (oid == m_currentTransactionObjectID) { m_transactionOK = success; m_eventLoop.quit(); } @@ -593,7 +588,7 @@ void VehicleConfigurationHelper::uAVOTransactionCompleted(int oid, bool success) void VehicleConfigurationHelper::uAVOTransactionCompleted(UAVObject *object, bool success) { - if(object) { + if (object) { uAVOTransactionCompleted(object->getObjID(), success); } } @@ -608,7 +603,7 @@ void VehicleConfigurationHelper::saveChangesTimeout() void VehicleConfigurationHelper::resetVehicleConfig() { // Reset all vehicle data - MixerSettings* mSettings = MixerSettings::GetInstance(m_uavoManager); + MixerSettings *mSettings = MixerSettings::GetInstance(m_uavoManager); // Reset feed forward, accel times etc mSettings->setFeedForward(0.0f); @@ -618,41 +613,42 @@ void VehicleConfigurationHelper::resetVehicleConfig() // Reset throttle curves QString throttlePattern = "ThrottleCurve%1"; - for(int i = 1; i <= 2; i++) { + for (int i = 1; i <= 2; i++) { UAVObjectField *field = mSettings->getField(throttlePattern.arg(i)); Q_ASSERT(field); - for(quint32 i = 0; i < field->getNumElements(); i++){ - field->setValue(i * ( 1.0f / (field->getNumElements() - 1)), i); + for (quint32 i = 0; i < field->getNumElements(); i++) { + field->setValue(i * (1.0f / (field->getNumElements() - 1)), i); } } // Reset Mixer types and values - QString mixerTypePattern = "Mixer%1Type"; + QString mixerTypePattern = "Mixer%1Type"; QString mixerVectorPattern = "Mixer%1Vector"; - for(int i = 1; i <= 10; i++) { + for (int i = 1; i <= 10; i++) { UAVObjectField *field = mSettings->getField(mixerTypePattern.arg(i)); Q_ASSERT(field); field->setValue(field->getOptions().at(0)); field = mSettings->getField(mixerVectorPattern.arg(i)); Q_ASSERT(field); - for(quint32 i = 0; i < field->getNumElements(); i++){ + for (quint32 i = 0; i < field->getNumElements(); i++) { field->setValue(0, i); } } // Apply updates - //mSettings->setData(mSettings->getData()); + // mSettings->setData(mSettings->getData()); addModifiedObject(mSettings, tr("Preparing mixer settings")); } void VehicleConfigurationHelper::resetGUIData() { - SystemSettings * sSettings = SystemSettings::GetInstance(m_uavoManager); + SystemSettings *sSettings = SystemSettings::GetInstance(m_uavoManager); + Q_ASSERT(sSettings); SystemSettings::DataFields data = sSettings->getData(); data.AirframeType = SystemSettings::AIRFRAMETYPE_CUSTOM; - for(quint32 i = 0; i < SystemSettings::GUICONFIGDATA_NUMELEM; i++) { + for (quint32 i = 0; i < SystemSettings::GUICONFIGDATA_NUMELEM; i++) { data.GUIConfigData[i] = 0; } sSettings->setData(data); @@ -670,37 +666,37 @@ void VehicleConfigurationHelper::setupTriCopter() mixerChannelSettings channels[10]; GUIConfigDataUnion guiSettings = getGUIConfigData(); - channels[0].type = MIXER_TYPE_MOTOR; + channels[0].type = MIXER_TYPE_MOTOR; channels[0].throttle1 = 100; channels[0].throttle2 = 0; - channels[0].roll = 100; - channels[0].pitch = 50; + channels[0].roll = 100; + channels[0].pitch = 50; channels[0].yaw = 0; - channels[1].type = MIXER_TYPE_MOTOR; + channels[1].type = MIXER_TYPE_MOTOR; channels[1].throttle1 = 100; channels[1].throttle2 = 0; - channels[1].roll = -100; - channels[1].pitch = 50; + channels[1].roll = -100; + channels[1].pitch = 50; channels[1].yaw = 0; - channels[2].type = MIXER_TYPE_MOTOR; + channels[2].type = MIXER_TYPE_MOTOR; channels[2].throttle1 = 100; channels[2].throttle2 = 0; - channels[2].roll = 0; - channels[2].pitch = -100; + channels[2].roll = 0; + channels[2].pitch = -100; channels[2].yaw = 0; - channels[3].type = MIXER_TYPE_SERVO; + channels[3].type = MIXER_TYPE_SERVO; channels[3].throttle1 = 0; channels[3].throttle2 = 0; - channels[3].roll = 0; - channels[3].pitch = 0; + channels[3].roll = 0; + channels[3].pitch = 0; channels[3].yaw = 100; guiSettings.multi.VTOLMotorNW = 1; guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorS = 3; + guiSettings.multi.VTOLMotorS = 3; guiSettings.multi.TRIYaw = 4; applyMixerConfiguration(channels); @@ -711,12 +707,13 @@ GUIConfigDataUnion VehicleConfigurationHelper::getGUIConfigData() { GUIConfigDataUnion configData; - SystemSettings * systemSettings = SystemSettings::GetInstance(m_uavoManager); + SystemSettings *systemSettings = SystemSettings::GetInstance(m_uavoManager); + Q_ASSERT(systemSettings); SystemSettings::DataFields systemSettingsData = systemSettings->getData(); - for(int i = 0; i < (int)(SystemSettings::GUICONFIGDATA_NUMELEM); i++) { - configData.UAVObject[i] = 0; //systemSettingsData.GUIConfigData[i]; + for (int i = 0; i < (int)(SystemSettings::GUICONFIGDATA_NUMELEM); i++) { + configData.UAVObject[i] = 0; // systemSettingsData.GUIConfigData[i]; } return configData; @@ -728,84 +725,85 @@ void VehicleConfigurationHelper::setupQuadCopter() GUIConfigDataUnion guiSettings = getGUIConfigData(); SystemSettings::AirframeTypeOptions frame; - switch(m_configSource->getVehicleSubType()) + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: { - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: { - frame = SystemSettings::AIRFRAMETYPE_QUADP; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 0; - channels[0].pitch = 100; - channels[0].yaw = -50; + frame = SystemSettings::AIRFRAMETYPE_QUADP; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 0; + channels[0].pitch = 100; + channels[0].yaw = -50; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = -100; - channels[1].pitch = 0; - channels[1].yaw = 50; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -100; + channels[1].pitch = 0; + channels[1].yaw = 50; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = 0; - channels[2].pitch = -100; - channels[2].yaw = -50; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = 0; + channels[2].pitch = -100; + channels[2].yaw = -50; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = 100; - channels[3].pitch = 0; - channels[3].yaw = 50; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = 100; + channels[3].pitch = 0; + channels[3].yaw = 50; - guiSettings.multi.VTOLMotorN = 1; - guiSettings.multi.VTOLMotorE = 2; - guiSettings.multi.VTOLMotorS = 3; - guiSettings.multi.VTOLMotorW = 4; + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorE = 2; + guiSettings.multi.VTOLMotorS = 3; + guiSettings.multi.VTOLMotorW = 4; - break; - } - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: { - frame = SystemSettings::AIRFRAMETYPE_QUADX; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 50; - channels[0].pitch = 50; - channels[0].yaw = -50; + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: + { + frame = SystemSettings::AIRFRAMETYPE_QUADX; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 50; + channels[0].pitch = 50; + channels[0].yaw = -50; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = -50; - channels[1].pitch = 50; - channels[1].yaw = 50; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -50; + channels[1].pitch = 50; + channels[1].yaw = 50; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -50; - channels[2].pitch = -50; - channels[2].yaw = -50; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -50; + channels[2].pitch = -50; + channels[2].yaw = -50; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = 50; - channels[3].pitch = -50; - channels[3].yaw = 50; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = 50; + channels[3].pitch = -50; + channels[3].yaw = 50; - guiSettings.multi.VTOLMotorNW = 1; - guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorSE = 3; - guiSettings.multi.VTOLMotorSW = 4; + guiSettings.multi.VTOLMotorNW = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorSE = 3; + guiSettings.multi.VTOLMotorSW = 4; - break; - } - default: - break; + break; + } + default: + break; } applyMixerConfiguration(channels); applyMultiGUISettings(frame, guiSettings); @@ -817,172 +815,174 @@ void VehicleConfigurationHelper::setupHexaCopter() GUIConfigDataUnion guiSettings = getGUIConfigData(); SystemSettings::AirframeTypeOptions frame; - switch(m_configSource->getVehicleSubType()) + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::MULTI_ROTOR_HEXA: { - case VehicleConfigurationSource::MULTI_ROTOR_HEXA: { - frame = SystemSettings::AIRFRAMETYPE_HEXA; + frame = SystemSettings::AIRFRAMETYPE_HEXA; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 0; - channels[0].pitch = 33; - channels[0].yaw = -33; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 0; + channels[0].pitch = 33; + channels[0].yaw = -33; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = -50; - channels[1].pitch = 33; - channels[1].yaw = 33; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -50; + channels[1].pitch = 33; + channels[1].yaw = 33; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -50; - channels[2].pitch = -33; - channels[2].yaw = -33; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -50; + channels[2].pitch = -33; + channels[2].yaw = -33; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = 0; - channels[3].pitch = -33; - channels[3].yaw = 33; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = 0; + channels[3].pitch = -33; + channels[3].yaw = 33; - channels[4].type = MIXER_TYPE_MOTOR; - channels[4].throttle1 = 100; - channels[4].throttle2 = 0; - channels[4].roll = 50; - channels[4].pitch = -33; - channels[4].yaw = -33; + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 50; + channels[4].pitch = -33; + channels[4].yaw = -33; - channels[5].type = MIXER_TYPE_MOTOR; - channels[5].throttle1 = 100; - channels[5].throttle2 = 0; - channels[5].roll = 50; - channels[5].pitch = 33; - channels[5].yaw = 33; + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 50; + channels[5].pitch = 33; + channels[5].yaw = 33; - guiSettings.multi.VTOLMotorN = 1; - guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorSE = 3; - guiSettings.multi.VTOLMotorS = 4; - guiSettings.multi.VTOLMotorSW = 5; - guiSettings.multi.VTOLMotorNW = 6; + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorSE = 3; + guiSettings.multi.VTOLMotorS = 4; + guiSettings.multi.VTOLMotorSW = 5; + guiSettings.multi.VTOLMotorNW = 6; - break; - } - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: { - frame = SystemSettings::AIRFRAMETYPE_HEXACOAX; + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: + { + frame = SystemSettings::AIRFRAMETYPE_HEXACOAX; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 100; - channels[0].pitch = 25; - channels[0].yaw = -66; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 100; + channels[0].pitch = 25; + channels[0].yaw = -66; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = 100; - channels[1].pitch = 25; - channels[1].yaw = 66; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = 100; + channels[1].pitch = 25; + channels[1].yaw = 66; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -100; - channels[2].pitch = 25; - channels[2].yaw = -66; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -100; + channels[2].pitch = 25; + channels[2].yaw = -66; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = -100; - channels[3].pitch = 25; - channels[3].yaw = 66; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -100; + channels[3].pitch = 25; + channels[3].yaw = 66; - channels[4].type = MIXER_TYPE_MOTOR; - channels[4].throttle1 = 100; - channels[4].throttle2 = 0; - channels[4].roll = 0; - channels[4].pitch = -50; - channels[4].yaw = -66; + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 0; + channels[4].pitch = -50; + channels[4].yaw = -66; - channels[5].type = MIXER_TYPE_MOTOR; - channels[5].throttle1 = 100; - channels[5].throttle2 = 0; - channels[5].roll = 0; - channels[5].pitch = -50; - channels[5].yaw = 66; + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 0; + channels[5].pitch = -50; + channels[5].yaw = 66; - guiSettings.multi.VTOLMotorNW = 1; - guiSettings.multi.VTOLMotorW = 2; - guiSettings.multi.VTOLMotorNE = 3; - guiSettings.multi.VTOLMotorE = 4; - guiSettings.multi.VTOLMotorS = 5; - guiSettings.multi.VTOLMotorSE = 6; + guiSettings.multi.VTOLMotorNW = 1; + guiSettings.multi.VTOLMotorW = 2; + guiSettings.multi.VTOLMotorNE = 3; + guiSettings.multi.VTOLMotorE = 4; + guiSettings.multi.VTOLMotorS = 5; + guiSettings.multi.VTOLMotorSE = 6; - break; - } - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: { - frame = SystemSettings::AIRFRAMETYPE_HEXAX; + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: + { + frame = SystemSettings::AIRFRAMETYPE_HEXAX; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = -33; - channels[0].pitch = 50; - channels[0].yaw = -33; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = -33; + channels[0].pitch = 50; + channels[0].yaw = -33; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = -33; - channels[1].pitch = 0; - channels[1].yaw = 33; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -33; + channels[1].pitch = 0; + channels[1].yaw = 33; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -33; - channels[2].pitch = -50; - channels[2].yaw = -33; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -33; + channels[2].pitch = -50; + channels[2].yaw = -33; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = -33; - channels[3].pitch = -50; - channels[3].yaw = 33; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -33; + channels[3].pitch = -50; + channels[3].yaw = 33; - channels[4].type = MIXER_TYPE_MOTOR; - channels[4].throttle1 = 100; - channels[4].throttle2 = 0; - channels[4].roll = 33; - channels[4].pitch = 0; - channels[4].yaw = -33; + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 33; + channels[4].pitch = 0; + channels[4].yaw = -33; - channels[5].type = MIXER_TYPE_MOTOR; - channels[5].throttle1 = 100; - channels[5].throttle2 = 0; - channels[5].roll = 33; - channels[5].pitch = 50; - channels[5].yaw = -33; + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 33; + channels[5].pitch = 50; + channels[5].yaw = -33; - guiSettings.multi.VTOLMotorNE = 1; - guiSettings.multi.VTOLMotorE = 2; - guiSettings.multi.VTOLMotorSE = 3; - guiSettings.multi.VTOLMotorSW = 4; - guiSettings.multi.VTOLMotorW = 5; - guiSettings.multi.VTOLMotorNW = 6; + guiSettings.multi.VTOLMotorNE = 1; + guiSettings.multi.VTOLMotorE = 2; + guiSettings.multi.VTOLMotorSE = 3; + guiSettings.multi.VTOLMotorSW = 4; + guiSettings.multi.VTOLMotorW = 5; + guiSettings.multi.VTOLMotorNW = 6; - break; - } - default: - break; + break; + } + default: + break; } applyMixerConfiguration(channels); applyMultiGUISettings(frame, guiSettings); @@ -994,289 +994,292 @@ void VehicleConfigurationHelper::setupOctoCopter() GUIConfigDataUnion guiSettings = getGUIConfigData(); SystemSettings::AirframeTypeOptions frame; - switch(m_configSource->getVehicleSubType()) + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::MULTI_ROTOR_OCTO: { - case VehicleConfigurationSource::MULTI_ROTOR_OCTO: { - frame = SystemSettings::AIRFRAMETYPE_OCTO; + frame = SystemSettings::AIRFRAMETYPE_OCTO; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 0; - channels[0].pitch = 33; - channels[0].yaw = -25; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 0; + channels[0].pitch = 33; + channels[0].yaw = -25; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = -33; - channels[1].pitch = 33; - channels[1].yaw = 25; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -33; + channels[1].pitch = 33; + channels[1].yaw = 25; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -33; - channels[2].pitch = 0; - channels[2].yaw = -25; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -33; + channels[2].pitch = 0; + channels[2].yaw = -25; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = -33; - channels[3].pitch = -33; - channels[3].yaw = 25; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -33; + channels[3].pitch = -33; + channels[3].yaw = 25; - channels[4].type = MIXER_TYPE_MOTOR; - channels[4].throttle1 = 100; - channels[4].throttle2 = 0; - channels[4].roll = 0; - channels[4].pitch = -33; - channels[4].yaw = -25; + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 0; + channels[4].pitch = -33; + channels[4].yaw = -25; - channels[5].type = MIXER_TYPE_MOTOR; - channels[5].throttle1 = 100; - channels[5].throttle2 = 0; - channels[5].roll = 33; - channels[5].pitch = -33; - channels[5].yaw = 25; + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 33; + channels[5].pitch = -33; + channels[5].yaw = 25; - channels[6].type = MIXER_TYPE_MOTOR; - channels[6].throttle1 = 100; - channels[6].throttle2 = 0; - channels[6].roll = 33; - channels[6].pitch = 0; - channels[6].yaw = -25; + channels[6].type = MIXER_TYPE_MOTOR; + channels[6].throttle1 = 100; + channels[6].throttle2 = 0; + channels[6].roll = 33; + channels[6].pitch = 0; + channels[6].yaw = -25; - channels[7].type = MIXER_TYPE_MOTOR; - channels[7].throttle1 = 100; - channels[7].throttle2 = 0; - channels[7].roll = 33; - channels[7].pitch = 33; - channels[7].yaw = 25; + channels[7].type = MIXER_TYPE_MOTOR; + channels[7].throttle1 = 100; + channels[7].throttle2 = 0; + channels[7].roll = 33; + channels[7].pitch = 33; + channels[7].yaw = 25; - guiSettings.multi.VTOLMotorN = 1; - guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorE = 3; - guiSettings.multi.VTOLMotorSE = 4; - guiSettings.multi.VTOLMotorS = 5; - guiSettings.multi.VTOLMotorSW = 6; - guiSettings.multi.VTOLMotorW = 7; - guiSettings.multi.VTOLMotorNW = 8; + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorE = 3; + guiSettings.multi.VTOLMotorSE = 4; + guiSettings.multi.VTOLMotorS = 5; + guiSettings.multi.VTOLMotorSW = 6; + guiSettings.multi.VTOLMotorW = 7; + guiSettings.multi.VTOLMotorNW = 8; - break; - } - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: { - frame = SystemSettings::AIRFRAMETYPE_OCTOCOAXX; + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: + { + frame = SystemSettings::AIRFRAMETYPE_OCTOCOAXX; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 50; - channels[0].pitch = 50; - channels[0].yaw = -50; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 50; + channels[0].pitch = 50; + channels[0].yaw = -50; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = 50; - channels[1].pitch = 50; - channels[1].yaw = 50; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = 50; + channels[1].pitch = 50; + channels[1].yaw = 50; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -50; - channels[2].pitch = 50; - channels[2].yaw = -50; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -50; + channels[2].pitch = 50; + channels[2].yaw = -50; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = -50; - channels[3].pitch = 50; - channels[3].yaw = 50; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -50; + channels[3].pitch = 50; + channels[3].yaw = 50; - channels[4].type = MIXER_TYPE_MOTOR; - channels[4].throttle1 = 100; - channels[4].throttle2 = 0; - channels[4].roll = -50; - channels[4].pitch = -50; - channels[4].yaw = -50; + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = -50; + channels[4].pitch = -50; + channels[4].yaw = -50; - channels[5].type = MIXER_TYPE_MOTOR; - channels[5].throttle1 = 100; - channels[5].throttle2 = 0; - channels[5].roll = -50; - channels[5].pitch = -50; - channels[5].yaw = 50; + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = -50; + channels[5].pitch = -50; + channels[5].yaw = 50; - channels[6].type = MIXER_TYPE_MOTOR; - channels[6].throttle1 = 100; - channels[6].throttle2 = 0; - channels[6].roll = 50; - channels[6].pitch = -50; - channels[6].yaw = -50; + channels[6].type = MIXER_TYPE_MOTOR; + channels[6].throttle1 = 100; + channels[6].throttle2 = 0; + channels[6].roll = 50; + channels[6].pitch = -50; + channels[6].yaw = -50; - channels[7].type = MIXER_TYPE_MOTOR; - channels[7].throttle1 = 100; - channels[7].throttle2 = 0; - channels[7].roll = 50; - channels[7].pitch = -50; - channels[7].yaw = 50; + channels[7].type = MIXER_TYPE_MOTOR; + channels[7].throttle1 = 100; + channels[7].throttle2 = 0; + channels[7].roll = 50; + channels[7].pitch = -50; + channels[7].yaw = 50; - guiSettings.multi.VTOLMotorNW = 1; - guiSettings.multi.VTOLMotorN = 2; - guiSettings.multi.VTOLMotorNE = 3; - guiSettings.multi.VTOLMotorE = 4; - guiSettings.multi.VTOLMotorSE = 5; - guiSettings.multi.VTOLMotorS = 6; - guiSettings.multi.VTOLMotorSW = 7; - guiSettings.multi.VTOLMotorW = 8; + guiSettings.multi.VTOLMotorNW = 1; + guiSettings.multi.VTOLMotorN = 2; + guiSettings.multi.VTOLMotorNE = 3; + guiSettings.multi.VTOLMotorE = 4; + guiSettings.multi.VTOLMotorSE = 5; + guiSettings.multi.VTOLMotorS = 6; + guiSettings.multi.VTOLMotorSW = 7; + guiSettings.multi.VTOLMotorW = 8; - break; - } - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: { - frame = SystemSettings::AIRFRAMETYPE_OCTOCOAXP; + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: + { + frame = SystemSettings::AIRFRAMETYPE_OCTOCOAXP; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 0; - channels[0].pitch = 100; - channels[0].yaw = -50; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 0; + channels[0].pitch = 100; + channels[0].yaw = -50; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = 0; - channels[1].pitch = 100; - channels[1].yaw = 50; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = 0; + channels[1].pitch = 100; + channels[1].yaw = 50; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -100; - channels[2].pitch = 0; - channels[2].yaw = -50; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -100; + channels[2].pitch = 0; + channels[2].yaw = -50; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = -100; - channels[3].pitch = 0; - channels[3].yaw = 50; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -100; + channels[3].pitch = 0; + channels[3].yaw = 50; - channels[4].type = MIXER_TYPE_MOTOR; - channels[4].throttle1 = 100; - channels[4].throttle2 = 0; - channels[4].roll = 0; - channels[4].pitch = -100; - channels[4].yaw = -50; + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 0; + channels[4].pitch = -100; + channels[4].yaw = -50; - channels[5].type = MIXER_TYPE_MOTOR; - channels[5].throttle1 = 100; - channels[5].throttle2 = 0; - channels[5].roll = 0; - channels[5].pitch = -100; - channels[5].yaw = 50; + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 0; + channels[5].pitch = -100; + channels[5].yaw = 50; - channels[6].type = MIXER_TYPE_MOTOR; - channels[6].throttle1 = 100; - channels[6].throttle2 = 0; - channels[6].roll = 100; - channels[6].pitch = 0; - channels[6].yaw = -50; + channels[6].type = MIXER_TYPE_MOTOR; + channels[6].throttle1 = 100; + channels[6].throttle2 = 0; + channels[6].roll = 100; + channels[6].pitch = 0; + channels[6].yaw = -50; - channels[7].type = MIXER_TYPE_MOTOR; - channels[7].throttle1 = 100; - channels[7].throttle2 = 0; - channels[7].roll = 100; - channels[7].pitch = 0; - channels[7].yaw = 50; + channels[7].type = MIXER_TYPE_MOTOR; + channels[7].throttle1 = 100; + channels[7].throttle2 = 0; + channels[7].roll = 100; + channels[7].pitch = 0; + channels[7].yaw = 50; - guiSettings.multi.VTOLMotorN = 1; - guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorE = 3; - guiSettings.multi.VTOLMotorSE = 4; - guiSettings.multi.VTOLMotorS = 5; - guiSettings.multi.VTOLMotorSW = 6; - guiSettings.multi.VTOLMotorW = 7; - guiSettings.multi.VTOLMotorNW = 8; + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorE = 3; + guiSettings.multi.VTOLMotorSE = 4; + guiSettings.multi.VTOLMotorS = 5; + guiSettings.multi.VTOLMotorSW = 6; + guiSettings.multi.VTOLMotorW = 7; + guiSettings.multi.VTOLMotorNW = 8; - break; - } - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: { - frame = SystemSettings::AIRFRAMETYPE_OCTOV; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = -25; - channels[0].pitch = 8; - channels[0].yaw = -25; + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: + { + frame = SystemSettings::AIRFRAMETYPE_OCTOV; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = -25; + channels[0].pitch = 8; + channels[0].yaw = -25; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = -25; - channels[1].pitch = 25; - channels[1].yaw = 25; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -25; + channels[1].pitch = 25; + channels[1].yaw = 25; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -25; - channels[2].pitch = -25; - channels[2].yaw = -25; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -25; + channels[2].pitch = -25; + channels[2].yaw = -25; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = -25; - channels[3].pitch = -8; - channels[3].yaw = 25; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -25; + channels[3].pitch = -8; + channels[3].yaw = 25; - channels[4].type = MIXER_TYPE_MOTOR; - channels[4].throttle1 = 100; - channels[4].throttle2 = 0; - channels[4].roll = 25; - channels[4].pitch = -8; - channels[4].yaw = -25; + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 25; + channels[4].pitch = -8; + channels[4].yaw = -25; - channels[5].type = MIXER_TYPE_MOTOR; - channels[5].throttle1 = 100; - channels[5].throttle2 = 0; - channels[5].roll = 25; - channels[5].pitch = -25; - channels[5].yaw = 25; + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 25; + channels[5].pitch = -25; + channels[5].yaw = 25; - channels[6].type = MIXER_TYPE_MOTOR; - channels[6].throttle1 = 100; - channels[6].throttle2 = 0; - channels[6].roll = 25; - channels[6].pitch = 25; - channels[6].yaw = -25; + channels[6].type = MIXER_TYPE_MOTOR; + channels[6].throttle1 = 100; + channels[6].throttle2 = 0; + channels[6].roll = 25; + channels[6].pitch = 25; + channels[6].yaw = -25; - channels[7].type = MIXER_TYPE_MOTOR; - channels[7].throttle1 = 100; - channels[7].throttle2 = 0; - channels[7].roll = 25; - channels[7].pitch = 8; - channels[7].yaw = 25; + channels[7].type = MIXER_TYPE_MOTOR; + channels[7].throttle1 = 100; + channels[7].throttle2 = 0; + channels[7].roll = 25; + channels[7].pitch = 8; + channels[7].yaw = 25; - guiSettings.multi.VTOLMotorN = 1; - guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorE = 3; - guiSettings.multi.VTOLMotorSE = 4; - guiSettings.multi.VTOLMotorS = 5; - guiSettings.multi.VTOLMotorSW = 6; - guiSettings.multi.VTOLMotorW = 7; - guiSettings.multi.VTOLMotorNW = 8; + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorE = 3; + guiSettings.multi.VTOLMotorSE = 4; + guiSettings.multi.VTOLMotorS = 5; + guiSettings.multi.VTOLMotorSW = 6; + guiSettings.multi.VTOLMotorW = 7; + guiSettings.multi.VTOLMotorNW = 8; - break; - } - default: - break; + break; + } + default: + break; } applyMixerConfiguration(channels); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h index d890db66a..1ed1aa576 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h @@ -50,12 +50,11 @@ struct mixerChannelSettings { : type(t), throttle1(th1), throttle2(th2), roll(r), pitch(p), yaw(y) {} }; -class VehicleConfigurationHelper : public QObject -{ +class VehicleConfigurationHelper : public QObject { Q_OBJECT public: - VehicleConfigurationHelper(VehicleConfigurationSource* configSource); + VehicleConfigurationHelper(VehicleConfigurationSource *configSource); bool setupVehicle(bool save = true); bool setupHardwareSettings(bool save = true); static const qint16 LEGACY_ESC_FREQUENCE; @@ -66,15 +65,15 @@ signals: private: static const int MIXER_TYPE_DISABLED = 0; - static const int MIXER_TYPE_MOTOR = 1; - static const int MIXER_TYPE_SERVO = 2; + static const int MIXER_TYPE_MOTOR = 1; + static const int MIXER_TYPE_SERVO = 2; static const float DEFAULT_ENABLED_ACCEL_TAU = 0.1; VehicleConfigurationSource *m_configSource; UAVObjectManager *m_uavoManager; - QList* > m_modifiedObjects; - void addModifiedObject(UAVDataObject* object, QString description); + QList * > m_modifiedObjects; + void addModifiedObject(UAVDataObject *object, QString description); void clearModifiedObjects(); void applyHardwareConfiguration(); @@ -106,11 +105,9 @@ private: void setupOctoCopter(); private slots: - void uAVOTransactionCompleted(UAVObject* object, bool success); + void uAVOTransactionCompleted(UAVObject *object, bool success); void uAVOTransactionCompleted(int oid, bool success); void saveChangesTimeout(); - - }; #endif // VEHICLECONFIGURATIONHELPER_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.cpp index 3acb2400d..75b92a023 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.cpp @@ -28,5 +28,4 @@ #include "vehicleconfigurationsource.h" VehicleConfigurationSource::VehicleConfigurationSource() -{ -} +{} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h index a51dea708..98f06a991 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -46,35 +46,34 @@ struct actuatorChannelSettings { quint16 channelNeutral; quint16 channelMax; - //Default values - actuatorChannelSettings(): channelMin(1000), channelNeutral(1000), channelMax(1900) {} + // Default values + actuatorChannelSettings() : channelMin(1000), channelNeutral(1000), channelMax(1900) {} }; -class VehicleConfigurationSource -{ +class VehicleConfigurationSource { public: VehicleConfigurationSource(); - enum CONTROLLER_TYPE {CONTROLLER_UNKNOWN, CONTROLLER_CC, CONTROLLER_CC3D, CONTROLLER_REVO, CONTROLLER_OPLINK}; - enum VEHICLE_TYPE {VEHICLE_UNKNOWN, VEHICLE_MULTI, VEHICLE_FIXEDWING, VEHICLE_HELI, VEHICLE_SURFACE}; - enum VEHICLE_SUB_TYPE {MULTI_ROTOR_UNKNOWN, MULTI_ROTOR_TRI_Y, MULTI_ROTOR_QUAD_X, MULTI_ROTOR_QUAD_PLUS, - MULTI_ROTOR_HEXA, MULTI_ROTOR_HEXA_H, MULTI_ROTOR_HEXA_COAX_Y, MULTI_ROTOR_OCTO, - MULTI_ROTOR_OCTO_V, MULTI_ROTOR_OCTO_COAX_X, MULTI_ROTOR_OCTO_COAX_PLUS, FIXED_WING_AILERON, - FIXED_WING_VTAIL, HELI_CCPM}; - enum ESC_TYPE {ESC_RAPID, ESC_LEGACY, ESC_UNKNOWN}; - enum INPUT_TYPE {INPUT_PWM, INPUT_PPM, INPUT_SBUS, INPUT_DSMX10, INPUT_DSMX11, INPUT_DSM2, INPUT_UNKNOWN}; + enum CONTROLLER_TYPE { CONTROLLER_UNKNOWN, CONTROLLER_CC, CONTROLLER_CC3D, CONTROLLER_REVO, CONTROLLER_OPLINK }; + enum VEHICLE_TYPE { VEHICLE_UNKNOWN, VEHICLE_MULTI, VEHICLE_FIXEDWING, VEHICLE_HELI, VEHICLE_SURFACE }; + enum VEHICLE_SUB_TYPE { MULTI_ROTOR_UNKNOWN, MULTI_ROTOR_TRI_Y, MULTI_ROTOR_QUAD_X, MULTI_ROTOR_QUAD_PLUS, + MULTI_ROTOR_HEXA, MULTI_ROTOR_HEXA_H, MULTI_ROTOR_HEXA_COAX_Y, MULTI_ROTOR_OCTO, + MULTI_ROTOR_OCTO_V, MULTI_ROTOR_OCTO_COAX_X, MULTI_ROTOR_OCTO_COAX_PLUS, FIXED_WING_AILERON, + FIXED_WING_VTAIL, HELI_CCPM }; + enum ESC_TYPE { ESC_RAPID, ESC_LEGACY, ESC_UNKNOWN }; + enum INPUT_TYPE { INPUT_PWM, INPUT_PPM, INPUT_SBUS, INPUT_DSMX10, INPUT_DSMX11, INPUT_DSM2, INPUT_UNKNOWN }; - enum GPS_SETTING {GPS_UBX, GPS_NMEA, GPS_DISABLED}; - enum RADIO_SETTING {RADIO_TELEMETRY, RADIO_DISABLED}; + enum GPS_SETTING { GPS_UBX, GPS_NMEA, GPS_DISABLED }; + enum RADIO_SETTING { RADIO_TELEMETRY, RADIO_DISABLED }; - virtual VehicleConfigurationSource::CONTROLLER_TYPE getControllerType() const = 0; + virtual VehicleConfigurationSource::CONTROLLER_TYPE getControllerType() const = 0; virtual VehicleConfigurationSource::VEHICLE_TYPE getVehicleType() const = 0; virtual VehicleConfigurationSource::VEHICLE_SUB_TYPE getVehicleSubType() const = 0; - virtual VehicleConfigurationSource::INPUT_TYPE getInputType() const = 0; - virtual VehicleConfigurationSource::ESC_TYPE getESCType() const = 0; + virtual VehicleConfigurationSource::INPUT_TYPE getInputType() const = 0; + virtual VehicleConfigurationSource::ESC_TYPE getESCType() const = 0; - virtual VehicleConfigurationSource::GPS_SETTING getGPSSetting() const = 0; + virtual VehicleConfigurationSource::GPS_SETTING getGPSSetting() const = 0; virtual VehicleConfigurationSource::RADIO_SETTING getRadioSetting() const = 0; virtual bool isCalibrationPerformed() const = 0; @@ -85,7 +84,7 @@ public: virtual bool isRestartNeeded() const = 0; - virtual QString getSummaryText() = 0; + virtual QString getSummaryText() = 0; }; #endif // VEHICLECONFIGURATIONSOURCE_H diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.cpp b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.cpp index fe7406f6c..c58de58b0 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.cpp +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.cpp @@ -30,10 +30,9 @@ #include "systemhealthgadgetconfiguration.h" SystemHealthGadget::SystemHealthGadget(QString classId, SystemHealthGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} SystemHealthGadget::~SystemHealthGadget() { @@ -41,13 +40,14 @@ SystemHealthGadget::~SystemHealthGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void SystemHealthGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void SystemHealthGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - SystemHealthGadgetConfiguration *m = qobject_cast(config); + SystemHealthGadgetConfiguration *m = qobject_cast(config); + m_widget->setSystemFile(m->getSystemFile()); // Triggers widget repaint } diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.h b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.h index 2884fe14e..bcd4870f5 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.h +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.h @@ -38,15 +38,17 @@ class SystemHealthGadgetWidget; using namespace Core; -class SystemHealthGadget : public Core::IUAVGadget -{ +class SystemHealthGadget : public Core::IUAVGadget { Q_OBJECT public: SystemHealthGadget(QString classId, SystemHealthGadgetWidget *widget, QWidget *parent = 0); ~SystemHealthGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: SystemHealthGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.cpp index 6ddd9aaa8..de16464da 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.cpp @@ -32,13 +32,13 @@ * Loads a saved configuration or defaults if non exist. * */ -SystemHealthGadgetConfiguration::SystemHealthGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +SystemHealthGadgetConfiguration::SystemHealthGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), systemFile("Unknown") { - //if a saved configuration exists load it - if(qSettings != 0) { - QString diagram= qSettings->value("diagram").toString(); + // if a saved configuration exists load it + if (qSettings != 0) { + QString diagram = qSettings->value("diagram").toString(); systemFile = Utils::PathUtils().InsertDataPath(diagram); } } @@ -50,7 +50,8 @@ SystemHealthGadgetConfiguration::SystemHealthGadgetConfiguration(QString classId IUAVGadgetConfiguration *SystemHealthGadgetConfiguration::clone() { SystemHealthGadgetConfiguration *m = new SystemHealthGadgetConfiguration(this->classId()); - m->systemFile=systemFile; + + m->systemFile = systemFile; return m; } @@ -58,7 +59,9 @@ IUAVGadgetConfiguration *SystemHealthGadgetConfiguration::clone() * Saves a configuration. * */ -void SystemHealthGadgetConfiguration::saveConfig(QSettings* qSettings) const { +void SystemHealthGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ QString diagram = Utils::PathUtils().RemoveDataPath(systemFile); + qSettings->setValue("diagram", diagram); } diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.h index 4cafd7cdb..7eb42c80b 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.h @@ -34,26 +34,30 @@ using namespace Core; /* This is a generic system health gadget displaying system alarms for one or more components. - */ -class SystemHealthGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT + */ +class SystemHealthGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit SystemHealthGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit SystemHealthGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - //set system health configuration functions - void setSystemFile(QString filename){systemFile=filename;} + // set system health configuration functions + void setSystemFile(QString filename) + { + systemFile = filename; + } - //get dial configuration functions - QString getSystemFile() {return systemFile;} + // get dial configuration functions + QString getSystemFile() + { + return systemFile; + } - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: // systemFile contains the source SVG: QString systemFile; - }; #endif // SYSTEMHEALTHGADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.cpp index 5e9100a63..34a52364d 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.cpp @@ -32,29 +32,27 @@ #include SystemHealthGadgetFactory::SystemHealthGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("SystemHealthGadget"), - tr("System Health"), - parent) -{ -} + IUAVGadgetFactory(QString("SystemHealthGadget"), + tr("System Health"), + parent) +{} SystemHealthGadgetFactory::~SystemHealthGadgetFactory() -{ -} +{} -Core::IUAVGadget* SystemHealthGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *SystemHealthGadgetFactory::createGadget(QWidget *parent) { - SystemHealthGadgetWidget* gadgetWidget = new SystemHealthGadgetWidget(parent); + SystemHealthGadgetWidget *gadgetWidget = new SystemHealthGadgetWidget(parent); + return new SystemHealthGadget(QString("SystemHealthGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *SystemHealthGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *SystemHealthGadgetFactory::createConfiguration(QSettings *qSettings) { return new SystemHealthGadgetConfiguration(QString("SystemHealthGadget"), qSettings); } IOptionsPage *SystemHealthGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new SystemHealthGadgetOptionsPage(qobject_cast(config)); + return new SystemHealthGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.h b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.h index f17ed33f7..cdc38590b 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.h @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class SystemHealthGadgetFactory : public IUAVGadgetFactory -{ +class SystemHealthGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: SystemHealthGadgetFactory(QObject *parent = 0); ~SystemHealthGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.cpp index 229a8caff..4bb606833 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.cpp @@ -34,20 +34,18 @@ #include SystemHealthGadgetOptionsPage::SystemHealthGadgetOptionsPage(SystemHealthGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *SystemHealthGadgetOptionsPage::createPage(QWidget *parent) { - Q_UNUSED(parent); options_page = new Ui::SystemHealthGadgetOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget; - //main layout + // main layout options_page->setupUi(optionsPageWidget); // Restore the contents from the settings: diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.h index f5fbe7d8f..3b729d69a 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.h @@ -40,14 +40,13 @@ class IUAVGadgetConfiguration; class SystemHealthGadgetConfiguration; namespace Ui { - class SystemHealthGadgetOptionsPage; +class SystemHealthGadgetOptionsPage; } using namespace Core; -class SystemHealthGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class SystemHealthGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit SystemHealthGadgetOptionsPage(SystemHealthGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp index 68d19c532..38f62569d 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp @@ -39,15 +39,15 @@ */ SystemHealthGadgetWidget::SystemHealthGadgetWidget(QWidget *parent) : QGraphicsView(parent) { - setMinimumSize(128,128); + setMinimumSize(128, 128); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setScene(new QGraphicsScene(this)); - + m_renderer = new QSvgRenderer(); background = new QGraphicsSvgItem(); foreground = new QGraphicsSvgItem(); - nolink = new QGraphicsSvgItem(); + nolink = new QGraphicsSvgItem(); paint(); @@ -55,11 +55,11 @@ SystemHealthGadgetWidget::SystemHealthGadgetWidget(QWidget *parent) : QGraphicsV ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - SystemAlarms* obj = dynamic_cast(objManager->getObject(QString("SystemAlarms"))); - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateAlarms(UAVObject*))); + SystemAlarms *obj = dynamic_cast(objManager->getObject(QString("SystemAlarms"))); + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateAlarms(UAVObject *))); // Listen to autopilot connection events - TelemetryManager* telMngr = pm->getObject(); + TelemetryManager *telMngr = pm->getObject(); connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); @@ -67,53 +67,56 @@ SystemHealthGadgetWidget::SystemHealthGadgetWidget(QWidget *parent) : QGraphicsV } /** - * Hide the "No Link" overlay - */ + * Hide the "No Link" overlay + */ void SystemHealthGadgetWidget::onAutopilotConnect() { nolink->setVisible(false); } /** - * Show the "No Link" overlay - */ + * Show the "No Link" overlay + */ void SystemHealthGadgetWidget::onAutopilotDisconnect() { nolink->setVisible(true); } -void SystemHealthGadgetWidget::updateAlarms(UAVObject* systemAlarm) +void SystemHealthGadgetWidget::updateAlarms(UAVObject *systemAlarm) { // This code does not know anything about alarms beforehand, and // I found no efficient way to locate items inside the scene by // name, so it's just as simple to reset the scene: // And add the one with the right name. QGraphicsScene *m_scene = scene(); - foreach ( QGraphicsItem* item ,background->childItems()){ + + foreach(QGraphicsItem * item, background->childItems()) { m_scene->removeItem(item); delete item; // removeItem does _not_ delete the item. } QString alarm = systemAlarm->getName(); - foreach (UAVObjectField *field, systemAlarm->getFields()) { + foreach(UAVObjectField * field, systemAlarm->getFields()) { for (uint i = 0; i < field->getNumElements(); ++i) { QString element = field->getElementNames()[i]; - QString value = field->getValue(i).toString(); + QString value = field->getValue(i).toString(); if (m_renderer->elementExists(element)) { QMatrix blockMatrix = m_renderer->matrixForElement(element); qreal startX = blockMatrix.mapRect(m_renderer->boundsOnElement(element)).x(); qreal startY = blockMatrix.mapRect(m_renderer->boundsOnElement(element)).y(); - QString element2 = element + "-" + value; + QString element2 = element + "-" + value; if (m_renderer->elementExists(element2)) { QGraphicsSvgItem *ind = new QGraphicsSvgItem(); ind->setSharedRenderer(m_renderer); ind->setElementId(element2); ind->setParentItem(background); QTransform matrix; - matrix.translate(startX,startY); - ind->setTransform(matrix,false); + matrix.translate(startX, startY); + ind->setTransform(matrix, false); } else { - if (value.compare("Uninitialised")!=0)qDebug() << "Warning: element " << element2 << " not found in SVG."; + if (value.compare("Uninitialised") != 0) { + qDebug() << "Warning: element " << element2 << " not found in SVG."; + } } } else { qDebug() << "Warning: Element " << element << " not found in SVG."; @@ -124,54 +127,53 @@ void SystemHealthGadgetWidget::updateAlarms(UAVObject* systemAlarm) SystemHealthGadgetWidget::~SystemHealthGadgetWidget() { - // Do nothing + // Do nothing } void SystemHealthGadgetWidget::setSystemFile(QString dfn) { setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); - if (QFile::exists(dfn)) { - m_renderer->load(dfn); - if(m_renderer->isValid()) { - fgenabled = false; - background->setSharedRenderer(m_renderer); - background->setElementId("background"); + if (QFile::exists(dfn)) { + m_renderer->load(dfn); + if (m_renderer->isValid()) { + fgenabled = false; + background->setSharedRenderer(m_renderer); + background->setElementId("background"); - if (m_renderer->elementExists("foreground")) { - foreground->setSharedRenderer(m_renderer); - foreground->setElementId("foreground"); - foreground->setZValue(99); - fgenabled = true; - } - if (m_renderer->elementExists("nolink")) { - nolink->setSharedRenderer(m_renderer); - nolink->setElementId("nolink"); - nolink->setZValue(100); - } + if (m_renderer->elementExists("foreground")) { + foreground->setSharedRenderer(m_renderer); + foreground->setElementId("foreground"); + foreground->setZValue(99); + fgenabled = true; + } + if (m_renderer->elementExists("nolink")) { + nolink->setSharedRenderer(m_renderer); + nolink->setElementId("nolink"); + nolink->setZValue(100); + } - QGraphicsScene *l_scene = scene(); - l_scene->setSceneRect(background->boundingRect()); - fitInView(background, Qt::KeepAspectRatio ); + QGraphicsScene *l_scene = scene(); + l_scene->setSceneRect(background->boundingRect()); + fitInView(background, Qt::KeepAspectRatio); - // Check whether the autopilot is connected already, by the way: - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - TelemetryManager* telMngr = pm->getObject(); - if (telMngr->isConnected()) { - onAutopilotConnect(); - SystemAlarms* obj = dynamic_cast(objManager->getObject(QString("SystemAlarms"))); - updateAlarms(obj); - } - } - } - else - { qDebug() <<"SystemHealthGadget: no file"; } + // Check whether the autopilot is connected already, by the way: + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + TelemetryManager *telMngr = pm->getObject(); + if (telMngr->isConnected()) { + onAutopilotConnect(); + SystemAlarms *obj = dynamic_cast(objManager->getObject(QString("SystemAlarms"))); + updateAlarms(obj); + } + } + } else { qDebug() << "SystemHealthGadget: no file"; } } void SystemHealthGadgetWidget::paint() { QGraphicsScene *l_scene = scene(); + l_scene->clear(); l_scene->addItem(background); l_scene->addItem(foreground); @@ -182,11 +184,11 @@ void SystemHealthGadgetWidget::paint() void SystemHealthGadgetWidget::paintEvent(QPaintEvent *event) { // Skip painting until the dial file is loaded - if (! m_renderer->isValid()) { - qDebug() <<"SystemHealthGadget: System file not loaded, not rendering"; + if (!m_renderer->isValid()) { + qDebug() << "SystemHealthGadget: System file not loaded, not rendering"; return; } - QGraphicsView::paintEvent(event); + QGraphicsView::paintEvent(event); } // This event enables the dial to be dynamically resized @@ -195,34 +197,35 @@ void SystemHealthGadgetWidget::paintEvent(QPaintEvent *event) void SystemHealthGadgetWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - fitInView(background, Qt::KeepAspectRatio ); + fitInView(background, Qt::KeepAspectRatio); } -void SystemHealthGadgetWidget::mousePressEvent ( QMouseEvent * event ) +void SystemHealthGadgetWidget::mousePressEvent(QMouseEvent *event) { QGraphicsScene *graphicsScene = scene(); - if(graphicsScene){ + + if (graphicsScene) { QPoint point = event->pos(); bool haveAlarmItem = false; - foreach(QGraphicsItem* sceneItem, items(point)){ - QGraphicsSvgItem *clickedItem = dynamic_cast(sceneItem); + foreach(QGraphicsItem * sceneItem, items(point)) { + QGraphicsSvgItem *clickedItem = dynamic_cast(sceneItem); - if(clickedItem){ - if((clickedItem != foreground) && (clickedItem != background)){ + if (clickedItem) { + if ((clickedItem != foreground) && (clickedItem != background)) { // Clicked an actual alarm. We need to set haveAlarmItem to true // as two of the items in this loop will always be foreground and // background. Without this flag, at some point in the loop we // would always call showAllAlarmDescriptions... haveAlarmItem = true; QString itemId = clickedItem->elementId(); - if(itemId.contains("OK")){ + if (itemId.contains("OK")) { // No alarm set for this item showAlarmDescriptionForItemId("AlarmOK", event->globalPos()); - }else{ + } else { // Warning, error or critical alarm showAlarmDescriptionForItemId(itemId, event->globalPos()); } - }else if(!haveAlarmItem){ + } else if (!haveAlarmItem) { // Clicked foreground or background showAllAlarmDescriptions(event->globalPos()); } @@ -231,29 +234,34 @@ void SystemHealthGadgetWidget::mousePressEvent ( QMouseEvent * event ) } } -void SystemHealthGadgetWidget::showAlarmDescriptionForItemId(const QString itemId, const QPoint& location){ +void SystemHealthGadgetWidget::showAlarmDescriptionForItemId(const QString itemId, const QPoint & location) +{ QFile alarmDescription(":/systemhealth/html/" + itemId + ".html"); - if(alarmDescription.open(QIODevice::ReadOnly | QIODevice::Text)){ + + if (alarmDescription.open(QIODevice::ReadOnly | QIODevice::Text)) { QTextStream textStream(&alarmDescription); QWhatsThis::showText(location, textStream.readAll()); } } -void SystemHealthGadgetWidget::showAllAlarmDescriptions(const QPoint& location){ +void SystemHealthGadgetWidget::showAllAlarmDescriptions(const QPoint & location) +{ QGraphicsScene *graphicsScene = scene(); - if(graphicsScene){ + + if (graphicsScene) { QString alarmsText; // Loop through all items in the scene looking for svg items that represent alarms - foreach(QGraphicsItem* curItem, graphicsScene->items()){ - QGraphicsSvgItem* curSvgItem = dynamic_cast(curItem); - if(curSvgItem && (curSvgItem != foreground) && (curSvgItem != background)){ + foreach(QGraphicsItem * curItem, graphicsScene->items()) { + QGraphicsSvgItem *curSvgItem = dynamic_cast(curItem); + + if (curSvgItem && (curSvgItem != foreground) && (curSvgItem != background)) { QString elementId = curSvgItem->elementId(); - if(!elementId.contains("OK")){ + if (!elementId.contains("OK")) { // Found an alarm, get its corresponding alarm html file contents // and append to the cumulative string for all alarms. QFile alarmDescription(":/systemhealth/html/" + elementId + ".html"); - if(alarmDescription.open(QIODevice::ReadOnly | QIODevice::Text)){ + if (alarmDescription.open(QIODevice::ReadOnly | QIODevice::Text)) { QTextStream textStream(&alarmDescription); alarmsText.append(textStream.readAll()); } @@ -262,7 +270,7 @@ void SystemHealthGadgetWidget::showAllAlarmDescriptions(const QPoint& location){ } // Show alarms text if we have any - if(alarmsText.length() > 0){ + if (alarmsText.length() > 0) { QWhatsThis::showText(location, alarmsText); } } diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h index 15ab31952..a76c58e1a 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h @@ -39,38 +39,36 @@ #include #include -class SystemHealthGadgetWidget : public QGraphicsView -{ +class SystemHealthGadgetWidget : public QGraphicsView { Q_OBJECT public: SystemHealthGadgetWidget(QWidget *parent = 0); - ~SystemHealthGadgetWidget(); - void setSystemFile(QString dfn); - void setIndicator(QString indicator); - void paint(); + ~SystemHealthGadgetWidget(); + void setSystemFile(QString dfn); + void setIndicator(QString indicator); + void paint(); protected: - void paintEvent(QPaintEvent *event); - void resizeEvent(QResizeEvent *event); - void mousePressEvent ( QMouseEvent * event ); + void paintEvent(QPaintEvent *event); + void resizeEvent(QResizeEvent *event); + void mousePressEvent(QMouseEvent *event); private slots: - void updateAlarms(UAVObject *systemAlarm); // Called by the systemalarms UAVObject - void onAutopilotConnect(); - void onAutopilotDisconnect(); + void updateAlarms(UAVObject *systemAlarm); // Called by the systemalarms UAVObject + void onAutopilotConnect(); + void onAutopilotDisconnect(); private: - QSvgRenderer *m_renderer; - QGraphicsSvgItem *background; - QGraphicsSvgItem *foreground; - QGraphicsSvgItem *nolink; + QSvgRenderer *m_renderer; + QGraphicsSvgItem *background; + QGraphicsSvgItem *foreground; + QGraphicsSvgItem *nolink; - // Simple flag to skip rendering if the - bool fgenabled; // layer does not exist. - - void showAlarmDescriptionForItemId(const QString itemId, const QPoint& location); - void showAllAlarmDescriptions(const QPoint &location); + // Simple flag to skip rendering if the + bool fgenabled; // layer does not exist. + void showAlarmDescriptionForItemId(const QString itemId, const QPoint & location); + void showAllAlarmDescriptions(const QPoint &location); }; #endif /* SYSTEMHEALTHGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.cpp b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.cpp index 3975c8508..db5bc6aa9 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.cpp +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.cpp @@ -35,31 +35,31 @@ SystemHealthPlugin::SystemHealthPlugin() { - // Do nothing + // Do nothing } SystemHealthPlugin::~SystemHealthPlugin() { - // Do nothing + // Do nothing } -bool SystemHealthPlugin::initialize(const QStringList& args, QString *errMsg) +bool SystemHealthPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new SystemHealthGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new SystemHealthGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void SystemHealthPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void SystemHealthPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(SystemHealthPlugin) diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.h b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.h index 6f68288ea..332c7eee6 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.h +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.h @@ -32,16 +32,15 @@ class SystemHealthGadgetFactory; -class SystemHealthPlugin : public ExtensionSystem::IPlugin -{ +class SystemHealthPlugin : public ExtensionSystem::IPlugin { public: - SystemHealthPlugin(); - ~SystemHealthPlugin(); + SystemHealthPlugin(); + ~SystemHealthPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - SystemHealthGadgetFactory *mf; + SystemHealthGadgetFactory *mf; }; #endif /* SYSTEMHEALTHPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.cpp index c0ac4fa01..028dc82c3 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.cpp @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -29,16 +29,15 @@ #include "fieldtreeitem.h" BrowserItemDelegate::BrowserItemDelegate(QObject *parent) : - QStyledItemDelegate(parent) -{ -} + QStyledItemDelegate(parent) +{} QWidget *BrowserItemDelegate::createEditor(QWidget *parent, - const QStyleOptionViewItem & option , - const QModelIndex & index ) const + const QStyleOptionViewItem & option, + const QModelIndex & index) const { Q_UNUSED(option) - FieldTreeItem *item = static_cast(index.internalPointer()); + FieldTreeItem * item = static_cast(index.internalPointer()); QWidget *editor = item->createEditor(parent); Q_ASSERT(editor); return editor; @@ -48,21 +47,23 @@ QWidget *BrowserItemDelegate::createEditor(QWidget *parent, void BrowserItemDelegate::setEditorData(QWidget *editor, const QModelIndex &index) const { - FieldTreeItem *item = static_cast(index.internalPointer()); + FieldTreeItem *item = static_cast(index.internalPointer()); QVariant value = index.model()->data(index, Qt::EditRole); + item->setEditorValue(editor, value); } void BrowserItemDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, const QModelIndex &index) const { - FieldTreeItem *item = static_cast(index.internalPointer()); + FieldTreeItem *item = static_cast(index.internalPointer()); QVariant value = item->getEditorValue(editor); + model->setData(index, value, Qt::EditRole); } void BrowserItemDelegate::updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &/* index */) const + const QStyleOptionViewItem &option, const QModelIndex & /* index */) const { editor->setGeometry(option.rect); } diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.h index 9c6a0e983..17952b5db 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,9 +30,8 @@ #include -class BrowserItemDelegate : public QStyledItemDelegate -{ -Q_OBJECT +class BrowserItemDelegate : public QStyledItemDelegate { + Q_OBJECT public: explicit BrowserItemDelegate(QObject *parent = 0); @@ -44,15 +43,14 @@ public: const QModelIndex &index) const; void updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &index) const; + const QStyleOptionViewItem &option, const QModelIndex &index) const; QSize sizeHint(const QStyleOptionViewItem & option, - const QModelIndex &index) const; + const QModelIndex &index) const; signals: public slots: - }; #endif // BROWSERITEMDELEGATE_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.cpp index f30a08e0e..703ea1290 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.cpp @@ -32,32 +32,31 @@ BrowserPlugin::BrowserPlugin() { - // Do nothing + // Do nothing } BrowserPlugin::~BrowserPlugin() { - // Do nothing + // Do nothing } -bool BrowserPlugin::initialize(const QStringList& args, QString *errMsg) +bool BrowserPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new UAVObjectBrowserFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new UAVObjectBrowserFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void BrowserPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void BrowserPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(BrowserPlugin) - diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.h index 4644e7475..aac119a59 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.h @@ -32,16 +32,15 @@ class UAVObjectBrowserFactory; -class BrowserPlugin : public ExtensionSystem::IPlugin -{ +class BrowserPlugin : public ExtensionSystem::IPlugin { public: - BrowserPlugin(); - ~BrowserPlugin(); + BrowserPlugin(); + ~BrowserPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - UAVObjectBrowserFactory *mf; + UAVObjectBrowserFactory *mf; }; #endif /* UAVOBJECTBROWSERPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.cpp index fa3c9e203..2c1407d06 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.cpp @@ -10,20 +10,19 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "fieldtreeitem.h" - diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.h index bd980749f..256775172 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.h @@ -37,86 +37,100 @@ #include #include -#define QINT8MIN std::numeric_limits::min() -#define QINT8MAX std::numeric_limits::max() -#define QUINTMIN std::numeric_limits::min() -#define QUINT8MAX std::numeric_limits::max() -#define QINT16MIN std::numeric_limits::min() -#define QINT16MAX std::numeric_limits::max() +#define QINT8MIN std::numeric_limits::min() +#define QINT8MAX std::numeric_limits::max() +#define QUINTMIN std::numeric_limits::min() +#define QUINT8MAX std::numeric_limits::max() +#define QINT16MIN std::numeric_limits::min() +#define QINT16MAX std::numeric_limits::max() #define QUINT16MAX std::numeric_limits::max() -#define QINT32MIN std::numeric_limits::min() -#define QINT32MAX std::numeric_limits::max() +#define QINT32MIN std::numeric_limits::min() +#define QINT32MAX std::numeric_limits::max() #define QUINT32MAX std::numeric_limits::max() -class FieldTreeItem : public TreeItem -{ -Q_OBJECT +class FieldTreeItem : public TreeItem { + Q_OBJECT public: FieldTreeItem(int index, const QList &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_index(index) { } + TreeItem(data, parent), m_index(index) {} FieldTreeItem(int index, const QVariant &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_index(index) { } - bool isEditable() { return true; } - virtual QWidget *createEditor(QWidget *parent) = 0; + TreeItem(data, parent), m_index(index) {} + bool isEditable() + { + return true; + } + virtual QWidget *createEditor(QWidget *parent) = 0; virtual QVariant getEditorValue(QWidget *editor) = 0; virtual void setEditorValue(QWidget *editor, QVariant value) = 0; - virtual void apply() { } + virtual void apply() {} protected: int m_index; }; -class EnumFieldTreeItem : public FieldTreeItem -{ -Q_OBJECT +class EnumFieldTreeItem : public FieldTreeItem { + Q_OBJECT public: EnumFieldTreeItem(UAVObjectField *field, int index, const QList &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) { } + FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) {} EnumFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) { } - void setData(QVariant value, int column) { + FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) {} + void setData(QVariant value, int column) + { QStringList options = m_field->getOptions(); - QVariant tmpValue = m_field->getValue(m_index); - int tmpValIndex = options.indexOf(tmpValue.toString()); + QVariant tmpValue = m_field->getValue(m_index); + int tmpValIndex = options.indexOf(tmpValue.toString()); + setChanged(tmpValIndex != value); TreeItem::setData(value, column); } - QString enumOptions(int index) { - if((index < 0) || (index >= m_enumOptions.length())) { + QString enumOptions(int index) + { + if ((index < 0) || (index >= m_enumOptions.length())) { return QString("Invalid Value (") + QString().setNum(index) + QString(")"); } return m_enumOptions.at(index); } - void apply() { + void apply() + { int value = data(dataColumn).toInt(); QStringList options = m_field->getOptions(); + m_field->setValue(options[value], m_index); setChanged(false); } - void update() { + void update() + { QStringList options = m_field->getOptions(); QVariant value = m_field->getValue(m_index); - int valIndex = options.indexOf(value.toString()); + int valIndex = options.indexOf(value.toString()); + if (data() != valIndex || changed()) { TreeItem::setData(valIndex); setHighlight(true); } } - QWidget *createEditor(QWidget *parent) { + QWidget *createEditor(QWidget *parent) + { QComboBox *editor = new QComboBox(parent); - foreach (QString option, m_enumOptions) - editor->addItem(option); + + foreach(QString option, m_enumOptions) + editor->addItem(option); return editor; } - QVariant getEditorValue(QWidget *editor) { - QComboBox *comboBox = static_cast(editor); + QVariant getEditorValue(QWidget *editor) + { + QComboBox *comboBox = static_cast(editor); + return comboBox->currentIndex(); } - void setEditorValue(QWidget *editor, QVariant value) { - QComboBox *comboBox = static_cast(editor); + void setEditorValue(QWidget *editor, QVariant value) + { + QComboBox *comboBox = static_cast(editor); + comboBox->setCurrentIndex(value.toInt()); } private: @@ -124,20 +138,22 @@ private: UAVObjectField *m_field; }; -class IntFieldTreeItem : public FieldTreeItem -{ -Q_OBJECT +class IntFieldTreeItem : public FieldTreeItem { + Q_OBJECT public: IntFieldTreeItem(UAVObjectField *field, int index, const QList &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { + FieldTreeItem(index, data, parent), m_field(field) + { setMinMaxValues(); } IntFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { + FieldTreeItem(index, data, parent), m_field(field) + { setMinMaxValues(); } - void setMinMaxValues() { + void setMinMaxValues() + { switch (m_field->getType()) { case UAVObjectField::INT8: m_minValue = QINT8MIN; @@ -169,33 +185,43 @@ public: } } - QWidget *createEditor(QWidget *parent) { + QWidget *createEditor(QWidget *parent) + { QSpinBox *editor = new QSpinBox(parent); + editor->setMinimum(m_minValue); editor->setMaximum(m_maxValue); return editor; } - QVariant getEditorValue(QWidget *editor) { - QSpinBox *spinBox = static_cast(editor); + QVariant getEditorValue(QWidget *editor) + { + QSpinBox *spinBox = static_cast(editor); + spinBox->interpretText(); return spinBox->value(); } - void setEditorValue(QWidget *editor, QVariant value) { - QSpinBox *spinBox = static_cast(editor); + void setEditorValue(QWidget *editor, QVariant value) + { + QSpinBox *spinBox = static_cast(editor); + spinBox->setValue(value.toInt()); } - void setData(QVariant value, int column) { + void setData(QVariant value, int column) + { setChanged(m_field->getValue(m_index) != value); TreeItem::setData(value, column); } - void apply() { + void apply() + { m_field->setValue(data(dataColumn).toInt(), m_index); setChanged(false); } - void update() { + void update() + { int value = m_field->getValue(m_index).toInt(); + if (data() != value || changed()) { TreeItem::setData(value); setHighlight(true); @@ -208,72 +234,76 @@ private: int m_maxValue; }; -class FloatFieldTreeItem : public FieldTreeItem -{ -Q_OBJECT +class FloatFieldTreeItem : public FieldTreeItem { + Q_OBJECT public: FloatFieldTreeItem(UAVObjectField *field, int index, const QList &data, bool scientific = false, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field), m_useScientificNotation(scientific){} + FieldTreeItem(index, data, parent), m_field(field), m_useScientificNotation(scientific) {} FloatFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, bool scientific = false, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field), m_useScientificNotation(scientific) { } - void setData(QVariant value, int column) { + FieldTreeItem(index, data, parent), m_field(field), m_useScientificNotation(scientific) {} + void setData(QVariant value, int column) + { setChanged(m_field->getValue(m_index) != value); TreeItem::setData(value, column); } - void apply() { + void apply() + { m_field->setValue(data(dataColumn).toDouble(), m_index); setChanged(false); } - void update() { + void update() + { double value = m_field->getValue(m_index).toDouble(); + if (data() != value || changed()) { TreeItem::setData(value); setHighlight(true); } } - QWidget *createEditor(QWidget *parent) { - if(m_useScientificNotation) { + QWidget *createEditor(QWidget *parent) + { + if (m_useScientificNotation) { QScienceSpinBox *editor = new QScienceSpinBox(parent); editor->setDecimals(6); editor->setMinimum(-std::numeric_limits::max()); editor->setMaximum(std::numeric_limits::max()); return editor; } else { - QDoubleSpinBox *editor = new QDoubleSpinBox(parent); - editor->setDecimals(8); + QDoubleSpinBox *editor = new QDoubleSpinBox(parent); + editor->setDecimals(8); editor->setMinimum(-std::numeric_limits::max()); editor->setMaximum(std::numeric_limits::max()); return editor; } } - QVariant getEditorValue(QWidget *editor) { - if(m_useScientificNotation) { - QScienceSpinBox *spinBox = static_cast(editor); + QVariant getEditorValue(QWidget *editor) + { + if (m_useScientificNotation) { + QScienceSpinBox *spinBox = static_cast(editor); spinBox->interpretText(); return spinBox->value(); } else { - QDoubleSpinBox *spinBox = static_cast(editor); + QDoubleSpinBox *spinBox = static_cast(editor); spinBox->interpretText(); return spinBox->value(); } } - void setEditorValue(QWidget *editor, QVariant value) { - - if(m_useScientificNotation) { - QScienceSpinBox *spinBox = static_cast(editor); + void setEditorValue(QWidget *editor, QVariant value) + { + if (m_useScientificNotation) { + QScienceSpinBox *spinBox = static_cast(editor); spinBox->setValue(value.toDouble()); } else { - QDoubleSpinBox *spinBox = static_cast(editor); + QDoubleSpinBox *spinBox = static_cast(editor); spinBox->setValue(value.toDouble()); } } private: UAVObjectField *m_field; bool m_useScientificNotation; - }; #endif // FIELDTREEITEM_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp index fac18561b..85c5aa144 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -45,8 +45,7 @@ bool HighLightManager::add(TreeItem *itemToAdd) QMutexLocker locker(&m_listMutex); // Check so that the item isn't already in the list - if(!m_itemsList.contains(itemToAdd)) - { + if (!m_itemsList.contains(itemToAdd)) { m_itemsList.append(itemToAdd); return true; } @@ -78,17 +77,15 @@ void HighLightManager::checkItemsExpired() QMutexLocker locker(&m_listMutex); // Get a mutable iterator for the list - QMutableLinkedListIterator iter(m_itemsList); + QMutableLinkedListIterator iter(m_itemsList); // This is the timestamp to compare with QTime now = QTime::currentTime(); // Loop over all items, check if they expired. - while(iter.hasNext()) - { - TreeItem* item = iter.next(); - if(item->getHiglightExpires() < now) - { + while (iter.hasNext()) { + TreeItem *item = iter.next(); + if (item->getHiglightExpires() < now) { // If expired, call removeHighlight item->removeHighlight(); @@ -101,19 +98,18 @@ void HighLightManager::checkItemsExpired() int TreeItem::m_highlightTimeMs = 500; TreeItem::TreeItem(const QList &data, TreeItem *parent) : - QObject(0), - m_data(data), - m_parent(parent), - m_highlight(false), - m_changed(false) -{ -} + QObject(0), + m_data(data), + m_parent(parent), + m_highlight(false), + m_changed(false) +{} TreeItem::TreeItem(const QVariant &data, TreeItem *parent) : - QObject(0), - m_parent(parent), - m_highlight(false), - m_changed(false) + QObject(0), + m_parent(parent), + m_highlight(false), + m_changed(false) { m_data << data << "" << ""; } @@ -132,6 +128,7 @@ void TreeItem::appendChild(TreeItem *child) void TreeItem::insertChild(TreeItem *child) { int index = nameIndex(child->data(0).toString()); + m_children.insert(index, child); child->setParentTree(this); } @@ -148,8 +145,9 @@ int TreeItem::childCount() const int TreeItem::row() const { - if (m_parent) - return m_parent->m_children.indexOf(const_cast(this)); + if (m_parent) { + return m_parent->m_children.indexOf(const_cast(this)); + } return 0; } @@ -169,35 +167,35 @@ void TreeItem::setData(QVariant value, int column) m_data.replace(column, value); } -void TreeItem::update() { - foreach(TreeItem *child, treeChildren()) - child->update(); +void TreeItem::update() +{ + foreach(TreeItem * child, treeChildren()) + child->update(); } -void TreeItem::apply() { - foreach(TreeItem *child, treeChildren()) - child->apply(); +void TreeItem::apply() +{ + foreach(TreeItem * child, treeChildren()) + child->apply(); } /* * Called after a value has changed to trigger highlightning of tree item. */ -void TreeItem::setHighlight(bool highlight) { +void TreeItem::setHighlight(bool highlight) +{ m_highlight = highlight; - m_changed = false; + m_changed = false; if (highlight) { // Update the expires timestamp m_highlightExpires = QTime::currentTime().addMSecs(m_highlightTimeMs); // Add to highlightmanager - if(m_highlightManager->add(this)) - { + if (m_highlightManager->add(this)) { // Only emit signal if it was added emit updateHighlight(this); } - } - else if(m_highlightManager->remove(this)) - { + } else if (m_highlightManager->remove(this)) { // Only emit signal if it was removed emit updateHighlight(this); } @@ -205,15 +203,15 @@ void TreeItem::setHighlight(bool highlight) { // If we have a parent, call recursively to update highlight status of parents. // This will ensure that the root of a leaf that is changed also is highlighted. // Only updates that really changes values will trigger highlight of parents. - if(m_parent) - { + if (m_parent) { m_parent->setHighlight(highlight); } } -void TreeItem::removeHighlight() { +void TreeItem::removeHighlight() +{ m_highlight = false; - //update(); + // update(); emit updateHighlight(this); } diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h index f0209c598..9f693684b 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,33 +43,32 @@ class TreeItem; /* -* Small utility class that handles the higlighting of -* tree grid items. -* Basicly it maintains all items due to be restored to -* non highlighted state in a linked list. -* A timer traverses this list periodically to find out -* if any of the items should be restored. All items are -* updated withan expiration timestamp when they expires. -* An item that is beeing restored is removed from the -* list and its removeHighlight() method is called. Items -* that are not expired are left in the list til next time. -* Items that are updated during the expiration time are -* left untouched in the list. This reduces unwanted emits -* of signals to the repaint/update function. -*/ -class HighLightManager : public QObject -{ -Q_OBJECT + * Small utility class that handles the higlighting of + * tree grid items. + * Basicly it maintains all items due to be restored to + * non highlighted state in a linked list. + * A timer traverses this list periodically to find out + * if any of the items should be restored. All items are + * updated withan expiration timestamp when they expires. + * An item that is beeing restored is removed from the + * list and its removeHighlight() method is called. Items + * that are not expired are left in the list til next time. + * Items that are updated during the expiration time are + * left untouched in the list. This reduces unwanted emits + * of signals to the repaint/update function. + */ +class HighLightManager : public QObject { + Q_OBJECT public: // Constructor taking the checking interval in ms. HighLightManager(long checkingInterval); // This is called when an item has been set to // highlighted = true. - bool add(TreeItem* itemToAdd); + bool add(TreeItem *itemToAdd); - //This is called when an item is set to highlighted = false; - bool remove(TreeItem* itemToRemove); + // This is called when an item is set to highlighted = false; + bool remove(TreeItem *itemToRemove); private slots: // Timer callback method. @@ -80,15 +79,14 @@ private: QTimer m_expirationTimer; // The list holding all items due to be updated. - QLinkedList m_itemsList; + QLinkedList m_itemsList; - //Mutex to lock when accessing list. + // Mutex to lock when accessing list. QMutex m_listMutex; }; -class TreeItem : public QObject -{ -Q_OBJECT +class TreeItem : public QObject { + Q_OBJECT public: TreeItem(const QList &data, TreeItem *parent = 0); TreeItem(const QVariant &data, TreeItem *parent = 0); @@ -98,14 +96,22 @@ public: void insertChild(TreeItem *child); TreeItem *getChild(int index); - inline QList treeChildren() const { return m_children; } + inline QList treeChildren() const + { + return m_children; + } int childCount() const; int columnCount() const; QVariant data(int column = 1) const; - QString description() { return m_description; } - void setDescription(QString d) { // Split around 40 characters - int idx = d.indexOf(" ",40); - d.insert(idx,QString("
")); + QString description() + { + return m_description; + } + void setDescription(QString d) // Split around 40 characters + { + int idx = d.indexOf(" ", 40); + + d.insert(idx, QString("
")); d.remove("@Ref", Qt::CaseInsensitive); m_description = d; } @@ -113,36 +119,59 @@ public: // other columns are initialized in constructor virtual void setData(QVariant value, int column = 1); int row() const; - TreeItem *parent() { return m_parent; } - void setParentTree(TreeItem *parent) { m_parent = parent; } - inline virtual bool isEditable() { return false; } + TreeItem *parent() + { + return m_parent; + } + void setParentTree(TreeItem *parent) + { + m_parent = parent; + } + inline virtual bool isEditable() + { + return false; + } virtual void update(); virtual void apply(); - inline bool highlighted() { return m_highlight; } + inline bool highlighted() + { + return m_highlight; + } void setHighlight(bool highlight); - static void setHighlightTime(int time) { m_highlightTimeMs = time; } + static void setHighlightTime(int time) + { + m_highlightTimeMs = time; + } - inline bool changed() { return m_changed; } - inline void setChanged(bool changed) { m_changed = changed; } + inline bool changed() + { + return m_changed; + } + inline void setChanged(bool changed) + { + m_changed = changed; + } - virtual void setHighlightManager(HighLightManager* mgr); + virtual void setHighlightManager(HighLightManager *mgr); QTime getHiglightExpires(); virtual void removeHighlight(); - int nameIndex(QString name) { + int nameIndex(QString name) + { for (int i = 0; i < childCount(); ++i) { - if (name < getChild(i)->data(0).toString()) + if (name < getChild(i)->data(0).toString()) { return i; + } } return childCount(); } - TreeItem* findChildByName(QString name) + TreeItem *findChildByName(QString name) { - foreach (TreeItem* child, m_children) { + foreach(TreeItem * child, m_children) { if (name == child->data(0).toString()) { return child; } @@ -151,12 +180,12 @@ public: } signals: - void updateHighlight(TreeItem*); + void updateHighlight(TreeItem *); private slots: private: - QList m_children; + QList m_children; // m_data contains: [0] property name, [1] value, [2] unit QList m_data; QString m_description; @@ -164,7 +193,7 @@ private: bool m_highlight; bool m_changed; QTime m_highlightExpires; - HighLightManager* m_highlightManager; + HighLightManager *m_highlightManager; static int m_highlightTimeMs; public: static const int dataColumn = 1; @@ -173,102 +202,130 @@ public: class DataObjectTreeItem; class MetaObjectTreeItem; -class TopTreeItem : public TreeItem -{ -Q_OBJECT +class TopTreeItem : public TreeItem { + Q_OBJECT public: - TopTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) { } - TopTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { } + TopTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) {} + TopTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) {} - void addObjectTreeItem(quint32 objectId, DataObjectTreeItem* oti) { + void addObjectTreeItem(quint32 objectId, DataObjectTreeItem *oti) + { m_objectTreeItemsPerObjectIds[objectId] = oti; } - DataObjectTreeItem* findDataObjectTreeItemByObjectId(quint32 objectId) { + DataObjectTreeItem *findDataObjectTreeItemByObjectId(quint32 objectId) + { return m_objectTreeItemsPerObjectIds.contains(objectId) ? m_objectTreeItemsPerObjectIds[objectId] : 0; } - void addMetaObjectTreeItem(quint32 objectId, MetaObjectTreeItem* oti) { + void addMetaObjectTreeItem(quint32 objectId, MetaObjectTreeItem *oti) + { m_metaObjectTreeItemsPerObjectIds[objectId] = oti; } - MetaObjectTreeItem* findMetaObjectTreeItemByObjectId(quint32 objectId) { + MetaObjectTreeItem *findMetaObjectTreeItemByObjectId(quint32 objectId) + { return m_metaObjectTreeItemsPerObjectIds.contains(objectId) ? m_metaObjectTreeItemsPerObjectIds[objectId] : 0; } - QList getMetaObjectItems(); + QList getMetaObjectItems(); private: - QMap m_objectTreeItemsPerObjectIds; - QMap m_metaObjectTreeItemsPerObjectIds; + QMap m_objectTreeItemsPerObjectIds; + QMap m_metaObjectTreeItemsPerObjectIds; }; -class ObjectTreeItem : public TreeItem -{ -Q_OBJECT +class ObjectTreeItem : public TreeItem { + Q_OBJECT public: ObjectTreeItem(const QList &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_obj(0) { } + TreeItem(data, parent), m_obj(0) {} ObjectTreeItem(const QVariant &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_obj(0) { } - void setObject(UAVObject *obj) { m_obj = obj; setDescription(obj->getDescription()); } - inline UAVObject *object() { return m_obj; } + TreeItem(data, parent), m_obj(0) {} + void setObject(UAVObject *obj) + { + m_obj = obj; setDescription(obj->getDescription()); + } + inline UAVObject *object() + { + return m_obj; + } private: UAVObject *m_obj; }; -class MetaObjectTreeItem : public ObjectTreeItem -{ -Q_OBJECT +class MetaObjectTreeItem : public ObjectTreeItem { + Q_OBJECT public: MetaObjectTreeItem(UAVObject *obj, const QList &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { setObject(obj); } + ObjectTreeItem(data, parent) + { + setObject(obj); + } MetaObjectTreeItem(UAVObject *obj, const QVariant &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { setObject(obj); } + ObjectTreeItem(data, parent) + { + setObject(obj); + } }; -class DataObjectTreeItem : public ObjectTreeItem -{ -Q_OBJECT +class DataObjectTreeItem : public ObjectTreeItem { + Q_OBJECT public: DataObjectTreeItem(const QList &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { } + ObjectTreeItem(data, parent) {} DataObjectTreeItem(const QVariant &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { } - virtual void apply() { - foreach(TreeItem *child, treeChildren()) { - MetaObjectTreeItem *metaChild = dynamic_cast(child); - if (!metaChild) + ObjectTreeItem(data, parent) {} + virtual void apply() + { + foreach(TreeItem * child, treeChildren()) { + MetaObjectTreeItem *metaChild = dynamic_cast(child); + + if (!metaChild) { child->apply(); + } } } - virtual void update() { - foreach(TreeItem *child, treeChildren()) { - MetaObjectTreeItem *metaChild = dynamic_cast(child); - if (!metaChild) + virtual void update() + { + foreach(TreeItem * child, treeChildren()) { + MetaObjectTreeItem *metaChild = dynamic_cast(child); + + if (!metaChild) { child->update(); + } } } }; -class InstanceTreeItem : public DataObjectTreeItem -{ -Q_OBJECT +class InstanceTreeItem : public DataObjectTreeItem { + Q_OBJECT public: InstanceTreeItem(UAVObject *obj, const QList &data, TreeItem *parent = 0) : - DataObjectTreeItem(data, parent) { setObject(obj); } + DataObjectTreeItem(data, parent) + { + setObject(obj); + } InstanceTreeItem(UAVObject *obj, const QVariant &data, TreeItem *parent = 0) : - DataObjectTreeItem(data, parent) { setObject(obj); } - virtual void apply() { TreeItem::apply(); } - virtual void update() { TreeItem::update(); } + DataObjectTreeItem(data, parent) + { + setObject(obj); + } + virtual void apply() + { + TreeItem::apply(); + } + virtual void update() + { + TreeItem::update(); + } }; -class ArrayFieldTreeItem : public TreeItem -{ -Q_OBJECT +class ArrayFieldTreeItem : public TreeItem { + Q_OBJECT public: - ArrayFieldTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) { } - ArrayFieldTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { } + ArrayFieldTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) {} + ArrayFieldTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) {} }; #endif // TREEITEM_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp index 95288986e..6c5342e8d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp @@ -28,11 +28,11 @@ #include "uavobjectbrowserwidget.h" UAVObjectBrowser::UAVObjectBrowser(QString classId, UAVObjectBrowserWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget), - m_config(NULL) + IUAVGadget(classId, parent), + m_widget(widget), + m_config(NULL) { - connect(m_widget,SIGNAL(viewOptionsChanged(bool,bool,bool)),this,SLOT(viewOptionsChangedSlot(bool,bool,bool))); + connect(m_widget, SIGNAL(viewOptionsChanged(bool, bool, bool)), this, SLOT(viewOptionsChangedSlot(bool, bool, bool))); } UAVObjectBrowser::~UAVObjectBrowser() @@ -40,24 +40,23 @@ UAVObjectBrowser::~UAVObjectBrowser() delete m_widget; } -void UAVObjectBrowser::loadConfiguration(IUAVGadgetConfiguration* config) +void UAVObjectBrowser::loadConfiguration(IUAVGadgetConfiguration *config) { - UAVObjectBrowserConfiguration *m = qobject_cast(config); - m_config=m; + UAVObjectBrowserConfiguration *m = qobject_cast(config); + + m_config = m; m_widget->setRecentlyUpdatedColor(m->recentlyUpdatedColor()); m_widget->setManuallyChangedColor(m->manuallyChangedColor()); m_widget->setRecentlyUpdatedTimeout(m->recentlyUpdatedTimeout()); m_widget->setOnlyHilightChangedValues(m->onlyHighlightChangedValues()); - m_widget->setViewOptions(m->categorizedView(),m->scientificView(),m->showMetaData()); + m_widget->setViewOptions(m->categorizedView(), m->scientificView(), m->showMetaData()); } void UAVObjectBrowser::viewOptionsChangedSlot(bool categorized, bool scientific, bool metadata) { - if(m_config) - { + if (m_config) { m_config->setCategorizedView(categorized); m_config->setScientificView(scientific); m_config->setShowMetaData(metadata); } } - diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.h index 224be35a7..261583d06 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.h @@ -39,17 +39,19 @@ class UAVObjectBrowserWidget; using namespace Core; -class UAVObjectBrowser : public Core::IUAVGadget -{ +class UAVObjectBrowser : public Core::IUAVGadget { Q_OBJECT public: UAVObjectBrowser(QString classId, UAVObjectBrowserWidget *widget, QWidget *parent = 0); ~UAVObjectBrowser(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private slots: - void viewOptionsChangedSlot(bool categorized,bool scientific,bool metadata); + void viewOptionsChangedSlot(bool categorized, bool scientific, bool metadata); private: UAVObjectBrowserWidget *m_widget; UAVObjectBrowserConfiguration *m_config; diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp index 53b51003b..f249d496a 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp @@ -10,24 +10,24 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavobjectbrowserconfiguration.h" -UAVObjectBrowserConfiguration::UAVObjectBrowserConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +UAVObjectBrowserConfiguration::UAVObjectBrowserConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_recentlyUpdatedColor(QColor(255, 230, 230)), m_manuallyChangedColor(QColor(230, 230, 255)), @@ -37,19 +37,19 @@ UAVObjectBrowserConfiguration::UAVObjectBrowserConfiguration(QString classId, QS m_useScientificView(false), m_showMetaData(false) { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { QColor recent = qSettings->value("recentlyUpdatedColor").value(); QColor manual = qSettings->value("manuallyChangedColor").value(); - int timeout = qSettings->value("recentlyUpdatedTimeout").toInt(); - bool hilight = qSettings->value("onlyHilightChangedValues").toBool(); + int timeout = qSettings->value("recentlyUpdatedTimeout").toInt(); + bool hilight = qSettings->value("onlyHilightChangedValues").toBool(); - m_useCategorizedView = qSettings->value("CategorizedView").toBool(); - m_useScientificView = qSettings->value("ScientificView").toBool(); + m_useCategorizedView = qSettings->value("CategorizedView").toBool(); + m_useScientificView = qSettings->value("ScientificView").toBool(); m_showMetaData = qSettings->value("showMetaData").toBool(); - m_recentlyUpdatedColor = recent; - m_manuallyChangedColor = manual; - m_recentlyUpdatedTimeout = timeout; + m_recentlyUpdatedColor = recent; + m_manuallyChangedColor = manual; + m_recentlyUpdatedTimeout = timeout; m_onlyHilightChangedValues = hilight; } } @@ -57,12 +57,13 @@ UAVObjectBrowserConfiguration::UAVObjectBrowserConfiguration(QString classId, QS IUAVGadgetConfiguration *UAVObjectBrowserConfiguration::clone() { UAVObjectBrowserConfiguration *m = new UAVObjectBrowserConfiguration(this->classId()); - m->m_recentlyUpdatedColor = m_recentlyUpdatedColor; - m->m_manuallyChangedColor = m_manuallyChangedColor; - m->m_recentlyUpdatedTimeout = m_recentlyUpdatedTimeout; + + m->m_recentlyUpdatedColor = m_recentlyUpdatedColor; + m->m_manuallyChangedColor = m_manuallyChangedColor; + m->m_recentlyUpdatedTimeout = m_recentlyUpdatedTimeout; m->m_onlyHilightChangedValues = m_onlyHilightChangedValues; m->m_useCategorizedView = m_useCategorizedView; - m->m_useScientificView = m_useScientificView; + m->m_useScientificView = m_useScientificView; m->m_showMetaData = m_showMetaData; return m; } @@ -71,7 +72,8 @@ IUAVGadgetConfiguration *UAVObjectBrowserConfiguration::clone() * Saves a configuration. * */ -void UAVObjectBrowserConfiguration::saveConfig(QSettings* qSettings) const { +void UAVObjectBrowserConfiguration::saveConfig(QSettings *qSettings) const +{ qSettings->setValue("recentlyUpdatedColor", m_recentlyUpdatedColor); qSettings->setValue("manuallyChangedColor", m_manuallyChangedColor); qSettings->setValue("recentlyUpdatedTimeout", m_recentlyUpdatedTimeout); diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h index 82e442021..43152d9d6 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,40 +33,80 @@ using namespace Core; -class UAVObjectBrowserConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT -Q_PROPERTY(QColor m_recentlyUpdatedColor READ recentlyUpdatedColor WRITE setRecentlyUpdatedColor) -Q_PROPERTY(QColor m_manuallyChangedColor READ manuallyChangedColor WRITE setManuallyChangedColor) -Q_PROPERTY(int m_recentlyUpdatedTimeout READ recentlyUpdatedTimeout WRITE setRecentlyUpdatedTimeout) -Q_PROPERTY(bool m_onlyHilightChangedValues READ onlyHighlightChangedValues WRITE setOnlyHighlightChangedValues) -Q_PROPERTY(bool m_useCategorizedView READ categorizedView WRITE setCategorizedView) -Q_PROPERTY(bool m_useScientificView READ scientificView WRITE setScientificView) -Q_PROPERTY(bool m_showMetaData READ showMetaData WRITE setShowMetaData) +class UAVObjectBrowserConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT Q_PROPERTY(QColor m_recentlyUpdatedColor READ recentlyUpdatedColor WRITE setRecentlyUpdatedColor) + Q_PROPERTY(QColor m_manuallyChangedColor READ manuallyChangedColor WRITE setManuallyChangedColor) + Q_PROPERTY(int m_recentlyUpdatedTimeout READ recentlyUpdatedTimeout WRITE setRecentlyUpdatedTimeout) + Q_PROPERTY(bool m_onlyHilightChangedValues READ onlyHighlightChangedValues WRITE setOnlyHighlightChangedValues) + Q_PROPERTY(bool m_useCategorizedView READ categorizedView WRITE setCategorizedView) + Q_PROPERTY(bool m_useScientificView READ scientificView WRITE setScientificView) + Q_PROPERTY(bool m_showMetaData READ showMetaData WRITE setShowMetaData) public: - explicit UAVObjectBrowserConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit UAVObjectBrowserConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); - QColor recentlyUpdatedColor() const { return m_recentlyUpdatedColor; } - QColor manuallyChangedColor() const { return m_manuallyChangedColor; } - int recentlyUpdatedTimeout() const { return m_recentlyUpdatedTimeout; } - bool onlyHighlightChangedValues() const {return m_onlyHilightChangedValues;} - bool categorizedView() const { return m_useCategorizedView; } - bool scientificView() const { return m_useScientificView; } - bool showMetaData() const { return m_showMetaData; } + QColor recentlyUpdatedColor() const + { + return m_recentlyUpdatedColor; + } + QColor manuallyChangedColor() const + { + return m_manuallyChangedColor; + } + int recentlyUpdatedTimeout() const + { + return m_recentlyUpdatedTimeout; + } + bool onlyHighlightChangedValues() const + { + return m_onlyHilightChangedValues; + } + bool categorizedView() const + { + return m_useCategorizedView; + } + bool scientificView() const + { + return m_useScientificView; + } + bool showMetaData() const + { + return m_showMetaData; + } signals: public slots: - void setRecentlyUpdatedColor(QColor color) { m_recentlyUpdatedColor = color; } - void setManuallyChangedColor(QColor color) { m_manuallyChangedColor = color; } - void setRecentlyUpdatedTimeout(int timeout) { m_recentlyUpdatedTimeout = timeout; } - void setOnlyHighlightChangedValues(bool hilight) { m_onlyHilightChangedValues = hilight; } - void setCategorizedView(bool value) { m_useCategorizedView = value; } - void setScientificView(bool value) { m_useScientificView = value; } - void setShowMetaData(bool value) { m_showMetaData = value; } + void setRecentlyUpdatedColor(QColor color) + { + m_recentlyUpdatedColor = color; + } + void setManuallyChangedColor(QColor color) + { + m_manuallyChangedColor = color; + } + void setRecentlyUpdatedTimeout(int timeout) + { + m_recentlyUpdatedTimeout = timeout; + } + void setOnlyHighlightChangedValues(bool hilight) + { + m_onlyHilightChangedValues = hilight; + } + void setCategorizedView(bool value) + { + m_useCategorizedView = value; + } + void setScientificView(bool value) + { + m_useScientificView = value; + } + void setShowMetaData(bool value) + { + m_showMetaData = value; + } private: QColor m_recentlyUpdatedColor; diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.cpp index 3782ea87d..2fb7ee2ab 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.cpp @@ -32,21 +32,20 @@ #include UAVObjectBrowserFactory::UAVObjectBrowserFactory(QObject *parent) : - IUAVGadgetFactory(QString("UAVObjectBrowser"), tr("UAVObject Browser"), parent) -{ -} + IUAVGadgetFactory(QString("UAVObjectBrowser"), tr("UAVObject Browser"), parent) +{} UAVObjectBrowserFactory::~UAVObjectBrowserFactory() -{ -} +{} -Core::IUAVGadget* UAVObjectBrowserFactory::createGadget(QWidget *parent) +Core::IUAVGadget *UAVObjectBrowserFactory::createGadget(QWidget *parent) { - UAVObjectBrowserWidget* gadgetWidget = new UAVObjectBrowserWidget(parent); + UAVObjectBrowserWidget *gadgetWidget = new UAVObjectBrowserWidget(parent); + return new UAVObjectBrowser(QString("UAVObjectBrowser"), gadgetWidget, parent); } -IUAVGadgetConfiguration *UAVObjectBrowserFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *UAVObjectBrowserFactory::createConfiguration(QSettings *qSettings) { return new UAVObjectBrowserConfiguration(QString("UAVObjectBrowser"), qSettings); } @@ -54,6 +53,5 @@ IUAVGadgetConfiguration *UAVObjectBrowserFactory::createConfiguration(QSettings* IOptionsPage *UAVObjectBrowserFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new UAVObjectBrowserOptionsPage(qobject_cast(config)); + return new UAVObjectBrowserOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.h index cf8e988a6..a85fbeb1d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.h @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class UAVObjectBrowserFactory : public Core::IUAVGadgetFactory -{ +class UAVObjectBrowserFactory : public Core::IUAVGadgetFactory { Q_OBJECT public: UAVObjectBrowserFactory(QObject *parent = 0); ~UAVObjectBrowserFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.cpp index b36a33de7..a6cce1027 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.cpp @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,8 +40,7 @@ UAVObjectBrowserOptionsPage::UAVObjectBrowserOptionsPage(UAVObjectBrowserConfiguration *config, QObject *parent) : IOptionsPage(parent), m_config(config) -{ -} +{} QWidget *UAVObjectBrowserOptionsPage::createPage(QWidget *parent) { @@ -55,7 +54,6 @@ QWidget *UAVObjectBrowserOptionsPage::createPage(QWidget *parent) m_page->hilightBox->setChecked(m_config->onlyHighlightChangedValues()); return w; - } void UAVObjectBrowserOptionsPage::apply() diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.h index c49b9d7b2..ebe3e21eb 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,12 +44,11 @@ class QSpinBox; using namespace Core; namespace Ui { - class UAVObjectBrowserOptionsPage; +class UAVObjectBrowserOptionsPage; } -class UAVObjectBrowserOptionsPage : public IOptionsPage -{ -Q_OBJECT +class UAVObjectBrowserOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit UAVObjectBrowserOptionsPage(UAVObjectBrowserConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp index 06e2168dc..9ada2e9fa 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp @@ -42,7 +42,7 @@ UAVObjectBrowserWidget::UAVObjectBrowserWidget(QWidget *parent) : QWidget(parent) { - m_browser = new Ui_UAVObjectBrowser(); + m_browser = new Ui_UAVObjectBrowser(); m_viewoptions = new Ui_viewoptions(); m_viewoptionsDialog = new QDialog(this); m_viewoptions->setupUi(m_viewoptionsDialog); @@ -50,13 +50,13 @@ UAVObjectBrowserWidget::UAVObjectBrowserWidget(QWidget *parent) : QWidget(parent m_model = new UAVObjectTreeModel(); m_browser->treeView->setModel(m_model); m_browser->treeView->setColumnWidth(0, 300); - //m_browser->treeView->expandAll(); + // m_browser->treeView->expandAll(); BrowserItemDelegate *m_delegate = new BrowserItemDelegate(); m_browser->treeView->setItemDelegate(m_delegate); m_browser->treeView->setEditTriggers(QAbstractItemView::AllEditTriggers); m_browser->treeView->setSelectionBehavior(QAbstractItemView::SelectItems); showMetaData(m_viewoptions->cbMetaData->isChecked()); - connect(m_browser->treeView->selectionModel(), SIGNAL(currentChanged(QModelIndex,QModelIndex)), this, SLOT(currentChanged(QModelIndex,QModelIndex))); + connect(m_browser->treeView->selectionModel(), SIGNAL(currentChanged(QModelIndex, QModelIndex)), this, SLOT(currentChanged(QModelIndex, QModelIndex))); connect(m_viewoptions->cbMetaData, SIGNAL(toggled(bool)), this, SLOT(showMetaData(bool))); connect(m_viewoptions->cbCategorized, SIGNAL(toggled(bool)), this, SLOT(categorize(bool))); connect(m_browser->saveSDButton, SIGNAL(clicked()), this, SLOT(saveObject())); @@ -64,7 +64,7 @@ UAVObjectBrowserWidget::UAVObjectBrowserWidget(QWidget *parent) : QWidget(parent connect(m_browser->eraseSDButton, SIGNAL(clicked()), this, SLOT(eraseObject())); connect(m_browser->sendButton, SIGNAL(clicked()), this, SLOT(sendUpdate())); connect(m_browser->requestButton, SIGNAL(clicked()), this, SLOT(requestUpdate())); - connect(m_browser->tbView,SIGNAL(clicked()),this,SLOT(viewSlot())); + connect(m_browser->tbView, SIGNAL(clicked()), this, SLOT(viewSlot())); connect(m_viewoptions->cbScientific, SIGNAL(toggled(bool)), this, SLOT(useScientificNotation(bool))); connect(m_viewoptions->cbScientific, SIGNAL(toggled(bool)), this, SLOT(viewOptionsChangedSlot())); connect(m_viewoptions->cbMetaData, SIGNAL(toggled(bool)), this, SLOT(viewOptionsChangedSlot())); @@ -87,8 +87,7 @@ void UAVObjectBrowserWidget::setViewOptions(bool categorized, bool scientific, b void UAVObjectBrowserWidget::showMetaData(bool show) { QList metaIndexes = m_model->getMetaDataIndexes(); - foreach(QModelIndex index , metaIndexes) - { + foreach(QModelIndex index, metaIndexes) { m_browser->treeView->setRowHidden(index.row(), index.parent(), !show); } } @@ -96,12 +95,13 @@ void UAVObjectBrowserWidget::showMetaData(bool show) void UAVObjectBrowserWidget::categorize(bool categorize) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm); UAVObjectManager *objManager = pm->getObject(); Q_ASSERT(objManager); - UAVObjectTreeModel* tmpModel = m_model; - m_model = new UAVObjectTreeModel(0, categorize,m_viewoptions->cbScientific->isChecked()); + UAVObjectTreeModel *tmpModel = m_model; + m_model = new UAVObjectTreeModel(0, categorize, m_viewoptions->cbScientific->isChecked()); m_model->setRecentlyUpdatedColor(m_recentlyUpdatedColor); m_model->setManuallyChangedColor(m_manuallyChangedColor); m_model->setRecentlyUpdatedTimeout(m_recentlyUpdatedTimeout); @@ -115,12 +115,13 @@ void UAVObjectBrowserWidget::categorize(bool categorize) void UAVObjectBrowserWidget::useScientificNotation(bool scientific) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm); UAVObjectManager *objManager = pm->getObject(); Q_ASSERT(objManager); - UAVObjectTreeModel* tmpModel = m_model; - m_model = new UAVObjectTreeModel(0, m_viewoptions->cbCategorized,scientific); + UAVObjectTreeModel *tmpModel = m_model; + m_model = new UAVObjectTreeModel(0, m_viewoptions->cbCategorized, scientific); m_model->setRecentlyUpdatedColor(m_recentlyUpdatedColor); m_model->setManuallyChangedColor(m_manuallyChangedColor); m_model->setRecentlyUpdatedTimeout(m_recentlyUpdatedTimeout); @@ -144,6 +145,7 @@ void UAVObjectBrowserWidget::sendUpdate() void UAVObjectBrowserWidget::requestUpdate() { ObjectTreeItem *objItem = findCurrentObjectTreeItem(); + Q_ASSERT(objItem); UAVObject *obj = objItem->object(); Q_ASSERT(obj); @@ -152,13 +154,15 @@ void UAVObjectBrowserWidget::requestUpdate() ObjectTreeItem *UAVObjectBrowserWidget::findCurrentObjectTreeItem() { - QModelIndex current = m_browser->treeView->currentIndex(); - TreeItem *item = static_cast(current.internalPointer()); + QModelIndex current = m_browser->treeView->currentIndex(); + TreeItem *item = static_cast(current.internalPointer()); ObjectTreeItem *objItem = 0; + while (item) { - objItem = dynamic_cast(item); - if (objItem) + objItem = dynamic_cast(item); + if (objItem) { break; + } item = item->parent(); } return objItem; @@ -181,6 +185,7 @@ void UAVObjectBrowserWidget::loadObject() { // Load object ObjectTreeItem *objItem = findCurrentObjectTreeItem(); + Q_ASSERT(objItem); UAVObject *obj = objItem->object(); Q_ASSERT(obj); @@ -192,6 +197,7 @@ void UAVObjectBrowserWidget::loadObject() void UAVObjectBrowserWidget::eraseObject() { ObjectTreeItem *objItem = findCurrentObjectTreeItem(); + Q_ASSERT(objItem); UAVObject *obj = objItem->object(); Q_ASSERT(obj); @@ -202,13 +208,13 @@ void UAVObjectBrowserWidget::updateObjectPersistance(ObjectPersistence::Operatio { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - ObjectPersistence* objper = dynamic_cast( objManager->getObject(ObjectPersistence::NAME) ); - if (obj != NULL) - { + ObjectPersistence *objper = dynamic_cast(objManager->getObject(ObjectPersistence::NAME)); + + if (obj != NULL) { ObjectPersistence::DataFields data; - data.Operation = op; - data.Selection = ObjectPersistence::SELECTION_SINGLEOBJECT; - data.ObjectID = obj->getObjID(); + data.Operation = op; + data.Selection = ObjectPersistence::SELECTION_SINGLEOBJECT; + data.ObjectID = obj->getObjID(); data.InstanceID = obj->getInstID(); objper->setData(data); objper->updated(); @@ -219,25 +225,26 @@ void UAVObjectBrowserWidget::currentChanged(const QModelIndex ¤t, const QM { Q_UNUSED(previous); - TreeItem *item = static_cast(current.internalPointer()); - bool enable = true; - if (current == QModelIndex()) + TreeItem *item = static_cast(current.internalPointer()); + bool enable = true; + if (current == QModelIndex()) { enable = false; - TopTreeItem *top = dynamic_cast(item); - ObjectTreeItem *data = dynamic_cast(item); - if (top || (data && !data->object())) + } + TopTreeItem *top = dynamic_cast(item); + ObjectTreeItem *data = dynamic_cast(item); + if (top || (data && !data->object())) { enable = false; + } enableSendRequest(enable); } void UAVObjectBrowserWidget::viewSlot() { - if(m_viewoptionsDialog->isVisible()) + if (m_viewoptionsDialog->isVisible()) { m_viewoptionsDialog->setVisible(false); - else - { - QPoint pos=QCursor::pos(); - pos.setX(pos.x()-m_viewoptionsDialog->width()); + } else { + QPoint pos = QCursor::pos(); + pos.setX(pos.x() - m_viewoptionsDialog->width()); m_viewoptionsDialog->move(pos); m_viewoptionsDialog->show(); } @@ -245,7 +252,7 @@ void UAVObjectBrowserWidget::viewSlot() void UAVObjectBrowserWidget::viewOptionsChangedSlot() { - emit viewOptionsChanged(m_viewoptions->cbCategorized->isChecked(),m_viewoptions->cbScientific->isChecked(),m_viewoptions->cbMetaData->isChecked()); + emit viewOptionsChanged(m_viewoptions->cbCategorized->isChecked(), m_viewoptions->cbScientific->isChecked(), m_viewoptions->cbMetaData->isChecked()); } void UAVObjectBrowserWidget::enableSendRequest(bool enable) @@ -256,5 +263,3 @@ void UAVObjectBrowserWidget::enableSendRequest(bool enable) m_browser->readSDButton->setEnabled(enable); m_browser->eraseSDButton->setEnabled(enable); } - - diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h index 805bbbaf8..707473e3d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h @@ -38,18 +38,29 @@ class ObjectTreeItem; class Ui_UAVObjectBrowser; class Ui_viewoptions; -class UAVObjectBrowserWidget : public QWidget -{ +class UAVObjectBrowserWidget : public QWidget { Q_OBJECT public: UAVObjectBrowserWidget(QWidget *parent = 0); ~UAVObjectBrowserWidget(); - void setRecentlyUpdatedColor(QColor color) { m_recentlyUpdatedColor = color; m_model->setRecentlyUpdatedColor(color); } - void setManuallyChangedColor(QColor color) { m_manuallyChangedColor = color; m_model->setManuallyChangedColor(color); } - void setRecentlyUpdatedTimeout(int timeout) { m_recentlyUpdatedTimeout = timeout; m_model->setRecentlyUpdatedTimeout(timeout); } - void setOnlyHilightChangedValues(bool hilight) { m_onlyHilightChangedValues = hilight; m_model->setOnlyHilightChangedValues(hilight); } - void setViewOptions(bool categorized,bool scientific,bool metadata); + void setRecentlyUpdatedColor(QColor color) + { + m_recentlyUpdatedColor = color; m_model->setRecentlyUpdatedColor(color); + } + void setManuallyChangedColor(QColor color) + { + m_manuallyChangedColor = color; m_model->setManuallyChangedColor(color); + } + void setRecentlyUpdatedTimeout(int timeout) + { + m_recentlyUpdatedTimeout = timeout; m_model->setRecentlyUpdatedTimeout(timeout); + } + void setOnlyHilightChangedValues(bool hilight) + { + m_onlyHilightChangedValues = hilight; m_model->setOnlyHilightChangedValues(hilight); + } + void setViewOptions(bool categorized, bool scientific, bool metadata); public slots: void showMetaData(bool show); void categorize(bool categorize); @@ -65,7 +76,7 @@ private slots: void viewSlot(); void viewOptionsChangedSlot(); signals: - void viewOptionsChanged(bool categorized,bool scientific,bool metadata); + void viewOptionsChanged(bool categorized, bool scientific, bool metadata); private: QPushButton *m_requestUpdate; QPushButton *m_sendUpdate; diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp index 06c9f0016..42a9289b3 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,25 +33,25 @@ #include "uavobjectfield.h" #include "extensionsystem/pluginmanager.h" #include -//#include +// #include #include #include #include UAVObjectTreeModel::UAVObjectTreeModel(QObject *parent, bool categorize, bool useScientificNotation) : - QAbstractItemModel(parent), - m_useScientificFloatNotation(useScientificNotation), - m_recentlyUpdatedTimeout(500), // ms - m_recentlyUpdatedColor(QColor(255, 230, 230)), - m_manuallyChangedColor(QColor(230, 230, 255)) + QAbstractItemModel(parent), + m_useScientificFloatNotation(useScientificNotation), + m_recentlyUpdatedTimeout(500), // ms + m_recentlyUpdatedColor(QColor(255, 230, 230)), + m_manuallyChangedColor(QColor(230, 230, 255)) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); // Create highlight manager, let it run every 300 ms. m_highlightManager = new HighLightManager(300); - connect(objManager, SIGNAL(newObject(UAVObject*)), this, SLOT(newObject(UAVObject*))); - connect(objManager, SIGNAL(newInstance(UAVObject*)), this, SLOT(newObject(UAVObject*))); + connect(objManager, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *))); + connect(objManager, SIGNAL(newInstance(UAVObject *)), this, SLOT(newObject(UAVObject *))); TreeItem::setHighlightTime(m_recentlyUpdatedTimeout); setupModelData(objManager, categorize); @@ -68,21 +68,21 @@ void UAVObjectTreeModel::setupModelData(UAVObjectManager *objManager, bool categ // root QList rootData; rootData << tr("Property") << tr("Value") << tr("Unit"); - m_rootItem = new TreeItem(rootData); + m_rootItem = new TreeItem(rootData); - m_settingsTree = new TopTreeItem(tr("Settings"), m_rootItem); + m_settingsTree = new TopTreeItem(tr("Settings"), m_rootItem); m_settingsTree->setHighlightManager(m_highlightManager); m_rootItem->appendChild(m_settingsTree); m_nonSettingsTree = new TopTreeItem(tr("Data Objects"), m_rootItem); m_nonSettingsTree->setHighlightManager(m_highlightManager); m_rootItem->appendChild(m_nonSettingsTree); m_rootItem->setHighlightManager(m_highlightManager); - connect(m_settingsTree, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); - connect(m_nonSettingsTree, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(m_settingsTree, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); + connect(m_nonSettingsTree, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); - QList< QList > objList = objManager->getDataObjects(); - foreach (QList list, objList) { - foreach (UAVDataObject* obj, list) { + QList< QList > objList = objManager->getDataObjects(); + foreach(QList list, objList) { + foreach(UAVDataObject * obj, list) { addDataObject(obj, categorize); } } @@ -90,7 +90,8 @@ void UAVObjectTreeModel::setupModelData(UAVObjectManager *objManager, bool categ void UAVObjectTreeModel::newObject(UAVObject *obj) { - UAVDataObject *dobj = qobject_cast(obj); + UAVDataObject *dobj = qobject_cast(obj); + if (dobj) { addDataObject(dobj); } @@ -100,56 +101,57 @@ void UAVObjectTreeModel::addDataObject(UAVDataObject *obj, bool categorize) { TopTreeItem *root = obj->isSettings() ? m_settingsTree : m_nonSettingsTree; - TreeItem* parent = root; + TreeItem *parent = root; - if(categorize && obj->getCategory() != 0 && !obj->getCategory().isEmpty()) { + if (categorize && obj->getCategory() != 0 && !obj->getCategory().isEmpty()) { QStringList categoryPath = obj->getCategory().split('/'); parent = createCategoryItems(categoryPath, root); } - ObjectTreeItem* existing = root->findDataObjectTreeItemByObjectId(obj->getObjID()); + ObjectTreeItem *existing = root->findDataObjectTreeItemByObjectId(obj->getObjID()); if (existing) { addInstance(obj, existing); } else { DataObjectTreeItem *dataTreeItem = new DataObjectTreeItem(obj->getName() + " (" + QString::number(obj->getNumBytes()) + " bytes)"); dataTreeItem->setHighlightManager(m_highlightManager); - connect(dataTreeItem, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(dataTreeItem, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); parent->insertChild(dataTreeItem); root->addObjectTreeItem(obj->getObjID(), dataTreeItem); UAVMetaObject *meta = obj->getMetaObject(); - MetaObjectTreeItem* metaTreeItem = addMetaObject(meta, dataTreeItem); + MetaObjectTreeItem *metaTreeItem = addMetaObject(meta, dataTreeItem); root->addMetaObjectTreeItem(meta->getObjID(), metaTreeItem); addInstance(obj, dataTreeItem); } } -TreeItem* UAVObjectTreeModel::createCategoryItems(QStringList categoryPath, TreeItem* root) +TreeItem *UAVObjectTreeModel::createCategoryItems(QStringList categoryPath, TreeItem *root) { - TreeItem* parent = root; + TreeItem *parent = root; + foreach(QString category, categoryPath) { - TreeItem* existing = parent->findChildByName(category); - if(!existing) { - TreeItem* categoryItem = new TreeItem(category); - connect(categoryItem, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + TreeItem *existing = parent->findChildByName(category); + + if (!existing) { + TreeItem *categoryItem = new TreeItem(category); + connect(categoryItem, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); categoryItem->setHighlightManager(m_highlightManager); parent->insertChild(categoryItem); parent = categoryItem; - } - else { + } else { parent = existing; } } return parent; } -MetaObjectTreeItem* UAVObjectTreeModel::addMetaObject(UAVMetaObject *obj, TreeItem *parent) +MetaObjectTreeItem *UAVObjectTreeModel::addMetaObject(UAVMetaObject *obj, TreeItem *parent) { - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(highlightUpdatedObject(UAVObject*))); + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(highlightUpdatedObject(UAVObject *))); MetaObjectTreeItem *meta = new MetaObjectTreeItem(obj, tr("Meta Data")); meta->setHighlightManager(m_highlightManager); - connect(meta, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); - foreach (UAVObjectField *field, obj->getFields()) { + connect(meta, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); + foreach(UAVObjectField * field, obj->getFields()) { if (field->getNumElements() > 1) { addArrayField(field, meta); } else { @@ -162,20 +164,20 @@ MetaObjectTreeItem* UAVObjectTreeModel::addMetaObject(UAVMetaObject *obj, TreeIt void UAVObjectTreeModel::addInstance(UAVObject *obj, TreeItem *parent) { - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(highlightUpdatedObject(UAVObject*))); + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(highlightUpdatedObject(UAVObject *))); TreeItem *item; if (obj->isSingleInstance()) { item = parent; - DataObjectTreeItem *p = static_cast(parent); + DataObjectTreeItem *p = static_cast(parent); p->setObject(obj); } else { - QString name = tr("Instance") + " " + QString::number(obj->getInstID()); + QString name = tr("Instance") + " " + QString::number(obj->getInstID()); item = new InstanceTreeItem(obj, name); item->setHighlightManager(m_highlightManager); - connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(item, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); parent->appendChild(item); } - foreach (UAVObjectField *field, obj->getFields()) { + foreach(UAVObjectField * field, obj->getFields()) { if (field->getNumElements() > 1) { addArrayField(field, item); } else { @@ -187,8 +189,9 @@ void UAVObjectTreeModel::addInstance(UAVObject *obj, TreeItem *parent) void UAVObjectTreeModel::addArrayField(UAVObjectField *field, TreeItem *parent) { TreeItem *item = new ArrayFieldTreeItem(field->getName()); + item->setHighlightManager(m_highlightManager); - connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(item, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); for (uint i = 0; i < field->getNumElements(); ++i) { addSingleField(i, field, item); } @@ -198,19 +201,21 @@ void UAVObjectTreeModel::addArrayField(UAVObjectField *field, TreeItem *parent) void UAVObjectTreeModel::addSingleField(int index, UAVObjectField *field, TreeItem *parent) { QList data; - if (field->getNumElements() == 1) + if (field->getNumElements() == 1) { data.append(field->getName()); - else - data.append( QString("[%1]").arg((field->getElementNames())[index]) ); + } else { + data.append(QString("[%1]").arg((field->getElementNames())[index])); + } FieldTreeItem *item; UAVObjectField::FieldType type = field->getType(); switch (type) { case UAVObjectField::BITFIELD: - case UAVObjectField::ENUM: { + case UAVObjectField::ENUM: + { QStringList options = field->getOptions(); QVariant value = field->getValue(); - data.append( options.indexOf(value.toString()) ); + data.append(options.indexOf(value.toString())); data.append(field->getUnits()); item = new EnumFieldTreeItem(field, index, data); break; @@ -234,42 +239,47 @@ void UAVObjectTreeModel::addSingleField(int index, UAVObjectField *field, TreeIt Q_ASSERT(false); } item->setHighlightManager(m_highlightManager); - connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(item, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); parent->appendChild(item); } QModelIndex UAVObjectTreeModel::index(int row, int column, const QModelIndex &parent) - const +const { - if (!hasIndex(row, column, parent)) + if (!hasIndex(row, column, parent)) { return QModelIndex(); + } TreeItem *parentItem; - if (!parent.isValid()) + if (!parent.isValid()) { parentItem = m_rootItem; - else - parentItem = static_cast(parent.internalPointer()); + } else { + parentItem = static_cast(parent.internalPointer()); + } TreeItem *childItem = parentItem->getChild(row); - if (childItem) + if (childItem) { return createIndex(row, column, childItem); - else + } else { return QModelIndex(); + } } QModelIndex UAVObjectTreeModel::index(TreeItem *item) { - if (item->parent() == 0) + if (item->parent() == 0) { return QModelIndex(); + } QModelIndex root = index(item->parent()); for (int i = 0; i < rowCount(root); ++i) { QModelIndex childIndex = index(i, 0, root); - TreeItem *child = static_cast(childIndex.internalPointer()); - if (child == item) + TreeItem *child = static_cast(childIndex.internalPointer()); + if (child == item) { return childIndex; + } } Q_ASSERT(false); return QModelIndex(); @@ -277,14 +287,16 @@ QModelIndex UAVObjectTreeModel::index(TreeItem *item) QModelIndex UAVObjectTreeModel::parent(const QModelIndex &index) const { - if (!index.isValid()) + if (!index.isValid()) { return QModelIndex(); + } - TreeItem *childItem = static_cast(index.internalPointer()); + TreeItem *childItem = static_cast(index.internalPointer()); TreeItem *parentItem = childItem->parent(); - if (parentItem == m_rootItem) + if (parentItem == m_rootItem) { return QModelIndex(); + } return createIndex(parentItem->row(), 0, parentItem); } @@ -292,35 +304,37 @@ QModelIndex UAVObjectTreeModel::parent(const QModelIndex &index) const int UAVObjectTreeModel::rowCount(const QModelIndex &parent) const { TreeItem *parentItem; - if (parent.column() > 0) - return 0; - if (!parent.isValid()) + if (parent.column() > 0) { + return 0; + } + + if (!parent.isValid()) { parentItem = m_rootItem; - else - parentItem = static_cast(parent.internalPointer()); + } else { + parentItem = static_cast(parent.internalPointer()); + } return parentItem->childCount(); } int UAVObjectTreeModel::columnCount(const QModelIndex &parent) const { - if (parent.isValid()) - return static_cast(parent.internalPointer())->columnCount(); - else + if (parent.isValid()) { + return static_cast(parent.internalPointer())->columnCount(); + } else { return m_rootItem->columnCount(); + } } QList UAVObjectTreeModel::getMetaDataIndexes() { QList metaIndexes; - foreach(MetaObjectTreeItem *metaItem , m_settingsTree->getMetaObjectItems()) - { + foreach(MetaObjectTreeItem * metaItem, m_settingsTree->getMetaObjectItems()) { metaIndexes.append(index(metaItem)); } - foreach(MetaObjectTreeItem *metaItem , m_nonSettingsTree->getMetaObjectItems()) - { + foreach(MetaObjectTreeItem * metaItem, m_nonSettingsTree->getMetaObjectItems()) { metaIndexes.append(index(metaItem)); } return metaIndexes; @@ -328,43 +342,48 @@ QList UAVObjectTreeModel::getMetaDataIndexes() QVariant UAVObjectTreeModel::data(const QModelIndex &index, int role) const { - if (!index.isValid()) - return QVariant(); + if (!index.isValid()) { + return QVariant(); + } if (index.column() == TreeItem::dataColumn && role == Qt::EditRole) { - TreeItem *item = static_cast(index.internalPointer()); + TreeItem *item = static_cast(index.internalPointer()); return item->data(index.column()); } -// if (role == Qt::DecorationRole) -// return QIcon(":/core/images/openpilot_logo_128.png"); +// if (role == Qt::DecorationRole) +// return QIcon(":/core/images/openpilot_logo_128.png"); if (role == Qt::ToolTipRole) { - TreeItem *item = static_cast(index.internalPointer()); + TreeItem *item = static_cast(index.internalPointer()); return item->description(); } - TreeItem *item = static_cast(index.internalPointer()); + TreeItem *item = static_cast(index.internalPointer()); if (index.column() == 0 && role == Qt::BackgroundRole) { - if (!dynamic_cast(item) && item->highlighted()) + if (!dynamic_cast(item) && item->highlighted()) { return QVariant(m_recentlyUpdatedColor); + } } if (index.column() == TreeItem::dataColumn && role == Qt::BackgroundRole) { - FieldTreeItem *fieldItem = dynamic_cast(item); - if (fieldItem && fieldItem->highlighted()) + FieldTreeItem *fieldItem = dynamic_cast(item); + if (fieldItem && fieldItem->highlighted()) { return QVariant(m_recentlyUpdatedColor); + } - if (fieldItem && fieldItem->changed()) - return QVariant(m_manuallyChangedColor); + if (fieldItem && fieldItem->changed()) { + return QVariant(m_manuallyChangedColor); + } } - if (role != Qt::DisplayRole) + if (role != Qt::DisplayRole) { return QVariant(); + } if (index.column() == TreeItem::dataColumn) { - EnumFieldTreeItem *fieldItem = dynamic_cast(item); + EnumFieldTreeItem *fieldItem = dynamic_cast(item); if (fieldItem) { int enumIndex = fieldItem->data(index.column()).toInt(); return fieldItem->enumOptions(enumIndex); @@ -377,7 +396,7 @@ QVariant UAVObjectTreeModel::data(const QModelIndex &index, int role) const bool UAVObjectTreeModel::setData(const QModelIndex &index, const QVariant & value, int role) { Q_UNUSED(role) - TreeItem *item = static_cast(index.internalPointer()); + TreeItem * item = static_cast(index.internalPointer()); item->setData(value, index.column()); return true; } @@ -385,13 +404,15 @@ bool UAVObjectTreeModel::setData(const QModelIndex &index, const QVariant & valu Qt::ItemFlags UAVObjectTreeModel::flags(const QModelIndex &index) const { - if (!index.isValid()) + if (!index.isValid()) { return 0; + } if (index.column() == TreeItem::dataColumn) { - TreeItem *item = static_cast(index.internalPointer()); - if (item->isEditable()) + TreeItem *item = static_cast(index.internalPointer()); + if (item->isEditable()) { return Qt::ItemIsEnabled | Qt::ItemIsSelectable | Qt::ItemIsEditable; + } } return Qt::ItemIsEnabled | Qt::ItemIsSelectable; @@ -400,8 +421,9 @@ Qt::ItemFlags UAVObjectTreeModel::flags(const QModelIndex &index) const QVariant UAVObjectTreeModel::headerData(int section, Qt::Orientation orientation, int role) const { - if (orientation == Qt::Horizontal && role == Qt::DisplayRole) + if (orientation == Qt::Horizontal && role == Qt::DisplayRole) { return m_rootItem->data(section); + } return QVariant(); } @@ -411,11 +433,11 @@ void UAVObjectTreeModel::highlightUpdatedObject(UAVObject *obj) Q_ASSERT(obj); ObjectTreeItem *item = findObjectTreeItem(obj); Q_ASSERT(item); - if(!m_onlyHilightChangedValues){ + if (!m_onlyHilightChangedValues) { item->setHighlight(true); } item->update(); - if(!m_onlyHilightChangedValues){ + if (!m_onlyHilightChangedValues) { QModelIndex itemIndex = index(item); Q_ASSERT(itemIndex != QModelIndex()); emit dataChanged(itemIndex, itemIndex); @@ -424,8 +446,9 @@ void UAVObjectTreeModel::highlightUpdatedObject(UAVObject *obj) ObjectTreeItem *UAVObjectTreeModel::findObjectTreeItem(UAVObject *object) { - UAVDataObject *dataObject = qobject_cast(object); - UAVMetaObject *metaObject = qobject_cast(object); + UAVDataObject *dataObject = qobject_cast(object); + UAVMetaObject *metaObject = qobject_cast(object); + Q_ASSERT(dataObject || metaObject); if (dataObject) { return findDataObjectTreeItem(dataObject); @@ -435,15 +458,17 @@ ObjectTreeItem *UAVObjectTreeModel::findObjectTreeItem(UAVObject *object) return 0; } -DataObjectTreeItem* UAVObjectTreeModel::findDataObjectTreeItem(UAVDataObject *obj) +DataObjectTreeItem *UAVObjectTreeModel::findDataObjectTreeItem(UAVDataObject *obj) { TopTreeItem *root = obj->isSettings() ? m_settingsTree : m_nonSettingsTree; + return root->findDataObjectTreeItemByObjectId(obj->getObjID()); } -MetaObjectTreeItem* UAVObjectTreeModel::findMetaObjectTreeItem(UAVMetaObject *obj) +MetaObjectTreeItem *UAVObjectTreeModel::findMetaObjectTreeItem(UAVMetaObject *obj) { - UAVDataObject *dataObject = qobject_cast(obj->getParentObject()); + UAVDataObject *dataObject = qobject_cast(obj->getParentObject()); + Q_ASSERT(dataObject); TopTreeItem *root = dataObject->isSettings() ? m_settingsTree : m_nonSettingsTree; return root->findMetaObjectTreeItemByObjectId(obj->getObjID()); @@ -452,8 +477,7 @@ MetaObjectTreeItem* UAVObjectTreeModel::findMetaObjectTreeItem(UAVMetaObject *ob void UAVObjectTreeModel::updateHighlight(TreeItem *item) { QModelIndex itemIndex = index(item); + Q_ASSERT(itemIndex != QModelIndex()); emit dataChanged(itemIndex, itemIndex.sibling(itemIndex.row(), TreeItem::dataColumn)); } - - diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.h index 174210bd2..85e48159a 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -45,11 +45,10 @@ class UAVObjectManager; class QSignalMapper; class QTimer; -class UAVObjectTreeModel : public QAbstractItemModel -{ -Q_OBJECT +class UAVObjectTreeModel : public QAbstractItemModel { + Q_OBJECT public: - explicit UAVObjectTreeModel(QObject *parent = 0, bool categorize=true, bool useScientificNotation=false); + explicit UAVObjectTreeModel(QObject *parent = 0, bool categorize = true, bool useScientificNotation = false); ~UAVObjectTreeModel(); QVariant data(const QModelIndex &index, int role) const; @@ -63,13 +62,23 @@ public: int rowCount(const QModelIndex &parent = QModelIndex()) const; int columnCount(const QModelIndex &parent = QModelIndex()) const; - void setRecentlyUpdatedColor(QColor color) { m_recentlyUpdatedColor = color; } - void setManuallyChangedColor(QColor color) { m_manuallyChangedColor = color; } - void setRecentlyUpdatedTimeout(int timeout) { + void setRecentlyUpdatedColor(QColor color) + { + m_recentlyUpdatedColor = color; + } + void setManuallyChangedColor(QColor color) + { + m_manuallyChangedColor = color; + } + void setRecentlyUpdatedTimeout(int timeout) + { m_recentlyUpdatedTimeout = timeout; TreeItem::setHighlightTime(timeout); } - void setOnlyHilightChangedValues(bool hilight) {m_onlyHilightChangedValues = hilight; } + void setOnlyHilightChangedValues(bool hilight) + { + m_onlyHilightChangedValues = hilight; + } QList getMetaDataIndexes(); @@ -80,7 +89,7 @@ public slots: private slots: void highlightUpdatedObject(UAVObject *obj); - void updateHighlight(TreeItem*); + void updateHighlight(TreeItem *); private: void setupModelData(UAVObjectManager *objManager, bool categorize = true); diff --git a/ground/openpilotgcs/src/plugins/uavobjects/tests/main.cpp b/ground/openpilotgcs/src/plugins/uavobjects/tests/main.cpp index 2ffbb23b0..2f4b9bcda 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/tests/main.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/tests/main.cpp @@ -6,7 +6,7 @@ int main(int argc, char *argv[]) { QCoreApplication a(argc, argv); - UAVObjectsTest* test = new UAVObjectsTest(); + UAVObjectsTest *test = new UAVObjectsTest(); return a.exec(); } diff --git a/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.cpp b/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.cpp index 6b86ea6f9..8259dde64 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.cpp @@ -1,19 +1,19 @@ #include "uavobjectstest.h" -UAVObjectsTest::UAVObjectsTest(): sout(stdout), done(false) +UAVObjectsTest::UAVObjectsTest() : sout(stdout), done(false) { // Create object manager objMngr = new UAVObjectManager(); - connect(objMngr, SIGNAL(newObject(UAVObject*)), this, SLOT(newObject(UAVObject*))); - connect(objMngr, SIGNAL(newInstance(UAVObject*)), this, SLOT(newInstance(UAVObject*))); + connect(objMngr, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *))); + connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *))); // Create test objects obj1 = new ExampleObject1(); - connect(obj1, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objectUpdated(UAVObject*))); - connect(obj1, SIGNAL(objectUpdatedAuto(UAVObject*)), this, SLOT(objectUpdatedAuto(UAVObject*))); - connect(obj1, SIGNAL(objectUpdatedManual(UAVObject*)), this, SLOT(objectUpdatedManual(UAVObject*))); - connect(obj1, SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(objectUnpacked(UAVObject*))); - connect(obj1, SIGNAL(updateRequested(UAVObject*)), this, SLOT(updateRequested(UAVObject*))); + connect(obj1, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(objectUpdated(UAVObject *))); + connect(obj1, SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *))); + connect(obj1, SIGNAL(objectUpdatedManual(UAVObject *)), this, SLOT(objectUpdatedManual(UAVObject *))); + connect(obj1, SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *))); + connect(obj1, SIGNAL(updateRequested(UAVObject *)), this, SLOT(updateRequested(UAVObject *))); objMngr->registerObject(obj1); // Setup timer @@ -22,54 +22,53 @@ UAVObjectsTest::UAVObjectsTest(): sout(stdout), done(false) timer->start(1000); } -void UAVObjectsTest::objectUpdated(UAVObject* obj) +void UAVObjectsTest::objectUpdated(UAVObject *obj) { sout << QString("[Object Updated]\n%1").arg(obj->toString()); } -void UAVObjectsTest::objectUpdatedAuto(UAVObject* obj) +void UAVObjectsTest::objectUpdatedAuto(UAVObject *obj) { sout << QString("[Object Updated AUTO]\n%1").arg(obj->toString()); } -void UAVObjectsTest::objectUpdatedManual(UAVObject* obj) +void UAVObjectsTest::objectUpdatedManual(UAVObject *obj) { sout << QString("[Object Updated MANUAL]\n%1").arg(obj->toString()); } -void UAVObjectsTest::objectUnpacked(UAVObject* obj) +void UAVObjectsTest::objectUnpacked(UAVObject *obj) { sout << QString("[Object Updated UNPACKED]\n%1").arg(obj->toString()); } -void UAVObjectsTest::updateRequested(UAVObject* obj) +void UAVObjectsTest::updateRequested(UAVObject *obj) { sout << QString("[Object Update Requested]\n%1").arg(obj->toString()); } -void UAVObjectsTest::newObject(UAVObject* obj) +void UAVObjectsTest::newObject(UAVObject *obj) { - sout << QString("[New object]\n%1").arg(obj->toString()); + sout << QString("[New object]\n%1").arg(obj->toString()); } -void UAVObjectsTest::newInstance(UAVObject* obj) +void UAVObjectsTest::newInstance(UAVObject *obj) { sout << QString("[New instance]\n%1").arg(obj->toString()); } void UAVObjectsTest::runTest() { - if (!done) - { + if (!done) { // Create a new instance - ExampleObject1* obj2 = new ExampleObject1(); - objMngr->registerObject(obj2); + ExampleObject1 *obj2 = new ExampleObject1(); + objMngr->registerObject(obj2); // Set data ExampleObject1::DataFields data = obj1->getData(); - data.Field1 = 1; - data.Field2 = 2; - data.Field3 = 3; + data.Field1 = 1; + data.Field2 = 2; + data.Field3 = 3; data.Field4[0] = 4.1; data.Field4[1] = 4.2; data.Field4[2] = 4.3; @@ -86,15 +85,15 @@ void UAVObjectsTest::runTest() sout << obj2->getMetaObject()->toString(); // Create a new instance using clone() and an out of order instance ID - UAVDataObject* obj3 = obj2->clone(5); + UAVDataObject *obj3 = obj2->clone(5); objMngr->registerObject(obj3); // Pack, unpack testing - quint8* buf = new quint8[obj1->getNumBytes()]; + quint8 *buf = new quint8[obj1->getNumBytes()]; obj1->pack(buf); - data.Field1 = 10; - data.Field2 = 20; - data.Field3 = 30; + data.Field1 = 10; + data.Field2 = 20; + data.Field3 = 30; data.Field4[0] = 40.1; data.Field4[1] = 40.2; data.Field4[2] = 40.3; @@ -103,42 +102,39 @@ void UAVObjectsTest::runTest() // Save, load testing obj1->save(); - data.Field1 = 10; - data.Field2 = 20; - data.Field3 = 30; + data.Field1 = 10; + data.Field2 = 20; + data.Field3 = 30; data.Field4[0] = 40.1; data.Field4[1] = 40.2; data.Field4[2] = 40.3; obj1->setData(data); - obj1->load(); + obj1->load(); // Get all instances - QList objs = objMngr->getObjectInstances(ExampleObject1::OBJID); - for (int n = 0; n < objs.length(); ++n) - { + QList objs = objMngr->getObjectInstances(ExampleObject1::OBJID); + for (int n = 0; n < objs.length(); ++n) { sout << "[Printing object instances]\n"; sout << objs[n]->toString(); } // Get object fields QString objname("ExampleObject1"); - UAVObject* obj = objMngr->getObject(objname); - QList fields = obj->getFields(); + UAVObject *obj = objMngr->getObject(objname); + QList fields = obj->getFields(); // qint8 - UAVObjectFieldInt8* fieldint8 = dynamic_cast< UAVObjectFieldInt8* >(fields[0]); - if (fieldint8 != NULL) - { + UAVObjectFieldInt8 *fieldint8 = dynamic_cast< UAVObjectFieldInt8 * >(fields[0]); + if (fieldint8 != NULL) { fieldint8->setValue(10); qint8 value = fieldint8->getValue(); sout << value; } // enum - UAVObjectFieldEnum* fieldenum = dynamic_cast< UAVObjectFieldEnum* >(fields[7]); - if (fieldenum != NULL) - { + UAVObjectFieldEnum *fieldenum = dynamic_cast< UAVObjectFieldEnum * >(fields[7]); + if (fieldenum != NULL) { QStringList options = fieldenum->getOptions(); fieldenum->setSelected(options[1]); - QString selected = fieldenum->getSelected(); + QString selected = fieldenum->getSelected(); sout << selected; } diff --git a/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.h b/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.h index 46c390374..63ae2e7d1 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.h @@ -6,8 +6,7 @@ #include #include -class UAVObjectsTest: QObject -{ +class UAVObjectsTest : QObject { Q_OBJECT @@ -15,20 +14,20 @@ public: UAVObjectsTest(); private slots: - void objectUpdated(UAVObject* obj); - void objectUpdatedAuto(UAVObject* obj); - void objectUpdatedManual(UAVObject* obj); - void objectUpdatedPeriodic(UAVObject* obj); - void objectUnpacked(UAVObject* obj); - void updateRequested(UAVObject* obj); + void objectUpdated(UAVObject *obj); + void objectUpdatedAuto(UAVObject *obj); + void objectUpdatedManual(UAVObject *obj); + void objectUpdatedPeriodic(UAVObject *obj); + void objectUnpacked(UAVObject *obj); + void updateRequested(UAVObject *obj); void runTest(); - void newObject(UAVObject* obj); - void newInstance(UAVObject* obj); + void newObject(UAVObject *obj); + void newInstance(UAVObject *obj); private: - UAVObjectManager* objMngr; - ExampleObject1* obj1; - QTimer* timer; + UAVObjectManager *objMngr; + ExampleObject1 *obj1; + QTimer *timer; QTextStream sout; bool done; }; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.cpp index 046aff6e4..8a74eb97f 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.cpp @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavdataobject.h" @@ -30,8 +30,8 @@ /** * Constructor */ -UAVDataObject::UAVDataObject(quint32 objID, bool isSingleInst, bool isSet,const QString& name): - UAVObject(objID, isSingleInst, name) +UAVDataObject::UAVDataObject(quint32 objID, bool isSingleInst, bool isSet, const QString & name) : + UAVObject(objID, isSingleInst, name) { mobj = NULL; this->isSet = isSet; @@ -40,9 +40,10 @@ UAVDataObject::UAVDataObject(quint32 objID, bool isSingleInst, bool isSet,const /** * Initialize instance ID and assign a metaobject */ -void UAVDataObject::initialize(quint32 instID, UAVMetaObject* mobj) +void UAVDataObject::initialize(quint32 instID, UAVMetaObject *mobj) { QMutexLocker locker(mutex); + this->mobj = mobj; UAVObject::initialize(instID); } @@ -50,9 +51,10 @@ void UAVDataObject::initialize(quint32 instID, UAVMetaObject* mobj) /** * Assign a metaobject */ -void UAVDataObject::initialize(UAVMetaObject* mobj) +void UAVDataObject::initialize(UAVMetaObject *mobj) { QMutexLocker locker(mutex); + this->mobj = mobj; } @@ -67,10 +69,9 @@ bool UAVDataObject::isSettings() /** * Set the object's metadata */ -void UAVDataObject::setMetadata(const Metadata& mdata) +void UAVDataObject::setMetadata(const Metadata & mdata) { - if ( mobj!=NULL ) - { + if (mobj != NULL) { mobj->setData(mdata); } } @@ -80,12 +81,9 @@ void UAVDataObject::setMetadata(const Metadata& mdata) */ UAVObject::Metadata UAVDataObject::getMetadata(void) { - if ( mobj!=NULL) - { + if (mobj != NULL) { return mobj->getData(); - } - else - { + } else { return getDefaultMetadata(); } } @@ -93,8 +91,7 @@ UAVObject::Metadata UAVDataObject::getMetadata(void) /** * Get the metaobject */ -UAVMetaObject* UAVDataObject::getMetaObject() +UAVMetaObject *UAVDataObject::getMetaObject() { return mobj; } - diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.h b/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.h index e711d2162..fc679b7e1 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVDATAOBJECT_H @@ -34,25 +34,23 @@ #include "uavmetaobject.h" #include -class UAVOBJECTS_EXPORT UAVDataObject: public UAVObject -{ +class UAVOBJECTS_EXPORT UAVDataObject : public UAVObject { Q_OBJECT public: - UAVDataObject(quint32 objID, bool isSingleInst, bool isSet, const QString& name); - void initialize(quint32 instID, UAVMetaObject* mobj); - void initialize(UAVMetaObject* mobj); + UAVDataObject(quint32 objID, bool isSingleInst, bool isSet, const QString & name); + void initialize(quint32 instID, UAVMetaObject *mobj); + void initialize(UAVMetaObject *mobj); bool isSettings(); - void setMetadata(const Metadata& mdata); + void setMetadata(const Metadata & mdata); Metadata getMetadata(); - UAVMetaObject* getMetaObject(); - virtual UAVDataObject* clone(quint32 instID = 0) = 0; - virtual UAVDataObject* dirtyClone() = 0; + UAVMetaObject *getMetaObject(); + virtual UAVDataObject *clone(quint32 instID = 0) = 0; + virtual UAVDataObject *dirtyClone() = 0; private: - UAVMetaObject* mobj; + UAVMetaObject *mobj; bool isSet; - }; #endif // UAVDATAOBJECT_H diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp index d839d211f..92c6d8c3b 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavmetaobject.h" @@ -31,8 +31,8 @@ /** * Constructor */ -UAVMetaObject::UAVMetaObject(quint32 objID, const QString& name, UAVObject *parent): - UAVObject(objID, true, name) +UAVMetaObject::UAVMetaObject(quint32 objID, const QString & name, UAVObject *parent) : + UAVObject(objID, true, name) { this->parent = parent; // Setup default metadata of metaobject (can not be changed) @@ -40,14 +40,14 @@ UAVMetaObject::UAVMetaObject(quint32 objID, const QString& name, UAVObject *pare // Setup fields QStringList modesBitField; modesBitField << tr("FlightReadOnly") << tr("GCSReadOnly") << tr("FlightTelemetryAcked") << tr("GCSTelemetryAcked") << tr("FlightUpdatePeriodic") << tr("FlightUpdateOnChange") << tr("GCSUpdatePeriodic") << tr("GCSUpdateOnChange"); - QList fields; - fields.append( new UAVObjectField(tr("Modes"), tr("boolean"), UAVObjectField::BITFIELD, modesBitField, QStringList()) ); - fields.append( new UAVObjectField(tr("Flight Telemetry Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList()) ); - fields.append( new UAVObjectField(tr("GCS Telemetry Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList()) ); - fields.append( new UAVObjectField(tr("Logging Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList()) ); + QList fields; + fields.append(new UAVObjectField(tr("Modes"), tr("boolean"), UAVObjectField::BITFIELD, modesBitField, QStringList())); + fields.append(new UAVObjectField(tr("Flight Telemetry Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList())); + fields.append(new UAVObjectField(tr("GCS Telemetry Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList())); + fields.append(new UAVObjectField(tr("Logging Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList())); // Initialize parent UAVObject::initialize(0); - UAVObject::initializeFields(fields, (quint8*)&parentMetadata, sizeof(Metadata)); + UAVObject::initializeFields(fields, (quint8 *)&parentMetadata, sizeof(Metadata)); // Setup metadata of parent parentMetadata = parent->getDefaultMetadata(); } @@ -55,7 +55,7 @@ UAVMetaObject::UAVMetaObject(quint32 objID, const QString& name, UAVObject *pare /** * Get the parent object */ -UAVObject* UAVMetaObject::getParentObject() +UAVObject *UAVMetaObject::getParentObject() { return parent; } @@ -64,10 +64,10 @@ UAVObject* UAVMetaObject::getParentObject() * Set the metadata of the metaobject, this function will * do nothing since metaobjects have read-only metadata. */ -void UAVMetaObject::setMetadata(const Metadata& mdata) +void UAVMetaObject::setMetadata(const Metadata & mdata) { Q_UNUSED(mdata); - return; // can not update metaobject's metadata + // can not update metaobject's metadata } /** @@ -89,9 +89,10 @@ UAVObject::Metadata UAVMetaObject::getDefaultMetadata() /** * Set the metadata held by the metaobject */ -void UAVMetaObject::setData(const Metadata& mdata) +void UAVMetaObject::setData(const Metadata & mdata) { QMutexLocker locker(mutex); + parentMetadata = mdata; emit objectUpdatedAuto(this); // trigger object updated event emit objectUpdated(this); @@ -103,7 +104,6 @@ void UAVMetaObject::setData(const Metadata& mdata) UAVObject::Metadata UAVMetaObject::getData() { QMutexLocker locker(mutex); + return parentMetadata; } - - diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.h b/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.h index e3f270590..ef3be6a00 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVMETAOBJECT_H @@ -31,24 +31,22 @@ #include "uavobjects_global.h" #include "uavobject.h" -class UAVOBJECTS_EXPORT UAVMetaObject: public UAVObject -{ +class UAVOBJECTS_EXPORT UAVMetaObject : public UAVObject { Q_OBJECT public: - UAVMetaObject(quint32 objID, const QString& name, UAVObject* parent); - UAVObject* getParentObject(); - void setMetadata(const Metadata& mdata); + UAVMetaObject(quint32 objID, const QString & name, UAVObject *parent); + UAVObject *getParentObject(); + void setMetadata(const Metadata & mdata); Metadata getMetadata(); Metadata getDefaultMetadata(); - void setData(const Metadata& mdata); + void setData(const Metadata & mdata); Metadata getData(); private: - UAVObject* parent; + UAVObject *parent; Metadata ownMetadata; Metadata parentMetadata; - }; #endif // UAVMETAOBJECT_H diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp index 935a15d6b..d795e8cb3 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavobject.h" @@ -30,16 +30,16 @@ #include // Constants -#define UAVOBJ_ACCESS_SHIFT 0 -#define UAVOBJ_GCS_ACCESS_SHIFT 1 -#define UAVOBJ_TELEMETRY_ACKED_SHIFT 2 -#define UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT 3 -#define UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT 4 +#define UAVOBJ_ACCESS_SHIFT 0 +#define UAVOBJ_GCS_ACCESS_SHIFT 1 +#define UAVOBJ_TELEMETRY_ACKED_SHIFT 2 +#define UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT 3 +#define UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT 4 #define UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT 6 -#define UAVOBJ_UPDATE_MODE_MASK 0x3 +#define UAVOBJ_UPDATE_MODE_MASK 0x3 // Macros -#define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift); +#define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift); /** * Constructor @@ -47,13 +47,13 @@ * @param isSingleInst True if this object can only have a single instance * @param name Object name */ -UAVObject::UAVObject(quint32 objID, bool isSingleInst, const QString& name) +UAVObject::UAVObject(quint32 objID, bool isSingleInst, const QString & name) { - this->objID = objID; + this->objID = objID; this->instID = 0; this->isSingleInst = isSingleInst; - this->name = name; - this->mutex = new QMutex(QMutex::Recursive); + this->name = name; + this->mutex = new QMutex(QMutex::Recursive); } /** @@ -62,6 +62,7 @@ UAVObject::UAVObject(quint32 objID, bool isSingleInst, const QString& name) void UAVObject::initialize(quint32 instID) { QMutexLocker locker(mutex); + this->instID = instID; } @@ -71,30 +72,30 @@ void UAVObject::initialize(quint32 instID) * @param data Pointer to that actual object data, this is needed by the fields to access the data * @param numBytes Number of bytes in the object (total, including all fields) */ -void UAVObject::initializeFields(QList& fields, quint8* data, quint32 numBytes) +void UAVObject::initializeFields(QList & fields, quint8 *data, quint32 numBytes) { QMutexLocker locker(mutex); + this->numBytes = numBytes; - this->data = data; - this->fields = fields; + this->data = data; + this->fields = fields; // Initialize fields quint32 offset = 0; - for (int n = 0; n < fields.length(); ++n) - { + for (int n = 0; n < fields.length(); ++n) { fields[n]->initialize(data, offset, this); offset += fields[n]->getNumBytes(); - connect(fields[n], SIGNAL(fieldUpdated(UAVObjectField*)), this, SLOT(fieldUpdated(UAVObjectField*))); + connect(fields[n], SIGNAL(fieldUpdated(UAVObjectField *)), this, SLOT(fieldUpdated(UAVObjectField *))); } } /** * Called from the fields each time they are updated */ -void UAVObject::fieldUpdated(UAVObjectField* field) +void UAVObject::fieldUpdated(UAVObjectField *field) { Q_UNUSED(field); -// emit objectUpdatedAuto(this); // trigger object updated event -// emit objectUpdated(this); +// emit objectUpdatedAuto(this); // trigger object updated event +// emit objectUpdated(this); } /** @@ -140,7 +141,7 @@ QString UAVObject::getDescription() /** * Set the description of the object */ -void UAVObject::setDescription(const QString& description) +void UAVObject::setDescription(const QString & description) { this->description = description; } @@ -156,7 +157,7 @@ QString UAVObject::getCategory() /** * Set the category of the object */ -void UAVObject::setCategory(const QString& category) +void UAVObject::setCategory(const QString & category) { this->category = category; } @@ -214,7 +215,7 @@ void UAVObject::unlock() /** * Get object's mutex */ -QMutex* UAVObject::getMutex() +QMutex *UAVObject::getMutex() { return mutex; } @@ -225,15 +226,17 @@ QMutex* UAVObject::getMutex() qint32 UAVObject::getNumFields() { QMutexLocker locker(mutex); + return fields.count(); } /** * Get the object's fields */ -QList UAVObject::getFields() +QList UAVObject::getFields() { QMutexLocker locker(mutex); + return fields; } @@ -241,19 +244,18 @@ QList UAVObject::getFields() * Get a specific field * @returns The field or NULL if not found */ -UAVObjectField* UAVObject::getField(const QString& name) +UAVObjectField *UAVObject::getField(const QString & name) { QMutexLocker locker(mutex); + // Look for field - for (int n = 0; n < fields.length(); ++n) - { - if (name.compare(fields[n]->getName()) == 0) - { + for (int n = 0; n < fields.length(); ++n) { + if (name.compare(fields[n]->getName()) == 0) { return fields[n]; } } // If this point is reached then the field was not found - qWarning()<<"UAVObject::getField Non existant field "<pack(&dataOut[offset]); offset += fields[n]->getNumBytes(); } @@ -277,12 +279,12 @@ qint32 UAVObject::pack(quint8* dataOut) * Unpack the object data from a byte array * @returns The number of bytes copied */ -qint32 UAVObject::unpack(const quint8* dataIn) +qint32 UAVObject::unpack(const quint8 *dataIn) { QMutexLocker locker(mutex); qint32 offset = 0; - for (int n = 0; n < fields.length(); ++n) - { + + for (int n = 0; n < fields.length(); ++n) { fields[n]->unpack(&dataIn[offset]); offset += fields[n]->getNumBytes(); } @@ -305,14 +307,13 @@ bool UAVObject::save() // Open file QFile file(name + ".uavobj"); - if (!file.open(QFile::WriteOnly)) - { + + if (!file.open(QFile::WriteOnly)) { return false; } // Write object - if ( !save(file) ) - { + if (!save(file)) { return false; } @@ -327,7 +328,7 @@ bool UAVObject::save() * The data will be appended and the file will not be closed. * @returns True on success, false on failure */ -bool UAVObject::save(QFile& file) +bool UAVObject::save(QFile & file) { QMutexLocker locker(mutex); quint8 buffer[numBytes]; @@ -335,25 +336,21 @@ bool UAVObject::save(QFile& file) // Write the object ID qToLittleEndian(objID, tmpId); - if ( file.write((const char*)tmpId, 4) == -1 ) - { + if (file.write((const char *)tmpId, 4) == -1) { return false; } // Write the instance ID - if (!isSingleInst) - { + if (!isSingleInst) { qToLittleEndian(instID, tmpId); - if ( file.write((const char*)tmpId, 2) == -1 ) - { + if (file.write((const char *)tmpId, 2) == -1) { return false; } } // Write the data pack(buffer); - if ( file.write((const char*)buffer, numBytes) == -1 ) - { + if (file.write((const char *)buffer, numBytes) == -1) { return false; } @@ -374,14 +371,13 @@ bool UAVObject::load() // Open file QFile file(name + ".uavobj"); - if (!file.open(QFile::ReadOnly)) - { + + if (!file.open(QFile::ReadOnly)) { return false; } // Load object - if ( !load(file) ) - { + if (!load(file)) { return false; } @@ -396,39 +392,34 @@ bool UAVObject::load() * The data will be read and the file will not be closed. * @returns True on success, false on failure */ -bool UAVObject::load(QFile& file) +bool UAVObject::load(QFile & file) { QMutexLocker locker(mutex); quint8 buffer[numBytes]; quint8 tmpId[4]; // Read the object ID - if ( file.read((char*)tmpId, 4) != 4 ) - { + if (file.read((char *)tmpId, 4) != 4) { return false; } // Check that the IDs match - if (qFromLittleEndian(tmpId) != objID) - { + if (qFromLittleEndian(tmpId) != objID) { return false; } // Read the instance ID - if ( file.read((char*)tmpId, 2) != 2 ) - { + if (file.read((char *)tmpId, 2) != 2) { return false; } // Check that the IDs match - if (qFromLittleEndian(tmpId) != instID) - { + if (qFromLittleEndian(tmpId) != instID) { return false; } // Read and unpack the data - if ( file.read((char*)buffer, numBytes) != numBytes ) - { + if (file.read((char *)buffer, numBytes) != numBytes) { return false; } unpack(buffer); @@ -443,8 +434,9 @@ bool UAVObject::load(QFile& file) QString UAVObject::toString() { QString sout; - sout.append( toStringBrief() ); - sout.append( toStringData() ); + + sout.append(toStringBrief()); + sout.append(toStringData()); return sout; } @@ -454,12 +446,13 @@ QString UAVObject::toString() QString UAVObject::toStringBrief() { QString sout; - sout.append( QString("%1 (ID: %2, InstID: %3, NumBytes: %4, SInst: %5)\n") - .arg(getName()) - .arg(getObjID()) - .arg(getInstID()) - .arg(getNumBytes()) - .arg(isSingleInstance()) ); + + sout.append(QString("%1 (ID: %2, InstID: %3, NumBytes: %4, SInst: %5)\n") + .arg(getName()) + .arg(getObjID()) + .arg(getInstID()) + .arg(getNumBytes()) + .arg(isSingleInstance())); return sout; } @@ -469,10 +462,10 @@ QString UAVObject::toStringBrief() QString UAVObject::toStringData() { QString sout; + sout.append("Data:\n"); - for (int n = 0; n < fields.length(); ++n) - { - sout.append( QString("\t%1").arg(fields[n]->toString()) ); + for (int n = 0; n < fields.length(); ++n) { + sout.append(QString("\t%1").arg(fields[n]->toString())); } return sout; } @@ -488,7 +481,7 @@ void UAVObject::emitTransactionCompleted(bool success) /** * Emit the newInstance event */ -void UAVObject::emitNewInstance(UAVObject * obj) +void UAVObject::emitNewInstance(UAVObject *obj) { emit newInstance(obj); } @@ -497,18 +490,18 @@ void UAVObject::emitNewInstance(UAVObject * obj) * Initialize a UAVObjMetadata object. * \param[in] metadata The metadata object */ -void UAVObject::MetadataInitialize(UAVObject::Metadata& metadata) +void UAVObject::MetadataInitialize(UAVObject::Metadata & metadata) { - metadata.flags = - ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT | - ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT | - 1 << UAVOBJ_TELEMETRY_ACKED_SHIFT | - 1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT | - UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT | - UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT; - metadata.flightTelemetryUpdatePeriod = 0; - metadata.gcsTelemetryUpdatePeriod = 0; - metadata.loggingUpdatePeriod = 0; + metadata.flags = + ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT | + ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT | + 1 << UAVOBJ_TELEMETRY_ACKED_SHIFT | + 1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT | + UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT | + UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT; + metadata.flightTelemetryUpdatePeriod = 0; + metadata.gcsTelemetryUpdatePeriod = 0; + metadata.loggingUpdatePeriod = 0; } /** @@ -516,9 +509,9 @@ void UAVObject::MetadataInitialize(UAVObject::Metadata& metadata) * \param[in] metadata The metadata object * \return the access type */ -UAVObject::AccessMode UAVObject::GetFlightAccess(const UAVObject::Metadata& metadata) +UAVObject::AccessMode UAVObject::GetFlightAccess(const UAVObject::Metadata & metadata) { - return UAVObject::AccessMode((metadata.flags >> UAVOBJ_ACCESS_SHIFT) & 1); + return UAVObject::AccessMode((metadata.flags >> UAVOBJ_ACCESS_SHIFT) & 1); } /** @@ -526,9 +519,9 @@ UAVObject::AccessMode UAVObject::GetFlightAccess(const UAVObject::Metadata& meta * \param[in] metadata The metadata object * \param[in] mode The access mode */ -void UAVObject::SetFlightAccess(UAVObject::Metadata& metadata, UAVObject::AccessMode mode) +void UAVObject::SetFlightAccess(UAVObject::Metadata & metadata, UAVObject::AccessMode mode) { - SET_BITS(metadata.flags, UAVOBJ_ACCESS_SHIFT, mode, 1); + SET_BITS(metadata.flags, UAVOBJ_ACCESS_SHIFT, mode, 1); } /** @@ -536,9 +529,9 @@ void UAVObject::SetFlightAccess(UAVObject::Metadata& metadata, UAVObject::Access * \param[in] metadata The metadata object * \return the GCS access type */ -UAVObject::AccessMode UAVObject::GetGcsAccess(const UAVObject::Metadata& metadata) +UAVObject::AccessMode UAVObject::GetGcsAccess(const UAVObject::Metadata & metadata) { - return UAVObject::AccessMode((metadata.flags >> UAVOBJ_GCS_ACCESS_SHIFT) & 1); + return UAVObject::AccessMode((metadata.flags >> UAVOBJ_GCS_ACCESS_SHIFT) & 1); } /** @@ -546,8 +539,9 @@ UAVObject::AccessMode UAVObject::GetGcsAccess(const UAVObject::Metadata& metadat * \param[in] metadata The metadata object * \param[in] mode The access mode */ -void UAVObject::SetGcsAccess(UAVObject::Metadata& metadata, UAVObject::AccessMode mode) { - SET_BITS(metadata.flags, UAVOBJ_GCS_ACCESS_SHIFT, mode, 1); +void UAVObject::SetGcsAccess(UAVObject::Metadata & metadata, UAVObject::AccessMode mode) +{ + SET_BITS(metadata.flags, UAVOBJ_GCS_ACCESS_SHIFT, mode, 1); } /** @@ -555,8 +549,9 @@ void UAVObject::SetGcsAccess(UAVObject::Metadata& metadata, UAVObject::AccessMod * \param[in] metadata The metadata object * \return the telemetry acked boolean */ -quint8 UAVObject::GetFlightTelemetryAcked(const UAVObject::Metadata& metadata) { - return (metadata.flags >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 1; +quint8 UAVObject::GetFlightTelemetryAcked(const UAVObject::Metadata & metadata) +{ + return (metadata.flags >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 1; } /** @@ -564,8 +559,9 @@ quint8 UAVObject::GetFlightTelemetryAcked(const UAVObject::Metadata& metadata) { * \param[in] metadata The metadata object * \param[in] val The telemetry acked boolean */ -void UAVObject::SetFlightTelemetryAcked(UAVObject::Metadata& metadata, quint8 val) { - SET_BITS(metadata.flags, UAVOBJ_TELEMETRY_ACKED_SHIFT, val, 1); +void UAVObject::SetFlightTelemetryAcked(UAVObject::Metadata & metadata, quint8 val) +{ + SET_BITS(metadata.flags, UAVOBJ_TELEMETRY_ACKED_SHIFT, val, 1); } /** @@ -573,8 +569,9 @@ void UAVObject::SetFlightTelemetryAcked(UAVObject::Metadata& metadata, quint8 va * \param[in] metadata The metadata object * \return the telemetry acked boolean */ -quint8 UAVObject::GetGcsTelemetryAcked(const UAVObject::Metadata& metadata) { - return (metadata.flags >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 1; +quint8 UAVObject::GetGcsTelemetryAcked(const UAVObject::Metadata & metadata) +{ + return (metadata.flags >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 1; } /** @@ -582,8 +579,9 @@ quint8 UAVObject::GetGcsTelemetryAcked(const UAVObject::Metadata& metadata) { * \param[in] metadata The metadata object * \param[in] val The GCS telemetry acked boolean */ -void UAVObject::SetGcsTelemetryAcked(UAVObject::Metadata& metadata, quint8 val) { - SET_BITS(metadata.flags, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, val, 1); +void UAVObject::SetGcsTelemetryAcked(UAVObject::Metadata & metadata, quint8 val) +{ + SET_BITS(metadata.flags, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, val, 1); } /** @@ -591,8 +589,9 @@ void UAVObject::SetGcsTelemetryAcked(UAVObject::Metadata& metadata, quint8 val) * \param[in] metadata The metadata object * \return the telemetry update mode */ -UAVObject::UpdateMode UAVObject::GetFlightTelemetryUpdateMode(const UAVObject::Metadata& metadata) { - return UAVObject::UpdateMode((metadata.flags >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK); +UAVObject::UpdateMode UAVObject::GetFlightTelemetryUpdateMode(const UAVObject::Metadata & metadata) +{ + return UAVObject::UpdateMode((metadata.flags >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK); } /** @@ -600,8 +599,9 @@ UAVObject::UpdateMode UAVObject::GetFlightTelemetryUpdateMode(const UAVObject::M * \param[in] metadata The metadata object * \param[in] val The telemetry update mode */ -void UAVObject::SetFlightTelemetryUpdateMode(UAVObject::Metadata& metadata, UAVObject::UpdateMode val) { - SET_BITS(metadata.flags, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); +void UAVObject::SetFlightTelemetryUpdateMode(UAVObject::Metadata & metadata, UAVObject::UpdateMode val) +{ + SET_BITS(metadata.flags, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); } /** @@ -609,8 +609,9 @@ void UAVObject::SetFlightTelemetryUpdateMode(UAVObject::Metadata& metadata, UAVO * \param[in] metadata The metadata object * \return the GCS telemetry update mode */ -UAVObject::UpdateMode UAVObject::GetGcsTelemetryUpdateMode(const UAVObject::Metadata& metadata) { - return UAVObject::UpdateMode((metadata.flags >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK); +UAVObject::UpdateMode UAVObject::GetGcsTelemetryUpdateMode(const UAVObject::Metadata & metadata) +{ + return UAVObject::UpdateMode((metadata.flags >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK); } /** @@ -618,6 +619,7 @@ UAVObject::UpdateMode UAVObject::GetGcsTelemetryUpdateMode(const UAVObject::Meta * \param[in] metadata The metadata object * \param[in] val The GCS telemetry update mode */ -void UAVObject::SetGcsTelemetryUpdateMode(UAVObject::Metadata& metadata, UAVObject::UpdateMode val) { - SET_BITS(metadata.flags, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); +void UAVObject::SetGcsTelemetryUpdateMode(UAVObject::Metadata & metadata, UAVObject::UpdateMode val) +{ + SET_BITS(metadata.flags, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); } diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h index d29151579..882378e7a 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVOBJECT_H @@ -39,18 +39,17 @@ #include #include "uavobjectfield.h" -#define UAVOBJ_ACCESS_SHIFT 0 -#define UAVOBJ_GCS_ACCESS_SHIFT 1 -#define UAVOBJ_TELEMETRY_ACKED_SHIFT 2 -#define UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT 3 -#define UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT 4 +#define UAVOBJ_ACCESS_SHIFT 0 +#define UAVOBJ_GCS_ACCESS_SHIFT 1 +#define UAVOBJ_TELEMETRY_ACKED_SHIFT 2 +#define UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT 3 +#define UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT 4 #define UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT 6 -#define UAVOBJ_UPDATE_MODE_MASK 0x3 +#define UAVOBJ_UPDATE_MODE_MASK 0x3 class UAVObjectField; -class UAVOBJECTS_EXPORT UAVObject: public QObject -{ +class UAVOBJECTS_EXPORT UAVObject : public QObject { Q_OBJECT public: @@ -59,18 +58,18 @@ public: * Object update mode */ typedef enum { - UPDATEMODE_MANUAL = 0, /** Manually update object, by calling the updated() function */ - UPDATEMODE_PERIODIC = 1, /** Automatically update object at periodic intervals */ - UPDATEMODE_ONCHANGE = 2, /** Only update object when its data changes */ - UPDATEMODE_THROTTLED = 3 /** Object is updated on change, but not more often than the interval time */ + UPDATEMODE_MANUAL = 0, /** Manually update object, by calling the updated() function */ + UPDATEMODE_PERIODIC = 1, /** Automatically update object at periodic intervals */ + UPDATEMODE_ONCHANGE = 2, /** Only update object when its data changes */ + UPDATEMODE_THROTTLED = 3 /** Object is updated on change, but not more often than the interval time */ } UpdateMode; /** * Access mode */ typedef enum { - ACCESS_READWRITE = 0, - ACCESS_READONLY = 1 + ACCESS_READWRITE = 0, + ACCESS_READONLY = 1 } AccessMode; /** @@ -89,15 +88,15 @@ public: * 4-5 telemetryUpdateMode Update mode used by the telemetry module (UAVObjUpdateMode) * 6-7 gcsTelemetryUpdateMode Update mode used by the GCS (UAVObjUpdateMode) */ - typedef struct { - quint8 flags; /** Defines flags for update and logging modes and whether an update should be ACK'd (bits defined above) */ + typedef struct { + quint8 flags; /** Defines flags for update and logging modes and whether an update should be ACK'd (bits defined above) */ quint16 flightTelemetryUpdatePeriod; /** Update period used by the telemetry module (only if telemetry mode is PERIODIC) */ quint16 gcsTelemetryUpdatePeriod; /** Update period used by the GCS (only if telemetry mode is PERIODIC) */ quint16 loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */ - } __attribute__((packed)) Metadata; + } __attribute__((packed)) Metadata; - UAVObject(quint32 objID, bool isSingleInst, const QString& name); + UAVObject(quint32 objID, bool isSingleInst, const QString & name); void initialize(quint32 instID); quint32 getObjID(); quint32 getInstID(); @@ -105,23 +104,23 @@ public: QString getName(); QString getCategory(); QString getDescription(); - quint32 getNumBytes(); - qint32 pack(quint8* dataOut); - qint32 unpack(const quint8* dataIn); + quint32 getNumBytes(); + qint32 pack(quint8 *dataOut); + qint32 unpack(const quint8 *dataIn); bool save(); - bool save(QFile& file); + bool save(QFile & file); bool load(); - bool load(QFile& file); - virtual void setMetadata(const Metadata& mdata) = 0; + bool load(QFile & file); + virtual void setMetadata(const Metadata & mdata) = 0; virtual Metadata getMetadata() = 0; virtual Metadata getDefaultMetadata() = 0; void lock(); void lock(int timeoutMs); void unlock(); - QMutex* getMutex(); + QMutex *getMutex(); qint32 getNumFields(); - QList getFields(); - UAVObjectField* getField(const QString& name); + QList getFields(); + UAVObjectField *getField(const QString & name); QString toString(); QString toStringBrief(); QString toStringData(); @@ -129,36 +128,36 @@ public: void emitNewInstance(UAVObject *); // Metadata accessors - static void MetadataInitialize(Metadata& meta); - static AccessMode GetFlightAccess(const Metadata& meta); - static void SetFlightAccess(Metadata& meta, AccessMode mode); - static AccessMode GetGcsAccess(const Metadata& meta); - static void SetGcsAccess(Metadata& meta, AccessMode mode); - static quint8 GetFlightTelemetryAcked(const Metadata& meta); - static void SetFlightTelemetryAcked(Metadata& meta, quint8 val); - static quint8 GetGcsTelemetryAcked(const Metadata& meta); - static void SetGcsTelemetryAcked(Metadata& meta, quint8 val); - static UpdateMode GetFlightTelemetryUpdateMode(const Metadata& meta); - static void SetFlightTelemetryUpdateMode(Metadata& meta, UpdateMode val); - static UpdateMode GetGcsTelemetryUpdateMode(const Metadata& meta); - static void SetGcsTelemetryUpdateMode(Metadata& meta, UpdateMode val); - + static void MetadataInitialize(Metadata & meta); + static AccessMode GetFlightAccess(const Metadata & meta); + static void SetFlightAccess(Metadata & meta, AccessMode mode); + static AccessMode GetGcsAccess(const Metadata & meta); + static void SetGcsAccess(Metadata & meta, AccessMode mode); + static quint8 GetFlightTelemetryAcked(const Metadata & meta); + static void SetFlightTelemetryAcked(Metadata & meta, quint8 val); + static quint8 GetGcsTelemetryAcked(const Metadata & meta); + static void SetGcsTelemetryAcked(Metadata & meta, quint8 val); + static UpdateMode GetFlightTelemetryUpdateMode(const Metadata & meta); + static void SetFlightTelemetryUpdateMode(Metadata & meta, UpdateMode val); + static UpdateMode GetGcsTelemetryUpdateMode(const Metadata & meta); + static void SetGcsTelemetryUpdateMode(Metadata & meta, UpdateMode val); + public slots: void requestUpdate(); void updated(); signals: - void objectUpdated(UAVObject* obj); - void objectUpdatedAuto(UAVObject* obj); - void objectUpdatedManual(UAVObject* obj); - void objectUpdatedPeriodic(UAVObject* obj); - void objectUnpacked(UAVObject* obj); - void updateRequested(UAVObject* obj); - void transactionCompleted(UAVObject* obj, bool success); - void newInstance(UAVObject* obj); + void objectUpdated(UAVObject *obj); + void objectUpdatedAuto(UAVObject *obj); + void objectUpdatedManual(UAVObject *obj); + void objectUpdatedPeriodic(UAVObject *obj); + void objectUnpacked(UAVObject *obj); + void updateRequested(UAVObject *obj); + void transactionCompleted(UAVObject *obj, bool success); + void newInstance(UAVObject *obj); private slots: - void fieldUpdated(UAVObjectField* field); + void fieldUpdated(UAVObjectField *field); protected: quint32 objID; @@ -168,13 +167,13 @@ protected: QString description; QString category; quint32 numBytes; - QMutex* mutex; - quint8* data; - QList fields; + QMutex *mutex; + quint8 *data; + QList fields; - void initializeFields(QList& fields, quint8* data, quint32 numBytes); - void setDescription(const QString& description); - void setCategory(const QString& category); + void initializeFields(QList & fields, quint8 *data, quint32 numBytes); + void setDescription(const QString & description); + void setCategory(const QString & category); }; #endif // UAVOBJECT_H diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp index b3af75613..4e0c5c613 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp @@ -29,39 +29,37 @@ #include #include -UAVObjectField::UAVObjectField(const QString& name, const QString& units, FieldType type, quint32 numElements, const QStringList& options, const QString &limits) +UAVObjectField::UAVObjectField(const QString & name, const QString & units, FieldType type, quint32 numElements, const QStringList & options, const QString &limits) { QStringList elementNames; + // Set element names - for (quint32 n = 0; n < numElements; ++n) - { + for (quint32 n = 0; n < numElements; ++n) { elementNames.append(QString("%1").arg(n)); } // Initialize - constructorInitialize(name, units, type, elementNames, options,limits); - + constructorInitialize(name, units, type, elementNames, options, limits); } -UAVObjectField::UAVObjectField(const QString& name, const QString& units, FieldType type, const QStringList& elementNames, const QStringList& options, const QString &limits) +UAVObjectField::UAVObjectField(const QString & name, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString &limits) { - constructorInitialize(name, units, type, elementNames, options,limits); + constructorInitialize(name, units, type, elementNames, options, limits); } -void UAVObjectField::constructorInitialize(const QString& name, const QString& units, FieldType type, const QStringList& elementNames, const QStringList& options,const QString &limits) +void UAVObjectField::constructorInitialize(const QString & name, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString &limits) { // Copy params - this->name = name; - this->units = units; - this->type = type; - this->options = options; - this->numElements = elementNames.length(); - this->offset = 0; - this->data = NULL; + this->name = name; + this->units = units; + this->type = type; + this->options = options; + this->numElements = elementNames.length(); + this->offset = 0; + this->data = NULL; this->obj = NULL; this->elementNames = elementNames; // Set field size - switch (type) - { + switch (type) { case INT8: numBytesPerElement = sizeof(qint8); break; @@ -88,7 +86,7 @@ void UAVObjectField::constructorInitialize(const QString& name, const QString& u break; case BITFIELD: numBytesPerElement = sizeof(quint8); - this->options = QStringList()<options = QStringList() << tr("0") << tr("1"); break; case STRING: numBytesPerElement = sizeof(quint8); @@ -102,64 +100,67 @@ void UAVObjectField::constructorInitialize(const QString& name, const QString& u void UAVObjectField::limitsInitialize(const QString &limits) { // Limit string format: - // % - start char - // XXXX - optional BOARD_TYPE and BOARD_REVISION (4 hex digits) - // TY - rule type (EQ-equal, NE-not equal, BE-between, BI-bigger, SM-smaller) - // VAL - values for TY separated by colon - // , - rule separator (may have leading or trailing spaces) - // ; - element separator (may have leading or trailing spaces) + // % - start char + // XXXX - optional BOARD_TYPE and BOARD_REVISION (4 hex digits) + // TY - rule type (EQ-equal, NE-not equal, BE-between, BI-bigger, SM-smaller) + // VAL - values for TY separated by colon + // , - rule separator (may have leading or trailing spaces) + // ; - element separator (may have leading or trailing spaces) // // Examples: - // Disable few flight modes for Revo (00903): - // "%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner" - // Original CC board (rev 1), first element bigger than 3 and second element inside [2.3-5.0]: - // "%0401BI:3; %BE:2.3:5" - // Set applicable range [0-500] for 3 elements of array for all boards: - // "%BE:0:500; %BE:0:500; %BE:0:500" - if(limits.isEmpty()) + // Disable few flight modes for Revo (00903): + // "%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner" + // Original CC board (rev 1), first element bigger than 3 and second element inside [2.3-5.0]: + // "%0401BI:3; %BE:2.3:5" + // Set applicable range [0-500] for 3 elements of array for all boards: + // "%BE:0:500; %BE:0:500; %BE:0:500" + if (limits.isEmpty()) { return; + } QStringList stringPerElement = limits.split(";"); - quint32 index=0; - foreach (QString str, stringPerElement) { + quint32 index = 0; + foreach(QString str, stringPerElement) { QStringList ruleList = str.split(","); + QList limitList; - foreach(QString rule,ruleList) - { - QString _str=rule.trimmed(); - if(_str.isEmpty()) + foreach(QString rule, ruleList) { + QString _str = rule.trimmed(); + + if (_str.isEmpty()) { continue; - QStringList valuesPerElement=_str.split(":"); + } + QStringList valuesPerElement = _str.split(":"); LimitStruct lstruc; - bool startFlag=valuesPerElement.at(0).startsWith("%"); - bool maxIndexFlag=(int)(index)<(int)numElements; - bool elemNumberSizeFlag=valuesPerElement.at(0).size()==3; + bool startFlag = valuesPerElement.at(0).startsWith("%"); + bool maxIndexFlag = (int)(index) < (int)numElements; + bool elemNumberSizeFlag = valuesPerElement.at(0).size() == 3; bool aux; - valuesPerElement.at(0).mid(1,4).toInt(&aux,16); - bool b4=((valuesPerElement.at(0).size())==7 && aux); - if(startFlag && maxIndexFlag && (elemNumberSizeFlag || b4)) - { - if(b4) - lstruc.board=valuesPerElement.at(0).mid(1,4).toInt(&aux,16); - else - lstruc.board=0; - if(valuesPerElement.at(0).right(2)=="EQ") - lstruc.type=EQUAL; - else if(valuesPerElement.at(0).right(2)=="NE") - lstruc.type=NOT_EQUAL; - else if(valuesPerElement.at(0).right(2)=="BE") - lstruc.type=BETWEEN; - else if(valuesPerElement.at(0).right(2)=="BI") - lstruc.type=BIGGER; - else if(valuesPerElement.at(0).right(2)=="SM") - lstruc.type=SMALLER; - else - qDebug()<<"limits parsing failed (invalid property) on UAVObjectField"<numelements) on UAVObjectField"<numelements) on UAVObjectField" << name << "index" << index << "numElements" << numElements; + } else if (!elemNumberSizeFlag || !b4) { + qDebug() << "limits parsing failed limit not starting with %XX or %YYYYXX where XX is the limit type and YYYY is the board type on UAVObjectField" << name; + } } } - elementLimits.insert(index,limitList); + elementLimits.insert(index, limitList); ++index; - } - foreach(QList limitList,elementLimits) - { - foreach(LimitStruct limit,limitList) - { - qDebug()<<"Limit type"< limitList, elementLimits) { + foreach(LimitStruct limit, limitList) { + qDebug() << "Limit type" << limit.type << "for board" << limit.board << "for field" << getName(); + foreach(QVariant var, limit.values) { + qDebug() << "value" << var; } } } } -bool UAVObjectField::isWithinLimits(QVariant var,quint32 index, int board) +bool UAVObjectField::isWithinLimits(QVariant var, quint32 index, int board) { - if(!elementLimits.keys().contains(index)) + if (!elementLimits.keys().contains(index)) { return true; + } - foreach(LimitStruct struc,elementLimits.value(index)) - { - if((struc.board!=board) && board!=0 && struc.board!=0) + foreach(LimitStruct struc, elementLimits.value(index)) { + if ((struc.board != board) && board != 0 && struc.board != 0) { continue; - switch(struc.type) - { + } + switch (struc.type) { case EQUAL: - switch (type) - { + switch (type) { case INT8: case INT16: case INT32: - foreach (QVariant vars, struc.values) { - if(var.toInt()==vars.toInt()) + foreach(QVariant vars, struc.values) { + if (var.toInt() == vars.toInt()) { return true; + } } return false; + break; case UINT8: case UINT16: case UINT32: case BITFIELD: - foreach (QVariant vars, struc.values) { - if(var.toUInt()==vars.toUInt()) + foreach(QVariant vars, struc.values) { + if (var.toUInt() == vars.toUInt()) { return true; + } } return false; + break; case ENUM: case STRING: - foreach (QVariant vars, struc.values) { - if(var.toString()==vars.toString()) + foreach(QVariant vars, struc.values) { + if (var.toString() == vars.toString()) { return true; + } } return false; + break; case FLOAT32: - foreach (QVariant vars, struc.values) { - if(var.toFloat()==vars.toFloat()) + foreach(QVariant vars, struc.values) { + if (var.toFloat() == vars.toFloat()) { return true; + } } return false; + break; default: return true; } break; case NOT_EQUAL: - switch (type) - { + switch (type) { case INT8: case INT16: case INT32: - foreach (QVariant vars, struc.values) { - if(var.toInt()==vars.toInt()) + foreach(QVariant vars, struc.values) { + if (var.toInt() == vars.toInt()) { return false; + } } return true; + break; case UINT8: case UINT16: case UINT32: case BITFIELD: - foreach (QVariant vars, struc.values) { - if(var.toUInt()==vars.toUInt()) + foreach(QVariant vars, struc.values) { + if (var.toUInt() == vars.toUInt()) { return false; + } } return true; + break; case ENUM: case STRING: - foreach (QVariant vars, struc.values) { - if(var.toString()==vars.toString()) + foreach(QVariant vars, struc.values) { + if (var.toString() == vars.toString()) { return false; + } } return true; + break; case FLOAT32: - foreach (QVariant vars, struc.values) { - if(var.toFloat()==vars.toFloat()) + foreach(QVariant vars, struc.values) { + if (var.toFloat() == vars.toFloat()) { return false; + } } return true; + break; default: return true; } break; case BETWEEN: - if(struc.values.length()<2) - { - qDebug()<<__FUNCTION__<<"between limit with less than 1 pair, aborting; field:"<2) - qDebug()<<__FUNCTION__<<"between limit with more than 1 pair, using first; field"< 2) { + qDebug() << __FUNCTION__ << "between limit with more than 1 pair, using first; field" << name; + } + switch (type) { case INT8: case INT16: case INT32: - if(!(var.toInt()>=struc.values.at(0).toInt() && var.toInt()<=struc.values.at(1).toInt())) - return false; + if (!(var.toInt() >= struc.values.at(0).toInt() && var.toInt() <= struc.values.at(1).toInt())) { + return false; + } return true; + break; case UINT8: case UINT16: case UINT32: case BITFIELD: - if(!(var.toUInt()>=struc.values.at(0).toUInt() && var.toUInt()<=struc.values.at(1).toUInt())) - return false; + if (!(var.toUInt() >= struc.values.at(0).toUInt() && var.toUInt() <= struc.values.at(1).toUInt())) { + return false; + } return true; + break; case ENUM: - if(!(options.indexOf(var.toString())>=options.indexOf(struc.values.at(0).toString()) && options.indexOf(var.toString())<=options.indexOf(struc.values.at(1).toString()))) - return false; + if (!(options.indexOf(var.toString()) >= options.indexOf(struc.values.at(0).toString()) && options.indexOf(var.toString()) <= options.indexOf(struc.values.at(1).toString()))) { + return false; + } return true; + break; case STRING: return true; + break; case FLOAT32: - if(!(var.toFloat()>=struc.values.at(0).toFloat() && var.toFloat()<=struc.values.at(1).toFloat())) - return false; + if (!(var.toFloat() >= struc.values.at(0).toFloat() && var.toFloat() <= struc.values.at(1).toFloat())) { + return false; + } return true; + break; default: return true; } break; case BIGGER: - if(struc.values.length()<1) - { - qDebug()<<__FUNCTION__<<"BIGGER limit with less than 1 value, aborting; field:"<1) - qDebug()<<__FUNCTION__<<"BIGGER limit with more than 1 value, using first; field"< 1) { + qDebug() << __FUNCTION__ << "BIGGER limit with more than 1 value, using first; field" << name; + } + switch (type) { case INT8: case INT16: case INT32: - if(!(var.toInt()>=struc.values.at(0).toInt())) - return false; + if (!(var.toInt() >= struc.values.at(0).toInt())) { + return false; + } return true; + break; case UINT8: case UINT16: case UINT32: case BITFIELD: - if(!(var.toUInt()>=struc.values.at(0).toUInt())) - return false; + if (!(var.toUInt() >= struc.values.at(0).toUInt())) { + return false; + } return true; + break; case ENUM: - if(!(options.indexOf(var.toString())>=options.indexOf(struc.values.at(0).toString()))) - return false; + if (!(options.indexOf(var.toString()) >= options.indexOf(struc.values.at(0).toString()))) { + return false; + } return true; + break; case STRING: return true; + break; case FLOAT32: - if(!(var.toFloat()>=struc.values.at(0).toFloat())) - return false; + if (!(var.toFloat() >= struc.values.at(0).toFloat())) { + return false; + } return true; + break; default: return true; } break; case SMALLER: - switch (type) - { + switch (type) { case INT8: case INT16: case INT32: - if(!(var.toInt()<=struc.values.at(0).toInt())) - return false; + if (!(var.toInt() <= struc.values.at(0).toInt())) { + return false; + } return true; + break; case UINT8: case UINT16: case UINT32: case BITFIELD: - if(!(var.toUInt()<=struc.values.at(0).toUInt())) - return false; + if (!(var.toUInt() <= struc.values.at(0).toUInt())) { + return false; + } return true; + break; case ENUM: - if(!(options.indexOf(var.toString())<=options.indexOf(struc.values.at(0).toString()))) - return false; + if (!(options.indexOf(var.toString()) <= options.indexOf(struc.values.at(0).toString()))) { + return false; + } return true; + break; case STRING: return true; + break; case FLOAT32: - if(!(var.toFloat()<=struc.values.at(0).toFloat())) - return false; + if (!(var.toFloat() <= struc.values.at(0).toFloat())) { + return false; + } return true; + break; default: return true; @@ -428,30 +462,34 @@ bool UAVObjectField::isWithinLimits(QVariant var,quint32 index, int board) return true; } -QVariant UAVObjectField::getMaxLimit(quint32 index,int board) +QVariant UAVObjectField::getMaxLimit(quint32 index, int board) { - if(!elementLimits.keys().contains(index)) + if (!elementLimits.keys().contains(index)) { return QVariant(); - foreach(LimitStruct struc,elementLimits.value(index)) - { - if((struc.board!=board) && board!=0 && struc.board!=0) + } + foreach(LimitStruct struc, elementLimits.value(index)) { + if ((struc.board != board) && board != 0 && struc.board != 0) { continue; - switch(struc.type) - { + } + switch (struc.type) { case EQUAL: case NOT_EQUAL: case BIGGER: return QVariant(); + break; break; case BETWEEN: return struc.values.at(1); + break; case SMALLER: return struc.values.at(0); + break; default: return QVariant(); + break; } } @@ -459,38 +497,42 @@ QVariant UAVObjectField::getMaxLimit(quint32 index,int board) } QVariant UAVObjectField::getMinLimit(quint32 index, int board) { - if(!elementLimits.keys().contains(index)) + if (!elementLimits.keys().contains(index)) { return QVariant(); - foreach(LimitStruct struc,elementLimits.value(index)) - { - if((struc.board!=board) && board!=0 && struc.board!=0) + } + foreach(LimitStruct struc, elementLimits.value(index)) { + if ((struc.board != board) && board != 0 && struc.board != 0) { return QVariant(); - switch(struc.type) - { + } + switch (struc.type) { case EQUAL: case NOT_EQUAL: case SMALLER: return QVariant(); + break; break; case BETWEEN: return struc.values.at(0); + break; case BIGGER: return struc.values.at(0); + break; default: return QVariant(); + break; } } return QVariant(); } -void UAVObjectField::initialize(quint8* data, quint32 dataOffset, UAVObject* obj) +void UAVObjectField::initialize(quint8 *data, quint32 dataOffset, UAVObject *obj) { - this->data = data; + this->data = data; this->offset = dataOffset; - this->obj = obj; + this->obj = obj; clear(); } @@ -501,28 +543,37 @@ UAVObjectField::FieldType UAVObjectField::getType() QString UAVObjectField::getTypeAsString() { - switch (type) - { + switch (type) { case UAVObjectField::INT8: return "int8"; + case UAVObjectField::INT16: return "int16"; + case UAVObjectField::INT32: return "int32"; + case UAVObjectField::UINT8: return "uint8"; + case UAVObjectField::UINT16: return "uint16"; + case UAVObjectField::UINT32: return "uint32"; + case UAVObjectField::FLOAT32: return "float32"; + case UAVObjectField::ENUM: return "enum"; + case UAVObjectField::BITFIELD: return "bitfield"; + case UAVObjectField::STRING: return "string"; + default: return ""; } @@ -533,7 +584,7 @@ QStringList UAVObjectField::getElementNames() return elementNames; } -UAVObject* UAVObjectField::getObject() +UAVObject *UAVObjectField::getObject() { return obj; } @@ -541,13 +592,13 @@ UAVObject* UAVObjectField::getObject() void UAVObjectField::clear() { QMutexLocker locker(obj->getMutex()); - switch (type) - { + + switch (type) { case BITFIELD: - memset(&data[offset], 0, numBytesPerElement*((quint32)(1+(numElements-1)/8))); + memset(&data[offset], 0, numBytesPerElement * ((quint32)(1 + (numElements - 1) / 8))); break; default: - memset(&data[offset], 0, numBytesPerElement*numElements); + memset(&data[offset], 0, numBytesPerElement * numElements); break; } } @@ -579,13 +630,14 @@ quint32 UAVObjectField::getDataOffset() quint32 UAVObjectField::getNumBytes() { - switch (type) - { + switch (type) { case BITFIELD: - return numBytesPerElement * ((quint32) (1+(numElements-1)/8)); + return numBytesPerElement * ((quint32)(1 + (numElements - 1) / 8)); + break; default: return numBytesPerElement * numElements; + break; } } @@ -593,81 +645,73 @@ quint32 UAVObjectField::getNumBytes() QString UAVObjectField::toString() { QString sout; - sout.append ( QString("%1: [ ").arg(name) ); - for (unsigned int n = 0; n < numElements; ++n) - { - sout.append( QString("%1 ").arg(getDouble(n)) ); + + sout.append(QString("%1: [ ").arg(name)); + for (unsigned int n = 0; n < numElements; ++n) { + sout.append(QString("%1 ").arg(getDouble(n))); } - sout.append( QString("] %1\n").arg(units) ); + sout.append(QString("] %1\n").arg(units)); return sout; } -qint32 UAVObjectField::pack(quint8* dataOut) +qint32 UAVObjectField::pack(quint8 *dataOut) { QMutexLocker locker(obj->getMutex()); + // Pack each element in output buffer - switch (type) - { + switch (type) { case INT8: memcpy(dataOut, &data[offset], numElements); break; case INT16: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { qint16 value; - memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement); - qToLittleEndian(value, &dataOut[numBytesPerElement*index]); + memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement); + qToLittleEndian(value, &dataOut[numBytesPerElement * index]); } break; case INT32: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { qint32 value; - memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement); - qToLittleEndian(value, &dataOut[numBytesPerElement*index]); + memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement); + qToLittleEndian(value, &dataOut[numBytesPerElement * index]); } break; case UINT8: - for (quint32 index = 0; index < numElements; ++index) - { - dataOut[numBytesPerElement*index] = data[offset + numBytesPerElement*index]; + for (quint32 index = 0; index < numElements; ++index) { + dataOut[numBytesPerElement * index] = data[offset + numBytesPerElement * index]; } break; case UINT16: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { quint16 value; - memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement); - qToLittleEndian(value, &dataOut[numBytesPerElement*index]); + memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement); + qToLittleEndian(value, &dataOut[numBytesPerElement * index]); } break; case UINT32: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { quint32 value; - memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement); - qToLittleEndian(value, &dataOut[numBytesPerElement*index]); + memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement); + qToLittleEndian(value, &dataOut[numBytesPerElement * index]); } break; case FLOAT32: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { quint32 value; - memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement); - qToLittleEndian(value, &dataOut[numBytesPerElement*index]); + memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement); + qToLittleEndian(value, &dataOut[numBytesPerElement * index]); } break; case ENUM: - for (quint32 index = 0; index < numElements; ++index) - { - dataOut[numBytesPerElement*index] = data[offset + numBytesPerElement*index]; + for (quint32 index = 0; index < numElements; ++index) { + dataOut[numBytesPerElement * index] = data[offset + numBytesPerElement * index]; } break; case BITFIELD: - for (quint32 index = 0; index < (quint32)(1+(numElements-1)/8); ++index) - { - dataOut[numBytesPerElement*index] = data[offset + numBytesPerElement*index]; + for (quint32 index = 0; index < (quint32)(1 + (numElements - 1) / 8); ++index) { + dataOut[numBytesPerElement * index] = data[offset + numBytesPerElement * index]; } break; case STRING: @@ -678,71 +722,63 @@ qint32 UAVObjectField::pack(quint8* dataOut) return getNumBytes(); } -qint32 UAVObjectField::unpack(const quint8* dataIn) +qint32 UAVObjectField::unpack(const quint8 *dataIn) { QMutexLocker locker(obj->getMutex()); + // Unpack each element from input buffer - switch (type) - { + switch (type) { case INT8: memcpy(&data[offset], dataIn, numElements); break; case INT16: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { qint16 value; - value = qFromLittleEndian(&dataIn[numBytesPerElement*index]); - memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement); + value = qFromLittleEndian(&dataIn[numBytesPerElement * index]); + memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement); } break; case INT32: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { qint32 value; - value = qFromLittleEndian(&dataIn[numBytesPerElement*index]); - memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement); + value = qFromLittleEndian(&dataIn[numBytesPerElement * index]); + memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement); } break; case UINT8: - for (quint32 index = 0; index < numElements; ++index) - { - data[offset + numBytesPerElement*index] = dataIn[numBytesPerElement*index]; + for (quint32 index = 0; index < numElements; ++index) { + data[offset + numBytesPerElement * index] = dataIn[numBytesPerElement * index]; } break; case UINT16: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { quint16 value; - value = qFromLittleEndian(&dataIn[numBytesPerElement*index]); - memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement); + value = qFromLittleEndian(&dataIn[numBytesPerElement * index]); + memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement); } break; case UINT32: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { quint32 value; - value = qFromLittleEndian(&dataIn[numBytesPerElement*index]); - memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement); + value = qFromLittleEndian(&dataIn[numBytesPerElement * index]); + memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement); } break; case FLOAT32: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { quint32 value; - value = qFromLittleEndian(&dataIn[numBytesPerElement*index]); - memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement); + value = qFromLittleEndian(&dataIn[numBytesPerElement * index]); + memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement); } break; case ENUM: - for (quint32 index = 0; index < numElements; ++index) - { - data[offset + numBytesPerElement*index] = dataIn[numBytesPerElement*index]; + for (quint32 index = 0; index < numElements; ++index) { + data[offset + numBytesPerElement * index] = dataIn[numBytesPerElement * index]; } break; case BITFIELD: - for (quint32 index = 0; index < (quint32)(1+(numElements-1)/8); ++index) - { - data[offset + numBytesPerElement*index] = dataIn[numBytesPerElement*index]; + for (quint32 index = 0; index < (quint32)(1 + (numElements - 1) / 8); ++index) { + data[offset + numBytesPerElement * index] = dataIn[numBytesPerElement * index]; } break; case STRING: @@ -755,37 +791,46 @@ qint32 UAVObjectField::unpack(const quint8* dataIn) bool UAVObjectField::isNumeric() { - switch (type) - { + switch (type) { case INT8: return true; + break; case INT16: return true; + break; case INT32: return true; + break; case UINT8: return true; + break; case UINT16: return true; + break; case UINT32: return true; + break; case FLOAT32: return true; + break; case ENUM: return false; + break; case BITFIELD: return true; + break; case STRING: return false; + break; default: return false; @@ -794,37 +839,46 @@ bool UAVObjectField::isNumeric() bool UAVObjectField::isText() { - switch (type) - { + switch (type) { case INT8: return false; + break; case INT16: return false; + break; case INT32: return false; + break; case UINT8: return false; + break; case UINT16: return false; + break; case UINT32: return false; + break; case FLOAT32: return false; + break; case ENUM: return true; + break; case BITFIELD: return false; + break; case STRING: return true; + break; default: return false; @@ -834,87 +888,96 @@ bool UAVObjectField::isText() QVariant UAVObjectField::getValue(quint32 index) { QMutexLocker locker(obj->getMutex()); + // Check that index is not out of bounds - if ( index >= numElements ) - { + if (index >= numElements) { return QVariant(); } // Get value - switch (type) - { + switch (type) { case INT8: { qint8 tmpint8; - memcpy(&tmpint8, &data[offset + numBytesPerElement*index], numBytesPerElement); + memcpy(&tmpint8, &data[offset + numBytesPerElement * index], numBytesPerElement); return QVariant(tmpint8); + break; } case INT16: { qint16 tmpint16; - memcpy(&tmpint16, &data[offset + numBytesPerElement*index], numBytesPerElement); + memcpy(&tmpint16, &data[offset + numBytesPerElement * index], numBytesPerElement); return QVariant(tmpint16); + break; } case INT32: { qint32 tmpint32; - memcpy(&tmpint32, &data[offset + numBytesPerElement*index], numBytesPerElement); + memcpy(&tmpint32, &data[offset + numBytesPerElement * index], numBytesPerElement); return QVariant(tmpint32); + break; } case UINT8: { quint8 tmpuint8; - memcpy(&tmpuint8, &data[offset + numBytesPerElement*index], numBytesPerElement); + memcpy(&tmpuint8, &data[offset + numBytesPerElement * index], numBytesPerElement); return QVariant(tmpuint8); + break; } case UINT16: { quint16 tmpuint16; - memcpy(&tmpuint16, &data[offset + numBytesPerElement*index], numBytesPerElement); + memcpy(&tmpuint16, &data[offset + numBytesPerElement * index], numBytesPerElement); return QVariant(tmpuint16); + break; } case UINT32: { quint32 tmpuint32; - memcpy(&tmpuint32, &data[offset + numBytesPerElement*index], numBytesPerElement); + memcpy(&tmpuint32, &data[offset + numBytesPerElement * index], numBytesPerElement); return QVariant(tmpuint32); + break; } case FLOAT32: { float tmpfloat; - memcpy(&tmpfloat, &data[offset + numBytesPerElement*index], numBytesPerElement); + memcpy(&tmpfloat, &data[offset + numBytesPerElement * index], numBytesPerElement); return QVariant(tmpfloat); + break; } case ENUM: { quint8 tmpenum; - memcpy(&tmpenum, &data[offset + numBytesPerElement*index], numBytesPerElement); - if(tmpenum >= options.length()) { + memcpy(&tmpenum, &data[offset + numBytesPerElement * index], numBytesPerElement); + if (tmpenum >= options.length()) { qDebug() << "Invalid value for" << name; tmpenum = 0; } - return QVariant( options[tmpenum] ); + return QVariant(options[tmpenum]); + break; } case BITFIELD: { quint8 tmpbitfield; - memcpy(&tmpbitfield, &data[offset + numBytesPerElement*((quint32)(index/8))], numBytesPerElement); + memcpy(&tmpbitfield, &data[offset + numBytesPerElement * ((quint32)(index / 8))], numBytesPerElement); tmpbitfield = (tmpbitfield >> (index % 8)) & 1; - return QVariant( tmpbitfield ); + return QVariant(tmpbitfield); + break; } case STRING: { data[offset + numElements - 1] = '\0'; - QString str((char*)&data[offset]); - return QVariant( str ); + QString str((char *)&data[offset]); + return QVariant(str); + break; } } @@ -922,21 +985,19 @@ QVariant UAVObjectField::getValue(quint32 index) return QVariant(); } -bool UAVObjectField::checkValue(const QVariant& value, quint32 index) +bool UAVObjectField::checkValue(const QVariant & value, quint32 index) { QMutexLocker locker(obj->getMutex()); + // Check that index is not out of bounds - if ( index >= numElements ) - { + if (index >= numElements) { return false; } // Get metadata UAVObject::Metadata mdata = obj->getMetadata(); // Update value if the access mode permits - if ( UAVObject::GetFlightAccess(mdata) == UAVObject::ACCESS_READWRITE ) - { - switch (type) - { + if (UAVObject::GetFlightAccess(mdata) == UAVObject::ACCESS_READWRITE) { + switch (type) { case INT8: case INT16: case INT32: @@ -947,11 +1008,13 @@ bool UAVObjectField::checkValue(const QVariant& value, quint32 index) case STRING: case BITFIELD: return true; + break; case ENUM: { - qint8 tmpenum = options.indexOf( value.toString() ); - return ((tmpenum < 0) ? false : true); + qint8 tmpenum = options.indexOf(value.toString()); + return (tmpenum < 0) ? false : true; + break; } default: @@ -963,79 +1026,77 @@ bool UAVObjectField::checkValue(const QVariant& value, quint32 index) return true; } -void UAVObjectField::setValue(const QVariant& value, quint32 index) +void UAVObjectField::setValue(const QVariant & value, quint32 index) { QMutexLocker locker(obj->getMutex()); + // Check that index is not out of bounds - if ( index >= numElements ) - { + if (index >= numElements) { return; } // Get metadata UAVObject::Metadata mdata = obj->getMetadata(); // Update value if the access mode permits - if ( UAVObject::GetGcsAccess(mdata) == UAVObject::ACCESS_READWRITE ) - { - switch (type) - { + if (UAVObject::GetGcsAccess(mdata) == UAVObject::ACCESS_READWRITE) { + switch (type) { case INT8: { qint8 tmpint8 = value.toInt(); - memcpy(&data[offset + numBytesPerElement*index], &tmpint8, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpint8, numBytesPerElement); break; } case INT16: { qint16 tmpint16 = value.toInt(); - memcpy(&data[offset + numBytesPerElement*index], &tmpint16, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpint16, numBytesPerElement); break; } case INT32: { qint32 tmpint32 = value.toInt(); - memcpy(&data[offset + numBytesPerElement*index], &tmpint32, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpint32, numBytesPerElement); break; } case UINT8: { quint8 tmpuint8 = value.toUInt(); - memcpy(&data[offset + numBytesPerElement*index], &tmpuint8, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpuint8, numBytesPerElement); break; } case UINT16: { quint16 tmpuint16 = value.toUInt(); - memcpy(&data[offset + numBytesPerElement*index], &tmpuint16, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpuint16, numBytesPerElement); break; } case UINT32: { quint32 tmpuint32 = value.toUInt(); - memcpy(&data[offset + numBytesPerElement*index], &tmpuint32, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpuint32, numBytesPerElement); break; } case FLOAT32: { float tmpfloat = value.toFloat(); - memcpy(&data[offset + numBytesPerElement*index], &tmpfloat, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpfloat, numBytesPerElement); break; } case ENUM: { - qint8 tmpenum = options.indexOf( value.toString() ); + qint8 tmpenum = options.indexOf(value.toString()); // Default to 0 on invalid values. - if(tmpenum < 0) { + if (tmpenum < 0) { tmpenum = 0; } - memcpy(&data[offset + numBytesPerElement*index], &tmpenum, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpenum, numBytesPerElement); break; } case BITFIELD: { quint8 tmpbitfield; - memcpy(&tmpbitfield, &data[offset + numBytesPerElement*((quint32)(index/8))], numBytesPerElement); - tmpbitfield = (tmpbitfield & ~(1 << (index % 8))) | ( (value.toUInt()!=0?1:0) << (index % 8) ); - memcpy(&data[offset + numBytesPerElement*((quint32)(index/8))], &tmpbitfield, numBytesPerElement); + memcpy(&tmpbitfield, &data[offset + numBytesPerElement * ((quint32)(index / 8))], numBytesPerElement); + tmpbitfield = (tmpbitfield & ~(1 << (index % 8))) | ((value.toUInt() != 0 ? 1 : 0) << (index % 8)); + memcpy(&data[offset + numBytesPerElement * ((quint32)(index / 8))], &tmpbitfield, numBytesPerElement); break; } case STRING: @@ -1043,9 +1104,8 @@ void UAVObjectField::setValue(const QVariant& value, quint32 index) QString str = value.toString(); QByteArray barray = str.toAscii(); quint32 index; - for (index = 0; index < (quint32)barray.length() && index < (numElements-1); ++index) - { - data[offset+index] = barray[index]; + for (index = 0; index < (quint32)barray.length() && index < (numElements - 1); ++index) { + data[offset + index] = barray[index]; } barray[index] = '\0'; break; @@ -1063,4 +1123,3 @@ void UAVObjectField::setDouble(double value, quint32 index) { setValue(QVariant(value), index); } - diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.h index 653fcaa19..c782a86f3 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVOBJECTFIELD_H @@ -37,24 +37,22 @@ class UAVObject; -class UAVOBJECTS_EXPORT UAVObjectField: public QObject -{ +class UAVOBJECTS_EXPORT UAVObjectField : public QObject { Q_OBJECT public: typedef enum { INT8 = 0, INT16, INT32, UINT8, UINT16, UINT32, FLOAT32, ENUM, BITFIELD, STRING } FieldType; - typedef enum { EQUAL,NOT_EQUAL,BETWEEN,BIGGER,SMALLER } LimitType; - typedef struct - { + typedef enum { EQUAL, NOT_EQUAL, BETWEEN, BIGGER, SMALLER } LimitType; + typedef struct { LimitType type; QList values; int board; } LimitStruct; - UAVObjectField(const QString& name, const QString& units, FieldType type, quint32 numElements, const QStringList& options,const QString& limits=QString()); - UAVObjectField(const QString& name, const QString& units, FieldType type, const QStringList& elementNames, const QStringList& options,const QString& limits=QString()); - void initialize(quint8* data, quint32 dataOffset, UAVObject* obj); - UAVObject* getObject(); + UAVObjectField(const QString & name, const QString & units, FieldType type, quint32 numElements, const QStringList & options, const QString & limits = QString()); + UAVObjectField(const QString & name, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString & limits = QString()); + void initialize(quint8 *data, quint32 dataOffset, UAVObject *obj); + UAVObject *getObject(); FieldType getType(); QString getTypeAsString(); QString getName(); @@ -62,11 +60,11 @@ public: quint32 getNumElements(); QStringList getElementNames(); QStringList getOptions(); - qint32 pack(quint8* dataOut); - qint32 unpack(const quint8* dataIn); + qint32 pack(quint8 *dataOut); + qint32 unpack(const quint8 *dataIn); QVariant getValue(quint32 index = 0); - bool checkValue(const QVariant& data, quint32 index = 0); - void setValue(const QVariant& data, quint32 index = 0); + bool checkValue(const QVariant & data, quint32 index = 0); + void setValue(const QVariant & data, quint32 index = 0); double getDouble(quint32 index = 0); void setDouble(double value, quint32 index = 0); quint32 getDataOffset(); @@ -75,11 +73,11 @@ public: bool isText(); QString toString(); - bool isWithinLimits(QVariant var, quint32 index, int board=0); - QVariant getMaxLimit(quint32 index, int board=0); - QVariant getMinLimit(quint32 index, int board=0); + bool isWithinLimits(QVariant var, quint32 index, int board = 0); + QVariant getMaxLimit(quint32 index, int board = 0); + QVariant getMinLimit(quint32 index, int board = 0); signals: - void fieldUpdated(UAVObjectField* field); + void fieldUpdated(UAVObjectField *field); protected: QString name; @@ -90,14 +88,12 @@ protected: quint32 numElements; quint32 numBytesPerElement; quint32 offset; - quint8* data; - UAVObject* obj; + quint8 *data; + UAVObject *obj; QMap > elementLimits; void clear(); - void constructorInitialize(const QString& name, const QString& units, FieldType type, const QStringList& elementNames, const QStringList& options, const QString &limits); + void constructorInitialize(const QString & name, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString &limits); void limitsInitialize(const QString &limits); - - }; #endif // UAVOBJECTFIELD_H diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.cpp index c5d01f656..4b556b2cd 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.cpp @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavobjectmanager.h" @@ -46,47 +46,40 @@ UAVObjectManager::~UAVObjectManager() * an existing object. The object will be registered and will be properly initialized so that it can accept * updates. */ -bool UAVObjectManager::registerObject(UAVDataObject* obj) +bool UAVObjectManager::registerObject(UAVDataObject *obj) { QMutexLocker locker(mutex); + // Check if this object type is already in the list - for (int objidx = 0; objidx < objects.length(); ++objidx) - { + for (int objidx = 0; objidx < objects.length(); ++objidx) { // Check if the object ID is in the list - if (objects[objidx].length() > 0 && objects[objidx][0]->getObjID() == obj->getObjID()) - { + if (objects[objidx].length() > 0 && objects[objidx][0]->getObjID() == obj->getObjID()) { // Check if this is a single instance object, if yes we can not add a new instance - if (obj->isSingleInstance()) - { + if (obj->isSingleInstance()) { return false; } // The object type has alredy been added, so now we need to initialize the new instance with the appropriate id // There is a single metaobject for all object instances of this type, so no need to create a new one // Get object type metaobject from existing instance - UAVDataObject* refObj = dynamic_cast(objects[objidx][0]); - if (refObj == NULL) - { + UAVDataObject *refObj = dynamic_cast(objects[objidx][0]); + if (refObj == NULL) { return false; } - UAVMetaObject* mobj = refObj->getMetaObject(); + UAVMetaObject *mobj = refObj->getMetaObject(); // If the instance ID is specified and not at the default value (0) then we need to make sure // that there are no gaps in the instance list. If gaps are found then then additional instances // will be created. - if ( (obj->getInstID() > 0) && (obj->getInstID() < MAX_INSTANCES) ) - { - for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) - { - if ( objects[objidx][instidx]->getInstID() == obj->getInstID() ) - { + if ((obj->getInstID() > 0) && (obj->getInstID() < MAX_INSTANCES)) { + for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) { + if (objects[objidx][instidx]->getInstID() == obj->getInstID()) { // Instance conflict, do not add return false; } } // Check if there are any gaps between the requested instance ID and the ones in the list, // if any then create the missing instances. - for (quint32 instidx = objects[objidx].length(); instidx < obj->getInstID(); ++instidx) - { - UAVDataObject* cobj = obj->clone(instidx); + for (quint32 instidx = objects[objidx].length(); instidx < obj->getInstID(); ++instidx) { + UAVDataObject *cobj = obj->clone(instidx); cobj->initialize(mobj); objects[objidx].append(cobj); getObject(cobj->getObjID())->emitNewInstance(cobj); @@ -94,14 +87,10 @@ bool UAVObjectManager::registerObject(UAVDataObject* obj) } // Finally, initialize the actual object instance obj->initialize(mobj); - } - else if (obj->getInstID() == 0) - { + } else if (obj->getInstID() == 0) { // Assign the next available ID and initialize the object instance obj->initialize(objects[objidx].length(), mobj); - } - else - { + } else { return false; } // Add the actual object instance in the list @@ -116,7 +105,7 @@ bool UAVObjectManager::registerObject(UAVDataObject* obj) // Create metaobject QString mname = obj->getName(); mname.append("Meta"); - UAVMetaObject* mobj = new UAVMetaObject(obj->getObjID()+1, mname, obj); + UAVMetaObject *mobj = new UAVMetaObject(obj->getObjID() + 1, mname, obj); // Initialize object obj->initialize(0, mobj); // Add to list @@ -125,10 +114,10 @@ bool UAVObjectManager::registerObject(UAVDataObject* obj) return true; } -void UAVObjectManager::addObject(UAVObject* obj) +void UAVObjectManager::addObject(UAVObject *obj) { // Add to list - QList list; + QList list; list.append(obj); objects.append(list); emit newObject(obj); @@ -138,39 +127,36 @@ void UAVObjectManager::addObject(UAVObject* obj) * Get all objects. A two dimentional QList is returned. Objects are grouped by * instances of the same object type. */ -QList< QList > UAVObjectManager::getObjects() +QList< QList > UAVObjectManager::getObjects() { QMutexLocker locker(mutex); + return objects; } /** * Same as getObjects() but will only return DataObjects. */ -QList< QList > UAVObjectManager::getDataObjects() +QList< QList > UAVObjectManager::getDataObjects() { QMutexLocker locker(mutex); - QList< QList > dObjects; + + QList< QList > dObjects; // Go through objects and copy to new list when types match - for (int objidx = 0; objidx < objects.length(); ++objidx) - { - if (objects[objidx].length() > 0) - { + for (int objidx = 0; objidx < objects.length(); ++objidx) { + if (objects[objidx].length() > 0) { // Check type - UAVDataObject* obj = dynamic_cast(objects[objidx][0]); - if (obj != NULL) - { + UAVDataObject *obj = dynamic_cast(objects[objidx][0]); + if (obj != NULL) { // Create instance list - QList list; + QList list; // Go through instances and cast them to UAVDataObject, then add to list - for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) - { - obj = dynamic_cast(objects[objidx][instidx]); - if (obj != NULL) - { - list.append(obj); - } + for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) { + obj = dynamic_cast(objects[objidx][instidx]); + if (obj != NULL) { + list.append(obj); + } } // Append to object list dObjects.append(list); @@ -184,30 +170,26 @@ QList< QList > UAVObjectManager::getDataObjects() /** * Same as getObjects() but will only return MetaObjects. */ -QList > UAVObjectManager::getMetaObjects() +QList > UAVObjectManager::getMetaObjects() { QMutexLocker locker(mutex); - QList< QList > mObjects; + + QList< QList > mObjects; // Go through objects and copy to new list when types match - for (int objidx = 0; objidx < objects.length(); ++objidx) - { - if (objects[objidx].length() > 0) - { + for (int objidx = 0; objidx < objects.length(); ++objidx) { + if (objects[objidx].length() > 0) { // Check type - UAVMetaObject* obj = dynamic_cast(objects[objidx][0]); - if (obj != NULL) - { + UAVMetaObject *obj = dynamic_cast(objects[objidx][0]); + if (obj != NULL) { // Create instance list - QList list; + QList list; // Go through instances and cast them to UAVMetaObject, then add to list - for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) - { - obj = dynamic_cast(objects[objidx][instidx]); - if (obj != NULL) - { - list.append(obj); - } + for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) { + obj = dynamic_cast(objects[objidx][instidx]); + if (obj != NULL) { + list.append(obj); + } } // Append to object list mObjects.append(list); @@ -222,7 +204,7 @@ QList > UAVObjectManager::getMetaObjects() * Get a specific object given its name and instance ID * @returns The object is found or NULL if not */ -UAVObject* UAVObjectManager::getObject(const QString& name, quint32 instId) +UAVObject *UAVObjectManager::getObject(const QString & name, quint32 instId) { return getObject(&name, 0, instId); } @@ -231,7 +213,7 @@ UAVObject* UAVObjectManager::getObject(const QString& name, quint32 instId) * Get a specific object given its object and instance ID * @returns The object is found or NULL if not */ -UAVObject* UAVObjectManager::getObject(quint32 objId, quint32 instId) +UAVObject *UAVObjectManager::getObject(quint32 objId, quint32 instId) { return getObject(NULL, objId, instId); } @@ -239,29 +221,25 @@ UAVObject* UAVObjectManager::getObject(quint32 objId, quint32 instId) /** * Helper function for the public getObject() functions. */ -UAVObject* UAVObjectManager::getObject(const QString* name, quint32 objId, quint32 instId) +UAVObject *UAVObjectManager::getObject(const QString *name, quint32 objId, quint32 instId) { QMutexLocker locker(mutex); + // Check if this object type is already in the list - for (int objidx = 0; objidx < objects.length(); ++objidx) - { + for (int objidx = 0; objidx < objects.length(); ++objidx) { // Check if the object ID is in the list - if (objects[objidx].length() > 0) - { - if ( (name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId) ) - { + if (objects[objidx].length() > 0) { + if ((name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId)) { // Look for the requested instance ID - for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) - { - if (objects[objidx][instidx]->getInstID() == instId) - { + for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) { + if (objects[objidx][instidx]->getInstID() == instId) { return objects[objidx][instidx]; } } } } } - //qWarning("UAVObjectManager::getObject: Object not found. Probably a bug or mismatched GCS/flight versions."); + // qWarning("UAVObjectManager::getObject: Object not found. Probably a bug or mismatched GCS/flight versions."); // If this point is reached then the requested object could not be found return NULL; } @@ -269,7 +247,7 @@ UAVObject* UAVObjectManager::getObject(const QString* name, quint32 objId, quint /** * Get all the instances of the object specified by name */ -QList UAVObjectManager::getObjectInstances(const QString& name) +QList UAVObjectManager::getObjectInstances(const QString & name) { return getObjectInstances(&name, 0); } @@ -277,7 +255,7 @@ QList UAVObjectManager::getObjectInstances(const QString& name) /** * Get all the instances of the object specified by its ID */ -QList UAVObjectManager::getObjectInstances(quint32 objId) +QList UAVObjectManager::getObjectInstances(quint32 objId) { return getObjectInstances(NULL, objId); } @@ -285,29 +263,27 @@ QList UAVObjectManager::getObjectInstances(quint32 objId) /** * Helper function for the public getObjectInstances() */ -QList UAVObjectManager::getObjectInstances(const QString* name, quint32 objId) +QList UAVObjectManager::getObjectInstances(const QString *name, quint32 objId) { QMutexLocker locker(mutex); + // Check if this object type is already in the list - for (int objidx = 0; objidx < objects.length(); ++objidx) - { + for (int objidx = 0; objidx < objects.length(); ++objidx) { // Check if the object ID is in the list - if (objects[objidx].length() > 0) - { - if ( (name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId) ) - { + if (objects[objidx].length() > 0) { + if ((name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId)) { return objects[objidx]; } } } // If this point is reached then the requested object could not be found - return QList(); + return QList(); } /** * Get the number of instances for an object given its name */ -qint32 UAVObjectManager::getNumInstances(const QString& name) +qint32 UAVObjectManager::getNumInstances(const QString & name) { return getNumInstances(&name, 0); } @@ -323,17 +299,15 @@ qint32 UAVObjectManager::getNumInstances(quint32 objId) /** * Helper function for public getNumInstances */ -qint32 UAVObjectManager::getNumInstances(const QString* name, quint32 objId) +qint32 UAVObjectManager::getNumInstances(const QString *name, quint32 objId) { QMutexLocker locker(mutex); + // Check if this object type is already in the list - for (int objidx = 0; objidx < objects.length(); ++objidx) - { + for (int objidx = 0; objidx < objects.length(); ++objidx) { // Check if the object ID is in the list - if (objects[objidx].length() > 0) - { - if ( (name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId) ) - { + if (objects[objidx].length() > 0) { + if ((name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId)) { return objects[objidx].length(); } } diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.h index 5f17daa1f..a7fbfdb31 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVOBJECTMANAGER_H @@ -36,39 +36,38 @@ #include #include -class UAVOBJECTS_EXPORT UAVObjectManager: public QObject -{ +class UAVOBJECTS_EXPORT UAVObjectManager : public QObject { Q_OBJECT public: UAVObjectManager(); ~UAVObjectManager(); - bool registerObject(UAVDataObject* obj); - QList< QList > getObjects(); - QList< QList > getDataObjects(); - QList< QList > getMetaObjects(); - UAVObject* getObject(const QString& name, quint32 instId = 0); - UAVObject* getObject(quint32 objId, quint32 instId = 0); - QList getObjectInstances(const QString& name); - QList getObjectInstances(quint32 objId); - qint32 getNumInstances(const QString& name); + bool registerObject(UAVDataObject *obj); + QList< QList > getObjects(); + QList< QList > getDataObjects(); + QList< QList > getMetaObjects(); + UAVObject *getObject(const QString & name, quint32 instId = 0); + UAVObject *getObject(quint32 objId, quint32 instId = 0); + QList getObjectInstances(const QString & name); + QList getObjectInstances(quint32 objId); + qint32 getNumInstances(const QString & name); qint32 getNumInstances(quint32 objId); signals: - void newObject(UAVObject* obj); - void newInstance(UAVObject* obj); + void newObject(UAVObject *obj); + void newInstance(UAVObject *obj); private: static const quint32 MAX_INSTANCES = 1000; - QList< QList > objects; - QMutex* mutex; + QList< QList > objects; + QMutex *mutex; - void addObject(UAVObject* obj); - UAVObject* getObject(const QString* name, quint32 objId, quint32 instId); - QList getObjectInstances(const QString* name, quint32 objId); - qint32 getNumInstances(const QString* name, quint32 objId); + void addObject(UAVObject *obj); + UAVObject *getObject(const QString *name, quint32 objId, quint32 instId); + QList getObjectInstances(const QString *name, quint32 objId); + qint32 getNumInstances(const QString *name, quint32 objId); }; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects_global.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects_global.h index 9ef17f24b..b1e27f904 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects_global.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects_global.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsinit.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsinit.h index 03ae0b1d3..77e905ee1 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsinit.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsinit.h @@ -10,19 +10,19 @@ * @{ * @brief The UAVUObjects GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVOBJECTSINIT_H @@ -30,6 +30,6 @@ #include "uavobjectmanager.h" -void UAVObjectsInitialize(UAVObjectManager* objMngr); +void UAVObjectsInitialize(UAVObjectManager *objMngr); #endif // UAVOBJECTSINIT_H diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.cpp index c3f307927..892f2e11e 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.cpp @@ -11,42 +11,37 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavobjectsplugin.h" #include "uavobjectsinit.h" UAVObjectsPlugin::UAVObjectsPlugin() -{ - -} +{} UAVObjectsPlugin::~UAVObjectsPlugin() -{ - -} +{} void UAVObjectsPlugin::extensionsInitialized() -{ +{} -} - -bool UAVObjectsPlugin::initialize(const QStringList & arguments, QString * errorString) +bool UAVObjectsPlugin::initialize(const QStringList & arguments, QString *errorString) { // Create object manager and expose object - UAVObjectManager* objMngr = new UAVObjectManager(); + UAVObjectManager *objMngr = new UAVObjectManager(); + addAutoReleasedObject(objMngr); // Initialize UAVObjects UAVObjectsInitialize(objMngr); @@ -57,8 +52,6 @@ bool UAVObjectsPlugin::initialize(const QStringList & arguments, QString * error } void UAVObjectsPlugin::shutdown() -{ - -} +{} Q_EXPORT_PLUGIN(UAVObjectsPlugin) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.h index 137af2ed8..4a9238752 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVOBJECTSPLUGIN_H @@ -33,9 +33,8 @@ #include #include "uavobjectmanager.h" -class UAVOBJECTS_EXPORT UAVObjectsPlugin: - public ExtensionSystem::IPlugin -{ +class UAVOBJECTS_EXPORT UAVObjectsPlugin : + public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -43,7 +42,7 @@ public: ~UAVObjectsPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); }; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/wireshark/op-uavtalk/packet-op-uavtalk.c b/ground/openpilotgcs/src/plugins/uavobjects/wireshark/op-uavtalk/packet-op-uavtalk.c index df8134df8..f45c9e868 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/wireshark/op-uavtalk/packet-op-uavtalk.c +++ b/ground/openpilotgcs/src/plugins/uavobjects/wireshark/op-uavtalk/packet-op-uavtalk.c @@ -1,26 +1,26 @@ /* packet-op-uavtalk.c - * Routines for OpenPilot UAVTalk packet dissection - * Copyright 2012 Stacey Sheldon - * - * $Id$ - * - * Wireshark - Network traffic analyzer - * By Gerald Combs - * Copyright 1998 Gerald Combs - * - * This program is free software; you can redistribute it and/or - * modify it under the terms of the GNU General Public License - * as published by the Free Software Foundation; either version 2 - * of the License, or (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * Routines for OpenPilot UAVTalk packet dissection + * Copyright 2012 Stacey Sheldon + * + * $Id$ + * + * Wireshark - Network traffic analyzer + * By Gerald Combs + * Copyright 1998 Gerald Combs + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. */ @@ -30,7 +30,7 @@ #include #include -#include /* ptvcursor_* */ +#include /* ptvcursor_* */ #include #include @@ -39,167 +39,167 @@ static guint global_op_uavtalk_port = 9000; static int proto_op_uavtalk = -1; -static gint ett_op_uavtalk = -1; +static gint ett_op_uavtalk = -1; static dissector_handle_t data_handle; static dissector_table_t uavtalk_subdissector_table; -static int hf_op_uavtalk_sync = -1; +static int hf_op_uavtalk_sync = -1; static int hf_op_uavtalk_version = -1; -static int hf_op_uavtalk_type = -1; -static int hf_op_uavtalk_len = -1; -static int hf_op_uavtalk_objid = -1; -static int hf_op_uavtalk_crc8 = -1; +static int hf_op_uavtalk_type = -1; +static int hf_op_uavtalk_len = -1; +static int hf_op_uavtalk_objid = -1; +static int hf_op_uavtalk_crc8 = -1; #define UAVTALK_SYNC_VAL 0x3C -static const value_string uavtalk_packet_types[]={ - { 0, "TxObj" }, - { 1, "GetObj" }, - { 2, "SetObjAckd" }, - { 3, "Ack" }, - { 4, "Nack" }, - { 0, NULL } +static const value_string uavtalk_packet_types[] = { + { 0, "TxObj" }, + { 1, "GetObj" }, + { 2, "SetObjAckd" }, + { 3, "Ack" }, + { 4, "Nack" }, + { 0, NULL } }; void proto_reg_handoff_op_uavtalk(void); -#define UAVTALK_HEADER_SIZE 8 +#define UAVTALK_HEADER_SIZE 8 #define UAVTALK_TRAILER_SIZE 1 static int dissect_op_uavtalk(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree) { - gint offset = 0; + gint offset = 0; - guint8 packet_type = tvb_get_guint8(tvb, 1) & 0x7; - guint32 objid = tvb_get_letohl(tvb, 4); - guint32 payload_length = tvb_get_letohs(tvb, 2) - UAVTALK_HEADER_SIZE - UAVTALK_TRAILER_SIZE; - guint32 reported_length = tvb_reported_length(tvb); + guint8 packet_type = tvb_get_guint8(tvb, 1) & 0x7; + guint32 objid = tvb_get_letohl(tvb, 4); + guint32 payload_length = tvb_get_letohs(tvb, 2) - UAVTALK_HEADER_SIZE - UAVTALK_TRAILER_SIZE; + guint32 reported_length = tvb_reported_length(tvb); - col_set_str(pinfo->cinfo, COL_PROTOCOL, "UAVTALK"); - /* Clear out stuff in the info column */ - col_clear(pinfo->cinfo, COL_INFO); + col_set_str(pinfo->cinfo, COL_PROTOCOL, "UAVTALK"); + /* Clear out stuff in the info column */ + col_clear(pinfo->cinfo, COL_INFO); - col_append_fstr(pinfo->cinfo, COL_INFO, "%s: 0x%08x", val_to_str_const(packet_type, uavtalk_packet_types, ""), objid); - if (objid & 0x1) { - col_append_str(pinfo->cinfo, COL_INFO, "(META)"); - } - - - if (tree) { /* we are being asked for details */ - proto_tree *op_uavtalk_tree = NULL; - ptvcursor_t * cursor; - proto_item *ti = NULL; - - /* Add a top-level entry to the dissector tree for this protocol */ - ti = proto_tree_add_item(tree, proto_op_uavtalk, tvb, 0, -1, ENC_NA); - - /* Create a subtree to contain the dissection of this protocol */ - op_uavtalk_tree = proto_item_add_subtree(ti, ett_op_uavtalk); - - /* Dissect the packet and populate the subtree */ - cursor = ptvcursor_new(op_uavtalk_tree, tvb, 0); - - /* Populate the fields in this protocol */ - ptvcursor_add(cursor, hf_op_uavtalk_sync, 1, ENC_LITTLE_ENDIAN); - ptvcursor_add_no_advance(cursor, hf_op_uavtalk_version, 1, ENC_LITTLE_ENDIAN); - ptvcursor_add(cursor, hf_op_uavtalk_type, 1, ENC_LITTLE_ENDIAN); - ptvcursor_add(cursor, hf_op_uavtalk_len, 2, ENC_LITTLE_ENDIAN); - ptvcursor_add(cursor, hf_op_uavtalk_objid, 4, ENC_LITTLE_ENDIAN); - - offset = ptvcursor_current_offset(cursor); - - ptvcursor_free(cursor); - - proto_tree_add_item(op_uavtalk_tree, hf_op_uavtalk_crc8, tvb, reported_length - UAVTALK_TRAILER_SIZE, UAVTALK_TRAILER_SIZE, ENC_LITTLE_ENDIAN); - } else { - offset = UAVTALK_HEADER_SIZE; - } - - { - tvbuff_t * next_tvb = tvb_new_subset(tvb, offset, - reported_length - UAVTALK_HEADER_SIZE - UAVTALK_TRAILER_SIZE, - payload_length); - - /* Check if we have an embedded objid to decode */ - if ((packet_type == 0) || (packet_type == 2)) { - /* Call any registered subdissector for this objid */ - if (!dissector_try_uint(uavtalk_subdissector_table, objid, next_tvb, pinfo, tree)) { - /* No subdissector registered, use the default data dissector */ - call_dissector(data_handle, next_tvb, pinfo, tree); - } - } else { - /* Render any remaining data as raw bytes */ - call_dissector(data_handle, next_tvb, pinfo, tree); + col_append_fstr(pinfo->cinfo, COL_INFO, "%s: 0x%08x", val_to_str_const(packet_type, uavtalk_packet_types, ""), objid); + if (objid & 0x1) { + col_append_str(pinfo->cinfo, COL_INFO, "(META)"); } - } - return UAVTALK_HEADER_SIZE + UAVTALK_TRAILER_SIZE; + + if (tree) { /* we are being asked for details */ + proto_tree *op_uavtalk_tree = NULL; + ptvcursor_t *cursor; + proto_item *ti = NULL; + + /* Add a top-level entry to the dissector tree for this protocol */ + ti = proto_tree_add_item(tree, proto_op_uavtalk, tvb, 0, -1, ENC_NA); + + /* Create a subtree to contain the dissection of this protocol */ + op_uavtalk_tree = proto_item_add_subtree(ti, ett_op_uavtalk); + + /* Dissect the packet and populate the subtree */ + cursor = ptvcursor_new(op_uavtalk_tree, tvb, 0); + + /* Populate the fields in this protocol */ + ptvcursor_add(cursor, hf_op_uavtalk_sync, 1, ENC_LITTLE_ENDIAN); + ptvcursor_add_no_advance(cursor, hf_op_uavtalk_version, 1, ENC_LITTLE_ENDIAN); + ptvcursor_add(cursor, hf_op_uavtalk_type, 1, ENC_LITTLE_ENDIAN); + ptvcursor_add(cursor, hf_op_uavtalk_len, 2, ENC_LITTLE_ENDIAN); + ptvcursor_add(cursor, hf_op_uavtalk_objid, 4, ENC_LITTLE_ENDIAN); + + offset = ptvcursor_current_offset(cursor); + + ptvcursor_free(cursor); + + proto_tree_add_item(op_uavtalk_tree, hf_op_uavtalk_crc8, tvb, reported_length - UAVTALK_TRAILER_SIZE, UAVTALK_TRAILER_SIZE, ENC_LITTLE_ENDIAN); + } else { + offset = UAVTALK_HEADER_SIZE; + } + + { + tvbuff_t *next_tvb = tvb_new_subset(tvb, offset, + reported_length - UAVTALK_HEADER_SIZE - UAVTALK_TRAILER_SIZE, + payload_length); + + /* Check if we have an embedded objid to decode */ + if ((packet_type == 0) || (packet_type == 2)) { + /* Call any registered subdissector for this objid */ + if (!dissector_try_uint(uavtalk_subdissector_table, objid, next_tvb, pinfo, tree)) { + /* No subdissector registered, use the default data dissector */ + call_dissector(data_handle, next_tvb, pinfo, tree); + } + } else { + /* Render any remaining data as raw bytes */ + call_dissector(data_handle, next_tvb, pinfo, tree); + } + } + + return UAVTALK_HEADER_SIZE + UAVTALK_TRAILER_SIZE; } void proto_register_op_uavtalk(void) { - module_t * op_uavtalk_module; + module_t *op_uavtalk_module; - static hf_register_info hf[] = { - { &hf_op_uavtalk_sync, - { "Sync Byte", "uavtalk.sync", FT_UINT8, - BASE_HEX, NULL, 0x0, NULL, HFILL } - }, - { &hf_op_uavtalk_version, - { "Version", "uavtalk.ver", FT_UINT8, - BASE_DEC, NULL, 0xf8, NULL, HFILL } - }, - { &hf_op_uavtalk_type, - { "Type", "uavtalk.type", FT_UINT8, - BASE_HEX, VALS(uavtalk_packet_types), 0x07, NULL, HFILL } - }, - { &hf_op_uavtalk_len, - { "Length", "uavtalk.len", FT_UINT16, - BASE_DEC, NULL, 0x0, NULL, HFILL } - }, - { &hf_op_uavtalk_objid, - { "ObjID", "uavtalk.objid", FT_UINT32, - BASE_HEX, NULL, 0x0, NULL, HFILL } - }, - { &hf_op_uavtalk_crc8, - { "Crc8", "uavtalk.crc8", FT_UINT8, - BASE_HEX, NULL, 0x0, NULL, HFILL } - }, - }; + static hf_register_info hf[] = { + { &hf_op_uavtalk_sync, + { "Sync Byte", "uavtalk.sync", FT_UINT8, + BASE_HEX, NULL, 0x0, NULL, HFILL } + }, + { &hf_op_uavtalk_version, + { "Version", "uavtalk.ver", FT_UINT8, + BASE_DEC, NULL, 0xf8, NULL, HFILL } + }, + { &hf_op_uavtalk_type, + { "Type", "uavtalk.type", FT_UINT8, + BASE_HEX, VALS(uavtalk_packet_types), 0x07, NULL, HFILL } + }, + { &hf_op_uavtalk_len, + { "Length", "uavtalk.len", FT_UINT16, + BASE_DEC, NULL, 0x0, NULL, HFILL } + }, + { &hf_op_uavtalk_objid, + { "ObjID", "uavtalk.objid", FT_UINT32, + BASE_HEX, NULL, 0x0, NULL, HFILL } + }, + { &hf_op_uavtalk_crc8, + { "Crc8", "uavtalk.crc8", FT_UINT8, + BASE_HEX, NULL, 0x0, NULL, HFILL } + }, + }; /* Setup protocol subtree array */ - static gint *ett[] = { - &ett_op_uavtalk - }; + static gint *ett[] = { + &ett_op_uavtalk + }; - proto_op_uavtalk = proto_register_protocol("OpenPilot UAVTalk Protocol", - "UAVTALK", - "uavtalk"); + proto_op_uavtalk = proto_register_protocol("OpenPilot UAVTalk Protocol", + "UAVTALK", + "uavtalk"); - /* Allow subdissectors for each objid to bind for decoding */ - uavtalk_subdissector_table = register_dissector_table("uavtalk.objid", "UAVObject ID", FT_UINT32, BASE_HEX); + /* Allow subdissectors for each objid to bind for decoding */ + uavtalk_subdissector_table = register_dissector_table("uavtalk.objid", "UAVObject ID", FT_UINT32, BASE_HEX); - proto_register_subtree_array(ett, array_length(ett)); - proto_register_field_array(proto_op_uavtalk, hf, array_length(hf)); + proto_register_subtree_array(ett, array_length(ett)); + proto_register_field_array(proto_op_uavtalk, hf, array_length(hf)); - op_uavtalk_module = prefs_register_protocol(proto_op_uavtalk, proto_reg_handoff_op_uavtalk); + op_uavtalk_module = prefs_register_protocol(proto_op_uavtalk, proto_reg_handoff_op_uavtalk); - prefs_register_uint_preference(op_uavtalk_module, "udp.port", "UAVTALK UDP port", - "UAVTALK port (default 9000)", 10, &global_op_uavtalk_port); + prefs_register_uint_preference(op_uavtalk_module, "udp.port", "UAVTALK UDP port", + "UAVTALK port (default 9000)", 10, &global_op_uavtalk_port); } void proto_reg_handoff_op_uavtalk(void) { - static dissector_handle_t op_uavtalk_handle; + static dissector_handle_t op_uavtalk_handle; - op_uavtalk_handle = new_create_dissector_handle(dissect_op_uavtalk, proto_op_uavtalk); - dissector_add_handle("udp.port", op_uavtalk_handle); /* for "decode as" */ + op_uavtalk_handle = new_create_dissector_handle(dissect_op_uavtalk, proto_op_uavtalk); + dissector_add_handle("udp.port", op_uavtalk_handle); /* for "decode as" */ - if (global_op_uavtalk_port != 0) { - dissector_add_uint("udp.port", global_op_uavtalk_port, op_uavtalk_handle); - } + if (global_op_uavtalk_port != 0) { + dissector_add_uint("udp.port", global_op_uavtalk_port, op_uavtalk_handle); + } - /* Lookup the default dissector for raw data */ - data_handle = find_dissector("data"); + /* Lookup the default dissector for raw data */ + data_handle = find_dissector("data"); } diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h b/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h index fb7cf2288..aedba4c33 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h @@ -2,55 +2,62 @@ #define DEVICEDESCRIPTORSTRUCT_H #include -struct deviceDescriptorStruct -{ +struct deviceDescriptorStruct { public: - QString gitHash; - QString gitDate; - QString gitTag; - QByteArray fwHash; - QByteArray uavoHash; - int boardType; - int boardRevision; - static QString idToBoardName(int id) - { - switch (id) { - case 0x0101: - // MB - return QString("OpenPilot MainBoard"); - break; - case 0x0201: - // INS - return QString("OpenPilot INS"); - break; - case 0x0301: - // OPLink Mini - return QString("OPLink"); - break; - case 0x0401: - // Coptercontrol - return QString("CopterControl"); - break; - case 0x0402: - // Coptercontrol 3D - // It would be nice to say CC3D here but since currently we use string comparisons - // for firmware compatibility and the filename path that would break - return QString("CopterControl"); - break; - case 0x0901: - // Revolution - return QString("Revolution"); - break; - case 0x0903: - // Revo Mini - return QString("Revolution"); - break; - default: - return QString(""); - break; - } - } - deviceDescriptorStruct(){} + QString gitHash; + QString gitDate; + QString gitTag; + QByteArray fwHash; + QByteArray uavoHash; + int boardType; + int boardRevision; + static QString idToBoardName(int id) + { + switch (id) { + case 0x0101: + // MB + return QString("OpenPilot MainBoard"); + + break; + case 0x0201: + // INS + return QString("OpenPilot INS"); + + break; + case 0x0301: + // OPLink Mini + return QString("OPLink"); + + break; + case 0x0401: + // Coptercontrol + return QString("CopterControl"); + + break; + case 0x0402: + // Coptercontrol 3D + // It would be nice to say CC3D here but since currently we use string comparisons + // for firmware compatibility and the filename path that would break + return QString("CopterControl"); + + break; + case 0x0901: + // Revolution + return QString("Revolution"); + + break; + case 0x0903: + // Revo Mini + return QString("Revolution"); + + break; + default: + return QString(""); + + break; + } + } + deviceDescriptorStruct() {} }; #endif // DEVICEDESCRIPTORSTRUCT_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index 3f942e3e0..c89683412 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -45,43 +45,40 @@ UAVObjectUtilManager::UAVObjectUtilManager() { - mutex = new QMutex(QMutex::Recursive); + mutex = new QMutex(QMutex::Recursive); saveState = IDLE; failureTimer.stop(); failureTimer.setSingleShot(true); failureTimer.setInterval(1000); - connect(&failureTimer, SIGNAL(timeout()),this,SLOT(objectPersistenceOperationFailed())); + connect(&failureTimer, SIGNAL(timeout()), this, SLOT(objectPersistenceOperationFailed())); - pm = NULL; - obm = NULL; + pm = NULL; + obm = NULL; obum = NULL; - pm = ExtensionSystem::PluginManager::instance(); - if (pm) - { - obm = pm->getObject(); + pm = ExtensionSystem::PluginManager::instance(); + if (pm) { + obm = pm->getObject(); obum = pm->getObject(); } - } UAVObjectUtilManager::~UAVObjectUtilManager() { -// while (!queue.isEmpty()) - { - } +// while (!queue.isEmpty()) + {} - disconnect(); + disconnect(); - if (mutex) - { - delete mutex; - mutex = NULL; - } + if (mutex) { + delete mutex; + mutex = NULL; + } } -UAVObjectManager* UAVObjectUtilManager::getObjectManager() { +UAVObjectManager *UAVObjectUtilManager::getObjectManager() +{ Q_ASSERT(obm); return obm; } @@ -92,8 +89,8 @@ UAVObjectManager* UAVObjectUtilManager::getObjectManager() { // /* - Add a new object to save in the queue - */ + Add a new object to save in the queue + */ void UAVObjectUtilManager::saveObjectToSD(UAVObject *obj) { // Add to queue @@ -103,35 +100,32 @@ void UAVObjectUtilManager::saveObjectToSD(UAVObject *obj) // If queue length is one, then start sending (call sendNextObject) // Otherwise, do nothing, it's sending anyway - if (queue.length()==1) + if (queue.length() == 1) { saveNextObject(); - - + } } void UAVObjectUtilManager::saveNextObject() { - if ( queue.isEmpty() ) - { + if (queue.isEmpty()) { return; } Q_ASSERT(saveState == IDLE); // Get next object from the queue - UAVObject* obj = queue.head(); + UAVObject *obj = queue.head(); qDebug() << "Send save object request to board " << obj->getName(); - ObjectPersistence* objper = dynamic_cast( getObjectManager()->getObject(ObjectPersistence::NAME) ); - connect(objper, SIGNAL(transactionCompleted(UAVObject*,bool)), this, SLOT(objectPersistenceTransactionCompleted(UAVObject*,bool))); - connect(objper, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objectPersistenceUpdated(UAVObject *))); + ObjectPersistence *objper = dynamic_cast(getObjectManager()->getObject(ObjectPersistence::NAME)); + connect(objper, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(objectPersistenceTransactionCompleted(UAVObject *, bool))); + connect(objper, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(objectPersistenceUpdated(UAVObject *))); saveState = AWAITING_ACK; - if (obj != NULL) - { + if (obj != NULL) { ObjectPersistence::DataFields data; - data.Operation = ObjectPersistence::OPERATION_SAVE; - data.Selection = ObjectPersistence::SELECTION_SINGLEOBJECT; - data.ObjectID = obj->getObjID(); + data.Operation = ObjectPersistence::OPERATION_SAVE; + data.Selection = ObjectPersistence::SELECTION_SINGLEOBJECT; + data.ObjectID = obj->getObjID(); data.InstanceID = obj->getInstID(); objper->setData(data); objper->updated(); @@ -144,16 +138,16 @@ void UAVObjectUtilManager::saveNextObject() } /** - * @brief Process the transactionCompleted message from Telemetry indicating request sent successfully - * @param[in] The object just transsacted. Must be ObjectPersistance - * @param[in] success Indicates that the transaction did not time out - * - * After a failed transaction (usually timeout) resends the save request. After a succesful - * transaction will then wait for a save completed update from the autopilot. - */ -void UAVObjectUtilManager::objectPersistenceTransactionCompleted(UAVObject* obj, bool success) + * @brief Process the transactionCompleted message from Telemetry indicating request sent successfully + * @param[in] The object just transsacted. Must be ObjectPersistance + * @param[in] success Indicates that the transaction did not time out + * + * After a failed transaction (usually timeout) resends the save request. After a succesful + * transaction will then wait for a save completed update from the autopilot. + */ +void UAVObjectUtilManager::objectPersistenceTransactionCompleted(UAVObject *obj, bool success) { - if(success) { + if (success) { Q_ASSERT(obj->getName().compare("ObjectPersistence") == 0); Q_ASSERT(saveState == AWAITING_ACK); // Two things can happen: @@ -163,7 +157,7 @@ void UAVObjectUtilManager::objectPersistenceTransactionCompleted(UAVObject* obj, // For this reason, we will arm a 1 second timer to make provision for this and not block // the queue: saveState = AWAITING_COMPLETED; - disconnect(obj, SIGNAL(transactionCompleted(UAVObject*,bool)), this, SLOT(objectPersistenceTransactionCompleted(UAVObject*,bool))); + disconnect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(objectPersistenceTransactionCompleted(UAVObject *, bool))); failureTimer.start(2000); // Create a timeout } else { // Can be caused by timeout errors on sending. Forget it and send next. @@ -178,20 +172,20 @@ void UAVObjectUtilManager::objectPersistenceTransactionCompleted(UAVObject* obj, } /** - * @brief Object persistence operation failed, i.e. we never got an update - * from the board saying "completed". - */ + * @brief Object persistence operation failed, i.e. we never got an update + * from the board saying "completed". + */ void UAVObjectUtilManager::objectPersistenceOperationFailed() { - if(saveState == AWAITING_COMPLETED) { - //TODO: some warning that this operation failed somehow + if (saveState == AWAITING_COMPLETED) { + // TODO: some warning that this operation failed somehow // We have to disconnect the object persistence 'updated' signal // and ask to save the next object: - ObjectPersistence * objectPersistence = ObjectPersistence::GetInstance(getObjectManager()); + ObjectPersistence *objectPersistence = ObjectPersistence::GetInstance(getObjectManager()); Q_ASSERT(objectPersistence); - UAVObject* obj = queue.dequeue(); // We can now remove the object, it failed anyway. + UAVObject *obj = queue.dequeue(); // We can now remove the object, it failed anyway. Q_ASSERT(obj); objectPersistence->disconnect(this); @@ -201,31 +195,29 @@ void UAVObjectUtilManager::objectPersistenceOperationFailed() saveNextObject(); } - } - /** - * @brief Process the ObjectPersistence updated message to confirm the right object saved - * then requests next object be saved. - * @param[in] The object just received. Must be ObjectPersistance - */ -void UAVObjectUtilManager::objectPersistenceUpdated(UAVObject * obj) + * @brief Process the ObjectPersistence updated message to confirm the right object saved + * then requests next object be saved. + * @param[in] The object just received. Must be ObjectPersistance + */ +void UAVObjectUtilManager::objectPersistenceUpdated(UAVObject *obj) { Q_ASSERT(obj); Q_ASSERT(obj->getObjID() == ObjectPersistence::OBJID); ObjectPersistence::DataFields objectPersistence = ((ObjectPersistence *)obj)->getData(); - if(saveState == AWAITING_COMPLETED && objectPersistence.Operation == ObjectPersistence::OPERATION_ERROR) { + if (saveState == AWAITING_COMPLETED && objectPersistence.Operation == ObjectPersistence::OPERATION_ERROR) { failureTimer.stop(); objectPersistenceOperationFailed(); } else if (saveState == AWAITING_COMPLETED && objectPersistence.Operation == ObjectPersistence::OPERATION_COMPLETED) { failureTimer.stop(); // Check right object saved - UAVObject* savingObj = queue.head(); - if(objectPersistence.ObjectID != savingObj->getObjID() ) { + UAVObject *savingObj = queue.head(); + if (objectPersistence.ObjectID != savingObj->getObjID()) { objectPersistenceOperationFailed(); return; } @@ -240,45 +232,49 @@ void UAVObjectUtilManager::objectPersistenceUpdated(UAVObject * obj) } /** - * Helper function that makes sure FirmwareIAP is updated and then returns the data - */ + * Helper function that makes sure FirmwareIAP is updated and then returns the data + */ FirmwareIAPObj::DataFields UAVObjectUtilManager::getFirmwareIap() { FirmwareIAPObj::DataFields dummy; FirmwareIAPObj *firmwareIap = FirmwareIAPObj::GetInstance(obm); + Q_ASSERT(firmwareIap); - if (!firmwareIap) + if (!firmwareIap) { return dummy; + } return firmwareIap->getData(); } /** - * Get the UAV Board model, for anyone interested. Return format is: - * (Board Type << 8) + BoardRevision. - */ + * Get the UAV Board model, for anyone interested. Return format is: + * (Board Type << 8) + BoardRevision. + */ int UAVObjectUtilManager::getBoardModel() { FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); - qDebug()<<"Board type="< #include -class UAVOBJECTUTIL_EXPORT UAVObjectUtilManager: public QObject -{ +class UAVOBJECTUTIL_EXPORT UAVObjectUtilManager : public QObject { Q_OBJECT public: - UAVObjectUtilManager(); - ~UAVObjectUtilManager(); + UAVObjectUtilManager(); + ~UAVObjectUtilManager(); - int setHomeLocation(double LLA[3], bool save_to_sdcard); - int getHomeLocation(bool &set, double LLA[3]); + int setHomeLocation(double LLA[3], bool save_to_sdcard); + int getHomeLocation(bool &set, double LLA[3]); - int getGPSPosition(double LLA[3]); + int getGPSPosition(double LLA[3]); - int getBoardModel(); - QByteArray getBoardCPUSerial(); - quint32 getFirmwareCRC(); - QByteArray getBoardDescription(); - deviceDescriptorStruct getBoardDescriptionStruct(); - static bool descriptionToStructure(QByteArray desc,deviceDescriptorStruct & struc); - UAVObjectManager* getObjectManager(); - void saveObjectToSD(UAVObject *obj); + int getBoardModel(); + QByteArray getBoardCPUSerial(); + quint32 getFirmwareCRC(); + QByteArray getBoardDescription(); + deviceDescriptorStruct getBoardDescriptionStruct(); + static bool descriptionToStructure(QByteArray desc, deviceDescriptorStruct & struc); + UAVObjectManager *getObjectManager(); + void saveObjectToSD(UAVObject *obj); protected: - FirmwareIAPObj::DataFields getFirmwareIap(); + FirmwareIAPObj::DataFields getFirmwareIap(); signals: - void saveCompleted(int objectID, bool status); + void saveCompleted(int objectID, bool status); private: QMutex *mutex; QQueue queue; - enum {IDLE, AWAITING_ACK, AWAITING_COMPLETED} saveState; + enum { IDLE, AWAITING_ACK, AWAITING_COMPLETED } saveState; void saveNextObject(); QTimer failureTimer; @@ -84,13 +83,11 @@ private: UAVObjectUtilManager *obum; private slots: - //void transactionCompleted(UAVObject *obj, bool success); - void objectPersistenceTransactionCompleted(UAVObject* obj, bool success); - void objectPersistenceUpdated(UAVObject * obj); - void objectPersistenceOperationFailed(); - - + // void transactionCompleted(UAVObject *obj, bool success); + void objectPersistenceTransactionCompleted(UAVObject *obj, bool success); + void objectPersistenceUpdated(UAVObject *obj); + void objectPersistenceOperationFailed(); }; -#endif +#endif // ifndef UAVOBJECTUTILMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.cpp index 0900f9e86..6ec8cda12 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.cpp @@ -11,49 +11,45 @@ * @brief The UAVUObjectUtil GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavobjectutilplugin.h" UAVObjectUtilPlugin::UAVObjectUtilPlugin() -{ -} +{} UAVObjectUtilPlugin::~UAVObjectUtilPlugin() -{ -} +{} void UAVObjectUtilPlugin::extensionsInitialized() -{ -} +{} -bool UAVObjectUtilPlugin::initialize(const QStringList & arguments, QString * errorString) +bool UAVObjectUtilPlugin::initialize(const QStringList & arguments, QString *errorString) { - Q_UNUSED(arguments) - Q_UNUSED(errorString) + Q_UNUSED(arguments) + Q_UNUSED(errorString) - // Create manager and expose object - UAVObjectUtilManager *objUtilMngr = new UAVObjectUtilManager(); - addAutoReleasedObject(objUtilMngr); + // Create manager and expose object + UAVObjectUtilManager * objUtilMngr = new UAVObjectUtilManager(); + addAutoReleasedObject(objUtilMngr); return true; } void UAVObjectUtilPlugin::shutdown() -{ -} +{} Q_EXPORT_PLUGIN(UAVObjectUtilPlugin) diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.h b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.h index 7eef7f9ee..87cbd838a 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.h @@ -11,18 +11,18 @@ * @brief The UAVUObjectUtil GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVOBJECTUTILPLUGIN_H @@ -33,16 +33,15 @@ #include #include "uavobjectutilmanager.h" -class UAVOBJECTUTIL_EXPORT UAVObjectUtilPlugin: public ExtensionSystem::IPlugin -{ +class UAVOBJECTUTIL_EXPORT UAVObjectUtilPlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: - UAVObjectUtilPlugin(); - ~UAVObjectUtilPlugin(); + UAVObjectUtilPlugin(); + ~UAVObjectUtilPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); }; diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp index 0c4036091..2f007eec0 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp @@ -883,9 +883,9 @@ void ConfigTaskWidget::reloadButtonClicked() if (!list) { return; } - ObjectPersistence *objper = dynamic_cast(getObjectManager()->getObject(ObjectPersistence::NAME)); + ObjectPersistence *objper = dynamic_cast(getObjectManager()->getObject(ObjectPersistence::NAME)); timeOut = new QTimer(this); - QEventLoop *eventLoop = new QEventLoop(this); + QEventLoop *eventLoop = new QEventLoop(this); connect(timeOut, SIGNAL(timeout()), eventLoop, SLOT(quit())); connect(objper, SIGNAL(objectUpdated(UAVObject *)), eventLoop, SLOT(quit())); diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h index 461366251..c4c26f0fb 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h @@ -85,14 +85,14 @@ public: enum buttonTypeEnum { none, save_button, apply_button, reload_button, default_button, help_button }; struct uiRelationAutomation { - QString objname; - QString fieldname; - QString element; - QString url; + QString objname; + QString fieldname; + QString element; + QString url; buttonTypeEnum buttonType; QList buttonGroup; - double scale; - bool haslimits; + double scale; + bool haslimits; }; ConfigTaskWidget(QWidget *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.cpp index 7add08628..13c83bdca 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.cpp @@ -38,15 +38,14 @@ Edge::Edge(MixerNode *sourceNode, MixerNode *destNode) { setAcceptedMouseButtons(0); source = sourceNode; - dest = destNode; + dest = destNode; source->addEdge(this); dest->addEdge(this); adjust(); } Edge::~Edge() -{ -} +{} MixerNode *Edge::sourceNode() const { @@ -72,8 +71,9 @@ void Edge::setDestNode(MixerNode *node) void Edge::adjust() { - if (!source || !dest) + if (!source || !dest) { return; + } QLineF line(mapFromItem(source, 0, 0), mapFromItem(dest, 0, 0)); qreal length = line.length(); @@ -83,7 +83,7 @@ void Edge::adjust() if (length > qreal(20.)) { QPointF edgeOffset((line.dx() * 13) / length, (line.dy() * 13) / length); sourcePoint = line.p1() + edgeOffset; - destPoint = line.p2() - edgeOffset; + destPoint = line.p2() - edgeOffset; } else { sourcePoint = destPoint = line.p1(); } @@ -91,29 +91,31 @@ void Edge::adjust() QRectF Edge::boundingRect() const { - if (!source || !dest) + if (!source || !dest) { return QRectF(); + } qreal penWidth = 1; - qreal extra = (penWidth + arrowSize) / 2.0; + qreal extra = (penWidth + arrowSize) / 2.0; return QRectF(sourcePoint, QSizeF(destPoint.x() - sourcePoint.x(), destPoint.y() - sourcePoint.y())) - .normalized() - .adjusted(-extra, -extra, extra, extra); + .normalized() + .adjusted(-extra, -extra, extra, extra); } void Edge::paint(QPainter *painter, const QStyleOptionGraphicsItem *, QWidget *) { - if (!source || !dest) + if (!source || !dest) { return; + } QLineF line(sourcePoint, destPoint); - if (qFuzzyCompare(line.length(), qreal(0.))) + if (qFuzzyCompare(line.length(), qreal(0.))) { return; + } // Draw the line itself painter->setPen(QPen(Qt::black, 1, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); painter->drawLine(line); - } diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.h b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.h index 1c4281c4a..7392ceff9 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.h +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.h @@ -37,8 +37,7 @@ class MixerNode; -class Edge : public QGraphicsItem -{ +class Edge : public QGraphicsItem { public: Edge(MixerNode *sourceNode, MixerNode *destNode); ~Edge(); @@ -52,12 +51,15 @@ public: void adjust(); enum { Type = UserType + 12 }; - int type() const { return Type; } - + int type() const + { + return Type; + } + protected: QRectF boundingRect() const; void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget); - + private: MixerNode *source, *dest; @@ -67,4 +69,3 @@ private: }; #endif // MIXERCURVELINE_H - diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp index 2ad7d4b95..2a2dd2e1d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp @@ -43,14 +43,14 @@ MixerNode::MixerNode(MixerCurveWidget *graphWidget) setFlag(ItemSendsGeometryChanges); setCacheMode(DeviceCoordinateCache); setZValue(-1); - vertical = false; - drawNode = true; - drawText = true; + vertical = false; + drawNode = true; + drawText = true; - positiveColor = "#609FF2"; //blueish? - neutralColor = "#14CE24"; //greenish? - negativeColor = "#EF5F5F"; //redish? - disabledColor = "#dddddd"; + positiveColor = "#609FF2"; // blueish? + neutralColor = "#14CE24"; // greenish? + negativeColor = "#EF5F5F"; // redish? + disabledColor = "#dddddd"; disabledTextColor = "#aaaaaa"; } @@ -74,6 +74,7 @@ QRectF MixerNode::boundingRect() const QPainterPath MixerNode::shape() const { QPainterPath path; + path.addEllipse(boundingRect()); return path; } @@ -81,6 +82,7 @@ QPainterPath MixerNode::shape() const void MixerNode::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *) { QString text = QString().sprintf("%.2f", value()); + painter->setFont(graph->font()); if (drawNode) { QRadialGradient gradient(-3, -3, 10); @@ -88,11 +90,9 @@ void MixerNode::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QColor color; if (value() < 0) { color = negativeColor; - } - else if (value() == 0) { + } else if (value() == 0) { color = neutralColor; - } - else { + } else { color = positiveColor; } @@ -117,24 +117,27 @@ void MixerNode::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, } if (drawText) { - if(graph->isEnabled()) { + if (graph->isEnabled()) { painter->setPen(QPen(drawNode ? Qt::white : Qt::black, 0)); } else { painter->setPen(QPen(disabledTextColor)); } - painter->drawText( (value() < 0) ? -10 : -8, 3, text); + painter->drawText((value() < 0) ? -10 : -8, 3, text); } } -void MixerNode::verticalMove(bool flag){ +void MixerNode::verticalMove(bool flag) +{ vertical = flag; } -double MixerNode::value() { - double h = graph->sceneRect().height(); +double MixerNode::value() +{ + double h = graph->sceneRect().height(); double ratio = (h - pos().y()) / h; - return ((graph->getMax() - graph->getMin()) * ratio ) + graph->getMin(); + + return ((graph->getMax() - graph->getMin()) * ratio) + graph->getMin(); } @@ -144,26 +147,30 @@ QVariant MixerNode::itemChange(GraphicsItemChange change, const QVariant &val) double h = graph->sceneRect().height(); switch (change) { - case ItemPositionChange: { - - if (!vertical) - break; + case ItemPositionChange: + { + if (!vertical) { + break; + } // Force node to move vertically newPos.setX(pos().x()); // Stay inside graph - if (newPos.y() < 0) + if (newPos.y() < 0) { newPos.setY(0); - //qDebug() << h << " - " << newPos.y(); - if (newPos.y() > h) + } + // qDebug() << h << " - " << newPos.y(); + if (newPos.y() > h) { newPos.setY(h); + } - return newPos; + return newPos; } - case ItemPositionHasChanged: { - foreach (Edge *edge, edgeList) - edge->adjust(); + case ItemPositionHasChanged: + { + foreach(Edge * edge, edgeList) + edge->adjust(); update(); @@ -172,7 +179,8 @@ QVariant MixerNode::itemChange(GraphicsItemChange change, const QVariant &val) } default: break; - }; + } + ; return QGraphicsItem::itemChange(change, val); } diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp index 03e2be777..17815aca4 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp @@ -38,7 +38,6 @@ */ MixerCurveWidget::MixerCurveWidget(QWidget *parent) : QGraphicsView(parent) { - // Create a layout, add a QGraphicsView and put the SVG inside. // The Mixer Curve widget looks like this: // |--------------------| @@ -55,13 +54,13 @@ MixerCurveWidget::MixerCurveWidget(QWidget *parent) : QGraphicsView(parent) setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); setRenderHint(QPainter::Antialiasing); - curveMin=0.0; - curveMax=1.0; + curveMin = 0.0; + curveMax = 1.0; setFrameStyle(QFrame::NoFrame); setStyleSheet("background:transparent"); - QGraphicsScene *scene = new QGraphicsScene(this); + QGraphicsScene *scene = new QGraphicsScene(this); QSvgRenderer *renderer = new QSvgRenderer(); plot = new QGraphicsSvgItem(); renderer->load(QString(":/uavobjectwidgetutils/images/curve-bg.svg")); @@ -89,31 +88,31 @@ MixerCurveWidget::~MixerCurveWidget() void MixerCurveWidget::setPositiveColor(QString color) { - for (int i=0; i < nodeList.count(); i++) { - MixerNode* node = nodeList.at(i); + for (int i = 0; i < nodeList.count(); i++) { + MixerNode *node = nodeList.at(i); node->setPositiveColor(color); } } void MixerCurveWidget::setNegativeColor(QString color) { - for (int i=0; i < nodeList.count(); i++) { - MixerNode* node = nodeList.at(i); + for (int i = 0; i < nodeList.count(); i++) { + MixerNode *node = nodeList.at(i); node->setNegativeColor(color); } } /** - Init curve: create a (flat) curve with a specified number of points. + Init curve: create a (flat) curve with a specified number of points. - If a curve exists already, resets it. - Points should be between 0 and 1. - */ -void MixerCurveWidget::initCurve(const QList* points) + If a curve exists already, resets it. + Points should be between 0 and 1. + */ +void MixerCurveWidget::initCurve(const QList *points) { - if (points->length() < 2) + if (points->length() < 2) { return; // We need at least 2 points on a curve! - + } // finally, set node positions setCurve(points); } @@ -122,8 +121,8 @@ void MixerCurveWidget::initNodes(int numPoints) { // First of all, clear any existing list if (nodeList.count()) { - foreach (MixerNode *node, nodeList ) { - foreach(Edge *edge, node->edges()) { + foreach(MixerNode * node, nodeList) { + foreach(Edge * edge, node->edges()) { if (edge->sourceNode() == node) { scene()->removeItem(edge); delete edge; @@ -137,15 +136,14 @@ void MixerCurveWidget::initNodes(int numPoints) } // Create the nodes and edges - MixerNode* prevNode = 0; - for (int i=0; i < numPoints; i++) { - + MixerNode *prevNode = 0; + for (int i = 0; i < numPoints; i++) { MixerNode *node = new MixerNode(this); nodeList.append(node); scene()->addItem(node); - node->setPos(0,0); + node->setPos(0, 0); if (prevNode) { scene()->addItem(new Edge(prevNode, node)); @@ -156,57 +154,57 @@ void MixerCurveWidget::initNodes(int numPoints) } /** - Returns the current curve settings - */ -QList MixerCurveWidget::getCurve() { - + Returns the current curve settings + */ +QList MixerCurveWidget::getCurve() +{ QList list; - foreach(MixerNode *node, nodeList) { + foreach(MixerNode * node, nodeList) { list.append(node->value()); } return list; } /** - Sets a linear graph - */ + Sets a linear graph + */ void MixerCurveWidget::initLinearCurve(int numPoints, double maxValue, double minValue) { double range = maxValue - minValue; // setRange(minValue, maxValue); QList points; - for (double i=0; i < (double)numPoints; i++) { - double val = (range * ( i / (double)(numPoints-1) ) ) + minValue; + for (double i = 0; i < (double)numPoints; i++) { + double val = (range * (i / (double)(numPoints - 1))) + minValue; points.append(val); } initCurve(&points); } /** - Setd the current curve settings - */ -void MixerCurveWidget::setCurve(const QList* points) + Setd the current curve settings + */ +void MixerCurveWidget::setCurve(const QList *points) { curveUpdating = true; int ptCnt = points->count(); - if (nodeList.count() != ptCnt) + if (nodeList.count() != ptCnt) { initNodes(ptCnt); + } double range = curveMax - curveMin; - qreal w = plot->boundingRect().width()/(ptCnt-1); + qreal w = plot->boundingRect().width() / (ptCnt - 1); qreal h = plot->boundingRect().height(); - for (int i=0; iat(i) < curveMin) ? curveMin : (points->at(i) > curveMax) ? curveMax : points->at(i); val += range; val -= (curveMin + range); val /= range; - MixerNode* node = nodeList.at(i); - node->setPos(w*i, h - (val*h)); + MixerNode *node = nodeList.at(i); + node->setPos(w * i, h - (val * h)); node->verticalMove(true); node->update(); @@ -227,22 +225,23 @@ void MixerCurveWidget::showEvent(QShowEvent *event) // the result is usually a ahrsbargraph that is way too small. QRectF rect = plot->boundingRect(); - fitInView(rect.adjusted(-15,-15,15,15), Qt::KeepAspectRatio); + fitInView(rect.adjusted(-15, -15, 15, 15), Qt::KeepAspectRatio); } -void MixerCurveWidget::resizeEvent(QResizeEvent* event) +void MixerCurveWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); QRectF rect = plot->boundingRect(); - fitInView(rect.adjusted(-15,-15,15,15), Qt::KeepAspectRatio); + fitInView(rect.adjusted(-15, -15, 15, 15), Qt::KeepAspectRatio); } void MixerCurveWidget::changeEvent(QEvent *event) { QGraphicsView::changeEvent(event); - if(event->type() == QEvent::EnabledChange) { - foreach (MixerNode *node, nodeList) { + + if (event->type() == QEvent::EnabledChange) { + foreach(MixerNode * node, nodeList) { node->update(); } } @@ -259,16 +258,18 @@ void MixerCurveWidget::itemMoved(double itemValue) void MixerCurveWidget::setMin(double value) { - if (curveMin != value) + if (curveMin != value) { emit curveMinChanged(value); + } curveMin = value; } void MixerCurveWidget::setMax(double value) { - if (curveMax != value) + if (curveMax != value) { emit curveMaxChanged(value); + } curveMax = value; } @@ -287,4 +288,3 @@ double MixerCurveWidget::setRange(double min, double max) curveMax = max; return curveMax - curveMin; } - diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/popupwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/popupwidget.cpp index 94190cd09..727e1b0e3 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/popupwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/popupwidget.cpp @@ -4,15 +4,15 @@ PopupWidget::PopupWidget(QWidget *parent) : QDialog(parent) -{ +{ m_widget = 0; - QVBoxLayout* mainLayout = new QVBoxLayout(); + QVBoxLayout *mainLayout = new QVBoxLayout(); m_layout = new QHBoxLayout(); mainLayout->addLayout(m_layout); - QHBoxLayout* buttonLayout = new QHBoxLayout(); + QHBoxLayout *buttonLayout = new QHBoxLayout(); m_closeButton = new QPushButton(tr("Close")); buttonLayout->addWidget(m_closeButton); @@ -21,24 +21,24 @@ PopupWidget::PopupWidget(QWidget *parent) : setLayout(mainLayout); - connect(m_closeButton,SIGNAL(clicked()), this, SLOT(close())); - connect(this, SIGNAL(accepted()),this,SLOT(close())); - connect(this,SIGNAL(rejected()), this, SLOT(close())); + connect(m_closeButton, SIGNAL(clicked()), this, SLOT(close())); + connect(this, SIGNAL(accepted()), this, SLOT(close())); + connect(this, SIGNAL(rejected()), this, SLOT(close())); } -void PopupWidget::popUp(QWidget* widget) +void PopupWidget::popUp(QWidget *widget) { setWidget(widget); exec(); } -void PopupWidget::setWidget(QWidget* widget) +void PopupWidget::setWidget(QWidget *widget) { m_widget = widget; m_widgetParent = widget->parentWidget(); // save the current width,height so we can restore when closed - m_widgetWidth = m_widget->width(); + m_widgetWidth = m_widget->width(); m_widgetHeight = m_widget->height(); // double the size of the widget for the dialog @@ -63,8 +63,7 @@ void PopupWidget::done(int result) void PopupWidget::closePopup() { if (m_widget && m_widgetParent) { - if(QGroupBox * w =qobject_cast(m_widgetParent)) - { + if (QGroupBox * w = qobject_cast(m_widgetParent)) { m_widget->resize(m_widgetWidth, m_widgetHeight); w->layout()->addWidget(m_widget); } diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.cpp index 5bc29994b..20f26e7ed 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.cpp @@ -27,149 +27,147 @@ #include "smartsavebutton.h" smartSaveButton::smartSaveButton() -{ - -} +{} void smartSaveButton::addButtons(QPushButton *save, QPushButton *apply) { - buttonList.insert(save,save_button); - buttonList.insert(apply,apply_button); - connect(save,SIGNAL(clicked()),this,SLOT(processClick())); - connect(apply,SIGNAL(clicked()),this,SLOT(processClick())); + buttonList.insert(save, save_button); + buttonList.insert(apply, apply_button); + connect(save, SIGNAL(clicked()), this, SLOT(processClick())); + connect(apply, SIGNAL(clicked()), this, SLOT(processClick())); } void smartSaveButton::addApplyButton(QPushButton *apply) { - buttonList.insert(apply,apply_button); - connect(apply,SIGNAL(clicked()),this,SLOT(processClick())); + buttonList.insert(apply, apply_button); + connect(apply, SIGNAL(clicked()), this, SLOT(processClick())); } void smartSaveButton::addSaveButton(QPushButton *save) { - buttonList.insert(save,save_button); - connect(save,SIGNAL(clicked()),this,SLOT(processClick())); + buttonList.insert(save, save_button); + connect(save, SIGNAL(clicked()), this, SLOT(processClick())); } void smartSaveButton::processClick() { emit beginOp(); - bool save=false; - QPushButton *button=qobject_cast(sender()); - if(!button) - return; - if(buttonList.value(button)==save_button) - save=true; - processOperation(button,save); + bool save = false; + QPushButton *button = qobject_cast(sender()); + if (!button) { + return; + } + if (buttonList.value(button) == save_button) { + save = true; + } + processOperation(button, save); } -void smartSaveButton::processOperation(QPushButton * button,bool save) +void smartSaveButton::processOperation(QPushButton *button, bool save) { emit preProcessOperations(); - if(button) - { + + if (button) { button->setEnabled(false); button->setIcon(QIcon(":/uploader/images/system-run.svg")); } QTimer timer; timer.setSingleShot(true); - bool error=false; + bool error = false; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectUtilManager* utilMngr = pm->getObject(); - foreach(UAVDataObject * obj,objects) - { - UAVObject::Metadata mdata= obj->getMetadata(); - if(UAVObject::GetGcsAccess(mdata)==UAVObject::ACCESS_READONLY) + UAVObjectUtilManager *utilMngr = pm->getObject(); + foreach(UAVDataObject * obj, objects) { + UAVObject::Metadata mdata = obj->getMetadata(); + + if (UAVObject::GetGcsAccess(mdata) == UAVObject::ACCESS_READONLY) { continue; - up_result=false; - current_object=obj; - for(int i=0;i<3;++i) - { - qDebug()<<"SMARTSAVEBUTTON"<<"Upload try number"<getName(); - connect(obj,SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(transaction_finished(UAVObject*, bool))); - connect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); + } + up_result = false; + current_object = obj; + for (int i = 0; i < 3; ++i) { + qDebug() << "SMARTSAVEBUTTON" << "Upload try number" << i << "Object" << obj->getName(); + connect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transaction_finished(UAVObject *, bool))); + connect(&timer, SIGNAL(timeout()), &loop, SLOT(quit())); obj->updated(); timer.start(3000); - //qDebug()<<"begin loop"; + // qDebug()<<"begin loop"; loop.exec(); - if(timer.isActive()) - { - qDebug()<<"SMARTSAVEBUTTON"<<"Upload did not timeout"<getName(); + if (timer.isActive()) { + qDebug() << "SMARTSAVEBUTTON" << "Upload did not timeout" << i << "Object" << obj->getName(); + } else { + qDebug() << "SMARTSAVEBUTTON" << "Upload TIMEOUT" << i << "Object" << obj->getName(); } - else - qDebug()<<"SMARTSAVEBUTTON"<<"Upload TIMEOUT"<getName(); timer.stop(); - disconnect(obj,SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(transaction_finished(UAVObject*, bool))); - disconnect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); - if(up_result) + disconnect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transaction_finished(UAVObject *, bool))); + disconnect(&timer, SIGNAL(timeout()), &loop, SLOT(quit())); + if (up_result) { break; + } } - if(up_result==false) - { - qDebug()<<"SMARTSAVEBUTTON"<<"Object upload error:"<getName(); - error=true; + if (up_result == false) { + qDebug() << "SMARTSAVEBUTTON" << "Object upload error:" << obj->getName(); + error = true; continue; } - sv_result=false; - current_objectID=obj->getObjID(); - if(save && (obj->isSettings())) - { - for(int i=0;i<3;++i) - { - qDebug()<<"SMARTSAVEBUTTON"<<"Save try number"<getName(); - connect(utilMngr,SIGNAL(saveCompleted(int,bool)),this,SLOT(saving_finished(int,bool))); - connect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); + sv_result = false; + current_objectID = obj->getObjID(); + if (save && (obj->isSettings())) { + for (int i = 0; i < 3; ++i) { + qDebug() << "SMARTSAVEBUTTON" << "Save try number" << i << "Object" << obj->getName(); + connect(utilMngr, SIGNAL(saveCompleted(int, bool)), this, SLOT(saving_finished(int, bool))); + connect(&timer, SIGNAL(timeout()), &loop, SLOT(quit())); utilMngr->saveObjectToSD(obj); timer.start(3000); loop.exec(); - if(timer.isActive()) - { - qDebug()<<"SMARTSAVEBUTTON"<<"Saving did not timeout"<getName(); + if (timer.isActive()) { + qDebug() << "SMARTSAVEBUTTON" << "Saving did not timeout" << i << "Object" << obj->getName(); + } else { + qDebug() << "SMARTSAVEBUTTON" << "Saving TIMEOUT" << i << "Object" << obj->getName(); } - else - qDebug()<<"SMARTSAVEBUTTON"<<"Saving TIMEOUT"<getName(); timer.stop(); - disconnect(utilMngr,SIGNAL(saveCompleted(int,bool)),this,SLOT(saving_finished(int,bool))); - disconnect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); - if(sv_result) + disconnect(utilMngr, SIGNAL(saveCompleted(int, bool)), this, SLOT(saving_finished(int, bool))); + disconnect(&timer, SIGNAL(timeout()), &loop, SLOT(quit())); + if (sv_result) { break; + } } - if(sv_result==false) - { - qDebug()<<"SMARTSAVEBUTTON"<<"failed to save:"<getName(); - error=true; + if (sv_result == false) { + qDebug() << "SMARTSAVEBUTTON" << "failed to save:" << obj->getName(); + error = true; } } } - if(button) + if (button) { button->setEnabled(true); - if(!error) - { - if(button) - button->setIcon(QIcon(":/uploader/images/dialog-apply.svg")); - emit saveSuccessfull(); } - else - { - if(button) + if (!error) { + if (button) { + button->setIcon(QIcon(":/uploader/images/dialog-apply.svg")); + } + emit saveSuccessfull(); + } else { + if (button) { button->setIcon(QIcon(":/uploader/images/process-stop.svg")); + } } emit endOp(); } void smartSaveButton::setObjects(QList list) { - objects=list; + objects = list; } -void smartSaveButton::addObject(UAVDataObject * obj) +void smartSaveButton::addObject(UAVDataObject *obj) { Q_ASSERT(obj); - if(!objects.contains(obj)) + if (!objects.contains(obj)) { objects.append(obj); + } } -void smartSaveButton::removeObject(UAVDataObject * obj) +void smartSaveButton::removeObject(UAVDataObject *obj) { - if(objects.contains(obj)) + if (objects.contains(obj)) { objects.removeAll(obj); + } } void smartSaveButton::removeAllObjects() { @@ -179,45 +177,41 @@ void smartSaveButton::clearObjects() { objects.clear(); } -void smartSaveButton::transaction_finished(UAVObject* obj, bool result) +void smartSaveButton::transaction_finished(UAVObject *obj, bool result) { - if(current_object==obj) - { - up_result=result; + if (current_object == obj) { + up_result = result; loop.quit(); } } void smartSaveButton::saving_finished(int id, bool result) { - if(id==current_objectID) - { - sv_result=result; - //qDebug()<<"saving_finished result="< #include -class UAVOBJECTWIDGETUTILS_EXPORT UAVObjectWidgetUtilsPlugin: public ExtensionSystem::IPlugin -{ +class UAVOBJECTWIDGETUTILS_EXPORT UAVObjectWidgetUtilsPlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -40,7 +39,7 @@ public: ~UAVObjectWidgetUtilsPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); }; diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.cpp b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.cpp index eb45634ff..6a35d30cb 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.cpp +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.cpp @@ -26,28 +26,27 @@ */ #include "importsummary.h" -ImportSummaryDialog::ImportSummaryDialog( QWidget *parent) : +ImportSummaryDialog::ImportSummaryDialog(QWidget *parent) : QDialog(parent), ui(new Ui::ImportSummaryDialog) { - ui->setupUi(this); - setWindowTitle(tr("Import Summary")); + ui->setupUi(this); + setWindowTitle(tr("Import Summary")); - ui->importSummaryList->setColumnCount(3); - ui->importSummaryList->setRowCount(0); - QStringList header; - header.append("Save"); - header.append("Name"); - header.append("Status"); - ui->importSummaryList->setHorizontalHeaderLabels(header); - ui->progressBar->setValue(0); + ui->importSummaryList->setColumnCount(3); + ui->importSummaryList->setRowCount(0); + QStringList header; + header.append("Save"); + header.append("Name"); + header.append("Status"); + ui->importSummaryList->setHorizontalHeaderLabels(header); + ui->progressBar->setValue(0); - connect( ui->closeButton, SIGNAL(clicked()), this, SLOT(close())); - connect(ui->saveToFlash, SIGNAL(clicked()), this, SLOT(doTheSaving())); - - // Connect the help button - connect(ui->helpButton, SIGNAL(clicked()), this, SLOT(openHelp())); + connect(ui->closeButton, SIGNAL(clicked()), this, SLOT(close())); + connect(ui->saveToFlash, SIGNAL(clicked()), this, SLOT(doTheSaving())); + // Connect the help button + connect(ui->helpButton, SIGNAL(clicked()), this, SLOT(openHelp())); } ImportSummaryDialog::~ImportSummaryDialog() @@ -56,30 +55,30 @@ ImportSummaryDialog::~ImportSummaryDialog() } /* - Open the right page on the wiki - */ + Open the right page on the wiki + */ void ImportSummaryDialog::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/UAV+Settings+import-export", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/display/Doc/UAV+Settings+import-export", QUrl::StrictMode)); } /* - Adds a new line about a UAVObject along with its status - (whether it got saved OK or not) - */ + Adds a new line about a UAVObject along with its status + (whether it got saved OK or not) + */ void ImportSummaryDialog::addLine(QString uavObjectName, QString text, bool status) { - ui->importSummaryList->setRowCount(ui->importSummaryList->rowCount()+1); - int row = ui->importSummaryList->rowCount()-1; - ui->importSummaryList->setCellWidget(row,0,new QCheckBox(ui->importSummaryList)); + ui->importSummaryList->setRowCount(ui->importSummaryList->rowCount() + 1); + int row = ui->importSummaryList->rowCount() - 1; + ui->importSummaryList->setCellWidget(row, 0, new QCheckBox(ui->importSummaryList)); QTableWidgetItem *objName = new QTableWidgetItem(uavObjectName); ui->importSummaryList->setItem(row, 1, objName); - QCheckBox *box = dynamic_cast(ui->importSummaryList->cellWidget(row,0)); - ui->importSummaryList->setItem(row,2,new QTableWidgetItem(text)); + QCheckBox *box = dynamic_cast(ui->importSummaryList->cellWidget(row, 0)); + ui->importSummaryList->setItem(row, 2, new QTableWidgetItem(text)); - //Disable editability and selectability in table elements - ui->importSummaryList->item(row,1)->setFlags(!Qt::ItemIsEditable); - ui->importSummaryList->item(row,2)->setFlags(!Qt::ItemIsEditable); + // Disable editability and selectability in table elements + ui->importSummaryList->item(row, 1)->setFlags(!Qt::ItemIsEditable); + ui->importSummaryList->item(row, 2)->setFlags(!Qt::ItemIsEditable); if (status) { box->setChecked(true); @@ -88,36 +87,38 @@ void ImportSummaryDialog::addLine(QString uavObjectName, QString text, bool stat box->setEnabled(false); } - this->repaint(); - this->showEvent(NULL); + this->repaint(); + this->showEvent(NULL); } /* - Saves every checked UAVObjet in the list to Flash - */ + Saves every checked UAVObjet in the list to Flash + */ void ImportSummaryDialog::doTheSaving() { - int itemCount=0; + int itemCount = 0; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVObjectUtilManager *utilManager = pm->getObject(); - connect(utilManager, SIGNAL(saveCompleted(int,bool)), this, SLOT(updateSaveCompletion())); + UAVObjectUtilManager *utilManager = pm->getObject(); - for(int i=0; i < ui->importSummaryList->rowCount(); i++) { - QCheckBox *box = dynamic_cast(ui->importSummaryList->cellWidget(i,0)); + connect(utilManager, SIGNAL(saveCompleted(int, bool)), this, SLOT(updateSaveCompletion())); + + for (int i = 0; i < ui->importSummaryList->rowCount(); i++) { + QCheckBox *box = dynamic_cast(ui->importSummaryList->cellWidget(i, 0)); if (box->isChecked()) { - ++itemCount; + ++itemCount; } } - if(itemCount==0) + if (itemCount == 0) { return; - ui->progressBar->setMaximum(itemCount+1); + } + ui->progressBar->setMaximum(itemCount + 1); ui->progressBar->setValue(1); - for(int i=0; i < ui->importSummaryList->rowCount(); i++) { - QString uavObjectName = ui->importSummaryList->item(i,1)->text(); - QCheckBox *box = dynamic_cast(ui->importSummaryList->cellWidget(i,0)); + for (int i = 0; i < ui->importSummaryList->rowCount(); i++) { + QString uavObjectName = ui->importSummaryList->item(i, 1)->text(); + QCheckBox *box = dynamic_cast(ui->importSummaryList->cellWidget(i, 0)); if (box->isChecked()) { - UAVObject* obj = objManager->getObject(uavObjectName); + UAVObject *obj = objManager->getObject(uavObjectName); utilManager->saveObjectToSD(obj); this->repaint(); } @@ -125,15 +126,13 @@ void ImportSummaryDialog::doTheSaving() ui->saveToFlash->setEnabled(false); ui->closeButton->setEnabled(false); - } void ImportSummaryDialog::updateSaveCompletion() { - ui->progressBar->setValue(ui->progressBar->value()+1); - if(ui->progressBar->value()==ui->progressBar->maximum()) - { + ui->progressBar->setValue(ui->progressBar->value() + 1); + if (ui->progressBar->value() == ui->progressBar->maximum()) { ui->saveToFlash->setEnabled(true); ui->closeButton->setEnabled(true); } @@ -142,6 +141,7 @@ void ImportSummaryDialog::updateSaveCompletion() void ImportSummaryDialog::changeEvent(QEvent *e) { QDialog::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: ui->retranslateUi(this); @@ -155,8 +155,7 @@ void ImportSummaryDialog::showEvent(QShowEvent *event) { Q_UNUSED(event) ui->importSummaryList->resizeColumnsToContents(); - int width = ui->importSummaryList->width()-(ui->importSummaryList->columnWidth(0)+ - ui->importSummaryList->columnWidth(2)); - ui->importSummaryList->setColumnWidth(1,width-15); + int width = ui->importSummaryList->width() - (ui->importSummaryList->columnWidth(0) + + ui->importSummaryList->columnWidth(2)); + ui->importSummaryList->setColumnWidth(1, width - 15); } - diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.h b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.h index ab88eb835..524897c5b 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.h +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.h @@ -38,17 +38,15 @@ #include "uavobjectutil/uavobjectutilmanager.h" - namespace Ui { - class ImportSummaryDialog; +class ImportSummaryDialog; } -class ImportSummaryDialog : public QDialog -{ +class ImportSummaryDialog : public QDialog { Q_OBJECT public: - ImportSummaryDialog(QWidget *parent=0); + ImportSummaryDialog(QWidget *parent = 0); ~ImportSummaryDialog(); void addLine(QString objectName, QString text, bool status); @@ -65,7 +63,6 @@ public slots: private slots: void doTheSaving(); void openHelp(); - }; #endif // IMPORTSUMMARY_H diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.cpp b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.cpp index acfcc7076..6f20de57c 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.cpp +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.cpp @@ -10,25 +10,25 @@ * @brief UAVSettings Import/Export Plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavsettingsimportexport.h" -#include -#include +#include +#include #include #include #include "importsummary.h" @@ -50,18 +50,18 @@ #include #include -UAVSettingsImportExportPlugin::UAVSettingsImportExportPlugin() -{ - // Do nothing -} +UAVSettingsImportExportPlugin::UAVSettingsImportExportPlugin() +{ + // Do nothing +} -UAVSettingsImportExportPlugin::~UAVSettingsImportExportPlugin() -{ - // Do nothing -} +UAVSettingsImportExportPlugin::~UAVSettingsImportExportPlugin() +{ + // Do nothing +} -bool UAVSettingsImportExportPlugin::initialize(const QStringList& args, QString *errMsg) -{ +bool UAVSettingsImportExportPlugin::initialize(const QStringList & args, QString *errMsg) +{ Q_UNUSED(args); Q_UNUSED(errMsg); mf = new UAVSettingsImportExportFactory(this); @@ -70,12 +70,9 @@ bool UAVSettingsImportExportPlugin::initialize(const QStringList& args, QString } void UAVSettingsImportExportPlugin::shutdown() -{ - // Do nothing +{ + // Do nothing } void UAVSettingsImportExportPlugin::extensionsInitialized() -{ -} +{} Q_EXPORT_PLUGIN(UAVSettingsImportExportPlugin) - - diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.h b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.h index 3cc77a9fc..685638c30 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.h +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.h @@ -10,44 +10,40 @@ * @brief UAVSettings Import/Export Plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef UAVSETTINGSIMPORTEXPORT_H -#define UAVSETTINGSIMPORTEXPORT_H +#ifndef UAVSETTINGSIMPORTEXPORT_H +#define UAVSETTINGSIMPORTEXPORT_H -#include +#include #include "uavobjectutil/uavobjectutilmanager.h" #include "uavsettingsimportexport_global.h" #include "../../gcs_version_info.h" #include "uavsettingsimportexportfactory.h" -class UAVSETTINGSIMPORTEXPORT_EXPORT UAVSettingsImportExportPlugin : public ExtensionSystem::IPlugin -{ +class UAVSETTINGSIMPORTEXPORT_EXPORT UAVSettingsImportExportPlugin : public ExtensionSystem::IPlugin { Q_OBJECT -public: - UAVSettingsImportExportPlugin(); - ~UAVSettingsImportExportPlugin(); +public: + UAVSettingsImportExportPlugin(); + ~UAVSettingsImportExportPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - UAVSettingsImportExportFactory *mf; - - - -}; + UAVSettingsImportExportFactory *mf; +}; #endif // UAVSETTINGSIMPORTEXPORT_H diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.cpp b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.cpp index 01557a4a1..d783d6c2c 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.cpp +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.cpp @@ -52,19 +52,19 @@ UAVSettingsImportExportFactory::~UAVSettingsImportExportFactory() { - // Do nothing + // Do nothing } -UAVSettingsImportExportFactory::UAVSettingsImportExportFactory(QObject *parent):QObject(parent) +UAVSettingsImportExportFactory::UAVSettingsImportExportFactory(QObject *parent) : QObject(parent) { + // Add Menu entry + Core::ActionManager *am = Core::ICore::instance()->actionManager(); + Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_FILE); + Core::Command *cmd = am->registerAction(new QAction(this), + "UAVSettingsImportExportPlugin.UAVSettingsExport", + QList() << + Core::Constants::C_GLOBAL_ID); - // Add Menu entry - Core::ActionManager *am = Core::ICore::instance()->actionManager(); - Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_FILE); - Core::Command *cmd = am->registerAction(new QAction(this), - "UAVSettingsImportExportPlugin.UAVSettingsExport", - QList() << - Core::Constants::C_GLOBAL_ID); cmd->setDefaultKeySequence(QKeySequence("Ctrl+E")); cmd->action()->setText(tr("Export UAV Settings...")); ac->addAction(cmd, Core::Constants::G_FILE_SAVE); @@ -79,7 +79,7 @@ UAVSettingsImportExportFactory::UAVSettingsImportExportFactory(QObject *parent): ac->addAction(cmd, Core::Constants::G_FILE_SAVE); connect(cmd->action(), SIGNAL(triggered(bool)), this, SLOT(importUAVSettings())); - ac = am->actionContainer(Core::Constants::M_HELP); + ac = am->actionContainer(Core::Constants::M_HELP); cmd = am->registerAction(new QAction(this), "UAVSettingsImportExportPlugin.UAVDataExport", QList() << @@ -95,6 +95,7 @@ void UAVSettingsImportExportFactory::importUAVSettings() // ask for file name QString fileName; QString filters = tr("UAVObjects XML files (*.uav);; XML files (*.xml)"); + fileName = QFileDialog::getOpenFileName(0, tr("Import UAV Settings"), "", filters); if (fileName.isEmpty()) { return; @@ -103,7 +104,7 @@ void UAVSettingsImportExportFactory::importUAVSettings() // Now open the file QFile file(fileName); QDomDocument doc("UAVObjects"); - file.open(QFile::ReadOnly|QFile::Text); + file.open(QFile::ReadOnly | QFile::Text); if (!doc.setContent(file.readAll())) { QMessageBox msgBox; msgBox.setText(tr("File Parsing Failed.")); @@ -116,7 +117,7 @@ void UAVSettingsImportExportFactory::importUAVSettings() // find the root of settings subtree emit importAboutToBegin(); - qDebug()<<"Import about to begin"; + qDebug() << "Import about to begin"; QDomElement root = doc.documentElement(); if (root.tagName() == "uavobjects") { @@ -133,7 +134,7 @@ void UAVSettingsImportExportFactory::importUAVSettings() // We are now ok: setup the import summary dialog & update it as we // go along. - ImportSummaryDialog swui((QWidget*)Core::ICore::instance()->mainWindow()); + ImportSummaryDialog swui((QWidget *)Core::ICore::instance()->mainWindow()); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); @@ -143,24 +144,23 @@ void UAVSettingsImportExportFactory::importUAVSettings() while (!node.isNull()) { QDomElement e = node.toElement(); if (e.tagName() == "object") { - - // - Read each object - QString uavObjectName = e.attribute("name"); - uint uavObjectID = e.attribute("id").toUInt(NULL,16); + // - Read each object + QString uavObjectName = e.attribute("name"); + uint uavObjectID = e.attribute("id").toUInt(NULL, 16); // Sanity Check: - UAVObject *obj = objManager->getObject(uavObjectName); + UAVObject *obj = objManager->getObject(uavObjectName); if (obj == NULL) { // This object is unknown! qDebug() << "Object unknown:" << uavObjectName << uavObjectID; swui.addLine(uavObjectName, "Error (Object unknown)", false); } else { - // - Update each field - // - Issue and "updated" command - bool error = false; - bool setError = false; + // - Update each field + // - Issue and "updated" command + bool error = false; + bool setError = false; QDomNode field = node.firstChild(); - while(!field.isNull()) { + while (!field.isNull()) { QDomElement f = field.toElement(); if (f.tagName() == "field") { UAVObjectField *uavfield = obj->getField(f.attribute("name")); @@ -177,12 +177,12 @@ void UAVSettingsImportExportFactory::importUAVSettings() // This is an enum: int i = 0; QStringList list = f.attribute("values").split(","); - foreach (QString element, list) { + foreach(QString element, list) { if (false == uavfield->checkValue(element, i)) { qDebug() << "checkValue(list) returned false on: " << uavObjectName << list; setError = true; } else { - uavfield->setValue(element,i); + uavfield->setValue(element, i); } i++; } @@ -223,6 +223,7 @@ QString UAVSettingsImportExportFactory::createXMLDocument(const enum storedData // create an XML root QDomDocument doc("UAVObjects"); QDomElement root = doc.createElement("uavobjects"); + doc.appendChild(root); // add hardware, firmware and GCS version info @@ -230,7 +231,7 @@ QString UAVSettingsImportExportFactory::createXMLDocument(const enum storedData root.appendChild(versionInfo); UAVObjectUtilManager *utilMngr = pm->getObject(); - deviceDescriptorStruct board = utilMngr->getBoardDescriptionStruct(); + deviceDescriptorStruct board = utilMngr->getBoardDescriptionStruct(); QDomElement hw = doc.createElement("hardware"); hw.setAttribute("type", QString().setNum(board.boardType, 16)); @@ -245,11 +246,11 @@ QString UAVSettingsImportExportFactory::createXMLDocument(const enum storedData versionInfo.appendChild(fw); QString gcsRevision = QString::fromLatin1(Core::Constants::GCS_REVISION_STR); - QString gcsGitDate = gcsRevision.mid(gcsRevision.indexOf(" ") + 1, 14); - QString gcsGitHash = gcsRevision.mid(gcsRevision.indexOf(":") + 1, 8); - QString gcsGitTag = gcsRevision.left(gcsRevision.indexOf(":")); + QString gcsGitDate = gcsRevision.mid(gcsRevision.indexOf(" ") + 1, 14); + QString gcsGitHash = gcsRevision.mid(gcsRevision.indexOf(":") + 1, 8); + QString gcsGitTag = gcsRevision.left(gcsRevision.indexOf(":")); - QDomElement gcs = doc.createElement("gcs"); + QDomElement gcs = doc.createElement("gcs"); gcs.setAttribute("date", gcsGitDate); gcs.setAttribute("hash", gcsGitHash); gcs.setAttribute("tag", gcsGitTag); @@ -257,10 +258,9 @@ QString UAVSettingsImportExportFactory::createXMLDocument(const enum storedData // create settings and/or data elements QDomElement settings = doc.createElement("settings"); - QDomElement data = doc.createElement("data"); + QDomElement data = doc.createElement("data"); - switch (what) - { + switch (what) { case Settings: root.appendChild(settings); break; @@ -274,33 +274,33 @@ QString UAVSettingsImportExportFactory::createXMLDocument(const enum storedData } // iterate over settings objects - QList< QList > objList = objManager->getDataObjects(); - foreach (QList list, objList) { - foreach (UAVDataObject *obj, list) { + QList< QList > objList = objManager->getDataObjects(); + foreach(QList list, objList) { + foreach(UAVDataObject * obj, list) { if (((what == Settings) && obj->isSettings()) || - ((what == Data) && !obj->isSettings()) || - (what == Both)) { - + ((what == Data) && !obj->isSettings()) || + (what == Both)) { // add each object to the XML QDomElement o = doc.createElement("object"); o.setAttribute("name", obj->getName()); - o.setAttribute("id", QString("0x")+ QString().setNum(obj->getObjID(),16).toUpper()); + o.setAttribute("id", QString("0x") + QString().setNum(obj->getObjID(), 16).toUpper()); if (fullExport) { QDomElement d = doc.createElement("description"); - QDomText t = doc.createTextNode(obj->getDescription().remove("@Ref ", Qt::CaseInsensitive)); + QDomText t = doc.createTextNode(obj->getDescription().remove("@Ref ", Qt::CaseInsensitive)); d.appendChild(t); o.appendChild(d); } // iterate over fields - QList fieldList = obj->getFields(); + QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { + foreach(UAVObjectField * field, fieldList) { QDomElement f = doc.createElement("field"); // iterate over values QString vals; quint32 nelem = field->getNumElements(); + for (unsigned int n = 0; n < nelem; ++n) { vals.append(QString("%1,").arg(field->getValue(n).toString())); } @@ -320,10 +320,11 @@ QString UAVSettingsImportExportFactory::createXMLDocument(const enum storedData } // append to the settings or data element - if (obj->isSettings()) + if (obj->isSettings()) { settings.appendChild(o); - else + } else { data.appendChild(o); + } } } } @@ -357,7 +358,7 @@ void UAVSettingsImportExportFactory::exportUAVSettings() // save file QFile file(fileName); if (file.open(QIODevice::WriteOnly) && - (file.write(xml.toAscii()) != -1)) { + (file.write(xml.toAscii()) != -1)) { file.close(); } else { QMessageBox::critical(0, @@ -377,11 +378,11 @@ void UAVSettingsImportExportFactory::exportUAVSettings() void UAVSettingsImportExportFactory::exportUAVData() { if (QMessageBox::question(0, tr("Are you sure?"), - tr("This option is only useful for passing your current " - "system data to the technical support staff. " - "Do you really want to export?"), - QMessageBox::Ok | QMessageBox::Cancel, - QMessageBox::Ok) != QMessageBox::Ok) { + tr("This option is only useful for passing your current " + "system data to the technical support staff. " + "Do you really want to export?"), + QMessageBox::Ok | QMessageBox::Cancel, + QMessageBox::Ok) != QMessageBox::Ok) { return; } @@ -408,7 +409,7 @@ void UAVSettingsImportExportFactory::exportUAVData() // save file QFile file(fileName); if (file.open(QIODevice::WriteOnly) && - (file.write(xml.toAscii()) != -1)) { + (file.write(xml.toAscii()) != -1)) { file.close(); } else { QMessageBox::critical(0, diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.h b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.h index aa07d62e1..f49aaf6f0 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.h +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.h @@ -29,8 +29,7 @@ #include "uavsettingsimportexport_global.h" #include "uavobjectutil/uavobjectutilmanager.h" #include "../../gcs_version_info.h" -class UAVSETTINGSIMPORTEXPORT_EXPORT UAVSettingsImportExportFactory : public QObject -{ +class UAVSETTINGSIMPORTEXPORT_EXPORT UAVSettingsImportExportFactory : public QObject { Q_OBJECT public: @@ -48,7 +47,6 @@ private slots: signals: void importAboutToBegin(); void importEnded(); - }; #endif // UAVSETTINGSIMPORTEXPORTFACTORY_H diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp index 80214ae13..bc7cfc6a3 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp @@ -10,18 +10,18 @@ * @brief The UAVTalk protocol plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,22 +37,21 @@ /** * Constructor */ -Telemetry::Telemetry(UAVTalk* utalk, UAVObjectManager* objMngr) +Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr) { - this->utalk = utalk; + this->utalk = utalk; this->objMngr = objMngr; mutex = new QMutex(QMutex::Recursive); // Process all objects in the list - QList< QList > objs = objMngr->getObjects(); - for (int objidx = 0; objidx < objs.length(); ++objidx) - { + QList< QList > objs = objMngr->getObjects(); + for (int objidx = 0; objidx < objs.length(); ++objidx) { registerObject(objs[objidx][0]); // we only need to register one instance per object type } // Listen to new object creations - connect(objMngr, SIGNAL(newObject(UAVObject*)), this, SLOT(newObject(UAVObject*))); - connect(objMngr, SIGNAL(newInstance(UAVObject*)), this, SLOT(newInstance(UAVObject*))); + connect(objMngr, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *))); + connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *))); // Listen to transaction completions - connect(utalk, SIGNAL(transactionCompleted(UAVObject*,bool)), this, SLOT(transactionCompleted(UAVObject*,bool))); + connect(utalk, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool))); // Get GCS stats object gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr); // Setup and start the periodic timer @@ -61,20 +60,21 @@ Telemetry::Telemetry(UAVTalk* utalk, UAVObjectManager* objMngr) connect(updateTimer, SIGNAL(timeout()), this, SLOT(processPeriodicUpdates())); updateTimer->start(1000); // Setup and start the stats timer - txErrors = 0; + txErrors = 0; txRetries = 0; } Telemetry::~Telemetry() { - for (QMap::iterator itr = transMap.begin(); itr != transMap.end(); ++itr) + for (QMap::iterator itr = transMap.begin(); itr != transMap.end(); ++itr) { delete itr.value(); + } } /** * Register a new object for periodic updates (if enabled) */ -void Telemetry::registerObject(UAVObject* obj) +void Telemetry::registerObject(UAVObject *obj) { // Setup object for periodic updates addObject(obj); @@ -86,13 +86,11 @@ void Telemetry::registerObject(UAVObject* obj) /** * Add an object in the list used for periodic updates */ -void Telemetry::addObject(UAVObject* obj) +void Telemetry::addObject(UAVObject *obj) { // Check if object type is already in the list - for (int n = 0; n < objList.length(); ++n) - { - if ( objList[n].obj->getObjID() == obj->getObjID() ) - { + for (int n = 0; n < objList.length(); ++n) { + if (objList[n].obj->getObjID() == obj->getObjID()) { // Object type (not instance!) is already in the list, do nothing return; } @@ -102,21 +100,19 @@ void Telemetry::addObject(UAVObject* obj) ObjectTimeInfo timeInfo; timeInfo.obj = obj; timeInfo.timeToNextUpdateMs = 0; - timeInfo.updatePeriodMs = 0; + timeInfo.updatePeriodMs = 0; objList.append(timeInfo); } /** * Update the object's timers */ -void Telemetry::setUpdatePeriod(UAVObject* obj, qint32 periodMs) +void Telemetry::setUpdatePeriod(UAVObject *obj, qint32 periodMs) { // Find object type (not instance!) and update its period - for (int n = 0; n < objList.length(); ++n) - { - if ( objList[n].obj->getObjID() == obj->getObjID() ) - { - objList[n].updatePeriodMs = periodMs; + for (int n = 0; n < objList.length(); ++n) { + if (objList[n].obj->getObjID() == obj->getObjID()) { + objList[n].updatePeriodMs = periodMs; objList[n].timeToNextUpdateMs = quint32((float)periodMs * (float)qrand() / (float)RAND_MAX); // avoid bunching of updates } } @@ -125,33 +121,27 @@ void Telemetry::setUpdatePeriod(UAVObject* obj, qint32 periodMs) /** * Connect to all instances of an object depending on the event mask specified */ -void Telemetry::connectToObjectInstances(UAVObject* obj, quint32 eventMask) +void Telemetry::connectToObjectInstances(UAVObject *obj, quint32 eventMask) { - QList objs = objMngr->getObjectInstances(obj->getObjID()); - for (int n = 0; n < objs.length(); ++n) - { + QList objs = objMngr->getObjectInstances(obj->getObjID()); + for (int n = 0; n < objs.length(); ++n) { // Disconnect all objs[n]->disconnect(this); // Connect only the selected events - if ( (eventMask&EV_UNPACKED) != 0) - { - connect(objs[n], SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(objectUnpacked(UAVObject*))); + if ((eventMask & EV_UNPACKED) != 0) { + connect(objs[n], SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *))); } - if ( (eventMask&EV_UPDATED) != 0) - { - connect(objs[n], SIGNAL(objectUpdatedAuto(UAVObject*)), this, SLOT(objectUpdatedAuto(UAVObject*))); + if ((eventMask & EV_UPDATED) != 0) { + connect(objs[n], SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *))); } - if ( (eventMask&EV_UPDATED_MANUAL) != 0) - { - connect(objs[n], SIGNAL(objectUpdatedManual(UAVObject*)), this, SLOT(objectUpdatedManual(UAVObject*))); + if ((eventMask & EV_UPDATED_MANUAL) != 0) { + connect(objs[n], SIGNAL(objectUpdatedManual(UAVObject *)), this, SLOT(objectUpdatedManual(UAVObject *))); } - if ( (eventMask&EV_UPDATED_PERIODIC) != 0) - { - connect(objs[n], SIGNAL(objectUpdatedPeriodic(UAVObject*)), this, SLOT(objectUpdatedPeriodic(UAVObject*))); + if ((eventMask & EV_UPDATED_PERIODIC) != 0) { + connect(objs[n], SIGNAL(objectUpdatedPeriodic(UAVObject *)), this, SLOT(objectUpdatedPeriodic(UAVObject *))); } - if ( (eventMask&EV_UPDATE_REQ) != 0) - { - connect(objs[n], SIGNAL(updateRequested(UAVObject*)), this, SLOT(updateRequested(UAVObject*))); + if ((eventMask & EV_UPDATE_REQ) != 0) { + connect(objs[n], SIGNAL(updateRequested(UAVObject *)), this, SLOT(updateRequested(UAVObject *))); } } } @@ -159,68 +149,57 @@ void Telemetry::connectToObjectInstances(UAVObject* obj, quint32 eventMask) /** * Update an object based on its metadata properties */ -void Telemetry::updateObject(UAVObject* obj, quint32 eventType) +void Telemetry::updateObject(UAVObject *obj, quint32 eventType) { // Get metadata - UAVObject::Metadata metadata = obj->getMetadata(); + UAVObject::Metadata metadata = obj->getMetadata(); UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata); // Setup object depending on update mode qint32 eventMask; - if ( updateMode == UAVObject::UPDATEMODE_PERIODIC ) - { + + if (updateMode == UAVObject::UPDATEMODE_PERIODIC) { // Set update period setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod); // Connect signals for all instances eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC; - if( dynamic_cast(obj) != NULL ) - { + if (dynamic_cast(obj) != NULL) { eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } connectToObjectInstances(obj, eventMask); - } - else if ( updateMode == UAVObject::UPDATEMODE_ONCHANGE ) - { + } else if (updateMode == UAVObject::UPDATEMODE_ONCHANGE) { // Set update period setUpdatePeriod(obj, 0); // Connect signals for all instances eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - if( dynamic_cast(obj) != NULL ) - { + if (dynamic_cast(obj) != NULL) { eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } connectToObjectInstances(obj, eventMask); - } - else if ( updateMode == UAVObject::UPDATEMODE_THROTTLED ) - { + } else if (updateMode == UAVObject::UPDATEMODE_THROTTLED) { // If we received a periodic update, we can change back to update on change - if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) { + if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) { // Set update period - if (eventType == EV_NONE) - setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod); + if (eventType == EV_NONE) { + setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod); + } // Connect signals for all instances - eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC; - } - else - { + eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC; + } else { // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates // Connect signals for all instances eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - } - if( dynamic_cast(obj) != NULL ) - { - eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) + } + if (dynamic_cast(obj) != NULL) { + eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } connectToObjectInstances(obj, eventMask); - } - else if ( updateMode == UAVObject::UPDATEMODE_MANUAL ) - { + } else if (updateMode == UAVObject::UPDATEMODE_MANUAL) { // Set update period setUpdatePeriod(obj, 0); // Connect signals for all instances eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ; - if( dynamic_cast(obj) != NULL ) - { + if (dynamic_cast(obj) != NULL) { eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } connectToObjectInstances(obj, eventMask); @@ -230,25 +209,24 @@ void Telemetry::updateObject(UAVObject* obj, quint32 eventType) /** * Called when a transaction is successfully completed (uavtalk event) */ -void Telemetry::transactionCompleted(UAVObject* obj, bool success) +void Telemetry::transactionCompleted(UAVObject *obj, bool success) { // Lookup the transaction in the transaction map. quint32 objId = obj->getObjID(); - QMap::iterator itr = transMap.find(objId); - if ( itr != transMap.end() ) - { - ObjectTransactionInfo *transInfo = itr.value(); + + QMap::iterator itr = transMap.find(objId); + if (itr != transMap.end()) { + ObjectTransactionInfo *transInfo = itr.value(); // Remove this transaction as it's complete. - transInfo->timer->stop(); - transMap.remove(objId); - delete transInfo; + transInfo->timer->stop(); + transMap.remove(objId); + delete transInfo; // Send signal obj->emitTransactionCompleted(success); // Process new object updates from queue processObjectQueue(); - } else - { - qDebug() << "Error: received a transaction completed when did not expect it."; + } else { + qDebug() << "Error: received a transaction completed when did not expect it."; } } @@ -259,24 +237,21 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo) { transInfo->timer->stop(); // Check if more retries are pending - if (transInfo->retriesRemaining > 0) - { + if (transInfo->retriesRemaining > 0) { --transInfo->retriesRemaining; processObjectTransaction(transInfo); ++txRetries; - } - else - { - // Stop the timer. - transInfo->timer->stop(); + } else { + // Stop the timer. + transInfo->timer->stop(); // Terminate transaction utalk->cancelTransaction(transInfo->obj); // Send signal transInfo->obj->emitTransactionCompleted(false); // Remove this transaction as it's complete. - transMap.remove(transInfo->obj->getObjID()); - delete transInfo; - // Process new object updates from queue + transMap.remove(transInfo->obj->getObjID()); + delete transInfo; + // Process new object updates from queue processObjectQueue(); ++txErrors; } @@ -287,60 +262,45 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo) */ void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo) { - // Initiate transaction - if (transInfo->objRequest) - { + if (transInfo->objRequest) { utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances); - } - else - { + } else { utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances); } // Start timer if a response is expected - if ( transInfo->objRequest || transInfo->acked ) - { + if (transInfo->objRequest || transInfo->acked) { transInfo->timer->start(REQ_TIMEOUT_MS); - } - else - { + } else { // Otherwise, remove this transaction as it's complete. - transMap.remove(transInfo->obj->getObjID()); - delete transInfo; + transMap.remove(transInfo->obj->getObjID()); + delete transInfo; } } /** * Process the event received from an object */ -void Telemetry::processObjectUpdates(UAVObject* obj, EventMask event, bool allInstances, bool priority) +void Telemetry::processObjectUpdates(UAVObject *obj, EventMask event, bool allInstances, bool priority) { // Push event into queue ObjectQueueInfo objInfo; - objInfo.obj = obj; + + objInfo.obj = obj; objInfo.event = event; objInfo.allInstances = allInstances; - if (priority) - { - if ( objPriorityQueue.length() < MAX_QUEUE_SIZE ) - { + if (priority) { + if (objPriorityQueue.length() < MAX_QUEUE_SIZE) { objPriorityQueue.enqueue(objInfo); - } - else - { + } else { ++txErrors; obj->emitTransactionCompleted(false); qxtLog->warning(tr("Telemetry: priority event queue is full, event lost (%1)").arg(obj->getName())); } - } - else - { - if ( objQueue.length() < MAX_QUEUE_SIZE ) - { + } else { + if (objQueue.length() < MAX_QUEUE_SIZE) { objQueue.enqueue(objInfo); - } - else - { + } else { ++txErrors; obj->emitTransactionCompleted(false); } @@ -357,53 +317,43 @@ void Telemetry::processObjectQueue() { // Get object information from queue (first the priority and then the regular queue) ObjectQueueInfo objInfo; - if ( !objPriorityQueue.isEmpty() ) - { + + if (!objPriorityQueue.isEmpty()) { objInfo = objPriorityQueue.dequeue(); - } - else if ( !objQueue.isEmpty() ) - { + } else if (!objQueue.isEmpty()) { objInfo = objQueue.dequeue(); - } - else - { + } else { return; } // Check if a connection has been established, only process GCSTelemetryStats updates // (used to establish the connection) GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); - if ( gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED ) - { + if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED) { objQueue.clear(); - if ( objInfo.obj->getObjID() != GCSTelemetryStats::OBJID && objInfo.obj->getObjID() != OPLinkSettings::OBJID && objInfo.obj->getObjID() != ObjectPersistence::OBJID ) - { + if (objInfo.obj->getObjID() != GCSTelemetryStats::OBJID && objInfo.obj->getObjID() != OPLinkSettings::OBJID && objInfo.obj->getObjID() != ObjectPersistence::OBJID) { objInfo.obj->emitTransactionCompleted(false); return; } } // Setup transaction (skip if unpack event) - UAVObject::Metadata metadata = objInfo.obj->getMetadata(); + UAVObject::Metadata metadata = objInfo.obj->getMetadata(); UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata); - if ( ( objInfo.event != EV_UNPACKED ) && ( ( objInfo.event != EV_UPDATED_PERIODIC ) || ( updateMode != UAVObject::UPDATEMODE_THROTTLED ) ) ) - { - QMap::iterator itr = transMap.find(objInfo.obj->getObjID()); - if ( itr != transMap.end() ) { + if ((objInfo.event != EV_UNPACKED) && ((objInfo.event != EV_UPDATED_PERIODIC) || (updateMode != UAVObject::UPDATEMODE_THROTTLED))) { + QMap::iterator itr = transMap.find(objInfo.obj->getObjID()); + if (itr != transMap.end()) { qDebug() << "!!!!!! Making request for an object: " << objInfo.obj->getName() << " for which a request is already in progress!!!!!!"; } - UAVObject::Metadata metadata = objInfo.obj->getMetadata(); + UAVObject::Metadata metadata = objInfo.obj->getMetadata(); ObjectTransactionInfo *transInfo = new ObjectTransactionInfo(this); - transInfo->obj = objInfo.obj; + transInfo->obj = objInfo.obj; transInfo->allInstances = objInfo.allInstances; transInfo->retriesRemaining = MAX_RETRIES; transInfo->acked = UAVObject::GetGcsTelemetryAcked(metadata); - if ( objInfo.event == EV_UPDATED || objInfo.event == EV_UPDATED_MANUAL || objInfo.event == EV_UPDATED_PERIODIC ) - { + if (objInfo.event == EV_UPDATED || objInfo.event == EV_UPDATED_MANUAL || objInfo.event == EV_UPDATED_PERIODIC) { transInfo->objRequest = false; - } - else if ( objInfo.event == EV_UPDATE_REQ ) - { + } else if (objInfo.event == EV_UPDATE_REQ) { transInfo->objRequest = true; } transInfo->telem = this; @@ -413,22 +363,20 @@ void Telemetry::processObjectQueue() } // If this is a metaobject then make necessary telemetry updates - UAVMetaObject* metaobj = dynamic_cast(objInfo.obj); - if ( metaobj != NULL ) - { - updateObject( metaobj->getParentObject(), EV_NONE ); - } - else if ( updateMode != UAVObject::UPDATEMODE_THROTTLED ) - { - updateObject( objInfo.obj, objInfo.event ); + UAVMetaObject *metaobj = dynamic_cast(objInfo.obj); + if (metaobj != NULL) { + updateObject(metaobj->getParentObject(), EV_NONE); + } else if (updateMode != UAVObject::UPDATEMODE_THROTTLED) { + updateObject(objInfo.obj, objInfo.event); } // The fact we received an unpacked event does not mean that // we do not have additional objects still in the queue, // so we have to reschedule queue processing to make sure they are not // stuck: - if ( objInfo.event == EV_UNPACKED ) + if (objInfo.event == EV_UNPACKED) { processObjectQueue(); + } } /** @@ -444,21 +392,18 @@ void Telemetry::processPeriodicUpdates() // Iterate through each object and update its timer, if zero then transmit object. // Also calculate smallest delay to next update (will be used for setting timeToNextUpdateMs) - qint32 minDelay = MAX_UPDATE_PERIOD_MS; + qint32 minDelay = MAX_UPDATE_PERIOD_MS; ObjectTimeInfo *objinfo; qint32 elapsedMs = 0; QTime time; qint32 offset; - for (int n = 0; n < objList.length(); ++n) - { + for (int n = 0; n < objList.length(); ++n) { objinfo = &objList[n]; // If object is configured for periodic updates - if (objinfo->updatePeriodMs > 0) - { + if (objinfo->updatePeriodMs > 0) { objinfo->timeToNextUpdateMs -= timeToNextUpdateMs; // Check if time for the next update - if (objinfo->timeToNextUpdateMs <= 0) - { + if (objinfo->timeToNextUpdateMs <= 0) { // Reset timer offset = (-objinfo->timeToNextUpdateMs) % objinfo->updatePeriodMs; objinfo->timeToNextUpdateMs = objinfo->updatePeriodMs - offset; @@ -470,16 +415,14 @@ void Telemetry::processPeriodicUpdates() timeToNextUpdateMs += elapsedMs; } // Update minimum delay - if (objinfo->timeToNextUpdateMs < minDelay) - { + if (objinfo->timeToNextUpdateMs < minDelay) { minDelay = objinfo->timeToNextUpdateMs; } } } // Check if delay for the next update is too short - if (minDelay < MIN_UPDATE_PERIOD_MS) - { + if (minDelay < MIN_UPDATE_PERIOD_MS) { minDelay = MIN_UPDATE_PERIOD_MS; } @@ -499,15 +442,16 @@ Telemetry::TelemetryStats Telemetry::getStats() // Update stats TelemetryStats stats; - stats.txBytes = utalkStats.txBytes; - stats.rxBytes = utalkStats.rxBytes; + + stats.txBytes = utalkStats.txBytes; + stats.rxBytes = utalkStats.rxBytes; stats.txObjectBytes = utalkStats.txObjectBytes; stats.rxObjectBytes = utalkStats.rxObjectBytes; - stats.rxObjects = utalkStats.rxObjects; - stats.txObjects = utalkStats.txObjects; - stats.txErrors = utalkStats.txErrors + txErrors; - stats.rxErrors = utalkStats.rxErrors; - stats.txRetries = txRetries; + stats.rxObjects = utalkStats.rxObjects; + stats.txObjects = utalkStats.txObjects; + stats.txErrors = utalkStats.txErrors + txErrors; + stats.rxErrors = utalkStats.rxErrors; + stats.txRetries = txRetries; // Done return stats; @@ -516,58 +460,66 @@ Telemetry::TelemetryStats Telemetry::getStats() void Telemetry::resetStats() { QMutexLocker locker(mutex); + utalk->resetStats(); - txErrors = 0; + txErrors = 0; txRetries = 0; } -void Telemetry::objectUpdatedAuto(UAVObject* obj) +void Telemetry::objectUpdatedAuto(UAVObject *obj) { QMutexLocker locker(mutex); + processObjectUpdates(obj, EV_UPDATED, false, true); } -void Telemetry::objectUpdatedManual(UAVObject* obj) +void Telemetry::objectUpdatedManual(UAVObject *obj) { QMutexLocker locker(mutex); + processObjectUpdates(obj, EV_UPDATED_MANUAL, false, true); } -void Telemetry::objectUpdatedPeriodic(UAVObject* obj) +void Telemetry::objectUpdatedPeriodic(UAVObject *obj) { QMutexLocker locker(mutex); + processObjectUpdates(obj, EV_UPDATED_PERIODIC, false, true); } -void Telemetry::objectUnpacked(UAVObject* obj) +void Telemetry::objectUnpacked(UAVObject *obj) { QMutexLocker locker(mutex); + processObjectUpdates(obj, EV_UNPACKED, false, true); } -void Telemetry::updateRequested(UAVObject* obj) +void Telemetry::updateRequested(UAVObject *obj) { QMutexLocker locker(mutex); + processObjectUpdates(obj, EV_UPDATE_REQ, false, true); } -void Telemetry::newObject(UAVObject* obj) +void Telemetry::newObject(UAVObject *obj) { QMutexLocker locker(mutex); + registerObject(obj); } -void Telemetry::newInstance(UAVObject* obj) +void Telemetry::newInstance(UAVObject *obj) { QMutexLocker locker(mutex); + registerObject(obj); } -ObjectTransactionInfo::ObjectTransactionInfo(QObject* parent):QObject(parent) +ObjectTransactionInfo::ObjectTransactionInfo(QObject *parent) : QObject(parent) { obj = 0; - allInstances = false; - objRequest = false; + allInstances = false; + objRequest = false; retriesRemaining = 0; acked = false; telem = 0; @@ -586,6 +538,7 @@ ObjectTransactionInfo::~ObjectTransactionInfo() void ObjectTransactionInfo::timeout() { - if (!telem.isNull()) + if (!telem.isNull()) { telem->transactionTimeout(this); + } } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h index 97d564557..751aeaeae 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h @@ -10,18 +10,18 @@ * @brief The UAVTalk protocol plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,25 +37,24 @@ #include #include -class ObjectTransactionInfo: public QObject { +class ObjectTransactionInfo : public QObject { Q_OBJECT public: - ObjectTransactionInfo(QObject * parent); + ObjectTransactionInfo(QObject *parent); ~ObjectTransactionInfo(); - UAVObject* obj; + UAVObject *obj; bool allInstances; bool objRequest; qint32 retriesRemaining; bool acked; QPointertelem; - QTimer* timer; + QTimer *timer; private slots: void timeout(); }; -class Telemetry: public QObject -{ +class Telemetry : public QObject { Q_OBJECT public: @@ -71,7 +70,7 @@ public: quint32 txRetries; } TelemetryStats; - Telemetry(UAVTalk* utalk, UAVObjectManager* objMngr); + Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr); ~Telemetry(); TelemetryStats getStats(); void resetStats(); @@ -82,7 +81,7 @@ signals: private: // Constants static const int REQ_TIMEOUT_MS = 250; - static const int MAX_RETRIES = 2; + static const int MAX_RETRIES = 2; static const int MAX_UPDATE_PERIOD_MS = 1000; static const int MIN_UPDATE_PERIOD_MS = 1; static const int MAX_QUEUE_SIZE = 20; @@ -92,62 +91,61 @@ private: * Events generated by objects */ typedef enum { - EV_NONE = 0x00, /** No event */ - EV_UNPACKED = 0x01, /** Object data updated by unpacking */ - EV_UPDATED = 0x02, /** Object data updated by changing the data structure */ - EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */ + EV_NONE = 0x00, /** No event */ + EV_UNPACKED = 0x01, /** Object data updated by unpacking */ + EV_UPDATED = 0x02, /** Object data updated by changing the data structure */ + EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */ EV_UPDATED_PERIODIC = 0x8, /** Object update event generated by timer */ - EV_UPDATE_REQ = 0x010 /** Request to update object data */ + EV_UPDATE_REQ = 0x010 /** Request to update object data */ } EventMask; typedef struct { - UAVObject* obj; - qint32 updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */ - qint32 timeToNextUpdateMs; /** Time delay to the next update */ + UAVObject *obj; + qint32 updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */ + qint32 timeToNextUpdateMs; /** Time delay to the next update */ } ObjectTimeInfo; typedef struct { - UAVObject* obj; + UAVObject *obj; EventMask event; bool allInstances; } ObjectQueueInfo; // Variables - UAVObjectManager* objMngr; - UAVTalk* utalk; - GCSTelemetryStats* gcsStatsObj; + UAVObjectManager *objMngr; + UAVTalk *utalk; + GCSTelemetryStats *gcsStatsObj; QList objList; QQueue objQueue; QQueue objPriorityQueue; - QMaptransMap; - QMutex* mutex; - QTimer* updateTimer; - QTimer* statsTimer; + QMaptransMap; + QMutex *mutex; + QTimer *updateTimer; + QTimer *statsTimer; qint32 timeToNextUpdateMs; quint32 txErrors; quint32 txRetries; // Methods - void registerObject(UAVObject* obj); - void addObject(UAVObject* obj); - void setUpdatePeriod(UAVObject* obj, qint32 periodMs); - void connectToObjectInstances(UAVObject* obj, quint32 eventMask); - void updateObject(UAVObject* obj, quint32 eventMask); - void processObjectUpdates(UAVObject* obj, EventMask event, bool allInstances, bool priority); + void registerObject(UAVObject *obj); + void addObject(UAVObject *obj); + void setUpdatePeriod(UAVObject *obj, qint32 periodMs); + void connectToObjectInstances(UAVObject *obj, quint32 eventMask); + void updateObject(UAVObject *obj, quint32 eventMask); + void processObjectUpdates(UAVObject *obj, EventMask event, bool allInstances, bool priority); void processObjectTransaction(ObjectTransactionInfo *transInfo); void processObjectQueue(); private slots: - void objectUpdatedAuto(UAVObject* obj); - void objectUpdatedManual(UAVObject* obj); - void objectUpdatedPeriodic(UAVObject* obj); - void objectUnpacked(UAVObject* obj); - void updateRequested(UAVObject* obj); - void newObject(UAVObject* obj); - void newInstance(UAVObject* obj); + void objectUpdatedAuto(UAVObject *obj); + void objectUpdatedManual(UAVObject *obj); + void objectUpdatedPeriodic(UAVObject *obj); + void objectUnpacked(UAVObject *obj); + void updateRequested(UAVObject *obj); + void newObject(UAVObject *obj); + void newInstance(UAVObject *obj); void processPeriodicUpdates(); - void transactionCompleted(UAVObject* obj, bool success); - + void transactionCompleted(UAVObject *obj, bool success); }; #endif // TELEMETRY_H diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp index 61705fb81..008e3af73 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp @@ -35,17 +35,16 @@ TelemetryManager::TelemetryManager() : { moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread()); // Get UAVObjectManager instance - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); objMngr = pm->getObject(); // connect to start stop signals - connect(this, SIGNAL(myStart()), this, SLOT(onStart()),Qt::QueuedConnection); - connect(this, SIGNAL(myStop()), this, SLOT(onStop()),Qt::QueuedConnection); + connect(this, SIGNAL(myStart()), this, SLOT(onStart()), Qt::QueuedConnection); + connect(this, SIGNAL(myStop()), this, SLOT(onStop()), Qt::QueuedConnection); } TelemetryManager::~TelemetryManager() -{ -} +{} bool TelemetryManager::isConnected() { @@ -54,14 +53,14 @@ bool TelemetryManager::isConnected() void TelemetryManager::start(QIODevice *dev) { - device=dev; + device = dev; emit myStart(); } void TelemetryManager::onStart() { - utalk = new UAVTalk(device, objMngr); - telemetry = new Telemetry(utalk, objMngr); + utalk = new UAVTalk(device, objMngr); + telemetry = new Telemetry(utalk, objMngr); telemetryMon = new TelemetryMonitor(objMngr, telemetry); connect(telemetryMon, SIGNAL(connected()), this, SLOT(onConnect())); connect(telemetryMon, SIGNAL(disconnected()), this, SLOT(onDisconnect())); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h index 7006f1d7e..c4e7301ec 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h @@ -36,8 +36,7 @@ #include #include -class UAVTALK_EXPORT TelemetryManager: public QObject -{ +class UAVTALK_EXPORT TelemetryManager : public QObject { Q_OBJECT public: @@ -61,10 +60,10 @@ private slots: void onStop(); private: - UAVObjectManager* objMngr; - UAVTalk* utalk; - Telemetry* telemetry; - TelemetryMonitor* telemetryMon; + UAVObjectManager *objMngr; + UAVTalk *utalk; + Telemetry *telemetry; + TelemetryMonitor *telemetryMon; QIODevice *device; bool autopilotConnected; }; diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp index 32437f374..6d1a8268e 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp @@ -33,10 +33,10 @@ /** * Constructor */ -TelemetryMonitor::TelemetryMonitor(UAVObjectManager* objMngr, Telemetry* tel) +TelemetryMonitor::TelemetryMonitor(UAVObjectManager *objMngr, Telemetry *tel) { - this->objMngr = objMngr; - this->tel = tel; + this->objMngr = objMngr; + this->tel = tel; this->objPending = NULL; this->connectionTimer = new QTime(); @@ -44,11 +44,11 @@ TelemetryMonitor::TelemetryMonitor(UAVObjectManager* objMngr, Telemetry* tel) mutex = new QMutex(QMutex::Recursive); // Get stats objects - gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr); + gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr); flightStatsObj = FlightTelemetryStats::GetInstance(objMngr); // Listen for flight stats updates - connect(flightStatsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(flightStatsUpdated(UAVObject*))); + connect(flightStatsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(flightStatsUpdated(UAVObject *))); // Start update timer statsTimer = new QTimer(this); @@ -56,14 +56,16 @@ TelemetryMonitor::TelemetryMonitor(UAVObjectManager* objMngr, Telemetry* tel) statsTimer->start(STATS_CONNECT_PERIOD_MS); Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager(); - connect(this,SIGNAL(connected()),cm,SLOT(telemetryConnected())); - connect(this,SIGNAL(disconnected()),cm,SLOT(telemetryDisconnected())); - connect(this,SIGNAL(telemetryUpdated(double,double)),cm,SLOT(telemetryUpdated(double,double))); + connect(this, SIGNAL(connected()), cm, SLOT(telemetryConnected())); + connect(this, SIGNAL(disconnected()), cm, SLOT(telemetryDisconnected())); + connect(this, SIGNAL(telemetryUpdated(double, double)), cm, SLOT(telemetryUpdated(double, double))); } -TelemetryMonitor::~TelemetryMonitor() { +TelemetryMonitor::~TelemetryMonitor() +{ // Before saying goodbye, set the GCS connection status to disconnected too: GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); + gcsStats.Status = GCSTelemetryStats::STATUS_DISCONNECTED; // Set data gcsStatsObj->setData(gcsStats); @@ -77,27 +79,19 @@ void TelemetryMonitor::startRetrievingObjects() // Clear object queue queue.clear(); // Get all objects, add metaobjects, settings and data objects with OnChange update mode to the queue - QList< QList > objs = objMngr->getObjects(); - for (int n = 0; n < objs.length(); ++n) - { - UAVObject* obj = objs[n][0]; - UAVMetaObject* mobj = dynamic_cast(obj); - UAVDataObject* dobj = dynamic_cast(obj); + QList< QList > objs = objMngr->getObjects(); + for (int n = 0; n < objs.length(); ++n) { + UAVObject *obj = objs[n][0]; + UAVMetaObject *mobj = dynamic_cast(obj); + UAVDataObject *dobj = dynamic_cast(obj); UAVObject::Metadata mdata = obj->getMetadata(); - if ( mobj != NULL ) - { + if (mobj != NULL) { queue.enqueue(obj); - } - else if ( dobj != NULL ) - { - if ( dobj->isSettings() ) - { + } else if (dobj != NULL) { + if (dobj->isSettings()) { queue.enqueue(obj); - } - else - { - if ( UAVObject::GetFlightTelemetryUpdateMode(mdata) == UAVObject::UPDATEMODE_ONCHANGE ) - { + } else { + if (UAVObject::GetFlightTelemetryUpdateMode(mdata) == UAVObject::UPDATEMODE_ONCHANGE) { queue.enqueue(obj); } } @@ -105,7 +99,7 @@ void TelemetryMonitor::startRetrievingObjects() } // Start retrieving qxtLog->debug(tr("Starting to retrieve meta and settings objects from the autopilot (%1 objects)") - .arg( queue.length()) ); + .arg(queue.length())); retrieveNextObject(); } @@ -124,17 +118,16 @@ void TelemetryMonitor::stopRetrievingObjects() void TelemetryMonitor::retrieveNextObject() { // If queue is empty return - if ( queue.isEmpty() ) - { + if (queue.isEmpty()) { qxtLog->debug("Object retrieval completed"); emit connected(); return; } // Get next object from the queue - UAVObject* obj = queue.dequeue(); - //qxtLog->trace( tr("Retrieving object: %1").arg(obj->getName()) ); + UAVObject *obj = queue.dequeue(); + // qxtLog->trace( tr("Retrieving object: %1").arg(obj->getName()) ); // Connect to object - connect(obj, SIGNAL(transactionCompleted(UAVObject*,bool)), this, SLOT(transactionCompleted(UAVObject*,bool))); + connect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool))); // Request update obj->requestUpdate(); objPending = obj; @@ -143,7 +136,7 @@ void TelemetryMonitor::retrieveNextObject() /** * Called by the retrieved object when a transaction is completed. */ -void TelemetryMonitor::transactionCompleted(UAVObject* obj, bool success) +void TelemetryMonitor::transactionCompleted(UAVObject *obj, bool success) { Q_UNUSED(success); QMutexLocker locker(mutex); @@ -152,12 +145,9 @@ void TelemetryMonitor::transactionCompleted(UAVObject* obj, bool success) objPending = NULL; // Process next object if telemetry is still available GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); - if ( gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED ) - { + if (gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED) { retrieveNextObject(); - } - else - { + } else { stopRetrievingObjects(); } } @@ -165,7 +155,7 @@ void TelemetryMonitor::transactionCompleted(UAVObject* obj, bool success) /** * Called each time the flight stats object is updated by the autopilot */ -void TelemetryMonitor::flightStatsUpdated(UAVObject* obj) +void TelemetryMonitor::flightStatsUpdated(UAVObject *obj) { Q_UNUSED(obj); QMutexLocker locker(mutex); @@ -173,9 +163,8 @@ void TelemetryMonitor::flightStatsUpdated(UAVObject* obj) // Force update if not yet connected GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); FlightTelemetryStats::DataFields flightStats = flightStatsObj->getData(); - if ( gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED || - flightStats.Status != FlightTelemetryStats::STATUS_CONNECTED ) - { + if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED || + flightStats.Status != FlightTelemetryStats::STATUS_CONNECTED) { processStatsUpdates(); } } @@ -190,51 +179,41 @@ void TelemetryMonitor::processStatsUpdates() // Get telemetry stats GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); FlightTelemetryStats::DataFields flightStats = flightStatsObj->getData(); - Telemetry::TelemetryStats telStats = tel->getStats(); + Telemetry::TelemetryStats telStats = tel->getStats(); + tel->resetStats(); - // Update stats object - gcsStats.RxDataRate = (float)telStats.rxBytes / ((float)statsTimer->interval()/1000.0); - gcsStats.TxDataRate = (float)telStats.txBytes / ((float)statsTimer->interval()/1000.0); + // Update stats object + gcsStats.RxDataRate = (float)telStats.rxBytes / ((float)statsTimer->interval() / 1000.0); + gcsStats.TxDataRate = (float)telStats.txBytes / ((float)statsTimer->interval() / 1000.0); gcsStats.RxFailures += telStats.rxErrors; gcsStats.TxFailures += telStats.txErrors; - gcsStats.TxRetries += telStats.txRetries; + gcsStats.TxRetries += telStats.txRetries; // Check for a connection timeout bool connectionTimeout; - if ( telStats.rxObjects > 0 ) - { + if (telStats.rxObjects > 0) { connectionTimer->start(); } - if ( connectionTimer->elapsed() > CONNECTION_TIMEOUT_MS ) - { + if (connectionTimer->elapsed() > CONNECTION_TIMEOUT_MS) { connectionTimeout = true; - } - else - { + } else { connectionTimeout = false; } // Update connection state int oldStatus = gcsStats.Status; - if ( gcsStats.Status == GCSTelemetryStats::STATUS_DISCONNECTED ) - { + if (gcsStats.Status == GCSTelemetryStats::STATUS_DISCONNECTED) { // Request connection gcsStats.Status = GCSTelemetryStats::STATUS_HANDSHAKEREQ; - } - else if ( gcsStats.Status == GCSTelemetryStats::STATUS_HANDSHAKEREQ ) - { + } else if (gcsStats.Status == GCSTelemetryStats::STATUS_HANDSHAKEREQ) { // Check for connection acknowledge - if ( flightStats.Status == FlightTelemetryStats::STATUS_HANDSHAKEACK ) - { + if (flightStats.Status == FlightTelemetryStats::STATUS_HANDSHAKEACK) { gcsStats.Status = GCSTelemetryStats::STATUS_CONNECTED; } - } - else if ( gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED ) - { + } else if (gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED) { // Check if the connection is still active and the the autopilot is still connected - if (flightStats.Status == FlightTelemetryStats::STATUS_DISCONNECTED || connectionTimeout) - { + if (flightStats.Status == FlightTelemetryStats::STATUS_DISCONNECTED || connectionTimeout) { gcsStats.Status = GCSTelemetryStats::STATUS_DISCONNECTED; } } @@ -245,25 +224,21 @@ void TelemetryMonitor::processStatsUpdates() gcsStatsObj->setData(gcsStats); // Force telemetry update if not yet connected - if ( gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED || - flightStats.Status != FlightTelemetryStats::STATUS_CONNECTED ) - { + if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED || + flightStats.Status != FlightTelemetryStats::STATUS_CONNECTED) { gcsStatsObj->updated(); } // Act on new connections or disconnections - if (gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED && gcsStats.Status != oldStatus) - { + if (gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED && gcsStats.Status != oldStatus) { statsTimer->setInterval(STATS_UPDATE_PERIOD_MS); qxtLog->info("Connection with the autopilot established"); startRetrievingObjects(); } - if (gcsStats.Status == GCSTelemetryStats::STATUS_DISCONNECTED && gcsStats.Status != oldStatus) - { + if (gcsStats.Status == GCSTelemetryStats::STATUS_DISCONNECTED && gcsStats.Status != oldStatus) { statsTimer->setInterval(STATS_CONNECT_PERIOD_MS); qxtLog->info("Connection with the autopilot lost"); qxtLog->info("Trying to connect to the autopilot"); emit disconnected(); } } - diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.h b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.h index 6a1ad7d9f..58d70dff6 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.h @@ -40,12 +40,11 @@ #include "systemstats.h" #include "telemetry.h" -class TelemetryMonitor : public QObject -{ +class TelemetryMonitor : public QObject { Q_OBJECT public: - TelemetryMonitor(UAVObjectManager* objMngr, Telemetry* tel); + TelemetryMonitor(UAVObjectManager *objMngr, Telemetry *tel); ~TelemetryMonitor(); signals: @@ -54,24 +53,24 @@ signals: void telemetryUpdated(double txRate, double rxRate); public slots: - void transactionCompleted(UAVObject* obj, bool success); + void transactionCompleted(UAVObject *obj, bool success); void processStatsUpdates(); - void flightStatsUpdated(UAVObject* obj); + void flightStatsUpdated(UAVObject *obj); private: - static const int STATS_UPDATE_PERIOD_MS = 4000; + static const int STATS_UPDATE_PERIOD_MS = 4000; static const int STATS_CONNECT_PERIOD_MS = 2000; - static const int CONNECTION_TIMEOUT_MS = 8000; + static const int CONNECTION_TIMEOUT_MS = 8000; - UAVObjectManager* objMngr; - Telemetry* tel; - QQueue queue; - GCSTelemetryStats* gcsStatsObj; - FlightTelemetryStats* flightStatsObj; - QTimer* statsTimer; - UAVObject* objPending; - QMutex* mutex; - QTime* connectionTimer; + UAVObjectManager *objMngr; + Telemetry *tel; + QQueue queue; + GCSTelemetryStats *gcsStatsObj; + FlightTelemetryStats *flightStatsObj; + QTimer *statsTimer; + UAVObject *objPending; + QMutex *mutex; + QTime *connectionTimer; void startRetrievingObjects(); void retrieveNextObject(); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index da98bee98..13f621880 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -10,18 +10,18 @@ * @brief The UAVTalk protocol plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavtalk.h" @@ -29,13 +29,13 @@ #include #include #include -//#define UAVTALK_DEBUG +// #define UAVTALK_DEBUG #ifdef UAVTALK_DEBUG #include "qxtlogger.h" - #define UAVTALK_QXTLOG_DEBUG(args...) qxtLog->debug(args...) -#else // UAVTALK_DEBUG - #define UAVTALK_QXTLOG_DEBUG(args...) -#endif // UAVTALK_DEBUG + #define UAVTALK_QXTLOG_DEBUG(args ...) qxtLog->debug(args ...) +#else // UAVTALK_DEBUG + #define UAVTALK_QXTLOG_DEBUG(args ...) +#endif // UAVTALK_DEBUG #define SYNC_VAL 0x3C @@ -62,11 +62,11 @@ const quint8 UAVTalk::crc_table[256] = { /** * Constructor */ -UAVTalk::UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr) +UAVTalk::UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr) { io = iodev; - this->objMngr = objMngr; + this->objMngr = objMngr; rxState = STATE_SYNC; rxPacketLength = 0; @@ -77,17 +77,16 @@ UAVTalk::UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr) connect(io, SIGNAL(readyRead()), this, SLOT(processInputStream())); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); - useUDPMirror=settings->useUDPMirror(); - qDebug()<<"USE UDP:::::::::::."<getObject(); + useUDPMirror = settings->useUDPMirror(); + qDebug() << "USE UDP:::::::::::." << useUDPMirror; + if (useUDPMirror) { + udpSocketTx = new QUdpSocket(this); + udpSocketRx = new QUdpSocket(this); udpSocketTx->bind(9000); - udpSocketRx->connectToHost(QHostAddress::LocalHost,9000); - connect(udpSocketTx,SIGNAL(readyRead()),this,SLOT(dummyUDPRead())); - connect(udpSocketRx,SIGNAL(readyRead()),this,SLOT(dummyUDPRead())); + udpSocketRx->connectToHost(QHostAddress::LocalHost, 9000); + connect(udpSocketTx, SIGNAL(readyRead()), this, SLOT(dummyUDPRead())); + connect(udpSocketRx, SIGNAL(readyRead()), this, SLOT(dummyUDPRead())); } } @@ -95,7 +94,7 @@ UAVTalk::~UAVTalk() { // According to Qt, it is not necessary to disconnect upon // object deletion. - //disconnect(io, SIGNAL(readyRead()), this, SLOT(processInputStream())); + // disconnect(io, SIGNAL(readyRead()), this, SLOT(processInputStream())); } @@ -105,6 +104,7 @@ UAVTalk::~UAVTalk() void UAVTalk::resetStats() { QMutexLocker locker(mutex); + memset(&stats, 0, sizeof(ComStats)); } @@ -114,6 +114,7 @@ void UAVTalk::resetStats() UAVTalk::ComStats UAVTalk::getStats() { QMutexLocker locker(mutex); + return stats; } @@ -125,9 +126,8 @@ void UAVTalk::processInputStream() quint8 tmp; if (io && io->isReadable()) { - while (io->bytesAvailable() > 0) - { - io->read((char*)&tmp, 1); + while (io->bytesAvailable() > 0) { + io->read((char *)&tmp, 1); processInputByte(tmp); } } @@ -135,12 +135,12 @@ void UAVTalk::processInputStream() void UAVTalk::dummyUDPRead() { - QUdpSocket *socket=qobject_cast(sender()); + QUdpSocket *socket = qobject_cast(sender()); QByteArray junk; - while(socket->hasPendingDatagrams()) - { + + while (socket->hasPendingDatagrams()) { junk.resize(socket->pendingDatagramSize()); - socket->readDatagram(junk.data(),junk.size()); + socket->readDatagram(junk.data(), junk.size()); } } @@ -151,9 +151,10 @@ void UAVTalk::dummyUDPRead() * \param[in] allInstances If set true then all instances will be updated * \return Success (true), Failure (false) */ -bool UAVTalk::sendObjectRequest(UAVObject* obj, bool allInstances) +bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances) { QMutexLocker locker(mutex); + return objectTransaction(obj, TYPE_OBJ_REQ, allInstances); } @@ -164,15 +165,13 @@ bool UAVTalk::sendObjectRequest(UAVObject* obj, bool allInstances) * \param[in] allInstances If set true then all instances will be updated * \return Success (true), Failure (false) */ -bool UAVTalk::sendObject(UAVObject* obj, bool acked, bool allInstances) +bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances) { QMutexLocker locker(mutex); - if (acked) - { + + if (acked) { return objectTransaction(obj, TYPE_OBJ_ACK, allInstances); - } - else - { + } else { return objectTransaction(obj, TYPE_OBJ, allInstances); } } @@ -180,17 +179,18 @@ bool UAVTalk::sendObject(UAVObject* obj, bool acked, bool allInstances) /** * Cancel a pending transaction */ -void UAVTalk::cancelTransaction(UAVObject* obj) +void UAVTalk::cancelTransaction(UAVObject *obj) { QMutexLocker locker(mutex); quint32 objId = obj->getObjID(); - if(io.isNull()) + + if (io.isNull()) { return; - QMap::iterator itr = transMap.find(objId); - if ( itr != transMap.end() ) - { + } + QMap::iterator itr = transMap.find(objId); + if (itr != transMap.end()) { transMap.remove(objId); - delete itr.value(); + delete itr.value(); } } @@ -198,36 +198,28 @@ void UAVTalk::cancelTransaction(UAVObject* obj) * Execute the requested transaction on an object. * \param[in] obj Object * \param[in] type Transaction type - * TYPE_OBJ: send object, - * TYPE_OBJ_REQ: request object update - * TYPE_OBJ_ACK: send object with an ack + * TYPE_OBJ: send object, + * TYPE_OBJ_REQ: request object update + * TYPE_OBJ_ACK: send object with an ack * \param[in] allInstances If set true then all instances will be updated * \return Success (true), Failure (false) */ -bool UAVTalk::objectTransaction(UAVObject* obj, quint8 type, bool allInstances) +bool UAVTalk::objectTransaction(UAVObject *obj, quint8 type, bool allInstances) { // Send object depending on if a response is needed - if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) - { - if ( transmitObject(obj, type, allInstances) ) - { - Transaction *trans = new Transaction(); - trans->obj = obj; - trans->allInstances = allInstances; + if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) { + if (transmitObject(obj, type, allInstances)) { + Transaction *trans = new Transaction(); + trans->obj = obj; + trans->allInstances = allInstances; transMap.insert(obj->getObjID(), trans); return true; - } - else - { + } else { return false; } - } - else if (type == TYPE_OBJ) - { + } else if (type == TYPE_OBJ) { return transmitObject(obj, TYPE_OBJ, allInstances); - } - else - { + } else { return false; } } @@ -242,260 +234,232 @@ bool UAVTalk::processInputByte(quint8 rxbyte) // Update stats stats.rxBytes++; - rxPacketLength++; // update packet byte count + rxPacketLength++; // update packet byte count - if(useUDPMirror) + if (useUDPMirror) { rxDataArray.append(rxbyte); + } // Receive state machine - switch (rxState) - { - case STATE_SYNC: + switch (rxState) { + case STATE_SYNC: - if (rxbyte != SYNC_VAL) - { - UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Sync (" + QString::number(rxbyte) + " " + QString("0x%1").arg(rxbyte,2,16) + ")"); - break; - } - - // Initialize and update CRC - rxCS = updateCRC(0, rxbyte); - - rxPacketLength = 1; - - if(useUDPMirror) - { - rxDataArray.clear(); - rxDataArray.append(rxbyte); - } - - rxState = STATE_TYPE; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Type"); + if (rxbyte != SYNC_VAL) { + UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Sync (" + QString::number(rxbyte) + " " + QString("0x%1").arg(rxbyte, 2, 16) + ")"); break; + } - case STATE_TYPE: + // Initialize and update CRC + rxCS = updateCRC(0, rxbyte); - // Update CRC - rxCS = updateCRC(rxCS, rxbyte); + rxPacketLength = 1; - if ((rxbyte & TYPE_MASK) != TYPE_VER) - { + if (useUDPMirror) { + rxDataArray.clear(); + rxDataArray.append(rxbyte); + } + + rxState = STATE_TYPE; + UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Type"); + break; + + case STATE_TYPE: + + // Update CRC + rxCS = updateCRC(rxCS, rxbyte); + + if ((rxbyte & TYPE_MASK) != TYPE_VER) { + rxState = STATE_SYNC; + UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Sync"); + break; + } + + rxType = rxbyte; + + packetSize = 0; + + rxState = STATE_SIZE; + UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Size"); + rxCount = 0; + break; + + case STATE_SIZE: + + // Update CRC + rxCS = updateCRC(rxCS, rxbyte); + + if (rxCount == 0) { + packetSize += rxbyte; + rxCount++; + UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Size"); + break; + } + + packetSize += (quint32)rxbyte << 8; + + if (packetSize < MIN_HEADER_LENGTH || packetSize > MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH) { // incorrect packet size + rxState = STATE_SYNC; + UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Sync"); + break; + } + + rxCount = 0; + rxState = STATE_OBJID; + UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->ObjID"); + break; + + case STATE_OBJID: + + // Update CRC + rxCS = updateCRC(rxCS, rxbyte); + + rxTmpBuffer[rxCount++] = rxbyte; + if (rxCount < 4) { + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->ObjID"); + break; + } + + // Search for object, if not found reset state machine + rxObjId = (qint32)qFromLittleEndian(rxTmpBuffer); + { + UAVObject *rxObj = objMngr->getObject(rxObjId); + if (rxObj == NULL && rxType != TYPE_OBJ_REQ) { + stats.rxErrors++; rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Sync"); + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (badtype)"); break; } - rxType = rxbyte; - - packetSize = 0; - - rxState = STATE_SIZE; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Size"); - rxCount = 0; - break; - - case STATE_SIZE: - - // Update CRC - rxCS = updateCRC(rxCS, rxbyte); - - if (rxCount == 0) - { - packetSize += rxbyte; - rxCount++; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Size"); - break; + // Determine data length + if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK) { + rxLength = 0; + rxInstanceLength = 0; + } else { + rxLength = rxObj->getNumBytes(); + rxInstanceLength = (rxObj->isSingleInstance() ? 0 : 2); } - packetSize += (quint32)rxbyte << 8; - - if (packetSize < MIN_HEADER_LENGTH || packetSize > MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH) - { // incorrect packet size + // Check length and determine next state + if (rxLength >= MAX_PAYLOAD_LENGTH) { + stats.rxErrors++; rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Sync"); + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (oversize)"); break; } - rxCount = 0; - rxState = STATE_OBJID; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->ObjID"); + // Check the lengths match + if ((rxPacketLength + rxInstanceLength + rxLength) != packetSize) { // packet error - mismatched packet size + stats.rxErrors++; + rxState = STATE_SYNC; + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (length mismatch)"); + break; + } + + // Check if this is a single instance object (i.e. if the instance ID field is coming next) + if (rxObj == NULL) { + // This is a non-existing object, just skip to checksum + // and we'll send a NACK next. + rxState = STATE_CS; + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->CSum (no obj)"); + rxInstId = 0; + rxCount = 0; + } else if (rxObj->isSingleInstance()) { + // If there is a payload get it, otherwise receive checksum + if (rxLength > 0) { + rxState = STATE_DATA; + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Data (needs data)"); + } else { + rxState = STATE_CS; + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Checksum"); + } + rxInstId = 0; + rxCount = 0; + } else { + rxState = STATE_INSTID; + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->InstID"); + rxCount = 0; + } + } + + break; + + case STATE_INSTID: + + // Update CRC + rxCS = updateCRC(rxCS, rxbyte); + + rxTmpBuffer[rxCount++] = rxbyte; + if (rxCount < 2) { + UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->InstID"); break; + } - case STATE_OBJID: + rxInstId = (qint16)qFromLittleEndian(rxTmpBuffer); - // Update CRC - rxCS = updateCRC(rxCS, rxbyte); - - rxTmpBuffer[rxCount++] = rxbyte; - if (rxCount < 4) - { - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->ObjID"); - break; - } - - // Search for object, if not found reset state machine - rxObjId = (qint32)qFromLittleEndian(rxTmpBuffer); - { - UAVObject *rxObj = objMngr->getObject(rxObjId); - if (rxObj == NULL && rxType != TYPE_OBJ_REQ) - { - stats.rxErrors++; - rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (badtype)"); - break; - } - - // Determine data length - if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK) - { - rxLength = 0; - rxInstanceLength = 0; - } - else - { - rxLength = rxObj->getNumBytes(); - rxInstanceLength = (rxObj->isSingleInstance() ? 0 : 2); - } - - // Check length and determine next state - if (rxLength >= MAX_PAYLOAD_LENGTH) - { - stats.rxErrors++; - rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (oversize)"); - break; - } - - // Check the lengths match - if ((rxPacketLength + rxInstanceLength + rxLength) != packetSize) - { // packet error - mismatched packet size - stats.rxErrors++; - rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (length mismatch)"); - break; - } - - // Check if this is a single instance object (i.e. if the instance ID field is coming next) - if (rxObj == NULL) - { - // This is a non-existing object, just skip to checksum - // and we'll send a NACK next. - rxState = STATE_CS; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->CSum (no obj)"); - rxInstId = 0; - rxCount = 0; - } - else if (rxObj->isSingleInstance()) - { - // If there is a payload get it, otherwise receive checksum - if (rxLength > 0) - { - rxState = STATE_DATA; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Data (needs data)"); - } - else - { - rxState = STATE_CS; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Checksum"); - } - rxInstId = 0; - rxCount = 0; - } - else - { - rxState = STATE_INSTID; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->InstID"); - rxCount = 0; - } - } - - break; - - case STATE_INSTID: - - // Update CRC - rxCS = updateCRC(rxCS, rxbyte); - - rxTmpBuffer[rxCount++] = rxbyte; - if (rxCount < 2) - { - UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->InstID"); - break; - } - - rxInstId = (qint16)qFromLittleEndian(rxTmpBuffer); - - rxCount = 0; - - // If there is a payload get it, otherwise receive checksum - if (rxLength > 0) - { - rxState = STATE_DATA; - UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->Data"); - } - else - { - rxState = STATE_CS; - UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->CSum"); - } - break; - - case STATE_DATA: - - // Update CRC - rxCS = updateCRC(rxCS, rxbyte); - - rxBuffer[rxCount++] = rxbyte; - if (rxCount < rxLength) - { - UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->Data"); - break; - } + rxCount = 0; + // If there is a payload get it, otherwise receive checksum + if (rxLength > 0) { + rxState = STATE_DATA; + UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->Data"); + } else { rxState = STATE_CS; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->CSum"); - rxCount = 0; + UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->CSum"); + } + break; + + case STATE_DATA: + + // Update CRC + rxCS = updateCRC(rxCS, rxbyte); + + rxBuffer[rxCount++] = rxbyte; + if (rxCount < rxLength) { + UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->Data"); break; + } - case STATE_CS: + rxState = STATE_CS; + UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->CSum"); + rxCount = 0; + break; - // The CRC byte - rxCSPacket = rxbyte; + case STATE_CS: - if (rxCS != rxCSPacket) - { // packet error - faulty CRC - stats.rxErrors++; - rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (badcrc)"); - break; - } + // The CRC byte + rxCSPacket = rxbyte; - if (rxPacketLength != packetSize + 1) - { // packet error - mismatched packet size - stats.rxErrors++; - rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (length mismatch)"); - break; - } - - mutex->lock(); - receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength); - if(useUDPMirror) - { - udpSocketTx->writeDatagram(rxDataArray,QHostAddress::LocalHost,udpSocketRx->localPort()); - } - stats.rxObjectBytes += rxLength; - stats.rxObjects++; - mutex->unlock(); - - rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (OK)"); - break; - - default: - rxState = STATE_SYNC; + if (rxCS != rxCSPacket) { // packet error - faulty CRC stats.rxErrors++; - UAVTALK_QXTLOG_DEBUG("UAVTalk: \?\?\?->Sync"); //Use the escape character for '?' so that the tripgraph isn't triggered. + rxState = STATE_SYNC; + UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (badcrc)"); + break; + } + + if (rxPacketLength != packetSize + 1) { // packet error - mismatched packet size + stats.rxErrors++; + rxState = STATE_SYNC; + UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (length mismatch)"); + break; + } + + mutex->lock(); + receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength); + if (useUDPMirror) { + udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort()); + } + stats.rxObjectBytes += rxLength; + stats.rxObjects++; + mutex->unlock(); + + rxState = STATE_SYNC; + UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (OK)"); + break; + + default: + rxState = STATE_SYNC; + stats.rxErrors++; + UAVTALK_QXTLOG_DEBUG("UAVTalk: \?\?\?->Sync"); // Use the escape character for '?' so that the tripgraph isn't triggered. } // Done @@ -511,74 +475,56 @@ bool UAVTalk::processInputByte(quint8 rxbyte) * \param[in] length Buffer length * \return Success (true), Failure (false) */ -bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* data, qint32 length) +bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length) { Q_UNUSED(length); - UAVObject* obj = NULL; - bool error = false; - bool allInstances = (instId == ALL_INSTANCES); + UAVObject *obj = NULL; + bool error = false; + bool allInstances = (instId == ALL_INSTANCES); // Process message type switch (type) { case TYPE_OBJ: // All instances, not allowed for OBJ messages - if (!allInstances) - { + if (!allInstances) { // Get object and update its data obj = updateObject(objId, instId, data); // Check if an ack is pending - if ( obj != NULL ) - { + if (obj != NULL) { updateAck(obj); - } - else - { + } else { error = true; } - } - else - { + } else { error = true; } break; case TYPE_OBJ_ACK: // All instances, not allowed for OBJ_ACK messages - if (!allInstances) - { + if (!allInstances) { // Get object and update its data obj = updateObject(objId, instId, data); // Transmit ACK - if ( obj != NULL ) - { - transmitObject(obj, TYPE_ACK, false); - } - else - { + if (obj != NULL) { + transmitObject(obj, TYPE_ACK, false); + } else { error = true; } - } - else - { + } else { error = true; } break; case TYPE_OBJ_REQ: // Get object, if all instances are requested get instance 0 of the object - if (allInstances) - { + if (allInstances) { obj = objMngr->getObject(objId); - } - else - { + } else { obj = objMngr->getObject(objId, instId); } // If object was found transmit it - if (obj != NULL) - { + if (obj != NULL) { transmitObject(obj, TYPE_OBJ, allInstances); - } - else - { + } else { // Object was not found, transmit a NACK with the // objId which was not found. transmitNack(objId); @@ -587,34 +533,26 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* break; case TYPE_NACK: // All instances, not allowed for NACK messages - if (!allInstances) - { + if (!allInstances) { // Get object obj = objMngr->getObject(objId, instId); // Check if object exists: - if (obj != NULL) - { + if (obj != NULL) { updateNack(obj); - } - else - { - error = true; + } else { + error = true; } } break; case TYPE_ACK: // All instances, not allowed for ACK messages - if (!allInstances) - { + if (!allInstances) { // Get object obj = objMngr->getObject(objId, instId); // Check if an ack is pending - if (obj != NULL) - { + if (obj != NULL) { updateAck(obj); - } - else - { + } else { error = true; } } @@ -631,36 +569,31 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* * If the object instance could not be found in the list, then a * new one is created. */ -UAVObject* UAVTalk::updateObject(quint32 objId, quint16 instId, quint8* data) +UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data) { // Get object - UAVObject* obj = objMngr->getObject(objId, instId); + UAVObject *obj = objMngr->getObject(objId, instId); + // If the instance does not exist create it - if (obj == NULL) - { + if (obj == NULL) { // Get the object type - UAVObject* tobj = objMngr->getObject(objId); - if (tobj == NULL) - { + UAVObject *tobj = objMngr->getObject(objId); + if (tobj == NULL) { return NULL; } // Make sure this is a data object - UAVDataObject* dobj = dynamic_cast(tobj); - if (dobj == NULL) - { + UAVDataObject *dobj = dynamic_cast(tobj); + if (dobj == NULL) { return NULL; } // Create a new instance, unpack and register - UAVDataObject* instobj = dobj->clone(instId); - if ( !objMngr->registerObject(instobj) ) - { + UAVDataObject *instobj = dobj->clone(instId); + if (!objMngr->registerObject(instobj)) { return NULL; } instobj->unpack(data); return instobj; - } - else - { + } else { // Unpack data into object instance obj->unpack(data); return obj; @@ -670,17 +603,17 @@ UAVObject* UAVTalk::updateObject(quint32 objId, quint16 instId, quint8* data) /** * Check if a transaction is pending and if yes complete it. */ -void UAVTalk::updateNack(UAVObject* obj) +void UAVTalk::updateNack(UAVObject *obj) { Q_ASSERT(obj); - if ( ! obj ) + if (!obj) { return; + } quint32 objId = obj->getObjID(); - QMap::iterator itr = transMap.find(objId); - if ( itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) - { + QMap::iterator itr = transMap.find(objId); + if (itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) { transMap.remove(objId); - delete itr.value(); + delete itr.value(); emit transactionCompleted(obj, false); } } @@ -689,14 +622,14 @@ void UAVTalk::updateNack(UAVObject* obj) /** * Check if a transaction is pending and if yes complete it. */ -void UAVTalk::updateAck(UAVObject* obj) +void UAVTalk::updateAck(UAVObject *obj) { quint32 objId = obj->getObjID(); - QMap::iterator itr = transMap.find(objId); - if ( itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) - { + + QMap::iterator itr = transMap.find(objId); + if (itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) { transMap.remove(objId); - delete itr.value(); + delete itr.value(); emit transactionCompleted(obj, true); } } @@ -709,51 +642,36 @@ void UAVTalk::updateAck(UAVObject* obj) * \param[in] allInstances True is all instances of the object are to be sent * \return Success (true), Failure (false) */ -bool UAVTalk::transmitObject(UAVObject* obj, quint8 type, bool allInstances) -{ +bool UAVTalk::transmitObject(UAVObject *obj, quint8 type, bool allInstances) +{ // If all instances are requested on a single instance object it is an error - if (allInstances && obj->isSingleInstance()) - { + if (allInstances && obj->isSingleInstance()) { allInstances = false; } // Process message type - if ( type == TYPE_OBJ || type == TYPE_OBJ_ACK ) - { - if (allInstances) - { + if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) { + if (allInstances) { // Get number of instances quint32 numInst = objMngr->getNumInstances(obj->getObjID()); // Send all instances - for (quint32 instId = 0; instId < numInst; ++instId) - { - UAVObject* inst = objMngr->getObject(obj->getObjID(), instId); + for (quint32 instId = 0; instId < numInst; ++instId) { + UAVObject *inst = objMngr->getObject(obj->getObjID(), instId); transmitSingleObject(inst, type, false); } return true; - } - else - { + } else { return transmitSingleObject(obj, type, false); } - } - else if (type == TYPE_OBJ_REQ) - { + } else if (type == TYPE_OBJ_REQ) { return transmitSingleObject(obj, TYPE_OBJ_REQ, allInstances); - } - else if (type == TYPE_ACK) - { - if (!allInstances) - { + } else if (type == TYPE_ACK) { + if (!allInstances) { return transmitSingleObject(obj, TYPE_ACK, false); - } - else - { + } else { return false; } - } - else - { + } else { return false; } } @@ -776,26 +694,21 @@ bool UAVTalk::transmitNack(quint32 objId) qToLittleEndian(dataOffset, &txBuffer[2]); // Send buffer, check that the transmit backlog does not grow above limit - if (io && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE ) - { - io->write((const char*)txBuffer, dataOffset+CHECKSUM_LENGTH); - if(useUDPMirror) - { - udpSocketRx->writeDatagram((const char*)txBuffer,dataOffset+CHECKSUM_LENGTH,QHostAddress::LocalHost,udpSocketTx->localPort()); + if (io && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE) { + io->write((const char *)txBuffer, dataOffset + CHECKSUM_LENGTH); + if (useUDPMirror) { + udpSocketRx->writeDatagram((const char *)txBuffer, dataOffset + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort()); } - } - else - { + } else { ++stats.txErrors; return false; } // Update stats - stats.txBytes += 8+CHECKSUM_LENGTH; + stats.txBytes += 8 + CHECKSUM_LENGTH; // Done return true; - } @@ -805,7 +718,7 @@ bool UAVTalk::transmitNack(quint32 objId) * \param[in] type Transaction type * \return Success (true), Failure (false) */ -bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances) +bool UAVTalk::transmitSingleObject(UAVObject *obj, quint8 type, bool allInstances) { qint32 length; qint32 dataOffset; @@ -820,19 +733,13 @@ bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstance qToLittleEndian(objId, &txBuffer[4]); // Setup instance ID if one is required - if ( obj->isSingleInstance() ) - { + if (obj->isSingleInstance()) { dataOffset = 8; - } - else - { + } else { // Check if all instances are requested - if (allInstances) - { + if (allInstances) { qToLittleEndian(allInstId, &txBuffer[8]); - } - else - { + } else { instId = obj->getInstID(); qToLittleEndian(instId, &txBuffer[8]); } @@ -840,26 +747,20 @@ bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstance } // Determine data length - if (type == TYPE_OBJ_REQ || type == TYPE_ACK) - { + if (type == TYPE_OBJ_REQ || type == TYPE_ACK) { length = 0; - } - else - { + } else { length = obj->getNumBytes(); } // Check length - if (length >= MAX_PAYLOAD_LENGTH) - { + if (length >= MAX_PAYLOAD_LENGTH) { return false; } // Copy data (if any) - if (length > 0) - { - if ( !obj->pack(&txBuffer[dataOffset]) ) - { + if (length > 0) { + if (!obj->pack(&txBuffer[dataOffset])) { return false; } } @@ -867,26 +768,22 @@ bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstance qToLittleEndian(dataOffset + length, &txBuffer[2]); // Calculate checksum - txBuffer[dataOffset+length] = updateCRC(0, txBuffer, dataOffset + length); + txBuffer[dataOffset + length] = updateCRC(0, txBuffer, dataOffset + length); // Send buffer, check that the transmit backlog does not grow above limit - if (!io.isNull() && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE ) - { - io->write((const char*)txBuffer, dataOffset+length+CHECKSUM_LENGTH); - if(useUDPMirror) - { - udpSocketRx->writeDatagram((const char*)txBuffer,dataOffset+length+CHECKSUM_LENGTH,QHostAddress::LocalHost,udpSocketTx->localPort()); + if (!io.isNull() && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE) { + io->write((const char *)txBuffer, dataOffset + length + CHECKSUM_LENGTH); + if (useUDPMirror) { + udpSocketRx->writeDatagram((const char *)txBuffer, dataOffset + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort()); } - } - else - { + } else { ++stats.txErrors; return false; } // Update stats ++stats.txObjects; - stats.txBytes += dataOffset+length+CHECKSUM_LENGTH; + stats.txBytes += dataOffset + length + CHECKSUM_LENGTH; stats.txObjectBytes += length; // Done @@ -915,9 +812,10 @@ quint8 UAVTalk::updateCRC(quint8 crc, const quint8 data) { return crc_table[crc ^ data]; } -quint8 UAVTalk::updateCRC(quint8 crc, const quint8* data, qint32 length) +quint8 UAVTalk::updateCRC(quint8 crc, const quint8 *data, qint32 length) { - while (length--) + while (length--) { crc = crc_table[crc ^ *data++]; + } return crc; } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index 3d2c69d7b..fd22b66ef 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h @@ -10,18 +10,18 @@ * @brief The UAVTalk protocol plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVTALK_H @@ -37,8 +37,7 @@ #include "uavtalk_global.h" #include -class UAVTALK_EXPORT UAVTalk: public QObject -{ +class UAVTALK_EXPORT UAVTalk : public QObject { Q_OBJECT public: @@ -53,16 +52,16 @@ public: quint32 rxErrors; } ComStats; - UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr); + UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr); ~UAVTalk(); - bool sendObject(UAVObject* obj, bool acked, bool allInstances); - bool sendObjectRequest(UAVObject* obj, bool allInstances); - void cancelTransaction(UAVObject* obj); + bool sendObject(UAVObject *obj, bool acked, bool allInstances); + bool sendObjectRequest(UAVObject *obj, bool allInstances); + void cancelTransaction(UAVObject *obj); ComStats getStats(); void resetStats(); signals: - void transactionCompleted(UAVObject* obj, bool success); + void transactionCompleted(UAVObject *obj, bool success); private slots: void processInputStream(void); @@ -71,42 +70,42 @@ private slots: private: typedef struct { - UAVObject* obj; + UAVObject *obj; bool allInstances; } Transaction; // Constants - static const int TYPE_MASK = 0xF8; - static const int TYPE_VER = 0x20; - static const int TYPE_OBJ = (TYPE_VER | 0x00); + static const int TYPE_MASK = 0xF8; + static const int TYPE_VER = 0x20; + static const int TYPE_OBJ = (TYPE_VER | 0x00); static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01); static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02); - static const int TYPE_ACK = (TYPE_VER | 0x03); - static const int TYPE_NACK = (TYPE_VER | 0x04); + static const int TYPE_ACK = (TYPE_VER | 0x03); + static const int TYPE_NACK = (TYPE_VER | 0x04); - static const int MIN_HEADER_LENGTH = 8; // sync(1), type (1), size(2), object ID(4) - static const int MAX_HEADER_LENGTH = 10; // sync(1), type (1), size(2), object ID (4), instance ID(2, not used in single objects) + static const int MIN_HEADER_LENGTH = 8; // sync(1), type (1), size(2), object ID(4) + static const int MAX_HEADER_LENGTH = 10; // sync(1), type (1), size(2), object ID (4), instance ID(2, not used in single objects) - static const int CHECKSUM_LENGTH = 1; + static const int CHECKSUM_LENGTH = 1; static const int MAX_PAYLOAD_LENGTH = 256; - static const int MAX_PACKET_LENGTH = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH); + static const int MAX_PACKET_LENGTH = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH); - static const quint16 ALL_INSTANCES = 0xFFFF; + static const quint16 ALL_INSTANCES = 0xFFFF; static const quint16 OBJID_NOTFOUND = 0x0000; - static const int TX_BUFFER_SIZE = 2*1024; + static const int TX_BUFFER_SIZE = 2 * 1024; static const quint8 crc_table[256]; // Types - typedef enum {STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxStateType; + typedef enum { STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS } RxStateType; // Variables QPointer io; - UAVObjectManager* objMngr; - QMutex* mutex; - QMap transMap; + UAVObjectManager *objMngr; + QMutex *mutex; + QMap transMap; quint8 rxBuffer[MAX_PACKET_LENGTH]; quint8 txBuffer[MAX_PACKET_LENGTH]; // Variables used by the receive state machine @@ -125,22 +124,22 @@ private: ComStats stats; bool useUDPMirror; - QUdpSocket * udpSocketTx; - QUdpSocket * udpSocketRx; + QUdpSocket *udpSocketTx; + QUdpSocket *udpSocketRx; QByteArray rxDataArray; // Methods - bool objectTransaction(UAVObject* obj, quint8 type, bool allInstances); + bool objectTransaction(UAVObject *obj, quint8 type, bool allInstances); bool processInputByte(quint8 rxbyte); - bool receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* data, qint32 length); - UAVObject* updateObject(quint32 objId, quint16 instId, quint8* data); - void updateAck(UAVObject* obj); - void updateNack(UAVObject* obj); + bool receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length); + UAVObject *updateObject(quint32 objId, quint16 instId, quint8 *data); + void updateAck(UAVObject *obj); + void updateNack(UAVObject *obj); bool transmitNack(quint32 objId); - bool transmitObject(UAVObject* obj, quint8 type, bool allInstances); - bool transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances); + bool transmitObject(UAVObject *obj, quint8 type, bool allInstances); + bool transmitSingleObject(UAVObject *obj, quint8 type, bool allInstances); quint8 updateCRC(quint8 crc, const quint8 data); - quint8 updateCRC(quint8 crc, const quint8* data, qint32 length); + quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length); }; #endif // UAVTALK_H diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk_global.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk_global.h index d442e99ad..819070dff 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk_global.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk_global.h @@ -10,18 +10,18 @@ * @brief The UAVTalk protocol plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.cpp index 15d024f98..398e0b016 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.cpp @@ -10,18 +10,18 @@ * @brief The UAVTalk protocol plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavtalkplugin.h" @@ -30,31 +30,26 @@ #include UAVTalkPlugin::UAVTalkPlugin() -{ - -} +{} UAVTalkPlugin::~UAVTalkPlugin() -{ - -} +{} /** - * Called once all the plugins which depend on us have been loaded - */ + * Called once all the plugins which depend on us have been loaded + */ void UAVTalkPlugin::extensionsInitialized() -{ -} +{} /** - * Called at startup, before any plugin which depends on us is initialized - */ -bool UAVTalkPlugin::initialize(const QStringList & arguments, QString * errorString) + * Called at startup, before any plugin which depends on us is initialized + */ +bool UAVTalkPlugin::initialize(const QStringList & arguments, QString *errorString) { // Done Q_UNUSED(arguments); Q_UNUSED(errorString); // Get UAVObjectManager instance - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); objMngr = pm->getObject(); // Create TelemetryManager @@ -71,9 +66,7 @@ bool UAVTalkPlugin::initialize(const QStringList & arguments, QString * errorStr } void UAVTalkPlugin::shutdown() -{ - -} +{} void UAVTalkPlugin::onDeviceConnect(QIODevice *dev) { diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.h index 7a5508078..e28acfc1f 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.h @@ -10,18 +10,18 @@ * @brief The UAVTalk protocol plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVTALKPLUGIN_H @@ -36,8 +36,7 @@ #include "telemetrymanager.h" #include "uavobjectmanager.h" -class UAVTALK_EXPORT UAVTalkPlugin: public ExtensionSystem::IPlugin -{ +class UAVTALK_EXPORT UAVTalkPlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -45,7 +44,7 @@ public: ~UAVTalkPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); protected slots: @@ -53,8 +52,8 @@ protected slots: void onDeviceDisconnect(); private: - UAVObjectManager* objMngr; - TelemetryManager* telMngr; + UAVObjectManager *objMngr; + TelemetryManager *telMngr; }; #endif // UAVTALKPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/uploader/SSP/port.cpp b/ground/openpilotgcs/src/plugins/uploader/SSP/port.cpp index 499c94323..54ff148c8 100644 --- a/ground/openpilotgcs/src/plugins/uploader/SSP/port.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/SSP/port.cpp @@ -26,20 +26,20 @@ */ #include "port.h" #include "delay.h" -port::port(PortSettings settings,QString name):mstatus(port::closed) +port::port(PortSettings settings, QString name) : mstatus(port::closed) { timer.start(); - sport = new QextSerialPort(name,settings, QextSerialPort::Polling); - if(sport->open(QIODevice::ReadWrite|QIODevice::Unbuffered)) - { - mstatus=port::open; - // sport->setDtr(); + sport = new QextSerialPort(name, settings, QextSerialPort::Polling); + if (sport->open(QIODevice::ReadWrite | QIODevice::Unbuffered)) { + mstatus = port::open; + // sport->setDtr(); + } else { + mstatus = port::error; } - else - mstatus=port::error; } -port::~port() { +port::~port() +{ sport->close(); } @@ -49,21 +49,20 @@ port::portstatus port::status() } int16_t port::pfSerialRead(void) { - char c[1]; - if( sport->bytesAvailable() ) - { - sport->read(c,1); - } - else return -1; + + if (sport->bytesAvailable()) { + sport->read(c, 1); + } else { return -1; } return (uint8_t)c[0]; } void port::pfSerialWrite(uint8_t c) { char cc[1]; - cc[0]=c; - sport->write(cc,1); + + cc[0] = c; + sport->write(cc, 1); } uint32_t port::pfGetTime(void) diff --git a/ground/openpilotgcs/src/plugins/uploader/SSP/port.h b/ground/openpilotgcs/src/plugins/uploader/SSP/port.h index 1521ab412..8cc87f48f 100644 --- a/ground/openpilotgcs/src/plugins/uploader/SSP/port.h +++ b/ground/openpilotgcs/src/plugins/uploader/SSP/port.h @@ -32,38 +32,37 @@ #include #include "common.h" -class port -{ +class port { public: - enum portstatus{open,closed,error}; - virtual int16_t pfSerialRead(void); // function to read a character from the serial input stream - virtual void pfSerialWrite( uint8_t ); // function to write a byte to be sent out the serial port + enum portstatus { open, closed, error }; + virtual int16_t pfSerialRead(void); // function to read a character from the serial input stream + virtual void pfSerialWrite(uint8_t); // function to write a byte to be sent out the serial port virtual uint32_t pfGetTime(void); - uint8_t retryCount; // how many times have we tried to transmit the 'send' packet - uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet - uint16_t max_retry; // Maximum number of retrys for a single transmit. - int32_t timeoutLen; // how long to wait for each retry to succeed - uint32_t timeout; // current timeout. when 'time' reaches this point we have timed out - uint8_t txSeqNo; // current 'send' packet sequence number - uint16_t rxBufPos; // current buffer position in the receive packet - uint16_t rxBufLen; // number of 'data' bytes in the buffer - uint8_t rxSeqNo; // current 'receive' packet number - uint16_t rxBufSize; // size of the receive buffer. - uint16_t txBufSize; // size of the transmit buffer. - uint8_t *txBuf; // transmit buffer. REquired to store a copy of packet data in case a retry is needed. - uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received. - uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host + uint8_t retryCount; // how many times have we tried to transmit the 'send' packet + uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet + uint16_t max_retry; // Maximum number of retrys for a single transmit. + int32_t timeoutLen; // how long to wait for each retry to succeed + uint32_t timeout; // current timeout. when 'time' reaches this point we have timed out + uint8_t txSeqNo; // current 'send' packet sequence number + uint16_t rxBufPos; // current buffer position in the receive packet + uint16_t rxBufLen; // number of 'data' bytes in the buffer + uint8_t rxSeqNo; // current 'receive' packet number + uint16_t rxBufSize; // size of the receive buffer. + uint16_t txBufSize; // size of the transmit buffer. + uint8_t *txBuf; // transmit buffer. REquired to store a copy of packet data in case a retry is needed. + uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received. + uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host // this is required when switching from the application to the bootloader // and vice-versa. This fixes the firwmare download timeout. // when this flag is set to true, the next time we send a packet we will first // send a synchronize packet. - ReceiveState InputState; - decodeState_ DecodeState; - uint16_t SendState; - uint16_t crc; - uint32_t RxError; - uint32_t TxError; - uint16_t flags; - port(PortSettings settings,QString name); + ReceiveState InputState; + decodeState_ DecodeState; + uint16_t SendState; + uint16_t crc; + uint32_t RxError; + uint32_t TxError; + uint16_t flags; + port(PortSettings settings, QString name); ~port(); portstatus status(); private: diff --git a/ground/openpilotgcs/src/plugins/uploader/SSP/qssp.cpp b/ground/openpilotgcs/src/plugins/uploader/SSP/qssp.cpp index b5eb5b1b7..517772ad9 100644 --- a/ground/openpilotgcs/src/plugins/uploader/SSP/qssp.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/SSP/qssp.cpp @@ -33,48 +33,47 @@ /** PRIVATE DEFINITIONS **/ -#define SYNC 225 // Sync character used in Serial Protocol -#define ESC 224 // ESC character used in Serial Protocol -#define ESC_SYNC 1 // ESC_SYNC character used in Serial Protocol -#define ACK_BIT 0x80 // Ack bit, bit 7 of sequence number, 1 = Acknowledge, 0 = -// new packet +#define SYNC 225 // Sync character used in Serial Protocol +#define ESC 224 // ESC character used in Serial Protocol +#define ESC_SYNC 1 // ESC_SYNC character used in Serial Protocol +#define ACK_BIT 0x80 // Ack bit, bit 7 of sequence number, 1 = Acknowledge, 0 = +// new packet // packet location definitions. -#define LENGTH 0 -#define SEQNUM 1 -#define DATA 2 +#define LENGTH 0 +#define SEQNUM 1 +#define DATA 2 // Make larger sized integers from smaller sized integers -#define MAKEWORD16( ub, lb ) ((uint16_t)0x0000 | ((uint16_t)(ub) << 8) | (uint16_t)(lb) ) -#define MAKEWORD32( uw, lw ) ((uint32_t)(0x0UL | ((uint32_t)(uw) << 16) | (uint32_t)(lw)) ) -#define MAKEWORD32B( b3, b2, b1, b0 ) ((uint32_t)((uint32_t)(b3)<< 24) | ((uint32_t)(b2)<<16) | ((uint32_t)(b1)<<8) | ((uint32_t)(b0) ) +#define MAKEWORD16(ub, lb) ((uint16_t)0x0000 | ((uint16_t)(ub) << 8) | (uint16_t)(lb)) +#define MAKEWORD32(uw, lw) ((uint32_t)(0x0UL | ((uint32_t)(uw) << 16) | (uint32_t)(lw))) +#define MAKEWORD32B(b3, b2, b1, b0) ((uint32_t)((uint32_t)(b3) << 24) | ((uint32_t)(b2) << 16) | ((uint32_t)(b1) << 8) | ((uint32_t)(b0)) // Used to extract smaller integers from larger sized intergers -#define LOWERBYTE( w ) (uint8_t)((w) & 0x00ff ) -#define UPPERBYTE( w ) (uint8_t)(((w) & 0xff00) >> 8 ) -#define UPPERWORD(lw) (uint16_t)(((lw) & 0xffff0000) >> 16 ) -#define LOWERWORD(lw) (uint16_t)((lw) & 0x0000ffff) +#define LOWERBYTE(w) (uint8_t)((w) & 0x00ff) +#define UPPERBYTE(w) (uint8_t)(((w) & 0xff00) >> 8) +#define UPPERWORD(lw) (uint16_t)(((lw) & 0xffff0000) >> 16) +#define LOWERWORD(lw) (uint16_t)((lw) & 0x0000ffff) // Macros to operate on a target and bitmask. -#define CLEARBIT( a, b ) ((a) = (a) & ~(b)) -#define SETBIT( a, b ) ((a) = (a) | (b) ) -#define TOGGLEBIT(a,b) ((a) = (a) ^ (b) ) +#define CLEARBIT(a, b) ((a) = (a) & ~(b)) +#define SETBIT(a, b) ((a) = (a) | (b)) +#define TOGGLEBIT(a, b) ((a) = (a) ^ (b)) // test bit macros operate using a bit mask. -#define ISBITSET( a, b ) ( ((a) & (b)) == (b) ? TRUE : FALSE ) -#define ISBITCLEAR( a, b) ( (~(a) & (b)) == (b) ? TRUE : FALSE ) - +#define ISBITSET(a, b) (((a) & (b)) == (b) ? TRUE : FALSE) +#define ISBITCLEAR(a, b) ((~(a) & (b)) == (b) ? TRUE : FALSE) /* Flag bit masks...*/ -#define SENT_SYNCH (0x01) -#define ACK_RECEIVED (0x02) -#define ACK_EXPECTED (0x04) +#define SENT_SYNCH (0x01) +#define ACK_RECEIVED (0x02) +#define ACK_EXPECTED (0x04) -#define SSP_AWAITING_ACK 0 -#define SSP_ACKED 1 -#define SSP_IDLE 2 +#define SSP_AWAITING_ACK 0 +#define SSP_ACKED 1 +#define SSP_IDLE 2 /** PRIVATE DATA **/ static const uint16_t CRC_TABLE[] = { @@ -131,27 +130,25 @@ static const uint16_t CRC_TABLE[] = { * Must be called before calling the Send or REceive process functions. */ -void qssp::ssp_Init(const PortConfig_t* const info) +void qssp::ssp_Init(const PortConfig_t *const info) { - - - thisport->maxRetryCount = info->max_retry; - thisport->timeoutLen = info->timeoutLen; + thisport->maxRetryCount = info->max_retry; + thisport->timeoutLen = info->timeoutLen; thisport->txBufSize = info->txBufSize; - thisport->rxBufSize = info->rxBufSize; - thisport->txBuf = info->txBuf; - thisport->rxBuf = info->rxBuf; + thisport->rxBufSize = info->rxBufSize; + thisport->txBuf = info->txBuf; + thisport->rxBuf = info->rxBuf; thisport->retryCount = 0; - thisport->sendSynch = FALSE; //TRUE; - thisport->rxSeqNo = 255; - thisport->txSeqNo = 255; - thisport->SendState = SSP_IDLE; - thisport->InputState =(ReceiveState)0; - thisport->DecodeState =(decodeState_)0; - thisport->TxError =0; - thisport->RxError =0; - thisport->txSeqNo =0; - thisport->rxSeqNo =0; + thisport->sendSynch = FALSE; // TRUE; + thisport->rxSeqNo = 255; + thisport->txSeqNo = 255; + thisport->SendState = SSP_IDLE; + thisport->InputState = (ReceiveState)0; + thisport->DecodeState = (decodeState_)0; + thisport->TxError = 0; + thisport->RxError = 0; + thisport->txSeqNo = 0; + thisport->rxSeqNo = 0; } /*! @@ -165,11 +162,11 @@ void qssp::ssp_Init(const PortConfig_t* const info) * \note * */ -int16_t qssp::ssp_SendProcess( ) +int16_t qssp::ssp_SendProcess() { int16_t value = SSP_TX_WAITING; - if (thisport->SendState == SSP_AWAITING_ACK ) { + if (thisport->SendState == SSP_AWAITING_ACK) { if (sf_CheckTimeout() == TRUE) { if (thisport->retryCount < thisport->maxRetryCount) { // Try again @@ -179,16 +176,17 @@ int16_t qssp::ssp_SendProcess( ) } else { // Give up, # of trys has exceded the limit value = SSP_TX_TIMEOUT; - CLEARBIT( thisport->flags, ACK_RECEIVED); + CLEARBIT(thisport->flags, ACK_RECEIVED); thisport->SendState = SSP_IDLE; - if (debug) - qDebug()<<"Send TimeOut!"; + if (debug) { + qDebug() << "Send TimeOut!"; + } } } else { value = SSP_TX_WAITING; } - } else if( thisport->SendState == SSP_ACKED ) { - SETBIT( thisport->flags, ACK_RECEIVED); + } else if (thisport->SendState == SSP_ACKED) { + SETBIT(thisport->flags, ACK_RECEIVED); value = SSP_TX_ACKED; thisport->SendState = SSP_IDLE; } else { @@ -231,13 +229,13 @@ int16_t qssp::ssp_ReceiveProcess() * */ -int16_t qssp::ssp_ReceiveByte() +int16_t qssp::ssp_ReceiveByte() { int16_t b; int16_t packet_status = SSP_RX_IDLE; b = thisport->pfSerialRead(); - if( b != -1 ) { + if (b != -1) { packet_status = sf_ReceiveState(b); } return packet_status; @@ -254,17 +252,17 @@ int16_t qssp::ssp_ReceiveByte() * \note * */ -uint16_t qssp::ssp_SendDataBlock(uint8_t *data, uint16_t length ) +uint16_t qssp::ssp_SendDataBlock(uint8_t *data, uint16_t length) { - int16_t packet_status = SSP_TX_WAITING; - uint16_t retval = FALSE; + int16_t packet_status = SSP_TX_WAITING; + uint16_t retval = FALSE; - packet_status = ssp_SendData(data, length ); // send the data - while( packet_status == SSP_TX_WAITING ) { // check the status - (void)ssp_ReceiveProcess(); // process any bytes received. - packet_status = ssp_SendProcess(); // check the send status + packet_status = ssp_SendData(data, length); // send the data + while (packet_status == SSP_TX_WAITING) { // check the status + (void)ssp_ReceiveProcess(); // process any bytes received. + packet_status = ssp_SendProcess(); // check the send status } - if( packet_status == SSP_TX_ACKED ) { // figure out what happened to the packet + if (packet_status == SSP_TX_ACKED) { // figure out what happened to the packet retval = TRUE; } else { retval = FALSE; @@ -284,61 +282,62 @@ uint16_t qssp::ssp_SendDataBlock(uint8_t *data, uint16_t length ) * \note * */ -int16_t qssp::ssp_SendData(const uint8_t *data, const uint16_t length ) +int16_t qssp::ssp_SendData(const uint8_t *data, const uint16_t length) { - int16_t value = SSP_TX_WAITING; - if( (length + 2) > thisport->txBufSize ) { + if ((length + 2) > thisport->txBufSize) { // TRYING to send too much data. value = SSP_TX_BUFOVERRUN; - } else if( thisport->SendState == SSP_IDLE ) { + } else if (thisport->SendState == SSP_IDLE) { #ifdef ACTIVE_SYNCH - if( thisport->sendSynch == TRUE ) { + if (thisport->sendSynch == TRUE) { sf_SendSynchPacket(); } #endif #ifdef SYNCH_SEND - if( length == 0 ) { + if (length == 0) { // TODO this method could allow a task/user to start a synchronisation step if a zero is mistakenly passed to this function. - // could add a check for a NULL data pointer, or use some sort of static flag that can only be accessed by a static function - // that must be called before calling this function. + // could add a check for a NULL data pointer, or use some sort of static flag that can only be accessed by a static function + // that must be called before calling this function. // we are attempting to send a synch packet - thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us + thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us SETBIT(thisport->flags, SENT_SYNCH); } else { // we are sending a data packet - CLEARBIT( thisport->txSeqNo, ACK_BIT ); // make sure we are not sending a ACK packet - thisport->txSeqNo++; // update the sequence number. - if( thisport->txSeqNo > 0x7F) { // check for sequence number rollover - thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, + CLEARBIT(thisport->txSeqNo, ACK_BIT); // make sure we are not sending a ACK packet + thisport->txSeqNo++; // update the sequence number. + if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover + thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, // zero is reserviced for synchronization requests } } #else - CLEARBIT( thisport->txSeqNo, ACK_BIT ); // make sure we are not sending a ACK packet - thisport->txSeqNo++; // update the sequence number. - if( thisport->txSeqNo > 0x7F) { // check for sequence number rollover - thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, + CLEARBIT(thisport->txSeqNo, ACK_BIT); // make sure we are not sending a ACK packet + thisport->txSeqNo++; // update the sequence number. + if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover + thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, // zero is reserved for synchronization requests } -#endif - CLEARBIT( thisport->flags, ACK_RECEIVED); - thisport->SendState = SSP_AWAITING_ACK; +#endif // ifdef SYNCH_SEND + CLEARBIT(thisport->flags, ACK_RECEIVED); + thisport->SendState = SSP_AWAITING_ACK; value = SSP_TX_WAITING; - thisport->retryCount = 0; // zero out the retry counter for this transmission - sf_MakePacket( thisport->txBuf, data, length, thisport->txSeqNo ); - sf_SendPacket( ); // punch out the packet to the serial port - sf_SetSendTimeout( ); // do the timeout values - if (debug) - qDebug()<<"Sent DATA PACKET:"<txSeqNo; + thisport->retryCount = 0; // zero out the retry counter for this transmission + sf_MakePacket(thisport->txBuf, data, length, thisport->txSeqNo); + sf_SendPacket(); // punch out the packet to the serial port + sf_SetSendTimeout(); // do the timeout values + if (debug) { + qDebug() << "Sent DATA PACKET:" << thisport->txSeqNo; + } } else { // error we are already sending a packet. Need to wait for the current packet to be acked or timeout. value = SSP_TX_BUSY; - if (debug) - qDebug()<<"Error sending TX was busy"; + if (debug) { + qDebug() << "Error sending TX was busy"; + } } return value; } @@ -352,46 +351,47 @@ int16_t qssp::ssp_SendData(const uint8_t *data, const uint16_t length ) * \note * A. send a packet with a sequence number equal to zero * B. if timed out then: - * send synch packet again - * increment try counter - * if number of tries exceed maximum try limit then exit + * send synch packet again + * increment try counter + * if number of tries exceed maximum try limit then exit * C. goto A */ -uint16_t qssp::ssp_Synchronise( ) +uint16_t qssp::ssp_Synchronise() { - int16_t packet_status; - uint16_t retval = FALSE; + int16_t packet_status; + uint16_t retval = FALSE; #ifndef USE_SENDPACKET_DATA - thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us + thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us SETBIT(thisport->flags, SENT_SYNCH); // TODO - should this be using ssp_SendPacketData()?? - sf_MakePacket( thisport->txBuf, NULL, 0, thisport->txSeqNo ); // construct the packet - sf_SendPacket( ); - sf_SetSendTimeout( ); + sf_MakePacket(thisport->txBuf, NULL, 0, thisport->txSeqNo); // construct the packet + sf_SendPacket(); + sf_SetSendTimeout(); thisport->SendState = SSP_AWAITING_ACK; packet_status = SSP_TX_WAITING; #else - packet_status = ssp_SendData( NULL, 0 ); + packet_status = ssp_SendData(NULL, 0); #endif - while( packet_status == SSP_TX_WAITING ) { // we loop until we time out. - (void)ssp_ReceiveProcess( ); // do the receive process - packet_status = ssp_SendProcess( ); // do the send process + while (packet_status == SSP_TX_WAITING) { // we loop until we time out. + (void)ssp_ReceiveProcess(); // do the receive process + packet_status = ssp_SendProcess(); // do the send process } thisport->sendSynch = FALSE; - switch( packet_status ) { + switch (packet_status) { case SSP_TX_ACKED: retval = TRUE; break; - case SSP_TX_BUSY: // intentional fall through. - case SSP_TX_TIMEOUT: // intentional fall through. + case SSP_TX_BUSY: // intentional fall through. + case SSP_TX_TIMEOUT: // intentional fall through. case SSP_TX_BUFOVERRUN: retval = FALSE; break; default: retval = FALSE; break; - }; + } + ; return retval; } @@ -411,8 +411,8 @@ void qssp::sf_SendPacket() // use the raw serial write function so the SYNC byte does not get 'escaped' thisport->pfSerialWrite(SYNC); - for( uint8_t x = 0; x < packetLen; x++ ) { - sf_write_byte(thisport->txBuf[x] ); + for (uint8_t x = 0; x < packetLen; x++) { + sf_write_byte(thisport->txBuf[x]); } thisport->retryCount++; } @@ -434,26 +434,25 @@ void qssp::sf_SendPacket() * 3. TODO: Should this function return an error if data length to be sent is greater th tx buffer size? * */ -void qssp::sf_MakePacket( uint8_t *txBuf, const uint8_t * pdata, uint16_t length, uint8_t seqNo ) +void qssp::sf_MakePacket(uint8_t *txBuf, const uint8_t *pdata, uint16_t length, uint8_t seqNo) { - uint16_t crc = 0xffff; - uint16_t bufPos = 0; - uint8_t b; + uint16_t crc = 0xffff; + uint16_t bufPos = 0; + uint8_t b; // add 1 for the seq. number txBuf[LENGTH] = length + 1; txBuf[SEQNUM] = seqNo; - crc = sf_crc16( crc, seqNo ); + crc = sf_crc16(crc, seqNo); - length = length + 2; // add two for the length and seqno bytes which are added before the loop. - for( bufPos = 2; bufPos < length; bufPos++ ) { - b = *pdata++; + length = length + 2; // add two for the length and seqno bytes which are added before the loop. + for (bufPos = 2; bufPos < length; bufPos++) { + b = *pdata++; txBuf[bufPos] = b; - crc = sf_crc16( crc, b ); // update CRC value + crc = sf_crc16(crc, b); // update CRC value } txBuf[bufPos++] = LOWERBYTE(crc); txBuf[bufPos] = UPPERBYTE(crc); - } /*! @@ -468,13 +467,14 @@ void qssp::sf_MakePacket( uint8_t *txBuf, const uint8_t * pdata, uint16_t length void qssp::sf_SendAckPacket(uint8_t seqNumber) { - uint8_t AckSeqNumber = SETBIT( seqNumber, ACK_BIT ); + uint8_t AckSeqNumber = SETBIT(seqNumber, ACK_BIT); // create the packet, note we pass AckSequenceNumber directly - sf_MakePacket( thisport->txBuf, NULL, 0, AckSeqNumber ); - sf_SendPacket( ); - if (debug) - qDebug()<<"Sent ACK PACKET:"<txBuf, NULL, 0, AckSeqNumber); + sf_SendPacket(); + if (debug) { + qDebug() << "Sent ACK PACKET:" << seqNumber; + } // we don't set the timeout for an ACK because we don't ACK our ACKs in this protocol } @@ -487,30 +487,30 @@ void qssp::sf_SendAckPacket(uint8_t seqNumber) * \note * */ -void qssp::sf_write_byte(uint8_t c ) +void qssp::sf_write_byte(uint8_t c) { - if( c == SYNC ) { // check for SYNC byte - thisport->pfSerialWrite( ESC ); // since we are not starting a packet we must ESCAPE the SYNCH byte - thisport->pfSerialWrite( ESC_SYNC ); // now send the escaped synch char - } else if( c == ESC ) { // Check for ESC character - thisport->pfSerialWrite( ESC ); // if it is, we need to send it twice - thisport->pfSerialWrite( ESC ); + if (c == SYNC) { // check for SYNC byte + thisport->pfSerialWrite(ESC); // since we are not starting a packet we must ESCAPE the SYNCH byte + thisport->pfSerialWrite(ESC_SYNC); // now send the escaped synch char + } else if (c == ESC) { // Check for ESC character + thisport->pfSerialWrite(ESC); // if it is, we need to send it twice + thisport->pfSerialWrite(ESC); } else { - thisport->pfSerialWrite( c ); // otherwise write the byte to serial port + thisport->pfSerialWrite(c); // otherwise write the byte to serial port } } /************************************************************************************************************ -* -* NAME: uint16_t ssp_crc16( uint16_t crc, uint16_t data ) -* DESCRIPTION: Uses crc_table to calculate new crc -* ARGUMENTS: -* arg1: crc -* arg2: data - byte to calculate into CRC -* RETURN: New crc -* CREATED: 5/8/02 -* -*************************************************************************************************************/ + * + * NAME: uint16_t ssp_crc16( uint16_t crc, uint16_t data ) + * DESCRIPTION: Uses crc_table to calculate new crc + * ARGUMENTS: + * arg1: crc + * arg2: data - byte to calculate into CRC + * RETURN: New crc + * CREATED: 5/8/02 + * + *************************************************************************************************************/ /*! * \brief calculates the new CRC value for 'data' * \param crc = current CRC value @@ -521,9 +521,9 @@ void qssp::sf_write_byte(uint8_t c ) * */ -uint16_t qssp::sf_crc16( uint16_t crc, uint8_t data ) +uint16_t qssp::sf_crc16(uint16_t crc, uint8_t data) { - return (crc >> 8) ^ CRC_TABLE[( crc ^ data ) & 0x00FF ]; + return (crc >> 8) ^ CRC_TABLE[(crc ^ data) & 0x00FF]; } @@ -538,7 +538,8 @@ uint16_t qssp::sf_crc16( uint16_t crc, uint8_t data ) void qssp::sf_SetSendTimeout() { - uint32_t timeout; + uint32_t timeout; + timeout = thisport->pfGetTime() + thisport->timeoutLen; thisport->timeout = timeout; } @@ -552,32 +553,34 @@ void qssp::sf_SetSendTimeout() * \note * */ -uint16_t qssp::sf_CheckTimeout() +uint16_t qssp::sf_CheckTimeout() { - uint16_t retval = FALSE; - uint32_t current_time; + uint16_t retval = FALSE; + uint32_t current_time; current_time = thisport->pfGetTime(); - if( current_time > thisport->timeout ) { + if (current_time > thisport->timeout) { retval = TRUE; } - if(retval) - if (debug) - qDebug()<<"timeout "<timeout; + if (retval) { + if (debug) { + qDebug() << "timeout " << current_time << thisport->timeout; + } + } return retval; } /**************************************************************************** - * NAME: sf_ReceiveState - * DESC: Implements the receive state handling code for escaped and unescaped data - * ARGS: thisport - which port to operate on - * c - incoming byte - * RETURN: - * CREATED: - * NOTES: - * 1. change from using pointer to functions. - ****************************************************************************/ +* NAME: sf_ReceiveState +* DESC: Implements the receive state handling code for escaped and unescaped data +* ARGS: thisport - which port to operate on +* c - incoming byte +* RETURN: +* CREATED: +* NOTES: +* 1. change from using pointer to functions. +****************************************************************************/ /*! * \brief implements the receive state handling code for escaped and unescaped data * \param thisport = which port to use @@ -587,46 +590,46 @@ uint16_t qssp::sf_CheckTimeout() * \note * */ -int16_t qssp::sf_ReceiveState(uint8_t c ) +int16_t qssp::sf_ReceiveState(uint8_t c) { - int16_t retval = SSP_RX_RECEIVING; + int16_t retval = SSP_RX_RECEIVING; - switch( thisport->InputState ) { + switch (thisport->InputState) { case state_unescaped_e: - if( c == SYNC ) { + if (c == SYNC) { thisport->DecodeState = decode_len1_e; - } else if ( c == ESC ) { + } else if (c == ESC) { thisport->InputState = state_escaped_e; } else { retval = sf_DecodeState(c); } - break; // end of unescaped state. - case state_escaped_e: + break; // end of unescaped state. + case state_escaped_e: thisport->InputState = state_unescaped_e; - if( c == SYNC ) { + if (c == SYNC) { thisport->DecodeState = decode_len1_e; - } else if (c == ESC_SYNC ) { + } else if (c == ESC_SYNC) { retval = sf_DecodeState(SYNC); } else { retval = sf_DecodeState(c); } - break; // end of the escaped state. - default: + break; // end of the escaped state. + default: break; } return retval; } /**************************************************************************** - * NAME: sf_DecodeState - * DESC: Implements the receive state finite state machine - * ARGS: thisport - which port to operate on - * c - incoming byte - * RETURN: - * CREATED: - * NOTES: - * 1. change from using pointer to functions. - ****************************************************************************/ +* NAME: sf_DecodeState +* DESC: Implements the receive state finite state machine +* ARGS: thisport - which port to operate on +* c - incoming byte +* RETURN: +* CREATED: +* NOTES: +* 1. change from using pointer to functions. +****************************************************************************/ /*! * \brief implements the receiving decoding state machine @@ -637,19 +640,20 @@ int16_t qssp::sf_ReceiveState(uint8_t c ) * \note * */ -int16_t qssp::sf_DecodeState( uint8_t c ) +int16_t qssp::sf_DecodeState(uint8_t c) { - int16_t retval; - switch( thisport->DecodeState ) { + int16_t retval; + + switch (thisport->DecodeState) { case decode_idle_e: // 'c' is ignored in this state as the only way to leave the idle state is // recognition of the SYNC byte in the sf_ReceiveState function. retval = SSP_RX_IDLE; break; case decode_len1_e: - thisport->rxBuf[LENGTH]= c; + thisport->rxBuf[LENGTH] = c; thisport->rxBufLen = c; - if( thisport->rxBufLen <= thisport->rxBufSize ) { + if (thisport->rxBufLen <= thisport->rxBufSize) { thisport->DecodeState = decode_seqNo_e; retval = SSP_RX_RECEIVING; } else { @@ -657,37 +661,37 @@ int16_t qssp::sf_DecodeState( uint8_t c ) retval = SSP_RX_IDLE; } break; - case decode_seqNo_e: + case decode_seqNo_e: thisport->rxBuf[SEQNUM] = c; thisport->crc = 0xffff; - thisport->rxBufLen--; // subtract 1 for the seq. no. + thisport->rxBufLen--; // subtract 1 for the seq. no. thisport->rxBufPos = 2; - thisport->crc = sf_crc16( thisport->crc, c ); - if( thisport->rxBufLen > 0 ) { + thisport->crc = sf_crc16(thisport->crc, c); + if (thisport->rxBufLen > 0) { thisport->DecodeState = decode_data_e; } else { thisport->DecodeState = decode_crc1_e; } retval = SSP_RX_RECEIVING; break; - case decode_data_e: - thisport->rxBuf[ (thisport->rxBufPos)++] = c; - thisport->crc = sf_crc16( thisport->crc, c ); - if( thisport->rxBufPos == (thisport->rxBufLen+2) ) { + case decode_data_e: + thisport->rxBuf[(thisport->rxBufPos)++] = c; + thisport->crc = sf_crc16(thisport->crc, c); + if (thisport->rxBufPos == (thisport->rxBufLen + 2)) { thisport->DecodeState = decode_crc1_e; } retval = SSP_RX_RECEIVING; break; - case decode_crc1_e: - thisport->crc = sf_crc16( thisport->crc, c ); + case decode_crc1_e: + thisport->crc = sf_crc16(thisport->crc, c); thisport->DecodeState = decode_crc2_e; retval = SSP_RX_RECEIVING; break; - case decode_crc2_e: + case decode_crc2_e: thisport->DecodeState = decode_idle_e; // verify the CRC value for the packet - if( sf_crc16( thisport->crc, c) == 0) { + if (sf_crc16(thisport->crc, c) == 0) { // TODO shouldn't the return value of sf_ReceivePacket() be checked? sf_ReceivePacket(); retval = SSP_RX_COMPLETE; @@ -696,8 +700,8 @@ int16_t qssp::sf_DecodeState( uint8_t c ) retval = SSP_RX_IDLE; } break; - default: - thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet. + default: + thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet. retval = SSP_RX_IDLE; break; } @@ -705,18 +709,18 @@ int16_t qssp::sf_DecodeState( uint8_t c ) } /************************************************************************************************************ -* -* NAME: int16_t sf_ReceivePacket( ) -* DESCRIPTION: Receive one packet, assumed that data is in rec.buff[] -* ARGUMENTS: -* RETURN: 0 . no new packet was received, could be ack or same packet -* 1 . new packet received -* SSP_PACKET_? -* SSP_PACKET_COMPLETE -* SSP_PACKET_ACK -* CREATED: 5/8/02 -* -*************************************************************************************************************/ + * + * NAME: int16_t sf_ReceivePacket( ) + * DESCRIPTION: Receive one packet, assumed that data is in rec.buff[] + * ARGUMENTS: + * RETURN: 0 . no new packet was received, could be ack or same packet + * 1 . new packet received + * SSP_PACKET_? + * SSP_PACKET_COMPLETE + * SSP_PACKET_ACK + * CREATED: 5/8/02 + * + *************************************************************************************************************/ /*! * \brief receive one packet. calls the callback function if needed. * \param thisport = which port to use @@ -732,78 +736,81 @@ int16_t qssp::sf_ReceivePacket() { int16_t value = FALSE; - if( ISBITSET(thisport->rxBuf[SEQNUM], ACK_BIT ) ) { - // Received an ACK packet, need to check if it matches the previous sent packet - if( ( thisport->rxBuf[SEQNUM] & 0x7F) == (thisport->txSeqNo & 0x7f)) { - // It matches the last packet sent by us - SETBIT( thisport->txSeqNo, ACK_BIT ); + if (ISBITSET(thisport->rxBuf[SEQNUM], ACK_BIT)) { + // Received an ACK packet, need to check if it matches the previous sent packet + if ((thisport->rxBuf[SEQNUM] & 0x7F) == (thisport->txSeqNo & 0x7f)) { + // It matches the last packet sent by us + SETBIT(thisport->txSeqNo, ACK_BIT); thisport->SendState = SSP_ACKED; value = FALSE; - if (debug) - qDebug()<<"Received ACK:"<<(thisport->txSeqNo & 0x7F); + if (debug) { + qDebug() << "Received ACK:" << (thisport->txSeqNo & 0x7F); + } } // else ignore the ACK packet } else { - // Received a 'data' packet, figure out what type of packet we received... - if( thisport->rxBuf[SEQNUM] == 0 ) { - if (debug) - qDebug()<<"Received SYNC Request"; + // Received a 'data' packet, figure out what type of packet we received... + if (thisport->rxBuf[SEQNUM] == 0) { + if (debug) { + qDebug() << "Received SYNC Request"; + } // Synchronize sequence number with host #ifdef ACTIVE_SYNCH thisport->sendSynch = TRUE; #endif - sf_SendAckPacket(thisport->rxBuf[SEQNUM] ); - thisport->rxSeqNo = 0; + sf_SendAckPacket(thisport->rxBuf[SEQNUM]); + thisport->rxSeqNo = 0; value = FALSE; - } else if( thisport->rxBuf[SEQNUM] == thisport->rxSeqNo ) { + } else if (thisport->rxBuf[SEQNUM] == thisport->rxSeqNo) { // Already seen this packet, just ack it, don't act on the packet. - sf_SendAckPacket(thisport->rxBuf[SEQNUM] ); + sf_SendAckPacket(thisport->rxBuf[SEQNUM]); value = FALSE; } else { - //New Packet + // New Packet thisport->rxSeqNo = thisport->rxBuf[SEQNUM]; // Let the application do something with the data/packet. // skip the first two bytes (length and seq. no.) in the buffer. - if (debug) - qDebug()<<"Received DATA PACKET seq="<rxSeqNo<<"Data="<<(uint8_t)thisport->rxBuf[2]<<(uint8_t)thisport->rxBuf[3]<<(uint8_t)thisport->rxBuf[4]; - pfCallBack( &(thisport->rxBuf[2]), thisport->rxBufLen); + if (debug) { + qDebug() << "Received DATA PACKET seq=" << thisport->rxSeqNo << "Data=" << (uint8_t)thisport->rxBuf[2] << (uint8_t)thisport->rxBuf[3] << (uint8_t)thisport->rxBuf[4]; + } + pfCallBack(&(thisport->rxBuf[2]), thisport->rxBufLen); // after we send the ACK, it is possible for the host to send a new packet. // Thus the application needs to copy the data and reset the receive buffer // inside of thisport->pfCallBack() - sf_SendAckPacket(thisport->rxBuf[SEQNUM] ); + sf_SendAckPacket(thisport->rxBuf[SEQNUM]); value = TRUE; } } return value; } -qssp::qssp(port * info,bool debug):debug(debug) +qssp::qssp(port *info, bool debug) : debug(debug) { - - thisport=info; - thisport->maxRetryCount = info->max_retry; - thisport->timeoutLen = info->timeoutLen; + thisport = info; + thisport->maxRetryCount = info->max_retry; + thisport->timeoutLen = info->timeoutLen; thisport->txBufSize = info->txBufSize; - thisport->rxBufSize = info->rxBufSize; - thisport->txBuf = info->txBuf; - thisport->rxBuf = info->rxBuf; + thisport->rxBufSize = info->rxBufSize; + thisport->txBuf = info->txBuf; + thisport->rxBuf = info->rxBuf; thisport->retryCount = 0; - thisport->sendSynch = FALSE; //TRUE; - thisport->rxSeqNo = 255; - thisport->txSeqNo = 255; - thisport->SendState = SSP_IDLE; - thisport->InputState =(ReceiveState)0; - thisport->DecodeState =(decodeState_)0; - thisport->TxError =0; - thisport->RxError =0; - thisport->txSeqNo =0; - thisport->rxSeqNo =0; + thisport->sendSynch = FALSE; // TRUE; + thisport->rxSeqNo = 255; + thisport->txSeqNo = 255; + thisport->SendState = SSP_IDLE; + thisport->InputState = (ReceiveState)0; + thisport->DecodeState = (decodeState_)0; + thisport->TxError = 0; + thisport->RxError = 0; + thisport->txSeqNo = 0; + thisport->rxSeqNo = 0; } -void qssp::pfCallBack( uint8_t * buf, uint16_t size) +void qssp::pfCallBack(uint8_t *buf, uint16_t size) { Q_UNUSED(size); - if (debug) - qDebug()<<"receive callback"<ssp_ReceiveProcess(); - sendstatus=this->ssp_SendProcess(); + while (!endthread) { + receivestatus = this->ssp_ReceiveProcess(); + sendstatus = this->ssp_SendProcess(); msleep(1); sendbufmutex.lock(); - if(datapending && receivestatus==SSP_TX_IDLE) - { - this->ssp_SendData(mbuf,msize); - datapending=false; + if (datapending && receivestatus == SSP_TX_IDLE) { + this->ssp_SendData(mbuf, msize); + datapending = false; } sendbufmutex.unlock(); - if(sendstatus==SSP_TX_ACKED) + if (sendstatus == SSP_TX_ACKED) { sendwait.wakeAll(); + } } - } -bool qsspt::sendData(uint8_t * buf,uint16_t size) +bool qsspt::sendData(uint8_t *buf, uint16_t size) { - if(datapending) + if (datapending) { return false; + } sendbufmutex.lock(); - datapending=true; - mbuf=buf; - msize=size; + datapending = true; + mbuf = buf; + msize = size; sendbufmutex.unlock(); msendwait.lock(); - sendwait.wait(&msendwait,10000); + sendwait.wait(&msendwait, 10000); msendwait.unlock(); return true; } -void qsspt::pfCallBack( uint8_t * buf, uint16_t size) +void qsspt::pfCallBack(uint8_t *buf, uint16_t size) { - if (debug) - qDebug()<<"receive callback"< -class delay : public QThread -{ +class delay : public QThread { public: static void msleep(unsigned long msecs) { diff --git a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp index f2e7b9c73..0cb91125a 100644 --- a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp @@ -53,10 +53,10 @@ void DeviceWidget::showEvent(QShowEvent *event) // Thit fitInView method should only be called now, once the // widget is shown, otherwise it cannot compute its values and // the result is usually a ahrsbargraph that is way too small - myDevice->gVDevice->fitInView(devicePic.rect(),Qt::KeepAspectRatio); + myDevice->gVDevice->fitInView(devicePic.rect(), Qt::KeepAspectRatio); } -void DeviceWidget::resizeEvent(QResizeEvent* event) +void DeviceWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); myDevice->gVDevice->fitInView(devicePic.rect(), Qt::KeepAspectRatio); @@ -73,17 +73,18 @@ void DeviceWidget::setDfu(DFUObject *dfu) } /** - Fills the various fields for the device - */ + Fills the various fields for the device + */ void DeviceWidget::populate() { int id = m_dfu->devices[deviceID].ID; + myDevice->lbldevID->setText(QString("Device ID: ") + QString::number(id, 16)); // DeviceID tells us what sort of HW we have detected: // display a nice icon: myDevice->gVDevice->scene()->clear(); myDevice->lblDevName->setText(deviceDescriptorStruct::idToBoardName(id)); - myDevice->lblHWRev->setText(QString(tr("HW Revision: "))+QString::number(id & 0x00FF, 16)); + myDevice->lblHWRev->setText(QString(tr("HW Revision: ")) + QString::number(id & 0x00FF, 16)); switch (id) { case 0x0101: @@ -109,24 +110,24 @@ void DeviceWidget::populate() } myDevice->gVDevice->scene()->addPixmap(devicePic); myDevice->gVDevice->setSceneRect(devicePic.rect()); - myDevice->gVDevice->fitInView(devicePic.rect(),Qt::KeepAspectRatio); + myDevice->gVDevice->fitInView(devicePic.rect(), Qt::KeepAspectRatio); bool r = m_dfu->devices[deviceID].Readable; bool w = m_dfu->devices[deviceID].Writable; myDevice->lblAccess->setText(QString("Flash access: ") + QString(r ? "R" : "-") + QString(w ? "W" : "-")); - myDevice->lblMaxCode->setText(QString("Max code size: ") +QString::number(m_dfu->devices[deviceID].SizeOfCode)); + myDevice->lblMaxCode->setText(QString("Max code size: ") + QString::number(m_dfu->devices[deviceID].SizeOfCode)); myDevice->lblCRC->setText(QString::number(m_dfu->devices[deviceID].FW_CRC)); myDevice->lblBLVer->setText(QString("BL version: ") + QString::number(m_dfu->devices[deviceID].BL_Version)); - int size=((OP_DFU::device)m_dfu->devices[deviceID]).SizeOfDesc; + int size = ((OP_DFU::device)m_dfu->devices[deviceID]).SizeOfDesc; m_dfu->enterDFU(deviceID); QByteArray desc = m_dfu->DownloadDescriptionAsBA(size); - if (! populateBoardStructuredDescription(desc)) { - //TODO + if (!populateBoardStructuredDescription(desc)) { + // TODO // desc was not a structured description QString str = m_dfu->DownloadDescription(size); - myDevice->lblDescription->setText(QString("Firmware custom description: ")+str.left(str.indexOf(QChar(255)))); + myDevice->lblDescription->setText(QString("Firmware custom description: ") + str.left(str.indexOf(QChar(255)))); QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); myDevice->lblCertified->setPixmap(pix); myDevice->lblCertified->setToolTip(tr("Custom Firmware Build")); @@ -139,9 +140,9 @@ void DeviceWidget::populate() } /** - Freezes the contents of the widget so that a user cannot - try to modify the contents - */ + Freezes the contents of the widget so that a user cannot + try to modify the contents + */ void DeviceWidget::freeze() { updateButtons(false); @@ -155,8 +156,7 @@ void DeviceWidget::updateButtons(bool enabled) myDevice->confirmCheckBox->setEnabled(false); myDevice->updateButton->setEnabled(false); myDevice->retrieveButton->setEnabled(false); - } - else { + } else { myDevice->description->setEnabled(true); // Load button (i.e. choose file) is always enabled myDevice->pbLoad->setEnabled(true); @@ -169,22 +169,20 @@ void DeviceWidget::updateButtons(bool enabled) } /** - Populates the widget field with the description in case - it is structured properly - */ + Populates the widget field with the description in case + it is structured properly + */ bool DeviceWidget::populateBoardStructuredDescription(QByteArray desc) { - if(UAVObjectUtilManager::descriptionToStructure(desc,onBoardDescription)) { + if (UAVObjectUtilManager::descriptionToStructure(desc, onBoardDescription)) { myDevice->lblGitTag->setText(onBoardDescription.gitHash); - myDevice->lblBuildDate->setText(onBoardDescription.gitDate.insert(4,"-").insert(7,"-")); - if(onBoardDescription.gitTag.startsWith("RELEASE",Qt::CaseSensitive)) { + myDevice->lblBuildDate->setText(onBoardDescription.gitDate.insert(4, "-").insert(7, "-")); + if (onBoardDescription.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { myDevice->lblDescription->setText(onBoardDescription.gitTag); QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); myDevice->lblCertified->setPixmap(pix); myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build")); - - } - else { + } else { myDevice->lblDescription->setText(onBoardDescription.gitTag); QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); myDevice->lblCertified->setPixmap(pix); @@ -201,10 +199,10 @@ bool DeviceWidget::populateBoardStructuredDescription(QByteArray desc) bool DeviceWidget::populateLoadedStructuredDescription(QByteArray desc) { - if(UAVObjectUtilManager::descriptionToStructure(desc,LoadedDescription)) { + if (UAVObjectUtilManager::descriptionToStructure(desc, LoadedDescription)) { myDevice->lblGitTagL->setText(LoadedDescription.gitHash); - myDevice->lblBuildDateL->setText( LoadedDescription.gitDate.insert(4, "-").insert(7, "-")); - if(LoadedDescription.gitTag.startsWith("RELEASE",Qt::CaseSensitive)) { + myDevice->lblBuildDateL->setText(LoadedDescription.gitDate.insert(4, "-").insert(7, "-")); + if (LoadedDescription.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { myDevice->lblDescritpionL->setText(LoadedDescription.gitTag); myDevice->description->setText(LoadedDescription.gitTag); QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); @@ -225,8 +223,8 @@ bool DeviceWidget::populateLoadedStructuredDescription(QByteArray desc) } /** - Updates status message for messages coming from DFU - */ + Updates status message for messages coming from DFU + */ void DeviceWidget::dfuStatus(QString str) { status(str, STATUSICON_RUNNING); @@ -238,11 +236,12 @@ void DeviceWidget::confirmCB(int value) } /** - Updates status message - */ + Updates status message + */ void DeviceWidget::status(QString str, StatusIcon ic) { QPixmap px; + myDevice->statusLabel->setText(str); switch (ic) { case STATUSICON_RUNNING: @@ -285,13 +284,13 @@ void DeviceWidget::loadFirmware() QByteArray desc = loadedFW.right(100); QPixmap px; - if (loadedFW.length()>m_dfu->devices[deviceID].SizeOfCode) { + if (loadedFW.length() > m_dfu->devices[deviceID].SizeOfCode) { myDevice->lblCRCL->setText(tr("Can't calculate, file too big for device")); } else { - myDevice->lblCRCL->setText( QString::number(DFUObject::CRCFromQBArray(loadedFW,m_dfu->devices[deviceID].SizeOfCode))); + myDevice->lblCRCL->setText(QString::number(DFUObject::CRCFromQBArray(loadedFW, m_dfu->devices[deviceID].SizeOfCode))); } - //myDevice->lblFirmwareSizeL->setText(QString("Firmware size: ")+QVariant(loadedFW.length()).toString()+ QString(" bytes")); + // myDevice->lblFirmwareSizeL->setText(QString("Firmware size: ")+QVariant(loadedFW.length()).toString()+ QString(" bytes")); if (populateLoadedStructuredDescription(desc)) { myDevice->confirmCheckBox->setChecked(true); myDevice->verticalGroupBox_loaded->setVisible(true); @@ -324,8 +323,8 @@ void DeviceWidget::loadFirmware() } /** - Sends a firmware to the device - */ + Sends a firmware to the device + */ void DeviceWidget::uploadFirmware() { // clear progress bar now @@ -339,9 +338,9 @@ void DeviceWidget::uploadFirmware() return; } - bool verify = false; + bool verify = false; /* TODO: does not work properly on current Bootloader! - if (m_dfu->devices[deviceID].Readable) + if (m_dfu->devices[deviceID].Readable) verify = true; */ @@ -351,11 +350,10 @@ void DeviceWidget::uploadFirmware() // Now do sanity checking: // - Check whether board type matches firmware: int board = m_dfu->devices[deviceID].ID; - int firmwareBoard = ((desc.at(12)&0xff)<<8) + (desc.at(13)&0xff); - if((board == 0x401 && firmwareBoard == 0x402) || - (board == 0x901 && firmwareBoard == 0x902) || // L3GD20 revo supports Revolution firmware - (board == 0x902 && firmwareBoard == 0x903)) // RevoMini1 supporetd by RevoMini2 firmware - { + int firmwareBoard = ((desc.at(12) & 0xff) << 8) + (desc.at(13) & 0xff); + if ((board == 0x401 && firmwareBoard == 0x402) || + (board == 0x901 && firmwareBoard == 0x902) || // L3GD20 revo supports Revolution firmware + (board == 0x902 && firmwareBoard == 0x903)) { // RevoMini1 supporetd by RevoMini2 firmware // These firmwares are designed to be backwards compatible } else if (firmwareBoard != board) { status("Error: firmware does not match board", STATUSICON_FAIL); @@ -363,8 +361,8 @@ void DeviceWidget::uploadFirmware() return; } // Check the firmware embedded in the file: - QByteArray firmwareHash = desc.mid(40,20); - QByteArray fileHash = QCryptographicHash::hash(loadedFW.left(loadedFW.length()-100), QCryptographicHash::Sha1); + QByteArray firmwareHash = desc.mid(40, 20); + QByteArray fileHash = QCryptographicHash::hash(loadedFW.left(loadedFW.length() - 100), QCryptographicHash::Sha1); if (firmwareHash != fileHash) { status("Error: firmware file corrupt", STATUSICON_FAIL); updateButtons(true); @@ -381,14 +379,13 @@ void DeviceWidget::uploadFirmware() // We don't know which device was used previously, so we // are cautious and reenter DFU for this deviceID: - if(!m_dfu->enterDFU(deviceID)) - { + if (!m_dfu->enterDFU(deviceID)) { emit uploadEnded(false); status("Error:Could not enter DFU mode", STATUSICON_FAIL); updateButtons(true); return; } - OP_DFU::Status ret=m_dfu->StatusRequest(); + OP_DFU::Status ret = m_dfu->StatusRequest(); qDebug() << m_dfu->StatusToString(ret); m_dfu->AbortOperation(); // Necessary, otherwise I get random failures. @@ -407,8 +404,8 @@ void DeviceWidget::uploadFirmware() } /** - Retrieves the firmware from the device - */ + Retrieves the firmware from the device + */ void DeviceWidget::downloadFirmware() { // clear progress bar now @@ -437,7 +434,7 @@ void DeviceWidget::downloadFirmware() connect(m_dfu, SIGNAL(downloadFinished()), this, SLOT(downloadFinished())); downloadedFirmware.clear(); // Empty the byte array - bool ret = m_dfu->DownloadFirmware(&downloadedFirmware,deviceID); + bool ret = m_dfu->DownloadFirmware(&downloadedFirmware, deviceID); if (!ret) { emit downloadEnded(false); @@ -447,12 +444,11 @@ void DeviceWidget::downloadFirmware() } status("Downloading, please wait...", STATUSICON_RUNNING); - return; } /** - Callback for the firmware download result - */ + Callback for the firmware download result + */ void DeviceWidget::downloadFinished() { disconnect(m_dfu, SIGNAL(downloadFinished()), this, SLOT(downloadFinished())); @@ -467,8 +463,8 @@ void DeviceWidget::downloadFinished() } /** - Callback for the firmware upload result - */ + Callback for the firmware upload result + */ void DeviceWidget::uploadFinished(OP_DFU::Status retstatus) { disconnect(m_dfu, SIGNAL(uploadFinished(OP_DFU::Status)), this, SLOT(uploadFinished(OP_DFU::Status))); @@ -480,31 +476,29 @@ void DeviceWidget::uploadFinished(OP_DFU::Status retstatus) status(QString("Upload failed with code: ") + m_dfu->StatusToString(retstatus).toLatin1().data(), STATUSICON_FAIL); updateButtons(true); return; - } else - if (!descriptionArray.isEmpty()) { - // We have a structured array to save - status(QString("Updating description"), STATUSICON_RUNNING); - repaint(); // Make sure the text above shows right away - retstatus = m_dfu->UploadDescription(descriptionArray); - if (retstatus != OP_DFU::Last_operation_Success) { - emit uploadEnded(false); - status(QString("Upload failed with code: ") + m_dfu->StatusToString(retstatus).toLatin1().data(), STATUSICON_FAIL); - updateButtons(true); - return; - } - - } else if (!myDevice->description->text().isEmpty()) { - // Fallback: we save the description field: - status(QString("Updating description"), STATUSICON_RUNNING); - repaint(); // Make sure the text above shows right away - retstatus = m_dfu->UploadDescription(myDevice->description->text()); - if (retstatus != OP_DFU::Last_operation_Success) { - emit uploadEnded(false); - status(QString("Upload failed with code: ") + m_dfu->StatusToString(retstatus).toLatin1().data(), STATUSICON_FAIL); - updateButtons(true); - return; - } + } else if (!descriptionArray.isEmpty()) { + // We have a structured array to save + status(QString("Updating description"), STATUSICON_RUNNING); + repaint(); // Make sure the text above shows right away + retstatus = m_dfu->UploadDescription(descriptionArray); + if (retstatus != OP_DFU::Last_operation_Success) { + emit uploadEnded(false); + status(QString("Upload failed with code: ") + m_dfu->StatusToString(retstatus).toLatin1().data(), STATUSICON_FAIL); + updateButtons(true); + return; } + } else if (!myDevice->description->text().isEmpty()) { + // Fallback: we save the description field: + status(QString("Updating description"), STATUSICON_RUNNING); + repaint(); // Make sure the text above shows right away + retstatus = m_dfu->UploadDescription(myDevice->description->text()); + if (retstatus != OP_DFU::Last_operation_Success) { + emit uploadEnded(false); + status(QString("Upload failed with code: ") + m_dfu->StatusToString(retstatus).toLatin1().data(), STATUSICON_FAIL); + updateButtons(true); + return; + } + } populate(); @@ -514,15 +508,15 @@ void DeviceWidget::uploadFinished(OP_DFU::Status retstatus) } /** - Slot to update the progress bar - */ + Slot to update the progress bar + */ void DeviceWidget::setProgress(int percent) { myDevice->progressBar->setValue(percent); } /** - *Opens an open file dialog. + * Opens an open file dialog. */ QString DeviceWidget::setOpenFileName() { @@ -531,25 +525,25 @@ QString DeviceWidget::setOpenFileName() QString fwDirectoryStr; QDir fwDirectory; - //Format filename for file chooser + // Format filename for file chooser #ifdef Q_OS_WIN - fwDirectoryStr=QCoreApplication::applicationDirPath(); - fwDirectory=QDir(fwDirectoryStr); - fwDirectory.cdUp(); - fwDirectory.cd("firmware"); - fwDirectoryStr=fwDirectory.absolutePath(); + fwDirectoryStr = QCoreApplication::applicationDirPath(); + fwDirectory = QDir(fwDirectoryStr); + fwDirectory.cdUp(); + fwDirectory.cd("firmware"); + fwDirectoryStr = fwDirectory.absolutePath(); #elif defined Q_OS_LINUX - fwDirectoryStr=QCoreApplication::applicationDirPath(); - fwDirectory=QDir(fwDirectoryStr); + fwDirectoryStr = QCoreApplication::applicationDirPath(); + fwDirectory = QDir(fwDirectoryStr); fwDirectory.cd("../../.."); - fwDirectoryStr=fwDirectory.absolutePath(); - fwDirectoryStr=fwDirectoryStr+"/fw_"+myDevice->lblBrdName->text().toLower()+"/fw_"+myDevice->lblBrdName->text().toLower()+".opfw"; + fwDirectoryStr = fwDirectory.absolutePath(); + fwDirectoryStr = fwDirectoryStr + "/fw_" + myDevice->lblBrdName->text().toLower() + "/fw_" + myDevice->lblBrdName->text().toLower() + ".opfw"; #elif defined Q_OS_MAC - fwDirectoryStr=QCoreApplication::applicationDirPath(); - fwDirectory=QDir(fwDirectoryStr); + fwDirectoryStr = QCoreApplication::applicationDirPath(); + fwDirectory = QDir(fwDirectoryStr); fwDirectory.cd("../../../../../.."); - fwDirectoryStr=fwDirectory.absolutePath(); - fwDirectoryStr=fwDirectoryStr+"/fw_"+myDevice->lblBrdName->text().toLower()+"/fw_"+myDevice->lblBrdName->text().toLower()+".opfw"; + fwDirectoryStr = fwDirectory.absolutePath(); + fwDirectoryStr = fwDirectoryStr + "/fw_" + myDevice->lblBrdName->text().toLower() + "/fw_" + myDevice->lblBrdName->text().toLower() + ".opfw"; #endif QString fileName = QFileDialog::getOpenFileName(this, tr("Select firmware file"), @@ -561,7 +555,7 @@ QString DeviceWidget::setOpenFileName() } /** - *Set the save file name + * Set the save file name */ QString DeviceWidget::setSaveFileName() { @@ -573,5 +567,6 @@ QString DeviceWidget::setSaveFileName() tr("Firmware Files (*.bin)"), &selectedFilter, options); + return fileName; } diff --git a/ground/openpilotgcs/src/plugins/uploader/devicewidget.h b/ground/openpilotgcs/src/plugins/uploader/devicewidget.h index b87988c16..489b81168 100644 --- a/ground/openpilotgcs/src/plugins/uploader/devicewidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/devicewidget.h @@ -45,16 +45,15 @@ #include "uploader_global.h" using namespace OP_DFU; -class UPLOADER_EXPORT DeviceWidget : public QWidget -{ +class UPLOADER_EXPORT DeviceWidget : public QWidget { Q_OBJECT public: DeviceWidget(QWidget *parent = 0); void setDeviceID(int devID); - void setDfu(DFUObject* dfu); + void setDfu(DFUObject *dfu); void populate(); void freeze(); - typedef enum { STATUSICON_OK, STATUSICON_RUNNING, STATUSICON_FAIL, STATUSICON_INFO} StatusIcon; + typedef enum { STATUSICON_OK, STATUSICON_RUNNING, STATUSICON_FAIL, STATUSICON_INFO } StatusIcon; QString setOpenFileName(); QString setSaveFileName(); @@ -93,7 +92,6 @@ public slots: protected: void showEvent(QShowEvent *event); void resizeEvent(QResizeEvent *event); - }; #endif // DEVICEWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/uploader/enums.h b/ground/openpilotgcs/src/plugins/uploader/enums.h index 2c89bdf1d..2b3f540e7 100644 --- a/ground/openpilotgcs/src/plugins/uploader/enums.h +++ b/ground/openpilotgcs/src/plugins/uploader/enums.h @@ -27,9 +27,8 @@ #ifndef ENUMS_H #define ENUMS_H -namespace uploader -{ - typedef enum { IAP_STATE_READY, IAP_STATE_STEP_1, IAP_STATE_STEP_2, IAP_STEP_RESET, IAP_STATE_BOOTLOADER} IAPStep; - typedef enum { WAITING_DISCONNECT, WAITING_CONNECT, JUMP_TO_BL, LOADING_FW, UPLOADING_FW, UPLOADING_DESC, BOOTING, SUCCESS, FAILURE} AutoUpdateStep; +namespace uploader { +typedef enum { IAP_STATE_READY, IAP_STATE_STEP_1, IAP_STATE_STEP_2, IAP_STEP_RESET, IAP_STATE_BOOTLOADER } IAPStep; +typedef enum { WAITING_DISCONNECT, WAITING_CONNECT, JUMP_TO_BL, LOADING_FW, UPLOADING_FW, UPLOADING_DESC, BOOTING, SUCCESS, FAILURE } AutoUpdateStep; } #endif // ENUMS_H diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp index 921473b7e..67a0a080f 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp @@ -33,44 +33,42 @@ using namespace OP_DFU; -DFUObject::DFUObject(bool _debug,bool _use_serial,QString portname): - debug(_debug),use_serial(_use_serial),mready(true) +DFUObject::DFUObject(bool _debug, bool _use_serial, QString portname) : + debug(_debug), use_serial(_use_serial), mready(true) { info = NULL; numberOfDevices = 0; qRegisterMetaType("Status"); - if(use_serial) - { + if (use_serial) { PortSettings settings; - settings.BaudRate=BAUD57600; - settings.DataBits=DATA_8; - settings.FlowControl=FLOW_OFF; - settings.Parity=PAR_NONE; - settings.StopBits=STOP_1; - settings.Timeout_Millisec=1000; - info = new port(settings,portname); - info->rxBuf = sspRxBuf; - info->rxBufSize = MAX_PACKET_DATA_LEN; - info->txBuf = sspTxBuf; - info->txBufSize = MAX_PACKET_DATA_LEN; - info->max_retry = 10; - info->timeoutLen = 1000; - if(info->status()!=port::open) - { + settings.BaudRate = BAUD57600; + settings.DataBits = DATA_8; + settings.FlowControl = FLOW_OFF; + settings.Parity = PAR_NONE; + settings.StopBits = STOP_1; + settings.Timeout_Millisec = 1000; + info = new port(settings, portname); + info->rxBuf = sspRxBuf; + info->rxBufSize = MAX_PACKET_DATA_LEN; + info->txBuf = sspTxBuf; + info->txBufSize = MAX_PACKET_DATA_LEN; + info->max_retry = 10; + info->timeoutLen = 1000; + if (info->status() != port::open) { cout << "Could not open serial port\n"; mready = false; return; } - serialhandle = new qsspt(info,debug); + serialhandle = new qsspt(info, debug); int count = 0; - while((serialhandle->ssp_Synchronise()==false) && (count < 10)) - { - if (debug) - qDebug()<<"SYNC failed, resending"; - count++; + while ((serialhandle->ssp_Synchronise() == false) && (count < 10)) { + if (debug) { + qDebug() << "SYNC failed, resending"; + } + count++; } if (count == 10) { mready = false; @@ -78,56 +76,54 @@ DFUObject::DFUObject(bool _debug,bool _use_serial,QString portname): } qDebug() << "SYNC Succeded"; serialhandle->start(); - } - else - { + } else { mready = false; QEventLoop m_eventloop; - QTimer::singleShot(200,&m_eventloop, SLOT(quit())); + QTimer::singleShot(200, &m_eventloop, SLOT(quit())); m_eventloop.exec(); QList devices; - devices = USBMonitor::instance()->availableDevices(0x20a0,-1,-1,USBMonitor::Bootloader); - if (devices.length()==1) { - if (hidHandle.open(1,devices.first().vendorID,devices.first().productID,0,0)==1) { - mready=true; - QTimer::singleShot(200,&m_eventloop, SLOT(quit())); + devices = USBMonitor::instance()->availableDevices(0x20a0, -1, -1, USBMonitor::Bootloader); + if (devices.length() == 1) { + if (hidHandle.open(1, devices.first().vendorID, devices.first().productID, 0, 0) == 1) { + mready = true; + QTimer::singleShot(200, &m_eventloop, SLOT(quit())); m_eventloop.exec(); - } else + } else { hidHandle.close(0); + } } else { // Wait for the board to appear on the USB bus: - USBSignalFilter filter(0x20a0,-1,-1,USBMonitor::Bootloader); - connect(&filter, SIGNAL(deviceDiscovered()),&m_eventloop, SLOT(quit())); - for(int x=0;x<4;++x) - { - qDebug()<<"OP_DFU trying to detect bootloader:"<availableDevices(0x20a0,-1,-1,USBMonitor::Bootloader); + devices = USBMonitor::instance()->availableDevices(0x20a0, -1, -1, USBMonitor::Bootloader); qDebug() << "Devices length: " << devices.length(); - if (devices.length()==1) { + if (devices.length() == 1) { qDebug() << "Opening device"; - if(hidHandle.open(1,devices.first().vendorID,devices.first().productID,0,0)==1) - { - QTimer::singleShot(200,&m_eventloop, SLOT(quit())); + if (hidHandle.open(1, devices.first().vendorID, devices.first().productID, 0, 0) == 1) { + QTimer::singleShot(200, &m_eventloop, SLOT(quit())); m_eventloop.exec(); - qDebug()<<"OP_DFU detected after delay"; - mready=true; + qDebug() << "OP_DFU detected after delay"; + mready = true; qDebug() << "Detected"; break; - } else + } else { hidHandle.close(0); + } } else { - qDebug() << devices.length() << " device(s) detected, don't know what to do!"; + qDebug() << devices.length() << " device(s) detected, don't know what to do!"; mready = false; } } } - } } @@ -141,16 +137,16 @@ DFUObject::~DFUObject() } else { hidHandle.close(0); } - } bool DFUObject::SaveByteArrayToFile(QString const & sfile, const QByteArray &array) { QFile file(sfile); - if (!file.open(QIODevice::WriteOnly)) - { - if(debug) - qDebug()<<"Can't open file"; + + if (!file.open(QIODevice::WriteOnly)) { + if (debug) { + qDebug() << "Can't open file"; + } return false; } file.write(array); @@ -159,72 +155,74 @@ bool DFUObject::SaveByteArrayToFile(QString const & sfile, const QByteArray &arr } /** - Tells the mainboard to enter DFU Mode. - */ + Tells the mainboard to enter DFU Mode. + */ bool DFUObject::enterDFU(int const &devNumber) { char buf[BUF_LEN]; - buf[0] =0x02; //reportID - buf[1] = OP_DFU::EnterDFU; //DFU Command - buf[2] = 0; //DFU Count - buf[3] = 0; //DFU Count - buf[4] = 0; //DFU Count - buf[5] = 0; //DFU Count - buf[6] = devNumber; //DFU Data0 - buf[7] = 1; //DFU Data1 - buf[8] = 1; //DFU Data2 - buf[9] = 1; //DFU Data3 + + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::EnterDFU; // DFU Command + buf[2] = 0; // DFU Count + buf[3] = 0; // DFU Count + buf[4] = 0; // DFU Count + buf[5] = 0; // DFU Count + buf[6] = devNumber; // DFU Data0 + buf[7] = 1; // DFU Data1 + buf[8] = 1; // DFU Data2 + buf[9] = 1; // DFU Data3 int result = sendData(buf, BUF_LEN); - if(result<1) + if (result < 1) { return false; - if(debug) + } + if (debug) { qDebug() << "EnterDFU: " << result << " bytes sent"; + } return true; } /** - Tells the board to get ready for an upload. It will in particular - erase the memory to make room for the data. You will have to query - its status to wait until erase is done before doing the actual upload. - */ -bool DFUObject::StartUpload(qint32 const & numberOfBytes, TransferTypes const & type,quint32 crc) + Tells the board to get ready for an upload. It will in particular + erase the memory to make room for the data. You will have to query + its status to wait until erase is done before doing the actual upload. + */ +bool DFUObject::StartUpload(qint32 const & numberOfBytes, TransferTypes const & type, quint32 crc) { int lastPacketCount; - qint32 numberOfPackets=numberOfBytes/4/14; - int pad=(numberOfBytes-numberOfPackets*4*14)/4; - if(pad==0) - { - lastPacketCount=14; - } - else - { + qint32 numberOfPackets = numberOfBytes / 4 / 14; + int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4; + + if (pad == 0) { + lastPacketCount = 14; + } else { ++numberOfPackets; - lastPacketCount=pad; + lastPacketCount = pad; } char buf[BUF_LEN]; - buf[0] =0x02;//reportID - buf[1] = setStartBit(OP_DFU::Upload);//DFU Command - buf[2] = numberOfPackets>>24;//DFU Count - buf[3] = numberOfPackets>>16;//DFU Count - buf[4] = numberOfPackets>>8;//DFU Count - buf[5] = numberOfPackets;//DFU Count - buf[6] = (int)type;//DFU Data0 - buf[7] = lastPacketCount;//DFU Data1 - buf[8] = crc>>24; - buf[9] = crc>>16; - buf[10] = crc>>8; + buf[0] = 0x02; // reportID + buf[1] = setStartBit(OP_DFU::Upload); // DFU Command + buf[2] = numberOfPackets >> 24; // DFU Count + buf[3] = numberOfPackets >> 16; // DFU Count + buf[4] = numberOfPackets >> 8; // DFU Count + buf[5] = numberOfPackets; // DFU Count + buf[6] = (int)type; // DFU Data0 + buf[7] = lastPacketCount; // DFU Data1 + buf[8] = crc >> 24; + buf[9] = crc >> 16; + buf[10] = crc >> 8; buf[11] = crc; - if(debug) - qDebug()<<"Number of packets:"<0) - { + } + if (result > 0) { return true; } return false; @@ -232,154 +230,148 @@ bool DFUObject::StartUpload(qint32 const & numberOfBytes, TransferTypes const & /** - Does the actual data upload to the board. Needs to be called once the - board is ready to accept data following a StartUpload command, and it is erased. - */ -bool DFUObject::UploadData(qint32 const & numberOfBytes, QByteArray & data) + Does the actual data upload to the board. Needs to be called once the + board is ready to accept data following a StartUpload command, and it is erased. + */ +bool DFUObject::UploadData(qint32 const & numberOfBytes, QByteArray & data) { int lastPacketCount; - qint32 numberOfPackets=numberOfBytes/4/14; - int pad=(numberOfBytes-numberOfPackets*4*14)/4; - if(pad==0) - { - lastPacketCount=14; - } - else - { + qint32 numberOfPackets = numberOfBytes / 4 / 14; + int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4; + + if (pad == 0) { + lastPacketCount = 14; + } else { ++numberOfPackets; - lastPacketCount=pad; + lastPacketCount = pad; + } + if (debug) { + qDebug() << "Start Uploading:" << numberOfPackets << "4Bytes"; } - if(debug) - qDebug()<<"Start Uploading:"<>24;//DFU Count - buf[3] = packetcount>>16;//DFU Count - buf[4] = packetcount>>8;//DFU Count - buf[5] = packetcount;//DFU Count - char *pointer=data.data(); - pointer=pointer+4*14*packetcount; - // qDebug()<<"Packet Number="<> 24; // DFU Count + buf[3] = packetcount >> 16; // DFU Count + buf[4] = packetcount >> 8; // DFU Count + buf[5] = packetcount; // DFU Count + char *pointer = data.data(); + pointer = pointer + 4 * 14 * packetcount; + // qDebug()<<"Packet Number="<append(buf+6,size); + result = receiveData(buf, BUF_LEN); + if (debug) { + qDebug() << result << " bytes received" << " Count=" << x << "-" << (int)buf[2] << ";" << (int)buf[3] << ";" << (int)buf[4] << ";" << (int)buf[5] << " Data=" << (int)buf[6] << ";" << (int)buf[7] << ";" << (int)buf[8] << ";" << (int)buf[9]; + } + if (x == numberOfPackets - 1) { + size = lastPacketCount * 4; + } else { + size = 14 * 4; + } + fw->append(buf + 6, size); } StatusRequest(); @@ -474,13 +467,14 @@ bool DFUObject::StartDownloadT(QByteArray *fw, qint32 const & numberOfBytes, Tra /** - Resets the device - */ + Resets the device + */ int DFUObject::ResetDevice(void) { char buf[BUF_LEN]; - buf[0] =0x02; //reportID - buf[1] = OP_DFU::Reset; //DFU Command + + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::Reset; // DFU Command buf[2] = 0; buf[3] = 0; buf[4] = 0; @@ -491,14 +485,15 @@ int DFUObject::ResetDevice(void) buf[9] = 0; return sendData(buf, BUF_LEN); - //return hidHandle.send(0,buf, BUF_LEN, 500); + // return hidHandle.send(0,buf, BUF_LEN, 500); } int DFUObject::AbortOperation(void) { char buf[BUF_LEN]; - buf[0] =0x02;//reportID - buf[1] = OP_DFU::Abort_Operation;//DFU Command + + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::Abort_Operation; // DFU Command buf[2] = 0; buf[3] = 0; buf[4] = 0; @@ -511,13 +506,14 @@ int DFUObject::AbortOperation(void) return sendData(buf, BUF_LEN); } /** - Starts the firmware (leaves bootloader and boots the main software) - */ + Starts the firmware (leaves bootloader and boots the main software) + */ int DFUObject::JumpToApp(bool safeboot, bool erase) { char buf[BUF_LEN]; - buf[0] =0x02;//reportID - buf[1] = OP_DFU::JumpFW;//DFU Command + + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::JumpFW; // DFU Command buf[2] = 0; buf[3] = 0; buf[4] = 0; @@ -533,37 +529,37 @@ int DFUObject::JumpToApp(bool safeboot, bool erase) buf[9] = 0; } if (erase) { - // force data flash clear - buf[10] = 0x00; - buf[11] = 0x00; - buf[12] = 0xFA; - buf[13] = 0x5F; + // force data flash clear + buf[10] = 0x00; + buf[11] = 0x00; + buf[12] = 0xFA; + buf[13] = 0x5F; - buf[14] = 0x00; - buf[15] = 0x00; - buf[16] = 0x00; - buf[17] = 0x01; + buf[14] = 0x00; + buf[15] = 0x00; + buf[16] = 0x00; + buf[17] = 0x01; - buf[18] = 0x00; - buf[19] = 0x00; - buf[20] = 0x00; - buf[21] = 0x00; - } else { - buf[10] = 0x00; - buf[11] = 0x00; - buf[12] = 0x00; - buf[13] = 0x00; + buf[18] = 0x00; + buf[19] = 0x00; + buf[20] = 0x00; + buf[21] = 0x00; + } else { + buf[10] = 0x00; + buf[11] = 0x00; + buf[12] = 0x00; + buf[13] = 0x00; - buf[14] = 0x00; - buf[15] = 0x00; - buf[16] = 0x00; - buf[17] = 0x00; + buf[14] = 0x00; + buf[15] = 0x00; + buf[16] = 0x00; + buf[17] = 0x00; - buf[18] = 0x00; - buf[19] = 0x00; - buf[20] = 0x00; - buf[21] = 0x00; - } + buf[18] = 0x00; + buf[19] = 0x00; + buf[20] = 0x00; + buf[21] = 0x00; + } return sendData(buf, BUF_LEN); } @@ -571,8 +567,9 @@ int DFUObject::JumpToApp(bool safeboot, bool erase) OP_DFU::Status DFUObject::StatusRequest() { char buf[BUF_LEN]; - buf[0] =0x02; //reportID - buf[1] = OP_DFU::Status_Request; //DFU Command + + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::Status_Request; // DFU Command buf[2] = 0; buf[3] = 0; buf[4] = 0; @@ -583,28 +580,29 @@ OP_DFU::Status DFUObject::StatusRequest() buf[9] = 0; int result = sendData(buf, BUF_LEN); - if(debug) + if (debug) { qDebug() << "StatusRequest: " << result << " bytes sent"; - result = receiveData(buf,BUF_LEN); - if(debug) - qDebug() << "StatusRequest: " << result << " bytes received"; - if(buf[1]==OP_DFU::Status_Rep) - { - return (OP_DFU::Status)buf[6]; } - else + result = receiveData(buf, BUF_LEN); + if (debug) { + qDebug() << "StatusRequest: " << result << " bytes received"; + } + if (buf[1] == OP_DFU::Status_Rep) { + return (OP_DFU::Status)buf[6]; + } else { return OP_DFU::abort; + } } /** - Ask the bootloader for the list of devices available - */ + Ask the bootloader for the list of devices available + */ bool DFUObject::findDevices() { devices.clear(); char buf[BUF_LEN]; - buf[0] =0x02; //reportID - buf[1] = OP_DFU::Req_Capabilities; //DFU Command + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::Req_Capabilities; // DFU Command buf[2] = 0; buf[3] = 0; buf[4] = 0; @@ -615,70 +613,68 @@ bool DFUObject::findDevices() buf[9] = 0; int result = sendData(buf, BUF_LEN); - if (result < 1) + if (result < 1) { return false; + } - result = receiveData(buf,BUF_LEN); - if (result < 1) + result = receiveData(buf, BUF_LEN); + if (result < 1) { return false; + } - numberOfDevices=buf[7]; - RWFlags=buf[8]; - RWFlags=RWFlags<<8 | buf[9]; + numberOfDevices = buf[7]; + RWFlags = buf[8]; + RWFlags = RWFlags << 8 | buf[9]; - if(buf[1]==OP_DFU::Rep_Capabilities) - { - for(int x=0;x>(x*2) & 1); - dev.Writable=(bool)(RWFlags>>(x*2+1) & 1); + dev.Readable = (bool)(RWFlags >> (x * 2) & 1); + dev.Writable = (bool)(RWFlags >> (x * 2 + 1) & 1); devices.append(dev); - buf[0] =0x02; //reportID - buf[1] = OP_DFU::Req_Capabilities; //DFU Command + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::Req_Capabilities; // DFU Command buf[2] = 0; buf[3] = 0; buf[4] = 0; buf[5] = 0; - buf[6] = x+1; + buf[6] = x + 1; buf[7] = 0; buf[8] = 0; buf[9] = 0; int result = sendData(buf, BUF_LEN); - result = receiveData(buf,BUF_LEN); - devices[x].ID=buf[14]; - devices[x].ID=devices[x].ID<<8 | (quint8)buf[15]; - devices[x].BL_Version=buf[7]; - devices[x].SizeOfDesc=buf[8]; + result = receiveData(buf, BUF_LEN); + devices[x].ID = buf[14]; + devices[x].ID = devices[x].ID << 8 | (quint8)buf[15]; + devices[x].BL_Version = buf[7]; + devices[x].SizeOfDesc = buf[8]; quint32 aux; - aux=(quint8)buf[10]; - aux=aux<<8 |(quint8)buf[11]; - aux=aux<<8 |(quint8)buf[12]; - aux=aux<<8 |(quint8)buf[13]; + aux = (quint8)buf[10]; + aux = aux << 8 | (quint8)buf[11]; + aux = aux << 8 | (quint8)buf[12]; + aux = aux << 8 | (quint8)buf[13]; - devices[x].FW_CRC=aux; + devices[x].FW_CRC = aux; - aux=(quint8)buf[2]; - aux=aux<<8 |(quint8)buf[3]; - aux=aux<<8 |(quint8)buf[4]; - aux=aux<<8 |(quint8)buf[5]; - devices[x].SizeOfCode=aux; + aux = (quint8)buf[2]; + aux = aux << 8 | (quint8)buf[3]; + aux = aux << 8 | (quint8)buf[4]; + aux = aux << 8 | (quint8)buf[5]; + devices[x].SizeOfCode = aux; } - if(debug) - { + if (debug) { qDebug() << "Found " << numberOfDevices << " devices"; - for(int x=0;x0) + } + if (result > 0) { return true; + } return false; } // /** - Starts a firmware upload (asynchronous) - */ -bool DFUObject::UploadFirmware(const QString &sfile, const bool &verify,int device) + Starts a firmware upload (asynchronous) + */ +bool DFUObject::UploadFirmware(const QString &sfile, const bool &verify, int device) { - - if (isRunning()) + if (isRunning()) { return false; + } requestedOperation = OP_DFU::Upload; - requestFilename = sfile; + requestFilename = sfile; requestDevice = device; requestVerify = verify; start(); return true; } -OP_DFU::Status DFUObject::UploadFirmwareT(const QString &sfile, const bool &verify,int device) +OP_DFU::Status DFUObject::UploadFirmwareT(const QString &sfile, const bool &verify, int device) { OP_DFU::Status ret; - if (debug) - qDebug() <<"Starting Firmware Uploading..."; + if (debug) { + qDebug() << "Starting Firmware Uploading..."; + } QFile file(sfile); - if (!file.open(QIODevice::ReadOnly)) - { - if(debug) - qDebug()<<"Cant open file"; + if (!file.open(QIODevice::ReadOnly)) { + if (debug) { + qDebug() << "Cant open file"; + } return OP_DFU::abort; } QByteArray arr = file.readAll(); - if(debug) - qDebug()<<"Bytes Loaded="<"); - }else{ - bar.replace(i,1," "); + for (int i = 0; i < 50; i++) { + if (i < (percent / 2)) { + bar.replace(i, 1, "="); + } else if (i == (percent / 2)) { + bar.replace(i, 1, ">"); + } else { + bar.replace(i, 1, " "); } } - std::cout<< "\r"<> 2; // /4 Size passed in as a byte count, assumed to be a multiple of 4 + // Size = Size >> 2; // /4 Size passed in as a byte count, assumed to be a multiple of 4 - while(Size--) - { + while (Size--) { static const quint32 CrcTable[16] = { // Nibble lookup table for 0x04C11DB7 polynomial - 0x00000000,0x04C11DB7,0x09823B6E,0x0D4326D9,0x130476DC,0x17C56B6B,0x1A864DB2,0x1E475005, - 0x2608EDB8,0x22C9F00F,0x2F8AD6D6,0x2B4BCB61,0x350C9B64,0x31CD86D3,0x3C8EA00A,0x384FBDBD }; + 0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005, + 0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61, 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD + }; - Crc = Crc ^ *((quint32 *)Buffer); // Apply all 32-bits + Crc = Crc ^ *((quint32 *)Buffer); // Apply all 32-bits Buffer += 1; // Process 32-bits, 4 at a time, or 8 rounds Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // Assumes 32-bit reg, masking index to 4-bits - Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // 0x04C11DB7 Polynomial used in STM32 + Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // 0x04C11DB7 Polynomial used in STM32 Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; @@ -991,113 +997,114 @@ quint32 DFUObject::CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer) Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; } - return(Crc); + return Crc; } /** - Utility function - */ + Utility function + */ quint32 DFUObject::CRCFromQBArray(QByteArray array, quint32 Size) { - quint32 pad=Size-array.length(); - array.append(QByteArray(pad,255)); - quint32 t[Size/4]; - for(int x=0;xsendData((uint8_t*)data+1,size-1)) - { - if (debug) - qDebug() << "packet sent" << "data0" << ((uint8_t*)data+1)[0]; + if (serialhandle->sendData((uint8_t *)data + 1, size - 1)) { + if (debug) { + qDebug() << "packet sent" << "data0" << ((uint8_t *)data + 1)[0]; + } return size; } - if(debug) + if (debug) { qDebug() << "Serial send OVERRUN"; + } return -1; } /** - Receive data from the bootloader, either through the serial port - of through the HID handle, depending on the mode we're using - */ -int DFUObject::receiveData(void * data,int size) + Receive data from the bootloader, either through the serial port + of through the HID handle, depending on the mode we're using + */ +int DFUObject::receiveData(void *data, int size) { - if(!use_serial) - return hidHandle.receive(0,data, size, 10000); + if (!use_serial) { + return hidHandle.receive(0, data, size, 10000); + } // Serial Mode: int x; QTime time; time.start(); - while(true) - { - if((x=serialhandle->read_Packet(((char *) data)+1)!=-1) || time.elapsed()>10000) - { - if(time.elapsed()>10000) - qDebug()<<"____timeout"; + while (true) { + if ((x = serialhandle->read_Packet(((char *)data) + 1) != -1) || time.elapsed() > 10000) { + if (time.elapsed() > 10000) { + qDebug() << "____timeout"; + } return x; } } } -#define BOARD_ID_MB 1 -#define BOARD_ID_INS 2 -#define BOARD_ID_PIP 3 -#define BOARD_ID_CC 4 -#define BOARD_ID_REVO 9 +#define BOARD_ID_MB 1 +#define BOARD_ID_INS 2 +#define BOARD_ID_PIP 3 +#define BOARD_ID_CC 4 +#define BOARD_ID_REVO 9 /** - Gets the type of board connected - */ + Gets the type of board connected + */ OP_DFU::eBoardType DFUObject::GetBoardType(int boardNum) { OP_DFU::eBoardType brdType = eBoardUnkwn; // First of all, check what Board type we are talking to int board = devices[boardNum].ID; + qDebug() << "Board model: " << board; switch (board >> 8) { - case BOARD_ID_MB: // Mainboard family - brdType = eBoardMainbrd; - break; - case BOARD_ID_INS: // Inertial Nav - brdType = eBoardINS; - break; - case BOARD_ID_PIP: // PIP RF Modem - brdType = eBoardPip; - break; - case BOARD_ID_CC: // CopterControl family - brdType = eBoardCC; - break; - case BOARD_ID_REVO: // Revo board - brdType = eBoardRevo; - break; - } + case BOARD_ID_MB: // Mainboard family + brdType = eBoardMainbrd; + break; + case BOARD_ID_INS: // Inertial Nav + brdType = eBoardINS; + break; + case BOARD_ID_PIP: // PIP RF Modem + brdType = eBoardPip; + break; + case BOARD_ID_CC: // CopterControl family + brdType = eBoardCC; + break; + case BOARD_ID_REVO: // Revo board + brdType = eBoardRevo; + break; + } return brdType; } - - diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.h b/ground/openpilotgcs/src/plugins/uploader/op_dfu.h index 14615943d..8805fe477 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.h +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.h @@ -24,198 +24,190 @@ #include "SSP/qsspt.h" using namespace std; -#define BUF_LEN 64 +#define BUF_LEN 64 -#define MAX_PACKET_DATA_LEN 255 -#define MAX_PACKET_BUF_SIZE (1+1+MAX_PACKET_DATA_LEN+2) +#define MAX_PACKET_DATA_LEN 255 +#define MAX_PACKET_BUF_SIZE (1 + 1 + MAX_PACKET_DATA_LEN + 2) namespace OP_DFU { +enum TransferTypes { + FW, + Descript +}; - enum TransferTypes +enum CompareType { + crccompare, + bytetobytecompare +}; + +Q_ENUMS(Status) +enum Status { + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + idle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, // 12 + abort // 13 +}; + +enum Actions { + actionProgram, + actionProgramAndVerify, + actionDownload, + actionCompareAll, + actionCompareCrc, + actionListDevs, + actionStatusReq, + actionReset, + actionJump +}; + +enum Commands { + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep, // 12 +}; + +enum eBoardType { + eBoardUnkwn = 0, + eBoardMainbrd = 1, + eBoardINS, + eBoardPip = 3, + eBoardCC = 4, + eBoardRevo = 9, +}; + +struct device { + int ID; + quint32 FW_CRC; + int BL_Version; + int SizeOfDesc; + quint32 SizeOfCode; + bool Readable; + bool Writable; +}; + + +class DFUObject : public QThread { + Q_OBJECT; + +public: + static quint32 CRCFromQBArray(QByteArray array, quint32 Size); + // DFUObject(bool debug); + DFUObject(bool debug, bool use_serial, QString port); + + ~DFUObject(); + + // Service commands: + bool enterDFU(int const &devNumber); + bool findDevices(); + int JumpToApp(bool safeboot, bool erase); + int ResetDevice(void); + OP_DFU::Status StatusRequest(); + bool EndOperation(); + int AbortOperation(void); + bool ready() { - FW, - Descript - }; + return mready; + } - enum CompareType + // Upload (send to device) commands + OP_DFU::Status UploadDescription(QVariant description); + bool UploadFirmware(const QString &sfile, const bool &verify, int device); + + // Download (get from device) commands: + // DownloadDescription is synchronous + QString DownloadDescription(int const & numberOfChars); + QByteArray DownloadDescriptionAsBA(int const & numberOfChars); + // Asynchronous firmware download: initiates fw download, + // and a downloadFinished signal is emitted when download + // if finished: + bool DownloadFirmware(QByteArray *byteArray, int device); + + // Comparison functions (is this needed?) + OP_DFU::Status CompareFirmware(const QString &sfile, const CompareType &type, int device); + + bool SaveByteArrayToFile(QString const & file, QByteArray const &array); + + + // Variables: + QList devices; + int numberOfDevices; + int send_delay; + bool use_delay; + + // Helper functions: + QString StatusToString(OP_DFU::Status const & status); + static quint32 CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer); + OP_DFU::eBoardType GetBoardType(int boardNum); + + +signals: + void progressUpdated(int); + void downloadFinished(); + void uploadFinished(OP_DFU::Status); + void operationProgress(QString status); + +private: + // Generic variables: + bool debug; + bool use_serial; + bool mready; + int RWFlags; + qsspt *serialhandle; + int sendData(void *, int); + int receiveData(void *data, int size); + uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; + uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; + port *info; + + + // USB Bootloader: + pjrc_rawhid hidHandle; + int setStartBit(int command) { - crccompare, - bytetobytecompare - }; + return command | 0x20; + } - Q_ENUMS(Status) - enum Status - { - DFUidle,//0 - uploading,//1 - wrong_packet_received,//2 - too_many_packets,//3 - too_few_packets,//4 - Last_operation_Success,//5 - downloading,//6 - idle,//7 - Last_operation_failed,//8 - uploadingStarting,//9 - outsideDevCapabilities,//10 - CRC_Fail,//11 - failed_jump,//12 - abort//13 - }; + void CopyWords(char *source, char *destination, int count); + void printProgBar(int const & percent, QString const & label); + bool StartUpload(qint32 const &numberOfBytes, TransferTypes const & type, quint32 crc); + bool UploadData(qint32 const & numberOfPackets, QByteArray & data); - enum Actions - { - actionProgram, - actionProgramAndVerify, - actionDownload, - actionCompareAll, - actionCompareCrc, - actionListDevs, - actionStatusReq, - actionReset, - actionJump - }; - - enum Commands - { - Reserved,//0 - Req_Capabilities,//1 - Rep_Capabilities,//2 - EnterDFU,//3 - JumpFW,//4 - Reset,//5 - Abort_Operation,//6 - Upload,//7 - Op_END,//8 - Download_Req,//9 - Download,//10 - Status_Request,//11 - Status_Rep,//12 - - }; - - enum eBoardType - { - eBoardUnkwn = 0, - eBoardMainbrd = 1, - eBoardINS, - eBoardPip = 3, - eBoardCC = 4, - eBoardRevo = 9, - }; - - struct device - { - int ID; - quint32 FW_CRC; - int BL_Version; - int SizeOfDesc; - quint32 SizeOfCode; - bool Readable; - bool Writable; - }; - - - class DFUObject : public QThread - { - Q_OBJECT; - - public: - static quint32 CRCFromQBArray(QByteArray array, quint32 Size); - //DFUObject(bool debug); - DFUObject(bool debug,bool use_serial,QString port); - - ~DFUObject(); - - // Service commands: - bool enterDFU(int const &devNumber); - bool findDevices(); - int JumpToApp(bool safeboot, bool erase); - int ResetDevice(void); - OP_DFU::Status StatusRequest(); - bool EndOperation(); - int AbortOperation(void); - bool ready() { return mready; } - - // Upload (send to device) commands - OP_DFU::Status UploadDescription(QVariant description); - bool UploadFirmware(const QString &sfile, const bool &verify,int device); - - // Download (get from device) commands: - // DownloadDescription is synchronous - QString DownloadDescription(int const & numberOfChars); - QByteArray DownloadDescriptionAsBA(int const & numberOfChars); - // Asynchronous firmware download: initiates fw download, - // and a downloadFinished signal is emitted when download - // if finished: - bool DownloadFirmware(QByteArray *byteArray, int device); - - // Comparison functions (is this needed?) - OP_DFU::Status CompareFirmware(const QString &sfile, const CompareType &type,int device); - - bool SaveByteArrayToFile(QString const & file,QByteArray const &array); - - - - // Variables: - QList devices; - int numberOfDevices; - int send_delay; - bool use_delay; - - // Helper functions: - QString StatusToString(OP_DFU::Status const & status); - static quint32 CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer); - OP_DFU::eBoardType GetBoardType(int boardNum); - - - - signals: - void progressUpdated(int); - void downloadFinished(); - void uploadFinished(OP_DFU::Status); - void operationProgress(QString status); - - private: - // Generic variables: - bool debug; - bool use_serial; - bool mready; - int RWFlags; - qsspt * serialhandle; - int sendData(void*,int); - int receiveData(void * data,int size); - uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; - uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; - port * info; - - - // USB Bootloader: - pjrc_rawhid hidHandle; - int setStartBit(int command){ return command|0x20; } - - void CopyWords(char * source, char* destination, int count); - void printProgBar( int const & percent,QString const& label); - bool StartUpload(qint32 const &numberOfBytes, TransferTypes const & type,quint32 crc); - bool UploadData(qint32 const & numberOfPackets,QByteArray & data); - - // Thread management: - // Same as startDownload except that we store in an external array: - bool StartDownloadT(QByteArray *fw, qint32 const & numberOfBytes, TransferTypes const & type); - OP_DFU::Status UploadFirmwareT(const QString &sfile, const bool &verify,int device); - QMutex mutex; - OP_DFU::Commands requestedOperation; - qint32 requestSize; - OP_DFU::TransferTypes requestTransferType; - QByteArray *requestStorage; - QString requestFilename; - bool requestVerify; - int requestDevice; - - protected: - void run();// Executes the upload or download operations - - }; + // Thread management: + // Same as startDownload except that we store in an external array: + bool StartDownloadT(QByteArray *fw, qint32 const & numberOfBytes, TransferTypes const & type); + OP_DFU::Status UploadFirmwareT(const QString &sfile, const bool &verify, int device); + QMutex mutex; + OP_DFU::Commands requestedOperation; + qint32 requestSize; + OP_DFU::TransferTypes requestTransferType; + QByteArray *requestStorage; + QString requestFilename; + bool requestVerify; + int requestDevice; +protected: + void run(); // Executes the upload or download operations +}; } Q_DECLARE_METATYPE(OP_DFU::Status) diff --git a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp index 539a3042f..904080ef5 100644 --- a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp @@ -44,22 +44,22 @@ void RunningDeviceWidget::showEvent(QShowEvent *event) // Thit fitInView method should only be called now, once the // widget is shown, otherwise it cannot compute its values and // the result is usually a ahrsbargraph that is way too small. - myDevice->devicePicture->fitInView(devicePic.rect(),Qt::KeepAspectRatio); + myDevice->devicePicture->fitInView(devicePic.rect(), Qt::KeepAspectRatio); } -void RunningDeviceWidget::resizeEvent(QResizeEvent* event) +void RunningDeviceWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); myDevice->devicePicture->fitInView(devicePic.rect(), Qt::KeepAspectRatio); } /** - Fills the various fields for the device - */ + Fills the various fields for the device + */ void RunningDeviceWidget::populate() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectUtilManager* utilMngr = pm->getObject(); + UAVObjectUtilManager *utilMngr = pm->getObject(); int id = utilMngr->getBoardModel(); myDevice->lblDeviceID->setText(QString("Device ID: ") + QString::number(id, 16)); @@ -95,20 +95,19 @@ void RunningDeviceWidget::populate() } myDevice->devicePicture->scene()->addPixmap(devicePic); myDevice->devicePicture->setSceneRect(devicePic.rect()); - myDevice->devicePicture->fitInView(devicePic.rect(),Qt::KeepAspectRatio); + myDevice->devicePicture->fitInView(devicePic.rect(), Qt::KeepAspectRatio); QString serial = utilMngr->getBoardCPUSerial().toHex(); myDevice->CPUSerial->setText(serial); QByteArray description = utilMngr->getBoardDescription(); deviceDescriptorStruct devDesc; - if(UAVObjectUtilManager::descriptionToStructure(description, devDesc)) { - if(devDesc.gitTag.startsWith("RELEASE",Qt::CaseSensitive)) { + if (UAVObjectUtilManager::descriptionToStructure(description, devDesc)) { + if (devDesc.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { myDevice->lblFWTag->setText(QString("Firmware tag: ") + devDesc.gitTag); QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); myDevice->lblCertified->setPixmap(pix); myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build")); - } else { myDevice->lblFWTag->setText(QString("Firmware tag: ") + devDesc.gitTag); QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); @@ -116,7 +115,7 @@ void RunningDeviceWidget::populate() myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build")); } myDevice->lblGitCommitTag->setText("Git commit hash: " + devDesc.gitHash); - myDevice->lblFWDate->setText(QString("Firmware date: ") + devDesc.gitDate.insert(4,"-").insert(7,"-")); + myDevice->lblFWDate->setText(QString("Firmware date: ") + devDesc.gitDate.insert(4, "-").insert(7, "-")); } else { myDevice->lblFWTag->setText(QString("Firmware tag: ") + QString(description).left(QString(description).indexOf(QChar(255)))); myDevice->lblGitCommitTag->setText("Git commit tag: Unknown"); diff --git a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.h b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.h index 15a3d5b71..44fcdc944 100644 --- a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.h @@ -41,29 +41,27 @@ #include "uavobjectutilmanager.h" #include "uploader_global.h" -class UPLOADER_EXPORT RunningDeviceWidget : public QWidget -{ +class UPLOADER_EXPORT RunningDeviceWidget : public QWidget { Q_OBJECT public: - RunningDeviceWidget( QWidget *parent = 0); + RunningDeviceWidget(QWidget *parent = 0); void populate(); void freeze(); QString setOpenFileName(); QString setSaveFileName(); - typedef enum { STATUSICON_OK, STATUSICON_RUNNING, STATUSICON_FAIL, STATUSICON_INFO} StatusIcon; + typedef enum { STATUSICON_OK, STATUSICON_RUNNING, STATUSICON_FAIL, STATUSICON_INFO } StatusIcon; private: Ui_runningDeviceWidget *myDevice; int deviceID; QPixmap devicePic; - //void status(QString str, StatusIcon ic); + // void status(QString str, StatusIcon ic); signals: protected: void showEvent(QShowEvent *event); void resizeEvent(QResizeEvent *event); - }; #endif // RUNNINGDEVICEWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadget.cpp index 5481753e9..bc52ab2de 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadget.cpp @@ -28,11 +28,10 @@ #include "uploadergadgetwidget.h" #include "uploadergadgetconfiguration.h" -UploaderGadget::UploaderGadget(QString classId, UploaderGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} +UploaderGadget::UploaderGadget(QString classId, UploaderGadgetWidget *widget, QWidget *parent) : + IUAVGadget(classId, parent), + m_widget(widget) +{} UploaderGadget::~UploaderGadget() { @@ -42,10 +41,9 @@ UploaderGadget::~UploaderGadget() * Loads a configuration. * */ -void UploaderGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void UploaderGadget::loadConfiguration(IUAVGadgetConfiguration *config) { Q_UNUSED(config); /* UploaderGadgetConfiguration *m = qobject_cast< UploaderGadgetConfiguration*>(config); - */ + */ } - diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadget.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadget.h index 440c9d8e3..ee195bc32 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadget.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadget.h @@ -39,19 +39,20 @@ class UploaderGadgetWidget; using namespace Core; -class UPLOADER_EXPORT UploaderGadget : public Core::IUAVGadget -{ +class UPLOADER_EXPORT UploaderGadget : public Core::IUAVGadget { Q_OBJECT public: UploaderGadget(QString classId, UploaderGadgetWidget *widget, QWidget *parent = 0); ~UploaderGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: UploaderGadgetWidget *m_widget; }; #endif // UPLOADERGADGET_H - diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.cpp index 3c5b6d7c7..91e08c15f 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.cpp @@ -32,7 +32,7 @@ * Loads a saved configuration or defaults if non exist. * */ -UploaderGadgetConfiguration::UploaderGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +UploaderGadgetConfiguration::UploaderGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_defaultPort("Unknown"), m_defaultSpeed(BAUD19200), @@ -41,37 +41,34 @@ UploaderGadgetConfiguration::UploaderGadgetConfiguration(QString classId, QSetti m_defaultParity(PAR_NONE), m_defaultStopBits(STOP_1), m_defaultTimeOut(5000) - { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { BaudRateType speed; DataBitsType databits; FlowType flow; ParityType parity; StopBitsType stopbits; - int ispeed = qSettings->value("defaultSpeed").toInt(); + int ispeed = qSettings->value("defaultSpeed").toInt(); int idatabits = qSettings->value("defaultDataBits").toInt(); - int iflow = qSettings->value("defaultFlow").toInt(); - int iparity = qSettings->value("defaultParity").toInt(); + int iflow = qSettings->value("defaultFlow").toInt(); + int iparity = qSettings->value("defaultParity").toInt(); int istopbits = qSettings->value("defaultStopBits").toInt(); - QString port = qSettings->value("defaultPort").toString(); - - databits=(DataBitsType) idatabits; - flow=(FlowType)iflow; - parity=(ParityType)iparity; - stopbits=(StopBitsType)istopbits; - speed=(BaudRateType)ispeed; - m_defaultPort=port; - m_defaultSpeed=speed; - m_defaultDataBits=databits; - m_defaultFlow=flow; - m_defaultParity=parity; - m_defaultStopBits=stopbits; + QString port = qSettings->value("defaultPort").toString(); + databits = (DataBitsType)idatabits; + flow = (FlowType)iflow; + parity = (ParityType)iparity; + stopbits = (StopBitsType)istopbits; + speed = (BaudRateType)ispeed; + m_defaultPort = port; + m_defaultSpeed = speed; + m_defaultDataBits = databits; + m_defaultFlow = flow; + m_defaultParity = parity; + m_defaultStopBits = stopbits; } - } /** @@ -82,12 +79,12 @@ IUAVGadgetConfiguration *UploaderGadgetConfiguration::clone() { UploaderGadgetConfiguration *m = new UploaderGadgetConfiguration(this->classId()); - m->m_defaultSpeed=m_defaultSpeed; - m->m_defaultDataBits=m_defaultDataBits; - m->m_defaultFlow=m_defaultFlow; - m->m_defaultParity=m_defaultParity; - m->m_defaultStopBits=m_defaultStopBits; - m->m_defaultPort=m_defaultPort; + m->m_defaultSpeed = m_defaultSpeed; + m->m_defaultDataBits = m_defaultDataBits; + m->m_defaultFlow = m_defaultFlow; + m->m_defaultParity = m_defaultParity; + m->m_defaultStopBits = m_defaultStopBits; + m->m_defaultPort = m_defaultPort; return m; } @@ -95,7 +92,8 @@ IUAVGadgetConfiguration *UploaderGadgetConfiguration::clone() * Saves a configuration. * */ -void UploaderGadgetConfiguration::saveConfig(QSettings* qSettings) const { +void UploaderGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ qSettings->setValue("defaultSpeed", m_defaultSpeed); qSettings->setValue("defaultDataBits", m_defaultDataBits); qSettings->setValue("defaultFlow", m_defaultFlow); diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.h index 78bc5fb5b..558cb96ae 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.h @@ -34,31 +34,72 @@ using namespace Core; -class UPLOADER_EXPORT UploaderGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT +class UPLOADER_EXPORT UploaderGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit UploaderGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit UploaderGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - //set port configuration functions - void setSpeed(BaudRateType speed) {m_defaultSpeed=speed;} - void setDataBits(DataBitsType databits) {m_defaultDataBits=databits;} - void setFlow(FlowType flow) {m_defaultFlow=flow;} - void setParity(ParityType parity) {m_defaultParity=parity;} - void setStopBits(StopBitsType stopbits) {m_defaultStopBits=stopbits;} - void setPort(QString port){m_defaultPort=port;} - void setTimeOut(long timeout){m_defaultTimeOut=timeout;} + // set port configuration functions + void setSpeed(BaudRateType speed) + { + m_defaultSpeed = speed; + } + void setDataBits(DataBitsType databits) + { + m_defaultDataBits = databits; + } + void setFlow(FlowType flow) + { + m_defaultFlow = flow; + } + void setParity(ParityType parity) + { + m_defaultParity = parity; + } + void setStopBits(StopBitsType stopbits) + { + m_defaultStopBits = stopbits; + } + void setPort(QString port) + { + m_defaultPort = port; + } + void setTimeOut(long timeout) + { + m_defaultTimeOut = timeout; + } - //get port configuration functions - BaudRateType Speed() {return m_defaultSpeed;} - DataBitsType DataBits() {return m_defaultDataBits;} - FlowType Flow() {return m_defaultFlow;} - ParityType Parity() {return m_defaultParity;} - StopBitsType StopBits() {return m_defaultStopBits;} - QString Port(){return m_defaultPort;} - long TimeOut(){return m_defaultTimeOut;} + // get port configuration functions + BaudRateType Speed() + { + return m_defaultSpeed; + } + DataBitsType DataBits() + { + return m_defaultDataBits; + } + FlowType Flow() + { + return m_defaultFlow; + } + ParityType Parity() + { + return m_defaultParity; + } + StopBitsType StopBits() + { + return m_defaultStopBits; + } + QString Port() + { + return m_defaultPort; + } + long TimeOut() + { + return m_defaultTimeOut; + } - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.cpp index f69735b72..35733ce6c 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.cpp @@ -33,24 +33,23 @@ #include "uploadergadgetwidget.h" UploaderGadgetFactory::UploaderGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("Uploader"), tr("Uploader"), parent),isautocapable(false) -{ -} + IUAVGadgetFactory(QString("Uploader"), tr("Uploader"), parent), isautocapable(false) +{} UploaderGadgetFactory::~UploaderGadgetFactory() -{ -} +{} -Core::IUAVGadget* UploaderGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *UploaderGadgetFactory::createGadget(QWidget *parent) { - UploaderGadgetWidget* gadgetWidget = new UploaderGadgetWidget(parent); - isautocapable=gadgetWidget->autoUpdateCapable(); - connect(this,SIGNAL(autoUpdate()),gadgetWidget,SLOT(autoUpdate())); - connect(gadgetWidget,SIGNAL(autoUpdateSignal(uploader::AutoUpdateStep,QVariant)),this,SIGNAL(autoUpdateSignal(uploader::AutoUpdateStep,QVariant))); + UploaderGadgetWidget *gadgetWidget = new UploaderGadgetWidget(parent); + + isautocapable = gadgetWidget->autoUpdateCapable(); + connect(this, SIGNAL(autoUpdate()), gadgetWidget, SLOT(autoUpdate())); + connect(gadgetWidget, SIGNAL(autoUpdateSignal(uploader::AutoUpdateStep, QVariant)), this, SIGNAL(autoUpdateSignal(uploader::AutoUpdateStep, QVariant))); return new UploaderGadget(QString("Uploader"), gadgetWidget, parent); } -IUAVGadgetConfiguration *UploaderGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *UploaderGadgetFactory::createConfiguration(QSettings *qSettings) { return new UploaderGadgetConfiguration(QString("Uploader"), qSettings); } diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.h index 9953201b8..3303148f4 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.h @@ -38,20 +38,19 @@ class IUAVGadgetFactory; using namespace Core; -class UPLOADER_EXPORT UploaderGadgetFactory : public Core::IUAVGadgetFactory -{ +class UPLOADER_EXPORT UploaderGadgetFactory : public Core::IUAVGadgetFactory { Q_OBJECT public: UploaderGadgetFactory(QObject *parent = 0); ~UploaderGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); bool isAutoUpdateCapable(); private: bool isautocapable; signals: - void autoUpdateSignal(uploader::AutoUpdateStep,QVariant); + void autoUpdateSignal(uploader::AutoUpdateStep, QVariant); void autoUpdate(); }; diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.cpp index 9bc3f6e22..ccaa091a1 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.cpp @@ -39,17 +39,16 @@ UploaderGadgetOptionsPage::UploaderGadgetOptionsPage(UploaderGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget +// creates options page widget QWidget *UploaderGadgetOptionsPage::createPage(QWidget *parent) { Q_UNUSED(parent); - //main widget + // main widget QWidget *widget = new QWidget; return widget; @@ -61,13 +60,7 @@ QWidget *UploaderGadgetOptionsPage::createPage(QWidget *parent) * */ void UploaderGadgetOptionsPage::apply() -{ - -} +{} void UploaderGadgetOptionsPage::finish() -{ - -} - - +{} diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.h index 4546ea423..63a72b936 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.h @@ -44,9 +44,8 @@ class QSpinBox; using namespace Core; -class UPLOADER_EXPORT UploaderGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class UPLOADER_EXPORT UploaderGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit UploaderGadgetOptionsPage(UploaderGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index aeab03d8f..62793298a 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -34,17 +34,17 @@ UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) { - m_config = new Ui_UploaderWidget(); + m_config = new Ui_UploaderWidget(); m_config->setupUi(this); currentStep = IAP_STATE_READY; - resetOnly=false; + resetOnly = false; dfu = NULL; - m_timer = 0; - m_progress = 0; - msg=new QErrorMessage(this); + m_timer = 0; + m_progress = 0; + msg = new QErrorMessage(this); // Listen to autopilot connection events ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - TelemetryManager* telMngr = pm->getObject(); + TelemetryManager *telMngr = pm->getObject(); connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); connect(telMngr, SIGNAL(connected()), this, SLOT(versionMatchCheck())); @@ -57,7 +57,7 @@ UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) connect(m_config->eraseBootButton, SIGNAL(clicked()), this, SLOT(systemEraseBoot())); connect(m_config->rescueButton, SIGNAL(clicked()), this, SLOT(systemRescue())); Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager(); - connect(cm,SIGNAL(deviceConnected(QIODevice*)),this,SLOT(onPhisicalHWConnect())); + connect(cm, SIGNAL(deviceConnected(QIODevice *)), this, SLOT(onPhisicalHWConnect())); getSerialPorts(); QIcon rbi; @@ -68,37 +68,37 @@ UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) connect(m_config->refreshPorts, SIGNAL(clicked()), this, SLOT(getSerialPorts())); - connect(m_config->pbHelp,SIGNAL(clicked()),this,SLOT(openHelp())); + connect(m_config->pbHelp, SIGNAL(clicked()), this, SLOT(openHelp())); // And check whether by any chance we are not already connected - if (telMngr->isConnected()) - { + if (telMngr->isConnected()) { onAutopilotConnect(); versionMatchCheck(); } } -bool sortPorts(const QextPortInfo &s1,const QextPortInfo &s2) +bool sortPorts(const QextPortInfo &s1, const QextPortInfo &s2) { - return s1.portNametelemetryLink->clear(); list.append(QString("USB")); QList ports = QextSerialEnumerator::getPorts(); - //sort the list by port number (nice idea from PT_Dreamer :)) - qSort(ports.begin(), ports.end(),sortPorts); - foreach( QextPortInfo port, ports ) { - list.append(port.friendName); + // sort the list by port number (nice idea from PT_Dreamer :)) + qSort(ports.begin(), ports.end(), sortPorts); + foreach(QextPortInfo port, ports) { + list.append(port.friendName); } m_config->telemetryLink->addItems(list); @@ -108,32 +108,33 @@ void UploaderGadgetWidget::getSerialPorts() QString UploaderGadgetWidget::getPortDevice(const QString &friendName) { QList ports = QextSerialEnumerator::getPorts(); - foreach( QextPortInfo port, ports ) { - if(port.friendName == friendName) + foreach(QextPortInfo port, ports) { + if (port.friendName == friendName) #ifdef Q_OS_WIN - return port.portName; + { return port.portName; } #else - return port.physName; + { return port.physName; } #endif - } + } return ""; } void UploaderGadgetWidget::connectSignalSlot(QWidget *widget) { - connect(qobject_cast(widget),SIGNAL(uploadStarted()),this,SLOT(uploadStarted())); - connect(qobject_cast(widget),SIGNAL(uploadEnded(bool)),this,SLOT(uploadEnded(bool))); - connect(qobject_cast(widget),SIGNAL(downloadStarted()),this,SLOT(downloadStarted())); - connect(qobject_cast(widget),SIGNAL(downloadEnded(bool)),this,SLOT(downloadEnded(bool))); + connect(qobject_cast(widget), SIGNAL(uploadStarted()), this, SLOT(uploadStarted())); + connect(qobject_cast(widget), SIGNAL(uploadEnded(bool)), this, SLOT(uploadEnded(bool))); + connect(qobject_cast(widget), SIGNAL(downloadStarted()), this, SLOT(downloadStarted())); + connect(qobject_cast(widget), SIGNAL(downloadEnded(bool)), this, SLOT(downloadEnded(bool))); } FlightStatus *UploaderGadgetWidget::getFlightStatus() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm); UAVObjectManager *objManager = pm->getObject(); Q_ASSERT(objManager); - FlightStatus *status = dynamic_cast(objManager->getObject(QString("FlightStatus"))); + FlightStatus *status = dynamic_cast(objManager->getObject(QString("FlightStatus"))); Q_ASSERT(status); return status; } @@ -144,11 +145,11 @@ void UploaderGadgetWidget::bootButtonsSetEnable(bool enabled) m_config->safeBootButton->setEnabled(enabled); - m_config->eraseBootButton->setEnabled( - enabled && - // this feature is supported only on BL revision >= 4 - ((dfu != NULL) && (dfu->devices[0].BL_Version >= 4))); - } + m_config->eraseBootButton->setEnabled( + enabled && + // this feature is supported only on BL revision >= 4 + ((dfu != NULL) && (dfu->devices[0].BL_Version >= 4))); +} void UploaderGadgetWidget::onPhisicalHWConnect() { @@ -158,9 +159,10 @@ void UploaderGadgetWidget::onPhisicalHWConnect() } /** - Enables widget buttons if autopilot connected - */ -void UploaderGadgetWidget::onAutopilotConnect(){ + Enables widget buttons if autopilot connected + */ +void UploaderGadgetWidget::onAutopilotConnect() +{ QTimer::singleShot(1000, this, SLOT(populate())); } @@ -175,20 +177,20 @@ void UploaderGadgetWidget::populate() // Add a very simple widget with Board model & serial number // Delete all previous tabs: while (m_config->systemElements->count()) { - QWidget *qw = m_config->systemElements->widget(0); - m_config->systemElements->removeTab(0); - delete qw; + QWidget *qw = m_config->systemElements->widget(0); + m_config->systemElements->removeTab(0); + delete qw; } - RunningDeviceWidget* dw = new RunningDeviceWidget(this); + RunningDeviceWidget *dw = new RunningDeviceWidget(this); dw->populate(); m_config->systemElements->addTab(dw, QString("Connected Device")); } /** - Enables widget buttons if autopilot disconnected - */ -void UploaderGadgetWidget::onAutopilotDisconnect(){ - + Enables widget buttons if autopilot disconnected + */ +void UploaderGadgetWidget::onAutopilotDisconnect() +{ m_config->haltButton->setEnabled(false); m_config->resetButton->setEnabled(false); bootButtonsSetEnable(true); @@ -202,17 +204,17 @@ void UploaderGadgetWidget::onAutopilotDisconnect(){ } /** - Tell the mainboard to go to bootloader: + Tell the mainboard to go to bootloader: - Send the relevant IAP commands - setup callback for MoBo acknowledge - */ -void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) + */ +void UploaderGadgetWidget::goToBootloader(UAVObject *callerObj, bool success) { Q_UNUSED(callerObj); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVObject *fwIAP = dynamic_cast(objManager->getObject(QString("FirmwareIAPObj"))); + UAVObject *fwIAP = dynamic_cast(objManager->getObject(QString("FirmwareIAPObj"))); switch (currentStep) { case IAP_STATE_READY: @@ -223,18 +225,18 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) fwIAP->getField("Command")->setValue("1122"); fwIAP->getField("BoardRevision")->setDouble(0); fwIAP->getField("BoardType")->setDouble(0); - connect(fwIAP,SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(goToBootloader(UAVObject*, bool))); + connect(fwIAP, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(goToBootloader(UAVObject *, bool))); currentStep = IAP_STATE_STEP_1; clearLog(); log(QString("IAP Step 1")); fwIAP->updated(); break; case IAP_STATE_STEP_1: - if (!success) { + if (!success) { log(QString("Oops, failure step 1")); log("Reset did NOT happen"); currentStep = IAP_STATE_READY; - disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(goToBootloader(UAVObject*, bool))); + disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(goToBootloader(UAVObject *, bool))); m_config->haltButton->setEnabled(true); break; } @@ -250,7 +252,7 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) log(QString("Oops, failure step 2")); log("Reset did NOT happen"); currentStep = IAP_STATE_READY; - disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(goToBootloader(UAVObject*, bool))); + disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(goToBootloader(UAVObject *, bool))); m_config->haltButton->setEnabled(true); break; } @@ -267,7 +269,7 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) if (!success) { log("Oops, failure step 3"); log("Reset did NOT happen"); - disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(goToBootloader(UAVObject*, bool))); + disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(goToBootloader(UAVObject *, bool))); m_config->haltButton->setEnabled(true); break; } @@ -285,25 +287,26 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) m_eventloop.exec(); log("Board Halt"); m_config->boardStatus->setText("Bootloader"); - if (dlj.startsWith("USB")) + if (dlj.startsWith("USB")) { m_config->telemetryLink->setCurrentIndex(m_config->telemetryLink->findText("USB")); - else + } else { m_config->telemetryLink->setCurrentIndex(m_config->telemetryLink->findText(dli)); + } - disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(goToBootloader(UAVObject*, bool))); + disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(goToBootloader(UAVObject *, bool))); currentStep = IAP_STATE_BOOTLOADER; // Tell the mainboard to get into bootloader state: log("Detecting devices, please wait a few seconds..."); if (!dfu) { - if (dlj.startsWith("USB")) + if (dlj.startsWith("USB")) { dfu = new DFUObject(DFU_DEBUG, false, QString()); - else - dfu = new DFUObject(DFU_DEBUG,true, getPortDevice(dli)); + } else { + dfu = new DFUObject(DFU_DEBUG, true, getPortDevice(dli)); + } } - if (!dfu->ready()) - { + if (!dfu->ready()) { log("Could not enter DFU mode."); delete dfu; dfu = NULL; @@ -314,8 +317,7 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) return; } dfu->AbortOperation(); - if(!dfu->enterDFU(0)) - { + if (!dfu->enterDFU(0)) { log("Could not enter DFU mode."); delete dfu; dfu = NULL; @@ -324,10 +326,10 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) m_config->boardStatus->setText("Bootloader?"); return; } - //dfu.StatusRequest(); + // dfu.StatusRequest(); - QTimer::singleShot(500, &m_eventloop, SLOT(quit())); - m_eventloop.exec(); + QTimer::singleShot(500, &m_eventloop, SLOT(quit())); + m_eventloop.exec(); dfu->findDevices(); log(QString("Found ") + QString::number(dfu->numberOfDevices) + QString(" device(s).")); if (dfu->numberOfDevices < 0 || dfu->numberOfDevices > 3) { @@ -339,12 +341,12 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) } // Delete all previous tabs: while (m_config->systemElements->count()) { - QWidget *qw = m_config->systemElements->widget(0); - m_config->systemElements->removeTab(0); - delete qw; + QWidget *qw = m_config->systemElements->widget(0); + m_config->systemElements->removeTab(0); + delete qw; } - for(int i=0;inumberOfDevices;i++) { - DeviceWidget* dw = new DeviceWidget(this); + for (int i = 0; i < dfu->numberOfDevices; i++) { + DeviceWidget *dw = new DeviceWidget(this); connectSignalSlot(dw); dw->setDeviceID(i); dw->setDfu(dfu); @@ -352,41 +354,38 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) m_config->systemElements->addTab(dw, QString("Device") + QString::number(i)); } /* - m_config->haltButton->setEnabled(false); - m_config->resetButton->setEnabled(false); - */ + m_config->haltButton->setEnabled(false); + m_config->resetButton->setEnabled(false); + */ // Need to re-enable in case we were not connected bootButtonsSetEnable(true); /* - m_config->telemetryLink->setEnabled(false); - m_config->rescueButton->setEnabled(false); - */ + m_config->telemetryLink->setEnabled(false); + m_config->rescueButton->setEnabled(false); + */ if (resetOnly) { - resetOnly=false; + resetOnly = false; delay::msleep(3500); systemBoot(); break; } - } - break; + break; case IAP_STATE_BOOTLOADER: // We should never end up here anyway. break; } - } void UploaderGadgetWidget::systemHalt() { - FlightStatus* status = getFlightStatus(); + FlightStatus *status = getFlightStatus(); // The board can not be halted when in armed state. // If board is armed, or arming. Show message with notice. if (status->getArmed() == FlightStatus::ARMED_DISARMED) { goToBootloader(); - } - else { + } else { QMessageBox mbox(this); mbox.setText(QString(tr("The controller board is armed and can not be halted.\n\n" "Please make sure the board is not armed and then press halt again to proceed\n" @@ -398,13 +397,13 @@ void UploaderGadgetWidget::systemHalt() } /** - Tell the mainboard to reset: + Tell the mainboard to reset: - Send the relevant IAP commands - setup callback for MoBo acknowledge - */ + */ void UploaderGadgetWidget::systemReset() { - FlightStatus* status = getFlightStatus(); + FlightStatus *status = getFlightStatus(); // The board can not be reset when in armed state. // If board is armed, or arming. Show message with notice. @@ -417,8 +416,7 @@ void UploaderGadgetWidget::systemReset() clearLog(); log("Board Reset initiated."); goToBootloader(); - } - else { + } else { QMessageBox mbox(this); mbox.setText(QString(tr("The controller board is armed and can not be reset.\n\n" "Please make sure the board is not armed and then press reset again to proceed\n" @@ -431,34 +429,34 @@ void UploaderGadgetWidget::systemReset() void UploaderGadgetWidget::systemBoot() { - commonSystemBoot(false, false); + commonSystemBoot(false, false); } void UploaderGadgetWidget::systemSafeBoot() { - commonSystemBoot(true, false); + commonSystemBoot(true, false); } void UploaderGadgetWidget::systemEraseBoot() { - QMessageBox msgBox; - int result; - msgBox.setWindowTitle(tr("Erase Settings")); - msgBox.setInformativeText(tr("Do you want to erase all settings from the board?\nSettings cannot be recovered after this operation.\nThe board will be restarted and all the setting erased")); - msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel | QMessageBox::Help); - result = msgBox.exec(); - if(result == QMessageBox::Ok) - { - commonSystemBoot(true, true); - } else if(result == QMessageBox::Help) { - QDesktopServices::openUrl( QUrl(tr("http://wiki.openpilot.org/display/Doc/Erase+board+settings"), QUrl::StrictMode) ); - } + QMessageBox msgBox; + int result; + + msgBox.setWindowTitle(tr("Erase Settings")); + msgBox.setInformativeText(tr("Do you want to erase all settings from the board?\nSettings cannot be recovered after this operation.\nThe board will be restarted and all the setting erased")); + msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel | QMessageBox::Help); + result = msgBox.exec(); + if (result == QMessageBox::Ok) { + commonSystemBoot(true, true); + } else if (result == QMessageBox::Help) { + QDesktopServices::openUrl(QUrl(tr("http://wiki.openpilot.org/display/Doc/Erase+board+settings"), QUrl::StrictMode)); + } } /** - * Tells the system to boot (from Bootloader state) - * @param[in] safeboot Indicates whether the firmware should use the stock HWSettings - */ + * Tells the system to boot (from Bootloader state) + * @param[in] safeboot Indicates whether the firmware should use the stock HWSettings + */ void UploaderGadgetWidget::commonSystemBoot(bool safeboot, bool erase) { clearLog(); @@ -474,14 +472,14 @@ void UploaderGadgetWidget::commonSystemBoot(bool safeboot, bool erase) repaint(); if (!dfu) { - if (devName == "USB") + if (devName == "USB") { dfu = new DFUObject(DFU_DEBUG, false, QString()); - else - dfu = new DFUObject(DFU_DEBUG,true,getPortDevice(devName)); + } else { + dfu = new DFUObject(DFU_DEBUG, true, getPortDevice(devName)); + } } dfu->AbortOperation(); - if(!dfu->enterDFU(0)) - { + if (!dfu->enterDFU(0)) { log("Could not enter DFU mode."); delete dfu; dfu = NULL; @@ -496,13 +494,13 @@ void UploaderGadgetWidget::commonSystemBoot(bool safeboot, bool erase) m_config->rescueButton->setEnabled(true); m_config->telemetryLink->setEnabled(true); m_config->boardStatus->setText("Running"); - if (currentStep == IAP_STATE_BOOTLOADER ) { + if (currentStep == IAP_STATE_BOOTLOADER) { // Freeze the tabs, they are not useful anymore and their buttons // will cause segfaults or weird stuff if we use them. - for (int i=0; i< m_config->systemElements->count(); i++) { + for (int i = 0; i < m_config->systemElements->count(); i++) { // OP-682 arriving here too "early" (before the devices are refreshed) was leading to a crash // OP-682 the crash was due to an unchecked cast in the line below that would cast a RunningDeviceGadget to a DeviceGadget - DeviceWidget *qw = dynamic_cast(m_config->systemElements->widget(i)); + DeviceWidget *qw = dynamic_cast(m_config->systemElements->widget(i)); if (qw) { // OP-682 fixed a second crash by disabling *all* buttons in the device widget // disabling the buttons is only half of the solution as even if the buttons are enabled @@ -525,6 +523,7 @@ bool UploaderGadgetWidget::autoUpdateCapable() bool UploaderGadgetWidget::autoUpdate() { Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager(); + cm->disconnectDevice(); cm->suspendPolling(); if (dfu) { @@ -534,60 +533,55 @@ bool UploaderGadgetWidget::autoUpdate() QEventLoop loop; QTimer timer; timer.setSingleShot(true); - connect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); - while(USBMonitor::instance()->availableDevices(0x20a0,-1,-1,-1).length()>0) - { - emit autoUpdateSignal(WAITING_DISCONNECT,QVariant()); - if(QMessageBox::warning(this,tr("OpenPilot Uploader"),tr("Please disconnect all openpilot boards"),QMessageBox::Ok,QMessageBox::Cancel)==QMessageBox::Cancel) - { - emit autoUpdateSignal(FAILURE,QVariant()); - return false; - } - timer.start(500); - loop.exec(); + connect(&timer, SIGNAL(timeout()), &loop, SLOT(quit())); + while (USBMonitor::instance()->availableDevices(0x20a0, -1, -1, -1).length() > 0) { + emit autoUpdateSignal(WAITING_DISCONNECT, QVariant()); + if (QMessageBox::warning(this, tr("OpenPilot Uploader"), tr("Please disconnect all openpilot boards"), QMessageBox::Ok, QMessageBox::Cancel) == QMessageBox::Cancel) { + emit autoUpdateSignal(FAILURE, QVariant()); + return false; + } + timer.start(500); + loop.exec(); } - emit autoUpdateSignal(WAITING_CONNECT,0); - autoUpdateConnectTimeout=0; + emit autoUpdateSignal(WAITING_CONNECT, 0); + autoUpdateConnectTimeout = 0; m_timer = new QTimer(this); connect(m_timer, SIGNAL(timeout()), this, SLOT(performAuto())); m_timer->start(1000); - connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)),&m_eventloop, SLOT(quit())); + connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), &m_eventloop, SLOT(quit())); m_eventloop.exec(); - if(!m_timer->isActive()) - { + if (!m_timer->isActive()) { m_timer->stop(); - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(FAILURE, QVariant()); return false; } m_timer->stop(); dfu = new DFUObject(DFU_DEBUG, false, QString()); dfu->AbortOperation(); - emit autoUpdateSignal(JUMP_TO_BL,QVariant()); - if(!dfu->enterDFU(0)) - { + emit autoUpdateSignal(JUMP_TO_BL, QVariant()); + if (!dfu->enterDFU(0)) { delete dfu; dfu = NULL; cm->resumePolling(); - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(FAILURE, QVariant()); return false; } - if(!dfu->findDevices() || (dfu->numberOfDevices != 1)) - { + if (!dfu->findDevices() || (dfu->numberOfDevices != 1)) { delete dfu; dfu = NULL; cm->resumePolling(); - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(FAILURE, QVariant()); return false; } if (dfu->numberOfDevices > 5) { delete dfu; dfu = NULL; cm->resumePolling(); - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(FAILURE, QVariant()); return false; } QString filename; - emit autoUpdateSignal(LOADING_FW,QVariant()); + emit autoUpdateSignal(LOADING_FW, QVariant()); switch (dfu->devices[0].ID) { case 0x301: filename = "fw_oplinkmini"; @@ -606,62 +600,60 @@ bool UploaderGadgetWidget::autoUpdate() filename = "fw_revolution"; break; default: - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(FAILURE, QVariant()); return false; + break; } filename = ":/firmware/" + filename + ".opfw"; QByteArray firmware; - if(!QFile::exists(filename)) - { - emit autoUpdateSignal(FAILURE,QVariant()); + if (!QFile::exists(filename)) { + emit autoUpdateSignal(FAILURE, QVariant()); return false; } QFile file(filename); if (!file.open(QIODevice::ReadOnly)) { - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(FAILURE, QVariant()); return false; } firmware = file.readAll(); connect(dfu, SIGNAL(progressUpdated(int)), this, SLOT(autoUpdateProgress(int))); connect(dfu, SIGNAL(uploadFinished(OP_DFU::Status)), &m_eventloop, SLOT(quit())); - emit autoUpdateSignal(UPLOADING_FW,QVariant()); - if(!dfu->enterDFU(0)) - { - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(UPLOADING_FW, QVariant()); + if (!dfu->enterDFU(0)) { + emit autoUpdateSignal(FAILURE, QVariant()); return false; } dfu->AbortOperation(); - if(!dfu->UploadFirmware(filename,false,0)) - { - emit autoUpdateSignal(FAILURE,QVariant()); + if (!dfu->UploadFirmware(filename, false, 0)) { + emit autoUpdateSignal(FAILURE, QVariant()); return false; } m_eventloop.exec(); QByteArray desc = firmware.right(100); - emit autoUpdateSignal(UPLOADING_DESC,QVariant()); - if(dfu->UploadDescription(desc)!= OP_DFU::Last_operation_Success) - { - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(UPLOADING_DESC, QVariant()); + if (dfu->UploadDescription(desc) != OP_DFU::Last_operation_Success) { + emit autoUpdateSignal(FAILURE, QVariant()); return false; } systemBoot(); - emit autoUpdateSignal(SUCCESS,QVariant()); + emit autoUpdateSignal(SUCCESS, QVariant()); return true; } void UploaderGadgetWidget::autoUpdateProgress(int value) { - emit autoUpdateSignal(UPLOADING_FW,value); + emit autoUpdateSignal(UPLOADING_FW, value); } /** - Attempt a guided procedure to put both boards in BL mode when - the system is not bootable - */ + Attempt a guided procedure to put both boards in BL mode when + the system is not bootable + */ void UploaderGadgetWidget::systemRescue() { Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager(); + cm->disconnectDevice(); // stop the polling thread: otherwise it will mess up DFU cm->suspendPolling(); @@ -686,10 +678,8 @@ void UploaderGadgetWidget::systemRescue() log("** Follow those instructions to attempt a system rescue **"); log("**********************************************************"); log("You will be prompted to first connect USB, then system power"); - if(USBMonitor::instance()->availableDevices(0x20a0,-1,-1,-1).length()>0) - { - if(QMessageBox::warning(this,tr("OpenPilot Uploader"),tr("Please disconnect all openpilot boards"),QMessageBox::Ok,QMessageBox::Cancel)==QMessageBox::Cancel) - { + if (USBMonitor::instance()->availableDevices(0x20a0, -1, -1, -1).length() > 0) { + if (QMessageBox::warning(this, tr("OpenPilot Uploader"), tr("Please disconnect all openpilot boards"), QMessageBox::Ok, QMessageBox::Cancel) == QMessageBox::Cancel) { m_config->rescueButton->setEnabled(true); return; } @@ -701,22 +691,21 @@ void UploaderGadgetWidget::systemRescue() log("**********************************************************"); log("You will be prompted to first connect USB, then system power"); m_progress = new QProgressDialog(tr("Please connect the board (USB only!)"), tr("Cancel"), 0, 20); - QProgressBar * bar=new QProgressBar(m_progress); + QProgressBar *bar = new QProgressBar(m_progress); bar->setFormat("Timeout"); m_progress->setBar(bar); m_progress->setMinimumDuration(0); - m_progress->setRange(0,20); + m_progress->setRange(0, 20); connect(m_progress, SIGNAL(canceled()), this, SLOT(cancel())); m_timer = new QTimer(this); connect(m_timer, SIGNAL(timeout()), this, SLOT(perform())); m_timer->start(1000); - connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)),&m_eventloop, SLOT(quit())); + connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), &m_eventloop, SLOT(quit())); m_eventloop.exec(); - if(!m_timer->isActive()) - { + if (!m_timer->isActive()) { m_progress->close(); m_timer->stop(); - QMessageBox::warning(this,tr("Openpilot Uploader"),tr("No board connection was detected!")); + QMessageBox::warning(this, tr("Openpilot Uploader"), tr("No board connection was detected!")); m_config->rescueButton->setEnabled(true); return; } @@ -726,8 +715,7 @@ void UploaderGadgetWidget::systemRescue() repaint(); dfu = new DFUObject(DFU_DEBUG, false, QString()); dfu->AbortOperation(); - if(!dfu->enterDFU(0)) - { + if (!dfu->enterDFU(0)) { log("Could not enter DFU mode."); delete dfu; dfu = NULL; @@ -735,8 +723,7 @@ void UploaderGadgetWidget::systemRescue() m_config->rescueButton->setEnabled(true); return; } - if(!dfu->findDevices() || (dfu->numberOfDevices != 1)) - { + if (!dfu->findDevices() || (dfu->numberOfDevices != 1)) { log("Could not detect a board, aborting!"); delete dfu; dfu = NULL; @@ -753,8 +740,8 @@ void UploaderGadgetWidget::systemRescue() m_config->rescueButton->setEnabled(true); return; } - for(int i=0;inumberOfDevices;i++) { - DeviceWidget* dw = new DeviceWidget(this); + for (int i = 0; i < dfu->numberOfDevices; i++) { + DeviceWidget *dw = new DeviceWidget(this); connectSignalSlot(dw); dw->setDeviceID(i); dw->setDfu(dfu); @@ -770,23 +757,20 @@ void UploaderGadgetWidget::systemRescue() void UploaderGadgetWidget::perform() { - if(m_progress->value()==19) - { + if (m_progress->value() == 19) { m_timer->stop(); m_eventloop.exit(); } - m_progress->setValue(m_progress->value()+1); + m_progress->setValue(m_progress->value() + 1); } void UploaderGadgetWidget::performAuto() { ++autoUpdateConnectTimeout; - emit autoUpdateSignal(WAITING_CONNECT,autoUpdateConnectTimeout*5); - if(autoUpdateConnectTimeout==20) - { + emit autoUpdateSignal(WAITING_CONNECT, autoUpdateConnectTimeout * 5); + if (autoUpdateConnectTimeout == 20) { m_timer->stop(); m_eventloop.exit(); } - } void UploaderGadgetWidget::cancel() { @@ -833,13 +817,13 @@ void UploaderGadgetWidget::downloadEnded(bool succeed) } /** - Update log entry - */ + Update log entry + */ void UploaderGadgetWidget::log(QString str) { - qDebug() << str; - m_config->textBrowser->append(str); - m_config->textBrowser->repaint(); + qDebug() << str; + m_config->textBrowser->append(str); + m_config->textBrowser->repaint(); } void UploaderGadgetWidget::clearLog() @@ -848,15 +832,15 @@ void UploaderGadgetWidget::clearLog() } /** - * Remove all the device widgets... - */ + * Remove all the device widgets... + */ UploaderGadgetWidget::~UploaderGadgetWidget() { while (m_config->systemElements->count()) { - QWidget *qw = m_config->systemElements->widget(0); - m_config->systemElements->removeTab(0); - delete qw; - qw = 0; + QWidget *qw = m_config->systemElements->widget(0); + m_config->systemElements->removeTab(0); + delete qw; + qw = 0; } if (m_progress) { delete m_progress; @@ -870,13 +854,13 @@ UploaderGadgetWidget::~UploaderGadgetWidget() /** -Shows a message box with an error string. + Shows a message box with an error string. -@param errorString The error string to display. + @param errorString The error string to display. -@param errorNumber Not used + @param errorNumber Not used -*/ + */ void UploaderGadgetWidget::error(QString errorString, int errorNumber) { Q_UNUSED(errorNumber); @@ -888,13 +872,13 @@ void UploaderGadgetWidget::error(QString errorString, int errorNumber) } /** -Shows a message box with an information string. + Shows a message box with an information string. -@param infoString The information string to display. + @param infoString The information string to display. -@param infoNumber Not used + @param infoNumber Not used -*/ + */ void UploaderGadgetWidget::info(QString infoString, int infoNumber) { Q_UNUSED(infoNumber); @@ -904,48 +888,45 @@ void UploaderGadgetWidget::info(QString infoString, int infoNumber) void UploaderGadgetWidget::versionMatchCheck() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectUtilManager *utilMngr = pm->getObject(); + UAVObjectUtilManager *utilMngr = pm->getObject(); deviceDescriptorStruct boardDescription = utilMngr->getBoardDescriptionStruct(); QByteArray uavoHashArray; QString uavoHash = QString::fromLatin1(Core::Constants::UAVOSHA1_STR); + uavoHash.chop(2); - uavoHash.remove(0,2); - uavoHash=uavoHash.trimmed(); + uavoHash.remove(0, 2); + uavoHash = uavoHash.trimmed(); bool ok; - foreach(QString str,uavoHash.split(",")) - { - uavoHashArray.append(str.toInt(&ok,16)); + foreach(QString str, uavoHash.split(",")) { + uavoHashArray.append(str.toInt(&ok, 16)); } - QByteArray fwVersion=boardDescription.uavoHash; + QByteArray fwVersion = boardDescription.uavoHash; if (fwVersion != uavoHashArray) { - QString gcsDescription = QString::fromLatin1(Core::Constants::GCS_REVISION_STR); - QString gcsGitHash = gcsDescription.mid(gcsDescription.indexOf(":")+1, 8); - gcsGitHash.remove( QRegExp("^[0]*") ); - QString gcsGitDate = gcsDescription.mid(gcsDescription.indexOf(" ")+1, 14); + QString gcsGitHash = gcsDescription.mid(gcsDescription.indexOf(":") + 1, 8); + gcsGitHash.remove(QRegExp("^[0]*")); + QString gcsGitDate = gcsDescription.mid(gcsDescription.indexOf(" ") + 1, 14); QString gcsUavoHashStr; QString fwUavoHashStr; - foreach(char i, fwVersion) - { - fwUavoHashStr.append(QString::number(i,16).right(2)); + foreach(char i, fwVersion) { + fwUavoHashStr.append(QString::number(i, 16).right(2)); } - foreach(char i, uavoHashArray) - { - gcsUavoHashStr.append(QString::number(i,16).right(2)); + foreach(char i, uavoHashArray) { + gcsUavoHashStr.append(QString::number(i, 16).right(2)); } - QString gcsVersion = gcsGitDate + " (" + gcsGitHash + "-"+ gcsUavoHashStr.left(8) + ")"; - QString fwVersion = boardDescription.gitDate + " (" + boardDescription.gitHash + "-" + fwUavoHashStr.left(8) + ")"; + QString gcsVersion = gcsGitDate + " (" + gcsGitHash + "-" + gcsUavoHashStr.left(8) + ")"; + QString fwVersion = boardDescription.gitDate + " (" + boardDescription.gitHash + "-" + fwUavoHashStr.left(8) + ")"; - QString warning = QString(tr( - "GCS and firmware versions of the UAV objects set do not match which can cause configuration problems. " - "GCS version: %1 Firmware version: %2.")).arg(gcsVersion).arg(fwVersion); + QString warning = QString(tr( + "GCS and firmware versions of the UAV objects set do not match which can cause configuration problems. " + "GCS version: %1 Firmware version: %2.")).arg(gcsVersion).arg(fwVersion); msg->showMessage(warning); } } void UploaderGadgetWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/AoBZ", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/AoBZ", QUrl::StrictMode)); } diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h index 0654aa1cc..4f088c9aa 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h @@ -63,14 +63,13 @@ using namespace uploader; class FlightStatus; -class UPLOADER_EXPORT UploaderGadgetWidget : public QWidget -{ +class UPLOADER_EXPORT UploaderGadgetWidget : public QWidget { Q_OBJECT public: UploaderGadgetWidget(QWidget *parent = 0); - ~UploaderGadgetWidget(); + ~UploaderGadgetWidget(); void log(QString str); bool autoUpdateCapable(); public slots: @@ -81,35 +80,35 @@ public slots: bool autoUpdate(); void autoUpdateProgress(int); signals: - void autoUpdateSignal(uploader::AutoUpdateStep,QVariant); + void autoUpdateSignal(uploader::AutoUpdateStep, QVariant); private: - Ui_UploaderWidget *m_config; - DFUObject *dfu; - IAPStep currentStep; - bool resetOnly; - void clearLog(); - QString getPortDevice(const QString &friendName); - QProgressDialog* m_progress; - QTimer* m_timer; - QLineEdit* openFileNameLE; - QEventLoop m_eventloop; - QErrorMessage * msg; - void connectSignalSlot(QWidget * widget); - int autoUpdateConnectTimeout; - FlightStatus * getFlightStatus(); - void bootButtonsSetEnable(bool enabled); + Ui_UploaderWidget *m_config; + DFUObject *dfu; + IAPStep currentStep; + bool resetOnly; + void clearLog(); + QString getPortDevice(const QString &friendName); + QProgressDialog *m_progress; + QTimer *m_timer; + QLineEdit *openFileNameLE; + QEventLoop m_eventloop; + QErrorMessage *msg; + void connectSignalSlot(QWidget *widget); + int autoUpdateConnectTimeout; + FlightStatus *getFlightStatus(); + void bootButtonsSetEnable(bool enabled); private slots: void onPhisicalHWConnect(); void versionMatchCheck(); - void error(QString errorString,int errorNumber); - void info(QString infoString,int infoNumber); - void goToBootloader(UAVObject* = NULL, bool = false); + void error(QString errorString, int errorNumber); + void info(QString infoString, int infoNumber); + void goToBootloader(UAVObject * = NULL, bool = false); void systemHalt(); void systemReset(); void systemBoot(); void systemSafeBoot(); - void systemEraseBoot(); - void commonSystemBoot(bool safeboot = false, bool erase = false); + void systemEraseBoot(); + void commonSystemBoot(bool safeboot = false, bool erase = false); void systemRescue(); void getSerialPorts(); void perform(); diff --git a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp index 7fd5dd79d..629eb82d6 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp @@ -1,4 +1,3 @@ - /** ****************************************************************************** * @@ -33,31 +32,30 @@ UploaderPlugin::UploaderPlugin() { - // Do nothing + // Do nothing } UploaderPlugin::~UploaderPlugin() { - // Do nothing + // Do nothing } -bool UploaderPlugin::initialize(const QStringList& args, QString *errMsg) +bool UploaderPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new UploaderGadgetFactory(this); - addAutoReleasedObject(mf); - return true; + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new UploaderGadgetFactory(this); + addAutoReleasedObject(mf); + return true; } void UploaderPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void UploaderPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(UploaderPlugin) - diff --git a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h index e74c342ca..082519f43 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h @@ -33,16 +33,15 @@ class UploaderGadgetFactory; -class UPLOADER_EXPORT UploaderPlugin : public ExtensionSystem::IPlugin -{ +class UPLOADER_EXPORT UploaderPlugin : public ExtensionSystem::IPlugin { public: - UploaderPlugin(); - ~UploaderPlugin(); + UploaderPlugin(); + ~UploaderPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - UploaderGadgetFactory *mf; + UploaderGadgetFactory *mf; }; #endif // UPLOADERPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/welcome/welcome_global.h b/ground/openpilotgcs/src/plugins/welcome/welcome_global.h index 91386815e..15fd8de88 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcome_global.h +++ b/ground/openpilotgcs/src/plugins/welcome/welcome_global.h @@ -11,18 +11,18 @@ * @brief The GCS Welcome plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp b/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp index a109f328a..895597672 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp +++ b/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp @@ -11,18 +11,18 @@ * @brief The GCS Welcome plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -56,17 +56,14 @@ using namespace ExtensionSystem; using namespace Utils; namespace Welcome { - -struct WelcomeModePrivate -{ +struct WelcomeModePrivate { WelcomeModePrivate(); QDeclarativeView *declarativeView; }; WelcomeModePrivate::WelcomeModePrivate() -{ -} +{} // --- WelcomeMode WelcomeMode::WelcomeMode() : @@ -100,12 +97,12 @@ int WelcomeMode::priority() const return m_priority; } -QWidget* WelcomeMode::widget() +QWidget *WelcomeMode::widget() { return m_d->declarativeView; } -const char* WelcomeMode::uniqueModeName() const +const char *WelcomeMode::uniqueModeName() const { return Core::Constants::MODE_WELCOME; } @@ -114,6 +111,7 @@ QList WelcomeMode::context() const { static QList contexts = QList() << Core::UniqueIDManager::instance()->uniqueIdentifier(Core::Constants::C_WELCOME_MODE); + return contexts; } @@ -131,5 +129,4 @@ void WelcomeMode::triggerAction(const QString &actionId) { Core::ModeManager::instance()->triggerAction(actionId); } - } // namespace Welcome diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomemode.h b/ground/openpilotgcs/src/plugins/welcome/welcomemode.h index 14a00312e..fa3f22856 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomemode.h +++ b/ground/openpilotgcs/src/plugins/welcome/welcomemode.h @@ -11,18 +11,18 @@ * @brief The GCS Welcome plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,11 +40,9 @@ class QUrl; QT_END_NAMESPACE namespace Welcome { - struct WelcomeModePrivate; -class WELCOME_EXPORT WelcomeMode : public Core::IMode -{ +class WELCOME_EXPORT WelcomeMode : public Core::IMode { Q_OBJECT public: @@ -59,8 +57,14 @@ public: const char *uniqueModeName() const; QList context() const; void activated(); - QString contextHelpId() const { return QLatin1String("OpenPilot GCS"); } - void setPriority(int priority) { m_priority = priority; } + QString contextHelpId() const + { + return QLatin1String("OpenPilot GCS"); + } + void setPriority(int priority) + { + m_priority = priority; + } public slots: void openUrl(const QString &url); @@ -71,7 +75,6 @@ private: WelcomeModePrivate *m_d; int m_priority; }; - } // namespace Welcome #endif // WELCOMEMODE_H diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.cpp b/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.cpp index 3e79878b5..85e427739 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.cpp +++ b/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.cpp @@ -11,18 +11,18 @@ * @brief The GCS Welcome plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -47,9 +47,8 @@ using namespace Welcome::Internal; WelcomePlugin::WelcomePlugin() - : m_welcomeMode(0) -{ -} + : m_welcomeMode(0) +{} WelcomePlugin::~WelcomePlugin() { @@ -64,7 +63,7 @@ WelcomePlugin::~WelcomePlugin() \a error_message can be used to pass an error message to the plugin system, if there was any. -*/ + */ bool WelcomePlugin::initialize(const QStringList &arguments, QString *error_message) { Q_UNUSED(arguments) @@ -86,7 +85,7 @@ bool WelcomePlugin::initialize(const QStringList &arguments, QString *error_mess The WelcomePlugin doesn't need things from other plugins, so it does nothing here. -*/ + */ void WelcomePlugin::extensionsInitialized() { Core::ModeManager::instance()->activateMode(m_welcomeMode->uniqueModeName()); diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.h b/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.h index 1f8e31988..0a9d65604 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.h +++ b/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.h @@ -11,18 +11,18 @@ * @brief The GCS Welcome plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,14 +32,11 @@ #include namespace Welcome { - class WelcomeMode; namespace Internal { - class WelcomePlugin - : public ExtensionSystem::IPlugin -{ + : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -53,7 +50,6 @@ public: private: WelcomeMode *m_welcomeMode; }; - } // namespace Welcome } // namespace Internal diff --git a/ground/openpilotgcs/src/shared/namespace_global.h b/ground/openpilotgcs/src/shared/namespace_global.h index 80ac9aaa4..c72a21a8a 100644 --- a/ground/openpilotgcs/src/shared/namespace_global.h +++ b/ground/openpilotgcs/src/shared/namespace_global.h @@ -4,25 +4,25 @@ * @file namespace_global.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,7 +41,7 @@ # define QT_BEGIN_MOC_NAMESPACE # define QT_END_MOC_NAMESPACE # define QT_FORWARD_DECLARE_CLASS(name) class name; -# define QT_MANGLE_NAMESPACE(name) name +# define QT_MANGLE_NAMESPACE(name) name #endif #endif // NAMESPACE_GLOBAL_H diff --git a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.cpp b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.cpp index baed7993b..e4ee86cb9 100644 --- a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.cpp +++ b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.cpp @@ -4,32 +4,31 @@ * @file qtlockedfile.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "qtlockedfile.h" namespace SharedTools { - /*! \class QtLockedFile @@ -52,7 +51,7 @@ namespace SharedTools { The lock provided by an instance of \e QtLockedFile is released whenever the program terminates. This is true even when the program crashes and no destructors are called. -*/ + */ /*! \enum QtLockedFile::LockMode @@ -61,22 +60,22 @@ namespace SharedTools { \value ReadLock A read lock. \value WriteLock A write lock. \value NoLock Neither a read lock nor a write lock. -*/ + */ /*! Constructs an unlocked \e QtLockedFile object. This constructor behaves in the same way as \e QFile::QFile(). \sa QFile::QFile() -*/ + */ QtLockedFile::QtLockedFile() : QFile() { #ifdef Q_OS_WIN m_semaphore_hnd = 0; - m_mutex_hnd = 0; + m_mutex_hnd = 0; #endif - m_lock_mode = NoLock; + m_lock_mode = NoLock; } /*! @@ -84,15 +83,15 @@ QtLockedFile::QtLockedFile() the same way as \e QFile::QFile(const QString&). \sa QFile::QFile() -*/ + */ QtLockedFile::QtLockedFile(const QString &name) : QFile(name) { #ifdef Q_OS_WIN m_semaphore_hnd = 0; - m_mutex_hnd = 0; + m_mutex_hnd = 0; #endif - m_lock_mode = NoLock; + m_lock_mode = NoLock; } /*! @@ -100,7 +99,7 @@ QtLockedFile::QtLockedFile(const QString &name) otherwise returns \e false. \sa lockMode() -*/ + */ bool QtLockedFile::isLocked() const { return m_lock_mode != NoLock; @@ -110,7 +109,7 @@ bool QtLockedFile::isLocked() const Returns the type of lock currently held by this object, or \e QtLockedFile::NoLock. \sa isLocked() -*/ + */ QtLockedFile::LockMode QtLockedFile::lockMode() const { return m_lock_mode; @@ -133,7 +132,7 @@ QtLockedFile::LockMode QtLockedFile::lockMode() const and \e false otherwise. \sa unlock(), isLocked(), lockMode() -*/ + */ /*! \fn bool QtLockedFile::unlock() @@ -146,12 +145,11 @@ QtLockedFile::LockMode QtLockedFile::lockMode() const this object, and \e false otherwise. \sa lock(), isLocked(), lockMode() -*/ + */ /*! \fn QtLockedFile::~QtLockedFile() Destroys the \e QtLockedFile object. If any locks were held, they are released. -*/ - + */ } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.h b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.h index cbeb80fed..3f8622523 100644 --- a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.h +++ b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.h @@ -4,25 +4,25 @@ * @file qtlockedfile.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -48,21 +48,19 @@ #endif namespace SharedTools { - -class QT_QTLOCKEDFILE_EXPORT QtLockedFile : public QFile -{ -public: +class QT_QTLOCKEDFILE_EXPORT QtLockedFile : public QFile { +public: enum LockMode { NoLock = 0, ReadLock, WriteLock }; QtLockedFile(); QtLockedFile(const QString &name); ~QtLockedFile(); - + bool lock(LockMode mode, bool block = true); bool unlock(); bool isLocked() const; LockMode lockMode() const; - + private: #ifdef Q_OS_WIN Qt::HANDLE m_semaphore_hnd; @@ -70,7 +68,6 @@ private: #endif LockMode m_lock_mode; }; - } // namespace SharedTools #endif // QTLOCKEDFILE_H diff --git a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_unix.cpp b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_unix.cpp index f4a0decc0..e0a460daf 100644 --- a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_unix.cpp +++ b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_unix.cpp @@ -4,25 +4,25 @@ * @file qtlockedfile_unix.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,38 +34,41 @@ #include namespace SharedTools { - bool QtLockedFile::lock(LockMode mode, bool block) { if (!isOpen()) { qWarning("QtLockedFile::lock(): file is not opened"); return false; } - - if (mode == NoLock) - return unlock(); - - if (mode == m_lock_mode) - return true; - if (m_lock_mode != NoLock) + if (mode == NoLock) { + return unlock(); + } + + if (mode == m_lock_mode) { + return true; + } + + if (m_lock_mode != NoLock) { unlock(); + } struct flock fl; fl.l_whence = SEEK_SET; - fl.l_start = 0; - fl.l_len = 0; - fl.l_type = (mode == ReadLock) ? F_RDLCK : F_WRLCK; + fl.l_start = 0; + fl.l_len = 0; + fl.l_type = (mode == ReadLock) ? F_RDLCK : F_WRLCK; int cmd = block ? F_SETLKW : F_SETLK; int ret = fcntl(handle(), cmd, &fl); - + if (ret == -1) { - if (errno != EINTR && errno != EAGAIN) + if (errno != EINTR && errno != EAGAIN) { qWarning("QtLockedFile::lock(): fcntl: %s", strerror(errno)); + } return false; } - + m_lock_mode = mode; return true; } @@ -78,29 +81,30 @@ bool QtLockedFile::unlock() return false; } - if (!isLocked()) + if (!isLocked()) { return true; + } struct flock fl; fl.l_whence = SEEK_SET; - fl.l_start = 0; - fl.l_len = 0; - fl.l_type = F_UNLCK; + fl.l_start = 0; + fl.l_len = 0; + fl.l_type = F_UNLCK; int ret = fcntl(handle(), F_SETLKW, &fl); - + if (ret == -1) { qWarning("QtLockedFile::lock(): fcntl: %s", strerror(errno)); return false; } - + m_lock_mode = NoLock; return true; } QtLockedFile::~QtLockedFile() { - if (isOpen()) + if (isOpen()) { unlock(); + } } - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_win.cpp b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_win.cpp index 061787896..fa63c2e22 100644 --- a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_win.cpp +++ b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_win.cpp @@ -4,25 +4,25 @@ * @file qtlockedfile_win.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,24 +32,26 @@ #include namespace SharedTools { - #define SEMAPHORE_PREFIX "QtLockedFile semaphore " -#define MUTEX_PREFIX "QtLockedFile mutex " -#define SEMAPHORE_MAX 100 +#define MUTEX_PREFIX "QtLockedFile mutex " +#define SEMAPHORE_MAX 100 static QString errorCodeToString(DWORD errorCode) { QString result; char *data = 0; - FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, - 0, errorCode, 0, - (char*)&data, 0, 0); - result = QString::fromLocal8Bit(data); - if (data != 0) - LocalFree(data); - if (result.endsWith("\n")) + FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, + 0, errorCode, 0, + (char *)&data, 0, 0); + result = QString::fromLocal8Bit(data); + if (data != 0) { + LocalFree(data); + } + + if (result.endsWith("\n")) { result.truncate(result.length() - 1); + } return result; } @@ -61,24 +63,27 @@ bool QtLockedFile::lock(LockMode mode, bool block) return false; } - if (mode == m_lock_mode) + if (mode == m_lock_mode) { return true; + } - if (m_lock_mode != 0) + if (m_lock_mode != 0) { unlock(); + } if (m_semaphore_hnd == 0) { QFileInfo fi(*this); QString sem_name = QString::fromLatin1(SEMAPHORE_PREFIX) + fi.absoluteFilePath().toLower(); - QT_WA( { - m_semaphore_hnd = CreateSemaphoreW(0, SEMAPHORE_MAX, SEMAPHORE_MAX, - (TCHAR*)sem_name.utf16()); - } , { - m_semaphore_hnd = CreateSemaphoreA(0, SEMAPHORE_MAX, SEMAPHORE_MAX, - sem_name.toLocal8Bit().constData()); - } ); + QT_WA({ + m_semaphore_hnd = CreateSemaphoreW(0, SEMAPHORE_MAX, SEMAPHORE_MAX, + (TCHAR *)sem_name.utf16()); + }, { + m_semaphore_hnd = CreateSemaphoreA(0, SEMAPHORE_MAX, SEMAPHORE_MAX, + sem_name.toLocal8Bit().constData()); + } + ); if (m_semaphore_hnd == 0) { qWarning("QtLockedFile::lock(): CreateSemaphore: %s", @@ -97,11 +102,12 @@ bool QtLockedFile::lock(LockMode mode, bool block) QFileInfo fi(*this); QString mut_name = QString::fromLatin1(MUTEX_PREFIX) + fi.absoluteFilePath().toLower(); - QT_WA( { - m_mutex_hnd = CreateMutexW(NULL, FALSE, (TCHAR*)mut_name.utf16()); - } , { - m_mutex_hnd = CreateMutexA(NULL, FALSE, mut_name.toLocal8Bit().constData()); - } ); + QT_WA({ + m_mutex_hnd = CreateMutexW(NULL, FALSE, (TCHAR *)mut_name.utf16()); + }, { + m_mutex_hnd = CreateMutexA(NULL, FALSE, mut_name.toLocal8Bit().constData()); + } + ); if (m_mutex_hnd == 0) { qWarning("QtLockedFile::lock(): CreateMutex: %s", @@ -110,8 +116,9 @@ bool QtLockedFile::lock(LockMode mode, bool block) } } DWORD res = WaitForSingleObject(m_mutex_hnd, block ? INFINITE : 0); - if (res == WAIT_TIMEOUT) + if (res == WAIT_TIMEOUT) { return false; + } if (res == WAIT_FAILED) { qWarning("QtLockedFile::lock(): WaitForSingleObject (mutex): %s", errorCodeToString(GetLastError()).toLatin1().constData()); @@ -131,22 +138,25 @@ bool QtLockedFile::lock(LockMode mode, bool block) // Fall through } } - if (gotMutex) + if (gotMutex) { ReleaseMutex(m_mutex_hnd); + } return false; - } + } if (res != WAIT_OBJECT_0) { - if (gotMutex) + if (gotMutex) { ReleaseMutex(m_mutex_hnd); + } qWarning("QtLockedFile::lock(): WaitForSingleObject (semaphore): %s", - errorCodeToString(GetLastError()).toLatin1().constData()); + errorCodeToString(GetLastError()).toLatin1().constData()); return false; } } m_lock_mode = mode; - if (gotMutex) + if (gotMutex) { ReleaseMutex(m_mutex_hnd); + } return true; } @@ -157,19 +167,21 @@ bool QtLockedFile::unlock() return false; } - if (!isLocked()) + if (!isLocked()) { return true; + } int increment; - if (m_lock_mode == ReadLock) + if (m_lock_mode == ReadLock) { increment = 1; - else + } else { increment = SEMAPHORE_MAX; + } DWORD ret = ReleaseSemaphore(m_semaphore_hnd, increment, 0); if (ret == 0) { qWarning("QtLockedFile::unlock(): ReleaseSemaphore: %s", - errorCodeToString(GetLastError()).toLatin1().constData()); + errorCodeToString(GetLastError()).toLatin1().constData()); return false; } @@ -179,13 +191,14 @@ bool QtLockedFile::unlock() QtLockedFile::~QtLockedFile() { - if (isOpen()) + if (isOpen()) { unlock(); + } if (m_mutex_hnd != 0) { DWORD ret = CloseHandle(m_mutex_hnd); if (ret == 0) { qWarning("QtLockedFile::~QtLockedFile(): CloseHandle (mutex): %s", - errorCodeToString(GetLastError()).toLatin1().constData()); + errorCodeToString(GetLastError()).toLatin1().constData()); } m_mutex_hnd = 0; } @@ -193,10 +206,9 @@ QtLockedFile::~QtLockedFile() DWORD ret = CloseHandle(m_semaphore_hnd); if (ret == 0) { qWarning("QtLockedFile::~QtLockedFile(): CloseHandle (semaphore): %s", - errorCodeToString(GetLastError()).toLatin1().constData()); + errorCodeToString(GetLastError()).toLatin1().constData()); } m_semaphore_hnd = 0; } } - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.cpp b/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.cpp index 1d9a8dd8d..448c26b49 100644 --- a/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.cpp +++ b/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.cpp @@ -4,25 +4,25 @@ * @file qtlocalpeer.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,7 @@ #if defined(Q_OS_WIN) #include #include -typedef BOOL(WINAPI*PProcessIdToSessionId)(DWORD,DWORD*); +typedef BOOL (WINAPI * PProcessIdToSessionId)(DWORD, DWORD *); static PProcessIdToSessionId pProcessIdToSessionId = 0; #endif @@ -44,18 +44,17 @@ static PProcessIdToSessionId pProcessIdToSessionId = 0; #endif namespace SharedTools { - const char *QtLocalPeer::ack = "ack"; QtLocalPeer::QtLocalPeer(QObject *parent, const QString &appId) : QObject(parent), id(appId) { - if (id.isEmpty()) - id = QCoreApplication::applicationFilePath(); //### On win, check if this returns .../argv[0] without casefolding; .\MYAPP == .\myapp on Win - + if (id.isEmpty()) { + id = QCoreApplication::applicationFilePath(); // ### On win, check if this returns .../argv[0] without casefolding; .\MYAPP == .\myapp on Win + } QByteArray idc = id.toUtf8(); - quint16 idNum = qChecksum(idc.constData(), idc.size()); - //### could do: two 16bit checksums over separate halves of id, for a 32bit result - improved uniqeness probability. Every-other-char split would be best. + quint16 idNum = qChecksum(idc.constData(), idc.size()); + // ### could do: two 16bit checksums over separate halves of id, for a 32bit result - improved uniqeness probability. Every-other-char split would be best. socketName = QLatin1String("qtsingleapplication-") + QString::number(idNum, 16); @@ -73,7 +72,7 @@ QtLocalPeer::QtLocalPeer(QObject *parent, const QString &appId) socketName += QLatin1Char('-') + QString::number(::getuid(), 16); #endif - server = new QLocalServer(this); + server = new QLocalServer(this); QString lockName = QDir(QDir::tempPath()).absolutePath() + QLatin1Char('/') + socketName + QLatin1String("-lockfile"); @@ -83,34 +82,40 @@ QtLocalPeer::QtLocalPeer(QObject *parent, const QString &appId) bool QtLocalPeer::isClient() { - if (lockFile.isLocked()) + if (lockFile.isLocked()) { return false; + } - if (!lockFile.lock(QtLockedFile::WriteLock, false)) + if (!lockFile.lock(QtLockedFile::WriteLock, false)) { return true; + } - if (!QLocalServer::removeServer(socketName)) + if (!QLocalServer::removeServer(socketName)) { qWarning("QtSingleCoreApplication: could not cleanup socket"); + } bool res = server->listen(socketName); - if (!res) + if (!res) { qWarning("QtSingleCoreApplication: listen on local socket failed, %s", qPrintable(server->errorString())); + } QObject::connect(server, SIGNAL(newConnection()), SLOT(receiveConnection())); return false; } bool QtLocalPeer::sendMessage(const QString &message, int timeout) { - if (!isClient()) + if (!isClient()) { return false; + } QLocalSocket socket; bool connOk = false; for (int i = 0; i < 2; i++) { // Try twice, in case the other instance is just starting up socket.connectToServer(socketName); - connOk = socket.waitForConnected(timeout/2); - if (connOk || i) + connOk = socket.waitForConnected(timeout / 2); + if (connOk || i) { break; + } int ms = 250; #if defined(Q_OS_WIN) Sleep(DWORD(ms)); @@ -119,8 +124,9 @@ bool QtLocalPeer::sendMessage(const QString &message, int timeout) nanosleep(&ts, NULL); #endif } - if (!connOk) + if (!connOk) { return false; + } QByteArray uMsg(message.toUtf8()); QDataStream ds(&socket); @@ -133,28 +139,31 @@ bool QtLocalPeer::sendMessage(const QString &message, int timeout) void QtLocalPeer::receiveConnection() { - QLocalSocket* socket = server->nextPendingConnection(); - if (!socket) + QLocalSocket *socket = server->nextPendingConnection(); + + if (!socket) { return; + } // Why doesn't Qt have a blocking stream that takes care of this shait??? - while (socket->bytesAvailable() < static_cast(sizeof(quint32))) + while (socket->bytesAvailable() < static_cast(sizeof(quint32))) { socket->waitForReadyRead(); + } QDataStream ds(socket); QByteArray uMsg; quint32 remaining; ds >> remaining; uMsg.resize(remaining); int got = 0; - char* uMsgBuf = uMsg.data(); - //qDebug() << "RCV: remaining" << remaining; + char *uMsgBuf = uMsg.data(); + // qDebug() << "RCV: remaining" << remaining; do { - got = ds.readRawData(uMsgBuf, remaining); + got = ds.readRawData(uMsgBuf, remaining); remaining -= got; - uMsgBuf += got; - //qDebug() << "RCV: got" << got << "remaining" << remaining; + uMsgBuf += got; + // qDebug() << "RCV: got" << got << "remaining" << remaining; } while (remaining && got >= 0 && socket->waitForReadyRead(2000)); - //### error check: got<0 + // ### error check: got<0 if (got < 0) { qWarning() << "QtLocalPeer: Message reception failed" << socket->errorString(); delete socket; @@ -167,5 +176,4 @@ void QtLocalPeer::receiveConnection() delete socket; emit messageReceived(message); // ##(might take a long time to return) } - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.h b/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.h index 7d2a39152..82c215d04 100644 --- a/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.h +++ b/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.h @@ -4,25 +4,25 @@ * @file qtlocalpeer.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,9 +33,7 @@ #include namespace SharedTools { - -class QtLocalPeer : public QObject -{ +class QtLocalPeer : public QObject { Q_OBJECT public: @@ -43,7 +41,9 @@ public: bool isClient(); bool sendMessage(const QString &message, int timeout); QString applicationId() const - { return id; } + { + return id; + } Q_SIGNALS: void messageReceived(const QString &message); @@ -54,11 +54,10 @@ protected Q_SLOTS: protected: QString id; QString socketName; - QLocalServer* server; + QLocalServer *server; QtLockedFile lockFile; private: - static const char* ack; + static const char *ack; }; - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.cpp b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.cpp index 724fc20dd..5d1394cac 100644 --- a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.cpp +++ b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.cpp @@ -4,25 +4,25 @@ * @file qtsingleapplication.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,30 +33,29 @@ #include namespace SharedTools { - void QtSingleApplication::sysInit(const QString &appId) { actWin = 0; - peer = new QtLocalPeer(this, appId); - connect(peer, SIGNAL(messageReceived(const QString&)), SIGNAL(messageReceived(const QString&))); + peer = new QtLocalPeer(this, appId); + connect(peer, SIGNAL(messageReceived(const QString &)), SIGNAL(messageReceived(const QString &))); } -QtSingleApplication::QtSingleApplication(int &argc, char **argv, bool GUIenabled) +QtSingleApplication::QtSingleApplication(int &argc, char * *argv, bool GUIenabled) : QApplication(argc, argv, GUIenabled) { sysInit(); } -QtSingleApplication::QtSingleApplication(const QString &appId, int &argc, char **argv) +QtSingleApplication::QtSingleApplication(const QString &appId, int &argc, char * *argv) : QApplication(argc, argv) { sysInit(appId); } -QtSingleApplication::QtSingleApplication(int &argc, char **argv, Type type) +QtSingleApplication::QtSingleApplication(int &argc, char * *argv, Type type) : QApplication(argc, argv, type) { sysInit(); @@ -64,20 +63,20 @@ QtSingleApplication::QtSingleApplication(int &argc, char **argv, Type type) #if defined(Q_WS_X11) -QtSingleApplication::QtSingleApplication(Display* dpy, Qt::HANDLE visual, Qt::HANDLE colormap) +QtSingleApplication::QtSingleApplication(Display *dpy, Qt::HANDLE visual, Qt::HANDLE colormap) : QApplication(dpy, visual, colormap) { sysInit(); } -QtSingleApplication::QtSingleApplication(Display *dpy, int &argc, char **argv, Qt::HANDLE visual, Qt::HANDLE cmap) +QtSingleApplication::QtSingleApplication(Display *dpy, int &argc, char * *argv, Qt::HANDLE visual, Qt::HANDLE cmap) : QApplication(dpy, argc, argv, visual, cmap) { sysInit(); } -QtSingleApplication::QtSingleApplication(Display* dpy, const QString &appId, - int argc, char **argv, Qt::HANDLE visual, Qt::HANDLE colormap) +QtSingleApplication::QtSingleApplication(Display *dpy, const QString &appId, + int argc, char * *argv, Qt::HANDLE visual, Qt::HANDLE colormap) : QApplication(dpy, argc, argv, visual, colormap) { sysInit(appId); @@ -87,7 +86,7 @@ QtSingleApplication::QtSingleApplication(Display* dpy, const QString &appId, bool QtSingleApplication::event(QEvent *event) { if (event->type() == QEvent::FileOpen) { - QFileOpenEvent *foe = static_cast(event); + QFileOpenEvent *foe = static_cast(event); emit fileOpenRequest(foe->file()); return true; } @@ -115,14 +114,15 @@ QString QtSingleApplication::id() const void QtSingleApplication::setActivationWindow(QWidget *aw, bool activateOnMessage) { actWin = aw; - if (activateOnMessage) + if (activateOnMessage) { connect(peer, SIGNAL(messageReceived(QString)), this, SLOT(activateWindow())); - else + } else { disconnect(peer, SIGNAL(messageReceived(QString)), this, SLOT(activateWindow())); + } } -QWidget* QtSingleApplication::activationWindow() const +QWidget *QtSingleApplication::activationWindow() const { return actWin; } @@ -136,5 +136,4 @@ void QtSingleApplication::activateWindow() actWin->activateWindow(); } } - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.h b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.h index 9e0e7cd2b..2d1a6d05e 100644 --- a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.h +++ b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.h @@ -4,52 +4,50 @@ * @file qtsingleapplication.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include namespace SharedTools { - class QtLocalPeer; -class QtSingleApplication : public QApplication -{ +class QtSingleApplication : public QApplication { Q_OBJECT public: - QtSingleApplication(int &argc, char **argv, bool GUIenabled = true); - QtSingleApplication(const QString &id, int &argc, char **argv); - QtSingleApplication(int &argc, char **argv, Type type); + QtSingleApplication(int &argc, char * *argv, bool GUIenabled = true); + QtSingleApplication(const QString &id, int &argc, char * *argv); + QtSingleApplication(int &argc, char * *argv, Type type); #if defined(Q_WS_X11) QtSingleApplication(Display *dpy, Qt::HANDLE visual = 0, Qt::HANDLE colormap = 0); - QtSingleApplication(Display *dpy, int &argc, char **argv, Qt::HANDLE visual = 0, Qt::HANDLE cmap = 0); + QtSingleApplication(Display *dpy, int &argc, char * *argv, Qt::HANDLE visual = 0, Qt::HANDLE cmap = 0); #endif bool isRunning(); QString id() const; - void setActivationWindow(QWidget* aw, bool activateOnMessage = true); - QWidget* activationWindow() const; + void setActivationWindow(QWidget *aw, bool activateOnMessage = true); + QWidget *activationWindow() const; bool event(QEvent *event); @@ -57,13 +55,15 @@ public Q_SLOTS: bool sendMessage(const QString &message, int timeout = 5000); void activateWindow(); -//Obsolete methods: +// Obsolete methods: public: void initialize(bool = true) - { isRunning(); } + { + isRunning(); + } #if defined(Q_WS_X11) - QtSingleApplication(Display* dpy, const QString &id, int argc, char **argv, Qt::HANDLE visual = 0, Qt::HANDLE colormap = 0); + QtSingleApplication(Display *dpy, const QString &id, int argc, char * *argv, Qt::HANDLE visual = 0, Qt::HANDLE colormap = 0); #endif // end obsolete methods @@ -76,5 +76,4 @@ private: QtLocalPeer *peer; QWidget *actWin; }; - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.cpp b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.cpp index ac556eae7..47eb0c359 100644 --- a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.cpp +++ b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.cpp @@ -4,25 +4,25 @@ * @file qtsinglecoreapplication.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,8 +30,7 @@ #include "qtlocalpeer.h" namespace SharedTools { - -QtSingleCoreApplication::QtSingleCoreApplication(int &argc, char **argv) +QtSingleCoreApplication::QtSingleCoreApplication(int &argc, char * *argv) : QCoreApplication(argc, argv) { peer = new QtLocalPeer(this); @@ -39,7 +38,7 @@ QtSingleCoreApplication::QtSingleCoreApplication(int &argc, char **argv) } -QtSingleCoreApplication::QtSingleCoreApplication(const QString &appId, int &argc, char **argv) +QtSingleCoreApplication::QtSingleCoreApplication(const QString &appId, int &argc, char * *argv) : QCoreApplication(argc, argv) { peer = new QtLocalPeer(this, appId); @@ -63,5 +62,4 @@ QString QtSingleCoreApplication::id() const { return peer->applicationId(); } - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.h b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.h index 9e8ca5344..0a63e243a 100644 --- a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.h +++ b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.h @@ -4,41 +4,39 @@ * @file qtsinglecoreapplication.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include namespace SharedTools { - class QtLocalPeer; -class QtSingleCoreApplication : public QCoreApplication -{ +class QtSingleCoreApplication : public QCoreApplication { Q_OBJECT public: - QtSingleCoreApplication(int &argc, char **argv); - QtSingleCoreApplication(const QString &id, int &argc, char **argv); + QtSingleCoreApplication(int &argc, char * *argv); + QtSingleCoreApplication(const QString &id, int &argc, char * *argv); bool isRunning(); QString id() const; @@ -52,7 +50,6 @@ Q_SIGNALS: private: - QtLocalPeer* peer; + QtLocalPeer *peer; }; - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/scriptwrapper/interface_wrap_helpers.h b/ground/openpilotgcs/src/shared/scriptwrapper/interface_wrap_helpers.h index aacc00dd2..a754a63b0 100644 --- a/ground/openpilotgcs/src/shared/scriptwrapper/interface_wrap_helpers.h +++ b/ground/openpilotgcs/src/shared/scriptwrapper/interface_wrap_helpers.h @@ -4,25 +4,25 @@ * @file interface_wrap_helpers.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,21 +32,21 @@ #include namespace SharedTools { - // Convert a QObjectInterface to Scriptvalue // To be registered as a magic creation function with qScriptRegisterMetaType(). // (see registerQObjectInterface) template -static QScriptValue qObjectInterfaceToScriptValue(QScriptEngine *engine, QObjectInterface* const &qoif) +static QScriptValue qObjectInterfaceToScriptValue(QScriptEngine *engine, QObjectInterface *const &qoif) { - if (!qoif) - return QScriptValue(engine, QScriptValue::NullValue); + if (!qoif) { + return QScriptValue(engine, QScriptValue::NullValue); + } QObject *qObject = const_cast(qoif); const QScriptEngine::QObjectWrapOptions wrapOptions = - QScriptEngine::ExcludeChildObjects|QScriptEngine::ExcludeSuperClassMethods|QScriptEngine::ExcludeSuperClassProperties; + QScriptEngine::ExcludeChildObjects | QScriptEngine::ExcludeSuperClassMethods | QScriptEngine::ExcludeSuperClassProperties; return engine->newQObject(qObject, QScriptEngine::QtOwnership, wrapOptions); } @@ -55,10 +55,11 @@ static QScriptValue qObjectInterfaceToScriptValue(QScriptEngine *engine, QObject // (see registerQObjectInterface) template -static void scriptValueToQObjectInterface(const QScriptValue &sv, QObjectInterface *&p) +static void scriptValueToQObjectInterface(const QScriptValue &sv, QObjectInterface * &p) { - QObject *qObject = sv.toQObject(); - p = qobject_cast(qObject); + QObject *qObject = sv.toQObject(); + + p = qobject_cast(qObject); } // Magically register a Workbench interface derived from @@ -74,14 +75,14 @@ static void registerQObjectInterface(QScriptEngine *engine) Prototype *protoType = new Prototype(engine); const QScriptValue scriptProtoType = engine->newQObject(protoType); - const int metaTypeId = qScriptRegisterMetaType( + const int metaTypeId = qScriptRegisterMetaType( engine, qObjectInterfaceToScriptValue, scriptValueToQObjectInterface, scriptProtoType); + Q_UNUSED(metaTypeId) } - } // namespace SharedTools #endif // INTERFACE_WRAP_HELPERS_H diff --git a/ground/openpilotgcs/src/shared/scriptwrapper/wrap_helpers.h b/ground/openpilotgcs/src/shared/scriptwrapper/wrap_helpers.h index 6a8b98d0f..8c045227f 100644 --- a/ground/openpilotgcs/src/shared/scriptwrapper/wrap_helpers.h +++ b/ground/openpilotgcs/src/shared/scriptwrapper/wrap_helpers.h @@ -4,25 +4,25 @@ * @file wrap_helpers.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,27 +34,27 @@ #include namespace SharedTools { - // Strip a const ref from a type via template specialization trick. // Use for determining function call args template - struct RemoveConstRef { - typedef T Result; - }; +struct RemoveConstRef { + typedef T Result; +}; template - struct RemoveConstRef { - typedef T Result; - }; +struct RemoveConstRef { + typedef T Result; +}; // Template that retrieves a QObject-derived class from a QScriptValue. template - QObjectDerived *qObjectFromScriptValue(const QScriptValue &v) +QObjectDerived *qObjectFromScriptValue(const QScriptValue &v) { - if (!v.isQObject()) + if (!v.isQObject()) { return 0; + } QObject *o = v.toQObject(); return qobject_cast(o); } @@ -63,13 +63,15 @@ template // The wrapped object is accessed through an accessor of // the QObject-derived wrapper. -template - Wrapped *wrappedFromScriptValue(const QScriptValue &v, - Wrapped * (Wrapper::*wrappedAccessor) () const) +template +Wrapped *wrappedFromScriptValue(const QScriptValue &v, + Wrapped * (Wrapper::*wrappedAccessor)() const) { Wrapper *wrapper = qObjectFromScriptValue(v); - if (!wrapper) + + if (!wrapper) { return 0; + } return (wrapper->*wrappedAccessor)(); } @@ -77,11 +79,12 @@ template // a QObject-derived script wrapper object that is set as 'this' in // a script context via accessor. -template - static inline Wrapped *wrappedThisFromContext(QScriptContext *context, - Wrapped * (Wrapper::*wrappedAccessor) () const) +template +static inline Wrapped *wrappedThisFromContext(QScriptContext *context, + Wrapped * (Wrapper::*wrappedAccessor)() const) { Wrapped *wrapped = wrappedFromScriptValue(context->thisObject(), wrappedAccessor); + Q_ASSERT(wrapped); return wrapped; } @@ -91,12 +94,13 @@ template // the core interface accessors). Mangles out the wrapper object from // thisObject(), accesses the wrapped object and returns the contained object. -template - static inline Contained *containedFromContext(QScriptContext *context, - Wrapped * (Wrapper::*wrappedAccessor) () const, - Contained * (Wrapped::*containedAccessor)() const) +template +static inline Contained *containedFromContext(QScriptContext *context, + Wrapped * (Wrapper::*wrappedAccessor)() const, + Contained * (Wrapped::*containedAccessor)() const) { Wrapped *wrapped = wrappedThisFromContext(context, wrappedAccessor); + return (wrapped->*containedAccessor)(); } @@ -104,10 +108,10 @@ template // in a script getter call and creates a new script-object via engine->newQObject(). // To be called from a script getter callback. -template - static inline QScriptValue containedQObjectFromContextToScriptValue(QScriptContext *context, QScriptEngine *engine, - Wrapped * (Wrapper::*wrappedAccessor) () const, - Contained * (Wrapped::*containedAccessor)() const) +template +static inline QScriptValue containedQObjectFromContextToScriptValue(QScriptContext *context, QScriptEngine *engine, + Wrapped * (Wrapper::*wrappedAccessor)() const, + Contained * (Wrapped::*containedAccessor)() const) { return engine->newQObject(containedFromContext(context, wrappedAccessor, containedAccessor)); } @@ -117,14 +121,16 @@ template // a new instance of ContainedWrapper (which casts to QScriptValue). // To be called from a script getter callback. -template - static inline QScriptValue wrapContainedFromContextAsScriptValue(QScriptContext *context, QScriptEngine *engine, - Wrapped * (Wrapper::*wrappedAccessor) () const, - Contained * (Wrapped::*containedAccessor)() const) +template +static inline QScriptValue wrapContainedFromContextAsScriptValue(QScriptContext *context, QScriptEngine *engine, + Wrapped * (Wrapper::*wrappedAccessor)() const, + Contained * (Wrapped::*containedAccessor)() const) { Contained *c = containedFromContext(context, wrappedAccessor, containedAccessor); - if (!c) + + if (!c) { return QScriptValue(engine, QScriptValue::NullValue); + } ContainedWrapper *cw = new ContainedWrapper(*engine, c); return *cw; // cast to QScriptValue @@ -134,64 +140,70 @@ template - static inline QScriptValue scriptCallConstMember_0(QScriptContext *context, QScriptEngine *engine, - Wrapped * (Wrapper::*wrappedAccessor) () const, - Ret (Wrapped::*member)() const) +template +static inline QScriptValue scriptCallConstMember_0(QScriptContext *context, QScriptEngine *engine, + Wrapped * (Wrapper::*wrappedAccessor)() const, + Ret (Wrapped::*member)() const) { Wrapped *wrapped = wrappedThisFromContext(context, wrappedAccessor); - return engine->toScriptValue( (wrapped->*member)() ); + + return engine->toScriptValue((wrapped->*member)()); } // Ditto for non-const -template - static inline QScriptValue scriptCallMember_0(QScriptContext *context, QScriptEngine *engine, - Wrapped * (Wrapper::*wrappedAccessor) () const, - Ret (Wrapped::*member)()) +template +static inline QScriptValue scriptCallMember_0(QScriptContext *context, QScriptEngine *engine, + Wrapped * (Wrapper::*wrappedAccessor)() const, + Ret (Wrapped::*member)()) { Wrapped *wrapped = wrappedThisFromContext(context, wrappedAccessor); - return engine->toScriptValue( (wrapped->*member)() ); + + return engine->toScriptValue((wrapped->*member)()); } // Template that retrieves a wrapped object from context (this) // and calls a const-member function with 1 parameter on it. // To be called from a script getter callback. -template - static inline QScriptValue scriptCallConstMember_1(QScriptContext *context, QScriptEngine *engine, - Wrapped * (Wrapper::*wrappedAccessor) () const, - Ret (Wrapped::*member)(Argument a1) const) +template +static inline QScriptValue scriptCallConstMember_1(QScriptContext *context, QScriptEngine *engine, + Wrapped * (Wrapper::*wrappedAccessor)() const, + Ret (Wrapped::*member)(Argument a1) const) { const int argumentCount = context->argumentCount(); - if ( argumentCount != 1) - return QScriptValue (engine, QScriptValue::NullValue); + + if (argumentCount != 1) { + return QScriptValue(engine, QScriptValue::NullValue); + } Wrapped *wrapped = wrappedThisFromContext(context, wrappedAccessor); // call member. If the argument is a const ref, strip it. typedef typename RemoveConstRef::Result ArgumentBase; - ArgumentBase a = qscriptvalue_cast(context->argument(0)); - return engine->toScriptValue( (wrapped->*member)(a) ); + ArgumentBase a = qscriptvalue_cast(context->argument(0)); + return engine->toScriptValue((wrapped->*member)(a)); } // Template that retrieves a wrapped object // and calls a member function with 1 parameter on it. // To be called from a script getter callback. -template - static inline QScriptValue scriptCallMember_1(QScriptContext *context, QScriptEngine *engine, - Wrapped * (Wrapper::*wrappedAccessor) () const, - Ret (Wrapped::*member)(Argument a1)) +template +static inline QScriptValue scriptCallMember_1(QScriptContext *context, QScriptEngine *engine, + Wrapped * (Wrapper::*wrappedAccessor)() const, + Ret (Wrapped::*member)(Argument a1)) { const int argumentCount = context->argumentCount(); - if ( argumentCount != 1) - return QScriptValue (engine, QScriptValue::NullValue); + + if (argumentCount != 1) { + return QScriptValue(engine, QScriptValue::NullValue); + } Wrapped *wrapped = wrappedThisFromContext(context, wrappedAccessor); // call member. If the argument is a const ref, strip it. typedef typename RemoveConstRef::Result ArgumentBase; - ArgumentBase a = qscriptvalue_cast(context->argument(0)); - return engine->toScriptValue( (wrapped->*member)(a) ); + ArgumentBase a = qscriptvalue_cast(context->argument(0)); + return engine->toScriptValue((wrapped->*member)(a)); } // Template that retrieves a wrapped object @@ -200,52 +212,56 @@ template // Typically used for something like 'setCurrentEditor(Editor*)' // To be called from a script callback. -template +template static QScriptValue scriptCallVoidMember_Wrapped1(QScriptContext *context, QScriptEngine *engine, - ThisWrapped * (ThisWrapper::*thisWrappedAccessor) () const, + ThisWrapped * (ThisWrapper::*thisWrappedAccessor)() const, ArgumentWrapped *(ArgumentWrapper::*argumentWrappedAccessor)() const, - void (ThisWrapped::*member)(ArgumentWrapped *a1), + void (ThisWrapped::*member)(ArgumentWrapped *a1), bool acceptNullArgument = false) { const QScriptValue voidRC = QScriptValue(engine, QScriptValue::UndefinedValue); - if (context->argumentCount() < 1) + + if (context->argumentCount() < 1) { return voidRC; + } ThisWrapped *thisWrapped = wrappedThisFromContext(context, thisWrappedAccessor); ArgumentWrapped *aw = wrappedFromScriptValue(context->argument(0), argumentWrappedAccessor); - if (acceptNullArgument || aw) + if (acceptNullArgument || aw) { (thisWrapped->*member)(aw); + } return voidRC; } // Macros that define the static functions to call members #define SCRIPT_CALL_CONST_MEMBER_0(funcName, accessor, member) \ -static QScriptValue funcName(QScriptContext *context, QScriptEngine *engine) \ -{ return SharedTools::scriptCallConstMember_0(context, engine, accessor, member); } + static QScriptValue funcName(QScriptContext * context, QScriptEngine * engine) \ + { return SharedTools::scriptCallConstMember_0(context, engine, accessor, member); } #define SCRIPT_CALL_MEMBER_0(funcName, accessor, member) \ -static QScriptValue funcName(QScriptContext *context, QScriptEngine *engine) \ -{ return SharedTools::scriptCallMember_0(context, engine, accessor, member); } + static QScriptValue funcName(QScriptContext * context, QScriptEngine * engine) \ + { return SharedTools::scriptCallMember_0(context, engine, accessor, member); } #define SCRIPT_CALL_CONST_MEMBER_1(funcName, accessor, member) \ -static QScriptValue funcName(QScriptContext *context, QScriptEngine *engine) \ -{ return SharedTools::scriptCallConstMember_1(context, engine, accessor, member); } + static QScriptValue funcName(QScriptContext * context, QScriptEngine * engine) \ + { return SharedTools::scriptCallConstMember_1(context, engine, accessor, member); } #define SCRIPT_CALL_MEMBER_1(funcName, accessor, member) \ -static QScriptValue funcName(QScriptContext *context, QScriptEngine *engine) \ -{ return SharedTools::scriptCallMember_1(context, engine, accessor, member); } + static QScriptValue funcName(QScriptContext * context, QScriptEngine * engine) \ + { return SharedTools::scriptCallMember_1(context, engine, accessor, member); } // Create a script list of wrapped non-qobjects by wrapping them. // Wrapper must cast to QScriptValue. template - static inline QScriptValue wrapObjectList( QScriptEngine *engine, Iterator i1, Iterator i2) +static inline QScriptValue wrapObjectList(QScriptEngine *engine, Iterator i1, Iterator i2) { QScriptValue rc = engine->newArray(i2 - i1); // Grrr! quint32 i = 0; - for ( ; i1 != i2 ; ++i1, i++) { - Wrapper * wrapper = new Wrapper(*engine, *i1); + + for (; i1 != i2; ++i1, i++) { + Wrapper *wrapper = new Wrapper(*engine, *i1); rc.setProperty(i, *wrapper); } return rc; @@ -254,24 +270,27 @@ template // Unwrap a list of wrapped objects from a script list. template - static inline QList unwrapObjectList(const QScriptValue &v, - Wrapped *(Wrapper::*wrappedAccessor)() const) +static inline QList unwrapObjectList(const QScriptValue &v, + Wrapped *(Wrapper::*wrappedAccessor)() const) { - QList rc; + QList rc; - if (!v.isArray()) + if (!v.isArray()) { return rc; + } - const quint32 len = v.property(QLatin1String("length")).toUInt32(); - if (!len) + const quint32 len = v.property(QLatin1String("length")).toUInt32(); + if (!len) { return rc; + } for (quint32 i = 0; i < len; i++) { const QScriptValue e = v.property(i); if (e.isQObject()) { QObject *o = e.toQObject(); - if (Wrapper * wrapper = qobject_cast(o)) + if (Wrapper * wrapper = qobject_cast(o)) { rc.push_back((wrapper->*wrappedAccessor)()); + } } } return rc; @@ -281,12 +300,12 @@ template // that can be converted via script value casts via Q_DECLARE_METATYPE. template - static void registerInterfaceWithDefaultPrototype(QScriptEngine &engine) +static void registerInterfaceWithDefaultPrototype(QScriptEngine &engine) { Prototype *protoType = new Prototype(&engine); const QScriptValue scriptProtoType = engine.newQObject(protoType); - engine.setDefaultPrototype(qMetaTypeId(), scriptProtoType); + engine.setDefaultPrototype(qMetaTypeId(), scriptProtoType); } // Convert a class derived from QObject to Scriptvalue via engine->newQObject() to make @@ -295,7 +314,7 @@ template // (see registerQObject() template -static QScriptValue qObjectToScriptValue(QScriptEngine *engine, SomeQObject * const &qo) +static QScriptValue qObjectToScriptValue(QScriptEngine *engine, SomeQObject *const &qo) { return engine->newQObject(qo, QScriptEngine::QtOwnership, QScriptEngine::ExcludeChildObjects); } @@ -307,8 +326,9 @@ static QScriptValue qObjectToScriptValue(QScriptEngine *engine, SomeQObject * co template static void scriptValueToQObject(const QScriptValue &sv, SomeQObject * &p) { - QObject *qObject = sv.toQObject(); - p = qobject_cast(qObject); + QObject *qObject = sv.toQObject(); + + p = qobject_cast(qObject); Q_ASSERT(p); } @@ -320,11 +340,10 @@ static void scriptValueToQObject(const QScriptValue &sv, SomeQObject * &p) template static void registerQObject(QScriptEngine *engine) { - qScriptRegisterMetaType(engine, - qObjectToScriptValue, - scriptValueToQObject); + qScriptRegisterMetaType(engine, + qObjectToScriptValue, + scriptValueToQObject); } - } // namespace SharedTools #endif // WRAP_HELPERS_H diff --git a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp index 18b97a225..2bc7623e7 100644 --- a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp +++ b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp @@ -28,67 +28,67 @@ using namespace std; -bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString templatepath,QString outputpath) { +bool UAVObjectGeneratorFlight::generate(UAVObjectParser *parser, QString templatepath, QString outputpath) +{ + fieldTypeStrC << "int8_t" << "int16_t" << "int32_t" << "uint8_t" + << "uint16_t" << "uint32_t" << "float" << "uint8_t"; - fieldTypeStrC << "int8_t" << "int16_t" << "int32_t" <<"uint8_t" - <<"uint16_t" << "uint32_t" << "float" << "uint8_t"; - - QString flightObjInit,objInc,objFileNames,objNames; + QString flightObjInit, objInc, objFileNames, objNames; qint32 sizeCalc; - flightCodePath = QDir( templatepath + QString(FLIGHT_CODE_DIR)); - flightOutputPath = QDir( outputpath + QString("flight") ); + flightCodePath = QDir(templatepath + QString(FLIGHT_CODE_DIR)); + flightOutputPath = QDir(outputpath + QString("flight")); flightOutputPath.mkpath(flightOutputPath.absolutePath()); - flightCodeTemplate = readFile( flightCodePath.absoluteFilePath("uavobject.c.template") ); - flightIncludeTemplate = readFile( flightCodePath.absoluteFilePath("inc/uavobject.h.template") ); - flightInitTemplate = readFile( flightCodePath.absoluteFilePath("uavobjectsinit.c.template") ); - flightInitIncludeTemplate = readFile( flightCodePath.absoluteFilePath("inc/uavobjectsinit.h.template") ); - flightMakeTemplate = readFile( flightCodePath.absoluteFilePath("Makefile.inc.template") ); + flightCodeTemplate = readFile(flightCodePath.absoluteFilePath("uavobject.c.template")); + flightIncludeTemplate = readFile(flightCodePath.absoluteFilePath("inc/uavobject.h.template")); + flightInitTemplate = readFile(flightCodePath.absoluteFilePath("uavobjectsinit.c.template")); + flightInitIncludeTemplate = readFile(flightCodePath.absoluteFilePath("inc/uavobjectsinit.h.template")); + flightMakeTemplate = readFile(flightCodePath.absoluteFilePath("Makefile.inc.template")); - if ( flightCodeTemplate.isNull() || flightIncludeTemplate.isNull() || flightInitTemplate.isNull()) { - cerr << "Error: Could not open flight template files." << endl; - return false; - } + if (flightCodeTemplate.isNull() || flightIncludeTemplate.isNull() || flightInitTemplate.isNull()) { + cerr << "Error: Could not open flight template files." << endl; + return false; + } sizeCalc = 0; for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { - ObjectInfo* info=parser->getObjectByIndex(objidx); + ObjectInfo *info = parser->getObjectByIndex(objidx); process_object(info); - flightObjInit.append("#ifdef UAVOBJ_INIT_" + info->namelc +"\n"); + flightObjInit.append("#ifdef UAVOBJ_INIT_" + info->namelc + "\n"); flightObjInit.append(" " + info->name + "Initialize();\n"); flightObjInit.append("#endif\n"); objInc.append("#include \"" + info->namelc + ".h\"\n"); - objFileNames.append(" " + info->namelc); - objNames.append(" " + info->name); - if (parser->getNumBytes(objidx)>sizeCalc) { - sizeCalc = parser->getNumBytes(objidx); - } + objFileNames.append(" " + info->namelc); + objNames.append(" " + info->name); + if (parser->getNumBytes(objidx) > sizeCalc) { + sizeCalc = parser->getNumBytes(objidx); + } } // Write the flight object inialization files - flightInitTemplate.replace( QString("$(OBJINC)"), objInc); - flightInitTemplate.replace( QString("$(OBJINIT)"), flightObjInit); - bool res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/uavobjectsinit.c", - flightInitTemplate ); + flightInitTemplate.replace(QString("$(OBJINC)"), objInc); + flightInitTemplate.replace(QString("$(OBJINIT)"), flightObjInit); + bool res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/uavobjectsinit.c", + flightInitTemplate); if (!res) { cout << "Error: Could not write flight object init file" << endl; return false; } // Write the flight object initialization header - flightInitIncludeTemplate.replace( QString("$(SIZECALCULATION)"), QString().setNum(sizeCalc)); - res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/uavobjectsinit.h", - flightInitIncludeTemplate ); + flightInitIncludeTemplate.replace(QString("$(SIZECALCULATION)"), QString().setNum(sizeCalc)); + res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/uavobjectsinit.h", + flightInitIncludeTemplate); if (!res) { cout << "Error: Could not write flight object init header file" << endl; return false; } // Write the flight object Makefile - flightMakeTemplate.replace( QString("$(UAVOBJFILENAMES)"), objFileNames); - flightMakeTemplate.replace( QString("$(UAVOBJNAMES)"), objNames); - res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/Makefile.inc", - flightMakeTemplate ); + flightMakeTemplate.replace(QString("$(UAVOBJFILENAMES)"), objFileNames); + flightMakeTemplate.replace(QString("$(UAVOBJNAMES)"), objNames); + res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/Makefile.inc", + flightMakeTemplate); if (!res) { cout << "Error: Could not write flight Makefile" << endl; return false; @@ -100,15 +100,16 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template /** * Generate the Flight object files -**/ -bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info) + **/ +bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info) { - if (info == NULL) + if (info == NULL) { return false; + } // Prepare output strings QString outInclude = flightIncludeTemplate; - QString outCode = flightCodeTemplate; + QString outCode = flightCodeTemplate; // Replace common tags replaceCommonTags(outInclude, info); @@ -117,77 +118,66 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info) // Replace the $(DATAFIELDS) tag QString type; QString fields; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { // Determine type type = fieldTypeStrC[info->fields[n]->type]; // Append field - if ( info->fields[n]->numElements > 1 ) - { - fields.append( QString(" %1 %2[%3];\n").arg(type) - .arg(info->fields[n]->name).arg(info->fields[n]->numElements) ); - } - else - { - fields.append( QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name) ); + if (info->fields[n]->numElements > 1) { + fields.append(QString(" %1 %2[%3];\n").arg(type) + .arg(info->fields[n]->name).arg(info->fields[n]->numElements)); + } else { + fields.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name)); } } outInclude.replace(QString("$(DATAFIELDS)"), fields); // Replace the $(DATAFIELDINFO) tag QString enums; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { enums.append(QString("/* Field %1 information */\n").arg(info->fields[n]->name)); // Only for enum types - if (info->fields[n]->type == FIELDTYPE_ENUM) - { + if (info->fields[n]->type == FIELDTYPE_ENUM) { enums.append(QString("\n// Enumeration options for field %1\n").arg(info->fields[n]->name)); enums.append("typedef enum {\n"); // Go through each option QStringList options = info->fields[n]->options; for (int m = 0; m < options.length(); ++m) { - QString s = (m == (options.length()-1)) ? " %1_%2_%3=%4\n" : " %1_%2_%3=%4,\n"; - enums.append( s - .arg( info->name.toUpper() ) - .arg( info->fields[n]->name.toUpper() ) - .arg( options[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), "") ) - .arg(m) ); - + QString s = (m == (options.length() - 1)) ? " %1_%2_%3=%4\n" : " %1_%2_%3=%4,\n"; + enums.append(s + .arg(info->name.toUpper()) + .arg(info->fields[n]->name.toUpper()) + .arg(options[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), "")) + .arg(m)); } - enums.append( QString("} %1%2Options;\n") - .arg( info->name ) - .arg( info->fields[n]->name ) ); + enums.append(QString("} %1%2Options;\n") + .arg(info->name) + .arg(info->fields[n]->name)); } // Generate element names (only if field has more than one element) - if (info->fields[n]->numElements > 1 && !info->fields[n]->defaultElementNames) - { + if (info->fields[n]->numElements > 1 && !info->fields[n]->defaultElementNames) { enums.append(QString("\n// Array element names for field %1\n").arg(info->fields[n]->name)); enums.append("typedef enum {\n"); // Go through the element names QStringList elemNames = info->fields[n]->elementNames; - for (int m = 0; m < elemNames.length(); ++m) - { - QString s = (m != (elemNames.length()-1)) ? " %1_%2_%3=%4,\n" : " %1_%2_%3=%4\n"; - enums.append( s - .arg( info->name.toUpper() ) - .arg( info->fields[n]->name.toUpper() ) - .arg( elemNames[m].toUpper() ) - .arg(m) ); - + for (int m = 0; m < elemNames.length(); ++m) { + QString s = (m != (elemNames.length() - 1)) ? " %1_%2_%3=%4,\n" : " %1_%2_%3=%4\n"; + enums.append(s + .arg(info->name.toUpper()) + .arg(info->fields[n]->name.toUpper()) + .arg(elemNames[m].toUpper()) + .arg(m)); } - enums.append( QString("} %1%2Elem;\n") - .arg( info->name ) - .arg( info->fields[n]->name ) ); + enums.append(QString("} %1%2Elem;\n") + .arg(info->name) + .arg(info->fields[n]->name)); } // Generate array information - if (info->fields[n]->numElements > 1) - { + if (info->fields[n]->numElements > 1) { enums.append(QString("\n// Number of elements for field %1\n").arg(info->fields[n]->name)); - enums.append( QString("#define %1_%2_NUMELEM %3\n") - .arg( info->name.toUpper() ) - .arg( info->fields[n]->name.toUpper() ) - .arg( info->fields[n]->numElements ) ); + enums.append(QString("#define %1_%2_NUMELEM %3\n") + .arg(info->name.toUpper()) + .arg(info->fields[n]->name.toUpper()) + .arg(info->fields[n]->numElements)); } enums.append(QString("\n")); @@ -196,57 +186,41 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info) // Replace the $(INITFIELDS) tag QString initfields; - for (int n = 0; n < info->fields.length(); ++n) - { - if (!info->fields[n]->defaultValues.isEmpty() ) - { + for (int n = 0; n < info->fields.length(); ++n) { + if (!info->fields[n]->defaultValues.isEmpty()) { // For non-array fields - if ( info->fields[n]->numElements == 1) - { - if ( info->fields[n]->type == FIELDTYPE_ENUM ) - { - initfields.append( QString(" data.%1 = %2;\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->options.indexOf( info->fields[n]->defaultValues[0] ) ) ); + if (info->fields[n]->numElements == 1) { + if (info->fields[n]->type == FIELDTYPE_ENUM) { + initfields.append(QString(" data.%1 = %2;\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->options.indexOf(info->fields[n]->defaultValues[0]))); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + initfields.append(QString(" data.%1 = %2f;\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0].toFloat(), 0, 'e', 6)); + } else { + initfields.append(QString(" data.%1 = %2;\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0].toInt())); } - else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) - { - initfields.append( QString(" data.%1 = %2f;\n") - .arg(info->fields[n]->name) - .arg(info->fields[n]->defaultValues[0].toFloat(), 0, 'e', 6)); - } - else - { - initfields.append( QString(" data.%1 = %2;\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->defaultValues[0].toInt() ) ); - } - } - else - { + } else { // Initialize all fields in the array - for (int idx = 0; idx < info->fields[n]->numElements; ++idx) - { - if ( info->fields[n]->type == FIELDTYPE_ENUM ) - { - initfields.append( QString(" data.%1[%2] = %3;\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->options.indexOf( info->fields[n]->defaultValues[idx] ) ) ); - } - else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) - { - initfields.append( QString(" data.%1[%2] = %3f;\n") - .arg(info->fields[n]->name) - .arg(idx) - .arg(info->fields[n]->defaultValues[idx].toFloat(), 0, 'e', 6)); - } - else - { - initfields.append( QString(" data.%1[%2] = %3;\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->defaultValues[idx].toInt() ) ); + for (int idx = 0; idx < info->fields[n]->numElements; ++idx) { + if (info->fields[n]->type == FIELDTYPE_ENUM) { + initfields.append(QString(" data.%1[%2] = %3;\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->options.indexOf(info->fields[n]->defaultValues[idx]))); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + initfields.append(QString(" data.%1[%2] = %3f;\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx].toFloat(), 0, 'e', 6)); + } else { + initfields.append(QString(" data.%1[%2] = %3;\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx].toInt())); } } } @@ -256,102 +230,93 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info) // Replace the $(SETGETFIELDS) tag QString setgetfields; - for (int n = 0; n < info->fields.length(); ++n) - { - //if (!info->fields[n]->defaultValues.isEmpty() ) + for (int n = 0; n < info->fields.length(); ++n) { + // if (!info->fields[n]->defaultValues.isEmpty() ) { // For non-array fields - if ( info->fields[n]->numElements == 1) - { + if (info->fields[n]->numElements == 1) { + /* Set */ + setgetfields.append(QString("void %2%3Set(%1 *New%3)\n") + .arg(fieldTypeStrC[info->fields[n]->type]) + .arg(info->name) + .arg(info->fields[n]->name)); + setgetfields.append(QString("{\n")); + setgetfields.append(QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), sizeof(%3));\n") + .arg(info->name) + .arg(info->fields[n]->name) + .arg(fieldTypeStrC[info->fields[n]->type])); + setgetfields.append(QString("}\n")); - /* Set */ - setgetfields.append( QString("void %2%3Set(%1 *New%3)\n") - .arg( fieldTypeStrC[info->fields[n]->type] ) - .arg( info->name ) - .arg( info->fields[n]->name ) ); - setgetfields.append( QString("{\n") ); - setgetfields.append( QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), sizeof(%3));\n") - .arg( info->name ) - .arg( info->fields[n]->name ) - .arg( fieldTypeStrC[info->fields[n]->type] ) ); - setgetfields.append( QString("}\n") ); + /* GET */ + setgetfields.append(QString("void %2%3Get(%1 *New%3)\n") + .arg(fieldTypeStrC[info->fields[n]->type]) + .arg(info->name) + .arg(info->fields[n]->name)); + setgetfields.append(QString("{\n")); + setgetfields.append(QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), sizeof(%3));\n") + .arg(info->name) + .arg(info->fields[n]->name) + .arg(fieldTypeStrC[info->fields[n]->type])); + setgetfields.append(QString("}\n")); + } else { + /* SET */ + setgetfields.append(QString("void %2%3Set( %1 *New%3 )\n") + .arg(fieldTypeStrC[info->fields[n]->type]) + .arg(info->name) + .arg(info->fields[n]->name)); + setgetfields.append(QString("{\n")); + setgetfields.append(QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n") + .arg(info->name) + .arg(info->fields[n]->name) + .arg(info->fields[n]->numElements) + .arg(fieldTypeStrC[info->fields[n]->type])); + setgetfields.append(QString("}\n")); - /* GET */ - setgetfields.append( QString("void %2%3Get(%1 *New%3)\n") - .arg( fieldTypeStrC[info->fields[n]->type] ) - .arg( info->name ) - .arg( info->fields[n]->name )); - setgetfields.append( QString("{\n") ); - setgetfields.append( QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), sizeof(%3));\n") - .arg( info->name ) - .arg( info->fields[n]->name ) - .arg( fieldTypeStrC[info->fields[n]->type] ) ); - setgetfields.append( QString("}\n") ); - - } - else - { - - /* SET */ - setgetfields.append( QString("void %2%3Set( %1 *New%3 )\n") - .arg( fieldTypeStrC[info->fields[n]->type] ) - .arg( info->name ) - .arg( info->fields[n]->name ) ); - setgetfields.append( QString("{\n") ); - setgetfields.append( QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n") - .arg( info->name ) - .arg( info->fields[n]->name ) - .arg( info->fields[n]->numElements ) - .arg( fieldTypeStrC[info->fields[n]->type] ) ); - setgetfields.append( QString("}\n") ); - - /* GET */ - setgetfields.append( QString("void %2%3Get( %1 *New%3 )\n") - .arg( fieldTypeStrC[info->fields[n]->type] ) - .arg( info->name ) - .arg( info->fields[n]->name ) ); - setgetfields.append( QString("{\n") ); - setgetfields.append( QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n") - .arg( info->name ) - .arg( info->fields[n]->name ) - .arg( info->fields[n]->numElements ) - .arg( fieldTypeStrC[info->fields[n]->type] ) ); - setgetfields.append( QString("}\n") ); + /* GET */ + setgetfields.append(QString("void %2%3Get( %1 *New%3 )\n") + .arg(fieldTypeStrC[info->fields[n]->type]) + .arg(info->name) + .arg(info->fields[n]->name)); + setgetfields.append(QString("{\n")); + setgetfields.append(QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n") + .arg(info->name) + .arg(info->fields[n]->name) + .arg(info->fields[n]->numElements) + .arg(fieldTypeStrC[info->fields[n]->type])); + setgetfields.append(QString("}\n")); } } } outCode.replace(QString("$(SETGETFIELDS)"), setgetfields); // Replace the $(SETGETFIELDSEXTERN) tag - QString setgetfieldsextern; - for (int n = 0; n < info->fields.length(); ++n) - { - //if (!info->fields[n]->defaultValues.isEmpty() ) - { + QString setgetfieldsextern; + for (int n = 0; n < info->fields.length(); ++n) { + // if (!info->fields[n]->defaultValues.isEmpty() ) + { + /* SET */ + setgetfieldsextern.append(QString("extern void %2%3Set(%1 *New%3);\n") + .arg(fieldTypeStrC[info->fields[n]->type]) + .arg(info->name) + .arg(info->fields[n]->name)); - /* SET */ - setgetfieldsextern.append( QString("extern void %2%3Set(%1 *New%3);\n") - .arg( fieldTypeStrC[info->fields[n]->type] ) - .arg( info->name ) - .arg( info->fields[n]->name ) ); - - /* GET */ - setgetfieldsextern.append( QString("extern void %2%3Get(%1 *New%3);\n") - .arg( fieldTypeStrC[info->fields[n]->type] ) - .arg( info->name ) - .arg( info->fields[n]->name ) ); - } - } - outInclude.replace(QString("$(SETGETFIELDSEXTERN)"), setgetfieldsextern); + /* GET */ + setgetfieldsextern.append(QString("extern void %2%3Get(%1 *New%3);\n") + .arg(fieldTypeStrC[info->fields[n]->type]) + .arg(info->name) + .arg(info->fields[n]->name)); + } + } + outInclude.replace(QString("$(SETGETFIELDSEXTERN)"), setgetfieldsextern); // Write the flight code - bool res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/" + info->namelc + ".c", outCode ); + bool res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/" + info->namelc + ".c", outCode); if (!res) { cout << "Error: Could not write flight code files" << endl; return false; } - res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude ); + res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude); if (!res) { cout << "Error: Could not write flight include files" << endl; return false; diff --git a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h index e475c21ba..118d11e54 100644 --- a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h +++ b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h @@ -31,19 +31,16 @@ #include "../generator_common.h" -class UAVObjectGeneratorFlight -{ +class UAVObjectGeneratorFlight { public: - bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); + bool generate(UAVObjectParser *gen, QString templatepath, QString outputpath); QStringList fieldTypeStrC; QString flightCodeTemplate, flightIncludeTemplate, flightInitTemplate, flightInitIncludeTemplate, flightMakeTemplate; QDir flightCodePath; QDir flightOutputPath; private: - bool process_object(ObjectInfo* info); - + bool process_object(ObjectInfo *info); }; #endif - diff --git a/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp b/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp index 93fb980e2..2b1eb5d14 100644 --- a/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp +++ b/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp @@ -27,21 +27,21 @@ #include "uavobjectgeneratorgcs.h" using namespace std; -bool UAVObjectGeneratorGCS::generate(UAVObjectParser* parser,QString templatepath,QString outputpath) { - +bool UAVObjectGeneratorGCS::generate(UAVObjectParser *parser, QString templatepath, QString outputpath) +{ fieldTypeStrCPP << "qint8" << "qint16" << "qint32" << "quint8" << "quint16" << "quint32" << "float" << "quint8"; fieldTypeStrCPPClass << "INT8" << "INT16" << "INT32" - << "UINT8" << "UINT16" << "UINT32" << "FLOAT32" << "ENUM"; + << "UINT8" << "UINT16" << "UINT32" << "FLOAT32" << "ENUM"; - gcsCodePath = QDir( templatepath + QString(GCS_CODE_DIR)); - gcsOutputPath = QDir( outputpath + QString("gcs") ); + gcsCodePath = QDir(templatepath + QString(GCS_CODE_DIR)); + gcsOutputPath = QDir(outputpath + QString("gcs")); gcsOutputPath.mkpath(gcsOutputPath.absolutePath()); - gcsCodeTemplate = readFile( gcsCodePath.absoluteFilePath("uavobject.cpp.template") ); - gcsIncludeTemplate = readFile( gcsCodePath.absoluteFilePath("uavobject.h.template") ); - QString gcsInitTemplate = readFile( gcsCodePath.absoluteFilePath("uavobjectsinit.cpp.template") ); + gcsCodeTemplate = readFile(gcsCodePath.absoluteFilePath("uavobject.cpp.template")); + gcsIncludeTemplate = readFile(gcsCodePath.absoluteFilePath("uavobject.h.template")); + QString gcsInitTemplate = readFile(gcsCodePath.absoluteFilePath("uavobjectsinit.cpp.template")); if (gcsCodeTemplate.isEmpty() || gcsIncludeTemplate.isEmpty() || gcsInitTemplate.isEmpty()) { std::cerr << "Problem reading gcs code templates" << endl; @@ -52,7 +52,7 @@ bool UAVObjectGeneratorGCS::generate(UAVObjectParser* parser,QString templatepat QString gcsObjInit; for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { - ObjectInfo* info=parser->getObjectByIndex(objidx); + ObjectInfo *info = parser->getObjectByIndex(objidx); process_object(info); gcsObjInit.append(" objMngr->registerObject( new " + info->name + "() );\n"); @@ -60,9 +60,9 @@ bool UAVObjectGeneratorGCS::generate(UAVObjectParser* parser,QString templatepat } // Write the gcs object inialization files - gcsInitTemplate.replace( QString("$(OBJINC)"), objInc); - gcsInitTemplate.replace( QString("$(OBJINIT)"), gcsObjInit); - bool res = writeFileIfDiffrent( gcsOutputPath.absolutePath() + "/uavobjectsinit.cpp", gcsInitTemplate ); + gcsInitTemplate.replace(QString("$(OBJINC)"), objInc); + gcsInitTemplate.replace(QString("$(OBJINIT)"), gcsObjInit); + bool res = writeFileIfDiffrent(gcsOutputPath.absolutePath() + "/uavobjectsinit.cpp", gcsInitTemplate); if (!res) { cout << "Error: Could not write output files" << endl; return false; @@ -74,14 +74,15 @@ bool UAVObjectGeneratorGCS::generate(UAVObjectParser* parser,QString templatepat /** * Generate the GCS object files */ -bool UAVObjectGeneratorGCS::process_object(ObjectInfo* info) +bool UAVObjectGeneratorGCS::process_object(ObjectInfo *info) { - if (info == NULL) + if (info == NULL) { return false; + } // Prepare output strings QString outInclude = gcsIncludeTemplate; - QString outCode = gcsCodeTemplate; + QString outCode = gcsCodeTemplate; // Replace common tags replaceCommonTags(outInclude, info); @@ -90,19 +91,15 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo* info) // Replace the $(DATAFIELDS) tag QString type; QString fields; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { // Determine type type = fieldTypeStrCPP[info->fields[n]->type]; // Append field - if ( info->fields[n]->numElements > 1 ) - { - fields.append( QString(" %1 %2[%3];\n").arg(type).arg(info->fields[n]->name) - .arg(info->fields[n]->numElements) ); - } - else - { - fields.append( QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name) ); + if (info->fields[n]->numElements > 1) { + fields.append(QString(" %1 %2[%3];\n").arg(type).arg(info->fields[n]->name) + .arg(info->fields[n]->numElements)); + } else { + fields.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name)); } } outInclude.replace(QString("$(DATAFIELDS)"), fields); @@ -115,80 +112,80 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo* info) QString propertyNotifications; QString propertyNotificationsImpl; - //to avoid name conflicts + // to avoid name conflicts QStringList reservedProperties; reservedProperties << "Description" << "Metadata"; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { FieldInfo *field = info->fields[n]; - if (reservedProperties.contains(field->name)) + if (reservedProperties.contains(field->name)) { continue; + } // Determine type type = fieldTypeStrCPP[field->type]; // Append field - if ( field->numElements > 1 ) { + if (field->numElements > 1) { propertyGetters += - QString(" Q_INVOKABLE %1 get%2(quint32 index) const;\n") - .arg(type).arg(field->name); - propertiesImpl += - QString("%1 %2::get%3(quint32 index) const\n" - "{\n" - " QMutexLocker locker(mutex);\n" - " return data.%3[index];\n" - "}\n") - .arg(type).arg(info->name).arg(field->name); + QString(" Q_INVOKABLE %1 get%2(quint32 index) const;\n") + .arg(type).arg(field->name); + propertiesImpl += + QString("%1 %2::get%3(quint32 index) const\n" + "{\n" + " QMutexLocker locker(mutex);\n" + " return data.%3[index];\n" + "}\n") + .arg(type).arg(info->name).arg(field->name); propertySetters += - QString(" void set%1(quint32 index, %2 value);\n") - .arg(field->name).arg(type); - propertiesImpl += - QString("void %1::set%2(quint32 index, %3 value)\n" - "{\n" - " mutex->lock();\n" - " bool changed = data.%2[index] != value;\n" - " data.%2[index] = value;\n" - " mutex->unlock();\n" - " if (changed) emit %2Changed(index,value);\n" - "}\n\n") - .arg(info->name).arg(field->name).arg(type); + QString(" void set%1(quint32 index, %2 value);\n") + .arg(field->name).arg(type); + propertiesImpl += + QString("void %1::set%2(quint32 index, %3 value)\n" + "{\n" + " mutex->lock();\n" + " bool changed = data.%2[index] != value;\n" + " data.%2[index] = value;\n" + " mutex->unlock();\n" + " if (changed) emit %2Changed(index,value);\n" + "}\n\n") + .arg(info->name).arg(field->name).arg(type); propertyNotifications += - QString(" void %1Changed(quint32 index, %2 value);\n") - .arg(field->name).arg(type); + QString(" void %1Changed(quint32 index, %2 value);\n") + .arg(field->name).arg(type); } else { properties += QString(" Q_PROPERTY(%1 %2 READ get%2 WRITE set%2 NOTIFY %2Changed);\n") - .arg(type).arg(field->name); + .arg(type).arg(field->name); propertyGetters += - QString(" Q_INVOKABLE %1 get%2() const;\n") - .arg(type).arg(field->name); - propertiesImpl += - QString("%1 %2::get%3() const\n" - "{\n" - " QMutexLocker locker(mutex);\n" - " return data.%3;\n" - "}\n") - .arg(type).arg(info->name).arg(field->name); + QString(" Q_INVOKABLE %1 get%2() const;\n") + .arg(type).arg(field->name); + propertiesImpl += + QString("%1 %2::get%3() const\n" + "{\n" + " QMutexLocker locker(mutex);\n" + " return data.%3;\n" + "}\n") + .arg(type).arg(info->name).arg(field->name); propertySetters += - QString(" void set%1(%2 value);\n") - .arg(field->name).arg(type); - propertiesImpl += - QString("void %1::set%2(%3 value)\n" - "{\n" - " mutex->lock();\n" - " bool changed = data.%2 != value;\n" - " data.%2 = value;\n" - " mutex->unlock();\n" - " if (changed) emit %2Changed(value);\n" - "}\n\n") - .arg(info->name).arg(field->name).arg(type); - propertyNotifications += - QString(" void %1Changed(%2 value);\n") - .arg(field->name).arg(type); + QString(" void set%1(%2 value);\n") + .arg(field->name).arg(type); + propertiesImpl += + QString("void %1::set%2(%3 value)\n" + "{\n" + " mutex->lock();\n" + " bool changed = data.%2 != value;\n" + " data.%2 = value;\n" + " mutex->unlock();\n" + " if (changed) emit %2Changed(value);\n" + "}\n\n") + .arg(info->name).arg(field->name).arg(type); + propertyNotifications += + QString(" void %1Changed(%2 value);\n") + .arg(field->name).arg(type); propertyNotificationsImpl += - QString(" //if (data.%1 != oldData.%1)\n" - " emit %1Changed(data.%1);\n") - .arg(field->name); + QString(" //if (data.%1 != oldData.%1)\n" + " emit %1Changed(data.%1);\n") + .arg(field->name); } } @@ -202,43 +199,42 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo* info) // Replace the $(FIELDSINIT) tag QString finit; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { // Setup element names - QString varElemName = info->fields[n]->name + "ElemNames"; - finit.append( QString(" QStringList %1;\n").arg(varElemName) ); + QString varElemName = info->fields[n]->name + "ElemNames"; + finit.append(QString(" QStringList %1;\n").arg(varElemName)); QStringList elemNames = info->fields[n]->elementNames; - for (int m = 0; m < elemNames.length(); ++m) - finit.append( QString(" %1.append(\"%2\");\n") - .arg(varElemName) - .arg(elemNames[m]) ); + for (int m = 0; m < elemNames.length(); ++m) { + finit.append(QString(" %1.append(\"%2\");\n") + .arg(varElemName) + .arg(elemNames[m])); + } // Only for enum types if (info->fields[n]->type == FIELDTYPE_ENUM) { QString varOptionName = info->fields[n]->name + "EnumOptions"; - finit.append( QString(" QStringList %1;\n").arg(varOptionName) ); - QStringList options = info->fields[n]->options; - for (int m = 0; m < options.length(); ++m) - { - finit.append( QString(" %1.append(\"%2\");\n") - .arg(varOptionName) - .arg(options[m]) ); + finit.append(QString(" QStringList %1;\n").arg(varOptionName)); + QStringList options = info->fields[n]->options; + for (int m = 0; m < options.length(); ++m) { + finit.append(QString(" %1.append(\"%2\");\n") + .arg(varOptionName) + .arg(options[m])); } - finit.append( QString(" fields.append( new UAVObjectField(QString(\"%1\"), QString(\"%2\"), UAVObjectField::ENUM, %3, %4, QString(\"%5\")));\n") - .arg(info->fields[n]->name) - .arg(info->fields[n]->units) - .arg(varElemName) - .arg(varOptionName) - .arg(info->fields[n]->limitValues)); + finit.append(QString(" fields.append( new UAVObjectField(QString(\"%1\"), QString(\"%2\"), UAVObjectField::ENUM, %3, %4, QString(\"%5\")));\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->units) + .arg(varElemName) + .arg(varOptionName) + .arg(info->fields[n]->limitValues)); } // For all other types else { - finit.append( QString(" fields.append( new UAVObjectField(QString(\"%1\"), QString(\"%2\"), UAVObjectField::%3, %4, QStringList(), QString(\"%5\")));\n") - .arg(info->fields[n]->name) - .arg(info->fields[n]->units) - .arg(fieldTypeStrCPPClass[info->fields[n]->type]) - .arg(varElemName) - .arg(info->fields[n]->limitValues)); + finit.append(QString(" fields.append( new UAVObjectField(QString(\"%1\"), QString(\"%2\"), UAVObjectField::%3, %4, QStringList(), QString(\"%5\")));\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->units) + .arg(fieldTypeStrCPPClass[info->fields[n]->type]) + .arg(varElemName) + .arg(info->fields[n]->limitValues)); } } outCode.replace(QString("$(FIELDSINIT)"), finit); @@ -246,25 +242,22 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo* info) // Replace the $(DATAFIELDINFO) tag QString name; QString enums; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { enums.append(QString(" // Field %1 information\n").arg(info->fields[n]->name)); // Only for enum types - if (info->fields[n]->type == FIELDTYPE_ENUM) - { + if (info->fields[n]->type == FIELDTYPE_ENUM) { enums.append(QString(" /* Enumeration options for field %1 */\n").arg(info->fields[n]->name)); enums.append(" typedef enum { "); // Go through each option QStringList options = info->fields[n]->options; for (int m = 0; m < options.length(); ++m) { - QString s = (m != (options.length()-1)) ? "%1_%2=%3, " : "%1_%2=%3"; - enums.append( s.arg( info->fields[n]->name.toUpper() ) - .arg( options[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), "") ) - .arg(m) ); - + QString s = (m != (options.length() - 1)) ? "%1_%2=%3, " : "%1_%2=%3"; + enums.append(s.arg(info->fields[n]->name.toUpper()) + .arg(options[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), "")) + .arg(m)); } - enums.append( QString(" } %1Options;\n") - .arg( info->fields[n]->name ) ); + enums.append(QString(" } %1Options;\n") + .arg(info->fields[n]->name)); } // Generate element names (only if field has more than one element) if (info->fields[n]->numElements > 1 && !info->fields[n]->defaultElementNames) { @@ -273,75 +266,61 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo* info) // Go through the element names QStringList elemNames = info->fields[n]->elementNames; for (int m = 0; m < elemNames.length(); ++m) { - QString s = (m != (elemNames.length()-1)) ? "%1_%2=%3, " : "%1_%2=%3"; - enums.append( s.arg( info->fields[n]->name.toUpper() ) - .arg( elemNames[m].toUpper() ) - .arg(m) ); - + QString s = (m != (elemNames.length() - 1)) ? "%1_%2=%3, " : "%1_%2=%3"; + enums.append(s.arg(info->fields[n]->name.toUpper()) + .arg(elemNames[m].toUpper()) + .arg(m)); } - enums.append( QString(" } %1Elem;\n") - .arg( info->fields[n]->name ) ); + enums.append(QString(" } %1Elem;\n") + .arg(info->fields[n]->name)); } // Generate array information if (info->fields[n]->numElements > 1) { enums.append(QString(" /* Number of elements for field %1 */\n").arg(info->fields[n]->name)); - enums.append( QString(" static const quint32 %1_NUMELEM = %2;\n") - .arg( info->fields[n]->name.toUpper() ) - .arg( info->fields[n]->numElements ) ); + enums.append(QString(" static const quint32 %1_NUMELEM = %2;\n") + .arg(info->fields[n]->name.toUpper()) + .arg(info->fields[n]->numElements)); } } outInclude.replace(QString("$(DATAFIELDINFO)"), enums); // Replace the $(INITFIELDS) tag QString initfields; - for (int n = 0; n < info->fields.length(); ++n) - { - if (!info->fields[n]->defaultValues.isEmpty() ) - { + for (int n = 0; n < info->fields.length(); ++n) { + if (!info->fields[n]->defaultValues.isEmpty()) { // For non-array fields - if ( info->fields[n]->numElements == 1) - { - if ( info->fields[n]->type == FIELDTYPE_ENUM ) - { - initfields.append( QString(" data.%1 = %2;\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->options.indexOf( info->fields[n]->defaultValues[0] ) ) ); + if (info->fields[n]->numElements == 1) { + if (info->fields[n]->type == FIELDTYPE_ENUM) { + initfields.append(QString(" data.%1 = %2;\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->options.indexOf(info->fields[n]->defaultValues[0]))); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + initfields.append(QString(" data.%1 = %2;\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0].toFloat())); + } else { + initfields.append(QString(" data.%1 = %2;\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0].toInt())); } - else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) - { - initfields.append( QString(" data.%1 = %2;\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->defaultValues[0].toFloat() ) ); - } - else - { - initfields.append( QString(" data.%1 = %2;\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->defaultValues[0].toInt() ) ); - } - } - else - { + } else { // Initialize all fields in the array - for (int idx = 0; idx < info->fields[n]->numElements; ++idx) - { - if ( info->fields[n]->type == FIELDTYPE_ENUM ) { - initfields.append( QString(" data.%1[%2] = %3;\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->options.indexOf( info->fields[n]->defaultValues[idx] ) ) ); - } - else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) { - initfields.append( QString(" data.%1[%2] = %3;\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->defaultValues[idx].toFloat() ) ); - } - else { - initfields.append( QString(" data.%1[%2] = %3;\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->defaultValues[idx].toInt() ) ); + for (int idx = 0; idx < info->fields[n]->numElements; ++idx) { + if (info->fields[n]->type == FIELDTYPE_ENUM) { + initfields.append(QString(" data.%1[%2] = %3;\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->options.indexOf(info->fields[n]->defaultValues[idx]))); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + initfields.append(QString(" data.%1[%2] = %3;\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx].toFloat())); + } else { + initfields.append(QString(" data.%1[%2] = %3;\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx].toInt())); } } } @@ -351,12 +330,12 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo* info) outCode.replace(QString("$(INITFIELDS)"), initfields); // Write the GCS code - bool res = writeFileIfDiffrent( gcsOutputPath.absolutePath() + "/" + info->namelc + ".cpp", outCode ); + bool res = writeFileIfDiffrent(gcsOutputPath.absolutePath() + "/" + info->namelc + ".cpp", outCode); if (!res) { cout << "Error: Could not write gcs output files" << endl; return false; } - res = writeFileIfDiffrent( gcsOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude ); + res = writeFileIfDiffrent(gcsOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude); if (!res) { cout << "Error: Could not write gcs output files" << endl; return false; diff --git a/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.h b/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.h index a0106d6bc..a41e47011 100644 --- a/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.h +++ b/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.h @@ -31,16 +31,15 @@ #include "../generator_common.h" -class UAVObjectGeneratorGCS -{ +class UAVObjectGeneratorGCS { public: - bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); + bool generate(UAVObjectParser *gen, QString templatepath, QString outputpath); private: - bool process_object(ObjectInfo* info); + bool process_object(ObjectInfo *info); - QString gcsCodeTemplate,gcsIncludeTemplate; - QStringList fieldTypeStrCPP,fieldTypeStrCPPClass; + QString gcsCodeTemplate, gcsIncludeTemplate; + QStringList fieldTypeStrCPP, fieldTypeStrCPPClass; QDir gcsCodePath; QDir gcsOutputPath; }; diff --git a/ground/uavobjgenerator/generators/generator_common.cpp b/ground/uavobjgenerator/generators/generator_common.cpp index 12fae596d..3a96ca982 100644 --- a/ground/uavobjgenerator/generators/generator_common.cpp +++ b/ground/uavobjgenerator/generators/generator_common.cpp @@ -25,7 +25,7 @@ */ #include "generator_common.h" -void replaceCommonTags(QString& out) +void replaceCommonTags(QString & out) { // Replace $(GENERATEDWARNING) tag out.replace(QString("$(GENERATEDWARNING)"), "This is a autogenerated file!! Do not modify and expect a result."); @@ -33,10 +33,10 @@ void replaceCommonTags(QString& out) /** * Replace all the common tags from the template file with actual object information. */ -void replaceCommonTags(QString& out, ObjectInfo* info) +void replaceCommonTags(QString & out, ObjectInfo *info) { + QStringList updateModeStr, accessModeStr; - QStringList updateModeStr,accessModeStr; updateModeStr << "UPDATEMODE_MANUAL" << "UPDATEMODE_PERIODIC" << "UPDATEMODE_ONCHANGE" << "UPDATEMODE_THROTTLED"; @@ -45,7 +45,7 @@ void replaceCommonTags(QString& out, ObjectInfo* info) QString value; - // replace the tags which don't need info + // replace the tags which don't need info replaceCommonTags(out); // Replace $(XMLFILE) tag @@ -65,13 +65,13 @@ void replaceCommonTags(QString& out, ObjectInfo* info) // Replace $(UOBJID) tag out.replace(QString("$(UOBJID)"), QString().setNum((qint32)info->id)); // Replace $(OBJIDHEX) tag - out.replace(QString("$(OBJIDHEX)"),QString("0x")+ QString().setNum(info->id,16).toUpper()); + out.replace(QString("$(OBJIDHEX)"), QString("0x") + QString().setNum(info->id, 16).toUpper()); // Replace $(ISSINGLEINST) tag - out.replace(QString("$(ISSINGLEINST)"), boolTo01String( info->isSingleInst )); - out.replace(QString("$(ISSINGLEINSTTF)"), boolToTRUEFALSEString( info->isSingleInst )); + out.replace(QString("$(ISSINGLEINST)"), boolTo01String(info->isSingleInst)); + out.replace(QString("$(ISSINGLEINSTTF)"), boolToTRUEFALSEString(info->isSingleInst)); // Replace $(ISSETTINGS) tag - out.replace(QString("$(ISSETTINGS)"), boolTo01String( info->isSettings )); - out.replace(QString("$(ISSETTINGSTF)"), boolToTRUEFALSEString( info->isSettings )); + out.replace(QString("$(ISSETTINGS)"), boolTo01String(info->isSettings)); + out.replace(QString("$(ISSETTINGSTF)"), boolToTRUEFALSEString(info->isSettings)); // Replace $(GCSACCESS) tag value = accessModeStr[info->gcsAccess]; out.replace(QString("$(GCSACCESS)"), value); @@ -79,16 +79,16 @@ void replaceCommonTags(QString& out, ObjectInfo* info) value = accessModeStr[info->flightAccess]; out.replace(QString("$(FLIGHTACCESS)"), value); // Replace $(FLIGHTTELEM_ACKED) tag - out.replace(QString("$(FLIGHTTELEM_ACKED)"), boolTo01String( info->flightTelemetryAcked )); - out.replace(QString("$(FLIGHTTELEM_ACKEDTF)"), boolToTRUEFALSEString( info->flightTelemetryAcked )); + out.replace(QString("$(FLIGHTTELEM_ACKED)"), boolTo01String(info->flightTelemetryAcked)); + out.replace(QString("$(FLIGHTTELEM_ACKEDTF)"), boolToTRUEFALSEString(info->flightTelemetryAcked)); // Replace $(FLIGHTTELEM_UPDATEMODE) tag - value =updateModeStr[info->flightTelemetryUpdateMode]; + value = updateModeStr[info->flightTelemetryUpdateMode]; out.replace(QString("$(FLIGHTTELEM_UPDATEMODE)"), value); // Replace $(FLIGHTTELEM_UPDATEPERIOD) tag out.replace(QString("$(FLIGHTTELEM_UPDATEPERIOD)"), QString().setNum(info->flightTelemetryUpdatePeriod)); // Replace $(GCSTELEM_ACKED) tag - out.replace(QString("$(GCSTELEM_ACKED)"), boolTo01String( info->gcsTelemetryAcked )); - out.replace(QString("$(GCSTELEM_ACKEDTF)"), boolToTRUEFALSEString( info->gcsTelemetryAcked )); + out.replace(QString("$(GCSTELEM_ACKED)"), boolTo01String(info->gcsTelemetryAcked)); + out.replace(QString("$(GCSTELEM_ACKEDTF)"), boolToTRUEFALSEString(info->gcsTelemetryAcked)); // Replace $(GCSTELEM_UPDATEMODE) tag value = updateModeStr[info->gcsTelemetryUpdateMode]; out.replace(QString("$(GCSTELEM_UPDATEMODE)"), value); @@ -99,8 +99,6 @@ void replaceCommonTags(QString& out, ObjectInfo* info) out.replace(QString("$(LOGGING_UPDATEMODE)"), value); // Replace $(LOGGING_UPDATEPERIOD) tag out.replace(QString("$(LOGGING_UPDATEPERIOD)"), QString().setNum(info->loggingUpdatePeriod)); - - } /** @@ -108,8 +106,9 @@ void replaceCommonTags(QString& out, ObjectInfo* info) */ QString boolTo01String(bool value) { - if ( value ) + if (value) { return QString("1"); + } return QString("0"); } @@ -119,9 +118,9 @@ QString boolTo01String(bool value) */ QString boolToTRUEFALSEString(bool value) { - if ( value ) + if (value) { return QString("TRUE"); + } return QString("FALSE"); } - diff --git a/ground/uavobjgenerator/generators/generator_common.h b/ground/uavobjgenerator/generators/generator_common.h index 95dab21e5..ab648b5c4 100644 --- a/ground/uavobjgenerator/generators/generator_common.h +++ b/ground/uavobjgenerator/generators/generator_common.h @@ -33,8 +33,8 @@ // These special chars (regexp) will be removed from C/java identifiers #define ENUM_SPECIAL_CHARS "[\\.\\-\\s\\+/\\(\\)]" -void replaceCommonTags(QString& out, ObjectInfo* info); -void replaceCommonTags(QString& out); +void replaceCommonTags(QString & out, ObjectInfo *info); +void replaceCommonTags(QString & out); QString boolTo01String(bool value); QString boolToTRUEFALSEString(bool value); diff --git a/ground/uavobjgenerator/generators/generator_io.cpp b/ground/uavobjgenerator/generators/generator_io.cpp index ac81f20b3..67afdd05a 100644 --- a/ground/uavobjgenerator/generators/generator_io.cpp +++ b/ground/uavobjgenerator/generators/generator_io.cpp @@ -31,12 +31,14 @@ using namespace std; /** * Read a file and return its contents as a string */ -QString readFile(QString name,bool do_warn) +QString readFile(QString name, bool do_warn) { QFile file(name); - if (!file.open(QFile::ReadOnly)) { - if (do_warn) + + if (!file.open(QFile::ReadOnly)) { + if (do_warn) { std::cout << "Warning: Could not open " << name.toStdString() << endl; + } return QString(); } @@ -51,17 +53,19 @@ QString readFile(QString name,bool do_warn) */ QString readFile(QString name) { - return readFile(name,true); + return readFile(name, true); } /** * Write contents of string to file */ -bool writeFile(QString name, QString& str) +bool writeFile(QString name, QString & str) { QFile file(name); - if (!file.open(QFile::WriteOnly)) + + if (!file.open(QFile::WriteOnly)) { return false; + } QTextStream fileStr(&file); fileStr << str; file.close(); @@ -71,9 +75,10 @@ bool writeFile(QString name, QString& str) /** * Write contents of string to file if the content changes */ -bool writeFileIfDiffrent(QString name, QString& str) +bool writeFileIfDiffrent(QString name, QString & str) { - if (str==readFile(name,false)) + if (str == readFile(name, false)) { return true; - return writeFile(name,str); + } + return writeFile(name, str); } diff --git a/ground/uavobjgenerator/generators/generator_io.h b/ground/uavobjgenerator/generators/generator_io.h index 47c554c7e..8d4500bf4 100644 --- a/ground/uavobjgenerator/generators/generator_io.h +++ b/ground/uavobjgenerator/generators/generator_io.h @@ -36,7 +36,7 @@ QString readFile(QString name); QString readFile(QString name); -bool writeFile(QString name, QString& str); -bool writeFileIfDiffrent(QString name, QString& str); +bool writeFile(QString name, QString & str); +bool writeFileIfDiffrent(QString name, QString & str); #endif diff --git a/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.cpp b/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.cpp index 3698ebba1..a4e97457d 100644 --- a/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.cpp +++ b/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.cpp @@ -28,19 +28,20 @@ #include "uavobjectgeneratorjava.h" using namespace std; -bool UAVObjectGeneratorJava::generate(UAVObjectParser* parser,QString templatepath,QString outputpath) { +bool UAVObjectGeneratorJava::generate(UAVObjectParser *parser, QString templatepath, QString outputpath) +{ fieldTypeStrCPP << "Byte" << "Short" << "Int" << "Short" << "Int" << "Long" << "Float" << "Byte"; fieldTypeStrCPPClass << "INT8" << "INT16" << "INT32" - << "UINT8" << "UINT16" << "UINT32" << "FLOAT32" << "ENUM"; + << "UINT8" << "UINT16" << "UINT32" << "FLOAT32" << "ENUM"; - javaCodePath = QDir( templatepath + QString(JAVA_TEMPLATE_DIR)); - javaOutputPath = QDir( outputpath + QString("java") ); + javaCodePath = QDir(templatepath + QString(JAVA_TEMPLATE_DIR)); + javaOutputPath = QDir(outputpath + QString("java")); javaOutputPath.mkpath(javaOutputPath.absolutePath()); - javaCodeTemplate = readFile( javaCodePath.absoluteFilePath("uavobject.java.template") ); - QString javaInitTemplate = readFile( javaCodePath.absoluteFilePath("uavobjectsinit.java.template") ); + javaCodeTemplate = readFile(javaCodePath.absoluteFilePath("uavobject.java.template")); + QString javaInitTemplate = readFile(javaCodePath.absoluteFilePath("uavobjectsinit.java.template")); if (javaCodeTemplate.isEmpty() || javaInitTemplate.isEmpty()) { std::cerr << "Problem reading java code templates" << endl; @@ -51,7 +52,7 @@ bool UAVObjectGeneratorJava::generate(UAVObjectParser* parser,QString templatepa QString javaObjInit; for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { - ObjectInfo* info=parser->getObjectByIndex(objidx); + ObjectInfo *info = parser->getObjectByIndex(objidx); process_object(info); javaObjInit.append("\t\t\tobjMngr.registerObject( new " + info->name + "() );\n"); @@ -59,9 +60,9 @@ bool UAVObjectGeneratorJava::generate(UAVObjectParser* parser,QString templatepa } // Write the gcs object inialization files - javaInitTemplate.replace( QString("$(OBJINC)"), objInc); - javaInitTemplate.replace( QString("$(OBJINIT)"), javaObjInit); - bool res = writeFileIfDiffrent( javaOutputPath.absolutePath() + "/UAVObjectsInitialize.java", javaInitTemplate ); + javaInitTemplate.replace(QString("$(OBJINC)"), objInc); + javaInitTemplate.replace(QString("$(OBJINIT)"), javaObjInit); + bool res = writeFileIfDiffrent(javaOutputPath.absolutePath() + "/UAVObjectsInitialize.java", javaInitTemplate); if (!res) { cout << "Error: Could not write output files" << endl; return false; @@ -74,14 +75,15 @@ bool UAVObjectGeneratorJava::generate(UAVObjectParser* parser,QString templatepa /** * Generate the java object files */ -bool UAVObjectGeneratorJava::process_object(ObjectInfo* info) +bool UAVObjectGeneratorJava::process_object(ObjectInfo *info) { - if (info == NULL) + if (info == NULL) { return false; + } // Prepare output strings QString outInclude = javaIncludeTemplate; - QString outCode = javaCodeTemplate; + QString outCode = javaCodeTemplate; // Replace common tags replaceCommonTags(outInclude, info); @@ -90,62 +92,57 @@ bool UAVObjectGeneratorJava::process_object(ObjectInfo* info) // Replace the $(DATAFIELDS) tag QString type; QString fields; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { // Determine type type = fieldTypeStrCPP[info->fields[n]->type]; // Append field - if ( info->fields[n]->numElements > 1 ) - { - fields.append( QString(" %1 %2[%3];\n").arg(type).arg(info->fields[n]->name) - .arg(info->fields[n]->numElements) ); - } - else - { - fields.append( QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name) ); + if (info->fields[n]->numElements > 1) { + fields.append(QString(" %1 %2[%3];\n").arg(type).arg(info->fields[n]->name) + .arg(info->fields[n]->numElements)); + } else { + fields.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name)); } } outInclude.replace(QString("$(DATAFIELDS)"), fields); // Replace the $(FIELDSINIT) tag QString finit; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { finit.append("\n"); // Setup element names - QString varElemName = info->fields[n]->name + "ElemNames"; - finit.append( QString("\t\tList %1 = new ArrayList();\n").arg(varElemName) ); + QString varElemName = info->fields[n]->name + "ElemNames"; + finit.append(QString("\t\tList %1 = new ArrayList();\n").arg(varElemName)); QStringList elemNames = info->fields[n]->elementNames; - for (int m = 0; m < elemNames.length(); ++m) - finit.append( QString("\t\t%1.add(\"%2\");\n") - .arg(varElemName) - .arg(elemNames[m]) ); + for (int m = 0; m < elemNames.length(); ++m) { + finit.append(QString("\t\t%1.add(\"%2\");\n") + .arg(varElemName) + .arg(elemNames[m])); + } // Only for enum types if (info->fields[n]->type == FIELDTYPE_ENUM) { QString varOptionName = info->fields[n]->name + "EnumOptions"; - finit.append( QString("\t\tList %1 = new ArrayList();\n").arg(varOptionName) ); - QStringList options = info->fields[n]->options; - for (int m = 0; m < options.length(); ++m) - { - finit.append( QString("\t\t%1.add(\"%2\");\n") - .arg(varOptionName) - .arg(options[m]) ); + finit.append(QString("\t\tList %1 = new ArrayList();\n").arg(varOptionName)); + QStringList options = info->fields[n]->options; + for (int m = 0; m < options.length(); ++m) { + finit.append(QString("\t\t%1.add(\"%2\");\n") + .arg(varOptionName) + .arg(options[m])); } - finit.append( QString("\t\tfields.add( new UAVObjectField(\"%1\", \"%2\", UAVObjectField.FieldType.ENUM, %3, %4) );\n") - .arg(info->fields[n]->name) - .arg(info->fields[n]->units) - .arg(varElemName) - .arg(varOptionName) ); + finit.append(QString("\t\tfields.add( new UAVObjectField(\"%1\", \"%2\", UAVObjectField.FieldType.ENUM, %3, %4) );\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->units) + .arg(varElemName) + .arg(varOptionName)); } // For all other types else { - finit.append( QString("\t\tfields.add( new UAVObjectField(\"%1\", \"%2\", UAVObjectField.FieldType.%3, %4, null) );\n") - .arg(info->fields[n]->name) - .arg(info->fields[n]->units) - .arg(fieldTypeStrCPPClass[info->fields[n]->type]) - .arg(varElemName) ); + finit.append(QString("\t\tfields.add( new UAVObjectField(\"%1\", \"%2\", UAVObjectField.FieldType.%3, %4, null) );\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->units) + .arg(fieldTypeStrCPPClass[info->fields[n]->type]) + .arg(varElemName)); } } outCode.replace(QString("$(FIELDSINIT)"), finit); @@ -153,25 +150,22 @@ bool UAVObjectGeneratorJava::process_object(ObjectInfo* info) // Replace the $(DATAFIELDINFO) tag QString name; QString enums; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { enums.append(QString(" // Field %1 information\n").arg(info->fields[n]->name)); // Only for enum types - if (info->fields[n]->type == FIELDTYPE_ENUM) - { + if (info->fields[n]->type == FIELDTYPE_ENUM) { enums.append(QString(" /* Enumeration options for field %1 */\n").arg(info->fields[n]->name)); enums.append(" typedef enum { "); // Go through each option QStringList options = info->fields[n]->options; for (int m = 0; m < options.length(); ++m) { - QString s = (m != (options.length()-1)) ? "%1_%2=%3, " : "%1_%2=%3"; - enums.append( s.arg( info->fields[n]->name.toUpper() ) - .arg( options[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), "") ) - .arg(m) ); - + QString s = (m != (options.length() - 1)) ? "%1_%2=%3, " : "%1_%2=%3"; + enums.append(s.arg(info->fields[n]->name.toUpper()) + .arg(options[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), "")) + .arg(m)); } - enums.append( QString(" } %1Options;\n") - .arg( info->fields[n]->name ) ); + enums.append(QString(" } %1Options;\n") + .arg(info->fields[n]->name)); } // Generate element names (only if field has more than one element) if (info->fields[n]->numElements > 1 && !info->fields[n]->defaultElementNames) { @@ -180,75 +174,61 @@ bool UAVObjectGeneratorJava::process_object(ObjectInfo* info) // Go through the element names QStringList elemNames = info->fields[n]->elementNames; for (int m = 0; m < elemNames.length(); ++m) { - QString s = (m != (elemNames.length()-1)) ? "%1_%2=%3, " : "%1_%2=%3"; - enums.append( s.arg( info->fields[n]->name.toUpper() ) - .arg( elemNames[m].toUpper() ) - .arg(m) ); - + QString s = (m != (elemNames.length() - 1)) ? "%1_%2=%3, " : "%1_%2=%3"; + enums.append(s.arg(info->fields[n]->name.toUpper()) + .arg(elemNames[m].toUpper()) + .arg(m)); } - enums.append( QString(" } %1Elem;\n") - .arg( info->fields[n]->name ) ); + enums.append(QString(" } %1Elem;\n") + .arg(info->fields[n]->name)); } // Generate array information if (info->fields[n]->numElements > 1) { enums.append(QString(" /* Number of elements for field %1 */\n").arg(info->fields[n]->name)); - enums.append( QString(" static const quint32 %1_NUMELEM = %2;\n") - .arg( info->fields[n]->name.toUpper() ) - .arg( info->fields[n]->numElements ) ); + enums.append(QString(" static const quint32 %1_NUMELEM = %2;\n") + .arg(info->fields[n]->name.toUpper()) + .arg(info->fields[n]->numElements)); } } outInclude.replace(QString("$(DATAFIELDINFO)"), enums); // Replace the $(INITFIELDS) tag QString initfields; - for (int n = 0; n < info->fields.length(); ++n) - { - if (!info->fields[n]->defaultValues.isEmpty() ) - { + for (int n = 0; n < info->fields.length(); ++n) { + if (!info->fields[n]->defaultValues.isEmpty()) { // For non-array fields - if ( info->fields[n]->numElements == 1) - { - if ( info->fields[n]->type == FIELDTYPE_ENUM ) - { - initfields.append( QString("\t\tgetField(\"%1\").setValue(\"%2\");\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->defaultValues[0] ) ); + if (info->fields[n]->numElements == 1) { + if (info->fields[n]->type == FIELDTYPE_ENUM) { + initfields.append(QString("\t\tgetField(\"%1\").setValue(\"%2\");\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0])); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + initfields.append(QString("\t\tgetField(\"%1\").setValue(%2);\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0].toFloat())); + } else { + initfields.append(QString("\t\tgetField(\"%1\").setValue(%2);\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0].toInt())); } - else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) - { - initfields.append( QString("\t\tgetField(\"%1\").setValue(%2);\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->defaultValues[0].toFloat() ) ); - } - else - { - initfields.append( QString("\t\tgetField(\"%1\").setValue(%2);\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->defaultValues[0].toInt() ) ); - } - } - else - { + } else { // Initialize all fields in the array - for (int idx = 0; idx < info->fields[n]->numElements; ++idx) - { - if ( info->fields[n]->type == FIELDTYPE_ENUM ) { - initfields.append( QString("\t\tgetField(\"%1\").setValue(\"%3\",%2);\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->defaultValues[idx] ) ); - } - else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) { - initfields.append( QString("\t\tgetField(\"%1\").setValue(%3,%2);\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->defaultValues[idx].toFloat() ) ); - } - else { - initfields.append( QString("\t\tgetField(\"%1\").setValue(%3,%2);\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->defaultValues[idx].toInt() ) ); + for (int idx = 0; idx < info->fields[n]->numElements; ++idx) { + if (info->fields[n]->type == FIELDTYPE_ENUM) { + initfields.append(QString("\t\tgetField(\"%1\").setValue(\"%3\",%2);\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx])); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + initfields.append(QString("\t\tgetField(\"%1\").setValue(%3,%2);\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx].toFloat())); + } else { + initfields.append(QString("\t\tgetField(\"%1\").setValue(%3,%2);\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx].toInt())); } } } @@ -258,7 +238,7 @@ bool UAVObjectGeneratorJava::process_object(ObjectInfo* info) outCode.replace(QString("$(INITFIELDS)"), initfields); // Write the java code - bool res = writeFileIfDiffrent( javaOutputPath.absolutePath() + "/" + info->name + ".java", outCode ); + bool res = writeFileIfDiffrent(javaOutputPath.absolutePath() + "/" + info->name + ".java", outCode); if (!res) { cout << "Error: Could not write gcs output files" << endl; return false; @@ -266,4 +246,3 @@ bool UAVObjectGeneratorJava::process_object(ObjectInfo* info) return true; } - diff --git a/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.h b/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.h index 8d07484f6..8488d57ed 100644 --- a/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.h +++ b/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.h @@ -28,22 +28,21 @@ #define UAVOBJECTGENERATORJAVA_H #define JAVA_TEMPLATE_DIR "ground/openpilotgcs/src/libs/juavobjects/templates/" -#define JAVA_CODE_DIR "java/src/org/openpilot/uavtalk" +#define JAVA_CODE_DIR "java/src/org/openpilot/uavtalk" #include "../generator_common.h" -class UAVObjectGeneratorJava -{ +class UAVObjectGeneratorJava { public: - bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); + bool generate(UAVObjectParser *gen, QString templatepath, QString outputpath); private: - bool process_object(ObjectInfo* info); + bool process_object(ObjectInfo *info); QString javaCodeTemplate, javaIncludeTemplate; - QStringList fieldTypeStrCPP,fieldTypeStrCPPClass; + QStringList fieldTypeStrCPP, fieldTypeStrCPPClass; QDir javaCodePath; QDir javaOutputPath; - }; +}; -#endif +#endif // ifndef UAVOBJECTGENERATORJAVA_H diff --git a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp index d0894b892..b6611164b 100644 --- a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp +++ b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp @@ -28,38 +28,38 @@ using namespace std; -bool UAVObjectGeneratorMatlab::generate(UAVObjectParser* parser,QString templatepath,QString outputpath) { - +bool UAVObjectGeneratorMatlab::generate(UAVObjectParser *parser, QString templatepath, QString outputpath) +{ fieldTypeStrMatlab << "int8" << "int16" << "int32" - << "uint8" << "uint16" << "uint32" << "single" << "uint8"; + << "uint8" << "uint16" << "uint32" << "single" << "uint8"; fieldSizeStrMatlab << "1" << "2" << "4" - << "1" << "2" << "4" << "4" << "1"; + << "1" << "2" << "4" << "4" << "1"; - QDir matlabTemplatePath = QDir( templatepath + QString(MATLAB_CODE_DIR)); - QDir matlabOutputPath = QDir( outputpath + QString("matlab") ); + QDir matlabTemplatePath = QDir(templatepath + QString(MATLAB_CODE_DIR)); + QDir matlabOutputPath = QDir(outputpath + QString("matlab")); matlabOutputPath.mkpath(matlabOutputPath.absolutePath()); - QString matlabCodeTemplate = readFile( matlabTemplatePath.absoluteFilePath( "uavobject.m.template") ); + QString matlabCodeTemplate = readFile(matlabTemplatePath.absoluteFilePath("uavobject.m.template")); - if (matlabCodeTemplate.isEmpty() ) { + if (matlabCodeTemplate.isEmpty()) { std::cerr << "Problem reading matlab templates" << endl; return false; } for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { - ObjectInfo* info=parser->getObjectByIndex(objidx); - int numBytes=parser->getNumBytes(objidx); + ObjectInfo *info = parser->getObjectByIndex(objidx); + int numBytes = parser->getNumBytes(objidx); process_object(info, numBytes); } - matlabCodeTemplate.replace( QString("$(INSTANTIATIONCODE)"), matlabInstantiationCode); - matlabCodeTemplate.replace( QString("$(SWITCHCODE)"), matlabSwitchCode); - matlabCodeTemplate.replace( QString("$(CLEANUPCODE)"), matlabCleanupCode); - matlabCodeTemplate.replace( QString("$(SAVEOBJECTSCODE)"), matlabSaveObjectsCode); - matlabCodeTemplate.replace( QString("$(ALLOCATIONCODE)"), matlabAllocationCode); - matlabCodeTemplate.replace( QString("$(EXPORTCSVCODE)"), matlabExportCsvCode); + matlabCodeTemplate.replace(QString("$(INSTANTIATIONCODE)"), matlabInstantiationCode); + matlabCodeTemplate.replace(QString("$(SWITCHCODE)"), matlabSwitchCode); + matlabCodeTemplate.replace(QString("$(CLEANUPCODE)"), matlabCleanupCode); + matlabCodeTemplate.replace(QString("$(SAVEOBJECTSCODE)"), matlabSaveObjectsCode); + matlabCodeTemplate.replace(QString("$(ALLOCATIONCODE)"), matlabAllocationCode); + matlabCodeTemplate.replace(QString("$(EXPORTCSVCODE)"), matlabExportCsvCode); - bool res = writeFile( matlabOutputPath.absolutePath() + "/OPLogConvert.m", matlabCodeTemplate ); + bool res = writeFile(matlabOutputPath.absolutePath() + "/OPLogConvert.m", matlabCodeTemplate); if (!res) { cout << "Error: Could not write output files" << endl; return false; @@ -71,23 +71,24 @@ bool UAVObjectGeneratorMatlab::generate(UAVObjectParser* parser,QString template /** * Generate the matlab object files */ -bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info, int numBytes) +bool UAVObjectGeneratorMatlab::process_object(ObjectInfo *info, int numBytes) { - if (info == NULL) + if (info == NULL) { return false; + } - //Declare variables + // Declare variables QString objectName(info->name); // QString objectTableName(objectName + "Objects"); QString objectTableName(objectName); QString tableIdxName(objectName.toLower() + "Idx"); QString objectID(QString().setNum(info->id)); - QString numBytesString=QString("%1").arg(numBytes); + QString numBytesString = QString("%1").arg(numBytes); - //=========================================================================// + // =========================================================================// // Generate instantiation code (will replace the $(INSTANTIATIONCODE) tag) // - //=========================================================================// + // =========================================================================// QString type; QString instantiationFields; @@ -100,10 +101,11 @@ bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info, int numBytes) // Determine type type = fieldTypeStrMatlab[info->fields[n]->type]; // Append field - if ( info->fields[n]->numElements > 1 ) + if (info->fields[n]->numElements > 1) { instantiationFields.append(",...\n\t\t '" + info->fields[n]->name + "', zeros(" + QString::number(info->fields[n]->numElements, 10) + ",1)"); - else + } else { instantiationFields.append(",...\n\t\t '" + info->fields[n]->name + "', 0"); + } } instantiationFields.append(");\n"); @@ -113,83 +115,81 @@ bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info, int numBytes) matlabInstantiationCode.append("\t" + objectName + "FidIdx = [];\n"); - //==============================================================// + // ==============================================================// // Generate 'Switch:' code (will replace the $(SWITCHCODE) tag) // - //==============================================================// + // ==============================================================// matlabSwitchCode.append("\t\tcase " + objectTableName.toUpper() + "_OBJID\n"); - matlabSwitchCode.append("\t\t\t" + tableIdxName + " = " + tableIdxName +" + 1;\n"); + matlabSwitchCode.append("\t\t\t" + tableIdxName + " = " + tableIdxName + " + 1;\n"); matlabSwitchCode.append("\t\t\t" + objectTableName + "FidIdx(" + tableIdxName + ") = bufferIdx; %#ok<*AGROW>\n"); - matlabSwitchCode.append("\t\t\tbufferIdx=bufferIdx + " + objectTableName.toUpper() + "_NUMBYTES+1; %+1 is for CRC\n"); - if(!info->isSingleInst){ + matlabSwitchCode.append("\t\t\tbufferIdx=bufferIdx + " + objectTableName.toUpper() + "_NUMBYTES+1; %+1 is for CRC\n"); + if (!info->isSingleInst) { matlabSwitchCode.append("\t\t\tbufferIdx = bufferIdx + 2; %An extra two bytes for the instance ID\n"); } - matlabSwitchCode.append("\t\t\tif " + tableIdxName + " >= length(" + objectTableName +"FidIdx) %Check to see if pre-allocated memory is exhausted\n"); + matlabSwitchCode.append("\t\t\tif " + tableIdxName + " >= length(" + objectTableName + "FidIdx) %Check to see if pre-allocated memory is exhausted\n"); matlabSwitchCode.append("\t\t\t\t" + objectTableName + "FidIdx(" + tableIdxName + "*2) = 0;\n"); matlabSwitchCode.append("\t\t\tend\n"); - - //============================================================// - // Generate 'Cleanup:' code (will replace the $(CLEANUP) tag) // - //============================================================// - matlabCleanupCode.append(objectTableName + "FidIdx =" + objectTableName + "FidIdx(1:" + tableIdxName +");\n" ); - //=================================================================// + // ============================================================// + // Generate 'Cleanup:' code (will replace the $(CLEANUP) tag) // + // ============================================================// + matlabCleanupCode.append(objectTableName + "FidIdx =" + objectTableName + "FidIdx(1:" + tableIdxName + ");\n"); + + // =================================================================// // Generate functions code (will replace the $(ALLOCATIONCODE) tag) // - //=================================================================// - //Generate function description comment + // =================================================================// + // Generate function description comment matlabAllocationCode.append("% " + objectName + " typecasting\n"); QString allocationFields; - //Add timestamp + // Add timestamp allocationFields.append("\t" + objectName + ".timestamp = " + - "double(typecast(buffer(mcolon(" + objectName + "FidIdx " - "- 20, " + objectName + "FidIdx + 4-1 -20)), 'uint32'))';\n"); + "double(typecast(buffer(mcolon(" + objectName + "FidIdx " + "- 20, " + objectName + "FidIdx + 4-1 -20)), 'uint32'))';\n"); - int currentIdx=0; + int currentIdx = 0; - //Add Instance ID, if necessary - if(!info->isSingleInst){ + // Add Instance ID, if necessary + if (!info->isSingleInst) { allocationFields.append("\t" + objectName + ".instanceID = " + - "double(typecast(buffer(mcolon(" + objectName + "FidIdx " - ", " + objectName + "FidIdx + 2-1)), 'uint16'))';\n"); - currentIdx+=2; + "double(typecast(buffer(mcolon(" + objectName + "FidIdx " + ", " + objectName + "FidIdx + 2-1)), 'uint16'))';\n"); + currentIdx += 2; } for (int n = 0; n < info->fields.length(); ++n) { // Determine variabel type type = fieldTypeStrMatlab[info->fields[n]->type]; - //Determine variable type length + // Determine variable type length QString size = fieldSizeStrMatlab[info->fields[n]->type]; // Append field - if ( info->fields[n]->numElements > 1 ){ + if (info->fields[n]->numElements > 1) { allocationFields.append("\t" + objectName + "." + info->fields[n]->name + " = " + - "reshape(double(typecast(buffer(mcolon(" + objectName + "FidIdx + " + QString("%1").arg(currentIdx) + - ", " + objectName + "FidIdx + " + QString("%1").arg(currentIdx + size.toInt()*info->fields[n]->numElements - 1) + ")), '" + type + "')), "+ QString::number(info->fields[n]->numElements, 10) + ", [] );\n"); - } - else{ + "reshape(double(typecast(buffer(mcolon(" + objectName + "FidIdx + " + QString("%1").arg(currentIdx) + + ", " + objectName + "FidIdx + " + QString("%1").arg(currentIdx + size.toInt() * info->fields[n]->numElements - 1) + ")), '" + type + "')), " + QString::number(info->fields[n]->numElements, 10) + ", [] );\n"); + } else { allocationFields.append("\t" + objectName + "." + info->fields[n]->name + " = " + - "double(typecast(buffer(mcolon(" + objectName + "FidIdx + " + QString("%1").arg(currentIdx) + - ", " + objectName + "FidIdx + " + QString("%1").arg(currentIdx + size.toInt() - 1) + ")), '" + type + "'))';\n"); + "double(typecast(buffer(mcolon(" + objectName + "FidIdx + " + QString("%1").arg(currentIdx) + + ", " + objectName + "FidIdx + " + QString("%1").arg(currentIdx + size.toInt() - 1) + ")), '" + type + "'))';\n"); } - currentIdx+=size.toInt()*info->fields[n]->numElements; + currentIdx += size.toInt() * info->fields[n]->numElements; } matlabAllocationCode.append(allocationFields); matlabAllocationCode.append("\n"); - //========================================================================// + // ========================================================================// // Generate objects saving code (will replace the $(SAVEOBJECTSCODE) tag) // - //========================================================================// - matlabSaveObjectsCode.append(",'"+objectTableName+"'"); + // ========================================================================// + matlabSaveObjectsCode.append(",'" + objectTableName + "'"); - //==========================================================================// + // ==========================================================================// // Generate objects csv export code (will replace the $(EXPORTCSVCODE) tag) // - //==========================================================================// - matlabExportCsvCode.append("\tOPLog2csv(" + objectTableName + ", '"+objectTableName+"', logfile);\n"); -// OPLog2csv(ActuatorCommand, 'ActuatorCommand', logfile) - + // ==========================================================================// + matlabExportCsvCode.append("\tOPLog2csv(" + objectTableName + ", '" + objectTableName + "', logfile);\n"); +// OPLog2csv(ActuatorCommand, 'ActuatorCommand', logfile) return true; diff --git a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h index 1d3761724..68593f358 100644 --- a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h +++ b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h @@ -31,13 +31,12 @@ #include "../generator_common.h" -class UAVObjectGeneratorMatlab -{ +class UAVObjectGeneratorMatlab { public: - bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); + bool generate(UAVObjectParser *gen, QString templatepath, QString outputpath); private: - bool process_object(ObjectInfo* info, int numBytes); + bool process_object(ObjectInfo *info, int numBytes); QString matlabInstantiationCode; QString matlabSwitchCode; QString matlabCleanupCode; @@ -46,7 +45,6 @@ private: QString matlabExportCsvCode; QStringList fieldTypeStrMatlab; QStringList fieldSizeStrMatlab; - }; -#endif +#endif // ifndef UAVOBJECTGENERATORMATLAB_H diff --git a/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.cpp b/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.cpp index 35e8e017c..f1ee41143 100644 --- a/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.cpp +++ b/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.cpp @@ -27,12 +27,13 @@ #include "uavobjectgeneratorpython.h" using namespace std; -bool UAVObjectGeneratorPython::generate(UAVObjectParser* parser,QString templatepath,QString outputpath) { +bool UAVObjectGeneratorPython::generate(UAVObjectParser *parser, QString templatepath, QString outputpath) +{ // Load template and setup output directory - pythonCodePath = QDir( templatepath + QString("flight/modules/FlightPlan/lib")); - pythonOutputPath = QDir( outputpath + QString("python") ); + pythonCodePath = QDir(templatepath + QString("flight/modules/FlightPlan/lib")); + pythonOutputPath = QDir(outputpath + QString("python")); pythonOutputPath.mkpath(pythonOutputPath.absolutePath()); - pythonCodeTemplate = readFile( pythonCodePath.absoluteFilePath("uavobject.pyt.template") ); + pythonCodeTemplate = readFile(pythonCodePath.absoluteFilePath("uavobject.pyt.template")); if (pythonCodeTemplate.isEmpty()) { std::cerr << "Problem reading python templates" << endl; return false; @@ -40,7 +41,7 @@ bool UAVObjectGeneratorPython::generate(UAVObjectParser* parser,QString template // Process each object for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { - ObjectInfo* info=parser->getObjectByIndex(objidx); + ObjectInfo *info = parser->getObjectByIndex(objidx); process_object(info); } @@ -50,10 +51,11 @@ bool UAVObjectGeneratorPython::generate(UAVObjectParser* parser,QString template /** * Generate the python object files */ -bool UAVObjectGeneratorPython::process_object(ObjectInfo* info) +bool UAVObjectGeneratorPython::process_object(ObjectInfo *info) { - if (info == NULL) + if (info == NULL) { return false; + } // Prepare output strings QString outCode = pythonCodeTemplate; @@ -63,35 +65,33 @@ bool UAVObjectGeneratorPython::process_object(ObjectInfo* info) // Replace the ($DATAFIELDS) tag QString datafields; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { // Class header datafields.append(QString("# Field %1 definition\n").arg(info->fields[n]->name)); datafields.append(QString("class %1Field(UAVObjectField):\n").arg(info->fields[n]->name)); // Only for enum types - if (info->fields[n]->type == FIELDTYPE_ENUM) - { + if (info->fields[n]->type == FIELDTYPE_ENUM) { datafields.append(QString(" # Enumeration options\n")); // Go through each option QStringList options = info->fields[n]->options; for (int m = 0; m < options.length(); ++m) { QString name = options[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), ""); - if (name[0].isDigit()) + if (name[0].isDigit()) { name = QString("N%1").arg(name); + } datafields.append(QString(" %1 = %2\n").arg(name).arg(m)); } } // Generate element names (only if field has more than one element) - if (info->fields[n]->numElements > 1 && !info->fields[n]->defaultElementNames) - { + if (info->fields[n]->numElements > 1 && !info->fields[n]->defaultElementNames) { datafields.append(QString(" # Array element names\n")); // Go through the element names QStringList elemNames = info->fields[n]->elementNames; - for (int m = 0; m < elemNames.length(); ++m) - { + for (int m = 0; m < elemNames.length(); ++m) { QString name = elemNames[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), ""); - if (name[0].isDigit()) + if (name[0].isDigit()) { name = QString("N%1").arg(name); + } datafields.append(QString(" %1 = %2\n").arg(name).arg(m)); } } @@ -103,15 +103,14 @@ bool UAVObjectGeneratorPython::process_object(ObjectInfo* info) // Replace the $(DATAFIELDINIT) tag QString fields; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { fields.append(QString(" self.%1 = %1Field()\n").arg(info->fields[n]->name)); fields.append(QString(" self.addField(self.%1)\n").arg(info->fields[n]->name)); } outCode.replace(QString("$(DATAFIELDINIT)"), fields); // Write the Python code - bool res = writeFileIfDiffrent( pythonOutputPath.absolutePath() + "/" + info->namelc + ".py", outCode ); + bool res = writeFileIfDiffrent(pythonOutputPath.absolutePath() + "/" + info->namelc + ".py", outCode); if (!res) { cout << "Error: Could not write Python output files" << endl; return false; diff --git a/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.h b/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.h index 3d21b2923..6cb31d616 100644 --- a/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.h +++ b/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.h @@ -29,13 +29,12 @@ #include "../generator_common.h" -class UAVObjectGeneratorPython -{ +class UAVObjectGeneratorPython { public: - bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); + bool generate(UAVObjectParser *gen, QString templatepath, QString outputpath); private: - bool process_object(ObjectInfo* info); + bool process_object(ObjectInfo *info); QString pythonCodeTemplate; QDir pythonCodePath; diff --git a/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.cpp b/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.cpp index 653e466e3..8a095f342 100644 --- a/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.cpp +++ b/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.cpp @@ -28,36 +28,36 @@ using namespace std; -bool UAVObjectGeneratorWireshark::generate(UAVObjectParser* parser,QString templatepath,QString outputpath) { +bool UAVObjectGeneratorWireshark::generate(UAVObjectParser *parser, QString templatepath, QString outputpath) +{ + fieldTypeStrHf << "FT_INT8" << "FT_INT16" << "FT_INT32" << "FT_UINT8" + << "FT_UINT16" << "FT_UINT32" << "FT_FLOAT" << "FT_UINT8"; + fieldTypeStrGlib << "gint8" << "gint16" << "gint32" << "guint8" + << "guint16" << "guint32" << "gfloat" << "guint8"; - fieldTypeStrHf << "FT_INT8" << "FT_INT16" << "FT_INT32" <<"FT_UINT8" - <<"FT_UINT16" << "FT_UINT32" << "FT_FLOAT" << "FT_UINT8"; - fieldTypeStrGlib << "gint8" << "gint16" << "gint32" <<"guint8" - <<"guint16" << "guint32" << "gfloat" << "guint8"; + wiresharkCodePath = QDir(templatepath + QString("ground/openpilotgcs/src/plugins/uavobjects/wireshark")); - wiresharkCodePath = QDir( templatepath + QString("ground/openpilotgcs/src/plugins/uavobjects/wireshark")); - - wiresharkOutputPath = QDir( outputpath + QString("wireshark") ); + wiresharkOutputPath = QDir(outputpath + QString("wireshark")); wiresharkOutputPath.mkpath(wiresharkOutputPath.absolutePath()); - wiresharkCodeTemplate = readFile( wiresharkCodePath.absoluteFilePath("op-uavobjects/packet-op-uavobjects.c.template") ); - wiresharkMakeTemplate = readFile( wiresharkCodePath.absoluteFilePath("op-uavobjects/Makefile.common.template") ); + wiresharkCodeTemplate = readFile(wiresharkCodePath.absoluteFilePath("op-uavobjects/packet-op-uavobjects.c.template")); + wiresharkMakeTemplate = readFile(wiresharkCodePath.absoluteFilePath("op-uavobjects/Makefile.common.template")); - if ( wiresharkCodeTemplate.isNull() || wiresharkMakeTemplate.isNull()) { - cerr << "Error: Could not open wireshark template files." << endl; - return false; + if (wiresharkCodeTemplate.isNull() || wiresharkMakeTemplate.isNull()) { + cerr << "Error: Could not open wireshark template files." << endl; + return false; } /* Copy static files for wireshark plugins root directory into output directory */ QStringList topstaticfiles; topstaticfiles << "Custom.m4" << "Custom.make" << "Custom.nmake"; for (int i = 0; i < topstaticfiles.length(); ++i) { - QFile::copy(wiresharkCodePath.absoluteFilePath(topstaticfiles[i]), - wiresharkOutputPath.absoluteFilePath(topstaticfiles[i])); + QFile::copy(wiresharkCodePath.absoluteFilePath(topstaticfiles[i]), + wiresharkOutputPath.absoluteFilePath(topstaticfiles[i])); } /* Copy static files for op-uavtalk dissector into output directory */ - QDir uavtalkOutputPath = QDir( outputpath + QString("wireshark/op-uavtalk") ); + QDir uavtalkOutputPath = QDir(outputpath + QString("wireshark/op-uavtalk")); uavtalkOutputPath.mkpath(uavtalkOutputPath.absolutePath()); QStringList uavtalkstaticfiles; uavtalkstaticfiles << "AUTHORS" << "COPYING" << "ChangeLog"; @@ -66,12 +66,12 @@ bool UAVObjectGeneratorWireshark::generate(UAVObjectParser* parser,QString templ uavtalkstaticfiles << "plugin.rc.in"; uavtalkstaticfiles << "Makefile.common" << "packet-op-uavtalk.c"; for (int i = 0; i < uavtalkstaticfiles.length(); ++i) { - QFile::copy(wiresharkCodePath.absoluteFilePath("op-uavtalk/" + uavtalkstaticfiles[i]), - uavtalkOutputPath.absoluteFilePath(uavtalkstaticfiles[i])); + QFile::copy(wiresharkCodePath.absoluteFilePath("op-uavtalk/" + uavtalkstaticfiles[i]), + uavtalkOutputPath.absoluteFilePath(uavtalkstaticfiles[i])); } /* Copy static files for op-uavobjects dissector into output directory */ - QDir uavobjectsOutputPath = QDir( outputpath + QString("wireshark/op-uavobjects") ); + QDir uavobjectsOutputPath = QDir(outputpath + QString("wireshark/op-uavobjects")); uavobjectsOutputPath.mkpath(uavobjectsOutputPath.absolutePath()); QStringList uavostaticfiles; uavostaticfiles << "AUTHORS" << "COPYING" << "ChangeLog"; @@ -79,25 +79,25 @@ bool UAVObjectGeneratorWireshark::generate(UAVObjectParser* parser,QString templ uavostaticfiles << "Makefile.am" << "moduleinfo.h" << "moduleinfo.nmake"; uavostaticfiles << "plugin.rc.in"; for (int i = 0; i < uavostaticfiles.length(); ++i) { - QFile::copy(wiresharkCodePath.absoluteFilePath("op-uavobjects/" + uavostaticfiles[i]), - uavobjectsOutputPath.absoluteFilePath(uavostaticfiles[i])); + QFile::copy(wiresharkCodePath.absoluteFilePath("op-uavobjects/" + uavostaticfiles[i]), + uavobjectsOutputPath.absoluteFilePath(uavostaticfiles[i])); } /* Generate the per-object files from the templates, and keep track of the list of generated filenames */ QString objFileNames; for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { - ObjectInfo* info = parser->getObjectByIndex(objidx); - process_object(info, uavobjectsOutputPath); - objFileNames.append(" packet-op-uavobjects-" + info->namelc + ".c"); + ObjectInfo *info = parser->getObjectByIndex(objidx); + process_object(info, uavobjectsOutputPath); + objFileNames.append(" packet-op-uavobjects-" + info->namelc + ".c"); } /* Write the uavobject dissector's Makefile.common */ - wiresharkMakeTemplate.replace( QString("$(UAVOBJFILENAMES)"), objFileNames); - bool res = writeFileIfDiffrent( uavobjectsOutputPath.absolutePath() + "/Makefile.common", - wiresharkMakeTemplate ); + wiresharkMakeTemplate.replace(QString("$(UAVOBJFILENAMES)"), objFileNames); + bool res = writeFileIfDiffrent(uavobjectsOutputPath.absolutePath() + "/Makefile.common", + wiresharkMakeTemplate); if (!res) { - cout << "Error: Could not write wireshark Makefile" << endl; - return false; + cout << "Error: Could not write wireshark Makefile" << endl; + return false; } return true; @@ -106,11 +106,12 @@ bool UAVObjectGeneratorWireshark::generate(UAVObjectParser* parser,QString templ /** * Generate the Flight object files -**/ -bool UAVObjectGeneratorWireshark::process_object(ObjectInfo* info, QDir outputpath) + **/ +bool UAVObjectGeneratorWireshark::process_object(ObjectInfo *info, QDir outputpath) { - if (info == NULL) + if (info == NULL) { return false; + } // Prepare output strings QString outCode = wiresharkCodeTemplate; @@ -121,18 +122,18 @@ bool UAVObjectGeneratorWireshark::process_object(ObjectInfo* info, QDir outputpa // Replace the $(SUBTREES) and $(SUBTREESTATICS) tags QString subtrees; QString subtreestatics; - subtreestatics.append( QString("static gint ett_uavo = -1;\r\n") ); - subtrees.append( QString("&ett_uavo,\r\n") ); + subtreestatics.append(QString("static gint ett_uavo = -1;\r\n")); + subtrees.append(QString("&ett_uavo,\r\n")); for (int n = 0; n < info->fields.length(); ++n) { - if (info->fields[n]->numElements > 1) { - /* Reserve a subtree for each array */ - subtreestatics.append( QString("static gint ett_%1_%2 = -1;\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) ); - subtrees.append( QString("&ett_%1_%2,\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) ); - } + if (info->fields[n]->numElements > 1) { + /* Reserve a subtree for each array */ + subtreestatics.append(QString("static gint ett_%1_%2 = -1;\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + subtrees.append(QString("&ett_%1_%2,\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + } } outCode.replace(QString("$(SUBTREES)"), subtrees); outCode.replace(QString("$(SUBTREESTATICS)"), subtreestatics); @@ -141,144 +142,144 @@ bool UAVObjectGeneratorWireshark::process_object(ObjectInfo* info, QDir outputpa QString type; QString fields; for (int n = 0; n < info->fields.length(); ++n) { - fields.append( QString("static int hf_op_uavobjects_%1_%2 = -1;\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name)); - if (info->fields[n]->numElements > 1) { - QStringList elemNames = info->fields[n]->elementNames; - for (int m = 0; m < elemNames.length(); ++m) { - fields.append( QString("static int hf_op_uavobjects_%1_%2_%3 = -1;\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) - .arg(elemNames[m]) ); - } - } + fields.append(QString("static int hf_op_uavobjects_%1_%2 = -1;\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + if (info->fields[n]->numElements > 1) { + QStringList elemNames = info->fields[n]->elementNames; + for (int m = 0; m < elemNames.length(); ++m) { + fields.append(QString("static int hf_op_uavobjects_%1_%2_%3 = -1;\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name) + .arg(elemNames[m])); + } + } } outCode.replace(QString("$(FIELDHANDLES)"), fields); // Replace the $(ENUMFIELDNAMES) tag QString enums; for (int n = 0; n < info->fields.length(); ++n) { - // Only for enum types - if (info->fields[n]->type == FIELDTYPE_ENUM) { - enums.append(QString("/* Field %1 information */\r\n").arg(info->fields[n]->name) ); - enums.append(QString("/* Enumeration options for field %1 */\r\n").arg(info->fields[n]->name)); - enums.append( QString("static const value_string uavobjects_%1_%2[]= {\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) ); - // Go through each option - QStringList options = info->fields[n]->options; - for (int m = 0; m < options.length(); ++m) { - enums.append ( QString("\t{ %1, \"%2\" },\r\n") - .arg(m) - .arg(options[m].replace(QRegExp(ENUM_SPECIAL_CHARS), "") ) ); - } - enums.append( QString("\t{ 0, NULL }\r\n") ); - enums.append( QString("};\r\n") ); - } + // Only for enum types + if (info->fields[n]->type == FIELDTYPE_ENUM) { + enums.append(QString("/* Field %1 information */\r\n").arg(info->fields[n]->name)); + enums.append(QString("/* Enumeration options for field %1 */\r\n").arg(info->fields[n]->name)); + enums.append(QString("static const value_string uavobjects_%1_%2[]= {\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + // Go through each option + QStringList options = info->fields[n]->options; + for (int m = 0; m < options.length(); ++m) { + enums.append(QString("\t{ %1, \"%2\" },\r\n") + .arg(m) + .arg(options[m].replace(QRegExp(ENUM_SPECIAL_CHARS), ""))); + } + enums.append(QString("\t{ 0, NULL }\r\n")); + enums.append(QString("};\r\n")); + } } outCode.replace(QString("$(ENUMFIELDNAMES)"), enums); // Replace the $(POPULATETREE) tag QString treefields; for (int n = 0; n < info->fields.length(); ++n) { - if ( info->fields[n]->numElements == 1 ) { - treefields.append( QString(" ptvcursor_add(cursor, hf_op_uavobjects_%1_%2, sizeof(%3), ENC_LITTLE_ENDIAN);\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) - .arg(fieldTypeStrGlib[info->fields[n]->type]) ); - } else { - treefields.append( QString(" {\r\n") ); - treefields.append( QString(" proto_item * it = NULL;\r\n") ); - treefields.append( QString(" it = ptvcursor_add_no_advance(cursor, hf_op_uavobjects_%1_%2, 1, ENC_NA);\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) ); - treefields.append( QString(" ptvcursor_push_subtree(cursor, it, ett_%1_%2);\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) ); - /* Populate each array element into the table */ - QStringList elemNames = info->fields[n]->elementNames; - for (int m = 0; m < elemNames.length(); ++m) { - treefields.append( QString(" ptvcursor_add(cursor, hf_op_uavobjects_%1_%2_%3, sizeof(%4), ENC_LITTLE_ENDIAN);\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) - .arg(elemNames[m]) - .arg(fieldTypeStrGlib[info->fields[n]->type]) ); - } - treefields.append( QString(" ptvcursor_pop_subtree(cursor);\r\n") ); - treefields.append( QString(" }\r\n") ); - } + if (info->fields[n]->numElements == 1) { + treefields.append(QString(" ptvcursor_add(cursor, hf_op_uavobjects_%1_%2, sizeof(%3), ENC_LITTLE_ENDIAN);\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name) + .arg(fieldTypeStrGlib[info->fields[n]->type])); + } else { + treefields.append(QString(" {\r\n")); + treefields.append(QString(" proto_item * it = NULL;\r\n")); + treefields.append(QString(" it = ptvcursor_add_no_advance(cursor, hf_op_uavobjects_%1_%2, 1, ENC_NA);\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + treefields.append(QString(" ptvcursor_push_subtree(cursor, it, ett_%1_%2);\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + /* Populate each array element into the table */ + QStringList elemNames = info->fields[n]->elementNames; + for (int m = 0; m < elemNames.length(); ++m) { + treefields.append(QString(" ptvcursor_add(cursor, hf_op_uavobjects_%1_%2_%3, sizeof(%4), ENC_LITTLE_ENDIAN);\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name) + .arg(elemNames[m]) + .arg(fieldTypeStrGlib[info->fields[n]->type])); + } + treefields.append(QString(" ptvcursor_pop_subtree(cursor);\r\n")); + treefields.append(QString(" }\r\n")); + } } outCode.replace(QString("$(POPULATETREE)"), treefields); // Replace the $(HEADERFIELDS) tag QString headerfields; - headerfields.append( QString(" static hf_register_info hf[] = {\r\n") ); + headerfields.append(QString(" static hf_register_info hf[] = {\r\n")); for (int n = 0; n < info->fields.length(); ++n) { - // For non-array fields - if ( info->fields[n]->numElements == 1) { - headerfields.append( QString("\t { &hf_op_uavobjects_%1_%2,\r\n") - .arg( info->namelc ) - .arg( info->fields[n]->name ) ); - headerfields.append( QString("\t { \"%1\", \"%2.%3\", %4,\r\n") - .arg( info->fields[n]->name ) - .arg( info->namelc ) - .arg( info->fields[n]->name ) - .arg( fieldTypeStrHf[info->fields[n]->type] ) ); - if ( info->fields[n]->type == FIELDTYPE_ENUM ) { - headerfields.append( QString("\t BASE_DEC, VALS(uavobjects_%1_%2), 0x0, NULL, HFILL \r\n") - .arg( info->namelc ) - .arg( info->fields[n]->name ) ); - } else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) { - headerfields.append( QString("\t BASE_NONE, NULL, 0x0, NULL, HFILL \r\n") ); - } else { - headerfields.append( QString("\t BASE_DEC_HEX, NULL, 0x0, NULL, HFILL\r\n") ); - } - headerfields.append( QString("\t },\r\n") ); - headerfields.append( QString("\t },\r\n") ); - } else { - headerfields.append( QString("\t { &hf_op_uavobjects_%1_%2,\r\n") - .arg( info->namelc ) - .arg( info->fields[n]->name ) ); - headerfields.append( QString("\t { \"%1\", \"%2.%3\", FT_NONE,\r\n") - .arg( info->fields[n]->name ) - .arg( info->namelc ) - .arg( info->fields[n]->name ) ); - headerfields.append( QString("\t BASE_NONE, NULL, 0x0, NULL, HFILL\r\n") ); - headerfields.append( QString("\t },\r\n") ); - headerfields.append( QString("\t },\r\n") ); + // For non-array fields + if (info->fields[n]->numElements == 1) { + headerfields.append(QString("\t { &hf_op_uavobjects_%1_%2,\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + headerfields.append(QString("\t { \"%1\", \"%2.%3\", %4,\r\n") + .arg(info->fields[n]->name) + .arg(info->namelc) + .arg(info->fields[n]->name) + .arg(fieldTypeStrHf[info->fields[n]->type])); + if (info->fields[n]->type == FIELDTYPE_ENUM) { + headerfields.append(QString("\t BASE_DEC, VALS(uavobjects_%1_%2), 0x0, NULL, HFILL \r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + headerfields.append(QString("\t BASE_NONE, NULL, 0x0, NULL, HFILL \r\n")); + } else { + headerfields.append(QString("\t BASE_DEC_HEX, NULL, 0x0, NULL, HFILL\r\n")); + } + headerfields.append(QString("\t },\r\n")); + headerfields.append(QString("\t },\r\n")); + } else { + headerfields.append(QString("\t { &hf_op_uavobjects_%1_%2,\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + headerfields.append(QString("\t { \"%1\", \"%2.%3\", FT_NONE,\r\n") + .arg(info->fields[n]->name) + .arg(info->namelc) + .arg(info->fields[n]->name)); + headerfields.append(QString("\t BASE_NONE, NULL, 0x0, NULL, HFILL\r\n")); + headerfields.append(QString("\t },\r\n")); + headerfields.append(QString("\t },\r\n")); - QStringList elemNames = info->fields[n]->elementNames; - for (int m = 0; m < elemNames.length(); ++m) { - headerfields.append( QString("\t { &hf_op_uavobjects_%1_%2_%3,\r\n") - .arg( info->namelc ) - .arg( info->fields[n]->name ) - .arg( elemNames[m]) ); - headerfields.append( QString("\t { \"%1\", \"%2.%3.%4\", %5,\r\n") - .arg( elemNames[m] ) - .arg( info->namelc ) - .arg( info->fields[n]->name ) - .arg( elemNames[m] ) - .arg( fieldTypeStrHf[info->fields[n]->type] ) ); - if ( info->fields[n]->type == FIELDTYPE_ENUM ) { - headerfields.append( QString("\t BASE_DEC, VALS(uavobjects_%1_%2), 0x0, NULL, HFILL \r\n") - .arg( info->namelc ) - .arg( info->fields[n]->name ) ); - } else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) { - headerfields.append( QString("\t BASE_NONE, NULL, 0x0, NULL, HFILL \r\n") ); - } else { - headerfields.append( QString("\t BASE_DEC_HEX, NULL, 0x0, NULL, HFILL\r\n") ); - } - headerfields.append( QString("\t },\r\n") ); - headerfields.append( QString("\t },\r\n") ); - } - } + QStringList elemNames = info->fields[n]->elementNames; + for (int m = 0; m < elemNames.length(); ++m) { + headerfields.append(QString("\t { &hf_op_uavobjects_%1_%2_%3,\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name) + .arg(elemNames[m])); + headerfields.append(QString("\t { \"%1\", \"%2.%3.%4\", %5,\r\n") + .arg(elemNames[m]) + .arg(info->namelc) + .arg(info->fields[n]->name) + .arg(elemNames[m]) + .arg(fieldTypeStrHf[info->fields[n]->type])); + if (info->fields[n]->type == FIELDTYPE_ENUM) { + headerfields.append(QString("\t BASE_DEC, VALS(uavobjects_%1_%2), 0x0, NULL, HFILL \r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + headerfields.append(QString("\t BASE_NONE, NULL, 0x0, NULL, HFILL \r\n")); + } else { + headerfields.append(QString("\t BASE_DEC_HEX, NULL, 0x0, NULL, HFILL\r\n")); + } + headerfields.append(QString("\t },\r\n")); + headerfields.append(QString("\t },\r\n")); + } + } } - headerfields.append( QString(" };\r\n") ); + headerfields.append(QString(" };\r\n")); outCode.replace(QString("$(HEADERFIELDS)"), headerfields); // Write the flight code - bool res = writeFileIfDiffrent( outputpath.absolutePath() + "/packet-op-uavobjects-" + info->namelc + ".c", outCode ); + bool res = writeFileIfDiffrent(outputpath.absolutePath() + "/packet-op-uavobjects-" + info->namelc + ".c", outCode); if (!res) { cout << "Error: Could not write wireshark code files" << endl; return false; @@ -286,5 +287,3 @@ bool UAVObjectGeneratorWireshark::process_object(ObjectInfo* info, QDir outputpa return true; } - - diff --git a/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.h b/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.h index 402aac17e..cd87dca80 100644 --- a/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.h +++ b/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.h @@ -29,10 +29,9 @@ #include "../generator_common.h" -class UAVObjectGeneratorWireshark -{ +class UAVObjectGeneratorWireshark { public: - bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); + bool generate(UAVObjectParser *gen, QString templatepath, QString outputpath); QStringList fieldTypeStrHf; QStringList fieldTypeStrGlib; QString wiresharkCodeTemplate, wiresharkMakeTemplate; @@ -40,9 +39,7 @@ public: QDir wiresharkOutputPath; private: - bool process_object(ObjectInfo* info, QDir outputpath); - + bool process_object(ObjectInfo *info, QDir outputpath); }; #endif - diff --git a/ground/uavobjgenerator/main.cpp b/ground/uavobjgenerator/main.cpp index 8bce4fd09..55de04f3d 100644 --- a/ground/uavobjgenerator/main.cpp +++ b/ground/uavobjgenerator/main.cpp @@ -37,17 +37,18 @@ #include "generators/wireshark/uavobjectgeneratorwireshark.h" #define RETURN_ERR_USAGE 1 -#define RETURN_ERR_XML 2 -#define RETURN_OK 0 +#define RETURN_ERR_XML 2 +#define RETURN_OK 0 using namespace std; /** * print usage info */ -void usage() { +void usage() +{ cout << "Usage: uavobjectgenerator [-gcs] [-flight] [-java] [-python] [-matlab] [-wireshark] [-none] [-v] xml_path template_base [UAVObj1] ... [UAVObjN]" << endl; - cout << "Languages: "<< endl; + cout << "Languages: " << endl; cout << "\t-gcs build groundstation code" << endl; cout << "\t-flight build flight code" << endl; cout << "\t-java build java code" << endl; @@ -55,7 +56,7 @@ void usage() { cout << "\t-matlab build matlab code" << endl; cout << "\t-wireshark build wireshark plugin" << endl; cout << "\tIf no language is specified ( and not -none ) -> all are built." << endl; - cout << "Misc: "<< endl; + cout << "Misc: " << endl; cout << "\t-none build no language - just parse xml's" << endl; cout << "\t-h this help" << endl; cout << "\t-v verbose" << endl; @@ -69,7 +70,8 @@ void usage() { /** * inform user of invalid usage */ -int usage_err() { +int usage_err() +{ cout << "Invalid usage!" << endl; usage(); return RETURN_ERR_USAGE; @@ -91,78 +93,81 @@ int main(int argc, char *argv[]) QStringList objects_stringlist; // process arguments - for (int argi=1;argi0)||(arguments_stringlist.removeAll("-h")>0)) { - usage(); - return RETURN_OK; } - bool verbose=(arguments_stringlist.removeAll("-v")>0); - bool do_gcs=(arguments_stringlist.removeAll("-gcs")>0); - bool do_flight=(arguments_stringlist.removeAll("-flight")>0); - bool do_java=(arguments_stringlist.removeAll("-java")>0); - bool do_python=(arguments_stringlist.removeAll("-python")>0); - bool do_matlab=(arguments_stringlist.removeAll("-matlab")>0); - bool do_wireshark=(arguments_stringlist.removeAll("-wireshark")>0); - bool do_none=(arguments_stringlist.removeAll("-none")>0); // + if ((arguments_stringlist.removeAll("-h") > 0) || (arguments_stringlist.removeAll("-h") > 0)) { + usage(); + return RETURN_OK; + } - bool do_all=((do_gcs||do_flight||do_java||do_python||do_matlab)==false); - bool do_allObjects=true; + bool verbose = (arguments_stringlist.removeAll("-v") > 0); + bool do_gcs = (arguments_stringlist.removeAll("-gcs") > 0); + bool do_flight = (arguments_stringlist.removeAll("-flight") > 0); + bool do_java = (arguments_stringlist.removeAll("-java") > 0); + bool do_python = (arguments_stringlist.removeAll("-python") > 0); + bool do_matlab = (arguments_stringlist.removeAll("-matlab") > 0); + bool do_wireshark = (arguments_stringlist.removeAll("-wireshark") > 0); + bool do_none = (arguments_stringlist.removeAll("-none") > 0); // + + bool do_all = ((do_gcs || do_flight || do_java || do_python || do_matlab) == false); + bool do_allObjects = true; if (arguments_stringlist.length() >= 2) { - inputpath = arguments_stringlist.at(0); + inputpath = arguments_stringlist.at(0); templatepath = arguments_stringlist.at(1); } else { // wrong number of arguments return usage_err(); } - if (arguments_stringlist.length() >2) { - do_allObjects=false; - for (int argi=2;argi 2) { + do_allObjects = false; + for (int argi = 2; argi < arguments_stringlist.length(); argi++) { + objects_stringlist << (arguments_stringlist.at(argi).toLower() + ".xml"); } } - if (!inputpath.endsWith("/")) + if (!inputpath.endsWith("/")) { inputpath.append("/"); // append a slash if it is not there - - if (!templatepath.endsWith("/")) + } + if (!templatepath.endsWith("/")) { templatepath.append("/"); // append a slash if it is not there - + } // put all output files in the current directory outputpath = QString("./"); QDir xmlPath = QDir(inputpath); - UAVObjectParser* parser = new UAVObjectParser(); + UAVObjectParser *parser = new UAVObjectParser(); - QStringList filters=QStringList("*.xml"); + QStringList filters = QStringList("*.xml"); xmlPath.setNameFilters(filters); - QFileInfoList xmlList = xmlPath.entryInfoList(); + QFileInfoList xmlList = xmlPath.entryInfoList(); // Read in each XML file and parse object(s) in them - + for (int n = 0; n < xmlList.length(); ++n) { QFileInfo fileinfo = xmlList[n]; if (!do_allObjects) { if (!objects_stringlist.removeAll(fileinfo.fileName().toLower())) { - if (verbose) - cout << "Skipping XML file: " << fileinfo.fileName().toStdString() << endl; - continue; + if (verbose) { + cout << "Skipping XML file: " << fileinfo.fileName().toStdString() << endl; + } + continue; } } - if (verbose) - cout << "Parsing XML file: " << fileinfo.fileName().toStdString() << endl; + if (verbose) { + cout << "Parsing XML file: " << fileinfo.fileName().toStdString() << endl; + } QString filename = fileinfo.fileName(); - QString xmlstr = readFile(fileinfo.absoluteFilePath()); + QString xmlstr = readFile(fileinfo.absoluteFilePath()); QString res = parser->parseXML(xmlstr, filename); if (!res.isNull()) { - if (!verbose) { - cout << "Error in XML file: " << fileinfo.fileName().toStdString() << endl; + if (!verbose) { + cout << "Error in XML file: " << fileinfo.fileName().toStdString() << endl; } cout << "Error parsing " << res.toStdString() << endl; return RETURN_ERR_XML; @@ -176,13 +181,14 @@ int main(int argc, char *argv[]) // check for duplicate object ID's QList objIDList; - int numBytesTotal=0; + int numBytesTotal = 0; for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { quint32 id = parser->getObjectID(objidx); - numBytesTotal+=parser->getNumBytes(objidx); - if (verbose) - cout << "Checking object " << parser->getObjectName(objidx).toStdString() << " (" << parser->getNumBytes(objidx) << " bytes)" << endl; - if ( objIDList.contains(id) || id == 0 ) { + numBytesTotal += parser->getNumBytes(objidx); + if (verbose) { + cout << "Checking object " << parser->getObjectName(objidx).toStdString() << " (" << parser->getNumBytes(objidx) << " bytes)" << endl; + } + if (objIDList.contains(id) || id == 0) { cout << "Error: Object ID collision found in object " << parser->getObjectName(objidx).toStdString() << ", modify object name" << endl; return RETURN_ERR_XML; } @@ -193,56 +199,57 @@ int main(int argc, char *argv[]) // done parsing and checking cout << "Done: processed " << xmlList.length() << " XML files and generated " << objIDList.length() << " objects with no ID collisions. Total size of the data fields is " << numBytesTotal << " bytes." << endl; - - if (verbose) + + if (verbose) { cout << "used units: " << parser->all_units.join(",").toStdString() << endl; + } - if (do_none) - return RETURN_OK; + if (do_none) { + return RETURN_OK; + } // generate flight code if wanted - if (do_flight|do_all) { - cout << "generating flight code" << endl ; + if (do_flight | do_all) { + cout << "generating flight code" << endl; UAVObjectGeneratorFlight flightgen; - flightgen.generate(parser,templatepath,outputpath); + flightgen.generate(parser, templatepath, outputpath); } // generate gcs code if wanted - if (do_gcs|do_all) { - cout << "generating gcs code" << endl ; + if (do_gcs | do_all) { + cout << "generating gcs code" << endl; UAVObjectGeneratorGCS gcsgen; - gcsgen.generate(parser,templatepath,outputpath); + gcsgen.generate(parser, templatepath, outputpath); } // generate java code if wanted - if (do_java|do_all) { - cout << "generating java code" << endl ; + if (do_java | do_all) { + cout << "generating java code" << endl; UAVObjectGeneratorJava javagen; - javagen.generate(parser,templatepath,outputpath); + javagen.generate(parser, templatepath, outputpath); } // generate python code if wanted - if (do_python|do_all) { - cout << "generating python code" << endl ; + if (do_python | do_all) { + cout << "generating python code" << endl; UAVObjectGeneratorPython pygen; - pygen.generate(parser,templatepath,outputpath); + pygen.generate(parser, templatepath, outputpath); } // generate matlab code if wanted - if (do_matlab|do_all) { - cout << "generating matlab code" << endl ; + if (do_matlab | do_all) { + cout << "generating matlab code" << endl; UAVObjectGeneratorMatlab matlabgen; - matlabgen.generate(parser,templatepath,outputpath); + matlabgen.generate(parser, templatepath, outputpath); } // generate wireshark plugin if wanted - if (do_wireshark|do_all) { - cout << "generating wireshark code" << endl ; + if (do_wireshark | do_all) { + cout << "generating wireshark code" << endl; UAVObjectGeneratorWireshark wiresharkgen; - wiresharkgen.generate(parser,templatepath,outputpath); + wiresharkgen.generate(parser, templatepath, outputpath); } return RETURN_OK; } - diff --git a/ground/uavobjgenerator/uavobjectparser.cpp b/ground/uavobjgenerator/uavobjectparser.cpp index 9bf26f014..118e35029 100644 --- a/ground/uavobjgenerator/uavobjectparser.cpp +++ b/ground/uavobjgenerator/uavobjectparser.cpp @@ -32,18 +32,17 @@ UAVObjectParser::UAVObjectParser() { fieldTypeStrXML << "int8" << "int16" << "int32" << "uint8" - << "uint16" << "uint32" <<"float" << "enum"; + << "uint16" << "uint32" << "float" << "enum"; updateModeStrXML << "manual" << "periodic" << "onchange" << "throttled"; accessModeStr << "ACCESS_READWRITE" << "ACCESS_READONLY"; fieldTypeNumBytes << int(1) << int(2) << int(4) << - int(1) << int(2) << int(4) << - int(4) << int(1); + int(1) << int(2) << int(4) << + int(4) << int(1); accessModeStrXML << "readwrite" << "readonly"; - } /** @@ -57,12 +56,12 @@ int UAVObjectParser::getNumObjects() /** * Get the detailed object information */ -QList UAVObjectParser::getObjectInfo() +QList UAVObjectParser::getObjectInfo() { return objInfo; } -ObjectInfo* UAVObjectParser::getObjectByIndex(int objIndex) +ObjectInfo *UAVObjectParser::getObjectByIndex(int objIndex) { return objInfo[objIndex]; } @@ -72,9 +71,11 @@ ObjectInfo* UAVObjectParser::getObjectByIndex(int objIndex) */ QString UAVObjectParser::getObjectName(int objIndex) { - ObjectInfo* info = objInfo[objIndex]; - if (info == NULL) + ObjectInfo *info = objInfo[objIndex]; + + if (info == NULL) { return QString(); + } return info->name; } @@ -84,9 +85,11 @@ QString UAVObjectParser::getObjectName(int objIndex) */ quint32 UAVObjectParser::getObjectID(int objIndex) { - ObjectInfo* info = objInfo[objIndex]; - if (info == NULL) + ObjectInfo *info = objInfo[objIndex]; + + if (info == NULL) { return 0; + } return info->id; } @@ -95,23 +98,20 @@ quint32 UAVObjectParser::getObjectID(int objIndex) */ int UAVObjectParser::getNumBytes(int objIndex) { - ObjectInfo* info = objInfo[objIndex]; - if (info == NULL) - { + ObjectInfo *info = objInfo[objIndex]; + + if (info == NULL) { return 0; - } - else - { + } else { int numBytes = 0; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { numBytes += info->fields[n]->numBytes * info->fields[n]->numElements; } return numBytes; } } -bool fieldTypeLessThan(const FieldInfo* f1, const FieldInfo* f2) +bool fieldTypeLessThan(const FieldInfo *f1, const FieldInfo *f2) { return f1->numBytes > f2->numBytes; } @@ -122,115 +122,125 @@ bool fieldTypeLessThan(const FieldInfo* f1, const FieldInfo* f2) * @param filename The xml filename * @returns Null QString() on success, error message on failure */ -QString UAVObjectParser::parseXML(QString& xml, QString& filename) +QString UAVObjectParser::parseXML(QString & xml, QString & filename) { // Create DOM document and parse it QDomDocument doc("UAVObjects"); bool parsed = doc.setContent(xml); - if (!parsed) return QString("Improperly formated XML file"); + + if (!parsed) { + return QString("Improperly formated XML file"); + } // Read all objects contained in the XML file, creating an new ObjectInfo for each QDomElement docElement = doc.documentElement(); QDomNode node = docElement.firstChild(); - while ( !node.isNull() ) { + while (!node.isNull()) { // Create new object entry - ObjectInfo* info = new ObjectInfo; + ObjectInfo *info = new ObjectInfo; - info->filename=filename; + info->filename = filename; // Process object attributes QString status = processObjectAttributes(node, info); - if (!status.isNull()) + if (!status.isNull()) { return status; + } // Process child elements (fields and metadata) - QDomNode childNode = node.firstChild(); - bool fieldFound = false; - bool accessFound = false; - bool telGCSFound = false; - bool telFlightFound = false; + QDomNode childNode = node.firstChild(); + bool fieldFound = false; + bool accessFound = false; + bool telGCSFound = false; + bool telFlightFound = false; bool logFound = false; bool descriptionFound = false; - while ( !childNode.isNull() ) { + while (!childNode.isNull()) { // Process element depending on its type - if ( childNode.nodeName().compare(QString("field")) == 0 ) { + if (childNode.nodeName().compare(QString("field")) == 0) { QString status = processObjectFields(childNode, info); - if (!status.isNull()) + if (!status.isNull()) { return status; + } fieldFound = true; - } - else if ( childNode.nodeName().compare(QString("access")) == 0 ) { + } else if (childNode.nodeName().compare(QString("access")) == 0) { QString status = processObjectAccess(childNode, info); - if (!status.isNull()) + if (!status.isNull()) { return status; + } accessFound = true; - } - else if ( childNode.nodeName().compare(QString("telemetrygcs")) == 0 ) { + } else if (childNode.nodeName().compare(QString("telemetrygcs")) == 0) { QString status = processObjectMetadata(childNode, &info->gcsTelemetryUpdateMode, &info->gcsTelemetryUpdatePeriod, &info->gcsTelemetryAcked); - if (!status.isNull()) + if (!status.isNull()) { return status; + } telGCSFound = true; - } - else if ( childNode.nodeName().compare(QString("telemetryflight")) == 0 ) { + } else if (childNode.nodeName().compare(QString("telemetryflight")) == 0) { QString status = processObjectMetadata(childNode, &info->flightTelemetryUpdateMode, &info->flightTelemetryUpdatePeriod, &info->flightTelemetryAcked); - if (!status.isNull()) + if (!status.isNull()) { return status; + } telFlightFound = true; - } - else if ( childNode.nodeName().compare(QString("logging")) == 0 ) { + } else if (childNode.nodeName().compare(QString("logging")) == 0) { QString status = processObjectMetadata(childNode, &info->loggingUpdateMode, &info->loggingUpdatePeriod, NULL); - if (!status.isNull()) + if (!status.isNull()) { return status; + } logFound = true; - } - else if ( childNode.nodeName().compare(QString("description")) == 0 ) { + } else if (childNode.nodeName().compare(QString("description")) == 0) { QString status = processObjectDescription(childNode, &info->description); - if (!status.isNull()) + if (!status.isNull()) { return status; + } descriptionFound = true; - } - else if (!childNode.isComment()) { + } else if (!childNode.isComment()) { return QString("Unknown object element"); } // Get next element childNode = childNode.nextSibling(); } - - // Sort all fields according to size + + // Sort all fields according to size qStableSort(info->fields.begin(), info->fields.end(), fieldTypeLessThan); // Sort all fields according to size qStableSort(info->fields.begin(), info->fields.end(), fieldTypeLessThan); // Make sure that required elements were found - if ( !fieldFound ) + if (!fieldFound) { return QString("Object::field element is missing"); + } - if ( !accessFound ) + if (!accessFound) { return QString("Object::access element is missing"); + } - if ( !telGCSFound ) + if (!telGCSFound) { return QString("Object::telemetrygcs element is missing"); + } - if ( !telFlightFound ) + if (!telFlightFound) { return QString("Object::telemetryflight element is missing"); + } - if ( !logFound ) + if (!logFound) { return QString("Object::logging element is missing"); + } // TODO: Make into error once all objects updated - if ( !descriptionFound ) + if (!descriptionFound) { return QString("Object::description element is missing"); + } // Calculate ID calculateID(info); @@ -253,10 +263,11 @@ QString UAVObjectParser::parseXML(QString& xml, QString& filename) * and is used to avoid connecting objects with incompatible configurations. * The LSB is set to zero and is reserved for metadata */ -void UAVObjectParser::calculateID(ObjectInfo* info) +void UAVObjectParser::calculateID(ObjectInfo *info) { // Hash object name quint32 hash = updateHash(info->name, 0); + // Hash object attributes hash = updateHash(info->isSettings, hash); hash = updateHash(info->isSingleInst, hash); @@ -265,10 +276,11 @@ void UAVObjectParser::calculateID(ObjectInfo* info) hash = updateHash(info->fields[n]->name, hash); hash = updateHash(info->fields[n]->numElements, hash); hash = updateHash(info->fields[n]->type, hash); - if(info->fields[n]->type == FIELDTYPE_ENUM) { + if (info->fields[n]->type == FIELDTYPE_ENUM) { QStringList options = info->fields[n]->options; - for (int m = 0; m < options.length(); m++) + for (int m = 0; m < options.length(); m++) { hash = updateHash(options[m], hash); + } } } // Done @@ -283,18 +295,20 @@ void UAVObjectParser::calculateID(ObjectInfo* info) */ quint32 UAVObjectParser::updateHash(quint32 value, quint32 hash) { - return (hash ^ ((hash<<5) + (hash>>2) + value)); + return hash ^ ((hash << 5) + (hash >> 2) + value); } /** * Update the hash given a string */ -quint32 UAVObjectParser::updateHash(QString& value, quint32 hash) +quint32 UAVObjectParser::updateHash(QString & value, quint32 hash) { QByteArray bytes = value.toAscii(); - quint32 hashout = hash; - for (int n = 0; n < bytes.length(); ++n) + quint32 hashout = hash; + + for (int n = 0; n < bytes.length(); ++n) { hashout = updateHash(bytes[n], hashout); + } return hashout; } @@ -302,41 +316,47 @@ quint32 UAVObjectParser::updateHash(QString& value, quint32 hash) /** * Process the metadata part of the XML */ -QString UAVObjectParser::processObjectMetadata(QDomNode& childNode, UpdateMode* mode, int* period, bool* acked) +QString UAVObjectParser::processObjectMetadata(QDomNode & childNode, UpdateMode *mode, int *period, bool *acked) { // Get updatemode attribute QDomNamedNodeMap elemAttributes = childNode.attributes(); QDomNode elemAttr = elemAttributes.namedItem("updatemode"); - if ( elemAttr.isNull() ) + + if (elemAttr.isNull()) { return QString("Object:telemetrygcs:updatemode attribute is missing"); + } - int index = updateModeStrXML.indexOf( elemAttr.nodeValue() ); + int index = updateModeStrXML.indexOf(elemAttr.nodeValue()); - if (index<0) + if (index < 0) { return QString("Object:telemetrygcs:updatemode attribute value is invalid"); + } - *mode = (UpdateMode)index; + *mode = (UpdateMode)index; // Get period attribute elemAttr = elemAttributes.namedItem("period"); - if ( elemAttr.isNull() ) + if (elemAttr.isNull()) { return QString("Object:telemetrygcs:period attribute is missing"); + } *period = elemAttr.nodeValue().toInt(); // Get acked attribute (only if acked parameter is not null, not applicable for logging metadata) - if ( acked != NULL) { + if (acked != NULL) { elemAttr = elemAttributes.namedItem("acked"); - if ( elemAttr.isNull()) + if (elemAttr.isNull()) { return QString("Object:telemetrygcs:acked attribute is missing"); + } - if ( elemAttr.nodeValue().compare(QString("true")) == 0 ) + if (elemAttr.nodeValue().compare(QString("true")) == 0) { *acked = true; - else if ( elemAttr.nodeValue().compare(QString("false")) == 0 ) + } else if (elemAttr.nodeValue().compare(QString("false")) == 0) { *acked = false; - else + } else { return QString("Object:telemetrygcs:acked attribute value is invalid"); + } } // Done return QString(); @@ -345,30 +365,35 @@ QString UAVObjectParser::processObjectMetadata(QDomNode& childNode, UpdateMode* /** * Process the object access tag of the XML */ -QString UAVObjectParser::processObjectAccess(QDomNode& childNode, ObjectInfo* info) +QString UAVObjectParser::processObjectAccess(QDomNode & childNode, ObjectInfo *info) { // Get gcs attribute QDomNamedNodeMap elemAttributes = childNode.attributes(); QDomNode elemAttr = elemAttributes.namedItem("gcs"); - if ( elemAttr.isNull() ) - return QString("Object:access:gcs attribute is missing"); - int index = accessModeStrXML.indexOf( elemAttr.nodeValue() ); - if (index >= 0) + if (elemAttr.isNull()) { + return QString("Object:access:gcs attribute is missing"); + } + + int index = accessModeStrXML.indexOf(elemAttr.nodeValue()); + if (index >= 0) { info->gcsAccess = (AccessMode)index; - else + } else { return QString("Object:access:gcs attribute value is invalid"); + } // Get flight attribute elemAttr = elemAttributes.namedItem("flight"); - if ( elemAttr.isNull() ) + if (elemAttr.isNull()) { return QString("Object:access:flight attribute is missing"); + } - index = accessModeStrXML.indexOf( elemAttr.nodeValue() ); - if (index >= 0) + index = accessModeStrXML.indexOf(elemAttr.nodeValue()); + if (index >= 0) { info->flightAccess = (AccessMode)index; - else + } else { return QString("Object:access:flight attribute value is invalid"); + } // Done return QString(); @@ -377,13 +402,14 @@ QString UAVObjectParser::processObjectAccess(QDomNode& childNode, ObjectInfo* in /** * Process the object fields of the XML */ -QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* info) +QString UAVObjectParser::processObjectFields(QDomNode & childNode, ObjectInfo *info) { // Create field - FieldInfo* field = new FieldInfo; + FieldInfo *field = new FieldInfo; // Get name attribute QDomNamedNodeMap elemAttributes = childNode.attributes(); QDomNode elemAttr = elemAttributes.namedItem("name"); + if (elemAttr.isNull()) { return QString("Object:field:name attribute is missing"); } @@ -393,12 +419,12 @@ QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* in // field that has already been declared elemAttr = elemAttributes.namedItem("cloneof"); if (!elemAttr.isNull()) { - QString parentName = elemAttr.nodeValue(); + QString parentName = elemAttr.nodeValue(); if (!parentName.isEmpty()) { - foreach(FieldInfo * parent, info->fields) { + foreach(FieldInfo * parent, info->fields) { if (parent->name == parentName) { // clone from this parent - *field = *parent; // safe shallow copy, no ptrs in struct + *field = *parent; // safe shallow copy, no ptrs in struct field->name = name; // set our name // Add field to object info->fields.append(field); @@ -407,35 +433,34 @@ QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* in } } return QString("Object:field::cloneof parent unknown"); - } - else { + } else { return QString("Object:field:cloneof attribute is empty"); } - } - else { + } else { // this field is not a clone, so remember its name field->name = name; } // Get units attribute elemAttr = elemAttributes.namedItem("units"); - if ( elemAttr.isNull() ) + if (elemAttr.isNull()) { return QString("Object:field:units attribute is missing"); + } field->units = elemAttr.nodeValue(); all_units << field->units; // Get type attribute elemAttr = elemAttributes.namedItem("type"); - if ( elemAttr.isNull() ) + if (elemAttr.isNull()) { return QString("Object:field:type attribute is missing"); + } int index = fieldTypeStrXML.indexOf(elemAttr.nodeValue()); if (index >= 0) { - field->type = (FieldType)index; + field->type = (FieldType)index; field->numBytes = fieldTypeNumBytes[index]; - } - else { + } else { return QString("Object:field:type attribute value is invalid"); } @@ -443,28 +468,28 @@ QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* in field->numElements = 0; // Look for element names as an attribute first elemAttr = elemAttributes.namedItem("elementnames"); - if ( !elemAttr.isNull() ) { + if (!elemAttr.isNull()) { // Get element names QStringList names = elemAttr.nodeValue().split(",", QString::SkipEmptyParts); - for (int n = 0; n < names.length(); ++n) + for (int n = 0; n < names.length(); ++n) { names[n] = names[n].trimmed(); + } field->elementNames = names; - field->numElements = names.length(); + field->numElements = names.length(); field->defaultElementNames = false; - } - else { + } else { // Look for a list of child elementname nodes QDomNode listNode = childNode.firstChildElement("elementnames"); if (!listNode.isNull()) { for (QDomElement node = listNode.firstChildElement("elementname"); !node.isNull(); node = node.nextSiblingElement("elementname")) { - QDomNode name = node.firstChild(); + QDomNode name = node.firstChild(); if (!name.isNull() && name.isText() && !name.nodeValue().isEmpty()) { field->elementNames.append(name.nodeValue()); } } - field->numElements = field->elementNames.length(); + field->numElements = field->elementNames.length(); field->defaultElementNames = false; } } @@ -472,20 +497,19 @@ QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* in // for the number of elements in the 'elements' attribute if (field->numElements == 0) { elemAttr = elemAttributes.namedItem("elements"); - if ( elemAttr.isNull() ) { + if (elemAttr.isNull()) { return QString("Object:field:elements and Object:field:elementnames attribute/element is missing"); - } - else { + } else { field->numElements = elemAttr.nodeValue().toInt(); - for (int n = 0; n < field->numElements; ++n) + for (int n = 0; n < field->numElements; ++n) { field->elementNames.append(QString("%1").arg(n)); + } field->defaultElementNames = true; } } // Get options attribute or child elements (only if an enum type) if (field->type == FIELDTYPE_ENUM) { - // Look for options attribute elemAttr = elemAttributes.namedItem("options"); if (!elemAttr.isNull()) { @@ -494,19 +518,18 @@ QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* in options[n] = options[n].trimmed(); } field->options = options; - } - else { - // Look for a list of child 'option' nodes - QDomNode listNode = childNode.firstChildElement("options"); - if (!listNode.isNull()) { - for (QDomElement node = listNode.firstChildElement("option"); - !node.isNull(); node = node.nextSiblingElement("option")) { - QDomNode name = node.firstChild(); + } else { + // Look for a list of child 'option' nodes + QDomNode listNode = childNode.firstChildElement("options"); + if (!listNode.isNull()) { + for (QDomElement node = listNode.firstChildElement("option"); + !node.isNull(); node = node.nextSiblingElement("option")) { + QDomNode name = node.firstChild(); if (!name.isNull() && name.isText() && !name.nodeValue().isEmpty()) { - field->options.append(name.nodeValue()); - } - } - } + field->options.append(name.nodeValue()); + } + } + } } if (field->options.length() == 0) { return QString("Object:field:options attribute/element is missing"); @@ -515,35 +538,37 @@ QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* in // Get the default value attribute (required for settings objects, optional for the rest) elemAttr = elemAttributes.namedItem("defaultvalue"); - if ( elemAttr.isNull() ) { - if ( info->isSettings ) + if (elemAttr.isNull()) { + if (info->isSettings) { return QString("Object:field:defaultvalue attribute is missing (required for settings objects)"); + } field->defaultValues = QStringList(); - } - else { - QStringList defaults = elemAttr.nodeValue().split(",", QString::SkipEmptyParts); - for (int n = 0; n < defaults.length(); ++n) - defaults[n] = defaults[n].trimmed(); + } else { + QStringList defaults = elemAttr.nodeValue().split(",", QString::SkipEmptyParts); + for (int n = 0; n < defaults.length(); ++n) { + defaults[n] = defaults[n].trimmed(); + } - if(defaults.length() != field->numElements) { - if(defaults.length() != 1) - return QString("Object:field:incorrect number of default values"); + if (defaults.length() != field->numElements) { + if (defaults.length() != 1) { + return QString("Object:field:incorrect number of default values"); + } - /*support legacy single default for multiple elements - We should really issue a warning*/ - for(int ct=1; ct< field->numElements; ct++) - defaults.append(defaults[0]); - } - field->defaultValues = defaults; + /*support legacy single default for multiple elements + We should really issue a warning*/ + for (int ct = 1; ct < field->numElements; ct++) { + defaults.append(defaults[0]); + } + } + field->defaultValues = defaults; } // Limits attribute elemAttr = elemAttributes.namedItem("limits"); - if ( elemAttr.isNull() ) { - field->limitValues=QString(); - } - else{ - field->limitValues=elemAttr.nodeValue(); + if (elemAttr.isNull()) { + field->limitValues = QString(); + } else { + field->limitValues = elemAttr.nodeValue(); } // Add field to object info->fields.append(field); @@ -554,52 +579,58 @@ QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* in /** * Process the object attributes from the XML */ -QString UAVObjectParser::processObjectAttributes(QDomNode& node, ObjectInfo* info) +QString UAVObjectParser::processObjectAttributes(QDomNode & node, ObjectInfo *info) { // Get name attribute QDomNamedNodeMap attributes = node.attributes(); QDomNode attr = attributes.namedItem("name"); - if ( attr.isNull() ) - return QString("Object:name attribute is missing"); - info->name = attr.nodeValue(); + if (attr.isNull()) { + return QString("Object:name attribute is missing"); + } + + info->name = attr.nodeValue(); info->namelc = attr.nodeValue().toLower(); // Get category attribute if present attr = attributes.namedItem("category"); - if ( !attr.isNull() ) - { + if (!attr.isNull()) { info->category = attr.nodeValue(); } // Get singleinstance attribute attr = attributes.namedItem("singleinstance"); - if ( attr.isNull() ) + if (attr.isNull()) { return QString("Object:singleinstance attribute is missing"); + } - if ( attr.nodeValue().compare(QString("true")) == 0 ) + if (attr.nodeValue().compare(QString("true")) == 0) { info->isSingleInst = true; - else if ( attr.nodeValue().compare(QString("false")) == 0 ) + } else if (attr.nodeValue().compare(QString("false")) == 0) { info->isSingleInst = false; - else + } else { return QString("Object:singleinstance attribute value is invalid"); + } // Get settings attribute attr = attributes.namedItem("settings"); - if ( attr.isNull() ) + if (attr.isNull()) { return QString("Object:settings attribute is missing"); + } - if ( attr.nodeValue().compare(QString("true")) == 0 ) - info->isSettings = true; - else if ( attr.nodeValue().compare(QString("false")) == 0 ) + if (attr.nodeValue().compare(QString("true")) == 0) { + info->isSettings = true; + } else if (attr.nodeValue().compare(QString("false")) == 0) { info->isSettings = false; - else + } else { return QString("Object:settings attribute value is invalid"); + } // Settings objects can only have a single instance - if ( info->isSettings && !info->isSingleInst ) + if (info->isSettings && !info->isSingleInst) { return QString("Object: Settings objects can not have multiple instances"); + } // Done return QString(); @@ -608,7 +639,7 @@ QString UAVObjectParser::processObjectAttributes(QDomNode& node, ObjectInfo* inf /** * Process the description field from the XML file */ -QString UAVObjectParser::processObjectDescription(QDomNode& childNode, QString * description) +QString UAVObjectParser::processObjectDescription(QDomNode & childNode, QString *description) { description->append(childNode.firstChild().nodeValue()); return QString(); diff --git a/ground/uavobjgenerator/uavobjectparser.h b/ground/uavobjgenerator/uavobjectparser.h index bab24142e..d6ae4b60c 100644 --- a/ground/uavobjgenerator/uavobjectparser.h +++ b/ground/uavobjgenerator/uavobjectparser.h @@ -48,75 +48,74 @@ typedef enum { } FieldType; typedef struct { - QString name; - QString units; - FieldType type; + QString name; + QString units; + FieldType type; int numElements; int numBytes; QStringList elementNames; QStringList options; // for enums only bool defaultElementNames; QStringList defaultValues; - QString limitValues; + QString limitValues; } FieldInfo; /** * Object update mode */ typedef enum { - UPDATEMODE_MANUAL = 0, /** Manually update object, by calling the updated() function */ - UPDATEMODE_PERIODIC = 1, /** Automatically update object at periodic intervals */ + UPDATEMODE_MANUAL = 0, /** Manually update object, by calling the updated() function */ + UPDATEMODE_PERIODIC = 1, /** Automatically update object at periodic intervals */ UPDATEMODE_ONCHANGE = 2, /** Only update object when its data changes */ - UPDATEMODE_THROTTLED = 3 /** Object is updated on change, but not more often than the interval time */ + UPDATEMODE_THROTTLED = 3 /** Object is updated on change, but not more often than the interval time */ } UpdateMode; typedef enum { ACCESS_READWRITE = 0, - ACCESS_READONLY = 1 + ACCESS_READONLY = 1 } AccessMode; typedef struct { - QString name; - QString namelc; /** name in lowercase */ - QString filename; - quint32 id; - bool isSingleInst; - bool isSettings; + QString name; + QString namelc; /** name in lowercase */ + QString filename; + quint32 id; + bool isSingleInst; + bool isSettings; AccessMode gcsAccess; AccessMode flightAccess; - bool flightTelemetryAcked; + bool flightTelemetryAcked; UpdateMode flightTelemetryUpdateMode; /** Update mode used by the autopilot (UpdateMode) */ int flightTelemetryUpdatePeriod; /** Update period used by the autopilot (only if telemetry mode is PERIODIC) */ - bool gcsTelemetryAcked; + bool gcsTelemetryAcked; UpdateMode gcsTelemetryUpdateMode; /** Update mode used by the GCS (UpdateMode) */ int gcsTelemetryUpdatePeriod; /** Update period used by the GCS (only if telemetry mode is PERIODIC) */ UpdateMode loggingUpdateMode; /** Update mode used by the logging module (UpdateMode) */ int loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */ - QList fields; /** The data fields for the object **/ - QString description; /** Description used for Doxygen **/ - QString category; /** Description used for Doxygen **/ + QList fields; /** The data fields for the object **/ + QString description; /** Description used for Doxygen **/ + QString category; /** Description used for Doxygen **/ } ObjectInfo; -class UAVObjectParser -{ +class UAVObjectParser { public: // Functions UAVObjectParser(); - QString parseXML(QString& xml, QString& filename); + QString parseXML(QString & xml, QString & filename); int getNumObjects(); - QList getObjectInfo(); + QList getObjectInfo(); QString getObjectName(int objIndex); quint32 getObjectID(int objIndex); - ObjectInfo* getObjectByIndex(int objIndex); + ObjectInfo *getObjectByIndex(int objIndex); int getNumBytes(int objIndex); QStringList all_units; private: - QList objInfo; + QList objInfo; QStringList fieldTypeStrXML; QList fieldTypeNumBytes; QStringList updateModeStr; @@ -124,15 +123,15 @@ private: QStringList accessModeStr; QStringList accessModeStrXML; - QString processObjectAttributes(QDomNode& node, ObjectInfo* info); - QString processObjectFields(QDomNode& childNode, ObjectInfo* info); - QString processObjectAccess(QDomNode& childNode, ObjectInfo* info); - QString processObjectDescription(QDomNode& childNode, QString * description); - QString processObjectCategory(QDomNode& childNode, QString * category); - QString processObjectMetadata(QDomNode& childNode, UpdateMode* mode, int* period, bool* acked); - void calculateID(ObjectInfo* info); + QString processObjectAttributes(QDomNode & node, ObjectInfo *info); + QString processObjectFields(QDomNode & childNode, ObjectInfo *info); + QString processObjectAccess(QDomNode & childNode, ObjectInfo *info); + QString processObjectDescription(QDomNode & childNode, QString *description); + QString processObjectCategory(QDomNode & childNode, QString *category); + QString processObjectMetadata(QDomNode & childNode, UpdateMode *mode, int *period, bool *acked); + void calculateID(ObjectInfo *info); quint32 updateHash(quint32 value, quint32 hash); - quint32 updateHash(QString& value, quint32 hash); + quint32 updateHash(QString & value, quint32 hash); }; #endif // UAVOBJECTPARSER_H From 5284195c291cd869c675a061c20cef395a2eafe8 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 18 May 2013 19:36:45 +0200 Subject: [PATCH 02/61] Refaktored sensor and state UAVObjects consistently into XXYSensor and XXYState --- .../Airspeed/revolution/gps_airspeed.c | 10 +- flight/modules/Altitude/altitude.c | 14 +- flight/modules/Altitude/revolution/altitude.c | 14 +- flight/modules/AltitudeHold/altitudehold.c | 58 ++-- flight/modules/Attitude/attitude.c | 118 +++---- flight/modules/Attitude/revolution/attitude.c | 328 +++++++++--------- flight/modules/CameraStab/camerastab.c | 18 +- .../modules/Extensions/MagBaro/inc/magbaro.h | 2 +- flight/modules/Extensions/MagBaro/magbaro.c | 24 +- .../fixedwingpathfollower.c | 117 +++---- flight/modules/MK/MKSerial/MKSerial.c | 16 +- flight/modules/ManualControl/manualcontrol.c | 47 ++- flight/modules/Osd/OsdEtStd/OsdEtStd.c | 22 +- flight/modules/Osd/osdgen/osdgen.c | 16 +- flight/modules/Osd/osdinput/osdinput.c | 16 +- flight/modules/Osd/osdoutput/osdoutput.c | 20 +- flight/modules/PathPlanner/pathplanner.c | 68 ++-- flight/modules/Sensors/sensors.c | 104 +++--- flight/modules/Sensors/simulated/sensors.c | 260 +++++++------- .../modules/Stabilization/inc/relay_tuning.h | 2 +- .../modules/Stabilization/inc/stabilization.h | 2 +- flight/modules/Stabilization/relay_tuning.c | 2 +- flight/modules/Stabilization/stabilization.c | 38 +- .../VtolPathFollower/vtolpathfollower.c | 106 +++--- .../boards/coptercontrol/firmware/Makefile | 10 +- flight/targets/boards/osd/firmware/Makefile | 6 +- .../boards/revolution/firmware/UAVObjects.inc | 21 +- .../revolution/firmware/pios_board_sim.c | 18 +- .../boards/revoproto/firmware/UAVObjects.inc | 21 +- .../revoproto/firmware/pios_board_sim.c | 19 +- .../boards/simposix/firmware/UAVObjects.inc | 20 +- .../default_configurations/Developer.xml | 188 +++++----- .../default_configurations/OpenPilotGCS.xml | 190 +++++----- .../OpenPilotGCS_wide.xml | 190 +++++----- .../pfd/default/AltitudeScale.qml | 6 +- .../openpilotgcs/pfd/default/Compass.qml | 4 +- .../share/openpilotgcs/pfd/default/Pfd.qml | 2 +- .../pfd/default/PfdTerrainView.qml | 6 +- .../openpilotgcs/pfd/default/PfdWorldView.qml | 4 +- .../openpilotgcs/pfd/default/SpeedScale.qml | 4 +- .../openpilotgcs/pfd/default/VsiScale.qml | 2 +- .../translations/openpilotgcs_de.ts | 2 +- .../translations/openpilotgcs_es.ts | 2 +- .../translations/openpilotgcs_fr.ts | 2 +- .../translations/openpilotgcs_ru.ts | 2 +- .../translations/openpilotgcs_zh_CN.ts | 2 +- .../plugins/config/configccattitudewidget.cpp | 74 ++-- .../plugins/config/configccattitudewidget.h | 4 +- .../src/plugins/config/configrevowidget.cpp | 194 +++++------ .../src/plugins/config/configrevowidget.h | 8 +- .../src/plugins/hitl/fgsimulator.cpp | 24 +- .../src/plugins/hitl/hitlconfiguration.cpp | 90 ++--- .../src/plugins/hitl/hitlnoisegeneration.cpp | 32 +- .../src/plugins/hitl/hitlnoisegeneration.h | 20 +- .../src/plugins/hitl/hitloptionspage.cpp | 66 ++-- .../src/plugins/hitl/hitloptionspage.ui | 8 +- .../src/plugins/hitl/il2simulator.cpp | 6 +- .../src/plugins/hitl/isimulator.h | 16 +- .../src/plugins/hitl/simulator.cpp | 196 +++++------ .../openpilotgcs/src/plugins/hitl/simulator.h | 38 +- .../src/plugins/hitl/xplanesimulator.cpp | 12 +- .../magicwaypointgadgetwidget.cpp | 22 +- .../magicwaypoint/magicwaypointgadgetwidget.h | 8 +- .../modelview/modelviewgadgetwidget.cpp | 4 +- .../plugins/modelview/modelviewgadgetwidget.h | 4 +- .../src/plugins/opmap/opmapgadgetwidget.cpp | 64 ++-- .../osgearthview/osgearthviewwidget.cpp | 4 +- .../plugins/osgearthview/osgviewerwidget.cpp | 20 +- .../src/plugins/pfd/pfdgadgetwidget.cpp | 32 +- .../src/plugins/pfdqml/pfdqmlgadgetwidget.cpp | 10 +- .../plugins/qmlview/qmlviewgadgetwidget.cpp | 6 +- .../setupwizard/biascalibrationutil.cpp | 32 +- .../src/plugins/uavobjects/OPPlots.m | 26 +- .../src/plugins/uavobjects/uavobjects.pro | 42 +-- .../{accels.xml => accelsensor.xml} | 4 +- shared/uavobjectdefinition/accelstate.xml | 12 + .../{airspeedactual.xml => airspeedstate.xml} | 2 +- .../{attitudeactual.xml => attitudestate.xml} | 2 +- .../{baroaltitude.xml => barosensor.xml} | 2 +- .../{gyros.xml => gyrosensor.xml} | 2 +- .../{gyrosbias.xml => gyrostate.xml} | 4 +- shared/uavobjectdefinition/magbias.xml | 12 - shared/uavobjectdefinition/magnetosensor.xml | 12 + .../{magnetometer.xml => magnetostate.xml} | 2 +- .../{positionactual.xml => positionstate.xml} | 2 +- .../{velocityactual.xml => velocitystate.xml} | 4 +- 86 files changed, 1631 insertions(+), 1634 deletions(-) rename shared/uavobjectdefinition/{accels.xml => accelsensor.xml} (77%) create mode 100644 shared/uavobjectdefinition/accelstate.xml rename shared/uavobjectdefinition/{airspeedactual.xml => airspeedstate.xml} (88%) rename shared/uavobjectdefinition/{attitudeactual.xml => attitudestate.xml} (91%) rename shared/uavobjectdefinition/{baroaltitude.xml => barosensor.xml} (89%) rename shared/uavobjectdefinition/{gyros.xml => gyrosensor.xml} (89%) rename shared/uavobjectdefinition/{gyrosbias.xml => gyrostate.xml} (77%) delete mode 100644 shared/uavobjectdefinition/magbias.xml create mode 100644 shared/uavobjectdefinition/magnetosensor.xml rename shared/uavobjectdefinition/{magnetometer.xml => magnetostate.xml} (88%) rename shared/uavobjectdefinition/{positionactual.xml => positionstate.xml} (88%) rename shared/uavobjectdefinition/{velocityactual.xml => velocitystate.xml} (70%) diff --git a/flight/modules/Airspeed/revolution/gps_airspeed.c b/flight/modules/Airspeed/revolution/gps_airspeed.c index d81f66fef..6a0d665ec 100644 --- a/flight/modules/Airspeed/revolution/gps_airspeed.c +++ b/flight/modules/Airspeed/revolution/gps_airspeed.c @@ -33,7 +33,7 @@ #include "openpilot.h" #include "gps_airspeed.h" #include "gpsvelocity.h" -#include "attitudeactual.h" +#include "attitudestate.h" #include "CoordinateConversions.h" #include @@ -73,8 +73,8 @@ void gps_airspeedInitialize() gps->gpsVelOld_E = gpsVelData.East; gps->gpsVelOld_D = gpsVelData.Down; - AttitudeActualData attData; - AttitudeActualGet(&attData); + AttitudeStateData attData; + AttitudeStateGet(&attData); float Rbe[3][3]; float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 }; @@ -101,8 +101,8 @@ void gps_airspeedGet(float *v_air_GPS) float Rbe[3][3]; { // Scoping to save memory. We really just need Rbe. - AttitudeActualData attData; - AttitudeActualGet(&attData); + AttitudeStateData attData; + AttitudeStateGet(&attData); float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 }; diff --git a/flight/modules/Altitude/altitude.c b/flight/modules/Altitude/altitude.c index c6302c39f..1230f33fa 100644 --- a/flight/modules/Altitude/altitude.c +++ b/flight/modules/Altitude/altitude.c @@ -3,7 +3,7 @@ * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup AltitudeModule Altitude Module - * @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object" + * @brief Communicate with BMP085 and update @ref BaroSensor "BaroSensor UAV Object" * @{ * * @file altitude.c @@ -30,9 +30,9 @@ */ /** - * Output object: BaroAltitude + * Output object: BaroSensor * - * This module will periodically update the value of the BaroAltitude object. + * This module will periodically update the value of the BaroSensor object. * */ @@ -41,7 +41,7 @@ #include "hwsettings.h" #include "altitude.h" #if defined(PIOS_INCLUDE_BMP085) -#include "baroaltitude.h" // object that will be updated by the module +#include "barosensor.h" // object that will be updated by the module #endif #if defined(PIOS_INCLUDE_HCSR04) #include "sonaraltitude.h" // object that will be updated by the module @@ -80,7 +80,7 @@ int32_t AltitudeStart() { if (altitudeEnabled) { #if defined(PIOS_INCLUDE_BMP085) - BaroAltitudeInitialize(); + BaroSensorInitialize(); #endif #if defined(PIOS_INCLUDE_HCSR04) SonarAltitudeInitialize(); @@ -139,7 +139,7 @@ static void altitudeTask(__attribute__((unused)) void *parameters) } #endif #if defined(PIOS_INCLUDE_BMP085) - BaroAltitudeData data; + BaroSensorData data; PIOS_BMP085_Init(); #endif @@ -206,7 +206,7 @@ static void altitudeTask(__attribute__((unused)) void *parameters) data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255))); // Update the AltitudeActual UAVObject - BaroAltitudeSet(&data); + BaroSensorSet(&data); } #endif /* if defined(PIOS_INCLUDE_BMP085) */ diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index 0c0c59664..f5f758d6e 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -3,7 +3,7 @@ * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup AltitudeModule Altitude Module - * @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object" + * @brief Communicate with BMP085 and update @ref BaroSensor "BaroSensor UAV Object" * @{ * * @file altitude.c @@ -30,16 +30,16 @@ */ /** - * Output object: BaroAltitude + * Output object: BaroSensor * - * This module will periodically update the value of the BaroAltitude object. + * This module will periodically update the value of the BaroSensor object. * */ #include #include "altitude.h" -#include "baroaltitude.h" // object that will be updated by the module +#include "barosensor.h" // object that will be updated by the module #if defined(PIOS_INCLUDE_HCSR04) #include "sonaraltitude.h" // object that will be updated by the module #endif @@ -76,7 +76,7 @@ int32_t AltitudeStart() */ int32_t AltitudeInitialize() { - BaroAltitudeInitialize(); + BaroSensorInitialize(); #if defined(PIOS_INCLUDE_HCSR04) SonarAltitudeInitialize(); #endif @@ -88,7 +88,7 @@ MODULE_INITCALL(AltitudeInitialize, AltitudeStart) */ static void altitudeTask(__attribute__((unused)) void *parameters) { - BaroAltitudeData data; + BaroSensorData data; #if defined(PIOS_INCLUDE_HCSR04) SonarAltitudeData sonardata; @@ -163,7 +163,7 @@ static void altitudeTask(__attribute__((unused)) void *parameters) data.Altitude = 44330.0f * (1.0f - powf(data.Pressure / MS5611_P0, (1.0f / 5.255f))); // Update the AltitudeActual UAVObject - BaroAltitudeSet(&data); + BaroSensorSet(&data); } } diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 823be87ca..14a242c11 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -28,7 +28,7 @@ /** * Input object: ActiveWaypoint - * Input object: PositionActual + * Input object: PositionState * Input object: ManualControlCommand * Output object: AttitudeDesired * @@ -48,14 +48,14 @@ #include #include #include -#include +#include #include #include // object that will be updated by the module -#include -#include +#include +#include #include #include -#include +#include #include #include // Private constants @@ -133,11 +133,11 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Listen for updates. AltitudeHoldDesiredConnectQueue(queue); - BaroAltitudeConnectQueue(queue); + BaroSensorConnectQueue(queue); FlightStatusConnectQueue(queue); - AccelsConnectQueue(queue); + AccelStateConnectQueue(queue); - BaroAltitudeAltitudeGet(&smoothed_altitude); + BaroSensorAltitudeGet(&smoothed_altitude); running = false; enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO; @@ -152,7 +152,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // Todo: Add alarm if it should be running continue; - } else if (ev.obj == BaroAltitudeHandle()) { + } else if (ev.obj == BaroSensorHandle()) { baro_updated = true; init = (init == WAITING_BARO) ? WAITIING_INIT : init; @@ -174,7 +174,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) } else if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) { running = false; } - } else if (ev.obj == AccelsHandle()) { + } else if (ev.obj == AccelStateHandle()) { static uint32_t timeval; static float z[4] = { 0, 0, 0, 0 }; @@ -197,17 +197,17 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) S[1] = altitudeHoldSettings.AccelNoise; G[2] = altitudeHoldSettings.AccelDrift; - AccelsData accels; - AccelsGet(&accels); - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - BaroAltitudeData baro; - BaroAltitudeGet(&baro); + AccelStateData accelState; + AccelStateGet(&accelState); + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + BaroSensorData baro; + BaroSensorGet(&baro); /* Downsample accels to stop this calculation consuming too much CPU */ - accels_accum[0] += accels.x; - accels_accum[1] += accels.y; - accels_accum[2] += accels.z; + accels_accum[0] += accelState.x; + accels_accum[1] += accelState.y; + accels_accum[2] += accelState.z; accel_downsample_count++; if (accel_downsample_count < ACCEL_DOWNSAMPLE) { @@ -215,9 +215,9 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) } accel_downsample_count = 0; - accels.x = accels_accum[0] / ACCEL_DOWNSAMPLE; - accels.y = accels_accum[1] / ACCEL_DOWNSAMPLE; - accels.z = accels_accum[2] / ACCEL_DOWNSAMPLE; + accelState.x = accels_accum[0] / ACCEL_DOWNSAMPLE; + accelState.y = accels_accum[1] / ACCEL_DOWNSAMPLE; + accelState.z = accels_accum[2] / ACCEL_DOWNSAMPLE; accels_accum[0] = accels_accum[1] = accels_accum[2] = 0; thisTime = xTaskGetTickCount(); @@ -225,7 +225,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) if (init == WAITIING_INIT) { z[0] = baro.Altitude; z[1] = 0; - z[2] = accels.z; + z[2] = accelState.z; z[3] = 0; init = INITED; } else if (init == WAITING_BARO) { @@ -236,14 +236,14 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // rotate avg accels into earth frame and store it if (1) { float q[4], Rbe[3][3]; - q[0] = attitudeActual.q1; - q[1] = attitudeActual.q2; - q[2] = attitudeActual.q3; - q[3] = attitudeActual.q4; + q[0] = attitudeState.q1; + q[1] = attitudeState.q2; + q[2] = attitudeState.q3; + q[3] = attitudeState.q4; Quaternion2R(q, Rbe); - x[1] = -(Rbe[0][2] * accels.x + Rbe[1][2] * accels.y + Rbe[2][2] * accels.z + 9.81f); + x[1] = -(Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f); } else { - x[1] = -accels.z + 9.81f; + x[1] = -accelState.z + 9.81f; } dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index 688d97dbc..2964b3880 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -4,7 +4,7 @@ * @{ * @addtogroup Attitude Copter Control Attitude Estimation * @brief Acquires sensor data and computes attitude estimate - * Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects + * Specifically updates the the @ref AttitudeState "AttitudeState" and @ref AttitudeRaw "AttitudeRaw" settings objects * @{ * * @file attitude.c @@ -32,7 +32,7 @@ /** * Input objects: None, takes sensor data via pios - * Output objects: @ref AttitudeRaw @ref AttitudeActual + * Output objects: @ref AttitudeRaw @ref AttitudeState * * This module computes an attitude estimate from the sensor data * @@ -53,9 +53,9 @@ #include #include "attitude.h" -#include "gyros.h" -#include "accels.h" -#include "attitudeactual.h" +#include "gyrostate.h" +#include "accelstate.h" +#include "attitudestate.h" #include "attitudesettings.h" #include "flightstatus.h" #include "manualcontrolcommand.h" @@ -83,9 +83,9 @@ static void AttitudeTask(void *parameters); static float gyro_correct_int[3] = { 0, 0, 0 }; static xQueueHandle gyro_queue; -static int32_t updateSensors(AccelsData *, GyrosData *); -static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData); -static void updateAttitude(AccelsData *, GyrosData *); +static int32_t updateSensors(AccelStateData *, GyroStateData *); +static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData); +static void updateAttitude(AccelStateData *, GyroStateData *); static void settingsUpdatedCb(UAVObjEvent *objEv); static float accelKi = 0; @@ -134,19 +134,19 @@ int32_t AttitudeStart(void) */ int32_t AttitudeInitialize(void) { - AttitudeActualInitialize(); + AttitudeStateInitialize(); AttitudeSettingsInitialize(); - AccelsInitialize(); - GyrosInitialize(); + AccelStateInitialize(); + GyroStateInitialize(); // Initialize quaternion - AttitudeActualData attitude; - AttitudeActualGet(&attitude); + AttitudeStateData attitude; + AttitudeStateGet(&attitude); attitude.q1 = 1; attitude.q2 = 0; attitude.q3 = 0; attitude.q4 = 0; - AttitudeActualSet(&attitude); + AttitudeStateSet(&attitude); // Cannot trust the values to init right above if BL runs gyro_correct_int[0] = 0; @@ -248,14 +248,14 @@ static void AttitudeTask(__attribute__((unused)) void *parameters) PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); - AccelsData accels; - GyrosData gyros; + AccelStateData accelState; + GyroStateData gyros; int32_t retval = 0; if (cc3d) { - retval = updateSensorsCC3D(&accels, &gyros); + retval = updateSensorsCC3D(&accelState, &gyros); } else { - retval = updateSensors(&accels, &gyros); + retval = updateSensors(&accelState, &gyros); } // Only update attitude when sensor data is good @@ -263,8 +263,8 @@ static void AttitudeTask(__attribute__((unused)) void *parameters) AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); } else { // Do not update attitude data in simulation mode - if (!AttitudeActualReadOnly()) { - updateAttitude(&accels, &gyros); + if (!AttitudeStateReadOnly()) { + updateAttitude(&accelState, &gyros); } AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); @@ -279,7 +279,7 @@ float gyros_passed[3]; * @param[in] attitudeRaw Populate the UAVO instead of saving right here * @return 0 if successfull, -1 if not */ -static int32_t updateSensors(AccelsData *accels, GyrosData *gyros) +static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros) { struct pios_adxl345_data accel_data; float gyro[4]; @@ -291,7 +291,7 @@ static int32_t updateSensors(AccelsData *accels, GyrosData *gyros) } // Do not read raw sensor data in simulation mode - if (GyrosReadOnly() || AccelsReadOnly()) { + if (GyroStateReadOnly() || AccelStateReadOnly()) { return 0; } @@ -317,7 +317,7 @@ static int32_t updateSensors(AccelsData *accels, GyrosData *gyros) y += -accel_data.y; z += -accel_data.z; } while ((i < 32) && (samples_remaining > 0)); - gyros->temperature = samples_remaining; + // gyros->temperature = samples_remaining; float accel[3] = { (float)x / i, (float)y / i, (float)z / i }; @@ -325,17 +325,17 @@ static int32_t updateSensors(AccelsData *accels, GyrosData *gyros) // TODO: rotate sensors too so stabilization is well behaved float vec_out[3]; rot_mult(R, accel, vec_out); - accels->x = vec_out[0]; - accels->y = vec_out[1]; - accels->z = vec_out[2]; + accelState->x = vec_out[0]; + accelState->y = vec_out[1]; + accelState->z = vec_out[2]; rot_mult(R, &gyros->x, vec_out); - gyros->x = vec_out[0]; - gyros->y = vec_out[1]; - gyros->z = vec_out[2]; + gyros->x = vec_out[0]; + gyros->y = vec_out[1]; + gyros->z = vec_out[2]; } else { - accels->x = accel[0]; - accels->y = accel[1]; - accels->z = accel[2]; + accelState->x = accel[0]; + accelState->y = accel[1]; + accelState->z = accel[2]; } if (trim_requested) { @@ -349,17 +349,17 @@ static int32_t updateSensors(AccelsData *accels, GyrosData *gyros) if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0.0f)) { trim_samples++; // Store the digitally scaled version since that is what we use for bias - trim_accels[0] += accels->x; - trim_accels[1] += accels->y; - trim_accels[2] += accels->z; + trim_accels[0] += accelState->x; + trim_accels[1] += accelState->y; + trim_accels[2] += accelState->z; } } } // Scale accels and correct bias - accels->x = (accels->x - accelbias[0]) * ACCEL_SCALE; - accels->y = (accels->y - accelbias[1]) * ACCEL_SCALE; - accels->z = (accels->z - accelbias[2]) * ACCEL_SCALE; + accelState->x = (accelState->x - accelbias[0]) * ACCEL_SCALE; + accelState->y = (accelState->y - accelbias[1]) * ACCEL_SCALE; + accelState->z = (accelState->z - accelbias[2]) * ACCEL_SCALE; if (bias_correct_gyro) { // Applying integral component here so it can be seen on the gyros and correct bias @@ -376,8 +376,8 @@ static int32_t updateSensors(AccelsData *accels, GyrosData *gyros) // and make it average zero (weakly) gyro_correct_int[2] += -gyros->z * yawBiasRate; - GyrosSet(gyros); - AccelsSet(accels); + GyroStateSet(gyros); + AccelStateSet(accelState); return 0; } @@ -388,7 +388,7 @@ static int32_t updateSensors(AccelsData *accels, GyrosData *gyros) * @return 0 if successfull, -1 if not */ struct pios_mpu6000_data mpu6000_data; -static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData) +static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData) { float accels[3], gyros[3]; @@ -400,7 +400,7 @@ static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData) return -1; // Error, no data } // Do not read raw sensor data in simulation mode - if (GyrosReadOnly() || AccelsReadOnly()) { + if (GyroStateReadOnly() || AccelStateReadOnly()) { return 0; } @@ -412,8 +412,8 @@ static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData) accels[1] = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale(); accels[2] = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale(); - gyrosData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; - accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; + // gyrosData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; + // accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; #endif /* if defined(PIOS_INCLUDE_MPU6000) */ if (rotate) { @@ -429,13 +429,13 @@ static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData) gyros[2] = vec_out[2]; } - accelsData->x = accels[0] - accelbias[0] * ACCEL_SCALE; // Applying arbitrary scale here to match CC v1 - accelsData->y = accels[1] - accelbias[1] * ACCEL_SCALE; - accelsData->z = accels[2] - accelbias[2] * ACCEL_SCALE; + accelStateData->x = accels[0] - accelbias[0] * ACCEL_SCALE; // Applying arbitrary scale here to match CC v1 + accelStateData->y = accels[1] - accelbias[1] * ACCEL_SCALE; + accelStateData->z = accels[2] - accelbias[2] * ACCEL_SCALE; - gyrosData->x = gyros[0]; - gyrosData->y = gyros[1]; - gyrosData->z = gyros[2]; + gyrosData->x = gyros[0]; + gyrosData->y = gyros[1]; + gyrosData->z = gyros[2]; if (bias_correct_gyro) { // Applying integral component here so it can be seen on the gyros and correct bias @@ -452,8 +452,8 @@ static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData) // and make it average zero (weakly) gyro_correct_int[2] += -gyrosData->z * yawBiasRate; - GyrosSet(gyrosData); - AccelsSet(accelsData); + GyroStateSet(gyrosData); + AccelStateSet(accelStateData); return 0; } @@ -471,7 +471,7 @@ static inline void apply_accel_filter(const float *raw, float *filtered) } } -static void updateAttitude(AccelsData *accelsData, GyrosData *gyrosData) +static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosData) { float dT; portTickType thisSysTime = xTaskGetTickCount(); @@ -482,7 +482,7 @@ static void updateAttitude(AccelsData *accelsData, GyrosData *gyrosData) // Bad practice to assume structure order, but saves memory float *gyros = &gyrosData->x; - float *accels = &accelsData->x; + float *accels = &accelStateData->x; float grot[3]; float accel_err[3]; @@ -573,15 +573,15 @@ static void updateAttitude(AccelsData *accelsData, GyrosData *gyrosData) q[3] = q[3] / qmag; } - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); - quat_copy(q, &attitudeActual.q1); + quat_copy(q, &attitudeState.q1); // Convert into eueler degrees (makes assumptions about RPY order) - Quaternion2RPY(&attitudeActual.q1, &attitudeActual.Roll); + Quaternion2RPY(&attitudeState.q1, &attitudeState.Roll); - AttitudeActualSet(&attitudeActual); + AttitudeStateSet(&attitudeState); } static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index 43f060c92..6036a1691 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -4,7 +4,7 @@ * @{ * @addtogroup Attitude Copter Control Attitude Estimation * @brief Acquires sensor data and computes attitude estimate - * Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects + * Specifically updates the the @ref AttitudeState "AttitudeState" and @ref AttitudeRaw "AttitudeRaw" settings objects * @{ * * @file attitude.c @@ -32,7 +32,7 @@ /** * Input objects: None, takes sensor data via pios - * Output objects: @ref AttitudeRaw @ref AttitudeActual + * Output objects: @ref AttitudeRaw @ref AttitudeState * * This module computes an attitude estimate from the sensor data * @@ -51,25 +51,27 @@ #include #include "attitude.h" -#include "accels.h" +#include "accelsensor.h" +#include "accelstate.h" #include "airspeedsensor.h" -#include "airspeedactual.h" -#include "attitudeactual.h" +#include "airspeedstate.h" +#include "attitudestate.h" #include "attitudesettings.h" -#include "baroaltitude.h" +#include "barosensor.h" #include "flightstatus.h" #include "gpsposition.h" #include "gpsvelocity.h" -#include "gyros.h" -#include "gyrosbias.h" +#include "gyrostate.h" +#include "gyrosensor.h" #include "homelocation.h" -#include "magnetometer.h" -#include "positionactual.h" +#include "magnetosensor.h" +#include "magnetostate.h" +#include "positionstate.h" #include "ekfconfiguration.h" #include "ekfstatevariance.h" #include "revocalibration.h" #include "revosettings.h" -#include "velocityactual.h" +#include "velocitystate.h" #include "taskinfo.h" #include "CoordinateConversions.h" @@ -159,12 +161,12 @@ static inline bool invalid_var(float data) */ int32_t AttitudeInitialize(void) { - AttitudeActualInitialize(); - AirspeedActualInitialize(); + AttitudeStateInitialize(); + AirspeedStateInitialize(); AirspeedSensorInitialize(); AttitudeSettingsInitialize(); - PositionActualInitialize(); - VelocityActualInitialize(); + PositionStateInitialize(); + VelocityStateInitialize(); RevoSettingsInitialize(); RevoCalibrationInitialize(); EKFConfigurationInitialize(); @@ -175,21 +177,13 @@ int32_t AttitudeInitialize(void) HomeLocationInitialize(); // Initialize quaternion - AttitudeActualData attitude; - AttitudeActualGet(&attitude); + AttitudeStateData attitude; + AttitudeStateGet(&attitude); attitude.q1 = 1.0f; attitude.q2 = 0.0f; attitude.q3 = 0.0f; attitude.q4 = 0.0f; - AttitudeActualSet(&attitude); - - // Cannot trust the values to init right above if BL runs - GyrosBiasData gyrosBias; - GyrosBiasGet(&gyrosBias); - gyrosBias.x = 0.0f; - gyrosBias.y = 0.0f; - gyrosBias.z = 0.0f; - GyrosBiasSet(&gyrosBias); + AttitudeStateSet(&attitude); AttitudeSettingsConnectCallback(&settingsUpdatedCb); RevoSettingsConnectCallback(&settingsUpdatedCb); @@ -221,11 +215,11 @@ int32_t AttitudeStart(void) PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle); PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); - GyrosConnectQueue(gyroQueue); - AccelsConnectQueue(accelQueue); - MagnetometerConnectQueue(magQueue); + GyroSensorConnectQueue(gyroQueue); + AccelSensorConnectQueue(accelQueue); + MagnetoSensorConnectQueue(magQueue); AirspeedSensorConnectQueue(airspeedQueue); - BaroAltitudeConnectQueue(baroQueue); + BaroSensorConnectQueue(baroQueue); GPSPositionConnectQueue(gpsQueue); GPSVelocityConnectQueue(gpsVelQueue); @@ -291,8 +285,8 @@ float magKp = 0.01f; static int32_t updateAttitudeComplementary(bool first_run) { UAVObjEvent ev; - GyrosData gyrosData; - AccelsData accelsData; + GyroSensorData gyroSensorData; + AccelSensorData accelSensorData; static int32_t timeval; float dT; static uint8_t init = 0; @@ -302,13 +296,13 @@ static int32_t updateAttitudeComplementary(bool first_run) xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) { // When one of these is updated so should the other // Do not set attitude timeout warnings in simulation mode - if (!AttitudeActualReadOnly()) { + if (!AttitudeStateReadOnly()) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); return -1; } } - AccelsGet(&accelsData); + AccelSensorGet(&accelSensorData); // During initialization and if (first_run) { @@ -317,43 +311,43 @@ static int32_t updateAttitudeComplementary(bool first_run) if (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE) { return -1; } - MagnetometerData magData; - MagnetometerGet(&magData); + MagnetoSensorData magData; + MagnetoSensorGet(&magData); #else - MagnetometerData magData; + MagnetoSensorData magData; magData.x = 100.0f; magData.y = 0.0f; magData.z = 0.0f; #endif - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); init = 0; // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, // so pseudo "north" vector can be estimated even if the board is not level - attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z); - float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y; - float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z; + attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z); + float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y; + float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z; // rotate accels z vector according to roll - float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y; - attitudeActual.Pitch = atan2f(accelsData.x, -azn); + float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y; + attitudeState.Pitch = atan2f(accelSensorData.x, -azn); - float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn; + float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn; - attitudeActual.Yaw = atan2f(-yn, xn); + attitudeState.Yaw = atan2f(-yn, xn); // TODO: This is still a hack // Put this in a proper generic function in CoordinateConversion.c // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) // should calculate the rotation in 3d space using proper cross product math // SUBTODO: formulate the math required - attitudeActual.Roll = RAD2DEG(attitudeActual.Roll); - attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch); - attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw); + attitudeState.Roll = RAD2DEG(attitudeState.Roll); + attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); + attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); - RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1); - AttitudeActualSet(&attitudeActual); + RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1); + AttitudeStateSet(&attitudeState); timeval = PIOS_DELAY_GetRaw(); @@ -379,7 +373,7 @@ static int32_t updateAttitudeComplementary(bool first_run) init = 1; } - GyrosGet(&gyrosData); + GyroSensorGet(&gyroSensorData); // Compute the dT using the cpu clock dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; @@ -387,23 +381,23 @@ static int32_t updateAttitudeComplementary(bool first_run) float q[4]; - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); float grot[3]; float accel_err[3]; // Get the current attitude estimate - quat_copy(&attitudeActual.q1, q); + quat_copy(&attitudeState.q1, q); // Rotate gravity to body frame and cross with accels grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2])); grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1])); grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); - CrossProduct((const float *)&accelsData.x, (const float *)grot, accel_err); + CrossProduct((const float *)&accelSensorData.x, (const float *)grot, accel_err); // Account for accel magnitude - accel_mag = accelsData.x * accelsData.x + accelsData.y * accelsData.y + accelsData.z * accelsData.z; + accel_mag = accelSensorData.x * accelSensorData.x + accelSensorData.y * accelSensorData.y + accelSensorData.z * accelSensorData.z; accel_mag = sqrtf(accel_mag); accel_err[0] /= accel_mag; accel_err[1] /= accel_mag; @@ -413,10 +407,10 @@ static int32_t updateAttitudeComplementary(bool first_run) // Rotate gravity to body frame and cross with accels float brot[3]; float Rbe[3][3]; - MagnetometerData mag; + MagnetoSensorData mag; Quaternion2R(q, Rbe); - MagnetometerGet(&mag); + MagnetoSensorGet(&mag); // If the mag is producing bad data don't use it (normally bad calibration) if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) { @@ -444,7 +438,9 @@ static int32_t updateAttitudeComplementary(bool first_run) } // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s - GyrosBiasData gyrosBias; +// TODO +/* + GyrosBiasData gyrosBias; GyrosBiasGet(&gyrosBias); gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi; gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi; @@ -453,23 +449,24 @@ static int32_t updateAttitudeComplementary(bool first_run) if (revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { // if the raw values are not adjusted, we need to adjust here. - gyrosData.x -= gyrosBias.x; - gyrosData.y -= gyrosBias.y; - gyrosData.z -= gyrosBias.z; + gyroSensorData.x -= gyrosBias.x; + gyroSensorData.y -= gyrosBias.y; + gyroSensorData.z -= gyrosBias.z; } + */ // Correct rates based on error, integral component dealt with in updateSensors - gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT; - gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT; - gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; + gyroSensorData.x += accel_err[0] * attitudeSettings.AccelKp / dT; + gyroSensorData.y += accel_err[1] * attitudeSettings.AccelKp / dT; + gyroSensorData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; // Work out time derivative from INSAlgo writeup // Also accounts for the fact that gyros are in deg/s float qdot[4]; - qdot[0] = DEG2RAD(-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT / 2; - qdot[1] = DEG2RAD(q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT / 2; - qdot[2] = DEG2RAD(q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT / 2; - qdot[3] = DEG2RAD(-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT / 2; + qdot[0] = DEG2RAD(-q[1] * gyroSensorData.x - q[2] * gyroSensorData.y - q[3] * gyroSensorData.z) * dT / 2; + qdot[1] = DEG2RAD(q[0] * gyroSensorData.x - q[3] * gyroSensorData.y + q[2] * gyroSensorData.z) * dT / 2; + qdot[2] = DEG2RAD(q[3] * gyroSensorData.x + q[0] * gyroSensorData.y - q[1] * gyroSensorData.z) * dT / 2; + qdot[3] = DEG2RAD(-q[2] * gyroSensorData.x + q[1] * gyroSensorData.y + q[0] * gyroSensorData.z) * dT / 2; // Take a time step q[0] = q[0] + qdot[0]; @@ -500,12 +497,12 @@ static int32_t updateAttitudeComplementary(bool first_run) q[3] = 0.0f; } - quat_copy(q, &attitudeActual.q1); + quat_copy(q, &attitudeState.q1); // Convert into eueler degrees (makes assumptions about RPY order) - Quaternion2RPY(&attitudeActual.q1, &attitudeActual.Roll); + Quaternion2RPY(&attitudeState.q1, &attitudeState.Roll); - AttitudeActualSet(&attitudeActual); + AttitudeStateSet(&attitudeState); // Flush these queues for avoid errors xQueueReceive(baroQueue, &ev, 0); @@ -516,12 +513,12 @@ static int32_t updateAttitudeComplementary(bool first_run) GPSPositionGet(&gpsPosition); getNED(&gpsPosition, NED); - PositionActualData positionActual; - PositionActualGet(&positionActual); - positionActual.North = NED[0]; - positionActual.East = NED[1]; - positionActual.Down = NED[2]; - PositionActualSet(&positionActual); + PositionStateData positionState; + PositionStateGet(&positionState); + positionState.North = NED[0]; + positionState.East = NED[1]; + positionState.Down = NED[2]; + PositionStateSet(&positionState); } if (xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE) { @@ -529,12 +526,12 @@ static int32_t updateAttitudeComplementary(bool first_run) GPSVelocityData gpsVelocity; GPSVelocityGet(&gpsVelocity); - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); - velocityActual.North = gpsVelocity.North; - velocityActual.East = gpsVelocity.East; - velocityActual.Down = gpsVelocity.Down; - VelocityActualSet(&velocityActual); + VelocityStateData velocityState; + VelocityStateGet(&velocityState); + velocityState.North = gpsVelocity.North; + velocityState.East = gpsVelocity.East; + velocityState.Down = gpsVelocity.Down; + VelocityStateSet(&velocityState); } if (xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE) { @@ -542,17 +539,17 @@ static int32_t updateAttitudeComplementary(bool first_run) AirspeedSensorData airspeedSensor; AirspeedSensorGet(&airspeedSensor); - AirspeedActualData airspeed; - AirspeedActualGet(&airspeed); + AirspeedStateData airspeed; + AirspeedStateGet(&airspeed); - PositionActualData positionActual; - PositionActualGet(&positionActual); + PositionStateData positionState; + PositionStateGet(&positionState); if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { // we have airspeed available airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed; - airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionActual.Down); - AirspeedActualSet(&airspeed); + airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionState.Down); + AirspeedStateSet(&airspeed); } } @@ -581,14 +578,13 @@ int32_t init_stage = 0; static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) { UAVObjEvent ev; - GyrosData gyrosData; - AccelsData accelsData; - MagnetometerData magData; + GyroSensorData gyroSensorData; + AccelSensorData accelSensorData; + MagnetoSensorData magData; AirspeedSensorData airspeedData; - BaroAltitudeData baroData; + BaroSensorData baroData; GPSPositionData gpsData; GPSVelocityData gpsVelData; - GyrosBiasData gyrosBias; static bool mag_updated = false; static bool baro_updated; @@ -615,7 +611,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) if ((xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) || (xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE)) { // Do not set attitude timeout warnings in simulation mode - if (!AttitudeActualReadOnly()) { + if (!AttitudeStateReadOnly()) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); return -1; } @@ -662,34 +658,26 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) } // Get most recent data - GyrosGet(&gyrosData); - AccelsGet(&accelsData); - MagnetometerGet(&magData); - BaroAltitudeGet(&baroData); + GyroSensorGet(&gyroSensorData); + AccelSensorGet(&accelSensorData); + MagnetoSensorGet(&magData); + BaroSensorGet(&baroData); AirspeedSensorGet(&airspeedData); GPSPositionGet(&gpsData); GPSVelocityGet(&gpsVelData); - GyrosBiasGet(&gyrosBias); value_error = false; // safety checks - if (invalid(gyrosData.x) || - invalid(gyrosData.y) || - invalid(gyrosData.z) || - invalid(accelsData.x) || - invalid(accelsData.y) || - invalid(accelsData.z)) { + if (invalid(gyroSensorData.x) || + invalid(gyroSensorData.y) || + invalid(gyroSensorData.z) || + invalid(accelSensorData.x) || + invalid(accelSensorData.y) || + invalid(accelSensorData.z)) { // cannot run process update, raise error! AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); return 0; } - if (invalid(gyrosBias.x) || - invalid(gyrosBias.y) || - invalid(gyrosBias.z)) { - gyrosBias.x = 0.0f; - gyrosBias.y = 0.0f; - gyrosBias.z = 0.0f; - } if (invalid(magData.x) || invalid(magData.y) || @@ -819,38 +807,38 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) } xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); - MagnetometerGet(&magData); + MagnetoSensorGet(&magData); - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, // so pseudo "north" vector can be estimated even if the board is not level - attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z); - float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y; - float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z; + attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z); + float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y; + float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z; // rotate accels z vector according to roll - float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y; - attitudeActual.Pitch = atan2f(accelsData.x, -azn); + float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y; + attitudeState.Pitch = atan2f(accelSensorData.x, -azn); - float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn; + float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn; - attitudeActual.Yaw = atan2f(-yn, xn); + attitudeState.Yaw = atan2f(-yn, xn); // TODO: This is still a hack // Put this in a proper generic function in CoordinateConversion.c // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) // should calculate the rotation in 3d space using proper cross product math // SUBTODO: formulate the math required - attitudeActual.Roll = RAD2DEG(attitudeActual.Roll); - attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch); - attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw); + attitudeState.Roll = RAD2DEG(attitudeState.Roll); + attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); + attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); - RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1); - AttitudeActualSet(&attitudeActual); + RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1); + AttitudeStateSet(&attitudeState); - float q[4] = { attitudeActual.q1, attitudeActual.q2, attitudeActual.q3, attitudeActual.q4 }; + float q[4] = { attitudeState.q1, attitudeState.q2, attitudeState.q3, attitudeState.q4 }; INSSetState(pos, zeros, q, zeros, zeros); INSResetP(ekfConfiguration.P); @@ -859,22 +847,24 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) // Because the sensor module remove the bias we need to add it // back in here so that the INS algorithm can track it correctly - float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) }; - if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { - gyros[0] += DEG2RAD(gyrosBias.x); - gyros[1] += DEG2RAD(gyrosBias.y); - gyros[2] += DEG2RAD(gyrosBias.z); - } - INSStatePrediction(gyros, &accelsData.x, dT); + float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) }; + /* TODO + if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { + gyros[0] += DEG2RAD(gyrosBias.x); + gyros[1] += DEG2RAD(gyrosBias.y); + gyros[2] += DEG2RAD(gyrosBias.z); + } + */ + INSStatePrediction(gyros, &accelSensorData.x, dT); - AttitudeActualData attitude; - AttitudeActualGet(&attitude); + AttitudeStateData attitude; + AttitudeStateGet(&attitude); attitude.q1 = Nav.q[0]; attitude.q2 = Nav.q[1]; attitude.q3 = Nav.q[2]; attitude.q4 = Nav.q[3]; Quaternion2RPY(&attitude.q1, &attitude.Roll); - AttitudeActualSet(&attitude); + AttitudeStateSet(&attitude); } init_stage++; @@ -891,25 +881,27 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) // Because the sensor module remove the bias we need to add it // back in here so that the INS algorithm can track it correctly - float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) }; - if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { - gyros[0] += DEG2RAD(gyrosBias.x); - gyros[1] += DEG2RAD(gyrosBias.y); - gyros[2] += DEG2RAD(gyrosBias.z); - } + float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) }; + /* TODO + if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { + gyros[0] += DEG2RAD(gyrosBias.x); + gyros[1] += DEG2RAD(gyrosBias.y); + gyros[2] += DEG2RAD(gyrosBias.z); + } + */ // Advance the state estimate - INSStatePrediction(gyros, &accelsData.x, dT); + INSStatePrediction(gyros, &accelSensorData.x, dT); // Copy the attitude into the UAVO - AttitudeActualData attitude; - AttitudeActualGet(&attitude); + AttitudeStateData attitude; + AttitudeStateGet(&attitude); attitude.q1 = Nav.q[0]; attitude.q2 = Nav.q[1]; attitude.q3 = Nav.q[2]; attitude.q4 = Nav.q[3]; Quaternion2RPY(&attitude.q1, &attitude.Roll); - AttitudeActualSet(&attitude); + AttitudeStateSet(&attitude); // Advance the covariance estimate INSCovariancePrediction(dT); @@ -971,12 +963,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) if (airspeed_updated) { // we have airspeed available - AirspeedActualData airspeed; - AirspeedActualGet(&airspeed); + AirspeedStateData airspeed; + AirspeedStateGet(&airspeed); airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed; airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - Nav.Pos[2]); - AirspeedActualSet(&airspeed); + AirspeedStateSet(&airspeed); if (!gps_vel_updated && !gps_updated) { // feed airspeed into EKF, treat wind as 1e2 variance @@ -1005,24 +997,26 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) } // Copy the position and velocity into the UAVO - PositionActualData positionActual; - PositionActualGet(&positionActual); - positionActual.North = Nav.Pos[0]; - positionActual.East = Nav.Pos[1]; - positionActual.Down = Nav.Pos[2]; - PositionActualSet(&positionActual); + PositionStateData positionState; + PositionStateGet(&positionState); + positionState.North = Nav.Pos[0]; + positionState.East = Nav.Pos[1]; + positionState.Down = Nav.Pos[2]; + PositionStateSet(&positionState); - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); - velocityActual.North = Nav.Vel[0]; - velocityActual.East = Nav.Vel[1]; - velocityActual.Down = Nav.Vel[2]; - VelocityActualSet(&velocityActual); + VelocityStateData velocityState; + VelocityStateGet(&velocityState); + velocityState.North = Nav.Vel[0]; + velocityState.East = Nav.Vel[1]; + velocityState.Down = Nav.Vel[2]; + VelocityStateSet(&velocityState); +/* TODO gyrosBias.x = RAD2DEG(Nav.gyro_bias[0]); gyrosBias.y = RAD2DEG(Nav.gyro_bias[1]); gyrosBias.z = RAD2DEG(Nav.gyro_bias[2]); GyrosBiasSet(&gyrosBias); + */ EKFStateVarianceData vardata; EKFStateVarianceGet(&vardata); @@ -1120,7 +1114,7 @@ static void settingsUpdatedCb(UAVObjEvent *ev) T[1] = cosf(lat) * (alt + 6.378137E6f); T[2] = -1.0f; - // TODO: convert positionActual to new reference frame and gracefully update EKF state! + // TODO: convert positionState to new reference frame and gracefully update EKF state! // needed for long range flights where the reference coordinate is adjusted in flight } if (ev == NULL || ev->obj == AttitudeSettingsHandle()) { diff --git a/flight/modules/CameraStab/camerastab.c b/flight/modules/CameraStab/camerastab.c index 04350c033..1fe650254 100644 --- a/flight/modules/CameraStab/camerastab.c +++ b/flight/modules/CameraStab/camerastab.c @@ -48,7 +48,7 @@ #include "openpilot.h" #include "accessorydesired.h" -#include "attitudeactual.h" +#include "attitudestate.h" #include "camerastabsettings.h" #include "cameradesired.h" #include "hwsettings.h" @@ -119,12 +119,12 @@ int32_t CameraStabInitialize(void) memset(csd, 0, sizeof(struct CameraStab_data)); csd->lastSysTime = xTaskGetTickCount(); - AttitudeActualInitialize(); + AttitudeStateInitialize(); CameraStabSettingsInitialize(); CameraDesiredInitialize(); UAVObjEvent ev = { - .obj = AttitudeActualHandle(), + .obj = AttitudeStateHandle(), .instId = 0, .event = 0, }; @@ -146,7 +146,7 @@ MODULE_INITCALL(CameraStabInitialize, CameraStabStart) static void attitudeUpdated(UAVObjEvent *ev) { - if (ev->obj != AttitudeActualHandle()) { + if (ev->obj != AttitudeStateHandle()) { return; } @@ -195,13 +195,13 @@ static void attitudeUpdated(UAVObjEvent *ev) switch (i) { case CAMERASTABSETTINGS_INPUT_ROLL: - AttitudeActualRollGet(&attitude); + AttitudeStateRollGet(&attitude); break; case CAMERASTABSETTINGS_INPUT_PITCH: - AttitudeActualPitchGet(&attitude); + AttitudeStatePitchGet(&attitude); break; case CAMERASTABSETTINGS_INPUT_YAW: - AttitudeActualYawGet(&attitude); + AttitudeStateYawGet(&attitude); break; default: PIOS_Assert(0); @@ -297,7 +297,7 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta case CAMERASTABSETTINGS_GIMBALTYPE_YAWROLLPITCH: if (index == CAMERASTABSETTINGS_INPUT_ROLL) { float pitch; - AttitudeActualPitchGet(&pitch); + AttitudeStatePitchGet(&pitch); gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabsf(pitch)) / cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]; } @@ -305,7 +305,7 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta case CAMERASTABSETTINGS_GIMBALTYPE_YAWPITCHROLL: if (index == CAMERASTABSETTINGS_INPUT_PITCH) { float roll; - AttitudeActualRollGet(&roll); + AttitudeStateRollGet(&roll); gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabsf(roll)) / cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]; } diff --git a/flight/modules/Extensions/MagBaro/inc/magbaro.h b/flight/modules/Extensions/MagBaro/inc/magbaro.h index ad495fb1a..ce77f1ce4 100644 --- a/flight/modules/Extensions/MagBaro/inc/magbaro.h +++ b/flight/modules/Extensions/MagBaro/inc/magbaro.h @@ -3,7 +3,7 @@ * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup AltitudeModule Altitude Module - * @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object" + * @brief Communicate with BMP085 and update @ref BaroSensor "BaroSensor UAV Object" * @{ * * @file altitude.h diff --git a/flight/modules/Extensions/MagBaro/magbaro.c b/flight/modules/Extensions/MagBaro/magbaro.c index cc3835c54..3bff3cbca 100644 --- a/flight/modules/Extensions/MagBaro/magbaro.c +++ b/flight/modules/Extensions/MagBaro/magbaro.c @@ -3,7 +3,7 @@ * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup AltitudeModule Altitude Module - * @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object" + * @brief Communicate with BMP085 and update @ref BaroSensor "BaroSensor UAV Object" * @{ * * @file altitude.c @@ -30,9 +30,9 @@ */ /** - * Output object: BaroAltitude + * Output object: BaroSensor * - * This module will periodically update the value of the BaroAltitude object. + * This module will periodically update the value of the BaroSensor object. * */ @@ -40,8 +40,8 @@ #include "hwsettings.h" #include "magbaro.h" -#include "baroaltitude.h" // object that will be updated by the module -#include "magnetometer.h" +#include "barosensor.h" // object that will be updated by the module +#include "magnetosensor.h" #include "taskinfo.h" // Private constants @@ -109,11 +109,11 @@ int32_t MagBaroInitialize() if (magbaroEnabled) { #if defined(PIOS_INCLUDE_HMC5883) - MagnetometerInitialize(); + MagnetoSensorInitialize(); #endif #if defined(PIOS_INCLUDE_BMP085) - BaroAltitudeInitialize(); + BaroSensorInitialize(); // init down-sampling data alt_ds_temp = 0; @@ -144,12 +144,12 @@ static void magbaroTask(__attribute__((unused)) void *parameters) portTickType lastSysTime; #if defined(PIOS_INCLUDE_BMP085) - BaroAltitudeData data; + BaroSensorData data; PIOS_BMP085_Init(); #endif #if defined(PIOS_INCLUDE_HMC5883) - MagnetometerData mag; + MagnetoSensorData mag; PIOS_HMC5883_Init(&pios_hmc5883_cfg); uint32_t mag_update_time = PIOS_DELAY_GetRaw(); #endif @@ -192,8 +192,8 @@ static void magbaroTask(__attribute__((unused)) void *parameters) // Compute the current altitude (all pressures in kPa) data.Altitude = 44330.0f * (1.0f - powf((data.Pressure / (BMP085_P0 / 1000.0f)), (1.0f / 5.255f))); - // Update the AltitudeActual UAVObject - BaroAltitudeSet(&data); + // Update the BaroSensor UAVObject + BaroSensorSet(&data); } #endif /* if defined(PIOS_INCLUDE_BMP085) */ @@ -207,7 +207,7 @@ static void magbaroTask(__attribute__((unused)) void *parameters) mag.x = mags[0]; mag.y = mags[1]; mag.z = mags[2]; - MagnetometerSet(&mag); + MagnetoSensorSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); } #endif diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index fec984a87..665496a44 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -28,7 +28,7 @@ /** * Input object: ActiveWaypoint - * Input object: PositionActual + * Input object: PositionState * Input object: ManualControlCommand * Output object: AttitudeDesired * @@ -45,15 +45,14 @@ #include -#include "accels.h" #include "hwsettings.h" -#include "attitudeactual.h" +#include "attitudestate.h" #include "pathdesired.h" // object that will be updated by the module -#include "positionactual.h" +#include "positionstate.h" #include "manualcontrol.h" #include "flightstatus.h" #include "pathstatus.h" -#include "airspeedactual.h" +#include "airspeedstate.h" #include "gpsvelocity.h" #include "gpsposition.h" #include "fixedwingpathfollowersettings.h" @@ -63,7 +62,7 @@ #include "stabilizationsettings.h" #include "systemsettings.h" #include "velocitydesired.h" -#include "velocityactual.h" +#include "velocitystate.h" #include "taskinfo.h" #include "paths.h" @@ -87,7 +86,7 @@ static void SettingsUpdatedCb(UAVObjEvent *ev); static void updatePathVelocity(); static uint8_t updateFixedDesiredAttitude(); static void updateFixedAttitude(); -static void airspeedActualUpdatedCb(UAVObjEvent *ev); +static void airspeedStateUpdatedCb(UAVObjEvent *ev); static float bound(float val, float min, float max); /** @@ -121,7 +120,7 @@ int32_t FixedWingPathFollowerInitialize() PathDesiredInitialize(); PathStatusInitialize(); VelocityDesiredInitialize(); - AirspeedActualInitialize(); + AirspeedStateInitialize(); } else { followerEnabled = false; } @@ -139,7 +138,7 @@ static float powerIntegral = 0; static float airspeedErrorInt = 0; // correct speed by measured airspeed -static float indicatedAirspeedActualBias = 0; +static float indicatedAirspeedStateBias = 0; /** * Module thread, should not return. @@ -151,7 +150,7 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) portTickType lastUpdateTime; - AirspeedActualConnectCallback(airspeedActualUpdatedCb); + AirspeedStateConnectCallback(airspeedStateUpdatedCb); FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); @@ -247,21 +246,21 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) /** * Compute desired velocity from the current position and path * - * Takes in @ref PositionActual and compares it to @ref PathDesired + * Takes in @ref PositionState and compares it to @ref PathDesired * and computes @ref VelocityDesired */ static void updatePathVelocity() { - PositionActualData positionActual; + PositionStateData positionState; - PositionActualGet(&positionActual); - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); + PositionStateGet(&positionState); + VelocityStateData velocityState; + VelocityStateGet(&velocityState); // look ahead fixedwingpathfollowerSettings.HeadingFeedForward seconds - float cur[3] = { positionActual.North + (velocityActual.North * fixedwingpathfollowerSettings.HeadingFeedForward), - positionActual.East + (velocityActual.East * fixedwingpathfollowerSettings.HeadingFeedForward), - positionActual.Down + (velocityActual.Down * fixedwingpathfollowerSettings.HeadingFeedForward) }; + float cur[3] = { positionState.North + (velocityState.North * fixedwingpathfollowerSettings.HeadingFeedForward), + positionState.East + (velocityState.East * fixedwingpathfollowerSettings.HeadingFeedForward), + positionState.Down + (velocityState.Down * fixedwingpathfollowerSettings.HeadingFeedForward) }; struct path_status progress; path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); @@ -307,11 +306,11 @@ static void updatePathVelocity() // turn steep unless there is enough space complete the turn before crossing the flightpath // in this case the plane effectively needs to be turned around // indicators: - // difference between correction_direction and velocityactual >90 degrees and - // difference between path_direction and velocityactual >90 degrees ( 4th sector, facing away from eerything ) + // difference between correction_direction and velocitystate >90 degrees and + // difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from eerything ) // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) - float angle1 = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0]) - atan2f(velocityActual.East, velocityActual.North)); - float angle2 = RAD2DEG(atan2f(progress.correction_direction[1], progress.correction_direction[0]) - atan2f(velocityActual.East, velocityActual.North)); + float angle1 = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0]) - atan2f(velocityState.East, velocityState.North)); + float angle2 = RAD2DEG(atan2f(progress.correction_direction[1], progress.correction_direction[0]) - atan2f(velocityState.East, velocityState.North)); if (angle1 < -180.0f) { angle1 += 360.0f; } @@ -337,7 +336,7 @@ static void updatePathVelocity() velocityDesired.North *= groundspeed / l; velocityDesired.East *= groundspeed / l; - float downError = altitudeSetpoint - positionActual.Down; + float downError = altitudeSetpoint - positionState.Down; velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP; // update pathstatus @@ -370,9 +369,9 @@ static void updateFixedAttitude(float *attitude) /** * Compute desired attitude from the desired velocity * - * Takes in @ref NedActual which has the acceleration in the + * Takes in @ref NedState which has the acceleration in the * NED frame as the feedback term and then compares the - * @ref VelocityActual against the @ref VelocityDesired + * @ref VelocityState against the @ref VelocityDesired */ static uint8_t updateFixedDesiredAttitude() { @@ -381,17 +380,16 @@ static uint8_t updateFixedDesiredAttitude() float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; // Convert from [ms] to [s] VelocityDesiredData velocityDesired; - VelocityActualData velocityActual; + VelocityStateData velocityState; StabilizationDesiredData stabDesired; - AttitudeActualData attitudeActual; - AccelsData accels; + AttitudeStateData attitudeState; StabilizationSettingsData stabSettings; FixedWingPathFollowerStatusData fixedwingpathfollowerStatus; - AirspeedActualData airspeedActual; + AirspeedStateData airspeedState; - float groundspeedActual; + float groundspeedState; float groundspeedDesired; - float indicatedAirspeedActual; + float indicatedAirspeedState; float indicatedAirspeedDesired; float airspeedError; @@ -406,13 +404,12 @@ static uint8_t updateFixedDesiredAttitude() FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus); - VelocityActualGet(&velocityActual); + VelocityStateGet(&velocityState); StabilizationDesiredGet(&stabDesired); VelocityDesiredGet(&velocityDesired); - AttitudeActualGet(&attitudeActual); - AccelsGet(&accels); + AttitudeStateGet(&attitudeState); StabilizationSettingsGet(&stabSettings); - AirspeedActualGet(&airspeedActual); + AirspeedStateGet(&airspeedState); /** @@ -420,59 +417,59 @@ static uint8_t updateFixedDesiredAttitude() */ // Current ground speed - groundspeedActual = sqrtf(velocityActual.East * velocityActual.East + velocityActual.North * velocityActual.North); - // note that airspeedActualBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement, + groundspeedState = sqrtf(velocityState.East * velocityState.East + velocityState.North * velocityState.North); + // note that airspeedStateBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement, // but thanks to accelerometers, groundspeed reacts faster to changes in direction // than airspeed and gps sensors alone - indicatedAirspeedActual = groundspeedActual + indicatedAirspeedActualBias; + indicatedAirspeedState = groundspeedState + indicatedAirspeedStateBias; // Desired ground speed - groundspeedDesired = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); - indicatedAirspeedDesired = bound(groundspeedDesired + indicatedAirspeedActualBias, + groundspeedDesired = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); + indicatedAirspeedDesired = bound(groundspeedDesired + indicatedAirspeedStateBias, fixedwingpathfollowerSettings.BestClimbRateSpeed, fixedwingpathfollowerSettings.CruiseSpeed); // Airspeed error - airspeedError = indicatedAirspeedDesired - indicatedAirspeedActual; + airspeedError = indicatedAirspeedDesired - indicatedAirspeedState; // Vertical speed error descentspeedDesired = bound( velocityDesired.Down, -fixedwingpathfollowerSettings.VerticalVelMax, fixedwingpathfollowerSettings.VerticalVelMax); - descentspeedError = descentspeedDesired - velocityActual.Down; + descentspeedError = descentspeedDesired - velocityState.Down; // Error condition: wind speed is higher than maximum allowed speed. We are forced backwards! fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0; - if (groundspeedDesired - indicatedAirspeedActualBias <= 0) { + if (groundspeedDesired - indicatedAirspeedStateBias <= 0) { fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1; result = 0; } // Error condition: plane too slow or too fast fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0; - if (indicatedAirspeedActual > fixedwingpathfollowerSettings.AirSpeedMax) { + if (indicatedAirspeedState > fixedwingpathfollowerSettings.AirSpeedMax) { fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1; result = 0; } - if (indicatedAirspeedActual > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) { + if (indicatedAirspeedState > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) { fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1; result = 0; } - if (indicatedAirspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { // The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF + if (indicatedAirspeedState < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { // The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; result = 0; } - if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { // Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not + if (indicatedAirspeedState < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { // Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; result = 0; } - if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) { + if (indicatedAirspeedState < fixedwingpathfollowerSettings.StallSpeedDirty && 1) { fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; result = 0; } - if (indicatedAirspeedActual < 1e-6f) { + if (indicatedAirspeedState < 1e-6f) { // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes // also we cannot handle planes flying backwards, lets just wait until the nose drops fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; @@ -515,7 +512,7 @@ static uint8_t updateFixedDesiredAttitude() // Error condition: plane cannot hold altitude at current speed. fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0; if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] && // throttle at maximum - velocityActual.Down > 0 && // we ARE going down + velocityState.Down > 0 && // we ARE going down descentspeedDesired < 0 && // we WANT to go up airspeedError > 0) { // we are too slow already fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1; @@ -524,7 +521,7 @@ static uint8_t updateFixedDesiredAttitude() // Error condition: plane keeps climbing despite minimum throttle (opposite of above) fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0; if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] && // throttle at minimum - velocityActual.Down < 0 && // we ARE going up + velocityState.Down < 0 && // we ARE going up descentspeedDesired > 0 && // we WANT to go down airspeedError < 0) { // we are too fast already fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1; @@ -565,7 +562,7 @@ static uint8_t updateFixedDesiredAttitude() // Error condition: high speed dive fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0; if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] && // pitch demand is full up - velocityActual.Down > 0 && // we ARE going down + velocityState.Down > 0 && // we ARE going down descentspeedDesired < 0 && // we WANT to go up airspeedError < 0) { // we are too fast already fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1; @@ -576,7 +573,7 @@ static uint8_t updateFixedDesiredAttitude() * Compute desired roll command */ if (groundspeedDesired > 1e-6f) { - bearingError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityActual.East, velocityActual.North)); + bearingError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North)); } else { // if we are not supposed to move, run in a circle bearingError = -90.0f; @@ -645,17 +642,17 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) PathDesiredGet(&pathDesired); } -static void airspeedActualUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - AirspeedActualData airspeedActual; - VelocityActualData velocityActual; + AirspeedStateData airspeedState; + VelocityStateData velocityState; - AirspeedActualGet(&airspeedActual); - VelocityActualGet(&velocityActual); - float groundspeed = sqrtf(velocityActual.East * velocityActual.East + velocityActual.North * velocityActual.North); + AirspeedStateGet(&airspeedState); + VelocityStateGet(&velocityState); + float groundspeed = sqrtf(velocityState.East * velocityState.East + velocityState.North * velocityState.North); - indicatedAirspeedActualBias = airspeedActual.CalibratedAirspeed - groundspeed; + indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeed; // note - we do fly by Indicated Airspeed (== calibrated airspeed) // however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement. } diff --git a/flight/modules/MK/MKSerial/MKSerial.c b/flight/modules/MK/MKSerial/MKSerial.c index 53f19d737..b1d99edc4 100644 --- a/flight/modules/MK/MKSerial/MKSerial.c +++ b/flight/modules/MK/MKSerial/MKSerial.c @@ -31,8 +31,8 @@ #include "openpilot.h" #include "MkSerial.h" -#include "attitudeactual.h" // object that will be updated by the module -#include "positionactual.h" +#include "attitudestate.h" // object that will be updated by the module +#include "positionstate.h" #include "flightbatterystate.h" // @@ -411,7 +411,7 @@ static uint16_t VersionMsg_GetVersion(const MkMsg_t *msg) static void DoConnectedToFC(void) { - AttitudeActualData attitudeData; + AttitudeStateData attitudeData; MkMsg_t msg; DEBUG_MSG("FC\n\r"); @@ -434,7 +434,7 @@ static void DoConnectedToFC(void) attitudeData.Pitch = -(float)nick / 10; attitudeData.Roll = -(float)roll / 10; - AttitudeActualSet(&attitudeData); + AttitudeStateSet(&attitudeData); } else { DEBUG_MSG("TO\n\r"); break; @@ -446,8 +446,8 @@ static void DoConnectedToNC(void) { MkMsg_t msg; GpsPosition_t pos; - AttitudeActualData attitudeData; - PositionActualData positionData; + AttitudeStateData attitudeData; + PositionStateData positionData; FlightBatteryStateData flightBatteryData; #ifdef GENERATE_BATTERY_INFO @@ -478,7 +478,7 @@ static void DoConnectedToNC(void) #endif attitudeData.Pitch = -Par2Int8(&msg, OSD_MSG_NICK_IDX); attitudeData.Roll = -Par2Int8(&msg, OSD_MSG_ROLL_IDX); - AttitudeActualSet(&attitudeData); + AttitudeStateSet(&attitudeData); positionData.Longitude = pos.longitute; positionData.Latitude = pos.latitude; @@ -491,7 +491,7 @@ static void DoConnectedToNC(void) } else { positionData.Status = POSITIONACTUAL_STATUS_FIX3D; } - PositionActualSet(&positionData); + PositionStateSet(&positionData); #if GENERATE_BATTERY_INFO if (++battStateCnt > 2) { diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index aac310600..21f42e2d4 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -38,14 +38,13 @@ #include "accessorydesired.h" #include "actuatordesired.h" #include "altitudeholddesired.h" -#include "baroaltitude.h" #include "flighttelemetrystats.h" #include "flightstatus.h" #include "sanitycheck.h" #include "manualcontrol.h" #include "manualcontrolsettings.h" #include "manualcontrolcommand.h" -#include "positionactual.h" +#include "positionstate.h" #include "pathdesired.h" #include "stabilizationsettings.h" #include "stabilizationdesired.h" @@ -722,34 +721,34 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * if (home && changed) { // Simple Return To Base mode - keep altitude the same, fly to home position - PositionActualData positionActual; - PositionActualGet(&positionActual); + PositionStateData positionState; + PositionStateGet(&positionState); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); pathDesired.Start[PATHDESIRED_START_NORTH] = 0; pathDesired.Start[PATHDESIRED_START_EAST] = 0; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down - 10; pathDesired.End[PATHDESIRED_END_NORTH] = 0; pathDesired.End[PATHDESIRED_END_EAST] = 0; - pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10; + pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down - 10; pathDesired.StartingVelocity = 1; pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); } else if (changed) { // After not being in this mode for a while init at current height - PositionActualData positionActual; - PositionActualGet(&positionActual); + PositionStateData positionState; + PositionStateGet(&positionState); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; - pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; - pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; - pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; + pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down; + pathDesired.End[PATHDESIRED_END_NORTH] = positionState.North; + pathDesired.End[PATHDESIRED_END_EAST] = positionState.East; + pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down; pathDesired.StartingVelocity = 1; pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; @@ -778,25 +777,25 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * lastSysTime = thisSysTime; */ - PositionActualData positionActual; + PositionStateData positionState; - PositionActualGet(&positionActual); + PositionStateGet(&positionState); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); if (changed) { // After not being in this mode for a while init at current height - pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; - pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; - pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; - pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; + pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down; + pathDesired.End[PATHDESIRED_END_NORTH] = positionState.North; + pathDesired.End[PATHDESIRED_END_EAST] = positionState.East; + pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down; pathDesired.StartingVelocity = 1; pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; } - pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down + 5; + pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down + 5; PathDesiredSet(&pathDesired); } @@ -839,7 +838,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; float currentDown; - PositionActualDownGet(¤tDown); + PositionStateDownGet(¤tDown); if (changed) { // After not being in this mode for a while init at current height altitudeHoldDesiredData.Altitude = 0; diff --git a/flight/modules/Osd/OsdEtStd/OsdEtStd.c b/flight/modules/Osd/OsdEtStd/OsdEtStd.c index 363f125e1..29b04c4cc 100644 --- a/flight/modules/Osd/OsdEtStd/OsdEtStd.c +++ b/flight/modules/Osd/OsdEtStd/OsdEtStd.c @@ -33,8 +33,8 @@ #include "flightbatterystate.h" #include "gpsposition.h" -#include "attitudeactual.h" -#include "baroaltitude.h" +#include "attitudestate.h" +#include "barosensor.h" // // Configuration @@ -168,7 +168,7 @@ static void SetCourse(uint16_t dir) WriteToMsg16(OSDMSG_HOME_IDX, dir); } -static void SetBaroAltitude(int16_t altitudeMeter) +static void SetBaroSensor(int16_t altitudeMeter) { // calculated formula // ET OSD uses first update as zeropoint and then +- from that @@ -214,7 +214,7 @@ static void GPSPositionUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) newPosData = TRUE; } -static void BaroAltitudeUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +static void BaroSensorUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { newBaroData = TRUE; } @@ -441,10 +441,10 @@ static void Run(void) if (newPosData) { GPSPositionData positionData; - AttitudeActualData attitudeActualData; + AttitudeStateData attitudeStateData; GPSPositionGet(&positionData); - AttitudeActualGet(&attitudeActualData); + AttitudeStateGet(&attitudeStateData); // DEBUG_MSG("%5d Pos: #stat=%d #sats=%d alt=%d\n\r", cnt, // positionData.Status, positionData.Satellites, (uint32_t)positionData.Altitude); @@ -462,17 +462,17 @@ static void Run(void) SetCoord(OSDMSG_LON_IDX, positionData.Longitude); SetAltitude(positionData.Altitude); SetNbSats(positionData.Satellites); - SetCourse(attitudeActualData.Yaw); + SetCourse(attitudeStateData.Yaw); newPosData = FALSE; } else { msg[OSDMSG_GPS_STAT] &= ~OSDMSG_GPS_STAT_HB_FLAG; } if (newBaroData) { - BaroAltitudeData baroData; + BaroSensorData baroData; - BaroAltitudeGet(&baroData); - SetBaroAltitude(baroData.Altitude); + BaroSensorGet(&baroData); + SetBaroSensor(baroData.Altitude); newBaroData = FALSE; } @@ -545,7 +545,7 @@ int32_t OsdEtStdInitialize(void) { GPSPositionConnectCallback(GPSPositionUpdatedCb); FlightBatteryStateConnectCallback(FlightBatteryStateUpdatedCb); - BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb); + BaroSensorConnectCallback(BaroSensorUpdatedCb); memset(&ev, 0, sizeof(UAVObjEvent)); EventPeriodicCallbackCreate(&ev, onTimer, 100 / portTICK_RATE_MS); diff --git a/flight/modules/Osd/osdgen/osdgen.c b/flight/modules/Osd/osdgen/osdgen.c index 654b0aeef..23539bcc7 100644 --- a/flight/modules/Osd/osdgen/osdgen.c +++ b/flight/modules/Osd/osdgen/osdgen.c @@ -34,13 +34,13 @@ #include "osdgen.h" -#include "attitudeactual.h" +#include "attitudestate.h" #include "gpsposition.h" #include "homelocation.h" #include "gpstime.h" #include "gpssatellites.h" #include "osdsettings.h" -#include "baroaltitude.h" +#include "barosensor.h" #include "taskinfo.h" #include "flightstatus.h" @@ -2137,14 +2137,14 @@ void updateGraphics() OsdSettingsData OsdSettings; OsdSettingsGet(&OsdSettings); - AttitudeActualData attitude; - AttitudeActualGet(&attitude); + AttitudeStateData attitude; + AttitudeStateGet(&attitude); GPSPositionData gpsData; GPSPositionGet(&gpsData); HomeLocationData home; HomeLocationGet(&home); - BaroAltitudeData baro; - BaroAltitudeGet(&baro); + BaroSensorData baro; + BaroSensorGet(&baro); FlightStatusData status; FlightStatusGet(&status); @@ -2419,7 +2419,7 @@ int32_t osdgenStart(void) */ int32_t osdgenInitialize(void) { - AttitudeActualInitialize(); + AttitudeStateInitialize(); #ifdef PIOS_INCLUDE_GPS GPSPositionInitialize(); #if !defined(PIOS_GPS_MINIMAL) @@ -2431,7 +2431,7 @@ int32_t osdgenInitialize(void) #endif #endif OsdSettingsInitialize(); - BaroAltitudeInitialize(); + BaroSensorInitialize(); FlightStatusInitialize(); return 0; diff --git a/flight/modules/Osd/osdinput/osdinput.c b/flight/modules/Osd/osdinput/osdinput.c index 6387eeb27..db51c2a8f 100644 --- a/flight/modules/Osd/osdinput/osdinput.c +++ b/flight/modules/Osd/osdinput/osdinput.c @@ -34,7 +34,7 @@ #include "osdinput.h" -#include "attitudeactual.h" +#include "attitudestate.h" #include "taskinfo.h" #include "flightstatus.h" @@ -86,11 +86,11 @@ int32_t osdinputStart(void) */ int32_t osdinputInitialize(void) { - AttitudeActualInitialize(); + AttitudeStateInitialize(); FlightStatusInitialize(); // Initialize quaternion - AttitudeActualData attitude; - AttitudeActualGet(&attitude); + AttitudeStateData attitude; + AttitudeStateGet(&attitude); attitude.q1 = 1; attitude.q2 = 0; attitude.q3 = 0; @@ -98,7 +98,7 @@ int32_t osdinputInitialize(void) attitude.Roll = 0; attitude.Pitch = 0; attitude.Yaw = 0; - AttitudeActualSet(&attitude); + AttitudeStateSet(&attitude); oposdPort = PIOS_COM_OSD; @@ -148,8 +148,8 @@ static void osdinputTask(__attribute__((unused)) void *parameters) } if (rx_count == 11) { if (oposd_rx_buffer[1] == OSD_PKT_TYPE_ATT) { - AttitudeActualData attitude; - AttitudeActualGet(&attitude); + AttitudeStateData attitude; + AttitudeStateGet(&attitude); attitude.q1 = 1; attitude.q2 = 0; attitude.q3 = 0; @@ -157,7 +157,7 @@ static void osdinputTask(__attribute__((unused)) void *parameters) attitude.Roll = (float)((int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4] << 8)) / 10.0f; attitude.Pitch = (float)((int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6] << 8)) / 10.0f; attitude.Yaw = (float)((int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8] << 8)) / 10.0f; - AttitudeActualSet(&attitude); + AttitudeStateSet(&attitude); } else if (oposd_rx_buffer[1] == OSD_PKT_TYPE_MODE) { FlightStatusData status; FlightStatusGet(&status); diff --git a/flight/modules/Osd/osdoutput/osdoutput.c b/flight/modules/Osd/osdoutput/osdoutput.c index e6e5ff023..f21135661 100644 --- a/flight/modules/Osd/osdoutput/osdoutput.c +++ b/flight/modules/Osd/osdoutput/osdoutput.c @@ -35,11 +35,11 @@ #endif #if POSITIONACTUAL_SUPPORTED -#include "positionactual.h" +#include "positionstate.h" #endif #include "systemalarms.h" -#include "attitudeactual.h" +#include "attitudestate.h" #include "hwsettings.h" #include "flightstatus.h" @@ -117,7 +117,7 @@ struct osd_hk_msg { static struct osd_hk_msg osd_hk_msg_buf; -static volatile bool newPositionActualData = false; +static volatile bool newPositionStateData = false; static volatile bool newBattData = false; static volatile bool newAttitudeData = false; static volatile bool newAlarmData = false; @@ -155,15 +155,15 @@ static void send_update(__attribute__((unused)) UAVObjEvent *ev) case OSD_HK_PKT_TYPE_ATT: msg->t = OSD_HK_PKT_TYPE_ATT; float roll; - AttitudeActualRollGet(&roll); + AttitudeStateRollGet(&roll); blob->att.roll = (int16_t)(roll * 10); float pitch; - AttitudeActualPitchGet(&pitch); + AttitudeStatePitchGet(&pitch); blob->att.pitch = (int16_t)(pitch * 10); float yaw; - AttitudeActualYawGet(&yaw); + AttitudeStateYawGet(&yaw); blob->att.yaw = (int16_t)(yaw * 10); break; case OSD_HK_PKT_TYPE_MODE: @@ -217,9 +217,9 @@ static void send_update(__attribute__((unused)) UAVObjEvent *ev) #endif #if POSITIONACTUAL_SUPPORTED - if (newPositionActualData) { - PositionActualData position; - PositionActualGet(&position); + if (newPositionStateData) { + PositionStateData position; + PositionStateGet(&position); /* compute 3D distance */ float d = sqrt( @@ -234,7 +234,7 @@ static void send_update(__attribute__((unused)) UAVObjEvent *ev) (home & 0xFF00 >> 8) | (home & 0x00FF << 8)); - newPositionActualData = false; + newPositionStateData = false; } #else // blob->misc.home = 0; diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 30bd42fa3..9d59f3b27 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -32,12 +32,12 @@ #include "openpilot.h" #include "flightstatus.h" -#include "airspeedactual.h" +#include "airspeedstate.h" #include "pathaction.h" #include "pathdesired.h" #include "pathstatus.h" -#include "positionactual.h" -#include "velocityactual.h" +#include "positionstate.h" +#include "velocitystate.h" #include "waypoint.h" #include "waypointactive.h" #include "taskinfo.h" @@ -102,9 +102,9 @@ int32_t PathPlannerInitialize() PathActionInitialize(); PathStatusInitialize(); PathDesiredInitialize(); - PositionActualInitialize(); - AirspeedActualInitialize(); - VelocityActualInitialize(); + PositionStateInitialize(); + AirspeedStateInitialize(); + VelocityStateInitialize(); WaypointInitialize(); WaypointActiveInitialize(); @@ -239,16 +239,16 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev) pathDesired.UID = waypointActiveData.Index; if (waypointActiveData.Index == 0) { - PositionActualData positionActual; - PositionActualGet(&positionActual); + PositionStateData positionState; + PositionStateGet(&positionState); // First waypoint has itself as start point (used to be home position but that proved dangerous when looping) /*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST]; pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/ - pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; + pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down; pathDesired.StartingVelocity = pathDesired.EndingVelocity; } else { // Get previous waypoint as start point @@ -365,16 +365,16 @@ static uint8_t conditionTimeOut() static uint8_t conditionDistanceToTarget() { float distance; - PositionActualData positionActual; + PositionStateData positionState; - PositionActualGet(&positionActual); + PositionStateGet(&positionState); if (pathAction.ConditionParameters[1] > 0.5f) { - distance = sqrtf(powf(waypoint.Position[0] - positionActual.North, 2) - + powf(waypoint.Position[1] - positionActual.East, 2) - + powf(waypoint.Position[1] - positionActual.Down, 2)); + distance = sqrtf(powf(waypoint.Position[0] - positionState.North, 2) + + powf(waypoint.Position[1] - positionState.East, 2) + + powf(waypoint.Position[1] - positionState.Down, 2)); } else { - distance = sqrtf(powf(waypoint.Position[0] - positionActual.North, 2) - + powf(waypoint.Position[1] - positionActual.East, 2)); + distance = sqrtf(powf(waypoint.Position[0] - positionState.North, 2) + + powf(waypoint.Position[1] - positionState.East, 2)); } if (distance <= pathAction.ConditionParameters[0]) { @@ -392,12 +392,12 @@ static uint8_t conditionDistanceToTarget() static uint8_t conditionLegRemaining() { PathDesiredData pathDesired; - PositionActualData positionActual; + PositionStateData positionState; PathDesiredGet(&pathDesired); - PositionActualGet(&positionActual); + PositionStateGet(&positionState); - float cur[3] = { positionActual.North, positionActual.East, positionActual.Down }; + float cur[3] = { positionState.North, positionState.East, positionState.Down }; struct path_status progress; path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); @@ -415,12 +415,12 @@ static uint8_t conditionLegRemaining() static uint8_t conditionBelowError() { PathDesiredData pathDesired; - PositionActualData positionActual; + PositionStateData positionState; PathDesiredGet(&pathDesired); - PositionActualGet(&positionActual); + PositionStateGet(&positionState); - float cur[3] = { positionActual.North, positionActual.East, positionActual.Down }; + float cur[3] = { positionState.North, positionState.East, positionState.Down }; struct path_status progress; path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); @@ -438,11 +438,11 @@ static uint8_t conditionBelowError() */ static uint8_t conditionAboveAltitude() { - PositionActualData positionActual; + PositionStateData positionState; - PositionActualGet(&positionActual); + PositionStateGet(&positionState); - if (positionActual.Down <= pathAction.ConditionParameters[0]) { + if (positionState.Down <= pathAction.ConditionParameters[0]) { return true; } return false; @@ -456,15 +456,15 @@ static uint8_t conditionAboveAltitude() */ static uint8_t conditionAboveSpeed() { - VelocityActualData velocityActual; + VelocityStateData velocityState; - VelocityActualGet(&velocityActual); - float velocity = sqrtf(velocityActual.North * velocityActual.North + velocityActual.East * velocityActual.East + velocityActual.Down * velocityActual.Down); + VelocityStateGet(&velocityState); + float velocity = sqrtf(velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down); // use airspeed if requested and available if (pathAction.ConditionParameters[1] > 0.5f) { - AirspeedActualData airspeed; - AirspeedActualGet(&airspeed); + AirspeedStateData airspeed; + AirspeedStateGet(&airspeed); velocity = airspeed.CalibratedAirspeed; } @@ -494,8 +494,8 @@ static uint8_t conditionPointingTowardsNext() float angle1 = atan2f((nextWaypoint.Position[0] - waypoint.Position[0]), (nextWaypoint.Position[1] - waypoint.Position[1])); - VelocityActualData velocity; - VelocityActualGet(&velocity); + VelocityStateData velocity; + VelocityStateGet(&velocity); float angle2 = atan2f(velocity.North, velocity.East); // calculate the absolute angular difference diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index ddfbf29c7..bfbe62f18 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -4,7 +4,7 @@ * @{ * @addtogroup Sensors * @brief Acquires sensor data - * Specifically updates the the @ref Gyros, @ref Accels, and @ref Magnetometer objects + * Specifically updates the the @ref GyroSensor, @ref AccelSensor, and @ref MagnetoSensor objects * @{ * * @file sensors.c @@ -32,7 +32,7 @@ /** * Input objects: None, takes sensor data via pios - * Output objects: @ref Gyros @ref Accels @ref Magnetometer + * Output objects: @ref GyroSensor @ref AccelSensor @ref MagnetoSensor * * The module executes in its own thread. * @@ -49,12 +49,10 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -75,14 +73,13 @@ // Private functions static void SensorsTask(void *parameters); static void settingsUpdatedCb(UAVObjEvent *objEv); -static void magOffsetEstimation(MagnetometerData *mag); +// static void magOffsetEstimation(MagnetoSensorData *mag); // Private variables static xTaskHandle sensorsTaskHandle; RevoCalibrationData cal; // These values are initialized by settings but can be updated by the attitude algorithm -static bool bias_correct_gyro = true; static float mag_bias[3] = { 0, 0, 0 }; static float mag_scale[3] = { 0, 0, 0 }; @@ -111,11 +108,9 @@ static int8_t rotate = 0; */ int32_t SensorsInitialize(void) { - GyrosInitialize(); - GyrosBiasInitialize(); - AccelsInitialize(); - MagnetometerInitialize(); - MagBiasInitialize(); + GyroSensorInitialize(); + AccelSensorInitialize(); + MagnetoSensorInitialize(); RevoCalibrationInitialize(); AttitudeSettingsInitialize(); @@ -238,8 +233,8 @@ static void SensorsTask(__attribute__((unused)) void *parameters) accel_samples = 0; gyro_samples = 0; - AccelsData accelsData; - GyrosData gyrosData; + AccelSensorData accelSensorData; + GyroSensorData gyroSensorData; switch (bdinfo->board_rev) { case 0x01: // L3GD20 + BMA180 board @@ -275,7 +270,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters) accel_scaling = PIOS_BMA180_GetScale(); // Get temp from last reading - accelsData.temperature = 25.0f + ((float)accel.temperature - 2.0f) / 2.0f; + accelSensorData.temperature = 25.0f + ((float)accel.temperature - 2.0f) / 2.0f; } #endif /* if defined(PIOS_INCLUDE_BMA180) */ #if defined(PIOS_INCLUDE_L3GD20) @@ -297,7 +292,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters) gyro_scaling = PIOS_L3GD20_GetScale(); // Get temp from last reading - gyrosData.temperature = gyro.temperature; + gyroSensorData.temperature = gyro.temperature; } #endif /* if defined(PIOS_INCLUDE_L3GD20) */ break; @@ -330,8 +325,8 @@ static void SensorsTask(__attribute__((unused)) void *parameters) gyro_scaling = PIOS_MPU6000_GetScale(); accel_scaling = PIOS_MPU6000_GetAccelScale(); - gyrosData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; - accelsData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; + gyroSensorData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; + accelSensorData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; } #endif /* PIOS_INCLUDE_MPU6000 */ break; @@ -348,15 +343,15 @@ static void SensorsTask(__attribute__((unused)) void *parameters) accels[2] * accel_scaling * accel_scale[2] - accel_bias[2] }; if (rotate) { rot_mult(R, accels_out, accels); - accelsData.x = accels[0]; - accelsData.y = accels[1]; - accelsData.z = accels[2]; + accelSensorData.x = accels[0]; + accelSensorData.y = accels[1]; + accelSensorData.z = accels[2]; } else { - accelsData.x = accels_out[0]; - accelsData.y = accels_out[1]; - accelsData.z = accels_out[2]; + accelSensorData.x = accels_out[0]; + accelSensorData.y = accels_out[1]; + accelSensorData.z = accels_out[2]; } - AccelsSet(&accelsData); + AccelSensorSet(&accelSensorData); // Scale the gyros float gyros[3] = { (float)gyro_accum[0] / gyro_samples, @@ -367,30 +362,30 @@ static void SensorsTask(__attribute__((unused)) void *parameters) gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2] }; if (rotate) { rot_mult(R, gyros_out, gyros); - gyrosData.x = gyros[0]; - gyrosData.y = gyros[1]; - gyrosData.z = gyros[2]; + gyroSensorData.x = gyros[0]; + gyroSensorData.y = gyros[1]; + gyroSensorData.z = gyros[2]; } else { - gyrosData.x = gyros_out[0]; - gyrosData.y = gyros_out[1]; - gyrosData.z = gyros_out[2]; + gyroSensorData.x = gyros_out[0]; + gyroSensorData.y = gyros_out[1]; + gyroSensorData.z = gyros_out[2]; } - if (bias_correct_gyro) { +/* TODO if (bias_correct_gyro) { // Apply bias correction to the gyros from the state estimator GyrosBiasData gyrosBias; GyrosBiasGet(&gyrosBias); gyrosData.x -= gyrosBias.x; gyrosData.y -= gyrosBias.y; gyrosData.z -= gyrosBias.z; - } - GyrosSet(&gyrosData); + }*/ + GyroSensorSet(&gyroSensorData); // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) #if defined(PIOS_INCLUDE_HMC5883) - MagnetometerData mag; + MagnetoSensorData mag; if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { int16_t values[3]; PIOS_HMC5883_ReadMag(values); @@ -410,11 +405,14 @@ static void SensorsTask(__attribute__((unused)) void *parameters) } // Correct for mag bias and update if the rate is non zero +// TODO +/* if (cal.MagBiasNullingRate > 0) { magOffsetEstimation(&mag); } + */ - MagnetometerSet(&mag); + MagnetoSensorSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); } #endif /* if defined(PIOS_INCLUDE_HMC5883) */ @@ -430,9 +428,11 @@ static void SensorsTask(__attribute__((unused)) void *parameters) * Magnetometer Offset Cancellation: Theory and Implementation, * revisited William Premerlani, October 14, 2011 */ -static void magOffsetEstimation(MagnetometerData *mag) -{ -#if 0 +// TODO +/* + static void magOffsetEstimation(MagnetoSensorData *mag) + { + #if 0 // Constants, to possibly go into a UAVO static const float MIN_NORM_DIFFERENCE = 50; @@ -471,7 +471,7 @@ static void magOffsetEstimation(MagnetometerData *mag) // Store this value to compare against next update B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; } -#else /* if 0 */ + #else // if 0 MagBiasData magBias; MagBiasGet(&magBias); @@ -483,8 +483,8 @@ static void magOffsetEstimation(MagnetometerData *mag) HomeLocationData homeLocation; HomeLocationGet(&homeLocation); - AttitudeActualData attitude; - AttitudeActualGet(&attitude); + AttitudeStateData attitude; + AttitudeStateGet(&attitude); const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]); const float Rz = homeLocation.Be[2]; @@ -523,8 +523,9 @@ static void magOffsetEstimation(MagnetometerData *mag) magBias.z += delta[2]; MagBiasSet(&magBias); } -#endif /* if 0 */ -} + #endif // if 0 + } + */ /** * Locally cache some variables from the AtttitudeSettings object @@ -552,18 +553,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y]; gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z]; - // Zero out any adaptive tracking - MagBiasData magBias; - MagBiasGet(&magBias); - magBias.x = 0; - magBias.y = 0; - magBias.z = 0; - MagBiasSet(&magBias); - AttitudeSettingsData attitudeSettings; AttitudeSettingsGet(&attitudeSettings); - bias_correct_gyro = (cal.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE); // Indicates not to expend cycles on rotation if (attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && diff --git a/flight/modules/Sensors/simulated/sensors.c b/flight/modules/Sensors/simulated/sensors.c index 6f973f2f5..9fea56af1 100644 --- a/flight/modules/Sensors/simulated/sensors.c +++ b/flight/modules/Sensors/simulated/sensors.c @@ -4,7 +4,7 @@ * @{ * @addtogroup Sensors * @brief Acquires sensor data - * Specifically updates the the @ref Gyros, @ref Accels, and @ref Magnetometer objects + * Specifically updates the the @ref GyroSensor, @ref AccelSensor, and @ref MagnetoSensor objects * @{ * * @file sensors.c @@ -32,7 +32,7 @@ /** * Input objects: None, takes sensor data via pios - * Output objects: @ref Gyros @ref Accels @ref Magnetometer + * Output objects: @ref GyroSensor @ref AccelSensor @ref MagnetoSensor * * The module executes in its own thread. * @@ -49,21 +49,19 @@ #include #include "attitude.h" -#include "accels.h" +#include "accelsensor.h" #include "actuatordesired.h" -#include "attitudeactual.h" +#include "attitudestate.h" #include "attitudesimulated.h" #include "attitudesettings.h" #include "rawairspeed.h" -#include "baroaltitude.h" -#include "gyros.h" -#include "gyrosbias.h" +#include "barosensor.h" +#include "gyrosensor.h" #include "flightstatus.h" #include "gpsposition.h" #include "gpsvelocity.h" #include "homelocation.h" -#include "magnetometer.h" -#include "magbias.h" +#include "sensor.h" #include "ratedesired.h" #include "revocalibration.h" #include "systemsettings.h" @@ -90,8 +88,6 @@ static void simulateModelAgnostic(); static void simulateModelQuadcopter(); static void simulateModelAirplane(); -static void magOffsetEstimation(MagnetometerData *mag); - static float accel_bias[3]; static float rand_gauss(); @@ -109,15 +105,15 @@ int32_t SensorsInitialize(void) accel_bias[1] = rand_gauss() / 10; accel_bias[2] = rand_gauss() / 10; - AccelsInitialize(); + AccelSensorInitialize(); AttitudeSimulatedInitialize(); - BaroAltitudeInitialize(); + BaroSensorInitialize(); AirspeedSensorInitialize(); - GyrosInitialize(); - GyrosBiasInitialize(); + GyroSensorInitialize(); + // GyrosBiasInitialize(); GPSPositionInitialize(); GPSVelocityInitialize(); - MagnetometerInitialize(); + MagnetoSensorInitialize(); MagBiasInitialize(); RevoCalibrationInitialize(); @@ -218,32 +214,34 @@ static void SensorsTask(__attribute__((unused)) void *parameters) static void simulateConstant() { - AccelsData accelsData; // Skip get as we set all the fields + AccelSensorData accelSensorData; // Skip get as we set all the fields - accelsData.x = 0; - accelsData.y = 0; - accelsData.z = -GRAV; - accelsData.temperature = 0; - AccelsSet(&accelsData); + accelSensorData.x = 0; + accelSensorData.y = 0; + accelSensorData.z = -GRAV; + accelSensorData.temperature = 0; + AccelSensorSet(&accelSensorData); - GyrosData gyrosData; // Skip get as we set all the fields - gyrosData.x = 0; - gyrosData.y = 0; - gyrosData.z = 0; + GyroSensorData gyroSensorData; // Skip get as we set all the fields + gyroSensorData.x = 0; + gyroSensorData.y = 0; + gyroSensorData.z = 0; +/* TODO // Apply bias correction to the gyros GyrosBiasData gyrosBias; GyrosBiasGet(&gyrosBias); - gyrosData.x += gyrosBias.x; - gyrosData.y += gyrosBias.y; - gyrosData.z += gyrosBias.z; + gyroSensorData.x += gyrosBias.x; + gyroSensorData.y += gyrosBias.y; + gyroSensorData.z += gyrosBias.z; + */ - GyrosSet(&gyrosData); + GyroSensorSet(&gyroSensorData); - BaroAltitudeData baroAltitude; - BaroAltitudeGet(&baroAltitude); - baroAltitude.Altitude = 1; - BaroAltitudeSet(&baroAltitude); + BaroSensorData baroSensor; + BaroSensorGet(&baroSensor); + baroSensor.Altitude = 1; + BaroSensorSet(&baroSensor); GPSPositionData gpsPosition; GPSPositionGet(&gpsPosition); @@ -254,11 +252,11 @@ static void simulateConstant() // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) - MagnetometerData mag; + MagnetoSensorData mag; mag.x = 400; mag.y = 0; mag.z = 800; - MagnetometerSet(&mag); + MagnetoSensorSet(&mag); } static void simulateModelAgnostic() @@ -267,43 +265,45 @@ static void simulateModelAgnostic() float q[4]; // Simulate accels based on current attitude - AttitudeActualData attitudeActual; + AttitudeStateData attitudeState; - AttitudeActualGet(&attitudeActual); - q[0] = attitudeActual.q1; - q[1] = attitudeActual.q2; - q[2] = attitudeActual.q3; - q[3] = attitudeActual.q4; + AttitudeStateGet(&attitudeState); + q[0] = attitudeState.q1; + q[1] = attitudeState.q2; + q[2] = attitudeState.q3; + q[3] = attitudeState.q4; Quaternion2R(q, Rbe); - AccelsData accelsData; // Skip get as we set all the fields - accelsData.x = -GRAV * Rbe[0][2]; - accelsData.y = -GRAV * Rbe[1][2]; - accelsData.z = -GRAV * Rbe[2][2]; - accelsData.temperature = 30; - AccelsSet(&accelsData); + AccelSensorData accelSensorData; // Skip get as we set all the fields + accelSensorData.x = -GRAV * Rbe[0][2]; + accelSensorData.y = -GRAV * Rbe[1][2]; + accelSensorData.z = -GRAV * Rbe[2][2]; + accelSensorData.temperature = 30; + AccelSensorSet(&accelSensorData); RateDesiredData rateDesired; RateDesiredGet(&rateDesired); - GyrosData gyrosData; // Skip get as we set all the fields - gyrosData.x = rateDesired.Roll + rand_gauss(); - gyrosData.y = rateDesired.Pitch + rand_gauss(); - gyrosData.z = rateDesired.Yaw + rand_gauss(); + GyroSensorData gyroSensorData; // Skip get as we set all the fields + gyroSensorData.x = rateDesired.Roll + rand_gauss(); + gyroSensorData.y = rateDesired.Pitch + rand_gauss(); + gyroSensorData.z = rateDesired.Yaw + rand_gauss(); +/* TODO // Apply bias correction to the gyros GyrosBiasData gyrosBias; GyrosBiasGet(&gyrosBias); - gyrosData.x += gyrosBias.x; - gyrosData.y += gyrosBias.y; - gyrosData.z += gyrosBias.z; + gyroSensorData.x += gyrosBias.x; + gyroSensorData.y += gyrosBias.y; + gyroSensorData.z += gyrosBias.z; + */ - GyrosSet(&gyrosData); + GyroSensorSet(&gyroSensorData); - BaroAltitudeData baroAltitude; - BaroAltitudeGet(&baroAltitude); - baroAltitude.Altitude = 1; - BaroAltitudeSet(&baroAltitude); + BaroSensorData baroSensor; + BaroSensorGet(&baroSensor); + baroSensor.Altitude = 1; + BaroSensorSet(&baroSensor); GPSPositionData gpsPosition; GPSPositionGet(&gpsPosition); @@ -314,11 +314,11 @@ static void simulateModelAgnostic() // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) - MagnetometerData mag; + MagnetoSensorData mag; mag.x = 400; mag.y = 0; mag.z = 800; - MagnetometerSet(&mag); + MagnetoSensorSet(&mag); } float thrustToDegs = 50; @@ -369,10 +369,10 @@ static void simulateModelQuadcopter() // rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; // rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; // -// GyrosData gyrosData; // Skip get as we set all the fields -// gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss(); -// gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss(); -// gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss(); +// GyrosData gyroSensorData; // Skip get as we set all the fields +// gyroSensorData.x = rpy[0] * 180 / M_PI + rand_gauss(); +// gyroSensorData.y = rpy[1] * 180 / M_PI + rand_gauss(); +// gyroSensorData.z = rpy[2] * 180 / M_PI + rand_gauss(); RateDesiredData rateDesired; RateDesiredGet(&rateDesired); @@ -381,11 +381,11 @@ static void simulateModelQuadcopter() rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; - GyrosData gyrosData; // Skip get as we set all the fields - gyrosData.x = rpy[0] + rand_gauss(); - gyrosData.y = rpy[1] + rand_gauss(); - gyrosData.z = rpy[2] + rand_gauss(); - GyrosSet(&gyrosData); + GyroSensorData gyroSensorData; // Skip get as we set all the fields + gyroSensorData.x = rpy[0] + rand_gauss(); + gyroSensorData.y = rpy[1] + rand_gauss(); + gyroSensorData.z = rpy[2] + rand_gauss(); + GyroSensorSet(&gyroSensorData); // Predict the attitude forward in time float qdot[4]; @@ -407,13 +407,13 @@ static void simulateModelQuadcopter() q[3] = q[3] / qmag; if (overideAttitude) { - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - attitudeActual.q1 = q[0]; - attitudeActual.q2 = q[1]; - attitudeActual.q3 = q[2]; - attitudeActual.q4 = q[3]; - AttitudeActualSet(&attitudeActual); + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + attitudeState.q1 = q[0]; + attitudeState.q2 = q[1]; + attitudeState.q3 = q[2]; + attitudeState.q4 = q[3]; + AttitudeStateSet(&attitudeState); } static float wind[3] = { 0, 0, 0 }; @@ -454,12 +454,12 @@ static void simulateModelQuadcopter() ned_accel[2] -= 9.81; // Transform the accels back in to body frame - AccelsData accelsData; // Skip get as we set all the fields - accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0]; - accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1]; - accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2]; - accelsData.temperature = 30; - AccelsSet(&accelsData); + AccelSensorData accelSensorData; // Skip get as we set all the fields + accelSensorData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0]; + accelSensorData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1]; + accelSensorData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2]; + accelSensorData.temperature = 30; + AccelSensorSet(&accelSensorData); if (baro_offset == 0) { // Hacky initialization @@ -471,10 +471,10 @@ static void simulateModelQuadcopter() // Update baro periodically static uint32_t last_baro_time = 0; if (PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) { - BaroAltitudeData baroAltitude; - BaroAltitudeGet(&baroAltitude); - baroAltitude.Altitude = -pos[2] + baro_offset; - BaroAltitudeSet(&baroAltitude); + BaroSensorData baroSensor; + BaroSensorGet(&baroSensor); + baroSensor.Altitude = -pos[2] + baro_offset; + BaroSensorSet(&baroSensor); last_baro_time = PIOS_DELAY_GetRaw(); } @@ -528,15 +528,12 @@ static void simulateModelQuadcopter() // Update mag periodically static uint32_t last_mag_time = 0; if (PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { - MagnetometerData mag; + MagnetoSensorData mag; mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; - // Run the offset compensation algorithm from the firmware - magOffsetEstimation(&mag); - - MagnetometerSet(&mag); + MagnetoSensorSet(&mag); last_mag_time = PIOS_DELAY_GetRaw(); } @@ -615,20 +612,20 @@ static void simulateModelAirplane() // rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; // rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; // - // GyrosData gyrosData; // Skip get as we set all the fields - // gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss(); - // gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss(); - // gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss(); + // GyrosData gyroSensorData; // Skip get as we set all the fields + // gyroSensorData.x = rpy[0] * 180 / M_PI + rand_gauss(); + // gyroSensorData.y = rpy[1] * 180 / M_PI + rand_gauss(); + // gyroSensorData.z = rpy[2] * 180 / M_PI + rand_gauss(); /**** 1. Update attitude ****/ RateDesiredData rateDesired; RateDesiredGet(&rateDesired); // Need to get roll angle for easy cross coupling - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - double roll = attitudeActual.Roll; - double pitch = attitudeActual.Pitch; + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + double roll = attitudeState.Roll; + double pitch = attitudeState.Pitch; rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; @@ -636,11 +633,11 @@ static void simulateModelAirplane() rpy[2] += roll * ROLL_HEADING_COUPLING; - GyrosData gyrosData; // Skip get as we set all the fields - gyrosData.x = rpy[0] + rand_gauss(); - gyrosData.y = rpy[1] + rand_gauss(); - gyrosData.z = rpy[2] + rand_gauss(); - GyrosSet(&gyrosData); + GyroSensorData gyroSensorData; // Skip get as we set all the fields + gyroSensorData.x = rpy[0] + rand_gauss(); + gyroSensorData.y = rpy[1] + rand_gauss(); + gyroSensorData.z = rpy[2] + rand_gauss(); + GyroSensorSet(&gyroSensorData); // Predict the attitude forward in time float qdot[4]; @@ -662,13 +659,13 @@ static void simulateModelAirplane() q[3] = q[3] / qmag; if (overideAttitude) { - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - attitudeActual.q1 = q[0]; - attitudeActual.q2 = q[1]; - attitudeActual.q3 = q[2]; - attitudeActual.q4 = q[3]; - AttitudeActualSet(&attitudeActual); + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + attitudeState.q1 = q[0]; + attitudeState.q2 = q[1]; + attitudeState.q3 = q[2]; + attitudeState.q4 = q[3]; + AttitudeStateSet(&attitudeState); } /**** 2. Update position based on velocity ****/ @@ -730,12 +727,12 @@ static void simulateModelAirplane() ned_accel[2] -= GRAV; // Transform the accels back in to body frame - AccelsData accelsData; // Skip get as we set all the fields - accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0]; - accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1]; - accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2]; - accelsData.temperature = 30; - AccelsSet(&accelsData); + AccelSensorData accelSensorData; // Skip get as we set all the fields + accelSensorData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0]; + accelSensorData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1]; + accelSensorData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2]; + accelSensorData.temperature = 30; + AccelSensorSet(&accelSensorData); if (baro_offset == 0) { // Hacky initialization @@ -747,10 +744,10 @@ static void simulateModelAirplane() // Update baro periodically static uint32_t last_baro_time = 0; if (PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) { - BaroAltitudeData baroAltitude; - BaroAltitudeGet(&baroAltitude); - baroAltitude.Altitude = -pos[2] + baro_offset; - BaroAltitudeSet(&baroAltitude); + BaroSensorData baroSensor; + BaroSensorGet(&baroSensor); + baroSensor.Altitude = -pos[2] + baro_offset; + BaroSensorSet(&baroSensor); last_baro_time = PIOS_DELAY_GetRaw(); } @@ -814,12 +811,11 @@ static void simulateModelAirplane() // Update mag periodically static uint32_t last_mag_time = 0; if (PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { - MagnetometerData mag; + MagnetoSensorData mag; mag.x = 100 + homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; mag.y = 100 + homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; mag.z = 100 + homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; - magOffsetEstimation(&mag); - MagnetometerSet(&mag); + MagnetoSensorSet(&mag); last_mag_time = PIOS_DELAY_GetRaw(); } @@ -857,12 +853,15 @@ static float rand_gauss(void) } } + +// TODO +#if 0 /** * Perform an update of the @ref MagBias based on - * Magnetometer Offset Cancellation: Theory and Implementation, + * MagnetoSensor Offset Cancellation: Theory and Implementation, * revisited William Premerlani, October 14, 2011 */ -static void magOffsetEstimation(MagnetometerData *mag) +static void magOffsetEstimation(MagnetoSensorData *mag) { #if 0 RevoCalibrationData cal; @@ -910,8 +909,8 @@ static void magOffsetEstimation(MagnetometerData *mag) HomeLocationData homeLocation; HomeLocationGet(&homeLocation); - AttitudeActualData attitude; - AttitudeActualGet(&attitude); + AttitudeStateData attitude; + AttitudeStateGet(&attitude); MagBiasData magBias; MagBiasGet(&magBias); @@ -957,6 +956,7 @@ static void magOffsetEstimation(MagnetometerData *mag) #endif /* if 0 */ } +#endif /* if 0 */ /** * @} * @} diff --git a/flight/modules/Stabilization/inc/relay_tuning.h b/flight/modules/Stabilization/inc/relay_tuning.h index 2fd69cafa..6af8a3592 100644 --- a/flight/modules/Stabilization/inc/relay_tuning.h +++ b/flight/modules/Stabilization/inc/relay_tuning.h @@ -5,7 +5,7 @@ * @addtogroup StabilizationModule Stabilization Module * @brief Relay tuning controller * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the - * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual" + * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State" * @{ * * @file relay_tuning.h diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index 9d8192c43..396a91e89 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -5,7 +5,7 @@ * @addtogroup StabilizationModule Stabilization Module * @brief Stabilization PID loops in an airframe type independent manner * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the - * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual" + * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State" * @{ * * @file stabilization.h diff --git a/flight/modules/Stabilization/relay_tuning.c b/flight/modules/Stabilization/relay_tuning.c index f9b85c190..7fc25aa3e 100644 --- a/flight/modules/Stabilization/relay_tuning.c +++ b/flight/modules/Stabilization/relay_tuning.c @@ -5,7 +5,7 @@ * @addtogroup StabilizationModule Stabilization Module * @brief Relay tuning controller * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the - * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual" + * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State" * @{ * * @file stabilization.c diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 3a8a464e8..b02406a78 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -5,7 +5,7 @@ * @addtogroup StabilizationModule Stabilization Module * @brief Stabilization PID loops in an airframe type independent manner * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the - * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual" + * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State" * @{ * * @file stabilization.c @@ -40,8 +40,8 @@ #include "relaytuning.h" #include "relaytuningsettings.h" #include "stabilizationdesired.h" -#include "attitudeactual.h" -#include "gyros.h" +#include "attitudestate.h" +#include "gyrostate.h" #include "flightstatus.h" #include "manualcontrol.h" // Just to get a macro #include "taskinfo.h" @@ -104,8 +104,8 @@ int32_t StabilizationStart() queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); // Listen for updates. - // AttitudeActualConnectQueue(queue); - GyrosConnectQueue(queue); + // AttitudeStateConnectQueue(queue); + GyroStateConnectQueue(queue); StabilizationSettingsConnectCallback(SettingsUpdatedCb); SettingsUpdatedCb(StabilizationSettingsHandle()); @@ -152,8 +152,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) ActuatorDesiredData actuatorDesired; StabilizationDesiredData stabDesired; RateDesiredData rateDesired; - AttitudeActualData attitudeActual; - GyrosData gyrosData; + AttitudeStateData attitudeState; + GyroStateData gyroStateData; FlightStatusData flightStatus; SettingsUpdatedCb((UAVObjEvent *)NULL); @@ -178,8 +178,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) FlightStatusGet(&flightStatus); StabilizationDesiredGet(&stabDesired); - AttitudeActualGet(&attitudeActual); - GyrosGet(&gyrosData); + AttitudeStateGet(&attitudeState); + GyroStateGet(&gyroStateData); #ifdef DIAG_RATEDESIRED RateDesiredGet(&rateDesired); #endif @@ -195,32 +195,32 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { rpy_desired[0] = stabDesired.Roll; } else { - rpy_desired[0] = attitudeActual.Roll; + rpy_desired[0] = attitudeState.Roll; } if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { rpy_desired[1] = stabDesired.Pitch; } else { - rpy_desired[1] = attitudeActual.Pitch; + rpy_desired[1] = attitudeState.Pitch; } if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { rpy_desired[2] = stabDesired.Yaw; } else { - rpy_desired[2] = attitudeActual.Yaw; + rpy_desired[2] = attitudeState.Yaw; } RPY2Quaternion(rpy_desired, q_desired); quat_inverse(q_desired); - quat_mult(q_desired, &attitudeActual.q1, q_error); + quat_mult(q_desired, &attitudeState.q1, q_error); quat_inverse(q_error); Quaternion2RPY(q_error, local_error); #else /* if defined(PIOS_QUATERNION_STABILIZATION) */ // Simpler algorithm for CC, less memory - float local_error[3] = { stabDesired.Roll - attitudeActual.Roll, - stabDesired.Pitch - attitudeActual.Pitch, - stabDesired.Yaw - attitudeActual.Yaw }; + float local_error[3] = { stabDesired.Roll - attitudeState.Roll, + stabDesired.Pitch - attitudeState.Pitch, + stabDesired.Yaw - attitudeState.Yaw }; // find shortest way float modulo = fmodf(local_error[2] + 180.0f, 360.0f); if (modulo < 0) { @@ -231,9 +231,9 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) #endif /* if defined(PIOS_QUATERNION_STABILIZATION) */ float gyro_filtered[3]; - gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha); - gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha); - gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha); + gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyroStateData.x * (1 - gyro_alpha); + gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyroStateData.y * (1 - gyro_alpha); + gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyroStateData.z * (1 - gyro_alpha); float *attitudeDesiredAxis = &stabDesired.Roll; float *actuatorDesiredAxis = &actuatorDesired.Roll; diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index b07601c0c..764c094d0 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -3,7 +3,7 @@ * * @file vtolpathfollower.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief This module compared @ref PositionActual to @ref PathDesired + * @brief This module compared @ref PositionState to @ref PathDesired * and sets @ref Stabilization. It only does this when the FlightMode field * of @ref FlightStatus is PathPlanner or RTH. * @@ -29,11 +29,11 @@ /** * Input object: FlightStatus * Input object: PathDesired - * Input object: PositionActual + * Input object: PositionState * Output object: StabilizationDesired * * This module will periodically update the value of the @ref StabilizationDesired object based on - * @ref PathDesired and @PositionActual when the Flight Mode selected in @FlightStatus is supported + * @ref PathDesired and @PositionState when the Flight Mode selected in @FlightStatus is supported * by this module. Otherwise another module (e.g. @ref ManualControlCommand) is expected to be * writing to @ref StabilizationDesired. * @@ -50,11 +50,11 @@ #include "vtolpathfollower.h" -#include "accels.h" -#include "attitudeactual.h" +#include "accelstate.h" +#include "attitudestate.h" #include "hwsettings.h" #include "pathdesired.h" // object that will be updated by the module -#include "positionactual.h" +#include "positionstate.h" #include "manualcontrol.h" #include "flightstatus.h" #include "pathstatus.h" @@ -67,7 +67,7 @@ #include "stabilizationsettings.h" #include "systemsettings.h" #include "velocitydesired.h" -#include "velocityactual.h" +#include "velocitystate.h" #include "taskinfo.h" #include "paths.h" @@ -286,8 +286,8 @@ static void updatePOIBearing() PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - PositionActualData positionActual; - PositionActualGet(&positionActual); + PositionStateData positionState; + PositionStateGet(&positionState); CameraDesiredData cameraDesired; CameraDesiredGet(&cameraDesired); StabilizationDesiredData stabDesired; @@ -299,9 +299,9 @@ static void updatePOIBearing() float yaw = 0; /*float elevation = 0;*/ - dLoc[0] = positionActual.North - poi.North; - dLoc[1] = positionActual.East - poi.East; - dLoc[2] = positionActual.Down - poi.Down; + dLoc[0] = positionState.North - poi.North; + dLoc[1] = positionState.East - poi.East; + dLoc[2] = positionState.Down - poi.Down; if (dLoc[1] < 0) { yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f; @@ -364,7 +364,7 @@ static void updatePOIBearing() /** * Compute desired velocity from the current position and path * - * Takes in @ref PositionActual and compares it to @ref PathDesired + * Takes in @ref PositionState and compares it to @ref PathDesired * and computes @ref VelocityDesired */ static void updatePathVelocity() @@ -375,11 +375,11 @@ static void updatePathVelocity() PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - PositionActualData positionActual; - PositionActualGet(&positionActual); + PositionStateData positionState; + PositionStateGet(&positionState); float cur[3] = - { positionActual.North, positionActual.East, positionActual.Down }; + { positionState.North, positionState.East, positionState.Down }; struct path_status progress; path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); @@ -429,7 +429,7 @@ static void updatePathVelocity() float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound(progress.fractional_progress, 0, 1); - float downError = altitudeSetpoint - positionActual.Down; + float downError = altitudeSetpoint - positionState.Down; downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); @@ -446,7 +446,7 @@ static void updatePathVelocity() /** * Compute desired velocity from the current position * - * Takes in @ref PositionActual and compares it to @ref PositionDesired + * Takes in @ref PositionState and compares it to @ref PositionDesired * and computes @ref VelocityDesired */ void updateEndpointVelocity() @@ -457,10 +457,10 @@ void updateEndpointVelocity() PathDesiredGet(&pathDesired); - PositionActualData positionActual; + PositionStateData positionState; VelocityDesiredData velocityDesired; - PositionActualGet(&positionActual); + PositionStateGet(&positionState); VelocityDesiredGet(&velocityDesired); float northError; @@ -473,9 +473,9 @@ void updateEndpointVelocity() float northPos = 0, eastPos = 0, downPos = 0; switch (vtolpathfollowerSettings.PositionSource) { case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF: - northPos = positionActual.North; - eastPos = positionActual.East; - downPos = positionActual.Down; + northPos = positionState.North; + eastPos = positionState.East; + downPos = positionState.Down; break; case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS: { @@ -559,18 +559,18 @@ static void updateFixedAttitude(float *attitude) /** * Compute desired attitude from the desired velocity * - * Takes in @ref NedActual which has the acceleration in the + * Takes in @ref NedState which has the acceleration in the * NED frame as the feedback term and then compares the - * @ref VelocityActual against the @ref VelocityDesired + * @ref VelocityState against the @ref VelocityDesired */ static void updateVtolDesiredAttitude(bool yaw_attitude) { float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; VelocityDesiredData velocityDesired; - VelocityActualData velocityActual; + VelocityStateData velocityState; StabilizationDesiredData stabDesired; - AttitudeActualData attitudeActual; + AttitudeStateData attitudeState; NedAccelData nedAccel; StabilizationSettingsData stabSettings; SystemSettingsData systemSettings; @@ -585,20 +585,20 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) float downCommand; SystemSettingsGet(&systemSettings); - VelocityActualGet(&velocityActual); + VelocityStateGet(&velocityState); VelocityDesiredGet(&velocityDesired); StabilizationDesiredGet(&stabDesired); VelocityDesiredGet(&velocityDesired); - AttitudeActualGet(&attitudeActual); + AttitudeStateGet(&attitudeState); StabilizationSettingsGet(&stabSettings); NedAccelGet(&nedAccel); float northVel = 0, eastVel = 0, downVel = 0; switch (vtolpathfollowerSettings.VelocitySource) { case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF: - northVel = velocityActual.North; - eastVel = velocityActual.East; - downVel = velocityActual.Down; + northVel = velocityState.North; + eastVel = velocityState.East; + downVel = velocityState.Down; break; case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: { @@ -615,7 +615,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) GPSPositionGet(&gpsPosition); northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading)); eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading)); - downVel = velocityActual.Down; + downVel = velocityState.Down; } break; default: @@ -659,11 +659,11 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the // craft should move similarly for 5 deg roll versus 5 deg pitch - stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeActual.Yaw)) + - -eastCommand * sinf(DEG2RAD(attitudeActual.Yaw)), + stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) + + -eastCommand * sinf(DEG2RAD(attitudeState.Yaw)), -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); - stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeActual.Yaw)) + - eastCommand * cosf(DEG2RAD(attitudeActual.Yaw)), + stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) + + eastCommand * cosf(DEG2RAD(attitudeState.Yaw)), -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { @@ -695,20 +695,20 @@ static void updateNedAccel() float accel_ned[3]; // Collect downsampled attitude data - AccelsData accels; + AccelStateData accelState; - AccelsGet(&accels); - accel[0] = accels.x; - accel[1] = accels.y; - accel[2] = accels.z; + AccelStateGet(&accelState); + accel[0] = accelState.x; + accel[1] = accelState.y; + accel[2] = accelState.z; // rotate avg accels into earth frame and store it - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - q[0] = attitudeActual.q1; - q[1] = attitudeActual.q2; - q[2] = attitudeActual.q3; - q[3] = attitudeActual.q4; + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + q[0] = attitudeState.q1; + q[1] = attitudeState.q2; + q[2] = attitudeState.q3; + q[3] = attitudeState.q4; Quaternion2R(q, Rbe); for (uint8_t i = 0; i < 3; i++) { accel_ned[i] = 0; @@ -757,13 +757,13 @@ static void accessoryUpdated(UAVObjEvent *ev) if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) { if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { if (accessory.AccessoryVal < -0.5f) { - PositionActualData positionActual; - PositionActualGet(&positionActual); + PositionStateData positionState; + PositionStateGet(&positionState); PoiLocationData poi; PoiLocationGet(&poi); - poi.North = positionActual.North; - poi.East = positionActual.East; - poi.Down = positionActual.Down; + poi.North = positionState.North; + poi.East = positionState.East; + poi.Down = positionState.Down; PoiLocationSet(&poi); } } diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index 24c5f3deb..e8ca6cbd2 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -87,9 +87,9 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c SRC += $(OPUAVSYNTHDIR)/actuatordesired.c SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c - SRC += $(OPUAVSYNTHDIR)/accels.c - SRC += $(OPUAVSYNTHDIR)/gyros.c - SRC += $(OPUAVSYNTHDIR)/attitudeactual.c + SRC += $(OPUAVSYNTHDIR)/accelstate.c + SRC += $(OPUAVSYNTHDIR)/gyrostate.c + SRC += $(OPUAVSYNTHDIR)/attitudestate.c SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c SRC += $(OPUAVSYNTHDIR)/i2cstats.c SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c @@ -109,9 +109,9 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/taskinfo.c SRC += $(OPUAVSYNTHDIR)/mixerstatus.c SRC += $(OPUAVSYNTHDIR)/ratedesired.c - SRC += $(OPUAVSYNTHDIR)/baroaltitude.c + SRC += $(OPUAVSYNTHDIR)/barosensor.c SRC += $(OPUAVSYNTHDIR)/txpidsettings.c - SRC += $(OPUAVSYNTHDIR)/airspeedactual.c + SRC += $(OPUAVSYNTHDIR)/airspeedstate.c SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c else ## Test Code diff --git a/flight/targets/boards/osd/firmware/Makefile b/flight/targets/boards/osd/firmware/Makefile index fb2b56603..0ce968734 100644 --- a/flight/targets/boards/osd/firmware/Makefile +++ b/flight/targets/boards/osd/firmware/Makefile @@ -79,7 +79,7 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/systemstats.c SRC += $(OPUAVSYNTHDIR)/systemalarms.c SRC += $(OPUAVSYNTHDIR)/systemsettings.c - SRC += $(OPUAVSYNTHDIR)/attitudeactual.c + SRC += $(OPUAVSYNTHDIR)/attitudestate.c SRC += $(OPUAVSYNTHDIR)/i2cstats.c SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c @@ -92,8 +92,8 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/gpsvelocity.c SRC += $(OPUAVSYNTHDIR)/gpstime.c SRC += $(OPUAVSYNTHDIR)/osdsettings.c - SRC += $(OPUAVSYNTHDIR)/baroaltitude.c - SRC += $(OPUAVSYNTHDIR)/magnetometer.c + SRC += $(OPUAVSYNTHDIR)/barosensor.c + SRC += $(OPUAVSYNTHDIR)/magnetosensor.c else ## Test Code SRC += $(OPTESTS)/test_common.c diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 57c9f180e..34f387250 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -25,16 +25,17 @@ UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatorsettings UAVOBJSRCFILENAMES += altholdsmoothed UAVOBJSRCFILENAMES += attitudesettings -UAVOBJSRCFILENAMES += attitudeactual -UAVOBJSRCFILENAMES += gyros -UAVOBJSRCFILENAMES += gyrosbias -UAVOBJSRCFILENAMES += accels -UAVOBJSRCFILENAMES += magnetometer -UAVOBJSRCFILENAMES += magbias -UAVOBJSRCFILENAMES += baroaltitude +UAVOBJSRCFILENAMES += attitudestate +UAVOBJSRCFILENAMES += gyrostate +UAVOBJSRCFILENAMES += gyrosensor +UAVOBJSRCFILENAMES += accelstate +UAVOBJSRCFILENAMES += accelsensor +UAVOBJSRCFILENAMES += magnetosensor +UAVOBJSRCFILENAMES += magnetostate +UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings -UAVOBJSRCFILENAMES += airspeedactual +UAVOBJSRCFILENAMES += airspeedstate UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate @@ -64,7 +65,7 @@ UAVOBJSRCFILENAMES += overosyncsettings UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathstatus -UAVOBJSRCFILENAMES += positionactual +UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += relaytuning UAVOBJSRCFILENAMES += relaytuningsettings @@ -79,7 +80,7 @@ UAVOBJSRCFILENAMES += systemalarms UAVOBJSRCFILENAMES += systemsettings UAVOBJSRCFILENAMES += systemstats UAVOBJSRCFILENAMES += taskinfo -UAVOBJSRCFILENAMES += velocityactual +UAVOBJSRCFILENAMES += velocitystate UAVOBJSRCFILENAMES += velocitydesired UAVOBJSRCFILENAMES += watchdogstatus UAVOBJSRCFILENAMES += flightstatus diff --git a/flight/targets/boards/revolution/firmware/pios_board_sim.c b/flight/targets/boards/revolution/firmware/pios_board_sim.c index 8af566e93..ca2c3de98 100644 --- a/flight/targets/boards/revolution/firmware/pios_board_sim.c +++ b/flight/targets/boards/revolution/firmware/pios_board_sim.c @@ -31,12 +31,11 @@ #include #include -#include -#include +#include +#include #include -#include -#include -#include +#include +#include #include void Stack_Change() {} @@ -137,12 +136,11 @@ void PIOS_Board_Init(void) UAVObjInitialize(); UAVObjectsInitializeAll(); - AccelsInitialize(); - BaroAltitudeInitialize(); - MagnetometerInitialize(); + AccelSensorInitialize(); + BaroSensorInitialize(); + MagnetoSensorInitialize(); GPSPositionInitialize(); - GyrosInitialize(); - GyrosBiasInitialize(); + GyroSensorInitialize(); /* Initialize the alarms library */ AlarmsInitialize(); diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index c7e12bdea..02169d2b8 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -30,16 +30,17 @@ UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatorsettings UAVOBJSRCFILENAMES += altholdsmoothed UAVOBJSRCFILENAMES += attitudesettings -UAVOBJSRCFILENAMES += attitudeactual -UAVOBJSRCFILENAMES += gyros -UAVOBJSRCFILENAMES += gyrosbias -UAVOBJSRCFILENAMES += accels -UAVOBJSRCFILENAMES += magnetometer -UAVOBJSRCFILENAMES += magbias -UAVOBJSRCFILENAMES += baroaltitude +UAVOBJSRCFILENAMES += attitudestate +UAVOBJSRCFILENAMES += gyrostate +UAVOBJSRCFILENAMES += gyrosensor +UAVOBJSRCFILENAMES += accelstate +UAVOBJSRCFILENAMES += accelsensor +UAVOBJSRCFILENAMES += magnetostate +UAVOBJSRCFILENAMES += magnetosensor +UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings -UAVOBJSRCFILENAMES += airspeedactual +UAVOBJSRCFILENAMES += airspeedstate UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate @@ -69,7 +70,7 @@ UAVOBJSRCFILENAMES += overosyncsettings UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathstatus -UAVOBJSRCFILENAMES += positionactual +UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += relaytuning UAVOBJSRCFILENAMES += relaytuningsettings @@ -84,7 +85,7 @@ UAVOBJSRCFILENAMES += systemalarms UAVOBJSRCFILENAMES += systemsettings UAVOBJSRCFILENAMES += systemstats UAVOBJSRCFILENAMES += taskinfo -UAVOBJSRCFILENAMES += velocityactual +UAVOBJSRCFILENAMES += velocitystate UAVOBJSRCFILENAMES += velocitydesired UAVOBJSRCFILENAMES += watchdogstatus UAVOBJSRCFILENAMES += flightstatus diff --git a/flight/targets/boards/revoproto/firmware/pios_board_sim.c b/flight/targets/boards/revoproto/firmware/pios_board_sim.c index a73beba28..0b026b530 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board_sim.c +++ b/flight/targets/boards/revoproto/firmware/pios_board_sim.c @@ -31,12 +31,11 @@ #include #include -#include -#include +#include +#include #include -#include -#include -#include +#include +#include #include void Stack_Change() {} @@ -137,12 +136,12 @@ void PIOS_Board_Init(void) UAVObjInitialize(); UAVObjectsInitializeAll(); - AccelsInitialize(); - BaroAltitudeInitialize(); - MagnetometerInitialize(); + AccelSensorInitialize(); + BaroSensorInitialize(); + MagnetoSensorInitialize(); GPSPositionInitialize(); - GyrosInitialize(); - GyrosBiasInitialize(); + GyroStatInitialize(); + GyroSensorInitialize(); /* Initialize the alarms library */ AlarmsInitialize(); diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 23ffde045..c5a401d52 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -30,15 +30,17 @@ UAVOBJSRCFILENAMES += actuatordesired UAVOBJSRCFILENAMES += actuatorsettings UAVOBJSRCFILENAMES += altholdsmoothed UAVOBJSRCFILENAMES += attitudesettings -UAVOBJSRCFILENAMES += attitudeactual -UAVOBJSRCFILENAMES += gyros -UAVOBJSRCFILENAMES += gyrosbias -UAVOBJSRCFILENAMES += accels -UAVOBJSRCFILENAMES += magnetometer -UAVOBJSRCFILENAMES += baroaltitude +UAVOBJSRCFILENAMES += attitudestate +UAVOBJSRCFILENAMES += gyrostate +UAVOBJSRCFILENAMES += gyrosensor +UAVOBJSRCFILENAMES += accelstate +UAVOBJSRCFILENAMES += accelsensor +UAVOBJSRCFILENAMES += magnetosensor +UAVOBJSRCFILENAMES += magnetostate +UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings -UAVOBJSRCFILENAMES += airspeedactual +UAVOBJSRCFILENAMES += airspeedstate UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate @@ -66,7 +68,7 @@ UAVOBJSRCFILENAMES += overosyncstats UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathstatus -UAVOBJSRCFILENAMES += positionactual +UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += revocalibration UAVOBJSRCFILENAMES += relaytuning @@ -78,7 +80,7 @@ UAVOBJSRCFILENAMES += systemalarms UAVOBJSRCFILENAMES += systemsettings UAVOBJSRCFILENAMES += systemstats UAVOBJSRCFILENAMES += taskinfo -UAVOBJSRCFILENAMES += velocityactual +UAVOBJSRCFILENAMES += velocitystate UAVOBJSRCFILENAMES += velocitydesired UAVOBJSRCFILENAMES += watchdogstatus UAVOBJSRCFILENAMES += waypoint diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml index d04c74168..5b1282f5e 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml @@ -49,7 +49,7 @@ default - Accels + AccelState 0 false @@ -99,19 +99,19 @@ needle needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -134,19 +134,19 @@ needle2 needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -169,19 +169,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -204,19 +204,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 12 -12 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -239,19 +239,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -274,19 +274,19 @@ needle needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -309,19 +309,19 @@ needle2 needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -344,19 +344,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -379,19 +379,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 11.2 -11.2 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -414,19 +414,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -455,13 +455,13 @@ 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -484,19 +484,19 @@ needle2 needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -519,19 +519,19 @@ needle2 needle2 Lucida Grande,13,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - Accels + AccelState 1 20 -20 Horizontal x - Accels + AccelState -1 360 0 @@ -560,13 +560,13 @@ 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -589,19 +589,19 @@ needle needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -624,19 +624,19 @@ needle2 needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -659,19 +659,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -694,19 +694,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 12 -12 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -729,19 +729,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -770,13 +770,13 @@ 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -799,19 +799,19 @@ needle2 needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -840,13 +840,13 @@ 1000 Rotate Channel-3 - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -869,19 +869,19 @@ needle2 needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -1022,7 +1022,7 @@ false false true - true + true true 20 @@ -1064,7 +1064,7 @@ -11 11 -11 - Accels + AccelState x false -5 @@ -1087,7 +1087,7 @@ -11 11 -11 - Accels + AccelState y false -5 @@ -1110,7 +1110,7 @@ -11 11 -11 - Accels + AccelState z false -5 @@ -1301,7 +1301,7 @@ -0.8 - + false 0.0.0 @@ -1317,13 +1317,13 @@ -90 1 0 - AttitudeActual + AttitudeState Pitch false 0.9 0.1 - + false @@ -1871,7 +1871,7 @@ 4294901760 None x - Accels + AccelState 0 1 6.92920863607354e-310 @@ -1881,7 +1881,7 @@ 4283782655 None y - Accels + AccelState 1.78017180972965e-306 1 9.34609790258712e-307 @@ -1891,7 +1891,7 @@ 4283804160 None z - Accels + AccelState 2.6501977682312e-318 1 6.92920723246466e-310 @@ -1973,7 +1973,7 @@ 4283760895 None Roll - AttitudeActual + AttitudeState 6.92921152826347e-310 1 6.92921152826031e-310 @@ -1983,7 +1983,7 @@ 4278233600 None Yaw - AttitudeActual + AttitudeState 0 1 0 @@ -1993,7 +1993,7 @@ 4294901760 None Pitch - AttitudeActual + AttitudeState 0 1 0 @@ -2019,7 +2019,7 @@ 4278190080 None Pressure - BaroAltitude + BaroSensor 1.72723371102043e-77 1 3.86203233966055e-312 @@ -2126,7 +2126,7 @@ 200 - + false 0.0.0 @@ -2141,7 +2141,7 @@ 4294901760 None x - Accels + AccelState 0 1 0 @@ -2151,7 +2151,7 @@ 4283782655 None y - Accels + AccelState 0 1 0 @@ -2161,7 +2161,7 @@ 4283804160 None z - Accels + AccelState 0 1 0 @@ -2171,8 +2171,8 @@ 1 500 - - + + false 0.0.0 @@ -2187,7 +2187,7 @@ 4283804160 None z - Gyros + gyroState 1.02360527502876e-306 1 6.92921152846347e-310 @@ -2197,7 +2197,7 @@ 4283782655 None y - Gyros + gyroState 0 1 0 @@ -2207,7 +2207,7 @@ 4294901760 None x - Gyros + gyroState 0 1 0 @@ -2217,7 +2217,7 @@ 1 500 - + false @@ -2233,7 +2233,7 @@ 4294901760 None x - Magnetometer + MagnetoState 6.92916505601691e-310 1 3.86203233966055e-312 @@ -2243,7 +2243,7 @@ 4283782655 None y - Magnetometer + MagnetoState 1.72723371101889e-77 1 -1 @@ -2253,7 +2253,7 @@ 4283804160 None z - Magnetometer + MagnetoState 1.72723371102028e-77 1 7.21478569792807e-313 @@ -2694,7 +2694,7 @@ ScopeGadget - Raw Gyros + Raw gyroState uavGadget diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml index bee98b3e0..f00c303fe 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml @@ -115,19 +115,19 @@ needle needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -150,19 +150,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -182,19 +182,19 @@ %%DATAPATH%%dials/default/barometer.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -214,19 +214,19 @@ %%DATAPATH%%dials/default/vsi.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 12 -12 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -247,19 +247,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -282,19 +282,19 @@ needle needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -317,19 +317,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -350,19 +350,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -383,19 +383,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 11.2 -11.2 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -416,19 +416,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -455,13 +455,13 @@ 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -484,19 +484,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -519,19 +519,19 @@ needle2 needle2 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - Accels + AccelState 1 20 -20 Horizontal x - Accels + AccelState -1 360 0 @@ -557,13 +557,13 @@ 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -586,19 +586,19 @@ needle needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -621,19 +621,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -653,19 +653,19 @@ %%DATAPATH%%dials/hi-contrast/barometer.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -685,19 +685,19 @@ %%DATAPATH%%dials/hi-contrast/vsi.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 12 -12 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -718,19 +718,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -756,13 +756,13 @@ 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -784,19 +784,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -824,13 +824,13 @@ 1000 Rotate Channel-3 - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -852,19 +852,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -965,7 +965,7 @@ false false true - true + true true 20 50 @@ -1001,7 +1001,7 @@ false false true - true + true true 20 50 @@ -1037,7 +1037,7 @@ false false true - true + true true 20 50 @@ -1081,7 +1081,7 @@ -11 11 -11 - Accels + AccelState x false -5 @@ -1104,7 +1104,7 @@ -11 11 -11 - Accels + AccelState y false -5 @@ -1127,7 +1127,7 @@ -11 11 -11 - Accels + AccelState z false -5 @@ -1318,7 +1318,7 @@ -0.8 - + false 0.0.0 @@ -1334,13 +1334,13 @@ -90 1 0 - AttitudeActual + AttitudeState Pitch false 0.9 0.1 - + false @@ -1909,7 +1909,7 @@ 4294901760 None x - Accels + AccelState 0 1 0 @@ -1919,7 +1919,7 @@ 4283782655 None y - Accels + AccelState 0 1 0 @@ -1929,7 +1929,7 @@ 4283804160 None z - Accels + AccelState 0 1 0 @@ -2009,7 +2009,7 @@ 4283760895 None Roll - AttitudeActual + AttitudeState 0 1 0 @@ -2019,7 +2019,7 @@ 4278233600 None Yaw - AttitudeActual + AttitudeState 0 1 0 @@ -2029,7 +2029,7 @@ 4294901760 None Pitch - AttitudeActual + AttitudeState 0 1 0 @@ -2054,7 +2054,7 @@ 4278190080 None Pressure - BaroAltitude + BaroSensor 0 1 0 @@ -2160,7 +2160,7 @@ 200 - + false 0.0.0 @@ -2174,7 +2174,7 @@ 4294901760 None x - Accels + AccelState 0 1 0 @@ -2184,7 +2184,7 @@ 4283782655 None y - Accels + AccelState 0 1 0 @@ -2194,7 +2194,7 @@ 4283804160 None z - Accels + AccelState 0 1 0 @@ -2204,8 +2204,8 @@ 1 500 - - + + false 0.0.0 @@ -2219,7 +2219,7 @@ 4283804160 None z - Gyros + GyroState 0 1 0 @@ -2229,7 +2229,7 @@ 4283782655 None y - Gyros + GyroState 0 1 0 @@ -2239,7 +2239,7 @@ 4294901760 None x - Gyros + GyroState 0 1 0 @@ -2249,7 +2249,7 @@ 1 500 - + false @@ -2264,7 +2264,7 @@ 4294901760 None x - Magnetometer + MagnetoState 0 1 0 @@ -2274,7 +2274,7 @@ 4283782655 None y - Magnetometer + MagnetoState 0 1 0 @@ -2284,7 +2284,7 @@ 4283804160 None z - Magnetometer + MagnetoState 0 1 0 @@ -2675,7 +2675,7 @@ ScopeGadget - Raw Gyros + Raw GyroState uavGadget diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml index bd99b9685..916b88152 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml @@ -115,19 +115,19 @@ needle needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -150,19 +150,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -182,19 +182,19 @@ %%DATAPATH%%dials/default/barometer.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -214,19 +214,19 @@ %%DATAPATH%%dials/default/vsi.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 12 -12 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -247,19 +247,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -282,19 +282,19 @@ needle needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -317,19 +317,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -350,19 +350,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -383,19 +383,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 11.2 -11.2 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -416,19 +416,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -455,13 +455,13 @@ 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -484,19 +484,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -519,19 +519,19 @@ needle2 needle2 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - Accels + AccelState 1 20 -20 Horizontal x - Accels + AccelState -1 360 0 @@ -557,13 +557,13 @@ 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -586,19 +586,19 @@ needle needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -621,19 +621,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -653,19 +653,19 @@ %%DATAPATH%%dials/hi-contrast/barometer.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -685,19 +685,19 @@ %%DATAPATH%%dials/hi-contrast/vsi.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 12 -12 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -718,19 +718,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -756,13 +756,13 @@ 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -784,19 +784,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -824,13 +824,13 @@ 1000 Rotate Channel-3 - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -852,19 +852,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -965,7 +965,7 @@ false false true - true + true true 20 50 @@ -1001,7 +1001,7 @@ false false true - true + true true 20 50 @@ -1037,7 +1037,7 @@ false false true - true + true true 20 50 @@ -1081,7 +1081,7 @@ -11 11 -11 - Accels + AccelState x false -5 @@ -1104,7 +1104,7 @@ -11 11 -11 - Accels + AccelState y false -5 @@ -1127,7 +1127,7 @@ -11 11 -11 - Accels + AccelState z false -5 @@ -1318,7 +1318,7 @@ -0.8 - + false 0.0.0 @@ -1334,13 +1334,13 @@ -90 1 0 - AttitudeActual + AttitudeState Pitch false 0.9 0.1 - + false @@ -1887,7 +1887,7 @@ 4294901760 None x - Accels + AccelState 0 1 0 @@ -1897,7 +1897,7 @@ 4283782655 None y - Accels + AccelState 0 1 0 @@ -1907,7 +1907,7 @@ 4283804160 None z - Accels + AccelState 0 1 0 @@ -1987,7 +1987,7 @@ 4283760895 None Roll - AttitudeActual + AttitudeState 0 1 0 @@ -1997,7 +1997,7 @@ 4278233600 None Yaw - AttitudeActual + AttitudeState 0 1 0 @@ -2007,7 +2007,7 @@ 4294901760 None Pitch - AttitudeActual + AttitudeState 0 1 0 @@ -2032,7 +2032,7 @@ 4278190080 None Pressure - BaroAltitude + BaroSensor 0 1 0 @@ -2138,7 +2138,7 @@ 200 - + false 0.0.0 @@ -2152,7 +2152,7 @@ 4294901760 None x - Accels + AccelState 0 1 0 @@ -2162,7 +2162,7 @@ 4283782655 None y - Accels + AccelState 0 1 0 @@ -2172,7 +2172,7 @@ 4283804160 None z - Accels + AccelState 0 1 0 @@ -2182,8 +2182,8 @@ 1 500 - - + + false 0.0.0 @@ -2197,7 +2197,7 @@ 4283804160 None z - Gyros + GyroState 0 1 0 @@ -2207,7 +2207,7 @@ 4283782655 None y - Gyros + GyroState 0 1 0 @@ -2217,7 +2217,7 @@ 4294901760 None x - Gyros + GyroState 0 1 0 @@ -2227,7 +2227,7 @@ 1 500 - + false @@ -2242,7 +2242,7 @@ 4294901760 None x - Magnetometer + MagnetoState 0 1 0 @@ -2252,7 +2252,7 @@ 4283782655 None y - Magnetometer + MagnetoState 0 1 0 @@ -2262,7 +2262,7 @@ 4283804160 None z - Magnetometer + MagnetoState 0 1 0 @@ -2665,7 +2665,7 @@ ScopeGadget - Raw Gyros + Raw GyroState uavGadget diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/AltitudeScale.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/AltitudeScale.qml index a8f4d66b2..873f86c98 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/AltitudeScale.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/AltitudeScale.qml @@ -24,10 +24,10 @@ Item { anchors.verticalCenter: parent.verticalCenter // The altitude scale represents 30 meters, // move it in 0..5m range - anchors.verticalCenterOffset: -height/30 * (PositionActual.Down-Math.floor(PositionActual.Down/5)*5) + anchors.verticalCenterOffset: -height/30 * (PositionState.Down-Math.floor(PositionState.Down/5)*5) anchors.left: parent.left - property int topNumber: 15-Math.floor(PositionActual.Down/5)*5 + property int topNumber: 15-Math.floor(PositionState.Down/5)*5 // Altitude numbers Column { @@ -69,7 +69,7 @@ Item { Text { id: altitude_text - text: Math.floor(-PositionActual.Down).toFixed() + text: Math.floor(-PositionState.Down).toFixed() color: "white" font { family: "Arial" diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml index 3c956d526..db2ba4d65 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml @@ -19,8 +19,8 @@ Item { //split compass band to 8 parts to ensure it doesn't exceed the max texture size Row { anchors.centerIn: parent - //the band is 540 degrees wide, AttitudeActual.Yaw is converted to -180..180 range - anchors.horizontalCenterOffset: -1*((AttitudeActual.Yaw+180+720) % 360 - 180)/540*width + //the band is 540 degrees wide, AttitudeState.Yaw is converted to -180..180 range + anchors.horizontalCenterOffset: -1*((AttitudeState.Yaw+180+720) % 360 - 180)/540*width Repeater { model: 5 diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml index e393b185e..29ac73e9a 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml @@ -34,7 +34,7 @@ Rectangle { anchors.centerIn: parent //rotate it around the center of scene transform: Rotation { - angle: -AttitudeActual.Roll + angle: -AttitudeState.Roll origin.x : sceneItem.width/2 - x origin.y : sceneItem.height/2 - y } diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdTerrainView.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdTerrainView.qml index f56f6d89c..6a9a8d24b 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdTerrainView.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdTerrainView.qml @@ -7,9 +7,9 @@ OsgEarth { sceneFile: qmlWidget.earthFile fieldOfView: 90 - yaw: AttitudeActual.Yaw - pitch: AttitudeActual.Pitch - roll: AttitudeActual.Roll + yaw: AttitudeState.Yaw + pitch: AttitudeState.Pitch + roll: AttitudeState.Roll latitude: qmlWidget.actualPositionUsed ? GPSPosition.Latitude/10000000.0 : qmlWidget.latitude diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml index 9a461464f..4ce07956b 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml @@ -23,10 +23,10 @@ Item { id: pitchTranslate x: Math.round((world.parent.width - world.width)/2) y: Math.round((world.parent.height - world.height)/2 + - AttitudeActual.Pitch*world.parent.height/94) + AttitudeState.Pitch*world.parent.height/94) }, Rotation { - angle: -AttitudeActual.Roll + angle: -AttitudeState.Roll origin.x : world.parent.width/2 origin.y : world.parent.height/2 } diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/SpeedScale.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/SpeedScale.qml index 1340b9cdb..8b050e75a 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/SpeedScale.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/SpeedScale.qml @@ -3,8 +3,8 @@ import Qt 4.7 Item { id: sceneItem property variant sceneSize - property real groundSpeed : 3.6 * Math.sqrt(Math.pow(VelocityActual.North,2)+ - Math.pow(VelocityActual.East,2)) + property real groundSpeed : 3.6 * Math.sqrt(Math.pow(VelocityState.North,2)+ + Math.pow(VelocityState.East,2)) SvgElementImage { id: speed_bg diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml index 171c048e0..dbcf6aa63 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml @@ -20,7 +20,7 @@ Item { sceneSize: sceneItem.sceneSize //the scale in 1000 ft/min with height == 5200 ft/min - height: (-VelocityActual.Down*3.28*60/1000)*(vsi_scale.height/5.2) + height: (-VelocityState.Down*3.28*60/1000)*(vsi_scale.height/5.2) anchors.bottom: parent.verticalCenter diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_de.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_de.ts index 554a8c5c7..cfd19556d 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_de.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_de.ts @@ -1767,7 +1767,7 @@ p, li { white-space: pre-wrap; } - AttitudeActual + AttitudeState diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_es.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_es.ts index 32554073c..5182fddf2 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_es.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_es.ts @@ -1768,7 +1768,7 @@ p, li { white-space: pre-wrap; } - AttitudeActual + AttitudeState diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts index 529fd4f7a..e74746db8 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts @@ -1789,7 +1789,7 @@ p, li { white-space: pre-wrap; } - AttitudeActual + AttitudeState diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_ru.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_ru.ts index 611533c3e..e368aca9d 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_ru.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_ru.ts @@ -1721,7 +1721,7 @@ Sat SNR is displayed above (in dBHz) - AttitudeActual + AttitudeState diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_zh_CN.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_zh_CN.ts index a27bc2920..b80073462 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_zh_CN.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_zh_CN.ts @@ -2607,7 +2607,7 @@ Sat SNR is displayed above (in dBHz) - AttitudeActual + AttitudeState diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index 79d69382e..17a4269ff 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -33,8 +33,8 @@ #include #include #include -#include "accels.h" -#include "gyros.h" +#include "accelstate.h" +#include "gyrostate.h" #include #include @@ -80,26 +80,26 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj) return; } - Accels *accels = Accels::GetInstance(getObjectManager()); - Gyros *gyros = Gyros::GetInstance(getObjectManager()); + AccelState *accelState = AccelState::GetInstance(getObjectManager()); + GyroState *gyroState = GyroState::GetInstance(getObjectManager()); // Accumulate samples until we have _at least_ NUM_SENSOR_UPDATES samples // for both gyros and accels. // Note that, at present, we stash the samples and then compute the bias // at the end, even though the mean could be accumulated as we go. // In future, a better algorithm could be used. - if (obj->getObjID() == Accels::OBJID) { + if (obj->getObjID() == AccelState::OBJID) { accelUpdates++; - Accels::DataFields accelsData = accels->getData(); - x_accum.append(accelsData.x); - y_accum.append(accelsData.y); - z_accum.append(accelsData.z); - } else if (obj->getObjID() == Gyros::OBJID) { + AccelState::DataFields accelStateData = accelState->getData(); + x_accum.append(accelStateData.x); + y_accum.append(accelStateData.y); + z_accum.append(accelStateData.z); + } else if (obj->getObjID() == GyroState::OBJID) { gyroUpdates++; - Gyros::DataFields gyrosData = gyros->getData(); - x_gyro_accum.append(gyrosData.x); - y_gyro_accum.append(gyrosData.y); - z_gyro_accum.append(gyrosData.z); + GyroState::DataFields gyroStateData = gyroState->getData(); + x_gyro_accum.append(gyroStateData.x); + y_gyro_accum.append(gyroStateData.y); + z_gyro_accum.append(gyroStateData.z); } // update the progress indicator @@ -118,8 +118,8 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj) float x_gyro_bias = listMean(x_gyro_accum) * 100.0f; float y_gyro_bias = listMean(y_gyro_accum) * 100.0f; float z_gyro_bias = listMean(z_gyro_accum) * 100.0f; - accels->setMetadata(initialAccelsMdata); - gyros->setMetadata(initialGyrosMdata); + accelState->setMetadata(initialAccelStateMdata); + gyroState->setMetadata(initialGyroStateMdata); AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData(); // We offset the gyro bias by current bias to help precision @@ -140,15 +140,15 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj) void ConfigCCAttitudeWidget::timeout() { - UAVDataObject *obj = Accels::GetInstance(getObjectManager()); + UAVDataObject *obj = AccelState::GetInstance(getObjectManager()); disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); - Accels *accels = Accels::GetInstance(getObjectManager()); - Gyros *gyros = Gyros::GetInstance(getObjectManager()); - accels->setMetadata(initialAccelsMdata); - gyros->setMetadata(initialGyrosMdata); + AccelState *accelState = AccelState::GetInstance(getObjectManager()); + GyroState *gyroState = GyroState::GetInstance(getObjectManager()); + accelState->setMetadata(initialAccelStateMdata); + gyroState->setMetadata(initialGyroStateMdata); QMessageBox msgBox; msgBox.setText(tr("Calibration timed out before receiving required updates.")); @@ -182,28 +182,28 @@ void ConfigCCAttitudeWidget::startAccelCalibration() AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); // Set up to receive updates - UAVDataObject *accels = Accels::GetInstance(getObjectManager()); - UAVDataObject *gyros = Gyros::GetInstance(getObjectManager()); - connect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); - connect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); + UAVDataObject *accelState = AccelState::GetInstance(getObjectManager()); + UAVDataObject *gyroState = GyroState::GetInstance(getObjectManager()); + connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); + connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); // Speed up updates - initialAccelsMdata = accels->getMetadata(); - UAVObject::Metadata accelsMdata = initialAccelsMdata; - UAVObject::SetFlightTelemetryUpdateMode(accelsMdata, UAVObject::UPDATEMODE_PERIODIC); - accelsMdata.flightTelemetryUpdatePeriod = 30; // ms - accels->setMetadata(accelsMdata); + initialAccelStateMdata = accelState->getMetadata(); + UAVObject::Metadata accelStateMdata = initialAccelStateMdata; + UAVObject::SetFlightTelemetryUpdateMode(accelStateMdata, UAVObject::UPDATEMODE_PERIODIC); + accelStateMdata.flightTelemetryUpdatePeriod = 30; // ms + accelState->setMetadata(accelStateMdata); - initialGyrosMdata = gyros->getMetadata(); - UAVObject::Metadata gyrosMdata = initialGyrosMdata; - UAVObject::SetFlightTelemetryUpdateMode(gyrosMdata, UAVObject::UPDATEMODE_PERIODIC); - gyrosMdata.flightTelemetryUpdatePeriod = 30; // ms - gyros->setMetadata(gyrosMdata); + initialGyroStateMdata = gyroState->getMetadata(); + UAVObject::Metadata gyroStateMdata = initialGyroStateMdata; + UAVObject::SetFlightTelemetryUpdateMode(gyroStateMdata, UAVObject::UPDATEMODE_PERIODIC); + gyroStateMdata.flightTelemetryUpdatePeriod = 30; // ms + gyroState->setMetadata(gyroStateMdata); // Set up timeout timer timer.setSingleShot(true); - timer.start(5000 + (NUM_SENSOR_UPDATES * qMax(accelsMdata.flightTelemetryUpdatePeriod, - gyrosMdata.flightTelemetryUpdatePeriod))); + timer.start(5000 + (NUM_SENSOR_UPDATES * qMax(accelStateMdata.flightTelemetryUpdatePeriod, + gyroStateMdata.flightTelemetryUpdatePeriod))); connect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); } diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h index c9b835050..0c74326b8 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h @@ -56,8 +56,8 @@ private slots: private: Ui_ccattitude *ui; QTimer timer; - UAVObject::Metadata initialAccelsMdata; - UAVObject::Metadata initialGyrosMdata; + UAVObject::Metadata initialAccelStateMdata; + UAVObject::Metadata initialGyroStateMdata; int accelUpdates; int gyroUpdates; diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index 5859a299f..c3dcbba6a 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -44,9 +44,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #define GRAVITY 9.81f #include "assertions.h" @@ -297,26 +297,26 @@ void ConfigRevoWidget::doStartAccelGyroBiasCalibration() UAVObject::Metadata mdata; /* Need to get as many accel updates as possible */ - Accels *accels = Accels::GetInstance(getObjectManager()); - Q_ASSERT(accels); - initialAccelsMdata = accels->getMetadata(); - mdata = initialAccelsMdata; + AccelState *accelState = AccelState::GetInstance(getObjectManager()); + Q_ASSERT(accelState); + initialAccelStateMdata = accelState->getMetadata(); + mdata = initialAccelStateMdata; UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; - accels->setMetadata(mdata); + accelState->setMetadata(mdata); - Gyros *gyros = Gyros::GetInstance(getObjectManager()); - Q_ASSERT(gyros); - initialGyrosMdata = gyros->getMetadata(); - mdata = initialGyrosMdata; + GyroState *gyroState = GyroState::GetInstance(getObjectManager()); + Q_ASSERT(gyroState); + initialGyroStateMdata = gyroState->getMetadata(); + mdata = initialGyroStateMdata; UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; - gyros->setMetadata(mdata); + gyroState->setMetadata(mdata); // Now connect to the accels and mag updates, gather for 100 samples collectingData = true; - connect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *))); - connect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *))); + connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *))); + connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *))); } /** @@ -329,26 +329,26 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj) Q_UNUSED(lock); switch (obj->getObjID()) { - case Accels::OBJID: + case AccelState::OBJID: { - Accels *accels = Accels::GetInstance(getObjectManager()); - Q_ASSERT(accels); - Accels::DataFields accelsData = accels->getData(); + AccelState *accelState = AccelState::GetInstance(getObjectManager()); + Q_ASSERT(accelState); + AccelState::DataFields accelStateData = accelState->getData(); - accel_accum_x.append(accelsData.x); - accel_accum_y.append(accelsData.y); - accel_accum_z.append(accelsData.z); + accel_accum_x.append(accelStateData.x); + accel_accum_y.append(accelStateData.y); + accel_accum_z.append(accelStateData.z); break; } - case Gyros::OBJID: + case GyroState::OBJID: { - Gyros *gyros = Gyros::GetInstance(getObjectManager()); - Q_ASSERT(gyros); - Gyros::DataFields gyrosData = gyros->getData(); + GyroState *gyroState = GyroState::GetInstance(getObjectManager()); + Q_ASSERT(gyroState); + GyroState::DataFields gyroStateData = gyroState->getData(); - gyro_accum_x.append(gyrosData.x); - gyro_accum_y.append(gyrosData.y); - gyro_accum_z.append(gyrosData.z); + gyro_accum_x.append(gyroStateData.x); + gyro_accum_y.append(gyroStateData.y); + gyro_accum_z.append(gyroStateData.z); break; } default: @@ -365,11 +365,11 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj) collectingData == true) { collectingData = false; - Accels *accels = Accels::GetInstance(getObjectManager()); - Gyros *gyros = Gyros::GetInstance(getObjectManager()); + AccelState *accelState = AccelState::GetInstance(getObjectManager()); + GyroState *gyroState = GyroState::GetInstance(getObjectManager()); - disconnect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *))); - disconnect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *))); + disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *))); + disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *))); m_ui->accelBiasStart->setEnabled(true); @@ -396,8 +396,8 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj) attitudeSettings->setData(attitudeSettingsData); attitudeSettings->updated(); - accels->setMetadata(initialAccelsMdata); - gyros->setMetadata(initialGyrosMdata); + accelState->setMetadata(initialAccelStateMdata); + gyroState->setMetadata(initialGyroStateMdata); // Recall saved board rotation recallBoardRotation(); @@ -574,21 +574,21 @@ void ConfigRevoWidget::doStartSixPointCalibration() #ifdef SIX_POINT_CAL_ACCEL /* Need to get as many accel updates as possible */ - Accels *accels = Accels::GetInstance(getObjectManager()); - Q_ASSERT(accels); + AccelState *accelState = AccelState::GetInstance(getObjectManager()); + Q_ASSERT(accelState); - initialAccelsMdata = accels->getMetadata(); - mdata = initialAccelsMdata; + initialAccelStateMdata = accelState->getMetadata(); + mdata = initialAccelStateMdata; UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; - accels->setMetadata(mdata); + accelState->setMetadata(mdata); #endif /* Need to get as many mag updates as possible */ - Magnetometer *mag = Magnetometer::GetInstance(getObjectManager()); + MagnetoState *mag = MagnetoState::GetInstance(getObjectManager()); Q_ASSERT(mag); - initialMagMdata = mag->getMetadata(); - mdata = initialMagMdata; + initialMagnetoStateMdata = mag->getMetadata(); + mdata = initialMagnetoStateMdata; UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; mag->setMetadata(mdata); @@ -622,12 +622,12 @@ void ConfigRevoWidget::savePositionData() collectingData = true; - Accels *accels = Accels::GetInstance(getObjectManager()); - Q_ASSERT(accels); - Magnetometer *mag = Magnetometer::GetInstance(getObjectManager()); + AccelState *accelState = AccelState::GetInstance(getObjectManager()); + Q_ASSERT(accelState); + MagnetoState *mag = MagnetoState::GetInstance(getObjectManager()); Q_ASSERT(mag); - connect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *))); + connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *))); connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *))); m_ui->sixPointCalibInstructions->append("Hold..."); @@ -644,19 +644,19 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj) // This is necessary to prevent a race condition on disconnect signal and another update if (collectingData == true) { - if (obj->getObjID() == Accels::OBJID) { + if (obj->getObjID() == AccelState::OBJID) { #ifdef SIX_POINT_CAL_ACCEL - Accels *accels = Accels::GetInstance(getObjectManager()); - Q_ASSERT(accels); - Accels::DataFields accelsData = accels->getData(); - accel_accum_x.append(accelsData.x); - accel_accum_y.append(accelsData.y); - accel_accum_z.append(accelsData.z); + AccelState *accelState = AccelState::GetInstance(getObjectManager()); + Q_ASSERT(accelState); + AccelState::DataFields accelStateData = accelState->getData(); + accel_accum_x.append(accelStateData.x); + accel_accum_y.append(accelStateData.y); + accel_accum_z.append(accelStateData.z); #endif - } else if (obj->getObjID() == Magnetometer::OBJID) { - Magnetometer *mag = Magnetometer::GetInstance(getObjectManager()); + } else if (obj->getObjID() == MagnetoState::OBJID) { + MagnetoState *mag = MagnetoState::GetInstance(getObjectManager()); Q_ASSERT(mag); - Magnetometer::DataFields magData = mag->getData(); + MagnetoState::DataFields magData = mag->getData(); mag_accum_x.append(magData.x); mag_accum_y.append(magData.y); mag_accum_z.append(magData.z); @@ -676,16 +676,16 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj) #ifdef SIX_POINT_CAL_ACCEL // Store the mean for this position for the accel - Accels *accels = Accels::GetInstance(getObjectManager()); - Q_ASSERT(accels); - disconnect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *))); + AccelState *accelState = AccelState::GetInstance(getObjectManager()); + Q_ASSERT(accelState); + disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *))); accel_data_x[position] = listMean(accel_accum_x); accel_data_y[position] = listMean(accel_accum_y); accel_data_z[position] = listMean(accel_accum_z); #endif // Store the mean for this position for the mag - Magnetometer *mag = Magnetometer::GetInstance(getObjectManager()); + MagnetoState *mag = MagnetoState::GetInstance(getObjectManager()); Q_ASSERT(mag); disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *))); mag_data_x[position] = listMean(mag_accum_x); @@ -720,9 +720,9 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj) /* Cleanup original settings */ #ifdef SIX_POINT_CAL_ACCEL - accels->setMetadata(initialAccelsMdata); + accelState->setMetadata(initialAccelStateMdata); #endif - mag->setMetadata(initialMagMdata); + mag->setMetadata(initialMagnetoStateMdata); // Recall saved board rotation recallBoardRotation(); @@ -924,36 +924,36 @@ void ConfigRevoWidget::doStartNoiseMeasurement() mag_accum_z.clear(); /* Need to get as many accel, mag and gyro updates as possible */ - Accels *accels = Accels::GetInstance(getObjectManager()); - Q_ASSERT(accels); - Gyros *gyros = Gyros::GetInstance(getObjectManager()); - Q_ASSERT(gyros); - Magnetometer *mag = Magnetometer::GetInstance(getObjectManager()); + AccelState *accelState = AccelState::GetInstance(getObjectManager()); + Q_ASSERT(accelState); + GyroState *gyroState = GyroState::GetInstance(getObjectManager()); + Q_ASSERT(gyroState); + MagnetoState *mag = MagnetoState::GetInstance(getObjectManager()); Q_ASSERT(mag); UAVObject::Metadata mdata; - initialAccelsMdata = accels->getMetadata(); - mdata = initialAccelsMdata; + initialAccelStateMdata = accelState->getMetadata(); + mdata = initialAccelStateMdata; UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; - accels->setMetadata(mdata); + accelState->setMetadata(mdata); - initialGyrosMdata = gyros->getMetadata(); - mdata = initialGyrosMdata; + initialGyroStateMdata = gyroState->getMetadata(); + mdata = initialGyroStateMdata; UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; - gyros->setMetadata(mdata); + gyroState->setMetadata(mdata); - initialMagMdata = mag->getMetadata(); - mdata = initialMagMdata; + initialMagnetoStateMdata = mag->getMetadata(); + mdata = initialMagnetoStateMdata; UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; mag->setMetadata(mdata); /* Connect for updates */ - connect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *))); - connect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *))); + connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *))); + connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *))); connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *))); } @@ -970,31 +970,31 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject *obj) Q_ASSERT(obj); switch (obj->getObjID()) { - case Gyros::OBJID: + case GyroState::OBJID: { - Gyros *gyros = Gyros::GetInstance(getObjectManager()); - Q_ASSERT(gyros); - Gyros::DataFields gyroData = gyros->getData(); + GyroState *gyroState = GyroState::GetInstance(getObjectManager()); + Q_ASSERT(gyroState); + GyroState::DataFields gyroData = gyroState->getData(); gyro_accum_x.append(gyroData.x); gyro_accum_y.append(gyroData.y); gyro_accum_z.append(gyroData.z); break; } - case Accels::OBJID: + case AccelState::OBJID: { - Accels *accels = Accels::GetInstance(getObjectManager()); - Q_ASSERT(accels); - Accels::DataFields accelsData = accels->getData(); - accel_accum_x.append(accelsData.x); - accel_accum_y.append(accelsData.y); - accel_accum_z.append(accelsData.z); + AccelState *accelState = AccelState::GetInstance(getObjectManager()); + Q_ASSERT(accelState); + AccelState::DataFields accelStateData = accelState->getData(); + accel_accum_x.append(accelStateData.x); + accel_accum_y.append(accelStateData.y); + accel_accum_z.append(accelStateData.z); break; } - case Magnetometer::OBJID: + case MagnetoState::OBJID: { - Magnetometer *mags = Magnetometer::GetInstance(getObjectManager()); + MagnetoState *mags = MagnetoState::GetInstance(getObjectManager()); Q_ASSERT(mags); - Magnetometer::DataFields magData = mags->getData(); + MagnetoState::DataFields magData = mags->getData(); mag_accum_x.append(magData.x); mag_accum_y.append(magData.y); mag_accum_z.append(magData.z); @@ -1017,11 +1017,11 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject *obj) gyro_accum_x.length() >= NOISE_SAMPLES && accel_accum_x.length() >= NOISE_SAMPLES) { // No need to for more updates - Magnetometer *mags = Magnetometer::GetInstance(getObjectManager()); - Accels *accels = Accels::GetInstance(getObjectManager()); - Gyros *gyros = Gyros::GetInstance(getObjectManager()); - disconnect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *))); - disconnect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *))); + MagnetoState *mags = MagnetoState::GetInstance(getObjectManager()); + AccelState *accelState = AccelState::GetInstance(getObjectManager()); + GyroState *gyroState = GyroState::GetInstance(getObjectManager()); + disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *))); + disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *))); disconnect(mags, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *))); EKFConfiguration *ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager()); diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.h b/ground/openpilotgcs/src/plugins/config/configrevowidget.h index 9eb349612..2e3789025 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.h +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.h @@ -90,10 +90,10 @@ private: double accel_data_x[6], accel_data_y[6], accel_data_z[6]; double mag_data_x[6], mag_data_y[6], mag_data_z[6]; - UAVObject::Metadata initialAccelsMdata; - UAVObject::Metadata initialGyrosMdata; - UAVObject::Metadata initialMagMdata; - UAVObject::Metadata initialBaroMdata; + UAVObject::Metadata initialAccelStateMdata; + UAVObject::Metadata initialGyroStateMdata; + UAVObject::Metadata initialMagnetoStateMdata; + UAVObject::Metadata initialBaroSensorMdata; float initialMagCorrectionRate; int position; diff --git a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp index 790265ea2..a73501b8e 100644 --- a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp @@ -268,12 +268,12 @@ void FGSimulator::processUpdate(const QByteArray & inp) float temperature = fields[19].toFloat(); // Get pressure (kpa) float pressure = fields[20].toFloat() * INHG2KPA; - // Get VelocityActual Down (cm/s) - float velocityActualDown = -fields[21].toFloat() * FPS2CMPS; - // Get VelocityActual East (cm/s) - float velocityActualEast = fields[22].toFloat() * FPS2CMPS; - // Get VelocityActual Down (cm/s) - float velocityActualNorth = fields[23].toFloat() * FPS2CMPS; + // Get VelocityState Down (cm/s) + float velocityStateDown = -fields[21].toFloat() * FPS2CMPS; + // Get VelocityState East (cm/s) + float velocityStateEast = fields[22].toFloat() * FPS2CMPS; + // Get VelocityState Down (cm/s) + float velocityStateNorth = fields[23].toFloat() * FPS2CMPS; // Get UDP packets received by FG int n = fields[24].toInt(); @@ -307,11 +307,11 @@ void FGSimulator::processUpdate(const QByteArray & inp) out.calibratedAirspeed = airspeed; - // Update BaroAltitude object + // Update BaroSensor object out.temperature = temperature; out.pressure = pressure; - // Update attActual object + // Update attState object out.roll = roll; // roll; out.pitch = pitch; // pitch out.heading = yaw; // yaw @@ -320,10 +320,10 @@ void FGSimulator::processUpdate(const QByteArray & inp) out.dstE = NED[1]; out.dstD = NED[2]; - // Update VelocityActual.{North,East,Down} - out.velNorth = velocityActualNorth; - out.velEast = velocityActualEast; - out.velDown = velocityActualDown; + // Update VelocityState.{North,East,Down} + out.velNorth = velocityStateNorth; + out.velEast = velocityStateEast; + out.velDown = velocityStateDown; // Update gyroscope sensor data out.rollRate = rollRate; diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp index 244c27b6c..a59a6c9d6 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp @@ -31,40 +31,40 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings *qSettings, QObj IUAVGadgetConfiguration(classId, parent) { // Default settings values - settings.simulatorId = ""; - settings.binPath = ""; - settings.dataPath = ""; - settings.manualControlEnabled = true; - settings.startSim = false; - settings.addNoise = false; - settings.hostAddress = "127.0.0.1"; - settings.remoteAddress = "127.0.0.1"; - settings.outPort = 0; + settings.simulatorId = ""; + settings.binPath = ""; + settings.dataPath = ""; + settings.manualControlEnabled = true; + settings.startSim = false; + settings.addNoise = false; + settings.hostAddress = "127.0.0.1"; + settings.remoteAddress = "127.0.0.1"; + settings.outPort = 0; settings.inPort = 0; - settings.latitude = ""; - settings.longitude = ""; + settings.latitude = ""; + settings.longitude = ""; - settings.attRawEnabled = false; - settings.attRawRate = 20; + settings.attRawEnabled = false; + settings.attRawRate = 20; - settings.attActualEnabled = true; - settings.attActHW = false; - settings.attActSim = true; - settings.attActCalc = false; + settings.attStateEnabled = true; + settings.attActHW = false; + settings.attActSim = true; + settings.attActCalc = false; - settings.gpsPositionEnabled = false; - settings.gpsPosRate = 100; + settings.gpsPositionEnabled = false; + settings.gpsPosRate = 100; - settings.groundTruthEnabled = false; - settings.groundTruthRate = 100; + settings.groundTruthEnabled = false; + settings.groundTruthRate = 100; - settings.inputCommand = false; - settings.gcsReceiverEnabled = false; - settings.manualControlEnabled = false; - settings.minOutputPeriod = 100; + settings.inputCommand = false; + settings.gcsReceiverEnabled = false; + settings.manualControlEnabled = false; + settings.minOutputPeriod = 100; - settings.airspeedActualEnabled = false; - settings.airspeedActualRate = 100; + settings.airspeedStateEnabled = false; + settings.airspeedStateRate = 100; // if a saved configuration exists load it, and overwrite defaults @@ -86,28 +86,28 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings *qSettings, QObj settings.gcsReceiverEnabled = qSettings->value("gcsReceiverEnabled").toBool(); settings.manualControlEnabled = qSettings->value("manualControlEnabled").toBool(); - settings.attRawEnabled = qSettings->value("attRawEnabled").toBool(); - settings.attRawRate = qSettings->value("attRawRate").toInt(); + settings.attRawEnabled = qSettings->value("attRawEnabled").toBool(); + settings.attRawRate = qSettings->value("attRawRate").toInt(); - settings.attActualEnabled = qSettings->value("attActualEnabled").toBool(); + settings.attStateEnabled = qSettings->value("attStateEnabled").toBool(); settings.attActHW = qSettings->value("attActHW").toBool(); settings.attActSim = qSettings->value("attActSim").toBool(); - settings.attActCalc = qSettings->value("attActCalc").toBool(); + settings.attActCalc = qSettings->value("attActCalc").toBool(); - settings.baroAltitudeEnabled = qSettings->value("baroAltitudeEnabled").toBool(); - settings.baroAltRate = qSettings->value("baroAltRate").toInt(); + settings.baroSensorEnabled = qSettings->value("baroSensorEnabled").toBool(); + settings.baroAltRate = qSettings->value("baroAltRate").toInt(); - settings.gpsPositionEnabled = qSettings->value("gpsPositionEnabled").toBool(); - settings.gpsPosRate = qSettings->value("gpsPosRate").toInt(); + settings.gpsPositionEnabled = qSettings->value("gpsPositionEnabled").toBool(); + settings.gpsPosRate = qSettings->value("gpsPosRate").toInt(); - settings.groundTruthEnabled = qSettings->value("groundTruthEnabled").toBool(); - settings.groundTruthRate = qSettings->value("groundTruthRate").toInt(); + settings.groundTruthEnabled = qSettings->value("groundTruthEnabled").toBool(); + settings.groundTruthRate = qSettings->value("groundTruthRate").toInt(); - settings.inputCommand = qSettings->value("inputCommand").toBool(); - settings.minOutputPeriod = qSettings->value("minOutputPeriod").toInt(); + settings.inputCommand = qSettings->value("inputCommand").toBool(); + settings.minOutputPeriod = qSettings->value("minOutputPeriod").toInt(); - settings.airspeedActualEnabled = qSettings->value("airspeedActualEnabled").toBool(); - settings.airspeedActualRate = qSettings->value("airspeedActualRate").toInt(); + settings.airspeedStateEnabled = qSettings->value("airspeedStateEnabled").toBool(); + settings.airspeedStateRate = qSettings->value("airspeedStateRate").toInt(); } } @@ -144,11 +144,11 @@ void HITLConfiguration::saveConfig(QSettings *qSettings) const qSettings->setValue("attRawEnabled", settings.attRawEnabled); qSettings->setValue("attRawRate", settings.attRawRate); - qSettings->setValue("attActualEnabled", settings.attActualEnabled); + qSettings->setValue("attStateEnabled", settings.attStateEnabled); qSettings->setValue("attActHW", settings.attActHW); qSettings->setValue("attActSim", settings.attActSim); qSettings->setValue("attActCalc", settings.attActCalc); - qSettings->setValue("baroAltitudeEnabled", settings.baroAltitudeEnabled); + qSettings->setValue("baroSensorEnabled", settings.baroSensorEnabled); qSettings->setValue("baroAltRate", settings.baroAltRate); qSettings->setValue("gpsPositionEnabled", settings.gpsPositionEnabled); qSettings->setValue("gpsPosRate", settings.gpsPosRate); @@ -157,6 +157,6 @@ void HITLConfiguration::saveConfig(QSettings *qSettings) const qSettings->setValue("inputCommand", settings.inputCommand); qSettings->setValue("minOutputPeriod", settings.minOutputPeriod); - qSettings->setValue("airspeedActualEnabled", settings.airspeedActualEnabled); - qSettings->setValue("airspeedActualRate", settings.airspeedActualRate); + qSettings->setValue("airspeedStateEnabled", settings.airspeedStateEnabled); + qSettings->setValue("airspeedStateRate", settings.airspeedStateRate); } diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp index 637707c44..a9ec1414e 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp @@ -44,9 +44,9 @@ Noise HitlNoiseGeneration::getNoise() Noise HitlNoiseGeneration::generateNoise() { - noise.accelData.x = 0; - noise.accelData.y = 0; - noise.accelData.z = 0; + noise.accelStateData.x = 0; + noise.accelStateData.y = 0; + noise.accelStateData.z = 0; noise.gpsPosData.Latitude = 0; noise.gpsPosData.Longitude = 0; @@ -54,25 +54,25 @@ Noise HitlNoiseGeneration::generateNoise() noise.gpsPosData.Heading = 0; noise.gpsPosData.Altitude = 0; - noise.gpsVelData.North = 0; - noise.gpsVelData.East = 0; - noise.gpsVelData.Down = 0; + noise.gpsVelData.North = 0; + noise.gpsVelData.East = 0; + noise.gpsVelData.Down = 0; noise.baroAltData.Altitude = 0; - noise.attActualData.Roll = 0; - noise.attActualData.Pitch = 0; - noise.attActualData.Yaw = 0; + noise.attStateData.Roll = 0; + noise.attStateData.Pitch = 0; + noise.attStateData.Yaw = 0; - noise.gyroData.x = 0; - noise.gyroData.y = 0; - noise.gyroData.z = 0; + noise.gyroStateData.x = 0; + noise.gyroStateData.y = 0; + noise.gyroStateData.z = 0; - noise.accelData.x = 0; - noise.accelData.y = 0; - noise.accelData.z = 0; + noise.accelStateData.x = 0; + noise.accelStateData.y = 0; + noise.accelStateData.z = 0; - noise.airspeedActual.CalibratedAirspeed = 0; + noise.airspeedState.CalibratedAirspeed = 0; return noise; } diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h index ead9ba9dd..9ecbbd205 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h @@ -37,16 +37,16 @@ #include struct Noise { - Accels::DataFields accelData; - AttitudeActual::DataFields attActualData; - BaroAltitude::DataFields baroAltData; - AirspeedActual::DataFields airspeedActual; - GPSPosition::DataFields gpsPosData; - GPSVelocity::DataFields gpsVelData; - Gyros::DataFields gyroData; - HomeLocation::DataFields homeData; - PositionActual::DataFields positionActualData; - VelocityActual::DataFields velocityActualData; + AccelState::DataFields accelStateData; + AttitudeState::DataFields attStateData; + BaroSensor::DataFields baroAltData; + AirspeedState::DataFields airspeedState; + GPSPosition::DataFields gpsPosData; + GPSVelocity::DataFields gpsVelData; + GyroState::DataFields gyroStateData; + HomeLocation::DataFields homeData; + PositionState::DataFields positionStateData; + VelocityState::DataFields velocityStateData; }; class HitlNoiseGeneration { diff --git a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp index c85b2fb11..6cb69ddc7 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp @@ -92,16 +92,16 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent) m_optionsPage->groundTruthCheckbox->setChecked(config->Settings().groundTruthEnabled); m_optionsPage->gpsPositionCheckbox->setChecked(config->Settings().gpsPositionEnabled); - m_optionsPage->attActualCheckbox->setChecked(config->Settings().attActualEnabled); + m_optionsPage->attStateCheckbox->setChecked(config->Settings().attStateEnabled); m_optionsPage->attRawCheckbox->setChecked(config->Settings().attRawEnabled); m_optionsPage->attRawRateSpinbox->setValue(config->Settings().attRawRate); m_optionsPage->gpsPosRateSpinbox->setValue(config->Settings().gpsPosRate); m_optionsPage->groundTruthRateSpinbox->setValue(config->Settings().groundTruthRate); -// m_optionsPage->attActualRate->setValue(config->Settings().attActualRate); +// m_optionsPage->attStateRate->setValue(config->Settings().attStateRate); - m_optionsPage->baroAltitudeCheckbox->setChecked(config->Settings().baroAltitudeEnabled); + m_optionsPage->baroAltitudeCheckbox->setChecked(config->Settings().baroSensorEnabled); m_optionsPage->baroAltRateSpinbox->setValue(config->Settings().baroAltRate); m_optionsPage->minOutputPeriodSpinbox->setValue(config->Settings().minOutputPeriod); @@ -110,8 +110,8 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent) m_optionsPage->attActCalc->setChecked(config->Settings().attActCalc); m_optionsPage->attActSim->setChecked(config->Settings().attActSim); - m_optionsPage->airspeedActualCheckbox->setChecked(config->Settings().airspeedActualEnabled); - m_optionsPage->airspeedRateSpinbox->setValue(config->Settings().airspeedActualRate); + m_optionsPage->airspeedStateCheckbox->setChecked(config->Settings().airspeedStateEnabled); + m_optionsPage->airspeedRateSpinbox->setValue(config->Settings().airspeedStateRate); return optionsPageWidget; } @@ -121,46 +121,46 @@ void HITLOptionsPage::apply() SimulatorSettings settings; int i = m_optionsPage->chooseFlightSimulator->currentIndex(); - settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); - settings.binPath = m_optionsPage->executablePath->path(); - settings.dataPath = m_optionsPage->dataPath->path(); - settings.startSim = m_optionsPage->startSim->isChecked(); - settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); - settings.hostAddress = m_optionsPage->hostAddress->text(); - settings.remoteAddress = m_optionsPage->remoteAddress->text(); + settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); + settings.binPath = m_optionsPage->executablePath->path(); + settings.dataPath = m_optionsPage->dataPath->path(); + settings.startSim = m_optionsPage->startSim->isChecked(); + settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); + settings.hostAddress = m_optionsPage->hostAddress->text(); + settings.remoteAddress = m_optionsPage->remoteAddress->text(); settings.inPort = m_optionsPage->inputPort->text().toInt(); - settings.outPort = m_optionsPage->outputPort->text().toInt(); - settings.longitude = m_optionsPage->longitude->text(); - settings.latitude = m_optionsPage->latitude->text(); + settings.outPort = m_optionsPage->outputPort->text().toInt(); + settings.longitude = m_optionsPage->longitude->text(); + settings.latitude = m_optionsPage->latitude->text(); - settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); + settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); - settings.attRawEnabled = m_optionsPage->attRawCheckbox->isChecked(); - settings.attRawRate = m_optionsPage->attRawRateSpinbox->value(); + settings.attRawEnabled = m_optionsPage->attRawCheckbox->isChecked(); + settings.attRawRate = m_optionsPage->attRawRateSpinbox->value(); - settings.attActualEnabled = m_optionsPage->attActualCheckbox->isChecked(); + settings.attStateEnabled = m_optionsPage->attStateCheckbox->isChecked(); - settings.gpsPositionEnabled = m_optionsPage->gpsPositionCheckbox->isChecked(); - settings.gpsPosRate = m_optionsPage->gpsPosRateSpinbox->value(); + settings.gpsPositionEnabled = m_optionsPage->gpsPositionCheckbox->isChecked(); + settings.gpsPosRate = m_optionsPage->gpsPosRateSpinbox->value(); - settings.groundTruthEnabled = m_optionsPage->groundTruthCheckbox->isChecked(); - settings.groundTruthRate = m_optionsPage->groundTruthRateSpinbox->value(); + settings.groundTruthEnabled = m_optionsPage->groundTruthCheckbox->isChecked(); + settings.groundTruthRate = m_optionsPage->groundTruthRateSpinbox->value(); - settings.baroAltitudeEnabled = m_optionsPage->baroAltitudeCheckbox->isChecked(); - settings.baroAltRate = m_optionsPage->baroAltRateSpinbox->value(); + settings.baroSensorEnabled = m_optionsPage->baroAltitudeCheckbox->isChecked(); + settings.baroAltRate = m_optionsPage->baroAltRateSpinbox->value(); - settings.minOutputPeriod = m_optionsPage->minOutputPeriodSpinbox->value(); + settings.minOutputPeriod = m_optionsPage->minOutputPeriodSpinbox->value(); - settings.manualControlEnabled = m_optionsPage->manualControlRadioButton->isChecked(); - settings.gcsReceiverEnabled = m_optionsPage->gcsReceiverRadioButton->isChecked(); + settings.manualControlEnabled = m_optionsPage->manualControlRadioButton->isChecked(); + settings.gcsReceiverEnabled = m_optionsPage->gcsReceiverRadioButton->isChecked(); - settings.attActHW = m_optionsPage->attActHW->isChecked(); - settings.attActSim = m_optionsPage->attActSim->isChecked(); - settings.attActCalc = m_optionsPage->attActCalc->isChecked(); + settings.attActHW = m_optionsPage->attActHW->isChecked(); + settings.attActSim = m_optionsPage->attActSim->isChecked(); + settings.attActCalc = m_optionsPage->attActCalc->isChecked(); - settings.airspeedActualEnabled = m_optionsPage->airspeedActualCheckbox->isChecked(); - settings.airspeedActualRate = m_optionsPage->airspeedRateSpinbox->value(); + settings.airspeedStateEnabled = m_optionsPage->airspeedStateCheckbox->isChecked(); + settings.airspeedStateRate = m_optionsPage->airspeedRateSpinbox->value(); // Write settings to file config->setSimulatorSettings(settings); diff --git a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui index 543feff6a..1c822fa45 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui +++ b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui @@ -510,12 +510,12 @@ - + true - AttitudeActual + AttitudeState true @@ -760,7 +760,7 @@ 0 - + 0 @@ -768,7 +768,7 @@ - AirspeedActual + AirspeedState true diff --git a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp index c4936d33d..ffe1764d4 100644 --- a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp @@ -262,20 +262,20 @@ void IL2Simulator::processUpdate(const QByteArray & inp) out.dstE = current.X; out.dstD = -current.Z; - // Update BaroAltitude object + // Update BaroSensor object out.altitude = current.Z; out.agl = current.Z; out.temperature = airParameters.groundTemp + (current.Z * airParameters.tempLapseRate) - 273.0; out.pressure = airPressureFromAltitude(current.Z, airParameters, gravity); // kpa - // Update attActual object + // Update attState object out.roll = current.roll; // roll; out.pitch = current.pitch; // pitch out.heading = current.azimuth; // yaw - // Update VelocityActual.{North,East,Down} + // Update VelocityState.{North,East,Down} out.velNorth = current.dY; out.velEast = current.dX; out.velDown = -current.dZ; diff --git a/ground/openpilotgcs/src/plugins/hitl/isimulator.h b/ground/openpilotgcs/src/plugins/hitl/isimulator.h index 842e69a8f..12124a5bf 100644 --- a/ground/openpilotgcs/src/plugins/hitl/isimulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/isimulator.h @@ -9,10 +9,10 @@ #include "uavtalk/telemetrymanager.h" #include "uavobjectmanager.h" #include "actuatordesired.h" -#include "altitudeactual.h" -#include "attitudeactual.h" -#include "velocityactual.h" -#include "positionactual.h" +#include "altitudestate.h" +#include "attitudestate.h" +#include "velocitystate.h" +#include "positionstate.h" #include "gcstelemetrystats.h" class Simulator : public QObject { @@ -48,10 +48,10 @@ private: QUdpSocket *inSocket; QUdpSocket *outSocket; ActuatorDesired *actDesired; - AltitudeActual *altActual; - VelocityActual *velActual; - AttitudeActual *attActual; - PositionActual *posActual; + AltitudeState *altState; + VelocityState *velState; + AttitudeState *attState; + PositionState *posState; GCSTelemetryStats *telStats; QHostAddress fgHost; int inPort; diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp index f3d594d36..13bd35bbf 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp @@ -64,13 +64,13 @@ Simulator::Simulator(const SimulatorSettings & params) : emit myStart(); QTime currentTime = QTime::currentTime(); - gpsPosTime = currentTime; - groundTruthTime = currentTime; - gcsRcvrTime = currentTime; - attRawTime = currentTime; - baroAltTime = currentTime; + gpsPosTime = currentTime; + groundTruthTime = currentTime; + gcsRcvrTime = currentTime; + attRawTime = currentTime; + baroAltTime = currentTime; battTime = currentTime; - airspeedActualTime = currentTime; + airspeedStateTime = currentTime; // Define standard atmospheric constants airParameters.univGasConstant = 8.31447; // [J/(mol·K)] @@ -146,20 +146,20 @@ void Simulator::onStart() manCtrlCommand = ManualControlCommand::GetInstance(objManager); gcsReceiver = GCSReceiver::GetInstance(objManager); flightStatus = FlightStatus::GetInstance(objManager); - posHome = HomeLocation::GetInstance(objManager); - velActual = VelocityActual::GetInstance(objManager); - posActual = PositionActual::GetInstance(objManager); - baroAlt = BaroAltitude::GetInstance(objManager); - flightBatt = FlightBatteryState::GetInstance(objManager); - airspeedActual = AirspeedActual::GetInstance(objManager); - attActual = AttitudeActual::GetInstance(objManager); - attSettings = AttitudeSettings::GetInstance(objManager); - accels = Accels::GetInstance(objManager); - gyros = Gyros::GetInstance(objManager); - gpsPos = GPSPosition::GetInstance(objManager); - gpsVel = GPSVelocity::GetInstance(objManager); - telStats = GCSTelemetryStats::GetInstance(objManager); - groundTruth = GroundTruth::GetInstance(objManager); + posHome = HomeLocation::GetInstance(objManager); + velState = VelocityState::GetInstance(objManager); + posState = PositionState::GetInstance(objManager); + baroAlt = BaroSensor::GetInstance(objManager); + flightBatt = FlightBatteryState::GetInstance(objManager); + airspeedState = AirspeedState::GetInstance(objManager); + attState = AttitudeState::GetInstance(objManager); + attSettings = AttitudeSettings::GetInstance(objManager); + accelState = AccelState::GetInstance(objManager); + gyroState = GyroState::GetInstance(objManager); + gpsPos = GPSPosition::GetInstance(objManager); + gpsVel = GPSVelocity::GetInstance(objManager); + telStats = GCSTelemetryStats::GetInstance(objManager); + groundTruth = GroundTruth::GetInstance(objManager); // Listen to autopilot connection events TelemetryManager *telMngr = pm->getObject(); @@ -257,30 +257,30 @@ void Simulator::setupObjects() } if (settings.groundTruthEnabled) { - setupOutputObject(posActual, settings.groundTruthRate); - setupOutputObject(velActual, settings.groundTruthRate); + setupOutputObject(posState, settings.groundTruthRate); + setupOutputObject(velState, settings.groundTruthRate); } if (settings.attRawEnabled) { - setupOutputObject(accels, settings.attRawRate); - setupOutputObject(gyros, settings.attRawRate); + setupOutputObject(accelState, settings.attRawRate); + setupOutputObject(gyroState, settings.attRawRate); } - if (settings.attActualEnabled && settings.attActHW) { - setupOutputObject(accels, settings.attRawRate); - setupOutputObject(gyros, settings.attRawRate); + if (settings.attStateEnabled && settings.attActHW) { + setupOutputObject(accelState, settings.attRawRate); + setupOutputObject(gyroState, settings.attRawRate); } - if (settings.attActualEnabled && !settings.attActHW) { - setupOutputObject(attActual, 20); // Hardcoded? Bleh. + if (settings.attStateEnabled && !settings.attActHW) { + setupOutputObject(attState, 20); // Hardcoded? Bleh. } else { - setupWatchedObject(attActual, 100); // Hardcoded? Bleh. + setupWatchedObject(attState, 100); // Hardcoded? Bleh. } - if (settings.airspeedActualEnabled) { - setupOutputObject(airspeedActual, settings.airspeedActualRate); + if (settings.airspeedStateEnabled) { + setupOutputObject(airspeedState, settings.airspeedStateRate); } - if (settings.baroAltitudeEnabled) { + if (settings.baroSensorEnabled) { setupOutputObject(baroAlt, settings.baroAltRate); setupOutputObject(flightBatt, settings.baroAltRate); } @@ -462,31 +462,31 @@ void Simulator::updateUAVOs(Output2Hardware out) groundTruth->setData(groundTruthData); /*******************************/ - // Update attActual object - AttitudeActual::DataFields attActualData; - attActualData = attActual->getData(); + // Update attState object + AttitudeState::DataFields attStateData; + attStateData = attState->getData(); if (settings.attActHW) { // do nothing /*****************************************/ } else if (settings.attActSim) { // take all data from simulator - attActualData.Roll = out.roll + noise.attActualData.Roll; // roll; - attActualData.Pitch = out.pitch + noise.attActualData.Pitch; // pitch - attActualData.Yaw = out.heading + noise.attActualData.Yaw; // Yaw + attStateData.Roll = out.roll + noise.attStateData.Roll; // roll; + attStateData.Pitch = out.pitch + noise.attStateData.Pitch; // pitch + attStateData.Yaw = out.heading + noise.attStateData.Yaw; // Yaw float rpy[3]; float quat[4]; - rpy[0] = attActualData.Roll; - rpy[1] = attActualData.Pitch; - rpy[2] = attActualData.Yaw; + rpy[0] = attStateData.Roll; + rpy[1] = attStateData.Pitch; + rpy[2] = attStateData.Yaw; Utils::CoordinateConversions().RPY2Quaternion(rpy, quat); - attActualData.q1 = quat[0]; - attActualData.q2 = quat[1]; - attActualData.q3 = quat[2]; - attActualData.q4 = quat[3]; + attStateData.q1 = quat[0]; + attStateData.q2 = quat[1]; + attStateData.q3 = quat[2]; + attStateData.q4 = quat[3]; // Set UAVO - attActual->setData(attActualData); + attState->setData(attStateData); /*****************************************/ } else if (settings.attActCalc) { // calculate RPY with code from Attitude module @@ -598,16 +598,16 @@ void Simulator::updateUAVOs(Output2Hardware out) rpy2[0] = RAD2DEG * atan2f(R23, R33); } - attActualData.Roll = rpy2[0]; - attActualData.Pitch = rpy2[1]; - attActualData.Yaw = rpy2[2]; - attActualData.q1 = q[0]; - attActualData.q2 = q[1]; - attActualData.q3 = q[2]; - attActualData.q4 = q[3]; + attStateData.Roll = rpy2[0]; + attStateData.Pitch = rpy2[1]; + attStateData.Yaw = rpy2[2]; + attStateData.q1 = q[0]; + attStateData.q2 = q[1]; + attStateData.q3 = q[2]; + attStateData.q4 = q[3]; // Set UAVO - attActual->setData(attActualData); + attState->setData(attStateData); /*****************************************/ } @@ -662,23 +662,23 @@ void Simulator::updateUAVOs(Output2Hardware out) } /*******************************/ - // Update VelocityActual.{North,East,Down} + // Update VelocityState.{North,East,Down} if (settings.groundTruthEnabled) { if (groundTruthTime.msecsTo(currentTime) >= settings.groundTruthRate) { - VelocityActual::DataFields velocityActualData; - memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields)); - velocityActualData.North = out.velNorth + noise.velocityActualData.North; - velocityActualData.East = out.velEast + noise.velocityActualData.East; - velocityActualData.Down = out.velDown + noise.velocityActualData.Down; - velActual->setData(velocityActualData); + VelocityState::DataFields velocityStateData; + memset(&velocityStateData, 0, sizeof(VelocityState::DataFields)); + velocityStateData.North = out.velNorth + noise.velocityStateData.North; + velocityStateData.East = out.velEast + noise.velocityStateData.East; + velocityStateData.Down = out.velDown + noise.velocityStateData.Down; + velState->setData(velocityStateData); - // Update PositionActual.{Nort,East,Down} - PositionActual::DataFields positionActualData; - memset(&positionActualData, 0, sizeof(PositionActual::DataFields)); - positionActualData.North = (out.dstN - initN) + noise.positionActualData.North; - positionActualData.East = (out.dstE - initE) + noise.positionActualData.East; - positionActualData.Down = (out.dstD /*-initD*/) + noise.positionActualData.Down; - posActual->setData(positionActualData); + // Update PositionState.{Nort,East,Down} + PositionState::DataFields positionStateData; + memset(&positionStateData, 0, sizeof(PositionState::DataFields)); + positionStateData.North = (out.dstN - initN) + noise.positionStateData.North; + positionStateData.East = (out.dstE - initE) + noise.positionStateData.East; + positionStateData.Down = (out.dstD /*-initD*/) + noise.positionStateData.Down; + posState->setData(positionStateData); groundTruthTime = groundTruthTime.addMSecs(settings.groundTruthRate); } @@ -707,11 +707,11 @@ void Simulator::updateUAVOs(Output2Hardware out) // } /*******************************/ - // Update BaroAltitude object - if (settings.baroAltitudeEnabled) { + // Update BaroSensor object + if (settings.baroSensorEnabled) { if (baroAltTime.msecsTo(currentTime) >= settings.baroAltRate) { - BaroAltitude::DataFields baroAltData; - memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields)); + BaroSensor::DataFields baroAltData; + memset(&baroAltData, 0, sizeof(BaroSensor::DataFields)); baroAltData.Altitude = out.altitude + noise.baroAltData.Altitude; baroAltData.Temperature = out.temperature + noise.baroAltData.Temperature; baroAltData.Pressure = out.pressure + noise.baroAltData.Pressure; @@ -723,7 +723,7 @@ void Simulator::updateUAVOs(Output2Hardware out) /*******************************/ // Update FlightBatteryState object - if (settings.baroAltitudeEnabled) { + if (settings.baroSensorEnabled) { if (battTime.msecsTo(currentTime) >= settings.baroAltRate) { FlightBatteryState::DataFields batteryData; memset(&batteryData, 0, sizeof(FlightBatteryState::DataFields)); @@ -737,18 +737,18 @@ void Simulator::updateUAVOs(Output2Hardware out) } /*******************************/ - // Update AirspeedActual object - if (settings.airspeedActualEnabled) { - if (airspeedActualTime.msecsTo(currentTime) >= settings.airspeedActualRate) { - AirspeedActual::DataFields airspeedActualData; - memset(&airspeedActualData, 0, sizeof(AirspeedActual::DataFields)); - airspeedActualData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedActual.CalibratedAirspeed; - airspeedActualData.TrueAirspeed = out.trueAirspeed + noise.airspeedActual.TrueAirspeed; - // airspeedActualData.alpha=out.angleOfAttack; // to be implemented - // airspeedActualData.beta=out.angleOfSlip; - airspeedActual->setData(airspeedActualData); + // Update AirspeedState object + if (settings.airspeedStateEnabled) { + if (airspeedStateTime.msecsTo(currentTime) >= settings.airspeedStateRate) { + AirspeedState::DataFields airspeedStateData; + memset(&airspeedStateData, 0, sizeof(AirspeedState::DataFields)); + airspeedStateData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedState.CalibratedAirspeed; + airspeedStateData.TrueAirspeed = out.trueAirspeed + noise.airspeedState.TrueAirspeed; + // airspeedStateData.alpha=out.angleOfAttack; // to be implemented + // airspeedStateData.beta=out.angleOfSlip; + airspeedState->setData(airspeedStateData); - airspeedActualTime = airspeedActualTime.addMSecs(settings.airspeedActualRate); + airspeedStateTime = airspeedStateTime.addMSecs(settings.airspeedStateRate); } } @@ -757,22 +757,22 @@ void Simulator::updateUAVOs(Output2Hardware out) if (settings.attRawEnabled) { if (attRawTime.msecsTo(currentTime) >= settings.attRawRate) { // Update gyroscope sensor data - Gyros::DataFields gyroData; - memset(&gyroData, 0, sizeof(Gyros::DataFields)); - gyroData.x = out.rollRate + noise.gyroData.x; - gyroData.y = out.pitchRate + noise.gyroData.y; - gyroData.z = out.yawRate + noise.gyroData.z; - gyros->setData(gyroData); + GyroState::DataFields gyroStateData; + memset(&gyroStateData, 0, sizeof(GyroState::DataFields)); + gyroStateData.x = out.rollRate + noise.gyroStateData.x; + gyroStateData.y = out.pitchRate + noise.gyroStateData.y; + gyroStateData.z = out.yawRate + noise.gyroStateData.z; + gyroState->setData(gyroStateData); // Update accelerometer sensor data - Accels::DataFields accelData; - memset(&accelData, 0, sizeof(Accels::DataFields)); - accelData.x = out.accX + noise.accelData.x; - accelData.y = out.accY + noise.accelData.y; - accelData.z = out.accZ + noise.accelData.z; - accels->setData(accelData); + AccelState::DataFields accelStateData; + memset(&accelStateData, 0, sizeof(AccelState::DataFields)); + accelStateData.x = out.accX + noise.accelStateData.x; + accelStateData.y = out.accY + noise.accelStateData.y; + accelStateData.z = out.accZ + noise.accelStateData.z; + accelState->setData(accelStateData); - attRawTime = attRawTime.addMSecs(settings.attRawRate); + attRawTime = attRawTime.addMSecs(settings.attRawRate); } } } diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.h b/ground/openpilotgcs/src/plugins/hitl/simulator.h index e9136a031..ecf4ca872 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.h @@ -38,13 +38,13 @@ #include "uavtalk/telemetrymanager.h" #include "uavobjectmanager.h" -#include "accels.h" +#include "accelstate.h" #include "actuatorcommand.h" #include "actuatordesired.h" -#include "airspeedactual.h" -#include "attitudeactual.h" +#include "airspeedstate.h" +#include "attitudestate.h" #include "attitudesettings.h" -#include "baroaltitude.h" +#include "barosensor.h" #include "flightbatterystate.h" #include "flightstatus.h" #include "gcsreceiver.h" @@ -52,12 +52,12 @@ #include "gpsposition.h" #include "gpsvelocity.h" #include "groundtruth.h" -#include "gyros.h" +#include "gyrostate.h" #include "homelocation.h" #include "manualcontrolcommand.h" -#include "positionactual.h" +#include "positionstate.h" #include "sonaraltitude.h" -#include "velocityactual.h" +#include "velocitystate.h" #include "utils/coordinateconversions.h" @@ -130,12 +130,12 @@ typedef struct _CONNECTION { bool attRawEnabled; quint8 attRawRate; - bool attActualEnabled; + bool attStateEnabled; bool attActHW; bool attActSim; bool attActCalc; - bool baroAltitudeEnabled; + bool baroSensorEnabled; quint16 baroAltRate; bool groundTruthEnabled; @@ -149,8 +149,8 @@ typedef struct _CONNECTION { bool manualControlEnabled; quint16 minOutputPeriod; - bool airspeedActualEnabled; - quint16 airspeedActualRate; + bool airspeedStateEnabled; + quint16 airspeedStateRate; } SimulatorSettings; @@ -321,17 +321,17 @@ protected: ManualControlCommand *manCtrlCommand; FlightStatus *flightStatus; FlightBatteryState *flightBatt; - BaroAltitude *baroAlt; - AirspeedActual *airspeedActual; - AttitudeActual *attActual; + BaroSensor *baroAlt; + AirspeedState *airspeedState; + AttitudeState *attState; AttitudeSettings *attSettings; - VelocityActual *velActual; + VelocityState *velState; GPSPosition *gpsPos; GPSVelocity *gpsVel; - PositionActual *posActual; + PositionState *posState; HomeLocation *posHome; - Accels *accels; - Gyros *gyros; + AccelState *accelState; + GyroState *gyroState; GCSTelemetryStats *telStats; GCSReceiver *gcsReceiver; GroundTruth *groundTruth; @@ -361,7 +361,7 @@ private: QTime baroAltTime; QTime battTime; QTime gcsRcvrTime; - QTime airspeedActualTime; + QTime airspeedStateTime; QString name; QString simulatorId; diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp index f26dc3229..f450c25b1 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp @@ -300,11 +300,11 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf) out.calibratedAirspeed = airspeed_keas * 1.15 * 1.6089 / 3.6; // Convert from [kts] to [m/s] - // Update BaroAltitude object + // Update BaroSensor object out.temperature = temperature; out.pressure = pressure; - // Update attActual object + // Update attState object out.roll = roll; // roll; out.pitch = pitch; // pitch out.heading = heading; // yaw @@ -314,7 +314,7 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf) out.dstE = dstX; out.dstD = -dstZ; - // Update VelocityActual.{North,East,Down} + // Update VelocityState.{North,East,Down} out.velNorth = velY; out.velEast = velX; out.velDown = -velZ; @@ -332,9 +332,9 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf) updateUAVOs(out); } // issue manual update - // attActual->updated(); - // altActual->updated(); - // posActual->updated(); + // attState->updated(); + // altState->updated(); + // posState->updated(); } diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp index bd7d9e585..91a46e787 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp @@ -48,11 +48,11 @@ MagicWaypointGadgetWidget::MagicWaypointGadgetWidget(QWidget *parent) : QLabel(p // Connect object updated event from UAVObject to also update check boxes connect(getPathDesired(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(positionObjectChanged(UAVObject *))); - connect(getPositionActual(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(positionObjectChanged(UAVObject *))); + connect(getPositionState(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(positionObjectChanged(UAVObject *))); // Connect updates from the position widget to this widget connect(m_magicwaypoint->widgetPosition, SIGNAL(positionClicked(double, double)), this, SLOT(positionSelected(double, double))); - connect(this, SIGNAL(positionActualObjectChanged(double, double)), m_magicwaypoint->widgetPosition, SLOT(updateActualIndicator(double, double))); + connect(this, SIGNAL(positionStateObjectChanged(double, double)), m_magicwaypoint->widgetPosition, SLOT(updateActualIndicator(double, double))); connect(this, SIGNAL(positionDesiredObjectChanged(double, double)), m_magicwaypoint->widgetPosition, SLOT(updateDesiredIndicator(double, double))); // Catch changes in scale for visualization @@ -81,13 +81,13 @@ PathDesired *MagicWaypointGadgetWidget::getPathDesired() } /*! - \brief Returns the @ref PositionActual UAVObject + \brief Returns the @ref PositionState UAVObject */ -PositionActual *MagicWaypointGadgetWidget::getPositionActual() +PositionState *MagicWaypointGadgetWidget::getPositionState() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - PositionActual *obj = PositionActual::GetInstance(objManager); + PositionState *obj = PositionState::GetInstance(objManager); Q_ASSERT(obj != NULL); return obj; @@ -100,19 +100,19 @@ void MagicWaypointGadgetWidget::scaleChanged(int scale) { Q_UNUSED(scale); pathDesiredChanged(getPathDesired()); - positionActualChanged(getPositionActual()); + positionStateChanged(getPositionState()); } /** - * Emit a position changed signal when @ref PositionActual object is changed + * Emit a position changed signal when @ref PositionState object is changed */ -void MagicWaypointGadgetWidget::positionActualChanged(UAVObject *) +void MagicWaypointGadgetWidget::positionStateChanged(UAVObject *) { - PositionActual::DataFields positionActual = getPositionActual()->getData(); + PositionState::DataFields positionState = getPositionState()->getData(); double scale = m_magicwaypoint->horizontalSliderScale->value(); - emit positionActualObjectChanged(positionActual.North / scale, - positionActual.East / scale); + emit positionStateObjectChanged(positionState.North / scale, + positionState.East / scale); } /** diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h index 49e8f1251..17994072d 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h @@ -30,7 +30,7 @@ #include #include "pathdesired.h" -#include "positionactual.h" +#include "positionstate.h" class Ui_MagicWaypoint; @@ -42,18 +42,18 @@ public: ~MagicWaypointGadgetWidget(); signals: - void positionActualObjectChanged(double north, double east); + void positionStateObjectChanged(double north, double east); void positionDesiredObjectChanged(double north, double east); protected slots: void scaleChanged(int scale); - void positionActualChanged(UAVObject *); + void positionStateChanged(UAVObject *); void pathDesiredChanged(UAVObject *); void positionSelected(double north, double east); private: PathDesired *getPathDesired(); - PositionActual *getPositionActual(); + PositionState *getPositionState(); Ui_MagicWaypoint *m_magicwaypoint; }; diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp index 11047c7b5..0c7a16a2c 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp @@ -57,7 +57,7 @@ ModelViewGadgetWidget::ModelViewGadgetWidget(QWidget *parent) // Get required UAVObjects ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - attActual = AttitudeActual::GetInstance(objManager); + attState = AttitudeState::GetInstance(objManager); connect(&m_MotionTimer, SIGNAL(timeout()), this, SLOT(updateAttitude())); } @@ -311,7 +311,7 @@ void ModelViewGadgetWidget::keyPressEvent(QKeyEvent *e) // switch between camera ////////////////////////////////////////////////////////////////////// void ModelViewGadgetWidget::updateAttitude() { - AttitudeActual::DataFields data = attActual->getData(); // get attitude data + AttitudeState::DataFields data = attState->getData(); // get attitude data GLC_StructOccurence *rootObject = m_World.rootOccurence(); // get the full 3D model double x = data.q3; double y = data.q2; diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h index 353aeba7a..930b462e3 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h @@ -39,7 +39,7 @@ #include "glc_exception.h" #include "uavobjectmanager.h" -#include "attitudeactual.h" +#include "attitudestate.h" class ModelViewGadgetWidget : public QGLWidget { @@ -89,7 +89,7 @@ private: QString bgFilename; bool vboEnable; - AttitudeActual *attActual; + AttitudeState *attState; }; #endif /* MODELVIEWGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index c7d193f24..a5a60b187 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -48,14 +48,14 @@ #include "uavtalk/telemetrymanager.h" #include "uavobject.h" -#include "positionactual.h" +#include "positionstate.h" #include "homelocation.h" #include "gpsposition.h" -#include "gyros.h" -#include "attitudeactual.h" -#include "positionactual.h" -#include "velocityactual.h" -#include "airspeedactual.h" +#include "gyrostate.h" +#include "attitudestate.h" +#include "positionstate.h" +#include "velocitystate.h" +#include "airspeedstate.h" #define allow_manual_home_location_move @@ -593,36 +593,36 @@ void OPMapGadgetWidget::updatePosition() // ********************** // get the current position and heading estimates - AttitudeActual *attitudeActualObj = AttitudeActual::GetInstance(obm); - PositionActual *positionActualObj = PositionActual::GetInstance(obm); - VelocityActual *velocityActualObj = VelocityActual::GetInstance(obm); - AirspeedActual *airspeedActualObj = AirspeedActual::GetInstance(obm); + AttitudeState *attitudeStateObj = AttitudeState::GetInstance(obm); + PositionState *positionStateObj = PositionState::GetInstance(obm); + VelocityState *velocityStateObj = VelocityState::GetInstance(obm); + AirspeedState *airspeedStateObj = AirspeedState::GetInstance(obm); - Gyros *gyrosObj = Gyros::GetInstance(obm); + GyroState *gyroStateObj = GyroState::GetInstance(obm); - Q_ASSERT(attitudeActualObj); - Q_ASSERT(positionActualObj); - Q_ASSERT(velocityActualObj); - Q_ASSERT(airspeedActualObj); - Q_ASSERT(gyrosObj); + Q_ASSERT(attitudeStateObj); + Q_ASSERT(positionStateObj); + Q_ASSERT(velocityStateObj); + Q_ASSERT(airspeedStateObj); + Q_ASSERT(gyroStateObj); - AttitudeActual::DataFields attitudeActualData = attitudeActualObj->getData(); - PositionActual::DataFields positionActualData = positionActualObj->getData(); - VelocityActual::DataFields velocityActualData = velocityActualObj->getData(); - AirspeedActual::DataFields airspeedActualData = airspeedActualObj->getData(); + AttitudeState::DataFields attitudeStateData = attitudeStateObj->getData(); + PositionState::DataFields positionStateData = positionStateObj->getData(); + VelocityState::DataFields velocityStateData = velocityStateObj->getData(); + AirspeedState::DataFields airspeedStateData = airspeedStateObj->getData(); - Gyros::DataFields gyrosData = gyrosObj->getData(); + GyroState::DataFields gyroStateData = gyroStateObj->getData(); - double NED[3] = { positionActualData.North, positionActualData.East, positionActualData.Down }; - double vNED[3] = { velocityActualData.North, velocityActualData.East, velocityActualData.Down }; + double NED[3] = { positionStateData.North, positionStateData.East, positionStateData.Down }; + double vNED[3] = { velocityStateData.North, velocityStateData.East, velocityStateData.Down }; // Set the position and heading estimates in the painter module m_map->UAV->SetNED(NED); - m_map->UAV->SetCAS(airspeedActualData.CalibratedAirspeed); + m_map->UAV->SetCAS(airspeedStateData.CalibratedAirspeed); m_map->UAV->SetGroundspeed(vNED, m_maxUpdateRate); // Convert angular velocities into a rotationg rate around the world-frame yaw axis. This is found by simply taking the dot product of the angular Euler-rate matrix with the angular rates. - float psiRate_dps = 0 * gyrosData.z + sin(attitudeActualData.Roll * deg_to_rad) / cos(attitudeActualData.Pitch * deg_to_rad) * gyrosData.y + cos(attitudeActualData.Roll * deg_to_rad) / cos(attitudeActualData.Pitch * deg_to_rad) * gyrosData.z; + float psiRate_dps = 0 * gyroStateData.z + sin(attitudeStateData.Roll * deg_to_rad) / cos(attitudeStateData.Pitch * deg_to_rad) * gyroStateData.y + cos(attitudeStateData.Roll * deg_to_rad) / cos(attitudeStateData.Pitch * deg_to_rad) * gyroStateData.z; // Set the angular rate in the painter module m_map->UAV->SetYawRate(psiRate_dps); // Not correct, but I'm being lazy right now. @@ -2178,13 +2178,13 @@ bool OPMapGadgetWidget::getUAVPosition(double &latitude, double &longitude, doub homeLLA[1] = homeLocationData.Longitude / 1e7; homeLLA[2] = homeLocationData.Altitude; - PositionActual *positionActual = PositionActual::GetInstance(obm); - Q_ASSERT(positionActual != NULL); - PositionActual::DataFields positionActualData = positionActual->getData(); + PositionState *positionState = PositionState::GetInstance(obm); + Q_ASSERT(positionState != NULL); + PositionState::DataFields positionStateData = positionState->getData(); - NED[0] = positionActualData.North; - NED[1] = positionActualData.East; - NED[2] = positionActualData.Down; + NED[0] = positionStateData.North; + NED[1] = positionStateData.East; + NED[2] = positionStateData.Down; Utils::CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA); @@ -2220,7 +2220,7 @@ double OPMapGadgetWidget::getUAV_Yaw() return 0; } - UAVObject *obj = dynamic_cast(obm->getObject(QString("AttitudeActual"))); + UAVObject *obj = dynamic_cast(obm->getObject(QString("AttitudeState"))); double yaw = obj->getField(QString("Yaw"))->getDouble(); if (yaw != yaw) { diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp index c96ab3f77..006579148 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp @@ -93,9 +93,9 @@ using namespace osgEarth::Annotation; #include "utils/homelocationutil.h" #include "utils/worldmagmodel.h" #include "utils/coordinateconversions.h" -#include "attitudeactual.h" +#include "attitudestate.h" #include "homelocation.h" -#include "positionactual.h" +#include "positionstate.h" using namespace Utils; diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp index 2b2d11b73..cd08ce5dd 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp @@ -95,10 +95,10 @@ using namespace osgEarth::Annotation; #include "utils/homelocationutil.h" #include "utils/worldmagmodel.h" #include "utils/coordinateconversions.h" -#include "attitudeactual.h" +#include "attitudestate.h" #include "gpsposition.h" #include "homelocation.h" -#include "positionactual.h" +#include "positionstate.h" #include "systemsettings.h" using namespace Utils; @@ -256,12 +256,12 @@ void OsgViewerWidget::paintEvent(QPaintEvent *event) ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objMngr = pm->getObject(); - PositionActual *positionActualObj = PositionActual::GetInstance(objMngr); - PositionActual::DataFields positionActual = positionActualObj->getData(); - double NED[3] = { positionActual.North, positionActual.East, positionActual.Down }; + PositionState *positionStateObj = PositionState::GetInstance(objMngr); + PositionState::DataFields positionState = positionStateObj->getData(); + double NED[3] = { positionState.North, positionState.East, positionState.Down }; - bool positionActualUpdate = true; - if (positionActualUpdate) { + bool positionStateUpdate = true; + if (positionStateUpdate) { HomeLocation *homeLocationObj = HomeLocation::GetInstance(objMngr); HomeLocation::DataFields homeLocation = homeLocationObj->getData(); double homeLLA[3] = { homeLocation.Latitude / 10.0e6, homeLocation.Longitude / 10.0e6, homeLocation.Altitude }; @@ -276,9 +276,9 @@ void OsgViewerWidget::paintEvent(QPaintEvent *event) } // Set the attitude (reverse the attitude) - AttitudeActual *attitudeActualObj = AttitudeActual::GetInstance(objMngr); - AttitudeActual::DataFields attitudeActual = attitudeActualObj->getData(); - osg::Quat quat(attitudeActual.q2, attitudeActual.q3, attitudeActual.q4, attitudeActual.q1); + AttitudeState *attitudeStateObj = AttitudeState::GetInstance(objMngr); + AttitudeState::DataFields attitudeState = attitudeStateObj->getData(); + osg::Quat quat(attitudeState.q2, attitudeState.q3, attitudeState.q4, attitudeState.q1); // Have to rotate the axes from OP NED frame to OSG frame (X east, Y north, Z down) double angle; diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp index bbd034193..bbb1f4c48 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp @@ -86,7 +86,7 @@ void PFDGadgetWidget::setToolTipPrivate() UAVObject::Metadata mdata = attitudeObj->getMetadata(); if (mdata.flightTelemetryUpdatePeriod != updateRate) { - this->setToolTip("Current refresh rate:" + QString::number(mdata.flightTelemetryUpdatePeriod) + " miliseconds" + "\nIf you want to change it please edit the AttitudeActual metadata on the object browser."); + this->setToolTip("Current refresh rate:" + QString::number(mdata.flightTelemetryUpdatePeriod) + " miliseconds" + "\nIf you want to change it please edit the AttitudeState metadata on the object browser."); } } @@ -106,7 +106,7 @@ void PFDGadgetWidget::enableOpenGL(bool flag) \brief Connects the widget to the relevant UAVObjects Should only be called after the PFD artwork is loaded. - We want: AttitudeActual, FlightBattery, Location. + We want: AttitudeState, FlightBattery, Location. */ void PFDGadgetWidget::connectNeedles() @@ -134,39 +134,39 @@ void PFDGadgetWidget::connectNeedles() ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - airspeedObj = dynamic_cast(objManager->getObject("AirspeedActual")); + airspeedObj = dynamic_cast(objManager->getObject("AirspeedState")); if (airspeedObj != NULL) { connect(airspeedObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateAirspeed(UAVObject *))); } else { - qDebug() << "Error: Object is unknown (AirspeedActual)."; + qDebug() << "Error: Object is unknown (AirspeedState)."; } - groundspeedObj = dynamic_cast(objManager->getObject("VelocityActual")); + groundspeedObj = dynamic_cast(objManager->getObject("VelocityState")); if (groundspeedObj != NULL) { connect(groundspeedObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGroundspeed(UAVObject *))); } else { - qDebug() << "Error: Object is unknown (VelocityActual)."; + qDebug() << "Error: Object is unknown (VelocityState)."; } - altitudeObj = dynamic_cast(objManager->getObject("PositionActual")); + altitudeObj = dynamic_cast(objManager->getObject("PositionState")); if (altitudeObj != NULL) { connect(altitudeObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateAltitude(UAVObject *))); } else { - qDebug() << "Error: Object is unknown (PositionActual)."; + qDebug() << "Error: Object is unknown (PositionState)."; } - attitudeObj = dynamic_cast(objManager->getObject("AttitudeActual")); + attitudeObj = dynamic_cast(objManager->getObject("AttitudeState")); if (attitudeObj != NULL) { connect(attitudeObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateAttitude(UAVObject *))); } else { - qDebug() << "Error: Object is unknown (AttitudeActual)."; + qDebug() << "Error: Object is unknown (AttitudeState)."; } - headingObj = dynamic_cast(objManager->getObject("PositionActual")); + headingObj = dynamic_cast(objManager->getObject("PositionState")); if (headingObj != NULL) { connect(headingObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateHeading(UAVObject *))); } else { - qDebug() << "Error: Object is unknown (PositionActual)."; + qDebug() << "Error: Object is unknown (PositionState)."; } if (gcsGPSStats) { @@ -307,7 +307,7 @@ void PFDGadgetWidget::updateAttitude(UAVObject *object1) /*! \brief Updates the compass reading and speed dial. - This also updates speed & altitude according to PositionActual data. + This also updates speed & altitude according to PositionState data. Note: the speed dial shows the ground speed at the moment, because there is no airspeed by default. Should become configurable in a future @@ -319,7 +319,7 @@ void PFDGadgetWidget::updateHeading(UAVObject *object) } /*! - \brief Called by updates to @PositionActual to compute groundspeed from velocity + \brief Called by updates to @PositionState to compute groundspeed from velocity */ void PFDGadgetWidget::updateGroundspeed(UAVObject *object) { @@ -340,7 +340,7 @@ void PFDGadgetWidget::updateGroundspeed(UAVObject *object) /*! - \brief Called by updates to @AirspeedActual + \brief Called by updates to @AirspeedState */ void PFDGadgetWidget::updateAirspeed(UAVObject *object) { @@ -358,7 +358,7 @@ void PFDGadgetWidget::updateAirspeed(UAVObject *object) } /*! - \brief Called by the @ref PositionActual updates to show altitude + \brief Called by the @ref PositionState updates to show altitude */ void PFDGadgetWidget::updateAltitude(UAVObject *object) { diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp index 60fe0539c..aca816bea 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp @@ -48,10 +48,10 @@ PfdQmlGadgetWidget::PfdQmlGadgetWidget(QWidget *parent) : // setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); QStringList objectsToExport; - objectsToExport << "VelocityActual" << - "PositionActual" << - "AttitudeActual" << - "Accels" << + objectsToExport << "VelocityState" << + "PositionState" << + "AttitudeState" << + "AccelState" << "VelocityDesired" << "PositionDesired" << "AttitudeHoldDesired" << @@ -138,7 +138,7 @@ void PfdQmlGadgetWidget::setOpenGLEnabled(bool arg) } } -// Switch between PositionActual UAVObject position +// Switch between PositionState UAVObject position // and pre-defined latitude/longitude/altitude properties void PfdQmlGadgetWidget::setActualPositionUsed(bool arg) { diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp index 4bc172fee..337faf7c0 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp @@ -49,9 +49,9 @@ QmlViewGadgetWidget::QmlViewGadgetWidget(QWidget *parent) : setResizeMode(SizeRootObjectToView); QStringList objectsToExport; - objectsToExport << "VelocityActual" << - "PositionActual" << - "AttitudeActual" << + objectsToExport << "VelocityState" << + "PositionState" << + "AttitudeState" << "GPSPosition" << "GCSTelemetryStats" << "FlightBatteryState"; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp index 1b3f64804..f36445ea1 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp @@ -29,8 +29,8 @@ #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" #include "attitudesettings.h" -#include "accels.h" -#include "gyros.h" +#include "accelstate.h" +#include "gyrostate.h" #include "qdebug.h" @@ -73,12 +73,12 @@ void BiasCalibrationUtil::gyroMeasurementsUpdated(UAVObject *obj) UAVObjectManager *uavObjectManager = pm->getObject(); Q_ASSERT(uavObjectManager); - Gyros *gyros = Gyros::GetInstance(uavObjectManager); - Gyros::DataFields gyrosData = gyros->getData(); + GyroState *gyroState = GyroState::GetInstance(uavObjectManager); + GyroState::DataFields gyroStateData = gyroState->getData(); - m_gyroX += gyrosData.x; - m_gyroY += gyrosData.y; - m_gyroZ += gyrosData.z; + m_gyroX += gyroStateData.x; + m_gyroY += gyroStateData.y; + m_gyroZ += gyroStateData.z; m_receivedGyroUpdates++; emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount); @@ -97,12 +97,12 @@ void BiasCalibrationUtil::accelMeasurementsUpdated(UAVObject *obj) UAVObjectManager *uavObjectManager = pm->getObject(); Q_ASSERT(uavObjectManager); - Accels *accels = Accels::GetInstance(uavObjectManager); - Accels::DataFields accelsData = accels->getData(); + AccelState *accelState = AccelState::GetInstance(uavObjectManager); + AccelState::DataFields AccelStateData = accelState->getData(); - m_accelerometerX += accelsData.x; - m_accelerometerY += accelsData.y; - m_accelerometerZ += accelsData.z; + m_accelerometerX += AccelStateData.x; + m_accelerometerY += AccelStateData.y; + m_accelerometerZ += AccelStateData.z; m_receivedAccelUpdates++; emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount); @@ -143,7 +143,7 @@ void BiasCalibrationUtil::startMeasurement() AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData); // Set up to receive updates for accels - UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager); + UAVDataObject *uavObject = AccelState::GetInstance(uavObjectManager); connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(accelMeasurementsUpdated(UAVObject *))); // Set update period for accels @@ -154,7 +154,7 @@ void BiasCalibrationUtil::startMeasurement() uavObject->setMetadata(newMetaData); // Set up to receive updates from gyros - uavObject = Gyros::GetInstance(uavObjectManager); + uavObject = GyroState::GetInstance(uavObjectManager); connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroMeasurementsUpdated(UAVObject *))); // Set update period for gyros @@ -180,12 +180,12 @@ void BiasCalibrationUtil::stopMeasurement() Q_ASSERT(uavObjectManager); // Stop listening for updates from accels - UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager); + UAVDataObject *uavObject = AccelState::GetInstance(uavObjectManager); disconnect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(accelMeasurementsUpdated(UAVObject *))); uavObject->setMetadata(m_previousAccelMetaData); // Stop listening for updates from gyros - uavObject = Gyros::GetInstance(uavObjectManager); + uavObject = GyroState::GetInstance(uavObjectManager); disconnect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroMeasurementsUpdated(UAVObject *))); uavObject->setMetadata(m_previousGyroMetaData); diff --git a/ground/openpilotgcs/src/plugins/uavobjects/OPPlots.m b/ground/openpilotgcs/src/plugins/uavobjects/OPPlots.m index b350458fd..3dabe5a39 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/OPPlots.m +++ b/ground/openpilotgcs/src/plugins/uavobjects/OPPlots.m @@ -5,10 +5,10 @@ function OPPlots() %load('specificfilename') - TimeVA = [VelocityActual.timestamp]/1000; - VA = [[VelocityActual.North] - [VelocityActual.East] - [VelocityActual.Down]]/100; + TimeVA = [VelocityState.timestamp]/1000; + VA = [[VelocityState.North] + [VelocityState.East] + [VelocityState.Down]]/100; TimeGPSPos = [GPSPosition.timestamp]/1000; Vgps=[[GPSPosition.Groundspeed].*cos([GPSPosition.Heading]*pi/180) @@ -16,8 +16,8 @@ function OPPlots() figure(1); plot(TimeVA,VA(1,:),TimeVA,VA(2,:),TimeGPSPos,Vgps(1,:),TimeGPSPos,Vgps(2,:)); - s1='Velocity Actual North'; - s2='Velocity Actual East'; + s1='Velocity State North'; + s2='Velocity State East'; s3='GPS Velocity North'; s4='GPS Velocity East'; legend(s1,s2,s3,s4); @@ -25,10 +25,10 @@ function OPPlots() ylabel('Velocity (m/s)'); - TimePA = [PositionActual.timestamp]/1000; - PA = [[PositionActual.North] - [PositionActual.East] - [PositionActual.Down]]/100; + TimePA = [PositionState.timestamp]/1000; + PA = [[PositionState.North] + [PositionState.East] + [PositionState.Down]]/100; TimeGPSPos = [GPSPosition.timestamp]/1000; LLA=[[GPSPosition.Latitude]*1e-7; @@ -40,8 +40,8 @@ function OPPlots() figure(2); plot(TimePA,PA(1,:),TimePA,PA(2,:),TimeGPSPos,GPSPos(1,:),TimeGPSPos,GPSPos(2,:)); - s1='Position Actual North'; - s2='Position Actual East'; + s1='Position State North'; + s2='Position State East'; s3='GPS Position North'; s4='GPS Position East'; legend(s1,s2,s3,s4); @@ -50,7 +50,7 @@ function OPPlots() figure(3); plot3(PA(2,:),PA(1,:),PA(3,:),GPSPos(2,:),GPSPos(1,:),GPSPos(3,:)); - s1='Pos Actual'; + s1='Pos State'; s2='GPS Pos'; legend(s1,s2); xlabel('East (m)'); diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 43a772af5..745b0b935 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -24,11 +24,11 @@ OTHER_FILES += UAVObjects.pluginspec # Add in all of the synthetic/generated uavobject files HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ - $$UAVOBJECT_SYNTHETICS/baroaltitude.h \ + $$UAVOBJECT_SYNTHETICS/barosensor.h \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.h \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.h \ - $$UAVOBJECT_SYNTHETICS/airspeedactual.h \ - $$UAVOBJECT_SYNTHETICS/attitudeactual.h \ + $$UAVOBJECT_SYNTHETICS/airspeedstate.h \ + $$UAVOBJECT_SYNTHETICS/attitudestate.h \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.h \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ @@ -38,11 +38,12 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/revocalibration.h \ $$UAVOBJECT_SYNTHETICS/revosettings.h \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \ - $$UAVOBJECT_SYNTHETICS/gyros.h \ - $$UAVOBJECT_SYNTHETICS/gyrosbias.h \ - $$UAVOBJECT_SYNTHETICS/accels.h \ - $$UAVOBJECT_SYNTHETICS/magnetometer.h \ - $$UAVOBJECT_SYNTHETICS/magbias.h \ + $$UAVOBJECT_SYNTHETICS/gyrostate.h \ + $$UAVOBJECT_SYNTHETICS/gyrosensor.h \ + $$UAVOBJECT_SYNTHETICS/accelsensor.h \ + $$UAVOBJECT_SYNTHETICS/accelstate.h \ + $$UAVOBJECT_SYNTHETICS/magnetosensor.h \ + $$UAVOBJECT_SYNTHETICS/magnetostate.h \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.h \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.h \ $$UAVOBJECT_SYNTHETICS/systemstats.h \ @@ -65,13 +66,13 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/pathdesired.h \ $$UAVOBJECT_SYNTHETICS/pathstatus.h \ $$UAVOBJECT_SYNTHETICS/gpsvelocity.h \ - $$UAVOBJECT_SYNTHETICS/positionactual.h \ + $$UAVOBJECT_SYNTHETICS/positionstate.h \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.h \ $$UAVOBJECT_SYNTHETICS/homelocation.h \ $$UAVOBJECT_SYNTHETICS/mixersettings.h \ $$UAVOBJECT_SYNTHETICS/mixerstatus.h \ $$UAVOBJECT_SYNTHETICS/velocitydesired.h \ - $$UAVOBJECT_SYNTHETICS/velocityactual.h \ + $$UAVOBJECT_SYNTHETICS/velocitystate.h \ $$UAVOBJECT_SYNTHETICS/groundtruth.h \ $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.h \ $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowerstatus.h \ @@ -107,11 +108,11 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/mpu6000settings.h SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ - $$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \ + $$UAVOBJECT_SYNTHETICS/barosensor.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \ - $$UAVOBJECT_SYNTHETICS/airspeedactual.cpp \ - $$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \ + $$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \ + $$UAVOBJECT_SYNTHETICS/attitudestate.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ @@ -121,11 +122,12 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/revocalibration.cpp \ $$UAVOBJECT_SYNTHETICS/revosettings.cpp \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \ - $$UAVOBJECT_SYNTHETICS/accels.cpp \ - $$UAVOBJECT_SYNTHETICS/gyros.cpp \ - $$UAVOBJECT_SYNTHETICS/gyrosbias.cpp \ - $$UAVOBJECT_SYNTHETICS/magnetometer.cpp \ - $$UAVOBJECT_SYNTHETICS/magbias.cpp \ + $$UAVOBJECT_SYNTHETICS/accelsensor.cpp \ + $$UAVOBJECT_SYNTHETICS/accelstate.cpp \ + $$UAVOBJECT_SYNTHETICS/gyrostate.cpp \ + $$UAVOBJECT_SYNTHETICS/gyrosensor.cpp \ + $$UAVOBJECT_SYNTHETICS/magnetosensor.cpp \ + $$UAVOBJECT_SYNTHETICS/magnetostate.cpp \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.cpp \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.cpp \ $$UAVOBJECT_SYNTHETICS/systemstats.cpp \ @@ -148,13 +150,13 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/pathdesired.cpp \ $$UAVOBJECT_SYNTHETICS/pathstatus.cpp \ $$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \ - $$UAVOBJECT_SYNTHETICS/positionactual.cpp \ + $$UAVOBJECT_SYNTHETICS/positionstate.cpp \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \ $$UAVOBJECT_SYNTHETICS/homelocation.cpp \ $$UAVOBJECT_SYNTHETICS/mixersettings.cpp \ $$UAVOBJECT_SYNTHETICS/mixerstatus.cpp \ $$UAVOBJECT_SYNTHETICS/velocitydesired.cpp \ - $$UAVOBJECT_SYNTHETICS/velocityactual.cpp \ + $$UAVOBJECT_SYNTHETICS/velocitystate.cpp \ $$UAVOBJECT_SYNTHETICS/groundtruth.cpp \ $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.cpp \ $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowerstatus.cpp \ diff --git a/shared/uavobjectdefinition/accels.xml b/shared/uavobjectdefinition/accelsensor.xml similarity index 77% rename from shared/uavobjectdefinition/accels.xml rename to shared/uavobjectdefinition/accelsensor.xml index 3a5e34b84..f4c1c1038 100644 --- a/shared/uavobjectdefinition/accels.xml +++ b/shared/uavobjectdefinition/accelsensor.xml @@ -1,5 +1,5 @@ - + The accel data. @@ -7,7 +7,7 @@ - + diff --git a/shared/uavobjectdefinition/accelstate.xml b/shared/uavobjectdefinition/accelstate.xml new file mode 100644 index 000000000..c2236e875 --- /dev/null +++ b/shared/uavobjectdefinition/accelstate.xml @@ -0,0 +1,12 @@ + + + The filtered acceleration data. + + + + + + + + + diff --git a/shared/uavobjectdefinition/airspeedactual.xml b/shared/uavobjectdefinition/airspeedstate.xml similarity index 88% rename from shared/uavobjectdefinition/airspeedactual.xml rename to shared/uavobjectdefinition/airspeedstate.xml index d5b0f90ae..f1b1a78f7 100644 --- a/shared/uavobjectdefinition/airspeedactual.xml +++ b/shared/uavobjectdefinition/airspeedstate.xml @@ -1,5 +1,5 @@ - + UAVO for true airspeed, calibrated airspeed, angle of attack, and angle of slip diff --git a/shared/uavobjectdefinition/attitudeactual.xml b/shared/uavobjectdefinition/attitudestate.xml similarity index 91% rename from shared/uavobjectdefinition/attitudeactual.xml rename to shared/uavobjectdefinition/attitudestate.xml index b9f143b35..ae89f073a 100644 --- a/shared/uavobjectdefinition/attitudeactual.xml +++ b/shared/uavobjectdefinition/attitudestate.xml @@ -1,5 +1,5 @@ - + The updated Attitude estimation from @ref AHRSCommsModule. diff --git a/shared/uavobjectdefinition/baroaltitude.xml b/shared/uavobjectdefinition/barosensor.xml similarity index 89% rename from shared/uavobjectdefinition/baroaltitude.xml rename to shared/uavobjectdefinition/barosensor.xml index 8335b56dd..32f49c17e 100644 --- a/shared/uavobjectdefinition/baroaltitude.xml +++ b/shared/uavobjectdefinition/barosensor.xml @@ -1,5 +1,5 @@ - + The raw data from the barometric sensor with pressure, temperature and altitude estimate. diff --git a/shared/uavobjectdefinition/gyros.xml b/shared/uavobjectdefinition/gyrosensor.xml similarity index 89% rename from shared/uavobjectdefinition/gyros.xml rename to shared/uavobjectdefinition/gyrosensor.xml index 8240d2a68..36f2d3f0b 100644 --- a/shared/uavobjectdefinition/gyros.xml +++ b/shared/uavobjectdefinition/gyrosensor.xml @@ -1,5 +1,5 @@ - + The gyro data. diff --git a/shared/uavobjectdefinition/gyrosbias.xml b/shared/uavobjectdefinition/gyrostate.xml similarity index 77% rename from shared/uavobjectdefinition/gyrosbias.xml rename to shared/uavobjectdefinition/gyrostate.xml index b8543e641..b8111b33e 100644 --- a/shared/uavobjectdefinition/gyrosbias.xml +++ b/shared/uavobjectdefinition/gyrostate.xml @@ -1,6 +1,6 @@ - - The gyro data. + + The filtered rotation sensor data. diff --git a/shared/uavobjectdefinition/magbias.xml b/shared/uavobjectdefinition/magbias.xml deleted file mode 100644 index 63319466f..000000000 --- a/shared/uavobjectdefinition/magbias.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - The gyro data. - - - - - - - - - diff --git a/shared/uavobjectdefinition/magnetosensor.xml b/shared/uavobjectdefinition/magnetosensor.xml new file mode 100644 index 000000000..a11c6c420 --- /dev/null +++ b/shared/uavobjectdefinition/magnetosensor.xml @@ -0,0 +1,12 @@ + + + The mag data. + + + + + + + + + diff --git a/shared/uavobjectdefinition/magnetometer.xml b/shared/uavobjectdefinition/magnetostate.xml similarity index 88% rename from shared/uavobjectdefinition/magnetometer.xml rename to shared/uavobjectdefinition/magnetostate.xml index 006c40298..546b52d69 100644 --- a/shared/uavobjectdefinition/magnetometer.xml +++ b/shared/uavobjectdefinition/magnetostate.xml @@ -1,5 +1,5 @@ - + The mag data. diff --git a/shared/uavobjectdefinition/positionactual.xml b/shared/uavobjectdefinition/positionstate.xml similarity index 88% rename from shared/uavobjectdefinition/positionactual.xml rename to shared/uavobjectdefinition/positionstate.xml index 0d5eaf006..afa3c201b 100644 --- a/shared/uavobjectdefinition/positionactual.xml +++ b/shared/uavobjectdefinition/positionstate.xml @@ -1,5 +1,5 @@ - + Contains the current position relative to @ref HomeLocation diff --git a/shared/uavobjectdefinition/velocityactual.xml b/shared/uavobjectdefinition/velocitystate.xml similarity index 70% rename from shared/uavobjectdefinition/velocityactual.xml rename to shared/uavobjectdefinition/velocitystate.xml index a4173b033..ad50b4725 100644 --- a/shared/uavobjectdefinition/velocityactual.xml +++ b/shared/uavobjectdefinition/velocitystate.xml @@ -1,6 +1,6 @@ - - Updated by @ref AHRSCommsModule and used within @ref GuidanceModule for velocity control + + Updated by filters, velocity relative to @ref HomeLocation From 9c3a8369cd1cef65e2a24d0dd9d02cef5aec0567 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 18 May 2013 21:55:47 +0200 Subject: [PATCH 03/61] OP-946: State And Sensor Refaktoring: Moved Filtering for Magnetometers, Gyros and Accels into Attitude --- flight/modules/Attitude/revolution/attitude.c | 230 ++++++++++++++---- flight/modules/Sensors/sensors.c | 104 -------- flight/modules/Sensors/simulated/sensors.c | 103 -------- shared/uavobjectdefinition/accelsensor.xml | 2 +- shared/uavobjectdefinition/magnetosensor.xml | 2 +- 5 files changed, 183 insertions(+), 258 deletions(-) diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index 6036a1691..b518a00d8 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -125,6 +125,8 @@ static void settingsUpdatedCb(UAVObjEvent *objEv); static int32_t getNED(GPSPositionData *gpsPosition, float *NED); +static void magOffsetEstimation(MagnetoSensorData *mag); + // check for invalid values static inline bool invalid(float data) { @@ -161,10 +163,19 @@ static inline bool invalid_var(float data) */ int32_t AttitudeInitialize(void) { - AttitudeStateInitialize(); - AirspeedStateInitialize(); + GyroSensorInitialize(); + GyroStateInitialize(); + AccelSensorInitialize(); + AccelStateInitialize(); + MagnetoSensorInitialize(); + MagnetoStateInitialize(); AirspeedSensorInitialize(); + AirspeedStateInitialize(); + BaroSensorInitialize(); + GPSPositionInitialize(); + GPSVelocityInitialize(); AttitudeSettingsInitialize(); + AttitudeStateInitialize(); PositionStateInitialize(); VelocityStateInitialize(); RevoSettingsInitialize(); @@ -286,10 +297,12 @@ static int32_t updateAttitudeComplementary(bool first_run) { UAVObjEvent ev; GyroSensorData gyroSensorData; + GyroStateData gyroStateData; AccelSensorData accelSensorData; static int32_t timeval; float dT; static uint8_t init = 0; + static float gyro_bias[3] = { 0, 0, 0 }; // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE || @@ -304,6 +317,13 @@ static int32_t updateAttitudeComplementary(bool first_run) AccelSensorGet(&accelSensorData); +// TODO: put in separate filter + AccelStateData accelState; + accelState.x = accelSensorData.x; + accelState.y = accelSensorData.y; + accelState.z = accelSensorData.z; + AccelStateSet(&accelState); + // During initialization and if (first_run) { #if defined(PIOS_INCLUDE_HMC5883) @@ -374,6 +394,9 @@ static int32_t updateAttitudeComplementary(bool first_run) } GyroSensorGet(&gyroSensorData); + gyroStateData.x = gyroSensorData.x; + gyroStateData.y = gyroSensorData.y; + gyroStateData.z = gyroSensorData.z; // Compute the dT using the cpu clock dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; @@ -412,6 +435,16 @@ static int32_t updateAttitudeComplementary(bool first_run) Quaternion2R(q, Rbe); MagnetoSensorGet(&mag); +// TODO: separate filter! + if (revoCalibration.MagBiasNullingRate > 0) { + magOffsetEstimation(&mag); + } + MagnetoStateData mags; + mags.x = mag.x; + mags.y = mag.y; + mags.z = mag.z; + MagnetoStateSet(&mags); + // If the mag is producing bad data don't use it (normally bad calibration) if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) { rot_mult(Rbe, homeLocation.Be, brot); @@ -438,35 +471,30 @@ static int32_t updateAttitudeComplementary(bool first_run) } // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s -// TODO -/* - GyrosBiasData gyrosBias; - GyrosBiasGet(&gyrosBias); - gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi; - gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi; - gyrosBias.z -= mag_err[2] * magKi; - GyrosBiasSet(&gyrosBias); + gyro_bias[0] -= accel_err[0] * attitudeSettings.AccelKi; + gyro_bias[0] -= accel_err[1] * attitudeSettings.AccelKi; + gyro_bias[0] -= mag_err[2] * magKi; - if (revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { - // if the raw values are not adjusted, we need to adjust here. - gyroSensorData.x -= gyrosBias.x; - gyroSensorData.y -= gyrosBias.y; - gyroSensorData.z -= gyrosBias.z; - } - */ + // Correct rates based on integral coefficient + gyroStateData.x -= gyro_bias[0]; + gyroStateData.y -= gyro_bias[1]; + gyroStateData.z -= gyro_bias[2]; - // Correct rates based on error, integral component dealt with in updateSensors - gyroSensorData.x += accel_err[0] * attitudeSettings.AccelKp / dT; - gyroSensorData.y += accel_err[1] * attitudeSettings.AccelKp / dT; - gyroSensorData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; + // save gyroscope state + GyroStateSet(&gyroStateData); + + // Correct rates based on proportional coefficient + gyroStateData.x += accel_err[0] * attitudeSettings.AccelKp / dT; + gyroStateData.y += accel_err[1] * attitudeSettings.AccelKp / dT; + gyroStateData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; // Work out time derivative from INSAlgo writeup // Also accounts for the fact that gyros are in deg/s float qdot[4]; - qdot[0] = DEG2RAD(-q[1] * gyroSensorData.x - q[2] * gyroSensorData.y - q[3] * gyroSensorData.z) * dT / 2; - qdot[1] = DEG2RAD(q[0] * gyroSensorData.x - q[3] * gyroSensorData.y + q[2] * gyroSensorData.z) * dT / 2; - qdot[2] = DEG2RAD(q[3] * gyroSensorData.x + q[0] * gyroSensorData.y - q[1] * gyroSensorData.z) * dT / 2; - qdot[3] = DEG2RAD(-q[2] * gyroSensorData.x + q[1] * gyroSensorData.y + q[0] * gyroSensorData.z) * dT / 2; + qdot[0] = DEG2RAD(-q[1] * gyroStateData.x - q[2] * gyroStateData.y - q[3] * gyroStateData.z) * dT / 2; + qdot[1] = DEG2RAD(q[0] * gyroStateData.x - q[3] * gyroStateData.y + q[2] * gyroStateData.z) * dT / 2; + qdot[2] = DEG2RAD(q[3] * gyroStateData.x + q[0] * gyroStateData.y - q[1] * gyroStateData.z) * dT / 2; + qdot[3] = DEG2RAD(-q[2] * gyroStateData.x + q[1] * gyroStateData.y + q[0] * gyroStateData.z) * dT / 2; // Take a time step q[0] = q[0] + qdot[0]; @@ -580,7 +608,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) UAVObjEvent ev; GyroSensorData gyroSensorData; AccelSensorData accelSensorData; - MagnetoSensorData magData; + MagnetoStateData magData; AirspeedSensorData airspeedData; BaroSensorData baroData; GPSPositionData gpsData; @@ -660,12 +688,33 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) // Get most recent data GyroSensorGet(&gyroSensorData); AccelSensorGet(&accelSensorData); - MagnetoSensorGet(&magData); +// TODO: separate filter! + if (mag_updated) { + MagnetoSensorData mags; + MagnetoSensorGet(&mags); + if (revoCalibration.MagBiasNullingRate > 0) { + magOffsetEstimation(&mags); + } + magData.x = mags.x; + magData.y = mags.y; + magData.z = mags.z; + MagnetoStateSet(&magData); + } + MagnetoStateGet(&magData); + BaroSensorGet(&baroData); AirspeedSensorGet(&airspeedData); GPSPositionGet(&gpsData); GPSVelocityGet(&gpsVelData); +// TODO: put in separate filter + AccelStateData accelState; + accelState.x = accelSensorData.x; + accelState.y = accelSensorData.y; + accelState.z = accelSensorData.z; + AccelStateSet(&accelState); + + value_error = false; // safety checks if (invalid(gyroSensorData.x) || @@ -806,8 +855,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) pos[2] = -(baroData.Altitude + baroOffset); } - xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); - MagnetoSensorGet(&magData); + // xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); + // MagnetoSensorGet(&magData); AttitudeStateData attitudeState; AttitudeStateGet(&attitudeState); @@ -848,13 +897,6 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) // Because the sensor module remove the bias we need to add it // back in here so that the INS algorithm can track it correctly float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) }; - /* TODO - if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { - gyros[0] += DEG2RAD(gyrosBias.x); - gyros[1] += DEG2RAD(gyrosBias.y); - gyros[2] += DEG2RAD(gyrosBias.z); - } - */ INSStatePrediction(gyros, &accelSensorData.x, dT); AttitudeStateData attitude; @@ -882,13 +924,6 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) // Because the sensor module remove the bias we need to add it // back in here so that the INS algorithm can track it correctly float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) }; - /* TODO - if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { - gyros[0] += DEG2RAD(gyrosBias.x); - gyros[1] += DEG2RAD(gyrosBias.y); - gyros[2] += DEG2RAD(gyrosBias.z); - } - */ // Advance the state estimate INSStatePrediction(gyros, &accelSensorData.x, dT); @@ -1011,12 +1046,11 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) velocityState.Down = Nav.Vel[2]; VelocityStateSet(&velocityState); -/* TODO - gyrosBias.x = RAD2DEG(Nav.gyro_bias[0]); - gyrosBias.y = RAD2DEG(Nav.gyro_bias[1]); - gyrosBias.z = RAD2DEG(Nav.gyro_bias[2]); - GyrosBiasSet(&gyrosBias); - */ + GyroStateData gyroState; + gyroState.x = RAD2DEG(gyros[0] - Nav.gyro_bias[0]); + gyroState.y = RAD2DEG(gyros[1] - Nav.gyro_bias[1]); + gyroState.z = RAD2DEG(gyros[2] - Nav.gyro_bias[2]); + GyroStateSet(&gyroState); EKFStateVarianceData vardata; EKFStateVarianceGet(&vardata); @@ -1121,6 +1155,104 @@ static void settingsUpdatedCb(UAVObjEvent *ev) AttitudeSettingsGet(&attitudeSettings); } } + +/** + * Perform an update of the @ref MagBias based on + * Magnetometer Offset Cancellation: Theory and Implementation, + * revisited William Premerlani, October 14, 2011 + */ +static void magOffsetEstimation(MagnetoSensorData *mag) +{ + #if 0 + // Constants, to possibly go into a UAVO + static const float MIN_NORM_DIFFERENCE = 50; + + static float B2[3] = { 0, 0, 0 }; + + MagBiasData magBias; + MagBiasGet(&magBias); + + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; + + // First call + if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { + B2[0] = mag->x; + B2[1] = mag->y; + B2[2] = mag->z; + return; + } + + float B1[3] = { mag->x, mag->y, mag->z }; + float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2)); + if (norm_diff > MIN_NORM_DIFFERENCE) { + float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]); + float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]); + float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; + float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale }; + + magBias.x += b_error[0]; + magBias.y += b_error[1]; + magBias.z += b_error[2]; + + MagBiasSet(&magBias); + + // Store this value to compare against next update + B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; + } + #else // if 0 + static float magBias[3] = { 0 }; + + // Remove the current estimate of the bias + mag->x -= magBias[0]; + mag->y -= magBias[1]; + mag->z -= magBias[2]; + + AttitudeStateData attitude; + AttitudeStateGet(&attitude); + + const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]); + const float Rz = homeLocation.Be[2]; + + const float rate = revoCalibration.MagBiasNullingRate; + float Rot[3][3]; + float B_e[3]; + float xy[2]; + float delta[3]; + + // Get the rotation matrix + Quaternion2R(&attitude.q1, Rot); + + // Rotate the mag into the NED frame + B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z; + B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z; + B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z; + + float cy = cosf(DEG2RAD(attitude.Yaw)); + float sy = sinf(DEG2RAD(attitude.Yaw)); + + xy[0] = cy * B_e[0] + sy * B_e[1]; + xy[1] = -sy * B_e[0] + cy * B_e[1]; + + float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]); + + delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); + delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); + delta[2] = -rate * (Rz - B_e[2]); + + if (!isnan(delta[0]) && !isinf(delta[0]) && + !isnan(delta[1]) && !isinf(delta[1]) && + !isnan(delta[2]) && !isinf(delta[2])) { + magBias[0] += delta[0]; + magBias[1] += delta[1]; + magBias[2] += delta[2]; + } + #endif // if 0 +} + + /** * @} * @} diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index bfbe62f18..4984e2350 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -423,110 +423,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters) } } -/** - * Perform an update of the @ref MagBias based on - * Magnetometer Offset Cancellation: Theory and Implementation, - * revisited William Premerlani, October 14, 2011 - */ -// TODO -/* - static void magOffsetEstimation(MagnetoSensorData *mag) - { - #if 0 - // Constants, to possibly go into a UAVO - static const float MIN_NORM_DIFFERENCE = 50; - - static float B2[3] = { 0, 0, 0 }; - - MagBiasData magBias; - MagBiasGet(&magBias); - - // Remove the current estimate of the bias - mag->x -= magBias.x; - mag->y -= magBias.y; - mag->z -= magBias.z; - - // First call - if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { - B2[0] = mag->x; - B2[1] = mag->y; - B2[2] = mag->z; - return; - } - - float B1[3] = { mag->x, mag->y, mag->z }; - float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2)); - if (norm_diff > MIN_NORM_DIFFERENCE) { - float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]); - float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]); - float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; - float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale }; - - magBias.x += b_error[0]; - magBias.y += b_error[1]; - magBias.z += b_error[2]; - - MagBiasSet(&magBias); - - // Store this value to compare against next update - B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; - } - #else // if 0 - MagBiasData magBias; - MagBiasGet(&magBias); - - // Remove the current estimate of the bias - mag->x -= magBias.x; - mag->y -= magBias.y; - mag->z -= magBias.z; - - HomeLocationData homeLocation; - HomeLocationGet(&homeLocation); - - AttitudeStateData attitude; - AttitudeStateGet(&attitude); - - const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]); - const float Rz = homeLocation.Be[2]; - - const float rate = cal.MagBiasNullingRate; - float Rot[3][3]; - float B_e[3]; - float xy[2]; - float delta[3]; - - // Get the rotation matrix - Quaternion2R(&attitude.q1, R); - - // Rotate the mag into the NED frame - B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z; - B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z; - B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z; - - float cy = cosf(DEG2RAD(attitude.Yaw)); - float sy = sinf(DEG2RAD(attitude.Yaw)); - - xy[0] = cy * B_e[0] + sy * B_e[1]; - xy[1] = -sy * B_e[0] + cy * B_e[1]; - - float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]); - - delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); - delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); - delta[2] = -rate * (Rz - B_e[2]); - - if (!isnan(delta[0]) && !isinf(delta[0]) && - !isnan(delta[1]) && !isinf(delta[1]) && - !isnan(delta[2]) && !isinf(delta[2])) { - magBias.x += delta[0]; - magBias.y += delta[1]; - magBias.z += delta[2]; - MagBiasSet(&magBias); - } - #endif // if 0 - } - */ - /** * Locally cache some variables from the AtttitudeSettings object */ diff --git a/flight/modules/Sensors/simulated/sensors.c b/flight/modules/Sensors/simulated/sensors.c index 9fea56af1..fdd58abf0 100644 --- a/flight/modules/Sensors/simulated/sensors.c +++ b/flight/modules/Sensors/simulated/sensors.c @@ -854,109 +854,6 @@ static float rand_gauss(void) } -// TODO -#if 0 -/** - * Perform an update of the @ref MagBias based on - * MagnetoSensor Offset Cancellation: Theory and Implementation, - * revisited William Premerlani, October 14, 2011 - */ -static void magOffsetEstimation(MagnetoSensorData *mag) -{ -#if 0 - RevoCalibrationData cal; - RevoCalibrationGet(&cal); - - // Constants, to possibly go into a UAVO - static const float MIN_NORM_DIFFERENCE = 50; - - static float B2[3] = { 0, 0, 0 }; - - MagBiasData magBias; - MagBiasGet(&magBias); - - // Remove the current estimate of the bias - mag->x -= magBias.x; - mag->y -= magBias.y; - mag->z -= magBias.z; - - // First call - if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { - B2[0] = mag->x; - B2[1] = mag->y; - B2[2] = mag->z; - return; - } - - float B1[3] = { mag->x, mag->y, mag->z }; - float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2)); - if (norm_diff > MIN_NORM_DIFFERENCE) { - float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]); - float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]); - float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; - float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale }; - - magBias.x += b_error[0]; - magBias.y += b_error[1]; - magBias.z += b_error[2]; - - MagBiasSet(&magBias); - - // Store this value to compare against next update - B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; - } -#else /* if 0 */ - HomeLocationData homeLocation; - HomeLocationGet(&homeLocation); - - AttitudeStateData attitude; - AttitudeStateGet(&attitude); - - MagBiasData magBias; - MagBiasGet(&magBias); - - // Remove the current estimate of the bias - mag->x -= magBias.x; - mag->y -= magBias.y; - mag->z -= magBias.z; - - const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]); - const float Rz = homeLocation.Be[2]; - - const float rate = 0.01; - float R[3][3]; - float B_e[3]; - float xy[2]; - float delta[3]; - - // Get the rotation matrix - Quaternion2R(&attitude.q1, R); - - // Rotate the mag into the NED frame - B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z; - B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z; - B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z; - - float cy = cosf(attitude.Yaw * M_PI / 180.0f); - float sy = sinf(attitude.Yaw * M_PI / 180.0f); - - xy[0] = cy * B_e[0] + sy * B_e[1]; - xy[1] = -sy * B_e[0] + cy * B_e[1]; - - float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]); - - delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); - delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); - delta[2] = -rate * (Rz - B_e[2]); - - magBias.x += delta[0]; - magBias.y += delta[1]; - magBias.z += delta[2]; - MagBiasSet(&magBias); -#endif /* if 0 */ -} - -#endif /* if 0 */ /** * @} * @} diff --git a/shared/uavobjectdefinition/accelsensor.xml b/shared/uavobjectdefinition/accelsensor.xml index f4c1c1038..664533eb2 100644 --- a/shared/uavobjectdefinition/accelsensor.xml +++ b/shared/uavobjectdefinition/accelsensor.xml @@ -7,7 +7,7 @@ - + diff --git a/shared/uavobjectdefinition/magnetosensor.xml b/shared/uavobjectdefinition/magnetosensor.xml index a11c6c420..5974636f6 100644 --- a/shared/uavobjectdefinition/magnetosensor.xml +++ b/shared/uavobjectdefinition/magnetosensor.xml @@ -6,7 +6,7 @@ - + From 39cde8947cd61d79f18979651f41543f06501b4e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 18 May 2013 22:19:22 +0200 Subject: [PATCH 04/61] removed forgotten commented out code --- flight/modules/Sensors/sensors.c | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index 4984e2350..6fd7fe32f 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -371,14 +371,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters) gyroSensorData.z = gyros_out[2]; } -/* TODO if (bias_correct_gyro) { - // Apply bias correction to the gyros from the state estimator - GyrosBiasData gyrosBias; - GyrosBiasGet(&gyrosBias); - gyrosData.x -= gyrosBias.x; - gyrosData.y -= gyrosBias.y; - gyrosData.z -= gyrosBias.z; - }*/ GyroSensorSet(&gyroSensorData); // Because most crafts wont get enough information from gravity to zero yaw gyro, we try @@ -404,14 +396,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters) mag.z = mags[2]; } - // Correct for mag bias and update if the rate is non zero -// TODO -/* - if (cal.MagBiasNullingRate > 0) { - magOffsetEstimation(&mag); - } - */ - MagnetoSensorSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); } From daf329d8f528859429dbadc1814b93256df95cbb Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 19 May 2013 11:38:18 +0200 Subject: [PATCH 05/61] uncrustification (again) --- flight/libraries/CoordinateConversions.c | 522 +- flight/libraries/WorldMagModel.c | 1850 +- flight/libraries/aes.c | 636 +- flight/libraries/alarms.c | 15 +- flight/libraries/fifo_buffer.c | 232 +- flight/libraries/inc/CoordinateConversions.h | 30 +- flight/libraries/inc/WMMInternal.h | 155 +- flight/libraries/inc/WorldMagModel.h | 2 +- flight/libraries/inc/aes.h | 7 +- flight/libraries/inc/alarms.h | 6 +- flight/libraries/inc/fifo_buffer.h | 7 +- flight/libraries/inc/insgps.h | 42 +- flight/libraries/inc/op_dfu.h | 26 +- flight/libraries/inc/packet_handler.h | 76 +- flight/libraries/inc/paths.h | 12 +- flight/libraries/inc/sanitycheck.h | 6 +- flight/libraries/inc/stopwatch.h | 6 +- flight/libraries/insgps13state.c | 1137 +- flight/libraries/insgps16state.c | 1290 +- flight/libraries/insgps_helper.c | 522 +- flight/libraries/math/pid.c | 109 +- flight/libraries/math/pid.h | 20 +- flight/libraries/math/sin_lookup.c | 103 +- flight/libraries/math/sin_lookup.h | 2 +- flight/libraries/op_dfu.c | 790 +- flight/libraries/paths.c | 98 +- flight/libraries/printf-stdarg.c | 312 +- flight/libraries/printf2.c | 769 +- flight/libraries/sanitycheck.c | 229 +- flight/libraries/stopwatch.c | 137 +- flight/modules/Actuator/actuator.c | 973 +- flight/modules/Actuator/inc/actuator.h | 10 +- flight/modules/Airspeed/revolution/airspeed.c | 164 +- .../revolution/baro_airspeed_etasv3.c | 79 +- .../Airspeed/revolution/baro_airspeed_mpxv.c | 105 +- .../Airspeed/revolution/gps_airspeed.c | 148 +- .../Airspeed/revolution/inc/airspeed.h | 4 +- .../revolution/inc/baro_airspeed_etasv3.h | 4 +- .../revolution/inc/baro_airspeed_mpxv.h | 4 +- .../Airspeed/revolution/inc/gps_airspeed.h | 4 +- flight/modules/Altitude/altitude.c | 203 +- flight/modules/Altitude/inc/altitude.h | 4 +- flight/modules/Altitude/revolution/altitude.c | 158 +- .../Altitude/revolution/inc/altitude.h | 4 +- flight/modules/AltitudeHold/altitudehold.c | 500 +- flight/modules/Attitude/attitude.c | 913 +- flight/modules/Attitude/inc/attitude.h | 4 +- flight/modules/Attitude/revolution/attitude.c | 1574 +- .../Attitude/revolution/inc/attitude.h | 4 +- flight/modules/Autotune/autotune.c | 348 +- flight/modules/Autotune/inc/autotune.h | 4 +- flight/modules/Battery/battery.c | 231 +- flight/modules/Battery/inc/battery.h | 10 +- flight/modules/CallbackTest/callbacktest.c | 136 +- flight/modules/CameraStab/camerastab.c | 406 +- flight/modules/CameraStab/inc/camerastab.h | 10 +- flight/modules/ComUsbBridge/ComUsbBridge.c | 188 +- flight/modules/Example/example.c | 6 +- flight/modules/Example/examplemodcallback.c | 87 +- flight/modules/Example/examplemodevent.c | 80 +- flight/modules/Example/examplemodperiodic.c | 84 +- flight/modules/Example/examplemodthread.c | 112 +- .../modules/Example/inc/examplemodperiodic.h | 1 - flight/modules/Example/inc/examplemodthread.h | 1 - .../modules/Extensions/MagBaro/inc/magbaro.h | 4 +- flight/modules/Extensions/MagBaro/magbaro.c | 187 +- flight/modules/Fault/Fault.c | 153 +- flight/modules/FirmwareIAP/firmwareiap.c | 282 +- flight/modules/FirmwareIAP/inc/firmwareiap.h | 1 - .../fixedwingpathfollower.c | 902 +- flight/modules/FlightPlan/flightplan.c | 352 +- flight/modules/GPS/GPS.c | 381 +- flight/modules/GPS/NMEA.c | 967 +- flight/modules/GPS/UBX.c | 474 +- flight/modules/GPS/inc/GPS.h | 32 +- flight/modules/GPS/inc/NMEA.h | 2 +- flight/modules/GPS/inc/UBX.h | 238 +- flight/modules/MK/MKSerial/MKSerial.c | 728 +- flight/modules/MK/MKSerial/inc/MkSerial.h | 16 +- .../modules/ManualControl/inc/manualcontrol.h | 160 +- flight/modules/ManualControl/manualcontrol.c | 740 +- flight/modules/OPLink/inc/oplinkmod.h | 4 +- flight/modules/OPLink/oplinkmod.c | 244 +- flight/modules/Osd/OsdEtStd/OsdEtStd.c | 651 +- flight/modules/Osd/WavPlayer/wavplayer.c | 12 +- flight/modules/Osd/osdgen/inc/osdgen.h | 137 +- flight/modules/Osd/osdgen/osdgen.c | 1181 +- flight/modules/Osd/osdinput/osdinput.c | 51 +- flight/modules/Osd/osdoutput/inc/osdoutput.h | 2 +- flight/modules/Osd/osdoutput/osdoutput.c | 212 +- flight/modules/OveroSync/inc/overosync.h | 14 +- flight/modules/OveroSync/overosync.c | 191 +- .../OveroSync/simulated/inc/overosync.h | 14 +- .../modules/OveroSync/simulated/overosync.c | 225 +- flight/modules/PathPlanner/pathplanner.c | 606 +- .../modules/RadioComBridge/RadioComBridge.c | 173 +- .../RadioComBridge/inc/radiocombridge.h | 10 +- flight/modules/Sensors/inc/sensors.h | 4 +- flight/modules/Sensors/sensors.c | 771 +- .../modules/Sensors/simulated/inc/sensors.h | 4 +- flight/modules/Sensors/simulated/sensors.c | 1495 +- .../modules/Stabilization/inc/stabilization.h | 14 +- .../modules/Stabilization/inc/virtualflybar.h | 2 +- flight/modules/Stabilization/relay_tuning.c | 152 +- flight/modules/Stabilization/stabilization.c | 661 +- flight/modules/Stabilization/virtualflybar.c | 98 +- flight/modules/System/inc/systemmod.h | 4 +- flight/modules/System/systemmod.c | 191 +- flight/modules/Telemetry/inc/telemetry.h | 14 +- flight/modules/Telemetry/telemetry.c | 689 +- flight/modules/TxPID/inc/txpid.h | 10 +- flight/modules/TxPID/txpid.c | 412 +- .../VtolPathFollower/vtolpathfollower.c | 454 +- flight/pios/common/pios_adxl345.c | 356 +- flight/pios/common/pios_bma180.c | 589 +- flight/pios/common/pios_bmp085.c | 446 +- flight/pios/common/pios_board_info.c | 22 +- flight/pios/common/pios_com.c | 678 +- flight/pios/common/pios_com_msg.c | 208 +- flight/pios/common/pios_crc.c | 221 +- flight/pios/common/pios_etasv3.c | 84 +- flight/pios/common/pios_flash_jedec.c | 603 +- flight/pios/common/pios_flashfs_logfs.c | 1471 +- flight/pios/common/pios_gcsrcvr.c | 157 +- flight/pios/common/pios_hcsr04.c | 395 +- flight/pios/common/pios_hmc5843.c | 572 +- flight/pios/common/pios_hmc5883.c | 472 +- flight/pios/common/pios_i2c_esc.c | 252 +- flight/pios/common/pios_iap.c | 166 +- flight/pios/common/pios_l3gd20.c | 450 +- flight/pios/common/pios_mpu6000.c | 774 +- flight/pios/common/pios_mpxv.c | 64 +- flight/pios/common/pios_ms5611.c | 323 +- flight/pios/common/pios_rcvr.c | 110 +- flight/pios/common/pios_rfm22b.c | 897 +- flight/pios/common/pios_rfm22b_com.c | 85 +- flight/pios/common/pios_rfm22b_rcvr.c | 36 +- flight/pios/common/pios_sbus.c | 363 +- flight/pios/common/pios_sdcard.c | 1504 +- flight/pios/common/pios_task_monitor.c | 127 +- flight/pios/common/pios_usb_desc_hid_cdc.c | 514 +- flight/pios/common/pios_usb_desc_hid_only.c | 222 +- flight/pios/common/pios_usb_util.c | 14 +- flight/pios/common/pios_video.c | 239 +- flight/pios/common/pios_wavplay.c | 830 +- flight/pios/inc/pios_adc.h | 36 +- flight/pios/inc/pios_adc_priv.h | 13 +- flight/pios/inc/pios_adxl345.h | 60 +- flight/pios/inc/pios_bkp.h | 129 +- flight/pios/inc/pios_bl_helper.h | 2 +- flight/pios/inc/pios_bma180.h | 93 +- flight/pios/inc/pios_bmp085.h | 78 +- flight/pios/inc/pios_board_info.h | 24 +- flight/pios/inc/pios_com.h | 52 +- flight/pios/inc/pios_com_msg.h | 34 +- flight/pios/inc/pios_com_msg_priv.h | 12 +- flight/pios/inc/pios_com_priv.h | 8 +- flight/pios/inc/pios_constants.h | 224 +- flight/pios/inc/pios_crc.h | 30 +- flight/pios/inc/pios_debug.h | 46 +- flight/pios/inc/pios_delay.h | 30 +- flight/pios/inc/pios_dsm.h | 22 +- flight/pios/inc/pios_dsm_priv.h | 40 +- flight/pios/inc/pios_eeprom.h | 34 +- flight/pios/inc/pios_etasv3.h | 52 +- flight/pios/inc/pios_exti.h | 42 +- flight/pios/inc/pios_flash.h | 20 +- flight/pios/inc/pios_flash_internal_priv.h | 34 +- flight/pios/inc/pios_flash_jedec_catalog.h | 58 +- flight/pios/inc/pios_flash_jedec_priv.h | 16 +- flight/pios/inc/pios_flashfs.h | 6 +- flight/pios/inc/pios_flashfs_logfs_priv.h | 20 +- flight/pios/inc/pios_gpio.h | 28 +- flight/pios/inc/pios_hcsr04.h | 30 +- flight/pios/inc/pios_hcsr04_priv.h | 10 +- flight/pios/inc/pios_hmc5843.h | 30 +- flight/pios/inc/pios_hmc5883.h | 131 +- flight/pios/inc/pios_i2c.h | 62 +- flight/pios/inc/pios_i2c_esc.h | 24 +- flight/pios/inc/pios_i2c_priv.h | 104 +- flight/pios/inc/pios_iap.h | 83 +- flight/pios/inc/pios_initcall.h | 84 +- flight/pios/inc/pios_irq.h | 26 +- flight/pios/inc/pios_l3gd20.h | 66 +- flight/pios/inc/pios_led.h | 24 +- flight/pios/inc/pios_led_priv.h | 18 +- flight/pios/inc/pios_math.h | 10 +- flight/pios/inc/pios_mpu6000.h | 118 +- flight/pios/inc/pios_mpu6000_config.h | 68 +- flight/pios/inc/pios_mpxv.h | 94 +- flight/pios/inc/pios_ms5611.h | 70 +- flight/pios/inc/pios_overo_priv.h | 22 +- flight/pios/inc/pios_posix.h | 36 +- flight/pios/inc/pios_ppm_out_priv.h | 6 +- flight/pios/inc/pios_ppm_priv.h | 8 +- flight/pios/inc/pios_pwm_priv.h | 8 +- flight/pios/inc/pios_rcvr.h | 46 +- flight/pios/inc/pios_rcvr_priv.h | 8 +- flight/pios/inc/pios_rfm22b.h | 128 +- flight/pios/inc/pios_rfm22b_com.h | 28 +- flight/pios/inc/pios_rfm22b_priv.h | 922 +- flight/pios/inc/pios_rtc.h | 24 +- flight/pios/inc/pios_rtc_priv.h | 9 +- flight/pios/inc/pios_sbus.h | 24 +- flight/pios/inc/pios_sbus_priv.h | 36 +- flight/pios/inc/pios_sdcard.h | 132 +- flight/pios/inc/pios_servo.h | 32 +- flight/pios/inc/pios_servo_priv.h | 14 +- flight/pios/inc/pios_spi.h | 16 +- flight/pios/inc/pios_spi_priv.h | 40 +- flight/pios/inc/pios_stm32.h | 58 +- flight/pios/inc/pios_struct_helper.h | 8 +- flight/pios/inc/pios_sys.h | 36 +- flight/pios/inc/pios_task_monitor.h | 20 +- flight/pios/inc/pios_tim.h | 2 +- flight/pios/inc/pios_tim_priv.h | 24 +- flight/pios/inc/pios_udp.h | 26 +- flight/pios/inc/pios_udp_priv.h | 38 +- flight/pios/inc/pios_usart.h | 30 +- flight/pios/inc/pios_usart_priv.h | 22 +- flight/pios/inc/pios_usb.h | 6 +- flight/pios/inc/pios_usb_board_data_priv.h | 28 +- flight/pios/inc/pios_usb_cdc_priv.h | 18 +- flight/pios/inc/pios_usb_defs.h | 404 +- flight/pios/inc/pios_usb_desc_hid_cdc_priv.h | 28 +- flight/pios/inc/pios_usb_desc_hid_only_priv.h | 28 +- flight/pios/inc/pios_usb_hid.h | 10 +- flight/pios/inc/pios_usb_hid_istr.h | 2 +- flight/pios/inc/pios_usb_hid_priv.h | 14 +- flight/pios/inc/pios_usb_hid_pwr.h | 34 +- flight/pios/inc/pios_usb_priv.h | 16 +- flight/pios/inc/pios_usb_rctx_priv.h | 12 +- flight/pios/inc/pios_usb_util.h | 6 +- flight/pios/inc/pios_usbhook.h | 31 +- flight/pios/inc/pios_video.h | 56 +- flight/pios/inc/pios_wavplay.h | 18 +- flight/pios/inc/pios_wdg.h | 2 +- flight/pios/inc/stm32f10x_conf.h | 58 +- flight/pios/inc/stm32f2xx_conf.h | 78 +- flight/pios/inc/stm32f4xx_conf.h | 78 +- flight/pios/inc/usb_conf.h | 159 +- flight/pios/osx/inc/FreeRTOSConfig.h | 150 +- flight/pios/osx/inc/pios_bl_helper.h | 2 +- flight/pios/osx/inc/pios_board_info.h | 24 +- flight/pios/osx/inc/pios_com.h | 52 +- flight/pios/osx/inc/pios_com_priv.h | 8 +- flight/pios/osx/inc/pios_crc.h | 26 +- flight/pios/osx/inc/pios_debug.h | 38 +- flight/pios/osx/inc/pios_delay.h | 30 +- flight/pios/osx/inc/pios_iap.h | 40 +- flight/pios/osx/inc/pios_initcall.h | 50 +- flight/pios/osx/inc/pios_irq.h | 26 +- flight/pios/osx/inc/pios_led.h | 24 +- flight/pios/osx/inc/pios_posix.h | 35 +- flight/pios/osx/inc/pios_rcvr.h | 50 +- flight/pios/osx/inc/pios_rcvr_priv.h | 10 +- flight/pios/osx/inc/pios_sdcard.h | 128 +- flight/pios/osx/inc/pios_servo.h | 32 +- flight/pios/osx/inc/pios_sim.h | 3 +- flight/pios/osx/inc/pios_sim_priv.h | 17 +- flight/pios/osx/inc/pios_struct_helper.h | 8 +- flight/pios/osx/inc/pios_sys.h | 36 +- flight/pios/osx/inc/pios_tcp.h | 26 +- flight/pios/osx/inc/pios_tcp_priv.h | 44 +- flight/pios/osx/inc/pios_udp.h | 26 +- flight/pios/osx/inc/pios_udp_priv.h | 37 +- flight/pios/osx/inc/pios_wdg.h | 12 +- flight/pios/osx/inc/sim_model.h | 3 +- flight/pios/osx/osx/pios_bl_helper.c | 16 +- flight/pios/osx/osx/pios_board_info.c | 22 +- flight/pios/osx/osx/pios_com.c | 663 +- flight/pios/osx/osx/pios_crc.c | 56 +- flight/pios/osx/osx/pios_debug.c | 80 +- flight/pios/osx/osx/pios_delay.c | 128 +- flight/pios/osx/osx/pios_gcsrcvr.c | 102 +- flight/pios/osx/osx/pios_iap.c | 44 +- flight/pios/osx/osx/pios_irq.c | 58 +- flight/pios/osx/osx/pios_led.c | 67 +- flight/pios/osx/osx/pios_rcvr.c | 112 +- flight/pios/osx/osx/pios_sdcard.c | 376 +- flight/pios/osx/osx/pios_servo.c | 67 +- flight/pios/osx/osx/pios_sim.c | 77 +- flight/pios/osx/osx/pios_sys.c | 119 +- flight/pios/osx/osx/pios_tcp.c | 381 +- flight/pios/osx/osx/pios_udp.c | 272 +- flight/pios/osx/osx/pios_wdg.c | 100 +- flight/pios/osx/pios.h | 28 +- flight/pios/pios.h | 24 +- flight/pios/pios_sim_posix.h | 26 +- flight/pios/posix/pios_bl_helper.c | 16 +- flight/pios/posix/pios_board_info.c | 22 +- flight/pios/posix/pios_com.c | 663 +- flight/pios/posix/pios_crc.c | 56 +- flight/pios/posix/pios_debug.c | 78 +- flight/pios/posix/pios_delay.c | 139 +- flight/pios/posix/pios_iap.c | 44 +- flight/pios/posix/pios_irq.c | 58 +- flight/pios/posix/pios_led.c | 97 +- flight/pios/posix/pios_rcvr.c | 128 +- flight/pios/posix/pios_sdcard.c | 376 +- flight/pios/posix/pios_servo.c | 67 +- flight/pios/posix/pios_sys.c | 240 +- flight/pios/posix/pios_udp.c | 269 +- flight/pios/posix/pios_wdg.c | 65 +- flight/pios/stm32f10x/pios_adc.c | 448 +- flight/pios/stm32f10x/pios_bkp.c | 181 +- flight/pios/stm32f10x/pios_bl_helper.c | 50 +- flight/pios/stm32f10x/pios_debug.c | 166 +- flight/pios/stm32f10x/pios_delay.c | 122 +- flight/pios/stm32f10x/pios_dsm.c | 465 +- flight/pios/stm32f10x/pios_eeprom.c | 202 +- flight/pios/stm32f10x/pios_exti.c | 364 +- flight/pios/stm32f10x/pios_flash_internal.c | 151 +- flight/pios/stm32f10x/pios_gpio.c | 81 +- flight/pios/stm32f10x/pios_i2c.c | 1518 +- flight/pios/stm32f10x/pios_irq.c | 96 +- flight/pios/stm32f10x/pios_led.c | 188 +- flight/pios/stm32f10x/pios_ppm.c | 448 +- flight/pios/stm32f10x/pios_ppm_out.c | 384 +- flight/pios/stm32f10x/pios_pwm.c | 322 +- flight/pios/stm32f10x/pios_rtc.c | 96 +- flight/pios/stm32f10x/pios_servo.c | 174 +- flight/pios/stm32f10x/pios_spi.c | 829 +- flight/pios/stm32f10x/pios_sys.c | 259 +- flight/pios/stm32f10x/pios_tim.c | 645 +- flight/pios/stm32f10x/pios_usart.c | 390 +- flight/pios/stm32f10x/pios_usb.c | 194 +- flight/pios/stm32f10x/pios_usb_cdc.c | 492 +- flight/pios/stm32f10x/pios_usb_hid.c | 384 +- flight/pios/stm32f10x/pios_usb_hid_istr.c | 353 +- flight/pios/stm32f10x/pios_usb_hid_pwr.c | 277 +- flight/pios/stm32f10x/pios_usb_rctx.c | 202 +- flight/pios/stm32f10x/pios_usbhook.c | 454 +- flight/pios/stm32f10x/pios_wdg.c | 150 +- flight/pios/stm32f4xx/inc/pios_i2c_config.h | 104 +- flight/pios/stm32f4xx/inc/pios_usart_config.h | 80 +- flight/pios/stm32f4xx/inc/stm32f4xx_conf.h | 60 +- flight/pios/stm32f4xx/pios_adc.c | 438 +- flight/pios/stm32f4xx/pios_bkp.c | 128 +- flight/pios/stm32f4xx/pios_bl_helper.c | 214 +- flight/pios/stm32f4xx/pios_debug.c | 172 +- flight/pios/stm32f4xx/pios_delay.c | 122 +- flight/pios/stm32f4xx/pios_dsm.c | 461 +- flight/pios/stm32f4xx/pios_exti.c | 364 +- flight/pios/stm32f4xx/pios_flash_internal.c | 465 +- flight/pios/stm32f4xx/pios_gpio.c | 83 +- flight/pios/stm32f4xx/pios_i2c.c | 1658 +- flight/pios/stm32f4xx/pios_irq.c | 94 +- flight/pios/stm32f4xx/pios_led.c | 228 +- flight/pios/stm32f4xx/pios_overo.c | 453 +- flight/pios/stm32f4xx/pios_ppm.c | 434 +- flight/pios/stm32f4xx/pios_pwm.c | 322 +- flight/pios/stm32f4xx/pios_rtc.c | 115 +- flight/pios/stm32f4xx/pios_servo.c | 187 +- flight/pios/stm32f4xx/pios_spi.c | 954 +- flight/pios/stm32f4xx/pios_sys.c | 218 +- flight/pios/stm32f4xx/pios_tim.c | 661 +- flight/pios/stm32f4xx/pios_usart.c | 430 +- flight/pios/stm32f4xx/pios_usb.c | 214 +- flight/pios/stm32f4xx/pios_usb_cdc.c | 460 +- flight/pios/stm32f4xx/pios_usb_hid.c | 625 +- flight/pios/stm32f4xx/pios_usbhook.c | 465 +- flight/pios/stm32f4xx/pios_wdg.c | 142 +- flight/pios/stm32f4xx/startup.c | 115 +- flight/pios/stm32f4xx/vectors_stm32f4xx.c | 339 +- flight/pios/win32/inc/FreeRTOSConfig.h | 102 +- flight/pios/win32/inc/pios_com.h | 38 +- flight/pios/win32/inc/pios_com_priv.h | 9 +- flight/pios/win32/inc/pios_crc.h | 26 +- flight/pios/win32/inc/pios_delay.h | 26 +- flight/pios/win32/inc/pios_initcall.h | 39 +- flight/pios/win32/inc/pios_led.h | 28 +- flight/pios/win32/inc/pios_posix.h | 29 +- flight/pios/win32/inc/pios_sdcard.h | 128 +- flight/pios/win32/inc/pios_servo.h | 32 +- flight/pios/win32/inc/pios_sys.h | 26 +- flight/pios/win32/inc/pios_udp.h | 28 +- flight/pios/win32/inc/pios_udp_priv.h | 29 +- flight/pios/win32/inc/pios_wdg.h | 20 +- flight/pios/win32/pios.h | 26 +- flight/pios/win32/win32/pios_com.c | 348 +- flight/pios/win32/win32/pios_crc.c | 56 +- flight/pios/win32/win32/pios_delay.c | 92 +- flight/pios/win32/win32/pios_led.c | 97 +- flight/pios/win32/win32/pios_sdcard.c | 376 +- flight/pios/win32/win32/pios_servo.c | 67 +- flight/pios/win32/win32/pios_sys.c | 227 +- flight/pios/win32/win32/pios_udp.c | 628 +- flight/pios/win32/win32/pios_wdg.c | 66 +- .../boards/coptercontrol/board_hw_defs.c | 2338 +- .../coptercontrol/bootloader/inc/common.h | 90 +- .../bootloader/inc/pios_config.h | 26 +- .../bootloader/inc/pios_usb_board_data.h | 12 +- .../boards/coptercontrol/bootloader/main.c | 275 +- .../coptercontrol/bootloader/pios_board.c | 92 +- .../coptercontrol/firmware/coptercontrol.c | 128 +- .../firmware/inc/FreeRTOSConfig.h | 132 +- .../coptercontrol/firmware/inc/openpilot.h | 22 +- .../firmware/inc/pios_board_posix.h | 83 +- .../coptercontrol/firmware/inc/pios_config.h | 28 +- .../firmware/inc/pios_config_posix.h | 30 +- .../firmware/inc/pios_usb_board_data.h | 12 +- .../coptercontrol/firmware/pios_board.c | 1400 +- .../coptercontrol/firmware/pios_board_posix.c | 137 +- .../targets/boards/coptercontrol/pios_board.h | 298 +- .../coptercontrol/pios_usb_board_data.c | 105 +- .../targets/boards/oplinkmini/board_hw_defs.c | 961 +- .../boards/oplinkmini/bootloader/inc/common.h | 90 +- .../oplinkmini/bootloader/inc/pios_config.h | 26 +- .../bootloader/inc/pios_usb_board_data.h | 12 +- .../boards/oplinkmini/bootloader/main.c | 277 +- .../boards/oplinkmini/bootloader/pios_board.c | 67 +- .../oplinkmini/firmware/inc/FreeRTOSConfig.h | 132 +- .../oplinkmini/firmware/inc/openpilot.h | 22 +- .../firmware/inc/pios_board_posix.h | 83 +- .../oplinkmini/firmware/inc/pios_config.h | 30 +- .../firmware/inc/pios_config_posix.h | 38 +- .../firmware/inc/pios_usb_board_data.h | 12 +- .../boards/oplinkmini/firmware/oplink.c | 90 +- .../boards/oplinkmini/firmware/pios_board.c | 399 +- .../oplinkmini/firmware/pios_board_posix.c | 137 +- flight/targets/boards/oplinkmini/pios_board.h | 362 +- .../boards/oplinkmini/pios_usb_board_data.c | 97 +- flight/targets/boards/osd/board_hw_defs.c | 1497 +- .../boards/osd/bootloader/inc/common.h | 90 +- .../boards/osd/bootloader/inc/pios_config.h | 26 +- .../osd/bootloader/inc/pios_usb_board_data.h | 12 +- flight/targets/boards/osd/bootloader/main.c | 279 +- .../boards/osd/bootloader/pios_board.c | 75 +- .../boards/osd/firmware/cm3_fault_handlers.c | 109 +- .../targets/boards/osd/firmware/dcc_stdio.c | 176 +- .../boards/osd/firmware/font_outlined8x14.c | 502 +- .../boards/osd/firmware/font_outlined8x8.c | 330 +- flight/targets/boards/osd/firmware/fonts.c | 26 +- .../boards/osd/firmware/inc/FreeRTOSConfig.h | 136 +- .../boards/osd/firmware/inc/dcc_stdio.h | 42 +- .../boards/osd/firmware/inc/font12x18.h | 19454 ++++++++-------- .../boards/osd/firmware/inc/font8x10.h | 11262 +++++---- .../targets/boards/osd/firmware/inc/fonts.h | 25 +- .../boards/osd/firmware/inc/openpilot.h | 22 +- .../boards/osd/firmware/inc/pios_config.h | 14 +- .../osd/firmware/inc/pios_usb_board_data.h | 12 +- .../targets/boards/osd/firmware/inc/splash.h | 2844 +-- flight/targets/boards/osd/firmware/osd.c | 184 +- .../targets/boards/osd/firmware/pios_board.c | 630 +- flight/targets/boards/osd/pios_board.h | 282 +- .../targets/boards/osd/pios_usb_board_data.c | 85 +- .../targets/boards/revolution/board_hw_defs.c | 3036 +-- .../boards/revolution/bootloader/inc/common.h | 90 +- .../revolution/bootloader/inc/pios_config.h | 26 +- .../bootloader/inc/pios_usb_board_data.h | 12 +- .../boards/revolution/bootloader/main.c | 310 +- .../boards/revolution/bootloader/pios_board.c | 83 +- .../revolution/firmware/cm3_fault_handlers.c | 109 +- .../boards/revolution/firmware/dcc_stdio.c | 176 +- .../revolution/firmware/inc/FreeRTOSConfig.h | 136 +- .../revolution/firmware/inc/dcc_stdio.h | 42 +- .../revolution/firmware/inc/openpilot.h | 22 +- .../revolution/firmware/inc/pios_board_sim.h | 71 +- .../revolution/firmware/inc/pios_config.h | 16 +- .../revolution/firmware/inc/pios_config_sim.h | 60 +- .../firmware/inc/pios_usb_board_data.h | 12 +- .../boards/revolution/firmware/pios_board.c | 1235 +- .../revolution/firmware/pios_board_sim.c | 241 +- .../boards/revolution/firmware/revolution.c | 121 +- flight/targets/boards/revolution/pios_board.h | 308 +- .../boards/revolution/pios_usb_board_data.c | 99 +- .../targets/boards/revoproto/board_hw_defs.c | 3195 +-- .../boards/revoproto/bootloader/inc/common.h | 90 +- .../revoproto/bootloader/inc/pios_config.h | 26 +- .../bootloader/inc/pios_usb_board_data.h | 12 +- .../boards/revoproto/bootloader/main.c | 304 +- .../boards/revoproto/bootloader/pios_board.c | 75 +- .../revoproto/firmware/cm3_fault_handlers.c | 109 +- .../boards/revoproto/firmware/dcc_stdio.c | 176 +- .../revoproto/firmware/inc/FreeRTOSConfig.h | 136 +- .../boards/revoproto/firmware/inc/dcc_stdio.h | 42 +- .../boards/revoproto/firmware/inc/openpilot.h | 22 +- .../revoproto/firmware/inc/pios_board_sim.h | 71 +- .../revoproto/firmware/inc/pios_config.h | 16 +- .../revoproto/firmware/inc/pios_config_sim.h | 60 +- .../firmware/inc/pios_usb_board_data.h | 12 +- .../boards/revoproto/firmware/pios_board.c | 1405 +- .../revoproto/firmware/pios_board_sim.c | 241 +- .../boards/revoproto/firmware/revolution.c | 121 +- flight/targets/boards/revoproto/pios_board.h | 262 +- .../boards/revoproto/pios_usb_board_data.c | 99 +- .../targets/boards/simposix/board_hw_defs.c | 35 +- .../simposix/firmware/inc/FreeRTOSConfig.h | 148 +- .../simposix/firmware/inc/FreeRTOSConfig_bk.h | 136 +- .../boards/simposix/firmware/inc/dcc_stdio.h | 42 +- .../boards/simposix/firmware/inc/openpilot.h | 22 +- .../simposix/firmware/inc/pios_config.h | 78 +- .../boards/simposix/firmware/pios_board.c | 239 +- .../boards/simposix/firmware/simposix.c | 119 +- flight/targets/boards/simposix/pios_board.h | 280 +- .../bootloader_updater/inc/pios_config.h | 26 +- .../targets/common/bootloader_updater/main.c | 63 +- .../common/bootloader_updater/pios_board.c | 22 +- flight/tests/logfs/FreeRTOS.h | 2 +- flight/tests/logfs/openpilot.h | 4 +- flight/tests/logfs/pios_config.h | 2 +- flight/tests/logfs/pios_flash_ut.c | 190 +- flight/tests/logfs/pios_flash_ut_priv.h | 6 +- flight/tests/logfs/unittest.cpp | 487 +- flight/tests/logfs/unittest_init.c | 35 +- flight/uavobjects/callbackscheduler.c | 456 +- flight/uavobjects/eventdispatcher.c | 318 +- flight/uavobjects/inc/callbackscheduler.h | 54 +- flight/uavobjects/inc/eventdispatcher.h | 16 +- flight/uavobjects/inc/uavobjectmanager.h | 132 +- flight/uavobjects/inc/utlist.h | 583 +- flight/uavobjects/uavobjectmanager.c | 2214 +- flight/uavtalk/inc/uavtalk.h | 6 +- flight/uavtalk/inc/uavtalk_priv.h | 98 +- flight/uavtalk/uavtalk.c | 1309 +- .../openpilotgcs/src/app/gcssplashscreen.cpp | 12 +- ground/openpilotgcs/src/app/gcssplashscreen.h | 11 +- ground/openpilotgcs/src/app/main.cpp | 611 +- .../src/experimental/HIDTest/main.cpp | 50 +- .../src/experimental/PowerLog6S/main.cpp | 310 +- .../src/experimental/SerialLogger/main.cpp | 56 +- .../experimental/USB_UPLOAD_TOOL/SSP/main.cpp | 91 +- .../experimental/USB_UPLOAD_TOOL/SSP/port.cpp | 30 +- .../experimental/USB_UPLOAD_TOOL/SSP/port.h | 53 +- .../experimental/USB_UPLOAD_TOOL/SSP/qssp.cpp | 515 +- .../experimental/USB_UPLOAD_TOOL/SSP/qssp.h | 104 +- .../USB_UPLOAD_TOOL/SSP/qsspt.cpp | 57 +- .../experimental/USB_UPLOAD_TOOL/SSP/qsspt.h | 11 +- .../experimental/USB_UPLOAD_TOOL/SSP/ssp.cpp | 709 +- .../experimental/USB_UPLOAD_TOOL/SSP/ssp.h | 163 +- .../experimental/USB_UPLOAD_TOOL/delay.cpp | 2 - .../src/experimental/USB_UPLOAD_TOOL/delay.h | 3 +- .../src/experimental/USB_UPLOAD_TOOL/main.cpp | 409 +- .../experimental/USB_UPLOAD_TOOL/op_dfu.cpp | 996 +- .../src/experimental/USB_UPLOAD_TOOL/op_dfu.h | 156 +- .../src/experimental/finaltest/main.cpp | 1 + .../src/experimental/finaltest/mainwindow.cpp | 89 +- .../src/experimental/finaltest/mainwindow.h | 8 +- .../experimental/finaltest/ui_mainwindow.h | 15 +- .../tools/DocumentationHelper/main.cpp | 54 +- .../tools/DocumentationHelper/mainwindow.cpp | 300 +- .../tools/DocumentationHelper/mainwindow.h | 52 +- .../src/libs/aggregation/aggregate.cpp | 81 +- .../src/libs/aggregation/aggregate.h | 65 +- .../src/libs/aggregation/aggregation_global.h | 28 +- .../libs/aggregation/examples/text/main.cpp | 30 +- .../src/libs/aggregation/examples/text/main.h | 31 +- .../aggregation/examples/text/myinterfaces.h | 60 +- .../extensionsystem/extensionsystem_global.h | 28 +- .../src/libs/extensionsystem/iplugin.cpp | 57 +- .../src/libs/extensionsystem/iplugin.h | 39 +- .../src/libs/extensionsystem/iplugin_p.h | 34 +- .../libs/extensionsystem/optionsparser.cpp | 118 +- .../src/libs/extensionsystem/optionsparser.h | 47 +- .../extensionsystem/plugindetailsview.cpp | 42 +- .../libs/extensionsystem/plugindetailsview.h | 39 +- .../libs/extensionsystem/pluginerrorview.cpp | 55 +- .../libs/extensionsystem/pluginerrorview.h | 35 +- .../libs/extensionsystem/pluginmanager.cpp | 219 +- .../src/libs/extensionsystem/pluginmanager.h | 60 +- .../libs/extensionsystem/pluginmanager_p.h | 38 +- .../src/libs/extensionsystem/pluginspec.cpp | 334 +- .../src/libs/extensionsystem/pluginspec.h | 46 +- .../src/libs/extensionsystem/pluginspec_p.h | 34 +- .../src/libs/extensionsystem/pluginview.cpp | 91 +- .../src/libs/extensionsystem/pluginview.h | 37 +- .../src/libs/extensionsystem/pluginview_p.h | 34 +- .../circularplugins/plugin1/plugin1.cpp | 35 +- .../circularplugins/plugin1/plugin1.h | 33 +- .../circularplugins/plugin2/plugin2.cpp | 35 +- .../circularplugins/plugin2/plugin2.h | 33 +- .../circularplugins/plugin3/plugin3.cpp | 35 +- .../circularplugins/plugin3/plugin3.h | 33 +- .../correctplugins1/plugin1/plugin1.cpp | 51 +- .../correctplugins1/plugin1/plugin1.h | 33 +- .../correctplugins1/plugin2/plugin2.cpp | 35 +- .../correctplugins1/plugin2/plugin2.h | 33 +- .../correctplugins1/plugin3/plugin3.cpp | 45 +- .../correctplugins1/plugin3/plugin3.h | 33 +- .../auto/pluginmanager/tst_pluginmanager.cpp | 129 +- .../auto/pluginspec/testplugin/testplugin.cpp | 32 +- .../auto/pluginspec/testplugin/testplugin.h | 43 +- .../pluginspec/testplugin/testplugin_global.h | 28 +- .../test/auto/pluginspec/tst_pluginspec.cpp | 60 +- .../test/manual/pluginview/plugindialog.cpp | 48 +- .../test/manual/pluginview/plugindialog.h | 31 +- .../pluginview/plugins/plugin1/plugin1.cpp | 50 +- .../pluginview/plugins/plugin1/plugin1.h | 33 +- .../pluginview/plugins/plugin2/plugin2.cpp | 34 +- .../pluginview/plugins/plugin2/plugin2.h | 33 +- .../pluginview/plugins/plugin3/plugin3.cpp | 45 +- .../pluginview/plugins/plugin3/plugin3.h | 33 +- .../src/libs/opmapcontrol/opmapcontrol.h | 25 +- .../libs/opmapcontrol/src/core/accessmode.h | 140 +- .../opmapcontrol/src/core/alllayersoftype.cpp | 147 +- .../opmapcontrol/src/core/alllayersoftype.h | 62 +- .../src/libs/opmapcontrol/src/core/cache.cpp | 364 +- .../src/libs/opmapcontrol/src/core/cache.h | 103 +- .../opmapcontrol/src/core/cacheitemqueue.cpp | 146 +- .../opmapcontrol/src/core/cacheitemqueue.h | 111 +- .../libs/opmapcontrol/src/core/debugheader.h | 16 +- .../opmapcontrol/src/core/diagnostics.cpp | 53 +- .../libs/opmapcontrol/src/core/diagnostics.h | 68 +- .../opmapcontrol/src/core/geodecoderstatus.h | 215 +- .../opmapcontrol/src/core/kibertilecache.cpp | 125 +- .../opmapcontrol/src/core/kibertilecache.h | 86 +- .../opmapcontrol/src/core/languagetype.cpp | 185 +- .../libs/opmapcontrol/src/core/languagetype.h | 236 +- .../src/libs/opmapcontrol/src/core/maptype.h | 212 +- .../opmapcontrol/src/core/memorycache.cpp | 103 +- .../libs/opmapcontrol/src/core/memorycache.h | 69 +- .../src/libs/opmapcontrol/src/core/opmaps.cpp | 538 +- .../src/libs/opmapcontrol/src/core/opmaps.h | 150 +- .../libs/opmapcontrol/src/core/placemark.cpp | 51 +- .../libs/opmapcontrol/src/core/placemark.h | 93 +- .../src/libs/opmapcontrol/src/core/point.cpp | 135 +- .../src/libs/opmapcontrol/src/core/point.h | 135 +- .../opmapcontrol/src/core/providerstrings.cpp | 147 +- .../opmapcontrol/src/core/providerstrings.h | 129 +- .../libs/opmapcontrol/src/core/pureimage.cpp | 57 +- .../libs/opmapcontrol/src/core/pureimage.h | 64 +- .../opmapcontrol/src/core/pureimagecache.cpp | 623 +- .../opmapcontrol/src/core/pureimagecache.h | 86 +- .../libs/opmapcontrol/src/core/rawtile.cpp | 75 +- .../src/libs/opmapcontrol/src/core/rawtile.h | 85 +- .../src/libs/opmapcontrol/src/core/size.cpp | 54 +- .../src/libs/opmapcontrol/src/core/size.h | 121 +- .../opmapcontrol/src/core/tilecachequeue.cpp | 142 +- .../opmapcontrol/src/core/tilecachequeue.h | 79 +- .../libs/opmapcontrol/src/core/urlfactory.cpp | 1313 +- .../libs/opmapcontrol/src/core/urlfactory.h | 124 +- .../src/internals/MouseWheelZoomType.cpp | 54 +- .../src/internals/copyrightstrings.h | 63 +- .../libs/opmapcontrol/src/internals/core.cpp | 1113 +- .../libs/opmapcontrol/src/internals/core.h | 462 +- .../opmapcontrol/src/internals/debugheader.h | 6 +- .../opmapcontrol/src/internals/loadtask.cpp | 56 +- .../opmapcontrol/src/internals/loadtask.h | 78 +- .../src/internals/mousewheelzoomtype.h | 84 +- .../src/internals/pointlatlng.cpp | 74 +- .../opmapcontrol/src/internals/pointlatlng.h | 174 +- .../internals/projections/lks94projection.cpp | 897 +- .../internals/projections/lks94projection.h | 101 +- .../projections/mercatorprojection.cpp | 77 +- .../projections/mercatorprojection.h | 71 +- .../projections/mercatorprojectionyandex.cpp | 109 +- .../projections/mercatorprojectionyandex.h | 73 +- .../projections/platecarreeprojection.cpp | 82 +- .../projections/platecarreeprojection.h | 72 +- .../platecarreeprojectionpergo.cpp | 82 +- .../projections/platecarreeprojectionpergo.h | 72 +- .../src/internals/pureprojection.cpp | 525 +- .../src/internals/pureprojection.h | 106 +- .../opmapcontrol/src/internals/rectangle.cpp | 125 +- .../opmapcontrol/src/internals/rectangle.h | 218 +- .../opmapcontrol/src/internals/rectlatlng.cpp | 64 +- .../opmapcontrol/src/internals/rectlatlng.h | 231 +- .../opmapcontrol/src/internals/sizelatlng.cpp | 80 +- .../opmapcontrol/src/internals/sizelatlng.h | 187 +- .../libs/opmapcontrol/src/internals/tile.cpp | 76 +- .../libs/opmapcontrol/src/internals/tile.h | 91 +- .../opmapcontrol/src/internals/tilematrix.cpp | 202 +- .../opmapcontrol/src/internals/tilematrix.h | 67 +- .../src/mapwidget/configuration.cpp | 65 +- .../src/mapwidget/configuration.h | 276 +- .../opmapcontrol/src/mapwidget/gpsitem.cpp | 329 +- .../libs/opmapcontrol/src/mapwidget/gpsitem.h | 443 +- .../opmapcontrol/src/mapwidget/homeitem.cpp | 291 +- .../opmapcontrol/src/mapwidget/homeitem.h | 169 +- .../src/mapwidget/mapgraphicitem.cpp | 922 +- .../src/mapwidget/mapgraphicitem.h | 433 +- .../opmapcontrol/src/mapwidget/mapripform.cpp | 52 +- .../opmapcontrol/src/mapwidget/mapripform.h | 59 +- .../opmapcontrol/src/mapwidget/mapripper.cpp | 217 +- .../opmapcontrol/src/mapwidget/mapripper.h | 102 +- .../src/mapwidget/opmapwidget.cpp | 1070 +- .../opmapcontrol/src/mapwidget/opmapwidget.h | 1057 +- .../opmapcontrol/src/mapwidget/trailitem.cpp | 108 +- .../opmapcontrol/src/mapwidget/trailitem.h | 91 +- .../src/mapwidget/traillineitem.cpp | 82 +- .../src/mapwidget/traillineitem.h | 85 +- .../opmapcontrol/src/mapwidget/uavitem.cpp | 806 +- .../libs/opmapcontrol/src/mapwidget/uavitem.h | 537 +- .../src/mapwidget/uavmapfollowtype.h | 138 +- .../opmapcontrol/src/mapwidget/uavtrailtype.h | 150 +- .../src/mapwidget/waypointcircle.cpp | 131 +- .../src/mapwidget/waypointcircle.h | 72 +- .../src/mapwidget/waypointitem.cpp | 934 +- .../opmapcontrol/src/mapwidget/waypointitem.h | 365 +- .../src/mapwidget/waypointline.cpp | 134 +- .../opmapcontrol/src/mapwidget/waypointline.h | 71 +- .../src/posix_qextserialport.cpp | 1125 +- .../qextserialport/src/qextserialenumerator.h | 243 +- .../src/qextserialenumerator_osx.cpp | 159 +- .../src/qextserialenumerator_unix.cpp | 43 +- .../src/qextserialenumerator_win.cpp | 122 +- .../qextserialport/src/qextserialport.cpp | 201 +- .../libs/qextserialport/src/qextserialport.h | 384 +- .../src/qextserialport_global.h | 3 - .../qextserialport/src/win_qextserialport.cpp | 1047 +- .../src/libs/qscispinbox/QScienceSpinBox.cpp | 394 +- .../src/libs/qscispinbox/QScienceSpinBox.h | 38 +- .../src/libs/qtconcurrent/multitask.h | 75 +- .../libs/qtconcurrent/qtconcurrent_global.h | 28 +- .../src/libs/qtconcurrent/runextensions.h | 142 +- .../src/libs/sdlgamepad/sdlgamepad.cpp | 178 +- .../src/libs/sdlgamepad/sdlgamepad.h | 79 +- .../src/libs/utils/abstractprocess.h | 60 +- .../src/libs/utils/abstractprocess_win.cpp | 57 +- .../src/libs/utils/basevalidatinglineedit.cpp | 54 +- .../src/libs/utils/basevalidatinglineedit.h | 38 +- .../src/libs/utils/cachedsvgitem.cpp | 61 +- .../src/libs/utils/cachedsvgitem.h | 15 +- .../src/libs/utils/checkablemessagebox.cpp | 66 +- .../src/libs/utils/checkablemessagebox.h | 75 +- .../utils/classnamevalidatinglineedit.cpp | 51 +- .../libs/utils/classnamevalidatinglineedit.h | 33 +- .../src/libs/utils/codegeneration.cpp | 58 +- .../src/libs/utils/codegeneration.h | 30 +- .../src/libs/utils/consoleprocess.cpp | 42 +- .../src/libs/utils/consoleprocess.h | 68 +- .../src/libs/utils/consoleprocess_unix.cpp | 96 +- .../src/libs/utils/consoleprocess_win.cpp | 69 +- .../src/libs/utils/coordinateconversions.cpp | 296 +- .../src/libs/utils/coordinateconversions.h | 5 +- .../src/libs/utils/detailsbutton.cpp | 28 +- .../src/libs/utils/detailsbutton.h | 31 +- .../src/libs/utils/detailswidget.cpp | 78 +- .../src/libs/utils/detailswidget.h | 34 +- .../src/libs/utils/fancylineedit.cpp | 71 +- .../src/libs/utils/fancylineedit.h | 40 +- .../src/libs/utils/fancymainwindow.cpp | 70 +- .../src/libs/utils/fancymainwindow.h | 45 +- .../libs/utils/filenamevalidatinglineedit.cpp | 63 +- .../libs/utils/filenamevalidatinglineedit.h | 36 +- .../src/libs/utils/filesearch.cpp | 151 +- .../openpilotgcs/src/libs/utils/filesearch.h | 42 +- .../src/libs/utils/filewizarddialog.cpp | 31 +- .../src/libs/utils/filewizarddialog.h | 35 +- .../src/libs/utils/filewizardpage.cpp | 41 +- .../src/libs/utils/filewizardpage.h | 36 +- .../src/libs/utils/homelocationutil.cpp | 69 +- .../src/libs/utils/homelocationutil.h | 16 +- .../src/libs/utils/iwelcomepage.cpp | 36 +- .../src/libs/utils/iwelcomepage.h | 38 +- .../src/libs/utils/linecolumnlabel.cpp | 41 +- .../src/libs/utils/linecolumnlabel.h | 36 +- .../openpilotgcs/src/libs/utils/listutils.h | 32 +- .../src/libs/utils/mylistwidget.cpp | 3 +- .../src/libs/utils/mylistwidget.h | 8 +- .../src/libs/utils/mytabbedstackwidget.cpp | 30 +- .../src/libs/utils/mytabbedstackwidget.h | 25 +- .../src/libs/utils/mytabwidget.cpp | 4 +- .../openpilotgcs/src/libs/utils/mytabwidget.h | 3 +- .../src/libs/utils/newclasswidget.cpp | 131 +- .../src/libs/utils/newclasswidget.h | 36 +- .../src/libs/utils/parameteraction.cpp | 43 +- .../src/libs/utils/parameteraction.h | 35 +- .../src/libs/utils/pathchooser.cpp | 94 +- .../openpilotgcs/src/libs/utils/pathchooser.h | 37 +- .../src/libs/utils/pathlisteditor.cpp | 98 +- .../src/libs/utils/pathlisteditor.h | 40 +- .../openpilotgcs/src/libs/utils/pathutils.cpp | 71 +- .../openpilotgcs/src/libs/utils/pathutils.h | 6 +- .../src/libs/utils/projectintropage.cpp | 44 +- .../src/libs/utils/projectintropage.h | 38 +- .../utils/projectnamevalidatinglineedit.cpp | 43 +- .../utils/projectnamevalidatinglineedit.h | 36 +- .../openpilotgcs/src/libs/utils/qtcassert.h | 34 +- .../src/libs/utils/qtcolorbutton.cpp | 99 +- .../src/libs/utils/qtcolorbutton.h | 36 +- .../src/libs/utils/qwineventnotifier_p.h | 11 +- .../src/libs/utils/reloadpromptutils.cpp | 47 +- .../src/libs/utils/reloadpromptutils.h | 30 +- .../src/libs/utils/savedaction.cpp | 233 +- .../openpilotgcs/src/libs/utils/savedaction.h | 43 +- .../src/libs/utils/settingsutils.cpp | 34 +- .../src/libs/utils/settingsutils.h | 30 +- .../openpilotgcs/src/libs/utils/styledbar.cpp | 34 +- .../openpilotgcs/src/libs/utils/styledbar.h | 36 +- .../src/libs/utils/stylehelper.cpp | 71 +- .../openpilotgcs/src/libs/utils/stylehelper.h | 47 +- .../src/libs/utils/submiteditorwidget.cpp | 150 +- .../src/libs/utils/submiteditorwidget.h | 40 +- .../src/libs/utils/submitfieldwidget.cpp | 139 +- .../src/libs/utils/submitfieldwidget.h | 38 +- .../src/libs/utils/svgimageprovider.cpp | 89 +- .../src/libs/utils/svgimageprovider.h | 13 +- .../src/libs/utils/synchronousprocess.cpp | 186 +- .../src/libs/utils/synchronousprocess.h | 47 +- .../libs/utils/treewidgetcolumnstretcher.cpp | 46 +- .../libs/utils/treewidgetcolumnstretcher.h | 41 +- .../src/libs/utils/uncommentselection.cpp | 70 +- .../src/libs/utils/uncommentselection.h | 30 +- .../src/libs/utils/utils_global.h | 28 +- .../src/libs/utils/welcomemodetreewidget.cpp | 58 +- .../src/libs/utils/welcomemodetreewidget.h | 38 +- .../openpilotgcs/src/libs/utils/winutils.cpp | 49 +- ground/openpilotgcs/src/libs/utils/winutils.h | 29 +- .../src/libs/utils/worldmagmodel.cpp | 1785 +- .../src/libs/utils/worldmagmodel.h | 212 +- .../openpilotgcs/src/libs/utils/xmlconfig.cpp | 154 +- .../openpilotgcs/src/libs/utils/xmlconfig.h | 8 +- .../antennatrack/antennatrackgadget.cpp | 93 +- .../plugins/antennatrack/antennatrackgadget.h | 12 +- .../antennatrackgadgetconfiguration.cpp | 65 +- .../antennatrackgadgetconfiguration.h | 114 +- .../antennatrackgadgetfactory.cpp | 22 +- .../antennatrack/antennatrackgadgetfactory.h | 5 +- .../antennatrackgadgetoptionspage.cpp | 273 +- .../antennatrackgadgetoptionspage.h | 7 +- .../antennatrack/antennatrackplugin.cpp | 21 +- .../plugins/antennatrack/antennatrackplugin.h | 5 +- .../antennatrack/antennatrackwidget.cpp | 150 +- .../plugins/antennatrack/antennatrackwidget.h | 43 +- .../src/plugins/antennatrack/gpsparser.cpp | 10 +- .../src/plugins/antennatrack/gpsparser.h | 28 +- .../plugins/antennatrack/telemetryparser.cpp | 27 +- .../plugins/antennatrack/telemetryparser.h | 14 +- .../plugins/config/alignment-calibration.cpp | 47 +- .../src/plugins/config/assertions.h | 53 +- .../src/plugins/config/calibration.h | 71 +- .../cfg_vehicletypes/configccpmwidget.cpp | 761 +- .../cfg_vehicletypes/configccpmwidget.h | 12 +- .../cfg_vehicletypes/configcustomwidget.cpp | 108 +- .../cfg_vehicletypes/configcustomwidget.h | 7 +- .../configfixedwingwidget.cpp | 235 +- .../cfg_vehicletypes/configfixedwingwidget.h | 4 +- .../configgroundvehiclewidget.cpp | 168 +- .../configgroundvehiclewidget.h | 4 +- .../configmultirotorwidget.cpp | 361 +- .../cfg_vehicletypes/configmultirotorwidget.h | 3 +- .../config/cfg_vehicletypes/vehicleconfig.cpp | 51 +- .../config/cfg_vehicletypes/vehicleconfig.h | 121 +- .../plugins/config/config_cc_hw_widget.cpp | 74 +- .../src/plugins/config/config_cc_hw_widget.h | 3 +- .../plugins/config/configautotunewidget.cpp | 62 +- .../src/plugins/config/configautotunewidget.h | 3 +- .../configcamerastabilizationwidget.cpp | 58 +- .../config/configcamerastabilizationwidget.h | 3 +- .../plugins/config/configccattitudewidget.cpp | 96 +- .../plugins/config/configccattitudewidget.h | 6 +- .../src/plugins/config/configgadget.cpp | 7 +- .../src/plugins/config/configgadget.h | 12 +- .../plugins/config/configgadgetfactory.cpp | 10 +- .../src/plugins/config/configgadgetfactory.h | 5 +- .../src/plugins/config/configgadgetwidget.cpp | 2 +- .../src/plugins/config/configgadgetwidget.h | 21 +- .../src/plugins/config/configinputwidget.cpp | 1229 +- .../src/plugins/config/configinputwidget.h | 209 +- .../src/plugins/config/configoutputwidget.cpp | 145 +- .../src/plugins/config/configoutputwidget.h | 43 +- .../plugins/config/configpipxtremewidget.cpp | 515 +- .../plugins/config/configpipxtremewidget.h | 61 +- .../src/plugins/config/configplugin.cpp | 116 +- .../src/plugins/config/configplugin.h | 12 +- .../src/plugins/config/configrevohwwidget.cpp | 216 +- .../src/plugins/config/configrevohwwidget.h | 6 +- .../src/plugins/config/configrevowidget.cpp | 23 +- .../config/configstabilizationwidget.h | 7 +- .../src/plugins/config/configtxpidwidget.cpp | 13 +- .../src/plugins/config/configtxpidwidget.h | 3 +- .../config/configvehicletypewidget.cpp | 124 +- .../plugins/config/configvehicletypewidget.h | 4 +- .../src/plugins/config/dblspindelegate.cpp | 36 +- .../src/plugins/config/dblspindelegate.h | 38 +- .../plugins/config/defaultattitudewidget.cpp | 5 +- .../plugins/config/defaultattitudewidget.h | 3 +- .../config/defaulthwsettingswidget.cpp | 5 +- .../plugins/config/defaulthwsettingswidget.h | 3 +- .../src/plugins/config/fancytabwidget.cpp | 113 +- .../src/plugins/config/fancytabwidget.h | 88 +- .../src/plugins/config/gyro-calibration.cpp | 124 +- .../src/plugins/config/inputchannelform.h | 7 +- .../src/plugins/config/legacy-calibration.cpp | 113 +- .../src/plugins/config/mixercurve.h | 25 +- .../src/plugins/config/outputchannelform.cpp | 179 +- .../src/plugins/config/outputchannelform.h | 3 +- .../src/plugins/config/twostep.cpp | 757 +- .../plugins/consolegadget/consolegadget.cpp | 9 +- .../src/plugins/consolegadget/consolegadget.h | 20 +- .../consolegadget/consolegadgetfactory.cpp | 19 +- .../consolegadget/consolegadgetfactory.h | 5 +- .../consolegadget/consolegadgetwidget.cpp | 7 +- .../consolegadget/consolegadgetwidget.h | 7 +- .../plugins/consolegadget/consoleplugin.cpp | 23 +- .../src/plugins/consolegadget/consoleplugin.h | 17 +- .../consolegadget/texteditloggerengine.cpp | 62 +- .../consolegadget/texteditloggerengine.h | 28 +- .../actionmanager/actioncontainer.cpp | 141 +- .../actionmanager/actioncontainer.h | 35 +- .../actionmanager/actioncontainer_p.h | 41 +- .../actionmanager/actionmanager.cpp | 117 +- .../coreplugin/actionmanager/actionmanager.h | 27 +- .../actionmanager/actionmanager_p.h | 45 +- .../coreplugin/actionmanager/command.cpp | 118 +- .../coreplugin/actionmanager/command.h | 41 +- .../coreplugin/actionmanager/command_p.h | 36 +- .../coreplugin/actionmanager/commandsfile.cpp | 51 +- .../coreplugin/actionmanager/commandsfile.h | 29 +- .../src/plugins/coreplugin/authorsdialog.cpp | 136 +- .../src/plugins/coreplugin/authorsdialog.h | 25 +- .../src/plugins/coreplugin/basemode.cpp | 31 +- .../src/plugins/coreplugin/basemode.h | 87 +- .../src/plugins/coreplugin/baseview.cpp | 27 +- .../src/plugins/coreplugin/baseview.h | 25 +- .../plugins/coreplugin/connectionmanager.cpp | 204 +- .../plugins/coreplugin/connectionmanager.h | 66 +- .../src/plugins/coreplugin/core_global.h | 20 +- .../src/plugins/coreplugin/coreconstants.h | 327 +- .../src/plugins/coreplugin/coreimpl.cpp | 39 +- .../src/plugins/coreplugin/coreimpl.h | 33 +- .../src/plugins/coreplugin/coreplugin.cpp | 29 +- .../src/plugins/coreplugin/coreplugin.h | 27 +- .../coreplugin/dialogs/ioptionspage.cpp | 44 +- .../plugins/coreplugin/dialogs/ioptionspage.h | 57 +- .../plugins/coreplugin/dialogs/iwizard.cpp | 67 +- .../src/plugins/coreplugin/dialogs/iwizard.h | 39 +- .../coreplugin/dialogs/settingsdialog.cpp | 124 +- .../coreplugin/dialogs/settingsdialog.h | 30 +- .../coreplugin/dialogs/shortcutsettings.cpp | 150 +- .../coreplugin/dialogs/shortcutsettings.h | 29 +- .../coreplugin/eventfilteringmainwindow.cpp | 23 +- .../coreplugin/eventfilteringmainwindow.h | 26 +- .../src/plugins/coreplugin/fancyactionbar.cpp | 66 +- .../src/plugins/coreplugin/fancyactionbar.h | 32 +- .../src/plugins/coreplugin/fancytabwidget.cpp | 105 +- .../src/plugins/coreplugin/fancytabwidget.h | 91 +- .../plugins/coreplugin/fileiconprovider.cpp | 83 +- .../src/plugins/coreplugin/fileiconprovider.h | 28 +- .../plugins/coreplugin/generalsettings.cpp | 70 +- .../src/plugins/coreplugin/generalsettings.h | 36 +- .../plugins/coreplugin/iconfigurableplugin.h | 9 +- .../src/plugins/coreplugin/iconnection.cpp | 1 - .../src/plugins/coreplugin/iconnection.h | 55 +- .../src/plugins/coreplugin/icontext.h | 46 +- .../src/plugins/coreplugin/icore.cpp | 95 +- .../src/plugins/coreplugin/icore.h | 61 +- .../src/plugins/coreplugin/icorelistener.h | 63 +- .../src/plugins/coreplugin/imode.h | 27 +- .../src/plugins/coreplugin/ioutputpane.h | 39 +- .../src/plugins/coreplugin/iuavgadget.cpp | 1 - .../src/plugins/coreplugin/iuavgadget.h | 55 +- .../coreplugin/iuavgadgetconfiguration.cpp | 23 +- .../coreplugin/iuavgadgetconfiguration.h | 72 +- .../plugins/coreplugin/iuavgadgetfactory.h | 60 +- .../src/plugins/coreplugin/iversioncontrol.h | 31 +- .../src/plugins/coreplugin/iview.h | 29 +- .../src/plugins/coreplugin/mainwindow.cpp | 381 +- .../src/plugins/coreplugin/mainwindow.h | 50 +- .../src/plugins/coreplugin/manhattanstyle.cpp | 896 +- .../src/plugins/coreplugin/manhattanstyle.h | 23 +- .../src/plugins/coreplugin/messagemanager.cpp | 33 +- .../src/plugins/coreplugin/messagemanager.h | 30 +- .../coreplugin/messageoutputwindow.cpp | 31 +- .../plugins/coreplugin/messageoutputwindow.h | 30 +- .../src/plugins/coreplugin/mimedatabase.cpp | 462 +- .../src/plugins/coreplugin/mimedatabase.h | 49 +- .../src/plugins/coreplugin/minisplitter.cpp | 33 +- .../src/plugins/coreplugin/minisplitter.h | 25 +- .../src/plugins/coreplugin/modemanager.cpp | 148 +- .../src/plugins/coreplugin/modemanager.h | 47 +- .../src/plugins/coreplugin/outputpane.h | 38 +- .../src/plugins/coreplugin/plugindialog.cpp | 40 +- .../src/plugins/coreplugin/plugindialog.h | 25 +- .../src/plugins/coreplugin/rightpane.cpp | 52 +- .../src/plugins/coreplugin/rightpane.h | 33 +- .../plugins/coreplugin/settingsdatabase.cpp | 64 +- .../src/plugins/coreplugin/settingsdatabase.h | 25 +- .../src/plugins/coreplugin/sidebar.cpp | 125 +- .../src/plugins/coreplugin/sidebar.h | 54 +- .../src/plugins/coreplugin/styleanimator.cpp | 103 +- .../src/plugins/coreplugin/styleanimator.h | 99 +- .../coreplugin/tabpositionindicator.cpp | 24 +- .../plugins/coreplugin/tabpositionindicator.h | 30 +- .../coreplugin/telemetrymonitorwidget.cpp | 60 +- .../coreplugin/telemetrymonitorwidget.h | 51 +- .../src/plugins/coreplugin/threadmanager.cpp | 26 +- .../src/plugins/coreplugin/threadmanager.h | 32 +- .../src/plugins/coreplugin/uavconfiginfo.cpp | 104 +- .../src/plugins/coreplugin/uavconfiginfo.h | 53 +- .../plugins/coreplugin/uavgadgetdecorator.cpp | 61 +- .../plugins/coreplugin/uavgadgetdecorator.h | 59 +- .../coreplugin/uavgadgetinstancemanager.cpp | 215 +- .../coreplugin/uavgadgetinstancemanager.h | 68 +- .../uavgadgetmanager/splitterorview.cpp | 115 +- .../uavgadgetmanager/splitterorview.h | 57 +- .../uavgadgetmanager/uavgadgetmanager.cpp | 129 +- .../uavgadgetmanager/uavgadgetmanager.h | 52 +- .../uavgadgetmanager/uavgadgetview.cpp | 70 +- .../uavgadgetmanager/uavgadgetview.h | 7 +- .../uavgadgetoptionspagedecorator.cpp | 24 +- .../uavgadgetoptionspagedecorator.h | 47 +- .../plugins/coreplugin/uniqueidmanager.cpp | 25 +- .../src/plugins/coreplugin/uniqueidmanager.h | 30 +- .../plugins/coreplugin/variablemanager.cpp | 23 +- .../src/plugins/coreplugin/variablemanager.h | 30 +- .../src/plugins/coreplugin/versiondialog.cpp | 80 +- .../src/plugins/coreplugin/versiondialog.h | 25 +- .../plugins/coreplugin/workspacesettings.cpp | 74 +- .../plugins/coreplugin/workspacesettings.h | 60 +- .../src/plugins/debuggadget/debugengine.cpp | 7 +- .../src/plugins/debuggadget/debugengine.h | 11 +- .../src/plugins/debuggadget/debuggadget.cpp | 9 +- .../src/plugins/debuggadget/debuggadget.h | 26 +- .../debuggadget/debuggadgetfactory.cpp | 19 +- .../plugins/debuggadget/debuggadgetfactory.h | 5 +- .../plugins/debuggadget/debuggadgetwidget.cpp | 17 +- .../plugins/debuggadget/debuggadgetwidget.h | 11 +- .../src/plugins/debuggadget/debugplugin.cpp | 23 +- .../src/plugins/debuggadget/debugplugin.h | 17 +- .../src/plugins/dial/dialgadget.cpp | 34 +- .../src/plugins/dial/dialgadget.h | 12 +- .../plugins/dial/dialgadgetconfiguration.cpp | 81 +- .../plugins/dial/dialgadgetconfiguration.h | 297 +- .../src/plugins/dial/dialgadgetfactory.cpp | 24 +- .../src/plugins/dial/dialgadgetfactory.h | 7 +- .../plugins/dial/dialgadgetoptionspage.cpp | 151 +- .../src/plugins/dial/dialgadgetoptionspage.h | 9 +- .../src/plugins/dial/dialgadgetwidget.cpp | 683 +- .../src/plugins/dial/dialgadgetwidget.h | 205 +- .../src/plugins/dial/dialplugin.cpp | 23 +- .../src/plugins/dial/dialplugin.h | 17 +- .../src/plugins/donothing/donothingplugin.cpp | 70 +- .../src/plugins/donothing/donothingplugin.h | 47 +- .../src/plugins/emptygadget/emptygadget.cpp | 9 +- .../src/plugins/emptygadget/emptygadget.h | 28 +- .../emptygadget/emptygadgetfactory.cpp | 19 +- .../plugins/emptygadget/emptygadgetfactory.h | 5 +- .../plugins/emptygadget/emptygadgetwidget.cpp | 7 +- .../plugins/emptygadget/emptygadgetwidget.h | 7 +- .../src/plugins/emptygadget/emptyplugin.cpp | 23 +- .../src/plugins/emptygadget/emptyplugin.h | 17 +- .../plugins/gcscontrol/gcscontrolgadget.cpp | 439 +- .../src/plugins/gcscontrol/gcscontrolgadget.h | 28 +- .../gcscontrolgadgetconfiguration.cpp | 83 +- .../gcscontrolgadgetconfiguration.h | 85 +- .../gcscontrol/gcscontrolgadgetfactory.cpp | 23 +- .../gcscontrol/gcscontrolgadgetfactory.h | 6 +- .../gcscontrolgadgetoptionspage.cpp | 357 +- .../gcscontrol/gcscontrolgadgetoptionspage.h | 59 +- .../gcscontrol/gcscontrolgadgetwidget.cpp | 94 +- .../gcscontrol/gcscontrolgadgetwidget.h | 7 +- .../plugins/gcscontrol/gcscontrolplugin.cpp | 36 +- .../src/plugins/gcscontrol/gcscontrolplugin.h | 16 +- .../gcscontrol/gcsonctrolgadgetwidget.h | 7 +- .../plugins/gcscontrol/joystickcontrol.cpp | 57 +- .../src/plugins/gcscontrol/joystickcontrol.h | 19 +- .../src/plugins/gpsdisplay/buffer.cpp | 131 +- .../src/plugins/gpsdisplay/buffer.h | 57 +- .../gpsdisplay/gpsconstellationwidget.cpp | 56 +- .../gpsdisplay/gpsconstellationwidget.h | 27 +- .../plugins/gpsdisplay/gpsdisplaygadget.cpp | 99 +- .../src/plugins/gpsdisplay/gpsdisplaygadget.h | 14 +- .../gpsdisplaygadgetconfiguration.cpp | 67 +- .../gpsdisplaygadgetconfiguration.h | 116 +- .../gpsdisplay/gpsdisplaygadgetfactory.cpp | 24 +- .../gpsdisplay/gpsdisplaygadgetfactory.h | 7 +- .../gpsdisplaygadgetoptionspage.cpp | 275 +- .../gpsdisplay/gpsdisplaygadgetoptionspage.h | 9 +- .../plugins/gpsdisplay/gpsdisplayplugin.cpp | 22 +- .../src/plugins/gpsdisplay/gpsdisplayplugin.h | 17 +- .../plugins/gpsdisplay/gpsdisplaywidget.cpp | 77 +- .../src/plugins/gpsdisplay/gpsdisplaywidget.h | 25 +- .../src/plugins/gpsdisplay/gpsparser.cpp | 10 +- .../src/plugins/gpsdisplay/gpsparser.h | 26 +- .../src/plugins/gpsdisplay/gpssnrwidget.cpp | 70 +- .../src/plugins/gpsdisplay/gpssnrwidget.h | 8 +- .../src/plugins/gpsdisplay/nmeaparser.cpp | 651 +- .../src/plugins/gpsdisplay/nmeaparser.h | 78 +- .../plugins/gpsdisplay/telemetryparser.cpp | 81 +- .../src/plugins/gpsdisplay/telemetryparser.h | 17 +- .../hitl/aerosimrc/src/aerosimrcdatastruct.h | 284 +- .../src/plugins/hitl/aerosimrc/src/enums.h | 27 +- .../src/plugins/hitl/aerosimrc/src/plugin.cpp | 256 +- .../src/plugins/hitl/aerosimrc/src/plugin.h | 9 +- .../hitl/aerosimrc/src/qdebughandler.cpp | 3 +- .../plugins/hitl/aerosimrc/src/settings.cpp | 48 +- .../src/plugins/hitl/aerosimrc/src/settings.h | 48 +- .../plugins/hitl/aerosimrc/src/udpconnect.cpp | 75 +- .../plugins/hitl/aerosimrc/src/udpconnect.h | 30 +- .../hitl/aerosimrc/src/udptestmain.cpp | 1 + .../hitl/aerosimrc/src/udptestwidget.cpp | 154 +- .../hitl/aerosimrc/src/udptestwidget.h | 9 +- .../src/plugins/hitl/aerosimrcsimulator.cpp | 125 +- .../src/plugins/hitl/aerosimrcsimulator.h | 14 +- .../src/plugins/hitl/fgsimulator.cpp | 225 +- .../src/plugins/hitl/fgsimulator.h | 39 +- .../src/plugins/hitl/hitlconfiguration.cpp | 156 +- .../src/plugins/hitl/hitlconfiguration.h | 46 +- .../src/plugins/hitl/hitlfactory.cpp | 27 +- .../src/plugins/hitl/hitlfactory.h | 9 +- .../src/plugins/hitl/hitlgadget.cpp | 18 +- .../src/plugins/hitl/hitlgadget.h | 22 +- .../src/plugins/hitl/hitlnoisegeneration.cpp | 54 +- .../src/plugins/hitl/hitlnoisegeneration.h | 19 +- .../src/plugins/hitl/hitloptionspage.cpp | 123 +- .../src/plugins/hitl/hitloptionspage.h | 43 +- .../src/plugins/hitl/hitlplugin.cpp | 33 +- .../src/plugins/hitl/hitlplugin.h | 45 +- .../src/plugins/hitl/hitlwidget.cpp | 180 +- .../src/plugins/hitl/hitlwidget.h | 32 +- .../src/plugins/hitl/il2simulator.cpp | 230 +- .../src/plugins/hitl/il2simulator.h | 34 +- .../src/plugins/hitl/isimulator.h | 85 +- .../src/plugins/hitl/simulator.cpp | 698 +- .../openpilotgcs/src/plugins/hitl/simulator.h | 258 +- .../src/plugins/hitl/xplanesimulator.cpp | 341 +- .../src/plugins/hitl/xplanesimulator.h | 79 +- .../importexport/importexportdialog.cpp | 3 +- .../plugins/importexport/importexportdialog.h | 5 +- .../importexport/importexportgadget.cpp | 19 +- .../plugins/importexport/importexportgadget.h | 8 +- .../importexport/importexportgadgetwidget.cpp | 96 +- .../importexport/importexportgadgetwidget.h | 18 +- .../importexport/importexportplugin.cpp | 14 +- .../plugins/importexport/importexportplugin.h | 6 +- .../ipconnection/ipconnection_internal.h | 9 +- .../ipconnectionconfiguration.cpp | 50 +- .../ipconnection/ipconnectionconfiguration.h | 54 +- .../ipconnection/ipconnectionoptionspage.cpp | 18 +- .../ipconnection/ipconnectionoptionspage.h | 30 +- .../ipconnection/ipconnectionplugin.cpp | 109 +- .../plugins/ipconnection/ipconnectionplugin.h | 39 +- .../plugins/lineardial/lineardialgadget.cpp | 26 +- .../src/plugins/lineardial/lineardialgadget.h | 12 +- .../lineardialgadgetconfiguration.cpp | 101 +- .../lineardialgadgetconfiguration.h | 149 +- .../lineardial/lineardialgadgetfactory.cpp | 24 +- .../lineardial/lineardialgadgetfactory.h | 7 +- .../lineardialgadgetoptionspage.cpp | 80 +- .../lineardial/lineardialgadgetoptionspage.h | 10 +- .../lineardial/lineardialgadgetwidget.cpp | 483 +- .../lineardial/lineardialgadgetwidget.h | 134 +- .../plugins/lineardial/lineardialplugin.cpp | 23 +- .../src/plugins/lineardial/lineardialplugin.h | 17 +- .../src/plugins/logging/logfile.cpp | 94 +- .../src/plugins/logging/logfile.h | 22 +- .../src/plugins/logging/logginggadget.cpp | 12 +- .../src/plugins/logging/logginggadget.h | 28 +- .../plugins/logging/logginggadgetfactory.cpp | 16 +- .../plugins/logging/logginggadgetfactory.h | 10 +- .../plugins/logging/logginggadgetwidget.cpp | 19 +- .../src/plugins/logging/logginggadgetwidget.h | 13 +- .../src/plugins/logging/loggingplugin.cpp | 227 +- .../src/plugins/logging/loggingplugin.h | 62 +- .../magicwaypoint/magicwaypointgadget.cpp | 9 +- .../magicwaypoint/magicwaypointgadget.h | 28 +- .../magicwaypointgadgetfactory.cpp | 17 +- .../magicwaypointgadgetfactory.h | 3 +- .../magicwaypointgadgetwidget.cpp | 59 +- .../magicwaypoint/magicwaypointgadgetwidget.h | 11 +- .../magicwaypoint/magicwaypointplugin.cpp | 26 +- .../magicwaypoint/magicwaypointplugin.h | 13 +- .../plugins/magicwaypoint/positionfield.cpp | 52 +- .../src/plugins/magicwaypoint/positionfield.h | 11 +- .../src/plugins/modelview/modelviewgadget.cpp | 15 +- .../src/plugins/modelview/modelviewgadget.h | 12 +- .../modelviewgadgetconfiguration.cpp | 44 +- .../modelview/modelviewgadgetconfiguration.h | 63 +- .../modelview/modelviewgadgetfactory.cpp | 24 +- .../modelview/modelviewgadgetfactory.h | 7 +- .../modelview/modelviewgadgetoptionspage.cpp | 26 +- .../modelview/modelviewgadgetoptionspage.h | 51 +- .../modelview/modelviewgadgetwidget.cpp | 312 +- .../plugins/modelview/modelviewgadgetwidget.h | 46 +- .../src/plugins/modelview/modelviewplugin.cpp | 23 +- .../src/plugins/modelview/modelviewplugin.h | 17 +- .../src/plugins/notify/notificationitem.cpp | 202 +- .../src/plugins/notify/notificationitem.h | 243 +- .../src/plugins/notify/notifyitemdelegate.cpp | 72 +- .../src/plugins/notify/notifyitemdelegate.h | 11 +- .../src/plugins/notify/notifylogging.h | 4 +- .../src/plugins/notify/notifyplugin.cpp | 312 +- .../src/plugins/notify/notifyplugin.h | 87 +- .../plugins/notify/notifypluginfactory.cpp | 24 +- .../src/plugins/notify/notifypluginfactory.h | 3 +- .../src/plugins/notify/notifyplugingadget.h | 13 +- .../notify/notifypluginoptionspage.cpp | 234 +- .../plugins/notify/notifypluginoptionspage.h | 104 +- .../src/plugins/notify/notifytablemodel.cpp | 119 +- .../src/plugins/notify/notifytablemodel.h | 24 +- .../src/plugins/opmap/flightdatamodel.cpp | 718 +- .../src/plugins/opmap/flightdatamodel.h | 57 +- .../src/plugins/opmap/homeeditor.cpp | 7 +- .../src/plugins/opmap/homeeditor.h | 11 +- .../src/plugins/opmap/modelmapproxy.cpp | 283 +- .../src/plugins/opmap/modelmapproxy.h | 25 +- .../src/plugins/opmap/modeluavoproxy.cpp | 290 +- .../src/plugins/opmap/modeluavoproxy.h | 11 +- .../opmap/opmap_edit_waypoint_dialog.cpp | 141 +- .../opmap/opmap_edit_waypoint_dialog.h | 23 +- .../plugins/opmap/opmap_statusbar_widget.cpp | 2 +- .../plugins/opmap/opmap_statusbar_widget.h | 7 +- .../opmap/opmap_zoom_slider_widget.cpp | 2 +- .../plugins/opmap/opmap_zoom_slider_widget.h | 7 +- .../src/plugins/opmap/opmapgadget.cpp | 21 +- .../src/plugins/opmap/opmapgadget.h | 12 +- .../opmap/opmapgadgetconfiguration.cpp | 145 +- .../plugins/opmap/opmapgadgetconfiguration.h | 154 +- .../src/plugins/opmap/opmapgadgetfactory.cpp | 20 +- .../src/plugins/opmap/opmapgadgetfactory.h | 13 +- .../plugins/opmap/opmapgadgetoptionspage.cpp | 57 +- .../plugins/opmap/opmapgadgetoptionspage.h | 11 +- .../src/plugins/opmap/opmapgadgetwidget.cpp | 1437 +- .../src/plugins/opmap/opmapgadgetwidget.h | 91 +- .../src/plugins/opmap/opmapplugin.cpp | 22 +- .../src/plugins/opmap/opmapplugin.h | 13 +- .../src/plugins/opmap/pathcompiler.cpp | 157 +- .../src/plugins/opmap/pathcompiler.h | 110 +- .../src/plugins/opmap/pathplanner.cpp | 35 +- .../src/plugins/opmap/pathplanner.h | 31 +- .../src/plugins/opmap/widgetdelegates.cpp | 120 +- .../src/plugins/opmap/widgetdelegates.h | 47 +- .../osgearthview/osgearthviewgadget.cpp | 35 +- .../plugins/osgearthview/osgearthviewgadget.h | 28 +- .../osgearthviewgadgetconfiguration.cpp | 27 +- .../osgearthviewgadgetconfiguration.h | 27 +- .../osgearthviewgadgetfactory.cpp | 38 +- .../osgearthview/osgearthviewgadgetfactory.h | 7 +- .../osgearthviewgadgetoptionspage.cpp | 37 +- .../osgearthviewgadgetoptionspage.h | 9 +- .../osgearthview/osgearthviewplugin.cpp | 39 +- .../plugins/osgearthview/osgearthviewplugin.h | 17 +- .../osgearthview/osgearthviewwidget.cpp | 35 +- .../plugins/osgearthview/osgearthviewwidget.h | 25 +- .../plugins/osgearthview/osgviewerwidget.cpp | 150 +- .../plugins/osgearthview/osgviewerwidget.h | 33 +- .../pathactioneditor/browseritemdelegate.cpp | 39 +- .../pathactioneditor/browseritemdelegate.h | 30 +- .../pathactioneditor/fieldtreeitem.cpp | 21 +- .../plugins/pathactioneditor/fieldtreeitem.h | 258 +- .../pathactioneditorgadget.cpp | 9 +- .../pathactioneditor/pathactioneditorgadget.h | 28 +- .../pathactioneditorgadgetfactory.cpp | 17 +- .../pathactioneditorgadgetfactory.h | 3 +- .../pathactioneditorgadgetwidget.cpp | 23 +- .../pathactioneditorgadgetwidget.h | 5 +- .../pathactioneditorplugin.cpp | 26 +- .../pathactioneditor/pathactioneditorplugin.h | 13 +- .../pathactioneditortreemodel.cpp | 313 +- .../pathactioneditortreemodel.h | 39 +- .../src/plugins/pathactioneditor/treeitem.cpp | 75 +- .../src/plugins/pathactioneditor/treeitem.h | 208 +- .../src/plugins/pfd/pfdgadget.cpp | 22 +- .../openpilotgcs/src/plugins/pfd/pfdgadget.h | 12 +- .../plugins/pfd/pfdgadgetconfiguration.cpp | 19 +- .../src/plugins/pfd/pfdgadgetconfiguration.h | 55 +- .../src/plugins/pfd/pfdgadgetfactory.cpp | 24 +- .../src/plugins/pfd/pfdgadgetfactory.h | 7 +- .../src/plugins/pfd/pfdgadgetoptionspage.cpp | 20 +- .../src/plugins/pfd/pfdgadgetoptionspage.h | 9 +- .../src/plugins/pfd/pfdgadgetwidget.cpp | 1294 +- .../src/plugins/pfd/pfdgadgetwidget.h | 192 +- .../src/plugins/pfd/pfdplugin.cpp | 23 +- .../openpilotgcs/src/plugins/pfd/pfdplugin.h | 17 +- .../src/plugins/pfdqml/osgearth.cpp | 100 +- .../src/plugins/pfdqml/osgearth.h | 58 +- .../src/plugins/pfdqml/pfdqmlgadget.cpp | 24 +- .../src/plugins/pfdqml/pfdqmlgadget.h | 10 +- .../pfdqml/pfdqmlgadgetconfiguration.cpp | 47 +- .../pfdqml/pfdqmlgadgetconfiguration.h | 99 +- .../plugins/pfdqml/pfdqmlgadgetfactory.cpp | 20 +- .../src/plugins/pfdqml/pfdqmlgadgetfactory.h | 3 +- .../pfdqml/pfdqmlgadgetoptionspage.cpp | 16 +- .../plugins/pfdqml/pfdqmlgadgetoptionspage.h | 7 +- .../src/plugins/pfdqml/pfdqmlgadgetwidget.cpp | 58 +- .../src/plugins/pfdqml/pfdqmlgadgetwidget.h | 40 +- .../src/plugins/pfdqml/pfdqmlplugin.cpp | 21 +- .../src/plugins/pfdqml/pfdqmlplugin.h | 15 +- .../src/plugins/powerlog/powerlogplugin.cpp | 245 +- .../src/plugins/powerlog/powerlogplugin.h | 66 +- .../src/plugins/qmlview/qmlviewgadget.cpp | 22 +- .../src/plugins/qmlview/qmlviewgadget.h | 12 +- .../qmlview/qmlviewgadgetconfiguration.cpp | 17 +- .../qmlview/qmlviewgadgetconfiguration.h | 35 +- .../plugins/qmlview/qmlviewgadgetfactory.cpp | 24 +- .../plugins/qmlview/qmlviewgadgetfactory.h | 7 +- .../qmlview/qmlviewgadgetoptionspage.cpp | 19 +- .../qmlview/qmlviewgadgetoptionspage.h | 9 +- .../plugins/qmlview/qmlviewgadgetwidget.cpp | 36 +- .../src/plugins/qmlview/qmlviewgadgetwidget.h | 22 +- .../src/plugins/qmlview/qmlviewplugin.cpp | 23 +- .../src/plugins/qmlview/qmlviewplugin.h | 17 +- .../src/plugins/rawhid/pjrc_rawhid.h | 85 +- .../src/plugins/rawhid/pjrc_rawhid_mac.cpp | 138 +- .../src/plugins/rawhid/pjrc_rawhid_unix.cpp | 453 +- .../src/plugins/rawhid/pjrc_rawhid_win.cpp | 665 +- .../src/plugins/rawhid/rawhid.cpp | 210 +- .../openpilotgcs/src/plugins/rawhid/rawhid.h | 23 +- .../src/plugins/rawhid/rawhid_const.h | 10 +- .../src/plugins/rawhid/rawhid_global.h | 20 +- .../src/plugins/rawhid/rawhidplugin.cpp | 89 +- .../src/plugins/rawhid/rawhidplugin.h | 44 +- .../src/plugins/rawhid/usbmonitor.h | 105 +- .../src/plugins/rawhid/usbmonitor_linux.cpp | 104 +- .../src/plugins/rawhid/usbmonitor_mac.cpp | 137 +- .../src/plugins/rawhid/usbmonitor_win.cpp | 320 +- .../src/plugins/rawhid/usbsignalfilter.cpp | 13 +- .../src/plugins/rawhid/usbsignalfilter.h | 3 +- .../src/plugins/scope/plotdata.cpp | 159 +- .../openpilotgcs/src/plugins/scope/plotdata.h | 95 +- .../src/plugins/scope/scope_global.h | 21 +- .../src/plugins/scope/scopegadget.cpp | 5 +- .../src/plugins/scope/scopegadget.h | 16 +- .../scope/scopegadgetconfiguration.cpp | 36 +- .../src/plugins/scope/scopegadgetfactory.cpp | 25 +- .../src/plugins/scope/scopegadgetfactory.h | 9 +- .../plugins/scope/scopegadgetoptionspage.cpp | 12 +- .../src/plugins/scope/scopegadgetwidget.h | 85 +- .../src/plugins/scope/scopeplugin.cpp | 3 +- .../src/plugins/scope/scopeplugin.h | 7 +- .../plugins/serialconnection/serial_global.h | 22 +- .../plugins/serialconnection/serialplugin.cpp | 173 +- .../plugins/serialconnection/serialplugin.h | 71 +- .../serialpluginconfiguration.cpp | 12 +- .../serialpluginconfiguration.h | 18 +- .../serialpluginoptionspage.cpp | 73 +- .../serialpluginoptionspage.h | 31 +- .../setupwizard/biascalibrationutil.cpp | 50 +- .../plugins/setupwizard/biascalibrationutil.h | 9 +- .../plugins/setupwizard/connectiondiagram.cpp | 149 +- .../plugins/setupwizard/connectiondiagram.h | 9 +- .../setupwizard/outputcalibrationutil.cpp | 32 +- .../setupwizard/outputcalibrationutil.h | 7 +- .../setupwizard/pages/abstractwizardpage.cpp | 4 +- .../setupwizard/pages/abstractwizardpage.h | 9 +- .../setupwizard/pages/autoupdatepage.cpp | 7 +- .../setupwizard/pages/autoupdatepage.h | 11 +- .../setupwizard/pages/biascalibrationpage.cpp | 20 +- .../setupwizard/pages/biascalibrationpage.h | 5 +- .../setupwizard/pages/controllerpage.cpp | 45 +- .../setupwizard/pages/controllerpage.h | 7 +- .../src/plugins/setupwizard/pages/endpage.cpp | 9 +- .../src/plugins/setupwizard/pages/endpage.h | 7 +- .../plugins/setupwizard/pages/fixedwingpage.h | 7 +- .../src/plugins/setupwizard/pages/helipage.h | 7 +- .../plugins/setupwizard/pages/inputpage.cpp | 97 +- .../src/plugins/setupwizard/pages/inputpage.h | 3 +- .../plugins/setupwizard/pages/multipage.cpp | 111 +- .../src/plugins/setupwizard/pages/multipage.h | 7 +- .../setupwizard/pages/notyetimplementedpage.h | 7 +- .../pages/outputcalibrationpage.cpp | 177 +- .../setupwizard/pages/outputcalibrationpage.h | 15 +- .../plugins/setupwizard/pages/outputpage.cpp | 7 +- .../plugins/setupwizard/pages/outputpage.h | 7 +- .../plugins/setupwizard/pages/rebootpage.cpp | 2 +- .../plugins/setupwizard/pages/rebootpage.h | 7 +- .../setupwizard/pages/revocalibrationpage.h | 7 +- .../plugins/setupwizard/pages/savepage.cpp | 16 +- .../src/plugins/setupwizard/pages/savepage.h | 3 +- .../plugins/setupwizard/pages/startpage.cpp | 2 +- .../src/plugins/setupwizard/pages/startpage.h | 7 +- .../plugins/setupwizard/pages/summarypage.cpp | 3 +- .../plugins/setupwizard/pages/summarypage.h | 3 +- .../plugins/setupwizard/pages/surfacepage.h | 7 +- .../plugins/setupwizard/pages/vehiclepage.cpp | 14 +- .../plugins/setupwizard/pages/vehiclepage.h | 5 +- .../src/plugins/setupwizard/setupwizard.cpp | 368 +- .../src/plugins/setupwizard/setupwizard.h | 124 +- .../plugins/setupwizard/setupwizardplugin.cpp | 24 +- .../plugins/setupwizard/setupwizardplugin.h | 19 +- .../vehicleconfigurationhelper.cpp | 1455 +- .../setupwizard/vehicleconfigurationhelper.h | 17 +- .../vehicleconfigurationsource.cpp | 3 +- .../setupwizard/vehicleconfigurationsource.h | 37 +- .../systemhealth/systemhealthgadget.cpp | 20 +- .../plugins/systemhealth/systemhealthgadget.h | 10 +- .../systemhealthgadgetconfiguration.cpp | 15 +- .../systemhealthgadgetconfiguration.h | 26 +- .../systemhealthgadgetfactory.cpp | 22 +- .../systemhealth/systemhealthgadgetfactory.h | 5 +- .../systemhealthgadgetoptionspage.cpp | 14 +- .../systemhealthgadgetoptionspage.h | 7 +- .../systemhealth/systemhealthgadgetwidget.cpp | 158 +- .../systemhealth/systemhealthgadgetwidget.h | 40 +- .../systemhealth/systemhealthplugin.cpp | 20 +- .../plugins/systemhealth/systemhealthplugin.h | 15 +- .../uavobjectbrowser/browseritemdelegate.cpp | 39 +- .../uavobjectbrowser/browseritemdelegate.h | 30 +- .../uavobjectbrowser/browserplugin.cpp | 21 +- .../plugins/uavobjectbrowser/browserplugin.h | 15 +- .../uavobjectbrowser/fieldtreeitem.cpp | 21 +- .../plugins/uavobjectbrowser/fieldtreeitem.h | 174 +- .../src/plugins/uavobjectbrowser/treeitem.cpp | 94 +- .../src/plugins/uavobjectbrowser/treeitem.h | 265 +- .../uavobjectbrowser/uavobjectbrowser.cpp | 21 +- .../uavobjectbrowser/uavobjectbrowser.h | 12 +- .../uavobjectbrowserconfiguration.cpp | 52 +- .../uavobjectbrowserconfiguration.h | 112 +- .../uavobjectbrowserfactory.cpp | 18 +- .../uavobjectbrowserfactory.h | 5 +- .../uavobjectbrowseroptionspage.cpp | 24 +- .../uavobjectbrowseroptionspage.h | 27 +- .../uavobjectbrowserwidget.cpp | 73 +- .../uavobjectbrowser/uavobjectbrowserwidget.h | 27 +- .../uavobjectbrowser/uavobjecttreemodel.cpp | 240 +- .../uavobjectbrowser/uavobjecttreemodel.h | 47 +- .../src/plugins/uavobjects/tests/main.cpp | 2 +- .../uavobjects/tests/uavobjectstest.cpp | 84 +- .../plugins/uavobjects/tests/uavobjectstest.h | 25 +- .../src/plugins/uavobjects/uavdataobject.cpp | 45 +- .../src/plugins/uavobjects/uavdataobject.h | 40 +- .../src/plugins/uavobjects/uavmetaobject.cpp | 48 +- .../src/plugins/uavobjects/uavmetaobject.h | 34 +- .../src/plugins/uavobjects/uavobject.cpp | 244 +- .../src/plugins/uavobjects/uavobject.h | 131 +- .../src/plugins/uavobjects/uavobjectfield.cpp | 663 +- .../src/plugins/uavobjects/uavobjectfield.h | 60 +- .../plugins/uavobjects/uavobjectmanager.cpp | 186 +- .../src/plugins/uavobjects/uavobjectmanager.h | 57 +- .../plugins/uavobjects/uavobjects_global.h | 20 +- .../src/plugins/uavobjects/uavobjectsinit.h | 24 +- .../plugins/uavobjects/uavobjectsplugin.cpp | 41 +- .../src/plugins/uavobjects/uavobjectsplugin.h | 27 +- .../wireshark/op-uavtalk/packet-op-uavtalk.c | 292 +- .../uavobjectutil/devicedescriptorstruct.h | 103 +- .../uavobjectutil/uavobjectutilmanager.cpp | 338 +- .../uavobjectutil/uavobjectutilmanager.h | 67 +- .../uavobjectutil/uavobjectutilplugin.cpp | 44 +- .../uavobjectutil/uavobjectutilplugin.h | 29 +- .../uavobjectwidgetutils/configtaskwidget.cpp | 4 +- .../uavobjectwidgetutils/configtaskwidget.h | 12 +- .../uavobjectwidgetutils/mixercurveline.cpp | 26 +- .../uavobjectwidgetutils/mixercurveline.h | 13 +- .../uavobjectwidgetutils/mixercurvepoint.cpp | 66 +- .../uavobjectwidgetutils/mixercurvewidget.cpp | 94 +- .../uavobjectwidgetutils/popupwidget.cpp | 21 +- .../uavobjectwidgetutils/smartsavebutton.cpp | 190 +- .../uavobjectwidgetutilsplugin.cpp | 38 +- .../uavobjectwidgetutilsplugin.h | 25 +- .../uavsettingsimportexport/importsummary.cpp | 107 +- .../uavsettingsimportexport/importsummary.h | 9 +- .../uavsettingsimportexport.cpp | 53 +- .../uavsettingsimportexport.h | 48 +- .../uavsettingsimportexportfactory.cpp | 105 +- .../uavsettingsimportexportfactory.h | 4 +- .../src/plugins/uavtalk/telemetry.cpp | 345 +- .../src/plugins/uavtalk/telemetry.h | 96 +- .../src/plugins/uavtalk/telemetrymanager.cpp | 15 +- .../src/plugins/uavtalk/telemetrymanager.h | 11 +- .../src/plugins/uavtalk/telemetrymonitor.cpp | 125 +- .../src/plugins/uavtalk/telemetrymonitor.h | 31 +- .../src/plugins/uavtalk/uavtalk.cpp | 784 +- .../src/plugins/uavtalk/uavtalk.h | 85 +- .../src/plugins/uavtalk/uavtalk_global.h | 20 +- .../src/plugins/uavtalk/uavtalkplugin.cpp | 47 +- .../src/plugins/uavtalk/uavtalkplugin.h | 29 +- .../src/plugins/uploader/SSP/port.cpp | 33 +- .../src/plugins/uploader/SSP/port.h | 53 +- .../src/plugins/uploader/SSP/qssp.cpp | 515 +- .../src/plugins/uploader/SSP/qssp.h | 104 +- .../src/plugins/uploader/SSP/qsspt.cpp | 57 +- .../src/plugins/uploader/SSP/qsspt.h | 11 +- .../src/plugins/uploader/delay.cpp | 1 - .../openpilotgcs/src/plugins/uploader/delay.h | 3 +- .../src/plugins/uploader/devicewidget.cpp | 193 +- .../src/plugins/uploader/devicewidget.h | 8 +- .../openpilotgcs/src/plugins/uploader/enums.h | 7 +- .../src/plugins/uploader/op_dfu.cpp | 1063 +- .../src/plugins/uploader/op_dfu.h | 356 +- .../plugins/uploader/runningdevicewidget.cpp | 19 +- .../plugins/uploader/runningdevicewidget.h | 10 +- .../src/plugins/uploader/uploadergadget.cpp | 14 +- .../src/plugins/uploader/uploadergadget.h | 11 +- .../uploader/uploadergadgetconfiguration.cpp | 54 +- .../uploader/uploadergadgetconfiguration.h | 83 +- .../uploader/uploadergadgetfactory.cpp | 21 +- .../plugins/uploader/uploadergadgetfactory.h | 7 +- .../uploader/uploadergadgetoptionspage.cpp | 21 +- .../uploader/uploadergadgetoptionspage.h | 5 +- .../plugins/uploader/uploadergadgetwidget.cpp | 433 +- .../plugins/uploader/uploadergadgetwidget.h | 47 +- .../src/plugins/uploader/uploaderplugin.cpp | 22 +- .../src/plugins/uploader/uploaderplugin.h | 15 +- .../src/plugins/welcome/welcome_global.h | 20 +- .../src/plugins/welcome/welcomemode.cpp | 33 +- .../src/plugins/welcome/welcomemode.h | 35 +- .../src/plugins/welcome/welcomeplugin.cpp | 29 +- .../src/plugins/welcome/welcomeplugin.h | 26 +- .../src/shared/namespace_global.h | 30 +- .../src/shared/qtlockedfile/qtlockedfile.cpp | 56 +- .../src/shared/qtlockedfile/qtlockedfile.h | 39 +- .../shared/qtlockedfile/qtlockedfile_unix.cpp | 76 +- .../shared/qtlockedfile/qtlockedfile_win.cpp | 116 +- .../qtsingleapplication/qtlocalpeer.cpp | 88 +- .../shared/qtsingleapplication/qtlocalpeer.h | 41 +- .../qtsingleapplication.cpp | 57 +- .../qtsingleapplication/qtsingleapplication.h | 53 +- .../qtsinglecoreapplication.cpp | 34 +- .../qtsinglecoreapplication.h | 39 +- .../scriptwrapper/interface_wrap_helpers.h | 49 +- .../src/shared/scriptwrapper/wrap_helpers.h | 221 +- .../flight/uavobjectgeneratorflight.cpp | 381 +- .../flight/uavobjectgeneratorflight.h | 9 +- .../generators/gcs/uavobjectgeneratorgcs.cpp | 331 +- .../generators/gcs/uavobjectgeneratorgcs.h | 11 +- .../generators/generator_common.cpp | 37 +- .../generators/generator_common.h | 4 +- .../generators/generator_io.cpp | 23 +- .../uavobjgenerator/generators/generator_io.h | 4 +- .../java/uavobjectgeneratorjava.cpp | 211 +- .../generators/java/uavobjectgeneratorjava.h | 15 +- .../matlab/uavobjectgeneratormatlab.cpp | 130 +- .../matlab/uavobjectgeneratormatlab.h | 10 +- .../python/uavobjectgeneratorpython.cpp | 39 +- .../python/uavobjectgeneratorpython.h | 7 +- .../wireshark/uavobjectgeneratorwireshark.cpp | 319 +- .../wireshark/uavobjectgeneratorwireshark.h | 9 +- ground/uavobjgenerator/main.cpp | 147 +- ground/uavobjgenerator/uavobjectparser.cpp | 355 +- ground/uavobjgenerator/uavobjectparser.h | 65 +- 1513 files changed, 122130 insertions(+), 119268 deletions(-) diff --git a/flight/libraries/CoordinateConversions.c b/flight/libraries/CoordinateConversions.c index 1bac5f502..5d286aac1 100644 --- a/flight/libraries/CoordinateConversions.c +++ b/flight/libraries/CoordinateConversions.c @@ -37,242 +37,239 @@ // ****** convert Lat,Lon,Alt to ECEF ************ void LLA2ECEF(float LLA[3], float ECEF[3]) { - const float a = 6378137.0f; // Equatorial Radius - const float e = 8.1819190842622e-2f; // Eccentricity - float sinLat, sinLon, cosLat, cosLon; - float N; + const float a = 6378137.0f; // Equatorial Radius + const float e = 8.1819190842622e-2f; // Eccentricity + float sinLat, sinLon, cosLat, cosLon; + float N; - sinLat = sinf(DEG2RAD(LLA[0])); - sinLon = sinf(DEG2RAD(LLA[1])); - cosLat = cosf(DEG2RAD(LLA[0])); - cosLon = cosf(DEG2RAD(LLA[1])); + sinLat = sinf(DEG2RAD(LLA[0])); + sinLon = sinf(DEG2RAD(LLA[1])); + cosLat = cosf(DEG2RAD(LLA[0])); + cosLon = cosf(DEG2RAD(LLA[1])); - N = a / sqrtf(1.0f - e * e * sinLat * sinLat); //prime vertical radius of curvature + N = a / sqrtf(1.0f - e * e * sinLat * sinLat); // prime vertical radius of curvature - ECEF[0] = (N + LLA[2]) * cosLat * cosLon; - ECEF[1] = (N + LLA[2]) * cosLat * sinLon; - ECEF[2] = ((1 - e * e) * N + LLA[2]) * sinLat; + ECEF[0] = (N + LLA[2]) * cosLat * cosLon; + ECEF[1] = (N + LLA[2]) * cosLat * sinLon; + ECEF[2] = ((1 - e * e) * N + LLA[2]) * sinLat; } // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* uint16_t ECEF2LLA(float ECEF[3], float LLA[3]) { - /** - * LLA parameter is used to prime the iteration. - * A position within 1 meter of the specified LLA - * will be calculated within at most 3 iterations. - * If unknown: Call with any valid LLA coordinate - * will compute within at most 5 iterations. - * Suggestion: [0,0,0] - **/ + /** + * LLA parameter is used to prime the iteration. + * A position within 1 meter of the specified LLA + * will be calculated within at most 3 iterations. + * If unknown: Call with any valid LLA coordinate + * will compute within at most 5 iterations. + * Suggestion: [0,0,0] + **/ - const float a = 6378137.0f; // Equatorial Radius - const float e = 8.1819190842622e-2f; // Eccentricity - float x = ECEF[0], y = ECEF[1], z = ECEF[2]; - float Lat, N, NplusH, delta, esLat; - uint16_t iter; -#define MAX_ITER 10 // should not take more than 5 for valid coordinates -#define ACCURACY 1.0e-11f // used to be e-14, but we don't need sub micrometer exact calculations + const float a = 6378137.0f; // Equatorial Radius + const float e = 8.1819190842622e-2f; // Eccentricity + float x = ECEF[0], y = ECEF[1], z = ECEF[2]; + float Lat, N, NplusH, delta, esLat; + uint16_t iter; - LLA[1] = RAD2DEG(atan2f(y, x)); - Lat = DEG2RAD(LLA[0]); - esLat = e * sinf(Lat); - N = a / sqrtf(1 - esLat * esLat); - NplusH = N + LLA[2]; - delta = 1; - iter = 0; +#define MAX_ITER 10 // should not take more than 5 for valid coordinates +#define ACCURACY 1.0e-11f // used to be e-14, but we don't need sub micrometer exact calculations - while (((delta > ACCURACY) || (delta < -ACCURACY)) - && (iter < MAX_ITER)) { - delta = Lat - atanf(z / (sqrtf(x * x + y * y) * (1 - (N * e * e / NplusH)))); - Lat = Lat - delta; - esLat = e * sinf(Lat); - N = a / sqrtf(1 - esLat * esLat); - NplusH = sqrtf(x * x + y * y) / cosf(Lat); - iter += 1; - } + LLA[1] = RAD2DEG(atan2f(y, x)); + Lat = DEG2RAD(LLA[0]); + esLat = e * sinf(Lat); + N = a / sqrtf(1 - esLat * esLat); + NplusH = N + LLA[2]; + delta = 1; + iter = 0; - LLA[0] = RAD2DEG(Lat); - LLA[2] = NplusH - N; + while (((delta > ACCURACY) || (delta < -ACCURACY)) + && (iter < MAX_ITER)) { + delta = Lat - atanf(z / (sqrtf(x * x + y * y) * (1 - (N * e * e / NplusH)))); + Lat = Lat - delta; + esLat = e * sinf(Lat); + N = a / sqrtf(1 - esLat * esLat); + NplusH = sqrtf(x * x + y * y) / cosf(Lat); + iter += 1; + } - return (iter < MAX_ITER); + LLA[0] = RAD2DEG(Lat); + LLA[2] = NplusH - N; + + return iter < MAX_ITER; } // ****** find ECEF to NED rotation matrix ******** void RneFromLLA(float LLA[3], float Rne[3][3]) { - float sinLat, sinLon, cosLat, cosLon; + float sinLat, sinLon, cosLat, cosLon; - sinLat = (float)sinf(DEG2RAD(LLA[0])); - sinLon = (float)sinf(DEG2RAD(LLA[1])); - cosLat = (float)cosf(DEG2RAD(LLA[0])); - cosLon = (float)cosf(DEG2RAD(LLA[1])); + sinLat = (float)sinf(DEG2RAD(LLA[0])); + sinLon = (float)sinf(DEG2RAD(LLA[1])); + cosLat = (float)cosf(DEG2RAD(LLA[0])); + cosLon = (float)cosf(DEG2RAD(LLA[1])); - Rne[0][0] = -sinLat * cosLon; - Rne[0][1] = -sinLat * sinLon; - Rne[0][2] = cosLat; - Rne[1][0] = -sinLon; - Rne[1][1] = cosLon; - Rne[1][2] = 0; - Rne[2][0] = -cosLat * cosLon; - Rne[2][1] = -cosLat * sinLon; - Rne[2][2] = -sinLat; + Rne[0][0] = -sinLat * cosLon; + Rne[0][1] = -sinLat * sinLon; + Rne[0][2] = cosLat; + Rne[1][0] = -sinLon; + Rne[1][1] = cosLon; + Rne[1][2] = 0; + Rne[2][0] = -cosLat * cosLon; + Rne[2][1] = -cosLat * sinLon; + Rne[2][2] = -sinLat; } // ****** find roll, pitch, yaw from quaternion ******** void Quaternion2RPY(const float q[4], float rpy[3]) { - float R13, R11, R12, R23, R33; - float q0s = q[0] * q[0]; - float q1s = q[1] * q[1]; - float q2s = q[2] * q[2]; - float q3s = q[3] * q[3]; + float R13, R11, R12, R23, R33; + float q0s = q[0] * q[0]; + float q1s = q[1] * q[1]; + float q2s = q[2] * q[2]; + float q3s = q[3] * q[3]; - R13 = 2.0f * (q[1] * q[3] - q[0] * q[2]); - R11 = q0s + q1s - q2s - q3s; - R12 = 2.0f * (q[1] * q[2] + q[0] * q[3]); - R23 = 2.0f * (q[2] * q[3] + q[0] * q[1]); - R33 = q0s - q1s - q2s + q3s; + R13 = 2.0f * (q[1] * q[3] - q[0] * q[2]); + R11 = q0s + q1s - q2s - q3s; + R12 = 2.0f * (q[1] * q[2] + q[0] * q[3]); + R23 = 2.0f * (q[2] * q[3] + q[0] * q[1]); + R33 = q0s - q1s - q2s + q3s; - rpy[1] = RAD2DEG(asinf(-R13)); // pitch always between -pi/2 to pi/2 - rpy[2] = RAD2DEG(atan2f(R12, R11)); - rpy[0] = RAD2DEG(atan2f(R23, R33)); + rpy[1] = RAD2DEG(asinf(-R13)); // pitch always between -pi/2 to pi/2 + rpy[2] = RAD2DEG(atan2f(R12, R11)); + rpy[0] = RAD2DEG(atan2f(R23, R33)); - //TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2 + // TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2 } // ****** find quaternion from roll, pitch, yaw ******** void RPY2Quaternion(const float rpy[3], float q[4]) { - float phi, theta, psi; - float cphi, sphi, ctheta, stheta, cpsi, spsi; + float phi, theta, psi; + float cphi, sphi, ctheta, stheta, cpsi, spsi; - phi = DEG2RAD(rpy[0] / 2); - theta = DEG2RAD(rpy[1] / 2); - psi = DEG2RAD(rpy[2] / 2); - cphi = cosf(phi); - sphi = sinf(phi); - ctheta = cosf(theta); - stheta = sinf(theta); - cpsi = cosf(psi); - spsi = sinf(psi); + phi = DEG2RAD(rpy[0] / 2); + theta = DEG2RAD(rpy[1] / 2); + psi = DEG2RAD(rpy[2] / 2); + cphi = cosf(phi); + sphi = sinf(phi); + ctheta = cosf(theta); + stheta = sinf(theta); + cpsi = cosf(psi); + spsi = sinf(psi); - q[0] = cphi * ctheta * cpsi + sphi * stheta * spsi; - q[1] = sphi * ctheta * cpsi - cphi * stheta * spsi; - q[2] = cphi * stheta * cpsi + sphi * ctheta * spsi; - q[3] = cphi * ctheta * spsi - sphi * stheta * cpsi; + q[0] = cphi * ctheta * cpsi + sphi * stheta * spsi; + q[1] = sphi * ctheta * cpsi - cphi * stheta * spsi; + q[2] = cphi * stheta * cpsi + sphi * ctheta * spsi; + q[3] = cphi * ctheta * spsi - sphi * stheta * cpsi; - if (q[0] < 0) { // q0 always positive for uniqueness - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; - } + if (q[0] < 0) { // q0 always positive for uniqueness + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } } -//** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** void Quaternion2R(float q[4], float Rbe[3][3]) { + float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3]; - float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3]; - - Rbe[0][0] = q0s + q1s - q2s - q3s; - Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]); - Rbe[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]); - Rbe[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]); - Rbe[1][1] = q0s - q1s + q2s - q3s; - Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]); - Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]); - Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]); - Rbe[2][2] = q0s - q1s - q2s + q3s; + Rbe[0][0] = q0s + q1s - q2s - q3s; + Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]); + Rbe[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]); + Rbe[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]); + Rbe[1][1] = q0s - q1s + q2s - q3s; + Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]); + Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]); + Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]); + Rbe[2][2] = q0s - q1s - q2s + q3s; } // ****** Express LLA in a local NED Base Frame ******** void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3]) { - float ECEF[3]; - float diff[3]; + float ECEF[3]; + float diff[3]; - LLA2ECEF(LLA, ECEF); + LLA2ECEF(LLA, ECEF); - diff[0] = (float)(ECEF[0] - BaseECEF[0]); - diff[1] = (float)(ECEF[1] - BaseECEF[1]); - diff[2] = (float)(ECEF[2] - BaseECEF[2]); + diff[0] = (float)(ECEF[0] - BaseECEF[0]); + diff[1] = (float)(ECEF[1] - BaseECEF[1]); + diff[2] = (float)(ECEF[2] - BaseECEF[2]); - NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; - NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; - NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; + NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; + NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; + NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; } // ****** Express ECEF in a local NED Base Frame ******** void ECEF2Base(float ECEF[3], float BaseECEF[3], float Rne[3][3], float NED[3]) { - float diff[3]; + float diff[3]; - diff[0] = (float)(ECEF[0] - BaseECEF[0]); - diff[1] = (float)(ECEF[1] - BaseECEF[1]); - diff[2] = (float)(ECEF[2] - BaseECEF[2]); + diff[0] = (float)(ECEF[0] - BaseECEF[0]); + diff[1] = (float)(ECEF[1] - BaseECEF[1]); + diff[2] = (float)(ECEF[2] - BaseECEF[2]); - NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; - NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; - NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; + NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; + NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; + NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; } // ****** convert Rotation Matrix to Quaternion ******** // ****** if R converts from e to b, q is rotation from e to b **** void R2Quaternion(float R[3][3], float q[4]) { - float m[4], mag; - uint8_t index,i; + float m[4], mag; + uint8_t index, i; - m[0] = 1 + R[0][0] + R[1][1] + R[2][2]; - m[1] = 1 + R[0][0] - R[1][1] - R[2][2]; - m[2] = 1 - R[0][0] + R[1][1] - R[2][2]; - m[3] = 1 - R[0][0] - R[1][1] + R[2][2]; + m[0] = 1 + R[0][0] + R[1][1] + R[2][2]; + m[1] = 1 + R[0][0] - R[1][1] - R[2][2]; + m[2] = 1 - R[0][0] + R[1][1] - R[2][2]; + m[3] = 1 - R[0][0] - R[1][1] + R[2][2]; - // find maximum divisor - index = 0; - mag = m[0]; - for (i=1;i<4;i++){ - if (m[i] > mag){ - mag = m[i]; - index = i; - } - } - mag = 2*sqrtf(mag); + // find maximum divisor + index = 0; + mag = m[0]; + for (i = 1; i < 4; i++) { + if (m[i] > mag) { + mag = m[i]; + index = i; + } + } + mag = 2 * sqrtf(mag); - if (index == 0) { - q[0] = mag/4; - q[1] = (R[1][2]-R[2][1])/mag; - q[2] = (R[2][0]-R[0][2])/mag; - q[3] = (R[0][1]-R[1][0])/mag; - } - else if (index == 1) { - q[1] = mag/4; - q[0] = (R[1][2]-R[2][1])/mag; - q[2] = (R[0][1]+R[1][0])/mag; - q[3] = (R[0][2]+R[2][0])/mag; - } - else if (index == 2) { - q[2] = mag/4; - q[0] = (R[2][0]-R[0][2])/mag; - q[1] = (R[0][1]+R[1][0])/mag; - q[3] = (R[1][2]+R[2][1])/mag; - } - else { - q[3] = mag/4; - q[0] = (R[0][1]-R[1][0])/mag; - q[1] = (R[0][2]+R[2][0])/mag; - q[2] = (R[1][2]+R[2][1])/mag; - } + if (index == 0) { + q[0] = mag / 4; + q[1] = (R[1][2] - R[2][1]) / mag; + q[2] = (R[2][0] - R[0][2]) / mag; + q[3] = (R[0][1] - R[1][0]) / mag; + } else if (index == 1) { + q[1] = mag / 4; + q[0] = (R[1][2] - R[2][1]) / mag; + q[2] = (R[0][1] + R[1][0]) / mag; + q[3] = (R[0][2] + R[2][0]) / mag; + } else if (index == 2) { + q[2] = mag / 4; + q[0] = (R[2][0] - R[0][2]) / mag; + q[1] = (R[0][1] + R[1][0]) / mag; + q[3] = (R[1][2] + R[2][1]) / mag; + } else { + q[3] = mag / 4; + q[0] = (R[0][1] - R[1][0]) / mag; + q[1] = (R[0][2] + R[2][0]) / mag; + q[2] = (R[1][2] + R[2][1]) / mag; + } - // q0 positive, i.e. angle between pi and -pi - if (q[0] < 0){ - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; - } + // q0 positive, i.e. angle between pi and -pi + if (q[0] < 0) { + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } } // ****** Rotation Matrix from Two Vector Directions ******** @@ -280,111 +277,122 @@ void R2Quaternion(float R[3][3], float q[4]) // ****** solution is approximate if can't be exact *** uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[3], const float v2e[3], float Rbe[3][3]) { - float Rib[3][3], Rie[3][3]; - float mag; - uint8_t i,j,k; + float Rib[3][3], Rie[3][3]; + float mag; + uint8_t i, j, k; - // identity rotation in case of error - for (i=0;i<3;i++){ - for (j=0;j<3;j++) - Rbe[i][j]=0; - Rbe[i][i]=1; - } + // identity rotation in case of error + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + Rbe[i][j] = 0; + } + Rbe[i][i] = 1; + } - // The first rows of rot matrices chosen in direction of v1 - mag = VectorMagnitude(v1b); - if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) - return (-1); - for (i=0;i<3;i++) - Rib[0][i]=v1b[i]/mag; + // The first rows of rot matrices chosen in direction of v1 + mag = VectorMagnitude(v1b); + if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) { + return -1; + } + for (i = 0; i < 3; i++) { + Rib[0][i] = v1b[i] / mag; + } - mag = VectorMagnitude(v1e); - if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) - return (-1); - for (i=0;i<3;i++) - Rie[0][i]=v1e[i]/mag; + mag = VectorMagnitude(v1e); + if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) { + return -1; + } + for (i = 0; i < 3; i++) { + Rie[0][i] = v1e[i] / mag; + } - // The second rows of rot matrices chosen in direction of v1xv2 - CrossProduct(v1b,v2b,&Rib[1][0]); - mag = VectorMagnitude(&Rib[1][0]); - if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) - return (-1); - for (i=0;i<3;i++) - Rib[1][i]=Rib[1][i]/mag; + // The second rows of rot matrices chosen in direction of v1xv2 + CrossProduct(v1b, v2b, &Rib[1][0]); + mag = VectorMagnitude(&Rib[1][0]); + if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) { + return -1; + } + for (i = 0; i < 3; i++) { + Rib[1][i] = Rib[1][i] / mag; + } - CrossProduct(v1e,v2e,&Rie[1][0]); - mag = VectorMagnitude(&Rie[1][0]); - if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) - return (-1); - for (i=0;i<3;i++) - Rie[1][i]=Rie[1][i]/mag; + CrossProduct(v1e, v2e, &Rie[1][0]); + mag = VectorMagnitude(&Rie[1][0]); + if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) { + return -1; + } + for (i = 0; i < 3; i++) { + Rie[1][i] = Rie[1][i] / mag; + } - // The third rows of rot matrices are XxY (Row1xRow2) - CrossProduct(&Rib[0][0],&Rib[1][0],&Rib[2][0]); - CrossProduct(&Rie[0][0],&Rie[1][0],&Rie[2][0]); + // The third rows of rot matrices are XxY (Row1xRow2) + CrossProduct(&Rib[0][0], &Rib[1][0], &Rib[2][0]); + CrossProduct(&Rie[0][0], &Rie[1][0], &Rie[2][0]); - // Rbe = Rbi*Rie = Rib'*Rie - for (i=0;i<3;i++) - for(j=0;j<3;j++){ - Rbe[i][j]=0; - for(k=0;k<3;k++) - Rbe[i][j] += Rib[k][i]*Rie[k][j]; - } + // Rbe = Rbi*Rie = Rib'*Rie + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + Rbe[i][j] = 0; + for (k = 0; k < 3; k++) { + Rbe[i][j] += Rib[k][i] * Rie[k][j]; + } + } + } - return 1; + return 1; } void Rv2Rot(float Rv[3], float R[3][3]) { - // Compute rotation matrix from a rotation vector - // To save .text space, uses Quaternion2R() - float q[4]; + // Compute rotation matrix from a rotation vector + // To save .text space, uses Quaternion2R() + float q[4]; - float angle = VectorMagnitude(Rv); - if (angle <= 0.00048828125f) { - // angle < sqrt(2*machine_epsilon(float)), so flush cos(x) to 1.0f - q[0] = 1.0f; + float angle = VectorMagnitude(Rv); + + if (angle <= 0.00048828125f) { + // angle < sqrt(2*machine_epsilon(float)), so flush cos(x) to 1.0f + q[0] = 1.0f; // and flush sin(x/2)/x to 0.5 - q[1] = 0.5f*Rv[0]; - q[2] = 0.5f*Rv[1]; - q[3] = 0.5f*Rv[2]; - // This prevents division by zero, while retaining full accuracy - } - else { - q[0] = cosf(angle*0.5f); - float scale = sinf(angle*0.5f) / angle; - q[1] = scale*Rv[0]; - q[2] = scale*Rv[1]; - q[3] = scale*Rv[2]; - } + q[1] = 0.5f * Rv[0]; + q[2] = 0.5f * Rv[1]; + q[3] = 0.5f * Rv[2]; + // This prevents division by zero, while retaining full accuracy + } else { + q[0] = cosf(angle * 0.5f); + float scale = sinf(angle * 0.5f) / angle; + q[1] = scale * Rv[0]; + q[2] = scale * Rv[1]; + q[3] = scale * Rv[2]; + } - Quaternion2R(q, R); + Quaternion2R(q, R); } // ****** Vector Cross Product ******** void CrossProduct(const float v1[3], const float v2[3], float result[3]) { - result[0] = v1[1]*v2[2] - v2[1]*v1[2]; - result[1] = v2[0]*v1[2] - v1[0]*v2[2]; - result[2] = v1[0]*v2[1] - v2[0]*v1[1]; + result[0] = v1[1] * v2[2] - v2[1] * v1[2]; + result[1] = v2[0] * v1[2] - v1[0] * v2[2]; + result[2] = v1[0] * v2[1] - v2[0] * v1[1]; } // ****** Vector Magnitude ******** float VectorMagnitude(const float v[3]) { - return(sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2])); + return sqrtf(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); } /** - * @brief Compute the inverse of a quaternion + * @brief Compute the inverse of a quaternion * @param [in][out] q The matrix to invert */ -void quat_inverse(float q[4]) +void quat_inverse(float q[4]) { - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; } /** @@ -392,12 +400,12 @@ void quat_inverse(float q[4]) * @param[in] q quaternion in * @param[out] qnew quaternion to copy to */ -void quat_copy(const float q[4], float qnew[4]) +void quat_copy(const float q[4], float qnew[4]) { - qnew[0] = q[0]; - qnew[1] = q[1]; - qnew[2] = q[2]; - qnew[3] = q[3]; + qnew[0] = q[0]; + qnew[1] = q[1]; + qnew[2] = q[2]; + qnew[3] = q[3]; } /** @@ -406,12 +414,12 @@ void quat_copy(const float q[4], float qnew[4]) * @param[in] q2 Second quaternion * @param[out] qout Output quaternion */ -void quat_mult(const float q1[4], const float q2[4], float qout[4]) +void quat_mult(const float q1[4], const float q2[4], float qout[4]) { - qout[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3]; - qout[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2]; - qout[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1]; - qout[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0]; + qout[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; + qout[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; + qout[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]; + qout[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]; } /** @@ -420,9 +428,9 @@ void quat_mult(const float q1[4], const float q2[4], float qout[4]) * @param[in] vec the source vector * @param[out] vec_out the output vector */ -void rot_mult(float R[3][3], const float vec[3], float vec_out[3]) +void rot_mult(float R[3][3], const float vec[3], float vec_out[3]) { - vec_out[0] = R[0][0] * vec[0] + R[0][1] * vec[1] + R[0][2] * vec[2]; - vec_out[1] = R[1][0] * vec[0] + R[1][1] * vec[1] + R[1][2] * vec[2]; - vec_out[2] = R[2][0] * vec[0] + R[2][1] * vec[1] + R[2][2] * vec[2]; + vec_out[0] = R[0][0] * vec[0] + R[0][1] * vec[1] + R[0][2] * vec[2]; + vec_out[1] = R[1][0] * vec[0] + R[1][1] * vec[1] + R[1][2] * vec[2]; + vec_out[2] = R[2][0] * vec[0] + R[2][1] * vec[1] + R[2][2] * vec[2]; } diff --git a/flight/libraries/WorldMagModel.c b/flight/libraries/WorldMagModel.c index af50c8a19..fe55c3b27 100644 --- a/flight/libraries/WorldMagModel.c +++ b/flight/libraries/WorldMagModel.c @@ -53,108 +53,109 @@ #include "WMMInternal.h" #define MALLOC(x) pvPortMalloc(x) -#define FREE(x) vPortFree(x) -//#define MALLOC(x) malloc(x) -//#define FREE(x) free(x) +#define FREE(x) vPortFree(x) +// #define MALLOC(x) malloc(x) +// #define FREE(x) free(x) // http://reviews.openpilot.org/cru/OPReview-436#c6476 : // first column not used but it will be optimized out by compiler -static const float CoeffFile[91][6] = { {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, - {1.0f, 0.0f, -29496.6f, 0.0f, 11.6f, 0.0f}, - {1.0f, 1.0f, -1586.3f, 4944.4f, 16.5f, -25.9f}, - {2.0f, 0.0f, -2396.6f, 0.0f, -12.1f, 0.0f}, - {2.0f, 1.0f, 3026.1f, -2707.7f, -4.4f, -22.5f}, - {2.0f, 2.0f, 1668.6f, -576.1f, 1.9f, -11.8f}, - {3.0f, 0.0f, 1340.1f, 0.0f, 0.4f, 0.0f}, - {3.0f, 1.0f, -2326.2f, -160.2f, -4.1f, 7.3f}, - {3.0f, 2.0f, 1231.9f, 251.9f, -2.9f, -3.9f}, - {3.0f, 3.0f, 634.0f, -536.6f, -7.7f, -2.6f}, - {4.0f, 0.0f, 912.6f, 0.0f, -1.8f, 0.0f}, - {4.0f, 1.0f, 808.9f, 286.4f, 2.3f, 1.1f}, - {4.0f, 2.0f, 166.7f, -211.2f, -8.7f, 2.7f}, - {4.0f, 3.0f, -357.1f, 164.3f, 4.6f, 3.9f}, - {4.0f, 4.0f, 89.4f, -309.1f, -2.1f, -0.8f}, - {5.0f, 0.0f, -230.9f, 0.0f, -1.0f, 0.0f}, - {5.0f, 1.0f, 357.2f, 44.6f, 0.6f, 0.4f}, - {5.0f, 2.0f, 200.3f, 188.9f, -1.8f, 1.8f}, - {5.0f, 3.0f, -141.1f, -118.2f, -1.0f, 1.2f}, - {5.0f, 4.0f, -163.0f, 0.0f, 0.9f, 4.0f}, - {5.0f, 5.0f, -7.8f, 100.9f, 1.0f, -0.6f}, - {6.0f, 0.0f, 72.8f, 0.0f, -0.2f, 0.0f}, - {6.0f, 1.0f, 68.6f, -20.8f, -0.2f, -0.2f}, - {6.0f, 2.0f, 76.0f, 44.1f, -0.1f, -2.1f}, - {6.0f, 3.0f, -141.4f, 61.5f, 2.0f, -0.4f}, - {6.0f, 4.0f, -22.8f, -66.3f, -1.7f, -0.6f}, - {6.0f, 5.0f, 13.2f, 3.1f, -0.3f, 0.5f}, - {6.0f, 6.0f, -77.9f, 55.0f, 1.7f, 0.9f}, - {7.0f, 0.0f, 80.5f, 0.0f, 0.1f, 0.0f}, - {7.0f, 1.0f, -75.1f, -57.9f, -0.1f, 0.7f}, - {7.0f, 2.0f, -4.7f, -21.1f, -0.6f, 0.3f}, - {7.0f, 3.0f, 45.3f, 6.5f, 1.3f, -0.1f}, - {7.0f, 4.0f, 13.9f, 24.9f, 0.4f, -0.1f}, - {7.0f, 5.0f, 10.4f, 7.0f, 0.3f, -0.8f}, - {7.0f, 6.0f, 1.7f, -27.7f, -0.7f, -0.3f}, - {7.0f, 7.0f, 4.9f, -3.3f, 0.6f, 0.3f}, - {8.0f, 0.0f, 24.4f, 0.0f, -0.1f, 0.0f}, - {8.0f, 1.0f, 8.1f, 11.0f, 0.1f, -0.1f}, - {8.0f, 2.0f, -14.5f, -20.0f, -0.6f, 0.2f}, - {8.0f, 3.0f, -5.6f, 11.9f, 0.2f, 0.4f}, - {8.0f, 4.0f, -19.3f, -17.4f, -0.2f, 0.4f}, - {8.0f, 5.0f, 11.5f, 16.7f, 0.3f, 0.1f}, - {8.0f, 6.0f, 10.9f, 7.0f, 0.3f, -0.1f}, - {8.0f, 7.0f, -14.1f, -10.8f, -0.6f, 0.4f}, - {8.0f, 8.0f, -3.7f, 1.7f, 0.2f, 0.3f}, - {9.0f, 0.0f, 5.4f, 0.0f, 0.0f, 0.0f}, - {9.0f, 1.0f, 9.4f, -20.5f, -0.1f, 0.0f}, - {9.0f, 2.0f, 3.4f, 11.5f, 0.0f, -0.2f}, - {9.0f, 3.0f, -5.2f, 12.8f, 0.3f, 0.0f}, - {9.0f, 4.0f, 3.1f, -7.2f, -0.4f, -0.1f}, - {9.0f, 5.0f, -12.4f, -7.4f, -0.3f, 0.1f}, - {9.0f, 6.0f, -0.7f, 8.0f, 0.1f, 0.0f}, - {9.0f, 7.0f, 8.4f, 2.1f, -0.1f, -0.2f}, - {9.0f, 8.0f, -8.5f, -6.1f, -0.4f, 0.3f}, - {9.0f, 9.0f, -10.1f, 7.0f, -0.2f, 0.2f}, - {10.0f, 0.0f, -2.0f, 0.0f, 0.0f, 0.0f}, - {10.0f, 1.0f, -6.3f, 2.8f, 0.0f, 0.1f}, - {10.0f, 2.0f, 0.9f, -0.1f, -0.1f, -0.1f}, - {10.0f, 3.0f, -1.1f, 4.7f, 0.2f, 0.0f}, - {10.0f, 4.0f, -0.2f, 4.4f, 0.0f, -0.1f}, - {10.0f, 5.0f, 2.5f, -7.2f, -0.1f, -0.1f}, - {10.0f, 6.0f, -0.3f, -1.0f, -0.2f, 0.0f}, - {10.0f, 7.0f, 2.2f, -3.9f, 0.0f, -0.1f}, - {10.0f, 8.0f, 3.1f, -2.0f, -0.1f, -0.2f}, - {10.0f, 9.0f, -1.0f, -2.0f, -0.2f, 0.0f}, - {10.0f, 10.0f, -2.8f, -8.3f, -0.2f, -0.1f}, - {11.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f}, - {11.0f, 1.0f, -1.5f, 0.2f, 0.0f, 0.0f}, - {11.0f, 2.0f, -2.1f, 1.7f, 0.0f, 0.1f}, - {11.0f, 3.0f, 1.7f, -0.6f, 0.1f, 0.0f}, - {11.0f, 4.0f, -0.5f, -1.8f, 0.0f, 0.1f}, - {11.0f, 5.0f, 0.5f, 0.9f, 0.0f, 0.0f}, - {11.0f, 6.0f, -0.8f, -0.4f, 0.0f, 0.1f}, - {11.0f, 7.0f, 0.4f, -2.5f, 0.0f, 0.0f}, - {11.0f, 8.0f, 1.8f, -1.3f, 0.0f, -0.1f}, - {11.0f, 9.0f, 0.1f, -2.1f, 0.0f, -0.1f}, - {11.0f, 10.0f, 0.7f, -1.9f, -0.1f, 0.0f}, - {11.0f, 11.0f, 3.8f, -1.8f, 0.0f, -0.1f}, - {12.0f, 0.0f, -2.2f, 0.0f, 0.0f, 0.0f}, - {12.0f, 1.0f, -0.2f, -0.9f, 0.0f, 0.0f}, - {12.0f, 2.0f, 0.3f, 0.3f, 0.1f, 0.0f}, - {12.0f, 3.0f, 1.0f, 2.1f, 0.1f, 0.0f}, - {12.0f, 4.0f, -0.6f, -2.5f, -0.1f, 0.0f}, - {12.0f, 5.0f, 0.9f, 0.5f, 0.0f, 0.0f}, - {12.0f, 6.0f, -0.1f, 0.6f, 0.0f, 0.1f}, - {12.0f, 7.0f, 0.5f, 0.0f, 0.0f, 0.0f}, - {12.0f, 8.0f, -0.4f, 0.1f, 0.0f, 0.0f}, - {12.0f, 9.0f, -0.4f, 0.3f, 0.0f, 0.0f}, - {12.0f, 10.0f, 0.2f, -0.9f, 0.0f, 0.0f}, - {12.0f, 11.0f, -0.8f, -0.2f, -0.1f, 0.0f}, - {12.0f, 12.0f, 0.0f, 0.9f, 0.1f, 0.0f} +static const float CoeffFile[91][6] = { + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }, + { 1.0f, 0.0f, -29496.6f, 0.0f, 11.6f, 0.0f }, + { 1.0f, 1.0f, -1586.3f, 4944.4f, 16.5f, -25.9f }, + { 2.0f, 0.0f, -2396.6f, 0.0f, -12.1f, 0.0f }, + { 2.0f, 1.0f, 3026.1f, -2707.7f, -4.4f, -22.5f }, + { 2.0f, 2.0f, 1668.6f, -576.1f, 1.9f, -11.8f }, + { 3.0f, 0.0f, 1340.1f, 0.0f, 0.4f, 0.0f }, + { 3.0f, 1.0f, -2326.2f, -160.2f, -4.1f, 7.3f }, + { 3.0f, 2.0f, 1231.9f, 251.9f, -2.9f, -3.9f }, + { 3.0f, 3.0f, 634.0f, -536.6f, -7.7f, -2.6f }, + { 4.0f, 0.0f, 912.6f, 0.0f, -1.8f, 0.0f }, + { 4.0f, 1.0f, 808.9f, 286.4f, 2.3f, 1.1f }, + { 4.0f, 2.0f, 166.7f, -211.2f, -8.7f, 2.7f }, + { 4.0f, 3.0f, -357.1f, 164.3f, 4.6f, 3.9f }, + { 4.0f, 4.0f, 89.4f, -309.1f, -2.1f, -0.8f }, + { 5.0f, 0.0f, -230.9f, 0.0f, -1.0f, 0.0f }, + { 5.0f, 1.0f, 357.2f, 44.6f, 0.6f, 0.4f }, + { 5.0f, 2.0f, 200.3f, 188.9f, -1.8f, 1.8f }, + { 5.0f, 3.0f, -141.1f, -118.2f, -1.0f, 1.2f }, + { 5.0f, 4.0f, -163.0f, 0.0f, 0.9f, 4.0f }, + { 5.0f, 5.0f, -7.8f, 100.9f, 1.0f, -0.6f }, + { 6.0f, 0.0f, 72.8f, 0.0f, -0.2f, 0.0f }, + { 6.0f, 1.0f, 68.6f, -20.8f, -0.2f, -0.2f }, + { 6.0f, 2.0f, 76.0f, 44.1f, -0.1f, -2.1f }, + { 6.0f, 3.0f, -141.4f, 61.5f, 2.0f, -0.4f }, + { 6.0f, 4.0f, -22.8f, -66.3f, -1.7f, -0.6f }, + { 6.0f, 5.0f, 13.2f, 3.1f, -0.3f, 0.5f }, + { 6.0f, 6.0f, -77.9f, 55.0f, 1.7f, 0.9f }, + { 7.0f, 0.0f, 80.5f, 0.0f, 0.1f, 0.0f }, + { 7.0f, 1.0f, -75.1f, -57.9f, -0.1f, 0.7f }, + { 7.0f, 2.0f, -4.7f, -21.1f, -0.6f, 0.3f }, + { 7.0f, 3.0f, 45.3f, 6.5f, 1.3f, -0.1f }, + { 7.0f, 4.0f, 13.9f, 24.9f, 0.4f, -0.1f }, + { 7.0f, 5.0f, 10.4f, 7.0f, 0.3f, -0.8f }, + { 7.0f, 6.0f, 1.7f, -27.7f, -0.7f, -0.3f }, + { 7.0f, 7.0f, 4.9f, -3.3f, 0.6f, 0.3f }, + { 8.0f, 0.0f, 24.4f, 0.0f, -0.1f, 0.0f }, + { 8.0f, 1.0f, 8.1f, 11.0f, 0.1f, -0.1f }, + { 8.0f, 2.0f, -14.5f, -20.0f, -0.6f, 0.2f }, + { 8.0f, 3.0f, -5.6f, 11.9f, 0.2f, 0.4f }, + { 8.0f, 4.0f, -19.3f, -17.4f, -0.2f, 0.4f }, + { 8.0f, 5.0f, 11.5f, 16.7f, 0.3f, 0.1f }, + { 8.0f, 6.0f, 10.9f, 7.0f, 0.3f, -0.1f }, + { 8.0f, 7.0f, -14.1f, -10.8f, -0.6f, 0.4f }, + { 8.0f, 8.0f, -3.7f, 1.7f, 0.2f, 0.3f }, + { 9.0f, 0.0f, 5.4f, 0.0f, 0.0f, 0.0f }, + { 9.0f, 1.0f, 9.4f, -20.5f, -0.1f, 0.0f }, + { 9.0f, 2.0f, 3.4f, 11.5f, 0.0f, -0.2f }, + { 9.0f, 3.0f, -5.2f, 12.8f, 0.3f, 0.0f }, + { 9.0f, 4.0f, 3.1f, -7.2f, -0.4f, -0.1f }, + { 9.0f, 5.0f, -12.4f, -7.4f, -0.3f, 0.1f }, + { 9.0f, 6.0f, -0.7f, 8.0f, 0.1f, 0.0f }, + { 9.0f, 7.0f, 8.4f, 2.1f, -0.1f, -0.2f }, + { 9.0f, 8.0f, -8.5f, -6.1f, -0.4f, 0.3f }, + { 9.0f, 9.0f, -10.1f, 7.0f, -0.2f, 0.2f }, + { 10.0f, 0.0f, -2.0f, 0.0f, 0.0f, 0.0f }, + { 10.0f, 1.0f, -6.3f, 2.8f, 0.0f, 0.1f }, + { 10.0f, 2.0f, 0.9f, -0.1f, -0.1f, -0.1f }, + { 10.0f, 3.0f, -1.1f, 4.7f, 0.2f, 0.0f }, + { 10.0f, 4.0f, -0.2f, 4.4f, 0.0f, -0.1f }, + { 10.0f, 5.0f, 2.5f, -7.2f, -0.1f, -0.1f }, + { 10.0f, 6.0f, -0.3f, -1.0f, -0.2f, 0.0f }, + { 10.0f, 7.0f, 2.2f, -3.9f, 0.0f, -0.1f }, + { 10.0f, 8.0f, 3.1f, -2.0f, -0.1f, -0.2f }, + { 10.0f, 9.0f, -1.0f, -2.0f, -0.2f, 0.0f }, + { 10.0f, 10.0f, -2.8f, -8.3f, -0.2f, -0.1f }, + { 11.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f }, + { 11.0f, 1.0f, -1.5f, 0.2f, 0.0f, 0.0f }, + { 11.0f, 2.0f, -2.1f, 1.7f, 0.0f, 0.1f }, + { 11.0f, 3.0f, 1.7f, -0.6f, 0.1f, 0.0f }, + { 11.0f, 4.0f, -0.5f, -1.8f, 0.0f, 0.1f }, + { 11.0f, 5.0f, 0.5f, 0.9f, 0.0f, 0.0f }, + { 11.0f, 6.0f, -0.8f, -0.4f, 0.0f, 0.1f }, + { 11.0f, 7.0f, 0.4f, -2.5f, 0.0f, 0.0f }, + { 11.0f, 8.0f, 1.8f, -1.3f, 0.0f, -0.1f }, + { 11.0f, 9.0f, 0.1f, -2.1f, 0.0f, -0.1f }, + { 11.0f, 10.0f, 0.7f, -1.9f, -0.1f, 0.0f }, + { 11.0f, 11.0f, 3.8f, -1.8f, 0.0f, -0.1f }, + { 12.0f, 0.0f, -2.2f, 0.0f, 0.0f, 0.0f }, + { 12.0f, 1.0f, -0.2f, -0.9f, 0.0f, 0.0f }, + { 12.0f, 2.0f, 0.3f, 0.3f, 0.1f, 0.0f }, + { 12.0f, 3.0f, 1.0f, 2.1f, 0.1f, 0.0f }, + { 12.0f, 4.0f, -0.6f, -2.5f, -0.1f, 0.0f }, + { 12.0f, 5.0f, 0.9f, 0.5f, 0.0f, 0.0f }, + { 12.0f, 6.0f, -0.1f, 0.6f, 0.0f, 0.1f }, + { 12.0f, 7.0f, 0.5f, 0.0f, 0.0f, 0.0f }, + { 12.0f, 8.0f, -0.4f, 0.1f, 0.0f, 0.0f }, + { 12.0f, 9.0f, -0.4f, 0.3f, 0.0f, 0.0f }, + { 12.0f, 10.0f, 0.2f, -0.9f, 0.0f, 0.0f }, + { 12.0f, 11.0f, -0.8f, -0.2f, -0.1f, 0.0f }, + { 12.0f, 12.0f, 0.0f, 0.9f, 0.1f, 0.0f } }; -static WMMtype_Ellipsoid *Ellip = NULL; -static WMMtype_MagneticModel *MagneticModel = NULL; -static float decimal_date; +static WMMtype_Ellipsoid *Ellip = NULL; +static WMMtype_MagneticModel *MagneticModel = NULL; +static float decimal_date; /************************************************************************************** * Example use - very simple - only two exposed functions @@ -168,31 +169,34 @@ static float decimal_date; **************************************************************************************/ int WMM_Initialize() -// Sets default values for WMM subroutines. -// UPDATES : Ellip and MagneticModel -{ - if (!Ellip) return -1; // invalid pointer - if (!MagneticModel) return -2; // invalid pointer - - // Sets WGS-84 parameters - Ellip->a = 6378.137f; // semi-major axis of the ellipsoid in km - Ellip->b = 6356.7523142f; // semi-minor axis of the ellipsoid in km - Ellip->fla = 1.0f / 298.257223563f; // flattening - Ellip->eps = sqrt(1 - (Ellip->b * Ellip->b) / (Ellip->a * Ellip->a)); // first eccentricity - Ellip->epssq = (Ellip->eps * Ellip->eps); // first eccentricity squared - Ellip->re = 6371.2f; // Earth's radius in km +// Sets default values for WMM subroutines. +// UPDATES : Ellip and MagneticModel +{ + if (!Ellip) { + return -1; // invalid pointer + } + if (!MagneticModel) { + return -2; // invalid pointer + } + // Sets WGS-84 parameters + Ellip->a = 6378.137f; // semi-major axis of the ellipsoid in km + Ellip->b = 6356.7523142f; // semi-minor axis of the ellipsoid in km + Ellip->fla = 1.0f / 298.257223563f; // flattening + Ellip->eps = sqrt(1 - (Ellip->b * Ellip->b) / (Ellip->a * Ellip->a)); // first eccentricity + Ellip->epssq = (Ellip->eps * Ellip->eps); // first eccentricity squared + Ellip->re = 6371.2f; // Earth's radius in km - // Sets Magnetic Model parameters - MagneticModel->nMax = WMM_MAX_MODEL_DEGREES; - MagneticModel->nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES; - MagneticModel->SecularVariationUsed = 0; + // Sets Magnetic Model parameters + MagneticModel->nMax = WMM_MAX_MODEL_DEGREES; + MagneticModel->nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES; + MagneticModel->SecularVariationUsed = 0; - // Really, Really needs to be read from a file - out of date in 2015 at latest - MagneticModel->EditionDate = 0.0f; /* OP change. Originally 5.7863328170559505e-307, truncates to 0.0f */ - MagneticModel->epoch = 2010.0f; - sprintf(MagneticModel->ModelName, "WMM-2010"); + // Really, Really needs to be read from a file - out of date in 2015 at latest + MagneticModel->EditionDate = 0.0f; /* OP change. Originally 5.7863328170559505e-307, truncates to 0.0f */ + MagneticModel->epoch = 2010.0f; + sprintf(MagneticModel->ModelName, "WMM-2010"); - return 0; // OK + return 0; // OK } int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, uint16_t Day, uint16_t Year, float B[3]) @@ -200,206 +204,213 @@ int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, u // return '0' if all appears to be OK // return < 0 if error - int returned = 0; // default to OK + int returned = 0; // default to OK // *********** // range check supplied params - if (Lat < -90.0f) return -1; // error - if (Lat > 90.0f) return -2; // error - - if (Lon < -180.0f) return -3; // error - if (Lon > 180.0f) return -4; // error - + if (Lat < -90.0f) { + return -1; // error + } + if (Lat > 90.0f) { + return -2; // error + } + if (Lon < -180.0f) { + return -3; // error + } + if (Lon > 180.0f) { + return -4; // error + } // *********** // allocated required memory -// Ellip = NULL; -// MagneticModel = NULL; +// Ellip = NULL; +// MagneticModel = NULL; -// MagneticModel = NULL; -// CoordGeodetic = NULL; -// GeoMagneticElements = NULL; +// MagneticModel = NULL; +// CoordGeodetic = NULL; +// GeoMagneticElements = NULL; - Ellip = (WMMtype_Ellipsoid *) MALLOC(sizeof(WMMtype_Ellipsoid)); - MagneticModel = (WMMtype_MagneticModel *) MALLOC(sizeof(WMMtype_MagneticModel)); + Ellip = (WMMtype_Ellipsoid *)MALLOC(sizeof(WMMtype_Ellipsoid)); + MagneticModel = (WMMtype_MagneticModel *)MALLOC(sizeof(WMMtype_MagneticModel)); - WMMtype_CoordSpherical *CoordSpherical = (WMMtype_CoordSpherical *) MALLOC(sizeof(WMMtype_CoordSpherical)); - WMMtype_CoordGeodetic *CoordGeodetic = (WMMtype_CoordGeodetic *) MALLOC(sizeof(WMMtype_CoordGeodetic)); - WMMtype_GeoMagneticElements *GeoMagneticElements = (WMMtype_GeoMagneticElements *) MALLOC(sizeof(WMMtype_GeoMagneticElements)); - - if (!Ellip || !MagneticModel || !CoordSpherical || !CoordGeodetic || !GeoMagneticElements) - returned = -5; // error + WMMtype_CoordSpherical *CoordSpherical = (WMMtype_CoordSpherical *)MALLOC(sizeof(WMMtype_CoordSpherical)); + WMMtype_CoordGeodetic *CoordGeodetic = (WMMtype_CoordGeodetic *)MALLOC(sizeof(WMMtype_CoordGeodetic)); + WMMtype_GeoMagneticElements *GeoMagneticElements = (WMMtype_GeoMagneticElements *)MALLOC(sizeof(WMMtype_GeoMagneticElements)); + if (!Ellip || !MagneticModel || !CoordSpherical || !CoordGeodetic || !GeoMagneticElements) { + returned = -5; // error + } // *********** - if (returned >= 0) - { - if (WMM_Initialize() < 0) - returned = -6; // error + if (returned >= 0) { + if (WMM_Initialize() < 0) { + returned = -6; // error + } } - if (returned >= 0) - { + if (returned >= 0) { CoordGeodetic->lambda = Lon; - CoordGeodetic->phi = Lat; - CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid/1000.0f; // convert to km + CoordGeodetic->phi = Lat; + CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid / 1000.0f; // convert to km // Convert from geodetic to Spherical Equations: 17-18, WMM Technical report - if (WMM_GeodeticToSpherical(CoordGeodetic, CoordSpherical) < 0) - returned = -7; // error + if (WMM_GeodeticToSpherical(CoordGeodetic, CoordSpherical) < 0) { + returned = -7; // error + } } - if (returned >= 0) - { - if (WMM_DateToYear(Month, Day, Year) < 0) - returned = -8; // error + if (returned >= 0) { + if (WMM_DateToYear(Month, Day, Year) < 0) { + returned = -8; // error + } } - if (returned >= 0) - { + if (returned >= 0) { // Compute the geoMagnetic field elements and their time change - if (WMM_Geomag(CoordSpherical, CoordGeodetic, GeoMagneticElements) < 0) - returned = -9; // error - else - { // set the returned values + if (WMM_Geomag(CoordSpherical, CoordGeodetic, GeoMagneticElements) < 0) { + returned = -9; // error + } else { // set the returned values B[0] = GeoMagneticElements->X; B[1] = GeoMagneticElements->Y; B[2] = GeoMagneticElements->Z; } } - // *********** - // free allocated memory + // *********** + // free allocated memory - if (GeoMagneticElements) + if (GeoMagneticElements) { FREE(GeoMagneticElements); - - if (CoordGeodetic) - FREE(CoordGeodetic); - - if (CoordSpherical) - FREE(CoordSpherical); - - if (MagneticModel) - { - FREE(MagneticModel); - MagneticModel = NULL; } - if (Ellip) - { + if (CoordGeodetic) { + FREE(CoordGeodetic); + } + + if (CoordSpherical) { + FREE(CoordSpherical); + } + + if (MagneticModel) { + FREE(MagneticModel); + MagneticModel = NULL; + } + + if (Ellip) { FREE(Ellip); Ellip = NULL; } - B[0] = GeoMagneticElements->X * 1e-2f; - B[1] = GeoMagneticElements->Y * 1e-2f; - B[2] = GeoMagneticElements->Z * 1e-2f; + B[0] = GeoMagneticElements->X * 1e-2f; + B[1] = GeoMagneticElements->Y * 1e-2f; + B[2] = GeoMagneticElements->Z * 1e-2f; return returned; } -int WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical, WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_GeoMagneticElements * GeoMagneticElements) - /* - The main subroutine that calls a sequence of WMM sub-functions to calculate the magnetic field elements for a single point. - The function expects the model coefficients and point coordinates as input and returns the magnetic field elements and - their rate of change. Though, this subroutine can be called successively to calculate a time series, profile or grid - of magnetic field, these are better achieved by the subroutine WMM_Grid. +int WMM_Geomag(WMMtype_CoordSpherical *CoordSpherical, WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_GeoMagneticElements *GeoMagneticElements) +/* + The main subroutine that calls a sequence of WMM sub-functions to calculate the magnetic field elements for a single point. + The function expects the model coefficients and point coordinates as input and returns the magnetic field elements and + their rate of change. Though, this subroutine can be called successively to calculate a time series, profile or grid + of magnetic field, these are better achieved by the subroutine WMM_Grid. - INPUT: Ellip - CoordSpherical - CoordGeodetic - TimedMagneticModel + INPUT: Ellip + CoordSpherical + CoordGeodetic + TimedMagneticModel - OUTPUT : GeoMagneticElements + OUTPUT : GeoMagneticElements - CALLS: WMM_ComputeSphericalHarmonicVariables( Ellip, CoordSpherical, TimedMagneticModel->nMax, &SphVariables); (Compute Spherical Harmonic variables ) - WMM_AssociatedLegendreFunction(CoordSpherical, TimedMagneticModel->nMax, LegendreFunction); Compute ALF - WMM_Summation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSph); Accumulate the spherical harmonic coefficients - WMM_SecVarSummation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSphVar); Sum the Secular Variation Coefficients - WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSph, &MagneticResultsGeo); Map the computed Magnetic fields to Geodeitic coordinates - WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSphVar, &MagneticResultsGeoVar); Map the secular variation field components to Geodetic coordinates - WMM_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); Calculate the Geomagnetic elements - WMM_CalculateSecularVariation(MagneticResultsGeoVar, GeoMagneticElements); Calculate the secular variation of each of the Geomagnetic elements + CALLS: WMM_ComputeSphericalHarmonicVariables( Ellip, CoordSpherical, TimedMagneticModel->nMax, &SphVariables); (Compute Spherical Harmonic variables ) + WMM_AssociatedLegendreFunction(CoordSpherical, TimedMagneticModel->nMax, LegendreFunction); Compute ALF + WMM_Summation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSph); Accumulate the spherical harmonic coefficients + WMM_SecVarSummation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSphVar); Sum the Secular Variation Coefficients + WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSph, &MagneticResultsGeo); Map the computed Magnetic fields to Geodeitic coordinates + WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSphVar, &MagneticResultsGeoVar); Map the secular variation field components to Geodetic coordinates + WMM_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); Calculate the Geomagnetic elements + WMM_CalculateSecularVariation(MagneticResultsGeoVar, GeoMagneticElements); Calculate the secular variation of each of the Geomagnetic elements - */ + */ { - int returned = 0; // default to OK + int returned = 0; // default to OK - WMMtype_MagneticResults MagneticResultsSph; - WMMtype_MagneticResults MagneticResultsGeo; - WMMtype_MagneticResults MagneticResultsSphVar; - WMMtype_MagneticResults MagneticResultsGeoVar; + WMMtype_MagneticResults MagneticResultsSph; + WMMtype_MagneticResults MagneticResultsGeo; + WMMtype_MagneticResults MagneticResultsSphVar; + WMMtype_MagneticResults MagneticResultsGeoVar; // ******** // allocate required memory - WMMtype_LegendreFunction *LegendreFunction = (WMMtype_LegendreFunction *) MALLOC(sizeof(WMMtype_LegendreFunction)); - WMMtype_SphericalHarmonicVariables *SphVariables = (WMMtype_SphericalHarmonicVariables *) MALLOC(sizeof(WMMtype_SphericalHarmonicVariables)); - - if (!LegendreFunction || !SphVariables) - returned = -1; // memory allocation error + WMMtype_LegendreFunction *LegendreFunction = (WMMtype_LegendreFunction *)MALLOC(sizeof(WMMtype_LegendreFunction)); + WMMtype_SphericalHarmonicVariables *SphVariables = (WMMtype_SphericalHarmonicVariables *)MALLOC(sizeof(WMMtype_SphericalHarmonicVariables)); + if (!LegendreFunction || !SphVariables) { + returned = -1; // memory allocation error + } // ******** - if (returned >= 0) - { // Compute Spherical Harmonic variables - if (WMM_ComputeSphericalHarmonicVariables(CoordSpherical, MagneticModel->nMax, SphVariables) < 0) - returned = -2; // error + if (returned >= 0) { // Compute Spherical Harmonic variables + if (WMM_ComputeSphericalHarmonicVariables(CoordSpherical, MagneticModel->nMax, SphVariables) < 0) { + returned = -2; // error + } } - if (returned >= 0) - { // Compute ALF - if (WMM_AssociatedLegendreFunction(CoordSpherical, MagneticModel->nMax, LegendreFunction) < 0) - returned = -3; // error + if (returned >= 0) { // Compute ALF + if (WMM_AssociatedLegendreFunction(CoordSpherical, MagneticModel->nMax, LegendreFunction) < 0) { + returned = -3; // error + } } - if (returned >= 0) - { // Accumulate the spherical harmonic coefficients - if (WMM_Summation(LegendreFunction, SphVariables, CoordSpherical, &MagneticResultsSph) < 0) - returned = -4; // error + if (returned >= 0) { // Accumulate the spherical harmonic coefficients + if (WMM_Summation(LegendreFunction, SphVariables, CoordSpherical, &MagneticResultsSph) < 0) { + returned = -4; // error + } } - if (returned >= 0) - { // Sum the Secular Variation Coefficients - if (WMM_SecVarSummation(LegendreFunction, SphVariables, CoordSpherical, &MagneticResultsSphVar) < 0) - returned = -5; // error + if (returned >= 0) { // Sum the Secular Variation Coefficients + if (WMM_SecVarSummation(LegendreFunction, SphVariables, CoordSpherical, &MagneticResultsSphVar) < 0) { + returned = -5; // error + } } - if (returned >= 0) - { // Map the computed Magnetic fields to Geodeitic coordinates - if (WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSph, &MagneticResultsGeo) < 0) - returned = -6; // error + if (returned >= 0) { // Map the computed Magnetic fields to Geodeitic coordinates + if (WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSph, &MagneticResultsGeo) < 0) { + returned = -6; // error + } } - if (returned >= 0) - { // Map the secular variation field components to Geodetic coordinates - if (WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSphVar, &MagneticResultsGeoVar) < 0) - returned = -7; // error + if (returned >= 0) { // Map the secular variation field components to Geodetic coordinates + if (WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSphVar, &MagneticResultsGeoVar) < 0) { + returned = -7; // error + } } - if (returned >= 0) - { // Calculate the Geomagnetic elements, Equation 18 , WMM Technical report - if (WMM_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements) < 0) - returned = -8; // error + if (returned >= 0) { // Calculate the Geomagnetic elements, Equation 18 , WMM Technical report + if (WMM_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements) < 0) { + returned = -8; // error + } } - if (returned >= 0) - { // Calculate the secular variation of each of the Geomagnetic elements - if (WMM_CalculateSecularVariation(&MagneticResultsGeoVar, GeoMagneticElements) < 0) - returned = -9; // error + if (returned >= 0) { // Calculate the secular variation of each of the Geomagnetic elements + if (WMM_CalculateSecularVariation(&MagneticResultsGeoVar, GeoMagneticElements) < 0) { + returned = -9; // error + } } // ******** // free allocated memory - if (SphVariables) + if (SphVariables) { FREE(SphVariables); + } - if (LegendreFunction) + if (LegendreFunction) { FREE(LegendreFunction); + } // ******** @@ -407,425 +418,416 @@ int WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical, WMMtype_CoordGeodetic * } int WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical, uint16_t nMax, WMMtype_SphericalHarmonicVariables *SphVariables) +/* Computes Spherical variables + Variables computed are (a/r)^(n+2), cos_m(lamda) and sin_m(lambda) for spherical harmonic + summations. (Equations 10-12 in the WMM Technical Report) + INPUT Ellip data structure with the following elements + float a; semi-major axis of the ellipsoid + float b; semi-minor axis of the ellipsoid + float fla; flattening + float epssq; first eccentricity squared + float eps; first eccentricity + float re; mean radius of ellipsoid + CoordSpherical A data structure with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) + nMax integer ( Maxumum degree of spherical harmonic secular model)\ - /* Computes Spherical variables - Variables computed are (a/r)^(n+2), cos_m(lamda) and sin_m(lambda) for spherical harmonic - summations. (Equations 10-12 in the WMM Technical Report) - INPUT Ellip data structure with the following elements - float a; semi-major axis of the ellipsoid - float b; semi-minor axis of the ellipsoid - float fla; flattening - float epssq; first eccentricity squared - float eps; first eccentricity - float re; mean radius of ellipsoid - CoordSpherical A data structure with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) - nMax integer ( Maxumum degree of spherical harmonic secular model)\ - - OUTPUT SphVariables Pointer to the data structure with the following elements - float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES+1]; [earth_reference_radius_km sph. radius ]^n - float cos_mlambda[WMM_MAX_MODEL_DEGREES+1]; cp(m) - cosine of (mspherical coord. longitude) - float sin_mlambda[WMM_MAX_MODEL_DEGREES+1]; sp(m) - sine of (mspherical coord. longitude) - CALLS : none - */ + OUTPUT SphVariables Pointer to the data structure with the following elements + float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES+1]; [earth_reference_radius_km sph. radius ]^n + float cos_mlambda[WMM_MAX_MODEL_DEGREES+1]; cp(m) - cosine of (mspherical coord. longitude) + float sin_mlambda[WMM_MAX_MODEL_DEGREES+1]; sp(m) - sine of (mspherical coord. longitude) + CALLS : none + */ { - float cos_lambda, sin_lambda; - uint16_t m, n; + float cos_lambda, sin_lambda; + uint16_t m, n; - cos_lambda = cosf(DEG2RAD(CoordSpherical->lambda)); - sin_lambda = sinf(DEG2RAD(CoordSpherical->lambda)); + cos_lambda = cosf(DEG2RAD(CoordSpherical->lambda)); + sin_lambda = sinf(DEG2RAD(CoordSpherical->lambda)); - /* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2) - for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ + /* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2) + for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ - SphVariables->RelativeRadiusPower[0] = (Ellip->re / CoordSpherical->r) * (Ellip->re / CoordSpherical->r); - for (n = 1; n <= nMax; n++) - SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n - 1] * (Ellip->re / CoordSpherical->r); + SphVariables->RelativeRadiusPower[0] = (Ellip->re / CoordSpherical->r) * (Ellip->re / CoordSpherical->r); + for (n = 1; n <= nMax; n++) { + SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n - 1] * (Ellip->re / CoordSpherical->r); + } - /* - Compute cosf(m*lambda), sinf(m*lambda) for m = 0 ... nMax - cosf(a + b) = cosf(a)*cosf(b) - sinf(a)*sinf(b) - sinf(a + b) = cosf(a)*sinf(b) + sinf(a)*cosf(b) - */ - SphVariables->cos_mlambda[0] = 1.0f; - SphVariables->sin_mlambda[0] = 0.0f; + /* + Compute cosf(m*lambda), sinf(m*lambda) for m = 0 ... nMax + cosf(a + b) = cosf(a)*cosf(b) - sinf(a)*sinf(b) + sinf(a + b) = cosf(a)*sinf(b) + sinf(a)*cosf(b) + */ + SphVariables->cos_mlambda[0] = 1.0f; + SphVariables->sin_mlambda[0] = 0.0f; - SphVariables->cos_mlambda[1] = cos_lambda; - SphVariables->sin_mlambda[1] = sin_lambda; - for (m = 2; m <= nMax; m++) - { - SphVariables->cos_mlambda[m] = SphVariables->cos_mlambda[m - 1] * cos_lambda - SphVariables->sin_mlambda[m - 1] * sin_lambda; - SphVariables->sin_mlambda[m] = SphVariables->cos_mlambda[m - 1] * sin_lambda + SphVariables->sin_mlambda[m - 1] * cos_lambda; - } + SphVariables->cos_mlambda[1] = cos_lambda; + SphVariables->sin_mlambda[1] = sin_lambda; + for (m = 2; m <= nMax; m++) { + SphVariables->cos_mlambda[m] = SphVariables->cos_mlambda[m - 1] * cos_lambda - SphVariables->sin_mlambda[m - 1] * sin_lambda; + SphVariables->sin_mlambda[m] = SphVariables->cos_mlambda[m - 1] * sin_lambda + SphVariables->sin_mlambda[m - 1] * cos_lambda; + } - return 0; // OK + return 0; // OK } -int WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical * CoordSpherical, uint16_t nMax, WMMtype_LegendreFunction * LegendreFunction) +int WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical *CoordSpherical, uint16_t nMax, WMMtype_LegendreFunction *LegendreFunction) +/* Computes all of the Schmidt-semi normalized associated Legendre + functions up to degree nMax. If nMax <= 16, function WMM_PcupLow is used. + Otherwise WMM_PcupHigh is called. + INPUT CoordSpherical A data structure with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) + nMax integer ( Maxumum degree of spherical harmonic secular model) + LegendreFunction Pointer to data structure with the following elements + float *Pcup; ( pointer to store Legendre Function ) + float *dPcup; ( pointer to store Derivative of Lagendre function ) - /* Computes all of the Schmidt-semi normalized associated Legendre - functions up to degree nMax. If nMax <= 16, function WMM_PcupLow is used. - Otherwise WMM_PcupHigh is called. - INPUT CoordSpherical A data structure with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) - nMax integer ( Maxumum degree of spherical harmonic secular model) - LegendreFunction Pointer to data structure with the following elements - float *Pcup; ( pointer to store Legendre Function ) - float *dPcup; ( pointer to store Derivative of Lagendre function ) + OUTPUT LegendreFunction Calculated Legendre variables in the data structure - OUTPUT LegendreFunction Calculated Legendre variables in the data structure - - */ + */ { - float sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); /* sinf (geocentric latitude) */ + float sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); /* sinf (geocentric latitude) */ - if (nMax <= 16 || (1 - fabsf(sin_phi)) < 1.0e-10f) /* If nMax is less tha 16 or at the poles */ - { - if (WMM_PcupLow(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0) - return -1; // error - } - else - { - if (WMM_PcupHigh(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0) - return -2; // error - } + if (nMax <= 16 || (1 - fabsf(sin_phi)) < 1.0e-10f) { /* If nMax is less tha 16 or at the poles */ + if (WMM_PcupLow(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0) { + return -1; // error + } + } else { + if (WMM_PcupHigh(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0) { + return -2; // error + } + } - return 0; // OK + return 0; // OK } -int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction, - WMMtype_SphericalHarmonicVariables * SphVariables, - WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults) +int WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables *SphVariables, + WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) { - /* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using - spherical harmonic summation. + /* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using + spherical harmonic summation. - The vector Magnetic field is given by -grad V, where V is Geomagnetic scalar potential - The gradient in spherical coordinates is given by: + The vector Magnetic field is given by -grad V, where V is Geomagnetic scalar potential + The gradient in spherical coordinates is given by: - dV ^ 1 dV ^ 1 dV ^ - grad V = -- r + - -- t + -------- -- p - dr r dt r sinf(t) dp + dV ^ 1 dV ^ 1 dV ^ + grad V = -- r + - -- t + -------- -- p + dr r dt r sinf(t) dp - INPUT : LegendreFunction - MagneticModel - SphVariables - CoordSpherical - OUTPUT : MagneticResults + INPUT : LegendreFunction + MagneticModel + SphVariables + CoordSpherical + OUTPUT : MagneticResults - CALLS : WMM_SummationSpecial + CALLS : WMM_SummationSpecial - Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov - */ + Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov + */ uint16_t m, n, index; - float cos_phi; + float cos_phi; - MagneticResults->Bz = 0.0f; - MagneticResults->By = 0.0f; - MagneticResults->Bx = 0.0f; + MagneticResults->Bz = 0.0f; + MagneticResults->By = 0.0f; + MagneticResults->Bx = 0.0f; - for (n = 1; n <= MagneticModel->nMax; n++) - { - for (m = 0; m <= n; m++) - { - index = (n * (n + 1) / 2 + m); + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); -/* nMax (n+2) n m m m - Bz = -SUM (a/r) (n+1) SUM [g cosf(m p) + h sinf(m p)] P (sinf(phi)) - n=1 m=0 n n n */ +/* nMax (n+2) n m m m + Bz = -SUM (a/r) (n+1) SUM [g cosf(m p) + h sinf(m p)] P (sinf(phi)) + n=1 m=0 n n n */ /* Equation 12 in the WMM Technical report. Derivative with respect to radius.*/ - MagneticResults->Bz -= - SphVariables->RelativeRadiusPower[n] * - (WMM_get_main_field_coeff_g(index) * - SphVariables->cos_mlambda[m] + WMM_get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) - * (float)(n + 1) * LegendreFunction->Pcup[index]; + MagneticResults->Bz -= + SphVariables->RelativeRadiusPower[n] * + (WMM_get_main_field_coeff_g(index) * + SphVariables->cos_mlambda[m] + WMM_get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) + * (float)(n + 1) * LegendreFunction->Pcup[index]; /* 1 nMax (n+2) n m m m - By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) - n=1 m=0 n n n */ + By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) + n=1 m=0 n n n */ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (WMM_get_main_field_coeff_g(index) * - SphVariables->sin_mlambda[m] - WMM_get_main_field_coeff_h(index) * SphVariables->cos_mlambda[m]) - * (float)(m) * LegendreFunction->Pcup[index]; + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (WMM_get_main_field_coeff_g(index) * + SphVariables->sin_mlambda[m] - WMM_get_main_field_coeff_h(index) * SphVariables->cos_mlambda[m]) + * (float)(m) * LegendreFunction->Pcup[index]; /* nMax (n+2) n m m m - Bx = - SUM (a/r) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) - n=1 m=0 n n n */ + Bx = - SUM (a/r) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) + n=1 m=0 n n n */ /* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */ - MagneticResults->Bx -= - SphVariables->RelativeRadiusPower[n] * - (WMM_get_main_field_coeff_g(index) * - SphVariables->cos_mlambda[m] + WMM_get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) - * LegendreFunction->dPcup[index]; + MagneticResults->Bx -= + SphVariables->RelativeRadiusPower[n] * + (WMM_get_main_field_coeff_g(index) * + SphVariables->cos_mlambda[m] + WMM_get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) + * LegendreFunction->dPcup[index]; + } + } - } - } - - cos_phi = cosf(DEG2RAD(CoordSpherical->phig)); - if (fabsf(cos_phi) > 1.0e-10f) - { - MagneticResults->By = MagneticResults->By / cos_phi; - } - else - { + cos_phi = cosf(DEG2RAD(CoordSpherical->phig)); + if (fabsf(cos_phi) > 1.0e-10f) { + MagneticResults->By = MagneticResults->By / cos_phi; + } else { /* Special calculation for component - By - at Geographic poles. * If the user wants to avoid using this function, please make sure that * the latitude is not exactly +/-90. An option is to make use the function * WMM_CheckGeographicPoles. */ - if (WMM_SummationSpecial(SphVariables, CoordSpherical, MagneticResults) < 0) - return -1; // error - } + if (WMM_SummationSpecial(SphVariables, CoordSpherical, MagneticResults) < 0) { + return -1; // error + } + } - return 0; // OK + return 0; // OK } -int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction, - WMMtype_SphericalHarmonicVariables * - SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults) +int WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables * + SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) { - /*This Function sums the secular variation coefficients to get the secular variation of the Magnetic vector. - INPUT : LegendreFunction - MagneticModel - SphVariables - CoordSpherical - OUTPUT : MagneticResults + /*This Function sums the secular variation coefficients to get the secular variation of the Magnetic vector. + INPUT : LegendreFunction + MagneticModel + SphVariables + CoordSpherical + OUTPUT : MagneticResults - CALLS : WMM_SecVarSummationSpecial + CALLS : WMM_SecVarSummationSpecial - */ + */ uint16_t m, n, index; - float cos_phi; + float cos_phi; - MagneticModel->SecularVariationUsed = TRUE; + MagneticModel->SecularVariationUsed = TRUE; - MagneticResults->Bz = 0.0f; - MagneticResults->By = 0.0f; - MagneticResults->Bx = 0.0f; + MagneticResults->Bz = 0.0f; + MagneticResults->By = 0.0f; + MagneticResults->Bx = 0.0f; - for (n = 1; n <= MagneticModel->nMaxSecVar; n++) - { - for (m = 0; m <= n; m++) - { - index = (n * (n + 1) / 2 + m); + for (n = 1; n <= MagneticModel->nMaxSecVar; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); -/* nMax (n+2) n m m m - Bz = -SUM (a/r) (n+1) SUM [g cosf(m p) + h sinf(m p)] P (sinf(phi)) - n=1 m=0 n n n */ +/* nMax (n+2) n m m m + Bz = -SUM (a/r) (n+1) SUM [g cosf(m p) + h sinf(m p)] P (sinf(phi)) + n=1 m=0 n n n */ /* Derivative with respect to radius.*/ - MagneticResults->Bz -= - SphVariables->RelativeRadiusPower[n] * - (WMM_get_secular_var_coeff_g(index) * - SphVariables->cos_mlambda[m] + WMM_get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) - * (float)(n + 1) * LegendreFunction->Pcup[index]; + MagneticResults->Bz -= + SphVariables->RelativeRadiusPower[n] * + (WMM_get_secular_var_coeff_g(index) * + SphVariables->cos_mlambda[m] + WMM_get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) + * (float)(n + 1) * LegendreFunction->Pcup[index]; /* 1 nMax (n+2) n m m m - By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) - n=1 m=0 n n n */ + By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) + n=1 m=0 n n n */ /* Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (WMM_get_secular_var_coeff_g(index) * - SphVariables->sin_mlambda[m] - WMM_get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[m]) - * (float)(m) * LegendreFunction->Pcup[index]; + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (WMM_get_secular_var_coeff_g(index) * + SphVariables->sin_mlambda[m] - WMM_get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[m]) + * (float)(m) * LegendreFunction->Pcup[index]; /* nMax (n+2) n m m m - Bx = - SUM (a/r) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) - n=1 m=0 n n n */ + Bx = - SUM (a/r) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) + n=1 m=0 n n n */ /* Derivative with respect to latitude, divided by radius. */ - MagneticResults->Bx -= - SphVariables->RelativeRadiusPower[n] * - (WMM_get_secular_var_coeff_g(index) * - SphVariables->cos_mlambda[m] + WMM_get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) - * LegendreFunction->dPcup[index]; - } - } - cos_phi = cosf(DEG2RAD(CoordSpherical->phig)); - if (fabsf(cos_phi) > 1.0e-10f) - { - MagneticResults->By = MagneticResults->By / cos_phi; - } - else - /* Special calculation for component By at Geographic poles */ - { - if (WMM_SecVarSummationSpecial(SphVariables, CoordSpherical, MagneticResults) < 0) - return -1; // error - } + MagneticResults->Bx -= + SphVariables->RelativeRadiusPower[n] * + (WMM_get_secular_var_coeff_g(index) * + SphVariables->cos_mlambda[m] + WMM_get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) + * LegendreFunction->dPcup[index]; + } + } + cos_phi = cosf(DEG2RAD(CoordSpherical->phig)); + if (fabsf(cos_phi) > 1.0e-10f) { + MagneticResults->By = MagneticResults->By / cos_phi; + } else { + /* Special calculation for component By at Geographic poles */ + if (WMM_SecVarSummationSpecial(SphVariables, CoordSpherical, MagneticResults) < 0) { + return -1; // error + } + } - return 0; // OK + return 0; // OK } -int WMM_RotateMagneticVector(WMMtype_CoordSpherical * CoordSpherical, - WMMtype_CoordGeodetic * CoordGeodetic, - WMMtype_MagneticResults * MagneticResultsSph, WMMtype_MagneticResults * MagneticResultsGeo) - /* Rotate the Magnetic Vectors to Geodetic Coordinates - Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov - Equation 16, WMM Technical report +int WMM_RotateMagneticVector(WMMtype_CoordSpherical *CoordSpherical, + WMMtype_CoordGeodetic *CoordGeodetic, + WMMtype_MagneticResults *MagneticResultsSph, WMMtype_MagneticResults *MagneticResultsGeo) +/* Rotate the Magnetic Vectors to Geodetic Coordinates + Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov + Equation 16, WMM Technical report - INPUT : CoordSpherical : Data structure WMMtype_CoordSpherical with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) + INPUT : CoordSpherical : Data structure WMMtype_CoordSpherical with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) - CoordGeodetic : Data structure WMMtype_CoordGeodetic with the following elements - float lambda; (longitude) - float phi; ( geodetic latitude) - float HeightAboveEllipsoid; (height above the ellipsoid (HaE) ) - float HeightAboveGeoid;(height above the Geoid ) + CoordGeodetic : Data structure WMMtype_CoordGeodetic with the following elements + float lambda; (longitude) + float phi; ( geodetic latitude) + float HeightAboveEllipsoid; (height above the ellipsoid (HaE) ) + float HeightAboveGeoid;(height above the Geoid ) - MagneticResultsSph : Data structure WMMtype_MagneticResults with the following elements - float Bx; North - float By; East - float Bz; Down + MagneticResultsSph : Data structure WMMtype_MagneticResults with the following elements + float Bx; North + float By; East + float Bz; Down - OUTPUT: MagneticResultsGeo Pointer to the data structure WMMtype_MagneticResults, with the following elements - float Bx; North - float By; East - float Bz; Down + OUTPUT: MagneticResultsGeo Pointer to the data structure WMMtype_MagneticResults, with the following elements + float Bx; North + float By; East + float Bz; Down - CALLS : none + CALLS : none - */ + */ { - /* Difference between the spherical and Geodetic latitudes */ - float Psi = DEG2RAD(CoordSpherical->phig - CoordGeodetic->phi); + /* Difference between the spherical and Geodetic latitudes */ + float Psi = DEG2RAD(CoordSpherical->phig - CoordGeodetic->phi); - /* Rotate spherical field components to the Geodeitic system */ - MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sinf(Psi) + MagneticResultsSph->Bz * cosf(Psi); - MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cosf(Psi) - MagneticResultsSph->Bz * sinf(Psi); - MagneticResultsGeo->By = MagneticResultsSph->By; + /* Rotate spherical field components to the Geodeitic system */ + MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sinf(Psi) + MagneticResultsSph->Bz * cosf(Psi); + MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cosf(Psi) - MagneticResultsSph->Bz * sinf(Psi); + MagneticResultsGeo->By = MagneticResultsSph->By; return 0; } -int WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults * MagneticResultsGeo, WMMtype_GeoMagneticElements * GeoMagneticElements) - - /* Calculate all the Geomagnetic elements from X,Y and Z components - INPUT MagneticResultsGeo Pointer to data structure with the following elements - float Bx; ( North ) - float By; ( East ) - float Bz; ( Down ) - OUTPUT GeoMagneticElements Pointer to data structure with the following elements - float Decl; (Angle between the magnetic field vector and true north, positive east) - float Incl; Angle between the magnetic field vector and the horizontal plane, positive down - float F; Magnetic Field Strength - float H; Horizontal Magnetic Field Strength - float X; Northern component of the magnetic field vector - float Y; Eastern component of the magnetic field vector - float Z; Downward component of the magnetic field vector - CALLS : none - */ +int WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements) +/* Calculate all the Geomagnetic elements from X,Y and Z components + INPUT MagneticResultsGeo Pointer to data structure with the following elements + float Bx; ( North ) + float By; ( East ) + float Bz; ( Down ) + OUTPUT GeoMagneticElements Pointer to data structure with the following elements + float Decl; (Angle between the magnetic field vector and true north, positive east) + float Incl; Angle between the magnetic field vector and the horizontal plane, positive down + float F; Magnetic Field Strength + float H; Horizontal Magnetic Field Strength + float X; Northern component of the magnetic field vector + float Y; Eastern component of the magnetic field vector + float Z; Downward component of the magnetic field vector + CALLS : none + */ { - GeoMagneticElements->X = MagneticResultsGeo->Bx; - GeoMagneticElements->Y = MagneticResultsGeo->By; - GeoMagneticElements->Z = MagneticResultsGeo->Bz; + GeoMagneticElements->X = MagneticResultsGeo->Bx; + GeoMagneticElements->Y = MagneticResultsGeo->By; + GeoMagneticElements->Z = MagneticResultsGeo->Bz; - GeoMagneticElements->H = sqrtf(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By); - GeoMagneticElements->F = sqrtf(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz); - GeoMagneticElements->Decl = RAD2DEG(atan2f(GeoMagneticElements->Y, GeoMagneticElements->X)); - GeoMagneticElements->Incl = RAD2DEG(atan2f(GeoMagneticElements->Z, GeoMagneticElements->H)); + GeoMagneticElements->H = sqrtf(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By); + GeoMagneticElements->F = sqrtf(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz); + GeoMagneticElements->Decl = RAD2DEG(atan2f(GeoMagneticElements->Y, GeoMagneticElements->X)); + GeoMagneticElements->Incl = RAD2DEG(atan2f(GeoMagneticElements->Z, GeoMagneticElements->H)); - return 0; // OK + return 0; // OK } -int WMM_CalculateSecularVariation(WMMtype_MagneticResults * MagneticVariation, WMMtype_GeoMagneticElements * MagneticElements) +int WMM_CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements) /*This takes the Magnetic Variation in x, y, and z and uses it to calculate the secular variation of each of the Geomagnetic elements. - INPUT MagneticVariation Data structure with the following elements - float Bx; ( North ) - float By; ( East ) - float Bz; ( Down ) - OUTPUT MagneticElements Pointer to the data structure with the following elements updated - float Decldot; Yearly Rate of change in declination - float Incldot; Yearly Rate of change in inclination - float Fdot; Yearly rate of change in Magnetic field strength - float Hdot; Yearly rate of change in horizontal field strength - float Xdot; Yearly rate of change in the northern component - float Ydot; Yearly rate of change in the eastern component - float Zdot; Yearly rate of change in the downward component - float GVdot;Yearly rate of chnage in grid variation - CALLS : none + INPUT MagneticVariation Data structure with the following elements + float Bx; ( North ) + float By; ( East ) + float Bz; ( Down ) + OUTPUT MagneticElements Pointer to the data structure with the following elements updated + float Decldot; Yearly Rate of change in declination + float Incldot; Yearly Rate of change in inclination + float Fdot; Yearly rate of change in Magnetic field strength + float Hdot; Yearly rate of change in horizontal field strength + float Xdot; Yearly rate of change in the northern component + float Ydot; Yearly rate of change in the eastern component + float Zdot; Yearly rate of change in the downward component + float GVdot;Yearly rate of chnage in grid variation + CALLS : none -*/ + */ { - MagneticElements->Xdot = MagneticVariation->Bx; - MagneticElements->Ydot = MagneticVariation->By; - MagneticElements->Zdot = MagneticVariation->Bz; - MagneticElements->Hdot = (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot) / MagneticElements->H; //See equation 19 in the WMM technical report - MagneticElements->Fdot = - (MagneticElements->X * MagneticElements->Xdot + - MagneticElements->Y * MagneticElements->Ydot + MagneticElements->Z * MagneticElements->Zdot) / MagneticElements->F; - MagneticElements->Decldot = - 180.0f / M_PI_F * (MagneticElements->X * MagneticElements->Ydot - - MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H); - MagneticElements->Incldot = - 180.0f / M_PI_F * (MagneticElements->H * MagneticElements->Zdot - - MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F); - MagneticElements->GVdot = MagneticElements->Decldot; + MagneticElements->Xdot = MagneticVariation->Bx; + MagneticElements->Ydot = MagneticVariation->By; + MagneticElements->Zdot = MagneticVariation->Bz; + MagneticElements->Hdot = (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot) / MagneticElements->H; // See equation 19 in the WMM technical report + MagneticElements->Fdot = + (MagneticElements->X * MagneticElements->Xdot + + MagneticElements->Y * MagneticElements->Ydot + MagneticElements->Z * MagneticElements->Zdot) / MagneticElements->F; + MagneticElements->Decldot = + 180.0f / M_PI_F * (MagneticElements->X * MagneticElements->Ydot - + MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H); + MagneticElements->Incldot = + 180.0f / M_PI_F * (MagneticElements->H * MagneticElements->Zdot - + MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F); + MagneticElements->GVdot = MagneticElements->Decldot; - return 0; // OK + return 0; // OK } int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax) - /* This function evaluates all of the Schmidt-semi normalized associated Legendre - functions up to degree nMax. The functions are initially scaled by - 10^280 sinf^m in order to minimize the effects of underflow at large m - near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299). - Note that this function performs the same operation as WMM_PcupLow. - However this function also can be used for high degree (large nMax) models. + functions up to degree nMax. The functions are initially scaled by + 10^280 sinf^m in order to minimize the effects of underflow at large m + near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299). + Note that this function performs the same operation as WMM_PcupLow. + However this function also can be used for high degree (large nMax) models. - Calling Parameters: - INPUT - nMax: Maximum spherical harmonic degree to compute. - x: cosf(colatitude) or sinf(latitude). + Calling Parameters: + INPUT + nMax: Maximum spherical harmonic degree to compute. + x: cosf(colatitude) or sinf(latitude). - OUTPUT - Pcup: A vector of all associated Legendgre polynomials evaluated at - x up to nMax. The lenght must by greater or equal to (nMax+1)*(nMax+2)/2. - dPcup: Derivative of Pcup(x) with respect to latitude + OUTPUT + Pcup: A vector of all associated Legendgre polynomials evaluated at + x up to nMax. The lenght must by greater or equal to (nMax+1)*(nMax+2)/2. + dPcup: Derivative of Pcup(x) with respect to latitude - CALLS : none - Notes: + CALLS : none + Notes: - Adopted from the FORTRAN code written by Mark Wieczorek September 25, 2005. + Adopted from the FORTRAN code written by Mark Wieczorek September 25, 2005. - Manoj Nair, Nov, 2009 Manoj.C.Nair@Noaa.Gov + Manoj Nair, Nov, 2009 Manoj.C.Nair@Noaa.Gov - Change from the previous version - The prevous version computes the derivatives as - dP(n,m)(x)/dx, where x = sinf(latitude) (or cosf(colatitude) ). - However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude. - Hence the derivatives are multiplied by sinf(latitude). - Removed the options for CS phase and normalizations. + Change from the previous version + The prevous version computes the derivatives as + dP(n,m)(x)/dx, where x = sinf(latitude) (or cosf(colatitude) ). + However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude. + Hence the derivatives are multiplied by sinf(latitude). + Removed the options for CS phase and normalizations. - Note: In geomagnetism, the derivatives of ALF are usually found with - respect to the colatitudes. Here the derivatives are found with respect - to the latitude. The difference is a sign reversal for the derivative of - the Associated Legendre Functions. + Note: In geomagnetism, the derivatives of ALF are usually found with + respect to the colatitudes. Here the derivatives are found with respect + to the latitude. The difference is a sign reversal for the derivative of + the Associated Legendre Functions. - The derivates can't be computed for latitude = |90| degrees. - */ + The derivates can't be computed for latitude = |90| degrees. + */ { - uint16_t k, kstart, m, n; - float pm2, pm1, pmm, plm, rescalem, z, scalef; + uint16_t k, kstart, m, n; + float pm2, pm1, pmm, plm, rescalem, z, scalef; - float *f1 = (float *) MALLOC(sizeof(float) * NUMPCUP); - float *f2 = (float *) MALLOC(sizeof(float) * NUMPCUP); - float *PreSqr = (float *) MALLOC(sizeof(float) * NUMPCUP); + float *f1 = (float *)MALLOC(sizeof(float) * NUMPCUP); + float *f2 = (float *)MALLOC(sizeof(float) * NUMPCUP); + float *PreSqr = (float *)MALLOC(sizeof(float) * NUMPCUP); - if (!PreSqr || !f2 || !f1) - { // memory allocation error - if (PreSqr) FREE(PreSqr); - if (f2) FREE(f2); - if (f1) FREE(f1); + if (!PreSqr || !f2 || !f1) { // memory allocation error + if (PreSqr) { + FREE(PreSqr); + } + if (f2) { + FREE(f2); + } + if (f1) { + FREE(f1); + } return -1; } @@ -834,499 +836,471 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax) * Note: OP code change to avoid floating point equality test. * Was: if (fabs(x) == 1.0) */ - if (fabsf(x) - 1.0f < 1e-9f) - { - FREE(PreSqr); - FREE(f2); - FREE(f1); + if (fabsf(x) - 1.0f < 1e-9f) { + FREE(PreSqr); + FREE(f2); + FREE(f1); - // printf("Error in PcupHigh: derivative cannot be calculated at poles\n"); - return -2; - } + // printf("Error in PcupHigh: derivative cannot be calculated at poles\n"); + return -2; + } - /* OP Change: 1.0e-280 is too small to store in a float - the compiler truncates - * it to 0.0f, which is bad as the code below divides by scalef. */ - scalef = 1.0e-20f; + /* OP Change: 1.0e-280 is too small to store in a float - the compiler truncates + * it to 0.0f, which is bad as the code below divides by scalef. */ + scalef = 1.0e-20f; - for (n = 0; n <= 2 * nMax + 1; ++n) - PreSqr[n] = sqrtf((float)(n)); + for (n = 0; n <= 2 * nMax + 1; ++n) { + PreSqr[n] = sqrtf((float)(n)); + } - k = 2; + k = 2; - for (n = 2; n <= nMax; n++) - { - k = k + 1; - f1[k] = (float)(2 * n - 1) / (float)(n); - f2[k] = (float)(n - 1) / (float)(n); - for (m = 1; m <= n - 2; m++) - { - k = k + 1; - f1[k] = (float)(2 * n - 1) / PreSqr[n + m] / PreSqr[n - m]; - f2[k] = PreSqr[n - m - 1] * PreSqr[n + m - 1] / PreSqr[n + m] / PreSqr[n - m]; - } - k = k + 2; - } + for (n = 2; n <= nMax; n++) { + k = k + 1; + f1[k] = (float)(2 * n - 1) / (float)(n); + f2[k] = (float)(n - 1) / (float)(n); + for (m = 1; m <= n - 2; m++) { + k = k + 1; + f1[k] = (float)(2 * n - 1) / PreSqr[n + m] / PreSqr[n - m]; + f2[k] = PreSqr[n - m - 1] * PreSqr[n + m - 1] / PreSqr[n + m] / PreSqr[n - m]; + } + k = k + 2; + } - /*z = sinf (geocentric latitude) */ - z = sqrtf((1.0f - x) * (1.0f + x)); - pm2 = 1.0f; - Pcup[0] = 1.0f; - dPcup[0] = 0.0f; - if (nMax == 0) - { + /*z = sinf (geocentric latitude) */ + z = sqrtf((1.0f - x) * (1.0f + x)); + pm2 = 1.0f; + Pcup[0] = 1.0f; + dPcup[0] = 0.0f; + if (nMax == 0) { FREE(PreSqr); FREE(f2); FREE(f1); return -3; } - pm1 = x; - Pcup[1] = pm1; - dPcup[1] = z; - k = 1; + pm1 = x; + Pcup[1] = pm1; + dPcup[1] = z; + k = 1; - for (n = 2; n <= nMax; n++) - { - k = k + n; - plm = f1[k] * x * pm1 - f2[k] * pm2; - Pcup[k] = plm; - dPcup[k] = (float)(n) * (pm1 - x * plm) / z; - pm2 = pm1; - pm1 = plm; - } + for (n = 2; n <= nMax; n++) { + k = k + n; + plm = f1[k] * x * pm1 - f2[k] * pm2; + Pcup[k] = plm; + dPcup[k] = (float)(n) * (pm1 - x * plm) / z; + pm2 = pm1; + pm1 = plm; + } - pmm = PreSqr[2] * scalef; - rescalem = 1.0f / scalef; - kstart = 0; + pmm = PreSqr[2] * scalef; + rescalem = 1.0f / scalef; + kstart = 0; - for (m = 1; m <= nMax - 1; ++m) - { - rescalem = rescalem * z; + for (m = 1; m <= nMax - 1; ++m) { + rescalem = rescalem * z; - /* Calculate Pcup(m,m) */ - kstart = kstart + m + 1; - pmm = pmm * PreSqr[2 * m + 1] / PreSqr[2 * m]; - Pcup[kstart] = pmm * rescalem / PreSqr[2 * m + 1]; - dPcup[kstart] = -((float)(m) * x * Pcup[kstart] / z); - pm2 = pmm / PreSqr[2 * m + 1]; - /* Calculate Pcup(m+1,m) */ - k = kstart + m + 1; - pm1 = x * PreSqr[2 * m + 1] * pm2; - Pcup[k] = pm1 * rescalem; - dPcup[k] = ((pm2 * rescalem) * PreSqr[2 * m + 1] - x * (float)(m + 1) * Pcup[k]) / z; - /* Calculate Pcup(n,m) */ - for (n = m + 2; n <= nMax; ++n) - { - k = k + n; - plm = x * f1[k] * pm1 - f2[k] * pm2; - Pcup[k] = plm * rescalem; - dPcup[k] = (PreSqr[n + m] * PreSqr[n - m] * (pm1 * rescalem) - (float)(n) * x * Pcup[k]) / z; - pm2 = pm1; - pm1 = plm; - } - } + /* Calculate Pcup(m,m) */ + kstart = kstart + m + 1; + pmm = pmm * PreSqr[2 * m + 1] / PreSqr[2 * m]; + Pcup[kstart] = pmm * rescalem / PreSqr[2 * m + 1]; + dPcup[kstart] = -((float)(m) * x * Pcup[kstart] / z); + pm2 = pmm / PreSqr[2 * m + 1]; + /* Calculate Pcup(m+1,m) */ + k = kstart + m + 1; + pm1 = x * PreSqr[2 * m + 1] * pm2; + Pcup[k] = pm1 * rescalem; + dPcup[k] = ((pm2 * rescalem) * PreSqr[2 * m + 1] - x * (float)(m + 1) * Pcup[k]) / z; + /* Calculate Pcup(n,m) */ + for (n = m + 2; n <= nMax; ++n) { + k = k + n; + plm = x * f1[k] * pm1 - f2[k] * pm2; + Pcup[k] = plm * rescalem; + dPcup[k] = (PreSqr[n + m] * PreSqr[n - m] * (pm1 * rescalem) - (float)(n) * x * Pcup[k]) / z; + pm2 = pm1; + pm1 = plm; + } + } - /* Calculate Pcup(nMax,nMax) */ - rescalem = rescalem * z; - kstart = kstart + m + 1; - pmm = pmm / PreSqr[2 * nMax]; - Pcup[kstart] = pmm * rescalem; - dPcup[kstart] = -(float)(nMax) * x * Pcup[kstart] / z; + /* Calculate Pcup(nMax,nMax) */ + rescalem = rescalem * z; + kstart = kstart + m + 1; + pmm = pmm / PreSqr[2 * nMax]; + Pcup[kstart] = pmm * rescalem; + dPcup[kstart] = -(float)(nMax) * x * Pcup[kstart] / z; - // ********* - // free allocated memory + // ********* + // free allocated memory - FREE(PreSqr); + FREE(PreSqr); FREE(f2); - FREE(f1); + FREE(f1); // ********* - return 0; // OK + return 0; // OK } int WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax) - /* This function evaluates all of the Schmidt-semi normalized associated Legendre - functions up to degree nMax. + functions up to degree nMax. - Calling Parameters: - INPUT - nMax: Maximum spherical harmonic degree to compute. - x: cosf(colatitude) or sinf(latitude). + Calling Parameters: + INPUT + nMax: Maximum spherical harmonic degree to compute. + x: cosf(colatitude) or sinf(latitude). - OUTPUT - Pcup: A vector of all associated Legendgre polynomials evaluated at - x up to nMax. - dPcup: Derivative of Pcup(x) with respect to latitude + OUTPUT + Pcup: A vector of all associated Legendgre polynomials evaluated at + x up to nMax. + dPcup: Derivative of Pcup(x) with respect to latitude - Notes: Overflow may occur if nMax > 20 , especially for high-latitudes. - Use WMM_PcupHigh for large nMax. + Notes: Overflow may occur if nMax > 20 , especially for high-latitudes. + Use WMM_PcupHigh for large nMax. Writted by Manoj Nair, June, 2009 . Manoj.C.Nair@Noaa.Gov. - Note: In geomagnetism, the derivatives of ALF are usually found with - respect to the colatitudes. Here the derivatives are found with respect - to the latitude. The difference is a sign reversal for the derivative of - the Associated Legendre Functions. -*/ + Note: In geomagnetism, the derivatives of ALF are usually found with + respect to the colatitudes. Here the derivatives are found with respect + to the latitude. The difference is a sign reversal for the derivative of + the Associated Legendre Functions. + */ { - uint16_t n, m, index, index1, index2; - float k, z; + uint16_t n, m, index, index1, index2; + float k, z; - float *schmidtQuasiNorm = (float *) MALLOC(sizeof(float) * NUMPCUP); - if (!schmidtQuasiNorm) - { // memory allocation error + float *schmidtQuasiNorm = (float *)MALLOC(sizeof(float) * NUMPCUP); + + if (!schmidtQuasiNorm) { // memory allocation error return -1; } - Pcup[0] = 1.0f; - dPcup[0] = 0.0f; + Pcup[0] = 1.0f; + dPcup[0] = 0.0f; - /*sinf (geocentric latitude) - sin_phi */ - z = sqrtf((1.0f - x) * (1.0f + x)); + /*sinf (geocentric latitude) - sin_phi */ + z = sqrtf((1.0f - x) * (1.0f + x)); - /* First, Compute the Gauss-normalized associated Legendre functions */ - for (n = 1; n <= nMax; n++) - { - for (m = 0; m <= n; m++) - { - index = (n * (n + 1) / 2 + m); - if (n == m) - { - index1 = (n - 1) * n / 2 + m - 1; - Pcup[index] = z * Pcup[index1]; - dPcup[index] = z * dPcup[index1] + x * Pcup[index1]; - } - else - if (n == 1 && m == 0) - { - index1 = (n - 1) * n / 2 + m; - Pcup[index] = x * Pcup[index1]; - dPcup[index] = x * dPcup[index1] - z * Pcup[index1]; - } - else - if (n > 1 && n != m) - { - index1 = (n - 2) * (n - 1) / 2 + m; - index2 = (n - 1) * n / 2 + m; - if (m > n - 2) - { - Pcup[index] = x * Pcup[index2]; - dPcup[index] = x * dPcup[index2] - z * Pcup[index2]; - } - else - { - k = (float)(((n - 1) * (n - 1)) - (m * m)) / (float)((2 * n - 1) - * (2 * n - 3)); - Pcup[index] = x * Pcup[index2] - k * Pcup[index1]; - dPcup[index] = x * dPcup[index2] - z * Pcup[index2] - k * dPcup[index1]; - } - } - } - } + /* First, Compute the Gauss-normalized associated Legendre functions */ + for (n = 1; n <= nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + if (n == m) { + index1 = (n - 1) * n / 2 + m - 1; + Pcup[index] = z * Pcup[index1]; + dPcup[index] = z * dPcup[index1] + x * Pcup[index1]; + } else if (n == 1 && m == 0) { + index1 = (n - 1) * n / 2 + m; + Pcup[index] = x * Pcup[index1]; + dPcup[index] = x * dPcup[index1] - z * Pcup[index1]; + } else if (n > 1 && n != m) { + index1 = (n - 2) * (n - 1) / 2 + m; + index2 = (n - 1) * n / 2 + m; + if (m > n - 2) { + Pcup[index] = x * Pcup[index2]; + dPcup[index] = x * dPcup[index2] - z * Pcup[index2]; + } else { + k = (float)(((n - 1) * (n - 1)) - (m * m)) / (float)((2 * n - 1) + * (2 * n - 3)); + Pcup[index] = x * Pcup[index2] - k * Pcup[index1]; + dPcup[index] = x * dPcup[index2] - z * Pcup[index2] - k * dPcup[index1]; + } + } + } + } /*Compute the ration between the Gauss-normalized associated Legendre - functions and the Schmidt quasi-normalized version. This is equivalent to - sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ + functions and the Schmidt quasi-normalized version. This is equivalent to + sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ - schmidtQuasiNorm[0] = 1.0f; - for (n = 1; n <= nMax; n++) - { - index = (n * (n + 1) / 2); - index1 = (n - 1) * n / 2; - /* for m = 0 */ - schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (float)(2 * n - 1) / (float)n; + schmidtQuasiNorm[0] = 1.0f; + for (n = 1; n <= nMax; n++) { + index = (n * (n + 1) / 2); + index1 = (n - 1) * n / 2; + /* for m = 0 */ + schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (float)(2 * n - 1) / (float)n; - for (m = 1; m <= n; m++) - { - index = (n * (n + 1) / 2 + m); - index1 = (n * (n + 1) / 2 + m - 1); - schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * sqrtf((float)((n - m + 1) * (m == 1 ? 2 : 1)) / (float)(n + m)); - } - - } + for (m = 1; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + index1 = (n * (n + 1) / 2 + m - 1); + schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * sqrtf((float)((n - m + 1) * (m == 1 ? 2 : 1)) / (float)(n + m)); + } + } /* Converts the Gauss-normalized associated Legendre - functions to the Schmidt quasi-normalized version using pre-computed - relation stored in the variable schmidtQuasiNorm */ + functions to the Schmidt quasi-normalized version using pre-computed + relation stored in the variable schmidtQuasiNorm */ - for (n = 1; n <= nMax; n++) - { - for (m = 0; m <= n; m++) - { - index = (n * (n + 1) / 2 + m); - Pcup[index] = Pcup[index] * schmidtQuasiNorm[index]; - dPcup[index] = -dPcup[index] * schmidtQuasiNorm[index]; - /* The sign is changed since the new WMM routines use derivative with respect to latitude - insted of co-latitude */ - } - } + for (n = 1; n <= nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + Pcup[index] = Pcup[index] * schmidtQuasiNorm[index]; + dPcup[index] = -dPcup[index] * schmidtQuasiNorm[index]; + /* The sign is changed since the new WMM routines use derivative with respect to latitude + insted of co-latitude */ + } + } - FREE(schmidtQuasiNorm); + FREE(schmidtQuasiNorm); - return 0; // OK + return 0; // OK } int WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables * - SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults) - /* Special calculation for the component By at Geographic poles. - Manoj Nair, June, 2009 manoj.c.nair@noaa.gov - INPUT: MagneticModel - SphVariables - CoordSpherical - OUTPUT: MagneticResults - CALLS : none - See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report + SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) +/* Special calculation for the component By at Geographic poles. + Manoj Nair, June, 2009 manoj.c.nair@noaa.gov + INPUT: MagneticModel + SphVariables + CoordSpherical + OUTPUT: MagneticResults + CALLS : none + See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report - */ + */ { - uint16_t n, index; - float k, sin_phi; - float schmidtQuasiNorm1; - float schmidtQuasiNorm2; - float schmidtQuasiNorm3; + uint16_t n, index; + float k, sin_phi; + float schmidtQuasiNorm1; + float schmidtQuasiNorm2; + float schmidtQuasiNorm3; - float *PcupS = (float *) MALLOC(sizeof(float) * NUMPCUPS); - if (!PcupS) - return -1; // memory allocation error + float *PcupS = (float *)MALLOC(sizeof(float) * NUMPCUPS); - PcupS[0] = 1; - schmidtQuasiNorm1 = 1.0f; + if (!PcupS) { + return -1; // memory allocation error + } + PcupS[0] = 1; + schmidtQuasiNorm1 = 1.0f; - MagneticResults->By = 0.0f; - sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); + MagneticResults->By = 0.0f; + sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); - for (n = 1; n <= MagneticModel->nMax; n++) - { + for (n = 1; n <= MagneticModel->nMax; n++) { + /*Compute the ration between the Gauss-normalized associated Legendre + functions and the Schmidt quasi-normalized version. This is equivalent to + sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ - /*Compute the ration between the Gauss-normalized associated Legendre - functions and the Schmidt quasi-normalized version. This is equivalent to - sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ - - index = (n * (n + 1) / 2 + 1); - schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n; - schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrtf((float)(n * 2) / (float)(n + 1)); - schmidtQuasiNorm1 = schmidtQuasiNorm2; - if (n == 1) - { - PcupS[n] = PcupS[n - 1]; - } - else - { - k = (float)(((n - 1) * (n - 1)) - 1) / (float)((2 * n - 1) * (2 * n - 3)); - PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; - } + index = (n * (n + 1) / 2 + 1); + schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n; + schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrtf((float)(n * 2) / (float)(n + 1)); + schmidtQuasiNorm1 = schmidtQuasiNorm2; + if (n == 1) { + PcupS[n] = PcupS[n - 1]; + } else { + k = (float)(((n - 1) * (n - 1)) - 1) / (float)((2 * n - 1) * (2 * n - 3)); + PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; + } /* 1 nMax (n+2) n m m m - By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) - n=1 m=0 n n n */ + By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) + n=1 m=0 n n n */ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (WMM_get_main_field_coeff_g(index) * - SphVariables->sin_mlambda[1] - WMM_get_main_field_coeff_h(index) * SphVariables->cos_mlambda[1]) - * PcupS[n] * schmidtQuasiNorm3; - } + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (WMM_get_main_field_coeff_g(index) * + SphVariables->sin_mlambda[1] - WMM_get_main_field_coeff_h(index) * SphVariables->cos_mlambda[1]) + * PcupS[n] * schmidtQuasiNorm3; + } - FREE(PcupS); + FREE(PcupS); - return 0; // OK + return 0; // OK } int WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * - SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults) + SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) { - /*Special calculation for the secular variation summation at the poles. + /*Special calculation for the secular variation summation at the poles. - INPUT: MagneticModel - SphVariables - CoordSpherical - OUTPUT: MagneticResults - CALLS : none + INPUT: MagneticModel + SphVariables + CoordSpherical + OUTPUT: MagneticResults + CALLS : none - */ - uint16_t n, index; - float k, sin_phi; - float schmidtQuasiNorm1; - float schmidtQuasiNorm2; - float schmidtQuasiNorm3; + */ + uint16_t n, index; + float k, sin_phi; + float schmidtQuasiNorm1; + float schmidtQuasiNorm2; + float schmidtQuasiNorm3; - float *PcupS = (float *) MALLOC(sizeof(float) * NUMPCUPS); - if (!PcupS) - return -1; // memory allocation error + float *PcupS = (float *)MALLOC(sizeof(float) * NUMPCUPS); - PcupS[0] = 1; - schmidtQuasiNorm1 = 1.0f; + if (!PcupS) { + return -1; // memory allocation error + } + PcupS[0] = 1; + schmidtQuasiNorm1 = 1.0f; - MagneticResults->By = 0.0f; - sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); + MagneticResults->By = 0.0f; + sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); - for (n = 1; n <= MagneticModel->nMaxSecVar; n++) - { - index = (n * (n + 1) / 2 + 1); - schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n; - schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrtf((float)(n * 2) / (float)(n + 1)); - schmidtQuasiNorm1 = schmidtQuasiNorm2; - if (n == 1) - { - PcupS[n] = PcupS[n - 1]; - } - else - { - k = (float)(((n - 1) * (n - 1)) - 1) / (float)((2 * n - 1) * (2 * n - 3)); - PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; - } + for (n = 1; n <= MagneticModel->nMaxSecVar; n++) { + index = (n * (n + 1) / 2 + 1); + schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n; + schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrtf((float)(n * 2) / (float)(n + 1)); + schmidtQuasiNorm1 = schmidtQuasiNorm2; + if (n == 1) { + PcupS[n] = PcupS[n - 1]; + } else { + k = (float)(((n - 1) * (n - 1)) - 1) / (float)((2 * n - 1) * (2 * n - 3)); + PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; + } /* 1 nMax (n+2) n m m m - By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) - n=1 m=0 n n n */ + By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi)) + n=1 m=0 n n n */ /* Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (WMM_get_secular_var_coeff_g(index) * - SphVariables->sin_mlambda[1] - WMM_get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[1]) - * PcupS[n] * schmidtQuasiNorm3; - } + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (WMM_get_secular_var_coeff_g(index) * + SphVariables->sin_mlambda[1] - WMM_get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[1]) + * PcupS[n] * schmidtQuasiNorm3; + } - FREE(PcupS); + FREE(PcupS); - return 0; // OK + return 0; // OK } /** * @brief Comput the MainFieldCoeffH accounting for the date */ -float WMM_get_main_field_coeff_g(uint16_t index) -{ - if (index >= NUMTERMS) - return 0; +float WMM_get_main_field_coeff_g(uint16_t index) +{ + if (index >= NUMTERMS) { + return 0; + } uint16_t n, m, sum_index, a, b; float coeff = CoeffFile[index][2]; - - a = MagneticModel->nMaxSecVar; - b = (a * (a + 1) / 2 + a); - for (n = 1; n <= MagneticModel->nMax; n++) - { - for (m = 0; m <= n; m++) - { - - sum_index = (n * (n + 1) / 2 + m); - - /* Hacky for now, will solve for which conditions need summing analytically */ - if (sum_index != index) - continue; - - if (index <= b) - coeff += (decimal_date - MagneticModel->epoch) * WMM_get_secular_var_coeff_g(sum_index); - - } - } - - return coeff; + + a = MagneticModel->nMaxSecVar; + b = (a * (a + 1) / 2 + a); + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { + sum_index = (n * (n + 1) / 2 + m); + + /* Hacky for now, will solve for which conditions need summing analytically */ + if (sum_index != index) { + continue; + } + + if (index <= b) { + coeff += (decimal_date - MagneticModel->epoch) * WMM_get_secular_var_coeff_g(sum_index); + } + } + } + + return coeff; } -float WMM_get_main_field_coeff_h(uint16_t index) -{ - if (index >= NUMTERMS) - return 0; - +float WMM_get_main_field_coeff_h(uint16_t index) +{ + if (index >= NUMTERMS) { + return 0; + } + uint16_t n, m, sum_index, a, b; - float coeff = CoeffFile[index][3]; - - a = MagneticModel->nMaxSecVar; - b = (a * (a + 1) / 2 + a); - for (n = 1; n <= MagneticModel->nMax; n++) - { - for (m = 0; m <= n; m++) - { - - sum_index = (n * (n + 1) / 2 + m); - - /* Hacky for now, will solve for which conditions need summing analytically */ - if (sum_index != index) - continue; - - if (index <= b) - coeff += (decimal_date - MagneticModel->epoch) * WMM_get_secular_var_coeff_h(sum_index); - } - } - - return coeff; + float coeff = CoeffFile[index][3]; + + a = MagneticModel->nMaxSecVar; + b = (a * (a + 1) / 2 + a); + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { + sum_index = (n * (n + 1) / 2 + m); + + /* Hacky for now, will solve for which conditions need summing analytically */ + if (sum_index != index) { + continue; + } + + if (index <= b) { + coeff += (decimal_date - MagneticModel->epoch) * WMM_get_secular_var_coeff_h(sum_index); + } + } + } + + return coeff; } -float WMM_get_secular_var_coeff_g(uint16_t index) -{ - if (index >= NUMTERMS) - return 0; - - return CoeffFile[index][4]; +float WMM_get_secular_var_coeff_g(uint16_t index) +{ + if (index >= NUMTERMS) { + return 0; + } + + return CoeffFile[index][4]; } -float WMM_get_secular_var_coeff_h(uint16_t index) -{ - if (index >= NUMTERMS) - return 0; - - return CoeffFile[index][5]; +float WMM_get_secular_var_coeff_h(uint16_t index) +{ + if (index >= NUMTERMS) { + return 0; + } + + return CoeffFile[index][5]; } int WMM_DateToYear(uint16_t month, uint16_t day, uint16_t year) // Converts a given calendar date into a decimal year { - uint16_t temp = 0; // Total number of days - uint16_t MonthDays[13] = { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; - uint16_t ExtraDay = 0; - uint16_t i; + uint16_t temp = 0; // Total number of days + uint16_t MonthDays[13] = { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; + uint16_t ExtraDay = 0; + uint16_t i; - if ((year % 4 == 0 && year % 100 != 0) || (year % 400 == 0)) - ExtraDay = 1; - MonthDays[2] += ExtraDay; + if ((year % 4 == 0 && year % 100 != 0) || (year % 400 == 0)) { + ExtraDay = 1; + } + MonthDays[2] += ExtraDay; - /******************Validation********************************/ + /******************Validation********************************/ - if (month <= 0 || month > 12) - return -1; // error + if (month <= 0 || month > 12) { + return -1; // error + } + if (day <= 0 || day > MonthDays[month]) { + return -2; // error + } + /****************Calculation of t***************************/ + for (i = 1; i <= month; i++) { + temp += MonthDays[i - 1]; + } + temp += day; - if (day <= 0 || day > MonthDays[month]) - return -2; // error + decimal_date = year + (temp - 1) / (365.0f + ExtraDay); - /****************Calculation of t***************************/ - for (i = 1; i <= month; i++) - temp += MonthDays[i - 1]; - temp += day; - - decimal_date = year + (temp - 1) / (365.0f + ExtraDay); - - return 0; // OK + return 0; // OK } -int WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_CoordSpherical * CoordSpherical) +int WMM_GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical) // Converts Geodetic coordinates to Spherical coordinates // Convert geodetic coordinates, (defined by the WGS-84 // reference ellipsoid), to Earth Centered Earth Fixed Cartesian // coordinates, and then to spherical coordinates. { - float CosLat, SinLat, rc, xp, zp; // all local variables + float CosLat, SinLat, rc, xp, zp; // all local variables - CosLat = cosf(DEG2RAD(CoordGeodetic->phi)); - SinLat = sinf(DEG2RAD(CoordGeodetic->phi)); + CosLat = cosf(DEG2RAD(CoordGeodetic->phi)); + SinLat = sinf(DEG2RAD(CoordGeodetic->phi)); - // compute the local radius of curvature on the WGS-84 reference ellipsoid - rc = Ellip->a / sqrtf(1.0f - Ellip->epssq * SinLat * SinLat); + // compute the local radius of curvature on the WGS-84 reference ellipsoid + rc = Ellip->a / sqrtf(1.0f - Ellip->epssq * SinLat * SinLat); - // compute ECEF Cartesian coordinates of specified point (for longitude=0) + // compute ECEF Cartesian coordinates of specified point (for longitude=0) - xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat; - zp = (rc * (1.0f - Ellip->epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat; + xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat; + zp = (rc * (1.0f - Ellip->epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat; - // compute spherical radius and angle lambda and phi of specified point + // compute spherical radius and angle lambda and phi of specified point - CoordSpherical->r = sqrtf(xp * xp + zp * zp); - CoordSpherical->phig = RAD2DEG(asinf(zp / CoordSpherical->r)); // geocentric latitude - CoordSpherical->lambda = CoordGeodetic->lambda; // longitude + CoordSpherical->r = sqrtf(xp * xp + zp * zp); + CoordSpherical->phig = RAD2DEG(asinf(zp / CoordSpherical->r)); // geocentric latitude + CoordSpherical->lambda = CoordGeodetic->lambda; // longitude - return 0; // OK + return 0; // OK } diff --git a/flight/libraries/aes.c b/flight/libraries/aes.c index c89636dfa..001b1d8eb 100644 --- a/flight/libraries/aes.c +++ b/flight/libraries/aes.c @@ -1,288 +1,291 @@ - // http://gladman.plushost.co.uk/oldsite/AES/index.php #include #include "aes.h" -#define BPOLY 0x1B -#define DPOLY 0x8D +#define BPOLY 0x1B +#define DPOLY 0x8D -const uint8_t sbox[256] = -{ - 0x63,0x7c,0x77,0x7b,0xf2,0x6b,0x6f,0xc5,0x30,0x01,0x67,0x2b,0xfe,0xd7,0xab,0x76, - 0xca,0x82,0xc9,0x7d,0xfa,0x59,0x47,0xf0,0xad,0xd4,0xa2,0xaf,0x9c,0xa4,0x72,0xc0, - 0xb7,0xfd,0x93,0x26,0x36,0x3f,0xf7,0xcc,0x34,0xa5,0xe5,0xf1,0x71,0xd8,0x31,0x15, - 0x04,0xc7,0x23,0xc3,0x18,0x96,0x05,0x9a,0x07,0x12,0x80,0xe2,0xeb,0x27,0xb2,0x75, - 0x09,0x83,0x2c,0x1a,0x1b,0x6e,0x5a,0xa0,0x52,0x3b,0xd6,0xb3,0x29,0xe3,0x2f,0x84, - 0x53,0xd1,0x00,0xed,0x20,0xfc,0xb1,0x5b,0x6a,0xcb,0xbe,0x39,0x4a,0x4c,0x58,0xcf, - 0xd0,0xef,0xaa,0xfb,0x43,0x4d,0x33,0x85,0x45,0xf9,0x02,0x7f,0x50,0x3c,0x9f,0xa8, - 0x51,0xa3,0x40,0x8f,0x92,0x9d,0x38,0xf5,0xbc,0xb6,0xda,0x21,0x10,0xff,0xf3,0xd2, - 0xcd,0x0c,0x13,0xec,0x5f,0x97,0x44,0x17,0xc4,0xa7,0x7e,0x3d,0x64,0x5d,0x19,0x73, - 0x60,0x81,0x4f,0xdc,0x22,0x2a,0x90,0x88,0x46,0xee,0xb8,0x14,0xde,0x5e,0x0b,0xdb, - 0xe0,0x32,0x3a,0x0a,0x49,0x06,0x24,0x5c,0xc2,0xd3,0xac,0x62,0x91,0x95,0xe4,0x79, - 0xe7,0xc8,0x37,0x6d,0x8d,0xd5,0x4e,0xa9,0x6c,0x56,0xf4,0xea,0x65,0x7a,0xae,0x08, - 0xba,0x78,0x25,0x2e,0x1c,0xa6,0xb4,0xc6,0xe8,0xdd,0x74,0x1f,0x4b,0xbd,0x8b,0x8a, - 0x70,0x3e,0xb5,0x66,0x48,0x03,0xf6,0x0e,0x61,0x35,0x57,0xb9,0x86,0xc1,0x1d,0x9e, - 0xe1,0xf8,0x98,0x11,0x69,0xd9,0x8e,0x94,0x9b,0x1e,0x87,0xe9,0xce,0x55,0x28,0xdf, - 0x8c,0xa1,0x89,0x0d,0xbf,0xe6,0x42,0x68,0x41,0x99,0x2d,0x0f,0xb0,0x54,0xbb,0x16 +const uint8_t sbox[256] = { + 0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76, + 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0, + 0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15, + 0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75, + 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84, + 0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf, + 0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8, + 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2, + 0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73, + 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb, + 0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79, + 0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08, + 0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a, + 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e, + 0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf, + 0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16 }; -const uint8_t isbox[256] = -{ - 0x52,0x09,0x6a,0xd5,0x30,0x36,0xa5,0x38,0xbf,0x40,0xa3,0x9e,0x81,0xf3,0xd7,0xfb, - 0x7c,0xe3,0x39,0x82,0x9b,0x2f,0xff,0x87,0x34,0x8e,0x43,0x44,0xc4,0xde,0xe9,0xcb, - 0x54,0x7b,0x94,0x32,0xa6,0xc2,0x23,0x3d,0xee,0x4c,0x95,0x0b,0x42,0xfa,0xc3,0x4e, - 0x08,0x2e,0xa1,0x66,0x28,0xd9,0x24,0xb2,0x76,0x5b,0xa2,0x49,0x6d,0x8b,0xd1,0x25, - 0x72,0xf8,0xf6,0x64,0x86,0x68,0x98,0x16,0xd4,0xa4,0x5c,0xcc,0x5d,0x65,0xb6,0x92, - 0x6c,0x70,0x48,0x50,0xfd,0xed,0xb9,0xda,0x5e,0x15,0x46,0x57,0xa7,0x8d,0x9d,0x84, - 0x90,0xd8,0xab,0x00,0x8c,0xbc,0xd3,0x0a,0xf7,0xe4,0x58,0x05,0xb8,0xb3,0x45,0x06, - 0xd0,0x2c,0x1e,0x8f,0xca,0x3f,0x0f,0x02,0xc1,0xaf,0xbd,0x03,0x01,0x13,0x8a,0x6b, - 0x3a,0x91,0x11,0x41,0x4f,0x67,0xdc,0xea,0x97,0xf2,0xcf,0xce,0xf0,0xb4,0xe6,0x73, - 0x96,0xac,0x74,0x22,0xe7,0xad,0x35,0x85,0xe2,0xf9,0x37,0xe8,0x1c,0x75,0xdf,0x6e, - 0x47,0xf1,0x1a,0x71,0x1d,0x29,0xc5,0x89,0x6f,0xb7,0x62,0x0e,0xaa,0x18,0xbe,0x1b, - 0xfc,0x56,0x3e,0x4b,0xc6,0xd2,0x79,0x20,0x9a,0xdb,0xc0,0xfe,0x78,0xcd,0x5a,0xf4, - 0x1f,0xdd,0xa8,0x33,0x88,0x07,0xc7,0x31,0xb1,0x12,0x10,0x59,0x27,0x80,0xec,0x5f, - 0x60,0x51,0x7f,0xa9,0x19,0xb5,0x4a,0x0d,0x2d,0xe5,0x7a,0x9f,0x93,0xc9,0x9c,0xef, - 0xa0,0xe0,0x3b,0x4d,0xae,0x2a,0xf5,0xb0,0xc8,0xeb,0xbb,0x3c,0x83,0x53,0x99,0x61, - 0x17,0x2b,0x04,0x7e,0xba,0x77,0xd6,0x26,0xe1,0x69,0x14,0x63,0x55,0x21,0x0c,0x7d +const uint8_t isbox[256] = { + 0x52, 0x09, 0x6a, 0xd5, 0x30, 0x36, 0xa5, 0x38, 0xbf, 0x40, 0xa3, 0x9e, 0x81, 0xf3, 0xd7, 0xfb, + 0x7c, 0xe3, 0x39, 0x82, 0x9b, 0x2f, 0xff, 0x87, 0x34, 0x8e, 0x43, 0x44, 0xc4, 0xde, 0xe9, 0xcb, + 0x54, 0x7b, 0x94, 0x32, 0xa6, 0xc2, 0x23, 0x3d, 0xee, 0x4c, 0x95, 0x0b, 0x42, 0xfa, 0xc3, 0x4e, + 0x08, 0x2e, 0xa1, 0x66, 0x28, 0xd9, 0x24, 0xb2, 0x76, 0x5b, 0xa2, 0x49, 0x6d, 0x8b, 0xd1, 0x25, + 0x72, 0xf8, 0xf6, 0x64, 0x86, 0x68, 0x98, 0x16, 0xd4, 0xa4, 0x5c, 0xcc, 0x5d, 0x65, 0xb6, 0x92, + 0x6c, 0x70, 0x48, 0x50, 0xfd, 0xed, 0xb9, 0xda, 0x5e, 0x15, 0x46, 0x57, 0xa7, 0x8d, 0x9d, 0x84, + 0x90, 0xd8, 0xab, 0x00, 0x8c, 0xbc, 0xd3, 0x0a, 0xf7, 0xe4, 0x58, 0x05, 0xb8, 0xb3, 0x45, 0x06, + 0xd0, 0x2c, 0x1e, 0x8f, 0xca, 0x3f, 0x0f, 0x02, 0xc1, 0xaf, 0xbd, 0x03, 0x01, 0x13, 0x8a, 0x6b, + 0x3a, 0x91, 0x11, 0x41, 0x4f, 0x67, 0xdc, 0xea, 0x97, 0xf2, 0xcf, 0xce, 0xf0, 0xb4, 0xe6, 0x73, + 0x96, 0xac, 0x74, 0x22, 0xe7, 0xad, 0x35, 0x85, 0xe2, 0xf9, 0x37, 0xe8, 0x1c, 0x75, 0xdf, 0x6e, + 0x47, 0xf1, 0x1a, 0x71, 0x1d, 0x29, 0xc5, 0x89, 0x6f, 0xb7, 0x62, 0x0e, 0xaa, 0x18, 0xbe, 0x1b, + 0xfc, 0x56, 0x3e, 0x4b, 0xc6, 0xd2, 0x79, 0x20, 0x9a, 0xdb, 0xc0, 0xfe, 0x78, 0xcd, 0x5a, 0xf4, + 0x1f, 0xdd, 0xa8, 0x33, 0x88, 0x07, 0xc7, 0x31, 0xb1, 0x12, 0x10, 0x59, 0x27, 0x80, 0xec, 0x5f, + 0x60, 0x51, 0x7f, 0xa9, 0x19, 0xb5, 0x4a, 0x0d, 0x2d, 0xe5, 0x7a, 0x9f, 0x93, 0xc9, 0x9c, 0xef, + 0xa0, 0xe0, 0x3b, 0x4d, 0xae, 0x2a, 0xf5, 0xb0, 0xc8, 0xeb, 0xbb, 0x3c, 0x83, 0x53, 0x99, 0x61, + 0x17, 0x2b, 0x04, 0x7e, 0xba, 0x77, 0xd6, 0x26, 0xe1, 0x69, 0x14, 0x63, 0x55, 0x21, 0x0c, 0x7d }; -//#define xtime(x) ( (x << 1) ^ (((x >> 7) & 1) * BPOLY) ) -//#define xtime(x) ( (x << 1) ^ ((x & 0x80) ? BPOLY : 0) ) +// #define xtime(x) ( (x << 1) ^ (((x >> 7) & 1) * BPOLY) ) +// #define xtime(x) ( (x << 1) ^ ((x & 0x80) ? BPOLY : 0) ) -const uint8_t xtime[256] = -{ - 0x00,0x02,0x04,0x06,0x08,0x0a,0x0c,0x0e,0x10,0x12,0x14,0x16,0x18,0x1a,0x1c,0x1e, - 0x20,0x22,0x24,0x26,0x28,0x2a,0x2c,0x2e,0x30,0x32,0x34,0x36,0x38,0x3a,0x3c,0x3e, - 0x40,0x42,0x44,0x46,0x48,0x4a,0x4c,0x4e,0x50,0x52,0x54,0x56,0x58,0x5a,0x5c,0x5e, - 0x60,0x62,0x64,0x66,0x68,0x6a,0x6c,0x6e,0x70,0x72,0x74,0x76,0x78,0x7a,0x7c,0x7e, - 0x80,0x82,0x84,0x86,0x88,0x8a,0x8c,0x8e,0x90,0x92,0x94,0x96,0x98,0x9a,0x9c,0x9e, - 0xa0,0xa2,0xa4,0xa6,0xa8,0xaa,0xac,0xae,0xb0,0xb2,0xb4,0xb6,0xb8,0xba,0xbc,0xbe, - 0xc0,0xc2,0xc4,0xc6,0xc8,0xca,0xcc,0xce,0xd0,0xd2,0xd4,0xd6,0xd8,0xda,0xdc,0xde, - 0xe0,0xe2,0xe4,0xe6,0xe8,0xea,0xec,0xee,0xf0,0xf2,0xf4,0xf6,0xf8,0xfa,0xfc,0xfe, - 0x1b,0x19,0x1f,0x1d,0x13,0x11,0x17,0x15,0x0b,0x09,0x0f,0x0d,0x03,0x01,0x07,0x05, - 0x3b,0x39,0x3f,0x3d,0x33,0x31,0x37,0x35,0x2b,0x29,0x2f,0x2d,0x23,0x21,0x27,0x25, - 0x5b,0x59,0x5f,0x5d,0x53,0x51,0x57,0x55,0x4b,0x49,0x4f,0x4d,0x43,0x41,0x47,0x45, - 0x7b,0x79,0x7f,0x7d,0x73,0x71,0x77,0x75,0x6b,0x69,0x6f,0x6d,0x63,0x61,0x67,0x65, - 0x9b,0x99,0x9f,0x9d,0x93,0x91,0x97,0x95,0x8b,0x89,0x8f,0x8d,0x83,0x81,0x87,0x85, - 0xbb,0xb9,0xbf,0xbd,0xb3,0xb1,0xb7,0xb5,0xab,0xa9,0xaf,0xad,0xa3,0xa1,0xa7,0xa5, - 0xdb,0xd9,0xdf,0xdd,0xd3,0xd1,0xd7,0xd5,0xcb,0xc9,0xcf,0xcd,0xc3,0xc1,0xc7,0xc5, - 0xfb,0xf9,0xff,0xfd,0xf3,0xf1,0xf7,0xf5,0xeb,0xe9,0xef,0xed,0xe3,0xe1,0xe7,0xe5 +const uint8_t xtime[256] = { + 0x00, 0x02, 0x04, 0x06, 0x08, 0x0a, 0x0c, 0x0e, 0x10, 0x12, 0x14, 0x16, 0x18, 0x1a, 0x1c, 0x1e, + 0x20, 0x22, 0x24, 0x26, 0x28, 0x2a, 0x2c, 0x2e, 0x30, 0x32, 0x34, 0x36, 0x38, 0x3a, 0x3c, 0x3e, + 0x40, 0x42, 0x44, 0x46, 0x48, 0x4a, 0x4c, 0x4e, 0x50, 0x52, 0x54, 0x56, 0x58, 0x5a, 0x5c, 0x5e, + 0x60, 0x62, 0x64, 0x66, 0x68, 0x6a, 0x6c, 0x6e, 0x70, 0x72, 0x74, 0x76, 0x78, 0x7a, 0x7c, 0x7e, + 0x80, 0x82, 0x84, 0x86, 0x88, 0x8a, 0x8c, 0x8e, 0x90, 0x92, 0x94, 0x96, 0x98, 0x9a, 0x9c, 0x9e, + 0xa0, 0xa2, 0xa4, 0xa6, 0xa8, 0xaa, 0xac, 0xae, 0xb0, 0xb2, 0xb4, 0xb6, 0xb8, 0xba, 0xbc, 0xbe, + 0xc0, 0xc2, 0xc4, 0xc6, 0xc8, 0xca, 0xcc, 0xce, 0xd0, 0xd2, 0xd4, 0xd6, 0xd8, 0xda, 0xdc, 0xde, + 0xe0, 0xe2, 0xe4, 0xe6, 0xe8, 0xea, 0xec, 0xee, 0xf0, 0xf2, 0xf4, 0xf6, 0xf8, 0xfa, 0xfc, 0xfe, + 0x1b, 0x19, 0x1f, 0x1d, 0x13, 0x11, 0x17, 0x15, 0x0b, 0x09, 0x0f, 0x0d, 0x03, 0x01, 0x07, 0x05, + 0x3b, 0x39, 0x3f, 0x3d, 0x33, 0x31, 0x37, 0x35, 0x2b, 0x29, 0x2f, 0x2d, 0x23, 0x21, 0x27, 0x25, + 0x5b, 0x59, 0x5f, 0x5d, 0x53, 0x51, 0x57, 0x55, 0x4b, 0x49, 0x4f, 0x4d, 0x43, 0x41, 0x47, 0x45, + 0x7b, 0x79, 0x7f, 0x7d, 0x73, 0x71, 0x77, 0x75, 0x6b, 0x69, 0x6f, 0x6d, 0x63, 0x61, 0x67, 0x65, + 0x9b, 0x99, 0x9f, 0x9d, 0x93, 0x91, 0x97, 0x95, 0x8b, 0x89, 0x8f, 0x8d, 0x83, 0x81, 0x87, 0x85, + 0xbb, 0xb9, 0xbf, 0xbd, 0xb3, 0xb1, 0xb7, 0xb5, 0xab, 0xa9, 0xaf, 0xad, 0xa3, 0xa1, 0xa7, 0xa5, + 0xdb, 0xd9, 0xdf, 0xdd, 0xd3, 0xd1, 0xd7, 0xd5, 0xcb, 0xc9, 0xcf, 0xcd, 0xc3, 0xc1, 0xc7, 0xc5, + 0xfb, 0xf9, 0xff, 0xfd, 0xf3, 0xf1, 0xf7, 0xf5, 0xeb, 0xe9, 0xef, 0xed, 0xe3, 0xe1, 0xe7, 0xe5 }; // *********************************************************************************** /* -void createXTimeTable(void) -{ - uint8_t i = 0; - do { - xtime[i] = (i << 1) ^ ((i & 0x80) ? BPOLY : 0); - } while (++i != 0); -} -*/ + void createXTimeTable(void) + { + uint8_t i = 0; + do { + xtime[i] = (i << 1) ^ ((i & 0x80) ? BPOLY : 0); + } while (++i != 0); + } + */ // *********************************************************************************** /* -uint8_t powTable[256]; -uint8_t logTable[256]; + uint8_t powTable[256]; + uint8_t logTable[256]; -uint8_t sbox[256]; -uint8_t isbox[256]; + uint8_t sbox[256]; + uint8_t isbox[256]; -void createPowLogTables(void) -{ - uint8_t i = 0; - uint8_t t = 1; + void createPowLogTables(void) + { + uint8_t i = 0; + uint8_t t = 1; - do { - powTable[i] = t; - logTable[t] = i; - i++; - t ^= (t << 1) ^ ((t & 0x80) ? BPOLY : 0); - } while (t != 1); + do { + powTable[i] = t; + logTable[t] = i; + i++; + t ^= (t << 1) ^ ((t & 0x80) ? BPOLY : 0); + } while (t != 1); - powTable[255] = powTable[0]; -} + powTable[255] = powTable[0]; + } -void createSubstitueBoxTable(void) -{ - int i = 0; - do { - uint8_t temp; + void createSubstitueBoxTable(void) + { + int i = 0; + do { + uint8_t temp; - if (i > 0) - temp = powTable[255 - logTable[i]]; - else - temp = 0; + if (i > 0) + temp = powTable[255 - logTable[i]]; + else + temp = 0; - uint8_t sb = temp ^ 0x63; - for (int j = 0; j < 4; j++) - { - temp = (temp << 1) | (temp >> 7); - sb ^= temp; - } + uint8_t sb = temp ^ 0x63; + for (int j = 0; j < 4; j++) + { + temp = (temp << 1) | (temp >> 7); + sb ^= temp; + } - sbox[i] = sb; - } while (++i != 0); -} + sbox[i] = sb; + } while (++i != 0); + } -void createInverseSubstitueBoxTable(void) -{ - uint8_t i = 0; - uint8_t j = 0; - do { - do { - if (sbox[j] == i) - { - isbox[i] = j; - j = 255; - } - } while (++j != 0); - } while (++i != 0); -} -*/ + void createInverseSubstitueBoxTable(void) + { + uint8_t i = 0; + uint8_t j = 0; + do { + do { + if (sbox[j] == i) + { + isbox[i] = j; + j = 255; + } + } while (++j != 0); + } while (++i != 0); + } + */ // *********************************************************************************** void copy_block(void *d, void *s) { - if (d == s) return; + if (d == s) { + return; + } - register uint8_t *src = s; - register uint8_t *dest = d; - for (int i = N_BLOCK; i; --i) - *dest++ = *src++; + register uint8_t *src = s; + register uint8_t *dest = d; + for (int i = N_BLOCK; i; --i) { + *dest++ = *src++; + } } void xor_block(void *d, void *s) { - register uint8_t *src = s; - register uint8_t *dest = d; - for (int i = N_BLOCK; i; --i) - *dest++ ^= *src++; + register uint8_t *src = s; + register uint8_t *dest = d; + + for (int i = N_BLOCK; i; --i) { + *dest++ ^= *src++; + } } void xor_word(uint8_t *d, uint8_t *s) { - *d++ ^= *s++; - *d++ ^= *s++; - *d++ ^= *s++; - *d++ ^= *s++; + *d++ ^= *s++; + *d++ ^= *s++; + *d++ ^= *s++; + *d++ ^= *s++; } void xor_sub_word(uint8_t *d, uint8_t *s) { - *d++ ^= sbox[*s++]; - *d++ ^= sbox[*s++]; - *d++ ^= sbox[*s++]; - *d++ ^= sbox[*s++]; + *d++ ^= sbox[*s++]; + *d++ ^= sbox[*s++]; + *d++ ^= sbox[*s++]; + *d++ ^= sbox[*s++]; } void xor_sub_rot_word(uint8_t *d, uint8_t *s, uint8_t rc) { - *d++ ^= sbox[s[1]] ^ rc; - *d++ ^= sbox[s[2]]; - *d++ ^= sbox[s[3]]; - *d++ ^= sbox[s[0]]; + *d++ ^= sbox[s[1]] ^ rc; + *d++ ^= sbox[s[2]]; + *d++ ^= sbox[s[3]]; + *d++ ^= sbox[s[0]]; } void mix_sub_column(uint8_t *a) { - uint8_t a0 = a[0]; - uint8_t a1 = a[1]; - uint8_t a2 = a[2]; - uint8_t a3 = a[3]; - uint8_t tmp = a0 ^ a1 ^ a2 ^ a3; - a[0] = a0 ^ xtime[a0 ^ a1] ^ tmp; - a[1] = a1 ^ xtime[a1 ^ a2] ^ tmp; - a[2] = a2 ^ xtime[a2 ^ a3] ^ tmp; - a[3] = a3 ^ xtime[a3 ^ a0] ^ tmp; + uint8_t a0 = a[0]; + uint8_t a1 = a[1]; + uint8_t a2 = a[2]; + uint8_t a3 = a[3]; + uint8_t tmp = a0 ^ a1 ^ a2 ^ a3; + + a[0] = a0 ^ xtime[a0 ^ a1] ^ tmp; + a[1] = a1 ^ xtime[a1 ^ a2] ^ tmp; + a[2] = a2 ^ xtime[a2 ^ a3] ^ tmp; + a[3] = a3 ^ xtime[a3 ^ a0] ^ tmp; } void mix_sub_columns(void *a) { - mix_sub_column((uint8_t*)a + 0); - mix_sub_column((uint8_t*)a + 4); - mix_sub_column((uint8_t*)a + 8); - mix_sub_column((uint8_t*)a + 12); + mix_sub_column((uint8_t *)a + 0); + mix_sub_column((uint8_t *)a + 4); + mix_sub_column((uint8_t *)a + 8); + mix_sub_column((uint8_t *)a + 12); } void inv_mix_sub_column(uint8_t *a) { - uint8_t tmp; - tmp = xtime[xtime[a[0] ^ a[2]]]; - a[0] ^= tmp; - a[2] ^= tmp; - tmp = xtime[xtime[a[1] ^ a[3]]]; - a[1] ^= tmp; - a[3] ^= tmp; + uint8_t tmp; + + tmp = xtime[xtime[a[0] ^ a[2]]]; + a[0] ^= tmp; + a[2] ^= tmp; + tmp = xtime[xtime[a[1] ^ a[3]]]; + a[1] ^= tmp; + a[3] ^= tmp; } void inv_mix_sub_columns(void *a) { - inv_mix_sub_column((uint8_t*)a + 0); - inv_mix_sub_column((uint8_t*)a + 4); - inv_mix_sub_column((uint8_t*)a + 8); - inv_mix_sub_column((uint8_t*)a + 12); + inv_mix_sub_column((uint8_t *)a + 0); + inv_mix_sub_column((uint8_t *)a + 4); + inv_mix_sub_column((uint8_t *)a + 8); + inv_mix_sub_column((uint8_t *)a + 12); - mix_sub_columns(a); + mix_sub_columns(a); } void shift_sub_rows(uint8_t *a) { - uint8_t tmp; + uint8_t tmp; - a[0] = sbox[a[0]]; - a[4] = sbox[a[4]]; - a[8] = sbox[a[8]]; - a[12] = sbox[a[12]]; + a[0] = sbox[a[0]]; + a[4] = sbox[a[4]]; + a[8] = sbox[a[8]]; + a[12] = sbox[a[12]]; - tmp = a[1]; - a[1] = sbox[a[5]]; - a[5] = sbox[a[9]]; - a[9] = sbox[a[13]]; - a[13] = sbox[tmp]; + tmp = a[1]; + a[1] = sbox[a[5]]; + a[5] = sbox[a[9]]; + a[9] = sbox[a[13]]; + a[13] = sbox[tmp]; - tmp = a[2]; - a[2] = sbox[a[10]]; - a[10] = sbox[tmp]; - tmp = a[6]; - a[6] = sbox[a[14]]; - a[14] = sbox[tmp]; + tmp = a[2]; + a[2] = sbox[a[10]]; + a[10] = sbox[tmp]; + tmp = a[6]; + a[6] = sbox[a[14]]; + a[14] = sbox[tmp]; - tmp = a[15]; - a[15] = sbox[a[11]]; - a[11] = sbox[a[7]]; - a[7] = sbox[a[3]]; - a[3] = sbox[tmp]; + tmp = a[15]; + a[15] = sbox[a[11]]; + a[11] = sbox[a[7]]; + a[7] = sbox[a[3]]; + a[3] = sbox[tmp]; } void inv_shift_sub_rows(uint8_t *a) { - uint8_t tmp; + uint8_t tmp; - a[0] = isbox[a[0]]; - a[4] = isbox[a[4]]; - a[8] = isbox[a[8]]; - a[12] = isbox[a[12]]; + a[0] = isbox[a[0]]; + a[4] = isbox[a[4]]; + a[8] = isbox[a[8]]; + a[12] = isbox[a[12]]; - tmp = a[13]; - a[13] = isbox[a[9]]; - a[9] = isbox[a[5]]; - a[5] = isbox[a[1]]; - a[1] = isbox[tmp]; + tmp = a[13]; + a[13] = isbox[a[9]]; + a[9] = isbox[a[5]]; + a[5] = isbox[a[1]]; + a[1] = isbox[tmp]; - tmp = a[2]; - a[2] = isbox[a[10]]; - a[10] = isbox[tmp]; - tmp = a[6]; - a[6] = isbox[a[14]]; - a[14] = isbox[tmp]; + tmp = a[2]; + a[2] = isbox[a[10]]; + a[10] = isbox[tmp]; + tmp = a[6]; + a[6] = isbox[a[14]]; + a[14] = isbox[tmp]; - tmp = a[3]; - a[3] = isbox[a[7]]; - a[7] = isbox[a[11]]; - a[11] = isbox[a[15]]; - a[15] = isbox[tmp]; + tmp = a[3]; + a[3] = isbox[a[7]]; + a[7] = isbox[a[11]]; + a[11] = isbox[a[15]]; + a[15] = isbox[tmp]; } // *********************************************************************************** @@ -290,80 +293,86 @@ void inv_shift_sub_rows(uint8_t *a) // 'on the fly' encryption key update for 128 bit keys void update_encrypt_key_128(uint8_t *k, uint8_t *rc) { - xor_sub_rot_word(k + 0, k + 12, *rc); + xor_sub_rot_word(k + 0, k + 12, *rc); - *rc = (*rc << 1) ^ ((*rc & 0x80) ? BPOLY : 0); + *rc = (*rc << 1) ^ ((*rc & 0x80) ? BPOLY : 0); - for (int i = 4; i < 16; i += 4) - xor_word(k + i + 0, k + i - 4); + for (int i = 4; i < 16; i += 4) { + xor_word(k + i + 0, k + i - 4); + } } // Encrypt a single block of 16 bytes void aes_encrypt_cbc_128(void *data, void *key, void *chain_block) { - uint8_t rc = 1; + uint8_t rc = 1; - if (chain_block) - xor_block(data, chain_block); + if (chain_block) { + xor_block(data, chain_block); + } - for (int round = 10; round; --round) - { - xor_block(data, key); // add_round_key - update_encrypt_key_128((uint8_t *)key, &rc); - shift_sub_rows(data); - if (round <= 1) continue; - mix_sub_columns(data); - } - xor_block(data, key); // add_round_key + for (int round = 10; round; --round) { + xor_block(data, key); // add_round_key + update_encrypt_key_128((uint8_t *)key, &rc); + shift_sub_rows(data); + if (round <= 1) { + continue; + } + mix_sub_columns(data); + } + xor_block(data, key); // add_round_key - if (chain_block) - copy_block(chain_block, data); + if (chain_block) { + copy_block(chain_block, data); + } } // 'on the fly' decryption key update for 128 bit keys void update_decrypt_key_128(uint8_t *k, uint8_t *rc) { - for (int i = 12; i; i -= 4) - xor_word(k + i + 0, k + i - 4); + for (int i = 12; i; i -= 4) { + xor_word(k + i + 0, k + i - 4); + } - *rc = (*rc >> 1) ^ ((*rc & 1) ? DPOLY : 0); + *rc = (*rc >> 1) ^ ((*rc & 1) ? DPOLY : 0); - xor_sub_rot_word(k + 0, k + 12, *rc); + xor_sub_rot_word(k + 0, k + 12, *rc); } // Decrypt a single block of 16 bytes void aes_decrypt_cbc_128(void *data, void *key, void *chain_block) { - uint8_t tmp_data[N_BLOCK]; + uint8_t tmp_data[N_BLOCK]; - uint8_t rc = 0x6c; + uint8_t rc = 0x6c; - copy_block(tmp_data, data); + copy_block(tmp_data, data); - xor_block(data, key); // add_round_key - for (int round = 10; round; --round) - { - inv_shift_sub_rows(data); - update_decrypt_key_128(key, &rc); - xor_block(data, key); // add_round_key - if (round <= 1) continue; - inv_mix_sub_columns(data); - } + xor_block(data, key); // add_round_key + for (int round = 10; round; --round) { + inv_shift_sub_rows(data); + update_decrypt_key_128(key, &rc); + xor_block(data, key); // add_round_key + if (round <= 1) { + continue; + } + inv_mix_sub_columns(data); + } - if (chain_block) - { - xor_block(data, chain_block); - copy_block(chain_block, tmp_data); - } + if (chain_block) { + xor_block(data, chain_block); + copy_block(chain_block, tmp_data); + } } void aes_decrypt_key_128_create(void *enc_key, void *dec_key) { - copy_block(dec_key, enc_key); + copy_block(dec_key, enc_key); - uint8_t rc = 1; - for (int i = 0; i < 10; i++) - update_encrypt_key_128(dec_key, &rc); + uint8_t rc = 1; + for (int i = 0; i < 10; i++) { + update_encrypt_key_128(dec_key, &rc); + } } // *********************************************************************************** @@ -371,105 +380,112 @@ void aes_decrypt_key_128_create(void *enc_key, void *dec_key) // 'on the fly' encryption key update for 256 bit keys void update_encrypt_key_256(uint8_t *k, uint8_t *rc) { - xor_sub_rot_word(k + 0, k + 28, *rc); + xor_sub_rot_word(k + 0, k + 28, *rc); - *rc = (*rc << 1) ^ ((*rc & 0x80) ? BPOLY : 0); + *rc = (*rc << 1) ^ ((*rc & 0x80) ? BPOLY : 0); - for (int i = 4; i < 16; i += 4) - xor_word(k + i + 0, k + i - 4); + for (int i = 4; i < 16; i += 4) { + xor_word(k + i + 0, k + i - 4); + } - xor_sub_word(k + 16, k + 12); + xor_sub_word(k + 16, k + 12); - for (int i = 20; i < 32; i += 4) - xor_word(k + i + 0, k + i - 4); + for (int i = 20; i < 32; i += 4) { + xor_word(k + i + 0, k + i - 4); + } } // Encrypt a single block of 16 bytes void aes_encrypt_cbc_256(void *data, void *key, void *chain_block) { - uint8_t rc = 1; + uint8_t rc = 1; - if (chain_block) - xor_block(data, chain_block); + if (chain_block) { + xor_block(data, chain_block); + } - for (int round = 7; round; --round) - { -// printf("key - "); -// uint8_t *p = key; -// for (int i = 0; i < 32; i++) printf("%0.2X ", *p++); -// printf("\r\n"); + for (int round = 7; round; --round) { +// printf("key - "); +// uint8_t *p = key; +// for (int i = 0; i < 32; i++) printf("%0.2X ", *p++); +// printf("\r\n"); - xor_block(data, key); // add_round_key - shift_sub_rows(data); - mix_sub_columns(data); - xor_block(data, (uint8_t*)key + 16); // add_round_key - update_encrypt_key_256(key, &rc); - shift_sub_rows(data); - if (round <= 1) continue; - mix_sub_columns(data); - } - xor_block(data, key); // add_round_key + xor_block(data, key); // add_round_key + shift_sub_rows(data); + mix_sub_columns(data); + xor_block(data, (uint8_t *)key + 16); // add_round_key + update_encrypt_key_256(key, &rc); + shift_sub_rows(data); + if (round <= 1) { + continue; + } + mix_sub_columns(data); + } + xor_block(data, key); // add_round_key - if (chain_block) - copy_block(chain_block, data); + if (chain_block) { + copy_block(chain_block, data); + } } // 'on the fly' decryption key update for 256 bit keys void update_decrypt_key_256(uint8_t *k, uint8_t *rc) { - for (int i = 28; i >= 20; i -= 4) - xor_word(k + i + 0, k + i - 4); + for (int i = 28; i >= 20; i -= 4) { + xor_word(k + i + 0, k + i - 4); + } - xor_sub_word(k + 16, k + 12); + xor_sub_word(k + 16, k + 12); - for (int i = 12; i; i -= 4) - xor_word(k + i + 0, k + i - 4); + for (int i = 12; i; i -= 4) { + xor_word(k + i + 0, k + i - 4); + } - *rc = (*rc >> 1) ^ ((*rc & 1) ? DPOLY : 0); + *rc = (*rc >> 1) ^ ((*rc & 1) ? DPOLY : 0); - xor_sub_rot_word(k + 0, k + 28, *rc); + xor_sub_rot_word(k + 0, k + 28, *rc); } // Decrypt a single block of 16 bytes void aes_decrypt_cbc_256(void *data, void *key, void *chain_block) { - uint8_t tmp_data[N_BLOCK]; + uint8_t tmp_data[N_BLOCK]; - uint8_t rc = 0x80; + uint8_t rc = 0x80; - copy_block(tmp_data, data); + copy_block(tmp_data, data); - xor_block(data, key); // add_round_key - for (int round = 7; round; --round) - { - inv_shift_sub_rows(data); - update_decrypt_key_256(key, &rc); - xor_block(data, (uint8_t*)key + 16); // add_round_key - inv_mix_sub_columns(data); - inv_shift_sub_rows(data); - xor_block(data, key); // add_round_key - if (round <= 1) continue; - inv_mix_sub_columns(data); - } + xor_block(data, key); // add_round_key + for (int round = 7; round; --round) { + inv_shift_sub_rows(data); + update_decrypt_key_256(key, &rc); + xor_block(data, (uint8_t *)key + 16); // add_round_key + inv_mix_sub_columns(data); + inv_shift_sub_rows(data); + xor_block(data, key); // add_round_key + if (round <= 1) { + continue; + } + inv_mix_sub_columns(data); + } - if (chain_block) - { - xor_block(data, chain_block); - copy_block(chain_block, tmp_data); - } + if (chain_block) { + xor_block(data, chain_block); + copy_block(chain_block, tmp_data); + } } void aes_decrypt_key_256_create(void *enc_key, void *dec_key) { - if (dec_key != enc_key) - { - copy_block(dec_key, enc_key); - copy_block((uint8_t*)dec_key + 16, (uint8_t*)enc_key + 16); - } + if (dec_key != enc_key) { + copy_block(dec_key, enc_key); + copy_block((uint8_t *)dec_key + 16, (uint8_t *)enc_key + 16); + } - uint8_t rc = 1; - for (int i = 7; i; --i) - update_encrypt_key_256(dec_key, &rc); + uint8_t rc = 1; + for (int i = 7; i; --i) { + update_encrypt_key_256(dec_key, &rc); + } } // *********************************************************************************** diff --git a/flight/libraries/alarms.c b/flight/libraries/alarms.c index 54899e986..20e6753eb 100644 --- a/flight/libraries/alarms.c +++ b/flight/libraries/alarms.c @@ -48,9 +48,9 @@ int32_t AlarmsInitialize(void) SystemAlarmsInitialize(); lock = xSemaphoreCreateRecursiveMutex(); - //do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that - //AlarmsClearAll(); - //AlarmsDefaultAll(); + // do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that + // AlarmsClearAll(); + // AlarmsDefaultAll(); return 0; } @@ -82,7 +82,6 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity // Release lock xSemaphoreGiveRecursive(lock); return 0; - } /** @@ -94,9 +93,9 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity * @return 0 if success, -1 if an error */ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm, - SystemAlarmsAlarmOptions severity, - SystemAlarmsExtendedAlarmStatusOptions status, - uint8_t subStatus) + SystemAlarmsAlarmOptions severity, + SystemAlarmsExtendedAlarmStatusOptions status, + uint8_t subStatus) { SystemAlarmsData alarms; @@ -111,7 +110,7 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm, // Read alarm and update its severity only if it was changed SystemAlarmsGet(&alarms); if (alarms.Alarm[alarm] != severity) { - alarms.ExtendedAlarmStatus[alarm] = status; + alarms.ExtendedAlarmStatus[alarm] = status; alarms.ExtendedAlarmSubStatus[alarm] = subStatus; alarms.Alarm[alarm] = severity; SystemAlarmsSet(&alarms); diff --git a/flight/libraries/fifo_buffer.c b/flight/libraries/fifo_buffer.c index 7b68fcf09..396ee6ec7 100644 --- a/flight/libraries/fifo_buffer.c +++ b/flight/libraries/fifo_buffer.c @@ -31,233 +31,239 @@ // circular buffer functions uint16_t fifoBuf_getSize(t_fifo_buffer *buf) -{ // return the usable size of the buffer - +{ // return the usable size of the buffer uint16_t buf_size = buf->buf_size; - if (buf_size > 0) - return (buf_size - 1); - else - return 0; + if (buf_size > 0) { + return buf_size - 1; + } else { + return 0; + } } uint16_t fifoBuf_getUsed(t_fifo_buffer *buf) -{ // return the number of bytes available in the rx buffer - +{ // return the number of bytes available in the rx buffer uint16_t rd = buf->rd; uint16_t wr = buf->wr; - uint16_t buf_size = buf->buf_size; + uint16_t buf_size = buf->buf_size; uint16_t num_bytes = wr - rd; - if (wr < rd) + + if (wr < rd) { num_bytes = (buf_size - rd) + wr; + } return num_bytes; } uint16_t fifoBuf_getFree(t_fifo_buffer *buf) -{ // return the free space size in the buffer - - uint16_t buf_size = buf->buf_size; +{ // return the free space size in the buffer + uint16_t buf_size = buf->buf_size; uint16_t num_bytes = fifoBuf_getUsed(buf); - return ((buf_size - num_bytes) - 1); + return (buf_size - num_bytes) - 1; } void fifoBuf_clearData(t_fifo_buffer *buf) -{ // remove all data from the buffer - buf->rd = buf->wr; +{ // remove all data from the buffer + buf->rd = buf->wr; } void fifoBuf_removeData(t_fifo_buffer *buf, uint16_t len) -{ // remove a number of bytes from the buffer - +{ // remove a number of bytes from the buffer uint16_t rd = buf->rd; - uint16_t buf_size = buf->buf_size; + uint16_t buf_size = buf->buf_size; // get number of bytes available uint16_t num_bytes = fifoBuf_getUsed(buf); - if (num_bytes > len) + if (num_bytes > len) { num_bytes = len; + } - if (num_bytes < 1) - return; // nothing to remove - + if (num_bytes < 1) { + return; // nothing to remove + } rd += num_bytes; - if (rd >= buf_size) + if (rd >= buf_size) { rd -= buf_size; + } buf->rd = rd; } int16_t fifoBuf_getBytePeek(t_fifo_buffer *buf) -{ // get a data byte from the buffer without removing it - +{ // get a data byte from the buffer without removing it uint16_t rd = buf->rd; // get number of bytes available uint16_t num_bytes = fifoBuf_getUsed(buf); - if (num_bytes < 1) - return -1; // no byte retuened - - return buf->buf_ptr[rd]; // return the byte + if (num_bytes < 1) { + return -1; // no byte retuened + } + return buf->buf_ptr[rd]; // return the byte } int16_t fifoBuf_getByte(t_fifo_buffer *buf) -{ // get a data byte from the buffer - - uint16_t rd = buf->rd; - uint16_t buf_size = buf->buf_size; - uint8_t *buff = buf->buf_ptr; +{ // get a data byte from the buffer + uint16_t rd = buf->rd; + uint16_t buf_size = buf->buf_size; + uint8_t *buff = buf->buf_ptr; // get number of bytes available uint16_t num_bytes = fifoBuf_getUsed(buf); - if (num_bytes < 1) - return -1; // no byte returned - + if (num_bytes < 1) { + return -1; // no byte returned + } uint8_t b = buff[rd]; - if (++rd >= buf_size) + if (++rd >= buf_size) { rd = 0; + } buf->rd = rd; - return b; // return the byte + return b; // return the byte } uint16_t fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, uint16_t len) -{ // get data from the buffer without removing it - - uint16_t rd = buf->rd; - uint16_t buf_size = buf->buf_size; - uint8_t *buff = buf->buf_ptr; +{ // get data from the buffer without removing it + uint16_t rd = buf->rd; + uint16_t buf_size = buf->buf_size; + uint8_t *buff = buf->buf_ptr; // get number of bytes available uint16_t num_bytes = fifoBuf_getUsed(buf); - if (num_bytes > len) + if (num_bytes > len) { num_bytes = len; + } - if (num_bytes < 1) - return 0; // return number of bytes copied - + if (num_bytes < 1) { + return 0; // return number of bytes copied + } uint8_t *p = (uint8_t *)data; uint16_t i = 0; - while (num_bytes > 0) - { + while (num_bytes > 0) { uint16_t j = buf_size - rd; - if (j > num_bytes) + if (j > num_bytes) { j = num_bytes; + } memcpy(p + i, buff + rd, j); - i += j; + i += j; num_bytes -= j; rd += j; - if (rd >= buf_size) + if (rd >= buf_size) { rd = 0; + } } - return i; // return number of bytes copied + return i; // return number of bytes copied } uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len) -{ // get data from our rx buffer - - uint16_t rd = buf->rd; - uint16_t buf_size = buf->buf_size; - uint8_t *buff = buf->buf_ptr; +{ // get data from our rx buffer + uint16_t rd = buf->rd; + uint16_t buf_size = buf->buf_size; + uint8_t *buff = buf->buf_ptr; // get number of bytes available uint16_t num_bytes = fifoBuf_getUsed(buf); - if (num_bytes > len) + if (num_bytes > len) { num_bytes = len; + } - if (num_bytes < 1) - return 0; // return number of bytes copied - + if (num_bytes < 1) { + return 0; // return number of bytes copied + } uint8_t *p = (uint8_t *)data; uint16_t i = 0; - while (num_bytes > 0) - { + while (num_bytes > 0) { uint16_t j = buf_size - rd; - if (j > num_bytes) + if (j > num_bytes) { j = num_bytes; + } memcpy(p + i, buff + rd, j); - i += j; + i += j; num_bytes -= j; rd += j; - if (rd >= buf_size) + if (rd >= buf_size) { rd = 0; + } } buf->rd = rd; - return i; // return number of bytes copied + return i; // return number of bytes copied } uint16_t fifoBuf_putByte(t_fifo_buffer *buf, const uint8_t b) -{ // add a data byte to the buffer - - uint16_t wr = buf->wr; - uint16_t buf_size = buf->buf_size; - uint8_t *buff = buf->buf_ptr; +{ // add a data byte to the buffer + uint16_t wr = buf->wr; + uint16_t buf_size = buf->buf_size; + uint8_t *buff = buf->buf_ptr; uint16_t num_bytes = fifoBuf_getFree(buf); - if (num_bytes < 1) + + if (num_bytes < 1) { return 0; + } buff[wr] = b; - if (++wr >= buf_size) + if (++wr >= buf_size) { wr = 0; - - buf->wr = wr; - - return 1; // return number of bytes copied -} - -uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len) -{ // add data to the buffer - - uint16_t wr = buf->wr; - uint16_t buf_size = buf->buf_size; - uint8_t *buff = buf->buf_ptr; - - uint16_t num_bytes = fifoBuf_getFree(buf); - if (num_bytes > len) - num_bytes = len; - - if (num_bytes < 1) - return 0; // return number of bytes copied - - uint8_t *p = (uint8_t *)data; - uint16_t i = 0; - - while (num_bytes > 0) - { - uint16_t j = buf_size - wr; - if (j > num_bytes) - j = num_bytes; - memcpy(buff + wr, p + i, j); - i += j; - num_bytes -= j; - wr += j; - if (wr >= buf_size) - wr = 0; } buf->wr = wr; - return i; // return number of bytes copied + return 1; // return number of bytes copied +} + +uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len) +{ // add data to the buffer + uint16_t wr = buf->wr; + uint16_t buf_size = buf->buf_size; + uint8_t *buff = buf->buf_ptr; + + uint16_t num_bytes = fifoBuf_getFree(buf); + + if (num_bytes > len) { + num_bytes = len; + } + + if (num_bytes < 1) { + return 0; // return number of bytes copied + } + uint8_t *p = (uint8_t *)data; + uint16_t i = 0; + + while (num_bytes > 0) { + uint16_t j = buf_size - wr; + if (j > num_bytes) { + j = num_bytes; + } + memcpy(buff + wr, p + i, j); + i += j; + num_bytes -= j; + wr += j; + if (wr >= buf_size) { + wr = 0; + } + } + + buf->wr = wr; + + return i; // return number of bytes copied } void fifoBuf_init(t_fifo_buffer *buf, const void *buffer, const uint16_t buffer_size) { - buf->buf_ptr = (uint8_t *)buffer; + buf->buf_ptr = (uint8_t *)buffer; buf->rd = 0; buf->wr = 0; buf->buf_size = buffer_size; diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h index 020628d72..46b100e0f 100644 --- a/flight/libraries/inc/CoordinateConversions.h +++ b/flight/libraries/inc/CoordinateConversions.h @@ -30,45 +30,45 @@ #ifndef COORDINATECONVERSIONS_H_ #define COORDINATECONVERSIONS_H_ - // ****** convert Lat,Lon,Alt to ECEF ************ +// ****** convert Lat,Lon,Alt to ECEF ************ void LLA2ECEF(float LLA[3], float ECEF[3]); - // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* +// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* uint16_t ECEF2LLA(float ECEF[3], float LLA[3]); void RneFromLLA(float LLA[3], float Rne[3][3]); - // ****** find rotation matrix from rotation vector +// ****** find rotation matrix from rotation vector void Rv2Rot(float Rv[3], float R[3][3]); - // ****** find roll, pitch, yaw from quaternion ******** +// ****** find roll, pitch, yaw from quaternion ******** void Quaternion2RPY(const float q[4], float rpy[3]); - // ****** find quaternion from roll, pitch, yaw ******** +// ****** find quaternion from roll, pitch, yaw ******** void RPY2Quaternion(const float rpy[3], float q[4]); - //** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** void Quaternion2R(float q[4], float Rbe[3][3]); - // ****** Express LLA in a local NED Base Frame ******** +// ****** Express LLA in a local NED Base Frame ******** void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3]); - // ****** Express ECEF in a local NED Base Frame ******** +// ****** Express ECEF in a local NED Base Frame ******** void ECEF2Base(float ECEF[3], float BaseECEF[3], float Rne[3][3], float NED[3]); - // ****** convert Rotation Matrix to Quaternion ******** - // ****** if R converts from e to b, q is rotation from e to b **** +// ****** convert Rotation Matrix to Quaternion ******** +// ****** if R converts from e to b, q is rotation from e to b **** void R2Quaternion(float R[3][3], float q[4]); - // ****** Rotation Matrix from Two Vector Directions ******** - // ****** given two vector directions (v1 and v2) known in two frames (b and e) find Rbe *** - // ****** solution is approximate if can't be exact *** +// ****** Rotation Matrix from Two Vector Directions ******** +// ****** given two vector directions (v1 and v2) known in two frames (b and e) find Rbe *** +// ****** solution is approximate if can't be exact *** uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[3], const float v2e[3], float Rbe[3][3]); - // ****** Vector Cross Product ******** +// ****** Vector Cross Product ******** void CrossProduct(const float v1[3], const float v2[3], float result[3]); - // ****** Vector Magnitude ******** +// ****** Vector Magnitude ******** float VectorMagnitude(const float v[3]); void quat_inverse(float q[4]); diff --git a/flight/libraries/inc/WMMInternal.h b/flight/libraries/inc/WMMInternal.h index 5604a0773..aab5269b8 100644 --- a/flight/libraries/inc/WMMInternal.h +++ b/flight/libraries/inc/WMMInternal.h @@ -28,131 +28,130 @@ #define WMMINTERNAL_H_ #include - // internal constants -#define TRUE ((uint16_t)1) -#define FALSE ((uint16_t)0) -#define WMM_MAX_MODEL_DEGREES 12 -#define WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES 12 -#define NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES+1)*(WMM_MAX_MODEL_DEGREES+2)/2); -#define NUMPCUP 92 // NUMTERMS +1 -#define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1 +// internal constants +#define TRUE ((uint16_t)1) +#define FALSE ((uint16_t)0) +#define WMM_MAX_MODEL_DEGREES 12 +#define WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES 12 +#define NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES+1)*(WMM_MAX_MODEL_DEGREES+2)/2); +#define NUMPCUP 92 // NUMTERMS +1 +#define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1 - // internal structure definitions +// internal structure definitions typedef struct { - float EditionDate; - float epoch; //Base time of Geomagnetic model epoch (yrs) - char ModelName[20]; -// float Main_Field_Coeff_G[NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) -// float Main_Field_Coeff_H[NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) -// float Secular_Var_Coeff_G[NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) -// float Secular_Var_Coeff_H[NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) - uint16_t nMax; // Maximum degree of spherical harmonic model - uint16_t nMaxSecVar; // Maxumum degree of spherical harmonic secular model - uint16_t SecularVariationUsed; // Whether or not the magnetic secular variation vector will be needed by program + float EditionDate; + float epoch; // Base time of Geomagnetic model epoch (yrs) + char ModelName[20]; +// float Main_Field_Coeff_G[NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) +// float Main_Field_Coeff_H[NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) +// float Secular_Var_Coeff_G[NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) +// float Secular_Var_Coeff_H[NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) + uint16_t nMax; // Maximum degree of spherical harmonic model + uint16_t nMaxSecVar; // Maxumum degree of spherical harmonic secular model + uint16_t SecularVariationUsed; // Whether or not the magnetic secular variation vector will be needed by program } WMMtype_MagneticModel; typedef struct { - float a; // semi-major axis of the ellipsoid - float b; // semi-minor axis of the ellipsoid - float fla; // flattening - float epssq; // first eccentricity squared - float eps; // first eccentricity - float re; // mean radius of ellipsoid + float a; // semi-major axis of the ellipsoid + float b; // semi-minor axis of the ellipsoid + float fla; // flattening + float epssq; // first eccentricity squared + float eps; // first eccentricity + float re; // mean radius of ellipsoid } WMMtype_Ellipsoid; typedef struct { - float lambda; // longitude - float phi; // geodetic latitude - float HeightAboveEllipsoid; // height above the ellipsoid (HaE) + float lambda; // longitude + float phi; // geodetic latitude + float HeightAboveEllipsoid; // height above the ellipsoid (HaE) } WMMtype_CoordGeodetic; typedef struct { - float lambda; // longitude - float phig; // geocentric latitude - float r; // distance from the center of the ellipsoid + float lambda; // longitude + float phig; // geocentric latitude + float r; // distance from the center of the ellipsoid } WMMtype_CoordSpherical; typedef struct { - uint16_t Year; - uint16_t Month; - uint16_t Day; - float DecimalYear; + uint16_t Year; + uint16_t Month; + uint16_t Day; + float DecimalYear; } WMMtype_Date; typedef struct { - float Pcup[NUMPCUP]; // Legendre Function - float dPcup[NUMPCUP]; // Derivative of Lagendre fn + float Pcup[NUMPCUP]; // Legendre Function + float dPcup[NUMPCUP]; // Derivative of Lagendre fn } WMMtype_LegendreFunction; typedef struct { - float Bx; // North - float By; // East - float Bz; // Down + float Bx; // North + float By; // East + float Bz; // Down } WMMtype_MagneticResults; typedef struct { - - float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES + 1]; // [earth_reference_radius_km / sph. radius ]^n - float cos_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // cp(m) - cosine of (m*spherical coord. longitude - float sin_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // sp(m) - sine of (m*spherical coord. longitude) + float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES + 1]; // [earth_reference_radius_km / sph. radius ]^n + float cos_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // cp(m) - cosine of (m*spherical coord. longitude + float sin_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // sp(m) - sine of (m*spherical coord. longitude) } WMMtype_SphericalHarmonicVariables; typedef struct { - float Decl; /* 1. Angle between the magnetic field vector and true north, positive east */ - float Incl; /*2. Angle between the magnetic field vector and the horizontal plane, positive down */ - float F; /*3. Magnetic Field Strength */ - float H; /*4. Horizontal Magnetic Field Strength */ - float X; /*5. Northern component of the magnetic field vector */ - float Y; /*6. Eastern component of the magnetic field vector */ - float Z; /*7. Downward component of the magnetic field vector */ - float GV; /*8. The Grid Variation */ - float Decldot; /*9. Yearly Rate of change in declination */ - float Incldot; /*10. Yearly Rate of change in inclination */ - float Fdot; /*11. Yearly rate of change in Magnetic field strength */ - float Hdot; /*12. Yearly rate of change in horizontal field strength */ - float Xdot; /*13. Yearly rate of change in the northern component */ - float Ydot; /*14. Yearly rate of change in the eastern component */ - float Zdot; /*15. Yearly rate of change in the downward component */ - float GVdot; /*16. Yearly rate of chnage in grid variation */ + float Decl; /* 1. Angle between the magnetic field vector and true north, positive east */ + float Incl; /*2. Angle between the magnetic field vector and the horizontal plane, positive down */ + float F; /*3. Magnetic Field Strength */ + float H; /*4. Horizontal Magnetic Field Strength */ + float X; /*5. Northern component of the magnetic field vector */ + float Y; /*6. Eastern component of the magnetic field vector */ + float Z; /*7. Downward component of the magnetic field vector */ + float GV; /*8. The Grid Variation */ + float Decldot; /*9. Yearly Rate of change in declination */ + float Incldot; /*10. Yearly Rate of change in inclination */ + float Fdot; /*11. Yearly rate of change in Magnetic field strength */ + float Hdot; /*12. Yearly rate of change in horizontal field strength */ + float Xdot; /*13. Yearly rate of change in the northern component */ + float Ydot; /*14. Yearly rate of change in the eastern component */ + float Zdot; /*15. Yearly rate of change in the downward component */ + float GVdot; /*16. Yearly rate of chnage in grid variation */ } WMMtype_GeoMagneticElements; - // Internal Function Prototypes +// Internal Function Prototypes void WMM_Set_Coeff_Array(); -int WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_CoordSpherical * CoordSpherical); +int WMM_GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical); int WMM_DateToYear(uint16_t month, uint16_t day, uint16_t year); -int WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical, - WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_GeoMagneticElements * GeoMagneticElements); +int WMM_Geomag(WMMtype_CoordSpherical *CoordSpherical, + WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_GeoMagneticElements *GeoMagneticElements); -int WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical * CoordSpherical, uint16_t nMax, WMMtype_LegendreFunction * LegendreFunction); +int WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical *CoordSpherical, uint16_t nMax, WMMtype_LegendreFunction *LegendreFunction); -int WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults * MagneticResultsGeo, WMMtype_GeoMagneticElements * GeoMagneticElements); +int WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements); -int WMM_CalculateSecularVariation(WMMtype_MagneticResults * MagneticVariation, WMMtype_GeoMagneticElements * MagneticElements); +int WMM_CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements); int WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical * - CoordSpherical, uint16_t nMax, WMMtype_SphericalHarmonicVariables * SphVariables); + CoordSpherical, uint16_t nMax, WMMtype_SphericalHarmonicVariables *SphVariables); int WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax); int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax); int WMM_RotateMagneticVector(WMMtype_CoordSpherical *, - WMMtype_CoordGeodetic * CoordGeodetic, - WMMtype_MagneticResults * MagneticResultsSph, WMMtype_MagneticResults * MagneticResultsGeo); + WMMtype_CoordGeodetic *CoordGeodetic, + WMMtype_MagneticResults *MagneticResultsSph, WMMtype_MagneticResults *MagneticResultsGeo); -int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction, - WMMtype_SphericalHarmonicVariables * - SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults); +int WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables * + SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); int WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * - SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults); + SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); -int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction, - WMMtype_SphericalHarmonicVariables * SphVariables, - WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults); +int WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables *SphVariables, + WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); int WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables * - SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults); + SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); float WMM_get_main_field_coeff_g(uint16_t index); float WMM_get_main_field_coeff_h(uint16_t index); diff --git a/flight/libraries/inc/WorldMagModel.h b/flight/libraries/inc/WorldMagModel.h index 0e1405af9..6251399c3 100644 --- a/flight/libraries/inc/WorldMagModel.h +++ b/flight/libraries/inc/WorldMagModel.h @@ -27,7 +27,7 @@ #ifndef WORLDMAGMODEL_H_ #define WORLDMAGMODEL_H_ - // Exposed Function Prototypes +// Exposed Function Prototypes int WMM_Initialize(); int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, uint16_t Day, uint16_t Year, float B[3]); diff --git a/flight/libraries/inc/aes.h b/flight/libraries/inc/aes.h index 44e12ba58..43bcdb926 100644 --- a/flight/libraries/inc/aes.h +++ b/flight/libraries/inc/aes.h @@ -1,10 +1,9 @@ - #ifndef _AES_H_ #define _AES_H_ -#define N_ROW 4 -#define N_COL 4 -#define N_BLOCK (N_ROW * N_COL) +#define N_ROW 4 +#define N_COL 4 +#define N_BLOCK (N_ROW * N_COL) void aes_encrypt_cbc_128(void *data, void *key, void *chain_block); void aes_decrypt_cbc_128(void *data, void *key, void *chain_block); diff --git a/flight/libraries/inc/alarms.h b/flight/libraries/inc/alarms.h index 8bb4b8873..1d7a4ed3b 100644 --- a/flight/libraries/inc/alarms.h +++ b/flight/libraries/inc/alarms.h @@ -34,9 +34,9 @@ int32_t AlarmsInitialize(void); int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity); int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm, - SystemAlarmsAlarmOptions severity, - SystemAlarmsExtendedAlarmStatusOptions status, - uint8_t subStatus); + SystemAlarmsAlarmOptions severity, + SystemAlarmsExtendedAlarmStatusOptions status, + uint8_t subStatus); SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm); int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm); void AlarmsDefaultAll(); diff --git a/flight/libraries/inc/fifo_buffer.h b/flight/libraries/inc/fifo_buffer.h index 6ba85877e..d653529d7 100644 --- a/flight/libraries/inc/fifo_buffer.h +++ b/flight/libraries/inc/fifo_buffer.h @@ -30,9 +30,8 @@ // ********************* -typedef struct -{ - uint8_t *buf_ptr; +typedef struct { + uint8_t *buf_ptr; volatile uint16_t rd; volatile uint16_t wr; uint16_t buf_size; @@ -62,4 +61,4 @@ void fifoBuf_init(t_fifo_buffer *buf, const void *buffer, const uint16_t buffer_ // ********************* -#endif +#endif // ifndef _FIFO_BUFFER_H_ diff --git a/flight/libraries/inc/insgps.h b/flight/libraries/inc/insgps.h index ea8e11e23..c77d44510 100644 --- a/flight/libraries/inc/insgps.h +++ b/flight/libraries/inc/insgps.h @@ -1,6 +1,6 @@ /** ****************************************************************************** - * @addtogroup AHRS + * @addtogroup AHRS * @{ * @addtogroup INSGPS * @{ @@ -35,24 +35,24 @@ #include "stdint.h" /** - * @addtogroup Constants - * @{ - */ -#define POS_SENSORS 0x007 + * @addtogroup Constants + * @{ + */ +#define POS_SENSORS 0x007 #define HORIZ_POS_SENSORS 0x003 -#define VER_POS_SENSORS 0x004 -#define HORIZ_SENSORS 0x018 -#define VERT_SENSORS 0x020 -#define MAG_SENSORS 0x1C0 -#define BARO_SENSOR 0x200 +#define VER_POS_SENSORS 0x004 +#define HORIZ_SENSORS 0x018 +#define VERT_SENSORS 0x020 +#define MAG_SENSORS 0x1C0 +#define BARO_SENSOR 0x200 -#define FULL_SENSORS 0x3FF +#define FULL_SENSORS 0x3FF /** - * @} - */ + * @} + */ -// Exposed Function Prototypes +// Exposed Function Prototypes void INSGPSInit(); void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT); void INSCovariancePrediction(float dT); @@ -74,20 +74,20 @@ void INSPosVelReset(float pos[3], float vel[3]); void MagCorrection(float mag_data[3]); void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt); void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], - float BaroAlt); + float BaroAlt); void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt); void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]); void VelBaroCorrection(float Vel[3], float BaroAlt); uint16_t ins_get_num_states(); -// Nav structure containing current solution +// Nav structure containing current solution extern struct NavStruct { - float Pos[3]; // Position in meters and relative to a local NED frame - float Vel[3]; // Velocity in meters and in NED - float q[4]; // unit quaternion rotation relative to NED - float gyro_bias[3]; - float accel_bias[3]; + float Pos[3]; // Position in meters and relative to a local NED frame + float Vel[3]; // Velocity in meters and in NED + float q[4]; // unit quaternion rotation relative to NED + float gyro_bias[3]; + float accel_bias[3]; } Nav; /** diff --git a/flight/libraries/inc/op_dfu.h b/flight/libraries/inc/op_dfu.h index 262852fc4..356e6827c 100644 --- a/flight/libraries/inc/op_dfu.h +++ b/flight/libraries/inc/op_dfu.h @@ -30,23 +30,23 @@ /* Includes ------------------------------------------------------------------*/ /* Exported types ------------------------------------------------------------*/ typedef struct { - uint8_t programmingType; - uint8_t readWriteFlags; - uint32_t startOfUserCode; - uint32_t sizeOfCode; - uint8_t sizeOfDescription; - uint8_t BL_Version; - uint16_t devID; - DeviceType devType; - uint32_t FW_Crc; + uint8_t programmingType; + uint8_t readWriteFlags; + uint32_t startOfUserCode; + uint32_t sizeOfCode; + uint8_t sizeOfDescription; + uint8_t BL_Version; + uint16_t devID; + DeviceType devType; + uint32_t FW_Crc; } Device; /* Exported constants --------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ /* Exported define -----------------------------------------------------------*/ -#define COMMAND 0 -#define COUNT 1 -#define DATA 5 +#define COMMAND 0 +#define COUNT 1 +#define DATA 5 /* Exported functions ------------------------------------------------------- */ void processComand(uint8_t *Receive_Buffer); @@ -54,7 +54,7 @@ uint32_t baseOfAdressType(uint8_t type); uint8_t isBiggerThanAvailable(uint8_t type, uint32_t size); void OPDfuIni(uint8_t discover); void DataDownload(DownloadAction); -bool flash_read(uint8_t * buffer, uint32_t adr, DFUProgType type); +bool flash_read(uint8_t *buffer, uint32_t adr, DFUProgType type); #endif /* __OP_DFU_H */ /******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/ diff --git a/flight/libraries/inc/packet_handler.h b/flight/libraries/inc/packet_handler.h index 855347a20..3df4f8609 100644 --- a/flight/libraries/inc/packet_handler.h +++ b/flight/libraries/inc/packet_handler.h @@ -1,16 +1,16 @@ /** -****************************************************************************** -* @addtogroup OpenPilotSystem OpenPilot System -* @{ -* @addtogroup OpenPilotLibraries OpenPilot System Libraries -* @{ -* -* @file packet_handler.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A packet handler for handeling radio packet transmission. -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotLibraries OpenPilot System Libraries + * @{ + * + * @file packet_handler.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A packet handler for handeling radio packet transmission. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -36,70 +36,70 @@ #include // Public defines / macros -#define PHPacketSize(p) ((uint8_t*)(p->data) + p->header.data_size - (uint8_t*)p) -#define PHPacketSizeECC(p) ((uint8_t*)(p->data) + p->header.data_size + RS_ECC_NPARITY - (uint8_t*)p) +#define PHPacketSize(p) ((uint8_t *)(p->data) + p->header.data_size - (uint8_t *)p) +#define PHPacketSizeECC(p) ((uint8_t *)(p->data) + p->header.data_size + RS_ECC_NPARITY - (uint8_t *)p) // Public types typedef enum { PACKET_TYPE_NONE = 0, - PACKET_TYPE_STATUS, // broadcasts status of this modem - PACKET_TYPE_CON_REQUEST, // request a connection to another modem - PACKET_TYPE_DATA, // data packet (packet contains user data) - PACKET_TYPE_DUPLICATE_DATA, // a duplicate data packet - PACKET_TYPE_PPM, // PPM relay values - PACKET_TYPE_ACK, // Acknowlege the receipt of a packet - PACKET_TYPE_NACK, // Acknowlege the receipt of an uncorrectable packet + PACKET_TYPE_STATUS, // broadcasts status of this modem + PACKET_TYPE_CON_REQUEST, // request a connection to another modem + PACKET_TYPE_DATA, // data packet (packet contains user data) + PACKET_TYPE_DUPLICATE_DATA, // a duplicate data packet + PACKET_TYPE_PPM, // PPM relay values + PACKET_TYPE_ACK, // Acknowlege the receipt of a packet + PACKET_TYPE_NACK, // Acknowlege the receipt of an uncorrectable packet } PHPacketType; typedef struct { - uint32_t destination_id; + uint32_t destination_id; portTickType prev_tx_time; - uint16_t seq_num; + uint16_t seq_num; uint8_t type; uint8_t data_size; } PHPacketHeader; #define PH_MAX_DATA (PIOS_PH_MAX_PACKET - sizeof(PHPacketHeader) - RS_ECC_NPARITY) -#define PH_PACKET_SIZE(p) ((p)->data + (p)->header.data_size - (uint8_t*)(p) + RS_ECC_NPARITY) +#define PH_PACKET_SIZE(p) ((p)->data + (p)->header.data_size - (uint8_t *)(p) + RS_ECC_NPARITY) typedef struct { PHPacketHeader header; uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY]; } PHPacket, *PHPacketHandle; -#define PH_ACK_NACK_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) +#define PH_ACK_NACK_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; uint8_t ecc[RS_ECC_NPARITY]; } PHAckNackPacket, *PHAckNackPacketHandle; -#define PH_PPM_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) +#define PH_PPM_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; int16_t channels[PIOS_RFM22B_RCVR_MAX_CHANNELS]; uint8_t ecc[RS_ECC_NPARITY]; } PHPpmPacket, *PHPpmPacketHandle; -#define PH_STATUS_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) +#define PH_STATUS_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; uint32_t source_id; - uint8_t link_quality; - int8_t received_rssi; - uint8_t ecc[RS_ECC_NPARITY]; + uint8_t link_quality; + int8_t received_rssi; + uint8_t ecc[RS_ECC_NPARITY]; } PHStatusPacket, *PHStatusPacketHandle; -#define PH_CONNECTION_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) +#define PH_CONNECTION_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; - uint32_t source_id; - uint32_t min_frequency; - uint32_t max_frequency; - uint32_t channel_spacing; + uint32_t source_id; + uint32_t min_frequency; + uint32_t max_frequency; + uint32_t channel_spacing; portTickType status_rx_time; - OPLinkSettingsMainPortOptions main_port; + OPLinkSettingsMainPortOptions main_port; OPLinkSettingsFlexiPortOptions flexi_port; - OPLinkSettingsVCPPortOptions vcp_port; - OPLinkSettingsComSpeedOptions com_speed; + OPLinkSettingsVCPPortOptions vcp_port; + OPLinkSettingsComSpeedOptions com_speed; uint8_t ecc[RS_ECC_NPARITY]; } PHConnectionPacket, *PHConnectionPacketHandle; diff --git a/flight/libraries/inc/paths.h b/flight/libraries/inc/paths.h index 39ed104cd..bbb3995fe 100644 --- a/flight/libraries/inc/paths.h +++ b/flight/libraries/inc/paths.h @@ -3,7 +3,7 @@ * * @file paths.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Header for path manipulation library + * @brief Header for path manipulation library * * @see The GNU Public License (GPL) Version 3 * @@ -28,12 +28,12 @@ #define PATHS_H_ struct path_status { - float fractional_progress; - float error; - float correction_direction[2]; - float path_direction[2]; + float fractional_progress; + float error; + float correction_direction[2]; + float path_direction[2]; }; -void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode); +void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode); #endif diff --git a/flight/libraries/inc/sanitycheck.h b/flight/libraries/inc/sanitycheck.h index 1ecaf8cc2..f324486ae 100644 --- a/flight/libraries/inc/sanitycheck.h +++ b/flight/libraries/inc/sanitycheck.h @@ -32,10 +32,10 @@ #include -#define SANITYCHECK_STATUS_ERROR_NONE SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE -#define SANITYCHECK_STATUS_ERROR_FLIGHTMODE SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE +#define SANITYCHECK_STATUS_ERROR_NONE SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE +#define SANITYCHECK_STATUS_ERROR_FLIGHTMODE SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE -#define BOOTFAULT_STATUS_ERROR_NONE SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE +#define BOOTFAULT_STATUS_ERROR_NONE SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE #define BOOTFAULT_STATUS_ERROR_REQUIRE_REBOOT SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED #if (SYSTEMALARMS_EXTENDEDALARMSTATUS_NUMELEM != SYSTEMALARMS_EXTENDEDALARMSUBSTATUS_NUMELEM) || \ diff --git a/flight/libraries/inc/stopwatch.h b/flight/libraries/inc/stopwatch.h index 0029a44ce..26922dff6 100644 --- a/flight/libraries/inc/stopwatch.h +++ b/flight/libraries/inc/stopwatch.h @@ -42,9 +42,9 @@ // Prototypes ///////////////////////////////////////////////////////////////////////////// -extern s32 STOPWATCH_Init(u32 resolution, TIM_TypeDef* TIM); -extern s32 STOPWATCH_Reset(TIM_TypeDef* TIM); -extern u32 STOPWATCH_ValueGet(TIM_TypeDef* TIM); +extern s32 STOPWATCH_Init(u32 resolution, TIM_TypeDef *TIM); +extern s32 STOPWATCH_Reset(TIM_TypeDef *TIM); +extern u32 STOPWATCH_ValueGet(TIM_TypeDef *TIM); ///////////////////////////////////////////////////////////////////////////// diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index b206f1772..414cbfb33 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -1,6 +1,6 @@ /** ****************************************************************************** - * @addtogroup AHRS + * @addtogroup AHRS * @{ * @addtogroup INSGPS * @{ @@ -35,21 +35,21 @@ #include // constants/macros/typdefs -#define NUMX 13 // number of states, X is the state vector -#define NUMW 9 // number of plant noise inputs, w is disturbance noise vector -#define NUMV 10 // number of measurements, v is the measurement noise vector -#define NUMU 6 // number of deterministic inputs, U is the input vector +#define NUMX 13 // number of states, X is the state vector +#define NUMW 9 // number of plant noise inputs, w is disturbance noise vector +#define NUMV 10 // number of measurements, v is the measurement noise vector +#define NUMU 6 // number of deterministic inputs, U is the input vector // Private functions void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], - float Q[NUMW], float dT, float P[NUMX][NUMX]); + float Q[NUMW], float dT, float P[NUMX][NUMX]); void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], - float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], float K[NUMX][NUMV], - uint16_t SensorsUsed); + float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], float K[NUMX][NUMV], + uint16_t SensorsUsed); void RungeKutta(float X[NUMX], float U[NUMU], float dT); void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]); void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX], - float G[NUMX][NUMW]); + float G[NUMX][NUMW]); void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]); void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); @@ -60,7 +60,7 @@ void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); // LinearizeFG() and LinearizeH(): // // usage F: usage G: usage H: -// 0123456789abc 012345678 0123456789abc +// 0123456789abc 012345678 0123456789abc // 0...X......... ......... X............ // 1....X........ ......... .X........... // 2.....X....... ......... ..X.......... @@ -75,747 +75,752 @@ void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); // b............. .......X. // c............. ........X -static const int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 7, 6, 6, 6,13,13,13 }; -static const int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9,12,12,12,12,-1,-1,-1 }; +static const int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 7, 6, 6, 6, 13, 13, 13 }; +static const int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 }; -static const int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 }; -static const int8_t GrowMax[NUMX] = { -1,-1,-1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 }; +static const int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 }; +static const int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 }; -static const int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 }; -static const int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 }; +static const int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 }; +static const int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 }; static struct EKFData { - // linearized system matrices - float F[NUMX][NUMX]; - float G[NUMX][NUMW]; - float H[NUMV][NUMX]; - // local magnetic unit vector in NED frame - float Be[3]; - // covariance matrix and state vector - float P[NUMX][NUMX]; - float X[NUMX]; - // input noise and measurement noise variances - float Q[NUMW]; - float R[NUMV]; - float K[NUMX][NUMV]; // feedback gain matrix + // linearized system matrices + float F[NUMX][NUMX]; + float G[NUMX][NUMW]; + float H[NUMV][NUMX]; + // local magnetic unit vector in NED frame + float Be[3]; + // covariance matrix and state vector + float P[NUMX][NUMX]; + float X[NUMX]; + // input noise and measurement noise variances + float Q[NUMW]; + float R[NUMV]; + float K[NUMX][NUMV]; // feedback gain matrix } ekf; // Global variables struct NavStruct Nav; -// ************* Exposed Functions **************** -// ************************************************* +// ************* Exposed Functions **************** +// ************************************************* -uint16_t ins_get_num_states() +uint16_t ins_get_num_states() { - return NUMX; + return NUMX; } -void INSGPSInit() //pretty much just a place holder for now +void INSGPSInit() // pretty much just a place holder for now { - ekf.Be[0] = 1.0f; - ekf.Be[1] = 0.0f; - ekf.Be[2] = 0.0f; // local magnetic unit vector + ekf.Be[0] = 1.0f; + ekf.Be[1] = 0.0f; + ekf.Be[2] = 0.0f; // local magnetic unit vector - for (int i = 0; i < NUMX; i++) { - for (int j = 0; j < NUMX; j++) { - ekf.P[i][j] = 0.0f; // zero all terms - ekf.F[i][j] = 0.0f; - } - - for (int j = 0; j < NUMW; j++) - ekf.G[i][j] = 0.0f; - - for (int j = 0; j < NUMV; j++) { - ekf.H[j][i] = 0.0f; - ekf.K[i][j] = 0.0f; - } - - ekf.X[i] = 0.0f; - } - for (int i = 0; i < NUMW; i++) - ekf.Q[i] = 0.0f; - for (int i = 0; i < NUMV; i++) - ekf.R[i] = 0.0f; + for (int i = 0; i < NUMX; i++) { + for (int j = 0; j < NUMX; j++) { + ekf.P[i][j] = 0.0f; // zero all terms + ekf.F[i][j] = 0.0f; + } - - ekf.P[0][0] = ekf.P[1][1] = ekf.P[2][2] = 25.0f; // initial position variance (m^2) - ekf.P[3][3] = ekf.P[4][4] = ekf.P[5][5] = 5.0f; // initial velocity variance (m/s)^2 - ekf.P[6][6] = ekf.P[7][7] = ekf.P[8][8] = ekf.P[9][9] = 1e-5f; // initial quaternion variance - ekf.P[10][10] = ekf.P[11][11] = ekf.P[12][12] = 1e-9f; // initial gyro bias variance (rad/s)^2 + for (int j = 0; j < NUMW; j++) { + ekf.G[i][j] = 0.0f; + } - ekf.X[0] = ekf.X[1] = ekf.X[2] = ekf.X[3] = ekf.X[4] = ekf.X[5] = 0.0f; // initial pos and vel (m) - ekf.X[6] = 1.0f; - ekf.X[7] = ekf.X[8] = ekf.X[9] = 0.0f; // initial quaternion (level and North) (m/s) - ekf.X[10] = ekf.X[11] = ekf.X[12] = 0.0f; // initial gyro bias (rad/s) + for (int j = 0; j < NUMV; j++) { + ekf.H[j][i] = 0.0f; + ekf.K[i][j] = 0.0f; + } - ekf.Q[0] = ekf.Q[1] = ekf.Q[2] = 50e-4f; // gyro noise variance (rad/s)^2 - ekf.Q[3] = ekf.Q[4] = ekf.Q[5] = 0.00001f; // accelerometer noise variance (m/s^2)^2 - ekf.Q[6] = ekf.Q[7] = ekf.Q[8] = 2e-8f; // gyro bias random walk variance (rad/s^2)^2 + ekf.X[i] = 0.0f; + } + for (int i = 0; i < NUMW; i++) { + ekf.Q[i] = 0.0f; + } + for (int i = 0; i < NUMV; i++) { + ekf.R[i] = 0.0f; + } - ekf.R[0] = ekf.R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2) - ekf.R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2) - ekf.R[3] = ekf.R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2 - ekf.R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2 - ekf.R[6] = ekf.R[7] = ekf.R[8] = 0.005f; // magnetometer unit vector noise variance - ekf.R[9] = .25f; // High freq altimeter noise variance (m^2) + + ekf.P[0][0] = ekf.P[1][1] = ekf.P[2][2] = 25.0f; // initial position variance (m^2) + ekf.P[3][3] = ekf.P[4][4] = ekf.P[5][5] = 5.0f; // initial velocity variance (m/s)^2 + ekf.P[6][6] = ekf.P[7][7] = ekf.P[8][8] = ekf.P[9][9] = 1e-5f; // initial quaternion variance + ekf.P[10][10] = ekf.P[11][11] = ekf.P[12][12] = 1e-9f; // initial gyro bias variance (rad/s)^2 + + ekf.X[0] = ekf.X[1] = ekf.X[2] = ekf.X[3] = ekf.X[4] = ekf.X[5] = 0.0f; // initial pos and vel (m) + ekf.X[6] = 1.0f; + ekf.X[7] = ekf.X[8] = ekf.X[9] = 0.0f; // initial quaternion (level and North) (m/s) + ekf.X[10] = ekf.X[11] = ekf.X[12] = 0.0f; // initial gyro bias (rad/s) + + ekf.Q[0] = ekf.Q[1] = ekf.Q[2] = 50e-4f; // gyro noise variance (rad/s)^2 + ekf.Q[3] = ekf.Q[4] = ekf.Q[5] = 0.00001f; // accelerometer noise variance (m/s^2)^2 + ekf.Q[6] = ekf.Q[7] = ekf.Q[8] = 2e-8f; // gyro bias random walk variance (rad/s^2)^2 + + ekf.R[0] = ekf.R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2) + ekf.R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2) + ekf.R[3] = ekf.R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2 + ekf.R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2 + ekf.R[6] = ekf.R[7] = ekf.R[8] = 0.005f; // magnetometer unit vector noise variance + ekf.R[9] = .25f; // High freq altimeter noise variance (m^2) } void INSResetP(float PDiag[NUMX]) { - uint8_t i,j; + uint8_t i, j; - // if PDiag[i] nonzero then clear row and column and set diagonal element - for (i=0;i // constants/macros/typdefs -#define NUMX 16 // number of states, X is the state vector -#define NUMW 12 // number of plant noise inputs, w is disturbance noise vector -#define NUMV 10 // number of measurements, v is the measurement noise vector -#define NUMU 6 // number of deterministic inputs, U is the input vector +#define NUMX 16 // number of states, X is the state vector +#define NUMW 12 // number of plant noise inputs, w is disturbance noise vector +#define NUMV 10 // number of measurements, v is the measurement noise vector +#define NUMU 6 // number of deterministic inputs, U is the input vector #if defined(GENERAL_COV) -// This might trick people so I have a note here. There is a slower but bigger version of the +// This might trick people so I have a note here. There is a slower but bigger version of the // code here but won't fit when debugging disabled (requires -Os) #define COVARIANCE_PREDICTION_GENERAL #endif // Private functions void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], - float Q[NUMW], float dT, float P[NUMX][NUMX]); + float Q[NUMW], float dT, float P[NUMX][NUMX]); void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], - float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], - uint16_t SensorsUsed); + float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], + uint16_t SensorsUsed); void RungeKutta(float X[NUMX], float U[NUMU], float dT); void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]); void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX], - float G[NUMX][NUMW]); + float G[NUMX][NUMW]); void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]); void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); // Private variables -float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices - // global to init to zero and maintain zero elements -float Be[3]; // local magnetic unit vector in NED frame -float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector -float Q[NUMW], R[NUMV]; // input noise and measurement noise variances -float K[NUMX][NUMV]; // feedback gain matrix +float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices +// global to init to zero and maintain zero elements +float Be[3]; // local magnetic unit vector in NED frame +float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector +float Q[NUMW], R[NUMV]; // input noise and measurement noise variances +float K[NUMX][NUMV]; // feedback gain matrix -// ************* Exposed Functions **************** -// ************************************************* +// ************* Exposed Functions **************** +// ************************************************* -uint16_t ins_get_num_states() +uint16_t ins_get_num_states() { - return NUMX; + return NUMX; } -void INSGPSInit() //pretty much just a place holder for now +void INSGPSInit() // pretty much just a place holder for now { - Be[0] = 1.0f; - Be[1] = 0; - Be[2] = 0; // local magnetic unit vector + Be[0] = 1.0f; + Be[1] = 0; + Be[2] = 0; // local magnetic unit vector - for (int i = 0; i < NUMX; i++) { - for (int j = 0; j < NUMX; j++) { - P[i][j] = 0.0f; // zero all terms - } - } - - P[0][0] = P[1][1] = P[2][2] = 25.0f; // initial position variance (m^2) - P[3][3] = P[4][4] = P[5][5] = 5.0f; // initial velocity variance (m/s)^2 - P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5f; // initial quaternion variance - P[10][10] = P[11][11] = P[12][12] = 1e-5f; // initial gyro bias variance (rad/s)^2 + for (int i = 0; i < NUMX; i++) { + for (int j = 0; j < NUMX; j++) { + P[i][j] = 0.0f; // zero all terms + } + } - X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0.0f; // initial pos and vel (m) - X[6] = 1.0f; - X[7] = X[8] = X[9] = 0.0f; // initial quaternion (level and North) (m/s) - X[10] = X[11] = X[12] = 0.0f; // initial gyro bias (rad/s) + P[0][0] = P[1][1] = P[2][2] = 25.0f; // initial position variance (m^2) + P[3][3] = P[4][4] = P[5][5] = 5.0f; // initial velocity variance (m/s)^2 + P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5f; // initial quaternion variance + P[10][10] = P[11][11] = P[12][12] = 1e-5f; // initial gyro bias variance (rad/s)^2 - Q[0] = Q[1] = Q[2] = 50e-8f; // gyro noise variance (rad/s)^2 - Q[3] = Q[4] = Q[5] = 0.01f; // accelerometer noise variance (m/s^2)^2 - Q[6] = Q[7] = Q[8] = 2e-9f; // gyro bias random walk variance (rad/s^2)^2 - Q[9] = Q[10] = Q[11] = 2e-20f; // accel bias random walk variance (m/s^3)^2 + X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0.0f; // initial pos and vel (m) + X[6] = 1.0f; + X[7] = X[8] = X[9] = 0.0f; // initial quaternion (level and North) (m/s) + X[10] = X[11] = X[12] = 0.0f; // initial gyro bias (rad/s) - R[0] = R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2) - R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2) - R[3] = R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2 - R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2 - R[6] = R[7] = R[8] = 0.005f; // magnetometer unit vector noise variance - R[9] = .05f; // High freq altimeter noise variance (m^2) + Q[0] = Q[1] = Q[2] = 50e-8f; // gyro noise variance (rad/s)^2 + Q[3] = Q[4] = Q[5] = 0.01f; // accelerometer noise variance (m/s^2)^2 + Q[6] = Q[7] = Q[8] = 2e-9f; // gyro bias random walk variance (rad/s^2)^2 + Q[9] = Q[10] = Q[11] = 2e-20f; // accel bias random walk variance (m/s^3)^2 + + R[0] = R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2) + R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2) + R[3] = R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2 + R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2 + R[6] = R[7] = R[8] = 0.005f; // magnetometer unit vector noise variance + R[9] = .05f; // High freq altimeter noise variance (m^2) } void INSResetP(float PDiag[NUMX]) { - uint8_t i,j; + uint8_t i, j; - // if PDiag[i] nonzero then clear row and column and set diagonal element - for (i=0;i 0.01) - dT = 0.01; - - // format data for INS algo - gyro[0] = gyro_data.filtered.x; - gyro[1] = gyro_data.filtered.y; - gyro[2] = gyro_data.filtered.z; - accel[0] = accel_data.filtered.x, - accel[1] = accel_data.filtered.y, - accel[2] = accel_data.filtered.z, - - INSStatePrediction(gyro, accel, dT); - attitude_data.quaternion.q1 = Nav.q[0]; - attitude_data.quaternion.q2 = Nav.q[1]; - attitude_data.quaternion.q3 = Nav.q[2]; - attitude_data.quaternion.q4 = Nav.q[3]; - send_attitude(); // get message out quickly - INSCovariancePrediction(dT); + static uint32_t ins_last_time; + float gyro[3], accel[3], vel[3]; + float dT; + uint16_t sensors; + static uint32_t gps_far_count = 0; - PositionActualData positionActual; - PositionActualGet(&positionActual); - positionActual.North = Nav.Pos[0]; - positionActual.East = Nav.Pos[1]; - positionActual.Down = Nav.Pos[2]; - PositionActualSet(&positionActual); - - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); - velocityActual.North = Nav.Vel[0]; - velocityActual.East = Nav.Vel[1]; - velocityActual.Down = Nav.Vel[2]; - VelocityActualSet(&velocityActual); + dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6; + ins_last_time = PIOS_DELAY_GetRaw(); - sensors = 0; - - if (gps_data.updated) - { - vel[0] = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD); - vel[1] = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD); - vel[2] = 0; - - if (abs(gps_data.NED[0] - Nav.Pos[0]) > INSGPS_GPS_FAR || - abs(gps_data.NED[1] - Nav.Pos[1]) > INSGPS_GPS_FAR || - abs(gps_data.NED[2] - Nav.Pos[2]) > INSGPS_GPS_FAR || - abs(vel[0] - Nav.Vel[0]) > INSGPS_GPS_FAR || - abs(vel[1] - Nav.Vel[1]) > INSGPS_GPS_FAR) { - gps_far_count++; - total_far_count++; - gps_data.updated = false; - - if(gps_far_count > 30) { - INSPosVelReset(gps_data.NED,vel); - relocated++; - } - } - else { - sensors |= HORIZ_SENSORS | POS_SENSORS; - - /* - * When using gps need to make sure that barometer is brought into NED frame - * we should try and see if the altitude from the home location is good enough - * to use for the offset but for now starting with this conservative filter - */ - if(fabs(gps_data.NED[2] + (altitude_data.altitude - baro_offset)) > 10) { - baro_offset = gps_data.NED[2] + altitude_data.altitude; - } else { - /* IIR filter with 100 second or so tau to keep them crudely in the same frame */ - baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001; - } - gps_data.updated = false; - } - } - - if(mag_data.updated) { - sensors |= MAG_SENSORS; - mag_data.updated = false; - } - - if(altitude_data.updated) { - sensors |= BARO_SENSOR; - altitude_data.updated = false; - } - - /* - * TODO: Need to add a general sanity check for all the inputs to make sure their kosher - * although probably should occur within INS itself - */ - INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude - baro_offset, sensors); - - if(fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) { - float zeros[3] = {0,0,0}; - INSSetGyroBias(zeros); - } + // This should only happen at start up or at mode switches + if (dT > 0.01) { + dT = 0.01; + } + + // format data for INS algo + gyro[0] = gyro_data.filtered.x; + gyro[1] = gyro_data.filtered.y; + gyro[2] = gyro_data.filtered.z; + accel[0] = accel_data.filtered.x, + accel[1] = accel_data.filtered.y, + accel[2] = accel_data.filtered.z, + + INSStatePrediction(gyro, accel, dT); + attitude_data.quaternion.q1 = Nav.q[0]; + attitude_data.quaternion.q2 = Nav.q[1]; + attitude_data.quaternion.q3 = Nav.q[2]; + attitude_data.quaternion.q4 = Nav.q[3]; + send_attitude(); // get message out quickly + INSCovariancePrediction(dT); + + PositionActualData positionActual; + PositionActualGet(&positionActual); + positionActual.North = Nav.Pos[0]; + positionActual.East = Nav.Pos[1]; + positionActual.Down = Nav.Pos[2]; + PositionActualSet(&positionActual); + + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); + velocityActual.North = Nav.Vel[0]; + velocityActual.East = Nav.Vel[1]; + velocityActual.Down = Nav.Vel[2]; + VelocityActualSet(&velocityActual); + + sensors = 0; + + if (gps_data.updated) { + vel[0] = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD); + vel[1] = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD); + vel[2] = 0; + + if (abs(gps_data.NED[0] - Nav.Pos[0]) > INSGPS_GPS_FAR || + abs(gps_data.NED[1] - Nav.Pos[1]) > INSGPS_GPS_FAR || + abs(gps_data.NED[2] - Nav.Pos[2]) > INSGPS_GPS_FAR || + abs(vel[0] - Nav.Vel[0]) > INSGPS_GPS_FAR || + abs(vel[1] - Nav.Vel[1]) > INSGPS_GPS_FAR) { + gps_far_count++; + total_far_count++; + gps_data.updated = false; + + if (gps_far_count > 30) { + INSPosVelReset(gps_data.NED, vel); + relocated++; + } + } else { + sensors |= HORIZ_SENSORS | POS_SENSORS; + + /* + * When using gps need to make sure that barometer is brought into NED frame + * we should try and see if the altitude from the home location is good enough + * to use for the offset but for now starting with this conservative filter + */ + if (fabs(gps_data.NED[2] + (altitude_data.altitude - baro_offset)) > 10) { + baro_offset = gps_data.NED[2] + altitude_data.altitude; + } else { + /* IIR filter with 100 second or so tau to keep them crudely in the same frame */ + baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001; + } + gps_data.updated = false; + } + } + + if (mag_data.updated) { + sensors |= MAG_SENSORS; + mag_data.updated = false; + } + + if (altitude_data.updated) { + sensors |= BARO_SENSOR; + altitude_data.updated = false; + } + + /* + * TODO: Need to add a general sanity check for all the inputs to make sure their kosher + * although probably should occur within INS itself + */ + INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude - baro_offset, sensors); + + if (fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) { + float zeros[3] = { 0, 0, 0 }; + INSSetGyroBias(zeros); + } } /** @@ -159,101 +157,102 @@ void ins_outdoor_update() */ void ins_indoor_update() { - static uint32_t updated_without_gps = 0; - - float gyro[3], accel[3]; - float zeros[3] = {0, 0, 0}; - static uint32_t ins_last_time = 0; - uint16_t sensors = 0; - float dT; + static uint32_t updated_without_gps = 0; - dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6; - ins_last_time = PIOS_DELAY_GetRaw(); - - // This should only happen at start up or at mode switches - if(dT > 0.01) - dT = 0.01; + float gyro[3], accel[3]; + float zeros[3] = { 0, 0, 0 }; + static uint32_t ins_last_time = 0; + uint16_t sensors = 0; + float dT; - // format data for INS algo - gyro[0] = gyro_data.filtered.x; - gyro[1] = gyro_data.filtered.y; - gyro[2] = gyro_data.filtered.z; - accel[0] = accel_data.filtered.x, - accel[1] = accel_data.filtered.y, - accel[2] = accel_data.filtered.z, - - INSStatePrediction(gyro, accel, dT); - attitude_data.quaternion.q1 = Nav.q[0]; - attitude_data.quaternion.q2 = Nav.q[1]; - attitude_data.quaternion.q3 = Nav.q[2]; - attitude_data.quaternion.q4 = Nav.q[3]; - send_attitude(); // get message out quickly - INSCovariancePrediction(dT); - - /* Indoors, update with zero position and velocity and high covariance */ - sensors = HORIZ_SENSORS | VERT_SENSORS; - - if(mag_data.updated && (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR)) { - sensors |= MAG_SENSORS; - mag_data.updated = false; - } - - if(altitude_data.updated) { - sensors |= BARO_SENSOR; - altitude_data.updated = false; - } - - if(gps_data.updated) { - PositionActualData positionActual; - PositionActualGet(&positionActual); - positionActual.North = gps_data.NED[0]; - positionActual.East = gps_data.NED[1]; - positionActual.Down = Nav.Pos[2]; - PositionActualSet(&positionActual); - - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); - velocityActual.North = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD); - velocityActual.East = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD); - velocityActual.Down = Nav.Vel[2]; - VelocityActualSet(&velocityActual); - - updated_without_gps = 0; - gps_data.updated = false; - } else { - PositionActualData positionActual; - PositionActualGet(&positionActual); + dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6; + ins_last_time = PIOS_DELAY_GetRaw(); - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); + // This should only happen at start up or at mode switches + if (dT > 0.01) { + dT = 0.01; + } - positionActual.Down = Nav.Pos[2]; - velocityActual.Down = Nav.Vel[2]; + // format data for INS algo + gyro[0] = gyro_data.filtered.x; + gyro[1] = gyro_data.filtered.y; + gyro[2] = gyro_data.filtered.z; + accel[0] = accel_data.filtered.x, + accel[1] = accel_data.filtered.y, + accel[2] = accel_data.filtered.z, - if(updated_without_gps > 500) { - // After 2-3 seconds without a GPS update set velocity estimate to NAN - positionActual.North = NAN; - positionActual.East = NAN; - velocityActual.North = NAN; - velocityActual.East = NAN; - } else - updated_without_gps++; + INSStatePrediction(gyro, accel, dT); + attitude_data.quaternion.q1 = Nav.q[0]; + attitude_data.quaternion.q2 = Nav.q[1]; + attitude_data.quaternion.q3 = Nav.q[2]; + attitude_data.quaternion.q4 = Nav.q[3]; + send_attitude(); // get message out quickly + INSCovariancePrediction(dT); - PositionActualSet(&positionActual); - VelocityActualSet(&velocityActual); - } - - /* - * TODO: Need to add a general sanity check for all the inputs to make sure their kosher - * although probably should occur within INS itself - */ - INSCorrection(mag_data.scaled.axis, zeros, zeros, altitude_data.altitude, sensors); - - if(fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) { - float zeros[3] = {0,0,0}; - INSSetGyroBias(zeros); - } + /* Indoors, update with zero position and velocity and high covariance */ + sensors = HORIZ_SENSORS | VERT_SENSORS; + if (mag_data.updated && (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR)) { + sensors |= MAG_SENSORS; + mag_data.updated = false; + } + + if (altitude_data.updated) { + sensors |= BARO_SENSOR; + altitude_data.updated = false; + } + + if (gps_data.updated) { + PositionActualData positionActual; + PositionActualGet(&positionActual); + positionActual.North = gps_data.NED[0]; + positionActual.East = gps_data.NED[1]; + positionActual.Down = Nav.Pos[2]; + PositionActualSet(&positionActual); + + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); + velocityActual.North = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD); + velocityActual.East = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD); + velocityActual.Down = Nav.Vel[2]; + VelocityActualSet(&velocityActual); + + updated_without_gps = 0; + gps_data.updated = false; + } else { + PositionActualData positionActual; + PositionActualGet(&positionActual); + + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); + + positionActual.Down = Nav.Pos[2]; + velocityActual.Down = Nav.Vel[2]; + + if (updated_without_gps > 500) { + // After 2-3 seconds without a GPS update set velocity estimate to NAN + positionActual.North = NAN; + positionActual.East = NAN; + velocityActual.North = NAN; + velocityActual.East = NAN; + } else { + updated_without_gps++; + } + + PositionActualSet(&positionActual); + VelocityActualSet(&velocityActual); + } + + /* + * TODO: Need to add a general sanity check for all the inputs to make sure their kosher + * although probably should occur within INS itself + */ + INSCorrection(mag_data.scaled.axis, zeros, zeros, altitude_data.altitude, sensors); + + if (fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) { + float zeros[3] = { 0, 0, 0 }; + INSSetGyroBias(zeros); + } } /** @@ -263,63 +262,64 @@ bool inited = false; float init_q[4]; void ins_init_algorithm() { - inited = true; - float Rbe[3][3], q[4], accels[3], rpy[3], mag; - float ge[3]={0,0,-9.81}, zeros[3]={0,0,0}, Pdiag[16]={25,25,25,5,5,5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-4,1e-4,1e-4}; - bool using_mags, using_gps; - - INSGPSInit(); - - HomeLocationData home; - HomeLocationGet(&home); - - accels[0]=accel_data.filtered.x; - accels[1]=accel_data.filtered.y; - accels[2]=accel_data.filtered.z; - - using_mags = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR); - using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0); /* only use mags when valid home location */ - - using_gps = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality >= INSGPS_GPS_MINSAT); - - /* Block till a data update */ - get_accel_gyro_data(); - - /* Ensure we get mag data in a timely manner */ - uint16_t fail_count = 50; // 50 at 200 Hz is up to 0.25 sec - while(using_mags && !mag_data.updated && fail_count--) { - get_mag_data(); - get_accel_gyro_data(); - AhrsPoll(); - PIOS_DELAY_WaituS(2000); - } - using_mags &= mag_data.updated; - - if (using_mags) { - RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe); - R2Quaternion(Rbe,q); - if (using_gps) - INSSetState(gps_data.NED, zeros, q, zeros, zeros); - else - INSSetState(zeros, zeros, q, zeros, zeros); - } else { - // assume yaw = 0 - mag = VectorMagnitude(accels); - rpy[1] = asinf(-accels[0]/mag); - rpy[0] = atan2(accels[1]/mag,accels[2]/mag); - rpy[2] = 0; - RPY2Quaternion(rpy,init_q); - if (using_gps) - INSSetState(gps_data.NED, zeros, init_q, zeros, zeros); - else { - for (uint32_t i = 0; i < 5; i++) { - INSSetState(zeros, zeros, init_q, zeros, zeros); - ins_indoor_update(); - } - } - } - - INSResetP(Pdiag); - - // TODO: include initial estimate of gyro bias? + inited = true; + float Rbe[3][3], q[4], accels[3], rpy[3], mag; + float ge[3] = { 0, 0, -9.81 }, zeros[3] = { 0, 0, 0 }, Pdiag[16] = { 25, 25, 25, 5, 5, 5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-4, 1e-4, 1e-4 }; + bool using_mags, using_gps; + + INSGPSInit(); + + HomeLocationData home; + HomeLocationGet(&home); + + accels[0] = accel_data.filtered.x; + accels[1] = accel_data.filtered.y; + accels[2] = accel_data.filtered.z; + + using_mags = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR); + using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0); /* only use mags when valid home location */ + + using_gps = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality >= INSGPS_GPS_MINSAT); + + /* Block till a data update */ + get_accel_gyro_data(); + + /* Ensure we get mag data in a timely manner */ + uint16_t fail_count = 50; // 50 at 200 Hz is up to 0.25 sec + while (using_mags && !mag_data.updated && fail_count--) { + get_mag_data(); + get_accel_gyro_data(); + AhrsPoll(); + PIOS_DELAY_WaituS(2000); + } + using_mags &= mag_data.updated; + + if (using_mags) { + RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe); + R2Quaternion(Rbe, q); + if (using_gps) { + INSSetState(gps_data.NED, zeros, q, zeros, zeros); + } else { + INSSetState(zeros, zeros, q, zeros, zeros); + } + } else { + // assume yaw = 0 + mag = VectorMagnitude(accels); + rpy[1] = asinf(-accels[0] / mag); + rpy[0] = atan2(accels[1] / mag, accels[2] / mag); + rpy[2] = 0; + RPY2Quaternion(rpy, init_q); + if (using_gps) { + INSSetState(gps_data.NED, zeros, init_q, zeros, zeros); + } else { + for (uint32_t i = 0; i < 5; i++) { + INSSetState(zeros, zeros, init_q, zeros, zeros); + ins_indoor_update(); + } + } + } + + INSResetP(Pdiag); + + // TODO: include initial estimate of gyro bias? } diff --git a/flight/libraries/math/pid.c b/flight/libraries/math/pid.c index eed613a73..2797b3e14 100644 --- a/flight/libraries/math/pid.c +++ b/flight/libraries/math/pid.c @@ -32,13 +32,13 @@ #include "pid.h" #include -//! Private method +// ! Private method static float bound(float val, float range); -//! Store the shared time constant for the derivative cutoff. -static float deriv_tau = 7.9577e-3f; +// ! Store the shared time constant for the derivative cutoff. +static float deriv_tau = 7.9577e-3f; -//! Store the setpoint weight to apply for the derivative term +// ! Store the setpoint weight to apply for the derivative term static float deriv_gamma = 1.0f; /** @@ -49,22 +49,21 @@ static float deriv_gamma = 1.0f; * @returns Output the computed controller value */ float pid_apply(struct pid *pid, const float err, float dT) -{ - // Scale up accumulator by 1000 while computing to avoid losing precision - pid->iAccumulator += err * (pid->i * dT * 1000.0f); - pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); +{ + // Scale up accumulator by 1000 while computing to avoid losing precision + pid->iAccumulator += err * (pid->i * dT * 1000.0f); + pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); - // Calculate DT1 term - float diff = (err - pid->lastErr); - float dterm = 0; - pid->lastErr = err; - if(pid->d > 0.0f && dT > 0.0f) - { - dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); - pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) - } // 7.9577e-3 means 20 Hz f_cutoff - - return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm); + // Calculate DT1 term + float diff = (err - pid->lastErr); + float dterm = 0; + pid->lastErr = err; + if (pid->d > 0.0f && dT > 0.0f) { + dterm = pid->lastDer + dT / (dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); + pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) + } // 7.9577e-3 means 20 Hz f_cutoff + + return (err * pid->p) + pid->iAccumulator / 1000.0f + dterm; } /** @@ -80,23 +79,22 @@ float pid_apply(struct pid *pid, const float err, float dT) */ float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT) { - float err = setpoint - measured; - - // Scale up accumulator by 1000 while computing to avoid losing precision - pid->iAccumulator += err * (pid->i * dT * 1000.0f); - pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); + float err = setpoint - measured; - // Calculate DT1 term, - float dterm = 0; - float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr); - pid->lastErr = (deriv_gamma * setpoint - measured); - if(pid->d > 0.0f && dT > 0.0f) - { - dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); - pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) - } // 7.9577e-3 means 20 Hz f_cutoff - - return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm); + // Scale up accumulator by 1000 while computing to avoid losing precision + pid->iAccumulator += err * (pid->i * dT * 1000.0f); + pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); + + // Calculate DT1 term, + float dterm = 0; + float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr); + pid->lastErr = (deriv_gamma * setpoint - measured); + if (pid->d > 0.0f && dT > 0.0f) { + dterm = pid->lastDer + dT / (dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); + pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) + } // 7.9577e-3 means 20 Hz f_cutoff + + return (err * pid->p) + pid->iAccumulator / 1000.0f + dterm; } /** @@ -105,12 +103,13 @@ float pid_apply_setpoint(struct pid *pid, const float setpoint, const float meas */ void pid_zero(struct pid *pid) { - if (!pid) - return; + if (!pid) { + return; + } - pid->iAccumulator = 0; - pid->lastErr = 0; - pid->lastDer = 0; + pid->iAccumulator = 0; + pid->lastErr = 0; + pid->lastDer = 0; } /** @@ -120,8 +119,8 @@ void pid_zero(struct pid *pid) */ void pid_configure_derivative(float cutoff, float g) { - deriv_tau = 1.0f / (2 * M_PI_F * cutoff); - deriv_gamma = g; + deriv_tau = 1.0f / (2 * M_PI_F * cutoff); + deriv_gamma = g; } /** @@ -133,13 +132,14 @@ void pid_configure_derivative(float cutoff, float g) */ void pid_configure(struct pid *pid, float p, float i, float d, float iLim) { - if (!pid) - return; + if (!pid) { + return; + } - pid->p = p; - pid->i = i; - pid->d = d; - pid->iLim = iLim; + pid->p = p; + pid->i = i; + pid->d = d; + pid->iLim = iLim; } /** @@ -147,11 +147,10 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim) */ static float bound(float val, float range) { - if(val < -range) { - val = -range; - } else if(val > range) { - val = range; - } - return val; + if (val < -range) { + val = -range; + } else if (val > range) { + val = range; + } + return val; } - diff --git a/flight/libraries/math/pid.h b/flight/libraries/math/pid.h index 602f560bf..7872ac559 100644 --- a/flight/libraries/math/pid.h +++ b/flight/libraries/math/pid.h @@ -31,22 +31,22 @@ #ifndef PID_H #define PID_H -//! +// ! struct pid { - float p; - float i; - float d; - float iLim; - float iAccumulator; - float lastErr; - float lastDer; + float p; + float i; + float d; + float iLim; + float iAccumulator; + float lastErr; + float lastDer; }; -//! Methods to use the pid structures +// ! Methods to use the pid structures float pid_apply(struct pid *pid, const float err, float dT); float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT); void pid_zero(struct pid *pid); void pid_configure(struct pid *pid, float p, float i, float d, float iLim); void pid_configure_derivative(float cutoff, float gamma); -#endif /* PID_H */ \ No newline at end of file +#endif /* PID_H */ diff --git a/flight/libraries/math/sin_lookup.c b/flight/libraries/math/sin_lookup.c index 9d98e2e86..6233dc592 100644 --- a/flight/libraries/math/sin_lookup.c +++ b/flight/libraries/math/sin_lookup.c @@ -36,36 +36,36 @@ #define FLASH_TABLE #ifdef FLASH_TABLE - /** Version of the code which precomputes the lookup table in flash **/ +/** Version of the code which precomputes the lookup table in flash **/ - // This is a precomputed sin lookup table over 90 degrees that - const float sin_table[] = { - 0.000000f,0.017452f,0.034899f,0.052336f,0.069756f,0.087156f,0.104528f,0.121869f,0.139173f,0.156434f, - 0.173648f,0.190809f,0.207912f,0.224951f,0.241922f,0.258819f,0.275637f,0.292372f,0.309017f,0.325568f, - 0.342020f,0.358368f,0.374607f,0.390731f,0.406737f,0.422618f,0.438371f,0.453990f,0.469472f,0.484810f, - 0.500000f,0.515038f,0.529919f,0.544639f,0.559193f,0.573576f,0.587785f,0.601815f,0.615661f,0.629320f, - 0.642788f,0.656059f,0.669131f,0.681998f,0.694658f,0.707107f,0.719340f,0.731354f,0.743145f,0.754710f, - 0.766044f,0.777146f,0.788011f,0.798636f,0.809017f,0.819152f,0.829038f,0.838671f,0.848048f,0.857167f, - 0.866025f,0.874620f,0.882948f,0.891007f,0.898794f,0.906308f,0.913545f,0.920505f,0.927184f,0.933580f, - 0.939693f,0.945519f,0.951057f,0.956305f,0.961262f,0.965926f,0.970296f,0.974370f,0.978148f,0.981627f, - 0.984808f,0.987688f,0.990268f,0.992546f,0.994522f,0.996195f,0.997564f,0.998630f,0.999391f,0.999848f, - 1.000000f,0.999848f,0.999391f,0.998630f,0.997564f,0.996195f,0.994522f,0.992546f,0.990268f,0.987688f, - 0.984808f,0.981627f,0.978148f,0.974370f,0.970296f,0.965926f,0.961262f,0.956305f,0.951057f,0.945519f, - 0.939693f,0.933580f,0.927184f,0.920505f,0.913545f,0.906308f,0.898794f,0.891007f,0.882948f,0.874620f, - 0.866025f,0.857167f,0.848048f,0.838671f,0.829038f,0.819152f,0.809017f,0.798636f,0.788011f,0.777146f, - 0.766044f,0.754710f,0.743145f,0.731354f,0.719340f,0.707107f,0.694658f,0.681998f,0.669131f,0.656059f, - 0.642788f,0.629320f,0.615661f,0.601815f,0.587785f,0.573576f,0.559193f,0.544639f,0.529919f,0.515038f, - 0.500000f,0.484810f,0.469472f,0.453990f,0.438371f,0.422618f,0.406737f,0.390731f,0.374607f,0.358368f, - 0.342020f,0.325568f,0.309017f,0.292372f,0.275637f,0.258819f,0.241922f,0.224951f,0.207912f,0.190809f, - 0.173648f,0.156434f,0.139173f,0.121869f,0.104528f,0.087156f,0.069756f,0.052336f,0.034899f,0.017452f - }; +// This is a precomputed sin lookup table over 90 degrees that +const float sin_table[] = { + 0.000000f, 0.017452f, 0.034899f, 0.052336f, 0.069756f, 0.087156f, 0.104528f, 0.121869f, 0.139173f, 0.156434f, + 0.173648f, 0.190809f, 0.207912f, 0.224951f, 0.241922f, 0.258819f, 0.275637f, 0.292372f, 0.309017f, 0.325568f, + 0.342020f, 0.358368f, 0.374607f, 0.390731f, 0.406737f, 0.422618f, 0.438371f, 0.453990f, 0.469472f, 0.484810f, + 0.500000f, 0.515038f, 0.529919f, 0.544639f, 0.559193f, 0.573576f, 0.587785f, 0.601815f, 0.615661f, 0.629320f, + 0.642788f, 0.656059f, 0.669131f, 0.681998f, 0.694658f, 0.707107f, 0.719340f, 0.731354f, 0.743145f, 0.754710f, + 0.766044f, 0.777146f, 0.788011f, 0.798636f, 0.809017f, 0.819152f, 0.829038f, 0.838671f, 0.848048f, 0.857167f, + 0.866025f, 0.874620f, 0.882948f, 0.891007f, 0.898794f, 0.906308f, 0.913545f, 0.920505f, 0.927184f, 0.933580f, + 0.939693f, 0.945519f, 0.951057f, 0.956305f, 0.961262f, 0.965926f, 0.970296f, 0.974370f, 0.978148f, 0.981627f, + 0.984808f, 0.987688f, 0.990268f, 0.992546f, 0.994522f, 0.996195f, 0.997564f, 0.998630f, 0.999391f, 0.999848f, + 1.000000f, 0.999848f, 0.999391f, 0.998630f, 0.997564f, 0.996195f, 0.994522f, 0.992546f, 0.990268f, 0.987688f, + 0.984808f, 0.981627f, 0.978148f, 0.974370f, 0.970296f, 0.965926f, 0.961262f, 0.956305f, 0.951057f, 0.945519f, + 0.939693f, 0.933580f, 0.927184f, 0.920505f, 0.913545f, 0.906308f, 0.898794f, 0.891007f, 0.882948f, 0.874620f, + 0.866025f, 0.857167f, 0.848048f, 0.838671f, 0.829038f, 0.819152f, 0.809017f, 0.798636f, 0.788011f, 0.777146f, + 0.766044f, 0.754710f, 0.743145f, 0.731354f, 0.719340f, 0.707107f, 0.694658f, 0.681998f, 0.669131f, 0.656059f, + 0.642788f, 0.629320f, 0.615661f, 0.601815f, 0.587785f, 0.573576f, 0.559193f, 0.544639f, 0.529919f, 0.515038f, + 0.500000f, 0.484810f, 0.469472f, 0.453990f, 0.438371f, 0.422618f, 0.406737f, 0.390731f, 0.374607f, 0.358368f, + 0.342020f, 0.325568f, 0.309017f, 0.292372f, 0.275637f, 0.258819f, 0.241922f, 0.224951f, 0.207912f, 0.190809f, + 0.173648f, 0.156434f, 0.139173f, 0.121869f, 0.104528f, 0.087156f, 0.069756f, 0.052336f, 0.034899f, 0.017452f +}; int sin_lookup_initalize() { - return 0; + return 0; } -#else +#else /* ifdef FLASH_TABLE */ /** Version of the code which allocates the lookup table in heap **/ const int SIN_RESOLUTION = 180; @@ -73,21 +73,24 @@ const int SIN_RESOLUTION = 180; static float *sin_table; int sin_lookup_initalize() { - // Previously initialized - if (sin_table) - return 0; + // Previously initialized + if (sin_table) { + return 0; + } - sin_table = (float *) pvPortMalloc(sizeof(float) * SIN_RESOLUTION); - if (sin_table == NULL) - return -1; + sin_table = (float *)pvPortMalloc(sizeof(float) * SIN_RESOLUTION); + if (sin_table == NULL) { + return -1; + } - for(uint32_t i = 0; i < 180; i++) - sin_table[i] = sinf(DEG2RAD((float)i)); + for (uint32_t i = 0; i < 180; i++) { + sin_table[i] = sinf(DEG2RAD((float)i)); + } - return 0; + return 0; } -#endif +#endif /* ifdef FLASH_TABLE */ /** * Use the lookup table to return sine(angle) where angle is in radians @@ -95,19 +98,21 @@ int sin_lookup_initalize() * 90 values * @param[in] angle Angle in degrees * @returns sin(angle) -*/ + */ float sin_lookup_deg(float angle) { - if (sin_table == NULL) - return 0; + if (sin_table == NULL) { + return 0; + } - int i_ang = ((int32_t) angle) % 360; - if (i_ang >= 180) // for 180 to 360 deg - return -sin_table[i_ang - 180]; - else // for 0 to 179 deg - return sin_table[i_ang]; + int i_ang = ((int32_t)angle) % 360; + if (i_ang >= 180) { // for 180 to 360 deg + return -sin_table[i_ang - 180]; + } else { // for 0 to 179 deg + return sin_table[i_ang]; + } - return 0; + return 0; } /** @@ -117,7 +122,7 @@ float sin_lookup_deg(float angle) */ float cos_lookup_deg(float angle) { - return sin_lookup_deg(angle + 90); + return sin_lookup_deg(angle + 90); } /** @@ -127,8 +132,9 @@ float cos_lookup_deg(float angle) */ float sin_lookup_rad(float angle) { - int degrees = RAD2DEG(angle); - return sin_lookup_deg(degrees); + int degrees = RAD2DEG(angle); + + return sin_lookup_deg(degrees); } /** @@ -138,6 +144,7 @@ float sin_lookup_rad(float angle) */ float cos_lookup_rad(float angle) { - int degrees = RAD2DEG(angle); - return cos_lookup_deg(degrees); + int degrees = RAD2DEG(angle); + + return cos_lookup_deg(degrees); } diff --git a/flight/libraries/math/sin_lookup.h b/flight/libraries/math/sin_lookup.h index adc29f66a..8c10219a3 100644 --- a/flight/libraries/math/sin_lookup.h +++ b/flight/libraries/math/sin_lookup.h @@ -37,4 +37,4 @@ float cos_lookup_deg(float angle); float sin_lookup_rad(float angle); float cos_lookup_rad(float angle); -#endif \ No newline at end of file +#endif diff --git a/flight/libraries/op_dfu.c b/flight/libraries/op_dfu.c index e231388bc..882b106a8 100644 --- a/flight/libraries/op_dfu.c +++ b/flight/libraries/op_dfu.c @@ -33,11 +33,11 @@ #include "pios_bl_helper.h" #include "pios_com_msg.h" #include -//programmable devices +// programmable devices Device devicesTable[10]; uint8_t numberOfDevices = 0; -DFUProgType currentProgrammingDestination; //flash, flash_trough spi +DFUProgType currentProgrammingDestination; // flash, flash_trough spi uint8_t currentDeviceCanRead; uint8_t currentDeviceCanWrite; Device currentDevice; @@ -45,15 +45,15 @@ Device currentDevice; uint8_t Buffer[64]; uint8_t echoBuffer[64]; uint8_t SendBuffer[64]; -uint8_t Command = 0; -uint8_t EchoReqFlag = 0; -uint8_t EchoAnsFlag = 0; -uint8_t StartFlag = 0; -uint32_t Aditionals = 0; -uint32_t SizeOfTransfer = 0; -uint32_t Expected_CRC = 0; +uint8_t Command = 0; +uint8_t EchoReqFlag = 0; +uint8_t EchoAnsFlag = 0; +uint8_t StartFlag = 0; +uint32_t Aditionals = 0; +uint32_t SizeOfTransfer = 0; +uint32_t Expected_CRC = 0; uint8_t SizeOfLastPacket = 0; -uint32_t Next_Packet = 0; +uint32_t Next_Packet = 0; uint8_t TransferType; uint32_t Count = 0; uint32_t Data; @@ -63,422 +63,430 @@ uint8_t Data2; uint8_t Data3; uint32_t Opt[3]; -//Download vars +// Download vars uint32_t downSizeOfLastPacket = 0; uint32_t downPacketTotal = 0; -uint32_t downPacketCurrent = 0; +uint32_t downPacketCurrent = 0; DFUTransfer downType = 0; /* Extern variables ----------------------------------------------------------*/ extern DFUStates DeviceState; extern uint8_t JumpToApp; /* Private function prototypes -----------------------------------------------*/ /* Private functions ---------------------------------------------------------*/ -void sendData(uint8_t * buf, uint16_t size); +void sendData(uint8_t *buf, uint16_t size); uint32_t CalcFirmCRC(void); -void DataDownload(__attribute__((unused)) DownloadAction action) { - if ((DeviceState == downloading)) { - - uint8_t packetSize; - uint32_t offset; - uint32_t partoffset; - SendBuffer[0] = 0x01; - SendBuffer[1] = Download; - SendBuffer[2] = downPacketCurrent >> 24; - SendBuffer[3] = downPacketCurrent >> 16; - SendBuffer[4] = downPacketCurrent >> 8; - SendBuffer[5] = downPacketCurrent; - if (downPacketCurrent == downPacketTotal - 1) { - packetSize = downSizeOfLastPacket; - } else { - packetSize = 14; - } - for (uint8_t x = 0; x < packetSize; ++x) { - partoffset = (downPacketCurrent * 14 * 4) + (x * 4); - offset = baseOfAdressType(downType) + partoffset; - if (!flash_read(SendBuffer + (6 + x * 4), offset, - currentProgrammingDestination)) { - DeviceState = Last_operation_failed; - } - } - downPacketCurrent = downPacketCurrent + 1; - if (downPacketCurrent > downPacketTotal - 1) { - DeviceState = Last_operation_Success; - Aditionals = (uint32_t) Download; - } - sendData(SendBuffer + 1, 63); - } +void DataDownload(__attribute__((unused)) DownloadAction action) +{ + if ((DeviceState == downloading)) { + uint8_t packetSize; + uint32_t offset; + uint32_t partoffset; + SendBuffer[0] = 0x01; + SendBuffer[1] = Download; + SendBuffer[2] = downPacketCurrent >> 24; + SendBuffer[3] = downPacketCurrent >> 16; + SendBuffer[4] = downPacketCurrent >> 8; + SendBuffer[5] = downPacketCurrent; + if (downPacketCurrent == downPacketTotal - 1) { + packetSize = downSizeOfLastPacket; + } else { + packetSize = 14; + } + for (uint8_t x = 0; x < packetSize; ++x) { + partoffset = (downPacketCurrent * 14 * 4) + (x * 4); + offset = baseOfAdressType(downType) + partoffset; + if (!flash_read(SendBuffer + (6 + x * 4), offset, + currentProgrammingDestination)) { + DeviceState = Last_operation_failed; + } + } + downPacketCurrent = downPacketCurrent + 1; + if (downPacketCurrent > downPacketTotal - 1) { + DeviceState = Last_operation_Success; + Aditionals = (uint32_t)Download; + } + sendData(SendBuffer + 1, 63); + } } -void processComand(uint8_t *xReceive_Buffer) { - - Command = xReceive_Buffer[COMMAND]; +void processComand(uint8_t *xReceive_Buffer) +{ + Command = xReceive_Buffer[COMMAND]; #ifdef DEBUG_SSP - char str[63]= {0}; - sprintf(str,"Received COMMAND:%d|",Command); - PIOS_COM_SendString(PIOS_COM_TELEM_USB,str); + char str[63] = { 0 }; + sprintf(str, "Received COMMAND:%d|", Command); + PIOS_COM_SendString(PIOS_COM_TELEM_USB, str); #endif - EchoReqFlag = (Command >> 7); - EchoAnsFlag = (Command >> 6) & 0x01; - StartFlag = (Command >> 5) & 0x01; - Count = xReceive_Buffer[COUNT] << 24; - Count += xReceive_Buffer[COUNT + 1] << 16; - Count += xReceive_Buffer[COUNT + 2] << 8; - Count += xReceive_Buffer[COUNT + 3]; + EchoReqFlag = (Command >> 7); + EchoAnsFlag = (Command >> 6) & 0x01; + StartFlag = (Command >> 5) & 0x01; + Count = xReceive_Buffer[COUNT] << 24; + Count += xReceive_Buffer[COUNT + 1] << 16; + Count += xReceive_Buffer[COUNT + 2] << 8; + Count += xReceive_Buffer[COUNT + 3]; - Data = xReceive_Buffer[DATA] << 24; - Data += xReceive_Buffer[DATA + 1] << 16; - Data += xReceive_Buffer[DATA + 2] << 8; - Data += xReceive_Buffer[DATA + 3]; - Data0 = xReceive_Buffer[DATA]; - Data1 = xReceive_Buffer[DATA + 1]; - Data2 = xReceive_Buffer[DATA + 2]; - Data3 = xReceive_Buffer[DATA + 3]; - for( uint32_t i=0; i < 3; i++ ) - { - Opt[i] = xReceive_Buffer[DATA + 4 * (i+1) ] << 24 | - xReceive_Buffer[DATA + 4 * (i+1) + 1] << 16 | - xReceive_Buffer[DATA + 4 * (i+1) + 2] << 8 | - xReceive_Buffer[DATA + 4 * (i+1) + 3]; - } - - Command = Command & 0b00011111; + Data = xReceive_Buffer[DATA] << 24; + Data += xReceive_Buffer[DATA + 1] << 16; + Data += xReceive_Buffer[DATA + 2] << 8; + Data += xReceive_Buffer[DATA + 3]; + Data0 = xReceive_Buffer[DATA]; + Data1 = xReceive_Buffer[DATA + 1]; + Data2 = xReceive_Buffer[DATA + 2]; + Data3 = xReceive_Buffer[DATA + 3]; + for (uint32_t i = 0; i < 3; i++) { + Opt[i] = xReceive_Buffer[DATA + 4 * (i + 1)] << 24 | + xReceive_Buffer[DATA + 4 * (i + 1) + 1] << 16 | + xReceive_Buffer[DATA + 4 * (i + 1) + 2] << 8 | + xReceive_Buffer[DATA + 4 * (i + 1) + 3]; + } - if (EchoReqFlag == 1) { - memcpy(echoBuffer, xReceive_Buffer, 64); - } - switch (Command) { - case EnterDFU: - if (((DeviceState == BLidle) && (Data0 < numberOfDevices)) - || (DeviceState == DFUidle)) { - if (Data0 > 0) - OPDfuIni(true); - DeviceState = DFUidle; - currentProgrammingDestination = devicesTable[Data0].programmingType; - currentDeviceCanRead = devicesTable[Data0].readWriteFlags & 0x01; - currentDeviceCanWrite = devicesTable[Data0].readWriteFlags >> 1 - & 0x01; - currentDevice = devicesTable[Data0]; - uint8_t result = 0; - switch (currentProgrammingDestination) { - case Self_flash: - result = PIOS_BL_HELPER_FLASH_Ini(); - break; - case Remote_flash_via_spi: - result = true; - break; - default: - DeviceState = Last_operation_failed; - Aditionals = (uint16_t) Command; - } - if (result != 1) { - DeviceState = Last_operation_failed; - Aditionals = (uint32_t) Command; - } - } - break; - case Upload: - if ((DeviceState == DFUidle) || (DeviceState == uploading)) { - if ((StartFlag == 1) && (Next_Packet == 0)) { - TransferType = Data0; - SizeOfTransfer = Count; - Next_Packet = 1; - Expected_CRC = Data2 << 24; - Expected_CRC += Data3 << 16; - Expected_CRC += xReceive_Buffer[DATA + 4] << 8; - Expected_CRC += xReceive_Buffer[DATA + 5]; - SizeOfLastPacket = Data1; + Command = Command & 0b00011111; - if (isBiggerThanAvailable(TransferType, (SizeOfTransfer - 1) - * 14 * 4 + SizeOfLastPacket * 4) == true) { - DeviceState = outsideDevCapabilities; - Aditionals = (uint32_t) Command; - } else { - uint8_t result = 1; - if (TransferType == FW) { - switch (currentProgrammingDestination) { - case Self_flash: - result = PIOS_BL_HELPER_FLASH_Start(); - break; - case Remote_flash_via_spi: - result = false; - break; - default: - break; - } - } - if (result != 1) { - DeviceState = Last_operation_failed; - Aditionals = (uint32_t) Command; - } else { + if (EchoReqFlag == 1) { + memcpy(echoBuffer, xReceive_Buffer, 64); + } + switch (Command) { + case EnterDFU: + if (((DeviceState == BLidle) && (Data0 < numberOfDevices)) + || (DeviceState == DFUidle)) { + if (Data0 > 0) { + OPDfuIni(true); + } + DeviceState = DFUidle; + currentProgrammingDestination = devicesTable[Data0].programmingType; + currentDeviceCanRead = devicesTable[Data0].readWriteFlags & 0x01; + currentDeviceCanWrite = devicesTable[Data0].readWriteFlags >> 1 + & 0x01; + currentDevice = devicesTable[Data0]; + uint8_t result = 0; + switch (currentProgrammingDestination) { + case Self_flash: + result = PIOS_BL_HELPER_FLASH_Ini(); + break; + case Remote_flash_via_spi: + result = true; + break; + default: + DeviceState = Last_operation_failed; + Aditionals = (uint16_t)Command; + } + if (result != 1) { + DeviceState = Last_operation_failed; + Aditionals = (uint32_t)Command; + } + } + break; + case Upload: + if ((DeviceState == DFUidle) || (DeviceState == uploading)) { + if ((StartFlag == 1) && (Next_Packet == 0)) { + TransferType = Data0; + SizeOfTransfer = Count; + Next_Packet = 1; + Expected_CRC = Data2 << 24; + Expected_CRC += Data3 << 16; + Expected_CRC += xReceive_Buffer[DATA + 4] << 8; + Expected_CRC += xReceive_Buffer[DATA + 5]; + SizeOfLastPacket = Data1; - DeviceState = uploading; - } - } - } else if ((StartFlag != 1) && (Next_Packet != 0)) { - if (Count > SizeOfTransfer) { - DeviceState = too_many_packets; - Aditionals = Count; - } else if (Count == Next_Packet - 1) { - uint8_t numberOfWords = 14; - if (Count == SizeOfTransfer - 1)//is this the last packet? - { - numberOfWords = SizeOfLastPacket; - } - uint8_t result = 0; - uint32_t offset; - uint32_t aux;; - switch (currentProgrammingDestination) { - case Self_flash: - for (uint8_t x = 0; x < numberOfWords; ++x) { - offset = 4 * x; - Data = xReceive_Buffer[DATA + offset] << 24; - Data += xReceive_Buffer[DATA + 1 + offset] << 16; - Data += xReceive_Buffer[DATA + 2 + offset] << 8; - Data += xReceive_Buffer[DATA + 3 + offset]; - aux = baseOfAdressType(TransferType) + (uint32_t)( - Count * 14 * 4 + x * 4); - result = 0; - for (int retry = 0; retry < MAX_WRI_RETRYS; ++retry) { - if (result == 0) { - result = (FLASH_ProgramWord(aux, Data) - == FLASH_COMPLETE) ? 1 : 0; - } - } - } - break; - case Remote_flash_via_spi: - result = false; // No support for this for the PipX - break; - default: - result = 0; - break; - } - if (result != 1) { - DeviceState = Last_operation_failed; - Aditionals = (uint32_t) Command; - } + if (isBiggerThanAvailable(TransferType, (SizeOfTransfer - 1) + * 14 * 4 + SizeOfLastPacket * 4) == true) { + DeviceState = outsideDevCapabilities; + Aditionals = (uint32_t)Command; + } else { + uint8_t result = 1; + if (TransferType == FW) { + switch (currentProgrammingDestination) { + case Self_flash: + result = PIOS_BL_HELPER_FLASH_Start(); + break; + case Remote_flash_via_spi: + result = false; + break; + default: + break; + } + } + if (result != 1) { + DeviceState = Last_operation_failed; + Aditionals = (uint32_t)Command; + } else { + DeviceState = uploading; + } + } + } else if ((StartFlag != 1) && (Next_Packet != 0)) { + if (Count > SizeOfTransfer) { + DeviceState = too_many_packets; + Aditionals = Count; + } else if (Count == Next_Packet - 1) { + uint8_t numberOfWords = 14; + if (Count == SizeOfTransfer - 1) { // is this the last packet? + numberOfWords = SizeOfLastPacket; + } + uint8_t result = 0; + uint32_t offset; + uint32_t aux;; + switch (currentProgrammingDestination) { + case Self_flash: + for (uint8_t x = 0; x < numberOfWords; ++x) { + offset = 4 * x; + Data = xReceive_Buffer[DATA + offset] << 24; + Data += xReceive_Buffer[DATA + 1 + offset] << 16; + Data += xReceive_Buffer[DATA + 2 + offset] << 8; + Data += xReceive_Buffer[DATA + 3 + offset]; + aux = baseOfAdressType(TransferType) + (uint32_t)( + Count * 14 * 4 + x * 4); + result = 0; + for (int retry = 0; retry < MAX_WRI_RETRYS; ++retry) { + if (result == 0) { + result = (FLASH_ProgramWord(aux, Data) + == FLASH_COMPLETE) ? 1 : 0; + } + } + } + break; + case Remote_flash_via_spi: + result = false; // No support for this for the PipX + break; + default: + result = 0; + break; + } + if (result != 1) { + DeviceState = Last_operation_failed; + Aditionals = (uint32_t)Command; + } - ++Next_Packet; - } else { - DeviceState = wrong_packet_received; - Aditionals = Count; - } - } else { - DeviceState = Last_operation_failed; - Aditionals = (uint32_t) Command; - } - } - break; - case Req_Capabilities: - OPDfuIni(true); - Buffer[0] = 0x01; - Buffer[1] = Rep_Capabilities; - if (Data0 == 0) { - Buffer[2] = 0; - Buffer[3] = 0; - Buffer[4] = 0; - Buffer[5] = 0; - Buffer[6] = 0; - Buffer[7] = numberOfDevices; - uint16_t WRFlags = 0; - for (int x = 0; x < numberOfDevices; ++x) { - WRFlags = ((devicesTable[x].readWriteFlags << (x * 2)) - | WRFlags); - } - Buffer[8] = WRFlags >> 8; - Buffer[9] = WRFlags; - } else { - Buffer[2] = devicesTable[Data0 - 1].sizeOfCode >> 24; - Buffer[3] = devicesTable[Data0 - 1].sizeOfCode >> 16; - Buffer[4] = devicesTable[Data0 - 1].sizeOfCode >> 8; - Buffer[5] = devicesTable[Data0 - 1].sizeOfCode; - Buffer[6] = Data0; - Buffer[7] = devicesTable[Data0 - 1].BL_Version; - Buffer[8] = devicesTable[Data0 - 1].sizeOfDescription; - Buffer[9] = devicesTable[Data0 - 1].devID; - Buffer[10] = devicesTable[Data0 - 1].FW_Crc >> 24; - Buffer[11] = devicesTable[Data0 - 1].FW_Crc >> 16; - Buffer[12] = devicesTable[Data0 - 1].FW_Crc >> 8; - Buffer[13] = devicesTable[Data0 - 1].FW_Crc; - Buffer[14] = devicesTable[Data0 - 1].devID >> 8; - Buffer[15] = devicesTable[Data0 - 1].devID; - } - sendData(Buffer + 1, 63); - break; - case JumpFW: - if (Data == 0x5AFE) - { - /* Force board into safe mode */ - PIOS_IAP_WriteBootCount(0xFFFF); - } - // pass any Opt value to the firmware - PIOS_IAP_WriteBootCmd(0, Opt[0]); - PIOS_IAP_WriteBootCmd(1, Opt[1]); - PIOS_IAP_WriteBootCmd(2, Opt[2]); + ++Next_Packet; + } else { + DeviceState = wrong_packet_received; + Aditionals = Count; + } + } else { + DeviceState = Last_operation_failed; + Aditionals = (uint32_t)Command; + } + } + break; + case Req_Capabilities: + OPDfuIni(true); + Buffer[0] = 0x01; + Buffer[1] = Rep_Capabilities; + if (Data0 == 0) { + Buffer[2] = 0; + Buffer[3] = 0; + Buffer[4] = 0; + Buffer[5] = 0; + Buffer[6] = 0; + Buffer[7] = numberOfDevices; + uint16_t WRFlags = 0; + for (int x = 0; x < numberOfDevices; ++x) { + WRFlags = ((devicesTable[x].readWriteFlags << (x * 2)) + | WRFlags); + } + Buffer[8] = WRFlags >> 8; + Buffer[9] = WRFlags; + } else { + Buffer[2] = devicesTable[Data0 - 1].sizeOfCode >> 24; + Buffer[3] = devicesTable[Data0 - 1].sizeOfCode >> 16; + Buffer[4] = devicesTable[Data0 - 1].sizeOfCode >> 8; + Buffer[5] = devicesTable[Data0 - 1].sizeOfCode; + Buffer[6] = Data0; + Buffer[7] = devicesTable[Data0 - 1].BL_Version; + Buffer[8] = devicesTable[Data0 - 1].sizeOfDescription; + Buffer[9] = devicesTable[Data0 - 1].devID; + Buffer[10] = devicesTable[Data0 - 1].FW_Crc >> 24; + Buffer[11] = devicesTable[Data0 - 1].FW_Crc >> 16; + Buffer[12] = devicesTable[Data0 - 1].FW_Crc >> 8; + Buffer[13] = devicesTable[Data0 - 1].FW_Crc; + Buffer[14] = devicesTable[Data0 - 1].devID >> 8; + Buffer[15] = devicesTable[Data0 - 1].devID; + } + sendData(Buffer + 1, 63); + break; + case JumpFW: + if (Data == 0x5AFE) { + /* Force board into safe mode */ + PIOS_IAP_WriteBootCount(0xFFFF); + } + // pass any Opt value to the firmware + PIOS_IAP_WriteBootCmd(0, Opt[0]); + PIOS_IAP_WriteBootCmd(1, Opt[1]); + PIOS_IAP_WriteBootCmd(2, Opt[2]); - FLASH_Lock(); - JumpToApp = 1; - break; - case Reset: - PIOS_SYS_Reset(); - break; - case Abort_Operation: - Next_Packet = 0; - DeviceState = DFUidle; - break; + FLASH_Lock(); + JumpToApp = 1; + break; + case Reset: + PIOS_SYS_Reset(); + break; + case Abort_Operation: + Next_Packet = 0; + DeviceState = DFUidle; + break; - case Op_END: - if (DeviceState == uploading) { - if (Next_Packet - 1 == SizeOfTransfer) { - Next_Packet = 0; - if ((TransferType != FW) || (Expected_CRC == CalcFirmCRC())) { - DeviceState = Last_operation_Success; - } else { - DeviceState = CRC_Fail; - } - } - if (Next_Packet - 1 < SizeOfTransfer) { - Next_Packet = 0; - DeviceState = too_few_packets; - } - } - break; - case Download_Req: + case Op_END: + if (DeviceState == uploading) { + if (Next_Packet - 1 == SizeOfTransfer) { + Next_Packet = 0; + if ((TransferType != FW) || (Expected_CRC == CalcFirmCRC())) { + DeviceState = Last_operation_Success; + } else { + DeviceState = CRC_Fail; + } + } + if (Next_Packet - 1 < SizeOfTransfer) { + Next_Packet = 0; + DeviceState = too_few_packets; + } + } + break; + case Download_Req: #ifdef DEBUG_SSP - sprintf(str,"COMMAND:DOWNLOAD_REQ 1 Status=%d|",DeviceState); - PIOS_COM_SendString(PIOS_COM_TELEM_USB,str); + sprintf(str, "COMMAND:DOWNLOAD_REQ 1 Status=%d|", DeviceState); + PIOS_COM_SendString(PIOS_COM_TELEM_USB, str); #endif - if (DeviceState == DFUidle) { + if (DeviceState == DFUidle) { #ifdef DEBUG_SSP - PIOS_COM_SendString(PIOS_COM_TELEM_USB,"COMMAND:DOWNLOAD_REQ 1|"); + PIOS_COM_SendString(PIOS_COM_TELEM_USB, "COMMAND:DOWNLOAD_REQ 1|"); #endif - downType = Data0; - downPacketTotal = Count; - downSizeOfLastPacket = Data1; - if (isBiggerThanAvailable(downType, (downPacketTotal - 1) * 14 - + downSizeOfLastPacket) == 1) { - DeviceState = outsideDevCapabilities; - Aditionals = (uint32_t) Command; + downType = Data0; + downPacketTotal = Count; + downSizeOfLastPacket = Data1; + if (isBiggerThanAvailable(downType, (downPacketTotal - 1) * 14 + + downSizeOfLastPacket) == 1) { + DeviceState = outsideDevCapabilities; + Aditionals = (uint32_t)Command; + } else { + downPacketCurrent = 0; + DeviceState = downloading; + } + } else { + DeviceState = Last_operation_failed; + Aditionals = (uint32_t)Command; + } + break; - } else { - downPacketCurrent = 0; - DeviceState = downloading; - } - } else { - DeviceState = Last_operation_failed; - Aditionals = (uint32_t) Command; - } - break; + case Status_Request: + Buffer[0] = 0x01; + Buffer[1] = Status_Rep; + if (DeviceState == wrong_packet_received) { + Buffer[2] = Aditionals >> 24; + Buffer[3] = Aditionals >> 16; + Buffer[4] = Aditionals >> 8; + Buffer[5] = Aditionals; + } else { + Buffer[2] = 0; + Buffer[3] = ((uint16_t)Aditionals) >> 8; + Buffer[4] = ((uint16_t)Aditionals); + Buffer[5] = 0; + } + Buffer[6] = DeviceState; + Buffer[7] = 0; + Buffer[8] = 0; + Buffer[9] = 0; + sendData(Buffer + 1, 63); + if (DeviceState == Last_operation_Success) { + DeviceState = DFUidle; + } + break; + case Status_Rep: - case Status_Request: - Buffer[0] = 0x01; - Buffer[1] = Status_Rep; - if (DeviceState == wrong_packet_received) { - Buffer[2] = Aditionals >> 24; - Buffer[3] = Aditionals >> 16; - Buffer[4] = Aditionals >> 8; - Buffer[5] = Aditionals; - } else { - Buffer[2] = 0; - Buffer[3] = ((uint16_t) Aditionals) >> 8; - Buffer[4] = ((uint16_t) Aditionals); - Buffer[5] = 0; - } - Buffer[6] = DeviceState; - Buffer[7] = 0; - Buffer[8] = 0; - Buffer[9] = 0; - sendData(Buffer + 1, 63); - if (DeviceState == Last_operation_Success) { - DeviceState = DFUidle; - } - break; - case Status_Rep: - - break; - - } - if (EchoReqFlag == 1) { - echoBuffer[1] = echoBuffer[1] | EchoAnsFlag; - sendData(echoBuffer + 1, 63); - } - return; + break; + } + if (EchoReqFlag == 1) { + echoBuffer[1] = echoBuffer[1] | EchoAnsFlag; + sendData(echoBuffer + 1, 63); + } } -void OPDfuIni(uint8_t discover) { - const struct pios_board_info * bdinfo = &pios_board_info_blob; - Device dev; +void OPDfuIni(uint8_t discover) +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + Device dev; - dev.programmingType = Self_flash; - dev.readWriteFlags = (BOARD_READABLE | (BOARD_WRITABLE << 1)); - dev.startOfUserCode = bdinfo->fw_base; - dev.sizeOfCode = bdinfo->fw_size; - dev.sizeOfDescription = bdinfo->desc_size; - dev.BL_Version = bdinfo->bl_rev; - dev.FW_Crc = CalcFirmCRC(); - dev.devID = (bdinfo->board_type << 8) | (bdinfo->board_rev); - dev.devType = bdinfo->hw_type; - numberOfDevices = 1; - devicesTable[0] = dev; - if (discover) { - //TODO check other devices trough spi or whatever - } + dev.programmingType = Self_flash; + dev.readWriteFlags = (BOARD_READABLE | (BOARD_WRITABLE << 1)); + dev.startOfUserCode = bdinfo->fw_base; + dev.sizeOfCode = bdinfo->fw_size; + dev.sizeOfDescription = bdinfo->desc_size; + dev.BL_Version = bdinfo->bl_rev; + dev.FW_Crc = CalcFirmCRC(); + dev.devID = (bdinfo->board_type << 8) | (bdinfo->board_rev); + dev.devType = bdinfo->hw_type; + numberOfDevices = 1; + devicesTable[0] = dev; + if (discover) { + // TODO check other devices trough spi or whatever + } } -uint32_t baseOfAdressType(DFUTransfer type) { - switch (type) { - case FW: - return currentDevice.startOfUserCode; - break; - case Descript: - return currentDevice.startOfUserCode + currentDevice.sizeOfCode; - break; - default: +uint32_t baseOfAdressType(DFUTransfer type) +{ + switch (type) { + case FW: + return currentDevice.startOfUserCode; - return 0; - } + break; + case Descript: + return currentDevice.startOfUserCode + currentDevice.sizeOfCode; + + break; + default: + + return 0; + } } -uint8_t isBiggerThanAvailable(DFUTransfer type, uint32_t size) { - switch (type) { - case FW: - return (size > currentDevice.sizeOfCode) ? 1 : 0; - break; - case Descript: - return (size > currentDevice.sizeOfDescription) ? 1 : 0; - break; - default: - return true; - } +uint8_t isBiggerThanAvailable(DFUTransfer type, uint32_t size) +{ + switch (type) { + case FW: + return (size > currentDevice.sizeOfCode) ? 1 : 0; + + break; + case Descript: + return (size > currentDevice.sizeOfDescription) ? 1 : 0; + + break; + default: + return true; + } } -uint32_t CalcFirmCRC() { - switch (currentProgrammingDestination) { - case Self_flash: - return PIOS_BL_HELPER_CRC_Memory_Calc(); - break; - case Remote_flash_via_spi: - return 0; - break; - default: - return 0; - break; - } +uint32_t CalcFirmCRC() +{ + switch (currentProgrammingDestination) { + case Self_flash: + return PIOS_BL_HELPER_CRC_Memory_Calc(); + break; + case Remote_flash_via_spi: + return 0; + + break; + default: + return 0; + + break; + } } -void sendData(uint8_t * buf, uint16_t size) { - PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, buf, size); +void sendData(uint8_t *buf, uint16_t size) +{ + PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, buf, size); } -bool flash_read(uint8_t * buffer, uint32_t adr, DFUProgType type) { - switch (type) { - case Remote_flash_via_spi: - return false; // We should not get this for the PipX - break; - case Self_flash: - for (uint8_t x = 0; x < 4; ++x) { - buffer[x] = *PIOS_BL_HELPER_FLASH_If_Read(adr + x); - } - return true; - break; - default: - return false; - } +bool flash_read(uint8_t *buffer, uint32_t adr, DFUProgType type) +{ + switch (type) { + case Remote_flash_via_spi: + return false; // We should not get this for the PipX + + break; + case Self_flash: + for (uint8_t x = 0; x < 4; ++x) { + buffer[x] = *PIOS_BL_HELPER_FLASH_If_Read(adr + x); + } + return true; + + break; + default: + return false; + } } diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 4b9394672..1cbddb7d2 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -3,7 +3,7 @@ * * @file paths.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Library path manipulation + * @brief Library path manipulation * * @see The GNU Public License (GPL) Version 3 * @@ -30,13 +30,13 @@ #include "paths.h" #include "uavobjectmanager.h" // <--. -#include "pathdesired.h" //<-- needed only for correct ENUM macro usage with path modes (PATHDESIRED_MODE_xxx, +#include "pathdesired.h" // <-- needed only for correct ENUM macro usage with path modes (PATHDESIRED_MODE_xxx, // no direct UAVObject usage allowed in this file // private functions -static void path_endpoint(float * start_point, float * end_point, float * cur_point, struct path_status * status); -static void path_vector(float * start_point, float * end_point, float * cur_point, struct path_status * status); -static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise); +static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status); +static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status); +static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise); /** * @brief Compute progress along path and deviation from it @@ -46,27 +46,31 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin * @param[in] mode Path following mode * @param[out] status Structure containing progress along path and deviation */ -void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode) +void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode) { switch (mode) { - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_DRIVEVECTOR: - return path_vector(start_point, end_point, cur_point, status); - break; - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - return path_circle(start_point, end_point, cur_point, status, 1); - break; - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - return path_circle(start_point, end_point, cur_point, status, 0); - break; - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_DRIVEENDPOINT: - default: - // use the endpoint as default failsafe if called in unknown modes - return path_endpoint(start_point, end_point, cur_point, status); - break; + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_DRIVEVECTOR: + return path_vector(start_point, end_point, cur_point, status); + + break; + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_DRIVECIRCLERIGHT: + return path_circle(start_point, end_point, cur_point, status, 1); + + break; + case PATHDESIRED_MODE_FLYCIRCLELEFT: + case PATHDESIRED_MODE_DRIVECIRCLELEFT: + return path_circle(start_point, end_point, cur_point, status, 0); + + break; + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_DRIVEENDPOINT: + default: + // use the endpoint as default failsafe if called in unknown modes + return path_endpoint(start_point, end_point, cur_point, status); + + break; } } @@ -77,7 +81,7 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation */ -static void path_endpoint(float * start_point, float * end_point, float * cur_point, struct path_status * status) +static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status) { float path_north, path_east, diff_north, diff_east; float dist_path, dist_diff; @@ -87,19 +91,19 @@ static void path_endpoint(float * start_point, float * end_point, float * cur_po // Distance to go path_north = end_point[0] - start_point[0]; - path_east = end_point[1] - start_point[1]; + path_east = end_point[1] - start_point[1]; // Current progress location relative to end diff_north = end_point[0] - cur_point[0]; - diff_east = end_point[1] - cur_point[1]; + diff_east = end_point[1] - cur_point[1]; - dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east); - dist_path = sqrtf(path_north * path_north + path_east * path_east); + dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east); + dist_path = sqrtf(path_north * path_north + path_east * path_east); - if (dist_diff < 1e-6f ) { + if (dist_diff < 1e-6f) { status->fractional_progress = 1; status->error = 0; - status->path_direction[0] = status->path_direction[1] = 0; + status->path_direction[0] = status->path_direction[1] = 0; return; } @@ -109,7 +113,6 @@ static void path_endpoint(float * start_point, float * end_point, float * cur_po // Compute direction to travel status->path_direction[0] = diff_north / dist_diff; status->path_direction[1] = diff_east / dist_diff; - } /** @@ -119,7 +122,7 @@ static void path_endpoint(float * start_point, float * end_point, float * cur_po * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation */ -static void path_vector(float * start_point, float * end_point, float * cur_point, struct path_status * status) +static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status) { float path_north, path_east, diff_north, diff_east; float dist_path; @@ -128,16 +131,16 @@ static void path_vector(float * start_point, float * end_point, float * cur_poin // Distance to go path_north = end_point[0] - start_point[0]; - path_east = end_point[1] - start_point[1]; + path_east = end_point[1] - start_point[1]; // Current progress location relative to start diff_north = cur_point[0] - start_point[0]; - diff_east = cur_point[1] - start_point[1]; + diff_east = cur_point[1] - start_point[1]; dot = path_north * diff_north + path_east * diff_east; - dist_path = sqrtf(path_north * path_north + path_east * path_east); + dist_path = sqrtf(path_north * path_north + path_east * path_east); - if (dist_path < 1e-6f){ + if (dist_path < 1e-6f) { // if the path is too short, we cannot determine vector direction. // Fly towards the endpoint to prevent flying away, // but assume progress=1 either way. @@ -147,8 +150,8 @@ static void path_vector(float * start_point, float * end_point, float * cur_poin } // Compute the normal to the path - normal[0] = -path_east / dist_path; - normal[1] = path_north / dist_path; + normal[0] = -path_east / dist_path; + normal[1] = path_north / dist_path; status->fractional_progress = dot / (dist_path * dist_path); status->error = normal[0] * diff_north + normal[1] * diff_east; @@ -163,7 +166,6 @@ static void path_vector(float * start_point, float * end_point, float * cur_poin // Compute direction to travel status->path_direction[0] = path_north / dist_path; status->path_direction[1] = path_east / dist_path; - } /** @@ -173,7 +175,7 @@ static void path_vector(float * start_point, float * end_point, float * cur_poin * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation */ -static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise) +static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise) { float radius_north, radius_east, diff_north, diff_east; float radius, cradius; @@ -183,18 +185,18 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin // Radius radius_north = end_point[0] - start_point[0]; - radius_east = end_point[1] - start_point[1]; + radius_east = end_point[1] - start_point[1]; // Current location relative to center - diff_north = cur_point[0] - end_point[0]; - diff_east = cur_point[1] - end_point[1]; + diff_north = cur_point[0] - end_point[0]; + diff_east = cur_point[1] - end_point[1]; - radius = sqrtf(powf(radius_north, 2) + powf(radius_east, 2)); + radius = sqrtf(powf(radius_north, 2) + powf(radius_east, 2)); cradius = sqrtf(powf(diff_north, 2) + powf(diff_east, 2)); - if (cradius < 1e-6f) { + if (cradius < 1e-6f) { // cradius is zero, just fly somewhere and make sure correction is still a normal - status->fractional_progress = 1; + status->fractional_progress = 1; status->error = radius; status->correction_direction[0] = 0; status->correction_direction[1] = 1; @@ -214,7 +216,7 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin } // normalize progress to 0..1 - a_diff = atan2f(diff_north, diff_east); + a_diff = atan2f(diff_north, diff_east); a_radius = atan2f(radius_north, radius_east); if (a_diff < 0) { diff --git a/flight/libraries/printf-stdarg.c b/flight/libraries/printf-stdarg.c index b579b9d53..e4c01552d 100644 --- a/flight/libraries/printf-stdarg.c +++ b/flight/libraries/printf-stdarg.c @@ -5,7 +5,7 @@ * * @file printf-stdarg.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief Formatted print functions * @see The GNU Public License (GPL) Version 3 * @@ -32,213 +32,221 @@ If not, uncomment the define below and replace outbyte(c) by your own function call. -#define putchar(c) outbyte(c) -*/ -//#define putchar(c) COMSendChar(c) + #define putchar(c) outbyte(c) + */ +// #define putchar(c) COMSendChar(c) #include -static void printchar(char **str, int c) +static void printchar(char * *str, int c) { - if (str) { - **str = c; - ++(*str); - } + if (str) { + **str = c; + ++(*str); + } #ifdef PIOS_COM_DEBUG - else - PIOS_COM_SendChar(PIOS_COM_DEBUG, c); + else { + PIOS_COM_SendChar(PIOS_COM_DEBUG, c); + } #endif - } #define PAD_RIGHT 1 -#define PAD_ZERO 2 +#define PAD_ZERO 2 -static int prints(char **out, const char *string, int width, int pad) +static int prints(char * *out, const char *string, int width, int pad) { - register int pc = 0, padchar = ' '; + register int pc = 0, padchar = ' '; - if (width > 0) { - register int len = 0; - register const char *ptr; - for (ptr = string; *ptr; ++ptr) - ++len; - if (len >= width) - width = 0; - else - width -= len; - if (pad & PAD_ZERO) - padchar = '0'; - } - if (!(pad & PAD_RIGHT)) { - for (; width > 0; --width) { - printchar(out, padchar); - ++pc; - } - } - for (; *string; ++string) { - printchar(out, *string); - ++pc; - } - for (; width > 0; --width) { - printchar(out, padchar); - ++pc; - } + if (width > 0) { + register int len = 0; + register const char *ptr; + for (ptr = string; *ptr; ++ptr) { + ++len; + } + if (len >= width) { + width = 0; + } else { + width -= len; + } + if (pad & PAD_ZERO) { + padchar = '0'; + } + } + if (!(pad & PAD_RIGHT)) { + for (; width > 0; --width) { + printchar(out, padchar); + ++pc; + } + } + for (; *string; ++string) { + printchar(out, *string); + ++pc; + } + for (; width > 0; --width) { + printchar(out, padchar); + ++pc; + } - return pc; + return pc; } /* the following should be enough for 32 bit int */ #define PRINT_BUF_LEN 12 -static int printi(char **out, int i, int b, int sg, int width, int pad, int letbase) +static int printi(char * *out, int i, int b, int sg, int width, int pad, int letbase) { - char print_buf[PRINT_BUF_LEN]; - register char *s; - register int t, neg = 0, pc = 0; - register unsigned int u = i; + char print_buf[PRINT_BUF_LEN]; + register char *s; + register int t, neg = 0, pc = 0; + register unsigned int u = i; - if (i == 0) { - print_buf[0] = '0'; - print_buf[1] = '\0'; - return prints(out, print_buf, width, pad); - } + if (i == 0) { + print_buf[0] = '0'; + print_buf[1] = '\0'; + return prints(out, print_buf, width, pad); + } - if (sg && b == 10 && i < 0) { - neg = 1; - u = -i; - } + if (sg && b == 10 && i < 0) { + neg = 1; + u = -i; + } - s = print_buf + PRINT_BUF_LEN - 1; - *s = '\0'; + s = print_buf + PRINT_BUF_LEN - 1; + *s = '\0'; - while (u) { - t = u % b; - if (t >= 10) - t += letbase - '0' - 10; - *--s = t + '0'; - u /= b; - } + while (u) { + t = u % b; + if (t >= 10) { + t += letbase - '0' - 10; + } + *--s = t + '0'; + u /= b; + } - if (neg) { - if (width && (pad & PAD_ZERO)) { - printchar(out, '-'); - ++pc; - --width; - } else { - *--s = '-'; - } - } + if (neg) { + if (width && (pad & PAD_ZERO)) { + printchar(out, '-'); + ++pc; + --width; + } else { + *--s = '-'; + } + } - return pc + prints(out, s, width, pad); + return pc + prints(out, s, width, pad); } -static int print(char **out, const char *format, va_list args) +static int print(char * *out, const char *format, va_list args) { - register int width, pad; - register int pc = 0; - char scr[2]; + register int width, pad; + register int pc = 0; + char scr[2]; - for (; *format != 0; ++format) { - if (*format == '%') { - ++format; - width = pad = 0; - if (*format == '\0') - break; - if (*format == '%') - goto out; - if (*format == '-') { - ++format; - pad = PAD_RIGHT; - } - while (*format == '0') { - ++format; - pad |= PAD_ZERO; - } - for (; *format >= '0' && *format <= '9'; ++format) { - width *= 10; - width += *format - '0'; - } - if (*format == 's') { - register char *s = (char *)va_arg(args, int); - pc += prints(out, s ? s : "(null)", width, pad); - continue; - } - if (*format == 'd') { - pc += printi(out, va_arg(args, int), 10, 1, width, pad, 'a'); - continue; - } - if (*format == 'x') { - pc += printi(out, va_arg(args, int), 16, 0, width, pad, 'a'); - continue; - } - if (*format == 'X') { - pc += printi(out, va_arg(args, int), 16, 0, width, pad, 'A'); - continue; - } - if (*format == 'u') { - pc += printi(out, va_arg(args, int), 10, 0, width, pad, 'a'); - continue; - } - if (*format == 'c') { - /* char are converted to int then pushed on the stack */ - scr[0] = (char)va_arg(args, int); - scr[1] = '\0'; - pc += prints(out, scr, width, pad); - continue; - } - } else { + for (; *format != 0; ++format) { + if (*format == '%') { + ++format; + width = pad = 0; + if (*format == '\0') { + break; + } + if (*format == '%') { + goto out; + } + if (*format == '-') { + ++format; + pad = PAD_RIGHT; + } + while (*format == '0') { + ++format; + pad |= PAD_ZERO; + } + for (; *format >= '0' && *format <= '9'; ++format) { + width *= 10; + width += *format - '0'; + } + if (*format == 's') { + register char *s = (char *)va_arg(args, int); + pc += prints(out, s ? s : "(null)", width, pad); + continue; + } + if (*format == 'd') { + pc += printi(out, va_arg(args, int), 10, 1, width, pad, 'a'); + continue; + } + if (*format == 'x') { + pc += printi(out, va_arg(args, int), 16, 0, width, pad, 'a'); + continue; + } + if (*format == 'X') { + pc += printi(out, va_arg(args, int), 16, 0, width, pad, 'A'); + continue; + } + if (*format == 'u') { + pc += printi(out, va_arg(args, int), 10, 0, width, pad, 'a'); + continue; + } + if (*format == 'c') { + /* char are converted to int then pushed on the stack */ + scr[0] = (char)va_arg(args, int); + scr[1] = '\0'; + pc += prints(out, scr, width, pad); + continue; + } + } else { out: - printchar(out, *format); - ++pc; - } - } - if (out) - **out = '\0'; - va_end(args); - return pc; + printchar(out, *format); + ++pc; + } + } + if (out) { + **out = '\0'; + } + va_end(args); + return pc; } int printf(const char *format, ...) { - va_list args; + va_list args; - va_start(args, format); - return print(0, format, args); + va_start(args, format); + return print(0, format, args); } // TK: added for alternative parameter passing int vprintf(const char *format, va_list args) { - return print(0, format, args); + return print(0, format, args); } int sprintf(char *out, const char *format, ...) { - va_list args; + va_list args; - va_start(args, format); - return print(&out, format, args); + va_start(args, format); + return print(&out, format, args); } // TK: added for alternative parameter passing int vsprintf(char *out, const char *format, va_list args) { - char *_out; - _out = out; - return print(&_out, format, args); + char *_out; + + _out = out; + return print(&_out, format, args); } int snprintf(char *buf, size_t count, const char *format, ...) { - va_list args; + va_list args; - (void)count; + (void)count; - va_start(args, format); - return print(&buf, format, args); + va_start(args, format); + return print(&buf, format, args); } /** - * @} - */ + * @} + */ diff --git a/flight/libraries/printf2.c b/flight/libraries/printf2.c index dee058976..93d11f467 100644 --- a/flight/libraries/printf2.c +++ b/flight/libraries/printf2.c @@ -1,62 +1,62 @@ /******************************************************************************* - Copyright 2001, 2002 Georges Menie () - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU Lesser General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Lesser General Public License for more details. - You should have received a copy of the GNU Lesser General Public License - along with this program; if not, write to the Free Software - Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + Copyright 2001, 2002 Georges Menie () + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Lesser General Public License for more details. + You should have received a copy of the GNU Lesser General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ /******************************************************************************* - putchar is the only external dependency for this file, - if you have a working putchar, just remove the following - define. If the function should be called something else, - replace outbyte(c) by your own function call. -*/ -//******************************************************************************* -// Updated by Daniel D Miller. Changes to the original Menie code are -// Copyright 2009-2012 Daniel D Miller -// All such changes are distributed under the same license as the original, -// as described above. -// 11/06/09 - adding floating-point support -// 03/19/10 - pad fractional portion of floating-point number with 0s -// 03/30/10 - document the \% bug -// 07/20/10 - Fix a round-off bug in floating-point conversions -// ( 0.99 with %.1f did not round to 1.0 ) -// 10/25/11 - Add support for %+ format (always show + on positive numbers) -// 01/19/12 - fix handling of %f with no decimal; it was defaulting to 0 -// decimal places, rather than printf's 6. -//******************************************************************************* -// BUGS -// If '%' is included in a format string, in the form \% with the intent -// of including the percent sign in the output string, this function will -// mis-handle the data entirely!! -// Essentially, it will just discard the character following the percent sign. -// This bug is not easy to fix in the existing code; -// for now, I'll just try to remember to use %% instead of \% ... -//******************************************************************************* + putchar is the only external dependency for this file, + if you have a working putchar, just remove the following + define. If the function should be called something else, + replace outbyte(c) by your own function call. + */ +// ******************************************************************************* +// Updated by Daniel D Miller. Changes to the original Menie code are +// Copyright 2009-2012 Daniel D Miller +// All such changes are distributed under the same license as the original, +// as described above. +// 11/06/09 - adding floating-point support +// 03/19/10 - pad fractional portion of floating-point number with 0s +// 03/30/10 - document the \% bug +// 07/20/10 - Fix a round-off bug in floating-point conversions +// ( 0.99 with %.1f did not round to 1.0 ) +// 10/25/11 - Add support for %+ format (always show + on positive numbers) +// 01/19/12 - fix handling of %f with no decimal; it was defaulting to 0 +// decimal places, rather than printf's 6. +// ******************************************************************************* +// BUGS +// If '%' is included in a format string, in the form \% with the intent +// of including the percent sign in the output string, this function will +// mis-handle the data entirely!! +// Essentially, it will just discard the character following the percent sign. +// This bug is not easy to fix in the existing code; +// for now, I'll just try to remember to use %% instead of \% ... +// ******************************************************************************* -//lint -esym(752, debug_output) -//lint -esym(766, stdio.h) +// lint -esym(752, debug_output) +// lint -esym(766, stdio.h) // #define TEST_PRINTF 1 #include -static uint use_leading_plus = 0 ; +static uint use_leading_plus = 0; /* based on a example-code from Keil for CS G++ */ /* for caddr_t (typedef char * caddr_t;) */ #include -//* NEWLIB STUBS *// +// * NEWLIB STUBS *// #include #include #include @@ -69,7 +69,7 @@ static uint use_leading_plus = 0 ; * environment, this empty list is adequate: */ char *__env[1] = { 0 }; -char **environ = __env; +char * *environ = __env; /*============================================================================== * Close a file. @@ -82,7 +82,7 @@ int _close(__attribute__((unused)) int file) /*============================================================================== * Transfer control to a new process. */ -int _execve(__attribute__((unused)) char *name, __attribute__((unused)) char **argv, __attribute__((unused)) char **env) +int _execve(__attribute__((unused)) char *name, __attribute__((unused)) char * *argv, __attribute__((unused)) char * *env) { errno = ENOMEM; return -1; @@ -91,13 +91,12 @@ int _execve(__attribute__((unused)) char *name, __attribute__((unused)) char **a /*============================================================================== * Exit a program without cleaning up files. */ -void _exit( __attribute__((unused)) int code ) +void _exit(__attribute__((unused)) int code) { - /* Should we force a system reset? */ - while( 1 ) - { - ; - } + /* Should we force a system reset? */ + while (1) { + ; + } } /*============================================================================== @@ -182,7 +181,7 @@ int _read(__attribute__((unused)) int file, __attribute__((unused)) char *ptr, _ * example to a serial port for debugging, you should make your minimal write * capable of doing this. */ -int _write_r( __attribute__((unused)) void * reent, __attribute__((unused)) int file, __attribute__((unused)) char * ptr, __attribute__((unused)) int len ) +int _write_r(__attribute__((unused)) void *reent, __attribute__((unused)) int file, __attribute__((unused)) char *ptr, __attribute__((unused)) int len) { return 0; } @@ -195,26 +194,24 @@ int _write_r( __attribute__((unused)) void * reent, __attribute__((unused)) int */ caddr_t _sbrk(int incr) { - extern char _end; /* Defined by the linker */ + extern char _end; /* Defined by the linker */ static char *heap_end; char *prev_heap_end; - char * stack_ptr; + char *stack_ptr; - if (heap_end == 0) - { - heap_end = &_end; + if (heap_end == 0) { + heap_end = &_end; } prev_heap_end = heap_end; - asm volatile ("MRS %0, msp" : "=r" (stack_ptr) ); - if (heap_end + incr > stack_ptr) - { - _write_r ((void *)0, 1, "Heap and stack collision\n", 25); - _exit (1); + asm volatile ("MRS %0, msp" : "=r" (stack_ptr)); + if (heap_end + incr > stack_ptr) { + _write_r((void *)0, 1, "Heap and stack collision\n", 25); + _exit(1); } heap_end += incr; - return (caddr_t) prev_heap_end; + return (caddr_t)prev_heap_end; } /*============================================================================== @@ -251,383 +248,395 @@ int _wait(__attribute__((unused)) int *status) errno = ECHILD; return -1; } -//* NEWLIB STUBS *// +// * NEWLIB STUBS *// -//**************************************************************************** -static void printchar (char **str, int c) +// **************************************************************************** +static void printchar(char * *str, int c) { - if (str) { - **str = c; - ++(*str); - } + if (str) { + **str = c; + ++(*str); + } #ifdef TEST_PRINTF - else { - extern int putchar (int c); - (void) putchar (c); - } + else { + extern int putchar(int c); + (void)putchar(c); + } #endif } -//**************************************************************************** +// **************************************************************************** static uint my_strlen(char *str) { - if (str == 0) - return 0; - uint slen = 0 ; - while (*str != 0) { - slen++ ; - str++ ; - } - return slen; + if (str == 0) { + return 0; + } + uint slen = 0; + while (*str != 0) { + slen++; + str++; + } + return slen; } -//**************************************************************************** -// This version returns the length of the output string. -// It is more useful when implementing a walking-string function. -//**************************************************************************** +// **************************************************************************** +// This version returns the length of the output string. +// It is more useful when implementing a walking-string function. +// **************************************************************************** static const double round_nums[8] = { - 0.5L, - 0.05L, - 0.005L, - 0.0005L, - 0.00005L, - 0.000005L, - 0.0000005L, - 0.00000005L -} ; + 0.5L, + 0.05L, + 0.005L, + 0.0005L, + 0.00005L, + 0.000005L, + 0.0000005L, + 0.00000005L +}; static unsigned dbl2stri(char *outbfr, double dbl, unsigned dec_digits) { - static char local_bfr[128] ; - char *output = (outbfr == 0) ? local_bfr : outbfr ; + static char local_bfr[128]; + char *output = (outbfr == 0) ? local_bfr : outbfr; - //******************************************* - // extract negative info - //******************************************* - if (dbl < 0.0L) { - *output++ = '-' ; - dbl *= -1.0L ; - } else { - if (use_leading_plus) { - *output++ = '+' ; - } + // ******************************************* + // extract negative info + // ******************************************* + if (dbl < 0.0L) { + *output++ = '-'; + dbl *= -1.0L; + } else { + if (use_leading_plus) { + *output++ = '+'; + } + } - } + // handling rounding by adding .5LSB to the floating-point data + if (dec_digits < 8) { + dbl += round_nums[dec_digits]; + } - // handling rounding by adding .5LSB to the floating-point data - if (dec_digits < 8) { - dbl += round_nums[dec_digits] ; - } + // ************************************************************************** + // construct fractional multiplier for specified number of digits. + // ************************************************************************** + uint mult = 1; + uint idx; + for (idx = 0; idx < dec_digits; idx++) { + mult *= 10; + } - //************************************************************************** - // construct fractional multiplier for specified number of digits. - //************************************************************************** - uint mult = 1 ; - uint idx ; - for (idx=0; idx < dec_digits; idx++) - mult *= 10 ; + // printf("mult=%u\n", mult) ; + uint wholeNum = (uint)dbl; + uint decimalNum = (uint)((dbl - wholeNum) * mult); - // printf("mult=%u\n", mult) ; - uint wholeNum = (uint) dbl ; - uint decimalNum = (uint) ((dbl - wholeNum) * mult); + // ******************************************* + // convert integer portion + // ******************************************* + char tbfr[40]; + idx = 0; + while (wholeNum != 0) { + tbfr[idx++] = '0' + (wholeNum % 10); + wholeNum /= 10; + } + // printf("%.3f: whole=%s, dec=%d\n", dbl, tbfr, decimalNum) ; + if (idx == 0) { + *output++ = '0'; + } else { + while (idx > 0) { + *output++ = tbfr[idx - 1]; // lint !e771 + idx--; + } + } + if (dec_digits > 0) { + *output++ = '.'; - //******************************************* - // convert integer portion - //******************************************* - char tbfr[40] ; - idx = 0 ; - while (wholeNum != 0) { - tbfr[idx++] = '0' + (wholeNum % 10) ; - wholeNum /= 10 ; - } - // printf("%.3f: whole=%s, dec=%d\n", dbl, tbfr, decimalNum) ; - if (idx == 0) { - *output++ = '0' ; - } else { - while (idx > 0) { - *output++ = tbfr[idx-1] ; //lint !e771 - idx-- ; - } - } - if (dec_digits > 0) { - *output++ = '.' ; + // ******************************************* + // convert fractional portion + // ******************************************* + idx = 0; + while (decimalNum != 0) { + tbfr[idx++] = '0' + (decimalNum % 10); + decimalNum /= 10; + } + // pad the decimal portion with 0s as necessary; + // We wouldn't want to report 3.093 as 3.93, would we?? + while (idx < dec_digits) { + tbfr[idx++] = '0'; + } + // printf("decimal=%s\n", tbfr) ; + if (idx == 0) { + *output++ = '0'; + } else { + while (idx > 0) { + *output++ = tbfr[idx - 1]; + idx--; + } + } + } + *output = 0; - //******************************************* - // convert fractional portion - //******************************************* - idx = 0 ; - while (decimalNum != 0) { - tbfr[idx++] = '0' + (decimalNum % 10) ; - decimalNum /= 10 ; - } - // pad the decimal portion with 0s as necessary; - // We wouldn't want to report 3.093 as 3.93, would we?? - while (idx < dec_digits) { - tbfr[idx++] = '0' ; - } - // printf("decimal=%s\n", tbfr) ; - if (idx == 0) { - *output++ = '0' ; - } else { - while (idx > 0) { - *output++ = tbfr[idx-1] ; - idx-- ; - } - } - } - *output = 0 ; - - // prepare output - output = (outbfr == 0) ? local_bfr : outbfr ; - return my_strlen(output) ; + // prepare output + output = (outbfr == 0) ? local_bfr : outbfr; + return my_strlen(output); } -//**************************************************************************** -#define PAD_RIGHT 1 -#define PAD_ZERO 2 +// **************************************************************************** +#define PAD_RIGHT 1 +#define PAD_ZERO 2 -static int prints (char **out, const char *string, int width, int pad) +static int prints(char * *out, const char *string, int width, int pad) { - register int pc = 0, padchar = ' '; - if (width > 0) { - int len = 0; - const char *ptr; - for (ptr = string; *ptr; ++ptr) - ++len; - if (len >= width) - width = 0; - else - width -= len; - if (pad & PAD_ZERO) - padchar = '0'; - } - if (!(pad & PAD_RIGHT)) { - for (; width > 0; --width) { - printchar (out, padchar); - ++pc; - } - } - for (; *string; ++string) { - printchar (out, *string); - ++pc; - } - for (; width > 0; --width) { - printchar (out, padchar); - ++pc; - } - return pc; + register int pc = 0, padchar = ' '; + + if (width > 0) { + int len = 0; + const char *ptr; + for (ptr = string; *ptr; ++ptr) { + ++len; + } + if (len >= width) { + width = 0; + } else { + width -= len; + } + if (pad & PAD_ZERO) { + padchar = '0'; + } + } + if (!(pad & PAD_RIGHT)) { + for (; width > 0; --width) { + printchar(out, padchar); + ++pc; + } + } + for (; *string; ++string) { + printchar(out, *string); + ++pc; + } + for (; width > 0; --width) { + printchar(out, padchar); + ++pc; + } + return pc; } -//**************************************************************************** +// **************************************************************************** /* the following should be enough for 32 bit int */ #define PRINT_BUF_LEN 12 -static int printi (char **out, int i, int b, int sg, int width, int pad, int letbase) +static int printi(char * *out, int i, int b, int sg, int width, int pad, int letbase) { - char print_buf[PRINT_BUF_LEN]; - char *s; - int t, neg = 0, pc = 0; - unsigned u = (unsigned) i; - if (i == 0) { - print_buf[0] = '0'; - print_buf[1] = '\0'; - return prints (out, print_buf, width, pad); - } - if (sg && b == 10 && i < 0) { - neg = 1; - u = (unsigned) -i; - } - // make sure print_buf is NULL-term - s = print_buf + PRINT_BUF_LEN - 1; - *s = '\0'; + char print_buf[PRINT_BUF_LEN]; + char *s; + int t, neg = 0, pc = 0; + unsigned u = (unsigned)i; + + if (i == 0) { + print_buf[0] = '0'; + print_buf[1] = '\0'; + return prints(out, print_buf, width, pad); + } + if (sg && b == 10 && i < 0) { + neg = 1; + u = (unsigned)-i; + } + // make sure print_buf is NULL-term + s = print_buf + PRINT_BUF_LEN - 1; + *s = '\0'; - while (u) { - t = u % b; //lint !e573 Warning 573: Signed-unsigned mix with divide - if (t >= 10) - t += letbase - '0' - 10; - *--s = t + '0'; - u /= b; //lint !e573 Warning 573: Signed-unsigned mix with divide - } - if (neg) { - if (width && (pad & PAD_ZERO)) { - printchar (out, '-'); - ++pc; - --width; - } - else { - *--s = '-'; - } - } else { - if (use_leading_plus) { - *--s = '+'; - } - } - return pc + prints (out, s, width, pad); + while (u) { + t = u % b; // lint !e573 Warning 573: Signed-unsigned mix with divide + if (t >= 10) { + t += letbase - '0' - 10; + } + *--s = t + '0'; + u /= b; // lint !e573 Warning 573: Signed-unsigned mix with divide + } + if (neg) { + if (width && (pad & PAD_ZERO)) { + printchar(out, '-'); + ++pc; + --width; + } else { + *--s = '-'; + } + } else { + if (use_leading_plus) { + *--s = '+'; + } + } + return pc + prints(out, s, width, pad); } -//**************************************************************************** -static int print (char **out, int *varg) +// **************************************************************************** +static int print(char * *out, int *varg) { - int post_decimal ; - int width, pad ; - unsigned dec_width = 6 ; - int pc = 0; - char *format = (char *) (*varg++); - char scr[2]; - use_leading_plus = 0 ; // start out with this clear - for (; *format != 0; ++format) { - if (*format == '%') { - dec_width = 6 ; - ++format; - width = pad = 0; - if (*format == '\0') - break; - if (*format == '%') - goto out_lbl; - if (*format == '-') { - ++format; - pad = PAD_RIGHT; - } - if (*format == '+') { - ++format; - use_leading_plus = 1 ; - } - while (*format == '0') { - ++format; - pad |= PAD_ZERO; - } - post_decimal = 0 ; - if (*format == '.' || - (*format >= '0' && *format <= '9')) { + int post_decimal; + int width, pad; + unsigned dec_width = 6; + int pc = 0; + char *format = (char *)(*varg++); + char scr[2]; - while (1) { - if (*format == '.') { - post_decimal = 1 ; - dec_width = 0 ; - format++ ; - } else if ((*format >= '0' && *format <= '9')) { - if (post_decimal) { - dec_width *= 10; - dec_width += *format - '0'; - } else { - width *= 10; - width += *format - '0'; - } - format++ ; - } else { - break; - } - } - } - if (*format == 'l') + use_leading_plus = 0; // start out with this clear + for (; *format != 0; ++format) { + if (*format == '%') { + dec_width = 6; ++format; - switch (*format) { - case 's': + width = pad = 0; + if (*format == '\0') { + break; + } + if (*format == '%') { + goto out_lbl; + } + if (*format == '-') { + ++format; + pad = PAD_RIGHT; + } + if (*format == '+') { + ++format; + use_leading_plus = 1; + } + while (*format == '0') { + ++format; + pad |= PAD_ZERO; + } + post_decimal = 0; + if (*format == '.' || + (*format >= '0' && *format <= '9')) { + while (1) { + if (*format == '.') { + post_decimal = 1; + dec_width = 0; + format++; + } else if ((*format >= '0' && *format <= '9')) { + if (post_decimal) { + dec_width *= 10; + dec_width += *format - '0'; + } else { + width *= 10; + width += *format - '0'; + } + format++; + } else { + break; + } + } + } + if (*format == 'l') { + ++format; + } + switch (*format) { + case 's': { - // char *s = *((char **) varg++); //lint !e740 - char *s = (char *) *varg++ ; //lint !e740 !e826 convert to double pointer - pc += prints (out, s ? s : "(null)", width, pad); - use_leading_plus = 0 ; // reset this flag after printing one value + // char *s = *((char **) varg++); //lint !e740 + char *s = (char *)*varg++; // lint !e740 !e826 convert to double pointer + pc += prints(out, s ? s : "(null)", width, pad); + use_leading_plus = 0; // reset this flag after printing one value } break; - case 'd': - pc += printi (out, *varg++, 10, 1, width, pad, 'a'); - use_leading_plus = 0 ; // reset this flag after printing one value - break; - case 'x': - pc += printi (out, *varg++, 16, 0, width, pad, 'a'); - use_leading_plus = 0 ; // reset this flag after printing one value - break; - case 'X': - pc += printi (out, *varg++, 16, 0, width, pad, 'A'); - use_leading_plus = 0 ; // reset this flag after printing one value - break; - case 'u': - pc += printi (out, *varg++, 10, 0, width, pad, 'a'); - use_leading_plus = 0 ; // reset this flag after printing one value - break; - case 'c': - /* char are converted to int then pushed on the stack */ - scr[0] = *varg++; - scr[1] = '\0'; - pc += prints (out, scr, width, pad); - use_leading_plus = 0 ; // reset this flag after printing one value - break; + case 'd': + pc += printi(out, *varg++, 10, 1, width, pad, 'a'); + use_leading_plus = 0; // reset this flag after printing one value + break; + case 'x': + pc += printi(out, *varg++, 16, 0, width, pad, 'a'); + use_leading_plus = 0; // reset this flag after printing one value + break; + case 'X': + pc += printi(out, *varg++, 16, 0, width, pad, 'A'); + use_leading_plus = 0; // reset this flag after printing one value + break; + case 'u': + pc += printi(out, *varg++, 10, 0, width, pad, 'a'); + use_leading_plus = 0; // reset this flag after printing one value + break; + case 'c': + /* char are converted to int then pushed on the stack */ + scr[0] = *varg++; + scr[1] = '\0'; + pc += prints(out, scr, width, pad); + use_leading_plus = 0; // reset this flag after printing one value + break; - case 'f': + case 'f': { - // http://wiki.debian.org/ArmEabiPort#Structpackingandalignment - // Stack alignment - // - // The ARM EABI requires 8-byte stack alignment at public function entry points, - // compared to the previous 4-byte alignment. + // http://wiki.debian.org/ArmEabiPort#Structpackingandalignment + // Stack alignment + // + // The ARM EABI requires 8-byte stack alignment at public function entry points, + // compared to the previous 4-byte alignment. #ifdef USE_NEWLIB - char *cptr = (char *) varg ; //lint !e740 !e826 convert to double pointer - uint caddr = (uint) cptr ; - if ((caddr & 0xF) != 0) { - cptr += 4 ; - } - double *dblptr = (double *) cptr ; //lint !e740 !e826 convert to double pointer + char *cptr = (char *)varg; // lint !e740 !e826 convert to double pointer + uint caddr = (uint)cptr; + if ((caddr & 0xF) != 0) { + cptr += 4; + } + double *dblptr = (double *)cptr; // lint !e740 !e826 convert to double pointer #else - double *dblptr = (double *) varg ; //lint !e740 !e826 convert to double pointer + double *dblptr = (double *)varg; // lint !e740 !e826 convert to double pointer #endif - double dbl = *dblptr++ ; // increment double pointer - varg = (int *) dblptr ; //lint !e740 copy updated pointer back to base pointer - char bfr[81] ; - // unsigned slen = - dbl2stri(bfr, dbl, dec_width) ; - // stuff_talkf("[%s], width=%u, dec_width=%u\n", bfr, width, dec_width) ; - pc += prints (out, bfr, width, pad); - use_leading_plus = 0 ; // reset this flag after printing one value + double dbl = *dblptr++; // increment double pointer + varg = (int *)dblptr; // lint !e740 copy updated pointer back to base pointer + char bfr[81]; + // unsigned slen = + dbl2stri(bfr, dbl, dec_width); + // stuff_talkf("[%s], width=%u, dec_width=%u\n", bfr, width, dec_width) ; + pc += prints(out, bfr, width, pad); + use_leading_plus = 0; // reset this flag after printing one value } break; - default: - printchar (out, '%'); - printchar (out, *format); - use_leading_plus = 0 ; // reset this flag after printing one value - break; - } - } else - // if (*format == '\\') { - // - // } else - { + default: + printchar(out, '%'); + printchar(out, *format); + use_leading_plus = 0; // reset this flag after printing one value + break; + } + } else { + // if (*format == '\\') { + // + // } else out_lbl: - printchar (out, *format); - ++pc; - } - } // for each char in format string - if (out) //lint !e850 - **out = '\0'; - return pc; + printchar(out, *format); + ++pc; + } + } // for each char in format string + if (out) { // lint !e850 + **out = '\0'; + } + return pc; } -//**************************************************************************** -int stringf (char *out, const char *format, ...) +// **************************************************************************** +int stringf(char *out, const char *format, ...) { - // create a pointer into the stack. - // Thematically this should be a void*, since the actual usage of the - // pointer will vary. However, int* works fine too. - // Either way, the called function will need to re-cast the pointer - // for any type which isn't sizeof(int) - int *varg = (int *) (char *) (&format); - return print (&out, varg); + // create a pointer into the stack. + // Thematically this should be a void*, since the actual usage of the + // pointer will vary. However, int* works fine too. + // Either way, the called function will need to re-cast the pointer + // for any type which isn't sizeof(int) + int *varg = (int *)(char *)(&format); + + return print(&out, varg); } int printf(const char *format, ...) { - int *varg = (int *) (char *) (&format); - return print (0, varg); + int *varg = (int *)(char *)(&format); + + return print(0, varg); } int sprintf(char *out, const char *format, ...) { - int *varg = (int *) (char *) (&format); - return print (&out, varg); + int *varg = (int *)(char *)(&format); + + return print(&out, varg); } /** - * @} - */ + * @} + */ diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index c7126ec8c..dfc7b2c31 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -38,12 +38,12 @@ #include /**************************** - * Current checks: - * 1. If a flight mode switch allows autotune and autotune module not running - * 2. If airframe is a multirotor and either manual is available or a stabilization mode uses "none" - ****************************/ +* Current checks: +* 1. If a flight mode switch allows autotune and autotune module not running +* 2. If airframe is a multirotor and either manual is available or a stabilization mode uses "none" +****************************/ -//! Check a stabilization mode switch position for safety +// ! Check a stabilization mode switch position for safety static int32_t check_stabilization_settings(int index, bool multirotor); /** @@ -56,28 +56,29 @@ int32_t configuration_check() SystemAlarmsExtendedAlarmStatusOptions alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE; uint8_t alarmsubstatus = 0; // Get board type - const struct pios_board_info * bdinfo = &pios_board_info_blob; - bool coptercontrol = bdinfo->board_type == 0x04; + const struct pios_board_info *bdinfo = &pios_board_info_blob; + bool coptercontrol = bdinfo->board_type == 0x04; // Classify airframe type bool multirotor = true; uint8_t airframe_type; + SystemSettingsAirframeTypeGet(&airframe_type); switch (airframe_type) { - case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX: - case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP: - case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA: - case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO: - case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX: - case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV: - case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP: - case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX: - case SYSTEMSETTINGS_AIRFRAMETYPE_TRI: - case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX: - multirotor = true; - break; - default: - multirotor = false; + case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX: + case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP: + case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO: + case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP: + case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX: + case SYSTEMSETTINGS_AIRFRAMETYPE_TRI: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX: + multirotor = true; + break; + default: + multirotor = false; } // For each available flight mode position sanity check the available @@ -89,96 +90,96 @@ int32_t configuration_check() for (uint32_t i = 0; i < num_modes; i++) { switch (modes[i]) { - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL: - if (multirotor) { - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1: - severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity; - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2: - severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity; - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3: - severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity; - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: - if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) { - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { - // Revo supports Position Hold - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { - // Revo supports AutoLand Mode - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { - // Revo supports POI Mode - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { - // Revo supports PathPlanner - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: - if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; - } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { - // Revo supports ReturnToBase - severity = SYSTEMALARMS_ALARM_ERROR; - } - break; - default: - // Uncovered modes are automatically an error + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL: + if (multirotor) { severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1: + severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity; + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2: + severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity; + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3: + severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity; + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: + if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) { + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { + // Revo supports Position Hold + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { + // Revo supports AutoLand Mode + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { + // Revo supports POI Mode + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { + // Revo supports PathPlanner + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: + if (coptercontrol) { + severity = SYSTEMALARMS_ALARM_ERROR; + } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { + // Revo supports ReturnToBase + severity = SYSTEMALARMS_ALARM_ERROR; + } + break; + default: + // Uncovered modes are automatically an error + severity = SYSTEMALARMS_ALARM_ERROR; } // mark the first encountered erroneous setting in status and substatus if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) { - alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE; + alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE; alarmsubstatus = i; } - } // TODO: Check on a multirotor no axis supports "None" - if (severity != SYSTEMALARMS_ALARM_OK) + if (severity != SYSTEMALARMS_ALARM_OK) { ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus); - else + } else { AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION); + } return 0; } @@ -193,31 +194,33 @@ static int32_t check_stabilization_settings(int index, bool multirotor) { // Make sure the modes have identical sizes if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM || - MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM) + MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM) { return SYSTEMALARMS_ALARM_ERROR; + } uint8_t modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM]; // Get the different axis modes for this switch position switch (index) { - case 1: - ManualControlSettingsStabilization1SettingsGet(modes); - break; - case 2: - ManualControlSettingsStabilization2SettingsGet(modes); - break; - case 3: - ManualControlSettingsStabilization3SettingsGet(modes); - break; - default: - return SYSTEMALARMS_ALARM_ERROR; + case 1: + ManualControlSettingsStabilization1SettingsGet(modes); + break; + case 2: + ManualControlSettingsStabilization2SettingsGet(modes); + break; + case 3: + ManualControlSettingsStabilization3SettingsGet(modes); + break; + default: + return SYSTEMALARMS_ALARM_ERROR; } // For multirotors verify that nothing is set to "none" if (multirotor) { for (uint32_t i = 0; i < NELEMENTS(modes); i++) { - if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE) + if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE) { return SYSTEMALARMS_ALARM_ERROR; + } } } diff --git a/flight/libraries/stopwatch.c b/flight/libraries/stopwatch.c index a42e52360..efa00d451 100644 --- a/flight/libraries/stopwatch.c +++ b/flight/libraries/stopwatch.c @@ -35,85 +35,92 @@ // Local definitions ///////////////////////////////////////////////////////////////////////////// -uint32_t STOPWATCH_Init(u32 resolution, TIM_TypeDef* TIM) { - uint32_t STOPWATCH_TIMER_RCC; - switch ((uint32_t) TIM) { - case (uint32_t)TIM1: - STOPWATCH_TIMER_RCC = RCC_APB2Periph_TIM1; - break; - case (uint32_t)TIM2: - STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM2; - break; - case (uint32_t)TIM3: - STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM3; - break; - case (uint32_t)TIM4: - STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM4; - break; - case (uint32_t)TIM5: - STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM5; - break; - case (uint32_t)TIM6: - STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM6; - break; - case (uint32_t)TIM7: - STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM7; - break; - case (uint32_t)TIM8: - STOPWATCH_TIMER_RCC = RCC_APB2Periph_TIM8; - break; - default: - /* Unsupported timer */ - while(1); - } +uint32_t STOPWATCH_Init(u32 resolution, TIM_TypeDef *TIM) +{ + uint32_t STOPWATCH_TIMER_RCC; - // enable timer clock - if (STOPWATCH_TIMER_RCC == RCC_APB2Periph_TIM1 || STOPWATCH_TIMER_RCC - == RCC_APB2Periph_TIM8) - RCC_APB2PeriphClockCmd(STOPWATCH_TIMER_RCC, ENABLE); - else - RCC_APB1PeriphClockCmd(STOPWATCH_TIMER_RCC, ENABLE); + switch ((uint32_t)TIM) { + case (uint32_t)TIM1: + STOPWATCH_TIMER_RCC = RCC_APB2Periph_TIM1; + break; + case (uint32_t)TIM2: + STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM2; + break; + case (uint32_t)TIM3: + STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM3; + break; + case (uint32_t)TIM4: + STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM4; + break; + case (uint32_t)TIM5: + STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM5; + break; + case (uint32_t)TIM6: + STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM6; + break; + case (uint32_t)TIM7: + STOPWATCH_TIMER_RCC = RCC_APB1Periph_TIM7; + break; + case (uint32_t)TIM8: + STOPWATCH_TIMER_RCC = RCC_APB2Periph_TIM8; + break; + default: + /* Unsupported timer */ + while (1) { + ; + } + } - // time base configuration - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - TIM_TimeBaseStructure.TIM_Period = 0xffff; // max period - TIM_TimeBaseStructure.TIM_Prescaler = (72 * resolution) - 1; // uS accuracy @ 72 MHz - TIM_TimeBaseStructure.TIM_ClockDivision = 0; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(TIM, &TIM_TimeBaseStructure); + // enable timer clock + if (STOPWATCH_TIMER_RCC == RCC_APB2Periph_TIM1 || STOPWATCH_TIMER_RCC + == RCC_APB2Periph_TIM8) { + RCC_APB2PeriphClockCmd(STOPWATCH_TIMER_RCC, ENABLE); + } else { + RCC_APB1PeriphClockCmd(STOPWATCH_TIMER_RCC, ENABLE); + } - // enable interrupt request - TIM_ITConfig(TIM, TIM_IT_Update, ENABLE); + // time base configuration + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + TIM_TimeBaseStructure.TIM_Period = 0xffff; // max period + TIM_TimeBaseStructure.TIM_Prescaler = (72 * resolution) - 1; // uS accuracy @ 72 MHz + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInit(TIM, &TIM_TimeBaseStructure); - // start counter - TIM_Cmd(TIM, ENABLE); + // enable interrupt request + TIM_ITConfig(TIM, TIM_IT_Update, ENABLE); - return 0; // no error + // start counter + TIM_Cmd(TIM, ENABLE); + + return 0; // no error } ///////////////////////////////////////////////////////////////////////////// -//! Resets the stopwatch -//! \return < 0 on errors +// ! Resets the stopwatch +// ! \return < 0 on errors ///////////////////////////////////////////////////////////////////////////// -uint32_t STOPWATCH_Reset(TIM_TypeDef* TIM) { - // reset counter - TIM->CNT = 1; // set to 1 instead of 0 to avoid new IRQ request - TIM_ClearITPendingBit(TIM, TIM_IT_Update); +uint32_t STOPWATCH_Reset(TIM_TypeDef *TIM) +{ + // reset counter + TIM->CNT = 1; // set to 1 instead of 0 to avoid new IRQ request + TIM_ClearITPendingBit(TIM, TIM_IT_Update); - return 0; // no error + return 0; // no error } ///////////////////////////////////////////////////////////////////////////// -//! Returns current value of stopwatch -//! \return 1..65535: valid stopwatch value -//! \return 0xffffffff: counter overrun +// ! Returns current value of stopwatch +// ! \return 1..65535: valid stopwatch value +// ! \return 0xffffffff: counter overrun ///////////////////////////////////////////////////////////////////////////// -uint32_t STOPWATCH_ValueGet(TIM_TypeDef* TIM) { - uint32_t value = TIM->CNT; +uint32_t STOPWATCH_ValueGet(TIM_TypeDef *TIM) +{ + uint32_t value = TIM->CNT; - if (TIM_GetITStatus(TIM, TIM_IT_Update) != RESET) - value = 0xffffffff; + if (TIM_GetITStatus(TIM, TIM_IT_Update) != RESET) { + value = 0xffffffff; + } - return value; + return value; } - diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index 9b5dcbf91..5e55c5b98 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -47,19 +47,19 @@ #include "taskinfo.h" // Private constants -#define MAX_QUEUE_SIZE 2 +#define MAX_QUEUE_SIZE 2 #if defined(PIOS_ACTUATOR_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_ACTUATOR_STACK_SIZE +#define STACK_SIZE_BYTES PIOS_ACTUATOR_STACK_SIZE #else -#define STACK_SIZE_BYTES 1312 +#define STACK_SIZE_BYTES 1312 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY+4) -#define FAILSAFE_TIMEOUT_MS 100 -#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) +#define FAILSAFE_TIMEOUT_MS 100 +#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM -#define CAMERA_BOOT_DELAY_MS 7000 +#define CAMERA_BOOT_DELAY_MS 7000 // Private types @@ -68,30 +68,30 @@ static xQueueHandle queue; static xTaskHandle taskHandle; -static float lastResult[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0}; -static float filterAccumulator[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0}; +static float lastResult[MAX_MIX_ACTUATORS] = { 0, 0, 0, 0, 0, 0, 0, 0 }; +static float filterAccumulator[MAX_MIX_ACTUATORS] = { 0, 0, 0, 0, 0, 0, 0, 0 }; // used to inform the actuator thread that actuator update rate is changed static volatile bool actuator_settings_updated; // used to inform the actuator thread that mixer settings are changed static volatile bool mixer_settings_updated; // Private functions -static void actuatorTask(void* parameters); +static void actuatorTask(void *parameters); static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral); -static void setFailsafe(const ActuatorSettingsData * actuatorSettings, const MixerSettingsData * mixerSettings); -static float MixerCurve(const float throttle, const float* curve, uint8_t elements); -static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData * actuatorSettings); -static void actuator_update_rate_if_changed(const ActuatorSettingsData * actuatorSettings, bool force_update); -static void MixerSettingsUpdatedCb(UAVObjEvent * ev); -static void ActuatorSettingsUpdatedCb(UAVObjEvent * ev); +static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const MixerSettingsData *mixerSettings); +static float MixerCurve(const float throttle, const float *curve, uint8_t elements); +static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings); +static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update); +static void MixerSettingsUpdatedCb(UAVObjEvent *ev); +static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev); float ProcessMixer(const int index, const float curve1, const float curve2, - const MixerSettingsData* mixerSettings, ActuatorDesiredData* desired, - const float period); + const MixerSettingsData *mixerSettings, ActuatorDesiredData *desired, + const float period); -//this structure is equivalent to the UAVObjects for one mixer. +// this structure is equivalent to the UAVObjects for one mixer. typedef struct { - uint8_t type; - int8_t matrix[5]; + uint8_t type; + int8_t matrix[5]; } __attribute__((packed)) Mixer_t; /** @@ -100,14 +100,14 @@ typedef struct { */ int32_t ActuatorStart() { - // Start main task - xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ACTUATOR, taskHandle); + // Start main task + xTaskCreate(actuatorTask, (signed char *)"Actuator", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ACTUATOR, taskHandle); #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR); + PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR); #endif - return 0; + return 0; } /** @@ -116,28 +116,28 @@ int32_t ActuatorStart() */ int32_t ActuatorInitialize() { - // Register for notification of changes to ActuatorSettings - ActuatorSettingsInitialize(); - ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb); + // Register for notification of changes to ActuatorSettings + ActuatorSettingsInitialize(); + ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb); - // Register for notification of changes to MixerSettings - MixerSettingsInitialize(); - MixerSettingsConnectCallback(MixerSettingsUpdatedCb); + // Register for notification of changes to MixerSettings + MixerSettingsInitialize(); + MixerSettingsConnectCallback(MixerSettingsUpdatedCb); - // Listen for ActuatorDesired updates (Primary input to this module) - ActuatorDesiredInitialize(); - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - ActuatorDesiredConnectQueue(queue); + // Listen for ActuatorDesired updates (Primary input to this module) + ActuatorDesiredInitialize(); + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + ActuatorDesiredConnectQueue(queue); - // Primary output of this module - ActuatorCommandInitialize(); + // Primary output of this module + ActuatorCommandInitialize(); #ifdef DIAG_MIXERSTATUS - // UAVO only used for inspecting the internal status of the mixer during debug - MixerStatusInitialize(); + // UAVO only used for inspecting the internal status of the mixer during debug + MixerStatusInitialize(); #endif - return 0; + return 0; } MODULE_INITCALL(ActuatorInitialize, ActuatorStart) @@ -154,351 +154,337 @@ MODULE_INITCALL(ActuatorInitialize, ActuatorStart) * * @return -1 if error, 0 if success */ -static void actuatorTask(__attribute__((unused)) void* parameters) +static void actuatorTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; - portTickType lastSysTime; - portTickType thisSysTime; - float dTSeconds; - uint32_t dTMilliseconds; + UAVObjEvent ev; + portTickType lastSysTime; + portTickType thisSysTime; + float dTSeconds; + uint32_t dTMilliseconds; - ActuatorCommandData command; - ActuatorDesiredData desired; - MixerStatusData mixerStatus; - FlightStatusData flightStatus; + ActuatorCommandData command; + ActuatorDesiredData desired; + MixerStatusData mixerStatus; + FlightStatusData flightStatus; - /* Read initial values of ActuatorSettings */ - ActuatorSettingsData actuatorSettings; - actuator_settings_updated = false; - ActuatorSettingsGet(&actuatorSettings); + /* Read initial values of ActuatorSettings */ + ActuatorSettingsData actuatorSettings; - /* Read initial values of MixerSettings */ - MixerSettingsData mixerSettings; - mixer_settings_updated = false; - MixerSettingsGet(&mixerSettings); + actuator_settings_updated = false; + ActuatorSettingsGet(&actuatorSettings); - /* Force an initial configuration of the actuator update rates */ - actuator_update_rate_if_changed(&actuatorSettings, true); + /* Read initial values of MixerSettings */ + MixerSettingsData mixerSettings; + mixer_settings_updated = false; + MixerSettingsGet(&mixerSettings); - // Go to the neutral (failsafe) values until an ActuatorDesired update is received - setFailsafe(&actuatorSettings, &mixerSettings); + /* Force an initial configuration of the actuator update rates */ + actuator_update_rate_if_changed(&actuatorSettings, true); - // Main task loop - lastSysTime = xTaskGetTickCount(); - while (1) - { + // Go to the neutral (failsafe) values until an ActuatorDesired update is received + setFailsafe(&actuatorSettings, &mixerSettings); + + // Main task loop + lastSysTime = xTaskGetTickCount(); + while (1) { #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR); + PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR); #endif - // Wait until the ActuatorDesired object is updated - uint8_t rc = xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS); + // Wait until the ActuatorDesired object is updated + uint8_t rc = xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS); - /* Process settings updated events even in timeout case so we always act on the latest settings */ - if (actuator_settings_updated) { - actuator_settings_updated = false; - ActuatorSettingsGet (&actuatorSettings); - actuator_update_rate_if_changed (&actuatorSettings, false); - } - if (mixer_settings_updated) { - mixer_settings_updated = false; - MixerSettingsGet (&mixerSettings); - } + /* Process settings updated events even in timeout case so we always act on the latest settings */ + if (actuator_settings_updated) { + actuator_settings_updated = false; + ActuatorSettingsGet(&actuatorSettings); + actuator_update_rate_if_changed(&actuatorSettings, false); + } + if (mixer_settings_updated) { + mixer_settings_updated = false; + MixerSettingsGet(&mixerSettings); + } - if (rc != pdTRUE) { - /* Update of ActuatorDesired timed out. Go to failsafe */ - setFailsafe(&actuatorSettings, &mixerSettings); - continue; - } + if (rc != pdTRUE) { + /* Update of ActuatorDesired timed out. Go to failsafe */ + setFailsafe(&actuatorSettings, &mixerSettings); + continue; + } - // Check how long since last update - thisSysTime = xTaskGetTickCount(); - dTMilliseconds = (thisSysTime == lastSysTime)? 1: (thisSysTime - lastSysTime) * portTICK_RATE_MS; - lastSysTime = thisSysTime; - dTSeconds = dTMilliseconds * 0.001f; + // Check how long since last update + thisSysTime = xTaskGetTickCount(); + dTMilliseconds = (thisSysTime == lastSysTime) ? 1 : (thisSysTime - lastSysTime) * portTICK_RATE_MS; + lastSysTime = thisSysTime; + dTSeconds = dTMilliseconds * 0.001f; - FlightStatusGet(&flightStatus); - ActuatorDesiredGet(&desired); - ActuatorCommandGet(&command); + FlightStatusGet(&flightStatus); + ActuatorDesiredGet(&desired); + ActuatorCommandGet(&command); #ifdef DIAG_MIXERSTATUS - MixerStatusGet(&mixerStatus); + MixerStatusGet(&mixerStatus); #endif - int nMixers = 0; - Mixer_t * mixers = (Mixer_t *)&mixerSettings.Mixer1Type; - for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++) - { - if(mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) - { - nMixers ++; - } - } - if((nMixers < 2) && !ActuatorCommandReadOnly()) //Nothing can fly with less than two mixers. - { - setFailsafe(&actuatorSettings, &mixerSettings); // So that channels like PWM buzzer keep working - continue; - } + int nMixers = 0; + Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; + for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) { + if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) { + nMixers++; + } + } + if ((nMixers < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers. + setFailsafe(&actuatorSettings, &mixerSettings); // So that channels like PWM buzzer keep working + continue; + } - AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); + AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); - bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; - bool positiveThrottle = desired.Throttle >= 0.00f; - bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; + bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; + bool positiveThrottle = desired.Throttle >= 0.00f; + bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; - float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1,MIXERSETTINGS_THROTTLECURVE1_NUMELEM); - - //The source for the secondary curve is selectable - float curve2 = 0; - AccessoryDesiredData accessory; - switch(mixerSettings.Curve2Source) { - case MIXERSETTINGS_CURVE2SOURCE_THROTTLE: - curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM); - break; - case MIXERSETTINGS_CURVE2SOURCE_ROLL: - curve2 = MixerCurve(desired.Roll,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM); - break; - case MIXERSETTINGS_CURVE2SOURCE_PITCH: - curve2 = MixerCurve(desired.Pitch,mixerSettings.ThrottleCurve2, - MIXERSETTINGS_THROTTLECURVE2_NUMELEM); - break; - case MIXERSETTINGS_CURVE2SOURCE_YAW: - curve2 = MixerCurve(desired.Yaw,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM); - break; - case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE: - ManualControlCommandCollectiveGet(&curve2); - curve2 = MixerCurve(curve2,mixerSettings.ThrottleCurve2, - MIXERSETTINGS_THROTTLECURVE2_NUMELEM); - break; - case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0: - case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1: - case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY2: - case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY3: - case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4: - case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5: - if(AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0,&accessory) == 0) - curve2 = MixerCurve(accessory.AccessoryVal,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM); - else - curve2 = 0; - break; - } + float curve1 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM); - float * status = (float *)&mixerStatus; //access status objects as an array of floats + // The source for the secondary curve is selectable + float curve2 = 0; + AccessoryDesiredData accessory; + switch (mixerSettings.Curve2Source) { + case MIXERSETTINGS_CURVE2SOURCE_THROTTLE: + curve2 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + break; + case MIXERSETTINGS_CURVE2SOURCE_ROLL: + curve2 = MixerCurve(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + break; + case MIXERSETTINGS_CURVE2SOURCE_PITCH: + curve2 = MixerCurve(desired.Pitch, mixerSettings.ThrottleCurve2, + MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + break; + case MIXERSETTINGS_CURVE2SOURCE_YAW: + curve2 = MixerCurve(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + break; + case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE: + ManualControlCommandCollectiveGet(&curve2); + curve2 = MixerCurve(curve2, mixerSettings.ThrottleCurve2, + MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + break; + case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0: + case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1: + case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY2: + case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY3: + case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4: + case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5: + if (AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0, &accessory) == 0) { + curve2 = MixerCurve(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + } else { + curve2 = 0; + } + break; + } - for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++) - { - // During boot all camera actuators should be completely disabled (PWM pulse = 0). - // command.Channel[i] is reused below as a channel PWM activity flag: - // 0 - PWM disabled, >0 - PWM set to real mixer value using scaleChannel() later. - // Setting it to 1 by default means "Rescale this channel and enable PWM on its output". - command.Channel[ct] = 1; + float *status = (float *)&mixerStatus; // access status objects as an array of floats - if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) { - // Set to minimum if disabled. This is not the same as saying PWM pulse = 0 us - status[ct] = -1; - continue; - } + for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) { + // During boot all camera actuators should be completely disabled (PWM pulse = 0). + // command.Channel[i] is reused below as a channel PWM activity flag: + // 0 - PWM disabled, >0 - PWM set to real mixer value using scaleChannel() later. + // Setting it to 1 by default means "Rescale this channel and enable PWM on its output". + command.Channel[ct] = 1; - if((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) - status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds); - else - status[ct] = -1; + if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) { + // Set to minimum if disabled. This is not the same as saying PWM pulse = 0 us + status[ct] = -1; + continue; + } - // Motors have additional protection for when to be on - if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { + if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) { + status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds); + } else { + status[ct] = -1; + } - // If not armed or motors aren't meant to spin all the time - if( !armed || - (!spinWhileArmed && !positiveThrottle)) - { - filterAccumulator[ct] = 0; - lastResult[ct] = 0; - status[ct] = -1; //force min throttle - } - // If armed meant to keep spinning, - else if ((spinWhileArmed && !positiveThrottle) || - (status[ct] < 0) ) - status[ct] = 0; - } + // Motors have additional protection for when to be on + if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { + // If not armed or motors aren't meant to spin all the time + if (!armed || + (!spinWhileArmed && !positiveThrottle)) { + filterAccumulator[ct] = 0; + lastResult[ct] = 0; + status[ct] = -1; // force min throttle + } + // If armed meant to keep spinning, + else if ((spinWhileArmed && !positiveThrottle) || + (status[ct] < 0)) { + status[ct] = 0; + } + } - // If an accessory channel is selected for direct bypass mode - // In this configuration the accessory channel is scaled and mapped - // directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING - // these also will not be updated in failsafe mode. I'm not sure what - // the correct behavior is since it seems domain specific. I don't love - // this code - if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) && - (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) - { - if(AccessoryDesiredInstGet(mixers[ct].type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0,&accessory) == 0) - status[ct] = accessory.AccessoryVal; - else - status[ct] = -1; - } + // If an accessory channel is selected for direct bypass mode + // In this configuration the accessory channel is scaled and mapped + // directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING + // these also will not be updated in failsafe mode. I'm not sure what + // the correct behavior is since it seems domain specific. I don't love + // this code + if ((mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) && + (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) { + if (AccessoryDesiredInstGet(mixers[ct].type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0, &accessory) == 0) { + status[ct] = accessory.AccessoryVal; + } else { + status[ct] = -1; + } + } - if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) && - (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) - { - CameraDesiredData cameraDesired; - if( CameraDesiredGet(&cameraDesired) == 0 ) { - switch(mixers[ct].type) { - case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1: - status[ct] = cameraDesired.RollOrServo1; - break; - case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCHORSERVO2: - status[ct] = cameraDesired.PitchOrServo2; - break; - case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW: - status[ct] = cameraDesired.Yaw; - break; - default: - break; - } - } - else - status[ct] = -1; + if ((mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) && + (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) { + CameraDesiredData cameraDesired; + if (CameraDesiredGet(&cameraDesired) == 0) { + switch (mixers[ct].type) { + case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1: + status[ct] = cameraDesired.RollOrServo1; + break; + case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCHORSERVO2: + status[ct] = cameraDesired.PitchOrServo2; + break; + case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW: + status[ct] = cameraDesired.Yaw; + break; + default: + break; + } + } else { + status[ct] = -1; + } - // Disable camera actuators for CAMERA_BOOT_DELAY_MS after boot - if (thisSysTime < (CAMERA_BOOT_DELAY_MS / portTICK_RATE_MS)) - command.Channel[ct] = 0; - } - } + // Disable camera actuators for CAMERA_BOOT_DELAY_MS after boot + if (thisSysTime < (CAMERA_BOOT_DELAY_MS / portTICK_RATE_MS)) { + command.Channel[ct] = 0; + } + } + } - // Set real actuator output values scaling them from mixers. All channels - // will be set except explicitly disabled (which will have PWM pulse = 0). - for (int i = 0; i < MAX_MIX_ACTUATORS; i++) - if (command.Channel[i]) - command.Channel[i] = scaleChannel(status[i], - actuatorSettings.ChannelMax[i], - actuatorSettings.ChannelMin[i], - actuatorSettings.ChannelNeutral[i]); + // Set real actuator output values scaling them from mixers. All channels + // will be set except explicitly disabled (which will have PWM pulse = 0). + for (int i = 0; i < MAX_MIX_ACTUATORS; i++) { + if (command.Channel[i]) { + command.Channel[i] = scaleChannel(status[i], + actuatorSettings.ChannelMax[i], + actuatorSettings.ChannelMin[i], + actuatorSettings.ChannelNeutral[i]); + } + } - // Store update time - command.UpdateTime = dTMilliseconds; - if (command.UpdateTime > command.MaxUpdateTime) - command.MaxUpdateTime = command.UpdateTime; - - // Update output object - ActuatorCommandSet(&command); - // Update in case read only (eg. during servo configuration) - ActuatorCommandGet(&command); + // Store update time + command.UpdateTime = dTMilliseconds; + if (command.UpdateTime > command.MaxUpdateTime) { + command.MaxUpdateTime = command.UpdateTime; + } + + // Update output object + ActuatorCommandSet(&command); + // Update in case read only (eg. during servo configuration) + ActuatorCommandGet(&command); #ifdef DIAG_MIXERSTATUS - MixerStatusSet(&mixerStatus); + MixerStatusSet(&mixerStatus); #endif - - // Update servo outputs - bool success = true; - for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) - { - success &= set_channel(n, command.Channel[n], &actuatorSettings); - } + // Update servo outputs + bool success = true; - if(!success) { - command.NumFailedUpdates++; - ActuatorCommandSet(&command); - AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); - } + for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { + success &= set_channel(n, command.Channel[n], &actuatorSettings); + } - } + if (!success) { + command.NumFailedUpdates++; + ActuatorCommandSet(&command); + AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); + } + } } - /** - *Process mixing for one actuator + * Process mixing for one actuator */ float ProcessMixer(const int index, const float curve1, const float curve2, - const MixerSettingsData* mixerSettings, ActuatorDesiredData* desired, const float period) + const MixerSettingsData *mixerSettings, ActuatorDesiredData *desired, const float period) { - static float lastFilteredResult[MAX_MIX_ACTUATORS]; - const Mixer_t * mixers = (Mixer_t *)&mixerSettings->Mixer1Type; //pointer to array of mixers in UAVObjects - const Mixer_t * mixer = &mixers[index]; + static float lastFilteredResult[MAX_MIX_ACTUATORS]; + const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects + const Mixer_t *mixer = &mixers[index]; - float result = (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1] / 128.0f) * curve1) + - (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2] / 128.0f) * curve2) + - (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] / 128.0f) * desired->Roll) + - (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) + - (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw); + float result = (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1] / 128.0f) * curve1) + + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2] / 128.0f) * curve2) + + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] / 128.0f) * desired->Roll) + + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) + + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw); - if(mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) - { - if(result < 0.0f) //idle throttle - { - result = 0.0f; - } + if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { + if (result < 0.0f) { // idle throttle + result = 0.0f; + } - //feed forward - float accumulator = filterAccumulator[index]; - accumulator += (result - lastResult[index]) * mixerSettings->FeedForward; - lastResult[index] = result; - result += accumulator; - if(period > 0.0f) - { - if(accumulator > 0.0f) - { - float filter = mixerSettings->AccelTime / period; - if(filter <1) - { - filter = 1; - } - accumulator -= accumulator / filter; - }else - { - float filter = mixerSettings->DecelTime / period; - if(filter <1) - { - filter = 1; - } - accumulator -= accumulator / filter; - } - } - filterAccumulator[index] = accumulator; - result += accumulator; + // feed forward + float accumulator = filterAccumulator[index]; + accumulator += (result - lastResult[index]) * mixerSettings->FeedForward; + lastResult[index] = result; + result += accumulator; + if (period > 0.0f) { + if (accumulator > 0.0f) { + float filter = mixerSettings->AccelTime / period; + if (filter < 1) { + filter = 1; + } + accumulator -= accumulator / filter; + } else { + float filter = mixerSettings->DecelTime / period; + if (filter < 1) { + filter = 1; + } + accumulator -= accumulator / filter; + } + } + filterAccumulator[index] = accumulator; + result += accumulator; - //acceleration limit - float dt = result - lastFilteredResult[index]; - float maxDt = mixerSettings->MaxAccel * period; - if(dt > maxDt) //we are accelerating too hard - { - result = lastFilteredResult[index] + maxDt; - } - lastFilteredResult[index] = result; - } + // acceleration limit + float dt = result - lastFilteredResult[index]; + float maxDt = mixerSettings->MaxAccel * period; + if (dt > maxDt) { // we are accelerating too hard + result = lastFilteredResult[index] + maxDt; + } + lastFilteredResult[index] = result; + } - return(result); + return result; } /** - *Interpolate a throttle curve. Throttle input should be in the range 0 to 1. - *Output is in the range 0 to 1. + * Interpolate a throttle curve. Throttle input should be in the range 0 to 1. + * Output is in the range 0 to 1. */ -static float MixerCurve(const float throttle, const float* curve, uint8_t elements) +static float MixerCurve(const float throttle, const float *curve, uint8_t elements) { - float scale = throttle * (float) (elements - 1); - int idx1 = scale; - scale -= (float)idx1; //remainder - if(curve[0] < -1) - { - return(throttle); - } - if (idx1 < 0) - { - idx1 = 0; //clamp to lowest entry in table - scale = 0; - } - int idx2 = idx1 + 1; - if(idx2 >= elements) - { - idx2 = elements -1; //clamp to highest entry in table - if(idx1 >= elements) - { - idx1 = elements -1; - } - } - return curve[idx1] * (1.0f - scale) + curve[idx2] * scale; + float scale = throttle * (float)(elements - 1); + int idx1 = scale; + + scale -= (float)idx1; // remainder + if (curve[0] < -1) { + return throttle; + } + if (idx1 < 0) { + idx1 = 0; // clamp to lowest entry in table + scale = 0; + } + int idx2 = idx1 + 1; + if (idx2 >= elements) { + idx2 = elements - 1; // clamp to highest entry in table + if (idx1 >= elements) { + idx1 = elements - 1; + } + } + return curve[idx1] * (1.0f - scale) + curve[idx2] * scale; } @@ -507,231 +493,226 @@ static float MixerCurve(const float throttle, const float* curve, uint8_t elemen */ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral) { - int16_t valueScaled; - // Scale - if ( value >= 0.0f) - { - valueScaled = (int16_t)(value*((float)(max-neutral))) + neutral; - } - else - { - valueScaled = (int16_t)(value*((float)(neutral-min))) + neutral; - } + int16_t valueScaled; - if (max>min) - { - if( valueScaled > max ) valueScaled = max; - if( valueScaled < min ) valueScaled = min; - } - else - { - if( valueScaled < max ) valueScaled = max; - if( valueScaled > min ) valueScaled = min; - } + // Scale + if (value >= 0.0f) { + valueScaled = (int16_t)(value * ((float)(max - neutral))) + neutral; + } else { + valueScaled = (int16_t)(value * ((float)(neutral - min))) + neutral; + } - return valueScaled; + if (max > min) { + if (valueScaled > max) { + valueScaled = max; + } + if (valueScaled < min) { + valueScaled = min; + } + } else { + if (valueScaled < max) { + valueScaled = max; + } + if (valueScaled > min) { + valueScaled = min; + } + } + + return valueScaled; } /** * Set actuator output to the neutral values (failsafe) */ -static void setFailsafe(const ActuatorSettingsData * actuatorSettings, const MixerSettingsData * mixerSettings) +static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const MixerSettingsData *mixerSettings) { - /* grab only the parts that we are going to use */ - int16_t Channel[ACTUATORCOMMAND_CHANNEL_NUMELEM]; - ActuatorCommandChannelGet(Channel); + /* grab only the parts that we are going to use */ + int16_t Channel[ACTUATORCOMMAND_CHANNEL_NUMELEM]; - const Mixer_t * mixers = (Mixer_t *)&mixerSettings->Mixer1Type; //pointer to array of mixers in UAVObjects + ActuatorCommandChannelGet(Channel); - // Reset ActuatorCommand to safe values - for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) - { + const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects - if(mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) - { - Channel[n] = actuatorSettings->ChannelMin[n]; - } - else if(mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO) - { - Channel[n] = actuatorSettings->ChannelNeutral[n]; - } - else - { - Channel[n] = 0; - } - - - } + // Reset ActuatorCommand to safe values + for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { + if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { + Channel[n] = actuatorSettings->ChannelMin[n]; + } else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO) { + Channel[n] = actuatorSettings->ChannelNeutral[n]; + } else { + Channel[n] = 0; + } + } - // Set alarm - AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); + // Set alarm + AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); - // Update servo outputs - for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) - { - set_channel(n, Channel[n], actuatorSettings); - } + // Update servo outputs + for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { + set_channel(n, Channel[n], actuatorSettings); + } - // Update output object's parts that we changed - ActuatorCommandChannelSet(Channel); + // Update output object's parts that we changed + ActuatorCommandChannelSet(Channel); } /** * determine buzzer or blink sequence **/ -typedef enum {BUZZ_BUZZER=0,BUZZ_ARMING=1,BUZZ_INFO=2,BUZZ_MAX=3} buzzertype; +typedef enum { BUZZ_BUZZER = 0, BUZZ_ARMING = 1, BUZZ_INFO = 2, BUZZ_MAX = 3 } buzzertype; static inline bool buzzerState(buzzertype type) { - // This is for buzzers that take a PWM input + // This is for buzzers that take a PWM input - static uint32_t tune[BUZZ_MAX]={0}; - static uint32_t tunestate[BUZZ_MAX]={0}; + static uint32_t tune[BUZZ_MAX] = { 0 }; + static uint32_t tunestate[BUZZ_MAX] = { 0 }; - uint32_t newTune = 0; - if(type==BUZZ_BUZZER) - { - // Decide what tune to play - if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) { - newTune = 0b11110110110000; // pause, short, short, short, long - } else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) { - newTune = 0x80000000; // pause, short - } else { - newTune = 0; - } + uint32_t newTune = 0; - } else { // BUZZ_ARMING || BUZZ_INFO - uint8_t arming; - FlightStatusArmedGet(&arming); - //base idle tune - newTune = 0x80000000; // 0b1000... - - // Merge the error pattern for InfoLed - if(type==BUZZ_INFO) - { - if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) - { - newTune |= 0b00000000001111111011111110000000; - } - else if(AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) - { - newTune |= 0b00000000000000110110110000000000; - } - } - // fast double blink pattern if armed - if (arming == FLIGHTSTATUS_ARMED_ARMED) - newTune |= 0xA0000000; // 0b101000... + if (type == BUZZ_BUZZER) { + // Decide what tune to play + if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) { + newTune = 0b11110110110000; // pause, short, short, short, long + } else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) { + newTune = 0x80000000; // pause, short + } else { + newTune = 0; + } + } else { // BUZZ_ARMING || BUZZ_INFO + uint8_t arming; + FlightStatusArmedGet(&arming); + // base idle tune + newTune = 0x80000000; // 0b1000... - } + // Merge the error pattern for InfoLed + if (type == BUZZ_INFO) { + if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) { + newTune |= 0b00000000001111111011111110000000; + } else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) { + newTune |= 0b00000000000000110110110000000000; + } + } + // fast double blink pattern if armed + if (arming == FLIGHTSTATUS_ARMED_ARMED) { + newTune |= 0xA0000000; // 0b101000... + } + } - // Do we need to change tune? - if (newTune != tune[type]) { - tune[type] = newTune; - // resynchronize all tunes on change, so they stay in sync - for (int i=0;i lastSysTime) { - dT = thisSysTime - lastSysTime; - } else { - lastSysTime = 0; // avoid the case where SysTimeMax-lastSysTime <80 - } + // For now, only look at the battery alarm, because functions like AlarmsHasCritical() can block for some time; to be discussed + if (tune[type]) { + if (thisSysTime > lastSysTime) { + dT = thisSysTime - lastSysTime; + } else { + lastSysTime = 0; // avoid the case where SysTimeMax-lastSysTime <80 + } - buzzOn = (tunestate[type]&1); + buzzOn = (tunestate[type] & 1); - if (dT > 80) { - // Go to next bit in alarm_seq_state - for (int i=0;i>=1; - if (tunestate[i]==0) // All done, re-start the tune - tunestate[i]=tune[i]; - } - lastSysTime = thisSysTime; - } - } - return buzzOn; + if (dT > 80) { + // Go to next bit in alarm_seq_state + for (int i = 0; i < BUZZ_MAX; i++) { + tunestate[i] >>= 1; + if (tunestate[i] == 0) { // All done, re-start the tune + tunestate[i] = tune[i]; + } + } + lastSysTime = thisSysTime; + } + } + return buzzOn; } #if defined(ARCH_POSIX) || defined(ARCH_WIN32) -static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData * actuatorSettings) +static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings) { - return true; + return true; } #else -static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData * actuatorSettings) +static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings) { - switch(actuatorSettings->ChannelType[mixer_channel]) { - case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: - PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], - buzzerState(BUZZ_BUZZER)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]); - return true; - case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED: - PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], - buzzerState(BUZZ_ARMING)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]); - return true; - case ACTUATORSETTINGS_CHANNELTYPE_INFOLED: - PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], - buzzerState(BUZZ_INFO)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]); - return true; - case ACTUATORSETTINGS_CHANNELTYPE_PWM: - PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value); - return true; + switch (actuatorSettings->ChannelType[mixer_channel]) { + case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: + PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], + buzzerState(BUZZ_BUZZER) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]); + return true; + + case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED: + PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], + buzzerState(BUZZ_ARMING) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]); + return true; + + case ACTUATORSETTINGS_CHANNELTYPE_INFOLED: + PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], + buzzerState(BUZZ_INFO) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]); + return true; + + case ACTUATORSETTINGS_CHANNELTYPE_PWM: + PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value); + return true; + #if defined(PIOS_INCLUDE_I2C_ESC) - case ACTUATORSETTINGS_CHANNELTYPE_MK: - return PIOS_SetMKSpeed(actuatorSettings->ChannelAddr[mixer_channel],value); - case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4: - return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel],value); + case ACTUATORSETTINGS_CHANNELTYPE_MK: + return PIOS_SetMKSpeed(actuatorSettings->ChannelAddr[mixer_channel], value); + + case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4: + return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel], value); + #endif - default: - return false; - } - - return false; + default: + return false; + } + return false; } -#endif +#endif /* if defined(ARCH_POSIX) || defined(ARCH_WIN32) */ /** * @brief Update the servo update rate */ -static void actuator_update_rate_if_changed(const ActuatorSettingsData * actuatorSettings, bool force_update) +static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update) { - static uint16_t prevChannelUpdateFreq[ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM]; + static uint16_t prevChannelUpdateFreq[ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM]; - // check if the any rate setting is changed - if (force_update || - memcmp (prevChannelUpdateFreq, - actuatorSettings->ChannelUpdateFreq, - sizeof(prevChannelUpdateFreq)) != 0) { - /* Something has changed, apply the settings to HW */ - memcpy (prevChannelUpdateFreq, - actuatorSettings->ChannelUpdateFreq, - sizeof(prevChannelUpdateFreq)); - PIOS_Servo_SetHz(actuatorSettings->ChannelUpdateFreq, ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM); - } + // check if the any rate setting is changed + if (force_update || + memcmp(prevChannelUpdateFreq, + actuatorSettings->ChannelUpdateFreq, + sizeof(prevChannelUpdateFreq)) != 0) { + /* Something has changed, apply the settings to HW */ + memcpy(prevChannelUpdateFreq, + actuatorSettings->ChannelUpdateFreq, + sizeof(prevChannelUpdateFreq)); + PIOS_Servo_SetHz(actuatorSettings->ChannelUpdateFreq, ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM); + } } static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - actuator_settings_updated = true; + actuator_settings_updated = true; } static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - mixer_settings_updated = true; + mixer_settings_updated = true; } /** diff --git a/flight/modules/Actuator/inc/actuator.h b/flight/modules/Actuator/inc/actuator.h index 315a99daa..86362a944 100644 --- a/flight/modules/Actuator/inc/actuator.h +++ b/flight/modules/Actuator/inc/actuator.h @@ -1,11 +1,11 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup ActuatorModule Actuator Module * @brief Compute servo/motor settings based on @ref ActuatorDesired "desired actuator positions" and aircraft type. * This is where all the mixing of channels is computed. - * @{ + * @{ * * @file actuator.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -39,6 +39,6 @@ int32_t ActuatorInitialize(); #endif // ACTUATOR_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/Airspeed/revolution/airspeed.c b/flight/modules/Airspeed/revolution/airspeed.c index 63df98072..340cb3315 100644 --- a/flight/modules/Airspeed/revolution/airspeed.c +++ b/flight/modules/Airspeed/revolution/airspeed.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Calculate airspeed from diverse sources and update @ref Airspeed "Airspeed UAV Object" - * @{ + * @{ * * @file airspeed.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. @@ -50,21 +50,21 @@ #define STACK_SIZE_BYTES 500 -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types // Private variables static xTaskHandle taskHandle; -static bool airspeedEnabled = false; +static bool airspeedEnabled = false; static AirspeedSettingsData airspeedSettings; -static int8_t airspeedADCPin=-1; +static int8_t airspeedADCPin = -1; // Private functions static void airspeedTask(void *parameters); -static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev); +static void AirspeedSettingsUpdatedCb(UAVObjEvent *ev); /** @@ -72,17 +72,16 @@ static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev); * \returns 0 on success or -1 if initialisation failed */ int32_t AirspeedStart() -{ - - //Check if module is enabled or not - if (airspeedEnabled == false) { - return -1; - } - - // Start main task - xTaskCreate(airspeedTask, (signed char *)"Airspeed", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AIRSPEED, taskHandle); - return 0; +{ + // Check if module is enabled or not + if (airspeedEnabled == false) { + return -1; + } + + // Start main task + xTaskCreate(airspeedTask, (signed char *)"Airspeed", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AIRSPEED, taskHandle); + return 0; } /** @@ -92,38 +91,38 @@ int32_t AirspeedStart() int32_t AirspeedInitialize() { #ifdef MODULE_AIRSPEED_BUILTIN - airspeedEnabled = true; + airspeedEnabled = true; #else - - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); - - - if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - airspeedEnabled = true; - } else { - airspeedEnabled = false; - return -1; - } + + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + + + if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + airspeedEnabled = true; + } else { + airspeedEnabled = false; + return -1; + } #endif - - uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM]; - HwSettingsADCRoutingGet(adcRouting); - - //Determine if the barometric airspeed sensor is routed to an ADC pin - for (int i=0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { - if (adcRouting[i] == HWSETTINGS_ADCROUTING_ANALOGAIRSPEED) { - airspeedADCPin = i; - } - } - - AirspeedSensorInitialize(); - AirspeedSettingsInitialize(); - - AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb); - - return 0; + + uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM]; + HwSettingsADCRoutingGet(adcRouting); + + // Determine if the barometric airspeed sensor is routed to an ADC pin + for (int i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { + if (adcRouting[i] == HWSETTINGS_ADCROUTING_ANALOGAIRSPEED) { + airspeedADCPin = i; + } + } + + AirspeedSensorInitialize(); + AirspeedSettingsInitialize(); + + AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb); + + return 0; } MODULE_INITCALL(AirspeedInitialize, AirspeedStart) @@ -133,59 +132,54 @@ MODULE_INITCALL(AirspeedInitialize, AirspeedStart) */ static void airspeedTask(__attribute__((unused)) void *parameters) { - AirspeedSettingsUpdatedCb(AirspeedSettingsHandle()); - - AirspeedSensorData airspeedData; - AirspeedSensorGet(&airspeedData); + AirspeedSettingsUpdatedCb(AirspeedSettingsHandle()); - AirspeedSettingsUpdatedCb(NULL); + AirspeedSensorData airspeedData; + AirspeedSensorGet(&airspeedData); + + AirspeedSettingsUpdatedCb(NULL); - airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - - // Main task loop - portTickType lastSysTime = xTaskGetTickCount(); - while (1) - { - vTaskDelayUntil(&lastSysTime, airspeedSettings.SamplePeriod / portTICK_RATE_MS); - - // Update the airspeed object - AirspeedSensorGet(&airspeedData); + airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - switch (airspeedSettings.AirspeedSensorType) { + // Main task loop + portTickType lastSysTime = xTaskGetTickCount(); + while (1) { + vTaskDelayUntil(&lastSysTime, airspeedSettings.SamplePeriod / portTICK_RATE_MS); + + // Update the airspeed object + AirspeedSensorGet(&airspeedData); + + switch (airspeedSettings.AirspeedSensorType) { #if defined(PIOS_INCLUDE_MPXV) - case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: - case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004: - //MPXV5004 and MPXV7002 sensors - baro_airspeedGetMPXV(&airspeedData, &airspeedSettings,airspeedADCPin); - break; + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004: + // MPXV5004 and MPXV7002 sensors + baro_airspeedGetMPXV(&airspeedData, &airspeedSettings, airspeedADCPin); + break; #endif #if defined(PIOS_INCLUDE_ETASV3) - case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3: - //Eagletree Airspeed v3 - baro_airspeedGetETASV3(&airspeedData, &airspeedSettings); - break; + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3: + // Eagletree Airspeed v3 + baro_airspeedGetETASV3(&airspeedData, &airspeedSettings); + break; #endif - default: - airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - } + default: + airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + } - //Set the UAVO - AirspeedSensorSet(&airspeedData); - - } + // Set the UAVO + AirspeedSensorSet(&airspeedData); + } } - - static void AirspeedSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - AirspeedSettingsGet(&airspeedSettings); - + AirspeedSettingsGet(&airspeedSettings); } - - + + /** * @} * @} diff --git a/flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c b/flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c index 3f6012c30..e26262ec2 100644 --- a/flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c +++ b/flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Communicate with airspeed sensors and return values - * @{ + * @{ * * @file baro_airspeed.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -39,12 +39,12 @@ #include "openpilot.h" #include "hwsettings.h" #include "airspeedsettings.h" -#include "airspeedsensor.h" // object that will be updated by the module +#include "airspeedsensor.h" // object that will be updated by the module #if defined(PIOS_INCLUDE_ETASV3) -#define CALIBRATION_IDLE_MS 2000 //Time to wait before calibrating, in [ms] -#define CALIBRATION_COUNT_MS 2000 //Time to spend calibrating, in [ms] +#define CALIBRATION_IDLE_MS 2000 // Time to wait before calibrating, in [ms] +#define CALIBRATION_COUNT_MS 2000 // Time to spend calibrating, in [ms] // Private types @@ -52,48 +52,47 @@ // Private functions -static uint16_t calibrationCount=0; -static uint16_t calibrationCount2=0; -static uint32_t calibrationSum = 0; +static uint16_t calibrationCount = 0; +static uint16_t calibrationCount2 = 0; +static uint32_t calibrationSum = 0; -void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings){ +void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings) +{ + // Check to see if airspeed sensor is returning airspeedSensor + airspeedSensor->SensorValue = PIOS_ETASV3_ReadAirspeed(); + if (airspeedSensor->SensorValue == (uint16_t)-1) { + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + airspeedSensor->CalibratedAirspeed = 0; + return; + } - //Check to see if airspeed sensor is returning airspeedSensor - airspeedSensor->SensorValue = PIOS_ETASV3_ReadAirspeed(); - if (airspeedSensor->SensorValue==(uint16_t)-1) { - airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - airspeedSensor->CalibratedAirspeed = 0; - return; - } - - // only calibrate if no stored calibration is available - if (!airspeedSettings->ZeroPoint) { - //Calibrate sensor by averaging zero point value - if (calibrationCount <= CALIBRATION_IDLE_MS/airspeedSettings->SamplePeriod) { - calibrationCount++; - calibrationCount2++; - return; - } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) { - calibrationCount++; - calibrationCount2++; - calibrationSum += airspeedSensor->SensorValue; - if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) { + // only calibrate if no stored calibration is available + if (!airspeedSettings->ZeroPoint) { + // Calibrate sensor by averaging zero point value + if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { + calibrationCount++; + calibrationCount2++; + return; + } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { + calibrationCount++; + calibrationCount2++; + calibrationSum += airspeedSensor->SensorValue; + if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { + airspeedSettings->ZeroPoint = (int16_t)(((float)calibrationSum) / calibrationCount2); + AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint); + } + return; + } + } - airspeedSettings->ZeroPoint = (int16_t) (((float)calibrationSum) / calibrationCount2); - AirspeedSettingsZeroPointSet( &airspeedSettings->ZeroPoint ); - } - return; - } - } - - //Compute airspeed - airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint)); - airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + // Compute airspeed + airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint)); + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; } -#endif +#endif /* if defined(PIOS_INCLUDE_ETASV3) */ /** * @} diff --git a/flight/modules/Airspeed/revolution/baro_airspeed_mpxv.c b/flight/modules/Airspeed/revolution/baro_airspeed_mpxv.c index 540a4a91d..c05322dfc 100644 --- a/flight/modules/Airspeed/revolution/baro_airspeed_mpxv.c +++ b/flight/modules/Airspeed/revolution/baro_airspeed_mpxv.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Communicate with airspeed sensors and return values - * @{ + * @{ * * @file baro_airspeed.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -39,13 +39,13 @@ #include "openpilot.h" #include "hwsettings.h" #include "airspeedsettings.h" -#include "airspeedsensor.h" // object that will be updated by the module +#include "airspeedsensor.h" // object that will be updated by the module #if defined(PIOS_INCLUDE_MPXV) -#define CALIBRATION_IDLE_MS 2000 //Time to wait before calibrating, in [ms] -#define CALIBRATION_COUNT_MS 2000 //Time to spend calibrating, in [ms] -#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 100 //low pass filter +#define CALIBRATION_IDLE_MS 2000 // Time to wait before calibrating, in [ms] +#define CALIBRATION_COUNT_MS 2000 // Time to spend calibrating, in [ms] +#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 100 // low pass filter // Private types @@ -53,64 +53,63 @@ // Private functions -static uint16_t calibrationCount=0; +static uint16_t calibrationCount = 0; -PIOS_MPXV_descriptor sensor = { .type=PIOS_MPXV_UNKNOWN }; +PIOS_MPXV_descriptor sensor = { .type = PIOS_MPXV_UNKNOWN }; -void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings, int8_t airspeedADCPin){ +void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings, int8_t airspeedADCPin) +{ + // Ensure that the ADC pin is properly configured + if (airspeedADCPin < 0) { + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - //Ensure that the ADC pin is properly configured - if(airspeedADCPin <0){ - airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - - return; - } - if (sensor.type==PIOS_MPXV_UNKNOWN) { - switch (airspeedSettings->AirspeedSensorType) { - case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: - sensor = PIOS_MPXV_7002_DESC(airspeedADCPin); - break; - case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004: - sensor = PIOS_MPXV_5004_DESC(airspeedADCPin); - break; - default: - airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - return; - } - } + return; + } + if (sensor.type == PIOS_MPXV_UNKNOWN) { + switch (airspeedSettings->AirspeedSensorType) { + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: + sensor = PIOS_MPXV_7002_DESC(airspeedADCPin); + break; + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004: + sensor = PIOS_MPXV_5004_DESC(airspeedADCPin); + break; + default: + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + return; + } + } - airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor); + airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor); - if (!airspeedSettings->ZeroPoint) { - //Calibrate sensor by averaging zero point value - if (calibrationCount < CALIBRATION_IDLE_MS/airspeedSettings->SamplePeriod) { //First let sensor warm up and stabilize. - calibrationCount++; - return; - } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) { //Then compute the average. - calibrationCount++; /*DO NOT MOVE FROM BEFORE sensorCalibration=... LINE, OR ELSE WILL HAVE DIVIDE BY ZERO */ + if (!airspeedSettings->ZeroPoint) { + // Calibrate sensor by averaging zero point value + if (calibrationCount < CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { // First let sensor warm up and stabilize. + calibrationCount++; + return; + } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { // Then compute the average. + calibrationCount++; /*DO NOT MOVE FROM BEFORE sensorCalibration=... LINE, OR ELSE WILL HAVE DIVIDE BY ZERO */ - PIOS_MPXV_Calibrate(&sensor,airspeedSensor->SensorValue); + PIOS_MPXV_Calibrate(&sensor, airspeedSensor->SensorValue); - //Set settings UAVO. The airspeed UAVO is set elsewhere in the function. - if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) { - airspeedSettings->ZeroPoint = sensor.zeroPoint; - AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint); - } - return; - } - } - sensor.zeroPoint = airspeedSettings->ZeroPoint; - - //Filter CAS - float alpha=airspeedSettings->SamplePeriod/(airspeedSettings->SamplePeriod + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter. - - airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor,airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed*(1.0f-alpha); - airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + // Set settings UAVO. The airspeed UAVO is set elsewhere in the function. + if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { + airspeedSettings->ZeroPoint = sensor.zeroPoint; + AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint); + } + return; + } + } + sensor.zeroPoint = airspeedSettings->ZeroPoint; + // Filter CAS + float alpha = airspeedSettings->SamplePeriod / (airspeedSettings->SamplePeriod + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); // Low pass filter. + + airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor, airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed * (1.0f - alpha); + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; } -#endif +#endif /* if defined(PIOS_INCLUDE_MPXV) */ /** * @} diff --git a/flight/modules/Airspeed/revolution/gps_airspeed.c b/flight/modules/Airspeed/revolution/gps_airspeed.c index ec85d10d5..d81f66fef 100644 --- a/flight/modules/Airspeed/revolution/gps_airspeed.c +++ b/flight/modules/Airspeed/revolution/gps_airspeed.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Use GPS data to estimate airspeed - * @{ + * @{ * * @file gps_airspeed.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. @@ -39,17 +39,17 @@ // Private constants -#define GPS_AIRSPEED_BIAS_KP 0.1f //Needs to be settable in a UAVO -#define GPS_AIRSPEED_BIAS_KI 0.1f //Needs to be settable in a UAVO -#define SAMPLING_DELAY_MS_GPS 100 //Needs to be settable in a UAVO -#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f //Needs to be settable in a UAVO +#define GPS_AIRSPEED_BIAS_KP 0.1f // Needs to be settable in a UAVO +#define GPS_AIRSPEED_BIAS_KI 0.1f // Needs to be settable in a UAVO +#define SAMPLING_DELAY_MS_GPS 100 // Needs to be settable in a UAVO +#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f // Needs to be settable in a UAVO // Private types struct GPSGlobals { - float RbeCol1_old[3]; - float gpsVelOld_N; - float gpsVelOld_E; - float gpsVelOld_D; + float RbeCol1_old[3]; + float gpsVelOld_N; + float gpsVelOld_E; + float gpsVelOld_D; }; // Private variables @@ -62,89 +62,83 @@ static struct GPSGlobals *gps; */ void gps_airspeedInitialize() { - //This method saves memory in case we don't use the GPS module. - gps=(struct GPSGlobals *)pvPortMalloc(sizeof(struct GPSGlobals)); - - //GPS airspeed calculation variables - GPSVelocityData gpsVelData; - GPSVelocityGet(&gpsVelData); - - gps->gpsVelOld_N=gpsVelData.North; - gps->gpsVelOld_E=gpsVelData.East; - gps->gpsVelOld_D=gpsVelData.Down; - - AttitudeActualData attData; - AttitudeActualGet(&attData); - - float Rbe[3][3]; - float q[4] ={attData.q1, attData.q2, attData.q3, attData.q4}; - - //Calculate rotation matrix - Quaternion2R(q, Rbe); - - gps->RbeCol1_old[0]=Rbe[0][0]; - gps->RbeCol1_old[1]=Rbe[0][1]; - gps->RbeCol1_old[2]=Rbe[0][2]; + // This method saves memory in case we don't use the GPS module. + gps = (struct GPSGlobals *)pvPortMalloc(sizeof(struct GPSGlobals)); + + // GPS airspeed calculation variables + GPSVelocityData gpsVelData; + GPSVelocityGet(&gpsVelData); + + gps->gpsVelOld_N = gpsVelData.North; + gps->gpsVelOld_E = gpsVelData.East; + gps->gpsVelOld_D = gpsVelData.Down; + + AttitudeActualData attData; + AttitudeActualGet(&attData); + + float Rbe[3][3]; + float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 }; + + // Calculate rotation matrix + Quaternion2R(q, Rbe); + + gps->RbeCol1_old[0] = Rbe[0][0]; + gps->RbeCol1_old[1] = Rbe[0][1]; + gps->RbeCol1_old[2] = Rbe[0][2]; } /* * Calculate airspeed as a function of GPS groundspeed and vehicle attitude. * From "IMU Wind Estimation (Theory)", by William Premerlani. - * The idea is that V_gps=V_air+V_wind. If we assume wind constant, => + * The idea is that V_gps=V_air+V_wind. If we assume wind constant, => * V_gps_2-V_gps_1 = (V_air_2+V_wind_2) -(V_air_1+V_wind_1) = V_air_2 - V_air_1. * If we assume airspeed constant, => V_gps_2-V_gps_1 = |V|*(f_2 - f1), * where "f" is the fuselage vector in earth coordinates. * We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|. */ void gps_airspeedGet(float *v_air_GPS) -{ - float Rbe[3][3]; - - { //Scoping to save memory. We really just need Rbe. - AttitudeActualData attData; - AttitudeActualGet(&attData); - - float q[4] ={attData.q1, attData.q2, attData.q3, attData.q4}; - - //Calculate rotation matrix - Quaternion2R(q, Rbe); - } - - //Calculate the cos(angle) between the two fuselage basis vectors - float cosDiff=(Rbe[0][0]*gps->RbeCol1_old[0]) + (Rbe[0][1]*gps->RbeCol1_old[1]) + (Rbe[0][2]*gps->RbeCol1_old[2]); - - //If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue. - if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) { - GPSVelocityData gpsVelData; - GPSVelocityGet(&gpsVelData); - - //Calculate the norm^2 of the difference between the two GPS vectors - float normDiffGPS2 = powf(gpsVelData.North-gps->gpsVelOld_N,2.0f) + powf(gpsVelData.East-gps->gpsVelOld_E,2.0f) + powf(gpsVelData.Down-gps->gpsVelOld_D,2.0f); +{ + float Rbe[3][3]; - //Calculate the norm^2 of the difference between the two fuselage vectors - float normDiffAttitude2=powf(Rbe[0][0]-gps->RbeCol1_old[0],2.0f) + powf(Rbe[0][1]-gps->RbeCol1_old[1],2.0f) + powf(Rbe[0][2]-gps->RbeCol1_old[2],2.0f); - - //Airspeed magnitude is the ratio between the two difference norms - *v_air_GPS = sqrtf(normDiffGPS2/normDiffAttitude2); - - //Save old variables for next pass - gps->gpsVelOld_N=gpsVelData.North; - gps->gpsVelOld_E=gpsVelData.East; - gps->gpsVelOld_D=gpsVelData.Down; - - gps->RbeCol1_old[0]=Rbe[0][0]; - gps->RbeCol1_old[1]=Rbe[0][1]; - gps->RbeCol1_old[2]=Rbe[0][2]; - } - else { - - } + { // Scoping to save memory. We really just need Rbe. + AttitudeActualData attData; + AttitudeActualGet(&attData); - + float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 }; + + // Calculate rotation matrix + Quaternion2R(q, Rbe); + } + + // Calculate the cos(angle) between the two fuselage basis vectors + float cosDiff = (Rbe[0][0] * gps->RbeCol1_old[0]) + (Rbe[0][1] * gps->RbeCol1_old[1]) + (Rbe[0][2] * gps->RbeCol1_old[2]); + + // If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue. + if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) { + GPSVelocityData gpsVelData; + GPSVelocityGet(&gpsVelData); + + // Calculate the norm^2 of the difference between the two GPS vectors + float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f); + + // Calculate the norm^2 of the difference between the two fuselage vectors + float normDiffAttitude2 = powf(Rbe[0][0] - gps->RbeCol1_old[0], 2.0f) + powf(Rbe[0][1] - gps->RbeCol1_old[1], 2.0f) + powf(Rbe[0][2] - gps->RbeCol1_old[2], 2.0f); + + // Airspeed magnitude is the ratio between the two difference norms + *v_air_GPS = sqrtf(normDiffGPS2 / normDiffAttitude2); + + // Save old variables for next pass + gps->gpsVelOld_N = gpsVelData.North; + gps->gpsVelOld_E = gpsVelData.East; + gps->gpsVelOld_D = gpsVelData.Down; + + gps->RbeCol1_old[0] = Rbe[0][0]; + gps->RbeCol1_old[1] = Rbe[0][1]; + gps->RbeCol1_old[2] = Rbe[0][2]; + } else {} } - /** * @} * @} diff --git a/flight/modules/Airspeed/revolution/inc/airspeed.h b/flight/modules/Airspeed/revolution/inc/airspeed.h index c1bd9c8a7..5b28f1c92 100644 --- a/flight/modules/Airspeed/revolution/inc/airspeed.h +++ b/flight/modules/Airspeed/revolution/inc/airspeed.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Communicate with EagleTree Airspeed Sensor and update @ref BaroAirspeed "BaroAirspeed UAV Object" - * @{ + * @{ * * @file airspeed.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. diff --git a/flight/modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h b/flight/modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h index b19a2b631..77f09cf95 100644 --- a/flight/modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h +++ b/flight/modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements - * @{ + * @{ * * @file baro_airspeed_etasv3.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. diff --git a/flight/modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h b/flight/modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h index bb98da565..5f0072872 100644 --- a/flight/modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h +++ b/flight/modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements - * @{ + * @{ * * @file baro_airspeed_mpxv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. diff --git a/flight/modules/Airspeed/revolution/inc/gps_airspeed.h b/flight/modules/Airspeed/revolution/inc/gps_airspeed.h index 2e744a551..6e0903b8d 100644 --- a/flight/modules/Airspeed/revolution/inc/gps_airspeed.h +++ b/flight/modules/Airspeed/revolution/inc/gps_airspeed.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AirspeedModule Airspeed Module * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements - * @{ + * @{ * * @file gps_airspeed.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. diff --git a/flight/modules/Altitude/altitude.c b/flight/modules/Altitude/altitude.c index 8d40706b3..c6302c39f 100644 --- a/flight/modules/Altitude/altitude.c +++ b/flight/modules/Altitude/altitude.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AltitudeModule Altitude Module * @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object" - * @{ + * @{ * * @file altitude.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -41,17 +41,17 @@ #include "hwsettings.h" #include "altitude.h" #if defined(PIOS_INCLUDE_BMP085) -#include "baroaltitude.h" // object that will be updated by the module +#include "baroaltitude.h" // object that will be updated by the module #endif #if defined(PIOS_INCLUDE_HCSR04) -#include "sonaraltitude.h" // object that will be updated by the module +#include "sonaraltitude.h" // object that will be updated by the module #endif #include "taskinfo.h" // Private constants #define STACK_SIZE_BYTES 500 -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) -#define UPDATE_PERIOD 50 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define UPDATE_PERIOD 50 // Private types @@ -60,16 +60,16 @@ static xTaskHandle taskHandle; // down sampling variables #if defined(PIOS_INCLUDE_BMP085) -#define alt_ds_size 4 +#define alt_ds_size 4 static int32_t alt_ds_temp = 0; static int32_t alt_ds_pres = 0; -static int alt_ds_count = 0; +static int alt_ds_count = 0; #endif static bool altitudeEnabled; static uint8_t hwsettings_rcvrport;; - // Private functions +// Private functions static void altitudeTask(void *parameters); /** @@ -78,21 +78,20 @@ static void altitudeTask(void *parameters); */ int32_t AltitudeStart() { - - if (altitudeEnabled) { + if (altitudeEnabled) { #if defined(PIOS_INCLUDE_BMP085) - BaroAltitudeInitialize(); + BaroAltitudeInitialize(); #endif #if defined(PIOS_INCLUDE_HCSR04) - SonarAltitudeInitialize(); + SonarAltitudeInitialize(); #endif - // Start main task - xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle); - return 0; - } - return -1; + // Start main task + xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle); + return 0; + } + return -1; } /** @@ -102,26 +101,26 @@ int32_t AltitudeStart() int32_t AltitudeInitialize() { #ifdef MODULE_ALTITUDE_BUILTIN - altitudeEnabled = 1; + altitudeEnabled = 1; #else - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_ALTITUDE] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - altitudeEnabled = 1; - } else { - altitudeEnabled = 0; - } + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + if (optionalModules[HWSETTINGS_OPTIONALMODULES_ALTITUDE] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + altitudeEnabled = 1; + } else { + altitudeEnabled = 0; + } #endif #if defined(PIOS_INCLUDE_BMP085) - // init down-sampling data - alt_ds_temp = 0; - alt_ds_pres = 0; - alt_ds_count = 0; + // init down-sampling data + alt_ds_temp = 0; + alt_ds_pres = 0; + alt_ds_count = 0; #endif - HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport); - return 0; + HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport); + return 0; } MODULE_INITCALL(AltitudeInitialize, AltitudeStart) /** @@ -129,93 +128,91 @@ MODULE_INITCALL(AltitudeInitialize, AltitudeStart) */ static void altitudeTask(__attribute__((unused)) void *parameters) { - portTickType lastSysTime; - + portTickType lastSysTime; + #if defined(PIOS_INCLUDE_HCSR04) - SonarAltitudeData sonardata; - int32_t value = 0, timeout = 5; - float coeff = 0.25, height_out = 0, height_in = 0; - if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) { - PIOS_HCSR04_Trigger(); - } + SonarAltitudeData sonardata; + int32_t value = 0, timeout = 5; + float coeff = 0.25, height_out = 0, height_in = 0; + if (hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_DISABLED) { + PIOS_HCSR04_Trigger(); + } #endif #if defined(PIOS_INCLUDE_BMP085) - BaroAltitudeData data; - PIOS_BMP085_Init(); + BaroAltitudeData data; + PIOS_BMP085_Init(); #endif - - // Main task loop - lastSysTime = xTaskGetTickCount(); - while (1) - { + + // Main task loop + lastSysTime = xTaskGetTickCount(); + while (1) { #if defined(PIOS_INCLUDE_HCSR04) - // Compute the current altitude - if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) { - if(PIOS_HCSR04_Completed()) { - value = PIOS_HCSR04_Get(); - //from 3.4cm to 5.1m - if((value > 100) && (value < 15000)) { - height_in = value * 0.00034f / 2.0f; - height_out = (height_out * (1 - coeff)) + (height_in * coeff); - sonardata.Altitude = height_out; // m/us - } + // Compute the current altitude + if (hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_DISABLED) { + if (PIOS_HCSR04_Completed()) { + value = PIOS_HCSR04_Get(); + // from 3.4cm to 5.1m + if ((value > 100) && (value < 15000)) { + height_in = value * 0.00034f / 2.0f; + height_out = (height_out * (1 - coeff)) + (height_in * coeff); + sonardata.Altitude = height_out; // m/us + } - // Update the AltitudeActual UAVObject - SonarAltitudeSet(&sonardata); - timeout = 5; - PIOS_HCSR04_Trigger(); - } - if(!(timeout--)) { - //retrigger - timeout = 5; - PIOS_HCSR04_Trigger(); - } - } -#endif + // Update the AltitudeActual UAVObject + SonarAltitudeSet(&sonardata); + timeout = 5; + PIOS_HCSR04_Trigger(); + } + if (!(timeout--)) { + // retrigger + timeout = 5; + PIOS_HCSR04_Trigger(); + } + } +#endif /* if defined(PIOS_INCLUDE_HCSR04) */ #if defined(PIOS_INCLUDE_BMP085) - // Update the temperature data - PIOS_BMP085_StartADC(TemperatureConv); + // Update the temperature data + PIOS_BMP085_StartADC(TemperatureConv); #ifdef PIOS_BMP085_HAS_GPIOS - xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); + xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); #else - vTaskDelay(5 / portTICK_RATE_MS); + vTaskDelay(5 / portTICK_RATE_MS); #endif - PIOS_BMP085_ReadADC(); - alt_ds_temp += PIOS_BMP085_GetTemperature(); - - // Update the pressure data - PIOS_BMP085_StartADC(PressureConv); + PIOS_BMP085_ReadADC(); + alt_ds_temp += PIOS_BMP085_GetTemperature(); + + // Update the pressure data + PIOS_BMP085_StartADC(PressureConv); #ifdef PIOS_BMP085_HAS_GPIOS - xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); + xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); #else - vTaskDelay(26 / portTICK_RATE_MS); + vTaskDelay(26 / portTICK_RATE_MS); #endif - PIOS_BMP085_ReadADC(); - alt_ds_pres += PIOS_BMP085_GetPressure(); - - if (++alt_ds_count >= alt_ds_size) - { - alt_ds_count = 0; + PIOS_BMP085_ReadADC(); + alt_ds_pres += PIOS_BMP085_GetPressure(); - // Convert from 1/10ths of degC to degC - data.Temperature = alt_ds_temp / (10.0 * alt_ds_size); - alt_ds_temp = 0; + if (++alt_ds_count >= alt_ds_size) { + alt_ds_count = 0; - // Convert from Pa to kPa - data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size); - alt_ds_pres = 0; + // Convert from 1/10ths of degC to degC + data.Temperature = alt_ds_temp / (10.0 * alt_ds_size); + alt_ds_temp = 0; - // Compute the current altitude (all pressures in kPa) - data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255))); + // Convert from Pa to kPa + data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size); + alt_ds_pres = 0; - // Update the AltitudeActual UAVObject - BaroAltitudeSet(&data); - } -#endif + // Compute the current altitude (all pressures in kPa) + data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255))); - // Delay until it is time to read the next sample - vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS); - } + // Update the AltitudeActual UAVObject + BaroAltitudeSet(&data); + } +#endif /* if defined(PIOS_INCLUDE_BMP085) */ + + // Delay until it is time to read the next sample + vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS); + } } /** diff --git a/flight/modules/Altitude/inc/altitude.h b/flight/modules/Altitude/inc/altitude.h index 2dad6acc2..651f98072 100644 --- a/flight/modules/Altitude/inc/altitude.h +++ b/flight/modules/Altitude/inc/altitude.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AltitudeModule Altitude Module * @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object" - * @{ + * @{ * * @file altitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index c6449bb4d..0c0c59664 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AltitudeModule Altitude Module * @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object" - * @{ + * @{ * * @file altitude.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -39,15 +39,15 @@ #include #include "altitude.h" -#include "baroaltitude.h" // object that will be updated by the module +#include "baroaltitude.h" // object that will be updated by the module #if defined(PIOS_INCLUDE_HCSR04) -#include "sonaraltitude.h" // object that will be updated by the module +#include "sonaraltitude.h" // object that will be updated by the module #endif #include "taskinfo.h" // Private constants #define STACK_SIZE_BYTES 500 -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types @@ -62,12 +62,12 @@ static void altitudeTask(void *parameters); * \returns 0 on success or -1 if initialisation failed */ int32_t AltitudeStart() -{ - // Start main task - xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle); +{ + // Start main task + xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle); - return 0; + return 0; } /** @@ -76,11 +76,11 @@ int32_t AltitudeStart() */ int32_t AltitudeInitialize() { - BaroAltitudeInitialize(); + BaroAltitudeInitialize(); #if defined(PIOS_INCLUDE_HCSR04) - SonarAltitudeInitialize(); + SonarAltitudeInitialize(); #endif - return 0; + return 0; } MODULE_INITCALL(AltitudeInitialize, AltitudeStart) /** @@ -88,85 +88,83 @@ MODULE_INITCALL(AltitudeInitialize, AltitudeStart) */ static void altitudeTask(__attribute__((unused)) void *parameters) { - BaroAltitudeData data; - + BaroAltitudeData data; + #if defined(PIOS_INCLUDE_HCSR04) - SonarAltitudeData sonardata; - int32_t value = 0, timeout = 10, sample_rate = 0; - float coeff = 0.25, height_out = 0, height_in = 0; - PIOS_HCSR04_Trigger(); + SonarAltitudeData sonardata; + int32_t value = 0, timeout = 10, sample_rate = 0; + float coeff = 0.25, height_out = 0, height_in = 0; + PIOS_HCSR04_Trigger(); #endif - // TODO: Check the pressure sensor and set a warning if it fails test - + // TODO: Check the pressure sensor and set a warning if it fails test + // Option to change the interleave between Temp and Pressure conversions // Undef for normal operation -//#define PIOS_MS5611_SLOW_TEMP_RATE 20 - +// #define PIOS_MS5611_SLOW_TEMP_RATE 20 + #ifdef PIOS_MS5611_SLOW_TEMP_RATE - uint8_t temp_press_interleave_count = 1; + uint8_t temp_press_interleave_count = 1; #endif - // Main task loop - while (1) - { + // Main task loop + while (1) { #if defined(PIOS_INCLUDE_HCSR04) - // Compute the current altitude - // depends on baro samplerate - if(!(sample_rate--)) { - if(PIOS_HCSR04_Completed()) { - value = PIOS_HCSR04_Get(); - //from 3.4cm to 5.1m - if((value > 100) && (value < 15000)) { - height_in = value * 0.00034f / 2.0f; - height_out = (height_out * (1 - coeff)) + (height_in * coeff); - sonardata.Altitude = height_out; // m/us - } + // Compute the current altitude + // depends on baro samplerate + if (!(sample_rate--)) { + if (PIOS_HCSR04_Completed()) { + value = PIOS_HCSR04_Get(); + // from 3.4cm to 5.1m + if ((value > 100) && (value < 15000)) { + height_in = value * 0.00034f / 2.0f; + height_out = (height_out * (1 - coeff)) + (height_in * coeff); + sonardata.Altitude = height_out; // m/us + } - // Update the SonarAltitude UAVObject - SonarAltitudeSet(&sonardata); - timeout = 10; - PIOS_HCSR04_Trigger(); - } - if(!(timeout--)) { - //retrigger - timeout = 10; - PIOS_HCSR04_Trigger(); - } - sample_rate = 25; - } -#endif - float temp, press; + // Update the SonarAltitude UAVObject + SonarAltitudeSet(&sonardata); + timeout = 10; + PIOS_HCSR04_Trigger(); + } + if (!(timeout--)) { + // retrigger + timeout = 10; + PIOS_HCSR04_Trigger(); + } + sample_rate = 25; + } +#endif /* if defined(PIOS_INCLUDE_HCSR04) */ + float temp, press; #ifdef PIOS_MS5611_SLOW_TEMP_RATE - temp_press_interleave_count --; - if(temp_press_interleave_count == 0) - { + temp_press_interleave_count--; + if (temp_press_interleave_count == 0) { #endif - // Update the temperature data - PIOS_MS5611_StartADC(TemperatureConv); - vTaskDelay(PIOS_MS5611_GetDelay()); - PIOS_MS5611_ReadADC(); - -#ifdef PIOS_MS5611_SLOW_TEMP_RATE - temp_press_interleave_count=PIOS_MS5611_SLOW_TEMP_RATE; - } -#endif - - // Update the pressure data - PIOS_MS5611_StartADC(PressureConv); - vTaskDelay(PIOS_MS5611_GetDelay()); - PIOS_MS5611_ReadADC(); + // Update the temperature data + PIOS_MS5611_StartADC(TemperatureConv); + vTaskDelay(PIOS_MS5611_GetDelay()); + PIOS_MS5611_ReadADC(); - - temp = PIOS_MS5611_GetTemperature(); - press = PIOS_MS5611_GetPressure(); - - data.Temperature = temp; - data.Pressure = press; - data.Altitude = 44330.0f * (1.0f - powf(data.Pressure / MS5611_P0, (1.0f / 5.255f))); - - // Update the AltitudeActual UAVObject - BaroAltitudeSet(&data); - } +#ifdef PIOS_MS5611_SLOW_TEMP_RATE + temp_press_interleave_count = PIOS_MS5611_SLOW_TEMP_RATE; + } +#endif + + // Update the pressure data + PIOS_MS5611_StartADC(PressureConv); + vTaskDelay(PIOS_MS5611_GetDelay()); + PIOS_MS5611_ReadADC(); + + + temp = PIOS_MS5611_GetTemperature(); + press = PIOS_MS5611_GetPressure(); + + data.Temperature = temp; + data.Pressure = press; + data.Altitude = 44330.0f * (1.0f - powf(data.Pressure / MS5611_P0, (1.0f / 5.255f))); + + // Update the AltitudeActual UAVObject + BaroAltitudeSet(&data); + } } /** diff --git a/flight/modules/Altitude/revolution/inc/altitude.h b/flight/modules/Altitude/revolution/inc/altitude.h index 2dad6acc2..651f98072 100644 --- a/flight/modules/Altitude/revolution/inc/altitude.h +++ b/flight/modules/Altitude/revolution/inc/altitude.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AltitudeModule Altitude Module * @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object" - * @{ + * @{ * * @file altitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index e7eaabbc7..823be87ca 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -3,7 +3,7 @@ * * @file guidance.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint + * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint * and sets @ref AttitudeDesired. It only does this when the FlightMode field * of @ref ManualControlCommand is Auto. * @@ -50,7 +50,7 @@ #include #include #include -#include // object that will be updated by the module +#include // object that will be updated by the module #include #include #include @@ -59,9 +59,9 @@ #include #include // Private constants -#define MAX_QUEUE_SIZE 2 +#define MAX_QUEUE_SIZE 2 #define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define ACCEL_DOWNSAMPLE 4 // Private types @@ -72,7 +72,7 @@ static AltitudeHoldSettingsData altitudeHoldSettings; // Private functions static void altitudeHoldTask(void *parameters); -static void SettingsUpdatedCb(UAVObjEvent * ev); +static void SettingsUpdatedCb(UAVObjEvent *ev); /** * Initialise the module, called on startup @@ -80,11 +80,11 @@ static void SettingsUpdatedCb(UAVObjEvent * ev); */ int32_t AltitudeHoldStart() { - // Start main task - xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle); + // Start main task + xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle); - return 0; + return 0; } /** @@ -93,16 +93,16 @@ int32_t AltitudeHoldStart() */ int32_t AltitudeHoldInitialize() { - AltitudeHoldSettingsInitialize(); - AltitudeHoldDesiredInitialize(); - AltHoldSmoothedInitialize(); + AltitudeHoldSettingsInitialize(); + AltitudeHoldDesiredInitialize(); + AltHoldSmoothedInitialize(); - // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + // Create object queue + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); + AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); - return 0; + return 0; } MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart) @@ -122,274 +122,278 @@ float starting_altitude; */ static void altitudeHoldTask(__attribute__((unused)) void *parameters) { - AltitudeHoldDesiredData altitudeHoldDesired; - StabilizationDesiredData stabilizationDesired; + AltitudeHoldDesiredData altitudeHoldDesired; + StabilizationDesiredData stabilizationDesired; - portTickType thisTime, lastUpdateTime; - UAVObjEvent ev; - - // Force update of the settings - SettingsUpdatedCb(&ev); + portTickType thisTime, lastUpdateTime; + UAVObjEvent ev; - // Listen for updates. - AltitudeHoldDesiredConnectQueue(queue); - BaroAltitudeConnectQueue(queue); - FlightStatusConnectQueue(queue); - AccelsConnectQueue(queue); - - BaroAltitudeAltitudeGet(&smoothed_altitude); - running = false; - enum init_state {WAITING_BARO, WAITIING_INIT, INITED} init = WAITING_BARO; + // Force update of the settings + SettingsUpdatedCb(&ev); - // Main task loop - bool baro_updated = false; - while (1) { - // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe - if ( xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE ) - { - if(!running) - throttleIntegral = 0; + // Listen for updates. + AltitudeHoldDesiredConnectQueue(queue); + BaroAltitudeConnectQueue(queue); + FlightStatusConnectQueue(queue); + AccelsConnectQueue(queue); - // Todo: Add alarm if it should be running - continue; - } else if (ev.obj == BaroAltitudeHandle()) { - baro_updated = true; - - init = (init == WAITING_BARO) ? WAITIING_INIT : init; - } else if (ev.obj == FlightStatusHandle()) { - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); + BaroAltitudeAltitudeGet(&smoothed_altitude); + running = false; + enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO; - if(flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !running) { - // Copy the current throttle as a starting point for integral - StabilizationDesiredThrottleGet(&throttleIntegral); - switchThrottle = throttleIntegral; - error = 0; - velocity = 0; - running = true; + // Main task loop + bool baro_updated = false; + while (1) { + // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe + if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) { + if (!running) { + throttleIntegral = 0; + } - AltHoldSmoothedData altHold; - AltHoldSmoothedGet(&altHold); - starting_altitude = altHold.Altitude; - } else if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) - running = false; - } else if (ev.obj == AccelsHandle()) { - static uint32_t timeval; - - static float z[4] = {0, 0, 0, 0}; - float z_new[4]; - float P[4][4], K[4][2], x[2]; - float G[4] = {1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7f}; - static float V[4][4] = {{10.0f, 0.0f, 0.0f, 0.0f}, - {0.0f, 100.0f, 0.0f, 0.0f}, - {0.0f, 0.0f, 100.0f, 0.0f}, - {0.0f, 0.0f, 0.0f, 1000.0f}}; - static uint32_t accel_downsample_count = 0; - static float accels_accum[3] = {0.0f, 0.0f, 0.0f}; - float dT; - static float S[2] = {1.0f,10.0f}; + // Todo: Add alarm if it should be running + continue; + } else if (ev.obj == BaroAltitudeHandle()) { + baro_updated = true; - /* Somehow this always assigns to zero. Compiler bug? Race condition? */ - S[0] = altitudeHoldSettings.PressureNoise; - S[1] = altitudeHoldSettings.AccelNoise; - G[2] = altitudeHoldSettings.AccelDrift; + init = (init == WAITING_BARO) ? WAITIING_INIT : init; + } else if (ev.obj == FlightStatusHandle()) { + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); - AccelsData accels; - AccelsGet(&accels); - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - BaroAltitudeData baro; - BaroAltitudeGet(&baro); + if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !running) { + // Copy the current throttle as a starting point for integral + StabilizationDesiredThrottleGet(&throttleIntegral); + switchThrottle = throttleIntegral; + error = 0; + velocity = 0; + running = true; - /* Downsample accels to stop this calculation consuming too much CPU */ - accels_accum[0] += accels.x; - accels_accum[1] += accels.y; - accels_accum[2] += accels.z; - accel_downsample_count++; + AltHoldSmoothedData altHold; + AltHoldSmoothedGet(&altHold); + starting_altitude = altHold.Altitude; + } else if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) { + running = false; + } + } else if (ev.obj == AccelsHandle()) { + static uint32_t timeval; - if (accel_downsample_count < ACCEL_DOWNSAMPLE) - continue; + static float z[4] = { 0, 0, 0, 0 }; + float z_new[4]; + float P[4][4], K[4][2], x[2]; + float G[4] = { 1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7f }; + static float V[4][4] = { + { 10.0f, 0.0f, 0.0f, 0.0f }, + { 0.0f, 100.0f, 0.0f, 0.0f }, + { 0.0f, 0.0f, 100.0f, 0.0f }, + { 0.0f, 0.0f, 0.0f, 1000.0f } + }; + static uint32_t accel_downsample_count = 0; + static float accels_accum[3] = { 0.0f, 0.0f, 0.0f }; + float dT; + static float S[2] = { 1.0f, 10.0f }; - accel_downsample_count = 0; - accels.x = accels_accum[0] / ACCEL_DOWNSAMPLE; - accels.y = accels_accum[1] / ACCEL_DOWNSAMPLE; - accels.z = accels_accum[2] / ACCEL_DOWNSAMPLE; - accels_accum[0] = accels_accum[1] = accels_accum[2] = 0; + /* Somehow this always assigns to zero. Compiler bug? Race condition? */ + S[0] = altitudeHoldSettings.PressureNoise; + S[1] = altitudeHoldSettings.AccelNoise; + G[2] = altitudeHoldSettings.AccelDrift; - thisTime = xTaskGetTickCount(); + AccelsData accels; + AccelsGet(&accels); + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); + BaroAltitudeData baro; + BaroAltitudeGet(&baro); - if (init == WAITIING_INIT) { - z[0] = baro.Altitude; - z[1] = 0; - z[2] = accels.z; - z[3] = 0; - init = INITED; - } else if (init == WAITING_BARO) - continue; + /* Downsample accels to stop this calculation consuming too much CPU */ + accels_accum[0] += accels.x; + accels_accum[1] += accels.y; + accels_accum[2] += accels.z; + accel_downsample_count++; - x[0] = baro.Altitude; - //rotate avg accels into earth frame and store it - if(1) { - float q[4], Rbe[3][3]; - q[0]=attitudeActual.q1; - q[1]=attitudeActual.q2; - q[2]=attitudeActual.q3; - q[3]=attitudeActual.q4; - Quaternion2R(q, Rbe); - x[1] = -(Rbe[0][2]*accels.x+ Rbe[1][2]*accels.y + Rbe[2][2]*accels.z + 9.81f); - } else { - x[1] = -accels.z + 9.81f; - } + if (accel_downsample_count < ACCEL_DOWNSAMPLE) { + continue; + } - dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; - timeval = PIOS_DELAY_GetRaw(); + accel_downsample_count = 0; + accels.x = accels_accum[0] / ACCEL_DOWNSAMPLE; + accels.y = accels_accum[1] / ACCEL_DOWNSAMPLE; + accels.z = accels_accum[2] / ACCEL_DOWNSAMPLE; + accels_accum[0] = accels_accum[1] = accels_accum[2] = 0; - P[0][0] = dT*(V[0][1]+dT*V[1][1])+V[0][0]+G[0]+dT*V[1][0]; - P[0][1] = dT*(V[0][2]+dT*V[1][2])+V[0][1]+dT*V[1][1]; - P[0][2] = V[0][2]+dT*V[1][2]; - P[0][3] = V[0][3]+dT*V[1][3]; - P[1][0] = dT*(V[1][1]+dT*V[2][1])+V[1][0]+dT*V[2][0]; - P[1][1] = dT*(V[1][2]+dT*V[2][2])+V[1][1]+G[1]+dT*V[2][1]; - P[1][2] = V[1][2]+dT*V[2][2]; - P[1][3] = V[1][3]+dT*V[2][3]; - P[2][0] = V[2][0]+dT*V[2][1]; - P[2][1] = V[2][1]+dT*V[2][2]; - P[2][2] = V[2][2]+G[2]; - P[2][3] = V[2][3]; - P[3][0] = V[3][0]+dT*V[3][1]; - P[3][1] = V[3][1]+dT*V[3][2]; - P[3][2] = V[3][2]; - P[3][3] = V[3][3]+G[3]; + thisTime = xTaskGetTickCount(); - if (baro_updated) { - K[0][0] = -(V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3])+1.0f; - K[0][1] = ((V[0][2]+V[0][3])*S[0]+dT*(V[1][2]+V[1][3])*S[0])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); - K[1][0] = (V[1][0]*G[2]+V[1][0]*G[3]+V[1][0]*S[1]+V[1][0]*V[2][2]-V[2][0]*V[1][2]+V[1][0]*V[2][3]+V[1][0]*V[3][2]-V[2][0]*V[1][3]-V[1][2]*V[3][0]+V[1][0]*V[3][3]-V[3][0]*V[1][3]+(dT*dT)*V[2][1]*V[3][2]-(dT*dT)*V[2][2]*V[3][1]+(dT*dT)*V[2][1]*V[3][3]-(dT*dT)*V[3][1]*V[2][3]+dT*V[1][1]*G[2]+dT*V[2][0]*G[2]+dT*V[1][1]*G[3]+dT*V[2][0]*G[3]+dT*V[1][1]*S[1]+dT*V[2][0]*S[1]+(dT*dT)*V[2][1]*G[2]+(dT*dT)*V[2][1]*G[3]+(dT*dT)*V[2][1]*S[1]+dT*V[1][1]*V[2][2]-dT*V[1][2]*V[2][1]+dT*V[1][1]*V[2][3]+dT*V[1][1]*V[3][2]+dT*V[2][0]*V[3][2]-dT*V[1][2]*V[3][1]-dT*V[2][1]*V[1][3]-dT*V[3][0]*V[2][2]+dT*V[1][1]*V[3][3]+dT*V[2][0]*V[3][3]-dT*V[3][0]*V[2][3]-dT*V[1][3]*V[3][1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); - K[1][1] = (V[1][2]*G[0]+V[1][3]*G[0]+V[1][2]*S[0]+V[1][3]*S[0]+V[0][0]*V[1][2]-V[1][0]*V[0][2]+V[0][0]*V[1][3]-V[1][0]*V[0][3]+(dT*dT)*V[0][1]*V[2][2]+(dT*dT)*V[1][0]*V[2][2]-(dT*dT)*V[0][2]*V[2][1]-(dT*dT)*V[2][0]*V[1][2]+(dT*dT)*V[0][1]*V[2][3]+(dT*dT)*V[1][0]*V[2][3]-(dT*dT)*V[2][0]*V[1][3]-(dT*dT)*V[0][3]*V[2][1]+(dT*dT*dT)*V[1][1]*V[2][2]-(dT*dT*dT)*V[1][2]*V[2][1]+(dT*dT*dT)*V[1][1]*V[2][3]-(dT*dT*dT)*V[2][1]*V[1][3]+dT*V[2][2]*G[0]+dT*V[2][3]*G[0]+dT*V[2][2]*S[0]+dT*V[2][3]*S[0]+dT*V[0][0]*V[2][2]+dT*V[0][1]*V[1][2]-dT*V[0][2]*V[1][1]-dT*V[0][2]*V[2][0]+dT*V[0][0]*V[2][3]+dT*V[0][1]*V[1][3]-dT*V[1][1]*V[0][3]-dT*V[2][0]*V[0][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); - K[2][0] = (V[2][0]*G[3]-V[3][0]*G[2]+V[2][0]*S[1]+V[2][0]*V[3][2]-V[3][0]*V[2][2]+V[2][0]*V[3][3]-V[3][0]*V[2][3]+dT*V[2][1]*G[3]-dT*V[3][1]*G[2]+dT*V[2][1]*S[1]+dT*V[2][1]*V[3][2]-dT*V[2][2]*V[3][1]+dT*V[2][1]*V[3][3]-dT*V[3][1]*V[2][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); - K[2][1] = (V[0][0]*G[2]+V[2][2]*G[0]+V[2][3]*G[0]+V[2][2]*S[0]+V[2][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]-V[2][0]*V[0][3]+G[0]*G[2]+G[2]*S[0]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]-(dT*dT)*V[2][1]*V[1][3]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+(dT*dT)*V[1][1]*G[2]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[1][0]*V[2][3]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); - K[3][0] = (-V[2][0]*G[3]+V[3][0]*G[2]+V[3][0]*S[1]-V[2][0]*V[3][2]+V[3][0]*V[2][2]-V[2][0]*V[3][3]+V[3][0]*V[2][3]-dT*V[2][1]*G[3]+dT*V[3][1]*G[2]+dT*V[3][1]*S[1]-dT*V[2][1]*V[3][2]+dT*V[2][2]*V[3][1]-dT*V[2][1]*V[3][3]+dT*V[3][1]*V[2][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); - K[3][1] = (V[0][0]*G[3]+V[3][2]*G[0]+V[3][3]*G[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[3][2]-V[0][2]*V[3][0]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[3]+G[3]*S[0]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+(dT*dT)*V[1][1]*G[3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); - - z_new[0] = -K[0][0]*(dT*z[1]-x[0]+z[0])+dT*z[1]-K[0][1]*(-x[1]+z[2]+z[3])+z[0]; - z_new[1] = -K[1][0]*(dT*z[1]-x[0]+z[0])+dT*z[2]-K[1][1]*(-x[1]+z[2]+z[3])+z[1]; - z_new[2] = -K[2][0]*(dT*z[1]-x[0]+z[0])-K[2][1]*(-x[1]+z[2]+z[3])+z[2]; - z_new[3] = -K[3][0]*(dT*z[1]-x[0]+z[0])-K[3][1]*(-x[1]+z[2]+z[3])+z[3]; + if (init == WAITIING_INIT) { + z[0] = baro.Altitude; + z[1] = 0; + z[2] = accels.z; + z[3] = 0; + init = INITED; + } else if (init == WAITING_BARO) { + continue; + } - memcpy(z, z_new, sizeof(z_new)); - - V[0][0] = -K[0][1]*P[2][0]-K[0][1]*P[3][0]-P[0][0]*(K[0][0]-1.0f); - V[0][1] = -K[0][1]*P[2][1]-K[0][1]*P[3][2]-P[0][1]*(K[0][0]-1.0f); - V[0][2] = -K[0][1]*P[2][2]-K[0][1]*P[3][2]-P[0][2]*(K[0][0]-1.0f); - V[0][3] = -K[0][1]*P[2][3]-K[0][1]*P[3][3]-P[0][3]*(K[0][0]-1.0f); - V[1][0] = P[1][0]-K[1][0]*P[0][0]-K[1][1]*P[2][0]-K[1][1]*P[3][0]; - V[1][1] = P[1][1]-K[1][0]*P[0][1]-K[1][1]*P[2][1]-K[1][1]*P[3][2]; - V[1][2] = P[1][2]-K[1][0]*P[0][2]-K[1][1]*P[2][2]-K[1][1]*P[3][2]; - V[1][3] = P[1][3]-K[1][0]*P[0][3]-K[1][1]*P[2][3]-K[1][1]*P[3][3]; - V[2][0] = -K[2][0]*P[0][0]-K[2][1]*P[3][0]-P[2][0]*(K[2][1]-1.0f); - V[2][1] = -K[2][0]*P[0][1]-K[2][1]*P[3][2]-P[2][1]*(K[2][1]-1.0f); - V[2][2] = -K[2][0]*P[0][2]-K[2][1]*P[3][2]-P[2][2]*(K[2][1]-1.0f); - V[2][3] = -K[2][0]*P[0][3]-K[2][1]*P[3][3]-P[2][3]*(K[2][1]-1.0f); - V[3][0] = -K[3][0]*P[0][0]-K[3][1]*P[2][0]-P[3][0]*(K[3][1]-1.0f); - V[3][1] = -K[3][0]*P[0][1]-K[3][1]*P[2][1]-P[3][2]*(K[3][1]-1.0f); - V[3][2] = -K[3][0]*P[0][2]-K[3][1]*P[2][2]-P[3][2]*(K[3][1]-1.0f); - V[3][3] = -K[3][0]*P[0][3]-K[3][1]*P[2][3]-P[3][3]*(K[3][1]-1.0f); - + x[0] = baro.Altitude; + // rotate avg accels into earth frame and store it + if (1) { + float q[4], Rbe[3][3]; + q[0] = attitudeActual.q1; + q[1] = attitudeActual.q2; + q[2] = attitudeActual.q3; + q[3] = attitudeActual.q4; + Quaternion2R(q, Rbe); + x[1] = -(Rbe[0][2] * accels.x + Rbe[1][2] * accels.y + Rbe[2][2] * accels.z + 9.81f); + } else { + x[1] = -accels.z + 9.81f; + } - baro_updated = false; - } else { - K[0][0] = (V[0][2]+V[0][3]+dT*V[1][2]+dT*V[1][3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]); - K[1][0] = (V[1][2]+V[1][3]+dT*V[2][2]+dT*V[2][3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]); - K[2][0] = (V[2][2]+V[2][3]+G[2])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]); - K[3][0] = (V[3][2]+V[3][3]+G[3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]); - + dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; + timeval = PIOS_DELAY_GetRaw(); - z_new[0] = dT*z[1]-K[0][0]*(-x[1]+z[2]+z[3])+z[0]; - z_new[1] = dT*z[2]-K[1][0]*(-x[1]+z[2]+z[3])+z[1]; - z_new[2] = -K[2][0]*(-x[1]+z[2]+z[3])+z[2]; - z_new[3] = -K[3][0]*(-x[1]+z[2]+z[3])+z[3]; + P[0][0] = dT * (V[0][1] + dT * V[1][1]) + V[0][0] + G[0] + dT * V[1][0]; + P[0][1] = dT * (V[0][2] + dT * V[1][2]) + V[0][1] + dT * V[1][1]; + P[0][2] = V[0][2] + dT * V[1][2]; + P[0][3] = V[0][3] + dT * V[1][3]; + P[1][0] = dT * (V[1][1] + dT * V[2][1]) + V[1][0] + dT * V[2][0]; + P[1][1] = dT * (V[1][2] + dT * V[2][2]) + V[1][1] + G[1] + dT * V[2][1]; + P[1][2] = V[1][2] + dT * V[2][2]; + P[1][3] = V[1][3] + dT * V[2][3]; + P[2][0] = V[2][0] + dT * V[2][1]; + P[2][1] = V[2][1] + dT * V[2][2]; + P[2][2] = V[2][2] + G[2]; + P[2][3] = V[2][3]; + P[3][0] = V[3][0] + dT * V[3][1]; + P[3][1] = V[3][1] + dT * V[3][2]; + P[3][2] = V[3][2]; + P[3][3] = V[3][3] + G[3]; - memcpy(z, z_new, sizeof(z_new)); + if (baro_updated) { + K[0][0] = -(V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) + 1.0f; + K[0][1] = ((V[0][2] + V[0][3]) * S[0] + dT * (V[1][2] + V[1][3]) * S[0]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); + K[1][0] = (V[1][0] * G[2] + V[1][0] * G[3] + V[1][0] * S[1] + V[1][0] * V[2][2] - V[2][0] * V[1][2] + V[1][0] * V[2][3] + V[1][0] * V[3][2] - V[2][0] * V[1][3] - V[1][2] * V[3][0] + V[1][0] * V[3][3] - V[3][0] * V[1][3] + (dT * dT) * V[2][1] * V[3][2] - (dT * dT) * V[2][2] * V[3][1] + (dT * dT) * V[2][1] * V[3][3] - (dT * dT) * V[3][1] * V[2][3] + dT * V[1][1] * G[2] + dT * V[2][0] * G[2] + dT * V[1][1] * G[3] + dT * V[2][0] * G[3] + dT * V[1][1] * S[1] + dT * V[2][0] * S[1] + (dT * dT) * V[2][1] * G[2] + (dT * dT) * V[2][1] * G[3] + (dT * dT) * V[2][1] * S[1] + dT * V[1][1] * V[2][2] - dT * V[1][2] * V[2][1] + dT * V[1][1] * V[2][3] + dT * V[1][1] * V[3][2] + dT * V[2][0] * V[3][2] - dT * V[1][2] * V[3][1] - dT * V[2][1] * V[1][3] - dT * V[3][0] * V[2][2] + dT * V[1][1] * V[3][3] + dT * V[2][0] * V[3][3] - dT * V[3][0] * V[2][3] - dT * V[1][3] * V[3][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); + K[1][1] = (V[1][2] * G[0] + V[1][3] * G[0] + V[1][2] * S[0] + V[1][3] * S[0] + V[0][0] * V[1][2] - V[1][0] * V[0][2] + V[0][0] * V[1][3] - V[1][0] * V[0][3] + (dT * dT) * V[0][1] * V[2][2] + (dT * dT) * V[1][0] * V[2][2] - (dT * dT) * V[0][2] * V[2][1] - (dT * dT) * V[2][0] * V[1][2] + (dT * dT) * V[0][1] * V[2][3] + (dT * dT) * V[1][0] * V[2][3] - (dT * dT) * V[2][0] * V[1][3] - (dT * dT) * V[0][3] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][2] - (dT * dT * dT) * V[1][2] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][3] - (dT * dT * dT) * V[2][1] * V[1][3] + dT * V[2][2] * G[0] + dT * V[2][3] * G[0] + dT * V[2][2] * S[0] + dT * V[2][3] * S[0] + dT * V[0][0] * V[2][2] + dT * V[0][1] * V[1][2] - dT * V[0][2] * V[1][1] - dT * V[0][2] * V[2][0] + dT * V[0][0] * V[2][3] + dT * V[0][1] * V[1][3] - dT * V[1][1] * V[0][3] - dT * V[2][0] * V[0][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); + K[2][0] = (V[2][0] * G[3] - V[3][0] * G[2] + V[2][0] * S[1] + V[2][0] * V[3][2] - V[3][0] * V[2][2] + V[2][0] * V[3][3] - V[3][0] * V[2][3] + dT * V[2][1] * G[3] - dT * V[3][1] * G[2] + dT * V[2][1] * S[1] + dT * V[2][1] * V[3][2] - dT * V[2][2] * V[3][1] + dT * V[2][1] * V[3][3] - dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); + K[2][1] = (V[0][0] * G[2] + V[2][2] * G[0] + V[2][3] * G[0] + V[2][2] * S[0] + V[2][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] - V[2][0] * V[0][3] + G[0] * G[2] + G[2] * S[0] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] - (dT * dT) * V[2][1] * V[1][3] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + (dT * dT) * V[1][1] * G[2] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[1][0] * V[2][3] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); + K[3][0] = (-V[2][0] * G[3] + V[3][0] * G[2] + V[3][0] * S[1] - V[2][0] * V[3][2] + V[3][0] * V[2][2] - V[2][0] * V[3][3] + V[3][0] * V[2][3] - dT * V[2][1] * G[3] + dT * V[3][1] * G[2] + dT * V[3][1] * S[1] - dT * V[2][1] * V[3][2] + dT * V[2][2] * V[3][1] - dT * V[2][1] * V[3][3] + dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); + K[3][1] = (V[0][0] * G[3] + V[3][2] * G[0] + V[3][3] * G[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[3][2] - V[0][2] * V[3][0] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[3] + G[3] * S[0] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + (dT * dT) * V[1][1] * G[3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]); - V[0][0] = P[0][0]-K[0][0]*P[2][0]-K[0][0]*P[3][0]; - V[0][1] = P[0][1]-K[0][0]*P[2][1]-K[0][0]*P[3][2]; - V[0][2] = P[0][2]-K[0][0]*P[2][2]-K[0][0]*P[3][2]; - V[0][3] = P[0][3]-K[0][0]*P[2][3]-K[0][0]*P[3][3]; - V[1][0] = P[1][0]-K[1][0]*P[2][0]-K[1][0]*P[3][0]; - V[1][1] = P[1][1]-K[1][0]*P[2][1]-K[1][0]*P[3][2]; - V[1][2] = P[1][2]-K[1][0]*P[2][2]-K[1][0]*P[3][2]; - V[1][3] = P[1][3]-K[1][0]*P[2][3]-K[1][0]*P[3][3]; - V[2][0] = -K[2][0]*P[3][0]-P[2][0]*(K[2][0]-1.0f); - V[2][1] = -K[2][0]*P[3][2]-P[2][1]*(K[2][0]-1.0f); - V[2][2] = -K[2][0]*P[3][2]-P[2][2]*(K[2][0]-1.0f); - V[2][3] = -K[2][0]*P[3][3]-P[2][3]*(K[2][0]-1.0f); - V[3][0] = -K[3][0]*P[2][0]-P[3][0]*(K[3][0]-1.0f); - V[3][1] = -K[3][0]*P[2][1]-P[3][2]*(K[3][0]-1.0f); - V[3][2] = -K[3][0]*P[2][2]-P[3][2]*(K[3][0]-1.0f); - V[3][3] = -K[3][0]*P[2][3]-P[3][3]*(K[3][0]-1.0f); - } + z_new[0] = -K[0][0] * (dT * z[1] - x[0] + z[0]) + dT * z[1] - K[0][1] * (-x[1] + z[2] + z[3]) + z[0]; + z_new[1] = -K[1][0] * (dT * z[1] - x[0] + z[0]) + dT * z[2] - K[1][1] * (-x[1] + z[2] + z[3]) + z[1]; + z_new[2] = -K[2][0] * (dT * z[1] - x[0] + z[0]) - K[2][1] * (-x[1] + z[2] + z[3]) + z[2]; + z_new[3] = -K[3][0] * (dT * z[1] - x[0] + z[0]) - K[3][1] * (-x[1] + z[2] + z[3]) + z[3]; - AltHoldSmoothedData altHold; - AltHoldSmoothedGet(&altHold); - altHold.Altitude = z[0]; - altHold.Velocity = z[1]; - altHold.Accel = z[2]; - AltHoldSmoothedSet(&altHold); + memcpy(z, z_new, sizeof(z_new)); - AltHoldSmoothedGet(&altHold); + V[0][0] = -K[0][1] * P[2][0] - K[0][1] * P[3][0] - P[0][0] * (K[0][0] - 1.0f); + V[0][1] = -K[0][1] * P[2][1] - K[0][1] * P[3][2] - P[0][1] * (K[0][0] - 1.0f); + V[0][2] = -K[0][1] * P[2][2] - K[0][1] * P[3][2] - P[0][2] * (K[0][0] - 1.0f); + V[0][3] = -K[0][1] * P[2][3] - K[0][1] * P[3][3] - P[0][3] * (K[0][0] - 1.0f); + V[1][0] = P[1][0] - K[1][0] * P[0][0] - K[1][1] * P[2][0] - K[1][1] * P[3][0]; + V[1][1] = P[1][1] - K[1][0] * P[0][1] - K[1][1] * P[2][1] - K[1][1] * P[3][2]; + V[1][2] = P[1][2] - K[1][0] * P[0][2] - K[1][1] * P[2][2] - K[1][1] * P[3][2]; + V[1][3] = P[1][3] - K[1][0] * P[0][3] - K[1][1] * P[2][3] - K[1][1] * P[3][3]; + V[2][0] = -K[2][0] * P[0][0] - K[2][1] * P[3][0] - P[2][0] * (K[2][1] - 1.0f); + V[2][1] = -K[2][0] * P[0][1] - K[2][1] * P[3][2] - P[2][1] * (K[2][1] - 1.0f); + V[2][2] = -K[2][0] * P[0][2] - K[2][1] * P[3][2] - P[2][2] * (K[2][1] - 1.0f); + V[2][3] = -K[2][0] * P[0][3] - K[2][1] * P[3][3] - P[2][3] * (K[2][1] - 1.0f); + V[3][0] = -K[3][0] * P[0][0] - K[3][1] * P[2][0] - P[3][0] * (K[3][1] - 1.0f); + V[3][1] = -K[3][0] * P[0][1] - K[3][1] * P[2][1] - P[3][2] * (K[3][1] - 1.0f); + V[3][2] = -K[3][0] * P[0][2] - K[3][1] * P[2][2] - P[3][2] * (K[3][1] - 1.0f); + V[3][3] = -K[3][0] * P[0][3] - K[3][1] * P[2][3] - P[3][3] * (K[3][1] - 1.0f); - // Verify that we are in altitude hold mode - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || - flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) { - running = false; - } - if (!running) - continue; + baro_updated = false; + } else { + K[0][0] = (V[0][2] + V[0][3] + dT * V[1][2] + dT * V[1][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); + K[1][0] = (V[1][2] + V[1][3] + dT * V[2][2] + dT * V[2][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); + K[2][0] = (V[2][2] + V[2][3] + G[2]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); + K[3][0] = (V[3][2] + V[3][3] + G[3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]); - // Compute the altitude error - error = (starting_altitude + altitudeHoldDesired.Altitude) - altHold.Altitude; - // Compute integral off altitude error - throttleIntegral += error * altitudeHoldSettings.Ki * dT; + z_new[0] = dT * z[1] - K[0][0] * (-x[1] + z[2] + z[3]) + z[0]; + z_new[1] = dT * z[2] - K[1][0] * (-x[1] + z[2] + z[3]) + z[1]; + z_new[2] = -K[2][0] * (-x[1] + z[2] + z[3]) + z[2]; + z_new[3] = -K[3][0] * (-x[1] + z[2] + z[3]) + z[3]; - // Only update stabilizationDesired less frequently - if((thisTime - lastUpdateTime) < 20) - continue; + memcpy(z, z_new, sizeof(z_new)); - lastUpdateTime = thisTime; + V[0][0] = P[0][0] - K[0][0] * P[2][0] - K[0][0] * P[3][0]; + V[0][1] = P[0][1] - K[0][0] * P[2][1] - K[0][0] * P[3][2]; + V[0][2] = P[0][2] - K[0][0] * P[2][2] - K[0][0] * P[3][2]; + V[0][3] = P[0][3] - K[0][0] * P[2][3] - K[0][0] * P[3][3]; + V[1][0] = P[1][0] - K[1][0] * P[2][0] - K[1][0] * P[3][0]; + V[1][1] = P[1][1] - K[1][0] * P[2][1] - K[1][0] * P[3][2]; + V[1][2] = P[1][2] - K[1][0] * P[2][2] - K[1][0] * P[3][2]; + V[1][3] = P[1][3] - K[1][0] * P[2][3] - K[1][0] * P[3][3]; + V[2][0] = -K[2][0] * P[3][0] - P[2][0] * (K[2][0] - 1.0f); + V[2][1] = -K[2][0] * P[3][2] - P[2][1] * (K[2][0] - 1.0f); + V[2][2] = -K[2][0] * P[3][2] - P[2][2] * (K[2][0] - 1.0f); + V[2][3] = -K[2][0] * P[3][3] - P[2][3] * (K[2][0] - 1.0f); + V[3][0] = -K[3][0] * P[2][0] - P[3][0] * (K[3][0] - 1.0f); + V[3][1] = -K[3][0] * P[2][1] - P[3][2] * (K[3][0] - 1.0f); + V[3][2] = -K[3][0] * P[2][2] - P[3][2] * (K[3][0] - 1.0f); + V[3][3] = -K[3][0] * P[2][3] - P[3][3] * (K[3][0] - 1.0f); + } - // Instead of explicit limit on integral you output limit feedback - StabilizationDesiredGet(&stabilizationDesired); - stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral - - altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; - if(stabilizationDesired.Throttle > 1) { - throttleIntegral -= (stabilizationDesired.Throttle - 1); - stabilizationDesired.Throttle = 1; - } - else if (stabilizationDesired.Throttle < 0) { - throttleIntegral -= stabilizationDesired.Throttle; - stabilizationDesired.Throttle = 0; - } + AltHoldSmoothedData altHold; + AltHoldSmoothedGet(&altHold); + altHold.Altitude = z[0]; + altHold.Velocity = z[1]; + altHold.Accel = z[2]; + AltHoldSmoothedSet(&altHold); - stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - stabilizationDesired.Roll = altitudeHoldDesired.Roll; - stabilizationDesired.Pitch = altitudeHoldDesired.Pitch; - stabilizationDesired.Yaw = altitudeHoldDesired.Yaw; - StabilizationDesiredSet(&stabilizationDesired); + AltHoldSmoothedGet(&altHold); - } else if (ev.obj == AltitudeHoldDesiredHandle()) { - AltitudeHoldDesiredGet(&altitudeHoldDesired); - } + // Verify that we are in altitude hold mode + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || + flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) { + running = false; + } - } + if (!running) { + continue; + } + + // Compute the altitude error + error = (starting_altitude + altitudeHoldDesired.Altitude) - altHold.Altitude; + + // Compute integral off altitude error + throttleIntegral += error * altitudeHoldSettings.Ki * dT; + + // Only update stabilizationDesired less frequently + if ((thisTime - lastUpdateTime) < 20) { + continue; + } + + lastUpdateTime = thisTime; + + // Instead of explicit limit on integral you output limit feedback + StabilizationDesiredGet(&stabilizationDesired); + stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral - + altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; + if (stabilizationDesired.Throttle > 1) { + throttleIntegral -= (stabilizationDesired.Throttle - 1); + stabilizationDesired.Throttle = 1; + } else if (stabilizationDesired.Throttle < 0) { + throttleIntegral -= stabilizationDesired.Throttle; + stabilizationDesired.Throttle = 0; + } + + stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabilizationDesired.Roll = altitudeHoldDesired.Roll; + stabilizationDesired.Pitch = altitudeHoldDesired.Pitch; + stabilizationDesired.Yaw = altitudeHoldDesired.Yaw; + StabilizationDesiredSet(&stabilizationDesired); + } else if (ev.obj == AltitudeHoldDesiredHandle()) { + AltitudeHoldDesiredGet(&altitudeHoldDesired); + } + } } static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - AltitudeHoldSettingsGet(&altitudeHoldSettings); + AltitudeHoldSettingsGet(&altitudeHoldSettings); } diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index 540cb154f..688d97dbc 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -66,11 +66,11 @@ // Private constants #define STACK_SIZE_BYTES 540 -#define TASK_PRIORITY (tskIDLE_PRIORITY+3) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) -#define SENSOR_PERIOD 4 -#define UPDATE_RATE 25.0f -#define GYRO_NEUTRAL 1665 +#define SENSOR_PERIOD 4 +#define UPDATE_RATE 25.0f +#define GYRO_NEUTRAL 1665 // Private types @@ -80,38 +80,38 @@ static xTaskHandle taskHandle; // Private functions static void AttitudeTask(void *parameters); -static float gyro_correct_int[3] = {0,0,0}; +static float gyro_correct_int[3] = { 0, 0, 0 }; static xQueueHandle gyro_queue; static int32_t updateSensors(AccelsData *, GyrosData *); -static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData); +static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData); static void updateAttitude(AccelsData *, GyrosData *); -static void settingsUpdatedCb(UAVObjEvent * objEv); +static void settingsUpdatedCb(UAVObjEvent *objEv); -static float accelKi = 0; -static float accelKp = 0; +static float accelKi = 0; +static float accelKp = 0; static float accel_alpha = 0; static bool accel_filter_enabled = false; static float accels_filtered[3]; static float grot_filtered[3]; static float yawBiasRate = 0; static float rollPitchBiasRate = 0.0f; -static float gyroGain = 0.42f; +static float gyroGain = 0.42f; static int16_t accelbias[3]; -static float q[4] = {1,0,0,0}; +static float q[4] = { 1, 0, 0, 0 }; static float R[3][3]; static int8_t rotate = 0; static bool zero_during_arming = false; -static bool bias_correct_gyro = true; +static bool bias_correct_gyro = true; // For running trim flights -static volatile bool trim_requested = false; +static volatile bool trim_requested = false; static volatile int32_t trim_accels[3]; static volatile int32_t trim_samples; int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535; -#define GRAV 9.81f -#define ACCEL_SCALE (GRAV * 0.004f) +#define GRAV 9.81f +#define ACCEL_SCALE (GRAV * 0.004f) /* 0.004f is gravity / LSB */ /** @@ -120,13 +120,12 @@ int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535; */ int32_t AttitudeStart(void) { - - // Start main task - xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, taskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); - - return 0; + // Start main task + xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, taskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); + + return 0; } /** @@ -135,38 +134,40 @@ int32_t AttitudeStart(void) */ int32_t AttitudeInitialize(void) { - AttitudeActualInitialize(); - AttitudeSettingsInitialize(); - AccelsInitialize(); - GyrosInitialize(); - - // Initialize quaternion - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - attitude.q1 = 1; - attitude.q2 = 0; - attitude.q3 = 0; - attitude.q4 = 0; - AttitudeActualSet(&attitude); - - // Cannot trust the values to init right above if BL runs - gyro_correct_int[0] = 0; - gyro_correct_int[1] = 0; - gyro_correct_int[2] = 0; - - q[0] = 1; - q[1] = 0; - q[2] = 0; - q[3] = 0; - for(uint8_t i = 0; i < 3; i++) - for(uint8_t j = 0; j < 3; j++) - R[i][j] = 0; - - trim_requested = false; - - AttitudeSettingsConnectCallback(&settingsUpdatedCb); - - return 0; + AttitudeActualInitialize(); + AttitudeSettingsInitialize(); + AccelsInitialize(); + GyrosInitialize(); + + // Initialize quaternion + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + attitude.q1 = 1; + attitude.q2 = 0; + attitude.q3 = 0; + attitude.q4 = 0; + AttitudeActualSet(&attitude); + + // Cannot trust the values to init right above if BL runs + gyro_correct_int[0] = 0; + gyro_correct_int[1] = 0; + gyro_correct_int[2] = 0; + + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; + for (uint8_t i = 0; i < 3; i++) { + for (uint8_t j = 0; j < 3; j++) { + R[i][j] = 0; + } + } + + trim_requested = false; + + AttitudeSettingsConnectCallback(&settingsUpdatedCb); + + return 0; } MODULE_INITCALL(AttitudeInitialize, AttitudeStart) @@ -174,100 +175,101 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart) /** * Module thread, should not return. */ - + int32_t accel_test; int32_t gyro_test; static void AttitudeTask(__attribute__((unused)) void *parameters) { - uint8_t init = 0; - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - - // Set critical error and wait until the accel is producing data - while(PIOS_ADXL345_FifoElements() == 0) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); - } - - const struct pios_board_info * bdinfo = &pios_board_info_blob; - - bool cc3d = bdinfo->board_rev == 0x02; + uint8_t init = 0; - if(cc3d) { + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + + // Set critical error and wait until the accel is producing data + while (PIOS_ADXL345_FifoElements() == 0) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); + } + + const struct pios_board_info *bdinfo = &pios_board_info_blob; + + bool cc3d = bdinfo->board_rev == 0x02; + + if (cc3d) { #if defined(PIOS_INCLUDE_MPU6000) - gyro_test = PIOS_MPU6000_Test(); + gyro_test = PIOS_MPU6000_Test(); #endif - } else { + } else { #if defined(PIOS_INCLUDE_ADXL345) - accel_test = PIOS_ADXL345_Test(); + accel_test = PIOS_ADXL345_Test(); #endif #if defined(PIOS_INCLUDE_ADC) - // Create queue for passing gyro data, allow 2 back samples in case - gyro_queue = xQueueCreate(1, sizeof(float) * 4); - PIOS_Assert(gyro_queue != NULL); - PIOS_ADC_SetQueue(gyro_queue); - PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE); + // Create queue for passing gyro data, allow 2 back samples in case + gyro_queue = xQueueCreate(1, sizeof(float) * 4); + PIOS_Assert(gyro_queue != NULL); + PIOS_ADC_SetQueue(gyro_queue); + PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE); #endif + } + // Force settings update to make sure rotation loaded + settingsUpdatedCb(AttitudeSettingsHandle()); - } - // Force settings update to make sure rotation loaded - settingsUpdatedCb(AttitudeSettingsHandle()); - - // Main task loop - while (1) { - - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - - if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { - // Use accels to initialise attitude and calculate gyro bias - accelKp = 1.0f; - accelKi = 0.0f; - yawBiasRate = 0.01f; - rollPitchBiasRate = 0.01f; - accel_filter_enabled = false; - init = 0; - } - else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { - accelKp = 1.0f; - accelKi = 0.0f; - yawBiasRate = 0.01f; - rollPitchBiasRate = 0.01f; - accel_filter_enabled = false; - init = 0; - } else if (init == 0) { - // Reload settings (all the rates) - AttitudeSettingsAccelKiGet(&accelKi); - AttitudeSettingsAccelKpGet(&accelKp); - AttitudeSettingsYawBiasRateGet(&yawBiasRate); - rollPitchBiasRate = 0.0f; - if (accel_alpha > 0.0f) - accel_filter_enabled = true; - init = 1; - } - - PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); + // Main task loop + while (1) { + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); - AccelsData accels; - GyrosData gyros; - int32_t retval = 0; + if ((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { + // Use accels to initialise attitude and calculate gyro bias + accelKp = 1.0f; + accelKi = 0.0f; + yawBiasRate = 0.01f; + rollPitchBiasRate = 0.01f; + accel_filter_enabled = false; + init = 0; + } else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { + accelKp = 1.0f; + accelKi = 0.0f; + yawBiasRate = 0.01f; + rollPitchBiasRate = 0.01f; + accel_filter_enabled = false; + init = 0; + } else if (init == 0) { + // Reload settings (all the rates) + AttitudeSettingsAccelKiGet(&accelKi); + AttitudeSettingsAccelKpGet(&accelKp); + AttitudeSettingsYawBiasRateGet(&yawBiasRate); + rollPitchBiasRate = 0.0f; + if (accel_alpha > 0.0f) { + accel_filter_enabled = true; + } + init = 1; + } - if (cc3d) - retval = updateSensorsCC3D(&accels, &gyros); - else - retval = updateSensors(&accels, &gyros); + PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); - // Only update attitude when sensor data is good - if (retval != 0) - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - else { - // Do not update attitude data in simulation mode - if (!AttitudeActualReadOnly()) - updateAttitude(&accels, &gyros); + AccelsData accels; + GyrosData gyros; + int32_t retval = 0; - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - } - } + if (cc3d) { + retval = updateSensorsCC3D(&accels, &gyros); + } else { + retval = updateSensors(&accels, &gyros); + } + + // Only update attitude when sensor data is good + if (retval != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + } else { + // Do not update attitude data in simulation mode + if (!AttitudeActualReadOnly()) { + updateAttitude(&accels, &gyros); + } + + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + } + } } float gyros_passed[3]; @@ -277,105 +279,107 @@ float gyros_passed[3]; * @param[in] attitudeRaw Populate the UAVO instead of saving right here * @return 0 if successfull, -1 if not */ -static int32_t updateSensors(AccelsData * accels, GyrosData * gyros) +static int32_t updateSensors(AccelsData *accels, GyrosData *gyros) { - struct pios_adxl345_data accel_data; - float gyro[4]; - - // Only wait the time for two nominal updates before setting an alarm - if(xQueueReceive(gyro_queue, (void * const) gyro, UPDATE_RATE * 2) == errQUEUE_EMPTY) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - return -1; - } + struct pios_adxl345_data accel_data; + float gyro[4]; - // Do not read raw sensor data in simulation mode - if (GyrosReadOnly() || AccelsReadOnly()) - return 0; + // Only wait the time for two nominal updates before setting an alarm + if (xQueueReceive(gyro_queue, (void *const)gyro, UPDATE_RATE * 2) == errQUEUE_EMPTY) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + return -1; + } - // No accel data available - if(PIOS_ADXL345_FifoElements() == 0) - return -1; - - // First sample is temperature - gyros->x = -(gyro[1] - GYRO_NEUTRAL) * gyroGain; - gyros->y = (gyro[2] - GYRO_NEUTRAL) * gyroGain; - gyros->z = -(gyro[3] - GYRO_NEUTRAL) * gyroGain; - - int32_t x = 0; - int32_t y = 0; - int32_t z = 0; - uint8_t i = 0; - uint8_t samples_remaining; - do { - i++; - samples_remaining = PIOS_ADXL345_Read(&accel_data); - x += accel_data.x; - y += -accel_data.y; - z += -accel_data.z; - } while ( (i < 32) && (samples_remaining > 0) ); - gyros->temperature = samples_remaining; + // Do not read raw sensor data in simulation mode + if (GyrosReadOnly() || AccelsReadOnly()) { + return 0; + } - float accel[3] = {(float) x / i, (float) y / i, (float) z / i}; - - if(rotate) { - // TODO: rotate sensors too so stabilization is well behaved - float vec_out[3]; - rot_mult(R, accel, vec_out); - accels->x = vec_out[0]; - accels->y = vec_out[1]; - accels->z = vec_out[2]; - rot_mult(R, &gyros->x, vec_out); - gyros->x = vec_out[0]; - gyros->y = vec_out[1]; - gyros->z = vec_out[2]; - } else { - accels->x = accel[0]; - accels->y = accel[1]; - accels->z = accel[2]; - } - - if (trim_requested) { - if (trim_samples >= MAX_TRIM_FLIGHT_SAMPLES) { - trim_requested = false; - } else { - uint8_t armed; - float throttle; - FlightStatusArmedGet(&armed); - ManualControlCommandThrottleGet(&throttle); // Until flight status indicates airborne - if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0.0f)) { - trim_samples++; - // Store the digitally scaled version since that is what we use for bias - trim_accels[0] += accels->x; - trim_accels[1] += accels->y; - trim_accels[2] += accels->z; - } - } - } - - // Scale accels and correct bias - accels->x = (accels->x - accelbias[0]) * ACCEL_SCALE; - accels->y = (accels->y - accelbias[1]) * ACCEL_SCALE; - accels->z = (accels->z - accelbias[2]) * ACCEL_SCALE; - - if(bias_correct_gyro) { - // Applying integral component here so it can be seen on the gyros and correct bias - gyros->x += gyro_correct_int[0]; - gyros->y += gyro_correct_int[1]; - gyros->z += gyro_correct_int[2]; - } + // No accel data available + if (PIOS_ADXL345_FifoElements() == 0) { + return -1; + } - // Force the roll & pitch gyro rates to average to zero during initialisation - gyro_correct_int[0] += - gyros->x * rollPitchBiasRate; - gyro_correct_int[1] += - gyros->y * rollPitchBiasRate; - - // Because most crafts wont get enough information from gravity to zero yaw gyro, we try - // and make it average zero (weakly) - gyro_correct_int[2] += - gyros->z * yawBiasRate; + // First sample is temperature + gyros->x = -(gyro[1] - GYRO_NEUTRAL) * gyroGain; + gyros->y = (gyro[2] - GYRO_NEUTRAL) * gyroGain; + gyros->z = -(gyro[3] - GYRO_NEUTRAL) * gyroGain; - GyrosSet(gyros); - AccelsSet(accels); + int32_t x = 0; + int32_t y = 0; + int32_t z = 0; + uint8_t i = 0; + uint8_t samples_remaining; + do { + i++; + samples_remaining = PIOS_ADXL345_Read(&accel_data); + x += accel_data.x; + y += -accel_data.y; + z += -accel_data.z; + } while ((i < 32) && (samples_remaining > 0)); + gyros->temperature = samples_remaining; - return 0; + float accel[3] = { (float)x / i, (float)y / i, (float)z / i }; + + if (rotate) { + // TODO: rotate sensors too so stabilization is well behaved + float vec_out[3]; + rot_mult(R, accel, vec_out); + accels->x = vec_out[0]; + accels->y = vec_out[1]; + accels->z = vec_out[2]; + rot_mult(R, &gyros->x, vec_out); + gyros->x = vec_out[0]; + gyros->y = vec_out[1]; + gyros->z = vec_out[2]; + } else { + accels->x = accel[0]; + accels->y = accel[1]; + accels->z = accel[2]; + } + + if (trim_requested) { + if (trim_samples >= MAX_TRIM_FLIGHT_SAMPLES) { + trim_requested = false; + } else { + uint8_t armed; + float throttle; + FlightStatusArmedGet(&armed); + ManualControlCommandThrottleGet(&throttle); // Until flight status indicates airborne + if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0.0f)) { + trim_samples++; + // Store the digitally scaled version since that is what we use for bias + trim_accels[0] += accels->x; + trim_accels[1] += accels->y; + trim_accels[2] += accels->z; + } + } + } + + // Scale accels and correct bias + accels->x = (accels->x - accelbias[0]) * ACCEL_SCALE; + accels->y = (accels->y - accelbias[1]) * ACCEL_SCALE; + accels->z = (accels->z - accelbias[2]) * ACCEL_SCALE; + + if (bias_correct_gyro) { + // Applying integral component here so it can be seen on the gyros and correct bias + gyros->x += gyro_correct_int[0]; + gyros->y += gyro_correct_int[1]; + gyros->z += gyro_correct_int[2]; + } + + // Force the roll & pitch gyro rates to average to zero during initialisation + gyro_correct_int[0] += -gyros->x * rollPitchBiasRate; + gyro_correct_int[1] += -gyros->y * rollPitchBiasRate; + + // Because most crafts wont get enough information from gravity to zero yaw gyro, we try + // and make it average zero (weakly) + gyro_correct_int[2] += -gyros->z * yawBiasRate; + + GyrosSet(gyros); + AccelsSet(accels); + + return 0; } /** @@ -384,263 +388,270 @@ static int32_t updateSensors(AccelsData * accels, GyrosData * gyros) * @return 0 if successfull, -1 if not */ struct pios_mpu6000_data mpu6000_data; -static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData) +static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData) { - float accels[3], gyros[3]; - + float accels[3], gyros[3]; + #if defined(PIOS_INCLUDE_MPU6000) - - xQueueHandle queue = PIOS_MPU6000_GetQueue(); - - if(xQueueReceive(queue, (void *) &mpu6000_data, SENSOR_PERIOD) == errQUEUE_EMPTY) - return -1; // Error, no data - // Do not read raw sensor data in simulation mode - if (GyrosReadOnly() || AccelsReadOnly()) - return 0; + xQueueHandle queue = PIOS_MPU6000_GetQueue(); - gyros[0] = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale(); - gyros[1] = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale(); - gyros[2] = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale(); - - accels[0] = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale(); - accels[1] = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale(); - accels[2] = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale(); + if (xQueueReceive(queue, (void *)&mpu6000_data, SENSOR_PERIOD) == errQUEUE_EMPTY) { + return -1; // Error, no data + } + // Do not read raw sensor data in simulation mode + if (GyrosReadOnly() || AccelsReadOnly()) { + return 0; + } - gyrosData->temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; - accelsData->temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; -#endif + gyros[0] = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale(); + gyros[1] = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale(); + gyros[2] = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale(); - if(rotate) { - // TODO: rotate sensors too so stabilization is well behaved - float vec_out[3]; - rot_mult(R, accels, vec_out); - accels[0] = vec_out[0]; - accels[1] = vec_out[1]; - accels[2] = vec_out[2]; - rot_mult(R, gyros, vec_out); - gyros[0] = vec_out[0]; - gyros[1] = vec_out[1]; - gyros[2] = vec_out[2]; - } + accels[0] = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale(); + accels[1] = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale(); + accels[2] = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale(); - accelsData->x = accels[0] - accelbias[0] * ACCEL_SCALE; // Applying arbitrary scale here to match CC v1 - accelsData->y = accels[1] - accelbias[1] * ACCEL_SCALE; - accelsData->z = accels[2] - accelbias[2] * ACCEL_SCALE; + gyrosData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; + accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; +#endif /* if defined(PIOS_INCLUDE_MPU6000) */ - gyrosData->x = gyros[0]; - gyrosData->y = gyros[1]; - gyrosData->z = gyros[2]; + if (rotate) { + // TODO: rotate sensors too so stabilization is well behaved + float vec_out[3]; + rot_mult(R, accels, vec_out); + accels[0] = vec_out[0]; + accels[1] = vec_out[1]; + accels[2] = vec_out[2]; + rot_mult(R, gyros, vec_out); + gyros[0] = vec_out[0]; + gyros[1] = vec_out[1]; + gyros[2] = vec_out[2]; + } - if(bias_correct_gyro) { - // Applying integral component here so it can be seen on the gyros and correct bias - gyrosData->x += gyro_correct_int[0]; - gyrosData->y += gyro_correct_int[1]; - gyrosData->z += gyro_correct_int[2]; - } + accelsData->x = accels[0] - accelbias[0] * ACCEL_SCALE; // Applying arbitrary scale here to match CC v1 + accelsData->y = accels[1] - accelbias[1] * ACCEL_SCALE; + accelsData->z = accels[2] - accelbias[2] * ACCEL_SCALE; - // Force the roll & pitch gyro rates to average to zero during initialisation - gyro_correct_int[0] += - gyrosData->x * rollPitchBiasRate; - gyro_correct_int[1] += - gyrosData->y * rollPitchBiasRate; + gyrosData->x = gyros[0]; + gyrosData->y = gyros[1]; + gyrosData->z = gyros[2]; - // Because most crafts wont get enough information from gravity to zero yaw gyro, we try - // and make it average zero (weakly) - gyro_correct_int[2] += - gyrosData->z * yawBiasRate; + if (bias_correct_gyro) { + // Applying integral component here so it can be seen on the gyros and correct bias + gyrosData->x += gyro_correct_int[0]; + gyrosData->y += gyro_correct_int[1]; + gyrosData->z += gyro_correct_int[2]; + } - GyrosSet(gyrosData); - AccelsSet(accelsData); + // Force the roll & pitch gyro rates to average to zero during initialisation + gyro_correct_int[0] += -gyrosData->x * rollPitchBiasRate; + gyro_correct_int[1] += -gyrosData->y * rollPitchBiasRate; - return 0; + // Because most crafts wont get enough information from gravity to zero yaw gyro, we try + // and make it average zero (weakly) + gyro_correct_int[2] += -gyrosData->z * yawBiasRate; + + GyrosSet(gyrosData); + AccelsSet(accelsData); + + return 0; } static inline void apply_accel_filter(const float *raw, float *filtered) { - if (accel_filter_enabled) { - filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha); - filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha); - filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha); - } else { - filtered[0] = raw[0]; - filtered[1] = raw[1]; - filtered[2] = raw[2]; - } + if (accel_filter_enabled) { + filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha); + filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha); + filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha); + } else { + filtered[0] = raw[0]; + filtered[1] = raw[1]; + filtered[2] = raw[2]; + } } -static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) +static void updateAttitude(AccelsData *accelsData, GyrosData *gyrosData) { - float dT; - portTickType thisSysTime = xTaskGetTickCount(); - static portTickType lastSysTime = 0; - - dT = (thisSysTime == lastSysTime) ? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f; - lastSysTime = thisSysTime; - - // Bad practice to assume structure order, but saves memory - float * gyros = &gyrosData->x; - float * accels = &accelsData->x; - - float grot[3]; - float accel_err[3]; + float dT; + portTickType thisSysTime = xTaskGetTickCount(); + static portTickType lastSysTime = 0; - // Apply smoothing to accel values, to reduce vibration noise before main calculations. - apply_accel_filter(accels, accels_filtered); - - // Rotate gravity unit vector to body frame, filter and cross with accels - grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); - grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); - grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); + dT = (thisSysTime == lastSysTime) ? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f; + lastSysTime = thisSysTime; - apply_accel_filter(grot, grot_filtered); - - CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); - - // Account for accel magnitude - float accel_mag = sqrtf(accels_filtered[0]*accels_filtered[0] + accels_filtered[1]*accels_filtered[1] + accels_filtered[2]*accels_filtered[2]); - if (accel_mag < 1.0e-3f) - return; + // Bad practice to assume structure order, but saves memory + float *gyros = &gyrosData->x; + float *accels = &accelsData->x; - // Account for filtered gravity vector magnitude - float grot_mag; + float grot[3]; + float accel_err[3]; - if (accel_filter_enabled) - grot_mag = sqrtf(grot_filtered[0]*grot_filtered[0] + grot_filtered[1]*grot_filtered[1] + grot_filtered[2]*grot_filtered[2]); - else - grot_mag = 1.0f; + // Apply smoothing to accel values, to reduce vibration noise before main calculations. + apply_accel_filter(accels, accels_filtered); - if (grot_mag < 1.0e-3f) - return; + // Rotate gravity unit vector to body frame, filter and cross with accels + grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); + grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); + grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); - accel_err[0] /= (accel_mag*grot_mag); - accel_err[1] /= (accel_mag*grot_mag); - accel_err[2] /= (accel_mag*grot_mag); - - // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s - gyro_correct_int[0] += accel_err[0] * accelKi; - gyro_correct_int[1] += accel_err[1] * accelKi; - - //gyro_correct_int[2] += accel_err[2] * accelKi; - - // Correct rates based on error, integral component dealt with in updateSensors - gyros[0] += accel_err[0] * accelKp / dT; - gyros[1] += accel_err[1] * accelKp / dT; - gyros[2] += accel_err[2] * accelKp / dT; - - { // scoping variables to save memory - // Work out time derivative from INSAlgo writeup - // Also accounts for the fact that gyros are in deg/s - float qdot[4]; - qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); - qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); - qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); - qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); - - // Take a time step - q[0] = q[0] + qdot[0]; - q[1] = q[1] + qdot[1]; - q[2] = q[2] + qdot[2]; - q[3] = q[3] + qdot[3]; - - if(q[0] < 0) { - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; - } - } - - // Renomalize - float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); - - // If quaternion has become inappropriately short or is nan reinit. - // THIS SHOULD NEVER ACTUALLY HAPPEN - if((fabsf(qmag) < 1e-3f) || isnan(qmag)) { - q[0] = 1; - q[1] = 0; - q[2] = 0; - q[3] = 0; - } else { - q[0] = q[0] / qmag; - q[1] = q[1] / qmag; - q[2] = q[2] / qmag; - q[3] = q[3] / qmag; - } - - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - - quat_copy(q, &attitudeActual.q1); - - // Convert into eueler degrees (makes assumptions about RPY order) - Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll); - - AttitudeActualSet(&attitudeActual); + apply_accel_filter(grot, grot_filtered); + + CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); + + // Account for accel magnitude + float accel_mag = sqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]); + if (accel_mag < 1.0e-3f) { + return; + } + + // Account for filtered gravity vector magnitude + float grot_mag; + + if (accel_filter_enabled) { + grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); + } else { + grot_mag = 1.0f; + } + + if (grot_mag < 1.0e-3f) { + return; + } + + accel_err[0] /= (accel_mag * grot_mag); + accel_err[1] /= (accel_mag * grot_mag); + accel_err[2] /= (accel_mag * grot_mag); + + // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s + gyro_correct_int[0] += accel_err[0] * accelKi; + gyro_correct_int[1] += accel_err[1] * accelKi; + + // gyro_correct_int[2] += accel_err[2] * accelKi; + + // Correct rates based on error, integral component dealt with in updateSensors + gyros[0] += accel_err[0] * accelKp / dT; + gyros[1] += accel_err[1] * accelKp / dT; + gyros[2] += accel_err[2] * accelKp / dT; + + { // scoping variables to save memory + // Work out time derivative from INSAlgo writeup + // Also accounts for the fact that gyros are in deg/s + float qdot[4]; + qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); + qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); + qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); + qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); + + // Take a time step + q[0] = q[0] + qdot[0]; + q[1] = q[1] + qdot[1]; + q[2] = q[2] + qdot[2]; + q[3] = q[3] + qdot[3]; + + if (q[0] < 0) { + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } + } + + // Renomalize + float qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + + // If quaternion has become inappropriately short or is nan reinit. + // THIS SHOULD NEVER ACTUALLY HAPPEN + if ((fabsf(qmag) < 1e-3f) || isnan(qmag)) { + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; + } else { + q[0] = q[0] / qmag; + q[1] = q[1] / qmag; + q[2] = q[2] / qmag; + q[3] = q[3] / qmag; + } + + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); + + quat_copy(q, &attitudeActual.q1); + + // Convert into eueler degrees (makes assumptions about RPY order) + Quaternion2RPY(&attitudeActual.q1, &attitudeActual.Roll); + + AttitudeActualSet(&attitudeActual); } -static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent * objEv) { - AttitudeSettingsData attitudeSettings; - AttitudeSettingsGet(&attitudeSettings); - - - accelKp = attitudeSettings.AccelKp; - accelKi = attitudeSettings.AccelKi; - yawBiasRate = attitudeSettings.YawBiasRate; - gyroGain = attitudeSettings.GyroGain; +static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) +{ + AttitudeSettingsData attitudeSettings; - // Calculate accel filter alpha, in the same way as for gyro data in stabilization module. - const float fakeDt = 0.0025f; - if (attitudeSettings.AccelTau < 0.0001f) { - accel_alpha = 0; // not trusting this to resolve to 0 - accel_filter_enabled = false; - } else { - accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau); - accel_filter_enabled = true; - } - - zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; - bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE; - - accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X]; - accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y]; - accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z]; - - gyro_correct_int[0] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f; - gyro_correct_int[1] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f; - gyro_correct_int[2] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f; - - // Indicates not to expend cycles on rotation - if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && - attitudeSettings.BoardRotation[2] == 0) { - rotate = 0; - - // Shouldn't be used but to be safe - float rotationQuat[4] = {1,0,0,0}; - Quaternion2R(rotationQuat, R); - } else { - float rotationQuat[4]; - const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL], - attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH], - attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]}; - RPY2Quaternion(rpy, rotationQuat); - Quaternion2R(rotationQuat, R); - rotate = 1; - } - - if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START) { - trim_accels[0] = 0; - trim_accels[1] = 0; - trim_accels[2] = 0; - trim_samples = 0; - trim_requested = true; - } else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) { - trim_requested = false; - attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X] = trim_accels[0] / trim_samples; - attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y] = trim_accels[1] / trim_samples; - // Z should average -grav - attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z] = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE; - attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL; - AttitudeSettingsSet(&attitudeSettings); - } else - trim_requested = false; + AttitudeSettingsGet(&attitudeSettings); + + + accelKp = attitudeSettings.AccelKp; + accelKi = attitudeSettings.AccelKi; + yawBiasRate = attitudeSettings.YawBiasRate; + gyroGain = attitudeSettings.GyroGain; + + // Calculate accel filter alpha, in the same way as for gyro data in stabilization module. + const float fakeDt = 0.0025f; + if (attitudeSettings.AccelTau < 0.0001f) { + accel_alpha = 0; // not trusting this to resolve to 0 + accel_filter_enabled = false; + } else { + accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau); + accel_filter_enabled = true; + } + + zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; + bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE; + + accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X]; + accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y]; + accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z]; + + gyro_correct_int[0] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f; + gyro_correct_int[1] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f; + gyro_correct_int[2] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f; + + // Indicates not to expend cycles on rotation + if (attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && + attitudeSettings.BoardRotation[2] == 0) { + rotate = 0; + + // Shouldn't be used but to be safe + float rotationQuat[4] = { 1, 0, 0, 0 }; + Quaternion2R(rotationQuat, R); + } else { + float rotationQuat[4]; + const float rpy[3] = { attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL], + attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH], + attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] }; + RPY2Quaternion(rpy, rotationQuat); + Quaternion2R(rotationQuat, R); + rotate = 1; + } + + if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START) { + trim_accels[0] = 0; + trim_accels[1] = 0; + trim_accels[2] = 0; + trim_samples = 0; + trim_requested = true; + } else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) { + trim_requested = false; + attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X] = trim_accels[0] / trim_samples; + attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y] = trim_accels[1] / trim_samples; + // Z should average -grav + attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z] = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE; + attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL; + AttitudeSettingsSet(&attitudeSettings); + } else { + trim_requested = false; + } } /** * @} diff --git a/flight/modules/Attitude/inc/attitude.h b/flight/modules/Attitude/inc/attitude.h index 3f90e56a6..64c556a8e 100644 --- a/flight/modules/Attitude/inc/attitude.h +++ b/flight/modules/Attitude/inc/attitude.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup Attitude Attitude Module - * @{ + * @{ * * @file attitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index 4321041f7..43f060c92 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -75,19 +75,19 @@ #include "CoordinateConversions.h" // Private constants -#define STACK_SIZE_BYTES 2048 -#define TASK_PRIORITY (tskIDLE_PRIORITY+3) +#define STACK_SIZE_BYTES 2048 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) #define FAILSAFE_TIMEOUT_MS 10 // low pass filter configuration to calculate offset // of barometric altitude sensor // reasoning: updates at: 10 Hz, tau= 300 s settle time // exp(-(1/f) / tau ) ~=~ 0.9997 -#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f +#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f // simple IAS to TAS aproximation - 2% increase per 1000ft // since we do not have flowing air temperature information -#define IAS2TAS(alt) (1.0f + (0.02f*(alt)/304.8f)) +#define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f)) // Private types @@ -108,38 +108,40 @@ static RevoCalibrationData revoCalibration; static EKFConfigurationData ekfConfiguration; static RevoSettingsData revoSettings; static FlightStatusData flightStatus; -const uint32_t SENSOR_QUEUE_SIZE = 10; +const uint32_t SENSOR_QUEUE_SIZE = 10; static bool volatile variance_error = true; static bool volatile initialization_required = true; -static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running +static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running // Private functions static void AttitudeTask(void *parameters); static int32_t updateAttitudeComplementary(bool first_run); static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode); -static void settingsUpdatedCb(UAVObjEvent * objEv); +static void settingsUpdatedCb(UAVObjEvent *objEv); -static int32_t getNED(GPSPositionData * gpsPosition, float * NED); +static int32_t getNED(GPSPositionData *gpsPosition, float *NED); // check for invalid values -static inline bool invalid(float data) { - if ( isnan(data) || isinf(data) ){ - return true; - } - return false; +static inline bool invalid(float data) +{ + if (isnan(data) || isinf(data)) { + return true; + } + return false; } // check for invalid variance values -static inline bool invalid_var(float data) { - if ( invalid(data) ) { - return true; - } - if ( data < 1e-15f ) { // var should not be close to zero. And not negative either. - return true; - } - return false; +static inline bool invalid_var(float data) +{ + if (invalid(data)) { + return true; + } + if (data < 1e-15f) { // var should not be close to zero. And not negative either. + return true; + } + return false; } /** @@ -157,46 +159,46 @@ static inline bool invalid_var(float data) { */ int32_t AttitudeInitialize(void) { - AttitudeActualInitialize(); - AirspeedActualInitialize(); - AirspeedSensorInitialize(); - AttitudeSettingsInitialize(); - PositionActualInitialize(); - VelocityActualInitialize(); - RevoSettingsInitialize(); - RevoCalibrationInitialize(); - EKFConfigurationInitialize(); - EKFStateVarianceInitialize(); - FlightStatusInitialize(); - - // Initialize this here while we aren't setting the homelocation in GPS - HomeLocationInitialize(); + AttitudeActualInitialize(); + AirspeedActualInitialize(); + AirspeedSensorInitialize(); + AttitudeSettingsInitialize(); + PositionActualInitialize(); + VelocityActualInitialize(); + RevoSettingsInitialize(); + RevoCalibrationInitialize(); + EKFConfigurationInitialize(); + EKFStateVarianceInitialize(); + FlightStatusInitialize(); - // Initialize quaternion - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - attitude.q1 = 1.0f; - attitude.q2 = 0.0f; - attitude.q3 = 0.0f; - attitude.q4 = 0.0f; - AttitudeActualSet(&attitude); - - // Cannot trust the values to init right above if BL runs - GyrosBiasData gyrosBias; - GyrosBiasGet(&gyrosBias); - gyrosBias.x = 0.0f; - gyrosBias.y = 0.0f; - gyrosBias.z = 0.0f; - GyrosBiasSet(&gyrosBias); - - AttitudeSettingsConnectCallback(&settingsUpdatedCb); - RevoSettingsConnectCallback(&settingsUpdatedCb); - RevoCalibrationConnectCallback(&settingsUpdatedCb); - HomeLocationConnectCallback(&settingsUpdatedCb); - EKFConfigurationConnectCallback(&settingsUpdatedCb); - FlightStatusConnectCallback(&settingsUpdatedCb); + // Initialize this here while we aren't setting the homelocation in GPS + HomeLocationInitialize(); - return 0; + // Initialize quaternion + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + attitude.q1 = 1.0f; + attitude.q2 = 0.0f; + attitude.q3 = 0.0f; + attitude.q4 = 0.0f; + AttitudeActualSet(&attitude); + + // Cannot trust the values to init right above if BL runs + GyrosBiasData gyrosBias; + GyrosBiasGet(&gyrosBias); + gyrosBias.x = 0.0f; + gyrosBias.y = 0.0f; + gyrosBias.z = 0.0f; + GyrosBiasSet(&gyrosBias); + + AttitudeSettingsConnectCallback(&settingsUpdatedCb); + RevoSettingsConnectCallback(&settingsUpdatedCb); + RevoCalibrationConnectCallback(&settingsUpdatedCb); + HomeLocationConnectCallback(&settingsUpdatedCb); + EKFConfigurationConnectCallback(&settingsUpdatedCb); + FlightStatusConnectCallback(&settingsUpdatedCb); + + return 0; } /** @@ -205,29 +207,29 @@ int32_t AttitudeInitialize(void) */ int32_t AttitudeStart(void) { - // Create the queues for the sensors - gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - accelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - magQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - baroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + // Create the queues for the sensors + gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + accelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + magQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + baroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - // Start main task - xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &attitudeTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); - - GyrosConnectQueue(gyroQueue); - AccelsConnectQueue(accelQueue); - MagnetometerConnectQueue(magQueue); - AirspeedSensorConnectQueue(airspeedQueue); - BaroAltitudeConnectQueue(baroQueue); - GPSPositionConnectQueue(gpsQueue); - GPSVelocityConnectQueue(gpsVelQueue); + // Start main task + xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &attitudeTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); - return 0; + GyrosConnectQueue(gyroQueue); + AccelsConnectQueue(accelQueue); + MagnetometerConnectQueue(magQueue); + AirspeedSensorConnectQueue(airspeedQueue); + BaroAltitudeConnectQueue(baroQueue); + GPSPositionConnectQueue(gpsQueue); + GPSVelocityConnectQueue(gpsVelQueue); + + return 0; } MODULE_INITCALL(AttitudeInitialize, AttitudeStart) @@ -237,48 +239,46 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart) */ static void AttitudeTask(__attribute__((unused)) void *parameters) { + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + // Force settings update to make sure rotation loaded + settingsUpdatedCb(NULL); - // Force settings update to make sure rotation loaded - settingsUpdatedCb(NULL); + // Wait for all the sensors be to read + vTaskDelay(100); - // Wait for all the sensors be to read - vTaskDelay(100); + // Main task loop - TODO: make it run as delayed callback + while (1) { + int32_t ret_val = -1; - // Main task loop - TODO: make it run as delayed callback - while (1) { + bool first_run = false; + if (initialization_required) { + initialization_required = false; + first_run = true; + } - int32_t ret_val = -1; + // This function blocks on data queue + switch (running_algorithm) { + case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: + ret_val = updateAttitudeComplementary(first_run); + break; + case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: + ret_val = updateAttitudeINSGPS(first_run, true); + break; + case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: + ret_val = updateAttitudeINSGPS(first_run, false); + break; + default: + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + break; + } - bool first_run = false; - if (initialization_required) { - initialization_required = false; - first_run = true; - } + if (ret_val != 0) { + initialization_required = true; + } - // This function blocks on data queue - switch (running_algorithm ) { - case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: - ret_val = updateAttitudeComplementary(first_run); - break; - case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: - ret_val = updateAttitudeINSGPS(first_run, true); - break; - case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: - ret_val = updateAttitudeINSGPS(first_run, false); - break; - default: - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL); - break; - } - - if(ret_val != 0) { - initialization_required = true; - } - - PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); - } + PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); + } } float accel_mag; @@ -290,282 +290,281 @@ float magKp = 0.01f; static int32_t updateAttitudeComplementary(bool first_run) { - UAVObjEvent ev; - GyrosData gyrosData; - AccelsData accelsData; - static int32_t timeval; - float dT; - static uint8_t init = 0; + UAVObjEvent ev; + GyrosData gyrosData; + AccelsData accelsData; + static int32_t timeval; + float dT; + static uint8_t init = 0; - // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe - if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE || - xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE ) - { - // When one of these is updated so should the other - // Do not set attitude timeout warnings in simulation mode - if (!AttitudeActualReadOnly()){ - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING); - return -1; - } - } + // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe + if (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE || + xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) { + // When one of these is updated so should the other + // Do not set attitude timeout warnings in simulation mode + if (!AttitudeActualReadOnly()) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); + return -1; + } + } - AccelsGet(&accelsData); + AccelsGet(&accelsData); - // During initialization and - if(first_run) { + // During initialization and + if (first_run) { #if defined(PIOS_INCLUDE_HMC5883) - // To initialize we need a valid mag reading - if ( xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE ) - return -1; - MagnetometerData magData; - MagnetometerGet(&magData); + // To initialize we need a valid mag reading + if (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE) { + return -1; + } + MagnetometerData magData; + MagnetometerGet(&magData); #else - MagnetometerData magData; - magData.x = 100.0f; - magData.y = 0.0f; - magData.z = 0.0f; + MagnetometerData magData; + magData.x = 100.0f; + magData.y = 0.0f; + magData.z = 0.0f; #endif - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - init = 0; + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); + init = 0; - // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, - // so pseudo "north" vector can be estimated even if the board is not level - attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z); - float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y; - float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z; + // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, + // so pseudo "north" vector can be estimated even if the board is not level + attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z); + float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y; + float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z; - // rotate accels z vector according to roll - float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y; - attitudeActual.Pitch = atan2f(accelsData.x, -azn); + // rotate accels z vector according to roll + float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y; + attitudeActual.Pitch = atan2f(accelsData.x, -azn); - float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn; + float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn; - attitudeActual.Yaw = atan2f(-yn, xn); - // TODO: This is still a hack - // Put this in a proper generic function in CoordinateConversion.c - // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) - // should calculate the rotation in 3d space using proper cross product math - // SUBTODO: formulate the math required + attitudeActual.Yaw = atan2f(-yn, xn); + // TODO: This is still a hack + // Put this in a proper generic function in CoordinateConversion.c + // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) + // should calculate the rotation in 3d space using proper cross product math + // SUBTODO: formulate the math required - attitudeActual.Roll = RAD2DEG(attitudeActual.Roll); - attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch); - attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw); + attitudeActual.Roll = RAD2DEG(attitudeActual.Roll); + attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch); + attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw); - RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); - AttitudeActualSet(&attitudeActual); + RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1); + AttitudeActualSet(&attitudeActual); - timeval = PIOS_DELAY_GetRaw(); + timeval = PIOS_DELAY_GetRaw(); - return 0; + return 0; + } - } + if ((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { + // For first 7 seconds use accels to get gyro bias + attitudeSettings.AccelKp = 1.0f; + attitudeSettings.AccelKi = 0.9f; + attitudeSettings.YawBiasRate = 0.23f; + magKp = 1.0f; + } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { + attitudeSettings.AccelKp = 1.0f; + attitudeSettings.AccelKi = 0.9f; + attitudeSettings.YawBiasRate = 0.23f; + magKp = 1.0f; + init = 0; + } else if (init == 0) { + // Reload settings (all the rates) + AttitudeSettingsGet(&attitudeSettings); + magKp = 0.01f; + init = 1; + } - if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { - // For first 7 seconds use accels to get gyro bias - attitudeSettings.AccelKp = 1.0f; - attitudeSettings.AccelKi = 0.9f; - attitudeSettings.YawBiasRate = 0.23f; - magKp = 1.0f; - } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { - attitudeSettings.AccelKp = 1.0f; - attitudeSettings.AccelKi = 0.9f; - attitudeSettings.YawBiasRate = 0.23f; - magKp = 1.0f; - init = 0; - } else if (init == 0) { - // Reload settings (all the rates) - AttitudeSettingsGet(&attitudeSettings); - magKp = 0.01f; - init = 1; - } + GyrosGet(&gyrosData); - GyrosGet(&gyrosData); + // Compute the dT using the cpu clock + dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; + timeval = PIOS_DELAY_GetRaw(); - // Compute the dT using the cpu clock - dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; - timeval = PIOS_DELAY_GetRaw(); + float q[4]; - float q[4]; + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); + float grot[3]; + float accel_err[3]; - float grot[3]; - float accel_err[3]; + // Get the current attitude estimate + quat_copy(&attitudeActual.q1, q); - // Get the current attitude estimate - quat_copy(&attitudeActual.q1, q); + // Rotate gravity to body frame and cross with accels + grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2])); + grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1])); + grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); + CrossProduct((const float *)&accelsData.x, (const float *)grot, accel_err); - // Rotate gravity to body frame and cross with accels - grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2])); - grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1])); - grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); - CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err); + // Account for accel magnitude + accel_mag = accelsData.x * accelsData.x + accelsData.y * accelsData.y + accelsData.z * accelsData.z; + accel_mag = sqrtf(accel_mag); + accel_err[0] /= accel_mag; + accel_err[1] /= accel_mag; + accel_err[2] /= accel_mag; - // Account for accel magnitude - accel_mag = accelsData.x*accelsData.x + accelsData.y*accelsData.y + accelsData.z*accelsData.z; - accel_mag = sqrtf(accel_mag); - accel_err[0] /= accel_mag; - accel_err[1] /= accel_mag; - accel_err[2] /= accel_mag; + if (xQueueReceive(magQueue, &ev, 0) != pdTRUE) { + // Rotate gravity to body frame and cross with accels + float brot[3]; + float Rbe[3][3]; + MagnetometerData mag; - if ( xQueueReceive(magQueue, &ev, 0) != pdTRUE ) - { - // Rotate gravity to body frame and cross with accels - float brot[3]; - float Rbe[3][3]; - MagnetometerData mag; - - Quaternion2R(q, Rbe); - MagnetometerGet(&mag); + Quaternion2R(q, Rbe); + MagnetometerGet(&mag); - // If the mag is producing bad data don't use it (normally bad calibration) - if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) { - rot_mult(Rbe, homeLocation.Be, brot); + // If the mag is producing bad data don't use it (normally bad calibration) + if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) { + rot_mult(Rbe, homeLocation.Be, brot); - float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z); - mag.x /= mag_len; - mag.y /= mag_len; - mag.z /= mag_len; + float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z); + mag.x /= mag_len; + mag.y /= mag_len; + mag.z /= mag_len; - float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]); - brot[0] /= bmag; - brot[1] /= bmag; - brot[2] /= bmag; + float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]); + brot[0] /= bmag; + brot[1] /= bmag; + brot[2] /= bmag; - // Only compute if neither vector is null - if (bmag < 1.0f || mag_len < 1.0f) - mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; - else - CrossProduct((const float *) &mag.x, (const float *) brot, mag_err); - } - } else { - mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; - } + // Only compute if neither vector is null + if (bmag < 1.0f || mag_len < 1.0f) { + mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; + } else { + CrossProduct((const float *)&mag.x, (const float *)brot, mag_err); + } + } + } else { + mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; + } - // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s - GyrosBiasData gyrosBias; - GyrosBiasGet(&gyrosBias); - gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi; - gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi; - gyrosBias.z -= mag_err[2] * magKi; - GyrosBiasSet(&gyrosBias); + // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s + GyrosBiasData gyrosBias; + GyrosBiasGet(&gyrosBias); + gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi; + gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi; + gyrosBias.z -= mag_err[2] * magKi; + GyrosBiasSet(&gyrosBias); - if (revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { - // if the raw values are not adjusted, we need to adjust here. - gyrosData.x -= gyrosBias.x; - gyrosData.y -= gyrosBias.y; - gyrosData.z -= gyrosBias.z; - } + if (revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { + // if the raw values are not adjusted, we need to adjust here. + gyrosData.x -= gyrosBias.x; + gyrosData.y -= gyrosBias.y; + gyrosData.z -= gyrosBias.z; + } - // Correct rates based on error, integral component dealt with in updateSensors - gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT; - gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT; - gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; + // Correct rates based on error, integral component dealt with in updateSensors + gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT; + gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT; + gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; - // Work out time derivative from INSAlgo writeup - // Also accounts for the fact that gyros are in deg/s - float qdot[4]; - qdot[0] = DEG2RAD(-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT / 2; - qdot[1] = DEG2RAD(q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT / 2; - qdot[2] = DEG2RAD(q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT / 2; - qdot[3] = DEG2RAD(-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT / 2; + // Work out time derivative from INSAlgo writeup + // Also accounts for the fact that gyros are in deg/s + float qdot[4]; + qdot[0] = DEG2RAD(-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT / 2; + qdot[1] = DEG2RAD(q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT / 2; + qdot[2] = DEG2RAD(q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT / 2; + qdot[3] = DEG2RAD(-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT / 2; - // Take a time step - q[0] = q[0] + qdot[0]; - q[1] = q[1] + qdot[1]; - q[2] = q[2] + qdot[2]; - q[3] = q[3] + qdot[3]; + // Take a time step + q[0] = q[0] + qdot[0]; + q[1] = q[1] + qdot[1]; + q[2] = q[2] + qdot[2]; + q[3] = q[3] + qdot[3]; - if(q[0] < 0.0f) { - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; - } + if (q[0] < 0.0f) { + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } - // Renomalize - qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); - q[0] = q[0] / qmag; - q[1] = q[1] / qmag; - q[2] = q[2] / qmag; - q[3] = q[3] / qmag; + // Renomalize + qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + q[0] = q[0] / qmag; + q[1] = q[1] / qmag; + q[2] = q[2] / qmag; + q[3] = q[3] / qmag; - // If quaternion has become inappropriately short or is nan reinit. - // THIS SHOULD NEVER ACTUALLY HAPPEN - if((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { - q[0] = 1.0f; - q[1] = 0.0f; - q[2] = 0.0f; - q[3] = 0.0f; - } + // If quaternion has become inappropriately short or is nan reinit. + // THIS SHOULD NEVER ACTUALLY HAPPEN + if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { + q[0] = 1.0f; + q[1] = 0.0f; + q[2] = 0.0f; + q[3] = 0.0f; + } - quat_copy(q, &attitudeActual.q1); + quat_copy(q, &attitudeActual.q1); - // Convert into eueler degrees (makes assumptions about RPY order) - Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll); + // Convert into eueler degrees (makes assumptions about RPY order) + Quaternion2RPY(&attitudeActual.q1, &attitudeActual.Roll); - AttitudeActualSet(&attitudeActual); + AttitudeActualSet(&attitudeActual); - // Flush these queues for avoid errors - xQueueReceive(baroQueue, &ev, 0); - if ( xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE ) { - float NED[3]; - // Transform the GPS position into NED coordinates - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - getNED(&gpsPosition, NED); - - PositionActualData positionActual; - PositionActualGet(&positionActual); - positionActual.North = NED[0]; - positionActual.East = NED[1]; - positionActual.Down = NED[2]; - PositionActualSet(&positionActual); - } + // Flush these queues for avoid errors + xQueueReceive(baroQueue, &ev, 0); + if (xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE) { + float NED[3]; + // Transform the GPS position into NED coordinates + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + getNED(&gpsPosition, NED); - if ( xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE ) { - // Transform the GPS position into NED coordinates - GPSVelocityData gpsVelocity; - GPSVelocityGet(&gpsVelocity); + PositionActualData positionActual; + PositionActualGet(&positionActual); + positionActual.North = NED[0]; + positionActual.East = NED[1]; + positionActual.Down = NED[2]; + PositionActualSet(&positionActual); + } - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); - velocityActual.North = gpsVelocity.North; - velocityActual.East = gpsVelocity.East; - velocityActual.Down = gpsVelocity.Down; - VelocityActualSet(&velocityActual); - } - - if ( xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE ) { - // Calculate true airspeed from indicated airspeed - AirspeedSensorData airspeedSensor; - AirspeedSensorGet(&airspeedSensor); + if (xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE) { + // Transform the GPS position into NED coordinates + GPSVelocityData gpsVelocity; + GPSVelocityGet(&gpsVelocity); - AirspeedActualData airspeed; - AirspeedActualGet(&airspeed); + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); + velocityActual.North = gpsVelocity.North; + velocityActual.East = gpsVelocity.East; + velocityActual.Down = gpsVelocity.Down; + VelocityActualSet(&velocityActual); + } - PositionActualData positionActual; - PositionActualGet(&positionActual); + if (xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE) { + // Calculate true airspeed from indicated airspeed + AirspeedSensorData airspeedSensor; + AirspeedSensorGet(&airspeedSensor); - if (airspeedSensor.SensorConnected==AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { - // we have airspeed available - airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed; - airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS( homeLocation.Altitude - positionActual.Down ); - AirspeedActualSet(&airspeed); - } - } + AirspeedActualData airspeed; + AirspeedActualGet(&airspeed); + + PositionActualData positionActual; + PositionActualGet(&positionActual); + + if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { + // we have airspeed available + airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed; + airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionActual.Down); + AirspeedActualSet(&airspeed); + } + } - if ( variance_error ) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - } + if (variance_error) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + } - return 0; + return 0; } #include "insgps.h" @@ -581,81 +580,80 @@ int32_t init_stage = 0; */ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) { - UAVObjEvent ev; - GyrosData gyrosData; - AccelsData accelsData; - MagnetometerData magData; - AirspeedSensorData airspeedData; - BaroAltitudeData baroData; - GPSPositionData gpsData; - GPSVelocityData gpsVelData; - GyrosBiasData gyrosBias; + UAVObjEvent ev; + GyrosData gyrosData; + AccelsData accelsData; + MagnetometerData magData; + AirspeedSensorData airspeedData; + BaroAltitudeData baroData; + GPSPositionData gpsData; + GPSVelocityData gpsVelData; + GyrosBiasData gyrosBias; - static bool mag_updated = false; - static bool baro_updated; - static bool airspeed_updated; - static bool gps_updated; - static bool gps_vel_updated; + static bool mag_updated = false; + static bool baro_updated; + static bool airspeed_updated; + static bool gps_updated; + static bool gps_vel_updated; - static bool value_error = false; + static bool value_error = false; - static float baroOffset = 0.0f; + static float baroOffset = 0.0f; - static uint32_t ins_last_time = 0; - static bool inited; + static uint32_t ins_last_time = 0; + static bool inited; - float NED[3] = {0.0f, 0.0f, 0.0f}; - float vel[3] = {0.0f, 0.0f, 0.0f}; - float zeros[3] = {0.0f, 0.0f, 0.0f}; + float NED[3] = { 0.0f, 0.0f, 0.0f }; + float vel[3] = { 0.0f, 0.0f, 0.0f }; + float zeros[3] = { 0.0f, 0.0f, 0.0f }; - // Perform the update - uint16_t sensors = 0; - float dT; + // Perform the update + uint16_t sensors = 0; + float dT; - // Wait until the gyro and accel object is updated, if a timeout then go to failsafe - if ( (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) || - (xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) ) - { - // Do not set attitude timeout warnings in simulation mode - if (!AttitudeActualReadOnly()){ - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING); - return -1; - } - } + // Wait until the gyro and accel object is updated, if a timeout then go to failsafe + if ((xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) || + (xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE)) { + // Do not set attitude timeout warnings in simulation mode + if (!AttitudeActualReadOnly()) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); + return -1; + } + } - if (inited) { - mag_updated = 0; - baro_updated = 0; - airspeed_updated = 0; - gps_updated = 0; - gps_vel_updated = 0; - } + if (inited) { + mag_updated = 0; + baro_updated = 0; + airspeed_updated = 0; + gps_updated = 0; + gps_vel_updated = 0; + } - if (first_run) { - inited = false; - init_stage = 0; + if (first_run) { + inited = false; + init_stage = 0; - mag_updated = 0; - baro_updated = 0; - airspeed_updated = 0; - gps_updated = 0; - gps_vel_updated = 0; + mag_updated = 0; + baro_updated = 0; + airspeed_updated = 0; + gps_updated = 0; + gps_vel_updated = 0; - ins_last_time = PIOS_DELAY_GetRaw(); + ins_last_time = PIOS_DELAY_GetRaw(); - return 0; - } + return 0; + } - mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE); - baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; - airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; + mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE); + baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; + airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; - // Check if we are running simulation - if (!GPSPositionReadOnly()) { - gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; - } else { - gps_updated |= pdTRUE && outdoor_mode; - } + // Check if we are running simulation + if (!GPSPositionReadOnly()) { + gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; + } else { + gps_updated |= pdTRUE && outdoor_mode; + } if (!GPSVelocityReadOnly()) { gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; @@ -663,373 +661,375 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) gps_vel_updated |= pdTRUE && outdoor_mode; } - // Get most recent data - GyrosGet(&gyrosData); - AccelsGet(&accelsData); - MagnetometerGet(&magData); - BaroAltitudeGet(&baroData); - AirspeedSensorGet(&airspeedData); - GPSPositionGet(&gpsData); - GPSVelocityGet(&gpsVelData); - GyrosBiasGet(&gyrosBias); + // Get most recent data + GyrosGet(&gyrosData); + AccelsGet(&accelsData); + MagnetometerGet(&magData); + BaroAltitudeGet(&baroData); + AirspeedSensorGet(&airspeedData); + GPSPositionGet(&gpsData); + GPSVelocityGet(&gpsVelData); + GyrosBiasGet(&gyrosBias); - value_error = false; - // safety checks - if ( invalid(gyrosData.x) || - invalid(gyrosData.y) || - invalid(gyrosData.z) || - invalid(accelsData.x) || - invalid(accelsData.y) || - invalid(accelsData.z) ) { - // cannot run process update, raise error! - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); - return 0; - } - if ( invalid(gyrosBias.x) || - invalid(gyrosBias.y) || - invalid(gyrosBias.z) ) { - gyrosBias.x = 0.0f; - gyrosBias.y = 0.0f; - gyrosBias.z = 0.0f; - } + value_error = false; + // safety checks + if (invalid(gyrosData.x) || + invalid(gyrosData.y) || + invalid(gyrosData.z) || + invalid(accelsData.x) || + invalid(accelsData.y) || + invalid(accelsData.z)) { + // cannot run process update, raise error! + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + return 0; + } + if (invalid(gyrosBias.x) || + invalid(gyrosBias.y) || + invalid(gyrosBias.z)) { + gyrosBias.x = 0.0f; + gyrosBias.y = 0.0f; + gyrosBias.z = 0.0f; + } - if ( invalid(magData.x) || - invalid(magData.y) || - invalid(magData.z) ) { + if (invalid(magData.x) || + invalid(magData.y) || + invalid(magData.z)) { + // magnetometers can be ignored for a while + mag_updated = false; + value_error = true; + } - // magnetometers can be ignored for a while - mag_updated = false; - value_error = true; - } + // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily + // switching between indoor and outdoor mode with Set = false) + if ((homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f)) { + mag_updated = false; + value_error = true; + } - // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily - // switching between indoor and outdoor mode with Set = false) - if ( (homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f) ) { - mag_updated = false; - value_error = true; - } + if (invalid(baroData.Altitude)) { + baro_updated = false; + value_error = true; + } - if ( invalid(baroData.Altitude) ) { - baro_updated = false; - value_error = true; - } + if (invalid(airspeedData.CalibratedAirspeed)) { + airspeed_updated = false; + value_error = true; + } - if ( invalid(airspeedData.CalibratedAirspeed) ) { - airspeed_updated = false; - value_error = true; - } + if (invalid(gpsData.Altitude)) { + gps_updated = false; + value_error = true; + } - if ( invalid(gpsData.Altitude) ) { - gps_updated = false; - value_error = true; - } + if (invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH]) || + invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST]) || + invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN]) || + invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH]) || + invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST]) || + invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN])) { + gps_updated = false; + value_error = true; + } - if ( invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN]) ) { - gps_updated = false; - value_error = true; - } + if (invalid(gpsVelData.North) || + invalid(gpsVelData.East) || + invalid(gpsVelData.Down)) { + gps_vel_updated = false; + value_error = true; + } - if ( invalid(gpsVelData.North) || - invalid(gpsVelData.East) || - invalid(gpsVelData.Down) ) { - gps_vel_updated = false; - value_error = true; - } + // Discard airspeed if sensor not connected + if (airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { + airspeed_updated = false; + } - // Discard airspeed if sensor not connected - if ( airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE ) { - airspeed_updated = false; - } + // Have a minimum requirement for gps usage + if ((gpsData.Satellites < 7) || + (gpsData.PDOP > 4.0f) || + (gpsData.Latitude == 0 && gpsData.Longitude == 0) || + (homeLocation.Set != HOMELOCATION_SET_TRUE)) { + gps_updated = false; + gps_vel_updated = false; + } - // Have a minimum requirement for gps usage - if ( ( gpsData.Satellites < 7 ) || - ( gpsData.PDOP > 4.0f ) || - ( gpsData.Latitude==0 && gpsData.Longitude==0 ) || - ( homeLocation.Set != HOMELOCATION_SET_TRUE ) ) { - gps_updated = false; - gps_vel_updated = false; - } + if (!inited) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + } else if (value_error) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + } else if (variance_error) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + } else if (outdoor_mode && gpsData.Satellites < 7) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + } - if ( !inited ) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); - } else if ( value_error ) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL); - } else if ( variance_error ) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL); - } else if (outdoor_mode && gpsData.Satellites < 7) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - } - - dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; - ins_last_time = PIOS_DELAY_GetRaw(); + dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; + ins_last_time = PIOS_DELAY_GetRaw(); - // This should only happen at start up or at mode switches - if(dT > 0.01f) { - dT = 0.01f; - } else if(dT <= 0.001f) { - dT = 0.001f; - } + // This should only happen at start up or at mode switches + if (dT > 0.01f) { + dT = 0.01f; + } else if (dT <= 0.001f) { + dT = 0.001f; + } - if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode) && !variance_error) { + if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode) && !variance_error) { + // Don't initialize until all sensors are read + if (init_stage == 0) { + // Reset the INS algorithm + INSGPSInit(); + INSSetMagVar((float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_MAGX], + ekfConfiguration.R[EKFCONFIGURATION_R_MAGY], + ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] } + ); + INSSetAccelVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX], + ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY], + ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } + ); + INSSetGyroVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX], + ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY], + ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } + ); + INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX], + ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY], + ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } + ); + INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]); - // Don't initialize until all sensors are read - if (init_stage == 0) { + // Initialize the gyro bias + float gyro_bias[3] = { 0.0f, 0.0f, 0.0f }; + INSSetGyroBias(gyro_bias); - // Reset the INS algorithm - INSGPSInit(); - INSSetMagVar( (float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_MAGX], - ekfConfiguration.R[EKFCONFIGURATION_R_MAGY], - ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] } ); - INSSetAccelVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX], - ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY], - ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } ); - INSSetGyroVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } ); - INSSetGyroBiasVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } ); - INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]); + float pos[3] = { 0.0f, 0.0f, 0.0f }; - // Initialize the gyro bias - float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; - INSSetGyroBias(gyro_bias); + if (outdoor_mode) { + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); - float pos[3] = {0.0f, 0.0f, 0.0f}; + // Transform the GPS position into NED coordinates + getNED(&gpsPosition, pos); - if (outdoor_mode) { + // Initialize barometric offset to current GPS NED coordinate + baroOffset = -pos[2] - baroData.Altitude; + } else { + // Initialize barometric offset to homelocation altitude + baroOffset = -baroData.Altitude; + pos[2] = -(baroData.Altitude + baroOffset); + } - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); + xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); + MagnetometerGet(&magData); - // Transform the GPS position into NED coordinates - getNED(&gpsPosition, pos); + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); - // Initialize barometric offset to current GPS NED coordinate - baroOffset = -pos[2] - baroData.Altitude; + // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, + // so pseudo "north" vector can be estimated even if the board is not level + attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z); + float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y; + float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z; - } else { + // rotate accels z vector according to roll + float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y; + attitudeActual.Pitch = atan2f(accelsData.x, -azn); - // Initialize barometric offset to homelocation altitude - baroOffset = -baroData.Altitude; - pos[2] = -(baroData.Altitude + baroOffset); + float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn; - } + attitudeActual.Yaw = atan2f(-yn, xn); + // TODO: This is still a hack + // Put this in a proper generic function in CoordinateConversion.c + // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) + // should calculate the rotation in 3d space using proper cross product math + // SUBTODO: formulate the math required - xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); - MagnetometerGet(&magData); + attitudeActual.Roll = RAD2DEG(attitudeActual.Roll); + attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch); + attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw); - AttitudeActualData attitudeActual; - AttitudeActualGet (&attitudeActual); + RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1); + AttitudeActualSet(&attitudeActual); - // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, - // so pseudo "north" vector can be estimated even if the board is not level - attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z); - float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y; - float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z; + float q[4] = { attitudeActual.q1, attitudeActual.q2, attitudeActual.q3, attitudeActual.q4 }; + INSSetState(pos, zeros, q, zeros, zeros); - // rotate accels z vector according to roll - float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y; - attitudeActual.Pitch = atan2f(accelsData.x, -azn); + INSResetP(ekfConfiguration.P); + } else { + // Run prediction a bit before any corrections - float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn; + // Because the sensor module remove the bias we need to add it + // back in here so that the INS algorithm can track it correctly + float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) }; + if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { + gyros[0] += DEG2RAD(gyrosBias.x); + gyros[1] += DEG2RAD(gyrosBias.y); + gyros[2] += DEG2RAD(gyrosBias.z); + } + INSStatePrediction(gyros, &accelsData.x, dT); - attitudeActual.Yaw = atan2f(-yn, xn); - // TODO: This is still a hack - // Put this in a proper generic function in CoordinateConversion.c - // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) - // should calculate the rotation in 3d space using proper cross product math - // SUBTODO: formulate the math required + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + attitude.q1 = Nav.q[0]; + attitude.q2 = Nav.q[1]; + attitude.q3 = Nav.q[2]; + attitude.q4 = Nav.q[3]; + Quaternion2RPY(&attitude.q1, &attitude.Roll); + AttitudeActualSet(&attitude); + } - attitudeActual.Roll = RAD2DEG(attitudeActual.Roll); - attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch); - attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw); + init_stage++; + if (init_stage > 10) { + inited = true; + } - RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); - AttitudeActualSet(&attitudeActual); + return 0; + } - float q[4] = { attitudeActual.q1, attitudeActual.q2, attitudeActual.q3, attitudeActual.q4 }; - INSSetState(pos, zeros, q, zeros, zeros); - - INSResetP(ekfConfiguration.P); + if (!inited) { + return 0; + } - } else { - // Run prediction a bit before any corrections + // Because the sensor module remove the bias we need to add it + // back in here so that the INS algorithm can track it correctly + float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) }; + if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { + gyros[0] += DEG2RAD(gyrosBias.x); + gyros[1] += DEG2RAD(gyrosBias.y); + gyros[2] += DEG2RAD(gyrosBias.z); + } - // Because the sensor module remove the bias we need to add it - // back in here so that the INS algorithm can track it correctly - float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) }; - if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { - gyros[0] += DEG2RAD(gyrosBias.x); - gyros[1] += DEG2RAD(gyrosBias.y); - gyros[2] += DEG2RAD(gyrosBias.z); - } - INSStatePrediction(gyros, &accelsData.x, dT); + // Advance the state estimate + INSStatePrediction(gyros, &accelsData.x, dT); - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - attitude.q1 = Nav.q[0]; - attitude.q2 = Nav.q[1]; - attitude.q3 = Nav.q[2]; - attitude.q4 = Nav.q[3]; - Quaternion2RPY(&attitude.q1,&attitude.Roll); - AttitudeActualSet(&attitude); - } + // Copy the attitude into the UAVO + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + attitude.q1 = Nav.q[0]; + attitude.q2 = Nav.q[1]; + attitude.q3 = Nav.q[2]; + attitude.q4 = Nav.q[3]; + Quaternion2RPY(&attitude.q1, &attitude.Roll); + AttitudeActualSet(&attitude); - init_stage++; - if(init_stage > 10) - inited = true; + // Advance the covariance estimate + INSCovariancePrediction(dT); - return 0; - } + if (mag_updated) { + sensors |= MAG_SENSORS; + } - if (!inited) - return 0; + if (baro_updated) { + sensors |= BARO_SENSOR; + } - // Because the sensor module remove the bias we need to add it - // back in here so that the INS algorithm can track it correctly - float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) }; - if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { - gyros[0] += DEG2RAD(gyrosBias.x); - gyros[1] += DEG2RAD(gyrosBias.y); - gyros[2] += DEG2RAD(gyrosBias.z); - } + INSSetMagNorth(homeLocation.Be); - // Advance the state estimate - INSStatePrediction(gyros, &accelsData.x, dT); + if (gps_updated && outdoor_mode) { + INSSetPosVelVar((float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH], + ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST], + ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] }, + (float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH], + ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST], + ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] } + ); + sensors |= POS_SENSORS; - // Copy the attitude into the UAVO - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - attitude.q1 = Nav.q[0]; - attitude.q2 = Nav.q[1]; - attitude.q3 = Nav.q[2]; - attitude.q4 = Nav.q[3]; - Quaternion2RPY(&attitude.q1,&attitude.Roll); - AttitudeActualSet(&attitude); + if (0) { // Old code to take horizontal velocity from GPS Position update + sensors |= HORIZ_SENSORS; + vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading)); + vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading)); + vel[2] = 0.0f; + } + // Transform the GPS position into NED coordinates + getNED(&gpsData, NED); - // Advance the covariance estimate - INSCovariancePrediction(dT); + // Track barometric altitude offset with a low pass filter + baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + + (1.0f - BARO_OFFSET_LOWPASS_ALPHA) + * (-NED[2] - baroData.Altitude); + } else if (!outdoor_mode) { + INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, + (float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] } + ); + vel[0] = vel[1] = vel[2] = 0.0f; + NED[0] = NED[1] = 0.0f; + NED[2] = -(baroData.Altitude + baroOffset); + sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS; + sensors |= POS_SENSORS | VERT_SENSORS; + } - if(mag_updated) - sensors |= MAG_SENSORS; + if (gps_vel_updated && outdoor_mode) { + sensors |= HORIZ_SENSORS | VERT_SENSORS; + vel[0] = gpsVelData.North; + vel[1] = gpsVelData.East; + vel[2] = gpsVelData.Down; + } - if(baro_updated) - sensors |= BARO_SENSOR; + if (airspeed_updated) { + // we have airspeed available + AirspeedActualData airspeed; + AirspeedActualGet(&airspeed); - INSSetMagNorth(homeLocation.Be); - - if (gps_updated && outdoor_mode) - { - INSSetPosVelVar((float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] }, - (float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] }); - sensors |= POS_SENSORS; + airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed; + airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - Nav.Pos[2]); + AirspeedActualSet(&airspeed); - if (0) { // Old code to take horizontal velocity from GPS Position update - sensors |= HORIZ_SENSORS; - vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading)); - vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading)); - vel[2] = 0.0f; - } - // Transform the GPS position into NED coordinates - getNED(&gpsData, NED); + if (!gps_vel_updated && !gps_updated) { + // feed airspeed into EKF, treat wind as 1e2 variance + sensors |= HORIZ_SENSORS | VERT_SENSORS; + INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, + (float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] } + ); + // rotate airspeed vector into NED frame - airspeed is measured in X axis only + float R[3][3]; + Quaternion2R(Nav.q, R); + float vtas[3] = { airspeed.TrueAirspeed, 0.0f, 0.0f }; + rot_mult(R, vtas, vel); + } + } - // Track barometric altitude offset with a low pass filter - baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + - (1.0f - BARO_OFFSET_LOWPASS_ALPHA ) - * ( -NED[2] - baroData.Altitude ); + /* + * TODO: Need to add a general sanity check for all the inputs to make sure their kosher + * although probably should occur within INS itself + */ + if (sensors) { + INSCorrection(&magData.x, NED, vel, (baroData.Altitude + baroOffset), sensors); + } - } else if (!outdoor_mode) { - INSSetPosVelVar((float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, - (float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] }); - vel[0] = vel[1] = vel[2] = 0.0f; - NED[0] = NED[1] = 0.0f; - NED[2] = -(baroData.Altitude + baroOffset); - sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS; - sensors |= POS_SENSORS |VERT_SENSORS; - } + // Copy the position and velocity into the UAVO + PositionActualData positionActual; + PositionActualGet(&positionActual); + positionActual.North = Nav.Pos[0]; + positionActual.East = Nav.Pos[1]; + positionActual.Down = Nav.Pos[2]; + PositionActualSet(&positionActual); - if (gps_vel_updated && outdoor_mode) { - sensors |= HORIZ_SENSORS | VERT_SENSORS; - vel[0] = gpsVelData.North; - vel[1] = gpsVelData.East; - vel[2] = gpsVelData.Down; - } + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); + velocityActual.North = Nav.Vel[0]; + velocityActual.East = Nav.Vel[1]; + velocityActual.Down = Nav.Vel[2]; + VelocityActualSet(&velocityActual); - if (airspeed_updated) { - // we have airspeed available - AirspeedActualData airspeed; - AirspeedActualGet(&airspeed); + gyrosBias.x = RAD2DEG(Nav.gyro_bias[0]); + gyrosBias.y = RAD2DEG(Nav.gyro_bias[1]); + gyrosBias.z = RAD2DEG(Nav.gyro_bias[2]); + GyrosBiasSet(&gyrosBias); - airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed; - airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS( homeLocation.Altitude - Nav.Pos[2] ); - AirspeedActualSet(&airspeed); - - if ( !gps_vel_updated && !gps_updated) { - // feed airspeed into EKF, treat wind as 1e2 variance - sensors |= HORIZ_SENSORS | VERT_SENSORS; - INSSetPosVelVar((float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, - (float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] }); - // rotate airspeed vector into NED frame - airspeed is measured in X axis only - float R[3][3]; - Quaternion2R(Nav.q,R); - float vtas[3] = {airspeed.TrueAirspeed,0.0f,0.0f}; - rot_mult(R,vtas,vel); - } - } - - /* - * TODO: Need to add a general sanity check for all the inputs to make sure their kosher - * although probably should occur within INS itself - */ - if (sensors) - INSCorrection(&magData.x, NED, vel, ( baroData.Altitude + baroOffset ), sensors); + EKFStateVarianceData vardata; + EKFStateVarianceGet(&vardata); + INSGetP(vardata.P); + EKFStateVarianceSet(&vardata); - // Copy the position and velocity into the UAVO - PositionActualData positionActual; - PositionActualGet(&positionActual); - positionActual.North = Nav.Pos[0]; - positionActual.East = Nav.Pos[1]; - positionActual.Down = Nav.Pos[2]; - PositionActualSet(&positionActual); - - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); - velocityActual.North = Nav.Vel[0]; - velocityActual.East = Nav.Vel[1]; - velocityActual.Down = Nav.Vel[2]; - VelocityActualSet(&velocityActual); - - gyrosBias.x = RAD2DEG(Nav.gyro_bias[0]); - gyrosBias.y = RAD2DEG(Nav.gyro_bias[1]); - gyrosBias.z = RAD2DEG(Nav.gyro_bias[2]); - GyrosBiasSet(&gyrosBias); - - EKFStateVarianceData vardata; - EKFStateVarianceGet(&vardata); - INSGetP(vardata.P); - EKFStateVarianceSet(&vardata); - - return 0; + return 0; } /** @@ -1042,90 +1042,90 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) * @returns 0 for success, -1 for failure */ float T[3]; -static int32_t getNED(GPSPositionData * gpsPosition, float * NED) +static int32_t getNED(GPSPositionData *gpsPosition, float *NED) { - float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f), - DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f), - (gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) }; + float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f), + DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f), + (gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) }; - NED[0] = T[0] * dL[0]; - NED[1] = T[1] * dL[1]; - NED[2] = T[2] * dL[2]; + NED[0] = T[0] * dL[0]; + NED[1] = T[1] * dL[1]; + NED[2] = T[2] * dL[2]; - return 0; + return 0; } -static void settingsUpdatedCb(UAVObjEvent * ev) +static void settingsUpdatedCb(UAVObjEvent *ev) { - if (ev == NULL || ev->obj == FlightStatusHandle()) { - FlightStatusGet(&flightStatus); - } - if (ev == NULL || ev->obj == RevoCalibrationHandle()) { - RevoCalibrationGet(&revoCalibration); - } - // change of these settings require reinitialization of the EKF - // when an error flag has been risen, we also listen to flightStatus updates, - // since we are waiting for the system to get disarmed so we can reinitialize safely. - if (ev == NULL || - ev->obj == EKFConfigurationHandle() || - ev->obj == RevoSettingsHandle() || - ( variance_error==true && ev->obj == FlightStatusHandle() ) - ) { + if (ev == NULL || ev->obj == FlightStatusHandle()) { + FlightStatusGet(&flightStatus); + } + if (ev == NULL || ev->obj == RevoCalibrationHandle()) { + RevoCalibrationGet(&revoCalibration); + } + // change of these settings require reinitialization of the EKF + // when an error flag has been risen, we also listen to flightStatus updates, + // since we are waiting for the system to get disarmed so we can reinitialize safely. + if (ev == NULL || + ev->obj == EKFConfigurationHandle() || + ev->obj == RevoSettingsHandle() || + (variance_error == true && ev->obj == FlightStatusHandle()) + ) { + bool error = false; - bool error = false; + EKFConfigurationGet(&ekfConfiguration); + int t; + for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { + if (invalid_var(ekfConfiguration.P[t])) { + error = true; + } + } + for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { + if (invalid_var(ekfConfiguration.Q[t])) { + error = true; + } + } + for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) { + if (invalid_var(ekfConfiguration.R[t])) { + error = true; + } + } - EKFConfigurationGet(&ekfConfiguration); - int t; - for (t=0; t < EKFCONFIGURATION_P_NUMELEM; t++) { - if (invalid_var(ekfConfiguration.P[t])) { - error = true; - } - } - for (t=0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { - if (invalid_var(ekfConfiguration.Q[t])) { - error = true; - } - } - for (t=0; t < EKFCONFIGURATION_R_NUMELEM; t++) { - if (invalid_var(ekfConfiguration.R[t])) { - error = true; - } - } + RevoSettingsGet(&revoSettings); - RevoSettingsGet(&revoSettings); + // Reinitialization of the EKF is not desired during flight. + // It will be delayed until the board is disarmed by raising the error flag. + // We will not prevent the initial initialization though, since the board could be in always armed mode. + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required) { + error = true; + } - // Reinitialization of the EKF is not desired during flight. - // It will be delayed until the board is disarmed by raising the error flag. - // We will not prevent the initial initialization though, since the board could be in always armed mode. - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required ) { - error = true; - } + if (error) { + variance_error = true; + } else { + // trigger reinitialization - possibly with new algorithm + running_algorithm = revoSettings.FusionAlgorithm; + variance_error = false; + initialization_required = true; + } + } + if (ev == NULL || ev->obj == HomeLocationHandle()) { + HomeLocationGet(&homeLocation); + // Compute matrix to convert deltaLLA to NED + float lat, alt; + lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); + alt = homeLocation.Altitude; - if (error) { - variance_error = true; - } else { - // trigger reinitialization - possibly with new algorithm - running_algorithm = revoSettings.FusionAlgorithm; - variance_error = false; - initialization_required = true; - } - } - if(ev == NULL || ev->obj == HomeLocationHandle()) { - HomeLocationGet(&homeLocation); - // Compute matrix to convert deltaLLA to NED - float lat, alt; - lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); - alt = homeLocation.Altitude; + T[0] = alt + 6.378137E6f; + T[1] = cosf(lat) * (alt + 6.378137E6f); + T[2] = -1.0f; - T[0] = alt+6.378137E6f; - T[1] = cosf(lat)*(alt+6.378137E6f); - T[2] = -1.0f; - - // TODO: convert positionActual to new reference frame and gracefully update EKF state! - // needed for long range flights where the reference coordinate is adjusted in flight - } - if (ev == NULL || ev->obj == AttitudeSettingsHandle()) - AttitudeSettingsGet(&attitudeSettings); + // TODO: convert positionActual to new reference frame and gracefully update EKF state! + // needed for long range flights where the reference coordinate is adjusted in flight + } + if (ev == NULL || ev->obj == AttitudeSettingsHandle()) { + AttitudeSettingsGet(&attitudeSettings); + } } /** * @} diff --git a/flight/modules/Attitude/revolution/inc/attitude.h b/flight/modules/Attitude/revolution/inc/attitude.h index 3f90e56a6..64c556a8e 100644 --- a/flight/modules/Attitude/revolution/inc/attitude.h +++ b/flight/modules/Attitude/revolution/inc/attitude.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup Attitude Attitude Module - * @{ + * @{ * * @file attitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. diff --git a/flight/modules/Autotune/autotune.c b/flight/modules/Autotune/autotune.c index 119791993..d14c75090 100644 --- a/flight/modules/Autotune/autotune.c +++ b/flight/modules/Autotune/autotune.c @@ -3,7 +3,7 @@ * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup Autotuning module - * @brief Reads from @ref ManualControlCommand and fakes a rate mode while + * @brief Reads from @ref ManualControlCommand and fakes a rate mode while * toggling the channels to relay mode * @{ * @@ -59,13 +59,13 @@ #include "stabilizationdesired.h" #include "stabilizationsettings.h" #include "taskinfo.h" - + // Private constants #define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY+2) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) // Private types -enum AUTOTUNE_STATE {AT_INIT, AT_START, AT_ROLL, AT_PITCH, AT_FINISHED, AT_SET}; +enum AUTOTUNE_STATE { AT_INIT, AT_START, AT_ROLL, AT_PITCH, AT_FINISHED, AT_SET }; // Private variables static xTaskHandle taskHandle; @@ -81,22 +81,23 @@ static void update_stabilization_settings(); */ int32_t AutotuneInitialize(void) { - // Create a queue, connect to manual control command and flightstatus + // Create a queue, connect to manual control command and flightstatus #ifdef MODULE_AUTOTUNE_BUILTIN - autotuneEnabled = true; + autotuneEnabled = true; #else - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_AUTOTUNE] == HWSETTINGS_OPTIONALMODULES_ENABLED) - autotuneEnabled = true; - else - autotuneEnabled = false; + if (optionalModules[HWSETTINGS_OPTIONALMODULES_AUTOTUNE] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + autotuneEnabled = true; + } else { + autotuneEnabled = false; + } #endif - return 0; + return 0; } /** @@ -105,14 +106,14 @@ int32_t AutotuneInitialize(void) */ int32_t AutotuneStart(void) { - // Start main task if it is enabled - if(autotuneEnabled) { - xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); + // Start main task if it is enabled + if (autotuneEnabled) { + xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE); - } - return 0; + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE); + } + return 0; } MODULE_INITCALL(AutotuneInitialize, AutotuneStart) @@ -122,210 +123,211 @@ MODULE_INITCALL(AutotuneInitialize, AutotuneStart) */ static void AutotuneTask(__attribute__((unused)) void *parameters) { - //AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - - enum AUTOTUNE_STATE state = AT_INIT; + // AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - portTickType lastUpdateTime = xTaskGetTickCount(); + enum AUTOTUNE_STATE state = AT_INIT; - while(1) { + portTickType lastUpdateTime = xTaskGetTickCount(); - PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); - // TODO: - // 1. get from queue - // 2. based on whether it is flightstatus or manualcontrol + while (1) { + PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); + // TODO: + // 1. get from queue + // 2. based on whether it is flightstatus or manualcontrol - portTickType diffTime; + portTickType diffTime; - const uint32_t PREPARE_TIME = 2000; - const uint32_t MEAURE_TIME = 30000; + const uint32_t PREPARE_TIME = 2000; + const uint32_t MEAURE_TIME = 30000; - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); - // Only allow this module to run when autotuning - if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { - state = AT_INIT; - vTaskDelay(50); - continue; - } + // Only allow this module to run when autotuning + if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { + state = AT_INIT; + vTaskDelay(50); + continue; + } - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); - StabilizationSettingsData stabSettings; - StabilizationSettingsGet(&stabSettings); + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); - ManualControlSettingsData manualSettings; - ManualControlSettingsGet(&manualSettings); + ManualControlSettingsData manualSettings; + ManualControlSettingsGet(&manualSettings); - ManualControlCommandData manualControl; - ManualControlCommandGet(&manualControl); + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); - RelayTuningSettingsData relaySettings; - RelayTuningSettingsGet(&relaySettings); + RelayTuningSettingsData relaySettings; + RelayTuningSettingsGet(&relaySettings); - bool rate = relaySettings.Mode == RELAYTUNINGSETTINGS_MODE_RATE; + bool rate = relaySettings.Mode == RELAYTUNINGSETTINGS_MODE_RATE; - if (rate) { // rate mode - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + if (rate) { // rate mode + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; - stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; - } else { - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; + stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; + } else { + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.Roll = manualControl.Roll * stabSettings.RollMax; - stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax; - } + stabDesired.Roll = manualControl.Roll * stabSettings.RollMax; + stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax; + } - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; - stabDesired.Throttle = manualControl.Throttle; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; + stabDesired.Throttle = manualControl.Throttle; - switch(state) { - case AT_INIT: + switch (state) { + case AT_INIT: - lastUpdateTime = xTaskGetTickCount(); + lastUpdateTime = xTaskGetTickCount(); - // Only start when armed and flying - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0) - state = AT_START; - break; + // Only start when armed and flying + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0) { + state = AT_START; + } + break; - case AT_START: + case AT_START: - diffTime = xTaskGetTickCount() - lastUpdateTime; + diffTime = xTaskGetTickCount() - lastUpdateTime; - // Spend the first block of time in normal rate mode to get airborne - if (diffTime > PREPARE_TIME) { - state = AT_ROLL; - lastUpdateTime = xTaskGetTickCount(); - } - break; + // Spend the first block of time in normal rate mode to get airborne + if (diffTime > PREPARE_TIME) { + state = AT_ROLL; + lastUpdateTime = xTaskGetTickCount(); + } + break; - case AT_ROLL: + case AT_ROLL: - diffTime = xTaskGetTickCount() - lastUpdateTime; + diffTime = xTaskGetTickCount() - lastUpdateTime; - // Run relay mode on the roll axis for the measurement time - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : - STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; - if (diffTime > MEAURE_TIME) { // Move on to next state - state = AT_PITCH; - lastUpdateTime = xTaskGetTickCount(); - } - break; + // Run relay mode on the roll axis for the measurement time + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : + STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; + if (diffTime > MEAURE_TIME) { // Move on to next state + state = AT_PITCH; + lastUpdateTime = xTaskGetTickCount(); + } + break; - case AT_PITCH: + case AT_PITCH: - diffTime = xTaskGetTickCount() - lastUpdateTime; + diffTime = xTaskGetTickCount() - lastUpdateTime; - // Run relay mode on the pitch axis for the measurement time - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : - STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; - if (diffTime > MEAURE_TIME) { // Move on to next state - state = AT_FINISHED; - lastUpdateTime = xTaskGetTickCount(); - } - break; + // Run relay mode on the pitch axis for the measurement time + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : + STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; + if (diffTime > MEAURE_TIME) { // Move on to next state + state = AT_FINISHED; + lastUpdateTime = xTaskGetTickCount(); + } + break; - case AT_FINISHED: + case AT_FINISHED: - // Wait until disarmed and landed before updating the settings - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0) - state = AT_SET; + // Wait until disarmed and landed before updating the settings + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0) { + state = AT_SET; + } - break; + break; - case AT_SET: - update_stabilization_settings(); - state = AT_INIT; - break; + case AT_SET: + update_stabilization_settings(); + state = AT_INIT; + break; - default: - // Set an alarm or some shit like that - break; - } + default: + // Set an alarm or some shit like that + break; + } - StabilizationDesiredSet(&stabDesired); + StabilizationDesiredSet(&stabDesired); - vTaskDelay(10); - } + vTaskDelay(10); + } } /** * Called after measuring roll and pitch to update the * stabilization settings - * + * * takes in @ref RelayTuning and outputs @ref StabilizationSettings */ static void update_stabilization_settings() { - RelayTuningData relayTuning; - RelayTuningGet(&relayTuning); + RelayTuningData relayTuning; - RelayTuningSettingsData relaySettings; - RelayTuningSettingsGet(&relaySettings); + RelayTuningGet(&relayTuning); - StabilizationSettingsData stabSettings; - StabilizationSettingsGet(&stabSettings); + RelayTuningSettingsData relaySettings; + RelayTuningSettingsGet(&relaySettings); - // Eventually get these settings from RelayTuningSettings - const float gain_ratio_r = 1.0f / 3.0f; - const float zero_ratio_r = 1.0f / 3.0f; - const float gain_ratio_p = 1.0f / 5.0f; - const float zero_ratio_p = 1.0f / 5.0f; + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); - // For now just run over roll and pitch - for (uint i = 0; i < 2; i++) { - float wu = 1000.0f * 2 * M_PI / relayTuning.Period[i]; // ultimate freq = output osc freq (rad/s) + // Eventually get these settings from RelayTuningSettings + const float gain_ratio_r = 1.0f / 3.0f; + const float zero_ratio_r = 1.0f / 3.0f; + const float gain_ratio_p = 1.0f / 5.0f; + const float zero_ratio_p = 1.0f / 5.0f; - float wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s) - float zc = wc * zero_ratio_r; // controller zero location (rad/s) - float kpu = 4.0f / M_PI / relayTuning.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity - float kp = kpu * gain_ratio_r; // proportional gain - float ki = zc * kp; // integral gain + // For now just run over roll and pitch + for (uint i = 0; i < 2; i++) { + float wu = 1000.0f * 2 * M_PI / relayTuning.Period[i]; // ultimate freq = output osc freq (rad/s) - // Now calculate gains for the next loop out knowing it is the integral of - // the inner loop -- the plant is position/velocity = scale*1/s - float wc2 = wc * gain_ratio_p; // crossover of the attitude loop - float kp2 = wc2; // kp of attitude - float ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude + float wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s) + float zc = wc * zero_ratio_r; // controller zero location (rad/s) + float kpu = 4.0f / M_PI / relayTuning.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity + float kp = kpu * gain_ratio_r; // proportional gain + float ki = zc * kp; // integral gain - switch(i) { - case 0: // roll - stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp; - stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki; - stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2; - stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2; - break; - case 1: // Pitch - stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp; - stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki; - stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2; - stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2; - break; - default: - // Oh shit oh shit oh shit - break; - } - } - switch(relaySettings.Behavior) { - case RELAYTUNINGSETTINGS_BEHAVIOR_MEASURE: - // Just measure, don't update the stab settings - break; - case RELAYTUNINGSETTINGS_BEHAVIOR_COMPUTE: - StabilizationSettingsSet(&stabSettings); - break; - case RELAYTUNINGSETTINGS_BEHAVIOR_SAVE: - StabilizationSettingsSet(&stabSettings); - UAVObjSave(StabilizationSettingsHandle(), 0); - break; - } - + // Now calculate gains for the next loop out knowing it is the integral of + // the inner loop -- the plant is position/velocity = scale*1/s + float wc2 = wc * gain_ratio_p; // crossover of the attitude loop + float kp2 = wc2; // kp of attitude + float ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude + + switch (i) { + case 0: // roll + stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp; + stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki; + stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2; + stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2; + break; + case 1: // Pitch + stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp; + stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki; + stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2; + stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2; + break; + default: + // Oh shit oh shit oh shit + break; + } + } + switch (relaySettings.Behavior) { + case RELAYTUNINGSETTINGS_BEHAVIOR_MEASURE: + // Just measure, don't update the stab settings + break; + case RELAYTUNINGSETTINGS_BEHAVIOR_COMPUTE: + StabilizationSettingsSet(&stabSettings); + break; + case RELAYTUNINGSETTINGS_BEHAVIOR_SAVE: + StabilizationSettingsSet(&stabSettings); + UAVObjSave(StabilizationSettingsHandle(), 0); + break; + } } /** diff --git a/flight/modules/Autotune/inc/autotune.h b/flight/modules/Autotune/inc/autotune.h index 3f90e56a6..64c556a8e 100644 --- a/flight/modules/Autotune/inc/autotune.h +++ b/flight/modules/Autotune/inc/autotune.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup Attitude Attitude Module - * @{ + * @{ * * @file attitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. diff --git a/flight/modules/Battery/battery.c b/flight/modules/Battery/battery.c index c0f6db89e..66ed8893a 100644 --- a/flight/modules/Battery/battery.c +++ b/flight/modules/Battery/battery.c @@ -54,21 +54,21 @@ // // Configuration // -#define SAMPLE_PERIOD_MS 500 +#define SAMPLE_PERIOD_MS 500 // Private types // Private variables static bool batteryEnabled = false; -//THESE COULD BE BETTER AS SOME KIND OF UNION OR STRUCT, BY WHICH 4 BITS ARE USED FOR EACH -//PIN VARIABLE, ONE OF WHICH INDICATES SIGN, AND THE OTHER 3 BITS INDICATE POSITION. THIS WILL -//WORK FOR QUITE SOMETIME, UNTIL MORE THAN 8 ADC ARE AVAILABLE. EVEN AT THIS POINT, THE STRUCTURE -//CAN SIMPLY BE MODIFIED TO SUPPORT 15 ADC PINS, BY USING ALL AVAILABLE BITS. -static int8_t voltageADCPin=-1; //ADC pin for voltage -static int8_t currentADCPin=-1; //ADC pin for current +// THESE COULD BE BETTER AS SOME KIND OF UNION OR STRUCT, BY WHICH 4 BITS ARE USED FOR EACH +// PIN VARIABLE, ONE OF WHICH INDICATES SIGN, AND THE OTHER 3 BITS INDICATE POSITION. THIS WILL +// WORK FOR QUITE SOMETIME, UNTIL MORE THAN 8 ADC ARE AVAILABLE. EVEN AT THIS POINT, THE STRUCTURE +// CAN SIMPLY BE MODIFIED TO SUPPORT 15 ADC PINS, BY USING ALL AVAILABLE BITS. +static int8_t voltageADCPin = -1; // ADC pin for voltage +static int8_t currentADCPin = -1; // ADC pin for current // Private functions -static void onTimer(UAVObjEvent* ev); +static void onTimer(UAVObjEvent *ev); /** * Initialise the module, called on startup @@ -76,137 +76,136 @@ static void onTimer(UAVObjEvent* ev); */ int32_t BatteryInitialize(void) { - - #ifdef MODULE_BATTERY_BUILTIN - batteryEnabled = true; + batteryEnabled = true; #else - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(optionalModules); - if ((optionalModules[HWSETTINGS_OPTIONALMODULES_BATTERY] == HWSETTINGS_OPTIONALMODULES_ENABLED)) - batteryEnabled = true; - else - batteryEnabled = false; + if ((optionalModules[HWSETTINGS_OPTIONALMODULES_BATTERY] == HWSETTINGS_OPTIONALMODULES_ENABLED)) { + batteryEnabled = true; + } else { + batteryEnabled = false; + } #endif - uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM]; - HwSettingsADCRoutingGet(adcRouting); - - //Determine if the battery sensors are routed to ADC pins - for (int i=0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { - if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYVOLTAGE) { - voltageADCPin = i; - } - if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYCURRENT) { - currentADCPin = i; - } - } - - //Don't enable module if no ADC pins are routed to the sensors - if (voltageADCPin <0 && currentADCPin <0) - batteryEnabled = false; + uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM]; + HwSettingsADCRoutingGet(adcRouting); - //Start module - if (batteryEnabled) { - FlightBatteryStateInitialize(); - FlightBatterySettingsInitialize(); - - static UAVObjEvent ev; + // Determine if the battery sensors are routed to ADC pins + for (int i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { + if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYVOLTAGE) { + voltageADCPin = i; + } + if (adcRouting[i] == HWSETTINGS_ADCROUTING_BATTERYCURRENT) { + currentADCPin = i; + } + } - memset(&ev,0,sizeof(UAVObjEvent)); - EventPeriodicCallbackCreate(&ev, onTimer, SAMPLE_PERIOD_MS / portTICK_RATE_MS); - } + // Don't enable module if no ADC pins are routed to the sensors + if (voltageADCPin < 0 && currentADCPin < 0) { + batteryEnabled = false; + } - return 0; + // Start module + if (batteryEnabled) { + FlightBatteryStateInitialize(); + FlightBatterySettingsInitialize(); + + static UAVObjEvent ev; + + memset(&ev, 0, sizeof(UAVObjEvent)); + EventPeriodicCallbackCreate(&ev, onTimer, SAMPLE_PERIOD_MS / portTICK_RATE_MS); + } + + return 0; } MODULE_INITCALL(BatteryInitialize, 0) -#define HAS_SENSOR(x) batterySettings.SensorType[x]==FLIGHTBATTERYSETTINGS_SENSORTYPE_ENABLED -static void onTimer(__attribute__((unused)) UAVObjEvent* ev) +#define HAS_SENSOR(x) batterySettings.SensorType[x] == FLIGHTBATTERYSETTINGS_SENSORTYPE_ENABLED +static void onTimer(__attribute__((unused)) UAVObjEvent *ev) { - static FlightBatteryStateData flightBatteryData; - FlightBatterySettingsData batterySettings; + static FlightBatteryStateData flightBatteryData; + FlightBatterySettingsData batterySettings; - FlightBatterySettingsGet(&batterySettings); + FlightBatterySettingsGet(&batterySettings); - static float dT = SAMPLE_PERIOD_MS / 1000.0f; - float energyRemaining; + static float dT = SAMPLE_PERIOD_MS / 1000.0f; + float energyRemaining; - //calculate the battery parameters - if (voltageADCPin >=0) { - flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(voltageADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts - } - else { - flightBatteryData.Voltage=1234; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC - } + // calculate the battery parameters + if (voltageADCPin >= 0) { + flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(voltageADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; // in Volts + } else { + flightBatteryData.Voltage = 1234; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC + } - if (currentADCPin >=0) { - flightBatteryData.Current = ((float)PIOS_ADC_PinGet(currentADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; //in Amps - if (flightBatteryData.Current > flightBatteryData.PeakCurrent) - flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps - } - else { //If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm - flightBatteryData.Current=-0.1234f; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC - } - - flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f) ;//in mAh - - //Apply a 2 second rise time low-pass filter to average the current - float alpha = 1.0f-dT/(dT+2.0f); - flightBatteryData.AvgCurrent=alpha*flightBatteryData.AvgCurrent+(1-alpha)*flightBatteryData.Current; //in Amps + if (currentADCPin >= 0) { + flightBatteryData.Current = ((float)PIOS_ADC_PinGet(currentADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; // in Amps + if (flightBatteryData.Current > flightBatteryData.PeakCurrent) { + flightBatteryData.PeakCurrent = flightBatteryData.Current; // in Amps + } + } else { // If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm + flightBatteryData.Current = -0.1234f; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC + } - /*The motor could regenerate power. Or we could have solar cells. - In short, is there any likelihood of measuring negative current? If it's a bad current reading we want to check, then - it makes sense to saturate at max and min values, because a misreading could as easily be very large, as negative. The simple - sign check doesn't catch this.*/ -// //sanity checks -// if (flightBatteryData.AvgCurrent<0) flightBatteryData.AvgCurrent=0.0f; -// if (flightBatteryData.PeakCurrent<0) flightBatteryData.PeakCurrent=0.0f; -// if (flightBatteryData.ConsumedEnergy<0) flightBatteryData.ConsumedEnergy=0.0f; + flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f); // in mAh - energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh - if (flightBatteryData.AvgCurrent > 0) - flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent*1000.0f))*3600.0f;//in Sec - else - flightBatteryData.EstimatedFlightTime = 9999; + // Apply a 2 second rise time low-pass filter to average the current + float alpha = 1.0f - dT / (dT + 2.0f); + flightBatteryData.AvgCurrent = alpha * flightBatteryData.AvgCurrent + (1 - alpha) * flightBatteryData.Current; // in Amps - //generate alarms where needed... - if ((flightBatteryData.Voltage<=0) && (flightBatteryData.Current<=0)) - { - //FIXME: There's no guarantee that a floating ADC will give 0. So this - // check might fail, even when there's nothing attached. - AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR); - AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR); - } - else - { - // FIXME: should make the timer alarms user configurable - if (flightBatteryData.EstimatedFlightTime < 30) - AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL); - else if (flightBatteryData.EstimatedFlightTime < 120) - AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING); - else - AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME); + /*The motor could regenerate power. Or we could have solar cells. + In short, is there any likelihood of measuring negative current? If it's a bad current reading we want to check, then + it makes sense to saturate at max and min values, because a misreading could as easily be very large, as negative. The simple + sign check doesn't catch this.*/ +////sanity checks +// if (flightBatteryData.AvgCurrent<0) flightBatteryData.AvgCurrent=0.0f; +// if (flightBatteryData.PeakCurrent<0) flightBatteryData.PeakCurrent=0.0f; +// if (flightBatteryData.ConsumedEnergy<0) flightBatteryData.ConsumedEnergy=0.0f; - // FIXME: should make the battery voltage detection dependent on battery type. - /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/ - if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM]) - AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); - else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING]) - AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); - else - AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); - } - - FlightBatteryStateSet(&flightBatteryData); + energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh + if (flightBatteryData.AvgCurrent > 0) { + flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent * 1000.0f)) * 3600.0f; // in Sec + } else { + flightBatteryData.EstimatedFlightTime = 9999; + } + + // generate alarms where needed... + if ((flightBatteryData.Voltage <= 0) && (flightBatteryData.Current <= 0)) { + // FIXME: There's no guarantee that a floating ADC will give 0. So this + // check might fail, even when there's nothing attached. + AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR); + } else { + // FIXME: should make the timer alarms user configurable + if (flightBatteryData.EstimatedFlightTime < 30) { + AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL); + } else if (flightBatteryData.EstimatedFlightTime < 120) { + AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME); + } + + // FIXME: should make the battery voltage detection dependent on battery type. + /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/ + if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM]) { + AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); + } else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING]) { + AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); + } + } + + FlightBatteryStateSet(&flightBatteryData); } -/** - * @} - */ - +/** + * @} + */ + /** * @} */ diff --git a/flight/modules/Battery/inc/battery.h b/flight/modules/Battery/inc/battery.h index bc2515696..206a9d92a 100644 --- a/flight/modules/Battery/inc/battery.h +++ b/flight/modules/Battery/inc/battery.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup BatteryModule Battery Module - * @{ + * @{ * * @file battery.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -37,6 +37,6 @@ int32_t BatteryInitialize(void); #endif // BATTERY_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/CallbackTest/callbacktest.c b/flight/modules/CallbackTest/callbacktest.c index 1f5a63b83..0b4ccf474 100644 --- a/flight/modules/CallbackTest/callbacktest.c +++ b/flight/modules/CallbackTest/callbacktest.c @@ -35,20 +35,20 @@ #include "openpilot.h" // Private constants -#define STACK_SIZE configMINIMAL_STACK_SIZE +#define STACK_SIZE configMINIMAL_STACK_SIZE #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW -#define TASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY +#define TASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY // Private types -//#define DEBUGPRINT(...) fprintf (stderr, __VA_ARGS__) -#define DEBUGPRINT(...) xSemaphoreTakeRecursive(mutex, portMAX_DELAY); fprintf(stderr, __VA_ARGS__ ); xSemaphoreGiveRecursive(mutex); +// #define DEBUGPRINT(...) fprintf (stderr, __VA_ARGS__) +#define DEBUGPRINT(...) xSemaphoreTakeRecursive(mutex, portMAX_DELAY); fprintf(stderr, __VA_ARGS__); xSemaphoreGiveRecursive(mutex); static xSemaphoreHandle mutex; // Private variables static DelayedCallbackInfo *cbinfo[10]; -static volatile int32_t counter[10]={0}; +static volatile int32_t counter[10] = { 0 }; static void DelayedCb0(); static void DelayedCb1(); @@ -63,70 +63,88 @@ static void DelayedCb6(); */ int32_t CallbackTestInitialize() { + mutex = xSemaphoreCreateRecursiveMutex(); - mutex = xSemaphoreCreateRecursiveMutex(); + cbinfo[0] = DelayedCallbackCreate(&DelayedCb0, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE); + cbinfo[1] = DelayedCallbackCreate(&DelayedCb1, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE); + cbinfo[2] = DelayedCallbackCreate(&DelayedCb2, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, STACK_SIZE); + cbinfo[3] = DelayedCallbackCreate(&DelayedCb3, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, STACK_SIZE); + cbinfo[4] = DelayedCallbackCreate(&DelayedCb4, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE); + cbinfo[5] = DelayedCallbackCreate(&DelayedCb5, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE); + cbinfo[6] = DelayedCallbackCreate(&DelayedCb6, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 20, STACK_SIZE); - cbinfo[0] = DelayedCallbackCreate(&DelayedCb0,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE); - cbinfo[1] = DelayedCallbackCreate(&DelayedCb1,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE); - cbinfo[2] = DelayedCallbackCreate(&DelayedCb2,CALLBACK_PRIORITY_CRITICAL,tskIDLE_PRIORITY+2,STACK_SIZE); - cbinfo[3] = DelayedCallbackCreate(&DelayedCb3,CALLBACK_PRIORITY_CRITICAL,tskIDLE_PRIORITY+2,STACK_SIZE); - cbinfo[4] = DelayedCallbackCreate(&DelayedCb4,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE); - cbinfo[5] = DelayedCallbackCreate(&DelayedCb5,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE); - cbinfo[6] = DelayedCallbackCreate(&DelayedCb6,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+20,STACK_SIZE); - - return 0; + return 0; } -int32_t CallbackTestStart() { - - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - DelayedCallbackDispatch(cbinfo[3]); - DelayedCallbackDispatch(cbinfo[2]); - DelayedCallbackDispatch(cbinfo[1]); - DelayedCallbackDispatch(cbinfo[0]); - // different callback priorities within a taskpriority - DelayedCallbackSchedule(cbinfo[4],30000,CALLBACK_UPDATEMODE_NONE); - DelayedCallbackSchedule(cbinfo[4],5000,CALLBACK_UPDATEMODE_OVERRIDE); - DelayedCallbackSchedule(cbinfo[4],4000,CALLBACK_UPDATEMODE_SOONER); - DelayedCallbackSchedule(cbinfo[4],10000,CALLBACK_UPDATEMODE_SOONER); - DelayedCallbackSchedule(cbinfo[4],1000,CALLBACK_UPDATEMODE_LATER); - DelayedCallbackSchedule(cbinfo[4],4800,CALLBACK_UPDATEMODE_LATER); - DelayedCallbackSchedule(cbinfo[4],48000,CALLBACK_UPDATEMODE_NONE); - // should be at 4.8 seconds after this, allowing for exactly 9 prints of the following - DelayedCallbackSchedule(cbinfo[5],500,CALLBACK_UPDATEMODE_NONE); - // delayed counter with 500 ms - DelayedCallbackDispatch(cbinfo[6]); - // high task prio - xSemaphoreGiveRecursive(mutex); - return 0; +int32_t CallbackTestStart() +{ + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + DelayedCallbackDispatch(cbinfo[3]); + DelayedCallbackDispatch(cbinfo[2]); + DelayedCallbackDispatch(cbinfo[1]); + DelayedCallbackDispatch(cbinfo[0]); + // different callback priorities within a taskpriority + DelayedCallbackSchedule(cbinfo[4], 30000, CALLBACK_UPDATEMODE_NONE); + DelayedCallbackSchedule(cbinfo[4], 5000, CALLBACK_UPDATEMODE_OVERRIDE); + DelayedCallbackSchedule(cbinfo[4], 4000, CALLBACK_UPDATEMODE_SOONER); + DelayedCallbackSchedule(cbinfo[4], 10000, CALLBACK_UPDATEMODE_SOONER); + DelayedCallbackSchedule(cbinfo[4], 1000, CALLBACK_UPDATEMODE_LATER); + DelayedCallbackSchedule(cbinfo[4], 4800, CALLBACK_UPDATEMODE_LATER); + DelayedCallbackSchedule(cbinfo[4], 48000, CALLBACK_UPDATEMODE_NONE); + // should be at 4.8 seconds after this, allowing for exactly 9 prints of the following + DelayedCallbackSchedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE); + // delayed counter with 500 ms + DelayedCallbackDispatch(cbinfo[6]); + // high task prio + xSemaphoreGiveRecursive(mutex); + return 0; } -static void DelayedCb0() { - DEBUGPRINT("delayed counter low prio 0 updated: %i\n",counter[0]); - if (++counter[0]<10) DelayedCallbackDispatch(cbinfo[0]); +static void DelayedCb0() +{ + DEBUGPRINT("delayed counter low prio 0 updated: %i\n", counter[0]); + if (++counter[0] < 10) { + DelayedCallbackDispatch(cbinfo[0]); + } } -static void DelayedCb1() { - DEBUGPRINT("delayed counter low prio 1 updated: %i\n",counter[1]); - if (++counter[1]<10) DelayedCallbackDispatch(cbinfo[1]); +static void DelayedCb1() +{ + DEBUGPRINT("delayed counter low prio 1 updated: %i\n", counter[1]); + if (++counter[1] < 10) { + DelayedCallbackDispatch(cbinfo[1]); + } } -static void DelayedCb2() { - DEBUGPRINT("delayed counter high prio 2 updated: %i\n",counter[2]); - if (++counter[2]<10) DelayedCallbackDispatch(cbinfo[2]); +static void DelayedCb2() +{ + DEBUGPRINT("delayed counter high prio 2 updated: %i\n", counter[2]); + if (++counter[2] < 10) { + DelayedCallbackDispatch(cbinfo[2]); + } } -static void DelayedCb3() { - DEBUGPRINT("delayed counter high prio 3 updated: %i\n",counter[3]); - if (++counter[3]<10) DelayedCallbackDispatch(cbinfo[3]); +static void DelayedCb3() +{ + DEBUGPRINT("delayed counter high prio 3 updated: %i\n", counter[3]); + if (++counter[3] < 10) { + DelayedCallbackDispatch(cbinfo[3]); + } } -static void DelayedCb4() { - DEBUGPRINT("delayed scheduled callback 4 reached!\n"); - exit(0); +static void DelayedCb4() +{ + DEBUGPRINT("delayed scheduled callback 4 reached!\n"); + exit(0); } -static void DelayedCb5() { - DEBUGPRINT("delayed scheduled counter 5 updated: %i\n",counter[5]); - if (++counter[5]<10) DelayedCallbackSchedule(cbinfo[5],500,CALLBACK_UPDATEMODE_NONE); - // it will likely only reach 8 due to cb4 aborting the run +static void DelayedCb5() +{ + DEBUGPRINT("delayed scheduled counter 5 updated: %i\n", counter[5]); + if (++counter[5] < 10) { + DelayedCallbackSchedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE); + } + // it will likely only reach 8 due to cb4 aborting the run } -static void DelayedCb6() { - DEBUGPRINT("delayed counter 6 (high task prio) updated: %i\n",counter[6]); - if (++counter[6]<10) DelayedCallbackDispatch(cbinfo[6]); +static void DelayedCb6() +{ + DEBUGPRINT("delayed counter 6 (high task prio) updated: %i\n", counter[6]); + if (++counter[6] < 10) { + DelayedCallbackDispatch(cbinfo[6]); + } } diff --git a/flight/modules/CameraStab/camerastab.c b/flight/modules/CameraStab/camerastab.c index 48f7a308f..04350c033 100644 --- a/flight/modules/CameraStab/camerastab.c +++ b/flight/modules/CameraStab/camerastab.c @@ -56,29 +56,28 @@ // // Configuration // -#define SAMPLE_PERIOD_MS 10 +#define SAMPLE_PERIOD_MS 10 // Private types // Private variables static struct CameraStab_data { - portTickType lastSysTime; - float inputs[CAMERASTABSETTINGS_INPUT_NUMELEM]; + portTickType lastSysTime; + float inputs[CAMERASTABSETTINGS_INPUT_NUMELEM]; #ifdef USE_GIMBAL_LPF - float attitudeFiltered[CAMERASTABSETTINGS_INPUT_NUMELEM]; + float attitudeFiltered[CAMERASTABSETTINGS_INPUT_NUMELEM]; #endif #ifdef USE_GIMBAL_FF - float ffLastAttitude[CAMERASTABSETTINGS_INPUT_NUMELEM]; - float ffLastAttitudeFiltered[CAMERASTABSETTINGS_INPUT_NUMELEM]; - float ffFilterAccumulator[CAMERASTABSETTINGS_INPUT_NUMELEM]; + float ffLastAttitude[CAMERASTABSETTINGS_INPUT_NUMELEM]; + float ffLastAttitudeFiltered[CAMERASTABSETTINGS_INPUT_NUMELEM]; + float ffFilterAccumulator[CAMERASTABSETTINGS_INPUT_NUMELEM]; #endif - } *csd; // Private functions -static void attitudeUpdated(UAVObjEvent* ev); +static void attitudeUpdated(UAVObjEvent *ev); static float bound(float val, float limit); #ifdef USE_GIMBAL_FF @@ -92,253 +91,258 @@ static void applyFeedForward(uint8_t index, float dT, float *attitude, CameraSta */ int32_t CameraStabInitialize(void) { - bool cameraStabEnabled; + bool cameraStabEnabled; #ifdef MODULE_CAMERASTAB_BUILTIN - cameraStabEnabled = true; + cameraStabEnabled = true; #else - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsInitialize(); - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsInitialize(); + HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_CAMERASTAB] == HWSETTINGS_OPTIONALMODULES_ENABLED) - cameraStabEnabled = true; - else - cameraStabEnabled = false; + if (optionalModules[HWSETTINGS_OPTIONALMODULES_CAMERASTAB] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + cameraStabEnabled = true; + } else { + cameraStabEnabled = false; + } #endif - if (cameraStabEnabled) { + if (cameraStabEnabled) { + // allocate and initialize the static data storage only if module is enabled + csd = (struct CameraStab_data *)pvPortMalloc(sizeof(struct CameraStab_data)); + if (!csd) { + return -1; + } - // allocate and initialize the static data storage only if module is enabled - csd = (struct CameraStab_data *) pvPortMalloc(sizeof(struct CameraStab_data)); - if (!csd) - return -1; + // initialize camera state variables + memset(csd, 0, sizeof(struct CameraStab_data)); + csd->lastSysTime = xTaskGetTickCount(); - // initialize camera state variables - memset(csd, 0, sizeof(struct CameraStab_data)); - csd->lastSysTime = xTaskGetTickCount(); + AttitudeActualInitialize(); + CameraStabSettingsInitialize(); + CameraDesiredInitialize(); - AttitudeActualInitialize(); - CameraStabSettingsInitialize(); - CameraDesiredInitialize(); + UAVObjEvent ev = { + .obj = AttitudeActualHandle(), + .instId = 0, + .event = 0, + }; + EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS); - UAVObjEvent ev = { - .obj = AttitudeActualHandle(), - .instId = 0, - .event = 0, - }; - EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS); + return 0; + } - return 0; - } - - return -1; + return -1; } /* stub: module has no module thread */ int32_t CameraStabStart(void) { - return 0; + return 0; } MODULE_INITCALL(CameraStabInitialize, CameraStabStart) -static void attitudeUpdated(UAVObjEvent* ev) +static void attitudeUpdated(UAVObjEvent *ev) { - if (ev->obj != AttitudeActualHandle()) - return; + if (ev->obj != AttitudeActualHandle()) { + return; + } - AccessoryDesiredData accessory; + AccessoryDesiredData accessory; - CameraStabSettingsData cameraStab; - CameraStabSettingsGet(&cameraStab); + CameraStabSettingsData cameraStab; + CameraStabSettingsGet(&cameraStab); - // check how long since last update, time delta between calls in ms - portTickType thisSysTime = xTaskGetTickCount(); - float dT_millis = (thisSysTime > csd->lastSysTime) ? - (float)((thisSysTime - csd->lastSysTime) * portTICK_RATE_MS) : - (float)SAMPLE_PERIOD_MS; - csd->lastSysTime = thisSysTime; + // check how long since last update, time delta between calls in ms + portTickType thisSysTime = xTaskGetTickCount(); + float dT_millis = (thisSysTime > csd->lastSysTime) ? + (float)((thisSysTime - csd->lastSysTime) * portTICK_RATE_MS) : + (float)SAMPLE_PERIOD_MS; + csd->lastSysTime = thisSysTime; - // storage for elevon roll component before the pitch component has been generated - // we are guaranteed that the iteration order of i is roll pitch yaw - // that guarnteees this won't be used uninited, but the compiler doesn't know that - // so we init it or turn the warning/error off for each compiler - float elevon_roll = 0.0f; + // storage for elevon roll component before the pitch component has been generated + // we are guaranteed that the iteration order of i is roll pitch yaw + // that guarnteees this won't be used uninited, but the compiler doesn't know that + // so we init it or turn the warning/error off for each compiler + float elevon_roll = 0.0f; - // process axes - for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUT_NUMELEM; i++) { + // process axes + for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUT_NUMELEM; i++) { + // read and process control input + if (cameraStab.Input[i] != CAMERASTABSETTINGS_INPUT_NONE) { + if (AccessoryDesiredInstGet(cameraStab.Input[i] - CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { + float input_rate; + switch (cameraStab.StabilizationMode[i]) { + case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE: + csd->inputs[i] = accessory.AccessoryVal * cameraStab.InputRange[i]; + break; + case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK: + input_rate = accessory.AccessoryVal * cameraStab.InputRate[i]; + if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) { + csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis, cameraStab.InputRange[i]); + } + break; + default: + PIOS_Assert(0); + } + } + } - // read and process control input - if (cameraStab.Input[i] != CAMERASTABSETTINGS_INPUT_NONE) { - if (AccessoryDesiredInstGet(cameraStab.Input[i] - CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { - float input_rate; - switch (cameraStab.StabilizationMode[i]) { - case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE: - csd->inputs[i] = accessory.AccessoryVal * cameraStab.InputRange[i]; - break; - case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK: - input_rate = accessory.AccessoryVal * cameraStab.InputRate[i]; - if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) - csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis, cameraStab.InputRange[i]); - break; - default: - PIOS_Assert(0); - } - } - } + // calculate servo output + float attitude; - // calculate servo output - float attitude; - - switch (i) { - case CAMERASTABSETTINGS_INPUT_ROLL: - AttitudeActualRollGet(&attitude); - break; - case CAMERASTABSETTINGS_INPUT_PITCH: - AttitudeActualPitchGet(&attitude); - break; - case CAMERASTABSETTINGS_INPUT_YAW: - AttitudeActualYawGet(&attitude); - break; - default: - PIOS_Assert(0); - } + switch (i) { + case CAMERASTABSETTINGS_INPUT_ROLL: + AttitudeActualRollGet(&attitude); + break; + case CAMERASTABSETTINGS_INPUT_PITCH: + AttitudeActualPitchGet(&attitude); + break; + case CAMERASTABSETTINGS_INPUT_YAW: + AttitudeActualYawGet(&attitude); + break; + default: + PIOS_Assert(0); + } #ifdef USE_GIMBAL_LPF - if (cameraStab.ResponseTime) { - float rt = (float)cameraStab.ResponseTime[i]; - attitude = csd->attitudeFiltered[i] = ((rt * csd->attitudeFiltered[i]) + (dT_millis * attitude)) / (rt + dT_millis); - } + if (cameraStab.ResponseTime) { + float rt = (float)cameraStab.ResponseTime[i]; + attitude = csd->attitudeFiltered[i] = ((rt * csd->attitudeFiltered[i]) + (dT_millis * attitude)) / (rt + dT_millis); + } #endif #ifdef USE_GIMBAL_FF - if (cameraStab.FeedForward[i]) - applyFeedForward(i, dT_millis, &attitude, &cameraStab); + if (cameraStab.FeedForward[i]) { + applyFeedForward(i, dT_millis, &attitude, &cameraStab); + } #endif - // bounding for elevon mixing occurs on the unmixed output - // to limit the range of the mixed output you must limit the range - // of both the unmixed pitch and unmixed roll - float output = bound((attitude + csd->inputs[i]) / cameraStab.OutputRange[i], 1.0f); + // bounding for elevon mixing occurs on the unmixed output + // to limit the range of the mixed output you must limit the range + // of both the unmixed pitch and unmixed roll + float output = bound((attitude + csd->inputs[i]) / cameraStab.OutputRange[i], 1.0f); - // set output channels - switch (i) { - case CAMERASTABSETTINGS_INPUT_ROLL: - // we are guaranteed that the iteration order of i is roll pitch yaw - // for elevon mixing we simply grab the value for later use - if (cameraStab.GimbalType == CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED) - elevon_roll = output; - else - CameraDesiredRollOrServo1Set(&output); - break; - case CAMERASTABSETTINGS_INPUT_PITCH: - // we are guaranteed that the iteration order of i is roll pitch yaw - // for elevon mixing we use the value we previously grabbed and set both s1 and s2 - if (cameraStab.GimbalType == CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED) { - float elevon_pitch = output; - // elevon reversing works like this: - // first use the normal reversing facilities to get servo 1 roll working in the correct direction - // then use the normal reversing facilities to get servo 2 roll working in the correct direction - // then use these new reversing switches to reverse servo 1 and/or 2 pitch as needed - // if servo 1 pitch is reversed - if (cameraStab.Servo1PitchReverse == CAMERASTABSETTINGS_SERVO1PITCHREVERSE_TRUE) { - // use (reversed pitch) + roll - output = ((1.0f - elevon_pitch) + elevon_roll) / 2.0f; - } else { - // use pitch + roll - output = (elevon_pitch + elevon_roll) / 2.0f; - } - CameraDesiredRollOrServo1Set(&output); - // if servo 2 pitch is reversed - if (cameraStab.Servo2PitchReverse == CAMERASTABSETTINGS_SERVO2PITCHREVERSE_TRUE) { - // use (reversed pitch) - roll - output = ((1.0f - elevon_pitch) - elevon_roll) / 2.0f; - } else { - // use pitch - roll - output = (elevon_pitch - elevon_roll) / 2.0f; - } - CameraDesiredPitchOrServo2Set(&output); - } else { - CameraDesiredPitchOrServo2Set(&output); - } - break; - case CAMERASTABSETTINGS_INPUT_YAW: - CameraDesiredYawSet(&output); - break; - default: - PIOS_Assert(0); - } - } + // set output channels + switch (i) { + case CAMERASTABSETTINGS_INPUT_ROLL: + // we are guaranteed that the iteration order of i is roll pitch yaw + // for elevon mixing we simply grab the value for later use + if (cameraStab.GimbalType == CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED) { + elevon_roll = output; + } else { + CameraDesiredRollOrServo1Set(&output); + } + break; + case CAMERASTABSETTINGS_INPUT_PITCH: + // we are guaranteed that the iteration order of i is roll pitch yaw + // for elevon mixing we use the value we previously grabbed and set both s1 and s2 + if (cameraStab.GimbalType == CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED) { + float elevon_pitch = output; + // elevon reversing works like this: + // first use the normal reversing facilities to get servo 1 roll working in the correct direction + // then use the normal reversing facilities to get servo 2 roll working in the correct direction + // then use these new reversing switches to reverse servo 1 and/or 2 pitch as needed + // if servo 1 pitch is reversed + if (cameraStab.Servo1PitchReverse == CAMERASTABSETTINGS_SERVO1PITCHREVERSE_TRUE) { + // use (reversed pitch) + roll + output = ((1.0f - elevon_pitch) + elevon_roll) / 2.0f; + } else { + // use pitch + roll + output = (elevon_pitch + elevon_roll) / 2.0f; + } + CameraDesiredRollOrServo1Set(&output); + // if servo 2 pitch is reversed + if (cameraStab.Servo2PitchReverse == CAMERASTABSETTINGS_SERVO2PITCHREVERSE_TRUE) { + // use (reversed pitch) - roll + output = ((1.0f - elevon_pitch) - elevon_roll) / 2.0f; + } else { + // use pitch - roll + output = (elevon_pitch - elevon_roll) / 2.0f; + } + CameraDesiredPitchOrServo2Set(&output); + } else { + CameraDesiredPitchOrServo2Set(&output); + } + break; + case CAMERASTABSETTINGS_INPUT_YAW: + CameraDesiredYawSet(&output); + break; + default: + PIOS_Assert(0); + } + } } float bound(float val, float limit) { - return (val > limit) ? limit : - (val < -limit) ? -limit : - val; + return (val > limit) ? limit : + (val < -limit) ? -limit : + val; } #ifdef USE_GIMBAL_FF void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraStabSettingsData *cameraStab) { - // compensate high feed forward values depending on gimbal type - float gimbalTypeCorrection = 1.0f; + // compensate high feed forward values depending on gimbal type + float gimbalTypeCorrection = 1.0f; - switch (cameraStab->GimbalType) { - case CAMERASTABSETTINGS_GIMBALTYPE_GENERIC: - case CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED: - // no correction - break; - case CAMERASTABSETTINGS_GIMBALTYPE_YAWROLLPITCH: - if (index == CAMERASTABSETTINGS_INPUT_ROLL) { - float pitch; - AttitudeActualPitchGet(&pitch); - gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabsf(pitch)) - / cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]; - } - break; - case CAMERASTABSETTINGS_GIMBALTYPE_YAWPITCHROLL: - if (index == CAMERASTABSETTINGS_INPUT_PITCH) { - float roll; - AttitudeActualRollGet(&roll); - gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabsf(roll)) - / cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]; - } - break; - default: - PIOS_Assert(0); - } + switch (cameraStab->GimbalType) { + case CAMERASTABSETTINGS_GIMBALTYPE_GENERIC: + case CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED: + // no correction + break; + case CAMERASTABSETTINGS_GIMBALTYPE_YAWROLLPITCH: + if (index == CAMERASTABSETTINGS_INPUT_ROLL) { + float pitch; + AttitudeActualPitchGet(&pitch); + gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabsf(pitch)) + / cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]; + } + break; + case CAMERASTABSETTINGS_GIMBALTYPE_YAWPITCHROLL: + if (index == CAMERASTABSETTINGS_INPUT_PITCH) { + float roll; + AttitudeActualRollGet(&roll); + gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabsf(roll)) + / cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]; + } + break; + default: + PIOS_Assert(0); + } - // apply feed forward - float accumulator = csd->ffFilterAccumulator[index]; - accumulator += (*attitude - csd->ffLastAttitude[index]) * (float)cameraStab->FeedForward[index] * gimbalTypeCorrection; - csd->ffLastAttitude[index] = *attitude; - *attitude += accumulator; + // apply feed forward + float accumulator = csd->ffFilterAccumulator[index]; + accumulator += (*attitude - csd->ffLastAttitude[index]) * (float)cameraStab->FeedForward[index] * gimbalTypeCorrection; + csd->ffLastAttitude[index] = *attitude; + *attitude += accumulator; - float filter = (float)((accumulator > 0.0f) ? cameraStab->AccelTime[index] : cameraStab->DecelTime[index]) / dT_millis; - if (filter < 1.0f) - filter = 1.0f; - accumulator -= accumulator / filter; - csd->ffFilterAccumulator[index] = accumulator; - *attitude += accumulator; + float filter = (float)((accumulator > 0.0f) ? cameraStab->AccelTime[index] : cameraStab->DecelTime[index]) / dT_millis; + if (filter < 1.0f) { + filter = 1.0f; + } + accumulator -= accumulator / filter; + csd->ffFilterAccumulator[index] = accumulator; + *attitude += accumulator; - // apply acceleration limit - float delta = *attitude - csd->ffLastAttitudeFiltered[index]; - float maxDelta = (float)cameraStab->MaxAccel * 0.001f * dT_millis; + // apply acceleration limit + float delta = *attitude - csd->ffLastAttitudeFiltered[index]; + float maxDelta = (float)cameraStab->MaxAccel * 0.001f * dT_millis; - if (fabsf(delta) > maxDelta) { - // we are accelerating too hard - *attitude = csd->ffLastAttitudeFiltered[index] + ((delta > 0.0f) ? maxDelta : -maxDelta); - } - csd->ffLastAttitudeFiltered[index] = *attitude; + if (fabsf(delta) > maxDelta) { + // we are accelerating too hard + *attitude = csd->ffLastAttitudeFiltered[index] + ((delta > 0.0f) ? maxDelta : -maxDelta); + } + csd->ffLastAttitudeFiltered[index] = *attitude; } #endif // USE_GIMBAL_FF /** - * @} - */ + * @} + */ /** * @} diff --git a/flight/modules/CameraStab/inc/camerastab.h b/flight/modules/CameraStab/inc/camerastab.h index 5d7bf9d06..fc3614044 100644 --- a/flight/modules/CameraStab/inc/camerastab.h +++ b/flight/modules/CameraStab/inc/camerastab.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup BatteryModule Battery Module - * @{ + * @{ * * @file battery.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -37,6 +37,6 @@ int32_t CameraStabInitialize(void); #endif // BATTERY_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/ComUsbBridge/ComUsbBridge.c b/flight/modules/ComUsbBridge/ComUsbBridge.c index 84c4e5ec7..09425e008 100644 --- a/flight/modules/ComUsbBridge/ComUsbBridge.c +++ b/flight/modules/ComUsbBridge/ComUsbBridge.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup ComUsbBridgeModule Com Port to USB VCP Bridge Module * @brief Bridge Com and USB VCP ports - * @{ + * @{ * * @file ComUsbBridge.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. @@ -47,10 +47,10 @@ static void updateSettings(); // **************** // Private constants -#define STACK_SIZE_BYTES 280 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define STACK_SIZE_BYTES 280 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define BRIDGE_BUF_LEN 10 +#define BRIDGE_BUF_LEN 10 // **************** // Private variables @@ -58,8 +58,8 @@ static void updateSettings(); static xTaskHandle com2UsbBridgeTaskHandle; static xTaskHandle usb2ComBridgeTaskHandle; -static uint8_t * com2usb_buf; -static uint8_t * usb2com_buf; +static uint8_t *com2usb_buf; +static uint8_t *usb2com_buf; static uint32_t usart_port; static uint32_t vcp_port; @@ -74,16 +74,16 @@ static bool bridge_enabled = false; static int32_t comUsbBridgeStart(void) { - if (bridge_enabled) { - // Start tasks - xTaskCreate(com2UsbBridgeTask, (signed char *)"Com2UsbBridge", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &com2UsbBridgeTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_COM2USBBRIDGE, com2UsbBridgeTaskHandle); - xTaskCreate(usb2ComBridgeTask, (signed char *)"Usb2ComBridge", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &usb2ComBridgeTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_USB2COMBRIDGE, usb2ComBridgeTaskHandle); - return 0; - } + if (bridge_enabled) { + // Start tasks + xTaskCreate(com2UsbBridgeTask, (signed char *)"Com2UsbBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &com2UsbBridgeTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_COM2USBBRIDGE, com2UsbBridgeTaskHandle); + xTaskCreate(usb2ComBridgeTask, (signed char *)"Usb2ComBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &usb2ComBridgeTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_USB2COMBRIDGE, usb2ComBridgeTaskHandle); + return 0; + } - return -1; + return -1; } /** * Initialise the module @@ -92,35 +92,36 @@ static int32_t comUsbBridgeStart(void) */ static int32_t comUsbBridgeInitialize(void) { - // TODO: Get from settings object - usart_port = PIOS_COM_BRIDGE; - vcp_port = PIOS_COM_VCP; + // TODO: Get from settings object + usart_port = PIOS_COM_BRIDGE; + vcp_port = PIOS_COM_VCP; #ifdef MODULE_COMUSBBRIDGE_BUILTIN - bridge_enabled = true; + bridge_enabled = true; #else - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(optionalModules); - if (usart_port && vcp_port && - (optionalModules[HWSETTINGS_OPTIONALMODULES_COMUSBBRIDGE] == HWSETTINGS_OPTIONALMODULES_ENABLED)) - bridge_enabled = true; - else - bridge_enabled = false; + if (usart_port && vcp_port && + (optionalModules[HWSETTINGS_OPTIONALMODULES_COMUSBBRIDGE] == HWSETTINGS_OPTIONALMODULES_ENABLED)) { + bridge_enabled = true; + } else { + bridge_enabled = false; + } #endif - if (bridge_enabled) { - com2usb_buf = pvPortMalloc(BRIDGE_BUF_LEN); - PIOS_Assert(com2usb_buf); - usb2com_buf = pvPortMalloc(BRIDGE_BUF_LEN); - PIOS_Assert(usb2com_buf); + if (bridge_enabled) { + com2usb_buf = pvPortMalloc(BRIDGE_BUF_LEN); + PIOS_Assert(com2usb_buf); + usb2com_buf = pvPortMalloc(BRIDGE_BUF_LEN); + PIOS_Assert(usb2com_buf); - updateSettings(); - } + updateSettings(); + } - return 0; + return 0; } MODULE_INITCALL(comUsbBridgeInitialize, comUsbBridgeStart) @@ -130,72 +131,73 @@ MODULE_INITCALL(comUsbBridgeInitialize, comUsbBridgeStart) static void com2UsbBridgeTask(__attribute__((unused)) void *parameters) { - /* Handle usart -> vcp direction */ - volatile uint32_t tx_errors = 0; - while (1) { - uint32_t rx_bytes; + /* Handle usart -> vcp direction */ + volatile uint32_t tx_errors = 0; - rx_bytes = PIOS_COM_ReceiveBuffer(usart_port, com2usb_buf, BRIDGE_BUF_LEN, 500); - if (rx_bytes > 0) { - /* Bytes available to transfer */ - if (PIOS_COM_SendBuffer(vcp_port, com2usb_buf, rx_bytes) != (int32_t)rx_bytes) { - /* Error on transmit */ - tx_errors++; - } - } - } + while (1) { + uint32_t rx_bytes; + + rx_bytes = PIOS_COM_ReceiveBuffer(usart_port, com2usb_buf, BRIDGE_BUF_LEN, 500); + if (rx_bytes > 0) { + /* Bytes available to transfer */ + if (PIOS_COM_SendBuffer(vcp_port, com2usb_buf, rx_bytes) != (int32_t)rx_bytes) { + /* Error on transmit */ + tx_errors++; + } + } + } } -static void usb2ComBridgeTask(__attribute__((unused)) void * parameters) +static void usb2ComBridgeTask(__attribute__((unused)) void *parameters) { - /* Handle vcp -> usart direction */ - volatile uint32_t tx_errors = 0; - while (1) { - uint32_t rx_bytes; + /* Handle vcp -> usart direction */ + volatile uint32_t tx_errors = 0; - rx_bytes = PIOS_COM_ReceiveBuffer(vcp_port, usb2com_buf, BRIDGE_BUF_LEN, 500); - if (rx_bytes > 0) { - /* Bytes available to transfer */ - if (PIOS_COM_SendBuffer(usart_port, usb2com_buf, rx_bytes) != (int32_t)rx_bytes) { - /* Error on transmit */ - tx_errors++; - } - } - } + while (1) { + uint32_t rx_bytes; + + rx_bytes = PIOS_COM_ReceiveBuffer(vcp_port, usb2com_buf, BRIDGE_BUF_LEN, 500); + if (rx_bytes > 0) { + /* Bytes available to transfer */ + if (PIOS_COM_SendBuffer(usart_port, usb2com_buf, rx_bytes) != (int32_t)rx_bytes) { + /* Error on transmit */ + tx_errors++; + } + } + } } static void updateSettings() { - if (usart_port) { + if (usart_port) { + // Retrieve settings + uint8_t speed; + HwSettingsComUsbBridgeSpeedGet(&speed); - // Retrieve settings - uint8_t speed; - HwSettingsComUsbBridgeSpeedGet(&speed); - - // Set port speed - switch (speed) { - case HWSETTINGS_COMUSBBRIDGESPEED_2400: - PIOS_COM_ChangeBaud(usart_port, 2400); - break; - case HWSETTINGS_COMUSBBRIDGESPEED_4800: - PIOS_COM_ChangeBaud(usart_port, 4800); - break; - case HWSETTINGS_COMUSBBRIDGESPEED_9600: - PIOS_COM_ChangeBaud(usart_port, 9600); - break; - case HWSETTINGS_COMUSBBRIDGESPEED_19200: - PIOS_COM_ChangeBaud(usart_port, 19200); - break; - case HWSETTINGS_COMUSBBRIDGESPEED_38400: - PIOS_COM_ChangeBaud(usart_port, 38400); - break; - case HWSETTINGS_COMUSBBRIDGESPEED_57600: - PIOS_COM_ChangeBaud(usart_port, 57600); - break; - case HWSETTINGS_COMUSBBRIDGESPEED_115200: - PIOS_COM_ChangeBaud(usart_port, 115200); - break; - } - } + // Set port speed + switch (speed) { + case HWSETTINGS_COMUSBBRIDGESPEED_2400: + PIOS_COM_ChangeBaud(usart_port, 2400); + break; + case HWSETTINGS_COMUSBBRIDGESPEED_4800: + PIOS_COM_ChangeBaud(usart_port, 4800); + break; + case HWSETTINGS_COMUSBBRIDGESPEED_9600: + PIOS_COM_ChangeBaud(usart_port, 9600); + break; + case HWSETTINGS_COMUSBBRIDGESPEED_19200: + PIOS_COM_ChangeBaud(usart_port, 19200); + break; + case HWSETTINGS_COMUSBBRIDGESPEED_38400: + PIOS_COM_ChangeBaud(usart_port, 38400); + break; + case HWSETTINGS_COMUSBBRIDGESPEED_57600: + PIOS_COM_ChangeBaud(usart_port, 57600); + break; + case HWSETTINGS_COMUSBBRIDGESPEED_115200: + PIOS_COM_ChangeBaud(usart_port, 115200); + break; + } + } } diff --git a/flight/modules/Example/example.c b/flight/modules/Example/example.c index d50809415..13bcaf539 100644 --- a/flight/modules/Example/example.c +++ b/flight/modules/Example/example.c @@ -51,12 +51,12 @@ void ExampleStart(void) { - ExampleModPeriodicInitialize(); - ExampleModThreadInitialize(); + ExampleModPeriodicInitialize(); + ExampleModThreadInitialize(); } void ExampleInitialize(void) { - ExampleModEventInitialize(); + ExampleModEventInitialize(); } MODULE_INITCALL(ExampleInitialize, ExampleStart) diff --git a/flight/modules/Example/examplemodcallback.c b/flight/modules/Example/examplemodcallback.c index 6faba152f..349b80934 100644 --- a/flight/modules/Example/examplemodcallback.c +++ b/flight/modules/Example/examplemodcallback.c @@ -45,14 +45,14 @@ */ #include "openpilot.h" -#include "exampleobject1.h" // object the module will listen for updates (input) -#include "exampleobject2.h" // object the module will update (output) -#include "examplesettings.h" // object holding module settings (input) +#include "exampleobject1.h" // object the module will listen for updates (input) +#include "exampleobject2.h" // object the module will update (output) +#include "examplesettings.h" // object holding module settings (input) // Private constants -#define STACK_SIZE configMINIMAL_STACK_SIZE +#define STACK_SIZE configMINIMAL_STACK_SIZE #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW -#define CBTASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY +#define CBTASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY // Private types // Private variables @@ -68,12 +68,12 @@ static void DelayedCb(); */ int32_t ExampleModCallbackInitialize() { - // Listen for ExampleObject1 updates, connect a callback function - ExampleObject1ConnectCallback(&ObjectUpdatedCb); + // Listen for ExampleObject1 updates, connect a callback function + ExampleObject1ConnectCallback(&ObjectUpdatedCb); - cbinfo = DelayedCallbackCreate(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE); - - return 0; + cbinfo = DelayedCallbackCreate(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE); + + return 0; } /** @@ -85,7 +85,7 @@ int32_t ExampleModCallbackInitialize() */ static void ObjectUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - DelayedCallbackDispatch(cbinfo); + DelayedCallbackDispatch(cbinfo); } /** @@ -101,40 +101,39 @@ static void ObjectUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) * do not block regular threads. They are therefore saver to use. */ static void DelayedCb(); - ExampleSettingsData settings; - ExampleObject1Data data1; - ExampleObject2Data data2; - int32_t step; +ExampleSettingsData settings; +ExampleObject1Data data1; +ExampleObject2Data data2; +int32_t step; - // Update settings with latest value - ExampleSettingsGet(&settings); +// Update settings with latest value +ExampleSettingsGet(&settings); - // Get the input object - ExampleObject1Get(&data1); +// Get the input object +ExampleObject1Get(&data1); - // Determine how to update the output object - if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { - step = settings.StepSize; - } else { - step = -settings.StepSize; - } - - // Update data - data2.Field1 = data1.Field1 + step; - data2.Field2 = data1.Field2 + step; - data2.Field3 = data1.Field3 + step; - data2.Field4[0] = data1.Field4[0] + step; - data2.Field4[1] = data1.Field4[1] + step; - - // Update the ExampleObject2, after this function is called - // notifications to any other modules listening to that object - // will be sent and the GCS object will be updated through the - // telemetry link. All operations will take place asynchronously - // and the following call will return immediately. - ExampleObject2Set(&data2); - - //call the module again 10 seconds later, - //even if the exampleobject has not been updated - DelayedCallbackSchedule(cbinfo, 10*1000, CALLBACK_UPDATEMODE_NONE); - +// Determine how to update the output object +if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { + step = settings.StepSize; +} else { + step = -settings.StepSize; +} + +// Update data +data2.Field1 = data1.Field1 + step; +data2.Field2 = data1.Field2 + step; +data2.Field3 = data1.Field3 + step; +data2.Field4[0] = data1.Field4[0] + step; +data2.Field4[1] = data1.Field4[1] + step; + +// Update the ExampleObject2, after this function is called +// notifications to any other modules listening to that object +// will be sent and the GCS object will be updated through the +// telemetry link. All operations will take place asynchronously +// and the following call will return immediately. +ExampleObject2Set(&data2); + +// call the module again 10 seconds later, +// even if the exampleobject has not been updated +DelayedCallbackSchedule(cbinfo, 10 * 1000, CALLBACK_UPDATEMODE_NONE); } diff --git a/flight/modules/Example/examplemodevent.c b/flight/modules/Example/examplemodevent.c index 5af686abf..f38bf294c 100644 --- a/flight/modules/Example/examplemodevent.c +++ b/flight/modules/Example/examplemodevent.c @@ -46,9 +46,9 @@ #include "openpilot.h" #include "examplemodevent.h" -#include "exampleobject1.h" // object the module will listen for updates (input) -#include "exampleobject2.h" // object the module will update (output) -#include "examplesettings.h" // object holding module settings (input) +#include "exampleobject1.h" // object the module will listen for updates (input) +#include "exampleobject2.h" // object the module will update (output) +#include "examplesettings.h" // object holding module settings (input) // Private constants @@ -57,7 +57,7 @@ // Private variables // Private functions -static void ObjectUpdatedCb(UAVObjEvent * ev); +static void ObjectUpdatedCb(UAVObjEvent *ev); /** * Initialise the module, called on startup. @@ -65,10 +65,10 @@ static void ObjectUpdatedCb(UAVObjEvent * ev); */ int32_t ExampleModEventInitialize() { - // Listen for ExampleObject1 updates, connect a callback function - ExampleObject1ConnectCallback(&ObjectUpdatedCb); + // Listen for ExampleObject1 updates, connect a callback function + ExampleObject1ConnectCallback(&ObjectUpdatedCb); - return 0; + return 0; } /** @@ -81,43 +81,43 @@ int32_t ExampleModEventInitialize() * important since all callbacks are executed from the same thread, so other * queued events can not be executed until the currently active callback returns. */ -static void ObjectUpdatedCb(UAVObjEvent * ev) +static void ObjectUpdatedCb(UAVObjEvent *ev) { - ExampleSettingsData settings; - ExampleObject1Data data1; - ExampleObject2Data data2; - int32_t step; + ExampleSettingsData settings; + ExampleObject1Data data1; + ExampleObject2Data data2; + int32_t step; - // Make sure that the object update is for ExampleObject1 - // This is redundant in this case since this callback will - // only be called for a single object, it is however possible - // to use the same callback for multiple object updates. - if (ev->obj == ExampleObject1Handle()) { - // Update settings with latest value - ExampleSettingsGet(&settings); + // Make sure that the object update is for ExampleObject1 + // This is redundant in this case since this callback will + // only be called for a single object, it is however possible + // to use the same callback for multiple object updates. + if (ev->obj == ExampleObject1Handle()) { + // Update settings with latest value + ExampleSettingsGet(&settings); - // Get the input object - ExampleObject1Get(&data1); + // Get the input object + ExampleObject1Get(&data1); - // Determine how to update the output object - if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { - step = settings.StepSize; - } else { - step = -settings.StepSize; - } + // Determine how to update the output object + if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { + step = settings.StepSize; + } else { + step = -settings.StepSize; + } - // Update data - data2.Field1 = data1.Field1 + step; - data2.Field2 = data1.Field2 + step; - data2.Field3 = data1.Field3 + step; - data2.Field4[0] = data1.Field4[0] + step; - data2.Field4[1] = data1.Field4[1] + step; + // Update data + data2.Field1 = data1.Field1 + step; + data2.Field2 = data1.Field2 + step; + data2.Field3 = data1.Field3 + step; + data2.Field4[0] = data1.Field4[0] + step; + data2.Field4[1] = data1.Field4[1] + step; - // Update the ExampleObject2, after this function is called - // notifications to any other modules listening to that object - // will be sent and the GCS object will be updated through the - // telemetry link. All operations will take place asynchronously - // and the following call will return immediately. - ExampleObject2Set(&data2); - } + // Update the ExampleObject2, after this function is called + // notifications to any other modules listening to that object + // will be sent and the GCS object will be updated through the + // telemetry link. All operations will take place asynchronously + // and the following call will return immediately. + ExampleObject2Set(&data2); + } } diff --git a/flight/modules/Example/examplemodperiodic.c b/flight/modules/Example/examplemodperiodic.c index 8bfe98211..4a45c71c1 100644 --- a/flight/modules/Example/examplemodperiodic.c +++ b/flight/modules/Example/examplemodperiodic.c @@ -46,12 +46,12 @@ #include "openpilot.h" #include "examplemodperiodic.h" -#include "exampleobject2.h" // object that will be updated by the module -#include "examplesettings.h" // object holding module settings +#include "exampleobject2.h" // object that will be updated by the module +#include "examplesettings.h" // object holding module settings // Private constants -#define STACK_SIZE configMINIMAL_STACK_SIZE -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) +#define STACK_SIZE configMINIMAL_STACK_SIZE +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types @@ -67,10 +67,10 @@ static void exampleTask(void *parameters); */ int32_t ExampleModPeriodicInitialize() { - // Start main task - xTaskCreate(exampleTask, (signed char *)"ExamplePeriodic", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); + // Start main task + xTaskCreate(exampleTask, (signed char *)"ExamplePeriodic", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); - return 0; + return 0; } /** @@ -78,45 +78,45 @@ int32_t ExampleModPeriodicInitialize() */ static void exampleTask(__attribute__((unused)) void *parameters) { - ExampleSettingsData settings; - ExampleObject2Data data; - int32_t step; - portTickType lastSysTime; + ExampleSettingsData settings; + ExampleObject2Data data; + int32_t step; + portTickType lastSysTime; - // Main task loop - lastSysTime = xTaskGetTickCount(); - while (1) { - // Update settings with latest value - ExampleSettingsGet(&settings); + // Main task loop + lastSysTime = xTaskGetTickCount(); + while (1) { + // Update settings with latest value + ExampleSettingsGet(&settings); - // Get the object data - ExampleObject2Get(&data); + // Get the object data + ExampleObject2Get(&data); - // Determine how to update the data - if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { - step = settings.StepSize; - } else { - step = -settings.StepSize; - } + // Determine how to update the data + if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { + step = settings.StepSize; + } else { + step = -settings.StepSize; + } - // Update the data - data.Field1 += step; - data.Field2 += step; - data.Field3 += step; - data.Field4[0] += step; - data.Field4[1] += step; + // Update the data + data.Field1 += step; + data.Field2 += step; + data.Field3 += step; + data.Field4[0] += step; + data.Field4[1] += step; - // Update the ExampleObject, after this function is called - // notifications to any other modules listening to that object - // will be sent and the GCS object will be updated through the - // telemetry link. All operations will take place asynchronously - // and the following call will return immediately. - ExampleObject2Set(&data); + // Update the ExampleObject, after this function is called + // notifications to any other modules listening to that object + // will be sent and the GCS object will be updated through the + // telemetry link. All operations will take place asynchronously + // and the following call will return immediately. + ExampleObject2Set(&data); - // Since this module executes at fixed time intervals, we need to - // block the task until it is time for the next update. - // The settings field is in ms, to convert to RTOS ticks we need - // to divide by portTICK_RATE_MS. - vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS); - } + // Since this module executes at fixed time intervals, we need to + // block the task until it is time for the next update. + // The settings field is in ms, to convert to RTOS ticks we need + // to divide by portTICK_RATE_MS. + vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS); + } } diff --git a/flight/modules/Example/examplemodthread.c b/flight/modules/Example/examplemodthread.c index 813a437e4..d0f49dfd6 100644 --- a/flight/modules/Example/examplemodthread.c +++ b/flight/modules/Example/examplemodthread.c @@ -45,14 +45,14 @@ #include "openpilot.h" #include "examplemodthread.h" -#include "exampleobject1.h" // object the module will listen for updates (input) -#include "exampleobject2.h" // object the module will update (output) -#include "examplesettings.h" // object holding module settings (input) +#include "exampleobject1.h" // object the module will listen for updates (input) +#include "exampleobject2.h" // object the module will update (output) +#include "examplesettings.h" // object holding module settings (input) // Private constants #define MAX_QUEUE_SIZE 20 -#define STACK_SIZE configMINIMAL_STACK_SIZE -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) +#define STACK_SIZE configMINIMAL_STACK_SIZE +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types @@ -69,16 +69,16 @@ static void exampleTask(void *parameters); */ int32_t ExampleModThreadInitialize() { - // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + // Create object queue + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - // Listen for ExampleObject1 updates - ExampleObject1ConnectQueue(queue); + // Listen for ExampleObject1 updates + ExampleObject1ConnectQueue(queue); - // Start main task - xTaskCreate(exampleTask, (signed char *)"ExampleThread", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); + // Start main task + xTaskCreate(exampleTask, (signed char *)"ExampleThread", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); - return 0; + return 0; } /** @@ -86,53 +86,55 @@ int32_t ExampleModThreadInitialize() */ static void exampleTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; - ExampleSettingsData settings; - ExampleObject1Data data1; - ExampleObject1Data data2; - int32_t step; + UAVObjEvent ev; + ExampleSettingsData settings; + ExampleObject1Data data1; + ExampleObject1Data data2; + int32_t step; - // Main task loop - while (1) { - // Check the event queue for any object updates. In this case - // we are only listening for the ExampleSettings object. However - // the module could listen to multiple objects. - // Since this object only executes on object updates, the task - // will block until an object event is pushed in the queue. - while (xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE) ; + // Main task loop + while (1) { + // Check the event queue for any object updates. In this case + // we are only listening for the ExampleSettings object. However + // the module could listen to multiple objects. + // Since this object only executes on object updates, the task + // will block until an object event is pushed in the queue. + while (xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE) { + ; + } - // Make sure that the object update is for ExampleObject1 - // This is redundant in this example since we have only - // registered a single object in the queue, however - // in most practical cases there will be more than one - // object connected in the queue. - if (ev.obj == ExampleObject1Handle()) { - // Update settings with latest value - ExampleSettingsGet(&settings); + // Make sure that the object update is for ExampleObject1 + // This is redundant in this example since we have only + // registered a single object in the queue, however + // in most practical cases there will be more than one + // object connected in the queue. + if (ev.obj == ExampleObject1Handle()) { + // Update settings with latest value + ExampleSettingsGet(&settings); - // Get the input object - ExampleObject1Get(&data1); + // Get the input object + ExampleObject1Get(&data1); - // Determine how to update the output object - if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { - step = settings.StepSize; - } else { - step = -settings.StepSize; - } + // Determine how to update the output object + if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) { + step = settings.StepSize; + } else { + step = -settings.StepSize; + } - // Update data - data2.Field1 = data1.Field1 + step; - data2.Field2 = data1.Field2 + step; - data2.Field3 = data1.Field3 + step; - data2.Field4[0] = data1.Field4[0] + step; - data2.Field4[1] = data1.Field4[1] + step; + // Update data + data2.Field1 = data1.Field1 + step; + data2.Field2 = data1.Field2 + step; + data2.Field3 = data1.Field3 + step; + data2.Field4[0] = data1.Field4[0] + step; + data2.Field4[1] = data1.Field4[1] + step; - // Update the ExampleObject2, after this function is called - // notifications to any other modules listening to that object - // will be sent and the GCS object will be updated through the - // telemetry link. All operations will take place asynchronously - // and the following call will return immediately. - ExampleObject2Set(&data2); - } - } + // Update the ExampleObject2, after this function is called + // notifications to any other modules listening to that object + // will be sent and the GCS object will be updated through the + // telemetry link. All operations will take place asynchronously + // and the following call will return immediately. + ExampleObject2Set(&data2); + } + } } diff --git a/flight/modules/Example/inc/examplemodperiodic.h b/flight/modules/Example/inc/examplemodperiodic.h index 330d02ec1..8e877edca 100644 --- a/flight/modules/Example/inc/examplemodperiodic.h +++ b/flight/modules/Example/inc/examplemodperiodic.h @@ -29,4 +29,3 @@ int32_t ExampleModPeriodicInitialize(); #endif // EXAMPLEMODPERIODIC_H - diff --git a/flight/modules/Example/inc/examplemodthread.h b/flight/modules/Example/inc/examplemodthread.h index a3afba343..5cf4c0952 100644 --- a/flight/modules/Example/inc/examplemodthread.h +++ b/flight/modules/Example/inc/examplemodthread.h @@ -29,4 +29,3 @@ int32_t ExampleModThreadInitialize(); #endif // EXAMPLEMODTHREAD_H - diff --git a/flight/modules/Extensions/MagBaro/inc/magbaro.h b/flight/modules/Extensions/MagBaro/inc/magbaro.h index 14c8da6d5..ad495fb1a 100644 --- a/flight/modules/Extensions/MagBaro/inc/magbaro.h +++ b/flight/modules/Extensions/MagBaro/inc/magbaro.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AltitudeModule Altitude Module * @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object" - * @{ + * @{ * * @file altitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. diff --git a/flight/modules/Extensions/MagBaro/magbaro.c b/flight/modules/Extensions/MagBaro/magbaro.c index 81b8a722b..cc3835c54 100644 --- a/flight/modules/Extensions/MagBaro/magbaro.c +++ b/flight/modules/Extensions/MagBaro/magbaro.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup AltitudeModule Altitude Module * @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object" - * @{ + * @{ * * @file altitude.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -40,14 +40,14 @@ #include "hwsettings.h" #include "magbaro.h" -#include "baroaltitude.h" // object that will be updated by the module +#include "baroaltitude.h" // object that will be updated by the module #include "magnetometer.h" #include "taskinfo.h" // Private constants #define STACK_SIZE_BYTES 600 -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) -#define UPDATE_PERIOD 100 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define UPDATE_PERIOD 100 // Private types @@ -58,19 +58,19 @@ static xTaskHandle taskHandle; static bool magbaroEnabled; #if defined(PIOS_INCLUDE_BMP085) -#define alt_ds_size 4 +#define alt_ds_size 4 static int32_t alt_ds_temp = 0; static int32_t alt_ds_pres = 0; -static int alt_ds_count = 0; +static int alt_ds_count = 0; #endif #if defined(PIOS_INCLUDE_HMC5883) int32_t mag_test; -static float mag_bias[3] = {0,0,0}; -static float mag_scale[3] = {1,1,1}; +static float mag_bias[3] = { 0, 0, 0 }; +static float mag_scale[3] = { 1, 1, 1 }; #endif - // Private functions +// Private functions static void magbaroTask(void *parameters); /** @@ -79,14 +79,13 @@ static void magbaroTask(void *parameters); */ int32_t MagBaroStart() { - - if (magbaroEnabled) { - // Start main task - xTaskCreate(magbaroTask, (signed char *)"MagBaro", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MAGBARO, taskHandle); - return 0; - } - return -1; + if (magbaroEnabled) { + // Start main task + xTaskCreate(magbaroTask, (signed char *)"MagBaro", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MAGBARO, taskHandle); + return 0; + } + return -1; } /** @@ -96,34 +95,33 @@ int32_t MagBaroStart() int32_t MagBaroInitialize() { #ifdef MODULE_MAGBARO_BUILTIN - magbaroEnabled = 1; + magbaroEnabled = 1; #else - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_MAGBARO] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - magbaroEnabled = 1; - } else { - magbaroEnabled = 0; - } + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + if (optionalModules[HWSETTINGS_OPTIONALMODULES_MAGBARO] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + magbaroEnabled = 1; + } else { + magbaroEnabled = 0; + } #endif - if(magbaroEnabled) - { + if (magbaroEnabled) { #if defined(PIOS_INCLUDE_HMC5883) - MagnetometerInitialize(); + MagnetometerInitialize(); #endif #if defined(PIOS_INCLUDE_BMP085) - BaroAltitudeInitialize(); + BaroAltitudeInitialize(); - // init down-sampling data - alt_ds_temp = 0; - alt_ds_pres = 0; - alt_ds_count = 0; + // init down-sampling data + alt_ds_temp = 0; + alt_ds_pres = 0; + alt_ds_count = 0; #endif - } - return 0; + } + return 0; } MODULE_INITCALL(MagBaroInitialize, MagBaroStart) /** @@ -132,94 +130,91 @@ MODULE_INITCALL(MagBaroInitialize, MagBaroStart) #if defined(PIOS_INCLUDE_HMC5883) static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { #ifdef PIOS_HMC5883_HAS_GPIOS - .exti_cfg = 0, + .exti_cfg = 0, #endif - .M_ODR = PIOS_HMC5883_ODR_15, - .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, - .Gain = PIOS_HMC5883_GAIN_1_9, - .Mode = PIOS_HMC5883_MODE_CONTINUOUS, - + .M_ODR = PIOS_HMC5883_ODR_15, + .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, + .Gain = PIOS_HMC5883_GAIN_1_9, + .Mode = PIOS_HMC5883_MODE_CONTINUOUS, }; #endif static void magbaroTask(__attribute__((unused)) void *parameters) { - portTickType lastSysTime; - + portTickType lastSysTime; + #if defined(PIOS_INCLUDE_BMP085) - BaroAltitudeData data; - PIOS_BMP085_Init(); + BaroAltitudeData data; + PIOS_BMP085_Init(); #endif #if defined(PIOS_INCLUDE_HMC5883) - MagnetometerData mag; - PIOS_HMC5883_Init(&pios_hmc5883_cfg); - uint32_t mag_update_time = PIOS_DELAY_GetRaw(); + MagnetometerData mag; + PIOS_HMC5883_Init(&pios_hmc5883_cfg); + uint32_t mag_update_time = PIOS_DELAY_GetRaw(); #endif - // Main task loop - lastSysTime = xTaskGetTickCount(); - while (1) - { + // Main task loop + lastSysTime = xTaskGetTickCount(); + while (1) { #if defined(PIOS_INCLUDE_BMP085) - // Update the temperature data - PIOS_BMP085_StartADC(TemperatureConv); + // Update the temperature data + PIOS_BMP085_StartADC(TemperatureConv); #ifdef PIOS_BMP085_HAS_GPIOS - xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); + xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); #else - vTaskDelay(5 / portTICK_RATE_MS); + vTaskDelay(5 / portTICK_RATE_MS); #endif - PIOS_BMP085_ReadADC(); - alt_ds_temp += PIOS_BMP085_GetTemperature(); + PIOS_BMP085_ReadADC(); + alt_ds_temp += PIOS_BMP085_GetTemperature(); - // Update the pressure data - PIOS_BMP085_StartADC(PressureConv); + // Update the pressure data + PIOS_BMP085_StartADC(PressureConv); #ifdef PIOS_BMP085_HAS_GPIOS - xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); + xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); #else - vTaskDelay(26 / portTICK_RATE_MS); + vTaskDelay(26 / portTICK_RATE_MS); #endif - PIOS_BMP085_ReadADC(); - alt_ds_pres += PIOS_BMP085_GetPressure(); + PIOS_BMP085_ReadADC(); + alt_ds_pres += PIOS_BMP085_GetPressure(); - if (++alt_ds_count >= alt_ds_size) - { - alt_ds_count = 0; + if (++alt_ds_count >= alt_ds_size) { + alt_ds_count = 0; - // Convert from 1/10ths of degC to degC - data.Temperature = alt_ds_temp / (10.0f * alt_ds_size); - alt_ds_temp = 0; + // Convert from 1/10ths of degC to degC + data.Temperature = alt_ds_temp / (10.0f * alt_ds_size); + alt_ds_temp = 0; - // Convert from Pa to kPa - data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size); - alt_ds_pres = 0; + // Convert from Pa to kPa + data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size); + alt_ds_pres = 0; - // Compute the current altitude (all pressures in kPa) - data.Altitude = 44330.0f * (1.0f - powf((data.Pressure / (BMP085_P0 / 1000.0f)), (1.0f / 5.255f))); + // Compute the current altitude (all pressures in kPa) + data.Altitude = 44330.0f * (1.0f - powf((data.Pressure / (BMP085_P0 / 1000.0f)), (1.0f / 5.255f))); - // Update the AltitudeActual UAVObject - BaroAltitudeSet(&data); - } -#endif + // Update the AltitudeActual UAVObject + BaroAltitudeSet(&data); + } +#endif /* if defined(PIOS_INCLUDE_BMP085) */ #if defined(PIOS_INCLUDE_HMC5883) - if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) { - int16_t values[3]; - PIOS_HMC5883_ReadMag(values); - float mags[3] = {(float) values[1] * mag_scale[0] - mag_bias[0], - (float) values[0] * mag_scale[1] - mag_bias[1], - -(float) values[2] * mag_scale[2] - mag_bias[2]}; - mag.x = mags[0]; - mag.y = mags[1]; - mag.z = mags[2]; - MagnetometerSet(&mag); - mag_update_time = PIOS_DELAY_GetRaw(); - } + if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) { + int16_t values[3]; + PIOS_HMC5883_ReadMag(values); + float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0], + (float)values[0] * mag_scale[1] - mag_bias[1], + -(float)values[2] * mag_scale[2] - mag_bias[2] }; + mag.x = mags[0]; + mag.y = mags[1]; + mag.z = mags[2]; + MagnetometerSet(&mag); + mag_update_time = PIOS_DELAY_GetRaw(); + } #endif - // Delay until it is time to read the next sample - vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS); - } + // Delay until it is time to read the next sample + vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS); + } } /** diff --git a/flight/modules/Fault/Fault.c b/flight/modules/Fault/Fault.c index 33b15f7cb..86c052ce0 100644 --- a/flight/modules/Fault/Fault.c +++ b/flight/modules/Fault/Fault.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup FaultModule Fault Module * @brief Insert various fault conditions for testing - * @{ + * @{ * * @file Fault.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -41,95 +41,102 @@ static uint8_t active_fault; static int32_t fault_initialize(void) { #ifdef MODULE_FAULT_BUILTIN - module_enabled = true; + module_enabled = true; #else - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_FAULT] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - module_enabled = true; - } else { - module_enabled = false; - } + if (optionalModules[HWSETTINGS_OPTIONALMODULES_FAULT] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + module_enabled = true; + } else { + module_enabled = false; + } #endif - /* Do this outside the module_enabled test so that it - * can be changed even when the module has been disabled. - * This is important so we can remove faults even when - * we've booted in BootFault recovery mode with all optional - * modules disabled. - */ - FaultSettingsInitialize(); + /* Do this outside the module_enabled test so that it + * can be changed even when the module has been disabled. + * This is important so we can remove faults even when + * we've booted in BootFault recovery mode with all optional + * modules disabled. + */ + FaultSettingsInitialize(); - if (module_enabled) { - FaultSettingsActivateFaultGet(&active_fault); + if (module_enabled) { + FaultSettingsActivateFaultGet(&active_fault); - switch (active_fault) { - case FAULTSETTINGS_ACTIVATEFAULT_MODULEINITASSERT: - /* Simulate an assert during module init */ - PIOS_Assert(0); - break; - case FAULTSETTINGS_ACTIVATEFAULT_INITOUTOFMEMORY: - /* Leak all available memory */ - while (pvPortMalloc(10)) ; - break; - case FAULTSETTINGS_ACTIVATEFAULT_INITBUSERROR: - { - /* Force a bad access */ - uint32_t * bad_ptr = (uint32_t *)0xFFFFFFFF; - *bad_ptr = 0xAA55AA55; - } - break; - } - } + switch (active_fault) { + case FAULTSETTINGS_ACTIVATEFAULT_MODULEINITASSERT: + /* Simulate an assert during module init */ + PIOS_Assert(0); + break; + case FAULTSETTINGS_ACTIVATEFAULT_INITOUTOFMEMORY: + /* Leak all available memory */ + while (pvPortMalloc(10)) { + ; + } + break; + case FAULTSETTINGS_ACTIVATEFAULT_INITBUSERROR: + { + /* Force a bad access */ + uint32_t *bad_ptr = (uint32_t *)0xFFFFFFFF; + *bad_ptr = 0xAA55AA55; + } + break; + } + } - return 0; + return 0; } static void fault_task(void *parameters); static int32_t fault_start(void) { - xTaskHandle fault_task_handle; + xTaskHandle fault_task_handle; - if (module_enabled) { - switch (active_fault) { - case FAULTSETTINGS_ACTIVATEFAULT_RUNAWAYTASK: - case FAULTSETTINGS_ACTIVATEFAULT_TASKOUTOFMEMORY: - xTaskCreate(fault_task, - (signed char *)"Fault", - configMINIMAL_STACK_SIZE, - NULL, - configMAX_PRIORITIES-1, - &fault_task_handle); - return 0; - break; - } - } - return -1; + if (module_enabled) { + switch (active_fault) { + case FAULTSETTINGS_ACTIVATEFAULT_RUNAWAYTASK: + case FAULTSETTINGS_ACTIVATEFAULT_TASKOUTOFMEMORY: + xTaskCreate(fault_task, + (signed char *)"Fault", + configMINIMAL_STACK_SIZE, + NULL, + configMAX_PRIORITIES - 1, + &fault_task_handle); + return 0; + + break; + } + } + return -1; } MODULE_INITCALL(fault_initialize, fault_start) -static void fault_task(__attribute__((unused))void *parameters) +static void fault_task(__attribute__((unused)) void *parameters) { - switch (active_fault) { - case FAULTSETTINGS_ACTIVATEFAULT_RUNAWAYTASK: - /* Consume all realtime, not letting the systemtask run */ - while(1); - break; - case FAULTSETTINGS_ACTIVATEFAULT_TASKOUTOFMEMORY: - /* Leak all available memory and then sleep */ - while (pvPortMalloc(10)) ; - while (1) { - vTaskDelay(1000); - } - break; - } + switch (active_fault) { + case FAULTSETTINGS_ACTIVATEFAULT_RUNAWAYTASK: + /* Consume all realtime, not letting the systemtask run */ + while (1) { + ; + } + break; + case FAULTSETTINGS_ACTIVATEFAULT_TASKOUTOFMEMORY: + /* Leak all available memory and then sleep */ + while (pvPortMalloc(10)) { + ; + } + while (1) { + vTaskDelay(1000); + } + break; + } } -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/modules/FirmwareIAP/firmwareiap.c b/flight/modules/FirmwareIAP/firmwareiap.c index 992971b3c..553eef2c6 100644 --- a/flight/modules/FirmwareIAP/firmwareiap.c +++ b/flight/modules/FirmwareIAP/firmwareiap.c @@ -4,7 +4,7 @@ * @file firmwareiap.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief In Application Programming module to support firmware upgrades by - * providing a means to enter the bootloader. + * providing a means to enter the bootloader. * * @see The GNU Public License (GPL) Version 3 * @@ -40,7 +40,7 @@ #define IAP_CMD_CRC 100 #define IAP_CMD_VERIFY 101 -#define IAP_CMD_VERSION 102 +#define IAP_CMD_VERSION 102 #define IAP_STATE_READY 0 #define IAP_STATE_STEP_1 1 @@ -49,13 +49,13 @@ #define RESET_DELAY 500 /* delay between sending reset ot INS */ -#define TICKS2MS(t) ((t)/portTICK_RATE_MS) -#define MS2TICKS(m) ((m)*portTICK_RATE_MS) +#define TICKS2MS(t) ((t) / portTICK_RATE_MS) +#define MS2TICKS(m) ((m) * portTICK_RATE_MS) -const uint32_t iap_time_2_low_end = 500; -const uint32_t iap_time_2_high_end = 5000; -const uint32_t iap_time_3_low_end = 500; -const uint32_t iap_time_3_high_end = 5000; +const uint32_t iap_time_2_low_end = 500; +const uint32_t iap_time_2_high_end = 5000; +const uint32_t iap_time_3_low_end = 500; +const uint32_t iap_time_3_high_end = 5000; // Private types @@ -64,9 +64,9 @@ static uint8_t reset_count = 0; static portTickType lastResetSysTime; // Private functions -static void FirmwareIAPCallback(UAVObjEvent* ev); +static void FirmwareIAPCallback(UAVObjEvent *ev); -static uint32_t get_time(void); +static uint32_t get_time(void); // Private types @@ -89,28 +89,29 @@ static void resetTask(UAVObjEvent *); MODULE_INITCALL(FirmwareIAPInitialize, 0) int32_t FirmwareIAPInitialize() { - - FirmwareIAPObjInitialize(); - - const struct pios_board_info * bdinfo = &pios_board_info_blob; + FirmwareIAPObjInitialize(); - FirmwareIAPObjData data; - FirmwareIAPObjGet(&data); + const struct pios_board_info *bdinfo = &pios_board_info_blob; - data.BoardType= bdinfo->board_type; - PIOS_BL_HELPER_FLASH_Read_Description(data.Description,FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM); - PIOS_SYS_SerialNumberGetBinary(data.CPUSerial); - data.BoardRevision= bdinfo->board_rev; - data.ArmReset=0; - data.crc = 0; - FirmwareIAPObjSet( &data ); - if(bdinfo->magic==PIOS_BOARD_INFO_BLOB_MAGIC) FirmwareIAPObjConnectCallback( &FirmwareIAPCallback ); - return 0; + FirmwareIAPObjData data; + FirmwareIAPObjGet(&data); + + data.BoardType = bdinfo->board_type; + PIOS_BL_HELPER_FLASH_Read_Description(data.Description, FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM); + PIOS_SYS_SerialNumberGetBinary(data.CPUSerial); + data.BoardRevision = bdinfo->board_rev; + data.ArmReset = 0; + data.crc = 0; + FirmwareIAPObjSet(&data); + if (bdinfo->magic == PIOS_BOARD_INFO_BLOB_MAGIC) { + FirmwareIAPObjConnectCallback(&FirmwareIAPCallback); + } + return 0; } int32_t FirmwareIAPStart() { - return 0; + return 0; } /*! @@ -121,100 +122,98 @@ int32_t FirmwareIAPStart() * \note * */ -static uint8_t iap_state = IAP_STATE_READY; -static void FirmwareIAPCallback(UAVObjEvent* ev) +static uint8_t iap_state = IAP_STATE_READY; +static void FirmwareIAPCallback(UAVObjEvent *ev) { - const struct pios_board_info * bdinfo = &pios_board_info_blob; - static uint32_t last_time = 0; - uint32_t this_time; - uint32_t delta; - - if(iap_state == IAP_STATE_RESETTING) - return; + const struct pios_board_info *bdinfo = &pios_board_info_blob; + static uint32_t last_time = 0; + uint32_t this_time; + uint32_t delta; - FirmwareIAPObjData data; - FirmwareIAPObjGet(&data); - - if ( ev->obj == FirmwareIAPObjHandle() ) { - // Get the input object data - FirmwareIAPObjGet(&data); - this_time = get_time(); - delta = this_time - last_time; - last_time = this_time; - if((data.BoardType==bdinfo->board_type)&&(data.crc != PIOS_BL_HELPER_CRC_Memory_Calc())) - { - PIOS_BL_HELPER_FLASH_Read_Description(data.Description,FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM); - PIOS_SYS_SerialNumberGetBinary(data.CPUSerial); - data.BoardRevision=bdinfo->board_rev; - data.crc = PIOS_BL_HELPER_CRC_Memory_Calc(); - FirmwareIAPObjSet( &data ); - } - if((data.ArmReset==1)&&(iap_state!=IAP_STATE_RESETTING)) - { - data.ArmReset=0; - FirmwareIAPObjSet( &data ); - } - switch(iap_state) { - case IAP_STATE_READY: - if( data.Command == IAP_CMD_STEP_1 ) { - iap_state = IAP_STATE_STEP_1; - } break; - case IAP_STATE_STEP_1: - if( data.Command == IAP_CMD_STEP_2 ) { - if( delta > iap_time_2_low_end && delta < iap_time_2_high_end ) { - iap_state = IAP_STATE_STEP_2; - } else { - iap_state = IAP_STATE_READY; - } - } else { - iap_state = IAP_STATE_READY; - } - break; - case IAP_STATE_STEP_2: - if( data.Command == IAP_CMD_STEP_3 ) { - if( delta > iap_time_3_low_end && delta < iap_time_3_high_end ) { - - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - - if(flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) { - // Abort any attempts if not disarmed - iap_state = IAP_STATE_READY; - break; - } - - // we've met the three sequence of command numbers - // we've met the time requirements. - PIOS_IAP_SetRequest1(); - PIOS_IAP_SetRequest2(); - - /* Note: Cant just wait timeout value, because first time is randomized */ - reset_count = 0; - lastResetSysTime = xTaskGetTickCount(); - UAVObjEvent *event = pvPortMalloc(sizeof(UAVObjEvent)); - memset(event, 0, sizeof(UAVObjEvent)); - EventPeriodicCallbackCreate(event, resetTask, 100); - iap_state = IAP_STATE_RESETTING; - } else { - iap_state = IAP_STATE_READY; - } - } else { - iap_state = IAP_STATE_READY; - } - break; - case IAP_STATE_RESETTING: - // stay here permanentally, should reboot - break; - default: - iap_state = IAP_STATE_READY; - last_time = 0; // Reset the time counter, as we are not doing a IAP reset - break; - } - } + if (iap_state == IAP_STATE_RESETTING) { + return; + } + + FirmwareIAPObjData data; + FirmwareIAPObjGet(&data); + + if (ev->obj == FirmwareIAPObjHandle()) { + // Get the input object data + FirmwareIAPObjGet(&data); + this_time = get_time(); + delta = this_time - last_time; + last_time = this_time; + if ((data.BoardType == bdinfo->board_type) && (data.crc != PIOS_BL_HELPER_CRC_Memory_Calc())) { + PIOS_BL_HELPER_FLASH_Read_Description(data.Description, FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM); + PIOS_SYS_SerialNumberGetBinary(data.CPUSerial); + data.BoardRevision = bdinfo->board_rev; + data.crc = PIOS_BL_HELPER_CRC_Memory_Calc(); + FirmwareIAPObjSet(&data); + } + if ((data.ArmReset == 1) && (iap_state != IAP_STATE_RESETTING)) { + data.ArmReset = 0; + FirmwareIAPObjSet(&data); + } + switch (iap_state) { + case IAP_STATE_READY: + if (data.Command == IAP_CMD_STEP_1) { + iap_state = IAP_STATE_STEP_1; + } + break; + case IAP_STATE_STEP_1: + if (data.Command == IAP_CMD_STEP_2) { + if (delta > iap_time_2_low_end && delta < iap_time_2_high_end) { + iap_state = IAP_STATE_STEP_2; + } else { + iap_state = IAP_STATE_READY; + } + } else { + iap_state = IAP_STATE_READY; + } + break; + case IAP_STATE_STEP_2: + if (data.Command == IAP_CMD_STEP_3) { + if (delta > iap_time_3_low_end && delta < iap_time_3_high_end) { + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + if (flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) { + // Abort any attempts if not disarmed + iap_state = IAP_STATE_READY; + break; + } + + // we've met the three sequence of command numbers + // we've met the time requirements. + PIOS_IAP_SetRequest1(); + PIOS_IAP_SetRequest2(); + + /* Note: Cant just wait timeout value, because first time is randomized */ + reset_count = 0; + lastResetSysTime = xTaskGetTickCount(); + UAVObjEvent *event = pvPortMalloc(sizeof(UAVObjEvent)); + memset(event, 0, sizeof(UAVObjEvent)); + EventPeriodicCallbackCreate(event, resetTask, 100); + iap_state = IAP_STATE_RESETTING; + } else { + iap_state = IAP_STATE_READY; + } + } else { + iap_state = IAP_STATE_READY; + } + break; + case IAP_STATE_RESETTING: + // stay here permanentally, should reboot + break; + default: + iap_state = IAP_STATE_READY; + last_time = 0; // Reset the time counter, as we are not doing a IAP reset + break; + } + } } - // Returns number of milliseconds from the start of the kernel. /*! @@ -228,39 +227,38 @@ static void FirmwareIAPCallback(UAVObjEvent* ev) static uint32_t get_time(void) { - portTickType ticks; - - ticks = xTaskGetTickCount(); - - return TICKS2MS(ticks); + portTickType ticks; + + ticks = xTaskGetTickCount(); + + return TICKS2MS(ticks); } /** - * Executed by event dispatcher callback to reset INS before resetting OP + * Executed by event dispatcher callback to reset INS before resetting OP */ -static void resetTask(__attribute__((unused)) UAVObjEvent * ev) +static void resetTask(__attribute__((unused)) UAVObjEvent *ev) { -#if defined (PIOS_LED_HEARTBEAT) - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ -#if defined (PIOS_LED_ALARM) - PIOS_LED_Toggle(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ +#if defined(PIOS_LED_ALARM) + PIOS_LED_Toggle(PIOS_LED_ALARM); +#endif /* PIOS_LED_ALARM */ - FirmwareIAPObjData data; - FirmwareIAPObjGet(&data); + FirmwareIAPObjData data; + FirmwareIAPObjGet(&data); - if((portTickType) (xTaskGetTickCount() - lastResetSysTime) > RESET_DELAY / portTICK_RATE_MS) { - lastResetSysTime = xTaskGetTickCount(); - data.BoardType=0xFF; - data.ArmReset=1; - data.crc=reset_count; /* Must change a value for this to get to INS */ - FirmwareIAPObjSet(&data); - ++reset_count; - if(reset_count>3) - { - PIOS_SYS_Reset(); - } - } + if ((portTickType)(xTaskGetTickCount() - lastResetSysTime) > RESET_DELAY / portTICK_RATE_MS) { + lastResetSysTime = xTaskGetTickCount(); + data.BoardType = 0xFF; + data.ArmReset = 1; + data.crc = reset_count; /* Must change a value for this to get to INS */ + FirmwareIAPObjSet(&data); + ++reset_count; + if (reset_count > 3) { + PIOS_SYS_Reset(); + } + } } diff --git a/flight/modules/FirmwareIAP/inc/firmwareiap.h b/flight/modules/FirmwareIAP/inc/firmwareiap.h index f62a51d4a..81c39d880 100644 --- a/flight/modules/FirmwareIAP/inc/firmwareiap.h +++ b/flight/modules/FirmwareIAP/inc/firmwareiap.h @@ -30,4 +30,3 @@ int32_t FirmwareIAPInitialize(); int32_t FirmwareIAPStart(); #endif // FIRMWAREIAP_H - diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 66e69f4c5..fec984a87 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -3,7 +3,7 @@ * * @file fixedwingpathfollower.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint + * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint * and sets @ref AttitudeDesired. It only does this when the FlightMode field * of @ref ManualControlCommand is Auto. * @@ -48,7 +48,7 @@ #include "accels.h" #include "hwsettings.h" #include "attitudeactual.h" -#include "pathdesired.h" // object that will be updated by the module +#include "pathdesired.h" // object that will be updated by the module #include "positionactual.h" #include "manualcontrol.h" #include "flightstatus.h" @@ -70,9 +70,9 @@ #include "CoordinateConversions.h" // Private constants -#define MAX_QUEUE_SIZE 4 +#define MAX_QUEUE_SIZE 4 #define STACK_SIZE_BYTES 1548 -#define TASK_PRIORITY (tskIDLE_PRIORITY+2) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) // Private variables static bool followerEnabled = false; @@ -83,11 +83,11 @@ static FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings; // Private functions static void pathfollowerTask(void *parameters); -static void SettingsUpdatedCb(UAVObjEvent * ev); +static void SettingsUpdatedCb(UAVObjEvent *ev); static void updatePathVelocity(); static uint8_t updateFixedDesiredAttitude(); static void updateFixedAttitude(); -static void airspeedActualUpdatedCb(UAVObjEvent * ev); +static void airspeedActualUpdatedCb(UAVObjEvent *ev); static float bound(float val, float min, float max); /** @@ -96,13 +96,13 @@ static float bound(float val, float min, float max); */ int32_t FixedWingPathFollowerStart() { - if (followerEnabled) { - // Start main task - xTaskCreate(pathfollowerTask, (signed char *)"PathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle); - } + if (followerEnabled) { + // Start main task + xTaskCreate(pathfollowerTask, (signed char *)"PathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle); + } - return 0; + return 0; } /** @@ -111,32 +111,32 @@ int32_t FixedWingPathFollowerStart() */ int32_t FixedWingPathFollowerInitialize() { - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_FIXEDWINGPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - followerEnabled = true; - FixedWingPathFollowerSettingsInitialize(); - FixedWingPathFollowerStatusInitialize(); - PathDesiredInitialize(); - PathStatusInitialize(); - VelocityDesiredInitialize(); - AirspeedActualInitialize(); - } else { - followerEnabled = false; - } - return 0; + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + if (optionalModules[HWSETTINGS_OPTIONALMODULES_FIXEDWINGPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + followerEnabled = true; + FixedWingPathFollowerSettingsInitialize(); + FixedWingPathFollowerStatusInitialize(); + PathDesiredInitialize(); + PathStatusInitialize(); + VelocityDesiredInitialize(); + AirspeedActualInitialize(); + } else { + followerEnabled = false; + } + return 0; } MODULE_INITCALL(FixedWingPathFollowerInitialize, FixedWingPathFollowerStart) static float northVelIntegral = 0; -static float eastVelIntegral = 0; -static float downVelIntegral = 0; +static float eastVelIntegral = 0; +static float downVelIntegral = 0; -static float bearingIntegral = 0; -static float speedIntegral = 0; -static float powerIntegral = 0; -static float airspeedErrorInt=0; +static float bearingIntegral = 0; +static float speedIntegral = 0; +static float powerIntegral = 0; +static float airspeedErrorInt = 0; // correct speed by measured airspeed static float indicatedAirspeedActualBias = 0; @@ -146,197 +146,205 @@ static float indicatedAirspeedActualBias = 0; */ static void pathfollowerTask(__attribute__((unused)) void *parameters) { - SystemSettingsData systemSettings; - FlightStatusData flightStatus; - - portTickType lastUpdateTime; - - AirspeedActualConnectCallback(airspeedActualUpdatedCb); - FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb); - PathDesiredConnectCallback(SettingsUpdatedCb); - - FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); - PathDesiredGet(&pathDesired); - - // Main task loop - lastUpdateTime = xTaskGetTickCount(); - while (1) { + SystemSettingsData systemSettings; + FlightStatusData flightStatus; - // Conditions when this runs: - // 1. Must have FixedWing type airframe - // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR - // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path + portTickType lastUpdateTime; - SystemSettingsGet(&systemSettings); - if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) ) - { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING); - vTaskDelay(1000); - continue; - } + AirspeedActualConnectCallback(airspeedActualUpdatedCb); + FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb); + PathDesiredConnectCallback(SettingsUpdatedCb); - // Continue collecting data if not enough time - vTaskDelayUntil(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS); + FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); + PathDesiredGet(&pathDesired); - - FlightStatusGet(&flightStatus); - PathStatusGet(&pathStatus); - - uint8_t result; - // Check the combinations of flightmode and pathdesired mode - switch(flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updatePathVelocity(); - result = updateFixedDesiredAttitude(); - if (result) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING); - } - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR); - } - break; - case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: - pathStatus.UID = pathDesired.UID; - pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; - switch(pathDesired.Mode) { - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - updatePathVelocity(); - result = updateFixedDesiredAttitude(); - if (result) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); - } else { - pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING); - } - break; - case PATHDESIRED_MODE_FIXEDATTITUDE: - updateFixedAttitude(pathDesired.ModeParameters); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_DISARMALARM: - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_CRITICAL); - break; - default: - pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR); - break; - } - break; - default: - // Be cleaner and get rid of global variables - northVelIntegral = 0; - eastVelIntegral = 0; - downVelIntegral = 0; - bearingIntegral = 0; - speedIntegral = 0; - powerIntegral = 0; + // Main task loop + lastUpdateTime = xTaskGetTickCount(); + while (1) { + // Conditions when this runs: + // 1. Must have FixedWing type airframe + // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR + // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path - break; - } - PathStatusSet(&pathStatus); - } + SystemSettingsGet(&systemSettings); + if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) && + (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) && + (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL)) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + vTaskDelay(1000); + continue; + } + + // Continue collecting data if not enough time + vTaskDelayUntil(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS); + + + FlightStatusGet(&flightStatus); + PathStatusGet(&pathStatus); + + uint8_t result; + // Check the combinations of flightmode and pathdesired mode + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { + updatePathVelocity(); + result = updateFixedDesiredAttitude(); + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + } + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + } + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + pathStatus.UID = pathDesired.UID; + pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + updatePathVelocity(); + result = updateFixedDesiredAttitude(); + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + } + break; + case PATHDESIRED_MODE_FIXEDATTITUDE: + updateFixedAttitude(pathDesired.ModeParameters); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + break; + case PATHDESIRED_MODE_DISARMALARM: + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + break; + default: + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + break; + } + break; + default: + // Be cleaner and get rid of global variables + northVelIntegral = 0; + eastVelIntegral = 0; + downVelIntegral = 0; + bearingIntegral = 0; + speedIntegral = 0; + powerIntegral = 0; + + break; + } + PathStatusSet(&pathStatus); + } } /** * Compute desired velocity from the current position and path * - * Takes in @ref PositionActual and compares it to @ref PathDesired + * Takes in @ref PositionActual and compares it to @ref PathDesired * and computes @ref VelocityDesired */ static void updatePathVelocity() { - PositionActualData positionActual; - PositionActualGet(&positionActual); - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); + PositionActualData positionActual; - // look ahead fixedwingpathfollowerSettings.HeadingFeedForward seconds - float cur[3] = {positionActual.North + (velocityActual.North * fixedwingpathfollowerSettings.HeadingFeedForward), - positionActual.East + (velocityActual.East * fixedwingpathfollowerSettings.HeadingFeedForward), - positionActual.Down + (velocityActual.Down * fixedwingpathfollowerSettings.HeadingFeedForward) - }; - struct path_status progress; + PositionActualGet(&positionActual); + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); - path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); - - float groundspeed; - float altitudeSetpoint; - switch (pathDesired.Mode) { - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - groundspeed = pathDesired.EndingVelocity; - altitudeSetpoint = pathDesired.End[2]; - break; - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_DRIVEENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_DRIVEVECTOR: - default: - groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * - bound(progress.fractional_progress,0,1); - altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * - bound(progress.fractional_progress,0,1); - break; - } - // make sure groundspeed is not zero - if (groundspeed<1e-2f) groundspeed=1e-2f; - - // calculate velocity - can be zero if waypoints are too close - VelocityDesiredData velocityDesired; - velocityDesired.North = progress.path_direction[0]; - velocityDesired.East = progress.path_direction[1]; - - float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP; + // look ahead fixedwingpathfollowerSettings.HeadingFeedForward seconds + float cur[3] = { positionActual.North + (velocityActual.North * fixedwingpathfollowerSettings.HeadingFeedForward), + positionActual.East + (velocityActual.East * fixedwingpathfollowerSettings.HeadingFeedForward), + positionActual.Down + (velocityActual.Down * fixedwingpathfollowerSettings.HeadingFeedForward) }; + struct path_status progress; - // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) - // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector - // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading) - // leading to an S-shape snake course the wrong way - // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't - // turn steep unless there is enough space complete the turn before crossing the flightpath - // in this case the plane effectively needs to be turned around - // indicators: - // difference between correction_direction and velocityactual >90 degrees and - // difference between path_direction and velocityactual >90 degrees ( 4th sector, facing away from eerything ) - // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) - float angle1=RAD2DEG( atan2f(progress.path_direction[1],progress.path_direction[0]) - atan2f(velocityActual.East,velocityActual.North)); - float angle2=RAD2DEG( atan2f(progress.correction_direction[1],progress.correction_direction[0]) - atan2f(velocityActual.East,velocityActual.North)); - if (angle1<-180.0f) angle1+=360.0f; - if (angle1>180.0f) angle1-=360.0f; - if (angle2<-180.0f) angle2+=360.0f; - if (angle2>180.0f) angle2-=360.0f; - if (fabsf(angle1)>=90.0f && fabsf(angle2)>=90.0f) { - error_speed=0; - } + path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); - // calculate correction - can also be zero if correction vector is 0 or no error present - velocityDesired.North += progress.correction_direction[0] * error_speed; - velocityDesired.East += progress.correction_direction[1] * error_speed; + float groundspeed; + float altitudeSetpoint; + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_DRIVECIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + case PATHDESIRED_MODE_DRIVECIRCLELEFT: + groundspeed = pathDesired.EndingVelocity; + altitudeSetpoint = pathDesired.End[2]; + break; + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_DRIVEENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_DRIVEVECTOR: + default: + groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * + bound(progress.fractional_progress, 0, 1); + altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * + bound(progress.fractional_progress, 0, 1); + break; + } + // make sure groundspeed is not zero + if (groundspeed < 1e-2f) { + groundspeed = 1e-2f; + } - //scale to correct length - float l=sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East); - velocityDesired.North *= groundspeed/l; - velocityDesired.East *= groundspeed/l; - - float downError = altitudeSetpoint - positionActual.Down; - velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP; + // calculate velocity - can be zero if waypoints are too close + VelocityDesiredData velocityDesired; + velocityDesired.North = progress.path_direction[0]; + velocityDesired.East = progress.path_direction[1]; - // update pathstatus - pathStatus.error = progress.error; - pathStatus.fractional_progress = progress.fractional_progress; + float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP; - VelocityDesiredSet(&velocityDesired); + // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) + // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector + // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading) + // leading to an S-shape snake course the wrong way + // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't + // turn steep unless there is enough space complete the turn before crossing the flightpath + // in this case the plane effectively needs to be turned around + // indicators: + // difference between correction_direction and velocityactual >90 degrees and + // difference between path_direction and velocityactual >90 degrees ( 4th sector, facing away from eerything ) + // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) + float angle1 = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0]) - atan2f(velocityActual.East, velocityActual.North)); + float angle2 = RAD2DEG(atan2f(progress.correction_direction[1], progress.correction_direction[0]) - atan2f(velocityActual.East, velocityActual.North)); + if (angle1 < -180.0f) { + angle1 += 360.0f; + } + if (angle1 > 180.0f) { + angle1 -= 360.0f; + } + if (angle2 < -180.0f) { + angle2 += 360.0f; + } + if (angle2 > 180.0f) { + angle2 -= 360.0f; + } + if (fabsf(angle1) >= 90.0f && fabsf(angle2) >= 90.0f) { + error_speed = 0; + } + + // calculate correction - can also be zero if correction vector is 0 or no error present + velocityDesired.North += progress.correction_direction[0] * error_speed; + velocityDesired.East += progress.correction_direction[1] * error_speed; + + // scale to correct length + float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); + velocityDesired.North *= groundspeed / l; + velocityDesired.East *= groundspeed / l; + + float downError = altitudeSetpoint - positionActual.Down; + velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP; + + // update pathstatus + pathStatus.error = progress.error; + pathStatus.fractional_progress = progress.fractional_progress; + + VelocityDesiredSet(&velocityDesired); } @@ -344,273 +352,277 @@ static void updatePathVelocity() * Compute desired attitude from a fixed preset * */ -static void updateFixedAttitude(float* attitude) +static void updateFixedAttitude(float *attitude) { - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); - stabDesired.Roll = attitude[0]; - stabDesired.Pitch = attitude[1]; - stabDesired.Yaw = attitude[2]; - stabDesired.Throttle = attitude[3]; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - StabilizationDesiredSet(&stabDesired); + StabilizationDesiredData stabDesired; + + StabilizationDesiredGet(&stabDesired); + stabDesired.Roll = attitude[0]; + stabDesired.Pitch = attitude[1]; + stabDesired.Yaw = attitude[2]; + stabDesired.Throttle = attitude[3]; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + StabilizationDesiredSet(&stabDesired); } /** * Compute desired attitude from the desired velocity * - * Takes in @ref NedActual which has the acceleration in the - * NED frame as the feedback term and then compares the + * Takes in @ref NedActual which has the acceleration in the + * NED frame as the feedback term and then compares the * @ref VelocityActual against the @ref VelocityDesired */ static uint8_t updateFixedDesiredAttitude() { + uint8_t result = 1; - uint8_t result = 1; + float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; // Convert from [ms] to [s] - float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; //Convert from [ms] to [s] + VelocityDesiredData velocityDesired; + VelocityActualData velocityActual; + StabilizationDesiredData stabDesired; + AttitudeActualData attitudeActual; + AccelsData accels; + StabilizationSettingsData stabSettings; + FixedWingPathFollowerStatusData fixedwingpathfollowerStatus; + AirspeedActualData airspeedActual; - VelocityDesiredData velocityDesired; - VelocityActualData velocityActual; - StabilizationDesiredData stabDesired; - AttitudeActualData attitudeActual; - AccelsData accels; - StabilizationSettingsData stabSettings; - FixedWingPathFollowerStatusData fixedwingpathfollowerStatus; - AirspeedActualData airspeedActual; - - float groundspeedActual; - float groundspeedDesired; - float indicatedAirspeedActual; - float indicatedAirspeedDesired; - float airspeedError; - - float pitchCommand; + float groundspeedActual; + float groundspeedDesired; + float indicatedAirspeedActual; + float indicatedAirspeedDesired; + float airspeedError; - float descentspeedDesired; - float descentspeedError; - float powerCommand; + float pitchCommand; - float bearingError; - float bearingCommand; + float descentspeedDesired; + float descentspeedError; + float powerCommand; - FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus); - - VelocityActualGet(&velocityActual); - StabilizationDesiredGet(&stabDesired); - VelocityDesiredGet(&velocityDesired); - AttitudeActualGet(&attitudeActual); - AccelsGet(&accels); - StabilizationSettingsGet(&stabSettings); - AirspeedActualGet(&airspeedActual); + float bearingError; + float bearingCommand; + + FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus); + + VelocityActualGet(&velocityActual); + StabilizationDesiredGet(&stabDesired); + VelocityDesiredGet(&velocityDesired); + AttitudeActualGet(&attitudeActual); + AccelsGet(&accels); + StabilizationSettingsGet(&stabSettings); + AirspeedActualGet(&airspeedActual); - /** - * Compute speed error (required for throttle and pitch) - */ + /** + * Compute speed error (required for throttle and pitch) + */ - // Current ground speed - groundspeedActual = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North ); - // note that airspeedActualBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement, - // but thanks to accelerometers, groundspeed reacts faster to changes in direction - // than airspeed and gps sensors alone - indicatedAirspeedActual = groundspeedActual + indicatedAirspeedActualBias; + // Current ground speed + groundspeedActual = sqrtf(velocityActual.East * velocityActual.East + velocityActual.North * velocityActual.North); + // note that airspeedActualBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement, + // but thanks to accelerometers, groundspeed reacts faster to changes in direction + // than airspeed and gps sensors alone + indicatedAirspeedActual = groundspeedActual + indicatedAirspeedActualBias; - // Desired ground speed - groundspeedDesired = sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East); - indicatedAirspeedDesired = bound( groundspeedDesired + indicatedAirspeedActualBias, - fixedwingpathfollowerSettings.BestClimbRateSpeed, - fixedwingpathfollowerSettings.CruiseSpeed); + // Desired ground speed + groundspeedDesired = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); + indicatedAirspeedDesired = bound(groundspeedDesired + indicatedAirspeedActualBias, + fixedwingpathfollowerSettings.BestClimbRateSpeed, + fixedwingpathfollowerSettings.CruiseSpeed); - // Airspeed error - airspeedError = indicatedAirspeedDesired - indicatedAirspeedActual; + // Airspeed error + airspeedError = indicatedAirspeedDesired - indicatedAirspeedActual; - // Vertical speed error - descentspeedDesired = bound ( - velocityDesired.Down, - -fixedwingpathfollowerSettings.VerticalVelMax, - fixedwingpathfollowerSettings.VerticalVelMax); - descentspeedError = descentspeedDesired - velocityActual.Down; + // Vertical speed error + descentspeedDesired = bound( + velocityDesired.Down, + -fixedwingpathfollowerSettings.VerticalVelMax, + fixedwingpathfollowerSettings.VerticalVelMax); + descentspeedError = descentspeedDesired - velocityActual.Down; - // Error condition: wind speed is higher than maximum allowed speed. We are forced backwards! - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0; - if (groundspeedDesired - indicatedAirspeedActualBias <= 0 ) { - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1; - result = 0; - } - // Error condition: plane too slow or too fast - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0; - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0; - if ( indicatedAirspeedActual > fixedwingpathfollowerSettings.AirSpeedMax) { - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1; - result = 0; - } - if ( indicatedAirspeedActual > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) { - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1; - result = 0; - } - if (indicatedAirspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { //The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; - result = 0; - } - if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { //Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; - result = 0; - } - if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) { - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; - result = 0; - } - - if (indicatedAirspeedActual<1e-6f) { - // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes - // also we cannot handle planes flying backwards, lets just wait until the nose drops - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; - return 0; - } + // Error condition: wind speed is higher than maximum allowed speed. We are forced backwards! + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0; + if (groundspeedDesired - indicatedAirspeedActualBias <= 0) { + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1; + result = 0; + } + // Error condition: plane too slow or too fast + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0; + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0; + if (indicatedAirspeedActual > fixedwingpathfollowerSettings.AirSpeedMax) { + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1; + result = 0; + } + if (indicatedAirspeedActual > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) { + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1; + result = 0; + } + if (indicatedAirspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { // The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; + result = 0; + } + if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { // Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; + result = 0; + } + if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) { + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; + result = 0; + } - /** - * Compute desired throttle command - */ - // compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant. - if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] >0) { - powerIntegral = bound(powerIntegral + -descentspeedError * dT, - -fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI], - fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] - )*(1.0f-1.0f/(1.0f+30.0f/dT)); - } else powerIntegral = 0; - - //Compute the cross feed from vertical speed to pitch, with saturation - float speedErrorToPowerCommandComponent = bound ( - (airspeedError/fixedwingpathfollowerSettings.BestClimbRateSpeed)* fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_KP] , - -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX], - fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX] - ); + if (indicatedAirspeedActual < 1e-6f) { + // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes + // also we cannot handle planes flying backwards, lets just wait until the nose drops + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; + return 0; + } - // Compute final throttle response - powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] + - powerIntegral* fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] + - speedErrorToPowerCommandComponent; + /** + * Compute desired throttle command + */ + // compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant. + if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] > 0) { + powerIntegral = bound(powerIntegral + -descentspeedError * dT, + -fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT] / fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI], + fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT] / fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] + ) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); + } else { powerIntegral = 0; } - //Output internal state to telemetry - fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = descentspeedError; - fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_POWER] = powerIntegral; - fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_POWER] = powerCommand; + // Compute the cross feed from vertical speed to pitch, with saturation + float speedErrorToPowerCommandComponent = bound( + (airspeedError / fixedwingpathfollowerSettings.BestClimbRateSpeed) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_KP], + -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX], + fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX] + ); - // set throttle - stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL] + powerCommand, - fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN], - fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]); + // Compute final throttle response + powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] + + powerIntegral * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] + + speedErrorToPowerCommandComponent; - // Error condition: plane cannot hold altitude at current speed. - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0; - if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] && // throttle at maximum - velocityActual.Down > 0 && // we ARE going down - descentspeedDesired < 0 && // we WANT to go up - airspeedError > 0) { // we are too slow already - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1; - result = 0; - } - // Error condition: plane keeps climbing despite minimum throttle (opposite of above) - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0; - if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] && // throttle at minimum - velocityActual.Down < 0 && // we ARE going up - descentspeedDesired > 0 && // we WANT to go down - airspeedError < 0 ) { // we are too fast already - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1; - result = 0; - } + // Output internal state to telemetry + fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = descentspeedError; + fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_POWER] = powerIntegral; + fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_POWER] = powerCommand; + + // set throttle + stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL] + powerCommand, + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN], + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]); + + // Error condition: plane cannot hold altitude at current speed. + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0; + if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] && // throttle at maximum + velocityActual.Down > 0 && // we ARE going down + descentspeedDesired < 0 && // we WANT to go up + airspeedError > 0) { // we are too slow already + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1; + result = 0; + } + // Error condition: plane keeps climbing despite minimum throttle (opposite of above) + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0; + if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] && // throttle at minimum + velocityActual.Down < 0 && // we ARE going up + descentspeedDesired > 0 && // we WANT to go down + airspeedError < 0) { // we are too fast already + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1; + result = 0; + } - /** - * Compute desired pitch command - */ - - if (fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] > 0){ - //Integrate with saturation - airspeedErrorInt=bound(airspeedErrorInt + airspeedError * dT, - -fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI], - fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]); - } - - //Compute the cross feed from vertical speed to pitch, with saturation - float verticalSpeedToPitchCommandComponent=bound (-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP], - -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX], - fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX] - ); - - //Compute the pitch command as err*Kp + errInt*Ki + X_feed. - pitchCommand= -(airspeedError*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KP] - + airspeedErrorInt*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] - ) + verticalSpeedToPitchCommandComponent; - - fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError; - fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt; - fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand; + /** + * Compute desired pitch command + */ - stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] + pitchCommand, - fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN], - fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]); + if (fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] > 0) { + // Integrate with saturation + airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT, + -fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT] / fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI], + fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT] / fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]); + } - // Error condition: high speed dive - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0; - if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] && // pitch demand is full up - velocityActual.Down > 0 && // we ARE going down - descentspeedDesired < 0 && // we WANT to go up - airspeedError < 0 ) { // we are too fast already - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1; - result = 0; - } + // Compute the cross feed from vertical speed to pitch, with saturation + float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP], + -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX], + fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX] + ); - /** - * Compute desired roll command - */ - if (groundspeedDesired> 1e-6f) { - bearingError = RAD2DEG(atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North)); - } else { - // if we are not supposed to move, run in a circle - bearingError = -90.0f; - } - - if (bearingError<-180.0f) bearingError+=360.0f; - if (bearingError>180.0f) bearingError-=360.0f; + // Compute the pitch command as err*Kp + errInt*Ki + X_feed. + pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KP] + + airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] + ) + verticalSpeedToPitchCommandComponent; - bearingIntegral = bound(bearingIntegral + bearingError * dT * fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_KI], - -fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_ILIMIT], - fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_ILIMIT]); - bearingCommand = (bearingError * fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_KP] + - bearingIntegral); + fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError; + fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt; + fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand; - fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_BEARING] = bearingError; - fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_BEARING] = bearingIntegral; - fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_BEARING] = bearingCommand; - - stabDesired.Roll = bound( fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] + - bearingCommand, - fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN], - fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MAX] ); + stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] + pitchCommand, + fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN], + fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]); - // TODO: find a check to determine loss of directional control. Likely needs some check of derivative + // Error condition: high speed dive + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0; + if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] && // pitch demand is full up + velocityActual.Down > 0 && // we ARE going down + descentspeedDesired < 0 && // we WANT to go up + airspeedError < 0) { // we are too fast already + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1; + result = 0; + } + + /** + * Compute desired roll command + */ + if (groundspeedDesired > 1e-6f) { + bearingError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityActual.East, velocityActual.North)); + } else { + // if we are not supposed to move, run in a circle + bearingError = -90.0f; + } + + if (bearingError < -180.0f) { + bearingError += 360.0f; + } + if (bearingError > 180.0f) { + bearingError -= 360.0f; + } + + bearingIntegral = bound(bearingIntegral + bearingError * dT * fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_KI], + -fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_ILIMIT], + fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_ILIMIT]); + bearingCommand = (bearingError * fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_KP] + + bearingIntegral); + + fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_BEARING] = bearingError; + fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_BEARING] = bearingIntegral; + fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_BEARING] = bearingCommand; + + stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] + + bearingCommand, + fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN], + fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MAX]); + + // TODO: find a check to determine loss of directional control. Likely needs some check of derivative - /** - * Compute desired yaw command - */ - // TODO implement raw control mode for yaw and base on Accels.Y - stabDesired.Yaw = 0; + /** + * Compute desired yaw command + */ + // TODO implement raw control mode for yaw and base on Accels.Y + stabDesired.Yaw = 0; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE; - - StabilizationDesiredSet(&stabDesired); + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE; - FixedWingPathFollowerStatusSet(&fixedwingpathfollowerStatus); + StabilizationDesiredSet(&stabDesired); - return result; + FixedWingPathFollowerStatusSet(&fixedwingpathfollowerStatus); + + return result; } @@ -619,33 +631,31 @@ static uint8_t updateFixedDesiredAttitude() */ static float bound(float val, float min, float max) { - if (val < min) { - val = min; - } else if (val > max) { - val = max; - } - return val; + if (val < min) { + val = min; + } else if (val > max) { + val = max; + } + return val; } static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); - PathDesiredGet(&pathDesired); + FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); + PathDesiredGet(&pathDesired); } static void airspeedActualUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { + AirspeedActualData airspeedActual; + VelocityActualData velocityActual; - AirspeedActualData airspeedActual; - VelocityActualData velocityActual; + AirspeedActualGet(&airspeedActual); + VelocityActualGet(&velocityActual); + float groundspeed = sqrtf(velocityActual.East * velocityActual.East + velocityActual.North * velocityActual.North); - AirspeedActualGet(&airspeedActual); - VelocityActualGet(&velocityActual); - float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North ); - - - indicatedAirspeedActualBias = airspeedActual.CalibratedAirspeed - groundspeed; - // note - we do fly by Indicated Airspeed (== calibrated airspeed) - // however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement. + indicatedAirspeedActualBias = airspeedActual.CalibratedAirspeed - groundspeed; + // note - we do fly by Indicated Airspeed (== calibrated airspeed) + // however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement. } diff --git a/flight/modules/FlightPlan/flightplan.c b/flight/modules/FlightPlan/flightplan.c index 0529f1f0d..545b4b437 100644 --- a/flight/modules/FlightPlan/flightplan.c +++ b/flight/modules/FlightPlan/flightplan.c @@ -41,8 +41,8 @@ // Private constants #define STACK_SIZE_BYTES 1500 -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) -#define MAX_QUEUE_SIZE 2 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define MAX_QUEUE_SIZE 2 // Private types @@ -52,7 +52,7 @@ static xQueueHandle queue; // Private functions static void flightPlanTask(void *parameters); -static void objectUpdatedCb(UAVObjEvent * ev); +static void objectUpdatedCb(UAVObjEvent *ev); // External variables (temporary, TODO: this will be loaded from the SD card) extern unsigned char usrlib_img[]; @@ -62,13 +62,13 @@ extern unsigned char usrlib_img[]; */ int32_t FlightPlanStart() { - taskHandle = NULL; + taskHandle = NULL; - // Start VM thread - xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle); + // Start VM thread + xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle); - return 0; + return 0; } /** @@ -76,22 +76,22 @@ int32_t FlightPlanStart() */ int32_t FlightPlanInitialize() { - taskHandle = NULL; - - FlightPlanStatusInitialize(); - FlightPlanControlInitialize(); - FlightPlanSettingsInitialize(); - - // Listen for object updates - FlightPlanControlConnectCallback(&objectUpdatedCb); + taskHandle = NULL; - // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + FlightPlanStatusInitialize(); + FlightPlanControlInitialize(); + FlightPlanSettingsInitialize(); - // Listen for FlightPlanControl updates - FlightPlanControlConnectQueue(queue); + // Listen for object updates + FlightPlanControlConnectCallback(&objectUpdatedCb); - return 0; + // Create object queue + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + + // Listen for FlightPlanControl updates + FlightPlanControlConnectQueue(queue); + + return 0; } MODULE_INITCALL(FlightPlanInitialize, FlightPlanStart) /** @@ -99,195 +99,151 @@ MODULE_INITCALL(FlightPlanInitialize, FlightPlanStart) */ static void flightPlanTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; - PmReturn_t retval; - FlightPlanStatusData status; - FlightPlanControlData control; + UAVObjEvent ev; + PmReturn_t retval; + FlightPlanStatusData status; + FlightPlanControlData control; - // Setup status object - status.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; - status.ErrorFileID = 0; - status.ErrorLineNum = 0; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; - status.Debug[0] = 0.0; - status.Debug[1] = 0.0; - FlightPlanStatusSet(&status); + // Setup status object + status.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; + status.ErrorFileID = 0; + status.ErrorLineNum = 0; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; + status.Debug[0] = 0.0; + status.Debug[1] = 0.0; + FlightPlanStatusSet(&status); - // Main thread loop - while (1) - { - // Wait for FlightPlanControl updates - while (xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE) ; + // Main thread loop + while (1) { + // Wait for FlightPlanControl updates + while (xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE) { + ; + } - // Get object and check if a start command was sent - FlightPlanControlGet(&control); - if ( control.Command == FLIGHTPLANCONTROL_COMMAND_START ) - { - // Init PyMite - retval = pm_init(MEMSPACE_PROG, usrlib_img); - if (retval == PM_RET_OK) - { - // Update status - FlightPlanStatusGet(&status); - status.Status = FLIGHTPLANSTATUS_STATUS_RUNNING; - FlightPlanStatusSet(&status); - // Run the test script (TODO: load from SD card) - retval = pm_run((uint8_t *)"test"); - // Check if an error or exception was thrown - if (retval == PM_RET_OK || retval == PM_RET_EX_EXIT) - { - status.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; - } - else if (retval == PM_RET_EX) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_EXCEPTION; - } - else if (retval == PM_RET_EX_IO) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IOERROR; - } - else if (retval == PM_RET_EX_ZDIV) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_DIVBYZERO; - } - else if (retval == PM_RET_EX_ASSRT) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ASSERTERROR; - } - else if (retval == PM_RET_EX_ATTR) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ATTRIBUTEERROR; - } - else if (retval == PM_RET_EX_IMPRT) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IMPORTERROR; - } - else if (retval == PM_RET_EX_INDX) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_INDEXERROR; - } - else if (retval == PM_RET_EX_KEY) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_KEYERROR; - } - else if (retval == PM_RET_EX_MEM) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_MEMORYERROR; - } - else if (retval == PM_RET_EX_NAME) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NAMEERROR; - } - else if (retval == PM_RET_EX_SYNTAX) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYNTAXERROR; - } - else if (retval == PM_RET_EX_SYS) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYSTEMERROR; - } - else if (retval == PM_RET_EX_TYPE) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_TYPEERROR; - } - else if (retval == PM_RET_EX_VAL) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VALUEERROR; - } - else if (retval == PM_RET_EX_STOP) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_STOPITERATION; - } - else if (retval == PM_RET_EX_WARN) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_WARNING; - } - else - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_UNKNOWNERROR; - } - // Get file ID and line number of error (if one) - status.ErrorFileID = gVmGlobal.errFileId; - status.ErrorLineNum = gVmGlobal.errLineNum; - } - else - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VMINITERROR; - } + // Get object and check if a start command was sent + FlightPlanControlGet(&control); + if (control.Command == FLIGHTPLANCONTROL_COMMAND_START) { + // Init PyMite + retval = pm_init(MEMSPACE_PROG, usrlib_img); + if (retval == PM_RET_OK) { + // Update status + FlightPlanStatusGet(&status); + status.Status = FLIGHTPLANSTATUS_STATUS_RUNNING; + FlightPlanStatusSet(&status); + // Run the test script (TODO: load from SD card) + retval = pm_run((uint8_t *)"test"); + // Check if an error or exception was thrown + if (retval == PM_RET_OK || retval == PM_RET_EX_EXIT) { + status.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; + } else if (retval == PM_RET_EX) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_EXCEPTION; + } else if (retval == PM_RET_EX_IO) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IOERROR; + } else if (retval == PM_RET_EX_ZDIV) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_DIVBYZERO; + } else if (retval == PM_RET_EX_ASSRT) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ASSERTERROR; + } else if (retval == PM_RET_EX_ATTR) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ATTRIBUTEERROR; + } else if (retval == PM_RET_EX_IMPRT) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IMPORTERROR; + } else if (retval == PM_RET_EX_INDX) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_INDEXERROR; + } else if (retval == PM_RET_EX_KEY) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_KEYERROR; + } else if (retval == PM_RET_EX_MEM) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_MEMORYERROR; + } else if (retval == PM_RET_EX_NAME) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NAMEERROR; + } else if (retval == PM_RET_EX_SYNTAX) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYNTAXERROR; + } else if (retval == PM_RET_EX_SYS) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYSTEMERROR; + } else if (retval == PM_RET_EX_TYPE) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_TYPEERROR; + } else if (retval == PM_RET_EX_VAL) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VALUEERROR; + } else if (retval == PM_RET_EX_STOP) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_STOPITERATION; + } else if (retval == PM_RET_EX_WARN) { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_WARNING; + } else { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_UNKNOWNERROR; + } + // Get file ID and line number of error (if one) + status.ErrorFileID = gVmGlobal.errFileId; + status.ErrorLineNum = gVmGlobal.errLineNum; + } else { + status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; + status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VMINITERROR; + } - // Update status object - FlightPlanStatusSet(&status); - } - } + // Update status object + FlightPlanStatusSet(&status); + } + } } /** * Function called in response to object updates. * Used to force kill the VM thread. */ -static void objectUpdatedCb(UAVObjEvent * ev) +static void objectUpdatedCb(UAVObjEvent *ev) { - FlightPlanControlData controlData; - FlightPlanStatusData statusData; + FlightPlanControlData controlData; + FlightPlanStatusData statusData; - // If the object updated was the FlightPlanControl execute requested action - if ( ev->obj == FlightPlanControlHandle() ) - { - // Get data - FlightPlanControlGet(&controlData); - // Execute command - if ( controlData.Command == FLIGHTPLANCONTROL_COMMAND_START ) - { - // Start VM task if not running already - if ( taskHandle == NULL ) - { - xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle); - } - } - else if ( controlData.Command == FLIGHTPLANCONTROL_COMMAND_KILL ) - { - // Force kill VM task if it is already running - // (NOTE: the STOP command is preferred as it allows the script to terminate without killing the VM) - if ( taskHandle != NULL ) - { - // Kill VM - PIOS_TASK_MONITOR_UnregisterTask(TASKINFO_RUNNING_FLIGHTPLAN); - vTaskDelete(taskHandle); - taskHandle = NULL; - // Update status object - statusData.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; - statusData.ErrorFileID = 0; - statusData.ErrorLineNum = 0; - statusData.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; - statusData.Debug[0] = 0.0; - statusData.Debug[1] = 0.0; - FlightPlanStatusSet(&statusData); - } - } - } + // If the object updated was the FlightPlanControl execute requested action + if (ev->obj == FlightPlanControlHandle()) { + // Get data + FlightPlanControlGet(&controlData); + // Execute command + if (controlData.Command == FLIGHTPLANCONTROL_COMMAND_START) { + // Start VM task if not running already + if (taskHandle == NULL) { + xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle); + } + } else if (controlData.Command == FLIGHTPLANCONTROL_COMMAND_KILL) { + // Force kill VM task if it is already running + // (NOTE: the STOP command is preferred as it allows the script to terminate without killing the VM) + if (taskHandle != NULL) { + // Kill VM + PIOS_TASK_MONITOR_UnregisterTask(TASKINFO_RUNNING_FLIGHTPLAN); + vTaskDelete(taskHandle); + taskHandle = NULL; + // Update status object + statusData.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; + statusData.ErrorFileID = 0; + statusData.ErrorLineNum = 0; + statusData.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; + statusData.Debug[0] = 0.0; + statusData.Debug[1] = 0.0; + FlightPlanStatusSet(&statusData); + } + } + } } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index d241998a9..0fdb9142e 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup GSPModule GPS Module * @brief Process GPS information - * @{ + * @{ * * @file GPS.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -56,28 +56,28 @@ static void gpsTask(void *parameters); static void updateSettings(); #ifdef PIOS_GPS_SETS_HOMELOCATION -static void setHomeLocation(GPSPositionData * gpsData); +static void setHomeLocation(GPSPositionData *gpsData); static float GravityAccel(float latitude, float longitude, float altitude); #endif // **************** // Private constants -#define GPS_TIMEOUT_MS 500 +#define GPS_TIMEOUT_MS 500 #ifdef PIOS_GPS_SETS_HOMELOCATION // Unfortunately need a good size stack for the WMM calculation - #define STACK_SIZE_BYTES 850 + #define STACK_SIZE_BYTES 850 #else #if defined(PIOS_GPS_MINIMAL) - #define STACK_SIZE_BYTES 500 + #define STACK_SIZE_BYTES 500 #else - #define STACK_SIZE_BYTES 650 + #define STACK_SIZE_BYTES 650 #endif // PIOS_GPS_MINIMAL #endif // PIOS_GPS_SETS_HOMELOCATION -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // **************** // Private variables @@ -87,7 +87,7 @@ static bool gpsEnabled = false; static xTaskHandle gpsTaskHandle; -static char* gps_rx_buffer; +static char *gps_rx_buffer; static uint32_t timeOfLastCommandMs; static uint32_t timeOfLastUpdateMs; @@ -105,17 +105,17 @@ static struct GPS_RX_STATS gpsRxStats; int32_t GPSStart(void) { - if (gpsEnabled) { - if (gpsPort) { - // Start gps task - xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &gpsTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle); - return 0; - } + if (gpsEnabled) { + if (gpsPort) { + // Start gps task + xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &gpsTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle); + return 0; + } - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); - } - return -1; + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); + } + return -1; } /** @@ -125,68 +125,69 @@ int32_t GPSStart(void) */ int32_t GPSInitialize(void) { - gpsPort = PIOS_COM_GPS; - uint8_t gpsProtocol; + gpsPort = PIOS_COM_GPS; + uint8_t gpsProtocol; #ifdef MODULE_GPS_BUILTIN - gpsEnabled = true; + gpsEnabled = true; #else - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_GPS] == HWSETTINGS_OPTIONALMODULES_ENABLED) - gpsEnabled = true; - else - gpsEnabled = false; + if (optionalModules[HWSETTINGS_OPTIONALMODULES_GPS] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + gpsEnabled = true; + } else { + gpsEnabled = false; + } #endif #if defined(REVOLUTION) - // These objects MUST be initialized for Revolution - // because the rest of the system expects to just - // attach to their queues - GPSPositionInitialize(); - GPSVelocityInitialize(); - GPSTimeInitialize(); - GPSSatellitesInitialize(); - HomeLocationInitialize(); - updateSettings(); + // These objects MUST be initialized for Revolution + // because the rest of the system expects to just + // attach to their queues + GPSPositionInitialize(); + GPSVelocityInitialize(); + GPSTimeInitialize(); + GPSSatellitesInitialize(); + HomeLocationInitialize(); + updateSettings(); #else - if (gpsPort && gpsEnabled) { - GPSPositionInitialize(); - GPSVelocityInitialize(); + if (gpsPort && gpsEnabled) { + GPSPositionInitialize(); + GPSVelocityInitialize(); #if !defined(PIOS_GPS_MINIMAL) - GPSTimeInitialize(); - GPSSatellitesInitialize(); + GPSTimeInitialize(); + GPSSatellitesInitialize(); #endif #ifdef PIOS_GPS_SETS_HOMELOCATION - HomeLocationInitialize(); -#endif - updateSettings(); - } + HomeLocationInitialize(); #endif + updateSettings(); + } +#endif /* if defined(REVOLUTION) */ - if (gpsPort && gpsEnabled) { - SystemSettingsInitialize(); - SystemSettingsGPSDataProtocolGet(&gpsProtocol); - switch (gpsProtocol) { - case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA: - gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); - break; - case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX: - gps_rx_buffer = pvPortMalloc(sizeof(struct UBXPacket)); - break; - default: - gps_rx_buffer = NULL; - } - PIOS_Assert(gps_rx_buffer); + if (gpsPort && gpsEnabled) { + SystemSettingsInitialize(); + SystemSettingsGPSDataProtocolGet(&gpsProtocol); + switch (gpsProtocol) { + case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA: + gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); + break; + case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX: + gps_rx_buffer = pvPortMalloc(sizeof(struct UBXPacket)); + break; + default: + gps_rx_buffer = NULL; + } + PIOS_Assert(gps_rx_buffer); - return 0; - } + return 0; + } - return -1; + return -1; } MODULE_INITCALL(GPSInitialize, GPSStart) @@ -198,79 +199,78 @@ MODULE_INITCALL(GPSInitialize, GPSStart) static void gpsTask(__attribute__((unused)) void *parameters) { - portTickType xDelay = 100 / portTICK_RATE_MS; - uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + portTickType xDelay = 100 / portTICK_RATE_MS; + uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - GPSPositionData gpsposition; - uint8_t gpsProtocol; + GPSPositionData gpsposition; + uint8_t gpsProtocol; - SystemSettingsGPSDataProtocolGet(&gpsProtocol); + SystemSettingsGPSDataProtocolGet(&gpsProtocol); - timeOfLastUpdateMs = timeNowMs; - timeOfLastCommandMs = timeNowMs; + timeOfLastUpdateMs = timeNowMs; + timeOfLastCommandMs = timeNowMs; - GPSPositionGet(&gpsposition); - // Loop forever - while (1) - { - uint8_t c; + GPSPositionGet(&gpsposition); + // Loop forever + while (1) { + uint8_t c; - // This blocks the task until there is something on the buffer - while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) - { - int res; - switch (gpsProtocol) { + // This blocks the task until there is something on the buffer + while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) { + int res; + switch (gpsProtocol) { #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) - case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA: - res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats); - break; + case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA: + res = parse_nmea_stream(c, gps_rx_buffer, &gpsposition, &gpsRxStats); + break; #endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) - case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX: - res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats); - break; + case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX: + res = parse_ubx_stream(c, gps_rx_buffer, &gpsposition, &gpsRxStats); + break; #endif - default: - res = NO_PARSER; // this should not happen - break; - } + default: + res = NO_PARSER; // this should not happen + break; + } - if (res == PARSER_COMPLETE) { - timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - timeOfLastUpdateMs = timeNowMs; - timeOfLastCommandMs = timeNowMs; - } - } + if (res == PARSER_COMPLETE) { + timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + timeOfLastUpdateMs = timeNowMs; + timeOfLastCommandMs = timeNowMs; + } + } - // Check for GPS timeout - timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) { - // we have not received any valid GPS sentences for a while. - // either the GPS is not plugged in or a hardware problem or the GPS has locked up. - uint8_t status = GPSPOSITION_STATUS_NOGPS; - GPSPositionStatusSet(&status); - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); - } else { - // we appear to be receiving GPS sentences OK, we've had an update - //criteria for GPS-OK taken from this post... - //http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 - if ((gpsposition.PDOP < 3.5f) && (gpsposition.Satellites >= 7) && - (gpsposition.Status == GPSPOSITION_STATUS_FIX3D)) { - AlarmsClear(SYSTEMALARMS_ALARM_GPS); + // Check for GPS timeout + timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) { + // we have not received any valid GPS sentences for a while. + // either the GPS is not plugged in or a hardware problem or the GPS has locked up. + uint8_t status = GPSPOSITION_STATUS_NOGPS; + GPSPositionStatusSet(&status); + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); + } else { + // we appear to be receiving GPS sentences OK, we've had an update + // criteria for GPS-OK taken from this post... + // http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 + if ((gpsposition.PDOP < 3.5f) && (gpsposition.Satellites >= 7) && + (gpsposition.Status == GPSPOSITION_STATUS_FIX3D)) { + AlarmsClear(SYSTEMALARMS_ALARM_GPS); #ifdef PIOS_GPS_SETS_HOMELOCATION - HomeLocationData home; - HomeLocationGet(&home); + HomeLocationData home; + HomeLocationGet(&home); - if (home.Set == HOMELOCATION_SET_FALSE) - setHomeLocation(&gpsposition); + if (home.Set == HOMELOCATION_SET_FALSE) { + setHomeLocation(&gpsposition); + } #endif - } else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D) - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); - else - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); - } - - } + } else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D) { + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); + } + } + } } #ifdef PIOS_GPS_SETS_HOMELOCATION @@ -279,51 +279,51 @@ static void gpsTask(__attribute__((unused)) void *parameters) */ static float GravityAccel(float latitude, __attribute__((unused)) float longitude, float altitude) { - /* WGS84 gravity model. The effect of gravity over latitude is strong - * enough to change the estimated accelerometer bias in those apps. */ - float sinsq = sinf(latitude); - sinsq *= sinsq; - /* Likewise, over the altitude range of a high-altitude balloon, the effect - * due to change in altitude can also affect the model. */ - return (9.7803267714f * (1.0f + 0.00193185138639f*sinsq) / sqrtf(1.0f - 0.00669437999013f*sinsq) - - 3.086e-6f*altitude); + /* WGS84 gravity model. The effect of gravity over latitude is strong + * enough to change the estimated accelerometer bias in those apps. */ + float sinsq = sinf(latitude); + + sinsq *= sinsq; + /* Likewise, over the altitude range of a high-altitude balloon, the effect + * due to change in altitude can also affect the model. */ + return 9.7803267714f * (1.0f + 0.00193185138639f * sinsq) / sqrtf(1.0f - 0.00669437999013f * sinsq) + - 3.086e-6f * altitude; } // **************** -static void setHomeLocation(GPSPositionData * gpsData) +static void setHomeLocation(GPSPositionData *gpsData) { - HomeLocationData home; - HomeLocationGet(&home); - GPSTimeData gps; - GPSTimeGet(&gps); + HomeLocationData home; - if (gps.Year >= 2000) - { - /* Store LLA */ - home.Latitude = gpsData->Latitude; - home.Longitude = gpsData->Longitude; - home.Altitude = gpsData->Altitude + gpsData->GeoidSeparation; + HomeLocationGet(&home); + GPSTimeData gps; + GPSTimeGet(&gps); - /* Compute home ECEF coordinates and the rotation matrix into NED - * Note that floats are used here - they should give enough precision - * for this application.*/ + if (gps.Year >= 2000) { + /* Store LLA */ + home.Latitude = gpsData->Latitude; + home.Longitude = gpsData->Longitude; + home.Altitude = gpsData->Altitude + gpsData->GeoidSeparation; - float LLA[3] = { (home.Latitude)/10e6f, (home.Longitude)/10e6f, (home.Altitude) }; + /* Compute home ECEF coordinates and the rotation matrix into NED + * Note that floats are used here - they should give enough precision + * for this application.*/ - /* Compute magnetic flux direction at home location */ - if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) == 0) { + float LLA[3] = { (home.Latitude) / 10e6f, (home.Longitude) / 10e6f, (home.Altitude) }; - /*Compute local acceleration due to gravity. Vehicles that span a very large - * range of altitude (say, weather balloons) may need to update this during the - * flight. */ - home.g_e = GravityAccel(LLA[0], LLA[1], LLA[2]); - home.Set = HOMELOCATION_SET_TRUE; - HomeLocationSet(&home); - } - } + /* Compute magnetic flux direction at home location */ + if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) == 0) { + /*Compute local acceleration due to gravity. Vehicles that span a very large + * range of altitude (say, weather balloons) may need to update this during the + * flight. */ + home.g_e = GravityAccel(LLA[0], LLA[1], LLA[2]); + home.Set = HOMELOCATION_SET_TRUE; + HomeLocationSet(&home); + } + } } -#endif +#endif /* ifdef PIOS_GPS_SETS_HOMELOCATION */ /** * Update the GPS settings, called on startup. @@ -334,40 +334,39 @@ static void setHomeLocation(GPSPositionData * gpsData) */ static void updateSettings() { - if (gpsPort) { + if (gpsPort) { + // Retrieve settings + uint8_t speed; + HwSettingsGPSSpeedGet(&speed); - // Retrieve settings - uint8_t speed; - HwSettingsGPSSpeedGet(&speed); - - // Set port speed - switch (speed) { - case HWSETTINGS_GPSSPEED_2400: - PIOS_COM_ChangeBaud(gpsPort, 2400); - break; - case HWSETTINGS_GPSSPEED_4800: - PIOS_COM_ChangeBaud(gpsPort, 4800); - break; - case HWSETTINGS_GPSSPEED_9600: - PIOS_COM_ChangeBaud(gpsPort, 9600); - break; - case HWSETTINGS_GPSSPEED_19200: - PIOS_COM_ChangeBaud(gpsPort, 19200); - break; - case HWSETTINGS_GPSSPEED_38400: - PIOS_COM_ChangeBaud(gpsPort, 38400); - break; - case HWSETTINGS_GPSSPEED_57600: - PIOS_COM_ChangeBaud(gpsPort, 57600); - break; - case HWSETTINGS_GPSSPEED_115200: - PIOS_COM_ChangeBaud(gpsPort, 115200); - break; - } - } + // Set port speed + switch (speed) { + case HWSETTINGS_GPSSPEED_2400: + PIOS_COM_ChangeBaud(gpsPort, 2400); + break; + case HWSETTINGS_GPSSPEED_4800: + PIOS_COM_ChangeBaud(gpsPort, 4800); + break; + case HWSETTINGS_GPSSPEED_9600: + PIOS_COM_ChangeBaud(gpsPort, 9600); + break; + case HWSETTINGS_GPSSPEED_19200: + PIOS_COM_ChangeBaud(gpsPort, 19200); + break; + case HWSETTINGS_GPSSPEED_38400: + PIOS_COM_ChangeBaud(gpsPort, 38400); + break; + case HWSETTINGS_GPSSPEED_57600: + PIOS_COM_ChangeBaud(gpsPort, 57600); + break; + case HWSETTINGS_GPSSPEED_115200: + PIOS_COM_ChangeBaud(gpsPort, 115200); + break; + } + } } -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/modules/GPS/NMEA.c b/flight/modules/GPS/NMEA.c index 98cfe48cc..d5be3131d 100644 --- a/flight/modules/GPS/NMEA.c +++ b/flight/modules/GPS/NMEA.c @@ -39,24 +39,23 @@ #include "gpssatellites.h" #include "GPS.h" -//#define ENABLE_DEBUG_MSG ///< define to enable debug-messages -#define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is ued for debug-messages - +// #define ENABLE_DEBUG_MSG ///< define to enable debug-messages +#define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is ued for debug-messages // Debugging #ifdef ENABLE_DEBUG_MSG -//#define DEBUG_MSG_IN ///< define to display the incoming NMEA messages -//#define DEBUG_PARAMS ///< define to display the incoming NMEA messages split into its parameters -//#define DEBUG_MGSID_IN ///< define to display the the names of the incoming NMEA messages -//#define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages -//#define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages -//#define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages -//#define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages -//#define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages -//#define NMEA_DEBUG_GSV ///< define to enable debug of GSV messages -//#define NMEA_DEBUG_ZDA ///< define to enable debug of ZDA messages -#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__) +// #define DEBUG_MSG_IN ///< define to display the incoming NMEA messages +// #define DEBUG_PARAMS ///< define to display the incoming NMEA messages split into its parameters +// #define DEBUG_MGSID_IN ///< define to display the the names of the incoming NMEA messages +// #define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages +// #define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages +// #define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages +// #define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages +// #define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages +// #define NMEA_DEBUG_GSV ///< define to enable debug of GSV messages +// #define NMEA_DEBUG_ZDA ///< define to enable debug of ZDA messages +#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format,##__VA_ARGS__) #else #define DEBUG_MSG(format, ...) #endif @@ -65,148 +64,137 @@ /* NMEA sentence parsers */ struct nmea_parser { - const char *prefix; - bool(*handler) (GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); + const char *prefix; + bool (*handler)(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); }; -static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); -static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); -static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); -static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); +static bool nmeaProcessGPGGA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +static bool nmeaProcessGPRMC(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +static bool nmeaProcessGPVTG(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +static bool nmeaProcessGPGSA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); #if !defined(PIOS_GPS_MINIMAL) - static bool nmeaProcessGPZDA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); -#endif //PIOS_GPS_MINIMAL +static bool nmeaProcessGPZDA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +static bool nmeaProcessGPGSV(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +#endif // PIOS_GPS_MINIMAL static const struct nmea_parser nmea_parsers[] = { - { - .prefix = "GPGGA", - .handler = nmeaProcessGPGGA, - }, - { - .prefix = "GPVTG", - .handler = nmeaProcessGPVTG, - }, - { - .prefix = "GPGSA", - .handler = nmeaProcessGPGSA, - }, - { - .prefix = "GPRMC", - .handler = nmeaProcessGPRMC, - }, + { + .prefix = "GPGGA", + .handler = nmeaProcessGPGGA, + }, + { + .prefix = "GPVTG", + .handler = nmeaProcessGPVTG, + }, + { + .prefix = "GPGSA", + .handler = nmeaProcessGPGSA, + }, + { + .prefix = "GPRMC", + .handler = nmeaProcessGPRMC, + }, #if !defined(PIOS_GPS_MINIMAL) - { - .prefix = "GPZDA", - .handler = nmeaProcessGPZDA, - }, - { - .prefix = "GPGSV", - .handler = nmeaProcessGPGSV, - }, -#endif //PIOS_GPS_MINIMAL + { + .prefix = "GPZDA", + .handler = nmeaProcessGPZDA, + }, + { + .prefix = "GPGSV", + .handler = nmeaProcessGPGSV, + }, +#endif // PIOS_GPS_MINIMAL }; -int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats) +int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats) { - static uint8_t rx_count = 0; - static bool start_flag = false; - static bool found_cr = false; + static uint8_t rx_count = 0; + static bool start_flag = false; + static bool found_cr = false; - // detect start while acquiring stream - if (!start_flag && (c == '$')) // NMEA identifier found - { - start_flag = true; - found_cr = false; - rx_count = 0; - } - else - if (!start_flag) - return PARSER_ERROR; + // detect start while acquiring stream + if (!start_flag && (c == '$')) { // NMEA identifier found + start_flag = true; + found_cr = false; + rx_count = 0; + } else if (!start_flag) { + return PARSER_ERROR; + } - if (rx_count >= NMEA_MAX_PACKET_LENGTH) - { - // The buffer is already full and we haven't found a valid NMEA sentence. - // Flush the buffer and note the overflow event. - gpsRxStats->gpsRxOverflow++; - start_flag = false; - found_cr = false; - rx_count = 0; - return PARSER_OVERRUN; - } - else - { - gps_rx_buffer[rx_count] = c; - rx_count++; - } + if (rx_count >= NMEA_MAX_PACKET_LENGTH) { + // The buffer is already full and we haven't found a valid NMEA sentence. + // Flush the buffer and note the overflow event. + gpsRxStats->gpsRxOverflow++; + start_flag = false; + found_cr = false; + rx_count = 0; + return PARSER_OVERRUN; + } else { + gps_rx_buffer[rx_count] = c; + rx_count++; + } - // look for ending '\r\n' sequence - if (!found_cr && (c == '\r') ) - found_cr = true; - else - if (found_cr && (c != '\n') ) - found_cr = false; // false end flag - else - if (found_cr && (c == '\n') ) - { - // The NMEA functions require a zero-terminated string - // As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n - gps_rx_buffer[rx_count-2] = 0; + // look for ending '\r\n' sequence + if (!found_cr && (c == '\r')) { + found_cr = true; + } else if (found_cr && (c != '\n')) { + found_cr = false; // false end flag + } else if (found_cr && (c == '\n')) { + // The NMEA functions require a zero-terminated string + // As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n + gps_rx_buffer[rx_count - 2] = 0; - // prepare to parse next sentence - start_flag = false; - found_cr = false; - rx_count = 0; - // Our rxBuffer must look like this now: - // [0] = '$' - // ... = zero or more bytes of sentence payload - // [end_pos - 1] = '\r' - // [end_pos] = '\n' - // - // Prepare to consume the sentence from the buffer + // prepare to parse next sentence + start_flag = false; + found_cr = false; + rx_count = 0; + // Our rxBuffer must look like this now: + // [0] = '$' + // ... = zero or more bytes of sentence payload + // [end_pos - 1] = '\r' + // [end_pos] = '\n' + // + // Prepare to consume the sentence from the buffer - // Validate the checksum over the sentence - if (!NMEA_checksum(&gps_rx_buffer[1])) - { // Invalid checksum. May indicate dropped characters on Rx. - //PIOS_DEBUG_PinHigh(2); - gpsRxStats->gpsRxChkSumError++; - //PIOS_DEBUG_PinLow(2); - return PARSER_ERROR; - } - else - { // Valid checksum, use this packet to update the GPS position - if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) { - //PIOS_DEBUG_PinHigh(2); - gpsRxStats->gpsRxParserError++; - //PIOS_DEBUG_PinLow(2); - } - else - gpsRxStats->gpsRxReceived++;; + // Validate the checksum over the sentence + if (!NMEA_checksum(&gps_rx_buffer[1])) { // Invalid checksum. May indicate dropped characters on Rx. + // PIOS_DEBUG_PinHigh(2); + gpsRxStats->gpsRxChkSumError++; + // PIOS_DEBUG_PinLow(2); + return PARSER_ERROR; + } else { // Valid checksum, use this packet to update the GPS position + if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) { + // PIOS_DEBUG_PinHigh(2); + gpsRxStats->gpsRxParserError++; + // PIOS_DEBUG_PinLow(2); + } else { + gpsRxStats->gpsRxReceived++; + }; - return PARSER_COMPLETE; - } - } - return PARSER_INCOMPLETE; + return PARSER_COMPLETE; + } + } + return PARSER_INCOMPLETE; } static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix) { - if (!prefix) { - return (NULL); - } + if (!prefix) { + return NULL; + } - for (uint8_t i = 0; i < NELEMENTS(nmea_parsers); i++) { - const struct nmea_parser *parser = &nmea_parsers[i]; + for (uint8_t i = 0; i < NELEMENTS(nmea_parsers); i++) { + const struct nmea_parser *parser = &nmea_parsers[i]; - /* Use strcmp to check for exact equality over the entire prefix */ - if (!strcmp(prefix, parser->prefix)) { - /* Found an appropriate parser */ - return (parser); - } - } + /* Use strcmp to check for exact equality over the entire prefix */ + if (!strcmp(prefix, parser->prefix)) { + /* Found an appropriate parser */ + return parser; + } + } - /* No matching parser for this prefix */ - return (NULL); + /* No matching parser for this prefix */ + return NULL; } /** @@ -217,26 +205,26 @@ static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix) */ bool NMEA_checksum(char *nmea_sentence) { - uint8_t checksum_computed = 0; - uint8_t checksum_received; + uint8_t checksum_computed = 0; + uint8_t checksum_received; - while (*nmea_sentence != '\0' && *nmea_sentence != '*') { - checksum_computed ^= *nmea_sentence; - nmea_sentence++; - } + while (*nmea_sentence != '\0' && *nmea_sentence != '*') { + checksum_computed ^= *nmea_sentence; + nmea_sentence++; + } - /* Make sure we're now pointing at the checksum */ - if (*nmea_sentence == '\0') { - /* Buffer ran out before we found a checksum marker */ - return false; - } + /* Make sure we're now pointing at the checksum */ + if (*nmea_sentence == '\0') { + /* Buffer ran out before we found a checksum marker */ + return false; + } - /* Load the checksum from the buffer */ - checksum_received = strtol(nmea_sentence + 1, NULL, 16); + /* Load the checksum from the buffer */ + checksum_received = strtol(nmea_sentence + 1, NULL, 16); - //PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum_computed); + // PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum_computed); - return (checksum_computed == checksum_received); + return checksum_computed == checksum_received; } /* @@ -252,49 +240,49 @@ bool NMEA_checksum(char *nmea_sentence) * The fract_units field indicates the units of the fractional part as * 1 whole = 10^fract_units fract */ -static bool NMEA_parse_real(int32_t * whole, uint32_t * fract, uint8_t * fract_units, char *field) +static bool NMEA_parse_real(int32_t *whole, uint32_t *fract, uint8_t *fract_units, char *field) { - char *s = field; - char *field_w; - char *field_f; + char *s = field; + char *field_w; + char *field_f; - PIOS_DEBUG_Assert(whole); - PIOS_DEBUG_Assert(fract); - PIOS_DEBUG_Assert(fract_units); + PIOS_DEBUG_Assert(whole); + PIOS_DEBUG_Assert(fract); + PIOS_DEBUG_Assert(fract_units); - field_w = strsep(&s, "."); - field_f = s; + field_w = strsep(&s, "."); + field_f = s; - *whole = strtol(field_w, NULL, 10); + *whole = strtol(field_w, NULL, 10); - if (field_w) { - /* decimal was found so we may have a fractional part */ - *fract = strtoul(field_f, NULL, 10); - *fract_units = strlen(field_f); - } else { - /* no decimal was found, fractional part is zero */ - *fract = 0; - *fract_units = 0; - } + if (field_w) { + /* decimal was found so we may have a fractional part */ + *fract = strtoul(field_f, NULL, 10); + *fract_units = strlen(field_f); + } else { + /* no decimal was found, fractional part is zero */ + *fract = 0; + *fract_units = 0; + } - return true; + return true; } static float NMEA_real_to_float(char *nmea_real) { - int32_t whole; - uint32_t fract; - uint8_t fract_units; + int32_t whole; + uint32_t fract; + uint8_t fract_units; - /* Sanity checks */ - PIOS_DEBUG_Assert(nmea_real); + /* Sanity checks */ + PIOS_DEBUG_Assert(nmea_real); - if (!NMEA_parse_real(&whole, &fract, &fract_units, nmea_real)) { - return false; - } + if (!NMEA_parse_real(&whole, &fract, &fract_units, nmea_real)) { + return false; + } - /* Convert to float */ - return (((float)whole) + fract * powf(10.0f, -fract_units)); + /* Convert to float */ + return ((float)whole) + fract * powf(10.0f, -fract_units); } /* @@ -302,62 +290,63 @@ static float NMEA_real_to_float(char *nmea_real) * DD[D]MM.mmmm[mm] * into a fixed-point representation in units of (degrees * 1e-7) */ -static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool negative) +static bool NMEA_latlon_to_fixed_point(int32_t *latlon, char *nmea_latlon, bool negative) { - int32_t num_DDDMM; - uint32_t num_m; - uint8_t units; + int32_t num_DDDMM; + uint32_t num_m; + uint8_t units; - /* Sanity checks */ - PIOS_DEBUG_Assert(nmea_latlon); - PIOS_DEBUG_Assert(latlon); + /* Sanity checks */ + PIOS_DEBUG_Assert(nmea_latlon); + PIOS_DEBUG_Assert(latlon); - if (*nmea_latlon == '\0') { /* empty lat/lon field */ - return false; - } + if (*nmea_latlon == '\0') { /* empty lat/lon field */ + return false; + } - if (!NMEA_parse_real(&num_DDDMM, &num_m, &units, nmea_latlon)) { - return false; - } + if (!NMEA_parse_real(&num_DDDMM, &num_m, &units, nmea_latlon)) { + return false; + } - /* scale up the mmmm[mm] field apropriately depending on # of digits */ - /* not using 1eN notation because that forces fixed point and lost precision */ - switch (units) { - case 0: - /* no digits, value is zero so no scaling */ - break; - case 1: /* m */ - num_m *= 1000000; /* m000000 */ - break; - case 2: /* mm */ - num_m *= 100000; /* mm00000 */ - break; - case 3: /* mmm */ - num_m *= 10000; /* mmm0000 */ - break; - case 4: /* mmmm */ - num_m *= 1000; /* mmmm000 */ - break; - case 5: /* mmmmm */ - num_m *= 100; /* mmmmm00 */ - break; - case 6: /* mmmmmm */ - num_m *= 10; /* mmmmmm0 */ - break; - default: - /* unhandled format */ - num_m = 0.0f; - break; - } + /* scale up the mmmm[mm] field apropriately depending on # of digits */ + /* not using 1eN notation because that forces fixed point and lost precision */ + switch (units) { + case 0: + /* no digits, value is zero so no scaling */ + break; + case 1: /* m */ + num_m *= 1000000; /* m000000 */ + break; + case 2: /* mm */ + num_m *= 100000; /* mm00000 */ + break; + case 3: /* mmm */ + num_m *= 10000; /* mmm0000 */ + break; + case 4: /* mmmm */ + num_m *= 1000; /* mmmm000 */ + break; + case 5: /* mmmmm */ + num_m *= 100; /* mmmmm00 */ + break; + case 6: /* mmmmmm */ + num_m *= 10; /* mmmmmm0 */ + break; + default: + /* unhandled format */ + num_m = 0.0f; + break; + } - *latlon = (num_DDDMM / 100) * 10000000; /* scale the whole degrees */ - *latlon += (num_DDDMM % 100) * 10000000 / 60; /* add in the scaled decimal whole minutes */ - *latlon += num_m / 60; /* add in the scaled decimal fractional minutes */ + *latlon = (num_DDDMM / 100) * 10000000; /* scale the whole degrees */ + *latlon += (num_DDDMM % 100) * 10000000 / 60; /* add in the scaled decimal whole minutes */ + *latlon += num_m / 60; /* add in the scaled decimal fractional minutes */ - if (negative) - *latlon *= -1; + if (negative) { + *latlon *= -1; + } - return true; + return true; } @@ -369,86 +358,87 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool */ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) { - char* p = nmea_sentence; - char* params[MAX_NB_PARAMS]; - uint8_t nbParams; + char *p = nmea_sentence; + char *params[MAX_NB_PARAMS]; + uint8_t nbParams; #ifdef DEBUG_MSG_IN - DEBUG_MSG("\"%s\"\n", nmea_sentence); + DEBUG_MSG("\"%s\"\n", nmea_sentence); #endif - // Split the nmea sentence it its parameters, separated by "," - // Sample NMEA message: "GPRMC,000131.736,V,,,,,0.00,0.00,060180,,,N*43" + // Split the nmea sentence it its parameters, separated by "," + // Sample NMEA message: "GPRMC,000131.736,V,,,,,0.00,0.00,060180,,,N*43" - // The first parameter starts at the beginning of the message - params[0] = p; - nbParams = 1; - while (*p != 0) { - if (*p == '*') { - // After the * comes the "CRC", we are done, - *p = 0; // Zero-terminate this parameter - break; - } else if (*p == ',') { - // This is the end of this parameter - *p = 0; // Zero-terminate this parameter - // Start new parameter - if (nbParams==MAX_NB_PARAMS) - break; - params[nbParams] = p+1; // For sure there is something at p+1 because at p there is "," - nbParams++; - } - p++; - } + // The first parameter starts at the beginning of the message + params[0] = p; + nbParams = 1; + while (*p != 0) { + if (*p == '*') { + // After the * comes the "CRC", we are done, + *p = 0; // Zero-terminate this parameter + break; + } else if (*p == ',') { + // This is the end of this parameter + *p = 0; // Zero-terminate this parameter + // Start new parameter + if (nbParams == MAX_NB_PARAMS) { + break; + } + params[nbParams] = p + 1; // For sure there is something at p+1 because at p there is "," + nbParams++; + } + p++; + } #ifdef DEBUG_PARAMS - int i; - for (i=0;ihandler(GpsData, &gpsDataUpdated, params, nbParams)) { - // Parse failed - DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]); - if (gpsDataUpdated && (GpsData->Status == GPSPOSITION_STATUS_NOFIX)) { - GPSPositionSet(GpsData); - } - return false; - } + if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) { + // Parse failed + DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]); + if (gpsDataUpdated && (GpsData->Status == GPSPOSITION_STATUS_NOFIX)) { + GPSPositionSet(GpsData); + } + return false; + } - // All is fine :) Update object if data has changed - if (gpsDataUpdated) { - #ifdef DEBUG_MGSID_IN - DEBUG_MSG("U"); - #endif - GPSPositionSet(GpsData); - } + // All is fine :) Update object if data has changed + if (gpsDataUpdated) { + #ifdef DEBUG_MGSID_IN + DEBUG_MSG("U"); + #endif + GPSPositionSet(GpsData); + } - #ifdef DEBUG_MGSID_IN - DEBUG_MSG("\n"); - #endif - return true; + #ifdef DEBUG_MGSID_IN + DEBUG_MSG("\n"); + #endif + return true; } @@ -457,52 +447,52 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) * \param[in] A pointer to a GPSPosition UAVObject to be updated. * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam) +static bool nmeaProcessGPGGA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { - - if (nbParam != 15) - return false; + if (nbParam != 15) { + return false; + } #ifdef NMEA_DEBUG_GGA - DEBUG_MSG("\n UTC=%s\n", param[1]); - DEBUG_MSG(" Lat=%s %s\n", param[2], param[3]); - DEBUG_MSG(" Long=%s %s\n", param[4], param[5]); - DEBUG_MSG(" Fix=%s\n", param[6]); - DEBUG_MSG(" Sat=%s\n", param[7]); - DEBUG_MSG(" HDOP=%s\n", param[8]); - DEBUG_MSG(" Alt=%s %s\n", param[9], param[10]); - DEBUG_MSG(" GeoidSep=%s %s\n\n", param[11]); + DEBUG_MSG("\n UTC=%s\n", param[1]); + DEBUG_MSG(" Lat=%s %s\n", param[2], param[3]); + DEBUG_MSG(" Long=%s %s\n", param[4], param[5]); + DEBUG_MSG(" Fix=%s\n", param[6]); + DEBUG_MSG(" Sat=%s\n", param[7]); + DEBUG_MSG(" HDOP=%s\n", param[8]); + DEBUG_MSG(" Alt=%s %s\n", param[9], param[10]); + DEBUG_MSG(" GeoidSep=%s %s\n\n", param[11]); #endif - *gpsDataUpdated = true; + *gpsDataUpdated = true; - // check for invalid GPS fix - // do this first to make sure we get this information, even if later checks exit - // this function early - if (param[6][0] == '0') { - GpsData->Status = GPSPOSITION_STATUS_NOFIX; // treat invalid fix as NOFIX - } + // check for invalid GPS fix + // do this first to make sure we get this information, even if later checks exit + // this function early + if (param[6][0] == '0') { + GpsData->Status = GPSPOSITION_STATUS_NOFIX; // treat invalid fix as NOFIX + } - // get latitude [DDMM.mmmmm] [N|S] - if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[2], param[3][0] == 'S')) { - return false; - } + // get latitude [DDMM.mmmmm] [N|S] + if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[2], param[3][0] == 'S')) { + return false; + } - // get longitude [dddmm.mmmmm] [E|W] - if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, param[4], param[5][0] == 'W')) { - return false; - } + // get longitude [dddmm.mmmmm] [E|W] + if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, param[4], param[5][0] == 'W')) { + return false; + } - // get number of satellites used in GPS solution - GpsData->Satellites = atoi(param[7]); + // get number of satellites used in GPS solution + GpsData->Satellites = atoi(param[7]); - // get altitude (in meters mm.m) - GpsData->Altitude = NMEA_real_to_float(param[9]); + // get altitude (in meters mm.m) + GpsData->Altitude = NMEA_real_to_float(param[9]); - // geoid separation - GpsData->GeoidSeparation = NMEA_real_to_float(param[11]); + // geoid separation + GpsData->GeoidSeparation = NMEA_real_to_float(param[11]); - return true; + return true; } /** @@ -510,66 +500,67 @@ static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch * \param[in] A pointer to a GPSPosition UAVObject to be updated. * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam) +static bool nmeaProcessGPRMC(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { - if (nbParam != 13) - return false; + if (nbParam != 13) { + return false; + } #ifdef NMEA_DEBUG_RMC - DEBUG_MSG("\n UTC=%s\n", param[1]); - DEBUG_MSG(" Lat=%s %s\n", param[3], param[4]); - DEBUG_MSG(" Long=%s %s\n", param[5], param[6]); - DEBUG_MSG(" Speed=%s\n", param[7]); - DEBUG_MSG(" Course=%s\n", param[8]); - DEBUG_MSG(" DateOfFix=%s\n\n", param[9]); + DEBUG_MSG("\n UTC=%s\n", param[1]); + DEBUG_MSG(" Lat=%s %s\n", param[3], param[4]); + DEBUG_MSG(" Long=%s %s\n", param[5], param[6]); + DEBUG_MSG(" Speed=%s\n", param[7]); + DEBUG_MSG(" Course=%s\n", param[8]); + DEBUG_MSG(" DateOfFix=%s\n\n", param[9]); #endif - *gpsDataUpdated = false; + *gpsDataUpdated = false; #if !defined(PIOS_GPS_MINIMAL) - GPSTimeData gpst; - GPSTimeGet(&gpst); + GPSTimeData gpst; + GPSTimeGet(&gpst); - // get UTC time [hhmmss.sss] - float hms = NMEA_real_to_float(param[1]); - gpst.Second = (int)hms % 100; - gpst.Minute = (((int)hms - gpst.Second) / 100) % 100; - gpst.Hour = (int)hms / 10000; -#endif //PIOS_GPS_MINIMAL + // get UTC time [hhmmss.sss] + float hms = NMEA_real_to_float(param[1]); + gpst.Second = (int)hms % 100; + gpst.Minute = (((int)hms - gpst.Second) / 100) % 100; + gpst.Hour = (int)hms / 10000; +#endif // PIOS_GPS_MINIMAL - // don't process void sentences - if (param[2][0] == 'V') { - return false; - } + // don't process void sentences + if (param[2][0] == 'V') { + return false; + } - // get latitude [DDMM.mmmmm] [N|S] - if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[3], param[4][0] == 'S')) { - return false; - } + // get latitude [DDMM.mmmmm] [N|S] + if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[3], param[4][0] == 'S')) { + return false; + } - // get longitude [dddmm.mmmmm] [E|W] - if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, param[5], param[6][0] == 'W')) { - return false; - } + // get longitude [dddmm.mmmmm] [E|W] + if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, param[5], param[6][0] == 'W')) { + return false; + } - // get speed in knots - GpsData->Groundspeed = NMEA_real_to_float(param[7]) * 0.51444f; // to m/s + // get speed in knots + GpsData->Groundspeed = NMEA_real_to_float(param[7]) * 0.51444f; // to m/s - // get True course - GpsData->Heading = NMEA_real_to_float(param[8]); + // get True course + GpsData->Heading = NMEA_real_to_float(param[8]); #if !defined(PIOS_GPS_MINIMAL) - // get Date of fix - // TODO: Should really not use a float here to be safe - float date = NMEA_real_to_float(param[9]); - gpst.Year = (int)date % 100; - gpst.Month = (((int)date - gpst.Year) / 100) % 100; - gpst.Day = (int)(date / 10000); - gpst.Year += 2000; - GPSTimeSet(&gpst); -#endif //PIOS_GPS_MINIMAL + // get Date of fix + // TODO: Should really not use a float here to be safe + float date = NMEA_real_to_float(param[9]); + gpst.Year = (int)date % 100; + gpst.Month = (((int)date - gpst.Year) / 100) % 100; + gpst.Day = (int)(date / 10000); + gpst.Year += 2000; + GPSTimeSet(&gpst); +#endif // PIOS_GPS_MINIMAL - return true; + return true; } /** @@ -577,22 +568,23 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, ch * \param[in] A pointer to a GPSPosition UAVObject to be updated. * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam) +static bool nmeaProcessGPVTG(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { - if (nbParam != 9 && nbParam != 10 /*GTOP GPS seems to gemnerate an extra parameter...*/) - return false; + if (nbParam != 9 && nbParam != 10 /*GTOP GPS seems to gemnerate an extra parameter...*/) { + return false; + } #ifdef NMEA_DEBUG_RMC - DEBUG_MSG("\n Heading=%s %s\n", param[1], param[2]); - DEBUG_MSG(" GroundSpeed=%s %s\n", param[5], param[6]); + DEBUG_MSG("\n Heading=%s %s\n", param[1], param[2]); + DEBUG_MSG(" GroundSpeed=%s %s\n", param[5], param[6]); #endif - *gpsDataUpdated = false; + *gpsDataUpdated = false; - GpsData->Heading = NMEA_real_to_float(param[1]); - GpsData->Groundspeed = NMEA_real_to_float(param[5]) * 0.51444f; // to m/s + GpsData->Heading = NMEA_real_to_float(param[1]); + GpsData->Groundspeed = NMEA_real_to_float(param[5]) * 0.51444f; // to m/s - return true; + return true; } #if !defined(PIOS_GPS_MINIMAL) @@ -601,35 +593,36 @@ static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, ch * \param[in] A pointer to a GPSPosition UAVObject to be updated (unused). * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPZDA(__attribute__((unused)) GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam) +static bool nmeaProcessGPZDA(__attribute__((unused)) GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { - if (nbParam != 7) - return false; + if (nbParam != 7) { + return false; + } - #ifdef NMEA_DEBUG_ZDA - DEBUG_MSG("\n Time=%s (hhmmss.ss)\n", param[1]); - DEBUG_MSG(" Date=%s/%s/%s (d/m/y)\n", param[2], param[3], param[4]); - #endif + #ifdef NMEA_DEBUG_ZDA + DEBUG_MSG("\n Time=%s (hhmmss.ss)\n", param[1]); + DEBUG_MSG(" Date=%s/%s/%s (d/m/y)\n", param[2], param[3], param[4]); + #endif - *gpsDataUpdated = false; // Here we will never provide a new GPS value + *gpsDataUpdated = false; // Here we will never provide a new GPS value - // No new data data extracted - GPSTimeData gpst; - GPSTimeGet(&gpst); + // No new data data extracted + GPSTimeData gpst; + GPSTimeGet(&gpst); - // get UTC time [hhmmss.sss] - float hms = NMEA_real_to_float(param[1]); - gpst.Second = (int)hms % 100; - gpst.Minute = (((int)hms - gpst.Second) / 100) % 100; - gpst.Hour = (int)hms / 10000; + // get UTC time [hhmmss.sss] + float hms = NMEA_real_to_float(param[1]); + gpst.Second = (int)hms % 100; + gpst.Minute = (((int)hms - gpst.Second) / 100) % 100; + gpst.Hour = (int)hms / 10000; - // Get Date - gpst.Day = atoi(param[2]); - gpst.Month = atoi(param[3]); - gpst.Year = atoi(param[4]); + // Get Date + gpst.Day = atoi(param[2]); + gpst.Month = atoi(param[3]); + gpst.Year = atoi(param[4]); - GPSTimeSet(&gpst); - return true; + GPSTimeSet(&gpst); + return true; } static GPSSatellitesData gsv_partial; @@ -640,131 +633,135 @@ static uint8_t gsv_processed_mask; static uint16_t gsv_incomplete_error; static uint16_t gsv_duplicate_error; -static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam) +static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { - if (nbParam < 4) - return false; + if (nbParam < 4) { + return false; + } #ifdef NMEA_DEBUG_GSV - DEBUG_MSG("\n Sentence=%s/%s\n", param[2], param[1]); - DEBUG_MSG(" Sats=%s\n", param[3]); + DEBUG_MSG("\n Sentence=%s/%s\n", param[2], param[1]); + DEBUG_MSG(" Sats=%s\n", param[3]); #endif - uint8_t nbSentences = atoi(param[1]); - uint8_t currSentence = atoi(param[2]); + uint8_t nbSentences = atoi(param[1]); + uint8_t currSentence = atoi(param[2]); - *gpsDataUpdated = false; + *gpsDataUpdated = false; - if (nbSentences < 1 || nbSentences > 8 || currSentence < 1 || currSentence > nbSentences) - return false; + if (nbSentences < 1 || nbSentences > 8 || currSentence < 1 || currSentence > nbSentences) { + return false; + } - gsv_partial.SatsInView = atoi(param[3]); + gsv_partial.SatsInView = atoi(param[3]); - // Find out if this is the first sentence in the GSV set - if (currSentence == 1) { - if (gsv_expected_mask != gsv_processed_mask) { - // We are starting over when we haven't yet finished our previous GSV group - gsv_incomplete_error++; - } + // Find out if this is the first sentence in the GSV set + if (currSentence == 1) { + if (gsv_expected_mask != gsv_processed_mask) { + // We are starting over when we haven't yet finished our previous GSV group + gsv_incomplete_error++; + } - // First GSV sentence in the sequence, reset our expected_mask - gsv_expected_mask = (1 << nbSentences) - 1; - } + // First GSV sentence in the sequence, reset our expected_mask + gsv_expected_mask = (1 << nbSentences) - 1; + } - uint8_t current_sentence_id = (1 << (currSentence - 1)); - if (gsv_processed_mask & current_sentence_id) { - /* Duplicate sentence in this GSV set */ - gsv_duplicate_error++; - } else { - /* Note that we've seen this sentence */ - gsv_processed_mask |= current_sentence_id; - } + uint8_t current_sentence_id = (1 << (currSentence - 1)); + if (gsv_processed_mask & current_sentence_id) { + /* Duplicate sentence in this GSV set */ + gsv_duplicate_error++; + } else { + /* Note that we've seen this sentence */ + gsv_processed_mask |= current_sentence_id; + } - uint8_t parIdx = 4; + uint8_t parIdx = 4; #ifdef NMEA_DEBUG_GSV - DEBUG_MSG(" PRN:"); + DEBUG_MSG(" PRN:"); #endif - /* Make sure this sentence can fit in our GPSSatellites object */ - if ((currSentence * 4) <= NELEMENTS(gsv_partial.PRN)) { - /* Process 4 blocks of satellite info */ - for (uint8_t i = 0; parIdx+4 <= nbParam && i < 4; i++) { - uint8_t sat_index = ((currSentence - 1) * 4) + i; + /* Make sure this sentence can fit in our GPSSatellites object */ + if ((currSentence * 4) <= NELEMENTS(gsv_partial.PRN)) { + /* Process 4 blocks of satellite info */ + for (uint8_t i = 0; parIdx + 4 <= nbParam && i < 4; i++) { + uint8_t sat_index = ((currSentence - 1) * 4) + i; - // Get sat info - gsv_partial.PRN[sat_index] = atoi(param[parIdx++]); - gsv_partial.Elevation[sat_index] = NMEA_real_to_float(param[parIdx++]); - gsv_partial.Azimuth[sat_index] = NMEA_real_to_float(param[parIdx++]); - gsv_partial.SNR[sat_index] = atoi(param[parIdx++]); + // Get sat info + gsv_partial.PRN[sat_index] = atoi(param[parIdx++]); + gsv_partial.Elevation[sat_index] = NMEA_real_to_float(param[parIdx++]); + gsv_partial.Azimuth[sat_index] = NMEA_real_to_float(param[parIdx++]); + gsv_partial.SNR[sat_index] = atoi(param[parIdx++]); #ifdef NMEA_DEBUG_GSV - DEBUG_MSG(" %d", gsv_partial.PRN[sat_index]); + DEBUG_MSG(" %d", gsv_partial.PRN[sat_index]); #endif - } - } + } + } #ifdef NMEA_DEBUG_GSV - DEBUG_MSG("\n"); + DEBUG_MSG("\n"); #endif - /* Find out if we're finished processing all GSV sentences in the set */ - if ((gsv_expected_mask != 0) && (gsv_processed_mask == gsv_expected_mask)) { - /* GSV set has been fully processed. Update the GPSSatellites object. */ - GPSSatellitesSet(&gsv_partial); - memset((void *)&gsv_partial, 0, sizeof(gsv_partial)); - gsv_expected_mask = 0; - gsv_processed_mask = 0; - } + /* Find out if we're finished processing all GSV sentences in the set */ + if ((gsv_expected_mask != 0) && (gsv_processed_mask == gsv_expected_mask)) { + /* GSV set has been fully processed. Update the GPSSatellites object. */ + GPSSatellitesSet(&gsv_partial); + memset((void *)&gsv_partial, 0, sizeof(gsv_partial)); + gsv_expected_mask = 0; + gsv_processed_mask = 0; + } - return true; + return true; } -#endif //PIOS_GPS_MINIMAL +#endif // PIOS_GPS_MINIMAL /** * Parse an NMEA GPGSA sentence and update the given UAVObject * \param[in] A pointer to a GPSPosition UAVObject to be updated. * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam) +static bool nmeaProcessGPGSA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { - if (nbParam != 18) - return false; + if (nbParam != 18) { + return false; + } #ifdef NMEA_DEBUG_GSA - DEBUG_MSG("\n Status=%s\n", param[2]); - DEBUG_MSG(" PDOP=%s\n", param[15]); - DEBUG_MSG(" HDOP=%s\n", param[16]); - DEBUG_MSG(" VDOP=%s\n", param[17]); + DEBUG_MSG("\n Status=%s\n", param[2]); + DEBUG_MSG(" PDOP=%s\n", param[15]); + DEBUG_MSG(" HDOP=%s\n", param[16]); + DEBUG_MSG(" VDOP=%s\n", param[17]); #endif - *gpsDataUpdated = false; + *gpsDataUpdated = false; - switch (atoi(param[2])) { - case 1: - GpsData->Status = GPSPOSITION_STATUS_NOFIX; - break; - case 2: - GpsData->Status = GPSPOSITION_STATUS_FIX2D; - break; - case 3: - GpsData->Status = GPSPOSITION_STATUS_FIX3D; - break; - default: - /* Unhandled */ - return false; - break; - } + switch (atoi(param[2])) { + case 1: + GpsData->Status = GPSPOSITION_STATUS_NOFIX; + break; + case 2: + GpsData->Status = GPSPOSITION_STATUS_FIX2D; + break; + case 3: + GpsData->Status = GPSPOSITION_STATUS_FIX3D; + break; + default: + /* Unhandled */ + return false; - // next field: PDOP - GpsData->PDOP = NMEA_real_to_float(param[15]); + break; + } - // next field: HDOP - GpsData->HDOP = NMEA_real_to_float(param[16]); + // next field: PDOP + GpsData->PDOP = NMEA_real_to_float(param[15]); - // next field: VDOP - GpsData->VDOP = NMEA_real_to_float(param[17]); + // next field: HDOP + GpsData->HDOP = NMEA_real_to_float(param[16]); - return true; + // next field: VDOP + GpsData->VDOP = NMEA_real_to_float(param[17]); + + return true; } #endif // PIOS_INCLUDE_GPS_NMEA_PARSER diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 92d5e08bf..bf8fe8039 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -38,304 +38,306 @@ // parse incoming character stream for messages in UBX binary format -int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats) +int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats) { - enum proto_states { - START, - UBX_SY2, - UBX_CLASS, - UBX_ID, - UBX_LEN1, - UBX_LEN2, - UBX_PAYLOAD, - UBX_CHK1, - UBX_CHK2, - FINISHED - }; + enum proto_states { + START, + UBX_SY2, + UBX_CLASS, + UBX_ID, + UBX_LEN1, + UBX_LEN2, + UBX_PAYLOAD, + UBX_CHK1, + UBX_CHK2, + FINISHED + }; - static enum proto_states proto_state = START; - static uint8_t rx_count = 0; - struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer; + static enum proto_states proto_state = START; + static uint8_t rx_count = 0; + struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer; - switch (proto_state) { - case START: // detect protocol - if (c == UBX_SYNC1) // first UBX sync char found - proto_state = UBX_SY2; - break; - case UBX_SY2: - if (c == UBX_SYNC2) // second UBX sync char found - proto_state = UBX_CLASS; - else - proto_state = START; // reset state - break; - case UBX_CLASS: - ubx->header.class = c; - proto_state = UBX_ID; - break; - case UBX_ID: - ubx->header.id = c; - proto_state = UBX_LEN1; - break; - case UBX_LEN1: - ubx->header.len = c; - proto_state = UBX_LEN2; - break; - case UBX_LEN2: - ubx->header.len += (c << 8); - if (ubx->header.len > sizeof(UBXPayload)) { - gpsRxStats->gpsRxOverflow++; - proto_state = START; - } else { - rx_count = 0; - proto_state = UBX_PAYLOAD; - } - break; - case UBX_PAYLOAD: - if (rx_count < ubx->header.len) { - ubx->payload.payload[rx_count] = c; - if (++rx_count == ubx->header.len) - proto_state = UBX_CHK1; - } else { - gpsRxStats->gpsRxOverflow++; - proto_state = START; - } - break; - case UBX_CHK1: - ubx->header.ck_a = c; - proto_state = UBX_CHK2; - break; - case UBX_CHK2: - ubx->header.ck_b = c; - if (checksum_ubx_message(ubx)) { // message complete and valid - parse_ubx_message(ubx, GpsData); - proto_state = FINISHED; - } else { - gpsRxStats->gpsRxChkSumError++; - proto_state = START; - } - break; - default: break; - } + switch (proto_state) { + case START: // detect protocol + if (c == UBX_SYNC1) { // first UBX sync char found + proto_state = UBX_SY2; + } + break; + case UBX_SY2: + if (c == UBX_SYNC2) { // second UBX sync char found + proto_state = UBX_CLASS; + } else { + proto_state = START; // reset state + } + break; + case UBX_CLASS: + ubx->header.class = c; + proto_state = UBX_ID; + break; + case UBX_ID: + ubx->header.id = c; + proto_state = UBX_LEN1; + break; + case UBX_LEN1: + ubx->header.len = c; + proto_state = UBX_LEN2; + break; + case UBX_LEN2: + ubx->header.len += (c << 8); + if (ubx->header.len > sizeof(UBXPayload)) { + gpsRxStats->gpsRxOverflow++; + proto_state = START; + } else { + rx_count = 0; + proto_state = UBX_PAYLOAD; + } + break; + case UBX_PAYLOAD: + if (rx_count < ubx->header.len) { + ubx->payload.payload[rx_count] = c; + if (++rx_count == ubx->header.len) { + proto_state = UBX_CHK1; + } + } else { + gpsRxStats->gpsRxOverflow++; + proto_state = START; + } + break; + case UBX_CHK1: + ubx->header.ck_a = c; + proto_state = UBX_CHK2; + break; + case UBX_CHK2: + ubx->header.ck_b = c; + if (checksum_ubx_message(ubx)) { // message complete and valid + parse_ubx_message(ubx, GpsData); + proto_state = FINISHED; + } else { + gpsRxStats->gpsRxChkSumError++; + proto_state = START; + } + break; + default: break; + } - if (proto_state == START) - return PARSER_ERROR; // parser couldn't use this byte - else if (proto_state == FINISHED) { - gpsRxStats->gpsRxReceived++; - proto_state = START; - return PARSER_COMPLETE; // message complete & processed - } + if (proto_state == START) { + return PARSER_ERROR; // parser couldn't use this byte + } else if (proto_state == FINISHED) { + gpsRxStats->gpsRxReceived++; + proto_state = START; + return PARSER_COMPLETE; // message complete & processed + } - return PARSER_INCOMPLETE; // message not (yet) complete + return PARSER_INCOMPLETE; // message not (yet) complete } // Keep track of various GPS messages needed to make up a single UAVO update // time-of-week timestamp is used to correlate matching messages -#define POSLLH_RECEIVED (1 << 0) -#define STATUS_RECEIVED (1 << 1) -#define DOP_RECEIVED (1 << 2) -#define VELNED_RECEIVED (1 << 3) -#define SOL_RECEIVED (1 << 4) -#define ALL_RECEIVED (SOL_RECEIVED | VELNED_RECEIVED | DOP_RECEIVED | POSLLH_RECEIVED) -#define NONE_RECEIVED 0 +#define POSLLH_RECEIVED (1 << 0) +#define STATUS_RECEIVED (1 << 1) +#define DOP_RECEIVED (1 << 2) +#define VELNED_RECEIVED (1 << 3) +#define SOL_RECEIVED (1 << 4) +#define ALL_RECEIVED (SOL_RECEIVED | VELNED_RECEIVED | DOP_RECEIVED | POSLLH_RECEIVED) +#define NONE_RECEIVED 0 -static struct msgtracker{ - uint32_t currentTOW; // TOW of the message set currently in progress - uint8_t msg_received; // keep track of received message types - } msgtracker; +static struct msgtracker { + uint32_t currentTOW; // TOW of the message set currently in progress + uint8_t msg_received; // keep track of received message types +} msgtracker; // Check if a message belongs to the current data set and register it as 'received' -bool check_msgtracker (uint32_t tow, uint8_t msg_flag) +bool check_msgtracker(uint32_t tow, uint8_t msg_flag) { + if (tow > msgtracker.currentTOW ? true // start of a new message set + : (msgtracker.currentTOW - tow > 6 * 24 * 3600 * 1000)) { // 6 days, TOW wrap around occured + msgtracker.currentTOW = tow; + msgtracker.msg_received = NONE_RECEIVED; + } else if (tow < msgtracker.currentTOW) { // message outdated (don't process) + return false; + } - if (tow > msgtracker.currentTOW ? true // start of a new message set - : (msgtracker.currentTOW - tow > 6*24*3600*1000)) { // 6 days, TOW wrap around occured - msgtracker.currentTOW = tow; - msgtracker.msg_received = NONE_RECEIVED; - } else if (tow < msgtracker.currentTOW) // message outdated (don't process) - return false; - - msgtracker.msg_received |= msg_flag; // register reception of this msg type - return true; + msgtracker.msg_received |= msg_flag; // register reception of this msg type + return true; } -bool checksum_ubx_message (struct UBXPacket *ubx) +bool checksum_ubx_message(struct UBXPacket *ubx) { - int i; - uint8_t ck_a, ck_b; + int i; + uint8_t ck_a, ck_b; - ck_a = ubx->header.class; - ck_b = ck_a; + ck_a = ubx->header.class; + ck_b = ck_a; - ck_a += ubx->header.id; - ck_b += ck_a; + ck_a += ubx->header.id; + ck_b += ck_a; - ck_a += ubx->header.len & 0xff; - ck_b += ck_a; + ck_a += ubx->header.len & 0xff; + ck_b += ck_a; - ck_a += ubx->header.len >> 8; - ck_b += ck_a; + ck_a += ubx->header.len >> 8; + ck_b += ck_a; - for (i = 0; i < ubx->header.len; i++) { - ck_a += ubx->payload.payload[i]; - ck_b += ck_a; - } - - if (ubx->header.ck_a == ck_a && - ubx->header.ck_b == ck_b) - return true; - else - return false; + for (i = 0; i < ubx->header.len; i++) { + ck_a += ubx->payload.payload[i]; + ck_b += ck_a; + } + if (ubx->header.ck_a == ck_a && + ubx->header.ck_b == ck_b) { + return true; + } else { + return false; + } } -void parse_ubx_nav_posllh (struct UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPosition) +void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPosition) { - if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) { - if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { - GpsPosition->Altitude = (float)posllh->hMSL*0.001f; - GpsPosition->GeoidSeparation = (float)(posllh->height - posllh->hMSL)*0.001f; - GpsPosition->Latitude = posllh->lat; - GpsPosition->Longitude = posllh->lon; - } - } + if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) { + if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { + GpsPosition->Altitude = (float)posllh->hMSL * 0.001f; + GpsPosition->GeoidSeparation = (float)(posllh->height - posllh->hMSL) * 0.001f; + GpsPosition->Latitude = posllh->lat; + GpsPosition->Longitude = posllh->lon; + } + } } -void parse_ubx_nav_sol (struct UBX_NAV_SOL *sol, GPSPositionData *GpsPosition) +void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionData *GpsPosition) { - if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) { - GpsPosition->Satellites = sol->numSV; + if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) { + GpsPosition->Satellites = sol->numSV; - if (sol->flags & STATUS_FLAGS_GPSFIX_OK) { - switch (sol->gpsFix) { - case STATUS_GPSFIX_2DFIX: - GpsPosition->Status = GPSPOSITION_STATUS_FIX2D; - break; - case STATUS_GPSFIX_3DFIX: - GpsPosition->Status = GPSPOSITION_STATUS_FIX3D; - break; - default: GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; - } - } - else // fix is not valid so we make sure to treat is as NOFIX - GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; - } + if (sol->flags & STATUS_FLAGS_GPSFIX_OK) { + switch (sol->gpsFix) { + case STATUS_GPSFIX_2DFIX: + GpsPosition->Status = GPSPOSITION_STATUS_FIX2D; + break; + case STATUS_GPSFIX_3DFIX: + GpsPosition->Status = GPSPOSITION_STATUS_FIX3D; + break; + default: GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; + } + } else { // fix is not valid so we make sure to treat is as NOFIX + GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; + } + } } -void parse_ubx_nav_dop (struct UBX_NAV_DOP *dop, GPSPositionData *GpsPosition) +void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionData *GpsPosition) { - if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) { - GpsPosition->HDOP = (float)dop->hDOP * 0.01f; - GpsPosition->VDOP = (float)dop->vDOP * 0.01f; - GpsPosition->PDOP = (float)dop->pDOP * 0.01f; - } + if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) { + GpsPosition->HDOP = (float)dop->hDOP * 0.01f; + GpsPosition->VDOP = (float)dop->vDOP * 0.01f; + GpsPosition->PDOP = (float)dop->pDOP * 0.01f; + } } -void parse_ubx_nav_velned (struct UBX_NAV_VELNED *velned, GPSPositionData *GpsPosition) +void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionData *GpsPosition) { - GPSVelocityData GpsVelocity; + GPSVelocityData GpsVelocity; - if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) { - if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { - GpsVelocity.North = (float)velned->velN/100.0f; - GpsVelocity.East = (float)velned->velE/100.0f; - GpsVelocity.Down = (float)velned->velD/100.0f; - GPSVelocitySet(&GpsVelocity); - GpsPosition->Groundspeed = (float)velned->gSpeed * 0.01f; - GpsPosition->Heading = (float)velned->heading * 1.0e-5f; - } - } + if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) { + if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { + GpsVelocity.North = (float)velned->velN / 100.0f; + GpsVelocity.East = (float)velned->velE / 100.0f; + GpsVelocity.Down = (float)velned->velD / 100.0f; + GPSVelocitySet(&GpsVelocity); + GpsPosition->Groundspeed = (float)velned->gSpeed * 0.01f; + GpsPosition->Heading = (float)velned->heading * 1.0e-5f; + } + } } #if !defined(PIOS_GPS_MINIMAL) -void parse_ubx_nav_timeutc (struct UBX_NAV_TIMEUTC *timeutc) +void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc) { - if (!(timeutc->valid & TIMEUTC_VALIDUTC)) - return; + if (!(timeutc->valid & TIMEUTC_VALIDUTC)) { + return; + } - GPSTimeData GpsTime; + GPSTimeData GpsTime; - GpsTime.Year = timeutc->year; - GpsTime.Month = timeutc->month; - GpsTime.Day = timeutc->day; - GpsTime.Hour = timeutc->hour; - GpsTime.Minute = timeutc->min; - GpsTime.Second = timeutc->sec; + GpsTime.Year = timeutc->year; + GpsTime.Month = timeutc->month; + GpsTime.Day = timeutc->day; + GpsTime.Hour = timeutc->hour; + GpsTime.Minute = timeutc->min; + GpsTime.Second = timeutc->sec; - GPSTimeSet(&GpsTime); + GPSTimeSet(&GpsTime); } #endif #if !defined(PIOS_GPS_MINIMAL) -void parse_ubx_nav_svinfo (struct UBX_NAV_SVINFO *svinfo) +void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo) { - uint8_t chan; - GPSSatellitesData svdata; + uint8_t chan; + GPSSatellitesData svdata; - svdata.SatsInView = 0; - for (chan = 0; chan < svinfo->numCh; chan++) { - if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) { - svdata.Azimuth[svdata.SatsInView] = (float)svinfo->sv[chan].azim; - svdata.Elevation[svdata.SatsInView] = (float)svinfo->sv[chan].elev; - svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid; - svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno; - svdata.SatsInView++; - } + svdata.SatsInView = 0; + for (chan = 0; chan < svinfo->numCh; chan++) { + if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) { + svdata.Azimuth[svdata.SatsInView] = (float)svinfo->sv[chan].azim; + svdata.Elevation[svdata.SatsInView] = (float)svinfo->sv[chan].elev; + svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid; + svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno; + svdata.SatsInView++; + } + } + // fill remaining slots (if any) + for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) { + svdata.Azimuth[chan] = (float)0.0f; + svdata.Elevation[chan] = (float)0.0f; + svdata.PRN[chan] = 0; + svdata.SNR[chan] = 0; + } - } - // fill remaining slots (if any) - for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) { - svdata.Azimuth[chan] = (float)0.0f; - svdata.Elevation[chan] = (float)0.0f; - svdata.PRN[chan] = 0; - svdata.SNR[chan] = 0; - } - - GPSSatellitesSet(&svdata); + GPSSatellitesSet(&svdata); } -#endif +#endif /* if !defined(PIOS_GPS_MINIMAL) */ // UBX message parser // returns UAVObjectID if a UAVObject structure is ready for further processing -uint32_t parse_ubx_message (struct UBXPacket *ubx, GPSPositionData *GpsPosition) +uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionData *GpsPosition) { - uint32_t id = 0; + uint32_t id = 0; - switch (ubx->header.class) { - case UBX_CLASS_NAV: - switch (ubx->header.id) { - case UBX_ID_POSLLH: - parse_ubx_nav_posllh (&ubx->payload.nav_posllh, GpsPosition); - break; - case UBX_ID_DOP: - parse_ubx_nav_dop (&ubx->payload.nav_dop, GpsPosition); - break; - case UBX_ID_SOL: - parse_ubx_nav_sol (&ubx->payload.nav_sol, GpsPosition); - break; - case UBX_ID_VELNED: - parse_ubx_nav_velned (&ubx->payload.nav_velned, GpsPosition); - break; + switch (ubx->header.class) { + case UBX_CLASS_NAV: + switch (ubx->header.id) { + case UBX_ID_POSLLH: + parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition); + break; + case UBX_ID_DOP: + parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition); + break; + case UBX_ID_SOL: + parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition); + break; + case UBX_ID_VELNED: + parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition); + break; #if !defined(PIOS_GPS_MINIMAL) - case UBX_ID_TIMEUTC: - parse_ubx_nav_timeutc (&ubx->payload.nav_timeutc); - break; - case UBX_ID_SVINFO: - parse_ubx_nav_svinfo (&ubx->payload.nav_svinfo); - break; + case UBX_ID_TIMEUTC: + parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc); + break; + case UBX_ID_SVINFO: + parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo); + break; #endif - } - break; - } - if (msgtracker.msg_received == ALL_RECEIVED) { - GPSPositionSet(GpsPosition); - msgtracker.msg_received = NONE_RECEIVED; - id = GPSPOSITION_OBJID; - } - return id; + } + break; + } + if (msgtracker.msg_received == ALL_RECEIVED) { + GPSPositionSet(GpsPosition); + msgtracker.msg_received = NONE_RECEIVED; + id = GPSPOSITION_OBJID; + } + return id; } #endif // PIOS_INCLUDE_GPS_UBX_PARSER - diff --git a/flight/modules/GPS/inc/GPS.h b/flight/modules/GPS/inc/GPS.h index 6e243c21f..ecc7470d8 100644 --- a/flight/modules/GPS/inc/GPS.h +++ b/flight/modules/GPS/inc/GPS.h @@ -1,16 +1,16 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup GSPModule GPS Module * @brief Process GPS information - * @{ + * @{ * * @file GPS.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Include file of the GPS module. - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and * objects. * @see The GNU Public License (GPL) Version 3 * @@ -39,17 +39,17 @@ #include "gpsposition.h" #include "gpstime.h" -#define NO_PARSER -3 // no parser available -#define PARSER_OVERRUN -2 // message buffer overrun before completing the message -#define PARSER_ERROR -1 // message unparsable by this parser -#define PARSER_INCOMPLETE 0 // parser needs more data to complete the message -#define PARSER_COMPLETE 1 // parser has received a complete message and finished processing +#define NO_PARSER -3 // no parser available +#define PARSER_OVERRUN -2 // message buffer overrun before completing the message +#define PARSER_ERROR -1 // message unparsable by this parser +#define PARSER_INCOMPLETE 0 // parser needs more data to complete the message +#define PARSER_COMPLETE 1 // parser has received a complete message and finished processing struct GPS_RX_STATS { - uint16_t gpsRxReceived; - uint16_t gpsRxChkSumError; - uint16_t gpsRxOverflow; - uint16_t gpsRxParserError; + uint16_t gpsRxReceived; + uint16_t gpsRxChkSumError; + uint16_t gpsRxOverflow; + uint16_t gpsRxParserError; }; int32_t GPSInitialize(void); @@ -57,6 +57,6 @@ int32_t GPSInitialize(void); #endif // GPS_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/GPS/inc/NMEA.h b/flight/modules/GPS/inc/NMEA.h index 49213e716..a1e94a24e 100644 --- a/flight/modules/GPS/inc/NMEA.h +++ b/flight/modules/GPS/inc/NMEA.h @@ -35,7 +35,7 @@ #include #include "GPS.h" -#define NMEA_MAX_PACKET_LENGTH 96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed +#define NMEA_MAX_PACKET_LENGTH 96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed extern bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData); extern bool NMEA_checksum(char *nmea_sentence); diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index f7718bb3c..9a1c86eeb 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -35,190 +35,190 @@ #include "GPS.h" -#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters -#define UBX_SYNC2 0x62 +#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters +#define UBX_SYNC2 0x62 // From u-blox6 receiver protocol specification // Messages classes -#define UBX_CLASS_NAV 0x01 +#define UBX_CLASS_NAV 0x01 // Message IDs -#define UBX_ID_POSLLH 0x02 -#define UBX_ID_STATUS 0x03 -#define UBX_ID_DOP 0x04 -#define UBX_ID_SOL 0x06 -#define UBX_ID_VELNED 0x12 -#define UBX_ID_TIMEUTC 0x21 -#define UBX_ID_SVINFO 0x30 +#define UBX_ID_POSLLH 0x02 +#define UBX_ID_STATUS 0x03 +#define UBX_ID_DOP 0x04 +#define UBX_ID_SOL 0x06 +#define UBX_ID_VELNED 0x12 +#define UBX_ID_TIMEUTC 0x21 +#define UBX_ID_SVINFO 0x30 // private structures // Geodetic Position Solution struct UBX_NAV_POSLLH { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - int32_t lon; // Longitude (deg*1e-7) - int32_t lat; // Latitude (deg*1e-7) - int32_t height; // Height above Ellipsoid (mm) - int32_t hMSL; // Height above mean sea level (mm) - uint32_t hAcc; // Horizontal Accuracy Estimate (mm) - uint32_t vAcc; // Vertical Accuracy Estimate (mm) + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + int32_t lon; // Longitude (deg*1e-7) + int32_t lat; // Latitude (deg*1e-7) + int32_t height; // Height above Ellipsoid (mm) + int32_t hMSL; // Height above mean sea level (mm) + uint32_t hAcc; // Horizontal Accuracy Estimate (mm) + uint32_t vAcc; // Vertical Accuracy Estimate (mm) }; // Receiver Navigation Status -#define STATUS_GPSFIX_NOFIX 0x00 -#define STATUS_GPSFIX_DRONLY 0x01 -#define STATUS_GPSFIX_2DFIX 0x02 -#define STATUS_GPSFIX_3DFIX 0x03 -#define STATUS_GPSFIX_GPSDR 0x04 -#define STATUS_GPSFIX_TIMEONLY 0x05 +#define STATUS_GPSFIX_NOFIX 0x00 +#define STATUS_GPSFIX_DRONLY 0x01 +#define STATUS_GPSFIX_2DFIX 0x02 +#define STATUS_GPSFIX_3DFIX 0x03 +#define STATUS_GPSFIX_GPSDR 0x04 +#define STATUS_GPSFIX_TIMEONLY 0x05 -#define STATUS_FLAGS_GPSFIX_OK (1 << 0) -#define STATUS_FLAGS_DIFFSOLN (1 << 1) -#define STATUS_FLAGS_WKNSET (1 << 2) -#define STATUS_FLAGS_TOWSET (1 << 3) +#define STATUS_FLAGS_GPSFIX_OK (1 << 0) +#define STATUS_FLAGS_DIFFSOLN (1 << 1) +#define STATUS_FLAGS_WKNSET (1 << 2) +#define STATUS_FLAGS_TOWSET (1 << 3) struct UBX_NAV_STATUS { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - uint8_t gpsFix; // GPS fix type - uint8_t flags; // Navigation Status Flags - uint8_t fixStat; // Fix Status Information - uint8_t flags2; // Additional navigation output information - uint32_t ttff; // Time to first fix (ms) - uint32_t msss; // Milliseconds since startup/reset (ms) + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint8_t gpsFix; // GPS fix type + uint8_t flags; // Navigation Status Flags + uint8_t fixStat; // Fix Status Information + uint8_t flags2; // Additional navigation output information + uint32_t ttff; // Time to first fix (ms) + uint32_t msss; // Milliseconds since startup/reset (ms) }; // Dilution of precision struct UBX_NAV_DOP { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - uint16_t gDOP; // Geometric DOP - uint16_t pDOP; // Position DOP - uint16_t tDOP; // Time DOP - uint16_t vDOP; // Vertical DOP - uint16_t hDOP; // Horizontal DOP - uint16_t nDOP; // Northing DOP - uint16_t eDOP; // Easting DOP + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint16_t gDOP; // Geometric DOP + uint16_t pDOP; // Position DOP + uint16_t tDOP; // Time DOP + uint16_t vDOP; // Vertical DOP + uint16_t hDOP; // Horizontal DOP + uint16_t nDOP; // Northing DOP + uint16_t eDOP; // Easting DOP }; // Navigation solution struct UBX_NAV_SOL { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - int32_t fTOW; // fractional nanoseconds (ns) - int16_t week; // GPS week - uint8_t gpsFix; // GPS fix type - uint8_t flags; // Fix status flags - int32_t ecefX; // ECEF X coordinate (cm) - int32_t ecefY; // ECEF Y coordinate (cm) - int32_t ecefZ; // ECEF Z coordinate (cm) - uint32_t pAcc; // 3D Position Accuracy Estimate (cm) - int32_t ecefVX; // ECEF X coordinate (cm/s) - int32_t ecefVY; // ECEF Y coordinate (cm/s) - int32_t ecefVZ; // ECEF Z coordinate (cm/s) - uint32_t sAcc; // Speed Accuracy Estimate - uint16_t pDOP; // Position DOP - uint8_t reserved1; // Reserved - uint8_t numSV; // Number of SVs used in Nav Solution - uint32_t reserved2; // Reserved + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + int32_t fTOW; // fractional nanoseconds (ns) + int16_t week; // GPS week + uint8_t gpsFix; // GPS fix type + uint8_t flags; // Fix status flags + int32_t ecefX; // ECEF X coordinate (cm) + int32_t ecefY; // ECEF Y coordinate (cm) + int32_t ecefZ; // ECEF Z coordinate (cm) + uint32_t pAcc; // 3D Position Accuracy Estimate (cm) + int32_t ecefVX; // ECEF X coordinate (cm/s) + int32_t ecefVY; // ECEF Y coordinate (cm/s) + int32_t ecefVZ; // ECEF Z coordinate (cm/s) + uint32_t sAcc; // Speed Accuracy Estimate + uint16_t pDOP; // Position DOP + uint8_t reserved1; // Reserved + uint8_t numSV; // Number of SVs used in Nav Solution + uint32_t reserved2; // Reserved }; // North/East/Down velocity struct UBX_NAV_VELNED { - uint32_t iTOW; // ms GPS Millisecond Time of Week - int32_t velN; // cm/s NED north velocity - int32_t velE; // cm/s NED east velocity - int32_t velD; // cm/s NED down velocity - uint32_t speed; // cm/s Speed (3-D) - uint32_t gSpeed; // cm/s Ground Speed (2-D) - int32_t heading; // 1e-5 *deg Heading of motion 2-D - uint32_t sAcc; // cm/s Speed Accuracy Estimate - uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate + uint32_t iTOW; // ms GPS Millisecond Time of Week + int32_t velN; // cm/s NED north velocity + int32_t velE; // cm/s NED east velocity + int32_t velD; // cm/s NED down velocity + uint32_t speed; // cm/s Speed (3-D) + uint32_t gSpeed; // cm/s Ground Speed (2-D) + int32_t heading; // 1e-5 *deg Heading of motion 2-D + uint32_t sAcc; // cm/s Speed Accuracy Estimate + uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate }; // UTC Time Solution -#define TIMEUTC_VALIDTOW (1 << 0) -#define TIMEUTC_VALIDWKN (1 << 1) -#define TIMEUTC_VALIDUTC (1 << 2) +#define TIMEUTC_VALIDTOW (1 << 0) +#define TIMEUTC_VALIDWKN (1 << 1) +#define TIMEUTC_VALIDUTC (1 << 2) struct UBX_NAV_TIMEUTC { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - uint32_t tAcc; // Time Accuracy Estimate (ns) - int32_t nano; // Nanoseconds of second - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t min; - uint8_t sec; - uint8_t valid; // Validity Flags + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint32_t tAcc; // Time Accuracy Estimate (ns) + int32_t nano; // Nanoseconds of second + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; // Validity Flags }; // Space Vehicle (SV) Information // Single SV information block -#define SVUSED (1 << 0) // This SV is used for navigation -#define DIFFCORR (1 << 1) // Differential correction available -#define ORBITAVAIL (1 << 2) // Orbit information available -#define ORBITEPH (1 << 3) // Orbit information is Ephemeris -#define UNHEALTHY (1 << 4) // SV is unhealthy -#define ORBITALM (1 << 5) // Orbit information is Almanac Plus -#define ORBITAOP (1 << 6) // Orbit information is AssistNow Autonomous -#define SMOOTHED (1 << 7) // Carrier smoothed pseudoranges used +#define SVUSED (1 << 0) // This SV is used for navigation +#define DIFFCORR (1 << 1) // Differential correction available +#define ORBITAVAIL (1 << 2) // Orbit information available +#define ORBITEPH (1 << 3) // Orbit information is Ephemeris +#define UNHEALTHY (1 << 4) // SV is unhealthy +#define ORBITALM (1 << 5) // Orbit information is Almanac Plus +#define ORBITAOP (1 << 6) // Orbit information is AssistNow Autonomous +#define SMOOTHED (1 << 7) // Carrier smoothed pseudoranges used struct UBX_NAV_SVINFO_SV { - uint8_t chn; // Channel number - uint8_t svid; // Satellite ID - uint8_t flags; // Misc SV information - uint8_t quality; // Misc quality indicators - uint8_t cno; // Carrier to Noise Ratio (dbHz) - int8_t elev; // Elevation (integer degrees) - int16_t azim; // Azimuth (integer degrees) - int32_t prRes; // Pseudo range residual (cm) + uint8_t chn; // Channel number + uint8_t svid; // Satellite ID + uint8_t flags; // Misc SV information + uint8_t quality; // Misc quality indicators + uint8_t cno; // Carrier to Noise Ratio (dbHz) + int8_t elev; // Elevation (integer degrees) + int16_t azim; // Azimuth (integer degrees) + int32_t prRes; // Pseudo range residual (cm) }; // SV information message -#define MAX_SVS 16 +#define MAX_SVS 16 struct UBX_NAV_SVINFO { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - uint8_t numCh; // Number of channels - uint8_t globalFlags; // - uint16_t reserved2; // Reserved - struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint8_t numCh; // Number of channels + uint8_t globalFlags; // + uint16_t reserved2; // Reserved + struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times }; typedef union { - uint8_t payload[0]; - struct UBX_NAV_POSLLH nav_posllh; - struct UBX_NAV_STATUS nav_status; - struct UBX_NAV_DOP nav_dop; - struct UBX_NAV_SOL nav_sol; - struct UBX_NAV_VELNED nav_velned; + uint8_t payload[0]; + struct UBX_NAV_POSLLH nav_posllh; + struct UBX_NAV_STATUS nav_status; + struct UBX_NAV_DOP nav_dop; + struct UBX_NAV_SOL nav_sol; + struct UBX_NAV_VELNED nav_velned; #if !defined(PIOS_GPS_MINIMAL) - struct UBX_NAV_TIMEUTC nav_timeutc; - struct UBX_NAV_SVINFO nav_svinfo; + struct UBX_NAV_TIMEUTC nav_timeutc; + struct UBX_NAV_SVINFO nav_svinfo; #endif } UBXPayload; struct UBXHeader { - uint8_t class; - uint8_t id; - uint16_t len; - uint8_t ck_a; - uint8_t ck_b; + uint8_t class; + uint8_t id; + uint16_t len; + uint8_t ck_a; + uint8_t ck_b; }; struct UBXPacket { - struct UBXHeader header; - UBXPayload payload; + struct UBXHeader header; + UBXPayload payload; }; bool checksum_ubx_message(struct UBXPacket *); uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionData *); -int parse_ubx_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *); +int parse_ubx_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *); #endif /* UBX_H */ diff --git a/flight/modules/MK/MKSerial/MKSerial.c b/flight/modules/MK/MKSerial/MKSerial.c index ed1cb7006..53f19d737 100644 --- a/flight/modules/MK/MKSerial/MKSerial.c +++ b/flight/modules/MK/MKSerial/MKSerial.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup MKSerialModule MK Serial Control Module * @brief Connect to MK module - * @{ + * @{ * * @file MKSerial.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -31,20 +31,20 @@ #include "openpilot.h" #include "MkSerial.h" -#include "attitudeactual.h" // object that will be updated by the module +#include "attitudeactual.h" // object that will be updated by the module #include "positionactual.h" #include "flightbatterystate.h" // // Configuration // -#define PORT PIOS_COM_AUX -#define DEBUG_PORT PIOS_COM_GPS -#define STACK_SIZE 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) -#define MAX_NB_PARS 100 -//#define ENABLE_DEBUG_MSG -//#define GENERATE_BATTERY_INFO // The MK can report battery voltage, but normally the current sensor will be used, so this module should not report battery state +#define PORT PIOS_COM_AUX +#define DEBUG_PORT PIOS_COM_GPS +#define STACK_SIZE 1024 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) +#define MAX_NB_PARS 100 +// #define ENABLE_DEBUG_MSG +// #define GENERATE_BATTERY_INFO // The MK can report battery voltage, but normally the current sensor will be used, so this module should not report battery state #if PORT == PIOS_COM_AUX #ifndef PIOS_ENABLE_AUX_UART @@ -54,27 +54,27 @@ // // Private constants // -#define MSGCMD_ANY 0 -#define MSGCMD_GET_DEBUG 'd' -#define MSGCMD_DEBUG 'D' -#define MSGCMD_GET_VERSION 'v' -#define MSGCMD_VERSION 'V' -#define MSGCMD_GET_OSD 'o' -#define MSGCMD_OSD 'O' +#define MSGCMD_ANY 0 +#define MSGCMD_GET_DEBUG 'd' +#define MSGCMD_DEBUG 'D' +#define MSGCMD_GET_VERSION 'v' +#define MSGCMD_VERSION 'V' +#define MSGCMD_GET_OSD 'o' +#define MSGCMD_OSD 'O' -#define DEBUG_MSG_NICK_IDX (2+2*2) -#define DEBUG_MSG_ROLL_IDX (2+3*2) +#define DEBUG_MSG_NICK_IDX (2 + 2 * 2) +#define DEBUG_MSG_ROLL_IDX (2 + 3 * 2) -#define OSD_MSG_CURRPOS_IDX 1 -#define OSD_MSG_NB_SATS_IDX 50 -#define OSD_MSG_BATT_IDX 57 +#define OSD_MSG_CURRPOS_IDX 1 +#define OSD_MSG_NB_SATS_IDX 50 +#define OSD_MSG_BATT_IDX 57 #define OSD_MSG_GNDSPEED_IDX 58 -#define OSD_MSG_COMPHEADING_IDX 62 -#define OSD_MSG_NICK_IDX 64 -#define OSD_MSG_ROLL_IDX 65 +#define OSD_MSG_COMPHEADING_IDX 62 +#define OSD_MSG_NICK_IDX 64 +#define OSD_MSG_ROLL_IDX 65 #ifdef ENABLE_DEBUG_MSG -#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__) +#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format,##__VA_ARGS__) #else #define DEBUG_MSG(format, ...) #endif @@ -83,24 +83,24 @@ // Private types // typedef struct { - uint8_t address; - uint8_t cmd; - uint8_t nbPars; - uint8_t pars[MAX_NB_PARS]; + uint8_t address; + uint8_t cmd; + uint8_t nbPars; + uint8_t pars[MAX_NB_PARS]; } MkMsg_t; typedef struct { - float longitute; - float latitude; - float altitude; - uint8_t status; + float longitute; + float latitude; + float altitude; + uint8_t status; } GpsPosition_t; enum { - MK_ADDR_ALL = 0, - MK_ADDR_FC = 1, - MK_ADDR_NC = 2, - MK_ADDR_MAG = 3, + MK_ADDR_ALL = 0, + MK_ADDR_FC = 1, + MK_ADDR_NC = 2, + MK_ADDR_MAG = 3, }; // @@ -111,433 +111,439 @@ enum { // Private functions // static void OnError(int line); -//static void PrintMsg(const MkMsg_t* msg); -static int16_t Par2Int16(const MkMsg_t * msg, uint8_t index); -static int32_t Par2Int32(const MkMsg_t * msg, uint8_t index); -static int8_t Par2Int8(const MkMsg_t * msg, uint8_t index); -static void GetGpsPos(const MkMsg_t * msg, uint8_t index, GpsPosition_t * pos); -static uint8_t WaitForBytes(uint8_t * buf, uint8_t nbBytes, portTickType xTicksToWait); -static bool WaitForMsg(uint8_t cmd, MkMsg_t * msg, portTickType xTicksToWait); -static void SendMsg(const MkMsg_t * msg); +// static void PrintMsg(const MkMsg_t* msg); +static int16_t Par2Int16(const MkMsg_t *msg, uint8_t index); +static int32_t Par2Int32(const MkMsg_t *msg, uint8_t index); +static int8_t Par2Int8(const MkMsg_t *msg, uint8_t index); +static void GetGpsPos(const MkMsg_t *msg, uint8_t index, GpsPosition_t *pos); +static uint8_t WaitForBytes(uint8_t *buf, uint8_t nbBytes, portTickType xTicksToWait); +static bool WaitForMsg(uint8_t cmd, MkMsg_t *msg, portTickType xTicksToWait); +static void SendMsg(const MkMsg_t *msg); static void SendMsgParNone(uint8_t address, uint8_t cmd); static void SendMsgPar8(uint8_t address, uint8_t cmd, uint8_t par0); static void MkSerialTask(void *parameters); static void OnError(int line) { - DEBUG_MSG("MKProcol error %d\n\r", line); + DEBUG_MSG("MKProcol error %d\n\r", line); } #if 0 -static void PrintMsg(const MkMsg_t * msg) +static void PrintMsg(const MkMsg_t *msg) { - switch (msg->address) { - case MK_ADDR_ALL: - DEBUG_MSG("ALL "); - break; - case MK_ADDR_FC: - DEBUG_MSG("FC "); - break; - case MK_ADDR_NC: - DEBUG_MSG("NC "); - break; - case MK_ADDR_MAG: - DEBUG_MSG("MAG "); - break; - default: - DEBUG_MSG("??? "); - break; - } + switch (msg->address) { + case MK_ADDR_ALL: + DEBUG_MSG("ALL "); + break; + case MK_ADDR_FC: + DEBUG_MSG("FC "); + break; + case MK_ADDR_NC: + DEBUG_MSG("NC "); + break; + case MK_ADDR_MAG: + DEBUG_MSG("MAG "); + break; + default: + DEBUG_MSG("??? "); + break; + } - DEBUG_MSG("%c ", msg->cmd); + DEBUG_MSG("%c ", msg->cmd); - for (int i = 0; i < msg->nbPars; i++) { - DEBUG_MSG("%02x ", msg->pars[i]); - } - DEBUG_MSG("\n\r"); + for (int i = 0; i < msg->nbPars; i++) { + DEBUG_MSG("%02x ", msg->pars[i]); + } + DEBUG_MSG("\n\r"); } -#endif +#endif /* if 0 */ -static int16_t Par2Int16(const MkMsg_t * msg, uint8_t index) +static int16_t Par2Int16(const MkMsg_t *msg, uint8_t index) { - int16_t res; + int16_t res; - res = (int)(msg->pars[index + 1]) * 256 + msg->pars[index]; - if (res > 0xFFFF / 2) - res -= 0xFFFF; - return res; + res = (int)(msg->pars[index + 1]) * 256 + msg->pars[index]; + if (res > 0xFFFF / 2) { + res -= 0xFFFF; + } + return res; } -static int32_t Par2Int32(const MkMsg_t * msg, uint8_t index) +static int32_t Par2Int32(const MkMsg_t *msg, uint8_t index) { - uint32_t val = 0; + uint32_t val = 0; - val = (((int)msg->pars[index]) << 0) + (((int)msg->pars[index + 1]) << 8); - val += (((int)msg->pars[index + 2]) << 16) + ((int)msg->pars[index + 3] << 24); - if (val > 0xFFFFFFFF / 2) - val -= 0xFFFFFFFF; - return (int32_t) val; + val = (((int)msg->pars[index]) << 0) + (((int)msg->pars[index + 1]) << 8); + val += (((int)msg->pars[index + 2]) << 16) + ((int)msg->pars[index + 3] << 24); + if (val > 0xFFFFFFFF / 2) { + val -= 0xFFFFFFFF; + } + return (int32_t)val; } -static int8_t Par2Int8(const MkMsg_t * msg, uint8_t index) +static int8_t Par2Int8(const MkMsg_t *msg, uint8_t index) { - if (msg->pars[index] > 127) - return msg->pars[index] - 256; - else - return msg->pars[index]; + if (msg->pars[index] > 127) { + return msg->pars[index] - 256; + } else { + return msg->pars[index]; + } } -static void GetGpsPos(const MkMsg_t * msg, uint8_t index, GpsPosition_t * pos) +static void GetGpsPos(const MkMsg_t *msg, uint8_t index, GpsPosition_t *pos) { - pos->longitute = (float)Par2Int32(msg, index) * (float)1e-7; - pos->latitude = (float)Par2Int32(msg, index + 4) * (float)1e-7; - pos->altitude = (float)Par2Int32(msg, index + 8) * (float)1e-3; - pos->status = msg->pars[index + 12]; + pos->longitute = (float)Par2Int32(msg, index) * (float)1e-7; + pos->latitude = (float)Par2Int32(msg, index + 4) * (float)1e-7; + pos->altitude = (float)Par2Int32(msg, index + 8) * (float)1e-3; + pos->status = msg->pars[index + 12]; } -static uint8_t WaitForBytes(uint8_t * buf, uint8_t nbBytes, portTickType xTicksToWait) +static uint8_t WaitForBytes(uint8_t *buf, uint8_t nbBytes, portTickType xTicksToWait) { - uint8_t nbBytesLeft = nbBytes; - xTimeOutType xTimeOut; + uint8_t nbBytesLeft = nbBytes; + xTimeOutType xTimeOut; - vTaskSetTimeOutState(&xTimeOut); + vTaskSetTimeOutState(&xTimeOut); - // Loop until - // - all bytes are received - // - \r is seen - // - Timeout occurs - do { - // Check if timeout occured - if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait)) - break; + // Loop until + // - all bytes are received + // - \r is seen + // - Timeout occurs + do { + // Check if timeout occured + if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait)) { + break; + } - // Check if there are some bytes - if (PIOS_COM_ReceiveBufferUsed(PORT)) { - *buf = PIOS_COM_ReceiveBuffer(PORT); + // Check if there are some bytes + if (PIOS_COM_ReceiveBufferUsed(PORT)) { + *buf = PIOS_COM_ReceiveBuffer(PORT); - nbBytesLeft--; - if (buf[0] == '\r') - break; - buf++; - } else { - // Avoid tight loop - // FIXME: should be blocking - vTaskDelay(5); - } - } while (nbBytesLeft); + nbBytesLeft--; + if (buf[0] == '\r') { + break; + } + buf++; + } else { + // Avoid tight loop + // FIXME: should be blocking + vTaskDelay(5); + } + } while (nbBytesLeft); - return nbBytes - nbBytesLeft; + return nbBytes - nbBytesLeft; } -static bool WaitForMsg(uint8_t cmd, MkMsg_t * msg, portTickType xTicksToWait) +static bool WaitForMsg(uint8_t cmd, MkMsg_t *msg, portTickType xTicksToWait) { - uint8_t buf[10]; - uint8_t n; - bool done = FALSE; - bool error = FALSE; - unsigned int checkVal; - xTimeOutType xTimeOut; + uint8_t buf[10]; + uint8_t n; + bool done = FALSE; + bool error = FALSE; + unsigned int checkVal; + xTimeOutType xTimeOut; - vTaskSetTimeOutState(&xTimeOut); + vTaskSetTimeOutState(&xTimeOut); - while (!done && !error) { - // When we are here, it means we did not encounter the message we are waiting for - // Check if we did not timeout yet. + while (!done && !error) { + // When we are here, it means we did not encounter the message we are waiting for + // Check if we did not timeout yet. - // Wait for start - buf[0] = 0; - do { - if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait)) { - return FALSE; - } - WaitForBytes(buf, 1, 100 / portTICK_RATE_MS); - } while (buf[0] != '#'); + // Wait for start + buf[0] = 0; + do { + if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait)) { + return FALSE; + } + WaitForBytes(buf, 1, 100 / portTICK_RATE_MS); + } while (buf[0] != '#'); - // Wait for cmd and address - if (WaitForBytes(buf, 2, 10 / portTICK_RATE_MS) != 2) { - OnError(__LINE__); - continue; - } - // Is this the command we are waiting for? - if (cmd == 0 || cmd == buf[1]) { - // OK follow this message to the end - msg->address = buf[0] - 'a'; - msg->cmd = buf[1]; + // Wait for cmd and address + if (WaitForBytes(buf, 2, 10 / portTICK_RATE_MS) != 2) { + OnError(__LINE__); + continue; + } + // Is this the command we are waiting for? + if (cmd == 0 || cmd == buf[1]) { + // OK follow this message to the end + msg->address = buf[0] - 'a'; + msg->cmd = buf[1]; - checkVal = '#' + buf[0] + buf[1]; + checkVal = '#' + buf[0] + buf[1]; - // Parse parameters - msg->nbPars = 0; - while (!done && !error) { - n = WaitForBytes(buf, 4, 10 / portTICK_RATE_MS); - if (n > 0 && buf[n - 1] == '\r') { - n--; - // This is the end of the message - // Get check bytes - if (n >= 2) { - unsigned int msgCeckVal; - msgCeckVal = (buf[n - 1] - '=') + (buf[n - 2] - '=') * 64; - //printf("%x %x\n", msgCeckVal, checkVal&0xFFF); - n -= 2; + // Parse parameters + msg->nbPars = 0; + while (!done && !error) { + n = WaitForBytes(buf, 4, 10 / portTICK_RATE_MS); + if (n > 0 && buf[n - 1] == '\r') { + n--; + // This is the end of the message + // Get check bytes + if (n >= 2) { + unsigned int msgCeckVal; + msgCeckVal = (buf[n - 1] - '=') + (buf[n - 2] - '=') * 64; + // printf("%x %x\n", msgCeckVal, checkVal&0xFFF); + n -= 2; - if (msgCeckVal == (checkVal & 0xFFF)) { - done = TRUE; - } else { - OnError(__LINE__); - error = TRUE; - } - } else { - OnError(__LINE__); - error = TRUE; - } - } else if (n == 4) { - // Parse parameters - int i; - for (i = 0; i < 4; i++) { - checkVal += buf[i]; - buf[i] -= '='; - } - if (msg->nbPars < MAX_NB_PARS) { - msg->pars[msg->nbPars] = (((buf[0] << 2) & 0xFF) | ((buf[1] >> 4))); - msg->nbPars++; - } - if (msg->nbPars < MAX_NB_PARS) { - msg->pars[msg->nbPars] = ((buf[1] & 0x0F) << 4 | (buf[2] >> 2)); - msg->nbPars++; - } - if (msg->nbPars < MAX_NB_PARS) { - msg->pars[msg->nbPars] = ((buf[2] & 0x03) << 6 | buf[3]); - msg->nbPars++; - } - } else { - OnError(__LINE__); - error = TRUE; - } - } - } - } + if (msgCeckVal == (checkVal & 0xFFF)) { + done = TRUE; + } else { + OnError(__LINE__); + error = TRUE; + } + } else { + OnError(__LINE__); + error = TRUE; + } + } else if (n == 4) { + // Parse parameters + int i; + for (i = 0; i < 4; i++) { + checkVal += buf[i]; + buf[i] -= '='; + } + if (msg->nbPars < MAX_NB_PARS) { + msg->pars[msg->nbPars] = (((buf[0] << 2) & 0xFF) | ((buf[1] >> 4))); + msg->nbPars++; + } + if (msg->nbPars < MAX_NB_PARS) { + msg->pars[msg->nbPars] = ((buf[1] & 0x0F) << 4 | (buf[2] >> 2)); + msg->nbPars++; + } + if (msg->nbPars < MAX_NB_PARS) { + msg->pars[msg->nbPars] = ((buf[2] & 0x03) << 6 | buf[3]); + msg->nbPars++; + } + } else { + OnError(__LINE__); + error = TRUE; + } + } + } + } - return (done && !error); + return done && !error; } -static void SendMsg(const MkMsg_t * msg) +static void SendMsg(const MkMsg_t *msg) { - uint8_t buf[10]; - uint16_t checkVal; - uint8_t nbParsRemaining; - const uint8_t *pPar; + uint8_t buf[10]; + uint16_t checkVal; + uint8_t nbParsRemaining; + const uint8_t *pPar; - // Header - buf[0] = '#'; - buf[1] = msg->address + 'a'; - buf[2] = msg->cmd; - PIOS_COM_SendBuffer(PORT, buf, 3); - checkVal = (unsigned int)'#' + buf[1] + buf[2]; + // Header + buf[0] = '#'; + buf[1] = msg->address + 'a'; + buf[2] = msg->cmd; + PIOS_COM_SendBuffer(PORT, buf, 3); + checkVal = (unsigned int)'#' + buf[1] + buf[2]; - // Parameters - nbParsRemaining = msg->nbPars; - pPar = msg->pars; - while (nbParsRemaining) { - uint8_t a, b, c; + // Parameters + nbParsRemaining = msg->nbPars; + pPar = msg->pars; + while (nbParsRemaining) { + uint8_t a, b, c; - a = *pPar; - b = 0; - c = 0; + a = *pPar; + b = 0; + c = 0; - nbParsRemaining--; - pPar++; - if (nbParsRemaining) { - b = *pPar; - nbParsRemaining--; - pPar++; - if (nbParsRemaining) { - c = *pPar; - nbParsRemaining--; - pPar++; - } - } + nbParsRemaining--; + pPar++; + if (nbParsRemaining) { + b = *pPar; + nbParsRemaining--; + pPar++; + if (nbParsRemaining) { + c = *pPar; + nbParsRemaining--; + pPar++; + } + } - buf[0] = (a >> 2) + '='; - buf[1] = (((a & 0x03) << 4) | ((b & 0xf0) >> 4)) + '='; - buf[2] = (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)) + '='; - buf[3] = (c & 0x3f) + '='; - checkVal += buf[0]; - checkVal += buf[1]; - checkVal += buf[2]; - checkVal += buf[3]; + buf[0] = (a >> 2) + '='; + buf[1] = (((a & 0x03) << 4) | ((b & 0xf0) >> 4)) + '='; + buf[2] = (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)) + '='; + buf[3] = (c & 0x3f) + '='; + checkVal += buf[0]; + checkVal += buf[1]; + checkVal += buf[2]; + checkVal += buf[3]; - PIOS_COM_SendBuffer(PORT, buf, 4); - } + PIOS_COM_SendBuffer(PORT, buf, 4); + } - checkVal &= 0xFFF; - buf[0] = (checkVal / 64) + '='; - buf[1] = (checkVal % 64) + '='; - buf[2] = '\r'; - PIOS_COM_SendBuffer(PORT, buf, 3); + checkVal &= 0xFFF; + buf[0] = (checkVal / 64) + '='; + buf[1] = (checkVal % 64) + '='; + buf[2] = '\r'; + PIOS_COM_SendBuffer(PORT, buf, 3); } static void SendMsgParNone(uint8_t address, uint8_t cmd) { - MkMsg_t msg; + MkMsg_t msg; - msg.address = address; - msg.cmd = cmd; - msg.nbPars = 0; + msg.address = address; + msg.cmd = cmd; + msg.nbPars = 0; - SendMsg(&msg); + SendMsg(&msg); } static void SendMsgPar8(uint8_t address, uint8_t cmd, uint8_t par0) { - MkMsg_t msg; + MkMsg_t msg; - msg.address = address; - msg.cmd = cmd; - msg.nbPars = 1; - msg.pars[0] = par0; + msg.address = address; + msg.cmd = cmd; + msg.nbPars = 1; + msg.pars[0] = par0; - SendMsg(&msg); + SendMsg(&msg); } -static uint16_t VersionMsg_GetVersion(const MkMsg_t * msg) +static uint16_t VersionMsg_GetVersion(const MkMsg_t *msg) { - return msg->pars[0] * 100 + msg->pars[1]; + return msg->pars[0] * 100 + msg->pars[1]; } static void DoConnectedToFC(void) { - AttitudeActualData attitudeData; - MkMsg_t msg; + AttitudeActualData attitudeData; + MkMsg_t msg; - DEBUG_MSG("FC\n\r"); + DEBUG_MSG("FC\n\r"); - memset(&attitudeData, 0, sizeof(attitudeData)); + memset(&attitudeData, 0, sizeof(attitudeData)); - // Configure FC for fast reporting of the debug-message - SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_DEBUG, 10); + // Configure FC for fast reporting of the debug-message + SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_DEBUG, 10); - while (TRUE) { - if (WaitForMsg(MSGCMD_DEBUG, &msg, 500 / portTICK_RATE_MS)) { - int16_t nick; - int16_t roll; + while (TRUE) { + if (WaitForMsg(MSGCMD_DEBUG, &msg, 500 / portTICK_RATE_MS)) { + int16_t nick; + int16_t roll; - //PrintMsg(&msg); - nick = Par2Int16(&msg, DEBUG_MSG_NICK_IDX); - roll = Par2Int16(&msg, DEBUG_MSG_ROLL_IDX); + // PrintMsg(&msg); + nick = Par2Int16(&msg, DEBUG_MSG_NICK_IDX); + roll = Par2Int16(&msg, DEBUG_MSG_ROLL_IDX); - DEBUG_MSG("Att: Nick=%5d Roll=%5d\n\r", nick, roll); + DEBUG_MSG("Att: Nick=%5d Roll=%5d\n\r", nick, roll); - attitudeData.Pitch = -(float)nick / 10; - attitudeData.Roll = -(float)roll / 10; - AttitudeActualSet(&attitudeData); - } else { - DEBUG_MSG("TO\n\r"); - break; - } - } + attitudeData.Pitch = -(float)nick / 10; + attitudeData.Roll = -(float)roll / 10; + AttitudeActualSet(&attitudeData); + } else { + DEBUG_MSG("TO\n\r"); + break; + } + } } static void DoConnectedToNC(void) { - MkMsg_t msg; - GpsPosition_t pos; - AttitudeActualData attitudeData; - PositionActualData positionData; - FlightBatteryStateData flightBatteryData; + MkMsg_t msg; + GpsPosition_t pos; + AttitudeActualData attitudeData; + PositionActualData positionData; + FlightBatteryStateData flightBatteryData; + #ifdef GENERATE_BATTERY_INFO - uint8_t battStateCnt = 0; + uint8_t battStateCnt = 0; #endif - DEBUG_MSG("NC\n\r"); + DEBUG_MSG("NC\n\r"); - memset(&attitudeData, 0, sizeof(attitudeData)); - memset(&positionData, 0, sizeof(positionData)); - memset(&flightBatteryData, 0, sizeof(flightBatteryData)); + memset(&attitudeData, 0, sizeof(attitudeData)); + memset(&positionData, 0, sizeof(positionData)); + memset(&flightBatteryData, 0, sizeof(flightBatteryData)); - // Configure NC for fast reporting of the osd-message - SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_OSD, 10); + // Configure NC for fast reporting of the osd-message + SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_OSD, 10); - while (TRUE) { - if (WaitForMsg(MSGCMD_OSD, &msg, 500 / portTICK_RATE_MS)) { - //PrintMsg(&msg); - GetGpsPos(&msg, OSD_MSG_CURRPOS_IDX, &pos); + while (TRUE) { + if (WaitForMsg(MSGCMD_OSD, &msg, 500 / portTICK_RATE_MS)) { + // PrintMsg(&msg); + GetGpsPos(&msg, OSD_MSG_CURRPOS_IDX, &pos); #if 0 - DEBUG_MSG("Bat=%d\n\r", msg.pars[OSD_MSG_BATT_IDX]); - DEBUG_MSG("Nick=%d Roll=%d\n\r", Par2Int8(&msg, OSD_MSG_NICK_IDX), Par2Int8(&msg, OSD_MSG_ROLL_IDX)); - DEBUG_MSG("POS #Sats=%d stat=%d lat=%d lon=%d alt=%d\n\r", msg.pars[OSD_MSG_NB_SATS_IDX], pos.status, (int)pos.latitude, - (int)pos.longitute, (int)pos.altitude); + DEBUG_MSG("Bat=%d\n\r", msg.pars[OSD_MSG_BATT_IDX]); + DEBUG_MSG("Nick=%d Roll=%d\n\r", Par2Int8(&msg, OSD_MSG_NICK_IDX), Par2Int8(&msg, OSD_MSG_ROLL_IDX)); + DEBUG_MSG("POS #Sats=%d stat=%d lat=%d lon=%d alt=%d\n\r", msg.pars[OSD_MSG_NB_SATS_IDX], pos.status, (int)pos.latitude, + (int)pos.longitute, (int)pos.altitude); #else - DEBUG_MSG("."); + DEBUG_MSG("."); #endif - attitudeData.Pitch = -Par2Int8(&msg, OSD_MSG_NICK_IDX); - attitudeData.Roll = -Par2Int8(&msg, OSD_MSG_ROLL_IDX); - AttitudeActualSet(&attitudeData); + attitudeData.Pitch = -Par2Int8(&msg, OSD_MSG_NICK_IDX); + attitudeData.Roll = -Par2Int8(&msg, OSD_MSG_ROLL_IDX); + AttitudeActualSet(&attitudeData); - positionData.Longitude = pos.longitute; - positionData.Latitude = pos.latitude; - positionData.Altitude = pos.altitude; - positionData.Satellites = msg.pars[OSD_MSG_NB_SATS_IDX]; - positionData.Heading = Par2Int16(&msg, OSD_MSG_COMPHEADING_IDX); - positionData.Groundspeed = ((float)Par2Int16(&msg, OSD_MSG_GNDSPEED_IDX)) / 100 /* cm/s => m/s */ ; - if (positionData.Satellites < 5) { - positionData.Status = POSITIONACTUAL_STATUS_NOFIX; - } else { - positionData.Status = POSITIONACTUAL_STATUS_FIX3D; - } - PositionActualSet(&positionData); + positionData.Longitude = pos.longitute; + positionData.Latitude = pos.latitude; + positionData.Altitude = pos.altitude; + positionData.Satellites = msg.pars[OSD_MSG_NB_SATS_IDX]; + positionData.Heading = Par2Int16(&msg, OSD_MSG_COMPHEADING_IDX); + positionData.Groundspeed = ((float)Par2Int16(&msg, OSD_MSG_GNDSPEED_IDX)) / 100 /* cm/s => m/s */; + if (positionData.Satellites < 5) { + positionData.Status = POSITIONACTUAL_STATUS_NOFIX; + } else { + positionData.Status = POSITIONACTUAL_STATUS_FIX3D; + } + PositionActualSet(&positionData); #if GENERATE_BATTERY_INFO - if (++battStateCnt > 2) { - flightBatteryData.Voltage = (float)msg.pars[OSD_MSG_BATT_IDX] / 10; - FlightBatteryStateSet(&flightBatteryData); - battStateCnt = 0; - } + if (++battStateCnt > 2) { + flightBatteryData.Voltage = (float)msg.pars[OSD_MSG_BATT_IDX] / 10; + FlightBatteryStateSet(&flightBatteryData); + battStateCnt = 0; + } #endif - } else { - DEBUG_MSG("TO\n\r"); - break; - } - } + } else { + DEBUG_MSG("TO\n\r"); + break; + } + } } static void MkSerialTask(__attribute__((unused)) void *parameters) { - MkMsg_t msg; - uint32_t version; - bool connectionOk = FALSE; + MkMsg_t msg; + uint32_t version; + bool connectionOk = FALSE; - PIOS_COM_ChangeBaud(PORT, 57600); - PIOS_COM_ChangeBaud(DEBUG_PORT, 57600); + PIOS_COM_ChangeBaud(PORT, 57600); + PIOS_COM_ChangeBaud(DEBUG_PORT, 57600); - DEBUG_MSG("MKSerial Started\n\r"); + DEBUG_MSG("MKSerial Started\n\r"); - while (1) { - // Wait until we get version from MK - while (!connectionOk) { - SendMsgParNone(MK_ADDR_ALL, MSGCMD_GET_VERSION); - DEBUG_MSG("Version... "); - if (WaitForMsg(MSGCMD_VERSION, &msg, 250 / portTICK_RATE_MS)) { - version = VersionMsg_GetVersion(&msg); - DEBUG_MSG("%d\n\r", version); - connectionOk = TRUE; - } else { - DEBUG_MSG("TO\n\r"); - } - } + while (1) { + // Wait until we get version from MK + while (!connectionOk) { + SendMsgParNone(MK_ADDR_ALL, MSGCMD_GET_VERSION); + DEBUG_MSG("Version... "); + if (WaitForMsg(MSGCMD_VERSION, &msg, 250 / portTICK_RATE_MS)) { + version = VersionMsg_GetVersion(&msg); + DEBUG_MSG("%d\n\r", version); + connectionOk = TRUE; + } else { + DEBUG_MSG("TO\n\r"); + } + } - // Dependent on version, decide it we are connected to NC or FC - // TODO: use slave-addr to distinguish FC/NC -> much safer - if (version < 60) { - DoConnectedToNC(); // Will only return after an error - } else { - DoConnectedToFC(); // Will only return after an error - } + // Dependent on version, decide it we are connected to NC or FC + // TODO: use slave-addr to distinguish FC/NC -> much safer + if (version < 60) { + DoConnectedToNC(); // Will only return after an error + } else { + DoConnectedToFC(); // Will only return after an error + } - connectionOk = FALSE; + connectionOk = FALSE; - vTaskDelay(250 / portTICK_RATE_MS); - } + vTaskDelay(250 / portTICK_RATE_MS); + } } /** @@ -547,13 +553,13 @@ static void MkSerialTask(__attribute__((unused)) void *parameters) */ int32_t MKSerialInitialize(void) { - // Start gps task - xTaskCreate(MkSerialTask, (signed char *)"MkSerial", STACK_SIZE, NULL, TASK_PRIORITY, NULL); + // Start gps task + xTaskCreate(MkSerialTask, (signed char *)"MkSerial", STACK_SIZE, NULL, TASK_PRIORITY, NULL); - return 0; + return 0; } -/** - * @} +/** + * @} * @} */ diff --git a/flight/modules/MK/MKSerial/inc/MkSerial.h b/flight/modules/MK/MKSerial/inc/MkSerial.h index 85915fe86..264d2bb3f 100644 --- a/flight/modules/MK/MKSerial/inc/MkSerial.h +++ b/flight/modules/MK/MKSerial/inc/MkSerial.h @@ -1,16 +1,16 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup MKSerialModule MK Serial Control Module * @brief Connect to MK module - * @{ + * @{ * * @file GPS.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Include file of the GPS module. - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and * objects. * @see The GNU Public License (GPL) Version 3 * @@ -38,7 +38,7 @@ int32_t MKSerialInitialize(void); #endif // MK_SER_INPUT_H -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/modules/ManualControl/inc/manualcontrol.h b/flight/modules/ManualControl/inc/manualcontrol.h index 7b841a035..6278daa45 100644 --- a/flight/modules/ManualControl/inc/manualcontrol.h +++ b/flight/modules/ManualControl/inc/manualcontrol.h @@ -32,23 +32,24 @@ #include "manualcontrolcommand.h" -typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3, FLIGHTMODE_TUNING = 4} flightmode_path; +typedef enum { FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3, FLIGHTMODE_TUNING = 4 } flightmode_path; -#define PARSE_FLIGHT_MODE(x) ( \ - (x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \ - (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \ - (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \ - (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \ - (x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_LAND) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : \ - (x == FLIGHTSTATUS_FLIGHTMODE_POI) ? FLIGHTMODE_GUIDANCE : \ - FLIGHTMODE_UNDEFINED \ - ) +#define PARSE_FLIGHT_MODE(x) \ + ( \ + (x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \ + (x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_LAND) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : \ + (x == FLIGHTSTATUS_FLIGHTMODE_POI) ? FLIGHTMODE_GUIDANCE : \ + FLIGHTMODE_UNDEFINED \ + ) int32_t ManualControlInitialize(); @@ -57,72 +58,79 @@ int32_t ManualControlInitialize(); * These are assumptions we make in the flight code about the order of settings and their consistency between * objects. Please keep this synchronized to the UAVObjects */ -#define assumptions1 ( \ -((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ -) +#define assumptions1 \ + ( \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ) -#define assumptions3 ( \ -((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ -) +#define assumptions3 \ + ( \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ) -#define assumptions5 ( \ -((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ -((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ -) +#define assumptions5 \ + ( \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ) -#define ARMING_CHANNEL_ROLL 0 -#define ARMING_CHANNEL_PITCH 1 -#define ARMING_CHANNEL_YAW 2 +#define ARMING_CHANNEL_ROLL 0 +#define ARMING_CHANNEL_PITCH 1 +#define ARMING_CHANNEL_YAW 2 -#define assumptions7 ( \ -( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \ -) +#define assumptions7 \ + ( \ + (((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_ROLL) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_ROLL) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_PITCH) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_PITCH) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_YAW) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_YAW) \ + ) -#define assumptions8 ( \ -( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ -( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \ -) +#define assumptions8 \ + ( \ + (((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 == 0) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 != 0) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 == 0) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 != 0) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 == 0) && \ + (((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 != 0) \ + ) -#define assumptions_flightmode ( \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int) FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int) FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND == (int) FLIGHTSTATUS_FLIGHTMODE_LAND) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int) FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \ -( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI == (int) FLIGHTSTATUS_FLIGHTMODE_POI) \ -) +#define assumptions_flightmode \ + ( \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int)FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \ + ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \ + ) -#define assumptions_channelcount ( \ -( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM ) && \ -( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM ) && \ -( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM ) && \ -( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM ) && \ -( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM ) ) +#define assumptions_channelcount \ + ( \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM)) #endif // MANUALCONTROL_H diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 91bac9c0e..aac310600 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -56,31 +56,30 @@ #if defined(PIOS_INCLUDE_USB_RCTX) #include "pios_usb_rctx.h" -#endif /* PIOS_INCLUDE_USB_RCTX */ +#endif /* PIOS_INCLUDE_USB_RCTX */ // Private constants #if defined(PIOS_MANUAL_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE +#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE #else -#define STACK_SIZE_BYTES 1024 +#define STACK_SIZE_BYTES 1024 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY+4) -#define UPDATE_PERIOD_MS 20 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) +#define UPDATE_PERIOD_MS 20 #define THROTTLE_FAILSAFE -0.1f -#define ARMED_TIME_MS 1000 -#define ARMED_THRESHOLD 0.50f -//safe band to allow a bit of calibration error or trim offset (in microseconds) +#define ARMED_TIME_MS 1000 +#define ARMED_THRESHOLD 0.50f +// safe band to allow a bit of calibration error or trim offset (in microseconds) #define CONNECTION_OFFSET 250 // Private types -typedef enum -{ - ARM_STATE_DISARMED, - ARM_STATE_ARMING_MANUAL, - ARM_STATE_ARMED, - ARM_STATE_DISARMING_MANUAL, - ARM_STATE_DISARMING_TIMEOUT +typedef enum { + ARM_STATE_DISARMED, + ARM_STATE_ARMING_MANUAL, + ARM_STATE_ARMED, + ARM_STATE_DISARMING_MANUAL, + ARM_STATE_DISARMING_TIMEOUT } ArmState_t; // Private variables @@ -94,15 +93,15 @@ static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM]; #endif // Private functions -static void updateActuatorDesired(ManualControlCommandData * cmd); -static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); -static void updateLandDesired(ManualControlCommandData * cmd, bool changed); -static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed); -static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home); -static void processFlightMode(ManualControlSettingsData * settings, float flightMode); -static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); +static void updateActuatorDesired(ManualControlCommandData *cmd); +static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings); +static void updateLandDesired(ManualControlCommandData *cmd, bool changed); +static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed); +static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home); +static void processFlightMode(ManualControlSettingsData *settings, float flightMode); +static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings); static void setArmedIfChanged(uint8_t val); -static void configurationUpdatedCb(UAVObjEvent * ev); +static void configurationUpdatedCb(UAVObjEvent *ev); static void manualControlTask(void *parameters); static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral); @@ -116,17 +115,16 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel #endif #define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 -#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10 -struct rcvr_activity_fsm -{ +#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10 +struct rcvr_activity_fsm { ManualControlSettingsChannelGroupsOptions group; uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; uint8_t sample_count; }; static struct rcvr_activity_fsm activity_fsm; -static void resetRcvrActivity(struct rcvr_activity_fsm * fsm); -static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm); +static void resetRcvrActivity(struct rcvr_activity_fsm *fsm); +static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm); #define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode && assumptions_channelcount) @@ -136,7 +134,7 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm); int32_t ManualControlStart() { // Start main task - xTaskCreate(manualControlTask, (signed char *) "ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); @@ -150,10 +148,10 @@ int32_t ManualControlStart() */ int32_t ManualControlInitialize() { - /* Check the assumptions about uavobject enum's are correct */ - if (!assumptions) + if (!assumptions) { return -1; + } AccessoryDesiredInitialize(); ManualControlCommandInitialize(); @@ -164,7 +162,7 @@ int32_t ManualControlInitialize() return 0; } -MODULE_INITCALL( ManualControlInitialize, ManualControlStart) +MODULE_INITCALL(ManualControlInitialize, ManualControlStart) /** * Module task @@ -177,7 +175,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) float flightMode = 0; uint8_t disconnected_count = 0; - uint8_t connected_count = 0; + uint8_t connected_count = 0; // For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically // this includes not even registering it if not used @@ -206,10 +204,9 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // Main task loop lastSysTime = xTaskGetTickCount(); - float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = {0}; + float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = { 0 }; while (1) { - // Wait until next update vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); #ifdef PIOS_INCLUDE_WDG @@ -244,7 +241,6 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } if (!ManualControlCommandReadOnly()) { - bool valid_input_detected = true; // Read channel values in us @@ -259,37 +255,37 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // If a channel has timed out this is not valid data and we shouldn't update anything // until we decide to go to failsafe - if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) + if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) { valid_input_detected = false; - else + } else { scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); + } } // Check settings, if error raise alarm if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || - // Check all channel mappings are valid - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID - || - // Check the driver exists - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER || - // Check the FlightModeNumber is valid - settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM || - // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care - ((settings.FlightModeNumber > 1) - && (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) { - + || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || + // Check all channel mappings are valid + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID + || + // Check the driver exists + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER || + // Check the FlightModeNumber is valid + settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM || + // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care + ((settings.FlightModeNumber > 1) + && (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); @@ -303,32 +299,32 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // decide if we have valid manual input or not valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) - && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) - && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) - && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) + && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) + && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) + && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); // Implement hysteresis loop on connection status if (valid_input_detected && (++connected_count > 10)) { - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; - connected_count = 0; + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; + connected_count = 0; disconnected_count = 0; } else if (!valid_input_detected && (++disconnected_count > 10)) { - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; - connected_count = 0; + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + connected_count = 0; disconnected_count = 0; } if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { - cmd.Throttle = -1; // Shut down engine with no control - cmd.Roll = 0; + cmd.Throttle = -1; // Shut down engine with no control + cmd.Roll = 0; cmd.Yaw = 0; - cmd.Pitch = 0; + cmd.Pitch = 0; cmd.Collective = 0; - //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning + // cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, // or leave throttle at IDLE speed or above when going into AUTO-failsafe. AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); @@ -337,31 +333,33 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // Set Accessory 0 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; - if (AccessoryDesiredInstSet(0, &accessory) != 0) + if (AccessoryDesiredInstSet(0, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } // Set Accessory 1 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; - if (AccessoryDesiredInstSet(1, &accessory) != 0) + if (AccessoryDesiredInstSet(1, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } // Set Accessory 2 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; - if (AccessoryDesiredInstSet(2, &accessory) != 0) + if (AccessoryDesiredInstSet(2, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } - } else if (valid_input_detected) { AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); // Scale channels to -1 -> +1 range - cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; - cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; - cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; + cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; + cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; + cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; - flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; + flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; // Apply deadband for Roll/Pitch/Yaw stick inputs if (settings.Deadband > 0.0f) { @@ -373,18 +371,19 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // Apply Low Pass Filter to input channels, time delta between calls in ms portTickType thisSysTime = xTaskGetTickCount(); float dT = (thisSysTime > lastSysTimeLPF) ? - (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : - (float)UPDATE_PERIOD_MS; + (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : + (float)UPDATE_PERIOD_MS; lastSysTimeLPF = thisSysTime; applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT); applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT); applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT); #endif // USE_INPUT_LPF - if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID - && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER - && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT) + if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID + && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER + && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) { cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; + } AccessoryDesiredData accessory; // Set Accessory 0 @@ -393,8 +392,9 @@ static void manualControlTask(__attribute__((unused)) void *parameters) #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); #endif - if (AccessoryDesiredInstSet(0, &accessory) != 0) + if (AccessoryDesiredInstSet(0, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } // Set Accessory 1 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { @@ -402,8 +402,9 @@ static void manualControlTask(__attribute__((unused)) void *parameters) #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); #endif - if (AccessoryDesiredInstSet(1, &accessory) != 0) + if (AccessoryDesiredInstSet(1, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } // Set Accessory 2 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { @@ -411,12 +412,12 @@ static void manualControlTask(__attribute__((unused)) void *parameters) #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); #endif - if (AccessoryDesiredInstSet(2, &accessory) != 0) + if (AccessoryDesiredInstSet(2, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } processFlightMode(&settings, flightMode); - } // Process arming outside conditional so system will disarm when disconnected @@ -427,13 +428,12 @@ static void manualControlTask(__attribute__((unused)) void *parameters) #if defined(PIOS_INCLUDE_USB_RCTX) if (pios_usb_rctx_id) { PIOS_USB_RCTX_Update(pios_usb_rctx_id, - cmd.Channel, - settings.ChannelMin, - settings.ChannelMax, - NELEMENTS(cmd.Channel)); + cmd.Channel, + settings.ChannelMin, + settings.ChannelMax, + NELEMENTS(cmd.Channel)); } -#endif /* PIOS_INCLUDE_USB_RCTX */ - +#endif /* PIOS_INCLUDE_USB_RCTX */ } else { ManualControlCommandGet(&cmd); /* Under GCS control */ } @@ -443,48 +443,48 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // Depending on the mode update the Stabilization or Actuator objects static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { - case FLIGHTMODE_UNDEFINED: - // This reflects a bug in the code architecture! + case FLIGHTMODE_UNDEFINED: + // This reflects a bug in the code architecture! + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + break; + case FLIGHTMODE_MANUAL: + updateActuatorDesired(&cmd); + break; + case FLIGHTMODE_STABILIZED: + updateStabilizationDesired(&cmd, &settings); + break; + case FLIGHTMODE_TUNING: + // Tuning takes settings directly from manualcontrolcommand. No need to + // call anything else. This just avoids errors. + break; + case FLIGHTMODE_GUIDANCE: + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: + altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode); + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + case FLIGHTSTATUS_FLIGHTMODE_POI: + updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false); + break; + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true); + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + // No need to call anything. This just avoids errors. + break; + case FLIGHTSTATUS_FLIGHTMODE_LAND: + updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode); + break; + default: AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - break; - case FLIGHTMODE_MANUAL: - updateActuatorDesired(&cmd); - break; - case FLIGHTMODE_STABILIZED: - updateStabilizationDesired(&cmd, &settings); - break; - case FLIGHTMODE_TUNING: - // Tuning takes settings directly from manualcontrolcommand. No need to - // call anything else. This just avoids errors. - break; - case FLIGHTMODE_GUIDANCE: - switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: - altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode); - break; - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - case FLIGHTSTATUS_FLIGHTMODE_POI: - updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false); - break; - case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true); - break; - case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: - // No need to call anything. This just avoids errors. - break; - case FLIGHTSTATUS_FLIGHTMODE_LAND: - updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode); - break; - default: - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - } - break; + } + break; } lastFlightMode = flightStatus.FlightMode; } } -static void resetRcvrActivity(struct rcvr_activity_fsm * fsm) +static void resetRcvrActivity(struct rcvr_activity_fsm *fsm) { ReceiverActivityData data; bool updated = false; @@ -492,7 +492,7 @@ static void resetRcvrActivity(struct rcvr_activity_fsm * fsm) /* Clear all channel activity flags */ ReceiverActivityGet(&data); if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) { - data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; + data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; data.ActiveChannel = 255; updated = true; } @@ -513,14 +513,14 @@ static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8 } } -static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm * fsm) +static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm *fsm) { bool activity_updated = false; /* Compare the current value to the previous sampled value */ for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) { uint16_t delta; - uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed + uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel); if (curr > prev) { delta = curr - prev; @@ -534,41 +534,41 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */ switch (fsm->group) { - case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: - group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: - group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: - group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: - group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK: - group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK; - break; - default: - PIOS_Assert(0); - break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: + group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: + group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK: + group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK; + break; + default: + PIOS_Assert(0); + break; } - ReceiverActivityActiveGroupSet((uint8_t*) &group); + ReceiverActivityActiveGroupSet((uint8_t *)&group); ReceiverActivityActiveChannelSet(&channel); activity_updated = true; } } - return (activity_updated); + return activity_updated; } -static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) +static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm) { bool activity_updated = false; @@ -587,13 +587,13 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) /* Take a sample of each channel in this group */ updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev)); fsm->sample_count++; - return (false); + return false; } /* Compare with previous sample */ activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm); - group_completed: +group_completed: /* Reset the sample counter */ fsm->sample_count = 0; @@ -617,87 +617,89 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) } } - return (activity_updated); + return activity_updated; } -static void updateActuatorDesired(ManualControlCommandData * cmd) +static void updateActuatorDesired(ManualControlCommandData *cmd) { ActuatorDesiredData actuator; + ActuatorDesiredGet(&actuator); - actuator.Roll = cmd->Roll; - actuator.Pitch = cmd->Pitch; - actuator.Yaw = cmd->Yaw; + actuator.Roll = cmd->Roll; + actuator.Pitch = cmd->Pitch; + actuator.Yaw = cmd->Yaw; actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; ActuatorDesiredSet(&actuator); } -static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings) +static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings) { StabilizationDesiredData stabilization; + StabilizationDesiredGet(&stabilization); StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); - uint8_t * stab_settings; + uint8_t *stab_settings; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - stab_settings = settings->Stabilization1Settings; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - stab_settings = settings->Stabilization2Settings; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - stab_settings = settings->Stabilization3Settings; - break; - default: - // Major error, this should not occur because only enter this block when one of these is true - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - return; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + stab_settings = settings->Stabilization1Settings; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + stab_settings = settings->Stabilization2Settings; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + stab_settings = settings->Stabilization3Settings; + break; + default: + // Major error, this should not occur because only enter this block when one of these is true + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + return; } // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; stabilization.Roll = - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? - cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? + cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? + cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode ; stabilization.Pitch = - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? - cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? - cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? + cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? + cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? + cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode stabilization.Yaw = - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? + cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; StabilizationDesiredSet(&stabilization); @@ -709,14 +711,14 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon * @brief Update the position desired to current location when * enabled and allow the waypoint to be moved by transmitter */ -static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * cmd, bool changed,bool home) +static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed, bool home) { /* - static portTickType lastSysTime; - portTickType thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); - lastSysTime = thisSysTime; - */ + static portTickType lastSysTime; + portTickType thisSysTime = xTaskGetTickCount(); + dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); + lastSysTime = thisSysTime; + */ if (home && changed) { // Simple Return To Base mode - keep altitude the same, fly to home position @@ -726,16 +728,16 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * PathDesiredData pathDesired; PathDesiredGet(&pathDesired); pathDesired.Start[PATHDESIRED_START_NORTH] = 0; - pathDesired.Start[PATHDESIRED_START_EAST] = 0; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10; - pathDesired.End[PATHDESIRED_END_NORTH] = 0; + pathDesired.Start[PATHDESIRED_START_EAST] = 0; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10; + pathDesired.End[PATHDESIRED_END_NORTH] = 0; pathDesired.End[PATHDESIRED_END_EAST] = 0; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10; - pathDesired.StartingVelocity=1; - pathDesired.EndingVelocity=0; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); - } else if(changed) { + } else if (changed) { // After not being in this mode for a while init at current height PositionActualData positionActual; PositionActualGet(&positionActual); @@ -743,57 +745,58 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * PathDesiredData pathDesired; PathDesiredGet(&pathDesired); pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; - pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; + pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; - pathDesired.StartingVelocity=1; - pathDesired.EndingVelocity=0; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. - } else { - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; - pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); + } else { + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; + pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); */ } } -static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * cmd, bool changed) +static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed) { /* - static portTickType lastSysTime; - portTickType thisSysTime; - float dT; + static portTickType lastSysTime; + portTickType thisSysTime; + float dT; - thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); - lastSysTime = thisSysTime; - */ + thisSysTime = xTaskGetTickCount(); + dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); + lastSysTime = thisSysTime; + */ PositionActualData positionActual; + PositionActualGet(&positionActual); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - if(changed) { + if (changed) { // After not being in this mode for a while init at current height pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; - pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; + pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; - pathDesired.StartingVelocity=1; - pathDesired.EndingVelocity=0; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; } - pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down+5; + pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down + 5; PathDesiredSet(&pathDesired); } @@ -802,7 +805,7 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * * enabled and enable altitude mode for stabilization * @todo: Need compile flag to exclude this from copter control */ -static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) +static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) { const float DEADBAND = 0.10f; const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; @@ -818,6 +821,7 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) float dT; AltitudeHoldDesiredData altitudeHoldDesiredData; + AltitudeHoldDesiredGet(&altitudeHoldDesiredData); AltitudeHoldSettingsThrottleExpGet(&throttleExp); @@ -826,17 +830,17 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); - thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTimeAH)? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f); - lastSysTimeAH = thisSysTime; + thisSysTime = xTaskGetTickCount(); + dT = ((thisSysTime == lastSysTimeAH) ? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f); + lastSysTimeAH = thisSysTime; - altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; + altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; - altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; + altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; float currentDown; PositionActualDownGet(¤tDown); - if(changed) { + if (changed) { // After not being in this mode for a while init at current height altitudeHoldDesiredData.Altitude = 0; zeroed = false; @@ -845,7 +849,7 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) // then apply an "exp" f(x,k) = (k∙x∙x∙x + x∙(256−k)) / 256 altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT; } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT; + altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT; } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) { // Require the stick to enter the dead band before they can move height zeroed = true; @@ -853,30 +857,30 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) AltitudeHoldDesiredSet(&altitudeHoldDesiredData); } -#else +#else /* if defined(REVOLUTION) */ // TODO: These functions should never be accessible on CC. Any configuration that // could allow them to be called should already throw an error to prevent this happening // in flight -static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * cmd, - __attribute__((unused)) bool changed, - __attribute__((unused)) bool home) +static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, + __attribute__((unused)) bool changed, + __attribute__((unused)) bool home) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); } -static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * cmd, - __attribute__((unused)) bool changed) +static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, + __attribute__((unused)) bool changed) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); } -static void altitudeHoldDesired(__attribute__((unused)) ManualControlCommandData * cmd, - __attribute__((unused)) bool changed) +static void altitudeHoldDesired(__attribute__((unused)) ManualControlCommandData *cmd, + __attribute__((unused)) bool changed) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); } -#endif +#endif /* if defined(REVOLUTION) */ /** * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. */ @@ -887,13 +891,13 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr // Scale if ((max > min && value >= neutral) || (min > max && value <= neutral)) { if (max != neutral) { - valueScaled = (float) (value - neutral) / (float) (max - neutral); + valueScaled = (float)(value - neutral) / (float)(max - neutral); } else { valueScaled = 0; } } else { if (min != neutral) { - valueScaled = (float) (value - neutral) / (float) (neutral - min); + valueScaled = (float)(value - neutral) / (float)(neutral - min); } else { valueScaled = 0; } @@ -909,8 +913,9 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr return valueScaled; } -static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) { - return (end_time - start_time) * portTICK_RATE_MS; +static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) +{ + return (end_time - start_time) * portTICK_RATE_MS; } /** @@ -926,27 +931,27 @@ static bool okToArm(void) // Check each alarm for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { - if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set + if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { continue; - } + } return false; } } - uint8_t flightMode; - FlightStatusFlightModeGet(&flightMode); - switch(flightMode) { - case FLIGHTSTATUS_FLIGHTMODE_MANUAL: - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - return true; - default: - return false; + uint8_t flightMode; + FlightStatusFlightModeGet(&flightMode); + switch (flightMode) { + case FLIGHTSTATUS_FLIGHTMODE_MANUAL: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + return true; - } + default: + return false; + } } /** * @brief Determine if the aircraft is forced to disarm by an explicit alarm @@ -956,6 +961,7 @@ static bool forcedDisArm(void) { // read alarms SystemAlarmsData alarms; + SystemAlarmsGet(&alarms); if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) { @@ -971,6 +977,7 @@ static bool forcedDisArm(void) static void setArmedIfChanged(uint8_t val) { FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); if (flightStatus.Armed != val) { @@ -984,9 +991,8 @@ static void setArmedIfChanged(uint8_t val) * @param[out] cmd The structure to set the armed in * @param[in] settings Settings indicating the necessary position */ -static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) +static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings) { - bool lowThrottle = cmd->Throttle <= 0; if (forcedDisArm()) { @@ -1000,22 +1006,23 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); } else { // Not really needed since this function not called when disconnected - if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) + if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { lowThrottle = true; + } // The throttle is not low, in case we where arming or disarming, abort if (!lowThrottle) { switch (armState) { - case ARM_STATE_DISARMING_MANUAL: - case ARM_STATE_DISARMING_TIMEOUT: - armState = ARM_STATE_ARMED; - break; - case ARM_STATE_ARMING_MANUAL: - armState = ARM_STATE_DISARMED; - break; - default: - // Nothing needs to be done in the other states - break; + case ARM_STATE_DISARMING_MANUAL: + case ARM_STATE_DISARMING_TIMEOUT: + armState = ARM_STATE_ARMED; + break; + case ARM_STATE_ARMING_MANUAL: + armState = ARM_STATE_DISARMED; + break; + default: + // Nothing needs to be done in the other states + break; } return; } @@ -1035,72 +1042,76 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData // Calc channel see assumptions7 int8_t sign = ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2) ? -1 : 1; switch ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) { - case ARMING_CHANNEL_ROLL: - armingInputLevel = sign * cmd->Roll; - break; - case ARMING_CHANNEL_PITCH: - armingInputLevel = sign * cmd->Pitch; - break; - case ARMING_CHANNEL_YAW: - armingInputLevel = sign * cmd->Yaw; - break; + case ARMING_CHANNEL_ROLL: + armingInputLevel = sign * cmd->Roll; + break; + case ARMING_CHANNEL_PITCH: + armingInputLevel = sign * cmd->Pitch; + break; + case ARMING_CHANNEL_YAW: + armingInputLevel = sign * cmd->Yaw; + break; } - bool manualArm = false; + bool manualArm = false; bool manualDisarm = false; - if (armingInputLevel <= -ARMED_THRESHOLD) + if (armingInputLevel <= -ARMED_THRESHOLD) { manualArm = true; - else if (armingInputLevel >= +ARMED_THRESHOLD) + } else if (armingInputLevel >= +ARMED_THRESHOLD) { manualDisarm = true; + } switch (armState) { - case ARM_STATE_DISARMED: - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + case ARM_STATE_DISARMED: + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - // only allow arming if it's OK too - if (manualArm && okToArm()) { - armedDisarmStart = lastSysTime; - armState = ARM_STATE_ARMING_MANUAL; - } - break; - - case ARM_STATE_ARMING_MANUAL: - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); - - if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) - armState = ARM_STATE_ARMED; - else if (!manualArm) - armState = ARM_STATE_DISARMED; - break; - - case ARM_STATE_ARMED: - // When we get here, the throttle is low, - // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled + // only allow arming if it's OK too + if (manualArm && okToArm()) { armedDisarmStart = lastSysTime; - armState = ARM_STATE_DISARMING_TIMEOUT; - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); - break; + armState = ARM_STATE_ARMING_MANUAL; + } + break; - case ARM_STATE_DISARMING_TIMEOUT: - // We get here when armed while throttle low, even when the arming timeout is not enabled - if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) - armState = ARM_STATE_DISARMED; + case ARM_STATE_ARMING_MANUAL: + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); - // Switch to disarming due to manual control when needed - if (manualDisarm) { - armedDisarmStart = lastSysTime; - armState = ARM_STATE_DISARMING_MANUAL; - } - break; + if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) { + armState = ARM_STATE_ARMED; + } else if (!manualArm) { + armState = ARM_STATE_DISARMED; + } + break; - case ARM_STATE_DISARMING_MANUAL: - if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) - armState = ARM_STATE_DISARMED; - else if (!manualDisarm) - armState = ARM_STATE_ARMED; - break; - } // End Switch + case ARM_STATE_ARMED: + // When we get here, the throttle is low, + // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled + armedDisarmStart = lastSysTime; + armState = ARM_STATE_DISARMING_TIMEOUT; + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); + break; + + case ARM_STATE_DISARMING_TIMEOUT: + // We get here when armed while throttle low, even when the arming timeout is not enabled + if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) { + armState = ARM_STATE_DISARMED; + } + + // Switch to disarming due to manual control when needed + if (manualDisarm) { + armedDisarmStart = lastSysTime; + armState = ARM_STATE_DISARMING_MANUAL; + } + break; + + case ARM_STATE_DISARMING_MANUAL: + if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) { + armState = ARM_STATE_DISARMED; + } else if (!manualDisarm) { + armState = ARM_STATE_ARMED; + } + break; + } // End Switch } } @@ -1113,12 +1124,14 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData static void processFlightMode(ManualControlSettingsData *settings, float flightMode) { FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); // Convert flightMode value into the switch position in the range [0..N-1] uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9; - if (pos >= settings->FlightModeNumber) + if (pos >= settings->FlightModeNumber) { pos = settings->FlightModeNumber - 1; + } uint8_t newMode = settings->FlightModePosition[pos]; @@ -1139,7 +1152,7 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value) min = max; max = tmp; } - return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET); + return value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET; } /** @@ -1147,12 +1160,13 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value) */ static void applyDeadband(float *value, float deadband) { - if (fabsf(*value) < deadband) + if (fabsf(*value) < deadband) { *value = 0.0f; - else if (*value > 0.0f) + } else if (*value > 0.0f) { *value -= deadband; - else + } else { *value += deadband; + } } #ifdef USE_INPUT_LPF diff --git a/flight/modules/OPLink/inc/oplinkmod.h b/flight/modules/OPLink/inc/oplinkmod.h index faeebad1b..e1481149b 100644 --- a/flight/modules/OPLink/inc/oplinkmod.h +++ b/flight/modules/OPLink/inc/oplinkmod.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup OPLinkModule OPLink Module - * @{ + * @{ * * @file oplinkmod.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. diff --git a/flight/modules/OPLink/oplinkmod.c b/flight/modules/OPLink/oplinkmod.c index ea7c58293..3947ee569 100644 --- a/flight/modules/OPLink/oplinkmod.c +++ b/flight/modules/OPLink/oplinkmod.c @@ -8,12 +8,12 @@ # which then contains hardware specific implementations * (currently only STM32 supported) * - * @{ + * @{ * @addtogroup OPLinkModule OPLink Module * @brief Initializes PIOS and other modules runs monitoring * After initializing all the modules runs basic monitoring and * alarms. - * @{ + * @{ * * @file oplinkmod.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. @@ -51,12 +51,12 @@ #define SYSTEM_UPDATE_PERIOD_MS 1000 #if defined(PIOS_SYSTEM_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE +#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE #else -#define STACK_SIZE_BYTES 924 +#define STACK_SIZE_BYTES 924 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY+2) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) // Private types @@ -76,15 +76,15 @@ static void systemTask(void *parameters); */ int32_t OPLinkModStart(void) { - // Initialize vars - stackOverflow = false; - mallocFailed = false; - // Create oplink system task - xTaskCreate(systemTask, (signed char *)"OPLink", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &systemTaskHandle); - // Register task - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle); + // Initialize vars + stackOverflow = false; + mallocFailed = false; + // Create oplink system task + xTaskCreate(systemTask, (signed char *)"OPLink", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle); + // Register task + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle); - return 0; + return 0; } /** @@ -93,29 +93,28 @@ int32_t OPLinkModStart(void) */ int32_t OPLinkModInitialize(void) { + // Must registers objects here for system thread because ObjectManager started in OpenPilotInit - // Must registers objects here for system thread because ObjectManager started in OpenPilotInit + // Initialize out status object. + OPLinkStatusInitialize(); + OPLinkStatusData oplinkStatus; + OPLinkStatusGet(&oplinkStatus); - // Initialize out status object. - OPLinkStatusInitialize(); - OPLinkStatusData oplinkStatus; - OPLinkStatusGet(&oplinkStatus); + // Get our hardware information. + const struct pios_board_info *bdinfo = &pios_board_info_blob; - // Get our hardware information. - const struct pios_board_info * bdinfo = &pios_board_info_blob; + oplinkStatus.BoardType = bdinfo->board_type; + PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); + PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); + oplinkStatus.BoardRevision = bdinfo->board_rev; - oplinkStatus.BoardType= bdinfo->board_type; - PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); - PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); - oplinkStatus.BoardRevision= bdinfo->board_rev; + // Update the object + OPLinkStatusSet(&oplinkStatus); - // Update the object - OPLinkStatusSet(&oplinkStatus); + // Call the module start function. + OPLinkModStart(); - // Call the module start function. - OPLinkModStart(); - - return 0; + return 0; } MODULE_INITCALL(OPLinkModInitialize, 0) @@ -125,88 +124,87 @@ MODULE_INITCALL(OPLinkModInitialize, 0) */ static void systemTask(__attribute__((unused)) void *parameters) { - portTickType lastSysTime; - uint16_t prev_tx_count = 0; - uint16_t prev_rx_count = 0; - bool first_time = true; + portTickType lastSysTime; + uint16_t prev_tx_count = 0; + uint16_t prev_rx_count = 0; + bool first_time = true; - /* create all modules thread */ - MODULE_TASKCREATE_ALL; + /* create all modules thread */ + MODULE_TASKCREATE_ALL; - if (mallocFailed) { - /* We failed to malloc during task creation, - * system behaviour is undefined. Reset and let - * the BootFault code recover for us. - */ - PIOS_SYS_Reset(); - } + if (mallocFailed) { + /* We failed to malloc during task creation, + * system behaviour is undefined. Reset and let + * the BootFault code recover for us. + */ + PIOS_SYS_Reset(); + } - // Initialize vars - idleCounter = 0; - idleCounterClear = 0; - lastSysTime = xTaskGetTickCount(); + // Initialize vars + idleCounter = 0; + idleCounterClear = 0; + lastSysTime = xTaskGetTickCount(); - // Main system loop - while (1) { - - // Flash the heartbeat LED + // Main system loop + while (1) { + // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ - // Update the OPLinkStatus UAVO - OPLinkStatusData oplinkStatus; - OPLinkStatusGet(&oplinkStatus); + // Update the OPLinkStatus UAVO + OPLinkStatusData oplinkStatus; + OPLinkStatusGet(&oplinkStatus); - // Get the other device stats. - PIOS_RFM2B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM); + // Get the other device stats. + PIOS_RFM2B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM); - // Get the stats from the radio device - struct rfm22b_stats radio_stats; - PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); + // Get the stats from the radio device + struct rfm22b_stats radio_stats; + PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); - // Update the status - oplinkStatus.HeapRemaining = xPortGetFreeHeapSize(); - oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); - oplinkStatus.RxGood = radio_stats.rx_good; - oplinkStatus.RxCorrected = radio_stats.rx_corrected; - oplinkStatus.RxErrors = radio_stats.rx_error; - oplinkStatus.RxMissed = radio_stats.rx_missed; - oplinkStatus.RxFailure = radio_stats.rx_failure; - oplinkStatus.TxDropped = radio_stats.tx_dropped; - oplinkStatus.TxResent = radio_stats.tx_resent; - oplinkStatus.TxFailure = radio_stats.tx_failure; - oplinkStatus.Resets = radio_stats.resets; - oplinkStatus.Timeouts = radio_stats.timeouts; - oplinkStatus.RSSI = radio_stats.rssi; - oplinkStatus.LinkQuality = radio_stats.link_quality; - if (first_time) - first_time = false; - else - { - uint16_t tx_count = radio_stats.tx_byte_count; - uint16_t rx_count = radio_stats.rx_byte_count; - uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count); - uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count); - oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); - oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); - prev_tx_count = tx_count; - prev_rx_count = rx_count; - } - oplinkStatus.TXSeq = radio_stats.tx_seq; - oplinkStatus.RXSeq = radio_stats.rx_seq; - oplinkStatus.LinkState = radio_stats.link_state; - if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) - LINK_LED_ON; - else - LINK_LED_OFF; + // Update the status + oplinkStatus.HeapRemaining = xPortGetFreeHeapSize(); + oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); + oplinkStatus.RxGood = radio_stats.rx_good; + oplinkStatus.RxCorrected = radio_stats.rx_corrected; + oplinkStatus.RxErrors = radio_stats.rx_error; + oplinkStatus.RxMissed = radio_stats.rx_missed; + oplinkStatus.RxFailure = radio_stats.rx_failure; + oplinkStatus.TxDropped = radio_stats.tx_dropped; + oplinkStatus.TxResent = radio_stats.tx_resent; + oplinkStatus.TxFailure = radio_stats.tx_failure; + oplinkStatus.Resets = radio_stats.resets; + oplinkStatus.Timeouts = radio_stats.timeouts; + oplinkStatus.RSSI = radio_stats.rssi; + oplinkStatus.LinkQuality = radio_stats.link_quality; + if (first_time) { + first_time = false; + } else { + uint16_t tx_count = radio_stats.tx_byte_count; + uint16_t rx_count = radio_stats.rx_byte_count; + uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count); + uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count); + oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); + oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); + prev_tx_count = tx_count; + prev_rx_count = rx_count; + } + oplinkStatus.TXSeq = radio_stats.tx_seq; + oplinkStatus.RXSeq = radio_stats.rx_seq; + oplinkStatus.LinkState = radio_stats.link_state; + if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) { + LINK_LED_ON; + } else { + LINK_LED_OFF; + } - // Update the object - OPLinkStatusSet(&oplinkStatus); + // Update the object + OPLinkStatusSet(&oplinkStatus); - // Wait until next period - vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS); - } + // Wait until next period + vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS); + } } /** @@ -214,27 +212,29 @@ static void systemTask(__attribute__((unused)) void *parameters) */ void vApplicationIdleHook(void) { - // Called when the scheduler has no tasks to run - if (idleCounterClear == 0) { - ++idleCounter; - } else { - idleCounter = 0; - idleCounterClear = 0; - } + // Called when the scheduler has no tasks to run + if (idleCounterClear == 0) { + ++idleCounter; + } else { + idleCounter = 0; + idleCounterClear = 0; + } } /** * Called by the RTOS when a stack overflow is detected. */ #define DEBUG_STACK_OVERFLOW 0 -void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle * pxTask, - __attribute__((unused)) signed portCHAR * pcTaskName) +void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask, + __attribute__((unused)) signed portCHAR *pcTaskName) { - stackOverflow = true; + stackOverflow = true; #if DEBUG_STACK_OVERFLOW - static volatile bool wait_here = true; - while(wait_here); - wait_here = true; + static volatile bool wait_here = true; + while (wait_here) { + ; + } + wait_here = true; #endif } @@ -244,15 +244,17 @@ void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle * pxTask, #define DEBUG_MALLOC_FAILURES 0 void vApplicationMallocFailedHook(void) { - mallocFailed = true; + mallocFailed = true; #if DEBUG_MALLOC_FAILURES - static volatile bool wait_here = true; - while(wait_here); - wait_here = true; + static volatile bool wait_here = true; + while (wait_here) { + ; + } + wait_here = true; #endif } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/Osd/OsdEtStd/OsdEtStd.c b/flight/modules/Osd/OsdEtStd/OsdEtStd.c index 2c806ad64..363f125e1 100644 --- a/flight/modules/Osd/OsdEtStd/OsdEtStd.c +++ b/flight/modules/Osd/OsdEtStd/OsdEtStd.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup OSDModule OSD Module * @brief On screen display support - * @{ + * @{ * * @file OsdEtStd.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -39,59 +39,59 @@ // // Configuration // -#define DEBUG_PORT PIOS_COM_GPS -//#define ENABLE_DEBUG_MSG -//#define PIOS_ENABLE_DEBUG_PINS -//#define DUMP_CONFIG // Enable this do read and dump the OSD config +#define DEBUG_PORT PIOS_COM_GPS +// #define ENABLE_DEBUG_MSG +// #define PIOS_ENABLE_DEBUG_PINS +// #define DUMP_CONFIG // Enable this do read and dump the OSD config // // Private constants // #ifdef ENABLE_DEBUG_MSG -#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__) +#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format,##__VA_ARGS__) #else #define DEBUG_MSG(format, ...) #endif -#define CONFIG_LENGTH 6726 -#define MIN(a,b) ((a)<(b)?(a):(b)) +#define CONFIG_LENGTH 6726 +#define MIN(a, b) ((a) < (b) ? (a) : (b)) -#define SUPPORTED_VERSION 115 +#define SUPPORTED_VERSION 115 -#define OSD_ADDRESS 0x30 +#define OSD_ADDRESS 0x30 -#define OSDMSG_V_LS_IDX 10 -#define OSDMSG_BALT_IDX1 11 -#define OSDMSG_BALT_IDX2 12 -#define OSDMSG_A_LS_IDX 17 -#define OSDMSG_VA_MS_IDX 18 -#define OSDMSG_LAT_IDX 33 -#define OSDMSG_LON_IDX 37 -#define OSDMSG_HOME_IDX 47 -#define OSDMSG_ALT_IDX 49 -#define OSDMSG_NB_SATS 58 -#define OSDMSG_GPS_STAT 59 +#define OSDMSG_V_LS_IDX 10 +#define OSDMSG_BALT_IDX1 11 +#define OSDMSG_BALT_IDX2 12 +#define OSDMSG_A_LS_IDX 17 +#define OSDMSG_VA_MS_IDX 18 +#define OSDMSG_LAT_IDX 33 +#define OSDMSG_LON_IDX 37 +#define OSDMSG_HOME_IDX 47 +#define OSDMSG_ALT_IDX 49 +#define OSDMSG_NB_SATS 58 +#define OSDMSG_GPS_STAT 59 -#define OSDMSG_GPS_STAT_NOFIX 0x03 -#define OSDMSG_GPS_STAT_FIX 0x2B -#define OSDMSG_GPS_STAT_HB_FLAG 0x10 +#define OSDMSG_GPS_STAT_NOFIX 0x03 +#define OSDMSG_GPS_STAT_FIX 0x2B +#define OSDMSG_GPS_STAT_HB_FLAG 0x10 #ifdef PIOS_ENABLE_DEBUG_PINS - #define DEBUG_PIN_RUNNING 0 - #define DEBUG_PIN_I2C 1 - #define DebugPinHigh(x) PIOS_DEBUG_PinHigh(x) - #define DebugPinLow(x) PIOS_DEBUG_PinLow(x) + #define DEBUG_PIN_RUNNING 0 + #define DEBUG_PIN_I2C 1 + #define DebugPinHigh(x) PIOS_DEBUG_PinHigh(x) + #define DebugPinLow(x) PIOS_DEBUG_PinLow(x) #else - #define DebugPinHigh(x) - #define DebugPinLow(x) + #define DebugPinHigh(x) + #define DebugPinLow(x) #endif static const char *UpdateConfFilePath = "/etosd/update.ocf"; #ifdef DUMP_CONFIG -static const char *DumpConfFilePath = "/etosd/dump.ocf"; +static const char *DumpConfFilePath = "/etosd/dump.ocf"; #endif // @@ -102,23 +102,21 @@ static const char *DumpConfFilePath = "/etosd/dump.ocf"; // Private variables // -// | Header / cmd?| | V | E3 | V | | LAT: 37 57.0000 | LONG: 24 00.4590 | | Hom<-179| Alt 545.4 m | |#sat|stat| -// 00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 +// | Header / cmd?| | V | E3 | V | | LAT: 37 57.0000 | LONG: 24 00.4590 | | Hom<-179| Alt 545.4 m | |#sat|stat| +// 00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 uint8_t msg[63] = - { 0x03, 0x3F, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x0A, 0x00, 0xE4, 0x30, 0x00, 0x00, 0x00, 0x00, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x60, 0x10, 0x02, 0x00, 0x00, 0x90, 0x00, -0x54, 0x54, 0x00, 0x00, 0x33, 0x28, 0x13, 0x00, 0x00, 0x08, 0x00, 0x00, 0x90, 0x0A }; -static volatile bool newPosData = FALSE; +{ 0x03, 0x3F, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x0A, 0x00, 0xE4, 0x30, 0x00, 0x00, 0x00, 0x00, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x60, 0x10, 0x02, 0x00, 0x00, 0x90,0x00, + 0x54, 0x54, 0x00, 0x00, 0x33, 0x28, 0x13, 0x00, 0x00, 0x08, 0x00, 0x00, 0x90, 0x0A }; +static volatile bool newPosData = FALSE; static volatile bool newBattData = FALSE; static volatile bool newBaroData = FALSE; -static enum -{ - STATE_DETECT, - STATE_UPDATE_CONF, - STATE_DUMP_CONF, - STTE_RUNNING - +static enum { + STATE_DETECT, + STATE_UPDATE_CONF, + STATE_DUMP_CONF, + STTE_RUNNING } state; static UAVObjEvent ev; @@ -130,408 +128,407 @@ static uint32_t version = 0; // static void WriteToMsg8(uint8_t index, uint8_t value) { - if (value > 100) - value = 100; + if (value > 100) { + value = 100; + } - msg[index] = ((value / 10) << 4) + (value % 10); + msg[index] = ((value / 10) << 4) + (value % 10); } static void WriteToMsg16(uint8_t index, uint16_t value) { - WriteToMsg8(index, value % 100); - WriteToMsg8(index + 1, value / 100); + WriteToMsg8(index, value % 100); + WriteToMsg8(index + 1, value / 100); } static void WriteToMsg24(uint8_t index, uint32_t value) { - WriteToMsg16(index, value % 10000); - WriteToMsg8(index + 2, value / 10000); + WriteToMsg16(index, value % 10000); + WriteToMsg8(index + 2, value / 10000); } static void WriteToMsg32(uint8_t index, uint32_t value) { - WriteToMsg16(index, value % 10000); - WriteToMsg16(index + 2, value / 10000); + WriteToMsg16(index, value % 10000); + WriteToMsg16(index + 2, value / 10000); } static void SetCoord(uint8_t index, uint32_t coord) { - #define E7 10000000 - uint8_t deg = coord / E7; - float sec = (float)(coord - deg*E7) / ((float)E7/(60.0*10000)); - - WriteToMsg8(index + 3, deg); - WriteToMsg24(index, sec); + uint8_t deg = coord / E7; + float sec = (float)(coord - deg * E7) / ((float)E7 / (60.0 * 10000)); + WriteToMsg8(index + 3, deg); + WriteToMsg24(index, sec); } static void SetCourse(uint16_t dir) { - WriteToMsg16(OSDMSG_HOME_IDX, dir); + WriteToMsg16(OSDMSG_HOME_IDX, dir); } static void SetBaroAltitude(int16_t altitudeMeter) { - // calculated formula - // ET OSD uses first update as zeropoint and then +- from that - altitudeMeter=(4571-altitudeMeter)/0.37; - msg[OSDMSG_BALT_IDX1] = (uint8_t)(altitudeMeter&0x00FF); - msg[OSDMSG_BALT_IDX2] = (altitudeMeter >> 8)&0x3F; + // calculated formula + // ET OSD uses first update as zeropoint and then +- from that + altitudeMeter = (4571 - altitudeMeter) / 0.37; + msg[OSDMSG_BALT_IDX1] = (uint8_t)(altitudeMeter & 0x00FF); + msg[OSDMSG_BALT_IDX2] = (altitudeMeter >> 8) & 0x3F; } static void SetAltitude(uint32_t altitudeMeter) { - WriteToMsg32(OSDMSG_ALT_IDX, altitudeMeter * 10); + WriteToMsg32(OSDMSG_ALT_IDX, altitudeMeter * 10); } static void SetVoltage(uint32_t milliVolt) { - msg[OSDMSG_VA_MS_IDX] &= 0x0F; - msg[OSDMSG_VA_MS_IDX] |= (milliVolt / 6444) << 4; - msg[OSDMSG_V_LS_IDX] = (milliVolt % 6444) * 256 / 6444; + msg[OSDMSG_VA_MS_IDX] &= 0x0F; + msg[OSDMSG_VA_MS_IDX] |= (milliVolt / 6444) << 4; + msg[OSDMSG_V_LS_IDX] = (milliVolt % 6444) * 256 / 6444; } static void SetCurrent(uint32_t milliAmp) { - uint32_t value = (milliAmp * 16570 / 1000000) + 0x7FA; - msg[OSDMSG_VA_MS_IDX] &= 0xF0; - msg[OSDMSG_VA_MS_IDX] |= ((value >> 8) & 0x0F); - msg[OSDMSG_A_LS_IDX] = (value & 0xFF); + uint32_t value = (milliAmp * 16570 / 1000000) + 0x7FA; + + msg[OSDMSG_VA_MS_IDX] &= 0xF0; + msg[OSDMSG_VA_MS_IDX] |= ((value >> 8) & 0x0F); + msg[OSDMSG_A_LS_IDX] = (value & 0xFF); } static void SetNbSats(uint8_t nb) { - msg[OSDMSG_NB_SATS] = nb; + msg[OSDMSG_NB_SATS] = nb; } static void FlightBatteryStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - newBattData = TRUE; + newBattData = TRUE; } static void GPSPositionUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - newPosData = TRUE; + newPosData = TRUE; } static void BaroAltitudeUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - newBaroData = TRUE; + newBaroData = TRUE; } -static bool Read(uint32_t start, uint8_t length, uint8_t * buffer) +static bool Read(uint32_t start, uint8_t length, uint8_t *buffer) { - uint8_t cmd[5]; + uint8_t cmd[5]; - const struct pios_i2c_txn txn_list[] = { - { - .addr = OSD_ADDRESS, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(cmd), - .buf = cmd, - } - , - { - .addr = OSD_ADDRESS, - .rw = PIOS_I2C_TXN_READ, - .len = length, - .buf = buffer, - } - , - }; + const struct pios_i2c_txn txn_list[] = { + { + .addr = OSD_ADDRESS, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(cmd), + .buf = cmd, + } + , + { + .addr = OSD_ADDRESS, + .rw = PIOS_I2C_TXN_READ, + .len = length, + .buf = buffer, + } + , + }; - cmd[0] = 0x02; - cmd[1] = 0x05; - cmd[2] = (uint8_t) (start & 0xFF); - cmd[3] = (uint8_t) (start >> 8); - cmd[4] = length; + cmd[0] = 0x02; + cmd[1] = 0x05; + cmd[2] = (uint8_t)(start & 0xFF); + cmd[3] = (uint8_t)(start >> 8); + cmd[4] = length; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)) == 0; + return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)) == 0; } -static bool Write(uint32_t start, uint8_t length, const uint8_t * buffer) +static bool Write(uint32_t start, uint8_t length, const uint8_t *buffer) { - uint8_t cmd[125]; - uint8_t ack[3]; + uint8_t cmd[125]; + uint8_t ack[3]; - const struct pios_i2c_txn txn_list1[] = { - { - .addr = OSD_ADDRESS, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(cmd), - .buf = cmd, - } - , - { - .addr = OSD_ADDRESS, - .rw = PIOS_I2C_TXN_READ, - .len = sizeof(ack), - .buf = ack, - } - , - }; + const struct pios_i2c_txn txn_list1[] = { + { + .addr = OSD_ADDRESS, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(cmd), + .buf = cmd, + } + , + { + .addr = OSD_ADDRESS, + .rw = PIOS_I2C_TXN_READ, + .len = sizeof(ack), + .buf = ack, + } + , + }; - if (length + 5 > sizeof(cmd)) { - // Too big - return FALSE; - } + if (length + 5 > sizeof(cmd)) { + // Too big + return FALSE; + } - cmd[0] = 0x01; - cmd[1] = 0x7D; - cmd[2] = (uint8_t) (start & 0xFF); - cmd[3] = (uint8_t) (start >> 8); - cmd[4] = length; - memcpy(&cmd[5], buffer, length); + cmd[0] = 0x01; + cmd[1] = 0x7D; + cmd[2] = (uint8_t)(start & 0xFF); + cmd[3] = (uint8_t)(start >> 8); + cmd[4] = length; + memcpy(&cmd[5], buffer, length); - ack[0] = 0; + ack[0] = 0; - // - // FIXME: See OP-305, the driver seems to return FALSE while all is OK - // - PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list1, NELEMENTS(txn_list1)); -// if (PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list1, NELEMENTS(txn_list1))) { - //DEBUG_MSG("ACK=%d ", ack[0]); - if (ack[0] == 49) { - return TRUE; - } -// } + // + // FIXME: See OP-305, the driver seems to return FALSE while all is OK + // + PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list1, NELEMENTS(txn_list1)); +// if (PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list1, NELEMENTS(txn_list1))) { + // DEBUG_MSG("ACK=%d ", ack[0]); + if (ack[0] == 49) { + return TRUE; + } +// } - return FALSE; + return FALSE; } static uint32_t ReadSwVersion(void) { - uint8_t buf[4]; - uint32_t version; + uint8_t buf[4]; + uint32_t version; - if (Read(0, 4, buf)) { - version = (buf[0] - '0') * 100; - version += (buf[2] - '0') * 10; - version += (buf[3] - '0'); - } else { - version = 0; - } + if (Read(0, 4, buf)) { + version = (buf[0] - '0') * 100; + version += (buf[2] - '0') * 10; + version += (buf[3] - '0'); + } else { + version = 0; + } - return version; + return version; } static void UpdateConfig(void) { - uint8_t buf[120]; - uint32_t addr = 0; - uint32_t n; - FILEINFO file; - uint32_t res; + uint8_t buf[120]; + uint32_t addr = 0; + uint32_t n; + FILEINFO file; + uint32_t res; - // Try to open the file that contains a new config - res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *) UpdateConfFilePath, DFS_READ, PIOS_SDCARD_Sector, &file); - if (res == DFS_OK) { - uint32_t bytesRead; - bool ok = TRUE; + // Try to open the file that contains a new config + res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)UpdateConfFilePath, DFS_READ, PIOS_SDCARD_Sector, &file); + if (res == DFS_OK) { + uint32_t bytesRead; + bool ok = TRUE; - DEBUG_MSG("Updating Config "); + DEBUG_MSG("Updating Config "); - // Write the config-data in blocks to OSD - while (addr < CONFIG_LENGTH && ok) { - n = MIN(CONFIG_LENGTH - addr, sizeof(buf)); - res = DFS_ReadFile(&file, PIOS_SDCARD_Sector, buf, &bytesRead, n); - if (res == DFS_OK && bytesRead == n) { - ok = Write(addr, n, buf); - if (ok) { - //DEBUG_MSG(" %d %d\n", addr, n); - DEBUG_MSG("."); - addr += n; - } - } else { - DEBUG_MSG(" FILEREAD FAILED "); - } - } + // Write the config-data in blocks to OSD + while (addr < CONFIG_LENGTH && ok) { + n = MIN(CONFIG_LENGTH - addr, sizeof(buf)); + res = DFS_ReadFile(&file, PIOS_SDCARD_Sector, buf, &bytesRead, n); + if (res == DFS_OK && bytesRead == n) { + ok = Write(addr, n, buf); + if (ok) { + // DEBUG_MSG(" %d %d\n", addr, n); + DEBUG_MSG("."); + addr += n; + } + } else { + DEBUG_MSG(" FILEREAD FAILED "); + } + } - DEBUG_MSG(ok ? " OK\n" : "FAILURE\n"); + DEBUG_MSG(ok ? " OK\n" : "FAILURE\n"); - // If writing is OK, read the data back and verify - if (ok) { - DEBUG_MSG("Verify Config "); - DFS_Seek(&file, 0, PIOS_SDCARD_Sector); + // If writing is OK, read the data back and verify + if (ok) { + DEBUG_MSG("Verify Config "); + DFS_Seek(&file, 0, PIOS_SDCARD_Sector); - addr = 0; - while (addr < CONFIG_LENGTH && ok) { - // First half of the buffer is used to store the data read from the OSD, the second half will contain the data from the file - n = MIN(CONFIG_LENGTH - addr, sizeof(buf) / 2); - ok = Read(addr, n, buf); - if (ok) { - uint32_t bytesRead; - res = DFS_ReadFile(&file, PIOS_SDCARD_Sector, buf + sizeof(buf) / 2, &bytesRead, n); - if (res == DFS_OK && bytesRead == n) { - DEBUG_MSG("."); - addr += n; + addr = 0; + while (addr < CONFIG_LENGTH && ok) { + // First half of the buffer is used to store the data read from the OSD, the second half will contain the data from the file + n = MIN(CONFIG_LENGTH - addr, sizeof(buf) / 2); + ok = Read(addr, n, buf); + if (ok) { + uint32_t bytesRead; + res = DFS_ReadFile(&file, PIOS_SDCARD_Sector, buf + sizeof(buf) / 2, &bytesRead, n); + if (res == DFS_OK && bytesRead == n) { + DEBUG_MSG("."); + addr += n; - if (memcmp(buf, buf + sizeof(buf) / 2, n) != 0) { - DEBUG_MSG(" MISMATCH "); - ok = FALSE; - } - } - } - } - DEBUG_MSG(ok ? " OK\n" : "FAILURE\n"); - } + if (memcmp(buf, buf + sizeof(buf) / 2, n) != 0) { + DEBUG_MSG(" MISMATCH "); + ok = FALSE; + } + } + } + } + DEBUG_MSG(ok ? " OK\n" : "FAILURE\n"); + } - DFS_Close(&file); + DFS_Close(&file); - // When the config was updated correctly, remove the config-file - if (ok) { - DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *) UpdateConfFilePath, PIOS_SDCARD_Sector); - } - - } + // When the config was updated correctly, remove the config-file + if (ok) { + DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)UpdateConfFilePath, PIOS_SDCARD_Sector); + } + } } static void DumpConfig(void) { #ifdef DUMP_CONFIG - uint8_t buf[100]; - uint32_t addr = 0; - uint32_t n; - FILEINFO file; - uint32_t res; + uint8_t buf[100]; + uint32_t addr = 0; + uint32_t n; + FILEINFO file; + uint32_t res; - DEBUG_MSG("Dumping Config "); + DEBUG_MSG("Dumping Config "); - res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *) DumpConfFilePath, DFS_WRITE, PIOS_SDCARD_Sector, &file); - if (res == DFS_OK) { - uint32_t bytesWritten; - bool ok = TRUE; + res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)DumpConfFilePath, DFS_WRITE, PIOS_SDCARD_Sector, &file); + if (res == DFS_OK) { + uint32_t bytesWritten; + bool ok = TRUE; - while (addr < CONFIG_LENGTH && ok) { - n = MIN(CONFIG_LENGTH - addr, sizeof(buf)); - ok = Read(addr, n, buf); - if (ok) { - res = DFS_WriteFile(&file, PIOS_SDCARD_Sector, buf, &bytesWritten, n); - if (res == DFS_OK && bytesWritten == n) { - //DEBUG_MSG(" %d %d\n", addr, n); - DEBUG_MSG("."); - addr += n; - } - } - } + while (addr < CONFIG_LENGTH && ok) { + n = MIN(CONFIG_LENGTH - addr, sizeof(buf)); + ok = Read(addr, n, buf); + if (ok) { + res = DFS_WriteFile(&file, PIOS_SDCARD_Sector, buf, &bytesWritten, n); + if (res == DFS_OK && bytesWritten == n) { + // DEBUG_MSG(" %d %d\n", addr, n); + DEBUG_MSG("."); + addr += n; + } + } + } - DEBUG_MSG(ok ? " OK\n" : "FAILURE\n"); + DEBUG_MSG(ok ? " OK\n" : "FAILURE\n"); - DFS_Close(&file); - } else { - DEBUG_MSG("Error Opening File %x\n", res); - } -#endif + DFS_Close(&file); + } else { + DEBUG_MSG("Error Opening File %x\n", res); + } +#endif /* ifdef DUMP_CONFIG */ } static void Run(void) { - static uint32_t cnt = 0; + static uint32_t cnt = 0; - if (newBattData) { - FlightBatteryStateData flightBatteryData; + if (newBattData) { + FlightBatteryStateData flightBatteryData; - FlightBatteryStateGet(&flightBatteryData); + FlightBatteryStateGet(&flightBatteryData); - //DEBUG_MSG("%5d Batt: V=%dmV\n\r", cnt, (uint32_t)(flightBatteryData.Voltage*1000)); + // DEBUG_MSG("%5d Batt: V=%dmV\n\r", cnt, (uint32_t)(flightBatteryData.Voltage*1000)); - SetVoltage((uint32_t) (flightBatteryData.Voltage * 1000)); - SetCurrent((uint32_t) (flightBatteryData.Current * 1000)); - newBattData = FALSE; - } + SetVoltage((uint32_t)(flightBatteryData.Voltage * 1000)); + SetCurrent((uint32_t)(flightBatteryData.Current * 1000)); + newBattData = FALSE; + } - if (newPosData) { - GPSPositionData positionData; - AttitudeActualData attitudeActualData; + if (newPosData) { + GPSPositionData positionData; + AttitudeActualData attitudeActualData; - GPSPositionGet(&positionData); - AttitudeActualGet(&attitudeActualData); + GPSPositionGet(&positionData); + AttitudeActualGet(&attitudeActualData); - //DEBUG_MSG("%5d Pos: #stat=%d #sats=%d alt=%d\n\r", cnt, - // positionData.Status, positionData.Satellites, (uint32_t)positionData.Altitude); + // DEBUG_MSG("%5d Pos: #stat=%d #sats=%d alt=%d\n\r", cnt, + // positionData.Status, positionData.Satellites, (uint32_t)positionData.Altitude); - // GPS Status - if (positionData.Status == GPSPOSITION_STATUS_FIX3D) - msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_FIX; - else - msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_NOFIX; - msg[OSDMSG_GPS_STAT] |= OSDMSG_GPS_STAT_HB_FLAG; + // GPS Status + if (positionData.Status == GPSPOSITION_STATUS_FIX3D) { + msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_FIX; + } else { + msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_NOFIX; + } + msg[OSDMSG_GPS_STAT] |= OSDMSG_GPS_STAT_HB_FLAG; - // GPS info - SetCoord(OSDMSG_LAT_IDX, positionData.Latitude); - SetCoord(OSDMSG_LON_IDX, positionData.Longitude); - SetAltitude(positionData.Altitude); - SetNbSats(positionData.Satellites); - SetCourse(attitudeActualData.Yaw); + // GPS info + SetCoord(OSDMSG_LAT_IDX, positionData.Latitude); + SetCoord(OSDMSG_LON_IDX, positionData.Longitude); + SetAltitude(positionData.Altitude); + SetNbSats(positionData.Satellites); + SetCourse(attitudeActualData.Yaw); - newPosData = FALSE; - } else { - msg[OSDMSG_GPS_STAT] &= ~OSDMSG_GPS_STAT_HB_FLAG; - } - if (newBaroData) { - BaroAltitudeData baroData; + newPosData = FALSE; + } else { + msg[OSDMSG_GPS_STAT] &= ~OSDMSG_GPS_STAT_HB_FLAG; + } + if (newBaroData) { + BaroAltitudeData baroData; - BaroAltitudeGet(&baroData); - SetBaroAltitude(baroData.Altitude); + BaroAltitudeGet(&baroData); + SetBaroAltitude(baroData.Altitude); - newBaroData = FALSE; - } + newBaroData = FALSE; + } - DEBUG_MSG("SendMsg %d\n",cnt); - { - DebugPinHigh(DEBUG_PIN_I2C); - const struct pios_i2c_txn txn_list[] = { - { - .addr = OSD_ADDRESS, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(msg), - .buf = msg, - } - , - }; + DEBUG_MSG("SendMsg %d\n", cnt); + { + DebugPinHigh(DEBUG_PIN_I2C); + const struct pios_i2c_txn txn_list[] = { + { + .addr = OSD_ADDRESS, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(msg), + .buf = msg, + } + , + }; - PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); - DebugPinLow(DEBUG_PIN_I2C); - } + PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + DebugPinLow(DEBUG_PIN_I2C); + } - cnt++; + cnt++; } -static void onTimer(UAVObjEvent * ev) +static void onTimer(UAVObjEvent *ev) { - DebugPinHigh(DEBUG_PIN_RUNNING); + DebugPinHigh(DEBUG_PIN_RUNNING); - #ifdef ENABLE_DEBUG_MSG - PIOS_COM_ChangeBaud(DEBUG_PORT, 57600); - #endif + #ifdef ENABLE_DEBUG_MSG + PIOS_COM_ChangeBaud(DEBUG_PORT, 57600); + #endif - if (state == STATE_DETECT) { - version = ReadSwVersion(); - DEBUG_MSG("SW: %d ", version); + if (state == STATE_DETECT) { + version = ReadSwVersion(); + DEBUG_MSG("SW: %d ", version); - if (version == SUPPORTED_VERSION) { - DEBUG_MSG("OK\n"); - state++; - } else { - DEBUG_MSG("INVALID\n"); - } - } else if (state == STATE_UPDATE_CONF) { - UpdateConfig(); - state++; - } else if (state == STATE_DUMP_CONF) { - DumpConfig(); - state++; - } else if (state == STTE_RUNNING) { - Run(); - } else { - // should not happen.. - state = STATE_DETECT; - } - - DebugPinLow(DEBUG_PIN_RUNNING); + if (version == SUPPORTED_VERSION) { + DEBUG_MSG("OK\n"); + state++; + } else { + DEBUG_MSG("INVALID\n"); + } + } else if (state == STATE_UPDATE_CONF) { + UpdateConfig(); + state++; + } else if (state == STATE_DUMP_CONF) { + DumpConfig(); + state++; + } else if (state == STTE_RUNNING) { + Run(); + } else { + // should not happen.. + state = STATE_DETECT; + } + DebugPinLow(DEBUG_PIN_RUNNING); } // @@ -546,12 +543,12 @@ static void onTimer(UAVObjEvent * ev) */ int32_t OsdEtStdInitialize(void) { - GPSPositionConnectCallback(GPSPositionUpdatedCb); - FlightBatteryStateConnectCallback(FlightBatteryStateUpdatedCb); - BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb); + GPSPositionConnectCallback(GPSPositionUpdatedCb); + FlightBatteryStateConnectCallback(FlightBatteryStateUpdatedCb); + BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb); - memset(&ev,0,sizeof(UAVObjEvent)); - EventPeriodicCallbackCreate(&ev, onTimer, 100 / portTICK_RATE_MS); + memset(&ev, 0, sizeof(UAVObjEvent)); + EventPeriodicCallbackCreate(&ev, onTimer, 100 / portTICK_RATE_MS); - return 0; + return 0; } diff --git a/flight/modules/Osd/WavPlayer/wavplayer.c b/flight/modules/Osd/WavPlayer/wavplayer.c index 2bf8949ea..bb1fd93dd 100644 --- a/flight/modules/Osd/WavPlayer/wavplayer.c +++ b/flight/modules/Osd/WavPlayer/wavplayer.c @@ -39,9 +39,9 @@ static void WavPlayerTask(void *parameters); // **************** // Private constants -#define STACK_SIZE_BYTES 1600 +#define STACK_SIZE_BYTES 1600 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // **************** // Private variables @@ -55,7 +55,7 @@ static uint32_t timeOfLastUpdateMs; int32_t WavPlayerStart(void) { // Start WavPlayer task - xTaskCreate(WavPlayerTask, (signed char *) "WavPlayer", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &WavPlayerTaskHandle); + xTaskCreate(WavPlayerTask, (signed char *)"WavPlayer", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &WavPlayerTaskHandle); return 0; } @@ -66,10 +66,9 @@ int32_t WavPlayerStart(void) */ int32_t WavPlayerInitialize(void) { - return 0; } -MODULE_INITCALL( WavPlayerInitialize, WavPlayerStart) +MODULE_INITCALL(WavPlayerInitialize, WavPlayerStart) // **************** /** @@ -79,11 +78,12 @@ MODULE_INITCALL( WavPlayerInitialize, WavPlayerStart) static void WavPlayerTask(__attribute__((unused)) void *parameters) { portTickType lastSysTime; + // Loop forever lastSysTime = xTaskGetTickCount(); uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - timeOfLastUpdateMs = timeNowMs; + timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; #if defined(PIOS_INCLUDE_WAVE) WavePlayer_Start(); diff --git a/flight/modules/Osd/osdgen/inc/osdgen.h b/flight/modules/Osd/osdgen/inc/osdgen.h index ac5621f51..35d7c3020 100644 --- a/flight/modules/Osd/osdgen/inc/osdgen.h +++ b/flight/modules/Osd/osdgen/inc/osdgen.h @@ -39,21 +39,21 @@ int32_t osdgenInitialize(void); // Size of an array (num items.) #define SIZEOF_ARRAY(x) (sizeof(x) / sizeof((x)[0])) -#define HUD_VSCALE_FLAG_CLEAR 1 -#define HUD_VSCALE_FLAG_NO_NEGATIVE 2 +#define HUD_VSCALE_FLAG_CLEAR 1 +#define HUD_VSCALE_FLAG_NO_NEGATIVE 2 // Macros for computing addresses and bit positions. // NOTE: /16 in y is because we are addressing by word not byte. -#define CALC_BUFF_ADDR(x, y) (((x) / 8) + ((y) * (GRAPHICS_WIDTH_REAL / 8))) -#define CALC_BIT_IN_WORD(x) ((x) & 7) +#define CALC_BUFF_ADDR(x, y) (((x) / 8) + ((y) * (GRAPHICS_WIDTH_REAL / 8))) +#define CALC_BIT_IN_WORD(x) ((x) & 7) #define DEBUG_DELAY // Macro for writing a word with a mode (NAND = clear, OR = set, XOR = toggle) // at a given position #define WRITE_WORD_MODE(buff, addr, mask, mode) \ - switch(mode) { \ - case 0: buff[addr] &= ~mask; break; \ - case 1: buff[addr] |= mask; break; \ - case 2: buff[addr] ^= mask; break; } + switch (mode) { \ + case 0: buff[addr] &= ~mask; break; \ + case 1: buff[addr] |= mask; break; \ + case 2: buff[addr] ^= mask; break; } #define WRITE_WORD_NAND(buff, addr, mask) { buff[addr] &= ~mask; DEBUG_DELAY; } #define WRITE_WORD_OR(buff, addr, mask) { buff[addr] |= mask; DEBUG_DELAY; } @@ -61,103 +61,102 @@ int32_t osdgenInitialize(void); // Horizontal line calculations. // Edge cases. -#define COMPUTE_HLINE_EDGE_L_MASK(b) ((1 << (8 - (b))) - 1) -#define COMPUTE_HLINE_EDGE_R_MASK(b) (~((1 << (7 - (b))) - 1)) +#define COMPUTE_HLINE_EDGE_L_MASK(b) ((1 << (8 - (b))) - 1) +#define COMPUTE_HLINE_EDGE_R_MASK(b) (~((1 << (7 - (b))) - 1)) // This computes an island mask. #define COMPUTE_HLINE_ISLAND_MASK(b0, b1) (COMPUTE_HLINE_EDGE_L_MASK(b0) ^ COMPUTE_HLINE_EDGE_L_MASK(b1)); // Macro for initializing stroke/fill modes. Add new modes here // if necessary. #define SETUP_STROKE_FILL(stroke, fill, mode) \ - stroke = 0; fill = 0; \ - if(mode == 0) { stroke = 0; fill = 1; } \ - if(mode == 1) { stroke = 1; fill = 0; } \ + stroke = 0; fill = 0; \ + if (mode == 0) { stroke = 0; fill = 1; } \ + if (mode == 1) { stroke = 1; fill = 0; } \ // Line endcaps (for horizontal and vertical lines.) -#define ENDCAP_NONE 0 -#define ENDCAP_ROUND 1 -#define ENDCAP_FLAT 2 +#define ENDCAP_NONE 0 +#define ENDCAP_ROUND 1 +#define ENDCAP_FLAT 2 #define DRAW_ENDCAP_HLINE(e, x, y, s, f, l) \ - if((e) == ENDCAP_ROUND) /* single pixel endcap */ \ - { write_pixel_lm(x, y, f, l); } \ - else if((e) == ENDCAP_FLAT) /* flat endcap: FIXME, quicker to draw a vertical line(?) */ \ - { write_pixel_lm(x, y - 1, s, l); write_pixel_lm(x, y, s, l); write_pixel_lm(x, y + 1, s, l); } + if ((e) == ENDCAP_ROUND) /* single pixel endcap */ \ + { write_pixel_lm(x, y, f, l); } \ + else if ((e) == ENDCAP_FLAT) /* flat endcap: FIXME, quicker to draw a vertical line(?) */ \ + { write_pixel_lm(x, y - 1, s, l); write_pixel_lm(x, y, s, l); write_pixel_lm(x, y + 1, s, l); } #define DRAW_ENDCAP_VLINE(e, x, y, s, f, l) \ - if((e) == ENDCAP_ROUND) /* single pixel endcap */ \ - { write_pixel_lm(x, y, f, l); } \ - else if((e) == ENDCAP_FLAT) /* flat endcap: FIXME, quicker to draw a horizontal line(?) */ \ - { write_pixel_lm(x - 1, y, s, l); write_pixel_lm(x, y, s, l); write_pixel_lm(x + 1, y, s, l); } + if ((e) == ENDCAP_ROUND) /* single pixel endcap */ \ + { write_pixel_lm(x, y, f, l); } \ + else if ((e) == ENDCAP_FLAT) /* flat endcap: FIXME, quicker to draw a horizontal line(?) */ \ + { write_pixel_lm(x - 1, y, s, l); write_pixel_lm(x, y, s, l); write_pixel_lm(x + 1, y, s, l); } // Macros for writing pixels in a midpoint circle algorithm. #define CIRCLE_PLOT_8(buff, cx, cy, x, y, mode) \ - CIRCLE_PLOT_4(buff, cx, cy, x, y, mode); \ - if((x) != (y)) CIRCLE_PLOT_4(buff, cx, cy, y, x, mode); + CIRCLE_PLOT_4(buff, cx, cy, x, y, mode); \ + if ((x) != (y)) { CIRCLE_PLOT_4(buff, cx, cy, y, x, mode); } #define CIRCLE_PLOT_4(buff, cx, cy, x, y, mode) \ - write_pixel(buff, (cx) + (x), (cy) + (y), mode); \ - write_pixel(buff, (cx) - (x), (cy) + (y), mode); \ - write_pixel(buff, (cx) + (x), (cy) - (y), mode); \ - write_pixel(buff, (cx) - (x), (cy) - (y), mode); + write_pixel(buff, (cx) + (x), (cy) + (y), mode); \ + write_pixel(buff, (cx) - (x), (cy) + (y), mode); \ + write_pixel(buff, (cx) + (x), (cy) - (y), mode); \ + write_pixel(buff, (cx) - (x), (cy) - (y), mode); // Font flags. -#define FONT_BOLD 1 // bold text (no outline) -#define FONT_INVERT 2 // invert: border white, inside black +#define FONT_BOLD 1 // bold text (no outline) +#define FONT_INVERT 2 // invert: border white, inside black // Text alignments. -#define TEXT_VA_TOP 0 -#define TEXT_VA_MIDDLE 1 -#define TEXT_VA_BOTTOM 2 -#define TEXT_HA_LEFT 0 -#define TEXT_HA_CENTER 1 -#define TEXT_HA_RIGHT 2 +#define TEXT_VA_TOP 0 +#define TEXT_VA_MIDDLE 1 +#define TEXT_VA_BOTTOM 2 +#define TEXT_HA_LEFT 0 +#define TEXT_HA_CENTER 1 +#define TEXT_HA_RIGHT 2 // Text dimension structures. -struct FontDimensions -{ +struct FontDimensions { int width, height; }; // Max/Min macros. -#define MAX(a, b) ((a) > (b) ? (a) : (b)) -#define MIN(a, b) ((a) < (b) ? (a) : (b)) -#define MAX3(a, b, c) MAX(a, MAX(b, c)) -#define MIN3(a, b, c) MIN(a, MIN(b, c)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#define MAX3(a, b, c) MAX(a, MAX(b, c)) +#define MIN3(a, b, c) MIN(a, MIN(b, c)) // Apply DeadBand -#define APPLY_DEADBAND(x, y) { x = (x)+GRAPHICS_HDEADBAND; y=(y)+GRAPHICS_VDEADBAND; } -#define APPLY_VDEADBAND(y) ((y)+GRAPHICS_VDEADBAND) -#define APPLY_HDEADBAND(x) ((x)+GRAPHICS_HDEADBAND) +#define APPLY_DEADBAND(x, y) { x = (x) + GRAPHICS_HDEADBAND; y = (y) + GRAPHICS_VDEADBAND; } +#define APPLY_VDEADBAND(y) ((y) + GRAPHICS_VDEADBAND) +#define APPLY_HDEADBAND(x) ((x) + GRAPHICS_HDEADBAND) // Check if coordinates are valid. If not, return. Assumes unsigned coordinate -#define CHECK_COORDS(x, y) if(x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) return; -#define CHECK_COORD_X(x) if(x >= GRAPHICS_WIDTH_REAL) return; -#define CHECK_COORD_Y(y) if(y >= GRAPHICS_HEIGHT_REAL) return; +#define CHECK_COORDS(x, y) if (x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) { return; } +#define CHECK_COORD_X(x) if (x >= GRAPHICS_WIDTH_REAL) { return; } +#define CHECK_COORD_Y(y) if (y >= GRAPHICS_HEIGHT_REAL) { return; } // Clip coordinates out of range - assumes unsigned coordinate -#define CLIP_COORD_X(x) { x = MIN(x, GRAPHICS_WIDTH_REAL); } -#define CLIP_COORD_Y(y) { y = MIN(y, GRAPHICS_HEIGHT_REAL); } -#define CLIP_COORDS(x, y) { CLIP_COORD_X(x); CLIP_COORD_Y(y); } +#define CLIP_COORD_X(x) { x = MIN(x, GRAPHICS_WIDTH_REAL); } +#define CLIP_COORD_Y(y) { y = MIN(y, GRAPHICS_HEIGHT_REAL); } +#define CLIP_COORDS(x, y) { CLIP_COORD_X(x); CLIP_COORD_Y(y); } // Macro to swap two variables using XOR swap. -#define SWAP(a, b) { a ^= b; b ^= a; a ^= b; } +#define SWAP(a, b) { a ^= b; b ^= a; a ^= b; } // Line triggering -#define LAST_LINE 312 //625/2 //PAL -//#define LAST_LINE 525/2 //NTSC +#define LAST_LINE 312 // 625/2 //PAL +// #define LAST_LINE 525/2 //NTSC // Global vars -#define DELAY_1_NOP() asm("nop\r\n") -#define DELAY_2_NOP() asm("nop\r\nnop\r\n") -#define DELAY_3_NOP() asm("nop\r\nnop\r\nnop\r\n") -#define DELAY_4_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\n") -#define DELAY_5_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") -#define DELAY_6_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") -#define DELAY_7_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") -#define DELAY_8_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") -#define DELAY_9_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") -#define DELAY_10_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") +#define DELAY_1_NOP() asm ("nop\r\n") +#define DELAY_2_NOP() asm ("nop\r\nnop\r\n") +#define DELAY_3_NOP() asm ("nop\r\nnop\r\nnop\r\n") +#define DELAY_4_NOP() asm ("nop\r\nnop\r\nnop\r\nnop\r\n") +#define DELAY_5_NOP() asm ("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") +#define DELAY_6_NOP() asm ("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") +#define DELAY_7_NOP() asm ("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") +#define DELAY_8_NOP() asm ("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") +#define DELAY_9_NOP() asm ("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") +#define DELAY_10_NOP() asm ("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n") uint8_t getCharData(uint16_t charPos); void introText(); @@ -166,7 +165,7 @@ void clearGraphics(); uint8_t validPos(uint16_t x, uint16_t y); void setPixel(uint16_t x, uint16_t y, uint8_t state); void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius); -void swap(uint16_t* a, uint16_t* b); +void swap(uint16_t *a, uint16_t *b); void drawLine(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1); void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2); void drawArrow(uint16_t x, uint16_t y, uint16_t angle, uint16_t size); @@ -197,9 +196,9 @@ void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsi void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff); void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff); void write_word_misaligned_lm(uint16_t wordl, uint16_t wordm, unsigned int addr, unsigned int xoff, int lmode, int mmode); -//int fetch_font_info(char ch, int font, struct FontEntry *font_info, char *lookup); +// int fetch_font_info(char ch, int font, struct FontEntry *font_info, char *lookup); void write_char(char ch, unsigned int x, unsigned int y, int flags, int font); -//void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, struct FontDimensions *dim); +// void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, struct FontDimensions *dim); void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags, int font); void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags); diff --git a/flight/modules/Osd/osdgen/osdgen.c b/flight/modules/Osd/osdgen/osdgen.c index 9e9d94d8a..654b0aeef 100644 --- a/flight/modules/Osd/osdgen/osdgen.c +++ b/flight/modules/Osd/osdgen/osdgen.c @@ -51,22 +51,22 @@ #include "splash.h" /* - static uint16_t angleA=0; - static int16_t angleB=90; - static int16_t angleC=0; - static int16_t sum=2; + static uint16_t angleA=0; + static int16_t angleB=90; + static int16_t angleC=0; + static int16_t sum=2; - static int16_t m_pitch=0; - static int16_t m_roll=0; - static int16_t m_yaw=0; - static int16_t m_batt=0; - static int16_t m_alt=0; + static int16_t m_pitch=0; + static int16_t m_roll=0; + static int16_t m_yaw=0; + static int16_t m_batt=0; + static int16_t m_alt=0; - static uint8_t m_gpsStatus=0; - static int32_t m_gpsLat=0; - static int32_t m_gpsLon=0; - static float m_gpsAlt=0; - static float m_gpsSpd=0;*/ + static uint8_t m_gpsStatus=0; + static int32_t m_gpsLat=0; + static int32_t m_gpsLon=0; + static float m_gpsAlt=0; + static float m_gpsSpd=0;*/ extern uint8_t *draw_buffer_level; extern uint8_t *draw_buffer_mask; @@ -82,75 +82,74 @@ static void osdgenTask(void *parameters); // **************** // Private constants -#define LONG_TIME 0xffff +#define LONG_TIME 0xffff xSemaphoreHandle osdSemaphore = NULL; -#define STACK_SIZE_BYTES 4096 +#define STACK_SIZE_BYTES 4096 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) -#define UPDATE_PERIOD 100 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) +#define UPDATE_PERIOD 100 // **************** // Private variables static xTaskHandle osdgenTaskHandle; -struct splashEntry -{ - unsigned int width, height; +struct splashEntry { + unsigned int width, height; const uint16_t *level; const uint16_t *mask; }; struct splashEntry splash[3] = { - { oplogo_width, - oplogo_height, - oplogo_bits, - oplogo_mask_bits }, - { level_width, - level_height, - level_bits, - level_mask_bits }, - { llama_width, - llama_height, - llama_bits, - llama_mask_bits } + { oplogo_width, + oplogo_height, + oplogo_bits, + oplogo_mask_bits }, + { level_width, + level_height, + level_bits, + level_mask_bits }, + { llama_width, + llama_height, + llama_bits, + llama_mask_bits } }; uint16_t mirror(uint16_t source) { int result = ((source & 0x8000) >> 7) | ((source & 0x4000) >> 5) | ((source & 0x2000) >> 3) | ((source & 0x1000) >> 1) | ((source & 0x0800) << 1) - | ((source & 0x0400) << 3) | ((source & 0x0200) << 5) | ((source & 0x0100) << 7) | ((source & 0x0080) >> 7) | ((source & 0x0040) >> 5) - | ((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) | ((source & 0x0008) << 1) | ((source & 0x0004) << 3) | ((source & 0x0002) << 5) - | ((source & 0x0001) << 7); + | ((source & 0x0400) << 3) | ((source & 0x0200) << 5) | ((source & 0x0100) << 7) | ((source & 0x0080) >> 7) | ((source & 0x0040) >> 5) + | ((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) | ((source & 0x0008) << 1) | ((source & 0x0004) << 3) | ((source & 0x0002) << 5) + | ((source & 0x0001) << 7); return result; } void clearGraphics() { - memset((uint8_t *) draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); - memset((uint8_t *) draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); + memset((uint8_t *)draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); + memset((uint8_t *)draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); } void copyimage(uint16_t offsetx, uint16_t offsety, int image) { - //check top/left position + // check top/left position if (!validPos(offsetx, offsety)) { return; } struct splashEntry splash_info; splash_info = splash[image]; - offsetx = offsetx / 8; + offsetx = offsetx / 8; for (uint16_t y = offsety; y < ((splash_info.height) + offsety); y++) { uint16_t x1 = offsetx; for (uint16_t x = offsetx; x < (((splash_info.width) / 16) + offsetx); x++) { draw_buffer_level[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)( - mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8); - draw_buffer_level[y * GRAPHICS_WIDTH + x1] = (uint8_t)( - mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF); - draw_buffer_mask[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)( - mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8); + mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8); + draw_buffer_level[y * GRAPHICS_WIDTH + x1] = (uint8_t)( + mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF); + draw_buffer_mask[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)( + mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8); draw_buffer_mask[y * GRAPHICS_WIDTH + x1] = (uint8_t)(mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF); x1 += 2; } @@ -168,11 +167,11 @@ uint8_t validPos(uint16_t x, uint16_t y) // Credit for this one goes to wikipedia! :-) void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) { - int f = 1 - radius; + int f = 1 - radius; int ddF_x = 1; int ddF_y = -2 * radius; - int x = 0; - int y = radius; + int x = 0; + int y = radius; write_pixel_lm(x0, y0 + radius, 1, 1); write_pixel_lm(x0, y0 - radius, 1, 1); @@ -186,11 +185,11 @@ void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) if (f >= 0) { y--; ddF_y += 2; - f += ddF_y; + f += ddF_y; } x++; ddF_x += 2; - f += ddF_x; + f += ddF_x; write_pixel_lm(x0 + x, y0 + y, 1, 1); write_pixel_lm(x0 - x, y0 + y, 1, 1); write_pixel_lm(x0 + x, y0 - y, 1, 1); @@ -202,26 +201,28 @@ void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) } } -void swap(uint16_t* a, uint16_t* b) +void swap(uint16_t *a, uint16_t *b) { uint16_t temp = *a; + *a = *b; *b = temp; } static const int8_t sinData[91] = -{ 0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33, 34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62, 63, 64, - 66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97, 97, 98, - 98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100 }; +{ 0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33, 34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62, 63, 64, + 66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97, 97, 98, + 98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100 }; static int8_t mySin(uint16_t angle) { uint16_t pos = 0; + pos = angle % 360; - int8_t mult = 1; + int8_t mult = 1; // 180-359 is same as 0-179 but negative. if (pos >= 180) { - pos = pos - 180; + pos = pos - 180; mult = -1; } // 0-89 is equal to 90-179 except backwards. @@ -250,10 +251,10 @@ static int8_t myCos(uint16_t angle) /// \param color the color to draw the pixels with. void plotFourQuadrants(int32_t centerX, int32_t centerY, int32_t deltaX, int32_t deltaY) { - write_pixel_lm(centerX + deltaX, centerY + deltaY, 1, 1); // Ist Quadrant - write_pixel_lm(centerX - deltaX, centerY + deltaY, 1, 1); // IInd Quadrant - write_pixel_lm(centerX - deltaX, centerY - deltaY, 1, 1); // IIIrd Quadrant - write_pixel_lm(centerX + deltaX, centerY - deltaY, 1, 1); // IVth Quadrant + write_pixel_lm(centerX + deltaX, centerY + deltaY, 1, 1); // Ist Quadrant + write_pixel_lm(centerX - deltaX, centerY + deltaY, 1, 1); // IInd Quadrant + write_pixel_lm(centerX - deltaX, centerY - deltaY, 1, 1); // IIIrd Quadrant + write_pixel_lm(centerX + deltaX, centerY - deltaY, 1, 1); // IVth Quadrant } /// Implements the midpoint ellipse drawing algorithm which is a bresenham @@ -267,7 +268,7 @@ void plotFourQuadrants(int32_t centerX, int32_t centerY, int32_t deltaX, int32_t void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius) { int64_t doubleHorizontalRadius = horizontalRadius * horizontalRadius; - int64_t doubleVerticalRadius = verticalRadius * verticalRadius; + int64_t doubleVerticalRadius = verticalRadius * verticalRadius; int64_t error = doubleVerticalRadius - doubleHorizontalRadius * verticalRadius + (doubleVerticalRadius >> 2); @@ -282,13 +283,13 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius) x++; deltaX += (doubleVerticalRadius << 1); - error += deltaX + doubleVerticalRadius; + error += deltaX + doubleVerticalRadius; if (error >= 0) { y--; deltaY -= (doubleHorizontalRadius << 1); - error -= deltaY; + error -= deltaY; } plotFourQuadrants(centerX, centerY, x, y); } @@ -296,15 +297,15 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius) error = (int64_t)(doubleVerticalRadius * (x + 1 / 2.0f) * (x + 1 / 2.0f) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius); while (y >= 0) { - error += doubleHorizontalRadius; + error += doubleHorizontalRadius; y--; deltaY -= (doubleHorizontalRadius << 1); - error -= deltaY; + error -= deltaY; if (error <= 0) { x++; deltaX += (doubleVerticalRadius << 1); - error += deltaX; + error += deltaX; } plotFourQuadrants(centerX, centerY, x, y); @@ -315,20 +316,21 @@ void drawArrow(uint16_t x, uint16_t y, uint16_t angle, uint16_t size) { int16_t a = myCos(angle); int16_t b = mySin(angle); + a = (a * (size / 2)) / 100; b = (b * (size / 2)) / 100; - write_line_lm((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 1, 1); //Direction line - //write_line_lm((GRAPHICS_SIZE/2)-1 + a/2, (GRAPHICS_SIZE/2)-1 + b/2, (GRAPHICS_SIZE/2)-1 - a/2, (GRAPHICS_SIZE/2)-1 - b/2, 1, 1); //Arrow bottom line + write_line_lm((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 1, 1); // Direction line + // write_line_lm((GRAPHICS_SIZE/2)-1 + a/2, (GRAPHICS_SIZE/2)-1 + b/2, (GRAPHICS_SIZE/2)-1 - a/2, (GRAPHICS_SIZE/2)-1 - b/2, 1, 1); //Arrow bottom line write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 - a / 2, (y) - 1 - b / 2, 1, 1); // Arrow "wings" write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 + a / 2, (y) - 1 + b / 2, 1, 1); } void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2) { - write_line_lm(x1, y1, x2, y1, 1, 1); //top - write_line_lm(x1, y1, x1, y2, 1, 1); //left - write_line_lm(x2, y1, x2, y2, 1, 1); //right - write_line_lm(x1, y2, x2, y2, 1, 1); //bottom + write_line_lm(x1, y1, x2, y1, 1, 1); // top + write_line_lm(x1, y1, x1, y2, 1, 1); // left + write_line_lm(x2, y1, x2, y2, 1, 1); // right + write_line_lm(x1, y2, x2, y2, 1, 1); // bottom } // simple routines @@ -348,8 +350,8 @@ void write_pixel(uint8_t *buff, unsigned int x, unsigned int y, int mode) CHECK_COORDS(x, y); // Determine the bit in the word to be set and the word // index to set it in. - int bitnum = CALC_BIT_IN_WORD(x); - int wordnum = CALC_BUFF_ADDR(x, y); + int bitnum = CALC_BIT_IN_WORD(x); + int wordnum = CALC_BUFF_ADDR(x, y); // Apply a mask. uint16_t mask = 1 << (7 - bitnum); WRITE_WORD_MODE(buff, wordnum, mask, mode); @@ -369,8 +371,8 @@ void write_pixel_lm(unsigned int x, unsigned int y, int mmode, int lmode) CHECK_COORDS(x, y); // Determine the bit in the word to be set and the word // index to set it in. - int bitnum = CALC_BIT_IN_WORD(x); - int wordnum = CALC_BUFF_ADDR(x, y); + int bitnum = CALC_BIT_IN_WORD(x); + int wordnum = CALC_BUFF_ADDR(x, y); // Apply the masks. uint16_t mask = 1 << (7 - bitnum); WRITE_WORD_MODE(draw_buffer_mask, wordnum, mask, mmode); @@ -397,9 +399,9 @@ void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y return; } /* This is an optimised algorithm for writing horizontal lines. - * We begin by finding the addresses of the x0 and x1 points. */ - int addr0 = CALC_BUFF_ADDR(x0, y); - int addr1 = CALC_BUFF_ADDR(x1, y); + * We begin by finding the addresses of the x0 and x1 points. */ + int addr0 = CALC_BUFF_ADDR(x0, y); + int addr1 = CALC_BUFF_ADDR(x1, y); int addr0_bit = CALC_BIT_IN_WORD(x0); int addr1_bit = CALC_BIT_IN_WORD(x1); int mask, mask_l, mask_r, i; @@ -454,6 +456,7 @@ void write_hline_lm(unsigned int x0, unsigned int x1, unsigned int y, int lmode, void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int endcap0, int endcap1, int mode, int mmode) { int stroke, fill; + SETUP_STROKE_FILL(stroke, fill, mode) if (x0 > x1) { SWAP(x0, x1); @@ -479,6 +482,7 @@ void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1, int mode) { unsigned int a; + CLIP_COORDS(x, y0); CLIP_COORDS(x, y1); if (y0 > y1) { @@ -489,8 +493,8 @@ void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1 } /* This is an optimised algorithm for writing vertical lines. * We begin by finding the addresses of the x,y0 and x,y1 points. */ - unsigned int addr0 = CALC_BUFF_ADDR(x, y0); - unsigned int addr1 = CALC_BUFF_ADDR(x, y1); + unsigned int addr0 = CALC_BUFF_ADDR(x, y0); + unsigned int addr1 = CALC_BUFF_ADDR(x, y1); /* Then we calculate the pixel data to be written. */ unsigned int bitnum = CALC_BIT_IN_WORD(x); uint16_t mask = 1 << (7 - bitnum); @@ -533,6 +537,7 @@ void write_vline_lm(unsigned int x, unsigned int y0, unsigned int y1, int lmode, void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int endcap0, int endcap1, int mode, int mmode) { int stroke, fill; + if (y0 > y1) { SWAP(y0, y1); } @@ -563,6 +568,7 @@ void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsigned int width, unsigned int height, int mode) { unsigned int yy, addr0_old, addr1_old; + CHECK_COORDS(x, y); CHECK_COORD_X(x + width); CHECK_COORD_Y(y + height); @@ -571,8 +577,8 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig } // Calculate as if the rectangle was only a horizontal line. We then // step these addresses through each row until we iterate `height` times. - unsigned int addr0 = CALC_BUFF_ADDR(x, y); - unsigned int addr1 = CALC_BUFF_ADDR(x + width, y); + unsigned int addr0 = CALC_BUFF_ADDR(x, y); + unsigned int addr1 = CALC_BUFF_ADDR(x + width, y); unsigned int addr0_bit = CALC_BIT_IN_WORD(x); unsigned int addr1_bit = CALC_BIT_IN_WORD(x + width); unsigned int mask, mask_l, mask_r, i; @@ -585,10 +591,10 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig } } else { // Otherwise we need to write the edges and then the middle repeatedly. - mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit); - mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit); + mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit); + mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit); // Write edges first. - yy = 0; + yy = 0; addr0_old = addr0; addr1_old = addr1; while (yy < height) { @@ -599,7 +605,7 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig yy++; } // Now write 0xffff words from start+1 to end-1 for each row. - yy = 0; + yy = 0; addr0 = addr0_old; addr1 = addr1_old; while (yy < height) { @@ -643,10 +649,10 @@ void write_filled_rectangle_lm(unsigned int x, unsigned int y, unsigned int widt */ void write_rectangle_outlined(unsigned int x, unsigned int y, int width, int height, int mode, int mmode) { - //CHECK_COORDS(x, y); - //CHECK_COORDS(x + width, y + height); - //if((x + width) > DISP_WIDTH) width = DISP_WIDTH - x; - //if((y + height) > DISP_HEIGHT) height = DISP_HEIGHT - y; + // CHECK_COORDS(x, y); + // CHECK_COORDS(x + width, y + height); + // if((x + width) > DISP_WIDTH) width = DISP_WIDTH - x; + // if((y + height) > DISP_HEIGHT) height = DISP_HEIGHT - y; write_hline_outlined(x, x + width, y, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); write_hline_outlined(x, x + width, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); write_vline_outlined(x, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); @@ -695,6 +701,7 @@ void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, unsigned int dashp, int bmode, int mode, int mmode) { int stroke, fill; + CHECK_COORDS(cx, cy); SETUP_STROKE_FILL(stroke, fill, mode); // This is a two step procedure. First, we draw the outline of the @@ -725,8 +732,8 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns } } error = -r; - x = r; - y = 0; + x = r; + y = 0; while (x >= y) { if (dashp == 0 || (y % dashp) < (dashp / 2)) { CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y, mmode); @@ -775,7 +782,7 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign y++; if (error >= 0) { --x; - xch = 1; + xch = 1; error -= x * 2; } } @@ -798,7 +805,8 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mode) { // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm - unsigned int steep = abs(y1 - y0) > abs(x1 - x0); + unsigned int steep = abs(y1 - y0) > abs(x1 - x0); + if (steep) { SWAP(x0, y0); SWAP(x1, y1); @@ -807,12 +815,12 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1 SWAP(x0, x1); SWAP(y0, y1); } - int deltax = x1 - x0; + int deltax = x1 - x0; unsigned int deltay = abs(y1 - y0); - int error = deltax / 2; + int error = deltax / 2; int ystep; unsigned int y = y0; - unsigned int x; //, lasty = y, stox = 0; + unsigned int x; // , lasty = y, stox = 0; if (y0 < y1) { ystep = 1; } else { @@ -826,7 +834,7 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1 } error -= deltay; if (error < 0) { - y += ystep; + y += ystep; error += deltax; } } @@ -862,12 +870,13 @@ void write_line_lm(unsigned int x0, unsigned int y0, unsigned int x1, unsigned i * @param mmode 0 = clear, 1 = set, 2 = toggle */ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, - __attribute__((unused)) int endcap0, __attribute__((unused)) int endcap1, - int mode, int mmode) + __attribute__((unused)) int endcap0, __attribute__((unused)) int endcap1, + int mode, int mmode) { // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm // This could be improved for speed. int omode, imode; + if (mode == 0) { omode = 0; imode = 1; @@ -884,9 +893,9 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi SWAP(x0, x1); SWAP(y0, y1); } - int deltax = x1 - x0; + int deltax = x1 - x0; unsigned int deltay = abs(y1 - y0); - int error = deltax / 2; + int error = deltax / 2; int ystep; unsigned int y = y0; unsigned int x; @@ -910,13 +919,13 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi } error -= deltay; if (error < 0) { - y += ystep; + y += ystep; error += deltax; } } // Now draw the innards. error = deltax / 2; - y = y0; + y = y0; for (x = x0; x < x1; x++) { if (steep) { write_pixel_lm(y, x, mmode, imode); @@ -925,7 +934,7 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi } error -= deltay; if (error < 0) { - y += ystep; + y += ystep; error += deltax; } } @@ -946,11 +955,12 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff, int mode) { int16_t firstmask = word >> xoff; - int16_t lastmask = word << (16 - xoff); - WRITE_WORD_MODE(buff, addr+1, firstmask && 0x00ff, mode); + int16_t lastmask = word << (16 - xoff); + + WRITE_WORD_MODE(buff, addr + 1, firstmask && 0x00ff, mode); WRITE_WORD_MODE(buff, addr, (firstmask & 0xff00) >> 8, mode); if (xoff > 0) { - WRITE_WORD_MODE(buff, addr+2, (lastmask & 0xff00) >> 8, mode); + WRITE_WORD_MODE(buff, addr + 2, (lastmask & 0xff00) >> 8, mode); } } @@ -972,11 +982,12 @@ void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsi void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff) { uint16_t firstmask = word >> xoff; - uint16_t lastmask = word << (16 - xoff); - WRITE_WORD_NAND(buff, addr+1, firstmask & 0x00ff); + uint16_t lastmask = word << (16 - xoff); + + WRITE_WORD_NAND(buff, addr + 1, firstmask & 0x00ff); WRITE_WORD_NAND(buff, addr, (firstmask & 0xff00) >> 8); if (xoff > 0) { - WRITE_WORD_NAND(buff, addr+2, (lastmask & 0xff00) >> 8); + WRITE_WORD_NAND(buff, addr + 2, (lastmask & 0xff00) >> 8); } } @@ -998,13 +1009,13 @@ void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr, void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff) { uint16_t firstmask = word >> xoff; - uint16_t lastmask = word << (16 - xoff); - WRITE_WORD_OR(buff, addr+1, firstmask & 0x00ff); + uint16_t lastmask = word << (16 - xoff); + + WRITE_WORD_OR(buff, addr + 1, firstmask & 0x00ff); WRITE_WORD_OR(buff, addr, (firstmask & 0xff00) >> 8); if (xoff > 0) { WRITE_WORD_OR(buff, addr + 2, (lastmask & 0xff00) >> 8); } - } /** @@ -1065,7 +1076,8 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font) unsigned int yy, addr_temp, row, row_temp, xshift; uint16_t and_mask, or_mask, levels; struct FontEntry font_info; - //char lookup = 0; + + // char lookup = 0; fetch_font_info(0, font, &font_info, NULL); // Compute starting address (for x,y) of character. @@ -1081,10 +1093,10 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font) return; } // Load data pointer. - row = ch * font_info.height; - row_temp = row; + row = ch * font_info.height; + row_temp = row; addr_temp = addr; - xshift = 16 - font_info.width; + xshift = 16 - font_info.width; // We can write mask words easily. for (yy = y; yy < y + font_info.height; yy++) { if (font == 3) { @@ -1099,25 +1111,25 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font) // level bits, but only where the mask bit is set; otherwise, // we need to leave them alone. To do this, for each word, we // construct an AND mask and an OR mask, and apply each individually. - row = row_temp; + row = row_temp; addr = addr_temp; for (yy = y; yy < y + font_info.height; yy++) { if (font == 3) { - levels = font_frame12x18[row]; - //if(!(flags & FONT_INVERT)) // data is normally inverted - levels = ~levels; - or_mask = font_mask12x18[row] << xshift; + levels = font_frame12x18[row]; + // if(!(flags & FONT_INVERT)) // data is normally inverted + levels = ~levels; + or_mask = font_mask12x18[row] << xshift; and_mask = (font_mask12x18[row] & levels) << xshift; } else { - levels = font_frame8x10[row]; - //if(!(flags & FONT_INVERT)) // data is normally inverted - levels = ~levels; - or_mask = font_mask8x10[row] << xshift; + levels = font_frame8x10[row]; + // if(!(flags & FONT_INVERT)) // data is normally inverted + levels = ~levels; + or_mask = font_mask8x10[row] << xshift; and_mask = (font_mask8x10[row] & levels) << xshift; } write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit); // If we're not bold write the AND mask. - //if(!(flags & FONT_BOLD)) + // if(!(flags & FONT_BOLD)) write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit); addr += GRAPHICS_WIDTH_REAL / 8; row++; @@ -1142,6 +1154,7 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) uint16_t and_mask, or_mask, levels; struct FontEntry font_info; char lookup = 0; + fetch_font_info(ch, font, &font_info, &lookup); // Compute starting address (for x,y) of character. unsigned int addr = CALC_BUFF_ADDR(x, y); @@ -1149,9 +1162,9 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) // If font only supports lowercase or uppercase, make the letter // lowercase or uppercase. /*if(font_info.flags & FONT_LOWERCASE_ONLY) - ch = tolower(ch); - if(font_info.flags & FONT_UPPERCASE_ONLY) - ch = toupper(ch);*/ + ch = tolower(ch); + if(font_info.flags & FONT_UPPERCASE_ONLY) + ch = toupper(ch);*/ fetch_font_info(ch, font, &font_info, &lookup); // How big is the character? We handle characters up to 8 pixels // wide for now. Support for large characters may be added in future. @@ -1161,10 +1174,10 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) return; } // Load data pointer. - row = lookup * font_info.height * 2; - row_temp = row; + row = lookup * font_info.height * 2; + row_temp = row; addr_temp = addr; - xshift = 16 - font_info.width; + xshift = 16 - font_info.width; // We can write mask words easily. for (yy = y; yy < y + font_info.height; yy++) { write_word_misaligned_OR(draw_buffer_mask, font_info.data[row] << xshift, addr, wbit); @@ -1175,7 +1188,7 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) // level bits, but only where the mask bit is set; otherwise, // we need to leave them alone. To do this, for each word, we // construct an AND mask and an OR mask, and apply each individually. - row = row_temp; + row = row_temp; addr = addr_temp; for (yy = y; yy < y + font_info.height; yy++) { levels = font_info.data[row + font_info.height]; @@ -1183,11 +1196,11 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) // data is normally inverted levels = ~levels; } - or_mask = font_info.data[row] << xshift; + or_mask = font_info.data[row] << xshift; and_mask = (font_info.data[row] & levels) << xshift; write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit); // If we're not bold write the AND mask. - //if(!(flags & FONT_BOLD)) + // if(!(flags & FONT_BOLD)) write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit); addr += GRAPHICS_WIDTH_REAL / 8; row++; @@ -1209,6 +1222,7 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, struct FontDimensions *dim) { int max_length = 0, line_length = 0, lines = 1; + while (*str != 0) { line_length++; if (*str == '\n' || *str == '\r') { @@ -1223,7 +1237,7 @@ void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, stru if (line_length > max_length) { max_length = line_length; } - dim->width = max_length * (font.width + xs); + dim->width = max_length * (font.width + xs); dim->height = lines * (font.height + ys); } @@ -1246,37 +1260,38 @@ void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, un int xx = 0, yy = 0, xx_original = 0; struct FontEntry font_info; struct FontDimensions dim; + // Determine font info and dimensions/position of the string. fetch_font_info(0, font, &font_info, NULL); calc_text_dimensions(str, font_info, xs, ys, &dim); switch (va) { - case TEXT_VA_TOP: - yy = y; - break; - case TEXT_VA_MIDDLE: - yy = y - (dim.height / 2); - break; - case TEXT_VA_BOTTOM: - yy = y - dim.height; - break; + case TEXT_VA_TOP: + yy = y; + break; + case TEXT_VA_MIDDLE: + yy = y - (dim.height / 2); + break; + case TEXT_VA_BOTTOM: + yy = y - dim.height; + break; } switch (ha) { - case TEXT_HA_LEFT: - xx = x; - break; - case TEXT_HA_CENTER: - xx = x - (dim.width / 2); - break; - case TEXT_HA_RIGHT: - xx = x - dim.width; - break; + case TEXT_HA_LEFT: + xx = x; + break; + case TEXT_HA_CENTER: + xx = x - (dim.width / 2); + break; + case TEXT_HA_RIGHT: + xx = x - dim.width; + break; } // Then write each character. xx_original = xx; while (*str != 0) { if (*str == '\n' || *str == '\r') { yy += ys + font_info.height; - xx = xx_original; + xx = xx_original; } else { if (xx >= 0 && xx < GRAPHICS_WIDTH_REAL) { if (font_info.id < 2) { @@ -1305,15 +1320,16 @@ void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, un * @param flags flags (passed to write_char) */ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, - __attribute__((unused)) int va, __attribute__((unused)) int ha, int flags) + __attribute__((unused)) int va, __attribute__((unused)) int ha, int flags) { int fcode = 0, fptr = 0, font = 0, fwidth = 0, fheight = 0, xx = x, yy = y, max_xx = 0, max_height = 0; struct FontEntry font_info; + // Retrieve sizes of the fonts: bigfont and smallfont. fetch_font_info(0, 0, &font_info, NULL); int smallfontwidth = font_info.width, smallfontheight = font_info.height; fetch_font_info(0, 1, &font_info, NULL); - int bigfontwidth = font_info.width, bigfontheight = font_info.height; + int bigfontwidth = font_info.width, bigfontheight = font_info.height; // 11 byte stack with last byte as NUL. char fstack[11]; fstack[10] = '\0'; @@ -1329,17 +1345,17 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned if (*str == '<' && fcode == 0) { // begin format code? fcode = 1; - fptr = 0; + fptr = 0; } if (*str == '>' && fcode == 1) { fcode = 0; if (strcmp(fstack, "B")) { // switch to "big" font (font #1) - fwidth = bigfontwidth; + fwidth = bigfontwidth; fheight = bigfontheight; } else if (strcmp(fstack, "S")) { // switch to "small" font (font #0) - fwidth = smallfontwidth; + fwidth = smallfontwidth; fheight = smallfontheight; } if (fheight > max_height) { @@ -1357,7 +1373,7 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned continue; } fstack[fptr++] = *str; - fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.) + fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.) } if (fcode == 0) { // Not a format code, raw text. @@ -1366,7 +1382,7 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned if (xx > max_xx) { max_xx = xx; } - xx = x; + xx = x; yy += fheight + ys; } } @@ -1377,27 +1393,27 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned // Now we've parsed it and got a bbox, we need to work out the dimensions of it // and how to align it. /*int width = max_xx - x; - int height = yy - y; - int ay, ax; - switch(va) - { - case TEXT_VA_TOP: ay = yy; break; - case TEXT_VA_MIDDLE: ay = yy - (height / 2); break; - case TEXT_VA_BOTTOM: ay = yy - height; break; - } - switch(ha) - { - case TEXT_HA_LEFT: ax = x; break; - case TEXT_HA_CENTER: ax = x - (width / 2); break; - case TEXT_HA_RIGHT: ax = x - width; break; - }*/ + int height = yy - y; + int ay, ax; + switch(va) + { + case TEXT_VA_TOP: ay = yy; break; + case TEXT_VA_MIDDLE: ay = yy - (height / 2); break; + case TEXT_VA_BOTTOM: ay = yy - height; break; + } + switch(ha) + { + case TEXT_HA_LEFT: ax = x; break; + case TEXT_HA_CENTER: ax = x - (width / 2); break; + case TEXT_HA_RIGHT: ax = x - width; break; + }*/ // So ax,ay is our new text origin. Parse the text format again and paint // the text on the display. fcode = 0; - fptr = 0; - font = 0; - xx = 0; - yy = 0; + fptr = 0; + font = 0; + xx = 0; + yy = 0; while (*str) { if (*str == '<' && fcode == 1) { // escape code: skip @@ -1406,20 +1422,20 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned if (*str == '<' && fcode == 0) { // begin format code? fcode = 1; - fptr = 0; + fptr = 0; } if (*str == '>' && fcode == 1) { fcode = 0; if (strcmp(fstack, "B")) { // switch to "big" font (font #1) - fwidth = bigfontwidth; + fwidth = bigfontwidth; fheight = bigfontheight; - font = 1; + font = 1; } else if (strcmp(fstack, "S")) { // switch to "small" font (font #0) - fwidth = smallfontwidth; + fwidth = smallfontwidth; fheight = smallfontheight; - font = 0; + font = 0; } // Skip over this byte. Go to next byte. str++; @@ -1433,7 +1449,7 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned continue; } fstack[fptr++] = *str; - fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.) + fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.) } if (fcode == 0) { // Not a format code, raw text. So we draw it. @@ -1444,7 +1460,7 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned if (xx > max_xx) { max_xx = xx; } - xx = x; + xx = x; yy += fheight + ys; } } @@ -1452,7 +1468,7 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned } } -//SUPEROSD- +// SUPEROSD- // graphics @@ -1495,61 +1511,62 @@ void drawAttitude(uint16_t x, uint16_t y, int16_t pitch, int16_t roll, uint16_t l = b * pitch / 90; // scale - //0 - //drawLine((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1); - //drawLine((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1); + // 0 + // drawLine((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1); + // drawLine((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1); write_line_outlined((x) - 1 - (size / 2 + 4), (y) - 1, (x) - 1 - (size / 2 + 1), (y) - 1, 0, 0, 0, 1); write_line_outlined((x) - 1 + (size / 2 + 4), (y) - 1, (x) - 1 + (size / 2 + 1), (y) - 1, 0, 0, 0, 1); - //30 - //drawLine((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2); - //drawLine((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2); + // 30 + // drawLine((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2); + // drawLine((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2); write_line_outlined((x) - 1 + indi30x1, (y) - 1 - indi30y1, (x) - 1 + indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1); write_line_outlined((x) - 1 - indi30x1, (y) - 1 - indi30y1, (x) - 1 - indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1); - //60 - //drawLine((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2); - //drawLine((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2); + // 60 + // drawLine((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2); + // drawLine((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2); write_line_outlined((x) - 1 + indi60x1, (y) - 1 - indi60y1, (x) - 1 + indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1); write_line_outlined((x) - 1 - indi60x1, (y) - 1 - indi60y1, (x) - 1 - indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1); - //90 - //drawLine((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1)); + // 90 + // drawLine((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1)); write_line_outlined((x) - 1, (y) - 1 - (size / 2 + 4), (x) - 1, (y) - 1 - (size / 2 + 1), 0, 0, 0, 1); - //roll - //drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line - write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1); //Direction line - //"wingtips" - //drawLine((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c); - //drawLine((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a); + // roll + // drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line + write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1); // Direction line + // "wingtips" + // drawLine((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c); + // drawLine((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a); write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 - b + d, (y) - 1 + a - c, 0, 0, 0, 1); write_line_outlined((x) - 1 + b + d, (y) - 1 - a - c, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1); - //pitch - //drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l); + // pitch + // drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l); write_line_outlined((x) - 1, (y) - 1, (x) - 1 - k, (y) - 1 - l, 0, 0, 0, 1); - //drawCircle(x-1, y-1, 5); - //write_circle_outlined(x-1, y-1, 5,0,0,0,1); - //drawCircle(x-1, y-1, size/2+4); - //write_circle_outlined(x-1, y-1, size/2+4,0,0,0,1); + // drawCircle(x-1, y-1, 5); + // write_circle_outlined(x-1, y-1, 5,0,0,0,1); + // drawCircle(x-1, y-1, size/2+4); + // write_circle_outlined(x-1, y-1, size/2+4,0,0,0,1); } void drawBattery(uint16_t x, uint16_t y, uint8_t battery, uint16_t size) { int i = 0; int batteryLines; - //top + + // top /*drawLine((x)-1+(size/2-size/4), (y)-1, (x)-1 + (size/2+size/4), (y)-1); - drawLine((x)-1+(size/2-size/4), (y)-1+1, (x)-1 + (size/2+size/4), (y)-1+1); + drawLine((x)-1+(size/2-size/4), (y)-1+1, (x)-1 + (size/2+size/4), (y)-1+1); - drawLine((x)-1, (y)-1+2, (x)-1 + size, (y)-1+2); - //bottom - drawLine((x)-1, (y)-1+size*3, (x)-1 + size, (y)-1+size*3); - //left - drawLine((x)-1, (y)-1+2, (x)-1, (y)-1+size*3); + drawLine((x)-1, (y)-1+2, (x)-1 + size, (y)-1+2); + //bottom + drawLine((x)-1, (y)-1+size*3, (x)-1 + size, (y)-1+size*3); + //left + drawLine((x)-1, (y)-1+2, (x)-1, (y)-1+size*3); - //right - drawLine((x)-1+size, (y)-1+2, (x)-1+size, (y)-1+size*3);*/ + //right + drawLine((x)-1+size, (y)-1+2, (x)-1+size, (y)-1+size*3);*/ write_rectangle_outlined((x) - 1, (y) - 1 + 2, size, size * 3, 0, 1); write_vline_lm((x) - 1 + (size / 2 + size / 4) + 1, (y) - 2, (y) - 1 + 1, 0, 1); @@ -1568,26 +1585,27 @@ void printTime(uint16_t x, uint16_t y) { char temp[9] = { 0 }; + sprintf(temp, "%02d:%02d:%02d", timex.hour, timex.min, timex.sec); - //printTextFB(x,y,temp); + // printTextFB(x,y,temp); write_string(temp, x, y, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); } /* - void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) { + void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) { - char temp[9]={0}; - char updown=' '; - uint16_t charx=x/16; - if(dir==0) - updown=24; - if(dir==1) - updown=25; - sprintf(temp,"%c%6dm",updown,alt); - printTextFB(charx,y+2,temp); - // frame - drawBox(charx*16-3,y,charx*16+strlen(temp)*8+3,y+11); - }*/ + char temp[9]={0}; + char updown=' '; + uint16_t charx=x/16; + if(dir==0) + updown=24; + if(dir==1) + updown=25; + sprintf(temp,"%c%6dm",updown,alt); + printTextFB(charx,y+2,temp); + // frame + drawBox(charx*16-3,y,charx*16+strlen(temp)*8+3,y+11); + }*/ /** * hud_draw_vertical_scale: Draw a vertical scale. @@ -1598,8 +1616,8 @@ void printTime(uint16_t x, uint16_t y) * @param x x displacement (typ. 0) * @param y y displacement (typ. half display height) * @param height height of scale - * @param mintick_step how often a minor tick is shown - * @param majtick_step how often a major tick is shown + * @param mintick_step how often a minor tick is shown + * @param majtick_step how often a major tick is shown * @param mintick_len minor tick length * @param majtick_len major tick length * @param boundtick_len boundary tick length @@ -1607,88 +1625,96 @@ void printTime(uint16_t x, uint16_t y) * @param flags special flags (see hud.h.) */ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int height, int mintick_step, int majtick_step, int mintick_len, int majtick_len, - int boundtick_len, __attribute__((unused)) int max_val, int flags) + int boundtick_len, __attribute__((unused)) int max_val, int flags) { - char temp[15]; //, temp2[15]; + char temp[15]; // , temp2[15]; struct FontEntry font_info; struct FontDimensions dim; // Halign should be in a small span. - //MY_ASSERT(halign >= -1 && halign <= 1); + // MY_ASSERT(halign >= -1 && halign <= 1); // Compute the position of the elements. int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, boundtick_start = 0, boundtick_end = 0; + if (halign == -1) { - majtick_start = x; - majtick_end = x + majtick_len; - mintick_start = x; - mintick_end = x + mintick_len; + majtick_start = x; + majtick_end = x + majtick_len; + mintick_start = x; + mintick_end = x + mintick_len; boundtick_start = x; - boundtick_end = x + boundtick_len; + boundtick_end = x + boundtick_len; } else if (halign == +1) { x = x - GRAPHICS_HDEADBAND; - majtick_start = GRAPHICS_WIDTH_REAL - x - 1; - majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1; - mintick_start = GRAPHICS_WIDTH_REAL - x - 1; - mintick_end = GRAPHICS_WIDTH_REAL - x - mintick_len - 1; + majtick_start = GRAPHICS_WIDTH_REAL - x - 1; + majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1; + mintick_start = GRAPHICS_WIDTH_REAL - x - 1; + mintick_end = GRAPHICS_WIDTH_REAL - x - mintick_len - 1; boundtick_start = GRAPHICS_WIDTH_REAL - x - 1; - boundtick_end = GRAPHICS_WIDTH_REAL - x - boundtick_len - 1; + boundtick_end = GRAPHICS_WIDTH_REAL - x - boundtick_len - 1; } // Retrieve width of large font (font #0); from this calculate the x spacing. fetch_font_info(0, 0, &font_info, NULL); - int arrow_len = (font_info.height / 2) + 1; // FIXME, font info being loaded correctly?? + int arrow_len = (font_info.height / 2) + 1; // FIXME, font info being loaded correctly?? int text_x_spacing = arrow_len; - int max_text_y = 0, text_length = 0; + int max_text_y = 0, text_length = 0; int small_font_char_width = font_info.width + 1; // +1 for horizontal spacing = 1 // For -(range / 2) to +(range / 2), draw the scale. - int range_2 = range / 2; //, height_2 = height / 2; - int r = 0, rr = 0, rv = 0, ys = 0, style = 0; //calc_ys = 0, + int range_2 = range / 2; // , height_2 = height / 2; + int r = 0, rr = 0, rv = 0, ys = 0, style = 0; // calc_ys = 0, // Iterate through each step. for (r = -range_2; r <= +range_2; r++) { style = 0; - rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape - rv = -rr + range_2; // for number display - if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE) + rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape + rv = -rr + range_2; // for number display + if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE) { rr += majtick_step / 2; - if (rr % majtick_step == 0) + } + if (rr % majtick_step == 0) { style = 1; // major tick - else if (rr % mintick_step == 0) + } else if (rr % mintick_step == 0) { style = 2; // minor tick - else + } else { style = 0; - if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0) + } + if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0) { continue; + } if (style) { // Calculate y position. - ys = ((long int) (r * height) / (long int) range) + y; - //sprintf(temp, "ys=%d", ys); - //con_puts(temp, 0); + ys = ((long int)(r * height) / (long int)range) + y; + // sprintf(temp, "ys=%d", ys); + // con_puts(temp, 0); // Depending on style, draw a minor or a major tick. if (style == 1) { write_hline_outlined(majtick_start, majtick_end, ys, 2, 2, 0, 1); memset(temp, ' ', 10); - //my_itoa(rv, temp); + // my_itoa(rv, temp); sprintf(temp, "%d", rv); text_length = (strlen(temp) + 1) * small_font_char_width; // add 1 for margin - if (text_length > max_text_y) + if (text_length > max_text_y) { max_text_y = text_length; - if (halign == -1) + } + if (halign == -1) { write_string(temp, majtick_end + text_x_spacing, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 1); - else + } else { write_string(temp, majtick_end - text_x_spacing + 1, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 1); - } else if (style == 2) + } + } else if (style == 2) { write_hline_outlined(mintick_start, mintick_end, ys, 2, 2, 0, 1); + } } } // Generate the string for the value, as well as calculating its dimensions. memset(temp, ' ', 10); - //my_itoa(v, temp); + // my_itoa(v, temp); sprintf(temp, "%d", v); // TODO: add auto-sizing. calc_text_dimensions(temp, font_info, 1, 0, &dim); int xx = 0, i = 0; - if (halign == -1) + if (halign == -1) { xx = majtick_end + text_x_spacing; - else + } else { xx = majtick_end - text_x_spacing; + } // Draw an arrow from the number to the point. for (i = 0; i < arrow_len; i++) { if (halign == -1) { @@ -1716,18 +1742,19 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei write_vline_lm(xx - dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1); } // Draw the text. - if (halign == -1) + if (halign == -1) { write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 0); - else + } else { write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 0); + } // Then, add a slow cut off on the edges, so the text doesn't sharply // disappear. We simply clear the areas above and below the ticker, and we // use little markers on the edges. if (halign == -1) { write_filled_rectangle_lm(majtick_end + text_x_spacing, y + (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, - 0); + 0); write_filled_rectangle_lm(majtick_end + text_x_spacing, y - (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, - 0); + 0); } else { write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y + (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0); write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y - (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0); @@ -1757,23 +1784,24 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, textoffset = 0; char headingstr[4]; majtick_start = y; - majtick_end = y - majtick_len; + majtick_end = y - majtick_len; mintick_start = y; - mintick_end = y - mintick_len; - textoffset = 8; + mintick_end = y - mintick_len; + textoffset = 8; int r, style, rr, xs; // rv, int range_2 = range / 2; for (r = -range_2; r <= +range_2; r++) { style = 0; - rr = (v + r + 360) % 360; // normalise range for modulo, add to move compass track - //rv = -rr + range_2; // for number display - if (rr % majtick_step == 0) + rr = (v + r + 360) % 360; // normalise range for modulo, add to move compass track + // rv = -rr + range_2; // for number display + if (rr % majtick_step == 0) { style = 1; // major tick - else if (rr % mintick_step == 0) + } else if (rr % mintick_step == 0) { style = 2; // minor tick + } if (style) { // Calculate x position. - xs = ((long int) (r * width) / (long int) range) + x; + xs = ((long int)(r * width) / (long int)range) + x; // Draw it. if (style == 1) { write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1); @@ -1788,18 +1816,18 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint headingstr[3] = 0; // nul to terminate } else { switch (rr) { - case 0: - headingstr[0] = 'N'; - break; - case 90: - headingstr[0] = 'E'; - break; - case 180: - headingstr[0] = 'S'; - break; - case 270: - headingstr[0] = 'W'; - break; + case 0: + headingstr[0] = 'N'; + break; + case 90: + headingstr[0] = 'E'; + break; + case 180: + headingstr[0] = 'S'; + break; + case 270: + headingstr[0] = 'W'; + break; } headingstr[1] = 0; headingstr[2] = 0; @@ -1807,8 +1835,9 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint } // +1 fudge...! write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1); - } else if (style == 2) + } else if (style == 2) { write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1); + } } } // Then, draw a rectangle with the present heading in it. @@ -1834,24 +1863,25 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, int16_t x1, x2; int16_t y1, y2; int16_t refx, refy; + alpha = DEG2RAD(angle); - refx = l_x + size / 2; - refy = l_y + size / 2; + refx = l_x + size / 2; + refy = l_y + size / 2; // - float k = 0; - float dx = sinf(alpha) * (pitch / 90.0f * (size / 2)); - float dy = cosf(alpha) * (pitch / 90.0f * (size / 2)); + float k = 0; + float dx = sinf(alpha) * (pitch / 90.0f * (size / 2)); + float dy = cosf(alpha) * (pitch / 90.0f * (size / 2)); int16_t x0 = (size / 2) - dx; int16_t y0 = (size / 2) + dy; // calculate the line function if ((angle < 90.0f) && (angle > -90)) { - vertical = 0; - if(fabsf(angle) < 1e-5f) { - horizontal = 1; - } else { - k = tanf(alpha); - } + vertical = 0; + if (fabsf(angle) < 1e-5f) { + horizontal = 1; + } else { + k = tanf(alpha); + } } else { vertical = 1; } @@ -1873,8 +1903,8 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, x1 = ((y1 - y0) + k * x0) / k; } // left crossing point - x = size; - y = k * (x - x0) + y0; + x = size; + y = k * (x - x0) + y0; x2 = x; y2 = y; if (y < 0) { @@ -1888,9 +1918,9 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, // move to location // horizon line write_line_outlined(x1 + l_x, y1 + l_y, x2 + l_x, y2 + l_y, 0, 0, 0, 1); - //fill + // fill if (angle <= 0.0f && angle > -90.0f) { - //write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = y2; i < size; i++) { x2 = ((i - y0) + k * x0) / k; if (x2 > size) { @@ -1902,7 +1932,7 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, write_hline_lm(x2 + l_x, size + l_x, i + l_y, 1, 1); } } else if (angle < -90.0f) { - //write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = 0; i < y2; i++) { x2 = ((i - y0) + k * x0) / k; if (x2 > size) { @@ -1914,7 +1944,7 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, write_hline_lm(size + l_x, x2 + l_x, i + l_y, 1, 1); } } else if (angle > 0.0f && angle < 90.0f) { - //write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = y1; i < size; i++) { x2 = ((i - y0) + k * x0) / k; if (x2 > size) { @@ -1926,7 +1956,7 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, write_hline_lm(0 + l_x, x2 + l_x, i + l_y, 1, 1); } } else if (angle > 90.0f) { - //write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = 0; i < y1; i++) { x2 = ((i - y0) + k * x0) / k; if (x2 > size) { @@ -1942,12 +1972,12 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, // horizon line write_line_outlined(x0 + l_x, 0 + l_y, x0 + l_x, size + l_y, 0, 0, 0, 1); if (angle >= 90.0f) { - //write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = 0; i < size; i++) { write_hline_lm(0 + l_x, x0 + l_x, i + l_y, 1, 1); } } else { - //write_string("6", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("6", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = 0; i < size; i++) { write_hline_lm(size + l_x, x0 + l_x, i + l_y, 1, 1); } @@ -1956,22 +1986,22 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, // horizon line write_hline_outlined(0 + l_x, size + l_x, y0 + l_y, 0, 0, 0, 1); if (angle < 0) { - //write_string("7", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("7", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = 0; i < y0; i++) { write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1); } } else { - //write_string("8", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + // write_string("8", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); for (int i = y0; i < size; i++) { write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1); } } } - //sides + // sides write_line_outlined(l_x, l_y, l_x, l_y + size, 0, 0, 0, 1); write_line_outlined(l_x + size, l_y, l_x + size, l_y + size, 0, 0, 0, 1); - //plane + // plane write_line_outlined(refx - 5, refy, refx + 6, refy, 0, 0, 0, 1); write_line_outlined(refx, refy, refx, refy - 3, 0, 0, 0, 1); } @@ -1986,12 +2016,13 @@ void introGraphics() /* logo */ int image = 0; struct splashEntry splash_info; + splash_info = splash[image]; copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - (splash_info.width) / 2), APPLY_VDEADBAND(GRAPHICS_BOTTOM / 2 - (splash_info.height) / 2), image); /* frame */ - drawBox(APPLY_HDEADBAND(0), APPLY_VDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT-8), APPLY_VDEADBAND(GRAPHICS_BOTTOM)); + drawBox(APPLY_HDEADBAND(0), APPLY_VDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT - 8), APPLY_VDEADBAND(GRAPHICS_BOTTOM)); // Must mask out last half-word because SPI keeps clocking it out otherwise for (uint32_t i = 0; i < 8; i++) { @@ -2003,6 +2034,7 @@ void introGraphics() void calcHomeArrow(int16_t m_yaw) { HomeLocationData home; + HomeLocationGet(&home); GPSPositionData gpsData; GPSPositionGet(&gpsData); @@ -2012,7 +2044,7 @@ void calcHomeArrow(int16_t m_yaw) float elevation; float gcsAlt = home.Altitude; // Home MSL altitude float uavAlt = gpsData.Altitude; // UAV MSL altitude - float dAlt = uavAlt - gcsAlt; // Altitude difference + float dAlt = uavAlt - gcsAlt; // Altitude difference // Convert to radians lat1 = DEG2RAD(home.Latitude) / 10000000.0f; // Home lat @@ -2022,14 +2054,14 @@ void calcHomeArrow(int16_t m_yaw) // Bearing /** - var y = Math.sin(dLon) * Math.cos(lat2); - var x = Math.cos(lat1)*Math.sin(lat2) - - Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon); - var brng = Math.atan2(y, x).toDeg(); + var y = Math.sin(dLon) * Math.cos(lat2); + var x = Math.cos(lat1)*Math.sin(lat2) - + Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon); + var brng = Math.atan2(y, x).toDeg(); **/ - y = sinf(lon2 - lon1) * cosf(lat2); - x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2 - lon1); - brng = RAD2DEG(atan2f(y,x)); + y = sinf(lon2 - lon1) * cosf(lat2); + x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2 - lon1); + brng = RAD2DEG(atan2f(y, x)); if (brng < 0) { brng += 360.0f; } @@ -2042,39 +2074,40 @@ void calcHomeArrow(int16_t m_yaw) // Haversine formula for distance /** - var R = 6371; // km - var dLat = (lat2-lat1).toRad(); - var dLon = (lon2-lon1).toRad(); - var a = Math.sin(dLat/2) * Math.sin(dLat/2) + - Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) * - Math.sin(dLon/2) * Math.sin(dLon/2); - var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)); - var d = R * c; + var R = 6371; // km + var dLat = (lat2-lat1).toRad(); + var dLon = (lon2-lon1).toRad(); + var a = Math.sin(dLat/2) * Math.sin(dLat/2) + + Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) * + Math.sin(dLon/2) * Math.sin(dLon/2); + var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)); + var d = R * c; **/ a = sinf((lat2 - lat1) / 2) * sinf((lat2 - lat1) / 2) + cosf(lat1) * cosf(lat2) * sinf((lon2 - lon1) / 2) * sinf((lon2 - lon1) / 2); c = 2.0f * atan2f(sqrtf(a), sqrtf(1.0f - a)); d = 6371.0f * 1000.0f * c; // Elevation v depends servo direction - if(d > 0.0f) - elevation = 90.0f - RAD2DEG(atanf(dAlt/d)); - else + if (d > 0.0f) { + elevation = 90.0f - RAD2DEG(atanf(dAlt / d)); + } else { elevation = 0.0f; - //! TODO: sanity check + } + // ! TODO: sanity check char temp[50] = { 0 }; - sprintf(temp, "hea:%d", (int) brng); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp, "ele:%d", (int) elevation); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp, "dis:%d", (int) d); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp, "u2g:%d", (int) u2g); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "hea:%d", (int)brng); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - 30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "ele:%d", (int)elevation); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - 30), APPLY_VDEADBAND(30 + 10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "dis:%d", (int)d); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - 30), APPLY_VDEADBAND(30 + 10 + 10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "u2g:%d", (int)u2g); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - 30), APPLY_VDEADBAND(30 + 10 + 10 + 10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp, "%c%c", (int) (u2g / 22.5f) * 2 + 0x90, (int) (u2g / 22.5f) * 2 + 0x91); - write_string(temp, APPLY_HDEADBAND(250), APPLY_VDEADBAND(40+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); + sprintf(temp, "%c%c", (int)(u2g / 22.5f) * 2 + 0x90, (int)(u2g / 22.5f) * 2 + 0x91); + write_string(temp, APPLY_HDEADBAND(250), APPLY_VDEADBAND(40 + 10 + 10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); } int lama = 10; @@ -2084,10 +2117,10 @@ void lamas(void) { char temp[10] = { 0 }; + lama++; if (lama % 10 == 0) { for (int z = 0; z < 30; z++) { - lama_loc[0][z] = rand() % (GRAPHICS_RIGHT - 10); lama_loc[1][z] = rand() % (GRAPHICS_BOTTOM - 10); } @@ -2098,10 +2131,11 @@ void lamas(void) } } -//main draw function +// main draw function void updateGraphics() { OsdSettingsData OsdSettings; + OsdSettingsGet(&OsdSettings); AttitudeActualData attitude; AttitudeActualGet(&attitude); @@ -2118,230 +2152,232 @@ void updateGraphics() PIOS_Servo_Set(1, OsdSettings.Black); switch (OsdSettings.Screen) { - case 0: // Dave simple - { - if (home.Set == HOMELOCATION_SET_FALSE) { - char temps[20] = - { 0 }; - sprintf(temps, "HOME NOT SET"); - //printTextFB(x,y,temp); - write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT/2), (GRAPHICS_BOTTOM / 2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3); - } - - char temp[50] = + case 0: // Dave simple + { + if (home.Set == HOMELOCATION_SET_FALSE) { + char temps[20] = { 0 }; - memset(temp, ' ', 40); - // Note: cast to double required due to -Wdouble-promotion compiler option is - // being used, and there is no way in C to pass a float to a variadic function like sprintf() - sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f)); - write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3); - sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f)); - write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3); - sprintf(temp, "Sat:%d", (int) gpsData.Satellites); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT-40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage FLIGHT*/ - sprintf(temp, "V:%5.2fV", (double)(PIOS_ADC_PinGet(2) * 3 * 6.1f / 4096)); - write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); - - if (gpsData.Heading > 180) - calcHomeArrow((int16_t)(gpsData.Heading - 360)); - else - calcHomeArrow((int16_t)(gpsData.Heading)); + sprintf(temps, "HOME NOT SET"); + // printTextFB(x,y,temp); + write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT / 2), (GRAPHICS_BOTTOM / 2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3); } - break; - case 1: - { - /*drawBox(2,2,GRAPHICS_WIDTH_REAL-4,GRAPHICS_HEIGHT_REAL-4); - write_filled_rectangle(draw_buffer_mask,0,0,GRAPHICS_WIDTH_REAL-2,GRAPHICS_HEIGHT_REAL-2,0); - write_filled_rectangle(draw_buffer_mask,2,2,GRAPHICS_WIDTH_REAL-4-2,GRAPHICS_HEIGHT_REAL-4-2,2); - write_filled_rectangle(draw_buffer_mask,3,3,GRAPHICS_WIDTH_REAL-4-1,GRAPHICS_HEIGHT_REAL-4-1,0);*/ - //write_filled_rectangle(draw_buffer_mask,5,5,GRAPHICS_WIDTH_REAL-4-5,GRAPHICS_HEIGHT_REAL-4-5,0); - //write_rectangle_outlined(10,10,GRAPHICS_WIDTH_REAL-20,GRAPHICS_HEIGHT_REAL-20,0,0); - //drawLine(GRAPHICS_WIDTH_REAL-1, GRAPHICS_HEIGHT_REAL-1,(GRAPHICS_WIDTH_REAL/2)-1, GRAPHICS_HEIGHT_REAL-1 ); - //drawCircle((GRAPHICS_WIDTH_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1); - //drawCircle((GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-2); - //drawLine(0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1, (GRAPHICS_SIZE/2)-1); - //drawLine((GRAPHICS_SIZE/2)-1, 0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1); - /*angleA++; - if(angleB<=-90) - { - sum=2; - } - if(angleB>=90) - { - sum=-2; - } - angleB+=sum; - angleC+=2;*/ - // GPS HACK - if (gpsData.Heading > 180) - calcHomeArrow((int16_t)(gpsData.Heading - 360)); - else - calcHomeArrow((int16_t)(gpsData.Heading)); + char temp[50] = + { 0 }; + memset(temp, ' ', 40); + // Note: cast to double required due to -Wdouble-promotion compiler option is + // being used, and there is no way in C to pass a float to a variadic function like sprintf() + sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f)); + write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM - 30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3); + sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f)); + write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM - 10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3); + sprintf(temp, "Sat:%d", (int)gpsData.Satellites); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT - 40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - /* Draw Attitude Indicator */ - if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) { - drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]), - APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]), attitude.Pitch, attitude.Roll, 96); - } - //write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0); - //printText16( 60, 12,"Hello OP-OSD"); + /* Print ADC voltage FLIGHT*/ + sprintf(temp, "V:%5.2fV", (double)(PIOS_ADC_PinGet(2) * 3 * 6.1f / 4096)); + write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); - char temp[50] = - { 0 }; - memset(temp, ' ', 40); - sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f)); - write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f)); - write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp, "Fix:%d", (int) gpsData.Status); - write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp, "Sat:%d", (int) gpsData.Satellites); - write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - - /* Print RTC time */ - if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) { - printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]), APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y])); - } - - /* Print Number of detected video Lines */ - sprintf(temp, "Lines:%4d", PIOS_Video_GetOSDLines()); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage */ - //sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096)); - //write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - sprintf(temp, "Rssi:%4.2fV", (double)(PIOS_ADC_PinGet(5) * 3.0f / 4096.0f)); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print CPU temperature */ - sprintf(temp, "Temp:%4.2fC", (double)(PIOS_ADC_PinGet(3) * 0.29296875f - 264)); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage FLIGHT*/ - sprintf(temp, "FltV:%4.2fV", (double)(PIOS_ADC_PinGet(2) * 3.0f * 6.1f / 4096.0f)); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage VIDEO*/ - sprintf(temp, "VidV:%4.2fV", (double)(PIOS_ADC_PinGet(4) * 3.0f * 6.1f / 4096.0f)); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage RSSI */ - //sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096)); - //write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - /* Draw Battery Gauge */ - /*m_batt++; - uint8_t dir=3; - if(m_batt==101) - m_batt=0; - if(m_pitch>0) - { - dir=0; - m_alt+=m_pitch/2; - } - else if(m_pitch<0) - { - dir=1; - m_alt+=m_pitch/2; - }*/ - - /*if(OsdSettings.Battery == OSDSETTINGS_BATTERY_ENABLED) - { - drawBattery(APPLY_HDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_X]),APPLY_VDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_Y]),m_batt,16); - }*/ - - //drawAltitude(200,50,m_alt,dir); - //drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32); - // Draw airspeed (left side.) - if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) { - hud_draw_vertical_scale((int) gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]), - APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE); - } - // Draw altimeter (right side.) - if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) { - hud_draw_vertical_scale((int) gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]), - APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0); - } - // Draw compass. - if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) { - if (attitude.Yaw < 0) { - hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), - APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0); - } else { - hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), - APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0); - } - } + if (gpsData.Heading > 180) { + calcHomeArrow((int16_t)(gpsData.Heading - 360)); + } else { + calcHomeArrow((int16_t)(gpsData.Heading)); } - break; - case 2: - { - int size = 64; - int x = ((GRAPHICS_RIGHT / 2) - (size / 2)), y = (GRAPHICS_BOTTOM - size - 2); - draw_artificial_horizon(-attitude.Roll, attitude.Pitch, APPLY_HDEADBAND(x), APPLY_VDEADBAND(y), size); - hud_draw_vertical_scale((int) gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT-(x-1)), APPLY_VDEADBAND(y+(size/2)), size, 5, 10, 4, 7, - 10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE); - if (OsdSettings.AltitudeSource == OSDSETTINGS_ALTITUDESOURCE_BARO) { - hud_draw_vertical_scale((int) baro.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0); + } + break; + case 1: + { + /*drawBox(2,2,GRAPHICS_WIDTH_REAL-4,GRAPHICS_HEIGHT_REAL-4); + write_filled_rectangle(draw_buffer_mask,0,0,GRAPHICS_WIDTH_REAL-2,GRAPHICS_HEIGHT_REAL-2,0); + write_filled_rectangle(draw_buffer_mask,2,2,GRAPHICS_WIDTH_REAL-4-2,GRAPHICS_HEIGHT_REAL-4-2,2); + write_filled_rectangle(draw_buffer_mask,3,3,GRAPHICS_WIDTH_REAL-4-1,GRAPHICS_HEIGHT_REAL-4-1,0);*/ + // write_filled_rectangle(draw_buffer_mask,5,5,GRAPHICS_WIDTH_REAL-4-5,GRAPHICS_HEIGHT_REAL-4-5,0); + // write_rectangle_outlined(10,10,GRAPHICS_WIDTH_REAL-20,GRAPHICS_HEIGHT_REAL-20,0,0); + // drawLine(GRAPHICS_WIDTH_REAL-1, GRAPHICS_HEIGHT_REAL-1,(GRAPHICS_WIDTH_REAL/2)-1, GRAPHICS_HEIGHT_REAL-1 ); + // drawCircle((GRAPHICS_WIDTH_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1); + // drawCircle((GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-2); + // drawLine(0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1, (GRAPHICS_SIZE/2)-1); + // drawLine((GRAPHICS_SIZE/2)-1, 0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1); + /*angleA++; + if(angleB<=-90) + { + sum=2; + } + if(angleB>=90) + { + sum=-2; + } + angleB+=sum; + angleC+=2;*/ + + // GPS HACK + if (gpsData.Heading > 180) { + calcHomeArrow((int16_t)(gpsData.Heading - 360)); + } else { + calcHomeArrow((int16_t)(gpsData.Heading)); + } + + /* Draw Attitude Indicator */ + if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) { + drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]), + APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]), attitude.Pitch, attitude.Roll, 96); + } + // write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0); + // printText16( 60, 12,"Hello OP-OSD"); + + char temp[50] = + { 0 }; + memset(temp, ' ', 40); + sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f)); + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f)); + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "Fix:%d", (int)gpsData.Status); + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "Sat:%d", (int)gpsData.Satellites); + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + + /* Print RTC time */ + if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) { + printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]), APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y])); + } + + /* Print Number of detected video Lines */ + sprintf(temp, "Lines:%4d", PIOS_Video_GetOSDLines()); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print ADC voltage */ + // sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096)); + // write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + sprintf(temp, "Rssi:%4.2fV", (double)(PIOS_ADC_PinGet(5) * 3.0f / 4096.0f)); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print CPU temperature */ + sprintf(temp, "Temp:%4.2fC", (double)(PIOS_ADC_PinGet(3) * 0.29296875f - 264)); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print ADC voltage FLIGHT*/ + sprintf(temp, "FltV:%4.2fV", (double)(PIOS_ADC_PinGet(2) * 3.0f * 6.1f / 4096.0f)); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print ADC voltage VIDEO*/ + sprintf(temp, "VidV:%4.2fV", (double)(PIOS_ADC_PinGet(4) * 3.0f * 6.1f / 4096.0f)); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print ADC voltage RSSI */ + // sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096)); + // write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + /* Draw Battery Gauge */ + /*m_batt++; + uint8_t dir=3; + if(m_batt==101) + m_batt=0; + if(m_pitch>0) + { + dir=0; + m_alt+=m_pitch/2; + } + else if(m_pitch<0) + { + dir=1; + m_alt+=m_pitch/2; + }*/ + + /*if(OsdSettings.Battery == OSDSETTINGS_BATTERY_ENABLED) + { + drawBattery(APPLY_HDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_X]),APPLY_VDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_Y]),m_batt,16); + }*/ + + // drawAltitude(200,50,m_alt,dir); + // drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32); + // Draw airspeed (left side.) + if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) { + hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]), + APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE); + } + // Draw altimeter (right side.) + if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) { + hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]), + APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0); + } + // Draw compass. + if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) { + if (attitude.Yaw < 0) { + hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), + APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0); } else { - hud_draw_vertical_scale((int) gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, - 0); + hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), + APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0); } + } + } + break; + case 2: + { + int size = 64; + int x = ((GRAPHICS_RIGHT / 2) - (size / 2)), y = (GRAPHICS_BOTTOM - size - 2); + draw_artificial_horizon(-attitude.Roll, attitude.Pitch, APPLY_HDEADBAND(x), APPLY_VDEADBAND(y), size); + hud_draw_vertical_scale((int)gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT - (x - 1)), APPLY_VDEADBAND(y + (size / 2)), size, 5, 10, 4, 7, + 10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE); + if (OsdSettings.AltitudeSource == OSDSETTINGS_ALTITUDESOURCE_BARO) { + hud_draw_vertical_scale((int)baro.Altitude, 50, -1, APPLY_HDEADBAND((x + size + 1)), APPLY_VDEADBAND(y + (size / 2)), size, 10, 20, 4, 7, 10, 500, 0); + } else { + hud_draw_vertical_scale((int)gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x + size + 1)), APPLY_VDEADBAND(y + (size / 2)), size, 10, 20, 4, 7, 10, 500, + 0); + } - char temp[50] = - { 0 }; - memset(temp, ' ', 50); - switch (status.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_MANUAL: - sprintf(temp, "Man"); - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - sprintf(temp, "Stab1"); - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - sprintf(temp, "Stab2"); - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - sprintf(temp, "Stab3"); - break; - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - sprintf(temp, "PH"); - break; - case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - sprintf(temp, "RTB"); - break; - case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: - sprintf(temp, "PATH"); - break; - default: - sprintf(temp, "Mode: %d", status.FlightMode); - break; - } - write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - } + char temp[50] = + { 0 }; + memset(temp, ' ', 50); + switch (status.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_MANUAL: + sprintf(temp, "Man"); break; - case 3: - { - lamas(); - } + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + sprintf(temp, "Stab1"); break; - case 4: - case 5: - case 6: - { - int image = OsdSettings.Screen - 4; - struct splashEntry splash_info; - splash_info = splash[image]; - - copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2), image); - } + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + sprintf(temp, "Stab2"); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + sprintf(temp, "Stab3"); + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + sprintf(temp, "PH"); + break; + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + sprintf(temp, "RTB"); + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + sprintf(temp, "PATH"); break; default: - write_vline_lm(APPLY_HDEADBAND(GRAPHICS_RIGHT/2), APPLY_VDEADBAND(0), APPLY_VDEADBAND(GRAPHICS_BOTTOM), 1, 1); - write_hline_lm(APPLY_HDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2), 1, 1); + sprintf(temp, "Mode: %d", status.FlightMode); break; + } + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + } + break; + case 3: + { + lamas(); + } + break; + case 4: + case 5: + case 6: + { + int image = OsdSettings.Screen - 4; + struct splashEntry splash_info; + splash_info = splash[image]; + + copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - (splash_info.width) / 2), APPLY_VDEADBAND(GRAPHICS_BOTTOM / 2 - (splash_info.height) / 2), image); + } + break; + default: + write_vline_lm(APPLY_HDEADBAND(GRAPHICS_RIGHT / 2), APPLY_VDEADBAND(0), APPLY_VDEADBAND(GRAPHICS_BOTTOM), 1, 1); + write_hline_lm(APPLY_HDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT), APPLY_VDEADBAND(GRAPHICS_BOTTOM / 2), 1, 1); + break; } // Must mask out last half-word because SPI keeps clocking it out otherwise @@ -2368,7 +2404,7 @@ int32_t osdgenStart(void) { // Start gps task vSemaphoreCreateBinary(osdSemaphore); - xTaskCreate(osdgenTask, (signed char *) "OSDGEN", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdgenTaskHandle); + xTaskCreate(osdgenTask, (signed char *)"OSDGEN", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdgenTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OSDGEN, osdgenTaskHandle); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_OSDGEN); @@ -2400,7 +2436,7 @@ int32_t osdgenInitialize(void) return 0; } -MODULE_INITCALL( osdgenInitialize, osdgenStart) +MODULE_INITCALL(osdgenInitialize, osdgenStart) // **************** /** @@ -2409,10 +2445,11 @@ MODULE_INITCALL( osdgenInitialize, osdgenStart) static void osdgenTask(__attribute__((unused)) void *parameters) { - //portTickType lastSysTime; + // portTickType lastSysTime; // Loop forever - //lastSysTime = xTaskGetTickCount(); + // lastSysTime = xTaskGetTickCount(); OsdSettingsData OsdSettings; + OsdSettingsGet(&OsdSettings); PIOS_Servo_Set(0, OsdSettings.White); @@ -2446,8 +2483,8 @@ static void osdgenTask(__attribute__((unused)) void *parameters) #endif updateOnceEveryFrame(); } - //xSemaphoreTake(osdSemaphore, portMAX_DELAY); - //vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS); + // xSemaphoreTake(osdSemaphore, portMAX_DELAY); + // vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS); } } diff --git a/flight/modules/Osd/osdinput/osdinput.c b/flight/modules/Osd/osdinput/osdinput.c index 0b74109d0..6387eeb27 100644 --- a/flight/modules/Osd/osdinput/osdinput.c +++ b/flight/modules/Osd/osdinput/osdinput.c @@ -48,8 +48,8 @@ static void osdinputTask(void *parameters); // **************** // Private constants -#define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) +#define STACK_SIZE_BYTES 1024 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) #define MAX_PACKET_LENGTH 33 // **************** // Private variables @@ -58,11 +58,10 @@ static uint32_t oposdPort; static xTaskHandle osdinputTaskHandle; -static char* oposd_rx_buffer; +static char *oposd_rx_buffer; t_fifo_buffer rx; -enum osd_pkt_type -{ +enum osd_pkt_type { OSD_PKT_TYPE_MISC = 0, OSD_PKT_TYPE_NAV = 1, OSD_PKT_TYPE_MAINT = 2, OSD_PKT_TYPE_ATT = 3, OSD_PKT_TYPE_MODE = 4, }; @@ -76,7 +75,7 @@ enum osd_pkt_type int32_t osdinputStart(void) { // Start osdinput task - xTaskCreate(osdinputTask, (signed char *) "OSDINPUT", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdinputTaskHandle); + xTaskCreate(osdinputTask, (signed char *)"OSDINPUT", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdinputTaskHandle); return 0; } @@ -92,13 +91,13 @@ int32_t osdinputInitialize(void) // Initialize quaternion AttitudeActualData attitude; AttitudeActualGet(&attitude); - attitude.q1 = 1; - attitude.q2 = 0; - attitude.q3 = 0; - attitude.q4 = 0; - attitude.Roll = 0; + attitude.q1 = 1; + attitude.q2 = 0; + attitude.q3 = 0; + attitude.q4 = 0; + attitude.Roll = 0; attitude.Pitch = 0; - attitude.Yaw = 0; + attitude.Yaw = 0; AttitudeActualSet(&attitude); oposdPort = PIOS_COM_OSD; @@ -108,7 +107,7 @@ int32_t osdinputInitialize(void) return 0; } -MODULE_INITCALL( osdinputInitialize, osdinputStart) +MODULE_INITCALL(osdinputInitialize, osdinputStart) // **************** /** @@ -119,21 +118,21 @@ static void osdinputTask(__attribute__((unused)) void *parameters) { portTickType xDelay = 100 / portTICK_RATE_MS; portTickType lastSysTime; + lastSysTime = xTaskGetTickCount(); uint8_t rx_count = 0; - bool start_flag = false; + bool start_flag = false; int32_t osdRxOverflow = 0; uint8_t c = 0xAA; // Loop forever while (1) { // This blocks the task until there is something on the buffer while (PIOS_COM_ReceiveBuffer(oposdPort, &c, 1, xDelay) > 0) { - // detect start while acquiring stream if (!start_flag && ((c == 0xCB) || (c == 0x34))) { start_flag = true; - rx_count = 0; + rx_count = 0; } else if (!start_flag) { continue; } @@ -142,7 +141,7 @@ static void osdinputTask(__attribute__((unused)) void *parameters) // Flush the buffer and note the overflow event. osdRxOverflow++; start_flag = false; - rx_count = 0; + rx_count = 0; } else { oposd_rx_buffer[rx_count] = c; rx_count++; @@ -151,13 +150,13 @@ static void osdinputTask(__attribute__((unused)) void *parameters) if (oposd_rx_buffer[1] == OSD_PKT_TYPE_ATT) { AttitudeActualData attitude; AttitudeActualGet(&attitude); - attitude.q1 = 1; - attitude.q2 = 0; - attitude.q3 = 0; - attitude.q4 = 0; - attitude.Roll = (float) ((int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4] << 8)) / 10.0f; - attitude.Pitch = (float) ((int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6] << 8)) / 10.0f; - attitude.Yaw = (float) ((int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8] << 8)) / 10.0f; + attitude.q1 = 1; + attitude.q2 = 0; + attitude.q3 = 0; + attitude.q4 = 0; + attitude.Roll = (float)((int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4] << 8)) / 10.0f; + attitude.Pitch = (float)((int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6] << 8)) / 10.0f; + attitude.Yaw = (float)((int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8] << 8)) / 10.0f; AttitudeActualSet(&attitude); } else if (oposd_rx_buffer[1] == OSD_PKT_TYPE_MODE) { FlightStatusData status; @@ -166,9 +165,9 @@ static void osdinputTask(__attribute__((unused)) void *parameters) status.FlightMode = oposd_rx_buffer[3]; FlightStatusSet(&status); } - //frame completed + // frame completed start_flag = false; - rx_count = 0; + rx_count = 0; } } vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS); diff --git a/flight/modules/Osd/osdoutput/inc/osdoutput.h b/flight/modules/Osd/osdoutput/inc/osdoutput.h index a6f2e80bd..d03c29e88 100644 --- a/flight/modules/Osd/osdoutput/inc/osdoutput.h +++ b/flight/modules/Osd/osdoutput/inc/osdoutput.h @@ -32,7 +32,7 @@ int32_t osdoutputInitialize(void); -#endif /* OSDOUTPUT_H */ +#endif /* OSDOUTPUT_H */ /** * @} diff --git a/flight/modules/Osd/osdoutput/osdoutput.c b/flight/modules/Osd/osdoutput/osdoutput.c index 5b329831e..e6e5ff023 100644 --- a/flight/modules/Osd/osdoutput/osdoutput.c +++ b/flight/modules/Osd/osdoutput/osdoutput.c @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup OSDOUTPUTModule OSDOutput Module * @brief On screen display support - * @{ + * @{ * * @file osdoutput.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -45,103 +45,93 @@ static bool osdoutputEnabled; -enum osd_hk_sync -{ +enum osd_hk_sync { OSD_HK_SYNC_A = 0xCB, OSD_HK_SYNC_B = 0x34, }; -enum osd_hk_pkt_type -{ +enum osd_hk_pkt_type { OSD_HK_PKT_TYPE_MISC = 0, OSD_HK_PKT_TYPE_NAV = 1, OSD_HK_PKT_TYPE_MAINT = 2, OSD_HK_PKT_TYPE_ATT = 3, OSD_HK_PKT_TYPE_MODE = 4, }; -enum osd_hk_control_mode -{ +enum osd_hk_control_mode { OSD_HK_CONTROL_MODE_MANUAL = 0, OSD_HK_CONTROL_MODE_STABILIZED = 1, OSD_HK_CONTROL_MODE_AUTO = 2, }; -struct osd_hk_blob_misc -{ - uint8_t type; /* Always OSD_HK_PKT_TYPE_MISC */ - int16_t roll; - int16_t pitch; - //uint16_t home; /* Big Endian */ +struct osd_hk_blob_misc { + uint8_t type; /* Always OSD_HK_PKT_TYPE_MISC */ + int16_t roll; + int16_t pitch; + // uint16_t home; /* Big Endian */ enum osd_hk_control_mode control_mode; - uint8_t low_battery; + uint8_t low_battery; uint16_t current; /* Big Endian */ -}__attribute__((packed)); +} __attribute__((packed)); -struct osd_hk_blob_att -{ +struct osd_hk_blob_att { uint8_t type; /* Always OSD_HK_PKT_TYPE_ATT */ int16_t roll; int16_t pitch; int16_t yaw; int16_t speed; /* Big Endian */ -}__attribute__((packed)); +} __attribute__((packed)); -struct osd_hk_blob_nav -{ - uint8_t type; /* Always OSD_HK_PKT_TYPE_NAV */ +struct osd_hk_blob_nav { + uint8_t type; /* Always OSD_HK_PKT_TYPE_NAV */ uint32_t gps_lat; /* Big Endian */ uint32_t gps_lon; /* Big Endian */ -}__attribute__((packed)); +} __attribute__((packed)); -struct osd_hk_blob_maint -{ - uint8_t type; /* Always OSD_HK_PKT_TYPE_MAINT */ - uint8_t gps_speed; +struct osd_hk_blob_maint { + uint8_t type; /* Always OSD_HK_PKT_TYPE_MAINT */ + uint8_t gps_speed; uint16_t gps_alt; /* Big Endian */ uint16_t gps_dis; /* Big Endian */ - uint8_t status; - uint8_t config; - uint8_t emerg; -}__attribute__((packed)); + uint8_t status; + uint8_t config; + uint8_t emerg; +} __attribute__((packed)); -struct osd_hk_blob_mode -{ - uint8_t type; /* Always OSD_HK_PKT_TYPE_MODE */ - uint8_t fltmode; +struct osd_hk_blob_mode { + uint8_t type; /* Always OSD_HK_PKT_TYPE_MODE */ + uint8_t fltmode; uint16_t gps_alt; /* Big Endian */ uint16_t gps_dis; /* Big Endian */ - uint8_t armed; - uint8_t config; - uint8_t emerg; -}__attribute__((packed)); + uint8_t armed; + uint8_t config; + uint8_t emerg; +} __attribute__((packed)); -union osd_hk_pkt_blobs -{ - struct osd_hk_blob_misc misc; - struct osd_hk_blob_nav nav; +union osd_hk_pkt_blobs { + struct osd_hk_blob_misc misc; + struct osd_hk_blob_nav nav; struct osd_hk_blob_maint maint; - struct osd_hk_blob_att att; - struct osd_hk_blob_mode mode; -}__attribute__((packed)); + struct osd_hk_blob_att att; + struct osd_hk_blob_mode mode; +} __attribute__((packed)); -struct osd_hk_msg -{ - enum osd_hk_sync sync; - enum osd_hk_pkt_type t; +struct osd_hk_msg { + enum osd_hk_sync sync; + enum osd_hk_pkt_type t; union osd_hk_pkt_blobs v; -}__attribute__((packed)); +} __attribute__((packed)); static struct osd_hk_msg osd_hk_msg_buf; static volatile bool newPositionActualData = false; -static volatile bool newBattData = false; +static volatile bool newBattData = false; static volatile bool newAttitudeData = false; -static volatile bool newAlarmData = false; +static volatile bool newAlarmData = false; static uint32_t osd_hk_com_id; static uint8_t osd_hk_msg_dropped; static uint8_t osd_packet; -static void send_update(__attribute__((unused)) UAVObjEvent * ev) +static void send_update(__attribute__((unused)) UAVObjEvent *ev) { static enum osd_hk_sync sync = OSD_HK_SYNC_A; - struct osd_hk_msg * msg = &osd_hk_msg_buf; - union osd_hk_pkt_blobs * blob = &(osd_hk_msg_buf.v); + struct osd_hk_msg *msg = &osd_hk_msg_buf; + union osd_hk_pkt_blobs *blob = &(osd_hk_msg_buf.v); /* Make sure we have a COM port bound */ if (!osd_hk_com_id) { @@ -156,57 +146,57 @@ static void send_update(__attribute__((unused)) UAVObjEvent * ev) msg->sync = sync; switch (osd_packet) { - case OSD_HK_PKT_TYPE_MISC: - break; - case OSD_HK_PKT_TYPE_NAV: - break; - case OSD_HK_PKT_TYPE_MAINT: - break; - case OSD_HK_PKT_TYPE_ATT: - msg->t = OSD_HK_PKT_TYPE_ATT; - float roll; - AttitudeActualRollGet(&roll); - blob->att.roll = (int16_t)(roll * 10); + case OSD_HK_PKT_TYPE_MISC: + break; + case OSD_HK_PKT_TYPE_NAV: + break; + case OSD_HK_PKT_TYPE_MAINT: + break; + case OSD_HK_PKT_TYPE_ATT: + msg->t = OSD_HK_PKT_TYPE_ATT; + float roll; + AttitudeActualRollGet(&roll); + blob->att.roll = (int16_t)(roll * 10); - float pitch; - AttitudeActualPitchGet(&pitch); - blob->att.pitch = (int16_t)(pitch * 10); + float pitch; + AttitudeActualPitchGet(&pitch); + blob->att.pitch = (int16_t)(pitch * 10); - float yaw; - AttitudeActualYawGet(&yaw); - blob->att.yaw = (int16_t)(yaw * 10); - break; - case OSD_HK_PKT_TYPE_MODE: - msg->t = OSD_HK_PKT_TYPE_MODE; - FlightStatusGet(&flightStatus); - blob->mode.fltmode = flightStatus.FlightMode; - blob->mode.armed = flightStatus.Armed; - break; - default: - break; + float yaw; + AttitudeActualYawGet(&yaw); + blob->att.yaw = (int16_t)(yaw * 10); + break; + case OSD_HK_PKT_TYPE_MODE: + msg->t = OSD_HK_PKT_TYPE_MODE; + FlightStatusGet(&flightStatus); + blob->mode.fltmode = flightStatus.FlightMode; + blob->mode.armed = flightStatus.Armed; + break; + default: + break; } /* Field not supported yet */ -//blob->misc.control_mode = 0; +// blob->misc.control_mode = 0; /*if (newAlarmData) { - SystemAlarmsData alarms; - SystemAlarmsGet(&alarms); + SystemAlarmsData alarms; + SystemAlarmsGet(&alarms); - switch (alarms.Alarm[SYSTEMALARMS_ALARM_BATTERY]) { - case SYSTEMALARMS_ALARM_UNINITIALISED: - case SYSTEMALARMS_ALARM_OK: - blob->misc.low_battery = 0; - break; - case SYSTEMALARMS_ALARM_WARNING: - case SYSTEMALARMS_ALARM_ERROR: - case SYSTEMALARMS_ALARM_CRITICAL: - default: - blob->misc.low_battery = 1; - break; - } + switch (alarms.Alarm[SYSTEMALARMS_ALARM_BATTERY]) { + case SYSTEMALARMS_ALARM_UNINITIALISED: + case SYSTEMALARMS_ALARM_OK: + blob->misc.low_battery = 0; + break; + case SYSTEMALARMS_ALARM_WARNING: + case SYSTEMALARMS_ALARM_ERROR: + case SYSTEMALARMS_ALARM_CRITICAL: + default: + blob->misc.low_battery = 1; + break; + } - newAlarmData = false; - }*/ + newAlarmData = false; + }*/ #if FLIGHTBATTERYSUPPORTED if (newBattData) { @@ -217,13 +207,13 @@ static void send_update(__attribute__((unused)) UAVObjEvent * ev) /* convert to big endian */ blob->misc.current = ( - (current & 0xFF00 >> 8) | - (current & 0x00FF << 8)); + (current & 0xFF00 >> 8) | + (current & 0x00FF << 8)); newBattData = false; } #else -//blob->misc.current = 0; +// blob->misc.current = 0; #endif #if POSITIONACTUAL_SUPPORTED @@ -233,24 +223,24 @@ static void send_update(__attribute__((unused)) UAVObjEvent * ev) /* compute 3D distance */ float d = sqrt( - pow(position.North,2) + - pow(position.East,2) + - pow(position.Down,2)); + pow(position.North, 2) + + pow(position.East, 2) + + pow(position.Down, 2)); /* convert from cm to dm (10ths of m) */ uint16_t home = (uint16_t)(d / 10); /* convert to big endian */ blob->misc.home = ( - (home & 0xFF00 >> 8) | - (home & 0x00FF << 8)); + (home & 0xFF00 >> 8) | + (home & 0x00FF << 8)); newPositionActualData = false; } #else -//blob->misc.home = 0; -#endif +// blob->misc.home = 0; +#endif /* if POSITIONACTUAL_SUPPORTED */ - if (!PIOS_COM_SendBufferNonBlocking(osd_hk_com_id, (uint8_t *) &osd_hk_msg_buf, sizeof(osd_hk_msg_buf))) { + if (!PIOS_COM_SendBufferNonBlocking(osd_hk_com_id, (uint8_t *)&osd_hk_msg_buf, sizeof(osd_hk_msg_buf))) { /* Sent a packet, flip to the opposite sync */ if (sync == OSD_HK_SYNC_A) { sync = OSD_HK_SYNC_B; @@ -281,7 +271,7 @@ static int32_t osdoutputStart(void) static int32_t osdoutputInitialize(void) { - osd_hk_com_id = PIOS_COM_OSDHK; + osd_hk_com_id = PIOS_COM_OSDHK; #ifdef MODULE_OSDOUTPUT_BUILTIN osdoutputEnabled = 1; #else diff --git a/flight/modules/OveroSync/inc/overosync.h b/flight/modules/OveroSync/inc/overosync.h index b4e449c9c..c3a19cb95 100644 --- a/flight/modules/OveroSync/inc/overosync.h +++ b/flight/modules/OveroSync/inc/overosync.h @@ -1,18 +1,18 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup TelemetryModule Telemetry Module * @brief Main telemetry module * Starts three tasks (RX, TX, and priority TX) that watch event queues * and handle all the telemetry of the UAVobjects - * @{ + * @{ * * @file telemetry.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Include file of the telemetry module. - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and * objects. * @see The GNU Public License (GPL) Version 3 * @@ -41,6 +41,6 @@ int32_t TelemetryInitialize(void); #endif // TELEMETRY_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/OveroSync/overosync.c b/flight/modules/OveroSync/overosync.c index 5a2f3f5c9..039bd35cb 100644 --- a/flight/modules/OveroSync/overosync.c +++ b/flight/modules/OveroSync/overosync.c @@ -1,12 +1,12 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup Overo Sync Module * @brief Overo sync module * Starts a sync tasks that watch event queues * and push to Overo spi port UAVobjects - * @{ + * @{ * * @file overosync.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -42,7 +42,7 @@ // Private constants #define MAX_QUEUE_SIZE 200 #define STACK_SIZE_BYTES 512 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 0) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 0) // Private types @@ -54,7 +54,7 @@ static bool overoEnabled; // Private functions static void overoSyncTask(void *parameters); -static int32_t packData(uint8_t * data, int32_t length); +static int32_t packData(uint8_t *data, int32_t length); static void registerObject(UAVObjHandle obj); // External variables @@ -62,10 +62,10 @@ extern uint32_t pios_com_overo_id; extern uint32_t pios_overo_id; struct overosync { - uint32_t sent_bytes; - uint32_t sent_objects; - uint32_t failed_objects; - uint32_t received_objects; + uint32_t sent_bytes; + uint32_t sent_objects; + uint32_t failed_objects; + uint32_t received_objects; }; struct overosync *overosync; @@ -77,34 +77,33 @@ struct overosync *overosync; */ int32_t OveroSyncInitialize(void) { - #ifdef MODULE_OVEROSYNC_BUILTIN - overoEnabled = true; + overoEnabled = true; #else - - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); - - if (optionalModules[HWSETTINGS_OPTIONALMODULES_OVERO] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - overoEnabled = true; - - // Create object queues - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - } else { - overoEnabled = false; - return -1; - } + + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + + if (optionalModules[HWSETTINGS_OPTIONALMODULES_OVERO] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + overoEnabled = true; + + // Create object queues + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + } else { + overoEnabled = false; + return -1; + } #endif - - - OveroSyncStatsInitialize(); - // Initialise UAVTalk - uavTalkCon = UAVTalkInitialize(&packData); + OveroSyncStatsInitialize(); - return 0; + + // Initialise UAVTalk + uavTalkCon = UAVTalkInitialize(&packData); + + return 0; } /** @@ -114,26 +113,27 @@ int32_t OveroSyncInitialize(void) */ int32_t OveroSyncStart(void) { - //Check if module is enabled or not - if (overoEnabled == false) { - return -1; - } - - overosync = (struct overosync *) pvPortMalloc(sizeof(*overosync)); - if(overosync == NULL) - return -1; + // Check if module is enabled or not + if (overoEnabled == false) { + return -1; + } - overosync->sent_bytes = 0; + overosync = (struct overosync *)pvPortMalloc(sizeof(*overosync)); + if (overosync == NULL) { + return -1; + } - // Process all registered objects and connect queue for updates - UAVObjIterate(®isterObject); - - // Start overosync tasks - xTaskCreate(overoSyncTask, (signed char *)"OveroSync", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &overoSyncTaskHandle); - - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle); - - return 0; + overosync->sent_bytes = 0; + + // Process all registered objects and connect queue for updates + UAVObjIterate(®isterObject); + + // Start overosync tasks + xTaskCreate(overoSyncTask, (signed char *)"OveroSync", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &overoSyncTaskHandle); + + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle); + + return 0; } MODULE_INITCALL(OveroSyncInitialize, OveroSyncStart) @@ -145,12 +145,13 @@ MODULE_INITCALL(OveroSyncInitialize, OveroSyncStart) */ static void registerObject(UAVObjHandle obj) { - int32_t eventMask; - eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - if (UAVObjIsMetaobject(obj)) { - eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) - } - UAVObjConnectQueue(obj, queue, eventMask); + int32_t eventMask; + + eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; + if (UAVObjIsMetaobject(obj)) { + eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) + } + UAVObjConnectQueue(obj, queue, eventMask); } /** @@ -164,41 +165,40 @@ static void registerObject(UAVObjHandle obj) */ static void overoSyncTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; + UAVObjEvent ev; - // Kick off SPI transfers (once one is completed another will automatically transmit) - overosync->sent_objects = 0; - overosync->failed_objects = 0; - overosync->received_objects = 0; - - portTickType lastUpdateTime = xTaskGetTickCount(); - portTickType updateTime; + // Kick off SPI transfers (once one is completed another will automatically transmit) + overosync->sent_objects = 0; + overosync->failed_objects = 0; + overosync->received_objects = 0; - // Loop forever - while (1) { - // Wait for queue message - if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) { + portTickType lastUpdateTime = xTaskGetTickCount(); + portTickType updateTime; - // Process event. This calls transmitData - UAVTalkSendObjectTimestamped(uavTalkCon, ev.obj, ev.instId, false, 0); - - updateTime = xTaskGetTickCount(); - if(((portTickType) (updateTime - lastUpdateTime)) > 1000) { - // Update stats. This will trigger a local send event too - OveroSyncStatsData syncStats; - syncStats.Send = overosync->sent_bytes; - syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE; - syncStats.DroppedUpdates = overosync->failed_objects; - syncStats.Packets = PIOS_OVERO_GetPacketCount(pios_overo_id); - OveroSyncStatsSet(&syncStats); - overosync->failed_objects = 0; - overosync->sent_bytes = 0; - lastUpdateTime = updateTime; - } + // Loop forever + while (1) { + // Wait for queue message + if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) { + // Process event. This calls transmitData + UAVTalkSendObjectTimestamped(uavTalkCon, ev.obj, ev.instId, false, 0); - // TODO: Check the receive buffer - } - } + updateTime = xTaskGetTickCount(); + if (((portTickType)(updateTime - lastUpdateTime)) > 1000) { + // Update stats. This will trigger a local send event too + OveroSyncStatsData syncStats; + syncStats.Send = overosync->sent_bytes; + syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE; + syncStats.DroppedUpdates = overosync->failed_objects; + syncStats.Packets = PIOS_OVERO_GetPacketCount(pios_overo_id); + OveroSyncStatsSet(&syncStats); + overosync->failed_objects = 0; + overosync->sent_bytes = 0; + lastUpdateTime = updateTime; + } + + // TODO: Check the receive buffer + } + } } /** @@ -208,21 +208,22 @@ static void overoSyncTask(__attribute__((unused)) void *parameters) * \return -1 on failure * \return number of bytes transmitted on success */ -static int32_t packData(uint8_t * data, int32_t length) +static int32_t packData(uint8_t *data, int32_t length) { - if( PIOS_COM_SendBufferNonBlocking(pios_com_overo_id, data, length) < 0) - goto fail; + if (PIOS_COM_SendBufferNonBlocking(pios_com_overo_id, data, length) < 0) { + goto fail; + } - overosync->sent_bytes += length; + overosync->sent_bytes += length; - return length; + return length; fail: - overosync->failed_objects++; - return -1; + overosync->failed_objects++; + return -1; } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/OveroSync/simulated/inc/overosync.h b/flight/modules/OveroSync/simulated/inc/overosync.h index b4e449c9c..c3a19cb95 100644 --- a/flight/modules/OveroSync/simulated/inc/overosync.h +++ b/flight/modules/OveroSync/simulated/inc/overosync.h @@ -1,18 +1,18 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup TelemetryModule Telemetry Module * @brief Main telemetry module * Starts three tasks (RX, TX, and priority TX) that watch event queues * and handle all the telemetry of the UAVobjects - * @{ + * @{ * * @file telemetry.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Include file of the telemetry module. - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and * objects. * @see The GNU Public License (GPL) Version 3 * @@ -41,6 +41,6 @@ int32_t TelemetryInitialize(void); #endif // TELEMETRY_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/OveroSync/simulated/overosync.c b/flight/modules/OveroSync/simulated/overosync.c index d8fa0058a..291765f8a 100644 --- a/flight/modules/OveroSync/simulated/overosync.c +++ b/flight/modules/OveroSync/simulated/overosync.c @@ -1,12 +1,12 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup TelemetryModule Telemetry Module * @brief Main telemetry module * Starts three tasks (RX, TX, and priority TX) that watch event queues * and handle all the telemetry of the UAVobjects - * @{ + * @{ * * @file telemetry.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -40,9 +40,9 @@ // Private constants #define OVEROSYNC_PACKET_SIZE 1024 -#define MAX_QUEUE_SIZE 40 -#define STACK_SIZE_BYTES 512 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 0) +#define MAX_QUEUE_SIZE 40 +#define STACK_SIZE_BYTES 512 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 0) // Private types @@ -52,31 +52,31 @@ static UAVTalkConnection uavTalkCon; static xTaskHandle overoSyncTaskHandle; volatile bool buffer_swap_failed; volatile uint32_t buffer_swap_timeval; -FILE * fid; +FILE *fid; // Private functions static void overoSyncTask(void *parameters); -static int32_t packData(uint8_t * data, int32_t length); +static int32_t packData(uint8_t *data, int32_t length); static void registerObject(UAVObjHandle obj); struct dma_transaction { - uint8_t tx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__ ((aligned(4))); - uint8_t rx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__ ((aligned(4))); + uint8_t tx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__((aligned(4))); + uint8_t rx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__((aligned(4))); }; struct overosync { - struct dma_transaction transactions[2]; - uint32_t active_transaction_id; - uint32_t loading_transaction_id; - xSemaphoreHandle transaction_lock; - xSemaphoreHandle buffer_lock; - volatile bool transaction_done; - uint32_t sent_bytes; - uint32_t write_pointer; - uint32_t sent_objects; - uint32_t failed_objects; - uint32_t received_objects; - uint32_t framesync_error; + struct dma_transaction transactions[2]; + uint32_t active_transaction_id; + uint32_t loading_transaction_id; + xSemaphoreHandle transaction_lock; + xSemaphoreHandle buffer_lock; + volatile bool transaction_done; + uint32_t sent_bytes; + uint32_t write_pointer; + uint32_t sent_objects; + uint32_t failed_objects; + uint32_t received_objects; + uint32_t framesync_error; }; struct overosync *overosync; @@ -88,15 +88,15 @@ struct overosync *overosync; */ int32_t OveroSyncInitialize(void) { - OveroSyncStatsInitialize(); + OveroSyncStatsInitialize(); - // Create object queues - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + // Create object queues + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - // Initialise UAVTalk - uavTalkCon = UAVTalkInitialize(&packData); + // Initialise UAVTalk + uavTalkCon = UAVTalkInitialize(&packData); - return 0; + return 0; } /** @@ -106,33 +106,36 @@ int32_t OveroSyncInitialize(void) */ int32_t OveroSyncStart(void) { - overosync = (struct overosync *) pvPortMalloc(sizeof(*overosync)); - if(overosync == NULL) - return -1; + overosync = (struct overosync *)pvPortMalloc(sizeof(*overosync)); + if (overosync == NULL) { + return -1; + } - overosync->transaction_lock = xSemaphoreCreateMutex(); - if(overosync->transaction_lock == NULL) - return -1; + overosync->transaction_lock = xSemaphoreCreateMutex(); + if (overosync->transaction_lock == NULL) { + return -1; + } - overosync->buffer_lock = xSemaphoreCreateMutex(); - if(overosync->buffer_lock == NULL) - return -1; + overosync->buffer_lock = xSemaphoreCreateMutex(); + if (overosync->buffer_lock == NULL) { + return -1; + } - overosync->active_transaction_id = 0; - overosync->loading_transaction_id = 0; - overosync->write_pointer = 0; - overosync->sent_bytes = 0; - overosync->framesync_error = 0; + overosync->active_transaction_id = 0; + overosync->loading_transaction_id = 0; + overosync->write_pointer = 0; + overosync->sent_bytes = 0; + overosync->framesync_error = 0; - // Process all registered objects and connect queue for updates - UAVObjIterate(®isterObject); - - // Start telemetry tasks - xTaskCreate(overoSyncTask, (signed char *)"OveroSync", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &overoSyncTaskHandle); - - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle); - - return 0; + // Process all registered objects and connect queue for updates + UAVObjIterate(®isterObject); + + // Start telemetry tasks + xTaskCreate(overoSyncTask, (signed char *)"OveroSync", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &overoSyncTaskHandle); + + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle); + + return 0; } MODULE_INITCALL(OveroSyncInitialize, OveroSyncStart) @@ -144,12 +147,13 @@ MODULE_INITCALL(OveroSyncInitialize, OveroSyncStart) */ static void registerObject(UAVObjHandle obj) { - int32_t eventMask; - eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - if (UAVObjIsMetaobject(obj)) { - eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) - } - UAVObjConnectQueue(obj, queue, eventMask); + int32_t eventMask; + + eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; + if (UAVObjIsMetaobject(obj)) { + eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) + } + UAVObjConnectQueue(obj, queue, eventMask); } /** @@ -163,48 +167,47 @@ static void registerObject(UAVObjHandle obj) */ static void overoSyncTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; + UAVObjEvent ev; - // Kick off SPI transfers (once one is completed another will automatically transmit) - overosync->transaction_done = true; - overosync->sent_objects = 0; - overosync->failed_objects = 0; - overosync->received_objects = 0; - - portTickType lastUpdateTime = xTaskGetTickCount(); - portTickType updateTime; - - fid = fopen("sim_log.opl", "w"); + // Kick off SPI transfers (once one is completed another will automatically transmit) + overosync->transaction_done = true; + overosync->sent_objects = 0; + overosync->failed_objects = 0; + overosync->received_objects = 0; - // Loop forever - while (1) { - // Wait for queue message - if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) { - - // Check it will fit before packetizing - if ((overosync->write_pointer + UAVObjGetNumBytes(ev.obj) + 12) >= - sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer)) { - overosync->failed_objects ++; - } else { - // Process event. This calls transmitData - UAVTalkSendObject(uavTalkCon, ev.obj, ev.instId, false, 0); - } + portTickType lastUpdateTime = xTaskGetTickCount(); + portTickType updateTime; - updateTime = xTaskGetTickCount(); - if(((portTickType) (updateTime - lastUpdateTime)) > 1000) { - // Update stats. This will trigger a local send event too - OveroSyncStatsData syncStats; - syncStats.Send = overosync->sent_bytes; - syncStats.Received = 0; - syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE; - syncStats.DroppedUpdates = overosync->failed_objects; - OveroSyncStatsSet(&syncStats); - overosync->failed_objects = 0; - overosync->sent_bytes = 0; - lastUpdateTime = updateTime; - } - } - } + fid = fopen("sim_log.opl", "w"); + + // Loop forever + while (1) { + // Wait for queue message + if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) { + // Check it will fit before packetizing + if ((overosync->write_pointer + UAVObjGetNumBytes(ev.obj) + 12) >= + sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer)) { + overosync->failed_objects++; + } else { + // Process event. This calls transmitData + UAVTalkSendObject(uavTalkCon, ev.obj, ev.instId, false, 0); + } + + updateTime = xTaskGetTickCount(); + if (((portTickType)(updateTime - lastUpdateTime)) > 1000) { + // Update stats. This will trigger a local send event too + OveroSyncStatsData syncStats; + syncStats.Send = overosync->sent_bytes; + syncStats.Received = 0; + syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE; + syncStats.DroppedUpdates = overosync->failed_objects; + OveroSyncStatsSet(&syncStats); + overosync->failed_objects = 0; + overosync->sent_bytes = 0; + lastUpdateTime = updateTime; + } + } + } } /** @@ -214,25 +217,25 @@ static void overoSyncTask(__attribute__((unused)) void *parameters) * \return -1 on failure * \return number of bytes transmitted on success */ -static int32_t packData(uint8_t * data, int32_t length) +static int32_t packData(uint8_t *data, int32_t length) { - // Get the lock for manipulating the buffer - xSemaphoreTake(overosync->buffer_lock, portMAX_DELAY); + // Get the lock for manipulating the buffer + xSemaphoreTake(overosync->buffer_lock, portMAX_DELAY); - portTickType tickTime = xTaskGetTickCount(); - uint64_t packetSize = data[2] + (data[3] << 8); - fwrite((void *) &tickTime, 1, sizeof(tickTime), fid); - fwrite((void *) &packetSize, sizeof(packetSize), 1, fid); - fwrite((void *) data, 1, length, fid); - overosync->sent_bytes += length; - overosync->sent_objects++; + portTickType tickTime = xTaskGetTickCount(); + uint64_t packetSize = data[2] + (data[3] << 8); + fwrite((void *)&tickTime, 1, sizeof(tickTime), fid); + fwrite((void *)&packetSize, sizeof(packetSize), 1, fid); + fwrite((void *)data, 1, length, fid); + overosync->sent_bytes += length; + overosync->sent_objects++; - xSemaphoreGive(overosync->buffer_lock); - - return length; + xSemaphoreGive(overosync->buffer_lock); + + return length; } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index d9bd44724..30bd42fa3 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -45,16 +45,16 @@ #include "paths.h" // Private constants -#define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) -#define MAX_QUEUE_SIZE 2 +#define STACK_SIZE_BYTES 1024 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define MAX_QUEUE_SIZE 2 #define PATH_PLANNER_UPDATE_RATE_MS 20 // Private types // Private functions static void pathPlannerTask(void *parameters); -static void updatePathDesired(UAVObjEvent * ev); +static void updatePathDesired(UAVObjEvent *ev); static void setWaypoint(uint16_t num); static uint8_t pathConditionCheck(); @@ -83,13 +83,13 @@ static bool pathplanner_active = false; */ int32_t PathPlannerStart() { - taskHandle = NULL; + taskHandle = NULL; - // Start VM thread - xTaskCreate(pathPlannerTask, (signed char *)"PathPlanner", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHPLANNER, taskHandle); + // Start VM thread + xTaskCreate(pathPlannerTask, (signed char *)"PathPlanner", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHPLANNER, taskHandle); - return 0; + return 0; } /** @@ -97,18 +97,18 @@ int32_t PathPlannerStart() */ int32_t PathPlannerInitialize() { - taskHandle = NULL; + taskHandle = NULL; - PathActionInitialize(); - PathStatusInitialize(); - PathDesiredInitialize(); - PositionActualInitialize(); - AirspeedActualInitialize(); - VelocityActualInitialize(); - WaypointInitialize(); - WaypointActiveInitialize(); - - return 0; + PathActionInitialize(); + PathStatusInitialize(); + PathDesiredInitialize(); + PositionActualInitialize(); + AirspeedActualInitialize(); + VelocityActualInitialize(); + WaypointInitialize(); + WaypointActiveInitialize(); + + return 0; } MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart) @@ -118,227 +118,242 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart) */ static void pathPlannerTask(__attribute__((unused)) void *parameters) { - // when the active waypoint changes, update pathDesired - WaypointConnectCallback(updatePathDesired); - WaypointActiveConnectCallback(updatePathDesired); - PathActionConnectCallback(updatePathDesired); + // when the active waypoint changes, update pathDesired + WaypointConnectCallback(updatePathDesired); + WaypointActiveConnectCallback(updatePathDesired); + PathActionConnectCallback(updatePathDesired); - FlightStatusData flightStatus; - PathDesiredData pathDesired; - PathStatusData pathStatus; - - // Main thread loop - bool endCondition = false; - while (1) - { + FlightStatusData flightStatus; + PathDesiredData pathDesired; + PathStatusData pathStatus; - vTaskDelay(PATH_PLANNER_UPDATE_RATE_MS); + // Main thread loop + bool endCondition = false; + while (1) { + vTaskDelay(PATH_PLANNER_UPDATE_RATE_MS); - FlightStatusGet(&flightStatus); - if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) { - pathplanner_active = false; - continue; - } + FlightStatusGet(&flightStatus); + if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) { + pathplanner_active = false; + continue; + } - WaypointActiveGet(&waypointActive); + WaypointActiveGet(&waypointActive); - if(pathplanner_active == false) { + if (pathplanner_active == false) { + pathplanner_active = true; - pathplanner_active = true; + // This triggers callback to update variable + waypointActive.Index = 0; + WaypointActiveSet(&waypointActive); - // This triggers callback to update variable - waypointActive.Index = 0; - WaypointActiveSet(&waypointActive); + continue; + } - continue; - } + WaypointInstGet(waypointActive.Index, &waypoint); + PathActionInstGet(waypoint.Action, &pathAction); + PathStatusGet(&pathStatus); + PathDesiredGet(&pathDesired); - WaypointInstGet(waypointActive.Index,&waypoint); - PathActionInstGet(waypoint.Action, &pathAction); - PathStatusGet(&pathStatus); - PathDesiredGet(&pathDesired); + // delay next step until path follower has acknowledged the path mode + if (pathStatus.UID != pathDesired.UID) { + continue; + } - // delay next step until path follower has acknowledged the path mode - if (pathStatus.UID != pathDesired.UID) { - continue; - } + // negative destinations DISABLE this feature + if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) { + setWaypoint(pathAction.ErrorDestination); + continue; + } - // negative destinations DISABLE this feature - if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) { - setWaypoint(pathAction.ErrorDestination); - continue; - } + // check if condition has been met + endCondition = pathConditionCheck(); - // check if condition has been met - endCondition = pathConditionCheck(); - - // decide what to do - switch (pathAction.Command) { - case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT: - endCondition = !endCondition; - case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT: - if (endCondition) setWaypoint(waypointActive.Index+1); - break; - case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT: - endCondition = !endCondition; - case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT: - if (endCondition) { - if (pathAction.JumpDestination<0) { - // waypoint ids <0 code relative jumps - setWaypoint(waypointActive.Index - pathAction.JumpDestination ); - } else { - setWaypoint(pathAction.JumpDestination); - } - } - break; - case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: - if (endCondition) { - if (pathAction.JumpDestination<0) { - // waypoint ids <0 code relative jumps - setWaypoint(waypointActive.Index - pathAction.JumpDestination ); - } else { - setWaypoint(pathAction.JumpDestination); - } - } else { - setWaypoint(waypointActive.Index+1); - } - break; - } - } + // decide what to do + switch (pathAction.Command) { + case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT: + endCondition = !endCondition; + case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT: + if (endCondition) { + setWaypoint(waypointActive.Index + 1); + } + break; + case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT: + endCondition = !endCondition; + case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT: + if (endCondition) { + if (pathAction.JumpDestination < 0) { + // waypoint ids <0 code relative jumps + setWaypoint(waypointActive.Index - pathAction.JumpDestination); + } else { + setWaypoint(pathAction.JumpDestination); + } + } + break; + case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: + if (endCondition) { + if (pathAction.JumpDestination < 0) { + // waypoint ids <0 code relative jumps + setWaypoint(waypointActive.Index - pathAction.JumpDestination); + } else { + setWaypoint(pathAction.JumpDestination); + } + } else { + setWaypoint(waypointActive.Index + 1); + } + break; + } + } } // callback function when waypoints changed in any way, update pathDesired -void updatePathDesired(__attribute__((unused)) UAVObjEvent * ev) { +void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev) +{ + // only ever touch pathDesired if pathplanner is enabled + if (!pathplanner_active) { + return; + } - // only ever touch pathDesired if pathplanner is enabled - if (!pathplanner_active) return; + // use local variables, dont use stack since this is huge and a callback, + // dont use the globals because we cant use mutexes here + static WaypointActiveData waypointActiveData; + static PathActionData pathActionData; + static WaypointData waypointData; + static PathDesiredData pathDesired; - // use local variables, dont use stack since this is huge and a callback, - // dont use the globals because we cant use mutexes here - static WaypointActiveData waypointActiveData; - static PathActionData pathActionData; - static WaypointData waypointData; - static PathDesiredData pathDesired; + // find out current waypoint + WaypointActiveGet(&waypointActiveData); - // find out current waypoint - WaypointActiveGet(&waypointActiveData); + WaypointInstGet(waypointActiveData.Index, &waypointData); + PathActionInstGet(waypointData.Action, &pathActionData); - WaypointInstGet(waypointActiveData.Index,&waypointData); - PathActionInstGet(waypointData.Action, &pathActionData); + pathDesired.End[PATHDESIRED_END_NORTH] = waypointData.Position[WAYPOINT_POSITION_NORTH]; + pathDesired.End[PATHDESIRED_END_EAST] = waypointData.Position[WAYPOINT_POSITION_EAST]; + pathDesired.End[PATHDESIRED_END_DOWN] = waypointData.Position[WAYPOINT_POSITION_DOWN]; + pathDesired.EndingVelocity = waypointData.Velocity; + pathDesired.Mode = pathActionData.Mode; + pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0]; + pathDesired.ModeParameters[1] = pathActionData.ModeParameters[1]; + pathDesired.ModeParameters[2] = pathActionData.ModeParameters[2]; + pathDesired.ModeParameters[3] = pathActionData.ModeParameters[3]; + pathDesired.UID = waypointActiveData.Index; - pathDesired.End[PATHDESIRED_END_NORTH] = waypointData.Position[WAYPOINT_POSITION_NORTH]; - pathDesired.End[PATHDESIRED_END_EAST] = waypointData.Position[WAYPOINT_POSITION_EAST]; - pathDesired.End[PATHDESIRED_END_DOWN] = waypointData.Position[WAYPOINT_POSITION_DOWN]; - pathDesired.EndingVelocity = waypointData.Velocity; - pathDesired.Mode = pathActionData.Mode; - pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0]; - pathDesired.ModeParameters[1] = pathActionData.ModeParameters[1]; - pathDesired.ModeParameters[2] = pathActionData.ModeParameters[2]; - pathDesired.ModeParameters[3] = pathActionData.ModeParameters[3]; - pathDesired.UID = waypointActiveData.Index; + if (waypointActiveData.Index == 0) { + PositionActualData positionActual; + PositionActualGet(&positionActual); + // First waypoint has itself as start point (used to be home position but that proved dangerous when looping) - if(waypointActiveData.Index == 0) { - PositionActualData positionActual; - PositionActualGet(&positionActual); - // First waypoint has itself as start point (used to be home position but that proved dangerous when looping) + /*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; + pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST]; + pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/ + pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; + pathDesired.StartingVelocity = pathDesired.EndingVelocity; + } else { + // Get previous waypoint as start point + WaypointData waypointPrev; + WaypointInstGet(waypointActive.Index - 1, &waypointPrev); - /*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; - pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST]; - pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/ - pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; - pathDesired.StartingVelocity = pathDesired.EndingVelocity; - } else { - // Get previous waypoint as start point - WaypointData waypointPrev; - WaypointInstGet(waypointActive.Index - 1, &waypointPrev); - - pathDesired.Start[PATHDESIRED_START_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH]; - pathDesired.Start[PATHDESIRED_START_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST]; - pathDesired.Start[PATHDESIRED_START_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN]; - pathDesired.StartingVelocity = waypointPrev.Velocity; - } - PathDesiredSet(&pathDesired); + pathDesired.Start[PATHDESIRED_START_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH]; + pathDesired.Start[PATHDESIRED_START_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST]; + pathDesired.Start[PATHDESIRED_START_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN]; + pathDesired.StartingVelocity = waypointPrev.Velocity; + } + PathDesiredSet(&pathDesired); } // helper function to go to a specific waypoint -static void setWaypoint(uint16_t num) { - // path plans wrap around - if (num>=UAVObjGetNumInstances(WaypointHandle())) { - num = 0; - } - - waypointActive.Index = num; - WaypointActiveSet(&waypointActive); +static void setWaypoint(uint16_t num) +{ + // path plans wrap around + if (num >= UAVObjGetNumInstances(WaypointHandle())) { + num = 0; + } + waypointActive.Index = num; + WaypointActiveSet(&waypointActive); } // execute the apropriate condition and report result -static uint8_t pathConditionCheck() { - // i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's - switch (pathAction.EndCondition) { - case PATHACTION_ENDCONDITION_NONE: - return conditionNone(); - break; - case PATHACTION_ENDCONDITION_TIMEOUT: - return conditionTimeOut(); - break; - case PATHACTION_ENDCONDITION_DISTANCETOTARGET: - return conditionDistanceToTarget(); - break; - case PATHACTION_ENDCONDITION_LEGREMAINING: - return conditionLegRemaining(); - break; - case PATHACTION_ENDCONDITION_BELOWERROR: - return conditionBelowError(); - break; - case PATHACTION_ENDCONDITION_ABOVEALTITUDE: - return conditionAboveAltitude(); - break; - case PATHACTION_ENDCONDITION_ABOVESPEED: - return conditionAboveSpeed(); - break; - case PATHACTION_ENDCONDITION_POINTINGTOWARDSNEXT: - return conditionPointingTowardsNext(); - break; - case PATHACTION_ENDCONDITION_PYTHONSCRIPT: - return conditionPythonScript(); - break; - case PATHACTION_ENDCONDITION_IMMEDIATE: - return conditionImmediate(); - break; - default: - // invalid endconditions are always true to prevent freezes - return true; - break; - } +static uint8_t pathConditionCheck() +{ + // i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's + switch (pathAction.EndCondition) { + case PATHACTION_ENDCONDITION_NONE: + return conditionNone(); + + break; + case PATHACTION_ENDCONDITION_TIMEOUT: + return conditionTimeOut(); + + break; + case PATHACTION_ENDCONDITION_DISTANCETOTARGET: + return conditionDistanceToTarget(); + + break; + case PATHACTION_ENDCONDITION_LEGREMAINING: + return conditionLegRemaining(); + + break; + case PATHACTION_ENDCONDITION_BELOWERROR: + return conditionBelowError(); + + break; + case PATHACTION_ENDCONDITION_ABOVEALTITUDE: + return conditionAboveAltitude(); + + break; + case PATHACTION_ENDCONDITION_ABOVESPEED: + return conditionAboveSpeed(); + + break; + case PATHACTION_ENDCONDITION_POINTINGTOWARDSNEXT: + return conditionPointingTowardsNext(); + + break; + case PATHACTION_ENDCONDITION_PYTHONSCRIPT: + return conditionPythonScript(); + + break; + case PATHACTION_ENDCONDITION_IMMEDIATE: + return conditionImmediate(); + + break; + default: + // invalid endconditions are always true to prevent freezes + return true; + + break; + } } /* the None condition is always false */ -static uint8_t conditionNone() { - return false; +static uint8_t conditionNone() +{ + return false; } /** * the Timeout condition measures time this waypoint is active * Parameter 0: timeout in seconds */ -static uint8_t conditionTimeOut() { - static uint16_t toWaypoint; - static uint32_t toStarttime; +static uint8_t conditionTimeOut() +{ + static uint16_t toWaypoint; + static uint32_t toStarttime; - // reset timer if waypoint changed - if (waypointActive.Index!=toWaypoint) { - toWaypoint = waypointActive.Index; - toStarttime = PIOS_DELAY_GetRaw(); - } - if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6f * pathAction.ConditionParameters[0]) { - // make sure we reinitialize even if the same waypoint comes twice - toWaypoint = 0xFFFF; - return true; - } - return false; + // reset timer if waypoint changed + if (waypointActive.Index != toWaypoint) { + toWaypoint = waypointActive.Index; + toStarttime = PIOS_DELAY_GetRaw(); + } + if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6f * pathAction.ConditionParameters[0]) { + // make sure we reinitialize even if the same waypoint comes twice + toWaypoint = 0xFFFF; + return true; + } + return false; } /** @@ -347,110 +362,116 @@ static uint8_t conditionTimeOut() { * Parameter 0: distance in meters * Parameter 1: flag: 0=2d 1=3d */ -static uint8_t conditionDistanceToTarget() { - float distance; - PositionActualData positionActual; +static uint8_t conditionDistanceToTarget() +{ + float distance; + PositionActualData positionActual; - PositionActualGet(&positionActual); - if (pathAction.ConditionParameters[1]>0.5f) { - distance=sqrtf(powf( waypoint.Position[0]-positionActual.North ,2) - +powf( waypoint.Position[1]-positionActual.East ,2) - +powf( waypoint.Position[1]-positionActual.Down ,2)); - } else { - distance=sqrtf(powf( waypoint.Position[0]-positionActual.North ,2) - +powf( waypoint.Position[1]-positionActual.East ,2)); - } + PositionActualGet(&positionActual); + if (pathAction.ConditionParameters[1] > 0.5f) { + distance = sqrtf(powf(waypoint.Position[0] - positionActual.North, 2) + + powf(waypoint.Position[1] - positionActual.East, 2) + + powf(waypoint.Position[1] - positionActual.Down, 2)); + } else { + distance = sqrtf(powf(waypoint.Position[0] - positionActual.North, 2) + + powf(waypoint.Position[1] - positionActual.East, 2)); + } - if (distance<=pathAction.ConditionParameters[0]) { - return true; - } - return false; + if (distance <= pathAction.ConditionParameters[0]) { + return true; + } + return false; } /** - * the LegRemaining measures how far the pathfollower got on a linear path segment + * the LegRemaining measures how far the pathfollower got on a linear path segment * returns true if closer to destination (path more complete) * Parameter 0: relative distance (0= complete, 1= just starting) */ -static uint8_t conditionLegRemaining() { +static uint8_t conditionLegRemaining() +{ + PathDesiredData pathDesired; + PositionActualData positionActual; - PathDesiredData pathDesired; - PositionActualData positionActual; - PathDesiredGet(&pathDesired); - PositionActualGet(&positionActual); + PathDesiredGet(&pathDesired); + PositionActualGet(&positionActual); - float cur[3] = {positionActual.North, positionActual.East, positionActual.Down}; - struct path_status progress; + float cur[3] = { positionActual.North, positionActual.East, positionActual.Down }; + struct path_status progress; - path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); - if ( progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0] ) { - return true; - } - return false; + path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); + if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) { + return true; + } + return false; } /** - * the BelowError measures the error on a path segment + * the BelowError measures the error on a path segment * returns true if error is below margin * Parameter 0: error margin (in m) */ -static uint8_t conditionBelowError() { +static uint8_t conditionBelowError() +{ + PathDesiredData pathDesired; + PositionActualData positionActual; - PathDesiredData pathDesired; - PositionActualData positionActual; - PathDesiredGet(&pathDesired); - PositionActualGet(&positionActual); + PathDesiredGet(&pathDesired); + PositionActualGet(&positionActual); - float cur[3] = {positionActual.North, positionActual.East, positionActual.Down}; - struct path_status progress; + float cur[3] = { positionActual.North, positionActual.East, positionActual.Down }; + struct path_status progress; - path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); - if ( progress.error <= pathAction.ConditionParameters[0] ) { - return true; - } - return false; + path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); + if (progress.error <= pathAction.ConditionParameters[0]) { + return true; + } + return false; } /** - * the AboveAltitude measures the flight altitude relative to home position + * the AboveAltitude measures the flight altitude relative to home position * returns true if above critical altitude * WARNING! Altitudes are always negative (down coordinate) * Parameter 0: altitude in meters (negative!) */ -static uint8_t conditionAboveAltitude() { - PositionActualData positionActual; - PositionActualGet(&positionActual); - - if (positionActual.Down <= pathAction.ConditionParameters[0]) { - return true; - } - return false; +static uint8_t conditionAboveAltitude() +{ + PositionActualData positionActual; + + PositionActualGet(&positionActual); + + if (positionActual.Down <= pathAction.ConditionParameters[0]) { + return true; + } + return false; } /** - * the AboveSpeed measures the movement speed (3d) + * the AboveSpeed measures the movement speed (3d) * returns true if above critical speed * Parameter 0: speed in m/s * Parameter 1: flag: 0=groundspeed 1=airspeed */ -static uint8_t conditionAboveSpeed() { +static uint8_t conditionAboveSpeed() +{ + VelocityActualData velocityActual; - VelocityActualData velocityActual; - VelocityActualGet(&velocityActual); - float velocity = sqrtf( velocityActual.North*velocityActual.North + velocityActual.East*velocityActual.East + velocityActual.Down*velocityActual.Down ); + VelocityActualGet(&velocityActual); + float velocity = sqrtf(velocityActual.North * velocityActual.North + velocityActual.East * velocityActual.East + velocityActual.Down * velocityActual.Down); - // use airspeed if requested and available - if (pathAction.ConditionParameters[1]>0.5f) { - AirspeedActualData airspeed; - AirspeedActualGet (&airspeed); - velocity = airspeed.CalibratedAirspeed; - } + // use airspeed if requested and available + if (pathAction.ConditionParameters[1] > 0.5f) { + AirspeedActualData airspeed; + AirspeedActualGet(&airspeed); + velocity = airspeed.CalibratedAirspeed; + } - if ( velocity >= pathAction.ConditionParameters[0]) { - return true; - } - return false; + if (velocity >= pathAction.ConditionParameters[0]) { + return true; + } + return false; } @@ -461,29 +482,32 @@ static uint8_t conditionAboveSpeed() { * returns true if within a certain angular margin * Parameter 0: degrees variation allowed */ -static uint8_t conditionPointingTowardsNext() { - uint16_t nextWaypointId = waypointActive.Index+1; - if (nextWaypointId>=UAVObjGetNumInstances(WaypointHandle())) { - nextWaypointId = 0; - } - WaypointData nextWaypoint; - WaypointInstGet(nextWaypointId,&nextWaypoint); +static uint8_t conditionPointingTowardsNext() +{ + uint16_t nextWaypointId = waypointActive.Index + 1; - float angle1 = atan2f((nextWaypoint.Position[0]-waypoint.Position[0]),(nextWaypoint.Position[1]-waypoint.Position[1])); + if (nextWaypointId >= UAVObjGetNumInstances(WaypointHandle())) { + nextWaypointId = 0; + } + WaypointData nextWaypoint; + WaypointInstGet(nextWaypointId, &nextWaypoint); - VelocityActualData velocity; - VelocityActualGet (&velocity); - float angle2 = atan2f(velocity.North,velocity.East); + float angle1 = atan2f((nextWaypoint.Position[0] - waypoint.Position[0]), (nextWaypoint.Position[1] - waypoint.Position[1])); - // calculate the absolute angular difference - angle1 = fabsf(RAD2DEG(angle1 - angle2)); - while (angle1>360) angle1-=360; + VelocityActualData velocity; + VelocityActualGet(&velocity); + float angle2 = atan2f(velocity.North, velocity.East); - if (angle1 <= pathAction.ConditionParameters[0]) { - return true; - } - return false; + // calculate the absolute angular difference + angle1 = fabsf(RAD2DEG(angle1 - angle2)); + while (angle1 > 360) { + angle1 -= 360; + } + if (angle1 <= pathAction.ConditionParameters[0]) { + return true; + } + return false; } /** @@ -493,13 +517,15 @@ static uint8_t conditionPointingTowardsNext() { * returns always true until implemented * Parameter 0-3: defined by user script */ -static uint8_t conditionPythonScript() { - return true; +static uint8_t conditionPythonScript() +{ + return true; } /* the immediate condition is always true */ -static uint8_t conditionImmediate() { - return true; +static uint8_t conditionImmediate() +{ + return true; } /** diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index 6fa47a93f..56a4e6ea8 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -1,17 +1,17 @@ /** -****************************************************************************** -* @addtogroup OpenPilotModules OpenPilot Modules -* @{ -* @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module -* @brief Bridge Com and Radio ports -* @{ -* -* @file RadioComBridge.c -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief Bridges selected Com Port to the COM VCP emulated serial port -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module + * @brief Bridge Com and Radio ports + * @{ + * + * @file RadioComBridge.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Bridges selected Com Port to the COM VCP emulated serial port + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -56,19 +56,18 @@ void PIOS_InitPPMFlexiPort(bool input); // Private constants #define STACK_SIZE_BYTES 150 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define MAX_RETRIES 2 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define MAX_RETRIES 2 #define RETRY_TIMEOUT_MS 20 #define EVENT_QUEUE_SIZE 10 -#define MAX_PORT_DELAY 200 -#define EV_SEND_ACK 0x20 -#define EV_SEND_NACK 0x30 +#define MAX_PORT_DELAY 200 +#define EV_SEND_ACK 0x20 +#define EV_SEND_NACK 0x30 // **************** // Private types typedef struct { - // The task handles. xTaskHandle telemetryTxTaskHandle; xTaskHandle telemetryRxTaskHandle; @@ -84,10 +83,10 @@ typedef struct { xQueueHandle uavtalkEventQueue; // Error statistics. - uint32_t comTxErrors; - uint32_t comTxRetries; - uint32_t UAVTalkErrors; - uint32_t droppedPackets; + uint32_t comTxErrors; + uint32_t comTxRetries; + uint32_t UAVTalkErrors; + uint32_t droppedPackets; // Should we parse UAVTalk? bool parseUAVTalk; @@ -97,7 +96,6 @@ typedef struct { // The current configured uart speed OPLinkSettingsComSpeedOptions comSpeed; - } RadioComBridgeData; // **************** @@ -112,8 +110,8 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length); static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte); static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type); static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, - uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); static void updateSettings(); // **************** @@ -128,21 +126,19 @@ static RadioComBridgeData *data; */ static int32_t RadioComBridgeStart(void) { - if(data) { - + if (data) { // Configure the com port configuration callback PIOS_RFM22B_SetComConfigCallback(pios_rfm22b_id, &configureComCallback); - // Get the settings. - OPLinkSettingsData oplinkSettings; - OPLinkSettingsGet(&oplinkSettings); + // Get the settings. + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); // Set the baudrates, etc. bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); if (is_coordinator) { - - // Set the frequency range. - PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency, oplinkSettings.ChannelSpacing); + // Set the frequency range. + PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency, oplinkSettings.ChannelSpacing); // Reinitilize the modem. PIOS_RFM22B_Reinit(pios_rfm22b_id); @@ -184,8 +180,8 @@ static int32_t RadioComBridgeStart(void) break; } - // Set the initial frequency. - PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, oplinkSettings.InitFrequency); + // Set the initial frequency. + PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, oplinkSettings.InitFrequency); // Start the primary tasks for receiving/sending UAVTalk packets from the GCS. xTaskCreate(telemetryTxTask, (signed char *)"telemetryTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle)); @@ -213,7 +209,6 @@ static int32_t RadioComBridgeStart(void) */ static int32_t RadioComBridgeInitialize(void) { - // allocate and initialize the static data storage only if module is enabled data = (RadioComBridgeData *)pvPortMalloc(sizeof(RadioComBridgeData)); if (!data) { @@ -225,8 +220,8 @@ static int32_t RadioComBridgeInitialize(void) ObjectPersistenceInitialize(); // Initialise UAVTalk - data->outUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); - data->inUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); + data->outUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); + data->inUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); // Initialize the queues. data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); @@ -239,11 +234,11 @@ static int32_t RadioComBridgeInitialize(void) #endif // Initialize the statistics. - data->comTxErrors = 0; - data->comTxRetries = 0; + data->comTxErrors = 0; + data->comTxRetries = 0; data->UAVTalkErrors = 0; - data->parseUAVTalk = true; - data->configured = false; + data->parseUAVTalk = true; + data->configured = false; data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; PIOS_COM_RADIO = PIOS_COM_RFM22B; @@ -270,31 +265,34 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) { // Send update (with retries) uint32_t retries = 0; - int32_t success = -1; + int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendObject(data->outUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; - if (!success) + if (!success) { ++retries; + } } data->comTxRetries += retries; - } else if(ev.event == EV_SEND_ACK) { + } else if (ev.event == EV_SEND_ACK) { // Send the ACK uint32_t retries = 0; - int32_t success = -1; + int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendAck(data->outUAVTalkCon, ev.obj, ev.instId) == 0; - if (!success) + if (!success) { ++retries; + } } data->comTxRetries += retries; - } else if(ev.event == EV_SEND_NACK) { + } else if (ev.event == EV_SEND_NACK) { // Send the NACK uint32_t retries = 0; - int32_t success = -1; + int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendNack(data->outUAVTalkCon, UAVObjGetID(ev.obj)) == 0; - if (!success) + if (!success) { ++retries; + } } data->comTxRetries += retries; } @@ -378,12 +376,13 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) inputPort = PIOS_COM_TELEM_USB_HID; } #endif /* PIOS_INCLUDE_USB */ - if(inputPort) { + if (inputPort) { uint8_t serial_data[1]; uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), MAX_PORT_DELAY); if (bytes_to_process > 0) { - for (uint8_t i = 0; i < bytes_to_process; i++) + for (uint8_t i = 0; i < bytes_to_process; i++) { ProcessInputStream(data->inUAVTalkCon, serial_data[i]); + } } } else { vTaskDelay(5); @@ -402,13 +401,14 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) { uint32_t outputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0; + #if defined(PIOS_INCLUDE_USB) // Determine output port (USB takes priority over telemetry port) if (PIOS_COM_TELEM_USB_HID && PIOS_COM_Available(PIOS_COM_TELEM_USB_HID)) { outputPort = PIOS_COM_TELEM_USB_HID; } #endif /* PIOS_INCLUDE_USB */ - if(outputPort) { + if (outputPort) { return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); } else { return -1; @@ -426,8 +426,9 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) static int32_t RadioSendHandler(uint8_t *buf, int32_t length) { uint32_t outputPort = PIOS_COM_RADIO; + // Don't send any data unless the radio port is available. - if(outputPort && PIOS_COM_Available(outputPort)) { + if (outputPort && PIOS_COM_Available(outputPort)) { return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); } else { // For some reason, if this function returns failure, it prevents saving settings. @@ -445,7 +446,7 @@ static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyt { // Keep reading until we receive a completed packet. UAVTalkRxState state = UAVTalkRelayInputStream(connectionHandle, rxbyte); - UAVTalkConnectionData *connection = (UAVTalkConnectionData*)(connectionHandle); + UAVTalkConnectionData *connection = (UAVTalkConnectionData *)(connectionHandle); UAVTalkInputProcessor *iproc = &(connection->iproc); if (state == UAVTALK_STATE_COMPLETE) { @@ -453,7 +454,7 @@ static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyt // We only generate GcsReceiver ojects, we don't consume them. if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID)) { // We treat the ObjectPersistence object differently - if(iproc->objId == OBJECTPERSISTENCE_OBJID) { + if (iproc->objId == OBJECTPERSISTENCE_OBJID) { // Unpack object, if the instance does not exist it will be created! UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); @@ -464,7 +465,7 @@ static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyt // Is this concerning or setting object? if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) { // Queue up the ACK. - queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK); + queueEvent(data->uavtalkEventQueue, (void *)iproc->obj, iproc->instId, EV_SEND_ACK); // Is this a save, load, or delete? bool success = true; @@ -525,22 +526,22 @@ static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyt break; case UAVTALK_TYPE_OBJ_REQ: // Queue up an object send request. - queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_UPDATE_REQ); + queueEvent(data->uavtalkEventQueue, (void *)iproc->obj, iproc->instId, EV_UPDATE_REQ); break; case UAVTALK_TYPE_OBJ_ACK: - if (UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer) == 0) + if (UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer) == 0) { // Queue up an ACK - queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK); + queueEvent(data->uavtalkEventQueue, (void *)iproc->obj, iproc->instId, EV_SEND_ACK); + } break; } } } - - } else if(state == UAVTALK_STATE_ERROR) { + } else if (state == UAVTALK_STATE_ERROR) { data->UAVTalkErrors++; // Send a NACK if required. - if((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK)) { + if ((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK)) { // Queue up a NACK queueEvent(data->uavtalkEventQueue, iproc->obj, iproc->instId, EV_SEND_NACK); } @@ -557,9 +558,10 @@ static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyt static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type) { UAVObjEvent ev; - ev.obj = (UAVObjHandle)obj; + + ev.obj = (UAVObjHandle)obj; ev.instId = instId; - ev.event = type; + ev.event = type; xQueueSend(queue, &ev, portMAX_DELAY); } @@ -570,56 +572,54 @@ static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEve * @param[in] com_speed The com port speed */ static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, - uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing) + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing) { - // Update the com baud rate data->comSpeed = com_speed; // Set the output main/flexi/vcp port and speed. bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); if (!is_coordinator) { - // Get the settings. OPLinkSettingsData oplinkSettings; OPLinkSettingsGet(&oplinkSettings); - switch (main_port) { - case OPLINKSETTINGS_REMOTEMAINPORT_DISABLED: + switch (main_port) { + case OPLINKSETTINGS_REMOTEMAINPORT_DISABLED: oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_DISABLED; break; - case OPLINKSETTINGS_REMOTEMAINPORT_SERIAL: + case OPLINKSETTINGS_REMOTEMAINPORT_SERIAL: oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_SERIAL; break; - case OPLINKSETTINGS_REMOTEMAINPORT_PPM: + case OPLINKSETTINGS_REMOTEMAINPORT_PPM: oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_PPM; break; - } + } - switch (flexi_port) { - case OPLINKSETTINGS_REMOTEFLEXIPORT_DISABLED: + switch (flexi_port) { + case OPLINKSETTINGS_REMOTEFLEXIPORT_DISABLED: oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_DISABLED; break; - case OPLINKSETTINGS_REMOTEFLEXIPORT_SERIAL: + case OPLINKSETTINGS_REMOTEFLEXIPORT_SERIAL: oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_SERIAL; break; - case OPLINKSETTINGS_REMOTEFLEXIPORT_PPM: + case OPLINKSETTINGS_REMOTEFLEXIPORT_PPM: oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_PPM; break; - } + } - switch (vcp_port) { - case OPLINKSETTINGS_REMOTEVCPPORT_DISABLED: + switch (vcp_port) { + case OPLINKSETTINGS_REMOTEVCPPORT_DISABLED: oplinkSettings.VCPPort = OPLINKSETTINGS_VCPPORT_DISABLED; break; - case OPLINKSETTINGS_REMOTEVCPPORT_SERIAL: + case OPLINKSETTINGS_REMOTEVCPPORT_SERIAL: oplinkSettings.VCPPort = OPLINKSETTINGS_VCPPORT_SERIAL; break; - } + } - // Set the frequency range. - PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, min_frequency, max_frequency, channel_spacing); + // Set the frequency range. + PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, min_frequency, max_frequency, channel_spacing); // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port). data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && @@ -641,6 +641,7 @@ static void updateSettings() { // Get the settings. OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); // We can only configure the hardware once. diff --git a/flight/modules/RadioComBridge/inc/radiocombridge.h b/flight/modules/RadioComBridge/inc/radiocombridge.h index cb9862b9a..f5b6ea509 100644 --- a/flight/modules/RadioComBridge/inc/radiocombridge.h +++ b/flight/modules/RadioComBridge/inc/radiocombridge.h @@ -1,10 +1,10 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module * @brief Bridge Com and Radio ports - * @{ + * @{ * * @file radiocombridge.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -34,6 +34,6 @@ #endif // RADIOCOMBRIDGE_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/Sensors/inc/sensors.h b/flight/modules/Sensors/inc/sensors.h index 16e24daab..6abdbdade 100644 --- a/flight/modules/Sensors/inc/sensors.h +++ b/flight/modules/Sensors/inc/sensors.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup Sensors Sensors Module - * @{ + * @{ * * @file attitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index feefbe1e2..ddfbf29c7 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -3,7 +3,7 @@ * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup Sensors - * @brief Acquires sensor data + * @brief Acquires sensor data * Specifically updates the the @ref Gyros, @ref Accels, and @ref Magnetometer objects * @{ * @@ -66,15 +66,15 @@ // Private constants #define STACK_SIZE_BYTES 1000 -#define TASK_PRIORITY (tskIDLE_PRIORITY+3) -#define SENSOR_PERIOD 2 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) +#define SENSOR_PERIOD 2 // Private types // Private functions static void SensorsTask(void *parameters); -static void settingsUpdatedCb(UAVObjEvent * objEv); +static void settingsUpdatedCb(UAVObjEvent *objEv); static void magOffsetEstimation(MagnetometerData *mag); // Private variables @@ -84,14 +84,16 @@ RevoCalibrationData cal; // These values are initialized by settings but can be updated by the attitude algorithm static bool bias_correct_gyro = true; -static float mag_bias[3] = {0,0,0}; -static float mag_scale[3] = {0,0,0}; -static float accel_bias[3] = {0,0,0}; -static float accel_scale[3] = {0,0,0}; -static float gyro_staticbias[3] = {0,0,0}; -static float gyro_scale[3] = {0,0,0}; +static float mag_bias[3] = { 0, 0, 0 }; +static float mag_scale[3] = { 0, 0, 0 }; +static float accel_bias[3] = { 0, 0, 0 }; +static float accel_scale[3] = { 0, 0, 0 }; +static float gyro_staticbias[3] = { 0, 0, 0 }; +static float gyro_scale[3] = { 0, 0, 0 }; -static float R[3][3] = {{0}}; +static float R[3][3] = { + { 0 } +}; static int8_t rotate = 0; /** @@ -109,20 +111,20 @@ static int8_t rotate = 0; */ int32_t SensorsInitialize(void) { - GyrosInitialize(); - GyrosBiasInitialize(); - AccelsInitialize(); - MagnetometerInitialize(); - MagBiasInitialize(); - RevoCalibrationInitialize(); - AttitudeSettingsInitialize(); + GyrosInitialize(); + GyrosBiasInitialize(); + AccelsInitialize(); + MagnetometerInitialize(); + MagBiasInitialize(); + RevoCalibrationInitialize(); + AttitudeSettingsInitialize(); - rotate = 0; + rotate = 0; - RevoCalibrationConnectCallback(&settingsUpdatedCb); - AttitudeSettingsConnectCallback(&settingsUpdatedCb); + RevoCalibrationConnectCallback(&settingsUpdatedCb); + AttitudeSettingsConnectCallback(&settingsUpdatedCb); - return 0; + return 0; } /** @@ -131,12 +133,12 @@ int32_t SensorsInitialize(void) */ int32_t SensorsStart(void) { - // Start main task - xTaskCreate(SensorsTask, (signed char *)"Sensors", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &sensorsTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS); + // Start main task + xTaskCreate(SensorsTask, (signed char *)"Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS); - return 0; + return 0; } MODULE_INITCALL(SensorsInitialize, SensorsStart) @@ -144,13 +146,13 @@ MODULE_INITCALL(SensorsInitialize, SensorsStart) int32_t accel_test; int32_t gyro_test; int32_t mag_test; -//int32_t pressure_test; +// int32_t pressure_test; /** * The sensor task. This polls the gyros at 500 Hz and pumps that data to * stabilization and to the attitude loop - * + * * This function has a lot of if/defs right now to allow these configurations: * 1. BMA180 accel and MPU6000 gyro * 2. MPU6000 gyro and accel @@ -160,423 +162,424 @@ int32_t mag_test; uint32_t sensor_dt_us; static void SensorsTask(__attribute__((unused)) void *parameters) { - portTickType lastSysTime; - uint32_t accel_samples = 0; - uint32_t gyro_samples = 0; - int32_t accel_accum[3] = {0, 0, 0}; - int32_t gyro_accum[3] = {0,0,0}; - float gyro_scaling = 0; - float accel_scaling = 0; - static int32_t timeval; + portTickType lastSysTime; + uint32_t accel_samples = 0; + uint32_t gyro_samples = 0; + int32_t accel_accum[3] = { 0, 0, 0 }; + int32_t gyro_accum[3] = { 0, 0, 0 }; + float gyro_scaling = 0; + float accel_scaling = 0; + static int32_t timeval; - AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); + AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); - UAVObjEvent ev; - settingsUpdatedCb(&ev); + UAVObjEvent ev; + settingsUpdatedCb(&ev); - const struct pios_board_info * bdinfo = &pios_board_info_blob; + const struct pios_board_info *bdinfo = &pios_board_info_blob; - switch(bdinfo->board_rev) { - case 0x01: + switch (bdinfo->board_rev) { + case 0x01: #if defined(PIOS_INCLUDE_L3GD20) - gyro_test = PIOS_L3GD20_Test(); + gyro_test = PIOS_L3GD20_Test(); #endif #if defined(PIOS_INCLUDE_BMA180) - accel_test = PIOS_BMA180_Test(); + accel_test = PIOS_BMA180_Test(); #endif - break; - case 0x02: + break; + case 0x02: #if defined(PIOS_INCLUDE_MPU6000) - gyro_test = PIOS_MPU6000_Test(); - accel_test = gyro_test; + gyro_test = PIOS_MPU6000_Test(); + accel_test = gyro_test; #endif - break; - default: - PIOS_DEBUG_Assert(0); - } + break; + default: + PIOS_DEBUG_Assert(0); + } #if defined(PIOS_INCLUDE_HMC5883) - mag_test = PIOS_HMC5883_Test(); + mag_test = PIOS_HMC5883_Test(); #else - mag_test = 0; + mag_test = 0; #endif - if(accel_test < 0 || gyro_test < 0 || mag_test < 0) { - AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); - while(1) { - PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); - vTaskDelay(10); - } - } - - // Main task loop - lastSysTime = xTaskGetTickCount(); - bool error = false; - uint32_t mag_update_time = PIOS_DELAY_GetRaw(); - while (1) { - // TODO: add timeouts to the sensor reads and set an error if the fail - sensor_dt_us = PIOS_DELAY_DiffuS(timeval); - timeval = PIOS_DELAY_GetRaw(); + if (accel_test < 0 || gyro_test < 0 || mag_test < 0) { + AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); + while (1) { + PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); + vTaskDelay(10); + } + } - if (error) { - PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); - lastSysTime = xTaskGetTickCount(); - vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS); - AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); - error = false; - } else { - AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); - } + // Main task loop + lastSysTime = xTaskGetTickCount(); + bool error = false; + uint32_t mag_update_time = PIOS_DELAY_GetRaw(); + while (1) { + // TODO: add timeouts to the sensor reads and set an error if the fail + sensor_dt_us = PIOS_DELAY_DiffuS(timeval); + timeval = PIOS_DELAY_GetRaw(); + + if (error) { + PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); + lastSysTime = xTaskGetTickCount(); + vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS); + AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); + error = false; + } else { + AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); + } - for (int i = 0; i < 3; i++) { - accel_accum[i] = 0; - gyro_accum[i] = 0; - } - accel_samples = 0; - gyro_samples = 0; + for (int i = 0; i < 3; i++) { + accel_accum[i] = 0; + gyro_accum[i] = 0; + } + accel_samples = 0; + gyro_samples = 0; - AccelsData accelsData; - GyrosData gyrosData; + AccelsData accelsData; + GyrosData gyrosData; - switch(bdinfo->board_rev) { - case 0x01: // L3GD20 + BMA180 board + switch (bdinfo->board_rev) { + case 0x01: // L3GD20 + BMA180 board #if defined(PIOS_INCLUDE_BMA180) - { - struct pios_bma180_data accel; - - int32_t read_good; - int32_t count; - - count = 0; - while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) - error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error; - if (error) { - // Unfortunately if the BMA180 ever misses getting read, then it will not - // trigger more interrupts. In this case we must force a read to kickstarts - // it. - struct pios_bma180_data data; - PIOS_BMA180_ReadAccels(&data); - continue; - } - while(read_good == 0) { - count++; - - accel_accum[1] += accel.x; - accel_accum[0] += accel.y; - accel_accum[2] -= accel.z; - - read_good = PIOS_BMA180_ReadFifo(&accel); - } - accel_samples = count; - accel_scaling = PIOS_BMA180_GetScale(); - - // Get temp from last reading - accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f; - } -#endif + { + struct pios_bma180_data accel; + + int32_t read_good; + int32_t count; + + count = 0; + while ((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) { + error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error; + } + if (error) { + // Unfortunately if the BMA180 ever misses getting read, then it will not + // trigger more interrupts. In this case we must force a read to kickstarts + // it. + struct pios_bma180_data data; + PIOS_BMA180_ReadAccels(&data); + continue; + } + while (read_good == 0) { + count++; + + accel_accum[1] += accel.x; + accel_accum[0] += accel.y; + accel_accum[2] -= accel.z; + + read_good = PIOS_BMA180_ReadFifo(&accel); + } + accel_samples = count; + accel_scaling = PIOS_BMA180_GetScale(); + + // Get temp from last reading + accelsData.temperature = 25.0f + ((float)accel.temperature - 2.0f) / 2.0f; + } +#endif /* if defined(PIOS_INCLUDE_BMA180) */ #if defined(PIOS_INCLUDE_L3GD20) - { - struct pios_l3gd20_data gyro; - gyro_samples = 0; - xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue(); - - if(xQueueReceive(gyro_queue, (void *) &gyro, 4) == errQUEUE_EMPTY) { - error = true; - continue; - } - - gyro_samples = 1; - gyro_accum[1] += gyro.gyro_x; - gyro_accum[0] += gyro.gyro_y; - gyro_accum[2] -= gyro.gyro_z; - - gyro_scaling = PIOS_L3GD20_GetScale(); + { + struct pios_l3gd20_data gyro; + gyro_samples = 0; + xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue(); - // Get temp from last reading - gyrosData.temperature = gyro.temperature; - } -#endif - break; - case 0x02: // MPU6000 board - case 0x03: // MPU6000 board + if (xQueueReceive(gyro_queue, (void *)&gyro, 4) == errQUEUE_EMPTY) { + error = true; + continue; + } + + gyro_samples = 1; + gyro_accum[1] += gyro.gyro_x; + gyro_accum[0] += gyro.gyro_y; + gyro_accum[2] -= gyro.gyro_z; + + gyro_scaling = PIOS_L3GD20_GetScale(); + + // Get temp from last reading + gyrosData.temperature = gyro.temperature; + } +#endif /* if defined(PIOS_INCLUDE_L3GD20) */ + break; + case 0x02: // MPU6000 board + case 0x03: // MPU6000 board #if defined(PIOS_INCLUDE_MPU6000) - { - struct pios_mpu6000_data mpu6000_data; - xQueueHandle queue = PIOS_MPU6000_GetQueue(); + { + struct pios_mpu6000_data mpu6000_data; + xQueueHandle queue = PIOS_MPU6000_GetQueue(); - while(xQueueReceive(queue, (void *) &mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY) - { - gyro_accum[0] += mpu6000_data.gyro_x; - gyro_accum[1] += mpu6000_data.gyro_y; - gyro_accum[2] += mpu6000_data.gyro_z; + while (xQueueReceive(queue, (void *)&mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY) { + gyro_accum[0] += mpu6000_data.gyro_x; + gyro_accum[1] += mpu6000_data.gyro_y; + gyro_accum[2] += mpu6000_data.gyro_z; - accel_accum[0] += mpu6000_data.accel_x; - accel_accum[1] += mpu6000_data.accel_y; - accel_accum[2] += mpu6000_data.accel_z; + accel_accum[0] += mpu6000_data.accel_x; + accel_accum[1] += mpu6000_data.accel_y; + accel_accum[2] += mpu6000_data.accel_z; - gyro_samples ++; - accel_samples ++; - } + gyro_samples++; + accel_samples++; + } - if (gyro_samples == 0) { - PIOS_MPU6000_ReadGyros(&mpu6000_data); - error = true; - continue; - } + if (gyro_samples == 0) { + PIOS_MPU6000_ReadGyros(&mpu6000_data); + error = true; + continue; + } - gyro_scaling = PIOS_MPU6000_GetScale(); - accel_scaling = PIOS_MPU6000_GetAccelScale(); + gyro_scaling = PIOS_MPU6000_GetScale(); + accel_scaling = PIOS_MPU6000_GetAccelScale(); - gyrosData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; - accelsData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; - } + gyrosData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; + accelsData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; + } #endif /* PIOS_INCLUDE_MPU6000 */ - break; - default: - PIOS_DEBUG_Assert(0); - } + break; + default: + PIOS_DEBUG_Assert(0); + } - // Scale the accels - float accels[3] = {(float) accel_accum[0] / accel_samples, - (float) accel_accum[1] / accel_samples, - (float) accel_accum[2] / accel_samples}; - float accels_out[3] = {accels[0] * accel_scaling * accel_scale[0] - accel_bias[0], - accels[1] * accel_scaling * accel_scale[1] - accel_bias[1], - accels[2] * accel_scaling * accel_scale[2] - accel_bias[2]}; - if (rotate) { - rot_mult(R, accels_out, accels); - accelsData.x = accels[0]; - accelsData.y = accels[1]; - accelsData.z = accels[2]; - } else { - accelsData.x = accels_out[0]; - accelsData.y = accels_out[1]; - accelsData.z = accels_out[2]; - } - AccelsSet(&accelsData); + // Scale the accels + float accels[3] = { (float)accel_accum[0] / accel_samples, + (float)accel_accum[1] / accel_samples, + (float)accel_accum[2] / accel_samples }; + float accels_out[3] = { accels[0] * accel_scaling * accel_scale[0] - accel_bias[0], + accels[1] * accel_scaling * accel_scale[1] - accel_bias[1], + accels[2] * accel_scaling * accel_scale[2] - accel_bias[2] }; + if (rotate) { + rot_mult(R, accels_out, accels); + accelsData.x = accels[0]; + accelsData.y = accels[1]; + accelsData.z = accels[2]; + } else { + accelsData.x = accels_out[0]; + accelsData.y = accels_out[1]; + accelsData.z = accels_out[2]; + } + AccelsSet(&accelsData); - // Scale the gyros - float gyros[3] = {(float) gyro_accum[0] / gyro_samples, - (float) gyro_accum[1] / gyro_samples, - (float) gyro_accum[2] / gyro_samples}; - float gyros_out[3] = {gyros[0] * gyro_scaling * gyro_scale[0] - gyro_staticbias[0], - gyros[1] * gyro_scaling * gyro_scale[1] - gyro_staticbias[1], - gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2]}; - if (rotate) { - rot_mult(R, gyros_out, gyros); - gyrosData.x = gyros[0]; - gyrosData.y = gyros[1]; - gyrosData.z = gyros[2]; - } else { - gyrosData.x = gyros_out[0]; - gyrosData.y = gyros_out[1]; - gyrosData.z = gyros_out[2]; - } - - if (bias_correct_gyro) { - // Apply bias correction to the gyros from the state estimator - GyrosBiasData gyrosBias; - GyrosBiasGet(&gyrosBias); - gyrosData.x -= gyrosBias.x; - gyrosData.y -= gyrosBias.y; - gyrosData.z -= gyrosBias.z; - } - GyrosSet(&gyrosData); - - // Because most crafts wont get enough information from gravity to zero yaw gyro, we try - // and make it average zero (weakly) + // Scale the gyros + float gyros[3] = { (float)gyro_accum[0] / gyro_samples, + (float)gyro_accum[1] / gyro_samples, + (float)gyro_accum[2] / gyro_samples }; + float gyros_out[3] = { gyros[0] * gyro_scaling * gyro_scale[0] - gyro_staticbias[0], + gyros[1] * gyro_scaling * gyro_scale[1] - gyro_staticbias[1], + gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2] }; + if (rotate) { + rot_mult(R, gyros_out, gyros); + gyrosData.x = gyros[0]; + gyrosData.y = gyros[1]; + gyrosData.z = gyros[2]; + } else { + gyrosData.x = gyros_out[0]; + gyrosData.y = gyros_out[1]; + gyrosData.z = gyros_out[2]; + } + + if (bias_correct_gyro) { + // Apply bias correction to the gyros from the state estimator + GyrosBiasData gyrosBias; + GyrosBiasGet(&gyrosBias); + gyrosData.x -= gyrosBias.x; + gyrosData.y -= gyrosBias.y; + gyrosData.z -= gyrosBias.z; + } + GyrosSet(&gyrosData); + + // Because most crafts wont get enough information from gravity to zero yaw gyro, we try + // and make it average zero (weakly) #if defined(PIOS_INCLUDE_HMC5883) - MagnetometerData mag; - if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { - int16_t values[3]; - PIOS_HMC5883_ReadMag(values); - float mags[3] = {(float) values[1] * mag_scale[0] - mag_bias[0], - (float) values[0] * mag_scale[1] - mag_bias[1], - -(float) values[2] * mag_scale[2] - mag_bias[2]}; - if (rotate) { - float mag_out[3]; - rot_mult(R, mags, mag_out); - mag.x = mag_out[0]; - mag.y = mag_out[1]; - mag.z = mag_out[2]; - } else { - mag.x = mags[0]; - mag.y = mags[1]; - mag.z = mags[2]; - } - - // Correct for mag bias and update if the rate is non zero - if(cal.MagBiasNullingRate > 0) - magOffsetEstimation(&mag); + MagnetometerData mag; + if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { + int16_t values[3]; + PIOS_HMC5883_ReadMag(values); + float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0], + (float)values[0] * mag_scale[1] - mag_bias[1], + -(float)values[2] * mag_scale[2] - mag_bias[2] }; + if (rotate) { + float mag_out[3]; + rot_mult(R, mags, mag_out); + mag.x = mag_out[0]; + mag.y = mag_out[1]; + mag.z = mag_out[2]; + } else { + mag.x = mags[0]; + mag.y = mags[1]; + mag.z = mags[2]; + } - MagnetometerSet(&mag); - mag_update_time = PIOS_DELAY_GetRaw(); - } -#endif + // Correct for mag bias and update if the rate is non zero + if (cal.MagBiasNullingRate > 0) { + magOffsetEstimation(&mag); + } - PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); + MagnetometerSet(&mag); + mag_update_time = PIOS_DELAY_GetRaw(); + } +#endif /* if defined(PIOS_INCLUDE_HMC5883) */ - lastSysTime = xTaskGetTickCount(); - } + PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); + + lastSysTime = xTaskGetTickCount(); + } } /** * Perform an update of the @ref MagBias based on - * Magnetometer Offset Cancellation: Theory and Implementation, + * Magnetometer Offset Cancellation: Theory and Implementation, * revisited William Premerlani, October 14, 2011 */ static void magOffsetEstimation(MagnetometerData *mag) { #if 0 - // Constants, to possibly go into a UAVO - static const float MIN_NORM_DIFFERENCE = 50; + // Constants, to possibly go into a UAVO + static const float MIN_NORM_DIFFERENCE = 50; - static float B2[3] = {0, 0, 0}; + static float B2[3] = { 0, 0, 0 }; - MagBiasData magBias; - MagBiasGet(&magBias); + MagBiasData magBias; + MagBiasGet(&magBias); - // Remove the current estimate of the bias - mag->x -= magBias.x; - mag->y -= magBias.y; - mag->z -= magBias.z; + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; - // First call - if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { - B2[0] = mag->x; - B2[1] = mag->y; - B2[2] = mag->z; - return; - } + // First call + if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { + B2[0] = mag->x; + B2[1] = mag->y; + B2[2] = mag->z; + return; + } - float B1[3] = {mag->x, mag->y, mag->z}; - float norm_diff = sqrtf(powf(B2[0] - B1[0],2) + powf(B2[1] - B1[1],2) + powf(B2[2] - B1[2],2)); - if (norm_diff > MIN_NORM_DIFFERENCE) { - float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]); - float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]); - float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; - float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale}; + float B1[3] = { mag->x, mag->y, mag->z }; + float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2)); + if (norm_diff > MIN_NORM_DIFFERENCE) { + float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]); + float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]); + float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; + float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale }; - magBias.x += b_error[0]; - magBias.y += b_error[1]; - magBias.z += b_error[2]; + magBias.x += b_error[0]; + magBias.y += b_error[1]; + magBias.z += b_error[2]; - MagBiasSet(&magBias); + MagBiasSet(&magBias); - // Store this value to compare against next update - B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; - } -#else - MagBiasData magBias; - MagBiasGet(&magBias); - - // Remove the current estimate of the bias - mag->x -= magBias.x; - mag->y -= magBias.y; - mag->z -= magBias.z; - - HomeLocationData homeLocation; - HomeLocationGet(&homeLocation); - - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - - const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]); - const float Rz = homeLocation.Be[2]; - - const float rate = cal.MagBiasNullingRate; - float Rot[3][3]; - float B_e[3]; - float xy[2]; - float delta[3]; - - // Get the rotation matrix - Quaternion2R(&attitude.q1, R); - - // Rotate the mag into the NED frame - B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z; - B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z; - B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z; - - float cy = cosf(DEG2RAD(attitude.Yaw)); - float sy = sinf(DEG2RAD(attitude.Yaw)); - - xy[0] = cy * B_e[0] + sy * B_e[1]; - xy[1] = -sy * B_e[0] + cy * B_e[1]; - - float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]); - - delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); - delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); - delta[2] = -rate * (Rz - B_e[2]); - - if (!isnan(delta[0]) && !isinf(delta[0]) && - !isnan(delta[1]) && !isinf(delta[1]) && - !isnan(delta[2]) && !isinf(delta[2])) { - magBias.x += delta[0]; - magBias.y += delta[1]; - magBias.z += delta[2]; - MagBiasSet(&magBias); - } -#endif + // Store this value to compare against next update + B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; + } +#else /* if 0 */ + MagBiasData magBias; + MagBiasGet(&magBias); + + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; + + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); + + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + + const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]); + const float Rz = homeLocation.Be[2]; + + const float rate = cal.MagBiasNullingRate; + float Rot[3][3]; + float B_e[3]; + float xy[2]; + float delta[3]; + + // Get the rotation matrix + Quaternion2R(&attitude.q1, R); + + // Rotate the mag into the NED frame + B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z; + B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z; + B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z; + + float cy = cosf(DEG2RAD(attitude.Yaw)); + float sy = sinf(DEG2RAD(attitude.Yaw)); + + xy[0] = cy * B_e[0] + sy * B_e[1]; + xy[1] = -sy * B_e[0] + cy * B_e[1]; + + float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]); + + delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); + delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); + delta[2] = -rate * (Rz - B_e[2]); + + if (!isnan(delta[0]) && !isinf(delta[0]) && + !isnan(delta[1]) && !isinf(delta[1]) && + !isnan(delta[2]) && !isinf(delta[2])) { + magBias.x += delta[0]; + magBias.y += delta[1]; + magBias.z += delta[2]; + MagBiasSet(&magBias); + } +#endif /* if 0 */ } /** * Locally cache some variables from the AtttitudeSettings object */ -static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent * objEv) { - RevoCalibrationGet(&cal); - - mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X]; - mag_bias[1] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Y]; - mag_bias[2] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Z]; - mag_scale[0] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_X]; - mag_scale[1] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Y]; - mag_scale[2] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Z]; - accel_bias[0] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_X]; - accel_bias[1] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Y]; - accel_bias[2] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Z]; - accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X]; - accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y]; - accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z]; - gyro_staticbias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X]; - gyro_staticbias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y]; - gyro_staticbias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z]; - gyro_scale[0] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_X]; - gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y]; - gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z]; - - // Zero out any adaptive tracking - MagBiasData magBias; - MagBiasGet(&magBias); - magBias.x = 0; - magBias.y = 0; - magBias.z = 0; - MagBiasSet(&magBias); - +static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) +{ + RevoCalibrationGet(&cal); - AttitudeSettingsData attitudeSettings; - AttitudeSettingsGet(&attitudeSettings); - bias_correct_gyro = (cal.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE); + mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X]; + mag_bias[1] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Y]; + mag_bias[2] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Z]; + mag_scale[0] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_X]; + mag_scale[1] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Y]; + mag_scale[2] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Z]; + accel_bias[0] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_X]; + accel_bias[1] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Y]; + accel_bias[2] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Z]; + accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X]; + accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y]; + accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z]; + gyro_staticbias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X]; + gyro_staticbias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y]; + gyro_staticbias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z]; + gyro_scale[0] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_X]; + gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y]; + gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z]; - // Indicates not to expend cycles on rotation - if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && - attitudeSettings.BoardRotation[2] == 0) { - rotate = 0; - } else { - float rotationQuat[4]; - const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL], - attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH], - attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]}; - RPY2Quaternion(rpy, rotationQuat); - Quaternion2R(rotationQuat, R); - rotate = 1; - } + // Zero out any adaptive tracking + MagBiasData magBias; + MagBiasGet(&magBias); + magBias.x = 0; + magBias.y = 0; + magBias.z = 0; + MagBiasSet(&magBias); + + AttitudeSettingsData attitudeSettings; + AttitudeSettingsGet(&attitudeSettings); + bias_correct_gyro = (cal.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE); + + // Indicates not to expend cycles on rotation + if (attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && + attitudeSettings.BoardRotation[2] == 0) { + rotate = 0; + } else { + float rotationQuat[4]; + const float rpy[3] = { attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL], + attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH], + attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] }; + RPY2Quaternion(rpy, rotationQuat); + Quaternion2R(rotationQuat, R); + rotate = 1; + } } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/Sensors/simulated/inc/sensors.h b/flight/modules/Sensors/simulated/inc/sensors.h index 16e24daab..6abdbdade 100644 --- a/flight/modules/Sensors/simulated/inc/sensors.h +++ b/flight/modules/Sensors/simulated/inc/sensors.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup Sensors Sensors Module - * @{ + * @{ * * @file attitude.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. diff --git a/flight/modules/Sensors/simulated/sensors.c b/flight/modules/Sensors/simulated/sensors.c index 929051b67..6f973f2f5 100644 --- a/flight/modules/Sensors/simulated/sensors.c +++ b/flight/modules/Sensors/simulated/sensors.c @@ -3,7 +3,7 @@ * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup Sensors - * @brief Acquires sensor data + * @brief Acquires sensor data * Specifically updates the the @ref Gyros, @ref Accels, and @ref Magnetometer objects * @{ * @@ -73,10 +73,10 @@ // Private constants #define STACK_SIZE_BYTES 1540 -#define TASK_PRIORITY (tskIDLE_PRIORITY+3) -#define SENSOR_PERIOD 2 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) +#define SENSOR_PERIOD 2 -#define F_PI 3.14159265358979323846f +#define F_PI 3.14159265358979323846f #define PI_MOD(x) (fmod(x + F_PI, F_PI * 2) - F_PI) // Private types @@ -96,7 +96,7 @@ static float accel_bias[3]; static float rand_gauss(); -enum sensor_sim_type {CONSTANT, MODEL_AGNOSTIC, MODEL_QUADCOPTER, MODEL_AIRPLANE} sensor_sim_type; +enum sensor_sim_type { CONSTANT, MODEL_AGNOSTIC, MODEL_QUADCOPTER, MODEL_AIRPLANE } sensor_sim_type; #define GRAV 9.81 /** @@ -105,38 +105,37 @@ enum sensor_sim_type {CONSTANT, MODEL_AGNOSTIC, MODEL_QUADCOPTER, MODEL_AIRPLANE */ int32_t SensorsInitialize(void) { + accel_bias[0] = rand_gauss() / 10; + accel_bias[1] = rand_gauss() / 10; + accel_bias[2] = rand_gauss() / 10; - accel_bias[0] = rand_gauss() / 10; - accel_bias[1] = rand_gauss() / 10; - accel_bias[2] = rand_gauss() / 10; + AccelsInitialize(); + AttitudeSimulatedInitialize(); + BaroAltitudeInitialize(); + AirspeedSensorInitialize(); + GyrosInitialize(); + GyrosBiasInitialize(); + GPSPositionInitialize(); + GPSVelocityInitialize(); + MagnetometerInitialize(); + MagBiasInitialize(); + RevoCalibrationInitialize(); - AccelsInitialize(); - AttitudeSimulatedInitialize(); - BaroAltitudeInitialize(); - AirspeedSensorInitialize(); - GyrosInitialize(); - GyrosBiasInitialize(); - GPSPositionInitialize(); - GPSVelocityInitialize(); - MagnetometerInitialize(); - MagBiasInitialize(); - RevoCalibrationInitialize(); - - return 0; + return 0; } /** * Start the task. Expects all objects to be initialized by this point. - *pick \returns 0 on success or -1 if initialisation failed + * pick \returns 0 on success or -1 if initialisation failed */ int32_t SensorsStart(void) { - // Start main task - xTaskCreate(SensorsTask, (signed char *)"Sensors", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &sensorsTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS); + // Start main task + xTaskCreate(SensorsTask, (signed char *)"Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS); - return 0; + return 0; } MODULE_INITCALL(SensorsInitialize, SensorsStart) @@ -147,414 +146,419 @@ MODULE_INITCALL(SensorsInitialize, SensorsStart) int sensors_count; static void SensorsTask(__attribute__((unused)) void *parameters) { - portTickType lastSysTime; + portTickType lastSysTime; - AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); - -// HomeLocationData homeLocation; -// HomeLocationGet(&homeLocation); -// homeLocation.Latitude = 0; -// homeLocation.Longitude = 0; -// homeLocation.Altitude = 0; -// homeLocation.Be[0] = 26000; -// homeLocation.Be[1] = 400; -// homeLocation.Be[2] = 40000; -// homeLocation.Set = HOMELOCATION_SET_TRUE; -// HomeLocationSet(&homeLocation); + AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); + +// HomeLocationData homeLocation; +// HomeLocationGet(&homeLocation); +// homeLocation.Latitude = 0; +// homeLocation.Longitude = 0; +// homeLocation.Altitude = 0; +// homeLocation.Be[0] = 26000; +// homeLocation.Be[1] = 400; +// homeLocation.Be[2] = 40000; +// homeLocation.Set = HOMELOCATION_SET_TRUE; +// HomeLocationSet(&homeLocation); - // Main task loop - lastSysTime = xTaskGetTickCount(); - uint32_t last_time = PIOS_DELAY_GetRaw(); - while (1) { - PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); + // Main task loop + lastSysTime = xTaskGetTickCount(); + uint32_t last_time = PIOS_DELAY_GetRaw(); + while (1) { + PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); - SystemSettingsData systemSettings; - SystemSettingsGet(&systemSettings); + SystemSettingsData systemSettings; + SystemSettingsGet(&systemSettings); - switch(systemSettings.AirframeType) { - case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING: - case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON: - case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL: - sensor_sim_type = MODEL_AIRPLANE; - break; - case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX: - case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP: - case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL: - case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA: - case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO: - sensor_sim_type = MODEL_QUADCOPTER; - break; - default: - sensor_sim_type = MODEL_AGNOSTIC; - } - - static int i; - i++; - if (i % 5000 == 0) { - //float dT = PIOS_DELAY_DiffuS(last_time) / 10.0e6; - //fprintf(stderr, "Sensor relative timing: %f\n", dT); - last_time = PIOS_DELAY_GetRaw(); - } - - sensors_count++; + switch (systemSettings.AirframeType) { + case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING: + case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON: + case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL: + sensor_sim_type = MODEL_AIRPLANE; + break; + case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX: + case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP: + case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL: + case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO: + sensor_sim_type = MODEL_QUADCOPTER; + break; + default: + sensor_sim_type = MODEL_AGNOSTIC; + } - switch(sensor_sim_type) { - case CONSTANT: - simulateConstant(); - break; - case MODEL_AGNOSTIC: - simulateModelAgnostic(); - break; - case MODEL_QUADCOPTER: - simulateModelQuadcopter(); - break; - case MODEL_AIRPLANE: - simulateModelAirplane(); - } + static int i; + i++; + if (i % 5000 == 0) { + // float dT = PIOS_DELAY_DiffuS(last_time) / 10.0e6; + // fprintf(stderr, "Sensor relative timing: %f\n", dT); + last_time = PIOS_DELAY_GetRaw(); + } - vTaskDelay(2 / portTICK_RATE_MS); + sensors_count++; - } + switch (sensor_sim_type) { + case CONSTANT: + simulateConstant(); + break; + case MODEL_AGNOSTIC: + simulateModelAgnostic(); + break; + case MODEL_QUADCOPTER: + simulateModelQuadcopter(); + break; + case MODEL_AIRPLANE: + simulateModelAirplane(); + } + + vTaskDelay(2 / portTICK_RATE_MS); + } } static void simulateConstant() { - AccelsData accelsData; // Skip get as we set all the fields - accelsData.x = 0; - accelsData.y = 0; - accelsData.z = -GRAV; - accelsData.temperature = 0; - AccelsSet(&accelsData); + AccelsData accelsData; // Skip get as we set all the fields - GyrosData gyrosData; // Skip get as we set all the fields - gyrosData.x = 0; - gyrosData.y = 0; - gyrosData.z = 0; + accelsData.x = 0; + accelsData.y = 0; + accelsData.z = -GRAV; + accelsData.temperature = 0; + AccelsSet(&accelsData); - // Apply bias correction to the gyros - GyrosBiasData gyrosBias; - GyrosBiasGet(&gyrosBias); - gyrosData.x += gyrosBias.x; - gyrosData.y += gyrosBias.y; - gyrosData.z += gyrosBias.z; + GyrosData gyrosData; // Skip get as we set all the fields + gyrosData.x = 0; + gyrosData.y = 0; + gyrosData.z = 0; - GyrosSet(&gyrosData); + // Apply bias correction to the gyros + GyrosBiasData gyrosBias; + GyrosBiasGet(&gyrosBias); + gyrosData.x += gyrosBias.x; + gyrosData.y += gyrosBias.y; + gyrosData.z += gyrosBias.z; - BaroAltitudeData baroAltitude; - BaroAltitudeGet(&baroAltitude); - baroAltitude.Altitude = 1; - BaroAltitudeSet(&baroAltitude); + GyrosSet(&gyrosData); - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - gpsPosition.Latitude = 0; - gpsPosition.Longitude = 0; - gpsPosition.Altitude = 0; - GPSPositionSet(&gpsPosition); + BaroAltitudeData baroAltitude; + BaroAltitudeGet(&baroAltitude); + baroAltitude.Altitude = 1; + BaroAltitudeSet(&baroAltitude); - // Because most crafts wont get enough information from gravity to zero yaw gyro, we try - // and make it average zero (weakly) - MagnetometerData mag; - mag.x = 400; - mag.y = 0; - mag.z = 800; - MagnetometerSet(&mag); + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + gpsPosition.Latitude = 0; + gpsPosition.Longitude = 0; + gpsPosition.Altitude = 0; + GPSPositionSet(&gpsPosition); + + // Because most crafts wont get enough information from gravity to zero yaw gyro, we try + // and make it average zero (weakly) + MagnetometerData mag; + mag.x = 400; + mag.y = 0; + mag.z = 800; + MagnetometerSet(&mag); } static void simulateModelAgnostic() { - float Rbe[3][3]; - float q[4]; + float Rbe[3][3]; + float q[4]; - // Simulate accels based on current attitude - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - q[0] = attitudeActual.q1; - q[1] = attitudeActual.q2; - q[2] = attitudeActual.q3; - q[3] = attitudeActual.q4; - Quaternion2R(q,Rbe); + // Simulate accels based on current attitude + AttitudeActualData attitudeActual; - AccelsData accelsData; // Skip get as we set all the fields - accelsData.x = -GRAV * Rbe[0][2]; - accelsData.y = -GRAV * Rbe[1][2]; - accelsData.z = -GRAV * Rbe[2][2]; - accelsData.temperature = 30; - AccelsSet(&accelsData); + AttitudeActualGet(&attitudeActual); + q[0] = attitudeActual.q1; + q[1] = attitudeActual.q2; + q[2] = attitudeActual.q3; + q[3] = attitudeActual.q4; + Quaternion2R(q, Rbe); - RateDesiredData rateDesired; - RateDesiredGet(&rateDesired); + AccelsData accelsData; // Skip get as we set all the fields + accelsData.x = -GRAV * Rbe[0][2]; + accelsData.y = -GRAV * Rbe[1][2]; + accelsData.z = -GRAV * Rbe[2][2]; + accelsData.temperature = 30; + AccelsSet(&accelsData); - GyrosData gyrosData; // Skip get as we set all the fields - gyrosData.x = rateDesired.Roll + rand_gauss(); - gyrosData.y = rateDesired.Pitch + rand_gauss(); - gyrosData.z = rateDesired.Yaw + rand_gauss(); + RateDesiredData rateDesired; + RateDesiredGet(&rateDesired); - // Apply bias correction to the gyros - GyrosBiasData gyrosBias; - GyrosBiasGet(&gyrosBias); - gyrosData.x += gyrosBias.x; - gyrosData.y += gyrosBias.y; - gyrosData.z += gyrosBias.z; + GyrosData gyrosData; // Skip get as we set all the fields + gyrosData.x = rateDesired.Roll + rand_gauss(); + gyrosData.y = rateDesired.Pitch + rand_gauss(); + gyrosData.z = rateDesired.Yaw + rand_gauss(); - GyrosSet(&gyrosData); + // Apply bias correction to the gyros + GyrosBiasData gyrosBias; + GyrosBiasGet(&gyrosBias); + gyrosData.x += gyrosBias.x; + gyrosData.y += gyrosBias.y; + gyrosData.z += gyrosBias.z; - BaroAltitudeData baroAltitude; - BaroAltitudeGet(&baroAltitude); - baroAltitude.Altitude = 1; - BaroAltitudeSet(&baroAltitude); + GyrosSet(&gyrosData); - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - gpsPosition.Latitude = 0; - gpsPosition.Longitude = 0; - gpsPosition.Altitude = 0; - GPSPositionSet(&gpsPosition); + BaroAltitudeData baroAltitude; + BaroAltitudeGet(&baroAltitude); + baroAltitude.Altitude = 1; + BaroAltitudeSet(&baroAltitude); - // Because most crafts wont get enough information from gravity to zero yaw gyro, we try - // and make it average zero (weakly) - MagnetometerData mag; - mag.x = 400; - mag.y = 0; - mag.z = 800; - MagnetometerSet(&mag); + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + gpsPosition.Latitude = 0; + gpsPosition.Longitude = 0; + gpsPosition.Altitude = 0; + GPSPositionSet(&gpsPosition); + + // Because most crafts wont get enough information from gravity to zero yaw gyro, we try + // and make it average zero (weakly) + MagnetometerData mag; + mag.x = 400; + mag.y = 0; + mag.z = 800; + MagnetometerSet(&mag); } -float thrustToDegs = 50; +float thrustToDegs = 50; bool overideAttitude = false; static void simulateModelQuadcopter() { - static double pos[3] = {0,0,0}; - static double vel[3] = {0,0,0}; - static double ned_accel[3] = {0,0,0}; - static float q[4] = {1,0,0,0}; - static float rpy[3] = {0,0,0}; // Low pass filtered actuator - static float baro_offset = 0.0f; - float Rbe[3][3]; - - const float ACTUATOR_ALPHA = 0.8; - const float MAX_THRUST = GRAV * 2; - const float K_FRICTION = 1; - const float GPS_PERIOD = 0.1; - const float MAG_PERIOD = 1.0 / 75.0; - const float BARO_PERIOD = 1.0 / 20.0; - - static uint32_t last_time; - - float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6); - if(dT < 1e-3) - dT = 2e-3; - last_time = PIOS_DELAY_GetRaw(); - - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - ActuatorDesiredData actuatorDesired; - ActuatorDesiredGet(&actuatorDesired); + static double pos[3] = { 0, 0, 0 }; + static double vel[3] = { 0, 0, 0 }; + static double ned_accel[3] = { 0, 0, 0 }; + static float q[4] = { 1, 0, 0, 0 }; + static float rpy[3] = { 0, 0, 0 }; // Low pass filtered actuator + static float baro_offset = 0.0f; + float Rbe[3][3]; - float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0; - if (thrust < 0) - thrust = 0; - - if (thrust != thrust) - thrust = 0; - -// float control_scaling = thrust * thrustToDegs; -// // In rad/s -// rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; -// rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; -// rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; -// -// GyrosData gyrosData; // Skip get as we set all the fields -// gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss(); -// gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss(); -// gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss(); - - RateDesiredData rateDesired; - RateDesiredGet(&rateDesired); - - rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; - rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; - rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; - - GyrosData gyrosData; // Skip get as we set all the fields - gyrosData.x = rpy[0] + rand_gauss(); - gyrosData.y = rpy[1] + rand_gauss(); - gyrosData.z = rpy[2] + rand_gauss(); - GyrosSet(&gyrosData); - - // Predict the attitude forward in time - float qdot[4]; - qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * M_PI / 180 / 2; - qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * M_PI / 180 / 2; - qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * M_PI / 180 / 2; - qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * M_PI / 180 / 2; - - // Take a time step - q[0] = q[0] + qdot[0]; - q[1] = q[1] + qdot[1]; - q[2] = q[2] + qdot[2]; - q[3] = q[3] + qdot[3]; - - float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); - q[0] = q[0] / qmag; - q[1] = q[1] / qmag; - q[2] = q[2] / qmag; - q[3] = q[3] / qmag; - - if(overideAttitude){ - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - attitudeActual.q1 = q[0]; - attitudeActual.q2 = q[1]; - attitudeActual.q3 = q[2]; - attitudeActual.q4 = q[3]; - AttitudeActualSet(&attitudeActual); - } - - static float wind[3] = {0,0,0}; - wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0; - wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0; - wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0; - - Quaternion2R(q,Rbe); - // Make thrust negative as down is positive - ned_accel[0] = -thrust * Rbe[2][0]; - ned_accel[1] = -thrust * Rbe[2][1]; - // Gravity causes acceleration of 9.81 in the down direction - ned_accel[2] = -thrust * Rbe[2][2] + GRAV; - - // Apply acceleration based on velocity - ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]); - ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]); - ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]); + const float ACTUATOR_ALPHA = 0.8; + const float MAX_THRUST = GRAV * 2; + const float K_FRICTION = 1; + const float GPS_PERIOD = 0.1; + const float MAG_PERIOD = 1.0 / 75.0; + const float BARO_PERIOD = 1.0 / 20.0; - // Predict the velocity forward in time - vel[0] = vel[0] + ned_accel[0] * dT; - vel[1] = vel[1] + ned_accel[1] * dT; - vel[2] = vel[2] + ned_accel[2] * dT; + static uint32_t last_time; - // Predict the position forward in time - pos[0] = pos[0] + vel[0] * dT; - pos[1] = pos[1] + vel[1] * dT; - pos[2] = pos[2] + vel[2] * dT; + float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6); - // Simulate hitting ground - if(pos[2] > 0) { - pos[2] = 0; - vel[2] = 0; - ned_accel[2] = 0; - } - - // Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0) - ned_accel[2] -= 9.81; - - // Transform the accels back in to body frame - AccelsData accelsData; // Skip get as we set all the fields - accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0]; - accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1]; - accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2]; - accelsData.temperature = 30; - AccelsSet(&accelsData); + if (dT < 1e-3) { + dT = 2e-3; + } + last_time = PIOS_DELAY_GetRaw(); - if(baro_offset == 0) { - // Hacky initialization - baro_offset = 50;// * rand_gauss(); - } else { - // Very small drift process - baro_offset += rand_gauss() / 100; - } - // Update baro periodically - static uint32_t last_baro_time = 0; - if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) { - BaroAltitudeData baroAltitude; - BaroAltitudeGet(&baroAltitude); - baroAltitude.Altitude = -pos[2] + baro_offset; - BaroAltitudeSet(&baroAltitude); - last_baro_time = PIOS_DELAY_GetRaw(); - } - - HomeLocationData homeLocation; - HomeLocationGet(&homeLocation); + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + ActuatorDesiredData actuatorDesired; + ActuatorDesiredGet(&actuatorDesired); - static float gps_vel_drift[3] = {0,0,0}; - gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0; - gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0; - gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0; + float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0; + if (thrust < 0) { + thrust = 0; + } - // Update GPS periodically - static uint32_t last_gps_time = 0; - if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) { - // Use double precision here as simulating what GPS produces - double T[3]; - T[0] = homeLocation.Altitude+6.378137E6f * M_PI / 180.0; - T[1] = cos(homeLocation.Latitude / 10e6 * M_PI / 180.0f)*(homeLocation.Altitude+6.378137E6) * M_PI / 180.0; - T[2] = -1.0; - - static float gps_drift[3] = {0,0,0}; - gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0; - gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0; - gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0; + if (thrust != thrust) { + thrust = 0; + } - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6); - gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6); - gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]); - gpsPosition.Groundspeed = sqrt(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2)); - gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]); - gpsPosition.Satellites = 7; - gpsPosition.PDOP = 1; - GPSPositionSet(&gpsPosition); - last_gps_time = PIOS_DELAY_GetRaw(); - } - - // Update GPS Velocity measurements - static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond - if(PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) { - GPSVelocityData gpsVelocity; - GPSVelocityGet(&gpsVelocity); - gpsVelocity.North = vel[0] + gps_vel_drift[0]; - gpsVelocity.East = vel[1] + gps_vel_drift[1]; - gpsVelocity.Down = vel[2] + gps_vel_drift[2]; - GPSVelocitySet(&gpsVelocity); - last_gps_vel_time = PIOS_DELAY_GetRaw(); - } +// float control_scaling = thrust * thrustToDegs; +//// In rad/s +// rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; +// rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; +// rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; +// +// GyrosData gyrosData; // Skip get as we set all the fields +// gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss(); +// gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss(); +// gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss(); - // Update mag periodically - static uint32_t last_mag_time = 0; - if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { - MagnetometerData mag; - mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; - mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; - mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; + RateDesiredData rateDesired; + RateDesiredGet(&rateDesired); - // Run the offset compensation algorithm from the firmware - magOffsetEstimation(&mag); + rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; + rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; + rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; - MagnetometerSet(&mag); - last_mag_time = PIOS_DELAY_GetRaw(); - } - - AttitudeSimulatedData attitudeSimulated; - AttitudeSimulatedGet(&attitudeSimulated); - attitudeSimulated.q1 = q[0]; - attitudeSimulated.q2 = q[1]; - attitudeSimulated.q3 = q[2]; - attitudeSimulated.q4 = q[3]; - Quaternion2RPY(q,&attitudeSimulated.Roll); - attitudeSimulated.Position[0] = pos[0]; - attitudeSimulated.Position[1] = pos[1]; - attitudeSimulated.Position[2] = pos[2]; - attitudeSimulated.Velocity[0] = vel[0]; - attitudeSimulated.Velocity[1] = vel[1]; - attitudeSimulated.Velocity[2] = vel[2]; - AttitudeSimulatedSet(&attitudeSimulated); + GyrosData gyrosData; // Skip get as we set all the fields + gyrosData.x = rpy[0] + rand_gauss(); + gyrosData.y = rpy[1] + rand_gauss(); + gyrosData.z = rpy[2] + rand_gauss(); + GyrosSet(&gyrosData); + + // Predict the attitude forward in time + float qdot[4]; + qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * M_PI / 180 / 2; + qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * M_PI / 180 / 2; + qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * M_PI / 180 / 2; + qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * M_PI / 180 / 2; + + // Take a time step + q[0] = q[0] + qdot[0]; + q[1] = q[1] + qdot[1]; + q[2] = q[2] + qdot[2]; + q[3] = q[3] + qdot[3]; + + float qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + q[0] = q[0] / qmag; + q[1] = q[1] / qmag; + q[2] = q[2] / qmag; + q[3] = q[3] / qmag; + + if (overideAttitude) { + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); + attitudeActual.q1 = q[0]; + attitudeActual.q2 = q[1]; + attitudeActual.q3 = q[2]; + attitudeActual.q4 = q[3]; + AttitudeActualSet(&attitudeActual); + } + + static float wind[3] = { 0, 0, 0 }; + wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0; + wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0; + wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0; + + Quaternion2R(q, Rbe); + // Make thrust negative as down is positive + ned_accel[0] = -thrust * Rbe[2][0]; + ned_accel[1] = -thrust * Rbe[2][1]; + // Gravity causes acceleration of 9.81 in the down direction + ned_accel[2] = -thrust * Rbe[2][2] + GRAV; + + // Apply acceleration based on velocity + ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]); + ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]); + ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]); + + // Predict the velocity forward in time + vel[0] = vel[0] + ned_accel[0] * dT; + vel[1] = vel[1] + ned_accel[1] * dT; + vel[2] = vel[2] + ned_accel[2] * dT; + + // Predict the position forward in time + pos[0] = pos[0] + vel[0] * dT; + pos[1] = pos[1] + vel[1] * dT; + pos[2] = pos[2] + vel[2] * dT; + + // Simulate hitting ground + if (pos[2] > 0) { + pos[2] = 0; + vel[2] = 0; + ned_accel[2] = 0; + } + + // Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0) + ned_accel[2] -= 9.81; + + // Transform the accels back in to body frame + AccelsData accelsData; // Skip get as we set all the fields + accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0]; + accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1]; + accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2]; + accelsData.temperature = 30; + AccelsSet(&accelsData); + + if (baro_offset == 0) { + // Hacky initialization + baro_offset = 50; // * rand_gauss(); + } else { + // Very small drift process + baro_offset += rand_gauss() / 100; + } + // Update baro periodically + static uint32_t last_baro_time = 0; + if (PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) { + BaroAltitudeData baroAltitude; + BaroAltitudeGet(&baroAltitude); + baroAltitude.Altitude = -pos[2] + baro_offset; + BaroAltitudeSet(&baroAltitude); + last_baro_time = PIOS_DELAY_GetRaw(); + } + + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); + + static float gps_vel_drift[3] = { 0, 0, 0 }; + gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0; + gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0; + gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0; + + // Update GPS periodically + static uint32_t last_gps_time = 0; + if (PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) { + // Use double precision here as simulating what GPS produces + double T[3]; + T[0] = homeLocation.Altitude + 6.378137E6f * M_PI / 180.0; + T[1] = cos(homeLocation.Latitude / 10e6 * M_PI / 180.0f) * (homeLocation.Altitude + 6.378137E6) * M_PI / 180.0; + T[2] = -1.0; + + static float gps_drift[3] = { 0, 0, 0 }; + gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0; + gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0; + gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0; + + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6); + gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1]) / T[1] * 10.0e6); + gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]); + gpsPosition.Groundspeed = sqrt(pow(vel[0] + gps_vel_drift[0], 2) + pow(vel[1] + gps_vel_drift[1], 2)); + gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1], vel[0] + gps_vel_drift[0]); + gpsPosition.Satellites = 7; + gpsPosition.PDOP = 1; + GPSPositionSet(&gpsPosition); + last_gps_time = PIOS_DELAY_GetRaw(); + } + + // Update GPS Velocity measurements + static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond + if (PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) { + GPSVelocityData gpsVelocity; + GPSVelocityGet(&gpsVelocity); + gpsVelocity.North = vel[0] + gps_vel_drift[0]; + gpsVelocity.East = vel[1] + gps_vel_drift[1]; + gpsVelocity.Down = vel[2] + gps_vel_drift[2]; + GPSVelocitySet(&gpsVelocity); + last_gps_vel_time = PIOS_DELAY_GetRaw(); + } + + // Update mag periodically + static uint32_t last_mag_time = 0; + if (PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { + MagnetometerData mag; + mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; + mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; + mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; + + // Run the offset compensation algorithm from the firmware + magOffsetEstimation(&mag); + + MagnetometerSet(&mag); + last_mag_time = PIOS_DELAY_GetRaw(); + } + + AttitudeSimulatedData attitudeSimulated; + AttitudeSimulatedGet(&attitudeSimulated); + attitudeSimulated.q1 = q[0]; + attitudeSimulated.q2 = q[1]; + attitudeSimulated.q3 = q[2]; + attitudeSimulated.q4 = q[3]; + Quaternion2RPY(q, &attitudeSimulated.Roll); + attitudeSimulated.Position[0] = pos[0]; + attitudeSimulated.Position[1] = pos[1]; + attitudeSimulated.Position[2] = pos[2]; + attitudeSimulated.Velocity[0] = vel[0]; + attitudeSimulated.Velocity[1] = vel[1]; + attitudeSimulated.Velocity[2] = vel[2]; + AttitudeSimulatedSet(&attitudeSimulated); } /** * This method performs a simple simulation of a quadcopter - * + * * It takes in the ActuatorDesired command to rotate the aircraft and performs * a simple kinetic model where the throttle increases the energy and drag decreases * it. Changing altitude moves energy from kinetic to potential. @@ -564,391 +568,396 @@ static void simulateModelQuadcopter() */ static void simulateModelAirplane() { - static double pos[3] = {0,0,0}; - static double vel[3] = {0,0,0}; - static double ned_accel[3] = {0,0,0}; - static float q[4] = {1,0,0,0}; - static float rpy[3] = {0,0,0}; // Low pass filtered actuator - static float baro_offset = 0.0f; - float Rbe[3][3]; - - const float LIFT_SPEED = 8; // (m/s) where achieve lift for zero pitch - const float ACTUATOR_ALPHA = 0.8; - const float MAX_THRUST = 9.81 * 2; - const float K_FRICTION = 0.2; - const float GPS_PERIOD = 0.1; - const float MAG_PERIOD = 1.0 / 75.0; - const float BARO_PERIOD = 1.0 / 20.0; - const float ROLL_HEADING_COUPLING = 0.1; // (deg/s) heading change per deg of roll - const float PITCH_THRUST_COUPLING = 0.2; // (m/s^2) of forward acceleration per deg of pitch - - static uint32_t last_time; - - float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6); - if(dT < 1e-3) - dT = 2e-3; - last_time = PIOS_DELAY_GetRaw(); - - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - ActuatorDesiredData actuatorDesired; - ActuatorDesiredGet(&actuatorDesired); - - float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0; - if (thrust < 0) - thrust = 0; - - if (thrust != thrust) - thrust = 0; - - // float control_scaling = thrust * thrustToDegs; - // // In rad/s - // rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; - // rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; - // rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; - // - // GyrosData gyrosData; // Skip get as we set all the fields - // gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss(); - // gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss(); - // gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss(); - - /**** 1. Update attitude ****/ - RateDesiredData rateDesired; - RateDesiredGet(&rateDesired); - - // Need to get roll angle for easy cross coupling - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - double roll = attitudeActual.Roll; - double pitch = attitudeActual.Pitch; + static double pos[3] = { 0, 0, 0 }; + static double vel[3] = { 0, 0, 0 }; + static double ned_accel[3] = { 0, 0, 0 }; + static float q[4] = { 1, 0, 0, 0 }; + static float rpy[3] = { 0, 0, 0 }; // Low pass filtered actuator + static float baro_offset = 0.0f; + float Rbe[3][3]; - rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; - rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; - rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; - rpy[2] += roll * ROLL_HEADING_COUPLING; - + const float LIFT_SPEED = 8; // (m/s) where achieve lift for zero pitch + const float ACTUATOR_ALPHA = 0.8; + const float MAX_THRUST = 9.81 * 2; + const float K_FRICTION = 0.2; + const float GPS_PERIOD = 0.1; + const float MAG_PERIOD = 1.0 / 75.0; + const float BARO_PERIOD = 1.0 / 20.0; + const float ROLL_HEADING_COUPLING = 0.1; // (deg/s) heading change per deg of roll + const float PITCH_THRUST_COUPLING = 0.2; // (m/s^2) of forward acceleration per deg of pitch - GyrosData gyrosData; // Skip get as we set all the fields - gyrosData.x = rpy[0] + rand_gauss(); - gyrosData.y = rpy[1] + rand_gauss(); - gyrosData.z = rpy[2] + rand_gauss(); - GyrosSet(&gyrosData); - - // Predict the attitude forward in time - float qdot[4]; - qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * M_PI / 180 / 2; - qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * M_PI / 180 / 2; - qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * M_PI / 180 / 2; - qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * M_PI / 180 / 2; - - // Take a time step - q[0] = q[0] + qdot[0]; - q[1] = q[1] + qdot[1]; - q[2] = q[2] + qdot[2]; - q[3] = q[3] + qdot[3]; - - float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); - q[0] = q[0] / qmag; - q[1] = q[1] / qmag; - q[2] = q[2] / qmag; - q[3] = q[3] / qmag; - - if(overideAttitude){ - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - attitudeActual.q1 = q[0]; - attitudeActual.q2 = q[1]; - attitudeActual.q3 = q[2]; - attitudeActual.q4 = q[3]; - AttitudeActualSet(&attitudeActual); - } - - /**** 2. Update position based on velocity ****/ - static float wind[3] = {0,0,0}; - wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0; - wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0; - wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0; - wind[0] = 0; - wind[1] = 0; - wind[2] = 0; - - // Rbe takes a vector from body to earth. If we take (1,0,0)^T through this and then dot with airspeed - // we get forward airspeed - Quaternion2R(q,Rbe); + static uint32_t last_time; - double airspeed[3] = {vel[0] - wind[0], vel[1] - wind[1], vel[2] - wind[2]}; - double forwardAirspeed = Rbe[0][0] * airspeed[0] + Rbe[0][1] * airspeed[1] + Rbe[0][2] * airspeed[2]; - double sidewaysAirspeed = Rbe[1][0] * airspeed[0] + Rbe[1][1] * airspeed[1] + Rbe[1][2] * airspeed[2]; - double downwardAirspeed = Rbe[2][0] * airspeed[0] + Rbe[2][1] * airspeed[1] + Rbe[2][2] * airspeed[2]; - - /* Compute aerodynamic forces in body referenced frame. Later use more sophisticated equations */ - /* TODO: This should become more accurate. Use the force equations to calculate lift from the */ - /* various surfaces based on AoA and airspeed. From that compute torques and forces. For later */ - double forces[3]; // X, Y, Z - forces[0] = thrust - pitch * PITCH_THRUST_COUPLING - forwardAirspeed * K_FRICTION; // Friction is applied in all directions in NED - forces[1] = 0 - sidewaysAirspeed * K_FRICTION * 100; // No side slip - forces[2] = GRAV * (forwardAirspeed - LIFT_SPEED) + downwardAirspeed * K_FRICTION * 100; // Stupidly simple, always have gravity lift when straight and level - - // Negate force[2] as NED defines down as possitive, aircraft convention is Z up is positive (?) - ned_accel[0] = forces[0] * Rbe[0][0] + forces[1] * Rbe[1][0] - forces[2] * Rbe[2][0]; - ned_accel[1] = forces[0] * Rbe[0][1] + forces[1] * Rbe[1][1] - forces[2] * Rbe[2][1]; - ned_accel[2] = forces[0] * Rbe[0][2] + forces[1] * Rbe[1][2] - forces[2] * Rbe[2][2]; - // Gravity causes acceleration of 9.81 in the down direction - ned_accel[2] += 9.81; + float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6); - // Apply acceleration based on velocity - ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]); - ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]); - ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]); - - // Predict the velocity forward in time - vel[0] = vel[0] + ned_accel[0] * dT; - vel[1] = vel[1] + ned_accel[1] * dT; - vel[2] = vel[2] + ned_accel[2] * dT; - - // Predict the position forward in time - pos[0] = pos[0] + vel[0] * dT; - pos[1] = pos[1] + vel[1] * dT; - pos[2] = pos[2] + vel[2] * dT; - - // Simulate hitting ground - if(pos[2] > 0) { - pos[2] = 0; - vel[2] = 0; - ned_accel[2] = 0; - } - - // Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0) - ned_accel[2] -= GRAV; - - // Transform the accels back in to body frame - AccelsData accelsData; // Skip get as we set all the fields - accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0]; - accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1]; - accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2]; - accelsData.temperature = 30; - AccelsSet(&accelsData); - - if(baro_offset == 0) { - // Hacky initialization - baro_offset = 50;// * rand_gauss(); - } else { - // Very small drift process - baro_offset += rand_gauss() / 100; - } - // Update baro periodically - static uint32_t last_baro_time = 0; - if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) { - BaroAltitudeData baroAltitude; - BaroAltitudeGet(&baroAltitude); - baroAltitude.Altitude = -pos[2] + baro_offset; - BaroAltitudeSet(&baroAltitude); - last_baro_time = PIOS_DELAY_GetRaw(); - } - - // Update baro airpseed periodically - static uint32_t last_airspeed_time = 0; - if(PIOS_DELAY_DiffuS(last_airspeed_time) / 1.0e6 > BARO_PERIOD) { - AirspeedSensorData airspeedSensor; - airspeedSensor.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; - airspeedSensor.CalibratedAirspeed = forwardAirspeed; - AirspeedSensorSet(&airspeedSensor); - last_airspeed_time = PIOS_DELAY_GetRaw(); - } - - HomeLocationData homeLocation; - HomeLocationGet(&homeLocation); - - static float gps_vel_drift[3] = {0,0,0}; - gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0; - gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0; - gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0; - - // Update GPS periodically - static uint32_t last_gps_time = 0; - if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) { - // Use double precision here as simulating what GPS produces - double T[3]; - T[0] = homeLocation.Altitude+6.378137E6f * M_PI / 180.0; - T[1] = cos(homeLocation.Latitude / 10e6 * M_PI / 180.0f)*(homeLocation.Altitude+6.378137E6) * M_PI / 180.0; - T[2] = -1.0; - - static float gps_drift[3] = {0,0,0}; - gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0; - gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0; - gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0; - - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6); - gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6); - gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]); - gpsPosition.Groundspeed = sqrt(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2)); - gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]); - gpsPosition.Satellites = 7; - gpsPosition.PDOP = 1; - GPSPositionSet(&gpsPosition); - last_gps_time = PIOS_DELAY_GetRaw(); - } - - // Update GPS Velocity measurements - static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond - if(PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) { - GPSVelocityData gpsVelocity; - GPSVelocityGet(&gpsVelocity); - gpsVelocity.North = vel[0] + gps_vel_drift[0]; - gpsVelocity.East = vel[1] + gps_vel_drift[1]; - gpsVelocity.Down = vel[2] + gps_vel_drift[2]; - GPSVelocitySet(&gpsVelocity); - last_gps_vel_time = PIOS_DELAY_GetRaw(); - } - - // Update mag periodically - static uint32_t last_mag_time = 0; - if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { - MagnetometerData mag; - mag.x = 100+homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; - mag.y = 100+homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; - mag.z = 100+homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; - magOffsetEstimation(&mag); - MagnetometerSet(&mag); - last_mag_time = PIOS_DELAY_GetRaw(); - } - - AttitudeSimulatedData attitudeSimulated; - AttitudeSimulatedGet(&attitudeSimulated); - attitudeSimulated.q1 = q[0]; - attitudeSimulated.q2 = q[1]; - attitudeSimulated.q3 = q[2]; - attitudeSimulated.q4 = q[3]; - Quaternion2RPY(q,&attitudeSimulated.Roll); - attitudeSimulated.Position[0] = pos[0]; - attitudeSimulated.Position[1] = pos[1]; - attitudeSimulated.Position[2] = pos[2]; - attitudeSimulated.Velocity[0] = vel[0]; - attitudeSimulated.Velocity[1] = vel[1]; - attitudeSimulated.Velocity[2] = vel[2]; - AttitudeSimulatedSet(&attitudeSimulated); + if (dT < 1e-3) { + dT = 2e-3; + } + last_time = PIOS_DELAY_GetRaw(); + + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + ActuatorDesiredData actuatorDesired; + ActuatorDesiredGet(&actuatorDesired); + + float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0; + if (thrust < 0) { + thrust = 0; + } + + if (thrust != thrust) { + thrust = 0; + } + + // float control_scaling = thrust * thrustToDegs; + //// In rad/s + // rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; + // rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; + // rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; + // + // GyrosData gyrosData; // Skip get as we set all the fields + // gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss(); + // gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss(); + // gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss(); + + /**** 1. Update attitude ****/ + RateDesiredData rateDesired; + RateDesiredGet(&rateDesired); + + // Need to get roll angle for easy cross coupling + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); + double roll = attitudeActual.Roll; + double pitch = attitudeActual.Pitch; + + rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; + rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; + rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; + rpy[2] += roll * ROLL_HEADING_COUPLING; + + + GyrosData gyrosData; // Skip get as we set all the fields + gyrosData.x = rpy[0] + rand_gauss(); + gyrosData.y = rpy[1] + rand_gauss(); + gyrosData.z = rpy[2] + rand_gauss(); + GyrosSet(&gyrosData); + + // Predict the attitude forward in time + float qdot[4]; + qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * M_PI / 180 / 2; + qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * M_PI / 180 / 2; + qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * M_PI / 180 / 2; + qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * M_PI / 180 / 2; + + // Take a time step + q[0] = q[0] + qdot[0]; + q[1] = q[1] + qdot[1]; + q[2] = q[2] + qdot[2]; + q[3] = q[3] + qdot[3]; + + float qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + q[0] = q[0] / qmag; + q[1] = q[1] / qmag; + q[2] = q[2] / qmag; + q[3] = q[3] / qmag; + + if (overideAttitude) { + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); + attitudeActual.q1 = q[0]; + attitudeActual.q2 = q[1]; + attitudeActual.q3 = q[2]; + attitudeActual.q4 = q[3]; + AttitudeActualSet(&attitudeActual); + } + + /**** 2. Update position based on velocity ****/ + static float wind[3] = { 0, 0, 0 }; + wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0; + wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0; + wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0; + wind[0] = 0; + wind[1] = 0; + wind[2] = 0; + + // Rbe takes a vector from body to earth. If we take (1,0,0)^T through this and then dot with airspeed + // we get forward airspeed + Quaternion2R(q, Rbe); + + double airspeed[3] = { vel[0] - wind[0], vel[1] - wind[1], vel[2] - wind[2] }; + double forwardAirspeed = Rbe[0][0] * airspeed[0] + Rbe[0][1] * airspeed[1] + Rbe[0][2] * airspeed[2]; + double sidewaysAirspeed = Rbe[1][0] * airspeed[0] + Rbe[1][1] * airspeed[1] + Rbe[1][2] * airspeed[2]; + double downwardAirspeed = Rbe[2][0] * airspeed[0] + Rbe[2][1] * airspeed[1] + Rbe[2][2] * airspeed[2]; + + /* Compute aerodynamic forces in body referenced frame. Later use more sophisticated equations */ + /* TODO: This should become more accurate. Use the force equations to calculate lift from the */ + /* various surfaces based on AoA and airspeed. From that compute torques and forces. For later */ + double forces[3]; // X, Y, Z + forces[0] = thrust - pitch * PITCH_THRUST_COUPLING - forwardAirspeed * K_FRICTION; // Friction is applied in all directions in NED + forces[1] = 0 - sidewaysAirspeed * K_FRICTION * 100; // No side slip + forces[2] = GRAV * (forwardAirspeed - LIFT_SPEED) + downwardAirspeed * K_FRICTION * 100; // Stupidly simple, always have gravity lift when straight and level + + // Negate force[2] as NED defines down as possitive, aircraft convention is Z up is positive (?) + ned_accel[0] = forces[0] * Rbe[0][0] + forces[1] * Rbe[1][0] - forces[2] * Rbe[2][0]; + ned_accel[1] = forces[0] * Rbe[0][1] + forces[1] * Rbe[1][1] - forces[2] * Rbe[2][1]; + ned_accel[2] = forces[0] * Rbe[0][2] + forces[1] * Rbe[1][2] - forces[2] * Rbe[2][2]; + // Gravity causes acceleration of 9.81 in the down direction + ned_accel[2] += 9.81; + + // Apply acceleration based on velocity + ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]); + ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]); + ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]); + + // Predict the velocity forward in time + vel[0] = vel[0] + ned_accel[0] * dT; + vel[1] = vel[1] + ned_accel[1] * dT; + vel[2] = vel[2] + ned_accel[2] * dT; + + // Predict the position forward in time + pos[0] = pos[0] + vel[0] * dT; + pos[1] = pos[1] + vel[1] * dT; + pos[2] = pos[2] + vel[2] * dT; + + // Simulate hitting ground + if (pos[2] > 0) { + pos[2] = 0; + vel[2] = 0; + ned_accel[2] = 0; + } + + // Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0) + ned_accel[2] -= GRAV; + + // Transform the accels back in to body frame + AccelsData accelsData; // Skip get as we set all the fields + accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0]; + accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1]; + accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2]; + accelsData.temperature = 30; + AccelsSet(&accelsData); + + if (baro_offset == 0) { + // Hacky initialization + baro_offset = 50; // * rand_gauss(); + } else { + // Very small drift process + baro_offset += rand_gauss() / 100; + } + // Update baro periodically + static uint32_t last_baro_time = 0; + if (PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) { + BaroAltitudeData baroAltitude; + BaroAltitudeGet(&baroAltitude); + baroAltitude.Altitude = -pos[2] + baro_offset; + BaroAltitudeSet(&baroAltitude); + last_baro_time = PIOS_DELAY_GetRaw(); + } + + // Update baro airpseed periodically + static uint32_t last_airspeed_time = 0; + if (PIOS_DELAY_DiffuS(last_airspeed_time) / 1.0e6 > BARO_PERIOD) { + AirspeedSensorData airspeedSensor; + airspeedSensor.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + airspeedSensor.CalibratedAirspeed = forwardAirspeed; + AirspeedSensorSet(&airspeedSensor); + last_airspeed_time = PIOS_DELAY_GetRaw(); + } + + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); + + static float gps_vel_drift[3] = { 0, 0, 0 }; + gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0; + gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0; + gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0; + + // Update GPS periodically + static uint32_t last_gps_time = 0; + if (PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) { + // Use double precision here as simulating what GPS produces + double T[3]; + T[0] = homeLocation.Altitude + 6.378137E6f * M_PI / 180.0; + T[1] = cos(homeLocation.Latitude / 10e6 * M_PI / 180.0f) * (homeLocation.Altitude + 6.378137E6) * M_PI / 180.0; + T[2] = -1.0; + + static float gps_drift[3] = { 0, 0, 0 }; + gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0; + gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0; + gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0; + + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6); + gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1]) / T[1] * 10.0e6); + gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]); + gpsPosition.Groundspeed = sqrt(pow(vel[0] + gps_vel_drift[0], 2) + pow(vel[1] + gps_vel_drift[1], 2)); + gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1], vel[0] + gps_vel_drift[0]); + gpsPosition.Satellites = 7; + gpsPosition.PDOP = 1; + GPSPositionSet(&gpsPosition); + last_gps_time = PIOS_DELAY_GetRaw(); + } + + // Update GPS Velocity measurements + static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond + if (PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) { + GPSVelocityData gpsVelocity; + GPSVelocityGet(&gpsVelocity); + gpsVelocity.North = vel[0] + gps_vel_drift[0]; + gpsVelocity.East = vel[1] + gps_vel_drift[1]; + gpsVelocity.Down = vel[2] + gps_vel_drift[2]; + GPSVelocitySet(&gpsVelocity); + last_gps_vel_time = PIOS_DELAY_GetRaw(); + } + + // Update mag periodically + static uint32_t last_mag_time = 0; + if (PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { + MagnetometerData mag; + mag.x = 100 + homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; + mag.y = 100 + homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; + mag.z = 100 + homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; + magOffsetEstimation(&mag); + MagnetometerSet(&mag); + last_mag_time = PIOS_DELAY_GetRaw(); + } + + AttitudeSimulatedData attitudeSimulated; + AttitudeSimulatedGet(&attitudeSimulated); + attitudeSimulated.q1 = q[0]; + attitudeSimulated.q2 = q[1]; + attitudeSimulated.q3 = q[2]; + attitudeSimulated.q4 = q[3]; + Quaternion2RPY(q, &attitudeSimulated.Roll); + attitudeSimulated.Position[0] = pos[0]; + attitudeSimulated.Position[1] = pos[1]; + attitudeSimulated.Position[2] = pos[2]; + attitudeSimulated.Velocity[0] = vel[0]; + attitudeSimulated.Velocity[1] = vel[1]; + attitudeSimulated.Velocity[2] = vel[2]; + AttitudeSimulatedSet(&attitudeSimulated); } -static float rand_gauss (void) { - float v1,v2,s; - - do { - v1 = 2.0 * ((float) rand()/RAND_MAX) - 1; - v2 = 2.0 * ((float) rand()/RAND_MAX) - 1; - - s = v1*v1 + v2*v2; - } while ( s >= 1.0 ); - - if (s == 0.0) - return 0.0; - else - return (v1*sqrt(-2.0 * log(s) / s)); +static float rand_gauss(void) +{ + float v1, v2, s; + + do { + v1 = 2.0 * ((float)rand() / RAND_MAX) - 1; + v2 = 2.0 * ((float)rand() / RAND_MAX) - 1; + + s = v1 * v1 + v2 * v2; + } while (s >= 1.0); + + if (s == 0.0) { + return 0.0; + } else { + return v1 * sqrt(-2.0 * log(s) / s); + } } /** * Perform an update of the @ref MagBias based on - * Magnetometer Offset Cancellation: Theory and Implementation, + * Magnetometer Offset Cancellation: Theory and Implementation, * revisited William Premerlani, October 14, 2011 */ static void magOffsetEstimation(MagnetometerData *mag) { #if 0 - RevoCalibrationData cal; - RevoCalibrationGet(&cal); + RevoCalibrationData cal; + RevoCalibrationGet(&cal); - // Constants, to possibly go into a UAVO - static const float MIN_NORM_DIFFERENCE = 50; + // Constants, to possibly go into a UAVO + static const float MIN_NORM_DIFFERENCE = 50; - static float B2[3] = {0, 0, 0}; + static float B2[3] = { 0, 0, 0 }; - MagBiasData magBias; - MagBiasGet(&magBias); + MagBiasData magBias; + MagBiasGet(&magBias); - // Remove the current estimate of the bias - mag->x -= magBias.x; - mag->y -= magBias.y; - mag->z -= magBias.z; + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; - // First call - if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { - B2[0] = mag->x; - B2[1] = mag->y; - B2[2] = mag->z; - return; - } + // First call + if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { + B2[0] = mag->x; + B2[1] = mag->y; + B2[2] = mag->z; + return; + } - float B1[3] = {mag->x, mag->y, mag->z}; - float norm_diff = sqrtf(powf(B2[0] - B1[0],2) + powf(B2[1] - B1[1],2) + powf(B2[2] - B1[2],2)); - if (norm_diff > MIN_NORM_DIFFERENCE) { - float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]); - float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]); - float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; - float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale}; + float B1[3] = { mag->x, mag->y, mag->z }; + float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2)); + if (norm_diff > MIN_NORM_DIFFERENCE) { + float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]); + float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]); + float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; + float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale }; - magBias.x += b_error[0]; - magBias.y += b_error[1]; - magBias.z += b_error[2]; + magBias.x += b_error[0]; + magBias.y += b_error[1]; + magBias.z += b_error[2]; - MagBiasSet(&magBias); + MagBiasSet(&magBias); - // Store this value to compare against next update - B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; - } -#else - HomeLocationData homeLocation; - HomeLocationGet(&homeLocation); - - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - - MagBiasData magBias; - MagBiasGet(&magBias); - - // Remove the current estimate of the bias - mag->x -= magBias.x; - mag->y -= magBias.y; - mag->z -= magBias.z; - - const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]); - const float Rz = homeLocation.Be[2]; - - const float rate = 0.01; - float R[3][3]; - float B_e[3]; - float xy[2]; - float delta[3]; - - // Get the rotation matrix - Quaternion2R(&attitude.q1, R); - - // Rotate the mag into the NED frame - B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z; - B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z; - B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z; - - float cy = cosf(attitude.Yaw * M_PI / 180.0f); - float sy = sinf(attitude.Yaw * M_PI / 180.0f); + // Store this value to compare against next update + B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; + } +#else /* if 0 */ + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); - xy[0] = cy * B_e[0] + sy * B_e[1]; - xy[1] = -sy * B_e[0] + cy * B_e[1]; - - float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]); - - delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); - delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); - delta[2] = -rate * (Rz - B_e[2]); - - magBias.x += delta[0]; - magBias.y += delta[1]; - magBias.z += delta[2]; - MagBiasSet(&magBias); -#endif + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + MagBiasData magBias; + MagBiasGet(&magBias); + + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; + + const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]); + const float Rz = homeLocation.Be[2]; + + const float rate = 0.01; + float R[3][3]; + float B_e[3]; + float xy[2]; + float delta[3]; + + // Get the rotation matrix + Quaternion2R(&attitude.q1, R); + + // Rotate the mag into the NED frame + B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z; + B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z; + B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z; + + float cy = cosf(attitude.Yaw * M_PI / 180.0f); + float sy = sinf(attitude.Yaw * M_PI / 180.0f); + + xy[0] = cy * B_e[0] + sy * B_e[1]; + xy[1] = -sy * B_e[0] + cy * B_e[1]; + + float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]); + + delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); + delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); + delta[2] = -rate * (Rz - B_e[2]); + + magBias.x += delta[0]; + magBias.y += delta[1]; + magBias.z += delta[2]; + MagBiasSet(&magBias); +#endif /* if 0 */ } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index 81adb6235..9d8192c43 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -1,12 +1,12 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup StabilizationModule Stabilization Module * @brief Stabilization PID loops in an airframe type independent manner - * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the + * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual" - * @{ + * @{ * * @file stabilization.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -33,13 +33,13 @@ #ifndef STABILIZATION_H #define STABILIZATION_H -enum {ROLL,PITCH,YAW,MAX_AXES}; +enum { ROLL, PITCH, YAW, MAX_AXES }; int32_t StabilizationInitialize(); #endif // STABILIZATION_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/Stabilization/inc/virtualflybar.h b/flight/modules/Stabilization/inc/virtualflybar.h index 8ef54ce52..d92de4801 100644 --- a/flight/modules/Stabilization/inc/virtualflybar.h +++ b/flight/modules/Stabilization/inc/virtualflybar.h @@ -39,4 +39,4 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings); int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT); - #endif /* VIRTUALFLYBAR_H */ \ No newline at end of file + #endif /* VIRTUALFLYBAR_H */ diff --git a/flight/modules/Stabilization/relay_tuning.c b/flight/modules/Stabilization/relay_tuning.c index be22585ed..f9b85c190 100644 --- a/flight/modules/Stabilization/relay_tuning.c +++ b/flight/modules/Stabilization/relay_tuning.c @@ -38,7 +38,7 @@ #include "sin_lookup.h" /** - * Apply a step function for the stabilization controller and monitor the + * Apply a step function for the stabilization controller and monitor the * result * * Used to Replace the rate PID with a relay to measure the critical properties of this axis @@ -46,101 +46,101 @@ */ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) { - RelayTuningData relay; - RelayTuningGet(&relay); + RelayTuningData relay; - static portTickType lastHighTime; - static portTickType lastLowTime; + RelayTuningGet(&relay); - static float accum_sin, accum_cos; - static uint32_t accumulated = 0; + static portTickType lastHighTime; + static portTickType lastLowTime; - const uint16_t DEGLITCH_TIME = 20; // ms - const float AMPLITUDE_ALPHA = 0.95f; - const float PERIOD_ALPHA = 0.95f; + static float accum_sin, accum_cos; + static uint32_t accumulated = 0; - portTickType thisTime = xTaskGetTickCount(); + const uint16_t DEGLITCH_TIME = 20; // ms + const float AMPLITUDE_ALPHA = 0.95f; + const float PERIOD_ALPHA = 0.95f; - static bool rateRelayRunning[MAX_AXES]; + portTickType thisTime = xTaskGetTickCount(); - // This indicates the current estimate of the smoothed error. So when it is high - // we are waiting for it to go low. - static bool high = false; + static bool rateRelayRunning[MAX_AXES]; - // On first run initialize estimates to something reasonable - if(reinit) { - rateRelayRunning[axis] = false; - relay.Period[axis] = 200; - relay.Gain[axis] = 0; + // This indicates the current estimate of the smoothed error. So when it is high + // we are waiting for it to go low. + static bool high = false; - accum_sin = 0; - accum_cos = 0; - accumulated = 0; + // On first run initialize estimates to something reasonable + if (reinit) { + rateRelayRunning[axis] = false; + relay.Period[axis] = 200; + relay.Gain[axis] = 0; - // These should get reinitialized anyway - high = true; - lastHighTime = thisTime; - lastLowTime = thisTime; - RelayTuningSet(&relay); - } + accum_sin = 0; + accum_cos = 0; + accumulated = 0; + + // These should get reinitialized anyway + high = true; + lastHighTime = thisTime; + lastLowTime = thisTime; + RelayTuningSet(&relay); + } - RelayTuningSettingsData relaySettings; - RelayTuningSettingsGet(&relaySettings); + RelayTuningSettingsData relaySettings; + RelayTuningSettingsGet(&relaySettings); - // Compute output, simple threshold on error - *output = high ? relaySettings.Amplitude : -relaySettings.Amplitude; + // Compute output, simple threshold on error + *output = high ? relaySettings.Amplitude : -relaySettings.Amplitude; - /**** The code below here is to estimate the properties of the oscillation ****/ + /**** The code below here is to estimate the properties of the oscillation ****/ - // Make sure the period can't go below limit - if (relay.Period[axis] < DEGLITCH_TIME) - relay.Period[axis] = DEGLITCH_TIME; + // Make sure the period can't go below limit + if (relay.Period[axis] < DEGLITCH_TIME) { + relay.Period[axis] = DEGLITCH_TIME; + } - // Project the error onto a sine and cosine of the same frequency - // to accumulate the average amplitude - int32_t dT = thisTime - lastHighTime; - float phase = ((float)360 * (float)dT) / relay.Period[axis]; - if(phase >= 360) - phase = 0; - accum_sin += sin_lookup_deg(phase) * error; - accum_cos += cos_lookup_deg(phase) * error; - accumulated ++; + // Project the error onto a sine and cosine of the same frequency + // to accumulate the average amplitude + int32_t dT = thisTime - lastHighTime; + float phase = ((float)360 * (float)dT) / relay.Period[axis]; + if (phase >= 360) { + phase = 0; + } + accum_sin += sin_lookup_deg(phase) * error; + accum_cos += cos_lookup_deg(phase) * error; + accumulated++; - // Make sure we've had enough time since last transition then check for a change in the output - bool time_hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME; + // Make sure we've had enough time since last transition then check for a change in the output + bool time_hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME; - if ( !high && time_hysteresis && error > relaySettings.HysteresisThresh ){ - /* POSITIVE CROSSING DETECTED */ + if (!high && time_hysteresis && error > relaySettings.HysteresisThresh) { + /* POSITIVE CROSSING DETECTED */ - float this_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated; - float this_gain = this_amplitude / relaySettings.Amplitude; + float this_amplitude = 2 * sqrtf(accum_sin * accum_sin + accum_cos * accum_cos) / accumulated; + float this_gain = this_amplitude / relaySettings.Amplitude; - accumulated = 0; - accum_sin = 0; - accum_cos = 0; + accumulated = 0; + accum_sin = 0; + accum_cos = 0; - if(rateRelayRunning[axis] == false) { - rateRelayRunning[axis] = true; - relay.Period[axis] = 200; - relay.Gain[axis] = 0; - } else { - // Low pass filter each amplitude and period - relay.Gain[axis] = relay.Gain[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); - relay.Period[axis] = relay.Period[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); - } - lastHighTime = thisTime; - high = true; - RelayTuningSet(&relay); + if (rateRelayRunning[axis] == false) { + rateRelayRunning[axis] = true; + relay.Period[axis] = 200; + relay.Gain[axis] = 0; + } else { + // Low pass filter each amplitude and period + relay.Gain[axis] = relay.Gain[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); + relay.Period[axis] = relay.Period[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); + } + lastHighTime = thisTime; + high = true; + RelayTuningSet(&relay); + } else if (high && time_hysteresis && error < -relaySettings.HysteresisThresh) { + /* FALLING CROSSING DETECTED */ - } else if ( high && time_hysteresis && error < -relaySettings.HysteresisThresh ) { - /* FALLING CROSSING DETECTED */ + lastLowTime = thisTime; + high = false; + } - lastLowTime = thisTime; - high = false; - - } - - return 0; + return 0; } - diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index d9b17f1c2..3a8a464e8 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -59,18 +59,18 @@ #include "relay_tuning.h" // Private constants -#define MAX_QUEUE_SIZE 1 +#define MAX_QUEUE_SIZE 1 #if defined(PIOS_STABILIZATION_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE +#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE #else -#define STACK_SIZE_BYTES 724 +#define STACK_SIZE_BYTES 724 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY+4) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) #define FAILSAFE_TIMEOUT_MS 30 -enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX}; +enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX }; // Private variables @@ -78,10 +78,10 @@ static xTaskHandle taskHandle; static StabilizationSettingsData settings; static xQueueHandle queue; float gyro_alpha = 0; -float axis_lock_accum[3] = {0,0,0}; -uint8_t max_axis_lock = 0; +float axis_lock_accum[3] = { 0, 0, 0 }; +uint8_t max_axis_lock = 0; uint8_t max_axislock_rate = 0; -float weak_leveling_kp = 0; +float weak_leveling_kp = 0; uint8_t weak_leveling_max = 0; bool lowThrottleZeroIntegral; bool lowThrottleZeroAxis[MAX_AXES]; @@ -89,34 +89,34 @@ float vbar_decay = 0.991f; struct pid pids[PID_MAX]; // Private functions -static void stabilizationTask(void* parameters); +static void stabilizationTask(void *parameters); static float bound(float val, float range); static void ZeroPids(void); -static void SettingsUpdatedCb(UAVObjEvent * ev); +static void SettingsUpdatedCb(UAVObjEvent *ev); /** * Module initialization */ int32_t StabilizationStart() { - // Initialize variables - // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + // Initialize variables + // Create object queue + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - // Listen for updates. - // AttitudeActualConnectQueue(queue); - GyrosConnectQueue(queue); - - StabilizationSettingsConnectCallback(SettingsUpdatedCb); - SettingsUpdatedCb(StabilizationSettingsHandle()); - - // Start main task - xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle); + // Listen for updates. + // AttitudeActualConnectQueue(queue); + GyrosConnectQueue(queue); + + StabilizationSettingsConnectCallback(SettingsUpdatedCb); + SettingsUpdatedCb(StabilizationSettingsHandle()); + + // Start main task + xTaskCreate(stabilizationTask, (signed char *)"Stabilization", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle); #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION); + PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION); #endif - return 0; + return 0; } /** @@ -124,18 +124,18 @@ int32_t StabilizationStart() */ int32_t StabilizationInitialize() { - // Initialize variables - StabilizationSettingsInitialize(); - ActuatorDesiredInitialize(); + // Initialize variables + StabilizationSettingsInitialize(); + ActuatorDesiredInitialize(); #ifdef DIAG_RATEDESIRED - RateDesiredInitialize(); + RateDesiredInitialize(); #endif - // Code required for relay tuning - sin_lookup_initalize(); - RelayTuningSettingsInitialize(); - RelayTuningInitialize(); + // Code required for relay tuning + sin_lookup_initalize(); + RelayTuningSettingsInitialize(); + RelayTuningInitialize(); - return 0; + return 0; } MODULE_INITCALL(StabilizationInitialize, StabilizationStart) @@ -143,271 +143,282 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart) /** * Module task */ -static void stabilizationTask(__attribute__((unused)) void* parameters) +static void stabilizationTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; - - uint32_t timeval = PIOS_DELAY_GetRaw(); - - ActuatorDesiredData actuatorDesired; - StabilizationDesiredData stabDesired; - RateDesiredData rateDesired; - AttitudeActualData attitudeActual; - GyrosData gyrosData; - FlightStatusData flightStatus; - - SettingsUpdatedCb((UAVObjEvent *) NULL); - - // Main task loop - ZeroPids(); - while(1) { - float dT; - + UAVObjEvent ev; + + uint32_t timeval = PIOS_DELAY_GetRaw(); + + ActuatorDesiredData actuatorDesired; + StabilizationDesiredData stabDesired; + RateDesiredData rateDesired; + AttitudeActualData attitudeActual; + GyrosData gyrosData; + FlightStatusData flightStatus; + + SettingsUpdatedCb((UAVObjEvent *)NULL); + + // Main task loop + ZeroPids(); + while (1) { + float dT; + #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); + PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); #endif - - // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe - if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ) - { - AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); - continue; - } - - dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f; - timeval = PIOS_DELAY_GetRaw(); - - FlightStatusGet(&flightStatus); - StabilizationDesiredGet(&stabDesired); - AttitudeActualGet(&attitudeActual); - GyrosGet(&gyrosData); + + // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe + if (xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) { + AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING); + continue; + } + + dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f; + timeval = PIOS_DELAY_GetRaw(); + + FlightStatusGet(&flightStatus); + StabilizationDesiredGet(&stabDesired); + AttitudeActualGet(&attitudeActual); + GyrosGet(&gyrosData); #ifdef DIAG_RATEDESIRED - RateDesiredGet(&rateDesired); + RateDesiredGet(&rateDesired); #endif - + #if defined(PIOS_QUATERNION_STABILIZATION) - // Quaternion calculation of error in each axis. Uses more memory. - float rpy_desired[3]; - float q_desired[4]; - float q_error[4]; - float local_error[3]; - - // Essentially zero errors for anything in rate or none - if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) - rpy_desired[0] = stabDesired.Roll; - else - rpy_desired[0] = attitudeActual.Roll; - - if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) - rpy_desired[1] = stabDesired.Pitch; - else - rpy_desired[1] = attitudeActual.Pitch; - - if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) - rpy_desired[2] = stabDesired.Yaw; - else - rpy_desired[2] = attitudeActual.Yaw; - - RPY2Quaternion(rpy_desired, q_desired); - quat_inverse(q_desired); - quat_mult(q_desired, &attitudeActual.q1, q_error); - quat_inverse(q_error); - Quaternion2RPY(q_error, local_error); - -#else - // Simpler algorithm for CC, less memory - float local_error[3] = {stabDesired.Roll - attitudeActual.Roll, - stabDesired.Pitch - attitudeActual.Pitch, - stabDesired.Yaw - attitudeActual.Yaw}; - // find shortest way - float modulo = fmodf(local_error[2] + 180.0f, 360.0f); - if(modulo<0) - local_error[2] = modulo + 180.0f; - else - local_error[2] = modulo - 180.0f; -#endif + // Quaternion calculation of error in each axis. Uses more memory. + float rpy_desired[3]; + float q_desired[4]; + float q_error[4]; + float local_error[3]; - float gyro_filtered[3]; - gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha); - gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha); - gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha); + // Essentially zero errors for anything in rate or none + if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { + rpy_desired[0] = stabDesired.Roll; + } else { + rpy_desired[0] = attitudeActual.Roll; + } - float *attitudeDesiredAxis = &stabDesired.Roll; - float *actuatorDesiredAxis = &actuatorDesired.Roll; - float *rateDesiredAxis = &rateDesired.Roll; + if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { + rpy_desired[1] = stabDesired.Pitch; + } else { + rpy_desired[1] = attitudeActual.Pitch; + } - ActuatorDesiredGet(&actuatorDesired); + if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { + rpy_desired[2] = stabDesired.Yaw; + } else { + rpy_desired[2] = attitudeActual.Yaw; + } - // A flag to track which stabilization mode each axis is in - static uint8_t previous_mode[MAX_AXES] = {255,255,255}; - bool error = false; + RPY2Quaternion(rpy_desired, q_desired); + quat_inverse(q_desired); + quat_mult(q_desired, &attitudeActual.q1, q_error); + quat_inverse(q_error); + Quaternion2RPY(q_error, local_error); - //Run the selected stabilization algorithm on each axis: - for(uint8_t i=0; i< MAX_AXES; i++) - { - // Check whether this axis mode needs to be reinitialized - bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]); - previous_mode[i] = stabDesired.StabilizationMode[i]; +#else /* if defined(PIOS_QUATERNION_STABILIZATION) */ + // Simpler algorithm for CC, less memory + float local_error[3] = { stabDesired.Roll - attitudeActual.Roll, + stabDesired.Pitch - attitudeActual.Pitch, + stabDesired.Yaw - attitudeActual.Yaw }; + // find shortest way + float modulo = fmodf(local_error[2] + 180.0f, 360.0f); + if (modulo < 0) { + local_error[2] = modulo + 180.0f; + } else { + local_error[2] = modulo - 180.0f; + } +#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */ - // Apply the selected control law - switch(stabDesired.StabilizationMode[i]) - { - case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: - if(reinit) - pids[PID_RATE_ROLL + i].iAccumulator = 0; + float gyro_filtered[3]; + gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha); + gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha); + gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha); - // Store to rate desired variable for storing to UAVO - rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); + float *attitudeDesiredAxis = &stabDesired.Roll; + float *actuatorDesiredAxis = &actuatorDesired.Roll; + float *rateDesiredAxis = &rateDesired.Roll; - // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + ActuatorDesiredGet(&actuatorDesired); - break; + // A flag to track which stabilization mode each axis is in + static uint8_t previous_mode[MAX_AXES] = { 255, 255, 255 }; + bool error = false; - case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: - if(reinit) { - pids[PID_ROLL + i].iAccumulator = 0; - pids[PID_RATE_ROLL + i].iAccumulator = 0; - } + // Run the selected stabilization algorithm on each axis: + for (uint8_t i = 0; i < MAX_AXES; i++) { + // Check whether this axis mode needs to be reinitialized + bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]); + previous_mode[i] = stabDesired.StabilizationMode[i]; - // Compute the outer loop - rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); - rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + // Apply the selected control law + switch (stabDesired.StabilizationMode[i]) { + case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: + if (reinit) { + pids[PID_RATE_ROLL + i].iAccumulator = 0; + } - // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + // Store to rate desired variable for storing to UAVO + rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); - break; + // Compute the inner loop + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); - case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: + break; - // Store for debugging output - rateDesiredAxis[i] = attitudeDesiredAxis[i]; + case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: + if (reinit) { + pids[PID_ROLL + i].iAccumulator = 0; + pids[PID_RATE_ROLL + i].iAccumulator = 0; + } - // Run a virtual flybar stabilization algorithm on this axis - stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings); + // Compute the outer loop + rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); - break; + // Compute the inner loop + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); - case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: - { - if (reinit) - pids[PID_RATE_ROLL + i].iAccumulator = 0; + break; - float weak_leveling = local_error[i] * weak_leveling_kp; - weak_leveling = bound(weak_leveling, weak_leveling_max); + case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: - // Compute desired rate as input biased towards leveling - rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + // Store for debugging output + rateDesiredAxis[i] = attitudeDesiredAxis[i]; - break; - } + // Run a virtual flybar stabilization algorithm on this axis + stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings); - case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: - if (reinit) - pids[PID_RATE_ROLL + i].iAccumulator = 0; + break; - if(fabsf(attitudeDesiredAxis[i]) > max_axislock_rate) { - // While getting strong commands act like rate mode - rateDesiredAxis[i] = attitudeDesiredAxis[i]; - axis_lock_accum[i] = 0; - } else { - // For weaker commands or no command simply attitude lock (almost) on no gyro change - axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT; - axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock); - rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT); - } + case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: + { + if (reinit) { + pids[PID_RATE_ROLL + i].iAccumulator = 0; + } - rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate[i]); + float weak_leveling = local_error[i] * weak_leveling_kp; + weak_leveling = bound(weak_leveling, weak_leveling_max); - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + // Compute desired rate as input biased towards leveling + rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); - break; + break; + } - case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: - // Store to rate desired variable for storing to UAVO - rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); + case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: + if (reinit) { + pids[PID_RATE_ROLL + i].iAccumulator = 0; + } - // Run the relay controller which also estimates the oscillation parameters - stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + if (fabsf(attitudeDesiredAxis[i]) > max_axislock_rate) { + // While getting strong commands act like rate mode + rateDesiredAxis[i] = attitudeDesiredAxis[i]; + axis_lock_accum[i] = 0; + } else { + // For weaker commands or no command simply attitude lock (almost) on no gyro change + axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT; + axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock); + rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT); + } - break; + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate[i]); - case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE: - if(reinit) - pids[PID_ROLL + i].iAccumulator = 0; + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); - // Compute the outer loop like attitude mode - rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); - rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + break; - // Run the relay controller which also estimates the oscillation parameters - stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: + // Store to rate desired variable for storing to UAVO + rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); - break; + // Run the relay controller which also estimates the oscillation parameters + stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); - case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: - actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i], 1.0f); - break; - default: - error = true; - break; - } - } + break; - if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) - stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT); + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE: + if (reinit) { + pids[PID_ROLL + i].iAccumulator = 0; + } + + // Compute the outer loop like attitude mode + rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + + // Run the relay controller which also estimates the oscillation parameters + stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + + break; + + case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: + actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i], 1.0f); + break; + default: + error = true; + break; + } + } + + if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) { + stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT); + } #ifdef DIAG_RATEDESIRED - RateDesiredSet(&rateDesired); + RateDesiredSet(&rateDesired); #endif - // Save dT - actuatorDesired.UpdateTime = dT * 1000; - actuatorDesired.Throttle = stabDesired.Throttle; + // Save dT + actuatorDesired.UpdateTime = dT * 1000; + actuatorDesired.Throttle = stabDesired.Throttle; - // Suppress desired output while disarmed or throttle low, for configured axis - if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || stabDesired.Throttle < 0) { - if (lowThrottleZeroAxis[ROLL]) - actuatorDesired.Roll = 0.0f; + // Suppress desired output while disarmed or throttle low, for configured axis + if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || stabDesired.Throttle < 0) { + if (lowThrottleZeroAxis[ROLL]) { + actuatorDesired.Roll = 0.0f; + } - if (lowThrottleZeroAxis[PITCH]) - actuatorDesired.Pitch = 0.0f; + if (lowThrottleZeroAxis[PITCH]) { + actuatorDesired.Pitch = 0.0f; + } - if (lowThrottleZeroAxis[YAW]) - actuatorDesired.Yaw = 0.0f; - } + if (lowThrottleZeroAxis[YAW]) { + actuatorDesired.Yaw = 0.0f; + } + } - if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) { - ActuatorDesiredSet(&actuatorDesired); - } else { - // Force all axes to reinitialize when engaged - for(uint8_t i=0; i< MAX_AXES; i++) - previous_mode[i] = 255; - } + if (PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) { + ActuatorDesiredSet(&actuatorDesired); + } else { + // Force all axes to reinitialize when engaged + for (uint8_t i = 0; i < MAX_AXES; i++) { + previous_mode[i] = 255; + } + } - if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || - (lowThrottleZeroIntegral && stabDesired.Throttle < 0)) - { - // Force all axes to reinitialize when engaged - for(uint8_t i=0; i< MAX_AXES; i++) - previous_mode[i] = 255; - } + if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || + (lowThrottleZeroIntegral && stabDesired.Throttle < 0)) { + // Force all axes to reinitialize when engaged + for (uint8_t i = 0; i < MAX_AXES; i++) { + previous_mode[i] = 255; + } + } - // Clear or set alarms. Done like this to prevent toggline each cycle - // and hammering system alarms - if (error) - AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_ERROR); - else - AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); - } + // Clear or set alarms. Done like this to prevent toggline each cycle + // and hammering system alarms + if (error) { + AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); + } + } } @@ -416,12 +427,14 @@ static void stabilizationTask(__attribute__((unused)) void* parameters) */ static void ZeroPids(void) { - for(uint32_t i = 0; i < PID_MAX; i++) - pid_zero(&pids[i]); + for (uint32_t i = 0; i < PID_MAX; i++) { + pid_zero(&pids[i]); + } - for(uint8_t i = 0; i < 3; i++) - axis_lock_accum[i] = 0.0f; + for (uint8_t i = 0; i < 3; i++) { + axis_lock_accum[i] = 0.0f; + } } @@ -430,83 +443,84 @@ static void ZeroPids(void) */ static float bound(float val, float range) { - if(val < -range) { - val = -range; - } else if(val > range) { - val = range; - } - return val; + if (val < -range) { + val = -range; + } else if (val > range) { + val = range; + } + return val; } -static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent * ev) +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - StabilizationSettingsGet(&settings); - - // Set the roll rate PID constants - pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], - settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], - pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], - pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]); - - // Set the pitch rate PID constants - pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], - pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], - pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], - pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]); - - // Set the yaw rate PID constants - pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], - pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], - pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], - pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]); - - // Set the roll attitude PI constants - pid_configure(&pids[PID_ROLL], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], - settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0, - pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]); - - // Set the pitch attitude PI constants - pid_configure(&pids[PID_PITCH], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], - pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0, - settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]); - - // Set the yaw attitude PI constants - pid_configure(&pids[PID_YAW], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], - settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0, - settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]); - - // Set up the derivative term - pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); + StabilizationSettingsGet(&settings); - // Maximum deviation to accumulate for axis lock - max_axis_lock = settings.MaxAxisLock; - max_axislock_rate = settings.MaxAxisLockRate; - - // Settings for weak leveling - weak_leveling_kp = settings.WeakLevelingKp; - weak_leveling_max = settings.MaxWeakLevelingRate; - - // Whether to zero the PID integrals while throttle is low - lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; + // Set the roll rate PID constants + pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], + settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], + pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], + pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]); - // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low - lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_ROLL] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; - lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_PITCH] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; - lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_YAW] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + // Set the pitch rate PID constants + pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], + pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], + pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], + pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]); - // The dT has some jitter iteration to iteration that we don't want to - // make thie result unpredictable. Still, it's nicer to specify the constant - // based on a time (in ms) rather than a fixed multiplier. The error between - // update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this - // calculation - const float fakeDt = 0.0025f; - if(settings.GyroTau < 0.0001f) - gyro_alpha = 0; // not trusting this to resolve to 0 - else - gyro_alpha = expf(-fakeDt / settings.GyroTau); - - // Compute time constant for vbar decay term based on a tau - vbar_decay = expf(-fakeDt / settings.VbarTau); + // Set the yaw rate PID constants + pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], + pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], + pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], + pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]); + + // Set the roll attitude PI constants + pid_configure(&pids[PID_ROLL], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], + settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0, + pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]); + + // Set the pitch attitude PI constants + pid_configure(&pids[PID_PITCH], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], + pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0, + settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]); + + // Set the yaw attitude PI constants + pid_configure(&pids[PID_YAW], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], + settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0, + settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]); + + // Set up the derivative term + pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); + + // Maximum deviation to accumulate for axis lock + max_axis_lock = settings.MaxAxisLock; + max_axislock_rate = settings.MaxAxisLockRate; + + // Settings for weak leveling + weak_leveling_kp = settings.WeakLevelingKp; + weak_leveling_max = settings.MaxWeakLevelingRate; + + // Whether to zero the PID integrals while throttle is low + lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; + + // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low + lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_ROLL] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_PITCH] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_YAW] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + + // The dT has some jitter iteration to iteration that we don't want to + // make thie result unpredictable. Still, it's nicer to specify the constant + // based on a time (in ms) rather than a fixed multiplier. The error between + // update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this + // calculation + const float fakeDt = 0.0025f; + if (settings.GyroTau < 0.0001f) { + gyro_alpha = 0; // not trusting this to resolve to 0 + } else { + gyro_alpha = expf(-fakeDt / settings.GyroTau); + } + + // Compute time constant for vbar decay term based on a tau + vbar_decay = expf(-fakeDt / settings.VbarTau); } @@ -514,4 +528,3 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent * ev) * @} * @} */ - diff --git a/flight/modules/Stabilization/virtualflybar.c b/flight/modules/Stabilization/virtualflybar.c index 0b9412081..153615c57 100644 --- a/flight/modules/Stabilization/virtualflybar.c +++ b/flight/modules/Stabilization/virtualflybar.c @@ -35,55 +35,55 @@ #include "stabilization.h" #include "stabilizationsettings.h" -//! Private variables +// ! Private variables static float vbar_integral[MAX_AXES]; static float vbar_decay = 0.991f; -//! Private methods +// ! Private methods static float bound(float val, float range); int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings) { - float gyro_gain = 1.0f; - float kp = 0, ki = 0; + float gyro_gain = 1.0f; + float kp = 0, ki = 0; - if(reinit) - vbar_integral[axis] = 0; + if (reinit) { + vbar_integral[axis] = 0; + } - // Track the angle of the virtual flybar which includes a slow decay - vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT; - vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle); + // Track the angle of the virtual flybar which includes a slow decay + vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT; + vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle); - // Command signal can indicate how much to disregard the gyro feedback (fast flips) - if (settings->VbarGyroSuppress > 0) { - gyro_gain = (1.0f - fabsf(command) * settings->VbarGyroSuppress / 100.0f); - gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain; - } + // Command signal can indicate how much to disregard the gyro feedback (fast flips) + if (settings->VbarGyroSuppress > 0) { + gyro_gain = (1.0f - fabsf(command) * settings->VbarGyroSuppress / 100.0f); + gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain; + } - // Get the settings for the correct axis - switch(axis) - { - case ROLL: - kp = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; - ki = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; - break; - case PITCH: - kp = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; - ki = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; - break; - case YAW: - kp = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; - ki = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; - break; - default: - PIOS_DEBUG_Assert(0); - } + // Get the settings for the correct axis + switch (axis) { + case ROLL: + kp = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; + ki = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + break; + case PITCH: + kp = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; + ki = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + break; + case YAW: + kp = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; + ki = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + break; + default: + PIOS_DEBUG_Assert(0); + } - // Command signal is composed of stick input added to the gyro and virtual flybar - *output = command * settings->VbarSensitivity[axis] - - gyro_gain * (vbar_integral[axis] * ki + gyro * kp); + // Command signal is composed of stick input added to the gyro and virtual flybar + *output = command * settings->VbarSensitivity[axis] - + gyro_gain * (vbar_integral[axis] * ki + gyro * kp); - return 0; + return 0; } /** @@ -93,16 +93,16 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float */ int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT) { - float cy = cosf(DEG2RAD(z_gyro) * dT); - float sy = sinf(DEG2RAD(z_gyro) * dT); + float cy = cosf(DEG2RAD(z_gyro) * dT); + float sy = sinf(DEG2RAD(z_gyro) * dT); - float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0]; - float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0]; + float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0]; + float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0]; - vbar_integral[1] = vbar_pitch; - vbar_integral[0] = vbar_roll; + vbar_integral[1] = vbar_pitch; + vbar_integral[0] = vbar_roll; - return 0; + return 0; } /** @@ -110,10 +110,10 @@ int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT) */ static float bound(float val, float range) { - if(val < -range) { - val = -range; - } else if(val > range) { - val = range; - } - return val; + if (val < -range) { + val = -range; + } else if (val > range) { + val = range; + } + return val; } diff --git a/flight/modules/System/inc/systemmod.h b/flight/modules/System/inc/systemmod.h index c487f4811..bd709f404 100644 --- a/flight/modules/System/inc/systemmod.h +++ b/flight/modules/System/inc/systemmod.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup SystemModule System Module - * @{ + * @{ * * @file systemmod.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 2492e1958..4ee66f54f 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -1,19 +1,19 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @brief The OpenPilot Modules do the majority of the control in OpenPilot. The + * @brief The OpenPilot Modules do the majority of the control in OpenPilot. The * @ref SystemModule "System Module" starts all the other modules that then take care - * of all the telemetry and control algorithms and such. This is done through the @ref PIOS + * of all the telemetry and control algorithms and such. This is done through the @ref PIOS * "PIOS Hardware abstraction layer" which then contains hardware specific implementations * (currently only STM32 supported) * - * @{ + * @{ * @addtogroup SystemModule System Module * @brief Initializes PIOS and other modules runs monitoring * After initializing all the modules (currently selected by Makefile but in * future controlled by configuration on SD card) runs basic monitoring and * alarms. - * @{ + * @{ * * @file systemmod.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -58,20 +58,20 @@ #include -//#define DEBUG_THIS_FILE +// #define DEBUG_THIS_FILE #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(DEBUG_THIS_FILE) -#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, format, ## __VA_ARGS__) +#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, format,##__VA_ARGS__) #else #define DEBUG_MSG(format, ...) #endif // Private constants -#define SYSTEM_UPDATE_PERIOD_MS 1000 -#define LED_BLINK_RATE_HZ 5 +#define SYSTEM_UPDATE_PERIOD_MS 1000 +#define LED_BLINK_RATE_HZ 5 #ifndef IDLE_COUNTS_PER_SEC_AT_NO_LOAD -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 // calibrated by running tests/test_cpuload.c +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 // calibrated by running tests/test_cpuload.c // must be updated if the FreeRTOS or compiler // optimisation options are changed. #endif @@ -82,7 +82,7 @@ #define STACK_SIZE_BYTES 924 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY+1) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types @@ -96,8 +96,8 @@ static bool mallocFailed; static HwSettingsData bootHwSettings; // Private functions -static void objectUpdatedCb(UAVObjEvent * ev); -static void hwSettingsUpdatedCb(UAVObjEvent * ev); +static void objectUpdatedCb(UAVObjEvent *ev); +static void hwSettingsUpdatedCb(UAVObjEvent *ev); #ifdef DIAG_TASKS static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_info *task_info, void *context); #endif @@ -116,7 +116,7 @@ int32_t SystemModStart(void) { // Initialize vars stackOverflow = false; - mallocFailed = false; + mallocFailed = false; // Create system task xTaskCreate(systemTask, (signed char *)"System", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle); // Register task @@ -131,7 +131,6 @@ int32_t SystemModStart(void) */ int32_t SystemModInitialize(void) { - // Must registers objects here for system thread because ObjectManager started in OpenPilotInit SystemSettingsInitialize(); SystemStatsInitialize(); @@ -146,8 +145,9 @@ int32_t SystemModInitialize(void) #endif objectPersistenceQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - if (objectPersistenceQueue == NULL) + if (objectPersistenceQueue == NULL) { return -1; + } SystemModStart(); @@ -158,13 +158,13 @@ MODULE_INITCALL(SystemModInitialize, 0) /** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ -static void systemTask(__attribute__((unused))void *parameters) +static void systemTask(__attribute__((unused)) void *parameters) { - /* start the delayed callback scheduler */ - CallbackSchedulerStart(); + /* start the delayed callback scheduler */ + CallbackSchedulerStart(); - /* create all modules thread */ - MODULE_TASKCREATE_ALL; + /* create all modules thread */ + MODULE_TASKCREATE_ALL; if (mallocFailed) { /* We failed to malloc during task creation, @@ -186,19 +186,19 @@ static void systemTask(__attribute__((unused))void *parameters) // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectQueue(objectPersistenceQueue); - // Load a copy of HwSetting active at boot time - HwSettingsGet(&bootHwSettings); + // Load a copy of HwSetting active at boot time + HwSettingsGet(&bootHwSettings); // Whenever the configuration changes, make sure it is safe to fly HwSettingsConnectCallback(hwSettingsUpdatedCb); #ifdef DIAG_TASKS - TaskInfoData taskInfoData; + TaskInfoData taskInfoData; #endif - // Main system loop - while (1) { - // Update the system statistics - updateStats(); + // Main system loop + while (1) { + // Update the system statistics + updateStats(); // Update the system alarms updateSystemAlarms(); @@ -208,35 +208,35 @@ static void systemTask(__attribute__((unused))void *parameters) #endif #ifdef DIAG_TASKS - // Update the task status object - PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData); - TaskInfoSet(&taskInfoData); + // Update the task status object + PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData); + TaskInfoSet(&taskInfoData); #endif // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF); -#endif /* PIOS_LED_HEARTBEAT */ +#endif /* PIOS_LED_HEARTBEAT */ // Turn on the error LED if an alarm is set -#if defined (PIOS_LED_ALARM) +#if defined(PIOS_LED_ALARM) if (AlarmsHasWarnings()) { PIOS_LED_On(PIOS_LED_ALARM); } else { PIOS_LED_Off(PIOS_LED_ALARM); } -#endif /* PIOS_LED_ALARM */ +#endif /* PIOS_LED_ALARM */ FlightStatusData flightStatus; FlightStatusGet(&flightStatus); UAVObjEvent ev; int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ? - SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) : - SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS; + SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) : + SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS; - if(xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) { + if (xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) { // If object persistence is updated call the callback objectUpdatedCb(&ev); } @@ -246,7 +246,7 @@ static void systemTask(__attribute__((unused))void *parameters) /** * Function called in response to object updates */ -static void objectUpdatedCb(UAVObjEvent * ev) +static void objectUpdatedCb(UAVObjEvent *ev) { ObjectPersistenceData objper; UAVObjHandle obj; @@ -296,8 +296,9 @@ static void objectUpdatedCb(UAVObjEvent * ev) vTaskDelay(10); // Verify saving worked - if (retval == 0) + if (retval == 0) { retval = UAVObjLoad(obj, objper.InstanceID); + } } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) { retval = UAVObjSaveSettings(); } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) { @@ -321,20 +322,20 @@ static void objectUpdatedCb(UAVObjEvent * ev) #if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) retval = PIOS_FLASHFS_Format(0); #else - retval = -1; + retval = -1; #endif } switch (retval) { - case 0: - objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; - ObjectPersistenceSet(&objper); - break; - case -1: - objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR; - ObjectPersistenceSet(&objper); - break; - default: - break; + case 0: + objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; + ObjectPersistenceSet(&objper); + break; + case -1: + objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR; + ObjectPersistenceSet(&objper); + break; + default: + break; } } } @@ -342,9 +343,10 @@ static void objectUpdatedCb(UAVObjEvent * ev) /** * Called whenever hardware settings changed */ -static void hwSettingsUpdatedCb(__attribute__((unused))UAVObjEvent * ev) +static void hwSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { HwSettingsData currentHwSettings; + HwSettingsGet(¤tHwSettings); // check whether the Hw Configuration has changed from the one used at boot time if (memcmp(&bootHwSettings, ¤tHwSettings, sizeof(HwSettingsData)) != 0) { @@ -355,18 +357,19 @@ static void hwSettingsUpdatedCb(__attribute__((unused))UAVObjEvent * ev) #ifdef DIAG_TASKS static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_info *task_info, void *context) { - TaskInfoData *taskData = (TaskInfoData *)context; - // By convention, there is a direct mapping between task monitor task_id's and members - // of the TaskInfoXXXXElem enums - PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM); - taskData->Running[task_id] = task_info->is_running? TASKINFO_RUNNING_TRUE: TASKINFO_RUNNING_FALSE; - taskData->StackRemaining[task_id] = task_info->stack_remaining; - taskData->RunningTime[task_id] = task_info->running_time_percentage; + TaskInfoData *taskData = (TaskInfoData *)context; + + // By convention, there is a direct mapping between task monitor task_id's and members + // of the TaskInfoXXXXElem enums + PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM); + taskData->Running[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE; + taskData->StackRemaining[task_id] = task_info->stack_remaining; + taskData->RunningTime[task_id] = task_info->running_time_percentage; } #endif /** - * Called periodically to update the I2C statistics + * Called periodically to update the I2C statistics */ #ifdef DIAG_I2C_WDG_STATS static void updateI2Cstats() @@ -378,7 +381,7 @@ static void updateI2Cstats() struct pios_i2c_fault_history history; PIOS_I2C_GetDiagnostics(&history, &i2cStats.event_errors); - for(uint8_t i = 0; (i < I2C_LOG_DEPTH) && (i < I2CSTATS_EVENT_LOG_NUMELEM); i++) { + for (uint8_t i = 0; (i < I2C_LOG_DEPTH) && (i < I2CSTATS_EVENT_LOG_NUMELEM); i++) { i2cStats.evirq_log[i] = history.evirq[i]; i2cStats.erirq_log[i] = history.erirq[i]; i2cStats.event_log[i] = history.event[i]; @@ -392,11 +395,12 @@ static void updateI2Cstats() static void updateWDGstats() { WatchdogStatusData watchdogStatus; + watchdogStatus.BootupFlags = PIOS_WDG_GetBootupFlags(); watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags(); WatchdogStatusSet(&watchdogStatus); } -#endif +#endif /* ifdef DIAG_I2C_WDG_STATS */ /** * Called periodically to update the system stats @@ -408,35 +412,28 @@ static uint16_t GetFreeIrqStackSize(void) #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) extern uint32_t _irq_stack_top; extern uint32_t _irq_stack_end; - uint32_t pattern = 0x0000A5A5; - uint32_t *ptr = &_irq_stack_end; + uint32_t pattern = 0x0000A5A5; + uint32_t *ptr = &_irq_stack_end; #if 1 /* the ugly way accurate but takes more time, useful for debugging */ - uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3 ) / 4; + uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3) / 4; - for (i=0; i< stack_size; i++) - { - if (ptr[i] != pattern) - { - i=i*4; + for (i = 0; i < stack_size; i++) { + if (ptr[i] != pattern) { + i = i * 4; break; } } #else /* faster way but not accurate */ - if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern) - { + if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern) { i = IRQSTACK_LIMIT_CRITICAL - 1; - } - else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern) - { + } else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern) { i = IRQSTACK_LIMIT_WARNING - 1; - } - else - { + } else { i = IRQSTACK_LIMIT_WARNING; } #endif -#endif +#endif /* if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) */ return i; } @@ -450,7 +447,7 @@ static void updateStats() // Get stats and update SystemStatsGet(&stats); - stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS; + stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS; #if defined(ARCH_POSIX) || defined(ARCH_WIN32) // POSIX port of FreeRTOS doesn't have xPortGetFreeHeapSize() stats.HeapRemaining = 10240; @@ -468,10 +465,10 @@ static void updateStats() portTickType now = xTaskGetTickCount(); if (now > lastTickCount) { - uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms - stats.CPULoad = 100 - (uint8_t) roundf(100.0f * ((float) idleCounter / ((float) dT / 1000.0f)) / (float) IDLE_COUNTS_PER_SEC_AT_NO_LOAD); - } //else: TickCount has wrapped, do not calc now - lastTickCount = now; + uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms + stats.CPULoad = 100 - (uint8_t)roundf(100.0f * ((float)idleCounter / ((float)dT / 1000.0f)) / (float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD); + } // else: TickCount has wrapped, do not calc now + lastTickCount = now; idleCounterClear = 1; #if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR) @@ -479,12 +476,12 @@ static void updateStats() float temp_voltage = 3.3f * PIOS_ADC_PinGet(3) / ((1 << 12) - 1); const float STM32_TEMP_V25 = 0.76f; /* V */ const float STM32_TEMP_AVG_SLOPE = 2.5f; /* mV/C */ - stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f; + stats.CPUTemp = (temp_voltage - STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f; #else float temp_voltage = 3.3f * PIOS_ADC_PinGet(0) / ((1 << 12) - 1); const float STM32_TEMP_V25 = 1.43f; /* V */ const float STM32_TEMP_AVG_SLOPE = 4.3f; /* mV/C */ - stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f; + stats.CPUTemp = (temp_voltage - STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f; #endif #endif SystemStatsSet(&stats); @@ -498,20 +495,21 @@ static void updateSystemAlarms() SystemStatsData stats; UAVObjStats objStats; EventStats evStats; + SystemStatsGet(&stats); // Check heap, IRQ stack and malloc failures if (mallocFailed || (stats.HeapRemaining < HEAP_LIMIT_CRITICAL) #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) - || (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL) + || (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL) #endif - ) { + ) { AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL); } else if ((stats.HeapRemaining < HEAP_LIMIT_WARNING) #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) - || (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING) + || (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING) #endif - ) { + ) { AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY); @@ -547,12 +545,11 @@ static void updateSystemAlarms() if (objStats.lastCallbackErrorID || objStats.lastQueueErrorID || evStats.lastErrorID) { SystemStatsData sysStats; SystemStatsGet(&sysStats); - sysStats.EventSystemWarningID = evStats.lastErrorID; + sysStats.EventSystemWarningID = evStats.lastErrorID; sysStats.ObjectManagerCallbackID = objStats.lastCallbackErrorID; - sysStats.ObjectManagerQueueID = objStats.lastQueueErrorID; + sysStats.ObjectManagerQueueID = objStats.lastQueueErrorID; SystemStatsSet(&sysStats); } - } /** @@ -573,13 +570,15 @@ void vApplicationIdleHook(void) * Called by the RTOS when a stack overflow is detected. */ #define DEBUG_STACK_OVERFLOW 0 -void vApplicationStackOverflowHook(__attribute__((unused))xTaskHandle * pxTask, - __attribute__((unused))signed portCHAR * pcTaskName) +void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask, + __attribute__((unused)) signed portCHAR *pcTaskName) { stackOverflow = true; #if DEBUG_STACK_OVERFLOW static volatile bool wait_here = true; - while(wait_here); + while (wait_here) { + ; + } wait_here = true; #endif } @@ -593,7 +592,9 @@ void vApplicationMallocFailedHook(void) mallocFailed = true; #if DEBUG_MALLOC_FAILURES static volatile bool wait_here = true; - while(wait_here); + while (wait_here) { + ; + } wait_here = true; #endif } diff --git a/flight/modules/Telemetry/inc/telemetry.h b/flight/modules/Telemetry/inc/telemetry.h index b4e449c9c..c3a19cb95 100644 --- a/flight/modules/Telemetry/inc/telemetry.h +++ b/flight/modules/Telemetry/inc/telemetry.h @@ -1,18 +1,18 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup TelemetryModule Telemetry Module * @brief Main telemetry module * Starts three tasks (RX, TX, and priority TX) that watch event queues * and handle all the telemetry of the UAVobjects - * @{ + * @{ * * @file telemetry.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Include file of the telemetry module. - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and * objects. * @see The GNU Public License (GPL) Version 3 * @@ -41,6 +41,6 @@ int32_t TelemetryInitialize(void); #endif // TELEMETRY_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 5dc576abd..a2abfffe5 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -1,12 +1,12 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup TelemetryModule Telemetry Module * @brief Main telemetry module * Starts three tasks (RX, TX, and priority TX) that watch event queues * and handle all the telemetry of the UAVobjects - * @{ + * @{ * * @file telemetry.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -40,15 +40,15 @@ #include "taskinfo.h" // Private constants -#define MAX_QUEUE_SIZE TELEM_QUEUE_SIZE -#define STACK_SIZE_BYTES PIOS_TELEM_STACK_SIZE -#define TASK_PRIORITY_RX (tskIDLE_PRIORITY + 2) -#define TASK_PRIORITY_TX (tskIDLE_PRIORITY + 2) -#define TASK_PRIORITY_TXPRI (tskIDLE_PRIORITY + 2) -#define REQ_TIMEOUT_MS 250 -#define MAX_RETRIES 2 +#define MAX_QUEUE_SIZE TELEM_QUEUE_SIZE +#define STACK_SIZE_BYTES PIOS_TELEM_STACK_SIZE +#define TASK_PRIORITY_RX (tskIDLE_PRIORITY + 2) +#define TASK_PRIORITY_TX (tskIDLE_PRIORITY + 2) +#define TASK_PRIORITY_TXPRI (tskIDLE_PRIORITY + 2) +#define REQ_TIMEOUT_MS 250 +#define MAX_RETRIES 2 #define STATS_UPDATE_PERIOD_MS 4000 -#define CONNECTION_TIMEOUT_MS 8000 +#define CONNECTION_TIMEOUT_MS 8000 // Private types @@ -74,11 +74,11 @@ static UAVTalkConnection uavTalkCon; // Private functions static void telemetryTxTask(void *parameters); static void telemetryRxTask(void *parameters); -static int32_t transmitData(uint8_t * data, int32_t length); +static int32_t transmitData(uint8_t *data, int32_t length); static void registerObject(UAVObjHandle obj); static void updateObject(UAVObjHandle obj, int32_t eventType); static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs); -static void processObjEvent(UAVObjEvent * ev); +static void processObjEvent(UAVObjEvent *ev); static void updateTelemetryStats(); static void gcsTelemetryStatsUpdated(); static void updateSettings(); @@ -91,24 +91,24 @@ static uint32_t getComPort(); */ int32_t TelemetryStart(void) { - // Process all registered objects and connect queue for updates - UAVObjIterate(®isterObject); - - // Listen to objects of interest - GCSTelemetryStatsConnectQueue(priorityQueue); - - // Start telemetry tasks - xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle); - xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle); + // Process all registered objects and connect queue for updates + UAVObjIterate(®isterObject); + + // Listen to objects of interest + GCSTelemetryStatsConnectQueue(priorityQueue); + + // Start telemetry tasks + xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle); + xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle); #if defined(PIOS_TELEM_PRIORITY_QUEUE) - xTaskCreate(telemetryTxPriTask, (signed char *)"TelPriTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle); + xTaskCreate(telemetryTxPriTask, (signed char *)"TelPriTx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle); #endif - return 0; + return 0; } /** @@ -118,34 +118,34 @@ int32_t TelemetryStart(void) */ int32_t TelemetryInitialize(void) { - FlightTelemetryStatsInitialize(); - GCSTelemetryStatsInitialize(); + FlightTelemetryStatsInitialize(); + GCSTelemetryStatsInitialize(); - // Initialize vars - timeOfLastObjectUpdate = 0; + // Initialize vars + timeOfLastObjectUpdate = 0; - // Create object queues - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + // Create object queues + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); #if defined(PIOS_TELEM_PRIORITY_QUEUE) - priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); #endif - // Update telemetry settings - telemetryPort = PIOS_COM_TELEM_RF; - HwSettingsInitialize(); - updateSettings(); - - // Initialise UAVTalk - uavTalkCon = UAVTalkInitialize(&transmitData); - - // Create periodic event that will be used to update the telemetry stats - txErrors = 0; - txRetries = 0; - UAVObjEvent ev; - memset(&ev, 0, sizeof(UAVObjEvent)); - EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS); + // Update telemetry settings + telemetryPort = PIOS_COM_TELEM_RF; + HwSettingsInitialize(); + updateSettings(); - return 0; + // Initialise UAVTalk + uavTalkCon = UAVTalkInitialize(&transmitData); + + // Create periodic event that will be used to update the telemetry stats + txErrors = 0; + txRetries = 0; + UAVObjEvent ev; + memset(&ev, 0, sizeof(UAVObjEvent)); + EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS); + + return 0; } MODULE_INITCALL(TelemetryInitialize, TelemetryStart) @@ -157,31 +157,31 @@ MODULE_INITCALL(TelemetryInitialize, TelemetryStart) */ static void registerObject(UAVObjHandle obj) { - if (UAVObjIsMetaobject(obj)) { - /* Only connect change notifications for meta objects. No periodic updates */ - UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES); - return; - } else { - UAVObjMetadata metadata; - UAVObjUpdateMode updateMode; - UAVObjGetMetadata(obj, &metadata); - updateMode = UAVObjGetTelemetryUpdateMode(&metadata); + if (UAVObjIsMetaobject(obj)) { + /* Only connect change notifications for meta objects. No periodic updates */ + UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES); + return; + } else { + UAVObjMetadata metadata; + UAVObjUpdateMode updateMode; + UAVObjGetMetadata(obj, &metadata); + updateMode = UAVObjGetTelemetryUpdateMode(&metadata); - /* Only create a periodic event for objects that are periodic */ - if ((updateMode == UPDATEMODE_PERIODIC) || - (updateMode == UPDATEMODE_THROTTLED)) { - // Setup object for periodic updates - UAVObjEvent ev = { - .obj = obj, - .instId = UAVOBJ_ALL_INSTANCES, - .event = EV_UPDATED_PERIODIC, - }; - EventPeriodicQueueCreate(&ev, queue, 0); - } + /* Only create a periodic event for objects that are periodic */ + if ((updateMode == UPDATEMODE_PERIODIC) || + (updateMode == UPDATEMODE_THROTTLED)) { + // Setup object for periodic updates + UAVObjEvent ev = { + .obj = obj, + .instId = UAVOBJ_ALL_INSTANCES, + .event = EV_UPDATED_PERIODIC, + }; + EventPeriodicQueueCreate(&ev, queue, 0); + } - // Setup object for telemetry updates - updateObject(obj, EV_NONE); - } + // Setup object for telemetry updates + updateObject(obj, EV_NONE); + } } /** @@ -190,118 +190,119 @@ static void registerObject(UAVObjHandle obj) */ static void updateObject(UAVObjHandle obj, int32_t eventType) { - UAVObjMetadata metadata; - UAVObjUpdateMode updateMode; - int32_t eventMask; + UAVObjMetadata metadata; + UAVObjUpdateMode updateMode; + int32_t eventMask; - if (UAVObjIsMetaobject(obj)) { - /* This function updates the periodic updates for the object. - * Meta Objects cannot have periodic updates. - */ - PIOS_Assert(false); - return; - } + if (UAVObjIsMetaobject(obj)) { + /* This function updates the periodic updates for the object. + * Meta Objects cannot have periodic updates. + */ + PIOS_Assert(false); + return; + } - // Get metadata - UAVObjGetMetadata(obj, &metadata); - updateMode = UAVObjGetTelemetryUpdateMode(&metadata); + // Get metadata + UAVObjGetMetadata(obj, &metadata); + updateMode = UAVObjGetTelemetryUpdateMode(&metadata); - // Setup object depending on update mode - switch (updateMode) { - case UPDATEMODE_PERIODIC: - // Set update period - setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); - // Connect queue - eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - UAVObjConnectQueue(obj, priorityQueue, eventMask); - break; - case UPDATEMODE_ONCHANGE: - // Set update period - setUpdatePeriod(obj, 0); - // Connect queue - eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - UAVObjConnectQueue(obj, priorityQueue, eventMask); - break; - case UPDATEMODE_THROTTLED: - if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) { - // If we received a periodic update, we can change back to update on change - eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - // Set update period on initialization and metadata change - if (eventType == EV_NONE) - setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); - } else { - // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates - eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - } - UAVObjConnectQueue(obj, priorityQueue, eventMask); - break; - case UPDATEMODE_MANUAL: - // Set update period - setUpdatePeriod(obj, 0); - // Connect queue - eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ; - UAVObjConnectQueue(obj, priorityQueue, eventMask); - break; - } + // Setup object depending on update mode + switch (updateMode) { + case UPDATEMODE_PERIODIC: + // Set update period + setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); + // Connect queue + eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ; + UAVObjConnectQueue(obj, priorityQueue, eventMask); + break; + case UPDATEMODE_ONCHANGE: + // Set update period + setUpdatePeriod(obj, 0); + // Connect queue + eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; + UAVObjConnectQueue(obj, priorityQueue, eventMask); + break; + case UPDATEMODE_THROTTLED: + if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) { + // If we received a periodic update, we can change back to update on change + eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; + // Set update period on initialization and metadata change + if (eventType == EV_NONE) { + setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); + } + } else { + // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates + eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ; + } + UAVObjConnectQueue(obj, priorityQueue, eventMask); + break; + case UPDATEMODE_MANUAL: + // Set update period + setUpdatePeriod(obj, 0); + // Connect queue + eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ; + UAVObjConnectQueue(obj, priorityQueue, eventMask); + break; + } } /** * Processes queue events */ -static void processObjEvent(UAVObjEvent * ev) +static void processObjEvent(UAVObjEvent *ev) { - UAVObjMetadata metadata; - UAVObjUpdateMode updateMode; - FlightTelemetryStatsData flightStats; - int32_t retries; - int32_t success; + UAVObjMetadata metadata; + UAVObjUpdateMode updateMode; + FlightTelemetryStatsData flightStats; + int32_t retries; + int32_t success; - if (ev->obj == 0) { - updateTelemetryStats(); - } else if (ev->obj == GCSTelemetryStatsHandle()) { - gcsTelemetryStatsUpdated(); - } else { - FlightTelemetryStatsGet(&flightStats); - // Get object metadata - UAVObjGetMetadata(ev->obj, &metadata); - updateMode = UAVObjGetTelemetryUpdateMode(&metadata); + if (ev->obj == 0) { + updateTelemetryStats(); + } else if (ev->obj == GCSTelemetryStatsHandle()) { + gcsTelemetryStatsUpdated(); + } else { + FlightTelemetryStatsGet(&flightStats); + // Get object metadata + UAVObjGetMetadata(ev->obj, &metadata); + updateMode = UAVObjGetTelemetryUpdateMode(&metadata); - // Act on event - retries = 0; - success = -1; - if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) { - // Send update to GCS (with retries) - while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout - ++retries; - } - // Update stats - txRetries += (retries - 1); - if (success == -1) { - ++txErrors; - } - } else if (ev->event == EV_UPDATE_REQ) { - // Request object update from GCS (with retries) - while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout - ++retries; - } - // Update stats - txRetries += (retries - 1); - if (success == -1) { - ++txErrors; - } - } - // If this is a metaobject then make necessary telemetry updates - if (UAVObjIsMetaobject(ev->obj)) { - updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); // linked object will be the actual object the metadata are for - } else { - if (updateMode == UPDATEMODE_THROTTLED) { - // If this is UPDATEMODE_THROTTLED, the event mask changes on every event. - updateObject(ev->obj, ev->event); - } - } - } + // Act on event + retries = 0; + success = -1; + if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) { + // Send update to GCS (with retries) + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout + ++retries; + } + // Update stats + txRetries += (retries - 1); + if (success == -1) { + ++txErrors; + } + } else if (ev->event == EV_UPDATE_REQ) { + // Request object update from GCS (with retries) + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout + ++retries; + } + // Update stats + txRetries += (retries - 1); + if (success == -1) { + ++txErrors; + } + } + // If this is a metaobject then make necessary telemetry updates + if (UAVObjIsMetaobject(ev->obj)) { + updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); // linked object will be the actual object the metadata are for + } else { + if (updateMode == UPDATEMODE_THROTTLED) { + // If this is UPDATEMODE_THROTTLED, the event mask changes on every event. + updateObject(ev->obj, ev->event); + } + } + } } /** @@ -309,16 +310,16 @@ static void processObjEvent(UAVObjEvent * ev) */ static void telemetryTxTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; + UAVObjEvent ev; - // Loop forever - while (1) { - // Wait for queue message - if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) { - // Process event - processObjEvent(&ev); - } - } + // Loop forever + while (1) { + // Wait for queue message + if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) { + // Process event + processObjEvent(&ev); + } + } } /** @@ -327,16 +328,16 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) #if defined(PIOS_TELEM_PRIORITY_QUEUE) static void telemetryTxPriTask(__attribute__((unused)) void *parameters) { - UAVObjEvent ev; + UAVObjEvent ev; - // Loop forever - while (1) { - // Wait for queue message - if (xQueueReceive(priorityQueue, &ev, portMAX_DELAY) == pdTRUE) { - // Process event - processObjEvent(&ev); - } - } + // Loop forever + while (1) { + // Wait for queue message + if (xQueueReceive(priorityQueue, &ev, portMAX_DELAY) == pdTRUE) { + // Process event + processObjEvent(&ev); + } + } } #endif @@ -345,26 +346,25 @@ static void telemetryTxPriTask(__attribute__((unused)) void *parameters) */ static void telemetryRxTask(__attribute__((unused)) void *parameters) { + // Task loop + while (1) { + uint32_t inputPort = getComPort(); - // Task loop - while (1) { - uint32_t inputPort = getComPort(); + if (inputPort) { + // Block until data are available + uint8_t serial_data[1]; + uint16_t bytes_to_process; - if (inputPort) { - // Block until data are available - uint8_t serial_data[1]; - uint16_t bytes_to_process; - - bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500); - if (bytes_to_process > 0) { - for (uint8_t i = 0; i < bytes_to_process; i++) { - UAVTalkProcessInputStream(uavTalkCon,serial_data[i]); - } - } - } else { - vTaskDelay(5); - } - } + bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500); + if (bytes_to_process > 0) { + for (uint8_t i = 0; i < bytes_to_process; i++) { + UAVTalkProcessInputStream(uavTalkCon, serial_data[i]); + } + } + } else { + vTaskDelay(5); + } + } } /** @@ -374,14 +374,15 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) * \return -1 on failure * \return number of bytes transmitted on success */ -static int32_t transmitData(uint8_t * data, int32_t length) +static int32_t transmitData(uint8_t *data, int32_t length) { - uint32_t outputPort = getComPort(); + uint32_t outputPort = getComPort(); - if (outputPort) - return PIOS_COM_SendBuffer(outputPort, data, length); + if (outputPort) { + return PIOS_COM_SendBuffer(outputPort, data, length); + } - return -1; + return -1; } /** @@ -393,13 +394,13 @@ static int32_t transmitData(uint8_t * data, int32_t length) */ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs) { - UAVObjEvent ev; + UAVObjEvent ev; - // Add object for periodic updates - ev.obj = obj; - ev.instId = UAVOBJ_ALL_INSTANCES; - ev.event = EV_UPDATED_PERIODIC; - return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs); + // Add object for periodic updates + ev.obj = obj; + ev.instId = UAVOBJ_ALL_INSTANCES; + ev.event = EV_UPDATED_PERIODIC; + return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs); } /** @@ -409,13 +410,14 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs) */ static void gcsTelemetryStatsUpdated() { - FlightTelemetryStatsData flightStats; - GCSTelemetryStatsData gcsStats; - FlightTelemetryStatsGet(&flightStats); - GCSTelemetryStatsGet(&gcsStats); - if (flightStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED) { - updateTelemetryStats(); - } + FlightTelemetryStatsData flightStats; + GCSTelemetryStatsData gcsStats; + + FlightTelemetryStatsGet(&flightStats); + GCSTelemetryStatsGet(&gcsStats); + if (flightStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED) { + updateTelemetryStats(); + } } /** @@ -423,89 +425,89 @@ static void gcsTelemetryStatsUpdated() */ static void updateTelemetryStats() { - UAVTalkStats utalkStats; - FlightTelemetryStatsData flightStats; - GCSTelemetryStatsData gcsStats; - uint8_t forceUpdate; - uint8_t connectionTimeout; - uint32_t timeNow; + UAVTalkStats utalkStats; + FlightTelemetryStatsData flightStats; + GCSTelemetryStatsData gcsStats; + uint8_t forceUpdate; + uint8_t connectionTimeout; + uint32_t timeNow; - // Get stats - UAVTalkGetStats(uavTalkCon, &utalkStats); - UAVTalkResetStats(uavTalkCon); + // Get stats + UAVTalkGetStats(uavTalkCon, &utalkStats); + UAVTalkResetStats(uavTalkCon); - // Get object data - FlightTelemetryStatsGet(&flightStats); - GCSTelemetryStatsGet(&gcsStats); + // Get object data + FlightTelemetryStatsGet(&flightStats); + GCSTelemetryStatsGet(&gcsStats); - // Update stats object - if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { - flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); - flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); - flightStats.RxFailures += utalkStats.rxErrors; - flightStats.TxFailures += txErrors; - flightStats.TxRetries += txRetries; - txErrors = 0; - txRetries = 0; - } else { - flightStats.RxDataRate = 0; - flightStats.TxDataRate = 0; - flightStats.RxFailures = 0; - flightStats.TxFailures = 0; - flightStats.TxRetries = 0; - txErrors = 0; - txRetries = 0; - } + // Update stats object + if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { + flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); + flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); + flightStats.RxFailures += utalkStats.rxErrors; + flightStats.TxFailures += txErrors; + flightStats.TxRetries += txRetries; + txErrors = 0; + txRetries = 0; + } else { + flightStats.RxDataRate = 0; + flightStats.TxDataRate = 0; + flightStats.RxFailures = 0; + flightStats.TxFailures = 0; + flightStats.TxRetries = 0; + txErrors = 0; + txRetries = 0; + } - // Check for connection timeout - timeNow = xTaskGetTickCount() * portTICK_RATE_MS; - if (utalkStats.rxObjects > 0) { - timeOfLastObjectUpdate = timeNow; - } - if ((timeNow - timeOfLastObjectUpdate) > CONNECTION_TIMEOUT_MS) { - connectionTimeout = 1; - } else { - connectionTimeout = 0; - } + // Check for connection timeout + timeNow = xTaskGetTickCount() * portTICK_RATE_MS; + if (utalkStats.rxObjects > 0) { + timeOfLastObjectUpdate = timeNow; + } + if ((timeNow - timeOfLastObjectUpdate) > CONNECTION_TIMEOUT_MS) { + connectionTimeout = 1; + } else { + connectionTimeout = 0; + } - // Update connection state - forceUpdate = 1; - if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED) { - // Wait for connection request - if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_HANDSHAKEREQ) { - flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK; - } - } else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK) { - // Wait for connection - if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_CONNECTED) { - flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED; - } else if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_DISCONNECTED) { - flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED; - } - } else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { - if (gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED || connectionTimeout) { - flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED; - } else { - forceUpdate = 0; - } - } else { - flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED; - } + // Update connection state + forceUpdate = 1; + if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED) { + // Wait for connection request + if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_HANDSHAKEREQ) { + flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK; + } + } else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK) { + // Wait for connection + if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_CONNECTED) { + flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED; + } else if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_DISCONNECTED) { + flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED; + } + } else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { + if (gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED || connectionTimeout) { + flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED; + } else { + forceUpdate = 0; + } + } else { + flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED; + } - // Update the telemetry alarm - if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { - AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR); - } + // Update the telemetry alarm + if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { + AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR); + } - // Update object - FlightTelemetryStatsSet(&flightStats); + // Update object + FlightTelemetryStatsSet(&flightStats); - // Force telemetry update if not connected - if (forceUpdate) { - FlightTelemetryStatsUpdated(); - } + // Force telemetry update if not connected + if (forceUpdate) { + FlightTelemetryStatsUpdated(); + } } /** @@ -517,55 +519,56 @@ static void updateTelemetryStats() */ static void updateSettings() { - - if (telemetryPort) { - // Retrieve settings - uint8_t speed; - HwSettingsTelemetrySpeedGet(&speed); + if (telemetryPort) { + // Retrieve settings + uint8_t speed; + HwSettingsTelemetrySpeedGet(&speed); - // Set port speed - switch (speed) { - case HWSETTINGS_TELEMETRYSPEED_2400: - PIOS_COM_ChangeBaud(telemetryPort, 2400); - break; - case HWSETTINGS_TELEMETRYSPEED_4800: - PIOS_COM_ChangeBaud(telemetryPort, 4800); - break; - case HWSETTINGS_TELEMETRYSPEED_9600: - PIOS_COM_ChangeBaud(telemetryPort, 9600); - break; - case HWSETTINGS_TELEMETRYSPEED_19200: - PIOS_COM_ChangeBaud(telemetryPort, 19200); - break; - case HWSETTINGS_TELEMETRYSPEED_38400: - PIOS_COM_ChangeBaud(telemetryPort, 38400); - break; - case HWSETTINGS_TELEMETRYSPEED_57600: - PIOS_COM_ChangeBaud(telemetryPort, 57600); - break; - case HWSETTINGS_TELEMETRYSPEED_115200: - PIOS_COM_ChangeBaud(telemetryPort, 115200); - break; - } - } + // Set port speed + switch (speed) { + case HWSETTINGS_TELEMETRYSPEED_2400: + PIOS_COM_ChangeBaud(telemetryPort, 2400); + break; + case HWSETTINGS_TELEMETRYSPEED_4800: + PIOS_COM_ChangeBaud(telemetryPort, 4800); + break; + case HWSETTINGS_TELEMETRYSPEED_9600: + PIOS_COM_ChangeBaud(telemetryPort, 9600); + break; + case HWSETTINGS_TELEMETRYSPEED_19200: + PIOS_COM_ChangeBaud(telemetryPort, 19200); + break; + case HWSETTINGS_TELEMETRYSPEED_38400: + PIOS_COM_ChangeBaud(telemetryPort, 38400); + break; + case HWSETTINGS_TELEMETRYSPEED_57600: + PIOS_COM_ChangeBaud(telemetryPort, 57600); + break; + case HWSETTINGS_TELEMETRYSPEED_115200: + PIOS_COM_ChangeBaud(telemetryPort, 115200); + break; + } + } } /** - * Determine input/output com port as highest priority available + * Determine input/output com port as highest priority available */ -static uint32_t getComPort() { +static uint32_t getComPort() +{ #if defined(PIOS_INCLUDE_USB) - if ( PIOS_COM_Available(PIOS_COM_TELEM_USB) ) - return PIOS_COM_TELEM_USB; - else + if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) { + return PIOS_COM_TELEM_USB; + } else #endif /* PIOS_INCLUDE_USB */ - if ( PIOS_COM_Available(telemetryPort) ) - return telemetryPort; - else - return 0; + if (PIOS_COM_Available(telemetryPort)) { + return telemetryPort; + } else { + return 0; + } } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/TxPID/inc/txpid.h b/flight/modules/TxPID/inc/txpid.h index c5a2e4fa3..44e61cbc1 100644 --- a/flight/modules/TxPID/inc/txpid.h +++ b/flight/modules/TxPID/inc/txpid.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules - * @{ + * @{ * @addtogroup TxPIDModule TxPID Module - * @{ + * @{ * * @file txpid.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. @@ -37,6 +37,6 @@ int32_t TxPIDInitialize(void); #endif // TXPID_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/modules/TxPID/txpid.c b/flight/modules/TxPID/txpid.c index 4c784894b..9f9c5f2cf 100644 --- a/flight/modules/TxPID/txpid.c +++ b/flight/modules/TxPID/txpid.c @@ -62,8 +62,8 @@ // // Configuration // -#define SAMPLE_PERIOD_MS 200 -#define TELEMETRY_UPDATE_PERIOD_MS 0 // 0 = update on change (default) +#define SAMPLE_PERIOD_MS 200 +#define TELEMETRY_UPDATE_PERIOD_MS 0 // 0 = update on change (default) // Sanity checks #if (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_INPUTS_NUMELEM) || \ @@ -77,7 +77,7 @@ // Private variables // Private functions -static void updatePIDs(UAVObjEvent* ev); +static void updatePIDs(UAVObjEvent *ev); static uint8_t update(float *var, float val); static float scale(float val, float inMin, float inMax, float outMin, float outMax); @@ -87,55 +87,55 @@ static float scale(float val, float inMin, float inMax, float outMin, float outM */ int32_t TxPIDInitialize(void) { - bool txPIDEnabled; - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + bool txPIDEnabled; + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsInitialize(); - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsInitialize(); + HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_TXPID] == HWSETTINGS_OPTIONALMODULES_ENABLED) - txPIDEnabled = true; - else - txPIDEnabled = false; + if (optionalModules[HWSETTINGS_OPTIONALMODULES_TXPID] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + txPIDEnabled = true; + } else { + txPIDEnabled = false; + } - if (txPIDEnabled) { + if (txPIDEnabled) { + TxPIDSettingsInitialize(); + AccessoryDesiredInitialize(); - TxPIDSettingsInitialize(); - AccessoryDesiredInitialize(); - - UAVObjEvent ev = { - .obj = AccessoryDesiredHandle(), - .instId = 0, - .event = 0, - }; - EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS); + UAVObjEvent ev = { + .obj = AccessoryDesiredHandle(), + .instId = 0, + .event = 0, + }; + EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS); #if (TELEMETRY_UPDATE_PERIOD_MS != 0) - // Change StabilizationSettings update rate from OnChange to periodic - // to prevent telemetry link flooding with frequent updates in case of - // control channel jitter. - // Warning: saving to flash with this code active will change the - // StabilizationSettings update rate permanently. Use Metadata via - // browser to reset to defaults (telemetryAcked=true, OnChange). - UAVObjMetadata metadata; - StabilizationSettingsInitialize(); - StabilizationSettingsGetMetadata(&metadata); - metadata.telemetryAcked = 0; - metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC; - metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS; - StabilizationSettingsSetMetadata(&metadata); + // Change StabilizationSettings update rate from OnChange to periodic + // to prevent telemetry link flooding with frequent updates in case of + // control channel jitter. + // Warning: saving to flash with this code active will change the + // StabilizationSettings update rate permanently. Use Metadata via + // browser to reset to defaults (telemetryAcked=true, OnChange). + UAVObjMetadata metadata; + StabilizationSettingsInitialize(); + StabilizationSettingsGetMetadata(&metadata); + metadata.telemetryAcked = 0; + metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC; + metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS; + StabilizationSettingsSetMetadata(&metadata); #endif - return 0; - } + return 0; + } - return -1; + return -1; } /* stub: module has no module thread */ int32_t TxPIDStart(void) { - return 0; + return 0; } MODULE_INITCALL(TxPIDInitialize, TxPIDStart) @@ -143,148 +143,151 @@ MODULE_INITCALL(TxPIDInitialize, TxPIDStart) /** * Update PIDs callback function */ -static void updatePIDs(UAVObjEvent* ev) +static void updatePIDs(UAVObjEvent *ev) { - if (ev->obj != AccessoryDesiredHandle()) - return; + if (ev->obj != AccessoryDesiredHandle()) { + return; + } - TxPIDSettingsData inst; - TxPIDSettingsGet(&inst); + TxPIDSettingsData inst; + TxPIDSettingsGet(&inst); - if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER) - return; + if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER) { + return; + } - uint8_t armed; - FlightStatusArmedGet(&armed); - if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) && - (armed == FLIGHTSTATUS_ARMED_DISARMED)) - return; + uint8_t armed; + FlightStatusArmedGet(&armed); + if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) && + (armed == FLIGHTSTATUS_ARMED_DISARMED)) { + return; + } - StabilizationSettingsData stab; - StabilizationSettingsGet(&stab); - AccessoryDesiredData accessory; + StabilizationSettingsData stab; + StabilizationSettingsGet(&stab); + AccessoryDesiredData accessory; - uint8_t needsUpdate = 0; + uint8_t needsUpdate = 0; - // Loop through every enabled instance - for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) { - if (inst.PIDs[i] != TXPIDSETTINGS_PIDS_DISABLED) { + // Loop through every enabled instance + for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) { + if (inst.PIDs[i] != TXPIDSETTINGS_PIDS_DISABLED) { + float value; + if (inst.Inputs[i] == TXPIDSETTINGS_INPUTS_THROTTLE) { + ManualControlCommandThrottleGet(&value); + value = scale(value, + inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MIN], + inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX], + inst.MinPID[i], inst.MaxPID[i]); + } else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) { + value = scale(accessory.AccessoryVal, -1.0f, 1.0f, inst.MinPID[i], inst.MaxPID[i]); + } else { + continue; + } - float value; - if (inst.Inputs[i] == TXPIDSETTINGS_INPUTS_THROTTLE) { - ManualControlCommandThrottleGet(&value); - value = scale(value, - inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MIN], - inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX], - inst.MinPID[i], inst.MaxPID[i]); - } else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) { - value = scale(accessory.AccessoryVal, -1.0f, 1.0f, inst.MinPID[i], inst.MaxPID[i]); - } else { - continue; - } - - switch (inst.PIDs[i]) { - case TXPIDSETTINGS_PIDS_ROLLRATEKP: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); - break; - case TXPIDSETTINGS_PIDS_ROLLRATEKI: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); - break; - case TXPIDSETTINGS_PIDS_ROLLRATEKD: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); - break; - case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); - break; - case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); - break; - case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_PITCHRATEKP: - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); - break; - case TXPIDSETTINGS_PIDS_PITCHRATEKI: - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); - break; - case TXPIDSETTINGS_PIDS_PITCHRATEKD: - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); - break; - case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT: - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP: - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); - break; - case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI: - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); - break; - case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT: - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); - break; - case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); - break; - case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); - break; - case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value); - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); - break; - case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); - break; - case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value); - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_YAWRATEKP: - needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value); - break; - case TXPIDSETTINGS_PIDS_YAWRATEKI: - needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value); - break; - case TXPIDSETTINGS_PIDS_YAWRATEKD: - needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value); - break; - case TXPIDSETTINGS_PIDS_YAWRATEILIMIT: - needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_YAWATTITUDEKP: - needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value); - break; - case TXPIDSETTINGS_PIDS_YAWATTITUDEKI: - needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value); - break; - case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT: - needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], value); - break; - case TXPIDSETTINGS_PIDS_GYROTAU: - needsUpdate |= update(&stab.GyroTau, value); - break; - default: - PIOS_Assert(0); - } - } - } - if (needsUpdate) - StabilizationSettingsSet(&stab); + switch (inst.PIDs[i]) { + case TXPIDSETTINGS_PIDS_ROLLRATEKP: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); + break; + case TXPIDSETTINGS_PIDS_ROLLRATEKI: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); + break; + case TXPIDSETTINGS_PIDS_ROLLRATEKD: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); + break; + case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); + break; + case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); + break; + case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_PITCHRATEKP: + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); + break; + case TXPIDSETTINGS_PIDS_PITCHRATEKI: + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); + break; + case TXPIDSETTINGS_PIDS_PITCHRATEKD: + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); + break; + case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT: + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP: + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); + break; + case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI: + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); + break; + case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT: + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value); + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value); + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_YAWRATEKP: + needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value); + break; + case TXPIDSETTINGS_PIDS_YAWRATEKI: + needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value); + break; + case TXPIDSETTINGS_PIDS_YAWRATEKD: + needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value); + break; + case TXPIDSETTINGS_PIDS_YAWRATEILIMIT: + needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_YAWATTITUDEKP: + needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value); + break; + case TXPIDSETTINGS_PIDS_YAWATTITUDEKI: + needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value); + break; + case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT: + needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], value); + break; + case TXPIDSETTINGS_PIDS_GYROTAU: + needsUpdate |= update(&stab.GyroTau, value); + break; + default: + PIOS_Assert(0); + } + } + } + if (needsUpdate) { + StabilizationSettingsSet(&stab); + } } /** @@ -296,25 +299,30 @@ static void updatePIDs(UAVObjEvent* ev) */ static float scale(float val, float inMin, float inMax, float outMin, float outMax) { - // bound input value - if (val > inMax) val = inMax; - if (val < inMin) val = inMin; + // bound input value + if (val > inMax) { + val = inMax; + } + if (val < inMin) { + val = inMin; + } - // normalize input value to [0..1] - if (inMax <= inMin) - val = 0.0f; - else - val = (val - inMin) / (inMax - inMin); + // normalize input value to [0..1] + if (inMax <= inMin) { + val = 0.0f; + } else { + val = (val - inMin) / (inMax - inMin); + } - // update output bounds - if (outMin > outMax) { - float t = outMin; - outMin = outMax; - outMax = t; - val = 1.0f - val; - } + // update output bounds + if (outMin > outMax) { + float t = outMin; + outMin = outMax; + outMax = t; + val = 1.0f - val; + } - return (outMax - outMin) * val + outMin; + return (outMax - outMin) * val + outMin; } /** @@ -323,22 +331,22 @@ static float scale(float val, float inMin, float inMax, float outMin, float outM */ static uint8_t update(float *var, float val) { - /* FIXME: this is not an entirely correct way - * to check if the two floating point - * numbers are 'not equal'. - * Epsilon of 1e-9 is probably okay for the range - * of numbers we see here*/ - if (fabsf(*var - val) > 1e-9f) { - *var = val; - return 1; - } - return 0; + /* FIXME: this is not an entirely correct way + * to check if the two floating point + * numbers are 'not equal'. + * Epsilon of 1e-9 is probably okay for the range + * of numbers we see here*/ + if (fabsf(*var - val) > 1e-9f) { + *var = val; + return 1; + } + return 0; } -/** - * @} - */ - +/** + * @} + */ + /** * @} */ diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index f18be2605..b07601c0c 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -3,7 +3,7 @@ * * @file vtolpathfollower.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief This module compared @ref PositionActual to @ref PathDesired + * @brief This module compared @ref PositionActual to @ref PathDesired * and sets @ref Stabilization. It only does this when the FlightMode field * of @ref FlightStatus is PathPlanner or RTH. * @@ -32,7 +32,7 @@ * Input object: PositionActual * Output object: StabilizationDesired * - * This module will periodically update the value of the @ref StabilizationDesired object based on + * This module will periodically update the value of the @ref StabilizationDesired object based on * @ref PathDesired and @PositionActual when the Flight Mode selected in @FlightStatus is supported * by this module. Otherwise another module (e.g. @ref ManualControlCommand) is expected to be * writing to @ref StabilizationDesired. @@ -53,7 +53,7 @@ #include "accels.h" #include "attitudeactual.h" #include "hwsettings.h" -#include "pathdesired.h" // object that will be updated by the module +#include "pathdesired.h" // object that will be updated by the module #include "positionactual.h" #include "manualcontrol.h" #include "flightstatus.h" @@ -79,9 +79,9 @@ #include "accessorydesired.h" // Private constants -#define MAX_QUEUE_SIZE 4 +#define MAX_QUEUE_SIZE 4 #define STACK_SIZE_BYTES 1548 -#define TASK_PRIORITY (tskIDLE_PRIORITY+2) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) // Private types @@ -93,16 +93,16 @@ static float poiRadius; // Private functions static void vtolPathFollowerTask(void *parameters); -static void SettingsUpdatedCb(UAVObjEvent * ev); +static void SettingsUpdatedCb(UAVObjEvent *ev); static void updateNedAccel(); static void updatePOIBearing(); static void updatePathVelocity(); static void updateEndpointVelocity(); -static void updateFixedAttitude(float* attitude); +static void updateFixedAttitude(float *attitude); static void updateVtolDesiredAttitude(bool yaw_attitude); static float bound(float val, float min, float max); static bool vtolpathfollower_enabled; -static void accessoryUpdated(UAVObjEvent* ev); +static void accessoryUpdated(UAVObjEvent *ev); /** * Initialise the module, called on startup @@ -147,17 +147,17 @@ int32_t VtolPathFollowerInitialize() return 0; } -MODULE_INITCALL( VtolPathFollowerInitialize, VtolPathFollowerStart) +MODULE_INITCALL(VtolPathFollowerInitialize, VtolPathFollowerStart) static float northVelIntegral = 0; -static float eastVelIntegral = 0; -static float downVelIntegral = 0; +static float eastVelIntegral = 0; +static float downVelIntegral = 0; static float northPosIntegral = 0; -static float eastPosIntegral = 0; -static float downPosIntegral = 0; +static float eastPosIntegral = 0; +static float downPosIntegral = 0; -static float throttleOffset = 0; +static float throttleOffset = 0; /** * Module thread, should not return. */ @@ -176,19 +176,18 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) // Main task loop lastUpdateTime = xTaskGetTickCount(); while (1) { - // Conditions when this runs: // 1. Must have VTOL type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR - // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path + // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path SystemSettingsGet(&systemSettings); if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI)) { + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI)) { AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); vTaskDelay(1000); continue; @@ -207,72 +206,71 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) // Check the combinations of flightmode and pathdesired mode switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_LAND: - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updateEndpointVelocity(); - updateVtolDesiredAttitude(false); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); - } + case FLIGHTSTATUS_FLIGHTMODE_LAND: + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { + updateEndpointVelocity(); + updateVtolDesiredAttitude(false); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + } + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + pathStatus.UID = pathDesired.UID; + pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; + switch (pathDesired.Mode) { + // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + updatePathVelocity(); + updateVtolDesiredAttitude(false); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); break; - case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: - pathStatus.UID = pathDesired.UID; - pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; - switch (pathDesired.Mode) { - // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - updatePathVelocity(); - updateVtolDesiredAttitude(false); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_FIXEDATTITUDE: - updateFixedAttitude(pathDesired.ModeParameters); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_DISARMALARM: - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); - break; - default: - pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); - break; - } - PathStatusSet(&pathStatus); + case PATHDESIRED_MODE_FIXEDATTITUDE: + updateFixedAttitude(pathDesired.ModeParameters); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); break; - case FLIGHTSTATUS_FLIGHTMODE_POI: - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updateEndpointVelocity(); - updateVtolDesiredAttitude(true); - updatePOIBearing(); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); - } + case PATHDESIRED_MODE_DISARMALARM: + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); break; default: - // Be cleaner and get rid of global variables - northVelIntegral = 0; - eastVelIntegral = 0; - downVelIntegral = 0; - northPosIntegral = 0; - eastPosIntegral = 0; - downPosIntegral = 0; - - // Track throttle before engaging this mode. Cheap system ident - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); - throttleOffset = stabDesired.Throttle; - + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); break; + } + PathStatusSet(&pathStatus); + break; + case FLIGHTSTATUS_FLIGHTMODE_POI: + if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { + updateEndpointVelocity(); + updateVtolDesiredAttitude(true); + updatePOIBearing(); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + } + break; + default: + // Be cleaner and get rid of global variables + northVelIntegral = 0; + eastVelIntegral = 0; + downVelIntegral = 0; + northPosIntegral = 0; + eastPosIntegral = 0; + downPosIntegral = 0; + + // Track throttle before engaging this mode. Cheap system ident + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + throttleOffset = stabDesired.Throttle; + + break; } AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE); - } } @@ -282,10 +280,11 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) static void updatePOIBearing() { const float DEADBAND_HIGH = 0.10f; - const float DEADBAND_LOW = -0.10f; + const float DEADBAND_LOW = -0.10f; float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); PositionActualData positionActual; PositionActualGet(&positionActual); @@ -305,9 +304,9 @@ static void updatePOIBearing() dLoc[2] = positionActual.Down - poi.Down; if (dLoc[1] < 0) { - yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) + 180.0f; + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f; } else { - yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) - 180.0f; + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f; } // distance @@ -339,23 +338,23 @@ static void updatePOIBearing() if (poiRadius >= 3.0f || changeRadius > 0) { if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) { pathDesired.End[PATHDESIRED_END_NORTH] = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); - pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); pathDesired.StartingVelocity = 1.0f; - pathDesired.EndingVelocity = 0.0f; + pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); } } - //not above + // not above if (distance >= 3.0f) { - //You can feed this into camerastabilization + // You can feed this into camerastabilization /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/ stabDesired.Yaw = yaw + (pathAngle / 2.0f); stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - //cameraDesired.Yaw=yaw; - //cameraDesired.PitchOrServo2=elevation; + // cameraDesired.Yaw=yaw; + // cameraDesired.PitchOrServo2=elevation; CameraDesiredSet(&cameraDesired); StabilizationDesiredSet(&stabDesired); @@ -365,7 +364,7 @@ static void updatePOIBearing() /** * Compute desired velocity from the current position and path * - * Takes in @ref PositionActual and compares it to @ref PathDesired + * Takes in @ref PositionActual and compares it to @ref PathDesired * and computes @ref VelocityDesired */ static void updatePathVelocity() @@ -374,6 +373,7 @@ static void updatePathVelocity() float downCommand; PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); PositionActualData positionActual; PositionActualGet(&positionActual); @@ -386,55 +386,58 @@ static void updatePathVelocity() float groundspeed; switch (pathDesired.Mode) { - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - groundspeed = pathDesired.EndingVelocity; - break; - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_DRIVEENDPOINT: - groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1); - if (progress.fractional_progress > 1) - groundspeed = 0; - break; - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_DRIVEVECTOR: - default: - groundspeed = pathDesired.StartingVelocity - + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1); - if (progress.fractional_progress > 1) - groundspeed = 0; - break; + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_DRIVECIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + case PATHDESIRED_MODE_DRIVECIRCLELEFT: + groundspeed = pathDesired.EndingVelocity; + break; + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_DRIVEENDPOINT: + groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1); + if (progress.fractional_progress > 1) { + groundspeed = 0; + } + break; + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_DRIVEVECTOR: + default: + groundspeed = pathDesired.StartingVelocity + + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1); + if (progress.fractional_progress > 1) { + groundspeed = 0; + } + break; } VelocityDesiredData velocityDesired; velocityDesired.North = progress.path_direction[0] * groundspeed; - velocityDesired.East = progress.path_direction[1] * groundspeed; + velocityDesired.East = progress.path_direction[1] * groundspeed; float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP]; float correction_velocity[2] = { progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed }; float total_vel = sqrtf(powf(correction_velocity[0], 2) + powf(correction_velocity[1], 2)); - float scale = 1; - if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) + float scale = 1; + if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) { scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel; + } velocityDesired.North += progress.correction_direction[0] * error_speed * scale; - velocityDesired.East += progress.correction_direction[1] * error_speed * scale; + velocityDesired.East += progress.correction_direction[1] * error_speed * scale; float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound(progress.fractional_progress, 0, 1); float downError = altitudeSetpoint - positionActual.Down; downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], - -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], - vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); + -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], + vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); + downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); // update pathstatus - pathStatus.error = progress.error; + pathStatus.error = progress.error; pathStatus.fractional_progress = progress.fractional_progress; VelocityDesiredSet(&velocityDesired); @@ -443,7 +446,7 @@ static void updatePathVelocity() /** * Compute desired velocity from the current position * - * Takes in @ref PositionActual and compares it to @ref PositionDesired + * Takes in @ref PositionActual and compares it to @ref PositionDesired * and computes @ref VelocityDesired */ void updateEndpointVelocity() @@ -451,6 +454,7 @@ void updateEndpointVelocity() float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); PositionActualData positionActual; @@ -468,65 +472,66 @@ void updateEndpointVelocity() float northPos = 0, eastPos = 0, downPos = 0; switch (vtolpathfollowerSettings.PositionSource) { - case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF: - northPos = positionActual.North; - eastPos = positionActual.East; - downPos = positionActual.Down; - break; - case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS: - { - // this used to work with the NEDposition UAVObject - // however this UAVObject has been removed - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - HomeLocationData homeLocation; - HomeLocationGet(&homeLocation); - float lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); - float alt = homeLocation.Altitude; - float T[3] = { alt+6.378137E6f, - cosf(lat)*(alt+6.378137E6f), - -1.0f}; - float NED[3] = {T[0] * (DEG2RAD((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f)), - T[1] * (DEG2RAD((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f)), - T[2] * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude))}; + case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF: + northPos = positionActual.North; + eastPos = positionActual.East; + downPos = positionActual.Down; + break; + case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS: + { + // this used to work with the NEDposition UAVObject + // however this UAVObject has been removed + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); + float lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); + float alt = homeLocation.Altitude; + float T[3] = { alt + 6.378137E6f, + cosf(lat) * (alt + 6.378137E6f), + -1.0f }; + float NED[3] = { T[0] * (DEG2RAD((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f)), + T[1] * (DEG2RAD((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f)), + T[2] * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude)) }; - northPos = NED[0]; - eastPos = NED[1]; - downPos = NED[2]; - } - break; - default: - PIOS_Assert(0); - break; + northPos = NED[0]; + eastPos = NED[1]; + downPos = NED[2]; + } + break; + default: + PIOS_Assert(0); + break; } // Compute desired north command northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos; northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], - -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], - vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); - northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral); + -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], + vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); + northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral); eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos; - eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], - -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], - vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); + eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], + -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], + vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral); // Limit the maximum velocity float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2)); - float scale = 1; - if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) + float scale = 1; + if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) { scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel; + } velocityDesired.North = northCommand * scale; - velocityDesired.East = eastCommand * scale; + velocityDesired.East = eastCommand * scale; downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos; downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], - -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], - vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); + -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], + vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); + downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); VelocityDesiredSet(&velocityDesired); @@ -536,25 +541,26 @@ void updateEndpointVelocity() * Compute desired attitude from a fixed preset * */ -static void updateFixedAttitude(float* attitude) +static void updateFixedAttitude(float *attitude) { StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); - stabDesired.Roll = attitude[0]; - stabDesired.Pitch = attitude[1]; - stabDesired.Yaw = attitude[2]; + stabDesired.Roll = attitude[0]; + stabDesired.Pitch = attitude[1]; + stabDesired.Yaw = attitude[2]; stabDesired.Throttle = attitude[3]; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; StabilizationDesiredSet(&stabDesired); } /** * Compute desired attitude from the desired velocity * - * Takes in @ref NedActual which has the acceleration in the - * NED frame as the feedback term and then compares the + * Takes in @ref NedActual which has the acceleration in the + * NED frame as the feedback term and then compares the * @ref VelocityActual against the @ref VelocityDesired */ static void updateVtolDesiredAttitude(bool yaw_attitude) @@ -589,32 +595,32 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) float northVel = 0, eastVel = 0, downVel = 0; switch (vtolpathfollowerSettings.VelocitySource) { - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF: - northVel = velocityActual.North; - eastVel = velocityActual.East; - downVel = velocityActual.Down; - break; - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: - { - GPSVelocityData gpsVelocity; - GPSVelocityGet(&gpsVelocity); - northVel = gpsVelocity.North; - eastVel = gpsVelocity.East; - downVel = gpsVelocity.Down; - } - break; - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS: - { - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading)); - eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading)); - downVel = velocityActual.Down; - } - break; - default: - PIOS_Assert(0); - break; + case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF: + northVel = velocityActual.North; + eastVel = velocityActual.East; + downVel = velocityActual.Down; + break; + case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: + { + GPSVelocityData gpsVelocity; + GPSVelocityGet(&gpsVelocity); + northVel = gpsVelocity.North; + eastVel = gpsVelocity.East; + downVel = gpsVelocity.Down; + } + break; + case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS: + { + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading)); + eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading)); + downVel = velocityActual.Down; + } + break; + default: + PIOS_Assert(0); + break; } // Testing code - refactor into manual control command @@ -624,41 +630,41 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) // Compute desired north command northError = velocityDesired.North - northVel; northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], - -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], - vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); - northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral - - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] - + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward); + -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], + vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); + northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral + - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] + + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward); // Compute desired east command eastError = velocityDesired.East - eastVel; eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], - -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], - vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); - eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral - - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] - + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward); + -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], + vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); + eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral + - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] + + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward); // Compute desired down command downError = velocityDesired.Down - downVel; // Must flip this sign downError = -downError; downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI], - -vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT], - vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]); - downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral - - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]); + -vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT], + vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]); + downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral + - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]); stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1); // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the // craft should move similarly for 5 deg roll versus 5 deg pitch - stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeActual.Yaw)) + - -eastCommand * sinf(DEG2RAD(attitudeActual.Yaw)), - -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); - stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeActual.Yaw)) + - eastCommand * cosf(DEG2RAD(attitudeActual.Yaw)), - -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); + stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeActual.Yaw)) + + -eastCommand * sinf(DEG2RAD(attitudeActual.Yaw)), + -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); + stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeActual.Yaw)) + + eastCommand * cosf(DEG2RAD(attitudeActual.Yaw)), + -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { // For now override throttle with manual control. Disable at your risk, quad goes to China. @@ -667,7 +673,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) stabDesired.Throttle = manualControl.Throttle; } - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; if (yaw_attitude) { stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; @@ -690,12 +696,13 @@ static void updateNedAccel() // Collect downsampled attitude data AccelsData accels; + AccelsGet(&accels); accel[0] = accels.x; accel[1] = accels.y; accel[2] = accels.z; - //rotate avg accels into earth frame and store it + // rotate avg accels into earth frame and store it AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); q[0] = attitudeActual.q1; @@ -705,16 +712,17 @@ static void updateNedAccel() Quaternion2R(q, Rbe); for (uint8_t i = 0; i < 3; i++) { accel_ned[i] = 0; - for (uint8_t j = 0; j < 3; j++) + for (uint8_t j = 0; j < 3; j++) { accel_ned[i] += Rbe[j][i] * accel[j]; + } } accel_ned[2] += 9.81f; NedAccelData accelData; NedAccelGet(&accelData); accelData.North = accel_ned[0]; - accelData.East = accel_ned[1]; - accelData.Down = accel_ned[2]; + accelData.East = accel_ned[1]; + accelData.Down = accel_ned[2]; NedAccelSet(&accelData); } @@ -736,10 +744,11 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); } -static void accessoryUpdated(UAVObjEvent* ev) +static void accessoryUpdated(UAVObjEvent *ev) { - if (ev->obj != AccessoryDesiredHandle()) + if (ev->obj != AccessoryDesiredHandle()) { return; + } AccessoryDesiredData accessory; PoiLearnSettingsData poiLearn; @@ -753,11 +762,10 @@ static void accessoryUpdated(UAVObjEvent* ev) PoiLocationData poi; PoiLocationGet(&poi); poi.North = positionActual.North; - poi.East = positionActual.East; - poi.Down = positionActual.Down; + poi.East = positionActual.East; + poi.Down = positionActual.Down; PoiLocationSet(&poi); } } } } - diff --git a/flight/pios/common/pios_adxl345.c b/flight/pios/common/pios_adxl345.c index 0b2b08a97..da2d1677c 100644 --- a/flight/pios/common/pios_adxl345.c +++ b/flight/pios/common/pios_adxl345.c @@ -33,21 +33,21 @@ #ifdef PIOS_INCLUDE_ADXL345 enum pios_adxl345_dev_magic { - PIOS_ADXL345_DEV_MAGIC = 0xcb55aa55, + PIOS_ADXL345_DEV_MAGIC = 0xcb55aa55, }; struct adxl345_dev { - uint32_t spi_id; - uint32_t slave_num; - enum pios_adxl345_dev_magic magic; + uint32_t spi_id; + uint32_t slave_num; + enum pios_adxl345_dev_magic magic; }; -//! Global structure for this device device -static struct adxl345_dev * dev; +// ! Global structure for this device device +static struct adxl345_dev *dev; -//! Private functions -static struct adxl345_dev * PIOS_ADXL345_alloc(void); -static int32_t PIOS_ADXL345_Validate(struct adxl345_dev * dev); +// ! Private functions +static struct adxl345_dev *PIOS_ADXL345_alloc(void); +static int32_t PIOS_ADXL345_Validate(struct adxl345_dev *dev); static int32_t PIOS_ADXL345_ClaimBus(); static int32_t PIOS_ADXL345_ReleaseBus(); static int32_t PIOS_ADXL345_FifoDepth(uint8_t depth); @@ -55,15 +55,17 @@ static int32_t PIOS_ADXL345_FifoDepth(uint8_t depth); /** * @brief Allocate a new device */ -static struct adxl345_dev * PIOS_ADXL345_alloc(void) +static struct adxl345_dev *PIOS_ADXL345_alloc(void) { - struct adxl345_dev * adxl345_dev; - - adxl345_dev = (struct adxl345_dev *)pvPortMalloc(sizeof(*adxl345_dev)); - if (!adxl345_dev) return (NULL); - - adxl345_dev->magic = PIOS_ADXL345_DEV_MAGIC; - return(adxl345_dev); + struct adxl345_dev *adxl345_dev; + + adxl345_dev = (struct adxl345_dev *)pvPortMalloc(sizeof(*adxl345_dev)); + if (!adxl345_dev) { + return NULL; + } + + adxl345_dev->magic = PIOS_ADXL345_DEV_MAGIC; + return adxl345_dev; } /** @@ -72,30 +74,35 @@ static struct adxl345_dev * PIOS_ADXL345_alloc(void) */ static int32_t PIOS_ADXL345_Validate(struct adxl345_dev *vdev) { - if (vdev == NULL) - return -1; - if (vdev->magic != PIOS_ADXL345_DEV_MAGIC) - return -2; - if (vdev->spi_id == 0) - return -3; - return 0; + if (vdev == NULL) { + return -1; + } + if (vdev->magic != PIOS_ADXL345_DEV_MAGIC) { + return -2; + } + if (vdev->spi_id == 0) { + return -3; + } + return 0; } /** * @brief Claim the SPI bus for the accel communications and select this chip * @return 0 for succesful claiming of bus or -1 otherwise */ -static int32_t PIOS_ADXL345_ClaimBus() +static int32_t PIOS_ADXL345_ClaimBus() { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } - if(PIOS_SPI_ClaimBus(dev->spi_id) != 0) - return -2; + if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { + return -2; + } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); - - return 0; + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + + return 0; } /** @@ -104,62 +111,68 @@ static int32_t PIOS_ADXL345_ClaimBus() */ static int32_t PIOS_ADXL345_ReleaseBus() { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - if(PIOS_SPI_ReleaseBus(dev->spi_id) != 0) - return -2; - - return 0; + if (PIOS_SPI_ReleaseBus(dev->spi_id) != 0) { + return -2; + } + + return 0; } /** * @brief Select the sampling rate of the chip - * + * * This also puts it into high power mode */ -int32_t PIOS_ADXL345_SelectRate(uint8_t rate) +int32_t PIOS_ADXL345_SelectRate(uint8_t rate) { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } - if(PIOS_ADXL345_ClaimBus() != 0) - return -2; + if (PIOS_ADXL345_ClaimBus() != 0) { + return -2; + } - uint8_t out[2] = {ADXL_RATE_ADDR, rate & 0x0F}; - if(PIOS_SPI_TransferBlock(dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_ADXL345_ReleaseBus(); - return -3; - } + uint8_t out[2] = { ADXL_RATE_ADDR, rate & 0x0F }; + if (PIOS_SPI_TransferBlock(dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_ADXL345_ReleaseBus(); + return -3; + } - PIOS_ADXL345_ReleaseBus(); - - return 0; + PIOS_ADXL345_ReleaseBus(); + + return 0; } /** * @brief Set the range of the accelerometer and set the data to be right justified * with sign extension. Also keep device in 4 wire mode. */ -int32_t PIOS_ADXL345_SetRange(uint8_t range) +int32_t PIOS_ADXL345_SetRange(uint8_t range) { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; - - if(PIOS_ADXL345_ClaimBus() != 0) - return -2; - - uint8_t out[2] = {ADXL_FORMAT_ADDR, (range & 0x03) | ADXL_FULL_RES | ADXL_4WIRE}; - if(PIOS_SPI_TransferBlock(dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_ADXL345_ReleaseBus(); - return -3; - } - - PIOS_ADXL345_ReleaseBus(); - - return 0; + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } + + if (PIOS_ADXL345_ClaimBus() != 0) { + return -2; + } + + uint8_t out[2] = { ADXL_FORMAT_ADDR, (range & 0x03) | ADXL_FULL_RES | ADXL_4WIRE }; + if (PIOS_SPI_TransferBlock(dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_ADXL345_ReleaseBus(); + return -3; + } + + PIOS_ADXL345_ReleaseBus(); + + return 0; } /** @@ -167,21 +180,23 @@ int32_t PIOS_ADXL345_SetRange(uint8_t range) */ static int32_t PIOS_ADXL345_FifoDepth(uint8_t depth) { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; - - if(PIOS_ADXL345_ClaimBus() != 0) - return -2; - - uint8_t out[2] = {ADXL_FIFO_ADDR, (depth & 0x1f) | ADXL_FIFO_STREAM}; - if(PIOS_SPI_TransferBlock(dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_ADXL345_ReleaseBus(); - return -3; - } + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } - PIOS_ADXL345_ReleaseBus(); - - return 0; + if (PIOS_ADXL345_ClaimBus() != 0) { + return -2; + } + + uint8_t out[2] = { ADXL_FIFO_ADDR, (depth & 0x1f) | ADXL_FIFO_STREAM }; + if (PIOS_SPI_TransferBlock(dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_ADXL345_ReleaseBus(); + return -3; + } + + PIOS_ADXL345_ReleaseBus(); + + return 0; } /** @@ -189,21 +204,23 @@ static int32_t PIOS_ADXL345_FifoDepth(uint8_t depth) */ static int32_t PIOS_ADXL345_SetMeasure(__attribute__((unused)) uint8_t enable) { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; - - if(PIOS_ADXL345_ClaimBus() != 0) - return -2; - - uint8_t out[2] = {ADXL_POWER_ADDR, ADXL_MEAURE}; - if(PIOS_SPI_TransferBlock(dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_ADXL345_ReleaseBus(); - return -3; - } + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } - PIOS_ADXL345_ReleaseBus(); - - return 0; + if (PIOS_ADXL345_ClaimBus() != 0) { + return -2; + } + + uint8_t out[2] = { ADXL_POWER_ADDR, ADXL_MEAURE }; + if (PIOS_SPI_TransferBlock(dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_ADXL345_ReleaseBus(); + return -3; + } + + PIOS_ADXL345_ReleaseBus(); + + return 0; } /** @@ -211,20 +228,21 @@ static int32_t PIOS_ADXL345_SetMeasure(__attribute__((unused)) uint8_t enable) */ int32_t PIOS_ADXL345_Init(uint32_t spi_id, uint32_t slave_num) { - dev = PIOS_ADXL345_alloc(); - if(dev == NULL) - return -1; - - dev->spi_id = spi_id; - dev->slave_num = slave_num; - - PIOS_ADXL345_ReleaseBus(); - PIOS_ADXL345_SelectRate(ADXL_RATE_3200); - PIOS_ADXL345_SetRange(ADXL_RANGE_8G); - PIOS_ADXL345_FifoDepth(16); - PIOS_ADXL345_SetMeasure(1); - - return 0; + dev = PIOS_ADXL345_alloc(); + if (dev == NULL) { + return -1; + } + + dev->spi_id = spi_id; + dev->slave_num = slave_num; + + PIOS_ADXL345_ReleaseBus(); + PIOS_ADXL345_SelectRate(ADXL_RATE_3200); + PIOS_ADXL345_SetRange(ADXL_RANGE_8G); + PIOS_ADXL345_FifoDepth(16); + PIOS_ADXL345_SetMeasure(1); + + return 0; } /** @@ -232,24 +250,26 @@ int32_t PIOS_ADXL345_Init(uint32_t spi_id, uint32_t slave_num) */ int32_t PIOS_ADXL345_Test() { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } - if(PIOS_ADXL345_ClaimBus() != 0) - return -2; + if (PIOS_ADXL345_ClaimBus() != 0) { + return -2; + } - uint8_t buf[2] = {0,0}; - uint8_t rec[2] = {0,0}; - buf[0] = ADXL_WHOAMI | ADXL_READ_BIT; + uint8_t buf[2] = { 0, 0 }; + uint8_t rec[2] = { 0, 0 }; + buf[0] = ADXL_WHOAMI | ADXL_READ_BIT; - if(PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],sizeof(buf),NULL) < 0) { - PIOS_ADXL345_ReleaseBus(); - return -3; - } + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { + PIOS_ADXL345_ReleaseBus(); + return -3; + } - PIOS_ADXL345_ReleaseBus(); + PIOS_ADXL345_ReleaseBus(); - return (rec[1] == ADXL_DEVICE_ID) ? 0 : -4; + return (rec[1] == ADXL_DEVICE_ID) ? 0 : -4; } /** @@ -257,56 +277,60 @@ int32_t PIOS_ADXL345_Test() */ int32_t PIOS_ADXL345_FifoElements() { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; - - if(PIOS_ADXL345_ClaimBus() != 0) - return -2; - - uint8_t buf[2] = {0,0}; - uint8_t rec[2] = {0,0}; - buf[0] = ADXL_FIFOSTATUS_ADDR | ADXL_READ_BIT ; // Read fifo status - - if(PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],sizeof(buf),NULL) < 0) { - PIOS_ADXL345_ReleaseBus(); - return -3; - } - - PIOS_ADXL345_ReleaseBus(); - - return rec[1] & 0x3f; + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } + + if (PIOS_ADXL345_ClaimBus() != 0) { + return -2; + } + + uint8_t buf[2] = { 0, 0 }; + uint8_t rec[2] = { 0, 0 }; + buf[0] = ADXL_FIFOSTATUS_ADDR | ADXL_READ_BIT; // Read fifo status + + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { + PIOS_ADXL345_ReleaseBus(); + return -3; + } + + PIOS_ADXL345_ReleaseBus(); + + return rec[1] & 0x3f; } /** * @brief Read a single set of values from the x y z channels * @returns The number of samples remaining in the fifo */ -uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data * data) +uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data *data) { - if(PIOS_ADXL345_Validate(dev) != 0) - return -1; - - if(PIOS_ADXL345_ClaimBus() != 0) - return -2; - - // To save memory use same buffer for in and out but offset by - // a byte - uint8_t buf[9] = {0,0,0,0,0,0,0,0}; - uint8_t rec[9] = {0,0,0,0,0,0,0,0}; - buf[0] = ADXL_X0_ADDR | ADXL_MULTI_BIT | ADXL_READ_BIT ; // Multibyte read starting at X0 - - if(PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],9,NULL) < 0) { - PIOS_ADXL345_ReleaseBus(); - return -3; - } + if (PIOS_ADXL345_Validate(dev) != 0) { + return -1; + } - PIOS_ADXL345_ReleaseBus(); - - data->x = rec[1] + (rec[2] << 8); - data->y = rec[3] + (rec[4] << 8); - data->z = rec[5] + (rec[6] << 8); - - return rec[8] & 0x7F; // return number of remaining entries + if (PIOS_ADXL345_ClaimBus() != 0) { + return -2; + } + + // To save memory use same buffer for in and out but offset by + // a byte + uint8_t buf[9] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + uint8_t rec[9] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + buf[0] = ADXL_X0_ADDR | ADXL_MULTI_BIT | ADXL_READ_BIT; // Multibyte read starting at X0 + + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], 9, NULL) < 0) { + PIOS_ADXL345_ReleaseBus(); + return -3; + } + + PIOS_ADXL345_ReleaseBus(); + + data->x = rec[1] + (rec[2] << 8); + data->y = rec[3] + (rec[4] << 8); + data->z = rec[5] + (rec[6] << 8); + + return rec[8] & 0x7F; // return number of remaining entries } #endif /* PIOS_INCLUDE_ADXL345 */ diff --git a/flight/pios/common/pios_bma180.c b/flight/pios/common/pios_bma180.c index c6a7210b5..5f3fb9451 100644 --- a/flight/pios/common/pios_bma180.c +++ b/flight/pios/common/pios_bma180.c @@ -36,27 +36,27 @@ #include "fifo_buffer.h" enum pios_bma180_dev_magic { - PIOS_BMA180_DEV_MAGIC = 0xcbb93755, + PIOS_BMA180_DEV_MAGIC = 0xcbb93755, }; #define PIOS_BMA180_MAX_DOWNSAMPLE 10 struct bma180_dev { - uint32_t spi_id; - uint32_t slave_num; - int16_t buffer[PIOS_BMA180_MAX_DOWNSAMPLE * sizeof(struct pios_bma180_data)]; - t_fifo_buffer fifo; - const struct pios_bma180_cfg * cfg; - enum bma180_bandwidth bandwidth; - enum bma180_range range; - enum pios_bma180_dev_magic magic; + uint32_t spi_id; + uint32_t slave_num; + int16_t buffer[PIOS_BMA180_MAX_DOWNSAMPLE * sizeof(struct pios_bma180_data)]; + t_fifo_buffer fifo; + const struct pios_bma180_cfg *cfg; + enum bma180_bandwidth bandwidth; + enum bma180_range range; + enum pios_bma180_dev_magic magic; }; -//! Global structure for this device device -static struct bma180_dev * dev; +// ! Global structure for this device device +static struct bma180_dev *dev; -//! Private functions -static struct bma180_dev * PIOS_BMA180_alloc(void); -static int32_t PIOS_BMA180_Validate(struct bma180_dev * dev); +// ! Private functions +static struct bma180_dev *PIOS_BMA180_alloc(void); +static int32_t PIOS_BMA180_Validate(struct bma180_dev *dev); static int32_t PIOS_BMA180_GetReg(uint8_t reg); static int32_t PIOS_BMA180_SetReg(uint8_t reg, uint8_t data); static int32_t PIOS_BMA180_SelectBW(enum bma180_bandwidth bw); @@ -69,17 +69,19 @@ static int32_t PIOS_BMA180_EnableIrq(); /** * @brief Allocate a new device */ -static struct bma180_dev * PIOS_BMA180_alloc(void) +static struct bma180_dev *PIOS_BMA180_alloc(void) { - struct bma180_dev * bma180_dev; - - bma180_dev = (struct bma180_dev *)pvPortMalloc(sizeof(*bma180_dev)); - if (!bma180_dev) return (NULL); - - fifoBuf_init(&bma180_dev->fifo, (uint8_t *) bma180_dev->buffer, sizeof(bma180_dev->buffer)); + struct bma180_dev *bma180_dev; - bma180_dev->magic = PIOS_BMA180_DEV_MAGIC; - return(bma180_dev); + bma180_dev = (struct bma180_dev *)pvPortMalloc(sizeof(*bma180_dev)); + if (!bma180_dev) { + return NULL; + } + + fifoBuf_init(&bma180_dev->fifo, (uint8_t *)bma180_dev->buffer, sizeof(bma180_dev->buffer)); + + bma180_dev->magic = PIOS_BMA180_DEV_MAGIC; + return bma180_dev; } /** @@ -88,38 +90,45 @@ static struct bma180_dev * PIOS_BMA180_alloc(void) */ static int32_t PIOS_BMA180_Validate(struct bma180_dev *vdev) { - if (vdev == NULL) - return -1; - if (vdev->magic != PIOS_BMA180_DEV_MAGIC) - return -2; - if (vdev->spi_id == 0) - return -3; - return 0; + if (vdev == NULL) { + return -1; + } + if (vdev->magic != PIOS_BMA180_DEV_MAGIC) { + return -2; + } + if (vdev->spi_id == 0) { + return -3; + } + return 0; } /** * @brief Initialize with good default settings * @returns 0 for success, -1 for failure */ -int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_bma180_cfg * cfg) -{ - dev = PIOS_BMA180_alloc(); - if(dev == NULL) - return -1; - - dev->spi_id = spi_id; - dev->slave_num = slave_num; - dev->cfg = cfg; - - if(PIOS_BMA180_Config() < 0) - return -1; - PIOS_DELAY_WaituS(50); - - PIOS_EXTI_Init(dev->cfg->exti_cfg); +int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_bma180_cfg *cfg) +{ + dev = PIOS_BMA180_alloc(); + if (dev == NULL) { + return -1; + } - while(PIOS_BMA180_EnableIrq() != 0); - - return 0; + dev->spi_id = spi_id; + dev->slave_num = slave_num; + dev->cfg = cfg; + + if (PIOS_BMA180_Config() < 0) { + return -1; + } + PIOS_DELAY_WaituS(50); + + PIOS_EXTI_Init(dev->cfg->exti_cfg); + + while (PIOS_BMA180_EnableIrq() != 0) { + ; + } + + return 0; } /** @@ -128,17 +137,17 @@ int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_ */ int32_t PIOS_BMA180_ClaimBus() { - if (PIOS_BMA180_Validate(dev) != 0) { - return -1; - } + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { - return -1; - } + if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { + return -1; + } - PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0); + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); - return 0; + return 0; } /** @@ -149,16 +158,16 @@ int32_t PIOS_BMA180_ClaimBus() */ int32_t PIOS_BMA180_ClaimBusISR(bool *woken) { - if (PIOS_BMA180_Validate(dev) != 0) { - return -1; - } + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { - return -1; - } + if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { + return -1; + } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); - return 0; + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + return 0; } /** @@ -167,12 +176,12 @@ int32_t PIOS_BMA180_ClaimBusISR(bool *woken) */ int32_t PIOS_BMA180_ReleaseBus() { - if (PIOS_BMA180_Validate(dev) != 0) { - return -1; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBus(dev->spi_id); + return PIOS_SPI_ReleaseBus(dev->spi_id); } /** @@ -183,12 +192,13 @@ int32_t PIOS_BMA180_ReleaseBus() */ int32_t PIOS_BMA180_ReleaseBusISR(bool *woken) { - if(PIOS_BMA180_Validate(dev) != 0) - return -1; + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); + return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); } /** @@ -198,19 +208,21 @@ int32_t PIOS_BMA180_ReleaseBusISR(bool *woken) */ int32_t PIOS_BMA180_GetReg(uint8_t reg) { - if(PIOS_BMA180_Validate(dev) != 0) - return -1; + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - uint8_t data; - - if(PIOS_BMA180_ClaimBus() != 0) - return -1; + uint8_t data; - PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte - data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response - - PIOS_BMA180_ReleaseBus(); - return data; + if (PIOS_BMA180_ClaimBus() != 0) { + return -1; + } + + PIOS_SPI_TransferByte(dev->spi_id, (0x80 | reg)); // request byte + data = PIOS_SPI_TransferByte(dev->spi_id, 0); // receive response + + PIOS_BMA180_ReleaseBus(); + return data; } /** @@ -221,88 +233,115 @@ int32_t PIOS_BMA180_GetReg(uint8_t reg) */ int32_t PIOS_BMA180_SetReg(uint8_t reg, uint8_t data) { - if(PIOS_BMA180_ClaimBus() != 0) - return -1; - - PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg); - PIOS_SPI_TransferByte(dev->spi_id, data); + if (PIOS_BMA180_ClaimBus() != 0) { + return -1; + } - PIOS_BMA180_ReleaseBus(); - - return 0; + PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg); + PIOS_SPI_TransferByte(dev->spi_id, data); + + PIOS_BMA180_ReleaseBus(); + + return 0; } -static int32_t PIOS_BMA180_EnableEeprom() { - // Enable EEPROM writing - int32_t byte = PIOS_BMA180_GetReg(BMA_CTRREG0); - if(byte < 0) - return -1; - byte |= 0x10; // Set bit 4 - if(PIOS_BMA180_SetReg(BMA_CTRREG0,(uint8_t) byte) < 0) // Have to set ee_w to - return -1; - return 0; +static int32_t PIOS_BMA180_EnableEeprom() +{ + // Enable EEPROM writing + int32_t byte = PIOS_BMA180_GetReg(BMA_CTRREG0); + + if (byte < 0) { + return -1; + } + byte |= 0x10; // Set bit 4 + if (PIOS_BMA180_SetReg(BMA_CTRREG0, (uint8_t)byte) < 0) { // Have to set ee_w to + return -1; + } + return 0; } -static int32_t PIOS_BMA180_DisableEeprom() { - // Enable EEPROM writing - int32_t byte = PIOS_BMA180_GetReg(BMA_CTRREG0); - if(byte < 0) - return -1; - byte |= 0x10; // Set bit 4 - if(PIOS_BMA180_SetReg(BMA_CTRREG0,(uint8_t) byte) < 0) // Have to set ee_w to - return -1; - return 0; +static int32_t PIOS_BMA180_DisableEeprom() +{ + // Enable EEPROM writing + int32_t byte = PIOS_BMA180_GetReg(BMA_CTRREG0); + + if (byte < 0) { + return -1; + } + byte |= 0x10; // Set bit 4 + if (PIOS_BMA180_SetReg(BMA_CTRREG0, (uint8_t)byte) < 0) { // Have to set ee_w to + return -1; + } + return 0; } /** * @brief Set the default register settings * @return 0 if successful, -1 if not */ -static int32_t PIOS_BMA180_Config() +static int32_t PIOS_BMA180_Config() { - /* - 0x35 = 0x81 //smp-skip = 1 for less interrupts - 0x33 = 0x81 //shadow-dis = 1, update MSB and LSB synchronously - 0x27 = 0x01 //dis-i2c - 0x21 = 0x02 //new_data_int = 1 - */ - - PIOS_DELAY_WaituS(20); + /* + 0x35 = 0x81 //smp-skip = 1 for less interrupts + 0x33 = 0x81 //shadow-dis = 1, update MSB and LSB synchronously + 0x27 = 0x01 //dis-i2c + 0x21 = 0x02 //new_data_int = 1 + */ - if(PIOS_BMA180_Validate(dev) != 0) - return -1; + PIOS_DELAY_WaituS(20); - while(PIOS_BMA180_EnableEeprom() != 0); - while(PIOS_BMA180_SetReg(BMA_RESET, BMA_RESET_CODE) != 0); - while(PIOS_BMA180_SetReg(BMA_OFFSET_LSB1, 0x81) != 0); - while(PIOS_BMA180_SetReg(BMA_GAIN_Y, 0x81) != 0); - while(PIOS_BMA180_SetReg(BMA_CTRREG3, 0xFF) != 0); - while(PIOS_BMA180_SelectBW(dev->cfg->bandwidth) != 0); - while(PIOS_BMA180_SetRange(dev->cfg->range) != 0); - while(PIOS_BMA180_DisableEeprom() != 0); + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - return 0; + while (PIOS_BMA180_EnableEeprom() != 0) { + ; + } + while (PIOS_BMA180_SetReg(BMA_RESET, BMA_RESET_CODE) != 0) { + ; + } + while (PIOS_BMA180_SetReg(BMA_OFFSET_LSB1, 0x81) != 0) { + ; + } + while (PIOS_BMA180_SetReg(BMA_GAIN_Y, 0x81) != 0) { + ; + } + while (PIOS_BMA180_SetReg(BMA_CTRREG3, 0xFF) != 0) { + ; + } + while (PIOS_BMA180_SelectBW(dev->cfg->bandwidth) != 0) { + ; + } + while (PIOS_BMA180_SetRange(dev->cfg->range) != 0) { + ; + } + while (PIOS_BMA180_DisableEeprom() != 0) { + ; + } + + return 0; } /** * @brief Select the bandwidth the digital filter pass allows. * @return 0 if successful, -1 if not * @param rate[in] Bandwidth setting to be used - * + * * EEPROM must be write-enabled before calling this function. */ static int32_t PIOS_BMA180_SelectBW(enum bma180_bandwidth bw) { - if(PIOS_BMA180_Validate(dev) != 0) - return -1; + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - dev->bandwidth = bw; - - uint8_t reg; - reg = PIOS_BMA180_GetReg(BMA_BW_ADDR); - reg = (reg & ~BMA_BW_MASK) | ((bw << BMA_BW_SHIFT) & BMA_BW_MASK); - return PIOS_BMA180_SetReg(BMA_BW_ADDR, reg); + dev->bandwidth = bw; + + uint8_t reg; + reg = PIOS_BMA180_GetReg(BMA_BW_ADDR); + reg = (reg & ~BMA_BW_MASK) | ((bw << BMA_BW_SHIFT) & BMA_BW_MASK); + return PIOS_BMA180_SetReg(BMA_BW_ADDR, reg); } /** @@ -311,32 +350,35 @@ static int32_t PIOS_BMA180_SelectBW(enum bma180_bandwidth bw) * @param rate[in] Range setting to be used * */ -static int32_t PIOS_BMA180_SetRange(enum bma180_range new_range) -{ - if(PIOS_BMA180_Validate(dev) != 0) - return -1; +static int32_t PIOS_BMA180_SetRange(enum bma180_range new_range) +{ + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - uint8_t reg; - - dev->range = new_range; - reg = PIOS_BMA180_GetReg(BMA_RANGE_ADDR); - reg = (reg & ~BMA_RANGE_MASK) | ((dev->range << BMA_RANGE_SHIFT) & BMA_RANGE_MASK); - return PIOS_BMA180_SetReg(BMA_RANGE_ADDR, reg); + uint8_t reg; + + dev->range = new_range; + reg = PIOS_BMA180_GetReg(BMA_RANGE_ADDR); + reg = (reg & ~BMA_RANGE_MASK) | ((dev->range << BMA_RANGE_SHIFT) & BMA_RANGE_MASK); + return PIOS_BMA180_SetReg(BMA_RANGE_ADDR, reg); } -static int32_t PIOS_BMA180_EnableIrq() +static int32_t PIOS_BMA180_EnableIrq() { + if (PIOS_BMA180_EnableEeprom() < 0) { + return -1; + } - if(PIOS_BMA180_EnableEeprom() < 0) - return -1; + if (PIOS_BMA180_SetReg(BMA_CTRREG3, BMA_NEW_DAT_INT) < 0) { + return -1; + } - if(PIOS_BMA180_SetReg(BMA_CTRREG3, BMA_NEW_DAT_INT) < 0) - return -1; + if (PIOS_BMA180_DisableEeprom() < 0) { + return -1; + } - if(PIOS_BMA180_DisableEeprom() < 0) - return -1; - - return 0; + return 0; } /** @@ -346,28 +388,30 @@ static int32_t PIOS_BMA180_EnableIrq() * @retval -1 unable to claim bus * @retval -2 unable to transfer data */ -int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data * data) +int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data *data) { - // To save memory use same buffer for in and out but offset by - // a byte - uint8_t buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0}; - uint8_t rec[7] = {0,0,0,0,0,0}; - - if(PIOS_BMA180_ClaimBus() != 0) - return -1; - if(PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],7,NULL) != 0) - return -2; - PIOS_BMA180_ReleaseBus(); - - // | MSB | LSB | 0 | new_data | - data->x = ((rec[2] << 8) | rec[1]); - data->y = ((rec[4] << 8) | rec[3]); - data->z = ((rec[6] << 8) | rec[5]); - data->x /= 4; - data->y /= 4; - data->z /= 4; - - return 0; // return number of remaining entries + // To save memory use same buffer for in and out but offset by + // a byte + uint8_t buf[7] = { BMA_X_LSB_ADDR | 0x80, 0, 0, 0, 0, 0 }; + uint8_t rec[7] = { 0, 0, 0, 0, 0, 0 }; + + if (PIOS_BMA180_ClaimBus() != 0) { + return -1; + } + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], 7, NULL) != 0) { + return -2; + } + PIOS_BMA180_ReleaseBus(); + + // | MSB | LSB | 0 | new_data | + data->x = ((rec[2] << 8) | rec[1]); + data->y = ((rec[4] << 8) | rec[3]); + data->z = ((rec[6] << 8) | rec[5]); + data->x /= 4; + data->y /= 4; + data->z /= 4; + + return 0; // return number of remaining entries } /** @@ -376,26 +420,33 @@ int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data * data) */ float PIOS_BMA180_GetScale() { - if(PIOS_BMA180_Validate(dev) != 0) - return -1; + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - switch (dev->cfg->range) { - case BMA_RANGE_1G: - return GRAV / 8192.0f; - case BMA_RANGE_1_5G: - return GRAV / 5460.0f; - case BMA_RANGE_2G: - return GRAV / 4096.0f; - case BMA_RANGE_3G: - return GRAV / 2730.0f; - case BMA_RANGE_4G: - return GRAV / 2048.0f; - case BMA_RANGE_8G: - return GRAV / 1024.0f; - case BMA_RANGE_16G: - return GRAV / 512.0f; - } - return 0; + switch (dev->cfg->range) { + case BMA_RANGE_1G: + return GRAV / 8192.0f; + + case BMA_RANGE_1_5G: + return GRAV / 5460.0f; + + case BMA_RANGE_2G: + return GRAV / 4096.0f; + + case BMA_RANGE_3G: + return GRAV / 2730.0f; + + case BMA_RANGE_4G: + return GRAV / 2048.0f; + + case BMA_RANGE_8G: + return GRAV / 1024.0f; + + case BMA_RANGE_16G: + return GRAV / 512.0f; + } + return 0; } /** @@ -403,17 +454,19 @@ float PIOS_BMA180_GetScale() * @param [out] buffer pointer to a @ref pios_bma180_data structure to receive data * @return 0 for success, -1 for failure (no data available) */ -int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data * buffer) +int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data *buffer) { - if(PIOS_BMA180_Validate(dev) != 0) - return -1; + if (PIOS_BMA180_Validate(dev) != 0) { + return -1; + } - if(fifoBuf_getUsed(&dev->fifo) < sizeof(*buffer)) - return -1; - - fifoBuf_getData(&dev->fifo, (uint8_t *) buffer, sizeof(*buffer)); - - return 0; + if (fifoBuf_getUsed(&dev->fifo) < sizeof(*buffer)) { + return -1; + } + + fifoBuf_getData(&dev->fifo, (uint8_t *)buffer, sizeof(*buffer)); + + return 0; } @@ -424,30 +477,35 @@ int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data * buffer) */ int32_t PIOS_BMA180_Test() { - // Read chip ID then version ID - uint8_t buf[3] = {0x80 | BMA_CHIPID_ADDR, 0, 0}; - uint8_t rec[3] = {0,0, 0}; - int32_t retval; + // Read chip ID then version ID + uint8_t buf[3] = { 0x80 | BMA_CHIPID_ADDR, 0, 0 }; + uint8_t rec[3] = { 0, 0, 0 }; + int32_t retval; - if(PIOS_BMA180_ClaimBus() != 0) - return -1; - retval = PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],sizeof(buf),NULL); - PIOS_BMA180_ReleaseBus(); - - if(retval != 0) - return -2; - - struct pios_bma180_data data; - if(PIOS_BMA180_ReadAccels(&data) != 0) - return -3; - - if(rec[1] != 0x3) - return -4; - - if(rec[2] < 0x12) - return -5; + if (PIOS_BMA180_ClaimBus() != 0) { + return -1; + } + retval = PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL); + PIOS_BMA180_ReleaseBus(); - return 0; + if (retval != 0) { + return -2; + } + + struct pios_bma180_data data; + if (PIOS_BMA180_ReadAccels(&data) != 0) { + return -3; + } + + if (rec[1] != 0x3) { + return -4; + } + + if (rec[2] < 0x12) { + return -5; + } + + return 0; } /** @@ -458,43 +516,44 @@ int32_t PIOS_BMA180_Test() int32_t bma180_irqs = 0; bool PIOS_BMA180_IRQHandler(void) { - bool woken = false; - static const uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0}; - uint8_t pios_bma180_dmabuf[8]; - bma180_irqs++; - - // If we can't get the bus then just move on for efficiency - if (PIOS_BMA180_ClaimBusISR(&woken) != 0) { - return woken; // Something else is using bus, miss this data - } - - PIOS_SPI_TransferBlock(dev->spi_id,pios_bma180_req_buf,(uint8_t *) pios_bma180_dmabuf, - sizeof(pios_bma180_dmabuf), NULL); + bool woken = false; + static const uint8_t pios_bma180_req_buf[7] = { BMA_X_LSB_ADDR | 0x80, 0, 0, 0, 0, 0 }; + uint8_t pios_bma180_dmabuf[8]; - // TODO: Make this conversion depend on configuration scale - struct pios_bma180_data data; - - // Don't release bus till data has copied - PIOS_BMA180_ReleaseBusISR(&woken); - - // Must not return before releasing bus - if (fifoBuf_getFree(&dev->fifo) < sizeof(data)) { - return woken; - } - - // Bottom two bits indicate new data and are constant zeros. Don't right - // shift because it drops sign bit - data.x = ((pios_bma180_dmabuf[2] << 8) | pios_bma180_dmabuf[1]); - data.y = ((pios_bma180_dmabuf[4] << 8) | pios_bma180_dmabuf[3]); - data.z = ((pios_bma180_dmabuf[6] << 8) | pios_bma180_dmabuf[5]); - data.x /= 4; - data.y /= 4; - data.z /= 4; - data.temperature = pios_bma180_dmabuf[7]; - - fifoBuf_putData(&dev->fifo, (uint8_t *) &data, sizeof(data)); - - return woken; + bma180_irqs++; + + // If we can't get the bus then just move on for efficiency + if (PIOS_BMA180_ClaimBusISR(&woken) != 0) { + return woken; // Something else is using bus, miss this data + } + + PIOS_SPI_TransferBlock(dev->spi_id, pios_bma180_req_buf, (uint8_t *)pios_bma180_dmabuf, + sizeof(pios_bma180_dmabuf), NULL); + + // TODO: Make this conversion depend on configuration scale + struct pios_bma180_data data; + + // Don't release bus till data has copied + PIOS_BMA180_ReleaseBusISR(&woken); + + // Must not return before releasing bus + if (fifoBuf_getFree(&dev->fifo) < sizeof(data)) { + return woken; + } + + // Bottom two bits indicate new data and are constant zeros. Don't right + // shift because it drops sign bit + data.x = ((pios_bma180_dmabuf[2] << 8) | pios_bma180_dmabuf[1]); + data.y = ((pios_bma180_dmabuf[4] << 8) | pios_bma180_dmabuf[3]); + data.z = ((pios_bma180_dmabuf[6] << 8) | pios_bma180_dmabuf[5]); + data.x /= 4; + data.y /= 4; + data.z /= 4; + data.temperature = pios_bma180_dmabuf[7]; + + fifoBuf_putData(&dev->fifo, (uint8_t *)&data, sizeof(data)); + + return woken; } #endif /* PIOS_INCLUDE_BMA180 */ diff --git a/flight/pios/common/pios_bmp085.c b/flight/pios/common/pios_bmp085.c index 83a77e95e..28d57c5dd 100644 --- a/flight/pios/common/pios_bmp085.c +++ b/flight/pios/common/pios_bmp085.c @@ -1,30 +1,30 @@ /** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_BMP085 BMP085 Functions - * @brief Hardware functions to deal with the altitude pressure sensor - * @{ - * - * @file pios_bmp085.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief BMP085 Pressure Sensor Routines - * @see The GNU Public License (GPL) Version 3 - * - ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_BMP085 BMP085 Functions + * @brief Hardware functions to deal with the altitude pressure sensor + * @{ + * + * @file pios_bmp085.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief BMP085 Pressure Sensor Routines + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -58,277 +58,285 @@ xSemaphoreHandle PIOS_BMP085_EOC; int32_t PIOS_BMP085_EOC; #endif -void PIOS_BMP085_EndOfConversion (void) +void PIOS_BMP085_EndOfConversion(void) { #if defined(PIOS_INCLUDE_FREERTOS) - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #endif - /* Read the ADC Value */ + /* Read the ADC Value */ #if defined(PIOS_INCLUDE_FREERTOS) - xSemaphoreGiveFromISR(PIOS_BMP085_EOC, &xHigherPriorityTaskWoken); + xSemaphoreGiveFromISR(PIOS_BMP085_EOC, &xHigherPriorityTaskWoken); #else - PIOS_BMP085_EOC=1; + PIOS_BMP085_EOC = 1; #endif #if defined(PIOS_INCLUDE_FREERTOS) - /* Yield From ISR if needed */ - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + /* Yield From ISR if needed */ + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } static const struct pios_exti_cfg pios_exti_bmp085_cfg __exti_config = { - .vector = PIOS_BMP085_EndOfConversion, - .line = PIOS_BMP085_EOC_EXTI_LINE, - .pin = { - .gpio = PIOS_BMP085_EOC_GPIO_PORT, - .init = { - .GPIO_Pin = PIOS_BMP085_EOC_GPIO_PIN, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = PIOS_BMP085_EOC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_BMP085_EOC_PRIO, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = PIOS_BMP085_EOC_EXTI_LINE, - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_BMP085_EndOfConversion, + .line = PIOS_BMP085_EOC_EXTI_LINE, + .pin = { + .gpio = PIOS_BMP085_EOC_GPIO_PORT, + .init = { + .GPIO_Pin = PIOS_BMP085_EOC_GPIO_PIN, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = PIOS_BMP085_EOC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_BMP085_EOC_PRIO, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = PIOS_BMP085_EOC_EXTI_LINE, + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; #endif /* PIOS_BMP085_HAS_GPIOS */ /** -* Initialise the BMP085 sensor -*/ + * Initialise the BMP085 sensor + */ void PIOS_BMP085_Init(void) { #ifdef PIOS_BMP085_HAS_GPIOS #if defined(PIOS_INCLUDE_FREERTOS) - /* Semaphore used by ISR to signal End-Of-Conversion */ - vSemaphoreCreateBinary(PIOS_BMP085_EOC); - /* Must start off empty so that first transfer waits for EOC */ - xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); + /* Semaphore used by ISR to signal End-Of-Conversion */ + vSemaphoreCreateBinary(PIOS_BMP085_EOC); + /* Must start off empty so that first transfer waits for EOC */ + xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); #else - PIOS_BMP085_EOC = 0; + PIOS_BMP085_EOC = 0; #endif - /* Enable EOC GPIO clock */ - RCC_APB2PeriphClockCmd(PIOS_BMP085_EOC_CLK | RCC_APB2Periph_AFIO, ENABLE); + /* Enable EOC GPIO clock */ + RCC_APB2PeriphClockCmd(PIOS_BMP085_EOC_CLK | RCC_APB2Periph_AFIO, ENABLE); - if (PIOS_EXTI_Init(&pios_exti_bmp085_cfg)) { - PIOS_Assert(0); - } + if (PIOS_EXTI_Init(&pios_exti_bmp085_cfg)) { + PIOS_Assert(0); + } - /* Configure XCLR pin as push/pull alternate funtion output */ - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Pin = PIOS_BMP085_XCLR_GPIO_PIN; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; - GPIO_Init(PIOS_BMP085_XCLR_GPIO_PORT, &GPIO_InitStructure); + /* Configure XCLR pin as push/pull alternate funtion output */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.GPIO_Pin = PIOS_BMP085_XCLR_GPIO_PIN; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; + GPIO_Init(PIOS_BMP085_XCLR_GPIO_PORT, &GPIO_InitStructure); -#endif /* PIOS_BMP085_HAS_GPIOS */ +#endif /* PIOS_BMP085_HAS_GPIOS */ - /* Read all 22 bytes of calibration data in one transfer, this is a very optimized way of doing things */ - uint8_t Data[BMP085_CALIB_LEN]; - while (PIOS_BMP085_Read(BMP085_CALIB_ADDR, Data, BMP085_CALIB_LEN) != 0) - continue; + /* Read all 22 bytes of calibration data in one transfer, this is a very optimized way of doing things */ + uint8_t Data[BMP085_CALIB_LEN]; + while (PIOS_BMP085_Read(BMP085_CALIB_ADDR, Data, BMP085_CALIB_LEN) != 0) { + continue; + } - /* Parameters AC1-AC6 */ - CalibData.AC1 = (Data[0] << 8) | Data[1]; - CalibData.AC2 = (Data[2] << 8) | Data[3]; - CalibData.AC3 = (Data[4] << 8) | Data[5]; - CalibData.AC4 = (Data[6] << 8) | Data[7]; - CalibData.AC5 = (Data[8] << 8) | Data[9]; - CalibData.AC6 = (Data[10] << 8) | Data[11]; + /* Parameters AC1-AC6 */ + CalibData.AC1 = (Data[0] << 8) | Data[1]; + CalibData.AC2 = (Data[2] << 8) | Data[3]; + CalibData.AC3 = (Data[4] << 8) | Data[5]; + CalibData.AC4 = (Data[6] << 8) | Data[7]; + CalibData.AC5 = (Data[8] << 8) | Data[9]; + CalibData.AC6 = (Data[10] << 8) | Data[11]; - /* Parameters B1, B2 */ - CalibData.B1 = (Data[12] << 8) | Data[13]; - CalibData.B2 = (Data[14] << 8) | Data[15]; + /* Parameters B1, B2 */ + CalibData.B1 = (Data[12] << 8) | Data[13]; + CalibData.B2 = (Data[14] << 8) | Data[15]; - /* Parameters MB, MC, MD */ - CalibData.MB = (Data[16] << 8) | Data[17]; - CalibData.MC = (Data[18] << 8) | Data[19]; - CalibData.MD = (Data[20] << 8) | Data[21]; + /* Parameters MB, MC, MD */ + CalibData.MB = (Data[16] << 8) | Data[17]; + CalibData.MC = (Data[18] << 8) | Data[19]; + CalibData.MD = (Data[20] << 8) | Data[21]; } /** -* Start the ADC conversion -* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR -* \return Raw ADC value -*/ + * Start the ADC conversion + * \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR + * \return Raw ADC value + */ void PIOS_BMP085_StartADC(ConversionTypeTypeDef Type) { - /* Start the conversion */ - if (Type == TemperatureConv) { - while (PIOS_BMP085_Write(BMP085_CTRL_ADDR, BMP085_TEMP_ADDR) != 0) - continue; - } else if (Type == PressureConv) { - while (PIOS_BMP085_Write(BMP085_CTRL_ADDR, BMP085_PRES_ADDR) != 0) - continue; - } + /* Start the conversion */ + if (Type == TemperatureConv) { + while (PIOS_BMP085_Write(BMP085_CTRL_ADDR, BMP085_TEMP_ADDR) != 0) { + continue; + } + } else if (Type == PressureConv) { + while (PIOS_BMP085_Write(BMP085_CTRL_ADDR, BMP085_PRES_ADDR) != 0) { + continue; + } + } - CurrentRead = Type; + CurrentRead = Type; } /** -* Read the ADC conversion value (once ADC conversion has completed) -* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR -* \return Raw ADC value -*/ + * Read the ADC conversion value (once ADC conversion has completed) + * \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR + * \return Raw ADC value + */ void PIOS_BMP085_ReadADC(void) { - uint8_t Data[3]; - Data[0] = 0; - Data[1] = 0; - Data[2] = 0; + uint8_t Data[3]; - /* Read and store the 16bit result */ - if (CurrentRead == TemperatureConv) { - /* Read the temperature conversion */ - while (PIOS_BMP085_Read(BMP085_ADC_MSB, Data, 2) != 0) - continue; - RawTemperature = ((Data[0] << 8) | Data[1]); + Data[0] = 0; + Data[1] = 0; + Data[2] = 0; - X1 = (RawTemperature - CalibData.AC6) * CalibData.AC5 >> 15; - X2 = ((int32_t) CalibData.MC << 11) / (X1 + CalibData.MD); - B5 = X1 + X2; - Temperature = (B5 + 8) >> 4; - } else { - /* Read the pressure conversion */ - while (PIOS_BMP085_Read(BMP085_ADC_MSB, Data, 3) != 0) - continue; - RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]) >> (8 - BMP085_OVERSAMPLING); + /* Read and store the 16bit result */ + if (CurrentRead == TemperatureConv) { + /* Read the temperature conversion */ + while (PIOS_BMP085_Read(BMP085_ADC_MSB, Data, 2) != 0) { + continue; + } + RawTemperature = ((Data[0] << 8) | Data[1]); - B6 = B5 - 4000; - X1 = (CalibData.B2 * (B6 * B6 >> 12)) >> 11; - X2 = CalibData.AC2 * B6 >> 11; - X3 = X1 + X2; - B3 = ((((int32_t) CalibData.AC1 * 4 + X3) << BMP085_OVERSAMPLING) + 2) >> 2; - X1 = CalibData.AC3 * B6 >> 13; - X2 = (CalibData.B1 * (B6 * B6 >> 12)) >> 16; - X3 = ((X1 + X2) + 2) >> 2; - B4 = (CalibData.AC4 * (uint32_t) (X3 + 32768)) >> 15; - B7 = ((uint32_t) RawPressure - B3) * (50000 >> BMP085_OVERSAMPLING); - P = B7 < 0x80000000 ? (B7 * 2) / B4 : (B7 / B4) * 2; + X1 = (RawTemperature - CalibData.AC6) * CalibData.AC5 >> 15; + X2 = ((int32_t)CalibData.MC << 11) / (X1 + CalibData.MD); + B5 = X1 + X2; + Temperature = (B5 + 8) >> 4; + } else { + /* Read the pressure conversion */ + while (PIOS_BMP085_Read(BMP085_ADC_MSB, Data, 3) != 0) { + continue; + } + RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]) >> (8 - BMP085_OVERSAMPLING); - X1 = (P >> 8) * (P >> 8); - X1 = (X1 * 3038) >> 16; - X2 = (-7357 * P) >> 16; - Pressure = P + ((X1 + X2 + 3791) >> 4); - } + B6 = B5 - 4000; + X1 = (CalibData.B2 * (B6 * B6 >> 12)) >> 11; + X2 = CalibData.AC2 * B6 >> 11; + X3 = X1 + X2; + B3 = ((((int32_t)CalibData.AC1 * 4 + X3) << BMP085_OVERSAMPLING) + 2) >> 2; + X1 = CalibData.AC3 * B6 >> 13; + X2 = (CalibData.B1 * (B6 * B6 >> 12)) >> 16; + X3 = ((X1 + X2) + 2) >> 2; + B4 = (CalibData.AC4 * (uint32_t)(X3 + 32768)) >> 15; + B7 = ((uint32_t)RawPressure - B3) * (50000 >> BMP085_OVERSAMPLING); + P = B7 < 0x80000000 ? (B7 * 2) / B4 : (B7 / B4) * 2; + + X1 = (P >> 8) * (P >> 8); + X1 = (X1 * 3038) >> 16; + X2 = (-7357 * P) >> 16; + Pressure = P + ((X1 + X2 + 3791) >> 4); + } } int16_t PIOS_BMP085_GetTemperature(void) { - return Temperature; + return Temperature; } int32_t PIOS_BMP085_GetPressure(void) { - return Pressure; + return Pressure; } /** -* Reads one or more bytes into a buffer -* \param[in] address BMP085 register address (depends on size) -* \param[out] buffer destination buffer -* \param[in] len number of bytes which should be read -* \return 0 if operation was successful -* \return -1 if error during I2C transfer -* \return -2 if BMP085 blocked by another task (retry it!) -* \return -4 if invalid length -*/ -bool PIOS_BMP085_Read(uint8_t address, uint8_t * buffer, uint8_t len) + * Reads one or more bytes into a buffer + * \param[in] address BMP085 register address (depends on size) + * \param[out] buffer destination buffer + * \param[in] len number of bytes which should be read + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + * \return -2 if BMP085 blocked by another task (retry it!) + * \return -4 if invalid length + */ +bool PIOS_BMP085_Read(uint8_t address, uint8_t *buffer, uint8_t len) { - uint8_t addr_buffer[] = { - address, - }; + uint8_t addr_buffer[] = { + address, + }; - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = BMP085_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(addr_buffer), - .buf = addr_buffer, - } - , - { - .info = __func__, - .addr = BMP085_I2C_ADDR, - .rw = PIOS_I2C_TXN_READ, - .len = len, - .buf = buffer, - } - }; + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = BMP085_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(addr_buffer), + .buf = addr_buffer, + } + , + { + .info = __func__, + .addr = BMP085_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; - return PIOS_I2C_Transfer(PIOS_I2C_BMP085_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_BMP085_ADAPTER, txn_list, NELEMENTS(txn_list)); } /** -* Writes one or more bytes to the BMP085 -* \param[in] address Register address -* \param[in] buffer source buffer -* \return 0 if operation was successful -* \return -1 if error during I2C transfer -* \return -2 if BMP085 blocked by another task (retry it!) -*/ + * Writes one or more bytes to the BMP085 + * \param[in] address Register address + * \param[in] buffer source buffer + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + * \return -2 if BMP085 blocked by another task (retry it!) + */ bool PIOS_BMP085_Write(uint8_t address, uint8_t buffer) { - uint8_t data[] = { - address, - buffer, - }; + uint8_t data[] = { + address, + buffer, + }; - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = BMP085_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(data), - .buf = data, - } - , - }; + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = BMP085_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(data), + .buf = data, + } + , + }; - return PIOS_I2C_Transfer(PIOS_I2C_BMP085_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_BMP085_ADAPTER, txn_list, NELEMENTS(txn_list)); } /** -* @brief Run self-test operation. -* \return 0 if self-test failed -* \return any non-0 number if test passed -*/ + * @brief Run self-test operation. + * \return 0 if self-test failed + * \return any non-0 number if test passed + */ int32_t PIOS_BMP085_Test() { - // TODO: Is there a better way to test this than just checking that pressure/temperature has changed? - uint32_t passed = 1; - uint32_t cur_value = 0; + // TODO: Is there a better way to test this than just checking that pressure/temperature has changed? + uint32_t passed = 1; + uint32_t cur_value = 0; - cur_value = Temperature; - PIOS_BMP085_StartADC(TemperatureConv); - PIOS_DELAY_WaitmS(5); - PIOS_BMP085_ReadADC(); - if (cur_value == Temperature) - passed = 0; + cur_value = Temperature; + PIOS_BMP085_StartADC(TemperatureConv); + PIOS_DELAY_WaitmS(5); + PIOS_BMP085_ReadADC(); + if (cur_value == Temperature) { + passed = 0; + } - cur_value=Pressure; - PIOS_BMP085_StartADC(PressureConv); - PIOS_DELAY_WaitmS(26); - PIOS_BMP085_ReadADC(); - if (cur_value == Pressure) - passed = 0; + cur_value = Pressure; + PIOS_BMP085_StartADC(PressureConv); + PIOS_DELAY_WaitmS(26); + PIOS_BMP085_ReadADC(); + if (cur_value == Pressure) { + passed = 0; + } - return passed; + return passed; } #endif /* PIOS_INCLUDE_BMP085 */ diff --git a/flight/pios/common/pios_board_info.c b/flight/pios/common/pios_board_info.c index c3a5a4dfb..97d9eb79f 100644 --- a/flight/pios/common/pios_board_info.c +++ b/flight/pios/common/pios_board_info.c @@ -3,17 +3,17 @@ #include "pios_board_info.h" const struct pios_board_info __attribute__((__used__)) __attribute__((__section__(".boardinfo"))) pios_board_info_blob = { - .magic = PIOS_BOARD_INFO_BLOB_MAGIC, - .board_type = BOARD_TYPE, - .board_rev = BOARD_REVISION, - .bl_rev = BOOTLOADER_VERSION, - .hw_type = HW_TYPE, - .fw_base = FW_BANK_BASE, - .fw_size = FW_BANK_SIZE - FW_DESC_SIZE, - .desc_base = FW_BANK_BASE + FW_BANK_SIZE - FW_DESC_SIZE, - .desc_size = FW_DESC_SIZE, + .magic = PIOS_BOARD_INFO_BLOB_MAGIC, + .board_type = BOARD_TYPE, + .board_rev = BOARD_REVISION, + .bl_rev = BOOTLOADER_VERSION, + .hw_type = HW_TYPE, + .fw_base = FW_BANK_BASE, + .fw_size = FW_BANK_SIZE - FW_DESC_SIZE, + .desc_base = FW_BANK_BASE + FW_BANK_SIZE - FW_DESC_SIZE, + .desc_size = FW_DESC_SIZE, #ifdef EE_BANK_BASE - .ee_base = EE_BANK_BASE, - .ee_size = EE_BANK_SIZE, + .ee_base = EE_BANK_BASE, + .ee_size = EE_BANK_SIZE, #endif }; diff --git a/flight/pios/common/pios_com.c b/flight/pios/common/pios_com.c index b15a176ef..86fbeca58 100644 --- a/flight/pios/common/pios_com.c +++ b/flight/pios/common/pios_com.c @@ -6,25 +6,25 @@ * @brief Hardware communication layer * @{ * - * @file pios_com.c + * @file pios_com.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief COM layer functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,470 +36,477 @@ #include #ifndef PIOS_INCLUDE_FREERTOS -#include "pios_delay.h" /* PIOS_DELAY_WaitmS */ +#include "pios_delay.h" /* PIOS_DELAY_WaitmS */ #endif enum pios_com_dev_magic { - PIOS_COM_DEV_MAGIC = 0xaa55aa55, + PIOS_COM_DEV_MAGIC = 0xaa55aa55, }; struct pios_com_dev { - enum pios_com_dev_magic magic; - uint32_t lower_id; - const struct pios_com_driver * driver; + enum pios_com_dev_magic magic; + uint32_t lower_id; + const struct pios_com_driver *driver; #if defined(PIOS_INCLUDE_FREERTOS) - xSemaphoreHandle tx_sem; - xSemaphoreHandle rx_sem; + xSemaphoreHandle tx_sem; + xSemaphoreHandle rx_sem; #endif - bool has_rx; - bool has_tx; + bool has_rx; + bool has_tx; - t_fifo_buffer rx; - t_fifo_buffer tx; + t_fifo_buffer rx; + t_fifo_buffer tx; }; -static bool PIOS_COM_validate(struct pios_com_dev * com_dev) +static bool PIOS_COM_validate(struct pios_com_dev *com_dev) { - return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC)); + return com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_com_dev * PIOS_COM_alloc(void) +static struct pios_com_dev *PIOS_COM_alloc(void) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - com_dev = (struct pios_com_dev *)pvPortMalloc(sizeof(*com_dev)); - if (!com_dev) return (NULL); + com_dev = (struct pios_com_dev *)pvPortMalloc(sizeof(*com_dev)); + if (!com_dev) { + return NULL; + } - com_dev->magic = PIOS_COM_DEV_MAGIC; - return(com_dev); + com_dev->magic = PIOS_COM_DEV_MAGIC; + return com_dev; } #else static struct pios_com_dev pios_com_devs[PIOS_COM_MAX_DEVS]; static uint8_t pios_com_num_devs; -static struct pios_com_dev * PIOS_COM_alloc(void) +static struct pios_com_dev *PIOS_COM_alloc(void) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) { - return (NULL); - } + if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) { + return NULL; + } - com_dev = &pios_com_devs[pios_com_num_devs++]; - com_dev->magic = PIOS_COM_DEV_MAGIC; + com_dev = &pios_com_devs[pios_com_num_devs++]; + com_dev->magic = PIOS_COM_DEV_MAGIC; - return (com_dev); + return com_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); -static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); -static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield); -static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield); +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); +static void PIOS_COM_UnblockRx(struct pios_com_dev *com_dev, bool *need_yield); +static void PIOS_COM_UnblockTx(struct pios_com_dev *com_dev, bool *need_yield); /** - * Initialises COM layer - * \param[out] handle - * \param[in] driver - * \param[in] id - * \return < 0 if initialisation failed - */ -int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len) + * Initialises COM layer + * \param[out] handle + * \param[in] driver + * \param[in] id + * \return < 0 if initialisation failed + */ +int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len) { - PIOS_Assert(com_id); - PIOS_Assert(driver); + PIOS_Assert(com_id); + PIOS_Assert(driver); - bool has_rx = (rx_buffer && rx_buffer_len > 0); - bool has_tx = (tx_buffer && tx_buffer_len > 0); - PIOS_Assert(has_rx || has_tx); - PIOS_Assert(driver->bind_tx_cb || !has_tx); - PIOS_Assert(driver->bind_rx_cb || !has_rx); + bool has_rx = (rx_buffer && rx_buffer_len > 0); + bool has_tx = (tx_buffer && tx_buffer_len > 0); + PIOS_Assert(has_rx || has_tx); + PIOS_Assert(driver->bind_tx_cb || !has_tx); + PIOS_Assert(driver->bind_rx_cb || !has_rx); - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - com_dev = (struct pios_com_dev *) PIOS_COM_alloc(); - if (!com_dev) goto out_fail; + com_dev = (struct pios_com_dev *)PIOS_COM_alloc(); + if (!com_dev) { + goto out_fail; + } - com_dev->driver = driver; - com_dev->lower_id = lower_id; + com_dev->driver = driver; + com_dev->lower_id = lower_id; - com_dev->has_rx = has_rx; - com_dev->has_tx = has_tx; + com_dev->has_rx = has_rx; + com_dev->has_tx = has_tx; - if (has_rx) { - fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len); + if (has_rx) { + fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len); #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(com_dev->rx_sem); -#endif /* PIOS_INCLUDE_FREERTOS */ - (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, (uint32_t)com_dev); - if (com_dev->driver->rx_start) { - /* Start the receiver */ - (com_dev->driver->rx_start)(com_dev->lower_id, - fifoBuf_getFree(&com_dev->rx)); - } - } + vSemaphoreCreateBinary(com_dev->rx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, (uint32_t)com_dev); + if (com_dev->driver->rx_start) { + /* Start the receiver */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } + } - if (has_tx) { - fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); + if (has_tx) { + fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(com_dev->tx_sem); -#endif /* PIOS_INCLUDE_FREERTOS */ - (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, (uint32_t)com_dev); - } + vSemaphoreCreateBinary(com_dev->tx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, (uint32_t)com_dev); + } - *com_id = (uint32_t)com_dev; - return(0); + *com_id = (uint32_t)com_dev; + return 0; out_fail: - return(-1); + return -1; } -static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield) +static void PIOS_COM_UnblockRx(struct pios_com_dev *com_dev, bool *need_yield) { #if defined(PIOS_INCLUDE_FREERTOS) - static signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken); + static signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken != pdFALSE) { - *need_yield = true; - } else { - *need_yield = false; - } + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } #else - *need_yield = false; + *need_yield = false; #endif } -static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield) +static void PIOS_COM_UnblockTx(struct pios_com_dev *com_dev, bool *need_yield) { #if defined(PIOS_INCLUDE_FREERTOS) - static signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken); + static signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken != pdFALSE) { - *need_yield = true; - } else { - *need_yield = false; - } + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } #else - *need_yield = false; + *need_yield = false; #endif } -static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield) { - struct pios_com_dev * com_dev = (struct pios_com_dev *)context; + struct pios_com_dev *com_dev = (struct pios_com_dev *)context; - bool valid = PIOS_COM_validate(com_dev); - PIOS_Assert(valid); - PIOS_Assert(com_dev->has_rx); + bool valid = PIOS_COM_validate(com_dev); - uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len); + PIOS_Assert(valid); + PIOS_Assert(com_dev->has_rx); - if (bytes_into_fifo > 0) { - /* Data has been added to the buffer */ - PIOS_COM_UnblockRx(com_dev, need_yield); - } + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len); - if (headroom) { - *headroom = fifoBuf_getFree(&com_dev->rx); - } + if (bytes_into_fifo > 0) { + /* Data has been added to the buffer */ + PIOS_COM_UnblockRx(com_dev, need_yield); + } - return (bytes_into_fifo); + if (headroom) { + *headroom = fifoBuf_getFree(&com_dev->rx); + } + + return bytes_into_fifo; } -static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield) { - struct pios_com_dev * com_dev = (struct pios_com_dev *)context; + struct pios_com_dev *com_dev = (struct pios_com_dev *)context; - bool valid = PIOS_COM_validate(com_dev); - PIOS_Assert(valid); - PIOS_Assert(buf); - PIOS_Assert(buf_len); - PIOS_Assert(com_dev->has_tx); + bool valid = PIOS_COM_validate(com_dev); - uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len); + PIOS_Assert(valid); + PIOS_Assert(buf); + PIOS_Assert(buf_len); + PIOS_Assert(com_dev->has_tx); - if (bytes_from_fifo > 0) { - /* More space has been made in the buffer */ - PIOS_COM_UnblockTx(com_dev, need_yield); - } + uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len); - if (headroom) { - *headroom = fifoBuf_getUsed(&com_dev->tx); - } + if (bytes_from_fifo > 0) { + /* More space has been made in the buffer */ + PIOS_COM_UnblockTx(com_dev, need_yield); + } - return (bytes_from_fifo); + if (headroom) { + *headroom = fifoBuf_getUsed(&com_dev->tx); + } + + return bytes_from_fifo; } /** -* Change the port speed without re-initializing -* \param[in] port COM port -* \param[in] baud Requested baud rate -* \return -1 if port not available -* \return 0 on success -*/ + * Change the port speed without re-initializing + * \param[in] port COM port + * \param[in] baud Requested baud rate + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) { - struct pios_com_dev * com_dev = (struct pios_com_dev *)com_id; + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - /* Invoke the driver function if it exists */ - if (com_dev->driver->set_baud) { - com_dev->driver->set_baud(com_dev->lower_id, baud); - } + /* Invoke the driver function if it exists */ + if (com_dev->driver->set_baud) { + com_dev->driver->set_baud(com_dev->lower_id, baud); + } - return 0; + return 0; } /** -* Sends a package over given port -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return number of bytes transmitted on success -*/ + * Sends a package over given port + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return number of bytes transmitted on success + */ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev = (struct pios_com_dev *)com_id; + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - PIOS_Assert(com_dev->has_tx); + PIOS_Assert(com_dev->has_tx); - if (len > fifoBuf_getFree(&com_dev->tx)) { - /* Buffer cannot accept all requested bytes (retry) */ - return -2; - } + if (len > fifoBuf_getFree(&com_dev->tx)) { + /* Buffer cannot accept all requested bytes (retry) */ + return -2; + } - uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len); + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len); - if (bytes_into_fifo > 0) { - /* More data has been put in the tx buffer, make sure the tx is started */ - if (com_dev->driver->tx_start) { - com_dev->driver->tx_start(com_dev->lower_id, - fifoBuf_getUsed(&com_dev->tx)); - } - } + if (bytes_into_fifo > 0) { + /* More data has been put in the tx buffer, make sure the tx is started */ + if (com_dev->driver->tx_start) { + com_dev->driver->tx_start(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } + } - return (bytes_into_fifo); + return bytes_into_fifo; } /** -* Sends a package over given port -* (blocking function) -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return number of bytes transmitted on success -*/ + * Sends a package over given port + * (blocking function) + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return number of bytes transmitted on success + */ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev = (struct pios_com_dev *)com_id; + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - PIOS_Assert(com_dev->has_tx); + PIOS_Assert(com_dev->has_tx); - uint32_t max_frag_len = fifoBuf_getSize(&com_dev->tx); - uint32_t bytes_to_send = len; - while (bytes_to_send) { - uint32_t frag_size; + uint32_t max_frag_len = fifoBuf_getSize(&com_dev->tx); + uint32_t bytes_to_send = len; + while (bytes_to_send) { + uint32_t frag_size; - if (bytes_to_send > max_frag_len) { - frag_size = max_frag_len; - } else { - frag_size = bytes_to_send; - } - int32_t rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, frag_size); - if (rc >= 0) { - bytes_to_send -= rc; - buffer += rc; - } else { - switch (rc) { - case -1: - /* Device is invalid, this will never work */ - return -1; - case -2: - /* Device is busy, wait for the underlying device to free some space and retry */ - /* Make sure the transmitter is running while we wait */ - if (com_dev->driver->tx_start) { - (com_dev->driver->tx_start)(com_dev->lower_id, - fifoBuf_getUsed(&com_dev->tx)); - } + if (bytes_to_send > max_frag_len) { + frag_size = max_frag_len; + } else { + frag_size = bytes_to_send; + } + int32_t rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, frag_size); + if (rc >= 0) { + bytes_to_send -= rc; + buffer += rc; + } else { + switch (rc) { + case -1: + /* Device is invalid, this will never work */ + return -1; + + case -2: + /* Device is busy, wait for the underlying device to free some space and retry */ + /* Make sure the transmitter is running while we wait */ + if (com_dev->driver->tx_start) { + (com_dev->driver->tx_start)(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreTake(com_dev->tx_sem, 5000) != pdTRUE) { - return -3; - } + if (xSemaphoreTake(com_dev->tx_sem, 5000) != pdTRUE) { + return -3; + } #endif - continue; - default: - /* Unhandled return code */ - return rc; - } - } - } + continue; + default: + /* Unhandled return code */ + return rc; + } + } + } - return len; + return len; } /** -* Sends a single character over given port -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a single character over given port + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c) { - return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1); + return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1); } /** -* Sends a single character over given port -* (blocking function) -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a single character over given port + * (blocking function) + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendChar(uint32_t com_id, char c) { - return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1); + return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1); } /** -* Sends a string over given port -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a string over given port + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str) { - return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str)); + return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str)); } /** -* Sends a string over given port -* (blocking function) -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a string over given port + * (blocking function) + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendString(uint32_t com_id, const char *str) { - return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str)); + return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str)); } /** -* Sends a formatted string (-> printf) over given port -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* 128 characters supported maximum! -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * 128 characters supported maximum! + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Sends a formatted string (-> printf) over given port -* (blocking function) -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * (blocking function) + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Transfer bytes from port buffers into another buffer -* \param[in] port COM port -* \returns Byte from buffer -*/ -uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms) + * Transfer bytes from port buffers into another buffer + * \param[in] port COM port + * \returns Byte from buffer + */ +uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t *buf, uint16_t buf_len, uint32_t timeout_ms) { - PIOS_Assert(buf); - PIOS_Assert(buf_len); - uint16_t bytes_from_fifo; + PIOS_Assert(buf); + PIOS_Assert(buf_len); + uint16_t bytes_from_fifo; - struct pios_com_dev * com_dev = (struct pios_com_dev *)com_id; + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - PIOS_Assert(0); - } - PIOS_Assert(com_dev->has_rx); + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + PIOS_Assert(0); + } + PIOS_Assert(com_dev->has_rx); - check_again: - bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len); +check_again: + bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len); - if (bytes_from_fifo == 0) { - /* No more bytes in receive buffer */ - /* Make sure the receiver is running while we wait */ - if (com_dev->driver->rx_start) { - /* Notify the lower layer that there is now room in the rx buffer */ - (com_dev->driver->rx_start)(com_dev->lower_id, - fifoBuf_getFree(&com_dev->rx)); - } - if (timeout_ms > 0) { + if (bytes_from_fifo == 0) { + /* No more bytes in receive buffer */ + /* Make sure the receiver is running while we wait */ + if (com_dev->driver->rx_start) { + /* Notify the lower layer that there is now room in the rx buffer */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } + if (timeout_ms > 0) { #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) { - /* Make sure we don't come back here again */ - timeout_ms = 0; - goto check_again; - } + if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) { + /* Make sure we don't come back here again */ + timeout_ms = 0; + goto check_again; + } #else - PIOS_DELAY_WaitmS(1); - timeout_ms--; - goto check_again; + PIOS_DELAY_WaitmS(1); + timeout_ms--; + goto check_again; #endif - } - } + } + } - /* Return received byte */ - return (bytes_from_fifo); + /* Return received byte */ + return bytes_from_fifo; } /** @@ -509,18 +516,19 @@ uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len */ bool PIOS_COM_Available(uint32_t com_id) { - struct pios_com_dev * com_dev = (struct pios_com_dev *)com_id; + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; - if (!PIOS_COM_validate(com_dev)) { - return false; - } + if (!PIOS_COM_validate(com_dev)) { + return false; + } - // If a driver does not provide a query method assume always - // available if valid - if (com_dev->driver->available == NULL) - return true; + // If a driver does not provide a query method assume always + // available if valid + if (com_dev->driver->available == NULL) { + return true; + } - return (com_dev->driver->available)(com_dev->lower_id); + return (com_dev->driver->available)(com_dev->lower_id); } #endif /* PIOS_INCLUDE_COM */ diff --git a/flight/pios/common/pios_com_msg.c b/flight/pios/common/pios_com_msg.c index 91e285a3d..0bd40805c 100644 --- a/flight/pios/common/pios_com_msg.c +++ b/flight/pios/common/pios_com_msg.c @@ -6,25 +6,25 @@ * @brief Hardware communication layer * @{ * - * @file pios_com_msg.c + * @file pios_com_msg.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief COM MSG layer functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,146 +37,146 @@ #define PIOS_COM_MSG_MAX_LEN 63 struct pios_com_msg_dev { - uint32_t lower_id; - const struct pios_com_driver * driver; + uint32_t lower_id; + const struct pios_com_driver *driver; - uint8_t rx_msg_buffer[PIOS_COM_MSG_MAX_LEN]; - volatile bool rx_msg_full; + uint8_t rx_msg_buffer[PIOS_COM_MSG_MAX_LEN]; + volatile bool rx_msg_full; - uint8_t tx_msg_buffer[PIOS_COM_MSG_MAX_LEN]; - volatile bool tx_msg_full; + uint8_t tx_msg_buffer[PIOS_COM_MSG_MAX_LEN]; + volatile bool tx_msg_full; }; static struct pios_com_msg_dev com_msg_dev; -static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); -static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); +static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); +static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); -int32_t PIOS_COM_MSG_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id) +int32_t PIOS_COM_MSG_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id) { - PIOS_Assert(com_id); - PIOS_Assert(driver); + PIOS_Assert(com_id); + PIOS_Assert(driver); - PIOS_Assert(driver->bind_tx_cb); - PIOS_Assert(driver->bind_rx_cb); + PIOS_Assert(driver->bind_tx_cb); + PIOS_Assert(driver->bind_rx_cb); - struct pios_com_msg_dev * com_dev = &com_msg_dev; + struct pios_com_msg_dev *com_dev = &com_msg_dev; - com_dev->driver = driver; - com_dev->lower_id = lower_id; + com_dev->driver = driver; + com_dev->lower_id = lower_id; - com_dev->rx_msg_full = false; - (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_MSG_RxInCallback, (uint32_t)com_dev); - (com_dev->driver->rx_start)(com_dev->lower_id, sizeof(com_dev->rx_msg_buffer)); + com_dev->rx_msg_full = false; + (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_MSG_RxInCallback, (uint32_t)com_dev); + (com_dev->driver->rx_start)(com_dev->lower_id, sizeof(com_dev->rx_msg_buffer)); - com_dev->tx_msg_full = false; - (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_MSG_TxOutCallback, (uint32_t)com_dev); + com_dev->tx_msg_full = false; + (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_MSG_TxOutCallback, (uint32_t)com_dev); - *com_id = (uint32_t)com_dev; - return(0); + *com_id = (uint32_t)com_dev; + return 0; } -static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, __attribute__((unused)) bool * need_yield) +static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, __attribute__((unused)) bool *need_yield) { - struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)context; + struct pios_com_msg_dev *com_dev = (struct pios_com_msg_dev *)context; - PIOS_Assert(buf); - PIOS_Assert(buf_len); + PIOS_Assert(buf); + PIOS_Assert(buf_len); - uint16_t bytes_from_fifo = 0; + uint16_t bytes_from_fifo = 0; - if (com_dev->tx_msg_full && (buf_len >= sizeof(com_dev->tx_msg_buffer))) { - /* Room for an entire message, send it */ - memcpy(buf, com_dev->tx_msg_buffer, sizeof(com_dev->tx_msg_buffer)); - bytes_from_fifo = sizeof(com_dev->tx_msg_buffer); - com_dev->tx_msg_full = false; - } + if (com_dev->tx_msg_full && (buf_len >= sizeof(com_dev->tx_msg_buffer))) { + /* Room for an entire message, send it */ + memcpy(buf, com_dev->tx_msg_buffer, sizeof(com_dev->tx_msg_buffer)); + bytes_from_fifo = sizeof(com_dev->tx_msg_buffer); + com_dev->tx_msg_full = false; + } - if (headroom) { - if (com_dev->tx_msg_full) { - *headroom = sizeof(com_dev->tx_msg_buffer); - } else { - *headroom = 0; - } - } + if (headroom) { + if (com_dev->tx_msg_full) { + *headroom = sizeof(com_dev->tx_msg_buffer); + } else { + *headroom = 0; + } + } - return (bytes_from_fifo); + return bytes_from_fifo; } -static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, __attribute__((unused)) bool * need_yield) +static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, __attribute__((unused)) bool *need_yield) { - struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)context; + struct pios_com_msg_dev *com_dev = (struct pios_com_msg_dev *)context; - uint16_t bytes_into_fifo = 0; + uint16_t bytes_into_fifo = 0; - if (!com_dev->rx_msg_full && (buf_len >= sizeof(com_dev->rx_msg_buffer))) { - memcpy(com_dev->rx_msg_buffer, buf, sizeof(com_dev->rx_msg_buffer)); - bytes_into_fifo = sizeof(com_dev->rx_msg_buffer); - com_dev->rx_msg_full = true; - } + if (!com_dev->rx_msg_full && (buf_len >= sizeof(com_dev->rx_msg_buffer))) { + memcpy(com_dev->rx_msg_buffer, buf, sizeof(com_dev->rx_msg_buffer)); + bytes_into_fifo = sizeof(com_dev->rx_msg_buffer); + com_dev->rx_msg_full = true; + } - if (headroom) { - if (!com_dev->rx_msg_full) { - *headroom = sizeof(com_dev->rx_msg_buffer); - } else { - *headroom = 0; - } - } + if (headroom) { + if (!com_dev->rx_msg_full) { + *headroom = sizeof(com_dev->rx_msg_buffer); + } else { + *headroom = 0; + } + } - return (bytes_into_fifo); + return bytes_into_fifo; } int32_t PIOS_COM_MSG_Send(uint32_t com_id, const uint8_t *msg, uint16_t msg_len) { - PIOS_Assert(msg); - PIOS_Assert(msg_len); + PIOS_Assert(msg); + PIOS_Assert(msg_len); - struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)com_id; + struct pios_com_msg_dev *com_dev = (struct pios_com_msg_dev *)com_id; - PIOS_Assert(msg_len == sizeof(com_dev->tx_msg_buffer)); + PIOS_Assert(msg_len == sizeof(com_dev->tx_msg_buffer)); - /* Wait forever for room in the tx buffer */ - while (com_dev->tx_msg_full) { - /* Kick the transmitter while we wait */ - if (com_dev->driver->tx_start) { - (com_dev->driver->tx_start)(com_dev->lower_id, sizeof(com_dev->tx_msg_buffer)); - } - } + /* Wait forever for room in the tx buffer */ + while (com_dev->tx_msg_full) { + /* Kick the transmitter while we wait */ + if (com_dev->driver->tx_start) { + (com_dev->driver->tx_start)(com_dev->lower_id, sizeof(com_dev->tx_msg_buffer)); + } + } - memcpy((void *) com_dev->tx_msg_buffer, msg, msg_len); - com_dev->tx_msg_full = true; + memcpy((void *)com_dev->tx_msg_buffer, msg, msg_len); + com_dev->tx_msg_full = true; - /* Kick the transmitter now that we've queued our message */ - if (com_dev->driver->tx_start) { - (com_dev->driver->tx_start)(com_dev->lower_id, sizeof(com_dev->tx_msg_buffer)); - } + /* Kick the transmitter now that we've queued our message */ + if (com_dev->driver->tx_start) { + (com_dev->driver->tx_start)(com_dev->lower_id, sizeof(com_dev->tx_msg_buffer)); + } - return 0; + return 0; } -uint16_t PIOS_COM_MSG_Receive(uint32_t com_id, uint8_t * msg, uint16_t msg_len) +uint16_t PIOS_COM_MSG_Receive(uint32_t com_id, uint8_t *msg, uint16_t msg_len) { - PIOS_Assert(msg); - PIOS_Assert(msg_len); + PIOS_Assert(msg); + PIOS_Assert(msg_len); - struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)com_id; + struct pios_com_msg_dev *com_dev = (struct pios_com_msg_dev *)com_id; - PIOS_Assert(msg_len == sizeof(com_dev->rx_msg_buffer)); + PIOS_Assert(msg_len == sizeof(com_dev->rx_msg_buffer)); - if (!com_dev->rx_msg_full) { - /* There's room in our buffer, kick the receiver */ - (com_dev->driver->rx_start)(com_dev->lower_id, sizeof(com_dev->rx_msg_buffer)); - } else { - memcpy(msg, com_dev->rx_msg_buffer, msg_len); - com_dev->rx_msg_full = false; + if (!com_dev->rx_msg_full) { + /* There's room in our buffer, kick the receiver */ + (com_dev->driver->rx_start)(com_dev->lower_id, sizeof(com_dev->rx_msg_buffer)); + } else { + memcpy(msg, com_dev->rx_msg_buffer, msg_len); + com_dev->rx_msg_full = false; - return msg_len; - } + return msg_len; + } - return 0; + return 0; } -#endif /* PIOS_INCLUDE_COM_MSG */ +#endif /* PIOS_INCLUDE_COM_MSG */ /** * @} diff --git a/flight/pios/common/pios_crc.c b/flight/pios/common/pios_crc.c index dcd8c80c7..9b2128f06 100644 --- a/flight/pios/common/pios_crc.c +++ b/flight/pios/common/pios_crc.c @@ -11,93 +11,93 @@ // CRC lookup table static const uint8_t crc_table[256] = { - 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, - 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, - 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, - 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, - 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, - 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, - 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, - 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, - 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, - 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, - 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, - 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, - 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, - 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, - 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, - 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 + 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, + 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, + 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, + 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, + 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, + 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, + 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, + 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, + 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, + 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, + 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, + 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, + 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, + 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, + 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, + 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 }; -static const uint16_t CRC_Table16[] = { // HDLC polynomial - 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, - 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, - 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, - 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, - 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, - 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, - 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, - 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, - 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, - 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, - 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, - 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, - 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, - 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, - 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, - 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, - 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, - 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, - 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, - 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, - 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, - 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, - 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, - 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, - 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, - 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, - 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, - 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, - 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, - 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, - 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, - 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 +static const uint16_t CRC_Table16[] = { // HDLC polynomial + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 }; -static const uint32_t CRC_Table32[] = { - 0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005, - 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd, - 0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9, 0x5f15adac, 0x5bd4b01b, 0x569796c2, 0x52568b75, - 0x6a1936c8, 0x6ed82b7f, 0x639b0da6, 0x675a1011, 0x791d4014, 0x7ddc5da3, 0x709f7b7a, 0x745e66cd, - 0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039, 0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5, - 0xbe2b5b58, 0xbaea46ef, 0xb7a96036, 0xb3687d81, 0xad2f2d84, 0xa9ee3033, 0xa4ad16ea, 0xa06c0b5d, - 0xd4326d90, 0xd0f37027, 0xddb056fe, 0xd9714b49, 0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95, - 0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1, 0xe13ef6f4, 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d, - 0x34867077, 0x30476dc0, 0x3d044b19, 0x39c556ae, 0x278206ab, 0x23431b1c, 0x2e003dc5, 0x2ac12072, - 0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16, 0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca, - 0x7897ab07, 0x7c56b6b0, 0x71159069, 0x75d48dde, 0x6b93dddb, 0x6f52c06c, 0x6211e6b5, 0x66d0fb02, - 0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1, 0x53dc6066, 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba, - 0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e, 0xbfa1b04b, 0xbb60adfc, 0xb6238b25, 0xb2e29692, - 0x8aad2b2f, 0x8e6c3698, 0x832f1041, 0x87ee0df6, 0x99a95df3, 0x9d684044, 0x902b669d, 0x94ea7b2a, - 0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e, 0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2, - 0xc6bcf05f, 0xc27dede8, 0xcf3ecb31, 0xcbffd686, 0xd5b88683, 0xd1799b34, 0xdc3abded, 0xd8fba05a, - 0x690ce0ee, 0x6dcdfd59, 0x608edb80, 0x644fc637, 0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb, - 0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f, 0x5c007b8a, 0x58c1663d, 0x558240e4, 0x51435d53, - 0x251d3b9e, 0x21dc2629, 0x2c9f00f0, 0x285e1d47, 0x36194d42, 0x32d850f5, 0x3f9b762c, 0x3b5a6b9b, - 0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff, 0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623, - 0xf12f560e, 0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7, 0xe22b20d2, 0xe6ea3d65, 0xeba91bbc, 0xef68060b, - 0xd727bbb6, 0xd3e6a601, 0xdea580d8, 0xda649d6f, 0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3, - 0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7, 0xae3afba2, 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b, - 0x9b3660c6, 0x9ff77d71, 0x92b45ba8, 0x9675461f, 0x8832161a, 0x8cf30bad, 0x81b02d74, 0x857130c3, - 0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640, 0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c, - 0x7b827d21, 0x7f436096, 0x7200464f, 0x76c15bf8, 0x68860bfd, 0x6c47164a, 0x61043093, 0x65c52d24, - 0x119b4be9, 0x155a565e, 0x18197087, 0x1cd86d30, 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec, - 0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088, 0x2497d08d, 0x2056cd3a, 0x2d15ebe3, 0x29d4f654, - 0xc5a92679, 0xc1683bce, 0xcc2b1d17, 0xc8ea00a0, 0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, 0xdbee767c, - 0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18, 0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4, - 0x89b8fd09, 0x8d79e0be, 0x803ac667, 0x84fbdbd0, 0x9abc8bd5, 0x9e7d9662, 0x933eb0bb, 0x97ffad0c, - 0xafb010b1, 0xab710d06, 0xa6322bdf, 0xa2f33668, 0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4 - }; +static const uint32_t CRC_Table32[] = { + 0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005, + 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd, + 0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9, 0x5f15adac, 0x5bd4b01b, 0x569796c2, 0x52568b75, + 0x6a1936c8, 0x6ed82b7f, 0x639b0da6, 0x675a1011, 0x791d4014, 0x7ddc5da3, 0x709f7b7a, 0x745e66cd, + 0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039, 0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5, + 0xbe2b5b58, 0xbaea46ef, 0xb7a96036, 0xb3687d81, 0xad2f2d84, 0xa9ee3033, 0xa4ad16ea, 0xa06c0b5d, + 0xd4326d90, 0xd0f37027, 0xddb056fe, 0xd9714b49, 0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95, + 0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1, 0xe13ef6f4, 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d, + 0x34867077, 0x30476dc0, 0x3d044b19, 0x39c556ae, 0x278206ab, 0x23431b1c, 0x2e003dc5, 0x2ac12072, + 0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16, 0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca, + 0x7897ab07, 0x7c56b6b0, 0x71159069, 0x75d48dde, 0x6b93dddb, 0x6f52c06c, 0x6211e6b5, 0x66d0fb02, + 0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1, 0x53dc6066, 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba, + 0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e, 0xbfa1b04b, 0xbb60adfc, 0xb6238b25, 0xb2e29692, + 0x8aad2b2f, 0x8e6c3698, 0x832f1041, 0x87ee0df6, 0x99a95df3, 0x9d684044, 0x902b669d, 0x94ea7b2a, + 0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e, 0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2, + 0xc6bcf05f, 0xc27dede8, 0xcf3ecb31, 0xcbffd686, 0xd5b88683, 0xd1799b34, 0xdc3abded, 0xd8fba05a, + 0x690ce0ee, 0x6dcdfd59, 0x608edb80, 0x644fc637, 0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb, + 0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f, 0x5c007b8a, 0x58c1663d, 0x558240e4, 0x51435d53, + 0x251d3b9e, 0x21dc2629, 0x2c9f00f0, 0x285e1d47, 0x36194d42, 0x32d850f5, 0x3f9b762c, 0x3b5a6b9b, + 0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff, 0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623, + 0xf12f560e, 0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7, 0xe22b20d2, 0xe6ea3d65, 0xeba91bbc, 0xef68060b, + 0xd727bbb6, 0xd3e6a601, 0xdea580d8, 0xda649d6f, 0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3, + 0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7, 0xae3afba2, 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b, + 0x9b3660c6, 0x9ff77d71, 0x92b45ba8, 0x9675461f, 0x8832161a, 0x8cf30bad, 0x81b02d74, 0x857130c3, + 0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640, 0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c, + 0x7b827d21, 0x7f436096, 0x7200464f, 0x76c15bf8, 0x68860bfd, 0x6c47164a, 0x61043093, 0x65c52d24, + 0x119b4be9, 0x155a565e, 0x18197087, 0x1cd86d30, 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec, + 0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088, 0x2497d08d, 0x2056cd3a, 0x2d15ebe3, 0x29d4f654, + 0xc5a92679, 0xc1683bce, 0xcc2b1d17, 0xc8ea00a0, 0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, 0xdbee767c, + 0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18, 0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4, + 0x89b8fd09, 0x8d79e0be, 0x803ac667, 0x84fbdbd0, 0x9abc8bd5, 0x9e7d9662, 0x933eb0bb, 0x97ffad0c, + 0xafb010b1, 0xab710d06, 0xa6322bdf, 0xa2f33668, 0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4 +}; /** * Update the crc value with new data. @@ -119,7 +119,7 @@ static const uint32_t CRC_Table32[] = { */ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) { - return crc_table[crc ^ data]; + return crc_table[crc ^ data]; } /** @@ -129,17 +129,18 @@ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) * @param[in] length Number of bytes to process * @returns Updated CRC */ -uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length) +uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t *data, int32_t length) { - // use registers for speed - register int32_t len = length; - register uint8_t crc8 = crc; - register const uint8_t *p = data; - - while (len--) - crc8 = crc_table[crc8 ^ *p++]; - - return crc8; + // use registers for speed + register int32_t len = length; + register uint8_t crc8 = crc; + register const uint8_t *p = data; + + while (len--) { + crc8 = crc_table[crc8 ^ *p++]; + } + + return crc8; } /** @@ -151,7 +152,7 @@ uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length) */ uint16_t PIOS_CRC16_updateByte(uint16_t crc, const uint8_t data) { - return ((crc >> 8) ^ CRC_Table16[(crc & 0xff) ^ data]); + return (crc >> 8) ^ CRC_Table16[(crc & 0xff) ^ data]; } /** @@ -161,13 +162,15 @@ uint16_t PIOS_CRC16_updateByte(uint16_t crc, const uint8_t data) * @param[in] length Number of bytes to process * @returns Updated CRC */ -uint16_t PIOS_CRC16_updateCRC(uint16_t crc, const uint8_t* data, int32_t length) +uint16_t PIOS_CRC16_updateCRC(uint16_t crc, const uint8_t *data, int32_t length) { - register uint8_t *p = (uint8_t *)data; - register uint16_t _crc = crc; - for (register uint32_t i = length; i > 0; i--) - _crc = (_crc >> 8) ^ CRC_Table16[(_crc ^ *p++) & 0xff]; - return _crc; + register uint8_t *p = (uint8_t *)data; + register uint16_t _crc = crc; + + for (register uint32_t i = length; i > 0; i--) { + _crc = (_crc >> 8) ^ CRC_Table16[(_crc ^ *p++) & 0xff]; + } + return _crc; } /** @@ -179,7 +182,7 @@ uint16_t PIOS_CRC16_updateCRC(uint16_t crc, const uint8_t* data, int32_t length) */ uint32_t PIOS_CRC32_updateByte(uint32_t crc, const uint8_t data) { - return ((crc << 8) ^ CRC_Table32[(crc >> 24) ^ data]); + return (crc << 8) ^ CRC_Table32[(crc >> 24) ^ data]; } /** @@ -189,11 +192,13 @@ uint32_t PIOS_CRC32_updateByte(uint32_t crc, const uint8_t data) * @param[in] length Number of bytes to process * @returns Updated CRC */ -uint32_t PIOS_CRC32_updateCRC(uint32_t crc, const uint8_t* data, int32_t length) +uint32_t PIOS_CRC32_updateCRC(uint32_t crc, const uint8_t *data, int32_t length) { - register uint8_t *p = (uint8_t *)data; - register uint32_t _crc = crc; - for (register uint32_t i = length; i > 0; i--) - _crc = (_crc << 8) ^ CRC_Table32[(_crc >> 24) ^ *p++]; - return _crc; + register uint8_t *p = (uint8_t *)data; + register uint32_t _crc = crc; + + for (register uint32_t i = length; i > 0; i--) { + _crc = (_crc << 8) ^ CRC_Table32[(_crc >> 24) ^ *p++]; + } + return _crc; } diff --git a/flight/pios/common/pios_etasv3.c b/flight/pios/common/pios_etasv3.c index 33090ba5e..7d773d95e 100644 --- a/flight/pios/common/pios_etasv3.c +++ b/flight/pios/common/pios_etasv3.c @@ -1,30 +1,30 @@ /** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_ETASV3 ETASV3 Functions - * @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3 - * @{ - * - * @file pios_etasv3.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief ETASV3 Airspeed Sensor Driver - * @see The GNU Public License (GPL) Version 3 - * - ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_ETASV3 ETASV3 Functions + * @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3 + * @{ + * + * @file pios_etasv3.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief ETASV3 Airspeed Sensor Driver + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,31 +32,31 @@ #ifdef PIOS_INCLUDE_ETASV3 -static bool PIOS_ETASV3_Read(uint8_t * buffer, uint8_t len) +static bool PIOS_ETASV3_Read(uint8_t *buffer, uint8_t len) { - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = ETASV3_I2C_ADDR, - .rw = PIOS_I2C_TXN_READ, - .len = len, - .buf = buffer, - } - }; + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = ETASV3_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; - return PIOS_I2C_Transfer(PIOS_I2C_ETASV3_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_ETASV3_ADAPTER, txn_list, NELEMENTS(txn_list)); } -int16_t PIOS_ETASV3_ReadAirspeed (void) +int16_t PIOS_ETASV3_ReadAirspeed(void) { - uint8_t airspeed_raw[2]; + uint8_t airspeed_raw[2]; - if (PIOS_ETASV3_Read(airspeed_raw, sizeof(airspeed_raw)) != 0) { - /* Failed to read airspeed */ - return -1; - } + if (PIOS_ETASV3_Read(airspeed_raw, sizeof(airspeed_raw)) != 0) { + /* Failed to read airspeed */ + return -1; + } - return (airspeed_raw[0] | (airspeed_raw[1]<<8)); + return airspeed_raw[0] | (airspeed_raw[1] << 8); } #endif /* PIOS_INCLUDE_ETASV3 */ diff --git a/flight/pios/common/pios_flash_jedec.c b/flight/pios/common/pios_flash_jedec.c index a05431e19..6d5050990 100644 --- a/flight/pios/common/pios_flash_jedec.c +++ b/flight/pios/common/pios_flash_jedec.c @@ -36,122 +36,129 @@ #include "pios_flash_jedec_priv.h" #include "pios_flash_jedec_catalog.h" -#define JEDEC_WRITE_ENABLE 0x06 -#define JEDEC_WRITE_DISABLE 0x04 -#define JEDEC_READ_STATUS 0x05 -#define JEDEC_WRITE_STATUS 0x01 -#define JEDEC_READ_DATA 0x03 -#define JEDEC_FAST_READ 0x0b -#define JEDEC_DEVICE_ID 0x9F -#define JEDEC_PAGE_WRITE 0x02 +#define JEDEC_WRITE_ENABLE 0x06 +#define JEDEC_WRITE_DISABLE 0x04 +#define JEDEC_READ_STATUS 0x05 +#define JEDEC_WRITE_STATUS 0x01 +#define JEDEC_READ_DATA 0x03 +#define JEDEC_FAST_READ 0x0b +#define JEDEC_DEVICE_ID 0x9F +#define JEDEC_PAGE_WRITE 0x02 -#define JEDEC_STATUS_BUSY 0x01 -#define JEDEC_STATUS_WRITEPROTECT 0x02 -#define JEDEC_STATUS_BP0 0x04 -#define JEDEC_STATUS_BP1 0x08 -#define JEDEC_STATUS_BP2 0x10 -#define JEDEC_STATUS_TP 0x20 -#define JEDEC_STATUS_SEC 0x40 -#define JEDEC_STATUS_SRP0 0x80 +#define JEDEC_STATUS_BUSY 0x01 +#define JEDEC_STATUS_WRITEPROTECT 0x02 +#define JEDEC_STATUS_BP0 0x04 +#define JEDEC_STATUS_BP1 0x08 +#define JEDEC_STATUS_BP2 0x10 +#define JEDEC_STATUS_TP 0x20 +#define JEDEC_STATUS_SEC 0x40 +#define JEDEC_STATUS_SRP0 0x80 enum pios_jedec_dev_magic { - PIOS_JEDEC_DEV_MAGIC = 0xcb55aa55, + PIOS_JEDEC_DEV_MAGIC = 0xcb55aa55, }; -//! Device handle structure +// ! Device handle structure struct jedec_flash_dev { - uint32_t spi_id; - uint32_t slave_num; - bool claimed; + uint32_t spi_id; + uint32_t slave_num; + bool claimed; - uint8_t manufacturer; - uint8_t memorytype; - uint8_t capacity; + uint8_t manufacturer; + uint8_t memorytype; + uint8_t capacity; - const struct pios_flash_jedec_cfg * cfg; + const struct pios_flash_jedec_cfg *cfg; #if defined(FLASH_FREERTOS) - xSemaphoreHandle transaction_lock; + xSemaphoreHandle transaction_lock; #endif - enum pios_jedec_dev_magic magic; + enum pios_jedec_dev_magic magic; }; -//! Private functions -static int32_t PIOS_Flash_Jedec_Validate(struct jedec_flash_dev * flash_dev); -static struct jedec_flash_dev * PIOS_Flash_Jedec_alloc(void); +// ! Private functions +static int32_t PIOS_Flash_Jedec_Validate(struct jedec_flash_dev *flash_dev); +static struct jedec_flash_dev *PIOS_Flash_Jedec_alloc(void); -static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev * flash_dev); -static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev * flash_dev); -static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev * flash_dev); -static int32_t PIOS_Flash_Jedec_ReleaseBus(struct jedec_flash_dev * flash_dev); -static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev * flash_dev); -static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev * flash_dev); +static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev *flash_dev); +static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev); +static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev); +static int32_t PIOS_Flash_Jedec_ReleaseBus(struct jedec_flash_dev *flash_dev); +static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev); +static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev *flash_dev); /** * @brief Allocate a new device */ -static struct jedec_flash_dev * PIOS_Flash_Jedec_alloc(void) +static struct jedec_flash_dev *PIOS_Flash_Jedec_alloc(void) { - struct jedec_flash_dev * flash_dev; + struct jedec_flash_dev *flash_dev; - flash_dev = (struct jedec_flash_dev *)pvPortMalloc(sizeof(*flash_dev)); - if (!flash_dev) return (NULL); + flash_dev = (struct jedec_flash_dev *)pvPortMalloc(sizeof(*flash_dev)); + if (!flash_dev) { + return NULL; + } - flash_dev->claimed = false; - flash_dev->magic = PIOS_JEDEC_DEV_MAGIC; + flash_dev->claimed = false; + flash_dev->magic = PIOS_JEDEC_DEV_MAGIC; #if defined(FLASH_FREERTOS) - flash_dev->transaction_lock = xSemaphoreCreateMutex(); + flash_dev->transaction_lock = xSemaphoreCreateMutex(); #endif - return(flash_dev); + return flash_dev; } /** * @brief Validate the handle to the spi device */ -static int32_t PIOS_Flash_Jedec_Validate(struct jedec_flash_dev * flash_dev) { - if (flash_dev == NULL) - return -1; - if (flash_dev->magic != PIOS_JEDEC_DEV_MAGIC) - return -2; - if (flash_dev->spi_id == 0) - return -3; - return 0; +static int32_t PIOS_Flash_Jedec_Validate(struct jedec_flash_dev *flash_dev) +{ + if (flash_dev == NULL) { + return -1; + } + if (flash_dev->magic != PIOS_JEDEC_DEV_MAGIC) { + return -2; + } + if (flash_dev->spi_id == 0) { + return -3; + } + return 0; } /** * @brief Initialize the flash device and enable write access */ -int32_t PIOS_Flash_Jedec_Init(uintptr_t * flash_id, uint32_t spi_id, uint32_t slave_num) +int32_t PIOS_Flash_Jedec_Init(uintptr_t *flash_id, uint32_t spi_id, uint32_t slave_num) { - struct jedec_flash_dev * flash_dev = PIOS_Flash_Jedec_alloc(); - if (!flash_dev) { - return -1; - } + struct jedec_flash_dev *flash_dev = PIOS_Flash_Jedec_alloc(); - flash_dev->spi_id = spi_id; - flash_dev->slave_num = slave_num; - flash_dev->cfg = NULL; + if (!flash_dev) { + return -1; + } - (void) PIOS_Flash_Jedec_ReadID(flash_dev); + flash_dev->spi_id = spi_id; + flash_dev->slave_num = slave_num; + flash_dev->cfg = NULL; - for (uint32_t i = 0; i < pios_flash_jedec_catalog_size; ++i) { - const struct pios_flash_jedec_cfg flash_jedec_entry = pios_flash_jedec_catalog[i]; + (void)PIOS_Flash_Jedec_ReadID(flash_dev); - if ((flash_dev->manufacturer == flash_jedec_entry.expect_manufacturer) - && (flash_dev->memorytype == flash_jedec_entry.expect_memorytype) - && (flash_dev->capacity == flash_jedec_entry.expect_capacity)) { - flash_dev->cfg = &pios_flash_jedec_catalog[i]; - break; - } - } + for (uint32_t i = 0; i < pios_flash_jedec_catalog_size; ++i) { + const struct pios_flash_jedec_cfg flash_jedec_entry = pios_flash_jedec_catalog[i]; - if (!flash_dev->cfg) { - return -1; - } + if ((flash_dev->manufacturer == flash_jedec_entry.expect_manufacturer) + && (flash_dev->memorytype == flash_jedec_entry.expect_memorytype) + && (flash_dev->capacity == flash_jedec_entry.expect_capacity)) { + flash_dev->cfg = &pios_flash_jedec_catalog[i]; + break; + } + } - /* Give back a handle to this flash device */ - *flash_id = (uintptr_t) flash_dev; + if (!flash_dev->cfg) { + return -1; + } - return 0; + /* Give back a handle to this flash device */ + *flash_id = (uintptr_t)flash_dev; + + return 0; } @@ -159,99 +166,105 @@ int32_t PIOS_Flash_Jedec_Init(uintptr_t * flash_id, uint32_t spi_id, uint32_t sl * @brief Claim the SPI bus for flash use and assert CS pin * @return 0 for sucess, -1 for failure to get semaphore */ -static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev * flash_dev) +static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev) { - if (PIOS_SPI_ClaimBus(flash_dev->spi_id) < 0) - return -1; + if (PIOS_SPI_ClaimBus(flash_dev->spi_id) < 0) { + return -1; + } - PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 0); - flash_dev->claimed = true; + PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 0); + flash_dev->claimed = true; - return 0; + return 0; } /** * @brief Release the SPI bus sempahore and ensure flash chip not using bus */ -static int32_t PIOS_Flash_Jedec_ReleaseBus(struct jedec_flash_dev * flash_dev) +static int32_t PIOS_Flash_Jedec_ReleaseBus(struct jedec_flash_dev *flash_dev) { - PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 1); - PIOS_SPI_ReleaseBus(flash_dev->spi_id); - flash_dev->claimed = false; - return 0; + PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 1); + PIOS_SPI_ReleaseBus(flash_dev->spi_id); + flash_dev->claimed = false; + return 0; } /** * @brief Returns if the flash chip is busy * @returns -1 for failure, 0 for not busy, 1 for busy */ -static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev * flash_dev) +static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev *flash_dev) { - int32_t status = PIOS_Flash_Jedec_ReadStatus(flash_dev); - if (status < 0) - return -1; - return status & JEDEC_STATUS_BUSY; + int32_t status = PIOS_Flash_Jedec_ReadStatus(flash_dev); + + if (status < 0) { + return -1; + } + return status & JEDEC_STATUS_BUSY; } /** * @brief Execute the write enable instruction and returns the status * @returns 0 if successful, -1 if unable to claim bus */ -static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev * flash_dev) +static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev) { - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + return -1; + } - uint8_t out[] = {JEDEC_WRITE_ENABLE}; - PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL); - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + uint8_t out[] = { JEDEC_WRITE_ENABLE }; + PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return 0; + return 0; } /** * @brief Read the status register from flash chip and return it */ -static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev * flash_dev) +static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev) { - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) - return -1; + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) { + return -1; + } - uint8_t out[2] = {JEDEC_READ_STATUS, 0}; - uint8_t in[2] = {0,0}; - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,in,sizeof(out),NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -2; - } + uint8_t out[2] = { JEDEC_READ_STATUS, 0 }; + uint8_t in[2] = { 0, 0 }; + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, in, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -2; + } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return in[1]; + return in[1]; } /** * @brief Read the status register from flash chip and return it */ -static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev * flash_dev) +static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev *flash_dev) { - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) - return -2; + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) { + return -2; + } - uint8_t out[] = {JEDEC_DEVICE_ID, 0, 0, 0}; - uint8_t in[4]; - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,in,sizeof(out),NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -3; - } + uint8_t out[] = { JEDEC_DEVICE_ID, 0, 0, 0 }; + uint8_t in[4]; + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, in, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -3; + } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - flash_dev->manufacturer = in[1]; - flash_dev->memorytype = in[2]; - flash_dev->capacity = in[3]; + flash_dev->manufacturer = in[1]; + flash_dev->memorytype = in[2]; + flash_dev->capacity = in[3]; - return flash_dev->manufacturer; + return flash_dev->manufacturer; } /********************************** @@ -269,17 +282,19 @@ static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev * flash_dev) */ static int32_t PIOS_Flash_Jedec_StartTransaction(uintptr_t flash_id) { - struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id; + struct jedec_flash_dev *flash_dev = (struct jedec_flash_dev *)flash_id; - if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { + return -1; + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreTake(flash_dev->transaction_lock, portMAX_DELAY) != pdTRUE) - return -2; + if (xSemaphoreTake(flash_dev->transaction_lock, portMAX_DELAY) != pdTRUE) { + return -2; + } #endif - return 0; + return 0; } /** @@ -288,32 +303,34 @@ static int32_t PIOS_Flash_Jedec_StartTransaction(uintptr_t flash_id) */ static int32_t PIOS_Flash_Jedec_EndTransaction(uintptr_t flash_id) { - struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id; + struct jedec_flash_dev *flash_dev = (struct jedec_flash_dev *)flash_id; - if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { + return -1; + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreGive(flash_dev->transaction_lock) != pdTRUE) - return -2; + if (xSemaphoreGive(flash_dev->transaction_lock) != pdTRUE) { + return -2; + } #endif - return 0; + return 0; } -#else /* FLASH_USE_FREERTOS_LOCKS */ +#else /* FLASH_USE_FREERTOS_LOCKS */ static int32_t PIOS_Flash_Jedec_StartTransaction(__attribute__((unused)) uintptr_t flash_id) { - return 0; + return 0; } static int32_t PIOS_Flash_Jedec_EndTransaction(__attribute__((unused)) uintptr_t flash_id) { - return 0; + return 0; } -#endif /* FLASH_USE_FREERTOS_LOCKS */ +#endif /* FLASH_USE_FREERTOS_LOCKS */ /** * @brief Erase a sector on the flash chip @@ -324,35 +341,38 @@ static int32_t PIOS_Flash_Jedec_EndTransaction(__attribute__((unused)) uintptr_t */ static int32_t PIOS_Flash_Jedec_EraseSector(uintptr_t flash_id, uint32_t addr) { - struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id; + struct jedec_flash_dev *flash_dev = (struct jedec_flash_dev *)flash_id; - if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { + return -1; + } - uint8_t ret; - uint8_t out[] = {flash_dev->cfg->sector_erase, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff}; + uint8_t ret; + uint8_t out[] = { flash_dev->cfg->sector_erase, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff }; - if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) - return ret; + if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) { + return ret; + } - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + return -1; + } - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -2; - } + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -2; + } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - // Keep polling when bus is busy too - while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) { + // Keep polling when bus is busy too + while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) { #if defined(FLASH_FREERTOS) - vTaskDelay(1); + vTaskDelay(1); #endif - } + } - return 0; + return 0; } /** @@ -361,45 +381,47 @@ static int32_t PIOS_Flash_Jedec_EraseSector(uintptr_t flash_id, uint32_t addr) */ static int32_t PIOS_Flash_Jedec_EraseChip(uintptr_t flash_id) { - struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id; + struct jedec_flash_dev *flash_dev = (struct jedec_flash_dev *)flash_id; - if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { + return -1; + } - uint8_t ret; - uint8_t out[] = {flash_dev->cfg->chip_erase}; + uint8_t ret; + uint8_t out[] = { flash_dev->cfg->chip_erase }; - if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) - return ret; + if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) { + return ret; + } - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + return -1; + } - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -2; - } + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -2; + } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - // Keep polling when bus is busy too - int i = 0; - while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) { + // Keep polling when bus is busy too + int i = 0; + while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) { #if defined(FLASH_FREERTOS) - vTaskDelay(1); - if ((i++) % 100 == 0) { + vTaskDelay(1); + if ((i++) % 100 == 0) { #else - if ((i++) % 10000 == 0) { + if ((i++) % 10000 == 0) { #endif #ifdef PIOS_LED_HEARTBEAT - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); #endif - } + } + } -} - - return 0; + return 0; } @@ -413,62 +435,70 @@ static int32_t PIOS_Flash_Jedec_EraseChip(uintptr_t flash_id) * @retval -2 Size exceeds 256 bytes * @retval -3 Length to write would wrap around page boundary */ -static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len) +static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) { - struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id; + struct jedec_flash_dev *flash_dev = (struct jedec_flash_dev *)flash_id; - if(PIOS_Flash_Jedec_Validate(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { + return -1; + } - uint8_t ret; - uint8_t out[4] = {JEDEC_PAGE_WRITE, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff}; + uint8_t ret; + uint8_t out[4] = { JEDEC_PAGE_WRITE, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff }; - /* Can only write one page at a time */ - if (len > 0x100) - return -2; + /* Can only write one page at a time */ + if (len > 0x100) { + return -2; + } - /* Ensure number of bytes fits after starting address before end of page */ - if (((addr & 0xff) + len) > 0x100) - return -3; + /* Ensure number of bytes fits after starting address before end of page */ + if (((addr & 0xff) + len) > 0x100) { + return -3; + } - if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) - return ret; + if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) { + return ret; + } - /* Execute write page command and clock in address. Keep CS asserted */ - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) - return -1; + /* Execute write page command and clock in address. Keep CS asserted */ + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + return -1; + } - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -1; - } + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -1; + } - /* Clock out data to flash */ - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,data,NULL,len,NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -1; - } + /* Clock out data to flash */ + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, data, NULL, len, NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -1; + } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - // Keep polling when bus is busy too + // Keep polling when bus is busy too #if defined(FLASH_FREERTOS) - while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) { - vTaskDelay(1); - } + while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) { + vTaskDelay(1); + } #else - // Query status this way to prevent accel chip locking us out - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) - return -1; + // Query status this way to prevent accel chip locking us out + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) { + return -1; + } - PIOS_SPI_TransferByte(flash_dev->spi_id, JEDEC_READ_STATUS); - while (PIOS_SPI_TransferByte(flash_dev->spi_id, JEDEC_READ_STATUS) & JEDEC_STATUS_BUSY); + PIOS_SPI_TransferByte(flash_dev->spi_id, JEDEC_READ_STATUS); + while (PIOS_SPI_TransferByte(flash_dev->spi_id, JEDEC_READ_STATUS) & JEDEC_STATUS_BUSY) { + ; + } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); #endif - return 0; + return 0; } /** @@ -483,53 +513,58 @@ static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uin */ static int32_t PIOS_Flash_Jedec_WriteChunks(uintptr_t flash_id, uint32_t addr, struct pios_flash_chunk chunks[], uint32_t num) { - struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id; + struct jedec_flash_dev *flash_dev = (struct jedec_flash_dev *)flash_id; - if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { + return -1; + } - uint8_t ret; - uint8_t out[4] = {JEDEC_PAGE_WRITE, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff}; + uint8_t ret; + uint8_t out[4] = { JEDEC_PAGE_WRITE, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff }; - /* Can only write one page at a time */ - uint32_t len = 0; - for (uint32_t i = 0; i < num; i++) - len += chunks[i].len; + /* Can only write one page at a time */ + uint32_t len = 0; + for (uint32_t i = 0; i < num; i++) { + len += chunks[i].len; + } - if (len > 0x100) - return -2; + if (len > 0x100) { + return -2; + } - /* Ensure number of bytes fits after starting address before end of page */ - if (((addr & 0xff) + len) > 0x100) - return -3; + /* Ensure number of bytes fits after starting address before end of page */ + if (((addr & 0xff) + len) > 0x100) { + return -3; + } - if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) - return ret; + if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) { + return ret; + } - /* Execute write page command and clock in address. Keep CS asserted */ - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) - return -1; + /* Execute write page command and clock in address. Keep CS asserted */ + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + return -1; + } - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -1; - } + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -1; + } - for (uint32_t i = 0; i < num; i++) { - struct pios_flash_chunk * chunk = &chunks[i]; + for (uint32_t i = 0; i < num; i++) { + struct pios_flash_chunk *chunk = &chunks[i]; - /* Clock out data to flash */ - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,chunk->addr,NULL,chunk->len,NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -1; - } + /* Clock out data to flash */ + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, chunk->addr, NULL, chunk->len, NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -1; + } + } + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + // Skip checking for busy with this to get OS running again fast - // Skip checking for busy with this to get OS running again fast - - return 0; + return 0; } /** @@ -540,44 +575,46 @@ static int32_t PIOS_Flash_Jedec_WriteChunks(uintptr_t flash_id, uint32_t addr, s * @return Zero if success or error code * @retval -1 Unable to claim SPI bus */ -static int32_t PIOS_Flash_Jedec_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len) +static int32_t PIOS_Flash_Jedec_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) { - struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id; + struct jedec_flash_dev *flash_dev = (struct jedec_flash_dev *)flash_id; - if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) - return -1; + if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { + return -1; + } - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) == -1) - return -1; + if (PIOS_Flash_Jedec_ClaimBus(flash_dev) == -1) { + return -1; + } - /* Execute read command and clock in address. Keep CS asserted */ - uint8_t out[] = {JEDEC_READ_DATA, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff}; + /* Execute read command and clock in address. Keep CS asserted */ + uint8_t out[] = { JEDEC_READ_DATA, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff }; - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -2; - } + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -2; + } - /* Copy the transfer data to the buffer */ - if (PIOS_SPI_TransferBlock(flash_dev->spi_id,NULL,data,len,NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -3; - } + /* Copy the transfer data to the buffer */ + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, NULL, data, len, NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -3; + } - PIOS_Flash_Jedec_ReleaseBus(flash_dev); + PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return 0; + return 0; } /* Provide a flash driver to external drivers */ const struct pios_flash_driver pios_jedec_flash_driver = { - .start_transaction = PIOS_Flash_Jedec_StartTransaction, - .end_transaction = PIOS_Flash_Jedec_EndTransaction, - .erase_chip = PIOS_Flash_Jedec_EraseChip, - .erase_sector = PIOS_Flash_Jedec_EraseSector, - .write_chunks = PIOS_Flash_Jedec_WriteChunks, - .write_data = PIOS_Flash_Jedec_WriteData, - .read_data = PIOS_Flash_Jedec_ReadData, + .start_transaction = PIOS_Flash_Jedec_StartTransaction, + .end_transaction = PIOS_Flash_Jedec_EndTransaction, + .erase_chip = PIOS_Flash_Jedec_EraseChip, + .erase_sector = PIOS_Flash_Jedec_EraseSector, + .write_chunks = PIOS_Flash_Jedec_WriteChunks, + .write_data = PIOS_Flash_Jedec_WriteData, + .read_data = PIOS_Flash_Jedec_ReadData, }; #endif /* PIOS_INCLUDE_FLASH */ diff --git a/flight/pios/common/pios_flashfs_logfs.c b/flight/pios/common/pios_flashfs_logfs.c index f72d07112..fe626b7d0 100644 --- a/flight/pios/common/pios_flashfs_logfs.c +++ b/flight/pios/common/pios_flashfs_logfs.c @@ -40,25 +40,25 @@ */ enum pios_flashfs_logfs_dev_magic { - PIOS_FLASHFS_LOGFS_DEV_MAGIC = 0x94938201, + PIOS_FLASHFS_LOGFS_DEV_MAGIC = 0x94938201, }; struct logfs_state { - enum pios_flashfs_logfs_dev_magic magic; - const struct flashfs_logfs_cfg * cfg; - bool mounted; - uint8_t active_arena_id; + enum pios_flashfs_logfs_dev_magic magic; + const struct flashfs_logfs_cfg *cfg; + bool mounted; + uint8_t active_arena_id; - /* NOTE: num_active_slots + num_free_slots will not typically add - * up to the number of slots in the arena since some of the - * slots will be obsolete or otherwise invalidated - */ - uint16_t num_free_slots; /* slots in free state */ - uint16_t num_active_slots; /* slots in active state */ + /* NOTE: num_active_slots + num_free_slots will not typically add + * up to the number of slots in the arena since some of the + * slots will be obsolete or otherwise invalidated + */ + uint16_t num_free_slots; /* slots in free state */ + uint16_t num_active_slots; /* slots in active state */ - /* Underlying flash driver glue */ - const struct pios_flash_driver * driver; - uintptr_t flash_id; + /* Underlying flash driver glue */ + const struct pios_flash_driver *driver; + uintptr_t flash_id; }; /* @@ -69,14 +69,14 @@ struct logfs_state { * @brief Return the offset in flash of a particular slot within an arena * @return address of the requested slot */ -static uintptr_t logfs_get_addr(const struct logfs_state * logfs, uint8_t arena_id, uint16_t slot_id) +static uintptr_t logfs_get_addr(const struct logfs_state *logfs, uint8_t arena_id, uint16_t slot_id) { - PIOS_Assert(arena_id < (logfs->cfg->total_fs_size / logfs->cfg->arena_size)); - PIOS_Assert(slot_id < (logfs->cfg->arena_size / logfs->cfg->slot_size)); + PIOS_Assert(arena_id < (logfs->cfg->total_fs_size / logfs->cfg->arena_size)); + PIOS_Assert(slot_id < (logfs->cfg->arena_size / logfs->cfg->slot_size)); - return (logfs->cfg->start_offset + - (arena_id * logfs->cfg->arena_size) + - (slot_id * logfs->cfg->slot_size)); + return logfs->cfg->start_offset + + (arena_id * logfs->cfg->arena_size) + + (slot_id * logfs->cfg->slot_size); } /* @@ -85,65 +85,65 @@ static uintptr_t logfs_get_addr(const struct logfs_state * logfs, uint8_t arena_ * of earlier ones in NOR flash without an erase cycle. */ enum arena_state { - /* - * The STM32F30X flash subsystem is only capable of - * writing words or halfwords. In this case we use halfwords. - * In addition to that it is only capable to write to erased - * cells (0xffff) or write a cell from anything to (0x0000). - * To cope with this, the F3 needs carefully crafted enum values. - * For this to work the underlying flash driver has to - * check each halfword if it has changed before writing. - */ - ARENA_STATE_ERASED = 0xFFFFFFFF, - ARENA_STATE_RESERVED = 0xE6E6FFFF, - ARENA_STATE_ACTIVE = 0xE6E66666, - ARENA_STATE_OBSOLETE = 0x00000000, + /* + * The STM32F30X flash subsystem is only capable of + * writing words or halfwords. In this case we use halfwords. + * In addition to that it is only capable to write to erased + * cells (0xffff) or write a cell from anything to (0x0000). + * To cope with this, the F3 needs carefully crafted enum values. + * For this to work the underlying flash driver has to + * check each halfword if it has changed before writing. + */ + ARENA_STATE_ERASED = 0xFFFFFFFF, + ARENA_STATE_RESERVED = 0xE6E6FFFF, + ARENA_STATE_ACTIVE = 0xE6E66666, + ARENA_STATE_OBSOLETE = 0x00000000, }; struct arena_header { - uint32_t magic; - enum arena_state state; + uint32_t magic; + enum arena_state state; } __attribute__((packed)); /**************************************** - * Arena life-cycle transition functions - ****************************************/ +* Arena life-cycle transition functions +****************************************/ /** * @brief Erases all sectors within the given arena and sets arena to erased state. * @return 0 if success, < 0 on failure * @note Must be called while holding the flash transaction lock */ -static int32_t logfs_erase_arena(const struct logfs_state * logfs, uint8_t arena_id) +static int32_t logfs_erase_arena(const struct logfs_state *logfs, uint8_t arena_id) { - uintptr_t arena_addr = logfs_get_addr (logfs, arena_id, 0); + uintptr_t arena_addr = logfs_get_addr(logfs, arena_id, 0); - /* Erase all of the sectors in the arena */ - for (uint8_t sector_id = 0; - sector_id < (logfs->cfg->arena_size / logfs->cfg->sector_size); - sector_id++) { - if (logfs->driver->erase_sector(logfs->flash_id, - arena_addr + (sector_id * logfs->cfg->sector_size))) { - return -1; - } - } + /* Erase all of the sectors in the arena */ + for (uint8_t sector_id = 0; + sector_id < (logfs->cfg->arena_size / logfs->cfg->sector_size); + sector_id++) { + if (logfs->driver->erase_sector(logfs->flash_id, + arena_addr + (sector_id * logfs->cfg->sector_size))) { + return -1; + } + } - /* Mark this arena as fully erased */ - struct arena_header arena_hdr = { - .magic = logfs->cfg->fs_magic, - .state = ARENA_STATE_ERASED, - }; + /* Mark this arena as fully erased */ + struct arena_header arena_hdr = { + .magic = logfs->cfg->fs_magic, + .state = ARENA_STATE_ERASED, + }; - if (logfs->driver->write_data(logfs->flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof(arena_hdr)) != 0) { - return -2; - } + if (logfs->driver->write_data(logfs->flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + return -2; + } - /* Arena is ready to be activated */ - return 0; + /* Arena is ready to be activated */ + return 0; } /** @@ -152,36 +152,37 @@ static int32_t logfs_erase_arena(const struct logfs_state * logfs, uint8_t arena * @note Arena must have been previously erased before calling this * @note Must be called while holding the flash transaction lock */ -static int32_t logfs_reserve_arena (const struct logfs_state * logfs, uint8_t arena_id) +static int32_t logfs_reserve_arena(const struct logfs_state *logfs, uint8_t arena_id) { - uintptr_t arena_addr = logfs_get_addr (logfs, arena_id, 0); + uintptr_t arena_addr = logfs_get_addr(logfs, arena_id, 0); - /* Read in the current arena header */ - struct arena_header arena_hdr; - if (logfs->driver->read_data(logfs->flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof(arena_hdr)) != 0) { - return -1; - } - if (arena_hdr.state != ARENA_STATE_ERASED) { - /* Arena was not erased, can't reserve it */ - return -2; - } + /* Read in the current arena header */ + struct arena_header arena_hdr; - /* Set the arena state to reserved */ - arena_hdr.state = ARENA_STATE_RESERVED; + if (logfs->driver->read_data(logfs->flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + return -1; + } + if (arena_hdr.state != ARENA_STATE_ERASED) { + /* Arena was not erased, can't reserve it */ + return -2; + } - /* Write the arena header back to flash */ - if (logfs->driver->write_data(logfs->flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof(arena_hdr)) != 0) { - return -3; - } + /* Set the arena state to reserved */ + arena_hdr.state = ARENA_STATE_RESERVED; - /* Arena is ready to be filled */ - return 0; + /* Write the arena header back to flash */ + if (logfs->driver->write_data(logfs->flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + return -3; + } + + /* Arena is ready to be filled */ + return 0; } /** @@ -189,22 +190,23 @@ static int32_t logfs_reserve_arena (const struct logfs_state * logfs, uint8_t ar * @return 0 if success, < 0 on failure * @note Must be called while holding the flash transaction lock */ -static int32_t logfs_erase_all_arenas(const struct logfs_state * logfs) +static int32_t logfs_erase_all_arenas(const struct logfs_state *logfs) { - uint16_t num_arenas = logfs->cfg->total_fs_size / logfs->cfg->arena_size; + uint16_t num_arenas = logfs->cfg->total_fs_size / logfs->cfg->arena_size; - for (uint16_t arena = 0; arena < num_arenas; arena++) { + for (uint16_t arena = 0; arena < num_arenas; arena++) { #ifdef PIOS_LED_HEARTBEAT - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); #endif #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_Clear(); + PIOS_WDG_Clear(); #endif - if (logfs_erase_arena(logfs, arena) != 0) - return -1; - } + if (logfs_erase_arena(logfs, arena) != 0) { + return -1; + } + } - return 0; + return 0; } /** @@ -213,36 +215,37 @@ static int32_t logfs_erase_all_arenas(const struct logfs_state * logfs) * @note Arena must have been previously erased or reserved before calling this * @note Must be called while holding the flash transaction lock */ -static int32_t logfs_activate_arena(const struct logfs_state * logfs, uint8_t arena_id) +static int32_t logfs_activate_arena(const struct logfs_state *logfs, uint8_t arena_id) { - uintptr_t arena_addr = logfs_get_addr(logfs, arena_id, 0); + uintptr_t arena_addr = logfs_get_addr(logfs, arena_id, 0); - /* Make sure this arena has been previously erased */ - struct arena_header arena_hdr; - if (logfs->driver->read_data(logfs->flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof (arena_hdr)) != 0) { - /* Failed to read arena header */ - return -1; - } - if ((arena_hdr.state != ARENA_STATE_RESERVED) && - (arena_hdr.state != ARENA_STATE_ERASED)) { - /* Arena was not erased or reserved, can't activate it */ - return -2; - } + /* Make sure this arena has been previously erased */ + struct arena_header arena_hdr; - /* Mark this arena as active */ - arena_hdr.state = ARENA_STATE_ACTIVE; - if (logfs->driver->write_data(logfs->flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof(arena_hdr)) != 0) { - return -3; - } + if (logfs->driver->read_data(logfs->flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + /* Failed to read arena header */ + return -1; + } + if ((arena_hdr.state != ARENA_STATE_RESERVED) && + (arena_hdr.state != ARENA_STATE_ERASED)) { + /* Arena was not erased or reserved, can't activate it */ + return -2; + } - /* The arena is now activated and the log may be mounted */ - return 0; + /* Mark this arena as active */ + arena_hdr.state = ARENA_STATE_ACTIVE; + if (logfs->driver->write_data(logfs->flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + return -3; + } + + /* The arena is now activated and the log may be mounted */ + return 0; } /** @@ -251,39 +254,39 @@ static int32_t logfs_activate_arena(const struct logfs_state * logfs, uint8_t ar * @note Arena must have been previously active before calling this * @note Must be called while holding the flash transaction lock */ -static int32_t logfs_obsolete_arena(const struct logfs_state * logfs, uint8_t arena_id) +static int32_t logfs_obsolete_arena(const struct logfs_state *logfs, uint8_t arena_id) { - uintptr_t arena_addr = logfs_get_addr (logfs, arena_id, 0); + uintptr_t arena_addr = logfs_get_addr(logfs, arena_id, 0); - /* We shouldn't be retiring the currently active arena */ - PIOS_Assert(!logfs->mounted); + /* We shouldn't be retiring the currently active arena */ + PIOS_Assert(!logfs->mounted); - /* Make sure this arena was previously active */ - struct arena_header arena_hdr; - if (logfs->driver->read_data(logfs->flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof (arena_hdr)) != 0) { - /* Failed to read arena header */ - return -1; - } + /* Make sure this arena was previously active */ + struct arena_header arena_hdr; + if (logfs->driver->read_data(logfs->flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + /* Failed to read arena header */ + return -1; + } - if (arena_hdr.state != ARENA_STATE_ACTIVE) { - /* Arena was not previously active, can't obsolete it */ - return -2; - } + if (arena_hdr.state != ARENA_STATE_ACTIVE) { + /* Arena was not previously active, can't obsolete it */ + return -2; + } - /* Mark this arena as obsolete */ - arena_hdr.state = ARENA_STATE_OBSOLETE; - if (logfs->driver->write_data(logfs->flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof(arena_hdr)) != 0) { - return -3; - } + /* Mark this arena as obsolete */ + arena_hdr.state = ARENA_STATE_OBSOLETE; + if (logfs->driver->write_data(logfs->flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + return -3; + } - /* Arena is now obsoleted */ - return 0; + /* Arena is now obsoleted */ + return 0; } /** @@ -293,34 +296,33 @@ static int32_t logfs_obsolete_arena(const struct logfs_state * logfs, uint8_t ar * @return -2 if failed to read arena header * @note Must be called while holding the flash transaction lock */ -static int32_t logfs_find_active_arena(const struct logfs_state * logfs) +static int32_t logfs_find_active_arena(const struct logfs_state *logfs) { - /* Search for the lowest numbered active arena */ - for (uint8_t arena_id = 0; - arena_id < logfs->cfg->total_fs_size / logfs->cfg->arena_size; - arena_id++) { - uintptr_t arena_addr = logfs_get_addr (logfs, arena_id, 0); - /* Load the arena header */ - struct arena_header arena_hdr; - if (logfs->driver->read_data(logfs->flash_id, - arena_addr, - (uint8_t *)&arena_hdr, - sizeof (arena_hdr)) != 0) { - return -2; - } - if ((arena_hdr.state == ARENA_STATE_ACTIVE) && - (arena_hdr.magic == logfs->cfg->fs_magic)) { - /* This is the first active arena */ - return arena_id; - } + /* Search for the lowest numbered active arena */ + for (uint8_t arena_id = 0; + arena_id < logfs->cfg->total_fs_size / logfs->cfg->arena_size; + arena_id++) { + uintptr_t arena_addr = logfs_get_addr(logfs, arena_id, 0); + /* Load the arena header */ + struct arena_header arena_hdr; + if (logfs->driver->read_data(logfs->flash_id, + arena_addr, + (uint8_t *)&arena_hdr, + sizeof(arena_hdr)) != 0) { + return -2; + } + if ((arena_hdr.state == ARENA_STATE_ACTIVE) && + (arena_hdr.magic == logfs->cfg->fs_magic)) { + /* This is the first active arena */ + return arena_id; + } #ifdef PIOS_INCLUDE_WDG PIOS_WDG_Clear(); #endif + } - } - - /* Didn't find an active arena */ - return -1; + /* Didn't find an active arena */ + return -1; } /* @@ -329,69 +331,69 @@ static int32_t logfs_find_active_arena(const struct logfs_state * logfs) * of earlier ones in NOR flash without an erase cycle. */ enum slot_state { - /* - * The STM32F30X flash subsystem is only capable of - * writing words or halfwords. In this case we use halfwords. - * In addition to that it is only capable to write to erased - * cells (0xffff) or write a cell from anything to (0x0000). - * To cope with this, the F3 needs carfully crafted enum values. - * For this to work the underlying flash driver has to - * check each halfword if it has changed before writing. - */ - SLOT_STATE_EMPTY = 0xFFFFFFFF, - SLOT_STATE_RESERVED = 0xFAFAFFFF, - SLOT_STATE_ACTIVE = 0xFAFAAAAA, - SLOT_STATE_OBSOLETE = 0x00000000, + /* + * The STM32F30X flash subsystem is only capable of + * writing words or halfwords. In this case we use halfwords. + * In addition to that it is only capable to write to erased + * cells (0xffff) or write a cell from anything to (0x0000). + * To cope with this, the F3 needs carfully crafted enum values. + * For this to work the underlying flash driver has to + * check each halfword if it has changed before writing. + */ + SLOT_STATE_EMPTY = 0xFFFFFFFF, + SLOT_STATE_RESERVED = 0xFAFAFFFF, + SLOT_STATE_ACTIVE = 0xFAFAAAAA, + SLOT_STATE_OBSOLETE = 0x00000000, }; struct slot_header { - enum slot_state state; - uint32_t obj_id; - uint16_t obj_inst_id; - uint16_t obj_size; + enum slot_state state; + uint32_t obj_id; + uint16_t obj_inst_id; + uint16_t obj_size; } __attribute__((packed)); /* NOTE: Must be called while holding the flash transaction lock */ -static int32_t logfs_raw_copy_bytes (const struct logfs_state * logfs, uintptr_t src_addr, uint16_t src_size, uintptr_t dst_addr) +static int32_t logfs_raw_copy_bytes(const struct logfs_state *logfs, uintptr_t src_addr, uint16_t src_size, uintptr_t dst_addr) { #define RAW_COPY_BLOCK_SIZE 16 - uint8_t data_block[RAW_COPY_BLOCK_SIZE]; + uint8_t data_block[RAW_COPY_BLOCK_SIZE]; - while (src_size) { - uint16_t blk_size; - if (src_size >= RAW_COPY_BLOCK_SIZE) { - /* Copy a full block */ - blk_size = RAW_COPY_BLOCK_SIZE; - } else { - /* Copy the remainder */ - blk_size = src_size; - } + while (src_size) { + uint16_t blk_size; + if (src_size >= RAW_COPY_BLOCK_SIZE) { + /* Copy a full block */ + blk_size = RAW_COPY_BLOCK_SIZE; + } else { + /* Copy the remainder */ + blk_size = src_size; + } - /* Read a block of data from source */ - if (logfs->driver->read_data(logfs->flash_id, - src_addr, - data_block, - blk_size) != 0) { - /* Failed to read next chunk from source */ - return -1; - } + /* Read a block of data from source */ + if (logfs->driver->read_data(logfs->flash_id, + src_addr, + data_block, + blk_size) != 0) { + /* Failed to read next chunk from source */ + return -1; + } - /* Write a block of data to destination */ - if (logfs->driver->write_data(logfs->flash_id, - dst_addr, - data_block, - blk_size) != 0) { - /* Failed to write chunk to destination */ - return -2; - } + /* Write a block of data to destination */ + if (logfs->driver->write_data(logfs->flash_id, + dst_addr, + data_block, + blk_size) != 0) { + /* Failed to write chunk to destination */ + return -2; + } - /* Update the src/dst pointers */ - src_size -= blk_size; - src_addr += blk_size; - dst_addr += blk_size; - } + /* Update the src/dst pointers */ + src_size -= blk_size; + src_addr += blk_size; + dst_addr += blk_size; + } - return 0; + return 0; } /* @@ -399,9 +401,9 @@ static int32_t logfs_raw_copy_bytes (const struct logfs_state * logfs, uintptr_t * true = all slots in the arena are in the ACTIVE state (ie. garbage collection won't free anything) * false = some slots in the arena are either currently free or could be free'd by garbage collection */ -static bool logfs_fs_is_full(const struct logfs_state * logfs) +static bool logfs_fs_is_full(const struct logfs_state *logfs) { - return (logfs->num_active_slots == (logfs->cfg->arena_size / logfs->cfg->slot_size) - 1); + return logfs->num_active_slots == (logfs->cfg->arena_size / logfs->cfg->slot_size) - 1; } /* @@ -409,478 +411,487 @@ static bool logfs_fs_is_full(const struct logfs_state * logfs) * true = there are no unwritten slots left in the log (garbage collection may or may not help) * false = there are still some entirely unused slots left in the log */ -static bool logfs_log_is_full(const struct logfs_state * logfs) +static bool logfs_log_is_full(const struct logfs_state *logfs) { - return (logfs->num_free_slots == 0); + return logfs->num_free_slots == 0; } -static int32_t logfs_unmount_log(struct logfs_state * logfs) +static int32_t logfs_unmount_log(struct logfs_state *logfs) { - PIOS_Assert (logfs->mounted); + PIOS_Assert(logfs->mounted); - logfs->num_active_slots = 0; - logfs->num_free_slots = 0; - logfs->mounted = false; + logfs->num_active_slots = 0; + logfs->num_free_slots = 0; + logfs->mounted = false; - return 0; + return 0; } -static int32_t logfs_mount_log(struct logfs_state * logfs, uint8_t arena_id) +static int32_t logfs_mount_log(struct logfs_state *logfs, uint8_t arena_id) { - PIOS_Assert (!logfs->mounted); + PIOS_Assert(!logfs->mounted); - logfs->num_active_slots = 0; - logfs->num_free_slots = 0; - logfs->active_arena_id = arena_id; + logfs->num_active_slots = 0; + logfs->num_free_slots = 0; + logfs->active_arena_id = arena_id; - /* Scan the log to find out how full it is */ - for (uint16_t slot_id = 1; - slot_id < (logfs->cfg->arena_size / logfs->cfg->slot_size); - slot_id++) { - struct slot_header slot_hdr; - uintptr_t slot_addr = logfs_get_addr (logfs, logfs->active_arena_id, slot_id); - if (logfs->driver->read_data(logfs->flash_id, - slot_addr, - (uint8_t *)&slot_hdr, - sizeof (slot_hdr)) != 0) { - return -1; - } + /* Scan the log to find out how full it is */ + for (uint16_t slot_id = 1; + slot_id < (logfs->cfg->arena_size / logfs->cfg->slot_size); + slot_id++) { + struct slot_header slot_hdr; + uintptr_t slot_addr = logfs_get_addr(logfs, logfs->active_arena_id, slot_id); + if (logfs->driver->read_data(logfs->flash_id, + slot_addr, + (uint8_t *)&slot_hdr, + sizeof(slot_hdr)) != 0) { + return -1; + } - /* - * Empty slots must be in a continguous block at the - * end of the arena. - */ - PIOS_Assert (slot_hdr.state == SLOT_STATE_EMPTY || - logfs->num_free_slots == 0); + /* + * Empty slots must be in a continguous block at the + * end of the arena. + */ + PIOS_Assert(slot_hdr.state == SLOT_STATE_EMPTY || + logfs->num_free_slots == 0); - switch (slot_hdr.state) { - case SLOT_STATE_EMPTY: - logfs->num_free_slots++; - break; - case SLOT_STATE_ACTIVE: - logfs->num_active_slots++; - break; - case SLOT_STATE_RESERVED: - case SLOT_STATE_OBSOLETE: - break; - } - } + switch (slot_hdr.state) { + case SLOT_STATE_EMPTY: + logfs->num_free_slots++; + break; + case SLOT_STATE_ACTIVE: + logfs->num_active_slots++; + break; + case SLOT_STATE_RESERVED: + case SLOT_STATE_OBSOLETE: + break; + } + } - /* Scan is complete, mark the arena mounted */ - logfs->active_arena_id = arena_id; - logfs->mounted = true; + /* Scan is complete, mark the arena mounted */ + logfs->active_arena_id = arena_id; + logfs->mounted = true; - return 0; + return 0; } -static bool PIOS_FLASHFS_Logfs_validate(const struct logfs_state * logfs) +static bool PIOS_FLASHFS_Logfs_validate(const struct logfs_state *logfs) { - return (logfs && (logfs->magic == PIOS_FLASHFS_LOGFS_DEV_MAGIC)); + return logfs && (logfs->magic == PIOS_FLASHFS_LOGFS_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) -static struct logfs_state * PIOS_FLASHFS_Logfs_alloc(void) +static struct logfs_state *PIOS_FLASHFS_Logfs_alloc(void) { - struct logfs_state * logfs; + struct logfs_state *logfs; - logfs = (struct logfs_state *)pvPortMalloc(sizeof(*logfs)); - if (!logfs) return (NULL); + logfs = (struct logfs_state *)pvPortMalloc(sizeof(*logfs)); + if (!logfs) { + return NULL; + } - logfs->magic = PIOS_FLASHFS_LOGFS_DEV_MAGIC; - return(logfs); + logfs->magic = PIOS_FLASHFS_LOGFS_DEV_MAGIC; + return logfs; } -static void PIOS_FLASHFS_Logfs_free(struct logfs_state * logfs) +static void PIOS_FLASHFS_Logfs_free(struct logfs_state *logfs) { - /* Invalidate the magic */ - logfs->magic = ~PIOS_FLASHFS_LOGFS_DEV_MAGIC; - vPortFree(logfs); + /* Invalidate the magic */ + logfs->magic = ~PIOS_FLASHFS_LOGFS_DEV_MAGIC; + vPortFree(logfs); } #else static struct logfs_state pios_flashfs_logfs_devs[PIOS_FLASHFS_LOGFS_MAX_DEVS]; static uint8_t pios_flashfs_logfs_num_devs; -static struct logfs_state * PIOS_FLASHFS_Logfs_alloc(void) +static struct logfs_state *PIOS_FLASHFS_Logfs_alloc(void) { - struct logfs_state * logfs; + struct logfs_state *logfs; - if (pios_flashfs_logfs_num_devs >= PIOS_FLASHFS_LOGFS_MAX_DEVS) { - return (NULL); - } + if (pios_flashfs_logfs_num_devs >= PIOS_FLASHFS_LOGFS_MAX_DEVS) { + return NULL; + } - logfs = &pios_flashfs_logfs_devs[pios_flashfs_logfs_num_devs++]; - logfs->magic = PIOS_FLASHFS_LOGFS_DEV_MAGIC; + logfs = &pios_flashfs_logfs_devs[pios_flashfs_logfs_num_devs++]; + logfs->magic = PIOS_FLASHFS_LOGFS_DEV_MAGIC; - return (logfs); + return logfs; } -static void PIOS_FLASHFS_Logfs_free(struct logfs_state * logfs) +static void PIOS_FLASHFS_Logfs_free(struct logfs_state *logfs) { - /* Invalidate the magic */ - logfs->magic = ~PIOS_FLASHFS_LOGFS_DEV_MAGIC; + /* Invalidate the magic */ + logfs->magic = ~PIOS_FLASHFS_LOGFS_DEV_MAGIC; - /* Can't free the resources with this simple allocator */ + /* Can't free the resources with this simple allocator */ } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /** * @brief Initialize the flash object setting FS * @return 0 if success, -1 if failure */ -int32_t PIOS_FLASHFS_Logfs_Init(uintptr_t * fs_id, const struct flashfs_logfs_cfg * cfg, const struct pios_flash_driver * driver, uintptr_t flash_id) +int32_t PIOS_FLASHFS_Logfs_Init(uintptr_t *fs_id, const struct flashfs_logfs_cfg *cfg, const struct pios_flash_driver *driver, uintptr_t flash_id) { - PIOS_Assert(cfg); - PIOS_Assert(fs_id); - PIOS_Assert(driver); + PIOS_Assert(cfg); + PIOS_Assert(fs_id); + PIOS_Assert(driver); - /* We must have at least 2 arenas for garbage collection to work */ - PIOS_Assert((cfg->total_fs_size / cfg->arena_size > 1)); + /* We must have at least 2 arenas for garbage collection to work */ + PIOS_Assert((cfg->total_fs_size / cfg->arena_size > 1)); - /* Make sure the underlying flash driver provides the minimal set of required methods */ - PIOS_Assert(driver->start_transaction); - PIOS_Assert(driver->end_transaction); - PIOS_Assert(driver->erase_sector); - PIOS_Assert(driver->write_data); - PIOS_Assert(driver->read_data); + /* Make sure the underlying flash driver provides the minimal set of required methods */ + PIOS_Assert(driver->start_transaction); + PIOS_Assert(driver->end_transaction); + PIOS_Assert(driver->erase_sector); + PIOS_Assert(driver->write_data); + PIOS_Assert(driver->read_data); - int8_t rc; + int8_t rc; - struct logfs_state * logfs; + struct logfs_state *logfs; - logfs = (struct logfs_state *) PIOS_FLASHFS_Logfs_alloc(); - if (!logfs) { - rc = -1; - goto out_exit; - } + logfs = (struct logfs_state *)PIOS_FLASHFS_Logfs_alloc(); + if (!logfs) { + rc = -1; + goto out_exit; + } - /* Bind configuration parameters to this filesystem instance */ - logfs->cfg = cfg; /* filesystem configuration */ - logfs->driver = driver; /* lower-level flash driver */ - logfs->flash_id = flash_id; /* lower-level flash device id */ - logfs->mounted = false; + /* Bind configuration parameters to this filesystem instance */ + logfs->cfg = cfg; /* filesystem configuration */ + logfs->driver = driver; /* lower-level flash driver */ + logfs->flash_id = flash_id; /* lower-level flash device id */ + logfs->mounted = false; - if (logfs->driver->start_transaction(logfs->flash_id) != 0) { - rc = -1; - goto out_exit; - } + if (logfs->driver->start_transaction(logfs->flash_id) != 0) { + rc = -1; + goto out_exit; + } - bool found = false; - int32_t arena_id; - for (uint8_t try = 0; !found && try < 2; try++) { - /* Find the active arena */ - arena_id = logfs_find_active_arena(logfs); - if (arena_id >= 0) { - /* Found the active arena */ - found = true; - break; - } else { - /* No active arena found, erase and activate arena 0 */ - if (logfs_erase_arena(logfs, 0) != 0) - break; - if (logfs_activate_arena(logfs, 0) != 0) - break; - } - } + bool found = false; + int32_t arena_id; + for (uint8_t try = 0; !found && try < 2; try++) { + /* Find the active arena */ + arena_id = logfs_find_active_arena(logfs); + if (arena_id >= 0) { + /* Found the active arena */ + found = true; + break; + } else { + /* No active arena found, erase and activate arena 0 */ + if (logfs_erase_arena(logfs, 0) != 0) { + break; + } + if (logfs_activate_arena(logfs, 0) != 0) { + break; + } + } + } - if (!found) { - /* Still no active arena, something is broken */ - rc = -2; - goto out_end_trans; - } + if (!found) { + /* Still no active arena, something is broken */ + rc = -2; + goto out_end_trans; + } - /* We've found an active arena, mount it */ - if (logfs_mount_log(logfs, arena_id) != 0) { - /* Failed to mount the log, something is broken */ - rc = -3; - goto out_end_trans; - } + /* We've found an active arena, mount it */ + if (logfs_mount_log(logfs, arena_id) != 0) { + /* Failed to mount the log, something is broken */ + rc = -3; + goto out_end_trans; + } - /* Log has been mounted */ - rc = 0; + /* Log has been mounted */ + rc = 0; - *fs_id = (uintptr_t) logfs; + *fs_id = (uintptr_t)logfs; out_end_trans: - logfs->driver->end_transaction(logfs->flash_id); + logfs->driver->end_transaction(logfs->flash_id); out_exit: - return rc; + return rc; } int32_t PIOS_FLASHFS_Logfs_Destroy(uintptr_t fs_id) { - int32_t rc; + int32_t rc; - struct logfs_state * logfs = (struct logfs_state *)fs_id; + struct logfs_state *logfs = (struct logfs_state *)fs_id; - if (!PIOS_FLASHFS_Logfs_validate(logfs)) { - rc = -1; - goto out_exit; - } + if (!PIOS_FLASHFS_Logfs_validate(logfs)) { + rc = -1; + goto out_exit; + } - PIOS_FLASHFS_Logfs_free(logfs); - rc = 0; + PIOS_FLASHFS_Logfs_free(logfs); + rc = 0; out_exit: - return rc; + return rc; } /* NOTE: Must be called while holding the flash transaction lock */ -static int32_t logfs_garbage_collect (struct logfs_state * logfs) { - PIOS_Assert (logfs->mounted); - - /* Source arena is the active arena */ - uint8_t src_arena_id = logfs->active_arena_id; - - /* Compute destination arena */ - uint8_t dst_arena_id = (logfs->active_arena_id + 1) % (logfs->cfg->total_fs_size / logfs->cfg->arena_size); - - /* Erase destination arena */ - if (logfs_erase_arena (logfs, dst_arena_id) != 0) { - return -1; - } - - /* Reserve the destination arena so we can start filling it */ - if (logfs_reserve_arena (logfs, dst_arena_id) != 0) { - /* Unable to reserve the arena */ - return -2; - } - - /* Copy active slots from active arena to destination arena */ - uint16_t dst_slot_id = 1; - for (uint16_t src_slot_id = 1; - src_slot_id < (logfs->cfg->arena_size / logfs->cfg->slot_size); - src_slot_id++) { - struct slot_header slot_hdr; - uintptr_t src_addr = logfs_get_addr (logfs, src_arena_id, src_slot_id); - if (logfs->driver->read_data(logfs->flash_id, - src_addr, - (uint8_t *)&slot_hdr, - sizeof (slot_hdr)) != 0) { - return -3; - } - - if (slot_hdr.state == SLOT_STATE_ACTIVE) { - uintptr_t dst_addr = logfs_get_addr (logfs, dst_arena_id, dst_slot_id); - if (logfs_raw_copy_bytes(logfs, - src_addr, - sizeof(slot_hdr) + slot_hdr.obj_size, - dst_addr) != 0) { - /* Failed to copy all bytes */ - return -4; - } - dst_slot_id++; - } -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_Clear(); -#endif - } - - /* Activate the destination arena */ - if (logfs_activate_arena (logfs, dst_arena_id) != 0) { - return -5; - } - - /* Unmount the source arena */ - if (logfs_unmount_log (logfs) != 0) { - return -6; - } - - /* Obsolete the source arena */ - if (logfs_obsolete_arena (logfs, src_arena_id) != 0) { - return -7; - } - - /* Mount the new arena */ - if (logfs_mount_log (logfs, dst_arena_id) != 0) { - return -8; - } - - return 0; -} - -/* NOTE: Must be called while holding the flash transaction lock */ -static int16_t logfs_object_find_next (const struct logfs_state * logfs, struct slot_header * slot_hdr, uint16_t * curr_slot, uint32_t obj_id, uint16_t obj_inst_id) +static int32_t logfs_garbage_collect(struct logfs_state *logfs) { - PIOS_Assert(slot_hdr); - PIOS_Assert(curr_slot); + PIOS_Assert(logfs->mounted); - /* First slot in the arena is reserved for arena header, skip it. */ - if (*curr_slot == 0) *curr_slot = 1; + /* Source arena is the active arena */ + uint8_t src_arena_id = logfs->active_arena_id; - for (uint16_t slot_id = *curr_slot; - slot_id < (logfs->cfg->arena_size / logfs->cfg->slot_size); - slot_id++) { - uintptr_t slot_addr = logfs_get_addr (logfs, logfs->active_arena_id, slot_id); + /* Compute destination arena */ + uint8_t dst_arena_id = (logfs->active_arena_id + 1) % (logfs->cfg->total_fs_size / logfs->cfg->arena_size); - if (logfs->driver->read_data(logfs->flash_id, - slot_addr, - (uint8_t *)slot_hdr, - sizeof (*slot_hdr)) != 0) { - return -2; - } - if (slot_hdr->state == SLOT_STATE_EMPTY) { - /* We hit the end of the log */ - break; - } - if (slot_hdr->state == SLOT_STATE_ACTIVE && - slot_hdr->obj_id == obj_id && - slot_hdr->obj_inst_id == obj_inst_id) { - /* Found what we were looking for */ - *curr_slot = slot_id; - return 0; - } + /* Erase destination arena */ + if (logfs_erase_arena(logfs, dst_arena_id) != 0) { + return -1; + } + + /* Reserve the destination arena so we can start filling it */ + if (logfs_reserve_arena(logfs, dst_arena_id) != 0) { + /* Unable to reserve the arena */ + return -2; + } + + /* Copy active slots from active arena to destination arena */ + uint16_t dst_slot_id = 1; + for (uint16_t src_slot_id = 1; + src_slot_id < (logfs->cfg->arena_size / logfs->cfg->slot_size); + src_slot_id++) { + struct slot_header slot_hdr; + uintptr_t src_addr = logfs_get_addr(logfs, src_arena_id, src_slot_id); + if (logfs->driver->read_data(logfs->flash_id, + src_addr, + (uint8_t *)&slot_hdr, + sizeof(slot_hdr)) != 0) { + return -3; + } + + if (slot_hdr.state == SLOT_STATE_ACTIVE) { + uintptr_t dst_addr = logfs_get_addr(logfs, dst_arena_id, dst_slot_id); + if (logfs_raw_copy_bytes(logfs, + src_addr, + sizeof(slot_hdr) + slot_hdr.obj_size, + dst_addr) != 0) { + /* Failed to copy all bytes */ + return -4; + } + dst_slot_id++; + } #ifdef PIOS_INCLUDE_WDG PIOS_WDG_Clear(); #endif - } + } - /* No matching entry was found */ - return -1; + /* Activate the destination arena */ + if (logfs_activate_arena(logfs, dst_arena_id) != 0) { + return -5; + } + + /* Unmount the source arena */ + if (logfs_unmount_log(logfs) != 0) { + return -6; + } + + /* Obsolete the source arena */ + if (logfs_obsolete_arena(logfs, src_arena_id) != 0) { + return -7; + } + + /* Mount the new arena */ + if (logfs_mount_log(logfs, dst_arena_id) != 0) { + return -8; + } + + return 0; +} + +/* NOTE: Must be called while holding the flash transaction lock */ +static int16_t logfs_object_find_next(const struct logfs_state *logfs, struct slot_header *slot_hdr, uint16_t *curr_slot, uint32_t obj_id, uint16_t obj_inst_id) +{ + PIOS_Assert(slot_hdr); + PIOS_Assert(curr_slot); + + /* First slot in the arena is reserved for arena header, skip it. */ + if (*curr_slot == 0) { + *curr_slot = 1; + } + + for (uint16_t slot_id = *curr_slot; + slot_id < (logfs->cfg->arena_size / logfs->cfg->slot_size); + slot_id++) { + uintptr_t slot_addr = logfs_get_addr(logfs, logfs->active_arena_id, slot_id); + + if (logfs->driver->read_data(logfs->flash_id, + slot_addr, + (uint8_t *)slot_hdr, + sizeof(*slot_hdr)) != 0) { + return -2; + } + if (slot_hdr->state == SLOT_STATE_EMPTY) { + /* We hit the end of the log */ + break; + } + if (slot_hdr->state == SLOT_STATE_ACTIVE && + slot_hdr->obj_id == obj_id && + slot_hdr->obj_inst_id == obj_inst_id) { + /* Found what we were looking for */ + *curr_slot = slot_id; + return 0; + } +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_Clear(); +#endif + } + + /* No matching entry was found */ + return -1; } /* NOTE: Must be called while holding the flash transaction lock */ /* OPTIMIZE: could trust that there is at most one active version of every object and terminate the search when we find one */ -static int8_t logfs_delete_object (struct logfs_state * logfs, uint32_t obj_id, uint16_t obj_inst_id) +static int8_t logfs_delete_object(struct logfs_state *logfs, uint32_t obj_id, uint16_t obj_inst_id) { - int8_t rc; + int8_t rc; - bool more = true; - uint16_t curr_slot_id = 0; - do { - struct slot_header slot_hdr; - switch (logfs_object_find_next (logfs, &slot_hdr, &curr_slot_id, obj_id, obj_inst_id)) { - case 0: - /* Found a matching slot. Obsolete it. */ - slot_hdr.state = SLOT_STATE_OBSOLETE; - uintptr_t slot_addr = logfs_get_addr (logfs, logfs->active_arena_id, curr_slot_id); + bool more = true; + uint16_t curr_slot_id = 0; - if (logfs->driver->write_data(logfs->flash_id, - slot_addr, - (uint8_t *)&slot_hdr, - sizeof(slot_hdr)) != 0) { - rc = -2; - goto out_exit; - } - /* Object has been successfully obsoleted and is no longer active */ - logfs->num_active_slots--; - break; - case -1: - /* Search completed, object not found */ - more = false; - rc = 0; - break; - default: - /* Error occurred during search */ - rc = -1; - goto out_exit; - } - } while (more); + do { + struct slot_header slot_hdr; + switch (logfs_object_find_next(logfs, &slot_hdr, &curr_slot_id, obj_id, obj_inst_id)) { + case 0: + /* Found a matching slot. Obsolete it. */ + slot_hdr.state = SLOT_STATE_OBSOLETE; + uintptr_t slot_addr = logfs_get_addr(logfs, logfs->active_arena_id, curr_slot_id); + + if (logfs->driver->write_data(logfs->flash_id, + slot_addr, + (uint8_t *)&slot_hdr, + sizeof(slot_hdr)) != 0) { + rc = -2; + goto out_exit; + } + /* Object has been successfully obsoleted and is no longer active */ + logfs->num_active_slots--; + break; + case -1: + /* Search completed, object not found */ + more = false; + rc = 0; + break; + default: + /* Error occurred during search */ + rc = -1; + goto out_exit; + } + } while (more); out_exit: - return rc; + return rc; } /* NOTE: Must be called while holding the flash transaction lock */ -static int8_t logfs_reserve_free_slot (struct logfs_state * logfs, uint16_t * slot_id, struct slot_header * slot_hdr, uint32_t obj_id, uint16_t obj_inst_id, uint16_t obj_size) +static int8_t logfs_reserve_free_slot(struct logfs_state *logfs, uint16_t *slot_id, struct slot_header *slot_hdr, uint32_t obj_id, uint16_t obj_inst_id, uint16_t obj_size) { - PIOS_Assert(slot_id); - PIOS_Assert(slot_hdr); + PIOS_Assert(slot_id); + PIOS_Assert(slot_hdr); - if (logfs->num_free_slots < 1) { - /* No free slots to allocate */ - return -1; - } + if (logfs->num_free_slots < 1) { + /* No free slots to allocate */ + return -1; + } - if (obj_size > (logfs->cfg->slot_size - sizeof (slot_hdr))) { - /* This object is too big for the slot */ - return -2; - } + if (obj_size > (logfs->cfg->slot_size - sizeof(slot_hdr))) { + /* This object is too big for the slot */ + return -2; + } - uint16_t candidate_slot_id = (logfs->cfg->arena_size / logfs->cfg->slot_size) - logfs->num_free_slots; - PIOS_Assert(candidate_slot_id > 0); + uint16_t candidate_slot_id = (logfs->cfg->arena_size / logfs->cfg->slot_size) - logfs->num_free_slots; + PIOS_Assert(candidate_slot_id > 0); - uintptr_t slot_addr = logfs_get_addr (logfs, logfs->active_arena_id, candidate_slot_id); + uintptr_t slot_addr = logfs_get_addr(logfs, logfs->active_arena_id, candidate_slot_id); - if (logfs->driver->read_data(logfs->flash_id, - slot_addr, - (uint8_t *)slot_hdr, - sizeof (*slot_hdr)) != 0) { - /* Failed to read slot header for candidate slot */ - return -3; - } + if (logfs->driver->read_data(logfs->flash_id, + slot_addr, + (uint8_t *)slot_hdr, + sizeof(*slot_hdr)) != 0) { + /* Failed to read slot header for candidate slot */ + return -3; + } - if (slot_hdr->state != SLOT_STATE_EMPTY) { - /* Candidate slot isn't empty! Something is broken. */ - PIOS_DEBUG_Assert(0); - return -4; - } + if (slot_hdr->state != SLOT_STATE_EMPTY) { + /* Candidate slot isn't empty! Something is broken. */ + PIOS_DEBUG_Assert(0); + return -4; + } - /* Mark this slot as RESERVED */ - slot_hdr->state = SLOT_STATE_RESERVED; - slot_hdr->obj_id = obj_id; - slot_hdr->obj_inst_id = obj_inst_id; - slot_hdr->obj_size = obj_size; + /* Mark this slot as RESERVED */ + slot_hdr->state = SLOT_STATE_RESERVED; + slot_hdr->obj_id = obj_id; + slot_hdr->obj_inst_id = obj_inst_id; + slot_hdr->obj_size = obj_size; - if (logfs->driver->write_data(logfs->flash_id, - slot_addr, - (uint8_t *)slot_hdr, - sizeof(*slot_hdr)) != 0) { - /* Failed to write the slot header */ - return -5; - } + if (logfs->driver->write_data(logfs->flash_id, + slot_addr, + (uint8_t *)slot_hdr, + sizeof(*slot_hdr)) != 0) { + /* Failed to write the slot header */ + return -5; + } - /* FIXME: If the header write (above) failed, may have partially written data, thus corrupting that slot but we would have missed decrementing this counter */ - logfs->num_free_slots--; + /* FIXME: If the header write (above) failed, may have partially written data, thus corrupting that slot but we would have missed decrementing this counter */ + logfs->num_free_slots--; - *slot_id = candidate_slot_id; - return 0; + *slot_id = candidate_slot_id; + return 0; } /* NOTE: Must be called while holding the flash transaction lock */ -static int8_t logfs_append_to_log (struct logfs_state * logfs, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size) +static int8_t logfs_append_to_log(struct logfs_state *logfs, uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size) { - /* Reserve a free slot for our new object */ - uint16_t free_slot_id; - struct slot_header slot_hdr; - if (logfs_reserve_free_slot (logfs, &free_slot_id, &slot_hdr, obj_id, obj_inst_id, obj_size) != 0) { - /* Failed to reserve a free slot */ - return -1; - } + /* Reserve a free slot for our new object */ + uint16_t free_slot_id; + struct slot_header slot_hdr; - /* Compute slot address */ - uintptr_t slot_addr = logfs_get_addr (logfs, logfs->active_arena_id, free_slot_id); + if (logfs_reserve_free_slot(logfs, &free_slot_id, &slot_hdr, obj_id, obj_inst_id, obj_size) != 0) { + /* Failed to reserve a free slot */ + return -1; + } - /* Write the data into the reserved slot, starting after the slot header */ - uintptr_t slot_offset = sizeof(slot_hdr); - while (obj_size > 0) { - /* Individual writes must fit entirely within a single page buffer. */ - uint16_t page_remaining = logfs->cfg->page_size - (slot_offset % logfs->cfg->page_size); - uint16_t write_size = MIN(obj_size, page_remaining); - if (logfs->driver->write_data (logfs->flash_id, - slot_addr + slot_offset, - obj_data, - write_size) != 0) { - /* Failed to write the object data to the slot */ - return -2; - } + /* Compute slot address */ + uintptr_t slot_addr = logfs_get_addr(logfs, logfs->active_arena_id, free_slot_id); - /* Update our accounting */ - obj_data += write_size; - slot_offset += write_size; - obj_size -= write_size; - } + /* Write the data into the reserved slot, starting after the slot header */ + uintptr_t slot_offset = sizeof(slot_hdr); + while (obj_size > 0) { + /* Individual writes must fit entirely within a single page buffer. */ + uint16_t page_remaining = logfs->cfg->page_size - (slot_offset % logfs->cfg->page_size); + uint16_t write_size = MIN(obj_size, page_remaining); + if (logfs->driver->write_data(logfs->flash_id, + slot_addr + slot_offset, + obj_data, + write_size) != 0) { + /* Failed to write the object data to the slot */ + return -2; + } - /* Mark this slot active in one atomic step */ - slot_hdr.state = SLOT_STATE_ACTIVE; - if (logfs->driver->write_data (logfs->flash_id, - slot_addr, - (uint8_t *)&slot_hdr, - sizeof(slot_hdr)) != 0) { - /* Failed to mark the slot active */ - return -4; - } + /* Update our accounting */ + obj_data += write_size; + slot_offset += write_size; + obj_size -= write_size; + } - /* Object has been successfully written to the slot */ - logfs->num_active_slots++; - return 0; + /* Mark this slot active in one atomic step */ + slot_hdr.state = SLOT_STATE_ACTIVE; + if (logfs->driver->write_data(logfs->flash_id, + slot_addr, + (uint8_t *)&slot_hdr, + sizeof(slot_hdr)) != 0) { + /* Failed to mark the slot active */ + return -4; + } + + /* Object has been successfully written to the slot */ + logfs->num_active_slots++; + return 0; } @@ -889,7 +900,7 @@ static int8_t logfs_append_to_log (struct logfs_state * logfs, uint32_t obj_id, * Provide a PIOS_FLASHFS_* driver * *********************************/ -#include "pios_flashfs.h" /* API for flash filesystem */ +#include "pios_flashfs.h" /* API for flash filesystem */ /** * @brief Saves one object instance to the filesystem @@ -907,76 +918,76 @@ static int8_t logfs_append_to_log (struct logfs_state * logfs, uint32_t obj_id, * @retval -6 if filesystem is full even after garbage collection should have freed space * @retval -7 if writing the new object to the filesystem failed */ -int32_t PIOS_FLASHFS_ObjSave(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size) +int32_t PIOS_FLASHFS_ObjSave(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size) { - int8_t rc; + int8_t rc; - struct logfs_state * logfs = (struct logfs_state *)fs_id; + struct logfs_state *logfs = (struct logfs_state *)fs_id; - if (!PIOS_FLASHFS_Logfs_validate(logfs)) { - rc = -1; - goto out_exit; - } + if (!PIOS_FLASHFS_Logfs_validate(logfs)) { + rc = -1; + goto out_exit; + } - PIOS_Assert(obj_size <= (logfs->cfg->slot_size - sizeof(struct slot_header))); + PIOS_Assert(obj_size <= (logfs->cfg->slot_size - sizeof(struct slot_header))); - if (logfs->driver->start_transaction(logfs->flash_id) != 0) { - rc = -2; - goto out_exit; - } + if (logfs->driver->start_transaction(logfs->flash_id) != 0) { + rc = -2; + goto out_exit; + } - if (logfs_delete_object (logfs, obj_id, obj_inst_id) != 0) { - rc = -3; - goto out_end_trans; - } + if (logfs_delete_object(logfs, obj_id, obj_inst_id) != 0) { + rc = -3; + goto out_end_trans; + } - /* - * All old versions of this object + instance have been invalidated. - * Write the new object. - */ + /* + * All old versions of this object + instance have been invalidated. + * Write the new object. + */ - /* Check if the arena is entirely full. */ - if (logfs_fs_is_full(logfs)) { - /* Note: Filesystem Full means we're full of *active* records so gc won't help at all. */ - rc = -4; - goto out_end_trans; - } + /* Check if the arena is entirely full. */ + if (logfs_fs_is_full(logfs)) { + /* Note: Filesystem Full means we're full of *active* records so gc won't help at all. */ + rc = -4; + goto out_end_trans; + } - /* Is garbage collection required? */ - if (logfs_log_is_full(logfs)) { - /* Note: Log Full means the log is full but may contain obsolete slots so gc may free some space */ - if (logfs_garbage_collect(logfs) != 0) { - rc = -5; - goto out_end_trans; - } - /* Check one more time just to be sure we actually free'd some space */ - if (logfs_log_is_full(logfs)) { - /* - * Log is still full even after gc! - * NOTE: This should not happen since the filesystem wasn't full - * when we checked above so gc should have helped. - */ - PIOS_DEBUG_Assert(0); - rc = -6; - goto out_end_trans; - } - } + /* Is garbage collection required? */ + if (logfs_log_is_full(logfs)) { + /* Note: Log Full means the log is full but may contain obsolete slots so gc may free some space */ + if (logfs_garbage_collect(logfs) != 0) { + rc = -5; + goto out_end_trans; + } + /* Check one more time just to be sure we actually free'd some space */ + if (logfs_log_is_full(logfs)) { + /* + * Log is still full even after gc! + * NOTE: This should not happen since the filesystem wasn't full + * when we checked above so gc should have helped. + */ + PIOS_DEBUG_Assert(0); + rc = -6; + goto out_end_trans; + } + } - /* We have room for our new object. Append it to the log. */ - if (logfs_append_to_log(logfs, obj_id, obj_inst_id, obj_data, obj_size) != 0) { - /* Error during append */ - rc = -7; - goto out_end_trans; - } + /* We have room for our new object. Append it to the log. */ + if (logfs_append_to_log(logfs, obj_id, obj_inst_id, obj_data, obj_size) != 0) { + /* Error during append */ + rc = -7; + goto out_end_trans; + } - /* Object successfully written to the log */ - rc = 0; + /* Object successfully written to the log */ + rc = 0; out_end_trans: - logfs->driver->end_transaction(logfs->flash_id); + logfs->driver->end_transaction(logfs->flash_id); out_exit: - return rc; + return rc; } /** @@ -993,61 +1004,61 @@ out_exit: * @retval -4 if object size in filesystem does not exactly match buffer size * @retval -5 if reading the object data from flash fails */ -int32_t PIOS_FLASHFS_ObjLoad(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size) +int32_t PIOS_FLASHFS_ObjLoad(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size) { - int8_t rc; + int8_t rc; - struct logfs_state * logfs = (struct logfs_state *)fs_id; + struct logfs_state *logfs = (struct logfs_state *)fs_id; - if (!PIOS_FLASHFS_Logfs_validate(logfs)) { - rc = -1; - goto out_exit; - } + if (!PIOS_FLASHFS_Logfs_validate(logfs)) { + rc = -1; + goto out_exit; + } - PIOS_Assert(obj_size <= (logfs->cfg->slot_size - sizeof(struct slot_header))); + PIOS_Assert(obj_size <= (logfs->cfg->slot_size - sizeof(struct slot_header))); - if (logfs->driver->start_transaction(logfs->flash_id) != 0) { - rc = -2; - goto out_exit; - } + if (logfs->driver->start_transaction(logfs->flash_id) != 0) { + rc = -2; + goto out_exit; + } - /* Find the object in the log */ - uint16_t slot_id = 0; - struct slot_header slot_hdr; - if (logfs_object_find_next (logfs, &slot_hdr, &slot_id, obj_id, obj_inst_id) != 0) { - /* Object does not exist in fs */ - rc = -3; - goto out_end_trans; - } + /* Find the object in the log */ + uint16_t slot_id = 0; + struct slot_header slot_hdr; + if (logfs_object_find_next(logfs, &slot_hdr, &slot_id, obj_id, obj_inst_id) != 0) { + /* Object does not exist in fs */ + rc = -3; + goto out_end_trans; + } - /* Sanity check what we've found */ - if (slot_hdr.obj_size != obj_size) { - /* Object sizes don't match. Not safe to copy contents. */ - rc = -4; - goto out_end_trans; - } + /* Sanity check what we've found */ + if (slot_hdr.obj_size != obj_size) { + /* Object sizes don't match. Not safe to copy contents. */ + rc = -4; + goto out_end_trans; + } - /* Read the contents of the object from the log */ - if (obj_size > 0) { - uintptr_t slot_addr = logfs_get_addr (logfs, logfs->active_arena_id, slot_id); - if (logfs->driver->read_data(logfs->flash_id, - slot_addr + sizeof(slot_hdr), - (uint8_t *)obj_data, - obj_size) != 0) { - /* Failed to read object data from the log */ - rc = -5; - goto out_end_trans; - } - } + /* Read the contents of the object from the log */ + if (obj_size > 0) { + uintptr_t slot_addr = logfs_get_addr(logfs, logfs->active_arena_id, slot_id); + if (logfs->driver->read_data(logfs->flash_id, + slot_addr + sizeof(slot_hdr), + (uint8_t *)obj_data, + obj_size) != 0) { + /* Failed to read object data from the log */ + rc = -5; + goto out_end_trans; + } + } - /* Object successfully loaded */ - rc = 0; + /* Object successfully loaded */ + rc = 0; out_end_trans: - logfs->driver->end_transaction(logfs->flash_id); + logfs->driver->end_transaction(logfs->flash_id); out_exit: - return rc; + return rc; } /** @@ -1062,33 +1073,33 @@ out_exit: */ int32_t PIOS_FLASHFS_ObjDelete(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id) { - int8_t rc; + int8_t rc; - struct logfs_state * logfs = (struct logfs_state *)fs_id; + struct logfs_state *logfs = (struct logfs_state *)fs_id; - if (!PIOS_FLASHFS_Logfs_validate(logfs)) { - rc = -1; - goto out_exit; - } + if (!PIOS_FLASHFS_Logfs_validate(logfs)) { + rc = -1; + goto out_exit; + } - if (logfs->driver->start_transaction(logfs->flash_id) != 0) { - rc = -2; - goto out_exit; - } + if (logfs->driver->start_transaction(logfs->flash_id) != 0) { + rc = -2; + goto out_exit; + } - if (logfs_delete_object (logfs, obj_id, obj_inst_id) != 0) { - rc = -3; - goto out_end_trans; - } + if (logfs_delete_object(logfs, obj_id, obj_inst_id) != 0) { + rc = -3; + goto out_end_trans; + } - /* Object successfully deleted from the log */ - rc = 0; + /* Object successfully deleted from the log */ + rc = 0; out_end_trans: - logfs->driver->end_transaction(logfs->flash_id); + logfs->driver->end_transaction(logfs->flash_id); out_exit: - return rc; + return rc; } /** @@ -1103,49 +1114,49 @@ out_exit: */ int32_t PIOS_FLASHFS_Format(uintptr_t fs_id) { - int32_t rc; + int32_t rc; - struct logfs_state * logfs = (struct logfs_state *)fs_id; + struct logfs_state *logfs = (struct logfs_state *)fs_id; - if (!PIOS_FLASHFS_Logfs_validate(logfs)) { - rc = -1; - goto out_exit; - } + if (!PIOS_FLASHFS_Logfs_validate(logfs)) { + rc = -1; + goto out_exit; + } - if (logfs->mounted) { - logfs_unmount_log(logfs); - } + if (logfs->mounted) { + logfs_unmount_log(logfs); + } - if (logfs->driver->start_transaction(logfs->flash_id) != 0) { - rc = -2; - goto out_exit; - } + if (logfs->driver->start_transaction(logfs->flash_id) != 0) { + rc = -2; + goto out_exit; + } - if (logfs_erase_all_arenas(logfs) != 0) { - rc = -3; - goto out_end_trans; - } + if (logfs_erase_all_arenas(logfs) != 0) { + rc = -3; + goto out_end_trans; + } - /* Reinitialize arena 0 */ - if (logfs_activate_arena(logfs, 0) != 0) { - rc = -4; - goto out_end_trans; - } + /* Reinitialize arena 0 */ + if (logfs_activate_arena(logfs, 0) != 0) { + rc = -4; + goto out_end_trans; + } - /* Mount arena 0 */ - if (logfs_mount_log(logfs, 0) != 0) { - rc = -5; - goto out_end_trans; - } + /* Mount arena 0 */ + if (logfs_mount_log(logfs, 0) != 0) { + rc = -5; + goto out_end_trans; + } - /* Chip erased and log remounted successfully */ - rc = 0; + /* Chip erased and log remounted successfully */ + rc = 0; out_end_trans: - logfs->driver->end_transaction(logfs->flash_id); + logfs->driver->end_transaction(logfs->flash_id); out_exit: - return rc; + return rc; } #endif /* PIOS_INCLUDE_FLASH */ diff --git a/flight/pios/common/pios_gcsrcvr.c b/flight/pios/common/pios_gcsrcvr.c index e7c281d9f..95ed04d7b 100644 --- a/flight/pios/common/pios_gcsrcvr.c +++ b/flight/pios/common/pios_gcsrcvr.c @@ -43,99 +43,103 @@ static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel); static void PIOS_gcsrcvr_Supervisor(uint32_t ppm_id); const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver = { - .read = PIOS_GCSRCVR_Get, + .read = PIOS_GCSRCVR_Get, }; /* Local Variables */ enum pios_gcsrcvr_dev_magic { - PIOS_GCSRCVR_DEV_MAGIC = 0xe9da5c56, + PIOS_GCSRCVR_DEV_MAGIC = 0xe9da5c56, }; struct pios_gcsrcvr_dev { - enum pios_gcsrcvr_dev_magic magic; + enum pios_gcsrcvr_dev_magic magic; - uint8_t supv_timer; - bool Fresh; + uint8_t supv_timer; + bool Fresh; }; static struct pios_gcsrcvr_dev *global_gcsrcvr_dev; static bool PIOS_gcsrcvr_validate(struct pios_gcsrcvr_dev *gcsrcvr_dev) { - return (gcsrcvr_dev->magic == PIOS_GCSRCVR_DEV_MAGIC); + return gcsrcvr_dev->magic == PIOS_GCSRCVR_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void) { - struct pios_gcsrcvr_dev * gcsrcvr_dev; + struct pios_gcsrcvr_dev *gcsrcvr_dev; - gcsrcvr_dev = (struct pios_gcsrcvr_dev *)pvPortMalloc(sizeof(*gcsrcvr_dev)); - if (!gcsrcvr_dev) return(NULL); + gcsrcvr_dev = (struct pios_gcsrcvr_dev *)pvPortMalloc(sizeof(*gcsrcvr_dev)); + if (!gcsrcvr_dev) { + return NULL; + } - gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; - gcsrcvr_dev->Fresh = false; - gcsrcvr_dev->supv_timer = 0; + gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; + gcsrcvr_dev->Fresh = false; + gcsrcvr_dev->supv_timer = 0; - /* The update callback cannot receive the device pointer, so set it in a global */ - global_gcsrcvr_dev = gcsrcvr_dev; + /* The update callback cannot receive the device pointer, so set it in a global */ + global_gcsrcvr_dev = gcsrcvr_dev; - return(gcsrcvr_dev); + return gcsrcvr_dev; } #else static struct pios_gcsrcvr_dev pios_gcsrcvr_devs[PIOS_GCSRCVR_MAX_DEVS]; static uint8_t pios_gcsrcvr_num_devs; static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void) { - struct pios_gcsrcvr_dev *gcsrcvr_dev; + struct pios_gcsrcvr_dev *gcsrcvr_dev; - if (pios_gcsrcvr_num_devs >= PIOS_GCSRCVR_MAX_DEVS) { - return (NULL); - } + if (pios_gcsrcvr_num_devs >= PIOS_GCSRCVR_MAX_DEVS) { + return NULL; + } - gcsrcvr_dev = &pios_gcsrcvr_devs[pios_gcsrcvr_num_devs++]; - gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; - gcsrcvr_dev->Fresh = false; - gcsrcvr_dev->supv_timer = 0; + gcsrcvr_dev = &pios_gcsrcvr_devs[pios_gcsrcvr_num_devs++]; + gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; + gcsrcvr_dev->Fresh = false; + gcsrcvr_dev->supv_timer = 0; - global_gcsrcvr_dev = gcsrcvr_dev; + global_gcsrcvr_dev = gcsrcvr_dev; - return (gcsrcvr_dev); + return gcsrcvr_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void gcsreceiver_updated(UAVObjEvent * ev) +static void gcsreceiver_updated(UAVObjEvent *ev) { - struct pios_gcsrcvr_dev *gcsrcvr_dev = global_gcsrcvr_dev; - if (ev->obj == GCSReceiverHandle()) { - GCSReceiverGet(&gcsreceiverdata); - gcsrcvr_dev->Fresh = true; - } + struct pios_gcsrcvr_dev *gcsrcvr_dev = global_gcsrcvr_dev; + + if (ev->obj == GCSReceiverHandle()) { + GCSReceiverGet(&gcsreceiverdata); + gcsrcvr_dev->Fresh = true; + } } extern int32_t PIOS_GCSRCVR_Init(__attribute__((unused)) uint32_t *gcsrcvr_id) { - struct pios_gcsrcvr_dev *gcsrcvr_dev; + struct pios_gcsrcvr_dev *gcsrcvr_dev; - /* Allocate the device structure */ - gcsrcvr_dev = (struct pios_gcsrcvr_dev *)PIOS_gcsrcvr_alloc(); - if (!gcsrcvr_dev) - return -1; + /* Allocate the device structure */ + gcsrcvr_dev = (struct pios_gcsrcvr_dev *)PIOS_gcsrcvr_alloc(); + if (!gcsrcvr_dev) { + return -1; + } - for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; i++) { - /* Flush channels */ - gcsreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT; - } + for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; i++) { + /* Flush channels */ + gcsreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT; + } - /* Register uavobj callback */ - GCSReceiverConnectCallback (gcsreceiver_updated); + /* Register uavobj callback */ + GCSReceiverConnectCallback(gcsreceiver_updated); - /* Register the failsafe timer callback. */ - if (!PIOS_RTC_RegisterTickCallback(PIOS_gcsrcvr_Supervisor, (uint32_t)gcsrcvr_dev)) { - PIOS_DEBUG_Assert(0); - } + /* Register the failsafe timer callback. */ + if (!PIOS_RTC_RegisterTickCallback(PIOS_gcsrcvr_Supervisor, (uint32_t)gcsrcvr_dev)) { + PIOS_DEBUG_Assert(0); + } - return 0; + return 0; } /** @@ -147,41 +151,44 @@ extern int32_t PIOS_GCSRCVR_Init(__attribute__((unused)) uint32_t *gcsrcvr_id) */ static int32_t PIOS_GCSRCVR_Get(__attribute__((unused)) uint32_t rcvr_id, uint8_t channel) { - if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) { - /* channel is out of range */ - return PIOS_RCVR_INVALID; - } + if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) { + /* channel is out of range */ + return PIOS_RCVR_INVALID; + } - return (gcsreceiverdata.Channel[channel]); + return gcsreceiverdata.Channel[channel]; } -static void PIOS_gcsrcvr_Supervisor(uint32_t gcsrcvr_id) { - /* Recover our device context */ - struct pios_gcsrcvr_dev * gcsrcvr_dev = (struct pios_gcsrcvr_dev *)gcsrcvr_id; +static void PIOS_gcsrcvr_Supervisor(uint32_t gcsrcvr_id) +{ + /* Recover our device context */ + struct pios_gcsrcvr_dev *gcsrcvr_dev = (struct pios_gcsrcvr_dev *)gcsrcvr_id; - if (!PIOS_gcsrcvr_validate(gcsrcvr_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_gcsrcvr_validate(gcsrcvr_dev)) { + /* Invalid device specified */ + return; + } - /* - * RTC runs at 625Hz. - */ - if(++(gcsrcvr_dev->supv_timer) < (PIOS_GCSRCVR_TIMEOUT_MS * 1000 / 625)) { - return; - } - gcsrcvr_dev->supv_timer = 0; + /* + * RTC runs at 625Hz. + */ + if (++(gcsrcvr_dev->supv_timer) < (PIOS_GCSRCVR_TIMEOUT_MS * 1000 / 625)) { + return; + } + gcsrcvr_dev->supv_timer = 0; - if (!gcsrcvr_dev->Fresh) - for (int32_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; i++) - gcsreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT; + if (!gcsrcvr_dev->Fresh) { + for (int32_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; i++) { + gcsreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT; + } + } - gcsrcvr_dev->Fresh = false; + gcsrcvr_dev->Fresh = false; } #endif /* PIOS_INCLUDE_GCSRCVR */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/common/pios_hcsr04.c b/flight/pios/common/pios_hcsr04.c index 925c5a175..da0c8f76d 100644 --- a/flight/pios/common/pios_hcsr04.c +++ b/flight/pios/common/pios_hcsr04.c @@ -1,30 +1,30 @@ /** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_HCSR04 HCSR04 Functions - * @brief Hardware functions to deal with the altitude pressure sensor - * @{ - * - * @file pios_hcsr04.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief HCSR04 sonar Sensor Routines - * @see The GNU Public License (GPL) Version 3 - * - ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_HCSR04 HCSR04 Functions + * @brief Hardware functions to deal with the altitude pressure sensor + * @{ + * + * @file pios_hcsr04.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief HCSR04 sonar Sensor Routines + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,258 +41,257 @@ /* 100 ms timeout without updates on channels */ const static uint32_t PWM_SUPERVISOR_TIMEOUT = 100000; -struct pios_hcsr04_dev * hcsr04_dev_loc; +struct pios_hcsr04_dev *hcsr04_dev_loc; enum pios_hcsr04_dev_magic { - PIOS_HCSR04_DEV_MAGIC = 0xab3029AA, + PIOS_HCSR04_DEV_MAGIC = 0xab3029AA, }; struct pios_hcsr04_dev { - enum pios_hcsr04_dev_magic magic; - const struct pios_hcsr04_cfg * cfg; + enum pios_hcsr04_dev_magic magic; + const struct pios_hcsr04_cfg *cfg; - uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; - uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; - uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; - uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; - uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; - uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; + uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; + uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; + uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; + uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; }; -static bool PIOS_HCSR04_validate(struct pios_hcsr04_dev * hcsr04_dev) +static bool PIOS_HCSR04_validate(struct pios_hcsr04_dev *hcsr04_dev) { - return (hcsr04_dev->magic == PIOS_HCSR04_DEV_MAGIC); + return hcsr04_dev->magic == PIOS_HCSR04_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_hcsr04_dev * PIOS_PWM_alloc(void) +static struct pios_hcsr04_dev *PIOS_PWM_alloc(void) { - struct pios_hcsr04_dev * hcsr04_dev; + struct pios_hcsr04_dev *hcsr04_dev; - hcsr04_dev = (struct pios_hcsr04_dev *)pvPortMalloc(sizeof(*hcsr04_dev)); - if (!hcsr04_dev) return(NULL); + hcsr04_dev = (struct pios_hcsr04_dev *)pvPortMalloc(sizeof(*hcsr04_dev)); + if (!hcsr04_dev) { + return NULL; + } - hcsr04_dev->magic = PIOS_HCSR04_DEV_MAGIC; - return(hcsr04_dev); + hcsr04_dev->magic = PIOS_HCSR04_DEV_MAGIC; + return hcsr04_dev; } #else static struct pios_hcsr04_dev pios_hcsr04_devs[PIOS_PWM_MAX_DEVS]; static uint8_t pios_hcsr04_num_devs; -static struct pios_hcsr04_dev * PIOS_PWM_alloc(void) +static struct pios_hcsr04_dev *PIOS_PWM_alloc(void) { - struct pios_hcsr04_dev * hcsr04_dev; + struct pios_hcsr04_dev *hcsr04_dev; - if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) { - return (NULL); - } + if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) { + return NULL; + } - hcsr04_dev = &pios_hcsr04_devs[pios_hcsr04_num_devs++]; - hcsr04_dev->magic = PIOS_HCSR04_DEV_MAGIC; + hcsr04_dev = &pios_hcsr04_devs[pios_hcsr04_num_devs++]; + hcsr04_dev->magic = PIOS_HCSR04_DEV_MAGIC; - return (hcsr04_dev); + return hcsr04_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void PIOS_HCSR04_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); -static void PIOS_HCSR04_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_HCSR04_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_HCSR04_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); const static struct pios_tim_callbacks tim_callbacks = { - .overflow = PIOS_HCSR04_tim_overflow_cb, - .edge = PIOS_HCSR04_tim_edge_cb, + .overflow = PIOS_HCSR04_tim_overflow_cb, + .edge = PIOS_HCSR04_tim_edge_cb, }; /** -* Initialises all the pins -*/ -int32_t PIOS_HCSR04_Init(uint32_t * pwm_id, const struct pios_hcsr04_cfg * cfg) + * Initialises all the pins + */ +int32_t PIOS_HCSR04_Init(uint32_t *pwm_id, const struct pios_hcsr04_cfg *cfg) { - PIOS_DEBUG_Assert(pwm_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(pwm_id); + PIOS_DEBUG_Assert(cfg); - struct pios_hcsr04_dev * hcsr04_dev; + struct pios_hcsr04_dev *hcsr04_dev; - hcsr04_dev = (struct pios_hcsr04_dev *) PIOS_PWM_alloc(); - if (!hcsr04_dev) goto out_fail; + hcsr04_dev = (struct pios_hcsr04_dev *)PIOS_PWM_alloc(); + if (!hcsr04_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - hcsr04_dev->cfg = cfg; - hcsr04_dev_loc = hcsr04_dev; + /* Bind the configuration to the device instance */ + hcsr04_dev->cfg = cfg; + hcsr04_dev_loc = hcsr04_dev; - for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { - /* Flush counter variables */ - hcsr04_dev->CaptureState[i] = 0; - hcsr04_dev->RiseValue[i] = 0; - hcsr04_dev->FallValue[i] = 0; - hcsr04_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - } + for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { + /* Flush counter variables */ + hcsr04_dev->CaptureState[i] = 0; + hcsr04_dev->RiseValue[i] = 0; + hcsr04_dev->FallValue[i] = 0; + hcsr04_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)hcsr04_dev)) { - return -1; - } + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)hcsr04_dev)) { + return -1; + } - /* Configure the channels to be in capture/compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel * chan = &cfg->channels[i]; + /* Configure the channels to be in capture/compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; - /* Configure timer for input capture */ - TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); + /* Configure timer for input capture */ + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); - /* Enable the Capture Compare Interrupt Request */ - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); - break; - case TIM_Channel_2: - TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); - break; - case TIM_Channel_3: - TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); - break; - case TIM_Channel_4: - TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); - break; - } + /* Enable the Capture Compare Interrupt Request */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); + break; + } - // Need the update event for that timer to detect timeouts - TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); - - } + // Need the update event for that timer to detect timeouts + TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); + } #ifndef STM32F4XX - /* Enable the peripheral clock for the GPIO */ - switch ((uint32_t)hcsr04_dev->cfg->trigger.gpio) { - case (uint32_t) GPIOA: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); - break; - case (uint32_t) GPIOB: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - break; - case (uint32_t) GPIOC: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); - break; - default: - PIOS_Assert(0); - break; - } + /* Enable the peripheral clock for the GPIO */ + switch ((uint32_t)hcsr04_dev->cfg->trigger.gpio) { + case (uint32_t)GPIOA: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); + break; + case (uint32_t)GPIOB: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + break; + case (uint32_t)GPIOC: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); + break; + default: + PIOS_Assert(0); + break; + } #endif - GPIO_Init(hcsr04_dev->cfg->trigger.gpio, &hcsr04_dev->cfg->trigger.init); + GPIO_Init(hcsr04_dev->cfg->trigger.gpio, &hcsr04_dev->cfg->trigger.init); - *pwm_id = (uint32_t) hcsr04_dev; + *pwm_id = (uint32_t)hcsr04_dev; - return (0); + return 0; out_fail: - return (-1); + return -1; } void PIOS_HCSR04_Trigger(void) { - GPIO_SetBits(hcsr04_dev_loc->cfg->trigger.gpio,hcsr04_dev_loc->cfg->trigger.init.GPIO_Pin); - PIOS_DELAY_WaituS(15); - GPIO_ResetBits(hcsr04_dev_loc->cfg->trigger.gpio,hcsr04_dev_loc->cfg->trigger.init.GPIO_Pin); + GPIO_SetBits(hcsr04_dev_loc->cfg->trigger.gpio, hcsr04_dev_loc->cfg->trigger.init.GPIO_Pin); + PIOS_DELAY_WaituS(15); + GPIO_ResetBits(hcsr04_dev_loc->cfg->trigger.gpio, hcsr04_dev_loc->cfg->trigger.init.GPIO_Pin); } /** -* Get the value of an input channel -* \param[in] Channel Number of the channel desired -* \output -1 Channel not available -* \output >0 Channel value -*/ + * Get the value of an input channel + * \param[in] Channel Number of the channel desired + * \output -1 Channel not available + * \output >0 Channel value + */ int32_t PIOS_HCSR04_Get(void) { - return hcsr04_dev_loc->CaptureValue[0]; + return hcsr04_dev_loc->CaptureValue[0]; } int32_t PIOS_HCSR04_Completed(void) { - return hcsr04_dev_loc->CapCounter[0]; + return hcsr04_dev_loc->CapCounter[0]; } -static void PIOS_HCSR04_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) +static void PIOS_HCSR04_tim_overflow_cb(uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) { - struct pios_hcsr04_dev * hcsr04_dev = (struct pios_hcsr04_dev *)context; + struct pios_hcsr04_dev *hcsr04_dev = (struct pios_hcsr04_dev *)context; - if (!PIOS_HCSR04_validate(hcsr04_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_HCSR04_validate(hcsr04_dev)) { + /* Invalid device specified */ + return; + } - if (channel >= hcsr04_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (channel >= hcsr04_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - hcsr04_dev->us_since_update[channel] += count; - if(hcsr04_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { - hcsr04_dev->CaptureState[channel] = 0; - hcsr04_dev->RiseValue[channel] = 0; - hcsr04_dev->FallValue[channel] = 0; - hcsr04_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; - hcsr04_dev->us_since_update[channel] = 0; - } - - return; + hcsr04_dev->us_since_update[channel] += count; + if (hcsr04_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { + hcsr04_dev->CaptureState[channel] = 0; + hcsr04_dev->RiseValue[channel] = 0; + hcsr04_dev->FallValue[channel] = 0; + hcsr04_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; + hcsr04_dev->us_since_update[channel] = 0; + } } -static void PIOS_HCSR04_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) +static void PIOS_HCSR04_tim_edge_cb(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) { - /* Recover our device context */ - struct pios_hcsr04_dev * hcsr04_dev = (struct pios_hcsr04_dev *)context; + /* Recover our device context */ + struct pios_hcsr04_dev *hcsr04_dev = (struct pios_hcsr04_dev *)context; - if (!PIOS_HCSR04_validate(hcsr04_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_HCSR04_validate(hcsr04_dev)) { + /* Invalid device specified */ + return; + } - if (chan_idx >= hcsr04_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (chan_idx >= hcsr04_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - const struct pios_tim_channel * chan = &hcsr04_dev->cfg->channels[chan_idx]; + const struct pios_tim_channel *chan = &hcsr04_dev->cfg->channels[chan_idx]; - if (hcsr04_dev->CaptureState[chan_idx] == 0) { - hcsr04_dev->RiseValue[chan_idx] = count; - hcsr04_dev->us_since_update[chan_idx] = 0; - } else { - hcsr04_dev->FallValue[chan_idx] = count; - } + if (hcsr04_dev->CaptureState[chan_idx] == 0) { + hcsr04_dev->RiseValue[chan_idx] = count; + hcsr04_dev->us_since_update[chan_idx] = 0; + } else { + hcsr04_dev->FallValue[chan_idx] = count; + } - // flip state machine and capture value here - /* Simple rise or fall state machine */ - TIM_ICInitTypeDef TIM_ICInitStructure = hcsr04_dev->cfg->tim_ic_init; - if (hcsr04_dev->CaptureState[chan_idx] == 0) { - /* Switch states */ - hcsr04_dev->CaptureState[chan_idx] = 1; + // flip state machine and capture value here + /* Simple rise or fall state machine */ + TIM_ICInitTypeDef TIM_ICInitStructure = hcsr04_dev->cfg->tim_ic_init; + if (hcsr04_dev->CaptureState[chan_idx] == 0) { + /* Switch states */ + hcsr04_dev->CaptureState[chan_idx] = 1; - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - } else { - /* Capture computation */ - if (hcsr04_dev->FallValue[chan_idx] > hcsr04_dev->RiseValue[chan_idx]) { - hcsr04_dev->CaptureValue[chan_idx] = (hcsr04_dev->FallValue[chan_idx] - hcsr04_dev->RiseValue[chan_idx]); - } else { - hcsr04_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - hcsr04_dev->RiseValue[chan_idx]) + hcsr04_dev->FallValue[chan_idx]); - } + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } else { + /* Capture computation */ + if (hcsr04_dev->FallValue[chan_idx] > hcsr04_dev->RiseValue[chan_idx]) { + hcsr04_dev->CaptureValue[chan_idx] = (hcsr04_dev->FallValue[chan_idx] - hcsr04_dev->RiseValue[chan_idx]); + } else { + hcsr04_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - hcsr04_dev->RiseValue[chan_idx]) + hcsr04_dev->FallValue[chan_idx]); + } - /* Switch states */ - hcsr04_dev->CaptureState[chan_idx] = 0; + /* Switch states */ + hcsr04_dev->CaptureState[chan_idx] = 0; - /* Increase supervisor counter */ - hcsr04_dev->CapCounter[chan_idx]++; - - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - } + /* Increase supervisor counter */ + hcsr04_dev->CapCounter[chan_idx]++; + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } } #endif /* PIOS_INCLUDE_HCSR04 */ - diff --git a/flight/pios/common/pios_hmc5843.c b/flight/pios/common/pios_hmc5843.c index 83dcc07bf..767e9fbc5 100644 --- a/flight/pios/common/pios_hmc5843.c +++ b/flight/pios/common/pios_hmc5843.c @@ -13,19 +13,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,343 +36,363 @@ #include /* HMC5843 Addresses */ -#define PIOS_HMC5843_I2C_ADDR 0x1E -#define PIOS_HMC5843_CONFIG_REG_A (uint8_t)0x00 -#define PIOS_HMC5843_CONFIG_REG_B (uint8_t)0x01 -#define PIOS_HMC5843_MODE_REG (uint8_t)0x02 -#define PIOS_HMC5843_DATAOUT_XMSB_REG 0x03 -#define PIOS_HMC5843_DATAOUT_XLSB_REG 0x04 -#define PIOS_HMC5843_DATAOUT_YMSB_REG 0x05 -#define PIOS_HMC5843_DATAOUT_YLSB_REG 0x06 -#define PIOS_HMC5843_DATAOUT_ZMSB_REG 0x07 -#define PIOS_HMC5843_DATAOUT_ZLSB_REG 0x08 -#define PIOS_HMC5843_DATAOUT_STATUS_REG 0x09 -#define PIOS_HMC5843_DATAOUT_IDA_REG 0x0A -#define PIOS_HMC5843_DATAOUT_IDB_REG 0x0B -#define PIOS_HMC5843_DATAOUT_IDC_REG 0x0C +#define PIOS_HMC5843_I2C_ADDR 0x1E +#define PIOS_HMC5843_CONFIG_REG_A (uint8_t)0x00 +#define PIOS_HMC5843_CONFIG_REG_B (uint8_t)0x01 +#define PIOS_HMC5843_MODE_REG (uint8_t)0x02 +#define PIOS_HMC5843_DATAOUT_XMSB_REG 0x03 +#define PIOS_HMC5843_DATAOUT_XLSB_REG 0x04 +#define PIOS_HMC5843_DATAOUT_YMSB_REG 0x05 +#define PIOS_HMC5843_DATAOUT_YLSB_REG 0x06 +#define PIOS_HMC5843_DATAOUT_ZMSB_REG 0x07 +#define PIOS_HMC5843_DATAOUT_ZLSB_REG 0x08 +#define PIOS_HMC5843_DATAOUT_STATUS_REG 0x09 +#define PIOS_HMC5843_DATAOUT_IDA_REG 0x0A +#define PIOS_HMC5843_DATAOUT_IDB_REG 0x0B +#define PIOS_HMC5843_DATAOUT_IDC_REG 0x0C /* Output Data Rate */ -#define PIOS_HMC5843_ODR_05 0x00 -#define PIOS_HMC5843_ODR_1 0x04 -#define PIOS_HMC5843_ODR_2 0x08 -#define PIOS_HMC5843_ODR_5 0x0C -#define PIOS_HMC5843_ODR_10 0x10 -#define PIOS_HMC5843_ODR_20 0x14 -#define PIOS_HMC5843_ODR_50 0x18 +#define PIOS_HMC5843_ODR_05 0x00 +#define PIOS_HMC5843_ODR_1 0x04 +#define PIOS_HMC5843_ODR_2 0x08 +#define PIOS_HMC5843_ODR_5 0x0C +#define PIOS_HMC5843_ODR_10 0x10 +#define PIOS_HMC5843_ODR_20 0x14 +#define PIOS_HMC5843_ODR_50 0x18 /* Measure configuration */ -#define PIOS_HMC5843_MEASCONF_NORMAL 0x00 -#define PIOS_HMC5843_MEASCONF_BIAS_POS 0x01 -#define PIOS_HMC5843_MEASCONF_BIAS_NEG 0x02 +#define PIOS_HMC5843_MEASCONF_NORMAL 0x00 +#define PIOS_HMC5843_MEASCONF_BIAS_POS 0x01 +#define PIOS_HMC5843_MEASCONF_BIAS_NEG 0x02 /* Gain settings */ -#define PIOS_HMC5843_GAIN_0_7 0x00 -#define PIOS_HMC5843_GAIN_1 0x20 -#define PIOS_HMC5843_GAIN_1_5 0x40 -#define PIOS_HMC5843_GAIN_2 0x60 -#define PIOS_HMC5843_GAIN_3_2 0x80 -#define PIOS_HMC5843_GAIN_3_8 0xA0 -#define PIOS_HMC5843_GAIN_4_5 0xC0 -#define PIOS_HMC5843_GAIN_6_5 0xE0 +#define PIOS_HMC5843_GAIN_0_7 0x00 +#define PIOS_HMC5843_GAIN_1 0x20 +#define PIOS_HMC5843_GAIN_1_5 0x40 +#define PIOS_HMC5843_GAIN_2 0x60 +#define PIOS_HMC5843_GAIN_3_2 0x80 +#define PIOS_HMC5843_GAIN_3_8 0xA0 +#define PIOS_HMC5843_GAIN_4_5 0xC0 +#define PIOS_HMC5843_GAIN_6_5 0xE0 /* Modes */ -#define PIOS_HMC5843_MODE_CONTINUOUS 0x00 -#define PIOS_HMC5843_MODE_SINGLE 0x01 -#define PIOS_HMC5843_MODE_IDLE 0x02 -#define PIOS_HMC5843_MODE_SLEEP 0x02 +#define PIOS_HMC5843_MODE_CONTINUOUS 0x00 +#define PIOS_HMC5843_MODE_SINGLE 0x01 +#define PIOS_HMC5843_MODE_IDLE 0x02 +#define PIOS_HMC5843_MODE_SLEEP 0x02 /* Sensitivity Conversion Values */ -#define PIOS_HMC5843_Sensitivity_0_7Ga 1602 // LSB/Ga -#define PIOS_HMC5843_Sensitivity_1Ga 1300 // LSB/Ga -#define PIOS_HMC5843_Sensitivity_1_5Ga 970 // LSB/Ga -#define PIOS_HMC5843_Sensitivity_2Ga 780 // LSB/Ga -#define PIOS_HMC5843_Sensitivity_3_2Ga 530 // LSB/Ga -#define PIOS_HMC5843_Sensitivity_3_8Ga 460 // LSB/Ga -#define PIOS_HMC5843_Sensitivity_4_5Ga 390 // LSB/Ga -#define PIOS_HMC5843_Sensitivity_6_5Ga 280 // LSB/Ga --> NOT RECOMMENDED +#define PIOS_HMC5843_Sensitivity_0_7Ga 1602 // LSB/Ga +#define PIOS_HMC5843_Sensitivity_1Ga 1300 // LSB/Ga +#define PIOS_HMC5843_Sensitivity_1_5Ga 970 // LSB/Ga +#define PIOS_HMC5843_Sensitivity_2Ga 780 // LSB/Ga +#define PIOS_HMC5843_Sensitivity_3_2Ga 530 // LSB/Ga +#define PIOS_HMC5843_Sensitivity_3_8Ga 460 // LSB/Ga +#define PIOS_HMC5843_Sensitivity_4_5Ga 390 // LSB/Ga +#define PIOS_HMC5843_Sensitivity_6_5Ga 280 // LSB/Ga --> NOT RECOMMENDED /* Global Variables */ /* Local Types */ typedef struct { - uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ - uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ - uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ - uint8_t Mode; + uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ + uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t Mode; } PIOS_HMC5843_ConfigTypeDef; /* Local Variables */ volatile bool pios_hmc5843_data_ready; -static void PIOS_HMC5843_Config(PIOS_HMC5843_ConfigTypeDef * HMC5843_Config_Struct); -static bool PIOS_HMC5843_Read(uint8_t address, uint8_t * buffer, uint8_t len); +static void PIOS_HMC5843_Config(PIOS_HMC5843_ConfigTypeDef *HMC5843_Config_Struct); +static bool PIOS_HMC5843_Read(uint8_t address, uint8_t *buffer, uint8_t len); static bool PIOS_HMC5843_Write(uint8_t address, uint8_t buffer); -void PIOS_HMC5843_EndOfConversion (void) +void PIOS_HMC5843_EndOfConversion(void) { - pios_hmc5843_data_ready = true; + pios_hmc5843_data_ready = true; } static const struct pios_exti_cfg pios_exti_hmc5843_cfg __exti_config = { - .vector = PIOS_HMC5843_EndOfConversion, - .line = PIOS_HMC5843_DRDY_EXTI_LINE, - .pin = { - .gpio = PIOS_HMC5843_DRDY_GPIO_PORT, - .init = { - .GPIO_Pin = PIOS_HMC5843_DRDY_GPIO_PIN, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = PIOS_HMC5843_DRDY_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_HMC5843_DRDY_PRIO, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = PIOS_HMC5843_DRDY_EXTI_LINE, - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_HMC5843_EndOfConversion, + .line = PIOS_HMC5843_DRDY_EXTI_LINE, + .pin = { + .gpio = PIOS_HMC5843_DRDY_GPIO_PORT, + .init = { + .GPIO_Pin = PIOS_HMC5843_DRDY_GPIO_PIN, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = PIOS_HMC5843_DRDY_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_HMC5843_DRDY_PRIO, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = PIOS_HMC5843_DRDY_EXTI_LINE, + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; /** - * @brief Initialise the HMC5843 sensor - */ + * @brief Initialise the HMC5843 sensor + */ void PIOS_HMC5843_Init(void) { - /* Enable DRDY GPIO clock */ - RCC_APB2PeriphClockCmd(PIOS_HMC5843_DRDY_CLK | RCC_APB2Periph_AFIO, ENABLE); + /* Enable DRDY GPIO clock */ + RCC_APB2PeriphClockCmd(PIOS_HMC5843_DRDY_CLK | RCC_APB2Periph_AFIO, ENABLE); - PIOS_EXTI_Init(&pios_exti_hmc5843_cfg); + PIOS_EXTI_Init(&pios_exti_hmc5843_cfg); - /* Configure the HMC5843 Sensor */ - PIOS_HMC5843_ConfigTypeDef HMC5843_InitStructure; - HMC5843_InitStructure.M_ODR = PIOS_HMC5843_ODR_10; - HMC5843_InitStructure.Meas_Conf = PIOS_HMC5843_MEASCONF_NORMAL; - HMC5843_InitStructure.Gain = PIOS_HMC5843_GAIN_2; - HMC5843_InitStructure.Mode = PIOS_HMC5843_MODE_CONTINUOUS; - PIOS_HMC5843_Config(&HMC5843_InitStructure); + /* Configure the HMC5843 Sensor */ + PIOS_HMC5843_ConfigTypeDef HMC5843_InitStructure; + HMC5843_InitStructure.M_ODR = PIOS_HMC5843_ODR_10; + HMC5843_InitStructure.Meas_Conf = PIOS_HMC5843_MEASCONF_NORMAL; + HMC5843_InitStructure.Gain = PIOS_HMC5843_GAIN_2; + HMC5843_InitStructure.Mode = PIOS_HMC5843_MODE_CONTINUOUS; + PIOS_HMC5843_Config(&HMC5843_InitStructure); - pios_hmc5843_data_ready = false; + pios_hmc5843_data_ready = false; } /** -* Initialise the HMC5843 sensor -* -* CTRL_REGA: Control Register A -* Read Write -* Default value: 0x10 -* 7:5 0 These bits must be cleared for correct operation. -* 4:2 DO2-DO0: Data Output Rate Bits -* DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz) -* ------------------------------------------------------ -* 0 | 0 | 0 | 0.5 -* 0 | 0 | 1 | 1 -* 0 | 1 | 0 | 2 -* 0 | 1 | 1 | 5 -* 1 | 0 | 0 | 10 (default) -* 1 | 0 | 1 | 20 -* 1 | 1 | 0 | 50 -* 1 | 1 | 1 | Not Used -* 1:0 MS1-MS0: Measurement Configuration Bits -* MS1 | MS0 | MODE -* ------------------------------ -* 0 | 0 | Normal -* 0 | 1 | Positive Bias -* 1 | 0 | Negative Bias -* 1 | 1 | Not Used -* -* CTRL_REGB: Control RegisterB -* Read Write -* Default value: 0x20 -* 7:5 GN2-GN0: Gain Configuration Bits. -* GN2 | GN1 | GN0 | Mag Input | Gain | Output Range -* | | | Range[Ga] | [LSB/mGa] | -* ------------------------------------------------------ -* 0 | 0 | 0 | ±0.7Ga | 1620 | 0xF800–0x07FF (-2048:2047) -* 0 | 0 | 1 | ±1.0Ga (def) | 1300 | 0xF800–0x07FF (-2048:2047) -* 0 | 1 | 0 | ±1.5Ga | 970 | 0xF800–0x07FF (-2048:2047) -* 0 | 1 | 1 | ±2.0Ga | 780 | 0xF800–0x07FF (-2048:2047) -* 1 | 0 | 0 | ±3.2Ga | 530 | 0xF800–0x07FF (-2048:2047) -* 1 | 0 | 1 | ±3.8Ga | 460 | 0xF800–0x07FF (-2048:2047) -* 1 | 1 | 0 | ±4.5Ga | 390 | 0xF800–0x07FF (-2048:2047) -* 1 | 1 | 1 | ±6.5Ga | 280 | 0xF800–0x07FF (-2048:2047) -* |Not recommended| -* -* 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation. -* -* _MODE_REG: Mode Register -* Read Write -* Default value: 0x02 -* 7:2 0 These bits must be cleared for correct operation. -* 1:0 MD1-MD0: Mode Select Bits -* MS1 | MS0 | MODE -* ------------------------------ -* 0 | 0 | Continuous-Conversion Mode. -* 0 | 1 | Single-Conversion Mode -* 1 | 0 | Negative Bias -* 1 | 1 | Sleep Mode -*/ -static void PIOS_HMC5843_Config(PIOS_HMC5843_ConfigTypeDef * HMC5843_Config_Struct) + * Initialise the HMC5843 sensor + * + * CTRL_REGA: Control Register A + * Read Write + * Default value: 0x10 + * 7:5 0 These bits must be cleared for correct operation. + * 4:2 DO2-DO0: Data Output Rate Bits + * DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz) + * ------------------------------------------------------ + * 0 | 0 | 0 | 0.5 + * 0 | 0 | 1 | 1 + * 0 | 1 | 0 | 2 + * 0 | 1 | 1 | 5 + * 1 | 0 | 0 | 10 (default) + * 1 | 0 | 1 | 20 + * 1 | 1 | 0 | 50 + * 1 | 1 | 1 | Not Used + * 1:0 MS1-MS0: Measurement Configuration Bits + * MS1 | MS0 | MODE + * ------------------------------ + * 0 | 0 | Normal + * 0 | 1 | Positive Bias + * 1 | 0 | Negative Bias + * 1 | 1 | Not Used + * + * CTRL_REGB: Control RegisterB + * Read Write + * Default value: 0x20 + * 7:5 GN2-GN0: Gain Configuration Bits. + * GN2 | GN1 | GN0 | Mag Input | Gain | Output Range + * | | | Range[Ga] | [LSB/mGa] | + * ------------------------------------------------------ + * 0 | 0 | 0 | ±0.7Ga | 1620 | 0xF800–0x07FF (-2048:2047) + * 0 | 0 | 1 | ±1.0Ga (def) | 1300 | 0xF800–0x07FF (-2048:2047) + * 0 | 1 | 0 | ±1.5Ga | 970 | 0xF800–0x07FF (-2048:2047) + * 0 | 1 | 1 | ±2.0Ga | 780 | 0xF800–0x07FF (-2048:2047) + * 1 | 0 | 0 | ±3.2Ga | 530 | 0xF800–0x07FF (-2048:2047) + * 1 | 0 | 1 | ±3.8Ga | 460 | 0xF800–0x07FF (-2048:2047) + * 1 | 1 | 0 | ±4.5Ga | 390 | 0xF800–0x07FF (-2048:2047) + * 1 | 1 | 1 | ±6.5Ga | 280 | 0xF800–0x07FF (-2048:2047) + * |Not recommended| + * + * 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation. + * + * _MODE_REG: Mode Register + * Read Write + * Default value: 0x02 + * 7:2 0 These bits must be cleared for correct operation. + * 1:0 MD1-MD0: Mode Select Bits + * MS1 | MS0 | MODE + * ------------------------------ + * 0 | 0 | Continuous-Conversion Mode. + * 0 | 1 | Single-Conversion Mode + * 1 | 0 | Negative Bias + * 1 | 1 | Sleep Mode + */ +static void PIOS_HMC5843_Config(PIOS_HMC5843_ConfigTypeDef *HMC5843_Config_Struct) { - uint8_t CRTLA = 0x00; - uint8_t CRTLB = 0x00; - uint8_t MODE = 0x00; + uint8_t CRTLA = 0x00; + uint8_t CRTLB = 0x00; + uint8_t MODE = 0x00; - CRTLA |= (uint8_t) (HMC5843_Config_Struct->M_ODR | HMC5843_Config_Struct->Meas_Conf); - CRTLB |= (uint8_t) (HMC5843_Config_Struct->Gain); - MODE |= (uint8_t) (HMC5843_Config_Struct->Mode); + CRTLA |= (uint8_t)(HMC5843_Config_Struct->M_ODR | HMC5843_Config_Struct->Meas_Conf); + CRTLB |= (uint8_t)(HMC5843_Config_Struct->Gain); + MODE |= (uint8_t)(HMC5843_Config_Struct->Mode); - // CRTL_REGA - while (!PIOS_HMC5843_Write(PIOS_HMC5843_CONFIG_REG_A, CRTLA)) ; + // CRTL_REGA + while (!PIOS_HMC5843_Write(PIOS_HMC5843_CONFIG_REG_A, CRTLA)) { + ; + } - // CRTL_REGB - while (!PIOS_HMC5843_Write(PIOS_HMC5843_CONFIG_REG_B, CRTLB)) ; + // CRTL_REGB + while (!PIOS_HMC5843_Write(PIOS_HMC5843_CONFIG_REG_B, CRTLB)) { + ; + } - // Mode register - while (!PIOS_HMC5843_Write(PIOS_HMC5843_MODE_REG, MODE)) ; + // Mode register + while (!PIOS_HMC5843_Write(PIOS_HMC5843_MODE_REG, MODE)) { + ; + } } /** -* Read the magnetic readings from the sensor -*/ + * Read the magnetic readings from the sensor + */ void PIOS_HMC5843_ReadMag(int16_t out[3]) { - uint8_t buffer[6]; - uint8_t crtlB; + uint8_t buffer[6]; + uint8_t crtlB; - pios_hmc5843_data_ready = false; + pios_hmc5843_data_ready = false; - while (!PIOS_HMC5843_Read(PIOS_HMC5843_CONFIG_REG_B, &crtlB, 1)) ; - while (!PIOS_HMC5843_Read(PIOS_HMC5843_DATAOUT_XMSB_REG, buffer, 6)) ; + while (!PIOS_HMC5843_Read(PIOS_HMC5843_CONFIG_REG_B, &crtlB, 1)) { + ; + } + while (!PIOS_HMC5843_Read(PIOS_HMC5843_DATAOUT_XMSB_REG, buffer, 6)) { + ; + } - switch (crtlB & 0xE0) { - case 0x00: - for (int i = 0; i < 3; i++) - out[i] = ((int16_t) ((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_0_7Ga; - break; - case 0x20: - for (int i = 0; i < 3; i++) - out[i] = ((int16_t) ((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_1Ga; - break; - case 0x40: - for (int i = 0; i < 3; i++) - out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_1_5Ga; - break; - case 0x60: - for (int i = 0; i < 3; i++) - out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_2Ga; - break; - case 0x80: - for (int i = 0; i < 3; i++) - out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_3_2Ga; - break; - case 0xA0: - for (int i = 0; i < 3; i++) - out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_3_8Ga; - break; - case 0xC0: - for (int i = 0; i < 3; i++) - out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_4_5Ga; - break; - case 0xE0: - for (int i = 0; i < 3; i++) - out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_6_5Ga; - break; - } + switch (crtlB & 0xE0) { + case 0x00: + for (int i = 0; i < 3; i++) { + out[i] = ((int16_t)((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_0_7Ga; + } + break; + case 0x20: + for (int i = 0; i < 3; i++) { + out[i] = ((int16_t)((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_1Ga; + } + break; + case 0x40: + for (int i = 0; i < 3; i++) { + out[i] = (int16_t)(((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_1_5Ga; + } + break; + case 0x60: + for (int i = 0; i < 3; i++) { + out[i] = (int16_t)(((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_2Ga; + } + break; + case 0x80: + for (int i = 0; i < 3; i++) { + out[i] = (int16_t)(((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_3_2Ga; + } + break; + case 0xA0: + for (int i = 0; i < 3; i++) { + out[i] = (int16_t)(((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_3_8Ga; + } + break; + case 0xC0: + for (int i = 0; i < 3; i++) { + out[i] = (int16_t)(((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_4_5Ga; + } + break; + case 0xE0: + for (int i = 0; i < 3; i++) { + out[i] = (int16_t)(((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / PIOS_HMC5843_Sensitivity_6_5Ga; + } + break; + } } /** -* Read the identification bytes from the sensor -*/ + * Read the identification bytes from the sensor + */ void PIOS_HMC5843_ReadID(uint8_t out[4]) { - while (!PIOS_HMC5843_Read(PIOS_HMC5843_DATAOUT_IDA_REG, out, 3)) ; - out[3] = '\0'; + while (!PIOS_HMC5843_Read(PIOS_HMC5843_DATAOUT_IDA_REG, out, 3)) { + ; + } + out[3] = '\0'; } bool PIOS_HMC5843_NewDataAvailable(void) { - return (pios_hmc5843_data_ready); + return pios_hmc5843_data_ready; } /** -* Reads one or more bytes into a buffer -* \param[in] address HMC5843 register address (depends on size) -* \param[out] buffer destination buffer -* \param[in] len number of bytes which should be read -* \return 0 if operation was successful -* \return -1 if error during I2C transfer -* \return -2 if unable to claim i2c device -*/ -static bool PIOS_HMC5843_Read(uint8_t address, uint8_t * buffer, uint8_t len) + * Reads one or more bytes into a buffer + * \param[in] address HMC5843 register address (depends on size) + * \param[out] buffer destination buffer + * \param[in] len number of bytes which should be read + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + * \return -2 if unable to claim i2c device + */ +static bool PIOS_HMC5843_Read(uint8_t address, uint8_t *buffer, uint8_t len) { - uint8_t addr_buffer[] = { - address, - }; + uint8_t addr_buffer[] = { + address, + }; - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = PIOS_HMC5843_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(addr_buffer), - .buf = addr_buffer, - } - , - { - .info = __func__, - .addr = PIOS_HMC5843_I2C_ADDR, - .rw = PIOS_I2C_TXN_READ, - .len = len, - .buf = buffer, - } - }; + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = PIOS_HMC5843_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(addr_buffer), + .buf = addr_buffer, + } + , + { + .info = __func__, + .addr = PIOS_HMC5843_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); } /** -* Writes one or more bytes to the HMC5843 -* \param[in] address Register address -* \param[in] buffer source buffer -* \return 0 if operation was successful -* \retval -1 if error during I2C transfer -* \retval -2 if unable to claim i2c device -*/ + * Writes one or more bytes to the HMC5843 + * \param[in] address Register address + * \param[in] buffer source buffer + * \return 0 if operation was successful + * \retval -1 if error during I2C transfer + * \retval -2 if unable to claim i2c device + */ static bool PIOS_HMC5843_Write(uint8_t address, uint8_t buffer) { - uint8_t data[] = { - address, - buffer, - }; + uint8_t data[] = { + address, + buffer, + }; - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = PIOS_HMC5843_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(data), - .buf = data, - } - , - }; + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = PIOS_HMC5843_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(data), + .buf = data, + } + , + }; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); } #endif /* PIOS_INCLUDE_HMC5843 */ diff --git a/flight/pios/common/pios_hmc5883.c b/flight/pios/common/pios_hmc5883.c index 1e676fbcb..d425e16dc 100644 --- a/flight/pios/common/pios_hmc5883.c +++ b/flight/pios/common/pios_hmc5883.c @@ -12,19 +12,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,29 +39,29 @@ /* Local Variables */ volatile bool pios_hmc5883_data_ready; -static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg * cfg); -static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t * buffer, uint8_t len); +static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg *cfg); +static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t *buffer, uint8_t len); static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer); -static const struct pios_hmc5883_cfg * dev_cfg; +static const struct pios_hmc5883_cfg *dev_cfg; /** * @brief Initialize the HMC5883 magnetometer sensor. * @return none */ -void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg * cfg) +void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg *cfg) { - dev_cfg = cfg; // store config before enabling interrupt + dev_cfg = cfg; // store config before enabling interrupt #ifdef PIOS_HMC5883_HAS_GPIOS - PIOS_EXTI_Init(cfg->exti_cfg); + PIOS_EXTI_Init(cfg->exti_cfg); #endif - int32_t val = PIOS_HMC5883_Config(cfg); - - PIOS_Assert(val == 0); - - pios_hmc5883_data_ready = false; + int32_t val = PIOS_HMC5883_Config(cfg); + + PIOS_Assert(val == 0); + + pios_hmc5883_data_ready = false; } /** @@ -124,29 +124,33 @@ void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg * cfg) * 1 | 1 | Sleep Mode */ static uint8_t CTRLB = 0x00; -static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg * cfg) +static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg *cfg) { - uint8_t CTRLA = 0x00; - uint8_t MODE = 0x00; - CTRLB = 0; - - CTRLA |= (uint8_t) (cfg->M_ODR | cfg->Meas_Conf); - CTRLB |= (uint8_t) (cfg->Gain); - MODE |= (uint8_t) (cfg->Mode); - - // CRTL_REGA - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, CTRLA) != 0) - return -1; - - // CRTL_REGB - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, CTRLB) != 0) - return -1; - - // Mode register - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, MODE) != 0) - return -1; - - return 0; + uint8_t CTRLA = 0x00; + uint8_t MODE = 0x00; + + CTRLB = 0; + + CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf); + CTRLB |= (uint8_t)(cfg->Gain); + MODE |= (uint8_t)(cfg->Mode); + + // CRTL_REGA + if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, CTRLA) != 0) { + return -1; + } + + // CRTL_REGB + if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, CTRLB) != 0) { + return -1; + } + + // Mode register + if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, MODE) != 0) { + return -1; + } + + return 0; } /** @@ -156,58 +160,58 @@ static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg * cfg) */ int32_t PIOS_HMC5883_ReadMag(int16_t out[3]) { - pios_hmc5883_data_ready = false; - uint8_t buffer[6]; - int32_t temp; - int32_t sensitivity; - - if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_XMSB_REG, buffer, 6) != 0) { - return -1; - } - - switch (CTRLB & 0xE0) { - case 0x00: - sensitivity = PIOS_HMC5883_Sensitivity_0_88Ga; - break; - case 0x20: - sensitivity = PIOS_HMC5883_Sensitivity_1_3Ga; - break; - case 0x40: - sensitivity = PIOS_HMC5883_Sensitivity_1_9Ga; - break; - case 0x60: - sensitivity = PIOS_HMC5883_Sensitivity_2_5Ga; - break; - case 0x80: - sensitivity = PIOS_HMC5883_Sensitivity_4_0Ga; - break; - case 0xA0: - sensitivity = PIOS_HMC5883_Sensitivity_4_7Ga; - break; - case 0xC0: - sensitivity = PIOS_HMC5883_Sensitivity_5_6Ga; - break; - case 0xE0: - sensitivity = PIOS_HMC5883_Sensitivity_8_1Ga; - break; - default: - PIOS_Assert(0); - } - - for (int i = 0; i < 3; i++) { - temp = ((int16_t) ((uint16_t) buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / sensitivity; - out[i] = temp; - } - // Data reads out as X,Z,Y - temp = out[2]; - out[2] = out[1]; - out[1] = temp; - - // This should not be necessary but for some reason it is coming out of continuous conversion mode - PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_CONTINUOUS); - - return 0; + pios_hmc5883_data_ready = false; + uint8_t buffer[6]; + int32_t temp; + int32_t sensitivity; + + if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_XMSB_REG, buffer, 6) != 0) { + return -1; + } + + switch (CTRLB & 0xE0) { + case 0x00: + sensitivity = PIOS_HMC5883_Sensitivity_0_88Ga; + break; + case 0x20: + sensitivity = PIOS_HMC5883_Sensitivity_1_3Ga; + break; + case 0x40: + sensitivity = PIOS_HMC5883_Sensitivity_1_9Ga; + break; + case 0x60: + sensitivity = PIOS_HMC5883_Sensitivity_2_5Ga; + break; + case 0x80: + sensitivity = PIOS_HMC5883_Sensitivity_4_0Ga; + break; + case 0xA0: + sensitivity = PIOS_HMC5883_Sensitivity_4_7Ga; + break; + case 0xC0: + sensitivity = PIOS_HMC5883_Sensitivity_5_6Ga; + break; + case 0xE0: + sensitivity = PIOS_HMC5883_Sensitivity_8_1Ga; + break; + default: + PIOS_Assert(0); + } + + for (int i = 0; i < 3; i++) { + temp = ((int16_t)((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / sensitivity; + out[i] = temp; + } + // Data reads out as X,Z,Y + temp = out[2]; + out[2] = out[1]; + out[1] = temp; + + // This should not be necessary but for some reason it is coming out of continuous conversion mode + PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_CONTINUOUS); + + return 0; } @@ -218,9 +222,10 @@ int32_t PIOS_HMC5883_ReadMag(int16_t out[3]) */ uint8_t PIOS_HMC5883_ReadID(uint8_t out[4]) { - uint8_t retval = PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_IDA_REG, out, 3); - out[3] = '\0'; - return retval; + uint8_t retval = PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_IDA_REG, out, 3); + + out[3] = '\0'; + return retval; } /** @@ -230,7 +235,7 @@ uint8_t PIOS_HMC5883_ReadID(uint8_t out[4]) */ bool PIOS_HMC5883_NewDataAvailable(void) { - return (pios_hmc5883_data_ready); + return pios_hmc5883_data_ready; } /** @@ -242,31 +247,31 @@ bool PIOS_HMC5883_NewDataAvailable(void) * \return -1 if error during I2C transfer * \return -2 if unable to claim i2c device */ -static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t * buffer, uint8_t len) +static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t *buffer, uint8_t len) { - uint8_t addr_buffer[] = { - address, - }; - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = PIOS_HMC5883_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(addr_buffer), - .buf = addr_buffer, - } - , - { - .info = __func__, - .addr = PIOS_HMC5883_I2C_ADDR, - .rw = PIOS_I2C_TXN_READ, - .len = len, - .buf = buffer, - } - }; - - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + uint8_t addr_buffer[] = { + address, + }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = PIOS_HMC5883_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(addr_buffer), + .buf = addr_buffer, + } + , + { + .info = __func__, + .addr = PIOS_HMC5883_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); } /** @@ -279,23 +284,24 @@ static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t * buffer, uint8_t len) */ static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer) { - uint8_t data[] = { - address, - buffer, - }; - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = PIOS_HMC5883_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(data), - .buf = data, - } - , - }; - ; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + uint8_t data[] = { + address, + buffer, + }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = PIOS_HMC5883_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(data), + .buf = data, + } + , + }; + + ; + return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); } /** @@ -304,89 +310,101 @@ static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer) */ int32_t PIOS_HMC5883_Test(void) { - int32_t failed = 0; - uint8_t registers[3] = {0,0,0}; - uint8_t status; - uint8_t ctrl_a_read; - uint8_t ctrl_b_read; - uint8_t mode_read; - int16_t values[3]; - - - - /* Verify that ID matches (HMC5883 ID is null-terminated ASCII string "H43") */ - char id[4]; - PIOS_HMC5883_ReadID((uint8_t *)id); - if((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) // Expect H43 - return -1; - - /* Backup existing configuration */ - if (PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A,registers,3) != 0) - return -1; - - /* Stop the device and read out last value */ - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_IDLE) != 0) - return -1; - if( PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status,1) != 0) - return -1; - if (PIOS_HMC5883_ReadMag(values) != 0) - return -1; - - /* - * Put HMC5883 into self test mode - * This is done by placing measurement config into positive (0x01) or negative (0x10) bias - * and then placing the mode register into single-measurement mode. This causes the HMC5883 - * to create an artificial magnetic field of ~1.1 Gauss. - * - * If gain were PIOS_HMC5883_GAIN_2_5, for example, X and Y will read around +766 LSB - * (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga) - * - * Changing measurement config back to PIOS_HMC5883_MEASCONF_NORMAL will leave self-test mode. - */ - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, PIOS_HMC5883_MEASCONF_BIAS_POS | PIOS_HMC5883_ODR_15) != 0) - return -1; - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, PIOS_HMC5883_GAIN_8_1) != 0) - return -1; - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_SINGLE) != 0) - return -1; - - /* Must wait for value to be updated */ - PIOS_DELAY_WaitmS(200); - - if (PIOS_HMC5883_ReadMag(values) != 0) - return -1; - - /* - if(abs(values[0] - 766) > 20) - failed |= 1; - if(abs(values[1] - 766) > 20) - failed |= 1; - if(abs(values[2] - 713) > 20) - failed |= 1; - */ - - PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, &ctrl_a_read,1); - PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_B, &ctrl_b_read,1); - PIOS_HMC5883_Read(PIOS_HMC5883_MODE_REG, &mode_read,1); - PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status,1); - - - /* Restore backup configuration */ - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, registers[0]) != 0) - return -1; - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, registers[1]) != 0) - return -1; - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, registers[2]) != 0) - return -1; - - return failed; + int32_t failed = 0; + uint8_t registers[3] = { 0, 0, 0 }; + uint8_t status; + uint8_t ctrl_a_read; + uint8_t ctrl_b_read; + uint8_t mode_read; + int16_t values[3]; + + + /* Verify that ID matches (HMC5883 ID is null-terminated ASCII string "H43") */ + char id[4]; + + PIOS_HMC5883_ReadID((uint8_t *)id); + if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43 + return -1; + } + + /* Backup existing configuration */ + if (PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, registers, 3) != 0) { + return -1; + } + + /* Stop the device and read out last value */ + PIOS_DELAY_WaitmS(10); + if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_IDLE) != 0) { + return -1; + } + if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status, 1) != 0) { + return -1; + } + if (PIOS_HMC5883_ReadMag(values) != 0) { + return -1; + } + + /* + * Put HMC5883 into self test mode + * This is done by placing measurement config into positive (0x01) or negative (0x10) bias + * and then placing the mode register into single-measurement mode. This causes the HMC5883 + * to create an artificial magnetic field of ~1.1 Gauss. + * + * If gain were PIOS_HMC5883_GAIN_2_5, for example, X and Y will read around +766 LSB + * (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga) + * + * Changing measurement config back to PIOS_HMC5883_MEASCONF_NORMAL will leave self-test mode. + */ + PIOS_DELAY_WaitmS(10); + if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, PIOS_HMC5883_MEASCONF_BIAS_POS | PIOS_HMC5883_ODR_15) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, PIOS_HMC5883_GAIN_8_1) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_SINGLE) != 0) { + return -1; + } + + /* Must wait for value to be updated */ + PIOS_DELAY_WaitmS(200); + + if (PIOS_HMC5883_ReadMag(values) != 0) { + return -1; + } + + /* + if(abs(values[0] - 766) > 20) + failed |= 1; + if(abs(values[1] - 766) > 20) + failed |= 1; + if(abs(values[2] - 713) > 20) + failed |= 1; + */ + + PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, &ctrl_a_read, 1); + PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_B, &ctrl_b_read, 1); + PIOS_HMC5883_Read(PIOS_HMC5883_MODE_REG, &mode_read, 1); + PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status, 1); + + + /* Restore backup configuration */ + PIOS_DELAY_WaitmS(10); + if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, registers[0]) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, registers[1]) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, registers[2]) != 0) { + return -1; + } + + return failed; } /** @@ -394,9 +412,9 @@ int32_t PIOS_HMC5883_Test(void) */ bool PIOS_HMC5883_IRQHandler(void) { - pios_hmc5883_data_ready = true; - - return false; + pios_hmc5883_data_ready = true; + + return false; } #endif /* PIOS_INCLUDE_HMC5883 */ diff --git a/flight/pios/common/pios_i2c_esc.c b/flight/pios/common/pios_i2c_esc.c index 9409b9a68..4800f1bc5 100644 --- a/flight/pios/common/pios_i2c_esc.c +++ b/flight/pios/common/pios_i2c_esc.c @@ -13,19 +13,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,8 +34,8 @@ #ifdef PIOS_INCLUDE_I2C_ESC /* Known i2c ESC addresses */ -#define MK_I2C_ADDR 0x29 -#define ASTEC4_I2C_ADDR 0x02 +#define MK_I2C_ADDR 0x29 +#define ASTEC4_I2C_ADDR 0x02 bool PIOS_SetMKSpeed(uint8_t motornum, uint8_t speed); @@ -43,124 +43,134 @@ uint8_t base_address = MK_I2C_ADDR; uint8_t valid_motors = 0; bool PIOS_I2C_ESC_Config() { - base_address = MK_I2C_ADDR; - valid_motors = 0; - for(uint8_t i = 0; i < 4; i ++) - if(PIOS_SetMKSpeed(i, 0)) - valid_motors |= (1 << i); - - return true; + base_address = MK_I2C_ADDR; + valid_motors = 0; + for (uint8_t i = 0; i < 4; i++) { + if (PIOS_SetMKSpeed(i, 0)) { + valid_motors |= (1 << i); + } + } + + return true; } bool PIOS_I2C_ESC_SetSpeed(uint8_t speed[4]) { - /*bool success = true; - for(uint8_t i = 0; i < 4; i++) { - //if(valid_motors & (1 << i)) - success &= PIOS_SetMKSpeed(i, speed[i]); - } - return success; */ - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = MK_I2C_ADDR + 0, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(speed[0]), - .buf = &speed[0], - } , - { - .info = __func__, - .addr = MK_I2C_ADDR + 1, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(speed[1]), - .buf = &speed[1], - }, - { - .info = __func__, - .addr = MK_I2C_ADDR + 2, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(speed[2]), - .buf = &speed[2], - }, - { - .info = __func__, - .addr = MK_I2C_ADDR + 3, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(speed[3]), - .buf = &speed[3], - } - }; - - return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); + /*bool success = true; + for(uint8_t i = 0; i < 4; i++) { + //if(valid_motors & (1 << i)) + success &= PIOS_SetMKSpeed(i, speed[i]); + } + return success; */ + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = MK_I2C_ADDR + 0, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed[0]), + .buf = &speed[0], + }, + { + .info = __func__, + .addr = MK_I2C_ADDR + 1, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed[1]), + .buf = &speed[1], + }, + { + .info = __func__, + .addr = MK_I2C_ADDR + 2, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed[2]), + .buf = &speed[2], + }, + { + .info = __func__, + .addr = MK_I2C_ADDR + 3, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed[3]), + .buf = &speed[3], + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); } -bool PIOS_SetMKSpeed(uint8_t motornum, uint8_t speed) { - static uint8_t speeds[4] = {0,0,0,0}; - - if(motornum >= 4) - return false; - - if(speeds[motornum] == speed) - return true; - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = MK_I2C_ADDR + motornum, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(speed), - .buf = &speed, - } - }; - - return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); +bool PIOS_SetMKSpeed(uint8_t motornum, uint8_t speed) +{ + static uint8_t speeds[4] = { 0, 0, 0, 0 }; + + if (motornum >= 4) { + return false; + } + + if (speeds[motornum] == speed) { + return true; + } + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = MK_I2C_ADDR + motornum, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speed), + .buf = &speed, + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); } -bool PIOS_SetAstec4Address(uint8_t new_address) { - if((new_address < 0) || (new_address > 4)) - return false; - - uint8_t data[4] = {250, 0, new_address, 230+new_address}; - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = ASTEC4_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(data), - .buf = &data[0], - } - }; - - return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); +bool PIOS_SetAstec4Address(uint8_t new_address) +{ + if ((new_address < 0) || (new_address > 4)) { + return false; + } + + uint8_t data[4] = { 250, 0, new_address, 230 + new_address }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = ASTEC4_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(data), + .buf = &data[0], + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); } -bool PIOS_SetAstec4Speed(uint8_t motornum, uint8_t speed) { - static uint8_t speeds[5] = {0,0,0,0}; - - if((motornum < 0) || (motornum >= 4)) - return false; - - speeds[motornum] = speed; - - if(motornum != 3) - return true; - - /* Write in chunks of four */ - speeds[4] = 0xAA + speeds[0] + speeds[1] + speeds[2] + speeds[3]; - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = ASTEC4_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(speeds), - .buf = &speeds[0], - } - }; - - return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); +bool PIOS_SetAstec4Speed(uint8_t motornum, uint8_t speed) +{ + static uint8_t speeds[5] = { 0, 0, 0, 0 }; + + if ((motornum < 0) || (motornum >= 4)) { + return false; + } + + speeds[motornum] = speed; + + if (motornum != 3) { + return true; + } + + /* Write in chunks of four */ + speeds[4] = 0xAA + speeds[0] + speeds[1] + speeds[2] + speeds[3]; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = ASTEC4_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(speeds), + .buf = &speeds[0], + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); } #endif /* PIOS_INCLUDE_I2C_ESC */ diff --git a/flight/pios/common/pios_iap.c b/flight/pios/common/pios_iap.c index 77a18b00c..b991bcf30 100644 --- a/flight/pios/common/pios_iap.c +++ b/flight/pios/common/pios_iap.c @@ -6,65 +6,64 @@ * @brief Common IAP functionality * @{ * - * @file pios_iap.c + * @file pios_iap.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief In application programming functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ #include #include #ifdef PIOS_INCLUDE_IAP /**************************************************************************************** - * Private Definitions/Macros - ****************************************************************************************/ +* Private Definitions/Macros +****************************************************************************************/ /* these definitions reside here for protection and privacy. */ -#define IAP_MAGIC_WORD_1 0x1122 -#define IAP_MAGIC_WORD_2 0xAA55 +#define IAP_MAGIC_WORD_1 0x1122 +#define IAP_MAGIC_WORD_2 0xAA55 -#define UPPERWORD16(lw) (uint16_t)((uint32_t)(lw)>>16) -#define LOWERWORD16(lw) (uint16_t)((uint32_t)(lw)&0x0000ffff) -#define UPPERBYTE(w) (uint8_t)((w)>>8) -#define LOWERBYTE(w) (uint8_t)((w)&0x00ff) +#define UPPERWORD16(lw) (uint16_t)((uint32_t)(lw) >> 16) +#define LOWERWORD16(lw) (uint16_t)((uint32_t)(lw) & 0x0000ffff) +#define UPPERBYTE(w) (uint8_t)((w) >> 8) +#define LOWERBYTE(w) (uint8_t)((w) & 0x00ff) /**************************************************************************************** - * Private Functions - ****************************************************************************************/ +* Private Functions +****************************************************************************************/ /**************************************************************************************** - * Private (static) Data - ****************************************************************************************/ -const uint16_t pios_iap_cmd_list[] = - { - IAP_CMD1, - IAP_CMD2, - IAP_CMD3 - }; +* Private (static) Data +****************************************************************************************/ +const uint16_t pios_iap_cmd_list[] = { + IAP_CMD1, + IAP_CMD2, + IAP_CMD3 +}; /**************************************************************************************** - * Public/Global Data - ****************************************************************************************/ +* Public/Global Data +****************************************************************************************/ /*! * \brief PIOS_IAP_Init - performs required initializations for iap module. @@ -74,104 +73,97 @@ const uint16_t pios_iap_cmd_list[] = * * Created: Sep 8, 2010 10:10:48 PM by joe */ -void PIOS_IAP_Init( void ) +void PIOS_IAP_Init(void) { - PIOS_BKP_Init(); - PIOS_BKP_EnableWrite(); + PIOS_BKP_Init(); + PIOS_BKP_EnableWrite(); } /*! * \brief Determines if an In-Application-Programming request has been made. * \param *comm - Which communication stream to use for the IAP (USB, Telemetry, I2C, SPI, etc) * \return TRUE - if correct sequence found, along with 'comm' updated. - * FALSE - Note that 'comm' will have an invalid comm identifier. + * FALSE - Note that 'comm' will have an invalid comm identifier. * \retval * */ -uint32_t PIOS_IAP_CheckRequest( void ) +uint32_t PIOS_IAP_CheckRequest(void) { - uint32_t retval = false; - uint16_t reg1; - uint16_t reg2; + uint32_t retval = false; + uint16_t reg1; + uint16_t reg2; - reg1 = PIOS_BKP_ReadRegister(MAGIC_REG_1); - reg2 = PIOS_BKP_ReadRegister(MAGIC_REG_2); + reg1 = PIOS_BKP_ReadRegister(MAGIC_REG_1); + reg2 = PIOS_BKP_ReadRegister(MAGIC_REG_2); - if( reg1 == IAP_MAGIC_WORD_1 && reg2 == IAP_MAGIC_WORD_2 ) { - // We have a match. - retval = true; - } else { - retval = false; - } - return retval; + if (reg1 == IAP_MAGIC_WORD_1 && reg2 == IAP_MAGIC_WORD_2) { + // We have a match. + retval = true; + } else { + retval = false; + } + return retval; } - /*! * \brief Sets the 1st word of the request sequence. * \param n/a * \return n/a * \retval */ -void PIOS_IAP_SetRequest1(void) +void PIOS_IAP_SetRequest1(void) { - PIOS_BKP_WriteRegister(MAGIC_REG_1, IAP_MAGIC_WORD_1); + PIOS_BKP_WriteRegister(MAGIC_REG_1, IAP_MAGIC_WORD_1); } -void PIOS_IAP_SetRequest2(void) +void PIOS_IAP_SetRequest2(void) { - PIOS_BKP_WriteRegister(MAGIC_REG_2, IAP_MAGIC_WORD_2); + PIOS_BKP_WriteRegister(MAGIC_REG_2, IAP_MAGIC_WORD_2); } -void PIOS_IAP_ClearRequest(void) +void PIOS_IAP_ClearRequest(void) { - PIOS_BKP_WriteRegister(MAGIC_REG_1, 0); - PIOS_BKP_WriteRegister(MAGIC_REG_2, 0); + PIOS_BKP_WriteRegister(MAGIC_REG_1, 0); + PIOS_BKP_WriteRegister(MAGIC_REG_2, 0); } uint16_t PIOS_IAP_ReadBootCount(void) { - return PIOS_BKP_ReadRegister(IAP_BOOTCOUNT); + return PIOS_BKP_ReadRegister(IAP_BOOTCOUNT); } -void PIOS_IAP_WriteBootCount (uint16_t boot_count) +void PIOS_IAP_WriteBootCount(uint16_t boot_count) { - PIOS_BKP_WriteRegister(IAP_BOOTCOUNT, boot_count); + PIOS_BKP_WriteRegister(IAP_BOOTCOUNT, boot_count); } /** - * @brief Return one of the IAP command values passed from bootloader. - * @param number: the index of the command value (0..2). - * @retval the selected command value. - */ + * @brief Return one of the IAP command values passed from bootloader. + * @param number: the index of the command value (0..2). + * @retval the selected command value. + */ uint32_t PIOS_IAP_ReadBootCmd(uint8_t number) { - if(PIOS_IAP_CMD_COUNT < number) - { - PIOS_Assert(0); - } - else - { - return PIOS_BKP_ReadRegister(pios_iap_cmd_list[number]); - } + if (PIOS_IAP_CMD_COUNT < number) { + PIOS_Assert(0); + } else { + return PIOS_BKP_ReadRegister(pios_iap_cmd_list[number]); + } } /** - * @brief Write one of the IAP command values to be passed to firmware from bootloader. - * @param number: the index of the command value (0..2). - * @param value: value to be written. - */ + * @brief Write one of the IAP command values to be passed to firmware from bootloader. + * @param number: the index of the command value (0..2). + * @param value: value to be written. + */ void PIOS_IAP_WriteBootCmd(uint8_t number, uint32_t value) { - if(PIOS_IAP_CMD_COUNT < number) - { - PIOS_Assert(0); - } - else - { - PIOS_BKP_WriteRegister(pios_iap_cmd_list[number], value); - } + if (PIOS_IAP_CMD_COUNT < number) { + PIOS_Assert(0); + } else { + PIOS_BKP_WriteRegister(pios_iap_cmd_list[number], value); + } } #endif /* PIOS_INCLUDE_IAP */ diff --git a/flight/pios/common/pios_l3gd20.c b/flight/pios/common/pios_l3gd20.c index fb621fffd..0dfe32bee 100644 --- a/flight/pios/common/pios_l3gd20.c +++ b/flight/pios/common/pios_l3gd20.c @@ -13,19 +13,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,27 +37,27 @@ /* Global Variables */ enum pios_l3gd20_dev_magic { - PIOS_L3GD20_DEV_MAGIC = 0x9d39bced, + PIOS_L3GD20_DEV_MAGIC = 0x9d39bced, }; #define PIOS_L3GD20_MAX_DOWNSAMPLE 2 struct l3gd20_dev { - uint32_t spi_id; - uint32_t slave_num; - xQueueHandle queue; - const struct pios_l3gd20_cfg * cfg; - enum pios_l3gd20_filter bandwidth; - enum pios_l3gd20_range range; - enum pios_l3gd20_dev_magic magic; + uint32_t spi_id; + uint32_t slave_num; + xQueueHandle queue; + const struct pios_l3gd20_cfg *cfg; + enum pios_l3gd20_filter bandwidth; + enum pios_l3gd20_range range; + enum pios_l3gd20_dev_magic magic; }; -//! Global structure for this device device -static struct l3gd20_dev * dev; +// ! Global structure for this device device +static struct l3gd20_dev *dev; -//! Private functions -static struct l3gd20_dev * PIOS_L3GD20_alloc(void); -static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev * dev); -static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const * cfg); +// ! Private functions +static struct l3gd20_dev *PIOS_L3GD20_alloc(void); +static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev *dev); +static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const *cfg); static int32_t PIOS_L3GD20_SetReg(uint8_t address, uint8_t buffer); static int32_t PIOS_L3GD20_GetReg(uint8_t address); static int32_t PIOS_L3GD20_GetRegISR(uint8_t address, bool *woken); @@ -73,22 +73,24 @@ volatile bool l3gd20_configured = false; /** * @brief Allocate a new device */ -static struct l3gd20_dev * PIOS_L3GD20_alloc(void) +static struct l3gd20_dev *PIOS_L3GD20_alloc(void) { - struct l3gd20_dev * l3gd20_dev; + struct l3gd20_dev *l3gd20_dev; - l3gd20_dev = (struct l3gd20_dev *)pvPortMalloc(sizeof(*l3gd20_dev)); - if (!l3gd20_dev) return (NULL); + l3gd20_dev = (struct l3gd20_dev *)pvPortMalloc(sizeof(*l3gd20_dev)); + if (!l3gd20_dev) { + return NULL; + } - l3gd20_dev->magic = PIOS_L3GD20_DEV_MAGIC; + l3gd20_dev->magic = PIOS_L3GD20_DEV_MAGIC; - l3gd20_dev->queue = xQueueCreate(PIOS_L3GD20_MAX_DOWNSAMPLE, sizeof(struct pios_l3gd20_data)); - if(l3gd20_dev->queue == NULL) { - vPortFree(l3gd20_dev); - return NULL; - } + l3gd20_dev->queue = xQueueCreate(PIOS_L3GD20_MAX_DOWNSAMPLE, sizeof(struct pios_l3gd20_data)); + if (l3gd20_dev->queue == NULL) { + vPortFree(l3gd20_dev); + return NULL; + } - return(l3gd20_dev); + return l3gd20_dev; } /** @@ -97,13 +99,16 @@ static struct l3gd20_dev * PIOS_L3GD20_alloc(void) */ static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev *vdev) { - if (vdev == NULL) - return -1; - if (vdev->magic != PIOS_L3GD20_DEV_MAGIC) - return -2; - if (vdev->spi_id == 0) - return -3; - return 0; + if (vdev == NULL) { + return -1; + } + if (vdev->magic != PIOS_L3GD20_DEV_MAGIC) { + return -2; + } + if (vdev->spi_id == 0) { + return -3; + } + return 0; } /** @@ -111,52 +116,65 @@ static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev *vdev) * @return none */ #include -int32_t PIOS_L3GD20_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_l3gd20_cfg * cfg) +int32_t PIOS_L3GD20_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_l3gd20_cfg *cfg) { - dev = PIOS_L3GD20_alloc(); - if(dev == NULL) - return -1; + dev = PIOS_L3GD20_alloc(); + if (dev == NULL) { + return -1; + } - dev->spi_id = spi_id; - dev->slave_num = slave_num; - dev->cfg = cfg; + dev->spi_id = spi_id; + dev->slave_num = slave_num; + dev->cfg = cfg; - /* Configure the MPU6050 Sensor */ - PIOS_L3GD20_Config(cfg); + /* Configure the MPU6050 Sensor */ + PIOS_L3GD20_Config(cfg); - /* Set up EXTI */ - PIOS_EXTI_Init(cfg->exti_cfg); - - // An initial read is needed to get it running - struct pios_l3gd20_data data; - PIOS_L3GD20_ReadGyros(&data); + /* Set up EXTI */ + PIOS_EXTI_Init(cfg->exti_cfg); - return 0; + // An initial read is needed to get it running + struct pios_l3gd20_data data; + PIOS_L3GD20_ReadGyros(&data); + + return 0; } /** * @brief Initialize the L3GD20 3-axis gyro sensor * \return none * \param[in] PIOS_L3GD20_ConfigTypeDef struct to be used to configure sensor. -* -*/ -static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const * cfg) + * + */ +static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const *cfg) { - // This register enables the channels and sets the bandwidth - while(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG1, PIOS_L3GD20_CTRL1_FASTEST | - PIOS_L3GD20_CTRL1_PD | PIOS_L3GD20_CTRL1_ZEN | - PIOS_L3GD20_CTRL1_YEN | PIOS_L3GD20_CTRL1_XEN) != 0); - - // Disable the high pass filters - while(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG2, 0) != 0); - // Set int2 to go high on data ready - while(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG3, 0x08) != 0); - // Select SPI interface, 500 deg/s, endianness? - while(PIOS_L3GD20_SetRange(cfg->range) != 0); - // Enable FIFO, disable HPF - while(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG5, 0x40) != 0); - // Fifo stream mode - while(PIOS_L3GD20_SetReg(PIOS_L3GD20_FIFO_CTRL_REG, 0x40) != 0); + // This register enables the channels and sets the bandwidth + while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG1, PIOS_L3GD20_CTRL1_FASTEST | + PIOS_L3GD20_CTRL1_PD | PIOS_L3GD20_CTRL1_ZEN | + PIOS_L3GD20_CTRL1_YEN | PIOS_L3GD20_CTRL1_XEN) != 0) { + ; + } + + // Disable the high pass filters + while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG2, 0) != 0) { + ; + } + // Set int2 to go high on data ready + while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG3, 0x08) != 0) { + ; + } + // Select SPI interface, 500 deg/s, endianness? + while (PIOS_L3GD20_SetRange(cfg->range) != 0) { + ; + } + // Enable FIFO, disable HPF + while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG5, 0x40) != 0) { + ; + } + // Fifo stream mode + while (PIOS_L3GD20_SetReg(PIOS_L3GD20_FIFO_CTRL_REG, 0x40) != 0) { + ; + } } /** @@ -165,14 +183,16 @@ static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const * cfg) */ int32_t PIOS_L3GD20_SetRange(enum pios_l3gd20_range range) { - if(PIOS_L3GD20_Validate(dev) != 0) - return -1; + if (PIOS_L3GD20_Validate(dev) != 0) { + return -1; + } - dev->range = range; - if(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG4, dev->range) != 0) - return -2; - - return 0; + dev->range = range; + if (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG4, dev->range) != 0) { + return -2; + } + + return 0; } /** @@ -181,14 +201,16 @@ int32_t PIOS_L3GD20_SetRange(enum pios_l3gd20_range range) */ static int32_t PIOS_L3GD20_ClaimBus() { - if(PIOS_L3GD20_Validate(dev) != 0) - return -1; + if (PIOS_L3GD20_Validate(dev) != 0) { + return -1; + } - if(PIOS_SPI_ClaimBus(dev->spi_id) != 0) - return -2; + if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { + return -2; + } - PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0); - return 0; + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + return 0; } /** @@ -199,14 +221,14 @@ static int32_t PIOS_L3GD20_ClaimBus() */ static int32_t PIOS_L3GD20_ClaimBusISR(bool *woken) { - if(PIOS_L3GD20_Validate(dev) != 0) { - return -1; - } - if(PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { - return -2; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); - return 0; + if (PIOS_L3GD20_Validate(dev) != 0) { + return -1; + } + if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { + return -2; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + return 0; } /** @@ -215,11 +237,11 @@ static int32_t PIOS_L3GD20_ClaimBusISR(bool *woken) */ int32_t PIOS_L3GD20_ReleaseBus() { - if(PIOS_L3GD20_Validate(dev) != 0) { - return -1; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBus(dev->spi_id); + if (PIOS_L3GD20_Validate(dev) != 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + return PIOS_SPI_ReleaseBus(dev->spi_id); } /** @@ -230,11 +252,11 @@ int32_t PIOS_L3GD20_ReleaseBus() */ int32_t PIOS_L3GD20_ReleaseBusISR(bool *woken) { - if(PIOS_L3GD20_Validate(dev) != 0) { - return -1; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); + if (PIOS_L3GD20_Validate(dev) != 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); } /** @@ -244,16 +266,17 @@ int32_t PIOS_L3GD20_ReleaseBusISR(bool *woken) */ static int32_t PIOS_L3GD20_GetReg(uint8_t reg) { - uint8_t data; + uint8_t data; - if(PIOS_L3GD20_ClaimBus() != 0) - return -1; - - PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte - data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response - - PIOS_L3GD20_ReleaseBus(); - return data; + if (PIOS_L3GD20_ClaimBus() != 0) { + return -1; + } + + PIOS_SPI_TransferByte(dev->spi_id, (0x80 | reg)); // request byte + data = PIOS_SPI_TransferByte(dev->spi_id, 0); // receive response + + PIOS_L3GD20_ReleaseBus(); + return data; } /** @@ -265,15 +288,15 @@ static int32_t PIOS_L3GD20_GetReg(uint8_t reg) */ static int32_t PIOS_L3GD20_GetRegISR(uint8_t reg, bool *woken) { - uint8_t data; + uint8_t data; - if(PIOS_L3GD20_ClaimBusISR(woken) != 0) { - return -1; - } - PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte - data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response - PIOS_L3GD20_ReleaseBusISR(woken); - return data; + if (PIOS_L3GD20_ClaimBusISR(woken) != 0) { + return -1; + } + PIOS_SPI_TransferByte(dev->spi_id, (0x80 | reg)); // request byte + data = PIOS_SPI_TransferByte(dev->spi_id, 0); // receive response + PIOS_L3GD20_ReleaseBusISR(woken); + return data; } /** @@ -286,15 +309,16 @@ static int32_t PIOS_L3GD20_GetRegISR(uint8_t reg, bool *woken) */ static int32_t PIOS_L3GD20_SetReg(uint8_t reg, uint8_t data) { - if(PIOS_L3GD20_ClaimBus() != 0) - return -1; - - PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg); - PIOS_SPI_TransferByte(dev->spi_id, data); - - PIOS_L3GD20_ReleaseBus(); - - return 0; + if (PIOS_L3GD20_ClaimBus() != 0) { + return -1; + } + + PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg); + PIOS_SPI_TransferByte(dev->spi_id, data); + + PIOS_L3GD20_ReleaseBus(); + + return 0; } /** @@ -303,41 +327,44 @@ static int32_t PIOS_L3GD20_SetReg(uint8_t reg, uint8_t data) * \returns The number of samples remaining in the fifo */ uint32_t l3gd20_irq = 0; -int32_t PIOS_L3GD20_ReadGyros(struct pios_l3gd20_data * data) +int32_t PIOS_L3GD20_ReadGyros(struct pios_l3gd20_data *data) { - uint8_t buf[7] = {PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0}; - uint8_t rec[7]; - - if(PIOS_L3GD20_ClaimBus() != 0) - return -1; + uint8_t buf[7] = { PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0 }; + uint8_t rec[7]; - if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { - PIOS_L3GD20_ReleaseBus(); - data->gyro_x = 0; - data->gyro_y = 0; - data->gyro_z = 0; - data->temperature = 0; - return -2; - } - - PIOS_L3GD20_ReleaseBus(); - - memcpy((uint8_t *) &(data->gyro_x), &rec[1], 6); - data->temperature = PIOS_L3GD20_GetReg(PIOS_L3GD20_OUT_TEMP); - - return 0; + if (PIOS_L3GD20_ClaimBus() != 0) { + return -1; + } + + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { + PIOS_L3GD20_ReleaseBus(); + data->gyro_x = 0; + data->gyro_y = 0; + data->gyro_z = 0; + data->temperature = 0; + return -2; + } + + PIOS_L3GD20_ReleaseBus(); + + memcpy((uint8_t *)&(data->gyro_x), &rec[1], 6); + data->temperature = PIOS_L3GD20_GetReg(PIOS_L3GD20_OUT_TEMP); + + return 0; } /** * @brief Read the identification bytes from the MPU6050 sensor * \return ID read from MPU6050 or -1 if failure -*/ + */ int32_t PIOS_L3GD20_ReadID() { - int32_t l3gd20_id = PIOS_L3GD20_GetReg(PIOS_L3GD20_WHOAMI); - if(l3gd20_id < 0) - return -1; - return l3gd20_id; + int32_t l3gd20_id = PIOS_L3GD20_GetReg(PIOS_L3GD20_WHOAMI); + + if (l3gd20_id < 0) { + return -1; + } + return l3gd20_id; } /** @@ -346,26 +373,30 @@ int32_t PIOS_L3GD20_ReadID() */ xQueueHandle PIOS_L3GD20_GetQueue() { - if(PIOS_L3GD20_Validate(dev) != 0) - return (xQueueHandle) NULL; + if (PIOS_L3GD20_Validate(dev) != 0) { + return (xQueueHandle)NULL; + } - return dev->queue; + return dev->queue; } -float PIOS_L3GD20_GetScale() +float PIOS_L3GD20_GetScale() { - if(PIOS_L3GD20_Validate(dev) != 0) - return -1; + if (PIOS_L3GD20_Validate(dev) != 0) { + return -1; + } - switch (dev->range) { - case PIOS_L3GD20_SCALE_250_DEG: - return 0.00875f; - case PIOS_L3GD20_SCALE_500_DEG: - return 0.01750f; - case PIOS_L3GD20_SCALE_2000_DEG: - return 0.070f; - } - return 0; + switch (dev->range) { + case PIOS_L3GD20_SCALE_250_DEG: + return 0.00875f; + + case PIOS_L3GD20_SCALE_500_DEG: + return 0.01750f; + + case PIOS_L3GD20_SCALE_2000_DEG: + return 0.070f; + } + return 0; } /** @@ -375,50 +406,53 @@ float PIOS_L3GD20_GetScale() */ uint8_t PIOS_L3GD20_Test(void) { - int32_t l3gd20_id = PIOS_L3GD20_ReadID(); - if(l3gd20_id < 0) - return -1; + int32_t l3gd20_id = PIOS_L3GD20_ReadID(); - uint8_t id = l3gd20_id; - if(id == 0xD4) - return 0; + if (l3gd20_id < 0) { + return -1; + } - return -2; + uint8_t id = l3gd20_id; + if (id == 0xD4) { + return 0; + } + + return -2; } /** -* @brief EXTI IRQ Handler. Read all the data from onboard buffer + * @brief EXTI IRQ Handler. Read all the data from onboard buffer * @return a boolean to the EXTI IRQ Handler wrapper indicating if a * higher priority task is now eligible to run -*/ + */ bool PIOS_L3GD20_IRQHandler(void) { - bool woken = false; - struct pios_l3gd20_data data; - uint8_t buf[7] = {PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0}; - uint8_t rec[7]; + bool woken = false; + struct pios_l3gd20_data data; + uint8_t buf[7] = { PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0 }; + uint8_t rec[7]; - l3gd20_irq++; + l3gd20_irq++; - /* This code duplicates ReadGyros above but uses ClaimBusIsr */ - if (PIOS_L3GD20_ClaimBusISR(&woken) != 0) { - return woken; - } - - if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { - PIOS_L3GD20_ReleaseBusISR(&woken); - return woken; - } - - PIOS_L3GD20_ReleaseBusISR(&woken); - - memcpy((uint8_t *) &(data.gyro_x), &rec[1], 6); - data.temperature = PIOS_L3GD20_GetRegISR(PIOS_L3GD20_OUT_TEMP, &woken); - - signed portBASE_TYPE higherPriorityTaskWoken; - xQueueSendToBackFromISR(dev->queue, (void *) &data, &higherPriorityTaskWoken); - - return (woken || higherPriorityTaskWoken == pdTRUE); + /* This code duplicates ReadGyros above but uses ClaimBusIsr */ + if (PIOS_L3GD20_ClaimBusISR(&woken) != 0) { + return woken; + } + + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { + PIOS_L3GD20_ReleaseBusISR(&woken); + return woken; + } + + PIOS_L3GD20_ReleaseBusISR(&woken); + + memcpy((uint8_t *)&(data.gyro_x), &rec[1], 6); + data.temperature = PIOS_L3GD20_GetRegISR(PIOS_L3GD20_OUT_TEMP, &woken); + + signed portBASE_TYPE higherPriorityTaskWoken; + xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken); + + return woken || higherPriorityTaskWoken == pdTRUE; } #endif /* PIOS_INCLUDE_L3GD20 */ diff --git a/flight/pios/common/pios_mpu6000.c b/flight/pios/common/pios_mpu6000.c index acbc9d4c5..371068e0c 100644 --- a/flight/pios/common/pios_mpu6000.c +++ b/flight/pios/common/pios_mpu6000.c @@ -13,19 +13,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,29 +38,29 @@ /* Global Variables */ enum pios_mpu6000_dev_magic { - PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed, + PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed, }; #define PIOS_MPU6000_MAX_DOWNSAMPLE 2 struct mpu6000_dev { - uint32_t spi_id; - uint32_t slave_num; - xQueueHandle queue; - const struct pios_mpu6000_cfg * cfg; - enum pios_mpu6000_range gyro_range; - enum pios_mpu6000_accel_range accel_range; - enum pios_mpu6000_filter filter; - enum pios_mpu6000_dev_magic magic; + uint32_t spi_id; + uint32_t slave_num; + xQueueHandle queue; + const struct pios_mpu6000_cfg *cfg; + enum pios_mpu6000_range gyro_range; + enum pios_mpu6000_accel_range accel_range; + enum pios_mpu6000_filter filter; + enum pios_mpu6000_dev_magic magic; }; -//! Global structure for this device device -static struct mpu6000_dev * dev; +// ! Global structure for this device device +static struct mpu6000_dev *dev; volatile bool mpu6000_configured = false; -//! Private functions -static struct mpu6000_dev * PIOS_MPU6000_alloc(void); -static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev * dev); -static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg); +// ! Private functions +static struct mpu6000_dev *PIOS_MPU6000_alloc(void); +static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev); +static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg); static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer); static int32_t PIOS_MPU6000_GetReg(uint8_t address); @@ -69,22 +69,24 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t address); /** * @brief Allocate a new device */ -static struct mpu6000_dev * PIOS_MPU6000_alloc(void) +static struct mpu6000_dev *PIOS_MPU6000_alloc(void) { - struct mpu6000_dev * mpu6000_dev; - - mpu6000_dev = (struct mpu6000_dev *)pvPortMalloc(sizeof(*mpu6000_dev)); - if (!mpu6000_dev) return (NULL); - - mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC; - - mpu6000_dev->queue = xQueueCreate(PIOS_MPU6000_MAX_DOWNSAMPLE, sizeof(struct pios_mpu6000_data)); - if(mpu6000_dev->queue == NULL) { - vPortFree(mpu6000_dev); - return NULL; - } - - return(mpu6000_dev); + struct mpu6000_dev *mpu6000_dev; + + mpu6000_dev = (struct mpu6000_dev *)pvPortMalloc(sizeof(*mpu6000_dev)); + if (!mpu6000_dev) { + return NULL; + } + + mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC; + + mpu6000_dev->queue = xQueueCreate(PIOS_MPU6000_MAX_DOWNSAMPLE, sizeof(struct pios_mpu6000_data)); + if (mpu6000_dev->queue == NULL) { + vPortFree(mpu6000_dev); + return NULL; + } + + return mpu6000_dev; } /** @@ -93,37 +95,41 @@ static struct mpu6000_dev * PIOS_MPU6000_alloc(void) */ static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *vdev) { - if (vdev == NULL) - return -1; - if (vdev->magic != PIOS_MPU6000_DEV_MAGIC) - return -2; - if (vdev->spi_id == 0) - return -3; - return 0; + if (vdev == NULL) { + return -1; + } + if (vdev->magic != PIOS_MPU6000_DEV_MAGIC) { + return -2; + } + if (vdev->spi_id == 0) { + return -3; + } + return 0; } /** * @brief Initialize the MPU6000 3-axis gyro sensor. * @return 0 for success, -1 for failure */ -int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg * cfg) +int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *cfg) { - dev = PIOS_MPU6000_alloc(); - if(dev == NULL) - return -1; - - dev->spi_id = spi_id; - dev->slave_num = slave_num; - dev->cfg = cfg; + dev = PIOS_MPU6000_alloc(); + if (dev == NULL) { + return -1; + } - /* Configure the MPU6000 Sensor */ - PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256); - PIOS_MPU6000_Config(cfg); - PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16); + dev->spi_id = spi_id; + dev->slave_num = slave_num; + dev->cfg = cfg; - /* Set up EXTI line */ - PIOS_EXTI_Init(cfg->exti_cfg); - return 0; + /* Configure the MPU6000 Sensor */ + PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256); + PIOS_MPU6000_Config(cfg); + PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16); + + /* Set up EXTI line */ + PIOS_EXTI_Init(cfg->exti_cfg); + return 0; } /** @@ -132,99 +138,131 @@ int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios * \param[in] PIOS_MPU6000_ConfigTypeDef struct to be used to configure sensor. * */ -static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg) +static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg) { + PIOS_MPU6000_Test(); - PIOS_MPU6000_Test(); + // Reset chip + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, PIOS_MPU6000_PWRMGMT_IMU_RST) != 0) { + ; + } + PIOS_DELAY_WaitmS(50); - // Reset chip - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, PIOS_MPU6000_PWRMGMT_IMU_RST) != 0); - PIOS_DELAY_WaitmS(50); + // Reset chip and fifo + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, + PIOS_MPU6000_USERCTL_GYRO_RST | + PIOS_MPU6000_USERCTL_SIG_COND | + PIOS_MPU6000_USERCTL_FIFO_RST) != 0) { + ; + } - // Reset chip and fifo - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, - PIOS_MPU6000_USERCTL_GYRO_RST | - PIOS_MPU6000_USERCTL_SIG_COND | - PIOS_MPU6000_USERCTL_FIFO_RST) != 0); + // Wait for reset to finish + while (PIOS_MPU6000_GetReg(PIOS_MPU6000_USER_CTRL_REG) & + (PIOS_MPU6000_USERCTL_GYRO_RST | + PIOS_MPU6000_USERCTL_SIG_COND | + PIOS_MPU6000_USERCTL_FIFO_RST)) { + ; + } + PIOS_DELAY_WaitmS(10); + // Power management configuration + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) { + ; + } - // Wait for reset to finish - while (PIOS_MPU6000_GetReg(PIOS_MPU6000_USER_CTRL_REG) & - (PIOS_MPU6000_USERCTL_GYRO_RST | - PIOS_MPU6000_USERCTL_SIG_COND | - PIOS_MPU6000_USERCTL_FIFO_RST)); - PIOS_DELAY_WaitmS(10); - //Power management configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0); + // Interrupt configuration + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) { + ; + } - // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0); + // Interrupt configuration + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) { + ; + } - // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0); - - // FIFO storage + // FIFO storage #if defined(PIOS_MPU6000_ACCEL) - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store | PIOS_MPU6000_ACCEL_OUT) != 0); + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store | PIOS_MPU6000_ACCEL_OUT) != 0) { + ; + } #else - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0); + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0) { + ; + } #endif - PIOS_MPU6000_ConfigureRanges(cfg->gyro_range, cfg->accel_range, cfg->filter); - // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) ; - - // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) ; - - // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) ; + PIOS_MPU6000_ConfigureRanges(cfg->gyro_range, cfg->accel_range, cfg->filter); + // Interrupt configuration + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) { + ; + } - // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) ; - if((PIOS_MPU6000_GetReg(PIOS_MPU6000_INT_EN_REG)) != cfg->interrupt_en) - return; - - mpu6000_configured = true; + // Interrupt configuration + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) { + ; + } + + // Interrupt configuration + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) { + ; + } + + // Interrupt configuration + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) { + ; + } + if ((PIOS_MPU6000_GetReg(PIOS_MPU6000_INT_EN_REG)) != cfg->interrupt_en) { + return; + } + + mpu6000_configured = true; } /** * @brief Configures Gyro, accel and Filter ranges/setings * @return 0 if successful, -1 if device has not been initialized */ int32_t PIOS_MPU6000_ConfigureRanges( - enum pios_mpu6000_range gyroRange, - enum pios_mpu6000_accel_range accelRange, - enum pios_mpu6000_filter filterSetting - ) + enum pios_mpu6000_range gyroRange, + enum pios_mpu6000_accel_range accelRange, + enum pios_mpu6000_filter filterSetting) { - if(dev == NULL) - return -1; + if (dev == NULL) { + return -1; + } - // TODO: check that changing the SPI clock speed is safe - // to do here given that interrupts are enabled and the bus has - // not been claimed/is not claimed during this call - PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256); + // TODO: check that changing the SPI clock speed is safe + // to do here given that interrupts are enabled and the bus has + // not been claimed/is not claimed during this call + PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256); - // update filter settings - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0); - - // Sample rate divider, chosen upon digital filtering settings - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_SMPLRT_DIV_REG, - filterSetting == PIOS_MPU6000_LOWPASS_256_HZ ? - dev->cfg->Smpl_rate_div_no_dlp : dev->cfg->Smpl_rate_div_dlp) != 0); - - dev->filter = filterSetting; - - // Gyro range - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_GYRO_CFG_REG, gyroRange) != 0); - - dev->gyro_range = gyroRange; + // update filter settings + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0) { + ; + } + + // Sample rate divider, chosen upon digital filtering settings + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_SMPLRT_DIV_REG, + filterSetting == PIOS_MPU6000_LOWPASS_256_HZ ? + dev->cfg->Smpl_rate_div_no_dlp : dev->cfg->Smpl_rate_div_dlp) != 0) { + ; + } + + dev->filter = filterSetting; + + // Gyro range + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_GYRO_CFG_REG, gyroRange) != 0) { + ; + } + + dev->gyro_range = gyroRange; #if defined(PIOS_MPU6000_ACCEL) - // Set the accel range - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0); - - dev->accel_range = accelRange; + // Set the accel range + while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0) { + ; + } + + dev->accel_range = accelRange; #endif - PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16); - return 0; + PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16); + return 0; } /** @@ -233,14 +271,14 @@ int32_t PIOS_MPU6000_ConfigureRanges( */ static int32_t PIOS_MPU6000_ClaimBus() { - if(PIOS_MPU6000_Validate(dev) != 0) { - return -1; - } - if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { - return -2; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); - return 0; + if (PIOS_MPU6000_Validate(dev) != 0) { + return -1; + } + if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { + return -2; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + return 0; } /** @@ -251,14 +289,14 @@ static int32_t PIOS_MPU6000_ClaimBus() */ static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken) { - if (PIOS_MPU6000_Validate(dev) != 0) { - return -1; - } - if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { - return -2; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); - return 0; + if (PIOS_MPU6000_Validate(dev) != 0) { + return -1; + } + if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { + return -2; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + return 0; } /** @@ -267,11 +305,11 @@ static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken) */ static int32_t PIOS_MPU6000_ReleaseBus() { - if(PIOS_MPU6000_Validate(dev) != 0) { - return -1; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBus(dev->spi_id); + if (PIOS_MPU6000_Validate(dev) != 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + return PIOS_SPI_ReleaseBus(dev->spi_id); } /** @@ -282,11 +320,11 @@ static int32_t PIOS_MPU6000_ReleaseBus() */ static int32_t PIOS_MPU6000_ReleaseBusISR(bool *woken) { - if(PIOS_MPU6000_Validate(dev) != 0) { - return -1; - } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); + if (PIOS_MPU6000_Validate(dev) != 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); + return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); } /** @@ -296,17 +334,17 @@ static int32_t PIOS_MPU6000_ReleaseBusISR(bool *woken) */ static int32_t PIOS_MPU6000_GetReg(uint8_t reg) { - uint8_t data; - - if(PIOS_MPU6000_ClaimBus() != 0) { - return -1; - } - - PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte - data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response - - PIOS_MPU6000_ReleaseBus(); - return data; + uint8_t data; + + if (PIOS_MPU6000_ClaimBus() != 0) { + return -1; + } + + PIOS_SPI_TransferByte(dev->spi_id, (0x80 | reg)); // request byte + data = PIOS_SPI_TransferByte(dev->spi_id, 0); // receive response + + PIOS_MPU6000_ReleaseBus(); + return data; } /** @@ -319,23 +357,23 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg) */ static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data) { - if (PIOS_MPU6000_ClaimBus() != 0) { - return -1; - } - - if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) { - PIOS_MPU6000_ReleaseBus(); - return -2; - } - - if (PIOS_SPI_TransferByte(dev->spi_id, data) != 0) { - PIOS_MPU6000_ReleaseBus(); - return -3; - } + if (PIOS_MPU6000_ClaimBus() != 0) { + return -1; + } - PIOS_MPU6000_ReleaseBus(); - - return 0; + if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) { + PIOS_MPU6000_ReleaseBus(); + return -2; + } + + if (PIOS_SPI_TransferByte(dev->spi_id, data) != 0) { + PIOS_MPU6000_ReleaseBus(); + return -3; + } + + PIOS_MPU6000_ReleaseBus(); + + return 0; } /** @@ -343,39 +381,41 @@ static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data) * \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings * \returns 0 if succesful */ -int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data * data) +int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data) { - // THIS FUNCTION IS DEPRECATED AND DOES NOT PERFORM A ROTATION - uint8_t buf[7] = {PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0}; - uint8_t rec[7]; - - if (PIOS_MPU6000_ClaimBus() != 0) { - return -1; - } + // THIS FUNCTION IS DEPRECATED AND DOES NOT PERFORM A ROTATION + uint8_t buf[7] = { PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0 }; + uint8_t rec[7]; - if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { - return -2; - } + if (PIOS_MPU6000_ClaimBus() != 0) { + return -1; + } - PIOS_MPU6000_ReleaseBus(); - - data->gyro_x = rec[1] << 8 | rec[2]; - data->gyro_y = rec[3] << 8 | rec[4]; - data->gyro_z = rec[5] << 8 | rec[6]; - - return 0; + if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { + return -2; + } + + PIOS_MPU6000_ReleaseBus(); + + data->gyro_x = rec[1] << 8 | rec[2]; + data->gyro_y = rec[3] << 8 | rec[4]; + data->gyro_z = rec[5] << 8 | rec[6]; + + return 0; } /* * @brief Read the identification bytes from the MPU6000 sensor * \return ID read from MPU6000 or -1 if failure -*/ + */ int32_t PIOS_MPU6000_ReadID() { - int32_t mpu6000_id = PIOS_MPU6000_GetReg(PIOS_MPU6000_WHOAMI); - if(mpu6000_id < 0) - return -1; - return mpu6000_id; + int32_t mpu6000_id = PIOS_MPU6000_GetReg(PIOS_MPU6000_WHOAMI); + + if (mpu6000_id < 0) { + return -1; + } + return mpu6000_id; } /** @@ -384,41 +424,48 @@ int32_t PIOS_MPU6000_ReadID() */ xQueueHandle PIOS_MPU6000_GetQueue() { - if(PIOS_MPU6000_Validate(dev) != 0) - return (xQueueHandle) NULL; - - return dev->queue; + if (PIOS_MPU6000_Validate(dev) != 0) { + return (xQueueHandle)NULL; + } + + return dev->queue; } -float PIOS_MPU6000_GetScale() +float PIOS_MPU6000_GetScale() { - switch (dev->gyro_range) { - case PIOS_MPU6000_SCALE_250_DEG: - return 1.0f / 131.0f; - case PIOS_MPU6000_SCALE_500_DEG: - return 1.0f / 65.5f; - case PIOS_MPU6000_SCALE_1000_DEG: - return 1.0f / 32.8f; - case PIOS_MPU6000_SCALE_2000_DEG: - return 1.0f / 16.4f; - } - return 0; + switch (dev->gyro_range) { + case PIOS_MPU6000_SCALE_250_DEG: + return 1.0f / 131.0f; + + case PIOS_MPU6000_SCALE_500_DEG: + return 1.0f / 65.5f; + + case PIOS_MPU6000_SCALE_1000_DEG: + return 1.0f / 32.8f; + + case PIOS_MPU6000_SCALE_2000_DEG: + return 1.0f / 16.4f; + } + return 0; } float PIOS_MPU6000_GetAccelScale() { - switch (dev->accel_range) { - case PIOS_MPU6000_ACCEL_2G: - return GRAV / 16384.0f; - case PIOS_MPU6000_ACCEL_4G: - return GRAV / 8192.0f; - case PIOS_MPU6000_ACCEL_8G: - return GRAV / 4096.0f; - case PIOS_MPU6000_ACCEL_16G: - return GRAV / 2048.0f; - } - return 0; + switch (dev->accel_range) { + case PIOS_MPU6000_ACCEL_2G: + return GRAV / 16384.0f; + + case PIOS_MPU6000_ACCEL_4G: + return GRAV / 8192.0f; + + case PIOS_MPU6000_ACCEL_8G: + return GRAV / 4096.0f; + + case PIOS_MPU6000_ACCEL_16G: + return GRAV / 2048.0f; + } + return 0; } /** @@ -428,15 +475,18 @@ float PIOS_MPU6000_GetAccelScale() */ int32_t PIOS_MPU6000_Test(void) { - /* Verify that ID matches (MPU6000 ID is 0x69) */ - int32_t mpu6000_id = PIOS_MPU6000_ReadID(); - if(mpu6000_id < 0) - return -1; - - if(mpu6000_id != 0x68) - return -2; - - return 0; + /* Verify that ID matches (MPU6000 ID is 0x69) */ + int32_t mpu6000_id = PIOS_MPU6000_ReadID(); + + if (mpu6000_id < 0) { + return -1; + } + + if (mpu6000_id != 0x68) { + return -2; + } + + return 0; } /** @@ -447,31 +497,31 @@ int32_t PIOS_MPU6000_Test(void) */ static int32_t PIOS_MPU6000_FifoDepthISR(bool *woken) { - uint8_t mpu6000_send_buf[3] = {PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0}; - uint8_t mpu6000_rec_buf[3]; + uint8_t mpu6000_send_buf[3] = { PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0 }; + uint8_t mpu6000_rec_buf[3]; - if(PIOS_MPU6000_ClaimBusISR(woken) != 0) { - return -1; - } + if (PIOS_MPU6000_ClaimBusISR(woken) != 0) { + return -1; + } - if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { - PIOS_MPU6000_ReleaseBusISR(woken); - return -1; - } + if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { + PIOS_MPU6000_ReleaseBusISR(woken); + return -1; + } - PIOS_MPU6000_ReleaseBusISR(woken); - - return (mpu6000_rec_buf[1] << 8) | mpu6000_rec_buf[2]; + PIOS_MPU6000_ReleaseBusISR(woken); + + return (mpu6000_rec_buf[1] << 8) | mpu6000_rec_buf[2]; } /** -* @brief EXTI IRQ Handler. Read all the data from onboard buffer -* @return a boolean to the EXTI IRQ Handler wrapper indicating if a -* higher priority task is now eligible to run -*/ + * @brief EXTI IRQ Handler. Read all the data from onboard buffer + * @return a boolean to the EXTI IRQ Handler wrapper indicating if a + * higher priority task is now eligible to run + */ uint32_t mpu6000_irq = 0; int32_t mpu6000_count; -uint32_t mpu6000_fifo_backup = 0; +uint32_t mpu6000_fifo_backup = 0; uint8_t mpu6000_last_read_count = 0; uint32_t mpu6000_fails = 0; @@ -482,120 +532,122 @@ uint32_t mpu6000_transfer_size; bool PIOS_MPU6000_IRQHandler(void) { - bool woken = false; - static uint32_t timeval; - mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval); - timeval = PIOS_DELAY_GetRaw(); + bool woken = false; + static uint32_t timeval; - if (!mpu6000_configured) { - return false; - } + mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval); + timeval = PIOS_DELAY_GetRaw(); - mpu6000_count = PIOS_MPU6000_FifoDepthISR(&woken); - if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data)) { - return woken; - } + if (!mpu6000_configured) { + return false; + } - if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) { - return woken; - } + mpu6000_count = PIOS_MPU6000_FifoDepthISR(&woken); + if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data)) { + return woken; + } - static uint8_t mpu6000_send_buf[1 + sizeof(struct pios_mpu6000_data) ] = {PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0}; - static uint8_t mpu6000_rec_buf[1 + sizeof(struct pios_mpu6000_data) ]; + if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) { + return woken; + } - if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { - PIOS_MPU6000_ReleaseBusISR(&woken); - mpu6000_fails++; - return woken; - } + static uint8_t mpu6000_send_buf[1 + sizeof(struct pios_mpu6000_data)] = { PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0 }; + static uint8_t mpu6000_rec_buf[1 + sizeof(struct pios_mpu6000_data)]; - PIOS_MPU6000_ReleaseBusISR(&woken); + if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { + PIOS_MPU6000_ReleaseBusISR(&woken); + mpu6000_fails++; + return woken; + } - static struct pios_mpu6000_data data; + PIOS_MPU6000_ReleaseBusISR(&woken); - // In the case where extras samples backed up grabbed an extra - if (mpu6000_count >= (int32_t)(sizeof(data) * 2)) { - mpu6000_fifo_backup++; - if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) - return woken; + static struct pios_mpu6000_data data; - if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { - PIOS_MPU6000_ReleaseBusISR(&woken); - mpu6000_fails++; - return woken; - } + // In the case where extras samples backed up grabbed an extra + if (mpu6000_count >= (int32_t)(sizeof(data) * 2)) { + mpu6000_fifo_backup++; + if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) { + return woken; + } - PIOS_MPU6000_ReleaseBusISR(&woken); - } + if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { + PIOS_MPU6000_ReleaseBusISR(&woken); + mpu6000_fails++; + return woken; + } - // Rotate the sensor to OP convention. The datasheet defines X as towards the right - // and Y as forward. OP convention transposes this. Also the Z is defined negatively - // to our convention + PIOS_MPU6000_ReleaseBusISR(&woken); + } + + // Rotate the sensor to OP convention. The datasheet defines X as towards the right + // and Y as forward. OP convention transposes this. Also the Z is defined negatively + // to our convention #if defined(PIOS_MPU6000_ACCEL) - // Currently we only support rotations on top so switch X/Y accordingly - switch (dev->cfg->orientation) { - case PIOS_MPU6000_TOP_0DEG: - data.accel_y = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X - data.accel_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y - data.gyro_y = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X - data.gyro_x = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y - break; - case PIOS_MPU6000_TOP_90DEG: - // -1 to bring it back to -32768 +32767 range - data.accel_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y - data.accel_x = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X - data.gyro_y = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y - data.gyro_x = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X - break; - case PIOS_MPU6000_TOP_180DEG: - data.accel_y = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X - data.accel_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y - data.gyro_y = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X - data.gyro_x = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y - break; - case PIOS_MPU6000_TOP_270DEG: - data.accel_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y - data.accel_x = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X - data.gyro_y = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y - data.gyro_x = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X - break; - } - data.gyro_z = -1 - (mpu6000_rec_buf[13] << 8 | mpu6000_rec_buf[14]); - data.accel_z = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); - data.temperature = mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]; -#else - data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; - data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; - switch (dev->cfg->orientation) { - case PIOS_MPU6000_TOP_0DEG: - data.gyro_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; - data.gyro_x = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; - break; - case PIOS_MPU6000_TOP_90DEG: - data.gyro_y = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); // chip Y - data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip X - break; - case PIOS_MPU6000_TOP_180DEG: - data.gyro_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); - data.gyro_x = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); - break; - case PIOS_MPU6000_TOP_270DEG: - data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; // chip Y - data.gyro_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip X - break; - } - data.gyro_z = -1 - (mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]); - data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; -#endif + // Currently we only support rotations on top so switch X/Y accordingly + switch (dev->cfg->orientation) { + case PIOS_MPU6000_TOP_0DEG: + data.accel_y = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X + data.accel_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y + data.gyro_y = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X + data.gyro_x = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y + break; + case PIOS_MPU6000_TOP_90DEG: + // -1 to bring it back to -32768 +32767 range + data.accel_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y + data.accel_x = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X + data.gyro_y = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y + data.gyro_x = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X + break; + case PIOS_MPU6000_TOP_180DEG: + data.accel_y = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X + data.accel_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y + data.gyro_y = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X + data.gyro_x = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y + break; + case PIOS_MPU6000_TOP_270DEG: + data.accel_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y + data.accel_x = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X + data.gyro_y = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y + data.gyro_x = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X + break; + } + data.gyro_z = -1 - (mpu6000_rec_buf[13] << 8 | mpu6000_rec_buf[14]); + data.accel_z = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); + data.temperature = mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]; +#else /* if defined(PIOS_MPU6000_ACCEL) */ + data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; + data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; + switch (dev->cfg->orientation) { + case PIOS_MPU6000_TOP_0DEG: + data.gyro_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; + data.gyro_x = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; + break; + case PIOS_MPU6000_TOP_90DEG: + data.gyro_y = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); // chip Y + data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip X + break; + case PIOS_MPU6000_TOP_180DEG: + data.gyro_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); + data.gyro_x = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); + break; + case PIOS_MPU6000_TOP_270DEG: + data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; // chip Y + data.gyro_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip X + break; + } + data.gyro_z = -1 - (mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]); + data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; +#endif /* if defined(PIOS_MPU6000_ACCEL) */ - signed portBASE_TYPE higherPriorityTaskWoken; - xQueueSendToBackFromISR(dev->queue, (void *) &data, &higherPriorityTaskWoken); + signed portBASE_TYPE higherPriorityTaskWoken; + xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken); - mpu6000_irq++; + mpu6000_irq++; - mpu6000_time_us = PIOS_DELAY_DiffuS(timeval); + mpu6000_time_us = PIOS_DELAY_DiffuS(timeval); - return (woken || higherPriorityTaskWoken == pdTRUE); + return woken || higherPriorityTaskWoken == pdTRUE; } #endif /* PIOS_INCLUDE_MPU6000 */ diff --git a/flight/pios/common/pios_mpxv.c b/flight/pios/common/pios_mpxv.c index eae443852..4e6e3ce82 100644 --- a/flight/pios/common/pios_mpxv.c +++ b/flight/pios/common/pios_mpxv.c @@ -3,8 +3,8 @@ * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ * @addtogroup PIOS_MPXV Functions - * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV*. - * This is a differential sensor, so the value returned is first converted into + * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV*. + * This is a differential sensor, so the value returned is first converted into * calibrated airspeed, using http://en.wikipedia.org/wiki/Calibrated_airspeed * @{ * @@ -14,19 +14,19 @@ * @see The GNU Public License (GPL) Version 3 * ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,9 @@ #ifdef PIOS_INCLUDE_MPXV -#define A0 340.27f //speed of sound at standard sea level in [m/s] -#define P0 101.325f //static air pressure at standard sea level in kPa -#define POWER (2.0f/7.0f) +#define A0 340.27f // speed of sound at standard sea level in [m/s] +#define P0 101.325f // static air pressure at standard sea level in kPa +#define POWER (2.0f / 7.0f) #include "pios_mpxv.h" @@ -45,15 +45,17 @@ */ uint16_t PIOS_MPXV_Measure(PIOS_MPXV_descriptor *desc) { - if (desc) - return PIOS_ADC_PinGet(desc->airspeedADCPin); + if (desc) { + return PIOS_ADC_PinGet(desc->airspeedADCPin); + } return 0; } /* - *Returns zeroPoint so that the user can inspect the calibration vs. the sensor value + * Returns zeroPoint so that the user can inspect the calibration vs. the sensor value */ -uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc,uint16_t measurement) { +uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc, uint16_t measurement) +{ desc->calibrationSum += measurement; desc->calibrationCount++; desc->zeroPoint = (uint16_t)(((float)desc->calibrationSum) / desc->calibrationCount); @@ -61,27 +63,27 @@ uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc,uint16_t measurement) { } /* - * Reads the airspeed and returns CAS (calibrated airspeed) in the case of success. + * Reads the airspeed and returns CAS (calibrated airspeed) in the case of success. * In the case of a failed read, returns -1. */ -float PIOS_MPXV_CalcAirspeed(PIOS_MPXV_descriptor *desc,uint16_t measurement) +float PIOS_MPXV_CalcAirspeed(PIOS_MPXV_descriptor *desc, uint16_t measurement) { - //Calculate dynamic pressure, as per docs - float Qc = 3.3f/4096.0f * (float)(measurement - desc->zeroPoint); + // Calculate dynamic pressure, as per docs + float Qc = 3.3f / 4096.0f * (float)(measurement - desc->zeroPoint); - //Saturate Qc on the lower bound, in order to make sure we don't have negative airspeeds. No need + // Saturate Qc on the lower bound, in order to make sure we don't have negative airspeeds. No need // to saturate on the upper bound, we'll handle that later with calibratedAirspeed. if (Qc < 0) { - Qc=0; + Qc = 0; } - //Compute calibrated airspeed, as per http://en.wikipedia.org/wiki/Calibrated_airspeed - float calibratedAirspeed = A0 * sqrtf( 5.0f * (powf(Qc / P0 + 1.0f, POWER) - 1.0f)); + // Compute calibrated airspeed, as per http://en.wikipedia.org/wiki/Calibrated_airspeed + float calibratedAirspeed = A0 * sqrtf(5.0f * (powf(Qc / P0 + 1.0f, POWER) - 1.0f)); - //Upper bound airspeed. No need to lower bound it, that comes from Qc - //in [m/s] + // Upper bound airspeed. No need to lower bound it, that comes from Qc + // in [m/s] if (calibratedAirspeed > desc->maxSpeed) { - calibratedAirspeed=desc->maxSpeed; + calibratedAirspeed = desc->maxSpeed; } return calibratedAirspeed; diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c index 7e4358b35..24ec4c006 100644 --- a/flight/pios/common/pios_ms5611.c +++ b/flight/pios/common/pios_ms5611.c @@ -6,25 +6,25 @@ * @brief Hardware functions to deal with the altitude pressure sensor * @{ * - * @file pios_ms5611.c + * @file pios_ms5611.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief MS5611 Pressure Sensor Routines * @see The GNU Public License (GPL) Version 3 * ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -49,12 +49,12 @@ static uint32_t RawPressure; static int64_t Pressure; static int64_t Temperature; -static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t * buffer, uint8_t len); +static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len); static int32_t PIOS_MS5611_WriteCommand(uint8_t command); // Move into proper driver structure with cfg stored static uint32_t oversampling; -static const struct pios_ms5611_cfg * dev_cfg; +static const struct pios_ms5611_cfg *dev_cfg; static int32_t i2c_id; @@ -62,107 +62,117 @@ static int32_t i2c_id; * Initialise the MS5611 sensor */ int32_t ms5611_read_flag; -void PIOS_MS5611_Init(const struct pios_ms5611_cfg * cfg, int32_t i2c_device) +void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device) { - i2c_id = i2c_device; + i2c_id = i2c_device; - oversampling = cfg->oversampling; - dev_cfg = cfg; // Store cfg before enabling interrupt + oversampling = cfg->oversampling; + dev_cfg = cfg; // Store cfg before enabling interrupt - PIOS_MS5611_WriteCommand(MS5611_RESET); - PIOS_DELAY_WaitmS(20); + PIOS_MS5611_WriteCommand(MS5611_RESET); + PIOS_DELAY_WaitmS(20); - uint8_t data[2]; + uint8_t data[2]; - /* Calibration parameters */ - for (int i = 0; i < 6; i++) { - PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2); - CalibData.C[i] = (data[0] << 8) | data[1]; - } + /* Calibration parameters */ + for (int i = 0; i < 6; i++) { + PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2); + CalibData.C[i] = (data[0] << 8) | data[1]; + } } /** -* Start the ADC conversion -* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR -* \return 0 for success, -1 for failure (conversion completed and not read) -*/ + * Start the ADC conversion + * \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR + * \return 0 for success, -1 for failure (conversion completed and not read) + */ int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type) { - /* Start the conversion */ - if (Type == TemperatureConv) { - while (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) - continue; - } else if (Type == PressureConv) { - while (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) - continue; - } + /* Start the conversion */ + if (Type == TemperatureConv) { + while (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) { + continue; + } + } else if (Type == PressureConv) { + while (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) { + continue; + } + } - CurrentRead = Type; - - return 0; + CurrentRead = Type; + + return 0; } /** * @brief Return the delay for the current osr */ -int32_t PIOS_MS5611_GetDelay() { - switch(oversampling) { - case MS5611_OSR_256: - return 2; - case MS5611_OSR_512: - return 2; - case MS5611_OSR_1024: - return 3; - case MS5611_OSR_2048: - return 5; - case MS5611_OSR_4096: - return 10; - default: - break; - } - return 10; +int32_t PIOS_MS5611_GetDelay() +{ + switch (oversampling) { + case MS5611_OSR_256: + return 2; + + case MS5611_OSR_512: + return 2; + + case MS5611_OSR_1024: + return 3; + + case MS5611_OSR_2048: + return 5; + + case MS5611_OSR_4096: + return 10; + + default: + break; + } + return 10; } /** -* Read the ADC conversion value (once ADC conversion has completed) -* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR -* \return 0 if successfully read the ADC, -1 if failed -*/ + * Read the ADC conversion value (once ADC conversion has completed) + * \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR + * \return 0 if successfully read the ADC, -1 if failed + */ int32_t PIOS_MS5611_ReadADC(void) { - uint8_t Data[3]; - Data[0] = 0; - Data[1] = 0; - Data[2] = 0; - - static int64_t deltaTemp; + uint8_t Data[3]; - /* Read and store the 16bit result */ - if (CurrentRead == TemperatureConv) { - /* Read the temperature conversion */ - if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) - return -1; + Data[0] = 0; + Data[1] = 0; + Data[2] = 0; - RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2]; - - deltaTemp = ((int32_t) RawTemperature) - (CalibData.C[4] << 8); - Temperature = 2000l + ((deltaTemp * CalibData.C[5]) >> 23); + static int64_t deltaTemp; - } else { - int64_t Offset; - int64_t Sens; + /* Read and store the 16bit result */ + if (CurrentRead == TemperatureConv) { + /* Read the temperature conversion */ + if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { + return -1; + } - /* Read the pressure conversion */ - if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) - return -1; - RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); - - Offset = (((int64_t) CalibData.C[1]) << 16) + ((((int64_t) CalibData.C[3]) * deltaTemp) >> 7); - Sens = ((int64_t) CalibData.C[0]) << 15; - Sens = Sens + ((((int64_t) CalibData.C[2]) * deltaTemp) >> 8); - - Pressure = (((((int64_t) RawPressure) * Sens) >> 21) - Offset) >> 15; - } - return 0; + RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2]; + + deltaTemp = ((int32_t)RawTemperature) - (CalibData.C[4] << 8); + Temperature = 2000l + ((deltaTemp * CalibData.C[5]) >> 23); + } else { + int64_t Offset; + int64_t Sens; + + /* Read the pressure conversion */ + if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { + return -1; + } + RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); + + Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7); + Sens = ((int64_t)CalibData.C[0]) << 15; + Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8); + + Pressure = (((((int64_t)RawPressure) * Sens) >> 21) - Offset) >> 15; + } + return 0; } /** @@ -170,7 +180,7 @@ int32_t PIOS_MS5611_ReadADC(void) */ float PIOS_MS5611_GetTemperature(void) { - return ((float) Temperature) / 100.0f; + return ((float)Temperature) / 100.0f; } /** @@ -178,88 +188,89 @@ float PIOS_MS5611_GetTemperature(void) */ float PIOS_MS5611_GetPressure(void) { - return ((float) Pressure) / 1000.0f; + return ((float)Pressure) / 1000.0f; } /** -* Reads one or more bytes into a buffer -* \param[in] the command indicating the address to read -* \param[out] buffer destination buffer -* \param[in] len number of bytes which should be read -* \return 0 if operation was successful -* \return -1 if error during I2C transfer -*/ -int32_t PIOS_MS5611_Read(uint8_t address, uint8_t * buffer, uint8_t len) + * Reads one or more bytes into a buffer + * \param[in] the command indicating the address to read + * \param[out] buffer destination buffer + * \param[in] len number of bytes which should be read + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + */ +int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len) { + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = MS5611_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = 1, + .buf = &address, + } + , + { + .info = __func__, + .addr = MS5611_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = MS5611_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = 1, - .buf = &address, - } - , - { - .info = __func__, - .addr = MS5611_I2C_ADDR, - .rw = PIOS_I2C_TXN_READ, - .len = len, - .buf = buffer, - } - }; - - return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); } /** -* Writes one or more bytes to the MS5611 -* \param[in] address Register address -* \param[in] buffer source buffer -* \return 0 if operation was successful -* \return -1 if error during I2C transfer -*/ + * Writes one or more bytes to the MS5611 + * \param[in] address Register address + * \param[in] buffer source buffer + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + */ int32_t PIOS_MS5611_WriteCommand(uint8_t command) { - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = MS5611_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = 1, - .buf = &command, - } - , - }; + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = MS5611_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = 1, + .buf = &command, + } + , + }; - return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); } /** -* @brief Run self-test operation. -* \return 0 if self-test succeed, -1 if failed -*/ + * @brief Run self-test operation. + * \return 0 if self-test succeed, -1 if failed + */ int32_t PIOS_MS5611_Test() { - // TODO: Is there a better way to test this than just checking that pressure/temperature has changed? - int32_t cur_value = 0; + // TODO: Is there a better way to test this than just checking that pressure/temperature has changed? + int32_t cur_value = 0; - cur_value = Temperature; - PIOS_MS5611_StartADC(TemperatureConv); - PIOS_DELAY_WaitmS(5); - PIOS_MS5611_ReadADC(); - if (cur_value == Temperature) - return -1; + cur_value = Temperature; + PIOS_MS5611_StartADC(TemperatureConv); + PIOS_DELAY_WaitmS(5); + PIOS_MS5611_ReadADC(); + if (cur_value == Temperature) { + return -1; + } - cur_value=Pressure; - PIOS_MS5611_StartADC(PressureConv); - PIOS_DELAY_WaitmS(26); - PIOS_MS5611_ReadADC(); - if (cur_value == Pressure) - return -1; - - return 0; + cur_value = Pressure; + PIOS_MS5611_StartADC(PressureConv); + PIOS_DELAY_WaitmS(26); + PIOS_MS5611_ReadADC(); + if (cur_value == Pressure) { + return -1; + } + + return 0; } #endif /* PIOS_INCLUDE_MS5611 */ diff --git a/flight/pios/common/pios_rcvr.c b/flight/pios/common/pios_rcvr.c index 4533d874b..5e2e856da 100644 --- a/flight/pios/common/pios_rcvr.c +++ b/flight/pios/common/pios_rcvr.c @@ -5,74 +5,78 @@ #include enum pios_rcvr_dev_magic { - PIOS_RCVR_DEV_MAGIC = 0x99aabbcc, + PIOS_RCVR_DEV_MAGIC = 0x99aabbcc, }; struct pios_rcvr_dev { - enum pios_rcvr_dev_magic magic; - uint32_t lower_id; - const struct pios_rcvr_driver * driver; + enum pios_rcvr_dev_magic magic; + uint32_t lower_id; + const struct pios_rcvr_driver *driver; }; -static bool PIOS_RCVR_validate(struct pios_rcvr_dev * rcvr_dev) +static bool PIOS_RCVR_validate(struct pios_rcvr_dev *rcvr_dev) { - return (rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC); + return rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) +static struct pios_rcvr_dev *PIOS_RCVR_alloc(void) { - struct pios_rcvr_dev * rcvr_dev; + struct pios_rcvr_dev *rcvr_dev; - rcvr_dev = (struct pios_rcvr_dev *)pvPortMalloc(sizeof(*rcvr_dev)); - if (!rcvr_dev) return (NULL); + rcvr_dev = (struct pios_rcvr_dev *)pvPortMalloc(sizeof(*rcvr_dev)); + if (!rcvr_dev) { + return NULL; + } - rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; - return(rcvr_dev); + rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; + return rcvr_dev; } #else static struct pios_rcvr_dev pios_rcvr_devs[PIOS_RCVR_MAX_DEVS]; static uint8_t pios_rcvr_num_devs; -static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) +static struct pios_rcvr_dev *PIOS_RCVR_alloc(void) { - struct pios_rcvr_dev * rcvr_dev; + struct pios_rcvr_dev *rcvr_dev; - if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) { - return (NULL); - } + if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) { + return NULL; + } - rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++]; - rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; + rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++]; + rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; - return (rcvr_dev); + return rcvr_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /** - * Initialises RCVR layer - * \param[out] handle - * \param[in] driver - * \param[in] id - * \return < 0 if initialisation failed - */ -int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, uint32_t lower_id) + * Initialises RCVR layer + * \param[out] handle + * \param[in] driver + * \param[in] id + * \return < 0 if initialisation failed + */ +int32_t PIOS_RCVR_Init(uint32_t *rcvr_id, const struct pios_rcvr_driver *driver, uint32_t lower_id) { - PIOS_DEBUG_Assert(rcvr_id); - PIOS_DEBUG_Assert(driver); + PIOS_DEBUG_Assert(rcvr_id); + PIOS_DEBUG_Assert(driver); - struct pios_rcvr_dev * rcvr_dev; + struct pios_rcvr_dev *rcvr_dev; - rcvr_dev = (struct pios_rcvr_dev *) PIOS_RCVR_alloc(); - if (!rcvr_dev) goto out_fail; + rcvr_dev = (struct pios_rcvr_dev *)PIOS_RCVR_alloc(); + if (!rcvr_dev) { + goto out_fail; + } - rcvr_dev->driver = driver; - rcvr_dev->lower_id = lower_id; + rcvr_dev->driver = driver; + rcvr_dev->lower_id = lower_id; - *rcvr_id = (uint32_t)rcvr_dev; - return(0); + *rcvr_id = (uint32_t)rcvr_dev; + return 0; out_fail: - return(-1); + return -1; } /** @@ -86,25 +90,27 @@ out_fail: */ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) { - // Publicly facing API uses channel 1 for first channel - if(channel == 0) - return PIOS_RCVR_INVALID; - else - channel--; + // Publicly facing API uses channel 1 for first channel + if (channel == 0) { + return PIOS_RCVR_INVALID; + } else { + channel--; + } - if (rcvr_id == 0) - return PIOS_RCVR_NODRIVER; + if (rcvr_id == 0) { + return PIOS_RCVR_NODRIVER; + } - struct pios_rcvr_dev * rcvr_dev = (struct pios_rcvr_dev *)rcvr_id; + struct pios_rcvr_dev *rcvr_dev = (struct pios_rcvr_dev *)rcvr_id; - if (!PIOS_RCVR_validate(rcvr_dev)) { - /* Undefined RCVR port for this board (see pios_board.c) */ - PIOS_Assert(0); - } + if (!PIOS_RCVR_validate(rcvr_dev)) { + /* Undefined RCVR port for this board (see pios_board.c) */ + PIOS_Assert(0); + } - PIOS_DEBUG_Assert(rcvr_dev->driver->read); + PIOS_DEBUG_Assert(rcvr_dev->driver->read); - return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); + return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); } #endif /* PIOS_INCLUDE_RCVR */ diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index a3542d946..8282ce9d7 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -1,30 +1,30 @@ /** -****************************************************************************** -* @addtogroup PIOS PIOS Core hardware abstraction layer -* @{ -* @addtogroup PIOS_RFM22B Radio Functions -* @brief PIOS interface for for the RFM22B radio -* @{ -* -* @file pios_rfm22b.c -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief Implements a driver the the RFM22B driver -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RFM22B Radio Functions + * @brief PIOS interface for for the RFM22B radio + * @{ + * + * @file pios_rfm22b.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Implements a driver the the RFM22B driver + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -60,55 +60,55 @@ #include /* Local Defines */ -#define STACK_SIZE_BYTES 200 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) -#define ISR_TIMEOUT 2 // ms -#define EVENT_QUEUE_SIZE 5 -#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600 -#define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_0 -#define RFM22B_LINK_QUALITY_THRESHOLD 20 +#define STACK_SIZE_BYTES 200 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) +#define ISR_TIMEOUT 2 // ms +#define EVENT_QUEUE_SIZE 5 +#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600 +#define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_0 +#define RFM22B_LINK_QUALITY_THRESHOLD 20 #define RFM22B_NOMINAL_CARRIER_FREQUENCY 430000000 -#define RFM22B_MAXIMUM_FREQUENCY 440000000 -#define RFM22B_DEFAULT_FREQUENCY 433000000 -#define RFM22B_FREQUENCY_HOP_STEP_SIZE 75000 +#define RFM22B_MAXIMUM_FREQUENCY 440000000 +#define RFM22B_DEFAULT_FREQUENCY 433000000 +#define RFM22B_FREQUENCY_HOP_STEP_SIZE 75000 // The maximum amount of time since the last message received to consider the connection broken. -#define DISCONNECT_TIMEOUT_MS 1000 // ms +#define DISCONNECT_TIMEOUT_MS 1000 // ms // The maximum amount of time without activity before initiating a reset. -#define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms +#define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms // The time between updates for sending stats the radio link. -#define RADIOSTATS_UPDATE_PERIOD_MS 250 +#define RADIOSTATS_UPDATE_PERIOD_MS 250 // The number of stats updates that a modem can miss before it's considered disconnected -#define MAX_RADIOSTATS_MISS_COUNT 3 +#define MAX_RADIOSTATS_MISS_COUNT 3 // The time between PPM updates -#define PPM_UPDATE_PERIOD_MS 20 +#define PPM_UPDATE_PERIOD_MS 20 // this is too adjust the RF module so that it is on frequency -#define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default +#define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default -#define TX_PREAMBLE_NIBBLES 12 // 7 to 511 (number of nibbles) -#define RX_PREAMBLE_NIBBLES 6 // 5 to 31 (number of nibbles) +#define TX_PREAMBLE_NIBBLES 12 // 7 to 511 (number of nibbles) +#define RX_PREAMBLE_NIBBLES 6 // 5 to 31 (number of nibbles) // the size of the rf modules internal FIFO buffers -#define FIFO_SIZE 64 +#define FIFO_SIZE 64 -#define TX_FIFO_HI_WATERMARK 62 // 0-63 -#define TX_FIFO_LO_WATERMARK 32 // 0-63 +#define TX_FIFO_HI_WATERMARK 62 // 0-63 +#define TX_FIFO_LO_WATERMARK 32 // 0-63 -#define RX_FIFO_HI_WATERMARK 32 // 0-63 +#define RX_FIFO_HI_WATERMARK 32 // 0-63 // preamble byte (preceeds SYNC_BYTE's) -#define PREAMBLE_BYTE 0x55 +#define PREAMBLE_BYTE 0x55 // RF sync bytes (32-bit in all) -#define SYNC_BYTE_1 0x2D -#define SYNC_BYTE_2 0xD4 -#define SYNC_BYTE_3 0x4B -#define SYNC_BYTE_4 0x59 +#define SYNC_BYTE_1 0x2D +#define SYNC_BYTE_2 0xD4 +#define SYNC_BYTE_3 0x4B +#define SYNC_BYTE_4 0x59 #ifndef RX_LED_ON #define RX_LED_ON @@ -124,40 +124,42 @@ /* Local type definitions */ struct pios_rfm22b_transition { - enum pios_radio_event (*entry_fn) (struct pios_rfm22b_dev *rfm22b_dev); + enum pios_radio_event (*entry_fn)(struct pios_rfm22b_dev *rfm22b_dev); enum pios_radio_state next_state[RADIO_EVENT_NUM_EVENTS]; }; // Must ensure these prefilled arrays match the define sizes static const uint8_t FULL_PREAMBLE[FIFO_SIZE] = { - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE}; // 64 bytes -static const uint8_t HEADER[(TX_PREAMBLE_NIBBLES + 1)/2 + 2] = {PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,PREAMBLE_BYTE, PREAMBLE_BYTE, SYNC_BYTE_1, SYNC_BYTE_2}; + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, + PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE +}; // 64 bytes +static const uint8_t HEADER[(TX_PREAMBLE_NIBBLES + 1) / 2 + 2] = { PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, SYNC_BYTE_1, SYNC_BYTE_2 }; static const uint8_t OUT_FF[64] = { - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}; + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF +}; /* Local function forwared declarations */ static void pios_rfm22_task(void *parameters); static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev); -static void pios_rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening); +static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening); static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev); static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event, bool inISR); static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev); @@ -209,221 +211,219 @@ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr); /* The state transition table */ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_STATES] = { - // Initialization thread - [RADIO_STATE_UNINITIALIZED] = { - .entry_fn = 0, - .next_state = { + [RADIO_STATE_UNINITIALIZED] = { + .entry_fn = 0, + .next_state = { [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, }, }, - [RADIO_STATE_INITIALIZING] = { - .entry_fn = rfm22_init, - .next_state = { + [RADIO_STATE_INITIALIZING] = { + .entry_fn = rfm22_init, + .next_state = { [RADIO_EVENT_INITIALIZED] = RADIO_STATE_RX_MODE, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, [RADIO_STATE_REQUESTING_CONNECTION] = { - .entry_fn = rfm22_requestConnection, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + .entry_fn = rfm22_requestConnection, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR }, }, - [RADIO_STATE_ACCEPTING_CONNECTION] = { - .entry_fn = rfm22_acceptConnection, - .next_state = { - [RADIO_EVENT_DEFAULT] = RADIO_STATE_SENDING_ACK, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_ACCEPTING_CONNECTION] = { + .entry_fn = rfm22_acceptConnection, + .next_state = { + [RADIO_EVENT_DEFAULT] = RADIO_STATE_SENDING_ACK, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_RX_MODE] = { - .entry_fn = radio_setRxMode, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK, + [RADIO_STATE_RX_MODE] = { + .entry_fn = radio_setRxMode, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK, [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA, - [RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_RX_DATA] = { - .entry_fn = radio_rxData, - .next_state = { - [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA, - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_REQUEST_CONNECTION] = RADIO_STATE_REQUESTING_CONNECTION, - [RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK, - [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_SENDING_ACK, + [RADIO_STATE_RX_DATA] = { + .entry_fn = radio_rxData, + .next_state = { + [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA, + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_REQUEST_CONNECTION] = RADIO_STATE_REQUESTING_CONNECTION, + [RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK, + [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_SENDING_ACK, [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_STATUS_RECEIVED] = RADIO_STATE_RECEIVING_STATUS, + [RADIO_EVENT_STATUS_RECEIVED] = RADIO_STATE_RECEIVING_STATUS, [RADIO_EVENT_CONNECTION_REQUESTED] = RADIO_STATE_ACCEPTING_CONNECTION, - [RADIO_EVENT_PACKET_ACKED] = RADIO_STATE_RECEIVING_ACK, - [RADIO_EVENT_PACKET_NACKED] = RADIO_STATE_RECEIVING_NACK, - [RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_PACKET_ACKED] = RADIO_STATE_RECEIVING_ACK, + [RADIO_EVENT_PACKET_NACKED] = RADIO_STATE_RECEIVING_NACK, + [RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_RECEIVING_ACK] = { - .entry_fn = rfm22_receiveAck, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_RECEIVING_ACK] = { + .entry_fn = rfm22_receiveAck, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_RECEIVING_NACK] = { - .entry_fn = rfm22_receiveNack, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_RECEIVING_NACK] = { + .entry_fn = rfm22_receiveNack, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_RECEIVING_STATUS] = { - .entry_fn = rfm22_receiveStatus, - .next_state = { + [RADIO_STATE_RECEIVING_STATUS] = { + .entry_fn = rfm22_receiveStatus, + .next_state = { [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_TX_START] = { - .entry_fn = radio_txStart, - .next_state = { + [RADIO_STATE_TX_START] = { + .entry_fn = radio_txStart, + .next_state = { [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA, - [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_TX_DATA] = { - .entry_fn = radio_txData, - .next_state = { + [RADIO_STATE_TX_DATA] = { + .entry_fn = radio_txData, + .next_state = { [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA, - [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_FAILURE] = RADIO_STATE_TX_FAILURE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, + [RADIO_EVENT_FAILURE] = RADIO_STATE_TX_FAILURE, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_TX_FAILURE] = { - .entry_fn = rfm22_txFailure, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_TX_FAILURE] = { + .entry_fn = rfm22_txFailure, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_SENDING_ACK] = { - .entry_fn = rfm22_sendAck, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_SENDING_ACK] = { + .entry_fn = rfm22_sendAck, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_SENDING_NACK] = { - .entry_fn = rfm22_sendNack, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_SENDING_NACK] = { + .entry_fn = rfm22_sendNack, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_TIMEOUT] = { - .entry_fn = rfm22_timeout, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_TIMEOUT] = { + .entry_fn = rfm22_timeout, + .next_state = { + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_ERROR] = { - .entry_fn = rfm22_error, - .next_state = { - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_STATE_ERROR] = { + .entry_fn = rfm22_error, + .next_state = { + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_FATAL_ERROR] = { - .entry_fn = rfm22_fatal_error, - .next_state = { - }, + [RADIO_STATE_FATAL_ERROR] = { + .entry_fn = rfm22_fatal_error, + .next_state = {}, }, }; // xtal 10 ppm, 434MHz -static const uint32_t data_rate[] = { 500, 1000, 2000, 4000, 8000, 9600, 16000, 19200, 24000, 32000, 57600, 64000, 128000, 192000, 256000}; -static const uint8_t modulation_index[] = { 16, 8, 4, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; +static const uint32_t data_rate[] = { 500, 1000, 2000, 4000, 8000, 9600, 16000, 19200, 24000, 32000, 57600, 64000, 128000, 192000, 256000 }; +static const uint8_t modulation_index[] = { 16, 8, 4, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }; -static const uint8_t reg_1C[] = { 0x37, 0x37, 0x37, 0x37, 0x3A, 0x3B, 0x26, 0x28, 0x2E, 0x16, 0x06, 0x07, 0x83, 0x8A, 0x8C}; // rfm22_if_filter_bandwidth +static const uint8_t reg_1C[] = { 0x37, 0x37, 0x37, 0x37, 0x3A, 0x3B, 0x26, 0x28, 0x2E, 0x16, 0x06, 0x07, 0x83, 0x8A, 0x8C }; // rfm22_if_filter_bandwidth -static const uint8_t reg_1D[] = { 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44}; // rfm22_afc_loop_gearshift_override -static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02}; // rfm22_afc_timing_control +static const uint8_t reg_1D[] = { 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44 }; // rfm22_afc_loop_gearshift_override +static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02 }; // rfm22_afc_timing_control -static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03}; // rfm22_clk_recovery_gearshift_override -static const uint8_t reg_20[] = { 0xE8, 0xF4, 0xFA, 0x70, 0x3F, 0x34, 0x3F, 0x34, 0x2A, 0x3F, 0x45, 0x3F, 0x5E, 0x3F, 0x2F}; // rfm22_clk_recovery_oversampling_ratio -static const uint8_t reg_21[] = { 0x60, 0x20, 0x00, 0x01, 0x02, 0x02, 0x02, 0x02, 0x03, 0x02, 0x01, 0x02, 0x01, 0x02, 0x02}; // rfm22_clk_recovery_offset2 -static const uint8_t reg_22[] = { 0x20, 0x41, 0x83, 0x06, 0x0C, 0x75, 0x0C, 0x75, 0x12, 0x0C, 0xD7, 0x0c, 0x5D, 0x0C, 0xBB}; // rfm22_clk_recovery_offset1 -static const uint8_t reg_23[] = { 0xC5, 0x89, 0x12, 0x25, 0x4A, 0x25, 0x4A, 0x25, 0x6F, 0x4A, 0xDC, 0x4A, 0x86, 0x4A, 0x0D}; // rfm22_clk_recovery_offset0 -static const uint8_t reg_24[] = { 0x00, 0x00, 0x00, 0x02, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07}; // rfm22_clk_recovery_timing_loop_gain1 -static const uint8_t reg_25[] = { 0x0A, 0x23, 0x85, 0x0E, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x6E, 0xFF, 0x74, 0xFF, 0xFF}; // rfm22_clk_recovery_timing_loop_gain0 +static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 }; // rfm22_clk_recovery_gearshift_override +static const uint8_t reg_20[] = { 0xE8, 0xF4, 0xFA, 0x70, 0x3F, 0x34, 0x3F, 0x34, 0x2A, 0x3F, 0x45, 0x3F, 0x5E, 0x3F, 0x2F }; // rfm22_clk_recovery_oversampling_ratio +static const uint8_t reg_21[] = { 0x60, 0x20, 0x00, 0x01, 0x02, 0x02, 0x02, 0x02, 0x03, 0x02, 0x01, 0x02, 0x01, 0x02, 0x02 }; // rfm22_clk_recovery_offset2 +static const uint8_t reg_22[] = { 0x20, 0x41, 0x83, 0x06, 0x0C, 0x75, 0x0C, 0x75, 0x12, 0x0C, 0xD7, 0x0c, 0x5D, 0x0C, 0xBB }; // rfm22_clk_recovery_offset1 +static const uint8_t reg_23[] = { 0xC5, 0x89, 0x12, 0x25, 0x4A, 0x25, 0x4A, 0x25, 0x6F, 0x4A, 0xDC, 0x4A, 0x86, 0x4A, 0x0D }; // rfm22_clk_recovery_offset0 +static const uint8_t reg_24[] = { 0x00, 0x00, 0x00, 0x02, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07 }; // rfm22_clk_recovery_timing_loop_gain1 +static const uint8_t reg_25[] = { 0x0A, 0x23, 0x85, 0x0E, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x6E, 0xFF, 0x74, 0xFF, 0xFF }; // rfm22_clk_recovery_timing_loop_gain0 -static const uint8_t reg_2A[] = { 0x0E, 0x0E, 0x0E, 0x0E, 0x0E, 0x0D, 0x0D, 0x0E, 0x12, 0x17, 0x2D, 0x31, 0x50, 0x50, 0x50}; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz +static const uint8_t reg_2A[] = { 0x0E, 0x0E, 0x0E, 0x0E, 0x0E, 0x0D, 0x0D, 0x0E, 0x12, 0x17, 0x2D, 0x31, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz -static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80}; // rfm22_cpcuu -static const uint8_t reg_69[] = { 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x60, 0x20, 0x20, 0x20, 0x20}; // rfm22_agc_override1 -static const uint8_t reg_6E[] = { 0x04, 0x08, 0x10, 0x20, 0x41, 0x4E, 0x83, 0x9D, 0xC4, 0x08, 0x0E, 0x10, 0x20, 0x31, 0x41}; // rfm22_tx_data_rate1 -static const uint8_t reg_6F[] = { 0x19, 0x31, 0x62, 0xC5, 0x89, 0xA5, 0x12, 0x49, 0x9C, 0x31, 0xBF, 0x62, 0xC5, 0x27, 0x89}; // rfm22_tx_data_rate0 +static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80 }; // rfm22_cpcuu +static const uint8_t reg_69[] = { 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x60, 0x20, 0x20, 0x20, 0x20 }; // rfm22_agc_override1 +static const uint8_t reg_6E[] = { 0x04, 0x08, 0x10, 0x20, 0x41, 0x4E, 0x83, 0x9D, 0xC4, 0x08, 0x0E, 0x10, 0x20, 0x31, 0x41 }; // rfm22_tx_data_rate1 +static const uint8_t reg_6F[] = { 0x19, 0x31, 0x62, 0xC5, 0x89, 0xA5, 0x12, 0x49, 0x9C, 0x31, 0xBF, 0x62, 0xC5, 0x27, 0x89 }; // rfm22_tx_data_rate0 -static const uint8_t reg_70[] = { 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x0D, 0x0C, 0x0C, 0x0D, 0x0D, 0x0D}; // rfm22_modulation_mode_control1 -static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23}; // rfm22_modulation_mode_control2 +static const uint8_t reg_70[] = { 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x0D, 0x0C, 0x0C, 0x0D, 0x0D, 0x0D }; // rfm22_modulation_mode_control1 +static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23 }; // rfm22_modulation_mode_control2 -static const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x06, 0x08, 0x0D, 0x0F, 0x13, 0x1A, 0x2E, 0x33, 0x66, 0x9A, 0xCD}; // rfm22_frequency_deviation +static const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x06, 0x08, 0x0D, 0x0F, 0x13, 0x1A, 0x2E, 0x33, 0x66, 0x9A, 0xCD }; // rfm22_frequency_deviation -static struct pios_rfm22b_dev * g_rfm22b_dev = NULL; +static struct pios_rfm22b_dev *g_rfm22b_dev = NULL; /***************************************************************************** - * External Interface Functions - *****************************************************************************/ +* External Interface Functions +*****************************************************************************/ /** * Initialise an RFM22B device @@ -441,40 +441,40 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu // Allocate the device structure. struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)pios_rfm22_alloc(); if (!rfm22b_dev) { - return(-1); + return -1; } - *rfm22b_id = (uint32_t)rfm22b_dev; + *rfm22b_id = (uint32_t)rfm22b_dev; g_rfm22b_dev = rfm22b_dev; // Store the SPI handle - rfm22b_dev->slave_num = slave_num; - rfm22b_dev->spi_id = spi_id; + rfm22b_dev->slave_num = slave_num; + rfm22b_dev->spi_id = spi_id; // Initialize our configuration parameters - rfm22b_dev->send_ppm = false; - rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; - rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; + rfm22b_dev->send_ppm = false; + rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; + rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; // Initialize the com callbacks. rfm22b_dev->com_config_cb = NULL; - rfm22b_dev->rx_in_cb = NULL; - rfm22b_dev->tx_out_cb = NULL; + rfm22b_dev->rx_in_cb = NULL; + rfm22b_dev->tx_out_cb = NULL; // Initialize the stats. rfm22b_dev->stats.packets_per_sec = 0; rfm22b_dev->stats.rx_good = 0; - rfm22b_dev->stats.rx_corrected = 0; - rfm22b_dev->stats.rx_error = 0; - rfm22b_dev->stats.rx_missed = 0; - rfm22b_dev->stats.tx_dropped = 0; - rfm22b_dev->stats.tx_resent = 0; - rfm22b_dev->stats.resets = 0; - rfm22b_dev->stats.timeouts = 0; + rfm22b_dev->stats.rx_corrected = 0; + rfm22b_dev->stats.rx_error = 0; + rfm22b_dev->stats.rx_missed = 0; + rfm22b_dev->stats.tx_dropped = 0; + rfm22b_dev->stats.tx_resent = 0; + rfm22b_dev->stats.resets = 0; + rfm22b_dev->stats.timeouts = 0; rfm22b_dev->stats.link_quality = 0; rfm22b_dev->stats.rssi = 0; - rfm22b_dev->stats.tx_seq = 0; - rfm22b_dev->stats.rx_seq = 0; - rfm22b_dev->stats.tx_failure = 0; + rfm22b_dev->stats.tx_seq = 0; + rfm22b_dev->stats.rx_seq = 0; + rfm22b_dev->stats.tx_failure = 0; // Initialize the frequencies. PIOS_RFM22B_SetInitialFrequency(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY); @@ -487,13 +487,13 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->coordinator = false; // Create the event queue - rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event)); + rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event)); // Bind the configuration to the device instance rfm22b_dev->cfg = *cfg; // Create a semaphore to know if an ISR needs responding to - vSemaphoreCreateBinary( rfm22b_dev->isrPending ); + vSemaphoreCreateBinary(rfm22b_dev->isrPending); // Create our (hopefully) unique 32 bit id from the processor serial number. uint8_t crcs[] = { 0, 0, 0, 0 }; @@ -501,8 +501,9 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu char serial_no_str[33]; PIOS_SYS_SerialNumberGet(serial_no_str); // Create a 32 bit value using 4 8 bit CRC values. - for (uint8_t i = 0; serial_no_str[i] != 0; ++i) + for (uint8_t i = 0; serial_no_str[i] != 0; ++i) { crcs[i % 4] = PIOS_CRC_updateByte(crcs[i % 4], serial_no_str[i]); + } } rfm22b_dev->deviceID = crcs[0] | crcs[1] << 8 | crcs[2] << 16 | crcs[3] << 24; DEBUG_PRINTF(2, "RF device ID: %x\n\r", rfm22b_dev->deviceID); @@ -530,9 +531,9 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu pios_rfm22_inject_event(rfm22b_dev, RADIO_EVENT_INITIALIZE, false); // Start the driver task. This task controls the radio state machine and removed all of the IO from the IRQ handler. - xTaskCreate(pios_rfm22_task, (signed char *)"PIOS_RFM22B_Task", STACK_SIZE_BYTES, (void*)rfm22b_dev, TASK_PRIORITY, &(rfm22b_dev->taskHandle)); + xTaskCreate(pios_rfm22_task, (signed char *)"PIOS_RFM22B_Task", STACK_SIZE_BYTES, (void *)rfm22b_dev, TASK_PRIORITY, &(rfm22b_dev->taskHandle)); - return(0); + return 0; } /** @@ -543,6 +544,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu void PIOS_RFM22B_Reinit(uint32_t rfm22b_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (PIOS_RFM22B_Validate(rfm22b_dev)) { pios_rfm22_inject_event(rfm22b_dev, RADIO_EVENT_INITIALIZE, false); } @@ -571,6 +573,7 @@ bool PIOS_RFM22_EXT_Int(void) uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (PIOS_RFM22B_Validate(rfm22b_dev)) { return rfm22b_dev->deviceID; } @@ -586,6 +589,7 @@ uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id) bool PIOS_RFM22B_IsCoordinator(uint32_t rfm22b_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (PIOS_RFM22B_Validate(rfm22b_dev)) { return rfm22b_dev->coordinator; } @@ -601,8 +605,9 @@ bool PIOS_RFM22B_IsCoordinator(uint32_t rfm22b_id) bool PIOS_RFM22B_InRxWait(uint32_t rfm22b_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (PIOS_RFM22B_Validate(rfm22b_dev)) { - return ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_TRANSITION)); + return (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_TRANSITION); } return false; } @@ -616,6 +621,7 @@ bool PIOS_RFM22B_InRxWait(uint32_t rfm22b_id) void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } @@ -633,11 +639,12 @@ void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr) void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_freq, uint32_t max_freq, uint32_t step_size) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } - rfm22b_dev->con_packet.min_frequency = min_freq; - rfm22b_dev->con_packet.max_frequency = max_freq; + rfm22b_dev->con_packet.min_frequency = min_freq; + rfm22b_dev->con_packet.max_frequency = max_freq; rfm22b_dev->con_packet.channel_spacing = step_size; } @@ -650,6 +657,7 @@ void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_freq, uint32 void PIOS_RFM22B_SetInitialFrequency(uint32_t rfm22b_id, uint32_t init_freq) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } @@ -665,7 +673,8 @@ void PIOS_RFM22B_SetInitialFrequency(uint32_t rfm22b_id, uint32_t init_freq) void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigCallback cb) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_Validate(rfm22b_dev)) { + + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } rfm22b_dev->com_config_cb = cb; @@ -682,21 +691,22 @@ void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigC * @param[in] comSpeeds The array of com port speeds. */ void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindingPairIDs[], const uint8_t mainPortSettings[], - const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]) + const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_Validate(rfm22b_dev)) { + + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } // This modem will be considered a coordinator if any bindings have been set. rfm22b_dev->coordinator = false; for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { - rfm22b_dev->bindings[i].pairID = bindingPairIDs[i]; - rfm22b_dev->bindings[i].main_port = mainPortSettings[i]; + rfm22b_dev->bindings[i].pairID = bindingPairIDs[i]; + rfm22b_dev->bindings[i].main_port = mainPortSettings[i]; rfm22b_dev->bindings[i].flexi_port = flexiPortSettings[i]; - rfm22b_dev->bindings[i].vcp_port = vcpPortSettings[i]; - rfm22b_dev->bindings[i].com_speed = comSpeeds[i]; + rfm22b_dev->bindings[i].vcp_port = vcpPortSettings[i]; + rfm22b_dev->bindings[i].com_speed = comSpeeds[i]; rfm22b_dev->coordinator |= (rfm22b_dev->bindings[i].pairID != 0); } } @@ -707,9 +717,11 @@ void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindingPairIDs[] * @param[in] rfm22b_id The RFM22B device index. * @param[out] stats The stats are returned in this structure */ -void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) { +void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) +{ struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_Validate(rfm22b_dev)) { + + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } @@ -741,6 +753,7 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) { uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return 0; } @@ -763,10 +776,11 @@ uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_Validate(rfm22b_dev)) { + + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return false; } - return (rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD)); + return rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD); } /** @@ -776,8 +790,10 @@ bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id) * @param[in] p The packet to receive into. * @return true if Rx mode was entered sucessfully. */ -bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) { +bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) +{ struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return false; } @@ -808,7 +824,7 @@ bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) { rfm22b_dev->rx_buffer_wr = 0; // Clear the TX buffer. - rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; + rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; // clear FIFOs rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx); @@ -816,9 +832,9 @@ bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) { // enable RX interrupts rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_encrcerror | RFM22_ie1_enpkvalid | - RFM22_ie1_enrxffafull | RFM22_ie1_enfferr); + RFM22_ie1_enrxffafull | RFM22_ie1_enfferr); rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, RFM22_ie2_enpreainval | RFM22_ie2_enpreaval | - RFM22_ie2_enswdet); + RFM22_ie2_enswdet); // enable the receiver rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon); @@ -839,8 +855,10 @@ bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) { * @param[in] p The packet to transmit. * @return true if there if the packet was queued for transmission. */ -bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) { +bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) +{ struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return false; } @@ -871,7 +889,7 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) { // Set the destination address in the transmit header. // The destination address is the first 4 bytes of the message. - uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet); + uint8_t *tx_buffer = (uint8_t *)(rfm22b_dev->tx_packet); rfm22_write(rfm22b_dev, RFM22_transmit_header0, tx_buffer[0]); rfm22_write(rfm22b_dev, RFM22_transmit_header1, tx_buffer[1]); rfm22_write(rfm22b_dev, RFM22_transmit_header2, tx_buffer[2]); @@ -892,7 +910,7 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) { rfm22_assertCs(rfm22b_dev); PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80); int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd); - bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE: bytes_to_write; + bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE : bytes_to_write; PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL); rfm22b_dev->tx_data_rd += bytes_to_write; rfm22_deassertCs(rfm22b_dev); @@ -920,8 +938,10 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) { * @param[in] rfm22b_id The rfm22b device. * @return PIOS_RFM22B_TX_COMPLETE on completed Tx, or PIOS_RFM22B_INT_SUCCESS/PIOS_RFM22B_INT_FAILURE. */ -pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) { +pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) +{ struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return PIOS_RFM22B_INT_FAILURE; } @@ -934,24 +954,21 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) { // TX FIFO almost empty, it needs filling up if (rfm22b_dev->status_regs.int_status_1.tx_fifo_almost_empty) { - // Add data to the TX FIFO buffer - uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet); + uint8_t *tx_buffer = (uint8_t *)(rfm22b_dev->tx_packet); uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1; rfm22_claimBus(rfm22b_dev); rfm22_assertCs(rfm22b_dev); PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80); int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd); - bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes: bytes_to_write; + bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes : bytes_to_write; PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL); rfm22b_dev->tx_data_rd += bytes_to_write; rfm22_deassertCs(rfm22b_dev); rfm22_releaseBus(rfm22b_dev); return PIOS_RFM22B_INT_SUCCESS; - } else if (rfm22b_dev->status_regs.int_status_1.packet_sent_interrupt) { - // Transition out of Tx mode. rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION; @@ -967,12 +984,14 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) { * @param[in] rfm22b_id The rfm22b device. * @return PIOS_RFM22B_RX_COMPLETE on completed Rx, or PIOS_RFM22B_INT_SUCCESS/PIOS_RFM22B_INT_FAILURE. */ -pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { +pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) +{ struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return PIOS_RFM22B_INT_FAILURE; } - uint8_t *rx_buffer = (uint8_t*)(rfm22b_dev->rx_packet_handle); + uint8_t *rx_buffer = (uint8_t *)(rfm22b_dev->rx_packet_handle); pios_rfm22b_int_result ret = PIOS_RFM22B_INT_SUCCESS; // Read the device status registers @@ -990,7 +1009,6 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { // Valid packet received if (rfm22b_dev->status_regs.int_status_1.valid_packet_received) { - // Claim the SPI bus. rfm22_claimBus(rfm22b_dev); @@ -1002,12 +1020,12 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr; // Fetch the data from the RX FIFO rfm22_assertCs(rfm22b_dev); - PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F); - rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], + PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access & 0x7F); + rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], bytes_to_read, NULL) == 0) ? bytes_to_read : 0; rfm22_deassertCs(rfm22b_dev); } - + // Release the SPI bus. rfm22_releaseBus(rfm22b_dev); @@ -1024,9 +1042,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION; ret = PIOS_RFM22B_RX_COMPLETE; - } else if (rfm22b_dev->status_regs.int_status_1.rx_fifo_almost_full) { - // RX FIFO almost full, it needs emptying // read data from the rf chips FIFO buffer @@ -1045,7 +1061,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { // Fetch the data from the RX FIFO rfm22_assertCs(rfm22b_dev); - PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F); + PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access & 0x7F); rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], RX_FIFO_HI_WATERMARK, NULL) == 0) ? RX_FIFO_HI_WATERMARK : 0; rfm22_deassertCs(rfm22b_dev); @@ -1055,9 +1071,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { // Make sure that we're in RX mode. rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE; - } else if (rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) { - // Valid preamble detected RX_LED_ON; #ifdef PIOS_RFM22B_DEBUG_ON_TELEM @@ -1066,9 +1080,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { // We detected the preamble, now wait for sync. rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_WAIT_SYNC; - } else if (rfm22b_dev->status_regs.int_status_2.sync_word_detected) { - // Sync word detected // Claim the SPI bus. @@ -1078,7 +1090,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { // bits 9 to 2 uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8; // bits 1 & 0 - afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0; + afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0; afc_correction >>= 6; // convert the afc value to Hz int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f); @@ -1094,9 +1106,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { // Indicate that we're in RX mode. rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE; - } else if ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) && !rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) { - // Waiting for the preamble timed out. rfm22_rxFailure(rfm22b_dev); return PIOS_RFM22B_INT_FAILURE; @@ -1106,8 +1116,9 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { if ((rfm22b_dev->packet_start_ticks == 0) && ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC))) { rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); - if (rfm22b_dev->packet_start_ticks == 0) + if (rfm22b_dev->packet_start_ticks == 0) { rfm22b_dev->packet_start_ticks = 1; + } } return ret; @@ -1120,13 +1131,13 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) { */ inline bool PIOS_RFM22B_Validate(struct pios_rfm22b_dev *rfm22b_dev) { - return (rfm22b_dev != NULL && rfm22b_dev->magic == PIOS_RFM22B_DEV_MAGIC); + return rfm22b_dev != NULL && rfm22b_dev->magic == PIOS_RFM22B_DEV_MAGIC; } /***************************************************************************** - * The Device Control Thread - *****************************************************************************/ +* The Device Control Thread +*****************************************************************************/ /** * The task that controls the radio state machine. @@ -1136,29 +1147,31 @@ inline bool PIOS_RFM22B_Validate(struct pios_rfm22b_dev *rfm22b_dev) static void pios_rfm22_task(void *parameters) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)parameters; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } - portTickType lastEventTicks = xTaskGetTickCount(); + portTickType lastEventTicks = xTaskGetTickCount(); portTickType lastStatusTicks = lastEventTicks; - portTickType lastPPMTicks = lastEventTicks; + portTickType lastPPMTicks = lastEventTicks; - while(1) { + while (1) { #ifdef PIOS_WDG_RFM22B // Update the watchdog timer PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B); #endif /* PIOS_WDG_RFM22B */ // Wait for a signal indicating an external interrupt or a pending send/receive request. - if (xSemaphoreTake(rfm22b_dev->isrPending, ISR_TIMEOUT / portTICK_RATE_MS) == pdTRUE) { + if (xSemaphoreTake(rfm22b_dev->isrPending, ISR_TIMEOUT / portTICK_RATE_MS) == pdTRUE) { lastEventTicks = xTaskGetTickCount(); // Process events through the state machine. enum pios_radio_event event; while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) { if ((event == RADIO_EVENT_INT_RECEIVED) && - ((rfm22b_dev->state == RADIO_STATE_UNINITIALIZED) || (rfm22b_dev->state == RADIO_STATE_INITIALIZING))) + ((rfm22b_dev->state == RADIO_STATE_UNINITIALIZED) || (rfm22b_dev->state == RADIO_STATE_INITIALIZING))) { continue; + } rfm22_process_event(rfm22b_dev, event); } } else { @@ -1183,7 +1196,7 @@ static void pios_rfm22_task(void *parameters) } portTickType curTicks = xTaskGetTickCount(); - uint32_t last_rec_ms = (rfm22b_dev->rx_complete_ticks == 0) ? 0 : pios_rfm22_time_difference_ms(rfm22b_dev->rx_complete_ticks, curTicks); + uint32_t last_rec_ms = (rfm22b_dev->rx_complete_ticks == 0) ? 0 : pios_rfm22_time_difference_ms(rfm22b_dev->rx_complete_ticks, curTicks); // Have we been sending / receiving this packet too long? if ((rfm22b_dev->packet_start_ticks > 0) && (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) { @@ -1192,19 +1205,15 @@ static void pios_rfm22_task(void *parameters) // Has it been too long since we received a packet rfm22_process_event(rfm22b_dev, RADIO_EVENT_ERROR); } else { - // Are we waiting for an ACK? if (rfm22b_dev->prev_tx_packet) { - // Should we resend the packet? if ((pios_rfm22_time_difference_ms(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) && PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) { rfm22b_dev->tx_complete_ticks = curTicks; rfm22_process_event(rfm22b_dev, RADIO_EVENT_ACK_TIMEOUT); } - } else { - // Queue up a PPM packet if it's time. if (pios_rfm22_time_difference_ms(lastPPMTicks, curTicks) > PPM_UPDATE_PERIOD_MS) { rfm22_sendPPM(rfm22b_dev); @@ -1217,7 +1226,6 @@ static void pios_rfm22_task(void *parameters) lastStatusTicks = curTicks; } } - } // Send a packet if it's our time slice @@ -1237,8 +1245,8 @@ static void pios_rfm22_task(void *parameters) /***************************************************************************** - * The State Machine Functions - *****************************************************************************/ +* The State Machine Functions +*****************************************************************************/ /** * Inject an event into the RFM22B state machine. @@ -1252,8 +1260,9 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio if (inISR) { // Store the event. portBASE_TYPE pxHigherPriorityTaskWoken1; - if (xQueueSendFromISR(rfm22b_dev->eventQueue, &event, &pxHigherPriorityTaskWoken1) != pdTRUE) + if (xQueueSendFromISR(rfm22b_dev->eventQueue, &event, &pxHigherPriorityTaskWoken1) != pdTRUE) { return; + } // Signal the semaphore to wake up the handler thread. portBASE_TYPE pxHigherPriorityTaskWoken2; if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken2) != pdTRUE) { @@ -1263,8 +1272,9 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken2 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE)); } else { // Store the event. - if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE) + if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE) { return; + } // Signal the semaphore to wake up the handler thread. if (xSemaphoreGive(rfm22b_dev->isrPending) != pdTRUE) { // Something went fairly seriously wrong @@ -1282,7 +1292,6 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio */ static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event) { - // No event if (event >= RADIO_EVENT_NUM_EVENTS) { return RADIO_EVENT_NUM_EVENTS; @@ -1297,7 +1306,7 @@ static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_d /* * Move to the next state * - * This is done prior to calling the new state's entry function to + * This is done prior to calling the new state's entry function to * guarantee that the entry function never depends on the previous * state. This way, it cannot ever know what the previous state was. */ @@ -1321,15 +1330,15 @@ static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_d static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event) { // Process all state transitions. - while(event != RADIO_EVENT_NUM_EVENTS) { + while (event != RADIO_EVENT_NUM_EVENTS) { event = rfm22_process_state_transition(rfm22b_dev, event); } } /***************************************************************************** - * The Device Initialization / Configuration Functions - *****************************************************************************/ +* The Device Initialization / Configuration Functions +*****************************************************************************/ /** * Initialize (or re-initialize) the RFM22B radio device. @@ -1339,12 +1348,11 @@ static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_ra */ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) { - // Initialize the register values. - rfm22b_dev->status_regs.int_status_1.raw = 0; - rfm22b_dev->status_regs.int_status_2.raw = 0; + rfm22b_dev->status_regs.int_status_1.raw = 0; + rfm22b_dev->status_regs.int_status_2.raw = 0; rfm22b_dev->status_regs.device_status.raw = 0; - rfm22b_dev->status_regs.ezmac_status.raw = 0; + rfm22b_dev->status_regs.ezmac_status.raw = 0; // Clean the LEDs rfm22_clearLEDs(); @@ -1352,7 +1360,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // Initialize the detected device statistics. for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) { rfm22b_dev->pair_stats[i].pairID = 0; - rfm22b_dev->pair_stats[i].rssi = -127; + rfm22b_dev->pair_stats[i].rssi = -127; rfm22b_dev->pair_stats[i].afc_correction = 0; rfm22b_dev->pair_stats[i].lastContact = 0; } @@ -1364,37 +1372,37 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // Initialize the state rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; - rfm22b_dev->destination_id = 0xffffffff; + rfm22b_dev->destination_id = 0xffffffff; rfm22b_dev->send_status = false; rfm22b_dev->send_connection_request = false; // Initialize the packets. - rfm22b_dev->rx_packet_len = 0; - rfm22b_dev->tx_packet = NULL; + rfm22b_dev->rx_packet_len = 0; + rfm22b_dev->tx_packet = NULL; rfm22b_dev->prev_tx_packet = NULL; rfm22b_dev->data_packet.header.data_size = 0; // Initialize the devide state rfm22b_dev->rx_buffer_wr = 0; - rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; + rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; rfm22b_dev->frequency_hop_channel = 0; - rfm22b_dev->afc_correction_Hz = 0; - rfm22b_dev->packet_start_ticks = 0; - rfm22b_dev->tx_complete_ticks = 0; - rfm22b_dev->rx_complete_ticks = 0; + rfm22b_dev->afc_correction_Hz = 0; + rfm22b_dev->packet_start_ticks = 0; + rfm22b_dev->tx_complete_ticks = 0; + rfm22b_dev->rx_complete_ticks = 0; rfm22b_dev->rfm22b_state = RFM22B_STATE_INITIALIZING; // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) rfm22_write_claim(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); for (uint8_t i = 0; i < 50; ++i) { - // read the status registers pios_rfm22_readStatus(rfm22b_dev); // Is the chip ready? - if (rfm22b_dev->status_regs.int_status_2.chip_ready) + if (rfm22b_dev->status_regs.int_status_2.chip_ready) { break; + } // Wait 1ms if not. PIOS_DELAY_WaitmS(1); @@ -1415,7 +1423,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // read the RF chip ID bytes // read the device type - uint8_t device_type = rfm22_read(rfm22b_dev, RFM22_DEVICE_TYPE) & RFM22_DT_MASK; + uint8_t device_type = rfm22_read(rfm22b_dev, RFM22_DEVICE_TYPE) & RFM22_DT_MASK; // read the device version uint8_t device_version = rfm22_read(rfm22b_dev, RFM22_DEVICE_VERSION) & RFM22_DV_MASK; @@ -1463,7 +1471,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) } else { // GPIO0 = TX State (to control RF Switch) rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_rxstate); - // GPIO1 = RX State (to control RF Switch) + // GPIO1 = RX State (to control RF Switch) rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_txstate); } // GPIO2 = Clear Channel Assessment @@ -1504,14 +1512,14 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // header control - using a 4 by header with broadcast of 0xffffffff rfm22_write(rfm22b_dev, RFM22_header_control1, - RFM22_header_cntl1_bcen_0 | - RFM22_header_cntl1_bcen_1 | - RFM22_header_cntl1_bcen_2 | - RFM22_header_cntl1_bcen_3 | - RFM22_header_cntl1_hdch_0 | - RFM22_header_cntl1_hdch_1 | - RFM22_header_cntl1_hdch_2 | - RFM22_header_cntl1_hdch_3); + RFM22_header_cntl1_bcen_0 | + RFM22_header_cntl1_bcen_1 | + RFM22_header_cntl1_bcen_2 | + RFM22_header_cntl1_bcen_3 | + RFM22_header_cntl1_hdch_0 | + RFM22_header_cntl1_hdch_1 | + RFM22_header_cntl1_hdch_2 | + RFM22_header_cntl1_hdch_3); // Check all bit of all bytes of the header rfm22_write(rfm22b_dev, RFM22_header_enable0, 0xff); rfm22_write(rfm22b_dev, RFM22_header_enable1, 0xff); @@ -1525,9 +1533,9 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22_write(rfm22b_dev, RFM22_check_header3, (id >> 24) & 0xff); // 4 header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header. rfm22_write(rfm22b_dev, RFM22_header_control2, - RFM22_header_cntl2_hdlen_3210 | - RFM22_header_cntl2_synclen_3210 | - ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); + RFM22_header_cntl2_hdlen_3210 | + RFM22_header_cntl2_synclen_3210 | + ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); // sync word rfm22_write(rfm22b_dev, RFM22_sync_word3, SYNC_BYTE_1); @@ -1573,6 +1581,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening) { uint32_t datarate_bps = data_rate[datarate]; + rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5f); // Generate a pseudo-random number from 0-8 to add to the delay @@ -1620,7 +1629,7 @@ static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm2 rfm22_write(rfm22b_dev, 0x70, reg_70[datarate] & ~RFM22_mmc1_enwhite); } else { // rfm22_modulation_mode_control1 - rfm22_write(rfm22b_dev, 0x70, reg_70[datarate] | RFM22_mmc1_enwhite); + rfm22_write(rfm22b_dev, 0x70, reg_70[datarate] | RFM22_mmc1_enwhite); } // rfm22_modulation_mode_control2 @@ -1652,7 +1661,8 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz = min_frequency; // holds the hbsel (1 or 2) - uint8_t hbsel; + uint8_t hbsel; + if (frequency_hz < 480000000) { hbsel = 0; } else { @@ -1660,11 +1670,11 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, } float freq_mhz = (float)(frequency_hz) / 1000000.0f; float xtal_freq_khz = 30000.0f; - float sfreq = freq_mhz / (10.0f * (xtal_freq_khz / 30000.0f) * (1 + hbsel)); - uint32_t fb = (uint32_t)sfreq - 24 + (64 + 32 * hbsel); - uint32_t fc = (uint32_t)((sfreq - (uint32_t)sfreq) * 64000.0f); - uint8_t fch = (fc >> 8) & 0xff; - uint8_t fcl = fc & 0xff; + float sfreq = freq_mhz / (10.0f * (xtal_freq_khz / 30000.0f) * (1 + hbsel)); + uint32_t fb = (uint32_t)sfreq - 24 + (64 + 32 * hbsel); + uint32_t fc = (uint32_t)((sfreq - (uint32_t)sfreq) * 64000.0f); + uint8_t fch = (fc >> 8) & 0xff; + uint8_t fcl = fc & 0xff; // Claim the SPI bus. rfm22_claimBus(rfm22b_dev); @@ -1680,7 +1690,7 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, (uint8_t)freq_hop_step_size); // frequency hopping channel (0-255) - rfm22b_dev->frequency_step_size = 156.25f * hbsel; + rfm22b_dev->frequency_step_size = 156.25f * hbsel; // frequency hopping channel (0-255) rfm22b_dev->frequency_hop_channel = 0; @@ -1726,22 +1736,21 @@ static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t */ static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev) { - // 1. Read the interrupt statuses with burst read - rfm22_claimBus(rfm22b_dev); // Set RC and the semaphore - uint8_t write_buf[3] = {RFM22_interrupt_status1 & 0x7f, 0xFF, 0xFF}; + rfm22_claimBus(rfm22b_dev); // Set RC and the semaphore + uint8_t write_buf[3] = { RFM22_interrupt_status1 &0x7f, 0xFF, 0xFF }; uint8_t read_buf[3]; rfm22_assertCs(rfm22b_dev); PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, write_buf, read_buf, sizeof(write_buf), NULL); rfm22_deassertCs(rfm22b_dev); - rfm22b_dev->status_regs.int_status_1.raw = read_buf[1]; - rfm22b_dev->status_regs.int_status_2.raw = read_buf[2]; + rfm22b_dev->status_regs.int_status_1.raw = read_buf[1]; + rfm22b_dev->status_regs.int_status_2.raw = read_buf[2]; // Device status rfm22b_dev->status_regs.device_status.raw = rfm22_read(rfm22b_dev, RFM22_device_status); // EzMAC status - rfm22b_dev->status_regs.ezmac_status.raw = rfm22_read(rfm22b_dev, RFM22_ezmac_status); + rfm22b_dev->status_regs.ezmac_status.raw = rfm22_read(rfm22b_dev, RFM22_ezmac_status); // Release the bus rfm22_releaseBus(rfm22b_dev); @@ -1774,8 +1783,8 @@ static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev) /***************************************************************************** - * Radio Transmit and Receive functions. - *****************************************************************************/ +* Radio Transmit and Receive functions. +*****************************************************************************/ /** * Start a transmit if possible @@ -1795,9 +1804,7 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) // See if there's a packet ready to send. if (radio_dev->tx_packet) { p = radio_dev->tx_packet; - } else { - // Don't send a packet if we're waiting for an ACK if (radio_dev->prev_tx_packet) { return RADIO_EVENT_RX_MODE; @@ -1805,26 +1812,26 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) // Send a connection request? if (!p && radio_dev->send_connection_request) { - p = (PHPacketHandle)&(radio_dev->con_packet); + p = (PHPacketHandle) & (radio_dev->con_packet); radio_dev->send_connection_request = false; } #ifdef PIOS_PPM_RECEIVER // Send a PPM packet? - if (!p && radio_dev->send_ppm) { - p = (PHPacketHandle)&(radio_dev->ppm_packet); + if (!p && radio_dev->send_ppm) { + p = (PHPacketHandle) & (radio_dev->ppm_packet); radio_dev->send_ppm = false; } #endif // Send status? if (!p && radio_dev->send_status) { - p = (PHPacketHandle)&(radio_dev->status_packet); + p = (PHPacketHandle) & (radio_dev->status_packet); radio_dev->send_status = false; } // Try to get some data to send - if (!p) { + if (!p) { bool need_yield = false; p = &radio_dev->data_packet; p->header.type = PACKET_TYPE_DATA; @@ -1862,7 +1869,7 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) } // Add the error correcting code. - encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p); + encode_data((unsigned char *)p, PHPacketSize(p), (unsigned char *)p); // Transmit the packet. PIOS_RFM22B_TransmitPacket((uint32_t)radio_dev, p); @@ -1884,27 +1891,23 @@ static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *radio_dev) // Is the transmition complete if (res == PIOS_RFM22B_TX_COMPLETE) { radio_dev->stats.tx_byte_count += PH_PACKET_SIZE(radio_dev->tx_packet); - radio_dev->tx_complete_ticks = xTaskGetTickCount(); + radio_dev->tx_complete_ticks = xTaskGetTickCount(); // Is this an ACK? bool is_ack = (radio_dev->tx_packet->header.type == PACKET_TYPE_ACK); ret_event = RADIO_EVENT_RX_MODE; if (is_ack) { - // If this is an ACK for a connection request message we need to // configure this modem from the connection request message. if (radio_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) { rfm22_setConnectionParameters(radio_dev); } - } else if ((radio_dev->tx_packet->header.type != PACKET_TYPE_NACK) && (radio_dev->tx_packet->header.type != PACKET_TYPE_PPM) && (radio_dev->tx_packet->header.type != PACKET_TYPE_STATUS) && (radio_dev->tx_packet->header.type != PACKET_TYPE_PPM)) { - // We need to wait for an ACK if this packet it not an ACK, NACK, or PPM. radio_dev->prev_tx_packet = radio_dev->tx_packet; - } - radio_dev->tx_packet = 0; + radio_dev->tx_packet = 0; radio_dev->tx_data_wr = radio_dev->tx_data_rd = 0; // Start a new transaction radio_dev->packet_start_ticks = 0; @@ -1947,12 +1950,12 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d portTickType curTicks = xTaskGetTickCount(); // Attempt to correct any errors in the packet. - decode_data((unsigned char*)p, rx_len); + decode_data((unsigned char *)p, rx_len); bool good_packet = check_syndrome() == 0; bool corrected_packet = false; // We have an error. Try to correct it. - if(!good_packet && (correct_errors_erasures((unsigned char*)p, rx_len, 0, 0) != 0)) { + if (!good_packet && (correct_errors_erasures((unsigned char *)p, rx_len, 0, 0) != 0)) { // We corrected it corrected_packet = true; } @@ -1960,7 +1963,7 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d // Set the packet status if (good_packet) { rfm22b_add_rx_status(radio_dev, RADIO_GOOD_RX_PACKET); - } else if(corrected_packet) { + } else if (corrected_packet) { // We corrected the error. rfm22b_add_rx_status(radio_dev, RADIO_CORRECTED_RX_PACKET); } else { @@ -1976,7 +1979,7 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d // Send a connection request message if we're not connected, and this is a status message from a modem that we're bound to. if (radio_dev->coordinator && !rfm22_isConnected(radio_dev)) { - PHStatusPacketHandle status = (PHStatusPacketHandle)&(radio_dev->rx_packet); + PHStatusPacketHandle status = (PHStatusPacketHandle) & (radio_dev->rx_packet); uint32_t source_id = status->source_id; for (uint8_t i = 0; OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { if (radio_dev->bindings[i].pairID == source_id) { @@ -1995,8 +1998,9 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d { // Send the data to the com port bool rx_need_yield; - if (radio_dev->rx_in_cb) + if (radio_dev->rx_in_cb) { (radio_dev->rx_in_cb)(radio_dev->rx_in_context, p->data, p->header.data_size, NULL, &rx_need_yield); + } break; } case PACKET_TYPE_DUPLICATE_DATA: @@ -2017,11 +2021,11 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d #endif #if defined(PIOS_INCLUDE_RFM22B_RCVR) ppm_output = true; - radio_dev->ppm_fresh = true; + radio_dev->ppm_fresh = true; for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { radio_dev->ppm_channel[i] = ppmp->channels[i]; } -#endif +#endif #if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT) if (PIOS_PPM_OUTPUT) { ppm_output = true; @@ -2036,7 +2040,7 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) { gcsRcvr.Channel[i] = ppmp->channels[i]; } - GCSReceiverSet(&gcsRcvr); + GCSReceiverSet(&gcsRcvr); } #endif break; @@ -2047,36 +2051,32 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d uint16_t seq_num = radio_dev->rx_packet.header.seq_num; if (rfm22_isConnected(radio_dev)) { - // Adjust the clock syncronization if this is the remote modem. // The coordinator sends the time that the previous packet was finised transmitting, // which should match the time that the last packet was received. uint16_t prev_seq_num = radio_dev->stats.rx_seq; if (seq_num == (prev_seq_num + 1)) { - portTickType local_rx_time = radio_dev->rx_complete_ticks; + portTickType local_rx_time = radio_dev->rx_complete_ticks; portTickType remote_tx_time = radio_dev->rx_packet.header.prev_tx_time; radio_dev->time_delta = remote_tx_time - local_rx_time; - } else if (seq_num > prev_seq_num) { - // Add any missed packets into the stats. uint16_t missed_packets = seq_num - prev_seq_num - 1; radio_dev->stats.rx_missed += missed_packets; } - } // Update the sequence number radio_dev->stats.rx_seq = seq_num; - } else { ret_event = RADIO_EVENT_RX_COMPLETE; } // Log the time that the packet was received. radio_dev->rx_complete_ticks = curTicks; - if (radio_dev->rx_complete_ticks == 0) + if (radio_dev->rx_complete_ticks == 0) { radio_dev->rx_complete_ticks = 1; + } return ret_event; } @@ -2093,7 +2093,6 @@ static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *radio_dev) pios_rfm22b_int_result res = PIOS_RFM22B_ProcessRx((uint32_t)radio_dev); switch (res) { - case PIOS_RFM22B_RX_COMPLETE: // Receive the packet. @@ -2121,8 +2120,8 @@ static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *radio_dev) } /***************************************************************************** - * Packet Transmition Functions - *****************************************************************************/ +* Packet Transmition Functions +*****************************************************************************/ /** * Send a radio status message. @@ -2147,14 +2146,12 @@ static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) } else { rfm22b_dev->status_packet.header.destination_id = 0xffffffff; // Broadcast } - rfm22b_dev->status_packet.header.type = PACKET_TYPE_STATUS; + rfm22b_dev->status_packet.header.type = PACKET_TYPE_STATUS; rfm22b_dev->status_packet.header.data_size = PH_STATUS_DATA_SIZE(&(rfm22b_dev->status_packet)); - rfm22b_dev->status_packet.source_id = rfm22b_dev->deviceID; - rfm22b_dev->status_packet.link_quality = rfm22b_dev->stats.link_quality; - rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm; + rfm22b_dev->status_packet.source_id = rfm22b_dev->deviceID; + rfm22b_dev->status_packet.link_quality = rfm22b_dev->stats.link_quality; + rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm; rfm22b_dev->send_status = true; - - return; } /** @@ -2179,8 +2176,9 @@ static void rfm22_sendPPM(__attribute__((unused)) struct pios_rfm22b_dev *rfm22b bool valid_input_detected = false; for (uint8_t i = 0; i < PIOS_PPM_NUM_INPUTS; ++i) { rfm22b_dev->ppm_packet.channels[i] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i + 1); - if((rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_INVALID) && (rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_TIMEOUT)) + if ((rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_INVALID) && (rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_TIMEOUT)) { valid_input_detected = true; + } } // Send the PPM packet if it's valid @@ -2190,7 +2188,7 @@ static void rfm22_sendPPM(__attribute__((unused)) struct pios_rfm22b_dev *rfm22b rfm22b_dev->ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&(rfm22b_dev->ppm_packet)); rfm22b_dev->send_ppm = true; } -#endif +#endif /* ifdef PIOS_PPM_RECEIVER */ } /** @@ -2221,6 +2219,7 @@ static enum pios_radio_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_radio_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev) { PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); + aph->header.destination_id = rfm22b_dev->destination_id; aph->header.type = PACKET_TYPE_NACK; aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); @@ -2242,16 +2241,16 @@ static enum pios_radio_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTING; // Fill in the connection request - rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID; - cph->header.destination_id = rfm22b_dev->destination_id; - cph->header.type = PACKET_TYPE_CON_REQUEST; + rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID; + cph->header.destination_id = rfm22b_dev->destination_id; + cph->header.type = PACKET_TYPE_CON_REQUEST; cph->header.data_size = PH_CONNECTION_DATA_SIZE(&(rfm22b_dev->con_packet)); - cph->source_id = rfm22b_dev->deviceID; - cph->status_rx_time = rfm22b_dev->rx_complete_ticks; - cph->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port; - cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port; + cph->source_id = rfm22b_dev->deviceID; + cph->status_rx_time = rfm22b_dev->rx_complete_ticks; + cph->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port; + cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port; cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port; - cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; + cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; rfm22b_dev->send_connection_request = true; rfm22b_dev->prev_tx_packet = NULL; @@ -2260,8 +2259,8 @@ static enum pios_radio_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm /***************************************************************************** - * Packet Receipt Functions - *****************************************************************************/ +* Packet Receipt Functions +*****************************************************************************/ /** * Receive an ACK. @@ -2298,7 +2297,6 @@ static enum pios_radio_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev */ static enum pios_radio_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev) { - // Resend the previous TX packet. rfm22b_dev->tx_packet = rfm22b_dev->prev_tx_packet; rfm22b_dev->prev_tx_packet = NULL; @@ -2318,39 +2316,40 @@ static enum pios_radio_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_de */ static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev) { - PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet); - int8_t rssi = rfm22b_dev->rssi_dBm; - int8_t afc = rfm22b_dev->afc_correction_Hz; - uint32_t id = status->source_id; + PHStatusPacketHandle status = (PHStatusPacketHandle) & (rfm22b_dev->rx_packet); + int8_t rssi = rfm22b_dev->rssi_dBm; + int8_t afc = rfm22b_dev->afc_correction_Hz; + uint32_t id = status->source_id; // Have we seen this device recently? - bool found = false; + bool found = false; uint8_t id_idx = 0; - for ( ; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) { - if(rfm22b_dev->pair_stats[id_idx].pairID == id) { + + for (; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) { + if (rfm22b_dev->pair_stats[id_idx].pairID == id) { found = true; break; } } // If we have seen it, update the RSSI and reset the last contact couter - if(found) { + if (found) { rfm22b_dev->pair_stats[id_idx].rssi = rssi; rfm22b_dev->pair_stats[id_idx].afc_correction = afc; - rfm22b_dev->pair_stats[id_idx].lastContact = 0; + rfm22b_dev->pair_stats[id_idx].lastContact = 0; - // If we haven't seen it, find a slot to put it in. + // If we haven't seen it, find a slot to put it in. } else { uint8_t min_idx = 0; int8_t min_rssi = rfm22b_dev->pair_stats[0].rssi; for (id_idx = 1; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) { - if(rfm22b_dev->pair_stats[id_idx].rssi < min_rssi) { + if (rfm22b_dev->pair_stats[id_idx].rssi < min_rssi) { min_rssi = rfm22b_dev->pair_stats[id_idx].rssi; - min_idx = id_idx; + min_idx = id_idx; } } rfm22b_dev->pair_stats[min_idx].pairID = id; - rfm22b_dev->pair_stats[min_idx].rssi = rssi; + rfm22b_dev->pair_stats[min_idx].rssi = rssi; rfm22b_dev->pair_stats[min_idx].afc_correction = afc; rfm22b_dev->pair_stats[min_idx].lastContact = 0; } @@ -2360,8 +2359,8 @@ static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_ /***************************************************************************** - * Link Statistics Functions - *****************************************************************************/ +* Link Statistics Functions +*****************************************************************************/ /** * Calculate the link quality from the packet receipt, tranmittion statistics. @@ -2371,10 +2370,10 @@ static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev) { // Add the RX packet statistics - rfm22b_dev->stats.rx_good = 0; + rfm22b_dev->stats.rx_good = 0; rfm22b_dev->stats.rx_corrected = 0; - rfm22b_dev->stats.rx_error = 0; - rfm22b_dev->stats.tx_resent = 0; + rfm22b_dev->stats.rx_error = 0; + rfm22b_dev->stats.tx_resent = 0; for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) { uint32_t val = rfm22b_dev->rx_packet_stats[i]; for (uint8_t j = 0; j < 16; ++j) { @@ -2419,8 +2418,8 @@ static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_r /***************************************************************************** - * Connection Handling Functions - *****************************************************************************/ +* Connection Handling Functions +*****************************************************************************/ /** * Are we connected to the remote modem? @@ -2429,7 +2428,7 @@ static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_r */ static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev) { - return (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED); + return rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED; } /** @@ -2467,16 +2466,16 @@ static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm2 rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; // Copy the connection packet - PHConnectionPacketHandle cph = (PHConnectionPacketHandle)(&(rfm22b_dev->rx_packet)); + PHConnectionPacketHandle cph = (PHConnectionPacketHandle)(&(rfm22b_dev->rx_packet)); PHConnectionPacketHandle lcph = (PHConnectionPacketHandle)(&(rfm22b_dev->con_packet)); - memcpy((uint8_t*)lcph, (uint8_t*)cph, PH_PACKET_SIZE((PHPacketHandle)cph)); + memcpy((uint8_t *)lcph, (uint8_t *)cph, PH_PACKET_SIZE((PHPacketHandle)cph)); // Set the destination ID to the source of the connection request message. rfm22b_dev->destination_id = cph->source_id; // The remote modem sets the time delta between the two modems using the differene between the clock // on the local modem when it sent the status packet and the time on the coordinator modem when it was received. - portTickType local_tx_time = rfm22b_dev->tx_complete_ticks; + portTickType local_tx_time = rfm22b_dev->tx_complete_ticks; portTickType remote_rx_time = cph->status_rx_time; rfm22b_dev->time_delta = remote_rx_time - local_tx_time; @@ -2485,8 +2484,8 @@ static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm2 /***************************************************************************** - * Frequency Hopping Functions - *****************************************************************************/ +* Frequency Hopping Functions +*****************************************************************************/ /** * Return the extimated current clock ticks count on the coordinator modem. @@ -2512,6 +2511,7 @@ static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev) portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); // Divide time into 8ms blocks. Coordinator sends in firs 2 ms, and remote send in 5th and 6th ms. bool tts = (rfm22b_dev->coordinator) ? ((time & 0x06) == 0) : (((time + 4) & 0x06) == 0); + // Noone starts a transmit just prior to a channel change. return tts && ((time & 0x7e) < 0x7b); } @@ -2526,6 +2526,7 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev) portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); // We change channels every 128 ms. uint16_t n = (time >> 7) & 0xffff; + // The channel is calculated using the 16 bit CRC as the pseudo random number generator. n = PIOS_CRC16_updateByte(n, 0); float num_channels = rfm22b_dev->num_channels; @@ -2547,8 +2548,8 @@ static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev) /***************************************************************************** - * Error Handling Functions - *****************************************************************************/ +* Error Handling Functions +*****************************************************************************/ /** * Recover from a transmit failure. @@ -2615,7 +2616,7 @@ static enum pios_radio_event rfm22_fatal_error(__attribute__((unused)) struct pi { // RF module error .. flash the LED's rfm22_clearLEDs(); - for(unsigned int j = 0; j < 16; j++) { + for (unsigned int j = 0; j < 16; j++) { USB_LED_ON; LINK_LED_ON; RX_LED_OFF; @@ -2640,8 +2641,8 @@ static enum pios_radio_event rfm22_fatal_error(__attribute__((unused)) struct pi /***************************************************************************** - * Utility Functions - *****************************************************************************/ +* Utility Functions +*****************************************************************************/ /** * Calculate the time difference between the start time and end time. @@ -2652,7 +2653,7 @@ static enum pios_radio_event rfm22_fatal_error(__attribute__((unused)) struct pi */ static uint32_t pios_rfm22_time_difference_ms(portTickType start_time, portTickType end_time) { - if(end_time >= start_time) { + if (end_time >= start_time) { return (end_time - start_time) * portTICK_RATE_MS; } // Rollover @@ -2665,7 +2666,7 @@ static uint32_t pios_rfm22_time_difference_ms(portTickType start_time, portTickT #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_rfm22b_dev *pios_rfm22_alloc(void) { - struct pios_rfm22b_dev * rfm22b_dev; + struct pios_rfm22b_dev *rfm22b_dev; rfm22b_dev = (struct pios_rfm22b_dev *)pvPortMalloc(sizeof(*rfm22b_dev)); rfm22b_dev->spi_id = 0; @@ -2674,14 +2675,14 @@ static struct pios_rfm22b_dev *pios_rfm22_alloc(void) } rfm22b_dev->magic = PIOS_RFM22B_DEV_MAGIC; - return(rfm22b_dev); + return rfm22b_dev; } #else static struct pios_rfm22b_dev pios_rfm22b_devs[PIOS_RFM22B_MAX_DEVS]; static uint8_t pios_rfm22b_num_devs; static struct pios_rfm22b_dev *pios_rfm22_alloc(void) { - struct pios_rfm22b_dev * rfm22b_dev; + struct pios_rfm22b_dev *rfm22b_dev; if (pios_rfm22b_num_devs >= PIOS_RFM22B_MAX_DEVS) { return NULL; @@ -2690,14 +2691,15 @@ static struct pios_rfm22b_dev *pios_rfm22_alloc(void) rfm22b_dev = &pios_rfm22b_devs[pios_rfm22b_num_devs++]; rfm22b_dev->magic = PIOS_RFM22B_DEV_MAGIC; - return (rfm22b_dev); + return rfm22b_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /** * Turn off all of the LEDs */ -static void rfm22_clearLEDs(void) { +static void rfm22_clearLEDs(void) +{ LINK_LED_OFF; RX_LED_OFF; TX_LED_OFF; @@ -2711,8 +2713,8 @@ static void rfm22_clearLEDs(void) { /***************************************************************************** - * SPI Read/Write Functions - *****************************************************************************/ +* SPI Read/Write Functions +*****************************************************************************/ /** * Assert the chip select line. @@ -2722,7 +2724,7 @@ static void rfm22_clearLEDs(void) { static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev) { PIOS_DELAY_WaituS(1); - if(rfm22b_dev->spi_id != 0) { + if (rfm22b_dev->spi_id != 0) { PIOS_SPI_RC_PinSet(rfm22b_dev->spi_id, rfm22b_dev->slave_num, 0); } } @@ -2734,7 +2736,7 @@ static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev) */ static void rfm22_deassertCs(struct pios_rfm22b_dev *rfm22b_dev) { - if(rfm22b_dev->spi_id != 0) { + if (rfm22b_dev->spi_id != 0) { PIOS_SPI_RC_PinSet(rfm22b_dev->spi_id, rfm22b_dev->slave_num, 1); } } @@ -2746,7 +2748,7 @@ static void rfm22_deassertCs(struct pios_rfm22b_dev *rfm22b_dev) */ static void rfm22_claimBus(struct pios_rfm22b_dev *rfm22b_dev) { - if(rfm22b_dev->spi_id != 0) { + if (rfm22b_dev->spi_id != 0) { PIOS_SPI_ClaimBus(rfm22b_dev->spi_id); } } @@ -2758,7 +2760,7 @@ static void rfm22_claimBus(struct pios_rfm22b_dev *rfm22b_dev) */ static void rfm22_releaseBus(struct pios_rfm22b_dev *rfm22b_dev) { - if(rfm22b_dev->spi_id != 0) { + if (rfm22b_dev->spi_id != 0) { PIOS_SPI_ReleaseBus(rfm22b_dev->spi_id); } } @@ -2774,7 +2776,7 @@ static void rfm22_write_claim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, { rfm22_claimBus(rfm22b_dev); rfm22_assertCs(rfm22b_dev); - uint8_t buf[2] = {addr | 0x80, data}; + uint8_t buf[2] = { addr | 0x80, data }; PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL); rfm22_deassertCs(rfm22b_dev); rfm22_releaseBus(rfm22b_dev); @@ -2790,7 +2792,7 @@ static void rfm22_write_claim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data) { rfm22_assertCs(rfm22b_dev); - uint8_t buf[2] = {addr | 0x80, data}; + uint8_t buf[2] = { addr | 0x80, data }; PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL); rfm22_deassertCs(rfm22b_dev); } @@ -2804,8 +2806,9 @@ static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_ */ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr) { - uint8_t out[2] = {addr & 0x7F, 0xFF}; + uint8_t out[2] = { addr &0x7F, 0xFF }; uint8_t in[2]; + rfm22_assertCs(rfm22b_dev); PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, out, in, sizeof(out), NULL); rfm22_deassertCs(rfm22b_dev); diff --git a/flight/pios/common/pios_rfm22b_com.c b/flight/pios/common/pios_rfm22b_com.c index e0800eb27..9275d48b6 100644 --- a/flight/pios/common/pios_rfm22b_com.c +++ b/flight/pios/common/pios_rfm22b_com.c @@ -1,30 +1,30 @@ /** -****************************************************************************** -* @addtogroup PIOS PIOS Core hardware abstraction layer -* @{ -* @addtogroup PIOS_RFM22B Radio Functions -* @brief PIOS COM interface for for the RFM22B radio -* @{ -* -* @file pios_rfm22b_com.c -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief Implements a driver the the RFM22B driver -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RFM22B Radio Functions + * @brief PIOS COM interface for for the RFM22B radio + * @{ + * + * @file pios_rfm22b_com.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Implements a driver the the RFM22B driver + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -61,27 +61,29 @@ const struct pios_com_driver pios_rfm22b_com_driver = { static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex. enum rfm22b_datarate datarate = RFM22_datarate_64000; - if (baud <= 1024) + if (baud <= 1024) { datarate = RFM22_datarate_500; - else if (baud <= 2048) + } else if (baud <= 2048) { datarate = RFM22_datarate_1000; - else if (baud <= 4096) + } else if (baud <= 4096) { datarate = RFM22_datarate_8000; - else if (baud <= 9600) + } else if (baud <= 9600) { datarate = RFM22_datarate_16000; - else if (baud <= 19200) + } else if (baud <= 19200) { datarate = RFM22_datarate_32000; - else if (baud <= 38400) + } else if (baud <= 38400) { datarate = RFM22_datarate_57600; - else if (baud <= 57600) + } else if (baud <= 57600) { datarate = RFM22_datarate_128000; - else if (baud <= 115200) + } else if (baud <= 115200) { datarate = RFM22_datarate_192000; + } rfm22b_dev->datarate = datarate; } @@ -92,9 +94,8 @@ static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) * @param[in] rx_bytes_available The number of bytes available to receive */ static void PIOS_RFM22B_COM_RxStart(__attribute__((unused)) uint32_t rfm22b_id, - __attribute__((unused)) uint16_t rx_bytes_avail) -{ -} + __attribute__((unused)) uint16_t rx_bytes_avail) +{} /** * Start a transmit from the COM device @@ -105,13 +106,14 @@ static void PIOS_RFM22B_COM_RxStart(__attribute__((unused)) uint32_t rfm22b_id, static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, __attribute__((unused)) uint16_t tx_bytes_avail) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } } /** - * Register the callback to pass received data to + * Register the callback to pass received data to * * @param[in] rfm22b_dev The device ID. * @param[in] rx_in_cb The Rx callback function. @@ -120,10 +122,12 @@ static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, __attribute__((unused)) static void PIOS_RFM22B_COM_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_Validate(rfm22b_dev)) - return; - /* + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } + + /* * Order is important in these assignments since ISR uses _cb * field to determine if it's ok to dereference _cb and _context */ @@ -141,11 +145,12 @@ static void PIOS_RFM22B_COM_RegisterRxCallback(uint32_t rfm22b_id, pios_com_call static void PIOS_RFM22B_COM_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } - /* + /* * Order is important in these assignments since ISR uses _cb * field to determine if it's ok to dereference _cb and _context */ diff --git a/flight/pios/common/pios_rfm22b_rcvr.c b/flight/pios/common/pios_rfm22b_rcvr.c index e718e8481..81f43efa8 100644 --- a/flight/pios/common/pios_rfm22b_rcvr.c +++ b/flight/pios/common/pios_rfm22b_rcvr.c @@ -1,17 +1,17 @@ /** -****************************************************************************** -* @addtogroup PIOS PIOS Core hardware abstraction layer -* @{ -* @addtogroup PIOS_RFM22B_RCVR RFM22B Receiver Input Functions -* @brief Code to output the PPM signal from the RFM22B -* @{ -* -* @file pios_rfm22b_rcvr.c -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief Implements a receiver interface to the RFM22B device -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RFM22B_RCVR RFM22B Receiver Input Functions + * @brief Code to output the PPM signal from the RFM22B + * @{ + * + * @file pios_rfm22b_rcvr.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Implements a receiver interface to the RFM22B device + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -51,6 +51,7 @@ const struct pios_rcvr_driver pios_rfm22b_rcvr_driver = { int32_t PIOS_RFM22B_RCVR_Init(uint32_t rcvr_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return -1; } @@ -78,6 +79,7 @@ int32_t PIOS_RFM22B_RCVR_Init(uint32_t rcvr_id) static int32_t PIOS_RFM22B_RCVR_Get(uint32_t rcvr_id, uint8_t channel) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return -1; } @@ -95,8 +97,10 @@ static int32_t PIOS_RFM22B_RCVR_Get(uint32_t rcvr_id, uint8_t channel) * * @param[in] rcvr_id The receiver ID. */ -static void PIOS_RFM22B_RCVR_Supervisor(uint32_t rcvr_id) { +static void PIOS_RFM22B_RCVR_Supervisor(uint32_t rcvr_id) +{ struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } @@ -116,9 +120,9 @@ static void PIOS_RFM22B_RCVR_Supervisor(uint32_t rcvr_id) { rfm22b_dev->ppm_fresh = false; } -#endif /* PIOS_INCLUDE_RFM22B_RCVR */ +#endif /* PIOS_INCLUDE_RFM22B_RCVR */ -/** +/** * @} * @} */ diff --git a/flight/pios/common/pios_sbus.c b/flight/pios/common/pios_sbus.c index 0444f15ab..cf9b3579c 100644 --- a/flight/pios/common/pios_sbus.c +++ b/flight/pios/common/pios_sbus.c @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,128 +37,132 @@ /* Forward Declarations */ static int32_t PIOS_SBus_Get(uint32_t rcvr_id, uint8_t channel); static uint16_t PIOS_SBus_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield); + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield); static void PIOS_SBus_Supervisor(uint32_t sbus_id); /* Local Variables */ const struct pios_rcvr_driver pios_sbus_rcvr_driver = { - .read = PIOS_SBus_Get, + .read = PIOS_SBus_Get, }; enum pios_sbus_dev_magic { - PIOS_SBUS_DEV_MAGIC = 0x53427573, + PIOS_SBUS_DEV_MAGIC = 0x53427573, }; struct pios_sbus_state { - uint16_t channel_data[PIOS_SBUS_NUM_INPUTS]; - uint8_t received_data[SBUS_FRAME_LENGTH - 2]; - uint8_t receive_timer; - uint8_t failsafe_timer; - uint8_t frame_found; - uint8_t byte_count; + uint16_t channel_data[PIOS_SBUS_NUM_INPUTS]; + uint8_t received_data[SBUS_FRAME_LENGTH - 2]; + uint8_t receive_timer; + uint8_t failsafe_timer; + uint8_t frame_found; + uint8_t byte_count; }; struct pios_sbus_dev { - enum pios_sbus_dev_magic magic; - const struct pios_sbus_cfg *cfg; - struct pios_sbus_state state; + enum pios_sbus_dev_magic magic; + const struct pios_sbus_cfg *cfg; + struct pios_sbus_state state; }; /* Allocate S.Bus device descriptor */ #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_sbus_dev *PIOS_SBus_Alloc(void) { - struct pios_sbus_dev *sbus_dev; + struct pios_sbus_dev *sbus_dev; - sbus_dev = (struct pios_sbus_dev *)pvPortMalloc(sizeof(*sbus_dev)); - if (!sbus_dev) return(NULL); + sbus_dev = (struct pios_sbus_dev *)pvPortMalloc(sizeof(*sbus_dev)); + if (!sbus_dev) { + return NULL; + } - sbus_dev->magic = PIOS_SBUS_DEV_MAGIC; - return(sbus_dev); + sbus_dev->magic = PIOS_SBUS_DEV_MAGIC; + return sbus_dev; } #else static struct pios_sbus_dev pios_sbus_devs[PIOS_SBUS_MAX_DEVS]; static uint8_t pios_sbus_num_devs; static struct pios_sbus_dev *PIOS_SBus_Alloc(void) { - struct pios_sbus_dev *sbus_dev; + struct pios_sbus_dev *sbus_dev; - if (pios_sbus_num_devs >= PIOS_SBUS_MAX_DEVS) { - return (NULL); - } + if (pios_sbus_num_devs >= PIOS_SBUS_MAX_DEVS) { + return NULL; + } - sbus_dev = &pios_sbus_devs[pios_sbus_num_devs++]; - sbus_dev->magic = PIOS_SBUS_DEV_MAGIC; + sbus_dev = &pios_sbus_devs[pios_sbus_num_devs++]; + sbus_dev->magic = PIOS_SBUS_DEV_MAGIC; - return (sbus_dev); + return sbus_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /* Validate S.Bus device descriptor */ static bool PIOS_SBus_Validate(struct pios_sbus_dev *sbus_dev) { - return (sbus_dev->magic == PIOS_SBUS_DEV_MAGIC); + return sbus_dev->magic == PIOS_SBUS_DEV_MAGIC; } /* Reset channels in case of lost signal or explicit failsafe receiver flag */ static void PIOS_SBus_ResetChannels(struct pios_sbus_state *state) { - for (int i = 0; i < PIOS_SBUS_NUM_INPUTS; i++) { - state->channel_data[i] = PIOS_RCVR_TIMEOUT; - } + for (int i = 0; i < PIOS_SBUS_NUM_INPUTS; i++) { + state->channel_data[i] = PIOS_RCVR_TIMEOUT; + } } /* Reset S.Bus receiver state */ static void PIOS_SBus_ResetState(struct pios_sbus_state *state) { - state->receive_timer = 0; - state->failsafe_timer = 0; - state->frame_found = 0; - PIOS_SBus_ResetChannels(state); + state->receive_timer = 0; + state->failsafe_timer = 0; + state->frame_found = 0; + PIOS_SBus_ResetChannels(state); } /* Initialise S.Bus receiver interface */ int32_t PIOS_SBus_Init(uint32_t *sbus_id, - const struct pios_sbus_cfg *cfg, - const struct pios_com_driver *driver, - uint32_t lower_id) + const struct pios_sbus_cfg *cfg, + const struct pios_com_driver *driver, + uint32_t lower_id) { - PIOS_DEBUG_Assert(sbus_id); - PIOS_DEBUG_Assert(cfg); - PIOS_DEBUG_Assert(driver); + PIOS_DEBUG_Assert(sbus_id); + PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(driver); - struct pios_sbus_dev *sbus_dev; + struct pios_sbus_dev *sbus_dev; - sbus_dev = (struct pios_sbus_dev *)PIOS_SBus_Alloc(); - if (!sbus_dev) goto out_fail; + sbus_dev = (struct pios_sbus_dev *)PIOS_SBus_Alloc(); + if (!sbus_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - sbus_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + sbus_dev->cfg = cfg; - PIOS_SBus_ResetState(&(sbus_dev->state)); + PIOS_SBus_ResetState(&(sbus_dev->state)); - *sbus_id = (uint32_t)sbus_dev; + *sbus_id = (uint32_t)sbus_dev; - /* Enable inverter clock and enable the inverter */ - (*cfg->gpio_clk_func)(cfg->gpio_clk_periph, ENABLE); - GPIO_Init(cfg->inv.gpio, &cfg->inv.init); - GPIO_WriteBit(cfg->inv.gpio, cfg->inv.init.GPIO_Pin, cfg->gpio_inv_enable); + /* Enable inverter clock and enable the inverter */ + (*cfg->gpio_clk_func)(cfg->gpio_clk_periph, ENABLE); + GPIO_Init(cfg->inv.gpio, &cfg->inv.init); + GPIO_WriteBit(cfg->inv.gpio, cfg->inv.init.GPIO_Pin, cfg->gpio_inv_enable); - /* Set comm driver callback */ - (driver->bind_rx_cb)(lower_id, PIOS_SBus_RxInCallback, *sbus_id); + /* Set comm driver callback */ + (driver->bind_rx_cb)(lower_id, PIOS_SBus_RxInCallback, *sbus_id); - if (!PIOS_RTC_RegisterTickCallback(PIOS_SBus_Supervisor, *sbus_id)) { - PIOS_DEBUG_Assert(0); - } + if (!PIOS_RTC_RegisterTickCallback(PIOS_SBus_Supervisor, *sbus_id)) { + PIOS_DEBUG_Assert(0); + } - return 0; + return 0; out_fail: - return -1; + return -1; } /** @@ -170,17 +174,18 @@ out_fail: */ static int32_t PIOS_SBus_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)rcvr_id; + struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)rcvr_id; - if (!PIOS_SBus_Validate(sbus_dev)) - return PIOS_RCVR_INVALID; + if (!PIOS_SBus_Validate(sbus_dev)) { + return PIOS_RCVR_INVALID; + } - /* return error if channel is not available */ - if (channel >= PIOS_SBUS_NUM_INPUTS) { - return PIOS_RCVR_INVALID; - } + /* return error if channel is not available */ + if (channel >= PIOS_SBUS_NUM_INPUTS) { + return PIOS_RCVR_INVALID; + } - return sbus_dev->state.channel_data[channel]; + return sbus_dev->state.channel_data[channel]; } /** @@ -190,111 +195,114 @@ static int32_t PIOS_SBus_Get(uint32_t rcvr_id, uint8_t channel) */ static void PIOS_SBus_UnrollChannels(struct pios_sbus_state *state) { - uint8_t *s = state->received_data; - uint16_t *d = state->channel_data; + uint8_t *s = state->received_data; + uint16_t *d = state->channel_data; -#define F(v,s) (((v) >> (s)) & 0x7ff) +#define F(v, s) (((v) >> (s)) & 0x7ff) - /* unroll channels 1-8 */ - *d++ = F(s[0] | s[1] << 8, 0); - *d++ = F(s[1] | s[2] << 8, 3); - *d++ = F(s[2] | s[3] << 8 | s[4] << 16, 6); - *d++ = F(s[4] | s[5] << 8, 1); - *d++ = F(s[5] | s[6] << 8, 4); - *d++ = F(s[6] | s[7] << 8 | s[8] << 16, 7); - *d++ = F(s[8] | s[9] << 8, 2); - *d++ = F(s[9] | s[10] << 8, 5); + /* unroll channels 1-8 */ + *d++ = F(s[0] | s[1] << 8, 0); + *d++ = F(s[1] | s[2] << 8, 3); + *d++ = F(s[2] | s[3] << 8 | s[4] << 16, 6); + *d++ = F(s[4] | s[5] << 8, 1); + *d++ = F(s[5] | s[6] << 8, 4); + *d++ = F(s[6] | s[7] << 8 | s[8] << 16, 7); + *d++ = F(s[8] | s[9] << 8, 2); + *d++ = F(s[9] | s[10] << 8, 5); - /* unroll channels 9-16 */ - *d++ = F(s[11] | s[12] << 8, 0); - *d++ = F(s[12] | s[13] << 8, 3); - *d++ = F(s[13] | s[14] << 8 | s[15] << 16, 6); - *d++ = F(s[15] | s[16] << 8, 1); - *d++ = F(s[16] | s[17] << 8, 4); - *d++ = F(s[17] | s[18] << 8 | s[19] << 16, 7); - *d++ = F(s[19] | s[20] << 8, 2); - *d++ = F(s[20] | s[21] << 8, 5); + /* unroll channels 9-16 */ + *d++ = F(s[11] | s[12] << 8, 0); + *d++ = F(s[12] | s[13] << 8, 3); + *d++ = F(s[13] | s[14] << 8 | s[15] << 16, 6); + *d++ = F(s[15] | s[16] << 8, 1); + *d++ = F(s[16] | s[17] << 8, 4); + *d++ = F(s[17] | s[18] << 8 | s[19] << 16, 7); + *d++ = F(s[19] | s[20] << 8, 2); + *d++ = F(s[20] | s[21] << 8, 5); - /* unroll discrete channels 17 and 18 */ - *d++ = (s[22] & SBUS_FLAG_DC1) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN; - *d++ = (s[22] & SBUS_FLAG_DC2) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN; + /* unroll discrete channels 17 and 18 */ + *d++ = (s[22] & SBUS_FLAG_DC1) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN; + *d++ = (s[22] & SBUS_FLAG_DC2) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN; } /* Update decoder state processing input byte from the S.Bus stream */ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b) { - /* should not process any data until new frame is found */ - if (!state->frame_found) - return; + /* should not process any data until new frame is found */ + if (!state->frame_found) { + return; + } - if (state->byte_count == 0) { - if (b != SBUS_SOF_BYTE) { - /* discard the whole frame if the 1st byte is not correct */ - state->frame_found = 0; - } else { - /* do not store the SOF byte */ - state->byte_count++; - } - return; - } + if (state->byte_count == 0) { + if (b != SBUS_SOF_BYTE) { + /* discard the whole frame if the 1st byte is not correct */ + state->frame_found = 0; + } else { + /* do not store the SOF byte */ + state->byte_count++; + } + return; + } - /* do not store last frame byte as well */ - if (state->byte_count < SBUS_FRAME_LENGTH - 1) { - /* store next byte */ - state->received_data[state->byte_count - 1] = b; - state->byte_count++; - } else { - if (b == SBUS_EOF_BYTE) { - /* full frame received */ - uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3]; - if (flags & SBUS_FLAG_FL) { - /* frame lost, do not update */ - } else if (flags & SBUS_FLAG_FS) { - /* failsafe flag active */ - PIOS_SBus_ResetChannels(state); - } else { - /* data looking good */ - PIOS_SBus_UnrollChannels(state); - state->failsafe_timer = 0; - } - } else { - /* discard whole frame */ - } + /* do not store last frame byte as well */ + if (state->byte_count < SBUS_FRAME_LENGTH - 1) { + /* store next byte */ + state->received_data[state->byte_count - 1] = b; + state->byte_count++; + } else { + if (b == SBUS_EOF_BYTE) { + /* full frame received */ + uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3]; + if (flags & SBUS_FLAG_FL) { + /* frame lost, do not update */ + } else if (flags & SBUS_FLAG_FS) { + /* failsafe flag active */ + PIOS_SBus_ResetChannels(state); + } else { + /* data looking good */ + PIOS_SBus_UnrollChannels(state); + state->failsafe_timer = 0; + } + } else { + /* discard whole frame */ + } - /* prepare for the next frame */ - state->frame_found = 0; - } + /* prepare for the next frame */ + state->frame_found = 0; + } } /* Comm byte received callback */ static uint16_t PIOS_SBus_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield) + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield) { - struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)context; + struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)context; - bool valid = PIOS_SBus_Validate(sbus_dev); - PIOS_Assert(valid); + bool valid = PIOS_SBus_Validate(sbus_dev); - struct pios_sbus_state *state = &(sbus_dev->state); + PIOS_Assert(valid); - /* process byte(s) and clear receive timer */ - for (uint8_t i = 0; i < buf_len; i++) { - PIOS_SBus_UpdateState(state, buf[i]); - state->receive_timer = 0; - } + struct pios_sbus_state *state = &(sbus_dev->state); - /* Always signal that we can accept another byte */ - if (headroom) - *headroom = SBUS_FRAME_LENGTH; + /* process byte(s) and clear receive timer */ + for (uint8_t i = 0; i < buf_len; i++) { + PIOS_SBus_UpdateState(state, buf[i]); + state->receive_timer = 0; + } - /* We never need a yield */ - *need_yield = false; + /* Always signal that we can accept another byte */ + if (headroom) { + *headroom = SBUS_FRAME_LENGTH; + } - /* Always indicate that all bytes were consumed */ - return buf_len; + /* We never need a yield */ + *need_yield = false; + + /* Always indicate that all bytes were consumed */ + return buf_len; } /** @@ -311,30 +319,31 @@ static uint16_t PIOS_SBus_RxInCallback(uint32_t context, */ static void PIOS_SBus_Supervisor(uint32_t sbus_id) { - struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)sbus_id; + struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)sbus_id; - bool valid = PIOS_SBus_Validate(sbus_dev); - PIOS_Assert(valid); + bool valid = PIOS_SBus_Validate(sbus_dev); - struct pios_sbus_state *state = &(sbus_dev->state); + PIOS_Assert(valid); - /* waiting for new frame if no bytes were received in 3.2ms */ - if (++state->receive_timer > 2) { - state->frame_found = 1; - state->byte_count = 0; - state->receive_timer = 0; - } + struct pios_sbus_state *state = &(sbus_dev->state); - /* activate failsafe if no frames have arrived in 102.4ms */ - if (++state->failsafe_timer > 64) { - PIOS_SBus_ResetChannels(state); - state->failsafe_timer = 0; - } + /* waiting for new frame if no bytes were received in 3.2ms */ + if (++state->receive_timer > 2) { + state->frame_found = 1; + state->byte_count = 0; + state->receive_timer = 0; + } + + /* activate failsafe if no frames have arrived in 102.4ms */ + if (++state->failsafe_timer > 64) { + PIOS_SBus_ResetChannels(state); + state->failsafe_timer = 0; + } } #endif /* PIOS_INCLUDE_SBUS */ -/** +/** * @} * @} */ diff --git a/flight/pios/common/pios_sdcard.c b/flight/pios/common/pios_sdcard.c index 1ae2ea50e..e5ba0dbf8 100644 --- a/flight/pios/common/pios_sdcard.c +++ b/flight/pios/common/pios_sdcard.c @@ -6,26 +6,26 @@ * @brief Code to deal with reading and writing to flash cards * @{ * - * @file pios_sdcard.c + * @file pios_sdcard.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief Sets up basic system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,765 +39,771 @@ uint8_t PIOS_SDCARD_Sector[SECTOR_SIZE]; /* Local Definitions */ #if !defined(SDCARD_MUTEX_TAKE) -#define SDCARD_MUTEX_TAKE {} -#define SDCARD_MUTEX_GIVE {} +#define SDCARD_MUTEX_TAKE {} +#define SDCARD_MUTEX_GIVE {} #endif /* Definitions for MMC/SDC command */ -#define SDCMD_GO_IDLE_STATE (0x40+0) -#define SDCMD_GO_IDLE_STATE_CRC 0x95 +#define SDCMD_GO_IDLE_STATE (0x40 + 0) +#define SDCMD_GO_IDLE_STATE_CRC 0x95 -#define SDCMD_SEND_OP_COND (0x40+1) -#define SDCMD_SEND_OP_COND_CRC 0xf9 +#define SDCMD_SEND_OP_COND (0x40 + 1) +#define SDCMD_SEND_OP_COND_CRC 0xf9 -#define SDCMD_SEND_OP_COND_SDC (0xC0+41) -#define SDCMD_SEND_OP_COND_SDC_CRC 0xff +#define SDCMD_SEND_OP_COND_SDC (0xC0 + 41) +#define SDCMD_SEND_OP_COND_SDC_CRC 0xff -#define SDCMD_READ_OCR (0x40+58) -#define SDCMD_READ_OCR_CRC 0xff +#define SDCMD_READ_OCR (0x40 + 58) +#define SDCMD_READ_OCR_CRC 0xff -#define SDCMD_APP_CMD (0x40+55) -#define SDCMD_APP_CMD_CRC 0xff +#define SDCMD_APP_CMD (0x40 + 55) +#define SDCMD_APP_CMD_CRC 0xff -#define SDCMD_SEND_IF_COND (0x40+8) -#define SDCMD_SEND_IF_COND_CRC 0x87 +#define SDCMD_SEND_IF_COND (0x40 + 8) +#define SDCMD_SEND_IF_COND_CRC 0x87 -#define SDCMD_SEND_CSD (0x40+9) -#define SDCMD_SEND_CSD_CRC 0xff +#define SDCMD_SEND_CSD (0x40 + 9) +#define SDCMD_SEND_CSD_CRC 0xff -#define SDCMD_SEND_CID (0x40+10) -#define SDCMD_SEND_CID_CRC 0xff +#define SDCMD_SEND_CID (0x40 + 10) +#define SDCMD_SEND_CID_CRC 0xff -#define SDCMD_SEND_STATUS (0x40+13) -#define SDCMD_SEND_STATUS_CRC 0xaf +#define SDCMD_SEND_STATUS (0x40 + 13) +#define SDCMD_SEND_STATUS_CRC 0xaf -#define SDCMD_READ_SINGLE_BLOCK (0x40+17) -#define SDCMD_READ_SINGLE_BLOCK_CRC 0xff +#define SDCMD_READ_SINGLE_BLOCK (0x40 + 17) +#define SDCMD_READ_SINGLE_BLOCK_CRC 0xff -#define SDCMD_SET_BLOCKLEN (0x40+16) -#define SDCMD_SET_BLOCKLEN_CRC 0xff +#define SDCMD_SET_BLOCKLEN (0x40 + 16) +#define SDCMD_SET_BLOCKLEN_CRC 0xff -#define SDCMD_WRITE_SINGLE_BLOCK (0x40+24) -#define SDCMD_WRITE_SINGLE_BLOCK_CRC 0xff +#define SDCMD_WRITE_SINGLE_BLOCK (0x40 + 24) +#define SDCMD_WRITE_SINGLE_BLOCK_CRC 0xff /* Card type flags (CardType) */ -#define CT_MMC 0x01 -#define CT_SD1 0x02 -#define CT_SD2 0x04 -#define CT_SDC (CT_SD1|CT_SD2) -#define CT_BLOCK 0x08 +#define CT_MMC 0x01 +#define CT_SD1 0x02 +#define CT_SD2 0x04 +#define CT_SDC (CT_SD1 | CT_SD2) +#define CT_BLOCK 0x08 static uint8_t CardType; static int32_t sdcard_mounted; static uint32_t PIOS_SDCARD_SPI; /** -* Initialises SPI pins and peripheral to access MMC/SD Card -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ + * Initialises SPI pins and peripheral to access MMC/SD Card + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ int32_t PIOS_SDCARD_Init(uint32_t spi_id) { - SDCARD_MUTEX_TAKE; + SDCARD_MUTEX_TAKE; - sdcard_mounted = 0; + sdcard_mounted = 0; - PIOS_SDCARD_SPI = spi_id; + PIOS_SDCARD_SPI = spi_id; - /* Init SPI port for slow frequency access (ca. 0.3 MBit/s) */ - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_256); + /* Init SPI port for slow frequency access (ca. 0.3 MBit/s) */ + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_256); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xFF); - SDCARD_MUTEX_GIVE; + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xFF); + SDCARD_MUTEX_GIVE; - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Connects to SD Card -* \return < 0 if initialisation sequence failed -*/ + * Connects to SD Card + * \return < 0 if initialisation sequence failed + */ int32_t PIOS_SDCARD_PowerOn(void) { - int32_t status; - int i; + int32_t status; + int i; - SDCARD_MUTEX_TAKE; - /* Ensure that chip select line deactivated */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + SDCARD_MUTEX_TAKE; + /* Ensure that chip select line deactivated */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* Init SPI port for slow frequency access (ca. 0.3 MBit/s) */ - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_256); + /* Init SPI port for slow frequency access (ca. 0.3 MBit/s) */ + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_256); - /* Send 80 clock cycles to start up */ - for (i = 0; i < 10; ++i) { - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - } + /* Send 80 clock cycles to start up */ + for (i = 0; i < 10; ++i) { + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + } - /* Activate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ + /* Activate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ - /* wait for 1 mS */ - PIOS_DELAY_WaituS(1000); + /* wait for 1 mS */ + PIOS_DELAY_WaituS(1000); - /* Send CMD0 to reset the media */ - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_GO_IDLE_STATE, 0, SDCMD_GO_IDLE_STATE_CRC)) < 0) { - goto error; - } + /* Send CMD0 to reset the media */ + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_GO_IDLE_STATE, 0, SDCMD_GO_IDLE_STATE_CRC)) < 0) { + goto error; + } - CardType = 0; + CardType = 0; - /* A card is detected, what type is it? Use SEND_IF_COND (CMD8) to find out */ - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_IF_COND, 0x1AA, SDCMD_SEND_IF_COND_CRC)) == 0x01aa) { - /* SDHC Card Detected. */ + /* A card is detected, what type is it? Use SEND_IF_COND (CMD8) to find out */ + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_IF_COND, 0x1AA, SDCMD_SEND_IF_COND_CRC)) == 0x01aa) { + /* SDHC Card Detected. */ - /* We now check to see if we should use block mode or byte mode. Command is SEND_OP_COND_SDC (ACMD41) with HCS (bit 30) set */ - for (i = 0; i < 16384; i++) - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND_SDC, 0x01 << 30, SDCMD_SEND_OP_COND_SDC_CRC)) == 0) - break; + /* We now check to see if we should use block mode or byte mode. Command is SEND_OP_COND_SDC (ACMD41) with HCS (bit 30) set */ + for (i = 0; i < 16384; i++) { + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND_SDC, 0x01 << 30, SDCMD_SEND_OP_COND_SDC_CRC)) == 0) { + break; + } + } - if (i < 16384) { - status = PIOS_SDCARD_SendSDCCmd(SDCMD_READ_OCR, 0, SDCMD_READ_OCR_CRC); - CardType = ((status >> 24) & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2; - status = 0; - } else { - /* We waited long enough! */ - status = -2; - } - } else { - /* Card is SDv1 or MMC. */ + if (i < 16384) { + status = PIOS_SDCARD_SendSDCCmd(SDCMD_READ_OCR, 0, SDCMD_READ_OCR_CRC); + CardType = ((status >> 24) & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2; + status = 0; + } else { + /* We waited long enough! */ + status = -2; + } + } else { + /* Card is SDv1 or MMC. */ - /* MMC will accept ACMD41 (SDv1 won't and requires CMD1) so send ACMD41 first and then CMD1 if that fails */ - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND_SDC, 0, SDCMD_SEND_OP_COND_SDC_CRC)) <= 1) { - CardType = CT_SD1; - } else { - CardType = CT_MMC; - } + /* MMC will accept ACMD41 (SDv1 won't and requires CMD1) so send ACMD41 first and then CMD1 if that fails */ + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND_SDC, 0, SDCMD_SEND_OP_COND_SDC_CRC)) <= 1) { + CardType = CT_SD1; + } else { + CardType = CT_MMC; + } - for (i = 0; i < 16384; i++) { - if (CardType == CT_SD1) { - status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND_SDC, 0, SDCMD_SEND_OP_COND_SDC_CRC); - } else { - status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND, 0, SDCMD_SEND_OP_COND_CRC); - } + for (i = 0; i < 16384; i++) { + if (CardType == CT_SD1) { + status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND_SDC, 0, SDCMD_SEND_OP_COND_SDC_CRC); + } else { + status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_OP_COND, 0, SDCMD_SEND_OP_COND_CRC); + } - if (status < 0) { - break; - } - if (status == 0) { - break; - } - } + if (status < 0) { + break; + } + if (status == 0) { + break; + } + } - /* The block size should already be 512 bytes but re-initialize just in case (ignore if it fails) */ - PIOS_SDCARD_SendSDCCmd(SDCMD_SET_BLOCKLEN, 512, SDCMD_SEND_OP_COND_CRC); - } + /* The block size should already be 512 bytes but re-initialize just in case (ignore if it fails) */ + PIOS_SDCARD_SendSDCCmd(SDCMD_SET_BLOCKLEN, 512, SDCMD_SEND_OP_COND_CRC); + } - if (i == 16384 || CardType == 0) { - /* The last loop timed out or the cardtype was not detected... */ - status = -2; - } + if (i == 16384 || CardType == 0) { + /* The last loop timed out or the cardtype was not detected... */ + status = -2; + } error: - /* Deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* Deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* Send dummy byte once deactivated to drop cards DO */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - SDCARD_MUTEX_GIVE; - return status; /* Status should be 0 if nothing went wrong! */ + /* Send dummy byte once deactivated to drop cards DO */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + SDCARD_MUTEX_GIVE; + return status; /* Status should be 0 if nothing went wrong! */ } /** -* Disconnects from SD Card -* \return < 0 on errors -*/ + * Disconnects from SD Card + * \return < 0 on errors + */ int32_t PIOS_SDCARD_PowerOff(void) { - return 0; // no error + return 0; // no error } /** -* If SD card was previously available: Checks if the SD Card is still -* available by sending the STATUS command.
-* This takes ca. 10 uS -* -* If SD card was previously not available: Checks if the SD Card is -* available by sending the IDLE command at low speed.
-* This takes ca. 500 uS!
-* Once we got a positive response, SDCARD_PowerOn() will be -* called by this function to initialize the card completely. -* -* Example for Connection/Disconnection detection: -* \code -* // this function is called each second from a low-priority task -* // If multiple tasks are accessing the SD card, add a semaphore/mutex -* // to avoid IO access collisions with other tasks! -* u8 sdcard_available; -* int32_t CheckSDCard(void) -* { -* // check if SD card is available -* // High speed access if the card was previously available -* u8 prev_sdcard_available = sdcard_available; -* sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); -* -* if(sdcard_available && !prev_sdcard_available) { -* // SD Card has been connected -* -* // now it's possible to read/write sectors -* -* } else if( !sdcard_available && prev_sdcard_available ) { -* // SD Card has been disconnected -* -* // here you can notify your application about this state -* } -* -* return 0; // no error -* } -* \endcode -* \param[in] was_available should only be set if the SD card was previously available -* \return 0 if no response from SD Card -* \return 1 if SD card is accessible -*/ + * If SD card was previously available: Checks if the SD Card is still + * available by sending the STATUS command.
+ * This takes ca. 10 uS + * + * If SD card was previously not available: Checks if the SD Card is + * available by sending the IDLE command at low speed.
+ * This takes ca. 500 uS!
+ * Once we got a positive response, SDCARD_PowerOn() will be + * called by this function to initialize the card completely. + * + * Example for Connection/Disconnection detection: + * \code + * // this function is called each second from a low-priority task + * // If multiple tasks are accessing the SD card, add a semaphore/mutex + * // to avoid IO access collisions with other tasks! + * u8 sdcard_available; + * int32_t CheckSDCard(void) + * { + * // check if SD card is available + * // High speed access if the card was previously available + * u8 prev_sdcard_available = sdcard_available; + * sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); + * + * if(sdcard_available && !prev_sdcard_available) { + * // SD Card has been connected + * + * // now it's possible to read/write sectors + * + * } else if( !sdcard_available && prev_sdcard_available ) { + * // SD Card has been disconnected + * + * // here you can notify your application about this state + * } + * + * return 0; // no error + * } + * \endcode + * \param[in] was_available should only be set if the SD card was previously available + * \return 0 if no response from SD Card + * \return 1 if SD card is accessible + */ int32_t PIOS_SDCARD_CheckAvailable(uint8_t was_available) { - int32_t ret; - SDCARD_MUTEX_TAKE; + int32_t ret; - if (was_available) { - /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ - /* This is required for the case that the SPI port is shared with other devices */ - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); + SDCARD_MUTEX_TAKE; - /* Activate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ + if (was_available) { + /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ + /* This is required for the case that the SPI port is shared with other devices */ + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); - /* Send STATUS command to check if media is available */ - ret = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_STATUS, 0, SDCMD_SEND_STATUS_CRC); - } else { - /* Ensure that SPI interface is clocked at low speed */ - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_256); + /* Activate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ - /* Deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* send 80 clock cycles to start up */ - uint8_t i; - for (i = 0; i < 10; ++i) { - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - } + /* Send STATUS command to check if media is available */ + ret = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_STATUS, 0, SDCMD_SEND_STATUS_CRC); + } else { + /* Ensure that SPI interface is clocked at low speed */ + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_256); - /* Activate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ + /* Deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* send 80 clock cycles to start up */ + uint8_t i; + for (i = 0; i < 10; ++i) { + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + } - /* Send CMD0 to reset the media */ - if ((ret = (PIOS_SDCARD_SendSDCCmd(SDCMD_GO_IDLE_STATE, 0, SDCMD_GO_IDLE_STATE_CRC))) < 0) { - goto not_available; - } + /* Activate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ - /* Deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* Send CMD0 to reset the media */ + if ((ret = (PIOS_SDCARD_SendSDCCmd(SDCMD_GO_IDLE_STATE, 0, SDCMD_GO_IDLE_STATE_CRC))) < 0) { + goto not_available; + } - SDCARD_MUTEX_GIVE; - /* Run power-on sequence (negative return = not available) */ - ret = PIOS_SDCARD_PowerOn(); - } + /* Deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + + SDCARD_MUTEX_GIVE; + /* Run power-on sequence (negative return = not available) */ + ret = PIOS_SDCARD_PowerOn(); + } not_available: - /* Deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* Send dummy byte once deactivated to drop cards DO */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - SDCARD_MUTEX_GIVE; + /* Deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* Send dummy byte once deactivated to drop cards DO */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + SDCARD_MUTEX_GIVE; - return (ret == 0) ? 1 : 0; /* 1 = available, 0 = not available. */ + return (ret == 0) ? 1 : 0; /* 1 = available, 0 = not available. */ } /** -* Sends command to SD card -* \param[in] cmd SD card command -* \param[in] addr 32bit address -* \param[in] crc precalculated CRC -* \return >= 0x00 if command has been sent successfully (contains received byte) -* \return -1 if no response from SD Card (timeout) -*/ + * Sends command to SD card + * \param[in] cmd SD card command + * \param[in] addr 32bit address + * \param[in] crc precalculated CRC + * \return >= 0x00 if command has been sent successfully (contains received byte) + * \return -1 if no response from SD Card (timeout) + */ int32_t PIOS_SDCARD_SendSDCCmd(uint8_t cmd, uint32_t addr, uint8_t crc) { - int i; - int32_t ret; + int i; + int32_t ret; - if (cmd & 0x80) { /* ACMD is the command sequence of CMD55-CMD */ - cmd &= 0x7F; - ret = PIOS_SDCARD_SendSDCCmd(SDCMD_APP_CMD, 0, SDCMD_APP_CMD_CRC); - if (ret > 1) { - return ret; - } - } + if (cmd & 0x80) { /* ACMD is the command sequence of CMD55-CMD */ + cmd &= 0x7F; + ret = PIOS_SDCARD_SendSDCCmd(SDCMD_APP_CMD, 0, SDCMD_APP_CMD_CRC); + if (ret > 1) { + return ret; + } + } - /* Activate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ + /* Activate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 0); /* spi, pin_value */ - /* Transfer to card */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (uint8_t) cmd); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 24) & 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 16) & 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 8) & 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 0) & 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, crc); + /* Transfer to card */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (uint8_t)cmd); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 24) & 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 16) & 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 8) & 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, (addr >> 0) & 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, crc); - uint8_t timeout = 0; + uint8_t timeout = 0; - if (cmd == SDCMD_SEND_STATUS) { - /* One dummy read */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + if (cmd == SDCMD_SEND_STATUS) { + /* One dummy read */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* Read two bytes (only last value will be returned) */ - ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Read two bytes (only last value will be returned) */ + ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* All-one read: we expect that SD card is not connected: notify timeout! */ - if (ret == 0xff) { - timeout = 1; - } - } else { - /* Wait for standard R1 response */ - for (i = 0; i < 8; ++i) { - if (!((ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff)) & 0x80)) { - break; - } - } - if (i == 8) { - timeout = 1; - } - } + /* All-one read: we expect that SD card is not connected: notify timeout! */ + if (ret == 0xff) { + timeout = 1; + } + } else { + /* Wait for standard R1 response */ + for (i = 0; i < 8; ++i) { + if (!((ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff)) & 0x80)) { + break; + } + } + if (i == 8) { + timeout = 1; + } + } - if ((cmd == SDCMD_SEND_IF_COND || cmd == SDCMD_READ_OCR) && timeout != 1) { - /* This is a 4 byte R3 or R7 response. */ - ret = (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 24); - ret |= (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 16); - ret |= (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 8); - ret |= (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 0); - } + if ((cmd == SDCMD_SEND_IF_COND || cmd == SDCMD_READ_OCR) && timeout != 1) { + /* This is a 4 byte R3 or R7 response. */ + ret = (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 24); + ret |= (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 16); + ret |= (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 8); + ret |= (PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff) << 0); + } - /* Required clocking (see spec) */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Required clocking (see spec) */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* Deactivate chip-select on timeout, and return error code */ - if (timeout) { - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* Deactivate chip-select on timeout, and return error code */ + if (timeout) { + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* Send dummy byte once deactivated to drop cards DO */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - return -1; - } + /* Send dummy byte once deactivated to drop cards DO */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + return -1; + } - /* Else return received value - don't deactivate chip select or mutex (for continuous access) */ - return ret; + /* Else return received value - don't deactivate chip select or mutex (for continuous access) */ + return ret; } /** -* Reads 512 bytes from selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ -int32_t PIOS_SDCARD_SectorRead(uint32_t sector, uint8_t * buffer) + * Reads 512 bytes from selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ +int32_t PIOS_SDCARD_SectorRead(uint32_t sector, uint8_t *buffer) { - int32_t status; - int i; - if (!(CardType & CT_BLOCK)) { - sector *= 512; - } + int32_t status; + int i; - SDCARD_MUTEX_TAKE; + if (!(CardType & CT_BLOCK)) { + sector *= 512; + } - /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ - /* this is required for the case that the SPI port is shared with other devices */ - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); + SDCARD_MUTEX_TAKE; - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_READ_SINGLE_BLOCK, sector, SDCMD_READ_SINGLE_BLOCK_CRC))) { - status = (status < 0) ? -256 : status; /* return timeout indicator or error flags */ - goto error; - } + /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ + /* this is required for the case that the SPI port is shared with other devices */ + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); - /* Wait for start token of the data block */ - for (i = 0; i < 65536; ++i) { // TODO: check if sufficient - uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - if (ret != 0xff) - break; - } + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_READ_SINGLE_BLOCK, sector, SDCMD_READ_SINGLE_BLOCK_CRC))) { + status = (status < 0) ? -256 : status; /* return timeout indicator or error flags */ + goto error; + } - if (i == 65536) { - status = -257; - goto error; - } + /* Wait for start token of the data block */ + for (i = 0; i < 65536; ++i) { // TODO: check if sufficient + uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + if (ret != 0xff) { + break; + } + } - /* Read 512 bytes via DMA */ - PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, NULL, buffer, 512, NULL); + if (i == 65536) { + status = -257; + goto error; + } - /* Read (and ignore) CRC */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Read 512 bytes via DMA */ + PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, NULL, buffer, 512, NULL); - /* Required for clocking (see spec) */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Read (and ignore) CRC */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + + /* Required for clocking (see spec) */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); error: - /* Deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); // spi, pin_value + /* Deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); // spi, pin_value - /* Send dummy byte once deactivated to drop cards DO */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - SDCARD_MUTEX_GIVE; - return status; + /* Send dummy byte once deactivated to drop cards DO */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + SDCARD_MUTEX_GIVE; + return status; } /** -* Writes 512 bytes into selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if write operation not accepted -* \return -258 if timeout during write operation -*/ -int32_t PIOS_SDCARD_SectorWrite(uint32_t sector, uint8_t * buffer) + * Writes 512 bytes into selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if write operation not accepted + * \return -258 if timeout during write operation + */ +int32_t PIOS_SDCARD_SectorWrite(uint32_t sector, uint8_t *buffer) { - int32_t status; - int i; + int32_t status; + int i; - SDCARD_MUTEX_TAKE; + SDCARD_MUTEX_TAKE; - if (!(CardType & CT_BLOCK)) { - sector *= 512; - } + if (!(CardType & CT_BLOCK)) { + sector *= 512; + } - /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ - /* This is required for the case that the SPI port is shared with other devices */ - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); + /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ + /* This is required for the case that the SPI port is shared with other devices */ + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_WRITE_SINGLE_BLOCK, sector, SDCMD_WRITE_SINGLE_BLOCK_CRC))) { - status = (status < 0) ? -256 : status; /* Return timeout indicator or error flags */ - goto error; - } + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_WRITE_SINGLE_BLOCK, sector, SDCMD_WRITE_SINGLE_BLOCK_CRC))) { + status = (status < 0) ? -256 : status; /* Return timeout indicator or error flags */ + goto error; + } - /* Send start token */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xfe); + /* Send start token */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xfe); - /* Send 512 bytes of data via DMA */ - PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, buffer, NULL, 512, NULL); + /* Send 512 bytes of data via DMA */ + PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, buffer, NULL, 512, NULL); - /* Send CRC */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Send CRC */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* Read response */ - uint8_t response = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - if ((response & 0x0f) != 0x5) { - status = -257; - goto error; - } + /* Read response */ + uint8_t response = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + if ((response & 0x0f) != 0x5) { + status = -257; + goto error; + } - /* Wait for write completion */ - for (i = 0; i < 32 * 65536; ++i) { /* TODO: check if sufficient */ - uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - if (ret != 0x00) { - break; - } - } - if (i == 32 * 65536) { - status = -258; - goto error; - } + /* Wait for write completion */ + for (i = 0; i < 32 * 65536; ++i) { /* TODO: check if sufficient */ + uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + if (ret != 0x00) { + break; + } + } + if (i == 32 * 65536) { + status = -258; + goto error; + } - /* Required for clocking (see spec) */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Required for clocking (see spec) */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); error: - /* Deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* Send dummy byte once deactivated to drop cards DO */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* Send dummy byte once deactivated to drop cards DO */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - SDCARD_MUTEX_GIVE; + SDCARD_MUTEX_GIVE; - return status; + return status; } /** -* Reads the CID informations from SD Card -* \param[in] *cid pointer to buffer which holds the CID informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ -int32_t PIOS_SDCARD_CIDRead(SDCARDCidTypeDef * cid) + * Reads the CID informations from SD Card + * \param[in] *cid pointer to buffer which holds the CID informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ +int32_t PIOS_SDCARD_CIDRead(SDCARDCidTypeDef *cid) { - int32_t status; - int i; + int32_t status; + int i; - /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ - /* This is required for the case that the SPI port is shared with other devices */ - SDCARD_MUTEX_TAKE; + /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ + /* This is required for the case that the SPI port is shared with other devices */ + SDCARD_MUTEX_TAKE; - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_CID, 0, SDCMD_SEND_CID_CRC))) { - status = (status < 0) ? -256 : status; /* return timeout indicator or error flags */ - goto error; - } + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_CID, 0, SDCMD_SEND_CID_CRC))) { + status = (status < 0) ? -256 : status; /* return timeout indicator or error flags */ + goto error; + } - /* Wait for start token of the data block */ - for (i = 0; i < 65536; ++i) { /* TODO: check if sufficient */ - uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - if (ret != 0xff) { - break; - } - } - if (i == 65536) { - status = -257; - } + /* Wait for start token of the data block */ + for (i = 0; i < 65536; ++i) { /* TODO: check if sufficient */ + uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + if (ret != 0xff) { + break; + } + } + if (i == 65536) { + status = -257; + } - /* Read 16 bytes via DMA */ - uint8_t cid_buffer[16]; - PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, NULL, cid_buffer, 16, NULL); + /* Read 16 bytes via DMA */ + uint8_t cid_buffer[16]; + PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, NULL, cid_buffer, 16, NULL); - /* Read (and ignore) CRC */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Read (and ignore) CRC */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* Required for clocking (see spec) */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Required for clocking (see spec) */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* Sort returned informations into CID structure */ - /* From STM Mass Storage example */ - /* Byte 0 */ - cid->ManufacturerID = cid_buffer[0]; - /* Byte 1 */ - cid->OEM_AppliID = cid_buffer[1] << 8; - /* Byte 2 */ - cid->OEM_AppliID |= cid_buffer[2]; - /* Byte 3..7 */ - for (i = 0; i < 5; ++i) - cid->ProdName[i] = cid_buffer[3 + i]; - cid->ProdName[5] = 0; /* string terminator */ - /* Byte 8 */ - cid->ProdRev = cid_buffer[8]; - /* Byte 9 */ - cid->ProdSN = cid_buffer[9] << 24; - /* Byte 10 */ - cid->ProdSN |= cid_buffer[10] << 16; - /* Byte 11 */ - cid->ProdSN |= cid_buffer[11] << 8; - /* Byte 12 */ - cid->ProdSN |= cid_buffer[12]; - /* Byte 13 */ - cid->Reserved1 |= (cid_buffer[13] & 0xF0) >> 4; - /* Byte 14 */ - cid->ManufactDate = (cid_buffer[13] & 0x0F) << 8; - /* Byte 15 */ - cid->ManufactDate |= cid_buffer[14]; - /* Byte 16 */ - cid->msd_CRC = (cid_buffer[15] & 0xFE) >> 1; - cid->Reserved2 = 1; + /* Sort returned informations into CID structure */ + /* From STM Mass Storage example */ + /* Byte 0 */ + cid->ManufacturerID = cid_buffer[0]; + /* Byte 1 */ + cid->OEM_AppliID = cid_buffer[1] << 8; + /* Byte 2 */ + cid->OEM_AppliID |= cid_buffer[2]; + /* Byte 3..7 */ + for (i = 0; i < 5; ++i) { + cid->ProdName[i] = cid_buffer[3 + i]; + } + cid->ProdName[5] = 0; /* string terminator */ + /* Byte 8 */ + cid->ProdRev = cid_buffer[8]; + /* Byte 9 */ + cid->ProdSN = cid_buffer[9] << 24; + /* Byte 10 */ + cid->ProdSN |= cid_buffer[10] << 16; + /* Byte 11 */ + cid->ProdSN |= cid_buffer[11] << 8; + /* Byte 12 */ + cid->ProdSN |= cid_buffer[12]; + /* Byte 13 */ + cid->Reserved1 |= (cid_buffer[13] & 0xF0) >> 4; + /* Byte 14 */ + cid->ManufactDate = (cid_buffer[13] & 0x0F) << 8; + /* Byte 15 */ + cid->ManufactDate |= cid_buffer[14]; + /* Byte 16 */ + cid->msd_CRC = (cid_buffer[15] & 0xFE) >> 1; + cid->Reserved2 = 1; error: - /* deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* Send dummy byte once deactivated to drop cards DO */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - SDCARD_MUTEX_GIVE; + /* deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* Send dummy byte once deactivated to drop cards DO */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + SDCARD_MUTEX_GIVE; - return status; + return status; } /** -* Reads the CSD informations from SD Card -* \param[in] *csd pointer to buffer which holds the CSD informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ -int32_t PIOS_SDCARD_CSDRead(SDCARDCsdTypeDef * csd) + * Reads the CSD informations from SD Card + * \param[in] *csd pointer to buffer which holds the CSD informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ +int32_t PIOS_SDCARD_CSDRead(SDCARDCsdTypeDef *csd) { - int32_t status; - int i; + int32_t status; + int i; - SDCARD_MUTEX_TAKE; + SDCARD_MUTEX_TAKE; - /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ - /* This is required for the case that the SPI port is shared with other devices */ - PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); + /* Init SPI port for fast frequency access (ca. 18 MBit/s) */ + /* This is required for the case that the SPI port is shared with other devices */ + PIOS_SPI_SetClockSpeed(PIOS_SDCARD_SPI, PIOS_SPI_PRESCALER_4); - if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_CSD, 0, SDCMD_SEND_CSD_CRC))) { - status = (status < 0) ? -256 : status; /* Return timeout indicator or error flags */ - goto error; - } - // wait for start token of the data block - for (i = 0; i < 65536; ++i) { /* TODO: check if sufficient */ - uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - if (ret != 0xff) { - break; - } - } - if (i == 65536) { - status = -257; - goto error; - } + if ((status = PIOS_SDCARD_SendSDCCmd(SDCMD_SEND_CSD, 0, SDCMD_SEND_CSD_CRC))) { + status = (status < 0) ? -256 : status; /* Return timeout indicator or error flags */ + goto error; + } + // wait for start token of the data block + for (i = 0; i < 65536; ++i) { /* TODO: check if sufficient */ + uint8_t ret = PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + if (ret != 0xff) { + break; + } + } + if (i == 65536) { + status = -257; + goto error; + } - /* Read 16 bytes via DMA */ - uint8_t csd_buffer[16]; - PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, NULL, csd_buffer, 16, NULL); + /* Read 16 bytes via DMA */ + uint8_t csd_buffer[16]; + PIOS_SPI_TransferBlock(PIOS_SDCARD_SPI, NULL, csd_buffer, 16, NULL); - /* Read (and ignore) CRC */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Read (and ignore) CRC */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* Required for clocking (see spec) */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + /* Required for clocking (see spec) */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - /* Sort returned informations into CSD structure */ - /* from STM Mass Storage example */ - /* Byte 0 */ - csd->CSDStruct = (csd_buffer[0] & 0xC0) >> 6; - csd->SysSpecVersion = (csd_buffer[0] & 0x3C) >> 2; - csd->Reserved1 = csd_buffer[0] & 0x03; - /* Byte 1 */ - csd->TAAC = csd_buffer[1]; - /* Byte 2 */ - csd->NSAC = csd_buffer[2]; - /* Byte 3 */ - csd->MaxBusClkFrec = csd_buffer[3]; - /* Byte 4 */ - csd->CardComdClasses = csd_buffer[4] << 4; - /* Byte 5 */ - csd->CardComdClasses |= (csd_buffer[5] & 0xF0) >> 4; - csd->RdBlockLen = csd_buffer[5] & 0x0F; - /* Byte 6 */ - csd->PartBlockRead = (csd_buffer[6] & 0x80) >> 7; - csd->WrBlockMisalign = (csd_buffer[6] & 0x40) >> 6; - csd->RdBlockMisalign = (csd_buffer[6] & 0x20) >> 5; - csd->DSRImpl = (csd_buffer[6] & 0x10) >> 4; - csd->Reserved2 = 0; /* Reserved */ - csd->DeviceSize = (csd_buffer[6] & 0x03) << 10; - /* Byte 7 */ - csd->DeviceSize |= (csd_buffer[7]) << 2; - /* Byte 8 */ - csd->DeviceSize |= (csd_buffer[8] & 0xC0) >> 6; - csd->MaxRdCurrentVDDMin = (csd_buffer[8] & 0x38) >> 3; - csd->MaxRdCurrentVDDMax = (csd_buffer[8] & 0x07); - /* Byte 9 */ - csd->MaxWrCurrentVDDMin = (csd_buffer[9] & 0xE0) >> 5; - csd->MaxWrCurrentVDDMax = (csd_buffer[9] & 0x1C) >> 2; - csd->DeviceSizeMul = (csd_buffer[9] & 0x03) << 1; - /* Byte 10 */ - csd->DeviceSizeMul |= (csd_buffer[10] & 0x80) >> 7; - csd->EraseGrSize = (csd_buffer[10] & 0x7C) >> 2; - csd->EraseGrMul = (csd_buffer[10] & 0x03) << 3; - /* Byte 11 */ - csd->EraseGrMul |= (csd_buffer[11] & 0xE0) >> 5; - csd->WrProtectGrSize = (csd_buffer[11] & 0x1F); - /* Byte 12 */ - csd->WrProtectGrEnable = (csd_buffer[12] & 0x80) >> 7; - csd->ManDeflECC = (csd_buffer[12] & 0x60) >> 5; - csd->WrSpeedFact = (csd_buffer[12] & 0x1C) >> 2; - csd->MaxWrBlockLen = (csd_buffer[12] & 0x03) << 2; - /* Byte 13 */ - csd->MaxWrBlockLen |= (csd_buffer[13] & 0xc0) >> 6; - csd->WriteBlockPaPartial = (csd_buffer[13] & 0x20) >> 5; - csd->Reserved3 = 0; - csd->ContentProtectAppli = (csd_buffer[13] & 0x01); - /* Byte 14 */ - csd->FileFormatGrouop = (csd_buffer[14] & 0x80) >> 7; - csd->CopyFlag = (csd_buffer[14] & 0x40) >> 6; - csd->PermWrProtect = (csd_buffer[14] & 0x20) >> 5; - csd->TempWrProtect = (csd_buffer[14] & 0x10) >> 4; - csd->FileFormat = (csd_buffer[14] & 0x0C) >> 2; - csd->ECC = (csd_buffer[14] & 0x03); - /* Byte 15 */ - csd->msd_CRC = (csd_buffer[15] & 0xFE) >> 1; - csd->Reserved4 = 1; + /* Sort returned informations into CSD structure */ + /* from STM Mass Storage example */ + /* Byte 0 */ + csd->CSDStruct = (csd_buffer[0] & 0xC0) >> 6; + csd->SysSpecVersion = (csd_buffer[0] & 0x3C) >> 2; + csd->Reserved1 = csd_buffer[0] & 0x03; + /* Byte 1 */ + csd->TAAC = csd_buffer[1]; + /* Byte 2 */ + csd->NSAC = csd_buffer[2]; + /* Byte 3 */ + csd->MaxBusClkFrec = csd_buffer[3]; + /* Byte 4 */ + csd->CardComdClasses = csd_buffer[4] << 4; + /* Byte 5 */ + csd->CardComdClasses |= (csd_buffer[5] & 0xF0) >> 4; + csd->RdBlockLen = csd_buffer[5] & 0x0F; + /* Byte 6 */ + csd->PartBlockRead = (csd_buffer[6] & 0x80) >> 7; + csd->WrBlockMisalign = (csd_buffer[6] & 0x40) >> 6; + csd->RdBlockMisalign = (csd_buffer[6] & 0x20) >> 5; + csd->DSRImpl = (csd_buffer[6] & 0x10) >> 4; + csd->Reserved2 = 0; /* Reserved */ + csd->DeviceSize = (csd_buffer[6] & 0x03) << 10; + /* Byte 7 */ + csd->DeviceSize |= (csd_buffer[7]) << 2; + /* Byte 8 */ + csd->DeviceSize |= (csd_buffer[8] & 0xC0) >> 6; + csd->MaxRdCurrentVDDMin = (csd_buffer[8] & 0x38) >> 3; + csd->MaxRdCurrentVDDMax = (csd_buffer[8] & 0x07); + /* Byte 9 */ + csd->MaxWrCurrentVDDMin = (csd_buffer[9] & 0xE0) >> 5; + csd->MaxWrCurrentVDDMax = (csd_buffer[9] & 0x1C) >> 2; + csd->DeviceSizeMul = (csd_buffer[9] & 0x03) << 1; + /* Byte 10 */ + csd->DeviceSizeMul |= (csd_buffer[10] & 0x80) >> 7; + csd->EraseGrSize = (csd_buffer[10] & 0x7C) >> 2; + csd->EraseGrMul = (csd_buffer[10] & 0x03) << 3; + /* Byte 11 */ + csd->EraseGrMul |= (csd_buffer[11] & 0xE0) >> 5; + csd->WrProtectGrSize = (csd_buffer[11] & 0x1F); + /* Byte 12 */ + csd->WrProtectGrEnable = (csd_buffer[12] & 0x80) >> 7; + csd->ManDeflECC = (csd_buffer[12] & 0x60) >> 5; + csd->WrSpeedFact = (csd_buffer[12] & 0x1C) >> 2; + csd->MaxWrBlockLen = (csd_buffer[12] & 0x03) << 2; + /* Byte 13 */ + csd->MaxWrBlockLen |= (csd_buffer[13] & 0xc0) >> 6; + csd->WriteBlockPaPartial = (csd_buffer[13] & 0x20) >> 5; + csd->Reserved3 = 0; + csd->ContentProtectAppli = (csd_buffer[13] & 0x01); + /* Byte 14 */ + csd->FileFormatGrouop = (csd_buffer[14] & 0x80) >> 7; + csd->CopyFlag = (csd_buffer[14] & 0x40) >> 6; + csd->PermWrProtect = (csd_buffer[14] & 0x20) >> 5; + csd->TempWrProtect = (csd_buffer[14] & 0x10) >> 4; + csd->FileFormat = (csd_buffer[14] & 0x0C) >> 2; + csd->ECC = (csd_buffer[14] & 0x03); + /* Byte 15 */ + csd->msd_CRC = (csd_buffer[15] & 0xFE) >> 1; + csd->Reserved4 = 1; error: - /* Deactivate chip select */ - PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ - /* Send dummy byte once deactivated to drop cards DO */ - PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); - SDCARD_MUTEX_GIVE; + /* Deactivate chip select */ + PIOS_SPI_RC_PinSet(PIOS_SDCARD_SPI, 0, 1); /* spi, pin_value */ + /* Send dummy byte once deactivated to drop cards DO */ + PIOS_SPI_TransferByte(PIOS_SDCARD_SPI, 0xff); + SDCARD_MUTEX_GIVE; - return status; + return status; } /** -* Attempts to write a startup log to the SDCard -* return 0 No errors -* return -1 Error deleting file -* return -2 Error opening file -* return -3 Error writing file -*/ + * Attempts to write a startup log to the SDCard + * return 0 No errors + * return -1 Error deleting file + * return -2 Error opening file + * return -3 Error writing file + */ int32_t PIOS_SDCARD_StartupLog(void) { - FILEINFO File; - char Buffer[1024]; - uint32_t Cache; + FILEINFO File; + char Buffer[1024]; + uint32_t Cache; - /* Delete the file if it exists - ignore errors */ - DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *) LOG_FILENAME, PIOS_SDCARD_Sector); + /* Delete the file if it exists - ignore errors */ + DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)LOG_FILENAME, PIOS_SDCARD_Sector); - if (DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *) LOG_FILENAME, DFS_WRITE, PIOS_SDCARD_Sector, &File)) { - /* Error opening file */ - return -2; - } + if (DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)LOG_FILENAME, DFS_WRITE, PIOS_SDCARD_Sector, &File)) { + /* Error opening file */ + return -2; + } - sprintf(Buffer, "PiOS Startup Log\r\n\r\nLog file creation completed.\r\n"); - if (DFS_WriteFile(&File, PIOS_SDCARD_Sector, (uint8_t *) Buffer, &Cache, strlen(Buffer))) { - /* Error writing to file */ - return -3; - } + sprintf(Buffer, "PiOS Startup Log\r\n\r\nLog file creation completed.\r\n"); + if (DFS_WriteFile(&File, PIOS_SDCARD_Sector, (uint8_t *)Buffer, &Cache, strlen(Buffer))) { + /* Error writing to file */ + return -3; + } - sprintf(Buffer, "------------------------------\r\nSD Card Information\r\n------------------------------\r\n"); - if (DFS_WriteFile(&File, PIOS_SDCARD_Sector, (uint8_t *) Buffer, &Cache, strlen(Buffer))) { - /* Error writing to file */ - return -2; - } + sprintf(Buffer, "------------------------------\r\nSD Card Information\r\n------------------------------\r\n"); + if (DFS_WriteFile(&File, PIOS_SDCARD_Sector, (uint8_t *)Buffer, &Cache, strlen(Buffer))) { + /* Error writing to file */ + return -2; + } - /* Disabled because it takes ca. 7 seconds with my 2GB card */ - /* sprintf(Buffer, "Total Space: %u MB\r\nFree Space: %u MB\r\n", (uint16_t)((VolInfo.numclusters * (VolInfo.secperclus / 2)) / 1024), (uint16_t)(PIOS_SDCARD_GetFree() / 1048576)); */ + /* Disabled because it takes ca. 7 seconds with my 2GB card */ + /* sprintf(Buffer, "Total Space: %u MB\r\nFree Space: %u MB\r\n", (uint16_t)((VolInfo.numclusters * (VolInfo.secperclus / 2)) / 1024), (uint16_t)(PIOS_SDCARD_GetFree() / 1048576)); */ - sprintf(Buffer, "Total Space: %u MB\r\n\r\n", (uint16_t) ((PIOS_SDCARD_VolInfo.numclusters * (PIOS_SDCARD_VolInfo.secperclus / 2)) / 1024)); - if (DFS_WriteFile(&File, PIOS_SDCARD_Sector, (uint8_t *) Buffer, &Cache, strlen(Buffer))) { - /* Error writing to file */ - return -2; - } + sprintf(Buffer, "Total Space: %u MB\r\n\r\n", (uint16_t)((PIOS_SDCARD_VolInfo.numclusters * (PIOS_SDCARD_VolInfo.secperclus / 2)) / 1024)); + if (DFS_WriteFile(&File, PIOS_SDCARD_Sector, (uint8_t *)Buffer, &Cache, strlen(Buffer))) { + /* Error writing to file */ + return -2; + } - /* No errors */ - return 0; + /* No errors */ + return 0; } /** @@ -807,201 +813,201 @@ int32_t PIOS_SDCARD_StartupLog(void) */ int32_t PIOS_SDCARD_IsMounted() { - return sdcard_mounted; + return sdcard_mounted; } /** -* Mounts the file system -* param[in] CreateStartupLog 1 = True, 0 = False -* return 0 No errors -* return -1 SDCard not available -* return -2 Cannot find first partition -* return -3 No volume information -* return -4 Error writing startup log file -*/ + * Mounts the file system + * param[in] CreateStartupLog 1 = True, 0 = False + * return 0 No errors + * return -1 SDCard not available + * return -2 Cannot find first partition + * return -3 No volume information + * return -4 Error writing startup log file + */ int32_t PIOS_SDCARD_MountFS(uint32_t CreateStartupLog) { - uint32_t pstart, psize; - uint8_t pactive, ptype; - uint8_t sdcard_available = 0; + uint32_t pstart, psize; + uint8_t pactive, ptype; + uint8_t sdcard_available = 0; - sdcard_available = PIOS_SDCARD_CheckAvailable(0); + sdcard_available = PIOS_SDCARD_CheckAvailable(0); - if (!sdcard_available) { - /* Disconnected */ - return -1; - } + if (!sdcard_available) { + /* Disconnected */ + return -1; + } - pstart = DFS_GetPtnStart(0, PIOS_SDCARD_Sector, 0, &pactive, &ptype, &psize); - if (pstart == 0xffffffff) { - /* Cannot find first partition */ - return -2; - } + pstart = DFS_GetPtnStart(0, PIOS_SDCARD_Sector, 0, &pactive, &ptype, &psize); + if (pstart == 0xffffffff) { + /* Cannot find first partition */ + return -2; + } - if (DFS_GetVolInfo(0, PIOS_SDCARD_Sector, pstart, &PIOS_SDCARD_VolInfo) != DFS_OK) { - /* No volume information */ - return -3; - } + if (DFS_GetVolInfo(0, PIOS_SDCARD_Sector, pstart, &PIOS_SDCARD_VolInfo) != DFS_OK) { + /* No volume information */ + return -3; + } - if (CreateStartupLog == 1) { - if (PIOS_SDCARD_StartupLog()) { - /* Error writing startup log file */ - return -4; - } - } + if (CreateStartupLog == 1) { + if (PIOS_SDCARD_StartupLog()) { + /* Error writing startup log file */ + return -4; + } + } - /* No errors */ - sdcard_mounted = 1; - return 0; + /* No errors */ + sdcard_mounted = 1; + return 0; } /** -* Mounts the file system -* return Amount of free bytes -*/ + * Mounts the file system + * return Amount of free bytes + */ int32_t PIOS_SDCARD_GetFree(void) { - uint32_t VolFreeBytes = 0xffffffff; - uint32_t ScratchCache = 0; + uint32_t VolFreeBytes = 0xffffffff; + uint32_t ScratchCache = 0; - /* This takes very long, since it scans ALL clusters */ - /* It takes ca. 7 seconds to determine free clusters out of ~47000 (2Gb) */ + /* This takes very long, since it scans ALL clusters */ + /* It takes ca. 7 seconds to determine free clusters out of ~47000 (2Gb) */ - /* Scan all the clusters */ - for (uint32_t i = 2; i < PIOS_SDCARD_VolInfo.numclusters; ++i) { - if (!DFS_GetFAT(&PIOS_SDCARD_VolInfo, PIOS_SDCARD_Sector, &ScratchCache, i)) { - VolFreeBytes += PIOS_SDCARD_VolInfo.secperclus * SECTOR_SIZE; - } - } + /* Scan all the clusters */ + for (uint32_t i = 2; i < PIOS_SDCARD_VolInfo.numclusters; ++i) { + if (!DFS_GetFAT(&PIOS_SDCARD_VolInfo, PIOS_SDCARD_Sector, &ScratchCache, i)) { + VolFreeBytes += PIOS_SDCARD_VolInfo.secperclus * SECTOR_SIZE; + } + } - return VolFreeBytes; + return VolFreeBytes; } /** -* Read from file -* return 0 No error -* return -1 DFS_ReadFile failed -* return -2 Less bytes read than expected -*/ -int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t * buffer, uint32_t len) + * Read from file + * return 0 No error + * return -1 DFS_ReadFile failed + * return -2 Less bytes read than expected + */ +int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len) { - uint32_t SuccessCount; + uint32_t SuccessCount; - if (DFS_ReadFile(fileinfo, PIOS_SDCARD_Sector, buffer, &SuccessCount, len)) { - /* DFS_ReadFile failed */ - return -1; - } - if (SuccessCount != len) { - /* Less bytes read than expected */ - return -2; - } + if (DFS_ReadFile(fileinfo, PIOS_SDCARD_Sector, buffer, &SuccessCount, len)) { + /* DFS_ReadFile failed */ + return -1; + } + if (SuccessCount != len) { + /* Less bytes read than expected */ + return -2; + } - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Read a line from file -* returns Number of bytes read -*/ -int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t * buffer, uint32_t max_len) + * Read a line from file + * returns Number of bytes read + */ +int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len) { - int32_t status; - uint32_t num_read = 0; + int32_t status; + uint32_t num_read = 0; - while (fileinfo->pointer < fileinfo->filelen) { - status = PIOS_SDCARD_ReadBuffer(fileinfo, buffer, 1); + while (fileinfo->pointer < fileinfo->filelen) { + status = PIOS_SDCARD_ReadBuffer(fileinfo, buffer, 1); - if (status < 0) { - return status; - } + if (status < 0) { + return status; + } - ++num_read; + ++num_read; - if (*buffer == '\n' || *buffer == '\r') { - break; - } + if (*buffer == '\n' || *buffer == '\r') { + break; + } - if (num_read < max_len) { - ++buffer; - } - } + if (num_read < max_len) { + ++buffer; + } + } - /* Replace newline by terminator */ - *buffer = 0; + /* Replace newline by terminator */ + *buffer = 0; - return num_read; + return num_read; } /** -* Copy a file -* WARNING: This will overwrite the destination file even if it exists -* param[in] *Source Path to file to copy -* param[in] *Destination Path to destination file -* return 0 No errors -* return -1 Source file doesn't exist -* return -2 Failed to create destination file -* return -3 DFS_ReadFile failed -* return -4 DFS_WriteFile failed -*/ + * Copy a file + * WARNING: This will overwrite the destination file even if it exists + * param[in] *Source Path to file to copy + * param[in] *Destination Path to destination file + * return 0 No errors + * return -1 Source file doesn't exist + * return -2 Failed to create destination file + * return -3 DFS_ReadFile failed + * return -4 DFS_WriteFile failed + */ int32_t PIOS_SDCARD_FileCopy(char *Source, char *Destination) { - FILEINFO SourceFile, DestFile; + FILEINFO SourceFile, DestFile; - /* Disable caching to avoid file inconsistencies while using different sector buffers! */ - DFS_CachingEnabledSet(0); + /* Disable caching to avoid file inconsistencies while using different sector buffers! */ + DFS_CachingEnabledSet(0); - if (DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *) Source, DFS_READ, PIOS_SDCARD_Sector, &SourceFile)) { - /* Source file doesn't exist */ - return -1; - } else { - /* Delete destination file if it already exists - ignore errors */ - DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *) Destination, PIOS_SDCARD_Sector); + if (DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)Source, DFS_READ, PIOS_SDCARD_Sector, &SourceFile)) { + /* Source file doesn't exist */ + return -1; + } else { + /* Delete destination file if it already exists - ignore errors */ + DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)Destination, PIOS_SDCARD_Sector); - if (DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *) Destination, DFS_WRITE, PIOS_SDCARD_Sector, &DestFile)) { - /* Failed to create destination file */ - return -2; - } - } + if (DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)Destination, DFS_WRITE, PIOS_SDCARD_Sector, &DestFile)) { + /* Failed to create destination file */ + return -2; + } + } - /* Copy operation */ - uint8_t WriteBuffer[SECTOR_SIZE]; - uint32_t SuccessCountRead; - uint32_t SuccessCountWrite; - do { - if (DFS_ReadFile(&SourceFile, PIOS_SDCARD_Sector, WriteBuffer, &SuccessCountRead, SECTOR_SIZE)) { - /* DFS_ReadFile failed */ - return -3; - } else if (DFS_WriteFile(&DestFile, PIOS_SDCARD_Sector, WriteBuffer, &SuccessCountWrite, SuccessCountRead)) { - /* DFS_WriteFile failed */ - return -4; - } - } while (SuccessCountRead > 0); + /* Copy operation */ + uint8_t WriteBuffer[SECTOR_SIZE]; + uint32_t SuccessCountRead; + uint32_t SuccessCountWrite; + do { + if (DFS_ReadFile(&SourceFile, PIOS_SDCARD_Sector, WriteBuffer, &SuccessCountRead, SECTOR_SIZE)) { + /* DFS_ReadFile failed */ + return -3; + } else if (DFS_WriteFile(&DestFile, PIOS_SDCARD_Sector, WriteBuffer, &SuccessCountWrite, SuccessCountRead)) { + /* DFS_WriteFile failed */ + return -4; + } + } while (SuccessCountRead > 0); - /* No errors */ - return 0; + /* No errors */ + return 0; } /** -* Delete a file -* param[in] *Filename File to delete -* return 0 No errors -* return -1 Error deleting file -*/ + * Delete a file + * param[in] *Filename File to delete + * return 0 No errors + * return -1 Error deleting file + */ int32_t PIOS_SDCARD_FileDelete(char *Filename) { - if (DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *) Filename, PIOS_SDCARD_Sector)) { - /* Error deleting file */ - return -1; - } + if (DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)Filename, PIOS_SDCARD_Sector)) { + /* Error deleting file */ + return -1; + } - /* No errors */ - return 0; + /* No errors */ + return 0; } #endif /* PIOS_INCLUDE_SDCARD */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/common/pios_task_monitor.c b/flight/pios/common/pios_task_monitor.c index c3e3f27d0..7a3c13234 100644 --- a/flight/pios/common/pios_task_monitor.c +++ b/flight/pios/common/pios_task_monitor.c @@ -30,45 +30,49 @@ #ifdef PIOS_INCLUDE_TASK_MONITOR // Private variables -static xSemaphoreHandle mLock; -static xTaskHandle *mTaskHandles; -static uint32_t mLastMonitorTime; -static uint16_t mMaxTasks; +static xSemaphoreHandle mLock; +static xTaskHandle *mTaskHandles; +static uint32_t mLastMonitorTime; +static uint16_t mMaxTasks; /** * Initialize the Task Monitor */ int32_t PIOS_TASK_MONITOR_Initialize(uint16_t max_tasks) { - mLock = xSemaphoreCreateRecursiveMutex(); - if (!mLock) { return -1; } + mLock = xSemaphoreCreateRecursiveMutex(); + if (!mLock) { + return -1; + } - mTaskHandles = (xTaskHandle*)pvPortMalloc(max_tasks * sizeof(xTaskHandle)); - if (!mTaskHandles) { return -1; } - memset(mTaskHandles, 0, max_tasks * sizeof(xTaskHandle)); + mTaskHandles = (xTaskHandle *)pvPortMalloc(max_tasks * sizeof(xTaskHandle)); + if (!mTaskHandles) { + return -1; + } + memset(mTaskHandles, 0, max_tasks * sizeof(xTaskHandle)); - mMaxTasks = max_tasks; + mMaxTasks = max_tasks; #if (configGENERATE_RUN_TIME_STATS == 1) - mLastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE(); + mLastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE(); #else - mLastMonitorTime = 0; + mLastMonitorTime = 0; #endif - return 0; + return 0; } /** - * Register a task handle + * Register a task handle */ int32_t PIOS_TASK_MONITOR_RegisterTask(uint16_t task_id, xTaskHandle handle) { - if (mTaskHandles && task_id < mMaxTasks) { - xSemaphoreTakeRecursive(mLock, portMAX_DELAY); - mTaskHandles[task_id] = handle; - xSemaphoreGiveRecursive(mLock); - return 0; - } else { - return -1; - } + if (mTaskHandles && task_id < mMaxTasks) { + xSemaphoreTakeRecursive(mLock, portMAX_DELAY); + mTaskHandles[task_id] = handle; + xSemaphoreGiveRecursive(mLock); + return 0; + } else { + return -1; + } } /** @@ -76,14 +80,14 @@ int32_t PIOS_TASK_MONITOR_RegisterTask(uint16_t task_id, xTaskHandle handle) */ int32_t PIOS_TASK_MONITOR_UnregisterTask(uint16_t task_id) { - if (mTaskHandles && task_id < mMaxTasks) { - xSemaphoreTakeRecursive(mLock, portMAX_DELAY); - mTaskHandles[task_id] = 0; - xSemaphoreGiveRecursive(mLock); - return 0; - } else { - return -1; - } + if (mTaskHandles && task_id < mMaxTasks) { + xSemaphoreTakeRecursive(mLock, portMAX_DELAY); + mTaskHandles[task_id] = 0; + xSemaphoreGiveRecursive(mLock); + return 0; + } else { + return -1; + } } /** @@ -91,7 +95,7 @@ int32_t PIOS_TASK_MONITOR_UnregisterTask(uint16_t task_id) */ bool PIOS_TASK_MONITOR_IsRunning(uint16_t task_id) { - return (mTaskHandles && task_id <= mMaxTasks && mTaskHandles[task_id]); + return mTaskHandles && task_id <= mMaxTasks && mTaskHandles[task_id]; } /** @@ -99,46 +103,47 @@ bool PIOS_TASK_MONITOR_IsRunning(uint16_t task_id) */ void PIOS_TASK_MONITOR_ForEachTask(TaskMonitorTaskInfoCallback callback, void *context) { - if (!mTaskHandles) { return; } + if (!mTaskHandles) { + return; + } - xSemaphoreTakeRecursive(mLock, portMAX_DELAY); + xSemaphoreTakeRecursive(mLock, portMAX_DELAY); #if (configGENERATE_RUN_TIME_STATS == 1) - /* Calculate the amount of elapsed run time between the last time we - * measured and now. Scale so that we can convert task run times - * directly to percentages. */ - uint32_t currentTime = portGET_RUN_TIME_COUNTER_VALUE(); - /* avoid divide-by-zero if the interval is too small */ - uint32_t deltaTime = ((currentTime - mLastMonitorTime)/100) ? : 1; - mLastMonitorTime = currentTime; + /* Calculate the amount of elapsed run time between the last time we + * measured and now. Scale so that we can convert task run times + * directly to percentages. */ + uint32_t currentTime = portGET_RUN_TIME_COUNTER_VALUE(); + /* avoid divide-by-zero if the interval is too small */ + uint32_t deltaTime = ((currentTime - mLastMonitorTime) / 100) ? : 1; + mLastMonitorTime = currentTime; #endif - /* Update all task information */ - for (uint16_t n = 0; n < mMaxTasks; ++n) { - struct pios_task_info info; - if (mTaskHandles[n]) { - info.is_running = true; + /* Update all task information */ + for (uint16_t n = 0; n < mMaxTasks; ++n) { + struct pios_task_info info; + if (mTaskHandles[n]) { + info.is_running = true; #if defined(ARCH_POSIX) || defined(ARCH_WIN32) - info.stack_remaining = 10000; + info.stack_remaining = 10000; #else - info.stack_remaining = uxTaskGetStackHighWaterMark(mTaskHandles[n]) * 4; + info.stack_remaining = uxTaskGetStackHighWaterMark(mTaskHandles[n]) * 4; #endif #if (configGENERATE_RUN_TIME_STATS == 1) - /* Generate run time percentage stats */ - info.running_time_percentage = uxTaskGetRunTime(mTaskHandles[n])/deltaTime; + /* Generate run time percentage stats */ + info.running_time_percentage = uxTaskGetRunTime(mTaskHandles[n]) / deltaTime; #else - info.running_time_percentage = 0; -#endif - } else { - info.is_running = false; - info.stack_remaining = 0; - info.running_time_percentage = 0; - } - /* Pass the information for this task back to the caller */ - callback(n, &info, context); - } + info.running_time_percentage = 0; +#endif + } else { + info.is_running = false; + info.stack_remaining = 0; + info.running_time_percentage = 0; + } + /* Pass the information for this task back to the caller */ + callback(n, &info, context); + } - xSemaphoreGiveRecursive(mLock); + xSemaphoreGiveRecursive(mLock); } #endif // PIOS_INCLUDE_TASK_MONITOR - diff --git a/flight/pios/common/pios_usb_desc_hid_cdc.c b/flight/pios/common/pios_usb_desc_hid_cdc.c index e102aefe3..137578c94 100644 --- a/flight/pios/common/pios_usb_desc_hid_cdc.c +++ b/flight/pios/common/pios_usb_desc_hid_cdc.c @@ -33,300 +33,300 @@ #ifdef PIOS_INCLUDE_USB #include "pios_usb_desc_hid_cdc_priv.h" /* exported API */ -#include "pios_usb_defs.h" /* struct usb_*, USB_* */ -#include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ -#include "pios_usbhook.h" /* PIOS_USBHOOK_Register* */ -#include "pios_usb_hid.h" /* PIOS_USB_HID_Register* */ +#include "pios_usb_defs.h" /* struct usb_*, USB_* */ +#include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ +#include "pios_usbhook.h" /* PIOS_USBHOOK_Register* */ +#include "pios_usb_hid.h" /* PIOS_USB_HID_Register* */ static const struct usb_device_desc device_desc = { - .bLength = sizeof(struct usb_device_desc), - .bDescriptorType = USB_DESC_TYPE_DEVICE, - .bcdUSB = htousbs(0x0200), - .bDeviceClass = 0xef, - .bDeviceSubClass = 0x02, - .bDeviceProtocol = 0x01, - .bMaxPacketSize0 = 64, /* Must be 64 for high-speed devices */ - .idVendor = htousbs(USB_VENDOR_ID_OPENPILOT), - .idProduct = htousbs(PIOS_USB_BOARD_PRODUCT_ID), - .bcdDevice = htousbs(PIOS_USB_BOARD_DEVICE_VER), - .iManufacturer = USB_STRING_DESC_VENDOR, - .iProduct = USB_STRING_DESC_PRODUCT, - .iSerialNumber = USB_STRING_DESC_SERIAL, - .bNumConfigurations = 1, + .bLength = sizeof(struct usb_device_desc), + .bDescriptorType = USB_DESC_TYPE_DEVICE, + .bcdUSB = htousbs(0x0200), + .bDeviceClass = 0xef, + .bDeviceSubClass = 0x02, + .bDeviceProtocol = 0x01, + .bMaxPacketSize0 = 64, /* Must be 64 for high-speed devices */ + .idVendor = htousbs(USB_VENDOR_ID_OPENPILOT), + .idProduct = htousbs(PIOS_USB_BOARD_PRODUCT_ID), + .bcdDevice = htousbs(PIOS_USB_BOARD_DEVICE_VER), + .iManufacturer = USB_STRING_DESC_VENDOR, + .iProduct = USB_STRING_DESC_PRODUCT, + .iSerialNumber = USB_STRING_DESC_SERIAL, + .bNumConfigurations = 1, }; static const uint8_t hid_report_desc[89] = { - HID_GLOBAL_ITEM_2 (HID_TAG_GLOBAL_USAGE_PAGE), - 0x9C, 0xFF, /* Usage Page 0xFF9C (Vendor Defined) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x01, /* Usage ID 0x0001 (0x01-0x1F uaually for top-level collections) */ + HID_GLOBAL_ITEM_2(HID_TAG_GLOBAL_USAGE_PAGE), + 0x9C, 0xFF, /* Usage Page 0xFF9C (Vendor Defined) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x01, /* Usage ID 0x0001 (0x01-0x1F uaually for top-level collections) */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_COLLECTION), - 0x01, /* Application */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_COLLECTION), + 0x01, /* Application */ - /* Device -> Host emulated serial channel */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_ID), - 0x01, /* OpenPilot emulated serial channel (Device -> Host) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x02, - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MIN), - 0x00, /* Values range from min = 0x00 */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MAX), - 0xFF, /* Values range to max = 0xFF */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x08, /* 8 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - PIOS_USB_BOARD_HID_DATA_LENGTH-1, /* Need to leave room for a report ID */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_INPUT), - 0x03, /* Variable, Constant (read-only) */ + /* Device -> Host emulated serial channel */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_ID), + 0x01, /* OpenPilot emulated serial channel (Device -> Host) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x02, + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MIN), + 0x00, /* Values range from min = 0x00 */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MAX), + 0xFF, /* Values range to max = 0xFF */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_SIZE), + 0x08, /* 8 bits wide */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_CNT), + PIOS_USB_BOARD_HID_DATA_LENGTH - 1, /* Need to leave room for a report ID */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_INPUT), + 0x03, /* Variable, Constant (read-only) */ - /* Host -> Host emulated serial channel */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_ID), - 0x02, /* OpenPilot emulated Serial Channel (Host -> Device) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x02, - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MIN), - 0x00, /* Values range from min = 0x00 */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MAX), - 0xFF, /* Values range to max = 0xFF */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x08, /* 8 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - PIOS_USB_BOARD_HID_DATA_LENGTH-1, /* Need to leave room for a report ID */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_OUTPUT), - 0x82, /* Volatile, Variable */ + /* Host -> Host emulated serial channel */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_ID), + 0x02, /* OpenPilot emulated Serial Channel (Host -> Device) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x02, + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MIN), + 0x00, /* Values range from min = 0x00 */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MAX), + 0xFF, /* Values range to max = 0xFF */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_SIZE), + 0x08, /* 8 bits wide */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_CNT), + PIOS_USB_BOARD_HID_DATA_LENGTH - 1, /* Need to leave room for a report ID */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_OUTPUT), + 0x82, /* Volatile, Variable */ - HID_MAIN_ITEM_0 (HID_TAG_MAIN_ENDCOLLECTION), + HID_MAIN_ITEM_0(HID_TAG_MAIN_ENDCOLLECTION), /* 36 bytes to here */ - /* Emulate a Joystick */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_USAGE_PAGE), - 0x01, /* Usage Page 0x01 (Generic Desktop Controls) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x04, /* Usage ID 0x0004 (Joystick) */ + /* Emulate a Joystick */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_USAGE_PAGE), + 0x01, /* Usage Page 0x01 (Generic Desktop Controls) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x04, /* Usage ID 0x0004 (Joystick) */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MIN), - 0x00, /* Values range from min = 0x00 */ - HID_GLOBAL_ITEM_4 (HID_TAG_GLOBAL_LOGICAL_MAX), - 0xFF, 0xFF, 0x00, 0x00, /* Values range to max = 0x0000FFFF */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MIN), + 0x00, /* Values range from min = 0x00 */ + HID_GLOBAL_ITEM_4(HID_TAG_GLOBAL_LOGICAL_MAX), + 0xFF, 0xFF,0x00, 0x00, /* Values range to max = 0x0000FFFF */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_COLLECTION), - 0x01, /* Application */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_COLLECTION), - 0x00, /* Physical */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_ID), - 0x03, /* OpenPilot Emulated joystick */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_COLLECTION), + 0x01, /* Application */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_COLLECTION), + 0x00, /* Physical */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_ID), + 0x03, /* OpenPilot Emulated joystick */ - /* X + Y controls */ + /* X + Y controls */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x30, /* Usage ID 0x00010030 (Generic Desktop: X) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x31, /* Usage ID 0x00010031 (Generic Desktop: Y) */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x10, /* 16 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - 2, - HID_MAIN_ITEM_1 (HID_TAG_MAIN_INPUT), - 0x82, /* Data, Var, Abs, Vol */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x30, /* Usage ID 0x00010030 (Generic Desktop: X) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x31, /* Usage ID 0x00010031 (Generic Desktop: Y) */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_SIZE), + 0x10, /* 16 bits wide */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_CNT), + 2, + HID_MAIN_ITEM_1(HID_TAG_MAIN_INPUT), + 0x82, /* Data, Var, Abs, Vol */ - /* Y + Rx controls */ + /* Y + Rx controls */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x32, /* Usage ID 0x00010032 (Generic Desktop: Z) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x33, /* Usage ID 0x00010031 (Generic Desktop: Rx) */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x10, /* 16 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - 2, - HID_MAIN_ITEM_1 (HID_TAG_MAIN_INPUT), - 0x82, /* Data, Var, Abs, Vol */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x32, /* Usage ID 0x00010032 (Generic Desktop: Z) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x33, /* Usage ID 0x00010031 (Generic Desktop: Rx) */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_SIZE), + 0x10, /* 16 bits wide */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_CNT), + 2, + HID_MAIN_ITEM_1(HID_TAG_MAIN_INPUT), + 0x82, /* Data, Var, Abs, Vol */ - /* Ry, Rz, Slider + Dial controls */ + /* Ry, Rz, Slider + Dial controls */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x34, /* Usage ID 0x00010034 (Generic Desktop: Ry) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x35, /* Usage ID 0x00010035 (Generic Desktop: Rz) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x36, /* Usage ID 0x00010036 (Generic Desktop: Slider) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x37, /* Usage ID 0x00010037 (Generic Desktop: Dial) */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x10, /* 16 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - 4, - HID_MAIN_ITEM_1 (HID_TAG_MAIN_INPUT), - 0x82, /* Data, Var, Abs, Vol */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x34, /* Usage ID 0x00010034 (Generic Desktop: Ry) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x35, /* Usage ID 0x00010035 (Generic Desktop: Rz) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x36, /* Usage ID 0x00010036 (Generic Desktop: Slider) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x37, /* Usage ID 0x00010037 (Generic Desktop: Dial) */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_SIZE), + 0x10, /* 16 bits wide */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_CNT), + 4, + HID_MAIN_ITEM_1(HID_TAG_MAIN_INPUT), + 0x82, /* Data, Var, Abs, Vol */ - HID_MAIN_ITEM_0 (HID_TAG_MAIN_ENDCOLLECTION), + HID_MAIN_ITEM_0(HID_TAG_MAIN_ENDCOLLECTION), - HID_MAIN_ITEM_0 (HID_TAG_MAIN_ENDCOLLECTION), + HID_MAIN_ITEM_0(HID_TAG_MAIN_ENDCOLLECTION), /* 89 bytes to here */ }; struct usb_config_hid_cdc { - struct usb_configuration_desc config; - struct usb_interface_association_desc iad; - struct usb_interface_desc cdc_control_if; - struct usb_cdc_header_func_desc cdc_header; - struct usb_cdc_callmgmt_func_desc cdc_callmgmt; - struct usb_cdc_acm_func_desc cdc_acm; - struct usb_cdc_union_func_desc cdc_union; - struct usb_endpoint_desc cdc_mgmt_in; - struct usb_interface_desc cdc_data_if; - struct usb_endpoint_desc cdc_in; - struct usb_endpoint_desc cdc_out; - struct usb_interface_desc hid_if; - struct usb_hid_desc hid; - struct usb_endpoint_desc hid_in; - struct usb_endpoint_desc hid_out; + struct usb_configuration_desc config; + struct usb_interface_association_desc iad; + struct usb_interface_desc cdc_control_if; + struct usb_cdc_header_func_desc cdc_header; + struct usb_cdc_callmgmt_func_desc cdc_callmgmt; + struct usb_cdc_acm_func_desc cdc_acm; + struct usb_cdc_union_func_desc cdc_union; + struct usb_endpoint_desc cdc_mgmt_in; + struct usb_interface_desc cdc_data_if; + struct usb_endpoint_desc cdc_in; + struct usb_endpoint_desc cdc_out; + struct usb_interface_desc hid_if; + struct usb_hid_desc hid; + struct usb_endpoint_desc hid_in; + struct usb_endpoint_desc hid_out; } __attribute__((packed)); static const struct usb_config_hid_cdc config_hid_cdc = { - .config = { - .bLength = sizeof(struct usb_configuration_desc), - .bDescriptorType = USB_DESC_TYPE_CONFIGURATION, - .wTotalLength = htousbs(sizeof(struct usb_config_hid_cdc)), - .bNumInterfaces = 3, - .bConfigurationValue = 1, - .iConfiguration = 0, - .bmAttributes = 0xC0, - .bMaxPower = 250/2, /* in units of 2ma */ - }, - .iad = { - .bLength = sizeof(struct usb_interface_association_desc), - .bDescriptorType = USB_DESC_TYPE_IAD, - .bFirstInterface = 0, - .bInterfaceCount = 2, - .bFunctionClass = USB_INTERFACE_CLASS_CDC, /* Communication */ - .bFunctionSubClass = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL, /* Abstract Control Model */ - .bFunctionProtocol = 1, /* V.25ter, Common AT commands */ - .iInterface = 0, - }, - .cdc_control_if = { - .bLength = sizeof(struct usb_interface_desc), - .bDescriptorType = USB_DESC_TYPE_INTERFACE, - .bInterfaceNumber = 0, - .bAlternateSetting = 0, - .bNumEndpoints = 1, - .bInterfaceClass = USB_INTERFACE_CLASS_CDC, - .bInterfaceSubClass = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL, /* Abstract Control Model */ - .nInterfaceProtocol = 1, /* V.25ter, Common AT commands */ - .iInterface = 0, - }, - .cdc_header = { - .bLength = sizeof(struct usb_cdc_header_func_desc), - .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, - .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_HEADER, - .bcdCDC = htousbs(0x0110), - }, - .cdc_callmgmt = { - .bLength = sizeof(struct usb_cdc_callmgmt_func_desc), - .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, - .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_CALLMGMT, - .bmCapabilities = 0x00, /* No call handling */ - .bDataInterface = 1, - }, - .cdc_acm = { - .bLength = sizeof(struct usb_cdc_acm_func_desc), - .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, - .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL, - .bmCapabilities = 0x00, - }, - .cdc_union = { - .bLength = sizeof(struct usb_cdc_union_func_desc), - .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, - .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_UNION, - .bMasterInterface = 0, - .bSlaveInterface = 1, - }, - .cdc_mgmt_in = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_IN(2), - .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_MGMT_LENGTH), - .bInterval = 4, /* ms */ - }, - .cdc_data_if = { - .bLength = sizeof(struct usb_interface_desc), - .bDescriptorType = USB_DESC_TYPE_INTERFACE, - .bInterfaceNumber = 1, - .bAlternateSetting = 0, - .bNumEndpoints = 2, - .bInterfaceClass = USB_INTERFACE_CLASS_DATA, - .bInterfaceSubClass = 0, - .nInterfaceProtocol = 0, /* No class specific protocol */ - .iInterface = 0, - }, - .cdc_in = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_IN(3), - .bmAttributes = USB_EP_ATTR_TT_BULK, - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_DATA_LENGTH), - .bInterval = 0, /* ms */ - }, - .cdc_out = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_OUT(3), - .bmAttributes = USB_EP_ATTR_TT_BULK, /* Bulk */ - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_DATA_LENGTH), - .bInterval = 0, /* ms */ - }, - .hid_if = { - .bLength = sizeof(struct usb_interface_desc), - .bDescriptorType = USB_DESC_TYPE_INTERFACE, - .bInterfaceNumber = 2, - .bAlternateSetting = 0, - .bNumEndpoints = 2, - .bInterfaceClass = USB_INTERFACE_CLASS_HID, - .bInterfaceSubClass = 0, /* no boot */ - .nInterfaceProtocol = 0, /* none */ - .iInterface = 0, - }, - .hid = { - .bLength = sizeof(struct usb_hid_desc), - .bDescriptorType = USB_DESC_TYPE_HID, - .bcdHID = htousbs(0x0110), - .bCountryCode = 0, - .bNumDescriptors = 1, - .bClassDescriptorType = USB_DESC_TYPE_REPORT, - .wItemLength = htousbs(sizeof(hid_report_desc)), - }, - .hid_in = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_IN(1), - .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), - .bInterval = 4, /* ms */ - }, - .hid_out = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_OUT(1), - .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), - .bInterval = 4, /* ms */ - }, + .config = { + .bLength = sizeof(struct usb_configuration_desc), + .bDescriptorType = USB_DESC_TYPE_CONFIGURATION, + .wTotalLength = htousbs(sizeof(struct usb_config_hid_cdc)), + .bNumInterfaces = 3, + .bConfigurationValue = 1, + .iConfiguration = 0, + .bmAttributes = 0xC0, + .bMaxPower = 250 / 2, /* in units of 2ma */ + }, + .iad = { + .bLength = sizeof(struct usb_interface_association_desc), + .bDescriptorType = USB_DESC_TYPE_IAD, + .bFirstInterface = 0, + .bInterfaceCount = 2, + .bFunctionClass = USB_INTERFACE_CLASS_CDC, /* Communication */ + .bFunctionSubClass = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL, /* Abstract Control Model */ + .bFunctionProtocol = 1, /* V.25ter, Common AT commands */ + .iInterface = 0, + }, + .cdc_control_if = { + .bLength = sizeof(struct usb_interface_desc), + .bDescriptorType = USB_DESC_TYPE_INTERFACE, + .bInterfaceNumber = 0, + .bAlternateSetting = 0, + .bNumEndpoints = 1, + .bInterfaceClass = USB_INTERFACE_CLASS_CDC, + .bInterfaceSubClass = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL, /* Abstract Control Model */ + .nInterfaceProtocol = 1, /* V.25ter, Common AT commands */ + .iInterface = 0, + }, + .cdc_header = { + .bLength = sizeof(struct usb_cdc_header_func_desc), + .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, + .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_HEADER, + .bcdCDC = htousbs(0x0110), + }, + .cdc_callmgmt = { + .bLength = sizeof(struct usb_cdc_callmgmt_func_desc), + .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, + .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_CALLMGMT, + .bmCapabilities = 0x00, /* No call handling */ + .bDataInterface = 1, + }, + .cdc_acm = { + .bLength = sizeof(struct usb_cdc_acm_func_desc), + .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, + .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL, + .bmCapabilities = 0x00, + }, + .cdc_union = { + .bLength = sizeof(struct usb_cdc_union_func_desc), + .bDescriptorType = USB_DESC_TYPE_CLASS_SPECIFIC, + .bDescriptorSubType = USB_CDC_DESC_SUBTYPE_UNION, + .bMasterInterface = 0, + .bSlaveInterface = 1, + }, + .cdc_mgmt_in = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_IN(2), + .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_MGMT_LENGTH), + .bInterval = 4, /* ms */ + }, + .cdc_data_if = { + .bLength = sizeof(struct usb_interface_desc), + .bDescriptorType = USB_DESC_TYPE_INTERFACE, + .bInterfaceNumber = 1, + .bAlternateSetting = 0, + .bNumEndpoints = 2, + .bInterfaceClass = USB_INTERFACE_CLASS_DATA, + .bInterfaceSubClass = 0, + .nInterfaceProtocol = 0, /* No class specific protocol */ + .iInterface = 0, + }, + .cdc_in = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_IN(3), + .bmAttributes = USB_EP_ATTR_TT_BULK, + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_DATA_LENGTH), + .bInterval = 0, /* ms */ + }, + .cdc_out = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_OUT(3), + .bmAttributes = USB_EP_ATTR_TT_BULK, /* Bulk */ + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_CDC_DATA_LENGTH), + .bInterval = 0, /* ms */ + }, + .hid_if = { + .bLength = sizeof(struct usb_interface_desc), + .bDescriptorType = USB_DESC_TYPE_INTERFACE, + .bInterfaceNumber = 2, + .bAlternateSetting = 0, + .bNumEndpoints = 2, + .bInterfaceClass = USB_INTERFACE_CLASS_HID, + .bInterfaceSubClass = 0, /* no boot */ + .nInterfaceProtocol = 0, /* none */ + .iInterface = 0, + }, + .hid = { + .bLength = sizeof(struct usb_hid_desc), + .bDescriptorType = USB_DESC_TYPE_HID, + .bcdHID = htousbs(0x0110), + .bCountryCode = 0, + .bNumDescriptors = 1, + .bClassDescriptorType = USB_DESC_TYPE_REPORT, + .wItemLength = htousbs(sizeof(hid_report_desc)), + }, + .hid_in = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_IN(1), + .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), + .bInterval = 4, /* ms */ + }, + .hid_out = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_OUT(1), + .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), + .bInterval = 4, /* ms */ + }, }; int32_t PIOS_USB_DESC_HID_CDC_Init(void) { - PIOS_USBHOOK_RegisterConfig(1, (uint8_t *)&config_hid_cdc, sizeof(config_hid_cdc)); + PIOS_USBHOOK_RegisterConfig(1, (uint8_t *)&config_hid_cdc, sizeof(config_hid_cdc)); - PIOS_USBHOOK_RegisterDevice((uint8_t *)&device_desc, sizeof(device_desc)); + PIOS_USBHOOK_RegisterDevice((uint8_t *)&device_desc, sizeof(device_desc)); - PIOS_USB_HID_RegisterHidDescriptor((uint8_t *)&(config_hid_cdc.hid), sizeof(config_hid_cdc.hid)); - PIOS_USB_HID_RegisterHidReport((uint8_t *)hid_report_desc, sizeof(hid_report_desc)); + PIOS_USB_HID_RegisterHidDescriptor((uint8_t *)&(config_hid_cdc.hid), sizeof(config_hid_cdc.hid)); + PIOS_USB_HID_RegisterHidReport((uint8_t *)hid_report_desc, sizeof(hid_report_desc)); - return 0; + return 0; } #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/common/pios_usb_desc_hid_only.c b/flight/pios/common/pios_usb_desc_hid_only.c index 0757d0937..89cf9ebe4 100644 --- a/flight/pios/common/pios_usb_desc_hid_only.c +++ b/flight/pios/common/pios_usb_desc_hid_only.c @@ -33,139 +33,139 @@ #ifdef PIOS_INCLUDE_USB #include "pios_usb_desc_hid_only_priv.h" /* exported API */ -#include "pios_usb_defs.h" /* struct usb_*, USB_* */ -#include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ -#include "pios_usbhook.h" /* PIOS_USBHOOK_Register* */ -#include "pios_usb_hid.h" /* PIOS_USB_HID_Register* */ +#include "pios_usb_defs.h" /* struct usb_*, USB_* */ +#include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ +#include "pios_usbhook.h" /* PIOS_USBHOOK_Register* */ +#include "pios_usb_hid.h" /* PIOS_USB_HID_Register* */ static const struct usb_device_desc device_desc = { - .bLength = sizeof(struct usb_device_desc), - .bDescriptorType = USB_DESC_TYPE_DEVICE, - .bcdUSB = htousbs(0x0200), - .bDeviceClass = 0x00, - .bDeviceSubClass = 0x00, - .bDeviceProtocol = 0x00, - .bMaxPacketSize0 = 64, /* Must be 64 for high-speed devices */ - .idVendor = htousbs(USB_VENDOR_ID_OPENPILOT), - .idProduct = htousbs(PIOS_USB_BOARD_PRODUCT_ID), - .bcdDevice = htousbs(PIOS_USB_BOARD_DEVICE_VER), - .iManufacturer = 1, - .iProduct = 2, - .iSerialNumber = 3, - .bNumConfigurations = 1, + .bLength = sizeof(struct usb_device_desc), + .bDescriptorType = USB_DESC_TYPE_DEVICE, + .bcdUSB = htousbs(0x0200), + .bDeviceClass = 0x00, + .bDeviceSubClass = 0x00, + .bDeviceProtocol = 0x00, + .bMaxPacketSize0 = 64, /* Must be 64 for high-speed devices */ + .idVendor = htousbs(USB_VENDOR_ID_OPENPILOT), + .idProduct = htousbs(PIOS_USB_BOARD_PRODUCT_ID), + .bcdDevice = htousbs(PIOS_USB_BOARD_DEVICE_VER), + .iManufacturer = 1, + .iProduct = 2, + .iSerialNumber = 3, + .bNumConfigurations = 1, }; static const uint8_t hid_report_desc[36] = { - HID_GLOBAL_ITEM_2 (HID_TAG_GLOBAL_USAGE_PAGE), - 0x9C, 0xFF, /* Usage Page 0xFF9C (Vendor Defined) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x01, /* Usage ID 0x0001 (0x01-0x1F uaually for top-level collections) */ + HID_GLOBAL_ITEM_2(HID_TAG_GLOBAL_USAGE_PAGE), + 0x9C, 0xFF, /* Usage Page 0xFF9C (Vendor Defined) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x01, /* Usage ID 0x0001 (0x01-0x1F uaually for top-level collections) */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_COLLECTION), - 0x01, /* Application */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_COLLECTION), + 0x01, /* Application */ - /* Device -> Host emulated serial channel */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_ID), - 0x01, /* OpenPilot emulated serial channel (Device -> Host) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x02, - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MIN), - 0x00, /* Values range from min = 0x00 */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MAX), - 0xFF, /* Values range to max = 0xFF */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x08, /* 8 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - PIOS_USB_BOARD_HID_DATA_LENGTH-1, /* Need to leave room for a report ID */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_INPUT), - 0x03, /* Variable, Constant (read-only) */ + /* Device -> Host emulated serial channel */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_ID), + 0x01, /* OpenPilot emulated serial channel (Device -> Host) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x02, + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MIN), + 0x00, /* Values range from min = 0x00 */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MAX), + 0xFF, /* Values range to max = 0xFF */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_SIZE), + 0x08, /* 8 bits wide */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_CNT), + PIOS_USB_BOARD_HID_DATA_LENGTH - 1, /* Need to leave room for a report ID */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_INPUT), + 0x03, /* Variable, Constant (read-only) */ - /* Host -> Device emulated serial channel */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_ID), - 0x02, /* OpenPilot emulated Serial Channel (Host -> Device) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x02, - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MIN), - 0x00, /* Values range from min = 0x00 */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MAX), - 0xFF, /* Values range to max = 0xFF */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x08, /* 8 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - PIOS_USB_BOARD_HID_DATA_LENGTH-1, /* Need to leave room for a report ID */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_OUTPUT), - 0x82, /* Volatile, Variable */ + /* Host -> Device emulated serial channel */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_ID), + 0x02, /* OpenPilot emulated Serial Channel (Host -> Device) */ + HID_LOCAL_ITEM_1(HID_TAG_LOCAL_USAGE), + 0x02, + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MIN), + 0x00, /* Values range from min = 0x00 */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_LOGICAL_MAX), + 0xFF, /* Values range to max = 0xFF */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_SIZE), + 0x08, /* 8 bits wide */ + HID_GLOBAL_ITEM_1(HID_TAG_GLOBAL_REPORT_CNT), + PIOS_USB_BOARD_HID_DATA_LENGTH - 1, /* Need to leave room for a report ID */ + HID_MAIN_ITEM_1(HID_TAG_MAIN_OUTPUT), + 0x82, /* Volatile, Variable */ - HID_MAIN_ITEM_0 (HID_TAG_MAIN_ENDCOLLECTION), + HID_MAIN_ITEM_0(HID_TAG_MAIN_ENDCOLLECTION), }; struct usb_config_hid_only { - struct usb_configuration_desc config; - struct usb_interface_desc hid_if; - struct usb_hid_desc hid; - struct usb_endpoint_desc hid_in; - struct usb_endpoint_desc hid_out; + struct usb_configuration_desc config; + struct usb_interface_desc hid_if; + struct usb_hid_desc hid; + struct usb_endpoint_desc hid_in; + struct usb_endpoint_desc hid_out; } __attribute__((packed)); const struct usb_config_hid_only config_hid_only = { - .config = { - .bLength = sizeof(struct usb_configuration_desc), - .bDescriptorType = USB_DESC_TYPE_CONFIGURATION, - .wTotalLength = htousbs(sizeof(struct usb_config_hid_only)), - .bNumInterfaces = 1, - .bConfigurationValue = 1, - .iConfiguration = 0, - .bmAttributes = 0xC0, - .bMaxPower = 250/2, /* in units of 2ma */ - }, - .hid_if = { - .bLength = sizeof(struct usb_interface_desc), - .bDescriptorType = USB_DESC_TYPE_INTERFACE, - .bInterfaceNumber = 0, - .bAlternateSetting = 0, - .bNumEndpoints = 2, - .bInterfaceClass = USB_INTERFACE_CLASS_HID, - .bInterfaceSubClass = 0, /* no boot */ - .nInterfaceProtocol = 0, /* none */ - .iInterface = 0, - }, - .hid = { - .bLength = sizeof(struct usb_hid_desc), - .bDescriptorType = USB_DESC_TYPE_HID, - .bcdHID = htousbs(0x0110), - .bCountryCode = 0, - .bNumDescriptors = 1, - .bClassDescriptorType = USB_DESC_TYPE_REPORT, - .wItemLength = htousbs(sizeof(hid_report_desc)), - }, - .hid_in = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_IN(1), - .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), - .bInterval = 4, /* ms */ - }, - .hid_out = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_OUT(1), - .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), - .bInterval = 4, /* ms */ - }, + .config = { + .bLength = sizeof(struct usb_configuration_desc), + .bDescriptorType = USB_DESC_TYPE_CONFIGURATION, + .wTotalLength = htousbs(sizeof(struct usb_config_hid_only)), + .bNumInterfaces = 1, + .bConfigurationValue = 1, + .iConfiguration = 0, + .bmAttributes = 0xC0, + .bMaxPower = 250 / 2, /* in units of 2ma */ + }, + .hid_if = { + .bLength = sizeof(struct usb_interface_desc), + .bDescriptorType = USB_DESC_TYPE_INTERFACE, + .bInterfaceNumber = 0, + .bAlternateSetting = 0, + .bNumEndpoints = 2, + .bInterfaceClass = USB_INTERFACE_CLASS_HID, + .bInterfaceSubClass = 0, /* no boot */ + .nInterfaceProtocol = 0, /* none */ + .iInterface = 0, + }, + .hid = { + .bLength = sizeof(struct usb_hid_desc), + .bDescriptorType = USB_DESC_TYPE_HID, + .bcdHID = htousbs(0x0110), + .bCountryCode = 0, + .bNumDescriptors = 1, + .bClassDescriptorType = USB_DESC_TYPE_REPORT, + .wItemLength = htousbs(sizeof(hid_report_desc)), + }, + .hid_in = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_IN(1), + .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), + .bInterval = 4, /* ms */ + }, + .hid_out = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_OUT(1), + .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), + .bInterval = 4, /* ms */ + }, }; int32_t PIOS_USB_DESC_HID_ONLY_Init(void) { - PIOS_USBHOOK_RegisterConfig(1, (uint8_t *)&config_hid_only, sizeof(config_hid_only)); + PIOS_USBHOOK_RegisterConfig(1, (uint8_t *)&config_hid_only, sizeof(config_hid_only)); - PIOS_USBHOOK_RegisterDevice((uint8_t *)&device_desc, sizeof(device_desc)); + PIOS_USBHOOK_RegisterDevice((uint8_t *)&device_desc, sizeof(device_desc)); - PIOS_USB_HID_RegisterHidDescriptor((uint8_t *)&(config_hid_only.hid), sizeof(config_hid_only.hid)); - PIOS_USB_HID_RegisterHidReport((uint8_t *)hid_report_desc, sizeof(hid_report_desc)); + PIOS_USB_HID_RegisterHidDescriptor((uint8_t *)&(config_hid_only.hid), sizeof(config_hid_only.hid)); + PIOS_USB_HID_RegisterHidReport((uint8_t *)hid_report_desc, sizeof(hid_report_desc)); - return 0; + return 0; } #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/common/pios_usb_util.c b/flight/pios/common/pios_usb_util.c index 9f7c8c94e..6b169566e 100644 --- a/flight/pios/common/pios_usb_util.c +++ b/flight/pios/common/pios_usb_util.c @@ -34,15 +34,15 @@ #include "pios_usb_util.h" -uint8_t * PIOS_USB_UTIL_AsciiToUtf8(uint8_t * dst, uint8_t * src, uint16_t srclen) +uint8_t *PIOS_USB_UTIL_AsciiToUtf8(uint8_t *dst, uint8_t *src, uint16_t srclen) { - for (uint8_t i = 0; i < srclen; i++) { - *dst = *src; - dst += 2; - src += 1; - } + for (uint8_t i = 0; i < srclen; i++) { + *dst = *src; + dst += 2; + src += 1; + } - return dst; + return dst; } #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/common/pios_video.c b/flight/pios/common/pios_video.c index 8e211d68c..3fcea2070 100644 --- a/flight/pios/common/pios_video.c +++ b/flight/pios/common/pios_video.c @@ -42,28 +42,27 @@ static void flush_spi(); // Private variables extern xSemaphoreHandle osdSemaphore; -static const struct pios_video_cfg * dev_cfg; +static const struct pios_video_cfg *dev_cfg; // Define the buffers. // For 256x192 pixel mode: -// buffer0_level/buffer0_mask becomes buffer_level; and -// buffer1_level/buffer1_mask becomes buffer_mask; +// buffer0_level/buffer0_mask becomes buffer_level; and +// buffer1_level/buffer1_mask becomes buffer_mask; // For 192x128 pixel mode, allocations are as the names are written. // divide by 8 because two bytes to a word. // Must be allocated in one block, so it is in a struct. -struct _buffers -{ - uint8_t buffer0_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; - uint8_t buffer0_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; - uint8_t buffer1_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; - uint8_t buffer1_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH]; -}buffers; +struct _buffers { + uint8_t buffer0_level[GRAPHICS_HEIGHT * GRAPHICS_WIDTH]; + uint8_t buffer0_mask[GRAPHICS_HEIGHT * GRAPHICS_WIDTH]; + uint8_t buffer1_level[GRAPHICS_HEIGHT * GRAPHICS_WIDTH]; + uint8_t buffer1_mask[GRAPHICS_HEIGHT * GRAPHICS_WIDTH]; +} buffers; // Remove the struct definition (makes it easier to write for.) -#define buffer0_level (buffers.buffer0_level) -#define buffer0_mask (buffers.buffer0_mask) -#define buffer1_level (buffers.buffer1_level) -#define buffer1_mask (buffers.buffer1_mask) +#define buffer0_level (buffers.buffer0_level) +#define buffer0_mask (buffers.buffer0_mask) +#define buffer1_level (buffers.buffer1_level) +#define buffer1_mask (buffers.buffer1_mask) // We define pointers to each of these buffers. uint8_t *draw_buffer_level; @@ -71,13 +70,13 @@ uint8_t *draw_buffer_mask; uint8_t *disp_buffer_level; uint8_t *disp_buffer_mask; -volatile uint8_t gLineType = LINE_TYPE_UNKNOWN; -volatile uint16_t gActiveLine = 0; +volatile uint8_t gLineType = LINE_TYPE_UNKNOWN; +volatile uint16_t gActiveLine = 0; volatile uint16_t gActivePixmapLine = 0; -volatile uint16_t line=0; -volatile uint16_t Vsync_update=0; -volatile uint16_t Hsync_update=0; -static int16_t m_osdLines=0; +volatile uint16_t line = 0; +volatile uint16_t Vsync_update = 0; +volatile uint16_t Hsync_update = 0; +static int16_t m_osdLines = 0; /** * swap_buffers: Swaps the two buffers. Contents in the display @@ -90,6 +89,7 @@ void swap_buffers() // dependable and it's only called a few times per second. // Many compliers should optimise these to EXCH instructions. uint8_t *tmp; + SWAP_BUFFS(tmp, disp_buffer_mask, draw_buffer_mask); SWAP_BUFFS(tmp, disp_buffer_level, draw_buffer_level); } @@ -97,7 +97,7 @@ void swap_buffers() bool PIOS_Hsync_ISR() { // On tenth line prepare data which will start clocking out on GRAPHICS_LINE+1 - if(Hsync_update==GRAPHICS_LINE) { + if (Hsync_update == GRAPHICS_LINE) { prepare_line(0); gActiveLine = 1; } @@ -105,7 +105,8 @@ bool PIOS_Hsync_ISR() return true; } -bool PIOS_Vsync_ISR() { +bool PIOS_Vsync_ISR() +{ static portBASE_TYPE xHigherPriorityTaskWoken; xHigherPriorityTaskWoken = pdFALSE; @@ -118,30 +119,30 @@ bool PIOS_Vsync_ISR() { flush_spi(); TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE); - gActiveLine = 0; + gActiveLine = 0; Hsync_update = 0; Vsync_update++; - if(Vsync_update>=2) - { + if (Vsync_update >= 2) { // load second image buffer swap_buffers(); - Vsync_update=0; + Vsync_update = 0; // trigger redraw every second field xHigherPriorityTaskWoken = xSemaphoreGiveFromISR(osdSemaphore, &xHigherPriorityTaskWoken); } - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); //portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); // portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); return xHigherPriorityTaskWoken == pdTRUE; } -uint16_t PIOS_Video_GetOSDLines(void) { +uint16_t PIOS_Video_GetOSDLines(void) +{ return m_osdLines; } /** - * Stops the pixel clock and ensures it ignores the rising edge. To be used after a + * Stops the pixel clock and ensures it ignores the rising edge. To be used after a * vsync until the first line is to be displayed */ static void stop_hsync_timers() @@ -153,21 +154,21 @@ static void stop_hsync_timers() const struct pios_tim_callbacks px_callback = { .overflow = NULL, - .edge = NULL, + .edge = NULL, }; #ifdef PAL const uint32_t period = 10; -const uint32_t dc = (10 / 2); +const uint32_t dc = (10 / 2); #else const uint32_t period = 11; -const uint32_t dc = (11 / 2); +const uint32_t dc = (11 / 2); #endif /** * Reset the timer and configure for next call. Keeps them synced. Ideally this won't even be needed * since I don't think the slave mode gets lost, and this can simply be disable timer */ -uint32_t failcount = 0; +uint32_t failcount = 0; static void reset_hsync_timers() { // Stop both timers @@ -176,25 +177,26 @@ static void reset_hsync_timers() uint32_t tim_id; const struct pios_tim_channel *channels = &dev_cfg->hsync_capture; - //BUG: This is nuts this line is needed. It simply results in allocating - //all the memory but somehow leaving it out breaks the timer functionality. + // BUG: This is nuts this line is needed. It simply results in allocating + // all the memory but somehow leaving it out breaks the timer functionality. // I do not see how these can be related if (failcount == 0) { - if(PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0) < 0) - failcount++; + if (PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0) < 0) { + failcount++; + } } - dev_cfg->pixel_timer.timer->CNT = 0xFFFF - 100; //dc; + dev_cfg->pixel_timer.timer->CNT = 0xFFFF - 100; // dc; // Listen to Channel1 (HSYNC) - switch(dev_cfg->hsync_capture.timer_chan) { - case TIM_Channel_1: + switch (dev_cfg->hsync_capture.timer_chan) { + case TIM_Channel_1: TIM_SelectInputTrigger(dev_cfg->pixel_timer.timer, TIM_TS_TI1FP1); break; - case TIM_Channel_2: + case TIM_Channel_2: TIM_SelectInputTrigger(dev_cfg->pixel_timer.timer, TIM_TS_TI2FP2); break; - default: + default: PIOS_Assert(0); } TIM_SelectSlaveMode(dev_cfg->pixel_timer.timer, TIM_SlaveMode_Trigger); @@ -220,41 +222,41 @@ static void configure_hsync_timers() // Configure the input capture channel TIM_ICInitTypeDef TIM_ICInitStructure; - switch(dev_cfg->hsync_capture.timer_chan) { - case TIM_Channel_1: + switch (dev_cfg->hsync_capture.timer_chan) { + case TIM_Channel_1: TIM_ICInitStructure.TIM_Channel = TIM_Channel_1; break; - case TIM_Channel_2: + case TIM_Channel_2: TIM_ICInitStructure.TIM_Channel = TIM_Channel_2; break; - default: + default: PIOS_Assert(0); } - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; - //TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + // TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; - TIM_ICInitStructure.TIM_ICFilter = 0; + TIM_ICInitStructure.TIM_ICFilter = 0; TIM_ICInit(dev_cfg->pixel_timer.timer, &TIM_ICInitStructure); // Set up the channel to output the pixel clock - switch(dev_cfg->pixel_timer.timer_chan) { - case TIM_Channel_1: + switch (dev_cfg->pixel_timer.timer_chan) { + case TIM_Channel_1: TIM_OC1Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); TIM_OC1PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); TIM_SetCompare1(dev_cfg->pixel_timer.timer, dc); break; - case TIM_Channel_2: + case TIM_Channel_2: TIM_OC2Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); TIM_OC2PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); TIM_SetCompare2(dev_cfg->pixel_timer.timer, dc); break; - case TIM_Channel_3: + case TIM_Channel_3: TIM_OC3Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); TIM_OC3PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); TIM_SetCompare3(dev_cfg->pixel_timer.timer, dc); break; - case TIM_Channel_4: + case TIM_Channel_4: TIM_OC4Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init); TIM_OC4PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable); TIM_SetCompare4(dev_cfg->pixel_timer.timer, dc); @@ -269,11 +271,11 @@ static void configure_hsync_timers() TIM_SetAutoreload(dev_cfg->pixel_timer.timer, period); } -DMA_TypeDef * main_dma; -DMA_TypeDef * mask_dma; -DMA_Stream_TypeDef * main_stream; -DMA_Stream_TypeDef * mask_stream; -void PIOS_Video_Init(const struct pios_video_cfg * cfg) +DMA_TypeDef *main_dma; +DMA_TypeDef *mask_dma; +DMA_Stream_TypeDef *main_stream; +DMA_Stream_TypeDef *mask_stream; +void PIOS_Video_Init(const struct pios_video_cfg *cfg) { dev_cfg = cfg; // store config before enabling interrupt @@ -281,43 +283,42 @@ void PIOS_Video_Init(const struct pios_video_cfg * cfg) /* needed for HW hack */ const GPIO_InitTypeDef initStruct = { - .GPIO_Pin = GPIO_Pin_12, + .GPIO_Pin = GPIO_Pin_12, .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN , + .GPIO_Mode = GPIO_Mode_IN, .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL + .GPIO_PuPd = GPIO_PuPd_NOPULL }; GPIO_Init(GPIOC, &initStruct); /* SPI3 - MASKBUFFER */ - GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->mask.sclk.init)); - GPIO_Init(cfg->mask.miso.gpio, (GPIO_InitTypeDef*)&(cfg->mask.miso.init)); + GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef *)&(cfg->mask.sclk.init)); + GPIO_Init(cfg->mask.miso.gpio, (GPIO_InitTypeDef *)&(cfg->mask.miso.init)); /* SPI1 SLAVE FRAMEBUFFER */ - GPIO_Init(cfg->level.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->level.sclk.init)); - GPIO_Init(cfg->level.miso.gpio, (GPIO_InitTypeDef*)&(cfg->level.miso.init)); + GPIO_Init(cfg->level.sclk.gpio, (GPIO_InitTypeDef *)&(cfg->level.sclk.init)); + GPIO_Init(cfg->level.miso.gpio, (GPIO_InitTypeDef *)&(cfg->level.miso.init)); if (cfg->mask.remap) { GPIO_PinAFConfig(cfg->mask.sclk.gpio, - __builtin_ctz(cfg->mask.sclk.init.GPIO_Pin), - cfg->mask.remap); + __builtin_ctz(cfg->mask.sclk.init.GPIO_Pin), + cfg->mask.remap); GPIO_PinAFConfig(cfg->mask.miso.gpio, - __builtin_ctz(cfg->mask.miso.init.GPIO_Pin), - cfg->mask.remap); + __builtin_ctz(cfg->mask.miso.init.GPIO_Pin), + cfg->mask.remap); } - if (cfg->level.remap) - { + if (cfg->level.remap) { GPIO_PinAFConfig(cfg->level.sclk.gpio, - __builtin_ctz(cfg->level.sclk.init.GPIO_Pin), - cfg->level.remap); + __builtin_ctz(cfg->level.sclk.init.GPIO_Pin), + cfg->level.remap); GPIO_PinAFConfig(cfg->level.miso.gpio, - __builtin_ctz(cfg->level.miso.init.GPIO_Pin), - cfg->level.remap); + __builtin_ctz(cfg->level.miso.init.GPIO_Pin), + cfg->level.remap); } /* Initialize the SPI block */ - SPI_Init(cfg->level.regs, (SPI_InitTypeDef*)&(cfg->level.init)); - SPI_Init(cfg->mask.regs, (SPI_InitTypeDef*)&(cfg->mask.init)); + SPI_Init(cfg->level.regs, (SPI_InitTypeDef *)&(cfg->level.init)); + SPI_Init(cfg->mask.regs, (SPI_InitTypeDef *)&(cfg->mask.init)); /* Enable SPI */ SPI_Cmd(cfg->level.regs, ENABLE); @@ -325,24 +326,24 @@ void PIOS_Video_Init(const struct pios_video_cfg * cfg) /* Configure DMA for SPI Tx SLAVE Maskbuffer */ DMA_Cmd(cfg->mask.dma.tx.channel, DISABLE); - DMA_Init(cfg->mask.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->mask.dma.tx.init)); + DMA_Init(cfg->mask.dma.tx.channel, (DMA_InitTypeDef *)&(cfg->mask.dma.tx.init)); /* Configure DMA for SPI Tx SLAVE Framebuffer*/ DMA_Cmd(cfg->level.dma.tx.channel, DISABLE); - DMA_Init(cfg->level.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->level.dma.tx.init)); + DMA_Init(cfg->level.dma.tx.channel, (DMA_InitTypeDef *)&(cfg->level.dma.tx.init)); /* Trigger interrupt when for half conversions too to indicate double buffer */ DMA_ITConfig(cfg->level.dma.tx.channel, DMA_IT_TC, ENABLE); /* Configure and clear buffers */ draw_buffer_level = buffer0_level; - draw_buffer_mask = buffer0_mask; + draw_buffer_mask = buffer0_mask; disp_buffer_level = buffer1_level; - disp_buffer_mask = buffer1_mask; - memset(disp_buffer_mask, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); - memset(disp_buffer_level, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); - memset(draw_buffer_mask, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); - memset(draw_buffer_level, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT); + disp_buffer_mask = buffer1_mask; + memset(disp_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); + memset(disp_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); + memset(draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); + memset(draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); /* Configure DMA interrupt */ @@ -352,17 +353,17 @@ void PIOS_Video_Init(const struct pios_video_cfg * cfg) SPI_I2S_DMACmd(cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE); SPI_I2S_DMACmd(cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE); - mask_dma = DMA1; - main_dma = DMA2; + mask_dma = DMA1; + main_dma = DMA2; main_stream = cfg->level.dma.tx.channel; mask_stream = cfg->mask.dma.tx.channel; /* Configure the Video Line interrupt */ PIOS_EXTI_Init(cfg->hsync); PIOS_EXTI_Init(cfg->vsync); - //set levels to zero - PIOS_Servo_Set(0,0); - PIOS_Servo_Set(1,0); + // set levels to zero + PIOS_Servo_Set(0, 0); + PIOS_Servo_Set(1, 0); } /** @@ -371,8 +372,7 @@ void PIOS_Video_Init(const struct pios_video_cfg * cfg) */ static void prepare_line(uint32_t line_num) { - if(line_numpixel_timer.timer->CNT = dc; @@ -381,8 +381,8 @@ static void prepare_line(uint32_t line_num) DMA_ClearFlag(dev_cfg->level.dma.tx.channel, DMA_FLAG_FEIF5 | DMA_FLAG_TEIF5); // Load new line - DMA_MemoryTargetConfig(dev_cfg->level.dma.tx.channel,(uint32_t)&disp_buffer_level[buf_offset],DMA_Memory_0); - DMA_MemoryTargetConfig(dev_cfg->mask.dma.tx.channel,(uint32_t)&disp_buffer_mask[buf_offset],DMA_Memory_0); + DMA_MemoryTargetConfig(dev_cfg->level.dma.tx.channel, (uint32_t)&disp_buffer_level[buf_offset], DMA_Memory_0); + DMA_MemoryTargetConfig(dev_cfg->mask.dma.tx.channel, (uint32_t)&disp_buffer_mask[buf_offset], DMA_Memory_0); // Enable DMA, Slave first DMA_SetCurrDataCounter(dev_cfg->level.dma.tx.channel, BUFFER_LINE_LENGTH); @@ -402,24 +402,23 @@ static void prepare_line(uint32_t line_num) } void PIOS_VIDEO_DMA_Handler(void); -void DMA1_Stream7_IRQHandler(void) __attribute__ ((alias("PIOS_VIDEO_DMA_Handler"))); -void DMA2_Stream5_IRQHandler(void) __attribute__ ((alias("PIOS_VIDEO_DMA_Handler"))); +void DMA1_Stream7_IRQHandler(void) __attribute__((alias("PIOS_VIDEO_DMA_Handler"))); +void DMA2_Stream5_IRQHandler(void) __attribute__((alias("PIOS_VIDEO_DMA_Handler"))); /** * Check both SPI for the stop sequence before disabling them */ static void flush_spi() { - bool level_empty = false; - bool mask_empty = false; + bool level_empty = false; + bool mask_empty = false; bool level_stopped = false; - bool mask_stopped = false; + bool mask_stopped = false; // Can't flush if clock not running - while((dev_cfg->pixel_timer.timer->CR1 & 0x0001) && ( !level_stopped | !mask_stopped )) { - - level_empty |= SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == SET; - mask_empty |= SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == SET; + while ((dev_cfg->pixel_timer.timer->CR1 & 0x0001) && (!level_stopped | !mask_stopped)) { + level_empty |= SPI_I2S_GetFlagStatus(dev_cfg->level.regs, SPI_I2S_FLAG_TXE) == SET; + mask_empty |= SPI_I2S_GetFlagStatus(dev_cfg->mask.regs, SPI_I2S_FLAG_TXE) == SET; if (level_empty && !level_stopped) { // && SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == RESET) { SPI_Cmd(dev_cfg->level.regs, DISABLE); @@ -432,11 +431,11 @@ static void flush_spi() } } /* - uint32_t i = 0; - while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; - while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; - while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++; - while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++;*/ + uint32_t i = 0; + while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; + while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; + while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++; + while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++;*/ SPI_Cmd(dev_cfg->mask.regs, DISABLE); SPI_Cmd(dev_cfg->level.regs, DISABLE); } @@ -447,20 +446,17 @@ static void flush_spi() void PIOS_VIDEO_DMA_Handler(void) { // Handle flags from stream channel - if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel,DMA_FLAG_TCIF5)) { // whole double buffer filled - DMA_ClearFlag(dev_cfg->level.dma.tx.channel,DMA_FLAG_TCIF5); - if(gActiveLine < GRAPHICS_HEIGHT) - { + if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel, DMA_FLAG_TCIF5)) { // whole double buffer filled + DMA_ClearFlag(dev_cfg->level.dma.tx.channel, DMA_FLAG_TCIF5); + if (gActiveLine < GRAPHICS_HEIGHT) { flush_spi(); stop_hsync_timers(); dev_cfg->pixel_timer.timer->CNT = dc; prepare_line(gActiveLine); - } - else if(gActiveLine >= GRAPHICS_HEIGHT) - { - //last line completed + } else if (gActiveLine >= GRAPHICS_HEIGHT) { + // last line completed flush_spi(); stop_hsync_timers(); @@ -469,12 +465,9 @@ void PIOS_VIDEO_DMA_Handler(void) DMA_Cmd(dev_cfg->level.dma.tx.channel, DISABLE); } gActiveLine++; - } - else if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel,DMA_FLAG_HTIF5)) { - DMA_ClearFlag(dev_cfg->level.dma.tx.channel,DMA_FLAG_HTIF5); - } - else { - } + } else if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel, DMA_FLAG_HTIF5)) { + DMA_ClearFlag(dev_cfg->level.dma.tx.channel, DMA_FLAG_HTIF5); + } else {} } #endif /* PIOS_INCLUDE_VIDEO */ diff --git a/flight/pios/common/pios_wavplay.c b/flight/pios/common/pios_wavplay.c index d2b82d65a..564e7b859 100644 --- a/flight/pios/common/pios_wavplay.c +++ b/flight/pios/common/pios_wavplay.c @@ -33,518 +33,488 @@ #ifdef PIOS_INCLUDE_WAVE -static const struct pios_dac_cfg * dev_cfg; +static const struct pios_dac_cfg *dev_cfg; -typedef enum -{ - LittleEndian, - BigEndian -}Endianness; +typedef enum { + LittleEndian, + BigEndian +} Endianness; /** - * @} - */ + * @} + */ /** @defgroup WAVEPLAYER_Private_Defines - * @{ - */ -#define CHUNK_ID 0x52494646 /* correspond to the letters 'RIFF' */ -#define FILE_FORMAT 0x57415645 /* correspond to the letters 'WAVE' */ -#define FORMAT_ID 0x666D7420 /* correspond to the letters 'fmt ' */ -#define DATA_ID 0x64617461 /* correspond to the letters 'data' */ -#define FACT_ID 0x66616374 /* correspond to the letters 'fact' */ -#define WAVE_FORMAT_PCM 0x01 -#define FORMAT_CHNUK_SIZE 0x10 -#define CHANNEL_MONO 0x01 -#define SAMPLE_RATE_8000 8000 -#define SAMPLE_RATE_11025 11025 -#define SAMPLE_RATE_22050 22050 -#define SAMPLE_RATE_44100 44100 -#define BITS_PER_SAMPLE_8 8 -#define WAVE_DUMMY_BYTE 0xA5 -#define DAC_DHLCD_REG_8LCD_REG_1_ADDRESS 0x40007410 + * @{ + */ +#define CHUNK_ID 0x52494646 /* correspond to the letters 'RIFF' */ +#define FILE_FORMAT 0x57415645 /* correspond to the letters 'WAVE' */ +#define FORMAT_ID 0x666D7420 /* correspond to the letters 'fmt ' */ +#define DATA_ID 0x64617461 /* correspond to the letters 'data' */ +#define FACT_ID 0x66616374 /* correspond to the letters 'fact' */ +#define WAVE_FORMAT_PCM 0x01 +#define FORMAT_CHNUK_SIZE 0x10 +#define CHANNEL_MONO 0x01 +#define SAMPLE_RATE_8000 8000 +#define SAMPLE_RATE_11025 11025 +#define SAMPLE_RATE_22050 22050 +#define SAMPLE_RATE_44100 44100 +#define BITS_PER_SAMPLE_8 8 +#define WAVE_DUMMY_BYTE 0xA5 +#define DAC_DHLCD_REG_8LCD_REG_1_ADDRESS 0x40007410 -typedef struct -{ - uint32_t RIFFchunksize; - uint16_t FormatTag; - uint16_t NumChannels; - uint32_t SampleRate; - uint32_t ByteRate; - uint16_t BlockAlign; - uint16_t BitsPerSample; - uint32_t DataSize; -} -WAVE_FormatTypeDef; -typedef enum -{ - Valid_WAVE_File = 0, - Unvalid_RIFF_ID, - Unvalid_WAVE_Format, - Unvalid_FormatChunk_ID, - Unsupporetd_FormatTag, - Unsupporetd_Number_Of_Channel, - Unsupporetd_Sample_Rate, - Unsupporetd_Bits_Per_Sample, - Unvalid_DataChunk_ID, - Unsupporetd_ExtraFormatBytes, - Unvalid_FactChunk_ID +typedef struct { + uint32_t RIFFchunksize; + uint16_t FormatTag; + uint16_t NumChannels; + uint32_t SampleRate; + uint32_t ByteRate; + uint16_t BlockAlign; + uint16_t BitsPerSample; + uint32_t DataSize; +} WAVE_FormatTypeDef; +typedef enum { + Valid_WAVE_File = 0, + Unvalid_RIFF_ID, + Unvalid_WAVE_Format, + Unvalid_FormatChunk_ID, + Unsupporetd_FormatTag, + Unsupporetd_Number_Of_Channel, + Unsupporetd_Sample_Rate, + Unsupporetd_Bits_Per_Sample, + Unvalid_DataChunk_ID, + Unsupporetd_ExtraFormatBytes, + Unvalid_FactChunk_ID } ErrorCode; /** - * @} - */ + * @} + */ /** @defgroup WAVEPLAYER_Exported_Constants - * @{ - */ -#define SpeechReadAddr 0x0 /* Speech wave start read address */ + * @{ + */ +#define SpeechReadAddr 0x0 /* Speech wave start read address */ /* Audio Play STATUS */ -#define AudioPlayStatus_STOPPED 0 -#define AudioPlayStatus_PLAYING 1 -#define AudioPlayStatus_PAUSED 2 +#define AudioPlayStatus_STOPPED 0 +#define AudioPlayStatus_PLAYING 1 +#define AudioPlayStatus_PAUSED 2 -#define MAX_WAVE_FILES 25 +#define MAX_WAVE_FILES 25 /** - * @} - */ + * @} + */ /** @defgroup WAVEPLAYER_Private_Macros - * @{ - */ + * @{ + */ /** - * @} - */ + * @} + */ /** @defgroup WAVEPLAYER_Private_Variables - * @{ - */ + * @{ + */ static WAVE_FormatTypeDef WAVE_Format; -static ErrorCode WaveFileStatus = Unvalid_RIFF_ID; -static uint16_t TIM6ARRValue = 1088; +static ErrorCode WaveFileStatus = Unvalid_RIFF_ID; +static uint16_t TIM6ARRValue = 1088; uint32_t WaveDataLength = 0; static uint32_t SpeechDataOffset = 0x00; static uint32_t wavelen = 0; FILEINFO fiwave; FILEINFO file; -static uint8_t buffer1[BUFFERSIZE], buffer2[BUFFERSIZE]={0}; //Two cycling buffers which contain the WAV data. +static uint8_t buffer1[BUFFERSIZE], buffer2[BUFFERSIZE] = { 0 }; // Two cycling buffers which contain the WAV data. uint32_t wavecounter; -typedef struct -{ - unsigned int format; - unsigned int sample_rate; - unsigned int bits_per_sample; -}wave_format; +typedef struct { + unsigned int format; + unsigned int sample_rate; + unsigned int bits_per_sample; +} wave_format; wave_format wave_info; /** - * @brief Decrements the played wave data length. - * @param None - * @retval Current value of WaveDataLength variable. - */ + * @brief Decrements the played wave data length. + * @param None + * @retval Current value of WaveDataLength variable. + */ uint32_t Decrement_WaveDataLength(void) { - if (WaveDataLength != 0x00) - { - WaveDataLength--; - } - return (WaveDataLength); + if (WaveDataLength != 0x00) { + WaveDataLength--; + } + return WaveDataLength; } /** - * @brief Decrements the played wave data length. - * @param None - * @retval Current value of WaveDataLength variable. - */ + * @brief Decrements the played wave data length. + * @param None + * @retval Current value of WaveDataLength variable. + */ void Set_WaveDataLength(uint32_t value) { - WaveDataLength = value; + WaveDataLength = value; } /** - * @brief Reads a number of bytes from the SPI Flash and reorder them in Big - * or little endian. - * @param NbrOfBytes: number of bytes to read. - * This parameter must be a number between 1 and 4. - * @param ReadAddr: external memory address to read from. - * @param Endians: specifies the bytes endianness. - * This parameter can be one of the following values: - * - LittleEndian - * - BigEndian - * @retval Bytes read from the SPI Flash. - */ + * @brief Reads a number of bytes from the SPI Flash and reorder them in Big + * or little endian. + * @param NbrOfBytes: number of bytes to read. + * This parameter must be a number between 1 and 4. + * @param ReadAddr: external memory address to read from. + * @param Endians: specifies the bytes endianness. + * This parameter can be one of the following values: + * - LittleEndian + * - BigEndian + * @retval Bytes read from the SPI Flash. + */ static uint32_t ReadUnit(uint8_t *buffer, uint8_t idx, uint8_t NbrOfBytes, Endianness BytesFormat) { - uint32_t index = 0; - uint32_t Temp = 0; + uint32_t index = 0; + uint32_t Temp = 0; - for (index = 0; index < NbrOfBytes; index++) - { - Temp |= buffer[idx + index] << (index * 8); - } + for (index = 0; index < NbrOfBytes; index++) { + Temp |= buffer[idx + index] << (index * 8); + } - if (BytesFormat == BigEndian) - { - Temp = __REV(Temp); - } - return Temp; + if (BytesFormat == BigEndian) { + Temp = __REV(Temp); + } + return Temp; } /** - * @brief Checks the format of the .WAV file and gets information about - * the audio format. This is done by reading the value of a - * number of parameters stored in the file header and comparing - * these to the values expected authenticates the format of a - * standard .WAV file (44 bytes will be read). If it is a valid - * .WAV file format, it continues reading the header to determine - * the audio format such as the sample rate and the sampled data - * size. If the audio format is supported by this application, - * it retrieves the audio format in WAVE_Format structure and - * returns a zero value. Otherwise the function fails and the - * return value is nonzero.In this case, the return value specifies - * the cause of the function fails. The error codes that can be - * returned by this function are declared in the header file. - * @param None - * @retval Zero value if the function succeed, otherwise it return - * a nonzero value which specifies the error code. - */ + * @brief Checks the format of the .WAV file and gets information about + * the audio format. This is done by reading the value of a + * number of parameters stored in the file header and comparing + * these to the values expected authenticates the format of a + * standard .WAV file (44 bytes will be read). If it is a valid + * .WAV file format, it continues reading the header to determine + * the audio format such as the sample rate and the sampled data + * size. If the audio format is supported by this application, + * it retrieves the audio format in WAVE_Format structure and + * returns a zero value. Otherwise the function fails and the + * return value is nonzero.In this case, the return value specifies + * the cause of the function fails. The error codes that can be + * returned by this function are declared in the header file. + * @param None + * @retval Zero value if the function succeed, otherwise it return + * a nonzero value which specifies the error code. + */ static ErrorCode WavePlayer_WaveParsing(uint8_t *DirName, uint8_t *FileName, uint32_t *FileLen) { - uint32_t Temp = 0x00; - uint32_t ExtraFormatBytes = 0; - __IO uint32_t err = 0; - uint32_t number_of_clusters; - uint32_t i; + uint32_t Temp = 0x00; + uint32_t ExtraFormatBytes = 0; + __IO uint32_t err = 0; + uint32_t number_of_clusters; + uint32_t i; - /* Directory enumeration test */ - if (PIOS_FOPEN_READ(FileName, file)) - { - err = 1; - } - else - { - *FileLen = file.filelen; - number_of_clusters = file.filelen / 512; - if ((file.filelen % SECTOR_SIZE) > 0) - { - number_of_clusters ++; + /* Directory enumeration test */ + if (PIOS_FOPEN_READ(FileName, file)) { + err = 1; + } else { + *FileLen = file.filelen; + number_of_clusters = file.filelen / 512; + if ((file.filelen % SECTOR_SIZE) > 0) { + number_of_clusters++; + } } - } - PIOS_FREAD(&file, buffer1, 44, &i); - //DFS_ReadFile(&file, sector, buffer1, &i, SECTOR_SIZE); + PIOS_FREAD(&file, buffer1, 44, &i); + // DFS_ReadFile(&file, sector, buffer1, &i, SECTOR_SIZE); - /* Read chunkID, must be 'RIFF' ----------------------------------------------*/ - Temp = ReadUnit(buffer1, 0, 4, BigEndian); - if (Temp != CHUNK_ID) - { - return(Unvalid_RIFF_ID); - } + /* Read chunkID, must be 'RIFF' ----------------------------------------------*/ + Temp = ReadUnit(buffer1, 0, 4, BigEndian); + if (Temp != CHUNK_ID) { + return Unvalid_RIFF_ID; + } - /* Read the file length ----------------------------------------------------*/ - WAVE_Format.RIFFchunksize = ReadUnit(buffer1, 4, 4, LittleEndian); + /* Read the file length ----------------------------------------------------*/ + WAVE_Format.RIFFchunksize = ReadUnit(buffer1, 4, 4, LittleEndian); - /* Read the file format, must be 'WAVE' ------------------------------------*/ - Temp = ReadUnit(buffer1, 8, 4, BigEndian); - if (Temp != FILE_FORMAT) - { - return(Unvalid_WAVE_Format); - } + /* Read the file format, must be 'WAVE' ------------------------------------*/ + Temp = ReadUnit(buffer1, 8, 4, BigEndian); + if (Temp != FILE_FORMAT) { + return Unvalid_WAVE_Format; + } - /* Read the format chunk, must be'fmt ' --------------------------------------*/ - Temp = ReadUnit(buffer1, 12, 4, BigEndian); - if (Temp != FORMAT_ID) - { - return(Unvalid_FormatChunk_ID); - } - /* Read the length of the 'fmt' data, must be 0x10 -------------------------*/ - Temp = ReadUnit(buffer1, 16, 4, LittleEndian); - if (Temp != 0x10) - { - ExtraFormatBytes = 1; - } - /* Read the audio format, must be 0x01 (PCM) -------------------------------*/ - WAVE_Format.FormatTag = ReadUnit(buffer1, 20, 2, LittleEndian); - if (WAVE_Format.FormatTag != WAVE_FORMAT_PCM) - { - return(Unsupporetd_FormatTag); - } + /* Read the format chunk, must be'fmt ' --------------------------------------*/ + Temp = ReadUnit(buffer1, 12, 4, BigEndian); + if (Temp != FORMAT_ID) { + return Unvalid_FormatChunk_ID; + } + /* Read the length of the 'fmt' data, must be 0x10 -------------------------*/ + Temp = ReadUnit(buffer1, 16, 4, LittleEndian); + if (Temp != 0x10) { + ExtraFormatBytes = 1; + } + /* Read the audio format, must be 0x01 (PCM) -------------------------------*/ + WAVE_Format.FormatTag = ReadUnit(buffer1, 20, 2, LittleEndian); + if (WAVE_Format.FormatTag != WAVE_FORMAT_PCM) { + return Unsupporetd_FormatTag; + } - /* Read the number of channels, must be 0x01 (Mono) ------------------------*/ - WAVE_Format.NumChannels = ReadUnit(buffer1, 22, 2, LittleEndian); - if (WAVE_Format.NumChannels != CHANNEL_MONO) - { - return(Unsupporetd_Number_Of_Channel); - } + /* Read the number of channels, must be 0x01 (Mono) ------------------------*/ + WAVE_Format.NumChannels = ReadUnit(buffer1, 22, 2, LittleEndian); + if (WAVE_Format.NumChannels != CHANNEL_MONO) { + return Unsupporetd_Number_Of_Channel; + } - /* Read the Sample Rate ----------------------------------------------------*/ - WAVE_Format.SampleRate = ReadUnit(buffer1, 24, 4, LittleEndian); - /* Update the OCA value according to the .WAV file Sample Rate */ - switch (WAVE_Format.SampleRate) - { - case SAMPLE_RATE_8000 : - TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK)/8000; - break; /* 8KHz = 24MHz / 3000 */ + /* Read the Sample Rate ----------------------------------------------------*/ + WAVE_Format.SampleRate = ReadUnit(buffer1, 24, 4, LittleEndian); + /* Update the OCA value according to the .WAV file Sample Rate */ + switch (WAVE_Format.SampleRate) { + case SAMPLE_RATE_8000: + TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK) / 8000; + break; /* 8KHz = 24MHz / 3000 */ case SAMPLE_RATE_11025: - TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK)/11025; - break; /* 11.025KHz = 24MHz / 2176 */ + TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK) / 11025; + break; /* 11.025KHz = 24MHz / 2176 */ case SAMPLE_RATE_22050: - TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK)/22050; - break; /* 22.05KHz = 24MHz / 1088 */ + TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK) / 22050; + break; /* 22.05KHz = 24MHz / 1088 */ case SAMPLE_RATE_44100: - TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK)/44100; - break; /* 44.1KHz = 24MHz / 544 */ + TIM6ARRValue = (PIOS_PERIPHERAL_APB1_CLOCK) / 44100; + break; /* 44.1KHz = 24MHz / 544 */ default: - return(Unsupporetd_Sample_Rate); - } - - /* Read the Byte Rate ------------------------------------------------------*/ - WAVE_Format.ByteRate = ReadUnit(buffer1, 28, 4, LittleEndian); - - /* Read the block alignment ------------------------------------------------*/ - WAVE_Format.BlockAlign = ReadUnit(buffer1, 32, 2, LittleEndian); - - /* Read the number of bits per sample --------------------------------------*/ - WAVE_Format.BitsPerSample = ReadUnit(buffer1, 34, 2, LittleEndian); - if (WAVE_Format.BitsPerSample != BITS_PER_SAMPLE_8) - { - return(Unsupporetd_Bits_Per_Sample); - } - SpeechDataOffset = 36; - /* If there is Extra format bytes, these bytes will be defined in "Fact Chunk" */ - if (ExtraFormatBytes == 1) - { - /* Read th Extra format bytes, must be 0x00 ------------------------------*/ - Temp = ReadUnit(buffer1, 36, 2, LittleEndian); - if (Temp != 0x00) - { - return(Unsupporetd_ExtraFormatBytes); + return Unsupporetd_Sample_Rate; } - /* Read the Fact chunk, must be 'fact' -----------------------------------*/ - Temp = ReadUnit(buffer1, 38, 4, BigEndian); - if (Temp != FACT_ID) - { - return(Unvalid_FactChunk_ID); + + /* Read the Byte Rate ------------------------------------------------------*/ + WAVE_Format.ByteRate = ReadUnit(buffer1, 28, 4, LittleEndian); + + /* Read the block alignment ------------------------------------------------*/ + WAVE_Format.BlockAlign = ReadUnit(buffer1, 32, 2, LittleEndian); + + /* Read the number of bits per sample --------------------------------------*/ + WAVE_Format.BitsPerSample = ReadUnit(buffer1, 34, 2, LittleEndian); + if (WAVE_Format.BitsPerSample != BITS_PER_SAMPLE_8) { + return Unsupporetd_Bits_Per_Sample; } - /* Read Fact chunk data Size ---------------------------------------------*/ - Temp = ReadUnit(buffer1, 42, 4, LittleEndian); + SpeechDataOffset = 36; + /* If there is Extra format bytes, these bytes will be defined in "Fact Chunk" */ + if (ExtraFormatBytes == 1) { + /* Read th Extra format bytes, must be 0x00 ------------------------------*/ + Temp = ReadUnit(buffer1, 36, 2, LittleEndian); + if (Temp != 0x00) { + return Unsupporetd_ExtraFormatBytes; + } + /* Read the Fact chunk, must be 'fact' -----------------------------------*/ + Temp = ReadUnit(buffer1, 38, 4, BigEndian); + if (Temp != FACT_ID) { + return Unvalid_FactChunk_ID; + } + /* Read Fact chunk data Size ---------------------------------------------*/ + Temp = ReadUnit(buffer1, 42, 4, LittleEndian); - SpeechDataOffset += 10 + Temp; - } - /* Read the Data chunk, must be 'data' ---------------------------------------*/ - Temp = ReadUnit(buffer1, SpeechDataOffset, 4, BigEndian); - SpeechDataOffset += 4; - if (Temp != DATA_ID) - { - return(Unvalid_DataChunk_ID); - } + SpeechDataOffset += 10 + Temp; + } + /* Read the Data chunk, must be 'data' ---------------------------------------*/ + Temp = ReadUnit(buffer1, SpeechDataOffset, 4, BigEndian); + SpeechDataOffset += 4; + if (Temp != DATA_ID) { + return Unvalid_DataChunk_ID; + } - /* Read the number of sample data ------------------------------------------*/ - WAVE_Format.DataSize = ReadUnit(buffer1, SpeechDataOffset, 4, LittleEndian); - SpeechDataOffset += 4; - wavecounter = SpeechDataOffset; - PIOS_FREAD(&file, buffer1, SECTOR_SIZE, &i); - PIOS_FREAD(&file, buffer2, SECTOR_SIZE, &i); - return(Valid_WAVE_File); + /* Read the number of sample data ------------------------------------------*/ + WAVE_Format.DataSize = ReadUnit(buffer1, SpeechDataOffset, 4, LittleEndian); + SpeechDataOffset += 4; + wavecounter = SpeechDataOffset; + PIOS_FREAD(&file, buffer1, SECTOR_SIZE, &i); + PIOS_FREAD(&file, buffer2, SECTOR_SIZE, &i); + return Valid_WAVE_File; } /** - * @brief Start wave playing - * @param None - * @retval None - */ -int wavfile=0; -const uint8_t table[5][20] = {"openpilo.wav","uav.wav","beepsoun.wav", "warning.wav", "lowaltit.wav"}; + * @brief Start wave playing + * @param None + * @retval None + */ +int wavfile = 0; +const uint8_t table[5][20] = { "openpilo.wav", "uav.wav", "beepsoun.wav", "warning.wav", "lowaltit.wav" }; uint8_t WavePlayer_Start(void) { - // Check for file system availability - if (PIOS_SDCARD_IsMounted() == 0) { - return -1; - } + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return -1; + } - /* Read the Speech wave file status */ + /* Read the Speech wave file status */ - if(wavfile<5) - { - WaveFileStatus = WavePlayer_WaveParsing(" ", table[wavfile++], &wavelen); - if(wavfile>4) - { - wavfile=5; - } - //TIM_ITConfig(TIM6, TIM_IT_Update, ENABLE); - //WaveDataLength = WAVE_Format.DataSize; - //TIM_Cmd(TIM6, ENABLE); - if (WaveFileStatus == Valid_WAVE_File) /* the .WAV file is valid */ - { - /* Set WaveDataLenght to the Speech wave length */ - WaveDataLength = WAVE_Format.DataSize; + if (wavfile < 5) { + WaveFileStatus = WavePlayer_WaveParsing(" ", table[wavfile++], &wavelen); + if (wavfile > 4) { + wavfile = 5; + } + // TIM_ITConfig(TIM6, TIM_IT_Update, ENABLE); + // WaveDataLength = WAVE_Format.DataSize; + // TIM_Cmd(TIM6, ENABLE); + if (WaveFileStatus == Valid_WAVE_File) { /* the .WAV file is valid */ + /* Set WaveDataLenght to the Speech wave length */ + WaveDataLength = WAVE_Format.DataSize; - TIM_Cmd(dev_cfg->timer, DISABLE); - TIM_SetAutoreload(dev_cfg->timer, TIM6ARRValue); - /* Start TIM6 */ - TIM_Cmd(dev_cfg->timer, ENABLE); - } - else - { - return -1; - } - } - return 0; + TIM_Cmd(dev_cfg->timer, DISABLE); + TIM_SetAutoreload(dev_cfg->timer, TIM6ARRValue); + /* Start TIM6 */ + TIM_Cmd(dev_cfg->timer, ENABLE); + } else { + return -1; + } + } + return 0; } -#define TIM6_PERIOD (PIOS_PERIPHERAL_APB1_CLOCK)/44100 -void PIOS_WavPlay_Init(const struct pios_dac_cfg * cfg){ - - dev_cfg = cfg; // store config before enabling interrupt +#define TIM6_PERIOD (PIOS_PERIPHERAL_APB1_CLOCK) / 44100 +void PIOS_WavPlay_Init(const struct pios_dac_cfg *cfg) +{ + dev_cfg = cfg; // store config before enabling interrupt #if 0 - GPIO_InitTypeDef GPIO_InitStructure; - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - DMA_InitTypeDef DMA_InitStructure; - NVIC_InitTypeDef NVIC_InitStructure; - DAC_InitTypeDef DAC_InitStructure; + GPIO_InitTypeDef GPIO_InitStructure; + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + DMA_InitTypeDef DMA_InitStructure; + NVIC_InitTypeDef NVIC_InitStructure; + DAC_InitTypeDef DAC_InitStructure; - /* DAC channel 1 & 2 (DAC_OUT1 = PA.4)(DAC_OUT2 = PA.5) configuration */ - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_Init(GPIOA, &GPIO_InitStructure); + /* DAC channel 1 & 2 (DAC_OUT1 = PA.4)(DAC_OUT2 = PA.5) configuration */ + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_Init(GPIOA, &GPIO_InitStructure); - NVIC_InitStructure.NVIC_IRQChannel = TIM6_DAC_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); + NVIC_InitStructure.NVIC_IRQChannel = TIM6_DAC_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); - TIM_DeInit(TIM6); + TIM_DeInit(TIM6); - /* Time base configuration */ - TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Period = TIM6_PERIOD; - TIM_TimeBaseStructure.TIM_Prescaler = 0; - TIM_TimeBaseStructure.TIM_ClockDivision = 0; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(TIM6, &TIM_TimeBaseStructure); + /* Time base configuration */ + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + TIM_TimeBaseStructure.TIM_Period = TIM6_PERIOD; + TIM_TimeBaseStructure.TIM_Prescaler = 0; + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInit(TIM6, &TIM_TimeBaseStructure); - /* TIM6 TRGO selection */ - TIM_SelectOutputTrigger(TIM6, TIM_TRGOSource_Update); + /* TIM6 TRGO selection */ + TIM_SelectOutputTrigger(TIM6, TIM_TRGOSource_Update); - NVIC_InitStructure.NVIC_IRQChannel = DMA1_Stream5_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); + NVIC_InitStructure.NVIC_IRQChannel = DMA1_Stream5_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); - /* DMA1_Stream5 channel7 configuration **************************************/ - DMA_DeInit(DMA1_Stream5); - DMA_InitStructure.DMA_Channel = DMA_Channel_7; - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&DAC->DHR8R1; - DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&buffer1; - DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; - DMA_InitStructure.DMA_BufferSize = BUFFERSIZE; - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; - DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; - DMA_InitStructure.DMA_Priority = DMA_Priority_High; - DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; - DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full; - DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; - DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; - DMA_Init(DMA1_Stream5, &DMA_InitStructure); - /* Configure double buffering */ - DMA_DoubleBufferModeConfig(DMA1_Stream5,(uint32_t)&buffer2,DMA_Memory_0); - DMA_DoubleBufferModeCmd(DMA1_Stream5,ENABLE); + /* DMA1_Stream5 channel7 configuration **************************************/ + DMA_DeInit(DMA1_Stream5); + DMA_InitStructure.DMA_Channel = DMA_Channel_7; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&DAC->DHR8R1; + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&buffer1; + DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; + DMA_InitStructure.DMA_BufferSize = BUFFERSIZE; + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; + DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; + DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full; + DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + DMA_Init(DMA1_Stream5, &DMA_InitStructure); + /* Configure double buffering */ + DMA_DoubleBufferModeConfig(DMA1_Stream5, (uint32_t)&buffer2, DMA_Memory_0); + DMA_DoubleBufferModeCmd(DMA1_Stream5, ENABLE); - /* Enable double buffering */ - DMA_Cmd(DMA1_Stream5, ENABLE); - DMA_ITConfig(DMA1_Stream5, DMA_IT_TC, ENABLE); + /* Enable double buffering */ + DMA_Cmd(DMA1_Stream5, ENABLE); + DMA_ITConfig(DMA1_Stream5, DMA_IT_TC, ENABLE); - /* DAC channel1 Configuration */ - DAC_DeInit(); - DAC_StructInit(&DAC_InitStructure); + /* DAC channel1 Configuration */ + DAC_DeInit(); + DAC_StructInit(&DAC_InitStructure); - DAC_InitStructure.DAC_Trigger = DAC_Trigger_T6_TRGO; - DAC_InitStructure.DAC_WaveGeneration = DAC_WaveGeneration_None; - DAC_InitStructure.DAC_OutputBuffer = DAC_OutputBuffer_Enable; - DAC_Init(DAC_Channel_1, &DAC_InitStructure); + DAC_InitStructure.DAC_Trigger = DAC_Trigger_T6_TRGO; + DAC_InitStructure.DAC_WaveGeneration = DAC_WaveGeneration_None; + DAC_InitStructure.DAC_OutputBuffer = DAC_OutputBuffer_Enable; + DAC_Init(DAC_Channel_1, &DAC_InitStructure); - DAC_Cmd(DAC_Channel_1, ENABLE); - DAC_DMACmd(DAC_Channel_1, ENABLE); -#endif + DAC_Cmd(DAC_Channel_1, ENABLE); + DAC_DMACmd(DAC_Channel_1, ENABLE); +#endif /* if 0 */ #if 1 - GPIO_Init(cfg->dac_io.gpio, (GPIO_InitTypeDef*)&(cfg->dac_io.init)); + GPIO_Init(cfg->dac_io.gpio, (GPIO_InitTypeDef *)&(cfg->dac_io.init)); - /* Configure the dividers for this timer */ - TIM_TimeBaseInit(cfg->timer, &cfg->time_base_init); + /* Configure the dividers for this timer */ + TIM_TimeBaseInit(cfg->timer, &cfg->time_base_init); - /* Enable Interrupts */ - NVIC_Init(&cfg->irq.init); + /* Enable Interrupts */ + NVIC_Init(&cfg->irq.init); - TIM_SelectOutputTrigger(cfg->timer, TIM_TRGOSource_Update); + TIM_SelectOutputTrigger(cfg->timer, TIM_TRGOSource_Update); - NVIC_Init(&cfg->dma.irq.init); + NVIC_Init(&cfg->dma.irq.init); - DMA_Cmd(cfg->dma.tx.channel, DISABLE); - DMA_Init(cfg->dma.tx.channel, (DMA_InitTypeDef*)&(cfg->dma.tx.init)); + DMA_Cmd(cfg->dma.tx.channel, DISABLE); + DMA_Init(cfg->dma.tx.channel, (DMA_InitTypeDef *)&(cfg->dma.tx.init)); - /* Enable double buffering */ - DMA_MemoryTargetConfig(cfg->dma.tx.channel,(uint32_t)&buffer1,DMA_Memory_0); - DMA_DoubleBufferModeConfig(cfg->dma.tx.channel,(uint32_t)&buffer2,DMA_Memory_0); - DMA_DoubleBufferModeCmd(cfg->dma.tx.channel,ENABLE); + /* Enable double buffering */ + DMA_MemoryTargetConfig(cfg->dma.tx.channel, (uint32_t)&buffer1, DMA_Memory_0); + DMA_DoubleBufferModeConfig(cfg->dma.tx.channel, (uint32_t)&buffer2, DMA_Memory_0); + DMA_DoubleBufferModeCmd(cfg->dma.tx.channel, ENABLE); - DMA_Cmd(cfg->dma.tx.channel, ENABLE); - DMA_ITConfig(cfg->dma.tx.channel, DMA_IT_TC, ENABLE); + DMA_Cmd(cfg->dma.tx.channel, ENABLE); + DMA_ITConfig(cfg->dma.tx.channel, DMA_IT_TC, ENABLE); - DAC_Init(cfg->channel, (DAC_InitTypeDef*)&(cfg->dac_init)); + DAC_Init(cfg->channel, (DAC_InitTypeDef *)&(cfg->dac_init)); - DAC_Cmd(cfg->channel, ENABLE); - DAC_DMACmd(cfg->channel, ENABLE); -#endif - //WavePlayer_Start(); + DAC_Cmd(cfg->channel, ENABLE); + DAC_DMACmd(cfg->channel, ENABLE); +#endif /* if 1 */ + // WavePlayer_Start(); } void WavePlayer_Stop(void) { - /* Disable TIM6 update interrupt */ - TIM_ITConfig(dev_cfg->timer, TIM_IT_Update, DISABLE); - /* Disable TIM6 */ - TIM_Cmd(dev_cfg->timer, DISABLE); + /* Disable TIM6 update interrupt */ + TIM_ITConfig(dev_cfg->timer, TIM_IT_Update, DISABLE); + /* Disable TIM6 */ + TIM_Cmd(dev_cfg->timer, DISABLE); } void DAC_TIM_Handler(void); -void TIM6_DAC_IRQHandler(void) __attribute__ ((alias("DAC_TIM_Handler"))); +void TIM6_DAC_IRQHandler(void) __attribute__((alias("DAC_TIM_Handler"))); /** - * @brief This function handles TIM6 global interrupt request. - * @param None - * @retval None - */ + * @brief This function handles TIM6 global interrupt request. + * @param None + * @retval None + */ void DAC_TIM_Handler(void) { - if (TIM_GetITStatus(dev_cfg->timer, TIM_IT_Update) != RESET) - { - /* Clear TIM6 update interrupt */ - TIM_ClearITPendingBit(dev_cfg->timer, TIM_IT_Update); - } + if (TIM_GetITStatus(dev_cfg->timer, TIM_IT_Update) != RESET) { + /* Clear TIM6 update interrupt */ + TIM_ClearITPendingBit(dev_cfg->timer, TIM_IT_Update); + } } void DAC_DMA_Handler(void); -void DMA1_Stream5_IRQHandler(void) __attribute__ ((alias("DAC_DMA_Handler"))); +void DMA1_Stream5_IRQHandler(void) __attribute__((alias("DAC_DMA_Handler"))); /** * @brief Interrupt for half and full buffer transfer @@ -557,54 +527,46 @@ void DMA1_Stream5_IRQHandler(void) __attribute__ ((alias("DAC_DMA_Handler"))); */ void DAC_DMA_Handler(void) { - uint8_t status=0; - uint32_t bytesRead=0; - if (DMA_GetFlagStatus(dev_cfg->dma.tx.channel,DMA_FLAG_TCIF5)) { // whole double buffer filled - if (WaveDataLength) - { - if(DMA_GetCurrentMemoryTarget(dev_cfg->dma.tx.channel) == 0) - { - //DMA_MemoryTargetConfig(DMA1_Stream5,(uint32_t)&buffer2,DMA_Memory_1); - PIOS_FREAD(&file, buffer2, BUFFERSIZE, &bytesRead); - if (bytesRead != BUFFERSIZE) { - status=2; - } - } - else - { - //DMA_MemoryTargetConfig(DMA1_Stream5,(uint32_t)&buffer1,DMA_Memory_0); - PIOS_FREAD(&file, buffer1, BUFFERSIZE, &bytesRead); - if (bytesRead != BUFFERSIZE) { - status=1; - } - } - if(status) - { - // STOP DMA, master first - /*DMA_Cmd(DMA1_Stream5, DISABLE);*/ - //PIOS_FCLOSE(file); - //LoadWav(); + uint8_t status = 0; + uint32_t bytesRead = 0; - } - WaveDataLength -= 512; - } - if (WaveDataLength < 512) WaveDataLength = 0; - /* If we reach the WaveDataLength of the wave to play */ - if (WaveDataLength == 0) - { - /* Stop wave playing */ - WavePlayer_Stop(); - PIOS_FCLOSE(file); - WavePlayer_Start(); - } - DMA_ClearFlag(dev_cfg->dma.tx.channel,DMA_FLAG_TCIF5); - } - else if (DMA_GetFlagStatus(dev_cfg->dma.tx.channel,DMA_FLAG_HTIF5)) { - DMA_ClearFlag(dev_cfg->dma.tx.channel,DMA_FLAG_HTIF5); - } - else { - - } + if (DMA_GetFlagStatus(dev_cfg->dma.tx.channel, DMA_FLAG_TCIF5)) { // whole double buffer filled + if (WaveDataLength) { + if (DMA_GetCurrentMemoryTarget(dev_cfg->dma.tx.channel) == 0) { + // DMA_MemoryTargetConfig(DMA1_Stream5,(uint32_t)&buffer2,DMA_Memory_1); + PIOS_FREAD(&file, buffer2, BUFFERSIZE, &bytesRead); + if (bytesRead != BUFFERSIZE) { + status = 2; + } + } else { + // DMA_MemoryTargetConfig(DMA1_Stream5,(uint32_t)&buffer1,DMA_Memory_0); + PIOS_FREAD(&file, buffer1, BUFFERSIZE, &bytesRead); + if (bytesRead != BUFFERSIZE) { + status = 1; + } + } + if (status) { + // STOP DMA, master first + /*DMA_Cmd(DMA1_Stream5, DISABLE);*/ + // PIOS_FCLOSE(file); + // LoadWav(); + } + WaveDataLength -= 512; + } + if (WaveDataLength < 512) { + WaveDataLength = 0; + } + /* If we reach the WaveDataLength of the wave to play */ + if (WaveDataLength == 0) { + /* Stop wave playing */ + WavePlayer_Stop(); + PIOS_FCLOSE(file); + WavePlayer_Start(); + } + DMA_ClearFlag(dev_cfg->dma.tx.channel, DMA_FLAG_TCIF5); + } else if (DMA_GetFlagStatus(dev_cfg->dma.tx.channel, DMA_FLAG_HTIF5)) { + DMA_ClearFlag(dev_cfg->dma.tx.channel, DMA_FLAG_HTIF5); + } else {} } #endif /* PIOS_INCLUDE_WAVE */ diff --git a/flight/pios/inc/pios_adc.h b/flight/pios/inc/pios_adc.h index f72b163ee..2d32641b5 100644 --- a/flight/pios/inc/pios_adc.h +++ b/flight/pios/inc/pios_adc.h @@ -6,25 +6,25 @@ * @brief STM32 ADC PIOS interface * @{ * - * @file pios_adc.h + * @file pios_adc.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief ADC functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,14 +32,14 @@ #define PIOS_ADC_H // Maximum of 50 oversampled points -#define PIOS_ADC_MAX_SAMPLES ((((PIOS_ADC_NUM_CHANNELS + PIOS_ADC_USE_ADC2) >> PIOS_ADC_USE_ADC2) << PIOS_ADC_USE_ADC2)* PIOS_ADC_MAX_OVERSAMPLING * 2) +#define PIOS_ADC_MAX_SAMPLES ((((PIOS_ADC_NUM_CHANNELS + PIOS_ADC_USE_ADC2) >> PIOS_ADC_USE_ADC2) << PIOS_ADC_USE_ADC2) * PIOS_ADC_MAX_OVERSAMPLING * 2) -typedef void (*ADCCallback) (float * data); +typedef void (*ADCCallback)(float *data); /* Public Functions */ void PIOS_ADC_Config(uint32_t oversampling); int32_t PIOS_ADC_PinGet(uint32_t pin); -int16_t * PIOS_ADC_GetRawBuffer(void); +int16_t *PIOS_ADC_GetRawBuffer(void); uint8_t PIOS_ADC_GetOverSampling(void); void PIOS_ADC_SetCallback(ADCCallback new_function); #if defined(PIOS_INCLUDE_FREERTOS) @@ -50,6 +50,6 @@ extern void PIOS_ADC_DMA_Handler(void); #endif /* PIOS_ADC_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_adc_priv.h b/flight/pios/inc/pios_adc_priv.h index 34b73f9ca..25dadbda0 100644 --- a/flight/pios/inc/pios_adc_priv.h +++ b/flight/pios/inc/pios_adc_priv.h @@ -37,14 +37,14 @@ #include struct pios_adc_cfg { - ADC_TypeDef* adc_dev; - struct stm32_dma dma; - uint32_t half_flag; - uint32_t full_flag; - uint16_t max_downsample; + ADC_TypeDef *adc_dev; + struct stm32_dma dma; + uint32_t half_flag; + uint32_t full_flag; + uint16_t max_downsample; }; -int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg); +int32_t PIOS_ADC_Init(const struct pios_adc_cfg *cfg); #endif /* PIOS_ADC_PRIV_H */ @@ -52,4 +52,3 @@ int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg); * @} * @} */ - diff --git a/flight/pios/inc/pios_adxl345.h b/flight/pios/inc/pios_adxl345.h index 10022569f..9118784f0 100644 --- a/flight/pios/inc/pios_adxl345.h +++ b/flight/pios/inc/pios_adxl345.h @@ -7,7 +7,7 @@ * @{ * @file pios_adxl345.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief PiOS SPI ADXL345 + * @brief PiOS SPI ADXL345 * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ @@ -31,53 +31,53 @@ // Defined by data rate, not BW -#define ADXL_READ_BIT 0x80 -#define ADXL_MULTI_BIT 0x40 +#define ADXL_READ_BIT 0x80 +#define ADXL_MULTI_BIT 0x40 -#define ADXL_WHOAMI 0x00 -#define ADXL_DEVICE_ID 0xE5 -#define ADXL_X0_ADDR 0x32 +#define ADXL_WHOAMI 0x00 +#define ADXL_DEVICE_ID 0xE5 +#define ADXL_X0_ADDR 0x32 #define ADXL_FIFOSTATUS_ADDR 0x39 -#define ADXL_RATE_ADDR 0x2C -#define ADXL_RATE_100 0x0A -#define ADXL_RATE_200 0x0B -#define ADXL_RATE_400 0x0C -#define ADXL_RATE_800 0x0D -#define ADXL_RATE_1600 0x0E -#define ADXL_RATE_3200 0x0F +#define ADXL_RATE_ADDR 0x2C +#define ADXL_RATE_100 0x0A +#define ADXL_RATE_200 0x0B +#define ADXL_RATE_400 0x0C +#define ADXL_RATE_800 0x0D +#define ADXL_RATE_1600 0x0E +#define ADXL_RATE_3200 0x0F -#define ADXL_POWER_ADDR 0x2D -#define ADXL_MEAURE 0x08 +#define ADXL_POWER_ADDR 0x2D +#define ADXL_MEAURE 0x08 -#define ADXL_FORMAT_ADDR 0x31 -#define ADXL_FULL_RES 0x08 -#define ADXL_4WIRE 0x00 -#define ADXL_RANGE_2G 0x00 -#define ADXL_RANGE_4G 0x01 -#define ADXL_RANGE_8G 0x02 -#define ADXL_RANGE_16G 0x03 +#define ADXL_FORMAT_ADDR 0x31 +#define ADXL_FULL_RES 0x08 +#define ADXL_4WIRE 0x00 +#define ADXL_RANGE_2G 0x00 +#define ADXL_RANGE_4G 0x01 +#define ADXL_RANGE_8G 0x02 +#define ADXL_RANGE_16G 0x03 -#define ADXL_FIFO_ADDR 0x38 -#define ADXL_FIFO_STREAM 0x80 +#define ADXL_FIFO_ADDR 0x38 +#define ADXL_FIFO_STREAM 0x80 struct pios_adxl345_data { - int16_t x; - int16_t y; - int16_t z; + int16_t x; + int16_t y; + int16_t z; }; -int32_t PIOS_ADXL345_SelectRate(uint8_t rate); +int32_t PIOS_ADXL345_SelectRate(uint8_t rate); int32_t PIOS_ADXL345_SetRange(uint8_t range); int32_t PIOS_ADXL345_Init(uint32_t spi_id, uint32_t slave_num); -uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data * data); +uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data *data); int32_t PIOS_ADXL345_FifoElements(); int32_t PIOS_ADXL345_Test(); #endif /* PIOS_ADXL345_H */ -/** +/** * @} * @} */ diff --git a/flight/pios/inc/pios_bkp.h b/flight/pios/inc/pios_bkp.h index 35dca462a..cc4d6a6e3 100644 --- a/flight/pios/inc/pios_bkp.h +++ b/flight/pios/inc/pios_bkp.h @@ -6,25 +6,25 @@ * @brief Hardware abstraction layer for backup sram * @{ * - * @file pios_bkp.c + * @file pios_bkp.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief IAP functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,85 +34,84 @@ /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ /***************************************************************************************** * Public Definitions/Macros ****************************************************************************************/ // Backup registers definitions // registers reserved for PIOS usage -#define PIOS_BKP_RESERVED_1 0 // IAP_MAGIC_REG_1 -#define PIOS_BKP_RESERVED_2 1 // IAP_MAGIC_REG_2 -#define PIOS_BKP_RESERVED_3 2 // IAP_BOOTCOUNT -#define PIOS_BKP_RESERVED_4 3 // PIOS_WDG_REGISTER -#define PIOS_BKP_RESERVED_5 4 // IAP_CMD1 -#define PIOS_BKP_RESERVED_6 5 // IAP_CMD2 -#define PIOS_BKP_RESERVED_7 6 // IAP_CMD3 -#define PIOS_BKP_RESERVED_8 7 -#define PIOS_BKP_RESERVED_9 8 -#define PIOS_BKP_RESERVED_10 9 +#define PIOS_BKP_RESERVED_1 0 // IAP_MAGIC_REG_1 +#define PIOS_BKP_RESERVED_2 1 // IAP_MAGIC_REG_2 +#define PIOS_BKP_RESERVED_3 2 // IAP_BOOTCOUNT +#define PIOS_BKP_RESERVED_4 3 // PIOS_WDG_REGISTER +#define PIOS_BKP_RESERVED_5 4 // IAP_CMD1 +#define PIOS_BKP_RESERVED_6 5 // IAP_CMD2 +#define PIOS_BKP_RESERVED_7 6 // IAP_CMD3 +#define PIOS_BKP_RESERVED_8 7 +#define PIOS_BKP_RESERVED_9 8 +#define PIOS_BKP_RESERVED_10 9 // registers reserved for BOARD specific usage #define PIOS_BKP_BOARD_RESERVED_1 10 #define PIOS_BKP_BOARD_RESERVED_2 11 #define PIOS_BKP_BOARD_RESERVED_3 12 // registers reserved for APP usage -#define PIOS_BKP_APP_RESERVED_1 13 -#define PIOS_BKP_APP_RESERVED_2 14 -#define PIOS_BKP_APP_RESERVED_3 15 -#define PIOS_BKP_APP_RESERVED_4 16 +#define PIOS_BKP_APP_RESERVED_1 13 +#define PIOS_BKP_APP_RESERVED_2 14 +#define PIOS_BKP_APP_RESERVED_3 15 +#define PIOS_BKP_APP_RESERVED_4 16 /**************************************************************************************** - * Public Functions - ****************************************************************************************/ +* Public Functions +****************************************************************************************/ /** @defgroup PIOS_BKP_Public_Functions - * @{ - */ + * @{ + */ /** - * @brief Initialize the Backup Register hardware - * @param None - * @retval None - */ -void PIOS_BKP_Init(void); + * @brief Initialize the Backup Register hardware + * @param None + * @retval None + */ +void PIOS_BKP_Init(void); /** - * @brief Reads data from the specified Backup Register. - * @param regnumber: specifies the Backup Register. - * @retval The content of the specified Data Backup Register - */ -uint16_t PIOS_BKP_ReadRegister(uint32_t regnumber); + * @brief Reads data from the specified Backup Register. + * @param regnumber: specifies the Backup Register. + * @retval The content of the specified Data Backup Register + */ +uint16_t PIOS_BKP_ReadRegister(uint32_t regnumber); /** - * @brief Writes user data to the specified Backup Register. - * @param regnumber: specifies the Data Backup Register. - * @param data: data to write - * @retval None - */ -void PIOS_BKP_WriteRegister(uint32_t regnumber,uint16_t data); + * @brief Writes user data to the specified Backup Register. + * @param regnumber: specifies the Data Backup Register. + * @param data: data to write + * @retval None + */ +void PIOS_BKP_WriteRegister(uint32_t regnumber, uint16_t data); /** - * @brief Enable Backup registers write access - * @param None - * @retval None - */ -void PIOS_BKP_EnableWrite(void); + * @brief Enable Backup registers write access + * @param None + * @retval None + */ +void PIOS_BKP_EnableWrite(void); /** - * @brief Disable Backup registers write access - * @param None - * @retval None - */ -void PIOS_BKP_DisableWrite(void); + * @brief Disable Backup registers write access + * @param None + * @retval None + */ +void PIOS_BKP_DisableWrite(void); /** - * @} - */ - + * @} + */ /**************************************************************************************** - * Public Data - ****************************************************************************************/ +* Public Data +****************************************************************************************/ -#endif /* PIOS_BKP_H_ */ \ No newline at end of file +#endif /* PIOS_BKP_H_ */ diff --git a/flight/pios/inc/pios_bl_helper.h b/flight/pios/inc/pios_bl_helper.h index 3a0c6bdb0..855ec3160 100644 --- a/flight/pios/inc/pios_bl_helper.h +++ b/flight/pios/inc/pios_bl_helper.h @@ -34,7 +34,7 @@ extern uint8_t *PIOS_BL_HELPER_FLASH_If_Read(uint32_t SectorAddress); extern uint8_t PIOS_BL_HELPER_FLASH_Ini(); extern uint32_t PIOS_BL_HELPER_CRC_Memory_Calc(); -extern void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t * array, uint8_t size); +extern void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t *array, uint8_t size); extern uint8_t PIOS_BL_HELPER_FLASH_Start(); extern uint8_t PIOS_BL_HELPER_FLASH_Erase_Bootloader(); extern void PIOS_BL_HELPER_CRC_Ini(); diff --git a/flight/pios/inc/pios_bma180.h b/flight/pios/inc/pios_bma180.h index 84c310dd8..01164658a 100644 --- a/flight/pios/inc/pios_bma180.h +++ b/flight/pios/inc/pios_bma180.h @@ -37,71 +37,70 @@ #include /* BMA180 Addresses */ -#define BMA_CHIPID_ADDR 0x00 -#define BMA_VERSION_ADDR 0x00 -#define BMA_X_LSB_ADDR 0x02 -#define BMA_Y_LSB_ADDR 0x04 -#define BMA_Z_LSB_ADDR 0x06 -#define BMA_WE_ADDR 0x0D -#define BMA_RESET 0x10 -#define BMA_BW_ADDR 0x20 -#define BMA_RANGE_ADDR 0x35 -#define BMA_OFFSET_LSB1 0x35 -#define BMA_GAIN_Y 0x33 -#define BMA_CTRREG3 0x21 -#define BMA_CTRREG0 0x0D +#define BMA_CHIPID_ADDR 0x00 +#define BMA_VERSION_ADDR 0x00 +#define BMA_X_LSB_ADDR 0x02 +#define BMA_Y_LSB_ADDR 0x04 +#define BMA_Z_LSB_ADDR 0x06 +#define BMA_WE_ADDR 0x0D +#define BMA_RESET 0x10 +#define BMA_BW_ADDR 0x20 +#define BMA_RANGE_ADDR 0x35 +#define BMA_OFFSET_LSB1 0x35 +#define BMA_GAIN_Y 0x33 +#define BMA_CTRREG3 0x21 +#define BMA_CTRREG0 0x0D -#define BMA_RESET_CODE 0x6B +#define BMA_RESET_CODE 0x6B /* Accel range */ -#define BMA_RANGE_MASK 0x0E -#define BMA_RANGE_SHIFT 1 -enum bma180_range { BMA_RANGE_1G = 0x00, - BMA_RANGE_1_5G = 0x01, - BMA_RANGE_2G = 0x02, - BMA_RANGE_3G = 0x03, - BMA_RANGE_4G = 0x04, - BMA_RANGE_8G = 0x05, - BMA_RANGE_16G = 0x06 -}; +#define BMA_RANGE_MASK 0x0E +#define BMA_RANGE_SHIFT 1 +enum bma180_range { BMA_RANGE_1G = 0x00, + BMA_RANGE_1_5G = 0x01, + BMA_RANGE_2G = 0x02, + BMA_RANGE_3G = 0x03, + BMA_RANGE_4G = 0x04, + BMA_RANGE_8G = 0x05, + BMA_RANGE_16G = 0x06 }; /* Measurement bandwidth */ -#define BMA_BW_MASK 0xF0 -#define BMA_BW_SHIFT 4 -enum bma180_bandwidth { BMA_BW_10HZ = 0x00, - BMA_BW_20HZ = 0x01, - BMA_BW_40HZ = 0x02, - BMA_BW_75HZ = 0x03, - BMA_BW_150HZ = 0x04, - BMA_BW_300HZ = 0x05, - BMA_BW_600HZ = 0x06, - BMA_BW_1200HZ =0x07, - BMA_BW_HP1HZ = 0x08, // High-pass, 1 Hz - BMA_BW_BP0_300HZ = 0x09 // Band-pass, 0.3Hz-300Hz +#define BMA_BW_MASK 0xF0 +#define BMA_BW_SHIFT 4 +enum bma180_bandwidth { BMA_BW_10HZ = 0x00, + BMA_BW_20HZ = 0x01, + BMA_BW_40HZ = 0x02, + BMA_BW_75HZ = 0x03, + BMA_BW_150HZ = 0x04, + BMA_BW_300HZ = 0x05, + BMA_BW_600HZ = 0x06, + BMA_BW_1200HZ = 0x07, + BMA_BW_HP1HZ = 0x08, // High-pass, 1 Hz + BMA_BW_BP0_300HZ = 0x09 // Band-pass, 0.3Hz-300Hz }; -#define BMA_NEW_DAT_INT 0x02 +#define BMA_NEW_DAT_INT 0x02 struct pios_bma180_data { - int16_t x; - int16_t y; - int16_t z; - int8_t temperature; + int16_t x; + int16_t y; + int16_t z; + int8_t temperature; }; struct pios_bma180_cfg { - const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ - enum bma180_bandwidth bandwidth; - enum bma180_range range; + const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ + enum bma180_bandwidth bandwidth; + enum bma180_range range; }; /* Public Functions */ -extern int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_bma180_cfg * cfg); +extern int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_bma180_cfg *cfg); extern void PIOS_BMA180_Attach(uint32_t spi_id); extern float PIOS_BMA180_GetScale(); -extern int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data * buffer); -extern int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data * data); +extern int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data *buffer); +extern int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data *data); extern int32_t PIOS_BMA180_Test(); extern bool PIOS_BMA180_IRQHandler(); diff --git a/flight/pios/inc/pios_bmp085.h b/flight/pios/inc/pios_bmp085.h index 8cd796453..c1e8477b4 100644 --- a/flight/pios/inc/pios_bmp085.h +++ b/flight/pios/inc/pios_bmp085.h @@ -6,25 +6,25 @@ * @brief Hardware functions to deal with the altitude pressure sensor * @{ * - * @file pios_bmp085.h + * @file pios_bmp085.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief BMP085 functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,34 +32,34 @@ #define PIOS_BMP085_H /* BMP085 Addresses */ -#define BMP085_I2C_ADDR 0x77 -#define BMP085_CALIB_ADDR 0xAA -#define BMP085_CALIB_LEN 22 -#define BMP085_CTRL_ADDR 0xF4 -#define BMP085_OVERSAMPLING PIOS_BMP085_OVERSAMPLING -#define BMP085_PRES_ADDR (0x34 + (BMP085_OVERSAMPLING << 6)) -#define BMP085_TEMP_ADDR 0x2E -#define BMP085_ADC_MSB 0xF6 -#define BMP085_P0 101325 +#define BMP085_I2C_ADDR 0x77 +#define BMP085_CALIB_ADDR 0xAA +#define BMP085_CALIB_LEN 22 +#define BMP085_CTRL_ADDR 0xF4 +#define BMP085_OVERSAMPLING PIOS_BMP085_OVERSAMPLING +#define BMP085_PRES_ADDR (0x34 + (BMP085_OVERSAMPLING << 6)) +#define BMP085_TEMP_ADDR 0x2E +#define BMP085_ADC_MSB 0xF6 +#define BMP085_P0 101325 /* Local Types */ typedef struct { - int16_t AC1; - int16_t AC2; - int16_t AC3; - uint16_t AC4; - uint16_t AC5; - uint16_t AC6; - int16_t B1; - int16_t B2; - int16_t MB; - int16_t MC; - int16_t MD; + int16_t AC1; + int16_t AC2; + int16_t AC3; + uint16_t AC4; + uint16_t AC5; + uint16_t AC6; + int16_t B1; + int16_t B2; + int16_t MB; + int16_t MC; + int16_t MD; } BMP085CalibDataTypeDef; typedef enum { - PressureConv, - TemperatureConv + PressureConv, + TemperatureConv } ConversionTypeTypeDef; /* Global Variables */ @@ -75,13 +75,13 @@ extern void PIOS_BMP085_StartADC(ConversionTypeTypeDef Type); extern void PIOS_BMP085_ReadADC(void); extern int16_t PIOS_BMP085_GetTemperature(void); extern int32_t PIOS_BMP085_GetPressure(void); -extern bool PIOS_BMP085_Read(uint8_t address, uint8_t * buffer, uint8_t len); +extern bool PIOS_BMP085_Read(uint8_t address, uint8_t *buffer, uint8_t len); extern bool PIOS_BMP085_Write(uint8_t address, uint8_t buffer); extern int32_t PIOS_BMP085_Test(); #endif /* PIOS_BMP085_H */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_board_info.h b/flight/pios/inc/pios_board_info.h index 3ff7aa886..2e549f090 100644 --- a/flight/pios/inc/pios_board_info.h +++ b/flight/pios/inc/pios_board_info.h @@ -1,22 +1,22 @@ #ifndef PIOS_BOARD_INFO_H #define PIOS_BOARD_INFO_H -#include /* uint* */ +#include /* uint* */ #define PIOS_BOARD_INFO_BLOB_MAGIC 0xBDBDBDBD struct pios_board_info { - uint32_t magic; - uint8_t board_type; - uint8_t board_rev; - uint8_t bl_rev; - uint8_t hw_type; - uint32_t fw_base; - uint32_t fw_size; - uint32_t desc_base; - uint32_t desc_size; - uint32_t ee_base; - uint32_t ee_size; + uint32_t magic; + uint8_t board_type; + uint8_t board_rev; + uint8_t bl_rev; + uint8_t hw_type; + uint32_t fw_base; + uint32_t fw_size; + uint32_t desc_base; + uint32_t desc_size; + uint32_t ee_base; + uint32_t ee_size; } __attribute__((packed)); extern const struct pios_board_info pios_board_info_blob; diff --git a/flight/pios/inc/pios_com.h b/flight/pios/inc/pios_com.h index 176a870b6..e2a9639c5 100644 --- a/flight/pios/inc/pios_com.h +++ b/flight/pios/inc/pios_com.h @@ -6,44 +6,44 @@ * @brief Hardware communication layer * @{ * - * @file pios_com.h + * @file pios_com.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief COM layer functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_COM_H #define PIOS_COM_H -#include /* uint*_t */ -#include /* bool */ +#include /* uint*_t */ +#include /* bool */ -typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * task_woken); +typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *task_woken); struct pios_com_driver { - void (*init)(uint32_t id); - void (*set_baud)(uint32_t id, uint32_t baud); - void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail); - void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); - void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); - void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); - bool (*available)(uint32_t id); + void (*init)(uint32_t id); + void (*set_baud)(uint32_t id, uint32_t baud); + void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail); + void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); + void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); + void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); + bool (*available)(uint32_t id); }; /* Public Functions */ @@ -56,12 +56,12 @@ extern int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str); extern int32_t PIOS_COM_SendString(uint32_t com_id, const char *str); extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...); extern int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...); -extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms); +extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t *buf, uint16_t buf_len, uint32_t timeout_ms); extern bool PIOS_COM_Available(uint32_t com_id); #endif /* PIOS_COM_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_com_msg.h b/flight/pios/inc/pios_com_msg.h index 1caaeabeb..f37bcb2c0 100644 --- a/flight/pios/inc/pios_com_msg.h +++ b/flight/pios/inc/pios_com_msg.h @@ -6,40 +6,40 @@ * @brief Hardware communication layer * @{ * - * @file pios_com_msg.h + * @file pios_com_msg.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief COM MSG layer functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_COM_MSG_H #define PIOS_COM_MSG_H -#include /* uint*_t */ +#include /* uint*_t */ /* Public Functions */ extern int32_t PIOS_COM_MSG_Send(uint32_t com_id, const uint8_t *msg, uint16_t msg_len); -extern uint16_t PIOS_COM_MSG_Receive(uint32_t com_id, uint8_t * buf, uint16_t buf_len); +extern uint16_t PIOS_COM_MSG_Receive(uint32_t com_id, uint8_t *buf, uint16_t buf_len); #endif /* PIOS_COM_MSG_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_com_msg_priv.h b/flight/pios/inc/pios_com_msg_priv.h index 20992de47..3e775ff9e 100644 --- a/flight/pios/inc/pios_com_msg_priv.h +++ b/flight/pios/inc/pios_com_msg_priv.h @@ -31,14 +31,14 @@ #ifndef PIOS_COM_MSG_PRIV_H #define PIOS_COM_MSG_PRIV_H -#include /* uint*_t */ -#include "pios_com_priv.h" /* struct pios_com_driver */ +#include /* uint*_t */ +#include "pios_com_priv.h" /* struct pios_com_driver */ -extern int32_t PIOS_COM_MSG_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id); +extern int32_t PIOS_COM_MSG_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id); #endif /* PIOS_COM_MSG_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_com_priv.h b/flight/pios/inc/pios_com_priv.h index c39522f79..565dae3ef 100644 --- a/flight/pios/inc/pios_com_priv.h +++ b/flight/pios/inc/pios_com_priv.h @@ -31,11 +31,11 @@ #ifndef PIOS_COM_PRIV_H #define PIOS_COM_PRIV_H -extern int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len); +extern int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len); #endif /* PIOS_COM_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_constants.h b/flight/pios/inc/pios_constants.h index a1889b099..c4bbde1f8 100644 --- a/flight/pios/inc/pios_constants.h +++ b/flight/pios/inc/pios_constants.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_constants.h + * @file pios_constants.h * @author The OpenPilot Team, http://www.openpilot.org Copyright_F (Cf) 2013. * @brief Shared phisical constants * -- * @see The GNU Public License_F (GPLf) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or *_F (at your optionf) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -29,106 +29,106 @@ // Constants borrowed straight from GSL http://www.gnu.org/software/gsl/ -#define PIOS_CONST_MKS_SPEED_OF_LIGHT_F (2.99792458e8f) /* m / s */ -#define PIOS_CONST_MKS_GRAVITATIONAL_CONSTANT_F (6.673e-11f) /* m^3 / kg s^2 */ -#define PIOS_CONST_MKS_PLANCKS_CONSTANT_H_F (6.62606896e-34f) /* kg m^2 / s */ -#define PIOS_CONST_MKS_PLANCKS_CONSTANT_HBAR_F (1.05457162825e-34f) /* kg m^2 / s */ -#define PIOS_CONST_MKS_ASTRONOMICAL_UNIT_F (1.49597870691e11f) /* m */ -#define PIOS_CONST_MKS_LIGHT_YEAR_F (9.46053620707e15f) /* m */ -#define PIOS_CONST_MKS_PARSEC_F (3.08567758135e16f) /* m */ -#define PIOS_CONST_MKS_GRAV_ACCEL_F (9.80665e0f) /* m / s^2 */ -#define PIOS_CONST_MKS_ELECTRON_VOLT_F (1.602176487e-19f) /* kg m^2 / s^2 */ -#define PIOS_CONST_MKS_MASS_ELECTRON_F (9.10938188e-31f) /* kg */ -#define PIOS_CONST_MKS_MASS_MUON_F (1.88353109e-28f) /* kg */ -#define PIOS_CONST_MKS_MASS_PROTON_F (1.67262158e-27f) /* kg */ -#define PIOS_CONST_MKS_MASS_NEUTRON_F (1.67492716e-27f) /* kg */ -#define PIOS_CONST_MKS_RYDBERG_F (2.17987196968e-18f) /* kg m^2 / s^2 */ -#define PIOS_CONST_MKS_BOLTZMANN_F (1.3806504e-23f) /* kg m^2 / K s^2 */ -#define PIOS_CONST_MKS_MOLAR_GAS_F (8.314472e0f) /* kg m^2 / K mol s^2 */ -#define PIOS_CONST_MKS_STANDARD_GAS_VOLUME_F (2.2710981e-2f) /* m^3 / mol */ -#define PIOS_CONST_MKS_MINUTE_F (6e1f) /* s */ -#define PIOS_CONST_MKS_HOUR_F (3.6e3f) /* s */ -#define PIOS_CONST_MKS_DAY_F (8.64e4f) /* s */ -#define PIOS_CONST_MKS_WEEK_F (6.048e5f) /* s */ -#define PIOS_CONST_MKS_INCH_F (2.54e-2f) /* m */ -#define PIOS_CONST_MKS_FOOT_F (3.048e-1f) /* m */ -#define PIOS_CONST_MKS_YARD_F (9.144e-1f) /* m */ -#define PIOS_CONST_MKS_MILE_F (1.609344e3f) /* m */ -#define PIOS_CONST_MKS_NAUTICAL_MILE_F (1.852e3f) /* m */ -#define PIOS_CONST_MKS_FATHOM_F (1.8288e0f) /* m */ -#define PIOS_CONST_MKS_MIL_F (2.54e-5f) /* m */ -#define PIOS_CONST_MKS_POINT_F (3.52777777778e-4f) /* m */ -#define PIOS_CONST_MKS_TEXPOINT_F (3.51459803515e-4f) /* m */ -#define PIOS_CONST_MKS_MICRON_F (1e-6f) /* m */ -#define PIOS_CONST_MKS_ANGSTROM_F (1e-10f) /* m */ -#define PIOS_CONST_MKS_HECTARE_F (1e4f) /* m^2 */ -#define PIOS_CONST_MKS_ACRE_F (4.04685642241e3f) /* m^2 */ -#define PIOS_CONST_MKS_BARN_F (1e-28f) /* m^2 */ -#define PIOS_CONST_MKS_LITER_F (1e-3f) /* m^3 */ -#define PIOS_CONST_MKS_US_GALLON_F (3.78541178402e-3f) /* m^3 */ -#define PIOS_CONST_MKS_QUART_F (9.46352946004e-4f) /* m^3 */ -#define PIOS_CONST_MKS_PINT_F (4.73176473002e-4f) /* m^3 */ -#define PIOS_CONST_MKS_CUP_F (2.36588236501e-4f) /* m^3 */ -#define PIOS_CONST_MKS_FLUID_OUNCE_F (2.95735295626e-5f) /* m^3 */ -#define PIOS_CONST_MKS_TABLESPOON_F (1.47867647813e-5f) /* m^3 */ -#define PIOS_CONST_MKS_TEASPOON_F (4.92892159375e-6f) /* m^3 */ -#define PIOS_CONST_MKS_CANADIAN_GALLON_F (4.54609e-3f) /* m^3 */ -#define PIOS_CONST_MKS_UK_GALLON_F (4.546092e-3f) /* m^3 */ -#define PIOS_CONST_MKS_MILES_PER_HOUR_F (4.4704e-1f) /* m / s */ -#define PIOS_CONST_MKS_KILOMETERS_PER_HOUR_F (2.77777777778e-1f) /* m / s */ -#define PIOS_CONST_MKS_KNOT_F (5.14444444444e-1f) /* m / s */ -#define PIOS_CONST_MKS_POUND_MASS_F (4.5359237e-1f) /* kg */ -#define PIOS_CONST_MKS_OUNCE_MASS_F (2.8349523125e-2f) /* kg */ -#define PIOS_CONST_MKS_TON_F (9.0718474e2f) /* kg */ -#define PIOS_CONST_MKS_METRIC_TON_F (1e3f) /* kg */ -#define PIOS_CONST_MKS_UK_TON_F (1.0160469088e3f) /* kg */ -#define PIOS_CONST_MKS_TROY_OUNCE_F (3.1103475e-2f) /* kg */ -#define PIOS_CONST_MKS_CARAT_F (2e-4f) /* kg */ -#define PIOS_CONST_MKS_UNIFIED_ATOMIC_MASS_F (1.660538782e-27f) /* kg */ -#define PIOS_CONST_MKS_GRAM_FORCE_F (9.80665e-3f) /* kg m / s^2 */ -#define PIOS_CONST_MKS_POUND_FORCE_F (4.44822161526e0f) /* kg m / s^2 */ -#define PIOS_CONST_MKS_KILOPOUND_FORCE_F (4.44822161526e3f) /* kg m / s^2 */ -#define PIOS_CONST_MKS_POUNDAL_F (1.38255e-1f) /* kg m / s^2 */ -#define PIOS_CONST_MKS_CALORIE_F (4.1868e0f) /* kg m^2 / s^2 */ -#define PIOS_CONST_MKS_BTU_F (1.05505585262e3f) /* kg m^2 / s^2 */ -#define PIOS_CONST_MKS_THERM_F (1.05506e8f) /* kg m^2 / s^2 */ -#define PIOS_CONST_MKS_HORSEPOWER_F (7.457e2f) /* kg m^2 / s^3 */ -#define PIOS_CONST_MKS_BAR_F (1e5f) /* kg / m s^2 */ -#define PIOS_CONST_MKS_STD_ATMOSPHERE_F (1.01325e5f) /* kg / m s^2 */ -#define PIOS_CONST_MKS_TORR_F (1.33322368421e2f) /* kg / m s^2 */ -#define PIOS_CONST_MKS_METER_OF_MERCURY_F (1.33322368421e5f) /* kg / m s^2 */ -#define PIOS_CONST_MKS_INCH_OF_MERCURY_F (3.38638815789e3f) /* kg / m s^2 */ -#define PIOS_CONST_MKS_INCH_OF_WATER_F (2.490889e2f) /* kg / m s^2 */ -#define PIOS_CONST_MKS_PSI_F (6.89475729317e3f) /* kg / m s^2 */ -#define PIOS_CONST_MKS_POISE_F (1e-1f) /* kg m^-1 s^-1 */ -#define PIOS_CONST_MKS_STOKES_F (1e-4f) /* m^2 / s */ -#define PIOS_CONST_MKS_STILB_F (1e4f) /* cd / m^2 */ -#define PIOS_CONST_MKS_LUMEN_F (1e0f) /* cd sr */ -#define PIOS_CONST_MKS_LUX_F (1e0f) /* cd sr / m^2 */ -#define PIOS_CONST_MKS_PHOT_F (1e4f) /* cd sr / m^2 */ -#define PIOS_CONST_MKS_FOOTCANDLE_F (1.076e1f) /* cd sr / m^2 */ -#define PIOS_CONST_MKS_LAMBERT_F (1e4f) /* cd sr / m^2 */ -#define PIOS_CONST_MKS_FOOTLAMBERT_F (1.07639104e1f) /* cd sr / m^2 */ -#define PIOS_CONST_MKS_CURIE_F (3.7e10f) /* 1 / s */ -#define PIOS_CONST_MKS_ROENTGEN_F (2.58e-4f) /* A s / kg */ -#define PIOS_CONST_MKS_RAD_F (1e-2f) /* m^2 / s^2 */ -#define PIOS_CONST_MKS_SOLAR_MASS_F (1.98892e30f) /* kg */ -#define PIOS_CONST_MKS_BOHR_RADIUS_F (5.291772083e-11f) /* m */ -#define PIOS_CONST_MKS_NEWTON_F (1e0f) /* kg m / s^2 */ -#define PIOS_CONST_MKS_DYNE_F (1e-5f) /* kg m / s^2 */ -#define PIOS_CONST_MKS_JOULE_F (1e0f) /* kg m^2 / s^2 */ -#define PIOS_CONST_MKS_ERG_F (1e-7f) /* kg m^2 / s^2 */ +#define PIOS_CONST_MKS_SPEED_OF_LIGHT_F (2.99792458e8f) /* m / s */ +#define PIOS_CONST_MKS_GRAVITATIONAL_CONSTANT_F (6.673e-11f) /* m^3 / kg s^2 */ +#define PIOS_CONST_MKS_PLANCKS_CONSTANT_H_F (6.62606896e-34f) /* kg m^2 / s */ +#define PIOS_CONST_MKS_PLANCKS_CONSTANT_HBAR_F (1.05457162825e-34f) /* kg m^2 / s */ +#define PIOS_CONST_MKS_ASTRONOMICAL_UNIT_F (1.49597870691e11f) /* m */ +#define PIOS_CONST_MKS_LIGHT_YEAR_F (9.46053620707e15f) /* m */ +#define PIOS_CONST_MKS_PARSEC_F (3.08567758135e16f) /* m */ +#define PIOS_CONST_MKS_GRAV_ACCEL_F (9.80665e0f) /* m / s^2 */ +#define PIOS_CONST_MKS_ELECTRON_VOLT_F (1.602176487e-19f) /* kg m^2 / s^2 */ +#define PIOS_CONST_MKS_MASS_ELECTRON_F (9.10938188e-31f) /* kg */ +#define PIOS_CONST_MKS_MASS_MUON_F (1.88353109e-28f) /* kg */ +#define PIOS_CONST_MKS_MASS_PROTON_F (1.67262158e-27f) /* kg */ +#define PIOS_CONST_MKS_MASS_NEUTRON_F (1.67492716e-27f) /* kg */ +#define PIOS_CONST_MKS_RYDBERG_F (2.17987196968e-18f) /* kg m^2 / s^2 */ +#define PIOS_CONST_MKS_BOLTZMANN_F (1.3806504e-23f) /* kg m^2 / K s^2 */ +#define PIOS_CONST_MKS_MOLAR_GAS_F (8.314472e0f) /* kg m^2 / K mol s^2 */ +#define PIOS_CONST_MKS_STANDARD_GAS_VOLUME_F (2.2710981e-2f) /* m^3 / mol */ +#define PIOS_CONST_MKS_MINUTE_F (6e1f) /* s */ +#define PIOS_CONST_MKS_HOUR_F (3.6e3f) /* s */ +#define PIOS_CONST_MKS_DAY_F (8.64e4f) /* s */ +#define PIOS_CONST_MKS_WEEK_F (6.048e5f) /* s */ +#define PIOS_CONST_MKS_INCH_F (2.54e-2f) /* m */ +#define PIOS_CONST_MKS_FOOT_F (3.048e-1f) /* m */ +#define PIOS_CONST_MKS_YARD_F (9.144e-1f) /* m */ +#define PIOS_CONST_MKS_MILE_F (1.609344e3f) /* m */ +#define PIOS_CONST_MKS_NAUTICAL_MILE_F (1.852e3f) /* m */ +#define PIOS_CONST_MKS_FATHOM_F (1.8288e0f) /* m */ +#define PIOS_CONST_MKS_MIL_F (2.54e-5f) /* m */ +#define PIOS_CONST_MKS_POINT_F (3.52777777778e-4f) /* m */ +#define PIOS_CONST_MKS_TEXPOINT_F (3.51459803515e-4f) /* m */ +#define PIOS_CONST_MKS_MICRON_F (1e-6f) /* m */ +#define PIOS_CONST_MKS_ANGSTROM_F (1e-10f) /* m */ +#define PIOS_CONST_MKS_HECTARE_F (1e4f) /* m^2 */ +#define PIOS_CONST_MKS_ACRE_F (4.04685642241e3f) /* m^2 */ +#define PIOS_CONST_MKS_BARN_F (1e-28f) /* m^2 */ +#define PIOS_CONST_MKS_LITER_F (1e-3f) /* m^3 */ +#define PIOS_CONST_MKS_US_GALLON_F (3.78541178402e-3f) /* m^3 */ +#define PIOS_CONST_MKS_QUART_F (9.46352946004e-4f) /* m^3 */ +#define PIOS_CONST_MKS_PINT_F (4.73176473002e-4f) /* m^3 */ +#define PIOS_CONST_MKS_CUP_F (2.36588236501e-4f) /* m^3 */ +#define PIOS_CONST_MKS_FLUID_OUNCE_F (2.95735295626e-5f) /* m^3 */ +#define PIOS_CONST_MKS_TABLESPOON_F (1.47867647813e-5f) /* m^3 */ +#define PIOS_CONST_MKS_TEASPOON_F (4.92892159375e-6f) /* m^3 */ +#define PIOS_CONST_MKS_CANADIAN_GALLON_F (4.54609e-3f) /* m^3 */ +#define PIOS_CONST_MKS_UK_GALLON_F (4.546092e-3f) /* m^3 */ +#define PIOS_CONST_MKS_MILES_PER_HOUR_F (4.4704e-1f) /* m / s */ +#define PIOS_CONST_MKS_KILOMETERS_PER_HOUR_F (2.77777777778e-1f) /* m / s */ +#define PIOS_CONST_MKS_KNOT_F (5.14444444444e-1f) /* m / s */ +#define PIOS_CONST_MKS_POUND_MASS_F (4.5359237e-1f) /* kg */ +#define PIOS_CONST_MKS_OUNCE_MASS_F (2.8349523125e-2f) /* kg */ +#define PIOS_CONST_MKS_TON_F (9.0718474e2f) /* kg */ +#define PIOS_CONST_MKS_METRIC_TON_F (1e3f) /* kg */ +#define PIOS_CONST_MKS_UK_TON_F (1.0160469088e3f) /* kg */ +#define PIOS_CONST_MKS_TROY_OUNCE_F (3.1103475e-2f) /* kg */ +#define PIOS_CONST_MKS_CARAT_F (2e-4f) /* kg */ +#define PIOS_CONST_MKS_UNIFIED_ATOMIC_MASS_F (1.660538782e-27f) /* kg */ +#define PIOS_CONST_MKS_GRAM_FORCE_F (9.80665e-3f) /* kg m / s^2 */ +#define PIOS_CONST_MKS_POUND_FORCE_F (4.44822161526e0f) /* kg m / s^2 */ +#define PIOS_CONST_MKS_KILOPOUND_FORCE_F (4.44822161526e3f) /* kg m / s^2 */ +#define PIOS_CONST_MKS_POUNDAL_F (1.38255e-1f) /* kg m / s^2 */ +#define PIOS_CONST_MKS_CALORIE_F (4.1868e0f) /* kg m^2 / s^2 */ +#define PIOS_CONST_MKS_BTU_F (1.05505585262e3f) /* kg m^2 / s^2 */ +#define PIOS_CONST_MKS_THERM_F (1.05506e8f) /* kg m^2 / s^2 */ +#define PIOS_CONST_MKS_HORSEPOWER_F (7.457e2f) /* kg m^2 / s^3 */ +#define PIOS_CONST_MKS_BAR_F (1e5f) /* kg / m s^2 */ +#define PIOS_CONST_MKS_STD_ATMOSPHERE_F (1.01325e5f) /* kg / m s^2 */ +#define PIOS_CONST_MKS_TORR_F (1.33322368421e2f) /* kg / m s^2 */ +#define PIOS_CONST_MKS_METER_OF_MERCURY_F (1.33322368421e5f) /* kg / m s^2 */ +#define PIOS_CONST_MKS_INCH_OF_MERCURY_F (3.38638815789e3f) /* kg / m s^2 */ +#define PIOS_CONST_MKS_INCH_OF_WATER_F (2.490889e2f) /* kg / m s^2 */ +#define PIOS_CONST_MKS_PSI_F (6.89475729317e3f) /* kg / m s^2 */ +#define PIOS_CONST_MKS_POISE_F (1e-1f) /* kg m^-1 s^-1 */ +#define PIOS_CONST_MKS_STOKES_F (1e-4f) /* m^2 / s */ +#define PIOS_CONST_MKS_STILB_F (1e4f) /* cd / m^2 */ +#define PIOS_CONST_MKS_LUMEN_F (1e0f) /* cd sr */ +#define PIOS_CONST_MKS_LUX_F (1e0f) /* cd sr / m^2 */ +#define PIOS_CONST_MKS_PHOT_F (1e4f) /* cd sr / m^2 */ +#define PIOS_CONST_MKS_FOOTCANDLE_F (1.076e1f) /* cd sr / m^2 */ +#define PIOS_CONST_MKS_LAMBERT_F (1e4f) /* cd sr / m^2 */ +#define PIOS_CONST_MKS_FOOTLAMBERT_F (1.07639104e1f) /* cd sr / m^2 */ +#define PIOS_CONST_MKS_CURIE_F (3.7e10f) /* 1 / s */ +#define PIOS_CONST_MKS_ROENTGEN_F (2.58e-4f) /* A s / kg */ +#define PIOS_CONST_MKS_RAD_F (1e-2f) /* m^2 / s^2 */ +#define PIOS_CONST_MKS_SOLAR_MASS_F (1.98892e30f) /* kg */ +#define PIOS_CONST_MKS_BOHR_RADIUS_F (5.291772083e-11f) /* m */ +#define PIOS_CONST_MKS_NEWTON_F (1e0f) /* kg m / s^2 */ +#define PIOS_CONST_MKS_DYNE_F (1e-5f) /* kg m / s^2 */ +#define PIOS_CONST_MKS_JOULE_F (1e0f) /* kg m^2 / s^2 */ +#define PIOS_CONST_MKS_ERG_F (1e-7f) /* kg m^2 / s^2 */ #define PIOS_CONST_MKS_STEFAN_BOLTZMANN_CONSTANT_F (5.67040047374e-8f) /* kg / K^4 s^3 */ -#define PIOS_CONST_MKS_THOMSON_CROSS_SECTION_F (6.65245893699e-29f) /* m^2 */ -#define PIOS_CONST_MKS_BOHR_MAGNETON_F (9.27400899e-24f) /* A m^2 */ -#define PIOS_CONST_MKS_NUCLEAR_MAGNETON_F (5.05078317e-27f) /* A m^2 */ -#define PIOS_CONST_MKS_ELECTRON_MAGNETIC_MOMENT_F (9.28476362e-24f) /* A m^2 */ -#define PIOS_CONST_MKS_PROTON_MAGNETIC_MOMENT_F (1.410606633e-26f) /* A m^2 */ -#define PIOS_CONST_MKS_FARADAY_F (9.64853429775e4f) /* A s / mol */ -#define PIOS_CONST_MKS_ELECTRON_CHARGE_F (1.602176487e-19f) /* A s */ -#define PIOS_CONST_MKS_VACUUM_PERMITTIVITY_F (8.854187817e-12f) /* A^2 s^4 / kg m^3 */ -#define PIOS_CONST_MKS_VACUUM_PERMEABILITY_F (1.25663706144e-6f) /* kg m / A^2 s^2 */ -#define PIOS_CONST_MKS_DEBYE_F (3.33564095198e-30f) /* A s^2 / m^2 */ -#define PIOS_CONST_MKS_GAUSS_F (1e-4f) /* kg / A s^2 */ +#define PIOS_CONST_MKS_THOMSON_CROSS_SECTION_F (6.65245893699e-29f) /* m^2 */ +#define PIOS_CONST_MKS_BOHR_MAGNETON_F (9.27400899e-24f) /* A m^2 */ +#define PIOS_CONST_MKS_NUCLEAR_MAGNETON_F (5.05078317e-27f) /* A m^2 */ +#define PIOS_CONST_MKS_ELECTRON_MAGNETIC_MOMENT_F (9.28476362e-24f) /* A m^2 */ +#define PIOS_CONST_MKS_PROTON_MAGNETIC_MOMENT_F (1.410606633e-26f) /* A m^2 */ +#define PIOS_CONST_MKS_FARADAY_F (9.64853429775e4f) /* A s / mol */ +#define PIOS_CONST_MKS_ELECTRON_CHARGE_F (1.602176487e-19f) /* A s */ +#define PIOS_CONST_MKS_VACUUM_PERMITTIVITY_F (8.854187817e-12f) /* A^2 s^4 / kg m^3 */ +#define PIOS_CONST_MKS_VACUUM_PERMEABILITY_F (1.25663706144e-6f) /* kg m / A^2 s^2 */ +#define PIOS_CONST_MKS_DEBYE_F (3.33564095198e-30f) /* A s^2 / m^2 */ +#define PIOS_CONST_MKS_GAUSS_F (1e-4f) /* kg / A s^2 */ #endif /* PIOS_CONSTANTS_H */ diff --git a/flight/pios/inc/pios_crc.h b/flight/pios/inc/pios_crc.h index 7e35dee02..c155f1fc9 100644 --- a/flight/pios/inc/pios_crc.h +++ b/flight/pios/inc/pios_crc.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_CRC CRC Functions * @{ * - * @file pios_crc.h + * @file pios_crc.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief CRC functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,12 +31,12 @@ #define PIOS_CRC_H uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data); -uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length); +uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t *data, int32_t length); uint16_t PIOS_CRC16_updateByte(uint16_t crc, const uint8_t data); -uint16_t PIOS_CRC16_updateCRC(uint16_t crc, const uint8_t* data, int32_t length); +uint16_t PIOS_CRC16_updateCRC(uint16_t crc, const uint8_t *data, int32_t length); uint32_t PIOS_CRC32_updateByte(uint32_t crc, const uint8_t data); -uint32_t PIOS_CRC32_updateCRC(uint32_t crc, const uint8_t* data, int32_t length); +uint32_t PIOS_CRC32_updateCRC(uint32_t crc, const uint8_t *data, int32_t length); #endif /* PIOS_CRC_H */ diff --git a/flight/pios/inc/pios_debug.h b/flight/pios/inc/pios_debug.h index 6cd151e30..8fd7016a9 100644 --- a/flight/pios/inc/pios_debug.h +++ b/flight/pios/inc/pios_debug.h @@ -6,25 +6,25 @@ * @brief Debugging functionality * @{ * - * @file pios_i2c.h + * @file pios_i2c.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Debug helper functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,7 +33,7 @@ #ifdef PIOS_INCLUDE_DEBUG_CONSOLE #ifndef DEBUG_LEVEL -#define DEBUG_LEVEL 0 +#define DEBUG_LEVEL 0 #endif #define DEBUG_PRINTF(level, ...) \ { \ @@ -43,7 +43,7 @@ } #else #define DEBUG_PRINTF(level, ...) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ extern const char *PIOS_DEBUG_AssertMsg; @@ -51,7 +51,7 @@ extern const char *PIOS_DEBUG_AssertMsg; void PIOS_DEBUG_Init(void); #else #include -void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_channels); +void PIOS_DEBUG_Init(const struct pios_tim_channel *channels, uint8_t num_channels); #endif void PIOS_DEBUG_PinHigh(uint8_t pin); @@ -61,22 +61,24 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value); void PIOS_DEBUG_Panic(const char *msg); #ifdef DEBUG -#define PIOS_DEBUG_Assert(test) if (!(test)) PIOS_DEBUG_Panic(PIOS_DEBUG_AssertMsg); -#define PIOS_Assert(test) PIOS_DEBUG_Assert(test) +#define PIOS_DEBUG_Assert(test) if (!(test)) { PIOS_DEBUG_Panic(PIOS_DEBUG_AssertMsg); } +#define PIOS_Assert(test) PIOS_DEBUG_Assert(test) #else #define PIOS_DEBUG_Assert(test) -#define PIOS_Assert(test) if (!(test)) while (1); +#define PIOS_Assert(test) \ + if (!(test)) { while (1) {; } \ + } #endif /* Static (compile-time) assertion for use in a function. If test evaluates to 0 (ie fails) at compile time then compilation will fail with the error: "size of unnamed array is negative" */ -#define PIOS_STATIC_ASSERT(test) ((void)sizeof(int[1 - 2*!(test)])) +#define PIOS_STATIC_ASSERT(test) ((void)sizeof(int[1 - 2 * !(test)])) #endif /* PIOS_DEBUG_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_delay.h b/flight/pios/inc/pios_delay.h index 4b79e3ef1..4a2da6347 100644 --- a/flight/pios/inc/pios_delay.h +++ b/flight/pios/inc/pios_delay.h @@ -8,23 +8,23 @@ * * @file pios_settings.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Settings functions header + * @brief Settings functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,6 +43,6 @@ extern uint32_t PIOS_DELAY_DiffuS(uint32_t raw); #endif /* PIOS_DELAY_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_dsm.h b/flight/pios/inc/pios_dsm.h index 4bd9ed481..815a1cf23 100644 --- a/flight/pios/inc/pios_dsm.h +++ b/flight/pios/inc/pios_dsm.h @@ -11,19 +11,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/inc/pios_dsm_priv.h b/flight/pios/inc/pios_dsm_priv.h index 480273e39..1cdf88f75 100644 --- a/flight/pios/inc/pios_dsm_priv.h +++ b/flight/pios/inc/pios_dsm_priv.h @@ -48,21 +48,21 @@ * for DSM2/DSMJ: * 1 byte - lost frame counter (8 bit) * 1 byte - data format (for master receiver bound with 3 or 5 pulses), - * or unknown (for slave receiver bound with 4 or 6 pulses, + * or unknown (for slave receiver bound with 4 or 6 pulses, * some sources call it also the lost frame counter) * for DSMX: * 1 byte - unknown data (does not look like lost frame counter) * 1 byte - unknown data, has been seen only 0xB2 so far * 14 bytes - up to 7 channels (16 bit word per channel) with encoded channel - * number, channel value, the "2nd frame in a sequence" flag. - * Unused channels have FF FF instead of data bytes. + * number, channel value, the "2nd frame in a sequence" flag. + * Unused channels have FF FF instead of data bytes. * * Data format identification: * - for DSM2/DSMJ: [0 0 0 R 0 0 N1 N0] * where - * R is data resolution (0 - 10 bits, 1 - 11 bits), - * N1..N0 is the number of frames required to receive all channel + * R is data resolution (0 - 10 bits, 1 - 11 bits), + * N1..N0 is the number of frames required to receive all channel * data (01 or 10 are known to the moment, which means 1 or 2 frames). * Three values for the transmitter information byte have been seen * thus far: 0x01, 0x02, 0x12. @@ -78,7 +78,7 @@ * - for 10 bit: [F 0 C3 C2 C1 C0 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0] * - for 11 bit: [F C3 C2 C1 C0 D10 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0] * where - * F is normally 0 but set to 1 for the first channel of the 2nd frame, + * F is normally 0 but set to 1 for the first channel of the 2nd frame, * C3 to C0 is the channel number, 4 bit, zero-based, in any order, * Dx..D0 - channel data (10 or 11 bits) * @@ -98,37 +98,37 @@ * master. */ -#define DSM_CHANNELS_PER_FRAME 7 -#define DSM_FRAME_LENGTH (1+1+DSM_CHANNELS_PER_FRAME*2) -#define DSM_DSM2_RES_MASK 0x0010 -#define DSM_2ND_FRAME_MASK 0x8000 +#define DSM_CHANNELS_PER_FRAME 7 +#define DSM_FRAME_LENGTH (1 + 1 + DSM_CHANNELS_PER_FRAME * 2) +#define DSM_DSM2_RES_MASK 0x0010 +#define DSM_2ND_FRAME_MASK 0x8000 /* * Include lost frame counter and provide it as a last channel value * for debugging. Currently is not used by the receiver layer. */ -//#define DSM_LOST_FRAME_COUNTER +// #define DSM_LOST_FRAME_COUNTER /* DSM protocol variations */ enum pios_dsm_proto { - PIOS_DSM_PROTO_DSM2, - PIOS_DSM_PROTO_DSMX10BIT, - PIOS_DSM_PROTO_DSMX11BIT, + PIOS_DSM_PROTO_DSM2, + PIOS_DSM_PROTO_DSMX10BIT, + PIOS_DSM_PROTO_DSMX11BIT, }; /* DSM receiver instance configuration */ struct pios_dsm_cfg { - struct stm32_gpio bind; + struct stm32_gpio bind; }; extern const struct pios_rcvr_driver pios_dsm_rcvr_driver; extern int32_t PIOS_DSM_Init(uint32_t *dsm_id, - const struct pios_dsm_cfg *cfg, - const struct pios_com_driver *driver, - uint32_t lower_id, - enum pios_dsm_proto proto, - uint8_t bind); + const struct pios_dsm_cfg *cfg, + const struct pios_com_driver *driver, + uint32_t lower_id, + enum pios_dsm_proto proto, + uint8_t bind); #endif /* PIOS_DSM_PRIV_H */ diff --git a/flight/pios/inc/pios_eeprom.h b/flight/pios/inc/pios_eeprom.h index 2ab9a0a90..ee83f8901 100644 --- a/flight/pios/inc/pios_eeprom.h +++ b/flight/pios/inc/pios_eeprom.h @@ -10,21 +10,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief COM layer functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,8 +33,8 @@ /* Public Structures */ struct pios_eeprom_cfg { - uint32_t base_address; - uint32_t max_size; + uint32_t base_address; + uint32_t max_size; }; /* Public Functions */ @@ -45,6 +45,6 @@ extern int32_t PIOS_EEPROM_Load(uint8_t *data, uint32_t len); #endif /* PIOS_EEPROM_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_etasv3.h b/flight/pios/inc/pios_etasv3.h index fab354e5a..177cdb02e 100644 --- a/flight/pios/inc/pios_etasv3.h +++ b/flight/pios/inc/pios_etasv3.h @@ -1,38 +1,38 @@ /** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_ETASV3 ETASV3 Functions - * @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3 - * @{ - * - * @file pios_etasv3.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief ETASV3 Airspeed Sensor Driver - * @see The GNU Public License (GPL) Version 3 - * - ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_ETASV3 ETASV3 Functions + * @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3 + * @{ + * + * @file pios_etasv3.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief ETASV3 Airspeed Sensor Driver + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_ETASV3_H #define PIOS_ETASV3_H -#define ETASV3_I2C_ADDR 0x75 +#define ETASV3_I2C_ADDR 0x75 -int16_t PIOS_ETASV3_ReadAirspeed (void); +int16_t PIOS_ETASV3_ReadAirspeed(void); #endif /* PIOS_ETASV3_H */ diff --git a/flight/pios/inc/pios_exti.h b/flight/pios/inc/pios_exti.h index 4b1203aa2..529e8ee2c 100644 --- a/flight/pios/inc/pios_exti.h +++ b/flight/pios/inc/pios_exti.h @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,21 +36,21 @@ #include struct pios_exti_cfg { - bool (* vector)(void); - uint32_t line; /* use EXTI_LineN macros */ - struct stm32_gpio pin; - struct stm32_irq irq; - struct stm32_exti exti; + bool (*vector)(void); + uint32_t line; /* use EXTI_LineN macros */ + struct stm32_gpio pin; + struct stm32_irq irq; + struct stm32_exti exti; }; /* must be added to any pios_exti_cfg definition for it to be valid */ -#define __exti_config __attribute__((section("_exti"))) +#define __exti_config __attribute__((section("_exti"))) -extern int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg); +extern int32_t PIOS_EXTI_Init(const struct pios_exti_cfg *cfg); #endif /* PIOS_EXTI_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_flash.h b/flight/pios/inc/pios_flash.h index a9de63593..e8e44f67b 100644 --- a/flight/pios/inc/pios_flash.h +++ b/flight/pios/inc/pios_flash.h @@ -30,18 +30,18 @@ #include struct pios_flash_chunk { - uint8_t * addr; - uint32_t len; + uint8_t *addr; + uint32_t len; }; struct pios_flash_driver { - int32_t (*start_transaction)(uintptr_t flash_id); - int32_t (*end_transaction)(uintptr_t flash_id); - int32_t (*erase_chip)(uintptr_t flash_id); - int32_t (*erase_sector)(uintptr_t flash_id, uint32_t addr); - int32_t (*write_data)(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len); - int32_t (*write_chunks)(uintptr_t flash_id, uint32_t addr, struct pios_flash_chunk chunks[], uint32_t num_chunks); - int32_t (*read_data)(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len); + int32_t (*start_transaction)(uintptr_t flash_id); + int32_t (*end_transaction)(uintptr_t flash_id); + int32_t (*erase_chip)(uintptr_t flash_id); + int32_t (*erase_sector)(uintptr_t flash_id, uint32_t addr); + int32_t (*write_data)(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len); + int32_t (*write_chunks)(uintptr_t flash_id, uint32_t addr, struct pios_flash_chunk chunks[], uint32_t num_chunks); + int32_t (*read_data)(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len); }; -#endif /* PIOS_FLASH_H */ +#endif /* PIOS_FLASH_H */ diff --git a/flight/pios/inc/pios_flash_internal_priv.h b/flight/pios/inc/pios_flash_internal_priv.h index 15be5df5f..e8edeed07 100644 --- a/flight/pios/inc/pios_flash_internal_priv.h +++ b/flight/pios/inc/pios_flash_internal_priv.h @@ -2,39 +2,39 @@ ****************************************************************************** * @file pios_flash_internal_priv.h * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 - * @addtogroup + * @addtogroup * @{ - * @addtogroup + * @addtogroup * @{ * @brief Provides a flash driver for the STM32 internal flash sectors *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_FLASH_INTERNAL_H #define PIOS_FLASH_INTERNAL_H -#include "pios_flash.h" /* API definition for flash drivers */ +#include "pios_flash.h" /* API definition for flash drivers */ extern const struct pios_flash_driver pios_internal_flash_driver; struct pios_flash_internal_cfg { - ; + ; }; -extern int32_t PIOS_Flash_Internal_Init(uintptr_t * flash_id, const struct pios_flash_internal_cfg * cfg); +extern int32_t PIOS_Flash_Internal_Init(uintptr_t *flash_id, const struct pios_flash_internal_cfg *cfg); -#endif /* PIOS_FLASH_INTERNAL_H */ +#endif /* PIOS_FLASH_INTERNAL_H */ diff --git a/flight/pios/inc/pios_flash_jedec_catalog.h b/flight/pios/inc/pios_flash_jedec_catalog.h index 4c6349be0..cb15fa8b9 100644 --- a/flight/pios/inc/pios_flash_jedec_catalog.h +++ b/flight/pios/inc/pios_flash_jedec_catalog.h @@ -34,36 +34,36 @@ #include // this structure contains the catalog of all "known" flash chip used -const struct pios_flash_jedec_cfg pios_flash_jedec_catalog [] = +const struct pios_flash_jedec_cfg pios_flash_jedec_catalog[] = { - { // m25p16 - .expect_manufacturer = JEDEC_MANUFACTURER_ST, - .expect_memorytype = 0x20, - .expect_capacity = 0x15, - .sector_erase = 0xD8, - .chip_erase = 0xC7, - }, - { // m25px16 - .expect_manufacturer = JEDEC_MANUFACTURER_ST, - .expect_memorytype = 0x71, - .expect_capacity = 0x15, - .sector_erase = 0xD8, - .chip_erase = 0xC7, - }, - { //w25x - .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, - .expect_memorytype = 0x30, - .expect_capacity = 0x13, - .sector_erase = 0x20, - .chip_erase = 0x60 - }, - { //25q16 - .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, - .expect_memorytype = 0x40, - .expect_capacity = 0x15, - .sector_erase = 0x20, - .chip_erase = 0x60 - } + { // m25p16 + .expect_manufacturer = JEDEC_MANUFACTURER_ST, + .expect_memorytype = 0x20, + .expect_capacity = 0x15, + .sector_erase = 0xD8, + .chip_erase = 0xC7, + }, + { // m25px16 + .expect_manufacturer = JEDEC_MANUFACTURER_ST, + .expect_memorytype = 0x71, + .expect_capacity = 0x15, + .sector_erase = 0xD8, + .chip_erase = 0xC7, + }, + { // w25x + .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, + .expect_memorytype = 0x30, + .expect_capacity = 0x13, + .sector_erase = 0x20, + .chip_erase = 0x60 + }, + { // 25q16 + .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, + .expect_memorytype = 0x40, + .expect_capacity = 0x15, + .sector_erase = 0x20, + .chip_erase = 0x60 + } }; const uint32_t pios_flash_jedec_catalog_size = NELEMENTS(pios_flash_jedec_catalog); diff --git a/flight/pios/inc/pios_flash_jedec_priv.h b/flight/pios/inc/pios_flash_jedec_priv.h index 845d4e296..fb021ee3c 100644 --- a/flight/pios/inc/pios_flash_jedec_priv.h +++ b/flight/pios/inc/pios_flash_jedec_priv.h @@ -31,7 +31,7 @@ #ifndef PIOS_FLASH_JEDEC_H #define PIOS_FLASH_JEDEC_H -#include "pios_flash.h" /* API definition for flash drivers */ +#include "pios_flash.h" /* API definition for flash drivers */ extern const struct pios_flash_driver pios_jedec_flash_driver; @@ -40,13 +40,13 @@ extern const struct pios_flash_driver pios_jedec_flash_driver; #define JEDEC_MANUFACTURER_WINBOND 0xEF struct pios_flash_jedec_cfg { - uint8_t expect_manufacturer; - uint8_t expect_memorytype; - uint8_t expect_capacity; - uint32_t sector_erase; - uint32_t chip_erase; + uint8_t expect_manufacturer; + uint8_t expect_memorytype; + uint8_t expect_capacity; + uint32_t sector_erase; + uint32_t chip_erase; }; -int32_t PIOS_Flash_Jedec_Init(uintptr_t * flash_id, uint32_t spi_id, uint32_t slave_num); +int32_t PIOS_Flash_Jedec_Init(uintptr_t *flash_id, uint32_t spi_id, uint32_t slave_num); -#endif /* PIOS_FLASH_JEDEC_H */ +#endif /* PIOS_FLASH_JEDEC_H */ diff --git a/flight/pios/inc/pios_flashfs.h b/flight/pios/inc/pios_flashfs.h index 0ecb713fd..beb4c1e63 100644 --- a/flight/pios/inc/pios_flashfs.h +++ b/flight/pios/inc/pios_flashfs.h @@ -30,8 +30,8 @@ #include int32_t PIOS_FLASHFS_Format(uintptr_t fs_id); -int32_t PIOS_FLASHFS_ObjSave(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size); -int32_t PIOS_FLASHFS_ObjLoad(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size); +int32_t PIOS_FLASHFS_ObjSave(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size); +int32_t PIOS_FLASHFS_ObjLoad(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size); int32_t PIOS_FLASHFS_ObjDelete(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id); -#endif /* PIOS_FLASHFS_H */ +#endif /* PIOS_FLASHFS_H */ diff --git a/flight/pios/inc/pios_flashfs_logfs_priv.h b/flight/pios/inc/pios_flashfs_logfs_priv.h index 24212d7e9..e037ae8b1 100644 --- a/flight/pios/inc/pios_flashfs_logfs_priv.h +++ b/flight/pios/inc/pios_flashfs_logfs_priv.h @@ -28,21 +28,21 @@ #define PIOS_FLASHFS_LOGFS_PRIV_H #include -#include "pios_flash.h" /* struct pios_flash_driver */ +#include "pios_flash.h" /* struct pios_flash_driver */ struct flashfs_logfs_cfg { - uint32_t fs_magic; - uint32_t total_fs_size; /* Total size of all generations of the filesystem */ - uint32_t arena_size; /* Max size of one generation of the filesystem */ - uint32_t slot_size; /* Max size of a "file" */ + uint32_t fs_magic; + uint32_t total_fs_size; /* Total size of all generations of the filesystem */ + uint32_t arena_size; /* Max size of one generation of the filesystem */ + uint32_t slot_size; /* Max size of a "file" */ - uint32_t start_offset; /* Offset into flash where this filesystem starts */ - uint32_t sector_size; /* Size of a flash erase block */ - uint32_t page_size; /* Maximum flash burst write size */ + uint32_t start_offset; /* Offset into flash where this filesystem starts */ + uint32_t sector_size; /* Size of a flash erase block */ + uint32_t page_size; /* Maximum flash burst write size */ }; -int32_t PIOS_FLASHFS_Logfs_Init(uintptr_t * fs_id, const struct flashfs_logfs_cfg * cfg, const struct pios_flash_driver * driver, uintptr_t flash_id); +int32_t PIOS_FLASHFS_Logfs_Init(uintptr_t *fs_id, const struct flashfs_logfs_cfg *cfg, const struct pios_flash_driver *driver, uintptr_t flash_id); int32_t PIOS_FLASHFS_Logfs_Destroy(uintptr_t fs_id); -#endif /* PIOS_FLASHFS_LOGFS_PRIV_H */ +#endif /* PIOS_FLASHFS_LOGFS_PRIV_H */ diff --git a/flight/pios/inc/pios_gpio.h b/flight/pios/inc/pios_gpio.h index a611ff053..aec0ba417 100644 --- a/flight/pios/inc/pios_gpio.h +++ b/flight/pios/inc/pios_gpio.h @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,6 +41,6 @@ extern void PIOS_GPIO_Toggle(uint8_t Pin); #endif /* PIOS_GPIO_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_hcsr04.h b/flight/pios/inc/pios_hcsr04.h index fe735e336..9de75ad62 100644 --- a/flight/pios/inc/pios_hcsr04.h +++ b/flight/pios/inc/pios_hcsr04.h @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,7 +38,7 @@ extern void PIOS_HCSR04_Trigger(void); #endif /* PIOS_HCSR04_H */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_hcsr04_priv.h b/flight/pios/inc/pios_hcsr04_priv.h index c53bdddf1..a60138bf5 100644 --- a/flight/pios/inc/pios_hcsr04_priv.h +++ b/flight/pios/inc/pios_hcsr04_priv.h @@ -37,15 +37,15 @@ #include struct pios_hcsr04_cfg { - TIM_ICInitTypeDef tim_ic_init; - const struct pios_tim_channel * channels; - uint8_t num_channels; - struct stm32_gpio trigger; + TIM_ICInitTypeDef tim_ic_init; + const struct pios_tim_channel *channels; + uint8_t num_channels; + struct stm32_gpio trigger; }; extern const struct pios_rcvr_driver pios_pwm_rcvr_driver; -extern int32_t PIOS_HCSR04_Init(uint32_t * pwm_id, const struct pios_hcsr04_cfg * cfg); +extern int32_t PIOS_HCSR04_Init(uint32_t *pwm_id, const struct pios_hcsr04_cfg *cfg); #endif /* PIOS_HCSR04_PRIV_H */ diff --git a/flight/pios/inc/pios_hmc5843.h b/flight/pios/inc/pios_hmc5843.h index 21b3f426a..72f1e02b0 100644 --- a/flight/pios/inc/pios_hmc5843.h +++ b/flight/pios/inc/pios_hmc5843.h @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,7 +39,7 @@ extern void PIOS_HMC5843_ReadID(uint8_t out[4]); #endif /* PIOS_HMC5843_H */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_hmc5883.h b/flight/pios/inc/pios_hmc5883.h index 921b99d07..6d34dae20 100644 --- a/flight/pios/inc/pios_hmc5883.h +++ b/flight/pios/inc/pios_hmc5883.h @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,77 +32,76 @@ #define PIOS_HMC5883_H /* HMC5883 Addresses */ -#define PIOS_HMC5883_I2C_ADDR 0x1E +#define PIOS_HMC5883_I2C_ADDR 0x1E #define PIOS_HMC5883_I2C_READ_ADDR 0x3D #define PIOS_HMC5883_I2C_WRITE_ADDR 0x3C -#define PIOS_HMC5883_CONFIG_REG_A (uint8_t)0x00 -#define PIOS_HMC5883_CONFIG_REG_B (uint8_t)0x01 -#define PIOS_HMC5883_MODE_REG (uint8_t)0x02 -#define PIOS_HMC5883_DATAOUT_XMSB_REG 0x03 -#define PIOS_HMC5883_DATAOUT_XLSB_REG 0x04 -#define PIOS_HMC5883_DATAOUT_ZMSB_REG 0x05 -#define PIOS_HMC5883_DATAOUT_ZLSB_REG 0x06 -#define PIOS_HMC5883_DATAOUT_YMSB_REG 0x07 -#define PIOS_HMC5883_DATAOUT_YLSB_REG 0x08 -#define PIOS_HMC5883_DATAOUT_STATUS_REG 0x09 -#define PIOS_HMC5883_DATAOUT_IDA_REG 0x0A -#define PIOS_HMC5883_DATAOUT_IDB_REG 0x0B -#define PIOS_HMC5883_DATAOUT_IDC_REG 0x0C +#define PIOS_HMC5883_CONFIG_REG_A (uint8_t)0x00 +#define PIOS_HMC5883_CONFIG_REG_B (uint8_t)0x01 +#define PIOS_HMC5883_MODE_REG (uint8_t)0x02 +#define PIOS_HMC5883_DATAOUT_XMSB_REG 0x03 +#define PIOS_HMC5883_DATAOUT_XLSB_REG 0x04 +#define PIOS_HMC5883_DATAOUT_ZMSB_REG 0x05 +#define PIOS_HMC5883_DATAOUT_ZLSB_REG 0x06 +#define PIOS_HMC5883_DATAOUT_YMSB_REG 0x07 +#define PIOS_HMC5883_DATAOUT_YLSB_REG 0x08 +#define PIOS_HMC5883_DATAOUT_STATUS_REG 0x09 +#define PIOS_HMC5883_DATAOUT_IDA_REG 0x0A +#define PIOS_HMC5883_DATAOUT_IDB_REG 0x0B +#define PIOS_HMC5883_DATAOUT_IDC_REG 0x0C /* Output Data Rate */ -#define PIOS_HMC5883_ODR_0_75 0x00 -#define PIOS_HMC5883_ODR_1_5 0x04 -#define PIOS_HMC5883_ODR_3 0x08 -#define PIOS_HMC5883_ODR_7_5 0x0C -#define PIOS_HMC5883_ODR_15 0x10 -#define PIOS_HMC5883_ODR_30 0x14 -#define PIOS_HMC5883_ODR_75 0x18 +#define PIOS_HMC5883_ODR_0_75 0x00 +#define PIOS_HMC5883_ODR_1_5 0x04 +#define PIOS_HMC5883_ODR_3 0x08 +#define PIOS_HMC5883_ODR_7_5 0x0C +#define PIOS_HMC5883_ODR_15 0x10 +#define PIOS_HMC5883_ODR_30 0x14 +#define PIOS_HMC5883_ODR_75 0x18 /* Measure configuration */ -#define PIOS_HMC5883_MEASCONF_NORMAL 0x00 -#define PIOS_HMC5883_MEASCONF_BIAS_POS 0x01 -#define PIOS_HMC5883_MEASCONF_BIAS_NEG 0x02 +#define PIOS_HMC5883_MEASCONF_NORMAL 0x00 +#define PIOS_HMC5883_MEASCONF_BIAS_POS 0x01 +#define PIOS_HMC5883_MEASCONF_BIAS_NEG 0x02 /* Gain settings */ -#define PIOS_HMC5883_GAIN_0_88 0x00 -#define PIOS_HMC5883_GAIN_1_3 0x20 -#define PIOS_HMC5883_GAIN_1_9 0x40 -#define PIOS_HMC5883_GAIN_2_5 0x60 -#define PIOS_HMC5883_GAIN_4_0 0x80 -#define PIOS_HMC5883_GAIN_4_7 0xA0 -#define PIOS_HMC5883_GAIN_5_6 0xC0 -#define PIOS_HMC5883_GAIN_8_1 0xE0 +#define PIOS_HMC5883_GAIN_0_88 0x00 +#define PIOS_HMC5883_GAIN_1_3 0x20 +#define PIOS_HMC5883_GAIN_1_9 0x40 +#define PIOS_HMC5883_GAIN_2_5 0x60 +#define PIOS_HMC5883_GAIN_4_0 0x80 +#define PIOS_HMC5883_GAIN_4_7 0xA0 +#define PIOS_HMC5883_GAIN_5_6 0xC0 +#define PIOS_HMC5883_GAIN_8_1 0xE0 /* Modes */ -#define PIOS_HMC5883_MODE_CONTINUOUS 0x00 -#define PIOS_HMC5883_MODE_SINGLE 0x01 -#define PIOS_HMC5883_MODE_IDLE 0x02 -#define PIOS_HMC5883_MODE_SLEEP 0x03 +#define PIOS_HMC5883_MODE_CONTINUOUS 0x00 +#define PIOS_HMC5883_MODE_SINGLE 0x01 +#define PIOS_HMC5883_MODE_IDLE 0x02 +#define PIOS_HMC5883_MODE_SLEEP 0x03 /* Sensitivity Conversion Values */ -#define PIOS_HMC5883_Sensitivity_0_88Ga 1370 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_1_3Ga 1090 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_1_9Ga 820 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_2_5Ga 660 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_4_0Ga 440 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_4_7Ga 390 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_5_6Ga 330 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED +#define PIOS_HMC5883_Sensitivity_0_88Ga 1370 // LSB/Ga +#define PIOS_HMC5883_Sensitivity_1_3Ga 1090 // LSB/Ga +#define PIOS_HMC5883_Sensitivity_1_9Ga 820 // LSB/Ga +#define PIOS_HMC5883_Sensitivity_2_5Ga 660 // LSB/Ga +#define PIOS_HMC5883_Sensitivity_4_0Ga 440 // LSB/Ga +#define PIOS_HMC5883_Sensitivity_4_7Ga 390 // LSB/Ga +#define PIOS_HMC5883_Sensitivity_5_6Ga 330 // LSB/Ga +#define PIOS_HMC5883_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED struct pios_hmc5883_cfg { #ifdef PIOS_HMC5883_HAS_GPIOS - const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ + const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ #endif - uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ - uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ - uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ - uint8_t Mode; - + uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ + uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t Mode; }; /* Public Functions */ -extern void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg * cfg); +extern void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg *cfg); extern bool PIOS_HMC5883_NewDataAvailable(void); extern int32_t PIOS_HMC5883_ReadMag(int16_t out[3]); extern uint8_t PIOS_HMC5883_ReadID(uint8_t out[4]); @@ -111,7 +110,7 @@ extern bool PIOS_HMC5883_IRQHandler(); #endif /* PIOS_HMC5883_H */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_i2c.h b/flight/pios/inc/pios_i2c.h index b34acf9e6..db6636f37 100644 --- a/flight/pios/inc/pios_i2c.h +++ b/flight/pios/inc/pios_i2c.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_I2C I2C Functions * @{ * - * @file pios_i2c.h + * @file pios_i2c.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief I2C functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,31 +36,31 @@ /* Global Types */ enum pios_i2c_txn_direction { - PIOS_I2C_TXN_READ, - PIOS_I2C_TXN_WRITE + PIOS_I2C_TXN_READ, + PIOS_I2C_TXN_WRITE }; struct pios_i2c_txn { - const char *info; - uint16_t addr; - enum pios_i2c_txn_direction rw; - uint32_t len; - uint8_t *buf; + const char *info; + uint16_t addr; + enum pios_i2c_txn_direction rw; + uint32_t len; + uint8_t *buf; }; #define I2C_LOG_DEPTH 20 enum pios_i2c_error_type { - PIOS_I2C_ERROR_EVENT, - PIOS_I2C_ERROR_FSM, - PIOS_I2C_ERROR_INTERRUPT + PIOS_I2C_ERROR_EVENT, + PIOS_I2C_ERROR_FSM, + PIOS_I2C_ERROR_INTERRUPT }; struct pios_i2c_fault_history { - enum pios_i2c_error_type type; - uint32_t evirq[I2C_LOG_DEPTH]; - uint32_t erirq[I2C_LOG_DEPTH]; - uint8_t event[I2C_LOG_DEPTH]; - uint8_t state[I2C_LOG_DEPTH]; + enum pios_i2c_error_type type; + uint32_t evirq[I2C_LOG_DEPTH]; + uint32_t erirq[I2C_LOG_DEPTH]; + uint8_t event[I2C_LOG_DEPTH]; + uint8_t state[I2C_LOG_DEPTH]; }; /* Public Functions */ @@ -68,11 +68,11 @@ extern int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_ extern int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns, void *callback); extern void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id); extern void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id); -extern void PIOS_I2C_GetDiagnostics(struct pios_i2c_fault_history * data, uint8_t * error_counts); +extern void PIOS_I2C_GetDiagnostics(struct pios_i2c_fault_history *data, uint8_t *error_counts); #endif /* PIOS_I2C_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_i2c_esc.h b/flight/pios/inc/pios_i2c_esc.h index c403030b3..67e84b80c 100644 --- a/flight/pios/inc/pios_i2c_esc.h +++ b/flight/pios/inc/pios_i2c_esc.h @@ -13,19 +13,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,7 +43,7 @@ bool PIOS_SetAstec4Speed(uint8_t motornum, uint8_t speed); #endif /* PIOS_I2C_ESC_H */ -/** +/** * @} * @} */ diff --git a/flight/pios/inc/pios_i2c_priv.h b/flight/pios/inc/pios_i2c_priv.h index 8a2769321..aeeff5648 100644 --- a/flight/pios/inc/pios_i2c_priv.h +++ b/flight/pios/inc/pios_i2c_priv.h @@ -31,81 +31,81 @@ #include struct pios_i2c_adapter_cfg { - I2C_TypeDef *regs; - uint32_t remap; - I2C_InitTypeDef init; + I2C_TypeDef *regs; + uint32_t remap; + I2C_InitTypeDef init; - uint32_t transfer_timeout_ms; - struct stm32_gpio scl; - struct stm32_gpio sda; - struct stm32_irq event; - struct stm32_irq error; + uint32_t transfer_timeout_ms; + struct stm32_gpio scl; + struct stm32_gpio sda; + struct stm32_irq event; + struct stm32_irq error; }; enum i2c_adapter_state { - I2C_STATE_FSM_FAULT = 0, /* Must be zero so undefined transitions land here */ + I2C_STATE_FSM_FAULT = 0, /* Must be zero so undefined transitions land here */ - I2C_STATE_BUS_ERROR, + I2C_STATE_BUS_ERROR, - I2C_STATE_STOPPED, - I2C_STATE_STOPPING, - I2C_STATE_STARTING, + I2C_STATE_STOPPED, + I2C_STATE_STOPPING, + I2C_STATE_STARTING, - I2C_STATE_R_MORE_TXN_ADDR, - I2C_STATE_R_MORE_TXN_PRE_ONE, - I2C_STATE_R_MORE_TXN_PRE_FIRST, - I2C_STATE_R_MORE_TXN_PRE_MIDDLE, - I2C_STATE_R_MORE_TXN_PRE_LAST, - I2C_STATE_R_MORE_TXN_POST_LAST, + I2C_STATE_R_MORE_TXN_ADDR, + I2C_STATE_R_MORE_TXN_PRE_ONE, + I2C_STATE_R_MORE_TXN_PRE_FIRST, + I2C_STATE_R_MORE_TXN_PRE_MIDDLE, + I2C_STATE_R_MORE_TXN_PRE_LAST, + I2C_STATE_R_MORE_TXN_POST_LAST, - I2C_STATE_R_LAST_TXN_ADDR, - I2C_STATE_R_LAST_TXN_PRE_ONE, - I2C_STATE_R_LAST_TXN_PRE_FIRST, - I2C_STATE_R_LAST_TXN_PRE_MIDDLE, - I2C_STATE_R_LAST_TXN_PRE_LAST, - I2C_STATE_R_LAST_TXN_POST_LAST, + I2C_STATE_R_LAST_TXN_ADDR, + I2C_STATE_R_LAST_TXN_PRE_ONE, + I2C_STATE_R_LAST_TXN_PRE_FIRST, + I2C_STATE_R_LAST_TXN_PRE_MIDDLE, + I2C_STATE_R_LAST_TXN_PRE_LAST, + I2C_STATE_R_LAST_TXN_POST_LAST, - I2C_STATE_W_MORE_TXN_ADDR, - I2C_STATE_W_MORE_TXN_MIDDLE, - I2C_STATE_W_MORE_TXN_LAST, + I2C_STATE_W_MORE_TXN_ADDR, + I2C_STATE_W_MORE_TXN_MIDDLE, + I2C_STATE_W_MORE_TXN_LAST, - I2C_STATE_W_LAST_TXN_ADDR, - I2C_STATE_W_LAST_TXN_MIDDLE, - I2C_STATE_W_LAST_TXN_LAST, - - I2C_STATE_NACK, - - I2C_STATE_NUM_STATES /* Must be last */ + I2C_STATE_W_LAST_TXN_ADDR, + I2C_STATE_W_LAST_TXN_MIDDLE, + I2C_STATE_W_LAST_TXN_LAST, + + I2C_STATE_NACK, + + I2C_STATE_NUM_STATES /* Must be last */ }; enum pios_i2c_adapter_magic { - PIOS_I2C_DEV_MAGIC = 0xa9a9b8b8, + PIOS_I2C_DEV_MAGIC = 0xa9a9b8b8, }; struct pios_i2c_adapter { - enum pios_i2c_adapter_magic magic; - const struct pios_i2c_adapter_cfg * cfg; + enum pios_i2c_adapter_magic magic; + const struct pios_i2c_adapter_cfg *cfg; #ifdef PIOS_INCLUDE_FREERTOS - xSemaphoreHandle sem_busy; - xSemaphoreHandle sem_ready; + xSemaphoreHandle sem_busy; + xSemaphoreHandle sem_ready; #else - uint8_t busy; + uint8_t busy; #endif - bool bus_error; - bool nack; + bool bus_error; + bool nack; - volatile enum i2c_adapter_state curr_state; - const struct pios_i2c_txn *first_txn; - const struct pios_i2c_txn *active_txn; - const struct pios_i2c_txn *last_txn; + volatile enum i2c_adapter_state curr_state; + const struct pios_i2c_txn *first_txn; + const struct pios_i2c_txn *active_txn; + const struct pios_i2c_txn *last_txn; - void (*callback) (); - - uint8_t *active_byte; - uint8_t *last_byte; + void (*callback)(); + + uint8_t *active_byte; + uint8_t *last_byte; }; -int32_t PIOS_I2C_Init(uint32_t * i2c_id, const struct pios_i2c_adapter_cfg * cfg); +int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg); #endif /* PIOS_I2C_PRIV_H */ diff --git a/flight/pios/inc/pios_iap.h b/flight/pios/inc/pios_iap.h index 20a119f4d..0c85ec6ba 100644 --- a/flight/pios/inc/pios_iap.h +++ b/flight/pios/inc/pios_iap.h @@ -6,25 +6,25 @@ * @brief In-Application-Programming Module * @{ * - * @file pios_iap.c + * @file pios_iap.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief IAP functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,54 +33,53 @@ #define PIOS_IAP_H - /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ #include /***************************************************************************************** * Public Definitions/Macros ****************************************************************************************/ -#define MAGIC_REG_1 PIOS_BKP_RESERVED_1 -#define MAGIC_REG_2 PIOS_BKP_RESERVED_2 -#define IAP_BOOTCOUNT PIOS_BKP_RESERVED_3 -#define IAP_CMD1 PIOS_BKP_RESERVED_5 -#define IAP_CMD2 PIOS_BKP_RESERVED_6 -#define IAP_CMD3 PIOS_BKP_RESERVED_7 +#define MAGIC_REG_1 PIOS_BKP_RESERVED_1 +#define MAGIC_REG_2 PIOS_BKP_RESERVED_2 +#define IAP_BOOTCOUNT PIOS_BKP_RESERVED_3 +#define IAP_CMD1 PIOS_BKP_RESERVED_5 +#define IAP_CMD2 PIOS_BKP_RESERVED_6 +#define IAP_CMD3 PIOS_BKP_RESERVED_7 #define PIOS_IAP_CLEAR_FLASH_CMD_0 0xFA5F #define PIOS_IAP_CLEAR_FLASH_CMD_1 0x0001 #define PIOS_IAP_CLEAR_FLASH_CMD_2 0x0000 -#define PIOS_IAP_CMD_COUNT 3 +#define PIOS_IAP_CMD_COUNT 3 /**************************************************************************************** - * Public Functions - ****************************************************************************************/ -void PIOS_IAP_Init(void); -uint32_t PIOS_IAP_CheckRequest( void ); -void PIOS_IAP_SetRequest1(void); -void PIOS_IAP_SetRequest2(void); -void PIOS_IAP_ClearRequest(void); -uint16_t PIOS_IAP_ReadBootCount(void); -void PIOS_IAP_WriteBootCount(uint16_t); +* Public Functions +****************************************************************************************/ +void PIOS_IAP_Init(void); +uint32_t PIOS_IAP_CheckRequest(void); +void PIOS_IAP_SetRequest1(void); +void PIOS_IAP_SetRequest2(void); +void PIOS_IAP_ClearRequest(void); +uint16_t PIOS_IAP_ReadBootCount(void); +void PIOS_IAP_WriteBootCount(uint16_t); /** - * @brief Return one of the IAP command values passed from bootloader. - * @param number: the index of the command value (0..2). - * @retval the selected command value. - */ + * @brief Return one of the IAP command values passed from bootloader. + * @param number: the index of the command value (0..2). + * @retval the selected command value. + */ uint32_t PIOS_IAP_ReadBootCmd(uint8_t number); /** - * @brief Write one of the IAP command values to be passed to firmware from bootloader. - * @param number: the index of the command value (0..2). - * @param value: value to be written. - */ + * @brief Write one of the IAP command values to be passed to firmware from bootloader. + * @param number: the index of the command value (0..2). + * @param value: value to be written. + */ void PIOS_IAP_WriteBootCmd(uint8_t number, uint32_t value); /**************************************************************************************** - * Public Data - ****************************************************************************************/ +* Public Data +****************************************************************************************/ #endif /* PIOS_IAP_H */ diff --git a/flight/pios/inc/pios_initcall.h b/flight/pios/inc/pios_initcall.h index 410c3c72a..6614b48dd 100644 --- a/flight/pios/inc/pios_initcall.h +++ b/flight/pios/inc/pios_initcall.h @@ -6,32 +6,32 @@ * @brief Initcall Macros * @{ * - * @file pios_initcall.h + * @file pios_initcall.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. * @brief Initcall header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_INITCALL_H #define PIOS_INITCALL_H -/* +/* * This implementation is heavily based on the Linux Kernel initcall * infrastructure: * http://lxr.linux.no/#linux/include/linux/init.h @@ -44,8 +44,8 @@ typedef int32_t (*initcall_t)(void); typedef struct { - initcall_t fn_minit; - initcall_t fn_tinit; + initcall_t fn_minit; + initcall_t fn_tinit; } initmodule_t; /* Init module section */ @@ -58,20 +58,22 @@ extern void StartModules(); #define MODULE_INITCALL(ifn, sfn) -#define MODULE_TASKCREATE_ALL { \ - /* Start all module threads */ \ - StartModules(); \ - } +#define MODULE_TASKCREATE_ALL \ + { \ + /* Start all module threads */ \ + StartModules(); \ + } -#define MODULE_INITIALISE_ALL { \ - /* Initialize modules */ \ - InitModules(); \ - /* Initialize the system thread */ \ - SystemModInitialize();} +#define MODULE_INITIALISE_ALL \ + { \ + /* Initialize modules */ \ + InitModules(); \ + /* Initialize the system thread */ \ + SystemModInitialize(); } #else -/* initcalls are now grouped by functionality into separate +/* initcalls are now grouped by functionality into separate * subsections. Ordering inside the subsections is determined * by link order. * @@ -79,27 +81,33 @@ extern void StartModules(); * can point at the same handler without causing duplicate-symbol build errors. */ -#define __define_initcall(level,fn,id) \ - static initcall_t __initcall_##fn##id __attribute__((__used__)) \ - __attribute__((__section__(".initcall" level ".init"))) = fn +#define __define_initcall(level, fn, id) \ + static initcall_t __initcall_##fn##id __attribute__((__used__)) \ + __attribute__((__section__(".initcall" level ".init"))) = fn #define __define_module_initcall(level, ifn, sfn) \ - static initmodule_t __initcall_##fn __attribute__((__used__)) \ - __attribute__((__section__(".initcall" level ".init"))) = { .fn_minit = ifn, .fn_tinit = sfn }; + static initmodule_t __initcall_##fn __attribute__((__used__)) \ + __attribute__((__section__(".initcall" level ".init"))) = { .fn_minit = ifn, .fn_tinit = sfn }; -#define MODULE_INITCALL(ifn, sfn) __define_module_initcall("module", ifn, sfn) +#define MODULE_INITCALL(ifn, sfn) __define_module_initcall("module", ifn, sfn) -#define MODULE_INITIALISE_ALL { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) \ - if (fn->fn_minit) \ - (fn->fn_minit)(); } +#define MODULE_INITIALISE_ALL \ + { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { \ + if (fn->fn_minit) { \ + (fn->fn_minit)(); } \ + } \ + } -#define MODULE_TASKCREATE_ALL { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) \ - if (fn->fn_tinit) \ - (fn->fn_tinit)(); } +#define MODULE_TASKCREATE_ALL \ + { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { \ + if (fn->fn_tinit) { \ + (fn->fn_tinit)(); } \ + } \ + } #endif /* USE_SIM_POSIX */ -#endif /* PIOS_INITCALL_H */ +#endif /* PIOS_INITCALL_H */ /** * @} diff --git a/flight/pios/inc/pios_irq.h b/flight/pios/inc/pios_irq.h index f9b9dbb97..e934ae1f6 100644 --- a/flight/pios/inc/pios_irq.h +++ b/flight/pios/inc/pios_irq.h @@ -5,26 +5,26 @@ * @addtogroup PIOS_IRQ IRQ Setup Functions * @{ * - * @file pios_irq.h + * @file pios_irq.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief IRQ functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/inc/pios_l3gd20.h b/flight/pios/inc/pios_l3gd20.h index 794c5c2f2..0c6b3e43c 100644 --- a/flight/pios/inc/pios_l3gd20.h +++ b/flight/pios/inc/pios_l3gd20.h @@ -13,19 +13,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -103,38 +103,38 @@ #define PIOS_L3GD20_PWRMGMT_STOP_CLK 0X07 enum pios_l3gd20_range { - PIOS_L3GD20_SCALE_250_DEG = 0x00, - PIOS_L3GD20_SCALE_500_DEG = 0x10, - PIOS_L3GD20_SCALE_2000_DEG = 0x3 + PIOS_L3GD20_SCALE_250_DEG = 0x00, + PIOS_L3GD20_SCALE_500_DEG = 0x10, + PIOS_L3GD20_SCALE_2000_DEG = 0x3 }; enum pios_l3gd20_filter { - PIOS_L3GD20_LOWPASS_256_HZ = 0x00, - PIOS_L3GD20_LOWPASS_188_HZ = 0x01, - PIOS_L3GD20_LOWPASS_98_HZ = 0x02, - PIOS_L3GD20_LOWPASS_42_HZ = 0x03, - PIOS_L3GD20_LOWPASS_20_HZ = 0x04, - PIOS_L3GD20_LOWPASS_10_HZ = 0x05, - PIOS_L3GD20_LOWPASS_5_HZ = 0x06 + PIOS_L3GD20_LOWPASS_256_HZ = 0x00, + PIOS_L3GD20_LOWPASS_188_HZ = 0x01, + PIOS_L3GD20_LOWPASS_98_HZ = 0x02, + PIOS_L3GD20_LOWPASS_42_HZ = 0x03, + PIOS_L3GD20_LOWPASS_20_HZ = 0x04, + PIOS_L3GD20_LOWPASS_10_HZ = 0x05, + PIOS_L3GD20_LOWPASS_5_HZ = 0x06 }; struct pios_l3gd20_data { - int16_t gyro_x; - int16_t gyro_y; - int16_t gyro_z; - int16_t temperature; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + int16_t temperature; }; struct pios_l3gd20_cfg { - const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ + const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ - enum pios_l3gd20_range range; + enum pios_l3gd20_range range; }; /* Public Functions */ -extern int32_t PIOS_L3GD20_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_l3gd20_cfg * cfg); +extern int32_t PIOS_L3GD20_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_l3gd20_cfg *cfg); extern xQueueHandle PIOS_L3GD20_GetQueue(); -extern int32_t PIOS_L3GD20_ReadGyros(struct pios_l3gd20_data * buffer); +extern int32_t PIOS_L3GD20_ReadGyros(struct pios_l3gd20_data *buffer); extern int32_t PIOS_L3GD20_SetRange(enum pios_l3gd20_range range); extern float PIOS_L3GD20_GetScale(); extern int32_t PIOS_L3GD20_ReadID(); @@ -143,7 +143,7 @@ extern bool PIOS_L3GD20_IRQHandler(); #endif /* PIOS_L3GD20_H */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_led.h b/flight/pios/inc/pios_led.h index 2506b75cf..2732ad56c 100644 --- a/flight/pios/inc/pios_led.h +++ b/flight/pios/inc/pios_led.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_LED LED Functions * @{ * - * @file pios_led.h + * @file pios_led.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief LED functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/inc/pios_led_priv.h b/flight/pios/inc/pios_led_priv.h index 36a1ee3f7..e0ce76a65 100644 --- a/flight/pios/inc/pios_led_priv.h +++ b/flight/pios/inc/pios_led_priv.h @@ -35,21 +35,21 @@ #include struct pios_led { - struct stm32_gpio pin; - uint32_t remap; - bool active_high; + struct stm32_gpio pin; + uint32_t remap; + bool active_high; }; struct pios_led_cfg { - const struct pios_led * leds; - uint8_t num_leds; + const struct pios_led *leds; + uint8_t num_leds; }; -extern int32_t PIOS_LED_Init(const struct pios_led_cfg * cfg); +extern int32_t PIOS_LED_Init(const struct pios_led_cfg *cfg); #endif /* PIOS_LED_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h index d81249e56..47c8d798b 100644 --- a/flight/pios/inc/pios_math.h +++ b/flight/pios/inc/pios_math.h @@ -37,7 +37,7 @@ #define M_PI_2_F 1.57079632679489661923132169164f /* pi/2 */ #define M_PI_4_F 0.78539816339744830961566084582f /* pi/4 */ #define M_SQRTPI_F 1.77245385090551602729816748334f /* sqrt(pi) */ -#define M_2_SQRTPI_F 1.12837916709551257389615890312f /* 2/sqrt(pi) */ +#define M_2_SQRTPI_F 1.12837916709551257389615890312f /* 2/sqrt(pi) */ #define M_1_PI_F 0.31830988618379067153776752675f /* 1/pi */ #define M_2_PI_F 0.63661977236758134307553505349f /* 2/pi */ #define M_LN10_F 2.30258509299404568401799145468f /* ln(10) */ @@ -46,11 +46,11 @@ #define M_EULER_F 0.57721566490153286060651209008f /* Euler constant */ // Conversion macro -#define RAD2DEG(rad) ((rad)*(180.0f/M_PI_F)) -#define DEG2RAD(deg) ((deg)*(M_PI_F/180.0f)) +#define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F)) +#define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f)) // Useful math macros -#define MAX(a,b) ((a) > (b) ? (a) : (b)) -#define MIN(a,b) ((a) < (b) ? (a) : (b)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#define MIN(a, b) ((a) < (b) ? (a) : (b)) #endif // PIOS_MATH_H diff --git a/flight/pios/inc/pios_mpu6000.h b/flight/pios/inc/pios_mpu6000.h index 80b924331..91ea89d3e 100644 --- a/flight/pios/inc/pios_mpu6000.h +++ b/flight/pios/inc/pios_mpu6000.h @@ -13,19 +13,19 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -100,71 +100,71 @@ #define PIOS_MPU6000_PWRMGMT_STOP_CLK 0X07 enum pios_mpu6000_range { - PIOS_MPU6000_SCALE_250_DEG = 0x00, - PIOS_MPU6000_SCALE_500_DEG = 0x08, - PIOS_MPU6000_SCALE_1000_DEG = 0x10, - PIOS_MPU6000_SCALE_2000_DEG = 0x18 + PIOS_MPU6000_SCALE_250_DEG = 0x00, + PIOS_MPU6000_SCALE_500_DEG = 0x08, + PIOS_MPU6000_SCALE_1000_DEG = 0x10, + PIOS_MPU6000_SCALE_2000_DEG = 0x18 }; enum pios_mpu6000_filter { - PIOS_MPU6000_LOWPASS_256_HZ = 0x00, - PIOS_MPU6000_LOWPASS_188_HZ = 0x01, - PIOS_MPU6000_LOWPASS_98_HZ = 0x02, - PIOS_MPU6000_LOWPASS_42_HZ = 0x03, - PIOS_MPU6000_LOWPASS_20_HZ = 0x04, - PIOS_MPU6000_LOWPASS_10_HZ = 0x05, - PIOS_MPU6000_LOWPASS_5_HZ = 0x06 + PIOS_MPU6000_LOWPASS_256_HZ = 0x00, + PIOS_MPU6000_LOWPASS_188_HZ = 0x01, + PIOS_MPU6000_LOWPASS_98_HZ = 0x02, + PIOS_MPU6000_LOWPASS_42_HZ = 0x03, + PIOS_MPU6000_LOWPASS_20_HZ = 0x04, + PIOS_MPU6000_LOWPASS_10_HZ = 0x05, + PIOS_MPU6000_LOWPASS_5_HZ = 0x06 }; enum pios_mpu6000_accel_range { - PIOS_MPU6000_ACCEL_2G = 0x00, - PIOS_MPU6000_ACCEL_4G = 0x08, - PIOS_MPU6000_ACCEL_8G = 0x10, - PIOS_MPU6000_ACCEL_16G = 0x18 + PIOS_MPU6000_ACCEL_2G = 0x00, + PIOS_MPU6000_ACCEL_4G = 0x08, + PIOS_MPU6000_ACCEL_8G = 0x10, + PIOS_MPU6000_ACCEL_16G = 0x18 }; enum pios_mpu6000_orientation { // clockwise rotation from board forward - PIOS_MPU6000_TOP_0DEG = 0x00, - PIOS_MPU6000_TOP_90DEG = 0x01, - PIOS_MPU6000_TOP_180DEG = 0x02, - PIOS_MPU6000_TOP_270DEG = 0x03 + PIOS_MPU6000_TOP_0DEG = 0x00, + PIOS_MPU6000_TOP_90DEG = 0x01, + PIOS_MPU6000_TOP_180DEG = 0x02, + PIOS_MPU6000_TOP_270DEG = 0x03 }; struct pios_mpu6000_data { - int16_t gyro_x; - int16_t gyro_y; - int16_t gyro_z; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; #if defined(PIOS_MPU6000_ACCEL) - int16_t accel_x; - int16_t accel_y; - int16_t accel_z; + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; #endif /* PIOS_MPU6000_ACCEL */ - int16_t temperature; + int16_t temperature; }; struct pios_mpu6000_cfg { - const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ - - uint8_t Fifo_store; /* FIFO storage of different readings (See datasheet page 31 for more details) */ - - /* Sample rate divider to use (See datasheet page 32 for more details).*/ - uint8_t Smpl_rate_div_no_dlp; /* used when no dlp is applied (fs=8KHz)*/ - uint8_t Smpl_rate_div_dlp; /* used when dlp is on (fs=1kHz)*/ - uint8_t interrupt_cfg; /* Interrupt configuration (See datasheet page 35 for more details) */ - uint8_t interrupt_en; /* Interrupt configuration (See datasheet page 35 for more details) */ - uint8_t User_ctl; /* User control settings (See datasheet page 41 for more details) */ - uint8_t Pwr_mgmt_clk; /* Power management and clock selection (See datasheet page 32 for more details) */ - enum pios_mpu6000_accel_range accel_range; - enum pios_mpu6000_range gyro_range; - enum pios_mpu6000_filter filter; - enum pios_mpu6000_orientation orientation; + const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ + + uint8_t Fifo_store; /* FIFO storage of different readings (See datasheet page 31 for more details) */ + + /* Sample rate divider to use (See datasheet page 32 for more details).*/ + uint8_t Smpl_rate_div_no_dlp; /* used when no dlp is applied (fs=8KHz)*/ + uint8_t Smpl_rate_div_dlp; /* used when dlp is on (fs=1kHz)*/ + uint8_t interrupt_cfg; /* Interrupt configuration (See datasheet page 35 for more details) */ + uint8_t interrupt_en; /* Interrupt configuration (See datasheet page 35 for more details) */ + uint8_t User_ctl; /* User control settings (See datasheet page 41 for more details) */ + uint8_t Pwr_mgmt_clk; /* Power management and clock selection (See datasheet page 32 for more details) */ + enum pios_mpu6000_accel_range accel_range; + enum pios_mpu6000_range gyro_range; + enum pios_mpu6000_filter filter; + enum pios_mpu6000_orientation orientation; }; /* Public Functions */ -extern int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg * new_cfg); -extern int32_t PIOS_MPU6000_ConfigureRanges(enum pios_mpu6000_range gyroRange, enum pios_mpu6000_accel_range accelRange,enum pios_mpu6000_filter filterSetting); +extern int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *new_cfg); +extern int32_t PIOS_MPU6000_ConfigureRanges(enum pios_mpu6000_range gyroRange, enum pios_mpu6000_accel_range accelRange, enum pios_mpu6000_filter filterSetting); extern xQueueHandle PIOS_MPU6000_GetQueue(); -extern int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data * buffer); +extern int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *buffer); extern int32_t PIOS_MPU6000_ReadID(); extern int32_t PIOS_MPU6000_Test(); extern float PIOS_MPU6000_GetScale(); @@ -173,7 +173,7 @@ extern bool PIOS_MPU6000_IRQHandler(void); #endif /* PIOS_MPU6000_H */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_mpu6000_config.h b/flight/pios/inc/pios_mpu6000_config.h index 59e355c9a..9594d9394 100644 --- a/flight/pios/inc/pios_mpu6000_config.h +++ b/flight/pios/inc/pios_mpu6000_config.h @@ -13,45 +13,48 @@ * ****************************************************************************** */ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_MPU6000_CONFIG_H -#define PIOS_MPU6000_CONFIG_H +#define PIOS_MPU6000_CONFIG_H #include "mpu6000settings.h" #include "pios_mpu6000.h" -#define PIOS_MPU6000_CONFIG_MAP_GYROSCALE(x) (x == MPU6000SETTINGS_GYROSCALE_SCALE_250 ? PIOS_MPU6000_SCALE_250_DEG : \ - x == MPU6000SETTINGS_GYROSCALE_SCALE_500 ? PIOS_MPU6000_SCALE_500_DEG : \ - x == MPU6000SETTINGS_GYROSCALE_SCALE_1000 ? PIOS_MPU6000_SCALE_1000_DEG : \ - PIOS_MPU6000_SCALE_2000_DEG) +#define PIOS_MPU6000_CONFIG_MAP_GYROSCALE(x) \ + (x == MPU6000SETTINGS_GYROSCALE_SCALE_250 ? PIOS_MPU6000_SCALE_250_DEG : \ + x == MPU6000SETTINGS_GYROSCALE_SCALE_500 ? PIOS_MPU6000_SCALE_500_DEG : \ + x == MPU6000SETTINGS_GYROSCALE_SCALE_1000 ? PIOS_MPU6000_SCALE_1000_DEG : \ + PIOS_MPU6000_SCALE_2000_DEG) -#define PIOS_MPU6000_CONFIG_MAP_ACCELSCALE(x) (x == MPU6000SETTINGS_ACCELSCALE_SCALE_2G ? PIOS_MPU6000_ACCEL_2G : \ - x == MPU6000SETTINGS_ACCELSCALE_SCALE_4G ? PIOS_MPU6000_ACCEL_4G : \ - x == MPU6000SETTINGS_ACCELSCALE_SCALE_16G ? PIOS_MPU6000_ACCEL_16G : \ - PIOS_MPU6000_ACCEL_8G) +#define PIOS_MPU6000_CONFIG_MAP_ACCELSCALE(x) \ + (x == MPU6000SETTINGS_ACCELSCALE_SCALE_2G ? PIOS_MPU6000_ACCEL_2G : \ + x == MPU6000SETTINGS_ACCELSCALE_SCALE_4G ? PIOS_MPU6000_ACCEL_4G : \ + x == MPU6000SETTINGS_ACCELSCALE_SCALE_16G ? PIOS_MPU6000_ACCEL_16G : \ + PIOS_MPU6000_ACCEL_8G) -#define PIOS_MPU6000_CONFIG_MAP_FILTERSETTING(x) (x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_188_HZ ? PIOS_MPU6000_LOWPASS_188_HZ : \ - x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_98_HZ ? PIOS_MPU6000_LOWPASS_98_HZ : \ - x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_42_HZ ? PIOS_MPU6000_LOWPASS_42_HZ : \ - x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_20_HZ ? PIOS_MPU6000_LOWPASS_20_HZ : \ - x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_10_HZ ? PIOS_MPU6000_LOWPASS_10_HZ : \ - x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_5_HZ ? PIOS_MPU6000_LOWPASS_5_HZ : \ - PIOS_MPU6000_LOWPASS_256_HZ) +#define PIOS_MPU6000_CONFIG_MAP_FILTERSETTING(x) \ + (x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_188_HZ ? PIOS_MPU6000_LOWPASS_188_HZ : \ + x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_98_HZ ? PIOS_MPU6000_LOWPASS_98_HZ : \ + x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_42_HZ ? PIOS_MPU6000_LOWPASS_42_HZ : \ + x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_20_HZ ? PIOS_MPU6000_LOWPASS_20_HZ : \ + x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_10_HZ ? PIOS_MPU6000_LOWPASS_10_HZ : \ + x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_5_HZ ? PIOS_MPU6000_LOWPASS_5_HZ : \ + PIOS_MPU6000_LOWPASS_256_HZ) /** * @brief Updates MPU6000 config based on Mpu6000Settings UAVO * @returns 0 if succeed or -1 otherwise @@ -62,11 +65,10 @@ int32_t PIOS_MPU6000_CONFIG_Configure() Mpu6000SettingsData mpu6000settings; Mpu6000SettingsGet(&mpu6000settings); return PIOS_MPU6000_ConfigureRanges( - PIOS_MPU6000_CONFIG_MAP_GYROSCALE (mpu6000settings.GyroScale), - PIOS_MPU6000_CONFIG_MAP_ACCELSCALE(mpu6000settings.AccelScale), - PIOS_MPU6000_CONFIG_MAP_FILTERSETTING(mpu6000settings.FilterSetting) - ); + PIOS_MPU6000_CONFIG_MAP_GYROSCALE(mpu6000settings.GyroScale), + PIOS_MPU6000_CONFIG_MAP_ACCELSCALE(mpu6000settings.AccelScale), + PIOS_MPU6000_CONFIG_MAP_FILTERSETTING(mpu6000settings.FilterSetting) + ); } -#endif /* PIOS_MPU6000_CONFIG_H */ - +#endif /* PIOS_MPU6000_CONFIG_H */ diff --git a/flight/pios/inc/pios_mpxv.h b/flight/pios/inc/pios_mpxv.h index 4e7dffaba..6b60eacdf 100644 --- a/flight/pios/inc/pios_mpxv.h +++ b/flight/pios/inc/pios_mpxv.h @@ -1,66 +1,66 @@ /** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_MPXV MPXV* Functions - * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV5004, 7002 or similar - * @{ - * - * @file pios_mpxv.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief ETASV3 Airspeed Sensor Driver - * @see The GNU Public License (GPL) Version 3 - * - ******************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_MPXV MPXV* Functions + * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV5004, 7002 or similar + * @{ + * + * @file pios_mpxv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief ETASV3 Airspeed Sensor Driver + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_MPXV_H #define PIOS_MPXV_H -typedef enum{ PIOS_MPXV_UNKNOWN,PIOS_MPXV_5004,PIOS_MPXV_7002 } PIOS_MPXV_sensortype; -typedef struct{ - PIOS_MPXV_sensortype type; - uint8_t airspeedADCPin; - uint16_t calibrationCount; - uint32_t calibrationSum; - uint16_t zeroPoint; - float maxSpeed; +typedef enum { PIOS_MPXV_UNKNOWN, PIOS_MPXV_5004, PIOS_MPXV_7002 } PIOS_MPXV_sensortype; +typedef struct { + PIOS_MPXV_sensortype type; + uint8_t airspeedADCPin; + uint16_t calibrationCount; + uint32_t calibrationSum; + uint16_t zeroPoint; + float maxSpeed; } PIOS_MPXV_descriptor; #define PIOS_MPXV_5004_DESC(pin) \ - (PIOS_MPXV_descriptor){ \ - .type = PIOS_MPXV_5004, \ - .airspeedADCPin = pin, \ - .maxSpeed = 80.0f, \ - .calibrationCount = 0, \ - .calibrationSum = 0, \ - } + (PIOS_MPXV_descriptor) { \ + .type = PIOS_MPXV_5004, \ + .airspeedADCPin = pin, \ + .maxSpeed = 80.0f, \ + .calibrationCount = 0, \ + .calibrationSum = 0, \ + } #define PIOS_MPXV_7002_DESC(pin) \ - (PIOS_MPXV_descriptor){ \ - .type = PIOS_MPXV_7002, \ - .airspeedADCPin = pin, \ - .maxSpeed = 56.0f, \ - .calibrationCount = 0, \ - .calibrationSum = 0, \ - } + (PIOS_MPXV_descriptor) { \ + .type = PIOS_MPXV_7002, \ + .airspeedADCPin = pin, \ + .maxSpeed = 56.0f, \ + .calibrationCount = 0, \ + .calibrationSum = 0, \ + } uint16_t PIOS_MPXV_Measure(PIOS_MPXV_descriptor *desc); uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc, uint16_t measurement); -float PIOS_MPXV_CalcAirspeed (PIOS_MPXV_descriptor *desc,uint16_t measurement); +float PIOS_MPXV_CalcAirspeed(PIOS_MPXV_descriptor *desc, uint16_t measurement); #endif /* PIOS_MPXV_H */ diff --git a/flight/pios/inc/pios_ms5611.h b/flight/pios/inc/pios_ms5611.h index ac0a79305..307b7296c 100644 --- a/flight/pios/inc/pios_ms5611.h +++ b/flight/pios/inc/pios_ms5611.h @@ -6,25 +6,25 @@ * @brief Hardware functions to deal with the altitude pressure sensor * @{ * - * @file pios_ms5611.h + * @file pios_ms5611.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief MS5611 functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,40 +32,40 @@ #define PIOS_MS5611_H /* BMP085 Addresses */ -#define MS5611_I2C_ADDR 0x77 -#define MS5611_RESET 0x1E -#define MS5611_CALIB_ADDR 0xA2 /* First sample is factory stuff */ -#define MS5611_CALIB_LEN 16 -#define MS5611_ADC_READ 0x00 -#define MS5611_PRES_ADDR 0x40 -#define MS5611_TEMP_ADDR 0x50 -#define MS5611_ADC_MSB 0xF6 -#define MS5611_P0 101.3250f +#define MS5611_I2C_ADDR 0x77 +#define MS5611_RESET 0x1E +#define MS5611_CALIB_ADDR 0xA2 /* First sample is factory stuff */ +#define MS5611_CALIB_LEN 16 +#define MS5611_ADC_READ 0x00 +#define MS5611_PRES_ADDR 0x40 +#define MS5611_TEMP_ADDR 0x50 +#define MS5611_ADC_MSB 0xF6 +#define MS5611_P0 101.3250f /* Local Types */ typedef struct { - uint16_t C[6]; + uint16_t C[6]; } MS5611CalibDataTypeDef; typedef enum { - PressureConv, - TemperatureConv + PressureConv, + TemperatureConv } ConversionTypeTypeDef; struct pios_ms5611_cfg { - uint32_t oversampling; + uint32_t oversampling; }; enum pios_ms5611_osr { - MS5611_OSR_256 = 0, - MS5611_OSR_512 = 2, - MS5611_OSR_1024 = 4, - MS5611_OSR_2048 = 6, - MS5611_OSR_4096 = 8, + MS5611_OSR_256 = 0, + MS5611_OSR_512 = 2, + MS5611_OSR_1024 = 4, + MS5611_OSR_2048 = 6, + MS5611_OSR_4096 = 8, }; /* Public Functions */ -extern void PIOS_MS5611_Init(const struct pios_ms5611_cfg * cfg, int32_t i2c_device); +extern void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device); extern int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type); extern int32_t PIOS_MS5611_ReadADC(void); extern float PIOS_MS5611_GetTemperature(void); @@ -75,7 +75,7 @@ extern int32_t PIOS_MS5611_GetDelay(); #endif /* PIOS_MS5611_H */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_overo_priv.h b/flight/pios/inc/pios_overo_priv.h index 990a922ac..cc4b9f60c 100644 --- a/flight/pios/inc/pios_overo_priv.h +++ b/flight/pios/inc/pios_overo_priv.h @@ -36,19 +36,19 @@ extern const struct pios_com_driver pios_overo_com_driver; struct pios_overo_cfg { - SPI_TypeDef *regs; - uint32_t remap; /* GPIO_Remap_* or GPIO_AF_* */ - SPI_InitTypeDef init; - bool use_crc; - struct stm32_dma dma; - struct stm32_gpio sclk; - struct stm32_gpio miso; - struct stm32_gpio mosi; - uint32_t slave_count; - struct stm32_gpio ssel[]; + SPI_TypeDef *regs; + uint32_t remap; /* GPIO_Remap_* or GPIO_AF_* */ + SPI_InitTypeDef init; + bool use_crc; + struct stm32_dma dma; + struct stm32_gpio sclk; + struct stm32_gpio miso; + struct stm32_gpio mosi; + uint32_t slave_count; + struct stm32_gpio ssel[]; }; -extern int32_t PIOS_OVERO_Init(uint32_t * overo_id, const struct pios_overo_cfg * cfg); +extern int32_t PIOS_OVERO_Init(uint32_t *overo_id, const struct pios_overo_cfg *cfg); #endif /* PIOS_OVERO_H */ diff --git a/flight/pios/inc/pios_posix.h b/flight/pios/inc/pios_posix.h index 7b6fe7fb1..74483b404 100644 --- a/flight/pios/inc/pios_posix.h +++ b/flight/pios/inc/pios_posix.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file posix.h + * @file posix.h * @author Corvus Corax Copyright (C) 2010. * @brief Definitions to run PiOS on posix * @see The GNU Public License (GPL) Version 2 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -28,16 +28,16 @@ #include -typedef enum {FALSE = 0, TRUE = !FALSE} bool; +typedef enum { FALSE = 0, TRUE = !FALSE } bool; #ifndef false - #define false FALSE - #define true TRUE + #define false FALSE + #define true TRUE #endif -//#define FILEINFO FILE* +// #define FILEINFO FILE* -//#define PIOS_SERVO_NUM_OUTPUTS 8 -//#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS +// #define PIOS_SERVO_NUM_OUTPUTS 8 +// #define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS #endif /* PIOS_POSIX_H */ diff --git a/flight/pios/inc/pios_ppm_out_priv.h b/flight/pios/inc/pios_ppm_out_priv.h index 7ecc98ffc..013599765 100644 --- a/flight/pios/inc/pios_ppm_out_priv.h +++ b/flight/pios/inc/pios_ppm_out_priv.h @@ -34,11 +34,11 @@ #include struct pios_ppm_out_cfg { - TIM_OCInitTypeDef tim_oc_init; - const struct pios_tim_channel * channel; + TIM_OCInitTypeDef tim_oc_init; + const struct pios_tim_channel *channel; }; -extern int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg * cfg); +extern int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg *cfg); extern void PIOS_PPM_OUT_Set(uint32_t ppm_out_id, uint8_t servo, uint16_t position); #endif /* PIOS_PPM_PRIV_H */ diff --git a/flight/pios/inc/pios_ppm_priv.h b/flight/pios/inc/pios_ppm_priv.h index 74d3aecfa..66ccd6eb7 100644 --- a/flight/pios/inc/pios_ppm_priv.h +++ b/flight/pios/inc/pios_ppm_priv.h @@ -35,14 +35,14 @@ #include struct pios_ppm_cfg { - TIM_ICInitTypeDef tim_ic_init; - const struct pios_tim_channel * channels; - uint8_t num_channels; + TIM_ICInitTypeDef tim_ic_init; + const struct pios_tim_channel *channels; + uint8_t num_channels; }; extern const struct pios_rcvr_driver pios_ppm_rcvr_driver; -extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg); +extern int32_t PIOS_PPM_Init(uint32_t *ppm_id, const struct pios_ppm_cfg *cfg); #endif /* PIOS_PPM_PRIV_H */ diff --git a/flight/pios/inc/pios_pwm_priv.h b/flight/pios/inc/pios_pwm_priv.h index fe11b3d60..fd6a253bd 100644 --- a/flight/pios/inc/pios_pwm_priv.h +++ b/flight/pios/inc/pios_pwm_priv.h @@ -37,14 +37,14 @@ #include struct pios_pwm_cfg { - TIM_ICInitTypeDef tim_ic_init; - const struct pios_tim_channel * channels; - uint8_t num_channels; + TIM_ICInitTypeDef tim_ic_init; + const struct pios_tim_channel *channels; + uint8_t num_channels; }; extern const struct pios_rcvr_driver pios_pwm_rcvr_driver; -extern int32_t PIOS_PWM_Init(uint32_t * pwm_id, const struct pios_pwm_cfg * cfg); +extern int32_t PIOS_PWM_Init(uint32_t *pwm_id, const struct pios_pwm_cfg *cfg); #endif /* PIOS_PWM_PRIV_H */ diff --git a/flight/pios/inc/pios_rcvr.h b/flight/pios/inc/pios_rcvr.h index 6b74c9b52..450e04628 100644 --- a/flight/pios/inc/pios_rcvr.h +++ b/flight/pios/inc/pios_rcvr.h @@ -6,25 +6,25 @@ * @brief Hardware communication layer * @{ * - * @file pios_rcvr.h + * @file pios_rcvr.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RCVR layer functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,8 +32,8 @@ #define PIOS_RCVR_H struct pios_rcvr_driver { - void (*init)(uint32_t id); - int32_t (*read)(uint32_t id, uint8_t channel); + void (*init)(uint32_t id); + int32_t (*read)(uint32_t id, uint8_t channel); }; /* Public Functions */ @@ -41,17 +41,17 @@ extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel); /*! Define error codes for PIOS_RCVR_Get */ enum PIOS_RCVR_errors { - /*! Indicates that a failsafe condition or missing receiver detected for that channel */ - PIOS_RCVR_TIMEOUT = -1, - /*! Channel is invalid for this driver (usually out of range supported) */ - PIOS_RCVR_INVALID = -2, - /*! Indicates that the driver for this channel has not been initialized */ - PIOS_RCVR_NODRIVER = -3 + /*! Indicates that a failsafe condition or missing receiver detected for that channel */ + PIOS_RCVR_TIMEOUT = -1, + /*! Channel is invalid for this driver (usually out of range supported) */ + PIOS_RCVR_INVALID = -2, + /*! Indicates that the driver for this channel has not been initialized */ + PIOS_RCVR_NODRIVER = -3 }; #endif /* PIOS_RCVR_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_rcvr_priv.h b/flight/pios/inc/pios_rcvr_priv.h index 46ced2e24..9f8193399 100644 --- a/flight/pios/inc/pios_rcvr_priv.h +++ b/flight/pios/inc/pios_rcvr_priv.h @@ -33,13 +33,13 @@ #include -extern int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id); +extern int32_t PIOS_RCVR_Init(uint32_t *rcvr_id, const struct pios_rcvr_driver *driver, const uint32_t lower_id); extern void PIOS_RCVR_IRQ_Handler(uint32_t rcvr_id); #endif /* PIOS_RCVR_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_rfm22b.h b/flight/pios/inc/pios_rfm22b.h index b1ce7a24b..0763a6ae1 100644 --- a/flight/pios/inc/pios_rfm22b.h +++ b/flight/pios/inc/pios_rfm22b.h @@ -1,30 +1,30 @@ /** -****************************************************************************** -* @addtogroup PIOS PIOS Core hardware abstraction layer -* @{ -* @addtogroup PIOS_RFM22B Radio Functions -* @brief PIOS interface for RFM22B Radio -* @{ -* -* @file pios_rfm22b.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief RFM22B functions header. -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RFM22B Radio Functions + * @brief PIOS interface for RFM22B Radio + * @{ + * + * @file pios_rfm22b.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief RFM22B functions header. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,41 +35,41 @@ #include /* Constant definitions */ -enum gpio_direction {GPIO0_TX_GPIO1_RX, GPIO0_RX_GPIO1_TX}; +enum gpio_direction { GPIO0_TX_GPIO1_RX, GPIO0_RX_GPIO1_TX }; /* Global Types */ struct pios_rfm22b_cfg { - const struct pios_spi_cfg * spi_cfg; /* Pointer to SPI interface configuration */ - const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ + const struct pios_spi_cfg *spi_cfg; /* Pointer to SPI interface configuration */ + const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ uint8_t RFXtalCap; uint8_t slave_num; enum gpio_direction gpio_direction; }; enum rfm22b_tx_power { - RFM22_tx_pwr_txpow_0 = 0x00, // +1dBm .. 1.25mW - RFM22_tx_pwr_txpow_1 = 0x01, // +2dBm .. 1.6mW - RFM22_tx_pwr_txpow_2 = 0x02, // +5dBm .. 3.16mW - RFM22_tx_pwr_txpow_3 = 0x03, // +8dBm .. 6.3mW - RFM22_tx_pwr_txpow_4 = 0x04, // +11dBm .. 12.6mW - RFM22_tx_pwr_txpow_5 = 0x05, // +14dBm .. 25mW - RFM22_tx_pwr_txpow_6 = 0x06, // +17dBm .. 50mW - RFM22_tx_pwr_txpow_7 = 0x07 // +20dBm .. 100mW + RFM22_tx_pwr_txpow_0 = 0x00, // +1dBm .. 1.25mW + RFM22_tx_pwr_txpow_1 = 0x01, // +2dBm .. 1.6mW + RFM22_tx_pwr_txpow_2 = 0x02, // +5dBm .. 3.16mW + RFM22_tx_pwr_txpow_3 = 0x03, // +8dBm .. 6.3mW + RFM22_tx_pwr_txpow_4 = 0x04, // +11dBm .. 12.6mW + RFM22_tx_pwr_txpow_5 = 0x05, // +14dBm .. 25mW + RFM22_tx_pwr_txpow_6 = 0x06, // +17dBm .. 50mW + RFM22_tx_pwr_txpow_7 = 0x07 // +20dBm .. 100mW }; enum rfm22b_datarate { - RFM22_datarate_500 = 0, - RFM22_datarate_1000 = 1, - RFM22_datarate_2000 = 2, - RFM22_datarate_4000 = 3, - RFM22_datarate_8000 = 4, - RFM22_datarate_9600 = 5, - RFM22_datarate_16000 = 6, - RFM22_datarate_19200 = 7, - RFM22_datarate_24000 = 8, - RFM22_datarate_32000 = 9, - RFM22_datarate_57600 = 10, - RFM22_datarate_64000 = 11, + RFM22_datarate_500 = 0, + RFM22_datarate_1000 = 1, + RFM22_datarate_2000 = 2, + RFM22_datarate_4000 = 3, + RFM22_datarate_8000 = 4, + RFM22_datarate_9600 = 5, + RFM22_datarate_16000 = 6, + RFM22_datarate_19200 = 7, + RFM22_datarate_24000 = 8, + RFM22_datarate_32000 = 9, + RFM22_datarate_57600 = 10, + RFM22_datarate_64000 = 11, RFM22_datarate_128000 = 12, RFM22_datarate_192000 = 13, RFM22_datarate_256000 = 14, @@ -88,26 +88,26 @@ struct rfm22b_stats { uint16_t rx_byte_count; uint16_t tx_seq; uint16_t rx_seq; - uint8_t rx_good; - uint8_t rx_corrected; - uint8_t rx_error; - uint8_t rx_missed; - uint8_t rx_failure; - uint8_t tx_dropped; - uint8_t tx_resent; - uint8_t tx_failure; - uint8_t resets; - uint8_t timeouts; - uint8_t link_quality; - int8_t rssi; - int8_t afc_correction; - uint8_t link_state; + uint8_t rx_good; + uint8_t rx_corrected; + uint8_t rx_error; + uint8_t rx_missed; + uint8_t rx_failure; + uint8_t tx_dropped; + uint8_t tx_resent; + uint8_t tx_failure; + uint8_t resets; + uint8_t timeouts; + uint8_t link_quality; + int8_t rssi; + int8_t afc_correction; + uint8_t link_state; }; /* Callback function prototypes */ typedef void (*PIOS_RFM22B_ComConfigCallback)(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, - uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); /* Public Functions */ extern int32_t PIOS_RFM22B_Init(uint32_t *rfb22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg); @@ -118,7 +118,7 @@ extern void PIOS_RFM22B_SetInitialFrequency(uint32_t rfm22b_id, uint32_t init_fr extern void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id); extern void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigCallback cb); extern void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindingPairIDs[], const uint8_t mainPortSettings[], - const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]); + const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); extern bool PIOS_RFM22B_IsCoordinator(uint32_t rfb22b_id); extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats); diff --git a/flight/pios/inc/pios_rfm22b_com.h b/flight/pios/inc/pios_rfm22b_com.h index f48c0af90..f461809a1 100644 --- a/flight/pios/inc/pios_rfm22b_com.h +++ b/flight/pios/inc/pios_rfm22b_com.h @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,6 +36,6 @@ extern const struct pios_com_driver pios_rfm22b_com_driver; #endif /* PIOS_RFM22B_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_rfm22b_priv.h b/flight/pios/inc/pios_rfm22b_priv.h index 7cecb0a36..467ace794 100644 --- a/flight/pios/inc/pios_rfm22b_priv.h +++ b/flight/pios/inc/pios_rfm22b_priv.h @@ -1,17 +1,17 @@ /** -****************************************************************************** -* @addtogroup PIOS PIOS Core hardware abstraction layer -* @{ -* @addtogroup PIOS_RFM22B Radio Functions -* @brief PIOS interface for RFM22B Radio -* @{ -* -* @file pios_rfm22b_priv.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief RFM22B private definitions. -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RFM22B Radio Functions + * @brief PIOS interface for RFM22B Radio + * @{ + * + * @file pios_rfm22b_priv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief RFM22B private definitions. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -40,493 +40,493 @@ // ************************************ -#define RFM22_DEVICE_VERSION_V2 0x02 -#define RFM22_DEVICE_VERSION_A0 0x04 -#define RFM22_DEVICE_VERSION_B1 0x06 +#define RFM22_DEVICE_VERSION_V2 0x02 +#define RFM22_DEVICE_VERSION_A0 0x04 +#define RFM22_DEVICE_VERSION_B1 0x06 // ************************************ -#define RFM22_MIN_CARRIER_FREQUENCY_HZ 240000000ul -#define RFM22_MAX_CARRIER_FREQUENCY_HZ 930000000ul +#define RFM22_MIN_CARRIER_FREQUENCY_HZ 240000000ul +#define RFM22_MAX_CARRIER_FREQUENCY_HZ 930000000ul // ************************************ -#define BIT0 (1u << 0) -#define BIT1 (1u << 1) -#define BIT2 (1u << 2) -#define BIT3 (1u << 3) -#define BIT4 (1u << 4) -#define BIT5 (1u << 5) -#define BIT6 (1u << 6) -#define BIT7 (1u << 7) +#define BIT0 (1u << 0) +#define BIT1 (1u << 1) +#define BIT2 (1u << 2) +#define BIT3 (1u << 3) +#define BIT4 (1u << 4) +#define BIT5 (1u << 5) +#define BIT6 (1u << 6) +#define BIT7 (1u << 7) // ************************************ -#define RFM22_DEVICE_TYPE 0x00 // R -#define RFM22_DT_MASK 0x1F +#define RFM22_DEVICE_TYPE 0x00 // R +#define RFM22_DT_MASK 0x1F -#define RFM22_DEVICE_VERSION 0x01 // R -#define RFM22_DV_MASK 0x1F +#define RFM22_DEVICE_VERSION 0x01 // R +#define RFM22_DV_MASK 0x1F -#define RFM22_device_status 0x02 // R -#define RFM22_ds_cps_mask 0x03 // Chip Power State mask -#define RFM22_ds_cps_idle 0x00 // IDLE Chip Power State -#define RFM22_ds_cps_rx 0x01 // RX Chip Power State -#define RFM22_ds_cps_tx 0x02 // TX Chip Power State -//#define RFM22_ds_lockdet 0x04 // -//#define RFM22_ds_freqerr 0x08 // -#define RFM22_ds_headerr 0x10 // Header Error Status. Indicates if the received packet has a header check error -#define RFM22_ds_rxffem 0x20 // RX FIFO Empty Status -#define RFM22_ds_ffunfl 0x40 // RX/TX FIFO Underflow Status -#define RFM22_ds_ffovfl 0x80 // RX/TX FIFO Overflow Status +#define RFM22_device_status 0x02 // R +#define RFM22_ds_cps_mask 0x03 // Chip Power State mask +#define RFM22_ds_cps_idle 0x00 // IDLE Chip Power State +#define RFM22_ds_cps_rx 0x01 // RX Chip Power State +#define RFM22_ds_cps_tx 0x02 // TX Chip Power State +// #define RFM22_ds_lockdet 0x04 // +// #define RFM22_ds_freqerr 0x08 // +#define RFM22_ds_headerr 0x10 // Header Error Status. Indicates if the received packet has a header check error +#define RFM22_ds_rxffem 0x20 // RX FIFO Empty Status +#define RFM22_ds_ffunfl 0x40 // RX/TX FIFO Underflow Status +#define RFM22_ds_ffovfl 0x80 // RX/TX FIFO Overflow Status -#define RFM22_interrupt_status1 0x03 // R -#define RFM22_is1_icrerror BIT0 // CRC Error. When set to 1 the cyclic redundancy check is failed. -#define RFM22_is1_ipkvalid BIT1 // Valid Packet Received.When set to 1 a valid packet has been received. -#define RFM22_is1_ipksent BIT2 // Packet Sent Interrupt. When set to1 a valid packet has been transmitted. -#define RFM22_is1_iext BIT3 // External Interrupt. When set to 1 an interrupt occurred on one of the GPIO�s if it is programmed so. The status can be checked in register 0Eh. See GPIOx Configuration section for the details. -#define RFM22_is1_irxffafull BIT4 // RX FIFO Almost Full.When set to 1 the RX FIFO has met its almost full threshold and needs to be read by the microcontroller. -#define RFM22_is1_ixtffaem BIT5 // TX FIFO Almost Empty. When set to 1 the TX FIFO is almost empty and needs to be filled. -#define RFM22_is1_itxffafull BIT6 // TX FIFO Almost Full. When set to 1 the TX FIFO has met its almost full threshold and needs to be transmitted. -#define RFM22_is1_ifferr BIT7 // FIFO Underflow/Overflow Error. When set to 1 the TX or RX FIFO has overflowed or underflowed. +#define RFM22_interrupt_status1 0x03 // R +#define RFM22_is1_icrerror BIT0 // CRC Error. When set to 1 the cyclic redundancy check is failed. +#define RFM22_is1_ipkvalid BIT1 // Valid Packet Received.When set to 1 a valid packet has been received. +#define RFM22_is1_ipksent BIT2 // Packet Sent Interrupt. When set to1 a valid packet has been transmitted. +#define RFM22_is1_iext BIT3 // External Interrupt. When set to 1 an interrupt occurred on one of the GPIO�s if it is programmed so. The status can be checked in register 0Eh. See GPIOx Configuration section for the details. +#define RFM22_is1_irxffafull BIT4 // RX FIFO Almost Full.When set to 1 the RX FIFO has met its almost full threshold and needs to be read by the microcontroller. +#define RFM22_is1_ixtffaem BIT5 // TX FIFO Almost Empty. When set to 1 the TX FIFO is almost empty and needs to be filled. +#define RFM22_is1_itxffafull BIT6 // TX FIFO Almost Full. When set to 1 the TX FIFO has met its almost full threshold and needs to be transmitted. +#define RFM22_is1_ifferr BIT7 // FIFO Underflow/Overflow Error. When set to 1 the TX or RX FIFO has overflowed or underflowed. -#define RFM22_interrupt_status2 0x04 // R -#define RFM22_is2_ipor BIT0 // Power-on-Reset (POR). When the chip detects a Power on Reset above the desired setting this bit will be set to 1. -#define RFM22_is2_ichiprdy BIT1 // Chip Ready (XTAL). When a chip ready event has been detected this bit will be set to 1. -#define RFM22_is2_ilbd BIT2 // Low Battery Detect. When a low battery event is been detected this bit will be set to 1. This interrupt event is saved even if it is not enabled by the mask register bit and causes an interrupt after it is enabled. -#define RFM22_is2_iwut BIT3 // Wake-Up-Timer. On the expiration of programmed wake-up timer this bit will be set to 1. -#define RFM22_is2_irssi BIT4 // RSSI. When RSSI level exceeds the programmed threshold this bit will be set to 1. -#define RFM22_is2_ipreainval BIT5 // Invalid Preamble Detected. When the preamble is not found within a period of time set by the invalid preamble detection threshold in Register 54h, this bit will be set to 1. -#define RFM22_is2_ipreaval BIT6 // Valid Preamble Detected. When a preamble is detected this bit will be set to 1. -#define RFM22_is2_iswdet BIT7 // Sync Word Detected. When a sync word is detected this bit will be set to 1. +#define RFM22_interrupt_status2 0x04 // R +#define RFM22_is2_ipor BIT0 // Power-on-Reset (POR). When the chip detects a Power on Reset above the desired setting this bit will be set to 1. +#define RFM22_is2_ichiprdy BIT1 // Chip Ready (XTAL). When a chip ready event has been detected this bit will be set to 1. +#define RFM22_is2_ilbd BIT2 // Low Battery Detect. When a low battery event is been detected this bit will be set to 1. This interrupt event is saved even if it is not enabled by the mask register bit and causes an interrupt after it is enabled. +#define RFM22_is2_iwut BIT3 // Wake-Up-Timer. On the expiration of programmed wake-up timer this bit will be set to 1. +#define RFM22_is2_irssi BIT4 // RSSI. When RSSI level exceeds the programmed threshold this bit will be set to 1. +#define RFM22_is2_ipreainval BIT5 // Invalid Preamble Detected. When the preamble is not found within a period of time set by the invalid preamble detection threshold in Register 54h, this bit will be set to 1. +#define RFM22_is2_ipreaval BIT6 // Valid Preamble Detected. When a preamble is detected this bit will be set to 1. +#define RFM22_is2_iswdet BIT7 // Sync Word Detected. When a sync word is detected this bit will be set to 1. -#define RFM22_interrupt_enable1 0x05 // R/W -#define RFM22_ie1_encrcerror BIT0 // Enable CRC Error. When set to 1 the CRC Error interrupt will be enabled. -#define RFM22_ie1_enpkvalid BIT1 // Enable Valid Packet Received. When ipkvalid = 1 the Valid Packet Received Interrupt will be enabled. -#define RFM22_ie1_enpksent BIT2 // Enable Packet Sent. When ipksent =1 the Packet Sense Interrupt will be enabled. -#define RFM22_ie1_enext BIT3 // Enable External Interrupt. When set to 1 the External Interrupt will be enabled. -#define RFM22_ie1_enrxffafull BIT4 // Enable RX FIFO Almost Full. When set to 1 the RX FIFO Almost Full interrupt will be enabled. -#define RFM22_ie1_entxffaem BIT5 // Enable TX FIFO Almost Empty. When set to 1 the TX FIFO Almost Empty interrupt will be enabled. -#define RFM22_ie1_entxffafull BIT6 // Enable TX FIFO Almost Full. When set to 1 the TX FIFO Almost Full interrupt will be enabled. -#define RFM22_ie1_enfferr BIT7 // Enable FIFO Underflow/Overflow. When set to 1 the FIFO Underflow/Overflow interrupt will be enabled. +#define RFM22_interrupt_enable1 0x05 // R/W +#define RFM22_ie1_encrcerror BIT0 // Enable CRC Error. When set to 1 the CRC Error interrupt will be enabled. +#define RFM22_ie1_enpkvalid BIT1 // Enable Valid Packet Received. When ipkvalid = 1 the Valid Packet Received Interrupt will be enabled. +#define RFM22_ie1_enpksent BIT2 // Enable Packet Sent. When ipksent =1 the Packet Sense Interrupt will be enabled. +#define RFM22_ie1_enext BIT3 // Enable External Interrupt. When set to 1 the External Interrupt will be enabled. +#define RFM22_ie1_enrxffafull BIT4 // Enable RX FIFO Almost Full. When set to 1 the RX FIFO Almost Full interrupt will be enabled. +#define RFM22_ie1_entxffaem BIT5 // Enable TX FIFO Almost Empty. When set to 1 the TX FIFO Almost Empty interrupt will be enabled. +#define RFM22_ie1_entxffafull BIT6 // Enable TX FIFO Almost Full. When set to 1 the TX FIFO Almost Full interrupt will be enabled. +#define RFM22_ie1_enfferr BIT7 // Enable FIFO Underflow/Overflow. When set to 1 the FIFO Underflow/Overflow interrupt will be enabled. -#define RFM22_interrupt_enable2 0x06 // R/W -#define RFM22_ie2_enpor BIT0 // Enable POR. When set to 1 the POR interrupt will be enabled. -#define RFM22_ie2_enchiprdy BIT1 // Enable Chip Ready (XTAL). When set to 1 the Chip Ready interrupt will be enabled. -#define RFM22_ie2_enlbd BIT2 // Enable Low Battery Detect. When set to 1 the Low Battery Detect interrupt will be enabled. -#define RFM22_ie2_enwut BIT3 // Enable Wake-Up Timer. When set to 1 the Wake-Up Timer interrupt will be enabled. -#define RFM22_ie2_enrssi BIT4 // Enable RSSI. When set to 1 the RSSI Interrupt will be enabled. -#define RFM22_ie2_enpreainval BIT5 // Enable Invalid Preamble Detected. When mpreadet =1 the Invalid Preamble Detected Interrupt will be enabled. -#define RFM22_ie2_enpreaval BIT6 // Enable Valid Preamble Detected. When mpreadet =1 the Valid Preamble Detected Interrupt will be enabled. -#define RFM22_ie2_enswdet BIT7 // Enable Sync Word Detected. When mpreadet =1 the Preamble Detected Interrupt will be enabled. +#define RFM22_interrupt_enable2 0x06 // R/W +#define RFM22_ie2_enpor BIT0 // Enable POR. When set to 1 the POR interrupt will be enabled. +#define RFM22_ie2_enchiprdy BIT1 // Enable Chip Ready (XTAL). When set to 1 the Chip Ready interrupt will be enabled. +#define RFM22_ie2_enlbd BIT2 // Enable Low Battery Detect. When set to 1 the Low Battery Detect interrupt will be enabled. +#define RFM22_ie2_enwut BIT3 // Enable Wake-Up Timer. When set to 1 the Wake-Up Timer interrupt will be enabled. +#define RFM22_ie2_enrssi BIT4 // Enable RSSI. When set to 1 the RSSI Interrupt will be enabled. +#define RFM22_ie2_enpreainval BIT5 // Enable Invalid Preamble Detected. When mpreadet =1 the Invalid Preamble Detected Interrupt will be enabled. +#define RFM22_ie2_enpreaval BIT6 // Enable Valid Preamble Detected. When mpreadet =1 the Valid Preamble Detected Interrupt will be enabled. +#define RFM22_ie2_enswdet BIT7 // Enable Sync Word Detected. When mpreadet =1 the Preamble Detected Interrupt will be enabled. -#define RFM22_op_and_func_ctrl1 0x07 // R/W -#define RFM22_opfc1_xton 0x01 // READY Mode (Xtal is ON). -#define RFM22_opfc1_pllon 0x02 // TUNE Mode (PLL is ON). When pllon = 1 the PLL will remain enabled in Idle State. This will for faster turn-around time at the cost of increased current consumption in Idle State. -#define RFM22_opfc1_rxon 0x04 // RX on in Manual Receiver Mode. Automatically cleared if Multiple Packets config. is disabled and a valid packet received. -#define RFM22_opfc1_txon 0x08 // TX on in Manual Transmit Mode. Automatically cleared in FIFO mode once the packet is sent. Transmission can be aborted during packet transmission, however, when no data has been sent yet, transmission can only be aborted after the device is programmed to �unmodulated carrier� ("Register 71h. Modulation Mode Control 2"). -#define RFM22_opfc1_x32ksel 0x10 // 32,768 kHz Crystal Oscillator Select. 0: RC oscillator 1: 32 kHz crystal -#define RFM22_opfc1_enwt 0x20 // Enable Wake-Up-Timer. Enabled when enwt = 1. If the Wake-up-Timer function is enabled it will operate in any mode and notify the microcontroller through the GPIO interrupt when the timer expires. -#define RFM22_opfc1_enlbd 0x40 // Enable Low Battery Detect. When this bit is set to 1 the Low Battery Detector circuit and threshold comparison will be enabled. -#define RFM22_opfc1_swres 0x80 // Software Register Reset Bit. This bit may be used to reset all registers simultaneously to a DEFAULT state, without the need for sequentially writing to each individual register. The RESET is accomplished by setting swres = 1. This bit will be automatically cleared. +#define RFM22_op_and_func_ctrl1 0x07 // R/W +#define RFM22_opfc1_xton 0x01 // READY Mode (Xtal is ON). +#define RFM22_opfc1_pllon 0x02 // TUNE Mode (PLL is ON). When pllon = 1 the PLL will remain enabled in Idle State. This will for faster turn-around time at the cost of increased current consumption in Idle State. +#define RFM22_opfc1_rxon 0x04 // RX on in Manual Receiver Mode. Automatically cleared if Multiple Packets config. is disabled and a valid packet received. +#define RFM22_opfc1_txon 0x08 // TX on in Manual Transmit Mode. Automatically cleared in FIFO mode once the packet is sent. Transmission can be aborted during packet transmission, however, when no data has been sent yet, transmission can only be aborted after the device is programmed to �unmodulated carrier� ("Register 71h. Modulation Mode Control 2"). +#define RFM22_opfc1_x32ksel 0x10 // 32,768 kHz Crystal Oscillator Select. 0: RC oscillator 1: 32 kHz crystal +#define RFM22_opfc1_enwt 0x20 // Enable Wake-Up-Timer. Enabled when enwt = 1. If the Wake-up-Timer function is enabled it will operate in any mode and notify the microcontroller through the GPIO interrupt when the timer expires. +#define RFM22_opfc1_enlbd 0x40 // Enable Low Battery Detect. When this bit is set to 1 the Low Battery Detector circuit and threshold comparison will be enabled. +#define RFM22_opfc1_swres 0x80 // Software Register Reset Bit. This bit may be used to reset all registers simultaneously to a DEFAULT state, without the need for sequentially writing to each individual register. The RESET is accomplished by setting swres = 1. This bit will be automatically cleared. -#define RFM22_op_and_func_ctrl2 0x08 // R/W -#define RFM22_opfc2_ffclrtx 0x01 // TX FIFO Reset/Clear. This has to be a two writes operation: Setting ffclrtx =1 followed by ffclrtx = 0 will clear the contents of the TX FIFO. -#define RFM22_opfc2_ffclrrx 0x02 // RX FIFO Reset/Clear. This has to be a two writes operation: Setting ffclrrx =1 followed by ffclrrx = 0 will clear the contents of the RX FIFO. -#define RFM22_opfc2_enldm 0x04 // Enable Low Duty Cycle Mode. If this bit is set to 1 then the chip turns on the RX regularly. The frequency should be set in the Wake-Up Timer Period register, while the minimum ON time should be set in the Low-Duty Cycle Mode Duration register. The FIFO mode should be enabled also. -#define RFM22_opfc2_autotx 0x08 // Automatic Transmission. When autotx = 1 the transceiver will enter automatically TX State when the FIFO is almost full. When the FIFO is empty it will automatically return to the Idle State. -#define RFM22_opfc2_rxmpk 0x10 // RX Multi Packet. When the chip is selected to use FIFO Mode (dtmod[1:0]) and RX Packet Handling (enpacrx) then it will fill up the FIFO with multiple valid packets if this bit is set, otherwise the transceiver will automatically leave the RX State after the first valid packet has been received. -#define RFM22_opfc2_antdiv_mask 0xE0 // Enable Antenna Diversity. The GPIO must be configured for Antenna Diversity for the algorithm to work properly. +#define RFM22_op_and_func_ctrl2 0x08 // R/W +#define RFM22_opfc2_ffclrtx 0x01 // TX FIFO Reset/Clear. This has to be a two writes operation: Setting ffclrtx =1 followed by ffclrtx = 0 will clear the contents of the TX FIFO. +#define RFM22_opfc2_ffclrrx 0x02 // RX FIFO Reset/Clear. This has to be a two writes operation: Setting ffclrrx =1 followed by ffclrrx = 0 will clear the contents of the RX FIFO. +#define RFM22_opfc2_enldm 0x04 // Enable Low Duty Cycle Mode. If this bit is set to 1 then the chip turns on the RX regularly. The frequency should be set in the Wake-Up Timer Period register, while the minimum ON time should be set in the Low-Duty Cycle Mode Duration register. The FIFO mode should be enabled also. +#define RFM22_opfc2_autotx 0x08 // Automatic Transmission. When autotx = 1 the transceiver will enter automatically TX State when the FIFO is almost full. When the FIFO is empty it will automatically return to the Idle State. +#define RFM22_opfc2_rxmpk 0x10 // RX Multi Packet. When the chip is selected to use FIFO Mode (dtmod[1:0]) and RX Packet Handling (enpacrx) then it will fill up the FIFO with multiple valid packets if this bit is set, otherwise the transceiver will automatically leave the RX State after the first valid packet has been received. +#define RFM22_opfc2_antdiv_mask 0xE0 // Enable Antenna Diversity. The GPIO must be configured for Antenna Diversity for the algorithm to work properly. -#define RFM22_xtal_osc_load_cap 0x09 // R/W -#define RFM22_xolc_xlc_mask 0x7F // Tuning Capacitance for the 30 MHz XTAL. -#define RFM22_xolc_xtalshft 0x80 // Additional capacitance to course shift the frequency if xlc[6:0] is not sufficient. Not binary with xlc[6:0]. +#define RFM22_xtal_osc_load_cap 0x09 // R/W +#define RFM22_xolc_xlc_mask 0x7F // Tuning Capacitance for the 30 MHz XTAL. +#define RFM22_xolc_xtalshft 0x80 // Additional capacitance to course shift the frequency if xlc[6:0] is not sufficient. Not binary with xlc[6:0]. -#define RFM22_cpu_output_clk 0x0A // R/W -#define RFM22_coc_30MHz 0x00 -#define RFM22_coc_15MHz 0x01 -#define RFM22_coc_10MHz 0x02 -#define RFM22_coc_4MHz 0x03 -#define RFM22_coc_3MHz 0x04 -#define RFM22_coc_2MHz 0x05 -#define RFM22_coc_1MHz 0x06 -#define RFM22_coc_32768Hz 0x07 -#define RFM22_coc_enlfc 0x08 -#define RFM22_coc_0cycle 0x00 -#define RFM22_coc_128cycles 0x10 -#define RFM22_coc_256cycles 0x20 -#define RFM22_coc_512cycles 0x30 +#define RFM22_cpu_output_clk 0x0A // R/W +#define RFM22_coc_30MHz 0x00 +#define RFM22_coc_15MHz 0x01 +#define RFM22_coc_10MHz 0x02 +#define RFM22_coc_4MHz 0x03 +#define RFM22_coc_3MHz 0x04 +#define RFM22_coc_2MHz 0x05 +#define RFM22_coc_1MHz 0x06 +#define RFM22_coc_32768Hz 0x07 +#define RFM22_coc_enlfc 0x08 +#define RFM22_coc_0cycle 0x00 +#define RFM22_coc_128cycles 0x10 +#define RFM22_coc_256cycles 0x20 +#define RFM22_coc_512cycles 0x30 -#define RFM22_gpio0_config 0x0B // R/W -#define RFM22_gpio0_config_por 0x00 // Power-On-Reset (output) -#define RFM22_gpio0_config_wut 0x01 // Wake-Up Timer: 1 when WUT has expired (output) -#define RFM22_gpio0_config_lbd 0x02 // Low Battery Detect: 1 when battery is below threshold setting (output) -#define RFM22_gpio0_config_ddi 0x03 // Direct Digital Input -#define RFM22_gpio0_config_eife 0x04 // External Interrupt, falling edge (input) -#define RFM22_gpio0_config_eire 0x05 // External Interrupt, rising edge (input) -#define RFM22_gpio0_config_eisc 0x06 // External Interrupt, state change (input) -#define RFM22_gpio0_config_ai 0x07 // ADC Analog Input -#define RFM22_gpio0_config_atni 0x08 // Reserved (Analog Test N Input) -#define RFM22_gpio0_config_atpi 0x09 // Reserved (Analog Test P Input) -#define RFM22_gpio0_config_ddo 0x0A // Direct Digital Output -#define RFM22_gpio0_config_dto 0x0B // Reserved (Digital Test Output) -#define RFM22_gpio0_config_atno 0x0C // Reserved (Analog Test N Output) -#define RFM22_gpio0_config_atpo 0x0D // Reserved (Analog Test P Output) -#define RFM22_gpio0_config_rv 0xOE // Reference Voltage (output) -#define RFM22_gpio0_config_dclk 0x0F // TX/RX Data CLK output to be used in conjunction with TX/RX Data pin (output) -#define RFM22_gpio0_config_txd 0x10 // TX Data input for direct modulation (input) -#define RFM22_gpio0_config_err 0x11 // External Retransmission Request (input) -#define RFM22_gpio0_config_txstate 0x12 // TX State (output) -#define RFM22_gpio0_config_txfifoaf 0x13 // TX FIFO Almost Full (output) -#define RFM22_gpio0_config_rxd 0x14 // RX Data (output) -#define RFM22_gpio0_config_rxstate 0x15 // RX State (output) -#define RFM22_gpio0_config_rxfifoaf 0x16 // RX FIFO Almost Full (output) -#define RFM22_gpio0_config_antswt1 0x17 // Antenna 1 Switch used for antenna diversity (output) -#define RFM22_gpio0_config_antswt2 0x18 // Antenna 2 Switch used for antenna diversity (output) -#define RFM22_gpio0_config_vpd 0x19 // Valid Preamble Detected (output) -#define RFM22_gpio0_config_ipd 0x1A // Invalid Preamble Detected (output) -#define RFM22_gpio0_config_swd 0x1B // Sync Word Detected (output) -#define RFM22_gpio0_config_cca 0x1C // Clear Channel Assessment (output) -#define RFM22_gpio0_config_vdd 0x1D // VDD -#define RFM22_gpio0_config_pup 0x20 -#define RFM22_gpio0_config_drv0 0x00 // output drive level -#define RFM22_gpio0_config_drv1 0x40 // output drive level -#define RFM22_gpio0_config_drv2 0x80 // output drive level -#define RFM22_gpio0_config_drv3 0xC0 // output drive level +#define RFM22_gpio0_config 0x0B // R/W +#define RFM22_gpio0_config_por 0x00 // Power-On-Reset (output) +#define RFM22_gpio0_config_wut 0x01 // Wake-Up Timer: 1 when WUT has expired (output) +#define RFM22_gpio0_config_lbd 0x02 // Low Battery Detect: 1 when battery is below threshold setting (output) +#define RFM22_gpio0_config_ddi 0x03 // Direct Digital Input +#define RFM22_gpio0_config_eife 0x04 // External Interrupt, falling edge (input) +#define RFM22_gpio0_config_eire 0x05 // External Interrupt, rising edge (input) +#define RFM22_gpio0_config_eisc 0x06 // External Interrupt, state change (input) +#define RFM22_gpio0_config_ai 0x07 // ADC Analog Input +#define RFM22_gpio0_config_atni 0x08 // Reserved (Analog Test N Input) +#define RFM22_gpio0_config_atpi 0x09 // Reserved (Analog Test P Input) +#define RFM22_gpio0_config_ddo 0x0A // Direct Digital Output +#define RFM22_gpio0_config_dto 0x0B // Reserved (Digital Test Output) +#define RFM22_gpio0_config_atno 0x0C // Reserved (Analog Test N Output) +#define RFM22_gpio0_config_atpo 0x0D // Reserved (Analog Test P Output) +#define RFM22_gpio0_config_rv 0xOE // Reference Voltage (output) +#define RFM22_gpio0_config_dclk 0x0F // TX/RX Data CLK output to be used in conjunction with TX/RX Data pin (output) +#define RFM22_gpio0_config_txd 0x10 // TX Data input for direct modulation (input) +#define RFM22_gpio0_config_err 0x11 // External Retransmission Request (input) +#define RFM22_gpio0_config_txstate 0x12 // TX State (output) +#define RFM22_gpio0_config_txfifoaf 0x13 // TX FIFO Almost Full (output) +#define RFM22_gpio0_config_rxd 0x14 // RX Data (output) +#define RFM22_gpio0_config_rxstate 0x15 // RX State (output) +#define RFM22_gpio0_config_rxfifoaf 0x16 // RX FIFO Almost Full (output) +#define RFM22_gpio0_config_antswt1 0x17 // Antenna 1 Switch used for antenna diversity (output) +#define RFM22_gpio0_config_antswt2 0x18 // Antenna 2 Switch used for antenna diversity (output) +#define RFM22_gpio0_config_vpd 0x19 // Valid Preamble Detected (output) +#define RFM22_gpio0_config_ipd 0x1A // Invalid Preamble Detected (output) +#define RFM22_gpio0_config_swd 0x1B // Sync Word Detected (output) +#define RFM22_gpio0_config_cca 0x1C // Clear Channel Assessment (output) +#define RFM22_gpio0_config_vdd 0x1D // VDD +#define RFM22_gpio0_config_pup 0x20 +#define RFM22_gpio0_config_drv0 0x00 // output drive level +#define RFM22_gpio0_config_drv1 0x40 // output drive level +#define RFM22_gpio0_config_drv2 0x80 // output drive level +#define RFM22_gpio0_config_drv3 0xC0 // output drive level -#define RFM22_gpio1_config 0x0C // R/W -#define RFM22_gpio1_config_ipor 0x00 // Inverted Power-On-Reset (output) -#define RFM22_gpio1_config_wut 0x01 // Wake-Up Timer: 1 when WUT has expired (output) -#define RFM22_gpio1_config_lbd 0x02 // Low Battery Detect: 1 when battery is below threshold setting (output) -#define RFM22_gpio1_config_ddi 0x03 // Direct Digital Input -#define RFM22_gpio1_config_eife 0x04 // External Interrupt, falling edge (input) -#define RFM22_gpio1_config_eire 0x05 // External Interrupt, rising edge (input) -#define RFM22_gpio1_config_eisc 0x06 // External Interrupt, state change (input) -#define RFM22_gpio1_config_ai 0x07 // ADC Analog Input -#define RFM22_gpio1_config_atni 0x08 // Reserved (Analog Test N Input) -#define RFM22_gpio1_config_atpi 0x09 // Reserved (Analog Test P Input) -#define RFM22_gpio1_config_ddo 0x0A // Direct Digital Output -#define RFM22_gpio1_config_dto 0x0B // Reserved (Digital Test Output) -#define RFM22_gpio1_config_atno 0x0C // Reserved (Analog Test N Output) -#define RFM22_gpio1_config_atpo 0x0D // Reserved (Analog Test P Output) -#define RFM22_gpio1_config_rv 0xOE // Reference Voltage (output) -#define RFM22_gpio1_config_dclk 0x0F // TX/RX Data CLK output to be used in conjunction with TX/RX Data pin (output) -#define RFM22_gpio1_config_txd 0x10 // TX Data input for direct modulation (input) -#define RFM22_gpio1_config_err 0x11 // External Retransmission Request (input) -#define RFM22_gpio1_config_txstate 0x12 // TX State (output) -#define RFM22_gpio1_config_txfifoaf 0x13 // TX FIFO Almost Full (output) -#define RFM22_gpio1_config_rxd 0x14 // RX Data (output) -#define RFM22_gpio1_config_rxstate 0x15 // RX State (output) -#define RFM22_gpio1_config_rxfifoaf 0x16 // RX FIFO Almost Full (output) -#define RFM22_gpio1_config_antswt1 0x17 // Antenna 1 Switch used for antenna diversity (output) -#define RFM22_gpio1_config_antswt2 0x18 // Antenna 2 Switch used for antenna diversity (output) -#define RFM22_gpio1_config_vpd 0x19 // Valid Preamble Detected (output) -#define RFM22_gpio1_config_ipd 0x1A // Invalid Preamble Detected (output) -#define RFM22_gpio1_config_swd 0x1B // Sync Word Detected (output) -#define RFM22_gpio1_config_cca 0x1C // Clear Channel Assessment (output) -#define RFM22_gpio1_config_vdd 0x1D // VDD -#define RFM22_gpio1_config_pup 0x20 -#define RFM22_gpio1_config_drv0 0x00 // output drive level -#define RFM22_gpio1_config_drv1 0x40 // output drive level -#define RFM22_gpio1_config_drv2 0x80 // output drive level -#define RFM22_gpio1_config_drv3 0xC0 // output drive level +#define RFM22_gpio1_config 0x0C // R/W +#define RFM22_gpio1_config_ipor 0x00 // Inverted Power-On-Reset (output) +#define RFM22_gpio1_config_wut 0x01 // Wake-Up Timer: 1 when WUT has expired (output) +#define RFM22_gpio1_config_lbd 0x02 // Low Battery Detect: 1 when battery is below threshold setting (output) +#define RFM22_gpio1_config_ddi 0x03 // Direct Digital Input +#define RFM22_gpio1_config_eife 0x04 // External Interrupt, falling edge (input) +#define RFM22_gpio1_config_eire 0x05 // External Interrupt, rising edge (input) +#define RFM22_gpio1_config_eisc 0x06 // External Interrupt, state change (input) +#define RFM22_gpio1_config_ai 0x07 // ADC Analog Input +#define RFM22_gpio1_config_atni 0x08 // Reserved (Analog Test N Input) +#define RFM22_gpio1_config_atpi 0x09 // Reserved (Analog Test P Input) +#define RFM22_gpio1_config_ddo 0x0A // Direct Digital Output +#define RFM22_gpio1_config_dto 0x0B // Reserved (Digital Test Output) +#define RFM22_gpio1_config_atno 0x0C // Reserved (Analog Test N Output) +#define RFM22_gpio1_config_atpo 0x0D // Reserved (Analog Test P Output) +#define RFM22_gpio1_config_rv 0xOE // Reference Voltage (output) +#define RFM22_gpio1_config_dclk 0x0F // TX/RX Data CLK output to be used in conjunction with TX/RX Data pin (output) +#define RFM22_gpio1_config_txd 0x10 // TX Data input for direct modulation (input) +#define RFM22_gpio1_config_err 0x11 // External Retransmission Request (input) +#define RFM22_gpio1_config_txstate 0x12 // TX State (output) +#define RFM22_gpio1_config_txfifoaf 0x13 // TX FIFO Almost Full (output) +#define RFM22_gpio1_config_rxd 0x14 // RX Data (output) +#define RFM22_gpio1_config_rxstate 0x15 // RX State (output) +#define RFM22_gpio1_config_rxfifoaf 0x16 // RX FIFO Almost Full (output) +#define RFM22_gpio1_config_antswt1 0x17 // Antenna 1 Switch used for antenna diversity (output) +#define RFM22_gpio1_config_antswt2 0x18 // Antenna 2 Switch used for antenna diversity (output) +#define RFM22_gpio1_config_vpd 0x19 // Valid Preamble Detected (output) +#define RFM22_gpio1_config_ipd 0x1A // Invalid Preamble Detected (output) +#define RFM22_gpio1_config_swd 0x1B // Sync Word Detected (output) +#define RFM22_gpio1_config_cca 0x1C // Clear Channel Assessment (output) +#define RFM22_gpio1_config_vdd 0x1D // VDD +#define RFM22_gpio1_config_pup 0x20 +#define RFM22_gpio1_config_drv0 0x00 // output drive level +#define RFM22_gpio1_config_drv1 0x40 // output drive level +#define RFM22_gpio1_config_drv2 0x80 // output drive level +#define RFM22_gpio1_config_drv3 0xC0 // output drive level -#define RFM22_gpio2_config 0x0D // R/W -#define RFM22_gpio2_config_mc 0x00 // Microcontroller Clock (output) -#define RFM22_gpio2_config_wut 0x01 // Wake-Up Timer: 1 when WUT has expired (output) -#define RFM22_gpio2_config_lbd 0x02 // Low Battery Detect: 1 when battery is below threshold setting (output) -#define RFM22_gpio2_config_ddi 0x03 // Direct Digital Input -#define RFM22_gpio2_config_eife 0x04 // External Interrupt, falling edge (input) -#define RFM22_gpio2_config_eire 0x05 // External Interrupt, rising edge (input) -#define RFM22_gpio2_config_eisc 0x06 // External Interrupt, state change (input) -#define RFM22_gpio2_config_ai 0x07 // ADC Analog Input -#define RFM22_gpio2_config_atni 0x08 // Reserved (Analog Test N Input) -#define RFM22_gpio2_config_atpi 0x09 // Reserved (Analog Test P Input) -#define RFM22_gpio2_config_ddo 0x0A // Direct Digital Output -#define RFM22_gpio2_config_dto 0x0B // Reserved (Digital Test Output) -#define RFM22_gpio2_config_atno 0x0C // Reserved (Analog Test N Output) -#define RFM22_gpio2_config_atpo 0x0D // Reserved (Analog Test P Output) -#define RFM22_gpio2_config_rv 0xOE // Reference Voltage (output) -#define RFM22_gpio2_config_dclk 0x0F // TX/RX Data CLK output to be used in conjunction with TX/RX Data pin (output) -#define RFM22_gpio2_config_txd 0x10 // TX Data input for direct modulation (input) -#define RFM22_gpio2_config_err 0x11 // External Retransmission Request (input) -#define RFM22_gpio2_config_txstate 0x12 // TX State (output) -#define RFM22_gpio2_config_txfifoaf 0x13 // TX FIFO Almost Full (output) -#define RFM22_gpio2_config_rxd 0x14 // RX Data (output) -#define RFM22_gpio2_config_rxstate 0x15 // RX State (output) -#define RFM22_gpio2_config_rxfifoaf 0x16 // RX FIFO Almost Full (output) -#define RFM22_gpio2_config_antswt1 0x17 // Antenna 1 Switch used for antenna diversity (output) -#define RFM22_gpio2_config_antswt2 0x18 // Antenna 2 Switch used for antenna diversity (output) -#define RFM22_gpio2_config_vpd 0x19 // Valid Preamble Detected (output) -#define RFM22_gpio2_config_ipd 0x1A // Invalid Preamble Detected (output) -#define RFM22_gpio2_config_swd 0x1B // Sync Word Detected (output) -#define RFM22_gpio2_config_cca 0x1C // Clear Channel Assessment (output) -#define RFM22_gpio2_config_vdd 0x1D // VDD -#define RFM22_gpio2_config_pup 0x20 -#define RFM22_gpio2_config_drv0 0x00 // output drive level -#define RFM22_gpio2_config_drv1 0x40 // output drive level -#define RFM22_gpio2_config_drv2 0x80 // output drive level -#define RFM22_gpio2_config_drv3 0xC0 // output drive level +#define RFM22_gpio2_config 0x0D // R/W +#define RFM22_gpio2_config_mc 0x00 // Microcontroller Clock (output) +#define RFM22_gpio2_config_wut 0x01 // Wake-Up Timer: 1 when WUT has expired (output) +#define RFM22_gpio2_config_lbd 0x02 // Low Battery Detect: 1 when battery is below threshold setting (output) +#define RFM22_gpio2_config_ddi 0x03 // Direct Digital Input +#define RFM22_gpio2_config_eife 0x04 // External Interrupt, falling edge (input) +#define RFM22_gpio2_config_eire 0x05 // External Interrupt, rising edge (input) +#define RFM22_gpio2_config_eisc 0x06 // External Interrupt, state change (input) +#define RFM22_gpio2_config_ai 0x07 // ADC Analog Input +#define RFM22_gpio2_config_atni 0x08 // Reserved (Analog Test N Input) +#define RFM22_gpio2_config_atpi 0x09 // Reserved (Analog Test P Input) +#define RFM22_gpio2_config_ddo 0x0A // Direct Digital Output +#define RFM22_gpio2_config_dto 0x0B // Reserved (Digital Test Output) +#define RFM22_gpio2_config_atno 0x0C // Reserved (Analog Test N Output) +#define RFM22_gpio2_config_atpo 0x0D // Reserved (Analog Test P Output) +#define RFM22_gpio2_config_rv 0xOE // Reference Voltage (output) +#define RFM22_gpio2_config_dclk 0x0F // TX/RX Data CLK output to be used in conjunction with TX/RX Data pin (output) +#define RFM22_gpio2_config_txd 0x10 // TX Data input for direct modulation (input) +#define RFM22_gpio2_config_err 0x11 // External Retransmission Request (input) +#define RFM22_gpio2_config_txstate 0x12 // TX State (output) +#define RFM22_gpio2_config_txfifoaf 0x13 // TX FIFO Almost Full (output) +#define RFM22_gpio2_config_rxd 0x14 // RX Data (output) +#define RFM22_gpio2_config_rxstate 0x15 // RX State (output) +#define RFM22_gpio2_config_rxfifoaf 0x16 // RX FIFO Almost Full (output) +#define RFM22_gpio2_config_antswt1 0x17 // Antenna 1 Switch used for antenna diversity (output) +#define RFM22_gpio2_config_antswt2 0x18 // Antenna 2 Switch used for antenna diversity (output) +#define RFM22_gpio2_config_vpd 0x19 // Valid Preamble Detected (output) +#define RFM22_gpio2_config_ipd 0x1A // Invalid Preamble Detected (output) +#define RFM22_gpio2_config_swd 0x1B // Sync Word Detected (output) +#define RFM22_gpio2_config_cca 0x1C // Clear Channel Assessment (output) +#define RFM22_gpio2_config_vdd 0x1D // VDD +#define RFM22_gpio2_config_pup 0x20 +#define RFM22_gpio2_config_drv0 0x00 // output drive level +#define RFM22_gpio2_config_drv1 0x40 // output drive level +#define RFM22_gpio2_config_drv2 0x80 // output drive level +#define RFM22_gpio2_config_drv3 0xC0 // output drive level -#define RFM22_io_port_config 0x0E // R/W -#define RFM22_io_port_extitst2 0x40 // External Interrupt Status. If the GPIO2 is programmed to be external interrupt sources then the status can be read here. -#define RFM22_io_port_extitst1 0x20 // External Interrupt Status. If the GPIO1 is programmed to be external interrupt sources then the status can be read here. -#define RFM22_io_port_extitst0 0x10 // External Interrupt Status. If the GPIO0 is programmed to be external interrupt sources then the status can be read here. -#define RFM22_io_port_itsdo 0x08 // Interrupt Request Output on the SDO Pin. nIRQ output is present on the SDO pin if this bit is set and the nSEL input is inactive (high). -#define RFM22_io_port_dio2 0x04 // Direct I/O for GPIO2. If the GPIO2 is configured to be a direct output then the value on the GPIO pin can be set here. If the GPIO2 is configured to be a direct input then the value of the pin can be read here. -#define RFM22_io_port_dio1 0x02 // Direct I/O for GPIO1. If the GPIO1 is configured to be a direct output then the value on the GPIO pin can be set here. If the GPIO1 is configured to be a direct input then the value of the pin can be read here. -#define RFM22_io_port_dio0 0x01 // Direct I/O for GPIO0. If the GPIO0 is configured to be a direct output then the value on the GPIO pin can be set here. If the GPIO0 is configured to be a direct input then the value of the pin can be read here. -#define RFM22_io_port_default 0x00 // GPIO pins are default +#define RFM22_io_port_config 0x0E // R/W +#define RFM22_io_port_extitst2 0x40 // External Interrupt Status. If the GPIO2 is programmed to be external interrupt sources then the status can be read here. +#define RFM22_io_port_extitst1 0x20 // External Interrupt Status. If the GPIO1 is programmed to be external interrupt sources then the status can be read here. +#define RFM22_io_port_extitst0 0x10 // External Interrupt Status. If the GPIO0 is programmed to be external interrupt sources then the status can be read here. +#define RFM22_io_port_itsdo 0x08 // Interrupt Request Output on the SDO Pin. nIRQ output is present on the SDO pin if this bit is set and the nSEL input is inactive (high). +#define RFM22_io_port_dio2 0x04 // Direct I/O for GPIO2. If the GPIO2 is configured to be a direct output then the value on the GPIO pin can be set here. If the GPIO2 is configured to be a direct input then the value of the pin can be read here. +#define RFM22_io_port_dio1 0x02 // Direct I/O for GPIO1. If the GPIO1 is configured to be a direct output then the value on the GPIO pin can be set here. If the GPIO1 is configured to be a direct input then the value of the pin can be read here. +#define RFM22_io_port_dio0 0x01 // Direct I/O for GPIO0. If the GPIO0 is configured to be a direct output then the value on the GPIO pin can be set here. If the GPIO0 is configured to be a direct input then the value of the pin can be read here. +#define RFM22_io_port_default 0x00 // GPIO pins are default -#define RFM22_adc_config 0x0F // R/W -#define RFM22_ac_adcgain0 0x00 -#define RFM22_ac_adcgain1 0x01 -#define RFM22_ac_adcgain2 0x02 -#define RFM22_ac_adcgain3 0x03 -#define RFM22_ac_adcref_bg 0x00 -#define RFM22_ac_adcref_vdd3 0x08 -#define RFM22_ac_adcref_vdd2 0x0C -#define RFM22_ac_adcsel_temp_sensor 0x00 -#define RFM22_ac_adcsel_gpio0 0x10 -#define RFM22_ac_adcsel_gpio1 0x20 -#define RFM22_ac_adcsel_gpio2 0x30 -#define RFM22_ac_adcsel_gpio01 0x40 -#define RFM22_ac_adcsel_gpio12 0x50 -#define RFM22_ac_adcsel_gpio02 0x60 -#define RFM22_ac_adcsel_gpio_gnd 0x70 -#define RFM22_ac_adcstartbusy 0x80 +#define RFM22_adc_config 0x0F // R/W +#define RFM22_ac_adcgain0 0x00 +#define RFM22_ac_adcgain1 0x01 +#define RFM22_ac_adcgain2 0x02 +#define RFM22_ac_adcgain3 0x03 +#define RFM22_ac_adcref_bg 0x00 +#define RFM22_ac_adcref_vdd3 0x08 +#define RFM22_ac_adcref_vdd2 0x0C +#define RFM22_ac_adcsel_temp_sensor 0x00 +#define RFM22_ac_adcsel_gpio0 0x10 +#define RFM22_ac_adcsel_gpio1 0x20 +#define RFM22_ac_adcsel_gpio2 0x30 +#define RFM22_ac_adcsel_gpio01 0x40 +#define RFM22_ac_adcsel_gpio12 0x50 +#define RFM22_ac_adcsel_gpio02 0x60 +#define RFM22_ac_adcsel_gpio_gnd 0x70 +#define RFM22_ac_adcstartbusy 0x80 -#define RFM22_adc_sensor_amp_offset 0x10 // R/W -#define RFM22_asao_adcoffs_mask 0x0F // ADC Sensor Amplifier Offset. The offset can be calculated as Offset = adcoffs[2:0] x VDD/1000; MSB = adcoffs[3] = Sign bit. +#define RFM22_adc_sensor_amp_offset 0x10 // R/W +#define RFM22_asao_adcoffs_mask 0x0F // ADC Sensor Amplifier Offset. The offset can be calculated as Offset = adcoffs[2:0] x VDD/1000; MSB = adcoffs[3] = Sign bit. -#define RFM22_adc_value 0x11 // R .. Internal 8 bit ADC Output Value. +#define RFM22_adc_value 0x11 // R .. Internal 8 bit ADC Output Value. -#define RFM22_temp_sensor_calib 0x12 // R/W -#define RFM22_tsc_tstrim_mask 0x0F // Temperature Sensor Trim Value. -#define RFM22_tsc_entstrim 0x10 // Temperature Sensor Trim Enable. -#define RFM22_tsc_entsoffs 0x20 // Temperature Sensor Offset to Convert from K to �C. -#define RFM22_tsc_tsrange0 0x00 // Temperature Sensor Range Selection. �64C to +64C 0.5C resolution -#define RFM22_tsc_tsrange1 0x40 // -40 to +85C with 1.0C resolution -#define RFM22_tsc_tsrange2 0x80 // 0C to 85C with 0.5C resolution -#define RFM22_tsc_tsrange3 0xC0 // -40F to 216F with 1.0F resolution +#define RFM22_temp_sensor_calib 0x12 // R/W +#define RFM22_tsc_tstrim_mask 0x0F // Temperature Sensor Trim Value. +#define RFM22_tsc_entstrim 0x10 // Temperature Sensor Trim Enable. +#define RFM22_tsc_entsoffs 0x20 // Temperature Sensor Offset to Convert from K to �C. +#define RFM22_tsc_tsrange0 0x00 // Temperature Sensor Range Selection. �64C to +64C 0.5C resolution +#define RFM22_tsc_tsrange1 0x40 // -40 to +85C with 1.0C resolution +#define RFM22_tsc_tsrange2 0x80 // 0C to 85C with 0.5C resolution +#define RFM22_tsc_tsrange3 0xC0 // -40F to 216F with 1.0F resolution -#define RFM22_temp_value_offset 0x13 // R/W +#define RFM22_temp_value_offset 0x13 // R/W -#define RFM22_wakeup_timer_period1 0x14 // R/W -#define RFM22_wakeup_timer_period2 0x15 // R/W -#define RFM22_wakeup_timer_period3 0x16 // R/W +#define RFM22_wakeup_timer_period1 0x14 // R/W +#define RFM22_wakeup_timer_period2 0x15 // R/W +#define RFM22_wakeup_timer_period3 0x16 // R/W -#define RFM22_wakeup_timer_value1 0x17 // R -#define RFM22_wakeup_timer_value2 0x18 // R +#define RFM22_wakeup_timer_value1 0x17 // R +#define RFM22_wakeup_timer_value2 0x18 // R -#define RFM22_low_dutycycle_mode_duration 0x19 // R/W -#define RFM22_low_battery_detector_threshold 0x1A // R/W +#define RFM22_low_dutycycle_mode_duration 0x19 // R/W +#define RFM22_low_battery_detector_threshold 0x1A // R/W -#define RFM22_battery_volateg_level 0x1B // R +#define RFM22_battery_volateg_level 0x1B // R -#define RFM22_if_filter_bandwidth 0x1C // R/W -#define RFM22_iffbw_filset_mask 0x0F -#define RFM22_iffbw_ndec_exp_mask 0x70 -#define RFM22_iffbw_dwn3_bypass 0x80 +#define RFM22_if_filter_bandwidth 0x1C // R/W +#define RFM22_iffbw_filset_mask 0x0F +#define RFM22_iffbw_ndec_exp_mask 0x70 +#define RFM22_iffbw_dwn3_bypass 0x80 -#define RFM22_afc_loop_gearshift_override 0x1D // R/W -#define RFM22_afc_lp_gs_ovrd_afcgearl_mask 0x07 // AFC Low Gear Setting. -#define RFM22_afc_lp_gs_ovrd_afcgearh_mask 0x38 // AFC High Gear Setting. -#define RFM22_afc_lp_gs_ovrd_enafc 0x40 // AFC Enable. -#define RFM22_afc_lp_gs_ovrd_afcbd 0x80 // If set, the tolerated AFC frequency error will be halved. +#define RFM22_afc_loop_gearshift_override 0x1D // R/W +#define RFM22_afc_lp_gs_ovrd_afcgearl_mask 0x07 // AFC Low Gear Setting. +#define RFM22_afc_lp_gs_ovrd_afcgearh_mask 0x38 // AFC High Gear Setting. +#define RFM22_afc_lp_gs_ovrd_enafc 0x40 // AFC Enable. +#define RFM22_afc_lp_gs_ovrd_afcbd 0x80 // If set, the tolerated AFC frequency error will be halved. -#define RFM22_afc_timing_control 0x1E // R/W +#define RFM22_afc_timing_control 0x1E // R/W -#define RFM22_clk_recovery_gearshift_override 0x1F // R/W -#define RFM22_clk_recovery_oversampling_ratio 0x20 // R/W -#define RFM22_clk_recovery_offset2 0x21 // R/W -#define RFM22_clk_recovery_offset1 0x22 // R/W -#define RFM22_clk_recovery_offset0 0x23 // R/W -#define RFM22_clk_recovery_timing_loop_gain1 0x24 // R/W -#define RFM22_clk_recovery_timing_loop_gain0 0x25 // R/W +#define RFM22_clk_recovery_gearshift_override 0x1F // R/W +#define RFM22_clk_recovery_oversampling_ratio 0x20 // R/W +#define RFM22_clk_recovery_offset2 0x21 // R/W +#define RFM22_clk_recovery_offset1 0x22 // R/W +#define RFM22_clk_recovery_offset0 0x23 // R/W +#define RFM22_clk_recovery_timing_loop_gain1 0x24 // R/W +#define RFM22_clk_recovery_timing_loop_gain0 0x25 // R/W -#define RFM22_rssi 0x26 // R -#define RFM22_rssi_threshold_clear_chan_indicator 0x27 // R/W +#define RFM22_rssi 0x26 // R +#define RFM22_rssi_threshold_clear_chan_indicator 0x27 // R/W -#define RFM22_antenna_diversity_register1 0x28 // R -#define RFM22_antenna_diversity_register2 0x29 // R +#define RFM22_antenna_diversity_register1 0x28 // R +#define RFM22_antenna_diversity_register2 0x29 // R -#define RFM22_afc_limiter 0x2A // R/W .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz +#define RFM22_afc_limiter 0x2A // R/W .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz -#define RFM22_afc_correction_read 0x2B // R +#define RFM22_afc_correction_read 0x2B // R -#define RFM22_ook_counter_value1 0x2C // R/W -#define RFM22_ook_counter_value2 0x2D // R/W +#define RFM22_ook_counter_value1 0x2C // R/W +#define RFM22_ook_counter_value2 0x2D // R/W -#define RFM22_slicer_peak_hold 0x2E // R/W +#define RFM22_slicer_peak_hold 0x2E // R/W -#define RFM22_data_access_control 0x30 // R/W -#define RFM22_dac_crc_ccitt 0x00 // -#define RFM22_dac_crc_crc16 0x01 // -#define RFM22_dac_crc_iec16 0x02 // -#define RFM22_dac_crc_biacheva 0x03 // -#define RFM22_dac_encrc 0x04 // CRC Enable. Cyclic Redundancy Check generation is enabled if this bit is set. -#define RFM22_dac_enpactx 0x08 // Enable Packet TX Handling. If FIFO Mode (dtmod = 10) is being used automatic packet handling may be enabled. Setting enpactx = 1 will enable automatic packet handling in the TX path. Register 30�4D allow for various configurations of the packet structure. Setting enpactx = 0 will not do any packet handling in the TX path. It will only transmit what is loaded to the FIFO. -#define RFM22_dac_skip2ph 0x10 // Skip 2nd Phase of Preamble Detection. If set, we skip the second phase of the preamble detection (under certain conditions) if antenna diversity is enabled. -#define RFM22_dac_crcdonly 0x20 // CRC Data Only Enable. When this bit is set to 1 the CRC is calculated on and checked against the packet data fields only. -#define RFM22_dac_lsbfrst 0x40 // LSB First Enable. The LSB of the data will be transmitted/received first if this bit is set. -#define RFM22_dac_enpacrx 0x80 // Enable Packet RX Handling. If FIFO Mode (dtmod = 10) is being used automatic packet handling may be enabled. Setting enpacrx = 1 will enable automatic packet handling in the RX path. Register 30�4D allow for various configurations of the packet structure. Setting enpacrx = 0 will not do any packet handling in the RX path. It will only receive everything after the sync word and fill up the RX FIFO. +#define RFM22_data_access_control 0x30 // R/W +#define RFM22_dac_crc_ccitt 0x00 // +#define RFM22_dac_crc_crc16 0x01 // +#define RFM22_dac_crc_iec16 0x02 // +#define RFM22_dac_crc_biacheva 0x03 // +#define RFM22_dac_encrc 0x04 // CRC Enable. Cyclic Redundancy Check generation is enabled if this bit is set. +#define RFM22_dac_enpactx 0x08 // Enable Packet TX Handling. If FIFO Mode (dtmod = 10) is being used automatic packet handling may be enabled. Setting enpactx = 1 will enable automatic packet handling in the TX path. Register 30�4D allow for various configurations of the packet structure. Setting enpactx = 0 will not do any packet handling in the TX path. It will only transmit what is loaded to the FIFO. +#define RFM22_dac_skip2ph 0x10 // Skip 2nd Phase of Preamble Detection. If set, we skip the second phase of the preamble detection (under certain conditions) if antenna diversity is enabled. +#define RFM22_dac_crcdonly 0x20 // CRC Data Only Enable. When this bit is set to 1 the CRC is calculated on and checked against the packet data fields only. +#define RFM22_dac_lsbfrst 0x40 // LSB First Enable. The LSB of the data will be transmitted/received first if this bit is set. +#define RFM22_dac_enpacrx 0x80 // Enable Packet RX Handling. If FIFO Mode (dtmod = 10) is being used automatic packet handling may be enabled. Setting enpacrx = 1 will enable automatic packet handling in the RX path. Register 30�4D allow for various configurations of the packet structure. Setting enpacrx = 0 will not do any packet handling in the RX path. It will only receive everything after the sync word and fill up the RX FIFO. -#define RFM22_ezmac_status 0x31 // R -#define RFM22_ezmac_status_pksent 0x01 // Packet Sent. A 1 a packet has been sent by the radio. (Same bit as in register 03, but reading it does not reset the IRQ) -#define RFM22_ezmac_status_pktx 0x02 // Packet Transmitting. When 1 the radio is currently transmitting a packet. -#define RFM22_ezmac_status_crcerror 0x04 // CRC Error. When 1 a Cyclic Redundancy Check error has been detected. (Same bit as in register 03, but reading it does not reset the IRQ) -#define RFM22_ezmac_status_pkvalid 0x08 // Valid Packet Received. When a 1 a valid packet has been received by the receiver. (Same bit as in register 03, but reading it does not reset the IRQ) -#define RFM22_ezmac_status_pkrx 0x10 // Packet Receiving. When 1 the radio is currently receiving a valid packet. -#define RFM22_ezmac_status_pksrch 0x20 // Packet Searching. When 1 the radio is searching for a valid packet. -#define RFM22_ezmac_status_rxcrc1 0x40 // If high, it indicates the last CRC received is all one�s. May indicated Transmitter underflow in case of CRC error. +#define RFM22_ezmac_status 0x31 // R +#define RFM22_ezmac_status_pksent 0x01 // Packet Sent. A 1 a packet has been sent by the radio. (Same bit as in register 03, but reading it does not reset the IRQ) +#define RFM22_ezmac_status_pktx 0x02 // Packet Transmitting. When 1 the radio is currently transmitting a packet. +#define RFM22_ezmac_status_crcerror 0x04 // CRC Error. When 1 a Cyclic Redundancy Check error has been detected. (Same bit as in register 03, but reading it does not reset the IRQ) +#define RFM22_ezmac_status_pkvalid 0x08 // Valid Packet Received. When a 1 a valid packet has been received by the receiver. (Same bit as in register 03, but reading it does not reset the IRQ) +#define RFM22_ezmac_status_pkrx 0x10 // Packet Receiving. When 1 the radio is currently receiving a valid packet. +#define RFM22_ezmac_status_pksrch 0x20 // Packet Searching. When 1 the radio is searching for a valid packet. +#define RFM22_ezmac_status_rxcrc1 0x40 // If high, it indicates the last CRC received is all one�s. May indicated Transmitter underflow in case of CRC error. -#define RFM22_header_control1 0x32 // R/W -#define RFM22_header_cntl1_bcen_none 0x00 // No broadcast address enable. -#define RFM22_header_cntl1_bcen_0 0x10 // Broadcast address enable for header byte 0. -#define RFM22_header_cntl1_bcen_1 0x20 // Broadcast address enable for header byte 1. -#define RFM22_header_cntl1_bcen_2 0x40 // Broadcast address enable for header byte 2. -#define RFM22_header_cntl1_bcen_3 0x80 // Broadcast address enable for header byte 3. -#define RFM22_header_cntl1_hdch_none 0x00 // No Received Header check -#define RFM22_header_cntl1_hdch_0 0x01 // Received Header check for byte 0. -#define RFM22_header_cntl1_hdch_1 0x02 // Received Header check for byte 1. -#define RFM22_header_cntl1_hdch_2 0x04 // Received Header check for byte 2. -#define RFM22_header_cntl1_hdch_3 0x08 // Received Header check for byte 3. +#define RFM22_header_control1 0x32 // R/W +#define RFM22_header_cntl1_bcen_none 0x00 // No broadcast address enable. +#define RFM22_header_cntl1_bcen_0 0x10 // Broadcast address enable for header byte 0. +#define RFM22_header_cntl1_bcen_1 0x20 // Broadcast address enable for header byte 1. +#define RFM22_header_cntl1_bcen_2 0x40 // Broadcast address enable for header byte 2. +#define RFM22_header_cntl1_bcen_3 0x80 // Broadcast address enable for header byte 3. +#define RFM22_header_cntl1_hdch_none 0x00 // No Received Header check +#define RFM22_header_cntl1_hdch_0 0x01 // Received Header check for byte 0. +#define RFM22_header_cntl1_hdch_1 0x02 // Received Header check for byte 1. +#define RFM22_header_cntl1_hdch_2 0x04 // Received Header check for byte 2. +#define RFM22_header_cntl1_hdch_3 0x08 // Received Header check for byte 3. -#define RFM22_header_control2 0x33 // R/W -#define RFM22_header_cntl2_prealen 0x01 // MSB of Preamble Length. See register Preamble Length. -#define RFM22_header_cntl2_synclen_3 0x00 // Synchronization Word 3 -#define RFM22_header_cntl2_synclen_32 0x02 // Synchronization Word 3 followed by 2 -#define RFM22_header_cntl2_synclen_321 0x04 // Synchronization Word 3 followed by 2 followed by 1 -#define RFM22_header_cntl2_synclen_3210 0x06 // Synchronization Word 3 followed by 2 followed by 1 followed by 0 -#define RFM22_header_cntl2_fixpklen 0x08 // Fix Packet Length. When fixpklen = 1 the packet length (pklen[7:0]) is not included in the header. When fixpklen = 0 the packet length is included in the header. -#define RFM22_header_cntl2_hdlen_none 0x00 // no header -#define RFM22_header_cntl2_hdlen_3 0x10 // header 3 -#define RFM22_header_cntl2_hdlen_32 0x20 // header 3 and 2 -#define RFM22_header_cntl2_hdlen_321 0x30 // header 3 and 2 and 1 -#define RFM22_header_cntl2_hdlen_3210 0x40 // header 3 and 2 and 1 and 0 -#define RFM22_header_cntl2_skipsyn 0x80 // If high, the system will ignore the syncword search timeout reset. The chip will not return to searching for Preamble, but instead will remain searching for Sync word. +#define RFM22_header_control2 0x33 // R/W +#define RFM22_header_cntl2_prealen 0x01 // MSB of Preamble Length. See register Preamble Length. +#define RFM22_header_cntl2_synclen_3 0x00 // Synchronization Word 3 +#define RFM22_header_cntl2_synclen_32 0x02 // Synchronization Word 3 followed by 2 +#define RFM22_header_cntl2_synclen_321 0x04 // Synchronization Word 3 followed by 2 followed by 1 +#define RFM22_header_cntl2_synclen_3210 0x06 // Synchronization Word 3 followed by 2 followed by 1 followed by 0 +#define RFM22_header_cntl2_fixpklen 0x08 // Fix Packet Length. When fixpklen = 1 the packet length (pklen[7:0]) is not included in the header. When fixpklen = 0 the packet length is included in the header. +#define RFM22_header_cntl2_hdlen_none 0x00 // no header +#define RFM22_header_cntl2_hdlen_3 0x10 // header 3 +#define RFM22_header_cntl2_hdlen_32 0x20 // header 3 and 2 +#define RFM22_header_cntl2_hdlen_321 0x30 // header 3 and 2 and 1 +#define RFM22_header_cntl2_hdlen_3210 0x40 // header 3 and 2 and 1 and 0 +#define RFM22_header_cntl2_skipsyn 0x80 // If high, the system will ignore the syncword search timeout reset. The chip will not return to searching for Preamble, but instead will remain searching for Sync word. -#define RFM22_preamble_length 0x34 // R/W +#define RFM22_preamble_length 0x34 // R/W -#define RFM22_preamble_detection_ctrl1 0x35 // R/W -#define RFM22_pre_det_ctrl1_preath_mask 0xF8 // Number of nibbles processed during detection. -#define RFM22_pre_det_ctrl1_rssi_offset_mask 0x07 // Value added as offset to RSSI calculation. Every increment in this register results in an increment of +4 dB in the RSSI. +#define RFM22_preamble_detection_ctrl1 0x35 // R/W +#define RFM22_pre_det_ctrl1_preath_mask 0xF8 // Number of nibbles processed during detection. +#define RFM22_pre_det_ctrl1_rssi_offset_mask 0x07 // Value added as offset to RSSI calculation. Every increment in this register results in an increment of +4 dB in the RSSI. -#define RFM22_sync_word3 0x36 // R/W -#define RFM22_sync_word2 0x37 // R/W -#define RFM22_sync_word1 0x38 // R/W -#define RFM22_sync_word0 0x39 // R/W +#define RFM22_sync_word3 0x36 // R/W +#define RFM22_sync_word2 0x37 // R/W +#define RFM22_sync_word1 0x38 // R/W +#define RFM22_sync_word0 0x39 // R/W -#define RFM22_transmit_header3 0x3A // R/W -#define RFM22_transmit_header2 0x3B // R/W -#define RFM22_transmit_header1 0x3C // R/W -#define RFM22_transmit_header0 0x3D // R/W +#define RFM22_transmit_header3 0x3A // R/W +#define RFM22_transmit_header2 0x3B // R/W +#define RFM22_transmit_header1 0x3C // R/W +#define RFM22_transmit_header0 0x3D // R/W -#define RFM22_transmit_packet_length 0x3E // R/W +#define RFM22_transmit_packet_length 0x3E // R/W -#define RFM22_check_header3 0x3F // R/W -#define RFM22_check_header2 0x40 // R/W -#define RFM22_check_header1 0x41 // R/W -#define RFM22_check_header0 0x42 // R/W +#define RFM22_check_header3 0x3F // R/W +#define RFM22_check_header2 0x40 // R/W +#define RFM22_check_header1 0x41 // R/W +#define RFM22_check_header0 0x42 // R/W -#define RFM22_header_enable3 0x43 // R/W -#define RFM22_header_enable2 0x44 // R/W -#define RFM22_header_enable1 0x45 // R/W -#define RFM22_header_enable0 0x46 // R/W +#define RFM22_header_enable3 0x43 // R/W +#define RFM22_header_enable2 0x44 // R/W +#define RFM22_header_enable1 0x45 // R/W +#define RFM22_header_enable0 0x46 // R/W -#define RFM22_received_header3 0x47 // R -#define RFM22_received_header2 0x48 // R -#define RFM22_received_header1 0x49 // R -#define RFM22_received_header0 0x4A // R +#define RFM22_received_header3 0x47 // R +#define RFM22_received_header2 0x48 // R +#define RFM22_received_header1 0x49 // R +#define RFM22_received_header0 0x4A // R -#define RFM22_received_packet_length 0x4B // R +#define RFM22_received_packet_length 0x4B // R -#define RFM22_adc8_control 0x4F // R/W +#define RFM22_adc8_control 0x4F // R/W -#define RFM22_channel_filter_coeff_addr 0x60 // R/W -#define RFM22_ch_fil_coeff_ad_inv_pre_th_mask 0xF0 // -#define RFM22_ch_fil_coeff_ad_chfiladd_mask 0x0F // Channel Filter Coefficient Look-up Table Address. The address for channel filter coefficients used in the RX path. +#define RFM22_channel_filter_coeff_addr 0x60 // R/W +#define RFM22_ch_fil_coeff_ad_inv_pre_th_mask 0xF0 // +#define RFM22_ch_fil_coeff_ad_chfiladd_mask 0x0F // Channel Filter Coefficient Look-up Table Address. The address for channel filter coefficients used in the RX path. -#define RFM22_xtal_osc_por_ctrl 0x62 // R/W -#define RFM22_xtal_osc_por_ctrl_pwst_mask 0xE0 // Internal Power States of the Chip. -#define RFM22_xtal_osc_por_ctrl_clkhyst 0x10 // Clock Hysteresis Setting. -#define RFM22_xtal_osc_por_ctrl_enbias2x 0x08 // 2 Times Higher Bias Current Enable. -#define RFM22_xtal_osc_por_ctrl_enamp2x 0x04 // 2 Times Higher Amplification Enable. -#define RFM22_xtal_osc_por_ctrl_bufovr 0x02 // Output Buffer Enable Override. -#define RFM22_xtal_osc_por_ctrl_enbuf 0x01 // Output Buffer Enable. +#define RFM22_xtal_osc_por_ctrl 0x62 // R/W +#define RFM22_xtal_osc_por_ctrl_pwst_mask 0xE0 // Internal Power States of the Chip. +#define RFM22_xtal_osc_por_ctrl_clkhyst 0x10 // Clock Hysteresis Setting. +#define RFM22_xtal_osc_por_ctrl_enbias2x 0x08 // 2 Times Higher Bias Current Enable. +#define RFM22_xtal_osc_por_ctrl_enamp2x 0x04 // 2 Times Higher Amplification Enable. +#define RFM22_xtal_osc_por_ctrl_bufovr 0x02 // Output Buffer Enable Override. +#define RFM22_xtal_osc_por_ctrl_enbuf 0x01 // Output Buffer Enable. -#define RFM22_agc_override1 0x69 // R/W -#define RFM22_agc_ovr1_sgi 0x40 // AGC Loop, Set Gain Increase. If set to 0 then gain increasing will not be allowed. If set to 1 then gain increasing is allowed, default is 0. -#define RFM22_agc_ovr1_agcen 0x20 // Automatic Gain Control Enable. When this bit is set then the result of the control can be read out from bits [4:0], otherwise the gain can be controlled manually by writing into bits [4:0]. -#define RFM22_agc_ovr1_lnagain 0x10 // LNA Gain Select. 0 = min gain = 5dB, 1 = max gain = 25 dB. -#define RFM22_agc_ovr1_pga_mask 0x0F // PGA Gain Override Value. +#define RFM22_agc_override1 0x69 // R/W +#define RFM22_agc_ovr1_sgi 0x40 // AGC Loop, Set Gain Increase. If set to 0 then gain increasing will not be allowed. If set to 1 then gain increasing is allowed, default is 0. +#define RFM22_agc_ovr1_agcen 0x20 // Automatic Gain Control Enable. When this bit is set then the result of the control can be read out from bits [4:0], otherwise the gain can be controlled manually by writing into bits [4:0]. +#define RFM22_agc_ovr1_lnagain 0x10 // LNA Gain Select. 0 = min gain = 5dB, 1 = max gain = 25 dB. +#define RFM22_agc_ovr1_pga_mask 0x0F // PGA Gain Override Value. -#define RFM22_tx_power 0x6D // R/W -#define RFM22_tx_pwr_lna_sw 0x08 // LNA Switch Controller. If set, lna_sw control from the digital will go high during TX modes, and low during other times. If reset, the digital control signal is low at all times. +#define RFM22_tx_power 0x6D // R/W +#define RFM22_tx_pwr_lna_sw 0x08 // LNA Switch Controller. If set, lna_sw control from the digital will go high during TX modes, and low during other times. If reset, the digital control signal is low at all times. -#define RFM22_tx_data_rate1 0x6E // R/W -#define RFM22_tx_data_rate0 0x6F // R/W +#define RFM22_tx_data_rate1 0x6E // R/W +#define RFM22_tx_data_rate0 0x6F // R/W -#define RFM22_modulation_mode_control1 0x70 // R/W -#define RFM22_mmc1_enwhite 0x01 // Data Whitening is Enabled if this bit is set. -#define RFM22_mmc1_enmanch 0x02 // Manchester Coding is Enabled if this bit is set. -#define RFM22_mmc1_enmaninv 0x04 // Manchester Data Inversion is Enabled if this bit is set. -#define RFM22_mmc1_manppol 0x08 // Manchester Preamble Polarity (will transmit a series of 1 if set, or series of 0 if reset). -#define RFM22_mmc1_enphpwdn 0x10 // If set, the Packet Handler will be powered down when chip is in low power mode. -#define RFM22_mmc1_txdtrtscale 0x20 // This bit should be set for Data Rates below 30 kbps. +#define RFM22_modulation_mode_control1 0x70 // R/W +#define RFM22_mmc1_enwhite 0x01 // Data Whitening is Enabled if this bit is set. +#define RFM22_mmc1_enmanch 0x02 // Manchester Coding is Enabled if this bit is set. +#define RFM22_mmc1_enmaninv 0x04 // Manchester Data Inversion is Enabled if this bit is set. +#define RFM22_mmc1_manppol 0x08 // Manchester Preamble Polarity (will transmit a series of 1 if set, or series of 0 if reset). +#define RFM22_mmc1_enphpwdn 0x10 // If set, the Packet Handler will be powered down when chip is in low power mode. +#define RFM22_mmc1_txdtrtscale 0x20 // This bit should be set for Data Rates below 30 kbps. -#define RFM22_modulation_mode_control2 0x71 // R/W -#define RFM22_mmc2_modtyp_mask 0x03 // Modulation type. -#define RFM22_mmc2_modtyp_none 0x00 // -#define RFM22_mmc2_modtyp_ook 0x01 // -#define RFM22_mmc2_modtyp_fsk 0x02 // -#define RFM22_mmc2_modtyp_gfsk 0x03 // -#define RFM22_mmc2_fd 0x04 // MSB of Frequency Deviation Setting, see "Register 72h. Frequency Deviation". -#define RFM22_mmc2_eninv 0x08 // Invert TX and RX Data. -#define RFM22_mmc2_dtmod_mask 0x30 // Modulation source. -#define RFM22_mmc2_dtmod_dm_gpio 0x00 // -#define RFM22_mmc2_dtmod_dm_sdi 0x10 // -#define RFM22_mmc2_dtmod_fifo 0x20 // -#define RFM22_mmc2_dtmod_pn9 0x30 // -#define RFM22_mmc2_trclk_mask 0xC0 // TX Data Clock Configuration. -#define RFM22_mmc2_trclk_clk_none 0x00 // -#define RFM22_mmc2_trclk_clk_gpio 0x40 // -#define RFM22_mmc2_trclk_clk_sdo 0x80 // -#define RFM22_mmc2_trclk_clk_nirq 0xC0 // +#define RFM22_modulation_mode_control2 0x71 // R/W +#define RFM22_mmc2_modtyp_mask 0x03 // Modulation type. +#define RFM22_mmc2_modtyp_none 0x00 // +#define RFM22_mmc2_modtyp_ook 0x01 // +#define RFM22_mmc2_modtyp_fsk 0x02 // +#define RFM22_mmc2_modtyp_gfsk 0x03 // +#define RFM22_mmc2_fd 0x04 // MSB of Frequency Deviation Setting, see "Register 72h. Frequency Deviation". +#define RFM22_mmc2_eninv 0x08 // Invert TX and RX Data. +#define RFM22_mmc2_dtmod_mask 0x30 // Modulation source. +#define RFM22_mmc2_dtmod_dm_gpio 0x00 // +#define RFM22_mmc2_dtmod_dm_sdi 0x10 // +#define RFM22_mmc2_dtmod_fifo 0x20 // +#define RFM22_mmc2_dtmod_pn9 0x30 // +#define RFM22_mmc2_trclk_mask 0xC0 // TX Data Clock Configuration. +#define RFM22_mmc2_trclk_clk_none 0x00 // +#define RFM22_mmc2_trclk_clk_gpio 0x40 // +#define RFM22_mmc2_trclk_clk_sdo 0x80 // +#define RFM22_mmc2_trclk_clk_nirq 0xC0 // -#define RFM22_frequency_deviation 0x72 // R/W +#define RFM22_frequency_deviation 0x72 // R/W -#define RFM22_frequency_offset1 0x73 // R/W -#define RFM22_frequency_offset2 0x74 // R/W +#define RFM22_frequency_offset1 0x73 // R/W +#define RFM22_frequency_offset2 0x74 // R/W -#define RFM22_frequency_band_select 0x75 // R/W -#define RFM22_fb_mask 0x1F -#define RFM22_fbs_hbsel 0x20 -#define RFM22_fbs_sbse 0x40 +#define RFM22_frequency_band_select 0x75 // R/W +#define RFM22_fb_mask 0x1F +#define RFM22_fbs_hbsel 0x20 +#define RFM22_fbs_sbse 0x40 -#define RFM22_nominal_carrier_frequency1 0x76 // R/W -#define RFM22_nominal_carrier_frequency0 0x77 // R/W +#define RFM22_nominal_carrier_frequency1 0x76 // R/W +#define RFM22_nominal_carrier_frequency0 0x77 // R/W -#define RFM22_frequency_hopping_channel_select 0x79 // R/W -#define RFM22_frequency_hopping_step_size 0x7A // R/W +#define RFM22_frequency_hopping_channel_select 0x79 // R/W +#define RFM22_frequency_hopping_step_size 0x7A // R/W -#define RFM22_tx_fifo_control1 0x7C // R/W .. TX FIFO Almost Full Threshold (0 - 63) -#define RFM22_tx_fifo_control1_mask 0x3F +#define RFM22_tx_fifo_control1 0x7C // R/W .. TX FIFO Almost Full Threshold (0 - 63) +#define RFM22_tx_fifo_control1_mask 0x3F -#define RFM22_tx_fifo_control2 0x7D // R/W .. TX FIFO Almost Empty Threshold (0 - 63) -#define RFM22_tx_fifo_control2_mask 0x3F +#define RFM22_tx_fifo_control2 0x7D // R/W .. TX FIFO Almost Empty Threshold (0 - 63) +#define RFM22_tx_fifo_control2_mask 0x3F -#define RFM22_rx_fifo_control 0x7E // R/W .. RX FIFO Almost Full Threshold (0 - 63) -#define RFM22_rx_fifo_control_mask 0x3F +#define RFM22_rx_fifo_control 0x7E // R/W .. RX FIFO Almost Full Threshold (0 - 63) +#define RFM22_rx_fifo_control_mask 0x3F -#define RFM22_fifo_access 0x7F // R/W +#define RFM22_fifo_access 0x7F // R/W // External type definitions -typedef int16_t (*t_rfm22_TxDataByteCallback) (void); -typedef bool (*t_rfm22_RxDataCallback) (void *data, uint8_t len); +typedef int16_t (*t_rfm22_TxDataByteCallback)(void); +typedef bool (*t_rfm22_RxDataCallback)(void *data, uint8_t len); enum pios_rfm22b_dev_magic { PIOS_RFM22B_DEV_MAGIC = 0x68e971b6, }; @@ -573,7 +573,7 @@ enum pios_radio_event { RADIO_EVENT_ERROR, RADIO_EVENT_FATAL_ERROR, - RADIO_EVENT_NUM_EVENTS // Must be last + RADIO_EVENT_NUM_EVENTS // Must be last }; enum pios_rfm22b_state { @@ -590,31 +590,31 @@ enum pios_rfm22b_state { #define RFM22B_RX_PACKET_STATS_LEN 4 enum pios_rfm22b_rx_packet_status { - RADIO_GOOD_RX_PACKET = 0x00, + RADIO_GOOD_RX_PACKET = 0x00, RADIO_CORRECTED_RX_PACKET = 0x01, - RADIO_ERROR_RX_PACKET = 0x2, - RADIO_RESENT_TX_PACKET = 0x3 + RADIO_ERROR_RX_PACKET = 0x2, + RADIO_RESENT_TX_PACKET = 0x3 }; typedef struct { uint32_t pairID; - int8_t rssi; - int8_t afc_correction; - uint8_t lastContact; + int8_t rssi; + int8_t afc_correction; + uint8_t lastContact; } rfm22b_pair_stats; typedef struct { uint32_t pairID; - OPLinkSettingsRemoteMainPortOptions main_port; + OPLinkSettingsRemoteMainPortOptions main_port; OPLinkSettingsRemoteFlexiPortOptions flexi_port; - OPLinkSettingsRemoteVCPPortOptions vcp_port; + OPLinkSettingsRemoteVCPPortOptions vcp_port; OPLinkSettingsComSpeedOptions com_speed; } rfm22b_binding; enum pios_rfm22b_chip_power_state { - RFM22B_IDLE_STATE = 0x00, - RFM22B_RX_STATE = 0x01, - RFM22B_TX_STATE = 0x10, + RFM22B_IDLE_STATE = 0x00, + RFM22B_RX_STATE = 0x01, + RFM22B_TX_STATE = 0x10, RFM22B_INVALID_STATE = 0x11 }; @@ -622,11 +622,11 @@ enum pios_rfm22b_chip_power_state { typedef union { struct { uint8_t state : 2; - bool frequency_error : 1; - bool header_error : 1; - bool rx_fifo_empty : 1; - bool fifo_underflow : 1; - bool fifo_overflow : 1; + bool frequency_error : 1; + bool header_error : 1; + bool rx_fifo_empty : 1; + bool fifo_underflow : 1; + bool fifo_overflow : 1; }; uint8_t raw; } rfm22b_device_status_reg; @@ -686,7 +686,7 @@ typedef struct { struct pios_rfm22b_dev { enum pios_rfm22b_dev_magic magic; - struct pios_rfm22b_cfg cfg; + struct pios_rfm22b_cfg cfg; // The SPI bus information uint32_t spi_id; @@ -700,10 +700,10 @@ struct pios_rfm22b_dev { // The list of bound radios. rfm22b_binding bindings[OPLINKSETTINGS_BINDINGS_NUMELEM]; - uint8_t cur_binding; + uint8_t cur_binding; // Is this device a coordinator? - bool coordinator; + bool coordinator; // The task handle xTaskHandle taskHandle; @@ -712,7 +712,7 @@ struct pios_rfm22b_dev { rfm22b_pair_stats pair_stats[OPLINKSTATUS_PAIRIDS_NUMELEM]; // ISR pending semaphore - xSemaphoreHandle isrPending; + xSemaphoreHandle isrPending; // The com configuration callback PIOS_RFM22B_ComConfigCallback com_config_cb; @@ -724,10 +724,10 @@ struct pios_rfm22b_dev { uint32_t tx_out_context; // the transmit power to use for data transmissions - uint8_t tx_power; + uint8_t tx_power; // The RF datarate lookup index. - uint8_t datarate; + uint8_t datarate; // The radio state machine state enum pios_radio_state state; @@ -746,13 +746,13 @@ struct pios_rfm22b_dev { enum pios_rfm22b_state rfm22b_state; // The packet statistics - struct rfm22b_stats stats; + struct rfm22b_stats stats; // Stats uint16_t errors; // RSSI in dBm - int8_t rssi_dBm; + int8_t rssi_dBm; // The tx data packet PHPacket data_packet; @@ -777,7 +777,7 @@ struct pios_rfm22b_dev { uint16_t rx_packet_len; // The status packet - PHStatusPacket status_packet; + PHStatusPacket status_packet; // The ACK/NACK packet PHAckNackPacket ack_nack_packet; @@ -791,21 +791,21 @@ struct pios_rfm22b_dev { PHConnectionPacket con_packet; // Send flags - bool send_status; - bool send_ppm; - bool send_connection_request; + bool send_status; + bool send_ppm; + bool send_connection_request; // The initial frequency - uint32_t init_frequency; + uint32_t init_frequency; // The number of frequency hopping channels. - uint16_t num_channels; + uint16_t num_channels; // The frequency hopping step size - float frequency_step_size; + float frequency_step_size; // current frequency hop channel - uint8_t frequency_hop_channel; + uint8_t frequency_hop_channel; // afc correction reading (in Hz) - int8_t afc_correction_Hz; + int8_t afc_correction_Hz; // The packet timers. portTickType packet_start_ticks; @@ -814,16 +814,16 @@ struct pios_rfm22b_dev { portTickType time_delta; // The maximum time (ms) that it should take to transmit / receive a packet. - uint32_t max_packet_time; + uint32_t max_packet_time; // The maximum time to wait for an ACK. - uint8_t max_ack_delay; + uint8_t max_ack_delay; #ifdef PIOS_INCLUDE_RFM22B_RCVR // The PPM channel values uint16_t ppm_channel[PIOS_RFM22B_RCVR_MAX_CHANNELS]; uint32_t ppm_supv_timer; - bool ppm_fresh; + bool ppm_fresh; #endif }; diff --git a/flight/pios/inc/pios_rtc.h b/flight/pios/inc/pios_rtc.h index 2e01912b9..196704927 100644 --- a/flight/pios/inc/pios_rtc.h +++ b/flight/pios/inc/pios_rtc.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_RTC RTC Functions * @{ * - * @file pios_rtc.h + * @file pios_rtc.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RTC functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/inc/pios_rtc_priv.h b/flight/pios/inc/pios_rtc_priv.h index 0a8c1c9b1..53c8a4024 100644 --- a/flight/pios/inc/pios_rtc_priv.h +++ b/flight/pios/inc/pios_rtc_priv.h @@ -35,12 +35,12 @@ #include struct pios_rtc_cfg { - uint32_t clksrc; - uint32_t prescaler; - struct stm32_irq irq; + uint32_t clksrc; + uint32_t prescaler; + struct stm32_irq irq; }; -extern void PIOS_RTC_Init(const struct pios_rtc_cfg * cfg); +extern void PIOS_RTC_Init(const struct pios_rtc_cfg *cfg); extern void PIOS_RTC_irq_handler(void); #endif /* PIOS_RTC_PRIV_H */ @@ -49,4 +49,3 @@ extern void PIOS_RTC_irq_handler(void); * @} * @} */ - diff --git a/flight/pios/inc/pios_sbus.h b/flight/pios/inc/pios_sbus.h index fd8580f2e..0c4dd6bc3 100644 --- a/flight/pios/inc/pios_sbus.h +++ b/flight/pios/inc/pios_sbus.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_SBus Futaba S.Bus receiver functions * @{ * - * @file pios_sbus.h + * @file pios_sbus.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. * @brief Futaba S.Bus functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/inc/pios_sbus_priv.h b/flight/pios/inc/pios_sbus_priv.h index b9aa88515..61a435e16 100644 --- a/flight/pios/inc/pios_sbus_priv.h +++ b/flight/pios/inc/pios_sbus_priv.h @@ -51,43 +51,43 @@ * 0xf0 - reserved * 1 byte - 0x00 (end of frame byte) */ -#define SBUS_FRAME_LENGTH (1+22+1+1) -#define SBUS_SOF_BYTE 0x0f -#define SBUS_EOF_BYTE 0x00 -#define SBUS_FLAG_DC1 0x01 -#define SBUS_FLAG_DC2 0x02 -#define SBUS_FLAG_FL 0x04 -#define SBUS_FLAG_FS 0x08 +#define SBUS_FRAME_LENGTH (1 + 22 + 1 + 1) +#define SBUS_SOF_BYTE 0x0f +#define SBUS_EOF_BYTE 0x00 +#define SBUS_FLAG_DC1 0x01 +#define SBUS_FLAG_DC2 0x02 +#define SBUS_FLAG_FL 0x04 +#define SBUS_FLAG_FS 0x08 /* * S.Bus protocol provides 16 proportional and 2 discrete channels. * Do not change unless driver code is updated accordingly. */ -#if (PIOS_SBUS_NUM_INPUTS != (16+2)) +#if (PIOS_SBUS_NUM_INPUTS != (16 + 2)) #error "S.Bus protocol provides 16 proportional and 2 discrete channels" #endif /* Discrete channels represented as bits, provide values for them */ -#define SBUS_VALUE_MIN 352 -#define SBUS_VALUE_MAX 1696 +#define SBUS_VALUE_MIN 352 +#define SBUS_VALUE_MAX 1696 /* * S.Bus configuration programmable invertor */ struct pios_sbus_cfg { - struct stm32_gpio inv; - void (*gpio_clk_func)(uint32_t periph, FunctionalState state); - uint32_t gpio_clk_periph; - BitAction gpio_inv_enable; - BitAction gpio_inv_disable; + struct stm32_gpio inv; + void (*gpio_clk_func)(uint32_t periph, FunctionalState state); + uint32_t gpio_clk_periph; + BitAction gpio_inv_enable; + BitAction gpio_inv_disable; }; extern const struct pios_rcvr_driver pios_sbus_rcvr_driver; extern int32_t PIOS_SBus_Init(uint32_t *sbus_id, - const struct pios_sbus_cfg *cfg, - const struct pios_com_driver *driver, - uint32_t lower_id); + const struct pios_sbus_cfg *cfg, + const struct pios_com_driver *driver, + uint32_t lower_id); #endif /* PIOS_SBUS_PRIV_H */ diff --git a/flight/pios/inc/pios_sdcard.h b/flight/pios/inc/pios_sdcard.h index df55d83f8..c428c8fc0 100644 --- a/flight/pios/inc/pios_sdcard.h +++ b/flight/pios/inc/pios_sdcard.h @@ -4,27 +4,27 @@ * @{ * @addtogroup PIOS_SDCARD SDCard Functions * @{ - * - * @file pios_sdcard.h + * + * @file pios_sdcard.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief System and hardware Init functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,56 +33,56 @@ /* Public Functions */ typedef struct { - uint8_t CSDStruct; /* CSD structure */ - uint8_t SysSpecVersion; /* System specification version */ - uint8_t Reserved1; /* Reserved */ - uint8_t TAAC; /* Data read access-time 1 */ - uint8_t NSAC; /* Data read access-time 2 in CLK cycles */ - uint8_t MaxBusClkFrec; /* Max. bus clock frequency */ - uint16_t CardComdClasses; /* Card command classes */ - uint8_t RdBlockLen; /* Max. read data block length */ - uint8_t PartBlockRead; /* Partial blocks for read allowed */ - uint8_t WrBlockMisalign; /* Write block misalignment */ - uint8_t RdBlockMisalign; /* Read block misalignment */ - uint8_t DSRImpl; /* DSR implemented */ - uint8_t Reserved2; /* Reserved */ - uint16_t DeviceSize; /* Device Size */ - uint8_t MaxRdCurrentVDDMin; /* Max. read current @ VDD min */ - uint8_t MaxRdCurrentVDDMax; /* Max. read current @ VDD max */ - uint8_t MaxWrCurrentVDDMin; /* Max. write current @ VDD min */ - uint8_t MaxWrCurrentVDDMax; /* Max. write current @ VDD max */ - uint8_t DeviceSizeMul; /* Device size multiplier */ - uint8_t EraseGrSize; /* Erase group size */ - uint8_t EraseGrMul; /* Erase group size multiplier */ - uint8_t WrProtectGrSize; /* Write protect group size */ - uint8_t WrProtectGrEnable; /* Write protect group enable */ - uint8_t ManDeflECC; /* Manufacturer default ECC */ - uint8_t WrSpeedFact; /* Write speed factor */ - uint8_t MaxWrBlockLen; /* Max. write data block length */ - uint8_t WriteBlockPaPartial; /* Partial blocks for write allowed */ - uint8_t Reserved3; /* Reserved */ - uint8_t ContentProtectAppli; /* Content protection application */ - uint8_t FileFormatGrouop; /* File format group */ - uint8_t CopyFlag; /* Copy flag (OTP) */ - uint8_t PermWrProtect; /* Permanent write protection */ - uint8_t TempWrProtect; /* Temporary write protection */ - uint8_t FileFormat; /* File Format */ - uint8_t ECC; /* ECC code */ - uint8_t msd_CRC; /* CRC */ - uint8_t Reserved4; /* always 1 */ + uint8_t CSDStruct; /* CSD structure */ + uint8_t SysSpecVersion; /* System specification version */ + uint8_t Reserved1; /* Reserved */ + uint8_t TAAC; /* Data read access-time 1 */ + uint8_t NSAC; /* Data read access-time 2 in CLK cycles */ + uint8_t MaxBusClkFrec; /* Max. bus clock frequency */ + uint16_t CardComdClasses; /* Card command classes */ + uint8_t RdBlockLen; /* Max. read data block length */ + uint8_t PartBlockRead; /* Partial blocks for read allowed */ + uint8_t WrBlockMisalign; /* Write block misalignment */ + uint8_t RdBlockMisalign; /* Read block misalignment */ + uint8_t DSRImpl; /* DSR implemented */ + uint8_t Reserved2; /* Reserved */ + uint16_t DeviceSize; /* Device Size */ + uint8_t MaxRdCurrentVDDMin; /* Max. read current @ VDD min */ + uint8_t MaxRdCurrentVDDMax; /* Max. read current @ VDD max */ + uint8_t MaxWrCurrentVDDMin; /* Max. write current @ VDD min */ + uint8_t MaxWrCurrentVDDMax; /* Max. write current @ VDD max */ + uint8_t DeviceSizeMul; /* Device size multiplier */ + uint8_t EraseGrSize; /* Erase group size */ + uint8_t EraseGrMul; /* Erase group size multiplier */ + uint8_t WrProtectGrSize; /* Write protect group size */ + uint8_t WrProtectGrEnable; /* Write protect group enable */ + uint8_t ManDeflECC; /* Manufacturer default ECC */ + uint8_t WrSpeedFact; /* Write speed factor */ + uint8_t MaxWrBlockLen; /* Max. write data block length */ + uint8_t WriteBlockPaPartial; /* Partial blocks for write allowed */ + uint8_t Reserved3; /* Reserved */ + uint8_t ContentProtectAppli; /* Content protection application */ + uint8_t FileFormatGrouop; /* File format group */ + uint8_t CopyFlag; /* Copy flag (OTP) */ + uint8_t PermWrProtect; /* Permanent write protection */ + uint8_t TempWrProtect; /* Temporary write protection */ + uint8_t FileFormat; /* File Format */ + uint8_t ECC; /* ECC code */ + uint8_t msd_CRC; /* CRC */ + uint8_t Reserved4; /* always 1 */ } SDCARDCsdTypeDef; /* Structure taken from Mass Storage Driver example provided by STM */ typedef struct { - uint8_t ManufacturerID; /* ManufacturerID */ - uint16_t OEM_AppliID; /* OEM/Application ID */ - char ProdName[6]; /* Product Name */ - uint8_t ProdRev; /* Product Revision */ - uint32_t ProdSN; /* Product Serial Number */ - uint8_t Reserved1; /* Reserved1 */ - uint16_t ManufactDate; /* Manufacturing Date */ - uint8_t msd_CRC; /* CRC */ - uint8_t Reserved2; /* always 1 */ + uint8_t ManufacturerID; /* ManufacturerID */ + uint16_t OEM_AppliID; /* OEM/Application ID */ + char ProdName[6]; /* Product Name */ + uint8_t ProdRev; /* Product Revision */ + uint32_t ProdSN; /* Product Serial Number */ + uint8_t Reserved1; /* Reserved1 */ + uint16_t ManufactDate; /* Manufacturing Date */ + uint8_t msd_CRC; /* CRC */ + uint8_t Reserved2; /* always 1 */ } SDCARDCidTypeDef; #ifndef USE_SIM_POSIX /* Global Variables */ @@ -95,18 +95,18 @@ extern int32_t PIOS_SDCARD_PowerOn(void); extern int32_t PIOS_SDCARD_PowerOff(void); extern int32_t PIOS_SDCARD_CheckAvailable(uint8_t was_available); extern int32_t PIOS_SDCARD_SendSDCCmd(uint8_t cmd, uint32_t addr, uint8_t crc); -extern int32_t PIOS_SDCARD_SectorRead(uint32_t sector, uint8_t * buffer); -extern int32_t PIOS_SDCARD_SectorWrite(uint32_t sector, uint8_t * buffer); -extern int32_t PIOS_SDCARD_CIDRead(SDCARDCidTypeDef * cid); -extern int32_t PIOS_SDCARD_CSDRead(SDCARDCsdTypeDef * csd); +extern int32_t PIOS_SDCARD_SectorRead(uint32_t sector, uint8_t *buffer); +extern int32_t PIOS_SDCARD_SectorWrite(uint32_t sector, uint8_t *buffer); +extern int32_t PIOS_SDCARD_CIDRead(SDCARDCidTypeDef *cid); +extern int32_t PIOS_SDCARD_CSDRead(SDCARDCsdTypeDef *csd); extern int32_t PIOS_SDCARD_StartupLog(void); extern int32_t PIOS_SDCARD_IsMounted(); extern int32_t PIOS_SDCARD_MountFS(uint32_t StartupLog); extern int32_t PIOS_SDCARD_GetFree(void); #ifndef USE_SIM_POSIX -extern int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t * buffer, uint32_t len); -extern int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t * buffer, uint32_t max_len); +extern int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len); +extern int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len); #endif extern int32_t PIOS_SDCARD_FileCopy(char *Source, char *Destination); extern int32_t PIOS_SDCARD_FileDelete(char *Filename); diff --git a/flight/pios/inc/pios_servo.h b/flight/pios/inc/pios_servo.h index 1a33149ea..fd905edbe 100644 --- a/flight/pios/inc/pios_servo.h +++ b/flight/pios/inc/pios_servo.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_SERVO RC Servo Functions * @{ * - * @file pios_servo.h + * @file pios_servo.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RC Servo functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,12 +31,12 @@ #define PIOS_SERVO_H /* Public Functions */ -extern void PIOS_Servo_SetHz(const uint16_t * update_rates, uint8_t banks); +extern void PIOS_Servo_SetHz(const uint16_t *update_rates, uint8_t banks); extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position); #endif /* PIOS_SERVO_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_servo_priv.h b/flight/pios/inc/pios_servo_priv.h index f5ed86e25..be692cc26 100644 --- a/flight/pios/inc/pios_servo_priv.h +++ b/flight/pios/inc/pios_servo_priv.h @@ -36,15 +36,15 @@ #include struct pios_servo_cfg { - TIM_TimeBaseInitTypeDef tim_base_init; - TIM_OCInitTypeDef tim_oc_init; - GPIO_InitTypeDef gpio_init; - uint32_t remap; - const struct pios_tim_channel * channels; - uint8_t num_channels; + TIM_TimeBaseInitTypeDef tim_base_init; + TIM_OCInitTypeDef tim_oc_init; + GPIO_InitTypeDef gpio_init; + uint32_t remap; + const struct pios_tim_channel *channels; + uint8_t num_channels; }; -extern int32_t PIOS_Servo_Init(const struct pios_servo_cfg * cfg); +extern int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg); #endif /* PIOS_SERVO_PRIV_H */ diff --git a/flight/pios/inc/pios_spi.h b/flight/pios/inc/pios_spi.h index 5740e1e67..93218d667 100644 --- a/flight/pios/inc/pios_spi.h +++ b/flight/pios/inc/pios_spi.h @@ -32,14 +32,14 @@ /* Global types */ typedef enum { - PIOS_SPI_PRESCALER_2 = 0, - PIOS_SPI_PRESCALER_4 = 1, - PIOS_SPI_PRESCALER_8 = 2, - PIOS_SPI_PRESCALER_16 = 3, - PIOS_SPI_PRESCALER_32 = 4, - PIOS_SPI_PRESCALER_64 = 5, - PIOS_SPI_PRESCALER_128 = 6, - PIOS_SPI_PRESCALER_256 = 7 + PIOS_SPI_PRESCALER_2 = 0, + PIOS_SPI_PRESCALER_4 = 1, + PIOS_SPI_PRESCALER_8 = 2, + PIOS_SPI_PRESCALER_16 = 3, + PIOS_SPI_PRESCALER_32 = 4, + PIOS_SPI_PRESCALER_64 = 5, + PIOS_SPI_PRESCALER_128 = 6, + PIOS_SPI_PRESCALER_256 = 7 } SPIPrescalerTypeDef; /* Public Functions */ diff --git a/flight/pios/inc/pios_spi_priv.h b/flight/pios/inc/pios_spi_priv.h index f89f52495..0a7f6a3da 100644 --- a/flight/pios/inc/pios_spi_priv.h +++ b/flight/pios/inc/pios_spi_priv.h @@ -35,35 +35,35 @@ #include struct pios_spi_cfg { - SPI_TypeDef *regs; - uint32_t remap; /* GPIO_Remap_* or GPIO_AF_* */ - SPI_InitTypeDef init; - bool use_crc; - struct stm32_dma dma; - struct stm32_gpio sclk; - struct stm32_gpio miso; - struct stm32_gpio mosi; - uint32_t slave_count; - struct stm32_gpio ssel[]; + SPI_TypeDef *regs; + uint32_t remap; /* GPIO_Remap_* or GPIO_AF_* */ + SPI_InitTypeDef init; + bool use_crc; + struct stm32_dma dma; + struct stm32_gpio sclk; + struct stm32_gpio miso; + struct stm32_gpio mosi; + uint32_t slave_count; + struct stm32_gpio ssel[]; }; struct pios_spi_dev { - const struct pios_spi_cfg * cfg; - void (*callback) (uint8_t, uint8_t); - uint8_t tx_dummy_byte; - uint8_t rx_dummy_byte; + const struct pios_spi_cfg *cfg; + void (*callback)(uint8_t, uint8_t); + uint8_t tx_dummy_byte; + uint8_t rx_dummy_byte; #if defined(PIOS_INCLUDE_FREERTOS) - xSemaphoreHandle busy; + xSemaphoreHandle busy; #else - uint8_t busy; + uint8_t busy; #endif }; -extern int32_t PIOS_SPI_Init(uint32_t * spi_id, const struct pios_spi_cfg * cfg); +extern int32_t PIOS_SPI_Init(uint32_t *spi_id, const struct pios_spi_cfg *cfg); #endif /* PIOS_SPI_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_stm32.h b/flight/pios/inc/pios_stm32.h index f9ca892e2..01aefe059 100644 --- a/flight/pios/inc/pios_stm32.h +++ b/flight/pios/inc/pios_stm32.h @@ -3,7 +3,7 @@ * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ * @addtogroup PIOS_STM32 STM32 HAL - * @brief STM32 specific global data structures + * @brief STM32 specific global data structures * @{ * * @file pios_stm32.h @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,40 +32,40 @@ #define PIOS_STM32_H struct stm32_irq { - void (*handler) (uint32_t); - uint32_t flags; - NVIC_InitTypeDef init; + void (*handler)(uint32_t); + uint32_t flags; + NVIC_InitTypeDef init; }; struct stm32_exti { - EXTI_InitTypeDef init; + EXTI_InitTypeDef init; }; struct stm32_dma_chan { #if defined(STM32F2XX) || defined(STM32F4XX) - DMA_Stream_TypeDef *channel; + DMA_Stream_TypeDef *channel; #else - DMA_Channel_TypeDef *channel; + DMA_Channel_TypeDef *channel; #endif - DMA_InitTypeDef init; + DMA_InitTypeDef init; }; struct stm32_dma { - uint32_t ahb_clk; /* ignored on STM32F2XX */ - struct stm32_irq irq; - struct stm32_dma_chan rx; - struct stm32_dma_chan tx; + uint32_t ahb_clk; /* ignored on STM32F2XX */ + struct stm32_irq irq; + struct stm32_dma_chan rx; + struct stm32_dma_chan tx; }; struct stm32_gpio { - GPIO_TypeDef *gpio; - GPIO_InitTypeDef init; - uint8_t pin_source; + GPIO_TypeDef *gpio; + GPIO_InitTypeDef init; + uint8_t pin_source; }; /** - * @} - * @} - */ + * @} + * @} + */ #endif /* PIOS_STM32_H */ diff --git a/flight/pios/inc/pios_struct_helper.h b/flight/pios/inc/pios_struct_helper.h index f061a6e01..1198b6fb1 100644 --- a/flight/pios/inc/pios_struct_helper.h +++ b/flight/pios/inc/pios_struct_helper.h @@ -11,8 +11,10 @@ #ifndef PIOS_STRUCT_HELPER_H #define PIOS_STRUCT_HELPER_H -#define container_of(ptr, type, member) ({ \ - const typeof( ((type *)0)->member ) *__mptr = (ptr); \ - (type *)( (char *)__mptr - offsetof(type,member) );}) +#define container_of(ptr, type, member) \ + ({ \ + const typeof(((type *)0)->member) * __mptr = (ptr); \ + (type *)((char *)__mptr - offsetof(type, member)); } \ + ) #endif /* PIOS_STRUCT_HELPER_H */ diff --git a/flight/pios/inc/pios_sys.h b/flight/pios/inc/pios_sys.h index 7f183a892..a67411db6 100644 --- a/flight/pios/inc/pios_sys.h +++ b/flight/pios/inc/pios_sys.h @@ -6,26 +6,26 @@ * @brief PIOS System Initialization code * @{ * - * @file pios_sys.h + * @file pios_sys.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief System and hardware Init functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,18 +33,18 @@ #define PIOS_SYS_H #define PIOS_SYS_SERIAL_NUM_BINARY_LEN 12 -#define PIOS_SYS_SERIAL_NUM_ASCII_LEN (PIOS_SYS_SERIAL_NUM_BINARY_LEN * 2) +#define PIOS_SYS_SERIAL_NUM_ASCII_LEN (PIOS_SYS_SERIAL_NUM_BINARY_LEN * 2) /* Public Functions */ extern void PIOS_SYS_Init(void); extern int32_t PIOS_SYS_Reset(void); extern uint32_t PIOS_SYS_getCPUFlashSize(void); extern int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t array[PIOS_SYS_SERIAL_NUM_BINARY_LEN]); -extern int32_t PIOS_SYS_SerialNumberGet(char str[PIOS_SYS_SERIAL_NUM_ASCII_LEN+1]); +extern int32_t PIOS_SYS_SerialNumberGet(char str[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]); #endif /* PIOS_SYS_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_task_monitor.h b/flight/pios/inc/pios_task_monitor.h index e969af3d8..ed3d1d519 100644 --- a/flight/pios/inc/pios_task_monitor.h +++ b/flight/pios/inc/pios_task_monitor.h @@ -71,22 +71,22 @@ extern int32_t PIOS_TASK_MONITOR_UnregisterTask(uint16_t task_id); */ extern bool PIOS_TASK_MONITOR_IsRunning(uint16_t task_id); -/** +/** * Information about a running task that has been registered * via a call to PIOS_TASK_MONITOR_Add(). */ struct pios_task_info { - /** Remaining task stack in bytes. */ - uint32_t stack_remaining; - /** Flag indicating whether or not the task is running. */ - bool is_running; - /** Percentage of cpu time used by the task since the last call - * to PIOS_TASK_MONITOR_ForEachTask(). Low-load tasks may - * report 0% load even though they have run during the interval. */ - uint8_t running_time_percentage; + /** Remaining task stack in bytes. */ + uint32_t stack_remaining; + /** Flag indicating whether or not the task is running. */ + bool is_running; + /** Percentage of cpu time used by the task since the last call + * to PIOS_TASK_MONITOR_ForEachTask(). Low-load tasks may + * report 0% load even though they have run during the interval. */ + uint8_t running_time_percentage; }; -/** +/** * Iterator callback, called for each monitored task by PIOS_TASK_MONITOR_TasksIterate(). * * @param task_id The id of the task the task_info refers to. diff --git a/flight/pios/inc/pios_tim.h b/flight/pios/inc/pios_tim.h index 0bad07f41..3ff1e5feb 100644 --- a/flight/pios/inc/pios_tim.h +++ b/flight/pios/inc/pios_tim.h @@ -1,4 +1,4 @@ #ifndef PIOS_TIM_H #define PIOS_TIM_H -#endif /* PIOS_TIM_H */ +#endif /* PIOS_TIM_H */ diff --git a/flight/pios/inc/pios_tim_priv.h b/flight/pios/inc/pios_tim_priv.h index 7f05719f8..e607f7033 100644 --- a/flight/pios/inc/pios_tim_priv.h +++ b/flight/pios/inc/pios_tim_priv.h @@ -4,25 +4,25 @@ #include struct pios_tim_clock_cfg { - TIM_TypeDef * timer; - const TIM_TimeBaseInitTypeDef * time_base_init; - struct stm32_irq irq; + TIM_TypeDef *timer; + const TIM_TimeBaseInitTypeDef *time_base_init; + struct stm32_irq irq; }; struct pios_tim_channel { - TIM_TypeDef * timer; - uint8_t timer_chan; + TIM_TypeDef *timer; + uint8_t timer_chan; - struct stm32_gpio pin; - uint32_t remap; + struct stm32_gpio pin; + uint32_t remap; }; struct pios_tim_callbacks { - void (*overflow)(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count); - void (*edge)(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count); + void (*overflow)(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count); + void (*edge)(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count); }; -extern int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg * cfg); -extern int32_t PIOS_TIM_InitChannels(uint32_t * tim_id, const struct pios_tim_channel * channels, uint8_t num_channels, const struct pios_tim_callbacks * callbacks, uint32_t context); +extern int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg *cfg); +extern int32_t PIOS_TIM_InitChannels(uint32_t *tim_id, const struct pios_tim_channel *channels, uint8_t num_channels, const struct pios_tim_callbacks *callbacks, uint32_t context); -#endif /* PIOS_TIM_PRIV_H */ +#endif /* PIOS_TIM_PRIV_H */ diff --git a/flight/pios/inc/pios_udp.h b/flight/pios/inc/pios_udp.h index 48a3b0987..28a095dce 100644 --- a/flight/pios/inc/pios_udp.h +++ b/flight/pios/inc/pios_udp.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_usart.h + * @file pios_usart.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief UDP functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/inc/pios_udp_priv.h b/flight/pios/inc/pios_udp_priv.h index 5ce32a01a..4105d9189 100644 --- a/flight/pios/inc/pios_udp_priv.h +++ b/flight/pios/inc/pios_udp_priv.h @@ -3,7 +3,7 @@ * * @file pios_udp_priv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief UDP private definitions. * @see The GNU Public License (GPL) Version 3 * @@ -39,36 +39,36 @@ #include struct pios_udp_cfg { - const char * ip; - uint16_t port; + const char *ip; + uint16_t port; }; typedef struct { - const struct pios_udp_cfg * cfg; + const struct pios_udp_cfg *cfg; #if defined(PIOS_INCLUDE_FREERTOS) - xTaskHandle rxThread; + xTaskHandle rxThread; #else - pthread_t rxThread; + pthread_t rxThread; #endif - int socket; - struct sockaddr_in server; - struct sockaddr_in client; - uint32_t clientLength; + int socket; + struct sockaddr_in server; + struct sockaddr_in client; + uint32_t clientLength; - pthread_cond_t cond; - pthread_mutex_t mutex; + pthread_cond_t cond; + pthread_mutex_t mutex; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; - uint8_t rx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; - uint8_t tx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; + uint8_t rx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; + uint8_t tx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; } pios_udp_dev; -extern int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg); +extern int32_t PIOS_UDP_Init(uint32_t *udp_id, const struct pios_udp_cfg *cfg); extern const struct pios_com_driver pios_udp_com_driver; diff --git a/flight/pios/inc/pios_usart.h b/flight/pios/inc/pios_usart.h index dd98a660f..379223a6f 100644 --- a/flight/pios/inc/pios_usart.h +++ b/flight/pios/inc/pios_usart.h @@ -6,25 +6,25 @@ * @brief PIOS interface for USART port * @{ * - * @file pios_usart.h + * @file pios_usart.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief USART functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,6 +37,6 @@ #endif /* PIOS_USART_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usart_priv.h b/flight/pios/inc/pios_usart_priv.h index defe3bb8a..52394e5f4 100644 --- a/flight/pios/inc/pios_usart_priv.h +++ b/flight/pios/inc/pios_usart_priv.h @@ -39,20 +39,20 @@ extern const struct pios_com_driver pios_usart_com_driver; struct pios_usart_cfg { - USART_TypeDef *regs; - uint32_t remap; /* GPIO_Remap_* */ - USART_InitTypeDef init; - struct stm32_gpio rx; - struct stm32_gpio tx; - struct stm32_irq irq; + USART_TypeDef *regs; + uint32_t remap; /* GPIO_Remap_* */ + USART_InitTypeDef init; + struct stm32_gpio rx; + struct stm32_gpio tx; + struct stm32_irq irq; }; -extern int32_t PIOS_USART_Init(uint32_t * usart_id, const struct pios_usart_cfg * cfg); -extern const struct pios_usart_cfg * PIOS_USART_GetConfig(uint32_t usart_id); +extern int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg); +extern const struct pios_usart_cfg *PIOS_USART_GetConfig(uint32_t usart_id); #endif /* PIOS_USART_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb.h b/flight/pios/inc/pios_usb.h index 24d1a0e3e..554f6b817 100644 --- a/flight/pios/inc/pios_usb.h +++ b/flight/pios/inc/pios_usb.h @@ -41,6 +41,6 @@ extern bool PIOS_USB_CheckAvailable(uint32_t id); #endif /* PIOS_USB_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_board_data_priv.h b/flight/pios/inc/pios_usb_board_data_priv.h index f4e7f7c05..9ee149ed1 100644 --- a/flight/pios/inc/pios_usb_board_data_priv.h +++ b/flight/pios/inc/pios_usb_board_data_priv.h @@ -11,19 +11,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,6 +35,6 @@ extern int32_t PIOS_USB_BOARD_DATA_Init(void); #endif /* PIOS_USB_BOARD_DATA_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_cdc_priv.h b/flight/pios/inc/pios_usb_cdc_priv.h index b87bd50cc..513303a47 100644 --- a/flight/pios/inc/pios_usb_cdc_priv.h +++ b/flight/pios/inc/pios_usb_cdc_priv.h @@ -32,21 +32,21 @@ #define PIOS_USB_CDC_PRIV_H struct pios_usb_cdc_cfg { - uint8_t ctrl_if; - uint8_t ctrl_tx_ep; + uint8_t ctrl_if; + uint8_t ctrl_tx_ep; - uint8_t data_if; - uint8_t data_rx_ep; - uint8_t data_tx_ep; + uint8_t data_if; + uint8_t data_rx_ep; + uint8_t data_tx_ep; }; extern const struct pios_com_driver pios_usb_cdc_com_driver; -extern int32_t PIOS_USB_CDC_Init(uint32_t * usbcdc_id, const struct pios_usb_cdc_cfg * cfg, uint32_t lower_id); +extern int32_t PIOS_USB_CDC_Init(uint32_t *usbcdc_id, const struct pios_usb_cdc_cfg *cfg, uint32_t lower_id); #endif /* PIOS_USB_CDC_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_defs.h b/flight/pios/inc/pios_usb_defs.h index 925e287ec..164d31fb3 100644 --- a/flight/pios/inc/pios_usb_defs.h +++ b/flight/pios/inc/pios_usb_defs.h @@ -31,62 +31,64 @@ #ifndef PIOS_USB_DEFS_H #define PIOS_USB_DEFS_H -#include /* uint*_t */ +#include /* uint*_t */ enum usb_desc_types { - USB_DESC_TYPE_DEVICE = 0x01, - USB_DESC_TYPE_CONFIGURATION = 0x02, - USB_DESC_TYPE_STRING = 0x03, - USB_DESC_TYPE_INTERFACE = 0x04, - USB_DESC_TYPE_ENDPOINT = 0x05, - USB_DESC_TYPE_IAD = 0x0B, - USB_DESC_TYPE_HID = 0x21, - USB_DESC_TYPE_REPORT = 0x22, - USB_DESC_TYPE_CLASS_SPECIFIC = 0x24, + USB_DESC_TYPE_DEVICE = 0x01, + USB_DESC_TYPE_CONFIGURATION = 0x02, + USB_DESC_TYPE_STRING = 0x03, + USB_DESC_TYPE_INTERFACE = 0x04, + USB_DESC_TYPE_ENDPOINT = 0x05, + USB_DESC_TYPE_IAD = 0x0B, + USB_DESC_TYPE_HID = 0x21, + USB_DESC_TYPE_REPORT = 0x22, + USB_DESC_TYPE_CLASS_SPECIFIC = 0x24, } __attribute__((packed)); enum usb_interface_class { - USB_INTERFACE_CLASS_CDC = 0x02, - USB_INTERFACE_CLASS_HID = 0x03, - USB_INTERFACE_CLASS_DATA = 0x0A, + USB_INTERFACE_CLASS_CDC = 0x02, + USB_INTERFACE_CLASS_HID = 0x03, + USB_INTERFACE_CLASS_DATA = 0x0A, } __attribute__((packed)); enum usb_cdc_desc_subtypes { - USB_CDC_DESC_SUBTYPE_HEADER = 0x00, - USB_CDC_DESC_SUBTYPE_CALLMGMT = 0x01, - USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL = 0x02, - USB_CDC_DESC_SUBTYPE_UNION = 0x06, + USB_CDC_DESC_SUBTYPE_HEADER = 0x00, + USB_CDC_DESC_SUBTYPE_CALLMGMT = 0x01, + USB_CDC_DESC_SUBTYPE_ABSTRACT_CTRL = 0x02, + USB_CDC_DESC_SUBTYPE_UNION = 0x06, } __attribute__((packed)); enum usb_ep_attr { - USB_EP_ATTR_TT_CONTROL = 0x00, - USB_EP_ATTR_TT_ISOCHRONOUS = 0x01, - USB_EP_ATTR_TT_BULK = 0x02, - USB_EP_ATTR_TT_INTERRUPT = 0x03, + USB_EP_ATTR_TT_CONTROL = 0x00, + USB_EP_ATTR_TT_ISOCHRONOUS = 0x01, + USB_EP_ATTR_TT_BULK = 0x02, + USB_EP_ATTR_TT_INTERRUPT = 0x03, } __attribute__((packed)); /* Standard macros to convert from host endian to USB endian (ie. little endian) */ #if __BIG_ENDIAN__ -#define htousbs(v) ((uint16_t)(\ - ((((v) >> 0) & 0xFF) << 8) | \ - ((((v) >> 8) & 0xFF) << 0))) -#define htousbl(v) ((uint32_t)(\ - ((((v) >> 0) & 0xFF) << 24) | \ - ((((v) >> 8) & 0xFF) << 16) | \ - ((((v) >> 16) & 0xFF) << 8) | \ - ((((v) >> 24) & 0xFF) << 0))) +#define htousbs(v) \ + ((uint16_t)( \ + ((((v) >> 0) & 0xFF) << 8) | \ + ((((v) >> 8) & 0xFF) << 0))) +#define htousbl(v) \ + ((uint32_t)( \ + ((((v) >> 0) & 0xFF) << 24) | \ + ((((v) >> 8) & 0xFF) << 16) | \ + ((((v) >> 16) & 0xFF) << 8) | \ + ((((v) >> 24) & 0xFF) << 0))) #else -#define htousbs(v) (v) -#define htousbl(v) (v) +#define htousbs(v) (v) +#define htousbl(v) (v) #endif -#define USB_EP_IN(ep) ((uint8_t) (0x80 | ((ep) & 0xF))) -#define USB_EP_OUT(ep) ((uint8_t) (0x00 | ((ep) & 0xF))) +#define USB_EP_IN(ep) ((uint8_t)(0x80 | ((ep) & 0xF))) +#define USB_EP_OUT(ep) ((uint8_t)(0x00 | ((ep) & 0xF))) -#define HID_ITEM_TYPE_MAIN 0x0 -#define HID_ITEM_TYPE_GLOBAL 0x1 -#define HID_ITEM_TYPE_LOCAL 0x2 -#define HID_ITEM_TYPE_RSVD 0x3 +#define HID_ITEM_TYPE_MAIN 0x0 +#define HID_ITEM_TYPE_GLOBAL 0x1 +#define HID_ITEM_TYPE_LOCAL 0x2 +#define HID_ITEM_TYPE_RSVD 0x3 #define HID_TAG_GLOBAL_USAGE_PAGE 0x0 /* 0b0000 */ #define HID_TAG_GLOBAL_LOGICAL_MIN 0x1 /* 0b0001 */ @@ -121,225 +123,226 @@ enum usb_ep_attr { #define HID_TAG_RSVD 0xF /* 0b1111 */ -#define HID_ITEM_SIZE_0 0 -#define HID_ITEM_SIZE_1 1 -#define HID_ITEM_SIZE_2 2 -#define HID_ITEM_SIZE_4 3 /* Yes, 4 bytes is represented with a size field = 3 */ +#define HID_ITEM_SIZE_0 0 +#define HID_ITEM_SIZE_1 1 +#define HID_ITEM_SIZE_2 2 +#define HID_ITEM_SIZE_4 3 /* Yes, 4 bytes is represented with a size field = 3 */ -#define HID_SHORT_ITEM(tag,type,size) (\ - (((tag) & 0xF) << 4) | \ - (((type) & 0x3) << 2) | \ - (((size) & 0x3) << 0)) +#define HID_SHORT_ITEM(tag, type, size) \ + ( \ + (((tag) & 0xF) << 4) | \ + (((type) & 0x3) << 2) | \ + (((size) & 0x3) << 0)) /* Long items have a fixed prefix */ -#define HID_LONG_ITEM HID_SHORT_ITEM(HID_TAG_RSVD, HID_ITEM_TYPE_RSVD, HID_ITEM_SIZE_2) +#define HID_LONG_ITEM HID_SHORT_ITEM(HID_TAG_RSVD, HID_ITEM_TYPE_RSVD, HID_ITEM_SIZE_2) -#define HID_MAIN_ITEM_0(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_0) -#define HID_MAIN_ITEM_1(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_1) -#define HID_MAIN_ITEM_2(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_2) -#define HID_MAIN_ITEM_4(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_4) +#define HID_MAIN_ITEM_0(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_0) +#define HID_MAIN_ITEM_1(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_1) +#define HID_MAIN_ITEM_2(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_2) +#define HID_MAIN_ITEM_4(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_MAIN, HID_ITEM_SIZE_4) #define HID_GLOBAL_ITEM_0(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_GLOBAL, HID_ITEM_SIZE_0) #define HID_GLOBAL_ITEM_1(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_GLOBAL, HID_ITEM_SIZE_1) #define HID_GLOBAL_ITEM_2(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_GLOBAL, HID_ITEM_SIZE_2) #define HID_GLOBAL_ITEM_4(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_GLOBAL, HID_ITEM_SIZE_4) -#define HID_LOCAL_ITEM_0(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_0) -#define HID_LOCAL_ITEM_1(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_1) -#define HID_LOCAL_ITEM_2(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_2) -#define HID_LOCAL_ITEM_4(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_4) +#define HID_LOCAL_ITEM_0(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_0) +#define HID_LOCAL_ITEM_1(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_1) +#define HID_LOCAL_ITEM_2(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_2) +#define HID_LOCAL_ITEM_4(tag) HID_SHORT_ITEM((tag), HID_ITEM_TYPE_LOCAL, HID_ITEM_SIZE_4) struct usb_device_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint16_t bcdUSB; - uint8_t bDeviceClass; - uint8_t bDeviceSubClass; - uint8_t bDeviceProtocol; - uint8_t bMaxPacketSize0; - uint16_t idVendor; - uint16_t idProduct; - uint16_t bcdDevice; - uint8_t iManufacturer; - uint8_t iProduct; - uint8_t iSerialNumber; - uint8_t bNumConfigurations; + uint8_t bLength; + uint8_t bDescriptorType; + uint16_t bcdUSB; + uint8_t bDeviceClass; + uint8_t bDeviceSubClass; + uint8_t bDeviceProtocol; + uint8_t bMaxPacketSize0; + uint16_t idVendor; + uint16_t idProduct; + uint16_t bcdDevice; + uint8_t iManufacturer; + uint8_t iProduct; + uint8_t iSerialNumber; + uint8_t bNumConfigurations; } __attribute__((packed)); struct usb_configuration_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint16_t wTotalLength; - uint8_t bNumInterfaces; - uint8_t bConfigurationValue; - uint8_t iConfiguration; - uint8_t bmAttributes; - uint8_t bMaxPower; + uint8_t bLength; + uint8_t bDescriptorType; + uint16_t wTotalLength; + uint8_t bNumInterfaces; + uint8_t bConfigurationValue; + uint8_t iConfiguration; + uint8_t bmAttributes; + uint8_t bMaxPower; } __attribute__((packed)); struct usb_interface_association_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bFirstInterface; - uint8_t bInterfaceCount; - uint8_t bFunctionClass; - uint8_t bFunctionSubClass; - uint8_t bFunctionProtocol; - uint8_t iInterface; + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bFirstInterface; + uint8_t bInterfaceCount; + uint8_t bFunctionClass; + uint8_t bFunctionSubClass; + uint8_t bFunctionProtocol; + uint8_t iInterface; } __attribute__((packed)); struct usb_interface_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bInterfaceNumber; - uint8_t bAlternateSetting; - uint8_t bNumEndpoints; - uint8_t bInterfaceClass; - uint8_t bInterfaceSubClass; - uint8_t nInterfaceProtocol; - uint8_t iInterface; + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bInterfaceNumber; + uint8_t bAlternateSetting; + uint8_t bNumEndpoints; + uint8_t bInterfaceClass; + uint8_t bInterfaceSubClass; + uint8_t nInterfaceProtocol; + uint8_t iInterface; } __attribute__((packed)); struct usb_hid_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint16_t bcdHID; - uint8_t bCountryCode; - uint8_t bNumDescriptors; - uint8_t bClassDescriptorType; - uint16_t wItemLength; + uint8_t bLength; + uint8_t bDescriptorType; + uint16_t bcdHID; + uint8_t bCountryCode; + uint8_t bNumDescriptors; + uint8_t bClassDescriptorType; + uint16_t wItemLength; } __attribute__((packed)); struct usb_endpoint_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bEndpointAddress; - uint8_t bmAttributes; - uint16_t wMaxPacketSize; - uint8_t bInterval; + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bEndpointAddress; + uint8_t bmAttributes; + uint16_t wMaxPacketSize; + uint8_t bInterval; } __attribute__((packed)); struct usb_setup_request { - uint8_t bmRequestType; - uint8_t bRequest; - uint16_t wValue; - uint16_t wIndex; - uint16_t wLength; + uint8_t bmRequestType; + uint8_t bRequest; + uint16_t wValue; + uint16_t wIndex; + uint16_t wLength; } __attribute__((packed)); -#define USB_REQ_TYPE_STANDARD 0x00 -#define USB_REQ_TYPE_CLASS 0x20 -#define USB_REQ_TYPE_VENDOR 0x40 -#define USB_REQ_TYPE_MASK 0x60 +#define USB_REQ_TYPE_STANDARD 0x00 +#define USB_REQ_TYPE_CLASS 0x20 +#define USB_REQ_TYPE_VENDOR 0x40 +#define USB_REQ_TYPE_MASK 0x60 -#define USB_REQ_RECIPIENT_DEVICE 0x00 -#define USB_REQ_RECIPIENT_INTERFACE 0x01 -#define USB_REQ_RECIPIENT_ENDPOINT 0x02 -#define USB_REQ_RECIPIENT_MASK 0x03 +#define USB_REQ_RECIPIENT_DEVICE 0x00 +#define USB_REQ_RECIPIENT_INTERFACE 0x01 +#define USB_REQ_RECIPIENT_ENDPOINT 0x02 +#define USB_REQ_RECIPIENT_MASK 0x03 enum usb_standard_requests { - USB_REQ_GET_STATUS = 0x00, - USB_REQ_CLEAR_FEATURE = 0x01, - /* what is 0x02? */ - USB_REQ_SET_FEATURE = 0x03, - /* what is 0x04? */ - USB_REQ_SET_ADDRESS = 0x05, - USB_REQ_GET_DESCRIPTOR = 0x06, - USB_REQ_SET_DESCRIPTOR = 0x07, - USB_REQ_GET_CONFIGURATION = 0x08, - USB_REQ_SET_CONFIGURATION = 0x09, - USB_REQ_GET_INTERFACE = 0x0A, - USB_REQ_SET_INTERFACE = 0x0B, - USB_REQ_SYNCH_FRAME = 0x0C, + USB_REQ_GET_STATUS = 0x00, + USB_REQ_CLEAR_FEATURE = 0x01, + /* what is 0x02? */ + USB_REQ_SET_FEATURE = 0x03, + /* what is 0x04? */ + USB_REQ_SET_ADDRESS = 0x05, + USB_REQ_GET_DESCRIPTOR = 0x06, + USB_REQ_SET_DESCRIPTOR = 0x07, + USB_REQ_GET_CONFIGURATION = 0x08, + USB_REQ_SET_CONFIGURATION = 0x09, + USB_REQ_GET_INTERFACE = 0x0A, + USB_REQ_SET_INTERFACE = 0x0B, + USB_REQ_SYNCH_FRAME = 0x0C, }; enum usb_hid_requests { - USB_HID_REQ_GET_REPORT = 0x01, - USB_HID_REQ_GET_IDLE = 0x02, - USB_HID_REQ_GET_PROTOCOL = 0x03, - /* 0x04-0x08 Reserved */ - USB_HID_REQ_SET_REPORT = 0x09, - USB_HID_REQ_SET_IDLE = 0x0A, - USB_HID_REQ_SET_PROTOCOL = 0x0B, + USB_HID_REQ_GET_REPORT = 0x01, + USB_HID_REQ_GET_IDLE = 0x02, + USB_HID_REQ_GET_PROTOCOL = 0x03, + /* 0x04-0x08 Reserved */ + USB_HID_REQ_SET_REPORT = 0x09, + USB_HID_REQ_SET_IDLE = 0x0A, + USB_HID_REQ_SET_PROTOCOL = 0x0B, }; enum usb_cdc_requests { - USB_CDC_REQ_SET_LINE_CODING = 0x20, - USB_CDC_REQ_GET_LINE_CODING = 0x21, + USB_CDC_REQ_SET_LINE_CODING = 0x20, + USB_CDC_REQ_GET_LINE_CODING = 0x21, - USB_CDC_REQ_SET_CONTROL_LINE_STATE = 0x22, + USB_CDC_REQ_SET_CONTROL_LINE_STATE = 0x22, }; struct usb_cdc_header_func_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bDescriptorSubType; - uint16_t bcdCDC; + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; + uint16_t bcdCDC; } __attribute__((packed)); struct usb_cdc_callmgmt_func_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bDescriptorSubType; - uint8_t bmCapabilities; - uint8_t bDataInterface; + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; + uint8_t bmCapabilities; + uint8_t bDataInterface; } __attribute__((packed)); struct usb_cdc_acm_func_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bDescriptorSubType; - uint8_t bmCapabilities; + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; + uint8_t bmCapabilities; } __attribute__((packed)); struct usb_cdc_union_func_desc { - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bDescriptorSubType; - uint8_t bMasterInterface; - uint8_t bSlaveInterface; + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; + uint8_t bMasterInterface; + uint8_t bSlaveInterface; } __attribute__((packed)); #define USB_LANGID_ENGLISH_US 0x0409 struct usb_string_langid { - uint8_t bLength; - uint8_t bDescriptorType; - uint16_t bLangID; + uint8_t bLength; + uint8_t bDescriptorType; + uint16_t bLangID; } __attribute__((packed)); struct usb_cdc_line_coding { - uint32_t dwDTERate; - uint8_t bCharFormat; - uint8_t bParityType; - uint8_t bDataBits; + uint32_t dwDTERate; + uint8_t bCharFormat; + uint8_t bParityType; + uint8_t bDataBits; } __attribute__((packed)); enum usb_cdc_line_coding_stop { - USB_CDC_LINE_CODING_STOP_1 = 0, - USB_CDC_LINE_CODING_STOP_1_5 = 1, - USB_CDC_LINE_CODING_STOP_2 = 2, + USB_CDC_LINE_CODING_STOP_1 = 0, + USB_CDC_LINE_CODING_STOP_1_5 = 1, + USB_CDC_LINE_CODING_STOP_2 = 2, } __attribute__((packed)); enum usb_cdc_line_coding_parity { - USB_CDC_LINE_CODING_PARITY_NONE = 0, - USB_CDC_LINE_CODING_PARITY_ODD = 1, - USB_CDC_LINE_CODING_PARITY_EVEN = 2, - USB_CDC_LINE_CODING_PARITY_MARK = 3, - USB_CDC_LINE_CODING_PARITY_SPACE = 4, + USB_CDC_LINE_CODING_PARITY_NONE = 0, + USB_CDC_LINE_CODING_PARITY_ODD = 1, + USB_CDC_LINE_CODING_PARITY_EVEN = 2, + USB_CDC_LINE_CODING_PARITY_MARK = 3, + USB_CDC_LINE_CODING_PARITY_SPACE = 4, } __attribute__((packed)); struct usb_cdc_serial_state_report { - uint8_t bmRequestType; - uint8_t bNotification; - uint16_t wValue; - uint16_t wIndex; - uint16_t wLength; - uint16_t bmUartState; + uint8_t bmRequestType; + uint8_t bNotification; + uint16_t wValue; + uint16_t wIndex; + uint16_t wLength; + uint16_t bmUartState; } __attribute__((packed)); enum usb_cdc_notification { - USB_CDC_NOTIFICATION_SERIAL_STATE = 0x20, + USB_CDC_NOTIFICATION_SERIAL_STATE = 0x20, } __attribute__((packed)); /* @@ -349,31 +352,32 @@ enum usb_cdc_notification { #define USB_VENDOR_ID_OPENPILOT 0x20A0 enum usb_product_ids { - USB_PRODUCT_ID_OPENPILOT_MAIN = 0x415A, - USB_PRODUCT_ID_COPTERCONTROL = 0x415B, - USB_PRODUCT_ID_OPLINK = 0x415C, - USB_PRODUCT_ID_CC3D = 0x415D, - USB_PRODUCT_ID_REVOLUTION = 0x415E, - USB_PRODUCT_ID_OSD = 0x4194, - USB_PRODUCT_ID_SPARE = 0x4195, + USB_PRODUCT_ID_OPENPILOT_MAIN = 0x415A, + USB_PRODUCT_ID_COPTERCONTROL = 0x415B, + USB_PRODUCT_ID_OPLINK = 0x415C, + USB_PRODUCT_ID_CC3D = 0x415D, + USB_PRODUCT_ID_REVOLUTION = 0x415E, + USB_PRODUCT_ID_OSD = 0x4194, + USB_PRODUCT_ID_SPARE = 0x4195, } __attribute__((packed)); enum usb_op_board_ids { - USB_OP_BOARD_ID_OPENPILOT_MAIN = 1, - /* Board ID 2 may be unused or AHRS */ - USB_OP_BOARD_ID_OPLINK = 3, - USB_OP_BOARD_ID_COPTERCONTROL = 4, - USB_OP_BOARD_ID_REVOLUTION = 5, - USB_OP_BOARD_ID_OSD = 6, + USB_OP_BOARD_ID_OPENPILOT_MAIN = 1, + /* Board ID 2 may be unused or AHRS */ + USB_OP_BOARD_ID_OPLINK = 3, + USB_OP_BOARD_ID_COPTERCONTROL = 4, + USB_OP_BOARD_ID_REVOLUTION = 5, + USB_OP_BOARD_ID_OSD = 6, } __attribute__((packed)); enum usb_op_board_modes { - USB_OP_BOARD_MODE_BL = 1, - USB_OP_BOARD_MODE_FW = 2, + USB_OP_BOARD_MODE_BL = 1, + USB_OP_BOARD_MODE_FW = 2, } __attribute__((packed)); -#define USB_OP_DEVICE_VER(board_id, board_mode) (\ - ((board_id & 0xFF) << 8) | \ - ((board_mode & 0xFF) << 0)) +#define USB_OP_DEVICE_VER(board_id, board_mode) \ + ( \ + ((board_id & 0xFF) << 8) | \ + ((board_mode & 0xFF) << 0)) #endif /* PIOS_USB_DEFS_H */ diff --git a/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h b/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h index 8a56d6b19..a828a1c6f 100644 --- a/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h +++ b/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h @@ -11,19 +11,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,6 +37,6 @@ extern int32_t PIOS_USB_DESC_HID_CDC_Init(void); #endif /* PIOS_USB_DESC_HID_CDC_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_desc_hid_only_priv.h b/flight/pios/inc/pios_usb_desc_hid_only_priv.h index 9e7d12401..6d98c3967 100644 --- a/flight/pios/inc/pios_usb_desc_hid_only_priv.h +++ b/flight/pios/inc/pios_usb_desc_hid_only_priv.h @@ -11,19 +11,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,6 +37,6 @@ extern int32_t PIOS_USB_DESC_HID_ONLY_Init(void); #endif /* PIOS_USB_DESC_HID_ONLY_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_hid.h b/flight/pios/inc/pios_usb_hid.h index 2f7a1d598..6d66a332d 100644 --- a/flight/pios/inc/pios_usb_hid.h +++ b/flight/pios/inc/pios_usb_hid.h @@ -35,12 +35,12 @@ extern int32_t PIOS_USB_HID_Reenumerate(void); extern int32_t PIOS_USB_HID_ChangeConnectionState(uint32_t Connected); extern bool PIOS_USB_HID_CheckAvailable(uint8_t id); -extern void PIOS_USB_HID_RegisterHidDescriptor(const uint8_t * desc, uint16_t length); -extern void PIOS_USB_HID_RegisterHidReport(const uint8_t * desc, uint16_t length); +extern void PIOS_USB_HID_RegisterHidDescriptor(const uint8_t *desc, uint16_t length); +extern void PIOS_USB_HID_RegisterHidReport(const uint8_t *desc, uint16_t length); #endif /* PIOS_USB_HID_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_hid_istr.h b/flight/pios/inc/pios_usb_hid_istr.h index 292680342..9ae0f7491 100644 --- a/flight/pios/inc/pios_usb_hid_istr.h +++ b/flight/pios/inc/pios_usb_hid_istr.h @@ -89,7 +89,7 @@ void ESOF_Callback(void); /* Interrupt subroutines user callbacks prototypes. These callbacks are called into the respective interrupt sunroutine functinos and can be tailored for various user application purposes. - Note: Make sure that the correspondant interrupt is enabled through the + Note: Make sure that the correspondant interrupt is enabled through the definition in usb_conf.h file */ void INTR_MODEMISMATCH_Callback(void); void INTR_SOFINTR_Callback(void); diff --git a/flight/pios/inc/pios_usb_hid_priv.h b/flight/pios/inc/pios_usb_hid_priv.h index fba585c18..30e99b2a6 100644 --- a/flight/pios/inc/pios_usb_hid_priv.h +++ b/flight/pios/inc/pios_usb_hid_priv.h @@ -32,18 +32,18 @@ #define PIOS_USB_HID_PRIV_H struct pios_usb_hid_cfg { - uint8_t data_if; - uint8_t data_rx_ep; - uint8_t data_tx_ep; + uint8_t data_if; + uint8_t data_rx_ep; + uint8_t data_tx_ep; }; extern const struct pios_com_driver pios_usb_hid_com_driver; -extern int32_t PIOS_USB_HID_Init(uint32_t * usbhid_id, const struct pios_usb_hid_cfg * cfg, uint32_t lower_id); +extern int32_t PIOS_USB_HID_Init(uint32_t *usbhid_id, const struct pios_usb_hid_cfg *cfg, uint32_t lower_id); #endif /* PIOS_USB_HID_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_hid_pwr.h b/flight/pios/inc/pios_usb_hid_pwr.h index 614344437..d1cb5da58 100644 --- a/flight/pios/inc/pios_usb_hid_pwr.h +++ b/flight/pios/inc/pios_usb_hid_pwr.h @@ -22,23 +22,23 @@ /* Includes ------------------------------------------------------------------*/ /* Exported types ------------------------------------------------------------*/ typedef enum _RESUME_STATE { - RESUME_EXTERNAL, - RESUME_INTERNAL, - RESUME_LATER, - RESUME_WAIT, - RESUME_START, - RESUME_ON, - RESUME_OFF, - RESUME_ESOF + RESUME_EXTERNAL, + RESUME_INTERNAL, + RESUME_LATER, + RESUME_WAIT, + RESUME_START, + RESUME_ON, + RESUME_OFF, + RESUME_ESOF } RESUME_STATE; typedef enum _DEVICE_STATE { - UNCONNECTED, - ATTACHED, - POWERED, - SUSPENDED, - ADDRESSED, - CONFIGURED + UNCONNECTED, + ATTACHED, + POWERED, + SUSPENDED, + ADDRESSED, + CONFIGURED } DEVICE_STATE; /* Exported constants --------------------------------------------------------*/ @@ -50,9 +50,9 @@ void Resume(RESUME_STATE eResumeSetVal); RESULT PowerOn(void); RESULT PowerOff(void); /* External variables --------------------------------------------------------*/ -extern __IO uint32_t bDeviceState; /* USB device status */ -extern __IO bool fSuspendEnabled; /* true when suspend is possible */ +extern __IO uint32_t bDeviceState; /* USB device status */ +extern __IO bool fSuspendEnabled; /* true when suspend is possible */ -#endif /*__USB_PWR_H*/ +#endif /*__USB_PWR_H*/ /******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/inc/pios_usb_priv.h b/flight/pios/inc/pios_usb_priv.h index ee3c26bd9..043671fd5 100644 --- a/flight/pios/inc/pios_usb_priv.h +++ b/flight/pios/inc/pios_usb_priv.h @@ -1,4 +1,4 @@ - /** +/** ****************************************************************************** * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ @@ -35,16 +35,16 @@ #include struct pios_usb_cfg { - struct stm32_irq irq; - struct stm32_gpio vsense; - bool vsense_active_low; + struct stm32_irq irq; + struct stm32_gpio vsense; + bool vsense_active_low; }; -extern int32_t PIOS_USB_Init(uint32_t * usb_id, const struct pios_usb_cfg * cfg); +extern int32_t PIOS_USB_Init(uint32_t *usb_id, const struct pios_usb_cfg *cfg); #endif /* PIOS_USB_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_rctx_priv.h b/flight/pios/inc/pios_usb_rctx_priv.h index cf9aab2b6..8074c3f1a 100644 --- a/flight/pios/inc/pios_usb_rctx_priv.h +++ b/flight/pios/inc/pios_usb_rctx_priv.h @@ -34,15 +34,15 @@ #include "pios_usb_rctx.h" struct pios_usb_rctx_cfg { - uint8_t data_if; - uint8_t data_tx_ep; + uint8_t data_if; + uint8_t data_tx_ep; }; -extern int32_t PIOS_USB_RCTX_Init(uint32_t * usbrctx_id, const struct pios_usb_rctx_cfg * cfg, uint32_t lower_id); +extern int32_t PIOS_USB_RCTX_Init(uint32_t *usbrctx_id, const struct pios_usb_rctx_cfg *cfg, uint32_t lower_id); #endif /* PIOS_USB_RCTX_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_usb_util.h b/flight/pios/inc/pios_usb_util.h index 111b49239..fe714db3d 100644 --- a/flight/pios/inc/pios_usb_util.h +++ b/flight/pios/inc/pios_usb_util.h @@ -31,8 +31,8 @@ #ifndef PIOS_USB_UTIL_H #define PIOS_USB_UTIL_H -#include /* uint8_t */ +#include /* uint8_t */ -uint8_t * PIOS_USB_UTIL_AsciiToUtf8(uint8_t * dst, uint8_t * src, uint16_t srclen); +uint8_t *PIOS_USB_UTIL_AsciiToUtf8(uint8_t *dst, uint8_t *src, uint16_t srclen); -#endif /* PIOS_USB_UTIL_H */ +#endif /* PIOS_USB_UTIL_H */ diff --git a/flight/pios/inc/pios_usbhook.h b/flight/pios/inc/pios_usbhook.h index d6ca8648f..8a923f824 100644 --- a/flight/pios/inc/pios_usbhook.h +++ b/flight/pios/inc/pios_usbhook.h @@ -33,32 +33,32 @@ #include #include -#include "pios_usb_defs.h" /* usb_setup_request */ +#include "pios_usb_defs.h" /* usb_setup_request */ struct pios_usbhook_descriptor { - const uint8_t * descriptor; - uint16_t length; + const uint8_t *descriptor; + uint16_t length; }; enum usb_string_desc { - USB_STRING_DESC_LANG = 0, - USB_STRING_DESC_VENDOR = 1, - USB_STRING_DESC_PRODUCT = 2, - USB_STRING_DESC_SERIAL = 3, + USB_STRING_DESC_LANG = 0, + USB_STRING_DESC_VENDOR = 1, + USB_STRING_DESC_PRODUCT = 2, + USB_STRING_DESC_SERIAL = 3, } __attribute__((packed)); -extern void PIOS_USBHOOK_RegisterDevice(const uint8_t * desc, uint16_t desc_size); -extern void PIOS_USBHOOK_RegisterConfig(uint8_t config_id, const uint8_t * desc, uint16_t desc_size); -extern void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t * desc, uint16_t desc_size); +extern void PIOS_USBHOOK_RegisterDevice(const uint8_t *desc, uint16_t desc_size); +extern void PIOS_USBHOOK_RegisterConfig(uint8_t config_id, const uint8_t *desc, uint16_t desc_size); +extern void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t *desc, uint16_t desc_size); struct pios_usb_ifops { - void (*init)(uint32_t context); - void (*deinit)(uint32_t context); - bool (*setup)(uint32_t context, struct usb_setup_request * req); - void (*ctrl_data_out)(uint32_t context, struct usb_setup_request * req); + void (*init)(uint32_t context); + void (*deinit)(uint32_t context); + bool (*setup)(uint32_t context, struct usb_setup_request *req); + void (*ctrl_data_out)(uint32_t context, struct usb_setup_request *req); }; -extern void PIOS_USBHOOK_RegisterIfOps(uint8_t ifnum, struct pios_usb_ifops * ifops, uint32_t context); +extern void PIOS_USBHOOK_RegisterIfOps(uint8_t ifnum, struct pios_usb_ifops *ifops, uint32_t context); typedef bool (*pios_usbhook_epcb)(uint32_t context, uint8_t epnum, uint16_t len); @@ -75,4 +75,3 @@ extern void PIOS_USBHOOK_Activate(void); extern void PIOS_USBHOOK_Deactivate(void); #endif /* PIOS_USBHOOK_H */ - diff --git a/flight/pios/inc/pios_video.h b/flight/pios/inc/pios_video.h index 1689b0a09..8781aa911 100644 --- a/flight/pios/inc/pios_video.h +++ b/flight/pios/inc/pios_video.h @@ -36,36 +36,36 @@ #include struct pios_video_cfg { - const struct pios_spi_cfg mask; - const struct pios_spi_cfg level; + const struct pios_spi_cfg mask; + const struct pios_spi_cfg level; - const struct pios_exti_cfg * hsync; - const struct pios_exti_cfg * vsync; + const struct pios_exti_cfg *hsync; + const struct pios_exti_cfg *vsync; - struct pios_tim_channel pixel_timer; - struct pios_tim_channel hsync_capture; + struct pios_tim_channel pixel_timer; + struct pios_tim_channel hsync_capture; - TIM_OCInitTypeDef tim_oc_init; + TIM_OCInitTypeDef tim_oc_init; }; // Time vars typedef struct { - uint8_t sec; - uint8_t min; - uint8_t hour; + uint8_t sec; + uint8_t min; + uint8_t hour; } TTime; extern TTime timex; -extern void PIOS_Video_Init(const struct pios_video_cfg * cfg); +extern void PIOS_Video_Init(const struct pios_video_cfg *cfg); uint16_t PIOS_Video_GetOSDLines(void); extern bool PIOS_Hsync_ISR(); extern bool PIOS_Vsync_ISR(); // First OSD line -#define GRAPHICS_LINE 25 +#define GRAPHICS_LINE 25 -//top/left deadband +// top/left deadband #define GRAPHICS_HDEADBAND 80 #define GRAPHICS_VDEADBAND 0 @@ -73,30 +73,30 @@ extern bool PIOS_Vsync_ISR(); // Real OSD size #ifdef PAL -//#define GRAPHICS_WIDTH_REAL (352+GRAPHICS_HDEADBAND) -#define GRAPHICS_WIDTH_REAL 416 - #define GRAPHICS_HEIGHT_REAL (270+GRAPHICS_VDEADBAND) +// #define GRAPHICS_WIDTH_REAL (352+GRAPHICS_HDEADBAND) +#define GRAPHICS_WIDTH_REAL 416 + #define GRAPHICS_HEIGHT_REAL (270 + GRAPHICS_VDEADBAND) #else - #define GRAPHICS_WIDTH_REAL (312+GRAPHICS_HDEADBAND) - #define GRAPHICS_HEIGHT_REAL (225+GRAPHICS_VDEADBAND) + #define GRAPHICS_WIDTH_REAL (312 + GRAPHICS_HDEADBAND) + #define GRAPHICS_HEIGHT_REAL (225 + GRAPHICS_VDEADBAND) #endif -//draw area -#define GRAPHICS_TOP 0 -#define GRAPHICS_LEFT 0 -#define GRAPHICS_BOTTOM (GRAPHICS_HEIGHT_REAL-GRAPHICS_VDEADBAND-1) -#define GRAPHICS_RIGHT (GRAPHICS_WIDTH_REAL-GRAPHICS_HDEADBAND-1) +// draw area +#define GRAPHICS_TOP 0 +#define GRAPHICS_LEFT 0 +#define GRAPHICS_BOTTOM (GRAPHICS_HEIGHT_REAL - GRAPHICS_VDEADBAND - 1) +#define GRAPHICS_RIGHT (GRAPHICS_WIDTH_REAL - GRAPHICS_HDEADBAND - 1) -#define GRAPHICS_WIDTH (GRAPHICS_WIDTH_REAL/8) -#define GRAPHICS_HEIGHT GRAPHICS_HEIGHT_REAL +#define GRAPHICS_WIDTH (GRAPHICS_WIDTH_REAL / 8) +#define GRAPHICS_HEIGHT GRAPHICS_HEIGHT_REAL // dma lenght -#define BUFFER_LINE_LENGTH (GRAPHICS_WIDTH) //Yes, in bytes. +#define BUFFER_LINE_LENGTH (GRAPHICS_WIDTH) // Yes, in bytes. // line types -#define LINE_TYPE_UNKNOWN 0 -#define LINE_TYPE_GRAPHICS 2 +#define LINE_TYPE_UNKNOWN 0 +#define LINE_TYPE_GRAPHICS 2 // Macro to swap buffers given a temporary pointer. #define SWAP_BUFFS(tmp, a, b) { tmp = a; a = b; b = tmp; } diff --git a/flight/pios/inc/pios_wavplay.h b/flight/pios/inc/pios_wavplay.h index 8d55ec5c4..e3fbb7757 100644 --- a/flight/pios/inc/pios_wavplay.h +++ b/flight/pios/inc/pios_wavplay.h @@ -34,19 +34,19 @@ #include -#define BUFFERSIZE 512 //Defines the buffer size to hold data from the SD card. +#define BUFFERSIZE 512 // Defines the buffer size to hold data from the SD card. struct pios_dac_cfg { - TIM_TypeDef *timer; - TIM_TimeBaseInitTypeDef time_base_init; - struct stm32_irq irq; - struct stm32_dma dma; - uint32_t channel; - DAC_InitTypeDef dac_init; - struct stm32_gpio dac_io; + TIM_TypeDef *timer; + TIM_TimeBaseInitTypeDef time_base_init; + struct stm32_irq irq; + struct stm32_dma dma; + uint32_t channel; + DAC_InitTypeDef dac_init; + struct stm32_gpio dac_io; }; -extern void PIOS_WavPlay_Init(const struct pios_dac_cfg * cfg); +extern void PIOS_WavPlay_Init(const struct pios_dac_cfg *cfg); extern uint8_t WavePlayer_Start(void); #endif /* PIOS_WAVPLAY_H */ diff --git a/flight/pios/inc/pios_wdg.h b/flight/pios/inc/pios_wdg.h index b8fa1bae5..d6583442e 100644 --- a/flight/pios/inc/pios_wdg.h +++ b/flight/pios/inc/pios_wdg.h @@ -7,7 +7,7 @@ * * @file pios_wdg.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief SPI functions header. * @see The GNU Public License (GPL) Version 3 * diff --git a/flight/pios/inc/stm32f10x_conf.h b/flight/pios/inc/stm32f10x_conf.h index dda33751a..1683690f4 100644 --- a/flight/pios/inc/stm32f10x_conf.h +++ b/flight/pios/inc/stm32f10x_conf.h @@ -1,22 +1,22 @@ /** - ****************************************************************************** - * @file Project/Template/stm32f10x_conf.h - * @author MCD Application Team - * @version V3.1.2 - * @date 09/28/2009 - * @brief Library configuration file. - ****************************************************************************** - * @copy - * - * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS - * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE - * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY - * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING - * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE - * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. - * - *

© COPYRIGHT 2009 STMicroelectronics

- */ + ****************************************************************************** + * @file Project/Template/stm32f10x_conf.h + * @author MCD Application Team + * @version V3.1.2 + * @date 09/28/2009 + * @brief Library configuration file. + ****************************************************************************** + * @copy + * + * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS + * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE + * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY + * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING + * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * + *

© COPYRIGHT 2009 STMicroelectronics

+ */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef __STM32F10x_CONF_H @@ -45,28 +45,28 @@ #include "stm32f10x_tim.h" #include "stm32f10x_usart.h" #include "stm32f10x_wwdg.h" -#include "misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */ +#include "misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */ /* Exported types ------------------------------------------------------------*/ /* Exported constants --------------------------------------------------------*/ -/* Uncomment the line below to expanse the "assert_param" macro in the +/* Uncomment the line below to expanse the "assert_param" macro in the Standard Peripheral Library drivers code */ -//#define USE_FULL_ASSERT 1 +// #define USE_FULL_ASSERT 1 /* Exported macro ------------------------------------------------------------*/ #ifdef USE_FULL_ASSERT /** -* The assert_param macro is used for function's parameters check. -* \param[in] expr: If expr is false, it calls assert_failed function -* which reports the name of the source file and the source -* line number of the call that failed. -* If expr is true, it returns no value. -* \retval None -*/ + * The assert_param macro is used for function's parameters check. + * \param[in] expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * \retval None + */ #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) /* Exported functions ------------------------------------------------------- */ -extern void assert_failed(uint8_t * file, uint32_t line); +extern void assert_failed(uint8_t *file, uint32_t line); #else #define assert_param(expr) ((void)0) #endif /* USE_FULL_ASSERT */ diff --git a/flight/pios/inc/stm32f2xx_conf.h b/flight/pios/inc/stm32f2xx_conf.h index bafb3715a..c1d4af1ce 100644 --- a/flight/pios/inc/stm32f2xx_conf.h +++ b/flight/pios/inc/stm32f2xx_conf.h @@ -1,23 +1,23 @@ /** - ****************************************************************************** - * @file Project/STM32F2xx_StdPeriph_Template/stm32f2xx_conf.h - * @author MCD Application Team - * @version V0.0.4 - * @date 13-January-2011 - * @brief Library configuration file. - ****************************************************************************** - * @attention - * - * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS - * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE - * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY - * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING - * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE - * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. - * - *

© COPYRIGHT 2011 STMicroelectronics

- ****************************************************************************** - */ + ****************************************************************************** + * @file Project/STM32F2xx_StdPeriph_Template/stm32f2xx_conf.h + * @author MCD Application Team + * @version V0.0.4 + * @date 13-January-2011 + * @brief Library configuration file. + ****************************************************************************** + * @attention + * + * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS + * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE + * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY + * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING + * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * + *

© COPYRIGHT 2011 STMicroelectronics

+ ****************************************************************************** + */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef __STM32F2xx_CONF_H @@ -26,25 +26,25 @@ /* Includes ------------------------------------------------------------------*/ /* Uncomment the line below to enable peripheral header file inclusion */ #include "stm32f2xx_adc.h" -//#include "stm32f2xx_can.h" +// #include "stm32f2xx_can.h" #include "stm32f2xx_crc.h" -//#include "stm32f2xx_cryp.h" +// #include "stm32f2xx_cryp.h" #include "stm32f2xx_dac.h" -//#include "stm32f2xx_dbgmcu.h" -//#include "stm32f2xx_dcmi.h" +// #include "stm32f2xx_dbgmcu.h" +// #include "stm32f2xx_dcmi.h" #include "stm32f2xx_dma.h" #include "stm32f2xx_exti.h" #include "stm32f2xx_flash.h" -//#include "stm32f2xx_fsmc.h" -//#include "stm32f2xx_hash.h" +// #include "stm32f2xx_fsmc.h" +// #include "stm32f2xx_hash.h" #include "stm32f2xx_gpio.h" #include "stm32f2xx_i2c.h" -//#include "stm32f2xx_iwdg.h" +// #include "stm32f2xx_iwdg.h" #include "stm32f2xx_pwr.h" #include "stm32f2xx_rcc.h" -//#include "stm32f2xx_rng.h" +// #include "stm32f2xx_rng.h" #include "stm32f2xx_rtc.h" -//#include "stm32f2xx_sdio.h" +// #include "stm32f2xx_sdio.h" #include "stm32f2xx_spi.h" #include "stm32f2xx_syscfg.h" #include "stm32f2xx_tim.h" @@ -55,13 +55,13 @@ /* Exported types ------------------------------------------------------------*/ /* Exported constants --------------------------------------------------------*/ -/* If an external clock source is used, then the value of the following define - should be set to the value of the external clock source, else, if no external +/* If an external clock source is used, then the value of the following define + should be set to the value of the external clock source, else, if no external clock is used, keep this define commented */ /*#define I2S_EXTERNAL_CLOCK_VAL 12288000 */ /* Value of the external clock in Hz */ -/* Uncomment the line below to expanse the "assert_param" macro in the +/* Uncomment the line below to expanse the "assert_param" macro in the Standard Peripheral Library drivers code */ /* #define USE_FULL_ASSERT 1 */ @@ -69,16 +69,16 @@ #ifdef USE_FULL_ASSERT /** - * @brief The assert_param macro is used for function's parameters check. - * @param expr: If expr is false, it calls assert_failed function - * which reports the name of the source file and the source - * line number of the call that failed. - * If expr is true, it returns no value. - * @retval None - */ + * @brief The assert_param macro is used for function's parameters check. + * @param expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) /* Exported functions ------------------------------------------------------- */ - void assert_failed(uint8_t* file, uint32_t line); +void assert_failed(uint8_t *file, uint32_t line); #else #define assert_param(expr) ((void)0) #endif /* USE_FULL_ASSERT */ diff --git a/flight/pios/inc/stm32f4xx_conf.h b/flight/pios/inc/stm32f4xx_conf.h index 4618691d2..d61e4f368 100644 --- a/flight/pios/inc/stm32f4xx_conf.h +++ b/flight/pios/inc/stm32f4xx_conf.h @@ -1,23 +1,23 @@ /** - ****************************************************************************** - * @file Project/STM32F4xx_StdPeriph_Template/stm32f4xx_conf.h - * @author MCD Application Team - * @version V0.0.4 - * @date 13-January-2011 - * @brief Library configuration file. - ****************************************************************************** - * @attention - * - * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS - * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE - * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY - * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING - * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE - * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. - * - *

© COPYRIGHT 2011 STMicroelectronics

- ****************************************************************************** - */ + ****************************************************************************** + * @file Project/STM32F4xx_StdPeriph_Template/stm32f4xx_conf.h + * @author MCD Application Team + * @version V0.0.4 + * @date 13-January-2011 + * @brief Library configuration file. + ****************************************************************************** + * @attention + * + * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS + * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE + * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY + * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING + * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * + *

© COPYRIGHT 2011 STMicroelectronics

+ ****************************************************************************** + */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef __STM32F4xx_CONF_H @@ -26,25 +26,25 @@ /* Includes ------------------------------------------------------------------*/ /* Uncomment the line below to enable peripheral header file inclusion */ #include "stm32f4xx_adc.h" -//#include "stm32f4xx_can.h" +// #include "stm32f4xx_can.h" #include "stm32f4xx_crc.h" -//#include "stm32f2xx_cryp.h" +// #include "stm32f2xx_cryp.h" #include "stm32f4xx_dac.h" -//#include "stm32f4xx_dbgmcu.h" -//#include "stm32f4xx_dcmi.h" +// #include "stm32f4xx_dbgmcu.h" +// #include "stm32f4xx_dcmi.h" #include "stm32f4xx_dma.h" #include "stm32f4xx_exti.h" #include "stm32f4xx_flash.h" -//#include "stm32f4xx_fsmc.h" -//#include "stm32f4xx_hash.h" +// #include "stm32f4xx_fsmc.h" +// #include "stm32f4xx_hash.h" #include "stm32f4xx_gpio.h" #include "stm32f4xx_i2c.h" -//#include "stm32f4xx_iwdg.h" +// #include "stm32f4xx_iwdg.h" #include "stm32f4xx_pwr.h" #include "stm32f4xx_rcc.h" -//#include "stm32f4xx_rng.h" +// #include "stm32f4xx_rng.h" #include "stm32f4xx_rtc.h" -//#include "stm32f4xx_sdio.h" +// #include "stm32f4xx_sdio.h" #include "stm32f4xx_spi.h" #include "stm32f4xx_syscfg.h" #include "stm32f4xx_tim.h" @@ -55,13 +55,13 @@ /* Exported types ------------------------------------------------------------*/ /* Exported constants --------------------------------------------------------*/ -/* If an external clock source is used, then the value of the following define - should be set to the value of the external clock source, else, if no external +/* If an external clock source is used, then the value of the following define + should be set to the value of the external clock source, else, if no external clock is used, keep this define commented */ /*#define I2S_EXTERNAL_CLOCK_VAL 12288000 */ /* Value of the external clock in Hz */ -/* Uncomment the line below to expanse the "assert_param" macro in the +/* Uncomment the line below to expanse the "assert_param" macro in the Standard Peripheral Library drivers code */ /* #define USE_FULL_ASSERT 1 */ @@ -69,16 +69,16 @@ #ifdef USE_FULL_ASSERT /** - * @brief The assert_param macro is used for function's parameters check. - * @param expr: If expr is false, it calls assert_failed function - * which reports the name of the source file and the source - * line number of the call that failed. - * If expr is true, it returns no value. - * @retval None - */ + * @brief The assert_param macro is used for function's parameters check. + * @param expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) /* Exported functions ------------------------------------------------------- */ - void assert_failed(uint8_t* file, uint32_t line); +void assert_failed(uint8_t *file, uint32_t line); #else #define assert_param(expr) ((void)0) #endif /* USE_FULL_ASSERT */ diff --git a/flight/pios/inc/usb_conf.h b/flight/pios/inc/usb_conf.h index f174eaf62..6c9ec9008 100644 --- a/flight/pios/inc/usb_conf.h +++ b/flight/pios/inc/usb_conf.h @@ -30,27 +30,27 @@ /*-------------------------------------------------------------*/ /* buffer table base address */ /* buffer table base address */ -#define BTABLE_ADDRESS (0x00) +#define BTABLE_ADDRESS (0x00) /* EP0 */ /* rx/tx buffer base address */ -#define ENDP0_RXADDR (0x20) -#define ENDP0_TXADDR (0x40) +#define ENDP0_RXADDR (0x20) +#define ENDP0_TXADDR (0x40) /* EP1 */ /* rx/tx buffer base address */ -#define ENDP1_TXADDR (0x60) -#define ENDP1_RXADDR (0x80) +#define ENDP1_TXADDR (0x60) +#define ENDP1_RXADDR (0x80) /* EP2 */ /* rx/tx buffer base address */ -#define ENDP2_TXADDR (0x100) -#define ENDP2_RXADDR (0x140) +#define ENDP2_TXADDR (0x100) +#define ENDP2_RXADDR (0x140) /* EP3 */ /* rx/tx buffer base address */ -#define ENDP3_TXADDR (0x180) -#define ENDP3_RXADDR (0x1C0) +#define ENDP3_TXADDR (0x180) +#define ENDP3_RXADDR (0x1C0) /*-------------------------------------------------------------*/ /* ------------------- ISTR events -------------------------*/ @@ -58,15 +58,16 @@ /* IMR_MSK */ /* mask defining which events has to be handled */ /* by the device application software */ -#define IMR_MSK (CNTR_CTRM | CNTR_WKUPM | CNTR_SUSPM | CNTR_ERRM | CNTR_SOFM \ - | CNTR_ESOFM | CNTR_RESETM ) +#define IMR_MSK \ + (CNTR_CTRM | CNTR_WKUPM | CNTR_SUSPM | CNTR_ERRM | CNTR_SOFM \ + | CNTR_ESOFM | CNTR_RESETM) #endif /* STM32F10X_CL */ #ifdef STM32F10X_CL /******************************************************************************* * FIFO Size Configuration -* +* * (i) Dedicated data FIFO SPRAM of 1.25 Kbytes = 1280 bytes = 320 32-bits words * available for the endpoints IN and OUT. * Device mode features: @@ -74,115 +75,115 @@ * -3 IN EPs to support any kind of Bulk, Interrupt or Isochronous transfer * -3 OUT EPs to support any kind of Bulk, Interrupt or Isochronous transfer * -* ii) Receive data FIFO size = RAM for setup packets + +* ii) Receive data FIFO size = RAM for setup packets + * OUT endpoint control information + * data OUT packets + miscellaneous * Space = ONE 32-bits words * --> RAM for setup packets = 4 * n + 6 space -* (n is the nbr of CTRL EPs the device core supports) +* (n is the nbr of CTRL EPs the device core supports) * --> OUT EP CTRL info = 1 space -* (one space for status information written to the FIFO along with each +* (one space for status information written to the FIFO along with each * received packet) -* --> data OUT packets = (Largest Packet Size / 4) + 1 spaces +* --> data OUT packets = (Largest Packet Size / 4) + 1 spaces * (MINIMUM to receive packets) -* --> OR data OUT packets = at least 2*(Largest Packet Size / 4) + 1 spaces +* --> OR data OUT packets = at least 2*(Largest Packet Size / 4) + 1 spaces * (if high-bandwidth EP is enabled or multiple isochronous EPs) * --> miscellaneous = 1 space per OUT EP -* (one space for transfer complete status information also pushed to the +* (one space for transfer complete status information also pushed to the * FIFO with each endpoint's last packet) * -* (iii)MINIMUM RAM space required for each IN EP Tx FIFO = MAX packet size for +* (iii)MINIMUM RAM space required for each IN EP Tx FIFO = MAX packet size for * that particular IN EP. More space allocated in the IN EP Tx FIFO results * in a better performance on the USB and can hide latencies on the AHB. * * (iv) TXn min size = 16 words. (n : Transmit FIFO index) -* (v) When a TxFIFO is not used, the Configuration should be as follows: +* (v) When a TxFIFO is not used, the Configuration should be as follows: * case 1 : n > m and Txn is not used (n,m : Transmit FIFO indexes) * --> Txm can use the space allocated for Txn. * case2 : n < m and Txn is not used (n,m : Transmit FIFO indexes) * --> Txn should be configured with the minimum space of 16 words -* (vi) The FIFO is used optimally when used TxFIFOs are allocated in the top +* (vi) The FIFO is used optimally when used TxFIFOs are allocated in the top * of the FIFO.Ex: use EP1 and EP2 as IN instead of EP1 and EP3 as IN ones. *******************************************************************************/ -#define RX_FIFO_SIZE 128 -#define TX0_FIFO_SIZE 64 -#define TX1_FIFO_SIZE 64 -#define TX2_FIFO_SIZE 16 -#define TX3_FIFO_SIZE 16 +#define RX_FIFO_SIZE 128 +#define TX0_FIFO_SIZE 64 +#define TX1_FIFO_SIZE 64 +#define TX2_FIFO_SIZE 16 +#define TX3_FIFO_SIZE 16 /* OTGD-FS-DEVICE IP interrupts Enable definitions */ /* Uncomment the define to enable the selected interrupt */ -//#define INTR_MODEMISMATCH +// #define INTR_MODEMISMATCH #define INTR_SOFINTR -#define INTR_RXSTSQLVL /* Mandatory */ -//#define INTR_NPTXFEMPTY -//#define INTR_GINNAKEFF -//#define INTR_GOUTNAKEFF -//#define INTR_ERLYSUSPEND -#define INTR_USBSUSPEND /* Mandatory */ -#define INTR_USBRESET /* Mandatory */ -#define INTR_ENUMDONE /* Mandatory */ -//#define INTR_ISOOUTDROP -//#define INTR_EOPFRAME -//#define INTR_EPMISMATCH -#define INTR_INEPINTR /* Mandatory */ -#define INTR_OUTEPINTR /* Mandatory */ -//#define INTR_INCOMPLISOIN -//#define INTR_INCOMPLISOOUT -#define INTR_WKUPINTR /* Mandatory */ +#define INTR_RXSTSQLVL /* Mandatory */ +// #define INTR_NPTXFEMPTY +// #define INTR_GINNAKEFF +// #define INTR_GOUTNAKEFF +// #define INTR_ERLYSUSPEND +#define INTR_USBSUSPEND /* Mandatory */ +#define INTR_USBRESET /* Mandatory */ +#define INTR_ENUMDONE /* Mandatory */ +// #define INTR_ISOOUTDROP +// #define INTR_EOPFRAME +// #define INTR_EPMISMATCH +#define INTR_INEPINTR /* Mandatory */ +#define INTR_OUTEPINTR /* Mandatory */ +// #define INTR_INCOMPLISOIN +// #define INTR_INCOMPLISOOUT +#define INTR_WKUPINTR /* Mandatory */ /* OTGD-FS-DEVICE IP interrupts subroutines */ /* Comment the define to enable the selected interrupt subroutine and replace it by user code */ -#define INTR_MODEMISMATCH_Callback NOP_Process -#define INTR_SOFINTR_Callback NOP_Process -#define INTR_RXSTSQLVL_Callback NOP_Process -#define INTR_NPTXFEMPTY_Callback NOP_Process -#define INTR_NPTXFEMPTY_Callback NOP_Process -#define INTR_GINNAKEFF_Callback NOP_Process -#define INTR_GOUTNAKEFF_Callback NOP_Process -#define INTR_ERLYSUSPEND_Callback NOP_Process -#define INTR_USBSUSPEND_Callback NOP_Process -#define INTR_USBRESET_Callback NOP_Process -#define INTR_ENUMDONE_Callback NOP_Process -#define INTR_ISOOUTDROP_Callback NOP_Process -#define INTR_EOPFRAME_Callback NOP_Process -#define INTR_EPMISMATCH_Callback NOP_Process -#define INTR_INEPINTR_Callback NOP_Process -#define INTR_OUTEPINTR_Callback NOP_Process -#define INTR_INCOMPLISOIN_Callback NOP_Process -#define INTR_INCOMPLISOOUT_Callback NOP_Process -#define INTR_WKUPINTR_Callback NOP_Process +#define INTR_MODEMISMATCH_Callback NOP_Process +#define INTR_SOFINTR_Callback NOP_Process +#define INTR_RXSTSQLVL_Callback NOP_Process +#define INTR_NPTXFEMPTY_Callback NOP_Process +#define INTR_NPTXFEMPTY_Callback NOP_Process +#define INTR_GINNAKEFF_Callback NOP_Process +#define INTR_GOUTNAKEFF_Callback NOP_Process +#define INTR_ERLYSUSPEND_Callback NOP_Process +#define INTR_USBSUSPEND_Callback NOP_Process +#define INTR_USBRESET_Callback NOP_Process +#define INTR_ENUMDONE_Callback NOP_Process +#define INTR_ISOOUTDROP_Callback NOP_Process +#define INTR_EOPFRAME_Callback NOP_Process +#define INTR_EPMISMATCH_Callback NOP_Process +#define INTR_INEPINTR_Callback NOP_Process +#define INTR_OUTEPINTR_Callback NOP_Process +#define INTR_INCOMPLISOIN_Callback NOP_Process +#define INTR_INCOMPLISOOUT_Callback NOP_Process +#define INTR_WKUPINTR_Callback NOP_Process /* Isochronous data update */ -#define INTR_RXSTSQLVL_ISODU_Callback NOP_Process +#define INTR_RXSTSQLVL_ISODU_Callback NOP_Process /* Isochronous transfer parameters */ /* Size of a single Isochronous buffer (size of a single transfer) */ -#define ISOC_BUFFER_SZE 1 +#define ISOC_BUFFER_SZE 1 /* Number of sub-buffers (number of single buffers/transfers), should be even */ -#define NUM_SUB_BUFFERS 2 +#define NUM_SUB_BUFFERS 2 #endif /* STM32F10X_CL */ /* CTR service routines */ /* associated to defined endpoints */ -#define EP1_IN_Callback NOP_Process -#define EP2_IN_Callback NOP_Process -#define EP3_IN_Callback NOP_Process -#define EP4_IN_Callback NOP_Process -#define EP5_IN_Callback NOP_Process -#define EP6_IN_Callback NOP_Process -#define EP7_IN_Callback NOP_Process +#define EP1_IN_Callback NOP_Process +#define EP2_IN_Callback NOP_Process +#define EP3_IN_Callback NOP_Process +#define EP4_IN_Callback NOP_Process +#define EP5_IN_Callback NOP_Process +#define EP6_IN_Callback NOP_Process +#define EP7_IN_Callback NOP_Process -#define EP1_OUT_Callback NOP_Process -#define EP2_OUT_Callback NOP_Process -#define EP3_OUT_Callback NOP_Process -#define EP4_OUT_Callback NOP_Process -#define EP5_OUT_Callback NOP_Process -#define EP6_OUT_Callback NOP_Process -#define EP7_OUT_Callback NOP_Process +#define EP1_OUT_Callback NOP_Process +#define EP2_OUT_Callback NOP_Process +#define EP3_OUT_Callback NOP_Process +#define EP4_OUT_Callback NOP_Process +#define EP5_OUT_Callback NOP_Process +#define EP6_OUT_Callback NOP_Process +#define EP7_OUT_Callback NOP_Process #endif /*__USB_CONF_H*/ diff --git a/flight/pios/osx/inc/FreeRTOSConfig.h b/flight/pios/osx/inc/FreeRTOSConfig.h index 46bb2dcb4..b2a58a77c 100644 --- a/flight/pios/osx/inc/FreeRTOSConfig.h +++ b/flight/pios/osx/inc/FreeRTOSConfig.h @@ -1,112 +1,110 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ -/* Notes: We use 5 task priorities */ +/* Notes: We use 5 task priorities */ -#ifdef __APPLE__ - #define COND_SIGNALING - #define CHECK_TASK_RESUMES - #define RUNNING_THREAD_MUTEX -// #define TICK_SIGNAL -// #define TICK_SIGWAIT - #define IDLE_SLEEPS +#ifdef __APPLE__ + #define COND_SIGNALING + #define CHECK_TASK_RESUMES + #define RUNNING_THREAD_MUTEX +// #define TICK_SIGNAL +// #define TICK_SIGWAIT + #define IDLE_SLEEPS - #define configUSE_PREEMPTION 1 - #define configIDLE_SHOULD_YIELD 0 + #define configUSE_PREEMPTION 1 + #define configIDLE_SHOULD_YIELD 0 #endif #ifdef __CYGWIN__ - #define COND_SIGNALING - #define CHECK_TASK_RESUMES -// #define RUNNING_THREAD_MUTEX -// #define TICK_SIGNAL - #define TICK_SIGWAIT - #define IDLE_SLEEPS + #define COND_SIGNALING + #define CHECK_TASK_RESUMES +// #define RUNNING_THREAD_MUTEX +// #define TICK_SIGNAL + #define TICK_SIGWAIT + #define IDLE_SLEEPS - #define configUSE_PREEMPTION 0 - #define configIDLE_SHOULD_YIELD 1 + #define configUSE_PREEMPTION 0 + #define configIDLE_SHOULD_YIELD 1 #endif #ifdef __linux__ - #define COND_SIGNALING - #define CHECK_TASK_RESUMES - #define RUNNING_THREAD_MUTEX -// #define TICK_SIGNAL -// #define TICK_SIGWAIT - #define IDLE_SLEEPS + #define COND_SIGNALING + #define CHECK_TASK_RESUMES + #define RUNNING_THREAD_MUTEX +// #define TICK_SIGNAL +// #define TICK_SIGWAIT + #define IDLE_SLEEPS - #define configUSE_PREEMPTION 1 - #define configIDLE_SHOULD_YIELD 0 + #define configUSE_PREEMPTION 1 + #define configIDLE_SHOULD_YIELD 0 #endif -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 ) -#define configTICK_RATE_HZ ( ( portTickType ) 1000 ) -#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) -#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 256 ) -#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 45 * 1024 ) ) -#define configMAX_TASK_NAME_LEN ( 16 ) -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)256) +#define configTOTAL_HEAP_SIZE ((size_t)(45 * 1024)) +#define configMAX_TASK_NAME_LEN (16) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ + to exclude the API function. */ -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 0 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 0 +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 0 /* Enable run time stats collection */ -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 #define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() #define portGET_RUN_TIME_COUNTER_VALUE() clock() #error Here /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 #endif /* FREERTOS_CONFIG_H */ - diff --git a/flight/pios/osx/inc/pios_bl_helper.h b/flight/pios/osx/inc/pios_bl_helper.h index e2e9e694e..88d67719f 100644 --- a/flight/pios/osx/inc/pios_bl_helper.h +++ b/flight/pios/osx/inc/pios_bl_helper.h @@ -37,7 +37,7 @@ extern uint8_t PIOS_BL_HELPER_FLASH_Ini(); extern uint32_t PIOS_BL_HELPER_CRC_Memory_Calc(); -extern void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t * array, uint8_t size); +extern void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t *array, uint8_t size); extern uint8_t PIOS_BL_HELPER_FLASH_Start(); diff --git a/flight/pios/osx/inc/pios_board_info.h b/flight/pios/osx/inc/pios_board_info.h index 3ff7aa886..2e549f090 100644 --- a/flight/pios/osx/inc/pios_board_info.h +++ b/flight/pios/osx/inc/pios_board_info.h @@ -1,22 +1,22 @@ #ifndef PIOS_BOARD_INFO_H #define PIOS_BOARD_INFO_H -#include /* uint* */ +#include /* uint* */ #define PIOS_BOARD_INFO_BLOB_MAGIC 0xBDBDBDBD struct pios_board_info { - uint32_t magic; - uint8_t board_type; - uint8_t board_rev; - uint8_t bl_rev; - uint8_t hw_type; - uint32_t fw_base; - uint32_t fw_size; - uint32_t desc_base; - uint32_t desc_size; - uint32_t ee_base; - uint32_t ee_size; + uint32_t magic; + uint8_t board_type; + uint8_t board_rev; + uint8_t bl_rev; + uint8_t hw_type; + uint32_t fw_base; + uint32_t fw_size; + uint32_t desc_base; + uint32_t desc_size; + uint32_t ee_base; + uint32_t ee_size; } __attribute__((packed)); extern const struct pios_board_info pios_board_info_blob; diff --git a/flight/pios/osx/inc/pios_com.h b/flight/pios/osx/inc/pios_com.h index badba0128..edac1e181 100644 --- a/flight/pios/osx/inc/pios_com.h +++ b/flight/pios/osx/inc/pios_com.h @@ -6,46 +6,46 @@ * @brief Hardware communication layer * @{ * - * @file pios_com.h + * @file pios_com.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM layer functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PIOS_COM_H #define PIOS_COM_H -typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * task_woken); +typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *task_woken); struct pios_com_driver { - void (*init)(uint32_t id); - void (*set_baud)(uint32_t id, uint32_t baud); - void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail); - void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); - void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); - void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); - bool (*available)(uint32_t id); + void (*init)(uint32_t id); + void (*set_baud)(uint32_t id, uint32_t baud); + void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail); + void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); + void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); + void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); + bool (*available)(uint32_t id); }; /* Public Functions */ -extern int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len); +extern int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len); extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud); extern int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c); extern int32_t PIOS_COM_SendChar(uint32_t com_id, char c); @@ -55,12 +55,12 @@ extern int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str); extern int32_t PIOS_COM_SendString(uint32_t com_id, const char *str); extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...); extern int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...); -extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms); +extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t *buf, uint16_t buf_len, uint32_t timeout_ms); extern bool PIOS_COM_Available(uint32_t com_id); #endif /* PIOS_COM_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_com_priv.h b/flight/pios/osx/inc/pios_com_priv.h index 54af82bcb..79caf2380 100644 --- a/flight/pios/osx/inc/pios_com_priv.h +++ b/flight/pios/osx/inc/pios_com_priv.h @@ -8,7 +8,7 @@ * * @file pios_com_priv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM private definitions. * @see The GNU Public License (GPL) Version 3 * @@ -39,6 +39,6 @@ extern int32_t PIOS_COM_ReceiveHandler(uint32_t com_id); #endif /* PIOS_COM_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_crc.h b/flight/pios/osx/inc/pios_crc.h index 3a64f8bab..5a737ade1 100644 --- a/flight/pios/osx/inc/pios_crc.h +++ b/flight/pios/osx/inc/pios_crc.h @@ -5,27 +5,27 @@ * @addtogroup PIOS_CRC CRC Functions * @{ * - * @file pios_crc.h + * @file pios_crc.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief CRC functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data); -uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length); +uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t *data, int32_t length); diff --git a/flight/pios/osx/inc/pios_debug.h b/flight/pios/osx/inc/pios_debug.h index e663d224e..1d4b88595 100644 --- a/flight/pios/osx/inc/pios_debug.h +++ b/flight/pios/osx/inc/pios_debug.h @@ -6,25 +6,25 @@ * @brief Debugging functionality * @{ * - * @file pios_i2c.h + * @file pios_i2c.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Debug helper functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,16 +41,18 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value); void PIOS_DEBUG_Panic(const char *msg); #ifdef DEBUG -#define PIOS_DEBUG_Assert(test) if (!(test)) PIOS_DEBUG_Panic(PIOS_DEBUG_AssertMsg); -#define PIOS_Assert(test) PIOS_DEBUG_Assert(test) +#define PIOS_DEBUG_Assert(test) if (!(test)) { PIOS_DEBUG_Panic(PIOS_DEBUG_AssertMsg); } +#define PIOS_Assert(test) PIOS_DEBUG_Assert(test) #else #define PIOS_DEBUG_Assert(test) -#define PIOS_Assert(test) if (!(test)) while (1); +#define PIOS_Assert(test) \ + if (!(test)) { while (1) {; } \ + } #endif #endif /* PIOS_DEBUG_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_delay.h b/flight/pios/osx/inc/pios_delay.h index 2039ce880..8b1caeed5 100644 --- a/flight/pios/osx/inc/pios_delay.h +++ b/flight/pios/osx/inc/pios_delay.h @@ -8,23 +8,23 @@ * * @file pios_settings.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Settings functions header + * @brief Settings functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,6 +43,6 @@ extern uint32_t PIOS_DELAY_DiffuS(uint32_t raw); #endif /* PIOS_DELAY_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_iap.h b/flight/pios/osx/inc/pios_iap.h index 5119711d5..a67ef11c4 100644 --- a/flight/pios/osx/inc/pios_iap.h +++ b/flight/pios/osx/inc/pios_iap.h @@ -1,5 +1,5 @@ /*! - * @File iap.h + * @File iap.h * @Brief Header file for the In-Application-Programming Module * * Created on: Sep 6, 2010 @@ -11,35 +11,35 @@ /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ /***************************************************************************************** * Public Definitions/Macros ****************************************************************************************/ #if defined(STM32F4XX) -#define MAGIC_REG_1 RTC_BKP_DR1 -#define MAGIC_REG_2 RTC_BKP_DR2 -#define IAP_BOOTCOUNT RTC_BKP_DR3 +#define MAGIC_REG_1 RTC_BKP_DR1 +#define MAGIC_REG_2 RTC_BKP_DR2 +#define IAP_BOOTCOUNT RTC_BKP_DR3 #else -#define MAGIC_REG_1 BKP_DR1 -#define MAGIC_REG_2 BKP_DR2 -#define IAP_BOOTCOUNT BKP_DR3 +#define MAGIC_REG_1 BKP_DR1 +#define MAGIC_REG_2 BKP_DR2 +#define IAP_BOOTCOUNT BKP_DR3 #endif /**************************************************************************************** - * Public Functions - ****************************************************************************************/ -void PIOS_IAP_Init(void); -uint32_t PIOS_IAP_CheckRequest( void ); -void PIOS_IAP_SetRequest1(void); -void PIOS_IAP_SetRequest2(void); -void PIOS_IAP_ClearRequest(void); -uint16_t PIOS_IAP_ReadBootCount(void); -void PIOS_IAP_WriteBootCount(uint16_t); +* Public Functions +****************************************************************************************/ +void PIOS_IAP_Init(void); +uint32_t PIOS_IAP_CheckRequest(void); +void PIOS_IAP_SetRequest1(void); +void PIOS_IAP_SetRequest2(void); +void PIOS_IAP_ClearRequest(void); +uint16_t PIOS_IAP_ReadBootCount(void); +void PIOS_IAP_WriteBootCount(uint16_t); /**************************************************************************************** - * Public Data - ****************************************************************************************/ +* Public Data +****************************************************************************************/ #endif /* PIOS_IAP_H_ */ diff --git a/flight/pios/osx/inc/pios_initcall.h b/flight/pios/osx/inc/pios_initcall.h index f88081c33..6b1c4421a 100644 --- a/flight/pios/osx/inc/pios_initcall.h +++ b/flight/pios/osx/inc/pios_initcall.h @@ -6,25 +6,25 @@ * @brief Initcall Macros * @{ * - * @file pios_initcall.h + * @file pios_initcall.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. * @brief Initcall header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,8 +41,8 @@ typedef int32_t (*initcall_t)(void); typedef struct { - initcall_t fn_minit; - initcall_t fn_tinit; + initcall_t fn_minit; + initcall_t fn_tinit; } initmodule_t; /* Init module section */ @@ -53,18 +53,20 @@ extern void StartModules(); #define MODULE_INITCALL(ifn, sfn) -#define MODULE_TASKCREATE_ALL { \ - /* Start all module threads */ \ - StartModules(); \ - } +#define MODULE_TASKCREATE_ALL \ + { \ + /* Start all module threads */ \ + StartModules(); \ + } -#define MODULE_INITIALISE_ALL { \ - /* Initialize modules */ \ - InitModules(); \ - /* Initialize the system thread */ \ - SystemModInitialize();} +#define MODULE_INITIALISE_ALL \ + { \ + /* Initialize modules */ \ + InitModules(); \ + /* Initialize the system thread */ \ + SystemModInitialize(); } -#endif /* PIOS_INITCALL_H */ +#endif /* PIOS_INITCALL_H */ /** * @} diff --git a/flight/pios/osx/inc/pios_irq.h b/flight/pios/osx/inc/pios_irq.h index f9b9dbb97..e934ae1f6 100644 --- a/flight/pios/osx/inc/pios_irq.h +++ b/flight/pios/osx/inc/pios_irq.h @@ -5,26 +5,26 @@ * @addtogroup PIOS_IRQ IRQ Setup Functions * @{ * - * @file pios_irq.h + * @file pios_irq.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief IRQ functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/osx/inc/pios_led.h b/flight/pios/osx/inc/pios_led.h index ba56b1f4a..92ed0f70e 100644 --- a/flight/pios/osx/inc/pios_led.h +++ b/flight/pios/osx/inc/pios_led.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_LED LED Functions * @{ * - * @file pios_led.h + * @file pios_led.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief LED functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/osx/inc/pios_posix.h b/flight/pios/osx/inc/pios_posix.h index b458f77b6..99dcbc001 100644 --- a/flight/pios/osx/inc/pios_posix.h +++ b/flight/pios/osx/inc/pios_posix.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file posix.h + * @file posix.h * @author Corvus Corax Copyright (C) 2010. * @brief Definitions to run PiOS on posix * @see The GNU Public License (GPL) Version 2 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -28,17 +28,16 @@ #include -typedef enum {FALSE = 0, TRUE = !FALSE} bool; +typedef enum { FALSE = 0, TRUE = !FALSE } bool; #ifndef false - #define false FALSE - #define true TRUE + #define false FALSE + #define true TRUE #endif -#define FILEINFO FILE* +#define FILEINFO FILE * #define PIOS_SERVO_NUM_OUTPUTS 8 -#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS +#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS #endif - diff --git a/flight/pios/osx/inc/pios_rcvr.h b/flight/pios/osx/inc/pios_rcvr.h index 472a4b33d..274ec80b9 100644 --- a/flight/pios/osx/inc/pios_rcvr.h +++ b/flight/pios/osx/inc/pios_rcvr.h @@ -6,25 +6,25 @@ * @brief Hardware communication layer * @{ * - * @file pios_rcvr.h + * @file pios_rcvr.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RCVR layer functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,15 +32,15 @@ #define PIOS_RCVR_H struct pios_rcvr_channel_map { - uint32_t id; - uint8_t channel; + uint32_t id; + uint8_t channel; }; extern struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[]; struct pios_rcvr_driver { - void (*init)(uint32_t id); - int32_t (*read)(uint32_t id, uint8_t channel); + void (*init)(uint32_t id); + int32_t (*read)(uint32_t id, uint8_t channel); }; /* Public Functions */ @@ -48,17 +48,17 @@ extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel); /*! Define error codes for PIOS_RCVR_Get */ enum PIOS_RCVR_errors { - /*! Indicates that a failsafe condition or missing receiver detected for that channel */ - PIOS_RCVR_TIMEOUT = 0, - /*! Channel is invalid for this driver (usually out of range supported) */ - PIOS_RCVR_INVALID = -1, - /*! Indicates that the driver for this channel has not been initialized */ - PIOS_RCVR_NODRIVER = -2 + /*! Indicates that a failsafe condition or missing receiver detected for that channel */ + PIOS_RCVR_TIMEOUT = 0, + /*! Channel is invalid for this driver (usually out of range supported) */ + PIOS_RCVR_INVALID = -1, + /*! Indicates that the driver for this channel has not been initialized */ + PIOS_RCVR_NODRIVER = -2 }; #endif /* PIOS_RCVR_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_rcvr_priv.h b/flight/pios/osx/inc/pios_rcvr_priv.h index 968dc2116..bde247d33 100644 --- a/flight/pios/osx/inc/pios_rcvr_priv.h +++ b/flight/pios/osx/inc/pios_rcvr_priv.h @@ -8,7 +8,7 @@ * * @file pios_rcvr_priv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief USART private definitions. * @see The GNU Public License (GPL) Version 3 * @@ -36,13 +36,13 @@ extern uint32_t pios_rcvr_max_channel; -extern int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id); +extern int32_t PIOS_RCVR_Init(uint32_t *rcvr_id, const struct pios_rcvr_driver *driver, const uint32_t lower_id); extern void PIOS_RCVR_IRQ_Handler(uint32_t rcvr_id); #endif /* PIOS_RCVR_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_sdcard.h b/flight/pios/osx/inc/pios_sdcard.h index 2927def2d..72bd9d042 100644 --- a/flight/pios/osx/inc/pios_sdcard.h +++ b/flight/pios/osx/inc/pios_sdcard.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_sdcard.h + * @file pios_sdcard.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief System and hardware Init functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,61 +31,61 @@ /* Public Functions */ typedef struct { - uint8_t CSDStruct; /* CSD structure */ - uint8_t SysSpecVersion; /* System specification version */ - uint8_t Reserved1; /* Reserved */ - uint8_t TAAC; /* Data read access-time 1 */ - uint8_t NSAC; /* Data read access-time 2 in CLK cycles */ - uint8_t MaxBusClkFrec; /* Max. bus clock frequency */ - uint16_t CardComdClasses; /* Card command classes */ - uint8_t RdBlockLen; /* Max. read data block length */ - uint8_t PartBlockRead; /* Partial blocks for read allowed */ - uint8_t WrBlockMisalign; /* Write block misalignment */ - uint8_t RdBlockMisalign; /* Read block misalignment */ - uint8_t DSRImpl; /* DSR implemented */ - uint8_t Reserved2; /* Reserved */ - uint16_t DeviceSize; /* Device Size */ - uint8_t MaxRdCurrentVDDMin; /* Max. read current @ VDD min */ - uint8_t MaxRdCurrentVDDMax; /* Max. read current @ VDD max */ - uint8_t MaxWrCurrentVDDMin; /* Max. write current @ VDD min */ - uint8_t MaxWrCurrentVDDMax; /* Max. write current @ VDD max */ - uint8_t DeviceSizeMul; /* Device size multiplier */ - uint8_t EraseGrSize; /* Erase group size */ - uint8_t EraseGrMul; /* Erase group size multiplier */ - uint8_t WrProtectGrSize; /* Write protect group size */ - uint8_t WrProtectGrEnable; /* Write protect group enable */ - uint8_t ManDeflECC; /* Manufacturer default ECC */ - uint8_t WrSpeedFact; /* Write speed factor */ - uint8_t MaxWrBlockLen; /* Max. write data block length */ - uint8_t WriteBlockPaPartial; /* Partial blocks for write allowed */ - uint8_t Reserved3; /* Reserved */ - uint8_t ContentProtectAppli; /* Content protection application */ - uint8_t FileFormatGrouop; /* File format group */ - uint8_t CopyFlag; /* Copy flag (OTP) */ - uint8_t PermWrProtect; /* Permanent write protection */ - uint8_t TempWrProtect; /* Temporary write protection */ - uint8_t FileFormat; /* File Format */ - uint8_t ECC; /* ECC code */ - uint8_t msd_CRC; /* CRC */ - uint8_t Reserved4; /* always 1*/ + uint8_t CSDStruct; /* CSD structure */ + uint8_t SysSpecVersion; /* System specification version */ + uint8_t Reserved1; /* Reserved */ + uint8_t TAAC; /* Data read access-time 1 */ + uint8_t NSAC; /* Data read access-time 2 in CLK cycles */ + uint8_t MaxBusClkFrec; /* Max. bus clock frequency */ + uint16_t CardComdClasses; /* Card command classes */ + uint8_t RdBlockLen; /* Max. read data block length */ + uint8_t PartBlockRead; /* Partial blocks for read allowed */ + uint8_t WrBlockMisalign; /* Write block misalignment */ + uint8_t RdBlockMisalign; /* Read block misalignment */ + uint8_t DSRImpl; /* DSR implemented */ + uint8_t Reserved2; /* Reserved */ + uint16_t DeviceSize; /* Device Size */ + uint8_t MaxRdCurrentVDDMin; /* Max. read current @ VDD min */ + uint8_t MaxRdCurrentVDDMax; /* Max. read current @ VDD max */ + uint8_t MaxWrCurrentVDDMin; /* Max. write current @ VDD min */ + uint8_t MaxWrCurrentVDDMax; /* Max. write current @ VDD max */ + uint8_t DeviceSizeMul; /* Device size multiplier */ + uint8_t EraseGrSize; /* Erase group size */ + uint8_t EraseGrMul; /* Erase group size multiplier */ + uint8_t WrProtectGrSize; /* Write protect group size */ + uint8_t WrProtectGrEnable; /* Write protect group enable */ + uint8_t ManDeflECC; /* Manufacturer default ECC */ + uint8_t WrSpeedFact; /* Write speed factor */ + uint8_t MaxWrBlockLen; /* Max. write data block length */ + uint8_t WriteBlockPaPartial; /* Partial blocks for write allowed */ + uint8_t Reserved3; /* Reserved */ + uint8_t ContentProtectAppli; /* Content protection application */ + uint8_t FileFormatGrouop; /* File format group */ + uint8_t CopyFlag; /* Copy flag (OTP) */ + uint8_t PermWrProtect; /* Permanent write protection */ + uint8_t TempWrProtect; /* Temporary write protection */ + uint8_t FileFormat; /* File Format */ + uint8_t ECC; /* ECC code */ + uint8_t msd_CRC; /* CRC */ + uint8_t Reserved4; /* always 1*/ } SDCARDCsdTypeDef; /* Structure taken from Mass Storage Driver example provided by STM */ typedef struct { - uint8_t ManufacturerID; /* ManufacturerID */ - uint16_t OEM_AppliID; /* OEM/Application ID */ - char ProdName[6]; /* Product Name */ - uint8_t ProdRev; /* Product Revision */ - uint32_t ProdSN; /* Product Serial Number */ - uint8_t Reserved1; /* Reserved1 */ - uint16_t ManufactDate; /* Manufacturing Date */ - uint8_t msd_CRC; /* CRC */ - uint8_t Reserved2; /* always 1*/ + uint8_t ManufacturerID; /* ManufacturerID */ + uint16_t OEM_AppliID; /* OEM/Application ID */ + char ProdName[6]; /* Product Name */ + uint8_t ProdRev; /* Product Revision */ + uint32_t ProdSN; /* Product Serial Number */ + uint8_t Reserved1; /* Reserved1 */ + uint16_t ManufactDate; /* Manufacturing Date */ + uint8_t msd_CRC; /* CRC */ + uint8_t Reserved2; /* always 1*/ } SDCARDCidTypeDef; /* Global Variables */ -//extern VOLINFO PIOS_SDCARD_VolInfo; -//extern uint8_t PIOS_SDCARD_Sector[SECTOR_SIZE]; +// extern VOLINFO PIOS_SDCARD_VolInfo; +// extern uint8_t PIOS_SDCARD_Sector[SECTOR_SIZE]; /* Prototypes */ extern int32_t PIOS_SDCARD_Init(void); @@ -103,11 +103,11 @@ extern int32_t PIOS_SDCARD_IsMounted(); extern int32_t PIOS_SDCARD_MountFS(uint32_t StartupLog); extern int32_t PIOS_SDCARD_GetFree(void); -//extern int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len); -//extern int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len); +// extern int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len); +// extern int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len); extern int32_t PIOS_SDCARD_FileCopy(char *Source, char *Destination); extern int32_t PIOS_SDCARD_FileDelete(char *Filename); -#endif +#endif // if defined(PIOS_INCLUDE_SDCARD) #endif /* PIOS_SDCARD_H */ diff --git a/flight/pios/osx/inc/pios_servo.h b/flight/pios/osx/inc/pios_servo.h index 952baa5a0..5e10b57d8 100644 --- a/flight/pios/osx/inc/pios_servo.h +++ b/flight/pios/osx/inc/pios_servo.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_SERVO RC Servo Functions * @{ * - * @file pios_servo.h + * @file pios_servo.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RC Servo functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,12 +32,12 @@ /* Public Functions */ extern void PIOS_Servo_Init(void); -extern void PIOS_Servo_SetHz(const uint16_t * speeds, uint8_t num_banks); +extern void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t num_banks); extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position); #endif /* PIOS_SERVO_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_sim.h b/flight/pios/osx/inc/pios_sim.h index 79d6d5650..65d7eb9b5 100644 --- a/flight/pios/osx/inc/pios_sim.h +++ b/flight/pios/osx/inc/pios_sim.h @@ -1,10 +1,9 @@ - #ifndef PIOS_SIM_H #define PIOS_SIM_H int PIOS_SIM_Init(); int PIOS_SIM_Step(float dT); -void PIOS_SIM_SetActuator(float * actuator_int, int nchannels); +void PIOS_SIM_SetActuator(float *actuator_int, int nchannels); void PIOS_SIM_GetAccels(float *); void PIOS_SIM_GetGyros(float *); void PIOS_SIM_GetAttitude(float *); diff --git a/flight/pios/osx/inc/pios_sim_priv.h b/flight/pios/osx/inc/pios_sim_priv.h index 2d9aff845..c7dad004c 100644 --- a/flight/pios/osx/inc/pios_sim_priv.h +++ b/flight/pios/osx/inc/pios_sim_priv.h @@ -1,4 +1,3 @@ - #ifndef PIOS_SIM_PRIV_H #define PIOS_SIM_PRIV_H @@ -6,14 +5,14 @@ * State of inputs and outputs to the simulation model */ struct pios_sim_state { - float accels[3]; - float gyros[3]; - float mag[3]; - float baro[1]; - float q[4]; - float velocity[3]; - float position[3]; - float actuator[8]; + float accels[3]; + float gyros[3]; + float mag[3]; + float baro[1]; + float q[4]; + float velocity[3]; + float position[3]; + float actuator[8]; }; #endif /* PIOS_SIM_PRIV */ diff --git a/flight/pios/osx/inc/pios_struct_helper.h b/flight/pios/osx/inc/pios_struct_helper.h index 6196d9b26..24898914c 100644 --- a/flight/pios/osx/inc/pios_struct_helper.h +++ b/flight/pios/osx/inc/pios_struct_helper.h @@ -7,6 +7,8 @@ * @member: the name of the member within the struct. * */ -#define container_of(ptr, type, member) ({ \ - const typeof( ((type *)0)->member ) *__mptr = (ptr); \ - (type *)( (char *)__mptr - offsetof(type,member) );}) +#define container_of(ptr, type, member) \ + ({ \ + const typeof(((type *)0)->member) * __mptr = (ptr); \ + (type *)((char *)__mptr - offsetof(type, member)); } \ + ) diff --git a/flight/pios/osx/inc/pios_sys.h b/flight/pios/osx/inc/pios_sys.h index 7f183a892..a67411db6 100644 --- a/flight/pios/osx/inc/pios_sys.h +++ b/flight/pios/osx/inc/pios_sys.h @@ -6,26 +6,26 @@ * @brief PIOS System Initialization code * @{ * - * @file pios_sys.h + * @file pios_sys.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief System and hardware Init functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,18 +33,18 @@ #define PIOS_SYS_H #define PIOS_SYS_SERIAL_NUM_BINARY_LEN 12 -#define PIOS_SYS_SERIAL_NUM_ASCII_LEN (PIOS_SYS_SERIAL_NUM_BINARY_LEN * 2) +#define PIOS_SYS_SERIAL_NUM_ASCII_LEN (PIOS_SYS_SERIAL_NUM_BINARY_LEN * 2) /* Public Functions */ extern void PIOS_SYS_Init(void); extern int32_t PIOS_SYS_Reset(void); extern uint32_t PIOS_SYS_getCPUFlashSize(void); extern int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t array[PIOS_SYS_SERIAL_NUM_BINARY_LEN]); -extern int32_t PIOS_SYS_SerialNumberGet(char str[PIOS_SYS_SERIAL_NUM_ASCII_LEN+1]); +extern int32_t PIOS_SYS_SerialNumberGet(char str[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]); #endif /* PIOS_SYS_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/inc/pios_tcp.h b/flight/pios/osx/inc/pios_tcp.h index 589c90978..daa2d42af 100644 --- a/flight/pios/osx/inc/pios_tcp.h +++ b/flight/pios/osx/inc/pios_tcp.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_usart.h + * @file pios_usart.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief UDP functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/osx/inc/pios_tcp_priv.h b/flight/pios/osx/inc/pios_tcp_priv.h index a44301f2f..31ff754de 100644 --- a/flight/pios/osx/inc/pios_tcp_priv.h +++ b/flight/pios/osx/inc/pios_tcp_priv.h @@ -39,31 +39,31 @@ #include "fifo_buffer.h" struct pios_tcp_cfg { - const char *ip; - uint16_t port; + const char *ip; + uint16_t port; }; typedef struct { - const struct pios_tcp_cfg * cfg; - pthread_t rxThread; - - int socket; - struct sockaddr_in server; - struct sockaddr_in client; - uint32_t clientLength; - int socket_connection; - - pthread_cond_t cond; - pthread_mutex_t mutex; - - pios_com_callback tx_out_cb; - uint32_t tx_out_context; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - - t_fifo_buffer rx_fifo; - uint8_t rx_buffer[PIOS_TCP_RX_BUFFER_SIZE]; - uint8_t tx_buffer[PIOS_TCP_RX_BUFFER_SIZE]; + const struct pios_tcp_cfg *cfg; + pthread_t rxThread; + + int socket; + struct sockaddr_in server; + struct sockaddr_in client; + uint32_t clientLength; + int socket_connection; + + pthread_cond_t cond; + pthread_mutex_t mutex; + + pios_com_callback tx_out_cb; + uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + + t_fifo_buffer rx_fifo; + uint8_t rx_buffer[PIOS_TCP_RX_BUFFER_SIZE]; + uint8_t tx_buffer[PIOS_TCP_RX_BUFFER_SIZE]; } pios_tcp_dev; extern int32_t PIOS_TCP_Init(uint32_t *tcp_id, const struct pios_tcp_cfg *cfg); diff --git a/flight/pios/osx/inc/pios_udp.h b/flight/pios/osx/inc/pios_udp.h index 48a3b0987..28a095dce 100644 --- a/flight/pios/osx/inc/pios_udp.h +++ b/flight/pios/osx/inc/pios_udp.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_usart.h + * @file pios_usart.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief UDP functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/osx/inc/pios_udp_priv.h b/flight/pios/osx/inc/pios_udp_priv.h index 67ae42057..57516ee63 100644 --- a/flight/pios/osx/inc/pios_udp_priv.h +++ b/flight/pios/osx/inc/pios_udp_priv.h @@ -3,7 +3,7 @@ * * @file pios_udp_priv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief UDP private definitions. * @see The GNU Public License (GPL) Version 3 * @@ -39,33 +39,32 @@ #include struct pios_udp_cfg { - const char * ip; - uint16_t port; + const char *ip; + uint16_t port; }; typedef struct { - const struct pios_udp_cfg * cfg; - xTaskHandle rxThread; + const struct pios_udp_cfg *cfg; + xTaskHandle rxThread; - int socket; - struct sockaddr_in server; - struct sockaddr_in client; - uint32_t clientLength; + int socket; + struct sockaddr_in server; + struct sockaddr_in client; + uint32_t clientLength; - pthread_cond_t cond; - pthread_mutex_t mutex; + pthread_cond_t cond; + pthread_mutex_t mutex; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; - uint8_t rx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; - uint8_t tx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; + uint8_t rx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; + uint8_t tx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; } pios_udp_dev; -extern int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg); - +extern int32_t PIOS_UDP_Init(uint32_t *udp_id, const struct pios_udp_cfg *cfg); #endif /* PIOS_UDP_PRIV_H */ diff --git a/flight/pios/osx/inc/pios_wdg.h b/flight/pios/osx/inc/pios_wdg.h index 4d8f9d944..3337b67ff 100644 --- a/flight/pios/osx/inc/pios_wdg.h +++ b/flight/pios/osx/inc/pios_wdg.h @@ -7,7 +7,7 @@ * * @file pios_wdg.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief SPI functions header. * @see The GNU Public License (GPL) Version 3 * @@ -39,10 +39,10 @@ uint16_t PIOS_WDG_GetActiveFlags(); void PIOS_WDG_Clear(void); bool PIOS_WDG_Check(); -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_SENSORS 0x0010 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_SENSORS 0x0010 #endif diff --git a/flight/pios/osx/inc/sim_model.h b/flight/pios/osx/inc/sim_model.h index 60c15c487..16d28fa2f 100644 --- a/flight/pios/osx/inc/sim_model.h +++ b/flight/pios/osx/inc/sim_model.h @@ -1,6 +1,5 @@ - #include "pios_sim_priv.h" extern int sim_model_init(); extern int sim_model_terminate(); -extern int sim_model_step(float dT, struct pios_sim_state * state); +extern int sim_model_step(float dT, struct pios_sim_state *state); diff --git a/flight/pios/osx/osx/pios_bl_helper.c b/flight/pios/osx/osx/pios_bl_helper.c index a7920e5d2..ec122564b 100644 --- a/flight/pios/osx/osx/pios_bl_helper.c +++ b/flight/pios/osx/osx/pios_bl_helper.c @@ -35,19 +35,19 @@ uint32_t PIOS_BL_HELPER_CRC_Memory_Calc() { - return 0; + return 0; } extern const struct fw_version_info fw_version_blob; -void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t * array, uint8_t size) +void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t *array, uint8_t size) { - uint8_t * desc = (uint8_t *) &fw_version_blob; - for (uint32_t i = 0; i < size; i++) { - array[i] = desc[i]; - } + uint8_t *desc = (uint8_t *)&fw_version_blob; + + for (uint32_t i = 0; i < size; i++) { + array[i] = desc[i]; + } } void PIOS_BL_HELPER_CRC_Ini() -{ -} +{} #endif diff --git a/flight/pios/osx/osx/pios_board_info.c b/flight/pios/osx/osx/pios_board_info.c index c273a881d..2ef651a21 100644 --- a/flight/pios/osx/osx/pios_board_info.c +++ b/flight/pios/osx/osx/pios_board_info.c @@ -4,17 +4,17 @@ #include "pios_board_info.h" const struct pios_board_info pios_board_info_blob = { - .magic = PIOS_BOARD_INFO_BLOB_MAGIC, - .board_type = BOARD_TYPE, - .board_rev = BOARD_REVISION, - .bl_rev = BOOTLOADER_VERSION, - .hw_type = HW_TYPE, - .fw_base = FW_BANK_BASE, - .fw_size = FW_BANK_SIZE - FW_DESC_SIZE, - .desc_base = FW_BANK_BASE + FW_BANK_SIZE - FW_DESC_SIZE, - .desc_size = FW_DESC_SIZE, + .magic = PIOS_BOARD_INFO_BLOB_MAGIC, + .board_type = BOARD_TYPE, + .board_rev = BOARD_REVISION, + .bl_rev = BOOTLOADER_VERSION, + .hw_type = HW_TYPE, + .fw_base = FW_BANK_BASE, + .fw_size = FW_BANK_SIZE - FW_DESC_SIZE, + .desc_base = FW_BANK_BASE + FW_BANK_SIZE - FW_DESC_SIZE, + .desc_size = FW_DESC_SIZE, #ifdef EE_BANK_BASE - .ee_base = EE_BANK_BASE, - .ee_size = EE_BANK_SIZE, + .ee_base = EE_BANK_BASE, + .ee_size = EE_BANK_SIZE, #endif }; diff --git a/flight/pios/osx/osx/pios_com.c b/flight/pios/osx/osx/pios_com.c index 4f2e1e3c9..8eab84a4e 100644 --- a/flight/pios/osx/osx/pios_com.c +++ b/flight/pios/osx/osx/pios_com.c @@ -6,26 +6,26 @@ * @brief Hardware communication layer * @{ * - * @file pios_com.c + * @file pios_com.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM layer functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,462 +38,470 @@ #include #if !defined(PIOS_INCLUDE_FREERTOS) -#include "pios_delay.h" /* PIOS_DELAY_WaitmS */ +#include "pios_delay.h" /* PIOS_DELAY_WaitmS */ #endif enum pios_com_dev_magic { - PIOS_COM_DEV_MAGIC = 0xaa55aa55, + PIOS_COM_DEV_MAGIC = 0xaa55aa55, }; struct pios_com_dev { - enum pios_com_dev_magic magic; - uint32_t lower_id; - const struct pios_com_driver * driver; + enum pios_com_dev_magic magic; + uint32_t lower_id; + const struct pios_com_driver *driver; #if defined(PIOS_INCLUDE_FREERTOS) - xSemaphoreHandle tx_sem; - xSemaphoreHandle rx_sem; + xSemaphoreHandle tx_sem; + xSemaphoreHandle rx_sem; #endif - bool has_rx; - bool has_tx; + bool has_rx; + bool has_tx; - t_fifo_buffer rx; - t_fifo_buffer tx; + t_fifo_buffer rx; + t_fifo_buffer tx; }; -static bool PIOS_COM_validate(struct pios_com_dev * com_dev) +static bool PIOS_COM_validate(struct pios_com_dev *com_dev) { - return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC)); + return com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) && 0 // static struct pios_com_dev * PIOS_COM_alloc(void) -//{ -// struct pios_com_dev * com_dev; +// { +// struct pios_com_dev * com_dev; // -// com_dev = (struct pios_com_dev *)malloc(sizeof(*com_dev)); -// if (!com_dev) return (NULL); +// com_dev = (struct pios_com_dev *)malloc(sizeof(*com_dev)); +// if (!com_dev) return (NULL); // -// com_dev->magic = PIOS_COM_DEV_MAGIC; -// return(com_dev); -//} +// com_dev->magic = PIOS_COM_DEV_MAGIC; +// return(com_dev); +// } #else static struct pios_com_dev pios_com_devs[PIOS_COM_MAX_DEVS]; static uint8_t pios_com_num_devs; static uint32_t PIOS_COM_create(void) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) { - return (PIOS_COM_MAX_DEVS+1); - } + if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) { + return PIOS_COM_MAX_DEVS + 1; + } - com_dev = &pios_com_devs[pios_com_num_devs++]; - com_dev->magic = PIOS_COM_DEV_MAGIC; + com_dev = &pios_com_devs[pios_com_num_devs++]; + com_dev->magic = PIOS_COM_DEV_MAGIC; - return (pios_com_num_devs); + return pios_com_num_devs; } -static struct pios_com_dev * PIOS_COM_find_dev(uint32_t com_dev_id) +static struct pios_com_dev *PIOS_COM_find_dev(uint32_t com_dev_id) { - if (!com_dev_id) return NULL; - if (com_dev_id>pios_com_num_devs+1) return NULL; - return &pios_com_devs[com_dev_id-1]; + if (!com_dev_id) { + return NULL; + } + if (com_dev_id > pios_com_num_devs + 1) { + return NULL; + } + return &pios_com_devs[com_dev_id - 1]; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) && 0 */ -static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); -static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); -static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield); -static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield); +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); +static void PIOS_COM_UnblockRx(struct pios_com_dev *com_dev, bool *need_yield); +static void PIOS_COM_UnblockTx(struct pios_com_dev *com_dev, bool *need_yield); /** - * Initialises COM layer - * \param[out] handle - * \param[in] driver - * \param[in] id - * \return < 0 if initialisation failed - */ -int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len) + * Initialises COM layer + * \param[out] handle + * \param[in] driver + * \param[in] id + * \return < 0 if initialisation failed + */ +int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len) { - PIOS_Assert(com_id); - PIOS_Assert(driver); + PIOS_Assert(com_id); + PIOS_Assert(driver); - bool has_rx = (rx_buffer && rx_buffer_len > 0); - bool has_tx = (tx_buffer && tx_buffer_len > 0); - PIOS_Assert(has_rx || has_tx); - PIOS_Assert(driver->bind_tx_cb || !has_tx); - PIOS_Assert(driver->bind_rx_cb || !has_rx); + bool has_rx = (rx_buffer && rx_buffer_len > 0); + bool has_tx = (tx_buffer && tx_buffer_len > 0); + PIOS_Assert(has_rx || has_tx); + PIOS_Assert(driver->bind_tx_cb || !has_tx); + PIOS_Assert(driver->bind_rx_cb || !has_rx); - uint32_t com_dev_id; - struct pios_com_dev * com_dev; + uint32_t com_dev_id; + struct pios_com_dev *com_dev; - com_dev_id = PIOS_COM_create(); - com_dev = PIOS_COM_find_dev(com_dev_id); - if (!com_dev) goto out_fail; + com_dev_id = PIOS_COM_create(); + com_dev = PIOS_COM_find_dev(com_dev_id); + if (!com_dev) { + goto out_fail; + } - com_dev->driver = driver; - com_dev->lower_id = lower_id; + com_dev->driver = driver; + com_dev->lower_id = lower_id; - com_dev->has_rx = has_rx; - com_dev->has_tx = has_tx; + com_dev->has_rx = has_rx; + com_dev->has_tx = has_tx; - if (has_rx) { - fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len); + if (has_rx) { + fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len); #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(com_dev->rx_sem); -#endif /* PIOS_INCLUDE_FREERTOS */ - (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, com_dev_id); - if (com_dev->driver->rx_start) { - /* Start the receiver */ - (com_dev->driver->rx_start)(com_dev->lower_id, - fifoBuf_getFree(&com_dev->rx)); - } - } + vSemaphoreCreateBinary(com_dev->rx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, com_dev_id); + if (com_dev->driver->rx_start) { + /* Start the receiver */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } + } - if (has_tx) { - fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); + if (has_tx) { + fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(com_dev->tx_sem); -#endif /* PIOS_INCLUDE_FREERTOS */ - (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, com_dev_id); - } + vSemaphoreCreateBinary(com_dev->tx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, com_dev_id); + } - *com_id = com_dev_id; - return(0); + *com_id = com_dev_id; + return 0; out_fail: - return(-1); + return -1; } -static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield) +static void PIOS_COM_UnblockRx(struct pios_com_dev *com_dev, bool *need_yield) { #if defined(PIOS_INCLUDE_FREERTOS) - static signed portBASE_TYPE xHigherPriorityTaskWoken; - xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken); + static signed portBASE_TYPE xHigherPriorityTaskWoken; + xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken != pdFALSE) { - *need_yield = true; - } else { - *need_yield = false; - } + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } #else - *need_yield = false; + *need_yield = false; #endif } -static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield) +static void PIOS_COM_UnblockTx(struct pios_com_dev *com_dev, bool *need_yield) { #if defined(PIOS_INCLUDE_FREERTOS) - static signed portBASE_TYPE xHigherPriorityTaskWoken; - xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken); + static signed portBASE_TYPE xHigherPriorityTaskWoken; + xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken != pdFALSE) { - *need_yield = true; - } else { - *need_yield = false; - } + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } #else - *need_yield = false; + *need_yield = false; #endif } -static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(context); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(context); - bool valid = PIOS_COM_validate(com_dev); - PIOS_Assert(valid); - PIOS_Assert(com_dev->has_rx); + bool valid = PIOS_COM_validate(com_dev); - PIOS_IRQ_Disable(); - uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len); - PIOS_IRQ_Enable(); + PIOS_Assert(valid); + PIOS_Assert(com_dev->has_rx); - if (bytes_into_fifo > 0) { - /* Data has been added to the buffer */ - PIOS_COM_UnblockRx(com_dev, need_yield); - } + PIOS_IRQ_Disable(); + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len); + PIOS_IRQ_Enable(); - if (headroom) { - *headroom = fifoBuf_getFree(&com_dev->rx); - } + if (bytes_into_fifo > 0) { + /* Data has been added to the buffer */ + PIOS_COM_UnblockRx(com_dev, need_yield); + } - return (bytes_into_fifo); + if (headroom) { + *headroom = fifoBuf_getFree(&com_dev->rx); + } + + return bytes_into_fifo; } -static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(context); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(context); - bool valid = PIOS_COM_validate(com_dev); - PIOS_Assert(valid); - PIOS_Assert(buf); - PIOS_Assert(buf_len); - PIOS_Assert(com_dev->has_tx); + bool valid = PIOS_COM_validate(com_dev); - PIOS_IRQ_Disable(); - uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len); - PIOS_IRQ_Enable(); + PIOS_Assert(valid); + PIOS_Assert(buf); + PIOS_Assert(buf_len); + PIOS_Assert(com_dev->has_tx); - if (bytes_from_fifo > 0) { - /* More space has been made in the buffer */ - PIOS_COM_UnblockTx(com_dev, need_yield); - } + PIOS_IRQ_Disable(); + uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len); + PIOS_IRQ_Enable(); - if (headroom) { - *headroom = fifoBuf_getUsed(&com_dev->tx); - } + if (bytes_from_fifo > 0) { + /* More space has been made in the buffer */ + PIOS_COM_UnblockTx(com_dev, need_yield); + } - return (bytes_from_fifo); + if (headroom) { + *headroom = fifoBuf_getUsed(&com_dev->tx); + } + + return bytes_from_fifo; } /** -* Change the port speed without re-initializing -* \param[in] port COM port -* \param[in] baud Requested baud rate -* \return -1 if port not available -* \return 0 on success -*/ + * Change the port speed without re-initializing + * \param[in] port COM port + * \param[in] baud Requested baud rate + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - /* Invoke the driver function if it exists */ - if (com_dev->driver->set_baud) { - com_dev->driver->set_baud(com_dev->lower_id, baud); - } + /* Invoke the driver function if it exists */ + if (com_dev->driver->set_baud) { + com_dev->driver->set_baud(com_dev->lower_id, baud); + } - return 0; + return 0; } /** -* Sends a package over given port -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a package over given port + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - PIOS_Assert(com_dev->has_tx); + PIOS_Assert(com_dev->has_tx); - if (len >= fifoBuf_getFree(&com_dev->tx)) { - /* Buffer cannot accept all requested bytes (retry) */ - return -2; - } + if (len >= fifoBuf_getFree(&com_dev->tx)) { + /* Buffer cannot accept all requested bytes (retry) */ + return -2; + } - PIOS_IRQ_Disable(); - uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len); - PIOS_IRQ_Enable(); + PIOS_IRQ_Disable(); + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len); + PIOS_IRQ_Enable(); - if (bytes_into_fifo > 0) { - /* More data has been put in the tx buffer, make sure the tx is started */ - if (com_dev->driver->tx_start) { - com_dev->driver->tx_start(com_dev->lower_id, - fifoBuf_getUsed(&com_dev->tx)); - } - } + if (bytes_into_fifo > 0) { + /* More data has been put in the tx buffer, make sure the tx is started */ + if (com_dev->driver->tx_start) { + com_dev->driver->tx_start(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } + } - return (0); + return 0; } /** -* Sends a package over given port -* (blocking function) -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a package over given port + * (blocking function) + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - PIOS_Assert(com_dev->has_tx); + PIOS_Assert(com_dev->has_tx); - int32_t rc; - do { - rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len); + int32_t rc; + do { + rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len); #if defined(PIOS_INCLUDE_FREERTOS) - if (rc == -2) { - /* Make sure the transmitter is running while we wait */ - if (com_dev->driver->tx_start) { - (com_dev->driver->tx_start)(com_dev->lower_id, - fifoBuf_getUsed(&com_dev->tx)); - } - if (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) { - return -3; - } - } + if (rc == -2) { + /* Make sure the transmitter is running while we wait */ + if (com_dev->driver->tx_start) { + (com_dev->driver->tx_start)(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } + if (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) { + return -3; + } + } #endif - } while (rc == -2); + } while (rc == -2); - return rc; + return rc; } /** -* Sends a single character over given port -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a single character over given port + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c) { - return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1); + return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1); } /** -* Sends a single character over given port -* (blocking function) -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a single character over given port + * (blocking function) + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendChar(uint32_t com_id, char c) { - return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1); + return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1); } /** -* Sends a string over given port -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a string over given port + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str) { - return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str)); + return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str)); } /** -* Sends a string over given port -* (blocking function) -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a string over given port + * (blocking function) + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendString(uint32_t com_id, const char *str) { - return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str)); + return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str)); } /** -* Sends a formatted string (-> printf) over given port -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* 128 characters supported maximum! -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * 128 characters supported maximum! + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Sends a formatted string (-> printf) over given port -* (blocking function) -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * (blocking function) + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Transfer bytes from port buffers into another buffer -* \param[in] port COM port -* \returns Byte from buffer -*/ -uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms) + * Transfer bytes from port buffers into another buffer + * \param[in] port COM port + * \returns Byte from buffer + */ +uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t *buf, uint16_t buf_len, uint32_t timeout_ms) { - PIOS_Assert(buf); - PIOS_Assert(buf_len); + PIOS_Assert(buf); + PIOS_Assert(buf_len); - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - PIOS_Assert(0); - } - PIOS_Assert(com_dev->has_rx); + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + PIOS_Assert(0); + } + PIOS_Assert(com_dev->has_rx); - check_again: - PIOS_IRQ_Disable(); - uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len); - PIOS_IRQ_Enable(); +check_again: + PIOS_IRQ_Disable(); + uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len); + PIOS_IRQ_Enable(); - if (bytes_from_fifo == 0 && timeout_ms > 0) { - /* No more bytes in receive buffer */ - /* Make sure the receiver is running while we wait */ - if (com_dev->driver->rx_start) { - /* Notify the lower layer that there is now room in the rx buffer */ - (com_dev->driver->rx_start)(com_dev->lower_id, - fifoBuf_getFree(&com_dev->rx)); - } + if (bytes_from_fifo == 0 && timeout_ms > 0) { + /* No more bytes in receive buffer */ + /* Make sure the receiver is running while we wait */ + if (com_dev->driver->rx_start) { + /* Notify the lower layer that there is now room in the rx buffer */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) { - /* Make sure we don't come back here again */ - timeout_ms = 0; - goto check_again; - } + if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) { + /* Make sure we don't come back here again */ + timeout_ms = 0; + goto check_again; + } #else - PIOS_DELAY_WaitmS(1); - timeout_ms--; - goto check_again; + PIOS_DELAY_WaitmS(1); + timeout_ms--; + goto check_again; #endif - } + } - /* Return received byte */ - return (bytes_from_fifo); + /* Return received byte */ + return bytes_from_fifo; } /** @@ -503,21 +511,22 @@ uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len */ bool PIOS_COM_Available(uint32_t com_id) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - return false; - } + if (!PIOS_COM_validate(com_dev)) { + return false; + } - // If a driver does not provide a query method assume always - // available if valid - if (com_dev->driver->available == NULL) - return true; + // If a driver does not provide a query method assume always + // available if valid + if (com_dev->driver->available == NULL) { + return true; + } - return (com_dev->driver->available)(com_dev->lower_id); + return (com_dev->driver->available)(com_dev->lower_id); } -#endif +#endif /* if defined(PIOS_INCLUDE_COM) */ /** * @} diff --git a/flight/pios/osx/osx/pios_crc.c b/flight/pios/osx/osx/pios_crc.c index 548ba2648..0e4cd5dec 100644 --- a/flight/pios/osx/osx/pios_crc.c +++ b/flight/pios/osx/osx/pios_crc.c @@ -11,22 +11,22 @@ // CRC lookup table static const uint8_t crc_table[256] = { - 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, - 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, - 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, - 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, - 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, - 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, - 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, - 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, - 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, - 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, - 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, - 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, - 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, - 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, - 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, - 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 + 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, + 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, + 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, + 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, + 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, + 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, + 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, + 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, + 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, + 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, + 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, + 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, + 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, + 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, + 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, + 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 }; /** @@ -49,7 +49,7 @@ static const uint8_t crc_table[256] = { */ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) { - return crc_table[crc ^ data]; + return crc_table[crc ^ data]; } /* @@ -59,16 +59,16 @@ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) * @param[in] length Number of bytes to process * @returns Updated CRC */ -uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length) +uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t *data, int32_t length) { - // use registers for speed - register int32_t len = length; - register uint8_t crc8 = crc; - register const uint8_t *p = data; - - while (len--) - crc8 = crc_table[crc8 ^ *p++]; - - return crc8; -} + // use registers for speed + register int32_t len = length; + register uint8_t crc8 = crc; + register const uint8_t *p = data; + while (len--) { + crc8 = crc_table[crc8 ^ *p++]; + } + + return crc8; +} diff --git a/flight/pios/osx/osx/pios_debug.c b/flight/pios/osx/osx/pios_debug.c index 5956a9d04..71120a39d 100644 --- a/flight/pios/osx/osx/pios_debug.c +++ b/flight/pios/osx/osx/pios_debug.c @@ -10,21 +10,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Debugging Functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,36 +37,31 @@ const char *PIOS_DEBUG_AssertMsg = "ASSERT FAILED"; /* Private Function Prototypes */ /** -* Initialise Debug-features -*/ + * Initialise Debug-features + */ void PIOS_DEBUG_Init(void) -{ -} +{} /** -* Set debug-pin high -* \param pin 0 for S1 output -*/ + * Set debug-pin high + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinHigh(uint8_t Pin) -{ -} +{} /** -* Set debug-pin low -* \param pin 0 for S1 output -*/ + * Set debug-pin low + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinLow(uint8_t Pin) -{ -} +{} void PIOS_DEBUG_PinValue8Bit(uint8_t value) -{ -} +{} void PIOS_DEBUG_PinValue4BitL(uint8_t value) -{ -} +{} /** @@ -75,22 +70,21 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value) void PIOS_DEBUG_Panic(const char *msg) { #ifdef PIOS_COM_DEBUG - register int *lr asm("lr"); // Link-register holds the PC of the caller - PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s @0x%x\r", msg, lr); + register int *lr asm ("lr"); // Link-register holds the PC of the caller + PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s @0x%x\r", msg, lr); #endif - // tell the user whats going on on commandline too - fprintf(stderr,"CRITICAL ERROR: %s\n",msg); - - // this helps debugging: causing a div by zero allows a backtrace - // and/or ends execution - int b = 0; - int a = (2/b); - b=a; + // tell the user whats going on on commandline too + fprintf(stderr, "CRITICAL ERROR: %s\n", msg); + // this helps debugging: causing a div by zero allows a backtrace + // and/or ends execution + int b = 0; + int a = (2 / b); + b = a; } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/osx/pios_delay.c b/flight/pios/osx/osx/pios_delay.c index eb4082268..a42d9023a 100644 --- a/flight/pios/osx/osx/pios_delay.c +++ b/flight/pios/osx/osx/pios_delay.c @@ -3,27 +3,27 @@ * * @file pios_delay.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) - * @brief Delay Functions - * - Provides a micro-second granular delay using a TIM + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * @brief Delay Functions + * - Provides a micro-second granular delay using a TIM * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_DELAY Delay Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,78 +35,82 @@ #if defined(PIOS_INCLUDE_DELAY) /** -* Initialises the Timer used by PIOS_DELAY functions
-* This is called from pios.c as part of the main() function -* at system start up. -* \return < 0 if initialisation failed -*/ + * Initialises the Timer used by PIOS_DELAY functions
+ * This is called from pios.c as part of the main() function + * at system start up. + * \return < 0 if initialisation failed + */ #include int32_t PIOS_DELAY_Init(void) { - // stub + // stub - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Waits for a specific number of uS
-* Example:
-* \code -* // Wait for 500 uS -* PIOS_DELAY_Wait_uS(500); -* \endcode -* \param[in] uS delay (1..65535 microseconds) -* \return < 0 on errors -*/ + * Waits for a specific number of uS
+ * Example:
+ * \code + * // Wait for 500 uS + * PIOS_DELAY_Wait_uS(500); + * \endcode + * \param[in] uS delay (1..65535 microseconds) + * \return < 0 on errors + */ int32_t PIOS_DELAY_WaituS(uint32_t uS) { - struct timespec wait,rest; - wait.tv_sec=0; - wait.tv_nsec=1000*uS; - while (!nanosleep(&wait,&rest)) { - wait=rest; - } + struct timespec wait, rest; - /* No error */ - return 0; + wait.tv_sec = 0; + wait.tv_nsec = 1000 * uS; + while (!nanosleep(&wait, &rest)) { + wait = rest; + } + + /* No error */ + return 0; } /** -* Waits for a specific number of mS
-* Example:
-* \code -* // Wait for 500 mS -* PIOS_DELAY_Wait_mS(500); -* \endcode -* \param[in] mS delay (1..65535 milliseconds) -* \return < 0 on errors -*/ + * Waits for a specific number of mS
+ * Example:
+ * \code + * // Wait for 500 mS + * PIOS_DELAY_Wait_mS(500); + * \endcode + * \param[in] mS delay (1..65535 milliseconds) + * \return < 0 on errors + */ int32_t PIOS_DELAY_WaitmS(uint32_t mS) { - struct timespec wait,rest; - wait.tv_sec=mS/1000; - wait.tv_nsec=(mS%1000)*1000000; - while (!nanosleep(&wait,&rest)) { - wait=rest; - } + struct timespec wait, rest; - /* No error */ - return 0; + wait.tv_sec = mS / 1000; + wait.tv_nsec = (mS % 1000) * 1000000; + while (!nanosleep(&wait, &rest)) { + wait = rest; + } + + /* No error */ + return 0; } uint32_t PIOS_DELAY_GetRaw() { - uint32_t raw_us = clock(); - return raw_us; + uint32_t raw_us = clock(); + + return raw_us; } uint32_t PIOS_DELAY_DiffuS(uint32_t ref) { - uint32_t diff_clock = clock() - ref; - uint32_t diff_us = diff_clock; // (CLOCKS_PER_SEC / 1000); - return diff_us; + uint32_t diff_clock = clock() - ref; + uint32_t diff_us = diff_clock; // (CLOCKS_PER_SEC / 1000); + + return diff_us; } -#endif +#endif /* if defined(PIOS_INCLUDE_DELAY) */ diff --git a/flight/pios/osx/osx/pios_gcsrcvr.c b/flight/pios/osx/osx/pios_gcsrcvr.c index f9c19cca3..1618c27c0 100644 --- a/flight/pios/osx/osx/pios_gcsrcvr.c +++ b/flight/pios/osx/osx/pios_gcsrcvr.c @@ -41,19 +41,19 @@ static GCSReceiverData gcsreceiverdata; static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver = { - .read = PIOS_GCSRCVR_Get, + .read = PIOS_GCSRCVR_Get, }; /* Local Variables */ enum pios_gcsrcvr_dev_magic { - PIOS_GCSRCVR_DEV_MAGIC = 0xe9da5c56, + PIOS_GCSRCVR_DEV_MAGIC = 0xe9da5c56, }; struct pios_gcsrcvr_dev { - enum pios_gcsrcvr_dev_magic magic; + enum pios_gcsrcvr_dev_magic magic; - uint8_t supv_timer; - bool Fresh; + uint8_t supv_timer; + bool Fresh; }; static struct pios_gcsrcvr_dev *global_gcsrcvr_dev; @@ -61,79 +61,83 @@ static struct pios_gcsrcvr_dev *global_gcsrcvr_dev; #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void) { - struct pios_gcsrcvr_dev * gcsrcvr_dev; + struct pios_gcsrcvr_dev *gcsrcvr_dev; - gcsrcvr_dev = (struct pios_gcsrcvr_dev *)pvPortMalloc(sizeof(*gcsrcvr_dev)); - if (!gcsrcvr_dev) return(NULL); + gcsrcvr_dev = (struct pios_gcsrcvr_dev *)pvPortMalloc(sizeof(*gcsrcvr_dev)); + if (!gcsrcvr_dev) { + return NULL; + } - gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; - gcsrcvr_dev->Fresh = FALSE; - gcsrcvr_dev->supv_timer = 0; + gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; + gcsrcvr_dev->Fresh = FALSE; + gcsrcvr_dev->supv_timer = 0; - /* The update callback cannot receive the device pointer, so set it in a global */ - global_gcsrcvr_dev = gcsrcvr_dev; + /* The update callback cannot receive the device pointer, so set it in a global */ + global_gcsrcvr_dev = gcsrcvr_dev; - return(gcsrcvr_dev); + return gcsrcvr_dev; } #else static struct pios_gcsrcvr_dev pios_gcsrcvr_devs[PIOS_GCSRCVR_MAX_DEVS]; static uint8_t pios_gcsrcvr_num_devs; static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void) { - struct pios_gcsrcvr_dev *gcsrcvr_dev; + struct pios_gcsrcvr_dev *gcsrcvr_dev; - if (pios_gcsrcvr_num_devs >= PIOS_GCSRCVR_MAX_DEVS) { - return (NULL); - } + if (pios_gcsrcvr_num_devs >= PIOS_GCSRCVR_MAX_DEVS) { + return NULL; + } - gcsrcvr_dev = &pios_gcsrcvr_devs[pios_gcsrcvr_num_devs++]; - gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; - gcsrcvr_dev->Fresh = FALSE; - gcsrcvr_dev->supv_timer = 0; + gcsrcvr_dev = &pios_gcsrcvr_devs[pios_gcsrcvr_num_devs++]; + gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC; + gcsrcvr_dev->Fresh = FALSE; + gcsrcvr_dev->supv_timer = 0; - global_gcsrcvr_dev = gcsrcvr_dev; + global_gcsrcvr_dev = gcsrcvr_dev; - return (gcsrcvr_dev); + return gcsrcvr_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void gcsreceiver_updated(UAVObjEvent * ev) +static void gcsreceiver_updated(UAVObjEvent *ev) { - struct pios_gcsrcvr_dev *gcsrcvr_dev = global_gcsrcvr_dev; - if (ev->obj == GCSReceiverHandle()) { - GCSReceiverGet(&gcsreceiverdata); - gcsrcvr_dev->Fresh = TRUE; - } + struct pios_gcsrcvr_dev *gcsrcvr_dev = global_gcsrcvr_dev; + + if (ev->obj == GCSReceiverHandle()) { + GCSReceiverGet(&gcsreceiverdata); + gcsrcvr_dev->Fresh = TRUE; + } } extern int32_t PIOS_GCSRCVR_Init(uint32_t *gcsrcvr_id) { - struct pios_gcsrcvr_dev *gcsrcvr_dev; + struct pios_gcsrcvr_dev *gcsrcvr_dev; - /* Allocate the device structure */ - gcsrcvr_dev = (struct pios_gcsrcvr_dev *)PIOS_gcsrcvr_alloc(); - if (!gcsrcvr_dev) - return -1; + /* Allocate the device structure */ + gcsrcvr_dev = (struct pios_gcsrcvr_dev *)PIOS_gcsrcvr_alloc(); + if (!gcsrcvr_dev) { + return -1; + } - /* Register uavobj callback */ - GCSReceiverConnectCallback (gcsreceiver_updated); + /* Register uavobj callback */ + GCSReceiverConnectCallback(gcsreceiver_updated); - return 0; + return 0; } static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel) { - if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) { - /* channel is out of range */ - return -1; - } + if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) { + /* channel is out of range */ + return -1; + } - return (gcsreceiverdata.Channel[channel]); + return gcsreceiverdata.Channel[channel]; } -#endif /* PIOS_INCLUDE_GCSRCVR */ +#endif /* PIOS_INCLUDE_GCSRCVR */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/osx/osx/pios_iap.c b/flight/pios/osx/osx/pios_iap.c index aef9f6c54..e38bf1d6d 100644 --- a/flight/pios/osx/osx/pios_iap.c +++ b/flight/pios/osx/osx/pios_iap.c @@ -1,6 +1,6 @@ /*! - * @File iap.c - * @Brief + * @File iap.c + * @Brief * * Created on: Sep 6, 2010 * Author: joe @@ -8,8 +8,8 @@ /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ #include /*! @@ -20,50 +20,42 @@ * * Created: Sep 8, 2010 10:10:48 PM by joe */ -void PIOS_IAP_Init( void ) -{ - -} +void PIOS_IAP_Init(void) +{} /*! * \brief Determines if an In-Application-Programming request has been made. * \param *comm - Which communication stream to use for the IAP (USB, Telemetry, I2C, SPI, etc) * \return TRUE - if correct sequence found, along with 'comm' updated. - * FALSE - Note that 'comm' will have an invalid comm identifier. + * FALSE - Note that 'comm' will have an invalid comm identifier. * \retval * */ -uint32_t PIOS_IAP_CheckRequest( void ) +uint32_t PIOS_IAP_CheckRequest(void) { - - return false; + return false; } - /*! * \brief Sets the 1st word of the request sequence. * \param n/a * \return n/a * \retval */ -void PIOS_IAP_SetRequest1(void) -{ -} +void PIOS_IAP_SetRequest1(void) +{} -void PIOS_IAP_SetRequest2(void) -{ -} +void PIOS_IAP_SetRequest2(void) +{} -void PIOS_IAP_ClearRequest(void) -{ -} +void PIOS_IAP_ClearRequest(void) +{} uint16_t PIOS_IAP_ReadBootCount(void) { - return 0; + return 0; } -void PIOS_IAP_WriteBootCount (uint16_t boot_count) -{ -} +void PIOS_IAP_WriteBootCount(uint16_t boot_count) +{} diff --git a/flight/pios/osx/osx/pios_irq.c b/flight/pios/osx/osx/pios_irq.c index b30d09fee..793df9ccf 100644 --- a/flight/pios/osx/osx/pios_irq.c +++ b/flight/pios/osx/osx/pios_irq.c @@ -6,26 +6,26 @@ * @brief STM32 Hardware code to enable and disable interrupts * @{ * - * @file pios_irq.c + * @file pios_irq.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief IRQ Enable/Disable routines * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,33 +37,33 @@ /* Private Function Prototypes */ /** -* Disables all interrupts (nested) -* \return < 0 On errors -*/ + * Disables all interrupts (nested) + * \return < 0 On errors + */ int32_t PIOS_IRQ_Disable(void) { #if defined(PIOS_INCLUDE_FREERTOS) - taskENTER_CRITICAL(); + taskENTER_CRITICAL(); #endif - return 0; + return 0; } /** -* Enables all interrupts (nested) -* \return < 0 on errors -* \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) -*/ + * Enables all interrupts (nested) + * \return < 0 on errors + * \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) + */ int32_t PIOS_IRQ_Enable(void) { #if defined(PIOS_INCLUDE_FREERTOS) - taskEXIT_CRITICAL(); + taskEXIT_CRITICAL(); #endif - return 0; + return 0; } -#endif +#endif /* if defined(PIOS_INCLUDE_IRQ) */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/osx/pios_led.c b/flight/pios/osx/osx/pios_led.c index 770e8b2c4..912929cbe 100644 --- a/flight/pios/osx/osx/pios_led.c +++ b/flight/pios/osx/osx/pios_led.c @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file pios_led.c + * @file pios_led.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief LED functions, init, toggle, on & off. * @see The GNU Public License (GPL) Version 3 @@ -9,19 +9,19 @@ * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,49 +38,50 @@ static uint8_t LED_GPIO[PIOS_LED_NUM]; -static inline void PIOS_SetLED(uint32_t LED,uint8_t stat) { - //printf("PIOS: LED %i status %i\n",LED,stat); - LED_GPIO[LED]=stat; +static inline void PIOS_SetLED(uint32_t LED, uint8_t stat) +{ + // printf("PIOS: LED %i status %i\n",LED,stat); + LED_GPIO[LED] = stat; } /** -* Initialises all the LED's -*/ + * Initialises all the LED's + */ void PIOS_LED_Init(void) { - for(int LEDNum = 0; LEDNum < PIOS_LED_NUM; LEDNum++) { - LED_GPIO[LEDNum]=0; - } + for (int LEDNum = 0; LEDNum < PIOS_LED_NUM; LEDNum++) { + LED_GPIO[LEDNum] = 0; + } } /** -* Turn on LED -* \param[in] LED LED Name (LED1, LED2) -*/ + * Turn on LED + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_On(uint32_t led) { - PIOS_SetLED(led,1); + PIOS_SetLED(led, 1); } /** -* Turn off LED -* \param[in] LED LED Name (LED1, LED2) -*/ + * Turn off LED + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_Off(uint32_t led) { - PIOS_SetLED(led,0); + PIOS_SetLED(led, 0); } /** -* Toggle LED on/off -* \param[in] LED LED Name (LED1, LED2) -*/ + * Toggle LED on/off + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_Toggle(uint32_t led) { - PIOS_SetLED(led,LED_GPIO[led]?0:1); + PIOS_SetLED(led, LED_GPIO[led] ? 0 : 1); } -#endif +#endif /* if defined(PIOS_INCLUDE_LED) */ diff --git a/flight/pios/osx/osx/pios_rcvr.c b/flight/pios/osx/osx/pios_rcvr.c index 0a2a8cd87..87d9b2e9b 100644 --- a/flight/pios/osx/osx/pios_rcvr.c +++ b/flight/pios/osx/osx/pios_rcvr.c @@ -6,102 +6,108 @@ #include enum pios_rcvr_dev_magic { - PIOS_RCVR_DEV_MAGIC = 0x99aabbcc, + PIOS_RCVR_DEV_MAGIC = 0x99aabbcc, }; struct pios_rcvr_dev { - enum pios_rcvr_dev_magic magic; - uint32_t lower_id; - const struct pios_rcvr_driver * driver; + enum pios_rcvr_dev_magic magic; + uint32_t lower_id; + const struct pios_rcvr_driver *driver; }; -static bool PIOS_RCVR_validate(struct pios_rcvr_dev * rcvr_dev) +static bool PIOS_RCVR_validate(struct pios_rcvr_dev *rcvr_dev) { - return (rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC); + return rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) && 0 -static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) +static struct pios_rcvr_dev *PIOS_RCVR_alloc(void) { - struct pios_rcvr_dev * rcvr_dev; + struct pios_rcvr_dev *rcvr_dev; - rcvr_dev = (struct pios_rcvr_dev *)malloc(sizeof(*rcvr_dev)); - if (!rcvr_dev) return (NULL); + rcvr_dev = (struct pios_rcvr_dev *)malloc(sizeof(*rcvr_dev)); + if (!rcvr_dev) { + return NULL; + } - rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; - return(rcvr_dev); + rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; + return rcvr_dev; } #else static struct pios_rcvr_dev pios_rcvr_devs[PIOS_RCVR_MAX_DEVS]; static uint8_t pios_rcvr_num_devs; -static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) +static struct pios_rcvr_dev *PIOS_RCVR_alloc(void) { - struct pios_rcvr_dev * rcvr_dev; + struct pios_rcvr_dev *rcvr_dev; - if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) { - return (NULL); - } + if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) { + return NULL; + } - rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++]; - rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; + rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++]; + rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; - return (rcvr_dev); + return rcvr_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) && 0 */ /** - * Initialises RCVR layer - * \param[out] handle - * \param[in] driver - * \param[in] id - * \return < 0 if initialisation failed - */ -int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id) + * Initialises RCVR layer + * \param[out] handle + * \param[in] driver + * \param[in] id + * \return < 0 if initialisation failed + */ +int32_t PIOS_RCVR_Init(uint32_t *rcvr_id, const struct pios_rcvr_driver *driver, const uint32_t lower_id) { - PIOS_DEBUG_Assert(rcvr_id); - PIOS_DEBUG_Assert(driver); + PIOS_DEBUG_Assert(rcvr_id); + PIOS_DEBUG_Assert(driver); - struct pios_rcvr_dev * rcvr_dev; + struct pios_rcvr_dev *rcvr_dev; - rcvr_dev = (struct pios_rcvr_dev *) PIOS_RCVR_alloc(); - if (!rcvr_dev) goto out_fail; + rcvr_dev = (struct pios_rcvr_dev *)PIOS_RCVR_alloc(); + if (!rcvr_dev) { + goto out_fail; + } - rcvr_dev->driver = driver; - rcvr_dev->lower_id = lower_id; + rcvr_dev->driver = driver; + rcvr_dev->lower_id = lower_id; - *rcvr_id = pios_rcvr_num_devs - 1; + *rcvr_id = pios_rcvr_num_devs - 1; - return(0); + return 0; out_fail: - return(-1); + return -1; } int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) { - // Publicly facing API uses channel 1 for first channel - if(channel == 0) - return PIOS_RCVR_INVALID; - else - channel--; + // Publicly facing API uses channel 1 for first channel + if (channel == 0) { + return PIOS_RCVR_INVALID; + } else { + channel--; + } - if (rcvr_id == 0) - return PIOS_RCVR_NODRIVER; + if (rcvr_id == 0) { + return PIOS_RCVR_NODRIVER; + } - struct pios_rcvr_dev * rcvr_dev = &pios_rcvr_devs[rcvr_id]; + struct pios_rcvr_dev *rcvr_dev = &pios_rcvr_devs[rcvr_id]; - if (!PIOS_RCVR_validate(rcvr_dev)) { - /* Undefined RCVR port for this board (see pios_board.c) */ - PIOS_DEBUG_Assert(0); - } + if (!PIOS_RCVR_validate(rcvr_dev)) { + /* Undefined RCVR port for this board (see pios_board.c) */ + PIOS_DEBUG_Assert(0); + } - PIOS_DEBUG_Assert(rcvr_dev->driver->read); + PIOS_DEBUG_Assert(rcvr_dev->driver->read); - return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); + return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); } -#endif +#endif /* if defined(PIOS_INCLUDE_RCVR) */ /** * @} diff --git a/flight/pios/osx/osx/pios_sdcard.c b/flight/pios/osx/osx/pios_sdcard.c index 78da48e46..a1190499e 100644 --- a/flight/pios/osx/osx/pios_sdcard.c +++ b/flight/pios/osx/osx/pios_sdcard.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_sdcard.c + * @file pios_sdcard.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief Sets up basic system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_SDCARD SDCard Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,186 +34,186 @@ /** -* Initialises SPI pins and peripheral to access MMC/SD Card -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ + * Initialises SPI pins and peripheral to access MMC/SD Card + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ int32_t PIOS_SDCARD_Init(void) { - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Connects to SD Card -* \return < 0 if initialisation sequence failed -*/ + * Connects to SD Card + * \return < 0 if initialisation sequence failed + */ int32_t PIOS_SDCARD_PowerOn(void) { - return 0; /* Status should be 0 if nothing went wrong! */ + return 0; /* Status should be 0 if nothing went wrong! */ } /** -* Disconnects from SD Card -* \return < 0 on errors -*/ + * Disconnects from SD Card + * \return < 0 on errors + */ int32_t PIOS_SDCARD_PowerOff(void) { - return 0; // no error + return 0; // no error } /** -* If SD card was previously available: Checks if the SD Card is still -* available by sending the STATUS command.
-* This takes ca. 10 uS -* -* If SD card was previously not available: Checks if the SD Card is -* available by sending the IDLE command at low speed.
-* This takes ca. 500 uS!
-* Once we got a positive response, SDCARD_PowerOn() will be -* called by this function to initialize the card completely. -* -* Example for Connection/Disconnection detection: -* \code -* // this function is called each second from a low-priority task -* // If multiple tasks are accessing the SD card, add a semaphore/mutex -* // to avoid IO access collisions with other tasks! -* u8 sdcard_available; -* int32_t CheckSDCard(void) -* { -* // check if SD card is available -* // High speed access if the card was previously available -* u8 prev_sdcard_available = sdcard_available; -* sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); -* -* if(sdcard_available && !prev_sdcard_available) { -* // SD Card has been connected -* -* // now it's possible to read/write sectors -* -* } else if( !sdcard_available && prev_sdcard_available ) { -* // SD Card has been disconnected -* -* // here you can notify your application about this state -* } -* -* return 0; // no error -* } -* \endcode -* \param[in] was_available should only be set if the SD card was previously available -* \return 0 if no response from SD Card -* \return 1 if SD card is accessible -*/ + * If SD card was previously available: Checks if the SD Card is still + * available by sending the STATUS command.
+ * This takes ca. 10 uS + * + * If SD card was previously not available: Checks if the SD Card is + * available by sending the IDLE command at low speed.
+ * This takes ca. 500 uS!
+ * Once we got a positive response, SDCARD_PowerOn() will be + * called by this function to initialize the card completely. + * + * Example for Connection/Disconnection detection: + * \code + * // this function is called each second from a low-priority task + * // If multiple tasks are accessing the SD card, add a semaphore/mutex + * // to avoid IO access collisions with other tasks! + * u8 sdcard_available; + * int32_t CheckSDCard(void) + * { + * // check if SD card is available + * // High speed access if the card was previously available + * u8 prev_sdcard_available = sdcard_available; + * sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); + * + * if(sdcard_available && !prev_sdcard_available) { + * // SD Card has been connected + * + * // now it's possible to read/write sectors + * + * } else if( !sdcard_available && prev_sdcard_available ) { + * // SD Card has been disconnected + * + * // here you can notify your application about this state + * } + * + * return 0; // no error + * } + * \endcode + * \param[in] was_available should only be set if the SD card was previously available + * \return 0 if no response from SD Card + * \return 1 if SD card is accessible + */ int32_t PIOS_SDCARD_CheckAvailable(uint8_t was_available) { - return 1; /* 1 = available, 0 = not available. */ + return 1; /* 1 = available, 0 = not available. */ } /** -* Sends command to SD card -* \param[in] cmd SD card command -* \param[in] addr 32bit address -* \param[in] crc precalculated CRC -* \return >= 0x00 if command has been sent successfully (contains received byte) -* \return -1 if no response from SD Card (timeout) -*/ + * Sends command to SD card + * \param[in] cmd SD card command + * \param[in] addr 32bit address + * \param[in] crc precalculated CRC + * \return >= 0x00 if command has been sent successfully (contains received byte) + * \return -1 if no response from SD Card (timeout) + */ int32_t PIOS_SDCARD_SendSDCCmd(uint8_t cmd, uint32_t addr, uint8_t crc) { - return -1; + return -1; } /** -* Reads 512 bytes from selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads 512 bytes from selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_SectorRead(uint32_t sector, uint8_t *buffer) { - return -256; + return -256; } /** -* Writes 512 bytes into selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if write operation not accepted -* \return -258 if timeout during write operation -*/ + * Writes 512 bytes into selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if write operation not accepted + * \return -258 if timeout during write operation + */ int32_t PIOS_SDCARD_SectorWrite(uint32_t sector, uint8_t *buffer) { - return -256; + return -256; } /** -* Reads the CID informations from SD Card -* \param[in] *cid pointer to buffer which holds the CID informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads the CID informations from SD Card + * \param[in] *cid pointer to buffer which holds the CID informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_CIDRead(SDCARDCidTypeDef *cid) { - return -256; + return -256; } /** -* Reads the CSD informations from SD Card -* \param[in] *csd pointer to buffer which holds the CSD informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads the CSD informations from SD Card + * \param[in] *csd pointer to buffer which holds the CSD informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_CSDRead(SDCARDCsdTypeDef *csd) { - return -256; + return -256; } /** -* Attempts to write a startup log to the SDCard -* return 0 No errors -* return -1 Error deleting file -* return -2 Error opening file -* return -3 Error writing file -*/ + * Attempts to write a startup log to the SDCard + * return 0 No errors + * return -1 Error deleting file + * return -2 Error opening file + * return -3 Error writing file + */ int32_t PIOS_SDCARD_StartupLog(void) { - return -3; + return -3; } /** @@ -223,78 +223,78 @@ int32_t PIOS_SDCARD_StartupLog(void) */ int32_t PIOS_SDCARD_IsMounted() { - return 1; + return 1; } /** -* Mounts the file system -* param[in] CreateStartupLog 1 = True, 0 = False -* return 0 No errors -* return -1 SDCard not available -* return -2 Cannot find first partition -* return -3 No volume information -* return -4 Error writing startup log file -*/ + * Mounts the file system + * param[in] CreateStartupLog 1 = True, 0 = False + * return 0 No errors + * return -1 SDCard not available + * return -2 Cannot find first partition + * return -3 No volume information + * return -4 Error writing startup log file + */ int32_t PIOS_SDCARD_MountFS(uint32_t CreateStartupLog) { - return 0; + return 0; } /** -* Mounts the file system -* return Amount of free bytes -*/ + * Mounts the file system + * return Amount of free bytes + */ int32_t PIOS_SDCARD_GetFree(void) { - return 10240; + return 10240; } /** -* Read from file -* return 0 No error -* return -1 DFS_ReadFile failed -* return -2 Less bytes read than expected -*/ -//int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len) -//{ - /* No error */ -// return -1; -//} + * Read from file + * return 0 No error + * return -1 DFS_ReadFile failed + * return -2 Less bytes read than expected + */ +// int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len) +// { +/* No error */ +// return -1; +// } /** -* Read a line from file -* returns Number of bytes read -*/ -//int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len) -//{ -// return -1; -//} + * Read a line from file + * returns Number of bytes read + */ +// int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len) +// { +// return -1; +// } /** -* Copy a file -* WARNING: This will overwrite the destination file even if it exists -* param[in] *Source Path to file to copy -* param[in] *Destination Path to destination file -* return 0 No errors -* return -1 Source file doesn't exist -* return -2 Failed to create destination file -* return -3 DFS_ReadFile failed -* return -4 DFS_WriteFile failed -*/ + * Copy a file + * WARNING: This will overwrite the destination file even if it exists + * param[in] *Source Path to file to copy + * param[in] *Destination Path to destination file + * return 0 No errors + * return -1 Source file doesn't exist + * return -2 Failed to create destination file + * return -3 DFS_ReadFile failed + * return -4 DFS_WriteFile failed + */ int32_t PIOS_SDCARD_FileCopy(char *Source, char *Destination) { - return -2; + return -2; } /** -* Delete a file -* param[in] *Filename File to delete -* return 0 No errors -* return -1 Error deleting file -*/ + * Delete a file + * param[in] *Filename File to delete + * return 0 No errors + * return -1 Error deleting file + */ int32_t PIOS_SDCARD_FileDelete(char *Filename) { - return -1; + return -1; } -#endif +#endif /* if defined(PIOS_INCLUDE_SDCARD) */ diff --git a/flight/pios/osx/osx/pios_servo.c b/flight/pios/osx/osx/pios_servo.c index d0cf530d3..c3b4ba05b 100644 --- a/flight/pios/osx/osx/pios_servo.c +++ b/flight/pios/osx/osx/pios_servo.c @@ -6,25 +6,25 @@ * @brief Code to do set RC servo output * @{ * - * @file pios_servo.c + * @file pios_servo.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RC Servo routines (STM32 dependent) * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,36 +39,33 @@ static volatile uint16_t ServoPosition[PIOS_SERVO_NUM_TIMERS]; /** -* Initialise Servos -*/ + * Initialise Servos + */ void PIOS_Servo_Init(void) -{ -} +{} /** -* Set the servo update rate (Max 500Hz) -* \param[in] onetofour Rate for outputs 1 to 4 (Hz) -* \param[in] fivetoeight Rate for outputs 5 to 8 (Hz) -*/ -void PIOS_Servo_SetHz(const uint16_t * banks, uint8_t num_banks) -{ -} + * Set the servo update rate (Max 500Hz) + * \param[in] onetofour Rate for outputs 1 to 4 (Hz) + * \param[in] fivetoeight Rate for outputs 5 to 8 (Hz) + */ +void PIOS_Servo_SetHz(const uint16_t *banks, uint8_t num_banks) +{} /** -* Set servo position -* \param[in] Servo Servo number (0-7) -* \param[in] Position Servo position in milliseconds -*/ + * Set servo position + * \param[in] Servo Servo number (0-7) + * \param[in] Position Servo position in milliseconds + */ void PIOS_Servo_Set(uint8_t Servo, uint16_t Position) { #ifndef PIOS_ENABLE_DEBUG_PINS - /* Make sure servo exists */ - if (Servo < PIOS_SERVO_NUM_OUTPUTS) { - /* Update the position */ - ServoPosition[Servo] = Position; - - } + /* Make sure servo exists */ + if (Servo < PIOS_SERVO_NUM_OUTPUTS) { + /* Update the position */ + ServoPosition[Servo] = Position; + } #endif // PIOS_ENABLE_DEBUG_PINS } -#endif +#endif /* if defined(PIOS_INCLUDE_SERVO) */ diff --git a/flight/pios/osx/osx/pios_sim.c b/flight/pios/osx/osx/pios_sim.c index 0a866a4f7..f8e351dd1 100644 --- a/flight/pios/osx/osx/pios_sim.c +++ b/flight/pios/osx/osx/pios_sim.c @@ -1,40 +1,41 @@ - #include "pios.h" #include "pios_sim_priv.h" #include "sim_model.h" struct pios_sim_state pios_sim_state = { - .accels = {0, 0, 0}, - .gyros = {0, 0, 0}, - .mag = {0, 0, 0}, - .baro = {0}, - .q = {1, 0, 0, 0}, - .velocity = {0, 0, 0}, - .position = {0, 0, 0}, - .actuator = {0, 0, 0, 0, 0, 0, 0, 0} + .accels = { 0, 0, 0 }, + .gyros = { 0, 0, 0 }, + .mag = { 0, 0, 0 }, + .baro = { 0 }, + .q = { 1, 0, 0, 0}, + .velocity = { 0, 0, 0 }, + .position = { 0, 0, 0 }, + .actuator = { 0, 0, 0, 0, 0, 0, 0, 0} }; /** * Initialize the model in the external library * @returns 0 for success, -1 if fails to initialize external library */ -int PIOS_SIM_Init() +int PIOS_SIM_Init() { - if (sim_model_init() != 0) - return -1; - return 0; + if (sim_model_init() != 0) { + return -1; + } + return 0; } /** * Step the model simulation in the external library * @returns 0 for success, -1 for failure to step external library */ -int PIOS_SIM_Step(float dT) +int PIOS_SIM_Step(float dT) { - if (sim_model_step(dT, &pios_sim_state) != 0) - return -1; + if (sim_model_step(dT, &pios_sim_state) != 0) { + return -1; + } - return 0; + return 0; } /** @@ -42,40 +43,44 @@ int PIOS_SIM_Step(float dT) * @param[in] actuator pointer to an array of actuators to set * @param[in] nchannels number of channels that are valid coming in */ -void PIOS_SIM_SetActuator(float * actuator, int nchannels) +void PIOS_SIM_SetActuator(float *actuator, int nchannels) { - for (int i = 0; i < NELEMENTS(pios_sim_state.actuator) && i < nchannels; i++) - pios_sim_state.actuator[i] = actuator[i]; + for (int i = 0; i < NELEMENTS(pios_sim_state.actuator) && i < nchannels; i++) { + pios_sim_state.actuator[i] = actuator[i]; + } } /** * Get the accelerometer data from the simulation model * @param[out] pointer to store the accelerometer data in */ -void PIOS_SIM_GetAccels(float * accels) +void PIOS_SIM_GetAccels(float *accels) { - for (int i = 0; i < NELEMENTS(pios_sim_state.accels); i++) - accels[i] = pios_sim_state.accels[i]; + for (int i = 0; i < NELEMENTS(pios_sim_state.accels); i++) { + accels[i] = pios_sim_state.accels[i]; + } } /** * Get the gyro data from the simulation model * @param[out] pointer to store the gyro data in */ -void PIOS_SIM_GetGyros(float * gyros) +void PIOS_SIM_GetGyros(float *gyros) { - for (int i = 0; i < NELEMENTS(pios_sim_state.accels); i++) - gyros[i] = pios_sim_state.gyros[i]; + for (int i = 0; i < NELEMENTS(pios_sim_state.accels); i++) { + gyros[i] = pios_sim_state.gyros[i]; + } } /** * Get the current attitude from the simulation model * @param[out] quat pointer to store the quaternion attitude in */ -void PIOS_SIM_GetAttitude(float * q) +void PIOS_SIM_GetAttitude(float *q) { - for (int i = 0; i < NELEMENTS(pios_sim_state.q); i++) - q[i] = pios_sim_state.q[i]; + for (int i = 0; i < NELEMENTS(pios_sim_state.q); i++) { + q[i] = pios_sim_state.q[i]; + } } /** @@ -83,10 +88,11 @@ void PIOS_SIM_GetAttitude(float * q) * @param[out] position pointer to store the current position in (cm in NED * frame) */ -void PIOS_SIM_GetVelocity(float * velocity) +void PIOS_SIM_GetVelocity(float *velocity) { - for (int i = 0; i < NELEMENTS(pios_sim_state.velocity); i++) - velocity[i] = pios_sim_state.velocity[i]; + for (int i = 0; i < NELEMENTS(pios_sim_state.velocity); i++) { + velocity[i] = pios_sim_state.velocity[i]; + } } /** @@ -94,8 +100,9 @@ void PIOS_SIM_GetVelocity(float * velocity) * @param[out] position pointer to store the current position in (cm in NED * frame) */ -void PIOS_SIM_GetPosition(float * position) +void PIOS_SIM_GetPosition(float *position) { - for (int i = 0; i < NELEMENTS(pios_sim_state.position); i++) - position[i] = pios_sim_state.position[i]; + for (int i = 0; i < NELEMENTS(pios_sim_state.position); i++) { + position[i] = pios_sim_state.position[i]; + } } diff --git a/flight/pios/osx/osx/pios_sys.c b/flight/pios/osx/osx/pios_sys.c index d2c15c7b5..fac9e85e0 100644 --- a/flight/pios/osx/osx/pios_sys.c +++ b/flight/pios/osx/osx/pios_sys.c @@ -6,26 +6,26 @@ * @brief PIOS System Initialization code * @{ * - * @file pios_sys.c + * @file pios_sys.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Sets up basic STM32 system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,76 +36,75 @@ /** -* Initialises all system peripherals -*/ + * Initialises all system peripherals + */ void PIOS_SYS_Init(void) -{ - -} +{} /** -* Shutdown PIOS and reset the microcontroller:
-*
    -*
  • Disable all RTOS tasks -*
  • Disable all interrupts -*
  • Turn off all board LEDs -*
  • Reset STM32 -*
-* \return < 0 if reset failed -*/ + * Shutdown PIOS and reset the microcontroller:
+ *
    + *
  • Disable all RTOS tasks + *
  • Disable all interrupts + *
  • Turn off all board LEDs + *
  • Reset STM32 + *
+ * \return < 0 if reset failed + */ int32_t PIOS_SYS_Reset(void) { - /* We will never reach this point */ - return -1; + /* We will never reach this point */ + return -1; } /** -* Returns the CPU's flash size (in bytes) -*/ + * Returns the CPU's flash size (in bytes) + */ uint32_t PIOS_SYS_getCPUFlashSize(void) { - return 1024000; + return 1024000; } /** -* Returns the serial number as a string -* param[out] uint8_t pointer to a string which can store at least 12 bytes -* (12 bytes returned for STM32) -* return < 0 if feature not supported -*/ + * Returns the serial number as a string + * param[out] uint8_t pointer to a string which can store at least 12 bytes + * (12 bytes returned for STM32) + * return < 0 if feature not supported + */ int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t *array) { - /* Stored in the so called "electronic signature" */ - for (int i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) { - array[i] = 0xff; - } + /* Stored in the so called "electronic signature" */ + for (int i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) { + array[i] = 0xff; + } - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Returns the serial number as a string -* param[out] str pointer to a string which can store at least 32 digits + zero terminator! -* (24 digits returned for STM32) -* return < 0 if feature not supported -*/ + * Returns the serial number as a string + * param[out] str pointer to a string which can store at least 32 digits + zero terminator! + * (24 digits returned for STM32) + * return < 0 if feature not supported + */ int32_t PIOS_SYS_SerialNumberGet(char *str) { - /* Stored in the so called "electronic signature" */ - int i; - for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { - str[i] = 'F'; - } - str[i] = '\0'; + /* Stored in the so called "electronic signature" */ + int i; - /* No error */ - return 0; + for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { + str[i] = 'F'; + } + str[i] = '\0'; + + /* No error */ + return 0; } -#endif +#endif /* if defined(PIOS_INCLUDE_SYS) */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/osx/osx/pios_tcp.c b/flight/pios/osx/osx/pios_tcp.c index 3938cb2b7..5d6671386 100644 --- a/flight/pios/osx/osx/pios_tcp.c +++ b/flight/pios/osx/osx/pios_tcp.c @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file pios_tcp.c + * @file pios_tcp.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief TCP commands. Inits UDPs, controls UDPs & Interupt handlers. * @see The GNU Public License (GPL) Version 3 @@ -9,19 +9,19 @@ * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,7 +42,6 @@ static int8_t pios_tcp_num_devices = 0; static pios_tcp_dev pios_tcp_devices[PIOS_TCP_MAX_DEV]; - /* Provide a COM driver */ static void PIOS_TCP_ChangeBaud(uint32_t udp_id, uint32_t baud); static void PIOS_TCP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context); @@ -51,103 +50,101 @@ static void PIOS_TCP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail); static void PIOS_TCP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_tcp_com_driver = { - .set_baud = PIOS_TCP_ChangeBaud, - .tx_start = PIOS_TCP_TxStart, - .rx_start = PIOS_TCP_RxStart, - .bind_tx_cb = PIOS_TCP_RegisterTxCallback, - .bind_rx_cb = PIOS_TCP_RegisterRxCallback, + .set_baud = PIOS_TCP_ChangeBaud, + .tx_start = PIOS_TCP_TxStart, + .rx_start = PIOS_TCP_RxStart, + .bind_tx_cb = PIOS_TCP_RegisterTxCallback, + .bind_rx_cb = PIOS_TCP_RegisterRxCallback, }; -static pios_tcp_dev * find_tcp_dev_by_id (uint8_t tcp) +static pios_tcp_dev *find_tcp_dev_by_id(uint8_t tcp) { - if (tcp >= pios_tcp_num_devices) { - /* Undefined UDP port for this board (see pios_board.c) */ - PIOS_Assert(0); - return NULL; - } - - /* Get a handle for the device configuration */ - return &(pios_tcp_devices[tcp]); + if (tcp >= pios_tcp_num_devices) { + /* Undefined UDP port for this board (see pios_board.c) */ + PIOS_Assert(0); + return NULL; + } + + /* Get a handle for the device configuration */ + return &(pios_tcp_devices[tcp]); } /** * RxThread */ - + static void PIOS_TCP_RxTask(void *tcp_dev_n) { - bool rx_need_yield; + bool rx_need_yield; - pios_tcp_dev *tcp_dev = (pios_tcp_dev *) tcp_dev_n; - while(1) { - - if (tcp_dev->rx_in_cb) { - char buffer[PIOS_TCP_RX_BUFFER_SIZE]; - int received = fifoBuf_getData(&tcp_dev->rx_fifo, buffer, PIOS_TCP_RX_BUFFER_SIZE); - (void) (tcp_dev->rx_in_cb)(tcp_dev->rx_in_context, (uint8_t *) buffer, received, NULL, &rx_need_yield); + pios_tcp_dev *tcp_dev = (pios_tcp_dev *)tcp_dev_n; - //fprintf(stderr, "Received %d\n", received); + while (1) { + if (tcp_dev->rx_in_cb) { + char buffer[PIOS_TCP_RX_BUFFER_SIZE]; + int received = fifoBuf_getData(&tcp_dev->rx_fifo, buffer, PIOS_TCP_RX_BUFFER_SIZE); + (void)(tcp_dev->rx_in_cb)(tcp_dev->rx_in_context, (uint8_t *)buffer, received, NULL, &rx_need_yield); + + // fprintf(stderr, "Received %d\n", received); #if defined(PIOS_INCLUDE_FREERTOS) - // Not sure about this - if (rx_need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ - - } else - fifoBuf_clearData(&tcp_dev->rx_fifo); - - vTaskDelay(1); - } + // Not sure about this + if (rx_need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ + } else { + fifoBuf_clearData(&tcp_dev->rx_fifo); + } + + vTaskDelay(1); + } } static void *PIOS_TCP_RxThread(void *tcp_dev_n) { - - /* needed because of FreeRTOS.posix scheduling */ - sigset_t set; - sigfillset(&set); - sigprocmask(SIG_BLOCK, &set, NULL); - - pios_tcp_dev *tcp_dev = (pios_tcp_dev*) tcp_dev_n; - - const int INCOMING_BUFFER_SIZE = 16; - char incoming_buffer[INCOMING_BUFFER_SIZE]; - /** - * com devices never get closed except by application "reboot" - * we also never give up our mutex except for waiting - */ - while(1) { - - tcp_dev->socket_connection = accept(tcp_dev->socket, NULL, NULL); - if (0 > tcp_dev->socket_connection) { - perror("Accept failed"); - close(tcp_dev->socket); - exit(EXIT_FAILURE); - } - - fprintf(stderr, "Connection accepted\n"); - - int received; - do { - // Received is used to track the scoket whereas the dev variable is only updated when it can be - received = read(tcp_dev->socket_connection, incoming_buffer, INCOMING_BUFFER_SIZE); - - //while(fifoBuf_getFree(&tcp_dev->rx_fifo) < received); - - fifoBuf_putData(&tcp_dev->rx_fifo, incoming_buffer, received); - } while(received > 0); - - if (-1 == shutdown(tcp_dev->socket_connection, SHUT_RDWR)) - { - //perror("can not shutdown socket"); - //close(tcp_dev->socket_connection); - //exit(EXIT_FAILURE); - } - close(tcp_dev->socket_connection); - tcp_dev->socket_connection = 0; - } + /* needed because of FreeRTOS.posix scheduling */ + sigset_t set; + + sigfillset(&set); + sigprocmask(SIG_BLOCK, &set, NULL); + + pios_tcp_dev *tcp_dev = (pios_tcp_dev *)tcp_dev_n; + + const int INCOMING_BUFFER_SIZE = 16; + char incoming_buffer[INCOMING_BUFFER_SIZE]; + /** + * com devices never get closed except by application "reboot" + * we also never give up our mutex except for waiting + */ + while (1) { + tcp_dev->socket_connection = accept(tcp_dev->socket, NULL, NULL); + if (0 > tcp_dev->socket_connection) { + perror("Accept failed"); + close(tcp_dev->socket); + exit(EXIT_FAILURE); + } + + fprintf(stderr, "Connection accepted\n"); + + int received; + do { + // Received is used to track the scoket whereas the dev variable is only updated when it can be + received = read(tcp_dev->socket_connection, incoming_buffer, INCOMING_BUFFER_SIZE); + + // while(fifoBuf_getFree(&tcp_dev->rx_fifo) < received); + + fifoBuf_putData(&tcp_dev->rx_fifo, incoming_buffer, received); + } while (received > 0); + + if (-1 == shutdown(tcp_dev->socket_connection, SHUT_RDWR)) { + // perror("can not shutdown socket"); + // close(tcp_dev->socket_connection); + // exit(EXIT_FAILURE); + } + close(tcp_dev->socket_connection); + tcp_dev->socket_connection = 0; + } } @@ -155,132 +152,130 @@ static void *PIOS_TCP_RxThread(void *tcp_dev_n) * Open UDP socket */ xTaskHandle tcpRxTaskHandle; -int32_t PIOS_TCP_Init(uint32_t *tcp_id, const struct pios_tcp_cfg * cfg) +int32_t PIOS_TCP_Init(uint32_t *tcp_id, const struct pios_tcp_cfg *cfg) { - - pios_tcp_dev *tcp_dev = &pios_tcp_devices[pios_tcp_num_devices]; - - pios_tcp_num_devices++; - - - /* initialize */ - tcp_dev->rx_in_cb = NULL; - tcp_dev->tx_out_cb = NULL; - tcp_dev->cfg=cfg; - - /* assign socket */ - tcp_dev->socket = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP); - memset(&tcp_dev->server,0,sizeof(tcp_dev->server)); - memset(&tcp_dev->client,0,sizeof(tcp_dev->client)); - tcp_dev->server.sin_family = AF_INET; - tcp_dev->server.sin_addr.s_addr = INADDR_ANY; //inet_addr(tcp_dev->cfg->ip); - tcp_dev->server.sin_port = htons(tcp_dev->cfg->port); - int res= bind(tcp_dev->socket, (struct sockaddr *)&tcp_dev->server,sizeof(tcp_dev->server)); - if (res == -1) { - perror("Binding socket failed\n"); - exit(EXIT_FAILURE); - } - - res = listen(tcp_dev->socket, 10); - if (res == -1) { - perror("Socket listen failed\n"); - exit(EXIT_FAILURE); - } - - fifoBuf_init(&tcp_dev->rx_fifo, tcp_dev->rx_buffer, PIOS_TCP_RX_BUFFER_SIZE); - - pthread_create(&tcp_dev->rxThread, NULL, PIOS_TCP_RxThread, (void*)tcp_dev); + pios_tcp_dev *tcp_dev = &pios_tcp_devices[pios_tcp_num_devices]; - xTaskCreate(PIOS_TCP_RxTask, (signed char *)"TcpRx", 1024, (void*)tcp_dev, 2, &tcpRxTaskHandle); - - printf("udp dev %i - socket %i opened - result %i\n",pios_tcp_num_devices-1,tcp_dev->socket,res); - - *tcp_id = pios_tcp_num_devices-1; - - return res; + pios_tcp_num_devices++; + + + /* initialize */ + tcp_dev->rx_in_cb = NULL; + tcp_dev->tx_out_cb = NULL; + tcp_dev->cfg = cfg; + + /* assign socket */ + tcp_dev->socket = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP); + memset(&tcp_dev->server, 0, sizeof(tcp_dev->server)); + memset(&tcp_dev->client, 0, sizeof(tcp_dev->client)); + tcp_dev->server.sin_family = AF_INET; + tcp_dev->server.sin_addr.s_addr = INADDR_ANY; // inet_addr(tcp_dev->cfg->ip); + tcp_dev->server.sin_port = htons(tcp_dev->cfg->port); + int res = bind(tcp_dev->socket, (struct sockaddr *)&tcp_dev->server, sizeof(tcp_dev->server)); + if (res == -1) { + perror("Binding socket failed\n"); + exit(EXIT_FAILURE); + } + + res = listen(tcp_dev->socket, 10); + if (res == -1) { + perror("Socket listen failed\n"); + exit(EXIT_FAILURE); + } + + fifoBuf_init(&tcp_dev->rx_fifo, tcp_dev->rx_buffer, PIOS_TCP_RX_BUFFER_SIZE); + + pthread_create(&tcp_dev->rxThread, NULL, PIOS_TCP_RxThread, (void *)tcp_dev); + + xTaskCreate(PIOS_TCP_RxTask, (signed char *)"TcpRx", 1024, (void *)tcp_dev, 2, &tcpRxTaskHandle); + + printf("udp dev %i - socket %i opened - result %i\n", pios_tcp_num_devices - 1, tcp_dev->socket, res); + + *tcp_id = pios_tcp_num_devices - 1; + + return res; } void PIOS_TCP_ChangeBaud(uint32_t tcp_id, uint32_t baud) { - /** - * doesn't apply! - */ + /** + * doesn't apply! + */ } static void PIOS_TCP_RxStart(uint32_t tp_id, uint16_t rx_bytes_avail) { - /** - * lazy! - */ + /** + * lazy! + */ } static void PIOS_TCP_TxStart(uint32_t tcp_id, uint16_t tx_bytes_avail) { - pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id); - - PIOS_Assert(tcp_dev); - - int32_t length,len,rem; - - /** - * we send everything directly whenever notified of data to send (lazy!) - */ - if (tcp_dev->tx_out_cb) { - while (tx_bytes_avail>0) { - bool tx_need_yield = false; - length = (tcp_dev->tx_out_cb)(tcp_dev->tx_out_context, tcp_dev->tx_buffer, PIOS_TCP_RX_BUFFER_SIZE, NULL, &tx_need_yield); - rem = length; - while (rem>0) { - if(tcp_dev->socket_connection != 0) { - len = write(tcp_dev->socket_connection, tcp_dev->tx_buffer, length); - } - if (len<=0) { - rem=0; - } else { - rem -= len; - } - } - tx_bytes_avail -= length; + pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id); + + PIOS_Assert(tcp_dev); + + int32_t length, len, rem; + + /** + * we send everything directly whenever notified of data to send (lazy!) + */ + if (tcp_dev->tx_out_cb) { + while (tx_bytes_avail > 0) { + bool tx_need_yield = false; + length = (tcp_dev->tx_out_cb)(tcp_dev->tx_out_context, tcp_dev->tx_buffer, PIOS_TCP_RX_BUFFER_SIZE, NULL, &tx_need_yield); + rem = length; + while (rem > 0) { + if (tcp_dev->socket_connection != 0) { + len = write(tcp_dev->socket_connection, tcp_dev->tx_buffer, length); + } + if (len <= 0) { + rem = 0; + } else { + rem -= len; + } + } + tx_bytes_avail -= length; #if defined(PIOS_INCLUDE_FREERTOS) - // Not sure about this - if (tx_need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ - } - } - + // Not sure about this + if (tx_need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ + } + } } static void PIOS_TCP_RegisterRxCallback(uint32_t tcp_id, pios_com_callback rx_in_cb, uint32_t context) { - pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id); - - PIOS_Assert(tcp_dev); - - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - tcp_dev->rx_in_context = context; - tcp_dev->rx_in_cb = rx_in_cb; + pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id); + + PIOS_Assert(tcp_dev); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + tcp_dev->rx_in_context = context; + tcp_dev->rx_in_cb = rx_in_cb; } static void PIOS_TCP_RegisterTxCallback(uint32_t tcp_id, pios_com_callback tx_out_cb, uint32_t context) { - pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id); - - PIOS_Assert(tcp_dev); - - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - tcp_dev->tx_out_context = context; - tcp_dev->tx_out_cb = tx_out_cb; + pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id); + + PIOS_Assert(tcp_dev); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + tcp_dev->tx_out_context = context; + tcp_dev->tx_out_cb = tx_out_cb; } -#endif +#endif /* if defined(PIOS_INCLUDE_TCP) */ diff --git a/flight/pios/osx/osx/pios_udp.c b/flight/pios/osx/osx/pios_udp.c index a263116d4..3f2425f19 100644 --- a/flight/pios/osx/osx/pios_udp.c +++ b/flight/pios/osx/osx/pios_udp.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_udp.c + * @file pios_udp.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief UDP commands. Inits UDPs, controls UDPs & Interupt handlers. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_UDP UDP Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,7 +43,6 @@ static int8_t pios_udp_num_devices = 0; static pios_udp_dev pios_udp_devices[PIOS_UDP_MAX_DEV]; - /* Provide a COM driver */ static void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud); static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context); @@ -52,193 +51,182 @@ static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail); static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_udp_com_driver = { - .set_baud = PIOS_UDP_ChangeBaud, - .tx_start = PIOS_UDP_TxStart, - .rx_start = PIOS_UDP_RxStart, - .bind_tx_cb = PIOS_UDP_RegisterTxCallback, - .bind_rx_cb = PIOS_UDP_RegisterRxCallback, + .set_baud = PIOS_UDP_ChangeBaud, + .tx_start = PIOS_UDP_TxStart, + .rx_start = PIOS_UDP_RxStart, + .bind_tx_cb = PIOS_UDP_RegisterTxCallback, + .bind_rx_cb = PIOS_UDP_RegisterRxCallback, }; -static pios_udp_dev * find_udp_dev_by_id (uint8_t udp) +static pios_udp_dev *find_udp_dev_by_id(uint8_t udp) { - if (udp >= pios_udp_num_devices) { - /* Undefined UDP port for this board (see pios_board.c) */ - PIOS_Assert(0); - return NULL; - } + if (udp >= pios_udp_num_devices) { + /* Undefined UDP port for this board (see pios_board.c) */ + PIOS_Assert(0); + return NULL; + } - /* Get a handle for the device configuration */ - return &(pios_udp_devices[udp]); + /* Get a handle for the device configuration */ + return &(pios_udp_devices[udp]); } /** * RxThread */ -void PIOS_UDP_RxThread(void * udp_dev_n) +void PIOS_UDP_RxThread(void *udp_dev_n) { + /* needed because of FreeRTOS.posix scheduling */ + sigset_t set; - /* needed because of FreeRTOS.posix scheduling */ - sigset_t set; - sigfillset(&set); - sigprocmask(SIG_BLOCK, &set, NULL); + sigfillset(&set); + sigprocmask(SIG_BLOCK, &set, NULL); - pios_udp_dev * udp_dev = (pios_udp_dev*) udp_dev_n; + pios_udp_dev *udp_dev = (pios_udp_dev *)udp_dev_n; - /** - * com devices never get closed except by application "reboot" - * we also never give up our mutex except for waiting - */ - while(1) { - - /** - * receive - */ - int received; - udp_dev->clientLength=sizeof(udp_dev->client); - if ((received = recvfrom(udp_dev->socket, - &udp_dev->rx_buffer, - PIOS_UDP_RX_BUFFER_SIZE, - 0, - (struct sockaddr *) &udp_dev->client, - (socklen_t*)&udp_dev->clientLength)) >= 0) - { - - /* copy received data to buffer if possible */ - /* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */ - /* (thats what the USART driver does too!) */ - bool rx_need_yield = false; - if (udp_dev->rx_in_cb) { - (void) (udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield); - } + /** + * com devices never get closed except by application "reboot" + * we also never give up our mutex except for waiting + */ + while (1) { + /** + * receive + */ + int received; + udp_dev->clientLength = sizeof(udp_dev->client); + if ((received = recvfrom(udp_dev->socket, + &udp_dev->rx_buffer, + PIOS_UDP_RX_BUFFER_SIZE, + 0, + (struct sockaddr *)&udp_dev->client, + (socklen_t *)&udp_dev->clientLength)) >= 0) { + /* copy received data to buffer if possible */ + /* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */ + /* (thats what the USART driver does too!) */ + bool rx_need_yield = false; + if (udp_dev->rx_in_cb) { + (void)(udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (rx_need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ - - } - - - } + if (rx_need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ + } + } } /** -* Open UDP socket -*/ -int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg) + * Open UDP socket + */ +int32_t PIOS_UDP_Init(uint32_t *udp_id, const struct pios_udp_cfg *cfg) { + pios_udp_dev *udp_dev = &pios_udp_devices[pios_udp_num_devices]; - pios_udp_dev * udp_dev = &pios_udp_devices[pios_udp_num_devices]; - - pios_udp_num_devices++; + pios_udp_num_devices++; - /* initialize */ - udp_dev->rx_in_cb = NULL; - udp_dev->tx_out_cb = NULL; - udp_dev->cfg=cfg; + /* initialize */ + udp_dev->rx_in_cb = NULL; + udp_dev->tx_out_cb = NULL; + udp_dev->cfg = cfg; - /* assign socket */ - udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - memset(&udp_dev->server,0,sizeof(udp_dev->server)); - memset(&udp_dev->client,0,sizeof(udp_dev->client)); - udp_dev->server.sin_family = AF_INET; - udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip); - udp_dev->server.sin_port = htons(udp_dev->cfg->port); - int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server)); + /* assign socket */ + udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + memset(&udp_dev->server, 0, sizeof(udp_dev->server)); + memset(&udp_dev->client, 0, sizeof(udp_dev->client)); + udp_dev->server.sin_family = AF_INET; + udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip); + udp_dev->server.sin_port = htons(udp_dev->cfg->port); + int res = bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server, sizeof(udp_dev->server)); - /* Create transmit thread for this connection */ - xTaskCreate(PIOS_UDP_RxThread, (signed char *)"UdpRx", 1024, (void*)udp_dev, 2, &udp_dev->rxThread); + /* Create transmit thread for this connection */ + xTaskCreate(PIOS_UDP_RxThread, (signed char *)"UdpRx", 1024, (void *)udp_dev, 2, &udp_dev->rxThread); - printf("udp dev %i - socket %i opened - result %i\n",pios_udp_num_devices-1,udp_dev->socket,res); + printf("udp dev %i - socket %i opened - result %i\n", pios_udp_num_devices - 1, udp_dev->socket, res); - *udp_id = pios_udp_num_devices-1; + *udp_id = pios_udp_num_devices - 1; - return res; + return res; } void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud) { - /** - * doesn't apply! - */ + /** + * doesn't apply! + */ } static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail) { - /** - * lazy! - */ + /** + * lazy! + */ } static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail) { - pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id); - PIOS_Assert(udp_dev); + PIOS_Assert(udp_dev); - int32_t length,len,rem; - - /** - * we send everything directly whenever notified of data to send (lazy!) - */ - if (udp_dev->tx_out_cb) { - while (tx_bytes_avail>0) { - bool tx_need_yield = false; - length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield); - rem = length; - while (rem>0) { - len = sendto(udp_dev->socket, udp_dev->tx_buffer, length, 0, - (struct sockaddr *) &udp_dev->client, - sizeof(udp_dev->client)); - if (len<=0) { - rem=0; - } else { - rem -= len; - } - } - tx_bytes_avail -= length; - } - } + int32_t length, len, rem; + /** + * we send everything directly whenever notified of data to send (lazy!) + */ + if (udp_dev->tx_out_cb) { + while (tx_bytes_avail > 0) { + bool tx_need_yield = false; + length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield); + rem = length; + while (rem > 0) { + len = sendto(udp_dev->socket, udp_dev->tx_buffer, length, 0, + (struct sockaddr *)&udp_dev->client, + sizeof(udp_dev->client)); + if (len <= 0) { + rem = 0; + } else { + rem -= len; + } + } + tx_bytes_avail -= length; + } + } } static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context) { - pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id); - PIOS_Assert(udp_dev); + PIOS_Assert(udp_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - udp_dev->rx_in_context = context; - udp_dev->rx_in_cb = rx_in_cb; + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + udp_dev->rx_in_context = context; + udp_dev->rx_in_cb = rx_in_cb; } static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context) { - pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id); - PIOS_Assert(udp_dev); + PIOS_Assert(udp_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - udp_dev->tx_out_context = context; - udp_dev->tx_out_cb = tx_out_cb; + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + udp_dev->tx_out_context = context; + udp_dev->tx_out_cb = tx_out_cb; } - - - -#endif +#endif /* if defined(PIOS_INCLUDE_UDP) */ diff --git a/flight/pios/osx/osx/pios_wdg.c b/flight/pios/osx/osx/pios_wdg.c index f2c35dac2..59dfcf858 100644 --- a/flight/pios/osx/osx/pios_wdg.c +++ b/flight/pios/osx/osx/pios_wdg.c @@ -8,12 +8,12 @@ * * @file pios_spi.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Hardware Abstraction Layer for SPI ports of STM32 * @see The GNU Public License (GPL) Version 3 * @notes * - * The PIOS Watchdog provides a HAL to initialize a watchdog + * The PIOS Watchdog provides a HAL to initialize a watchdog * *****************************************************************************/ /* @@ -40,17 +40,17 @@ unsigned int wdg_cleared_time; unsigned int wdg_last_update_time; bool wdg_expired; -/** +/** * @brief Initialize the watchdog timer for a specified timeout * - * It is important to note that this function returns the achieved timeout - * for this hardware. For hardware indendence this should be checked when - * scheduling updates. Other hardware dependent details may need to be - * considered such as a window time which sets a minimum update time, - * and this function should return a recommended delay for clearing. + * It is important to note that this function returns the achieved timeout + * for this hardware. For hardware indendence this should be checked when + * scheduling updates. Other hardware dependent details may need to be + * considered such as a window time which sets a minimum update time, + * and this function should return a recommended delay for clearing. * * For the STM32 nominal clock rate is 32 khz, but for the maximum clock rate of - * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is + * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is * set in the watchdog assumes the nominal clock rate, but the delay for FreeRTOS * to use is 75% of the minimal delay. * @@ -59,74 +59,74 @@ bool wdg_expired; */ void PIOS_WDG_Init() { - wdg_registered_flags = 0; - wdg_updated_flags = 0; + wdg_registered_flags = 0; + wdg_updated_flags = 0; } /** - * @brief Register a module against the watchdog - * + * @brief Register a module against the watchdog + * * There are two ways to use PIOS WDG: this is for when - * multiple modules must be monitored. In this case they - * must first register against the watchdog system and + * multiple modules must be monitored. In this case they + * must first register against the watchdog system and * only when all of the modules have been updated with the - * watchdog be cleared. Each module must have its own - * bit in the 16 bit + * watchdog be cleared. Each module must have its own + * bit in the 16 bit * * @param[in] flag the bit this module wants to use * @returns True if that bit is unregistered */ -bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) +bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) { - wdg_registered_flags |= flag_requested; - return true; + wdg_registered_flags |= flag_requested; + return true; } /** * @brief Function called by modules to indicate they are still running * - * This function will set this flag in the active flags register (which is + * This function will set this flag in the active flags register (which is * a backup regsiter) and if all the registered flags are set will clear * the watchdog and set only this flag in the backup register * * @param[in] flag the flag to set * @return true if the watchdog cleared, false if flags are pending */ -bool PIOS_WDG_UpdateFlag(uint16_t flag) -{ - PIOS_WDG_Check(); - wdg_updated_flags |= flag; - if( wdg_updated_flags == wdg_registered_flags) { - wdg_last_update_time = PIOS_DELAY_DiffuS(wdg_cleared_time); - wdg_updated_flags = 0; - wdg_cleared_time = PIOS_DELAY_GetRaw(); - } - return true; +bool PIOS_WDG_UpdateFlag(uint16_t flag) +{ + PIOS_WDG_Check(); + wdg_updated_flags |= flag; + if (wdg_updated_flags == wdg_registered_flags) { + wdg_last_update_time = PIOS_DELAY_DiffuS(wdg_cleared_time); + wdg_updated_flags = 0; + wdg_cleared_time = PIOS_DELAY_GetRaw(); + } + return true; } -/** +/** * @brief Returns the flags that were set at bootup - * - * This is used for diagnostics, if only one flag not set this + * + * This is used for diagnostics, if only one flag not set this * was likely the module that wasn't running before reset - * + * * @return The active flags register from bootup */ uint16_t PIOS_WDG_GetBootupFlags() { - return (uint16_t) 0xffff; + return (uint16_t)0xffff; } -/** +/** * @brief Returns the currently active flags - * + * * For external monitoring - * + * * @return The active flags register */ uint16_t PIOS_WDG_GetActiveFlags() { - return (uint16_t) 0xffff; + return (uint16_t)0xffff; } /** @@ -135,19 +135,19 @@ uint16_t PIOS_WDG_GetActiveFlags() * This function must be called at the appropriate delay to prevent a reset event occuring */ void PIOS_WDG_Clear(void) -{ -} +{} /** - * @brief This function returns true if the watchdog would + * @brief This function returns true if the watchdog would * have expired */ bool PIOS_WDG_Check() { - if(PIOS_DELAY_DiffuS(wdg_cleared_time) > 250000) { - if(!wdg_expired) - fprintf(stderr, "Watchdog fired!\r\n"); - wdg_expired = true; - } - return wdg_expired; -} \ No newline at end of file + if (PIOS_DELAY_DiffuS(wdg_cleared_time) > 250000) { + if (!wdg_expired) { + fprintf(stderr, "Watchdog fired!\r\n"); + } + wdg_expired = true; + } + return wdg_expired; +} diff --git a/flight/pios/osx/pios.h b/flight/pios/osx/pios.h index cc49e781e..2a59131bd 100644 --- a/flight/pios/osx/pios.h +++ b/flight/pios/osx/pios.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios.h + * @file pios.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Main PiOS header. + * @brief Main PiOS header. * - Central header for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,7 +39,7 @@ #include "queue.h" #include "semphr.h" -#define vPortInitialiseBlocks(); +#define vPortInitialiseBlocks() ; #endif /* C Lib Includes */ diff --git a/flight/pios/pios.h b/flight/pios/pios.h index ec3e105d4..6ac38703b 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -1,24 +1,24 @@ /** ****************************************************************************** - * @file pios.h + * @file pios.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013 * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 * @brief Main PiOS header. * @see The GNU Public License (GPL) Version 3 *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/pios_sim_posix.h b/flight/pios/pios_sim_posix.h index 86fb8bf41..642c0b6e9 100644 --- a/flight/pios/pios_sim_posix.h +++ b/flight/pios/pios_sim_posix.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios.h + * @file pios.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Main PiOS header. + * @brief Main PiOS header. * - Central header for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/posix/pios_bl_helper.c b/flight/pios/posix/pios_bl_helper.c index a7920e5d2..ec122564b 100644 --- a/flight/pios/posix/pios_bl_helper.c +++ b/flight/pios/posix/pios_bl_helper.c @@ -35,19 +35,19 @@ uint32_t PIOS_BL_HELPER_CRC_Memory_Calc() { - return 0; + return 0; } extern const struct fw_version_info fw_version_blob; -void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t * array, uint8_t size) +void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t *array, uint8_t size) { - uint8_t * desc = (uint8_t *) &fw_version_blob; - for (uint32_t i = 0; i < size; i++) { - array[i] = desc[i]; - } + uint8_t *desc = (uint8_t *)&fw_version_blob; + + for (uint32_t i = 0; i < size; i++) { + array[i] = desc[i]; + } } void PIOS_BL_HELPER_CRC_Ini() -{ -} +{} #endif diff --git a/flight/pios/posix/pios_board_info.c b/flight/pios/posix/pios_board_info.c index c273a881d..2ef651a21 100644 --- a/flight/pios/posix/pios_board_info.c +++ b/flight/pios/posix/pios_board_info.c @@ -4,17 +4,17 @@ #include "pios_board_info.h" const struct pios_board_info pios_board_info_blob = { - .magic = PIOS_BOARD_INFO_BLOB_MAGIC, - .board_type = BOARD_TYPE, - .board_rev = BOARD_REVISION, - .bl_rev = BOOTLOADER_VERSION, - .hw_type = HW_TYPE, - .fw_base = FW_BANK_BASE, - .fw_size = FW_BANK_SIZE - FW_DESC_SIZE, - .desc_base = FW_BANK_BASE + FW_BANK_SIZE - FW_DESC_SIZE, - .desc_size = FW_DESC_SIZE, + .magic = PIOS_BOARD_INFO_BLOB_MAGIC, + .board_type = BOARD_TYPE, + .board_rev = BOARD_REVISION, + .bl_rev = BOOTLOADER_VERSION, + .hw_type = HW_TYPE, + .fw_base = FW_BANK_BASE, + .fw_size = FW_BANK_SIZE - FW_DESC_SIZE, + .desc_base = FW_BANK_BASE + FW_BANK_SIZE - FW_DESC_SIZE, + .desc_size = FW_DESC_SIZE, #ifdef EE_BANK_BASE - .ee_base = EE_BANK_BASE, - .ee_size = EE_BANK_SIZE, + .ee_base = EE_BANK_BASE, + .ee_size = EE_BANK_SIZE, #endif }; diff --git a/flight/pios/posix/pios_com.c b/flight/pios/posix/pios_com.c index 4f2e1e3c9..8eab84a4e 100644 --- a/flight/pios/posix/pios_com.c +++ b/flight/pios/posix/pios_com.c @@ -6,26 +6,26 @@ * @brief Hardware communication layer * @{ * - * @file pios_com.c + * @file pios_com.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM layer functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,462 +38,470 @@ #include #if !defined(PIOS_INCLUDE_FREERTOS) -#include "pios_delay.h" /* PIOS_DELAY_WaitmS */ +#include "pios_delay.h" /* PIOS_DELAY_WaitmS */ #endif enum pios_com_dev_magic { - PIOS_COM_DEV_MAGIC = 0xaa55aa55, + PIOS_COM_DEV_MAGIC = 0xaa55aa55, }; struct pios_com_dev { - enum pios_com_dev_magic magic; - uint32_t lower_id; - const struct pios_com_driver * driver; + enum pios_com_dev_magic magic; + uint32_t lower_id; + const struct pios_com_driver *driver; #if defined(PIOS_INCLUDE_FREERTOS) - xSemaphoreHandle tx_sem; - xSemaphoreHandle rx_sem; + xSemaphoreHandle tx_sem; + xSemaphoreHandle rx_sem; #endif - bool has_rx; - bool has_tx; + bool has_rx; + bool has_tx; - t_fifo_buffer rx; - t_fifo_buffer tx; + t_fifo_buffer rx; + t_fifo_buffer tx; }; -static bool PIOS_COM_validate(struct pios_com_dev * com_dev) +static bool PIOS_COM_validate(struct pios_com_dev *com_dev) { - return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC)); + return com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) && 0 // static struct pios_com_dev * PIOS_COM_alloc(void) -//{ -// struct pios_com_dev * com_dev; +// { +// struct pios_com_dev * com_dev; // -// com_dev = (struct pios_com_dev *)malloc(sizeof(*com_dev)); -// if (!com_dev) return (NULL); +// com_dev = (struct pios_com_dev *)malloc(sizeof(*com_dev)); +// if (!com_dev) return (NULL); // -// com_dev->magic = PIOS_COM_DEV_MAGIC; -// return(com_dev); -//} +// com_dev->magic = PIOS_COM_DEV_MAGIC; +// return(com_dev); +// } #else static struct pios_com_dev pios_com_devs[PIOS_COM_MAX_DEVS]; static uint8_t pios_com_num_devs; static uint32_t PIOS_COM_create(void) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) { - return (PIOS_COM_MAX_DEVS+1); - } + if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) { + return PIOS_COM_MAX_DEVS + 1; + } - com_dev = &pios_com_devs[pios_com_num_devs++]; - com_dev->magic = PIOS_COM_DEV_MAGIC; + com_dev = &pios_com_devs[pios_com_num_devs++]; + com_dev->magic = PIOS_COM_DEV_MAGIC; - return (pios_com_num_devs); + return pios_com_num_devs; } -static struct pios_com_dev * PIOS_COM_find_dev(uint32_t com_dev_id) +static struct pios_com_dev *PIOS_COM_find_dev(uint32_t com_dev_id) { - if (!com_dev_id) return NULL; - if (com_dev_id>pios_com_num_devs+1) return NULL; - return &pios_com_devs[com_dev_id-1]; + if (!com_dev_id) { + return NULL; + } + if (com_dev_id > pios_com_num_devs + 1) { + return NULL; + } + return &pios_com_devs[com_dev_id - 1]; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) && 0 */ -static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); -static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); -static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield); -static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield); +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); +static void PIOS_COM_UnblockRx(struct pios_com_dev *com_dev, bool *need_yield); +static void PIOS_COM_UnblockTx(struct pios_com_dev *com_dev, bool *need_yield); /** - * Initialises COM layer - * \param[out] handle - * \param[in] driver - * \param[in] id - * \return < 0 if initialisation failed - */ -int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len) + * Initialises COM layer + * \param[out] handle + * \param[in] driver + * \param[in] id + * \return < 0 if initialisation failed + */ +int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len) { - PIOS_Assert(com_id); - PIOS_Assert(driver); + PIOS_Assert(com_id); + PIOS_Assert(driver); - bool has_rx = (rx_buffer && rx_buffer_len > 0); - bool has_tx = (tx_buffer && tx_buffer_len > 0); - PIOS_Assert(has_rx || has_tx); - PIOS_Assert(driver->bind_tx_cb || !has_tx); - PIOS_Assert(driver->bind_rx_cb || !has_rx); + bool has_rx = (rx_buffer && rx_buffer_len > 0); + bool has_tx = (tx_buffer && tx_buffer_len > 0); + PIOS_Assert(has_rx || has_tx); + PIOS_Assert(driver->bind_tx_cb || !has_tx); + PIOS_Assert(driver->bind_rx_cb || !has_rx); - uint32_t com_dev_id; - struct pios_com_dev * com_dev; + uint32_t com_dev_id; + struct pios_com_dev *com_dev; - com_dev_id = PIOS_COM_create(); - com_dev = PIOS_COM_find_dev(com_dev_id); - if (!com_dev) goto out_fail; + com_dev_id = PIOS_COM_create(); + com_dev = PIOS_COM_find_dev(com_dev_id); + if (!com_dev) { + goto out_fail; + } - com_dev->driver = driver; - com_dev->lower_id = lower_id; + com_dev->driver = driver; + com_dev->lower_id = lower_id; - com_dev->has_rx = has_rx; - com_dev->has_tx = has_tx; + com_dev->has_rx = has_rx; + com_dev->has_tx = has_tx; - if (has_rx) { - fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len); + if (has_rx) { + fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len); #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(com_dev->rx_sem); -#endif /* PIOS_INCLUDE_FREERTOS */ - (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, com_dev_id); - if (com_dev->driver->rx_start) { - /* Start the receiver */ - (com_dev->driver->rx_start)(com_dev->lower_id, - fifoBuf_getFree(&com_dev->rx)); - } - } + vSemaphoreCreateBinary(com_dev->rx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, com_dev_id); + if (com_dev->driver->rx_start) { + /* Start the receiver */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } + } - if (has_tx) { - fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); + if (has_tx) { + fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(com_dev->tx_sem); -#endif /* PIOS_INCLUDE_FREERTOS */ - (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, com_dev_id); - } + vSemaphoreCreateBinary(com_dev->tx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, com_dev_id); + } - *com_id = com_dev_id; - return(0); + *com_id = com_dev_id; + return 0; out_fail: - return(-1); + return -1; } -static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield) +static void PIOS_COM_UnblockRx(struct pios_com_dev *com_dev, bool *need_yield) { #if defined(PIOS_INCLUDE_FREERTOS) - static signed portBASE_TYPE xHigherPriorityTaskWoken; - xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken); + static signed portBASE_TYPE xHigherPriorityTaskWoken; + xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken != pdFALSE) { - *need_yield = true; - } else { - *need_yield = false; - } + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } #else - *need_yield = false; + *need_yield = false; #endif } -static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield) +static void PIOS_COM_UnblockTx(struct pios_com_dev *com_dev, bool *need_yield) { #if defined(PIOS_INCLUDE_FREERTOS) - static signed portBASE_TYPE xHigherPriorityTaskWoken; - xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken); + static signed portBASE_TYPE xHigherPriorityTaskWoken; + xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken != pdFALSE) { - *need_yield = true; - } else { - *need_yield = false; - } + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } #else - *need_yield = false; + *need_yield = false; #endif } -static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(context); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(context); - bool valid = PIOS_COM_validate(com_dev); - PIOS_Assert(valid); - PIOS_Assert(com_dev->has_rx); + bool valid = PIOS_COM_validate(com_dev); - PIOS_IRQ_Disable(); - uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len); - PIOS_IRQ_Enable(); + PIOS_Assert(valid); + PIOS_Assert(com_dev->has_rx); - if (bytes_into_fifo > 0) { - /* Data has been added to the buffer */ - PIOS_COM_UnblockRx(com_dev, need_yield); - } + PIOS_IRQ_Disable(); + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len); + PIOS_IRQ_Enable(); - if (headroom) { - *headroom = fifoBuf_getFree(&com_dev->rx); - } + if (bytes_into_fifo > 0) { + /* Data has been added to the buffer */ + PIOS_COM_UnblockRx(com_dev, need_yield); + } - return (bytes_into_fifo); + if (headroom) { + *headroom = fifoBuf_getFree(&com_dev->rx); + } + + return bytes_into_fifo; } -static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(context); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(context); - bool valid = PIOS_COM_validate(com_dev); - PIOS_Assert(valid); - PIOS_Assert(buf); - PIOS_Assert(buf_len); - PIOS_Assert(com_dev->has_tx); + bool valid = PIOS_COM_validate(com_dev); - PIOS_IRQ_Disable(); - uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len); - PIOS_IRQ_Enable(); + PIOS_Assert(valid); + PIOS_Assert(buf); + PIOS_Assert(buf_len); + PIOS_Assert(com_dev->has_tx); - if (bytes_from_fifo > 0) { - /* More space has been made in the buffer */ - PIOS_COM_UnblockTx(com_dev, need_yield); - } + PIOS_IRQ_Disable(); + uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len); + PIOS_IRQ_Enable(); - if (headroom) { - *headroom = fifoBuf_getUsed(&com_dev->tx); - } + if (bytes_from_fifo > 0) { + /* More space has been made in the buffer */ + PIOS_COM_UnblockTx(com_dev, need_yield); + } - return (bytes_from_fifo); + if (headroom) { + *headroom = fifoBuf_getUsed(&com_dev->tx); + } + + return bytes_from_fifo; } /** -* Change the port speed without re-initializing -* \param[in] port COM port -* \param[in] baud Requested baud rate -* \return -1 if port not available -* \return 0 on success -*/ + * Change the port speed without re-initializing + * \param[in] port COM port + * \param[in] baud Requested baud rate + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - /* Invoke the driver function if it exists */ - if (com_dev->driver->set_baud) { - com_dev->driver->set_baud(com_dev->lower_id, baud); - } + /* Invoke the driver function if it exists */ + if (com_dev->driver->set_baud) { + com_dev->driver->set_baud(com_dev->lower_id, baud); + } - return 0; + return 0; } /** -* Sends a package over given port -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a package over given port + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - PIOS_Assert(com_dev->has_tx); + PIOS_Assert(com_dev->has_tx); - if (len >= fifoBuf_getFree(&com_dev->tx)) { - /* Buffer cannot accept all requested bytes (retry) */ - return -2; - } + if (len >= fifoBuf_getFree(&com_dev->tx)) { + /* Buffer cannot accept all requested bytes (retry) */ + return -2; + } - PIOS_IRQ_Disable(); - uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len); - PIOS_IRQ_Enable(); + PIOS_IRQ_Disable(); + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len); + PIOS_IRQ_Enable(); - if (bytes_into_fifo > 0) { - /* More data has been put in the tx buffer, make sure the tx is started */ - if (com_dev->driver->tx_start) { - com_dev->driver->tx_start(com_dev->lower_id, - fifoBuf_getUsed(&com_dev->tx)); - } - } + if (bytes_into_fifo > 0) { + /* More data has been put in the tx buffer, make sure the tx is started */ + if (com_dev->driver->tx_start) { + com_dev->driver->tx_start(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } + } - return (0); + return 0; } /** -* Sends a package over given port -* (blocking function) -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a package over given port + * (blocking function) + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - PIOS_Assert(com_dev->has_tx); + PIOS_Assert(com_dev->has_tx); - int32_t rc; - do { - rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len); + int32_t rc; + do { + rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len); #if defined(PIOS_INCLUDE_FREERTOS) - if (rc == -2) { - /* Make sure the transmitter is running while we wait */ - if (com_dev->driver->tx_start) { - (com_dev->driver->tx_start)(com_dev->lower_id, - fifoBuf_getUsed(&com_dev->tx)); - } - if (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) { - return -3; - } - } + if (rc == -2) { + /* Make sure the transmitter is running while we wait */ + if (com_dev->driver->tx_start) { + (com_dev->driver->tx_start)(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } + if (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) { + return -3; + } + } #endif - } while (rc == -2); + } while (rc == -2); - return rc; + return rc; } /** -* Sends a single character over given port -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a single character over given port + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c) { - return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1); + return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1); } /** -* Sends a single character over given port -* (blocking function) -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a single character over given port + * (blocking function) + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendChar(uint32_t com_id, char c) { - return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1); + return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1); } /** -* Sends a string over given port -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a string over given port + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str) { - return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str)); + return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str)); } /** -* Sends a string over given port -* (blocking function) -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a string over given port + * (blocking function) + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendString(uint32_t com_id, const char *str) { - return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str)); + return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str)); } /** -* Sends a formatted string (-> printf) over given port -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* 128 characters supported maximum! -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * 128 characters supported maximum! + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Sends a formatted string (-> printf) over given port -* (blocking function) -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * (blocking function) + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Transfer bytes from port buffers into another buffer -* \param[in] port COM port -* \returns Byte from buffer -*/ -uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms) + * Transfer bytes from port buffers into another buffer + * \param[in] port COM port + * \returns Byte from buffer + */ +uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t *buf, uint16_t buf_len, uint32_t timeout_ms) { - PIOS_Assert(buf); - PIOS_Assert(buf_len); + PIOS_Assert(buf); + PIOS_Assert(buf_len); - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - PIOS_Assert(0); - } - PIOS_Assert(com_dev->has_rx); + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + PIOS_Assert(0); + } + PIOS_Assert(com_dev->has_rx); - check_again: - PIOS_IRQ_Disable(); - uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len); - PIOS_IRQ_Enable(); +check_again: + PIOS_IRQ_Disable(); + uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len); + PIOS_IRQ_Enable(); - if (bytes_from_fifo == 0 && timeout_ms > 0) { - /* No more bytes in receive buffer */ - /* Make sure the receiver is running while we wait */ - if (com_dev->driver->rx_start) { - /* Notify the lower layer that there is now room in the rx buffer */ - (com_dev->driver->rx_start)(com_dev->lower_id, - fifoBuf_getFree(&com_dev->rx)); - } + if (bytes_from_fifo == 0 && timeout_ms > 0) { + /* No more bytes in receive buffer */ + /* Make sure the receiver is running while we wait */ + if (com_dev->driver->rx_start) { + /* Notify the lower layer that there is now room in the rx buffer */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) { - /* Make sure we don't come back here again */ - timeout_ms = 0; - goto check_again; - } + if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) { + /* Make sure we don't come back here again */ + timeout_ms = 0; + goto check_again; + } #else - PIOS_DELAY_WaitmS(1); - timeout_ms--; - goto check_again; + PIOS_DELAY_WaitmS(1); + timeout_ms--; + goto check_again; #endif - } + } - /* Return received byte */ - return (bytes_from_fifo); + /* Return received byte */ + return bytes_from_fifo; } /** @@ -503,21 +511,22 @@ uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len */ bool PIOS_COM_Available(uint32_t com_id) { - struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + struct pios_com_dev *com_dev = PIOS_COM_find_dev(com_id); - if (!PIOS_COM_validate(com_dev)) { - return false; - } + if (!PIOS_COM_validate(com_dev)) { + return false; + } - // If a driver does not provide a query method assume always - // available if valid - if (com_dev->driver->available == NULL) - return true; + // If a driver does not provide a query method assume always + // available if valid + if (com_dev->driver->available == NULL) { + return true; + } - return (com_dev->driver->available)(com_dev->lower_id); + return (com_dev->driver->available)(com_dev->lower_id); } -#endif +#endif /* if defined(PIOS_INCLUDE_COM) */ /** * @} diff --git a/flight/pios/posix/pios_crc.c b/flight/pios/posix/pios_crc.c index 548ba2648..0e4cd5dec 100644 --- a/flight/pios/posix/pios_crc.c +++ b/flight/pios/posix/pios_crc.c @@ -11,22 +11,22 @@ // CRC lookup table static const uint8_t crc_table[256] = { - 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, - 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, - 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, - 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, - 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, - 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, - 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, - 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, - 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, - 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, - 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, - 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, - 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, - 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, - 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, - 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 + 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, + 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, + 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, + 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, + 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, + 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, + 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, + 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, + 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, + 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, + 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, + 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, + 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, + 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, + 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, + 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 }; /** @@ -49,7 +49,7 @@ static const uint8_t crc_table[256] = { */ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) { - return crc_table[crc ^ data]; + return crc_table[crc ^ data]; } /* @@ -59,16 +59,16 @@ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) * @param[in] length Number of bytes to process * @returns Updated CRC */ -uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length) +uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t *data, int32_t length) { - // use registers for speed - register int32_t len = length; - register uint8_t crc8 = crc; - register const uint8_t *p = data; - - while (len--) - crc8 = crc_table[crc8 ^ *p++]; - - return crc8; -} + // use registers for speed + register int32_t len = length; + register uint8_t crc8 = crc; + register const uint8_t *p = data; + while (len--) { + crc8 = crc_table[crc8 ^ *p++]; + } + + return crc8; +} diff --git a/flight/pios/posix/pios_debug.c b/flight/pios/posix/pios_debug.c index 02d41ee64..ac67e89a0 100644 --- a/flight/pios/posix/pios_debug.c +++ b/flight/pios/posix/pios_debug.c @@ -10,21 +10,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Debugging Functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,36 +37,31 @@ const char *PIOS_DEBUG_AssertMsg = "ASSERT FAILED"; /* Private Function Prototypes */ /** -* Initialise Debug-features -*/ + * Initialise Debug-features + */ void PIOS_DEBUG_Init(void) -{ -} +{} /** -* Set debug-pin high -* \param pin 0 for S1 output -*/ + * Set debug-pin high + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinHigh(uint8_t Pin) -{ -} +{} /** -* Set debug-pin low -* \param pin 0 for S1 output -*/ + * Set debug-pin low + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinLow(uint8_t Pin) -{ -} +{} void PIOS_DEBUG_PinValue8Bit(uint8_t value) -{ -} +{} void PIOS_DEBUG_PinValue4BitL(uint8_t value) -{ -} +{} /** @@ -75,21 +70,20 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value) void PIOS_DEBUG_Panic(const char *msg) { #ifdef PIOS_COM_AUX - PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s\r", msg); + PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s\r", msg); #endif - // tell the user whats going on on commandline too - fprintf(stderr,"CRITICAL ERROR: %s\n",msg); - - // this helps debugging: causing a div by zero allows a backtrace - // and/or ends execution - int b = 0; - int a = (2/b); - b=a; + // tell the user whats going on on commandline too + fprintf(stderr, "CRITICAL ERROR: %s\n", msg); + // this helps debugging: causing a div by zero allows a backtrace + // and/or ends execution + int b = 0; + int a = (2 / b); + b = a; } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/posix/pios_delay.c b/flight/pios/posix/pios_delay.c index f871e11fa..d812613f1 100644 --- a/flight/pios/posix/pios_delay.c +++ b/flight/pios/posix/pios_delay.c @@ -3,27 +3,27 @@ * * @file pios_delay.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) - * @brief Delay Functions - * - Provides a micro-second granular delay using a TIM + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * @brief Delay Functions + * - Provides a micro-second granular delay using a TIM * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_DELAY Delay Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,80 +34,83 @@ #if defined(PIOS_INCLUDE_DELAY) /** -* Initialises the Timer used by PIOS_DELAY functions
-* This is called from pios.c as part of the main() function -* at system start up. -* \return < 0 if initialisation failed -*/ + * Initialises the Timer used by PIOS_DELAY functions
+ * This is called from pios.c as part of the main() function + * at system start up. + * \return < 0 if initialisation failed + */ #include int32_t PIOS_DELAY_Init(void) { - // stub + // stub - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Waits for a specific number of uS
-* Example:
-* \code -* // Wait for 500 uS -* PIOS_DELAY_Wait_uS(500); -* \endcode -* \param[in] uS delay (1..65535 microseconds) -* \return < 0 on errors -*/ + * Waits for a specific number of uS
+ * Example:
+ * \code + * // Wait for 500 uS + * PIOS_DELAY_Wait_uS(500); + * \endcode + * \param[in] uS delay (1..65535 microseconds) + * \return < 0 on errors + */ int32_t PIOS_DELAY_WaituS(uint32_t uS) { - static struct timespec wait,rest; - wait.tv_sec=0; - wait.tv_nsec=1000*uS; - while (nanosleep(&wait,&rest)!=0) { - wait=rest; - } + static struct timespec wait, rest; - /* No error */ - return 0; + wait.tv_sec = 0; + wait.tv_nsec = 1000 * uS; + while (nanosleep(&wait, &rest) != 0) { + wait = rest; + } + + /* No error */ + return 0; } /** -* Waits for a specific number of mS
-* Example:
-* \code -* // Wait for 500 mS -* PIOS_DELAY_Wait_mS(500); -* \endcode -* \param[in] mS delay (1..65535 milliseconds) -* \return < 0 on errors -*/ + * Waits for a specific number of mS
+ * Example:
+ * \code + * // Wait for 500 mS + * PIOS_DELAY_Wait_mS(500); + * \endcode + * \param[in] mS delay (1..65535 milliseconds) + * \return < 0 on errors + */ int32_t PIOS_DELAY_WaitmS(uint32_t mS) { - //for(int i = 0; i < mS; i++) { - // PIOS_DELAY_WaituS(1000); - static struct timespec wait,rest; - wait.tv_sec=mS/1000; - wait.tv_nsec=(mS%1000)*1000000; - while (nanosleep(&wait,&rest)!=0) { - wait=rest; - } - //} + // for(int i = 0; i < mS; i++) { + // PIOS_DELAY_WaituS(1000); + static struct timespec wait, rest; - /* No error */ - return 0; + wait.tv_sec = mS / 1000; + wait.tv_nsec = (mS % 1000) * 1000000; + while (nanosleep(&wait, &rest) != 0) { + wait = rest; + } + // } + + /* No error */ + return 0; } /** - * @brief Query the Delay timer for the current uS + * @brief Query the Delay timer for the current uS * @return A microsecond value */ uint32_t PIOS_DELAY_GetuS() { - static struct timespec current; - clock_gettime(CLOCK_REALTIME, ¤t); - return ((current.tv_sec * 1000000) + (current.tv_nsec / 1000)); + static struct timespec current; + + clock_gettime(CLOCK_REALTIME, ¤t); + return (current.tv_sec * 1000000) + (current.tv_nsec / 1000); } /** @@ -117,7 +120,7 @@ uint32_t PIOS_DELAY_GetuS() */ uint32_t PIOS_DELAY_GetuSSince(uint32_t t) { - return (PIOS_DELAY_GetuS() - t); + return PIOS_DELAY_GetuS() - t; } /** @@ -126,17 +129,17 @@ uint32_t PIOS_DELAY_GetuSSince(uint32_t t) */ uint32_t PIOS_DELAY_GetRaw() { - return (PIOS_DELAY_GetuS()); + return PIOS_DELAY_GetuS(); } /** - * @brief Compare to raw times to and convert to us + * @brief Compare to raw times to and convert to us * @return A microsecond value */ uint32_t PIOS_DELAY_DiffuS(uint32_t raw) { - return ( PIOS_DELAY_GetuS() - raw ); + return PIOS_DELAY_GetuS() - raw; } -#endif +#endif /* if defined(PIOS_INCLUDE_DELAY) */ diff --git a/flight/pios/posix/pios_iap.c b/flight/pios/posix/pios_iap.c index aef9f6c54..e38bf1d6d 100644 --- a/flight/pios/posix/pios_iap.c +++ b/flight/pios/posix/pios_iap.c @@ -1,6 +1,6 @@ /*! - * @File iap.c - * @Brief + * @File iap.c + * @Brief * * Created on: Sep 6, 2010 * Author: joe @@ -8,8 +8,8 @@ /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ #include /*! @@ -20,50 +20,42 @@ * * Created: Sep 8, 2010 10:10:48 PM by joe */ -void PIOS_IAP_Init( void ) -{ - -} +void PIOS_IAP_Init(void) +{} /*! * \brief Determines if an In-Application-Programming request has been made. * \param *comm - Which communication stream to use for the IAP (USB, Telemetry, I2C, SPI, etc) * \return TRUE - if correct sequence found, along with 'comm' updated. - * FALSE - Note that 'comm' will have an invalid comm identifier. + * FALSE - Note that 'comm' will have an invalid comm identifier. * \retval * */ -uint32_t PIOS_IAP_CheckRequest( void ) +uint32_t PIOS_IAP_CheckRequest(void) { - - return false; + return false; } - /*! * \brief Sets the 1st word of the request sequence. * \param n/a * \return n/a * \retval */ -void PIOS_IAP_SetRequest1(void) -{ -} +void PIOS_IAP_SetRequest1(void) +{} -void PIOS_IAP_SetRequest2(void) -{ -} +void PIOS_IAP_SetRequest2(void) +{} -void PIOS_IAP_ClearRequest(void) -{ -} +void PIOS_IAP_ClearRequest(void) +{} uint16_t PIOS_IAP_ReadBootCount(void) { - return 0; + return 0; } -void PIOS_IAP_WriteBootCount (uint16_t boot_count) -{ -} +void PIOS_IAP_WriteBootCount(uint16_t boot_count) +{} diff --git a/flight/pios/posix/pios_irq.c b/flight/pios/posix/pios_irq.c index b30d09fee..793df9ccf 100644 --- a/flight/pios/posix/pios_irq.c +++ b/flight/pios/posix/pios_irq.c @@ -6,26 +6,26 @@ * @brief STM32 Hardware code to enable and disable interrupts * @{ * - * @file pios_irq.c + * @file pios_irq.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief IRQ Enable/Disable routines * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,33 +37,33 @@ /* Private Function Prototypes */ /** -* Disables all interrupts (nested) -* \return < 0 On errors -*/ + * Disables all interrupts (nested) + * \return < 0 On errors + */ int32_t PIOS_IRQ_Disable(void) { #if defined(PIOS_INCLUDE_FREERTOS) - taskENTER_CRITICAL(); + taskENTER_CRITICAL(); #endif - return 0; + return 0; } /** -* Enables all interrupts (nested) -* \return < 0 on errors -* \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) -*/ + * Enables all interrupts (nested) + * \return < 0 on errors + * \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) + */ int32_t PIOS_IRQ_Enable(void) { #if defined(PIOS_INCLUDE_FREERTOS) - taskEXIT_CRITICAL(); + taskEXIT_CRITICAL(); #endif - return 0; + return 0; } -#endif +#endif /* if defined(PIOS_INCLUDE_IRQ) */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/posix/pios_led.c b/flight/pios/posix/pios_led.c index db833e1c3..c3bd4e27d 100644 --- a/flight/pios/posix/pios_led.c +++ b/flight/pios/posix/pios_led.c @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file pios_led.c + * @file pios_led.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief LED functions, init, toggle, on & off. * @see The GNU Public License (GPL) Version 3 @@ -9,19 +9,19 @@ * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,68 +35,69 @@ /* Local Variables */ -//static GPIO_TypeDef* LED_GPIO_PORT[PIOS_LED_NUM] = PIOS_LED_PORTS; -//static const uint32_t LED_GPIO_PIN[PIOS_LED_NUM] = PIOS_LED_PINS; -//static const uint32_t LED_GPIO_CLK[PIOS_LED_NUM] = PIOS_LED_CLKS; +// static GPIO_TypeDef* LED_GPIO_PORT[PIOS_LED_NUM] = PIOS_LED_PORTS; +// static const uint32_t LED_GPIO_PIN[PIOS_LED_NUM] = PIOS_LED_PINS; +// static const uint32_t LED_GPIO_CLK[PIOS_LED_NUM] = PIOS_LED_CLKS; static uint8_t LED_GPIO[PIOS_LED_NUM]; -static inline void PIOS_SetLED(uint32_t LED,uint8_t stat) { - printf("PIOS: LED %i status %i\n",LED,stat); - LED_GPIO[LED]=stat; +static inline void PIOS_SetLED(uint32_t LED, uint8_t stat) +{ + printf("PIOS: LED %i status %i\n", LED, stat); + LED_GPIO[LED] = stat; } /** -* Initialises all the LED's -*/ + * Initialises all the LED's + */ void PIOS_LED_Init(void) { - //GPIO_InitTypeDef GPIO_InitStructure; - //GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - - for(int LEDNum = 0; LEDNum < PIOS_LED_NUM; LEDNum++) { - //RCC_APB2PeriphClockCmd(LED_GPIO_CLK[LEDNum], ENABLE); - //GPIO_InitStructure.GPIO_Pin = LED_GPIO_PIN[LEDNum]; - //GPIO_Init(LED_GPIO_PORT[LEDNum], &GPIO_InitStructure); + // GPIO_InitTypeDef GPIO_InitStructure; + // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; + // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - /* LED's Off */ - //LED_GPIO_PORT[LEDNum]->BSRR = LED_GPIO_PIN[LEDNum]; - LED_GPIO[LEDNum]=0; - } + for (int LEDNum = 0; LEDNum < PIOS_LED_NUM; LEDNum++) { + // RCC_APB2PeriphClockCmd(LED_GPIO_CLK[LEDNum], ENABLE); + // GPIO_InitStructure.GPIO_Pin = LED_GPIO_PIN[LEDNum]; + // GPIO_Init(LED_GPIO_PORT[LEDNum], &GPIO_InitStructure); + + /* LED's Off */ + // LED_GPIO_PORT[LEDNum]->BSRR = LED_GPIO_PIN[LEDNum]; + LED_GPIO[LEDNum] = 0; + } } /** -* Turn on LED -* \param[in] LED LED Name (LED1, LED2) -*/ + * Turn on LED + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_On(uint32_t LED) { - //LED_GPIO_PORT[LED]->BRR = LED_GPIO_PIN[LED]; - PIOS_SetLED(LED,1); + // LED_GPIO_PORT[LED]->BRR = LED_GPIO_PIN[LED]; + PIOS_SetLED(LED, 1); } /** -* Turn off LED -* \param[in] LED LED Name (LED1, LED2) -*/ + * Turn off LED + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_Off(uint32_t LED) { - //LED_GPIO_PORT[LED]->BSRR = LED_GPIO_PIN[LED]; - PIOS_SetLED(LED,0); + // LED_GPIO_PORT[LED]->BSRR = LED_GPIO_PIN[LED]; + PIOS_SetLED(LED, 0); } /** -* Toggle LED on/off -* \param[in] LED LED Name (LED1, LED2) -*/ + * Toggle LED on/off + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_Toggle(uint32_t LED) { - //LED_GPIO_PORT[LED]->ODR ^= LED_GPIO_PIN[LED]; - PIOS_SetLED(LED,LED_GPIO[LED]?0:1); + // LED_GPIO_PORT[LED]->ODR ^= LED_GPIO_PIN[LED]; + PIOS_SetLED(LED, LED_GPIO[LED] ? 0 : 1); } -#endif +#endif /* if defined(PIOS_INCLUDE_LED) */ diff --git a/flight/pios/posix/pios_rcvr.c b/flight/pios/posix/pios_rcvr.c index 719cb9769..362ceb1a8 100644 --- a/flight/pios/posix/pios_rcvr.c +++ b/flight/pios/posix/pios_rcvr.c @@ -6,83 +6,89 @@ #include enum pios_rcvr_dev_magic { - PIOS_RCVR_DEV_MAGIC = 0x99aabbcc, + PIOS_RCVR_DEV_MAGIC = 0x99aabbcc, }; struct pios_rcvr_dev { - enum pios_rcvr_dev_magic magic; - uint32_t lower_id; - const struct pios_rcvr_driver * driver; + enum pios_rcvr_dev_magic magic; + uint32_t lower_id; + const struct pios_rcvr_driver *driver; }; -static bool PIOS_RCVR_validate(struct pios_rcvr_dev * rcvr_dev) +static bool PIOS_RCVR_validate(struct pios_rcvr_dev *rcvr_dev) { - return (rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC); + return rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) && 0 -//static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) -//{ -// struct pios_rcvr_dev * rcvr_dev; +// static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) +// { +// struct pios_rcvr_dev * rcvr_dev; // -// rcvr_dev = (struct pios_rcvr_dev *)pvPortMalloc(sizeof(*rcvr_dev)); -// if (!rcvr_dev) return (NULL); +// rcvr_dev = (struct pios_rcvr_dev *)pvPortMalloc(sizeof(*rcvr_dev)); +// if (!rcvr_dev) return (NULL); // -// rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; -// return(rcvr_dev); -//} +// rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; +// return(rcvr_dev); +// } #else static struct pios_rcvr_dev pios_rcvr_devs[PIOS_RCVR_MAX_DEVS]; static uint8_t pios_rcvr_num_devs; static uint32_t PIOS_RCVR_alloc(void) { - struct pios_rcvr_dev * rcvr_dev; + struct pios_rcvr_dev *rcvr_dev; - if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) { - return (PIOS_RCVR_MAX_DEVS+1); - } + if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) { + return PIOS_RCVR_MAX_DEVS + 1; + } - rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++]; - rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; + rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++]; + rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; - return (pios_rcvr_num_devs); + return pios_rcvr_num_devs; } -static struct pios_rcvr_dev * PIOS_RCVR_find_dev(uint32_t rcvr_dev_id) +static struct pios_rcvr_dev *PIOS_RCVR_find_dev(uint32_t rcvr_dev_id) { - if (!rcvr_dev_id) return NULL; - if (rcvr_dev_id>pios_rcvr_num_devs+1) return NULL; - return &pios_rcvr_devs[rcvr_dev_id-1]; + if (!rcvr_dev_id) { + return NULL; + } + if (rcvr_dev_id > pios_rcvr_num_devs + 1) { + return NULL; + } + return &pios_rcvr_devs[rcvr_dev_id - 1]; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) && 0 */ /** - * Initialises RCVR layer - * \param[out] handle - * \param[in] driver - * \param[in] id - * \return < 0 if initialisation failed - */ -int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, uint32_t lower_id) + * Initialises RCVR layer + * \param[out] handle + * \param[in] driver + * \param[in] id + * \return < 0 if initialisation failed + */ +int32_t PIOS_RCVR_Init(uint32_t *rcvr_id, const struct pios_rcvr_driver *driver, uint32_t lower_id) { - PIOS_DEBUG_Assert(rcvr_id); - PIOS_DEBUG_Assert(driver); + PIOS_DEBUG_Assert(rcvr_id); + PIOS_DEBUG_Assert(driver); - uint32_t rcvr_dev_id; - struct pios_rcvr_dev * rcvr_dev; + uint32_t rcvr_dev_id; + struct pios_rcvr_dev *rcvr_dev; - rcvr_dev_id = PIOS_RCVR_alloc(); - rcvr_dev = PIOS_RCVR_find_dev(rcvr_dev_id); - if (!rcvr_dev) goto out_fail; + rcvr_dev_id = PIOS_RCVR_alloc(); + rcvr_dev = PIOS_RCVR_find_dev(rcvr_dev_id); + if (!rcvr_dev) { + goto out_fail; + } - rcvr_dev->driver = driver; - rcvr_dev->lower_id = lower_id; + rcvr_dev->driver = driver; + rcvr_dev->lower_id = lower_id; - *rcvr_id = rcvr_dev_id; - return(0); + *rcvr_id = rcvr_dev_id; + return 0; out_fail: - return(-1); + return -1; } /** @@ -96,28 +102,30 @@ out_fail: */ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) { - // Publicly facing API uses channel 1 for first channel - if(channel == 0) - return PIOS_RCVR_INVALID; - else - channel--; + // Publicly facing API uses channel 1 for first channel + if (channel == 0) { + return PIOS_RCVR_INVALID; + } else { + channel--; + } - if (rcvr_id == 0) - return PIOS_RCVR_NODRIVER; + if (rcvr_id == 0) { + return PIOS_RCVR_NODRIVER; + } - struct pios_rcvr_dev * rcvr_dev = PIOS_RCVR_find_dev(rcvr_id); + struct pios_rcvr_dev *rcvr_dev = PIOS_RCVR_find_dev(rcvr_id); - if (!PIOS_RCVR_validate(rcvr_dev)) { - /* Undefined RCVR port for this board (see pios_board.c) */ - PIOS_Assert(0); - } + if (!PIOS_RCVR_validate(rcvr_dev)) { + /* Undefined RCVR port for this board (see pios_board.c) */ + PIOS_Assert(0); + } - PIOS_DEBUG_Assert(rcvr_dev->driver->read); + PIOS_DEBUG_Assert(rcvr_dev->driver->read); - return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); + return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); } -#endif +#endif /* if defined(PIOS_INCLUDE_RCVR) */ /** * @} diff --git a/flight/pios/posix/pios_sdcard.c b/flight/pios/posix/pios_sdcard.c index 9aff090f0..ff7f330cd 100644 --- a/flight/pios/posix/pios_sdcard.c +++ b/flight/pios/posix/pios_sdcard.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_sdcard.c + * @file pios_sdcard.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief Sets up basic system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_SDCARD SDCard Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,186 +34,186 @@ /** -* Initialises SPI pins and peripheral to access MMC/SD Card -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ + * Initialises SPI pins and peripheral to access MMC/SD Card + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ int32_t PIOS_SDCARD_Init(uint32_t spi_id) { - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Connects to SD Card -* \return < 0 if initialisation sequence failed -*/ + * Connects to SD Card + * \return < 0 if initialisation sequence failed + */ int32_t PIOS_SDCARD_PowerOn(void) { - return 0; /* Status should be 0 if nothing went wrong! */ + return 0; /* Status should be 0 if nothing went wrong! */ } /** -* Disconnects from SD Card -* \return < 0 on errors -*/ + * Disconnects from SD Card + * \return < 0 on errors + */ int32_t PIOS_SDCARD_PowerOff(void) { - return 0; // no error + return 0; // no error } /** -* If SD card was previously available: Checks if the SD Card is still -* available by sending the STATUS command.
-* This takes ca. 10 uS -* -* If SD card was previously not available: Checks if the SD Card is -* available by sending the IDLE command at low speed.
-* This takes ca. 500 uS!
-* Once we got a positive response, SDCARD_PowerOn() will be -* called by this function to initialize the card completely. -* -* Example for Connection/Disconnection detection: -* \code -* // this function is called each second from a low-priority task -* // If multiple tasks are accessing the SD card, add a semaphore/mutex -* // to avoid IO access collisions with other tasks! -* u8 sdcard_available; -* int32_t CheckSDCard(void) -* { -* // check if SD card is available -* // High speed access if the card was previously available -* u8 prev_sdcard_available = sdcard_available; -* sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); -* -* if(sdcard_available && !prev_sdcard_available) { -* // SD Card has been connected -* -* // now it's possible to read/write sectors -* -* } else if( !sdcard_available && prev_sdcard_available ) { -* // SD Card has been disconnected -* -* // here you can notify your application about this state -* } -* -* return 0; // no error -* } -* \endcode -* \param[in] was_available should only be set if the SD card was previously available -* \return 0 if no response from SD Card -* \return 1 if SD card is accessible -*/ + * If SD card was previously available: Checks if the SD Card is still + * available by sending the STATUS command.
+ * This takes ca. 10 uS + * + * If SD card was previously not available: Checks if the SD Card is + * available by sending the IDLE command at low speed.
+ * This takes ca. 500 uS!
+ * Once we got a positive response, SDCARD_PowerOn() will be + * called by this function to initialize the card completely. + * + * Example for Connection/Disconnection detection: + * \code + * // this function is called each second from a low-priority task + * // If multiple tasks are accessing the SD card, add a semaphore/mutex + * // to avoid IO access collisions with other tasks! + * u8 sdcard_available; + * int32_t CheckSDCard(void) + * { + * // check if SD card is available + * // High speed access if the card was previously available + * u8 prev_sdcard_available = sdcard_available; + * sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); + * + * if(sdcard_available && !prev_sdcard_available) { + * // SD Card has been connected + * + * // now it's possible to read/write sectors + * + * } else if( !sdcard_available && prev_sdcard_available ) { + * // SD Card has been disconnected + * + * // here you can notify your application about this state + * } + * + * return 0; // no error + * } + * \endcode + * \param[in] was_available should only be set if the SD card was previously available + * \return 0 if no response from SD Card + * \return 1 if SD card is accessible + */ int32_t PIOS_SDCARD_CheckAvailable(uint8_t was_available) { - return 1; /* 1 = available, 0 = not available. */ + return 1; /* 1 = available, 0 = not available. */ } /** -* Sends command to SD card -* \param[in] cmd SD card command -* \param[in] addr 32bit address -* \param[in] crc precalculated CRC -* \return >= 0x00 if command has been sent successfully (contains received byte) -* \return -1 if no response from SD Card (timeout) -*/ + * Sends command to SD card + * \param[in] cmd SD card command + * \param[in] addr 32bit address + * \param[in] crc precalculated CRC + * \return >= 0x00 if command has been sent successfully (contains received byte) + * \return -1 if no response from SD Card (timeout) + */ int32_t PIOS_SDCARD_SendSDCCmd(uint8_t cmd, uint32_t addr, uint8_t crc) { - return -1; + return -1; } /** -* Reads 512 bytes from selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads 512 bytes from selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_SectorRead(uint32_t sector, uint8_t *buffer) { - return -256; + return -256; } /** -* Writes 512 bytes into selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if write operation not accepted -* \return -258 if timeout during write operation -*/ + * Writes 512 bytes into selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if write operation not accepted + * \return -258 if timeout during write operation + */ int32_t PIOS_SDCARD_SectorWrite(uint32_t sector, uint8_t *buffer) { - return -256; + return -256; } /** -* Reads the CID informations from SD Card -* \param[in] *cid pointer to buffer which holds the CID informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads the CID informations from SD Card + * \param[in] *cid pointer to buffer which holds the CID informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_CIDRead(SDCARDCidTypeDef *cid) { - return -256; + return -256; } /** -* Reads the CSD informations from SD Card -* \param[in] *csd pointer to buffer which holds the CSD informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads the CSD informations from SD Card + * \param[in] *csd pointer to buffer which holds the CSD informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_CSDRead(SDCARDCsdTypeDef *csd) { - return -256; + return -256; } /** -* Attempts to write a startup log to the SDCard -* return 0 No errors -* return -1 Error deleting file -* return -2 Error opening file -* return -3 Error writing file -*/ + * Attempts to write a startup log to the SDCard + * return 0 No errors + * return -1 Error deleting file + * return -2 Error opening file + * return -3 Error writing file + */ int32_t PIOS_SDCARD_StartupLog(void) { - return -3; + return -3; } /** @@ -223,78 +223,78 @@ int32_t PIOS_SDCARD_StartupLog(void) */ int32_t PIOS_SDCARD_IsMounted() { - return 1; + return 1; } /** -* Mounts the file system -* param[in] CreateStartupLog 1 = True, 0 = False -* return 0 No errors -* return -1 SDCard not available -* return -2 Cannot find first partition -* return -3 No volume information -* return -4 Error writing startup log file -*/ + * Mounts the file system + * param[in] CreateStartupLog 1 = True, 0 = False + * return 0 No errors + * return -1 SDCard not available + * return -2 Cannot find first partition + * return -3 No volume information + * return -4 Error writing startup log file + */ int32_t PIOS_SDCARD_MountFS(uint32_t CreateStartupLog) { - return 0; + return 0; } /** -* Mounts the file system -* return Amount of free bytes -*/ + * Mounts the file system + * return Amount of free bytes + */ int32_t PIOS_SDCARD_GetFree(void) { - return 10240; + return 10240; } /** -* Read from file -* return 0 No error -* return -1 DFS_ReadFile failed -* return -2 Less bytes read than expected -*/ -//int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len) -//{ - /* No error */ -// return -1; -//} + * Read from file + * return 0 No error + * return -1 DFS_ReadFile failed + * return -2 Less bytes read than expected + */ +// int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len) +// { +/* No error */ +// return -1; +// } /** -* Read a line from file -* returns Number of bytes read -*/ -//int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len) -//{ -// return -1; -//} + * Read a line from file + * returns Number of bytes read + */ +// int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len) +// { +// return -1; +// } /** -* Copy a file -* WARNING: This will overwrite the destination file even if it exists -* param[in] *Source Path to file to copy -* param[in] *Destination Path to destination file -* return 0 No errors -* return -1 Source file doesn't exist -* return -2 Failed to create destination file -* return -3 DFS_ReadFile failed -* return -4 DFS_WriteFile failed -*/ + * Copy a file + * WARNING: This will overwrite the destination file even if it exists + * param[in] *Source Path to file to copy + * param[in] *Destination Path to destination file + * return 0 No errors + * return -1 Source file doesn't exist + * return -2 Failed to create destination file + * return -3 DFS_ReadFile failed + * return -4 DFS_WriteFile failed + */ int32_t PIOS_SDCARD_FileCopy(char *Source, char *Destination) { - return -2; + return -2; } /** -* Delete a file -* param[in] *Filename File to delete -* return 0 No errors -* return -1 Error deleting file -*/ + * Delete a file + * param[in] *Filename File to delete + * return 0 No errors + * return -1 Error deleting file + */ int32_t PIOS_SDCARD_FileDelete(char *Filename) { - return -1; + return -1; } -#endif +#endif /* if defined(PIOS_INCLUDE_SDCARD) */ diff --git a/flight/pios/posix/pios_servo.c b/flight/pios/posix/pios_servo.c index d0cf530d3..c3b4ba05b 100644 --- a/flight/pios/posix/pios_servo.c +++ b/flight/pios/posix/pios_servo.c @@ -6,25 +6,25 @@ * @brief Code to do set RC servo output * @{ * - * @file pios_servo.c + * @file pios_servo.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RC Servo routines (STM32 dependent) * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,36 +39,33 @@ static volatile uint16_t ServoPosition[PIOS_SERVO_NUM_TIMERS]; /** -* Initialise Servos -*/ + * Initialise Servos + */ void PIOS_Servo_Init(void) -{ -} +{} /** -* Set the servo update rate (Max 500Hz) -* \param[in] onetofour Rate for outputs 1 to 4 (Hz) -* \param[in] fivetoeight Rate for outputs 5 to 8 (Hz) -*/ -void PIOS_Servo_SetHz(const uint16_t * banks, uint8_t num_banks) -{ -} + * Set the servo update rate (Max 500Hz) + * \param[in] onetofour Rate for outputs 1 to 4 (Hz) + * \param[in] fivetoeight Rate for outputs 5 to 8 (Hz) + */ +void PIOS_Servo_SetHz(const uint16_t *banks, uint8_t num_banks) +{} /** -* Set servo position -* \param[in] Servo Servo number (0-7) -* \param[in] Position Servo position in milliseconds -*/ + * Set servo position + * \param[in] Servo Servo number (0-7) + * \param[in] Position Servo position in milliseconds + */ void PIOS_Servo_Set(uint8_t Servo, uint16_t Position) { #ifndef PIOS_ENABLE_DEBUG_PINS - /* Make sure servo exists */ - if (Servo < PIOS_SERVO_NUM_OUTPUTS) { - /* Update the position */ - ServoPosition[Servo] = Position; - - } + /* Make sure servo exists */ + if (Servo < PIOS_SERVO_NUM_OUTPUTS) { + /* Update the position */ + ServoPosition[Servo] = Position; + } #endif // PIOS_ENABLE_DEBUG_PINS } -#endif +#endif /* if defined(PIOS_INCLUDE_SERVO) */ diff --git a/flight/pios/posix/pios_sys.c b/flight/pios/posix/pios_sys.c index d4645139b..7e9d5df7b 100644 --- a/flight/pios/posix/pios_sys.c +++ b/flight/pios/posix/pios_sys.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_sys.c + * @file pios_sys.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Sets up basic system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_SYS System Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,159 +38,161 @@ void NVIC_Configuration(void); void SysTick_Handler(void); /* Local Macros */ -#define MEM8(addr) (*((volatile uint8_t *)(addr))) +#define MEM8(addr) (*((volatile uint8_t *)(addr))) /** -* Initialises all system peripherals -*/ + * Initialises all system peripherals + */ void PIOS_SYS_Init(void) { + /** + * stub + */ + printf("PIOS_SYS_Init\n"); - /** - * stub - */ - printf("PIOS_SYS_Init\n"); - - /* Initialise Basic NVIC */ - NVIC_Configuration(); + /* Initialise Basic NVIC */ + NVIC_Configuration(); #if defined(PIOS_INCLUDE_LED) - /* Initialise LEDs */ - PIOS_LED_Init(); + /* Initialise LEDs */ + PIOS_LED_Init(); #endif } /** -* Shutdown PIOS and reset the microcontroller:
-*
    -*
  • Disable all RTOS tasks -*
  • Disable all interrupts -*
  • Turn off all board LEDs -*
  • Reset STM32 -*
-* \return < 0 if reset failed -*/ + * Shutdown PIOS and reset the microcontroller:
+ *
    + *
  • Disable all RTOS tasks + *
  • Disable all interrupts + *
  • Turn off all board LEDs + *
  • Reset STM32 + *
+ * \return < 0 if reset failed + */ int32_t PIOS_SYS_Reset(void) { - /** - * stub - */ - printf("PIOS_SYS_Reset\n"); - /* Disable all RTOS tasks */ + /** + * stub + */ + printf("PIOS_SYS_Reset\n"); + /* Disable all RTOS tasks */ #if defined(PIOS_INCLUDE_FREERTOS) - /* port specific FreeRTOS function to disable tasks (nested) */ - portENTER_CRITICAL(); + /* port specific FreeRTOS function to disable tasks (nested) */ + portENTER_CRITICAL(); #endif - // disable all interrupts - //PIOS_IRQ_Disable(); + // disable all interrupts + // PIOS_IRQ_Disable(); - // turn off all board LEDs - #if (PIOS_LED_NUM == 1) - PIOS_LED_Off(LED1); - #elif (PIOS_LED_NUM == 2) - PIOS_LED_Off(LED1); - PIOS_LED_Off(LED2); - #endif + // turn off all board LEDs + #if (PIOS_LED_NUM == 1) + PIOS_LED_Off(LED1); + #elif (PIOS_LED_NUM == 2) + PIOS_LED_Off(LED1); + PIOS_LED_Off(LED2); + #endif + /* Reset STM32 */ + // RCC_APB2PeriphResetCmd(0xfffffff8, ENABLE); /* MBHP_CORE_STM32: don't reset GPIOA/AF due to USB pins */ + // RCC_APB1PeriphResetCmd(0xff7fffff, ENABLE); /* don't reset USB, so that the connection can survive! */ - /* Reset STM32 */ - //RCC_APB2PeriphResetCmd(0xfffffff8, ENABLE); /* MBHP_CORE_STM32: don't reset GPIOA/AF due to USB pins */ - //RCC_APB1PeriphResetCmd(0xff7fffff, ENABLE); /* don't reset USB, so that the connection can survive! */ + // RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + // RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + // SCB->AIRCR = NVIC_AIRCR_VECTKEY | (1 << NVIC_VECTRESET); + exit(1); - //RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - //RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - //SCB->AIRCR = NVIC_AIRCR_VECTKEY | (1 << NVIC_VECTRESET); - exit(1); + while (1) { + ; + } - while(1); - - /* We will never reach this point */ - return -1; + /* We will never reach this point */ + return -1; } /** -* Returns the serial number as a string -* param[out] uint8_t pointer to a string which can store at least 12 bytes -* (12 bytes returned for STM32) -* return < 0 if feature not supported -*/ + * Returns the serial number as a string + * param[out] uint8_t pointer to a string which can store at least 12 bytes + * (12 bytes returned for STM32) + * return < 0 if feature not supported + */ int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t *array) { - /* Stored in the so called "electronic signature" */ - for (int i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) { - array[i] = 0xff; - } + /* Stored in the so called "electronic signature" */ + for (int i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) { + array[i] = 0xff; + } - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Returns the serial number as a string -* param[out] str pointer to a string which can store at least 32 digits + zero terminator! -* (24 digits returned for STM32) -* return < 0 if feature not supported -*/ + * Returns the serial number as a string + * param[out] str pointer to a string which can store at least 32 digits + zero terminator! + * (24 digits returned for STM32) + * return < 0 if feature not supported + */ int32_t PIOS_SYS_SerialNumberGet(char *str) { - /* Stored in the so called "electronic signature" */ - int i; - for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { - str[i] = 'F'; - } - str[i] = '\0'; + /* Stored in the so called "electronic signature" */ + int i; - /* No error */ - return 0; + for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { + str[i] = 'F'; + } + str[i] = '\0'; + + /* No error */ + return 0; } /** -* Configures Vector Table base location and SysTick -*/ + * Configures Vector Table base location and SysTick + */ void NVIC_Configuration(void) { - /** - * stub - */ - printf("NVIC_Configuration\n"); - /* Set the Vector Table base address as specified in .ld file */ - //NVIC_SetVectorTable(PIOS_NVIC_VECTTAB_FLASH, 0x0); + /** + * stub + */ + printf("NVIC_Configuration\n"); + /* Set the Vector Table base address as specified in .ld file */ + // NVIC_SetVectorTable(PIOS_NVIC_VECTTAB_FLASH, 0x0); - /* 4 bits for Interrupt priorities so no sub priorities */ - //NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + /* 4 bits for Interrupt priorities so no sub priorities */ + // NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); - /* Configure HCLK clock as SysTick clock source. */ - //SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK); + /* Configure HCLK clock as SysTick clock source. */ + // SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK); } #ifdef USE_FULL_ASSERT /** -* Reports the name of the source file and the source line number -* where the assert_param error has occurred. -* \param[in] file pointer to the source file name -* \param[in] line assert_param error line source number -* \retval None -*/ -void assert_failed(uint8_t* file, uint32_t line) + * Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * \param[in] file pointer to the source file name + * \param[in] line assert_param error line source number + * \retval None + */ +void assert_failed(uint8_t *file, uint32_t line) { - /* When serial debugging is implemented, use something like this. */ - /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ - printf("Wrong parameters value: file %s on line %d\r\n", file, line); + /* When serial debugging is implemented, use something like this. */ + /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ + printf("Wrong parameters value: file %s on line %d\r\n", file, line); - /* Setup the LEDs to Alternate */ - PIOS_LED_On(LED1); - PIOS_LED_Off(LED2); + /* Setup the LEDs to Alternate */ + PIOS_LED_On(LED1); + PIOS_LED_Off(LED2); - /* Infinite loop */ - while (1) - { - PIOS_LED_Toggle(LED1); - PIOS_LED_Toggle(LED2); - for(int i = 0; i < 1000000; i++); - } + /* Infinite loop */ + while (1) { + PIOS_LED_Toggle(LED1); + PIOS_LED_Toggle(LED2); + for (int i = 0; i < 1000000; i++) { + ; + } + } } -#endif +#endif /* ifdef USE_FULL_ASSERT */ -#endif +#endif /* if defined(PIOS_INCLUDE_SYS) */ diff --git a/flight/pios/posix/pios_udp.c b/flight/pios/posix/pios_udp.c index c3e495f23..8f25561ac 100644 --- a/flight/pios/posix/pios_udp.c +++ b/flight/pios/posix/pios_udp.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_udp.c + * @file pios_udp.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief UDP commands. Inits UDPs, controls UDPs & Interupt handlers. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_UDP UDP Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,7 +43,6 @@ static int8_t pios_udp_num_devices = 0; static pios_udp_dev pios_udp_devices[PIOS_UDP_MAX_DEV]; - /* Provide a COM driver */ static void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud); static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context); @@ -52,194 +51,182 @@ static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail); static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_udp_com_driver = { - .set_baud = PIOS_UDP_ChangeBaud, - .tx_start = PIOS_UDP_TxStart, - .rx_start = PIOS_UDP_RxStart, - .bind_tx_cb = PIOS_UDP_RegisterTxCallback, - .bind_rx_cb = PIOS_UDP_RegisterRxCallback, + .set_baud = PIOS_UDP_ChangeBaud, + .tx_start = PIOS_UDP_TxStart, + .rx_start = PIOS_UDP_RxStart, + .bind_tx_cb = PIOS_UDP_RegisterTxCallback, + .bind_rx_cb = PIOS_UDP_RegisterRxCallback, }; -static pios_udp_dev * find_udp_dev_by_id (uint8_t udp) +static pios_udp_dev *find_udp_dev_by_id(uint8_t udp) { - if (udp >= pios_udp_num_devices) { - /* Undefined UDP port for this board (see pios_board.c) */ - PIOS_Assert(0); - return NULL; - } + if (udp >= pios_udp_num_devices) { + /* Undefined UDP port for this board (see pios_board.c) */ + PIOS_Assert(0); + return NULL; + } - /* Get a handle for the device configuration */ - return &(pios_udp_devices[udp]); + /* Get a handle for the device configuration */ + return &(pios_udp_devices[udp]); } /** * RxThread */ -void * PIOS_UDP_RxThread(void * udp_dev_n) +void *PIOS_UDP_RxThread(void *udp_dev_n) { + pios_udp_dev *udp_dev = (pios_udp_dev *)udp_dev_n; - pios_udp_dev * udp_dev = (pios_udp_dev*) udp_dev_n; - - /** - * com devices never get closed except by application "reboot" - * we also never give up our mutex except for waiting - */ - while(1) { - - /** - * receive - */ - int received; - udp_dev->clientLength=sizeof(udp_dev->client); - if ((received = recvfrom(udp_dev->socket, - &udp_dev->rx_buffer, - PIOS_UDP_RX_BUFFER_SIZE, - 0, - (struct sockaddr *) &udp_dev->client, - (socklen_t*)&udp_dev->clientLength)) >= 0) - { - - /* copy received data to buffer if possible */ - /* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */ - /* (thats what the USART driver does too!) */ - bool rx_need_yield = false; - if (udp_dev->rx_in_cb) { - (void) (udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield); - } + /** + * com devices never get closed except by application "reboot" + * we also never give up our mutex except for waiting + */ + while (1) { + /** + * receive + */ + int received; + udp_dev->clientLength = sizeof(udp_dev->client); + if ((received = recvfrom(udp_dev->socket, + &udp_dev->rx_buffer, + PIOS_UDP_RX_BUFFER_SIZE, + 0, + (struct sockaddr *)&udp_dev->client, + (socklen_t *)&udp_dev->clientLength)) >= 0) { + /* copy received data to buffer if possible */ + /* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */ + /* (thats what the USART driver does too!) */ + bool rx_need_yield = false; + if (udp_dev->rx_in_cb) { + (void)(udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (rx_need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ - - } - - - } + if (rx_need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ + } + } } /** -* Open UDP socket -*/ -int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg) + * Open UDP socket + */ +int32_t PIOS_UDP_Init(uint32_t *udp_id, const struct pios_udp_cfg *cfg) { + pios_udp_dev *udp_dev = &pios_udp_devices[pios_udp_num_devices]; - pios_udp_dev * udp_dev = &pios_udp_devices[pios_udp_num_devices]; - - pios_udp_num_devices++; + pios_udp_num_devices++; - /* initialize */ - udp_dev->rx_in_cb = NULL; - udp_dev->tx_out_cb = NULL; - udp_dev->cfg=cfg; + /* initialize */ + udp_dev->rx_in_cb = NULL; + udp_dev->tx_out_cb = NULL; + udp_dev->cfg = cfg; - /* assign socket */ - udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - memset(&udp_dev->server,0,sizeof(udp_dev->server)); - memset(&udp_dev->client,0,sizeof(udp_dev->client)); - udp_dev->server.sin_family = AF_INET; - udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip); - udp_dev->server.sin_port = htons(udp_dev->cfg->port); - int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server)); + /* assign socket */ + udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + memset(&udp_dev->server, 0, sizeof(udp_dev->server)); + memset(&udp_dev->client, 0, sizeof(udp_dev->client)); + udp_dev->server.sin_family = AF_INET; + udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip); + udp_dev->server.sin_port = htons(udp_dev->cfg->port); + int res = bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server, sizeof(udp_dev->server)); - /* Create transmit thread for this connection */ + /* Create transmit thread for this connection */ #if defined(PIOS_INCLUDE_FREERTOS) -//( pdTASK_CODE pvTaskCode, const portCHAR * const pcName, unsigned portSHORT usStackDepth, void *pvParameters, unsigned portBASE_TYPE uxPriority, xTaskHandle *pvCreatedTask ); - xTaskCreate((pdTASK_CODE)PIOS_UDP_RxThread, (const signed char *)"UDP_Rx_Thread",1024,(void*)udp_dev,(tskIDLE_PRIORITY + 1),&udp_dev->rxThread); +// ( pdTASK_CODE pvTaskCode, const portCHAR * const pcName, unsigned portSHORT usStackDepth, void *pvParameters, unsigned portBASE_TYPE uxPriority, xTaskHandle *pvCreatedTask ); + xTaskCreate((pdTASK_CODE)PIOS_UDP_RxThread, (const signed char *)"UDP_Rx_Thread", 1024, (void *)udp_dev, (tskIDLE_PRIORITY + 1), &udp_dev->rxThread); #else - pthread_create(&udp_dev->rxThread, NULL, PIOS_UDP_RxThread, (void*)udp_dev); + pthread_create(&udp_dev->rxThread, NULL, PIOS_UDP_RxThread, (void *)udp_dev); #endif - printf("udp dev %i - socket %i opened - result %i\n",pios_udp_num_devices-1,udp_dev->socket,res); + printf("udp dev %i - socket %i opened - result %i\n", pios_udp_num_devices - 1, udp_dev->socket, res); - *udp_id = pios_udp_num_devices-1; + *udp_id = pios_udp_num_devices - 1; - return res; + return res; } void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud) { - /** - * doesn't apply! - */ + /** + * doesn't apply! + */ } static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail) { - /** - * lazy! - */ + /** + * lazy! + */ } static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail) { - pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id); - PIOS_Assert(udp_dev); + PIOS_Assert(udp_dev); - int32_t length,len,rem; - - /** - * we send everything directly whenever notified of data to send (lazy!) - */ - if (udp_dev->tx_out_cb) { - while (tx_bytes_avail>0) { - bool tx_need_yield = false; - length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield); - rem = length; - while (rem>0) { - len = sendto(udp_dev->socket, udp_dev->tx_buffer+length-rem, rem, 0, - (struct sockaddr *) &udp_dev->client, - sizeof(udp_dev->client)); - if (len<=0) { - rem=0; - } else { - rem -= len; - } - } - tx_bytes_avail -= length; - } - } + int32_t length, len, rem; + /** + * we send everything directly whenever notified of data to send (lazy!) + */ + if (udp_dev->tx_out_cb) { + while (tx_bytes_avail > 0) { + bool tx_need_yield = false; + length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield); + rem = length; + while (rem > 0) { + len = sendto(udp_dev->socket, udp_dev->tx_buffer + length - rem, rem, 0, + (struct sockaddr *)&udp_dev->client, + sizeof(udp_dev->client)); + if (len <= 0) { + rem = 0; + } else { + rem -= len; + } + } + tx_bytes_avail -= length; + } + } } static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context) { - pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id); - PIOS_Assert(udp_dev); + PIOS_Assert(udp_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - udp_dev->rx_in_context = context; - udp_dev->rx_in_cb = rx_in_cb; + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + udp_dev->rx_in_context = context; + udp_dev->rx_in_cb = rx_in_cb; } static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context) { - pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id); - PIOS_Assert(udp_dev); + PIOS_Assert(udp_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - udp_dev->tx_out_context = context; - udp_dev->tx_out_cb = tx_out_cb; + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + udp_dev->tx_out_context = context; + udp_dev->tx_out_cb = tx_out_cb; } - - - -#endif +#endif /* if defined(PIOS_INCLUDE_UDP) */ diff --git a/flight/pios/posix/pios_wdg.c b/flight/pios/posix/pios_wdg.c index eeb4d759a..11261c035 100644 --- a/flight/pios/posix/pios_wdg.c +++ b/flight/pios/posix/pios_wdg.c @@ -8,12 +8,12 @@ * * @file pios_spi.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Hardware Abstraction Layer for SPI ports of STM32 * @see The GNU Public License (GPL) Version 3 * @notes * - * The PIOS Watchdog provides a HAL to initialize a watchdog + * The PIOS Watchdog provides a HAL to initialize a watchdog * *****************************************************************************/ /* @@ -34,17 +34,17 @@ #include "pios.h" -/** +/** * @brief Initialize the watchdog timer for a specified timeout * - * It is important to note that this function returns the achieved timeout - * for this hardware. For hardware indendence this should be checked when - * scheduling updates. Other hardware dependent details may need to be - * considered such as a window time which sets a minimum update time, - * and this function should return a recommended delay for clearing. + * It is important to note that this function returns the achieved timeout + * for this hardware. For hardware indendence this should be checked when + * scheduling updates. Other hardware dependent details may need to be + * considered such as a window time which sets a minimum update time, + * and this function should return a recommended delay for clearing. * * For the STM32 nominal clock rate is 32 khz, but for the maximum clock rate of - * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is + * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is * set in the watchdog assumes the nominal clock rate, but the delay for FreeRTOS * to use is 75% of the minimal delay. * @@ -53,65 +53,65 @@ */ uint16_t PIOS_WDG_Init() { - return 0; + return 0; } /** - * @brief Register a module against the watchdog - * + * @brief Register a module against the watchdog + * * There are two ways to use PIOS WDG: this is for when - * multiple modules must be monitored. In this case they - * must first register against the watchdog system and + * multiple modules must be monitored. In this case they + * must first register against the watchdog system and * only when all of the modules have been updated with the - * watchdog be cleared. Each module must have its own - * bit in the 16 bit + * watchdog be cleared. Each module must have its own + * bit in the 16 bit * * @param[in] flag the bit this module wants to use * @returns True if that bit is unregistered */ -bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) +bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) { - return true; + return true; } /** * @brief Function called by modules to indicate they are still running * - * This function will set this flag in the active flags register (which is + * This function will set this flag in the active flags register (which is * a backup regsiter) and if all the registered flags are set will clear * the watchdog and set only this flag in the backup register * * @param[in] flag the flag to set * @return true if the watchdog cleared, false if flags are pending */ -bool PIOS_WDG_UpdateFlag(uint16_t flag) -{ - return true; +bool PIOS_WDG_UpdateFlag(uint16_t flag) +{ + return true; } -/** +/** * @brief Returns the flags that were set at bootup - * - * This is used for diagnostics, if only one flag not set this + * + * This is used for diagnostics, if only one flag not set this * was likely the module that wasn't running before reset - * + * * @return The active flags register from bootup */ uint16_t PIOS_WDG_GetBootupFlags() { - return (uint16_t) 0xffff; + return (uint16_t)0xffff; } -/** +/** * @brief Returns the currently active flags - * + * * For external monitoring - * + * * @return The active flags register */ uint16_t PIOS_WDG_GetActiveFlags() { - return (uint16_t) 0xffff; + return (uint16_t)0xffff; } /** @@ -120,5 +120,4 @@ uint16_t PIOS_WDG_GetActiveFlags() * This function must be called at the appropriate delay to prevent a reset event occuring */ void PIOS_WDG_Clear(void) -{ -} +{} diff --git a/flight/pios/stm32f10x/pios_adc.c b/flight/pios/stm32f10x/pios_adc.c index 2f5ed4198..c9ef301e3 100644 --- a/flight/pios/stm32f10x/pios_adc.c +++ b/flight/pios/stm32f10x/pios_adc.c @@ -6,24 +6,24 @@ * @brief STM32 ADC PIOS interface * @{ * - * @file pios_adc.c + * @file pios_adc.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Analog to Digital converstion routines * @see The GNU Public License (GPL) Version 3 *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,62 +36,65 @@ // Private types enum pios_adc_dev_magic { - PIOS_ADC_DEV_MAGIC = 0x58375124, + PIOS_ADC_DEV_MAGIC = 0x58375124, }; struct pios_adc_dev { - const struct pios_adc_cfg * cfg; - ADCCallback callback_function; + const struct pios_adc_cfg *cfg; + ADCCallback callback_function; #if defined(PIOS_INCLUDE_FREERTOS) - xQueueHandle data_queue; + xQueueHandle data_queue; #endif - volatile int16_t *valid_data_buffer; - volatile uint8_t adc_oversample; - uint8_t dma_block_size; - uint16_t dma_half_buffer_size; + volatile int16_t *valid_data_buffer; + volatile uint8_t adc_oversample; + uint8_t dma_block_size; + uint16_t dma_half_buffer_size; #if defined(PIOS_INCLUDE_ADC) - int16_t fir_coeffs[PIOS_ADC_MAX_SAMPLES+1] __attribute__ ((aligned(4))); - volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES] __attribute__ ((aligned(4))); // Double buffer that DMA just used - float downsampled_buffer[PIOS_ADC_NUM_CHANNELS] __attribute__ ((aligned(4))); + int16_t fir_coeffs[PIOS_ADC_MAX_SAMPLES + 1] __attribute__((aligned(4))); + volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES] __attribute__((aligned(4))); // Double buffer that DMA just used + float downsampled_buffer[PIOS_ADC_NUM_CHANNELS] __attribute__((aligned(4))); #endif - enum pios_adc_dev_magic magic; + enum pios_adc_dev_magic magic; }; #if defined(PIOS_INCLUDE_FREERTOS) -struct pios_adc_dev * pios_adc_dev; +struct pios_adc_dev *pios_adc_dev; #endif // Private functions void PIOS_ADC_downsample_data(); -static struct pios_adc_dev * PIOS_ADC_Allocate(); +static struct pios_adc_dev *PIOS_ADC_Allocate(); static bool PIOS_ADC_validate(struct pios_adc_dev *); /* Local Variables */ static GPIO_TypeDef *ADC_GPIO_PORT[PIOS_ADC_NUM_PINS] = PIOS_ADC_PORTS; static const uint32_t ADC_GPIO_PIN[PIOS_ADC_NUM_PINS] = PIOS_ADC_PINS; -static const uint32_t ADC_CHANNEL[PIOS_ADC_NUM_PINS] = PIOS_ADC_CHANNELS; +static const uint32_t ADC_CHANNEL[PIOS_ADC_NUM_PINS] = PIOS_ADC_CHANNELS; -static ADC_TypeDef *ADC_MAPPING[PIOS_ADC_NUM_PINS] = PIOS_ADC_MAPPING; +static ADC_TypeDef *ADC_MAPPING[PIOS_ADC_NUM_PINS] = PIOS_ADC_MAPPING; static const uint32_t ADC_CHANNEL_MAPPING[PIOS_ADC_NUM_PINS] = PIOS_ADC_CHANNEL_MAPPING; -static bool PIOS_ADC_validate(struct pios_adc_dev * dev) +static bool PIOS_ADC_validate(struct pios_adc_dev *dev) { - if (dev == NULL) - return false; + if (dev == NULL) { + return false; + } - return (dev->magic == PIOS_ADC_DEV_MAGIC); + return dev->magic == PIOS_ADC_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_adc_dev * PIOS_ADC_Allocate() +static struct pios_adc_dev *PIOS_ADC_Allocate() { - struct pios_adc_dev * adc_dev; - - adc_dev = (struct pios_adc_dev *)pvPortMalloc(sizeof(*adc_dev)); - if (!adc_dev) return (NULL); - - adc_dev->magic = PIOS_ADC_DEV_MAGIC; - return(adc_dev); + struct pios_adc_dev *adc_dev; + + adc_dev = (struct pios_adc_dev *)pvPortMalloc(sizeof(*adc_dev)); + if (!adc_dev) { + return NULL; + } + + adc_dev->magic = PIOS_ADC_DEV_MAGIC; + return adc_dev; } #else #error Not implemented @@ -100,34 +103,35 @@ static struct pios_adc_dev * PIOS_ADC_Allocate() /** * @brief Initialise the ADC Peripheral, configure to run at the max oversampling */ -int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg) +int32_t PIOS_ADC_Init(const struct pios_adc_cfg *cfg) { - pios_adc_dev = PIOS_ADC_Allocate(); - if (pios_adc_dev == NULL) - return -1; + pios_adc_dev = PIOS_ADC_Allocate(); + if (pios_adc_dev == NULL) { + return -1; + } + + pios_adc_dev->cfg = cfg; + pios_adc_dev->callback_function = NULL; - pios_adc_dev->cfg = cfg; - pios_adc_dev->callback_function = NULL; - #if defined(PIOS_INCLUDE_FREERTOS) - pios_adc_dev->data_queue = NULL; + pios_adc_dev->data_queue = NULL; #endif - - /* Setup analog pins */ - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; - - /* Enable each ADC pin in the array */ - for (int32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) { - GPIO_InitStructure.GPIO_Pin = ADC_GPIO_PIN[i]; - GPIO_Init(ADC_GPIO_PORT[i], &GPIO_InitStructure); - } - PIOS_ADC_Config(PIOS_ADC_MAX_OVERSAMPLING); - - return 0; + /* Setup analog pins */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; + + /* Enable each ADC pin in the array */ + for (int32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) { + GPIO_InitStructure.GPIO_Pin = ADC_GPIO_PIN[i]; + GPIO_Init(ADC_GPIO_PORT[i], &GPIO_InitStructure); + } + + PIOS_ADC_Config(PIOS_ADC_MAX_OVERSAMPLING); + + return 0; } /** @@ -135,100 +139,109 @@ int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg) * @param[in] oversampling the amount of oversampling to run at */ void PIOS_ADC_Config(uint32_t oversampling) -{ - pios_adc_dev->adc_oversample = (oversampling > PIOS_ADC_MAX_OVERSAMPLING) ? PIOS_ADC_MAX_OVERSAMPLING : oversampling; +{ + pios_adc_dev->adc_oversample = (oversampling > PIOS_ADC_MAX_OVERSAMPLING) ? PIOS_ADC_MAX_OVERSAMPLING : oversampling; + + ADC_DeInit(ADC1); + ADC_DeInit(ADC2); + + /* Disable interrupts */ + DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->dma.irq.flags, DISABLE); + + /* Enable ADC clocks */ + PIOS_ADC_CLOCK_FUNCTION; + + /* Map channels to conversion slots depending on the channel selection mask */ + for (int32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) { + ADC_RegularChannelConfig(ADC_MAPPING[i], ADC_CHANNEL[i], + ADC_CHANNEL_MAPPING[i], + PIOS_ADC_SAMPLE_TIME); + } - ADC_DeInit(ADC1); - ADC_DeInit(ADC2); - - /* Disable interrupts */ - DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->dma.irq.flags, DISABLE); - - /* Enable ADC clocks */ - PIOS_ADC_CLOCK_FUNCTION; - - /* Map channels to conversion slots depending on the channel selection mask */ - for (int32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) { - ADC_RegularChannelConfig(ADC_MAPPING[i], ADC_CHANNEL[i], - ADC_CHANNEL_MAPPING[i], - PIOS_ADC_SAMPLE_TIME); - } - #if (PIOS_ADC_USE_TEMP_SENSOR) - ADC_TempSensorVrefintCmd(ENABLE); - ADC_RegularChannelConfig(PIOS_ADC_TEMP_SENSOR_ADC, ADC_Channel_16, - PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL, - PIOS_ADC_SAMPLE_TIME); + ADC_TempSensorVrefintCmd(ENABLE); + ADC_RegularChannelConfig(PIOS_ADC_TEMP_SENSOR_ADC, ADC_Channel_16, + PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL, + PIOS_ADC_SAMPLE_TIME); #endif - // return - /* Configure ADCs */ - ADC_InitTypeDef ADC_InitStructure; - ADC_StructInit(&ADC_InitStructure); - ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult; - ADC_InitStructure.ADC_ScanConvMode = ENABLE; - ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; - ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; - ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; - ADC_InitStructure.ADC_NbrOfChannel = ((PIOS_ADC_NUM_CHANNELS + 1) >> 1); - ADC_Init(ADC1, &ADC_InitStructure); - -#if (PIOS_ADC_USE_ADC2) - ADC_Init(ADC2, &ADC_InitStructure); - - /* Enable ADC2 external trigger conversion (to synch with ADC1) */ - ADC_ExternalTrigConvCmd(ADC2, ENABLE); -#endif - - RCC_ADCCLKConfig(PIOS_ADC_ADCCLK); - - /* Enable ADC1->DMA request */ - ADC_DMACmd(ADC1, ENABLE); - - /* ADC1 calibration */ - ADC_Cmd(ADC1, ENABLE); - ADC_ResetCalibration(ADC1); - while (ADC_GetResetCalibrationStatus(ADC1)) ; - ADC_StartCalibration(ADC1); - while (ADC_GetCalibrationStatus(ADC1)) ; - -#if (PIOS_ADC_USE_ADC2) - /* ADC2 calibration */ - ADC_Cmd(ADC2, ENABLE); - ADC_ResetCalibration(ADC2); - while (ADC_GetResetCalibrationStatus(ADC2)) ; - ADC_StartCalibration(ADC2); - while (ADC_GetCalibrationStatus(ADC2)) ; -#endif - - /* This makes sure we have an even number of transfers if using ADC2 */ - pios_adc_dev->dma_block_size = ((PIOS_ADC_NUM_CHANNELS + PIOS_ADC_USE_ADC2) >> PIOS_ADC_USE_ADC2) << PIOS_ADC_USE_ADC2; - pios_adc_dev->dma_half_buffer_size = pios_adc_dev->dma_block_size * pios_adc_dev->adc_oversample; + // return + /* Configure ADCs */ + ADC_InitTypeDef ADC_InitStructure; + ADC_StructInit(&ADC_InitStructure); + ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult; + ADC_InitStructure.ADC_ScanConvMode = ENABLE; + ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; + ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; + ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; + ADC_InitStructure.ADC_NbrOfChannel = ((PIOS_ADC_NUM_CHANNELS + 1) >> 1); + ADC_Init(ADC1, &ADC_InitStructure); - /* Configure DMA channel */ - DMA_InitTypeDef dma_init = pios_adc_dev->cfg->dma.rx.init; - dma_init.DMA_MemoryBaseAddr = (uint32_t) &pios_adc_dev->raw_data_buffer[0]; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - dma_init.DMA_BufferSize = pios_adc_dev->dma_half_buffer_size; /* x2 for double buffer /2 for 32-bit xfr */ - DMA_Init(pios_adc_dev->cfg->dma.rx.channel, &dma_init); - DMA_Cmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE); - - /* Trigger interrupt when for half conversions too to indicate double buffer */ - DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_TC, ENABLE); - DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_HT, ENABLE); - - /* Configure DMA interrupt */ - NVIC_Init(&pios_adc_dev->cfg->dma.irq.init); - - /* Finally start initial conversion */ - ADC_SoftwareStartConvCmd(ADC1, ENABLE); - - /* Use simple averaging filter for now */ - for (int32_t i = 0; i < pios_adc_dev->adc_oversample; i++) - pios_adc_dev->fir_coeffs[i] = 1; - pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample] = pios_adc_dev->adc_oversample; - - /* Enable DMA1 clock */ - RCC_AHBPeriphClockCmd(pios_adc_dev->cfg->dma.ahb_clk, ENABLE); +#if (PIOS_ADC_USE_ADC2) + ADC_Init(ADC2, &ADC_InitStructure); + + /* Enable ADC2 external trigger conversion (to synch with ADC1) */ + ADC_ExternalTrigConvCmd(ADC2, ENABLE); +#endif + + RCC_ADCCLKConfig(PIOS_ADC_ADCCLK); + + /* Enable ADC1->DMA request */ + ADC_DMACmd(ADC1, ENABLE); + + /* ADC1 calibration */ + ADC_Cmd(ADC1, ENABLE); + ADC_ResetCalibration(ADC1); + while (ADC_GetResetCalibrationStatus(ADC1)) { + ; + } + ADC_StartCalibration(ADC1); + while (ADC_GetCalibrationStatus(ADC1)) { + ; + } + +#if (PIOS_ADC_USE_ADC2) + /* ADC2 calibration */ + ADC_Cmd(ADC2, ENABLE); + ADC_ResetCalibration(ADC2); + while (ADC_GetResetCalibrationStatus(ADC2)) { + ; + } + ADC_StartCalibration(ADC2); + while (ADC_GetCalibrationStatus(ADC2)) { + ; + } +#endif + + /* This makes sure we have an even number of transfers if using ADC2 */ + pios_adc_dev->dma_block_size = ((PIOS_ADC_NUM_CHANNELS + PIOS_ADC_USE_ADC2) >> PIOS_ADC_USE_ADC2) << PIOS_ADC_USE_ADC2; + pios_adc_dev->dma_half_buffer_size = pios_adc_dev->dma_block_size * pios_adc_dev->adc_oversample; + + /* Configure DMA channel */ + DMA_InitTypeDef dma_init = pios_adc_dev->cfg->dma.rx.init; + dma_init.DMA_MemoryBaseAddr = (uint32_t)&pios_adc_dev->raw_data_buffer[0]; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + dma_init.DMA_BufferSize = pios_adc_dev->dma_half_buffer_size; /* x2 for double buffer /2 for 32-bit xfr */ + DMA_Init(pios_adc_dev->cfg->dma.rx.channel, &dma_init); + DMA_Cmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE); + + /* Trigger interrupt when for half conversions too to indicate double buffer */ + DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_TC, ENABLE); + DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_HT, ENABLE); + + /* Configure DMA interrupt */ + NVIC_Init(&pios_adc_dev->cfg->dma.irq.init); + + /* Finally start initial conversion */ + ADC_SoftwareStartConvCmd(ADC1, ENABLE); + + /* Use simple averaging filter for now */ + for (int32_t i = 0; i < pios_adc_dev->adc_oversample; i++) { + pios_adc_dev->fir_coeffs[i] = 1; + } + pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample] = pios_adc_dev->adc_oversample; + + /* Enable DMA1 clock */ + RCC_AHBPeriphClockCmd(pios_adc_dev->cfg->dma.ahb_clk, ENABLE); } /** @@ -239,48 +252,48 @@ void PIOS_ADC_Config(uint32_t oversampling) */ int32_t PIOS_ADC_PinGet(uint32_t pin) { - /* Check if pin exists */ - if (pin >= PIOS_ADC_NUM_CHANNELS) { - return -1; - } - - /* Return last conversion result */ - return pios_adc_dev->downsampled_buffer[pin]; + /* Check if pin exists */ + if (pin >= PIOS_ADC_NUM_CHANNELS) { + return -1; + } + + /* Return last conversion result */ + return pios_adc_dev->downsampled_buffer[pin]; } /** * @brief Set a callback function that is executed whenever - * the ADC double buffer swaps + * the ADC double buffer swaps */ -void PIOS_ADC_SetCallback(ADCCallback new_function) +void PIOS_ADC_SetCallback(ADCCallback new_function) { - pios_adc_dev->callback_function = new_function; + pios_adc_dev->callback_function = new_function; } #if defined(PIOS_INCLUDE_FREERTOS) /** - * @brief Register a queue to add data to when downsampled + * @brief Register a queue to add data to when downsampled */ -void PIOS_ADC_SetQueue(xQueueHandle data_queue) +void PIOS_ADC_SetQueue(xQueueHandle data_queue) { - pios_adc_dev->data_queue = data_queue; + pios_adc_dev->data_queue = data_queue; } #endif /** - * @brief Return the address of the downsampled data buffer + * @brief Return the address of the downsampled data buffer */ -float * PIOS_ADC_GetBuffer(void) +float *PIOS_ADC_GetBuffer(void) { - return pios_adc_dev->downsampled_buffer; + return pios_adc_dev->downsampled_buffer; } /** - * @brief Return the address of the raw data data buffer + * @brief Return the address of the raw data data buffer */ -int16_t * PIOS_ADC_GetRawBuffer(void) +int16_t *PIOS_ADC_GetRawBuffer(void) { - return (int16_t *) pios_adc_dev->valid_data_buffer; + return (int16_t *)pios_adc_dev->valid_data_buffer; } /** @@ -288,85 +301,86 @@ int16_t * PIOS_ADC_GetRawBuffer(void) */ uint8_t PIOS_ADC_GetOverSampling(void) { - return pios_adc_dev->adc_oversample; + return pios_adc_dev->adc_oversample; } /** - * @brief Set the fir coefficients. Takes as many samples as the + * @brief Set the fir coefficients. Takes as many samples as the * current filter order plus one (normalization) * * @param new_filter Array of adc_oversampling floats plus one for the * filter coefficients */ -void PIOS_ADC_SetFIRCoefficients(float * new_filter) +void PIOS_ADC_SetFIRCoefficients(float *new_filter) { - // Less than or equal to get normalization constant - for(int i = 0; i <= pios_adc_dev->adc_oversample; i++) - pios_adc_dev->fir_coeffs[i] = new_filter[i]; + // Less than or equal to get normalization constant + for (int i = 0; i <= pios_adc_dev->adc_oversample; i++) { + pios_adc_dev->fir_coeffs[i] = new_filter[i]; + } } /** * @brief Downsample the data for each of the channels then call * callback function if installed - */ + */ void PIOS_ADC_downsample_data() { - uint16_t chan; - uint16_t sample; - float * downsampled_buffer = &pios_adc_dev->downsampled_buffer[0]; - - for (chan = 0; chan < PIOS_ADC_NUM_CHANNELS; chan++) { - int32_t sum = 0; - for (sample = 0; sample < pios_adc_dev->adc_oversample; sample++) { - sum += pios_adc_dev->valid_data_buffer[chan + sample * pios_adc_dev->dma_block_size] * pios_adc_dev->fir_coeffs[sample]; - } - downsampled_buffer[chan] = (float) sum / pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample]; - } - + uint16_t chan; + uint16_t sample; + float *downsampled_buffer = &pios_adc_dev->downsampled_buffer[0]; + + for (chan = 0; chan < PIOS_ADC_NUM_CHANNELS; chan++) { + int32_t sum = 0; + for (sample = 0; sample < pios_adc_dev->adc_oversample; sample++) { + sum += pios_adc_dev->valid_data_buffer[chan + sample * pios_adc_dev->dma_block_size] * pios_adc_dev->fir_coeffs[sample]; + } + downsampled_buffer[chan] = (float)sum / pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample]; + } + #if defined(PIOS_INCLUDE_FREERTOS) - if(pios_adc_dev->data_queue) { - static portBASE_TYPE xHigherPriorityTaskWoken; - xQueueSendFromISR(pios_adc_dev->data_queue, pios_adc_dev->downsampled_buffer, &xHigherPriorityTaskWoken); - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); - } + if (pios_adc_dev->data_queue) { + static portBASE_TYPE xHigherPriorityTaskWoken; + xQueueSendFromISR(pios_adc_dev->data_queue, pios_adc_dev->downsampled_buffer, &xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + } #endif - if(pios_adc_dev->callback_function) - pios_adc_dev->callback_function(pios_adc_dev->downsampled_buffer); + if (pios_adc_dev->callback_function) { + pios_adc_dev->callback_function(pios_adc_dev->downsampled_buffer); + } } /** * @brief Interrupt for half and full buffer transfer - * + * * This interrupt handler swaps between the two halfs of the double buffer to make * sure the ahrs uses the most recent data. Only swaps data when AHRS is idle, but - * really this is a pretense of a sanity check since the DMA engine is consantly + * really this is a pretense of a sanity check since the DMA engine is consantly * running in the background. Keep an eye on the ekf_too_slow variable to make sure * it's keeping up. */ void PIOS_ADC_DMA_Handler(void) { - if (!PIOS_ADC_validate(pios_adc_dev)) - return; + if (!PIOS_ADC_validate(pios_adc_dev)) { + return; + } - if (DMA_GetFlagStatus(pios_adc_dev->cfg->full_flag /*DMA1_IT_TC1*/)) { // whole double buffer filled - pios_adc_dev->valid_data_buffer = &pios_adc_dev->raw_data_buffer[pios_adc_dev->dma_half_buffer_size]; - DMA_ClearFlag(pios_adc_dev->cfg->full_flag); - PIOS_ADC_downsample_data(); - } - else if (DMA_GetFlagStatus(pios_adc_dev->cfg->half_flag /*DMA1_IT_HT1*/)) { - pios_adc_dev->valid_data_buffer = &pios_adc_dev->raw_data_buffer[0]; - DMA_ClearFlag(pios_adc_dev->cfg->half_flag); - PIOS_ADC_downsample_data(); - } - else { - // This should not happen, probably due to transfer errors - DMA_ClearFlag(pios_adc_dev->cfg->dma.irq.flags /*DMA1_FLAG_GL1*/); - } + if (DMA_GetFlagStatus(pios_adc_dev->cfg->full_flag /*DMA1_IT_TC1*/)) { // whole double buffer filled + pios_adc_dev->valid_data_buffer = &pios_adc_dev->raw_data_buffer[pios_adc_dev->dma_half_buffer_size]; + DMA_ClearFlag(pios_adc_dev->cfg->full_flag); + PIOS_ADC_downsample_data(); + } else if (DMA_GetFlagStatus(pios_adc_dev->cfg->half_flag /*DMA1_IT_HT1*/)) { + pios_adc_dev->valid_data_buffer = &pios_adc_dev->raw_data_buffer[0]; + DMA_ClearFlag(pios_adc_dev->cfg->half_flag); + PIOS_ADC_downsample_data(); + } else { + // This should not happen, probably due to transfer errors + DMA_ClearFlag(pios_adc_dev->cfg->dma.irq.flags /*DMA1_FLAG_GL1*/); + } } #endif /* PIOS_INCLUDE_ADC */ -/** +/** * @} * @} */ diff --git a/flight/pios/stm32f10x/pios_bkp.c b/flight/pios/stm32f10x/pios_bkp.c index b35e9177e..6238ee724 100644 --- a/flight/pios/stm32f10x/pios_bkp.c +++ b/flight/pios/stm32f10x/pios_bkp.c @@ -6,25 +6,25 @@ * @brief Hardware abstraction layer for backup sram * @{ * - * @file pios_bkp.c + * @file pios_bkp.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief IAP functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,115 +35,110 @@ #include /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ /***************************************************************************************** * Public Definitions/Macros ****************************************************************************************/ /**************************************************************************************** - * Public Functions - ****************************************************************************************/ -const uint32_t pios_bkp_registers_map[] = - { - BKP_DR1 , - BKP_DR2 , - BKP_DR3 , - BKP_DR4 , - BKP_DR5 , - BKP_DR6 , - BKP_DR7 , - BKP_DR8 , - BKP_DR9 , - BKP_DR10, - BKP_DR11, - BKP_DR12, - BKP_DR13, - BKP_DR14, - BKP_DR15, - BKP_DR16, - BKP_DR17, - BKP_DR18, - BKP_DR19, +* Public Functions +****************************************************************************************/ +const uint32_t pios_bkp_registers_map[] = { + BKP_DR1, + BKP_DR2, + BKP_DR3, + BKP_DR4, + BKP_DR5, + BKP_DR6, + BKP_DR7, + BKP_DR8, + BKP_DR9, + BKP_DR10, + BKP_DR11, + BKP_DR12, + BKP_DR13, + BKP_DR14, + BKP_DR15, + BKP_DR16, + BKP_DR17, + BKP_DR18, + BKP_DR19, -#if FALSE /* Not enabled as stm32f4 needs some modifications to - * accomodate more than 20 registers (like storing 2 uint16_t - * regs in one uint32_t bkp location) - */ - BKP_DR20, - BKP_DR21, - BKP_DR22, - BKP_DR23, - BKP_DR24, - BKP_DR25, - BKP_DR26, - BKP_DR27, - BKP_DR28, - BKP_DR29, - BKP_DR30, - BKP_DR32, - BKP_DR33, - BKP_DR34, - BKP_DR35, - BKP_DR36, - BKP_DR37, - BKP_DR38, - BKP_DR39, - BKP_DR40, - BKP_DR41, - BKP_DR42, -#endif - - }; +#if FALSE /* Not enabled as stm32f4 needs some modifications to + * accomodate more than 20 registers (like storing 2 uint16_t + * regs in one uint32_t bkp location) + */ + BKP_DR20, + BKP_DR21, + BKP_DR22, + BKP_DR23, + BKP_DR24, + BKP_DR25, + BKP_DR26, + BKP_DR27, + BKP_DR28, + BKP_DR29, + BKP_DR30, + BKP_DR32, + BKP_DR33, + BKP_DR34, + BKP_DR35, + BKP_DR36, + BKP_DR37, + BKP_DR38, + BKP_DR39, + BKP_DR40, + BKP_DR41, + BKP_DR42, +#endif /* if FALSE */ +}; #define PIOS_BKP_REGISTERS_COUNT NELEMENTS(pios_bkp_registers_map) void PIOS_BKP_Init(void) { - /* Enable CRC clock */ - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); + /* Enable CRC clock */ + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); - /* Enable PWR and BKP clock */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR | RCC_APB1Periph_BKP, ENABLE); + /* Enable PWR and BKP clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR | RCC_APB1Periph_BKP, ENABLE); - /* Clear Tamper pin Event(TE) pending flag */ - BKP_ClearFlag(); + /* Clear Tamper pin Event(TE) pending flag */ + BKP_ClearFlag(); } uint16_t PIOS_BKP_ReadRegister(uint32_t regnumber) { - if(PIOS_BKP_REGISTERS_COUNT < regnumber) - { - PIOS_Assert(0); - } else { - return (uint16_t) BKP_ReadBackupRegister(pios_bkp_registers_map[regnumber]); - } + if (PIOS_BKP_REGISTERS_COUNT < regnumber) { + PIOS_Assert(0); + } else { + return (uint16_t)BKP_ReadBackupRegister(pios_bkp_registers_map[regnumber]); + } } -void PIOS_BKP_WriteRegister(uint32_t regnumber,uint16_t data) +void PIOS_BKP_WriteRegister(uint32_t regnumber, uint16_t data) { - if(PIOS_BKP_REGISTERS_COUNT < regnumber) - { - PIOS_Assert(0); - } else { - BKP_WriteBackupRegister(pios_bkp_registers_map[regnumber],(uint32_t)data); - } + if (PIOS_BKP_REGISTERS_COUNT < regnumber) { + PIOS_Assert(0); + } else { + BKP_WriteBackupRegister(pios_bkp_registers_map[regnumber], (uint32_t)data); + } } void PIOS_BKP_EnableWrite(void) { - /* Enable write access to Backup domain */ - PWR_BackupAccessCmd(ENABLE); + /* Enable write access to Backup domain */ + PWR_BackupAccessCmd(ENABLE); } void PIOS_BKP_DisableWrite(void) { - /* Enable write access to Backup domain */ - PWR_BackupAccessCmd(DISABLE); + /* Enable write access to Backup domain */ + PWR_BackupAccessCmd(DISABLE); } - /**************************************************************************************** - * Public Data - ****************************************************************************************/ +* Public Data +****************************************************************************************/ diff --git a/flight/pios/stm32f10x/pios_bl_helper.c b/flight/pios/stm32f10x/pios_bl_helper.c index 0c2d7b7d9..56d4eb323 100644 --- a/flight/pios/stm32f10x/pios_bl_helper.c +++ b/flight/pios/stm32f10x/pios_bl_helper.c @@ -38,7 +38,7 @@ uint8_t *PIOS_BL_HELPER_FLASH_If_Read(uint32_t SectorAddress) { - return (uint8_t *) (SectorAddress); + return (uint8_t *)(SectorAddress); } #if defined(PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT) @@ -53,28 +53,31 @@ uint8_t PIOS_BL_HELPER_FLASH_Ini() uint8_t PIOS_BL_HELPER_FLASH_Start() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; + const struct pios_board_info *bdinfo = &pios_board_info_blob; uint32_t startAddress = bdinfo->fw_base; - uint32_t endAddress = bdinfo->fw_base + bdinfo->fw_size + bdinfo->desc_size; + uint32_t endAddress = bdinfo->fw_base + bdinfo->fw_size + bdinfo->desc_size; bool success = erase_flash(startAddress, endAddress); return (success) ? 1 : 0; } -uint8_t PIOS_BL_HELPER_FLASH_Erase_Bootloader() { +uint8_t PIOS_BL_HELPER_FLASH_Erase_Bootloader() +{ /// Bootloader memory space erase uint32_t startAddress = BL_BANK_BASE; - uint32_t endAddress = BL_BANK_BASE + BL_BANK_SIZE; + uint32_t endAddress = BL_BANK_BASE + BL_BANK_SIZE; bool success = erase_flash(startAddress, endAddress); return (success) ? 1 : 0; } -static bool erase_flash(uint32_t startAddress, uint32_t endAddress) { +static bool erase_flash(uint32_t startAddress, uint32_t endAddress) +{ uint32_t pageAddress = startAddress; uint8_t fail = false; + while ((pageAddress < endAddress) && (fail == false)) { for (int retry = 0; retry < MAX_DEL_RETRYS; ++retry) { if (FLASH_ErasePage(pageAddress) == FLASH_COMPLETE) { @@ -87,39 +90,42 @@ static bool erase_flash(uint32_t startAddress, uint32_t endAddress) { #ifdef STM32F10X_HD pageAddress += 2048; -#elif defined (STM32F10X_MD) +#elif defined(STM32F10X_MD) pageAddress += 1024; #endif } return !fail; } -#endif +#endif /* if defined(PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT) */ uint32_t PIOS_BL_HELPER_CRC_Memory_Calc() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; + const struct pios_board_info *bdinfo = &pios_board_info_blob; - PIOS_BL_HELPER_CRC_Ini(); - CRC_ResetDR(); - CRC_CalcBlockCRC((uint32_t *) bdinfo->fw_base, (bdinfo->fw_size) >> 2); - return CRC_GetCRC(); + PIOS_BL_HELPER_CRC_Ini(); + CRC_ResetDR(); + CRC_CalcBlockCRC((uint32_t *)bdinfo->fw_base, (bdinfo->fw_size) >> 2); + return CRC_GetCRC(); } -void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t * array, uint8_t size) +void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t *array, uint8_t size) { - const struct pios_board_info * bdinfo = &pios_board_info_blob; - uint8_t x = 0; - if (size > bdinfo->desc_size) size = bdinfo->desc_size; - for (uint32_t i = bdinfo->fw_base + bdinfo->fw_size; i < bdinfo->fw_base + bdinfo->fw_size + size; ++i) { - array[x] = *PIOS_BL_HELPER_FLASH_If_Read(i); - ++x; - } + const struct pios_board_info *bdinfo = &pios_board_info_blob; + uint8_t x = 0; + + if (size > bdinfo->desc_size) { + size = bdinfo->desc_size; + } + for (uint32_t i = bdinfo->fw_base + bdinfo->fw_size; i < bdinfo->fw_base + bdinfo->fw_size + size; ++i) { + array[x] = *PIOS_BL_HELPER_FLASH_If_Read(i); + ++x; + } } void PIOS_BL_HELPER_CRC_Ini() { - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); } #endif /* PIOS_INCLUDE_BL_HELPER */ diff --git a/flight/pios/stm32f10x/pios_debug.c b/flight/pios/stm32f10x/pios_debug.c index f0f6dbcaf..9f641f162 100644 --- a/flight/pios/stm32f10x/pios_debug.c +++ b/flight/pios/stm32f10x/pios_debug.c @@ -10,21 +10,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief Debugging Functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,76 +34,76 @@ const char *PIOS_DEBUG_AssertMsg = "ASSERT FAILED"; #ifdef PIOS_ENABLE_DEBUG_PINS -static const struct pios_tim_channel * debug_channels; +static const struct pios_tim_channel *debug_channels; static uint8_t debug_num_channels; -#endif /* PIOS_ENABLE_DEBUG_PINS */ +#endif /* PIOS_ENABLE_DEBUG_PINS */ /** -* Initialise Debug-features -*/ -void PIOS_DEBUG_Init(__attribute__((unused)) const struct pios_tim_channel * channels, - __attribute__((unused)) uint8_t num_channels) + * Initialise Debug-features + */ +void PIOS_DEBUG_Init(__attribute__((unused)) const struct pios_tim_channel *channels, + __attribute__((unused)) uint8_t num_channels) { #ifdef PIOS_ENABLE_DEBUG_PINS - PIOS_Assert(channels); - PIOS_Assert(num_channels); + PIOS_Assert(channels); + PIOS_Assert(num_channels); - /* Store away the GPIOs we've been given */ - debug_channels = channels; - debug_num_channels = num_channels; + /* Store away the GPIOs we've been given */ + debug_channels = channels; + debug_num_channels = num_channels; - /* Configure the GPIOs we've been given */ - for (uint8_t i = 0; i < num_channels; i++) { - const struct pios_tim_channel * chan = &channels[i]; + /* Configure the GPIOs we've been given */ + for (uint8_t i = 0; i < num_channels; i++) { + const struct pios_tim_channel *chan = &channels[i]; - // Initialise pins as standard output pins - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_InitStructure.GPIO_Pin = chan->pin.init.GPIO_Pin; + // Initialise pins as standard output pins + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_Pin = chan->pin.init.GPIO_Pin; - /* Initialize the GPIO */ - GPIO_Init(chan->pin.gpio, &GPIO_InitStructure); + /* Initialize the GPIO */ + GPIO_Init(chan->pin.gpio, &GPIO_InitStructure); - /* Set the pin low */ - GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); - } + /* Set the pin low */ + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); + } #endif // PIOS_ENABLE_DEBUG_PINS } /** -* Set debug-pin high -* \param pin 0 for S1 output -*/ + * Set debug-pin high + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinHigh(__attribute__((unused)) uint8_t pin) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels || pin >= debug_num_channels) { - return; - } + if (!debug_channels || pin >= debug_num_channels) { + return; + } - const struct pios_tim_channel * chan = &debug_channels[pin]; + const struct pios_tim_channel *chan = &debug_channels[pin]; - GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_SET); + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_SET); #endif // PIOS_ENABLE_DEBUG_PINS } /** -* Set debug-pin low -* \param pin 0 for S1 output -*/ + * Set debug-pin low + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinLow(__attribute__((unused)) uint8_t pin) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels || pin >= debug_num_channels) { - return; - } + if (!debug_channels || pin >= debug_num_channels) { + return; + } - const struct pios_tim_channel * chan = &debug_channels[pin]; + const struct pios_tim_channel *chan = &debug_channels[pin]; - GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); #endif // PIOS_ENABLE_DEBUG_PINS } @@ -112,39 +112,39 @@ void PIOS_DEBUG_PinLow(__attribute__((unused)) uint8_t pin) void PIOS_DEBUG_PinValue8Bit(__attribute__((unused)) uint8_t value) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels) { - return; - } + if (!debug_channels) { + return; + } - uint32_t bsrr_l = ( ((~value)&0x0F)<<(16+6) ) | ((value & 0x0F)<<6); - uint32_t bsrr_h = ( ((~value)&0xF0)<<(16+6-4) ) | ((value & 0xF0)<<(6-4)); + uint32_t bsrr_l = (((~value) & 0x0F) << (16 + 6)) | ((value & 0x0F) << 6); + uint32_t bsrr_h = (((~value) & 0xF0) << (16 + 6 - 4)) | ((value & 0xF0) << (6 - 4)); - PIOS_IRQ_Disable(); + PIOS_IRQ_Disable(); - /* - * This is sketchy since it assumes a particular ordering - * and bitwise layout of the channels provided to the debug code. - */ - debug_channels[0].pin.gpio->BSRR = bsrr_l; - debug_channels[4].pin.gpio->BSRR = bsrr_h; + /* + * This is sketchy since it assumes a particular ordering + * and bitwise layout of the channels provided to the debug code. + */ + debug_channels[0].pin.gpio->BSRR = bsrr_l; + debug_channels[4].pin.gpio->BSRR = bsrr_h; - PIOS_IRQ_Enable(); + PIOS_IRQ_Enable(); #endif // PIOS_ENABLE_DEBUG_PINS } void PIOS_DEBUG_PinValue4BitL(__attribute__((unused)) uint8_t value) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels) { - return; - } + if (!debug_channels) { + return; + } - /* - * This is sketchy since it assumes a particular ordering - * and bitwise layout of the channels provided to the debug code. - */ - uint32_t bsrr_l = ((~(value & 0x0F)<<(16+6))) | ((value & 0x0F)<<6); - debug_channels[0].pin.gpio->BSRR = bsrr_l; + /* + * This is sketchy since it assumes a particular ordering + * and bitwise layout of the channels provided to the debug code. + */ + uint32_t bsrr_l = ((~(value & 0x0F) << (16 + 6))) | ((value & 0x0F) << 6); + debug_channels[0].pin.gpio->BSRR = bsrr_l; #endif // PIOS_ENABLE_DEBUG_PINS } @@ -155,15 +155,17 @@ void PIOS_DEBUG_PinValue4BitL(__attribute__((unused)) uint8_t value) void PIOS_DEBUG_Panic(__attribute__((unused)) const char *msg) { #ifdef PIOS_INCLUDE_DEBUG_CONSOLE - register int *lr asm("lr"); // Link-register holds the PC of the caller - DEBUG_PRINTF(0, "\r%s @0x%x\r", msg, lr); + register int *lr asm ("lr"); // Link-register holds the PC of the caller + DEBUG_PRINTF(0, "\r%s @0x%x\r", msg, lr); #endif - // Stay put - while (1) ; + // Stay put + while (1) { + ; + } } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_delay.c b/flight/pios/stm32f10x/pios_delay.c index a61ab45f8..9effa1ec7 100644 --- a/flight/pios/stm32f10x/pios_delay.c +++ b/flight/pios/stm32f10x/pios_delay.c @@ -8,25 +8,25 @@ * * @file pios_delay.c * @author Michael Smith Copyright (C) 2011 - * @brief Delay Functions + * @brief Delay Functions * - Provides a micro-second granular delay using the CPU * cycle counter. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,9 @@ #ifdef PIOS_INCLUDE_DELAY /* these should be defined by CMSIS, but they aren't */ -#define DWT_CTRL (*(volatile uint32_t *)0xe0001000) -#define CYCCNTENA (1<<0) -#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004) +#define DWT_CTRL (*(volatile uint32_t *)0xe0001000) +#define CYCCNTENA (1 << 0) +#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004) /* cycles per microsecond */ @@ -51,20 +51,20 @@ static uint32_t us_ticks; int32_t PIOS_DELAY_Init(void) { - RCC_ClocksTypeDef clocks; + RCC_ClocksTypeDef clocks; - /* compute the number of system clocks per microsecond */ - RCC_GetClocksFreq(&clocks); - us_ticks = clocks.SYSCLK_Frequency / 1000000; - PIOS_DEBUG_Assert(us_ticks > 1); + /* compute the number of system clocks per microsecond */ + RCC_GetClocksFreq(&clocks); + us_ticks = clocks.SYSCLK_Frequency / 1000000; + PIOS_DEBUG_Assert(us_ticks > 1); - /* turn on access to the DWT registers */ - CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + /* turn on access to the DWT registers */ + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; - /* enable the CPU cycle counter */ - DWT_CTRL |= CYCCNTENA; + /* enable the CPU cycle counter */ + DWT_CTRL |= CYCCNTENA; - return 0; + return 0; } /** @@ -80,31 +80,32 @@ int32_t PIOS_DELAY_Init(void) */ int32_t PIOS_DELAY_WaituS(uint32_t uS) { - uint32_t elapsed = 0; - uint32_t last_count = DWT_CYCCNT; - - for (;;) { - uint32_t current_count = DWT_CYCCNT; - uint32_t elapsed_uS; + uint32_t elapsed = 0; + uint32_t last_count = DWT_CYCCNT; - /* measure the time elapsed since the last time we checked */ - elapsed += current_count - last_count; - last_count = current_count; + for (;;) { + uint32_t current_count = DWT_CYCCNT; + uint32_t elapsed_uS; - /* convert to microseconds */ - elapsed_uS = elapsed / us_ticks; - if (elapsed_uS >= uS) - break; + /* measure the time elapsed since the last time we checked */ + elapsed += current_count - last_count; + last_count = current_count; - /* reduce the delay by the elapsed time */ - uS -= elapsed_uS; + /* convert to microseconds */ + elapsed_uS = elapsed / us_ticks; + if (elapsed_uS >= uS) { + break; + } - /* keep fractional microseconds for the next iteration */ - elapsed %= us_ticks; - } + /* reduce the delay by the elapsed time */ + uS -= elapsed_uS; - /* No error */ - return 0; + /* keep fractional microseconds for the next iteration */ + elapsed %= us_ticks; + } + + /* No error */ + return 0; } /** @@ -120,21 +121,21 @@ int32_t PIOS_DELAY_WaituS(uint32_t uS) */ int32_t PIOS_DELAY_WaitmS(uint32_t mS) { - while (mS--) { - PIOS_DELAY_WaituS(1000); - } + while (mS--) { + PIOS_DELAY_WaituS(1000); + } - /* No error */ - return 0; + /* No error */ + return 0; } /** - * @brief Query the Delay timer for the current uS + * @brief Query the Delay timer for the current uS * @return A microsecond value */ uint32_t PIOS_DELAY_GetuS(void) { - return DWT_CYCCNT / us_ticks; + return DWT_CYCCNT / us_ticks; } /** @@ -144,7 +145,7 @@ uint32_t PIOS_DELAY_GetuS(void) */ uint32_t PIOS_DELAY_GetuSSince(uint32_t t) { - return (PIOS_DELAY_GetuS() - t); + return PIOS_DELAY_GetuS() - t; } /** @@ -153,22 +154,23 @@ uint32_t PIOS_DELAY_GetuSSince(uint32_t t) */ uint32_t PIOS_DELAY_GetRaw() { - return DWT_CYCCNT; + return DWT_CYCCNT; } /** - * @brief Compare to raw times to and convert to us + * @brief Compare to raw times to and convert to us * @return A microsecond value */ uint32_t PIOS_DELAY_DiffuS(uint32_t raw) { - uint32_t diff = DWT_CYCCNT - raw; - return diff / us_ticks; + uint32_t diff = DWT_CYCCNT - raw; + + return diff / us_ticks; } #endif /* PIOS_INCLUDE_DELAY */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_dsm.c b/flight/pios/stm32f10x/pios_dsm.c index 9ec036efa..d854d7b9c 100644 --- a/flight/pios/stm32f10x/pios_dsm.c +++ b/flight/pios/stm32f10x/pios_dsm.c @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,132 +38,138 @@ /* Forward Declarations */ static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel); static uint16_t PIOS_DSM_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield); + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield); static void PIOS_DSM_Supervisor(uint32_t dsm_id); /* Local Variables */ const struct pios_rcvr_driver pios_dsm_rcvr_driver = { - .read = PIOS_DSM_Get, + .read = PIOS_DSM_Get, }; enum pios_dsm_dev_magic { - PIOS_DSM_DEV_MAGIC = 0x44534d78, + PIOS_DSM_DEV_MAGIC = 0x44534d78, }; struct pios_dsm_state { - uint16_t channel_data[PIOS_DSM_NUM_INPUTS]; - uint8_t received_data[DSM_FRAME_LENGTH]; - uint8_t receive_timer; - uint8_t failsafe_timer; - uint8_t frame_found; - uint8_t byte_count; + uint16_t channel_data[PIOS_DSM_NUM_INPUTS]; + uint8_t received_data[DSM_FRAME_LENGTH]; + uint8_t receive_timer; + uint8_t failsafe_timer; + uint8_t frame_found; + uint8_t byte_count; #ifdef DSM_LOST_FRAME_COUNTER - uint8_t frames_lost_last; - uint16_t frames_lost; + uint8_t frames_lost_last; + uint16_t frames_lost; #endif }; struct pios_dsm_dev { - enum pios_dsm_dev_magic magic; - const struct pios_dsm_cfg *cfg; - enum pios_dsm_proto proto; - struct pios_dsm_state state; + enum pios_dsm_dev_magic magic; + const struct pios_dsm_cfg *cfg; + enum pios_dsm_proto proto; + struct pios_dsm_state state; }; /* Allocate DSM device descriptor */ #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_dsm_dev *PIOS_DSM_Alloc(void) { - struct pios_dsm_dev *dsm_dev; + struct pios_dsm_dev *dsm_dev; - dsm_dev = (struct pios_dsm_dev *)pvPortMalloc(sizeof(*dsm_dev)); - if (!dsm_dev) - return NULL; + dsm_dev = (struct pios_dsm_dev *)pvPortMalloc(sizeof(*dsm_dev)); + if (!dsm_dev) { + return NULL; + } - dsm_dev->magic = PIOS_DSM_DEV_MAGIC; - return dsm_dev; + dsm_dev->magic = PIOS_DSM_DEV_MAGIC; + return dsm_dev; } #else static struct pios_dsm_dev pios_dsm_devs[PIOS_DSM_MAX_DEVS]; static uint8_t pios_dsm_num_devs; static struct pios_dsm_dev *PIOS_DSM_Alloc(void) { - struct pios_dsm_dev *dsm_dev; + struct pios_dsm_dev *dsm_dev; - if (pios_dsm_num_devs >= PIOS_DSM_MAX_DEVS) - return NULL; + if (pios_dsm_num_devs >= PIOS_DSM_MAX_DEVS) { + return NULL; + } - dsm_dev = &pios_dsm_devs[pios_dsm_num_devs++]; - dsm_dev->magic = PIOS_DSM_DEV_MAGIC; + dsm_dev = &pios_dsm_devs[pios_dsm_num_devs++]; + dsm_dev->magic = PIOS_DSM_DEV_MAGIC; - return dsm_dev; + return dsm_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /* Validate DSM device descriptor */ static bool PIOS_DSM_Validate(struct pios_dsm_dev *dsm_dev) { - return (dsm_dev->magic == PIOS_DSM_DEV_MAGIC); + return dsm_dev->magic == PIOS_DSM_DEV_MAGIC; } /* Try to bind DSMx satellite using specified number of pulses */ static void PIOS_DSM_Bind(struct pios_dsm_dev *dsm_dev, uint8_t bind) { - const struct pios_dsm_cfg *cfg = dsm_dev->cfg; + const struct pios_dsm_cfg *cfg = dsm_dev->cfg; - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Pin = cfg->bind.init.GPIO_Pin; - GPIO_InitStructure.GPIO_Speed = cfg->bind.init.GPIO_Speed; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; + GPIO_InitTypeDef GPIO_InitStructure; - /* just to limit bind pulses */ - if (bind > 10) - bind = 10; + GPIO_InitStructure.GPIO_Pin = cfg->bind.init.GPIO_Pin; + GPIO_InitStructure.GPIO_Speed = cfg->bind.init.GPIO_Speed; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; - GPIO_Init(cfg->bind.gpio, &cfg->bind.init); + /* just to limit bind pulses */ + if (bind > 10) { + bind = 10; + } - /* RX line, set high */ - GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + GPIO_Init(cfg->bind.gpio, &cfg->bind.init); - /* on CC works up to 140ms, guess bind window is around 20-140ms after power up */ - PIOS_DELAY_WaitmS(20); + /* RX line, set high */ + GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - for (int i = 0; i < bind ; i++) { - /* RX line, drive low for 120us */ - GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - PIOS_DELAY_WaituS(120); - /* RX line, drive high for 120us */ - GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - PIOS_DELAY_WaituS(120); - } - /* RX line, set input and wait for data */ - GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure); + /* on CC works up to 140ms, guess bind window is around 20-140ms after power up */ + PIOS_DELAY_WaitmS(20); + + for (int i = 0; i < bind; i++) { + /* RX line, drive low for 120us */ + GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + PIOS_DELAY_WaituS(120); + /* RX line, drive high for 120us */ + GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + PIOS_DELAY_WaituS(120); + } + /* RX line, set input and wait for data */ + GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure); } /* Reset channels in case of lost signal or explicit failsafe receiver flag */ static void PIOS_DSM_ResetChannels(struct pios_dsm_dev *dsm_dev) { - struct pios_dsm_state *state = &(dsm_dev->state); - for (int i = 0; i < PIOS_DSM_NUM_INPUTS; i++) { - state->channel_data[i] = PIOS_RCVR_TIMEOUT; - } + struct pios_dsm_state *state = &(dsm_dev->state); + + for (int i = 0; i < PIOS_DSM_NUM_INPUTS; i++) { + state->channel_data[i] = PIOS_RCVR_TIMEOUT; + } } /* Reset DSM receiver state */ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev) { - struct pios_dsm_state *state = &(dsm_dev->state); - state->receive_timer = 0; - state->failsafe_timer = 0; - state->frame_found = 0; + struct pios_dsm_state *state = &(dsm_dev->state); + + state->receive_timer = 0; + state->failsafe_timer = 0; + state->frame_found = 0; #ifdef DSM_LOST_FRAME_COUNTER - state->frames_lost_last = 0; - state->frames_lost = 0; + state->frames_lost_last = 0; + state->frames_lost = 0; #endif - PIOS_DSM_ResetChannels(dsm_dev); + PIOS_DSM_ResetChannels(dsm_dev); } /** @@ -173,174 +179,182 @@ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev) */ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) { - struct pios_dsm_state *state = &(dsm_dev->state); - uint8_t resolution; + struct pios_dsm_state *state = &(dsm_dev->state); + uint8_t resolution; #ifdef DSM_LOST_FRAME_COUNTER - /* increment the lost frame counter */ - uint8_t frames_lost = state->received_data[0]; - state->frames_lost += (frames_lost - state->frames_lost_last); - state->frames_lost_last = frames_lost; + /* increment the lost frame counter */ + uint8_t frames_lost = state->received_data[0]; + state->frames_lost += (frames_lost - state->frames_lost_last); + state->frames_lost_last = frames_lost; #endif - /* check the frame type assuming master satellite stream */ - uint8_t type = state->received_data[1]; - switch (type) { - case 0x01: - case 0x02: - case 0x12: - /* DSM2, DSMJ stream */ - if (dsm_dev->proto == PIOS_DSM_PROTO_DSM2) { - /* DSM2/DSMJ resolution is known from the header */ - resolution = (type & DSM_DSM2_RES_MASK) ? 11 : 10; - } else { - /* DSMX resolution should explicitly be selected */ - goto stream_error; - } - break; - case 0xA2: - case 0xB2: - /* DSMX stream */ - if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) { - resolution = 10; - } else if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) { - resolution = 11; - } else { - /* DSMX resolution should explicitly be selected */ - goto stream_error; - } - break; - default: - /* unknown yet data stream */ - goto stream_error; - } + /* check the frame type assuming master satellite stream */ + uint8_t type = state->received_data[1]; + switch (type) { + case 0x01: + case 0x02: + case 0x12: + /* DSM2, DSMJ stream */ + if (dsm_dev->proto == PIOS_DSM_PROTO_DSM2) { + /* DSM2/DSMJ resolution is known from the header */ + resolution = (type & DSM_DSM2_RES_MASK) ? 11 : 10; + } else { + /* DSMX resolution should explicitly be selected */ + goto stream_error; + } + break; + case 0xA2: + case 0xB2: + /* DSMX stream */ + if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) { + resolution = 10; + } else if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) { + resolution = 11; + } else { + /* DSMX resolution should explicitly be selected */ + goto stream_error; + } + break; + default: + /* unknown yet data stream */ + goto stream_error; + } - /* unroll channels */ - uint8_t *s = &(state->received_data[2]); - uint16_t mask = (resolution == 10) ? 0x03ff : 0x07ff; + /* unroll channels */ + uint8_t *s = &(state->received_data[2]); + uint16_t mask = (resolution == 10) ? 0x03ff : 0x07ff; - for (int i = 0; i < DSM_CHANNELS_PER_FRAME; i++) { - uint16_t word = ((uint16_t)s[0] << 8) | s[1]; - s += 2; + for (int i = 0; i < DSM_CHANNELS_PER_FRAME; i++) { + uint16_t word = ((uint16_t)s[0] << 8) | s[1]; + s += 2; - /* skip empty channel slot */ - if (word == 0xffff) - continue; + /* skip empty channel slot */ + if (word == 0xffff) { + continue; + } - /* minimal data validation */ - if ((i > 0) && (word & DSM_2ND_FRAME_MASK)) { - /* invalid frame data, ignore rest of the frame */ - goto stream_error; - } + /* minimal data validation */ + if ((i > 0) && (word & DSM_2ND_FRAME_MASK)) { + /* invalid frame data, ignore rest of the frame */ + goto stream_error; + } - /* extract and save the channel value */ - uint8_t channel_num = (word >> resolution) & 0x0f; - if (channel_num < PIOS_DSM_NUM_INPUTS) - state->channel_data[channel_num] = (word & mask); - } + /* extract and save the channel value */ + uint8_t channel_num = (word >> resolution) & 0x0f; + if (channel_num < PIOS_DSM_NUM_INPUTS) { + state->channel_data[channel_num] = (word & mask); + } + } #ifdef DSM_LOST_FRAME_COUNTER - /* put lost frames counter into the last channel for debugging */ - state->channel_data[PIOS_DSM_NUM_INPUTS-1] = state->frames_lost; + /* put lost frames counter into the last channel for debugging */ + state->channel_data[PIOS_DSM_NUM_INPUTS - 1] = state->frames_lost; #endif - /* all channels processed */ - return 0; + /* all channels processed */ + return 0; stream_error: - /* either DSM2 selected with DSMX stream found, or vice-versa */ - return -1; + /* either DSM2 selected with DSMX stream found, or vice-versa */ + return -1; } /* Update decoder state processing input byte from the DSMx stream */ static void PIOS_DSM_UpdateState(struct pios_dsm_dev *dsm_dev, uint8_t byte) { - struct pios_dsm_state *state = &(dsm_dev->state); - if (state->frame_found) { - /* receiving the data frame */ - if (state->byte_count < DSM_FRAME_LENGTH) { - /* store next byte */ - state->received_data[state->byte_count++] = byte; - if (state->byte_count == DSM_FRAME_LENGTH) { - /* full frame received - process and wait for new one */ - if (!PIOS_DSM_UnrollChannels(dsm_dev)) - /* data looking good */ - state->failsafe_timer = 0; + struct pios_dsm_state *state = &(dsm_dev->state); - /* prepare for the next frame */ - state->frame_found = 0; - } - } - } + if (state->frame_found) { + /* receiving the data frame */ + if (state->byte_count < DSM_FRAME_LENGTH) { + /* store next byte */ + state->received_data[state->byte_count++] = byte; + if (state->byte_count == DSM_FRAME_LENGTH) { + /* full frame received - process and wait for new one */ + if (!PIOS_DSM_UnrollChannels(dsm_dev)) { + /* data looking good */ + state->failsafe_timer = 0; + } + + /* prepare for the next frame */ + state->frame_found = 0; + } + } + } } /* Initialise DSM receiver interface */ int32_t PIOS_DSM_Init(uint32_t *dsm_id, - const struct pios_dsm_cfg *cfg, - const struct pios_com_driver *driver, - uint32_t lower_id, - enum pios_dsm_proto proto, - uint8_t bind) + const struct pios_dsm_cfg *cfg, + const struct pios_com_driver *driver, + uint32_t lower_id, + enum pios_dsm_proto proto, + uint8_t bind) { - PIOS_DEBUG_Assert(dsm_id); - PIOS_DEBUG_Assert(cfg); - PIOS_DEBUG_Assert(driver); + PIOS_DEBUG_Assert(dsm_id); + PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(driver); - struct pios_dsm_dev *dsm_dev; + struct pios_dsm_dev *dsm_dev; - dsm_dev = (struct pios_dsm_dev *)PIOS_DSM_Alloc(); - if (!dsm_dev) - return -1; + dsm_dev = (struct pios_dsm_dev *)PIOS_DSM_Alloc(); + if (!dsm_dev) { + return -1; + } - /* Bind the configuration to the device instance */ - dsm_dev->cfg = cfg; - dsm_dev->proto = proto; + /* Bind the configuration to the device instance */ + dsm_dev->cfg = cfg; + dsm_dev->proto = proto; - /* Bind the receiver if requested */ - if (bind) - PIOS_DSM_Bind(dsm_dev, bind); + /* Bind the receiver if requested */ + if (bind) { + PIOS_DSM_Bind(dsm_dev, bind); + } - PIOS_DSM_ResetState(dsm_dev); + PIOS_DSM_ResetState(dsm_dev); - *dsm_id = (uint32_t)dsm_dev; + *dsm_id = (uint32_t)dsm_dev; - /* Set comm driver callback */ - (driver->bind_rx_cb)(lower_id, PIOS_DSM_RxInCallback, *dsm_id); + /* Set comm driver callback */ + (driver->bind_rx_cb)(lower_id, PIOS_DSM_RxInCallback, *dsm_id); - if (!PIOS_RTC_RegisterTickCallback(PIOS_DSM_Supervisor, *dsm_id)) { - PIOS_DEBUG_Assert(0); - } + if (!PIOS_RTC_RegisterTickCallback(PIOS_DSM_Supervisor, *dsm_id)) { + PIOS_DEBUG_Assert(0); + } - return 0; + return 0; } /* Comm byte received callback */ static uint16_t PIOS_DSM_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield) + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield) { - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)context; + struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)context; - bool valid = PIOS_DSM_Validate(dsm_dev); - PIOS_Assert(valid); + bool valid = PIOS_DSM_Validate(dsm_dev); - /* process byte(s) and clear receive timer */ - for (uint8_t i = 0; i < buf_len; i++) { - PIOS_DSM_UpdateState(dsm_dev, buf[i]); - dsm_dev->state.receive_timer = 0; - } + PIOS_Assert(valid); - /* Always signal that we can accept another byte */ - if (headroom) - *headroom = DSM_FRAME_LENGTH; + /* process byte(s) and clear receive timer */ + for (uint8_t i = 0; i < buf_len; i++) { + PIOS_DSM_UpdateState(dsm_dev, buf[i]); + dsm_dev->state.receive_timer = 0; + } - /* We never need a yield */ - *need_yield = false; + /* Always signal that we can accept another byte */ + if (headroom) { + *headroom = DSM_FRAME_LENGTH; + } - /* Always indicate that all bytes were consumed */ - return buf_len; + /* We never need a yield */ + *need_yield = false; + + /* Always indicate that all bytes were consumed */ + return buf_len; } /** @@ -352,17 +366,19 @@ static uint16_t PIOS_DSM_RxInCallback(uint32_t context, */ static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)rcvr_id; + struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)rcvr_id; - if (!PIOS_DSM_Validate(dsm_dev)) - return PIOS_RCVR_INVALID; + if (!PIOS_DSM_Validate(dsm_dev)) { + return PIOS_RCVR_INVALID; + } - /* return error if channel is not available */ - if (channel >= PIOS_DSM_NUM_INPUTS) - return PIOS_RCVR_INVALID; + /* return error if channel is not available */ + if (channel >= PIOS_DSM_NUM_INPUTS) { + return PIOS_RCVR_INVALID; + } - /* may also be PIOS_RCVR_TIMEOUT set by other function */ - return dsm_dev->state.channel_data[channel]; + /* may also be PIOS_RCVR_TIMEOUT set by other function */ + return dsm_dev->state.channel_data[channel]; } /** @@ -379,30 +395,31 @@ static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel) */ static void PIOS_DSM_Supervisor(uint32_t dsm_id) { - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id; + struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id; - bool valid = PIOS_DSM_Validate(dsm_dev); - PIOS_Assert(valid); + bool valid = PIOS_DSM_Validate(dsm_dev); - struct pios_dsm_state *state = &(dsm_dev->state); + PIOS_Assert(valid); - /* waiting for new frame if no bytes were received in 8ms */ - if (++state->receive_timer > 4) { - state->frame_found = 1; - state->byte_count = 0; - state->receive_timer = 0; - } + struct pios_dsm_state *state = &(dsm_dev->state); - /* activate failsafe if no frames have arrived in 102.4ms */ - if (++state->failsafe_timer > 64) { - PIOS_DSM_ResetChannels(dsm_dev); - state->failsafe_timer = 0; - } + /* waiting for new frame if no bytes were received in 8ms */ + if (++state->receive_timer > 4) { + state->frame_found = 1; + state->byte_count = 0; + state->receive_timer = 0; + } + + /* activate failsafe if no frames have arrived in 102.4ms */ + if (++state->failsafe_timer > 64) { + PIOS_DSM_ResetChannels(dsm_dev); + state->failsafe_timer = 0; + } } #endif /* PIOS_INCLUDE_DSM */ -/** +/** * @} * @} */ diff --git a/flight/pios/stm32f10x/pios_eeprom.c b/flight/pios/stm32f10x/pios_eeprom.c index 8771a522a..75fbced04 100644 --- a/flight/pios/stm32f10x/pios_eeprom.c +++ b/flight/pios/stm32f10x/pios_eeprom.c @@ -1,30 +1,30 @@ /** -****************************************************************************** -* @addtogroup PIOS PIOS Core hardware abstraction layer -* @{ -* @addtogroup PIOS_EEPROM EEPROM reading/writing functions -* @brief PIOS EEPROM reading/writing functions -* @{ -* -* @file pios_eeprom.c -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief COM layer functions -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_EEPROM EEPROM reading/writing functions + * @brief PIOS EEPROM reading/writing functions + * @{ + * + * @file pios_eeprom.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief COM layer functions + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,7 +43,7 @@ static struct pios_eeprom_cfg config; */ void PIOS_EEPROM_Init(const struct pios_eeprom_cfg *cfg) { - config = *cfg; + config = *cfg; } /** @@ -54,71 +54,78 @@ void PIOS_EEPROM_Init(const struct pios_eeprom_cfg *cfg) */ int32_t PIOS_EEPROM_Save(uint8_t *data, uint32_t len) { + // We need to write 32 bit words, so extend the length to be an even multiple of 4 bytes, + // and include 4 bytes for the 32 bit CRC. + uint32_t nwords = (len / 4) + 1 + (len % 4 ? 1 : 0); + uint32_t size = nwords * 4; - // We need to write 32 bit words, so extend the length to be an even multiple of 4 bytes, - // and include 4 bytes for the 32 bit CRC. - uint32_t nwords = (len / 4) + 1 + (len % 4 ? 1 : 0); - uint32_t size = nwords * 4; + // Ensure that the length is not longer than the max size. + if (size > config.max_size) { + return -1; + } - // Ensure that the length is not longer than the max size. - if (size > config.max_size) - return -1; + // Calculate a 32 bit CRC of the data. + uint32_t crc = PIOS_CRC32_updateCRC(0xffffffff, data, len); - // Calculate a 32 bit CRC of the data. - uint32_t crc = PIOS_CRC32_updateCRC(0xffffffff, data, len); + // Unlock the Flash Program Erase controller + FLASH_Unlock(); - // Unlock the Flash Program Erase controller - FLASH_Unlock(); + // See if we have to write the data. + if ((memcmp(data, (uint8_t *)config.base_address, len) == 0) && + (memcmp((uint8_t *)&crc, (uint8_t *)config.base_address + size - 4, 4) == 0)) { + return 0; + } - // See if we have to write the data. - if ((memcmp(data, (uint8_t*)config.base_address, len) == 0) && - (memcmp((uint8_t*)&crc, (uint8_t*)config.base_address + size - 4, 4) == 0)) - return 0; + // TODO: Check that the area isn't already erased - // TODO: Check that the area isn't already erased + // Erase page + FLASH_Status fs = FLASH_ErasePage(config.base_address); + if (fs != FLASH_COMPLETE) { // error + FLASH_Lock(); + return -2; + } - // Erase page - FLASH_Status fs = FLASH_ErasePage(config.base_address); - if (fs != FLASH_COMPLETE) - { // error - FLASH_Lock(); - return -2; - } - - // write 4 bytes at a time into program flash area (emulated EEPROM area) - uint8_t *p1 = data; - uint32_t *p3 = (uint32_t *)config.base_address; - for (uint32_t i = 0; i < size; p3++) - { - uint32_t value = 0; + // write 4 bytes at a time into program flash area (emulated EEPROM area) + uint8_t *p1 = data; + uint32_t *p3 = (uint32_t *)config.base_address; + for (uint32_t i = 0; i < size; p3++) { + uint32_t value = 0; - if (i == (size - 4)) - { - // write the CRC. - value = crc; - i += 4; - } - else - { - if (i < len) value |= (uint32_t)*p1++ << 0; else value |= 0x000000ff; i++; - if (i < len) value |= (uint32_t)*p1++ << 8; else value |= 0x0000ff00; i++; - if (i < len) value |= (uint32_t)*p1++ << 16; else value |= 0x00ff0000; i++; - if (i < len) value |= (uint32_t)*p1++ << 24; else value |= 0xff000000; i++; - } + if (i == (size - 4)) { + // write the CRC. + value = crc; + i += 4; + } else { + if (i < len) { + value |= (uint32_t)*p1++ << 0; + } else { value |= 0x000000ff; } + i++; + if (i < len) { + value |= (uint32_t)*p1++ << 8; + } else { value |= 0x0000ff00; } + i++; + if (i < len) { + value |= (uint32_t)*p1++ << 16; + } else { value |= 0x00ff0000; } + i++; + if (i < len) { + value |= (uint32_t)*p1++ << 24; + } else { value |= 0xff000000; } + i++; + } - // write a 32-bit value - fs = FLASH_ProgramWord((uint32_t)p3, value); - if (fs != FLASH_COMPLETE) - { - FLASH_Lock(); - return -3; - } - } - - // Lock the Flash Program Erase controller - FLASH_Lock(); + // write a 32-bit value + fs = FLASH_ProgramWord((uint32_t)p3, value); + if (fs != FLASH_COMPLETE) { + FLASH_Lock(); + return -3; + } + } - return 0; + // Lock the Flash Program Erase controller + FLASH_Lock(); + + return 0; } /** @@ -129,28 +136,29 @@ int32_t PIOS_EEPROM_Save(uint8_t *data, uint32_t len) */ int32_t PIOS_EEPROM_Load(uint8_t *data, uint32_t len) { + // We need to write 32 bit words, so the length should have been extended + // to an even multiple of 4 bytes, and should include 4 bytes for the 32 bit CRC. + uint32_t nwords = (len / 4) + 1 + (len % 4 ? 1 : 0); + uint32_t size = nwords * 4; - // We need to write 32 bit words, so the length should have been extended - // to an even multiple of 4 bytes, and should include 4 bytes for the 32 bit CRC. - uint32_t nwords = (len / 4) + 1 + (len % 4 ? 1 : 0); - uint32_t size = nwords * 4; + // Ensure that the length is not longer than the max size. + if (size > config.max_size) { + return -1; + } - // Ensure that the length is not longer than the max size. - if (size > config.max_size) - return -1; + // Read the data from flash. + memcpy(data, (uint8_t *)config.base_address, len); - // Read the data from flash. - memcpy(data, (uint8_t*)config.base_address, len); + // Read the CRC. + uint32_t crc_flash = *((uint32_t *)(config.base_address + size - 4)); - // Read the CRC. - uint32_t crc_flash = *((uint32_t*)(config.base_address + size - 4)); + // Calculate a 32 bit CRC of the data. + uint32_t crc = PIOS_CRC32_updateCRC(0xffffffff, data, len); + if (crc != crc_flash) { + return -2; + } - // Calculate a 32 bit CRC of the data. - uint32_t crc = PIOS_CRC32_updateCRC(0xffffffff, data, len); - if(crc != crc_flash) - return -2; - - return 0; + return 0; } #endif /* PIOS_INCLUDE_FLASH_EEPROM */ diff --git a/flight/pios/stm32f10x/pios_exti.c b/flight/pios/stm32f10x/pios_exti.c index 9d1980bcc..efdda068e 100644 --- a/flight/pios/stm32f10x/pios_exti.c +++ b/flight/pios/stm32f10x/pios_exti.c @@ -13,19 +13,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,262 +34,298 @@ #ifdef PIOS_INCLUDE_EXTI /* Map EXTI line to full config */ -#define EXTI_MAX_LINES 16 +#define EXTI_MAX_LINES 16 #define PIOS_EXTI_INVALID 0xFF static uint8_t pios_exti_line_to_cfg_map[EXTI_MAX_LINES] = { - [0 ... EXTI_MAX_LINES-1] = PIOS_EXTI_INVALID, + [0 ... EXTI_MAX_LINES - 1] = PIOS_EXTI_INVALID, }; /* Table of exti configs registered at compile time */ extern struct pios_exti_cfg __start__exti __attribute__((weak)); extern struct pios_exti_cfg __stop__exti __attribute__((weak)); -static uint8_t PIOS_EXTI_line_to_index (uint32_t line) +static uint8_t PIOS_EXTI_line_to_index(uint32_t line) { - switch (line) { - case EXTI_Line0: return 0; - case EXTI_Line1: return 1; - case EXTI_Line2: return 2; - case EXTI_Line3: return 3; - case EXTI_Line4: return 4; - case EXTI_Line5: return 5; - case EXTI_Line6: return 6; - case EXTI_Line7: return 7; - case EXTI_Line8: return 8; - case EXTI_Line9: return 9; - case EXTI_Line10: return 10; - case EXTI_Line11: return 11; - case EXTI_Line12: return 12; - case EXTI_Line13: return 13; - case EXTI_Line14: return 14; - case EXTI_Line15: return 15; - } + switch (line) { + case EXTI_Line0: return 0; - PIOS_Assert(0); - return 0xFF; + case EXTI_Line1: return 1; + + case EXTI_Line2: return 2; + + case EXTI_Line3: return 3; + + case EXTI_Line4: return 4; + + case EXTI_Line5: return 5; + + case EXTI_Line6: return 6; + + case EXTI_Line7: return 7; + + case EXTI_Line8: return 8; + + case EXTI_Line9: return 9; + + case EXTI_Line10: return 10; + + case EXTI_Line11: return 11; + + case EXTI_Line12: return 12; + + case EXTI_Line13: return 13; + + case EXTI_Line14: return 14; + + case EXTI_Line15: return 15; + } + + PIOS_Assert(0); + return 0xFF; } -uint8_t PIOS_EXTI_gpio_port_to_exti_source_port(GPIO_TypeDef * gpio_port) +uint8_t PIOS_EXTI_gpio_port_to_exti_source_port(GPIO_TypeDef *gpio_port) { - switch((uint32_t)gpio_port) { - case (uint32_t)GPIOA: return (GPIO_PortSourceGPIOA); - case (uint32_t)GPIOB: return (GPIO_PortSourceGPIOB); - case (uint32_t)GPIOC: return (GPIO_PortSourceGPIOC); - case (uint32_t)GPIOD: return (GPIO_PortSourceGPIOD); - case (uint32_t)GPIOE: return (GPIO_PortSourceGPIOE); - case (uint32_t)GPIOF: return (GPIO_PortSourceGPIOF); - case (uint32_t)GPIOG: return (GPIO_PortSourceGPIOG); - } + switch ((uint32_t)gpio_port) { + case (uint32_t)GPIOA: return GPIO_PortSourceGPIOA; - PIOS_Assert(0); - return 0xFF; + case (uint32_t)GPIOB: return GPIO_PortSourceGPIOB; + + case (uint32_t)GPIOC: return GPIO_PortSourceGPIOC; + + case (uint32_t)GPIOD: return GPIO_PortSourceGPIOD; + + case (uint32_t)GPIOE: return GPIO_PortSourceGPIOE; + + case (uint32_t)GPIOF: return GPIO_PortSourceGPIOF; + + case (uint32_t)GPIOG: return GPIO_PortSourceGPIOG; + } + + PIOS_Assert(0); + return 0xFF; } uint8_t PIOS_EXTI_gpio_pin_to_exti_source_pin(uint32_t gpio_pin) { - switch((uint32_t)gpio_pin) { - case GPIO_Pin_0: return (GPIO_PinSource0); - case GPIO_Pin_1: return (GPIO_PinSource1); - case GPIO_Pin_2: return (GPIO_PinSource2); - case GPIO_Pin_3: return (GPIO_PinSource3); - case GPIO_Pin_4: return (GPIO_PinSource4); - case GPIO_Pin_5: return (GPIO_PinSource5); - case GPIO_Pin_6: return (GPIO_PinSource6); - case GPIO_Pin_7: return (GPIO_PinSource7); - case GPIO_Pin_8: return (GPIO_PinSource8); - case GPIO_Pin_9: return (GPIO_PinSource9); - case GPIO_Pin_10: return (GPIO_PinSource10); - case GPIO_Pin_11: return (GPIO_PinSource11); - case GPIO_Pin_12: return (GPIO_PinSource12); - case GPIO_Pin_13: return (GPIO_PinSource13); - case GPIO_Pin_14: return (GPIO_PinSource14); - case GPIO_Pin_15: return (GPIO_PinSource15); - } + switch ((uint32_t)gpio_pin) { + case GPIO_Pin_0: return GPIO_PinSource0; - PIOS_Assert(0); - return 0xFF; + case GPIO_Pin_1: return GPIO_PinSource1; + + case GPIO_Pin_2: return GPIO_PinSource2; + + case GPIO_Pin_3: return GPIO_PinSource3; + + case GPIO_Pin_4: return GPIO_PinSource4; + + case GPIO_Pin_5: return GPIO_PinSource5; + + case GPIO_Pin_6: return GPIO_PinSource6; + + case GPIO_Pin_7: return GPIO_PinSource7; + + case GPIO_Pin_8: return GPIO_PinSource8; + + case GPIO_Pin_9: return GPIO_PinSource9; + + case GPIO_Pin_10: return GPIO_PinSource10; + + case GPIO_Pin_11: return GPIO_PinSource11; + + case GPIO_Pin_12: return GPIO_PinSource12; + + case GPIO_Pin_13: return GPIO_PinSource13; + + case GPIO_Pin_14: return GPIO_PinSource14; + + case GPIO_Pin_15: return GPIO_PinSource15; + } + + PIOS_Assert(0); + return 0xFF; } -int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg) +int32_t PIOS_EXTI_Init(const struct pios_exti_cfg *cfg) { - PIOS_Assert(cfg); - PIOS_Assert(&__start__exti); - PIOS_Assert(cfg >= &__start__exti); - PIOS_Assert(cfg < &__stop__exti); + PIOS_Assert(cfg); + PIOS_Assert(&__start__exti); + PIOS_Assert(cfg >= &__start__exti); + PIOS_Assert(cfg < &__stop__exti); - uint8_t cfg_index = cfg - &__start__exti; + uint8_t cfg_index = cfg - &__start__exti; - /* Connect this config to the requested vector */ - uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line); + /* Connect this config to the requested vector */ + uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line); - if (pios_exti_line_to_cfg_map[line_index] != PIOS_EXTI_INVALID) { - /* Someone else already has this mapped */ - goto out_fail; - } + if (pios_exti_line_to_cfg_map[line_index] != PIOS_EXTI_INVALID) { + /* Someone else already has this mapped */ + goto out_fail; + } - /* Bind the config to the exti line */ - pios_exti_line_to_cfg_map[line_index] = cfg_index; + /* Bind the config to the exti line */ + pios_exti_line_to_cfg_map[line_index] = cfg_index; - /* Initialize the GPIO pin */ - GPIO_Init(cfg->pin.gpio, &cfg->pin.init); + /* Initialize the GPIO pin */ + GPIO_Init(cfg->pin.gpio, &cfg->pin.init); - /* Set up the EXTI interrupt source */ - uint8_t exti_source_port = PIOS_EXTI_gpio_port_to_exti_source_port(cfg->pin.gpio); - uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin); - GPIO_EXTILineConfig(exti_source_port, exti_source_pin); - EXTI_Init(&cfg->exti.init); + /* Set up the EXTI interrupt source */ + uint8_t exti_source_port = PIOS_EXTI_gpio_port_to_exti_source_port(cfg->pin.gpio); + uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin); + GPIO_EXTILineConfig(exti_source_port, exti_source_pin); + EXTI_Init(&cfg->exti.init); - /* Enable the interrupt channel */ - NVIC_Init(&cfg->irq.init); + /* Enable the interrupt channel */ + NVIC_Init(&cfg->irq.init); - return 0; + return 0; out_fail: - return -1; + return -1; } static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index) { - uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index]; + uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index]; - PIOS_Assert(&__start__exti); + PIOS_Assert(&__start__exti); - if (cfg_index > NELEMENTS(pios_exti_line_to_cfg_map) || - cfg_index == PIOS_EXTI_INVALID) { - /* Unconfigured interrupt just fired! */ - return false; - } + if (cfg_index > NELEMENTS(pios_exti_line_to_cfg_map) || + cfg_index == PIOS_EXTI_INVALID) { + /* Unconfigured interrupt just fired! */ + return false; + } - struct pios_exti_cfg * cfg = &__start__exti + cfg_index; - return cfg->vector(); + struct pios_exti_cfg *cfg = &__start__exti + cfg_index; + return cfg->vector(); } #ifdef PIOS_INCLUDE_FREERTOS -#define PIOS_EXTI_HANDLE_LINE(line, woken) \ - if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ - EXTI_ClearITPendingBit(EXTI_Line##line); \ - woken = PIOS_EXTI_generic_irq_handler(line) ? pdTRUE : woken; \ - } +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + woken = PIOS_EXTI_generic_irq_handler(line) ? pdTRUE : woken; \ + } #else -#define PIOS_EXTI_HANDLE_LINE(line, woken) \ - if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ - EXTI_ClearITPendingBit(EXTI_Line##line); \ - PIOS_EXTI_generic_irq_handler(line); \ - } +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + PIOS_EXTI_generic_irq_handler(line); \ + } #endif /* Bind Interrupt Handlers */ -static void PIOS_EXTI_0_irq_handler (void) +static void PIOS_EXTI_0_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; // dummy variable + bool xHigherPriorityTaskWoken; // dummy variable #endif - PIOS_EXTI_HANDLE_LINE(0, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(0, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI0_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_0_irq_handler"))); +void EXTI0_IRQHandler(void) __attribute__((alias("PIOS_EXTI_0_irq_handler"))); -static void PIOS_EXTI_1_irq_handler (void) +static void PIOS_EXTI_1_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; // dummy variable + bool xHigherPriorityTaskWoken; // dummy variable #endif - PIOS_EXTI_HANDLE_LINE(1, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(1, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI1_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_1_irq_handler"))); +void EXTI1_IRQHandler(void) __attribute__((alias("PIOS_EXTI_1_irq_handler"))); -static void PIOS_EXTI_2_irq_handler (void) +static void PIOS_EXTI_2_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; // dummy variable + bool xHigherPriorityTaskWoken; // dummy variable #endif - PIOS_EXTI_HANDLE_LINE(2, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(2, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI2_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_2_irq_handler"))); +void EXTI2_IRQHandler(void) __attribute__((alias("PIOS_EXTI_2_irq_handler"))); -static void PIOS_EXTI_3_irq_handler (void) +static void PIOS_EXTI_3_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; // dummy variable + bool xHigherPriorityTaskWoken; // dummy variable #endif - PIOS_EXTI_HANDLE_LINE(3, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(3, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI3_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_3_irq_handler"))); +void EXTI3_IRQHandler(void) __attribute__((alias("PIOS_EXTI_3_irq_handler"))); -static void PIOS_EXTI_4_irq_handler (void) +static void PIOS_EXTI_4_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; // dummy variable + bool xHigherPriorityTaskWoken; // dummy variable #endif - PIOS_EXTI_HANDLE_LINE(4, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(4, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI4_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_4_irq_handler"))); +void EXTI4_IRQHandler(void) __attribute__((alias("PIOS_EXTI_4_irq_handler"))); -static void PIOS_EXTI_9_5_irq_handler (void) +static void PIOS_EXTI_9_5_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; // dummy variable + bool xHigherPriorityTaskWoken; // dummy variable #endif - PIOS_EXTI_HANDLE_LINE(5, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(6, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(7, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(8, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(9, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(5, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(6, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(7, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(8, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(9, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI9_5_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_9_5_irq_handler"))); +void EXTI9_5_IRQHandler(void) __attribute__((alias("PIOS_EXTI_9_5_irq_handler"))); -static void PIOS_EXTI_15_10_irq_handler (void) +static void PIOS_EXTI_15_10_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; // dummy variable + bool xHigherPriorityTaskWoken; // dummy variable #endif - PIOS_EXTI_HANDLE_LINE(10, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(11, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(12, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(13, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(14, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(15, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(10, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(11, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(12, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(13, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(14, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(15, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI15_10_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_15_10_irq_handler"))); +void EXTI15_10_IRQHandler(void) __attribute__((alias("PIOS_EXTI_15_10_irq_handler"))); #endif /* PIOS_INCLUDE_EXTI */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_flash_internal.c b/flight/pios/stm32f10x/pios_flash_internal.c index cbae0e068..2c76a2eb0 100644 --- a/flight/pios/stm32f10x/pios_flash_internal.c +++ b/flight/pios/stm32f10x/pios_flash_internal.c @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_flash_internal.c + * @file pios_flash_internal.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. - * @brief brief goes here. + * @brief brief goes here. * -- * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,18 +39,19 @@ struct device_flash_sector { uint16_t st_sector; }; -static bool PIOS_Flash_Internal_GetSectorInfo(uint32_t address, uint8_t * sector_number, uint32_t *sector_start, uint32_t *sector_size) +static bool PIOS_Flash_Internal_GetSectorInfo(uint32_t address, uint8_t *sector_number, uint32_t *sector_start, uint32_t *sector_size) { uint16_t sector = (address - 0x08000000) / 1024; - if(sector <= 127) { + + if (sector <= 127) { /* address lies within this sector */ *sector_number = sector; *sector_start = sector * 1024 + 0x08000000; *sector_size = 1024; - return (true); + return true; } - return (false); + return false; } enum pios_internal_flash_dev_magic { @@ -62,57 +63,61 @@ struct pios_internal_flash_dev { #if defined(PIOS_INCLUDE_FREERTOS) xSemaphoreHandle transaction_lock; -#endif /* defined(PIOS_INCLUDE_FREERTOS) */ +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ }; -static bool PIOS_Flash_Internal_Validate(struct pios_internal_flash_dev * flash_dev) { - return (flash_dev && (flash_dev->magic == PIOS_INTERNAL_FLASH_DEV_MAGIC)); +static bool PIOS_Flash_Internal_Validate(struct pios_internal_flash_dev *flash_dev) +{ + return flash_dev && (flash_dev->magic == PIOS_INTERNAL_FLASH_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_internal_flash_dev * PIOS_Flash_Internal_alloc(void) +static struct pios_internal_flash_dev *PIOS_Flash_Internal_alloc(void) { - struct pios_internal_flash_dev * flash_dev; + struct pios_internal_flash_dev *flash_dev; - flash_dev = (struct pios_internal_flash_dev *)pvPortMalloc(sizeof(* flash_dev)); - if (!flash_dev) return (NULL); + flash_dev = (struct pios_internal_flash_dev *)pvPortMalloc(sizeof(*flash_dev)); + if (!flash_dev) { + return NULL; + } flash_dev->magic = PIOS_INTERNAL_FLASH_DEV_MAGIC; - return(flash_dev); + return flash_dev; } #else static struct pios_internal_flash_dev pios_internal_flash_devs[PIOS_INTERNAL_FLASH_MAX_DEVS]; static uint8_t pios_internal_flash_num_devs; -static struct pios_internal_flash_dev * PIOS_Flash_Internal_alloc(void) +static struct pios_internal_flash_dev *PIOS_Flash_Internal_alloc(void) { - struct pios_internal_flash_dev * flash_dev; + struct pios_internal_flash_dev *flash_dev; if (pios_internal_flash_num_devs >= PIOS_INTERNAL_FLASH_MAX_DEVS) { - return (NULL); + return NULL; } flash_dev = &pios_internal_flash_devs[pios_internal_flash_num_devs++]; flash_dev->magic = PIOS_INTERNAL_FLASH_DEV_MAGIC; - return (flash_dev); + return flash_dev; } #endif /* defined(PIOS_INCLUDE_FREERTOS) */ -int32_t PIOS_Flash_Internal_Init(uintptr_t * flash_id, __attribute__((unused)) const struct pios_flash_internal_cfg * cfg) +int32_t PIOS_Flash_Internal_Init(uintptr_t *flash_id, __attribute__((unused)) const struct pios_flash_internal_cfg *cfg) { - struct pios_internal_flash_dev * flash_dev; + struct pios_internal_flash_dev *flash_dev; flash_dev = PIOS_Flash_Internal_alloc(); - if (flash_dev == NULL) + if (flash_dev == NULL) { return -1; + } #if defined(PIOS_INCLUDE_FREERTOS) flash_dev->transaction_lock = xSemaphoreCreateMutex(); -#endif /* defined(PIOS_INCLUDE_FREERTOS) */ +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ - *flash_id = (uintptr_t) flash_dev; + *flash_id = (uintptr_t)flash_dev; return 0; } @@ -126,15 +131,17 @@ int32_t PIOS_Flash_Internal_Init(uintptr_t * flash_id, __attribute__((unused)) c static int32_t PIOS_Flash_Internal_StartTransaction(uintptr_t flash_id) { - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; - if (!PIOS_Flash_Internal_Validate(flash_dev)) + if (!PIOS_Flash_Internal_Validate(flash_dev)) { return -1; + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreTake(flash_dev->transaction_lock, portMAX_DELAY) != pdTRUE) + if (xSemaphoreTake(flash_dev->transaction_lock, portMAX_DELAY) != pdTRUE) { return -2; -#endif /* defined(PIOS_INCLUDE_FREERTOS) */ + } +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ /* Unlock the internal flash so we can write to it */ FLASH_Unlock(); @@ -143,15 +150,17 @@ static int32_t PIOS_Flash_Internal_StartTransaction(uintptr_t flash_id) static int32_t PIOS_Flash_Internal_EndTransaction(uintptr_t flash_id) { - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; - if (!PIOS_Flash_Internal_Validate(flash_dev)) + if (!PIOS_Flash_Internal_Validate(flash_dev)) { return -1; + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreGive(flash_dev->transaction_lock) != pdTRUE) + if (xSemaphoreGive(flash_dev->transaction_lock) != pdTRUE) { return -2; -#endif /* defined(PIOS_INCLUDE_FREERTOS) */ + } +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ /* Lock the internal flash again so we can no longer write to it */ FLASH_Lock(); @@ -161,36 +170,39 @@ static int32_t PIOS_Flash_Internal_EndTransaction(uintptr_t flash_id) static int32_t PIOS_Flash_Internal_EraseSector(uintptr_t flash_id, uint32_t addr) { - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; - if (!PIOS_Flash_Internal_Validate(flash_dev)) + if (!PIOS_Flash_Internal_Validate(flash_dev)) { return -1; + } uint8_t sector_number; uint32_t sector_start; uint32_t sector_size; if (!PIOS_Flash_Internal_GetSectorInfo(addr, - §or_number, - §or_start, - §or_size)) { + §or_number, + §or_start, + §or_size)) { /* We're asking for an invalid flash address */ return -2; } - if (FLASH_ErasePage(sector_start) != FLASH_COMPLETE) + if (FLASH_ErasePage(sector_start) != FLASH_COMPLETE) { return -3; + } return 0; } -static int32_t PIOS_Flash_Internal_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len) +static int32_t PIOS_Flash_Internal_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) { PIOS_Assert(data); - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; - if (!PIOS_Flash_Internal_Validate(flash_dev)) + if (!PIOS_Flash_Internal_Validate(flash_dev)) { return -1; + } uint8_t sector_number; uint32_t sector_start; @@ -200,9 +212,9 @@ static int32_t PIOS_Flash_Internal_WriteData(uintptr_t flash_id, uint32_t addr, /* Ensure that the base address is in a valid sector */ if (!PIOS_Flash_Internal_GetSectorInfo(addr, - §or_number, - §or_start, - §or_size)) { + §or_number, + §or_start, + §or_size)) { /* We're asking for an invalid flash address */ return -2; } @@ -218,11 +230,11 @@ static int32_t PIOS_Flash_Internal_WriteData(uintptr_t flash_id, uint32_t addr, uint16_t numberOfhWords = len / 2; uint16_t x = 0; FLASH_Status status; - for ( x = 0; x < numberOfhWords; ++x) { - offset = 2 * x; + for (x = 0; x < numberOfhWords; ++x) { + offset = 2 * x; hword_data = (data[offset + 1] << 8) | data[offset]; - if(hword_data != *(uint16_t *)(temp_addr + offset)) { + if (hword_data != *(uint16_t *)(temp_addr + offset)) { status = FLASH_ProgramHalfWord(temp_addr + offset, hword_data); } else { status = FLASH_COMPLETE; @@ -231,10 +243,10 @@ static int32_t PIOS_Flash_Internal_WriteData(uintptr_t flash_id, uint32_t addr, } uint16_t mod = len % 2; - if(mod == 1) { - offset = 2 * x; + if (mod == 1) { + offset = 2 * x; hword_data = 0xFF00 | data[offset]; - if(hword_data != *(uint16_t *)(temp_addr + offset)) { + if (hword_data != *(uint16_t *)(temp_addr + offset)) { status = FLASH_ProgramHalfWord(temp_addr + offset, hword_data); } else { status = FLASH_COMPLETE; @@ -245,14 +257,15 @@ static int32_t PIOS_Flash_Internal_WriteData(uintptr_t flash_id, uint32_t addr, return 0; } -static int32_t PIOS_Flash_Internal_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len) +static int32_t PIOS_Flash_Internal_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) { PIOS_Assert(data); - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; - if (!PIOS_Flash_Internal_Validate(flash_dev)) + if (!PIOS_Flash_Internal_Validate(flash_dev)) { return -1; + } uint8_t sector_number; uint32_t sector_start; @@ -260,9 +273,9 @@ static int32_t PIOS_Flash_Internal_ReadData(uintptr_t flash_id, uint32_t addr, u /* Ensure that the base address is in a valid sector */ if (!PIOS_Flash_Internal_GetSectorInfo(addr, - §or_number, - §or_start, - §or_size)) { + §or_number, + §or_start, + §or_size)) { /* We're asking for an invalid flash address */ return -2; } @@ -283,9 +296,9 @@ static int32_t PIOS_Flash_Internal_ReadData(uintptr_t flash_id, uint32_t addr, u const struct pios_flash_driver pios_internal_flash_driver = { .start_transaction = PIOS_Flash_Internal_StartTransaction, .end_transaction = PIOS_Flash_Internal_EndTransaction, - .erase_sector = PIOS_Flash_Internal_EraseSector, - .write_data = PIOS_Flash_Internal_WriteData, - .read_data = PIOS_Flash_Internal_ReadData, + .erase_sector = PIOS_Flash_Internal_EraseSector, + .write_data = PIOS_Flash_Internal_WriteData, + .read_data = PIOS_Flash_Internal_ReadData, }; #endif /* PIOS_INCLUDE_FLASH_INTERNAL */ diff --git a/flight/pios/stm32f10x/pios_gpio.c b/flight/pios/stm32f10x/pios_gpio.c index abedc162d..81bdac8f2 100644 --- a/flight/pios/stm32f10x/pios_gpio.c +++ b/flight/pios/stm32f10x/pios_gpio.c @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,61 +40,62 @@ static const uint32_t GPIO_PIN[PIOS_GPIO_NUM] = PIOS_GPIO_PINS; static const uint32_t GPIO_CLK[PIOS_GPIO_NUM] = PIOS_GPIO_CLKS; /** -* Initialises all the GPIO's -*/ + * Initialises all the GPIO's + */ void PIOS_GPIO_Init(void) { - /* Do nothing */ + /* Do nothing */ } /** -* Enable a GPIO Pin -* \param[in] Pin Pin Number -*/ + * Enable a GPIO Pin + * \param[in] Pin Pin Number + */ void PIOS_GPIO_Enable(uint8_t Pin) { - //RCC_APB2PeriphClockCmd(GPIO_CLK[Pin], ENABLE); + // RCC_APB2PeriphClockCmd(GPIO_CLK[Pin], ENABLE); - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_InitStructure.GPIO_Pin = GPIO_PIN[Pin]; - GPIO_Init(GPIO_PORT[Pin], &GPIO_InitStructure); + GPIO_InitTypeDef GPIO_InitStructure; - /* GPIO's Off */ - GPIO_PORT[Pin]->BSRR = GPIO_PIN[Pin]; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_Pin = GPIO_PIN[Pin]; + GPIO_Init(GPIO_PORT[Pin], &GPIO_InitStructure); + + /* GPIO's Off */ + GPIO_PORT[Pin]->BSRR = GPIO_PIN[Pin]; } /** -* Turn on Pin -* \param[in] Pin Pin Number -*/ + * Turn on Pin + * \param[in] Pin Pin Number + */ void PIOS_GPIO_On(uint8_t Pin) { - GPIO_PORT[Pin]->BRR = GPIO_PIN[Pin]; + GPIO_PORT[Pin]->BRR = GPIO_PIN[Pin]; } /** -* Turn off Pin -* \param[in] Pin Pin Number -*/ + * Turn off Pin + * \param[in] Pin Pin Number + */ void PIOS_GPIO_Off(uint8_t Pin) { - GPIO_PORT[Pin]->BSRR = GPIO_PIN[Pin]; + GPIO_PORT[Pin]->BSRR = GPIO_PIN[Pin]; } /** -* Toggle Pin on/off -* \param[in] Pin Pin Number -*/ + * Toggle Pin on/off + * \param[in] Pin Pin Number + */ void PIOS_GPIO_Toggle(uint8_t Pin) { - GPIO_PORT[Pin]->ODR ^= GPIO_PIN[Pin]; + GPIO_PORT[Pin]->ODR ^= GPIO_PIN[Pin]; } #endif /* PIOS_INCLUDE_GPIO */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_i2c.c b/flight/pios/stm32f10x/pios_i2c.c index 054cc1454..4a95b7ee5 100644 --- a/flight/pios/stm32f10x/pios_i2c.c +++ b/flight/pios/stm32f10x/pios_i2c.c @@ -6,25 +6,25 @@ * @brief STM32 Hardware dependent I2C functionality * @{ * - * @file pios_i2c.c + * @file pios_i2c.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief I2C Enable/Disable routines * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,28 +38,28 @@ #include -//#define I2C_HALT_ON_ERRORS +// #define I2C_HALT_ON_ERRORS enum i2c_adapter_event { - I2C_EVENT_BUS_ERROR, - I2C_EVENT_START, - I2C_EVENT_STARTED_MORE_TXN_READ, - I2C_EVENT_STARTED_MORE_TXN_WRITE, - I2C_EVENT_STARTED_LAST_TXN_READ, - I2C_EVENT_STARTED_LAST_TXN_WRITE, - I2C_EVENT_ADDR_SENT_LEN_EQ_0, - I2C_EVENT_ADDR_SENT_LEN_EQ_1, - I2C_EVENT_ADDR_SENT_LEN_EQ_2, - I2C_EVENT_ADDR_SENT_LEN_GT_2, - I2C_EVENT_TRANSFER_DONE_LEN_EQ_0, - I2C_EVENT_TRANSFER_DONE_LEN_EQ_1, - I2C_EVENT_TRANSFER_DONE_LEN_EQ_2, - I2C_EVENT_TRANSFER_DONE_LEN_GT_2, - I2C_EVENT_NACK, - I2C_EVENT_STOPPED, - I2C_EVENT_AUTO, /* FIXME: remove this */ + I2C_EVENT_BUS_ERROR, + I2C_EVENT_START, + I2C_EVENT_STARTED_MORE_TXN_READ, + I2C_EVENT_STARTED_MORE_TXN_WRITE, + I2C_EVENT_STARTED_LAST_TXN_READ, + I2C_EVENT_STARTED_LAST_TXN_WRITE, + I2C_EVENT_ADDR_SENT_LEN_EQ_0, + I2C_EVENT_ADDR_SENT_LEN_EQ_1, + I2C_EVENT_ADDR_SENT_LEN_EQ_2, + I2C_EVENT_ADDR_SENT_LEN_GT_2, + I2C_EVENT_TRANSFER_DONE_LEN_EQ_0, + I2C_EVENT_TRANSFER_DONE_LEN_EQ_1, + I2C_EVENT_TRANSFER_DONE_LEN_EQ_2, + I2C_EVENT_TRANSFER_DONE_LEN_GT_2, + I2C_EVENT_NACK, + I2C_EVENT_STOPPED, + I2C_EVENT_AUTO, /* FIXME: remove this */ - I2C_EVENT_NUM_EVENTS /* Must be last */ + I2C_EVENT_NUM_EVENTS /* Must be last */ }; #if defined(PIOS_I2C_DIAGNOSTICS) @@ -77,11 +77,11 @@ volatile uint8_t i2c_state_history_pointer = 0; volatile enum i2c_adapter_event i2c_state_event_history[I2C_LOG_DEPTH]; volatile uint8_t i2c_state_event_history_pointer; -static uint8_t i2c_fsm_fault_count = 0; +static uint8_t i2c_fsm_fault_count = 0; static uint8_t i2c_bad_event_counter = 0; static uint8_t i2c_error_interrupt_counter = 0; static uint8_t i2c_nack_counter = 0; -static uint8_t i2c_timeout_counter = 0; +static uint8_t i2c_timeout_counter = 0; #endif static void go_fsm_fault(struct pios_i2c_adapter *i2c_adapter); @@ -114,8 +114,8 @@ static void go_w_last_txn_last(struct pios_i2c_adapter *i2c_adapter); static void go_nack(struct pios_i2c_adapter *i2c_adapter); struct i2c_adapter_transition { - void (*entry_fn) (struct pios_i2c_adapter * i2c_adapter); - enum i2c_adapter_state next_state[I2C_EVENT_NUM_EVENTS]; + void (*entry_fn)(struct pios_i2c_adapter *i2c_adapter); + enum i2c_adapter_state next_state[I2C_EVENT_NUM_EVENTS]; }; static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter); @@ -127,613 +127,619 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter); static void i2c_adapter_log_fault(enum pios_i2c_error_type type); static const struct i2c_adapter_transition i2c_adapter_transitions[I2C_STATE_NUM_STATES] = { - [I2C_STATE_FSM_FAULT] = { - .entry_fn = go_fsm_fault, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, - [I2C_STATE_BUS_ERROR] = { - .entry_fn = go_bus_error, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, + [I2C_STATE_FSM_FAULT] = { + .entry_fn = go_fsm_fault, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, + [I2C_STATE_BUS_ERROR] = { + .entry_fn = go_bus_error, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, - [I2C_STATE_STOPPED] = { - .entry_fn = go_stopped, - .next_state = { - [I2C_EVENT_START] = I2C_STATE_STARTING, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_STOPPED] = { + .entry_fn = go_stopped, + .next_state = { + [I2C_EVENT_START] = I2C_STATE_STARTING, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_STOPPING] = { - .entry_fn = go_stopping, - .next_state = { - [I2C_EVENT_STOPPED] = I2C_STATE_STOPPED, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_STOPPING] = { + .entry_fn = go_stopping, + .next_state = { + [I2C_EVENT_STOPPED] = I2C_STATE_STOPPED, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_STARTING] = { - .entry_fn = go_starting, - .next_state = { - [I2C_EVENT_STARTED_MORE_TXN_READ] = I2C_STATE_R_MORE_TXN_ADDR, - [I2C_EVENT_STARTED_MORE_TXN_WRITE] = I2C_STATE_W_MORE_TXN_ADDR, - [I2C_EVENT_STARTED_LAST_TXN_READ] = I2C_STATE_R_LAST_TXN_ADDR, - [I2C_EVENT_STARTED_LAST_TXN_WRITE] = I2C_STATE_W_LAST_TXN_ADDR, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_STARTING] = { + .entry_fn = go_starting, + .next_state = { + [I2C_EVENT_STARTED_MORE_TXN_READ] = I2C_STATE_R_MORE_TXN_ADDR, + [I2C_EVENT_STARTED_MORE_TXN_WRITE] = I2C_STATE_W_MORE_TXN_ADDR, + [I2C_EVENT_STARTED_LAST_TXN_READ] = I2C_STATE_R_LAST_TXN_ADDR, + [I2C_EVENT_STARTED_LAST_TXN_WRITE] = I2C_STATE_W_LAST_TXN_ADDR, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - /* - * Read with restart - */ + /* + * Read with restart + */ - [I2C_STATE_R_MORE_TXN_ADDR] = { - .entry_fn = go_r_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_PRE_ONE, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_ADDR] = { + .entry_fn = go_r_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_PRE_ONE, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_ONE] = { - .entry_fn = go_r_more_txn_pre_one, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_ONE] = { + .entry_fn = go_r_more_txn_pre_one, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_FIRST] = { - .entry_fn = go_r_any_txn_pre_first, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_FIRST] = { + .entry_fn = go_r_any_txn_pre_first, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_MIDDLE] = { - .entry_fn = go_r_any_txn_pre_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_MIDDLE] = { + .entry_fn = go_r_any_txn_pre_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_LAST] = { - .entry_fn = go_r_more_txn_pre_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_LAST] = { + .entry_fn = go_r_more_txn_pre_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_POST_LAST] = { - .entry_fn = go_r_any_txn_post_last, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STARTING, - }, - }, + [I2C_STATE_R_MORE_TXN_POST_LAST] = { + .entry_fn = go_r_any_txn_post_last, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STARTING, + }, + }, - /* - * Read - */ + /* + * Read + */ - [I2C_STATE_R_LAST_TXN_ADDR] = { - .entry_fn = go_r_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_PRE_ONE, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_ADDR] = { + .entry_fn = go_r_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_PRE_ONE, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_ONE] = { - .entry_fn = go_r_last_txn_pre_one, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_ONE] = { + .entry_fn = go_r_last_txn_pre_one, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_FIRST] = { - .entry_fn = go_r_any_txn_pre_first, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_FIRST] = { + .entry_fn = go_r_any_txn_pre_first, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_MIDDLE] = { - .entry_fn = go_r_any_txn_pre_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_MIDDLE] = { + .entry_fn = go_r_any_txn_pre_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_LAST] = { - .entry_fn = go_r_last_txn_pre_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_LAST] = { + .entry_fn = go_r_last_txn_pre_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_POST_LAST] = { - .entry_fn = go_r_any_txn_post_last, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, + [I2C_STATE_R_LAST_TXN_POST_LAST] = { + .entry_fn = go_r_any_txn_post_last, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, - /* - * Write with restart - */ + /* + * Write with restart + */ - [I2C_STATE_W_MORE_TXN_ADDR] = { - .entry_fn = go_w_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_MORE_TXN_ADDR] = { + .entry_fn = go_w_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_MORE_TXN_MIDDLE] = { - .entry_fn = go_w_any_txn_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_MORE_TXN_MIDDLE] = { + .entry_fn = go_w_any_txn_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_MORE_TXN_LAST] = { - .entry_fn = go_w_more_txn_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STARTING, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_MORE_TXN_LAST] = { + .entry_fn = go_w_more_txn_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STARTING, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - /* - * Write - */ + /* + * Write + */ - [I2C_STATE_W_LAST_TXN_ADDR] = { - .entry_fn = go_w_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_LAST_TXN_ADDR] = { + .entry_fn = go_w_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_LAST_TXN_MIDDLE] = { - .entry_fn = go_w_any_txn_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_LAST_TXN_MIDDLE] = { + .entry_fn = go_w_any_txn_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_LAST_TXN_LAST] = { - .entry_fn = go_w_last_txn_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STOPPING, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, - [I2C_STATE_NACK] = { - .entry_fn = go_nack, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, + [I2C_STATE_W_LAST_TXN_LAST] = { + .entry_fn = go_w_last_txn_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STOPPING, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, + [I2C_STATE_NACK] = { + .entry_fn = go_nack, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, }; static void go_fsm_fault(struct pios_i2c_adapter *i2c_adapter) { #if defined(I2C_HALT_ON_ERRORS) - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); #endif - /* Note that this transfer has hit a bus error */ - i2c_adapter->bus_error = true; + /* Note that this transfer has hit a bus error */ + i2c_adapter->bus_error = true; - i2c_adapter_reset_bus(i2c_adapter); - + i2c_adapter_reset_bus(i2c_adapter); } static void go_bus_error(struct pios_i2c_adapter *i2c_adapter) { - /* Note that this transfer has hit a bus error */ - i2c_adapter->bus_error = true; + /* Note that this transfer has hit a bus error */ + i2c_adapter->bus_error = true; - i2c_adapter_reset_bus(i2c_adapter); + i2c_adapter_reset_bus(i2c_adapter); } static void go_stopping(struct pios_i2c_adapter *i2c_adapter) { #ifdef USE_FREERTOS - signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; + signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; #endif - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); #ifdef USE_FREERTOS - if (xSemaphoreGiveFromISR(i2c_adapter->sem_ready, &pxHigherPriorityTaskWoken) != pdTRUE) { + if (xSemaphoreGiveFromISR(i2c_adapter->sem_ready, &pxHigherPriorityTaskWoken) != pdTRUE) { #if defined(I2C_HALT_ON_ERRORS) - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); #endif - } - portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for this? */ + } + portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for this? */ #endif /* USE_FREERTOS */ } static void go_stopped(struct pios_i2c_adapter *i2c_adapter) { - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); } static void go_starting(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]); - i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); + i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]); + i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); - I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); - if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) { - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); - } else { - // For write operations, do not enable the IT_BUF events. - // The current driver does not act when the TX data register is not full, only when the complete byte is sent. - // With the IT_BUF enabled, we constantly get IRQs, See OP-326 - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_ERR, ENABLE); - } + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); + if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) { + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); + } else { + // For write operations, do not enable the IT_BUF events. + // The current driver does not act when the TX data register is not full, only when the complete byte is sent. + // With the IT_BUF enabled, we constantly get IRQs, See OP-326 + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_ERR, ENABLE); + } } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_addr(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ); + PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ); - I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Receiver); + I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Receiver); } static void go_r_more_txn_pre_one(struct pios_i2c_adapter *i2c_adapter) { - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); } static void go_r_last_txn_pre_one(struct pios_i2c_adapter *i2c_adapter) { - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_pre_first(struct pios_i2c_adapter *i2c_adapter) { - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_pre_middle(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } static void go_r_more_txn_pre_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - PIOS_IRQ_Disable(); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); - PIOS_IRQ_Enable(); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + PIOS_IRQ_Disable(); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); + PIOS_IRQ_Enable(); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } static void go_r_last_txn_pre_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - PIOS_IRQ_Disable(); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); - PIOS_IRQ_Enable(); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + PIOS_IRQ_Disable(); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); + PIOS_IRQ_Enable(); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_post_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - /* Move to the next byte */ - i2c_adapter->active_byte++; + /* Move to the next byte */ + i2c_adapter->active_byte++; - /* Move to the next transaction */ - i2c_adapter->active_txn++; + /* Move to the next transaction */ + i2c_adapter->active_txn++; } /* Common to 'more' and 'last' transaction */ static void go_w_any_txn_addr(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_WRITE); + PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_WRITE); - I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Transmitter); + I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Transmitter); } static void go_w_any_txn_middle(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte < i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte < i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); + I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } static void go_w_more_txn_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); + I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); - /* Move to the next byte */ - i2c_adapter->active_byte++; + /* Move to the next byte */ + i2c_adapter->active_byte++; - /* Move to the next transaction */ - i2c_adapter->active_txn++; - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + /* Move to the next transaction */ + i2c_adapter->active_txn++; + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); } static void go_w_last_txn_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_BUF, DISABLE); - I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_BUF, DISABLE); + I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); // SHOULD MOVE THIS INTO A STOPPING STATE AND SET IT ONLY AFTER THE BYTE WAS SENT - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); - /* Move to the next byte */ - i2c_adapter->active_byte++; + /* Move to the next byte */ + i2c_adapter->active_byte++; } -static void go_nack(struct pios_i2c_adapter *i2c_adapter) +static void go_nack(struct pios_i2c_adapter *i2c_adapter) { - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); } static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum i2c_adapter_event event) { - PIOS_IRQ_Disable(); + PIOS_IRQ_Disable(); -#if defined(PIOS_I2C_DIAGNOSTICS) - i2c_state_event_history[i2c_state_event_history_pointer] = event; - i2c_state_event_history_pointer = (i2c_state_event_history_pointer + 1) % I2C_LOG_DEPTH; +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_state_event_history[i2c_state_event_history_pointer] = event; + i2c_state_event_history_pointer = (i2c_state_event_history_pointer + 1) % I2C_LOG_DEPTH; - i2c_state_history[i2c_state_history_pointer] = i2c_adapter->curr_state; - i2c_state_history_pointer = (i2c_state_history_pointer + 1) % I2C_LOG_DEPTH; - - if(i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event] == I2C_STATE_FSM_FAULT) - i2c_adapter_log_fault(PIOS_I2C_ERROR_FSM); -#endif - /* - * Move to the next state - * - * This is done prior to calling the new state's entry function to - * guarantee that the entry function never depends on the previous - * state. This way, it cannot ever know what the previous state was. - */ - i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event]; + i2c_state_history[i2c_state_history_pointer] = i2c_adapter->curr_state; + i2c_state_history_pointer = (i2c_state_history_pointer + 1) % I2C_LOG_DEPTH; - /* Call the entry function (if any) for the next state. */ - if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { - i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); - } + if (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event] == I2C_STATE_FSM_FAULT) { + i2c_adapter_log_fault(PIOS_I2C_ERROR_FSM); + } +#endif + /* + * Move to the next state + * + * This is done prior to calling the new state's entry function to + * guarantee that the entry function never depends on the previous + * state. This way, it cannot ever know what the previous state was. + */ + i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event]; - /* Process any AUTO transitions in the FSM */ - i2c_adapter_process_auto(i2c_adapter); + /* Call the entry function (if any) for the next state. */ + if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { + i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); + } - PIOS_IRQ_Enable(); + /* Process any AUTO transitions in the FSM */ + i2c_adapter_process_auto(i2c_adapter); + + PIOS_IRQ_Enable(); } static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter) { - PIOS_IRQ_Disable(); - while (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]) { - i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]; + PIOS_IRQ_Disable(); + while (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]) { + i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]; - /* Call the entry function (if any) for the next state. */ - if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { - i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); - } - } + /* Call the entry function (if any) for the next state. */ + if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { + i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); + } + } - PIOS_IRQ_Enable(); + PIOS_IRQ_Enable(); } static void i2c_adapter_fsm_init(struct pios_i2c_adapter *i2c_adapter) { - i2c_adapter_reset_bus(i2c_adapter); - i2c_adapter->curr_state = I2C_STATE_STOPPED; + i2c_adapter_reset_bus(i2c_adapter); + i2c_adapter->curr_state = I2C_STATE_STOPPED; } static bool i2c_adapter_wait_for_stopped(struct pios_i2c_adapter *i2c_adapter) { - uint32_t guard; + uint32_t guard; - /* - * Wait for the bus to return to the stopped state. - * This was pulled out of the FSM due to occasional - * failures at this transition which previously resulted - * in spinning on this bit in the ISR forever. - */ + /* + * Wait for the bus to return to the stopped state. + * This was pulled out of the FSM due to occasional + * failures at this transition which previously resulted + * in spinning on this bit in the ISR forever. + */ #define I2C_CR1_STOP_REQUESTED 0x0200 - for (guard = 1000000; /* FIXME: should use the configured bus timeout */ - guard && (i2c_adapter->cfg->regs->CR1 & I2C_CR1_STOP_REQUESTED); guard--) - continue; - if (!guard) { - /* We timed out waiting for the stop condition */ - return false; - } + for (guard = 1000000; /* FIXME: should use the configured bus timeout */ + guard && (i2c_adapter->cfg->regs->CR1 & I2C_CR1_STOP_REQUESTED); guard--) { + continue; + } + if (!guard) { + /* We timed out waiting for the stop condition */ + return false; + } - return true; + return true; } static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) { - /* Reset the I2C block */ - I2C_DeInit(i2c_adapter->cfg->regs); + /* Reset the I2C block */ + I2C_DeInit(i2c_adapter->cfg->regs); - /* Make sure the bus is free by clocking it until any slaves release the bus. */ - GPIO_InitTypeDef scl_gpio_init; - scl_gpio_init = i2c_adapter->cfg->scl.init; - scl_gpio_init.GPIO_Mode = GPIO_Mode_Out_OD; - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - GPIO_Init(i2c_adapter->cfg->scl.gpio, &scl_gpio_init); + /* Make sure the bus is free by clocking it until any slaves release the bus. */ + GPIO_InitTypeDef scl_gpio_init; + scl_gpio_init = i2c_adapter->cfg->scl.init; + scl_gpio_init.GPIO_Mode = GPIO_Mode_Out_OD; + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + GPIO_Init(i2c_adapter->cfg->scl.gpio, &scl_gpio_init); - GPIO_InitTypeDef sda_gpio_init; - sda_gpio_init = i2c_adapter->cfg->sda.init; - sda_gpio_init.GPIO_Mode = GPIO_Mode_Out_OD; - GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - GPIO_Init(i2c_adapter->cfg->sda.gpio, &sda_gpio_init); + GPIO_InitTypeDef sda_gpio_init; + sda_gpio_init = i2c_adapter->cfg->sda.init; + sda_gpio_init.GPIO_Mode = GPIO_Mode_Out_OD; + GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + GPIO_Init(i2c_adapter->cfg->sda.gpio, &sda_gpio_init); - /* Check SDA line to determine if slave is asserting bus and clock out if so, this may */ - /* have to be repeated (due to futher bus errors) but better than clocking 0xFF into an */ - /* ESC */ - //bool sda_hung = GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET; - while(GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET) { - - /* Set clock high and wait for any clock stretching to finish. */ - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) ; - PIOS_DELAY_WaituS(2); - - /* Set clock low */ - GPIO_ResetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - - /* Clock high again */ - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - } - - /* Generate a start then stop condition */ - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); + /* Check SDA line to determine if slave is asserting bus and clock out if so, this may */ + /* have to be repeated (due to futher bus errors) but better than clocking 0xFF into an */ + /* ESC */ + // bool sda_hung = GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET; + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET) { + /* Set clock high and wait for any clock stretching to finish. */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) { + ; + } + PIOS_DELAY_WaituS(2); - /* Set data and clock high and wait for any clock stretching to finish. */ - GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) ; - /* Wait for data to be high */ - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) != Bit_SET) ; + /* Set clock low */ + GPIO_ResetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); - - /* Bus signals are guaranteed to be high (ie. free) after this point */ - /* Initialize the GPIO pins to the peripheral function */ - GPIO_Init(i2c_adapter->cfg->scl.gpio, &(i2c_adapter->cfg->scl.init)); - GPIO_Init(i2c_adapter->cfg->sda.gpio, &(i2c_adapter->cfg->sda.init)); + /* Clock high again */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + } - /* Reset the I2C block */ - I2C_DeInit(i2c_adapter->cfg->regs); + /* Generate a start then stop condition */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); - /* Initialize the I2C block */ - I2C_Init(i2c_adapter->cfg->regs, &(i2c_adapter->cfg->init)); + /* Set data and clock high and wait for any clock stretching to finish. */ + GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) { + ; + } + /* Wait for data to be high */ + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) != Bit_SET) { + ; + } + + + /* Bus signals are guaranteed to be high (ie. free) after this point */ + /* Initialize the GPIO pins to the peripheral function */ + GPIO_Init(i2c_adapter->cfg->scl.gpio, &(i2c_adapter->cfg->scl.init)); + GPIO_Init(i2c_adapter->cfg->sda.gpio, &(i2c_adapter->cfg->sda.init)); + + /* Reset the I2C block */ + I2C_DeInit(i2c_adapter->cfg->regs); + + /* Initialize the I2C block */ + I2C_Init(i2c_adapter->cfg->regs, &(i2c_adapter->cfg->init)); #define I2C_BUSY 0x20 - if (i2c_adapter->cfg->regs->SR2 & I2C_BUSY) { - /* Reset the I2C block */ - I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, ENABLE); - I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, DISABLE); - } + if (i2c_adapter->cfg->regs->SR2 & I2C_BUSY) { + /* Reset the I2C block */ + I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, ENABLE); + I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, DISABLE); + } } #include @@ -741,13 +747,14 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) /* Return true if the FSM is in a terminal state */ static bool i2c_adapter_fsm_terminated(struct pios_i2c_adapter *i2c_adapter) { - switch (i2c_adapter->curr_state) { - case I2C_STATE_STOPPING: - case I2C_STATE_STOPPED: - return (true); - default: - return (false); - } + switch (i2c_adapter->curr_state) { + case I2C_STATE_STOPPING: + case I2C_STATE_STOPPED: + return true; + + default: + return false; + } } /** @@ -758,29 +765,29 @@ static bool i2c_adapter_fsm_terminated(struct pios_i2c_adapter *i2c_adapter) void i2c_adapter_log_fault(enum pios_i2c_error_type type) { #if defined(PIOS_I2C_DIAGNOSTICS) - i2c_adapter_fault_history.type = type; - for(uint8_t i = 0; i < I2C_LOG_DEPTH; i++) { - i2c_adapter_fault_history.evirq[i] = - i2c_evirq_history[(I2C_LOG_DEPTH + i2c_evirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - i2c_adapter_fault_history.erirq[i] = - i2c_erirq_history[(I2C_LOG_DEPTH + i2c_erirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - i2c_adapter_fault_history.event[i] = - i2c_state_event_history[(I2C_LOG_DEPTH + i2c_state_event_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - i2c_adapter_fault_history.state[i] = - i2c_state_history[(I2C_LOG_DEPTH + i2c_state_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - } - switch(type) { - case PIOS_I2C_ERROR_EVENT: - i2c_bad_event_counter++; - break; - case PIOS_I2C_ERROR_FSM: - i2c_fsm_fault_count++; - break; - case PIOS_I2C_ERROR_INTERRUPT: - i2c_error_interrupt_counter++; - break; - } -#endif + i2c_adapter_fault_history.type = type; + for (uint8_t i = 0; i < I2C_LOG_DEPTH; i++) { + i2c_adapter_fault_history.evirq[i] = + i2c_evirq_history[(I2C_LOG_DEPTH + i2c_evirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.erirq[i] = + i2c_erirq_history[(I2C_LOG_DEPTH + i2c_erirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.event[i] = + i2c_state_event_history[(I2C_LOG_DEPTH + i2c_state_event_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.state[i] = + i2c_state_history[(I2C_LOG_DEPTH + i2c_state_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + } + switch (type) { + case PIOS_I2C_ERROR_EVENT: + i2c_bad_event_counter++; + break; + case PIOS_I2C_ERROR_FSM: + i2c_fsm_fault_count++; + break; + case PIOS_I2C_ERROR_INTERRUPT: + i2c_error_interrupt_counter++; + break; + } +#endif /* if defined(PIOS_I2C_DIAGNOSTICS) */ } @@ -788,359 +795,376 @@ void i2c_adapter_log_fault(enum pios_i2c_error_type type) * Logs the last N state transitions and N IRQ events due to * an error condition * \param[out] data address where to copy the pios_i2c_fault_history structure to - * \param[out] counts three uint16 that receive the bad event, fsm, and error irq + * \param[out] counts three uint16 that receive the bad event, fsm, and error irq * counts */ -void PIOS_I2C_GetDiagnostics(struct pios_i2c_fault_history * data, uint8_t * counts) +void PIOS_I2C_GetDiagnostics(struct pios_i2c_fault_history *data, uint8_t *counts) { #if defined(PIOS_I2C_DIAGNOSTICS) - memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); - counts[0] = i2c_bad_event_counter; - counts[1] = i2c_fsm_fault_count; - counts[2] = i2c_error_interrupt_counter; - counts[3] = i2c_nack_counter; - counts[4] = i2c_timeout_counter; + memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); + counts[0] = i2c_bad_event_counter; + counts[1] = i2c_fsm_fault_count; + counts[2] = i2c_error_interrupt_counter; + counts[3] = i2c_nack_counter; + counts[4] = i2c_timeout_counter; #else - struct pios_i2c_fault_history i2c_adapter_fault_history; - i2c_adapter_fault_history.type = PIOS_I2C_ERROR_EVENT; + struct pios_i2c_fault_history i2c_adapter_fault_history; + i2c_adapter_fault_history.type = PIOS_I2C_ERROR_EVENT; - memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); - counts[0] = counts[1] = counts[2] = 0; + memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); + counts[0] = counts[1] = counts[2] = 0; #endif } -static bool PIOS_I2C_validate(struct pios_i2c_adapter * i2c_adapter) +static bool PIOS_I2C_validate(struct pios_i2c_adapter *i2c_adapter) { - return (i2c_adapter->magic == PIOS_I2C_DEV_MAGIC); + return i2c_adapter->magic == PIOS_I2C_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_i2c_adapter * PIOS_I2C_alloc(void) +static struct pios_i2c_adapter *PIOS_I2C_alloc(void) { - struct pios_i2c_adapter * i2c_adapter; + struct pios_i2c_adapter *i2c_adapter; - i2c_adapter = (struct pios_i2c_adapter *)pvPortMalloc(sizeof(*i2c_adapter)); - if (!i2c_adapter) return(NULL); + i2c_adapter = (struct pios_i2c_adapter *)pvPortMalloc(sizeof(*i2c_adapter)); + if (!i2c_adapter) { + return NULL; + } - i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; - return(i2c_adapter); + i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; + return i2c_adapter; } #else static struct pios_i2c_adapter pios_i2c_adapters[PIOS_I2C_MAX_DEVS]; static uint8_t pios_i2c_num_adapters; -static struct pios_i2c_adapter * PIOS_I2C_alloc(void) +static struct pios_i2c_adapter *PIOS_I2C_alloc(void) { - struct pios_i2c_adapter * i2c_adapter; + struct pios_i2c_adapter *i2c_adapter; - if (pios_i2c_num_adapters >= PIOS_I2C_MAX_DEVS) { - return (NULL); - } + if (pios_i2c_num_adapters >= PIOS_I2C_MAX_DEVS) { + return NULL; + } - i2c_adapter = &pios_i2c_adapters[pios_i2c_num_adapters++]; - i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; + i2c_adapter = &pios_i2c_adapters[pios_i2c_num_adapters++]; + i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; - return (i2c_adapter); + return i2c_adapter; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /** -* Initializes IIC driver -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ -int32_t PIOS_I2C_Init(uint32_t * i2c_id, const struct pios_i2c_adapter_cfg * cfg) + * Initializes IIC driver + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ +int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg) { - PIOS_DEBUG_Assert(i2c_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(i2c_id); + PIOS_DEBUG_Assert(cfg); - struct pios_i2c_adapter * i2c_adapter; + struct pios_i2c_adapter *i2c_adapter; - i2c_adapter = (struct pios_i2c_adapter *) PIOS_I2C_alloc(); - if (!i2c_adapter) goto out_fail; + i2c_adapter = (struct pios_i2c_adapter *)PIOS_I2C_alloc(); + if (!i2c_adapter) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - i2c_adapter->cfg = cfg; + /* Bind the configuration to the device instance */ + i2c_adapter->cfg = cfg; #ifdef USE_FREERTOS - /* - * Must be done prior to calling i2c_adapter_fsm_init() - * since the sem_ready mutex is used in the initial state. - */ - vSemaphoreCreateBinary(i2c_adapter->sem_ready); - i2c_adapter->sem_busy = xSemaphoreCreateMutex(); + /* + * Must be done prior to calling i2c_adapter_fsm_init() + * since the sem_ready mutex is used in the initial state. + */ + vSemaphoreCreateBinary(i2c_adapter->sem_ready); + i2c_adapter->sem_busy = xSemaphoreCreateMutex(); #endif // USE_FREERTOS - /* Enable the associated peripheral clock */ - switch ((uint32_t) i2c_adapter->cfg->regs) { - case (uint32_t) I2C1: - /* Enable I2C peripheral clock (APB1 == slow speed) */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); - break; - case (uint32_t) I2C2: - /* Enable I2C peripheral clock (APB1 == slow speed) */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); - break; - } + /* Enable the associated peripheral clock */ + switch ((uint32_t)i2c_adapter->cfg->regs) { + case (uint32_t)I2C1: + /* Enable I2C peripheral clock (APB1 == slow speed) */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); + break; + case (uint32_t)I2C2: + /* Enable I2C peripheral clock (APB1 == slow speed) */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); + break; + } - if (i2c_adapter->cfg->remap) { - GPIO_PinRemapConfig(i2c_adapter->cfg->remap, ENABLE); - } + if (i2c_adapter->cfg->remap) { + GPIO_PinRemapConfig(i2c_adapter->cfg->remap, ENABLE); + } - /* Initialize the state machine */ - i2c_adapter_fsm_init(i2c_adapter); + /* Initialize the state machine */ + i2c_adapter_fsm_init(i2c_adapter); - *i2c_id = (uint32_t)i2c_adapter; + *i2c_id = (uint32_t)i2c_adapter; - /* Configure and enable I2C interrupts */ - NVIC_Init(&(i2c_adapter->cfg->event.init)); - NVIC_Init(&(i2c_adapter->cfg->error.init)); - - /* No error */ - return 0; + /* Configure and enable I2C interrupts */ + NVIC_Init(&(i2c_adapter->cfg->event.init)); + NVIC_Init(&(i2c_adapter->cfg->error.init)); + + /* No error */ + return 0; out_fail: - return(-1); + return -1; } /** * @brief Perform a series of I2C transactions * @returns 0 if success or error code - * @retval -1 for failed transaction + * @retval -1 for failed transaction * @retval -2 for failure to get semaphore */ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns) { - struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_Assert(valid) + bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_DEBUG_Assert(txn_list); - PIOS_DEBUG_Assert(num_txns); + PIOS_Assert(valid) - bool semaphore_success = true; + PIOS_DEBUG_Assert(txn_list); + PIOS_DEBUG_Assert(num_txns); + + bool semaphore_success = true; #ifdef USE_FREERTOS - /* Lock the bus */ - portTickType timeout; - timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; - if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) - return -2; + /* Lock the bus */ + portTickType timeout; + timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; + if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { + return -2; + } #else - uint32_t timeout = 0xfff; - while(i2c_adapter->busy && --timeout); - if(timeout == 0) //timed out - return false; - - PIOS_IRQ_Disable(); - if(i2c_adapter->busy) - return false; - i2c_adapter->busy = 1; - PIOS_IRQ_Enable(); + uint32_t timeout = 0xfff; + while (i2c_adapter->busy && --timeout) { + ; + } + if (timeout == 0) { // timed out + return false; + } + + PIOS_IRQ_Disable(); + if (i2c_adapter->busy) { + return false; + } + i2c_adapter->busy = 1; + PIOS_IRQ_Enable(); #endif /* USE_FREERTOS */ - PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); + PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); - i2c_adapter->first_txn = &txn_list[0]; - i2c_adapter->last_txn = &txn_list[num_txns - 1]; - i2c_adapter->active_txn = i2c_adapter->first_txn; + i2c_adapter->first_txn = &txn_list[0]; + i2c_adapter->last_txn = &txn_list[num_txns - 1]; + i2c_adapter->active_txn = i2c_adapter->first_txn; #ifdef USE_FREERTOS - /* Make sure the done/ready semaphore is consumed before we start */ - semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); + /* Make sure the done/ready semaphore is consumed before we start */ + semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); #endif - i2c_adapter->bus_error = false; - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); + i2c_adapter->bus_error = false; + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); - /* Wait for the transfer to complete */ + /* Wait for the transfer to complete */ #ifdef USE_FREERTOS - semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); - xSemaphoreGive(i2c_adapter->sem_ready); + semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); + xSemaphoreGive(i2c_adapter->sem_ready); #else - PIOS_IRQ_Disable(); - i2c_adapter->busy = 0; - PIOS_IRQ_Enable(); + PIOS_IRQ_Disable(); + i2c_adapter->busy = 0; + PIOS_IRQ_Enable(); #endif /* USE_FREERTOS */ - /* Spin waiting for the transfer to finish */ - while (!i2c_adapter_fsm_terminated(i2c_adapter)) ; + /* Spin waiting for the transfer to finish */ + while (!i2c_adapter_fsm_terminated(i2c_adapter)) { + ; + } - if (i2c_adapter_wait_for_stopped(i2c_adapter)) { - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOPPED); - } else { - i2c_adapter_fsm_init(i2c_adapter); - } + if (i2c_adapter_wait_for_stopped(i2c_adapter)) { + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOPPED); + } else { + i2c_adapter_fsm_init(i2c_adapter); + } #ifdef USE_FREERTOS - /* Unlock the bus */ - xSemaphoreGive(i2c_adapter->sem_busy); - if(!semaphore_success) - i2c_timeout_counter++; + /* Unlock the bus */ + xSemaphoreGive(i2c_adapter->sem_busy); + if (!semaphore_success) { + i2c_timeout_counter++; + } #endif /* USE_FREERTOS */ - return !semaphore_success ? -2 : - i2c_adapter->bus_error ? -1 : - 0; + return !semaphore_success ? -2 : + i2c_adapter->bus_error ? -1 : + 0; } void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id) { - struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_Assert(valid) + bool valid = PIOS_I2C_validate(i2c_adapter); - uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + PIOS_Assert(valid) -#if defined(PIOS_I2C_DIAGNOSTICS) - /* Store event for diagnostics */ - i2c_evirq_history[i2c_evirq_history_pointer] = event; - i2c_evirq_history_pointer = (i2c_evirq_history_pointer + 1) % I2C_LOG_DEPTH; + uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + +#if defined(PIOS_I2C_DIAGNOSTICS) + /* Store event for diagnostics */ + i2c_evirq_history[i2c_evirq_history_pointer] = event; + i2c_evirq_history_pointer = (i2c_evirq_history_pointer + 1) % I2C_LOG_DEPTH; #endif - + #define EVENT_MASK 0x000700FF - event &= EVENT_MASK; - - - switch (event) { /* Mask out all the bits we don't care about */ - case (I2C_EVENT_MASTER_MODE_SELECT | 0x40): - /* Unexplained event: EV5 + RxNE : Extraneous Rx. Probably a late NACK from previous read. */ - /* Clean up the extra Rx until the root cause is identified and just keep going */ - (void)I2C_ReceiveData(i2c_adapter->cfg->regs); - /* Fall through */ - case I2C_EVENT_MASTER_MODE_SELECT: /* EV5 */ - switch (i2c_adapter->active_txn->rw) { - case PIOS_I2C_TXN_READ: - if (i2c_adapter->active_txn == i2c_adapter->last_txn) { - /* Final transaction */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_READ); - } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { - /* More transactions follow */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_READ); - } else { - PIOS_DEBUG_Assert(0); - } - break; - case PIOS_I2C_TXN_WRITE: - if (i2c_adapter->active_txn == i2c_adapter->last_txn) { - /* Final transaction */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_WRITE); - } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { - /* More transactions follow */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_WRITE); - } else { - PIOS_DEBUG_Assert(0); - } - break; - default: - PIOS_DEBUG_Assert(0); - break; - } - break; - case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED: /* EV6 */ - case I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED: /* EV6 */ - switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { - case 0: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_0); - break; - case 1: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_1); - break; - case 2: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_2); - break; - default: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_GT_2); - break; - } - break; - case 0x80: /* TxE only. TRA + MSL + BUSY have been cleared before we got here. */ - /* Ignore */ - { - static volatile bool halt = FALSE; - while (halt) ; - } - break; - case 0: /* This triggers an FSM fault sometimes, but not having it stops things working */ - case 0x40: /* RxNE only. MSL + BUSY have already been cleared by HW. */ - case 0x44: /* RxNE + BTF. MSL + BUSY have already been cleared by HW. */ - case I2C_EVENT_MASTER_BYTE_RECEIVED: /* EV7 */ - case (I2C_EVENT_MASTER_BYTE_RECEIVED | 0x4): /* EV7 + BTF */ - case I2C_EVENT_MASTER_BYTE_TRANSMITTED: /* EV8_2 */ - case 0x84: /* TxE + BTF. EV8_2 but TRA + MSL + BUSY have already been cleared by HW. */ - switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { - case 0: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_0); - break; - case 1: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_1); - break; - case 2: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_2); - break; - default: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_GT_2); - break; - } - break; - case I2C_EVENT_MASTER_BYTE_TRANSMITTING: /* EV8 */ - /* Ignore this event and wait for TRANSMITTED in case we can't keep up */ - goto skip_event; - break; - case 0x30084: /* Occurs between byte tranmistted and master mode selected */ - case 0x30000: /* Need to throw away this spurious event */ - case 0x30403 & EVENT_MASK: /* Detected this after got a NACK, probably stop bit */ - goto skip_event; - break; - default: - i2c_adapter_log_fault(PIOS_I2C_ERROR_EVENT); + event &= EVENT_MASK; + + + switch (event) { /* Mask out all the bits we don't care about */ + case (I2C_EVENT_MASTER_MODE_SELECT | 0x40): + /* Unexplained event: EV5 + RxNE : Extraneous Rx. Probably a late NACK from previous read. */ + /* Clean up the extra Rx until the root cause is identified and just keep going */ + (void)I2C_ReceiveData(i2c_adapter->cfg->regs); + /* Fall through */ + case I2C_EVENT_MASTER_MODE_SELECT: /* EV5 */ + switch (i2c_adapter->active_txn->rw) { + case PIOS_I2C_TXN_READ: + if (i2c_adapter->active_txn == i2c_adapter->last_txn) { + /* Final transaction */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_READ); + } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { + /* More transactions follow */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_READ); + } else { + PIOS_DEBUG_Assert(0); + } + break; + case PIOS_I2C_TXN_WRITE: + if (i2c_adapter->active_txn == i2c_adapter->last_txn) { + /* Final transaction */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_WRITE); + } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { + /* More transactions follow */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_WRITE); + } else { + PIOS_DEBUG_Assert(0); + } + break; + default: + PIOS_DEBUG_Assert(0); + break; + } + break; + case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED: /* EV6 */ + case I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED: /* EV6 */ + switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { + case 0: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_0); + break; + case 1: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_1); + break; + case 2: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_2); + break; + default: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_GT_2); + break; + } + break; + case 0x80: /* TxE only. TRA + MSL + BUSY have been cleared before we got here. */ + /* Ignore */ + { + static volatile bool halt = FALSE; + while (halt) { + ; + } + } + break; + case 0: /* This triggers an FSM fault sometimes, but not having it stops things working */ + case 0x40: /* RxNE only. MSL + BUSY have already been cleared by HW. */ + case 0x44: /* RxNE + BTF. MSL + BUSY have already been cleared by HW. */ + case I2C_EVENT_MASTER_BYTE_RECEIVED: /* EV7 */ + case (I2C_EVENT_MASTER_BYTE_RECEIVED | 0x4): /* EV7 + BTF */ + case I2C_EVENT_MASTER_BYTE_TRANSMITTED: /* EV8_2 */ + case 0x84: /* TxE + BTF. EV8_2 but TRA + MSL + BUSY have already been cleared by HW. */ + switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { + case 0: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_0); + break; + case 1: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_1); + break; + case 2: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_2); + break; + default: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_GT_2); + break; + } + break; + case I2C_EVENT_MASTER_BYTE_TRANSMITTING: /* EV8 */ + /* Ignore this event and wait for TRANSMITTED in case we can't keep up */ + goto skip_event; + break; + case 0x30084: /* Occurs between byte tranmistted and master mode selected */ + case 0x30000: /* Need to throw away this spurious event */ + case 0x30403 & EVENT_MASK: /* Detected this after got a NACK, probably stop bit */ + goto skip_event; + break; + default: + i2c_adapter_log_fault(PIOS_I2C_ERROR_EVENT); #if defined(I2C_HALT_ON_ERRORS) - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); #endif - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); - break; - } + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); + break; + } skip_event: - ; + ; } void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id) { - struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_Assert(valid) + bool valid = PIOS_I2C_validate(i2c_adapter); + + PIOS_Assert(valid) #if defined(PIOS_I2C_DIAGNOSTICS) - uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + + i2c_erirq_history[i2c_erirq_history_pointer] = event; + i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % 5; - i2c_erirq_history[i2c_erirq_history_pointer] = event; - i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % 5; - #endif - if(event & I2C_FLAG_AF) { - i2c_nack_counter++; + if (event & I2C_FLAG_AF) { + i2c_nack_counter++; - I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_AF); + I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_AF); - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_NACK); - } else { /* Mostly bus errors here */ - i2c_adapter_log_fault(PIOS_I2C_ERROR_INTERRUPT); - - /* Fail hard on any errors for now */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); - } + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_NACK); + } else { /* Mostly bus errors here */ + i2c_adapter_log_fault(PIOS_I2C_ERROR_INTERRUPT); + + /* Fail hard on any errors for now */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); + } } #endif /* PIOS_INCLUDE_I2C */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_irq.c b/flight/pios/stm32f10x/pios_irq.c index 77b3a3b7a..3a2a79b5d 100644 --- a/flight/pios/stm32f10x/pios_irq.c +++ b/flight/pios/stm32f10x/pios_irq.c @@ -6,26 +6,26 @@ * @brief STM32 Hardware code to enable and disable interrupts * @{ * - * @file pios_irq.c + * @file pios_irq.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief IRQ Enable/Disable routines * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,55 +43,55 @@ static uint32_t nested_ctr; static uint32_t prev_primask; /** -* Disables all interrupts (nested) -* \return < 0 On errors -*/ + * Disables all interrupts (nested) + * \return < 0 On errors + */ int32_t PIOS_IRQ_Disable(void) { - /* Get current priority if nested level == 0 */ - if (!nested_ctr) { - __asm volatile (" mrs %0, primask\n":"=r" (prev_primask) - ); - } + /* Get current priority if nested level == 0 */ + if (!nested_ctr) { + __asm volatile (" mrs %0, primask\n" : "=r" (prev_primask) + ); + } - /* Disable interrupts */ - __asm volatile (" mov r0, #1 \n" " msr primask, r0\n":::"r0"); + /* Disable interrupts */ + __asm volatile (" mov r0, #1 \n" " msr primask, r0\n" ::: "r0"); - ++nested_ctr; + ++nested_ctr; - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Enables all interrupts (nested) -* \return < 0 on errors -* \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) -*/ + * Enables all interrupts (nested) + * \return < 0 on errors + * \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) + */ int32_t PIOS_IRQ_Enable(void) { - /* Check for nesting error */ - if (nested_ctr == 0) { - /* Nesting error */ - return -1; - } + /* Check for nesting error */ + if (nested_ctr == 0) { + /* Nesting error */ + return -1; + } - /* Decrease nesting level */ - --nested_ctr; + /* Decrease nesting level */ + --nested_ctr; - /* Set back previous priority once nested level reached 0 again */ - if (nested_ctr == 0) { - __asm volatile (" msr primask, %0\n"::"r" (prev_primask) - ); - } + /* Set back previous priority once nested level reached 0 again */ + if (nested_ctr == 0) { + __asm volatile (" msr primask, %0\n" ::"r" (prev_primask) + ); + } - /* No error */ - return 0; + /* No error */ + return 0; } #endif /* PIOS_INCLUDE_IRQ */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_led.c b/flight/pios/stm32f10x/pios_led.c index c9d26da8a..696544189 100644 --- a/flight/pios/stm32f10x/pios_led.c +++ b/flight/pios/stm32f10x/pios_led.c @@ -6,25 +6,25 @@ * @brief STM32 Hardware LED handling code * @{ * - * @file pios_led.c + * @file pios_led.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief LED functions, init, toggle, on & off. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,122 +34,126 @@ #include -static const struct pios_led_cfg * led_cfg; +static const struct pios_led_cfg *led_cfg; /** -* Initialises all the LED's -*/ -int32_t PIOS_LED_Init(const struct pios_led_cfg * cfg) + * Initialises all the LED's + */ +int32_t PIOS_LED_Init(const struct pios_led_cfg *cfg) { - PIOS_Assert(cfg); + PIOS_Assert(cfg); - /* Store away the config in a global used by API functions */ - led_cfg = cfg; + /* Store away the config in a global used by API functions */ + led_cfg = cfg; - for (uint8_t i = 0; i < cfg->num_leds; i++) { - const struct pios_led * led = &(cfg->leds[i]); + for (uint8_t i = 0; i < cfg->num_leds; i++) { + const struct pios_led *led = &(cfg->leds[i]); - /* Enable the peripheral clock for the GPIO */ - switch ((uint32_t)led->pin.gpio) { - case (uint32_t) GPIOA: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); - break; - case (uint32_t) GPIOB: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - break; - case (uint32_t) GPIOC: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); - break; - default: - PIOS_Assert(0); - break; - } + /* Enable the peripheral clock for the GPIO */ + switch ((uint32_t)led->pin.gpio) { + case (uint32_t)GPIOA: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); + break; + case (uint32_t)GPIOB: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + break; + case (uint32_t)GPIOC: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); + break; + default: + PIOS_Assert(0); + break; + } - if (led->remap) { - GPIO_PinRemapConfig(led->remap, ENABLE); - } + if (led->remap) { + GPIO_PinRemapConfig(led->remap, ENABLE); + } - GPIO_Init(led->pin.gpio, &led->pin.init); + GPIO_Init(led->pin.gpio, &led->pin.init); - PIOS_LED_Off(i); - } + PIOS_LED_Off(i); + } - return 0; + return 0; } /** -* Turn on LED -* \param[in] LED LED id -*/ + * Turn on LED + * \param[in] LED LED id + */ void PIOS_LED_On(uint32_t led_id) { - PIOS_Assert(led_cfg); + PIOS_Assert(led_cfg); - if (led_id >= led_cfg->num_leds) { - /* LED index out of range */ - return; - } + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } - const struct pios_led * led = &(led_cfg->leds[led_id]); + const struct pios_led *led = &(led_cfg->leds[led_id]); - if (led->active_high) - GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); - else - GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + if (led->active_high) { + GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } else { + GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } } /** -* Turn off LED -* \param[in] LED LED id -*/ + * Turn off LED + * \param[in] LED LED id + */ void PIOS_LED_Off(uint32_t led_id) { - PIOS_Assert(led_cfg); + PIOS_Assert(led_cfg); - if (led_id >= led_cfg->num_leds) { - /* LED index out of range */ - return; - } + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } - const struct pios_led * led = &(led_cfg->leds[led_id]); + const struct pios_led *led = &(led_cfg->leds[led_id]); - if (led->active_high) - GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); - else - GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + if (led->active_high) { + GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } else { + GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } } /** -* Toggle LED on/off -* \param[in] LED LED id -*/ + * Toggle LED on/off + * \param[in] LED LED id + */ void PIOS_LED_Toggle(uint32_t led_id) { - PIOS_Assert(led_cfg); + PIOS_Assert(led_cfg); - if (led_id >= led_cfg->num_leds) { - /* LED index out of range */ - return; - } + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } - const struct pios_led * led = &(led_cfg->leds[led_id]); + const struct pios_led *led = &(led_cfg->leds[led_id]); - if (GPIO_ReadOutputDataBit(led->pin.gpio, led->pin.init.GPIO_Pin) == Bit_SET) { - if (led->active_high) - PIOS_LED_Off(led_id); - else - PIOS_LED_On(led_id); - } else { - if (led->active_high) - PIOS_LED_On(led_id); - else - PIOS_LED_Off(led_id); - } + if (GPIO_ReadOutputDataBit(led->pin.gpio, led->pin.init.GPIO_Pin) == Bit_SET) { + if (led->active_high) { + PIOS_LED_Off(led_id); + } else { + PIOS_LED_On(led_id); + } + } else { + if (led->active_high) { + PIOS_LED_On(led_id); + } else { + PIOS_LED_Off(led_id); + } + } } #endif /* PIOS_INCLUDE_LED */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_ppm.c b/flight/pios/stm32f10x/pios_ppm.c index 8933d9a46..39bb2618d 100644 --- a/flight/pios/stm32f10x/pios_ppm.c +++ b/flight/pios/stm32f10x/pios_ppm.c @@ -39,161 +39,164 @@ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_ppm_rcvr_driver = { - .read = PIOS_PPM_Get, + .read = PIOS_PPM_Get, }; -#define PIOS_PPM_IN_MIN_NUM_CHANNELS 4 -#define PIOS_PPM_IN_MAX_NUM_CHANNELS PIOS_PPM_NUM_INPUTS -#define PIOS_PPM_STABLE_CHANNEL_COUNT 25 // frames -#define PIOS_PPM_IN_MIN_SYNC_PULSE_US 3000 // microseconds -#define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds -#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds +#define PIOS_PPM_IN_MIN_NUM_CHANNELS 4 +#define PIOS_PPM_IN_MAX_NUM_CHANNELS PIOS_PPM_NUM_INPUTS +#define PIOS_PPM_STABLE_CHANNEL_COUNT 25 // frames +#define PIOS_PPM_IN_MIN_SYNC_PULSE_US 3000 // microseconds +#define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds +#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds /* Local Variables */ -//static TIM_ICInitTypeDef TIM_ICInitStructure; +// static TIM_ICInitTypeDef TIM_ICInitStructure; static void PIOS_PPM_Supervisor(uint32_t ppm_id); enum pios_ppm_dev_magic { - PIOS_PPM_DEV_MAGIC = 0xee014d8b, + PIOS_PPM_DEV_MAGIC = 0xee014d8b, }; struct pios_ppm_dev { - enum pios_ppm_dev_magic magic; - const struct pios_ppm_cfg * cfg; + enum pios_ppm_dev_magic magic; + const struct pios_ppm_cfg *cfg; - uint8_t PulseIndex; - uint32_t PreviousTime; - uint32_t CurrentTime; - uint32_t DeltaTime; - uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS]; - uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS]; - uint32_t LargeCounter; - int8_t NumChannels; - int8_t NumChannelsPrevFrame; - uint8_t NumChannelCounter; + uint8_t PulseIndex; + uint32_t PreviousTime; + uint32_t CurrentTime; + uint32_t DeltaTime; + uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS]; + uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS]; + uint32_t LargeCounter; + int8_t NumChannels; + int8_t NumChannelsPrevFrame; + uint8_t NumChannelCounter; - uint8_t supv_timer; - bool Tracking; - bool Fresh; + uint8_t supv_timer; + bool Tracking; + bool Fresh; }; -static bool PIOS_PPM_validate(struct pios_ppm_dev * ppm_dev) +static bool PIOS_PPM_validate(struct pios_ppm_dev *ppm_dev) { - return (ppm_dev->magic == PIOS_PPM_DEV_MAGIC); + return ppm_dev->magic == PIOS_PPM_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_ppm_dev * PIOS_PPM_alloc(void) +static struct pios_ppm_dev *PIOS_PPM_alloc(void) { - struct pios_ppm_dev * ppm_dev; + struct pios_ppm_dev *ppm_dev; - ppm_dev = (struct pios_ppm_dev *)pvPortMalloc(sizeof(*ppm_dev)); - if (!ppm_dev) return(NULL); + ppm_dev = (struct pios_ppm_dev *)pvPortMalloc(sizeof(*ppm_dev)); + if (!ppm_dev) { + return NULL; + } - ppm_dev->magic = PIOS_PPM_DEV_MAGIC; - return(ppm_dev); + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; + return ppm_dev; } #else static struct pios_ppm_dev pios_ppm_devs[PIOS_PPM_MAX_DEVS]; static uint8_t pios_ppm_num_devs; -static struct pios_ppm_dev * PIOS_PPM_alloc(void) +static struct pios_ppm_dev *PIOS_PPM_alloc(void) { - struct pios_ppm_dev * ppm_dev; + struct pios_ppm_dev *ppm_dev; - if (pios_ppm_num_devs >= PIOS_PPM_MAX_DEVS) { - return (NULL); - } + if (pios_ppm_num_devs >= PIOS_PPM_MAX_DEVS) { + return NULL; + } - ppm_dev = &pios_ppm_devs[pios_ppm_num_devs++]; - ppm_dev->magic = PIOS_PPM_DEV_MAGIC; + ppm_dev = &pios_ppm_devs[pios_ppm_num_devs++]; + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; - return (ppm_dev); + return ppm_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void PIOS_PPM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); -static void PIOS_PPM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PPM_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PPM_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); static const struct pios_tim_callbacks tim_callbacks = { - .overflow = PIOS_PPM_tim_overflow_cb, - .edge = PIOS_PPM_tim_edge_cb, + .overflow = PIOS_PPM_tim_overflow_cb, + .edge = PIOS_PPM_tim_edge_cb, }; -extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg) +extern int32_t PIOS_PPM_Init(uint32_t *ppm_id, const struct pios_ppm_cfg *cfg) { - PIOS_DEBUG_Assert(ppm_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(ppm_id); + PIOS_DEBUG_Assert(cfg); - struct pios_ppm_dev * ppm_dev; + struct pios_ppm_dev *ppm_dev; - ppm_dev = (struct pios_ppm_dev *) PIOS_PPM_alloc(); - if (!ppm_dev) goto out_fail; + ppm_dev = (struct pios_ppm_dev *)PIOS_PPM_alloc(); + if (!ppm_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - ppm_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + ppm_dev->cfg = cfg; - /* Set up the state variables */ - ppm_dev->PulseIndex = 0; - ppm_dev->PreviousTime = 0; - ppm_dev->CurrentTime = 0; - ppm_dev->DeltaTime = 0; - ppm_dev->LargeCounter = 0; - ppm_dev->NumChannels = -1; - ppm_dev->NumChannelsPrevFrame = -1; - ppm_dev->NumChannelCounter = 0; - ppm_dev->Tracking = FALSE; - ppm_dev->Fresh = FALSE; + /* Set up the state variables */ + ppm_dev->PulseIndex = 0; + ppm_dev->PreviousTime = 0; + ppm_dev->CurrentTime = 0; + ppm_dev->DeltaTime = 0; + ppm_dev->LargeCounter = 0; + ppm_dev->NumChannels = -1; + ppm_dev->NumChannelsPrevFrame = -1; + ppm_dev->NumChannelCounter = 0; + ppm_dev->Tracking = FALSE; + ppm_dev->Fresh = FALSE; - for (uint8_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { - /* Flush counter variables */ - ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + for (uint8_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + /* Flush counter variables */ + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } - } + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)ppm_dev)) { + return -1; + } - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)ppm_dev)) { - return -1; - } + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; - TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + /* Configure the channels to be in capture/compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; - /* Configure the channels to be in capture/compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel * chan = &cfg->channels[i]; + /* Configure timer for input capture */ + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); - /* Configure timer for input capture */ - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); + /* Enable the Capture Compare Interrupt Request */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE); + break; + } + } - /* Enable the Capture Compare Interrupt Request */ - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE); - break; - case TIM_Channel_2: - TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE); - break; - case TIM_Channel_3: - TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE); - break; - case TIM_Channel_4: - TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE); - break; - } - } + ppm_dev->supv_timer = 0; + if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) { + PIOS_DEBUG_Assert(0); + } - ppm_dev->supv_timer = 0; - if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) { - PIOS_DEBUG_Assert(0); - } + *ppm_id = (uint32_t)ppm_dev; - *ppm_id = (uint32_t)ppm_dev; - - return(0); + return 0; out_fail: - return(-1); + return -1; } /** @@ -205,157 +208,154 @@ out_fail: */ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)rcvr_id; + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)rcvr_id; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return PIOS_RCVR_INVALID; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return PIOS_RCVR_INVALID; + } - if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { - /* Channel out of range */ - return PIOS_RCVR_INVALID; - } - return ppm_dev->CaptureValue[channel]; + if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* Channel out of range */ + return PIOS_RCVR_INVALID; + } + return ppm_dev->CaptureValue[channel]; } -static void PIOS_PPM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id, - uint32_t context, - __attribute__((unused)) uint8_t channel, - uint16_t count) +static void PIOS_PPM_tim_overflow_cb(__attribute__((unused)) uint32_t tim_id, + uint32_t context, + __attribute__((unused)) uint8_t channel, + uint16_t count) { - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context; + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)context; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } - ppm_dev->LargeCounter += count; - - return; + ppm_dev->LargeCounter += count; } -static void PIOS_PPM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, - uint32_t context, - uint8_t chan_idx, - uint16_t count) +static void PIOS_PPM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, + uint32_t context, + uint8_t chan_idx, + uint16_t count) { - /* Recover our device context */ - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context; + /* Recover our device context */ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)context; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } - if (chan_idx >= ppm_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (chan_idx >= ppm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - /* Shift the last measurement out */ - ppm_dev->PreviousTime = ppm_dev->CurrentTime; + /* Shift the last measurement out */ + ppm_dev->PreviousTime = ppm_dev->CurrentTime; - /* Grab the new count */ - ppm_dev->CurrentTime = count; + /* Grab the new count */ + ppm_dev->CurrentTime = count; - /* Convert to 32-bit timer result */ - ppm_dev->CurrentTime += ppm_dev->LargeCounter; + /* Convert to 32-bit timer result */ + ppm_dev->CurrentTime += ppm_dev->LargeCounter; - /* Capture computation */ - ppm_dev->DeltaTime = ppm_dev->CurrentTime - ppm_dev->PreviousTime; + /* Capture computation */ + ppm_dev->DeltaTime = ppm_dev->CurrentTime - ppm_dev->PreviousTime; - ppm_dev->PreviousTime = ppm_dev->CurrentTime; + ppm_dev->PreviousTime = ppm_dev->CurrentTime; - /* Sync pulse detection */ - if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) { - if (ppm_dev->PulseIndex == ppm_dev->NumChannelsPrevFrame - && ppm_dev->PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS - && ppm_dev->PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS) - { - /* If we see n simultaneous frames of the same - number of channels we save it as our frame size */ - if (ppm_dev->NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT) - ppm_dev->NumChannelCounter++; - else - ppm_dev->NumChannels = ppm_dev->PulseIndex; - } else { - ppm_dev->NumChannelCounter = 0; - } + /* Sync pulse detection */ + if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) { + if (ppm_dev->PulseIndex == ppm_dev->NumChannelsPrevFrame + && ppm_dev->PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS + && ppm_dev->PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* If we see n simultaneous frames of the same + number of channels we save it as our frame size */ + if (ppm_dev->NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT) { + ppm_dev->NumChannelCounter++; + } else { + ppm_dev->NumChannels = ppm_dev->PulseIndex; + } + } else { + ppm_dev->NumChannelCounter = 0; + } - /* Check if the last frame was well formed */ - if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) { - /* The last frame was well formed */ - for (int32_t i = 0; i < ppm_dev->NumChannels; i++) { - ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i]; - } - for (uint32_t i = ppm_dev->NumChannels; - i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { - ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - } - } + /* Check if the last frame was well formed */ + if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) { + /* The last frame was well formed */ + for (int32_t i = 0; i < ppm_dev->NumChannels; i++) { + ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i]; + } + for (uint32_t i = ppm_dev->NumChannels; + i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } + } - ppm_dev->Fresh = TRUE; - ppm_dev->Tracking = TRUE; - ppm_dev->NumChannelsPrevFrame = ppm_dev->PulseIndex; - ppm_dev->PulseIndex = 0; + ppm_dev->Fresh = TRUE; + ppm_dev->Tracking = TRUE; + ppm_dev->NumChannelsPrevFrame = ppm_dev->PulseIndex; + ppm_dev->PulseIndex = 0; - /* We rely on the supervisor to set CaptureValue to invalid - if no valid frame is found otherwise we ride over it */ - - } else if (ppm_dev->Tracking) { - /* Valid pulse duration 0.75 to 2.5 ms*/ - if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US - && ppm_dev->DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US - && ppm_dev->PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) { - - ppm_dev->CaptureValueNewFrame[ppm_dev->PulseIndex] = ppm_dev->DeltaTime; - ppm_dev->PulseIndex++; - } else { - /* Not a valid pulse duration */ - ppm_dev->Tracking = FALSE; - for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { - ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; - } - } - } + /* We rely on the supervisor to set CaptureValue to invalid + if no valid frame is found otherwise we ride over it */ + } else if (ppm_dev->Tracking) { + /* Valid pulse duration 0.75 to 2.5 ms*/ + if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US + && ppm_dev->DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US + && ppm_dev->PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) { + ppm_dev->CaptureValueNewFrame[ppm_dev->PulseIndex] = ppm_dev->DeltaTime; + ppm_dev->PulseIndex++; + } else { + /* Not a valid pulse duration */ + ppm_dev->Tracking = FALSE; + for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } + } + } } -static void PIOS_PPM_Supervisor(uint32_t ppm_id) { - /* Recover our device context */ - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)ppm_id; +static void PIOS_PPM_Supervisor(uint32_t ppm_id) +{ + /* Recover our device context */ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)ppm_id; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } - /* - * RTC runs at 625Hz so divide down the base rate so - * that this loop runs at 25Hz. - */ - if(++(ppm_dev->supv_timer) < 25) { - return; - } - ppm_dev->supv_timer = 0; + /* + * RTC runs at 625Hz so divide down the base rate so + * that this loop runs at 25Hz. + */ + if (++(ppm_dev->supv_timer) < 25) { + return; + } + ppm_dev->supv_timer = 0; - if (!ppm_dev->Fresh) { - ppm_dev->Tracking = FALSE; + if (!ppm_dev->Fresh) { + ppm_dev->Tracking = FALSE; - for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { - ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; - } - } + for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } + } - ppm_dev->Fresh = FALSE; + ppm_dev->Fresh = FALSE; } #endif /* PIOS_INCLUDE_PPM */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_ppm_out.c b/flight/pios/stm32f10x/pios_ppm_out.c index 1352d38d2..d8e545079 100644 --- a/flight/pios/stm32f10x/pios_ppm_out.c +++ b/flight/pios/stm32f10x/pios_ppm_out.c @@ -34,30 +34,30 @@ #include "pios_ppm_out_priv.h" -#define PIOS_PPM_OUT_MAX_DEVS 1 -#define PIOS_PPM_OUT_MAX_CHANNELS 8 -#define PIOS_PPM_OUT_FRAME_PERIOD_US 22500 // microseconds -#define PIOS_PPM_OUT_HIGH_PULSE_US 400 // microseconds -#define PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US 1000 // microseconds -#define PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US 2000 // microseconds +#define PIOS_PPM_OUT_MAX_DEVS 1 +#define PIOS_PPM_OUT_MAX_CHANNELS 8 +#define PIOS_PPM_OUT_FRAME_PERIOD_US 22500 // microseconds +#define PIOS_PPM_OUT_HIGH_PULSE_US 400 // microseconds +#define PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US 1000 // microseconds +#define PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US 2000 // microseconds enum pios_ppm_out_dev_magic { - PIOS_PPM_OUT_DEV_MAGIC = 0x0e0210e2 + PIOS_PPM_OUT_DEV_MAGIC = 0x0e0210e2 }; struct pios_ppm_out_dev { - enum pios_ppm_out_dev_magic magic; - const struct pios_ppm_out_cfg * cfg; + enum pios_ppm_out_dev_magic magic; + const struct pios_ppm_out_cfg *cfg; - uint32_t TriggeringPeriod; - uint32_t ChannelSum; - uint8_t NumChannelCounter; - uint16_t ChannelValue[PIOS_PPM_OUT_MAX_CHANNELS]; + uint32_t TriggeringPeriod; + uint32_t ChannelSum; + uint8_t NumChannelCounter; + uint16_t ChannelValue[PIOS_PPM_OUT_MAX_CHANNELS]; - uint8_t SupvTimer; - bool Fresh; - bool Tracking; - bool Enabled; + uint8_t SupvTimer; + bool Fresh; + bool Tracking; + bool Enabled; }; static void PIOS_PPM_Out_Supervisor(uint32_t ppm_id); @@ -65,228 +65,242 @@ static void PIOS_PPM_Out_Enable_Disable(struct pios_ppm_out_dev *ppm_dev, bool e static bool PIOS_PPM_Out_validate(struct pios_ppm_out_dev *ppm_dev) { - return (ppm_dev->magic == PIOS_PPM_OUT_DEV_MAGIC); + return ppm_dev->magic == PIOS_PPM_OUT_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_ppm_out_dev * PIOS_PPM_OUT_alloc(void) +static struct pios_ppm_out_dev *PIOS_PPM_OUT_alloc(void) { - struct pios_ppm_out_dev * ppm_dev; + struct pios_ppm_out_dev *ppm_dev; - ppm_dev = (struct pios_ppm_out_dev *)pvPortMalloc(sizeof(*ppm_dev)); - if (!ppm_dev) return(NULL); + ppm_dev = (struct pios_ppm_out_dev *)pvPortMalloc(sizeof(*ppm_dev)); + if (!ppm_dev) { + return NULL; + } - ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC; - return(ppm_dev); + ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC; + return ppm_dev; } #else static struct pios_ppm_out_dev pios_ppm_out_devs[PIOS_PPM_OUT_MAX_DEVS]; static uint8_t pios_ppm_out_num_devs; -static struct pios_ppm_out_dev * PIOS_PPM_alloc(void) +static struct pios_ppm_out_dev *PIOS_PPM_alloc(void) { - struct pios_ppm_out_dev * ppm_dev; + struct pios_ppm_out_dev *ppm_dev; - if (pios_ppm_out_num_devs >= PIOS_PPM_OUT_MAX_DEVS) { - return (NULL); - } + if (pios_ppm_out_num_devs >= PIOS_PPM_OUT_MAX_DEVS) { + return NULL; + } - ppm_dev = &pios_ppm_out_devs[pios_ppm_out_num_devs++]; - ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC; + ppm_dev = &pios_ppm_out_devs[pios_ppm_out_num_devs++]; + ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC; - return (ppm_dev); + return ppm_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void PIOS_PPM_OUT_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count); +static void PIOS_PPM_OUT_tim_edge_cb(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count); static const struct pios_tim_callbacks tim_out_callbacks = { - .overflow = NULL, - .edge = PIOS_PPM_OUT_tim_edge_cb, + .overflow = NULL, + .edge = PIOS_PPM_OUT_tim_edge_cb, }; -int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg * cfg) +int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg *cfg) { - PIOS_DEBUG_Assert(ppm_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(ppm_id); + PIOS_DEBUG_Assert(cfg); - // Allocate the device structure - struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)PIOS_PPM_OUT_alloc(); - if (!ppm_dev) - return -1; - ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC; - *ppm_out_id = (uint32_t)ppm_dev; + // Allocate the device structure + struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)PIOS_PPM_OUT_alloc(); + if (!ppm_dev) { + return -1; + } + ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC; + *ppm_out_id = (uint32_t)ppm_dev; - // Bind the configuration to the device instance - ppm_dev->cfg = cfg; + // Bind the configuration to the device instance + ppm_dev->cfg = cfg; - // Set up the state variables - ppm_dev->TriggeringPeriod = PIOS_PPM_OUT_HIGH_PULSE_US; - ppm_dev->ChannelSum = 0; - ppm_dev->NumChannelCounter = 0; + // Set up the state variables + ppm_dev->TriggeringPeriod = PIOS_PPM_OUT_HIGH_PULSE_US; + ppm_dev->ChannelSum = 0; + ppm_dev->NumChannelCounter = 0; - // Flush counter variables - for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; ++i) - ppm_dev->ChannelValue[i] = 1000; + // Flush counter variables + for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; ++i) { + ppm_dev->ChannelValue[i] = 1000; + } - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channel, 1, &tim_out_callbacks, (uint32_t)ppm_dev)) - return -1; + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channel, 1, &tim_out_callbacks, (uint32_t)ppm_dev)) { + return -1; + } - // Configure the channels to be in output compare mode - const struct pios_tim_channel *chan = cfg->channel; + // Configure the channels to be in output compare mode + const struct pios_tim_channel *chan = cfg->channel; - /* Set up for output compare function */ - switch(chan->timer_chan) { - case TIM_Channel_1: - TIM_OC1Init(chan->timer, &cfg->tim_oc_init); - TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_2: - TIM_OC2Init(chan->timer, &cfg->tim_oc_init); - TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_3: - TIM_OC3Init(chan->timer, &cfg->tim_oc_init); - TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_4: - TIM_OC4Init(chan->timer, &cfg->tim_oc_init); - TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - } + /* Set up for output compare function */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_OC1Init(chan->timer, &cfg->tim_oc_init); + TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(chan->timer, &cfg->tim_oc_init); + TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(chan->timer, &cfg->tim_oc_init); + TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(chan->timer, &cfg->tim_oc_init); + TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + } - TIM_ARRPreloadConfig(chan->timer, ENABLE); - TIM_CtrlPWMOutputs(chan->timer, ENABLE); - TIM_Cmd(chan->timer, ENABLE); + TIM_ARRPreloadConfig(chan->timer, ENABLE); + TIM_CtrlPWMOutputs(chan->timer, ENABLE); + TIM_Cmd(chan->timer, ENABLE); - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1; - TIM_TimeBaseStructure.TIM_Period = ((1000000 / 100) - 1); - TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure); - PIOS_PPM_Out_Enable_Disable(ppm_dev, false); + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1; + TIM_TimeBaseStructure.TIM_Period = ((1000000 / 100) - 1); + TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure); + PIOS_PPM_Out_Enable_Disable(ppm_dev, false); - // Configure the supervisor - ppm_dev->SupvTimer = 0; - ppm_dev->Fresh = FALSE; - ppm_dev->Tracking = FALSE; - ppm_dev->Enabled = FALSE; - if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Out_Supervisor, (uint32_t)ppm_dev)) { - PIOS_DEBUG_Assert(0); - } + // Configure the supervisor + ppm_dev->SupvTimer = 0; + ppm_dev->Fresh = FALSE; + ppm_dev->Tracking = FALSE; + ppm_dev->Enabled = FALSE; + if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Out_Supervisor, (uint32_t)ppm_dev)) { + PIOS_DEBUG_Assert(0); + } - return 0; + return 0; } void PIOS_PPM_OUT_Set(uint32_t ppm_out_id, uint8_t servo, uint16_t position) { - struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)ppm_out_id; - if (!PIOS_PPM_Out_validate(ppm_dev) || (servo >= PIOS_PPM_OUT_MAX_CHANNELS)) - return; + struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)ppm_out_id; - // Don't allow positions that are out of range. - if (position < PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US) - position = PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US; - if (position > PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US) - position = PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US; + if (!PIOS_PPM_Out_validate(ppm_dev) || (servo >= PIOS_PPM_OUT_MAX_CHANNELS)) { + return; + } - // Update the supervisor tracking variables. - ppm_dev->Fresh = TRUE; + // Don't allow positions that are out of range. + if (position < PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US) { + position = PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US; + } + if (position > PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US) { + position = PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US; + } - // Reenable the TIM if it's been turned off. - if (!ppm_dev->Tracking) { - ppm_dev->Tracking = TRUE; - PIOS_PPM_Out_Enable_Disable(ppm_dev, true); - } - - // Update the position - ppm_dev->ChannelValue[servo] = position; + // Update the supervisor tracking variables. + ppm_dev->Fresh = TRUE; + + // Reenable the TIM if it's been turned off. + if (!ppm_dev->Tracking) { + ppm_dev->Tracking = TRUE; + PIOS_PPM_Out_Enable_Disable(ppm_dev, true); + } + + // Update the position + ppm_dev->ChannelValue[servo] = position; } -static void PIOS_PPM_OUT_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, - uint32_t context, - __attribute__((unused)) uint8_t chan_idx, - __attribute__((unused)) uint16_t count) +static void PIOS_PPM_OUT_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, + uint32_t context, + __attribute__((unused)) uint8_t chan_idx, + __attribute__((unused)) uint16_t count) { - struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)context; - if (!PIOS_PPM_Out_validate(ppm_dev)) - return; + struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)context; - // Just return if the device is disabled. - if (!ppm_dev->Enabled) { - return; + if (!PIOS_PPM_Out_validate(ppm_dev)) { + return; + } + + // Just return if the device is disabled. + if (!ppm_dev->Enabled) { + return; + } + + // Turn off the PPM stream if we are no longer receiving update + // Note: This must happen between frames. + if ((ppm_dev->NumChannelCounter == 0) && !ppm_dev->Tracking) { + // Flush counter variables + for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; ++i) { + ppm_dev->ChannelValue[i] = 1000; } + PIOS_PPM_Out_Enable_Disable(ppm_dev, false); + return; + } - // Turn off the PPM stream if we are no longer receiving update - // Note: This must happen between frames. - if ((ppm_dev->NumChannelCounter == 0) && !ppm_dev->Tracking) { - // Flush counter variables - for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; ++i) { - ppm_dev->ChannelValue[i] = 1000; - } - PIOS_PPM_Out_Enable_Disable(ppm_dev, false); - return; - } + // Finish out the frame if we reached the last channel. + uint32_t pulse_width; + if ((ppm_dev->NumChannelCounter >= PIOS_PPM_OUT_MAX_CHANNELS)) { + pulse_width = PIOS_PPM_OUT_FRAME_PERIOD_US - ppm_dev->ChannelSum; + ppm_dev->NumChannelCounter = 0; + ppm_dev->ChannelSum = 0; + } else { + ppm_dev->ChannelSum += (pulse_width = ppm_dev->ChannelValue[ppm_dev->NumChannelCounter++]); + } - // Finish out the frame if we reached the last channel. - uint32_t pulse_width; - if ((ppm_dev->NumChannelCounter >= PIOS_PPM_OUT_MAX_CHANNELS)) { - pulse_width = PIOS_PPM_OUT_FRAME_PERIOD_US - ppm_dev->ChannelSum; - ppm_dev->NumChannelCounter = 0; - ppm_dev->ChannelSum = 0; - } else - ppm_dev->ChannelSum += (pulse_width = ppm_dev->ChannelValue[ppm_dev->NumChannelCounter++]); - - // Initiate the pulse - TIM_SetAutoreload(ppm_dev->cfg->channel->timer, pulse_width - 1); - - return; + // Initiate the pulse + TIM_SetAutoreload(ppm_dev->cfg->channel->timer, pulse_width - 1); } static void PIOS_PPM_Out_Enable_Disable(struct pios_ppm_out_dev *ppm_dev, bool enable) { - const struct pios_tim_channel *chan = ppm_dev->cfg->channel; - uint32_t trig = enable ? ppm_dev->TriggeringPeriod : 0; - FunctionalState state = enable ? ENABLE : DISABLE; - ppm_dev->Enabled = enable; - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, state); - TIM_SetCompare1(chan->timer, trig); - break; - case TIM_Channel_2: - TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, state); - TIM_SetCompare2(chan->timer, trig); - break; - case TIM_Channel_3: - TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, state); - TIM_SetCompare3(chan->timer, trig); - break; - case TIM_Channel_4: - TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, state); - TIM_SetCompare4(chan->timer, trig); - break; - } + const struct pios_tim_channel *chan = ppm_dev->cfg->channel; + uint32_t trig = enable ? ppm_dev->TriggeringPeriod : 0; + FunctionalState state = enable ? ENABLE : DISABLE; + + ppm_dev->Enabled = enable; + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, state); + TIM_SetCompare1(chan->timer, trig); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, state); + TIM_SetCompare2(chan->timer, trig); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, state); + TIM_SetCompare3(chan->timer, trig); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, state); + TIM_SetCompare4(chan->timer, trig); + break; + } } -static void PIOS_PPM_Out_Supervisor(uint32_t ppm_out_id) { - struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)ppm_out_id; - if (!PIOS_PPM_Out_validate(ppm_dev)) - return; +static void PIOS_PPM_Out_Supervisor(uint32_t ppm_out_id) +{ + struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)ppm_out_id; - // RTC runs at 625Hz so divide down the base rate so that this loop runs at 12.5Hz. - if(++(ppm_dev->SupvTimer) < 50) { - return; - } - ppm_dev->SupvTimer = 0; + if (!PIOS_PPM_Out_validate(ppm_dev)) { + return; + } - // Go into failsafe the channel values haven't been refreshed since the last time through. - if (!ppm_dev->Fresh) { - ppm_dev->Tracking = FALSE; - } + // RTC runs at 625Hz so divide down the base rate so that this loop runs at 12.5Hz. + if (++(ppm_dev->SupvTimer) < 50) { + return; + } + ppm_dev->SupvTimer = 0; - // Set Fresh to false to test if channel values are being refreshed. - ppm_dev->Fresh = FALSE; + // Go into failsafe the channel values haven't been refreshed since the last time through. + if (!ppm_dev->Fresh) { + ppm_dev->Tracking = FALSE; + } + + // Set Fresh to false to test if channel values are being refreshed. + ppm_dev->Fresh = FALSE; } #endif /* PIOS_INCLUDE_PPM_OUT */ diff --git a/flight/pios/stm32f10x/pios_pwm.c b/flight/pios/stm32f10x/pios_pwm.c index d93d37ebc..a11a5f121 100644 --- a/flight/pios/stm32f10x/pios_pwm.c +++ b/flight/pios/stm32f10x/pios_pwm.c @@ -39,7 +39,7 @@ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_pwm_rcvr_driver = { - .read = PIOS_PWM_Get, + .read = PIOS_PWM_Get, }; /* Local Variables */ @@ -47,127 +47,130 @@ const struct pios_rcvr_driver pios_pwm_rcvr_driver = { static const uint32_t PWM_SUPERVISOR_TIMEOUT = 100000; enum pios_pwm_dev_magic { - PIOS_PWM_DEV_MAGIC = 0xab30293c, + PIOS_PWM_DEV_MAGIC = 0xab30293c, }; struct pios_pwm_dev { - enum pios_pwm_dev_magic magic; - const struct pios_pwm_cfg * cfg; + enum pios_pwm_dev_magic magic; + const struct pios_pwm_cfg *cfg; - uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; - uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; - uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; - uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; - uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; - uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; + uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; + uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; + uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; + uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; }; -static bool PIOS_PWM_validate(struct pios_pwm_dev * pwm_dev) +static bool PIOS_PWM_validate(struct pios_pwm_dev *pwm_dev) { - return (pwm_dev->magic == PIOS_PWM_DEV_MAGIC); + return pwm_dev->magic == PIOS_PWM_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_pwm_dev * PIOS_PWM_alloc(void) +static struct pios_pwm_dev *PIOS_PWM_alloc(void) { - struct pios_pwm_dev * pwm_dev; + struct pios_pwm_dev *pwm_dev; - pwm_dev = (struct pios_pwm_dev *)pvPortMalloc(sizeof(*pwm_dev)); - if (!pwm_dev) return(NULL); + pwm_dev = (struct pios_pwm_dev *)pvPortMalloc(sizeof(*pwm_dev)); + if (!pwm_dev) { + return NULL; + } - pwm_dev->magic = PIOS_PWM_DEV_MAGIC; - return(pwm_dev); + pwm_dev->magic = PIOS_PWM_DEV_MAGIC; + return pwm_dev; } #else static struct pios_pwm_dev pios_pwm_devs[PIOS_PWM_MAX_DEVS]; static uint8_t pios_pwm_num_devs; -static struct pios_pwm_dev * PIOS_PWM_alloc(void) +static struct pios_pwm_dev *PIOS_PWM_alloc(void) { - struct pios_pwm_dev * pwm_dev; + struct pios_pwm_dev *pwm_dev; - if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) { - return (NULL); - } + if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) { + return NULL; + } - pwm_dev = &pios_pwm_devs[pios_pwm_num_devs++]; - pwm_dev->magic = PIOS_PWM_DEV_MAGIC; + pwm_dev = &pios_pwm_devs[pios_pwm_num_devs++]; + pwm_dev->magic = PIOS_PWM_DEV_MAGIC; - return (pwm_dev); + return pwm_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void PIOS_PWM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); -static void PIOS_PWM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PWM_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PWM_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); static const struct pios_tim_callbacks tim_callbacks = { - .overflow = PIOS_PWM_tim_overflow_cb, - .edge = PIOS_PWM_tim_edge_cb, + .overflow = PIOS_PWM_tim_overflow_cb, + .edge = PIOS_PWM_tim_edge_cb, }; /** -* Initialises all the pins -*/ -int32_t PIOS_PWM_Init(uint32_t * pwm_id, const struct pios_pwm_cfg * cfg) + * Initialises all the pins + */ +int32_t PIOS_PWM_Init(uint32_t *pwm_id, const struct pios_pwm_cfg *cfg) { - PIOS_DEBUG_Assert(pwm_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(pwm_id); + PIOS_DEBUG_Assert(cfg); - struct pios_pwm_dev * pwm_dev; + struct pios_pwm_dev *pwm_dev; - pwm_dev = (struct pios_pwm_dev *) PIOS_PWM_alloc(); - if (!pwm_dev) goto out_fail; + pwm_dev = (struct pios_pwm_dev *)PIOS_PWM_alloc(); + if (!pwm_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - pwm_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + pwm_dev->cfg = cfg; - for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { - /* Flush counter variables */ - pwm_dev->CaptureState[i] = 0; - pwm_dev->RiseValue[i] = 0; - pwm_dev->FallValue[i] = 0; - pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - } + for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { + /* Flush counter variables */ + pwm_dev->CaptureState[i] = 0; + pwm_dev->RiseValue[i] = 0; + pwm_dev->FallValue[i] = 0; + pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)pwm_dev)) { - return -1; - } + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)pwm_dev)) { + return -1; + } - /* Configure the channels to be in capture/compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel * chan = &cfg->channels[i]; + /* Configure the channels to be in capture/compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; - /* Configure timer for input capture */ - TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - - /* Enable the Capture Compare Interrupt Request */ - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); - break; - case TIM_Channel_2: - TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); - break; - case TIM_Channel_3: - TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); - break; - case TIM_Channel_4: - TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); - break; - } + /* Configure timer for input capture */ + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); - // Need the update event for that timer to detect timeouts - TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); + /* Enable the Capture Compare Interrupt Request */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); + break; + } - } + // Need the update event for that timer to detect timeouts + TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); + } - *pwm_id = (uint32_t) pwm_dev; + *pwm_id = (uint32_t)pwm_dev; - return (0); + return 0; out_fail: - return (-1); + return -1; } /** @@ -179,106 +182,103 @@ out_fail: */ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)rcvr_id; + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)rcvr_id; - if (!PIOS_PWM_validate(pwm_dev)) { - /* Invalid device specified */ - return PIOS_RCVR_INVALID; - } + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return PIOS_RCVR_INVALID; + } - if (channel >= PIOS_PWM_NUM_INPUTS) { - /* Channel out of range */ - return PIOS_RCVR_INVALID; - } - return pwm_dev->CaptureValue[channel]; + if (channel >= PIOS_PWM_NUM_INPUTS) { + /* Channel out of range */ + return PIOS_RCVR_INVALID; + } + return pwm_dev->CaptureValue[channel]; } -static void PIOS_PWM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) +static void PIOS_PWM_tim_overflow_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) { - struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context; + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)context; - if (!PIOS_PWM_validate(pwm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return; + } - if (channel >= pwm_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (channel >= pwm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - pwm_dev->us_since_update[channel] += count; - if(pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { - pwm_dev->CaptureState[channel] = 0; - pwm_dev->RiseValue[channel] = 0; - pwm_dev->FallValue[channel] = 0; - pwm_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; - pwm_dev->us_since_update[channel] = 0; - } - - return; + pwm_dev->us_since_update[channel] += count; + if (pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { + pwm_dev->CaptureState[channel] = 0; + pwm_dev->RiseValue[channel] = 0; + pwm_dev->FallValue[channel] = 0; + pwm_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; + pwm_dev->us_since_update[channel] = 0; + } } -static void PIOS_PWM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) +static void PIOS_PWM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) { - /* Recover our device context */ - struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context; + /* Recover our device context */ + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)context; - if (!PIOS_PWM_validate(pwm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return; + } - if (chan_idx >= pwm_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (chan_idx >= pwm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - const struct pios_tim_channel * chan = &pwm_dev->cfg->channels[chan_idx]; + const struct pios_tim_channel *chan = &pwm_dev->cfg->channels[chan_idx]; - if (pwm_dev->CaptureState[chan_idx] == 0) { - pwm_dev->RiseValue[chan_idx] = count; - pwm_dev->us_since_update[chan_idx] = 0; - } else { - pwm_dev->FallValue[chan_idx] = count; - } - - // flip state machine and capture value here - /* Simple rise or fall state machine */ - TIM_ICInitTypeDef TIM_ICInitStructure = pwm_dev->cfg->tim_ic_init; - if (pwm_dev->CaptureState[chan_idx] == 0) { - /* Switch states */ - pwm_dev->CaptureState[chan_idx] = 1; + if (pwm_dev->CaptureState[chan_idx] == 0) { + pwm_dev->RiseValue[chan_idx] = count; + pwm_dev->us_since_update[chan_idx] = 0; + } else { + pwm_dev->FallValue[chan_idx] = count; + } - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - } else { - /* Capture computation */ - if (pwm_dev->FallValue[chan_idx] > pwm_dev->RiseValue[chan_idx]) { - pwm_dev->CaptureValue[chan_idx] = (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]); - } else { - pwm_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - pwm_dev->RiseValue[chan_idx]) + pwm_dev->FallValue[chan_idx]); - } + // flip state machine and capture value here + /* Simple rise or fall state machine */ + TIM_ICInitTypeDef TIM_ICInitStructure = pwm_dev->cfg->tim_ic_init; + if (pwm_dev->CaptureState[chan_idx] == 0) { + /* Switch states */ + pwm_dev->CaptureState[chan_idx] = 1; - /* Switch states */ - pwm_dev->CaptureState[chan_idx] = 0; + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } else { + /* Capture computation */ + if (pwm_dev->FallValue[chan_idx] > pwm_dev->RiseValue[chan_idx]) { + pwm_dev->CaptureValue[chan_idx] = (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]); + } else { + pwm_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - pwm_dev->RiseValue[chan_idx]) + pwm_dev->FallValue[chan_idx]); + } - /* Increase supervisor counter */ - pwm_dev->CapCounter[chan_idx]++; + /* Switch states */ + pwm_dev->CaptureState[chan_idx] = 0; - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - } - + /* Increase supervisor counter */ + pwm_dev->CapCounter[chan_idx]++; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } } #endif /* PIOS_INCLUDE_PWM */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_rtc.c b/flight/pios/stm32f10x/pios_rtc.c index 9d8f45b66..dbf89be6a 100644 --- a/flight/pios/stm32f10x/pios_rtc.c +++ b/flight/pios/stm32f10x/pios_rtc.c @@ -39,40 +39,40 @@ #endif struct rtc_callback_entry { - void (*fn)(uint32_t); - uint32_t data; + void (*fn)(uint32_t); + uint32_t data; }; #define PIOS_RTC_MAX_CALLBACKS 3 struct rtc_callback_entry rtc_callback_list[PIOS_RTC_MAX_CALLBACKS]; static uint8_t rtc_callback_next = 0; -void PIOS_RTC_Init(const struct pios_rtc_cfg * cfg) +void PIOS_RTC_Init(const struct pios_rtc_cfg *cfg) { - RCC_APB1PeriphClockCmd(RCC_APB1Periph_BKP | RCC_APB1Periph_PWR, - ENABLE); - PWR_BackupAccessCmd(ENABLE); - - RCC_RTCCLKConfig(cfg->clksrc); - RCC_RTCCLKCmd(ENABLE); - RTC_WaitForLastTask(); - RTC_WaitForSynchro(); - RTC_WaitForLastTask(); + RCC_APB1PeriphClockCmd(RCC_APB1Periph_BKP | RCC_APB1Periph_PWR, + ENABLE); + PWR_BackupAccessCmd(ENABLE); - /* Configure and enable the RTC Second interrupt */ - NVIC_Init(&cfg->irq.init); - RTC_ITConfig( RTC_IT_SEC, ENABLE ); - RTC_WaitForLastTask(); + RCC_RTCCLKConfig(cfg->clksrc); + RCC_RTCCLKCmd(ENABLE); + RTC_WaitForLastTask(); + RTC_WaitForSynchro(); + RTC_WaitForLastTask(); - RTC_SetPrescaler(cfg->prescaler); - RTC_WaitForLastTask(); - RTC_SetCounter(0); - RTC_WaitForLastTask(); + /* Configure and enable the RTC Second interrupt */ + NVIC_Init(&cfg->irq.init); + RTC_ITConfig(RTC_IT_SEC, ENABLE); + RTC_WaitForLastTask(); + + RTC_SetPrescaler(cfg->prescaler); + RTC_WaitForLastTask(); + RTC_SetCounter(0); + RTC_WaitForLastTask(); } uint32_t PIOS_RTC_Counter() { - return RTC_GetCounter(); + return RTC_GetCounter(); } /* FIXME: This shouldn't use hard-coded clock rates, dividers or prescalers. @@ -80,51 +80,51 @@ uint32_t PIOS_RTC_Counter() */ float PIOS_RTC_Rate() { - return (float) (8e6f / 128.0f) / (1 + PIOS_RTC_PRESCALER); + return (float)(8e6f / 128.0f) / (1 + PIOS_RTC_PRESCALER); } -float PIOS_RTC_MsPerTick() +float PIOS_RTC_MsPerTick() { - return 1000.0f / PIOS_RTC_Rate(); + return 1000.0f / PIOS_RTC_Rate(); } /* TODO: This needs a mutex around rtc_callbacks[] */ bool PIOS_RTC_RegisterTickCallback(void (*fn)(uint32_t id), uint32_t data) { - struct rtc_callback_entry * cb; - if (rtc_callback_next >= PIOS_RTC_MAX_CALLBACKS) { - return false; - } + struct rtc_callback_entry *cb; - cb = &rtc_callback_list[rtc_callback_next++]; + if (rtc_callback_next >= PIOS_RTC_MAX_CALLBACKS) { + return false; + } - cb->fn = fn; - cb->data = data; - return true; + cb = &rtc_callback_list[rtc_callback_next++]; + + cb->fn = fn; + cb->data = data; + return true; } -void PIOS_RTC_irq_handler (void) +void PIOS_RTC_irq_handler(void) { - if (RTC_GetITStatus(RTC_IT_SEC)) - { - /* Call all registered callbacks */ - for (uint8_t i = 0; i < rtc_callback_next; i++) { - struct rtc_callback_entry * cb = &rtc_callback_list[i]; - if (cb->fn) { - (cb->fn)(cb->data); - } - } + if (RTC_GetITStatus(RTC_IT_SEC)) { + /* Call all registered callbacks */ + for (uint8_t i = 0; i < rtc_callback_next; i++) { + struct rtc_callback_entry *cb = &rtc_callback_list[i]; + if (cb->fn) { + (cb->fn)(cb->data); + } + } - /* Wait until last write operation on RTC registers has finished */ - RTC_WaitForLastTask(); - /* Clear the RTC Second interrupt */ - RTC_ClearITPendingBit(RTC_IT_SEC); - } + /* Wait until last write operation on RTC registers has finished */ + RTC_WaitForLastTask(); + /* Clear the RTC Second interrupt */ + RTC_ClearITPendingBit(RTC_IT_SEC); + } } #endif /* PIOS_INCLUDE_RTC */ -/** +/** * @} * @} */ diff --git a/flight/pios/stm32f10x/pios_servo.c b/flight/pios/stm32f10x/pios_servo.c index 5ec4a3550..a0ea16916 100644 --- a/flight/pios/stm32f10x/pios_servo.c +++ b/flight/pios/stm32f10x/pios_servo.c @@ -37,115 +37,117 @@ /* Private Function Prototypes */ -static const struct pios_servo_cfg * servo_cfg; +static const struct pios_servo_cfg *servo_cfg; /** -* Initialise Servos -*/ -int32_t PIOS_Servo_Init(const struct pios_servo_cfg * cfg) + * Initialise Servos + */ +int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg) { - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) { - return -1; - } + uint32_t tim_id; - /* Store away the requested configuration */ - servo_cfg = cfg; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) { + return -1; + } - /* Configure the channels to be in output compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel * chan = &cfg->channels[i]; + /* Store away the requested configuration */ + servo_cfg = cfg; - /* Set up for output compare function */ - switch(chan->timer_chan) { - case TIM_Channel_1: - TIM_OC1Init(chan->timer, &cfg->tim_oc_init); - TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_2: - TIM_OC2Init(chan->timer, &cfg->tim_oc_init); - TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_3: - TIM_OC3Init(chan->timer, &cfg->tim_oc_init); - TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_4: - TIM_OC4Init(chan->timer, &cfg->tim_oc_init); - TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - } + /* Configure the channels to be in output compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; - TIM_ARRPreloadConfig(chan->timer, ENABLE); - TIM_CtrlPWMOutputs(chan->timer, ENABLE); - TIM_Cmd(chan->timer, ENABLE); - } + /* Set up for output compare function */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_OC1Init(chan->timer, &cfg->tim_oc_init); + TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(chan->timer, &cfg->tim_oc_init); + TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(chan->timer, &cfg->tim_oc_init); + TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(chan->timer, &cfg->tim_oc_init); + TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + } - return 0; + TIM_ARRPreloadConfig(chan->timer, ENABLE); + TIM_CtrlPWMOutputs(chan->timer, ENABLE); + TIM_Cmd(chan->timer, ENABLE); + } + + return 0; } /** -* Set the servo update rate (Max 500Hz) -* \param[in] array of rates in Hz -* \param[in] maximum number of banks -*/ -void PIOS_Servo_SetHz(const uint16_t * speeds, uint8_t banks) + * Set the servo update rate (Max 500Hz) + * \param[in] array of rates in Hz + * \param[in] maximum number of banks + */ +void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks) { - if (!servo_cfg) { - return; - } + if (!servo_cfg) { + return; + } - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init; - TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1; + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init; + TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1; - uint8_t set = 0; + uint8_t set = 0; - for(uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) { - bool new = true; - const struct pios_tim_channel * chan = &servo_cfg->channels[i]; + for (uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) { + bool new = true; + const struct pios_tim_channel *chan = &servo_cfg->channels[i]; - /* See if any previous channels use that same timer */ - for(uint8_t j = 0; (j < i) && new; j++) - new &= chan->timer != servo_cfg->channels[j].timer; + /* See if any previous channels use that same timer */ + for (uint8_t j = 0; (j < i) && new; j++) { + new &= chan->timer != servo_cfg->channels[j].timer; + } - if(new) { - TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1); - TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure); - set++; - } - } + if (new) { + TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1); + TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure); + set++; + } + } } /** -* Set servo position -* \param[in] Servo Servo number (0-7) -* \param[in] Position Servo position in microseconds -*/ + * Set servo position + * \param[in] Servo Servo number (0-7) + * \param[in] Position Servo position in microseconds + */ void PIOS_Servo_Set(uint8_t servo, uint16_t position) { - /* Make sure servo exists */ - if (!servo_cfg || servo >= servo_cfg->num_channels) { - return; - } + /* Make sure servo exists */ + if (!servo_cfg || servo >= servo_cfg->num_channels) { + return; + } - /* Update the position */ - const struct pios_tim_channel * chan = &servo_cfg->channels[servo]; - switch(chan->timer_chan) { - case TIM_Channel_1: - TIM_SetCompare1(chan->timer, position); - break; - case TIM_Channel_2: - TIM_SetCompare2(chan->timer, position); - break; - case TIM_Channel_3: - TIM_SetCompare3(chan->timer, position); - break; - case TIM_Channel_4: - TIM_SetCompare4(chan->timer, position); - break; - } + /* Update the position */ + const struct pios_tim_channel *chan = &servo_cfg->channels[servo]; + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_SetCompare1(chan->timer, position); + break; + case TIM_Channel_2: + TIM_SetCompare2(chan->timer, position); + break; + case TIM_Channel_3: + TIM_SetCompare3(chan->timer, position); + break; + case TIM_Channel_4: + TIM_SetCompare4(chan->timer, position); + break; + } } #endif /* PIOS_INCLUDE_SERVO */ diff --git a/flight/pios/stm32f10x/pios_spi.c b/flight/pios/stm32f10x/pios_spi.c index 6b8d2c88e..1c0582458 100644 --- a/flight/pios/stm32f10x/pios_spi.c +++ b/flight/pios/stm32f10x/pios_spi.c @@ -39,195 +39,198 @@ #include -static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev * com_dev) +static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev *com_dev) { - /* Should check device magic here */ - return(true); + /* Should check device magic here */ + return true; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_spi_dev * PIOS_SPI_alloc(void) +static struct pios_spi_dev *PIOS_SPI_alloc(void) { - return (pvPortMalloc(sizeof(struct pios_spi_dev))); + return pvPortMalloc(sizeof(struct pios_spi_dev)); } #else static struct pios_spi_dev pios_spi_devs[PIOS_SPI_MAX_DEVS]; static uint8_t pios_spi_num_devs; -static struct pios_spi_dev * PIOS_SPI_alloc(void) +static struct pios_spi_dev *PIOS_SPI_alloc(void) { - if (pios_spi_num_devs >= PIOS_SPI_MAX_DEVS) { - return (NULL); - } + if (pios_spi_num_devs >= PIOS_SPI_MAX_DEVS) { + return NULL; + } - return (&pios_spi_devs[pios_spi_num_devs++]); + return &pios_spi_devs[pios_spi_num_devs++]; } #endif /** -* Initialises SPI pins -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed + * Initialises SPI pins + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed */ -int32_t PIOS_SPI_Init(uint32_t * spi_id, const struct pios_spi_cfg * cfg) +int32_t PIOS_SPI_Init(uint32_t *spi_id, const struct pios_spi_cfg *cfg) { - uint32_t init_ssel = 0; + uint32_t init_ssel = 0; + + PIOS_Assert(spi_id); + PIOS_Assert(cfg); + + struct pios_spi_dev *spi_dev; + + spi_dev = (struct pios_spi_dev *)PIOS_SPI_alloc(); + if (!spi_dev) { + goto out_fail; + } + + /* Bind the configuration to the device instance */ + spi_dev->cfg = cfg; - PIOS_Assert(spi_id); - PIOS_Assert(cfg); - - struct pios_spi_dev * spi_dev; - - spi_dev = (struct pios_spi_dev *) PIOS_SPI_alloc(); - if (!spi_dev) goto out_fail; - - /* Bind the configuration to the device instance */ - spi_dev->cfg = cfg; - #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(spi_dev->busy); - xSemaphoreGive(spi_dev->busy); + vSemaphoreCreateBinary(spi_dev->busy); + xSemaphoreGive(spi_dev->busy); #else - spi_dev->busy = 0; + spi_dev->busy = 0; #endif - - /* Disable callback function */ - spi_dev->callback = NULL; - - /* Set rx/tx dummy bytes to a known value */ - spi_dev->rx_dummy_byte = 0xFF; - spi_dev->tx_dummy_byte = 0xFF; - - switch (spi_dev->cfg->init.SPI_NSS) { - case SPI_NSS_Soft: - if (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) { - /* We're a master in soft NSS mode, make sure we see NSS high at all times. */ - SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Set); - /* Init as many slave selects as the config advertises. */ - init_ssel = spi_dev->cfg->slave_count; - } else { - /* We're a slave in soft NSS mode, make sure we see NSS low at all times. */ - SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Reset); - } - break; - case SPI_NSS_Hard: - /* only legal for single-slave config */ - PIOS_Assert(spi_dev->cfg->slave_count == 1); - init_ssel = 1; - /* FIXME: Should this also call SPI_SSOutputCmd()? */ - break; - - default: - PIOS_Assert(0); - } - - /* Initialize the GPIO pins */ - GPIO_Init(spi_dev->cfg->sclk.gpio, &(spi_dev->cfg->sclk.init)); - GPIO_Init(spi_dev->cfg->mosi.gpio, &(spi_dev->cfg->mosi.init)); - GPIO_Init(spi_dev->cfg->miso.gpio, &(spi_dev->cfg->miso.init)); - for (uint32_t i = 0; i < init_ssel; i++) { - /* Since we're driving the SSEL pin in software, ensure that the slave is deselected */ - /* XXX multi-slave support - maybe have another SPI_NSS_ mode? */ - GPIO_SetBits(spi_dev->cfg->ssel[i].gpio, spi_dev->cfg->ssel[i].init.GPIO_Pin); - GPIO_Init(spi_dev->cfg->ssel[i].gpio, (GPIO_InitTypeDef*)&(spi_dev->cfg->ssel[i].init)); - } - /* Enable the associated peripheral clock */ - switch ((uint32_t) spi_dev->cfg->regs) { - case (uint32_t) SPI1: - /* Enable SPI peripheral clock (APB2 == high speed) */ - RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); - break; - case (uint32_t) SPI2: - /* Enable SPI peripheral clock (APB1 == slow speed) */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE); - break; - case (uint32_t) SPI3: - /* Enable SPI peripheral clock (APB1 == slow speed) */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI3, ENABLE); - break; - } - - /* Enable DMA clock */ - RCC_AHBPeriphClockCmd(spi_dev->cfg->dma.ahb_clk, ENABLE); - - /* Configure DMA for SPI Rx */ - DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); - DMA_Init(spi_dev->cfg->dma.rx.channel, &(spi_dev->cfg->dma.rx.init)); - - /* Configure DMA for SPI Tx */ - DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); - DMA_Init(spi_dev->cfg->dma.tx.channel, &(spi_dev->cfg->dma.tx.init)); - - /* Initialize the SPI block */ - SPI_Init(spi_dev->cfg->regs, &(spi_dev->cfg->init)); - - /* Configure CRC calculation */ - if (spi_dev->cfg->use_crc) { - SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); - } else { - SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); - } - - /* Enable SPI */ - SPI_Cmd(spi_dev->cfg->regs, ENABLE); - - /* Enable SPI interrupts to DMA */ - SPI_I2S_DMACmd(spi_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); - - /* Configure DMA interrupt */ - NVIC_Init(&(spi_dev->cfg->dma.irq.init)); - - *spi_id = (uint32_t)spi_dev; - return(0); - + /* Disable callback function */ + spi_dev->callback = NULL; + + /* Set rx/tx dummy bytes to a known value */ + spi_dev->rx_dummy_byte = 0xFF; + spi_dev->tx_dummy_byte = 0xFF; + + switch (spi_dev->cfg->init.SPI_NSS) { + case SPI_NSS_Soft: + if (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) { + /* We're a master in soft NSS mode, make sure we see NSS high at all times. */ + SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Set); + /* Init as many slave selects as the config advertises. */ + init_ssel = spi_dev->cfg->slave_count; + } else { + /* We're a slave in soft NSS mode, make sure we see NSS low at all times. */ + SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Reset); + } + break; + case SPI_NSS_Hard: + /* only legal for single-slave config */ + PIOS_Assert(spi_dev->cfg->slave_count == 1); + init_ssel = 1; + /* FIXME: Should this also call SPI_SSOutputCmd()? */ + break; + + default: + PIOS_Assert(0); + } + + /* Initialize the GPIO pins */ + GPIO_Init(spi_dev->cfg->sclk.gpio, &(spi_dev->cfg->sclk.init)); + GPIO_Init(spi_dev->cfg->mosi.gpio, &(spi_dev->cfg->mosi.init)); + GPIO_Init(spi_dev->cfg->miso.gpio, &(spi_dev->cfg->miso.init)); + for (uint32_t i = 0; i < init_ssel; i++) { + /* Since we're driving the SSEL pin in software, ensure that the slave is deselected */ + /* XXX multi-slave support - maybe have another SPI_NSS_ mode? */ + GPIO_SetBits(spi_dev->cfg->ssel[i].gpio, spi_dev->cfg->ssel[i].init.GPIO_Pin); + GPIO_Init(spi_dev->cfg->ssel[i].gpio, (GPIO_InitTypeDef *)&(spi_dev->cfg->ssel[i].init)); + } + + /* Enable the associated peripheral clock */ + switch ((uint32_t)spi_dev->cfg->regs) { + case (uint32_t)SPI1: + /* Enable SPI peripheral clock (APB2 == high speed) */ + RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); + break; + case (uint32_t)SPI2: + /* Enable SPI peripheral clock (APB1 == slow speed) */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE); + break; + case (uint32_t)SPI3: + /* Enable SPI peripheral clock (APB1 == slow speed) */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI3, ENABLE); + break; + } + + /* Enable DMA clock */ + RCC_AHBPeriphClockCmd(spi_dev->cfg->dma.ahb_clk, ENABLE); + + /* Configure DMA for SPI Rx */ + DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); + DMA_Init(spi_dev->cfg->dma.rx.channel, &(spi_dev->cfg->dma.rx.init)); + + /* Configure DMA for SPI Tx */ + DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); + DMA_Init(spi_dev->cfg->dma.tx.channel, &(spi_dev->cfg->dma.tx.init)); + + /* Initialize the SPI block */ + SPI_Init(spi_dev->cfg->regs, &(spi_dev->cfg->init)); + + /* Configure CRC calculation */ + if (spi_dev->cfg->use_crc) { + SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); + } else { + SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); + } + + /* Enable SPI */ + SPI_Cmd(spi_dev->cfg->regs, ENABLE); + + /* Enable SPI interrupts to DMA */ + SPI_I2S_DMACmd(spi_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); + + /* Configure DMA interrupt */ + NVIC_Init(&(spi_dev->cfg->dma.irq.init)); + + *spi_id = (uint32_t)spi_dev; + return 0; + out_fail: - return(-1); + return -1; } /** -* (Re-)initialises SPI peripheral clock rate -* -* \param[in] spi SPI number (0 or 1) -* \param[in] spi_prescaler configures the SPI speed: -*
    -*
  • PIOS_SPI_PRESCALER_2: sets clock rate 27.7~ nS @ 72 MHz (36 MBit/s) (only supported for spi==0, spi1 uses 4 instead) -*
  • PIOS_SPI_PRESCALER_4: sets clock rate 55.5~ nS @ 72 MHz (18 MBit/s) -*
  • PIOS_SPI_PRESCALER_8: sets clock rate 111.1~ nS @ 72 MHz (9 MBit/s) -*
  • PIOS_SPI_PRESCALER_16: sets clock rate 222.2~ nS @ 72 MHz (4.5 MBit/s) -*
  • PIOS_SPI_PRESCALER_32: sets clock rate 444.4~ nS @ 72 MHz (2.25 MBit/s) -*
  • PIOS_SPI_PRESCALER_64: sets clock rate 888.8~ nS @ 72 MHz (1.125 MBit/s) -*
  • PIOS_SPI_PRESCALER_128: sets clock rate 1.7~ nS @ 72 MHz (0.562 MBit/s) -*
  • PIOS_SPI_PRESCALER_256: sets clock rate 3.5~ nS @ 72 MHz (0.281 MBit/s) -*
-* \return 0 if no error -* \return -1 if disabled SPI port selected -* \return -3 if invalid spi_prescaler selected -*/ + * (Re-)initialises SPI peripheral clock rate + * + * \param[in] spi SPI number (0 or 1) + * \param[in] spi_prescaler configures the SPI speed: + *
    + *
  • PIOS_SPI_PRESCALER_2: sets clock rate 27.7~ nS @ 72 MHz (36 MBit/s) (only supported for spi==0, spi1 uses 4 instead) + *
  • PIOS_SPI_PRESCALER_4: sets clock rate 55.5~ nS @ 72 MHz (18 MBit/s) + *
  • PIOS_SPI_PRESCALER_8: sets clock rate 111.1~ nS @ 72 MHz (9 MBit/s) + *
  • PIOS_SPI_PRESCALER_16: sets clock rate 222.2~ nS @ 72 MHz (4.5 MBit/s) + *
  • PIOS_SPI_PRESCALER_32: sets clock rate 444.4~ nS @ 72 MHz (2.25 MBit/s) + *
  • PIOS_SPI_PRESCALER_64: sets clock rate 888.8~ nS @ 72 MHz (1.125 MBit/s) + *
  • PIOS_SPI_PRESCALER_128: sets clock rate 1.7~ nS @ 72 MHz (0.562 MBit/s) + *
  • PIOS_SPI_PRESCALER_256: sets clock rate 3.5~ nS @ 72 MHz (0.281 MBit/s) + *
+ * \return 0 if no error + * \return -1 if disabled SPI port selected + * \return -3 if invalid spi_prescaler selected + */ int32_t PIOS_SPI_SetClockSpeed(uint32_t spi_id, SPIPrescalerTypeDef spi_prescaler) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - SPI_InitTypeDef SPI_InitStructure; + PIOS_Assert(valid) - if (spi_prescaler >= 8) { - /* Invalid prescaler selected */ - return -3; - } + SPI_InitTypeDef SPI_InitStructure; - /* Start with a copy of the default configuration for the peripheral */ - SPI_InitStructure = spi_dev->cfg->init; + if (spi_prescaler >= 8) { + /* Invalid prescaler selected */ + return -3; + } - /* Adjust the prescaler for the peripheral's clock */ - SPI_InitStructure.SPI_BaudRatePrescaler = ((uint16_t) spi_prescaler & 7) << 3; + /* Start with a copy of the default configuration for the peripheral */ + SPI_InitStructure = spi_dev->cfg->init; - /* Write back the new configuration */ - SPI_Init(spi_dev->cfg->regs, &SPI_InitStructure); + /* Adjust the prescaler for the peripheral's clock */ + SPI_InitStructure.SPI_BaudRatePrescaler = ((uint16_t)spi_prescaler & 7) << 3; - PIOS_SPI_TransferByte(spi_id, 0xFF); - return 0; + /* Write back the new configuration */ + SPI_Init(spi_dev->cfg->regs, &SPI_InitStructure); + + PIOS_SPI_TransferByte(spi_id, 0xFF); + return 0; } /** @@ -239,27 +242,32 @@ int32_t PIOS_SPI_SetClockSpeed(uint32_t spi_id, SPIPrescalerTypeDef spi_prescale int32_t PIOS_SPI_ClaimBus(uint32_t spi_id) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) - if (xSemaphoreTake(spi_dev->busy, 0xffff) != pdTRUE) - return -1; + if (xSemaphoreTake(spi_dev->busy, 0xffff) != pdTRUE) { + return -1; + } #else - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - uint32_t timeout = 0xffff; - while((PIOS_SPI_Busy(spi_id) || spi_dev->busy) && --timeout); - if(timeout == 0) //timed out - return -1; - - PIOS_IRQ_Disable(); - if(spi_dev->busy) - return -1; - spi_dev->busy = 1; - PIOS_IRQ_Enable(); -#endif - return 0; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + uint32_t timeout = 0xffff; + while ((PIOS_SPI_Busy(spi_id) || spi_dev->busy) && --timeout) { + ; + } + if (timeout == 0) { // timed out + return -1; + } + + PIOS_IRQ_Disable(); + if (spi_dev->busy) { + return -1; + } + spi_dev->busy = 1; + PIOS_IRQ_Enable(); +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + return 0; } /** @@ -273,24 +281,26 @@ int32_t PIOS_SPI_ClaimBus(uint32_t spi_id) int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) - - if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE){ - return -1; - } - if (woken) { - *woken = *woken || (higherPriorityTaskWoken == pdTRUE); - } - return 0; + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) + + if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE) { + return -1; + } + if (woken) { + *woken = *woken || (higherPriorityTaskWoken == pdTRUE); + } + return 0; + #else - if (woken) { - *woken = false; - } - return PIOS_SPI_ClaimBus(spi_id); + if (woken) { + *woken = false; + } + return PIOS_SPI_ClaimBus(spi_id); + #endif } @@ -302,19 +312,19 @@ int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken) int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) - xSemaphoreGive(spi_dev->busy); + xSemaphoreGive(spi_dev->busy); #else - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - PIOS_IRQ_Disable(); - spi_dev->busy = 0; - PIOS_IRQ_Enable(); + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + PIOS_IRQ_Disable(); + spi_dev->busy = 0; + PIOS_IRQ_Enable(); #endif - return 0; + return 0; } /** @@ -327,22 +337,24 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id) int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; bool valid = PIOS_SPI_validate(spi_dev); PIOS_Assert(valid) xSemaphoreGiveFromISR(spi_dev->busy, &higherPriorityTaskWoken); if (woken) { - *woken = *woken || (higherPriorityTaskWoken == pdTRUE); + *woken = *woken || (higherPriorityTaskWoken == pdTRUE); } - return 0; + return 0; + #else - if (woken) { - *woken = false; - } - return PIOS_SPI_ReleaseBus(spi_id); + if (woken) { + *woken = false; + } + return PIOS_SPI_ReleaseBus(spi_id); + #endif } @@ -355,272 +367,293 @@ int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken) */ int32_t PIOS_SPI_RC_PinSet(uint32_t spi_id, uint32_t slave_id, uint8_t pin_value) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) - PIOS_Assert(slave_id <= spi_dev->cfg->slave_count) - - /* XXX multi-slave support? */ - if (pin_value) { - GPIO_SetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); - } else { - GPIO_ResetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); - } - - return 0; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + + PIOS_Assert(valid) + PIOS_Assert(slave_id <= spi_dev->cfg->slave_count) + + /* XXX multi-slave support? */ + if (pin_value) { + GPIO_SetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); + } else { + GPIO_ResetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); + } + + return 0; } /** -* Transfers a byte to SPI output and reads back the return value from SPI input -* \param[in] spi SPI number (0 or 1) -* \param[in] b the byte which should be transfered -*/ + * Transfers a byte to SPI output and reads back the return value from SPI input + * \param[in] spi SPI number (0 or 1) + * \param[in] b the byte which should be transfered + */ static uint8_t dummy; int32_t PIOS_SPI_TransferByte(uint32_t spi_id, uint8_t b) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - uint8_t rx_byte; + PIOS_Assert(valid) - /* - * Procedure taken from STM32F10xxx Reference Manual section 23.3.5 - */ + uint8_t rx_byte; - /* Make sure the RXNE flag is cleared by reading the DR register */ - dummy = spi_dev->cfg->regs->DR; + /* + * Procedure taken from STM32F10xxx Reference Manual section 23.3.5 + */ - /* Start the transfer */ - spi_dev->cfg->regs->DR = b; + /* Make sure the RXNE flag is cleared by reading the DR register */ + dummy = spi_dev->cfg->regs->DR; - /* Wait until there is a byte to read */ - while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_RXNE)) ; + /* Start the transfer */ + spi_dev->cfg->regs->DR = b; - /* Read the rx'd byte */ - rx_byte = spi_dev->cfg->regs->DR; + /* Wait until there is a byte to read */ + while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_RXNE)) { + ; + } - /* Wait until the TXE goes high */ - while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) ; + /* Read the rx'd byte */ + rx_byte = spi_dev->cfg->regs->DR; - /* Wait for SPI transfer to have fully completed */ - while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) ; + /* Wait until the TXE goes high */ + while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) { + ; + } - /* Return received byte */ - return rx_byte; + /* Wait for SPI transfer to have fully completed */ + while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) { + ; + } + + /* Return received byte */ + return rx_byte; } /** -* Transfers a block of bytes via DMA. -* \param[in] spi SPI number (0 or 1) -* \param[in] send_buffer pointer to buffer which should be sent.
-* If NULL, 0xff (all-one) will be sent. -* \param[in] receive_buffer pointer to buffer which should get the received values.
-* If NULL, received bytes will be discarded. -* \param[in] len number of bytes which should be transfered -* \param[in] callback pointer to callback function which will be executed -* from DMA channel interrupt once the transfer is finished. -* If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will -* block until the transfer is finished. -* \return >= 0 if no error during transfer -* \return -1 if disabled SPI port selected -* \return -3 if function has been called during an ongoing DMA transfer -*/ + * Transfers a block of bytes via DMA. + * \param[in] spi SPI number (0 or 1) + * \param[in] send_buffer pointer to buffer which should be sent.
+ * If NULL, 0xff (all-one) will be sent. + * \param[in] receive_buffer pointer to buffer which should get the received values.
+ * If NULL, received bytes will be discarded. + * \param[in] len number of bytes which should be transfered + * \param[in] callback pointer to callback function which will be executed + * from DMA channel interrupt once the transfer is finished. + * If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will + * block until the transfer is finished. + * \return >= 0 if no error during transfer + * \return -1 if disabled SPI port selected + * \return -3 if function has been called during an ongoing DMA transfer + */ int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - DMA_InitTypeDef dma_init; + PIOS_Assert(valid) - /* Exit if ongoing transfer */ - if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { - return -3; - } + DMA_InitTypeDef dma_init; - /* Disable the SPI peripheral */ - SPI_Cmd(spi_dev->cfg->regs, DISABLE); + /* Exit if ongoing transfer */ + if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { + return -3; + } - /* Disable the DMA channels */ - DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); - DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); + /* Disable the SPI peripheral */ + SPI_Cmd(spi_dev->cfg->regs, DISABLE); - /* Set callback function */ - spi_dev->callback = callback; + /* Disable the DMA channels */ + DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); + DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); - /* - * Configure Rx channel - */ + /* Set callback function */ + spi_dev->callback = callback; - /* Start with the default configuration for this peripheral */ - dma_init = spi_dev->cfg->dma.rx.init; + /* + * Configure Rx channel + */ - if (receive_buffer != NULL) { - /* Enable memory addr. increment - bytes written into receive buffer */ - dma_init.DMA_MemoryBaseAddr = (uint32_t) receive_buffer; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - } else { - /* Disable memory addr. increment - bytes written into dummy buffer */ - spi_dev->rx_dummy_byte = 0xFF; - dma_init.DMA_MemoryBaseAddr = (uint32_t) &spi_dev->rx_dummy_byte; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; - } - if (spi_dev->cfg->use_crc) { - /* Make sure the CRC error flag is cleared before we start */ - SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); - } + /* Start with the default configuration for this peripheral */ + dma_init = spi_dev->cfg->dma.rx.init; - dma_init.DMA_BufferSize = len; - DMA_Init(spi_dev->cfg->dma.rx.channel, &(dma_init)); + if (receive_buffer != NULL) { + /* Enable memory addr. increment - bytes written into receive buffer */ + dma_init.DMA_MemoryBaseAddr = (uint32_t)receive_buffer; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + } else { + /* Disable memory addr. increment - bytes written into dummy buffer */ + spi_dev->rx_dummy_byte = 0xFF; + dma_init.DMA_MemoryBaseAddr = (uint32_t)&spi_dev->rx_dummy_byte; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; + } + if (spi_dev->cfg->use_crc) { + /* Make sure the CRC error flag is cleared before we start */ + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + } - /* - * Configure Tx channel - */ + dma_init.DMA_BufferSize = len; + DMA_Init(spi_dev->cfg->dma.rx.channel, &(dma_init)); - /* Start with the default configuration for this peripheral */ - dma_init = spi_dev->cfg->dma.tx.init; + /* + * Configure Tx channel + */ - if (send_buffer != NULL) { - /* Enable memory addr. increment - bytes written into receive buffer */ - dma_init.DMA_MemoryBaseAddr = (uint32_t) send_buffer; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - } else { - /* Disable memory addr. increment - bytes written into dummy buffer */ - spi_dev->tx_dummy_byte = 0xFF; - dma_init.DMA_MemoryBaseAddr = (uint32_t) &spi_dev->tx_dummy_byte; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; - } + /* Start with the default configuration for this peripheral */ + dma_init = spi_dev->cfg->dma.tx.init; - if (spi_dev->cfg->use_crc) { - /* The last byte of the payload will be replaced with the CRC8 */ - dma_init.DMA_BufferSize = len - 1; - } else { - dma_init.DMA_BufferSize = len; - } + if (send_buffer != NULL) { + /* Enable memory addr. increment - bytes written into receive buffer */ + dma_init.DMA_MemoryBaseAddr = (uint32_t)send_buffer; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + } else { + /* Disable memory addr. increment - bytes written into dummy buffer */ + spi_dev->tx_dummy_byte = 0xFF; + dma_init.DMA_MemoryBaseAddr = (uint32_t)&spi_dev->tx_dummy_byte; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; + } - DMA_Init(spi_dev->cfg->dma.tx.channel, &(dma_init)); + if (spi_dev->cfg->use_crc) { + /* The last byte of the payload will be replaced with the CRC8 */ + dma_init.DMA_BufferSize = len - 1; + } else { + dma_init.DMA_BufferSize = len; + } - /* Enable DMA interrupt if callback function active */ - DMA_ITConfig(spi_dev->cfg->dma.rx.channel, DMA_IT_TC, (callback != NULL) ? ENABLE : DISABLE); + DMA_Init(spi_dev->cfg->dma.tx.channel, &(dma_init)); - /* Flush out the CRC registers */ - SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); - (void)SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); - SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + /* Enable DMA interrupt if callback function active */ + DMA_ITConfig(spi_dev->cfg->dma.rx.channel, DMA_IT_TC, (callback != NULL) ? ENABLE : DISABLE); - /* Make sure to flush out the receive buffer */ - (void)SPI_I2S_ReceiveData(spi_dev->cfg->regs); + /* Flush out the CRC registers */ + SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); + (void)SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); - if (spi_dev->cfg->use_crc) { - /* Need a 0->1 transition to reset the CRC logic */ - SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); - } + /* Make sure to flush out the receive buffer */ + (void)SPI_I2S_ReceiveData(spi_dev->cfg->regs); - /* Start DMA transfers */ - DMA_Cmd(spi_dev->cfg->dma.rx.channel, ENABLE); - DMA_Cmd(spi_dev->cfg->dma.tx.channel, ENABLE); + if (spi_dev->cfg->use_crc) { + /* Need a 0->1 transition to reset the CRC logic */ + SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); + } - /* Reenable the SPI device */ - SPI_Cmd(spi_dev->cfg->regs, ENABLE); + /* Start DMA transfers */ + DMA_Cmd(spi_dev->cfg->dma.rx.channel, ENABLE); + DMA_Cmd(spi_dev->cfg->dma.tx.channel, ENABLE); - if (callback) { - /* User has requested a callback, don't wait for the transfer to complete. */ - return 0; - } + /* Reenable the SPI device */ + SPI_Cmd(spi_dev->cfg->regs, ENABLE); - /* Wait until all bytes have been transmitted/received */ - while (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)); + if (callback) { + /* User has requested a callback, don't wait for the transfer to complete. */ + return 0; + } - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))); + /* Wait until all bytes have been transmitted/received */ + while (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { + ; + } - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)); + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))) { + ; + } - /* Check the CRC on the transfer if enabled. */ - if (spi_dev->cfg->use_crc) { - /* Check the SPI CRC error flag */ - if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { - return -4; - } - } + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + ; + } - /* No error */ - return 0; + /* Check the CRC on the transfer if enabled. */ + if (spi_dev->cfg->use_crc) { + /* Check the SPI CRC error flag */ + if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { + return -4; + } + } + + /* No error */ + return 0; } /** -* Check if a transfer is in progress -* \param[in] spi SPI number (0 or 1) -* \return >= 0 if no transfer is in progress -* \return -1 if disabled SPI port selected -* \return -2 if unsupported SPI port selected -* \return -3 if function has been called during an ongoing DMA transfer -*/ + * Check if a transfer is in progress + * \param[in] spi SPI number (0 or 1) + * \return >= 0 if no transfer is in progress + * \return -1 if disabled SPI port selected + * \return -2 if unsupported SPI port selected + * \return -3 if function has been called during an ongoing DMA transfer + */ int32_t PIOS_SPI_Busy(uint32_t spi_id) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - /* DMA buffer has data or SPI transmit register not empty or SPI is busy*/ - if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel) || - !SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE) || - SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) - { - return -3; - } + PIOS_Assert(valid) - return(0); + /* DMA buffer has data or SPI transmit register not empty or SPI is busy*/ + if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel) || + !SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE) || + SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + return -3; + } + + return 0; } void PIOS_SPI_SetPrescalar(uint32_t spi_id, uint32_t prescaler) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid); - PIOS_Assert(IS_SPI_BAUDRATE_PRESCALER(prescaler)); - - spi_dev->cfg->regs->CR1 = (spi_dev->cfg->regs->CR1 & ~0x0038) | prescaler; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + + PIOS_Assert(valid); + PIOS_Assert(IS_SPI_BAUDRATE_PRESCALER(prescaler)); + + spi_dev->cfg->regs->CR1 = (spi_dev->cfg->regs->CR1 & ~0x0038) | prescaler; } void PIOS_SPI_IRQ_Handler(uint32_t spi_id) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - DMA_ClearFlag(spi_dev->cfg->dma.irq.flags); + PIOS_Assert(valid) - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))) ; + DMA_ClearFlag(spi_dev->cfg->dma.irq.flags); - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) ; + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))) { + ; + } - if (spi_dev->callback != NULL) { - bool crc_ok = TRUE; - uint8_t crc_val; + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + ; + } - if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { - crc_ok = FALSE; - SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); - } - crc_val = SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); - spi_dev->callback(crc_ok, crc_val); - } + if (spi_dev->callback != NULL) { + bool crc_ok = TRUE; + uint8_t crc_val; + + if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { + crc_ok = FALSE; + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + } + crc_val = SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); + spi_dev->callback(crc_ok, crc_val); + } } #endif /* PIOS_INCLUDE_SPI */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_sys.c b/flight/pios/stm32f10x/pios_sys.c index 9d52e34eb..63791fa22 100644 --- a/flight/pios/stm32f10x/pios_sys.c +++ b/flight/pios/stm32f10x/pios_sys.c @@ -6,26 +6,26 @@ * @brief PIOS System Initialization code * @{ * - * @file pios_sys.c + * @file pios_sys.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Sets up basic STM32 system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,185 +38,190 @@ void NVIC_Configuration(void); void SysTick_Handler(void); /* Local Macros */ -#define MEM8(addr) (*((volatile uint8_t *)(addr))) -#define MEM16(addr) (*((volatile uint16_t *)(addr))) -#define MEM32(addr) (*((volatile uint32_t *)(addr))) +#define MEM8(addr) (*((volatile uint8_t *)(addr))) +#define MEM16(addr) (*((volatile uint16_t *)(addr))) +#define MEM32(addr) (*((volatile uint32_t *)(addr))) /** -* Initialises all system peripherals -*/ + * Initialises all system peripherals + */ void PIOS_SYS_Init(void) { - /* Setup STM32 system (RCC, clock, PLL and Flash configuration) - CMSIS Function */ - SystemInit(); + /* Setup STM32 system (RCC, clock, PLL and Flash configuration) - CMSIS Function */ + SystemInit(); - /* Init the delay system */ - PIOS_DELAY_Init(); + /* Init the delay system */ + PIOS_DELAY_Init(); - /* Enable GPIOA, GPIOB, GPIOC, GPIOD, GPIOE and AFIO clocks */ - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD | RCC_APB2Periph_GPIOE | - RCC_APB2Periph_AFIO, ENABLE); + /* Enable GPIOA, GPIOB, GPIOC, GPIOD, GPIOE and AFIO clocks */ + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD | RCC_APB2Periph_GPIOE | + RCC_APB2Periph_AFIO, ENABLE); #if (PIOS_USB_ENABLED) - /* Ensure that pull-up is active on detect pin */ - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; - GPIO_InitStructure.GPIO_Pin = PIOS_USB_DETECT_GPIO_PIN; - GPIO_Init(PIOS_USB_DETECT_GPIO_PORT, &GPIO_InitStructure); + /* Ensure that pull-up is active on detect pin */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; + GPIO_InitStructure.GPIO_Pin = PIOS_USB_DETECT_GPIO_PIN; + GPIO_Init(PIOS_USB_DETECT_GPIO_PORT, &GPIO_InitStructure); #endif - /* Initialise Basic NVIC */ - NVIC_Configuration(); - + /* Initialise Basic NVIC */ + NVIC_Configuration(); } /** -* Shutdown PIOS and reset the microcontroller:
-*
    -*
  • Disable all RTOS tasks -*
  • Disable all interrupts -*
  • Turn off all board LEDs -*
  • Reset STM32 -*
-* \return < 0 if reset failed -*/ + * Shutdown PIOS and reset the microcontroller:
+ *
    + *
  • Disable all RTOS tasks + *
  • Disable all interrupts + *
  • Turn off all board LEDs + *
  • Reset STM32 + *
+ * \return < 0 if reset failed + */ int32_t PIOS_SYS_Reset(void) { - /* Disable all RTOS tasks */ + /* Disable all RTOS tasks */ #if defined(PIOS_INCLUDE_FREERTOS) - /* port specific FreeRTOS function to disable tasks (nested) */ - portENTER_CRITICAL(); + /* port specific FreeRTOS function to disable tasks (nested) */ + portENTER_CRITICAL(); #endif - // disable all interrupts - PIOS_IRQ_Disable(); + // disable all interrupts + PIOS_IRQ_Disable(); - // turn off all board LEDs + // turn off all board LEDs #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Off(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ + PIOS_LED_Off(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ #if defined(PIOS_LED_ALARM) - PIOS_LED_Off(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ + PIOS_LED_Off(PIOS_LED_ALARM); +#endif /* PIOS_LED_ALARM */ - RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - NVIC_SystemReset(); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + NVIC_SystemReset(); - while (1) ; + while (1) { + ; + } - /* We will never reach this point */ - return -1; + /* We will never reach this point */ + return -1; } /** -* Returns the CPU's flash size (in bytes) -*/ + * Returns the CPU's flash size (in bytes) + */ uint32_t PIOS_SYS_getCPUFlashSize(void) { - return ((uint32_t) MEM16(0x1FFFF7E0) * 1000); + return (uint32_t)MEM16(0x1FFFF7E0) * 1000; } /** -* Returns the serial number as a string -* param[out] uint8_t pointer to a string which can store at least 12 bytes -* (12 bytes returned for STM32) -* return < 0 if feature not supported -*/ + * Returns the serial number as a string + * param[out] uint8_t pointer to a string which can store at least 12 bytes + * (12 bytes returned for STM32) + * return < 0 if feature not supported + */ int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t *array) { - int i; + int i; - /* Stored in the so called "electronic signature" */ - for (i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) { - uint8_t b = MEM8(0x1ffff7e8 + i); + /* Stored in the so called "electronic signature" */ + for (i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) { + uint8_t b = MEM8(0x1ffff7e8 + i); - array[i] = b; - } + array[i] = b; + } - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Returns the serial number as a string -* param[out] str pointer to a string which can store at least 32 digits + zero terminator! -* (24 digits returned for STM32) -* return < 0 if feature not supported -*/ + * Returns the serial number as a string + * param[out] str pointer to a string which can store at least 32 digits + zero terminator! + * (24 digits returned for STM32) + * return < 0 if feature not supported + */ int32_t PIOS_SYS_SerialNumberGet(char *str) { - int i; + int i; - /* Stored in the so called "electronic signature" */ - for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { - uint8_t b = MEM8(0x1ffff7e8 + (i / 2)); - if (!(i & 1)) - b >>= 4; - b &= 0x0f; + /* Stored in the so called "electronic signature" */ + for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { + uint8_t b = MEM8(0x1ffff7e8 + (i / 2)); + if (!(i & 1)) { + b >>= 4; + } + b &= 0x0f; - str[i] = ((b > 9) ? ('A' - 10) : '0') + b; - } - str[i] = '\0'; + str[i] = ((b > 9) ? ('A' - 10) : '0') + b; + } + str[i] = '\0'; - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Configures Vector Table base location and SysTick -*/ + * Configures Vector Table base location and SysTick + */ void NVIC_Configuration(void) { - /* Set the Vector Table base address as specified in .ld file */ - extern void *pios_isr_vector_table_base; - NVIC_SetVectorTable((uint32_t)&pios_isr_vector_table_base, 0x0); + /* Set the Vector Table base address as specified in .ld file */ + extern void *pios_isr_vector_table_base; - /* 4 bits for Interrupt priorities so no sub priorities */ - NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + NVIC_SetVectorTable((uint32_t)&pios_isr_vector_table_base, 0x0); - /* Configure HCLK clock as SysTick clock source. */ - SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK); + /* 4 bits for Interrupt priorities so no sub priorities */ + NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + + /* Configure HCLK clock as SysTick clock source. */ + SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK); } #ifdef USE_FULL_ASSERT /** -* Reports the name of the source file and the source line number -* where the assert_param error has occurred. -* \param[in] file pointer to the source file name -* \param[in] line assert_param error line source number -* \retval None -*/ -void assert_failed(uint8_t * file, uint32_t line) + * Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * \param[in] file pointer to the source file name + * \param[in] line assert_param error line source number + * \retval None + */ +void assert_failed(uint8_t *file, uint32_t line) { - /* When serial debugging is implemented, use something like this. */ - /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ + /* When serial debugging is implemented, use something like this. */ + /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ - /* Setup the LEDs to Alternate */ + /* Setup the LEDs to Alternate */ #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_On(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ + PIOS_LED_On(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ #if defined(PIOS_LED_ALARM) - PIOS_LED_Off(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ + PIOS_LED_Off(PIOS_LED_ALARM); +#endif /* PIOS_LED_ALARM */ - /* Infinite loop */ - while (1) { + /* Infinite loop */ + while (1) { #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ #if defined(PIOS_LED_ALARM) - PIOS_LED_Toggle(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ - for (int i = 0; i < 1000000; i++) ; - } + PIOS_LED_Toggle(PIOS_LED_ALARM); +#endif /* PIOS_LED_ALARM */ + for (int i = 0; i < 1000000; i++) { + ; + } + } } -#endif +#endif /* ifdef USE_FULL_ASSERT */ #endif /* PIOS_INCLUDE_SYS */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_tim.c b/flight/pios/stm32f10x/pios_tim.c index 28a187341..a519240ee 100644 --- a/flight/pios/stm32f10x/pios_tim.c +++ b/flight/pios/stm32f10x/pios_tim.c @@ -5,425 +5,426 @@ #include "pios_tim_priv.h" enum pios_tim_dev_magic { - PIOS_TIM_DEV_MAGIC = 0x87654098, + PIOS_TIM_DEV_MAGIC = 0x87654098, }; struct pios_tim_dev { - enum pios_tim_dev_magic magic; + enum pios_tim_dev_magic magic; - const struct pios_tim_channel * channels; - uint8_t num_channels; + const struct pios_tim_channel *channels; + uint8_t num_channels; - const struct pios_tim_callbacks * callbacks; - uint32_t context; + const struct pios_tim_callbacks *callbacks; + uint32_t context; }; #if 0 -static bool PIOS_TIM_validate(struct pios_tim_dev * tim_dev) +static bool PIOS_TIM_validate(struct pios_tim_dev *tim_dev) { - return (tim_dev->magic == PIOS_TIM_DEV_MAGIC); + return tim_dev->magic == PIOS_TIM_DEV_MAGIC; } #endif #if defined(PIOS_INCLUDE_FREERTOS) && 0 -static struct pios_tim_dev * PIOS_TIM_alloc(void) +static struct pios_tim_dev *PIOS_TIM_alloc(void) { - struct pios_tim_dev * tim_dev; + struct pios_tim_dev *tim_dev; - tim_dev = (struct pios_tim_dev *)malloc(sizeof(*tim_dev)); - if (!tim_dev) return(NULL); + tim_dev = (struct pios_tim_dev *)malloc(sizeof(*tim_dev)); + if (!tim_dev) { + return NULL; + } - tim_dev->magic = PIOS_TIM_DEV_MAGIC; - return(tim_dev); + tim_dev->magic = PIOS_TIM_DEV_MAGIC; + return tim_dev; } #else static struct pios_tim_dev pios_tim_devs[PIOS_TIM_MAX_DEVS]; static uint8_t pios_tim_num_devs; -static struct pios_tim_dev * PIOS_TIM_alloc(void) +static struct pios_tim_dev *PIOS_TIM_alloc(void) { - struct pios_tim_dev * tim_dev; + struct pios_tim_dev *tim_dev; - if (pios_tim_num_devs >= PIOS_TIM_MAX_DEVS) { - return (NULL); - } + if (pios_tim_num_devs >= PIOS_TIM_MAX_DEVS) { + return NULL; + } - tim_dev = &pios_tim_devs[pios_tim_num_devs++]; - tim_dev->magic = PIOS_TIM_DEV_MAGIC; + tim_dev = &pios_tim_devs[pios_tim_num_devs++]; + tim_dev->magic = PIOS_TIM_DEV_MAGIC; - return (tim_dev); + return tim_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) && 0 */ - - -int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg * cfg) +int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg *cfg) { - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(cfg); - /* Enable appropriate clock to timer module */ - switch((uint32_t) cfg->timer) { - case (uint32_t)TIM1: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); - break; - case (uint32_t)TIM2: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); - break; - case (uint32_t)TIM3: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); - break; - case (uint32_t)TIM4: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); - break; + /* Enable appropriate clock to timer module */ + switch ((uint32_t)cfg->timer) { + case (uint32_t)TIM1: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); + break; + case (uint32_t)TIM2: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); + break; + case (uint32_t)TIM3: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); + break; + case (uint32_t)TIM4: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); + break; #ifdef STM32F10X_HD - case (uint32_t)TIM5: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); - break; - case (uint32_t)TIM6: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); - break; - case (uint32_t)TIM7: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); - break; - case (uint32_t)TIM8: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); - break; + case (uint32_t)TIM5: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); + break; + case (uint32_t)TIM6: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); + break; + case (uint32_t)TIM7: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); + break; + case (uint32_t)TIM8: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); + break; #endif - } + } - /* Configure the dividers for this timer */ - TIM_TimeBaseInit(cfg->timer, cfg->time_base_init); + /* Configure the dividers for this timer */ + TIM_TimeBaseInit(cfg->timer, cfg->time_base_init); - /* Configure internal timer clocks */ - TIM_InternalClockConfig(cfg->timer); + /* Configure internal timer clocks */ + TIM_InternalClockConfig(cfg->timer); - /* Enable timers */ - TIM_Cmd(cfg->timer, ENABLE); + /* Enable timers */ + TIM_Cmd(cfg->timer, ENABLE); - /* Enable Interrupts */ - NVIC_Init(&cfg->irq.init); + /* Enable Interrupts */ + NVIC_Init(&cfg->irq.init); - return 0; + return 0; } -int32_t PIOS_TIM_InitChannels(uint32_t * tim_id, const struct pios_tim_channel * channels, uint8_t num_channels, const struct pios_tim_callbacks * callbacks, uint32_t context) +int32_t PIOS_TIM_InitChannels(uint32_t *tim_id, const struct pios_tim_channel *channels, uint8_t num_channels, const struct pios_tim_callbacks *callbacks, uint32_t context) { - PIOS_Assert(channels); - PIOS_Assert(num_channels); + PIOS_Assert(channels); + PIOS_Assert(num_channels); - struct pios_tim_dev * tim_dev; - tim_dev = (struct pios_tim_dev *) PIOS_TIM_alloc(); - if (!tim_dev) goto out_fail; + struct pios_tim_dev *tim_dev; + tim_dev = (struct pios_tim_dev *)PIOS_TIM_alloc(); + if (!tim_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - tim_dev->channels = channels; - tim_dev->num_channels = num_channels; - tim_dev->callbacks = callbacks; - tim_dev->context = context; + /* Bind the configuration to the device instance */ + tim_dev->channels = channels; + tim_dev->num_channels = num_channels; + tim_dev->callbacks = callbacks; + tim_dev->context = context; - /* Configure the pins */ - for (uint8_t i = 0; i < num_channels; i++) { - const struct pios_tim_channel * chan = &(channels[i]); + /* Configure the pins */ + for (uint8_t i = 0; i < num_channels; i++) { + const struct pios_tim_channel *chan = &(channels[i]); - /* Enable the peripheral clock for the GPIO */ - switch ((uint32_t)chan->pin.gpio) { - case (uint32_t) GPIOA: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); - break; - case (uint32_t) GPIOB: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - break; - case (uint32_t) GPIOC: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); - break; - default: - PIOS_Assert(0); - break; - } - GPIO_Init(chan->pin.gpio, &chan->pin.init); + /* Enable the peripheral clock for the GPIO */ + switch ((uint32_t)chan->pin.gpio) { + case (uint32_t)GPIOA: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); + break; + case (uint32_t)GPIOB: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + break; + case (uint32_t)GPIOC: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); + break; + default: + PIOS_Assert(0); + break; + } + GPIO_Init(chan->pin.gpio, &chan->pin.init); - if (chan->remap) { - GPIO_PinRemapConfig(chan->remap, ENABLE); - } - } + if (chan->remap) { + GPIO_PinRemapConfig(chan->remap, ENABLE); + } + } - *tim_id = (uint32_t)tim_dev; + *tim_id = (uint32_t)tim_dev; - return(0); + return 0; out_fail: - return(-1); + return -1; } -static void PIOS_TIM_generic_irq_handler(TIM_TypeDef * timer) +static void PIOS_TIM_generic_irq_handler(TIM_TypeDef *timer) { - /* Iterate over all registered clients of the TIM layer to find channels on this timer */ - for (uint8_t i = 0; i < pios_tim_num_devs; i++) { - const struct pios_tim_dev * tim_dev = &pios_tim_devs[i]; + /* Iterate over all registered clients of the TIM layer to find channels on this timer */ + for (uint8_t i = 0; i < pios_tim_num_devs; i++) { + const struct pios_tim_dev *tim_dev = &pios_tim_devs[i]; - if (!tim_dev->channels || tim_dev->num_channels == 0) { - /* No channels to process on this client */ - continue; - } + if (!tim_dev->channels || tim_dev->num_channels == 0) { + /* No channels to process on this client */ + continue; + } - /* Check for an overflow event on this timer */ - bool overflow_event; - uint16_t overflow_count; - if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) { - TIM_ClearITPendingBit(timer, TIM_IT_Update); - overflow_count = timer->ARR; - overflow_event = true; - } else { - overflow_count = 0; - overflow_event = false; - } + /* Check for an overflow event on this timer */ + bool overflow_event; + uint16_t overflow_count; + if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) { + TIM_ClearITPendingBit(timer, TIM_IT_Update); + overflow_count = timer->ARR; + overflow_event = true; + } else { + overflow_count = 0; + overflow_event = false; + } - for (uint8_t j = 0; j < tim_dev->num_channels; j++) { - const struct pios_tim_channel * chan = &tim_dev->channels[j]; + for (uint8_t j = 0; j < tim_dev->num_channels; j++) { + const struct pios_tim_channel *chan = &tim_dev->channels[j]; - if (chan->timer != timer) { - /* channel is not on this timer */ - continue; - } + if (chan->timer != timer) { + /* channel is not on this timer */ + continue; + } - /* Figure out which interrupt bit we should be looking at */ - uint16_t timer_it; - switch (chan->timer_chan) { - case TIM_Channel_1: - timer_it = TIM_IT_CC1; - break; - case TIM_Channel_2: - timer_it = TIM_IT_CC2; - break; - case TIM_Channel_3: - timer_it = TIM_IT_CC3; - break; - case TIM_Channel_4: - timer_it = TIM_IT_CC4; - break; - default: - PIOS_Assert(0); - break; - } + /* Figure out which interrupt bit we should be looking at */ + uint16_t timer_it; + switch (chan->timer_chan) { + case TIM_Channel_1: + timer_it = TIM_IT_CC1; + break; + case TIM_Channel_2: + timer_it = TIM_IT_CC2; + break; + case TIM_Channel_3: + timer_it = TIM_IT_CC3; + break; + case TIM_Channel_4: + timer_it = TIM_IT_CC4; + break; + default: + PIOS_Assert(0); + break; + } - bool edge_event; - uint16_t edge_count; - if (TIM_GetITStatus(chan->timer, timer_it) == SET) { - TIM_ClearITPendingBit(chan->timer, timer_it); + bool edge_event; + uint16_t edge_count; + if (TIM_GetITStatus(chan->timer, timer_it) == SET) { + TIM_ClearITPendingBit(chan->timer, timer_it); - /* Read the current counter */ - switch(chan->timer_chan) { - case TIM_Channel_1: - edge_count = TIM_GetCapture1(chan->timer); - break; - case TIM_Channel_2: - edge_count = TIM_GetCapture2(chan->timer); - break; - case TIM_Channel_3: - edge_count = TIM_GetCapture3(chan->timer); - break; - case TIM_Channel_4: - edge_count = TIM_GetCapture4(chan->timer); - break; - default: - PIOS_Assert(0); - break; - } - edge_event = true; - } else { - edge_event = false; - edge_count = 0; - } + /* Read the current counter */ + switch (chan->timer_chan) { + case TIM_Channel_1: + edge_count = TIM_GetCapture1(chan->timer); + break; + case TIM_Channel_2: + edge_count = TIM_GetCapture2(chan->timer); + break; + case TIM_Channel_3: + edge_count = TIM_GetCapture3(chan->timer); + break; + case TIM_Channel_4: + edge_count = TIM_GetCapture4(chan->timer); + break; + default: + PIOS_Assert(0); + break; + } + edge_event = true; + } else { + edge_event = false; + edge_count = 0; + } - if (!tim_dev->callbacks) { - /* No callbacks registered, we're done with this channel */ - continue; - } + if (!tim_dev->callbacks) { + /* No callbacks registered, we're done with this channel */ + continue; + } - /* Generate the appropriate callbacks */ - if (overflow_event & edge_event) { - /* - * When both edge and overflow happen in the same interrupt, we - * need a heuristic to determine the order of the edge and overflow - * events so that the callbacks happen in the right order. If we - * get the order wrong, our pulse width calculations could be off by up - * to ARR ticks. That could be bad. - * - * Heuristic: If the edge_count is < 16 ticks above zero then we assume the - * edge happened just after the overflow. - */ + /* Generate the appropriate callbacks */ + if (overflow_event & edge_event) { + /* + * When both edge and overflow happen in the same interrupt, we + * need a heuristic to determine the order of the edge and overflow + * events so that the callbacks happen in the right order. If we + * get the order wrong, our pulse width calculations could be off by up + * to ARR ticks. That could be bad. + * + * Heuristic: If the edge_count is < 16 ticks above zero then we assume the + * edge happened just after the overflow. + */ - if (edge_count < 16) { - /* Call the overflow callback first */ - if (tim_dev->callbacks->overflow) { - (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, - tim_dev->context, - j, - overflow_count); - } - /* Call the edge callback second */ - if (tim_dev->callbacks->edge) { - (*tim_dev->callbacks->edge)((uint32_t)tim_dev, - tim_dev->context, - j, - edge_count); - } - } else { - /* Call the edge callback first */ - if (tim_dev->callbacks->edge) { - (*tim_dev->callbacks->edge)((uint32_t)tim_dev, - tim_dev->context, - j, - edge_count); - } - /* Call the overflow callback second */ - if (tim_dev->callbacks->overflow) { - (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, - tim_dev->context, - j, - overflow_count); - } - } - } else if (overflow_event && tim_dev->callbacks->overflow) { - (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, - tim_dev->context, - j, - overflow_count); - } else if (edge_event && tim_dev->callbacks->edge) { - (*tim_dev->callbacks->edge)((uint32_t)tim_dev, - tim_dev->context, - j, - edge_count); - } - } - } + if (edge_count < 16) { + /* Call the overflow callback first */ + if (tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } + /* Call the edge callback second */ + if (tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + } else { + /* Call the edge callback first */ + if (tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + /* Call the overflow callback second */ + if (tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } + } + } else if (overflow_event && tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } else if (edge_event && tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + } + } } #if 0 - uint16_t val = 0; - for(uint8_t i = 0; i < pios_pwm_cfg.num_channels; i++) { - struct pios_pwm_channel channel = pios_pwm_cfg.channels[i]; - if ((channel.timer == timer) && (TIM_GetITStatus(channel.timer, channel.ccr) == SET)) { - - TIM_ClearITPendingBit(channel.timer, channel.ccr); - - switch(channel.channel) { - case TIM_Channel_1: - val = TIM_GetCapture1(channel.timer); - break; - case TIM_Channel_2: - val = TIM_GetCapture2(channel.timer); - break; - case TIM_Channel_3: - val = TIM_GetCapture3(channel.timer); - break; - case TIM_Channel_4: - val = TIM_GetCapture4(channel.timer); - break; - } - - if (CaptureState[i] == 0) { - RiseValue[i] = val; - } else { - FallValue[i] = val; - } - - // flip state machine and capture value here - /* Simple rise or fall state machine */ - TIM_ICInitTypeDef TIM_ICInitStructure = pios_pwm_cfg.tim_ic_init; - if (CaptureState[i] == 0) { - /* Switch states */ - CaptureState[i] = 1; - - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; - TIM_ICInitStructure.TIM_Channel = channel.channel; - TIM_ICInit(channel.timer, &TIM_ICInitStructure); - } else { - /* Capture computation */ - if (FallValue[i] > RiseValue[i]) { - CaptureValue[i] = (FallValue[i] - RiseValue[i]); - } else { - CaptureValue[i] = ((channel.timer->ARR - RiseValue[i]) + FallValue[i]); - } - - /* Switch states */ - CaptureState[i] = 0; - - /* Increase supervisor counter */ - CapCounter[i]++; - - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; - TIM_ICInitStructure.TIM_Channel = channel.channel; - TIM_ICInit(channel.timer, &TIM_ICInitStructure); - } - } - } -#endif +uint16_t val = 0; +for (uint8_t i = 0; i < pios_pwm_cfg.num_channels; i++) { + struct pios_pwm_channel channel = pios_pwm_cfg.channels[i]; + if ((channel.timer == timer) && (TIM_GetITStatus(channel.timer, channel.ccr) == SET)) { + TIM_ClearITPendingBit(channel.timer, channel.ccr); + + switch (channel.channel) { + case TIM_Channel_1: + val = TIM_GetCapture1(channel.timer); + break; + case TIM_Channel_2: + val = TIM_GetCapture2(channel.timer); + break; + case TIM_Channel_3: + val = TIM_GetCapture3(channel.timer); + break; + case TIM_Channel_4: + val = TIM_GetCapture4(channel.timer); + break; + } + + if (CaptureState[i] == 0) { + RiseValue[i] = val; + } else { + FallValue[i] = val; + } + + // flip state machine and capture value here + /* Simple rise or fall state machine */ + TIM_ICInitTypeDef TIM_ICInitStructure = pios_pwm_cfg.tim_ic_init; + if (CaptureState[i] == 0) { + /* Switch states */ + CaptureState[i] = 1; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + TIM_ICInitStructure.TIM_Channel = channel.channel; + TIM_ICInit(channel.timer, &TIM_ICInitStructure); + } else { + /* Capture computation */ + if (FallValue[i] > RiseValue[i]) { + CaptureValue[i] = (FallValue[i] - RiseValue[i]); + } else { + CaptureValue[i] = ((channel.timer->ARR - RiseValue[i]) + FallValue[i]); + } + + /* Switch states */ + CaptureState[i] = 0; + + /* Increase supervisor counter */ + CapCounter[i]++; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_Channel = channel.channel; + TIM_ICInit(channel.timer, &TIM_ICInitStructure); + } + } +} +#endif /* if 0 */ /* Bind Interrupt Handlers * * Map all valid TIM IRQs to the common interrupt handler * and give it enough context to properly demux the various timers */ -void TIM1_UP_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_1_UP_irq_handler"))); -static void PIOS_TIM_1_UP_irq_handler (void) +void TIM1_UP_IRQHandler(void) __attribute__((alias("PIOS_TIM_1_UP_irq_handler"))); +static void PIOS_TIM_1_UP_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM1); + PIOS_TIM_generic_irq_handler(TIM1); } -void TIM1_CC_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_1_CC_irq_handler"))); -static void PIOS_TIM_1_CC_irq_handler (void) +void TIM1_CC_IRQHandler(void) __attribute__((alias("PIOS_TIM_1_CC_irq_handler"))); +static void PIOS_TIM_1_CC_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM1); + PIOS_TIM_generic_irq_handler(TIM1); } -void TIM2_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_2_irq_handler"))); -static void PIOS_TIM_2_irq_handler (void) +void TIM2_IRQHandler(void) __attribute__((alias("PIOS_TIM_2_irq_handler"))); +static void PIOS_TIM_2_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM2); + PIOS_TIM_generic_irq_handler(TIM2); } -void TIM3_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_3_irq_handler"))); -static void PIOS_TIM_3_irq_handler (void) +void TIM3_IRQHandler(void) __attribute__((alias("PIOS_TIM_3_irq_handler"))); +static void PIOS_TIM_3_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM3); + PIOS_TIM_generic_irq_handler(TIM3); } -void TIM4_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_4_irq_handler"))); -static void PIOS_TIM_4_irq_handler (void) +void TIM4_IRQHandler(void) __attribute__((alias("PIOS_TIM_4_irq_handler"))); +static void PIOS_TIM_4_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM4); + PIOS_TIM_generic_irq_handler(TIM4); } -void TIM5_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_5_irq_handler"))); -static void PIOS_TIM_5_irq_handler (void) +void TIM5_IRQHandler(void) __attribute__((alias("PIOS_TIM_5_irq_handler"))); +static void PIOS_TIM_5_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM5); + PIOS_TIM_generic_irq_handler(TIM5); } -void TIM6_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_6_irq_handler"))); -static void PIOS_TIM_6_irq_handler (void) +void TIM6_IRQHandler(void) __attribute__((alias("PIOS_TIM_6_irq_handler"))); +static void PIOS_TIM_6_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM6); + PIOS_TIM_generic_irq_handler(TIM6); } -void TIM7_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_7_irq_handler"))); -static void PIOS_TIM_7_irq_handler (void) +void TIM7_IRQHandler(void) __attribute__((alias("PIOS_TIM_7_irq_handler"))); +static void PIOS_TIM_7_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM7); + PIOS_TIM_generic_irq_handler(TIM7); } -void TIM8_UP_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_8_UP_irq_handler"))); -static void PIOS_TIM_8_UP_irq_handler (void) +void TIM8_UP_IRQHandler(void) __attribute__((alias("PIOS_TIM_8_UP_irq_handler"))); +static void PIOS_TIM_8_UP_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM8); + PIOS_TIM_generic_irq_handler(TIM8); } -void TIM8_CC_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_8_CC_irq_handler"))); -static void PIOS_TIM_8_CC_irq_handler (void) +void TIM8_CC_IRQHandler(void) __attribute__((alias("PIOS_TIM_8_CC_irq_handler"))); +static void PIOS_TIM_8_CC_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM8); + PIOS_TIM_generic_irq_handler(TIM8); } #endif /* PIOS_INCLUDE_TIM */ diff --git a/flight/pios/stm32f10x/pios_usart.c b/flight/pios/stm32f10x/pios_usart.c index 3691c0dfd..21d29006a 100644 --- a/flight/pios/stm32f10x/pios_usart.c +++ b/flight/pios/stm32f10x/pios_usart.c @@ -6,25 +6,25 @@ * @brief PIOS interface for USART port * @{ * - * @file pios_usart.c + * @file pios_usart.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief USART commands. Inits USARTs, controls USARTs & Interupt handlers. (STM32 dependent) * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,62 +42,64 @@ static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail); static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usart_com_driver = { - .set_baud = PIOS_USART_ChangeBaud, - .tx_start = PIOS_USART_TxStart, - .rx_start = PIOS_USART_RxStart, - .bind_tx_cb = PIOS_USART_RegisterTxCallback, - .bind_rx_cb = PIOS_USART_RegisterRxCallback, + .set_baud = PIOS_USART_ChangeBaud, + .tx_start = PIOS_USART_TxStart, + .rx_start = PIOS_USART_RxStart, + .bind_tx_cb = PIOS_USART_RegisterTxCallback, + .bind_rx_cb = PIOS_USART_RegisterRxCallback, }; enum pios_usart_dev_magic { - PIOS_USART_DEV_MAGIC = 0x11223344, + PIOS_USART_DEV_MAGIC = 0x11223344, }; struct pios_usart_dev { - enum pios_usart_dev_magic magic; - const struct pios_usart_cfg * cfg; + enum pios_usart_dev_magic magic; + const struct pios_usart_cfg *cfg; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; - uint32_t rx_dropped; + uint32_t rx_dropped; }; -static bool PIOS_USART_validate(struct pios_usart_dev * usart_dev) +static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) { - return (usart_dev->magic == PIOS_USART_DEV_MAGIC); + return usart_dev->magic == PIOS_USART_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_usart_dev * PIOS_USART_alloc(void) +static struct pios_usart_dev *PIOS_USART_alloc(void) { - struct pios_usart_dev * usart_dev; + struct pios_usart_dev *usart_dev; - usart_dev = (struct pios_usart_dev *)pvPortMalloc(sizeof(*usart_dev)); - if (!usart_dev) return(NULL); + usart_dev = (struct pios_usart_dev *)pvPortMalloc(sizeof(*usart_dev)); + if (!usart_dev) { + return NULL; + } - usart_dev->magic = PIOS_USART_DEV_MAGIC; - return(usart_dev); + usart_dev->magic = PIOS_USART_DEV_MAGIC; + return usart_dev; } #else static struct pios_usart_dev pios_usart_devs[PIOS_USART_MAX_DEVS]; static uint8_t pios_usart_num_devs; -static struct pios_usart_dev * PIOS_USART_alloc(void) +static struct pios_usart_dev *PIOS_USART_alloc(void) { - struct pios_usart_dev * usart_dev; + struct pios_usart_dev *usart_dev; - if (pios_usart_num_devs >= PIOS_USART_MAX_DEVS) { - return (NULL); - } + if (pios_usart_num_devs >= PIOS_USART_MAX_DEVS) { + return NULL; + } - usart_dev = &pios_usart_devs[pios_usart_num_devs++]; - usart_dev->magic = PIOS_USART_DEV_MAGIC; + usart_dev = &pios_usart_devs[pios_usart_num_devs++]; + usart_dev->magic = PIOS_USART_DEV_MAGIC; - return (usart_dev); + return usart_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /* Bind Interrupt Handlers * @@ -108,224 +110,232 @@ static struct pios_usart_dev * PIOS_USART_alloc(void) static void PIOS_USART_generic_irq_handler(uint32_t usart_id); static uint32_t PIOS_USART_1_id; -void USART1_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_1_irq_handler"))); -static void PIOS_USART_1_irq_handler (void) +void USART1_IRQHandler(void) __attribute__((alias("PIOS_USART_1_irq_handler"))); +static void PIOS_USART_1_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_1_id); + PIOS_USART_generic_irq_handler(PIOS_USART_1_id); } static uint32_t PIOS_USART_2_id; -void USART2_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_2_irq_handler"))); -static void PIOS_USART_2_irq_handler (void) +void USART2_IRQHandler(void) __attribute__((alias("PIOS_USART_2_irq_handler"))); +static void PIOS_USART_2_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_2_id); + PIOS_USART_generic_irq_handler(PIOS_USART_2_id); } static uint32_t PIOS_USART_3_id; -void USART3_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_3_irq_handler"))); -static void PIOS_USART_3_irq_handler (void) +void USART3_IRQHandler(void) __attribute__((alias("PIOS_USART_3_irq_handler"))); +static void PIOS_USART_3_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_3_id); + PIOS_USART_generic_irq_handler(PIOS_USART_3_id); } /** -* Initialise a single USART device -*/ -int32_t PIOS_USART_Init(uint32_t * usart_id, const struct pios_usart_cfg * cfg) + * Initialise a single USART device + */ +int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) { - PIOS_DEBUG_Assert(usart_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(usart_id); + PIOS_DEBUG_Assert(cfg); - struct pios_usart_dev * usart_dev; + struct pios_usart_dev *usart_dev; - usart_dev = (struct pios_usart_dev *) PIOS_USART_alloc(); - if (!usart_dev) goto out_fail; + usart_dev = (struct pios_usart_dev *)PIOS_USART_alloc(); + if (!usart_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usart_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + usart_dev->cfg = cfg; - /* Enable the USART Pins Software Remapping */ - if (usart_dev->cfg->remap) { - GPIO_PinRemapConfig(usart_dev->cfg->remap, ENABLE); - } + /* Enable the USART Pins Software Remapping */ + if (usart_dev->cfg->remap) { + GPIO_PinRemapConfig(usart_dev->cfg->remap, ENABLE); + } - /* Initialize the USART Rx and Tx pins */ - GPIO_Init(usart_dev->cfg->rx.gpio, &usart_dev->cfg->rx.init); - GPIO_Init(usart_dev->cfg->tx.gpio, &usart_dev->cfg->tx.init); + /* Initialize the USART Rx and Tx pins */ + GPIO_Init(usart_dev->cfg->rx.gpio, &usart_dev->cfg->rx.init); + GPIO_Init(usart_dev->cfg->tx.gpio, &usart_dev->cfg->tx.init); - /* Enable USART clock */ - switch ((uint32_t)usart_dev->cfg->regs) { - case (uint32_t)USART1: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); - break; - case (uint32_t)USART2: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); - break; - case (uint32_t)USART3: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); - break; - } - - /* Configure the USART */ - USART_Init(usart_dev->cfg->regs, &usart_dev->cfg->init); - - *usart_id = (uint32_t)usart_dev; + /* Enable USART clock */ + switch ((uint32_t)usart_dev->cfg->regs) { + case (uint32_t)USART1: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); + break; + case (uint32_t)USART2: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); + break; + case (uint32_t)USART3: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); + break; + } - /* Configure USART Interrupts */ - switch ((uint32_t)usart_dev->cfg->regs) { - case (uint32_t)USART1: - PIOS_USART_1_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART2: - PIOS_USART_2_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART3: - PIOS_USART_3_id = (uint32_t)usart_dev; - break; - } - NVIC_Init(&usart_dev->cfg->irq.init); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); - - /* Enable USART */ - USART_Cmd(usart_dev->cfg->regs, ENABLE); + /* Configure the USART */ + USART_Init(usart_dev->cfg->regs, &usart_dev->cfg->init); - return(0); + *usart_id = (uint32_t)usart_dev; + + /* Configure USART Interrupts */ + switch ((uint32_t)usart_dev->cfg->regs) { + case (uint32_t)USART1: + PIOS_USART_1_id = (uint32_t)usart_dev; + break; + case (uint32_t)USART2: + PIOS_USART_2_id = (uint32_t)usart_dev; + break; + case (uint32_t)USART3: + PIOS_USART_3_id = (uint32_t)usart_dev; + break; + } + NVIC_Init(&usart_dev->cfg->irq.init); + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); + + /* Enable USART */ + USART_Cmd(usart_dev->cfg->regs, ENABLE); + + return 0; out_fail: - return(-1); + return -1; } static void PIOS_USART_RxStart(uint32_t usart_id, __attribute__((unused)) uint16_t rx_bytes_avail) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); + bool valid = PIOS_USART_validate(usart_dev); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); + PIOS_Assert(valid); + + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); } static void PIOS_USART_TxStart(uint32_t usart_id, __attribute__((unused)) uint16_t tx_bytes_avail) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); + bool valid = PIOS_USART_validate(usart_dev); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); + PIOS_Assert(valid); + + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); } /** -* Changes the baud rate of the USART peripheral without re-initialising. -* \param[in] usart_id USART name (GPS, TELEM, AUX) -* \param[in] baud Requested baud rate -*/ + * Changes the baud rate of the USART peripheral without re-initialising. + * \param[in] usart_id USART name (GPS, TELEM, AUX) + * \param[in] baud Requested baud rate + */ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); + bool valid = PIOS_USART_validate(usart_dev); - USART_InitTypeDef USART_InitStructure; + PIOS_Assert(valid); - /* Start with a copy of the default configuration for the peripheral */ - USART_InitStructure = usart_dev->cfg->init; + USART_InitTypeDef USART_InitStructure; - /* Adjust the baud rate */ - USART_InitStructure.USART_BaudRate = baud; + /* Start with a copy of the default configuration for the peripheral */ + USART_InitStructure = usart_dev->cfg->init; - /* Write back the new configuration */ - USART_Init(usart_dev->cfg->regs, &USART_InitStructure); + /* Adjust the baud rate */ + USART_InitStructure.USART_BaudRate = baud; + + /* Write back the new configuration */ + USART_Init(usart_dev->cfg->regs, &USART_InitStructure); } static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); + bool valid = PIOS_USART_validate(usart_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usart_dev->rx_in_context = context; - usart_dev->rx_in_cb = rx_in_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usart_dev->rx_in_context = context; + usart_dev->rx_in_cb = rx_in_cb; } static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); + bool valid = PIOS_USART_validate(usart_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usart_dev->tx_out_context = context; - usart_dev->tx_out_cb = tx_out_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usart_dev->tx_out_context = context; + usart_dev->tx_out_cb = tx_out_cb; } static void PIOS_USART_generic_irq_handler(uint32_t usart_id) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); + bool valid = PIOS_USART_validate(usart_dev); - /* Force read of dr after sr to make sure to clear error flags */ - volatile uint16_t sr = usart_dev->cfg->regs->SR; - volatile uint8_t dr = usart_dev->cfg->regs->DR; + PIOS_Assert(valid); - /* Check if RXNE flag is set */ - bool rx_need_yield = false; - if (sr & USART_SR_RXNE) { - uint8_t byte = dr; - if (usart_dev->rx_in_cb) { - uint16_t rc; - rc = (usart_dev->rx_in_cb)(usart_dev->rx_in_context, &byte, 1, NULL, &rx_need_yield); - if (rc < 1) { - /* Lost bytes on rx */ - usart_dev->rx_dropped += 1; - } - } - } + /* Force read of dr after sr to make sure to clear error flags */ + volatile uint16_t sr = usart_dev->cfg->regs->SR; + volatile uint8_t dr = usart_dev->cfg->regs->DR; - /* Check if TXE flag is set */ - bool tx_need_yield = false; - if (sr & USART_SR_TXE) { - if (usart_dev->tx_out_cb) { - uint8_t b; - uint16_t bytes_to_send; + /* Check if RXNE flag is set */ + bool rx_need_yield = false; + if (sr & USART_SR_RXNE) { + uint8_t byte = dr; + if (usart_dev->rx_in_cb) { + uint16_t rc; + rc = (usart_dev->rx_in_cb)(usart_dev->rx_in_context, &byte, 1, NULL, &rx_need_yield); + if (rc < 1) { + /* Lost bytes on rx */ + usart_dev->rx_dropped += 1; + } + } + } - bytes_to_send = (usart_dev->tx_out_cb)(usart_dev->tx_out_context, &b, 1, NULL, &tx_need_yield); + /* Check if TXE flag is set */ + bool tx_need_yield = false; + if (sr & USART_SR_TXE) { + if (usart_dev->tx_out_cb) { + uint8_t b; + uint16_t bytes_to_send; - if (bytes_to_send > 0) { - /* Send the byte we've been given */ - usart_dev->cfg->regs->DR = b; - } else { - /* No bytes to send, disable TXE interrupt */ - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); - } - } else { - /* No bytes to send, disable TXE interrupt */ - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); - } - } + bytes_to_send = (usart_dev->tx_out_cb)(usart_dev->tx_out_context, &b, 1, NULL, &tx_need_yield); + + if (bytes_to_send > 0) { + /* Send the byte we've been given */ + usart_dev->cfg->regs->DR = b; + } else { + /* No bytes to send, disable TXE interrupt */ + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); + } + } else { + /* No bytes to send, disable TXE interrupt */ + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); + } + } #if defined(PIOS_INCLUDE_FREERTOS) - if (rx_need_yield || tx_need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (rx_need_yield || tx_need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } #endif /* PIOS_INCLUDE_USART */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f10x/pios_usb.c b/flight/pios/stm32f10x/pios_usb.c index bfecbe3ff..1f085199d 100644 --- a/flight/pios/stm32f10x/pios_usb.c +++ b/flight/pios/stm32f10x/pios_usb.c @@ -44,57 +44,61 @@ static bool transfer_possible = false; enum pios_usb_dev_magic { - PIOS_USB_DEV_MAGIC = 0x17365904, + PIOS_USB_DEV_MAGIC = 0x17365904, }; struct pios_usb_dev { - enum pios_usb_dev_magic magic; - const struct pios_usb_cfg * cfg; + enum pios_usb_dev_magic magic; + const struct pios_usb_cfg *cfg; }; /** * @brief Validate the usb device structure * @returns 0 if valid device or -1 otherwise */ -static int32_t PIOS_USB_validate(struct pios_usb_dev * usb_dev) +static int32_t PIOS_USB_validate(struct pios_usb_dev *usb_dev) { - if(usb_dev == NULL) - return -1; + if (usb_dev == NULL) { + return -1; + } - if (usb_dev->magic != PIOS_USB_DEV_MAGIC) - return -1; + if (usb_dev->magic != PIOS_USB_DEV_MAGIC) { + return -1; + } - return 0; + return 0; } #ifdef PIOS_INCLUDE_FREERTOS -static struct pios_usb_dev * PIOS_USB_alloc(void) +static struct pios_usb_dev *PIOS_USB_alloc(void) { - struct pios_usb_dev * usb_dev; + struct pios_usb_dev *usb_dev; - usb_dev = (struct pios_usb_dev *)pvPortMalloc(sizeof(*usb_dev)); - if (!usb_dev) return(NULL); + usb_dev = (struct pios_usb_dev *)pvPortMalloc(sizeof(*usb_dev)); + if (!usb_dev) { + return NULL; + } - usb_dev->magic = PIOS_USB_DEV_MAGIC; - return(usb_dev); + usb_dev->magic = PIOS_USB_DEV_MAGIC; + return usb_dev; } #else static struct pios_usb_dev pios_usb_devs[PIOS_USB_MAX_DEVS]; static uint8_t pios_usb_num_devs; -static struct pios_usb_dev * PIOS_USB_alloc(void) +static struct pios_usb_dev *PIOS_USB_alloc(void) { - struct pios_usb_dev * usb_dev; + struct pios_usb_dev *usb_dev; - if (pios_usb_num_devs >= PIOS_USB_MAX_DEVS) { - return (NULL); - } + if (pios_usb_num_devs >= PIOS_USB_MAX_DEVS) { + return NULL; + } - usb_dev = &pios_usb_devs[pios_usb_num_devs++]; - usb_dev->magic = PIOS_USB_DEV_MAGIC; + usb_dev = &pios_usb_devs[pios_usb_num_devs++]; + usb_dev->magic = PIOS_USB_DEV_MAGIC; - return (usb_dev); + return usb_dev; } -#endif +#endif /* ifdef PIOS_INCLUDE_FREERTOS */ /** @@ -103,44 +107,46 @@ static struct pios_usb_dev * PIOS_USB_alloc(void) * \note Applications shouldn't call this function directly, instead please use \ref PIOS_COM layer functions */ static uint32_t pios_usb_com_id; -int32_t PIOS_USB_Init(uint32_t * usb_id, const struct pios_usb_cfg * cfg) +int32_t PIOS_USB_Init(uint32_t *usb_id, const struct pios_usb_cfg *cfg) { - PIOS_Assert(usb_id); - PIOS_Assert(cfg); + PIOS_Assert(usb_id); + PIOS_Assert(cfg); - struct pios_usb_dev * usb_dev; + struct pios_usb_dev *usb_dev; - usb_dev = (struct pios_usb_dev *) PIOS_USB_alloc(); - if (!usb_dev) goto out_fail; + usb_dev = (struct pios_usb_dev *)PIOS_USB_alloc(); + if (!usb_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usb_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + usb_dev->cfg = cfg; - PIOS_USB_Reenumerate(); + PIOS_USB_Reenumerate(); - /* - * This is a horrible hack to make this available to - * the interrupt callbacks. This should go away ASAP. - */ - pios_usb_com_id = (uint32_t) usb_dev; + /* + * This is a horrible hack to make this available to + * the interrupt callbacks. This should go away ASAP. + */ + pios_usb_com_id = (uint32_t)usb_dev; - /* Enable the USB Interrupts */ - NVIC_Init(&usb_dev->cfg->irq.init); + /* Enable the USB Interrupts */ + NVIC_Init(&usb_dev->cfg->irq.init); - /* Select USBCLK source */ - RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_1Div5); - /* Enable the USB clock */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USB, ENABLE); + /* Select USBCLK source */ + RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_1Div5); + /* Enable the USB clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_USB, ENABLE); - USB_Init(); - USB_SIL_Init(); + USB_Init(); + USB_SIL_Init(); - *usb_id = (uint32_t) usb_dev; + *usb_id = (uint32_t)usb_dev; - return 0; /* No error */ + return 0; /* No error */ out_fail: - return(-1); + return -1; } /** @@ -151,71 +157,72 @@ out_fail: */ int32_t PIOS_USB_ChangeConnectionState(bool Connected) { - // In all cases: re-initialise USB HID driver - if (Connected) { - transfer_possible = true; + // In all cases: re-initialise USB HID driver + if (Connected) { + transfer_possible = true; - //TODO: Check SetEPRxValid(ENDP1); + // TODO: Check SetEPRxValid(ENDP1); #ifdef USB_LED_ON - USB_LED_ON; // turn the USB led on + USB_LED_ON; // turn the USB led on #endif - } else { - // Cable disconnected: disable transfers - transfer_possible = false; + } else { + // Cable disconnected: disable transfers + transfer_possible = false; #ifdef USB_LED_OFF - USB_LED_OFF; // turn the USB led off + USB_LED_OFF; // turn the USB led off #endif - } + } - return 0; + return 0; } int32_t PIOS_USB_Reenumerate() { - /* Force USB reset and power-down (this will also release the USB pins for direct GPIO control) */ - _SetCNTR(CNTR_FRES | CNTR_PDWN); + /* Force USB reset and power-down (this will also release the USB pins for direct GPIO control) */ + _SetCNTR(CNTR_FRES | CNTR_PDWN); - /* Using a "dirty" method to force a re-enumeration: */ - /* Force DPM (Pin PA12) low for ca. 10 mS before USB Tranceiver will be enabled */ - /* This overrules the external Pull-Up at PA12, and at least Windows & MacOS will enumerate again */ - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_Init(GPIOA, &GPIO_InitStructure); + /* Using a "dirty" method to force a re-enumeration: */ + /* Force DPM (Pin PA12) low for ca. 10 mS before USB Tranceiver will be enabled */ + /* This overrules the external Pull-Up at PA12, and at least Windows & MacOS will enumerate again */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_Init(GPIOA, &GPIO_InitStructure); - PIOS_DELAY_WaitmS(50); + PIOS_DELAY_WaitmS(50); - /* Release power-down, still hold reset */ - _SetCNTR(CNTR_PDWN); - PIOS_DELAY_WaituS(5); + /* Release power-down, still hold reset */ + _SetCNTR(CNTR_PDWN); + PIOS_DELAY_WaituS(5); - /* CNTR_FRES = 0 */ - _SetCNTR(0); + /* CNTR_FRES = 0 */ + _SetCNTR(0); - /* Clear pending interrupts */ - _SetISTR(0); + /* Clear pending interrupts */ + _SetISTR(0); - /* Configure USB clock */ - /* USBCLK = PLLCLK / 1.5 */ - RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_1Div5); - /* Enable USB clock */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USB, ENABLE); + /* Configure USB clock */ + /* USBCLK = PLLCLK / 1.5 */ + RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_1Div5); + /* Enable USB clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_USB, ENABLE); - return 0; + return 0; } bool PIOS_USB_CableConnected(__attribute__((unused)) uint8_t id) { - struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_com_id; + struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_com_id; - if (PIOS_USB_validate(usb_dev) != 0) - return false; + if (PIOS_USB_validate(usb_dev) != 0) { + return false; + } - return ((usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin) != 0) ^ usb_dev->cfg->vsense_active_low; + return ((usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin) != 0) ^ usb_dev->cfg->vsense_active_low; } /** @@ -226,12 +233,13 @@ bool PIOS_USB_CableConnected(__attribute__((unused)) uint8_t id) */ bool PIOS_USB_CheckAvailable(uint32_t id) { - struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_com_id; + struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_com_id; - if (PIOS_USB_validate(usb_dev) != 0) - return false; + if (PIOS_USB_validate(usb_dev) != 0) { + return false; + } - return PIOS_USB_CableConnected(id) && transfer_possible; + return PIOS_USB_CableConnected(id) && transfer_possible; } #endif /* PIOS_INCLUDE_USB_HID */ diff --git a/flight/pios/stm32f10x/pios_usb_cdc.c b/flight/pios/stm32f10x/pios_usb_cdc.c index 2d470dee9..9d3724306 100644 --- a/flight/pios/stm32f10x/pios_usb_cdc.c +++ b/flight/pios/stm32f10x/pios_usb_cdc.c @@ -45,68 +45,70 @@ static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail); static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usb_cdc_com_driver = { - .tx_start = PIOS_USB_CDC_TxStart, - .rx_start = PIOS_USB_CDC_RxStart, - .bind_tx_cb = PIOS_USB_CDC_RegisterTxCallback, - .bind_rx_cb = PIOS_USB_CDC_RegisterRxCallback, - .available = PIOS_USB_CheckAvailable, + .tx_start = PIOS_USB_CDC_TxStart, + .rx_start = PIOS_USB_CDC_RxStart, + .bind_tx_cb = PIOS_USB_CDC_RegisterTxCallback, + .bind_rx_cb = PIOS_USB_CDC_RegisterRxCallback, + .available = PIOS_USB_CheckAvailable, }; enum pios_usb_cdc_dev_magic { - PIOS_USB_CDC_DEV_MAGIC = 0xAABBCCDD, + PIOS_USB_CDC_DEV_MAGIC = 0xAABBCCDD, }; struct pios_usb_cdc_dev { - enum pios_usb_cdc_dev_magic magic; - const struct pios_usb_cdc_cfg * cfg; + enum pios_usb_cdc_dev_magic magic; + const struct pios_usb_cdc_cfg *cfg; - uint32_t lower_id; + uint32_t lower_id; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; - uint8_t rx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; - uint8_t tx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; + uint8_t rx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; + uint8_t tx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; - uint32_t rx_dropped; - uint32_t rx_oversize; + uint32_t rx_dropped; + uint32_t rx_oversize; }; -static bool PIOS_USB_CDC_validate(struct pios_usb_cdc_dev * usb_cdc_dev) +static bool PIOS_USB_CDC_validate(struct pios_usb_cdc_dev *usb_cdc_dev) { - return (usb_cdc_dev->magic == PIOS_USB_CDC_DEV_MAGIC); + return usb_cdc_dev->magic == PIOS_USB_CDC_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_usb_cdc_dev * PIOS_USB_CDC_alloc(void) +static struct pios_usb_cdc_dev *PIOS_USB_CDC_alloc(void) { - struct pios_usb_cdc_dev * usb_cdc_dev; + struct pios_usb_cdc_dev *usb_cdc_dev; - usb_cdc_dev = (struct pios_usb_cdc_dev *)pvPortMalloc(sizeof(*usb_cdc_dev)); - if (!usb_cdc_dev) return(NULL); + usb_cdc_dev = (struct pios_usb_cdc_dev *)pvPortMalloc(sizeof(*usb_cdc_dev)); + if (!usb_cdc_dev) { + return NULL; + } - usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; - return(usb_cdc_dev); + usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; + return usb_cdc_dev; } #else static struct pios_usb_cdc_dev pios_usb_cdc_devs[PIOS_USB_CDC_MAX_DEVS]; static uint8_t pios_usb_cdc_num_devs; -static struct pios_usb_cdc_dev * PIOS_USB_CDC_alloc(void) +static struct pios_usb_cdc_dev *PIOS_USB_CDC_alloc(void) { - struct pios_usb_cdc_dev * usb_cdc_dev; + struct pios_usb_cdc_dev *usb_cdc_dev; - if (pios_usb_cdc_num_devs >= PIOS_USB_CDC_MAX_DEVS) { - return (NULL); - } + if (pios_usb_cdc_num_devs >= PIOS_USB_CDC_MAX_DEVS) { + return NULL; + } - usb_cdc_dev = &pios_usb_cdc_devs[pios_usb_cdc_num_devs++]; - usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; + usb_cdc_dev = &pios_usb_cdc_devs[pios_usb_cdc_num_devs++]; + usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; - return (usb_cdc_dev); + return usb_cdc_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ static void PIOS_USB_CDC_DATA_EP_IN_Callback(void); static void PIOS_USB_CDC_DATA_EP_OUT_Callback(void); @@ -115,304 +117,316 @@ static void PIOS_USB_CDC_CTRL_EP_IN_Callback(void); static uint32_t pios_usb_cdc_id; /* Need a better way to pull these in */ -extern void (*pEpInt_IN[7])(void); -extern void (*pEpInt_OUT[7])(void); +extern void(*pEpInt_IN[7]) (void); +extern void(*pEpInt_OUT[7]) (void); -int32_t PIOS_USB_CDC_Init(uint32_t * usbcdc_id, const struct pios_usb_cdc_cfg * cfg, uint32_t lower_id) +int32_t PIOS_USB_CDC_Init(uint32_t *usbcdc_id, const struct pios_usb_cdc_cfg *cfg, uint32_t lower_id) { - PIOS_Assert(usbcdc_id); - PIOS_Assert(cfg); + PIOS_Assert(usbcdc_id); + PIOS_Assert(cfg); - struct pios_usb_cdc_dev * usb_cdc_dev; + struct pios_usb_cdc_dev *usb_cdc_dev; - usb_cdc_dev = (struct pios_usb_cdc_dev *) PIOS_USB_CDC_alloc(); - if (!usb_cdc_dev) goto out_fail; + usb_cdc_dev = (struct pios_usb_cdc_dev *)PIOS_USB_CDC_alloc(); + if (!usb_cdc_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usb_cdc_dev->cfg = cfg; - usb_cdc_dev->lower_id = lower_id; + /* Bind the configuration to the device instance */ + usb_cdc_dev->cfg = cfg; + usb_cdc_dev->lower_id = lower_id; - pios_usb_cdc_id = (uint32_t) usb_cdc_dev; + pios_usb_cdc_id = (uint32_t)usb_cdc_dev; - /* Bind lower level callbacks into the USB infrastructure */ - pEpInt_OUT[cfg->ctrl_tx_ep - 1] = PIOS_USB_CDC_CTRL_EP_IN_Callback; - pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_CDC_DATA_EP_IN_Callback; - pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_CDC_DATA_EP_OUT_Callback; + /* Bind lower level callbacks into the USB infrastructure */ + pEpInt_OUT[cfg->ctrl_tx_ep - 1] = PIOS_USB_CDC_CTRL_EP_IN_Callback; + pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_CDC_DATA_EP_IN_Callback; + pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_CDC_DATA_EP_OUT_Callback; - *usbcdc_id = (uint32_t) usb_cdc_dev; + *usbcdc_id = (uint32_t)usb_cdc_dev; - return 0; + return 0; out_fail: - return -1; + return -1; } - static void PIOS_USB_CDC_RegisterRxCallback(uint32_t usbcdc_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_cdc_dev->rx_in_context = context; - usb_cdc_dev->rx_in_cb = rx_in_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->rx_in_context = context; + usb_cdc_dev->rx_in_cb = rx_in_cb; } static void PIOS_USB_CDC_RegisterTxCallback(uint32_t usbcdc_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_cdc_dev->tx_out_context = context; - usb_cdc_dev->tx_out_cb = tx_out_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->tx_out_context = context; + usb_cdc_dev->tx_out_cb = tx_out_cb; } -static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); - - if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { - return; - } - - // If endpoint was stalled and there is now space make it valid - PIOS_IRQ_Disable(); - if ((GetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep) != EP_RX_VALID) && - (rx_bytes_avail >= sizeof(usb_cdc_dev->rx_packet_buffer))) { - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); - } - PIOS_IRQ_Enable(); -} - -static void PIOS_USB_CDC_SendData(struct pios_usb_cdc_dev * usb_cdc_dev) +static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail) { - uint16_t bytes_to_tx; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - if (!usb_cdc_dev->tx_out_cb) { - return; - } + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - bool need_yield = false; - bytes_to_tx = (usb_cdc_dev->tx_out_cb)(usb_cdc_dev->tx_out_context, - usb_cdc_dev->tx_packet_buffer, - sizeof(usb_cdc_dev->tx_packet_buffer), - NULL, - &need_yield); - if (bytes_to_tx == 0) { - return; - } + PIOS_Assert(valid); - UserToPMABufferCopy(usb_cdc_dev->tx_packet_buffer, - GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), - bytes_to_tx); - SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, bytes_to_tx); - SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); + if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { + return; + } + + // If endpoint was stalled and there is now space make it valid + PIOS_IRQ_Disable(); + if ((GetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep) != EP_RX_VALID) && + (rx_bytes_avail >= sizeof(usb_cdc_dev->rx_packet_buffer))) { + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); + } + PIOS_IRQ_Enable(); +} + +static void PIOS_USB_CDC_SendData(struct pios_usb_cdc_dev *usb_cdc_dev) +{ + uint16_t bytes_to_tx; + + if (!usb_cdc_dev->tx_out_cb) { + return; + } + + bool need_yield = false; + bytes_to_tx = (usb_cdc_dev->tx_out_cb)(usb_cdc_dev->tx_out_context, + usb_cdc_dev->tx_packet_buffer, + sizeof(usb_cdc_dev->tx_packet_buffer), + NULL, + &need_yield); + if (bytes_to_tx == 0) { + return; + } + + UserToPMABufferCopy(usb_cdc_dev->tx_packet_buffer, + GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), + bytes_to_tx); + SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, bytes_to_tx); + SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); #if defined(PIOS_INCLUDE_FREERTOS) - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, __attribute__((unused)) uint16_t tx_bytes_avail) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { - return; - } + PIOS_Assert(valid); - if (GetEPTxStatus(usb_cdc_dev->cfg->data_tx_ep) == EP_TX_VALID) { - /* Endpoint is already transmitting */ - return; - } + if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { + return; + } - PIOS_USB_CDC_SendData(usb_cdc_dev); + if (GetEPTxStatus(usb_cdc_dev->cfg->data_tx_ep) == EP_TX_VALID) { + /* Endpoint is already transmitting */ + return; + } + + PIOS_USB_CDC_SendData(usb_cdc_dev); } static void PIOS_USB_CDC_DATA_EP_IN_Callback(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_USB_CDC_SendData(usb_cdc_dev); + PIOS_Assert(valid); + + PIOS_USB_CDC_SendData(usb_cdc_dev); } static void PIOS_USB_CDC_DATA_EP_OUT_Callback(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - uint32_t DataLength; + PIOS_Assert(valid); - /* Get the number of received data on the selected Endpoint */ - DataLength = GetEPRxCount(usb_cdc_dev->cfg->data_rx_ep); - if (DataLength > sizeof(usb_cdc_dev->rx_packet_buffer)) { - usb_cdc_dev->rx_oversize++; - DataLength = sizeof(usb_cdc_dev->rx_packet_buffer); - } + uint32_t DataLength; - /* Use the memory interface function to read from the selected endpoint */ - PMAToUserBufferCopy((uint8_t *) usb_cdc_dev->rx_packet_buffer, - GetEPRxAddr(usb_cdc_dev->cfg->data_rx_ep), - DataLength); + /* Get the number of received data on the selected Endpoint */ + DataLength = GetEPRxCount(usb_cdc_dev->cfg->data_rx_ep); + if (DataLength > sizeof(usb_cdc_dev->rx_packet_buffer)) { + usb_cdc_dev->rx_oversize++; + DataLength = sizeof(usb_cdc_dev->rx_packet_buffer); + } - if (!usb_cdc_dev->rx_in_cb) { - /* No Rx call back registered, disable the receiver */ - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); - return; - } + /* Use the memory interface function to read from the selected endpoint */ + PMAToUserBufferCopy((uint8_t *)usb_cdc_dev->rx_packet_buffer, + GetEPRxAddr(usb_cdc_dev->cfg->data_rx_ep), + DataLength); - uint16_t headroom; - bool need_yield = false; - uint16_t rc; - rc = (usb_cdc_dev->rx_in_cb)(usb_cdc_dev->rx_in_context, - usb_cdc_dev->rx_packet_buffer, - DataLength, - &headroom, - &need_yield); + if (!usb_cdc_dev->rx_in_cb) { + /* No Rx call back registered, disable the receiver */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); + return; + } - if (rc < DataLength) { - /* Lost bytes on rx */ - usb_cdc_dev->rx_dropped += (DataLength - rc); - } + uint16_t headroom; + bool need_yield = false; + uint16_t rc; + rc = (usb_cdc_dev->rx_in_cb)(usb_cdc_dev->rx_in_context, + usb_cdc_dev->rx_packet_buffer, + DataLength, + &headroom, + &need_yield); - if (headroom >= sizeof(usb_cdc_dev->rx_packet_buffer)) { - /* We have room for a maximum length message */ - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); - } else { - /* Not enough room left for a message, apply backpressure */ - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); - } + if (rc < DataLength) { + /* Lost bytes on rx */ + usb_cdc_dev->rx_dropped += (DataLength - rc); + } + + if (headroom >= sizeof(usb_cdc_dev->rx_packet_buffer)) { + /* We have room for a maximum length message */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); + } else { + /* Not enough room left for a message, apply backpressure */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } static uint16_t control_line_state; RESULT PIOS_USB_CDC_SetControlLineState(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (!valid) { - /* No CDC interface is configured */ - return USB_UNSUPPORT; - } + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - uint8_t wValue0 = pInformation->USBwValue0; - uint8_t wValue1 = pInformation->USBwValue1; + if (!valid) { + /* No CDC interface is configured */ + return USB_UNSUPPORT; + } - control_line_state = wValue1 << 8 | wValue0; + uint8_t wValue0 = pInformation->USBwValue0; + uint8_t wValue1 = pInformation->USBwValue1; - return USB_SUCCESS; + control_line_state = wValue1 << 8 | wValue0; + + return USB_SUCCESS; } static struct usb_cdc_line_coding line_coding = { - .dwDTERate = htousbl(57600), - .bCharFormat = USB_CDC_LINE_CODING_STOP_1, - .bParityType = USB_CDC_LINE_CODING_PARITY_NONE, - .bDataBits = 8, + .dwDTERate = htousbl(57600), + .bCharFormat = USB_CDC_LINE_CODING_STOP_1, + .bParityType = USB_CDC_LINE_CODING_PARITY_NONE, + .bDataBits = 8, }; uint8_t *PIOS_USB_CDC_SetLineCoding(uint16_t Length) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (!valid) { - /* No CDC interface is configured */ - return NULL; - } + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (Length == 0) { - /* Report the number of bytes we're prepared to consume */ - pInformation->Ctrl_Info.Usb_wLength = sizeof(line_coding); - pInformation->Ctrl_Info.Usb_rLength = sizeof(line_coding); - return NULL; - } else { - /* Give out a pointer to the data struct */ - return ((uint8_t *) &line_coding); - } + if (!valid) { + /* No CDC interface is configured */ + return NULL; + } + + if (Length == 0) { + /* Report the number of bytes we're prepared to consume */ + pInformation->Ctrl_Info.Usb_wLength = sizeof(line_coding); + pInformation->Ctrl_Info.Usb_rLength = sizeof(line_coding); + return NULL; + } else { + /* Give out a pointer to the data struct */ + return (uint8_t *)&line_coding; + } } const uint8_t *PIOS_USB_CDC_GetLineCoding(uint16_t Length) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (!valid) { - /* No CDC interface is configured */ - return NULL; - } + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (Length == 0) { - pInformation->Ctrl_Info.Usb_wLength = sizeof(line_coding); - return NULL; - } else { - return ((uint8_t *) &line_coding); - } + if (!valid) { + /* No CDC interface is configured */ + return NULL; + } + + if (Length == 0) { + pInformation->Ctrl_Info.Usb_wLength = sizeof(line_coding); + return NULL; + } else { + return (uint8_t *)&line_coding; + } } struct usb_cdc_serial_state_report uart_state = { - .bmRequestType = 0xA1, - .bNotification = USB_CDC_NOTIFICATION_SERIAL_STATE, - .wValue = 0, - .wIndex = htousbs(1), - .wLength = htousbs(2), - .bmUartState = htousbs(0), + .bmRequestType = 0xA1, + .bNotification = USB_CDC_NOTIFICATION_SERIAL_STATE, + .wValue = 0, + .wIndex = htousbs(1), + .wLength = htousbs(2), + .bmUartState = htousbs(0), }; - + static void PIOS_USB_CDC_CTRL_EP_IN_Callback(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - /* Give back UART State Bitmap */ - /* UART State Bitmap - * 15-7: reserved - * 6: bOverRun overrun error - * 5: bParity parity error - * 4: bFraming framing error - * 3: bRingSignal RI - * 2: bBreak break reception - * 1: bTxCarrier DSR - * 0: bRxCarrier DCD - */ - uart_state.bmUartState = htousbs(0x0003); + PIOS_Assert(valid); - UserToPMABufferCopy((uint8_t *) &uart_state, - GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), - sizeof(uart_state)); - SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, PIOS_USB_BOARD_CDC_MGMT_LENGTH); - SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); + /* Give back UART State Bitmap */ + /* UART State Bitmap + * 15-7: reserved + * 6: bOverRun overrun error + * 5: bParity parity error + * 4: bFraming framing error + * 3: bRingSignal RI + * 2: bBreak break reception + * 1: bTxCarrier DSR + * 0: bRxCarrier DCD + */ + uart_state.bmUartState = htousbs(0x0003); + + UserToPMABufferCopy((uint8_t *)&uart_state, + GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), + sizeof(uart_state)); + SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, PIOS_USB_BOARD_CDC_MGMT_LENGTH); + SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); } #endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/pios/stm32f10x/pios_usb_hid.c b/flight/pios/stm32f10x/pios_usb_hid.c index 2656c3dce..44db1d902 100644 --- a/flight/pios/stm32f10x/pios_usb_hid.c +++ b/flight/pios/stm32f10x/pios_usb_hid.c @@ -45,68 +45,70 @@ static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, uint16_t tx_bytes_avail); static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usb_hid_com_driver = { - .tx_start = PIOS_USB_HID_TxStart, - .rx_start = PIOS_USB_HID_RxStart, - .bind_tx_cb = PIOS_USB_HID_RegisterTxCallback, - .bind_rx_cb = PIOS_USB_HID_RegisterRxCallback, - .available = PIOS_USB_CheckAvailable, + .tx_start = PIOS_USB_HID_TxStart, + .rx_start = PIOS_USB_HID_RxStart, + .bind_tx_cb = PIOS_USB_HID_RegisterTxCallback, + .bind_rx_cb = PIOS_USB_HID_RegisterRxCallback, + .available = PIOS_USB_CheckAvailable, }; enum pios_usb_hid_dev_magic { - PIOS_USB_HID_DEV_MAGIC = 0xAA00BB00, + PIOS_USB_HID_DEV_MAGIC = 0xAA00BB00, }; struct pios_usb_hid_dev { - enum pios_usb_hid_dev_magic magic; - const struct pios_usb_hid_cfg * cfg; + enum pios_usb_hid_dev_magic magic; + const struct pios_usb_hid_cfg *cfg; - uint32_t lower_id; + uint32_t lower_id; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; - uint8_t rx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; - uint8_t tx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; + uint8_t rx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; + uint8_t tx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; - uint32_t rx_dropped; - uint32_t rx_oversize; + uint32_t rx_dropped; + uint32_t rx_oversize; }; -static bool PIOS_USB_HID_validate(struct pios_usb_hid_dev * usb_hid_dev) +static bool PIOS_USB_HID_validate(struct pios_usb_hid_dev *usb_hid_dev) { - return (usb_hid_dev->magic == PIOS_USB_HID_DEV_MAGIC); + return usb_hid_dev->magic == PIOS_USB_HID_DEV_MAGIC; } #ifdef PIOS_INCLUDE_FREERTOS -static struct pios_usb_hid_dev * PIOS_USB_HID_alloc(void) +static struct pios_usb_hid_dev *PIOS_USB_HID_alloc(void) { - struct pios_usb_hid_dev * usb_hid_dev; + struct pios_usb_hid_dev *usb_hid_dev; - usb_hid_dev = (struct pios_usb_hid_dev *)pvPortMalloc(sizeof(*usb_hid_dev)); - if (!usb_hid_dev) return(NULL); + usb_hid_dev = (struct pios_usb_hid_dev *)pvPortMalloc(sizeof(*usb_hid_dev)); + if (!usb_hid_dev) { + return NULL; + } - usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; - return(usb_hid_dev); + usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; + return usb_hid_dev; } #else static struct pios_usb_hid_dev pios_usb_hid_devs[PIOS_USB_HID_MAX_DEVS]; static uint8_t pios_usb_hid_num_devs; -static struct pios_usb_hid_dev * PIOS_USB_HID_alloc(void) +static struct pios_usb_hid_dev *PIOS_USB_HID_alloc(void) { - struct pios_usb_hid_dev * usb_hid_dev; + struct pios_usb_hid_dev *usb_hid_dev; - if (pios_usb_hid_num_devs >= PIOS_USB_HID_MAX_DEVS) { - return (NULL); - } + if (pios_usb_hid_num_devs >= PIOS_USB_HID_MAX_DEVS) { + return NULL; + } - usb_hid_dev = &pios_usb_hid_devs[pios_usb_hid_num_devs++]; - usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; + usb_hid_dev = &pios_usb_hid_devs[pios_usb_hid_num_devs++]; + usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; - return (usb_hid_dev); + return usb_hid_dev; } -#endif +#endif /* ifdef PIOS_INCLUDE_FREERTOS */ static void PIOS_USB_HID_EP_IN_Callback(void); static void PIOS_USB_HID_EP_OUT_Callback(void); @@ -114,162 +116,167 @@ static void PIOS_USB_HID_EP_OUT_Callback(void); static uint32_t pios_usb_hid_id; /* Need a better way to pull these in */ -extern void (*pEpInt_IN[7])(void); -extern void (*pEpInt_OUT[7])(void); +extern void(*pEpInt_IN[7]) (void); +extern void(*pEpInt_OUT[7]) (void); -int32_t PIOS_USB_HID_Init(uint32_t * usbhid_id, const struct pios_usb_hid_cfg * cfg, uint32_t lower_id) +int32_t PIOS_USB_HID_Init(uint32_t *usbhid_id, const struct pios_usb_hid_cfg *cfg, uint32_t lower_id) { - PIOS_Assert(usbhid_id); - PIOS_Assert(cfg); + PIOS_Assert(usbhid_id); + PIOS_Assert(cfg); - struct pios_usb_hid_dev * usb_hid_dev; + struct pios_usb_hid_dev *usb_hid_dev; - usb_hid_dev = (struct pios_usb_hid_dev *) PIOS_USB_HID_alloc(); - if (!usb_hid_dev) goto out_fail; + usb_hid_dev = (struct pios_usb_hid_dev *)PIOS_USB_HID_alloc(); + if (!usb_hid_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usb_hid_dev->cfg = cfg; - usb_hid_dev->lower_id = lower_id; + /* Bind the configuration to the device instance */ + usb_hid_dev->cfg = cfg; + usb_hid_dev->lower_id = lower_id; - pios_usb_hid_id = (uint32_t) usb_hid_dev; + pios_usb_hid_id = (uint32_t)usb_hid_dev; - /* Bind lower level callbacks into the USB infrastructure */ - pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_HID_EP_IN_Callback; - pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_HID_EP_OUT_Callback; + /* Bind lower level callbacks into the USB infrastructure */ + pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_HID_EP_IN_Callback; + pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_HID_EP_OUT_Callback; - *usbhid_id = (uint32_t) usb_hid_dev; + *usbhid_id = (uint32_t)usb_hid_dev; - return 0; + return 0; out_fail: - return -1; + return -1; } - - -static void PIOS_USB_HID_SendReport(struct pios_usb_hid_dev * usb_hid_dev) +static void PIOS_USB_HID_SendReport(struct pios_usb_hid_dev *usb_hid_dev) { - uint16_t bytes_to_tx; + uint16_t bytes_to_tx; - if (!usb_hid_dev->tx_out_cb) { - return; - } + if (!usb_hid_dev->tx_out_cb) { + return; + } - bool need_yield = false; + bool need_yield = false; #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, - &usb_hid_dev->tx_packet_buffer[1], - sizeof(usb_hid_dev->tx_packet_buffer)-1, - NULL, - &need_yield); + bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, + &usb_hid_dev->tx_packet_buffer[1], + sizeof(usb_hid_dev->tx_packet_buffer) - 1, + NULL, + &need_yield); #else - bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, - &usb_hid_dev->tx_packet_buffer[2], - sizeof(usb_hid_dev->tx_packet_buffer)-2, - NULL, - &need_yield); + bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, + &usb_hid_dev->tx_packet_buffer[2], + sizeof(usb_hid_dev->tx_packet_buffer) - 2, + NULL, + &need_yield); #endif - if (bytes_to_tx == 0) { - return; - } + if (bytes_to_tx == 0) { + return; + } - /* Always set type as report ID */ - usb_hid_dev->tx_packet_buffer[0] = 1; + /* Always set type as report ID */ + usb_hid_dev->tx_packet_buffer[0] = 1; #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - UserToPMABufferCopy(usb_hid_dev->tx_packet_buffer, - GetEPTxAddr(usb_hid_dev->cfg->data_tx_ep), - bytes_to_tx + 1); + UserToPMABufferCopy(usb_hid_dev->tx_packet_buffer, + GetEPTxAddr(usb_hid_dev->cfg->data_tx_ep), + bytes_to_tx + 1); #else - usb_hid_dev->tx_packet_buffer[1] = bytes_to_tx; - UserToPMABufferCopy(usb_hid_dev->tx_packet_buffer, - GetEPTxAddr(usb_hid_dev->cfg->data_tx_ep), - bytes_to_tx + 2); + usb_hid_dev->tx_packet_buffer[1] = bytes_to_tx; + UserToPMABufferCopy(usb_hid_dev->tx_packet_buffer, + GetEPTxAddr(usb_hid_dev->cfg->data_tx_ep), + bytes_to_tx + 2); #endif - /* Is this correct? Why do we always send the whole buffer? */ - SetEPTxCount(usb_hid_dev->cfg->data_tx_ep, sizeof(usb_hid_dev->tx_packet_buffer)); - SetEPTxValid(usb_hid_dev->cfg->data_tx_ep); + /* Is this correct? Why do we always send the whole buffer? */ + SetEPTxCount(usb_hid_dev->cfg->data_tx_ep, sizeof(usb_hid_dev->tx_packet_buffer)); + SetEPTxValid(usb_hid_dev->cfg->data_tx_ep); #ifdef PIOS_INCLUDE_FREERTOS - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } -static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; +static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) +{ + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { - return; - } + PIOS_Assert(valid); - // If endpoint was stalled and there is now space make it valid + if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { + return; + } + + // If endpoint was stalled and there is now space make it valid #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; #else - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; #endif - PIOS_IRQ_Disable(); - if ((GetEPRxStatus(usb_hid_dev->cfg->data_rx_ep) != EP_RX_VALID) && - (rx_bytes_avail >= max_payload_length)) { - SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_VALID); - } - PIOS_IRQ_Enable(); + PIOS_IRQ_Disable(); + if ((GetEPRxStatus(usb_hid_dev->cfg->data_rx_ep) != EP_RX_VALID) && + (rx_bytes_avail >= max_payload_length)) { + SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_VALID); + } + PIOS_IRQ_Enable(); } static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, __attribute__((unused)) uint16_t tx_bytes_avail) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { - return; - } + PIOS_Assert(valid); - if (GetEPTxStatus(usb_hid_dev->cfg->data_tx_ep) == EP_TX_VALID) { - /* Endpoint is already transmitting */ - return; - } + if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { + return; + } - PIOS_USB_HID_SendReport(usb_hid_dev); + if (GetEPTxStatus(usb_hid_dev->cfg->data_tx_ep) == EP_TX_VALID) { + /* Endpoint is already transmitting */ + return; + } + + PIOS_USB_HID_SendReport(usb_hid_dev); } static void PIOS_USB_HID_RegisterRxCallback(uint32_t usbhid_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_hid_dev->rx_in_context = context; - usb_hid_dev->rx_in_cb = rx_in_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_hid_dev->rx_in_context = context; + usb_hid_dev->rx_in_cb = rx_in_cb; } static void PIOS_USB_HID_RegisterTxCallback(uint32_t usbhid_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_hid_dev->tx_out_context = context; - usb_hid_dev->tx_out_cb = tx_out_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_hid_dev->tx_out_context = context; + usb_hid_dev->tx_out_cb = tx_out_cb; } /** @@ -278,16 +285,17 @@ static void PIOS_USB_HID_RegisterTxCallback(uint32_t usbhid_id, pios_com_callbac */ static void PIOS_USB_HID_EP_IN_Callback(void) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)pios_usb_hid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)pios_usb_hid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { - return; - } + PIOS_Assert(valid); - PIOS_USB_HID_SendReport(usb_hid_dev); + if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { + return; + } + + PIOS_USB_HID_SendReport(usb_hid_dev); } /** @@ -295,68 +303,68 @@ static void PIOS_USB_HID_EP_IN_Callback(void) */ static void PIOS_USB_HID_EP_OUT_Callback(void) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)pios_usb_hid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)pios_usb_hid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - uint32_t DataLength; + PIOS_Assert(valid); - /* Read received data (63 bytes) */ - /* Get the number of received data on the selected Endpoint */ - DataLength = GetEPRxCount(usb_hid_dev->cfg->data_rx_ep); - if (DataLength > sizeof(usb_hid_dev->rx_packet_buffer)) { - DataLength = sizeof(usb_hid_dev->rx_packet_buffer); - } + uint32_t DataLength; - /* Use the memory interface function to read from the selected endpoint */ - PMAToUserBufferCopy((uint8_t *) usb_hid_dev->rx_packet_buffer, - GetEPRxAddr(usb_hid_dev->cfg->data_rx_ep), - DataLength); + /* Read received data (63 bytes) */ + /* Get the number of received data on the selected Endpoint */ + DataLength = GetEPRxCount(usb_hid_dev->cfg->data_rx_ep); + if (DataLength > sizeof(usb_hid_dev->rx_packet_buffer)) { + DataLength = sizeof(usb_hid_dev->rx_packet_buffer); + } - if (!usb_hid_dev->rx_in_cb) { - /* No Rx call back registered, disable the receiver */ - SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_NAK); - return; - } + /* Use the memory interface function to read from the selected endpoint */ + PMAToUserBufferCopy((uint8_t *)usb_hid_dev->rx_packet_buffer, + GetEPRxAddr(usb_hid_dev->cfg->data_rx_ep), + DataLength); - /* The first byte is report ID (not checked), the second byte is the valid data length */ - uint16_t headroom; - bool need_yield = false; + if (!usb_hid_dev->rx_in_cb) { + /* No Rx call back registered, disable the receiver */ + SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_NAK); + return; + } + + /* The first byte is report ID (not checked), the second byte is the valid data length */ + uint16_t headroom; + bool need_yield = false; #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, - &usb_hid_dev->rx_packet_buffer[1], - sizeof(usb_hid_dev->rx_packet_buffer)-1, - &headroom, - &need_yield); + (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, + &usb_hid_dev->rx_packet_buffer[1], + sizeof(usb_hid_dev->rx_packet_buffer) - 1, + &headroom, + &need_yield); #else - (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, - &usb_hid_dev->rx_packet_buffer[2], - usb_hid_dev->rx_packet_buffer[1], - &headroom, - &need_yield); + (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, + &usb_hid_dev->rx_packet_buffer[2], + usb_hid_dev->rx_packet_buffer[1], + &headroom, + &need_yield); #endif #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; #else - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; #endif - if (headroom >= max_payload_length) { - - /* We have room for a maximum length message */ - SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_VALID); - } else { - /* Not enough room left for a message, apply backpressure */ - SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_NAK); - } + if (headroom >= max_payload_length) { + /* We have room for a maximum length message */ + SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_VALID); + } else { + /* Not enough room left for a message, apply backpressure */ + SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_NAK); + } #ifdef PIOS_INCLUDE_FREERTOS - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ diff --git a/flight/pios/stm32f10x/pios_usb_hid_istr.c b/flight/pios/stm32f10x/pios_usb_hid_istr.c index e26dd0b4f..30492c02f 100644 --- a/flight/pios/stm32f10x/pios_usb_hid_istr.c +++ b/flight/pios/stm32f10x/pios_usb_hid_istr.c @@ -27,18 +27,20 @@ /* Private define ------------------------------------------------------------*/ /* Private macro -------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ -__IO uint16_t wIstr; /* ISTR register last read value */ -__IO uint8_t bIntPackSOF = 0; /* SOFs received between 2 consecutive packets */ +__IO uint16_t wIstr; /* ISTR register last read value */ +__IO uint8_t bIntPackSOF = 0; /* SOFs received between 2 consecutive packets */ /* Extern variables ----------------------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/ /* Private functions ---------------------------------------------------------*/ /* function pointers to non-control endpoints service routines */ -void (*pEpInt_IN[7]) (void) = { -EP1_IN_Callback, EP2_IN_Callback, EP3_IN_Callback, EP4_IN_Callback, EP5_IN_Callback, EP6_IN_Callback, EP7_IN_Callback,}; +void(*pEpInt_IN[7]) (void) = { + EP1_IN_Callback, EP2_IN_Callback, EP3_IN_Callback, EP4_IN_Callback, EP5_IN_Callback, EP6_IN_Callback, EP7_IN_Callback, +}; -void (*pEpInt_OUT[7]) (void) = { -EP1_OUT_Callback, EP2_OUT_Callback, EP3_OUT_Callback, EP4_OUT_Callback, EP5_OUT_Callback, EP6_OUT_Callback, EP7_OUT_Callback,}; +void(*pEpInt_OUT[7]) (void) = { + EP1_OUT_Callback, EP2_OUT_Callback, EP3_OUT_Callback, EP4_OUT_Callback, EP5_OUT_Callback, EP6_OUT_Callback, EP7_OUT_Callback, +}; #ifndef STM32F10X_CL @@ -49,101 +51,99 @@ EP1_OUT_Callback, EP2_OUT_Callback, EP3_OUT_Callback, EP4_OUT_Callback, EP5_OUT_ * Output : * Return : *******************************************************************************/ -void USB_LP_CAN1_RX0_IRQHandler(void) //USB_Istr(void) +void USB_LP_CAN1_RX0_IRQHandler(void) // USB_Istr(void) { - - wIstr = _GetISTR(); + wIstr = _GetISTR(); #if (IMR_MSK & ISTR_CTR) - if (wIstr & ISTR_CTR & wInterrupt_Mask) { - /* servicing of the endpoint correct transfer interrupt */ - /* clear of the CTR flag into the sub */ - CTR_LP(); + if (wIstr & ISTR_CTR & wInterrupt_Mask) { + /* servicing of the endpoint correct transfer interrupt */ + /* clear of the CTR flag into the sub */ + CTR_LP(); #ifdef CTR_CALLBACK - CTR_Callback(); + CTR_Callback(); #endif - } + } #endif - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #if (IMR_MSK & ISTR_RESET) - if (wIstr & ISTR_RESET & wInterrupt_Mask) { - _SetISTR((uint16_t) CLR_RESET); - Device_Property.Reset(); + if (wIstr & ISTR_RESET & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_RESET); + Device_Property.Reset(); #ifdef RESET_CALLBACK - RESET_Callback(); + RESET_Callback(); #endif - } + } #endif - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #if (IMR_MSK & ISTR_DOVR) - if (wIstr & ISTR_DOVR & wInterrupt_Mask) { - _SetISTR((uint16_t) CLR_DOVR); + if (wIstr & ISTR_DOVR & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_DOVR); #ifdef DOVR_CALLBACK - DOVR_Callback(); + DOVR_Callback(); #endif - } + } #endif - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #if (IMR_MSK & ISTR_ERR) - if (wIstr & ISTR_ERR & wInterrupt_Mask) { - _SetISTR((uint16_t) CLR_ERR); + if (wIstr & ISTR_ERR & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_ERR); #ifdef ERR_CALLBACK - ERR_Callback(); + ERR_Callback(); #endif - } + } #endif - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #if (IMR_MSK & ISTR_WKUP) - if (wIstr & ISTR_WKUP & wInterrupt_Mask) { - _SetISTR((uint16_t) CLR_WKUP); - Resume(RESUME_EXTERNAL); + if (wIstr & ISTR_WKUP & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_WKUP); + Resume(RESUME_EXTERNAL); #ifdef WKUP_CALLBACK - WKUP_Callback(); + WKUP_Callback(); #endif - } + } #endif - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #if (IMR_MSK & ISTR_SUSP) - if (wIstr & ISTR_SUSP & wInterrupt_Mask) { - - /* check if SUSPEND is possible */ - if (fSuspendEnabled) { - Suspend(); - } else { - /* if not possible then resume after xx ms */ - Resume(RESUME_LATER); - } - /* clear of the ISTR bit must be done after setting of CNTR_FSUSP */ - _SetISTR((uint16_t) CLR_SUSP); + if (wIstr & ISTR_SUSP & wInterrupt_Mask) { + /* check if SUSPEND is possible */ + if (fSuspendEnabled) { + Suspend(); + } else { + /* if not possible then resume after xx ms */ + Resume(RESUME_LATER); + } + /* clear of the ISTR bit must be done after setting of CNTR_FSUSP */ + _SetISTR((uint16_t)CLR_SUSP); #ifdef SUSP_CALLBACK - SUSP_Callback(); + SUSP_Callback(); #endif - } + } #endif - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #if (IMR_MSK & ISTR_SOF) - if (wIstr & ISTR_SOF & wInterrupt_Mask) { - _SetISTR((uint16_t) CLR_SOF); - bIntPackSOF++; + if (wIstr & ISTR_SOF & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_SOF); + bIntPackSOF++; #ifdef SOF_CALLBACK - SOF_Callback(); + SOF_Callback(); #endif - } + } #endif - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #if (IMR_MSK & ISTR_ESOF) - if (wIstr & ISTR_ESOF & wInterrupt_Mask) { - _SetISTR((uint16_t) CLR_ESOF); - /* resume handling timing is made with ESOFs */ - Resume(RESUME_ESOF); /* request without change of the machine state */ + if (wIstr & ISTR_ESOF & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_ESOF); + /* resume handling timing is made with ESOFs */ + Resume(RESUME_ESOF); /* request without change of the machine state */ #ifdef ESOF_CALLBACK - ESOF_Callback(); + ESOF_Callback(); #endif - } + } #endif -} /* USB_Istr */ +} /* USB_Istr */ /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ #else /* STM32F10X_CL */ @@ -157,177 +157,176 @@ void USB_LP_CAN1_RX0_IRQHandler(void) //USB_Istr(void) *******************************************************************************/ u32 STM32_PCD_OTG_ISR_Handler(void) { - USB_OTG_GINTSTS_TypeDef gintr_status; - u32 retval = 0; + USB_OTG_GINTSTS_TypeDef gintr_status; + u32 retval = 0; - if (USBD_FS_IsDeviceMode()) { /* ensure that we are in device mode */ - gintr_status.d32 = OTGD_FS_ReadCoreItr(); + if (USBD_FS_IsDeviceMode()) { /* ensure that we are in device mode */ + gintr_status.d32 = OTGD_FS_ReadCoreItr(); - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* If there is no interrupt pending exit the interrupt routine */ - if (!gintr_status.d32) { - return 0; - } + /* If there is no interrupt pending exit the interrupt routine */ + if (!gintr_status.d32) { + return 0; + } - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Early Suspend interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Early Suspend interrupt */ #ifdef INTR_ERLYSUSPEND - if (gintr_status.b.erlysuspend) { - retval |= OTGD_FS_Handle_EarlySuspend_ISR(); - } + if (gintr_status.b.erlysuspend) { + retval |= OTGD_FS_Handle_EarlySuspend_ISR(); + } #endif /* INTR_ERLYSUSPEND */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* End of Periodic Frame interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* End of Periodic Frame interrupt */ #ifdef INTR_EOPFRAME - if (gintr_status.b.eopframe) { - retval |= OTGD_FS_Handle_EOPF_ISR(); - } + if (gintr_status.b.eopframe) { + retval |= OTGD_FS_Handle_EOPF_ISR(); + } #endif /* INTR_EOPFRAME */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Non Periodic Tx FIFO Emty interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Non Periodic Tx FIFO Emty interrupt */ #ifdef INTR_NPTXFEMPTY - if (gintr_status.b.nptxfempty) { - retval |= OTGD_FS_Handle_NPTxFE_ISR(); - } + if (gintr_status.b.nptxfempty) { + retval |= OTGD_FS_Handle_NPTxFE_ISR(); + } #endif /* INTR_NPTXFEMPTY */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Wakeup or RemoteWakeup interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Wakeup or RemoteWakeup interrupt */ #ifdef INTR_WKUPINTR - if (gintr_status.b.wkupintr) { - retval |= OTGD_FS_Handle_Wakeup_ISR(); - } + if (gintr_status.b.wkupintr) { + retval |= OTGD_FS_Handle_Wakeup_ISR(); + } #endif /* INTR_WKUPINTR */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Suspend interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Suspend interrupt */ #ifdef INTR_USBSUSPEND - if (gintr_status.b.usbsuspend) { - /* check if SUSPEND is possible */ - if (fSuspendEnabled) { - Suspend(); - } else { - /* if not possible then resume after xx ms */ - Resume(RESUME_LATER); /* This case shouldn't happen in OTG Device mode because - there's no ESOF interrupt to increment the ResumeS.bESOFcnt in the Resume state machine */ - } + if (gintr_status.b.usbsuspend) { + /* check if SUSPEND is possible */ + if (fSuspendEnabled) { + Suspend(); + } else { + /* if not possible then resume after xx ms */ + Resume(RESUME_LATER); /* This case shouldn't happen in OTG Device mode because + there's no ESOF interrupt to increment the ResumeS.bESOFcnt in the Resume state machine */ + } - retval |= OTGD_FS_Handle_USBSuspend_ISR(); - } + retval |= OTGD_FS_Handle_USBSuspend_ISR(); + } #endif /* INTR_USBSUSPEND */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Start of Frame interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Start of Frame interrupt */ #ifdef INTR_SOFINTR - if (gintr_status.b.sofintr) { - /* Update the frame number variable */ - bIntPackSOF++; + if (gintr_status.b.sofintr) { + /* Update the frame number variable */ + bIntPackSOF++; - retval |= OTGD_FS_Handle_Sof_ISR(); - } + retval |= OTGD_FS_Handle_Sof_ISR(); + } #endif /* INTR_SOFINTR */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Receive FIFO Queue Status Level interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Receive FIFO Queue Status Level interrupt */ #ifdef INTR_RXSTSQLVL - if (gintr_status.b.rxstsqlvl) { - retval |= OTGD_FS_Handle_RxStatusQueueLevel_ISR(); - } + if (gintr_status.b.rxstsqlvl) { + retval |= OTGD_FS_Handle_RxStatusQueueLevel_ISR(); + } #endif /* INTR_RXSTSQLVL */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Enumeration Done interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Enumeration Done interrupt */ #ifdef INTR_ENUMDONE - if (gintr_status.b.enumdone) { - retval |= OTGD_FS_Handle_EnumDone_ISR(); - } + if (gintr_status.b.enumdone) { + retval |= OTGD_FS_Handle_EnumDone_ISR(); + } #endif /* INTR_ENUMDONE */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Reset interrutp */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Reset interrutp */ #ifdef INTR_USBRESET - if (gintr_status.b.usbreset) { - retval |= OTGD_FS_Handle_UsbReset_ISR(); - } + if (gintr_status.b.usbreset) { + retval |= OTGD_FS_Handle_UsbReset_ISR(); + } #endif /* INTR_USBRESET */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* IN Endpoint interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* IN Endpoint interrupt */ #ifdef INTR_INEPINTR - if (gintr_status.b.inepint) { - retval |= OTGD_FS_Handle_InEP_ISR(); - } + if (gintr_status.b.inepint) { + retval |= OTGD_FS_Handle_InEP_ISR(); + } #endif /* INTR_INEPINTR */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* OUT Endpoint interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* OUT Endpoint interrupt */ #ifdef INTR_OUTEPINTR - if (gintr_status.b.outepintr) { - retval |= OTGD_FS_Handle_OutEP_ISR(); - } + if (gintr_status.b.outepintr) { + retval |= OTGD_FS_Handle_OutEP_ISR(); + } #endif /* INTR_OUTEPINTR */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Mode Mismatch interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Mode Mismatch interrupt */ #ifdef INTR_MODEMISMATCH - if (gintr_status.b.modemismatch) { - retval |= OTGD_FS_Handle_ModeMismatch_ISR(); - } + if (gintr_status.b.modemismatch) { + retval |= OTGD_FS_Handle_ModeMismatch_ISR(); + } #endif /* INTR_MODEMISMATCH */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Global IN Endpoints NAK Effective interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Global IN Endpoints NAK Effective interrupt */ #ifdef INTR_GINNAKEFF - if (gintr_status.b.ginnakeff) { - retval |= OTGD_FS_Handle_GInNakEff_ISR(); - } + if (gintr_status.b.ginnakeff) { + retval |= OTGD_FS_Handle_GInNakEff_ISR(); + } #endif /* INTR_GINNAKEFF */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Global OUT Endpoints NAK effective interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Global OUT Endpoints NAK effective interrupt */ #ifdef INTR_GOUTNAKEFF - if (gintr_status.b.goutnakeff) { - retval |= OTGD_FS_Handle_GOutNakEff_ISR(); - } + if (gintr_status.b.goutnakeff) { + retval |= OTGD_FS_Handle_GOutNakEff_ISR(); + } #endif /* INTR_GOUTNAKEFF */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Isochrounous Out packet Dropped interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Isochrounous Out packet Dropped interrupt */ #ifdef INTR_ISOOUTDROP - if (gintr_status.b.isooutdrop) { - retval |= OTGD_FS_Handle_IsoOutDrop_ISR(); - } + if (gintr_status.b.isooutdrop) { + retval |= OTGD_FS_Handle_IsoOutDrop_ISR(); + } #endif /* INTR_ISOOUTDROP */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Endpoint Mismatch error interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Endpoint Mismatch error interrupt */ #ifdef INTR_EPMISMATCH - if (gintr_status.b.epmismatch) { - retval |= OTGD_FS_Handle_EPMismatch_ISR(); - } + if (gintr_status.b.epmismatch) { + retval |= OTGD_FS_Handle_EPMismatch_ISR(); + } #endif /* INTR_EPMISMATCH */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Incomplete Isochrous IN tranfer error interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Incomplete Isochrous IN tranfer error interrupt */ #ifdef INTR_INCOMPLISOIN - if (gintr_status.b.incomplisoin) { - retval |= OTGD_FS_Handle_IncomplIsoIn_ISR(); - } + if (gintr_status.b.incomplisoin) { + retval |= OTGD_FS_Handle_IncomplIsoIn_ISR(); + } #endif /* INTR_INCOMPLISOIN */ - /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ - /* Incomplete Isochrous OUT tranfer error interrupt */ + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Incomplete Isochrous OUT tranfer error interrupt */ #ifdef INTR_INCOMPLISOOUT - if (gintr_status.b.outepintr) { - retval |= OTGD_FS_Handle_IncomplIsoOut_ISR(); - } + if (gintr_status.b.outepintr) { + retval |= OTGD_FS_Handle_IncomplIsoOut_ISR(); + } #endif /* INTR_INCOMPLISOOUT */ - - } - return retval; + } + return retval; } #endif /* STM32F10X_CL */ diff --git a/flight/pios/stm32f10x/pios_usb_hid_pwr.c b/flight/pios/stm32f10x/pios_usb_hid_pwr.c index 888f2d748..ffa9c3495 100644 --- a/flight/pios/stm32f10x/pios_usb_hid_pwr.c +++ b/flight/pios/stm32f10x/pios_usb_hid_pwr.c @@ -24,12 +24,12 @@ /* Private define ------------------------------------------------------------*/ /* Private macro -------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ -__IO uint32_t bDeviceState = UNCONNECTED; /* USB device status */ -__IO bool fSuspendEnabled = TRUE; /* true when suspend is possible */ +__IO uint32_t bDeviceState = UNCONNECTED; /* USB device status */ +__IO bool fSuspendEnabled = TRUE; /* true when suspend is possible */ struct { - __IO RESUME_STATE eState; - __IO uint8_t bESOFcnt; + __IO RESUME_STATE eState; + __IO uint8_t bESOFcnt; } ResumeS; /* Extern variables ----------------------------------------------------------*/ @@ -38,15 +38,14 @@ struct { /* Private functions ---------------------------------------------------------*/ /******************************************************************************* - * Function Name : USB_Cable_Config. - * Description : Software Connection/Disconnection of USB Cable. - * Input : NewState: new state. - * Output : None. - * Return : None - *******************************************************************************/ +* Function Name : USB_Cable_Config. +* Description : Software Connection/Disconnection of USB Cable. +* Input : NewState: new state. +* Output : None. +* Return : None +*******************************************************************************/ void USB_Cable_Config(__attribute__((unused)) FunctionalState NewState) -{ -} +{} /******************************************************************************* * Function Name : PowerOn @@ -58,26 +57,26 @@ void USB_Cable_Config(__attribute__((unused)) FunctionalState NewState) RESULT PowerOn(void) { #ifndef STM32F10X_CL - uint16_t wRegVal; + uint16_t wRegVal; - /*** cable plugged-in ? ***/ - USB_Cable_Config(ENABLE); + /*** cable plugged-in ? ***/ + USB_Cable_Config(ENABLE); - /*** CNTR_PWDN = 0 ***/ - wRegVal = CNTR_FRES; - _SetCNTR(wRegVal); + /*** CNTR_PWDN = 0 ***/ + wRegVal = CNTR_FRES; + _SetCNTR(wRegVal); - /*** CNTR_FRES = 0 ***/ - wInterrupt_Mask = 0; - _SetCNTR(wInterrupt_Mask); - /*** Clear pending interrupts ***/ - _SetISTR(0); - /*** Set interrupt mask ***/ - wInterrupt_Mask = CNTR_RESETM | CNTR_SUSPM | CNTR_WKUPM; - _SetCNTR(wInterrupt_Mask); + /*** CNTR_FRES = 0 ***/ + wInterrupt_Mask = 0; + _SetCNTR(wInterrupt_Mask); + /*** Clear pending interrupts ***/ + _SetISTR(0); + /*** Set interrupt mask ***/ + wInterrupt_Mask = CNTR_RESETM | CNTR_SUSPM | CNTR_WKUPM; + _SetCNTR(wInterrupt_Mask); #endif /* STM32F10X_CL */ - return USB_SUCCESS; + return USB_SUCCESS; } /******************************************************************************* @@ -90,33 +89,33 @@ RESULT PowerOn(void) RESULT PowerOff() { #ifndef STM32F10X_CL - /* disable all ints and force USB reset */ - _SetCNTR(CNTR_FRES); - /* clear interrupt status register */ - _SetISTR(0); - /* Disable the Pull-Up */ - USB_Cable_Config(DISABLE); - /* switch-off device */ - _SetCNTR(CNTR_FRES + CNTR_PDWN); + /* disable all ints and force USB reset */ + _SetCNTR(CNTR_FRES); + /* clear interrupt status register */ + _SetISTR(0); + /* Disable the Pull-Up */ + USB_Cable_Config(DISABLE); + /* switch-off device */ + _SetCNTR(CNTR_FRES + CNTR_PDWN); #endif /* STM32F10X_CL */ - /* sw variables reset */ - /* ... */ + /* sw variables reset */ + /* ... */ - return USB_SUCCESS; + return USB_SUCCESS; } /******************************************************************************* - * Function Name : Enter_LowPowerMode. - * Description : Power-off system clocks and power while entering suspend mode. - * Input : None. - * Output : None. - * Return : None. - *******************************************************************************/ +* Function Name : Enter_LowPowerMode. +* Description : Power-off system clocks and power while entering suspend mode. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ void Enter_LowPowerMode(void) { - /* Set the device state to suspend */ - bDeviceState = SUSPENDED; + /* Set the device state to suspend */ + bDeviceState = SUSPENDED; } /******************************************************************************* @@ -129,51 +128,50 @@ void Enter_LowPowerMode(void) void Suspend(void) { #ifndef STM32F10X_CL - uint16_t wCNTR; - /* suspend preparation */ - /* ... */ + uint16_t wCNTR; + /* suspend preparation */ + /* ... */ - /* macrocell enters suspend mode */ - wCNTR = _GetCNTR(); - wCNTR |= CNTR_FSUSP; - _SetCNTR(wCNTR); + /* macrocell enters suspend mode */ + wCNTR = _GetCNTR(); + wCNTR |= CNTR_FSUSP; + _SetCNTR(wCNTR); #endif /* STM32F10X_CL */ - /* ------------------ ONLY WITH BUS-POWERED DEVICES ---------------------- */ - /* power reduction */ - /* ... on connected devices */ + /* ------------------ ONLY WITH BUS-POWERED DEVICES ---------------------- */ + /* power reduction */ + /* ... on connected devices */ #ifndef STM32F10X_CL - /* force low-power mode in the macrocell */ - wCNTR = _GetCNTR(); - wCNTR |= CNTR_LPMODE; - _SetCNTR(wCNTR); + /* force low-power mode in the macrocell */ + wCNTR = _GetCNTR(); + wCNTR |= CNTR_LPMODE; + _SetCNTR(wCNTR); #endif /* STM32F10X_CL */ - /* switch-off the clocks */ - /* ... */ - Enter_LowPowerMode(); - + /* switch-off the clocks */ + /* ... */ + Enter_LowPowerMode(); } /******************************************************************************* - * Function Name : Leave_LowPowerMode. - * Description : Restores system clocks and power while exiting suspend mode. - * Input : None. - * Output : None. - * Return : None. - *******************************************************************************/ +* Function Name : Leave_LowPowerMode. +* Description : Restores system clocks and power while exiting suspend mode. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ void Leave_LowPowerMode(void) { - DEVICE_INFO *pInfo = &Device_Info; + DEVICE_INFO *pInfo = &Device_Info; - /* Set the device state to the correct state */ - if (pInfo->Current_Configuration != 0) { - /* Device configured */ - bDeviceState = CONFIGURED; - } else { - bDeviceState = ATTACHED; - } + /* Set the device state to the correct state */ + if (pInfo->Current_Configuration != 0) { + /* Device configured */ + bDeviceState = CONFIGURED; + } else { + bDeviceState = ATTACHED; + } } /******************************************************************************* @@ -186,32 +184,31 @@ void Leave_LowPowerMode(void) void Resume_Init(void) { #ifndef STM32F10X_CL - uint16_t wCNTR; + uint16_t wCNTR; #endif /* STM32F10X_CL */ - /* ------------------ ONLY WITH BUS-POWERED DEVICES ---------------------- */ - /* restart the clocks */ - /* ... */ + /* ------------------ ONLY WITH BUS-POWERED DEVICES ---------------------- */ + /* restart the clocks */ + /* ... */ #ifndef STM32F10X_CL - /* CNTR_LPMODE = 0 */ - wCNTR = _GetCNTR(); - wCNTR &= (~CNTR_LPMODE); - _SetCNTR(wCNTR); + /* CNTR_LPMODE = 0 */ + wCNTR = _GetCNTR(); + wCNTR &= (~CNTR_LPMODE); + _SetCNTR(wCNTR); #endif /* STM32F10X_CL */ - /* restore full power */ - /* ... on connected devices */ - Leave_LowPowerMode(); + /* restore full power */ + /* ... on connected devices */ + Leave_LowPowerMode(); #ifndef STM32F10X_CL - /* reset FSUSP bit */ - _SetCNTR(IMR_MSK); + /* reset FSUSP bit */ + _SetCNTR(IMR_MSK); #endif /* STM32F10X_CL */ - /* reverse suspend preparation */ - /* ... */ - + /* reverse suspend preparation */ + /* ... */ } /******************************************************************************* @@ -229,64 +226,66 @@ void Resume_Init(void) void Resume(RESUME_STATE eResumeSetVal) { #ifndef STM32F10X_CL - uint16_t wCNTR; + uint16_t wCNTR; #endif /* STM32F10X_CL */ - if (eResumeSetVal != RESUME_ESOF) - ResumeS.eState = eResumeSetVal; + if (eResumeSetVal != RESUME_ESOF) { + ResumeS.eState = eResumeSetVal; + } - switch (ResumeS.eState) { - case RESUME_EXTERNAL: - Resume_Init(); - ResumeS.eState = RESUME_OFF; - break; - case RESUME_INTERNAL: - Resume_Init(); - ResumeS.eState = RESUME_START; - break; - case RESUME_LATER: - ResumeS.bESOFcnt = 2; - ResumeS.eState = RESUME_WAIT; - break; - case RESUME_WAIT: - ResumeS.bESOFcnt--; - if (ResumeS.bESOFcnt == 0) - ResumeS.eState = RESUME_START; - break; - case RESUME_START: + switch (ResumeS.eState) { + case RESUME_EXTERNAL: + Resume_Init(); + ResumeS.eState = RESUME_OFF; + break; + case RESUME_INTERNAL: + Resume_Init(); + ResumeS.eState = RESUME_START; + break; + case RESUME_LATER: + ResumeS.bESOFcnt = 2; + ResumeS.eState = RESUME_WAIT; + break; + case RESUME_WAIT: + ResumeS.bESOFcnt--; + if (ResumeS.bESOFcnt == 0) { + ResumeS.eState = RESUME_START; + } + break; + case RESUME_START: #ifdef STM32F10X_CL - OTGD_FS_SetRemoteWakeup(); + OTGD_FS_SetRemoteWakeup(); #else - wCNTR = _GetCNTR(); - wCNTR |= CNTR_RESUME; - _SetCNTR(wCNTR); + wCNTR = _GetCNTR(); + wCNTR |= CNTR_RESUME; + _SetCNTR(wCNTR); #endif /* STM32F10X_CL */ - ResumeS.eState = RESUME_ON; - ResumeS.bESOFcnt = 10; - break; - case RESUME_ON: + ResumeS.eState = RESUME_ON; + ResumeS.bESOFcnt = 10; + break; + case RESUME_ON: #ifndef STM32F10X_CL - ResumeS.bESOFcnt--; - if (ResumeS.bESOFcnt == 0) { + ResumeS.bESOFcnt--; + if (ResumeS.bESOFcnt == 0) { #endif /* STM32F10X_CL */ #ifdef STM32F10X_CL - OTGD_FS_ResetRemoteWakeup(); + OTGD_FS_ResetRemoteWakeup(); #else - wCNTR = _GetCNTR(); - wCNTR &= (~CNTR_RESUME); - _SetCNTR(wCNTR); + wCNTR = _GetCNTR(); + wCNTR &= (~CNTR_RESUME); + _SetCNTR(wCNTR); #endif /* STM32F10X_CL */ - ResumeS.eState = RESUME_OFF; + ResumeS.eState = RESUME_OFF; #ifndef STM32F10X_CL - } + } #endif /* STM32F10X_CL */ - break; - case RESUME_OFF: - case RESUME_ESOF: - default: - ResumeS.eState = RESUME_OFF; - break; - } + break; + case RESUME_OFF: + case RESUME_ESOF: + default: + ResumeS.eState = RESUME_OFF; + break; + } } /******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f10x/pios_usb_rctx.c b/flight/pios/stm32f10x/pios_usb_rctx.c index 11e333295..3fc57dcc8 100644 --- a/flight/pios/stm32f10x/pios_usb_rctx.c +++ b/flight/pios/stm32f10x/pios_usb_rctx.c @@ -40,170 +40,180 @@ #define PIOS_USB_RCTX_NUM_CHANNELS 8 enum pios_usb_rctx_dev_magic { - PIOS_USB_RCTX_DEV_MAGIC = 0xAB98B745, + PIOS_USB_RCTX_DEV_MAGIC = 0xAB98B745, }; struct pios_usb_rctx_dev { - enum pios_usb_rctx_dev_magic magic; - const struct pios_usb_rctx_cfg * cfg; + enum pios_usb_rctx_dev_magic magic; + const struct pios_usb_rctx_cfg *cfg; - uint32_t lower_id; + uint32_t lower_id; - struct { - uint8_t id; - uint16_t vals[PIOS_USB_RCTX_NUM_CHANNELS]; - } __attribute__((packed)) report; + struct { + uint8_t id; + uint16_t vals[PIOS_USB_RCTX_NUM_CHANNELS]; + } __attribute__((packed)) report; }; -static bool PIOS_USB_RCTX_validate(struct pios_usb_rctx_dev * usb_rctx_dev) +static bool PIOS_USB_RCTX_validate(struct pios_usb_rctx_dev *usb_rctx_dev) { - return (usb_rctx_dev->magic == PIOS_USB_RCTX_DEV_MAGIC); + return usb_rctx_dev->magic == PIOS_USB_RCTX_DEV_MAGIC; } #ifdef PIOS_INCLUDE_FREERTOS -static struct pios_usb_rctx_dev * PIOS_USB_RCTX_alloc(void) +static struct pios_usb_rctx_dev *PIOS_USB_RCTX_alloc(void) { - struct pios_usb_rctx_dev * usb_rctx_dev; + struct pios_usb_rctx_dev *usb_rctx_dev; - usb_rctx_dev = (struct pios_usb_rctx_dev *)pvPortMalloc(sizeof(*usb_rctx_dev)); - if (!usb_rctx_dev) return(NULL); + usb_rctx_dev = (struct pios_usb_rctx_dev *)pvPortMalloc(sizeof(*usb_rctx_dev)); + if (!usb_rctx_dev) { + return NULL; + } - usb_rctx_dev->magic = PIOS_USB_RCTX_DEV_MAGIC; - return(usb_rctx_dev); + usb_rctx_dev->magic = PIOS_USB_RCTX_DEV_MAGIC; + return usb_rctx_dev; } #else static struct pios_usb_rctx_dev pios_usb_rctx_devs[PIOS_USB_RCTX_MAX_DEVS]; static uint8_t pios_usb_rctx_num_devs; -static struct pios_usb_rctx_dev * PIOS_USB_RCTX_alloc(void) +static struct pios_usb_rctx_dev *PIOS_USB_RCTX_alloc(void) { - struct pios_usb_rctx_dev * usb_rctx_dev; + struct pios_usb_rctx_dev *usb_rctx_dev; - if (pios_usb_rctx_num_devs >= PIOS_USB_RCTX_MAX_DEVS) { - return (NULL); - } + if (pios_usb_rctx_num_devs >= PIOS_USB_RCTX_MAX_DEVS) { + return NULL; + } - usb_rctx_dev = &pios_usb_rctx_devs[pios_usb_rctx_num_devs++]; - usb_rctx_dev->magic = PIOS_USB_RCTX_DEV_MAGIC; + usb_rctx_dev = &pios_usb_rctx_devs[pios_usb_rctx_num_devs++]; + usb_rctx_dev->magic = PIOS_USB_RCTX_DEV_MAGIC; - return (usb_rctx_dev); + return usb_rctx_dev; } -#endif +#endif /* ifdef PIOS_INCLUDE_FREERTOS */ static void PIOS_USB_RCTX_EP_IN_Callback(void); -static void PIOS_USB_RCTX_SendReport(struct pios_usb_rctx_dev * usb_rctx_dev); +static void PIOS_USB_RCTX_SendReport(struct pios_usb_rctx_dev *usb_rctx_dev); /* Need a better way to pull these in */ -extern void (*pEpInt_IN[7])(void); +extern void(*pEpInt_IN[7]) (void); -int32_t PIOS_USB_RCTX_Init(uint32_t * usbrctx_id, const struct pios_usb_rctx_cfg * cfg, uint32_t lower_id) +int32_t PIOS_USB_RCTX_Init(uint32_t *usbrctx_id, const struct pios_usb_rctx_cfg *cfg, uint32_t lower_id) { - PIOS_Assert(usbrctx_id); - PIOS_Assert(cfg); + PIOS_Assert(usbrctx_id); + PIOS_Assert(cfg); - struct pios_usb_rctx_dev * usb_rctx_dev; + struct pios_usb_rctx_dev *usb_rctx_dev; - usb_rctx_dev = (struct pios_usb_rctx_dev *) PIOS_USB_RCTX_alloc(); - if (!usb_rctx_dev) goto out_fail; + usb_rctx_dev = (struct pios_usb_rctx_dev *)PIOS_USB_RCTX_alloc(); + if (!usb_rctx_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usb_rctx_dev->cfg = cfg; - usb_rctx_dev->lower_id = lower_id; + /* Bind the configuration to the device instance */ + usb_rctx_dev->cfg = cfg; + usb_rctx_dev->lower_id = lower_id; - /* Set the initial report buffer */ - memset(&usb_rctx_dev->report, 0, sizeof(usb_rctx_dev->report)); + /* Set the initial report buffer */ + memset(&usb_rctx_dev->report, 0, sizeof(usb_rctx_dev->report)); - pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_RCTX_EP_IN_Callback; + pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_RCTX_EP_IN_Callback; - *usbrctx_id = (uint32_t) usb_rctx_dev; + *usbrctx_id = (uint32_t)usb_rctx_dev; - return 0; + return 0; out_fail: - return -1; + return -1; } -static void PIOS_USB_RCTX_SendReport(struct pios_usb_rctx_dev * usb_rctx_dev) +static void PIOS_USB_RCTX_SendReport(struct pios_usb_rctx_dev *usb_rctx_dev) { #ifdef PIOS_INCLUDE_FREERTOS - bool need_yield = false; -#endif /* PIOS_INCLUDE_FREERTOS */ + bool need_yield = false; +#endif /* PIOS_INCLUDE_FREERTOS */ - usb_rctx_dev->report.id = 3; /* FIXME: shouldn't hard-code this report ID */ + usb_rctx_dev->report.id = 3; /* FIXME: shouldn't hard-code this report ID */ - UserToPMABufferCopy((uint8_t *) &usb_rctx_dev->report, - GetEPTxAddr(usb_rctx_dev->cfg->data_tx_ep), - sizeof(usb_rctx_dev->report)); + UserToPMABufferCopy((uint8_t *)&usb_rctx_dev->report, + GetEPTxAddr(usb_rctx_dev->cfg->data_tx_ep), + sizeof(usb_rctx_dev->report)); - SetEPTxCount(usb_rctx_dev->cfg->data_tx_ep, sizeof(usb_rctx_dev->report)); - SetEPTxValid(usb_rctx_dev->cfg->data_tx_ep); + SetEPTxCount(usb_rctx_dev->cfg->data_tx_ep, sizeof(usb_rctx_dev->report)); + SetEPTxValid(usb_rctx_dev->cfg->data_tx_ep); #ifdef PIOS_INCLUDE_FREERTOS - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } static void PIOS_USB_RCTX_EP_IN_Callback(void) { - struct pios_usb_rctx_dev * usb_rctx_dev = (struct pios_usb_rctx_dev *)pios_usb_rctx_id; + struct pios_usb_rctx_dev *usb_rctx_dev = (struct pios_usb_rctx_dev *)pios_usb_rctx_id; - bool valid = PIOS_USB_RCTX_validate(usb_rctx_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_RCTX_validate(usb_rctx_dev); - if (!PIOS_USB_CheckAvailable(usb_rctx_dev->lower_id)) { - return; - } + PIOS_Assert(valid); - PIOS_USB_RCTX_SendReport(usb_rctx_dev); + if (!PIOS_USB_CheckAvailable(usb_rctx_dev->lower_id)) { + return; + } + + PIOS_USB_RCTX_SendReport(usb_rctx_dev); } void PIOS_USB_RCTX_Update(uint32_t usbrctx_id, const uint16_t channel[], const int16_t channel_min[], const int16_t channel_max[], uint8_t num_channels) { - struct pios_usb_rctx_dev * usb_rctx_dev = (struct pios_usb_rctx_dev *)usbrctx_id; + struct pios_usb_rctx_dev *usb_rctx_dev = (struct pios_usb_rctx_dev *)usbrctx_id; - bool valid = PIOS_USB_RCTX_validate(usb_rctx_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_RCTX_validate(usb_rctx_dev); - if (!PIOS_USB_CheckAvailable(usb_rctx_dev->lower_id)) { - return; - } + PIOS_Assert(valid); - for (uint8_t i = 0; - i < PIOS_USB_RCTX_NUM_CHANNELS && i < num_channels; - i++) { - int16_t min = channel_min[i]; - int16_t max = channel_max[i]; - uint16_t val = channel[i]; + if (!PIOS_USB_CheckAvailable(usb_rctx_dev->lower_id)) { + return; + } - if (channel_min[i] > channel_max[i]) { - /* This channel is reversed, flip min and max */ - min = channel_max[i]; - max = channel_min[i]; + for (uint8_t i = 0; + i < PIOS_USB_RCTX_NUM_CHANNELS && i < num_channels; + i++) { + int16_t min = channel_min[i]; + int16_t max = channel_max[i]; + uint16_t val = channel[i]; - /* and flip val to be an offset from the lower end of the range */ - val = channel_min[i] - channel[i] + channel_max[i]; - } + if (channel_min[i] > channel_max[i]) { + /* This channel is reversed, flip min and max */ + min = channel_max[i]; + max = channel_min[i]; - /* Scale channel linearly between min and max */ - if (min == max) { - val = 0; - } else { - if (val < min) val = min; - if (val > max) val = max; + /* and flip val to be an offset from the lower end of the range */ + val = channel_min[i] - channel[i] + channel_max[i]; + } - val = (val - min) * (65535 / (max - min)); - } + /* Scale channel linearly between min and max */ + if (min == max) { + val = 0; + } else { + if (val < min) { + val = min; + } + if (val > max) { + val = max; + } - usb_rctx_dev->report.vals[i] = val; - } + val = (val - min) * (65535 / (max - min)); + } - if (GetEPTxStatus(usb_rctx_dev->cfg->data_tx_ep) == EP_TX_VALID) { - /* Endpoint is already transmitting */ - return; - } + usb_rctx_dev->report.vals[i] = val; + } - PIOS_USB_RCTX_SendReport(usb_rctx_dev); + if (GetEPTxStatus(usb_rctx_dev->cfg->data_tx_ep) == EP_TX_VALID) { + /* Endpoint is already transmitting */ + return; + } + + PIOS_USB_RCTX_SendReport(usb_rctx_dev); } #endif /* PIOS_INCLUDE_USB_RCTX */ diff --git a/flight/pios/stm32f10x/pios_usbhook.c b/flight/pios/stm32f10x/pios_usbhook.c index ba01d8fee..c1173db7d 100644 --- a/flight/pios/stm32f10x/pios_usbhook.c +++ b/flight/pios/stm32f10x/pios_usbhook.c @@ -32,11 +32,11 @@ #ifdef PIOS_INCLUDE_USB -#include "pios_usb.h" /* PIOS_USB_* */ +#include "pios_usb.h" /* PIOS_USB_* */ #include "pios_usbhook.h" -#include "pios_usb_defs.h" /* struct usb_* */ +#include "pios_usb_defs.h" /* struct usb_* */ #include "pios_usb_hid_pwr.h" -#include "pios_usb_cdc_priv.h" /* PIOS_USB_CDC_* */ +#include "pios_usb_cdc_priv.h" /* PIOS_USB_CDC_* */ #include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ /* STM32 USB Library Definitions */ @@ -44,54 +44,54 @@ static ONE_DESCRIPTOR Device_Descriptor; -void PIOS_USBHOOK_RegisterDevice(const uint8_t * desc, uint16_t desc_size) +void PIOS_USBHOOK_RegisterDevice(const uint8_t *desc, uint16_t desc_size) { - Device_Descriptor.Descriptor = desc; - Device_Descriptor.Descriptor_Size = desc_size; + Device_Descriptor.Descriptor = desc; + Device_Descriptor.Descriptor_Size = desc_size; } static ONE_DESCRIPTOR Config_Descriptor; -void PIOS_USBHOOK_RegisterConfig(__attribute__((unused)) uint8_t config_id, const uint8_t * desc, uint16_t desc_size) +void PIOS_USBHOOK_RegisterConfig(__attribute__((unused)) uint8_t config_id, const uint8_t *desc, uint16_t desc_size) { - Config_Descriptor.Descriptor = desc; - Config_Descriptor.Descriptor_Size = desc_size; + Config_Descriptor.Descriptor = desc; + Config_Descriptor.Descriptor_Size = desc_size; } static ONE_DESCRIPTOR String_Descriptor[4]; -void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t * desc, uint16_t desc_size) +void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t *desc, uint16_t desc_size) { - if (string_id < NELEMENTS(String_Descriptor)) { - String_Descriptor[string_id].Descriptor = desc; - String_Descriptor[string_id].Descriptor_Size = desc_size; - } + if (string_id < NELEMENTS(String_Descriptor)) { + String_Descriptor[string_id].Descriptor = desc; + String_Descriptor[string_id].Descriptor_Size = desc_size; + } } static ONE_DESCRIPTOR Hid_Descriptor; -void PIOS_USB_HID_RegisterHidDescriptor(const uint8_t * desc, uint16_t desc_size) +void PIOS_USB_HID_RegisterHidDescriptor(const uint8_t *desc, uint16_t desc_size) { - Hid_Descriptor.Descriptor = desc; - Hid_Descriptor.Descriptor_Size = desc_size; + Hid_Descriptor.Descriptor = desc; + Hid_Descriptor.Descriptor_Size = desc_size; } static ONE_DESCRIPTOR Hid_Report_Descriptor; -void PIOS_USB_HID_RegisterHidReport(const uint8_t * desc, uint16_t desc_size) +void PIOS_USB_HID_RegisterHidReport(const uint8_t *desc, uint16_t desc_size) { - Hid_Report_Descriptor.Descriptor = desc; - Hid_Report_Descriptor.Descriptor_Size = desc_size; + Hid_Report_Descriptor.Descriptor = desc; + Hid_Report_Descriptor.Descriptor_Size = desc_size; } -#include "stm32f10x.h" /* __IO */ +#include "stm32f10x.h" /* __IO */ __IO uint8_t EXTI_Enable; uint32_t ProtocolValue; DEVICE Device_Table = { - PIOS_USB_BOARD_EP_NUM, - 1 + PIOS_USB_BOARD_EP_NUM, + 1 }; static void PIOS_USBHOOK_Init(void); @@ -106,33 +106,33 @@ static const uint8_t *PIOS_USBHOOK_GetConfigDescriptor(uint16_t Length); static const uint8_t *PIOS_USBHOOK_GetStringDescriptor(uint16_t Length); DEVICE_PROP Device_Property = { - .Init = PIOS_USBHOOK_Init, - .Reset = PIOS_USBHOOK_Reset, - .Process_Status_IN = PIOS_USBHOOK_Status_In, - .Process_Status_OUT = PIOS_USBHOOK_Status_Out, - .Class_Data_Setup = PIOS_USBHOOK_Data_Setup, - .Class_NoData_Setup = PIOS_USBHOOK_NoData_Setup, - .Class_Get_Interface_Setting = PIOS_USBHOOK_Get_Interface_Setting, - .GetDeviceDescriptor = PIOS_USBHOOK_GetDeviceDescriptor, - .GetConfigDescriptor = PIOS_USBHOOK_GetConfigDescriptor, - .GetStringDescriptor = PIOS_USBHOOK_GetStringDescriptor, - .RxEP_buffer = 0, - .MaxPacketSize = 0x40, + .Init = PIOS_USBHOOK_Init, + .Reset = PIOS_USBHOOK_Reset, + .Process_Status_IN = PIOS_USBHOOK_Status_In, + .Process_Status_OUT = PIOS_USBHOOK_Status_Out, + .Class_Data_Setup = PIOS_USBHOOK_Data_Setup, + .Class_NoData_Setup = PIOS_USBHOOK_NoData_Setup, + .Class_Get_Interface_Setting = PIOS_USBHOOK_Get_Interface_Setting, + .GetDeviceDescriptor = PIOS_USBHOOK_GetDeviceDescriptor, + .GetConfigDescriptor = PIOS_USBHOOK_GetConfigDescriptor, + .GetStringDescriptor = PIOS_USBHOOK_GetStringDescriptor, + .RxEP_buffer = 0, + .MaxPacketSize = 0x40, }; static void PIOS_USBHOOK_SetConfiguration(void); static void PIOS_USBHOOK_SetDeviceAddress(void); USER_STANDARD_REQUESTS User_Standard_Requests = { - .User_GetConfiguration = NOP_Process, - .User_SetConfiguration = PIOS_USBHOOK_SetConfiguration, - .User_GetInterface = NOP_Process, - .User_SetInterface = NOP_Process, - .User_GetStatus = NOP_Process, - .User_ClearFeature = NOP_Process, - .User_SetEndPointFeature = NOP_Process, - .User_SetDeviceFeature = NOP_Process, - .User_SetDeviceAddress = PIOS_USBHOOK_SetDeviceAddress + .User_GetConfiguration = NOP_Process, + .User_SetConfiguration = PIOS_USBHOOK_SetConfiguration, + .User_GetInterface = NOP_Process, + .User_SetInterface = NOP_Process, + .User_GetStatus = NOP_Process, + .User_ClearFeature = NOP_Process, + .User_SetEndPointFeature = NOP_Process, + .User_SetDeviceFeature = NOP_Process, + .User_SetDeviceAddress = PIOS_USBHOOK_SetDeviceAddress }; static RESULT PIOS_USBHOOK_SetProtocol(void); @@ -149,15 +149,15 @@ static const uint8_t *PIOS_USBHOOK_GetHIDDescriptor(uint16_t Length); *******************************************************************************/ static void PIOS_USBHOOK_Init(void) { - pInformation->Current_Configuration = 0; + pInformation->Current_Configuration = 0; - /* Connect the device */ - PowerOn(); + /* Connect the device */ + PowerOn(); - /* Perform basic device initialization operations */ - USB_SIL_Init(); + /* Perform basic device initialization operations */ + USB_SIL_Init(); - bDeviceState = UNCONNECTED; + bDeviceState = UNCONNECTED; } /******************************************************************************* @@ -169,72 +169,72 @@ static void PIOS_USBHOOK_Init(void) *******************************************************************************/ static void PIOS_USBHOOK_Reset(void) { - /* Set DEVICE as not configured */ - pInformation->Current_Configuration = 0; - pInformation->Current_Interface = 0; /*the default Interface */ + /* Set DEVICE as not configured */ + pInformation->Current_Configuration = 0; + pInformation->Current_Interface = 0; /*the default Interface */ - /* Current Feature initialization */ - pInformation->Current_Feature = 0; + /* Current Feature initialization */ + pInformation->Current_Feature = 0; #ifdef STM32F10X_CL - /* EP0 is already configured in DFU_Init() by USB_SIL_Init() function */ + /* EP0 is already configured in DFU_Init() by USB_SIL_Init() function */ - /* Init EP1 IN as Interrupt endpoint */ - OTG_DEV_EP_Init(EP1_IN, OTG_DEV_EP_TYPE_INT, 2); + /* Init EP1 IN as Interrupt endpoint */ + OTG_DEV_EP_Init(EP1_IN, OTG_DEV_EP_TYPE_INT, 2); - /* Init EP1 OUT as Interrupt endpoint */ - OTG_DEV_EP_Init(EP1_OUT, OTG_DEV_EP_TYPE_INT, 2); + /* Init EP1 OUT as Interrupt endpoint */ + OTG_DEV_EP_Init(EP1_OUT, OTG_DEV_EP_TYPE_INT, 2); #else - SetBTABLE(BTABLE_ADDRESS); + SetBTABLE(BTABLE_ADDRESS); - /* Initialize Endpoint 0 (Control) */ - SetEPType(ENDP0, EP_CONTROL); - SetEPTxAddr(ENDP0, ENDP0_TXADDR); - SetEPTxStatus(ENDP0, EP_TX_STALL); - Clear_Status_Out(ENDP0); + /* Initialize Endpoint 0 (Control) */ + SetEPType(ENDP0, EP_CONTROL); + SetEPTxAddr(ENDP0, ENDP0_TXADDR); + SetEPTxStatus(ENDP0, EP_TX_STALL); + Clear_Status_Out(ENDP0); - SetEPRxAddr(ENDP0, ENDP0_RXADDR); - SetEPRxCount(ENDP0, Device_Property.MaxPacketSize); - SetEPRxValid(ENDP0); + SetEPRxAddr(ENDP0, ENDP0_RXADDR); + SetEPRxCount(ENDP0, Device_Property.MaxPacketSize); + SetEPRxValid(ENDP0); #if defined(PIOS_INCLUDE_USB_HID) - /* Initialize Endpoint 1 (HID) */ - SetEPType(ENDP1, EP_INTERRUPT); - SetEPTxAddr(ENDP1, ENDP1_TXADDR); - SetEPTxCount(ENDP1, PIOS_USB_BOARD_HID_DATA_LENGTH); - SetEPTxStatus(ENDP1, EP_TX_NAK); + /* Initialize Endpoint 1 (HID) */ + SetEPType(ENDP1, EP_INTERRUPT); + SetEPTxAddr(ENDP1, ENDP1_TXADDR); + SetEPTxCount(ENDP1, PIOS_USB_BOARD_HID_DATA_LENGTH); + SetEPTxStatus(ENDP1, EP_TX_NAK); - SetEPRxAddr(ENDP1, ENDP1_RXADDR); - SetEPRxCount(ENDP1, PIOS_USB_BOARD_HID_DATA_LENGTH); - SetEPRxStatus(ENDP1, EP_RX_VALID); -#endif /* PIOS_INCLUDE_USB_HID */ + SetEPRxAddr(ENDP1, ENDP1_RXADDR); + SetEPRxCount(ENDP1, PIOS_USB_BOARD_HID_DATA_LENGTH); + SetEPRxStatus(ENDP1, EP_RX_VALID); +#endif /* PIOS_INCLUDE_USB_HID */ #if defined(PIOS_INCLUDE_USB_CDC) - /* Initialize Endpoint 2 (CDC Call Control) */ - SetEPType(ENDP2, EP_INTERRUPT); - SetEPTxAddr(ENDP2, ENDP2_TXADDR); - SetEPTxStatus(ENDP2, EP_TX_NAK); + /* Initialize Endpoint 2 (CDC Call Control) */ + SetEPType(ENDP2, EP_INTERRUPT); + SetEPTxAddr(ENDP2, ENDP2_TXADDR); + SetEPTxStatus(ENDP2, EP_TX_NAK); - SetEPRxAddr(ENDP2, ENDP2_RXADDR); - SetEPRxCount(ENDP2, PIOS_USB_BOARD_CDC_MGMT_LENGTH); - SetEPRxStatus(ENDP2, EP_RX_DIS); + SetEPRxAddr(ENDP2, ENDP2_RXADDR); + SetEPRxCount(ENDP2, PIOS_USB_BOARD_CDC_MGMT_LENGTH); + SetEPRxStatus(ENDP2, EP_RX_DIS); - /* Initialize Endpoint 3 (CDC Data) */ - SetEPType(ENDP3, EP_BULK); - SetEPTxAddr(ENDP3, ENDP3_TXADDR); - SetEPTxStatus(ENDP3, EP_TX_NAK); + /* Initialize Endpoint 3 (CDC Data) */ + SetEPType(ENDP3, EP_BULK); + SetEPTxAddr(ENDP3, ENDP3_TXADDR); + SetEPTxStatus(ENDP3, EP_TX_NAK); - SetEPRxAddr(ENDP3, ENDP3_RXADDR); - SetEPRxCount(ENDP3, PIOS_USB_BOARD_CDC_DATA_LENGTH); - SetEPRxStatus(ENDP3, EP_RX_VALID); + SetEPRxAddr(ENDP3, ENDP3_RXADDR); + SetEPRxCount(ENDP3, PIOS_USB_BOARD_CDC_DATA_LENGTH); + SetEPRxStatus(ENDP3, EP_RX_VALID); -#endif /* PIOS_INCLUDE_USB_CDC */ +#endif /* PIOS_INCLUDE_USB_CDC */ - /* Set this device to response on default address */ - SetDeviceAddress(0); + /* Set this device to response on default address */ + SetDeviceAddress(0); #endif /* STM32F10X_CL */ - bDeviceState = ATTACHED; + bDeviceState = ATTACHED; } /******************************************************************************* @@ -246,13 +246,13 @@ static void PIOS_USBHOOK_Reset(void) *******************************************************************************/ static void PIOS_USBHOOK_SetConfiguration(void) { - if (pInformation->Current_Configuration != 0) { - /* Device configured */ - bDeviceState = CONFIGURED; - } + if (pInformation->Current_Configuration != 0) { + /* Device configured */ + bDeviceState = CONFIGURED; + } - /* Enable transfers */ - PIOS_USB_ChangeConnectionState(pInformation->Current_Configuration != 0); + /* Enable transfers */ + PIOS_USB_ChangeConnectionState(pInformation->Current_Configuration != 0); } /******************************************************************************* @@ -264,7 +264,7 @@ static void PIOS_USBHOOK_SetConfiguration(void) *******************************************************************************/ static void PIOS_USBHOOK_SetDeviceAddress(void) { - bDeviceState = ADDRESSED; + bDeviceState = ADDRESSED; } /******************************************************************************* @@ -275,8 +275,7 @@ static void PIOS_USBHOOK_SetDeviceAddress(void) * Return : None. *******************************************************************************/ static void PIOS_USBHOOK_Status_In(void) -{ -} +{} /******************************************************************************* * Function Name : PIOS_USBHOOK_Status_Out @@ -286,8 +285,7 @@ static void PIOS_USBHOOK_Status_In(void) * Return : None. *******************************************************************************/ static void PIOS_USBHOOK_Status_Out(void) -{ -} +{} /******************************************************************************* * Function Name : PIOS_USBHOOK_Data_Setup @@ -301,95 +299,95 @@ extern const uint8_t *PIOS_USB_CDC_GetLineCoding(uint16_t Length); static RESULT PIOS_USBHOOK_Data_Setup(uint8_t RequestNo) { - uint8_t *(*CopyOutRoutine) (uint16_t); - const uint8_t *(*CopyInRoutine) (uint16_t); + uint8_t *(*CopyOutRoutine)(uint16_t); + const uint8_t *(*CopyInRoutine)(uint16_t); - CopyInRoutine = NULL; - CopyOutRoutine = NULL; + CopyInRoutine = NULL; + CopyOutRoutine = NULL; - switch (Type_Recipient) { - case (STANDARD_REQUEST | INTERFACE_RECIPIENT): - switch (pInformation->USBwIndex0) { + switch (Type_Recipient) { + case (STANDARD_REQUEST | INTERFACE_RECIPIENT): + switch (pInformation->USBwIndex0) { #if defined(PIOS_INCLUDE_USB_CDC) - case 2: /* HID Interface */ + case 2: /* HID Interface */ #else - case 0: /* HID Interface */ + case 0: /* HID Interface */ #endif - switch (RequestNo) { - case GET_DESCRIPTOR: - switch (pInformation->USBwValue1) { - case USB_DESC_TYPE_REPORT: - CopyInRoutine = PIOS_USBHOOK_GetReportDescriptor; - break; - case USB_DESC_TYPE_HID: - CopyInRoutine = PIOS_USBHOOK_GetHIDDescriptor; - break; - } - } - } - break; + switch (RequestNo) { + case GET_DESCRIPTOR: + switch (pInformation->USBwValue1) { + case USB_DESC_TYPE_REPORT: + CopyInRoutine = PIOS_USBHOOK_GetReportDescriptor; + break; + case USB_DESC_TYPE_HID: + CopyInRoutine = PIOS_USBHOOK_GetHIDDescriptor; + break; + } + } + } + break; - case (CLASS_REQUEST | INTERFACE_RECIPIENT): - switch (pInformation->USBwIndex0) { + case (CLASS_REQUEST | INTERFACE_RECIPIENT): + switch (pInformation->USBwIndex0) { #if defined(PIOS_INCLUDE_USB_CDC) - case 2: /* HID Interface */ + case 2: /* HID Interface */ #else - case 0: /* HID Interface */ + case 0: /* HID Interface */ #endif - switch (RequestNo) { - case USB_HID_REQ_GET_PROTOCOL: - CopyInRoutine = PIOS_USBHOOK_GetProtocolValue; - break; - } + switch (RequestNo) { + case USB_HID_REQ_GET_PROTOCOL: + CopyInRoutine = PIOS_USBHOOK_GetProtocolValue; + break; + } - break; + break; #if defined(PIOS_INCLUDE_USB_CDC) - case 0: /* CDC Call Control Interface */ - switch (RequestNo) { - case USB_CDC_REQ_SET_LINE_CODING: - CopyOutRoutine = PIOS_USB_CDC_SetLineCoding; - break; - case USB_CDC_REQ_GET_LINE_CODING: - CopyInRoutine = PIOS_USB_CDC_GetLineCoding; - break; - } + case 0: /* CDC Call Control Interface */ + switch (RequestNo) { + case USB_CDC_REQ_SET_LINE_CODING: + CopyOutRoutine = PIOS_USB_CDC_SetLineCoding; + break; + case USB_CDC_REQ_GET_LINE_CODING: + CopyInRoutine = PIOS_USB_CDC_GetLineCoding; + break; + } - break; + break; - case 1: /* CDC Data Interface */ - switch (RequestNo) { - case 0: - break; - } + case 1: /* CDC Data Interface */ + switch (RequestNo) { + case 0: + break; + } - break; -#endif /* PIOS_INCLUDE_USB_CDC */ - } - break; - } + break; +#endif /* PIOS_INCLUDE_USB_CDC */ + } + break; + } - /* No registered copy routine */ - if ((CopyInRoutine == NULL) && (CopyOutRoutine == NULL)) { - return USB_UNSUPPORT; - } + /* No registered copy routine */ + if ((CopyInRoutine == NULL) && (CopyOutRoutine == NULL)) { + return USB_UNSUPPORT; + } - /* Registered copy in AND copy out routine */ - if ((CopyInRoutine != NULL) && (CopyOutRoutine != NULL)) { - /* This should never happen */ - return USB_UNSUPPORT; - } + /* Registered copy in AND copy out routine */ + if ((CopyInRoutine != NULL) && (CopyOutRoutine != NULL)) { + /* This should never happen */ + return USB_UNSUPPORT; + } - if (CopyInRoutine != NULL) { - pInformation->Ctrl_Info.CopyDataIn = CopyInRoutine; - pInformation->Ctrl_Info.Usb_wOffset = 0; - (*CopyInRoutine) (0); - } else if (CopyOutRoutine != NULL) { - pInformation->Ctrl_Info.CopyDataOut = CopyOutRoutine; - pInformation->Ctrl_Info.Usb_rOffset = 0; - (*CopyOutRoutine) (0); - } + if (CopyInRoutine != NULL) { + pInformation->Ctrl_Info.CopyDataIn = CopyInRoutine; + pInformation->Ctrl_Info.Usb_wOffset = 0; + (*CopyInRoutine)(0); + } else if (CopyOutRoutine != NULL) { + pInformation->Ctrl_Info.CopyDataOut = CopyOutRoutine; + pInformation->Ctrl_Info.Usb_rOffset = 0; + (*CopyOutRoutine)(0); + } - return USB_SUCCESS; + return USB_SUCCESS; } /******************************************************************************* @@ -403,38 +401,40 @@ extern RESULT PIOS_USB_CDC_SetControlLineState(void); static RESULT PIOS_USBHOOK_NoData_Setup(uint8_t RequestNo) { - switch (Type_Recipient) { - case (CLASS_REQUEST | INTERFACE_RECIPIENT): - switch (pInformation->USBwIndex0) { + switch (Type_Recipient) { + case (CLASS_REQUEST | INTERFACE_RECIPIENT): + switch (pInformation->USBwIndex0) { #if defined(PIOS_INCLUDE_USB_CDC) - case 2: /* HID */ + case 2: /* HID */ #else - case 0: /* HID */ + case 0: /* HID */ #endif - switch (RequestNo) { - case USB_HID_REQ_SET_PROTOCOL: - return PIOS_USBHOOK_SetProtocol(); - break; - } + switch (RequestNo) { + case USB_HID_REQ_SET_PROTOCOL: + return PIOS_USBHOOK_SetProtocol(); - break; + break; + } + + break; #if defined(PIOS_INCLUDE_USB_CDC) - case 0: /* CDC Call Control Interface */ - switch (RequestNo) { - case USB_CDC_REQ_SET_CONTROL_LINE_STATE: - return PIOS_USB_CDC_SetControlLineState(); - break; - } + case 0: /* CDC Call Control Interface */ + switch (RequestNo) { + case USB_CDC_REQ_SET_CONTROL_LINE_STATE: + return PIOS_USB_CDC_SetControlLineState(); - break; -#endif /* PIOS_INCLUDE_USB_CDC */ - } + break; + } - break; - } + break; +#endif /* PIOS_INCLUDE_USB_CDC */ + } - return USB_UNSUPPORT; + break; + } + + return USB_UNSUPPORT; } /******************************************************************************* @@ -446,7 +446,7 @@ static RESULT PIOS_USBHOOK_NoData_Setup(uint8_t RequestNo) *******************************************************************************/ static const uint8_t *PIOS_USBHOOK_GetDeviceDescriptor(uint16_t Length) { - return Standard_GetDescriptorData(Length, &Device_Descriptor); + return Standard_GetDescriptorData(Length, &Device_Descriptor); } /******************************************************************************* @@ -458,7 +458,7 @@ static const uint8_t *PIOS_USBHOOK_GetDeviceDescriptor(uint16_t Length) *******************************************************************************/ static const uint8_t *PIOS_USBHOOK_GetConfigDescriptor(uint16_t Length) { - return Standard_GetDescriptorData(Length, &Config_Descriptor); + return Standard_GetDescriptorData(Length, &Config_Descriptor); } /******************************************************************************* @@ -470,12 +470,13 @@ static const uint8_t *PIOS_USBHOOK_GetConfigDescriptor(uint16_t Length) *******************************************************************************/ static const uint8_t *PIOS_USBHOOK_GetStringDescriptor(uint16_t Length) { - uint8_t wValue0 = pInformation->USBwValue0; - if (wValue0 > 4) { - return NULL; - } else { - return Standard_GetDescriptorData(Length, &String_Descriptor[wValue0]); - } + uint8_t wValue0 = pInformation->USBwValue0; + + if (wValue0 > 4) { + return NULL; + } else { + return Standard_GetDescriptorData(Length, &String_Descriptor[wValue0]); + } } /******************************************************************************* @@ -487,7 +488,7 @@ static const uint8_t *PIOS_USBHOOK_GetStringDescriptor(uint16_t Length) *******************************************************************************/ static const uint8_t *PIOS_USBHOOK_GetReportDescriptor(uint16_t Length) { - return Standard_GetDescriptorData(Length, &Hid_Report_Descriptor); + return Standard_GetDescriptorData(Length, &Hid_Report_Descriptor); } /******************************************************************************* @@ -499,7 +500,7 @@ static const uint8_t *PIOS_USBHOOK_GetReportDescriptor(uint16_t Length) *******************************************************************************/ static const uint8_t *PIOS_USBHOOK_GetHIDDescriptor(uint16_t Length) { - return Standard_GetDescriptorData(Length, &Hid_Descriptor); + return Standard_GetDescriptorData(Length, &Hid_Descriptor); } /******************************************************************************* @@ -513,12 +514,12 @@ static const uint8_t *PIOS_USBHOOK_GetHIDDescriptor(uint16_t Length) *******************************************************************************/ static RESULT PIOS_USBHOOK_Get_Interface_Setting(uint8_t Interface, uint8_t AlternateSetting) { - if (AlternateSetting > 0) { - return USB_UNSUPPORT; - } else if (Interface > 0) { - return USB_UNSUPPORT; - } - return USB_SUCCESS; + if (AlternateSetting > 0) { + return USB_UNSUPPORT; + } else if (Interface > 0) { + return USB_UNSUPPORT; + } + return USB_SUCCESS; } /******************************************************************************* @@ -530,9 +531,10 @@ static RESULT PIOS_USBHOOK_Get_Interface_Setting(uint8_t Interface, uint8_t Alte *******************************************************************************/ static RESULT PIOS_USBHOOK_SetProtocol(void) { - uint8_t wValue0 = pInformation->USBwValue0; - ProtocolValue = wValue0; - return USB_SUCCESS; + uint8_t wValue0 = pInformation->USBwValue0; + + ProtocolValue = wValue0; + return USB_SUCCESS; } /******************************************************************************* @@ -544,12 +546,12 @@ static RESULT PIOS_USBHOOK_SetProtocol(void) *******************************************************************************/ static const uint8_t *PIOS_USBHOOK_GetProtocolValue(uint16_t Length) { - if (Length == 0) { - pInformation->Ctrl_Info.Usb_wLength = 1; - return NULL; - } else { - return (uint8_t *) (&ProtocolValue); - } + if (Length == 0) { + pInformation->Ctrl_Info.Usb_wLength = 1; + return NULL; + } else { + return (uint8_t *)(&ProtocolValue); + } } #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/stm32f10x/pios_wdg.c b/flight/pios/stm32f10x/pios_wdg.c index 0f3e9d600..3f800be2c 100644 --- a/flight/pios/stm32f10x/pios_wdg.c +++ b/flight/pios/stm32f10x/pios_wdg.c @@ -8,12 +8,12 @@ * * @file pios_spi.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Hardware Abstraction Layer for SPI ports of STM32 * @see The GNU Public License (GPL) Version 3 * @notes * - * The PIOS Watchdog provides a HAL to initialize a watchdog + * The PIOS Watchdog provides a HAL to initialize a watchdog * *****************************************************************************/ /* @@ -40,21 +40,21 @@ #include "stm32f10x_dbgmcu.h" static struct wdg_configuration { - uint16_t used_flags; - uint16_t bootup_flags; + uint16_t used_flags; + uint16_t bootup_flags; } wdg_configuration; -/** +/** * @brief Initialize the watchdog timer for a specified timeout * - * It is important to note that this function returns the achieved timeout - * for this hardware. For hardware indendence this should be checked when - * scheduling updates. Other hardware dependent details may need to be - * considered such as a window time which sets a minimum update time, - * and this function should return a recommended delay for clearing. + * It is important to note that this function returns the achieved timeout + * for this hardware. For hardware indendence this should be checked when + * scheduling updates. Other hardware dependent details may need to be + * considered such as a window time which sets a minimum update time, + * and this function should return a recommended delay for clearing. * * For the STM32 nominal clock rate is 32 khz, but for the maximum clock rate of - * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is + * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is * set in the watchdog assumes the nominal clock rate, but the delay for FreeRTOS * to use is 75% of the minimal delay. * @@ -62,107 +62,109 @@ static struct wdg_configuration { */ uint16_t PIOS_WDG_Init() { - uint16_t delay = ((uint32_t) PIOS_WATCHDOG_TIMEOUT * 60) / 16; - if (delay > 0x0fff) - delay = 0x0fff; -#if defined(PIOS_INCLUDE_WDG) - DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); // make the watchdog stop counting in debug mode - IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); - IWDG_SetPrescaler(IWDG_Prescaler_16); - IWDG_SetReload(delay); - IWDG_ReloadCounter(); - IWDG_Enable(); + uint16_t delay = ((uint32_t)PIOS_WATCHDOG_TIMEOUT * 60) / 16; - // watchdog flags now stored in backup registers - PWR_BackupAccessCmd(ENABLE); - - BKP_WriteBackupRegister(PIOS_WDG_REGISTER, 0x0); - wdg_configuration.bootup_flags = BKP_ReadBackupRegister(PIOS_WDG_REGISTER); + if (delay > 0x0fff) { + delay = 0x0fff; + } +#if defined(PIOS_INCLUDE_WDG) + DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); // make the watchdog stop counting in debug mode + IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); + IWDG_SetPrescaler(IWDG_Prescaler_16); + IWDG_SetReload(delay); + IWDG_ReloadCounter(); + IWDG_Enable(); + + // watchdog flags now stored in backup registers + PWR_BackupAccessCmd(ENABLE); + + BKP_WriteBackupRegister(PIOS_WDG_REGISTER, 0x0); + wdg_configuration.bootup_flags = BKP_ReadBackupRegister(PIOS_WDG_REGISTER); #endif - return delay; + return delay; } /** - * @brief Register a module against the watchdog - * + * @brief Register a module against the watchdog + * * There are two ways to use PIOS WDG: this is for when - * multiple modules must be monitored. In this case they - * must first register against the watchdog system and + * multiple modules must be monitored. In this case they + * must first register against the watchdog system and * only when all of the modules have been updated with the - * watchdog be cleared. Each module must have its own - * bit in the 16 bit + * watchdog be cleared. Each module must have its own + * bit in the 16 bit * * @param[in] flag the bit this module wants to use * @returns True if that bit is unregistered */ -bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) +bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) { - // flag are being registered so we are in module initialization phase - // clear the WDG to prevent timeout while initializing modules. (OP-815) - PIOS_WDG_Clear(); - - /* Fail if flag already registered */ - if(wdg_configuration.used_flags & flag_requested) - return false; - - // FIXME: Protect with semaphore - wdg_configuration.used_flags |= flag_requested; - - return true; + // flag are being registered so we are in module initialization phase + // clear the WDG to prevent timeout while initializing modules. (OP-815) + PIOS_WDG_Clear(); + + /* Fail if flag already registered */ + if (wdg_configuration.used_flags & flag_requested) { + return false; + } + + // FIXME: Protect with semaphore + wdg_configuration.used_flags |= flag_requested; + + return true; } /** * @brief Function called by modules to indicate they are still running * - * This function will set this flag in the active flags register (which is + * This function will set this flag in the active flags register (which is * a backup regsiter) and if all the registered flags are set will clear * the watchdog and set only this flag in the backup register * * @param[in] flag the flag to set * @return true if the watchdog cleared, false if flags are pending */ -bool PIOS_WDG_UpdateFlag(uint16_t flag) -{ - // we can probably avoid using a semaphore here which will be good for - // efficiency and not blocking critical tasks. race condition could - // overwrite their flag update, but unlikely to block _all_ of them - // for the timeout window - uint16_t cur_flags = BKP_ReadBackupRegister(PIOS_WDG_REGISTER); - - if((cur_flags | flag) == wdg_configuration.used_flags) { - PIOS_WDG_Clear(); - BKP_WriteBackupRegister(PIOS_WDG_REGISTER, flag); - return true; - } else { - BKP_WriteBackupRegister(PIOS_WDG_REGISTER, cur_flags | flag); - return false; - } - +bool PIOS_WDG_UpdateFlag(uint16_t flag) +{ + // we can probably avoid using a semaphore here which will be good for + // efficiency and not blocking critical tasks. race condition could + // overwrite their flag update, but unlikely to block _all_ of them + // for the timeout window + uint16_t cur_flags = BKP_ReadBackupRegister(PIOS_WDG_REGISTER); + + if ((cur_flags | flag) == wdg_configuration.used_flags) { + PIOS_WDG_Clear(); + BKP_WriteBackupRegister(PIOS_WDG_REGISTER, flag); + return true; + } else { + BKP_WriteBackupRegister(PIOS_WDG_REGISTER, cur_flags | flag); + return false; + } } -/** +/** * @brief Returns the flags that were set at bootup - * - * This is used for diagnostics, if only one flag not set this + * + * This is used for diagnostics, if only one flag not set this * was likely the module that wasn't running before reset - * + * * @return The active flags register from bootup */ uint16_t PIOS_WDG_GetBootupFlags() { - return wdg_configuration.bootup_flags; + return wdg_configuration.bootup_flags; } -/** +/** * @brief Returns the currently active flags - * + * * For external monitoring - * + * * @return The active flags register */ uint16_t PIOS_WDG_GetActiveFlags() { - return BKP_ReadBackupRegister(PIOS_WDG_REGISTER); + return BKP_ReadBackupRegister(PIOS_WDG_REGISTER); } /** @@ -173,7 +175,7 @@ uint16_t PIOS_WDG_GetActiveFlags() void PIOS_WDG_Clear(void) { #if defined(PIOS_INCLUDE_WDG) - IWDG_ReloadCounter(); + IWDG_ReloadCounter(); #endif } diff --git a/flight/pios/stm32f4xx/inc/pios_i2c_config.h b/flight/pios/stm32f4xx/inc/pios_i2c_config.h index e269c0fe4..adabdcdc4 100644 --- a/flight/pios/stm32f4xx/inc/pios_i2c_config.h +++ b/flight/pios/stm32f4xx/inc/pios_i2c_config.h @@ -11,57 +11,57 @@ /** * Generic I2C configuration for the STM32F4xx */ -#define I2C_CONFIG(_i2c, _scl_gpio, _scl_pin, _sda_gpio, _sda_pin) \ -{ \ - .regs = _i2c, \ - .remap = GPIO_AF_ ## _i2c, \ - .init = { \ - .I2C_ClockSpeed = 400000, /* bits/s */ \ - .I2C_Mode = I2C_Mode_I2C, \ - .I2C_DutyCycle = I2C_DutyCycle_2, \ - .I2C_OwnAddress1 = 0, \ - .I2C_Ack = I2C_Ack_Enable, \ - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, \ - }, \ - .transfer_timeout_ms = 50, \ - .scl = { \ - .gpio = _scl_gpio, \ - .init = { \ - .GPIO_Pin = _scl_pin, \ - .GPIO_Mode = GPIO_Mode_AF, \ - .GPIO_Speed = GPIO_Speed_50MHz, \ - .GPIO_OType = GPIO_OType_OD, \ - .GPIO_PuPd = GPIO_PuPd_NOPULL, \ - }, \ - }, \ - .sda = { \ - .gpio = _sda_gpio, \ - .init = { \ - .GPIO_Pin = _sda_pin, \ - .GPIO_Mode = GPIO_Mode_AF, \ - .GPIO_Speed = GPIO_Speed_50MHz, \ - .GPIO_OType = GPIO_OType_OD, \ - .GPIO_PuPd = GPIO_PuPd_NOPULL, \ - }, \ - }, \ - .event = { \ - .flags = 0, /* FIXME: check this */ \ - .init = { \ - .NVIC_IRQChannel = _i2c ## _EV_IRQn, \ - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, \ - .NVIC_IRQChannelSubPriority = 0, \ - .NVIC_IRQChannelCmd = ENABLE, \ - }, \ - }, \ - .error = { \ - .flags = 0, /* FIXME: check this */ \ - .init = { \ - .NVIC_IRQChannel = _i2c ## _ER_IRQn, \ - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, \ - .NVIC_IRQChannelSubPriority = 0, \ - .NVIC_IRQChannelCmd = ENABLE, \ - }, \ - }, \ -} +#define I2C_CONFIG(_i2c, _scl_gpio, _scl_pin, _sda_gpio, _sda_pin) \ + { \ + .regs = _i2c, \ + .remap = GPIO_AF_##_i2c, \ + .init = { \ + .I2C_ClockSpeed = 400000, /* bits/s */ \ + .I2C_Mode = I2C_Mode_I2C, \ + .I2C_DutyCycle = I2C_DutyCycle_2, \ + .I2C_OwnAddress1 = 0, \ + .I2C_Ack = I2C_Ack_Enable, \ + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, \ + }, \ + .transfer_timeout_ms = 50, \ + .scl = { \ + .gpio = _scl_gpio, \ + .init = { \ + .GPIO_Pin = _scl_pin, \ + .GPIO_Mode = GPIO_Mode_AF, \ + .GPIO_Speed = GPIO_Speed_50MHz, \ + .GPIO_OType = GPIO_OType_OD, \ + .GPIO_PuPd = GPIO_PuPd_NOPULL, \ + }, \ + }, \ + .sda = { \ + .gpio = _sda_gpio, \ + .init = { \ + .GPIO_Pin = _sda_pin, \ + .GPIO_Mode = GPIO_Mode_AF, \ + .GPIO_Speed = GPIO_Speed_50MHz, \ + .GPIO_OType = GPIO_OType_OD, \ + .GPIO_PuPd = GPIO_PuPd_NOPULL, \ + }, \ + }, \ + .event = { \ + .flags = 0, /* FIXME: check this */ \ + .init = { \ + .NVIC_IRQChannel = _i2c##_EV_IRQn, \ + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, \ + .NVIC_IRQChannelSubPriority = 0, \ + .NVIC_IRQChannelCmd = ENABLE, \ + }, \ + }, \ + .error = { \ + .flags = 0, /* FIXME: check this */ \ + .init = { \ + .NVIC_IRQChannel = _i2c##_ER_IRQn, \ + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, \ + .NVIC_IRQChannelSubPriority = 0, \ + .NVIC_IRQChannelCmd = ENABLE, \ + }, \ + }, \ + } #endif /* PIOS_I2C_CONFIG_H_ */ diff --git a/flight/pios/stm32f4xx/inc/pios_usart_config.h b/flight/pios/stm32f4xx/inc/pios_usart_config.h index 4abed4e15..7c6d549dc 100644 --- a/flight/pios/stm32f4xx/inc/pios_usart_config.h +++ b/flight/pios/stm32f4xx/inc/pios_usart_config.h @@ -12,45 +12,45 @@ * Generic USART configuration structure for an STM32F2xx port. */ #define USART_CONFIG(_usart, _baudrate, _rx_gpio, _rx_pin, _tx_gpio, _tx_pin) \ -{ \ - .regs = _usart, \ - .remap = GPIO_AF_ ## _usart, \ - .init = { \ - .USART_BaudRate = _baudrate, \ - .USART_WordLength = USART_WordLength_8b, \ - .USART_Parity = USART_Parity_No, \ - .USART_StopBits = USART_StopBits_1, \ - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, \ - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, \ - }, \ - .irq = { \ - .init = { \ - .NVIC_IRQChannel = _usart ## _IRQn, \ - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, \ - .NVIC_IRQChannelSubPriority = 0, \ - .NVIC_IRQChannelCmd = ENABLE, \ - }, \ - }, \ - .rx = { \ - .gpio = _rx_gpio, \ - .init = { \ - .GPIO_Pin = _rx_pin, \ - .GPIO_Mode = GPIO_Mode_AF, \ - .GPIO_Speed = GPIO_Speed_50MHz, \ - .GPIO_OType = GPIO_OType_PP, \ - .GPIO_PuPd = GPIO_PuPd_UP, \ - }, \ - }, \ - .tx = { \ - .gpio = _tx_gpio, \ - .init = { \ - .GPIO_Pin = _tx_pin, \ - .GPIO_Mode = GPIO_Mode_AF, \ - .GPIO_Speed = GPIO_Speed_50MHz, \ - .GPIO_OType = GPIO_OType_PP, \ - .GPIO_PuPd = GPIO_PuPd_UP, \ - }, \ - }, \ -} + { \ + .regs = _usart, \ + .remap = GPIO_AF_##_usart, \ + .init = { \ + .USART_BaudRate = _baudrate, \ + .USART_WordLength = USART_WordLength_8b, \ + .USART_Parity = USART_Parity_No, \ + .USART_StopBits = USART_StopBits_1, \ + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, \ + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, \ + }, \ + .irq = { \ + .init = { \ + .NVIC_IRQChannel = _usart##_IRQn, \ + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, \ + .NVIC_IRQChannelSubPriority = 0, \ + .NVIC_IRQChannelCmd = ENABLE, \ + }, \ + }, \ + .rx = { \ + .gpio = _rx_gpio, \ + .init = { \ + .GPIO_Pin = _rx_pin, \ + .GPIO_Mode = GPIO_Mode_AF, \ + .GPIO_Speed = GPIO_Speed_50MHz, \ + .GPIO_OType = GPIO_OType_PP, \ + .GPIO_PuPd = GPIO_PuPd_UP, \ + }, \ + }, \ + .tx = { \ + .gpio = _tx_gpio, \ + .init = { \ + .GPIO_Pin = _tx_pin, \ + .GPIO_Mode = GPIO_Mode_AF, \ + .GPIO_Speed = GPIO_Speed_50MHz, \ + .GPIO_OType = GPIO_OType_PP, \ + .GPIO_PuPd = GPIO_PuPd_UP, \ + }, \ + }, \ + } #endif /* PIOS_USART_CONFIG_H_ */ diff --git a/flight/pios/stm32f4xx/inc/stm32f4xx_conf.h b/flight/pios/stm32f4xx/inc/stm32f4xx_conf.h index 673f5f3d0..ff66b4ab6 100644 --- a/flight/pios/stm32f4xx/inc/stm32f4xx_conf.h +++ b/flight/pios/stm32f4xx/inc/stm32f4xx_conf.h @@ -1,23 +1,23 @@ /** - ****************************************************************************** - * @file Project/STM32F4xx_StdPeriph_Templates/stm32f4xx_conf.h - * @author MCD Application Team - * @version V1.0.0 - * @date 30-September-2011 - * @brief Library configuration file. - ****************************************************************************** - * @attention - * - * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS - * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE - * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY - * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING - * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE - * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. - * - *

© COPYRIGHT 2011 STMicroelectronics

- ****************************************************************************** - */ + ****************************************************************************** + * @file Project/STM32F4xx_StdPeriph_Templates/stm32f4xx_conf.h + * @author MCD Application Team + * @version V1.0.0 + * @date 30-September-2011 + * @brief Library configuration file. + ****************************************************************************** + * @attention + * + * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS + * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE + * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY + * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING + * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * + *

© COPYRIGHT 2011 STMicroelectronics

+ ****************************************************************************** + */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef __STM32F4xx_CONF_H @@ -55,13 +55,13 @@ /* Exported types ------------------------------------------------------------*/ /* Exported constants --------------------------------------------------------*/ -/* If an external clock source is used, then the value of the following define - should be set to the value of the external clock source, else, if no external +/* If an external clock source is used, then the value of the following define + should be set to the value of the external clock source, else, if no external clock is used, keep this define commented */ /*#define I2S_EXTERNAL_CLOCK_VAL 12288000 */ /* Value of the external clock in Hz */ -/* Uncomment the line below to expanse the "assert_param" macro in the +/* Uncomment the line below to expanse the "assert_param" macro in the Standard Peripheral Library drivers code */ /* #define USE_FULL_ASSERT 1 */ @@ -69,16 +69,16 @@ #ifdef USE_FULL_ASSERT /** - * @brief The assert_param macro is used for function's parameters check. - * @param expr: If expr is false, it calls assert_failed function - * which reports the name of the source file and the source - * line number of the call that failed. - * If expr is true, it returns no value. - * @retval None - */ + * @brief The assert_param macro is used for function's parameters check. + * @param expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) /* Exported functions ------------------------------------------------------- */ - void assert_failed(uint8_t* file, uint32_t line); +void assert_failed(uint8_t *file, uint32_t line); #else #define assert_param(expr) ((void)0) #endif /* USE_FULL_ASSERT */ diff --git a/flight/pios/stm32f4xx/pios_adc.c b/flight/pios/stm32f4xx/pios_adc.c index 1f26dee4e..d4b772b22 100644 --- a/flight/pios/stm32f4xx/pios_adc.c +++ b/flight/pios/stm32f4xx/pios_adc.c @@ -6,25 +6,25 @@ * @brief STM32F4xx ADC PIOS interface * @{ * - * @file pios_adc.c + * @file pios_adc.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @author Michael Smith Copyright (C) 2011. * @brief Analog to Digital converstion routines * @see The GNU Public License (GPL) Version 3 *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -50,7 +50,7 @@ #if !defined(PIOS_ADC_MAX_SAMPLES) -#define PIOS_ADC_MAX_SAMPLES 0 +#define PIOS_ADC_MAX_SAMPLES 0 #endif #if !defined(PIOS_ADC_MAX_OVERSAMPLING) @@ -58,39 +58,39 @@ #endif #if !defined(PIOS_ADC_USE_ADC2) -#define PIOS_ADC_USE_ADC2 0 +#define PIOS_ADC_USE_ADC2 0 #endif #if !defined(PIOS_ADC_NUM_CHANNELS) -#define PIOS_ADC_NUM_CHANNELS 0 +#define PIOS_ADC_NUM_CHANNELS 0 #endif // Private types enum pios_adc_dev_magic { - PIOS_ADC_DEV_MAGIC = 0x58375124, + PIOS_ADC_DEV_MAGIC = 0x58375124, }; struct pios_adc_dev { - const struct pios_adc_cfg * cfg; - ADCCallback callback_function; + const struct pios_adc_cfg *cfg; + ADCCallback callback_function; #if defined(PIOS_INCLUDE_FREERTOS) - xQueueHandle data_queue; + xQueueHandle data_queue; #endif - volatile int16_t *valid_data_buffer; - volatile uint8_t adc_oversample; - uint8_t dma_block_size; - uint16_t dma_half_buffer_size; -// int16_t fir_coeffs[PIOS_ADC_MAX_SAMPLES+1] __attribute__ ((aligned(4))); -// volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES] __attribute__ ((aligned(4))); -// float downsampled_buffer[PIOS_ADC_NUM_CHANNELS] __attribute__ ((aligned(4))); - enum pios_adc_dev_magic magic; + volatile int16_t *valid_data_buffer; + volatile uint8_t adc_oversample; + uint8_t dma_block_size; + uint16_t dma_half_buffer_size; +// int16_t fir_coeffs[PIOS_ADC_MAX_SAMPLES+1] __attribute__ ((aligned(4))); +// volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES] __attribute__ ((aligned(4))); +// float downsampled_buffer[PIOS_ADC_NUM_CHANNELS] __attribute__ ((aligned(4))); + enum pios_adc_dev_magic magic; }; -struct pios_adc_dev * pios_adc_dev; +struct pios_adc_dev *pios_adc_dev; // Private functions void PIOS_ADC_downsample_data(); -static struct pios_adc_dev * PIOS_ADC_Allocate(); +static struct pios_adc_dev *PIOS_ADC_Allocate(); static bool PIOS_ADC_validate(struct pios_adc_dev *); #if defined(PIOS_INCLUDE_ADC) @@ -100,19 +100,19 @@ static void init_adc(void); #endif struct dma_config { - GPIO_TypeDef *port; - uint32_t pin; - uint32_t channel; + GPIO_TypeDef *port; + uint32_t pin; + uint32_t channel; }; struct adc_accumulator { - uint32_t accumulator; - uint32_t count; + uint32_t accumulator; + uint32_t count; }; #if defined(PIOS_INCLUDE_ADC) static const struct dma_config config[] = PIOS_DMA_PIN_CONFIG; -#define PIOS_ADC_NUM_PINS (sizeof(config) / sizeof(config[0])) +#define PIOS_ADC_NUM_PINS (sizeof(config) / sizeof(config[0])) static struct adc_accumulator accumulator[PIOS_ADC_NUM_PINS]; @@ -121,163 +121,166 @@ static uint16_t adc_raw_buffer[2][PIOS_ADC_MAX_SAMPLES][PIOS_ADC_NUM_PINS]; #endif #if defined(PIOS_INCLUDE_ADC) -static void -init_pins(void) +static void init_pins(void) { - /* Setup analog pins */ - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; - - for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; ++i) { - if (config[i].port == NULL) - continue; - GPIO_InitStructure.GPIO_Pin = config[i].pin; - GPIO_Init(config[i].port, &GPIO_InitStructure); - } + /* Setup analog pins */ + GPIO_InitTypeDef GPIO_InitStructure; + + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; + + for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; ++i) { + if (config[i].port == NULL) { + continue; + } + GPIO_InitStructure.GPIO_Pin = config[i].pin; + GPIO_Init(config[i].port, &GPIO_InitStructure); + } } -static void -init_dma(void) +static void init_dma(void) { - /* Disable interrupts */ - DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->dma.irq.flags, DISABLE); + /* Disable interrupts */ + DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->dma.irq.flags, DISABLE); - /* Configure DMA channel */ - DMA_DeInit(pios_adc_dev->cfg->dma.rx.channel); - DMA_InitTypeDef DMAInit = pios_adc_dev->cfg->dma.rx.init; - DMAInit.DMA_Memory0BaseAddr = (uint32_t)&adc_raw_buffer[0]; - DMAInit.DMA_BufferSize = sizeof(adc_raw_buffer[0]) / sizeof(uint16_t); - DMAInit.DMA_DIR = DMA_DIR_PeripheralToMemory; - DMAInit.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMAInit.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMAInit.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; - DMAInit.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; - DMAInit.DMA_Mode = DMA_Mode_Circular; - DMAInit.DMA_Priority = DMA_Priority_Low; - DMAInit.DMA_FIFOMode = DMA_FIFOMode_Disable; - DMAInit.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; - DMAInit.DMA_MemoryBurst = DMA_MemoryBurst_Single; - DMAInit.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + /* Configure DMA channel */ + DMA_DeInit(pios_adc_dev->cfg->dma.rx.channel); + DMA_InitTypeDef DMAInit = pios_adc_dev->cfg->dma.rx.init; + DMAInit.DMA_Memory0BaseAddr = (uint32_t)&adc_raw_buffer[0]; + DMAInit.DMA_BufferSize = sizeof(adc_raw_buffer[0]) / sizeof(uint16_t); + DMAInit.DMA_DIR = DMA_DIR_PeripheralToMemory; + DMAInit.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMAInit.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMAInit.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; + DMAInit.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; + DMAInit.DMA_Mode = DMA_Mode_Circular; + DMAInit.DMA_Priority = DMA_Priority_Low; + DMAInit.DMA_FIFOMode = DMA_FIFOMode_Disable; + DMAInit.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; + DMAInit.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMAInit.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; - DMA_Init(pios_adc_dev->cfg->dma.rx.channel, &DMAInit); /* channel is actually stream ... */ + DMA_Init(pios_adc_dev->cfg->dma.rx.channel, &DMAInit); /* channel is actually stream ... */ - /* configure for double-buffered mode and interrupt on every buffer flip */ - DMA_DoubleBufferModeConfig(pios_adc_dev->cfg->dma.rx.channel, (uint32_t)&adc_raw_buffer[1], DMA_Memory_0); - DMA_DoubleBufferModeCmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE); - DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_TC, ENABLE); - //DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_HT, ENABLE); + /* configure for double-buffered mode and interrupt on every buffer flip */ + DMA_DoubleBufferModeConfig(pios_adc_dev->cfg->dma.rx.channel, (uint32_t)&adc_raw_buffer[1], DMA_Memory_0); + DMA_DoubleBufferModeCmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE); + DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_TC, ENABLE); + // DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_HT, ENABLE); - /* enable DMA */ - DMA_Cmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE); + /* enable DMA */ + DMA_Cmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE); - /* Configure DMA interrupt */ - NVIC_InitTypeDef NVICInit = pios_adc_dev->cfg->dma.irq.init; - NVIC_Init(&NVICInit); + /* Configure DMA interrupt */ + NVIC_InitTypeDef NVICInit = pios_adc_dev->cfg->dma.irq.init; + NVIC_Init(&NVICInit); } -static void -init_adc(void) +static void init_adc(void) { - RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); + RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); - ADC_DeInit(); + ADC_DeInit(); - /* turn on VREFInt in case we need it */ - ADC_TempSensorVrefintCmd(ENABLE); + /* turn on VREFInt in case we need it */ + ADC_TempSensorVrefintCmd(ENABLE); - /* Do common ADC init */ - ADC_CommonInitTypeDef ADC_CommonInitStructure; - ADC_CommonStructInit(&ADC_CommonInitStructure); - ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; - ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8; - ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; - ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; - ADC_CommonInit(&ADC_CommonInitStructure); + /* Do common ADC init */ + ADC_CommonInitTypeDef ADC_CommonInitStructure; + ADC_CommonStructInit(&ADC_CommonInitStructure); + ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; + ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8; + ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; + ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; + ADC_CommonInit(&ADC_CommonInitStructure); - ADC_InitTypeDef ADC_InitStructure; - ADC_StructInit(&ADC_InitStructure); - ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; - ADC_InitStructure.ADC_ScanConvMode = ENABLE; - ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; - ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; - ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; - ADC_InitStructure.ADC_NbrOfConversion = ((PIOS_ADC_NUM_PINS)/* >> 1*/); - ADC_Init(pios_adc_dev->cfg->adc_dev, &ADC_InitStructure); + ADC_InitTypeDef ADC_InitStructure; + ADC_StructInit(&ADC_InitStructure); + ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; + ADC_InitStructure.ADC_ScanConvMode = ENABLE; + ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; + ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; + ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; + ADC_InitStructure.ADC_NbrOfConversion = ((PIOS_ADC_NUM_PINS) /* >> 1*/); + ADC_Init(pios_adc_dev->cfg->adc_dev, &ADC_InitStructure); - /* Enable DMA request */ - ADC_DMACmd(pios_adc_dev->cfg->adc_dev, ENABLE); + /* Enable DMA request */ + ADC_DMACmd(pios_adc_dev->cfg->adc_dev, ENABLE); - /* Configure input scan */ - for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) { - ADC_RegularChannelConfig(pios_adc_dev->cfg->adc_dev, - config[i].channel, - i+1, - ADC_SampleTime_56Cycles); /* XXX this is totally arbitrary... */ - } + /* Configure input scan */ + for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) { + ADC_RegularChannelConfig(pios_adc_dev->cfg->adc_dev, + config[i].channel, + i + 1, + ADC_SampleTime_56Cycles); /* XXX this is totally arbitrary... */ + } - ADC_DMARequestAfterLastTransferCmd(pios_adc_dev->cfg->adc_dev, ENABLE); + ADC_DMARequestAfterLastTransferCmd(pios_adc_dev->cfg->adc_dev, ENABLE); - /* Finally start initial conversion */ - ADC_Cmd(pios_adc_dev->cfg->adc_dev, ENABLE); - ADC_ContinuousModeCmd(pios_adc_dev->cfg->adc_dev, ENABLE); - ADC_SoftwareStartConv(pios_adc_dev->cfg->adc_dev); + /* Finally start initial conversion */ + ADC_Cmd(pios_adc_dev->cfg->adc_dev, ENABLE); + ADC_ContinuousModeCmd(pios_adc_dev->cfg->adc_dev, ENABLE); + ADC_SoftwareStartConv(pios_adc_dev->cfg->adc_dev); } -#endif +#endif /* if defined(PIOS_INCLUDE_ADC) */ -static bool PIOS_ADC_validate(struct pios_adc_dev * dev) +static bool PIOS_ADC_validate(struct pios_adc_dev *dev) { - if (dev == NULL) - return false; - - return (dev->magic == PIOS_ADC_DEV_MAGIC); + if (dev == NULL) { + return false; + } + + return dev->magic == PIOS_ADC_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_adc_dev * PIOS_ADC_Allocate() +static struct pios_adc_dev *PIOS_ADC_Allocate() { - struct pios_adc_dev * adc_dev; - - adc_dev = (struct pios_adc_dev *)pvPortMalloc(sizeof(*adc_dev)); - if (!adc_dev) return (NULL); - - adc_dev->magic = PIOS_ADC_DEV_MAGIC; - return(adc_dev); + struct pios_adc_dev *adc_dev; + + adc_dev = (struct pios_adc_dev *)pvPortMalloc(sizeof(*adc_dev)); + if (!adc_dev) { + return NULL; + } + + adc_dev->magic = PIOS_ADC_DEV_MAGIC; + return adc_dev; } #else #if defined(PIOS_INCLUDE_ADC) #error Not implemented #endif -static struct pios_adc_dev * PIOS_ADC_Allocate() +static struct pios_adc_dev *PIOS_ADC_Allocate() { - return (struct pios_adc_dev *) NULL; + return (struct pios_adc_dev *)NULL; } #endif /** * @brief Init the ADC. */ -int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg) +int32_t PIOS_ADC_Init(const struct pios_adc_cfg *cfg) { - pios_adc_dev = PIOS_ADC_Allocate(); - if (pios_adc_dev == NULL) - return -1; - - pios_adc_dev->cfg = cfg; - pios_adc_dev->callback_function = NULL; - + pios_adc_dev = PIOS_ADC_Allocate(); + if (pios_adc_dev == NULL) { + return -1; + } + + pios_adc_dev->cfg = cfg; + pios_adc_dev->callback_function = NULL; + #if defined(PIOS_INCLUDE_FREERTOS) - pios_adc_dev->data_queue = NULL; + pios_adc_dev->data_queue = NULL; #endif #if defined(PIOS_INCLUDE_ADC) - init_pins(); - init_dma(); - init_adc(); + init_pins(); + init_dma(); + init_adc(); #endif - return 0; + return 0; } /** @@ -286,7 +289,7 @@ int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg) */ void PIOS_ADC_Config(__attribute__((unused)) uint32_t oversampling) { - /* we ignore this */ + /* we ignore this */ } /** @@ -300,44 +303,46 @@ int32_t last_conv_value; int32_t PIOS_ADC_PinGet(uint32_t pin) { #if defined(PIOS_INCLUDE_ADC) - int32_t result; - - /* Check if pin exists */ - if (pin >= PIOS_ADC_NUM_PINS) { - return -1; - } - - if (accumulator[pin].accumulator <= 0) - return -2; + int32_t result; - /* return accumulated result and clear accumulator */ - result = accumulator[pin].accumulator / (accumulator[pin].count ?: 1); - accumulator[pin].accumulator = result; - accumulator[pin].count = 1; + /* Check if pin exists */ + if (pin >= PIOS_ADC_NUM_PINS) { + return -1; + } + + if (accumulator[pin].accumulator <= 0) { + return -2; + } + + /* return accumulated result and clear accumulator */ + result = accumulator[pin].accumulator / (accumulator[pin].count ? : 1); + accumulator[pin].accumulator = result; + accumulator[pin].count = 1; + + return result; - return result; #endif - return -1; + return -1; } /** * @brief Set a callback function that is executed whenever - * the ADC double buffer swaps + * the ADC double buffer swaps * @note Not currently supported. */ -void PIOS_ADC_SetCallback(ADCCallback new_function) +void PIOS_ADC_SetCallback(ADCCallback new_function) { - pios_adc_dev->callback_function = new_function; + pios_adc_dev->callback_function = new_function; } #if defined(PIOS_INCLUDE_FREERTOS) /** - * @brief Register a queue to add data to when downsampled + * @brief Register a queue to add data to when downsampled * @note Not currently supported. */ -void PIOS_ADC_SetQueue(xQueueHandle data_queue) +void PIOS_ADC_SetQueue(xQueueHandle data_queue) { - pios_adc_dev->data_queue = data_queue; + pios_adc_dev->data_queue = data_queue; } #endif @@ -345,18 +350,18 @@ void PIOS_ADC_SetQueue(xQueueHandle data_queue) * @brief Return the address of the downsampled data buffer * @note Not currently supported. */ -float * PIOS_ADC_GetBuffer(void) +float *PIOS_ADC_GetBuffer(void) { - return NULL; + return NULL; } /** - * @brief Return the address of the raw data data buffer + * @brief Return the address of the raw data data buffer * @note Not currently supported. */ -int16_t * PIOS_ADC_GetRawBuffer(void) +int16_t *PIOS_ADC_GetRawBuffer(void) { - return NULL; + return NULL; } /** @@ -365,20 +370,20 @@ int16_t * PIOS_ADC_GetRawBuffer(void) */ uint8_t PIOS_ADC_GetOverSampling(void) { - return 1; + return 1; } /** - * @brief Set the fir coefficients. Takes as many samples as the + * @brief Set the fir coefficients. Takes as many samples as the * current filter order plus one (normalization) * * @param new_filter Array of adc_oversampling floats plus one for the * filter coefficients * @note Not currently supported. */ -void PIOS_ADC_SetFIRCoefficients(__attribute__((unused)) float * new_filter) +void PIOS_ADC_SetFIRCoefficients(__attribute__((unused)) float *new_filter) { - // not implemented + // not implemented } /** @@ -387,40 +392,39 @@ void PIOS_ADC_SetFIRCoefficients(__attribute__((unused)) float * new_filter) void accumulate(uint16_t *buffer, uint32_t count) { #if defined(PIOS_INCLUDE_ADC) - uint16_t *sp = buffer; + uint16_t *sp = buffer; + + /* + * Accumulate sampled values. + */ + while (count--) { + for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; ++i) { + accumulator[i].accumulator += *sp++; + accumulator[i].count++; + /* + * If the accumulator reaches half-full, rescale in order to + * make more space. + */ + if (accumulator[i].accumulator >= (((uint32_t)1) << 31)) { + accumulator[i].accumulator /= 2; + accumulator[i].count /= 2; + } + } + } - /* - * Accumulate sampled values. - */ - while (count--) { - for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; ++i) { - accumulator[i].accumulator += *sp++; - accumulator[i].count++; - /* - * If the accumulator reaches half-full, rescale in order to - * make more space. - */ - if (accumulator[i].accumulator >= (((uint32_t)1) << 31)) { - accumulator[i].accumulator /= 2; - accumulator[i].count /= 2; - } - } - } - #if defined(PIOS_INCLUDE_FREERTOS) - // XXX should do something with this - if (pios_adc_dev->data_queue) { - static portBASE_TYPE xHigherPriorityTaskWoken; -// xQueueSendFromISR(pios_adc_dev->data_queue, pios_adc_dev->downsampled_buffer, &xHigherPriorityTaskWoken); - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); - } + // XXX should do something with this + if (pios_adc_dev->data_queue) { + static portBASE_TYPE xHigherPriorityTaskWoken; +// xQueueSendFromISR(pios_adc_dev->data_queue, pios_adc_dev->downsampled_buffer, &xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + } #endif -#endif - -// if(pios_adc_dev->callback_function) -// pios_adc_dev->callback_function(pios_adc_dev->downsampled_buffer); +#endif /* if defined(PIOS_INCLUDE_ADC) */ +// if(pios_adc_dev->callback_function) +// pios_adc_dev->callback_function(pios_adc_dev->downsampled_buffer); } /** @@ -430,25 +434,25 @@ void accumulate(uint16_t *buffer, uint32_t count) */ void PIOS_ADC_DMA_Handler(void) { - if (!PIOS_ADC_validate(pios_adc_dev)) - return; + if (!PIOS_ADC_validate(pios_adc_dev)) { + return; + } #if defined(PIOS_INCLUDE_ADC) - /* terminal count, buffer has flipped */ - if (DMA_GetITStatus(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->full_flag)) { - DMA_ClearITPendingBit(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->full_flag); + /* terminal count, buffer has flipped */ + if (DMA_GetITStatus(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->full_flag)) { + DMA_ClearITPendingBit(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->full_flag); - /* accumulate results from the buffer that was just completed */ - accumulate(&adc_raw_buffer[DMA_GetCurrentMemoryTarget(pios_adc_dev->cfg->dma.rx.channel) ? 0 : 1][0][0], - PIOS_ADC_MAX_SAMPLES); - - } + /* accumulate results from the buffer that was just completed */ + accumulate(&adc_raw_buffer[DMA_GetCurrentMemoryTarget(pios_adc_dev->cfg->dma.rx.channel) ? 0 : 1][0][0], + PIOS_ADC_MAX_SAMPLES); + } #endif } #endif /* PIOS_INCLUDE_ADC */ -/** +/** * @} * @} */ diff --git a/flight/pios/stm32f4xx/pios_bkp.c b/flight/pios/stm32f4xx/pios_bkp.c index d47d39f9e..8b9357c5f 100644 --- a/flight/pios/stm32f4xx/pios_bkp.c +++ b/flight/pios/stm32f4xx/pios_bkp.c @@ -6,25 +6,25 @@ * @brief Hardware abstraction layer for backup sram * @{ * - * @file pios_bkp.c + * @file pios_bkp.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief IAP functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,87 +33,83 @@ #include /**************************************************************************************** - * Header files - ****************************************************************************************/ +* Header files +****************************************************************************************/ /***************************************************************************************** * Public Definitions/Macros ****************************************************************************************/ /**************************************************************************************** - * Public Functions - ****************************************************************************************/ -const uint32_t pios_bkp_registers_map[] = - { - RTC_BKP_DR0, - RTC_BKP_DR1, - RTC_BKP_DR2, - RTC_BKP_DR3, - RTC_BKP_DR4, - RTC_BKP_DR5, - RTC_BKP_DR6, - RTC_BKP_DR7, - RTC_BKP_DR8, - RTC_BKP_DR9, - RTC_BKP_DR10, - RTC_BKP_DR11, - RTC_BKP_DR12, - RTC_BKP_DR13, - RTC_BKP_DR14, - RTC_BKP_DR15, - RTC_BKP_DR16, - RTC_BKP_DR17, - RTC_BKP_DR18, - RTC_BKP_DR19 - }; +* Public Functions +****************************************************************************************/ +const uint32_t pios_bkp_registers_map[] = { + RTC_BKP_DR0, + RTC_BKP_DR1, + RTC_BKP_DR2, + RTC_BKP_DR3, + RTC_BKP_DR4, + RTC_BKP_DR5, + RTC_BKP_DR6, + RTC_BKP_DR7, + RTC_BKP_DR8, + RTC_BKP_DR9, + RTC_BKP_DR10, + RTC_BKP_DR11, + RTC_BKP_DR12, + RTC_BKP_DR13, + RTC_BKP_DR14, + RTC_BKP_DR15, + RTC_BKP_DR16, + RTC_BKP_DR17, + RTC_BKP_DR18, + RTC_BKP_DR19 +}; #define PIOS_BKP_REGISTERS_COUNT NELEMENTS(pios_bkp_registers_map) void PIOS_BKP_Init(void) { - /* Enable CRC clock */ - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_CRC | RCC_AHB1Periph_BKPSRAM, ENABLE); + /* Enable CRC clock */ + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_CRC | RCC_AHB1Periph_BKPSRAM, ENABLE); - /* Enable PWR and BKP clock */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); + /* Enable PWR and BKP clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); - /* Clear Tamper pin Event(TE) pending flag */ - RTC_ClearFlag(RTC_FLAG_TAMP1F); + /* Clear Tamper pin Event(TE) pending flag */ + RTC_ClearFlag(RTC_FLAG_TAMP1F); } uint16_t PIOS_BKP_ReadRegister(uint32_t regnumber) { - if(PIOS_BKP_REGISTERS_COUNT < regnumber) - { - PIOS_Assert(0); - } else { - return (uint16_t) RTC_ReadBackupRegister(pios_bkp_registers_map[regnumber]); - } + if (PIOS_BKP_REGISTERS_COUNT < regnumber) { + PIOS_Assert(0); + } else { + return (uint16_t)RTC_ReadBackupRegister(pios_bkp_registers_map[regnumber]); + } } -void PIOS_BKP_WriteRegister(uint32_t regnumber,uint16_t data) +void PIOS_BKP_WriteRegister(uint32_t regnumber, uint16_t data) { - if(PIOS_BKP_REGISTERS_COUNT < regnumber) - { - PIOS_Assert(0); - } else { - RTC_WriteBackupRegister(pios_bkp_registers_map[regnumber],(uint32_t)data); - } + if (PIOS_BKP_REGISTERS_COUNT < regnumber) { + PIOS_Assert(0); + } else { + RTC_WriteBackupRegister(pios_bkp_registers_map[regnumber], (uint32_t)data); + } } void PIOS_BKP_EnableWrite(void) { - /* Enable write access to Backup domain */ - PWR_BackupAccessCmd(ENABLE); + /* Enable write access to Backup domain */ + PWR_BackupAccessCmd(ENABLE); } void PIOS_BKP_DisableWrite(void) { - /* Enable write access to Backup domain */ - PWR_BackupAccessCmd(DISABLE); + /* Enable write access to Backup domain */ + PWR_BackupAccessCmd(DISABLE); } - /**************************************************************************************** - * Public Data - ****************************************************************************************/ +* Public Data +****************************************************************************************/ diff --git a/flight/pios/stm32f4xx/pios_bl_helper.c b/flight/pios/stm32f4xx/pios_bl_helper.c index 562587819..045779245 100644 --- a/flight/pios/stm32f4xx/pios_bl_helper.c +++ b/flight/pios/stm32f4xx/pios_bl_helper.c @@ -38,7 +38,7 @@ uint8_t *PIOS_BL_HELPER_FLASH_If_Read(uint32_t SectorAddress) { - return (uint8_t *) (SectorAddress); + return (uint8_t *)(SectorAddress); } #if defined(PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT) @@ -47,113 +47,114 @@ static bool erase_flash(uint32_t startAddress, uint32_t endAddress); uint8_t PIOS_BL_HELPER_FLASH_Ini() { - FLASH_Unlock(); - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); - return 1; + FLASH_Unlock(); + FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); + return 1; } struct device_flash_sector { - uint32_t start; - uint32_t size; - uint16_t st_sector; + uint32_t start; + uint32_t size; + uint16_t st_sector; }; static struct device_flash_sector flash_sectors[] = { - [0] = { - .start = 0x08000000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_0, - }, - [1] = { - .start = 0x08004000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_1, - }, - [2] = { - .start = 0x08008000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_2, - }, - [3] = { - .start = 0x0800C000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_3, - }, - [4] = { - .start = 0x08010000, - .size = 64 * 1024, - .st_sector = FLASH_Sector_4, - }, - [5] = { - .start = 0x08020000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_5, - }, - [6] = { - .start = 0x08040000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_6, - }, - [7] = { - .start = 0x08060000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_7, - }, - [8] = { - .start = 0x08080000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_8, - }, - [9] = { - .start = 0x080A0000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_9, - }, - [10] = { - .start = 0x080C0000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_10, - }, - [11] = { - .start = 0x080E0000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_11, - }, + [0] = { + .start = 0x08000000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_0, + }, + [1] = { + .start = 0x08004000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_1, + }, + [2] = { + .start = 0x08008000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_2, + }, + [3] = { + .start = 0x0800C000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_3, + }, + [4] = { + .start = 0x08010000, + .size = 64 * 1024, + .st_sector = FLASH_Sector_4, + }, + [5] = { + .start = 0x08020000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_5, + }, + [6] = { + .start = 0x08040000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_6, + }, + [7] = { + .start = 0x08060000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_7, + }, + [8] = { + .start = 0x08080000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_8, + }, + [9] = { + .start = 0x080A0000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_9, + }, + [10] = { + .start = 0x080C0000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_10, + }, + [11] = { + .start = 0x080E0000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_11, + }, }; -static bool PIOS_BL_HELPER_FLASH_GetSectorInfo(uint32_t address, uint8_t * sector_number, uint32_t *sector_start, uint32_t *sector_size) +static bool PIOS_BL_HELPER_FLASH_GetSectorInfo(uint32_t address, uint8_t *sector_number, uint32_t *sector_start, uint32_t *sector_size) { - for (uint8_t i = 0; i < NELEMENTS(flash_sectors); i++) { - struct device_flash_sector * sector = &flash_sectors[i]; - if ((address >= sector->start) && - (address < (sector->start + sector->size))) { - /* address lies within this sector */ - *sector_number = sector->st_sector; - *sector_start = sector->start; - *sector_size = sector->size; - return (true); - } - } + for (uint8_t i = 0; i < NELEMENTS(flash_sectors); i++) { + struct device_flash_sector *sector = &flash_sectors[i]; + if ((address >= sector->start) && + (address < (sector->start + sector->size))) { + /* address lies within this sector */ + *sector_number = sector->st_sector; + *sector_start = sector->start; + *sector_size = sector->size; + return true; + } + } - return (false); + return false; } uint8_t PIOS_BL_HELPER_FLASH_Start() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; - uint32_t startAddress = bdinfo->fw_base; - uint32_t endAddress = bdinfo->fw_base + bdinfo->fw_size + bdinfo->desc_size; + const struct pios_board_info *bdinfo = &pios_board_info_blob; + uint32_t startAddress = bdinfo->fw_base; + uint32_t endAddress = bdinfo->fw_base + bdinfo->fw_size + bdinfo->desc_size; - bool success = erase_flash(startAddress, endAddress); + bool success = erase_flash(startAddress, endAddress); return (success) ? 1 : 0; } -uint8_t PIOS_BL_HELPER_FLASH_Erase_Bootloader() { +uint8_t PIOS_BL_HELPER_FLASH_Erase_Bootloader() +{ /// Bootloader memory space erase uint32_t startAddress = BL_BANK_BASE; - uint32_t endAddress = BL_BANK_BASE + BL_BANK_SIZE; + uint32_t endAddress = BL_BANK_BASE + BL_BANK_SIZE; bool success = erase_flash(startAddress, endAddress); @@ -164,14 +165,15 @@ static bool erase_flash(uint32_t startAddress, uint32_t endAddress) { uint32_t pageAddress = startAddress; bool fail = false; + while ((pageAddress < endAddress) && (fail == false)) { uint8_t sector_number; uint32_t sector_start; uint32_t sector_size; if (!PIOS_BL_HELPER_FLASH_GetSectorInfo(pageAddress, - §or_number, - §or_start, - §or_size)) { + §or_number, + §or_start, + §or_size)) { /* We're asking for an invalid flash address */ PIOS_Assert(0); } @@ -189,31 +191,33 @@ static bool erase_flash(uint32_t startAddress, uint32_t endAddress) return !fail; } -#endif +#endif /* if defined(PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT) */ uint32_t PIOS_BL_HELPER_CRC_Memory_Calc() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; + const struct pios_board_info *bdinfo = &pios_board_info_blob; - PIOS_BL_HELPER_CRC_Ini(); - CRC_ResetDR(); - CRC_CalcBlockCRC((uint32_t *) bdinfo->fw_base, (bdinfo->fw_size) >> 2); - return CRC_GetCRC(); + PIOS_BL_HELPER_CRC_Ini(); + CRC_ResetDR(); + CRC_CalcBlockCRC((uint32_t *)bdinfo->fw_base, (bdinfo->fw_size) >> 2); + return CRC_GetCRC(); } -void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t * array, uint8_t size) +void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t *array, uint8_t size) { - const struct pios_board_info * bdinfo = &pios_board_info_blob; - uint8_t x = 0; - if (size > bdinfo->desc_size) size = bdinfo->desc_size; - for (uint32_t i = bdinfo->fw_base + bdinfo->fw_size; i < bdinfo->fw_base + bdinfo->fw_size + size; ++i) { - array[x] = *PIOS_BL_HELPER_FLASH_If_Read(i); - ++x; - } + const struct pios_board_info *bdinfo = &pios_board_info_blob; + uint8_t x = 0; + + if (size > bdinfo->desc_size) { + size = bdinfo->desc_size; + } + for (uint32_t i = bdinfo->fw_base + bdinfo->fw_size; i < bdinfo->fw_base + bdinfo->fw_size + size; ++i) { + array[x] = *PIOS_BL_HELPER_FLASH_If_Read(i); + ++x; + } } void PIOS_BL_HELPER_CRC_Ini() -{ -} +{} #endif /* PIOS_INCLUDE_BL_HELPER */ diff --git a/flight/pios/stm32f4xx/pios_debug.c b/flight/pios/stm32f4xx/pios_debug.c index af10d3918..a7c9bf53a 100644 --- a/flight/pios/stm32f4xx/pios_debug.c +++ b/flight/pios/stm32f4xx/pios_debug.c @@ -10,21 +10,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief Debugging Functions * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,77 +34,77 @@ const char *PIOS_DEBUG_AssertMsg = "ASSERT FAILED"; #ifdef PIOS_ENABLE_DEBUG_PINS -static const struct pios_tim_channel * debug_channels; +static const struct pios_tim_channel *debug_channels; static uint8_t debug_num_channels; -#endif /* PIOS_ENABLE_DEBUG_PINS */ +#endif /* PIOS_ENABLE_DEBUG_PINS */ /** -* Initialise Debug-features -*/ -void PIOS_DEBUG_Init(__attribute__((unused)) const struct pios_tim_channel * channels, __attribute__((unused)) uint8_t num_channels) + * Initialise Debug-features + */ +void PIOS_DEBUG_Init(__attribute__((unused)) const struct pios_tim_channel *channels, __attribute__((unused)) uint8_t num_channels) { #ifdef PIOS_ENABLE_DEBUG_PINS - PIOS_Assert(channels); - PIOS_Assert(num_channels); + PIOS_Assert(channels); + PIOS_Assert(num_channels); - /* Store away the GPIOs we've been given */ - debug_channels = channels; - debug_num_channels = num_channels; + /* Store away the GPIOs we've been given */ + debug_channels = channels; + debug_num_channels = num_channels; - /* Configure the GPIOs we've been given */ - for (uint8_t i = 0; i < num_channels; i++) { - const struct pios_tim_channel * chan = &channels[i]; + /* Configure the GPIOs we've been given */ + for (uint8_t i = 0; i < num_channels; i++) { + const struct pios_tim_channel *chan = &channels[i]; - // Initialise pins as standard output pins - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_InitStructure.GPIO_Pin = chan->pin.init.GPIO_Pin; + // Initialise pins as standard output pins + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_Pin = chan->pin.init.GPIO_Pin; - /* Initialize the GPIO */ - GPIO_Init(chan->pin.gpio, &GPIO_InitStructure); + /* Initialize the GPIO */ + GPIO_Init(chan->pin.gpio, &GPIO_InitStructure); - /* Set the pin low */ - GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); - } + /* Set the pin low */ + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); + } #endif // PIOS_ENABLE_DEBUG_PINS } /** -* Set debug-pin high -* \param pin 0 for S1 output -*/ + * Set debug-pin high + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinHigh(__attribute__((unused)) uint8_t pin) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels || pin >= debug_num_channels) { - return; - } + if (!debug_channels || pin >= debug_num_channels) { + return; + } - const struct pios_tim_channel * chan = &debug_channels[pin]; + const struct pios_tim_channel *chan = &debug_channels[pin]; - GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_SET); + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_SET); #endif // PIOS_ENABLE_DEBUG_PINS } /** -* Set debug-pin low -* \param pin 0 for S1 output -*/ + * Set debug-pin low + * \param pin 0 for S1 output + */ void PIOS_DEBUG_PinLow(__attribute__((unused)) uint8_t pin) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels || pin >= debug_num_channels) { - return; - } + if (!debug_channels || pin >= debug_num_channels) { + return; + } - const struct pios_tim_channel * chan = &debug_channels[pin]; + const struct pios_tim_channel *chan = &debug_channels[pin]; - GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); #endif // PIOS_ENABLE_DEBUG_PINS } @@ -113,45 +113,45 @@ void PIOS_DEBUG_PinLow(__attribute__((unused)) uint8_t pin) void PIOS_DEBUG_PinValue8Bit(__attribute__((unused)) uint8_t value) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels) { - return; - } + if (!debug_channels) { + return; + } #pragma message("This code is not portable and should be revised") - PIOS_Assert(0); + PIOS_Assert(0); - uint32_t bsrr_l = ( ((~value)&0x0F)<<(16+6) ) | ((value & 0x0F)<<6); - uint32_t bsrr_h = ( ((~value)&0xF0)<<(16+6-4) ) | ((value & 0xF0)<<(6-4)); + uint32_t bsrr_l = (((~value) & 0x0F) << (16 + 6)) | ((value & 0x0F) << 6); + uint32_t bsrr_h = (((~value) & 0xF0) << (16 + 6 - 4)) | ((value & 0xF0) << (6 - 4)); - PIOS_IRQ_Disable(); + PIOS_IRQ_Disable(); - /* - * This is sketchy since it assumes a particular ordering - * and bitwise layout of the channels provided to the debug code. - */ - //debug_channels[0].pin.gpio->BSRR = bsrr_l; - //debug_channels[4].pin.gpio->BSRR = bsrr_h; + /* + * This is sketchy since it assumes a particular ordering + * and bitwise layout of the channels provided to the debug code. + */ + // debug_channels[0].pin.gpio->BSRR = bsrr_l; + // debug_channels[4].pin.gpio->BSRR = bsrr_h; - PIOS_IRQ_Enable(); + PIOS_IRQ_Enable(); #endif // PIOS_ENABLE_DEBUG_PINS } void PIOS_DEBUG_PinValue4BitL(__attribute__((unused)) uint8_t value) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (!debug_channels) { - return; - } + if (!debug_channels) { + return; + } #pragma message("This code is not portable and should be revised") - PIOS_Assert(0); + PIOS_Assert(0); - /* - * This is sketchy since it assumes a particular ordering - * and bitwise layout of the channels provided to the debug code. - */ - uint32_t bsrr_l = ((~(value & 0x0F)<<(16+6))) | ((value & 0x0F)<<6); - //debug_channels[0].pin.gpio->BSRR = bsrr_l; + /* + * This is sketchy since it assumes a particular ordering + * and bitwise layout of the channels provided to the debug code. + */ + uint32_t bsrr_l = ((~(value & 0x0F) << (16 + 6))) | ((value & 0x0F) << 6); + // debug_channels[0].pin.gpio->BSRR = bsrr_l; #endif // PIOS_ENABLE_DEBUG_PINS } @@ -162,15 +162,17 @@ void PIOS_DEBUG_PinValue4BitL(__attribute__((unused)) uint8_t value) void PIOS_DEBUG_Panic(__attribute__((unused)) const char *msg) { #ifdef PIOS_INCLUDE_DEBUG_CONSOLE - register int *lr asm("lr"); // Link-register holds the PC of the caller - DEBUG_PRINTF(0, "\r%s @0x%x\r", msg, lr); + register int *lr asm ("lr"); // Link-register holds the PC of the caller + DEBUG_PRINTF(0, "\r%s @0x%x\r", msg, lr); #endif - // Stay put - while (1) ; + // Stay put + while (1) { + ; + } } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_delay.c b/flight/pios/stm32f4xx/pios_delay.c index 5faeef2c7..55970d203 100644 --- a/flight/pios/stm32f4xx/pios_delay.c +++ b/flight/pios/stm32f4xx/pios_delay.c @@ -8,25 +8,25 @@ * * @file pios_delay.c * @author Michael Smith Copyright (C) 2012 - * @brief Delay Functions + * @brief Delay Functions * - Provides a micro-second granular delay using the CPU * cycle counter. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,9 @@ #ifdef PIOS_INCLUDE_DELAY /* these should be defined by CMSIS, but they aren't */ -#define DWT_CTRL (*(volatile uint32_t *)0xe0001000) -#define CYCCNTENA (1<<0) -#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004) +#define DWT_CTRL (*(volatile uint32_t *)0xe0001000) +#define CYCCNTENA (1 << 0) +#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004) /* cycles per microsecond */ @@ -51,20 +51,20 @@ static uint32_t us_ticks; int32_t PIOS_DELAY_Init(void) { - RCC_ClocksTypeDef clocks; + RCC_ClocksTypeDef clocks; - /* compute the number of system clocks per microsecond */ - RCC_GetClocksFreq(&clocks); - us_ticks = clocks.SYSCLK_Frequency / 1000000; - PIOS_DEBUG_Assert(us_ticks > 1); + /* compute the number of system clocks per microsecond */ + RCC_GetClocksFreq(&clocks); + us_ticks = clocks.SYSCLK_Frequency / 1000000; + PIOS_DEBUG_Assert(us_ticks > 1); - /* turn on access to the DWT registers */ - CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + /* turn on access to the DWT registers */ + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; - /* enable the CPU cycle counter */ - DWT_CTRL |= CYCCNTENA; + /* enable the CPU cycle counter */ + DWT_CTRL |= CYCCNTENA; - return 0; + return 0; } /** @@ -80,31 +80,32 @@ int32_t PIOS_DELAY_Init(void) */ int32_t PIOS_DELAY_WaituS(uint32_t uS) { - uint32_t elapsed = 0; - uint32_t last_count = DWT_CYCCNT; - - for (;;) { - uint32_t current_count = DWT_CYCCNT; - uint32_t elapsed_uS; + uint32_t elapsed = 0; + uint32_t last_count = DWT_CYCCNT; - /* measure the time elapsed since the last time we checked */ - elapsed += current_count - last_count; - last_count = current_count; + for (;;) { + uint32_t current_count = DWT_CYCCNT; + uint32_t elapsed_uS; - /* convert to microseconds */ - elapsed_uS = elapsed / us_ticks; - if (elapsed_uS >= uS) - break; + /* measure the time elapsed since the last time we checked */ + elapsed += current_count - last_count; + last_count = current_count; - /* reduce the delay by the elapsed time */ - uS -= elapsed_uS; + /* convert to microseconds */ + elapsed_uS = elapsed / us_ticks; + if (elapsed_uS >= uS) { + break; + } - /* keep fractional microseconds for the next iteration */ - elapsed %= us_ticks; - } + /* reduce the delay by the elapsed time */ + uS -= elapsed_uS; - /* No error */ - return 0; + /* keep fractional microseconds for the next iteration */ + elapsed %= us_ticks; + } + + /* No error */ + return 0; } /** @@ -120,21 +121,21 @@ int32_t PIOS_DELAY_WaituS(uint32_t uS) */ int32_t PIOS_DELAY_WaitmS(uint32_t mS) { - while (mS--) { - PIOS_DELAY_WaituS(1000); - } + while (mS--) { + PIOS_DELAY_WaituS(1000); + } - /* No error */ - return 0; + /* No error */ + return 0; } /** - * @brief Query the Delay timer for the current uS + * @brief Query the Delay timer for the current uS * @return A microsecond value */ uint32_t PIOS_DELAY_GetuS() { - return DWT_CYCCNT / us_ticks; + return DWT_CYCCNT / us_ticks; } /** @@ -144,7 +145,7 @@ uint32_t PIOS_DELAY_GetuS() */ uint32_t PIOS_DELAY_GetuSSince(uint32_t t) { - return (PIOS_DELAY_GetuS() - t); + return PIOS_DELAY_GetuS() - t; } /** @@ -153,22 +154,23 @@ uint32_t PIOS_DELAY_GetuSSince(uint32_t t) */ uint32_t PIOS_DELAY_GetRaw() { - return DWT_CYCCNT; + return DWT_CYCCNT; } /** - * @brief Compare to raw times to and convert to us + * @brief Compare to raw times to and convert to us * @return A microsecond value */ uint32_t PIOS_DELAY_DiffuS(uint32_t raw) { - uint32_t diff = DWT_CYCCNT - raw; - return diff / us_ticks; + uint32_t diff = DWT_CYCCNT - raw; + + return diff / us_ticks; } #endif /* PIOS_INCLUDE_DELAY */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_dsm.c b/flight/pios/stm32f4xx/pios_dsm.c index 8ca685011..6b1071a5d 100644 --- a/flight/pios/stm32f4xx/pios_dsm.c +++ b/flight/pios/stm32f4xx/pios_dsm.c @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,130 +41,136 @@ /* Forward Declarations */ static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel); static uint16_t PIOS_DSM_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield); + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield); static void PIOS_DSM_Supervisor(uint32_t dsm_id); /* Local Variables */ const struct pios_rcvr_driver pios_dsm_rcvr_driver = { - .read = PIOS_DSM_Get, + .read = PIOS_DSM_Get, }; enum pios_dsm_dev_magic { - PIOS_DSM_DEV_MAGIC = 0x44534d78, + PIOS_DSM_DEV_MAGIC = 0x44534d78, }; struct pios_dsm_state { - uint16_t channel_data[PIOS_DSM_NUM_INPUTS]; - uint8_t received_data[DSM_FRAME_LENGTH]; - uint8_t receive_timer; - uint8_t failsafe_timer; - uint8_t frame_found; - uint8_t byte_count; + uint16_t channel_data[PIOS_DSM_NUM_INPUTS]; + uint8_t received_data[DSM_FRAME_LENGTH]; + uint8_t receive_timer; + uint8_t failsafe_timer; + uint8_t frame_found; + uint8_t byte_count; #ifdef DSM_LOST_FRAME_COUNTER - uint8_t frames_lost_last; - uint16_t frames_lost; + uint8_t frames_lost_last; + uint16_t frames_lost; #endif }; struct pios_dsm_dev { - enum pios_dsm_dev_magic magic; - const struct pios_dsm_cfg *cfg; - enum pios_dsm_proto proto; - struct pios_dsm_state state; + enum pios_dsm_dev_magic magic; + const struct pios_dsm_cfg *cfg; + enum pios_dsm_proto proto; + struct pios_dsm_state state; }; /* Allocate DSM device descriptor */ #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_dsm_dev *PIOS_DSM_Alloc(void) { - struct pios_dsm_dev *dsm_dev; + struct pios_dsm_dev *dsm_dev; - dsm_dev = (struct pios_dsm_dev *)pvPortMalloc(sizeof(*dsm_dev)); - if (!dsm_dev) - return NULL; + dsm_dev = (struct pios_dsm_dev *)pvPortMalloc(sizeof(*dsm_dev)); + if (!dsm_dev) { + return NULL; + } - dsm_dev->magic = PIOS_DSM_DEV_MAGIC; - return dsm_dev; + dsm_dev->magic = PIOS_DSM_DEV_MAGIC; + return dsm_dev; } #else static struct pios_dsm_dev pios_dsm_devs[PIOS_DSM_MAX_DEVS]; static uint8_t pios_dsm_num_devs; static struct pios_dsm_dev *PIOS_DSM_Alloc(void) { - struct pios_dsm_dev *dsm_dev; + struct pios_dsm_dev *dsm_dev; - if (pios_dsm_num_devs >= PIOS_DSM_MAX_DEVS) - return NULL; + if (pios_dsm_num_devs >= PIOS_DSM_MAX_DEVS) { + return NULL; + } - dsm_dev = &pios_dsm_devs[pios_dsm_num_devs++]; - dsm_dev->magic = PIOS_DSM_DEV_MAGIC; + dsm_dev = &pios_dsm_devs[pios_dsm_num_devs++]; + dsm_dev->magic = PIOS_DSM_DEV_MAGIC; - return dsm_dev; + return dsm_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /* Validate DSM device descriptor */ static bool PIOS_DSM_Validate(struct pios_dsm_dev *dsm_dev) { - return (dsm_dev->magic == PIOS_DSM_DEV_MAGIC); + return dsm_dev->magic == PIOS_DSM_DEV_MAGIC; } /* Try to bind DSMx satellite using specified number of pulses */ static void PIOS_DSM_Bind(struct pios_dsm_dev *dsm_dev, uint8_t bind) { - const struct pios_dsm_cfg *cfg = dsm_dev->cfg; + const struct pios_dsm_cfg *cfg = dsm_dev->cfg; - GPIO_InitTypeDef GPIO_InitStructure = cfg->bind.init; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_InitTypeDef GPIO_InitStructure = cfg->bind.init; - /* just to limit bind pulses */ - if (bind > 10) - bind = 10; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_Init(cfg->bind.gpio, &cfg->bind.init); + /* just to limit bind pulses */ + if (bind > 10) { + bind = 10; + } - /* RX line, set high */ - GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + GPIO_Init(cfg->bind.gpio, &cfg->bind.init); - /* on CC works up to 140ms, guess bind window is around 20-140ms after power up */ - PIOS_DELAY_WaitmS(60); + /* RX line, set high */ + GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - for (int i = 0; i < bind ; i++) { - /* RX line, drive low for 120us */ - GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - PIOS_DELAY_WaituS(120); - /* RX line, drive high for 120us */ - GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - PIOS_DELAY_WaituS(120); - } - /* RX line, set input and wait for data */ - GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure); + /* on CC works up to 140ms, guess bind window is around 20-140ms after power up */ + PIOS_DELAY_WaitmS(60); + + for (int i = 0; i < bind; i++) { + /* RX line, drive low for 120us */ + GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + PIOS_DELAY_WaituS(120); + /* RX line, drive high for 120us */ + GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + PIOS_DELAY_WaituS(120); + } + /* RX line, set input and wait for data */ + GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure); } /* Reset channels in case of lost signal or explicit failsafe receiver flag */ static void PIOS_DSM_ResetChannels(struct pios_dsm_dev *dsm_dev) { - struct pios_dsm_state *state = &(dsm_dev->state); - for (int i = 0; i < PIOS_DSM_NUM_INPUTS; i++) { - state->channel_data[i] = PIOS_RCVR_TIMEOUT; - } + struct pios_dsm_state *state = &(dsm_dev->state); + + for (int i = 0; i < PIOS_DSM_NUM_INPUTS; i++) { + state->channel_data[i] = PIOS_RCVR_TIMEOUT; + } } /* Reset DSM receiver state */ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev) { - struct pios_dsm_state *state = &(dsm_dev->state); - state->receive_timer = 0; - state->failsafe_timer = 0; - state->frame_found = 0; + struct pios_dsm_state *state = &(dsm_dev->state); + + state->receive_timer = 0; + state->failsafe_timer = 0; + state->frame_found = 0; #ifdef DSM_LOST_FRAME_COUNTER - state->frames_lost_last = 0; - state->frames_lost = 0; + state->frames_lost_last = 0; + state->frames_lost = 0; #endif - PIOS_DSM_ResetChannels(dsm_dev); + PIOS_DSM_ResetChannels(dsm_dev); } /** @@ -174,174 +180,182 @@ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev) */ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) { - struct pios_dsm_state *state = &(dsm_dev->state); - uint8_t resolution; + struct pios_dsm_state *state = &(dsm_dev->state); + uint8_t resolution; #ifdef DSM_LOST_FRAME_COUNTER - /* increment the lost frame counter */ - uint8_t frames_lost = state->received_data[0]; - state->frames_lost += (frames_lost - state->frames_lost_last); - state->frames_lost_last = frames_lost; + /* increment the lost frame counter */ + uint8_t frames_lost = state->received_data[0]; + state->frames_lost += (frames_lost - state->frames_lost_last); + state->frames_lost_last = frames_lost; #endif - /* check the frame type assuming master satellite stream */ - uint8_t type = state->received_data[1]; - switch (type) { - case 0x01: - case 0x02: - case 0x12: - /* DSM2, DSMJ stream */ - if (dsm_dev->proto == PIOS_DSM_PROTO_DSM2) { - /* DSM2/DSMJ resolution is known from the header */ - resolution = (type & DSM_DSM2_RES_MASK) ? 11 : 10; - } else { - /* DSMX resolution should explicitly be selected */ - goto stream_error; - } - break; - case 0xA2: - case 0xB2: - /* DSMX stream */ - if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) { - resolution = 10; - } else if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) { - resolution = 11; - } else { - /* DSMX resolution should explicitly be selected */ - goto stream_error; - } - break; - default: - /* unknown yet data stream */ - goto stream_error; - } + /* check the frame type assuming master satellite stream */ + uint8_t type = state->received_data[1]; + switch (type) { + case 0x01: + case 0x02: + case 0x12: + /* DSM2, DSMJ stream */ + if (dsm_dev->proto == PIOS_DSM_PROTO_DSM2) { + /* DSM2/DSMJ resolution is known from the header */ + resolution = (type & DSM_DSM2_RES_MASK) ? 11 : 10; + } else { + /* DSMX resolution should explicitly be selected */ + goto stream_error; + } + break; + case 0xA2: + case 0xB2: + /* DSMX stream */ + if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) { + resolution = 10; + } else if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) { + resolution = 11; + } else { + /* DSMX resolution should explicitly be selected */ + goto stream_error; + } + break; + default: + /* unknown yet data stream */ + goto stream_error; + } - /* unroll channels */ - uint8_t *s = &(state->received_data[2]); - uint16_t mask = (resolution == 10) ? 0x03ff : 0x07ff; + /* unroll channels */ + uint8_t *s = &(state->received_data[2]); + uint16_t mask = (resolution == 10) ? 0x03ff : 0x07ff; - for (int i = 0; i < DSM_CHANNELS_PER_FRAME; i++) { - uint16_t word = ((uint16_t)s[0] << 8) | s[1]; - s += 2; + for (int i = 0; i < DSM_CHANNELS_PER_FRAME; i++) { + uint16_t word = ((uint16_t)s[0] << 8) | s[1]; + s += 2; - /* skip empty channel slot */ - if (word == 0xffff) - continue; + /* skip empty channel slot */ + if (word == 0xffff) { + continue; + } - /* minimal data validation */ - if ((i > 0) && (word & DSM_2ND_FRAME_MASK)) { - /* invalid frame data, ignore rest of the frame */ - goto stream_error; - } + /* minimal data validation */ + if ((i > 0) && (word & DSM_2ND_FRAME_MASK)) { + /* invalid frame data, ignore rest of the frame */ + goto stream_error; + } - /* extract and save the channel value */ - uint8_t channel_num = (word >> resolution) & 0x0f; - if (channel_num < PIOS_DSM_NUM_INPUTS) - state->channel_data[channel_num] = (word & mask); - } + /* extract and save the channel value */ + uint8_t channel_num = (word >> resolution) & 0x0f; + if (channel_num < PIOS_DSM_NUM_INPUTS) { + state->channel_data[channel_num] = (word & mask); + } + } #ifdef DSM_LOST_FRAME_COUNTER - /* put lost frames counter into the last channel for debugging */ - state->channel_data[PIOS_DSM_NUM_INPUTS-1] = state->frames_lost; + /* put lost frames counter into the last channel for debugging */ + state->channel_data[PIOS_DSM_NUM_INPUTS - 1] = state->frames_lost; #endif - /* all channels processed */ - return 0; + /* all channels processed */ + return 0; stream_error: - /* either DSM2 selected with DSMX stream found, or vice-versa */ - return -1; + /* either DSM2 selected with DSMX stream found, or vice-versa */ + return -1; } /* Update decoder state processing input byte from the DSMx stream */ static void PIOS_DSM_UpdateState(struct pios_dsm_dev *dsm_dev, uint8_t byte) { - struct pios_dsm_state *state = &(dsm_dev->state); - if (state->frame_found) { - /* receiving the data frame */ - if (state->byte_count < DSM_FRAME_LENGTH) { - /* store next byte */ - state->received_data[state->byte_count++] = byte; - if (state->byte_count == DSM_FRAME_LENGTH) { - /* full frame received - process and wait for new one */ - if (!PIOS_DSM_UnrollChannels(dsm_dev)) - /* data looking good */ - state->failsafe_timer = 0; + struct pios_dsm_state *state = &(dsm_dev->state); - /* prepare for the next frame */ - state->frame_found = 0; - } - } - } + if (state->frame_found) { + /* receiving the data frame */ + if (state->byte_count < DSM_FRAME_LENGTH) { + /* store next byte */ + state->received_data[state->byte_count++] = byte; + if (state->byte_count == DSM_FRAME_LENGTH) { + /* full frame received - process and wait for new one */ + if (!PIOS_DSM_UnrollChannels(dsm_dev)) { + /* data looking good */ + state->failsafe_timer = 0; + } + + /* prepare for the next frame */ + state->frame_found = 0; + } + } + } } /* Initialise DSM receiver interface */ int32_t PIOS_DSM_Init(uint32_t *dsm_id, - const struct pios_dsm_cfg *cfg, - const struct pios_com_driver *driver, - uint32_t lower_id, - enum pios_dsm_proto proto, - uint8_t bind) + const struct pios_dsm_cfg *cfg, + const struct pios_com_driver *driver, + uint32_t lower_id, + enum pios_dsm_proto proto, + uint8_t bind) { - PIOS_DEBUG_Assert(dsm_id); - PIOS_DEBUG_Assert(cfg); - PIOS_DEBUG_Assert(driver); + PIOS_DEBUG_Assert(dsm_id); + PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(driver); - struct pios_dsm_dev *dsm_dev; + struct pios_dsm_dev *dsm_dev; - dsm_dev = (struct pios_dsm_dev *)PIOS_DSM_Alloc(); - if (!dsm_dev) - return -1; + dsm_dev = (struct pios_dsm_dev *)PIOS_DSM_Alloc(); + if (!dsm_dev) { + return -1; + } - /* Bind the configuration to the device instance */ - dsm_dev->cfg = cfg; - dsm_dev->proto = proto; + /* Bind the configuration to the device instance */ + dsm_dev->cfg = cfg; + dsm_dev->proto = proto; - /* Bind the receiver if requested */ - if (bind) - PIOS_DSM_Bind(dsm_dev, bind); + /* Bind the receiver if requested */ + if (bind) { + PIOS_DSM_Bind(dsm_dev, bind); + } - PIOS_DSM_ResetState(dsm_dev); + PIOS_DSM_ResetState(dsm_dev); - *dsm_id = (uint32_t)dsm_dev; + *dsm_id = (uint32_t)dsm_dev; - /* Set comm driver callback */ - (driver->bind_rx_cb)(lower_id, PIOS_DSM_RxInCallback, *dsm_id); + /* Set comm driver callback */ + (driver->bind_rx_cb)(lower_id, PIOS_DSM_RxInCallback, *dsm_id); - if (!PIOS_RTC_RegisterTickCallback(PIOS_DSM_Supervisor, *dsm_id)) { - PIOS_DEBUG_Assert(0); - } + if (!PIOS_RTC_RegisterTickCallback(PIOS_DSM_Supervisor, *dsm_id)) { + PIOS_DEBUG_Assert(0); + } - return 0; + return 0; } /* Comm byte received callback */ static uint16_t PIOS_DSM_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield) + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield) { - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)context; + struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)context; - bool valid = PIOS_DSM_Validate(dsm_dev); - PIOS_Assert(valid); + bool valid = PIOS_DSM_Validate(dsm_dev); - /* process byte(s) and clear receive timer */ - for (uint8_t i = 0; i < buf_len; i++) { - PIOS_DSM_UpdateState(dsm_dev, buf[i]); - dsm_dev->state.receive_timer = 0; - } + PIOS_Assert(valid); - /* Always signal that we can accept another byte */ - if (headroom) - *headroom = DSM_FRAME_LENGTH; + /* process byte(s) and clear receive timer */ + for (uint8_t i = 0; i < buf_len; i++) { + PIOS_DSM_UpdateState(dsm_dev, buf[i]); + dsm_dev->state.receive_timer = 0; + } - /* We never need a yield */ - *need_yield = false; + /* Always signal that we can accept another byte */ + if (headroom) { + *headroom = DSM_FRAME_LENGTH; + } - /* Always indicate that all bytes were consumed */ - return buf_len; + /* We never need a yield */ + *need_yield = false; + + /* Always indicate that all bytes were consumed */ + return buf_len; } /** @@ -353,17 +367,19 @@ static uint16_t PIOS_DSM_RxInCallback(uint32_t context, */ static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)rcvr_id; + struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)rcvr_id; - if (!PIOS_DSM_Validate(dsm_dev)) - return PIOS_RCVR_INVALID; + if (!PIOS_DSM_Validate(dsm_dev)) { + return PIOS_RCVR_INVALID; + } - /* return error if channel is not available */ - if (channel >= PIOS_DSM_NUM_INPUTS) - return PIOS_RCVR_INVALID; + /* return error if channel is not available */ + if (channel >= PIOS_DSM_NUM_INPUTS) { + return PIOS_RCVR_INVALID; + } - /* may also be PIOS_RCVR_TIMEOUT set by other function */ - return dsm_dev->state.channel_data[channel]; + /* may also be PIOS_RCVR_TIMEOUT set by other function */ + return dsm_dev->state.channel_data[channel]; } /** @@ -380,30 +396,31 @@ static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel) */ static void PIOS_DSM_Supervisor(uint32_t dsm_id) { - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id; + struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id; - bool valid = PIOS_DSM_Validate(dsm_dev); - PIOS_Assert(valid); + bool valid = PIOS_DSM_Validate(dsm_dev); - struct pios_dsm_state *state = &(dsm_dev->state); + PIOS_Assert(valid); - /* waiting for new frame if no bytes were received in 8ms */ - if (++state->receive_timer > 4) { - state->frame_found = 1; - state->byte_count = 0; - state->receive_timer = 0; - } + struct pios_dsm_state *state = &(dsm_dev->state); - /* activate failsafe if no frames have arrived in 102.4ms */ - if (++state->failsafe_timer > 64) { - PIOS_DSM_ResetChannels(dsm_dev); - state->failsafe_timer = 0; - } + /* waiting for new frame if no bytes were received in 8ms */ + if (++state->receive_timer > 4) { + state->frame_found = 1; + state->byte_count = 0; + state->receive_timer = 0; + } + + /* activate failsafe if no frames have arrived in 102.4ms */ + if (++state->failsafe_timer > 64) { + PIOS_DSM_ResetChannels(dsm_dev); + state->failsafe_timer = 0; + } } #endif /* PIOS_INCLUDE_DSM */ -/** +/** * @} * @} */ diff --git a/flight/pios/stm32f4xx/pios_exti.c b/flight/pios/stm32f4xx/pios_exti.c index 3dc1e1158..e32cc98b9 100644 --- a/flight/pios/stm32f4xx/pios_exti.c +++ b/flight/pios/stm32f4xx/pios_exti.c @@ -13,19 +13,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,262 +34,298 @@ #ifdef PIOS_INCLUDE_EXTI /* Map EXTI line to full config */ -#define EXTI_MAX_LINES 16 +#define EXTI_MAX_LINES 16 #define PIOS_EXTI_INVALID 0xFF static uint8_t pios_exti_line_to_cfg_map[EXTI_MAX_LINES] = { - [0 ... EXTI_MAX_LINES-1] = PIOS_EXTI_INVALID, + [0 ... EXTI_MAX_LINES - 1] = PIOS_EXTI_INVALID, }; /* Table of exti configs registered at compile time */ extern struct pios_exti_cfg __start__exti __attribute__((weak)); extern struct pios_exti_cfg __stop__exti __attribute__((weak)); -static uint8_t PIOS_EXTI_line_to_index (uint32_t line) +static uint8_t PIOS_EXTI_line_to_index(uint32_t line) { - switch (line) { - case EXTI_Line0: return 0; - case EXTI_Line1: return 1; - case EXTI_Line2: return 2; - case EXTI_Line3: return 3; - case EXTI_Line4: return 4; - case EXTI_Line5: return 5; - case EXTI_Line6: return 6; - case EXTI_Line7: return 7; - case EXTI_Line8: return 8; - case EXTI_Line9: return 9; - case EXTI_Line10: return 10; - case EXTI_Line11: return 11; - case EXTI_Line12: return 12; - case EXTI_Line13: return 13; - case EXTI_Line14: return 14; - case EXTI_Line15: return 15; - } + switch (line) { + case EXTI_Line0: return 0; - PIOS_Assert(0); - return 0xFF; + case EXTI_Line1: return 1; + + case EXTI_Line2: return 2; + + case EXTI_Line3: return 3; + + case EXTI_Line4: return 4; + + case EXTI_Line5: return 5; + + case EXTI_Line6: return 6; + + case EXTI_Line7: return 7; + + case EXTI_Line8: return 8; + + case EXTI_Line9: return 9; + + case EXTI_Line10: return 10; + + case EXTI_Line11: return 11; + + case EXTI_Line12: return 12; + + case EXTI_Line13: return 13; + + case EXTI_Line14: return 14; + + case EXTI_Line15: return 15; + } + + PIOS_Assert(0); + return 0xFF; } -uint8_t PIOS_EXTI_gpio_port_to_exti_source_port(GPIO_TypeDef * gpio_port) +uint8_t PIOS_EXTI_gpio_port_to_exti_source_port(GPIO_TypeDef *gpio_port) { - switch((uint32_t)gpio_port) { - case (uint32_t)GPIOA: return (EXTI_PortSourceGPIOA); - case (uint32_t)GPIOB: return (EXTI_PortSourceGPIOB); - case (uint32_t)GPIOC: return (EXTI_PortSourceGPIOC); - case (uint32_t)GPIOD: return (EXTI_PortSourceGPIOD); - case (uint32_t)GPIOE: return (EXTI_PortSourceGPIOE); - case (uint32_t)GPIOF: return (EXTI_PortSourceGPIOF); - case (uint32_t)GPIOG: return (EXTI_PortSourceGPIOG); - } + switch ((uint32_t)gpio_port) { + case (uint32_t)GPIOA: return EXTI_PortSourceGPIOA; - PIOS_Assert(0); - return 0xFF; + case (uint32_t)GPIOB: return EXTI_PortSourceGPIOB; + + case (uint32_t)GPIOC: return EXTI_PortSourceGPIOC; + + case (uint32_t)GPIOD: return EXTI_PortSourceGPIOD; + + case (uint32_t)GPIOE: return EXTI_PortSourceGPIOE; + + case (uint32_t)GPIOF: return EXTI_PortSourceGPIOF; + + case (uint32_t)GPIOG: return EXTI_PortSourceGPIOG; + } + + PIOS_Assert(0); + return 0xFF; } uint8_t PIOS_EXTI_gpio_pin_to_exti_source_pin(uint32_t gpio_pin) { - switch((uint32_t)gpio_pin) { - case GPIO_Pin_0: return (GPIO_PinSource0); - case GPIO_Pin_1: return (GPIO_PinSource1); - case GPIO_Pin_2: return (GPIO_PinSource2); - case GPIO_Pin_3: return (GPIO_PinSource3); - case GPIO_Pin_4: return (GPIO_PinSource4); - case GPIO_Pin_5: return (GPIO_PinSource5); - case GPIO_Pin_6: return (GPIO_PinSource6); - case GPIO_Pin_7: return (GPIO_PinSource7); - case GPIO_Pin_8: return (GPIO_PinSource8); - case GPIO_Pin_9: return (GPIO_PinSource9); - case GPIO_Pin_10: return (GPIO_PinSource10); - case GPIO_Pin_11: return (GPIO_PinSource11); - case GPIO_Pin_12: return (GPIO_PinSource12); - case GPIO_Pin_13: return (GPIO_PinSource13); - case GPIO_Pin_14: return (GPIO_PinSource14); - case GPIO_Pin_15: return (GPIO_PinSource15); - } + switch ((uint32_t)gpio_pin) { + case GPIO_Pin_0: return GPIO_PinSource0; - PIOS_Assert(0); - return 0xFF; + case GPIO_Pin_1: return GPIO_PinSource1; + + case GPIO_Pin_2: return GPIO_PinSource2; + + case GPIO_Pin_3: return GPIO_PinSource3; + + case GPIO_Pin_4: return GPIO_PinSource4; + + case GPIO_Pin_5: return GPIO_PinSource5; + + case GPIO_Pin_6: return GPIO_PinSource6; + + case GPIO_Pin_7: return GPIO_PinSource7; + + case GPIO_Pin_8: return GPIO_PinSource8; + + case GPIO_Pin_9: return GPIO_PinSource9; + + case GPIO_Pin_10: return GPIO_PinSource10; + + case GPIO_Pin_11: return GPIO_PinSource11; + + case GPIO_Pin_12: return GPIO_PinSource12; + + case GPIO_Pin_13: return GPIO_PinSource13; + + case GPIO_Pin_14: return GPIO_PinSource14; + + case GPIO_Pin_15: return GPIO_PinSource15; + } + + PIOS_Assert(0); + return 0xFF; } -int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg) +int32_t PIOS_EXTI_Init(const struct pios_exti_cfg *cfg) { - PIOS_Assert(cfg); - PIOS_Assert(&__start__exti); - PIOS_Assert(cfg >= &__start__exti); - PIOS_Assert(cfg < &__stop__exti); + PIOS_Assert(cfg); + PIOS_Assert(&__start__exti); + PIOS_Assert(cfg >= &__start__exti); + PIOS_Assert(cfg < &__stop__exti); - uint8_t cfg_index = cfg - &__start__exti; + uint8_t cfg_index = cfg - &__start__exti; - /* Connect this config to the requested vector */ - uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line); + /* Connect this config to the requested vector */ + uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line); - if (pios_exti_line_to_cfg_map[line_index] != PIOS_EXTI_INVALID) { - /* Someone else already has this mapped */ - goto out_fail; - } + if (pios_exti_line_to_cfg_map[line_index] != PIOS_EXTI_INVALID) { + /* Someone else already has this mapped */ + goto out_fail; + } - /* Bind the config to the exti line */ - pios_exti_line_to_cfg_map[line_index] = cfg_index; + /* Bind the config to the exti line */ + pios_exti_line_to_cfg_map[line_index] = cfg_index; - /* Initialize the GPIO pin */ - GPIO_Init(cfg->pin.gpio, &cfg->pin.init); + /* Initialize the GPIO pin */ + GPIO_Init(cfg->pin.gpio, &cfg->pin.init); - /* Set up the EXTI interrupt source */ - uint8_t exti_source_port = PIOS_EXTI_gpio_port_to_exti_source_port(cfg->pin.gpio); - uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin); - SYSCFG_EXTILineConfig(exti_source_port, exti_source_pin); - EXTI_Init(&cfg->exti.init); + /* Set up the EXTI interrupt source */ + uint8_t exti_source_port = PIOS_EXTI_gpio_port_to_exti_source_port(cfg->pin.gpio); + uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin); + SYSCFG_EXTILineConfig(exti_source_port, exti_source_pin); + EXTI_Init(&cfg->exti.init); - /* Enable the interrupt channel */ - NVIC_Init(&cfg->irq.init); + /* Enable the interrupt channel */ + NVIC_Init(&cfg->irq.init); - return 0; + return 0; out_fail: - return -1; + return -1; } static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index) { - uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index]; + uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index]; - PIOS_Assert(&__start__exti); + PIOS_Assert(&__start__exti); - if (cfg_index > NELEMENTS(pios_exti_line_to_cfg_map) || - cfg_index == PIOS_EXTI_INVALID) { - /* Unconfigured interrupt just fired! */ - return false; - } + if (cfg_index > NELEMENTS(pios_exti_line_to_cfg_map) || + cfg_index == PIOS_EXTI_INVALID) { + /* Unconfigured interrupt just fired! */ + return false; + } - struct pios_exti_cfg * cfg = &__start__exti + cfg_index; - return cfg->vector(); + struct pios_exti_cfg *cfg = &__start__exti + cfg_index; + return cfg->vector(); } /* Bind Interrupt Handlers */ #ifdef PIOS_INCLUDE_FREERTOS -#define PIOS_EXTI_HANDLE_LINE(line, woken) \ - if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ - EXTI_ClearITPendingBit(EXTI_Line##line); \ - woken = PIOS_EXTI_generic_irq_handler(line) ? pdTRUE : woken; \ - } +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + woken = PIOS_EXTI_generic_irq_handler(line) ? pdTRUE : woken; \ + } #else -#define PIOS_EXTI_HANDLE_LINE(line, woken) \ - if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ - EXTI_ClearITPendingBit(EXTI_Line##line); \ - PIOS_EXTI_generic_irq_handler(line); \ - } +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + PIOS_EXTI_generic_irq_handler(line); \ + } #endif -static void PIOS_EXTI_0_irq_handler (void) +static void PIOS_EXTI_0_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; + bool xHigherPriorityTaskWoken; #endif - PIOS_EXTI_HANDLE_LINE(0, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(0, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI0_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_0_irq_handler"))); +void EXTI0_IRQHandler(void) __attribute__((alias("PIOS_EXTI_0_irq_handler"))); -static void PIOS_EXTI_1_irq_handler (void) +static void PIOS_EXTI_1_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; + bool xHigherPriorityTaskWoken; #endif - PIOS_EXTI_HANDLE_LINE(1, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(1, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI1_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_1_irq_handler"))); +void EXTI1_IRQHandler(void) __attribute__((alias("PIOS_EXTI_1_irq_handler"))); -static void PIOS_EXTI_2_irq_handler (void) +static void PIOS_EXTI_2_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; + bool xHigherPriorityTaskWoken; #endif - PIOS_EXTI_HANDLE_LINE(2, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(2, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI2_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_2_irq_handler"))); +void EXTI2_IRQHandler(void) __attribute__((alias("PIOS_EXTI_2_irq_handler"))); -static void PIOS_EXTI_3_irq_handler (void) +static void PIOS_EXTI_3_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; + bool xHigherPriorityTaskWoken; #endif - PIOS_EXTI_HANDLE_LINE(3, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(3, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI3_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_3_irq_handler"))); +void EXTI3_IRQHandler(void) __attribute__((alias("PIOS_EXTI_3_irq_handler"))); -static void PIOS_EXTI_4_irq_handler (void) +static void PIOS_EXTI_4_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; + bool xHigherPriorityTaskWoken; #endif - PIOS_EXTI_HANDLE_LINE(4, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(4, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI4_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_4_irq_handler"))); +void EXTI4_IRQHandler(void) __attribute__((alias("PIOS_EXTI_4_irq_handler"))); -static void PIOS_EXTI_9_5_irq_handler (void) +static void PIOS_EXTI_9_5_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; + bool xHigherPriorityTaskWoken; #endif - PIOS_EXTI_HANDLE_LINE(5, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(6, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(7, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(8, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(9, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(5, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(6, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(7, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(8, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(9, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI9_5_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_9_5_irq_handler"))); +void EXTI9_5_IRQHandler(void) __attribute__((alias("PIOS_EXTI_9_5_irq_handler"))); -static void PIOS_EXTI_15_10_irq_handler (void) +static void PIOS_EXTI_15_10_irq_handler(void) { #ifdef PIOS_INCLUDE_FREERTOS - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; #else - bool xHigherPriorityTaskWoken; + bool xHigherPriorityTaskWoken; #endif - PIOS_EXTI_HANDLE_LINE(10, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(11, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(12, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(13, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(14, xHigherPriorityTaskWoken); - PIOS_EXTI_HANDLE_LINE(15, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(10, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(11, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(12, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(13, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(14, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(15, xHigherPriorityTaskWoken); #ifdef PIOS_INCLUDE_FREERTOS - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); #endif } -void EXTI15_10_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_15_10_irq_handler"))); +void EXTI15_10_IRQHandler(void) __attribute__((alias("PIOS_EXTI_15_10_irq_handler"))); #endif /* PIOS_INCLUDE_EXTI */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_flash_internal.c b/flight/pios/stm32f4xx/pios_flash_internal.c index 5342e464e..68cadd3ef 100644 --- a/flight/pios/stm32f4xx/pios_flash_internal.c +++ b/flight/pios/stm32f4xx/pios_flash_internal.c @@ -2,25 +2,25 @@ ****************************************************************************** * @file pios_flash_internal.c * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 - * @addtogroup + * @addtogroup * @{ - * @addtogroup + * @addtogroup * @{ * @brief Provides a flash driver for the STM32 internal flash sectors *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,153 +34,157 @@ #include struct device_flash_sector { - uint32_t start; - uint32_t size; - uint16_t st_sector; + uint32_t start; + uint32_t size; + uint16_t st_sector; }; static struct device_flash_sector flash_sectors[] = { - [0] = { - .start = 0x08000000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_0, - }, - [1] = { - .start = 0x08004000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_1, - }, - [2] = { - .start = 0x08008000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_2, - }, - [3] = { - .start = 0x0800C000, - .size = 16 * 1024, - .st_sector = FLASH_Sector_3, - }, - [4] = { - .start = 0x08010000, - .size = 64 * 1024, - .st_sector = FLASH_Sector_4, - }, - [5] = { - .start = 0x08020000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_5, - }, - [6] = { - .start = 0x08040000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_6, - }, - [7] = { - .start = 0x08060000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_7, - }, - [8] = { - .start = 0x08080000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_8, - }, - [9] = { - .start = 0x080A0000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_9, - }, - [10] = { - .start = 0x080C0000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_10, - }, - [11] = { - .start = 0x080E0000, - .size = 128 * 1024, - .st_sector = FLASH_Sector_11, - }, + [0] = { + .start = 0x08000000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_0, + }, + [1] = { + .start = 0x08004000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_1, + }, + [2] = { + .start = 0x08008000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_2, + }, + [3] = { + .start = 0x0800C000, + .size = 16 * 1024, + .st_sector = FLASH_Sector_3, + }, + [4] = { + .start = 0x08010000, + .size = 64 * 1024, + .st_sector = FLASH_Sector_4, + }, + [5] = { + .start = 0x08020000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_5, + }, + [6] = { + .start = 0x08040000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_6, + }, + [7] = { + .start = 0x08060000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_7, + }, + [8] = { + .start = 0x08080000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_8, + }, + [9] = { + .start = 0x080A0000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_9, + }, + [10] = { + .start = 0x080C0000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_10, + }, + [11] = { + .start = 0x080E0000, + .size = 128 * 1024, + .st_sector = FLASH_Sector_11, + }, }; -static bool PIOS_Flash_Internal_GetSectorInfo(uint32_t address, uint8_t * sector_number, uint32_t *sector_start, uint32_t *sector_size) +static bool PIOS_Flash_Internal_GetSectorInfo(uint32_t address, uint8_t *sector_number, uint32_t *sector_start, uint32_t *sector_size) { - for (uint8_t i = 0; i < NELEMENTS(flash_sectors); i++) { - struct device_flash_sector * sector = &flash_sectors[i]; - if ((address >= sector->start) && - (address < (sector->start + sector->size))) { - /* address lies within this sector */ - *sector_number = sector->st_sector; - *sector_start = sector->start; - *sector_size = sector->size; - return (true); - } - } + for (uint8_t i = 0; i < NELEMENTS(flash_sectors); i++) { + struct device_flash_sector *sector = &flash_sectors[i]; + if ((address >= sector->start) && + (address < (sector->start + sector->size))) { + /* address lies within this sector */ + *sector_number = sector->st_sector; + *sector_start = sector->start; + *sector_size = sector->size; + return true; + } + } - return (false); + return false; } enum pios_internal_flash_dev_magic { - PIOS_INTERNAL_FLASH_DEV_MAGIC = 0x33445902, + PIOS_INTERNAL_FLASH_DEV_MAGIC = 0x33445902, }; struct pios_internal_flash_dev { - enum pios_internal_flash_dev_magic magic; + enum pios_internal_flash_dev_magic magic; #if defined(PIOS_INCLUDE_FREERTOS) - xSemaphoreHandle transaction_lock; -#endif /* defined(PIOS_INCLUDE_FREERTOS) */ + xSemaphoreHandle transaction_lock; +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ }; -static bool PIOS_Flash_Internal_Validate(struct pios_internal_flash_dev * flash_dev) { - return (flash_dev && (flash_dev->magic == PIOS_INTERNAL_FLASH_DEV_MAGIC)); +static bool PIOS_Flash_Internal_Validate(struct pios_internal_flash_dev *flash_dev) +{ + return flash_dev && (flash_dev->magic == PIOS_INTERNAL_FLASH_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_internal_flash_dev * PIOS_Flash_Internal_alloc(void) +static struct pios_internal_flash_dev *PIOS_Flash_Internal_alloc(void) { - struct pios_internal_flash_dev * flash_dev; + struct pios_internal_flash_dev *flash_dev; - flash_dev = (struct pios_internal_flash_dev *)pvPortMalloc(sizeof(* flash_dev)); - if (!flash_dev) return (NULL); + flash_dev = (struct pios_internal_flash_dev *)pvPortMalloc(sizeof(*flash_dev)); + if (!flash_dev) { + return NULL; + } - flash_dev->magic = PIOS_INTERNAL_FLASH_DEV_MAGIC; + flash_dev->magic = PIOS_INTERNAL_FLASH_DEV_MAGIC; - return(flash_dev); + return flash_dev; } #else static struct pios_internal_flash_dev pios_internal_flash_devs[PIOS_INTERNAL_FLASH_MAX_DEVS]; static uint8_t pios_internal_flash_num_devs; -static struct pios_internal_flash_dev * PIOS_Flash_Internal_alloc(void) +static struct pios_internal_flash_dev *PIOS_Flash_Internal_alloc(void) { - struct pios_internal_flash_dev * flash_dev; + struct pios_internal_flash_dev *flash_dev; - if (pios_internal_flash_num_devs >= PIOS_INTERNAL_FLASH_MAX_DEVS) { - return (NULL); - } + if (pios_internal_flash_num_devs >= PIOS_INTERNAL_FLASH_MAX_DEVS) { + return NULL; + } - flash_dev = &pios_internal_flash_devs[pios_internal_flash_num_devs++]; - flash_dev->magic = PIOS_INTERNAL_FLASH_DEV_MAGIC; + flash_dev = &pios_internal_flash_devs[pios_internal_flash_num_devs++]; + flash_dev->magic = PIOS_INTERNAL_FLASH_DEV_MAGIC; - return (flash_dev); + return flash_dev; } #endif /* defined(PIOS_INCLUDE_FREERTOS) */ -int32_t PIOS_Flash_Internal_Init(uintptr_t * flash_id, __attribute__((unused)) const struct pios_flash_internal_cfg * cfg) +int32_t PIOS_Flash_Internal_Init(uintptr_t *flash_id, __attribute__((unused)) const struct pios_flash_internal_cfg *cfg) { - struct pios_internal_flash_dev * flash_dev; + struct pios_internal_flash_dev *flash_dev; - flash_dev = PIOS_Flash_Internal_alloc(); - if (flash_dev == NULL) - return -1; + flash_dev = PIOS_Flash_Internal_alloc(); + if (flash_dev == NULL) { + return -1; + } #if defined(PIOS_INCLUDE_FREERTOS) - flash_dev->transaction_lock = xSemaphoreCreateMutex(); -#endif /* defined(PIOS_INCLUDE_FREERTOS) */ + flash_dev->transaction_lock = xSemaphoreCreateMutex(); +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ - *flash_id = (uintptr_t) flash_dev; + *flash_id = (uintptr_t)flash_dev; - return 0; + return 0; } /********************************** @@ -192,150 +196,159 @@ int32_t PIOS_Flash_Internal_Init(uintptr_t * flash_id, __attribute__((unused)) c static int32_t PIOS_Flash_Internal_StartTransaction(uintptr_t flash_id) { - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; - if (!PIOS_Flash_Internal_Validate(flash_dev)) - return -1; + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreTake(flash_dev->transaction_lock, portMAX_DELAY) != pdTRUE) - return -2; -#endif /* defined(PIOS_INCLUDE_FREERTOS) */ + if (xSemaphoreTake(flash_dev->transaction_lock, portMAX_DELAY) != pdTRUE) { + return -2; + } +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ - /* Unlock the internal flash so we can write to it */ - FLASH_Unlock(); - return 0; + /* Unlock the internal flash so we can write to it */ + FLASH_Unlock(); + return 0; } static int32_t PIOS_Flash_Internal_EndTransaction(uintptr_t flash_id) { - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; - if (!PIOS_Flash_Internal_Validate(flash_dev)) - return -1; + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; + } #if defined(PIOS_INCLUDE_FREERTOS) - if (xSemaphoreGive(flash_dev->transaction_lock) != pdTRUE) - return -2; -#endif /* defined(PIOS_INCLUDE_FREERTOS) */ + if (xSemaphoreGive(flash_dev->transaction_lock) != pdTRUE) { + return -2; + } +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ - /* Lock the internal flash again so we can no longer write to it */ - FLASH_Lock(); + /* Lock the internal flash again so we can no longer write to it */ + FLASH_Lock(); - return 0; + return 0; } static int32_t PIOS_Flash_Internal_EraseSector(uintptr_t flash_id, uint32_t addr) { - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; - if (!PIOS_Flash_Internal_Validate(flash_dev)) - return -1; - - uint8_t sector_number; - uint32_t sector_start; - uint32_t sector_size; - if (!PIOS_Flash_Internal_GetSectorInfo(addr, - §or_number, - §or_start, - §or_size)) { - /* We're asking for an invalid flash address */ - return -2; - } - - if (FLASH_EraseSector(sector_number, VoltageRange_3) != FLASH_COMPLETE) - return -3; - - return 0; -} - -static int32_t PIOS_Flash_Internal_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len) -{ - PIOS_Assert(data); - - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; - - if (!PIOS_Flash_Internal_Validate(flash_dev)) - return -1; - - uint8_t sector_number; - uint32_t sector_start; - uint32_t sector_size; - - /* Ensure that the base address is in a valid sector */ - if (!PIOS_Flash_Internal_GetSectorInfo(addr, - §or_number, - §or_start, - §or_size)) { - /* We're asking for an invalid flash address */ - return -2; - } - - /* Ensure that the entire write occurs within the same sector */ - if ((uintptr_t)addr + len > sector_start + sector_size) { - /* Write crosses the end of the sector */ - return -3; - } - FLASH_Status status; - for (uint16_t i = 0; i < len/4; i++){ - uint32_t data_word = *(uint32_t *)(data + i*4); - status = FLASH_ProgramWord(addr + i * 4, data_word); - if(status != FLASH_COMPLETE) - return -4; + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; } - /* Write the data */ - for (uint16_t i = len - len % 4; i < len; i++) { + uint8_t sector_number; + uint32_t sector_start; + uint32_t sector_size; + if (!PIOS_Flash_Internal_GetSectorInfo(addr, + §or_number, + §or_start, + §or_size)) { + /* We're asking for an invalid flash address */ + return -2; + } - status = FLASH_ProgramByte(addr + i, data[i]); - if(status != FLASH_COMPLETE) - return -5; - } + if (FLASH_EraseSector(sector_number, VoltageRange_3) != FLASH_COMPLETE) { + return -3; + } - return 0; + return 0; } -static int32_t PIOS_Flash_Internal_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len) +static int32_t PIOS_Flash_Internal_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) { - PIOS_Assert(data); + PIOS_Assert(data); - struct pios_internal_flash_dev * flash_dev = (struct pios_internal_flash_dev *)flash_id; + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; - if (!PIOS_Flash_Internal_Validate(flash_dev)) - return -1; + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; + } - uint8_t sector_number; - uint32_t sector_start; - uint32_t sector_size; + uint8_t sector_number; + uint32_t sector_start; + uint32_t sector_size; - /* Ensure that the base address is in a valid sector */ - if (!PIOS_Flash_Internal_GetSectorInfo(addr, - §or_number, - §or_start, - §or_size)) { - /* We're asking for an invalid flash address */ - return -2; - } + /* Ensure that the base address is in a valid sector */ + if (!PIOS_Flash_Internal_GetSectorInfo(addr, + §or_number, + §or_start, + §or_size)) { + /* We're asking for an invalid flash address */ + return -2; + } - /* Ensure that the entire read occurs within the same sector */ - if ((uintptr_t)addr + len > sector_start + sector_size) { - /* Read crosses the end of the sector */ - return -3; - } + /* Ensure that the entire write occurs within the same sector */ + if ((uintptr_t)addr + len > sector_start + sector_size) { + /* Write crosses the end of the sector */ + return -3; + } + FLASH_Status status; + for (uint16_t i = 0; i < len / 4; i++) { + uint32_t data_word = *(uint32_t *)(data + i * 4); + status = FLASH_ProgramWord(addr + i * 4, data_word); + if (status != FLASH_COMPLETE) { + return -4; + } + } - /* Read the data into the buffer directly */ - memcpy(data, (void *)addr, len); + /* Write the data */ + for (uint16_t i = len - len % 4; i < len; i++) { + status = FLASH_ProgramByte(addr + i, data[i]); + if (status != FLASH_COMPLETE) { + return -5; + } + } - return 0; + return 0; +} + +static int32_t PIOS_Flash_Internal_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) +{ + PIOS_Assert(data); + + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; + + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; + } + + uint8_t sector_number; + uint32_t sector_start; + uint32_t sector_size; + + /* Ensure that the base address is in a valid sector */ + if (!PIOS_Flash_Internal_GetSectorInfo(addr, + §or_number, + §or_start, + §or_size)) { + /* We're asking for an invalid flash address */ + return -2; + } + + /* Ensure that the entire read occurs within the same sector */ + if ((uintptr_t)addr + len > sector_start + sector_size) { + /* Read crosses the end of the sector */ + return -3; + } + + /* Read the data into the buffer directly */ + memcpy(data, (void *)addr, len); + + return 0; } /* Provide a flash driver to external drivers */ const struct pios_flash_driver pios_internal_flash_driver = { - .start_transaction = PIOS_Flash_Internal_StartTransaction, - .end_transaction = PIOS_Flash_Internal_EndTransaction, - .erase_sector = PIOS_Flash_Internal_EraseSector, - .write_data = PIOS_Flash_Internal_WriteData, - .read_data = PIOS_Flash_Internal_ReadData, + .start_transaction = PIOS_Flash_Internal_StartTransaction, + .end_transaction = PIOS_Flash_Internal_EndTransaction, + .erase_sector = PIOS_Flash_Internal_EraseSector, + .write_data = PIOS_Flash_Internal_WriteData, + .read_data = PIOS_Flash_Internal_ReadData, }; #endif /* PIOS_INCLUDE_FLASH_INTERNAL */ diff --git a/flight/pios/stm32f4xx/pios_gpio.c b/flight/pios/stm32f4xx/pios_gpio.c index 18c083d9b..46850f4c1 100644 --- a/flight/pios/stm32f4xx/pios_gpio.c +++ b/flight/pios/stm32f4xx/pios_gpio.c @@ -12,19 +12,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,61 +39,62 @@ static GPIO_TypeDef *GPIO_PORT[PIOS_GPIO_NUM] = PIOS_GPIO_PORTS; static const uint32_t GPIO_PIN[PIOS_GPIO_NUM] = PIOS_GPIO_PINS; /** -* Initialises all the GPIO's -*/ + * Initialises all the GPIO's + */ void PIOS_GPIO_Init(void) { - /* Do nothing */ + /* Do nothing */ } /** -* Enable a GPIO Pin -* \param[in] Pin Pin Number -*/ + * Enable a GPIO Pin + * \param[in] Pin Pin Number + */ void PIOS_GPIO_Enable(uint8_t Pin) { - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_InitStructure.GPIO_Pin = GPIO_PIN[Pin]; - GPIO_Init(GPIO_PORT[Pin], &GPIO_InitStructure); + GPIO_InitTypeDef GPIO_InitStructure; - /* GPIO's Off */ - PIOS_GPIO_Off(Pin); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_InitStructure.GPIO_Pin = GPIO_PIN[Pin]; + GPIO_Init(GPIO_PORT[Pin], &GPIO_InitStructure); + + /* GPIO's Off */ + PIOS_GPIO_Off(Pin); } /** -* Turn on Pin -* \param[in] Pin Pin Number -*/ + * Turn on Pin + * \param[in] Pin Pin Number + */ void PIOS_GPIO_On(uint8_t Pin) { - GPIO_ResetBits(GPIO_PORT[Pin], GPIO_PIN[Pin]); + GPIO_ResetBits(GPIO_PORT[Pin], GPIO_PIN[Pin]); } /** -* Turn off Pin -* \param[in] Pin Pin Number -*/ + * Turn off Pin + * \param[in] Pin Pin Number + */ void PIOS_GPIO_Off(uint8_t Pin) { - GPIO_SetBits(GPIO_PORT[Pin], GPIO_PIN[Pin]); + GPIO_SetBits(GPIO_PORT[Pin], GPIO_PIN[Pin]); } /** -* Toggle Pin on/off -* \param[in] Pin Pin Number -*/ + * Toggle Pin on/off + * \param[in] Pin Pin Number + */ void PIOS_GPIO_Toggle(uint8_t Pin) { - GPIO_ToggleBits(GPIO_PORT[Pin], GPIO_PIN[Pin]); + GPIO_ToggleBits(GPIO_PORT[Pin], GPIO_PIN[Pin]); } #endif /* PIOS_INCLUDE_GPIO */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_i2c.c b/flight/pios/stm32f4xx/pios_i2c.c index 9dec896da..4a98cff33 100644 --- a/flight/pios/stm32f4xx/pios_i2c.c +++ b/flight/pios/stm32f4xx/pios_i2c.c @@ -6,25 +6,25 @@ * @brief STM32F4xx Hardware dependent I2C functionality * @{ * - * @file pios_i2c.c + * @file pios_i2c.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief I2C Enable/Disable routines * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,28 +43,28 @@ #include -//#define I2C_HALT_ON_ERRORS +// #define I2C_HALT_ON_ERRORS enum i2c_adapter_event { - I2C_EVENT_BUS_ERROR, - I2C_EVENT_START, - I2C_EVENT_STARTED_MORE_TXN_READ, - I2C_EVENT_STARTED_MORE_TXN_WRITE, - I2C_EVENT_STARTED_LAST_TXN_READ, - I2C_EVENT_STARTED_LAST_TXN_WRITE, - I2C_EVENT_ADDR_SENT_LEN_EQ_0, - I2C_EVENT_ADDR_SENT_LEN_EQ_1, - I2C_EVENT_ADDR_SENT_LEN_EQ_2, - I2C_EVENT_ADDR_SENT_LEN_GT_2, - I2C_EVENT_TRANSFER_DONE_LEN_EQ_0, - I2C_EVENT_TRANSFER_DONE_LEN_EQ_1, - I2C_EVENT_TRANSFER_DONE_LEN_EQ_2, - I2C_EVENT_TRANSFER_DONE_LEN_GT_2, - I2C_EVENT_NACK, - I2C_EVENT_STOPPED, - I2C_EVENT_AUTO, /* FIXME: remove this */ + I2C_EVENT_BUS_ERROR, + I2C_EVENT_START, + I2C_EVENT_STARTED_MORE_TXN_READ, + I2C_EVENT_STARTED_MORE_TXN_WRITE, + I2C_EVENT_STARTED_LAST_TXN_READ, + I2C_EVENT_STARTED_LAST_TXN_WRITE, + I2C_EVENT_ADDR_SENT_LEN_EQ_0, + I2C_EVENT_ADDR_SENT_LEN_EQ_1, + I2C_EVENT_ADDR_SENT_LEN_EQ_2, + I2C_EVENT_ADDR_SENT_LEN_GT_2, + I2C_EVENT_TRANSFER_DONE_LEN_EQ_0, + I2C_EVENT_TRANSFER_DONE_LEN_EQ_1, + I2C_EVENT_TRANSFER_DONE_LEN_EQ_2, + I2C_EVENT_TRANSFER_DONE_LEN_GT_2, + I2C_EVENT_NACK, + I2C_EVENT_STOPPED, + I2C_EVENT_AUTO, /* FIXME: remove this */ - I2C_EVENT_NUM_EVENTS /* Must be last */ + I2C_EVENT_NUM_EVENTS /* Must be last */ }; #if defined(PIOS_I2C_DIAGNOSTICS) @@ -82,11 +82,11 @@ volatile uint8_t i2c_state_history_pointer = 0; volatile enum i2c_adapter_event i2c_state_event_history[I2C_LOG_DEPTH]; volatile uint8_t i2c_state_event_history_pointer; -static uint8_t i2c_fsm_fault_count = 0; +static uint8_t i2c_fsm_fault_count = 0; static uint8_t i2c_bad_event_counter = 0; static uint8_t i2c_error_interrupt_counter = 0; static uint8_t i2c_nack_counter = 0; -static uint8_t i2c_timeout_counter = 0; +static uint8_t i2c_timeout_counter = 0; #endif static void go_fsm_fault(struct pios_i2c_adapter *i2c_adapter); @@ -119,8 +119,8 @@ static void go_w_last_txn_last(struct pios_i2c_adapter *i2c_adapter); static void go_nack(struct pios_i2c_adapter *i2c_adapter); struct i2c_adapter_transition { - void (*entry_fn) (struct pios_i2c_adapter * i2c_adapter); - enum i2c_adapter_state next_state[I2C_EVENT_NUM_EVENTS]; + void (*entry_fn)(struct pios_i2c_adapter *i2c_adapter); + enum i2c_adapter_state next_state[I2C_EVENT_NUM_EVENTS]; }; static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter); @@ -133,624 +133,631 @@ static void i2c_adapter_log_fault(enum pios_i2c_error_type type); static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter); static const struct i2c_adapter_transition i2c_adapter_transitions[I2C_STATE_NUM_STATES] = { - [I2C_STATE_FSM_FAULT] = { - .entry_fn = go_fsm_fault, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, - [I2C_STATE_BUS_ERROR] = { - .entry_fn = go_bus_error, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, + [I2C_STATE_FSM_FAULT] = { + .entry_fn = go_fsm_fault, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, + [I2C_STATE_BUS_ERROR] = { + .entry_fn = go_bus_error, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, - [I2C_STATE_STOPPED] = { - .entry_fn = go_stopped, - .next_state = { - [I2C_EVENT_START] = I2C_STATE_STARTING, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_STOPPED] = { + .entry_fn = go_stopped, + .next_state = { + [I2C_EVENT_START] = I2C_STATE_STARTING, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_STOPPING] = { - .entry_fn = go_stopping, - .next_state = { - [I2C_EVENT_STOPPED] = I2C_STATE_STOPPED, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_STOPPING] = { + .entry_fn = go_stopping, + .next_state = { + [I2C_EVENT_STOPPED] = I2C_STATE_STOPPED, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_STARTING] = { - .entry_fn = go_starting, - .next_state = { - [I2C_EVENT_STARTED_MORE_TXN_READ] = I2C_STATE_R_MORE_TXN_ADDR, - [I2C_EVENT_STARTED_MORE_TXN_WRITE] = I2C_STATE_W_MORE_TXN_ADDR, - [I2C_EVENT_STARTED_LAST_TXN_READ] = I2C_STATE_R_LAST_TXN_ADDR, - [I2C_EVENT_STARTED_LAST_TXN_WRITE] = I2C_STATE_W_LAST_TXN_ADDR, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_STARTING] = { + .entry_fn = go_starting, + .next_state = { + [I2C_EVENT_STARTED_MORE_TXN_READ] = I2C_STATE_R_MORE_TXN_ADDR, + [I2C_EVENT_STARTED_MORE_TXN_WRITE] = I2C_STATE_W_MORE_TXN_ADDR, + [I2C_EVENT_STARTED_LAST_TXN_READ] = I2C_STATE_R_LAST_TXN_ADDR, + [I2C_EVENT_STARTED_LAST_TXN_WRITE] = I2C_STATE_W_LAST_TXN_ADDR, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - /* - * Read with restart - */ + /* + * Read with restart + */ - [I2C_STATE_R_MORE_TXN_ADDR] = { - .entry_fn = go_r_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_PRE_ONE, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_ADDR] = { + .entry_fn = go_r_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_PRE_ONE, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_FIRST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_ONE] = { - .entry_fn = go_r_more_txn_pre_one, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_ONE] = { + .entry_fn = go_r_more_txn_pre_one, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_FIRST] = { - .entry_fn = go_r_any_txn_pre_first, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_FIRST] = { + .entry_fn = go_r_any_txn_pre_first, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_MIDDLE] = { - .entry_fn = go_r_any_txn_pre_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_MIDDLE] = { + .entry_fn = go_r_any_txn_pre_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_MORE_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_MORE_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_PRE_LAST] = { - .entry_fn = go_r_more_txn_pre_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_MORE_TXN_PRE_LAST] = { + .entry_fn = go_r_more_txn_pre_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_MORE_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_MORE_TXN_POST_LAST] = { - .entry_fn = go_r_any_txn_post_last, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STARTING, - }, - }, + [I2C_STATE_R_MORE_TXN_POST_LAST] = { + .entry_fn = go_r_any_txn_post_last, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STARTING, + }, + }, - /* - * Read - */ + /* + * Read + */ - [I2C_STATE_R_LAST_TXN_ADDR] = { - .entry_fn = go_r_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_PRE_ONE, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_ADDR] = { + .entry_fn = go_r_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_PRE_ONE, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_FIRST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_ONE] = { - .entry_fn = go_r_last_txn_pre_one, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_ONE] = { + .entry_fn = go_r_last_txn_pre_one, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_FIRST] = { - .entry_fn = go_r_any_txn_pre_first, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_FIRST] = { + .entry_fn = go_r_any_txn_pre_first, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_MIDDLE] = { - .entry_fn = go_r_any_txn_pre_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_MIDDLE] = { + .entry_fn = go_r_any_txn_pre_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_R_LAST_TXN_PRE_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_R_LAST_TXN_PRE_MIDDLE, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_PRE_LAST] = { - .entry_fn = go_r_last_txn_pre_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_R_LAST_TXN_PRE_LAST] = { + .entry_fn = go_r_last_txn_pre_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_R_LAST_TXN_POST_LAST, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_R_LAST_TXN_POST_LAST] = { - .entry_fn = go_r_any_txn_post_last, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, + [I2C_STATE_R_LAST_TXN_POST_LAST] = { + .entry_fn = go_r_any_txn_post_last, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, - /* - * Write with restart - */ + /* + * Write with restart + */ - [I2C_STATE_W_MORE_TXN_ADDR] = { - .entry_fn = go_w_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_MORE_TXN_ADDR] = { + .entry_fn = go_w_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_MORE_TXN_MIDDLE] = { - .entry_fn = go_w_any_txn_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_MORE_TXN_MIDDLE] = { + .entry_fn = go_w_any_txn_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_MORE_TXN_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_MORE_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_MORE_TXN_LAST] = { - .entry_fn = go_w_more_txn_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STARTING, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_MORE_TXN_LAST] = { + .entry_fn = go_w_more_txn_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STARTING, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - /* - * Write - */ + /* + * Write + */ - [I2C_STATE_W_LAST_TXN_ADDR] = { - .entry_fn = go_w_any_txn_addr, - .next_state = { - [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, - [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_LAST_TXN_ADDR] = { + .entry_fn = go_w_any_txn_addr, + .next_state = { + [I2C_EVENT_ADDR_SENT_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, + [I2C_EVENT_ADDR_SENT_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_ADDR_SENT_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_LAST_TXN_MIDDLE] = { - .entry_fn = go_w_any_txn_middle, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, + [I2C_STATE_W_LAST_TXN_MIDDLE] = { + .entry_fn = go_w_any_txn_middle, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_1] = I2C_STATE_W_LAST_TXN_LAST, + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_TRANSFER_DONE_LEN_GT_2] = I2C_STATE_W_LAST_TXN_MIDDLE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, - [I2C_STATE_W_LAST_TXN_LAST] = { - .entry_fn = go_w_last_txn_last, - .next_state = { - [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STOPPING, - [I2C_EVENT_NACK] = I2C_STATE_NACK, - [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, - }, - }, - [I2C_STATE_NACK] = { - .entry_fn = go_nack, - .next_state = { - [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, - }, - }, + [I2C_STATE_W_LAST_TXN_LAST] = { + .entry_fn = go_w_last_txn_last, + .next_state = { + [I2C_EVENT_TRANSFER_DONE_LEN_EQ_0] = I2C_STATE_STOPPING, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, + [I2C_STATE_NACK] = { + .entry_fn = go_nack, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPING, + }, + }, }; static void go_fsm_fault(struct pios_i2c_adapter *i2c_adapter) { #if defined(I2C_HALT_ON_ERRORS) - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); #endif - /* Note that this transfer has hit a bus error */ - i2c_adapter->bus_error = true; + /* Note that this transfer has hit a bus error */ + i2c_adapter->bus_error = true; - i2c_adapter_reset_bus(i2c_adapter); - + i2c_adapter_reset_bus(i2c_adapter); } static void go_bus_error(struct pios_i2c_adapter *i2c_adapter) { - /* Note that this transfer has hit a bus error */ - i2c_adapter->bus_error = true; + /* Note that this transfer has hit a bus error */ + i2c_adapter->bus_error = true; - i2c_adapter_reset_bus(i2c_adapter); + i2c_adapter_reset_bus(i2c_adapter); } static void go_stopping(struct pios_i2c_adapter *i2c_adapter) { #ifdef USE_FREERTOS - signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; + signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; #endif - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); #ifdef USE_FREERTOS - if (xSemaphoreGiveFromISR(i2c_adapter->sem_ready, &pxHigherPriorityTaskWoken) != pdTRUE) { + if (xSemaphoreGiveFromISR(i2c_adapter->sem_ready, &pxHigherPriorityTaskWoken) != pdTRUE) { #if defined(I2C_HALT_ON_ERRORS) - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); #endif - } - portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for this? */ + } + portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for this? */ #endif /* USE_FREERTOS */ - if(i2c_adapter->callback) - i2c_adapter_callback_handler(i2c_adapter); + if (i2c_adapter->callback) { + i2c_adapter_callback_handler(i2c_adapter); + } } static void go_stopped(struct pios_i2c_adapter *i2c_adapter) { - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); } static void go_starting(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]); - i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); + i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]); + i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); - I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); - if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) { - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); - } else { - // For write operations, do not enable the IT_BUF events. - // The current driver does not act when the TX data register is not full, only when the complete byte is sent. - // With the IT_BUF enabled, we constantly get IRQs, See OP-326 - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_ERR, ENABLE); - } + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); + if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) { + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); + } else { + // For write operations, do not enable the IT_BUF events. + // The current driver does not act when the TX data register is not full, only when the complete byte is sent. + // With the IT_BUF enabled, we constantly get IRQs, See OP-326 + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_ERR, ENABLE); + } } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_addr(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ); + PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ); - I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Receiver); + I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Receiver); } static void go_r_more_txn_pre_one(struct pios_i2c_adapter *i2c_adapter) { - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); } static void go_r_last_txn_pre_one(struct pios_i2c_adapter *i2c_adapter) { - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_pre_first(struct pios_i2c_adapter *i2c_adapter) { - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE); } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_pre_middle(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } static void go_r_more_txn_pre_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - PIOS_IRQ_Disable(); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); - PIOS_IRQ_Enable(); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + PIOS_IRQ_Disable(); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); + PIOS_IRQ_Enable(); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } static void go_r_last_txn_pre_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - PIOS_IRQ_Disable(); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); - PIOS_IRQ_Enable(); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + PIOS_IRQ_Disable(); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); + PIOS_IRQ_Enable(); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } /* Common to 'more' and 'last' transaction */ static void go_r_any_txn_post_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); - /* Move to the next byte */ - i2c_adapter->active_byte++; + /* Move to the next byte */ + i2c_adapter->active_byte++; - /* Move to the next transaction */ - i2c_adapter->active_txn++; + /* Move to the next transaction */ + i2c_adapter->active_txn++; } /* Common to 'more' and 'last' transaction */ static void go_w_any_txn_addr(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_WRITE); + PIOS_DEBUG_Assert(i2c_adapter->active_txn->rw == PIOS_I2C_TXN_WRITE); - I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Transmitter); + I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Transmitter); } static void go_w_any_txn_middle(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte < i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte < i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); + I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); - /* Move to the next byte */ - i2c_adapter->active_byte++; - PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); } static void go_w_more_txn_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); + I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); - /* Move to the next byte */ - i2c_adapter->active_byte++; + /* Move to the next byte */ + i2c_adapter->active_byte++; - /* Move to the next transaction */ - i2c_adapter->active_txn++; - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + /* Move to the next transaction */ + i2c_adapter->active_txn++; + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); } static void go_w_last_txn_last(struct pios_i2c_adapter *i2c_adapter) { - PIOS_DEBUG_Assert(i2c_adapter->active_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); - PIOS_DEBUG_Assert(i2c_adapter->active_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); - PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_byte == i2c_adapter->last_byte); + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_BUF, DISABLE); - I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_BUF, DISABLE); + I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); // SHOULD MOVE THIS INTO A STOPPING STATE AND SET IT ONLY AFTER THE BYTE WAS SENT - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); - /* Move to the next byte */ - i2c_adapter->active_byte++; + /* Move to the next byte */ + i2c_adapter->active_byte++; } -static void go_nack(struct pios_i2c_adapter *i2c_adapter) +static void go_nack(struct pios_i2c_adapter *i2c_adapter) { - i2c_adapter->nack = true; - I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + i2c_adapter->nack = true; + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); } static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum i2c_adapter_event event) { - PIOS_IRQ_Disable(); + PIOS_IRQ_Disable(); -#if defined(PIOS_I2C_DIAGNOSTICS) - i2c_state_event_history[i2c_state_event_history_pointer] = event; - i2c_state_event_history_pointer = (i2c_state_event_history_pointer + 1) % I2C_LOG_DEPTH; +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_state_event_history[i2c_state_event_history_pointer] = event; + i2c_state_event_history_pointer = (i2c_state_event_history_pointer + 1) % I2C_LOG_DEPTH; - i2c_state_history[i2c_state_history_pointer] = i2c_adapter->curr_state; - i2c_state_history_pointer = (i2c_state_history_pointer + 1) % I2C_LOG_DEPTH; - - if(i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event] == I2C_STATE_FSM_FAULT) - i2c_adapter_log_fault(PIOS_I2C_ERROR_FSM); -#endif - /* - * Move to the next state - * - * This is done prior to calling the new state's entry function to - * guarantee that the entry function never depends on the previous - * state. This way, it cannot ever know what the previous state was. - */ - i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event]; + i2c_state_history[i2c_state_history_pointer] = i2c_adapter->curr_state; + i2c_state_history_pointer = (i2c_state_history_pointer + 1) % I2C_LOG_DEPTH; - /* Call the entry function (if any) for the next state. */ - if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { - i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); - } + if (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event] == I2C_STATE_FSM_FAULT) { + i2c_adapter_log_fault(PIOS_I2C_ERROR_FSM); + } +#endif + /* + * Move to the next state + * + * This is done prior to calling the new state's entry function to + * guarantee that the entry function never depends on the previous + * state. This way, it cannot ever know what the previous state was. + */ + i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event]; - /* Process any AUTO transitions in the FSM */ - i2c_adapter_process_auto(i2c_adapter); + /* Call the entry function (if any) for the next state. */ + if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { + i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); + } - PIOS_IRQ_Enable(); + /* Process any AUTO transitions in the FSM */ + i2c_adapter_process_auto(i2c_adapter); + + PIOS_IRQ_Enable(); } static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter) { - PIOS_IRQ_Disable(); + PIOS_IRQ_Disable(); - while (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]) { - i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]; + while (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]) { + i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]; - /* Call the entry function (if any) for the next state. */ - if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { - i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); - } - } + /* Call the entry function (if any) for the next state. */ + if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { + i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter); + } + } - PIOS_IRQ_Enable(); + PIOS_IRQ_Enable(); } static void i2c_adapter_fsm_init(struct pios_i2c_adapter *i2c_adapter) { - i2c_adapter_reset_bus(i2c_adapter); - i2c_adapter->curr_state = I2C_STATE_STOPPED; + i2c_adapter_reset_bus(i2c_adapter); + i2c_adapter->curr_state = I2C_STATE_STOPPED; } static bool i2c_adapter_wait_for_stopped(struct pios_i2c_adapter *i2c_adapter) { - uint32_t guard; + uint32_t guard; - /* - * Wait for the bus to return to the stopped state. - * This was pulled out of the FSM due to occasional - * failures at this transition which previously resulted - * in spinning on this bit in the ISR forever. - */ - for (guard = 1000000; /* FIXME: should use the configured bus timeout */ - guard && (i2c_adapter->cfg->regs->CR1 & I2C_CR1_STOP); guard--) - continue; - if (!guard) { - /* We timed out waiting for the stop condition */ - return false; - } + /* + * Wait for the bus to return to the stopped state. + * This was pulled out of the FSM due to occasional + * failures at this transition which previously resulted + * in spinning on this bit in the ISR forever. + */ + for (guard = 1000000; /* FIXME: should use the configured bus timeout */ + guard && (i2c_adapter->cfg->regs->CR1 & I2C_CR1_STOP); guard--) { + continue; + } + if (!guard) { + /* We timed out waiting for the stop condition */ + return false; + } - return true; + return true; } static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) { - /* Reset the I2C block */ - I2C_DeInit(i2c_adapter->cfg->regs); + /* Reset the I2C block */ + I2C_DeInit(i2c_adapter->cfg->regs); - /* Make sure the bus is free by clocking it until any slaves release the bus. */ - GPIO_InitTypeDef scl_gpio_init; - scl_gpio_init = i2c_adapter->cfg->scl.init; - scl_gpio_init.GPIO_Mode = GPIO_Mode_OUT; - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - GPIO_Init(i2c_adapter->cfg->scl.gpio, &scl_gpio_init); + /* Make sure the bus is free by clocking it until any slaves release the bus. */ + GPIO_InitTypeDef scl_gpio_init; + scl_gpio_init = i2c_adapter->cfg->scl.init; + scl_gpio_init.GPIO_Mode = GPIO_Mode_OUT; + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + GPIO_Init(i2c_adapter->cfg->scl.gpio, &scl_gpio_init); - GPIO_InitTypeDef sda_gpio_init; - sda_gpio_init = i2c_adapter->cfg->sda.init; - sda_gpio_init.GPIO_Mode = GPIO_Mode_OUT; - GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - GPIO_Init(i2c_adapter->cfg->sda.gpio, &sda_gpio_init); + GPIO_InitTypeDef sda_gpio_init; + sda_gpio_init = i2c_adapter->cfg->sda.init; + sda_gpio_init.GPIO_Mode = GPIO_Mode_OUT; + GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + GPIO_Init(i2c_adapter->cfg->sda.gpio, &sda_gpio_init); - /* Check SDA line to determine if slave is asserting bus and clock out if so, this may */ - /* have to be repeated (due to futher bus errors) but better than clocking 0xFF into an */ - /* ESC */ - //bool sda_hung = GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET; - while(GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET) { - - /* Set clock high and wait for any clock stretching to finish. */ - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) ; - PIOS_DELAY_WaituS(2); - - /* Set clock low */ - GPIO_ResetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - - /* Clock high again */ - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - } - - /* Generate a start then stop condition */ - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); - GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - PIOS_DELAY_WaituS(2); + /* Check SDA line to determine if slave is asserting bus and clock out if so, this may */ + /* have to be repeated (due to futher bus errors) but better than clocking 0xFF into an */ + /* ESC */ + // bool sda_hung = GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET; + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET) { + /* Set clock high and wait for any clock stretching to finish. */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) { + ; + } + PIOS_DELAY_WaituS(2); - /* Set data and clock high and wait for any clock stretching to finish. */ - GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); - GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) ; - /* Wait for data to be high */ - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) != Bit_SET) ; + /* Set clock low */ + GPIO_ResetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); - - /* Bus signals are guaranteed to be high (ie. free) after this point */ - /* Initialize the GPIO pins to the peripheral function */ - if (i2c_adapter->cfg->remap) { - GPIO_PinAFConfig(i2c_adapter->cfg->scl.gpio, - __builtin_ctz(i2c_adapter->cfg->scl.init.GPIO_Pin), - i2c_adapter->cfg->remap); - GPIO_PinAFConfig(i2c_adapter->cfg->sda.gpio, - __builtin_ctz(i2c_adapter->cfg->sda.init.GPIO_Pin), - i2c_adapter->cfg->remap); - } - GPIO_Init(i2c_adapter->cfg->scl.gpio, (GPIO_InitTypeDef*)&(i2c_adapter->cfg->scl.init)); // Struct is const, function signature not - GPIO_Init(i2c_adapter->cfg->sda.gpio, (GPIO_InitTypeDef*)&(i2c_adapter->cfg->sda.init)); + /* Clock high again */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + } - /* Reset the I2C block */ - I2C_DeInit(i2c_adapter->cfg->regs); + /* Generate a start then stop condition */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); - /* Initialize the I2C block */ - I2C_Init(i2c_adapter->cfg->regs, (I2C_InitTypeDef*)&(i2c_adapter->cfg->init)); + /* Set data and clock high and wait for any clock stretching to finish. */ + GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) { + ; + } + /* Wait for data to be high */ + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) != Bit_SET) { + ; + } - if (i2c_adapter->cfg->regs->SR2 & I2C_FLAG_BUSY) { - /* Reset the I2C block */ - I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, ENABLE); - I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, DISABLE); - } + + /* Bus signals are guaranteed to be high (ie. free) after this point */ + /* Initialize the GPIO pins to the peripheral function */ + if (i2c_adapter->cfg->remap) { + GPIO_PinAFConfig(i2c_adapter->cfg->scl.gpio, + __builtin_ctz(i2c_adapter->cfg->scl.init.GPIO_Pin), + i2c_adapter->cfg->remap); + GPIO_PinAFConfig(i2c_adapter->cfg->sda.gpio, + __builtin_ctz(i2c_adapter->cfg->sda.init.GPIO_Pin), + i2c_adapter->cfg->remap); + } + GPIO_Init(i2c_adapter->cfg->scl.gpio, (GPIO_InitTypeDef *)&(i2c_adapter->cfg->scl.init)); // Struct is const, function signature not + GPIO_Init(i2c_adapter->cfg->sda.gpio, (GPIO_InitTypeDef *)&(i2c_adapter->cfg->sda.init)); + + /* Reset the I2C block */ + I2C_DeInit(i2c_adapter->cfg->regs); + + /* Initialize the I2C block */ + I2C_Init(i2c_adapter->cfg->regs, (I2C_InitTypeDef *)&(i2c_adapter->cfg->init)); + + if (i2c_adapter->cfg->regs->SR2 & I2C_FLAG_BUSY) { + /* Reset the I2C block */ + I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, ENABLE); + I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, DISABLE); + } } #include @@ -758,54 +765,59 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) /* Return true if the FSM is in a terminal state */ static bool i2c_adapter_fsm_terminated(struct pios_i2c_adapter *i2c_adapter) { - switch (i2c_adapter->curr_state) { - case I2C_STATE_STOPPING: - case I2C_STATE_STOPPED: - return (true); - default: - return (false); - } + switch (i2c_adapter->curr_state) { + case I2C_STATE_STOPPING: + case I2C_STATE_STOPPED: + return true; + + default: + return false; + } } uint32_t i2c_cb_count = 0; -static bool i2c_adapter_callback_handler(struct pios_i2c_adapter * i2c_adapter) +static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter) { - bool semaphore_success = true; - /* Wait for the transfer to complete */ + bool semaphore_success = true; + + /* Wait for the transfer to complete */ #ifdef USE_FREERTOS - portTickType timeout; - timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; - semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); - xSemaphoreGive(i2c_adapter->sem_ready); + portTickType timeout; + timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; + semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); + xSemaphoreGive(i2c_adapter->sem_ready); #endif /* USE_FREERTOS */ - - /* Spin waiting for the transfer to finish */ - while (!i2c_adapter_fsm_terminated(i2c_adapter)) ; - - if (i2c_adapter_wait_for_stopped(i2c_adapter)) { - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOPPED); - } else { - i2c_adapter_fsm_init(i2c_adapter); - } - - // Execute user supplied function - i2c_adapter->callback(); - - i2c_cb_count++; + + /* Spin waiting for the transfer to finish */ + while (!i2c_adapter_fsm_terminated(i2c_adapter)) { + ; + } + + if (i2c_adapter_wait_for_stopped(i2c_adapter)) { + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOPPED); + } else { + i2c_adapter_fsm_init(i2c_adapter); + } + + // Execute user supplied function + i2c_adapter->callback(); + + i2c_cb_count++; #ifdef USE_FREERTOS - /* Unlock the bus */ - xSemaphoreGive(i2c_adapter->sem_busy); - if(!semaphore_success) - i2c_timeout_counter++; + /* Unlock the bus */ + xSemaphoreGive(i2c_adapter->sem_busy); + if (!semaphore_success) { + i2c_timeout_counter++; + } #else - PIOS_IRQ_Disable(); - i2c_adapter->busy = 0; - PIOS_IRQ_Enable(); + PIOS_IRQ_Disable(); + i2c_adapter->busy = 0; + PIOS_IRQ_Enable(); #endif /* USE_FREERTOS */ - - return (!i2c_adapter->bus_error) && semaphore_success; + + return (!i2c_adapter->bus_error) && semaphore_success; } /** @@ -816,29 +828,29 @@ static bool i2c_adapter_callback_handler(struct pios_i2c_adapter * i2c_adapter) void i2c_adapter_log_fault(enum pios_i2c_error_type type) { #if defined(PIOS_I2C_DIAGNOSTICS) - i2c_adapter_fault_history.type = type; - for(uint8_t i = 0; i < I2C_LOG_DEPTH; i++) { - i2c_adapter_fault_history.evirq[i] = - i2c_evirq_history[(I2C_LOG_DEPTH + i2c_evirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - i2c_adapter_fault_history.erirq[i] = - i2c_erirq_history[(I2C_LOG_DEPTH + i2c_erirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - i2c_adapter_fault_history.event[i] = - i2c_state_event_history[(I2C_LOG_DEPTH + i2c_state_event_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - i2c_adapter_fault_history.state[i] = - i2c_state_history[(I2C_LOG_DEPTH + i2c_state_history_pointer - 1 - i) % I2C_LOG_DEPTH]; - } - switch(type) { - case PIOS_I2C_ERROR_EVENT: - i2c_bad_event_counter++; - break; - case PIOS_I2C_ERROR_FSM: - i2c_fsm_fault_count++; - break; - case PIOS_I2C_ERROR_INTERRUPT: - i2c_error_interrupt_counter++; - break; - } -#endif + i2c_adapter_fault_history.type = type; + for (uint8_t i = 0; i < I2C_LOG_DEPTH; i++) { + i2c_adapter_fault_history.evirq[i] = + i2c_evirq_history[(I2C_LOG_DEPTH + i2c_evirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.erirq[i] = + i2c_erirq_history[(I2C_LOG_DEPTH + i2c_erirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.event[i] = + i2c_state_event_history[(I2C_LOG_DEPTH + i2c_state_event_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.state[i] = + i2c_state_history[(I2C_LOG_DEPTH + i2c_state_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + } + switch (type) { + case PIOS_I2C_ERROR_EVENT: + i2c_bad_event_counter++; + break; + case PIOS_I2C_ERROR_FSM: + i2c_fsm_fault_count++; + break; + case PIOS_I2C_ERROR_INTERRUPT: + i2c_error_interrupt_counter++; + break; + } +#endif /* if defined(PIOS_I2C_DIAGNOSTICS) */ } @@ -846,388 +858,404 @@ void i2c_adapter_log_fault(enum pios_i2c_error_type type) * Logs the last N state transitions and N IRQ events due to * an error condition * \param[out] data address where to copy the pios_i2c_fault_history structure to - * \param[out] counts three uint16 that receive the bad event, fsm, and error irq + * \param[out] counts three uint16 that receive the bad event, fsm, and error irq * counts */ -void PIOS_I2C_GetDiagnostics(struct pios_i2c_fault_history * data, uint8_t * counts) +void PIOS_I2C_GetDiagnostics(struct pios_i2c_fault_history *data, uint8_t *counts) { #if defined(PIOS_I2C_DIAGNOSTICS) - memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); - counts[0] = i2c_bad_event_counter; - counts[1] = i2c_fsm_fault_count; - counts[2] = i2c_error_interrupt_counter; - counts[3] = i2c_nack_counter; - counts[4] = i2c_timeout_counter; + memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); + counts[0] = i2c_bad_event_counter; + counts[1] = i2c_fsm_fault_count; + counts[2] = i2c_error_interrupt_counter; + counts[3] = i2c_nack_counter; + counts[4] = i2c_timeout_counter; #else - struct pios_i2c_fault_history i2c_adapter_fault_history; - i2c_adapter_fault_history.type = PIOS_I2C_ERROR_EVENT; + struct pios_i2c_fault_history i2c_adapter_fault_history; + i2c_adapter_fault_history.type = PIOS_I2C_ERROR_EVENT; - memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); - counts[0] = counts[1] = counts[2] = 0; + memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); + counts[0] = counts[1] = counts[2] = 0; #endif } -static bool PIOS_I2C_validate(struct pios_i2c_adapter * i2c_adapter) +static bool PIOS_I2C_validate(struct pios_i2c_adapter *i2c_adapter) { - return (i2c_adapter->magic == PIOS_I2C_DEV_MAGIC); + return i2c_adapter->magic == PIOS_I2C_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) && 0 -static struct pios_i2c_dev * PIOS_I2C_alloc(void) +static struct pios_i2c_dev *PIOS_I2C_alloc(void) { - struct pios_i2c_dev * i2c_adapter; + struct pios_i2c_dev *i2c_adapter; - i2c_adapter = (struct pios_i2c_adapter *)malloc(sizeof(*i2c_adapter)); - if (!i2c_adapter) return(NULL); + i2c_adapter = (struct pios_i2c_adapter *)malloc(sizeof(*i2c_adapter)); + if (!i2c_adapter) { + return NULL; + } - i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; - return(i2c_adapter); + i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; + return i2c_adapter; } #else static struct pios_i2c_adapter pios_i2c_adapters[PIOS_I2C_MAX_DEVS]; static uint8_t pios_i2c_num_adapters; -static struct pios_i2c_adapter * PIOS_I2C_alloc(void) +static struct pios_i2c_adapter *PIOS_I2C_alloc(void) { - struct pios_i2c_adapter * i2c_adapter; + struct pios_i2c_adapter *i2c_adapter; - if (pios_i2c_num_adapters >= PIOS_I2C_MAX_DEVS) { - return (NULL); - } + if (pios_i2c_num_adapters >= PIOS_I2C_MAX_DEVS) { + return NULL; + } - i2c_adapter = &pios_i2c_adapters[pios_i2c_num_adapters++]; - i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; + i2c_adapter = &pios_i2c_adapters[pios_i2c_num_adapters++]; + i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; - return (i2c_adapter); + return i2c_adapter; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) && 0 */ /** -* Initializes IIC driver -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ -int32_t PIOS_I2C_Init(uint32_t * i2c_id, const struct pios_i2c_adapter_cfg * cfg) + * Initializes IIC driver + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ +int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg) { - PIOS_DEBUG_Assert(i2c_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(i2c_id); + PIOS_DEBUG_Assert(cfg); - struct pios_i2c_adapter * i2c_adapter; + struct pios_i2c_adapter *i2c_adapter; - i2c_adapter = (struct pios_i2c_adapter *) PIOS_I2C_alloc(); - if (!i2c_adapter) goto out_fail; + i2c_adapter = (struct pios_i2c_adapter *)PIOS_I2C_alloc(); + if (!i2c_adapter) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - i2c_adapter->cfg = cfg; + /* Bind the configuration to the device instance */ + i2c_adapter->cfg = cfg; #ifdef USE_FREERTOS - /* - * Must be done prior to calling i2c_adapter_fsm_init() - * since the sem_ready mutex is used in the initial state. - */ - vSemaphoreCreateBinary(i2c_adapter->sem_ready); - i2c_adapter->sem_busy = xSemaphoreCreateMutex(); + /* + * Must be done prior to calling i2c_adapter_fsm_init() + * since the sem_ready mutex is used in the initial state. + */ + vSemaphoreCreateBinary(i2c_adapter->sem_ready); + i2c_adapter->sem_busy = xSemaphoreCreateMutex(); #else - i2c_adapter->busy = 0; + i2c_adapter->busy = 0; #endif // USE_FREERTOS - /* Initialize the state machine */ - i2c_adapter_fsm_init(i2c_adapter); + /* Initialize the state machine */ + i2c_adapter_fsm_init(i2c_adapter); - *i2c_id = (uint32_t)i2c_adapter; + *i2c_id = (uint32_t)i2c_adapter; - /* Configure and enable I2C interrupts */ - NVIC_Init((NVIC_InitTypeDef*)&(i2c_adapter->cfg->event.init)); - NVIC_Init((NVIC_InitTypeDef*)&(i2c_adapter->cfg->error.init)); - - /* No error */ - return 0; + /* Configure and enable I2C interrupts */ + NVIC_Init((NVIC_InitTypeDef *)&(i2c_adapter->cfg->event.init)); + NVIC_Init((NVIC_InitTypeDef *)&(i2c_adapter->cfg->error.init)); + + /* No error */ + return 0; out_fail: - return(-1); + return -1; } int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns) { - struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_Assert(valid) + bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_DEBUG_Assert(txn_list); - PIOS_DEBUG_Assert(num_txns); + PIOS_Assert(valid) - bool semaphore_success = true; + PIOS_DEBUG_Assert(txn_list); + PIOS_DEBUG_Assert(num_txns); + + bool semaphore_success = true; #ifdef USE_FREERTOS - /* Lock the bus */ - portTickType timeout; - timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; - if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) - return -2; -#else - PIOS_IRQ_Disable(); - if(i2c_adapter->busy) { - PIOS_IRQ_Enable(); - return -2; - } - i2c_adapter->busy = 1; - PIOS_IRQ_Enable(); + /* Lock the bus */ + portTickType timeout; + timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; + if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { + return -2; + } +#else + PIOS_IRQ_Disable(); + if (i2c_adapter->busy) { + PIOS_IRQ_Enable(); + return -2; + } + i2c_adapter->busy = 1; + PIOS_IRQ_Enable(); #endif /* USE_FREERTOS */ - PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); + PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); - i2c_adapter->first_txn = &txn_list[0]; - i2c_adapter->last_txn = &txn_list[num_txns - 1]; - i2c_adapter->active_txn = i2c_adapter->first_txn; + i2c_adapter->first_txn = &txn_list[0]; + i2c_adapter->last_txn = &txn_list[num_txns - 1]; + i2c_adapter->active_txn = i2c_adapter->first_txn; #ifdef USE_FREERTOS - /* Make sure the done/ready semaphore is consumed before we start */ - semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); + /* Make sure the done/ready semaphore is consumed before we start */ + semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); #endif - i2c_adapter->callback = NULL; - i2c_adapter->bus_error = false; - i2c_adapter->nack = false; - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); + i2c_adapter->callback = NULL; + i2c_adapter->bus_error = false; + i2c_adapter->nack = false; + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); - /* Wait for the transfer to complete */ + /* Wait for the transfer to complete */ #ifdef USE_FREERTOS - semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); - xSemaphoreGive(i2c_adapter->sem_ready); + semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); + xSemaphoreGive(i2c_adapter->sem_ready); #endif /* USE_FREERTOS */ - /* Spin waiting for the transfer to finish */ - while (!i2c_adapter_fsm_terminated(i2c_adapter)) ; + /* Spin waiting for the transfer to finish */ + while (!i2c_adapter_fsm_terminated(i2c_adapter)) { + ; + } - if (i2c_adapter_wait_for_stopped(i2c_adapter)) { - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOPPED); - } else { - i2c_adapter_fsm_init(i2c_adapter); - } + if (i2c_adapter_wait_for_stopped(i2c_adapter)) { + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOPPED); + } else { + i2c_adapter_fsm_init(i2c_adapter); + } #ifdef USE_FREERTOS - /* Unlock the bus */ - xSemaphoreGive(i2c_adapter->sem_busy); - if(!semaphore_success) - i2c_timeout_counter++; + /* Unlock the bus */ + xSemaphoreGive(i2c_adapter->sem_busy); + if (!semaphore_success) { + i2c_timeout_counter++; + } #else - PIOS_IRQ_Disable(); - i2c_adapter->busy = 0; - PIOS_IRQ_Enable(); + PIOS_IRQ_Disable(); + i2c_adapter->busy = 0; + PIOS_IRQ_Enable(); #endif /* USE_FREERTOS */ - return !semaphore_success ? -2 : - i2c_adapter->bus_error ? -1 : - i2c_adapter->nack ? -3 : - 0; + return !semaphore_success ? -2 : + i2c_adapter->bus_error ? -1 : + i2c_adapter->nack ? -3 : + 0; } int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns, void *callback) { - struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - - bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_Assert(valid) - PIOS_Assert(callback); - - PIOS_DEBUG_Assert(txn_list); - PIOS_DEBUG_Assert(num_txns); - - bool semaphore_success = true; - + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + + bool valid = PIOS_I2C_validate(i2c_adapter); + + PIOS_Assert(valid) + PIOS_Assert(callback); + + PIOS_DEBUG_Assert(txn_list); + PIOS_DEBUG_Assert(num_txns); + + bool semaphore_success = true; + #ifdef USE_FREERTOS - /* Lock the bus */ - portTickType timeout; - timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; - if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) - return -2; + /* Lock the bus */ + portTickType timeout; + timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; + if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { + return -2; + } #else - if(i2c_adapter->busy) { - PIOS_IRQ_Enable(); - return -2; - } + if (i2c_adapter->busy) { + PIOS_IRQ_Enable(); + return -2; + } #endif /* USE_FREERTOS */ - - PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); - - i2c_adapter->first_txn = &txn_list[0]; - i2c_adapter->last_txn = &txn_list[num_txns - 1]; - i2c_adapter->active_txn = i2c_adapter->first_txn; - + + PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); + + i2c_adapter->first_txn = &txn_list[0]; + i2c_adapter->last_txn = &txn_list[num_txns - 1]; + i2c_adapter->active_txn = i2c_adapter->first_txn; + #ifdef USE_FREERTOS - /* Make sure the done/ready semaphore is consumed before we start */ - semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); + /* Make sure the done/ready semaphore is consumed before we start */ + semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); #endif - - i2c_adapter->callback = callback; - i2c_adapter->bus_error = false; - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); - - return !semaphore_success ? -2 : 0; + + i2c_adapter->callback = callback; + i2c_adapter->bus_error = false; + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); + + return !semaphore_success ? -2 : 0; } void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id) { - struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_Assert(valid) + bool valid = PIOS_I2C_validate(i2c_adapter); - uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + PIOS_Assert(valid) -#if defined(PIOS_I2C_DIAGNOSTICS) - /* Store event for diagnostics */ - i2c_evirq_history[i2c_evirq_history_pointer] = event; - i2c_evirq_history_pointer = (i2c_evirq_history_pointer + 1) % I2C_LOG_DEPTH; + uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + +#if defined(PIOS_I2C_DIAGNOSTICS) + /* Store event for diagnostics */ + i2c_evirq_history[i2c_evirq_history_pointer] = event; + i2c_evirq_history_pointer = (i2c_evirq_history_pointer + 1) % I2C_LOG_DEPTH; #endif - + #define EVENT_MASK 0x000700FF - event &= EVENT_MASK; - - // This is very poor and inconsistent practice with the FSM since no other - // throw event depends on the current state. However when accelerated (-Os) - // we definitely catch this event twice and there is no clean way to do deal - // with that in the FMS short of a special state for it - if(i2c_adapter->curr_state == I2C_STATE_STARTING && event == 0x70084) - return; + event &= EVENT_MASK; - switch (event) { /* Mask out all the bits we don't care about */ - case (I2C_EVENT_MASTER_MODE_SELECT | 0x40): - /* Unexplained event: EV5 + RxNE : Extraneous Rx. Probably a late NACK from previous read. */ - /* Clean up the extra Rx until the root cause is identified and just keep going */ - (void)I2C_ReceiveData(i2c_adapter->cfg->regs); - /* Fall through */ - case I2C_EVENT_MASTER_MODE_SELECT: /* EV5 */ - switch (i2c_adapter->active_txn->rw) { - case PIOS_I2C_TXN_READ: - if (i2c_adapter->active_txn == i2c_adapter->last_txn) { - /* Final transaction */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_READ); - } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { - /* More transactions follow */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_READ); - } else { - PIOS_DEBUG_Assert(0); - } - break; - case PIOS_I2C_TXN_WRITE: - if (i2c_adapter->active_txn == i2c_adapter->last_txn) { - /* Final transaction */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_WRITE); - } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { - /* More transactions follow */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_WRITE); - } else { - PIOS_DEBUG_Assert(0); - } - break; - default: - PIOS_DEBUG_Assert(0); - break; - } - break; - case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED: /* EV6 */ - case I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED: /* EV6 */ - switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { - case 0: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_0); - break; - case 1: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_1); - break; - case 2: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_2); - break; - default: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_GT_2); - break; - } - break; - case 0x80: /* TxE only. TRA + MSL + BUSY have been cleared before we got here. */ - /* Ignore */ - { - static volatile bool halt = false; - while (halt) ; - } - break; - case 0: /* This triggers an FSM fault sometimes, but not having it stops things working */ - case 0x40: /* RxNE only. MSL + BUSY have already been cleared by HW. */ - case 0x44: /* RxNE + BTF. MSL + BUSY have already been cleared by HW. */ - case I2C_EVENT_MASTER_BYTE_RECEIVED: /* EV7 */ - case (I2C_EVENT_MASTER_BYTE_RECEIVED | 0x4): /* EV7 + BTF */ - case I2C_EVENT_MASTER_BYTE_TRANSMITTED: /* EV8_2 */ - case 0x84: /* TxE + BTF. EV8_2 but TRA + MSL + BUSY have already been cleared by HW. */ - switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { - case 0: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_0); - break; - case 1: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_1); - break; - case 2: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_2); - break; - default: - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_GT_2); - break; - } - break; - case I2C_EVENT_MASTER_BYTE_TRANSMITTING: /* EV8 */ - /* Ignore this event and wait for TRANSMITTED in case we can't keep up */ - goto skip_event; - break; - case 0x30084: /* Occurs between byte tranmistted and master mode selected */ - case 0x30000: /* Need to throw away this spurious event */ - case 0x30403 & EVENT_MASK: /* Detected this after got a NACK, probably stop bit */ - goto skip_event; - break; - default: - i2c_adapter_log_fault(PIOS_I2C_ERROR_EVENT); + // This is very poor and inconsistent practice with the FSM since no other + // throw event depends on the current state. However when accelerated (-Os) + // we definitely catch this event twice and there is no clean way to do deal + // with that in the FMS short of a special state for it + if (i2c_adapter->curr_state == I2C_STATE_STARTING && event == 0x70084) { + return; + } + + switch (event) { /* Mask out all the bits we don't care about */ + case (I2C_EVENT_MASTER_MODE_SELECT | 0x40): + /* Unexplained event: EV5 + RxNE : Extraneous Rx. Probably a late NACK from previous read. */ + /* Clean up the extra Rx until the root cause is identified and just keep going */ + (void)I2C_ReceiveData(i2c_adapter->cfg->regs); + /* Fall through */ + case I2C_EVENT_MASTER_MODE_SELECT: /* EV5 */ + switch (i2c_adapter->active_txn->rw) { + case PIOS_I2C_TXN_READ: + if (i2c_adapter->active_txn == i2c_adapter->last_txn) { + /* Final transaction */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_READ); + } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { + /* More transactions follow */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_READ); + } else { + PIOS_DEBUG_Assert(0); + } + break; + case PIOS_I2C_TXN_WRITE: + if (i2c_adapter->active_txn == i2c_adapter->last_txn) { + /* Final transaction */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_LAST_TXN_WRITE); + } else if (i2c_adapter->active_txn < i2c_adapter->last_txn) { + /* More transactions follow */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STARTED_MORE_TXN_WRITE); + } else { + PIOS_DEBUG_Assert(0); + } + break; + default: + PIOS_DEBUG_Assert(0); + break; + } + break; + case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED: /* EV6 */ + case I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED: /* EV6 */ + switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { + case 0: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_0); + break; + case 1: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_1); + break; + case 2: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_EQ_2); + break; + default: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_ADDR_SENT_LEN_GT_2); + break; + } + break; + case 0x80: /* TxE only. TRA + MSL + BUSY have been cleared before we got here. */ + /* Ignore */ + { + static volatile bool halt = false; + while (halt) { + ; + } + } + break; + case 0: /* This triggers an FSM fault sometimes, but not having it stops things working */ + case 0x40: /* RxNE only. MSL + BUSY have already been cleared by HW. */ + case 0x44: /* RxNE + BTF. MSL + BUSY have already been cleared by HW. */ + case I2C_EVENT_MASTER_BYTE_RECEIVED: /* EV7 */ + case (I2C_EVENT_MASTER_BYTE_RECEIVED | 0x4): /* EV7 + BTF */ + case I2C_EVENT_MASTER_BYTE_TRANSMITTED: /* EV8_2 */ + case 0x84: /* TxE + BTF. EV8_2 but TRA + MSL + BUSY have already been cleared by HW. */ + switch (i2c_adapter->last_byte - i2c_adapter->active_byte + 1) { + case 0: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_0); + break; + case 1: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_1); + break; + case 2: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_EQ_2); + break; + default: + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_DONE_LEN_GT_2); + break; + } + break; + case I2C_EVENT_MASTER_BYTE_TRANSMITTING: /* EV8 */ + /* Ignore this event and wait for TRANSMITTED in case we can't keep up */ + goto skip_event; + break; + case 0x30084: /* Occurs between byte tranmistted and master mode selected */ + case 0x30000: /* Need to throw away this spurious event */ + case 0x30403 & EVENT_MASK: /* Detected this after got a NACK, probably stop bit */ + goto skip_event; + break; + default: + i2c_adapter_log_fault(PIOS_I2C_ERROR_EVENT); #if defined(I2C_HALT_ON_ERRORS) - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); #endif - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); - break; - } + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); + break; + } skip_event: - ; + ; } void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id) { - struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - bool valid = PIOS_I2C_validate(i2c_adapter); - PIOS_Assert(valid) + bool valid = PIOS_I2C_validate(i2c_adapter); + + PIOS_Assert(valid) #if defined(PIOS_I2C_DIAGNOSTICS) - uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); + + i2c_erirq_history[i2c_erirq_history_pointer] = event; + i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % 5; - i2c_erirq_history[i2c_erirq_history_pointer] = event; - i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % 5; - #endif - if(event & I2C_FLAG_AF) { - i2c_nack_counter++; + if (event & I2C_FLAG_AF) { + i2c_nack_counter++; - I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_AF); + I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_AF); - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_NACK); - } else { /* Mostly bus errors here */ - i2c_adapter_log_fault(PIOS_I2C_ERROR_INTERRUPT); - - /* Fail hard on any errors for now */ - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); - } + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_NACK); + } else { /* Mostly bus errors here */ + i2c_adapter_log_fault(PIOS_I2C_ERROR_INTERRUPT); + + /* Fail hard on any errors for now */ + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR); + } } #endif /* PIOS_INCLUDE_I2C */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_irq.c b/flight/pios/stm32f4xx/pios_irq.c index d14120829..4c6e41008 100644 --- a/flight/pios/stm32f4xx/pios_irq.c +++ b/flight/pios/stm32f4xx/pios_irq.c @@ -6,25 +6,25 @@ * @brief STM32 Hardware code to enable and disable interrupts * @{ * - * @file pios_irq.c + * @file pios_irq.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012 * @brief IRQ Enable/Disable routines * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -46,55 +46,55 @@ static uint32_t nested_ctr; static uint32_t prev_primask; /** -* Disables all interrupts (nested) -* \return < 0 On errors -*/ + * Disables all interrupts (nested) + * \return < 0 On errors + */ int32_t PIOS_IRQ_Disable(void) { - /* Get current priority if nested level == 0 */ - if (!nested_ctr) { - __asm volatile (" mrs %0, primask\n":"=r" (prev_primask) - ); - } + /* Get current priority if nested level == 0 */ + if (!nested_ctr) { + __asm volatile (" mrs %0, primask\n" : "=r" (prev_primask) + ); + } - /* Disable interrupts */ - __asm volatile (" mov r0, #1 \n" " msr primask, r0\n":::"r0"); + /* Disable interrupts */ + __asm volatile (" mov r0, #1 \n" " msr primask, r0\n" ::: "r0"); - ++nested_ctr; + ++nested_ctr; - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Enables all interrupts (nested) -* \return < 0 on errors -* \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) -*/ + * Enables all interrupts (nested) + * \return < 0 on errors + * \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) + */ int32_t PIOS_IRQ_Enable(void) { - /* Check for nesting error */ - if (nested_ctr == 0) { - /* Nesting error */ - return -1; - } + /* Check for nesting error */ + if (nested_ctr == 0) { + /* Nesting error */ + return -1; + } - /* Decrease nesting level */ - --nested_ctr; + /* Decrease nesting level */ + --nested_ctr; - /* Set back previous priority once nested level reached 0 again */ - if (nested_ctr == 0) { - __asm volatile (" msr primask, %0\n"::"r" (prev_primask) - ); - } + /* Set back previous priority once nested level reached 0 again */ + if (nested_ctr == 0) { + __asm volatile (" msr primask, %0\n" ::"r" (prev_primask) + ); + } - /* No error */ - return 0; + /* No error */ + return 0; } #endif /* PIOS_INCLUDE_IRQ */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_led.c b/flight/pios/stm32f4xx/pios_led.c index b0d6a4f88..45174c645 100644 --- a/flight/pios/stm32f4xx/pios_led.c +++ b/flight/pios/stm32f4xx/pios_led.c @@ -6,25 +6,25 @@ * @brief STM32 Hardware LED handling code * @{ * - * @file pios_led.c + * @file pios_led.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief LED functions, init, toggle, on & off. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,65 +34,65 @@ #include -static const struct pios_led_cfg * led_cfg; +static const struct pios_led_cfg *led_cfg; /** * Initialises all the LED's */ -int32_t PIOS_LED_Init(const struct pios_led_cfg * cfg) +int32_t PIOS_LED_Init(const struct pios_led_cfg *cfg) { - PIOS_Assert(cfg); - - /* Store away the config in a global used by API functions */ - led_cfg = cfg; - - for (uint8_t i = 0; i < cfg->num_leds; i++) { - const struct pios_led * led = &(cfg->leds[i]); - - /* Enable the peripheral clock for the GPIO */ - switch ((uint32_t)led->pin.gpio) { - case (uint32_t) GPIOA: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); - break; - case (uint32_t) GPIOB: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); - break; - case (uint32_t) GPIOC: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); - break; - case (uint32_t) GPIOD: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE); - break; - case (uint32_t) GPIOE: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE); - break; - case (uint32_t) GPIOF: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOF, ENABLE); - break; - case (uint32_t) GPIOG: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOG, ENABLE); - break; - case (uint32_t) GPIOH: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOH, ENABLE); - break; - case (uint32_t) GPIOI: - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOI, ENABLE); - break; - default: - PIOS_Assert(0); - break; - } - - if (led->remap) { - GPIO_PinAFConfig(led->pin.gpio, led->pin.init.GPIO_Pin, led->remap); - } - - GPIO_Init(led->pin.gpio, &led->pin.init); - - PIOS_LED_Off(i); - } - - return 0; + PIOS_Assert(cfg); + + /* Store away the config in a global used by API functions */ + led_cfg = cfg; + + for (uint8_t i = 0; i < cfg->num_leds; i++) { + const struct pios_led *led = &(cfg->leds[i]); + + /* Enable the peripheral clock for the GPIO */ + switch ((uint32_t)led->pin.gpio) { + case (uint32_t)GPIOA: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); + break; + case (uint32_t)GPIOB: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); + break; + case (uint32_t)GPIOC: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); + break; + case (uint32_t)GPIOD: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE); + break; + case (uint32_t)GPIOE: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE); + break; + case (uint32_t)GPIOF: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOF, ENABLE); + break; + case (uint32_t)GPIOG: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOG, ENABLE); + break; + case (uint32_t)GPIOH: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOH, ENABLE); + break; + case (uint32_t)GPIOI: + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOI, ENABLE); + break; + default: + PIOS_Assert(0); + break; + } + + if (led->remap) { + GPIO_PinAFConfig(led->pin.gpio, led->pin.init.GPIO_Pin, led->remap); + } + + GPIO_Init(led->pin.gpio, &led->pin.init); + + PIOS_LED_Off(i); + } + + return 0; } /** @@ -101,19 +101,20 @@ int32_t PIOS_LED_Init(const struct pios_led_cfg * cfg) */ void PIOS_LED_On(uint32_t led_id) { - PIOS_Assert(led_cfg); - - if (led_id >= led_cfg->num_leds) { - /* LED index out of range */ - return; - } - - const struct pios_led * led = &(led_cfg->leds[led_id]); - - if (led->active_high) - GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); - else - GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + PIOS_Assert(led_cfg); + + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } + + const struct pios_led *led = &(led_cfg->leds[led_id]); + + if (led->active_high) { + GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } else { + GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } } /** @@ -122,19 +123,20 @@ void PIOS_LED_On(uint32_t led_id) */ void PIOS_LED_Off(uint32_t led_id) { - PIOS_Assert(led_cfg); - - if (led_id >= led_cfg->num_leds) { - /* LED index out of range */ - return; - } - - const struct pios_led * led = &(led_cfg->leds[led_id]); - - if (led->active_high) - GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); - else - GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + PIOS_Assert(led_cfg); + + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } + + const struct pios_led *led = &(led_cfg->leds[led_id]); + + if (led->active_high) { + GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } else { + GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); + } } /** @@ -143,26 +145,28 @@ void PIOS_LED_Off(uint32_t led_id) */ void PIOS_LED_Toggle(uint32_t led_id) { - PIOS_Assert(led_cfg); - - if (led_id >= led_cfg->num_leds) { - /* LED index out of range */ - return; - } - - const struct pios_led * led = &(led_cfg->leds[led_id]); - - if (GPIO_ReadOutputDataBit(led->pin.gpio, led->pin.init.GPIO_Pin) == Bit_SET) { - if (led->active_high) - PIOS_LED_Off(led_id); - else - PIOS_LED_On(led_id); - } else { - if (led->active_high) - PIOS_LED_On(led_id); - else - PIOS_LED_Off(led_id); - } + PIOS_Assert(led_cfg); + + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } + + const struct pios_led *led = &(led_cfg->leds[led_id]); + + if (GPIO_ReadOutputDataBit(led->pin.gpio, led->pin.init.GPIO_Pin) == Bit_SET) { + if (led->active_high) { + PIOS_LED_Off(led_id); + } else { + PIOS_LED_On(led_id); + } + } else { + if (led->active_high) { + PIOS_LED_On(led_id); + } else { + PIOS_LED_Off(led_id); + } + } } #endif /* PIOS_INCLUDE_LED */ diff --git a/flight/pios/stm32f4xx/pios_overo.c b/flight/pios/stm32f4xx/pios_overo.c index 609436295..ad8c73275 100644 --- a/flight/pios/stm32f4xx/pios_overo.c +++ b/flight/pios/stm32f4xx/pios_overo.c @@ -3,7 +3,7 @@ * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ * @addtogroup PIOS_OVERO OVERO Functions - * @brief PIOS interface to read and write to overo + * @brief PIOS interface to read and write to overo * @{ * * @file pios_overo.c @@ -53,61 +53,63 @@ static void PIOS_OVERO_TxStart(uint32_t overo_id, uint16_t tx_bytes_avail); static void PIOS_OVERO_RxStart(uint32_t overo_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_overo_com_driver = { - .set_baud = NULL, - .tx_start = PIOS_OVERO_TxStart, - .rx_start = PIOS_OVERO_RxStart, - .bind_tx_cb = PIOS_OVERO_RegisterTxCallback, - .bind_rx_cb = PIOS_OVERO_RegisterRxCallback, + .set_baud = NULL, + .tx_start = PIOS_OVERO_TxStart, + .rx_start = PIOS_OVERO_RxStart, + .bind_tx_cb = PIOS_OVERO_RegisterTxCallback, + .bind_rx_cb = PIOS_OVERO_RegisterRxCallback, }; -//! Data types +// ! Data types enum pios_overo_dev_magic { - PIOS_OVERO_DEV_MAGIC = 0x85A3834A, + PIOS_OVERO_DEV_MAGIC = 0x85A3834A, }; struct pios_overo_dev { - enum pios_overo_dev_magic magic; - const struct pios_overo_cfg * cfg; - - int8_t writing_buffer; - uint32_t writing_offset; - - uint32_t packets; - - uint8_t tx_buffer[2][PACKET_SIZE]; - uint8_t rx_buffer[2][PACKET_SIZE]; + enum pios_overo_dev_magic magic; + const struct pios_overo_cfg *cfg; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + int8_t writing_buffer; + uint32_t writing_offset; + + uint32_t packets; + + uint8_t tx_buffer[2][PACKET_SIZE]; + uint8_t rx_buffer[2][PACKET_SIZE]; + + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; }; #if defined(PIOS_INCLUDE_FREERTOS) -//! Private methods +// ! Private methods static void PIOS_OVERO_WriteData(struct pios_overo_dev *overo_dev); -static bool PIOS_OVERO_validate(struct pios_overo_dev * overo_dev); -static struct pios_overo_dev * PIOS_OVERO_alloc(void); +static bool PIOS_OVERO_validate(struct pios_overo_dev *overo_dev); +static struct pios_overo_dev *PIOS_OVERO_alloc(void); -static bool PIOS_OVERO_validate(struct pios_overo_dev * overo_dev) +static bool PIOS_OVERO_validate(struct pios_overo_dev *overo_dev) { - return (overo_dev->magic == PIOS_OVERO_DEV_MAGIC); + return overo_dev->magic == PIOS_OVERO_DEV_MAGIC; } -static struct pios_overo_dev * PIOS_OVERO_alloc(void) +static struct pios_overo_dev *PIOS_OVERO_alloc(void) { - struct pios_overo_dev * overo_dev; - - overo_dev = (struct pios_overo_dev *)pvPortMalloc(sizeof(*overo_dev)); - if (!overo_dev) return(NULL); - - overo_dev->rx_in_cb = 0; - overo_dev->rx_in_context = 0; - overo_dev->tx_out_cb = 0; - overo_dev->tx_out_context = 0; - overo_dev->packets = 0; - overo_dev->magic = PIOS_OVERO_DEV_MAGIC; - return(overo_dev); + struct pios_overo_dev *overo_dev; + + overo_dev = (struct pios_overo_dev *)pvPortMalloc(sizeof(*overo_dev)); + if (!overo_dev) { + return NULL; + } + + overo_dev->rx_in_cb = 0; + overo_dev->rx_in_context = 0; + overo_dev->tx_out_cb = 0; + overo_dev->tx_out_context = 0; + overo_dev->packets = 0; + overo_dev->magic = PIOS_OVERO_DEV_MAGIC; + return overo_dev; } /** @@ -116,27 +118,26 @@ static struct pios_overo_dev * PIOS_OVERO_alloc(void) */ static void PIOS_OVERO_WriteData(struct pios_overo_dev *overo_dev) { - // TODO: How do we protect against the DMA buffer swapping midway through adding data - // to this buffer. If we were writing at the beginning it could cause a weird race. - if (overo_dev->tx_out_cb) { + // TODO: How do we protect against the DMA buffer swapping midway through adding data + // to this buffer. If we were writing at the beginning it could cause a weird race. + if (overo_dev->tx_out_cb) { + int32_t max_bytes = PACKET_SIZE - overo_dev->writing_offset; - int32_t max_bytes = PACKET_SIZE - overo_dev->writing_offset; + if (max_bytes > 0) { + uint16_t bytes_added; + bool tx_need_yield = false; + uint8_t *writing_pointer = &overo_dev->tx_buffer[overo_dev->writing_buffer][overo_dev->writing_offset]; - if (max_bytes > 0) { - uint16_t bytes_added; - bool tx_need_yield = false; - uint8_t *writing_pointer = &overo_dev->tx_buffer[overo_dev->writing_buffer][overo_dev->writing_offset]; - - bytes_added = (overo_dev->tx_out_cb)(overo_dev->tx_out_context, writing_pointer, max_bytes, NULL, &tx_need_yield); + bytes_added = (overo_dev->tx_out_cb)(overo_dev->tx_out_context, writing_pointer, max_bytes, NULL, &tx_need_yield); #if defined(OVERO_USES_BLOCKING_WRITE) - if (tx_need_yield) { - vPortYieldFromISR(); - } + if (tx_need_yield) { + vPortYieldFromISR(); + } #endif - overo_dev->writing_offset += bytes_added; - } - } + overo_dev->writing_offset += bytes_added; + } + } } /** @@ -145,35 +146,37 @@ static void PIOS_OVERO_WriteData(struct pios_overo_dev *overo_dev) */ void PIOS_OVERO_DMA_irq_handler(uint32_t overo_id) { - struct pios_overo_dev * overo_dev = (struct pios_overo_dev *) overo_id; - if(!PIOS_OVERO_validate(overo_dev)) - return; - - DMA_ClearFlag(overo_dev->cfg->dma.tx.channel, overo_dev->cfg->dma.irq.flags); + struct pios_overo_dev *overo_dev = (struct pios_overo_dev *)overo_id; - overo_dev->writing_buffer = 1 - DMA_GetCurrentMemoryTarget(overo_dev->cfg->dma.tx.channel); - overo_dev->writing_offset = 0; + if (!PIOS_OVERO_validate(overo_dev)) { + return; + } + + DMA_ClearFlag(overo_dev->cfg->dma.tx.channel, overo_dev->cfg->dma.irq.flags); + + overo_dev->writing_buffer = 1 - DMA_GetCurrentMemoryTarget(overo_dev->cfg->dma.tx.channel); + overo_dev->writing_offset = 0; /* bool rx_need_yield; - // Get data from the Rx buffer and add to the fifo - (void) (overo_dev->rx_in_cb)(overo_dev->rx_in_context, - &overo_dev->rx_buffer[overo_dev->writing_buffer][0], - PACKET_SIZE, NULL, &rx_need_yield); + // Get data from the Rx buffer and add to the fifo + (void) (overo_dev->rx_in_cb)(overo_dev->rx_in_context, + &overo_dev->rx_buffer[overo_dev->writing_buffer][0], + PACKET_SIZE, NULL, &rx_need_yield); - if(rx_need_yield) { - vPortYieldFromISR(); - } + if(rx_need_yield) { + vPortYieldFromISR(); + } - // Fill the buffer with known value to prevent rereading these bytes - memset(&overo_dev->rx_buffer[overo_dev->writing_buffer][0], 0xFF, PACKET_SIZE); -*/ - // Fill the buffer with known value to prevent resending any bytes - memset(&overo_dev->tx_buffer[overo_dev->writing_buffer][0], 0xFF, PACKET_SIZE); - - // Load any pending bytes from TX fifo - PIOS_OVERO_WriteData(overo_dev); + // Fill the buffer with known value to prevent rereading these bytes + memset(&overo_dev->rx_buffer[overo_dev->writing_buffer][0], 0xFF, PACKET_SIZE); + */ + // Fill the buffer with known value to prevent resending any bytes + memset(&overo_dev->tx_buffer[overo_dev->writing_buffer][0], 0xFF, PACKET_SIZE); - overo_dev->packets++; + // Load any pending bytes from TX fifo + PIOS_OVERO_WriteData(overo_dev); + + overo_dev->packets++; } /** @@ -181,10 +184,11 @@ void PIOS_OVERO_DMA_irq_handler(uint32_t overo_id) */ int32_t PIOS_OVERO_GetPacketCount(uint32_t overo_id) { - struct pios_overo_dev * overo_dev = (struct pios_overo_dev *) overo_id; - PIOS_Assert(PIOS_OVERO_validate(overo_dev)); + struct pios_overo_dev *overo_dev = (struct pios_overo_dev *)overo_id; - return overo_dev->packets; + PIOS_Assert(PIOS_OVERO_validate(overo_dev)); + + return overo_dev->packets; } /** @@ -192,176 +196,183 @@ int32_t PIOS_OVERO_GetPacketCount(uint32_t overo_id) */ int32_t PIOS_OVERO_GetWrittenBytes(uint32_t overo_id) { - struct pios_overo_dev * overo_dev = (struct pios_overo_dev *) overo_id; - PIOS_Assert(PIOS_OVERO_validate(overo_dev)); - - return overo_dev->writing_offset; + struct pios_overo_dev *overo_dev = (struct pios_overo_dev *)overo_id; + + PIOS_Assert(PIOS_OVERO_validate(overo_dev)); + + return overo_dev->writing_offset; } /** * Initialise a single Overo device */ -int32_t PIOS_OVERO_Init(uint32_t * overo_id, const struct pios_overo_cfg * cfg) +int32_t PIOS_OVERO_Init(uint32_t *overo_id, const struct pios_overo_cfg *cfg) { - PIOS_DEBUG_Assert(overo_id); - PIOS_DEBUG_Assert(cfg); - - struct pios_overo_dev *overo_dev; - - overo_dev = (struct pios_overo_dev *) PIOS_OVERO_alloc(); - if (!overo_dev) goto out_fail; - - /* Bind the configuration to the device instance */ - overo_dev->cfg = cfg; - overo_dev->writing_buffer = 1; // First writes to second buffer + PIOS_DEBUG_Assert(overo_id); + PIOS_DEBUG_Assert(cfg); - /* Put buffers to a known state */ - memset(&overo_dev->tx_buffer[0][0], 0xFF, PACKET_SIZE); - memset(&overo_dev->tx_buffer[1][0], 0xFF, PACKET_SIZE); - memset(&overo_dev->rx_buffer[0][0], 0xFF, PACKET_SIZE); - memset(&overo_dev->rx_buffer[1][0], 0xFF, PACKET_SIZE); + struct pios_overo_dev *overo_dev; - /* - * Enable the SPI device - * - * 1. Enable the SPI port - * 2. Enable DMA with circular buffered DMA (validate config) - * 3. Enable the DMA Tx IRQ - */ - - //PIOS_Assert(overo_dev->cfg->dma.tx-> == CIRCULAR); - //PIOS_Assert(overo_dev->cfg->dma.rx-> == CIRCULAR); - - /* only legal for single-slave config */ - PIOS_Assert(overo_dev->cfg->slave_count == 1); - SPI_SSOutputCmd(overo_dev->cfg->regs, DISABLE); - - /* Initialize the GPIO pins */ - /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ - GPIO_PinAFConfig(overo_dev->cfg->sclk.gpio, - __builtin_ctz(overo_dev->cfg->sclk.init.GPIO_Pin), - overo_dev->cfg->remap); - GPIO_PinAFConfig(overo_dev->cfg->mosi.gpio, - __builtin_ctz(overo_dev->cfg->mosi.init.GPIO_Pin), - overo_dev->cfg->remap); - GPIO_PinAFConfig(overo_dev->cfg->miso.gpio, - __builtin_ctz(overo_dev->cfg->miso.init.GPIO_Pin), - overo_dev->cfg->remap); - GPIO_PinAFConfig(overo_dev->cfg->ssel[0].gpio, - __builtin_ctz(overo_dev->cfg->ssel[0].init.GPIO_Pin), - overo_dev->cfg->remap); - - GPIO_Init(overo_dev->cfg->sclk.gpio, (GPIO_InitTypeDef*)&(overo_dev->cfg->sclk.init)); - GPIO_Init(overo_dev->cfg->mosi.gpio, (GPIO_InitTypeDef*)&(overo_dev->cfg->mosi.init)); - GPIO_Init(overo_dev->cfg->miso.gpio, (GPIO_InitTypeDef*)&(overo_dev->cfg->miso.init)); - - /* Configure circular buffer targets. Configure 0 to be initially active */ - DMA_InitTypeDef dma_init; + overo_dev = (struct pios_overo_dev *)PIOS_OVERO_alloc(); + if (!overo_dev) { + goto out_fail; + } - DMA_DeInit(overo_dev->cfg->dma.rx.channel); - dma_init = overo_dev->cfg->dma.rx.init; - dma_init.DMA_Memory0BaseAddr = (uint32_t) overo_dev->rx_buffer[0]; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - dma_init.DMA_BufferSize = PACKET_SIZE; - DMA_Init(overo_dev->cfg->dma.rx.channel, &dma_init); - DMA_DoubleBufferModeConfig(overo_dev->cfg->dma.rx.channel, (uint32_t) overo_dev->rx_buffer[1], DMA_Memory_0); - DMA_DoubleBufferModeCmd(overo_dev->cfg->dma.rx.channel, ENABLE); - - DMA_DeInit(overo_dev->cfg->dma.tx.channel); - dma_init = overo_dev->cfg->dma.tx.init; - dma_init.DMA_Memory0BaseAddr = (uint32_t) overo_dev->tx_buffer[0]; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - dma_init.DMA_BufferSize = PACKET_SIZE; - DMA_Init(overo_dev->cfg->dma.tx.channel, &dma_init); - DMA_DoubleBufferModeConfig(overo_dev->cfg->dma.tx.channel, (uint32_t) overo_dev->tx_buffer[1], DMA_Memory_0); - DMA_DoubleBufferModeCmd(overo_dev->cfg->dma.tx.channel, ENABLE); + /* Bind the configuration to the device instance */ + overo_dev->cfg = cfg; + overo_dev->writing_buffer = 1; // First writes to second buffer - /* Set the packet size */ - DMA_SetCurrDataCounter(overo_dev->cfg->dma.rx.channel, PACKET_SIZE); - DMA_SetCurrDataCounter(overo_dev->cfg->dma.tx.channel, PACKET_SIZE); + /* Put buffers to a known state */ + memset(&overo_dev->tx_buffer[0][0], 0xFF, PACKET_SIZE); + memset(&overo_dev->tx_buffer[1][0], 0xFF, PACKET_SIZE); + memset(&overo_dev->rx_buffer[0][0], 0xFF, PACKET_SIZE); + memset(&overo_dev->rx_buffer[1][0], 0xFF, PACKET_SIZE); - /* Initialize the SPI block */ - SPI_DeInit(overo_dev->cfg->regs); - SPI_Init(overo_dev->cfg->regs, (SPI_InitTypeDef*)&(overo_dev->cfg->init)); - - SPI_CalculateCRC(overo_dev->cfg->regs, DISABLE); - - /* Enable SPI */ - SPI_Cmd(overo_dev->cfg->regs, ENABLE); - - /* Enable SPI interrupts to DMA */ - SPI_I2S_DMACmd(overo_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); + /* + * Enable the SPI device + * + * 1. Enable the SPI port + * 2. Enable DMA with circular buffered DMA (validate config) + * 3. Enable the DMA Tx IRQ + */ - /* Configure DMA interrupt */ - NVIC_Init((NVIC_InitTypeDef*)&(overo_dev->cfg->dma.irq.init)); - DMA_ITConfig(overo_dev->cfg->dma.tx.channel, DMA_IT_TC, ENABLE); + // PIOS_Assert(overo_dev->cfg->dma.tx-> == CIRCULAR); + // PIOS_Assert(overo_dev->cfg->dma.rx-> == CIRCULAR); - /* Enable the DMA channels */ - DMA_Cmd(overo_dev->cfg->dma.tx.channel, ENABLE); - DMA_Cmd(overo_dev->cfg->dma.rx.channel, ENABLE); - - *overo_id = (uint32_t) overo_dev; + /* only legal for single-slave config */ + PIOS_Assert(overo_dev->cfg->slave_count == 1); + SPI_SSOutputCmd(overo_dev->cfg->regs, DISABLE); + + /* Initialize the GPIO pins */ + /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ + GPIO_PinAFConfig(overo_dev->cfg->sclk.gpio, + __builtin_ctz(overo_dev->cfg->sclk.init.GPIO_Pin), + overo_dev->cfg->remap); + GPIO_PinAFConfig(overo_dev->cfg->mosi.gpio, + __builtin_ctz(overo_dev->cfg->mosi.init.GPIO_Pin), + overo_dev->cfg->remap); + GPIO_PinAFConfig(overo_dev->cfg->miso.gpio, + __builtin_ctz(overo_dev->cfg->miso.init.GPIO_Pin), + overo_dev->cfg->remap); + GPIO_PinAFConfig(overo_dev->cfg->ssel[0].gpio, + __builtin_ctz(overo_dev->cfg->ssel[0].init.GPIO_Pin), + overo_dev->cfg->remap); + + GPIO_Init(overo_dev->cfg->sclk.gpio, (GPIO_InitTypeDef *)&(overo_dev->cfg->sclk.init)); + GPIO_Init(overo_dev->cfg->mosi.gpio, (GPIO_InitTypeDef *)&(overo_dev->cfg->mosi.init)); + GPIO_Init(overo_dev->cfg->miso.gpio, (GPIO_InitTypeDef *)&(overo_dev->cfg->miso.init)); + + /* Configure circular buffer targets. Configure 0 to be initially active */ + DMA_InitTypeDef dma_init; + + DMA_DeInit(overo_dev->cfg->dma.rx.channel); + dma_init = overo_dev->cfg->dma.rx.init; + dma_init.DMA_Memory0BaseAddr = (uint32_t)overo_dev->rx_buffer[0]; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + dma_init.DMA_BufferSize = PACKET_SIZE; + DMA_Init(overo_dev->cfg->dma.rx.channel, &dma_init); + DMA_DoubleBufferModeConfig(overo_dev->cfg->dma.rx.channel, (uint32_t)overo_dev->rx_buffer[1], DMA_Memory_0); + DMA_DoubleBufferModeCmd(overo_dev->cfg->dma.rx.channel, ENABLE); + + DMA_DeInit(overo_dev->cfg->dma.tx.channel); + dma_init = overo_dev->cfg->dma.tx.init; + dma_init.DMA_Memory0BaseAddr = (uint32_t)overo_dev->tx_buffer[0]; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + dma_init.DMA_BufferSize = PACKET_SIZE; + DMA_Init(overo_dev->cfg->dma.tx.channel, &dma_init); + DMA_DoubleBufferModeConfig(overo_dev->cfg->dma.tx.channel, (uint32_t)overo_dev->tx_buffer[1], DMA_Memory_0); + DMA_DoubleBufferModeCmd(overo_dev->cfg->dma.tx.channel, ENABLE); + + /* Set the packet size */ + DMA_SetCurrDataCounter(overo_dev->cfg->dma.rx.channel, PACKET_SIZE); + DMA_SetCurrDataCounter(overo_dev->cfg->dma.tx.channel, PACKET_SIZE); + + /* Initialize the SPI block */ + SPI_DeInit(overo_dev->cfg->regs); + SPI_Init(overo_dev->cfg->regs, (SPI_InitTypeDef *)&(overo_dev->cfg->init)); + + SPI_CalculateCRC(overo_dev->cfg->regs, DISABLE); + + /* Enable SPI */ + SPI_Cmd(overo_dev->cfg->regs, ENABLE); + + /* Enable SPI interrupts to DMA */ + SPI_I2S_DMACmd(overo_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); + + /* Configure DMA interrupt */ + NVIC_Init((NVIC_InitTypeDef *)&(overo_dev->cfg->dma.irq.init)); + DMA_ITConfig(overo_dev->cfg->dma.tx.channel, DMA_IT_TC, ENABLE); + + /* Enable the DMA channels */ + DMA_Cmd(overo_dev->cfg->dma.tx.channel, ENABLE); + DMA_Cmd(overo_dev->cfg->dma.rx.channel, ENABLE); + + *overo_id = (uint32_t)overo_dev; + + return 0; - return(0); - out_fail: - return(-1); + return -1; } static void PIOS_OVERO_RxStart(uint32_t overo_id, __attribute__((unused)) uint16_t rx_bytes_avail) { - struct pios_overo_dev * overo_dev = (struct pios_overo_dev *)overo_id; - - bool valid = PIOS_OVERO_validate(overo_dev); - PIOS_Assert(valid); - - // DMA RX enable (enable IRQ) ? + struct pios_overo_dev *overo_dev = (struct pios_overo_dev *)overo_id; + + bool valid = PIOS_OVERO_validate(overo_dev); + + PIOS_Assert(valid); + + // DMA RX enable (enable IRQ) ? } static void PIOS_OVERO_TxStart(uint32_t overo_id, __attribute__((unused)) uint16_t tx_bytes_avail) { - struct pios_overo_dev * overo_dev = (struct pios_overo_dev *)overo_id; - - bool valid = PIOS_OVERO_validate(overo_dev); - PIOS_Assert(valid); - - // DMA TX enable (enable IRQ) ? + struct pios_overo_dev *overo_dev = (struct pios_overo_dev *)overo_id; - // Load any pending bytes from TX fifo - //PIOS_OVERO_WriteData(overo_dev); + bool valid = PIOS_OVERO_validate(overo_dev); + + PIOS_Assert(valid); + + // DMA TX enable (enable IRQ) ? + + // Load any pending bytes from TX fifo + // PIOS_OVERO_WriteData(overo_dev); } static void PIOS_OVERO_RegisterRxCallback(uint32_t overo_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_overo_dev * overo_dev = (struct pios_overo_dev *)overo_id; - - bool valid = PIOS_OVERO_validate(overo_dev); - PIOS_Assert(valid); - - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - overo_dev->rx_in_context = context; - overo_dev->rx_in_cb = rx_in_cb; + struct pios_overo_dev *overo_dev = (struct pios_overo_dev *)overo_id; + + bool valid = PIOS_OVERO_validate(overo_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + overo_dev->rx_in_context = context; + overo_dev->rx_in_cb = rx_in_cb; } static void PIOS_OVERO_RegisterTxCallback(uint32_t overo_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_overo_dev * overo_dev = (struct pios_overo_dev *)overo_id; - - bool valid = PIOS_OVERO_validate(overo_dev); - PIOS_Assert(valid); - - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - overo_dev->tx_out_context = context; - overo_dev->tx_out_cb = tx_out_cb; + struct pios_overo_dev *overo_dev = (struct pios_overo_dev *)overo_id; + + bool valid = PIOS_OVERO_validate(overo_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + overo_dev->tx_out_context = context; + overo_dev->tx_out_cb = tx_out_cb; } -#else +#else /* if defined(PIOS_INCLUDE_FREERTOS) */ static void PIOS_OVERO_RegisterTxCallback(uint32_t overo_id, pios_com_callback tx_out_cb, uint32_t context) {}; static void PIOS_OVERO_RegisterRxCallback(uint32_t overo_id, pios_com_callback rx_in_cb, uint32_t context) {}; @@ -375,6 +386,6 @@ static void PIOS_OVERO_RxStart(uint32_t overo_id, uint16_t rx_bytes_avail) {}; #endif /* PIOS_INCLUDE_OVERO */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_ppm.c b/flight/pios/stm32f4xx/pios_ppm.c index 9d976320b..91c5d2ea8 100644 --- a/flight/pios/stm32f4xx/pios_ppm.c +++ b/flight/pios/stm32f4xx/pios_ppm.c @@ -38,157 +38,160 @@ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_ppm_rcvr_driver = { - .read = PIOS_PPM_Get, + .read = PIOS_PPM_Get, }; -#define PIOS_PPM_IN_MIN_NUM_CHANNELS 4 -#define PIOS_PPM_IN_MAX_NUM_CHANNELS PIOS_PPM_NUM_INPUTS -#define PIOS_PPM_STABLE_CHANNEL_COUNT 25 // frames -#define PIOS_PPM_IN_MIN_SYNC_PULSE_US 3800 // microseconds -#define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds -#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds +#define PIOS_PPM_IN_MIN_NUM_CHANNELS 4 +#define PIOS_PPM_IN_MAX_NUM_CHANNELS PIOS_PPM_NUM_INPUTS +#define PIOS_PPM_STABLE_CHANNEL_COUNT 25 // frames +#define PIOS_PPM_IN_MIN_SYNC_PULSE_US 3800 // microseconds +#define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds +#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds static void PIOS_PPM_Supervisor(uint32_t ppm_id); enum pios_ppm_dev_magic { - PIOS_PPM_DEV_MAGIC = 0xee014d8b, + PIOS_PPM_DEV_MAGIC = 0xee014d8b, }; struct pios_ppm_dev { - enum pios_ppm_dev_magic magic; - const struct pios_ppm_cfg * cfg; + enum pios_ppm_dev_magic magic; + const struct pios_ppm_cfg *cfg; - uint8_t PulseIndex; - uint32_t PreviousTime; - uint32_t CurrentTime; - uint32_t DeltaTime; - uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS]; - uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS]; - uint32_t LargeCounter; - int8_t NumChannels; - int8_t NumChannelsPrevFrame; - uint8_t NumChannelCounter; + uint8_t PulseIndex; + uint32_t PreviousTime; + uint32_t CurrentTime; + uint32_t DeltaTime; + uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS]; + uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS]; + uint32_t LargeCounter; + int8_t NumChannels; + int8_t NumChannelsPrevFrame; + uint8_t NumChannelCounter; - uint8_t supv_timer; - bool Tracking; - bool Fresh; + uint8_t supv_timer; + bool Tracking; + bool Fresh; }; -static bool PIOS_PPM_validate(struct pios_ppm_dev * ppm_dev) +static bool PIOS_PPM_validate(struct pios_ppm_dev *ppm_dev) { - return (ppm_dev->magic == PIOS_PPM_DEV_MAGIC); + return ppm_dev->magic == PIOS_PPM_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_ppm_dev * PIOS_PPM_alloc(void) +static struct pios_ppm_dev *PIOS_PPM_alloc(void) { - struct pios_ppm_dev * ppm_dev; + struct pios_ppm_dev *ppm_dev; - ppm_dev = (struct pios_ppm_dev *)pvPortMalloc(sizeof(*ppm_dev)); - if (!ppm_dev) return(NULL); + ppm_dev = (struct pios_ppm_dev *)pvPortMalloc(sizeof(*ppm_dev)); + if (!ppm_dev) { + return NULL; + } - ppm_dev->magic = PIOS_PPM_DEV_MAGIC; - return(ppm_dev); + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; + return ppm_dev; } #else static struct pios_ppm_dev pios_ppm_devs[PIOS_PPM_MAX_DEVS]; static uint8_t pios_ppm_num_devs; -static struct pios_ppm_dev * PIOS_PPM_alloc(void) +static struct pios_ppm_dev *PIOS_PPM_alloc(void) { - struct pios_ppm_dev * ppm_dev; + struct pios_ppm_dev *ppm_dev; - if (pios_ppm_num_devs >= PIOS_PPM_MAX_DEVS) { - return (NULL); - } + if (pios_ppm_num_devs >= PIOS_PPM_MAX_DEVS) { + return NULL; + } - ppm_dev = &pios_ppm_devs[pios_ppm_num_devs++]; - ppm_dev->magic = PIOS_PPM_DEV_MAGIC; + ppm_dev = &pios_ppm_devs[pios_ppm_num_devs++]; + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; - return (ppm_dev); + return ppm_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void PIOS_PPM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); -static void PIOS_PPM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PPM_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PPM_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); static const struct pios_tim_callbacks tim_callbacks = { - .overflow = PIOS_PPM_tim_overflow_cb, - .edge = PIOS_PPM_tim_edge_cb, + .overflow = PIOS_PPM_tim_overflow_cb, + .edge = PIOS_PPM_tim_edge_cb, }; -extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg) +extern int32_t PIOS_PPM_Init(uint32_t *ppm_id, const struct pios_ppm_cfg *cfg) { - PIOS_DEBUG_Assert(ppm_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(ppm_id); + PIOS_DEBUG_Assert(cfg); - struct pios_ppm_dev * ppm_dev; + struct pios_ppm_dev *ppm_dev; - ppm_dev = (struct pios_ppm_dev *) PIOS_PPM_alloc(); - if (!ppm_dev) goto out_fail; + ppm_dev = (struct pios_ppm_dev *)PIOS_PPM_alloc(); + if (!ppm_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - ppm_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + ppm_dev->cfg = cfg; - /* Set up the state variables */ - ppm_dev->PulseIndex = 0; - ppm_dev->PreviousTime = 0; - ppm_dev->CurrentTime = 0; - ppm_dev->DeltaTime = 0; - ppm_dev->LargeCounter = 0; - ppm_dev->NumChannels = -1; - ppm_dev->NumChannelsPrevFrame = -1; - ppm_dev->NumChannelCounter = 0; - ppm_dev->Tracking = false; - ppm_dev->Fresh = false; + /* Set up the state variables */ + ppm_dev->PulseIndex = 0; + ppm_dev->PreviousTime = 0; + ppm_dev->CurrentTime = 0; + ppm_dev->DeltaTime = 0; + ppm_dev->LargeCounter = 0; + ppm_dev->NumChannels = -1; + ppm_dev->NumChannelsPrevFrame = -1; + ppm_dev->NumChannelCounter = 0; + ppm_dev->Tracking = false; + ppm_dev->Fresh = false; - for (uint8_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { - /* Flush counter variables */ - ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + for (uint8_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + /* Flush counter variables */ + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } - } + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)ppm_dev)) { + return -1; + } - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)ppm_dev)) { - return -1; - } + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; - TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + /* Configure the channels to be in capture/compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; - /* Configure the channels to be in capture/compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel * chan = &cfg->channels[i]; + /* Configure timer for input capture */ + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); - /* Configure timer for input capture */ - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); + /* Enable the Capture Compare Interrupt Request */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE); + break; + } + } - /* Enable the Capture Compare Interrupt Request */ - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE); - break; - case TIM_Channel_2: - TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE); - break; - case TIM_Channel_3: - TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE); - break; - case TIM_Channel_4: - TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE); - break; - } - } + if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) { + PIOS_DEBUG_Assert(0); + } - if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) { - PIOS_DEBUG_Assert(0); - } + *ppm_id = (uint32_t)ppm_dev; - *ppm_id = (uint32_t)ppm_dev; - - return(0); + return 0; out_fail: - return(-1); + return -1; } /** @@ -200,152 +203,149 @@ out_fail: */ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)rcvr_id; + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)rcvr_id; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return PIOS_RCVR_INVALID; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return PIOS_RCVR_INVALID; + } - if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { - /* Channel out of range */ - return PIOS_RCVR_INVALID; - } - return ppm_dev->CaptureValue[channel]; + if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* Channel out of range */ + return PIOS_RCVR_INVALID; + } + return ppm_dev->CaptureValue[channel]; } -static void PIOS_PPM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, - __attribute__((unused)) uint8_t channel, uint16_t count) +static void PIOS_PPM_tim_overflow_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, + __attribute__((unused)) uint8_t channel, uint16_t count) { - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context; + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)context; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } - ppm_dev->LargeCounter += count; - - return; + ppm_dev->LargeCounter += count; } -static void PIOS_PPM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) +static void PIOS_PPM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) { - /* Recover our device context */ - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context; + /* Recover our device context */ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)context; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } - if (chan_idx >= ppm_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (chan_idx >= ppm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - /* Shift the last measurement out */ - ppm_dev->PreviousTime = ppm_dev->CurrentTime; + /* Shift the last measurement out */ + ppm_dev->PreviousTime = ppm_dev->CurrentTime; - /* Grab the new count */ - ppm_dev->CurrentTime = count; + /* Grab the new count */ + ppm_dev->CurrentTime = count; - /* Convert to 32-bit timer result */ - ppm_dev->CurrentTime += ppm_dev->LargeCounter; + /* Convert to 32-bit timer result */ + ppm_dev->CurrentTime += ppm_dev->LargeCounter; - /* Capture computation */ - ppm_dev->DeltaTime = ppm_dev->CurrentTime - ppm_dev->PreviousTime; + /* Capture computation */ + ppm_dev->DeltaTime = ppm_dev->CurrentTime - ppm_dev->PreviousTime; - ppm_dev->PreviousTime = ppm_dev->CurrentTime; + ppm_dev->PreviousTime = ppm_dev->CurrentTime; - /* Sync pulse detection */ - if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) { - if (ppm_dev->PulseIndex == ppm_dev->NumChannelsPrevFrame - && ppm_dev->PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS - && ppm_dev->PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS) - { - /* If we see n simultaneous frames of the same - number of channels we save it as our frame size */ - if (ppm_dev->NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT) - ppm_dev->NumChannelCounter++; - else - ppm_dev->NumChannels = ppm_dev->PulseIndex; - } else { - ppm_dev->NumChannelCounter = 0; - } + /* Sync pulse detection */ + if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) { + if (ppm_dev->PulseIndex == ppm_dev->NumChannelsPrevFrame + && ppm_dev->PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS + && ppm_dev->PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* If we see n simultaneous frames of the same + number of channels we save it as our frame size */ + if (ppm_dev->NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT) { + ppm_dev->NumChannelCounter++; + } else { + ppm_dev->NumChannels = ppm_dev->PulseIndex; + } + } else { + ppm_dev->NumChannelCounter = 0; + } - /* Check if the last frame was well formed */ - if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) { - /* The last frame was well formed */ - for (int32_t i = 0; i < ppm_dev->NumChannels; i++) { - ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i]; - } - for (uint32_t i = ppm_dev->NumChannels; - i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { - ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - } - } + /* Check if the last frame was well formed */ + if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) { + /* The last frame was well formed */ + for (int32_t i = 0; i < ppm_dev->NumChannels; i++) { + ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i]; + } + for (uint32_t i = ppm_dev->NumChannels; + i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } + } - ppm_dev->Fresh = true; - ppm_dev->Tracking = true; - ppm_dev->NumChannelsPrevFrame = ppm_dev->PulseIndex; - ppm_dev->PulseIndex = 0; + ppm_dev->Fresh = true; + ppm_dev->Tracking = true; + ppm_dev->NumChannelsPrevFrame = ppm_dev->PulseIndex; + ppm_dev->PulseIndex = 0; - /* We rely on the supervisor to set CaptureValue to invalid - if no valid frame is found otherwise we ride over it */ - - } else if (ppm_dev->Tracking) { - /* Valid pulse duration 0.75 to 2.5 ms*/ - if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US - && ppm_dev->DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US - && ppm_dev->PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) { - - ppm_dev->CaptureValueNewFrame[ppm_dev->PulseIndex] = ppm_dev->DeltaTime; - ppm_dev->PulseIndex++; - } else { - /* Not a valid pulse duration */ - ppm_dev->Tracking = false; - for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { - ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; - } - } - } + /* We rely on the supervisor to set CaptureValue to invalid + if no valid frame is found otherwise we ride over it */ + } else if (ppm_dev->Tracking) { + /* Valid pulse duration 0.75 to 2.5 ms*/ + if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US + && ppm_dev->DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US + && ppm_dev->PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) { + ppm_dev->CaptureValueNewFrame[ppm_dev->PulseIndex] = ppm_dev->DeltaTime; + ppm_dev->PulseIndex++; + } else { + /* Not a valid pulse duration */ + ppm_dev->Tracking = false; + for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } + } + } } -static void PIOS_PPM_Supervisor(uint32_t ppm_id) { - /* Recover our device context */ - struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)ppm_id; +static void PIOS_PPM_Supervisor(uint32_t ppm_id) +{ + /* Recover our device context */ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)ppm_id; - if (!PIOS_PPM_validate(ppm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } - /* - * RTC runs at 625Hz so divide down the base rate so - * that this loop runs at 25Hz. - */ - if(++(ppm_dev->supv_timer) < 25) { - return; - } - ppm_dev->supv_timer = 0; + /* + * RTC runs at 625Hz so divide down the base rate so + * that this loop runs at 25Hz. + */ + if (++(ppm_dev->supv_timer) < 25) { + return; + } + ppm_dev->supv_timer = 0; - if (!ppm_dev->Fresh) { - ppm_dev->Tracking = false; + if (!ppm_dev->Fresh) { + ppm_dev->Tracking = false; - for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { - ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; - } - } + for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } + } - ppm_dev->Fresh = false; + ppm_dev->Fresh = false; } #endif /* PIOS_INCLUDE_PPM */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_pwm.c b/flight/pios/stm32f4xx/pios_pwm.c index 9c55cc624..5583196da 100644 --- a/flight/pios/stm32f4xx/pios_pwm.c +++ b/flight/pios/stm32f4xx/pios_pwm.c @@ -38,7 +38,7 @@ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_pwm_rcvr_driver = { - .read = PIOS_PWM_Get, + .read = PIOS_PWM_Get, }; /* Local Variables */ @@ -46,127 +46,130 @@ const struct pios_rcvr_driver pios_pwm_rcvr_driver = { static const uint32_t PWM_SUPERVISOR_TIMEOUT = 100000; enum pios_pwm_dev_magic { - PIOS_PWM_DEV_MAGIC = 0xab30293c, + PIOS_PWM_DEV_MAGIC = 0xab30293c, }; struct pios_pwm_dev { - enum pios_pwm_dev_magic magic; - const struct pios_pwm_cfg * cfg; + enum pios_pwm_dev_magic magic; + const struct pios_pwm_cfg *cfg; - uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; - uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; - uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; - uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; - uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; - uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; + uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; + uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; + uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; + uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; }; -static bool PIOS_PWM_validate(struct pios_pwm_dev * pwm_dev) +static bool PIOS_PWM_validate(struct pios_pwm_dev *pwm_dev) { - return (pwm_dev->magic == PIOS_PWM_DEV_MAGIC); + return pwm_dev->magic == PIOS_PWM_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_pwm_dev * PIOS_PWM_alloc(void) +static struct pios_pwm_dev *PIOS_PWM_alloc(void) { - struct pios_pwm_dev * pwm_dev; + struct pios_pwm_dev *pwm_dev; - pwm_dev = (struct pios_pwm_dev *)pvPortMalloc(sizeof(*pwm_dev)); - if (!pwm_dev) return(NULL); + pwm_dev = (struct pios_pwm_dev *)pvPortMalloc(sizeof(*pwm_dev)); + if (!pwm_dev) { + return NULL; + } - pwm_dev->magic = PIOS_PWM_DEV_MAGIC; - return(pwm_dev); + pwm_dev->magic = PIOS_PWM_DEV_MAGIC; + return pwm_dev; } #else static struct pios_pwm_dev pios_pwm_devs[PIOS_PWM_MAX_DEVS]; static uint8_t pios_pwm_num_devs; -static struct pios_pwm_dev * PIOS_PWM_alloc(void) +static struct pios_pwm_dev *PIOS_PWM_alloc(void) { - struct pios_pwm_dev * pwm_dev; + struct pios_pwm_dev *pwm_dev; - if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) { - return (NULL); - } + if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) { + return NULL; + } - pwm_dev = &pios_pwm_devs[pios_pwm_num_devs++]; - pwm_dev->magic = PIOS_PWM_DEV_MAGIC; + pwm_dev = &pios_pwm_devs[pios_pwm_num_devs++]; + pwm_dev->magic = PIOS_PWM_DEV_MAGIC; - return (pwm_dev); + return pwm_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ -static void PIOS_PWM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); -static void PIOS_PWM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PWM_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PWM_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); static const struct pios_tim_callbacks tim_callbacks = { - .overflow = PIOS_PWM_tim_overflow_cb, - .edge = PIOS_PWM_tim_edge_cb, + .overflow = PIOS_PWM_tim_overflow_cb, + .edge = PIOS_PWM_tim_edge_cb, }; /** -* Initialises all the pins -*/ -int32_t PIOS_PWM_Init(uint32_t * pwm_id, const struct pios_pwm_cfg * cfg) + * Initialises all the pins + */ +int32_t PIOS_PWM_Init(uint32_t *pwm_id, const struct pios_pwm_cfg *cfg) { - PIOS_DEBUG_Assert(pwm_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(pwm_id); + PIOS_DEBUG_Assert(cfg); - struct pios_pwm_dev * pwm_dev; + struct pios_pwm_dev *pwm_dev; - pwm_dev = (struct pios_pwm_dev *) PIOS_PWM_alloc(); - if (!pwm_dev) goto out_fail; + pwm_dev = (struct pios_pwm_dev *)PIOS_PWM_alloc(); + if (!pwm_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - pwm_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + pwm_dev->cfg = cfg; - for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { - /* Flush counter variables */ - pwm_dev->CaptureState[i] = 0; - pwm_dev->RiseValue[i] = 0; - pwm_dev->FallValue[i] = 0; - pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; - } + for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { + /* Flush counter variables */ + pwm_dev->CaptureState[i] = 0; + pwm_dev->RiseValue[i] = 0; + pwm_dev->FallValue[i] = 0; + pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)pwm_dev)) { - return -1; - } + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)pwm_dev)) { + return -1; + } - /* Configure the channels to be in capture/compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel * chan = &cfg->channels[i]; + /* Configure the channels to be in capture/compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; - /* Configure timer for input capture */ - TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - - /* Enable the Capture Compare Interrupt Request */ - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); - break; - case TIM_Channel_2: - TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); - break; - case TIM_Channel_3: - TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); - break; - case TIM_Channel_4: - TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); - break; - } + /* Configure timer for input capture */ + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); - // Need the update event for that timer to detect timeouts - TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); + /* Enable the Capture Compare Interrupt Request */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); + break; + } - } + // Need the update event for that timer to detect timeouts + TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); + } - *pwm_id = (uint32_t) pwm_dev; + *pwm_id = (uint32_t)pwm_dev; - return (0); + return 0; out_fail: - return (-1); + return -1; } /** @@ -178,106 +181,103 @@ out_fail: */ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)rcvr_id; + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)rcvr_id; - if (!PIOS_PWM_validate(pwm_dev)) { - /* Invalid device specified */ - return PIOS_RCVR_INVALID; - } + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return PIOS_RCVR_INVALID; + } - if (channel >= PIOS_PWM_NUM_INPUTS) { - /* Channel out of range */ - return PIOS_RCVR_INVALID; - } - return pwm_dev->CaptureValue[channel]; + if (channel >= PIOS_PWM_NUM_INPUTS) { + /* Channel out of range */ + return PIOS_RCVR_INVALID; + } + return pwm_dev->CaptureValue[channel]; } -static void PIOS_PWM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) +static void PIOS_PWM_tim_overflow_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) { - struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context; + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)context; - if (!PIOS_PWM_validate(pwm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return; + } - if (channel >= pwm_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (channel >= pwm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - pwm_dev->us_since_update[channel] += count; - if(pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { - pwm_dev->CaptureState[channel] = 0; - pwm_dev->RiseValue[channel] = 0; - pwm_dev->FallValue[channel] = 0; - pwm_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; - pwm_dev->us_since_update[channel] = 0; - } - - return; + pwm_dev->us_since_update[channel] += count; + if (pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { + pwm_dev->CaptureState[channel] = 0; + pwm_dev->RiseValue[channel] = 0; + pwm_dev->FallValue[channel] = 0; + pwm_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; + pwm_dev->us_since_update[channel] = 0; + } } -static void PIOS_PWM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) +static void PIOS_PWM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) { - /* Recover our device context */ - struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context; + /* Recover our device context */ + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)context; - if (!PIOS_PWM_validate(pwm_dev)) { - /* Invalid device specified */ - return; - } + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return; + } - if (chan_idx >= pwm_dev->cfg->num_channels) { - /* Channel out of range */ - return; - } + if (chan_idx >= pwm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } - const struct pios_tim_channel * chan = &pwm_dev->cfg->channels[chan_idx]; + const struct pios_tim_channel *chan = &pwm_dev->cfg->channels[chan_idx]; - if (pwm_dev->CaptureState[chan_idx] == 0) { - pwm_dev->RiseValue[chan_idx] = count; - pwm_dev->us_since_update[chan_idx] = 0; - } else { - pwm_dev->FallValue[chan_idx] = count; - } - - // flip state machine and capture value here - /* Simple rise or fall state machine */ - TIM_ICInitTypeDef TIM_ICInitStructure = pwm_dev->cfg->tim_ic_init; - if (pwm_dev->CaptureState[chan_idx] == 0) { - /* Switch states */ - pwm_dev->CaptureState[chan_idx] = 1; + if (pwm_dev->CaptureState[chan_idx] == 0) { + pwm_dev->RiseValue[chan_idx] = count; + pwm_dev->us_since_update[chan_idx] = 0; + } else { + pwm_dev->FallValue[chan_idx] = count; + } - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - } else { - /* Capture computation */ - if (pwm_dev->FallValue[chan_idx] > pwm_dev->RiseValue[chan_idx]) { - pwm_dev->CaptureValue[chan_idx] = (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]); - } else { - pwm_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - pwm_dev->RiseValue[chan_idx]) + pwm_dev->FallValue[chan_idx]); - } + // flip state machine and capture value here + /* Simple rise or fall state machine */ + TIM_ICInitTypeDef TIM_ICInitStructure = pwm_dev->cfg->tim_ic_init; + if (pwm_dev->CaptureState[chan_idx] == 0) { + /* Switch states */ + pwm_dev->CaptureState[chan_idx] = 1; - /* Switch states */ - pwm_dev->CaptureState[chan_idx] = 0; + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } else { + /* Capture computation */ + if (pwm_dev->FallValue[chan_idx] > pwm_dev->RiseValue[chan_idx]) { + pwm_dev->CaptureValue[chan_idx] = (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]); + } else { + pwm_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - pwm_dev->RiseValue[chan_idx]) + pwm_dev->FallValue[chan_idx]); + } - /* Increase supervisor counter */ - pwm_dev->CapCounter[chan_idx]++; + /* Switch states */ + pwm_dev->CaptureState[chan_idx] = 0; - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; - TIM_ICInitStructure.TIM_Channel = chan->timer_chan; - TIM_ICInit(chan->timer, &TIM_ICInitStructure); - } - + /* Increase supervisor counter */ + pwm_dev->CapCounter[chan_idx]++; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } } #endif /* PIOS_INCLUDE_PWM */ -/** - * @} - * @} - */ +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_rtc.c b/flight/pios/stm32f4xx/pios_rtc.c index 12cf4b4ae..ac0f28f0a 100644 --- a/flight/pios/stm32f4xx/pios_rtc.c +++ b/flight/pios/stm32f4xx/pios_rtc.c @@ -39,48 +39,48 @@ #endif struct rtc_callback_entry { - void (*fn)(uint32_t); - uint32_t data; + void (*fn)(uint32_t); + uint32_t data; }; #define PIOS_RTC_MAX_CALLBACKS 3 struct rtc_callback_entry rtc_callback_list[PIOS_RTC_MAX_CALLBACKS]; static uint8_t rtc_callback_next = 0; -void PIOS_RTC_Init(const struct pios_rtc_cfg * cfg) +void PIOS_RTC_Init(const struct pios_rtc_cfg *cfg) { - RCC_BackupResetCmd(ENABLE); - RCC_BackupResetCmd(DISABLE); - RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); - PWR_BackupAccessCmd(ENABLE); - // Divide external 8 MHz clock to 1 MHz - RCC_RTCCLKConfig(cfg->clksrc); - RCC_RTCCLKCmd(ENABLE); + RCC_BackupResetCmd(ENABLE); + RCC_BackupResetCmd(DISABLE); + RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); + PWR_BackupAccessCmd(ENABLE); + // Divide external 8 MHz clock to 1 MHz + RCC_RTCCLKConfig(cfg->clksrc); + RCC_RTCCLKCmd(ENABLE); - RTC_WakeUpCmd(DISABLE); - // Divide 1 Mhz clock by 16 -> 62.5 khz - RTC_WakeUpClockConfig(RTC_WakeUpClock_RTCCLK_Div16); - // Divide 62.5 khz by 200 to get 625 Hz - RTC_SetWakeUpCounter(cfg->prescaler); //cfg->prescaler); - RTC_WakeUpCmd(ENABLE); - - /* Configure and enable the RTC Second interrupt */ - EXTI_InitTypeDef ExtiInit = { - .EXTI_Line = EXTI_Line22, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }; - EXTI_Init(&ExtiInit); - NVIC_Init(&cfg->irq.init); - RTC_ITConfig(RTC_IT_WUT, ENABLE); - - RTC_ClearFlag(RTC_FLAG_WUTF); + RTC_WakeUpCmd(DISABLE); + // Divide 1 Mhz clock by 16 -> 62.5 khz + RTC_WakeUpClockConfig(RTC_WakeUpClock_RTCCLK_Div16); + // Divide 62.5 khz by 200 to get 625 Hz + RTC_SetWakeUpCounter(cfg->prescaler); // cfg->prescaler); + RTC_WakeUpCmd(ENABLE); + + /* Configure and enable the RTC Second interrupt */ + EXTI_InitTypeDef ExtiInit = { + .EXTI_Line = EXTI_Line22, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }; + EXTI_Init(&ExtiInit); + NVIC_Init(&cfg->irq.init); + RTC_ITConfig(RTC_IT_WUT, ENABLE); + + RTC_ClearFlag(RTC_FLAG_WUTF); } uint32_t PIOS_RTC_Counter() { - return RTC_GetWakeUpCounter(); + return RTC_GetWakeUpCounter(); } /* FIXME: This shouldn't use hard-coded clock rates, dividers or prescalers. @@ -88,52 +88,53 @@ uint32_t PIOS_RTC_Counter() */ float PIOS_RTC_Rate() { - return (float) (8e6f / 128.0f) / (1 + PIOS_RTC_PRESCALER); + return (float)(8e6f / 128.0f) / (1 + PIOS_RTC_PRESCALER); } -float PIOS_RTC_MsPerTick() +float PIOS_RTC_MsPerTick() { - return 1000.0f / PIOS_RTC_Rate(); + return 1000.0f / PIOS_RTC_Rate(); } /* TODO: This needs a mutex around rtc_callbacks[] */ bool PIOS_RTC_RegisterTickCallback(void (*fn)(uint32_t id), uint32_t data) { - struct rtc_callback_entry * cb; - if (rtc_callback_next >= PIOS_RTC_MAX_CALLBACKS) { - return false; - } + struct rtc_callback_entry *cb; - cb = &rtc_callback_list[rtc_callback_next++]; + if (rtc_callback_next >= PIOS_RTC_MAX_CALLBACKS) { + return false; + } - cb->fn = fn; - cb->data = data; - return true; + cb = &rtc_callback_list[rtc_callback_next++]; + + cb->fn = fn; + cb->data = data; + return true; } -void PIOS_RTC_irq_handler (void) +void PIOS_RTC_irq_handler(void) { - if (RTC_GetITStatus(RTC_IT_WUT)) - { - /* Call all registered callbacks */ - for (uint8_t i = 0; i < rtc_callback_next; i++) { - struct rtc_callback_entry * cb = &rtc_callback_list[i]; - if (cb->fn) { - (cb->fn)(cb->data); - } - } + if (RTC_GetITStatus(RTC_IT_WUT)) { + /* Call all registered callbacks */ + for (uint8_t i = 0; i < rtc_callback_next; i++) { + struct rtc_callback_entry *cb = &rtc_callback_list[i]; + if (cb->fn) { + (cb->fn)(cb->data); + } + } - /* Clear the RTC Second interrupt */ - RTC_ClearITPendingBit(RTC_IT_WUT); - } + /* Clear the RTC Second interrupt */ + RTC_ClearITPendingBit(RTC_IT_WUT); + } - if (EXTI_GetITStatus(EXTI_Line22) != RESET) - EXTI_ClearITPendingBit(EXTI_Line22); + if (EXTI_GetITStatus(EXTI_Line22) != RESET) { + EXTI_ClearITPendingBit(EXTI_Line22); + } } #endif /* PIOS_INCLUDE_RTC */ -/** +/** * @} * @} */ diff --git a/flight/pios/stm32f4xx/pios_servo.c b/flight/pios/stm32f4xx/pios_servo.c index 5c5d6ef52..32803a57b 100644 --- a/flight/pios/stm32f4xx/pios_servo.c +++ b/flight/pios/stm32f4xx/pios_servo.c @@ -37,123 +37,124 @@ /* Private Function Prototypes */ -static const struct pios_servo_cfg * servo_cfg; +static const struct pios_servo_cfg *servo_cfg; /** -* Initialise Servos -*/ -int32_t PIOS_Servo_Init(const struct pios_servo_cfg * cfg) + * Initialise Servos + */ +int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg) { - uint32_t tim_id; - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) { - return -1; - } + uint32_t tim_id; - /* Store away the requested configuration */ - servo_cfg = cfg; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) { + return -1; + } - /* Configure the channels to be in output compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel * chan = &cfg->channels[i]; + /* Store away the requested configuration */ + servo_cfg = cfg; - /* Set up for output compare function */ - switch(chan->timer_chan) { - case TIM_Channel_1: - TIM_OC1Init(chan->timer, &cfg->tim_oc_init); - TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_2: - TIM_OC2Init(chan->timer, &cfg->tim_oc_init); - TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_3: - TIM_OC3Init(chan->timer, &cfg->tim_oc_init); - TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_4: - TIM_OC4Init(chan->timer, &cfg->tim_oc_init); - TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - } + /* Configure the channels to be in output compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; - TIM_ARRPreloadConfig(chan->timer, ENABLE); - TIM_CtrlPWMOutputs(chan->timer, ENABLE); - TIM_Cmd(chan->timer, ENABLE); - } + /* Set up for output compare function */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_OC1Init(chan->timer, &cfg->tim_oc_init); + TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(chan->timer, &cfg->tim_oc_init); + TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(chan->timer, &cfg->tim_oc_init); + TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(chan->timer, &cfg->tim_oc_init); + TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + } - return 0; + TIM_ARRPreloadConfig(chan->timer, ENABLE); + TIM_CtrlPWMOutputs(chan->timer, ENABLE); + TIM_Cmd(chan->timer, ENABLE); + } + + return 0; } /** -* Set the servo update rate (Max 500Hz) -* \param[in] array of rates in Hz -* \param[in] maximum number of banks -*/ -void PIOS_Servo_SetHz(const uint16_t * speeds, uint8_t banks) + * Set the servo update rate (Max 500Hz) + * \param[in] array of rates in Hz + * \param[in] maximum number of banks + */ +void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks) { - if (!servo_cfg) { - return; - } + if (!servo_cfg) { + return; + } - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init; - TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - // + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init; + TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + // - uint8_t set = 0; + uint8_t set = 0; - for(uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) { - bool new = true; - const struct pios_tim_channel * chan = &servo_cfg->channels[i]; + for (uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) { + bool new = true; + const struct pios_tim_channel *chan = &servo_cfg->channels[i]; - /* See if any previous channels use that same timer */ - for(uint8_t j = 0; (j < i) && new; j++) - new &= chan->timer != servo_cfg->channels[j].timer; + /* See if any previous channels use that same timer */ + for (uint8_t j = 0; (j < i) && new; j++) { + new &= chan->timer != servo_cfg->channels[j].timer; + } - if(new) { - // Choose the correct prescaler value for the APB the timer is attached - if (chan->timer==TIM1 || chan->timer==TIM8 || chan->timer==TIM9 || chan->timer==TIM10 || chan->timer==TIM11 ){ - TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1; - } - else { - TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1; - } + if (new) { + // Choose the correct prescaler value for the APB the timer is attached + if (chan->timer == TIM1 || chan->timer == TIM8 || chan->timer == TIM9 || chan->timer == TIM10 || chan->timer == TIM11) { + TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1; + } else { + TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1; + } - TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1); - TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure); - set++; - } - } + TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1); + TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure); + set++; + } + } } /** -* Set servo position -* \param[in] Servo Servo number (0-7) -* \param[in] Position Servo position in microseconds -*/ + * Set servo position + * \param[in] Servo Servo number (0-7) + * \param[in] Position Servo position in microseconds + */ void PIOS_Servo_Set(uint8_t servo, uint16_t position) { - /* Make sure servo exists */ - if (!servo_cfg || servo >= servo_cfg->num_channels) { - return; - } + /* Make sure servo exists */ + if (!servo_cfg || servo >= servo_cfg->num_channels) { + return; + } - /* Update the position */ - const struct pios_tim_channel * chan = &servo_cfg->channels[servo]; - switch(chan->timer_chan) { - case TIM_Channel_1: - TIM_SetCompare1(chan->timer, position); - break; - case TIM_Channel_2: - TIM_SetCompare2(chan->timer, position); - break; - case TIM_Channel_3: - TIM_SetCompare3(chan->timer, position); - break; - case TIM_Channel_4: - TIM_SetCompare4(chan->timer, position); - break; - } + /* Update the position */ + const struct pios_tim_channel *chan = &servo_cfg->channels[servo]; + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_SetCompare1(chan->timer, position); + break; + case TIM_Channel_2: + TIM_SetCompare2(chan->timer, position); + break; + case TIM_Channel_3: + TIM_SetCompare3(chan->timer, position); + break; + case TIM_Channel_4: + TIM_SetCompare4(chan->timer, position); + break; + } } #endif /* PIOS_INCLUDE_SERVO */ diff --git a/flight/pios/stm32f4xx/pios_spi.c b/flight/pios/stm32f4xx/pios_spi.c index bc8304596..5442e5e26 100644 --- a/flight/pios/stm32f4xx/pios_spi.c +++ b/flight/pios/stm32f4xx/pios_spi.c @@ -42,157 +42,159 @@ #include -#define SPI_MAX_BLOCK_PIO 128 +#define SPI_MAX_BLOCK_PIO 128 -static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev * com_dev) +static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev *com_dev) { - /* Should check device magic here */ - return(true); + /* Should check device magic here */ + return true; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_spi_dev * PIOS_SPI_alloc(void) +static struct pios_spi_dev *PIOS_SPI_alloc(void) { - return (pvPortMalloc(sizeof(struct pios_spi_dev))); + return pvPortMalloc(sizeof(struct pios_spi_dev)); } #else static struct pios_spi_dev pios_spi_devs[PIOS_SPI_MAX_DEVS]; static uint8_t pios_spi_num_devs; -static struct pios_spi_dev * PIOS_SPI_alloc(void) +static struct pios_spi_dev *PIOS_SPI_alloc(void) { - if (pios_spi_num_devs >= PIOS_SPI_MAX_DEVS) { - return (NULL); - } + if (pios_spi_num_devs >= PIOS_SPI_MAX_DEVS) { + return NULL; + } - return (&pios_spi_devs[pios_spi_num_devs++]); + return &pios_spi_devs[pios_spi_num_devs++]; } #endif /** -* Initialises SPI pins -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ -int32_t PIOS_SPI_Init(uint32_t * spi_id, const struct pios_spi_cfg * cfg) + * Initialises SPI pins + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ +int32_t PIOS_SPI_Init(uint32_t *spi_id, const struct pios_spi_cfg *cfg) { - uint32_t init_ssel = 0; + uint32_t init_ssel = 0; - PIOS_Assert(spi_id); - PIOS_Assert(cfg); + PIOS_Assert(spi_id); + PIOS_Assert(cfg); - struct pios_spi_dev * spi_dev; + struct pios_spi_dev *spi_dev; - spi_dev = (struct pios_spi_dev *) PIOS_SPI_alloc(); - if (!spi_dev) goto out_fail; + spi_dev = (struct pios_spi_dev *)PIOS_SPI_alloc(); + if (!spi_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - spi_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + spi_dev->cfg = cfg; #if defined(PIOS_INCLUDE_FREERTOS) - vSemaphoreCreateBinary(spi_dev->busy); - xSemaphoreGive(spi_dev->busy); + vSemaphoreCreateBinary(spi_dev->busy); + xSemaphoreGive(spi_dev->busy); #endif - /* Disable callback function */ - spi_dev->callback = NULL; + /* Disable callback function */ + spi_dev->callback = NULL; - /* Set rx/tx dummy bytes to a known value */ - spi_dev->rx_dummy_byte = 0xFF; - spi_dev->tx_dummy_byte = 0xFF; + /* Set rx/tx dummy bytes to a known value */ + spi_dev->rx_dummy_byte = 0xFF; + spi_dev->tx_dummy_byte = 0xFF; - switch (spi_dev->cfg->init.SPI_NSS) { - case SPI_NSS_Soft: - if (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) { - /* We're a master in soft NSS mode, make sure we see NSS high at all times. */ - SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Set); - /* Init as many slave selects as the config advertises. */ - init_ssel = spi_dev->cfg->slave_count; - } else { - /* We're a slave in soft NSS mode, make sure we see NSS low at all times. */ - SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Reset); - } - break; - - case SPI_NSS_Hard: - /* only legal for single-slave config */ - PIOS_Assert(spi_dev->cfg->slave_count == 1); - init_ssel = 1; - SPI_SSOutputCmd(spi_dev->cfg->regs, (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) ? ENABLE : DISABLE); - /* FIXME: Should this also call SPI_SSOutputCmd()? */ - break; - - default: - PIOS_Assert(0); - } + switch (spi_dev->cfg->init.SPI_NSS) { + case SPI_NSS_Soft: + if (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) { + /* We're a master in soft NSS mode, make sure we see NSS high at all times. */ + SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Set); + /* Init as many slave selects as the config advertises. */ + init_ssel = spi_dev->cfg->slave_count; + } else { + /* We're a slave in soft NSS mode, make sure we see NSS low at all times. */ + SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Reset); + } + break; - /* Initialize the GPIO pins */ - /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ - if (spi_dev->cfg->remap) { - GPIO_PinAFConfig(spi_dev->cfg->sclk.gpio, - __builtin_ctz(spi_dev->cfg->sclk.init.GPIO_Pin), - spi_dev->cfg->remap); - GPIO_PinAFConfig(spi_dev->cfg->mosi.gpio, - __builtin_ctz(spi_dev->cfg->mosi.init.GPIO_Pin), - spi_dev->cfg->remap); - GPIO_PinAFConfig(spi_dev->cfg->miso.gpio, - __builtin_ctz(spi_dev->cfg->miso.init.GPIO_Pin), - spi_dev->cfg->remap); - for (uint32_t i = 0; i < init_ssel; i++) { - GPIO_PinAFConfig(spi_dev->cfg->ssel[i].gpio, - __builtin_ctz(spi_dev->cfg->ssel[i].init.GPIO_Pin), - spi_dev->cfg->remap); - } - } - GPIO_Init(spi_dev->cfg->sclk.gpio, (GPIO_InitTypeDef*)&(spi_dev->cfg->sclk.init)); - GPIO_Init(spi_dev->cfg->mosi.gpio, (GPIO_InitTypeDef*)&(spi_dev->cfg->mosi.init)); - GPIO_Init(spi_dev->cfg->miso.gpio, (GPIO_InitTypeDef*)&(spi_dev->cfg->miso.init)); - - if(spi_dev->cfg->init.SPI_NSS != SPI_NSS_Hard) { - for (uint32_t i = 0; i < init_ssel; i++) { - /* Since we're driving the SSEL pin in software, ensure that the slave is deselected */ - /* XXX multi-slave support - maybe have another SPI_NSS_ mode? */ - GPIO_SetBits(spi_dev->cfg->ssel[i].gpio, spi_dev->cfg->ssel[i].init.GPIO_Pin); - GPIO_Init(spi_dev->cfg->ssel[i].gpio, (GPIO_InitTypeDef*)&(spi_dev->cfg->ssel[i].init)); - } - } + case SPI_NSS_Hard: + /* only legal for single-slave config */ + PIOS_Assert(spi_dev->cfg->slave_count == 1); + init_ssel = 1; + SPI_SSOutputCmd(spi_dev->cfg->regs, (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) ? ENABLE : DISABLE); + /* FIXME: Should this also call SPI_SSOutputCmd()? */ + break; - /* Configure DMA for SPI Rx */ - DMA_DeInit(spi_dev->cfg->dma.rx.channel); - DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); - DMA_Init(spi_dev->cfg->dma.rx.channel, (DMA_InitTypeDef*)&(spi_dev->cfg->dma.rx.init)); + default: + PIOS_Assert(0); + } - /* Configure DMA for SPI Tx */ - DMA_DeInit(spi_dev->cfg->dma.tx.channel); - DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); - DMA_Init(spi_dev->cfg->dma.tx.channel, (DMA_InitTypeDef*)&(spi_dev->cfg->dma.tx.init)); + /* Initialize the GPIO pins */ + /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ + if (spi_dev->cfg->remap) { + GPIO_PinAFConfig(spi_dev->cfg->sclk.gpio, + __builtin_ctz(spi_dev->cfg->sclk.init.GPIO_Pin), + spi_dev->cfg->remap); + GPIO_PinAFConfig(spi_dev->cfg->mosi.gpio, + __builtin_ctz(spi_dev->cfg->mosi.init.GPIO_Pin), + spi_dev->cfg->remap); + GPIO_PinAFConfig(spi_dev->cfg->miso.gpio, + __builtin_ctz(spi_dev->cfg->miso.init.GPIO_Pin), + spi_dev->cfg->remap); + for (uint32_t i = 0; i < init_ssel; i++) { + GPIO_PinAFConfig(spi_dev->cfg->ssel[i].gpio, + __builtin_ctz(spi_dev->cfg->ssel[i].init.GPIO_Pin), + spi_dev->cfg->remap); + } + } + GPIO_Init(spi_dev->cfg->sclk.gpio, (GPIO_InitTypeDef *)&(spi_dev->cfg->sclk.init)); + GPIO_Init(spi_dev->cfg->mosi.gpio, (GPIO_InitTypeDef *)&(spi_dev->cfg->mosi.init)); + GPIO_Init(spi_dev->cfg->miso.gpio, (GPIO_InitTypeDef *)&(spi_dev->cfg->miso.init)); - /* Initialize the SPI block */ - SPI_DeInit(spi_dev->cfg->regs); - SPI_Init(spi_dev->cfg->regs, (SPI_InitTypeDef*)&(spi_dev->cfg->init)); + if (spi_dev->cfg->init.SPI_NSS != SPI_NSS_Hard) { + for (uint32_t i = 0; i < init_ssel; i++) { + /* Since we're driving the SSEL pin in software, ensure that the slave is deselected */ + /* XXX multi-slave support - maybe have another SPI_NSS_ mode? */ + GPIO_SetBits(spi_dev->cfg->ssel[i].gpio, spi_dev->cfg->ssel[i].init.GPIO_Pin); + GPIO_Init(spi_dev->cfg->ssel[i].gpio, (GPIO_InitTypeDef *)&(spi_dev->cfg->ssel[i].init)); + } + } - /* Configure CRC calculation */ - if (spi_dev->cfg->use_crc) { - SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); - } else { - SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); - } + /* Configure DMA for SPI Rx */ + DMA_DeInit(spi_dev->cfg->dma.rx.channel); + DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); + DMA_Init(spi_dev->cfg->dma.rx.channel, (DMA_InitTypeDef *)&(spi_dev->cfg->dma.rx.init)); - /* Enable SPI */ - SPI_Cmd(spi_dev->cfg->regs, ENABLE); + /* Configure DMA for SPI Tx */ + DMA_DeInit(spi_dev->cfg->dma.tx.channel); + DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); + DMA_Init(spi_dev->cfg->dma.tx.channel, (DMA_InitTypeDef *)&(spi_dev->cfg->dma.tx.init)); - /* Enable SPI interrupts to DMA */ - SPI_I2S_DMACmd(spi_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); + /* Initialize the SPI block */ + SPI_DeInit(spi_dev->cfg->regs); + SPI_Init(spi_dev->cfg->regs, (SPI_InitTypeDef *)&(spi_dev->cfg->init)); - /* Must store this before enabling interrupt */ - *spi_id = (uint32_t)spi_dev; + /* Configure CRC calculation */ + if (spi_dev->cfg->use_crc) { + SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); + } else { + SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); + } - /* Configure DMA interrupt */ - NVIC_Init((NVIC_InitTypeDef*)&(spi_dev->cfg->dma.irq.init)); + /* Enable SPI */ + SPI_Cmd(spi_dev->cfg->regs, ENABLE); - return(0); + /* Enable SPI interrupts to DMA */ + SPI_I2S_DMACmd(spi_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); + + /* Must store this before enabling interrupt */ + *spi_id = (uint32_t)spi_dev; + + /* Configure DMA interrupt */ + NVIC_Init((NVIC_InitTypeDef *)&(spi_dev->cfg->dma.irq.init)); + + return 0; out_fail: - return(-1); + return -1; } /** @@ -216,29 +218,30 @@ out_fail: */ int32_t PIOS_SPI_SetClockSpeed(uint32_t spi_id, SPIPrescalerTypeDef spi_prescaler) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) - - SPI_InitTypeDef SPI_InitStructure; - - if (spi_prescaler >= 8) { - /* Invalid prescaler selected */ - return -3; - } - - /* Start with a copy of the default configuration for the peripheral */ - SPI_InitStructure = spi_dev->cfg->init; - - /* Adjust the prescaler for the peripheral's clock */ - SPI_InitStructure.SPI_BaudRatePrescaler = ((uint16_t) spi_prescaler & 7) << 3; - - /* Write back the new configuration */ - SPI_Init(spi_dev->cfg->regs, &SPI_InitStructure); - - PIOS_SPI_TransferByte(spi_id, 0xFF); - return 0; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + + PIOS_Assert(valid) + + SPI_InitTypeDef SPI_InitStructure; + + if (spi_prescaler >= 8) { + /* Invalid prescaler selected */ + return -3; + } + + /* Start with a copy of the default configuration for the peripheral */ + SPI_InitStructure = spi_dev->cfg->init; + + /* Adjust the prescaler for the peripheral's clock */ + SPI_InitStructure.SPI_BaudRatePrescaler = ((uint16_t)spi_prescaler & 7) << 3; + + /* Write back the new configuration */ + SPI_Init(spi_dev->cfg->regs, &SPI_InitStructure); + + PIOS_SPI_TransferByte(spi_id, 0xFF); + return 0; } /** @@ -250,27 +253,32 @@ int32_t PIOS_SPI_SetClockSpeed(uint32_t spi_id, SPIPrescalerTypeDef spi_prescale int32_t PIOS_SPI_ClaimBus(uint32_t spi_id) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) - if (xSemaphoreTake(spi_dev->busy, 0xffff) != pdTRUE) - return -1; + if (xSemaphoreTake(spi_dev->busy, 0xffff) != pdTRUE) { + return -1; + } #else - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - uint32_t timeout = 0xffff; - while((PIOS_SPI_Busy(spi_id) || spi_dev->busy) && --timeout); - if(timeout == 0) //timed out - return -1; - - PIOS_IRQ_Disable(); - if(spi_dev->busy) - return -1; - spi_dev->busy = 1; - PIOS_IRQ_Enable(); -#endif - return 0; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + uint32_t timeout = 0xffff; + while ((PIOS_SPI_Busy(spi_id) || spi_dev->busy) && --timeout) { + ; + } + if (timeout == 0) { // timed out + return -1; + } + + PIOS_IRQ_Disable(); + if (spi_dev->busy) { + return -1; + } + spi_dev->busy = 1; + PIOS_IRQ_Enable(); +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + return 0; } /** @@ -284,24 +292,26 @@ int32_t PIOS_SPI_ClaimBus(uint32_t spi_id) int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) - - if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE){ - return -1; - } - if (woken) { - *woken = *woken || (higherPriorityTaskWoken == pdTRUE); - } - return 0; + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) + + if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE) { + return -1; + } + if (woken) { + *woken = *woken || (higherPriorityTaskWoken == pdTRUE); + } + return 0; + #else - if (woken) { - *woken = false; - } - return PIOS_SPI_ClaimBus(spi_id); + if (woken) { + *woken = false; + } + return PIOS_SPI_ClaimBus(spi_id); + #endif } @@ -313,19 +323,19 @@ int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken) int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) - xSemaphoreGive(spi_dev->busy); + xSemaphoreGive(spi_dev->busy); #else - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - PIOS_IRQ_Disable(); - spi_dev->busy = 0; - PIOS_IRQ_Enable(); + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + PIOS_IRQ_Disable(); + spi_dev->busy = 0; + PIOS_IRQ_Enable(); #endif - return 0; + return 0; } /** @@ -338,388 +348,422 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id) int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken) { #if defined(PIOS_INCLUDE_FREERTOS) - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; bool valid = PIOS_SPI_validate(spi_dev); PIOS_Assert(valid) xSemaphoreGiveFromISR(spi_dev->busy, &higherPriorityTaskWoken); if (woken) { - *woken = *woken || (higherPriorityTaskWoken == pdTRUE); + *woken = *woken || (higherPriorityTaskWoken == pdTRUE); } - return 0; + return 0; + #else - if (woken) { - *woken = false; - } - return PIOS_SPI_ReleaseBus(spi_id); + if (woken) { + *woken = false; + } + return PIOS_SPI_ReleaseBus(spi_id); + #endif } /** -* Controls the RC (Register Clock alias Chip Select) pin of a SPI port -* \param[in] spi SPI number (0 or 1) -* \param[in] pin_value 0 or 1 -* \return 0 if no error -*/ + * Controls the RC (Register Clock alias Chip Select) pin of a SPI port + * \param[in] spi SPI number (0 or 1) + * \param[in] pin_value 0 or 1 + * \return 0 if no error + */ int32_t PIOS_SPI_RC_PinSet(uint32_t spi_id, uint32_t slave_id, uint8_t pin_value) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) - PIOS_Assert(slave_id <= spi_dev->cfg->slave_count) + bool valid = PIOS_SPI_validate(spi_dev); - /* XXX multi-slave support? */ - if (pin_value) { - GPIO_SetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); - } else { - GPIO_ResetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); - } + PIOS_Assert(valid) + PIOS_Assert(slave_id <= spi_dev->cfg->slave_count) - return 0; + /* XXX multi-slave support? */ + if (pin_value) { + GPIO_SetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); + } else { + GPIO_ResetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); + } + + return 0; } /** -* Transfers a byte to SPI output and reads back the return value from SPI input -* \param[in] spi SPI number (0 or 1) -* \param[in] b the byte which should be transfered -*/ + * Transfers a byte to SPI output and reads back the return value from SPI input + * \param[in] spi SPI number (0 or 1) + * \param[in] b the byte which should be transfered + */ int32_t PIOS_SPI_TransferByte(uint32_t spi_id, uint8_t b) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); -// uint8_t dummy; - uint8_t rx_byte; + PIOS_Assert(valid) - /* - * Procedure taken from STM32F10xxx Reference Manual section 23.3.5 - */ +// uint8_t dummy; + uint8_t rx_byte; - /* Make sure the RXNE flag is cleared by reading the DR register */ - /*dummy =*/(void)spi_dev->cfg->regs->DR; + /* + * Procedure taken from STM32F10xxx Reference Manual section 23.3.5 + */ - /* Start the transfer */ - spi_dev->cfg->regs->DR = b; + /* Make sure the RXNE flag is cleared by reading the DR register */ + /*dummy =*/ (void)spi_dev->cfg->regs->DR; - /* Wait until there is a byte to read */ - while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_RXNE)) ; + /* Start the transfer */ + spi_dev->cfg->regs->DR = b; - /* Read the rx'd byte */ - rx_byte = spi_dev->cfg->regs->DR; + /* Wait until there is a byte to read */ + while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_RXNE)) { + ; + } - /* Wait until the TXE goes high */ - while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) ; + /* Read the rx'd byte */ + rx_byte = spi_dev->cfg->regs->DR; - /* Wait for SPI transfer to have fully completed */ - while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) ; + /* Wait until the TXE goes high */ + while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) { + ; + } - /* Return received byte */ - return rx_byte; + /* Wait for SPI transfer to have fully completed */ + while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) { + ; + } + + /* Return received byte */ + return rx_byte; } /** -* Transfers a block of bytes via DMA. -* \param[in] spi SPI number (0 or 1) -* \param[in] send_buffer pointer to buffer which should be sent.
-* If NULL, 0xff (all-one) will be sent. -* \param[in] receive_buffer pointer to buffer which should get the received values.
-* If NULL, received bytes will be discarded. -* \param[in] len number of bytes which should be transfered -* \param[in] callback pointer to callback function which will be executed -* from DMA channel interrupt once the transfer is finished. -* If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will -* block until the transfer is finished. -* \return >= 0 if no error during transfer -* \return -1 if disabled SPI port selected -* \return -3 if function has been called during an ongoing DMA transfer -*/ + * Transfers a block of bytes via DMA. + * \param[in] spi SPI number (0 or 1) + * \param[in] send_buffer pointer to buffer which should be sent.
+ * If NULL, 0xff (all-one) will be sent. + * \param[in] receive_buffer pointer to buffer which should get the received values.
+ * If NULL, received bytes will be discarded. + * \param[in] len number of bytes which should be transfered + * \param[in] callback pointer to callback function which will be executed + * from DMA channel interrupt once the transfer is finished. + * If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will + * block until the transfer is finished. + * \return >= 0 if no error during transfer + * \return -1 if disabled SPI port selected + * \return -3 if function has been called during an ongoing DMA transfer + */ static int32_t SPI_DMA_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - DMA_InitTypeDef dma_init; + PIOS_Assert(valid) - /* Exit if ongoing transfer */ - if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { - return -3; - } + DMA_InitTypeDef dma_init; - /* Disable the DMA channels */ - DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); - DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); + /* Exit if ongoing transfer */ + if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { + return -3; + } - while(DMA_GetCmdStatus(spi_dev->cfg->dma.rx.channel) == ENABLE); - while(DMA_GetCmdStatus(spi_dev->cfg->dma.tx.channel) == ENABLE); + /* Disable the DMA channels */ + DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); + DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); - /* Disable the SPI peripheral */ - /* Initialize the SPI block */ - SPI_DeInit(spi_dev->cfg->regs); - SPI_Init(spi_dev->cfg->regs, (SPI_InitTypeDef*)&(spi_dev->cfg->init)); - SPI_Cmd(spi_dev->cfg->regs, DISABLE); - /* Configure CRC calculation */ - if (spi_dev->cfg->use_crc) { - SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); - } else { - SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); - } - - /* Enable SPI interrupts to DMA */ - SPI_I2S_DMACmd(spi_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); + while (DMA_GetCmdStatus(spi_dev->cfg->dma.rx.channel) == ENABLE) { + ; + } + while (DMA_GetCmdStatus(spi_dev->cfg->dma.tx.channel) == ENABLE) { + ; + } - /* Set callback function */ - spi_dev->callback = callback; + /* Disable the SPI peripheral */ + /* Initialize the SPI block */ + SPI_DeInit(spi_dev->cfg->regs); + SPI_Init(spi_dev->cfg->regs, (SPI_InitTypeDef *)&(spi_dev->cfg->init)); + SPI_Cmd(spi_dev->cfg->regs, DISABLE); + /* Configure CRC calculation */ + if (spi_dev->cfg->use_crc) { + SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); + } else { + SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); + } - /* - * Configure Rx channel - */ + /* Enable SPI interrupts to DMA */ + SPI_I2S_DMACmd(spi_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); - /* Start with the default configuration for this peripheral */ - dma_init = spi_dev->cfg->dma.rx.init; - DMA_DeInit(spi_dev->cfg->dma.rx.channel); - if (receive_buffer != NULL) { - /* Enable memory addr. increment - bytes written into receive buffer */ - dma_init.DMA_Memory0BaseAddr = (uint32_t) receive_buffer; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - } else { - /* Disable memory addr. increment - bytes written into dummy buffer */ - spi_dev->rx_dummy_byte = 0xFF; - dma_init.DMA_Memory0BaseAddr = (uint32_t) &spi_dev->rx_dummy_byte; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; - } - if (spi_dev->cfg->use_crc) { - /* Make sure the CRC error flag is cleared before we start */ - SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); - } + /* Set callback function */ + spi_dev->callback = callback; - dma_init.DMA_BufferSize = len; - DMA_Init(spi_dev->cfg->dma.rx.channel, &(dma_init)); + /* + * Configure Rx channel + */ - /* - * Configure Tx channel - */ + /* Start with the default configuration for this peripheral */ + dma_init = spi_dev->cfg->dma.rx.init; + DMA_DeInit(spi_dev->cfg->dma.rx.channel); + if (receive_buffer != NULL) { + /* Enable memory addr. increment - bytes written into receive buffer */ + dma_init.DMA_Memory0BaseAddr = (uint32_t)receive_buffer; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + } else { + /* Disable memory addr. increment - bytes written into dummy buffer */ + spi_dev->rx_dummy_byte = 0xFF; + dma_init.DMA_Memory0BaseAddr = (uint32_t)&spi_dev->rx_dummy_byte; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; + } + if (spi_dev->cfg->use_crc) { + /* Make sure the CRC error flag is cleared before we start */ + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + } - /* Start with the default configuration for this peripheral */ - dma_init = spi_dev->cfg->dma.tx.init; - DMA_DeInit(spi_dev->cfg->dma.tx.channel); - if (send_buffer != NULL) { - /* Enable memory addr. increment - bytes written into receive buffer */ - dma_init.DMA_Memory0BaseAddr = (uint32_t) send_buffer; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; - } else { - /* Disable memory addr. increment - bytes written into dummy buffer */ - spi_dev->tx_dummy_byte = 0xFF; - dma_init.DMA_Memory0BaseAddr = (uint32_t) &spi_dev->tx_dummy_byte; - dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; - } + dma_init.DMA_BufferSize = len; + DMA_Init(spi_dev->cfg->dma.rx.channel, &(dma_init)); - if (spi_dev->cfg->use_crc) { - /* The last byte of the payload will be replaced with the CRC8 */ - dma_init.DMA_BufferSize = len - 1; - } else { - dma_init.DMA_BufferSize = len; - } + /* + * Configure Tx channel + */ - DMA_Init(spi_dev->cfg->dma.tx.channel, &(dma_init)); + /* Start with the default configuration for this peripheral */ + dma_init = spi_dev->cfg->dma.tx.init; + DMA_DeInit(spi_dev->cfg->dma.tx.channel); + if (send_buffer != NULL) { + /* Enable memory addr. increment - bytes written into receive buffer */ + dma_init.DMA_Memory0BaseAddr = (uint32_t)send_buffer; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + } else { + /* Disable memory addr. increment - bytes written into dummy buffer */ + spi_dev->tx_dummy_byte = 0xFF; + dma_init.DMA_Memory0BaseAddr = (uint32_t)&spi_dev->tx_dummy_byte; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; + } - /* Enable DMA interrupt if callback function active */ - DMA_ITConfig(spi_dev->cfg->dma.rx.channel, DMA_IT_TC, (callback != NULL) ? ENABLE : DISABLE); + if (spi_dev->cfg->use_crc) { + /* The last byte of the payload will be replaced with the CRC8 */ + dma_init.DMA_BufferSize = len - 1; + } else { + dma_init.DMA_BufferSize = len; + } - /* Flush out the CRC registers */ - SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); - (void)SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); - SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + DMA_Init(spi_dev->cfg->dma.tx.channel, &(dma_init)); - /* Make sure to flush out the receive buffer */ - (void)SPI_I2S_ReceiveData(spi_dev->cfg->regs); + /* Enable DMA interrupt if callback function active */ + DMA_ITConfig(spi_dev->cfg->dma.rx.channel, DMA_IT_TC, (callback != NULL) ? ENABLE : DISABLE); - if (spi_dev->cfg->use_crc) { - /* Need a 0->1 transition to reset the CRC logic */ - SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); - } + /* Flush out the CRC registers */ + SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); + (void)SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); - /* Start DMA transfers */ - DMA_Cmd(spi_dev->cfg->dma.rx.channel, ENABLE); - DMA_Cmd(spi_dev->cfg->dma.tx.channel, ENABLE); + /* Make sure to flush out the receive buffer */ + (void)SPI_I2S_ReceiveData(spi_dev->cfg->regs); - /* Reenable the SPI device */ - SPI_Cmd(spi_dev->cfg->regs, ENABLE); + if (spi_dev->cfg->use_crc) { + /* Need a 0->1 transition to reset the CRC logic */ + SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); + } - if (callback) { - /* User has requested a callback, don't wait for the transfer to complete. */ - return 0; - } + /* Start DMA transfers */ + DMA_Cmd(spi_dev->cfg->dma.rx.channel, ENABLE); + DMA_Cmd(spi_dev->cfg->dma.tx.channel, ENABLE); - /* Wait until all bytes have been transmitted/received */ - while (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)); + /* Reenable the SPI device */ + SPI_Cmd(spi_dev->cfg->regs, ENABLE); - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))); + if (callback) { + /* User has requested a callback, don't wait for the transfer to complete. */ + return 0; + } - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)); + /* Wait until all bytes have been transmitted/received */ + while (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { + ; + } - /* Check the CRC on the transfer if enabled. */ - if (spi_dev->cfg->use_crc) { - /* Check the SPI CRC error flag */ - if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { - return -4; - } - } + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))) { + ; + } - /* No error */ - return 0; + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + ; + } + + /* Check the CRC on the transfer if enabled. */ + if (spi_dev->cfg->use_crc) { + /* Check the SPI CRC error flag */ + if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { + return -4; + } + } + + /* No error */ + return 0; } /** -* Transfers a block of bytes via PIO. -* -* \param[in] spi_id SPI device handle -* \param[in] send_buffer pointer to buffer which should be sent.
-* If NULL, 0xff (all-one) will be sent. -* \param[in] receive_buffer pointer to buffer which should get the received values.
-* If NULL, received bytes will be discarded. -* \param[in] len number of bytes which should be transfered -* \return >= 0 if no error during transfer -* \return -1 if disabled SPI port selected -* \return -3 if function has been called during an ongoing DMA transfer -*/ + * Transfers a block of bytes via PIO. + * + * \param[in] spi_id SPI device handle + * \param[in] send_buffer pointer to buffer which should be sent.
+ * If NULL, 0xff (all-one) will be sent. + * \param[in] receive_buffer pointer to buffer which should get the received values.
+ * If NULL, received bytes will be discarded. + * \param[in] len number of bytes which should be transfered + * \return >= 0 if no error during transfer + * \return -1 if disabled SPI port selected + * \return -3 if function has been called during an ongoing DMA transfer + */ static int32_t SPI_PIO_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; - uint8_t b; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + uint8_t b; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - /* Exit if ongoing transfer */ - if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { - return -3; - } + PIOS_Assert(valid) - /* Make sure the RXNE flag is cleared by reading the DR register */ - b = spi_dev->cfg->regs->DR; + /* Exit if ongoing transfer */ + if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { + return -3; + } - while (len--) { - /* get the byte to send */ - b = send_buffer ? *(send_buffer++) : 0xff; + /* Make sure the RXNE flag is cleared by reading the DR register */ + b = spi_dev->cfg->regs->DR; - /* Start the transfer */ - spi_dev->cfg->regs->DR = b; + while (len--) { + /* get the byte to send */ + b = send_buffer ? *(send_buffer++) : 0xff; - /* Wait until there is a byte to read */ - while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_RXNE)) ; + /* Start the transfer */ + spi_dev->cfg->regs->DR = b; - /* Read the rx'd byte */ - b = spi_dev->cfg->regs->DR; + /* Wait until there is a byte to read */ + while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_RXNE)) { + ; + } - /* save the received byte */ - if (receive_buffer) - *(receive_buffer++) = b; + /* Read the rx'd byte */ + b = spi_dev->cfg->regs->DR; - /* Wait until the TXE goes high */ - while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) ; - } + /* save the received byte */ + if (receive_buffer) { + *(receive_buffer++) = b; + } - /* Wait for SPI transfer to have fully completed */ - while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) ; + /* Wait until the TXE goes high */ + while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) { + ; + } + } - return 0; + /* Wait for SPI transfer to have fully completed */ + while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) { + ; + } + + return 0; } /** -* Transfers a block of bytes via PIO or DMA. -* \param[in] spi_id SPI device handle -* \param[in] send_buffer pointer to buffer which should be sent.
-* If NULL, 0xff (all-one) will be sent. -* \param[in] receive_buffer pointer to buffer which should get the received values.
-* If NULL, received bytes will be discarded. -* \param[in] len number of bytes which should be transfered -* \param[in] callback pointer to callback function which will be executed -* from DMA channel interrupt once the transfer is finished. -* If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will -* block until the transfer is finished. -* \return >= 0 if no error during transfer -* \return -1 if disabled SPI port selected -* \return -3 if function has been called during an ongoing DMA transfer -*/ + * Transfers a block of bytes via PIO or DMA. + * \param[in] spi_id SPI device handle + * \param[in] send_buffer pointer to buffer which should be sent.
+ * If NULL, 0xff (all-one) will be sent. + * \param[in] receive_buffer pointer to buffer which should get the received values.
+ * If NULL, received bytes will be discarded. + * \param[in] len number of bytes which should be transfered + * \param[in] callback pointer to callback function which will be executed + * from DMA channel interrupt once the transfer is finished. + * If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will + * block until the transfer is finished. + * \return >= 0 if no error during transfer + * \return -1 if disabled SPI port selected + * \return -3 if function has been called during an ongoing DMA transfer + */ int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback) { - if (callback || len > SPI_MAX_BLOCK_PIO) { - return SPI_DMA_TransferBlock(spi_id, send_buffer, receive_buffer, len, callback); - } - return SPI_PIO_TransferBlock(spi_id, send_buffer, receive_buffer, len); + if (callback || len > SPI_MAX_BLOCK_PIO) { + return SPI_DMA_TransferBlock(spi_id, send_buffer, receive_buffer, len, callback); + } + return SPI_PIO_TransferBlock(spi_id, send_buffer, receive_buffer, len); } /** -* Check if a transfer is in progress -* \param[in] spi SPI number (0 or 1) -* \return >= 0 if no transfer is in progress -* \return -1 if disabled SPI port selected -* \return -2 if unsupported SPI port selected -* \return -3 if function has been called during an ongoing DMA transfer -*/ + * Check if a transfer is in progress + * \param[in] spi SPI number (0 or 1) + * \return >= 0 if no transfer is in progress + * \return -1 if disabled SPI port selected + * \return -2 if unsupported SPI port selected + * \return -3 if function has been called during an ongoing DMA transfer + */ int32_t PIOS_SPI_Busy(uint32_t spi_id) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) + bool valid = PIOS_SPI_validate(spi_dev); - /* DMA buffer has data or SPI transmit register not empty or SPI is busy*/ - if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel) || - !SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE) || - SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) - { - return -3; - } + PIOS_Assert(valid) - return(0); + /* DMA buffer has data or SPI transmit register not empty or SPI is busy*/ + if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel) || + !SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE) || + SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + return -3; + } + + return 0; } void PIOS_SPI_IRQ_Handler(uint32_t spi_id) { - struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id; + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; - bool valid = PIOS_SPI_validate(spi_dev); - PIOS_Assert(valid) - - // FIXME XXX Only RX channel or better clear flags for both channels? - DMA_ClearFlag(spi_dev->cfg->dma.rx.channel, spi_dev->cfg->dma.irq.flags); - - if(spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) { - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))) ; + bool valid = PIOS_SPI_validate(spi_dev); - /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ - while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) ; - } + PIOS_Assert(valid) - if (spi_dev->callback != NULL) { - bool crc_ok = true; - uint8_t crc_val; + // FIXME XXX Only RX channel or better clear flags for both channels? + DMA_ClearFlag(spi_dev->cfg->dma.rx.channel, spi_dev->cfg->dma.irq.flags); - if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { - crc_ok = false; - SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); - } - crc_val = SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); - spi_dev->callback(crc_ok, crc_val); - } + if (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) { + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))) { + ; + } + + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + ; + } + } + + if (spi_dev->callback != NULL) { + bool crc_ok = true; + uint8_t crc_val; + + if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { + crc_ok = false; + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + } + crc_val = SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); + spi_dev->callback(crc_ok, crc_val); + } } #endif /* PIOS_INCLUDE_SPI */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_sys.c b/flight/pios/stm32f4xx/pios_sys.c index 7819513a3..79b17fc7f 100644 --- a/flight/pios/stm32f4xx/pios_sys.c +++ b/flight/pios/stm32f4xx/pios_sys.c @@ -6,26 +6,26 @@ * @brief PIOS System Initialization code * @{ * - * @file pios_sys.c + * @file pios_sys.c * @author Michael Smith Copyright (C) 2011 - * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Sets up basic STM32 system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,9 +38,9 @@ void NVIC_Configuration(void); void SysTick_Handler(void); /* Local Macros */ -#define MEM8(addr) (*((volatile uint8_t *)(addr))) -#define MEM16(addr) (*((volatile uint16_t *)(addr))) -#define MEM32(addr) (*((volatile uint32_t *)(addr))) +#define MEM8(addr) (*((volatile uint8_t *)(addr))) +#define MEM16(addr) (*((volatile uint16_t *)(addr))) +#define MEM32(addr) (*((volatile uint32_t *)(addr))) /** * Initialises all system peripherals @@ -69,81 +69,81 @@ void PIOS_SYS_Init(void) * light up everything we might reasonably use here and just leave it on. */ RCC_AHB1PeriphClockCmd( - RCC_AHB1Periph_GPIOA | - RCC_AHB1Periph_GPIOB | - RCC_AHB1Periph_GPIOC | - RCC_AHB1Periph_GPIOD | - RCC_AHB1Periph_GPIOE | - RCC_AHB1Periph_GPIOF | - RCC_AHB1Periph_GPIOG | - RCC_AHB1Periph_GPIOH | - RCC_AHB1Periph_GPIOI | - RCC_AHB1Periph_CRC | - RCC_AHB1Periph_FLITF | - RCC_AHB1Periph_SRAM1 | - RCC_AHB1Periph_SRAM2 | - RCC_AHB1Periph_BKPSRAM | - RCC_AHB1Periph_DMA1 | - RCC_AHB1Periph_DMA2 | - //RCC_AHB1Periph_ETH_MAC | No ethernet - //RCC_AHB1Periph_ETH_MAC_Tx | - //RCC_AHB1Periph_ETH_MAC_Rx | - //RCC_AHB1Periph_ETH_MAC_PTP | - //RCC_AHB1Periph_OTG_HS | No high-speed USB (requires external PHY) - //RCC_AHB1Periph_OTG_HS_ULPI | No ULPI PHY (see above) - 0, ENABLE); + RCC_AHB1Periph_GPIOA | + RCC_AHB1Periph_GPIOB | + RCC_AHB1Periph_GPIOC | + RCC_AHB1Periph_GPIOD | + RCC_AHB1Periph_GPIOE | + RCC_AHB1Periph_GPIOF | + RCC_AHB1Periph_GPIOG | + RCC_AHB1Periph_GPIOH | + RCC_AHB1Periph_GPIOI | + RCC_AHB1Periph_CRC | + RCC_AHB1Periph_FLITF | + RCC_AHB1Periph_SRAM1 | + RCC_AHB1Periph_SRAM2 | + RCC_AHB1Periph_BKPSRAM | + RCC_AHB1Periph_DMA1 | + RCC_AHB1Periph_DMA2 | + // RCC_AHB1Periph_ETH_MAC | No ethernet + // RCC_AHB1Periph_ETH_MAC_Tx | + // RCC_AHB1Periph_ETH_MAC_Rx | + // RCC_AHB1Periph_ETH_MAC_PTP | + // RCC_AHB1Periph_OTG_HS | No high-speed USB (requires external PHY) + // RCC_AHB1Periph_OTG_HS_ULPI | No ULPI PHY (see above) + 0, ENABLE); RCC_AHB2PeriphClockCmd( - //RCC_AHB2Periph_DCMI | No camera @todo might make sense later for basic vision support? - //RCC_AHB2Periph_CRYP | No crypto - //RCC_AHB2Periph_HASH | No hash generator - //RCC_AHB2Periph_RNG | No random numbers @todo might be good to have later if entropy is desired - //RCC_AHB2Periph_OTG_FS | - 0, ENABLE); + // RCC_AHB2Periph_DCMI | No camera @todo might make sense later for basic vision support? + // RCC_AHB2Periph_CRYP | No crypto + // RCC_AHB2Periph_HASH | No hash generator + // RCC_AHB2Periph_RNG | No random numbers @todo might be good to have later if entropy is desired + // RCC_AHB2Periph_OTG_FS | + 0, ENABLE); RCC_AHB3PeriphClockCmd( - //RCC_AHB3Periph_FSMC | No external static memory - 0, ENABLE); + // RCC_AHB3Periph_FSMC | No external static memory + 0, ENABLE); RCC_APB1PeriphClockCmd( - RCC_APB1Periph_TIM2 | - RCC_APB1Periph_TIM3 | - RCC_APB1Periph_TIM4 | - RCC_APB1Periph_TIM5 | - RCC_APB1Periph_TIM6 | - RCC_APB1Periph_TIM7 | - RCC_APB1Periph_TIM12 | - RCC_APB1Periph_TIM13 | - RCC_APB1Periph_TIM14 | - RCC_APB1Periph_WWDG | - RCC_APB1Periph_SPI2 | - RCC_APB1Periph_SPI3 | - RCC_APB1Periph_USART2 | - RCC_APB1Periph_USART3 | - RCC_APB1Periph_UART4 | - RCC_APB1Periph_UART5 | - RCC_APB1Periph_I2C1 | - RCC_APB1Periph_I2C2 | - RCC_APB1Periph_I2C3 | - RCC_APB1Periph_CAN1 | - RCC_APB1Periph_CAN2 | - RCC_APB1Periph_PWR | - RCC_APB1Periph_DAC | - 0, ENABLE); + RCC_APB1Periph_TIM2 | + RCC_APB1Periph_TIM3 | + RCC_APB1Periph_TIM4 | + RCC_APB1Periph_TIM5 | + RCC_APB1Periph_TIM6 | + RCC_APB1Periph_TIM7 | + RCC_APB1Periph_TIM12 | + RCC_APB1Periph_TIM13 | + RCC_APB1Periph_TIM14 | + RCC_APB1Periph_WWDG | + RCC_APB1Periph_SPI2 | + RCC_APB1Periph_SPI3 | + RCC_APB1Periph_USART2 | + RCC_APB1Periph_USART3 | + RCC_APB1Periph_UART4 | + RCC_APB1Periph_UART5 | + RCC_APB1Periph_I2C1 | + RCC_APB1Periph_I2C2 | + RCC_APB1Periph_I2C3 | + RCC_APB1Periph_CAN1 | + RCC_APB1Periph_CAN2 | + RCC_APB1Periph_PWR | + RCC_APB1Periph_DAC | + 0, ENABLE); RCC_APB2PeriphClockCmd( - RCC_APB2Periph_TIM1 | - RCC_APB2Periph_TIM8 | - RCC_APB2Periph_USART1 | - RCC_APB2Periph_USART6 | - RCC_APB2Periph_ADC | - RCC_APB2Periph_ADC1 | - RCC_APB2Periph_ADC2 | - RCC_APB2Periph_ADC3 | - RCC_APB2Periph_SDIO | - RCC_APB2Periph_SPI1 | - RCC_APB2Periph_SYSCFG | - RCC_APB2Periph_TIM9 | - RCC_APB2Periph_TIM10 | - RCC_APB2Periph_TIM11 | - 0, ENABLE); + RCC_APB2Periph_TIM1 | + RCC_APB2Periph_TIM8 | + RCC_APB2Periph_USART1 | + RCC_APB2Periph_USART6 | + RCC_APB2Periph_ADC | + RCC_APB2Periph_ADC1 | + RCC_APB2Periph_ADC2 | + RCC_APB2Periph_ADC3 | + RCC_APB2Periph_SDIO | + RCC_APB2Periph_SPI1 | + RCC_APB2Periph_SYSCFG | + RCC_APB2Periph_TIM9 | + RCC_APB2Periph_TIM10 | + RCC_APB2Periph_TIM11 | + 0, ENABLE); /* * Configure all pins as input / pullup to avoid issues with @@ -152,20 +152,20 @@ void PIOS_SYS_Init(void) */ GPIO_InitTypeDef GPIO_InitStructure; GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; // default is un-pulled input + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; // default is un-pulled input - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; #if (PIOS_USB_ENABLED) - GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_11 | GPIO_Pin_12); // leave USB D+/D- alone + GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_11 | GPIO_Pin_12); // leave USB D+/D- alone #endif - GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15); // leave JTAG pins alone + GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15); // leave JTAG pins alone GPIO_Init(GPIOA, &GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; - GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_3 | GPIO_Pin_4);// leave JTAG pins alone + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; + GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_3 | GPIO_Pin_4); // leave JTAG pins alone GPIO_Init(GPIOB, &GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; GPIO_Init(GPIOC, &GPIO_InitStructure); GPIO_Init(GPIOD, &GPIO_InitStructure); GPIO_Init(GPIOE, &GPIO_InitStructure); @@ -199,17 +199,19 @@ int32_t PIOS_SYS_Reset(void) // turn off all board LEDs #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Off(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ +#endif /* PIOS_LED_HEARTBEAT */ #if defined(PIOS_LED_ALARM) PIOS_LED_Off(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ +#endif /* PIOS_LED_ALARM */ /* XXX F10x port resets most (but not all) peripherals ... do we care? */ /* Reset STM32 */ NVIC_SystemReset(); - while (1); + while (1) { + ; + } /* We will never reach this point */ return -1; @@ -220,7 +222,7 @@ int32_t PIOS_SYS_Reset(void) */ uint32_t PIOS_SYS_getCPUFlashSize(void) { - return ((uint32_t) MEM16(0x1fff7a22) * 1024); // it might be possible to locate in the OTP area, but haven't looked and not documented + return (uint32_t)MEM16(0x1fff7a22) * 1024; // it might be possible to locate in the OTP area, but haven't looked and not documented } /** @@ -257,8 +259,9 @@ int32_t PIOS_SYS_SerialNumberGet(char *str) /* Stored in the so called "electronic signature" */ for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { uint8_t b = MEM8(0x1fff7a10 + (i / 2)); - if (!(i & 1)) - b >>= 4; + if (!(i & 1)) { + b >>= 4; + } b &= 0x0f; str[i] = ((b > 9) ? ('A' - 10) : '0') + b; @@ -276,6 +279,7 @@ void NVIC_Configuration(void) { /* Set the Vector Table base address as specified in .ld file */ extern void *pios_isr_vector_table_base; + NVIC_SetVectorTable((uint32_t)&pios_isr_vector_table_base, 0x0); /* 4 bits for Interrupt priorities so no sub priorities */ @@ -293,7 +297,7 @@ void NVIC_Configuration(void) * \param[in] line assert_param error line source number * \retval None */ -void assert_failed(uint8_t * file, uint32_t line) +void assert_failed(uint8_t *file, uint32_t line) { /* When serial debugging is implemented, use something like this. */ /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ @@ -301,23 +305,25 @@ void assert_failed(uint8_t * file, uint32_t line) /* Setup the LEDs to Alternate */ #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_On(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ +#endif /* PIOS_LED_HEARTBEAT */ #if defined(PIOS_LED_ALARM) PIOS_LED_Off(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ +#endif /* PIOS_LED_ALARM */ /* Infinite loop */ while (1) { #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ +#endif /* PIOS_LED_HEARTBEAT */ #if defined(PIOS_LED_ALARM) PIOS_LED_Toggle(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ - for (int i = 0; i < 1000000; i++); +#endif /* PIOS_LED_ALARM */ + for (int i = 0; i < 1000000; i++) { + ; + } } } -#endif +#endif /* ifdef USE_FULL_ASSERT */ #endif /* PIOS_INCLUDE_SYS */ diff --git a/flight/pios/stm32f4xx/pios_tim.c b/flight/pios/stm32f4xx/pios_tim.c index 4f3828d68..ec72a9c6f 100644 --- a/flight/pios/stm32f4xx/pios_tim.c +++ b/flight/pios/stm32f4xx/pios_tim.c @@ -6,25 +6,25 @@ * @brief PIOS Timer control code * @{ * - * @file pios_tim.c + * @file pios_tim.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Sets up timers and ways to register callbacks on them * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,296 +36,297 @@ #include "pios_tim_priv.h" enum pios_tim_dev_magic { - PIOS_TIM_DEV_MAGIC = 0x87654098, + PIOS_TIM_DEV_MAGIC = 0x87654098, }; struct pios_tim_dev { - enum pios_tim_dev_magic magic; + enum pios_tim_dev_magic magic; - const struct pios_tim_channel * channels; - uint8_t num_channels; + const struct pios_tim_channel *channels; + uint8_t num_channels; - const struct pios_tim_callbacks * callbacks; - uint32_t context; + const struct pios_tim_callbacks *callbacks; + uint32_t context; }; #define PIOS_TIM_ALL_FLAGS TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4 | TIM_IT_Trigger | TIM_IT_Update static struct pios_tim_dev pios_tim_devs[PIOS_TIM_MAX_DEVS]; static uint8_t pios_tim_num_devs; -static struct pios_tim_dev * PIOS_TIM_alloc(void) +static struct pios_tim_dev *PIOS_TIM_alloc(void) { - struct pios_tim_dev * tim_dev; + struct pios_tim_dev *tim_dev; - if (pios_tim_num_devs >= PIOS_TIM_MAX_DEVS) { - return (NULL); - } + if (pios_tim_num_devs >= PIOS_TIM_MAX_DEVS) { + return NULL; + } - tim_dev = &pios_tim_devs[pios_tim_num_devs++]; - tim_dev->magic = PIOS_TIM_DEV_MAGIC; + tim_dev = &pios_tim_devs[pios_tim_num_devs++]; + tim_dev->magic = PIOS_TIM_DEV_MAGIC; - return (tim_dev); + return tim_dev; } - -int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg * cfg) +int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg *cfg) { - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(cfg); - /* Enable appropriate clock to timer module */ - switch((uint32_t) cfg->timer) { - case (uint32_t)TIM1: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); - break; - case (uint32_t)TIM2: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); - break; - case (uint32_t)TIM3: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); - break; - case (uint32_t)TIM4: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); - break; - case (uint32_t)TIM5: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); - break; - case (uint32_t)TIM6: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); - break; - case (uint32_t)TIM7: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); - break; - case (uint32_t)TIM8: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); - break; - case (uint32_t)TIM9: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM9, ENABLE); - break; - case (uint32_t)TIM10: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM10, ENABLE); - break; - case (uint32_t)TIM11: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM11, ENABLE); - break; - case (uint32_t)TIM12: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12, ENABLE); - break; - case (uint32_t)TIM13: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM13, ENABLE); - break; - case (uint32_t)TIM14: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM14, ENABLE); - break; - } + /* Enable appropriate clock to timer module */ + switch ((uint32_t)cfg->timer) { + case (uint32_t)TIM1: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); + break; + case (uint32_t)TIM2: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); + break; + case (uint32_t)TIM3: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); + break; + case (uint32_t)TIM4: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); + break; + case (uint32_t)TIM5: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); + break; + case (uint32_t)TIM6: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); + break; + case (uint32_t)TIM7: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); + break; + case (uint32_t)TIM8: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); + break; + case (uint32_t)TIM9: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM9, ENABLE); + break; + case (uint32_t)TIM10: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM10, ENABLE); + break; + case (uint32_t)TIM11: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM11, ENABLE); + break; + case (uint32_t)TIM12: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12, ENABLE); + break; + case (uint32_t)TIM13: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM13, ENABLE); + break; + case (uint32_t)TIM14: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM14, ENABLE); + break; + } - /* Configure the dividers for this timer */ - TIM_TimeBaseInit(cfg->timer, cfg->time_base_init); + /* Configure the dividers for this timer */ + TIM_TimeBaseInit(cfg->timer, cfg->time_base_init); - /* Configure internal timer clocks */ - TIM_InternalClockConfig(cfg->timer); + /* Configure internal timer clocks */ + TIM_InternalClockConfig(cfg->timer); - /* Enable timers */ - TIM_Cmd(cfg->timer, ENABLE); + /* Enable timers */ + TIM_Cmd(cfg->timer, ENABLE); - /* Enable Interrupts */ - NVIC_Init(&cfg->irq.init); + /* Enable Interrupts */ + NVIC_Init(&cfg->irq.init); - return 0; + return 0; } -int32_t PIOS_TIM_InitChannels(uint32_t * tim_id, const struct pios_tim_channel * channels, uint8_t num_channels, const struct pios_tim_callbacks * callbacks, uint32_t context) +int32_t PIOS_TIM_InitChannels(uint32_t *tim_id, const struct pios_tim_channel *channels, uint8_t num_channels, const struct pios_tim_callbacks *callbacks, uint32_t context) { - PIOS_Assert(channels); - PIOS_Assert(num_channels); + PIOS_Assert(channels); + PIOS_Assert(num_channels); - struct pios_tim_dev * tim_dev; - tim_dev = (struct pios_tim_dev *) PIOS_TIM_alloc(); - if (!tim_dev) goto out_fail; + struct pios_tim_dev *tim_dev; + tim_dev = (struct pios_tim_dev *)PIOS_TIM_alloc(); + if (!tim_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - tim_dev->channels = channels; - tim_dev->num_channels = num_channels; - tim_dev->callbacks = callbacks; - tim_dev->context = context; + /* Bind the configuration to the device instance */ + tim_dev->channels = channels; + tim_dev->num_channels = num_channels; + tim_dev->callbacks = callbacks; + tim_dev->context = context; - /* Configure the pins */ - for (uint8_t i = 0; i < num_channels; i++) { - const struct pios_tim_channel * chan = &(channels[i]); + /* Configure the pins */ + for (uint8_t i = 0; i < num_channels; i++) { + const struct pios_tim_channel *chan = &(channels[i]); - /* Enable the peripheral clock for the GPIO */ + /* Enable the peripheral clock for the GPIO */ /* switch ((uint32_t)chan->pin.gpio) { - case (uint32_t) GPIOA: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); - break; - case (uint32_t) GPIOB: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - break; - case (uint32_t) GPIOC: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - break; - default: - PIOS_Assert(0); - break; - } -*/ // commented out for now as f4 starts all clocks - GPIO_Init(chan->pin.gpio, &chan->pin.init); + case (uint32_t) GPIOA: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); + break; + case (uint32_t) GPIOB: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + break; + case (uint32_t) GPIOC: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + break; + default: + PIOS_Assert(0); + break; + } + */ // commented out for now as f4 starts all clocks + GPIO_Init(chan->pin.gpio, &chan->pin.init); - PIOS_DEBUG_Assert(chan->remaP); - - // Second parameter should technically be PinSource but they are numerically the same - GPIO_PinAFConfig(chan->pin.gpio, chan->pin.pin_source,chan->remap); - } + PIOS_DEBUG_Assert(chan->remaP); - *tim_id = (uint32_t)tim_dev; + // Second parameter should technically be PinSource but they are numerically the same + GPIO_PinAFConfig(chan->pin.gpio, chan->pin.pin_source, chan->remap); + } - return(0); + *tim_id = (uint32_t)tim_dev; + + return 0; out_fail: - return(-1); + return -1; } -static void PIOS_TIM_generic_irq_handler(TIM_TypeDef * timer) +static void PIOS_TIM_generic_irq_handler(TIM_TypeDef *timer) { - /* Iterate over all registered clients of the TIM layer to find channels on this timer */ - for (uint8_t i = 0; i < pios_tim_num_devs; i++) { - const struct pios_tim_dev * tim_dev = &pios_tim_devs[i]; + /* Iterate over all registered clients of the TIM layer to find channels on this timer */ + for (uint8_t i = 0; i < pios_tim_num_devs; i++) { + const struct pios_tim_dev *tim_dev = &pios_tim_devs[i]; - if (!tim_dev->channels || tim_dev->num_channels == 0) { - /* No channels to process on this client */ - continue; - } + if (!tim_dev->channels || tim_dev->num_channels == 0) { + /* No channels to process on this client */ + continue; + } - /* Check for an overflow event on this timer */ - bool overflow_event; - uint16_t overflow_count; - if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) { - TIM_ClearITPendingBit(timer, TIM_IT_Update); - overflow_count = timer->ARR; - overflow_event = true; - } else { - overflow_count = 0; - overflow_event = false; - } + /* Check for an overflow event on this timer */ + bool overflow_event; + uint16_t overflow_count; + if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) { + TIM_ClearITPendingBit(timer, TIM_IT_Update); + overflow_count = timer->ARR; + overflow_event = true; + } else { + overflow_count = 0; + overflow_event = false; + } - for (uint8_t j = 0; j < tim_dev->num_channels; j++) { - const struct pios_tim_channel * chan = &tim_dev->channels[j]; + for (uint8_t j = 0; j < tim_dev->num_channels; j++) { + const struct pios_tim_channel *chan = &tim_dev->channels[j]; - if (chan->timer != timer) { - /* channel is not on this timer */ - continue; - } + if (chan->timer != timer) { + /* channel is not on this timer */ + continue; + } - /* Figure out which interrupt bit we should be looking at */ - uint16_t timer_it; - switch (chan->timer_chan) { - case TIM_Channel_1: - timer_it = TIM_IT_CC1; - break; - case TIM_Channel_2: - timer_it = TIM_IT_CC2; - break; - case TIM_Channel_3: - timer_it = TIM_IT_CC3; - break; - case TIM_Channel_4: - timer_it = TIM_IT_CC4; - break; - default: - PIOS_Assert(0); - break; - } + /* Figure out which interrupt bit we should be looking at */ + uint16_t timer_it; + switch (chan->timer_chan) { + case TIM_Channel_1: + timer_it = TIM_IT_CC1; + break; + case TIM_Channel_2: + timer_it = TIM_IT_CC2; + break; + case TIM_Channel_3: + timer_it = TIM_IT_CC3; + break; + case TIM_Channel_4: + timer_it = TIM_IT_CC4; + break; + default: + PIOS_Assert(0); + break; + } - bool edge_event; - uint16_t edge_count; - if (TIM_GetITStatus(chan->timer, timer_it) == SET) { - TIM_ClearITPendingBit(chan->timer, timer_it); + bool edge_event; + uint16_t edge_count; + if (TIM_GetITStatus(chan->timer, timer_it) == SET) { + TIM_ClearITPendingBit(chan->timer, timer_it); - /* Read the current counter */ - switch(chan->timer_chan) { - case TIM_Channel_1: - edge_count = TIM_GetCapture1(chan->timer); - break; - case TIM_Channel_2: - edge_count = TIM_GetCapture2(chan->timer); - break; - case TIM_Channel_3: - edge_count = TIM_GetCapture3(chan->timer); - break; - case TIM_Channel_4: - edge_count = TIM_GetCapture4(chan->timer); - break; - default: - PIOS_Assert(0); - break; - } - edge_event = true; - } else { - edge_event = false; - edge_count = 0; - } + /* Read the current counter */ + switch (chan->timer_chan) { + case TIM_Channel_1: + edge_count = TIM_GetCapture1(chan->timer); + break; + case TIM_Channel_2: + edge_count = TIM_GetCapture2(chan->timer); + break; + case TIM_Channel_3: + edge_count = TIM_GetCapture3(chan->timer); + break; + case TIM_Channel_4: + edge_count = TIM_GetCapture4(chan->timer); + break; + default: + PIOS_Assert(0); + break; + } + edge_event = true; + } else { + edge_event = false; + edge_count = 0; + } - if (!tim_dev->callbacks) { - /* No callbacks registered, we're done with this channel */ - continue; - } + if (!tim_dev->callbacks) { + /* No callbacks registered, we're done with this channel */ + continue; + } - /* Generate the appropriate callbacks */ - if (overflow_event & edge_event) { - /* - * When both edge and overflow happen in the same interrupt, we - * need a heuristic to determine the order of the edge and overflow - * events so that the callbacks happen in the right order. If we - * get the order wrong, our pulse width calculations could be off by up - * to ARR ticks. That could be bad. - * - * Heuristic: If the edge_count is < 16 ticks above zero then we assume the - * edge happened just after the overflow. - */ + /* Generate the appropriate callbacks */ + if (overflow_event & edge_event) { + /* + * When both edge and overflow happen in the same interrupt, we + * need a heuristic to determine the order of the edge and overflow + * events so that the callbacks happen in the right order. If we + * get the order wrong, our pulse width calculations could be off by up + * to ARR ticks. That could be bad. + * + * Heuristic: If the edge_count is < 16 ticks above zero then we assume the + * edge happened just after the overflow. + */ - if (edge_count < 16) { - /* Call the overflow callback first */ - if (tim_dev->callbacks->overflow) { - (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, - tim_dev->context, - j, - overflow_count); - } - /* Call the edge callback second */ - if (tim_dev->callbacks->edge) { - (*tim_dev->callbacks->edge)((uint32_t)tim_dev, - tim_dev->context, - j, - edge_count); - } - } else { - /* Call the edge callback first */ - if (tim_dev->callbacks->edge) { - (*tim_dev->callbacks->edge)((uint32_t)tim_dev, - tim_dev->context, - j, - edge_count); - } - /* Call the overflow callback second */ - if (tim_dev->callbacks->overflow) { - (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, - tim_dev->context, - j, - overflow_count); - } - } - } else if (overflow_event && tim_dev->callbacks->overflow) { - (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, - tim_dev->context, - j, - overflow_count); - } else if (edge_event && tim_dev->callbacks->edge) { - (*tim_dev->callbacks->edge)((uint32_t)tim_dev, - tim_dev->context, - j, - edge_count); - } - } - } + if (edge_count < 16) { + /* Call the overflow callback first */ + if (tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } + /* Call the edge callback second */ + if (tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + } else { + /* Call the edge callback first */ + if (tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + /* Call the overflow callback second */ + if (tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } + } + } else if (overflow_event && tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } else if (edge_event && tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + } + } } /* Bind Interrupt Handlers @@ -333,137 +334,119 @@ static void PIOS_TIM_generic_irq_handler(TIM_TypeDef * timer) * Map all valid TIM IRQs to the common interrupt handler * and give it enough context to properly demux the various timers */ -void TIM1_CC_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_1_CC_irq_handler"))); -static void PIOS_TIM_1_CC_irq_handler (void) +void TIM1_CC_IRQHandler(void) __attribute__((alias("PIOS_TIM_1_CC_irq_handler"))); +static void PIOS_TIM_1_CC_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM1); + PIOS_TIM_generic_irq_handler(TIM1); } -void TIM2_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_2_irq_handler"))); -static void PIOS_TIM_2_irq_handler (void) +void TIM2_IRQHandler(void) __attribute__((alias("PIOS_TIM_2_irq_handler"))); +static void PIOS_TIM_2_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM2); + PIOS_TIM_generic_irq_handler(TIM2); } -void TIM3_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_3_irq_handler"))); -static void PIOS_TIM_3_irq_handler (void) +void TIM3_IRQHandler(void) __attribute__((alias("PIOS_TIM_3_irq_handler"))); +static void PIOS_TIM_3_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM3); + PIOS_TIM_generic_irq_handler(TIM3); } -void TIM4_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_4_irq_handler"))); -static void PIOS_TIM_4_irq_handler (void) +void TIM4_IRQHandler(void) __attribute__((alias("PIOS_TIM_4_irq_handler"))); +static void PIOS_TIM_4_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM4); + PIOS_TIM_generic_irq_handler(TIM4); } -void TIM5_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_5_irq_handler"))); -static void PIOS_TIM_5_irq_handler (void) +void TIM5_IRQHandler(void) __attribute__((alias("PIOS_TIM_5_irq_handler"))); +static void PIOS_TIM_5_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM5); + PIOS_TIM_generic_irq_handler(TIM5); } -void TIM6_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_6_irq_handler"))); -static void PIOS_TIM_6_irq_handler (void) +void TIM6_IRQHandler(void) __attribute__((alias("PIOS_TIM_6_irq_handler"))); +static void PIOS_TIM_6_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM6); + PIOS_TIM_generic_irq_handler(TIM6); } -void TIM7_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_7_irq_handler"))); -static void PIOS_TIM_7_irq_handler (void) +void TIM7_IRQHandler(void) __attribute__((alias("PIOS_TIM_7_irq_handler"))); +static void PIOS_TIM_7_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM7); + PIOS_TIM_generic_irq_handler(TIM7); } -void TIM8_UP_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_8_UP_irq_handler"))); -static void PIOS_TIM_8_UP_irq_handler (void) +void TIM8_UP_IRQHandler(void) __attribute__((alias("PIOS_TIM_8_UP_irq_handler"))); +static void PIOS_TIM_8_UP_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM8); + PIOS_TIM_generic_irq_handler(TIM8); } -void TIM8_CC_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_8_CC_irq_handler"))); -static void PIOS_TIM_8_CC_irq_handler (void) +void TIM8_CC_IRQHandler(void) __attribute__((alias("PIOS_TIM_8_CC_irq_handler"))); +static void PIOS_TIM_8_CC_irq_handler(void) { - PIOS_TIM_generic_irq_handler (TIM8); + PIOS_TIM_generic_irq_handler(TIM8); } // The rest of TIM1 interrupts are overlapped -void TIM1_BRK_TIM9_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_9_CC_irq_handler"))); -static void PIOS_TIM_9_CC_irq_handler (void) +void TIM1_BRK_TIM9_IRQHandler(void) __attribute__((alias("PIOS_TIM_9_CC_irq_handler"))); +static void PIOS_TIM_9_CC_irq_handler(void) { - if (TIM_GetITStatus(TIM1, TIM_IT_Break)) - { - PIOS_TIM_generic_irq_handler(TIM1); - } - else if (TIM_GetITStatus(TIM9, PIOS_TIM_ALL_FLAGS)) - { - PIOS_TIM_generic_irq_handler (TIM9); - } + if (TIM_GetITStatus(TIM1, TIM_IT_Break)) { + PIOS_TIM_generic_irq_handler(TIM1); + } else if (TIM_GetITStatus(TIM9, PIOS_TIM_ALL_FLAGS)) { + PIOS_TIM_generic_irq_handler(TIM9); + } } -void TIM1_UP_TIM10_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_10_CC_irq_handler"))); -static void PIOS_TIM_10_CC_irq_handler (void) +void TIM1_UP_TIM10_IRQHandler(void) __attribute__((alias("PIOS_TIM_10_CC_irq_handler"))); +static void PIOS_TIM_10_CC_irq_handler(void) { - if (TIM_GetITStatus(TIM1, TIM_IT_Update)) - { - PIOS_TIM_generic_irq_handler(TIM1); - } - else if (TIM_GetITStatus(TIM10, PIOS_TIM_ALL_FLAGS)) - { - PIOS_TIM_generic_irq_handler (TIM10); - } + if (TIM_GetITStatus(TIM1, TIM_IT_Update)) { + PIOS_TIM_generic_irq_handler(TIM1); + } else if (TIM_GetITStatus(TIM10, PIOS_TIM_ALL_FLAGS)) { + PIOS_TIM_generic_irq_handler(TIM10); + } } -void TIM1_TRG_COM_TIM11_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_11_CC_irq_handler"))); -static void PIOS_TIM_11_CC_irq_handler (void) +void TIM1_TRG_COM_TIM11_IRQHandler(void) __attribute__((alias("PIOS_TIM_11_CC_irq_handler"))); +static void PIOS_TIM_11_CC_irq_handler(void) { - if(TIM_GetITStatus(TIM1, TIM_IT_COM | TIM_IT_Trigger)) - { - PIOS_TIM_generic_irq_handler (TIM1); - } - else if (TIM_GetITStatus(TIM11, PIOS_TIM_ALL_FLAGS)) - { - PIOS_TIM_generic_irq_handler (TIM11); - } + if (TIM_GetITStatus(TIM1, TIM_IT_COM | TIM_IT_Trigger)) { + PIOS_TIM_generic_irq_handler(TIM1); + } else if (TIM_GetITStatus(TIM11, PIOS_TIM_ALL_FLAGS)) { + PIOS_TIM_generic_irq_handler(TIM11); + } } -void TIM8_BRK_TIM12_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_12_irq_handler"))); -static void PIOS_TIM_12_irq_handler (void) +void TIM8_BRK_TIM12_IRQHandler(void) __attribute__((alias("PIOS_TIM_12_irq_handler"))); +static void PIOS_TIM_12_irq_handler(void) { - if(TIM_GetITStatus(TIM8, TIM_IT_Break)) - { - PIOS_TIM_generic_irq_handler (TIM8); - } - else if (TIM_GetITStatus(TIM12, PIOS_TIM_ALL_FLAGS)) - { - PIOS_TIM_generic_irq_handler (TIM12); - } + if (TIM_GetITStatus(TIM8, TIM_IT_Break)) { + PIOS_TIM_generic_irq_handler(TIM8); + } else if (TIM_GetITStatus(TIM12, PIOS_TIM_ALL_FLAGS)) { + PIOS_TIM_generic_irq_handler(TIM12); + } } -void TIM8_UP_TIM13_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM8_UP_TIM13_IRQHandler"))); -static void PIOS_TIM8_UP_TIM13_IRQHandler (void) +void TIM8_UP_TIM13_IRQHandler(void) __attribute__((alias("PIOS_TIM8_UP_TIM13_IRQHandler"))); +static void PIOS_TIM8_UP_TIM13_IRQHandler(void) { - if(TIM_GetITStatus(TIM8, TIM_IT_Update)) - { - PIOS_TIM_generic_irq_handler (TIM8); - } - else if (TIM_GetITStatus(TIM13, PIOS_TIM_ALL_FLAGS)) - { - PIOS_TIM_generic_irq_handler (TIM13); - } + if (TIM_GetITStatus(TIM8, TIM_IT_Update)) { + PIOS_TIM_generic_irq_handler(TIM8); + } else if (TIM_GetITStatus(TIM13, PIOS_TIM_ALL_FLAGS)) { + PIOS_TIM_generic_irq_handler(TIM13); + } } -void TIM8_TRG_COM_TIM14_IRQHandler (void) __attribute__ ((alias ("PIOS_TIM8_TRG_COM_TIM14_IRQHandler"))); -static void PIOS_TIM8_TRG_COM_TIM14_IRQHandler (void) +void TIM8_TRG_COM_TIM14_IRQHandler(void) __attribute__((alias("PIOS_TIM8_TRG_COM_TIM14_IRQHandler"))); +static void PIOS_TIM8_TRG_COM_TIM14_IRQHandler(void) { - if(TIM_GetITStatus(TIM8, TIM_IT_COM | TIM_IT_Trigger)) - { - PIOS_TIM_generic_irq_handler (TIM8); - } - else if (TIM_GetITStatus(TIM14, PIOS_TIM_ALL_FLAGS)) - { - PIOS_TIM_generic_irq_handler (TIM14); - } + if (TIM_GetITStatus(TIM8, TIM_IT_COM | TIM_IT_Trigger)) { + PIOS_TIM_generic_irq_handler(TIM8); + } else if (TIM_GetITStatus(TIM14, PIOS_TIM_ALL_FLAGS)) { + PIOS_TIM_generic_irq_handler(TIM14); + } } #endif /* PIOS_INCLUDE_TIM */ diff --git a/flight/pios/stm32f4xx/pios_usart.c b/flight/pios/stm32f4xx/pios_usart.c index daa4d25bb..d16e41cee 100644 --- a/flight/pios/stm32f4xx/pios_usart.c +++ b/flight/pios/stm32f4xx/pios_usart.c @@ -6,25 +6,25 @@ * @brief PIOS interface for USART port * @{ * - * @file pios_usart.c + * @file pios_usart.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief USART commands. Inits USARTs, controls USARTs & Interupt handlers. (STM32 dependent) * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -46,64 +46,66 @@ static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail); static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usart_com_driver = { - .set_baud = PIOS_USART_ChangeBaud, - .tx_start = PIOS_USART_TxStart, - .rx_start = PIOS_USART_RxStart, - .bind_tx_cb = PIOS_USART_RegisterTxCallback, - .bind_rx_cb = PIOS_USART_RegisterRxCallback, + .set_baud = PIOS_USART_ChangeBaud, + .tx_start = PIOS_USART_TxStart, + .rx_start = PIOS_USART_RxStart, + .bind_tx_cb = PIOS_USART_RegisterTxCallback, + .bind_rx_cb = PIOS_USART_RegisterRxCallback, }; enum pios_usart_dev_magic { - PIOS_USART_DEV_MAGIC = 0x4152834A, + PIOS_USART_DEV_MAGIC = 0x4152834A, }; struct pios_usart_dev { - enum pios_usart_dev_magic magic; - const struct pios_usart_cfg * cfg; + enum pios_usart_dev_magic magic; + const struct pios_usart_cfg *cfg; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; }; -static bool PIOS_USART_validate(struct pios_usart_dev * usart_dev) +static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) { - return (usart_dev->magic == PIOS_USART_DEV_MAGIC); + return usart_dev->magic == PIOS_USART_DEV_MAGIC; } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_usart_dev * PIOS_USART_alloc(void) +static struct pios_usart_dev *PIOS_USART_alloc(void) { - struct pios_usart_dev * usart_dev; + struct pios_usart_dev *usart_dev; - usart_dev = (struct pios_usart_dev *)pvPortMalloc(sizeof(*usart_dev)); - if (!usart_dev) return(NULL); + usart_dev = (struct pios_usart_dev *)pvPortMalloc(sizeof(*usart_dev)); + if (!usart_dev) { + return NULL; + } - usart_dev->rx_in_cb = 0; - usart_dev->rx_in_context = 0; - usart_dev->tx_out_cb = 0; - usart_dev->tx_out_context = 0; - usart_dev->magic = PIOS_USART_DEV_MAGIC; - return(usart_dev); + usart_dev->rx_in_cb = 0; + usart_dev->rx_in_context = 0; + usart_dev->tx_out_cb = 0; + usart_dev->tx_out_context = 0; + usart_dev->magic = PIOS_USART_DEV_MAGIC; + return usart_dev; } #else static struct pios_usart_dev pios_usart_devs[PIOS_USART_MAX_DEVS]; static uint8_t pios_usart_num_devs; -static struct pios_usart_dev * PIOS_USART_alloc(void) +static struct pios_usart_dev *PIOS_USART_alloc(void) { - struct pios_usart_dev * usart_dev; + struct pios_usart_dev *usart_dev; - if (pios_usart_num_devs >= PIOS_USART_MAX_DEVS) { - return (NULL); - } + if (pios_usart_num_devs >= PIOS_USART_MAX_DEVS) { + return NULL; + } - usart_dev = &pios_usart_devs[pios_usart_num_devs++]; - usart_dev->magic = PIOS_USART_DEV_MAGIC; + usart_dev = &pios_usart_devs[pios_usart_num_devs++]; + usart_dev->magic = PIOS_USART_DEV_MAGIC; - return (usart_dev); + return usart_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /* Bind Interrupt Handlers * @@ -114,244 +116,252 @@ static struct pios_usart_dev * PIOS_USART_alloc(void) static void PIOS_USART_generic_irq_handler(uint32_t usart_id); static uint32_t PIOS_USART_1_id; -void USART1_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_1_irq_handler"))); -static void PIOS_USART_1_irq_handler (void) +void USART1_IRQHandler(void) __attribute__((alias("PIOS_USART_1_irq_handler"))); +static void PIOS_USART_1_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_1_id); + PIOS_USART_generic_irq_handler(PIOS_USART_1_id); } static uint32_t PIOS_USART_2_id; -void USART2_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_2_irq_handler"))); -static void PIOS_USART_2_irq_handler (void) +void USART2_IRQHandler(void) __attribute__((alias("PIOS_USART_2_irq_handler"))); +static void PIOS_USART_2_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_2_id); + PIOS_USART_generic_irq_handler(PIOS_USART_2_id); } static uint32_t PIOS_USART_3_id; -void USART3_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_3_irq_handler"))); -static void PIOS_USART_3_irq_handler (void) +void USART3_IRQHandler(void) __attribute__((alias("PIOS_USART_3_irq_handler"))); +static void PIOS_USART_3_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_3_id); + PIOS_USART_generic_irq_handler(PIOS_USART_3_id); } static uint32_t PIOS_USART_4_id; -void USART4_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_4_irq_handler"))); -static void PIOS_USART_4_irq_handler (void) +void USART4_IRQHandler(void) __attribute__((alias("PIOS_USART_4_irq_handler"))); +static void PIOS_USART_4_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_4_id); + PIOS_USART_generic_irq_handler(PIOS_USART_4_id); } static uint32_t PIOS_USART_5_id; -void USART5_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_5_irq_handler"))); -static void PIOS_USART_5_irq_handler (void) +void USART5_IRQHandler(void) __attribute__((alias("PIOS_USART_5_irq_handler"))); +static void PIOS_USART_5_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_5_id); + PIOS_USART_generic_irq_handler(PIOS_USART_5_id); } static uint32_t PIOS_USART_6_id; -void USART6_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_6_irq_handler"))); -static void PIOS_USART_6_irq_handler (void) +void USART6_IRQHandler(void) __attribute__((alias("PIOS_USART_6_irq_handler"))); +static void PIOS_USART_6_irq_handler(void) { - PIOS_USART_generic_irq_handler (PIOS_USART_6_id); + PIOS_USART_generic_irq_handler(PIOS_USART_6_id); } /** -* Initialise a single USART device -*/ -int32_t PIOS_USART_Init(uint32_t * usart_id, const struct pios_usart_cfg * cfg) + * Initialise a single USART device + */ +int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) { - PIOS_DEBUG_Assert(usart_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(usart_id); + PIOS_DEBUG_Assert(cfg); - struct pios_usart_dev * usart_dev; + struct pios_usart_dev *usart_dev; - usart_dev = (struct pios_usart_dev *) PIOS_USART_alloc(); - if (!usart_dev) goto out_fail; + usart_dev = (struct pios_usart_dev *)PIOS_USART_alloc(); + if (!usart_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usart_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + usart_dev->cfg = cfg; - /* Map pins to USART function */ - /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ - if (usart_dev->cfg->remap) { - GPIO_PinAFConfig(usart_dev->cfg->rx.gpio, - __builtin_ctz(usart_dev->cfg->rx.init.GPIO_Pin), - usart_dev->cfg->remap); - GPIO_PinAFConfig(usart_dev->cfg->tx.gpio, - __builtin_ctz(usart_dev->cfg->tx.init.GPIO_Pin), - usart_dev->cfg->remap); - } + /* Map pins to USART function */ + /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ + if (usart_dev->cfg->remap) { + GPIO_PinAFConfig(usart_dev->cfg->rx.gpio, + __builtin_ctz(usart_dev->cfg->rx.init.GPIO_Pin), + usart_dev->cfg->remap); + GPIO_PinAFConfig(usart_dev->cfg->tx.gpio, + __builtin_ctz(usart_dev->cfg->tx.init.GPIO_Pin), + usart_dev->cfg->remap); + } - /* Initialize the USART Rx and Tx pins */ - GPIO_Init(usart_dev->cfg->rx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->rx.init); - GPIO_Init(usart_dev->cfg->tx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->tx.init); + /* Initialize the USART Rx and Tx pins */ + GPIO_Init(usart_dev->cfg->rx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->rx.init); + GPIO_Init(usart_dev->cfg->tx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->tx.init); - /* Configure the USART */ - USART_Init(usart_dev->cfg->regs, (USART_InitTypeDef *)&usart_dev->cfg->init); + /* Configure the USART */ + USART_Init(usart_dev->cfg->regs, (USART_InitTypeDef *)&usart_dev->cfg->init); - *usart_id = (uint32_t)usart_dev; + *usart_id = (uint32_t)usart_dev; - /* Configure USART Interrupts */ - switch ((uint32_t)usart_dev->cfg->regs) { - case (uint32_t)USART1: - PIOS_USART_1_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART2: - PIOS_USART_2_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART3: - PIOS_USART_3_id = (uint32_t)usart_dev; - break; - case (uint32_t)UART4: - PIOS_USART_4_id = (uint32_t)usart_dev; - break; - case (uint32_t)UART5: - PIOS_USART_5_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART6: - PIOS_USART_6_id = (uint32_t)usart_dev; - break; - } - NVIC_Init((NVIC_InitTypeDef *)&(usart_dev->cfg->irq.init)); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); + /* Configure USART Interrupts */ + switch ((uint32_t)usart_dev->cfg->regs) { + case (uint32_t)USART1: + PIOS_USART_1_id = (uint32_t)usart_dev; + break; + case (uint32_t)USART2: + PIOS_USART_2_id = (uint32_t)usart_dev; + break; + case (uint32_t)USART3: + PIOS_USART_3_id = (uint32_t)usart_dev; + break; + case (uint32_t)UART4: + PIOS_USART_4_id = (uint32_t)usart_dev; + break; + case (uint32_t)UART5: + PIOS_USART_5_id = (uint32_t)usart_dev; + break; + case (uint32_t)USART6: + PIOS_USART_6_id = (uint32_t)usart_dev; + break; + } + NVIC_Init((NVIC_InitTypeDef *)&(usart_dev->cfg->irq.init)); + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); - // FIXME XXX Clear / reset uart here - sends NUL char else + // FIXME XXX Clear / reset uart here - sends NUL char else - /* Enable USART */ - USART_Cmd(usart_dev->cfg->regs, ENABLE); + /* Enable USART */ + USART_Cmd(usart_dev->cfg->regs, ENABLE); - return(0); + return 0; out_fail: - return(-1); + return -1; } static void PIOS_USART_RxStart(uint32_t usart_id, __attribute__((unused)) uint16_t rx_bytes_avail) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; - - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); - - USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); } static void PIOS_USART_TxStart(uint32_t usart_id, __attribute__((unused)) uint16_t tx_bytes_avail) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; - - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); - - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); } /** -* Changes the baud rate of the USART peripheral without re-initialising. -* \param[in] usart_id USART name (GPS, TELEM, AUX) -* \param[in] baud Requested baud rate -*/ + * Changes the baud rate of the USART peripheral without re-initialising. + * \param[in] usart_id USART name (GPS, TELEM, AUX) + * \param[in] baud Requested baud rate + */ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); + bool valid = PIOS_USART_validate(usart_dev); - USART_InitTypeDef USART_InitStructure; + PIOS_Assert(valid); - /* Start with a copy of the default configuration for the peripheral */ - USART_InitStructure = usart_dev->cfg->init; + USART_InitTypeDef USART_InitStructure; - /* Adjust the baud rate */ - USART_InitStructure.USART_BaudRate = baud; + /* Start with a copy of the default configuration for the peripheral */ + USART_InitStructure = usart_dev->cfg->init; - /* Write back the new configuration */ - USART_Init(usart_dev->cfg->regs, &USART_InitStructure); + /* Adjust the baud rate */ + USART_InitStructure.USART_BaudRate = baud; + + /* Write back the new configuration */ + USART_Init(usart_dev->cfg->regs, &USART_InitStructure); } static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); - - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usart_dev->rx_in_context = context; - usart_dev->rx_in_cb = rx_in_cb; + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usart_dev->rx_in_context = context; + usart_dev->rx_in_cb = rx_in_cb; } static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); - - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usart_dev->tx_out_context = context; - usart_dev->tx_out_cb = tx_out_cb; + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usart_dev->tx_out_context = context; + usart_dev->tx_out_cb = tx_out_cb; } static void PIOS_USART_generic_irq_handler(uint32_t usart_id) { - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + /* Force read of dr after sr to make sure to clear error flags */ + volatile uint16_t sr = usart_dev->cfg->regs->SR; + volatile uint8_t dr = usart_dev->cfg->regs->DR; + + /* Check if RXNE flag is set */ + bool rx_need_yield = false; + if (sr & USART_SR_RXNE) { + uint8_t byte = dr; + if (usart_dev->rx_in_cb) { + (void)(usart_dev->rx_in_cb)(usart_dev->rx_in_context, &byte, 1, NULL, &rx_need_yield); + } + } + + /* Check if TXE flag is set */ + bool tx_need_yield = false; + if (sr & USART_SR_TXE) { + if (usart_dev->tx_out_cb) { + uint8_t b; + uint16_t bytes_to_send; + + bytes_to_send = (usart_dev->tx_out_cb)(usart_dev->tx_out_context, &b, 1, NULL, &tx_need_yield); + + if (bytes_to_send > 0) { + /* Send the byte we've been given */ + usart_dev->cfg->regs->DR = b; + } else { + /* No bytes to send, disable TXE interrupt */ + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); + } + } else { + /* No bytes to send, disable TXE interrupt */ + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); + } + } - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid); - - /* Force read of dr after sr to make sure to clear error flags */ - volatile uint16_t sr = usart_dev->cfg->regs->SR; - volatile uint8_t dr = usart_dev->cfg->regs->DR; - - /* Check if RXNE flag is set */ - bool rx_need_yield = false; - if (sr & USART_SR_RXNE) { - uint8_t byte = dr; - if (usart_dev->rx_in_cb) { - (void) (usart_dev->rx_in_cb)(usart_dev->rx_in_context, &byte, 1, NULL, &rx_need_yield); - } - } - - /* Check if TXE flag is set */ - bool tx_need_yield = false; - if (sr & USART_SR_TXE) { - if (usart_dev->tx_out_cb) { - uint8_t b; - uint16_t bytes_to_send; - - bytes_to_send = (usart_dev->tx_out_cb)(usart_dev->tx_out_context, &b, 1, NULL, &tx_need_yield); - - if (bytes_to_send > 0) { - /* Send the byte we've been given */ - usart_dev->cfg->regs->DR = b; - } else { - /* No bytes to send, disable TXE interrupt */ - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); - } - } else { - /* No bytes to send, disable TXE interrupt */ - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); - } - } - #if defined(PIOS_INCLUDE_FREERTOS) - if (rx_need_yield || tx_need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (rx_need_yield || tx_need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } #endif /* PIOS_INCLUDE_USART */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/stm32f4xx/pios_usb.c b/flight/pios/stm32f4xx/pios_usb.c index 6666e120d..2184c303e 100644 --- a/flight/pios/stm32f4xx/pios_usb.c +++ b/flight/pios/stm32f4xx/pios_usb.c @@ -41,51 +41,53 @@ static uint8_t transfer_possible = 0; enum pios_usb_dev_magic { - PIOS_USB_DEV_MAGIC = 0x17365904, + PIOS_USB_DEV_MAGIC = 0x17365904, }; struct pios_usb_dev { - enum pios_usb_dev_magic magic; - const struct pios_usb_cfg * cfg; + enum pios_usb_dev_magic magic; + const struct pios_usb_cfg *cfg; }; /** * @brief Validate the usb device structure * @returns true if valid device or false otherwise */ -static bool PIOS_USB_validate(struct pios_usb_dev * usb_dev) +static bool PIOS_USB_validate(struct pios_usb_dev *usb_dev) { - return (usb_dev && (usb_dev->magic == PIOS_USB_DEV_MAGIC)); + return usb_dev && (usb_dev->magic == PIOS_USB_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_usb_dev * PIOS_USB_alloc(void) +static struct pios_usb_dev *PIOS_USB_alloc(void) { - struct pios_usb_dev * usb_dev; + struct pios_usb_dev *usb_dev; - usb_dev = (struct pios_usb_dev *)pvPortMalloc(sizeof(*usb_dev)); - if (!usb_dev) return(NULL); + usb_dev = (struct pios_usb_dev *)pvPortMalloc(sizeof(*usb_dev)); + if (!usb_dev) { + return NULL; + } - usb_dev->magic = PIOS_USB_DEV_MAGIC; - return(usb_dev); + usb_dev->magic = PIOS_USB_DEV_MAGIC; + return usb_dev; } #else static struct pios_usb_dev pios_usb_devs[PIOS_USB_MAX_DEVS]; static uint8_t pios_usb_num_devs; -static struct pios_usb_dev * PIOS_USB_alloc(void) +static struct pios_usb_dev *PIOS_USB_alloc(void) { - struct pios_usb_dev * usb_dev; + struct pios_usb_dev *usb_dev; - if (pios_usb_num_devs >= PIOS_USB_MAX_DEVS) { - return (NULL); - } + if (pios_usb_num_devs >= PIOS_USB_MAX_DEVS) { + return NULL; + } - usb_dev = &pios_usb_devs[pios_usb_num_devs++]; - usb_dev->magic = PIOS_USB_DEV_MAGIC; + usb_dev = &pios_usb_devs[pios_usb_num_devs++]; + usb_dev->magic = PIOS_USB_DEV_MAGIC; - return (usb_dev); + return usb_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ /** @@ -93,31 +95,33 @@ static struct pios_usb_dev * PIOS_USB_alloc(void) * \return < 0 if initialisation failed */ static uint32_t pios_usb_id; -int32_t PIOS_USB_Init(uint32_t * usb_id, const struct pios_usb_cfg * cfg) +int32_t PIOS_USB_Init(uint32_t *usb_id, const struct pios_usb_cfg *cfg) { - PIOS_Assert(usb_id); - PIOS_Assert(cfg); + PIOS_Assert(usb_id); + PIOS_Assert(cfg); - struct pios_usb_dev * usb_dev; + struct pios_usb_dev *usb_dev; - usb_dev = (struct pios_usb_dev *) PIOS_USB_alloc(); - if (!usb_dev) goto out_fail; + usb_dev = (struct pios_usb_dev *)PIOS_USB_alloc(); + if (!usb_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usb_dev->cfg = cfg; + /* Bind the configuration to the device instance */ + usb_dev->cfg = cfg; - /* - * This is a horrible hack to make this available to - * the interrupt callbacks. This should go away ASAP. - */ - pios_usb_id = (uint32_t) usb_dev; + /* + * This is a horrible hack to make this available to + * the interrupt callbacks. This should go away ASAP. + */ + pios_usb_id = (uint32_t)usb_dev; - *usb_id = (uint32_t) usb_dev; + *usb_id = (uint32_t)usb_dev; - return 0; /* No error */ + return 0; /* No error */ out_fail: - return(-1); + return -1; } /** @@ -128,25 +132,25 @@ out_fail: */ int32_t PIOS_USB_ChangeConnectionState(bool connected) { - // In all cases: re-initialise USB HID driver - if (connected) { - transfer_possible = 1; + // In all cases: re-initialise USB HID driver + if (connected) { + transfer_possible = 1; - //TODO: Check SetEPRxValid(ENDP1); + // TODO: Check SetEPRxValid(ENDP1); #if defined(USB_LED_ON) - USB_LED_ON; // turn the USB led on + USB_LED_ON; // turn the USB led on #endif - } else { - // Cable disconnected: disable transfers - transfer_possible = 0; + } else { + // Cable disconnected: disable transfers + transfer_possible = 0; #if defined(USB_LED_OFF) - USB_LED_OFF; // turn the USB led off + USB_LED_OFF; // turn the USB led off #endif - } + } - return 0; + return 0; } /** @@ -157,14 +161,16 @@ int32_t PIOS_USB_ChangeConnectionState(bool connected) uint32_t usb_found; bool PIOS_USB_CheckAvailable(__attribute__((unused)) uint32_t id) { - struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_id; + struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_id; - if(!PIOS_USB_validate(usb_dev)) - return false; + if (!PIOS_USB_validate(usb_dev)) { + return false; + } - usb_found = ((usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin) != 0) ^ usb_dev->cfg->vsense_active_low; - return usb_found; - return usb_found != 0 && transfer_possible ? 1 : 0; + usb_found = ((usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin) != 0) ^ usb_dev->cfg->vsense_active_low; + return usb_found; + + return usb_found != 0 && transfer_possible ? 1 : 0; } /* @@ -177,89 +183,83 @@ bool PIOS_USB_CheckAvailable(__attribute__((unused)) uint32_t id) void USB_OTG_BSP_Init(__attribute__((unused)) USB_OTG_CORE_HANDLE *pdev) { - struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_id; + struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_id; - bool valid = PIOS_USB_validate(usb_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_validate(usb_dev); + + PIOS_Assert(valid); #define FORCE_DISABLE_USB_IRQ 1 #if FORCE_DISABLE_USB_IRQ - /* Make sure we disable the USB interrupt since it may be left on by bootloader */ - NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure = usb_dev->cfg->irq.init; - NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE; - NVIC_Init(&NVIC_InitStructure); + /* Make sure we disable the USB interrupt since it may be left on by bootloader */ + NVIC_InitTypeDef NVIC_InitStructure; + NVIC_InitStructure = usb_dev->cfg->irq.init; + NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE; + NVIC_Init(&NVIC_InitStructure); #endif - /* Configure USB D-/D+ (DM/DP) pins */ - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_12; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_Init(GPIOA, &GPIO_InitStructure); + /* Configure USB D-/D+ (DM/DP) pins */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_12; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_Init(GPIOA, &GPIO_InitStructure); - GPIO_PinAFConfig(GPIOA, GPIO_PinSource11, GPIO_AF_OTG1_FS); - GPIO_PinAFConfig(GPIOA, GPIO_PinSource12, GPIO_AF_OTG1_FS); + GPIO_PinAFConfig(GPIOA, GPIO_PinSource11, GPIO_AF_OTG1_FS); + GPIO_PinAFConfig(GPIOA, GPIO_PinSource12, GPIO_AF_OTG1_FS); - /* Configure VBUS sense pin */ - GPIO_Init(usb_dev->cfg->vsense.gpio, &usb_dev->cfg->vsense.init); + /* Configure VBUS sense pin */ + GPIO_Init(usb_dev->cfg->vsense.gpio, &usb_dev->cfg->vsense.init); - /* Enable USB OTG Clock */ - RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, ENABLE); + /* Enable USB OTG Clock */ + RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, ENABLE); } void USB_OTG_BSP_EnableInterrupt(__attribute__((unused)) USB_OTG_CORE_HANDLE *pdev) { - struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_id; + struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_id; - bool valid = PIOS_USB_validate(usb_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_validate(usb_dev); - NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + PIOS_Assert(valid); - NVIC_Init(&usb_dev->cfg->irq.init); + NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + + NVIC_Init(&usb_dev->cfg->irq.init); } #ifdef USE_HOST_MODE void USB_OTG_BSP_DriveVBUS(USB_OTG_CORE_HANDLE *pdev, uint8_t state) -{ - -} +{} void USB_OTG_BSP_ConfigVBUS(USB_OTG_CORE_HANDLE *pdev) +{} +#endif /* USE_HOST_MODE */ + +void USB_OTG_BSP_TimeInit(void) +{} + +void USB_OTG_BSP_uDelay(const uint32_t usec) { + uint32_t count = 0; + const uint32_t utime = (120 * usec / 7); -} -#endif /* USE_HOST_MODE */ - -void USB_OTG_BSP_TimeInit ( void ) -{ - + do { + if (++count > utime) { + return; + } + } while (1); } -void USB_OTG_BSP_uDelay (const uint32_t usec) +void USB_OTG_BSP_mDelay(const uint32_t msec) { - uint32_t count = 0; - const uint32_t utime = (120 * usec / 7); - do { - if (++count > utime) { - return ; - } - } - while (1); + USB_OTG_BSP_uDelay(msec * 1000); } -void USB_OTG_BSP_mDelay (const uint32_t msec) -{ - USB_OTG_BSP_uDelay(msec * 1000); -} - -void USB_OTG_BSP_TimerIRQ (void) -{ - -} +void USB_OTG_BSP_TimerIRQ(void) +{} #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/stm32f4xx/pios_usb_cdc.c b/flight/pios/stm32f4xx/pios_usb_cdc.c index 30151330e..dbfd6d0cf 100644 --- a/flight/pios/stm32f4xx/pios_usb_cdc.c +++ b/flight/pios/stm32f4xx/pios_usb_cdc.c @@ -42,68 +42,70 @@ static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail); static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usb_cdc_com_driver = { - .tx_start = PIOS_USB_CDC_TxStart, - .rx_start = PIOS_USB_CDC_RxStart, - .bind_tx_cb = PIOS_USB_CDC_RegisterTxCallback, - .bind_rx_cb = PIOS_USB_CDC_RegisterRxCallback, - .available = PIOS_USB_CheckAvailable, + .tx_start = PIOS_USB_CDC_TxStart, + .rx_start = PIOS_USB_CDC_RxStart, + .bind_tx_cb = PIOS_USB_CDC_RegisterTxCallback, + .bind_rx_cb = PIOS_USB_CDC_RegisterRxCallback, + .available = PIOS_USB_CheckAvailable, }; enum pios_usb_cdc_dev_magic { - PIOS_USB_CDC_DEV_MAGIC = 0xAABBCCDD, + PIOS_USB_CDC_DEV_MAGIC = 0xAABBCCDD, }; struct pios_usb_cdc_dev { - enum pios_usb_cdc_dev_magic magic; - const struct pios_usb_cdc_cfg * cfg; + enum pios_usb_cdc_dev_magic magic; + const struct pios_usb_cdc_cfg *cfg; - uint32_t lower_id; + uint32_t lower_id; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; - uint8_t rx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; - uint8_t tx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; + uint8_t rx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; + uint8_t tx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; - uint32_t rx_dropped; - uint32_t rx_oversize; + uint32_t rx_dropped; + uint32_t rx_oversize; }; -static bool PIOS_USB_CDC_validate(struct pios_usb_cdc_dev * usb_cdc_dev) +static bool PIOS_USB_CDC_validate(struct pios_usb_cdc_dev *usb_cdc_dev) { - return (usb_cdc_dev && (usb_cdc_dev->magic == PIOS_USB_CDC_DEV_MAGIC)); + return usb_cdc_dev && (usb_cdc_dev->magic == PIOS_USB_CDC_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_usb_cdc_dev * PIOS_USB_CDC_alloc(void) +static struct pios_usb_cdc_dev *PIOS_USB_CDC_alloc(void) { - struct pios_usb_cdc_dev * usb_cdc_dev; + struct pios_usb_cdc_dev *usb_cdc_dev; - usb_cdc_dev = (struct pios_usb_cdc_dev *)pvPortMalloc(sizeof(*usb_cdc_dev)); - if (!usb_cdc_dev) return(NULL); + usb_cdc_dev = (struct pios_usb_cdc_dev *)pvPortMalloc(sizeof(*usb_cdc_dev)); + if (!usb_cdc_dev) { + return NULL; + } - usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; - return(usb_cdc_dev); + usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; + return usb_cdc_dev; } #else static struct pios_usb_cdc_dev pios_usb_cdc_devs[PIOS_USB_CDC_MAX_DEVS]; static uint8_t pios_usb_cdc_num_devs; -static struct pios_usb_cdc_dev * PIOS_USB_CDC_alloc(void) +static struct pios_usb_cdc_dev *PIOS_USB_CDC_alloc(void) { - struct pios_usb_cdc_dev * usb_cdc_dev; + struct pios_usb_cdc_dev *usb_cdc_dev; - if (pios_usb_cdc_num_devs >= PIOS_USB_CDC_MAX_DEVS) { - return (NULL); - } + if (pios_usb_cdc_num_devs >= PIOS_USB_CDC_MAX_DEVS) { + return NULL; + } - usb_cdc_dev = &pios_usb_cdc_devs[pios_usb_cdc_num_devs++]; - usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; + usb_cdc_dev = &pios_usb_cdc_devs[pios_usb_cdc_num_devs++]; + usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; - return (usb_cdc_dev); + return usb_cdc_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ static void PIOS_USB_CDC_DATA_EP_IN_Callback(void); static void PIOS_USB_CDC_DATA_EP_OUT_Callback(void); @@ -112,288 +114,300 @@ static void PIOS_USB_CDC_CTRL_EP_IN_Callback(void); static uint32_t pios_usb_cdc_id; /* Need a better way to pull these in */ -extern void (*pEpInt_IN[7])(void); -extern void (*pEpInt_OUT[7])(void); +extern void(*pEpInt_IN[7]) (void); +extern void(*pEpInt_OUT[7]) (void); -int32_t PIOS_USB_CDC_Init(uint32_t * usbcdc_id, const struct pios_usb_cdc_cfg * cfg, uint32_t lower_id) +int32_t PIOS_USB_CDC_Init(uint32_t *usbcdc_id, const struct pios_usb_cdc_cfg *cfg, uint32_t lower_id) { - PIOS_Assert(usbcdc_id); - PIOS_Assert(cfg); + PIOS_Assert(usbcdc_id); + PIOS_Assert(cfg); - struct pios_usb_cdc_dev * usb_cdc_dev; + struct pios_usb_cdc_dev *usb_cdc_dev; - usb_cdc_dev = (struct pios_usb_cdc_dev *) PIOS_USB_CDC_alloc(); - if (!usb_cdc_dev) goto out_fail; + usb_cdc_dev = (struct pios_usb_cdc_dev *)PIOS_USB_CDC_alloc(); + if (!usb_cdc_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usb_cdc_dev->cfg = cfg; - usb_cdc_dev->lower_id = lower_id; + /* Bind the configuration to the device instance */ + usb_cdc_dev->cfg = cfg; + usb_cdc_dev->lower_id = lower_id; - pios_usb_cdc_id = (uint32_t) usb_cdc_dev; + pios_usb_cdc_id = (uint32_t)usb_cdc_dev; - /* Bind lower level callbacks into the USB infrastructure */ - pEpInt_OUT[cfg->ctrl_tx_ep - 1] = PIOS_USB_CDC_CTRL_EP_IN_Callback; - pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_CDC_DATA_EP_IN_Callback; - pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_CDC_DATA_EP_OUT_Callback; + /* Bind lower level callbacks into the USB infrastructure */ + pEpInt_OUT[cfg->ctrl_tx_ep - 1] = PIOS_USB_CDC_CTRL_EP_IN_Callback; + pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_CDC_DATA_EP_IN_Callback; + pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_CDC_DATA_EP_OUT_Callback; - *usbcdc_id = (uint32_t) usb_cdc_dev; + *usbcdc_id = (uint32_t)usb_cdc_dev; - return 0; + return 0; out_fail: - return -1; + return -1; } - static void PIOS_USB_CDC_RegisterRxCallback(uint32_t usbcdc_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_cdc_dev->rx_in_context = context; - usb_cdc_dev->rx_in_cb = rx_in_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->rx_in_context = context; + usb_cdc_dev->rx_in_cb = rx_in_cb; } static void PIOS_USB_CDC_RegisterTxCallback(uint32_t usbcdc_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_cdc_dev->tx_out_context = context; - usb_cdc_dev->tx_out_cb = tx_out_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->tx_out_context = context; + usb_cdc_dev->tx_out_cb = tx_out_cb; } -static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); - - if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { - return; - } - - // If endpoint was stalled and there is now space make it valid - PIOS_IRQ_Disable(); - if ((GetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep) != EP_RX_VALID) && - (rx_bytes_avail >= sizeof(usb_cdc_dev->rx_packet_buffer))) { - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); - } - PIOS_IRQ_Enable(); -} - -static void PIOS_USB_CDC_SendData(struct pios_usb_cdc_dev * usb_cdc_dev) +static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail) { - uint16_t bytes_to_tx; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - if (!usb_cdc_dev->tx_out_cb) { - return; - } + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - bool need_yield = false; - bytes_to_tx = (usb_cdc_dev->tx_out_cb)(usb_cdc_dev->tx_out_context, - usb_cdc_dev->tx_packet_buffer, - sizeof(usb_cdc_dev->tx_packet_buffer), - NULL, - &need_yield); - if (bytes_to_tx == 0) { - return; - } + PIOS_Assert(valid); - UserToPMABufferCopy(usb_cdc_dev->tx_packet_buffer, - GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), - bytes_to_tx); - SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, bytes_to_tx); - SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); + if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { + return; + } + + // If endpoint was stalled and there is now space make it valid + PIOS_IRQ_Disable(); + if ((GetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep) != EP_RX_VALID) && + (rx_bytes_avail >= sizeof(usb_cdc_dev->rx_packet_buffer))) { + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); + } + PIOS_IRQ_Enable(); +} + +static void PIOS_USB_CDC_SendData(struct pios_usb_cdc_dev *usb_cdc_dev) +{ + uint16_t bytes_to_tx; + + if (!usb_cdc_dev->tx_out_cb) { + return; + } + + bool need_yield = false; + bytes_to_tx = (usb_cdc_dev->tx_out_cb)(usb_cdc_dev->tx_out_context, + usb_cdc_dev->tx_packet_buffer, + sizeof(usb_cdc_dev->tx_packet_buffer), + NULL, + &need_yield); + if (bytes_to_tx == 0) { + return; + } + + UserToPMABufferCopy(usb_cdc_dev->tx_packet_buffer, + GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), + bytes_to_tx); + SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, bytes_to_tx); + SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); #if defined(PIOS_INCLUDE_FREERTOS) - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { - return; - } + PIOS_Assert(valid); - if (GetEPTxStatus(usb_cdc_dev->cfg->data_tx_ep) == EP_TX_VALID) { - /* Endpoint is already transmitting */ - return; - } + if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { + return; + } - PIOS_USB_CDC_SendData(usb_cdc_dev); + if (GetEPTxStatus(usb_cdc_dev->cfg->data_tx_ep) == EP_TX_VALID) { + /* Endpoint is already transmitting */ + return; + } + + PIOS_USB_CDC_SendData(usb_cdc_dev); } static void PIOS_USB_CDC_DATA_EP_IN_Callback(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_USB_CDC_SendData(usb_cdc_dev); + PIOS_Assert(valid); + + PIOS_USB_CDC_SendData(usb_cdc_dev); } static void PIOS_USB_CDC_DATA_EP_OUT_Callback(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - uint32_t DataLength; + PIOS_Assert(valid); - /* Get the number of received data on the selected Endpoint */ - DataLength = GetEPRxCount(usb_cdc_dev->cfg->data_rx_ep); - if (DataLength > sizeof(usb_cdc_dev->rx_packet_buffer)) { - usb_cdc_dev->rx_oversize++; - DataLength = sizeof(usb_cdc_dev->rx_packet_buffer); - } + uint32_t DataLength; - /* Use the memory interface function to read from the selected endpoint */ - PMAToUserBufferCopy((uint8_t *) usb_cdc_dev->rx_packet_buffer, - GetEPRxAddr(usb_cdc_dev->cfg->data_rx_ep), - DataLength); + /* Get the number of received data on the selected Endpoint */ + DataLength = GetEPRxCount(usb_cdc_dev->cfg->data_rx_ep); + if (DataLength > sizeof(usb_cdc_dev->rx_packet_buffer)) { + usb_cdc_dev->rx_oversize++; + DataLength = sizeof(usb_cdc_dev->rx_packet_buffer); + } - if (!usb_cdc_dev->rx_in_cb) { - /* No Rx call back registered, disable the receiver */ - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); - return; - } + /* Use the memory interface function to read from the selected endpoint */ + PMAToUserBufferCopy((uint8_t *)usb_cdc_dev->rx_packet_buffer, + GetEPRxAddr(usb_cdc_dev->cfg->data_rx_ep), + DataLength); - uint16_t headroom; - bool need_yield = false; - uint16_t rc; - rc = (usb_cdc_dev->rx_in_cb)(usb_cdc_dev->rx_in_context, - usb_cdc_dev->rx_packet_buffer, - DataLength, - &headroom, - &need_yield); + if (!usb_cdc_dev->rx_in_cb) { + /* No Rx call back registered, disable the receiver */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); + return; + } - if (rc < DataLength) { - /* Lost bytes on rx */ - usb_cdc_dev->rx_dropped += (DataLength - rc); - } + uint16_t headroom; + bool need_yield = false; + uint16_t rc; + rc = (usb_cdc_dev->rx_in_cb)(usb_cdc_dev->rx_in_context, + usb_cdc_dev->rx_packet_buffer, + DataLength, + &headroom, + &need_yield); - if (headroom >= sizeof(usb_cdc_dev->rx_packet_buffer)) { - /* We have room for a maximum length message */ - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); - } else { - /* Not enough room left for a message, apply backpressure */ - SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); - } + if (rc < DataLength) { + /* Lost bytes on rx */ + usb_cdc_dev->rx_dropped += (DataLength - rc); + } + + if (headroom >= sizeof(usb_cdc_dev->rx_packet_buffer)) { + /* We have room for a maximum length message */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); + } else { + /* Not enough room left for a message, apply backpressure */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } RESULT PIOS_USB_CDC_SetControlLineState(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - static uint16_t control_line_state; + PIOS_Assert(valid); - uint8_t wValue0 = pInformation->USBwValue0; - uint8_t wValue1 = pInformation->USBwValue1; + static uint16_t control_line_state; - control_line_state = wValue1 << 8 | wValue0; + uint8_t wValue0 = pInformation->USBwValue0; + uint8_t wValue1 = pInformation->USBwValue1; - return USB_SUCCESS; + control_line_state = wValue1 << 8 | wValue0; + + return USB_SUCCESS; } static struct usb_cdc_line_coding line_coding = { - .dwDTERate = htousbl(57600), - .bCharFormat = USB_CDC_LINE_CODING_STOP_1, - .bParityType = USB_CDC_LINE_CODING_PARITY_NONE, - .bDataBits = 8, + .dwDTERate = htousbl(57600), + .bCharFormat = USB_CDC_LINE_CODING_STOP_1, + .bParityType = USB_CDC_LINE_CODING_PARITY_NONE, + .bDataBits = 8, }; RESULT PIOS_USB_CDC_SetLineCoding(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - return USB_SUCCESS; + PIOS_Assert(valid); + + return USB_SUCCESS; } const uint8_t *PIOS_USB_CDC_GetLineCoding(uint16_t Length) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - if (Length == 0) { - pInformation->Ctrl_Info.Usb_wLength = sizeof(line_coding); - return NULL; - } else { - return ((uint8_t *) &line_coding); - } + PIOS_Assert(valid); + + if (Length == 0) { + pInformation->Ctrl_Info.Usb_wLength = sizeof(line_coding); + return NULL; + } else { + return (uint8_t *)&line_coding; + } } struct usb_cdc_serial_state_report uart_state = { - .bmRequestType = 0xA1, - .bNotification = USB_CDC_NOTIFICATION_SERIAL_STATE, - .wValue = 0, - .wIndex = htousbs(1), - .wLength = htousbs(2), - .bmUartState = htousbs(0), + .bmRequestType = 0xA1, + .bNotification = USB_CDC_NOTIFICATION_SERIAL_STATE, + .wValue = 0, + .wIndex = htousbs(1), + .wLength = htousbs(2), + .bmUartState = htousbs(0), }; - + static void PIOS_USB_CDC_CTRL_EP_IN_Callback(void) { - struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; - bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); - /* Give back UART State Bitmap */ - /* UART State Bitmap - * 15-7: reserved - * 6: bOverRun overrun error - * 5: bParity parity error - * 4: bFraming framing error - * 3: bRingSignal RI - * 2: bBreak break reception - * 1: bTxCarrier DSR - * 0: bRxCarrier DCD - */ - uart_state.bmUartState = htousbs(0x0003); + PIOS_Assert(valid); - UserToPMABufferCopy((uint8_t *) &uart_state, - GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), - sizeof(uart_state)); - SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, PIOS_USB_BOARD_CDC_MGMT_LENGTH); - SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); + /* Give back UART State Bitmap */ + /* UART State Bitmap + * 15-7: reserved + * 6: bOverRun overrun error + * 5: bParity parity error + * 4: bFraming framing error + * 3: bRingSignal RI + * 2: bBreak break reception + * 1: bTxCarrier DSR + * 0: bRxCarrier DCD + */ + uart_state.bmUartState = htousbs(0x0003); + + UserToPMABufferCopy((uint8_t *)&uart_state, + GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), + sizeof(uart_state)); + SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, PIOS_USB_BOARD_CDC_MGMT_LENGTH); + SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); } #endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/pios/stm32f4xx/pios_usb_hid.c b/flight/pios/stm32f4xx/pios_usb_hid.c index 3f7a736b7..16ecdb940 100644 --- a/flight/pios/stm32f4xx/pios_usb_hid.c +++ b/flight/pios/stm32f4xx/pios_usb_hid.c @@ -35,7 +35,7 @@ #include "pios_usb_hid_priv.h" #include "pios_usb_board_data.h" /* PIOS_BOARD_*_DATA_LENGTH */ -#include "pios_usbhook.h" /* PIOS_USBHOOK_* */ +#include "pios_usbhook.h" /* PIOS_USBHOOK_* */ static void PIOS_USB_HID_RegisterTxCallback(uint32_t usbhid_id, pios_com_callback tx_out_cb, uint32_t context); static void PIOS_USB_HID_RegisterRxCallback(uint32_t usbhid_id, pios_com_callback rx_in_cb, uint32_t context); @@ -43,73 +43,75 @@ static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, uint16_t tx_bytes_avail); static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usb_hid_com_driver = { - .tx_start = PIOS_USB_HID_TxStart, - .rx_start = PIOS_USB_HID_RxStart, - .bind_tx_cb = PIOS_USB_HID_RegisterTxCallback, - .bind_rx_cb = PIOS_USB_HID_RegisterRxCallback, - .available = PIOS_USB_CheckAvailable, + .tx_start = PIOS_USB_HID_TxStart, + .rx_start = PIOS_USB_HID_RxStart, + .bind_tx_cb = PIOS_USB_HID_RegisterTxCallback, + .bind_rx_cb = PIOS_USB_HID_RegisterRxCallback, + .available = PIOS_USB_CheckAvailable, }; enum pios_usb_hid_dev_magic { - PIOS_USB_HID_DEV_MAGIC = 0xAA00BB00, + PIOS_USB_HID_DEV_MAGIC = 0xAA00BB00, }; struct pios_usb_hid_dev { - enum pios_usb_hid_dev_magic magic; - const struct pios_usb_hid_cfg * cfg; + enum pios_usb_hid_dev_magic magic; + const struct pios_usb_hid_cfg *cfg; - uint32_t lower_id; + uint32_t lower_id; - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; - bool usb_if_enabled; + bool usb_if_enabled; - uint8_t rx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; - bool rx_active; + uint8_t rx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; + bool rx_active; - uint8_t tx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; - bool tx_active; + uint8_t tx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; + bool tx_active; - uint32_t rx_dropped; - uint32_t rx_oversize; + uint32_t rx_dropped; + uint32_t rx_oversize; }; -static bool PIOS_USB_HID_validate(struct pios_usb_hid_dev * usb_hid_dev) +static bool PIOS_USB_HID_validate(struct pios_usb_hid_dev *usb_hid_dev) { - return (usb_hid_dev && (usb_hid_dev->magic == PIOS_USB_HID_DEV_MAGIC)); + return usb_hid_dev && (usb_hid_dev->magic == PIOS_USB_HID_DEV_MAGIC); } #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_usb_hid_dev * PIOS_USB_HID_alloc(void) +static struct pios_usb_hid_dev *PIOS_USB_HID_alloc(void) { - struct pios_usb_hid_dev * usb_hid_dev; + struct pios_usb_hid_dev *usb_hid_dev; - usb_hid_dev = (struct pios_usb_hid_dev *)pvPortMalloc(sizeof(*usb_hid_dev)); - if (!usb_hid_dev) return(NULL); + usb_hid_dev = (struct pios_usb_hid_dev *)pvPortMalloc(sizeof(*usb_hid_dev)); + if (!usb_hid_dev) { + return NULL; + } - usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; - return(usb_hid_dev); + usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; + return usb_hid_dev; } #else static struct pios_usb_hid_dev pios_usb_hid_devs[PIOS_USB_HID_MAX_DEVS]; static uint8_t pios_usb_hid_num_devs; -static struct pios_usb_hid_dev * PIOS_USB_HID_alloc(void) +static struct pios_usb_hid_dev *PIOS_USB_HID_alloc(void) { - struct pios_usb_hid_dev * usb_hid_dev; + struct pios_usb_hid_dev *usb_hid_dev; - if (pios_usb_hid_num_devs >= PIOS_USB_HID_MAX_DEVS) { - return (NULL); - } + if (pios_usb_hid_num_devs >= PIOS_USB_HID_MAX_DEVS) { + return NULL; + } - usb_hid_dev = &pios_usb_hid_devs[pios_usb_hid_num_devs++]; - usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; + usb_hid_dev = &pios_usb_hid_devs[pios_usb_hid_num_devs++]; + usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; - return (usb_hid_dev); + return usb_hid_dev; } -#endif +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ static void PIOS_USB_HID_IF_Init(uint32_t usb_hid_id); static void PIOS_USB_HID_IF_DeInit(uint32_t usb_hid_id); @@ -117,232 +119,238 @@ static bool PIOS_USB_HID_IF_Setup(uint32_t usb_hid_id, struct usb_setup_request static void PIOS_USB_HID_IF_CtrlDataOut(uint32_t usb_hid_id, struct usb_setup_request *req); static struct pios_usb_ifops usb_hid_ifops = { - .init = PIOS_USB_HID_IF_Init, - .deinit = PIOS_USB_HID_IF_DeInit, - .setup = PIOS_USB_HID_IF_Setup, - .ctrl_data_out = PIOS_USB_HID_IF_CtrlDataOut, + .init = PIOS_USB_HID_IF_Init, + .deinit = PIOS_USB_HID_IF_DeInit, + .setup = PIOS_USB_HID_IF_Setup, + .ctrl_data_out = PIOS_USB_HID_IF_CtrlDataOut, }; static bool PIOS_USB_HID_EP_IN_Callback(uint32_t usb_hid_id, uint8_t epnum, uint16_t len); static bool PIOS_USB_HID_EP_OUT_Callback(uint32_t usb_hid_id, uint8_t epnum, uint16_t len); -int32_t PIOS_USB_HID_Init(uint32_t * usbhid_id, const struct pios_usb_hid_cfg * cfg, uint32_t lower_id) +int32_t PIOS_USB_HID_Init(uint32_t *usbhid_id, const struct pios_usb_hid_cfg *cfg, uint32_t lower_id) { - PIOS_Assert(usbhid_id); - PIOS_Assert(cfg); + PIOS_Assert(usbhid_id); + PIOS_Assert(cfg); - struct pios_usb_hid_dev * usb_hid_dev; + struct pios_usb_hid_dev *usb_hid_dev; - usb_hid_dev = (struct pios_usb_hid_dev *) PIOS_USB_HID_alloc(); - if (!usb_hid_dev) goto out_fail; + usb_hid_dev = (struct pios_usb_hid_dev *)PIOS_USB_HID_alloc(); + if (!usb_hid_dev) { + goto out_fail; + } - /* Bind the configuration to the device instance */ - usb_hid_dev->cfg = cfg; - usb_hid_dev->lower_id = lower_id; + /* Bind the configuration to the device instance */ + usb_hid_dev->cfg = cfg; + usb_hid_dev->lower_id = lower_id; - /* Rx and Tx are not active yet */ - usb_hid_dev->rx_active = false; - usb_hid_dev->tx_active = false; + /* Rx and Tx are not active yet */ + usb_hid_dev->rx_active = false; + usb_hid_dev->tx_active = false; - /* Register class specific interface callbacks with the USBHOOK layer */ - usb_hid_dev->usb_if_enabled = false; - PIOS_USBHOOK_RegisterIfOps(cfg->data_if, &usb_hid_ifops, (uint32_t) usb_hid_dev); + /* Register class specific interface callbacks with the USBHOOK layer */ + usb_hid_dev->usb_if_enabled = false; + PIOS_USBHOOK_RegisterIfOps(cfg->data_if, &usb_hid_ifops, (uint32_t)usb_hid_dev); - *usbhid_id = (uint32_t) usb_hid_dev; + *usbhid_id = (uint32_t)usb_hid_dev; - return 0; + return 0; out_fail: - return -1; + return -1; } static struct pios_usbhook_descriptor hid_desc; -void PIOS_USB_HID_RegisterHidDescriptor(const uint8_t * desc, uint16_t length) +void PIOS_USB_HID_RegisterHidDescriptor(const uint8_t *desc, uint16_t length) { - hid_desc.descriptor = desc; - hid_desc.length = length; + hid_desc.descriptor = desc; + hid_desc.length = length; } static struct pios_usbhook_descriptor hid_report_desc; -void PIOS_USB_HID_RegisterHidReport(const uint8_t * desc, uint16_t length) +void PIOS_USB_HID_RegisterHidReport(const uint8_t *desc, uint16_t length) { - hid_report_desc.descriptor = desc; - hid_report_desc.length = length; + hid_report_desc.descriptor = desc; + hid_report_desc.length = length; } -static bool PIOS_USB_HID_SendReport(struct pios_usb_hid_dev * usb_hid_dev) +static bool PIOS_USB_HID_SendReport(struct pios_usb_hid_dev *usb_hid_dev) { - uint16_t bytes_to_tx; + uint16_t bytes_to_tx; - if (!usb_hid_dev->tx_out_cb) { - return false; - } + if (!usb_hid_dev->tx_out_cb) { + return false; + } - bool need_yield = false; + bool need_yield = false; #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, - &usb_hid_dev->tx_packet_buffer[1], - sizeof(usb_hid_dev->tx_packet_buffer)-1, - NULL, - &need_yield); + bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, + &usb_hid_dev->tx_packet_buffer[1], + sizeof(usb_hid_dev->tx_packet_buffer) - 1, + NULL, + &need_yield); #else - bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, - &usb_hid_dev->tx_packet_buffer[2], - sizeof(usb_hid_dev->tx_packet_buffer)-2, - NULL, - &need_yield); + bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, + &usb_hid_dev->tx_packet_buffer[2], + sizeof(usb_hid_dev->tx_packet_buffer) - 2, + NULL, + &need_yield); #endif - if (bytes_to_tx == 0) { - return false; - } + if (bytes_to_tx == 0) { + return false; + } - /* - * Mark this endpoint as being tx active _before_ actually transmitting - * to make sure we don't race with the Tx completion interrupt - */ - usb_hid_dev->tx_active = true; + /* + * Mark this endpoint as being tx active _before_ actually transmitting + * to make sure we don't race with the Tx completion interrupt + */ + usb_hid_dev->tx_active = true; - /* Always set type as report ID */ - usb_hid_dev->tx_packet_buffer[0] = 1; + /* Always set type as report ID */ + usb_hid_dev->tx_packet_buffer[0] = 1; #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - PIOS_USBHOOK_EndpointTx(usb_hid_dev->cfg->data_tx_ep, - usb_hid_dev->tx_packet_buffer, - sizeof(usb_hid_dev->tx_packet_buffer)); + PIOS_USBHOOK_EndpointTx(usb_hid_dev->cfg->data_tx_ep, + usb_hid_dev->tx_packet_buffer, + sizeof(usb_hid_dev->tx_packet_buffer)); #else - usb_hid_dev->tx_packet_buffer[1] = bytes_to_tx; - PIOS_USBHOOK_EndpointTx(usb_hid_dev->cfg->data_tx_ep, - usb_hid_dev->tx_packet_buffer, - sizeof(usb_hid_dev->tx_packet_buffer)); + usb_hid_dev->tx_packet_buffer[1] = bytes_to_tx; + PIOS_USBHOOK_EndpointTx(usb_hid_dev->cfg->data_tx_ep, + usb_hid_dev->tx_packet_buffer, + sizeof(usb_hid_dev->tx_packet_buffer)); #endif #if defined(PIOS_INCLUDE_FREERTOS) - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ - return true; + return true; } -static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; +static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) +{ + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - /* Make sure this USB interface has been initialized */ - if (!usb_hid_dev->usb_if_enabled) { - return; - } + PIOS_Assert(valid); - if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { - return; - } + /* Make sure this USB interface has been initialized */ + if (!usb_hid_dev->usb_if_enabled) { + return; + } - // If endpoint was stalled and there is now space make it valid + if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { + return; + } + + // If endpoint was stalled and there is now space make it valid #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; #else - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; #endif - if (!usb_hid_dev->rx_active && (rx_bytes_avail >= max_payload_length)) { - PIOS_USBHOOK_EndpointRx(usb_hid_dev->cfg->data_rx_ep, - usb_hid_dev->rx_packet_buffer, - sizeof(usb_hid_dev->rx_packet_buffer)); - usb_hid_dev->rx_active = true; - } + if (!usb_hid_dev->rx_active && (rx_bytes_avail >= max_payload_length)) { + PIOS_USBHOOK_EndpointRx(usb_hid_dev->cfg->data_rx_ep, + usb_hid_dev->rx_packet_buffer, + sizeof(usb_hid_dev->rx_packet_buffer)); + usb_hid_dev->rx_active = true; + } } static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, __attribute__((unused)) uint16_t tx_bytes_avail) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - /* Make sure this USB interface has been initialized */ - if (!usb_hid_dev->usb_if_enabled) { - return; - } + PIOS_Assert(valid); - if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { - return; - } + /* Make sure this USB interface has been initialized */ + if (!usb_hid_dev->usb_if_enabled) { + return; + } - if (!usb_hid_dev->tx_active) { - /* Transmitter is not currently active, send a report */ - PIOS_USB_HID_SendReport(usb_hid_dev); - } + if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { + return; + } + + if (!usb_hid_dev->tx_active) { + /* Transmitter is not currently active, send a report */ + PIOS_USB_HID_SendReport(usb_hid_dev); + } } static void PIOS_USB_HID_RegisterRxCallback(uint32_t usbhid_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_hid_dev->rx_in_context = context; - usb_hid_dev->rx_in_cb = rx_in_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_hid_dev->rx_in_context = context; + usb_hid_dev->rx_in_cb = rx_in_cb; } static void PIOS_USB_HID_RegisterTxCallback(uint32_t usbhid_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; - bool valid = PIOS_USB_HID_validate(usb_hid_dev); - PIOS_Assert(valid); + bool valid = PIOS_USB_HID_validate(usb_hid_dev); - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - usb_hid_dev->tx_out_context = context; - usb_hid_dev->tx_out_cb = tx_out_cb; + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_hid_dev->tx_out_context = context; + usb_hid_dev->tx_out_cb = tx_out_cb; } static void PIOS_USB_HID_IF_Init(uint32_t usb_hid_id) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; - if (!PIOS_USB_HID_validate(usb_hid_dev)) { - return; - } - - /* Register endpoint specific callbacks with the USBHOOK layer */ - PIOS_USBHOOK_RegisterEpInCallback(usb_hid_dev->cfg->data_tx_ep, - sizeof(usb_hid_dev->tx_packet_buffer), - PIOS_USB_HID_EP_IN_Callback, - (uint32_t) usb_hid_dev); - PIOS_USBHOOK_RegisterEpOutCallback(usb_hid_dev->cfg->data_rx_ep, - sizeof(usb_hid_dev->rx_packet_buffer), - PIOS_USB_HID_EP_OUT_Callback, - (uint32_t) usb_hid_dev); - usb_hid_dev->usb_if_enabled = true; + if (!PIOS_USB_HID_validate(usb_hid_dev)) { + return; + } + /* Register endpoint specific callbacks with the USBHOOK layer */ + PIOS_USBHOOK_RegisterEpInCallback(usb_hid_dev->cfg->data_tx_ep, + sizeof(usb_hid_dev->tx_packet_buffer), + PIOS_USB_HID_EP_IN_Callback, + (uint32_t)usb_hid_dev); + PIOS_USBHOOK_RegisterEpOutCallback(usb_hid_dev->cfg->data_rx_ep, + sizeof(usb_hid_dev->rx_packet_buffer), + PIOS_USB_HID_EP_OUT_Callback, + (uint32_t)usb_hid_dev); + usb_hid_dev->usb_if_enabled = true; } static void PIOS_USB_HID_IF_DeInit(uint32_t usb_hid_id) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; - if (!PIOS_USB_HID_validate(usb_hid_dev)) { - return; - } + if (!PIOS_USB_HID_validate(usb_hid_dev)) { + return; + } - /* DeRegister endpoint specific callbacks with the USBHOOK layer */ - usb_hid_dev->usb_if_enabled = false; - PIOS_USBHOOK_DeRegisterEpInCallback(usb_hid_dev->cfg->data_tx_ep); - PIOS_USBHOOK_DeRegisterEpOutCallback(usb_hid_dev->cfg->data_rx_ep); + /* DeRegister endpoint specific callbacks with the USBHOOK layer */ + usb_hid_dev->usb_if_enabled = false; + PIOS_USBHOOK_DeRegisterEpInCallback(usb_hid_dev->cfg->data_tx_ep); + PIOS_USBHOOK_DeRegisterEpOutCallback(usb_hid_dev->cfg->data_rx_ep); } static uint8_t hid_protocol; @@ -350,85 +358,88 @@ static uint8_t hid_altset; static bool PIOS_USB_HID_IF_Setup(uint32_t usb_hid_id, struct usb_setup_request *req) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; - if (!PIOS_USB_HID_validate(usb_hid_dev)) { - return false; - } + if (!PIOS_USB_HID_validate(usb_hid_dev)) { + return false; + } - /* Make sure this is a request for an interface we know about */ - uint8_t ifnum = req->wIndex & 0xFF; - if (ifnum != usb_hid_dev->cfg->data_if) { - return (false); - } + /* Make sure this is a request for an interface we know about */ + uint8_t ifnum = req->wIndex & 0xFF; + if (ifnum != usb_hid_dev->cfg->data_if) { + return false; + } - switch (req->bmRequestType & (USB_REQ_TYPE_MASK | USB_REQ_RECIPIENT_MASK)) { - case (USB_REQ_TYPE_STANDARD | USB_REQ_RECIPIENT_INTERFACE): - switch (req->bRequest) { - case USB_REQ_GET_DESCRIPTOR: - switch (req->wValue >> 8) { - case USB_DESC_TYPE_REPORT: - PIOS_USBHOOK_CtrlTx(hid_report_desc.descriptor, - MIN(hid_report_desc.length, req->wLength)); - break; - case USB_DESC_TYPE_HID: - PIOS_USBHOOK_CtrlTx(hid_desc.descriptor, - MIN(hid_desc.length, req->wLength)); - break; - default: - /* Unhandled descriptor request */ - return false; - break; - } - break; - case USB_REQ_GET_INTERFACE: - PIOS_USBHOOK_CtrlTx(&hid_altset, 1); - break; - case USB_REQ_SET_INTERFACE: - hid_altset = (uint8_t)(req->wValue); - break; - default: - /* Unhandled standard request */ - return false; - break; - } - break; - case (USB_REQ_TYPE_CLASS | USB_REQ_RECIPIENT_INTERFACE): - switch (req->bRequest) { - case USB_HID_REQ_SET_PROTOCOL: - hid_protocol = (uint8_t)(req->wValue); - break; - case USB_HID_REQ_GET_PROTOCOL: - PIOS_USBHOOK_CtrlTx(&hid_protocol, 1); - break; - case USB_HID_REQ_GET_REPORT: - { - /* Give back a dummy input report */ - uint8_t dummy_report[2] = { - [0] = req->wValue >> 8, /* Report ID */ - [1] = 0x00, - }; - PIOS_USBHOOK_CtrlTx(dummy_report, sizeof(dummy_report)); - } - break; - default: - /* Unhandled class request */ - return false; - break; - } - break; - default: - /* Unhandled request */ - return false; - } + switch (req->bmRequestType & (USB_REQ_TYPE_MASK | USB_REQ_RECIPIENT_MASK)) { + case (USB_REQ_TYPE_STANDARD | USB_REQ_RECIPIENT_INTERFACE): + switch (req->bRequest) { + case USB_REQ_GET_DESCRIPTOR: + switch (req->wValue >> 8) { + case USB_DESC_TYPE_REPORT: + PIOS_USBHOOK_CtrlTx(hid_report_desc.descriptor, + MIN(hid_report_desc.length, req->wLength)); + break; + case USB_DESC_TYPE_HID: + PIOS_USBHOOK_CtrlTx(hid_desc.descriptor, + MIN(hid_desc.length, req->wLength)); + break; + default: + /* Unhandled descriptor request */ + return false; - return true; + break; + } + break; + case USB_REQ_GET_INTERFACE: + PIOS_USBHOOK_CtrlTx(&hid_altset, 1); + break; + case USB_REQ_SET_INTERFACE: + hid_altset = (uint8_t)(req->wValue); + break; + default: + /* Unhandled standard request */ + return false; + + break; + } + break; + case (USB_REQ_TYPE_CLASS | USB_REQ_RECIPIENT_INTERFACE): + switch (req->bRequest) { + case USB_HID_REQ_SET_PROTOCOL: + hid_protocol = (uint8_t)(req->wValue); + break; + case USB_HID_REQ_GET_PROTOCOL: + PIOS_USBHOOK_CtrlTx(&hid_protocol, 1); + break; + case USB_HID_REQ_GET_REPORT: + { + /* Give back a dummy input report */ + uint8_t dummy_report[2] = { + [0] = req->wValue >> 8, /* Report ID */ + [1] = 0x00, + }; + PIOS_USBHOOK_CtrlTx(dummy_report, sizeof(dummy_report)); + } + break; + default: + /* Unhandled class request */ + return false; + + break; + } + break; + default: + /* Unhandled request */ + return false; + } + + return true; } static void PIOS_USB_HID_IF_CtrlDataOut(__attribute__((unused)) uint32_t usb_hid_id, __attribute__((unused)) struct usb_setup_request *req) { - /* HID devices don't have any OUT data stages on the control endpoint */ - PIOS_Assert(0); + /* HID devices don't have any OUT data stages on the control endpoint */ + PIOS_Assert(0); } /** @@ -437,21 +448,21 @@ static void PIOS_USB_HID_IF_CtrlDataOut(__attribute__((unused)) uint32_t usb_hid */ static bool PIOS_USB_HID_EP_IN_Callback(uint32_t usb_hid_id, __attribute__((unused)) uint8_t epnum, __attribute__((unused)) uint16_t len) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; - if (!PIOS_USB_HID_validate(usb_hid_dev)) { - return false; - } + if (!PIOS_USB_HID_validate(usb_hid_dev)) { + return false; + } - if (PIOS_USB_CheckAvailable(usb_hid_dev->lower_id) && - PIOS_USB_HID_SendReport(usb_hid_dev)) { - /* More data has been queued, leave tx_active set to true */ - return true; - } else { - /* Nothing new sent, transmitter is now inactive */ - usb_hid_dev->tx_active = false; - return false; - } + if (PIOS_USB_CheckAvailable(usb_hid_dev->lower_id) && + PIOS_USB_HID_SendReport(usb_hid_dev)) { + /* More data has been queued, leave tx_active set to true */ + return true; + } else { + /* Nothing new sent, transmitter is now inactive */ + usb_hid_dev->tx_active = false; + return false; + } } /** @@ -459,65 +470,65 @@ static bool PIOS_USB_HID_EP_IN_Callback(uint32_t usb_hid_id, __attribute__((unus */ static bool PIOS_USB_HID_EP_OUT_Callback(uint32_t usb_hid_id, __attribute__((unused)) uint8_t epnum, uint16_t len) { - struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; - if (!PIOS_USB_HID_validate(usb_hid_dev)) { - return false; - } + if (!PIOS_USB_HID_validate(usb_hid_dev)) { + return false; + } - if (len > sizeof(usb_hid_dev->rx_packet_buffer)) { - len = sizeof(usb_hid_dev->rx_packet_buffer); - } + if (len > sizeof(usb_hid_dev->rx_packet_buffer)) { + len = sizeof(usb_hid_dev->rx_packet_buffer); + } - if (!usb_hid_dev->rx_in_cb) { - /* No Rx call back registered, disable the receiver */ - usb_hid_dev->rx_active = false; - return false; - } + if (!usb_hid_dev->rx_in_cb) { + /* No Rx call back registered, disable the receiver */ + usb_hid_dev->rx_active = false; + return false; + } - /* The first byte is report ID (not checked), the second byte is the valid data length */ - uint16_t headroom; - bool need_yield = false; + /* The first byte is report ID (not checked), the second byte is the valid data length */ + uint16_t headroom; + bool need_yield = false; #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, - &usb_hid_dev->rx_packet_buffer[1], - len-1, - &headroom, - &need_yield); + (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, + &usb_hid_dev->rx_packet_buffer[1], + len - 1, + &headroom, + &need_yield); #else - (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, - &usb_hid_dev->rx_packet_buffer[2], - usb_hid_dev->rx_packet_buffer[1], - &headroom, - &need_yield); + (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, + &usb_hid_dev->rx_packet_buffer[2], + usb_hid_dev->rx_packet_buffer[1], + &headroom, + &need_yield); #endif #ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; #else - uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; #endif - bool rc; - if (headroom >= max_payload_length) { - /* We have room for a maximum length message */ - PIOS_USBHOOK_EndpointRx(usb_hid_dev->cfg->data_rx_ep, - usb_hid_dev->rx_packet_buffer, - sizeof(usb_hid_dev->rx_packet_buffer)); - rc = true; - } else { - /* Not enough room left for a message, apply backpressure */ - usb_hid_dev->rx_active = false; - rc = false; - } + bool rc; + if (headroom >= max_payload_length) { + /* We have room for a maximum length message */ + PIOS_USBHOOK_EndpointRx(usb_hid_dev->cfg->data_rx_ep, + usb_hid_dev->rx_packet_buffer, + sizeof(usb_hid_dev->rx_packet_buffer)); + rc = true; + } else { + /* Not enough room left for a message, apply backpressure */ + usb_hid_dev->rx_active = false; + rc = false; + } #if defined(PIOS_INCLUDE_FREERTOS) - if (need_yield) { - vPortYieldFromISR(); - } -#endif /* PIOS_INCLUDE_FREERTOS */ + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ - return rc; + return rc; } #endif /* PIOS_INCLUDE_USB_HID */ diff --git a/flight/pios/stm32f4xx/pios_usbhook.c b/flight/pios/stm32f4xx/pios_usbhook.c index 5f8b1dd0d..7e8840c98 100644 --- a/flight/pios/stm32f4xx/pios_usbhook.c +++ b/flight/pios/stm32f4xx/pios_usbhook.c @@ -32,47 +32,47 @@ #ifdef PIOS_INCLUDE_USB -#include "pios_usb.h" /* PIOS_USB_* */ +#include "pios_usb.h" /* PIOS_USB_* */ #include "pios_usbhook.h" -#include "pios_usb_defs.h" /* struct usb_* */ -#include "pios_usb_cdc_priv.h" /* PIOS_USB_CDC_* */ +#include "pios_usb_defs.h" /* struct usb_* */ +#include "pios_usb_cdc_priv.h" /* PIOS_USB_CDC_* */ #include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ /* STM32 USB Library Definitions */ -#include "usb_core.h" /* USBD_Class_cb_TypeDef */ -#include "usbd_core.h" /* USBD_Init USBD_OK*/ -#include "usbd_ioreq.h" /* USBD_CtlPrepareRx, USBD_CtlSendData */ -#include "usbd_req.h" /* USBD_CtlError */ -#include "usb_dcd_int.h" /* USBD_OTG_ISR_Handler */ +#include "usb_core.h" /* USBD_Class_cb_TypeDef */ +#include "usbd_core.h" /* USBD_Init USBD_OK*/ +#include "usbd_ioreq.h" /* USBD_CtlPrepareRx, USBD_CtlSendData */ +#include "usbd_req.h" /* USBD_CtlError */ +#include "usb_dcd_int.h" /* USBD_OTG_ISR_Handler */ /* * External API */ static struct pios_usbhook_descriptor Device_Descriptor; -void PIOS_USBHOOK_RegisterDevice(const uint8_t * desc, uint16_t length) +void PIOS_USBHOOK_RegisterDevice(const uint8_t *desc, uint16_t length) { - Device_Descriptor.descriptor = desc; - Device_Descriptor.length = length; + Device_Descriptor.descriptor = desc; + Device_Descriptor.length = length; } static struct pios_usbhook_descriptor String_Descriptor[4]; -void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t * desc, uint16_t desc_size) +void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t *desc, uint16_t desc_size) { - if (string_id < NELEMENTS(String_Descriptor)) { - String_Descriptor[string_id].descriptor = desc; - String_Descriptor[string_id].length = desc_size; - } + if (string_id < NELEMENTS(String_Descriptor)) { + String_Descriptor[string_id].descriptor = desc; + String_Descriptor[string_id].length = desc_size; + } } static struct pios_usbhook_descriptor Config_Descriptor; -void PIOS_USBHOOK_RegisterConfig(__attribute__((unused)) uint8_t config_id, const uint8_t * desc, uint16_t desc_size) +void PIOS_USBHOOK_RegisterConfig(__attribute__((unused)) uint8_t config_id, const uint8_t *desc, uint16_t desc_size) { - Config_Descriptor.descriptor = desc; - Config_Descriptor.length = desc_size; + Config_Descriptor.descriptor = desc; + Config_Descriptor.length = desc_size; } static USB_OTG_CORE_HANDLE pios_usb_otg_core_handle; @@ -82,409 +82,408 @@ static USBD_Usr_cb_TypeDef user_callbacks; void PIOS_USBHOOK_Activate(void) { - USBD_Init(&pios_usb_otg_core_handle, - USB_OTG_FS_CORE_ID, - &device_callbacks, - &class_callbacks, - &user_callbacks); + USBD_Init(&pios_usb_otg_core_handle, + USB_OTG_FS_CORE_ID, + &device_callbacks, + &class_callbacks, + &user_callbacks); } void PIOS_USBHOOK_Deactivate(void) { - DCD_DevDisconnect(&pios_usb_otg_core_handle); - USBD_DeInit(&pios_usb_otg_core_handle); - USB_OTG_StopDevice(&pios_usb_otg_core_handle); + DCD_DevDisconnect(&pios_usb_otg_core_handle); + USBD_DeInit(&pios_usb_otg_core_handle); + USB_OTG_StopDevice(&pios_usb_otg_core_handle); } void OTG_FS_IRQHandler(void) { - if(!USBD_OTG_ISR_Handler(&pios_usb_otg_core_handle)) { - /* spurious interrupt, disable IRQ */ - - } + if (!USBD_OTG_ISR_Handler(&pios_usb_otg_core_handle)) { + /* spurious interrupt, disable IRQ */ + } } struct usb_if_entry { - struct pios_usb_ifops *ifops; - uint32_t context; + struct pios_usb_ifops *ifops; + uint32_t context; }; static struct usb_if_entry usb_if_table[3]; -void PIOS_USBHOOK_RegisterIfOps(uint8_t ifnum, struct pios_usb_ifops * ifops, uint32_t context) +void PIOS_USBHOOK_RegisterIfOps(uint8_t ifnum, struct pios_usb_ifops *ifops, uint32_t context) { - PIOS_Assert(ifnum < NELEMENTS(usb_if_table)); - PIOS_Assert(ifops); + PIOS_Assert(ifnum < NELEMENTS(usb_if_table)); + PIOS_Assert(ifops); - usb_if_table[ifnum].ifops = ifops; - usb_if_table[ifnum].context = context; + usb_if_table[ifnum].ifops = ifops; + usb_if_table[ifnum].context = context; } struct usb_ep_entry { - pios_usbhook_epcb cb; - uint32_t context; - uint16_t max_len; + pios_usbhook_epcb cb; + uint32_t context; + uint16_t max_len; }; static struct usb_ep_entry usb_epin_table[6]; void PIOS_USBHOOK_RegisterEpInCallback(uint8_t epnum, uint16_t max_len, pios_usbhook_epcb cb, uint32_t context) { - PIOS_Assert(epnum < NELEMENTS(usb_epin_table)); - PIOS_Assert(cb); + PIOS_Assert(epnum < NELEMENTS(usb_epin_table)); + PIOS_Assert(cb); - usb_epin_table[epnum].cb = cb; - usb_epin_table[epnum].context = context; - usb_epin_table[epnum].max_len = max_len; + usb_epin_table[epnum].cb = cb; + usb_epin_table[epnum].context = context; + usb_epin_table[epnum].max_len = max_len; - DCD_EP_Open(&pios_usb_otg_core_handle, - epnum | 0x80, - max_len, - USB_OTG_EP_INT); - /* - * FIXME do not hardcode endpoint type - */ + DCD_EP_Open(&pios_usb_otg_core_handle, + epnum | 0x80, + max_len, + USB_OTG_EP_INT); + /* + * FIXME do not hardcode endpoint type + */ } extern void PIOS_USBHOOK_DeRegisterEpInCallback(uint8_t epnum) { - PIOS_Assert(epnum < NELEMENTS(usb_epin_table)); + PIOS_Assert(epnum < NELEMENTS(usb_epin_table)); - usb_epin_table[epnum].cb = NULL; + usb_epin_table[epnum].cb = NULL; - DCD_EP_Close(&pios_usb_otg_core_handle, epnum | 0x80); + DCD_EP_Close(&pios_usb_otg_core_handle, epnum | 0x80); } static struct usb_ep_entry usb_epout_table[6]; void PIOS_USBHOOK_RegisterEpOutCallback(uint8_t epnum, uint16_t max_len, pios_usbhook_epcb cb, uint32_t context) { - PIOS_Assert(epnum < NELEMENTS(usb_epout_table)); - PIOS_Assert(cb); + PIOS_Assert(epnum < NELEMENTS(usb_epout_table)); + PIOS_Assert(cb); - usb_epout_table[epnum].cb = cb; - usb_epout_table[epnum].context = context; - usb_epout_table[epnum].max_len = max_len; + usb_epout_table[epnum].cb = cb; + usb_epout_table[epnum].context = context; + usb_epout_table[epnum].max_len = max_len; - DCD_EP_Open(&pios_usb_otg_core_handle, - epnum, - max_len, - USB_OTG_EP_INT); - /* - * FIXME do not hardcode endpoint type - */ + DCD_EP_Open(&pios_usb_otg_core_handle, + epnum, + max_len, + USB_OTG_EP_INT); + /* + * FIXME do not hardcode endpoint type + */ - /* - * Make sure we refuse OUT transactions until we explicitly - * connect a receive buffer with PIOS_USBHOOK_EndpointRx(). - * - * Without this, the ST USB code will receive on this endpoint - * and blindly write the data to a NULL pointer which will - * have the side effect of placing the internal flash into an - * errored state. Address 0x0000_0000 is aliased into internal - * flash via the "Section 2.4 Boot configuration" BOOT0/1 pins. - */ - DCD_SetEPStatus(&pios_usb_otg_core_handle, - epnum, - USB_OTG_EP_RX_NAK); + /* + * Make sure we refuse OUT transactions until we explicitly + * connect a receive buffer with PIOS_USBHOOK_EndpointRx(). + * + * Without this, the ST USB code will receive on this endpoint + * and blindly write the data to a NULL pointer which will + * have the side effect of placing the internal flash into an + * errored state. Address 0x0000_0000 is aliased into internal + * flash via the "Section 2.4 Boot configuration" BOOT0/1 pins. + */ + DCD_SetEPStatus(&pios_usb_otg_core_handle, + epnum, + USB_OTG_EP_RX_NAK); } extern void PIOS_USBHOOK_DeRegisterEpOutCallback(uint8_t epnum) { - PIOS_Assert(epnum < NELEMENTS(usb_epout_table)); + PIOS_Assert(epnum < NELEMENTS(usb_epout_table)); - usb_epout_table[epnum].cb = NULL; + usb_epout_table[epnum].cb = NULL; - DCD_EP_Close(&pios_usb_otg_core_handle, epnum); + DCD_EP_Close(&pios_usb_otg_core_handle, epnum); } void PIOS_USBHOOK_CtrlTx(const uint8_t *buf, uint16_t len) { - USBD_CtlSendData(&pios_usb_otg_core_handle, buf, len); + USBD_CtlSendData(&pios_usb_otg_core_handle, buf, len); } void PIOS_USBHOOK_CtrlRx(uint8_t *buf, uint16_t len) { - USBD_CtlPrepareRx(&pios_usb_otg_core_handle, buf, len); + USBD_CtlPrepareRx(&pios_usb_otg_core_handle, buf, len); } void PIOS_USBHOOK_EndpointTx(uint8_t epnum, const uint8_t *buf, uint16_t len) { - if (pios_usb_otg_core_handle.dev.device_status == USB_OTG_CONFIGURED) { - DCD_EP_Tx(&pios_usb_otg_core_handle, epnum, buf, len); - } + if (pios_usb_otg_core_handle.dev.device_status == USB_OTG_CONFIGURED) { + DCD_EP_Tx(&pios_usb_otg_core_handle, epnum, buf, len); + } } void PIOS_USBHOOK_EndpointRx(uint8_t epnum, uint8_t *buf, uint16_t len) { - DCD_EP_PrepareRx(&pios_usb_otg_core_handle, epnum, buf, len); + DCD_EP_PrepareRx(&pios_usb_otg_core_handle, epnum, buf, len); } /* * Device level hooks into STM USB library */ -static const uint8_t * PIOS_USBHOOK_DEV_GetDeviceDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_DEV_GetDeviceDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { - *length = Device_Descriptor.length; - return Device_Descriptor.descriptor; + *length = Device_Descriptor.length; + return Device_Descriptor.descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetLangIDStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_DEV_GetLangIDStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { - *length = String_Descriptor[USB_STRING_DESC_LANG].length; - return String_Descriptor[USB_STRING_DESC_LANG].descriptor; + *length = String_Descriptor[USB_STRING_DESC_LANG].length; + return String_Descriptor[USB_STRING_DESC_LANG].descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetManufacturerStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_DEV_GetManufacturerStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { - *length = String_Descriptor[USB_STRING_DESC_VENDOR].length; - return String_Descriptor[USB_STRING_DESC_VENDOR].descriptor; + *length = String_Descriptor[USB_STRING_DESC_VENDOR].length; + return String_Descriptor[USB_STRING_DESC_VENDOR].descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetProductStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_DEV_GetProductStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { - *length = String_Descriptor[USB_STRING_DESC_PRODUCT].length; - return String_Descriptor[USB_STRING_DESC_PRODUCT].descriptor; + *length = String_Descriptor[USB_STRING_DESC_PRODUCT].length; + return String_Descriptor[USB_STRING_DESC_PRODUCT].descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetSerialStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_DEV_GetSerialStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { - *length = String_Descriptor[USB_STRING_DESC_SERIAL].length; - return String_Descriptor[USB_STRING_DESC_SERIAL].descriptor; + *length = String_Descriptor[USB_STRING_DESC_SERIAL].length; + return String_Descriptor[USB_STRING_DESC_SERIAL].descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetConfigurationStrDescriptor(__attribute__((unused)) uint8_t speed, __attribute__((unused)) uint16_t *length) +static const uint8_t *PIOS_USBHOOK_DEV_GetConfigurationStrDescriptor(__attribute__((unused)) uint8_t speed, __attribute__((unused)) uint16_t *length) { - return NULL; + return NULL; } -static const uint8_t * PIOS_USBHOOK_DEV_GetInterfaceStrDescriptor(__attribute__((unused)) uint8_t speed, __attribute__((unused)) uint16_t *length) +static const uint8_t *PIOS_USBHOOK_DEV_GetInterfaceStrDescriptor(__attribute__((unused)) uint8_t speed, __attribute__((unused)) uint16_t *length) { - return NULL; + return NULL; } static USBD_DEVICE device_callbacks = { - .GetDeviceDescriptor = PIOS_USBHOOK_DEV_GetDeviceDescriptor, - .GetLangIDStrDescriptor = PIOS_USBHOOK_DEV_GetLangIDStrDescriptor, - .GetManufacturerStrDescriptor = PIOS_USBHOOK_DEV_GetManufacturerStrDescriptor, - .GetProductStrDescriptor = PIOS_USBHOOK_DEV_GetProductStrDescriptor, - .GetSerialStrDescriptor = PIOS_USBHOOK_DEV_GetSerialStrDescriptor, - .GetConfigurationStrDescriptor = PIOS_USBHOOK_DEV_GetConfigurationStrDescriptor, - .GetInterfaceStrDescriptor = PIOS_USBHOOK_DEV_GetInterfaceStrDescriptor, + .GetDeviceDescriptor = PIOS_USBHOOK_DEV_GetDeviceDescriptor, + .GetLangIDStrDescriptor = PIOS_USBHOOK_DEV_GetLangIDStrDescriptor, + .GetManufacturerStrDescriptor = PIOS_USBHOOK_DEV_GetManufacturerStrDescriptor, + .GetProductStrDescriptor = PIOS_USBHOOK_DEV_GetProductStrDescriptor, + .GetSerialStrDescriptor = PIOS_USBHOOK_DEV_GetSerialStrDescriptor, + .GetConfigurationStrDescriptor = PIOS_USBHOOK_DEV_GetConfigurationStrDescriptor, + .GetInterfaceStrDescriptor = PIOS_USBHOOK_DEV_GetInterfaceStrDescriptor, }; static void PIOS_USBHOOK_USR_Init(void) { - PIOS_USB_ChangeConnectionState(false); + PIOS_USB_ChangeConnectionState(false); #if 1 - /* Force a physical disconnect/reconnect */ - DCD_DevDisconnect(&pios_usb_otg_core_handle); - DCD_DevConnect(&pios_usb_otg_core_handle); + /* Force a physical disconnect/reconnect */ + DCD_DevDisconnect(&pios_usb_otg_core_handle); + DCD_DevConnect(&pios_usb_otg_core_handle); #endif } static void PIOS_USBHOOK_USR_DeviceReset(__attribute__((unused)) uint8_t speed) { - PIOS_USB_ChangeConnectionState(false); + PIOS_USB_ChangeConnectionState(false); } static void PIOS_USBHOOK_USR_DeviceConfigured(void) { - PIOS_USB_ChangeConnectionState(true); + PIOS_USB_ChangeConnectionState(true); } static void PIOS_USBHOOK_USR_DeviceSuspended(void) { - /* Unhandled */ + /* Unhandled */ } static void PIOS_USBHOOK_USR_DeviceResumed(void) { - /* Unhandled */ + /* Unhandled */ } static void PIOS_USBHOOK_USR_DeviceConnected(void) { - /* NOP */ + /* NOP */ } static void PIOS_USBHOOK_USR_DeviceDisconnected(void) { - PIOS_USB_ChangeConnectionState(false); + PIOS_USB_ChangeConnectionState(false); } static USBD_Usr_cb_TypeDef user_callbacks = { - .Init = PIOS_USBHOOK_USR_Init, - .DeviceReset = PIOS_USBHOOK_USR_DeviceReset, - .DeviceConfigured = PIOS_USBHOOK_USR_DeviceConfigured, - .DeviceSuspended = PIOS_USBHOOK_USR_DeviceSuspended, - .DeviceResumed = PIOS_USBHOOK_USR_DeviceResumed, - .DeviceConnected = PIOS_USBHOOK_USR_DeviceConnected, - .DeviceDisconnected = PIOS_USBHOOK_USR_DeviceDisconnected, + .Init = PIOS_USBHOOK_USR_Init, + .DeviceReset = PIOS_USBHOOK_USR_DeviceReset, + .DeviceConfigured = PIOS_USBHOOK_USR_DeviceConfigured, + .DeviceSuspended = PIOS_USBHOOK_USR_DeviceSuspended, + .DeviceResumed = PIOS_USBHOOK_USR_DeviceResumed, + .DeviceConnected = PIOS_USBHOOK_USR_DeviceConnected, + .DeviceDisconnected = PIOS_USBHOOK_USR_DeviceDisconnected, }; static uint8_t PIOS_USBHOOK_CLASS_Init(__attribute__((unused)) void *pdev, __attribute__((unused)) uint8_t cfgidx) { - /* Call all of the registered init callbacks */ - for (uint8_t i = 0; i < NELEMENTS(usb_if_table); i++) { - struct usb_if_entry * usb_if = &(usb_if_table[i]); - if (usb_if->ifops && usb_if->ifops->init) { - usb_if->ifops->init(usb_if->context); + /* Call all of the registered init callbacks */ + for (uint8_t i = 0; i < NELEMENTS(usb_if_table); i++) { + struct usb_if_entry *usb_if = &(usb_if_table[i]); + if (usb_if->ifops && usb_if->ifops->init) { + usb_if->ifops->init(usb_if->context); + } } - } - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_DeInit(__attribute__((unused)) void *pdev, __attribute__((unused)) uint8_t cfgidx) { - /* Call all of the registered deinit callbacks */ - for (uint8_t i = 0; i < NELEMENTS(usb_if_table); i++) { - struct usb_if_entry * usb_if = &(usb_if_table[i]); - if (usb_if->ifops && usb_if->ifops->deinit) { - usb_if->ifops->deinit(usb_if->context); + /* Call all of the registered deinit callbacks */ + for (uint8_t i = 0; i < NELEMENTS(usb_if_table); i++) { + struct usb_if_entry *usb_if = &(usb_if_table[i]); + if (usb_if->ifops && usb_if->ifops->deinit) { + usb_if->ifops->deinit(usb_if->context); + } } - } - return USBD_OK; + return USBD_OK; } static struct usb_setup_request usb_ep0_active_req; static uint8_t PIOS_USBHOOK_CLASS_Setup(__attribute__((unused)) void *pdev, USB_SETUP_REQ *req) { - switch (req->bmRequest & (USB_REQ_TYPE_MASK | USB_REQ_RECIPIENT_MASK)) { - case (USB_REQ_TYPE_STANDARD | USB_REQ_RECIPIENT_INTERFACE): - case (USB_REQ_TYPE_CLASS | USB_REQ_RECIPIENT_INTERFACE): - { - uint8_t ifnum = LOBYTE(req->wIndex); - if ((ifnum < NELEMENTS(usb_if_table)) && - (usb_if_table[ifnum].ifops && usb_if_table[ifnum].ifops->setup)) { - usb_if_table[ifnum].ifops->setup(usb_if_table[ifnum].context, - (struct usb_setup_request *)req); - if (req->bmRequest & 0x80 && req->wLength > 0) { - /* Request is a host-to-device data setup packet, keep track of the request details for the EP0_RxReady call */ - usb_ep0_active_req.bmRequestType = req->bmRequest; - usb_ep0_active_req.bRequest = req->bRequest; - usb_ep0_active_req.wValue = req->wValue; - usb_ep0_active_req.wIndex = req->wIndex; - usb_ep0_active_req.wLength = req->wLength; - } - } else { - /* No Setup handler or Setup handler failed */ - USBD_CtlError (&pios_usb_otg_core_handle, req); - } - break; - } - default: - /* Unhandled Setup */ - USBD_CtlError (&pios_usb_otg_core_handle, req); - break; - } + switch (req->bmRequest & (USB_REQ_TYPE_MASK | USB_REQ_RECIPIENT_MASK)) { + case (USB_REQ_TYPE_STANDARD | USB_REQ_RECIPIENT_INTERFACE): + case (USB_REQ_TYPE_CLASS | USB_REQ_RECIPIENT_INTERFACE): + { + uint8_t ifnum = LOBYTE(req->wIndex); + if ((ifnum < NELEMENTS(usb_if_table)) && + (usb_if_table[ifnum].ifops && usb_if_table[ifnum].ifops->setup)) { + usb_if_table[ifnum].ifops->setup(usb_if_table[ifnum].context, + (struct usb_setup_request *)req); + if (req->bmRequest & 0x80 && req->wLength > 0) { + /* Request is a host-to-device data setup packet, keep track of the request details for the EP0_RxReady call */ + usb_ep0_active_req.bmRequestType = req->bmRequest; + usb_ep0_active_req.bRequest = req->bRequest; + usb_ep0_active_req.wValue = req->wValue; + usb_ep0_active_req.wIndex = req->wIndex; + usb_ep0_active_req.wLength = req->wLength; + } + } else { + /* No Setup handler or Setup handler failed */ + USBD_CtlError(&pios_usb_otg_core_handle, req); + } + break; + } + default: + /* Unhandled Setup */ + USBD_CtlError(&pios_usb_otg_core_handle, req); + break; + } - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_EP0_TxSent(__attribute__((unused)) void *pdev) { - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_EP0_RxReady(__attribute__((unused)) void *pdev) { - uint8_t ifnum = LOBYTE(usb_ep0_active_req.wIndex); + uint8_t ifnum = LOBYTE(usb_ep0_active_req.wIndex); - if ((ifnum < NELEMENTS(usb_if_table)) && - (usb_if_table[ifnum].ifops && usb_if_table[ifnum].ifops->ctrl_data_out)) { - usb_if_table[ifnum].ifops->ctrl_data_out(usb_if_table[ifnum].context, - &usb_ep0_active_req); - } + if ((ifnum < NELEMENTS(usb_if_table)) && + (usb_if_table[ifnum].ifops && usb_if_table[ifnum].ifops->ctrl_data_out)) { + usb_if_table[ifnum].ifops->ctrl_data_out(usb_if_table[ifnum].context, + &usb_ep0_active_req); + } - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_DataIn(void *pdev, uint8_t epnum) { - /* Make sure the previous transfer has completed before starting a new one */ - DCD_EP_Flush(pdev, epnum); /* NOT SURE IF THIS IS REQUIRED */ + /* Make sure the previous transfer has completed before starting a new one */ + DCD_EP_Flush(pdev, epnum); /* NOT SURE IF THIS IS REQUIRED */ - /* Remove the direction bit so we can use this as an index */ - uint8_t epnum_idx = epnum & 0x7F; + /* Remove the direction bit so we can use this as an index */ + uint8_t epnum_idx = epnum & 0x7F; - if ((epnum_idx < NELEMENTS(usb_epin_table)) && usb_epin_table[epnum_idx].cb) { - struct usb_ep_entry *ep = &(usb_epin_table[epnum_idx]); - if (!ep->cb(ep->context, epnum_idx, ep->max_len)) { - /* NOTE: use real endpoint number including direction bit */ - DCD_SetEPStatus(pdev, epnum, USB_OTG_EP_TX_NAK); - } - } + if ((epnum_idx < NELEMENTS(usb_epin_table)) && usb_epin_table[epnum_idx].cb) { + struct usb_ep_entry *ep = &(usb_epin_table[epnum_idx]); + if (!ep->cb(ep->context, epnum_idx, ep->max_len)) { + /* NOTE: use real endpoint number including direction bit */ + DCD_SetEPStatus(pdev, epnum, USB_OTG_EP_TX_NAK); + } + } - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_DataOut(void *pdev, uint8_t epnum) { - /* Remove the direction bit so we can use this as an index */ - uint8_t epnum_idx = epnum & 0x7F; + /* Remove the direction bit so we can use this as an index */ + uint8_t epnum_idx = epnum & 0x7F; - if ((epnum_idx < NELEMENTS(usb_epout_table)) && usb_epout_table[epnum_idx].cb) { - struct usb_ep_entry *ep = &(usb_epout_table[epnum_idx]); - if (!ep->cb(ep->context, epnum_idx, ep->max_len)) { - /* NOTE: use real endpoint number including direction bit */ - DCD_SetEPStatus(pdev, epnum, USB_OTG_EP_RX_NAK); - } - } + if ((epnum_idx < NELEMENTS(usb_epout_table)) && usb_epout_table[epnum_idx].cb) { + struct usb_ep_entry *ep = &(usb_epout_table[epnum_idx]); + if (!ep->cb(ep->context, epnum_idx, ep->max_len)) { + /* NOTE: use real endpoint number including direction bit */ + DCD_SetEPStatus(pdev, epnum, USB_OTG_EP_RX_NAK); + } + } - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_SOF(__attribute__((unused)) void *pdev) { - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_IsoINIncomplete(__attribute__((unused)) void *pdev) { - return USBD_OK; + return USBD_OK; } static uint8_t PIOS_USBHOOK_CLASS_IsoOUTIncomplete(__attribute__((unused)) void *pdev) { - return USBD_OK; + return USBD_OK; } -static const uint8_t * PIOS_USBHOOK_CLASS_GetConfigDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_CLASS_GetConfigDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { - *length = Config_Descriptor.length; - return Config_Descriptor.descriptor; + *length = Config_Descriptor.length; + return Config_Descriptor.descriptor; } #ifdef USB_OTG_HS_CORE -static const uint8_t * PIOS_USBHOOK_CLASS_GetOtherConfigDescriptor(uint8_t speed, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_CLASS_GetOtherConfigDescriptor(uint8_t speed, uint16_t *length) { - return PIOS_USBHOOK_CLASS_GetConfigDescriptor(speed, length); + return PIOS_USBHOOK_CLASS_GetConfigDescriptor(speed, length); } -#endif /* USB_OTG_HS_CORE */ +#endif /* USB_OTG_HS_CORE */ #ifdef USB_SUPPORT_USER_STRING_DESC -static const uint8_t * PIOS_USBHOOK_CLASS_GetUsrStrDescriptor(uint8_t speed, uint8_t index, uint16_t *length) +static const uint8_t *PIOS_USBHOOK_CLASS_GetUsrStrDescriptor(uint8_t speed, uint8_t index, uint16_t *length) { - return NULL; + return NULL; } -#endif /* USB_SUPPORT_USER_STRING_DESC */ +#endif /* USB_SUPPORT_USER_STRING_DESC */ static USBD_Class_cb_TypeDef class_callbacks = { - .Init = PIOS_USBHOOK_CLASS_Init, - .DeInit = PIOS_USBHOOK_CLASS_DeInit, - .Setup = PIOS_USBHOOK_CLASS_Setup, - .EP0_TxSent = PIOS_USBHOOK_CLASS_EP0_TxSent, - .EP0_RxReady = PIOS_USBHOOK_CLASS_EP0_RxReady, - .DataIn = PIOS_USBHOOK_CLASS_DataIn, - .DataOut = PIOS_USBHOOK_CLASS_DataOut, - .SOF = PIOS_USBHOOK_CLASS_SOF, - .IsoINIncomplete = PIOS_USBHOOK_CLASS_IsoINIncomplete, - .IsoOUTIncomplete = PIOS_USBHOOK_CLASS_IsoOUTIncomplete, - .GetConfigDescriptor = PIOS_USBHOOK_CLASS_GetConfigDescriptor, + .Init = PIOS_USBHOOK_CLASS_Init, + .DeInit = PIOS_USBHOOK_CLASS_DeInit, + .Setup = PIOS_USBHOOK_CLASS_Setup, + .EP0_TxSent = PIOS_USBHOOK_CLASS_EP0_TxSent, + .EP0_RxReady = PIOS_USBHOOK_CLASS_EP0_RxReady, + .DataIn = PIOS_USBHOOK_CLASS_DataIn, + .DataOut = PIOS_USBHOOK_CLASS_DataOut, + .SOF = PIOS_USBHOOK_CLASS_SOF, + .IsoINIncomplete = PIOS_USBHOOK_CLASS_IsoINIncomplete, + .IsoOUTIncomplete = PIOS_USBHOOK_CLASS_IsoOUTIncomplete, + .GetConfigDescriptor = PIOS_USBHOOK_CLASS_GetConfigDescriptor, #ifdef USB_OTG_HS_CORE - .GetOtherConfigDescriptor = PIOS_USBHOOK_CLASS_GetOtherConfigDescriptor, -#endif /* USB_OTG_HS_CORE */ + .GetOtherConfigDescriptor = PIOS_USBHOOK_CLASS_GetOtherConfigDescriptor, +#endif /* USB_OTG_HS_CORE */ #ifdef USB_SUPPORT_USER_STRING_DESC - .GetUsrStrDescriptor = PIOS_USBHOOK_CLASS_GetUsrStrDescriptor, -#endif /* USB_SUPPORT_USER_STRING_DESC */ + .GetUsrStrDescriptor = PIOS_USBHOOK_CLASS_GetUsrStrDescriptor, +#endif /* USB_SUPPORT_USER_STRING_DESC */ }; #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/stm32f4xx/pios_wdg.c b/flight/pios/stm32f4xx/pios_wdg.c index f1a5e5d10..ab34a03ed 100644 --- a/flight/pios/stm32f4xx/pios_wdg.c +++ b/flight/pios/stm32f4xx/pios_wdg.c @@ -8,12 +8,12 @@ * * @file pios_spi.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Hardware Abstraction Layer for SPI ports of STM32 * @see The GNU Public License (GPL) Version 3 * @notes * - * The PIOS Watchdog provides a HAL to initialize a watchdog + * The PIOS Watchdog provides a HAL to initialize a watchdog * *****************************************************************************/ /* @@ -45,18 +45,18 @@ #include "stm32f4xx_rtc.h" static struct wdg_configuration { - uint16_t used_flags; - uint16_t bootup_flags; + uint16_t used_flags; + uint16_t bootup_flags; } wdg_configuration; -/** +/** * @brief Initialize the watchdog timer for a specified timeout * - * It is important to note that this function returns the achieved timeout + * It is important to note that this function returns the achieved timeout * for this hardware. For hardware independence this should be checked when - * scheduling updates. Other hardware dependent details may need to be - * considered such as a window time which sets a minimum update time, - * and this function should return a recommended delay for clearing. + * scheduling updates. Other hardware dependent details may need to be + * considered such as a window time which sets a minimum update time, + * and this function should return a recommended delay for clearing. * * For the STM32 nominal clock rate is 32 khz, but for the maximum clock rate of * 60 khz and a prescaler of 4 yields a clock rate of 15 khz. The delay that is @@ -67,106 +67,108 @@ static struct wdg_configuration { */ uint16_t PIOS_WDG_Init() { - uint16_t delay = ((uint32_t) PIOS_WATCHDOG_TIMEOUT * 60) / 16; - if (delay > 0x0fff) - delay = 0x0fff; -#if defined(PIOS_INCLUDE_WDG) - DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); // make the watchdog stop counting in debug mode - IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); - IWDG_SetPrescaler(IWDG_Prescaler_16); - IWDG_SetReload(delay); - IWDG_ReloadCounter(); - IWDG_Enable(); + uint16_t delay = ((uint32_t)PIOS_WATCHDOG_TIMEOUT * 60) / 16; - // watchdog flags now stored in backup registers - PWR_BackupAccessCmd(ENABLE); - - wdg_configuration.bootup_flags = RTC_ReadBackupRegister(PIOS_WDG_REGISTER); + if (delay > 0x0fff) { + delay = 0x0fff; + } +#if defined(PIOS_INCLUDE_WDG) + DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); // make the watchdog stop counting in debug mode + IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); + IWDG_SetPrescaler(IWDG_Prescaler_16); + IWDG_SetReload(delay); + IWDG_ReloadCounter(); + IWDG_Enable(); + + // watchdog flags now stored in backup registers + PWR_BackupAccessCmd(ENABLE); + + wdg_configuration.bootup_flags = RTC_ReadBackupRegister(PIOS_WDG_REGISTER); #endif - return delay; + return delay; } /** - * @brief Register a module against the watchdog - * + * @brief Register a module against the watchdog + * * There are two ways to use PIOS WDG: this is for when - * multiple modules must be monitored. In this case they - * must first register against the watchdog system and + * multiple modules must be monitored. In this case they + * must first register against the watchdog system and * only when all of the modules have been updated with the - * watchdog be cleared. Each module must have its own - * bit in the 16 bit + * watchdog be cleared. Each module must have its own + * bit in the 16 bit * * @param[in] flag the bit this module wants to use * @returns True if that bit is unregistered */ -bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) +bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) { - // flag are being registered so we are in module initialization phase - // clear the WDG to prevent timeout while initializing modules. (OP-815) - PIOS_WDG_Clear(); + // flag are being registered so we are in module initialization phase + // clear the WDG to prevent timeout while initializing modules. (OP-815) + PIOS_WDG_Clear(); - /* Fail if flag already registered */ - if(wdg_configuration.used_flags & flag_requested) - return false; - - // FIXME: Protect with semaphore - wdg_configuration.used_flags |= flag_requested; - - return true; + /* Fail if flag already registered */ + if (wdg_configuration.used_flags & flag_requested) { + return false; + } + + // FIXME: Protect with semaphore + wdg_configuration.used_flags |= flag_requested; + + return true; } /** * @brief Function called by modules to indicate they are still running * - * This function will set this flag in the active flags register (which is + * This function will set this flag in the active flags register (which is * a backup regsiter) and if all the registered flags are set will clear * the watchdog and set only this flag in the backup register * * @param[in] flag the flag to set * @return true if the watchdog cleared, false if flags are pending */ -bool PIOS_WDG_UpdateFlag(uint16_t flag) -{ - // we can probably avoid using a semaphore here which will be good for - // efficiency and not blocking critical tasks. race condition could - // overwrite their flag update, but unlikely to block _all_ of them - // for the timeout window - uint16_t cur_flags = RTC_ReadBackupRegister(PIOS_WDG_REGISTER); - - if((cur_flags | flag) == wdg_configuration.used_flags) { - PIOS_WDG_Clear(); - RTC_WriteBackupRegister(PIOS_WDG_REGISTER, flag); - return true; - } else { - RTC_WriteBackupRegister(PIOS_WDG_REGISTER, cur_flags | flag); - return false; - } - +bool PIOS_WDG_UpdateFlag(uint16_t flag) +{ + // we can probably avoid using a semaphore here which will be good for + // efficiency and not blocking critical tasks. race condition could + // overwrite their flag update, but unlikely to block _all_ of them + // for the timeout window + uint16_t cur_flags = RTC_ReadBackupRegister(PIOS_WDG_REGISTER); + + if ((cur_flags | flag) == wdg_configuration.used_flags) { + PIOS_WDG_Clear(); + RTC_WriteBackupRegister(PIOS_WDG_REGISTER, flag); + return true; + } else { + RTC_WriteBackupRegister(PIOS_WDG_REGISTER, cur_flags | flag); + return false; + } } -/** +/** * @brief Returns the flags that were set at bootup - * - * This is used for diagnostics, if only one flag not set this + * + * This is used for diagnostics, if only one flag not set this * was likely the module that wasn't running before reset - * + * * @return The active flags register from bootup */ uint16_t PIOS_WDG_GetBootupFlags() { - return wdg_configuration.bootup_flags; + return wdg_configuration.bootup_flags; } -/** +/** * @brief Returns the currently active flags - * + * * For external monitoring - * + * * @return The active flags register */ uint16_t PIOS_WDG_GetActiveFlags() { - return RTC_ReadBackupRegister(PIOS_WDG_REGISTER); + return RTC_ReadBackupRegister(PIOS_WDG_REGISTER); } /** @@ -177,7 +179,7 @@ uint16_t PIOS_WDG_GetActiveFlags() void PIOS_WDG_Clear(void) { #if defined(PIOS_INCLUDE_WDG) - IWDG_ReloadCounter(); + IWDG_ReloadCounter(); #endif } diff --git a/flight/pios/stm32f4xx/startup.c b/flight/pios/stm32f4xx/startup.c index 933481714..ab17d61c9 100644 --- a/flight/pios/stm32f4xx/startup.c +++ b/flight/pios/stm32f4xx/startup.c @@ -30,83 +30,83 @@ #include /* prototype for main() that tells us not to worry about it possibly returning */ -extern int main(void) __attribute__((noreturn)); +extern int main(void) __attribute__((noreturn)); /* prototype our _main() to avoid prolog/epilog insertion and other related junk */ -void _main(void) __attribute__((noreturn, naked, no_instrument_function)); +void _main(void) __attribute__((noreturn, naked, no_instrument_function)); /** default handler for CPU exceptions */ -static void default_cpu_handler(void) __attribute__((noreturn, naked, no_instrument_function)); +static void default_cpu_handler(void) __attribute__((noreturn, naked, no_instrument_function)); /** BSS symbols XXX should have a header that defines all of these */ -extern char _sbss, _ebss; +extern char _sbss, _ebss; /** DATA symbols XXX should have a header that defines all of these */ -extern char _sidata, _sdata, _edata, _sfast, _efast; +extern char _sidata, _sdata, _edata, _sfast, _efast; /** The bootstrap/IRQ stack XXX should define size somewhere else */ -char irq_stack[1024] __attribute__((section(".irqstack"))); +char irq_stack[1024] __attribute__((section(".irqstack"))); /** exception handler */ -typedef void (vector)(void); +typedef void (vector)(void); /** CortexM3 CPU vectors */ struct cm3_vectors { - void *initial_stack; - vector *entry; - vector *vectors[14]; + void *initial_stack; + vector *entry; + vector *vectors[14]; }; /** * Initial startup code. */ -void -_main(void) +void _main(void) { - // load the stack base for the current stack before we attempt to branch to any function - // that might bounds-check the stack - asm volatile ("mov r10, %0" : : "r" (&irq_stack[0]) : ); + // load the stack base for the current stack before we attempt to branch to any function + // that might bounds-check the stack + asm volatile ("mov r10, %0" : : "r" (&irq_stack[0]) :); - /* enable usage, bus and memory faults */ - SCB->SHCSR |= SCB_SHCSR_USGFAULTENA_Msk | SCB_SHCSR_BUSFAULTENA_Msk | SCB_SHCSR_MEMFAULTENA_Msk; + /* enable usage, bus and memory faults */ + SCB->SHCSR |= SCB_SHCSR_USGFAULTENA_Msk | SCB_SHCSR_BUSFAULTENA_Msk | SCB_SHCSR_MEMFAULTENA_Msk; - /* configure FP state save behaviour - automatic, lazy save */ - FPU->FPCCR |= FPU_FPCCR_ASPEN_Msk | FPU_FPCCR_LSPEN_Msk; + /* configure FP state save behaviour - automatic, lazy save */ + FPU->FPCCR |= FPU_FPCCR_ASPEN_Msk | FPU_FPCCR_LSPEN_Msk; - /* configure default FPU state */ - FPU->FPDSCR |= FPU_FPDSCR_DN_Msk; /* enable Default NaN */ + /* configure default FPU state */ + FPU->FPDSCR |= FPU_FPDSCR_DN_Msk; /* enable Default NaN */ - /* enable the FPU */ - SCB->CPACR |= (0xf << 20); // turn on CP10/11 for FP support on cores that implement it + /* enable the FPU */ + SCB->CPACR |= (0xf << 20); // turn on CP10/11 for FP support on cores that implement it - /* copy initialised data from flash to RAM */ - memcpy(&_sdata, &_sidata, &_edata - &_sdata); + /* copy initialised data from flash to RAM */ + memcpy(&_sdata, &_sidata, &_edata - &_sdata); - /* zero the BSS */ - memset(&_sbss, 0, &_ebss - &_sbss); + /* zero the BSS */ + memset(&_sbss, 0, &_ebss - &_sbss); - /* zero any 'fast' RAM that's been used */ - memset(&_sfast, 0, &_efast - &_sfast); + /* zero any 'fast' RAM that's been used */ + memset(&_sfast, 0, &_efast - &_sfast); - /* fill most of the IRQ/bootstrap stack with a watermark pattern so we can measure how much is used */ - /* leave a little space at the top in case memset() isn't a leaf with no locals */ - memset(&irq_stack, 0xa5, sizeof(irq_stack) - 64); + /* fill most of the IRQ/bootstrap stack with a watermark pattern so we can measure how much is used */ + /* leave a little space at the top in case memset() isn't a leaf with no locals */ + memset(&irq_stack, 0xa5, sizeof(irq_stack) - 64); - /* call main */ - (void)main(); + /* call main */ + (void)main(); } /** * Default handler for CPU exceptions. */ -static void -default_cpu_handler(void) +static void default_cpu_handler(void) { - for (;;) ; + for (;;) { + ; + } } /** Prototype for optional exception vector handlers */ -#define HANDLER(_name) extern vector _name __attribute__((weak, alias("default_cpu_handler"))) +#define HANDLER(_name) extern vector _name __attribute__((weak, alias("default_cpu_handler"))) /* standard CMSIS vector names */ HANDLER(NMI_Handler); @@ -122,25 +122,26 @@ HANDLER(xPortPendSVHandler); HANDLER(xPortSysTickHandler); /** CortexM3 vector table */ -struct cm3_vectors cpu_vectors __attribute((section(".cpu_vectors"))) = { - .initial_stack = &irq_stack[sizeof(irq_stack)], - .entry = (vector *)_main, - .vectors = { - NMI_Handler, - HardFault_Handler, - MemManage_Handler, - BusFault_Handler, - UsageFault_Handler, - 0, - 0, - 0, - 0, - vPortSVCHandler, - DebugMon_Handler, - 0, - xPortPendSVHandler, - xPortSysTickHandler, - } +struct cm3_vectors cpu_vectors __attribute((section(".cpu_vectors"))) = +{ + .initial_stack = &irq_stack[sizeof(irq_stack)], + .entry = (vector *)_main, + .vectors = { + NMI_Handler, + HardFault_Handler, + MemManage_Handler, + BusFault_Handler, + UsageFault_Handler, + 0, + 0, + 0, + 0, + vPortSVCHandler, + DebugMon_Handler, + 0, + xPortPendSVHandler, + xPortSysTickHandler, + } }; /** diff --git a/flight/pios/stm32f4xx/vectors_stm32f4xx.c b/flight/pios/stm32f4xx/vectors_stm32f4xx.c index f1a958010..f1ac30559 100644 --- a/flight/pios/stm32f4xx/vectors_stm32f4xx.c +++ b/flight/pios/stm32f4xx/vectors_stm32f4xx.c @@ -26,185 +26,186 @@ */ /** interrupt handler */ -typedef void (vector)(void); +typedef void (vector)(void); /** default interrupt handler */ -static void -default_io_handler(void) +static void default_io_handler(void) { - for (;;) ; + for (;;) { + ; + } } /** prototypes an interrupt handler */ -#define HANDLER(_name) extern vector _name __attribute__((weak, alias("default_io_handler"))) +#define HANDLER(_name) extern vector _name __attribute__((weak, alias("default_io_handler"))) -HANDLER(WWDG_IRQHandler); // Window WatchDog -HANDLER(PVD_IRQHandler); // PVD through EXTI Line detection -HANDLER(TAMP_STAMP_IRQHandler); // Tamper and TimeStamps through the EXTI line -HANDLER(RTC_WKUP_IRQHandler); // RTC Wakeup through the EXTI line -HANDLER(FLASH_IRQHandler); // FLASH -HANDLER(RCC_IRQHandler); // RCC -HANDLER(EXTI0_IRQHandler); // EXTI Line0 -HANDLER(EXTI1_IRQHandler); // EXTI Line1 -HANDLER(EXTI2_IRQHandler); // EXTI Line2 -HANDLER(EXTI3_IRQHandler); // EXTI Line3 -HANDLER(EXTI4_IRQHandler); // EXTI Line4 -HANDLER(DMA1_Stream0_IRQHandler); // DMA1 Stream 0 -HANDLER(DMA1_Stream1_IRQHandler); // DMA1 Stream 1 -HANDLER(DMA1_Stream2_IRQHandler); // DMA1 Stream 2 -HANDLER(DMA1_Stream3_IRQHandler); // DMA1 Stream 3 -HANDLER(DMA1_Stream4_IRQHandler); // DMA1 Stream 4 -HANDLER(DMA1_Stream5_IRQHandler); // DMA1 Stream 5 -HANDLER(DMA1_Stream6_IRQHandler); // DMA1 Stream 6 -HANDLER(ADC_IRQHandler); // ADC1, ADC2 and ADC3s -HANDLER(CAN1_TX_IRQHandler); // CAN1 TX -HANDLER(CAN1_RX0_IRQHandler); // CAN1 RX0 -HANDLER(CAN1_RX1_IRQHandler); // CAN1 RX1 -HANDLER(CAN1_SCE_IRQHandler); // CAN1 SCE -HANDLER(EXTI9_5_IRQHandler); // External Line[9:5]s -HANDLER(TIM1_BRK_TIM9_IRQHandler); // TIM1 Break and TIM9 -HANDLER(TIM1_UP_TIM10_IRQHandler); // TIM1 Update and TIM10 -HANDLER(TIM1_TRG_COM_TIM11_IRQHandler); // TIM1 Trigger and Commutation and TIM11 -HANDLER(TIM1_CC_IRQHandler); // TIM1 Capture Compare -HANDLER(TIM2_IRQHandler); // TIM2 -HANDLER(TIM3_IRQHandler); // TIM3 -HANDLER(TIM4_IRQHandler); // TIM4 -HANDLER(I2C1_EV_IRQHandler); // I2C1 Event -HANDLER(I2C1_ER_IRQHandler); // I2C1 Error -HANDLER(I2C2_EV_IRQHandler); // I2C2 Event -HANDLER(I2C2_ER_IRQHandler); // I2C2 Error -HANDLER(SPI1_IRQHandler); // SPI1 -HANDLER(SPI2_IRQHandler); // SPI2 -HANDLER(USART1_IRQHandler); // USART1 -HANDLER(USART2_IRQHandler); // USART2 -HANDLER(USART3_IRQHandler); // USART3 -HANDLER(EXTI15_10_IRQHandler); // External Line[15:10]s -HANDLER(RTC_Alarm_IRQHandler); // RTC Alarm (A and B) through EXTI Line -HANDLER(OTG_FS_WKUP_IRQHandler); // USB OTG FS Wakeup through EXTI line -HANDLER(TIM8_BRK_TIM12_IRQHandler); // TIM8 Break and TIM12 -HANDLER(TIM8_UP_TIM13_IRQHandler); // TIM8 Update and TIM13 -HANDLER(TIM8_TRG_COM_TIM14_IRQHandler); // TIM8 Trigger and Commutation and TIM14 -HANDLER(TIM8_CC_IRQHandler); // TIM8 Capture Compare -HANDLER(DMA1_Stream7_IRQHandler); // DMA1 Stream7 -HANDLER(FSMC_IRQHandler); // FSMC -HANDLER(SDIO_IRQHandler); // SDIO -HANDLER(TIM5_IRQHandler); // TIM5 -HANDLER(SPI3_IRQHandler); // SPI3 -HANDLER(USART4_IRQHandler); // UART4 -HANDLER(USART5_IRQHandler); // UART5 -HANDLER(TIM6_DAC_IRQHandler); // TIM6 and DAC1&2 underrun errors -HANDLER(TIM7_IRQHandler); // TIM7 -HANDLER(DMA2_Stream0_IRQHandler); // DMA2 Stream 0 -HANDLER(DMA2_Stream1_IRQHandler); // DMA2 Stream 1 -HANDLER(DMA2_Stream2_IRQHandler); // DMA2 Stream 2 -HANDLER(DMA2_Stream3_IRQHandler); // DMA2 Stream 3 -HANDLER(DMA2_Stream4_IRQHandler); // DMA2 Stream 4 -HANDLER(ETH_IRQHandler); // Ethernet -HANDLER(ETH_WKUP_IRQHandler); // Ethernet Wakeup through EXTI line -HANDLER(CAN2_TX_IRQHandler); // CAN2 TX -HANDLER(CAN2_RX0_IRQHandler); // CAN2 RX0 -HANDLER(CAN2_RX1_IRQHandler); // CAN2 RX1 -HANDLER(CAN2_SCE_IRQHandler); // CAN2 SCE -HANDLER(OTG_FS_IRQHandler); // USB OTG FS -HANDLER(DMA2_Stream5_IRQHandler); // DMA2 Stream 5 -HANDLER(DMA2_Stream6_IRQHandler); // DMA2 Stream 6 -HANDLER(DMA2_Stream7_IRQHandler); // DMA2 Stream 7 -HANDLER(USART6_IRQHandler); // USART6 -HANDLER(I2C3_EV_IRQHandler); // I2C3 event -HANDLER(I2C3_ER_IRQHandler); // I2C3 error -HANDLER(OTG_HS_EP1_OUT_IRQHandler); // USB OTG HS End Point 1 Out -HANDLER(OTG_HS_EP1_IN_IRQHandler); // USB OTG HS End Point 1 In -HANDLER(OTG_HS_WKUP_IRQHandler); // USB OTG HS Wakeup through EXTI -HANDLER(OTG_HS_IRQHandler); // USB OTG HS -HANDLER(DCMI_IRQHandler); // DCMI -HANDLER(CRYP_IRQHandler); // CRYP crypto -HANDLER(HASH_RNG_IRQHandler); // Hash and Rng -HANDLER(FPU_IRQHandler); // FPU +HANDLER(WWDG_IRQHandler); // Window WatchDog +HANDLER(PVD_IRQHandler); // PVD through EXTI Line detection +HANDLER(TAMP_STAMP_IRQHandler); // Tamper and TimeStamps through the EXTI line +HANDLER(RTC_WKUP_IRQHandler); // RTC Wakeup through the EXTI line +HANDLER(FLASH_IRQHandler); // FLASH +HANDLER(RCC_IRQHandler); // RCC +HANDLER(EXTI0_IRQHandler); // EXTI Line0 +HANDLER(EXTI1_IRQHandler); // EXTI Line1 +HANDLER(EXTI2_IRQHandler); // EXTI Line2 +HANDLER(EXTI3_IRQHandler); // EXTI Line3 +HANDLER(EXTI4_IRQHandler); // EXTI Line4 +HANDLER(DMA1_Stream0_IRQHandler); // DMA1 Stream 0 +HANDLER(DMA1_Stream1_IRQHandler); // DMA1 Stream 1 +HANDLER(DMA1_Stream2_IRQHandler); // DMA1 Stream 2 +HANDLER(DMA1_Stream3_IRQHandler); // DMA1 Stream 3 +HANDLER(DMA1_Stream4_IRQHandler); // DMA1 Stream 4 +HANDLER(DMA1_Stream5_IRQHandler); // DMA1 Stream 5 +HANDLER(DMA1_Stream6_IRQHandler); // DMA1 Stream 6 +HANDLER(ADC_IRQHandler); // ADC1, ADC2 and ADC3s +HANDLER(CAN1_TX_IRQHandler); // CAN1 TX +HANDLER(CAN1_RX0_IRQHandler); // CAN1 RX0 +HANDLER(CAN1_RX1_IRQHandler); // CAN1 RX1 +HANDLER(CAN1_SCE_IRQHandler); // CAN1 SCE +HANDLER(EXTI9_5_IRQHandler); // External Line[9:5]s +HANDLER(TIM1_BRK_TIM9_IRQHandler); // TIM1 Break and TIM9 +HANDLER(TIM1_UP_TIM10_IRQHandler); // TIM1 Update and TIM10 +HANDLER(TIM1_TRG_COM_TIM11_IRQHandler); // TIM1 Trigger and Commutation and TIM11 +HANDLER(TIM1_CC_IRQHandler); // TIM1 Capture Compare +HANDLER(TIM2_IRQHandler); // TIM2 +HANDLER(TIM3_IRQHandler); // TIM3 +HANDLER(TIM4_IRQHandler); // TIM4 +HANDLER(I2C1_EV_IRQHandler); // I2C1 Event +HANDLER(I2C1_ER_IRQHandler); // I2C1 Error +HANDLER(I2C2_EV_IRQHandler); // I2C2 Event +HANDLER(I2C2_ER_IRQHandler); // I2C2 Error +HANDLER(SPI1_IRQHandler); // SPI1 +HANDLER(SPI2_IRQHandler); // SPI2 +HANDLER(USART1_IRQHandler); // USART1 +HANDLER(USART2_IRQHandler); // USART2 +HANDLER(USART3_IRQHandler); // USART3 +HANDLER(EXTI15_10_IRQHandler); // External Line[15:10]s +HANDLER(RTC_Alarm_IRQHandler); // RTC Alarm (A and B) through EXTI Line +HANDLER(OTG_FS_WKUP_IRQHandler); // USB OTG FS Wakeup through EXTI line +HANDLER(TIM8_BRK_TIM12_IRQHandler); // TIM8 Break and TIM12 +HANDLER(TIM8_UP_TIM13_IRQHandler); // TIM8 Update and TIM13 +HANDLER(TIM8_TRG_COM_TIM14_IRQHandler); // TIM8 Trigger and Commutation and TIM14 +HANDLER(TIM8_CC_IRQHandler); // TIM8 Capture Compare +HANDLER(DMA1_Stream7_IRQHandler); // DMA1 Stream7 +HANDLER(FSMC_IRQHandler); // FSMC +HANDLER(SDIO_IRQHandler); // SDIO +HANDLER(TIM5_IRQHandler); // TIM5 +HANDLER(SPI3_IRQHandler); // SPI3 +HANDLER(USART4_IRQHandler); // UART4 +HANDLER(USART5_IRQHandler); // UART5 +HANDLER(TIM6_DAC_IRQHandler); // TIM6 and DAC1&2 underrun errors +HANDLER(TIM7_IRQHandler); // TIM7 +HANDLER(DMA2_Stream0_IRQHandler); // DMA2 Stream 0 +HANDLER(DMA2_Stream1_IRQHandler); // DMA2 Stream 1 +HANDLER(DMA2_Stream2_IRQHandler); // DMA2 Stream 2 +HANDLER(DMA2_Stream3_IRQHandler); // DMA2 Stream 3 +HANDLER(DMA2_Stream4_IRQHandler); // DMA2 Stream 4 +HANDLER(ETH_IRQHandler); // Ethernet +HANDLER(ETH_WKUP_IRQHandler); // Ethernet Wakeup through EXTI line +HANDLER(CAN2_TX_IRQHandler); // CAN2 TX +HANDLER(CAN2_RX0_IRQHandler); // CAN2 RX0 +HANDLER(CAN2_RX1_IRQHandler); // CAN2 RX1 +HANDLER(CAN2_SCE_IRQHandler); // CAN2 SCE +HANDLER(OTG_FS_IRQHandler); // USB OTG FS +HANDLER(DMA2_Stream5_IRQHandler); // DMA2 Stream 5 +HANDLER(DMA2_Stream6_IRQHandler); // DMA2 Stream 6 +HANDLER(DMA2_Stream7_IRQHandler); // DMA2 Stream 7 +HANDLER(USART6_IRQHandler); // USART6 +HANDLER(I2C3_EV_IRQHandler); // I2C3 event +HANDLER(I2C3_ER_IRQHandler); // I2C3 error +HANDLER(OTG_HS_EP1_OUT_IRQHandler); // USB OTG HS End Point 1 Out +HANDLER(OTG_HS_EP1_IN_IRQHandler); // USB OTG HS End Point 1 In +HANDLER(OTG_HS_WKUP_IRQHandler); // USB OTG HS Wakeup through EXTI +HANDLER(OTG_HS_IRQHandler); // USB OTG HS +HANDLER(DCMI_IRQHandler); // DCMI +HANDLER(CRYP_IRQHandler); // CRYP crypto +HANDLER(HASH_RNG_IRQHandler); // Hash and Rng +HANDLER(FPU_IRQHandler); // FPU /** stm32f4xx interrupt vector table */ vector *io_vectors[] __attribute__((section(".io_vectors"))) = { - WWDG_IRQHandler, // Window WatchDog - PVD_IRQHandler, // PVD through EXTI Line detection - TAMP_STAMP_IRQHandler, // Tamper and TimeStamps through the EXTI line - RTC_WKUP_IRQHandler, // RTC Wakeup through the EXTI line - FLASH_IRQHandler, // FLASH - RCC_IRQHandler, // RCC - EXTI0_IRQHandler, // EXTI Line0 - EXTI1_IRQHandler, // EXTI Line1 - EXTI2_IRQHandler, // EXTI Line2 - EXTI3_IRQHandler, // EXTI Line3 - EXTI4_IRQHandler, // EXTI Line4 - DMA1_Stream0_IRQHandler, // DMA1 Stream 0 - DMA1_Stream1_IRQHandler, // DMA1 Stream 1 - DMA1_Stream2_IRQHandler, // DMA1 Stream 2 - DMA1_Stream3_IRQHandler, // DMA1 Stream 3 - DMA1_Stream4_IRQHandler, // DMA1 Stream 4 - DMA1_Stream5_IRQHandler, // DMA1 Stream 5 - DMA1_Stream6_IRQHandler, // DMA1 Stream 6 - ADC_IRQHandler, // ADC1, ADC2 and ADC3s - CAN1_TX_IRQHandler, // CAN1 TX - CAN1_RX0_IRQHandler, // CAN1 RX0 - CAN1_RX1_IRQHandler, // CAN1 RX1 - CAN1_SCE_IRQHandler, // CAN1 SCE - EXTI9_5_IRQHandler, // External Line[9:5]s - TIM1_BRK_TIM9_IRQHandler, // TIM1 Break and TIM9 - TIM1_UP_TIM10_IRQHandler, // TIM1 Update and TIM10 - TIM1_TRG_COM_TIM11_IRQHandler, // TIM1 Trigger and Commutation and TIM11 - TIM1_CC_IRQHandler, // TIM1 Capture Compare - TIM2_IRQHandler, // TIM2 - TIM3_IRQHandler, // TIM3 - TIM4_IRQHandler, // TIM4 - I2C1_EV_IRQHandler, // I2C1 Event - I2C1_ER_IRQHandler, // I2C1 Error - I2C2_EV_IRQHandler, // I2C2 Event - I2C2_ER_IRQHandler, // I2C2 Error - SPI1_IRQHandler, // SPI1 - SPI2_IRQHandler, // SPI2 - USART1_IRQHandler, // USART1 - USART2_IRQHandler, // USART2 - USART3_IRQHandler, // USART3 - EXTI15_10_IRQHandler, // External Line[15:10]s - RTC_Alarm_IRQHandler, // RTC Alarm (A and B) through EXTI Line - OTG_FS_WKUP_IRQHandler, // USB OTG FS Wakeup through EXTI line - TIM8_BRK_TIM12_IRQHandler, // TIM8 Break and TIM12 - TIM8_UP_TIM13_IRQHandler, // TIM8 Update and TIM13 - TIM8_TRG_COM_TIM14_IRQHandler, // TIM8 Trigger and Commutation and TIM14 - TIM8_CC_IRQHandler, // TIM8 Capture Compare - DMA1_Stream7_IRQHandler, // DMA1 Stream7 - FSMC_IRQHandler, // FSMC - SDIO_IRQHandler, // SDIO - TIM5_IRQHandler, // TIM5 - SPI3_IRQHandler, // SPI3 - USART4_IRQHandler, // UART4 - USART5_IRQHandler, // UART5 - TIM6_DAC_IRQHandler, // TIM6 and DAC1&2 underrun errors - TIM7_IRQHandler, // TIM7 - DMA2_Stream0_IRQHandler, // DMA2 Stream 0 - DMA2_Stream1_IRQHandler, // DMA2 Stream 1 - DMA2_Stream2_IRQHandler, // DMA2 Stream 2 - DMA2_Stream3_IRQHandler, // DMA2 Stream 3 - DMA2_Stream4_IRQHandler, // DMA2 Stream 4 - ETH_IRQHandler, // Ethernet - ETH_WKUP_IRQHandler, // Ethernet Wakeup through EXTI line - CAN2_TX_IRQHandler, // CAN2 TX - CAN2_RX0_IRQHandler, // CAN2 RX0 - CAN2_RX1_IRQHandler, // CAN2 RX1 - CAN2_SCE_IRQHandler, // CAN2 SCE - OTG_FS_IRQHandler, // USB OTG FS - DMA2_Stream5_IRQHandler, // DMA2 Stream 5 - DMA2_Stream6_IRQHandler, // DMA2 Stream 6 - DMA2_Stream7_IRQHandler, // DMA2 Stream 7 - USART6_IRQHandler, // USART6 - I2C3_EV_IRQHandler, // I2C3 event - I2C3_ER_IRQHandler, // I2C3 error - OTG_HS_EP1_OUT_IRQHandler, // USB OTG HS End Point 1 Out - OTG_HS_EP1_IN_IRQHandler, // USB OTG HS End Point 1 In - OTG_HS_WKUP_IRQHandler, // USB OTG HS Wakeup through EXTI - OTG_HS_IRQHandler, // USB OTG HS - DCMI_IRQHandler, // DCMI - CRYP_IRQHandler, // CRYP crypto - HASH_RNG_IRQHandler, // Hash and Rng - FPU_IRQHandler, // FPU + WWDG_IRQHandler, // Window WatchDog + PVD_IRQHandler, // PVD through EXTI Line detection + TAMP_STAMP_IRQHandler, // Tamper and TimeStamps through the EXTI line + RTC_WKUP_IRQHandler, // RTC Wakeup through the EXTI line + FLASH_IRQHandler, // FLASH + RCC_IRQHandler, // RCC + EXTI0_IRQHandler, // EXTI Line0 + EXTI1_IRQHandler, // EXTI Line1 + EXTI2_IRQHandler, // EXTI Line2 + EXTI3_IRQHandler, // EXTI Line3 + EXTI4_IRQHandler, // EXTI Line4 + DMA1_Stream0_IRQHandler, // DMA1 Stream 0 + DMA1_Stream1_IRQHandler, // DMA1 Stream 1 + DMA1_Stream2_IRQHandler, // DMA1 Stream 2 + DMA1_Stream3_IRQHandler, // DMA1 Stream 3 + DMA1_Stream4_IRQHandler, // DMA1 Stream 4 + DMA1_Stream5_IRQHandler, // DMA1 Stream 5 + DMA1_Stream6_IRQHandler, // DMA1 Stream 6 + ADC_IRQHandler, // ADC1, ADC2 and ADC3s + CAN1_TX_IRQHandler, // CAN1 TX + CAN1_RX0_IRQHandler, // CAN1 RX0 + CAN1_RX1_IRQHandler, // CAN1 RX1 + CAN1_SCE_IRQHandler, // CAN1 SCE + EXTI9_5_IRQHandler, // External Line[9:5]s + TIM1_BRK_TIM9_IRQHandler, // TIM1 Break and TIM9 + TIM1_UP_TIM10_IRQHandler, // TIM1 Update and TIM10 + TIM1_TRG_COM_TIM11_IRQHandler, // TIM1 Trigger and Commutation and TIM11 + TIM1_CC_IRQHandler, // TIM1 Capture Compare + TIM2_IRQHandler, // TIM2 + TIM3_IRQHandler, // TIM3 + TIM4_IRQHandler, // TIM4 + I2C1_EV_IRQHandler, // I2C1 Event + I2C1_ER_IRQHandler, // I2C1 Error + I2C2_EV_IRQHandler, // I2C2 Event + I2C2_ER_IRQHandler, // I2C2 Error + SPI1_IRQHandler, // SPI1 + SPI2_IRQHandler, // SPI2 + USART1_IRQHandler, // USART1 + USART2_IRQHandler, // USART2 + USART3_IRQHandler, // USART3 + EXTI15_10_IRQHandler, // External Line[15:10]s + RTC_Alarm_IRQHandler, // RTC Alarm (A and B) through EXTI Line + OTG_FS_WKUP_IRQHandler, // USB OTG FS Wakeup through EXTI line + TIM8_BRK_TIM12_IRQHandler, // TIM8 Break and TIM12 + TIM8_UP_TIM13_IRQHandler, // TIM8 Update and TIM13 + TIM8_TRG_COM_TIM14_IRQHandler, // TIM8 Trigger and Commutation and TIM14 + TIM8_CC_IRQHandler, // TIM8 Capture Compare + DMA1_Stream7_IRQHandler, // DMA1 Stream7 + FSMC_IRQHandler, // FSMC + SDIO_IRQHandler, // SDIO + TIM5_IRQHandler, // TIM5 + SPI3_IRQHandler, // SPI3 + USART4_IRQHandler, // UART4 + USART5_IRQHandler, // UART5 + TIM6_DAC_IRQHandler, // TIM6 and DAC1&2 underrun errors + TIM7_IRQHandler, // TIM7 + DMA2_Stream0_IRQHandler, // DMA2 Stream 0 + DMA2_Stream1_IRQHandler, // DMA2 Stream 1 + DMA2_Stream2_IRQHandler, // DMA2 Stream 2 + DMA2_Stream3_IRQHandler, // DMA2 Stream 3 + DMA2_Stream4_IRQHandler, // DMA2 Stream 4 + ETH_IRQHandler, // Ethernet + ETH_WKUP_IRQHandler, // Ethernet Wakeup through EXTI line + CAN2_TX_IRQHandler, // CAN2 TX + CAN2_RX0_IRQHandler, // CAN2 RX0 + CAN2_RX1_IRQHandler, // CAN2 RX1 + CAN2_SCE_IRQHandler, // CAN2 SCE + OTG_FS_IRQHandler, // USB OTG FS + DMA2_Stream5_IRQHandler, // DMA2 Stream 5 + DMA2_Stream6_IRQHandler, // DMA2 Stream 6 + DMA2_Stream7_IRQHandler, // DMA2 Stream 7 + USART6_IRQHandler, // USART6 + I2C3_EV_IRQHandler, // I2C3 event + I2C3_ER_IRQHandler, // I2C3 error + OTG_HS_EP1_OUT_IRQHandler, // USB OTG HS End Point 1 Out + OTG_HS_EP1_IN_IRQHandler, // USB OTG HS End Point 1 In + OTG_HS_WKUP_IRQHandler, // USB OTG HS Wakeup through EXTI + OTG_HS_IRQHandler, // USB OTG HS + DCMI_IRQHandler, // DCMI + CRYP_IRQHandler, // CRYP crypto + HASH_RNG_IRQHandler, // Hash and Rng + FPU_IRQHandler, // FPU }; /** diff --git a/flight/pios/win32/inc/FreeRTOSConfig.h b/flight/pios/win32/inc/FreeRTOSConfig.h index 063a843a1..87f9020c7 100644 --- a/flight/pios/win32/inc/FreeRTOSConfig.h +++ b/flight/pios/win32/inc/FreeRTOSConfig.h @@ -1,76 +1,72 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ -/* Notes: We use 5 task priorities */ +/* Notes: We use 5 task priorities */ #define IDLE_SLEEPS -#define configUSE_PREEMPTION 1 -#define configIDLE_SHOULD_YIELD 1 +#define configUSE_PREEMPTION 1 +#define configIDLE_SHOULD_YIELD 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 ) -#define configTICK_RATE_HZ ( ( portTickType ) 100 ) -#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) -#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 256 ) -#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 45 * 1024 ) ) -#define configMAX_TASK_NAME_LEN ( 16 ) -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)100) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)256) +#define configTOTAL_HEAP_SIZE ((size_t)(45 * 1024)) +#define configMAX_TASK_NAME_LEN (16) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ - -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 0 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 0 - + to exclude the API function. */ +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 0 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 #endif /* FREERTOS_CONFIG_H */ - diff --git a/flight/pios/win32/inc/pios_com.h b/flight/pios/win32/inc/pios_com.h index b18cb3d52..dd91a4d81 100644 --- a/flight/pios/win32/inc/pios_com.h +++ b/flight/pios/win32/inc/pios_com.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_com.h + * @file pios_com.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM layer functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -45,12 +45,12 @@ extern int32_t PIOS_COM_ReceiveBufferUsed(uint8_t port); extern int32_t PIOS_COM_ReceiveHandler(void); struct pios_com_driver { - void (*init)(uint8_t id); - void (*set_baud)(uint8_t id, uint32_t baud); - int32_t (*tx_nb)(uint8_t id, char *buffer, uint16_t len); - int32_t (*tx)(uint8_t id, char *buffer, uint16_t len); - int32_t (*rx)(uint8_t id); - int32_t (*rx_avail)(uint8_t id); + void (*init)(uint8_t id); + void (*set_baud)(uint8_t id, uint32_t baud); + int32_t (*tx_nb)(uint8_t id, char *buffer, uint16_t len); + int32_t (*tx)(uint8_t id, char *buffer, uint16_t len); + int32_t (*rx)(uint8_t id); + int32_t (*rx_avail)(uint8_t id); }; #endif /* PIOS_COM_H */ diff --git a/flight/pios/win32/inc/pios_com_priv.h b/flight/pios/win32/inc/pios_com_priv.h index 5b285edac..1acd8b1b1 100644 --- a/flight/pios/win32/inc/pios_com_priv.h +++ b/flight/pios/win32/inc/pios_com_priv.h @@ -3,7 +3,7 @@ * * @file pios_com_priv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM private definitions. * @see The GNU Public License (GPL) Version 3 * @@ -30,12 +30,11 @@ #include struct pios_com_dev { - uint8_t id; - const struct pios_com_driver * const driver; + uint8_t id; + const struct pios_com_driver *const driver; }; extern struct pios_com_dev pios_com_devs[]; -extern const uint8_t pios_com_num_devices; +extern const uint8_t pios_com_num_devices; #endif /* PIOS_COM_PRIV_H */ - diff --git a/flight/pios/win32/inc/pios_crc.h b/flight/pios/win32/inc/pios_crc.h index 3a64f8bab..5a737ade1 100644 --- a/flight/pios/win32/inc/pios_crc.h +++ b/flight/pios/win32/inc/pios_crc.h @@ -5,27 +5,27 @@ * @addtogroup PIOS_CRC CRC Functions * @{ * - * @file pios_crc.h + * @file pios_crc.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief CRC functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data); -uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length); +uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t *data, int32_t length); diff --git a/flight/pios/win32/inc/pios_delay.h b/flight/pios/win32/inc/pios_delay.h index 8c49603c4..fdb7d4644 100644 --- a/flight/pios/win32/inc/pios_delay.h +++ b/flight/pios/win32/inc/pios_delay.h @@ -3,24 +3,24 @@ * * @file pios_settings.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) - * @brief Settings functions header + * Parts by Thorsten Klose (tk@midibox.org) + * @brief Settings functions header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/win32/inc/pios_initcall.h b/flight/pios/win32/inc/pios_initcall.h index 3805a88c6..e60b8755d 100644 --- a/flight/pios/win32/inc/pios_initcall.h +++ b/flight/pios/win32/inc/pios_initcall.h @@ -6,25 +6,25 @@ * @brief Initcall Macros * @{ * - * @file pios_initcall.h + * @file pios_initcall.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. * @brief Initcall header * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,15 +42,16 @@ #define MODULE_TASKCREATE_ALL -#define MODULE_INITIALISE_ALL { \ - /* Initialize modules */ \ - InitModules(); \ - /* Start the FreeRTOS scheduler which never returns.*/ \ - /* Initialize the system thread */ \ - SystemModInitialize();} +#define MODULE_INITIALISE_ALL \ + { \ + /* Initialize modules */ \ + InitModules(); \ + /* Start the FreeRTOS scheduler which never returns.*/ \ + /* Initialize the system thread */ \ + SystemModInitialize(); } -#endif /* PIOS_INITCALL_H */ +#endif /* PIOS_INITCALL_H */ /** * @} diff --git a/flight/pios/win32/inc/pios_led.h b/flight/pios/win32/inc/pios_led.h index 94283e4b6..bbf728622 100644 --- a/flight/pios/win32/inc/pios_led.h +++ b/flight/pios/win32/inc/pios_led.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file pios_led.h + * @file pios_led.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief LED functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -29,9 +29,9 @@ /* Type Definitions */ #if (PIOS_LED_NUM == 1) -typedef enum {LED1 = 0} LedTypeDef; +typedef enum { LED1 = 0 } LedTypeDef; #elif (PIOS_LED_NUM == 2) -typedef enum {LED1 = 0, LED2 = 1} LedTypeDef; +typedef enum { LED1 = 0, LED2 = 1 } LedTypeDef; #endif /* Public Functions */ diff --git a/flight/pios/win32/inc/pios_posix.h b/flight/pios/win32/inc/pios_posix.h index 05b89533e..68300cfda 100644 --- a/flight/pios/win32/inc/pios_posix.h +++ b/flight/pios/win32/inc/pios_posix.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file posix.h + * @file posix.h * @author Corvus Corax Copyright (C) 2010. * @brief Definitions to run PiOS on posix * @see The GNU Public License (GPL) Version 2 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -28,10 +28,9 @@ #include -#define FILEINFO FILE* +#define FILEINFO FILE * #define PIOS_SERVO_NUM_OUTPUTS 8 -#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS +#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS #endif - diff --git a/flight/pios/win32/inc/pios_sdcard.h b/flight/pios/win32/inc/pios_sdcard.h index 2927def2d..72bd9d042 100644 --- a/flight/pios/win32/inc/pios_sdcard.h +++ b/flight/pios/win32/inc/pios_sdcard.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_sdcard.h + * @file pios_sdcard.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief System and hardware Init functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,61 +31,61 @@ /* Public Functions */ typedef struct { - uint8_t CSDStruct; /* CSD structure */ - uint8_t SysSpecVersion; /* System specification version */ - uint8_t Reserved1; /* Reserved */ - uint8_t TAAC; /* Data read access-time 1 */ - uint8_t NSAC; /* Data read access-time 2 in CLK cycles */ - uint8_t MaxBusClkFrec; /* Max. bus clock frequency */ - uint16_t CardComdClasses; /* Card command classes */ - uint8_t RdBlockLen; /* Max. read data block length */ - uint8_t PartBlockRead; /* Partial blocks for read allowed */ - uint8_t WrBlockMisalign; /* Write block misalignment */ - uint8_t RdBlockMisalign; /* Read block misalignment */ - uint8_t DSRImpl; /* DSR implemented */ - uint8_t Reserved2; /* Reserved */ - uint16_t DeviceSize; /* Device Size */ - uint8_t MaxRdCurrentVDDMin; /* Max. read current @ VDD min */ - uint8_t MaxRdCurrentVDDMax; /* Max. read current @ VDD max */ - uint8_t MaxWrCurrentVDDMin; /* Max. write current @ VDD min */ - uint8_t MaxWrCurrentVDDMax; /* Max. write current @ VDD max */ - uint8_t DeviceSizeMul; /* Device size multiplier */ - uint8_t EraseGrSize; /* Erase group size */ - uint8_t EraseGrMul; /* Erase group size multiplier */ - uint8_t WrProtectGrSize; /* Write protect group size */ - uint8_t WrProtectGrEnable; /* Write protect group enable */ - uint8_t ManDeflECC; /* Manufacturer default ECC */ - uint8_t WrSpeedFact; /* Write speed factor */ - uint8_t MaxWrBlockLen; /* Max. write data block length */ - uint8_t WriteBlockPaPartial; /* Partial blocks for write allowed */ - uint8_t Reserved3; /* Reserved */ - uint8_t ContentProtectAppli; /* Content protection application */ - uint8_t FileFormatGrouop; /* File format group */ - uint8_t CopyFlag; /* Copy flag (OTP) */ - uint8_t PermWrProtect; /* Permanent write protection */ - uint8_t TempWrProtect; /* Temporary write protection */ - uint8_t FileFormat; /* File Format */ - uint8_t ECC; /* ECC code */ - uint8_t msd_CRC; /* CRC */ - uint8_t Reserved4; /* always 1*/ + uint8_t CSDStruct; /* CSD structure */ + uint8_t SysSpecVersion; /* System specification version */ + uint8_t Reserved1; /* Reserved */ + uint8_t TAAC; /* Data read access-time 1 */ + uint8_t NSAC; /* Data read access-time 2 in CLK cycles */ + uint8_t MaxBusClkFrec; /* Max. bus clock frequency */ + uint16_t CardComdClasses; /* Card command classes */ + uint8_t RdBlockLen; /* Max. read data block length */ + uint8_t PartBlockRead; /* Partial blocks for read allowed */ + uint8_t WrBlockMisalign; /* Write block misalignment */ + uint8_t RdBlockMisalign; /* Read block misalignment */ + uint8_t DSRImpl; /* DSR implemented */ + uint8_t Reserved2; /* Reserved */ + uint16_t DeviceSize; /* Device Size */ + uint8_t MaxRdCurrentVDDMin; /* Max. read current @ VDD min */ + uint8_t MaxRdCurrentVDDMax; /* Max. read current @ VDD max */ + uint8_t MaxWrCurrentVDDMin; /* Max. write current @ VDD min */ + uint8_t MaxWrCurrentVDDMax; /* Max. write current @ VDD max */ + uint8_t DeviceSizeMul; /* Device size multiplier */ + uint8_t EraseGrSize; /* Erase group size */ + uint8_t EraseGrMul; /* Erase group size multiplier */ + uint8_t WrProtectGrSize; /* Write protect group size */ + uint8_t WrProtectGrEnable; /* Write protect group enable */ + uint8_t ManDeflECC; /* Manufacturer default ECC */ + uint8_t WrSpeedFact; /* Write speed factor */ + uint8_t MaxWrBlockLen; /* Max. write data block length */ + uint8_t WriteBlockPaPartial; /* Partial blocks for write allowed */ + uint8_t Reserved3; /* Reserved */ + uint8_t ContentProtectAppli; /* Content protection application */ + uint8_t FileFormatGrouop; /* File format group */ + uint8_t CopyFlag; /* Copy flag (OTP) */ + uint8_t PermWrProtect; /* Permanent write protection */ + uint8_t TempWrProtect; /* Temporary write protection */ + uint8_t FileFormat; /* File Format */ + uint8_t ECC; /* ECC code */ + uint8_t msd_CRC; /* CRC */ + uint8_t Reserved4; /* always 1*/ } SDCARDCsdTypeDef; /* Structure taken from Mass Storage Driver example provided by STM */ typedef struct { - uint8_t ManufacturerID; /* ManufacturerID */ - uint16_t OEM_AppliID; /* OEM/Application ID */ - char ProdName[6]; /* Product Name */ - uint8_t ProdRev; /* Product Revision */ - uint32_t ProdSN; /* Product Serial Number */ - uint8_t Reserved1; /* Reserved1 */ - uint16_t ManufactDate; /* Manufacturing Date */ - uint8_t msd_CRC; /* CRC */ - uint8_t Reserved2; /* always 1*/ + uint8_t ManufacturerID; /* ManufacturerID */ + uint16_t OEM_AppliID; /* OEM/Application ID */ + char ProdName[6]; /* Product Name */ + uint8_t ProdRev; /* Product Revision */ + uint32_t ProdSN; /* Product Serial Number */ + uint8_t Reserved1; /* Reserved1 */ + uint16_t ManufactDate; /* Manufacturing Date */ + uint8_t msd_CRC; /* CRC */ + uint8_t Reserved2; /* always 1*/ } SDCARDCidTypeDef; /* Global Variables */ -//extern VOLINFO PIOS_SDCARD_VolInfo; -//extern uint8_t PIOS_SDCARD_Sector[SECTOR_SIZE]; +// extern VOLINFO PIOS_SDCARD_VolInfo; +// extern uint8_t PIOS_SDCARD_Sector[SECTOR_SIZE]; /* Prototypes */ extern int32_t PIOS_SDCARD_Init(void); @@ -103,11 +103,11 @@ extern int32_t PIOS_SDCARD_IsMounted(); extern int32_t PIOS_SDCARD_MountFS(uint32_t StartupLog); extern int32_t PIOS_SDCARD_GetFree(void); -//extern int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len); -//extern int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len); +// extern int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len); +// extern int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len); extern int32_t PIOS_SDCARD_FileCopy(char *Source, char *Destination); extern int32_t PIOS_SDCARD_FileDelete(char *Filename); -#endif +#endif // if defined(PIOS_INCLUDE_SDCARD) #endif /* PIOS_SDCARD_H */ diff --git a/flight/pios/win32/inc/pios_servo.h b/flight/pios/win32/inc/pios_servo.h index 7cbf37a7d..31a5d6a72 100644 --- a/flight/pios/win32/inc/pios_servo.h +++ b/flight/pios/win32/inc/pios_servo.h @@ -5,25 +5,25 @@ * @addtogroup PIOS_SERVO RC Servo Functions * @{ * - * @file pios_servo.h + * @file pios_servo.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RC Servo functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,12 +32,12 @@ /* Public Functions */ extern void PIOS_Servo_Init(void); -extern void PIOS_Servo_SetHz(uint16_t * speeds, uint8_t num_banks); +extern void PIOS_Servo_SetHz(uint16_t *speeds, uint8_t num_banks); extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position); #endif /* PIOS_SERVO_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/win32/inc/pios_sys.h b/flight/pios/win32/inc/pios_sys.h index b99232649..c9e17535f 100644 --- a/flight/pios/win32/inc/pios_sys.h +++ b/flight/pios/win32/inc/pios_sys.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_sys.h + * @file pios_sys.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief System and hardware Init functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/win32/inc/pios_udp.h b/flight/pios/win32/inc/pios_udp.h index f35fe903f..f9f27b88a 100644 --- a/flight/pios/win32/inc/pios_udp.h +++ b/flight/pios/win32/inc/pios_udp.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_usart.h + * @file pios_usart.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief UDP functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,7 +31,7 @@ /* Global Types */ /* Public Functions */ -//extern void PIOS_UDP_Init(void); +// extern void PIOS_UDP_Init(void); void PIOS_UDP_Init(void); void PIOS_UDP_Close(void); extern void PIOS_UDP_ChangeBaud(uint8_t usart, uint32_t baud); diff --git a/flight/pios/win32/inc/pios_udp_priv.h b/flight/pios/win32/inc/pios_udp_priv.h index e7ba88ee9..c60cb628f 100644 --- a/flight/pios/win32/inc/pios_udp_priv.h +++ b/flight/pios/win32/inc/pios_udp_priv.h @@ -3,7 +3,7 @@ * * @file pios_udp_priv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief UDP private definitions. * @see The GNU Public License (GPL) Version 3 * @@ -36,29 +36,28 @@ #include - struct pios_udp_cfg { - const char * ip; - uint16_t port; + const char *ip; + uint16_t port; }; struct pios_udp_buffer { - uint8_t buf[PIOS_UDP_RX_BUFFER_SIZE]; - uint16_t head; - uint16_t tail; - uint16_t size; + uint8_t buf[PIOS_UDP_RX_BUFFER_SIZE]; + uint16_t head; + uint16_t tail; + uint16_t size; }; struct pios_udp_dev { - const struct pios_udp_cfg * const cfg; - struct pios_udp_buffer rx; - SOCKET socket; - struct sockaddr_in server; - struct sockaddr_in client; - uint32_t clientLength; + const struct pios_udp_cfg *const cfg; + struct pios_udp_buffer rx; + SOCKET socket; + struct sockaddr_in server; + struct sockaddr_in client; + uint32_t clientLength; }; extern struct pios_udp_dev pios_udp_devs[]; -extern uint8_t pios_udp_num_devices; +extern uint8_t pios_udp_num_devices; #endif /* PIOS_UDP_PRIV_H */ diff --git a/flight/pios/win32/inc/pios_wdg.h b/flight/pios/win32/inc/pios_wdg.h index 26bab0c8b..7fde01f1e 100644 --- a/flight/pios/win32/inc/pios_wdg.h +++ b/flight/pios/win32/inc/pios_wdg.h @@ -7,7 +7,7 @@ * * @file pios_wdg.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief SPI functions header. * @see The GNU Public License (GPL) Version 3 * @@ -33,15 +33,15 @@ #include -//------------------------ +// ------------------------ // WATCHDOG_SETTINGS -//------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -#define PIOS_WDG_REGISTER BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_AHRS 0x0004 -#define PIOS_WDG_MANUAL 0x0008 +// ------------------------ +#define PIOS_WATCHDOG_TIMEOUT 250 +#define PIOS_WDG_REGISTER BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_AHRS 0x0004 +#define PIOS_WDG_MANUAL 0x0008 void PIOS_WDG_Init(); bool PIOS_WDG_RegisterFlag(uint16_t flag_requested); @@ -50,4 +50,4 @@ uint16_t PIOS_WDG_GetBootupFlags(); uint16_t PIOS_WDG_GetActiveFlags(); void PIOS_WDG_Clear(void); -#endif +#endif // ifndef PIOS_WDG diff --git a/flight/pios/win32/pios.h b/flight/pios/win32/pios.h index 590c6a4df..d8e116344 100644 --- a/flight/pios/win32/pios.h +++ b/flight/pios/win32/pios.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios.h + * @file pios.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Main PiOS header. + * @brief Main PiOS header. * - Central header for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/pios/win32/win32/pios_com.c b/flight/pios/win32/win32/pios_com.c index 749a0bc38..365f056f0 100644 --- a/flight/pios/win32/win32/pios_com.c +++ b/flight/pios/win32/win32/pios_com.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_com.c + * @file pios_com.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief COM layer functions * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_COM COM layer functions * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,263 +34,263 @@ #include -static struct pios_com_dev * find_com_dev_by_id (uint8_t port) +static struct pios_com_dev *find_com_dev_by_id(uint8_t port) { - if (port >= pios_com_num_devices) { - /* Undefined COM port for this board (see pios_board.c) */ - return NULL; - } + if (port >= pios_com_num_devices) { + /* Undefined COM port for this board (see pios_board.c) */ + return NULL; + } - /* Get a handle for the device configuration */ - return &(pios_com_devs[port]); + /* Get a handle for the device configuration */ + return &(pios_com_devs[port]); } /** -* Initialises COM layer -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ + * Initialises COM layer + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ int32_t PIOS_COM_Init(void) { - int32_t ret = 0; + int32_t ret = 0; - /* If any COM assignment: */ + /* If any COM assignment: */ #if defined(PIOS_INCLUDE_SERIAL) - PIOS_SERIAL_Init(); + PIOS_SERIAL_Init(); #endif #if defined(PIOS_INCLUDE_UDP) - PIOS_UDP_Init(); + PIOS_UDP_Init(); #endif - return ret; + return ret; } int32_t PIOS_COM_Close(void) { - int32_t ret = 0; + int32_t ret = 0; #if defined(PIOS_INCLUDE_UDP) - PIOS_UDP_Close(); + PIOS_UDP_Close(); #endif - return ret; + return ret; } /** -* Change the port speed without re-initializing -* \param[in] port COM port -* \param[in] baud Requested baud rate -* \return -1 if port not available -* \return 0 on success -*/ + * Change the port speed without re-initializing + * \param[in] port COM port + * \param[in] baud Requested baud rate + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_ChangeBaud(uint8_t port, uint32_t baud) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - com_dev = find_com_dev_by_id (port); + com_dev = find_com_dev_by_id(port); - if (!com_dev) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!com_dev) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - /* Invoke the driver function if it exists */ - if (com_dev->driver->set_baud) { - com_dev->driver->set_baud(com_dev->id, baud); - } + /* Invoke the driver function if it exists */ + if (com_dev->driver->set_baud) { + com_dev->driver->set_baud(com_dev->id, baud); + } - return 0; + return 0; } /** -* Sends a package over given port -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a package over given port + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendBufferNonBlocking(uint8_t port, uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - com_dev = find_com_dev_by_id (port); + com_dev = find_com_dev_by_id(port); - if (!com_dev) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!com_dev) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - /* Invoke the driver function if it exists */ - if (com_dev->driver->tx_nb) { - return com_dev->driver->tx_nb(com_dev->id, (char *)buffer, len); - } + /* Invoke the driver function if it exists */ + if (com_dev->driver->tx_nb) { + return com_dev->driver->tx_nb(com_dev->id, (char *)buffer, len); + } - return 0; + return 0; } /** -* Sends a package over given port -* (blocking function) -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a package over given port + * (blocking function) + * \param[in] port COM port + * \param[in] buffer character buffer + * \param[in] len buffer length + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendBuffer(uint8_t port, uint8_t *buffer, uint16_t len) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - com_dev = find_com_dev_by_id (port); + com_dev = find_com_dev_by_id(port); - if (!com_dev) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + if (!com_dev) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } - /* Invoke the driver function if it exists */ - if (com_dev->driver->tx) { - return com_dev->driver->tx(com_dev->id, (char *)buffer, len); - } + /* Invoke the driver function if it exists */ + if (com_dev->driver->tx) { + return com_dev->driver->tx(com_dev->id, (char *)buffer, len); + } - return 0; + return 0; } /** -* Sends a single character over given port -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a single character over given port + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendCharNonBlocking(uint8_t port, char c) { - return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)&c, 1); + return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)&c, 1); } /** -* Sends a single character over given port -* (blocking function) -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a single character over given port + * (blocking function) + * \param[in] port COM port + * \param[in] c character + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendChar(uint8_t port, char c) { - return PIOS_COM_SendBuffer(port, (uint8_t *)&c, 1); + return PIOS_COM_SendBuffer(port, (uint8_t *)&c, 1); } /** -* Sends a string over given port -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a string over given port + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return -2 buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendStringNonBlocking(uint8_t port, char *str) { - return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)str, (uint16_t)strlen(str)); + return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)str, (uint16_t)strlen(str)); } /** -* Sends a string over given port -* (blocking function) -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a string over given port + * (blocking function) + * \param[in] port COM port + * \param[in] str zero-terminated string + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendString(uint8_t port, char *str) { - return PIOS_COM_SendBuffer(port, (uint8_t *)str, strlen(str)); + return PIOS_COM_SendBuffer(port, (uint8_t *)str, strlen(str)); } /** -* Sends a formatted string (-> printf) over given port -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* 128 characters supported maximum! -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * 128 characters supported maximum! + * \return -2 if non-blocking mode activated: buffer is full + * caller should retry until buffer is free again + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedStringNonBlocking(uint8_t port, char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBufferNonBlocking(port, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBufferNonBlocking(port, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Sends a formatted string (-> printf) over given port -* (blocking function) -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* \return -1 if port not available -* \return 0 on success -*/ + * Sends a formatted string (-> printf) over given port + * (blocking function) + * \param[in] port COM port + * \param[in] *format zero-terminated format string - 128 characters supported maximum! + * \param[in] ... optional arguments, + * \return -1 if port not available + * \return 0 on success + */ int32_t PIOS_COM_SendFormattedString(uint8_t port, char *format, ...) { - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + va_list args; - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBuffer(port, buffer, (uint16_t)strlen((char *)buffer)); + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBuffer(port, buffer, (uint16_t)strlen((char *)buffer)); } /** -* Transfer bytes from port buffers into another buffer -* \param[in] port COM port -* \returns Byte from buffer -*/ + * Transfer bytes from port buffers into another buffer + * \param[in] port COM port + * \returns Byte from buffer + */ uint8_t PIOS_COM_ReceiveBuffer(uint8_t port) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - com_dev = find_com_dev_by_id (port); - //PIOS_DEBUG_Assert(com_dev); - //PIOS_DEBUG_Assert(com_dev->driver->rx); + com_dev = find_com_dev_by_id(port); + // PIOS_DEBUG_Assert(com_dev); + // PIOS_DEBUG_Assert(com_dev->driver->rx); - return com_dev->driver->rx(com_dev->id); + return com_dev->driver->rx(com_dev->id); } /** -* Get the number of bytes waiting in the buffer -* \param[in] port COM port -* \return Number of bytes used in buffer -*/ + * Get the number of bytes waiting in the buffer + * \param[in] port COM port + * \return Number of bytes used in buffer + */ int32_t PIOS_COM_ReceiveBufferUsed(uint8_t port) { - struct pios_com_dev * com_dev; + struct pios_com_dev *com_dev; - com_dev = find_com_dev_by_id (port); + com_dev = find_com_dev_by_id(port); - if (!com_dev) { - /* Undefined COM port for this board (see pios_board.c) */ - return 0; - } + if (!com_dev) { + /* Undefined COM port for this board (see pios_board.c) */ + return 0; + } - if (!com_dev->driver->rx_avail) { - return 0; - } + if (!com_dev->driver->rx_avail) { + return 0; + } - return com_dev->driver->rx_avail(com_dev->id); + return com_dev->driver->rx_avail(com_dev->id); } -#endif +#endif /* if defined(PIOS_INCLUDE_COM) */ diff --git a/flight/pios/win32/win32/pios_crc.c b/flight/pios/win32/win32/pios_crc.c index 548ba2648..0e4cd5dec 100644 --- a/flight/pios/win32/win32/pios_crc.c +++ b/flight/pios/win32/win32/pios_crc.c @@ -11,22 +11,22 @@ // CRC lookup table static const uint8_t crc_table[256] = { - 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, - 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, - 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, - 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, - 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, - 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, - 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, - 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, - 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, - 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, - 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, - 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, - 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, - 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, - 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, - 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 + 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, + 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, + 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, + 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, + 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, + 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, + 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, + 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, + 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, + 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, + 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, + 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, + 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, + 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, + 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, + 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 }; /** @@ -49,7 +49,7 @@ static const uint8_t crc_table[256] = { */ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) { - return crc_table[crc ^ data]; + return crc_table[crc ^ data]; } /* @@ -59,16 +59,16 @@ uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data) * @param[in] length Number of bytes to process * @returns Updated CRC */ -uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length) +uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t *data, int32_t length) { - // use registers for speed - register int32_t len = length; - register uint8_t crc8 = crc; - register const uint8_t *p = data; - - while (len--) - crc8 = crc_table[crc8 ^ *p++]; - - return crc8; -} + // use registers for speed + register int32_t len = length; + register uint8_t crc8 = crc; + register const uint8_t *p = data; + while (len--) { + crc8 = crc_table[crc8 ^ *p++]; + } + + return crc8; +} diff --git a/flight/pios/win32/win32/pios_delay.c b/flight/pios/win32/win32/pios_delay.c index 37a430462..ef0287a99 100644 --- a/flight/pios/win32/win32/pios_delay.c +++ b/flight/pios/win32/win32/pios_delay.c @@ -3,27 +3,27 @@ * * @file pios_delay.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) - * @brief Delay Functions - * - Provides a micro-second granular delay using a TIM + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * @brief Delay Functions + * - Provides a micro-second granular delay using a TIM * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_DELAY Delay Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,55 +34,55 @@ #if defined(PIOS_INCLUDE_DELAY) /** -* Initialises the Timer used by PIOS_DELAY functions
-* This is called from pios.c as part of the main() function -* at system start up. -* \return < 0 if initialisation failed -*/ + * Initialises the Timer used by PIOS_DELAY functions
+ * This is called from pios.c as part of the main() function + * at system start up. + * \return < 0 if initialisation failed + */ #include int32_t PIOS_DELAY_Init(void) { - // stub + // stub - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Waits for a specific number of uS
-* Example:
-* \code -* // Wait for 500 uS -* PIOS_DELAY_Wait_uS(500); -* \endcode -* \param[in] uS delay (1..65535 microseconds) -* \return < 0 on errors -*/ + * Waits for a specific number of uS
+ * Example:
+ * \code + * // Wait for 500 uS + * PIOS_DELAY_Wait_uS(500); + * \endcode + * \param[in] uS delay (1..65535 microseconds) + * \return < 0 on errors + */ int32_t PIOS_DELAY_WaituS(uint16_t uS) { - Sleep(uS/1000); + Sleep(uS / 1000); - return 0; + return 0; } /** -* Waits for a specific number of mS
-* Example:
-* \code -* // Wait for 500 mS -* PIOS_DELAY_Wait_mS(500); -* \endcode -* \param[in] mS delay (1..65535 milliseconds) -* \return < 0 on errors -*/ + * Waits for a specific number of mS
+ * Example:
+ * \code + * // Wait for 500 mS + * PIOS_DELAY_Wait_mS(500); + * \endcode + * \param[in] mS delay (1..65535 milliseconds) + * \return < 0 on errors + */ int32_t PIOS_DELAY_WaitmS(uint16_t mS) { - Sleep(mS); + Sleep(mS); - /* No error */ - return 0; + /* No error */ + return 0; } -#endif +#endif /* if defined(PIOS_INCLUDE_DELAY) */ diff --git a/flight/pios/win32/win32/pios_led.c b/flight/pios/win32/win32/pios_led.c index 4ac23cb2c..4e537e750 100644 --- a/flight/pios/win32/win32/pios_led.c +++ b/flight/pios/win32/win32/pios_led.c @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file pios_led.c + * @file pios_led.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief LED functions, init, toggle, on & off. * @see The GNU Public License (GPL) Version 3 @@ -9,19 +9,19 @@ * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,68 +35,69 @@ /* Local Variables */ -//static GPIO_TypeDef* LED_GPIO_PORT[PIOS_LED_NUM] = PIOS_LED_PORTS; -//static const uint32_t LED_GPIO_PIN[PIOS_LED_NUM] = PIOS_LED_PINS; -//static const uint32_t LED_GPIO_CLK[PIOS_LED_NUM] = PIOS_LED_CLKS; +// static GPIO_TypeDef* LED_GPIO_PORT[PIOS_LED_NUM] = PIOS_LED_PORTS; +// static const uint32_t LED_GPIO_PIN[PIOS_LED_NUM] = PIOS_LED_PINS; +// static const uint32_t LED_GPIO_CLK[PIOS_LED_NUM] = PIOS_LED_CLKS; static uint8_t LED_GPIO[PIOS_LED_NUM]; -static inline void PIOS_SetLED(LedTypeDef LED,uint8_t stat) { - printf("PIOS: LED %i status %i\n",LED,stat); - LED_GPIO[LED]=stat; +static inline void PIOS_SetLED(LedTypeDef LED, uint8_t stat) +{ + printf("PIOS: LED %i status %i\n", LED, stat); + LED_GPIO[LED] = stat; } /** -* Initialises all the LED's -*/ + * Initialises all the LED's + */ void PIOS_LED_Init(void) { - //GPIO_InitTypeDef GPIO_InitStructure; - //GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - - for(int LEDNum = 0; LEDNum < PIOS_LED_NUM; LEDNum++) { - //RCC_APB2PeriphClockCmd(LED_GPIO_CLK[LEDNum], ENABLE); - //GPIO_InitStructure.GPIO_Pin = LED_GPIO_PIN[LEDNum]; - //GPIO_Init(LED_GPIO_PORT[LEDNum], &GPIO_InitStructure); + // GPIO_InitTypeDef GPIO_InitStructure; + // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; + // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - /* LED's Off */ - //LED_GPIO_PORT[LEDNum]->BSRR = LED_GPIO_PIN[LEDNum]; - LED_GPIO[LEDNum]=0; - } + for (int LEDNum = 0; LEDNum < PIOS_LED_NUM; LEDNum++) { + // RCC_APB2PeriphClockCmd(LED_GPIO_CLK[LEDNum], ENABLE); + // GPIO_InitStructure.GPIO_Pin = LED_GPIO_PIN[LEDNum]; + // GPIO_Init(LED_GPIO_PORT[LEDNum], &GPIO_InitStructure); + + /* LED's Off */ + // LED_GPIO_PORT[LEDNum]->BSRR = LED_GPIO_PIN[LEDNum]; + LED_GPIO[LEDNum] = 0; + } } /** -* Turn on LED -* \param[in] LED LED Name (LED1, LED2) -*/ + * Turn on LED + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_On(LedTypeDef LED) { - //LED_GPIO_PORT[LED]->BRR = LED_GPIO_PIN[LED]; - PIOS_SetLED(LED,1); + // LED_GPIO_PORT[LED]->BRR = LED_GPIO_PIN[LED]; + PIOS_SetLED(LED, 1); } /** -* Turn off LED -* \param[in] LED LED Name (LED1, LED2) -*/ + * Turn off LED + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_Off(LedTypeDef LED) { - //LED_GPIO_PORT[LED]->BSRR = LED_GPIO_PIN[LED]; - PIOS_SetLED(LED,0); + // LED_GPIO_PORT[LED]->BSRR = LED_GPIO_PIN[LED]; + PIOS_SetLED(LED, 0); } /** -* Toggle LED on/off -* \param[in] LED LED Name (LED1, LED2) -*/ + * Toggle LED on/off + * \param[in] LED LED Name (LED1, LED2) + */ void PIOS_LED_Toggle(LedTypeDef LED) { - //LED_GPIO_PORT[LED]->ODR ^= LED_GPIO_PIN[LED]; - PIOS_SetLED(LED,LED_GPIO[LED]?0:1); + // LED_GPIO_PORT[LED]->ODR ^= LED_GPIO_PIN[LED]; + PIOS_SetLED(LED, LED_GPIO[LED] ? 0 : 1); } -#endif +#endif /* if defined(PIOS_INCLUDE_LED) */ diff --git a/flight/pios/win32/win32/pios_sdcard.c b/flight/pios/win32/win32/pios_sdcard.c index 78da48e46..a1190499e 100644 --- a/flight/pios/win32/win32/pios_sdcard.c +++ b/flight/pios/win32/win32/pios_sdcard.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_sdcard.c + * @file pios_sdcard.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) * @brief Sets up basic system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_SDCARD SDCard Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,186 +34,186 @@ /** -* Initialises SPI pins and peripheral to access MMC/SD Card -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ + * Initialises SPI pins and peripheral to access MMC/SD Card + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ int32_t PIOS_SDCARD_Init(void) { - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Connects to SD Card -* \return < 0 if initialisation sequence failed -*/ + * Connects to SD Card + * \return < 0 if initialisation sequence failed + */ int32_t PIOS_SDCARD_PowerOn(void) { - return 0; /* Status should be 0 if nothing went wrong! */ + return 0; /* Status should be 0 if nothing went wrong! */ } /** -* Disconnects from SD Card -* \return < 0 on errors -*/ + * Disconnects from SD Card + * \return < 0 on errors + */ int32_t PIOS_SDCARD_PowerOff(void) { - return 0; // no error + return 0; // no error } /** -* If SD card was previously available: Checks if the SD Card is still -* available by sending the STATUS command.
-* This takes ca. 10 uS -* -* If SD card was previously not available: Checks if the SD Card is -* available by sending the IDLE command at low speed.
-* This takes ca. 500 uS!
-* Once we got a positive response, SDCARD_PowerOn() will be -* called by this function to initialize the card completely. -* -* Example for Connection/Disconnection detection: -* \code -* // this function is called each second from a low-priority task -* // If multiple tasks are accessing the SD card, add a semaphore/mutex -* // to avoid IO access collisions with other tasks! -* u8 sdcard_available; -* int32_t CheckSDCard(void) -* { -* // check if SD card is available -* // High speed access if the card was previously available -* u8 prev_sdcard_available = sdcard_available; -* sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); -* -* if(sdcard_available && !prev_sdcard_available) { -* // SD Card has been connected -* -* // now it's possible to read/write sectors -* -* } else if( !sdcard_available && prev_sdcard_available ) { -* // SD Card has been disconnected -* -* // here you can notify your application about this state -* } -* -* return 0; // no error -* } -* \endcode -* \param[in] was_available should only be set if the SD card was previously available -* \return 0 if no response from SD Card -* \return 1 if SD card is accessible -*/ + * If SD card was previously available: Checks if the SD Card is still + * available by sending the STATUS command.
+ * This takes ca. 10 uS + * + * If SD card was previously not available: Checks if the SD Card is + * available by sending the IDLE command at low speed.
+ * This takes ca. 500 uS!
+ * Once we got a positive response, SDCARD_PowerOn() will be + * called by this function to initialize the card completely. + * + * Example for Connection/Disconnection detection: + * \code + * // this function is called each second from a low-priority task + * // If multiple tasks are accessing the SD card, add a semaphore/mutex + * // to avoid IO access collisions with other tasks! + * u8 sdcard_available; + * int32_t CheckSDCard(void) + * { + * // check if SD card is available + * // High speed access if the card was previously available + * u8 prev_sdcard_available = sdcard_available; + * sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available); + * + * if(sdcard_available && !prev_sdcard_available) { + * // SD Card has been connected + * + * // now it's possible to read/write sectors + * + * } else if( !sdcard_available && prev_sdcard_available ) { + * // SD Card has been disconnected + * + * // here you can notify your application about this state + * } + * + * return 0; // no error + * } + * \endcode + * \param[in] was_available should only be set if the SD card was previously available + * \return 0 if no response from SD Card + * \return 1 if SD card is accessible + */ int32_t PIOS_SDCARD_CheckAvailable(uint8_t was_available) { - return 1; /* 1 = available, 0 = not available. */ + return 1; /* 1 = available, 0 = not available. */ } /** -* Sends command to SD card -* \param[in] cmd SD card command -* \param[in] addr 32bit address -* \param[in] crc precalculated CRC -* \return >= 0x00 if command has been sent successfully (contains received byte) -* \return -1 if no response from SD Card (timeout) -*/ + * Sends command to SD card + * \param[in] cmd SD card command + * \param[in] addr 32bit address + * \param[in] crc precalculated CRC + * \return >= 0x00 if command has been sent successfully (contains received byte) + * \return -1 if no response from SD Card (timeout) + */ int32_t PIOS_SDCARD_SendSDCCmd(uint8_t cmd, uint32_t addr, uint8_t crc) { - return -1; + return -1; } /** -* Reads 512 bytes from selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads 512 bytes from selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_SectorRead(uint32_t sector, uint8_t *buffer) { - return -256; + return -256; } /** -* Writes 512 bytes into selected sector -* \param[in] sector 32bit sector -* \param[in] *buffer pointer to 512 byte buffer -* \return 0 if whole sector has been successfully read -* \return -error if error occured during read operation:
-*
    -*
  • Bit 0 - In idle state if 1 -*
  • Bit 1 - Erase Reset if 1 -*
  • Bit 2 - Illgal Command if 1 -*
  • Bit 3 - Com CRC Error if 1 -*
  • Bit 4 - Erase Sequence Error if 1 -*
  • Bit 5 - Address Error if 1 -*
  • Bit 6 - Parameter Error if 1 -*
  • Bit 7 - Not used, always '0' -*
-* \return -256 if timeout during command has been sent -* \return -257 if write operation not accepted -* \return -258 if timeout during write operation -*/ + * Writes 512 bytes into selected sector + * \param[in] sector 32bit sector + * \param[in] *buffer pointer to 512 byte buffer + * \return 0 if whole sector has been successfully read + * \return -error if error occured during read operation:
+ *
    + *
  • Bit 0 - In idle state if 1 + *
  • Bit 1 - Erase Reset if 1 + *
  • Bit 2 - Illgal Command if 1 + *
  • Bit 3 - Com CRC Error if 1 + *
  • Bit 4 - Erase Sequence Error if 1 + *
  • Bit 5 - Address Error if 1 + *
  • Bit 6 - Parameter Error if 1 + *
  • Bit 7 - Not used, always '0' + *
+ * \return -256 if timeout during command has been sent + * \return -257 if write operation not accepted + * \return -258 if timeout during write operation + */ int32_t PIOS_SDCARD_SectorWrite(uint32_t sector, uint8_t *buffer) { - return -256; + return -256; } /** -* Reads the CID informations from SD Card -* \param[in] *cid pointer to buffer which holds the CID informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads the CID informations from SD Card + * \param[in] *cid pointer to buffer which holds the CID informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_CIDRead(SDCARDCidTypeDef *cid) { - return -256; + return -256; } /** -* Reads the CSD informations from SD Card -* \param[in] *csd pointer to buffer which holds the CSD informations -* \return 0 if the informations haven been successfully read -* \return -error if error occured during read operation -* \return -256 if timeout during command has been sent -* \return -257 if timeout while waiting for start token -*/ + * Reads the CSD informations from SD Card + * \param[in] *csd pointer to buffer which holds the CSD informations + * \return 0 if the informations haven been successfully read + * \return -error if error occured during read operation + * \return -256 if timeout during command has been sent + * \return -257 if timeout while waiting for start token + */ int32_t PIOS_SDCARD_CSDRead(SDCARDCsdTypeDef *csd) { - return -256; + return -256; } /** -* Attempts to write a startup log to the SDCard -* return 0 No errors -* return -1 Error deleting file -* return -2 Error opening file -* return -3 Error writing file -*/ + * Attempts to write a startup log to the SDCard + * return 0 No errors + * return -1 Error deleting file + * return -2 Error opening file + * return -3 Error writing file + */ int32_t PIOS_SDCARD_StartupLog(void) { - return -3; + return -3; } /** @@ -223,78 +223,78 @@ int32_t PIOS_SDCARD_StartupLog(void) */ int32_t PIOS_SDCARD_IsMounted() { - return 1; + return 1; } /** -* Mounts the file system -* param[in] CreateStartupLog 1 = True, 0 = False -* return 0 No errors -* return -1 SDCard not available -* return -2 Cannot find first partition -* return -3 No volume information -* return -4 Error writing startup log file -*/ + * Mounts the file system + * param[in] CreateStartupLog 1 = True, 0 = False + * return 0 No errors + * return -1 SDCard not available + * return -2 Cannot find first partition + * return -3 No volume information + * return -4 Error writing startup log file + */ int32_t PIOS_SDCARD_MountFS(uint32_t CreateStartupLog) { - return 0; + return 0; } /** -* Mounts the file system -* return Amount of free bytes -*/ + * Mounts the file system + * return Amount of free bytes + */ int32_t PIOS_SDCARD_GetFree(void) { - return 10240; + return 10240; } /** -* Read from file -* return 0 No error -* return -1 DFS_ReadFile failed -* return -2 Less bytes read than expected -*/ -//int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len) -//{ - /* No error */ -// return -1; -//} + * Read from file + * return 0 No error + * return -1 DFS_ReadFile failed + * return -2 Less bytes read than expected + */ +// int32_t PIOS_SDCARD_ReadBuffer(PFILEINFO fileinfo, uint8_t *buffer, uint32_t len) +// { +/* No error */ +// return -1; +// } /** -* Read a line from file -* returns Number of bytes read -*/ -//int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len) -//{ -// return -1; -//} + * Read a line from file + * returns Number of bytes read + */ +// int32_t PIOS_SDCARD_ReadLine(PFILEINFO fileinfo, uint8_t *buffer, uint32_t max_len) +// { +// return -1; +// } /** -* Copy a file -* WARNING: This will overwrite the destination file even if it exists -* param[in] *Source Path to file to copy -* param[in] *Destination Path to destination file -* return 0 No errors -* return -1 Source file doesn't exist -* return -2 Failed to create destination file -* return -3 DFS_ReadFile failed -* return -4 DFS_WriteFile failed -*/ + * Copy a file + * WARNING: This will overwrite the destination file even if it exists + * param[in] *Source Path to file to copy + * param[in] *Destination Path to destination file + * return 0 No errors + * return -1 Source file doesn't exist + * return -2 Failed to create destination file + * return -3 DFS_ReadFile failed + * return -4 DFS_WriteFile failed + */ int32_t PIOS_SDCARD_FileCopy(char *Source, char *Destination) { - return -2; + return -2; } /** -* Delete a file -* param[in] *Filename File to delete -* return 0 No errors -* return -1 Error deleting file -*/ + * Delete a file + * param[in] *Filename File to delete + * return 0 No errors + * return -1 Error deleting file + */ int32_t PIOS_SDCARD_FileDelete(char *Filename) { - return -1; + return -1; } -#endif +#endif /* if defined(PIOS_INCLUDE_SDCARD) */ diff --git a/flight/pios/win32/win32/pios_servo.c b/flight/pios/win32/win32/pios_servo.c index d0cf530d3..c3b4ba05b 100644 --- a/flight/pios/win32/win32/pios_servo.c +++ b/flight/pios/win32/win32/pios_servo.c @@ -6,25 +6,25 @@ * @brief Code to do set RC servo output * @{ * - * @file pios_servo.c + * @file pios_servo.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RC Servo routines (STM32 dependent) * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,36 +39,33 @@ static volatile uint16_t ServoPosition[PIOS_SERVO_NUM_TIMERS]; /** -* Initialise Servos -*/ + * Initialise Servos + */ void PIOS_Servo_Init(void) -{ -} +{} /** -* Set the servo update rate (Max 500Hz) -* \param[in] onetofour Rate for outputs 1 to 4 (Hz) -* \param[in] fivetoeight Rate for outputs 5 to 8 (Hz) -*/ -void PIOS_Servo_SetHz(const uint16_t * banks, uint8_t num_banks) -{ -} + * Set the servo update rate (Max 500Hz) + * \param[in] onetofour Rate for outputs 1 to 4 (Hz) + * \param[in] fivetoeight Rate for outputs 5 to 8 (Hz) + */ +void PIOS_Servo_SetHz(const uint16_t *banks, uint8_t num_banks) +{} /** -* Set servo position -* \param[in] Servo Servo number (0-7) -* \param[in] Position Servo position in milliseconds -*/ + * Set servo position + * \param[in] Servo Servo number (0-7) + * \param[in] Position Servo position in milliseconds + */ void PIOS_Servo_Set(uint8_t Servo, uint16_t Position) { #ifndef PIOS_ENABLE_DEBUG_PINS - /* Make sure servo exists */ - if (Servo < PIOS_SERVO_NUM_OUTPUTS) { - /* Update the position */ - ServoPosition[Servo] = Position; - - } + /* Make sure servo exists */ + if (Servo < PIOS_SERVO_NUM_OUTPUTS) { + /* Update the position */ + ServoPosition[Servo] = Position; + } #endif // PIOS_ENABLE_DEBUG_PINS } -#endif +#endif /* if defined(PIOS_INCLUDE_SERVO) */ diff --git a/flight/pios/win32/win32/pios_sys.c b/flight/pios/win32/win32/pios_sys.c index 999796407..382a94a87 100644 --- a/flight/pios/win32/win32/pios_sys.c +++ b/flight/pios/win32/win32/pios_sys.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_sys.c + * @file pios_sys.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Sets up basic system hardware, functions are called from Main. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_SYS System Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,149 +38,150 @@ void NVIC_Configuration(void); void SysTick_Handler(void); /* Local Macros */ -#define MEM8(addr) (*((volatile uint8_t *)(addr))) +#define MEM8(addr) (*((volatile uint8_t *)(addr))) /** -* Initialises all system peripherals -*/ + * Initialises all system peripherals + */ void PIOS_SYS_Init(void) { + /** + * stub + */ + printf("PIOS_SYS_Init\n"); - /** - * stub - */ - printf("PIOS_SYS_Init\n"); - - /* Initialise Basic NVIC */ - NVIC_Configuration(); + /* Initialise Basic NVIC */ + NVIC_Configuration(); #if defined(PIOS_INCLUDE_LED) - /* Initialise LEDs */ - PIOS_LED_Init(); + /* Initialise LEDs */ + PIOS_LED_Init(); #endif } /** -* Shutdown PIOS and reset the microcontroller:
-*
    -*
  • Disable all RTOS tasks -*
  • Disable all interrupts -*
  • Turn off all board LEDs -*
  • Reset STM32 -*
-* \return < 0 if reset failed -*/ + * Shutdown PIOS and reset the microcontroller:
+ *
    + *
  • Disable all RTOS tasks + *
  • Disable all interrupts + *
  • Turn off all board LEDs + *
  • Reset STM32 + *
+ * \return < 0 if reset failed + */ int32_t PIOS_SYS_Reset(void) { - /** - * stub - */ - printf("PIOS_SYS_Reset\n"); - /* Disable all RTOS tasks */ + /** + * stub + */ + printf("PIOS_SYS_Reset\n"); + /* Disable all RTOS tasks */ #if defined(PIOS_INCLUDE_FREERTOS) - /* port specific FreeRTOS function to disable tasks (nested) */ - portENTER_CRITICAL(); + /* port specific FreeRTOS function to disable tasks (nested) */ + portENTER_CRITICAL(); #endif - // disable all interrupts - //PIOS_IRQ_Disable(); + // disable all interrupts + // PIOS_IRQ_Disable(); - // turn off all board LEDs - #if (PIOS_LED_NUM == 1) - PIOS_LED_Off(LED1); - #elif (PIOS_LED_NUM == 2) - PIOS_LED_Off(LED1); - PIOS_LED_Off(LED2); - #endif + // turn off all board LEDs + #if (PIOS_LED_NUM == 1) + PIOS_LED_Off(LED1); + #elif (PIOS_LED_NUM == 2) + PIOS_LED_Off(LED1); + PIOS_LED_Off(LED2); + #endif + /* Reset STM32 */ + // RCC_APB2PeriphResetCmd(0xfffffff8, ENABLE); /* MBHP_CORE_STM32: don't reset GPIOA/AF due to USB pins */ + // RCC_APB1PeriphResetCmd(0xff7fffff, ENABLE); /* don't reset USB, so that the connection can survive! */ - /* Reset STM32 */ - //RCC_APB2PeriphResetCmd(0xfffffff8, ENABLE); /* MBHP_CORE_STM32: don't reset GPIOA/AF due to USB pins */ - //RCC_APB1PeriphResetCmd(0xff7fffff, ENABLE); /* don't reset USB, so that the connection can survive! */ + // RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + // RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + // SCB->AIRCR = NVIC_AIRCR_VECTKEY | (1 << NVIC_VECTRESET); + exit(1); - //RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - //RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - //SCB->AIRCR = NVIC_AIRCR_VECTKEY | (1 << NVIC_VECTRESET); - exit(1); + while (1) { + ; + } - while(1); - - /* We will never reach this point */ - return -1; + /* We will never reach this point */ + return -1; } /** -* Returns the serial number as a string -* param[out] str pointer to a string which can store at least 32 digits + zero terminator! -* (24 digits returned for STM32) -* return < 0 if feature not supported -*/ + * Returns the serial number as a string + * param[out] str pointer to a string which can store at least 32 digits + zero terminator! + * (24 digits returned for STM32) + * return < 0 if feature not supported + */ int32_t PIOS_SYS_SerialNumberGet(char *str) { - int i; + int i; - /* Stored in the so called "electronic signature" */ - for(i=0; i<24; ++i) { - //uint8_t b = MEM8(0x1ffff7e8 + (i/2)); - //if( !(i & 1) ) - //b >>= 4; - //b &= 0x0f; + /* Stored in the so called "electronic signature" */ + for (i = 0; i < 24; ++i) { + // uint8_t b = MEM8(0x1ffff7e8 + (i/2)); + // if( !(i & 1) ) + // b >>= 4; + // b &= 0x0f; - //str[i] = ((b > 9) ? ('A'-10) : '0') + b; - str[i]='6'; - } - str[i] = 0; + // str[i] = ((b > 9) ? ('A'-10) : '0') + b; + str[i] = '6'; + } + str[i] = 0; - /* No error */ - return 0; + /* No error */ + return 0; } /** -* Configures Vector Table base location and SysTick -*/ + * Configures Vector Table base location and SysTick + */ void NVIC_Configuration(void) { - /** - * stub - */ - printf("NVIC_Configuration\n"); - /* Set the Vector Table base address as specified in .ld file */ - //NVIC_SetVectorTable(PIOS_NVIC_VECTTAB_FLASH, 0x0); + /** + * stub + */ + printf("NVIC_Configuration\n"); + /* Set the Vector Table base address as specified in .ld file */ + // NVIC_SetVectorTable(PIOS_NVIC_VECTTAB_FLASH, 0x0); - /* 4 bits for Interrupt priorities so no sub priorities */ - //NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + /* 4 bits for Interrupt priorities so no sub priorities */ + // NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); - /* Configure HCLK clock as SysTick clock source. */ - //SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK); + /* Configure HCLK clock as SysTick clock source. */ + // SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK); } #ifdef USE_FULL_ASSERT /** -* Reports the name of the source file and the source line number -* where the assert_param error has occurred. -* \param[in] file pointer to the source file name -* \param[in] line assert_param error line source number -* \retval None -*/ -void assert_failed(uint8_t* file, uint32_t line) + * Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * \param[in] file pointer to the source file name + * \param[in] line assert_param error line source number + * \retval None + */ +void assert_failed(uint8_t *file, uint32_t line) { - /* When serial debugging is implemented, use something like this. */ - /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ - printf("Wrong parameters value: file %s on line %d\r\n", file, line); + /* When serial debugging is implemented, use something like this. */ + /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ + printf("Wrong parameters value: file %s on line %d\r\n", file, line); - /* Setup the LEDs to Alternate */ - PIOS_LED_On(LED1); - PIOS_LED_Off(LED2); + /* Setup the LEDs to Alternate */ + PIOS_LED_On(LED1); + PIOS_LED_Off(LED2); - /* Infinite loop */ - while (1) - { - PIOS_LED_Toggle(LED1); - PIOS_LED_Toggle(LED2); - for(int i = 0; i < 1000000; i++); - } + /* Infinite loop */ + while (1) { + PIOS_LED_Toggle(LED1); + PIOS_LED_Toggle(LED2); + for (int i = 0; i < 1000000; i++) { + ; + } + } } -#endif +#endif /* ifdef USE_FULL_ASSERT */ -#endif +#endif /* if defined(PIOS_INCLUDE_SYS) */ diff --git a/flight/pios/win32/win32/pios_udp.c b/flight/pios/win32/win32/pios_udp.c index ed4342e4c..841156598 100644 --- a/flight/pios/win32/win32/pios_udp.c +++ b/flight/pios/win32/win32/pios_udp.c @@ -1,28 +1,28 @@ /** ****************************************************************************** * - * @file pios_udp.c + * @file pios_udp.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief UDP commands. Inits UDPs, controls UDPs & Interupt handlers. * @see The GNU Public License (GPL) Version 3 * @defgroup PIOS_UDP UDP Functions * @{ * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,61 +36,61 @@ /* Provide a COM driver */ const struct pios_com_driver pios_udp_com_driver = { - .set_baud = PIOS_UDP_ChangeBaud, - .tx_nb = PIOS_UDP_TxBufferPutMoreNonBlocking, - .tx = PIOS_UDP_TxBufferPutMore, - .rx = PIOS_UDP_RxBufferGet, - .rx_avail = PIOS_UDP_RxBufferUsed, + .set_baud = PIOS_UDP_ChangeBaud, + .tx_nb = PIOS_UDP_TxBufferPutMoreNonBlocking, + .tx = PIOS_UDP_TxBufferPutMore, + .rx = PIOS_UDP_RxBufferGet, + .rx_avail = PIOS_UDP_RxBufferUsed, }; WSADATA wsaData; -static struct pios_udp_dev * find_udp_dev_by_id (uint8_t udp) +static struct pios_udp_dev *find_udp_dev_by_id(uint8_t udp) { - if (udp >= pios_udp_num_devices) { - /* Undefined UDP port for this board (see pios_board.c) */ - return NULL; - } + if (udp >= pios_udp_num_devices) { + /* Undefined UDP port for this board (see pios_board.c) */ + return NULL; + } - /* Get a handle for the device configuration */ - return &(pios_udp_devs[udp]); + /* Get a handle for the device configuration */ + return &(pios_udp_devs[udp]); } /** -* Open some UDP sockets -*/ + * Open some UDP sockets + */ void PIOS_UDP_Init(void) { - struct pios_udp_dev * udp_dev; - uint8_t i; + struct pios_udp_dev *udp_dev; + uint8_t i; - WSAStartup(MAKEWORD(2, 0), &wsaData); + WSAStartup(MAKEWORD(2, 0), &wsaData); - for (i = 0; i < pios_udp_num_devices; i++) { - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(i); - //PIOS_DEBUG_Assert(udp_dev); + for (i = 0; i < pios_udp_num_devices; i++) { + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(i); + // PIOS_DEBUG_Assert(udp_dev); - /* Clear buffer counters */ - udp_dev->rx.head = udp_dev->rx.tail = udp_dev->rx.size = 0; + /* Clear buffer counters */ + udp_dev->rx.head = udp_dev->rx.tail = udp_dev->rx.size = 0; - /* assign socket */ - udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - memset(&udp_dev->server,0,sizeof(udp_dev->server)); - memset(&udp_dev->client,0,sizeof(udp_dev->client)); - udp_dev->server.sin_family = AF_INET; - udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip); - udp_dev->server.sin_port = htons(udp_dev->cfg->port); - BOOL tmp = TRUE; - setsockopt(udp_dev->socket, SOL_SOCKET, SO_BROADCAST, (char *)&tmp, sizeof(tmp)); - int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server)); - /* use nonblocking IO */ - u_long argp = 1; - ioctlsocket(udp_dev->socket, FIONBIO, &argp); - printf("udp dev %i - socket %i opened - result %i\n",i,udp_dev->socket,res); + /* assign socket */ + udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + memset(&udp_dev->server, 0, sizeof(udp_dev->server)); + memset(&udp_dev->client, 0, sizeof(udp_dev->client)); + udp_dev->server.sin_family = AF_INET; + udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip); + udp_dev->server.sin_port = htons(udp_dev->cfg->port); + BOOL tmp = TRUE; + setsockopt(udp_dev->socket, SOL_SOCKET, SO_BROADCAST, (char *)&tmp, sizeof(tmp)); + int res = bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server, sizeof(udp_dev->server)); + /* use nonblocking IO */ + u_long argp = 1; + ioctlsocket(udp_dev->socket, FIONBIO, &argp); + printf("udp dev %i - socket %i opened - result %i\n", i, udp_dev->socket, res); - /* TODO do some error handling - wait no, we can't - we are void anyway ;) */ - } + /* TODO do some error handling - wait no, we can't - we are void anyway ;) */ + } } /** @@ -98,381 +98,377 @@ void PIOS_UDP_Init(void) */ void PIOS_UDP_Close(void) { - WSACleanup(); + WSACleanup(); } /** -* Changes the baud rate of the UDP peripheral without re-initialising. -* \param[in] udp UDP name (GPS, TELEM, AUX) -* \param[in] baud Requested baud rate -*/ + * Changes the baud rate of the UDP peripheral without re-initialising. + * \param[in] udp UDP name (GPS, TELEM, AUX) + * \param[in] baud Requested baud rate + */ void PIOS_UDP_ChangeBaud(uint8_t udp, uint32_t baud) -{ -} +{} /** -* puts a byte onto the receive buffer -* \param[in] UDP UDP name -* \param[in] b byte which should be put into Rx buffer -* \return 0 if no error -* \return -1 if UDP not available -* \return -2 if buffer full (retry) -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * puts a byte onto the receive buffer + * \param[in] UDP UDP name + * \param[in] b byte which should be put into Rx buffer + * \return 0 if no error + * \return -1 if UDP not available + * \return -2 if buffer full (retry) + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_RxBufferPut(uint8_t udp, uint8_t b) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -1; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return -1; + } - if (udp_dev->rx.size >= sizeof(udp_dev->rx.buf)) { - /* Buffer full (retry) */ - return -2; - } + if (udp_dev->rx.size >= sizeof(udp_dev->rx.buf)) { + /* Buffer full (retry) */ + return -2; + } - /* Copy received byte into receive buffer */ - udp_dev->rx.buf[udp_dev->rx.head++] = b; - if (udp_dev->rx.head >= sizeof(udp_dev->rx.buf)) { - udp_dev->rx.head = 0; - } - udp_dev->rx.size++; + /* Copy received byte into receive buffer */ + udp_dev->rx.buf[udp_dev->rx.head++] = b; + if (udp_dev->rx.head >= sizeof(udp_dev->rx.buf)) { + udp_dev->rx.head = 0; + } + udp_dev->rx.size++; - /* No error */ - return 0; + /* No error */ + return 0; } /** * attempt to receive */ -void PIOS_UDP_RECV(uint8_t udp) { +void PIOS_UDP_RECV(uint8_t udp) +{ + struct pios_udp_dev *udp_dev; + char localbuffer[PIOS_UDP_RX_BUFFER_SIZE]; - struct pios_udp_dev * udp_dev; - char localbuffer[PIOS_UDP_RX_BUFFER_SIZE]; + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); - - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return; - } - - /* use nonblocking IO */ - u_long argp = 1; - ioctlsocket(udp_dev->socket, FIONBIO, &argp); - - /* receive data */ - int received; - udp_dev->clientLength=sizeof(udp_dev->client); - if ((received = recvfrom(udp_dev->socket, - localbuffer, - (PIOS_UDP_RX_BUFFER_SIZE - udp_dev->rx.size), - 0, - (struct sockaddr *) &udp_dev->client, - (int*)&udp_dev->clientLength)) == SOCKET_ERROR) { - - return; - } - /* copy received data to buffer */ - int t; - for (t=0;tsocket, FIONBIO, &argp); + /* receive data */ + int received; + udp_dev->clientLength = sizeof(udp_dev->client); + if ((received = recvfrom(udp_dev->socket, + localbuffer, + (PIOS_UDP_RX_BUFFER_SIZE - udp_dev->rx.size), + 0, + (struct sockaddr *)&udp_dev->client, + (int *)&udp_dev->clientLength)) == SOCKET_ERROR) { + return; + } + /* copy received data to buffer */ + int t; + for (t = 0; t < received; t++) { + PIOS_UDP_RxBufferPut(udp, localbuffer[t]); + } } /** -* Returns number of free bytes in receive buffer -* \param[in] UDP UDP name -* \return UDP number of free bytes -* \return 1: UDP available -* \return 0: UDP not available -*/ + * Returns number of free bytes in receive buffer + * \param[in] UDP UDP name + * \return UDP number of free bytes + * \return 1: UDP available + * \return 0: UDP not available + */ int32_t PIOS_UDP_RxBufferFree(uint8_t udp) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -2; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return -2; + } - /* fill buffer */ - PIOS_UDP_RECV(udp); + /* fill buffer */ + PIOS_UDP_RECV(udp); - return (sizeof(udp_dev->rx.buf) - udp_dev->rx.size); + return sizeof(udp_dev->rx.buf) - udp_dev->rx.size; } /** -* Returns number of used bytes in receive buffer -* \param[in] UDP UDP name -* \return > 0: number of used bytes -* \return 0 if UDP not available -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * Returns number of used bytes in receive buffer + * \param[in] UDP UDP name + * \return > 0: number of used bytes + * \return 0 if UDP not available + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_RxBufferUsed(uint8_t udp) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -2; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return -2; + } - /* fill buffer */ - PIOS_UDP_RECV(udp); + /* fill buffer */ + PIOS_UDP_RECV(udp); - return (udp_dev->rx.size); + return udp_dev->rx.size; } /** -* Gets a byte from the receive buffer -* \param[in] UDP UDP name -* \return -1 if UDP not available -* \return -2 if no new byte available -* \return >= 0: number of received bytes -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * Gets a byte from the receive buffer + * \param[in] UDP UDP name + * \return -1 if UDP not available + * \return -2 if no new byte available + * \return >= 0: number of received bytes + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_RxBufferGet(uint8_t udp) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -2; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return -2; + } - /* fill buffer */ - PIOS_UDP_RECV(udp); + /* fill buffer */ + PIOS_UDP_RECV(udp); - if (!udp_dev->rx.size) { - /* Nothing new in the buffer */ - //printf("PIOS_UDP: nothing new in the buffer\n"); - return -1; - } + if (!udp_dev->rx.size) { + /* Nothing new in the buffer */ + // printf("PIOS_UDP: nothing new in the buffer\n"); + return -1; + } - /* get byte */ - uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail++]; - if (udp_dev->rx.tail >= sizeof(udp_dev->rx.buf)) { - udp_dev->rx.tail = 0; - } - udp_dev->rx.size--; - - //printf("PIOS_UDP: received byte: %c\n", (char) b); + /* get byte */ + uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail++]; + if (udp_dev->rx.tail >= sizeof(udp_dev->rx.buf)) { + udp_dev->rx.tail = 0; + } + udp_dev->rx.size--; - /* Return received byte */ - return b; + // printf("PIOS_UDP: received byte: %c\n", (char) b); + + /* Return received byte */ + return b; } /** -* Returns the next byte of the receive buffer without taking it -* \param[in] UDP UDP name -* \return -1 if UDP not available -* \return -2 if no new byte available -* \return >= 0: number of received bytes -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * Returns the next byte of the receive buffer without taking it + * \param[in] UDP UDP name + * \return -1 if UDP not available + * \return -2 if no new byte available + * \return >= 0: number of received bytes + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_RxBufferPeek(uint8_t udp) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -2; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return -2; + } - /* fill buffer */ - PIOS_UDP_RECV(udp); + /* fill buffer */ + PIOS_UDP_RECV(udp); - if (!udp_dev->rx.size) { - /* Nothing new in the buffer */ - return -1; - } + if (!udp_dev->rx.size) { + /* Nothing new in the buffer */ + return -1; + } - /* get byte */ - uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail]; - - /* Return received byte */ - return b; + /* get byte */ + uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail]; + + /* Return received byte */ + return b; } /** -* returns number of free bytes in transmit buffer -* \param[in] UDP UDP name -* \return number of free bytes -* \return 0 if UDP not available -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * returns number of free bytes in transmit buffer + * \param[in] UDP UDP name + * \return number of free bytes + * \return 0 if UDP not available + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_TxBufferFree(uint8_t udp) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return 0; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return 0; + } - return PIOS_UDP_RX_BUFFER_SIZE; + return PIOS_UDP_RX_BUFFER_SIZE; } /** -* returns number of used bytes in transmit buffer -* \param[in] UDP UDP name -* \return number of used bytes -* \return 0 if UDP not available -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * returns number of used bytes in transmit buffer + * \param[in] UDP UDP name + * \return number of used bytes + * \return 0 if UDP not available + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_TxBufferUsed(uint8_t udp) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); + + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return 0; + } - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ return 0; - } - - return 0; } /** -* puts more than one byte onto the transmit buffer (used for atomic sends) -* \param[in] UDP UDP name -* \param[in] *buffer pointer to buffer to be sent -* \param[in] len number of bytes to be sent -* \return 0 if no error -* \return -1 if UDP not available -* \return -2 if buffer full or cannot get all requested bytes (retry) -* \return -3 if UDP not supported by UDPTxBufferPut Routine -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * puts more than one byte onto the transmit buffer (used for atomic sends) + * \param[in] UDP UDP name + * \param[in] *buffer pointer to buffer to be sent + * \param[in] len number of bytes to be sent + * \return 0 if no error + * \return -1 if UDP not available + * \return -2 if buffer full or cannot get all requested bytes (retry) + * \return -3 if UDP not supported by UDPTxBufferPut Routine + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_TxBufferPutMoreNonBlocking(uint8_t udp, char *buffer, uint16_t len) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -1; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return -1; + } - if (len >= PIOS_UDP_RX_BUFFER_SIZE) { - /* Buffer cannot accept all requested bytes (retry) */ - return -2; - } - /* send data to client - non blocking*/ + if (len >= PIOS_UDP_RX_BUFFER_SIZE) { + /* Buffer cannot accept all requested bytes (retry) */ + return -2; + } + /* send data to client - non blocking*/ - /* use nonblocking IO */ - u_long argp = 1; - ioctlsocket(udp_dev->socket, FIONBIO, &argp); - sendto(udp_dev->socket, buffer, len, 0, - (struct sockaddr *) &udp_dev->client, - (int)sizeof(udp_dev->client)); + /* use nonblocking IO */ + u_long argp = 1; + ioctlsocket(udp_dev->socket, FIONBIO, &argp); + sendto(udp_dev->socket, buffer, len, 0, + (struct sockaddr *)&udp_dev->client, + (int)sizeof(udp_dev->client)); - /* No error */ - return 0; + /* No error */ + return 0; } /** -* puts more than one byte onto the transmit buffer (used for atomic sends)
-* (blocking function) -* \param[in] UDP UDP name -* \param[in] *buffer pointer to buffer to be sent -* \param[in] len number of bytes to be sent -* \return 0 if no error -* \return -1 if UDP not available -* \return -3 if UDP not supported by UDPTxBufferPut Routine -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * puts more than one byte onto the transmit buffer (used for atomic sends)
+ * (blocking function) + * \param[in] UDP UDP name + * \param[in] *buffer pointer to buffer to be sent + * \param[in] len number of bytes to be sent + * \return 0 if no error + * \return -1 if UDP not available + * \return -3 if UDP not supported by UDPTxBufferPut Routine + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_TxBufferPutMore(uint8_t udp, char *buffer, uint16_t len) { - struct pios_udp_dev * udp_dev; + struct pios_udp_dev *udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + /* Get a handle for the device configuration */ + udp_dev = find_udp_dev_by_id(udp); - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -1; - } + if (!udp_dev) { + /* Undefined UDP port for this board (see pios_board.c) */ + return -1; + } - if (len >= PIOS_UDP_RX_BUFFER_SIZE) { - /* Buffer cannot accept all requested bytes (retry) */ - return -2; - } + if (len >= PIOS_UDP_RX_BUFFER_SIZE) { + /* Buffer cannot accept all requested bytes (retry) */ + return -2; + } - /* send data to client - blocking*/ - /* use blocking IO */ - u_long argp = 1; - ioctlsocket(udp_dev->socket, FIONBIO, &argp); - sendto(udp_dev->socket, buffer, len, 0, - (struct sockaddr *) &udp_dev->client, - sizeof(udp_dev->client)); + /* send data to client - blocking*/ + /* use blocking IO */ + u_long argp = 1; + ioctlsocket(udp_dev->socket, FIONBIO, &argp); + sendto(udp_dev->socket, buffer, len, 0, + (struct sockaddr *)&udp_dev->client, + sizeof(udp_dev->client)); - //printf("PIOS_UDP: sent data\n"); + // printf("PIOS_UDP: sent data\n"); - /* No error */ - return 0; + /* No error */ + return 0; } /** -* puts a byte onto the transmit buffer -* \param[in] UDP UDP name -* \param[in] b byte which should be put into Tx buffer -* \return 0 if no error -* \return -1 if UDP not available -* \return -2 if buffer full (retry) -* \return -3 if UDP not supported by UDPTxBufferPut Routine -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * puts a byte onto the transmit buffer + * \param[in] UDP UDP name + * \param[in] b byte which should be put into Tx buffer + * \return 0 if no error + * \return -1 if UDP not available + * \return -2 if buffer full (retry) + * \return -3 if UDP not supported by UDPTxBufferPut Routine + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_TxBufferPut_NonBlocking(uint8_t udp, char b) { - return PIOS_UDP_TxBufferPutMoreNonBlocking(udp, &b, 1); + return PIOS_UDP_TxBufferPutMoreNonBlocking(udp, &b, 1); } /** -* puts a byte onto the transmit buffer
-* (blocking function) -* \param[in] UDP UDP name -* \param[in] b byte which should be put into Tx buffer -* \return 0 if no error -* \return -1 if UDP not available -* \return -3 if UDP not supported by UDPTxBufferPut Routine -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ + * puts a byte onto the transmit buffer
+ * (blocking function) + * \param[in] UDP UDP name + * \param[in] b byte which should be put into Tx buffer + * \return 0 if no error + * \return -1 if UDP not available + * \return -3 if UDP not supported by UDPTxBufferPut Routine + * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions + */ int32_t PIOS_UDP_TxBufferPut(uint8_t udp, char b) { - return PIOS_UDP_TxBufferPutMore(udp, &b, 1); + return PIOS_UDP_TxBufferPutMore(udp, &b, 1); } -#endif +#endif /* if defined(PIOS_INCLUDE_UDP) */ diff --git a/flight/pios/win32/win32/pios_wdg.c b/flight/pios/win32/win32/pios_wdg.c index e3b7701d4..c95907d1d 100644 --- a/flight/pios/win32/win32/pios_wdg.c +++ b/flight/pios/win32/win32/pios_wdg.c @@ -8,12 +8,12 @@ * * @file pios_spi.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief Hardware Abstraction Layer for SPI ports of STM32 * @see The GNU Public License (GPL) Version 3 * @notes * - * The PIOS Watchdog provides a HAL to initialize a watchdog + * The PIOS Watchdog provides a HAL to initialize a watchdog * *****************************************************************************/ /* @@ -34,17 +34,17 @@ #include "pios.h" -/** +/** * @brief Initialize the watchdog timer for a specified timeout * - * It is important to note that this function returns the achieved timeout - * for this hardware. For hardware indendence this should be checked when - * scheduling updates. Other hardware dependent details may need to be - * considered such as a window time which sets a minimum update time, - * and this function should return a recommended delay for clearing. + * It is important to note that this function returns the achieved timeout + * for this hardware. For hardware indendence this should be checked when + * scheduling updates. Other hardware dependent details may need to be + * considered such as a window time which sets a minimum update time, + * and this function should return a recommended delay for clearing. * * For the STM32 nominal clock rate is 32 khz, but for the maximum clock rate of - * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is + * 60 khz and a prescalar of 4 yields a clock rate of 15 khz. The delay that is * set in the watchdog assumes the nominal clock rate, but the delay for FreeRTOS * to use is 75% of the minimal delay. * @@ -52,65 +52,64 @@ * @returns Maximum recommended delay between updates */ void PIOS_WDG_Init() -{ -} +{} /** - * @brief Register a module against the watchdog - * + * @brief Register a module against the watchdog + * * There are two ways to use PIOS WDG: this is for when - * multiple modules must be monitored. In this case they - * must first register against the watchdog system and + * multiple modules must be monitored. In this case they + * must first register against the watchdog system and * only when all of the modules have been updated with the - * watchdog be cleared. Each module must have its own - * bit in the 16 bit + * watchdog be cleared. Each module must have its own + * bit in the 16 bit * * @param[in] flag the bit this module wants to use * @returns True if that bit is unregistered */ -bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) +bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) { - return true; + return true; } /** * @brief Function called by modules to indicate they are still running * - * This function will set this flag in the active flags register (which is + * This function will set this flag in the active flags register (which is * a backup regsiter) and if all the registered flags are set will clear * the watchdog and set only this flag in the backup register * * @param[in] flag the flag to set * @return true if the watchdog cleared, false if flags are pending */ -bool PIOS_WDG_UpdateFlag(uint16_t flag) -{ - return true; +bool PIOS_WDG_UpdateFlag(uint16_t flag) +{ + return true; } -/** +/** * @brief Returns the flags that were set at bootup - * - * This is used for diagnostics, if only one flag not set this + * + * This is used for diagnostics, if only one flag not set this * was likely the module that wasn't running before reset - * + * * @return The active flags register from bootup */ uint16_t PIOS_WDG_GetBootupFlags() { - return (uint16_t) 0xffff; + return (uint16_t)0xffff; } -/** +/** * @brief Returns the currently active flags - * + * * For external monitoring - * + * * @return The active flags register */ uint16_t PIOS_WDG_GetActiveFlags() { - return (uint16_t) 0xffff; + return (uint16_t)0xffff; } /** @@ -119,5 +118,4 @@ uint16_t PIOS_WDG_GetActiveFlags() * This function must be called at the appropriate delay to prevent a reset event occuring */ void PIOS_WDG_Clear(void) -{ -} \ No newline at end of file +{} diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index ad042fada..4b1625e9e 100644 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -9,19 +9,19 @@ * @{ * @brief Defines board specific static initializers for hardware for the CopterControl board. *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,52 +32,54 @@ #include static const struct pios_led pios_leds_cc[] = { - [PIOS_LED_HEARTBEAT] = { - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, }; static const struct pios_led_cfg pios_led_cfg_cc = { - .leds = pios_leds_cc, - .num_leds = NELEMENTS(pios_leds_cc), + .leds = pios_leds_cc, + .num_leds = NELEMENTS(pios_leds_cc), }; static const struct pios_led pios_leds_cc3d[] = { - [PIOS_LED_HEARTBEAT] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - .remap = GPIO_Remap_SWJ_JTAGDisable, - }, + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + .remap = GPIO_Remap_SWJ_JTAGDisable, + }, }; static const struct pios_led_cfg pios_led_cfg_cc3d = { - .leds = pios_leds_cc3d, - .num_leds = NELEMENTS(pios_leds_cc3d), + .leds = pios_leds_cc3d, + .num_leds = NELEMENTS(pios_leds_cc3d), }; -const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (uint32_t board_revision) +const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(uint32_t board_revision) { - switch (board_revision) { - case BOARD_REVISION_CC: return &pios_led_cfg_cc; - case BOARD_REVISION_CC3D: return &pios_led_cfg_cc3d; - default: return NULL; - } + switch (board_revision) { + case BOARD_REVISION_CC: return &pios_led_cfg_cc; + + case BOARD_REVISION_CC3D: return &pios_led_cfg_cc3d; + + default: return NULL; + } } -#endif /* PIOS_INCLUDE_LED */ +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_SPI) @@ -85,349 +87,356 @@ const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (uint32_t board_revisio /* Gyro interface */ void PIOS_SPI_gyro_irq_handler(void); -void DMA1_Channel2_IRQHandler() __attribute__ ((alias ("PIOS_SPI_gyro_irq_handler"))); -void DMA1_Channel3_IRQHandler() __attribute__ ((alias ("PIOS_SPI_gyro_irq_handler"))); +void DMA1_Channel2_IRQHandler() __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); +void DMA1_Channel3_IRQHandler() __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); static const struct pios_spi_cfg pios_spi_gyro_cfg = { - .regs = SPI1, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */ - }, - .use_crc = false, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), - .init = { - .NVIC_IRQChannel = DMA1_Channel2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel2, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel3, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .sclk = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .mosi = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .slave_count = 1, - .ssel = {{ - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }}, + .regs = SPI1, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */ + }, + .use_crc = false, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), + .init = { + .NVIC_IRQChannel = DMA1_Channel2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel2, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel3, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .sclk = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .mosi = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + } + }, }; static uint32_t pios_spi_gyro_id; void PIOS_SPI_gyro_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); } /* Flash/Accel Interface - * - * NOTE: Leave this declared as const data so that it ends up in the + * + * NOTE: Leave this declared as const data so that it ends up in the * .rodata section (ie. Flash) rather than in the .bss section (RAM). */ void PIOS_SPI_flash_accel_irq_handler(void); -void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler"))); -void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler"))); +void DMA1_Channel4_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler"))); +void DMA1_Channel5_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler"))); static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = { - .regs = SPI2, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, - }, - .use_crc = false, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), - .init = { - .NVIC_IRQChannel = DMA1_Channel4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel4, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel5, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .slave_count = 2, - .ssel = {{ - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }},{ - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }}, + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, + }, + .use_crc = false, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .slave_count = 2, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + } + },{ + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + } + }, }; static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = { - .regs = SPI2, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, - }, - .use_crc = false, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), - .init = { - .NVIC_IRQChannel = DMA1_Channel4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel4, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel5, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .slave_count = 2, - .ssel = {{ - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }},{ - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }}, + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, + }, + .use_crc = false, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .slave_count = 2, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + } + },{ + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + } + }, }; static uint32_t pios_spi_flash_accel_id; void PIOS_SPI_flash_accel_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_flash_accel_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_flash_accel_id); } -#endif /* PIOS_INCLUDE_SPI */ +#endif /* PIOS_INCLUDE_SPI */ #if defined(PIOS_INCLUDE_FLASH) #include "pios_flashfs_logfs_priv.h" #include "pios_flash_jedec_priv.h" static const struct flashfs_logfs_cfg flashfs_w25x_cfg = { - .fs_magic = 0x99abcdef, - .total_fs_size = 0x00080000, /* 512K bytes (128 sectors = entire chip) */ - .arena_size = 0x00010000, /* 256 * slot size */ - .slot_size = 0x00000100, /* 256 bytes */ + .fs_magic = 0x99abcdef, + .total_fs_size = 0x00080000, /* 512K bytes (128 sectors = entire chip) */ + .arena_size = 0x00010000, /* 256 * slot size */ + .slot_size = 0x00000100, /* 256 bytes */ - .start_offset = 0, /* start at the beginning of the chip */ - .sector_size = 0x00001000, /* 4K bytes */ - .page_size = 0x00000100, /* 256 bytes */ + .start_offset = 0, /* start at the beginning of the chip */ + .sector_size = 0x00001000, /* 4K bytes */ + .page_size = 0x00000100, /* 256 bytes */ }; - static const struct flashfs_logfs_cfg flashfs_m25p_cfg = { - .fs_magic = 0x99abceef, - .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ - .arena_size = 0x00010000, /* 256 * slot size */ - .slot_size = 0x00000100, /* 256 bytes */ + .fs_magic = 0x99abceef, + .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ + .arena_size = 0x00010000, /* 256 * slot size */ + .slot_size = 0x00000100, /* 256 bytes */ - .start_offset = 0, /* start at the beginning of the chip */ - .sector_size = 0x00010000, /* 64K bytes */ - .page_size = 0x00000100, /* 256 bytes */ + .start_offset = 0, /* start at the beginning of the chip */ + .sector_size = 0x00010000, /* 64K bytes */ + .page_size = 0x00000100, /* 256 bytes */ }; #include "pios_flash.h" -#endif /* PIOS_INCLUDE_FLASH */ +#endif /* PIOS_INCLUDE_FLASH */ /* * ADC system @@ -435,398 +444,399 @@ static const struct flashfs_logfs_cfg flashfs_m25p_cfg = { #if defined(PIOS_INCLUDE_ADC) #include "pios_adc_priv.h" extern void PIOS_ADC_handler(void); -void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); +void DMA1_Channel1_IRQHandler() __attribute__((alias("PIOS_ADC_handler"))); // Remap the ADC DMA handler to this one static const struct pios_adc_cfg pios_adc_cfg = { - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - .irq = { - .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), - .init = { - .NVIC_IRQChannel = DMA1_Channel1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA1_Channel1, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR, - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - } - }, - .half_flag = DMA1_IT_HT1, - .full_flag = DMA1_IT_TC1, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + .irq = { + .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), + .init = { + .NVIC_IRQChannel = DMA1_Channel1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA1_Channel1, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR, + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + } + }, + .half_flag = DMA1_IT_HT1, + .full_flag = DMA1_IT_TC1, }; -void PIOS_ADC_handler() { - PIOS_ADC_DMA_Handler(); +void PIOS_ADC_handler() +{ + PIOS_ADC_DMA_Handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_ADC) */ #include "pios_tim_priv.h" static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, }; static const struct pios_tim_clock_cfg tim_1_cfg = { - .timer = TIM1, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM1, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_2_cfg = { - .timer = TIM2, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM2, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_3_cfg = { - .timer = TIM3, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM3, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_4_cfg = { - .timer = TIM4, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM4, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, }; static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, }; static const struct pios_tim_channel pios_tim_servoport_rcvrport_pins[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - - // Receiver port pins - // S3-S6 inputs are used as outputs in this case - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + + // Receiver port pins + // S3-S6 inputs are used as outputs in this case + { + .timer = TIM3, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, }; static const struct pios_tim_channel pios_tim_ppm_flexi_port = { - .timer = TIM2, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap2_TIM2, + .timer = TIM2, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap2_TIM2, }; #if defined(PIOS_INCLUDE_USART) @@ -834,75 +844,75 @@ static const struct pios_tim_channel pios_tim_ppm_flexi_port = { #include "pios_usart_priv.h" static const struct pios_usart_cfg pios_usart_generic_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, + .regs = USART1, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, }; static const struct pios_usart_cfg pios_usart_generic_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, + .regs = USART3, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, }; #if defined(PIOS_INCLUDE_DSM) @@ -912,100 +922,100 @@ static const struct pios_usart_cfg pios_usart_generic_flexi_cfg = { #include static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, + .regs = USART1, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, }; static const struct pios_dsm_cfg pios_dsm_main_cfg = { - .bind = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, + .bind = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, }; static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, + .regs = USART3, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, }; static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, + .bind = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, }; -#endif /* PIOS_INCLUDE_DSM */ +#endif /* PIOS_INCLUDE_DSM */ #if defined(PIOS_INCLUDE_SBUS) @@ -1015,141 +1025,141 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { #include static const struct pios_usart_cfg pios_usart_sbus_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 100000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_Even, - .USART_StopBits = USART_StopBits_2, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, + .regs = USART1, + .init = { + .USART_BaudRate = 100000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_Even, + .USART_StopBits = USART_StopBits_2, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, }; static const struct pios_sbus_cfg pios_sbus_cfg = { - /* Inverter configuration */ - .inv = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .gpio_clk_func = RCC_APB2PeriphClockCmd, - .gpio_clk_periph = RCC_APB2Periph_GPIOB, - .gpio_inv_enable = Bit_SET, + /* Inverter configuration */ + .inv = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .gpio_clk_func = RCC_APB2PeriphClockCmd, + .gpio_clk_periph = RCC_APB2Periph_GPIOB, + .gpio_inv_enable = Bit_SET, }; -#endif /* PIOS_INCLUDE_SBUS */ +#endif /* PIOS_INCLUDE_SBUS */ /* * HK OSD */ static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, + .regs = USART1, .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, }, - }, }; static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, + .regs = USART3, .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, }, - }, }; -#endif /* PIOS_INCLUDE_USART */ +#endif /* PIOS_INCLUDE_USART */ #if defined(PIOS_INCLUDE_COM) #include "pios_com_priv.h" -#endif /* PIOS_INCLUDE_COM */ +#endif /* PIOS_INCLUDE_COM */ #if defined(PIOS_INCLUDE_RTC) /* @@ -1157,65 +1167,65 @@ static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { */ #include -void PIOS_RTC_IRQ_Handler (void); -void RTC_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler"))); +void PIOS_RTC_IRQ_Handler(void); +void RTC_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); static const struct pios_rtc_cfg pios_rtc_main_cfg = { - .clksrc = RCC_RTCCLKSource_HSE_Div128, - .prescaler = 100, - .irq = { - .init = { - .NVIC_IRQChannel = RTC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .clksrc = RCC_RTCCLKSource_HSE_Div128, + .prescaler = 100, + .irq = { + .init = { + .NVIC_IRQChannel = RTC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; -void PIOS_RTC_IRQ_Handler (void) +void PIOS_RTC_IRQ_Handler(void) { - PIOS_RTC_irq_handler (); + PIOS_RTC_irq_handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_RTC) */ #if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM) -/* - * Servo outputs +/* + * Servo outputs */ #include const struct pios_servo_cfg pios_servo_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_all_pins, - .num_channels = NELEMENTS(pios_tim_servoport_all_pins), + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_all_pins, + .num_channels = NELEMENTS(pios_tim_servoport_all_pins), }; const struct pios_servo_cfg pios_servo_rcvr_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_rcvrport_pins, - .num_channels = NELEMENTS(pios_tim_servoport_rcvrport_pins), + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_rcvrport_pins, + .num_channels = NELEMENTS(pios_tim_servoport_rcvrport_pins), }; -#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ +#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ /* * PPM Inputs @@ -1224,66 +1234,65 @@ const struct pios_servo_cfg pios_servo_rcvr_cfg = { #include const struct pios_ppm_cfg pios_ppm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - /* Use only the first channel for ppm */ - .channels = &pios_tim_rcvrport_all_channels[0], - .num_channels = 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_rcvrport_all_channels[0], + .num_channels = 1, }; -#endif /* PIOS_INCLUDE_PPM */ +#endif /* PIOS_INCLUDE_PPM */ #if defined(PIOS_INCLUDE_PPM_FLEXI) #include const struct pios_ppm_cfg pios_ppm_flexi_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = &pios_tim_ppm_flexi_port, - .num_channels = 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_ppm_flexi_port, + .num_channels = 1, }; -#endif /* PIOS_INCLUDE_PPM_FLEXI */ +#endif /* PIOS_INCLUDE_PPM_FLEXI */ -/* - * PWM Inputs +/* + * PWM Inputs */ #if defined(PIOS_INCLUDE_PWM) #include const struct pios_pwm_cfg pios_pwm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = pios_tim_rcvrport_all_channels, - .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_rcvrport_all_channels, + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), }; const struct pios_pwm_cfg pios_pwm_with_ppm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - /* Leave the first channel for PPM use and use the rest for PWM */ - .channels = &pios_tim_rcvrport_all_channels[1], - .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + /* Leave the first channel for PPM use and use the rest for PWM */ + .channels = &pios_tim_rcvrport_all_channels[1], + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1, }; -#endif - +#endif /* if defined(PIOS_INCLUDE_PWM) */ /* @@ -1293,40 +1302,40 @@ const struct pios_pwm_cfg pios_pwm_with_ppm_cfg = { #include static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = { -{ - .timer = TIM3, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, -}, + { + .timer = TIM3, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, }; const struct pios_hcsr04_cfg pios_hcsr04_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = pios_tim_hcsr04_port_all_channels, - .num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels), - .trigger = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_hcsr04_port_all_channels, + .num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels), + .trigger = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }; -#endif +#endif /* if defined(PIOS_INCLUDE_HCSR04) */ #if defined(PIOS_INCLUDE_I2C) @@ -1338,74 +1347,74 @@ const struct pios_hcsr04_cfg pios_hcsr04_cfg = { void PIOS_I2C_flexi_adapter_ev_irq_handler(void); void PIOS_I2C_flexi_adapter_er_irq_handler(void); -void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexi_adapter_ev_irq_handler"))); -void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexi_adapter_er_irq_handler"))); +void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexi_adapter_ev_irq_handler"))); +void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexi_adapter_er_irq_handler"))); static const struct pios_i2c_adapter_cfg pios_i2c_flexi_adapter_cfg = { - .regs = I2C2, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, + .regs = I2C2, .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, }, - }, }; uint32_t pios_i2c_flexi_adapter_id; void PIOS_I2C_flexi_adapter_ev_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexi_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexi_adapter_id); } void PIOS_I2C_flexi_adapter_er_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexi_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexi_adapter_id); } #endif /* PIOS_INCLUDE_I2C */ #if defined(PIOS_INCLUDE_GCSRCVR) #include "pios_gcsrcvr_priv.h" -#endif /* PIOS_INCLUDE_GCSRCVR */ +#endif /* PIOS_INCLUDE_GCSRCVR */ #if defined(PIOS_INCLUDE_RCVR) #include "pios_rcvr_priv.h" @@ -1416,50 +1425,50 @@ void PIOS_I2C_flexi_adapter_er_irq_handler(void) #include "pios_usb_priv.h" static const struct pios_usb_cfg pios_usb_main_cfg_cc = { - .irq = { - .init = { - .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, }, - .vsense_active_low = false + }, + .vsense = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .vsense_active_low = false }; static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = { - .irq = { - .init = { - .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, }, - .vsense_active_low = false + }, + .vsense = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .vsense_active_low = false }; #include "pios_usb_board_data_priv.h" #include "pios_usb_desc_hid_cdc_priv.h" #include "pios_usb_desc_hid_only_priv.h" -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ #if defined(PIOS_INCLUDE_COM_MSG) @@ -1471,9 +1480,9 @@ static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = { #include const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 2, - .data_rx_ep = 1, - .data_tx_ep = 1, + .data_if = 2, + .data_rx_ep = 1, + .data_tx_ep = 1, }; #endif /* PIOS_INCLUDE_USB_HID */ @@ -1482,22 +1491,21 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { #include const struct pios_usb_rctx_cfg pios_usb_rctx_cfg = { - .data_if = 2, - .data_tx_ep = 1, + .data_if = 2, + .data_tx_ep = 1, }; -#endif /* PIOS_INCLUDE_USB_RCTX */ +#endif /* PIOS_INCLUDE_USB_RCTX */ #if defined(PIOS_INCLUDE_USB_CDC) #include const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 0, - .ctrl_tx_ep = 2, + .ctrl_if = 0, + .ctrl_tx_ep = 2, - .data_if = 1, - .data_rx_ep = 3, - .data_tx_ep = 3, + .data_if = 1, + .data_rx_ep = 3, + .data_tx_ep = 3, }; -#endif /* PIOS_INCLUDE_USB_CDC */ - +#endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/targets/boards/coptercontrol/bootloader/inc/common.h b/flight/targets/boards/coptercontrol/bootloader/inc/common.h index 9010fddd9..e64b89014 100644 --- a/flight/targets/boards/coptercontrol/bootloader/inc/common.h +++ b/flight/targets/boards/coptercontrol/bootloader/inc/common.h @@ -28,10 +28,10 @@ #ifndef COMMON_H_ #define COMMON_H_ -//#include "board.h" +// #include "board.h" typedef enum { - start, keepgoing, + start, keepgoing, } DownloadAction; /**************************************************/ @@ -39,77 +39,77 @@ typedef enum { /**************************************************/ typedef enum { - DFUidle, //0 - uploading, //1 - wrong_packet_received, //2 - too_many_packets, //3 - too_few_packets, //4 - Last_operation_Success, //5 - downloading, //6 - BLidle, //7 - Last_operation_failed, //8 - uploadingStarting, //9 - outsideDevCapabilities, //10 - CRC_Fail,//11 - failed_jump, -//12 + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + BLidle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, +// 12 } DFUStates; /**************************************************/ /* OP_DFU commands */ /**************************************************/ typedef enum { - Reserved, //0 - Req_Capabilities, //1 - Rep_Capabilities, //2 - EnterDFU, //3 - JumpFW, //4 - Reset, //5 - Abort_Operation, //6 - Upload, //7 - Op_END, //8 - Download_Req, //9 - Download, //10 - Status_Request, //11 - Status_Rep -//12 + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep +// 12 } DFUCommands; typedef enum { - High_Density, Medium_Density + High_Density, Medium_Density } DeviceType; /**************************************************/ /* OP_DFU transfer types */ /**************************************************/ typedef enum { - FW, //0 - Descript -//2 + FW, // 0 + Descript +// 2 } DFUTransfer; /**************************************************/ /* OP_DFU transfer port */ /**************************************************/ typedef enum { - Usb, //0 - Serial -//2 + Usb, // 0 + Serial +// 2 } DFUPort; /**************************************************/ /* OP_DFU programable programable HW types */ /**************************************************/ typedef enum { - Self_flash, //0 - Remote_flash_via_spi -//1 + Self_flash, // 0 + Remote_flash_via_spi +// 1 } DFUProgType; /**************************************************/ /* OP_DFU programable sources */ /**************************************************/ -#define USB 0 -#define SPI 1 +#define USB 0 +#define SPI 1 -#define DownloadDelay 100000 +#define DownloadDelay 100000 -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 +#define MAX_DEL_RETRYS 3 +#define MAX_WRI_RETRYS 3 #endif /* COMMON_H_ */ diff --git a/flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h b/flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h index d5e32c891..43182e435 100644 --- a/flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h @@ -3,28 +3,28 @@ * @addtogroup CopterControlBL CopterControl BootLoader * @brief These files contain the code to the CopterControl Bootloader. * @{ - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * In particular, pios_config.h is where you define which PiOS libraries * and features are included in the firmware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h index 47d7d8255..6be4c4b70 100644 --- a/flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h +++ b/flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h @@ -33,13 +33,13 @@ #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 2 +#define PIOS_USB_BOARD_EP_NUM 2 -#include /* struct usb_* */ +#include /* struct usb_* */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_BL) -#define PIOS_USB_BOARD_SN_SUFFIX "+BL" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" /* * The bootloader uses a simplified report structure @@ -49,4 +49,4 @@ */ #define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/coptercontrol/bootloader/main.c b/flight/targets/boards/coptercontrol/bootloader/main.c index d603ea145..4c30b51f4 100644 --- a/flight/targets/boards/coptercontrol/bootloader/main.c +++ b/flight/targets/boards/coptercontrol/bootloader/main.c @@ -8,21 +8,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief This is the file with the main function of the OpenPilot BootLoader * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -59,10 +59,10 @@ uint8_t tempcount = 0; /* Extern variables ----------------------------------------------------------*/ DFUStates DeviceState; -int16_t status = 0; -uint8_t JumpToApp = FALSE; -uint8_t GO_dfu = FALSE; -uint8_t USB_connected = FALSE; +int16_t status = 0; +uint8_t JumpToApp = FALSE; +uint8_t GO_dfu = FALSE; +uint8_t USB_connected = FALSE; uint8_t User_DFU_request = FALSE; static uint8_t mReceive_Buffer[63]; /* Private function prototypes -----------------------------------------------*/ @@ -70,136 +70,149 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); uint8_t processRX(); void jump_to_app(); -int main() { - PIOS_SYS_Init(); - PIOS_Board_Init(); - PIOS_IAP_Init(); - - USB_connected = PIOS_USB_CableConnected(0); +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); - if (PIOS_IAP_CheckRequest() == TRUE) { - PIOS_DELAY_WaitmS(1000); - User_DFU_request = TRUE; - PIOS_IAP_ClearRequest(); - } + USB_connected = PIOS_USB_CableConnected(0); - GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); + if (PIOS_IAP_CheckRequest() == TRUE) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = TRUE; + PIOS_IAP_ClearRequest(); + } - if (GO_dfu == TRUE) { - PIOS_Board_Init(); - if (User_DFU_request == TRUE) - DeviceState = DFUidle; - else - DeviceState = BLidle; - } else - JumpToApp = TRUE; + GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); - uint32_t stopwatch = 0; - uint32_t prev_ticks = PIOS_DELAY_GetuS(); - while (TRUE) { - /* Update the stopwatch */ - uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); - prev_ticks += elapsed_ticks; - stopwatch += elapsed_ticks; + if (GO_dfu == TRUE) { + PIOS_Board_Init(); + if (User_DFU_request == TRUE) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = TRUE; + } - if (JumpToApp == TRUE) - jump_to_app(); + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (TRUE) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; - switch (DeviceState) { - case Last_operation_Success: - case uploadingStarting: - case DFUidle: - period1 = 5000; - sweep_steps1 = 100; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case uploading: - period1 = 5000; - sweep_steps1 = 100; - period2 = 2500; - sweep_steps2 = 50; - break; - case downloading: - period1 = 2500; - sweep_steps1 = 50; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case BLidle: - period1 = 0; - PIOS_LED_On(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - default://error - period1 = 5000; - sweep_steps1 = 100; - period2 = 5000; - sweep_steps2 = 100; - } + if (JumpToApp == TRUE) { + jump_to_app(); + } - if (period1 != 0) { - if (LedPWM(period1, sweep_steps1, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_On(PIOS_LED_HEARTBEAT); + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } - if (period2 != 0) { - if (LedPWM(period2, sweep_steps2, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } - if (stopwatch > 50 * 1000 * 1000) - stopwatch = 0; - if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) - JumpToApp = TRUE; + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } - processRX(); - DataDownload(start); - } + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) { + JumpToApp = TRUE; + } + + processRX(); + DataDownload(start); + } } -void jump_to_app() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; - if (((*(__IO uint32_t*) bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */ - FLASH_Lock(); - RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - _SetCNTR(0); // clear interrupt mask - _SetISTR(0); // clear all requests - JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); - Jump_To_Application = (pFunction) JumpAddress; - /* Initialize user application's Stack Pointer */ - __set_MSP(*(__IO uint32_t*) bdinfo->fw_base); - Jump_To_Application(); - } else { - DeviceState = failed_jump; - return; - } + if (((*(__IO uint32_t *)bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */ + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + _SetCNTR(0); // clear interrupt mask + _SetISTR(0); // clear all requests + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + return; + } } -uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { - uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ - uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ - uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ - if (curr_sweep & 1) { - pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ - } - return ((count % pwm_period) > pwm_duty) ? 1 : 0; + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; } -uint8_t processRX() { - if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { - processComand(mReceive_Buffer); - } - return TRUE; +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return TRUE; } - diff --git a/flight/targets/boards/coptercontrol/bootloader/pios_board.c b/flight/targets/boards/coptercontrol/bootloader/pios_board.c index e24a08c75..bd437df42 100644 --- a/flight/targets/boards/coptercontrol/bootloader/pios_board.c +++ b/flight/targets/boards/coptercontrol/bootloader/pios_board.c @@ -44,66 +44,66 @@ uint32_t pios_com_telem_usb_id; * called from System/openpilot.c */ static bool board_init_complete = false; -void PIOS_Board_Init(void) { - if (board_init_complete) { - return; - } +void PIOS_Board_Init(void) +{ + if (board_init_complete) { + return; + } - /* Enable Prefetch Buffer */ - FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); + /* Enable Prefetch Buffer */ + FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); - /* Flash 2 wait state */ - FLASH_SetLatency(FLASH_Latency_2); + /* Flash 2 wait state */ + FLASH_SetLatency(FLASH_Latency_2); - /* Delay system */ - PIOS_DELAY_Init(); + /* Delay system */ + PIOS_DELAY_Init(); - /* Initialize the PiOS library */ - PIOS_GPIO_Init(); + /* Initialize the PiOS library */ + PIOS_GPIO_Init(); + + const struct pios_board_info *bdinfo = &pios_board_info_blob; - const struct pios_board_info * bdinfo = &pios_board_info_blob; - #if defined(PIOS_INCLUDE_LED) - const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); - PIOS_Assert(led_cfg); - PIOS_LED_Init(led_cfg); -#endif /* PIOS_INCLUDE_LED */ + const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Activate the HID-only USB configuration */ - PIOS_USB_DESC_HID_ONLY_Init(); + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); - uint32_t pios_usb_id; - switch(bdinfo->board_rev) { - case BOARD_REVISION_CC: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); - break; - case BOARD_REVISION_CC3D: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); - break; - default: - PIOS_Assert(0); - } + uint32_t pios_usb_id; + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); + break; + case BOARD_REVISION_CC3D: + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); + break; + default: + PIOS_Assert(0); + } #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { - PIOS_Assert(0); - } -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); // TODO Tirar - board_init_complete = true; + board_init_complete = true; } void PIOS_ADC_DMA_Handler() -{ -} +{} diff --git a/flight/targets/boards/coptercontrol/firmware/coptercontrol.c b/flight/targets/boards/coptercontrol/firmware/coptercontrol.c index 3a7cdeac0..e748f6317 100644 --- a/flight/targets/boards/coptercontrol/firmware/coptercontrol.c +++ b/flight/targets/boards/coptercontrol/firmware/coptercontrol.c @@ -9,25 +9,25 @@ * @brief This is where the OP firmware starts. Those files also define the compile-time * options of the firmware. * @{ - * @file openpilot.c + * @file openpilot.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Sets up and runs main OpenPilot tasks. * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,7 +36,7 @@ #include /* Task Priorities */ -#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) /* Global Variables */ @@ -45,73 +45,73 @@ extern void PIOS_Board_Init(void); extern void Stack_Change(void); /** -* OpenPilot Main function: -* -* Initialize PiOS
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
-* Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) -* If something goes wrong, blink LED1 and LED2 every 100ms -* -*/ + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) + * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ int main() { - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ - /* Brings up System using CMSIS functions, enables the LEDs. */ - PIOS_SYS_Init(); + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); - /* Architecture dependant Hardware and - * core subsystem initialisation - * (see pios_board.c for your arch) - * */ - PIOS_Board_Init(); + /* Architecture dependant Hardware and + * core subsystem initialisation + * (see pios_board.c for your arch) + * */ + PIOS_Board_Init(); #ifdef ERASE_FLASH - PIOS_Flash_Jedec_EraseChip(); + PIOS_Flash_Jedec_EraseChip(); #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Off(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ - while (1) ; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + while (1) { + ; + } #endif - /* Initialize modules */ - MODULE_INITIALISE_ALL + /* Initialize modules */ + MODULE_INITIALISE_ALL + /* swap the stack to use the IRQ stack */ + Stack_Change(); - /* swap the stack to use the IRQ stack */ - Stack_Change(); + /* Start the FreeRTOS scheduler, which should never return. + * + * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls + * (schedules) function files (modules). These functions never return from their + * while loops, which explains why each module has a while(1){} segment. Thus, + * the OpenPilot software actually starts at the vTaskStartScheduler() function, + * even though this is somewhat obscure. + * + * In addition, there are many main() functions in the OpenPilot firmware source tree + * This is because each main() refers to a separate hardware platform. Of course, + * C only allows one main(), so only the relevant main() function is compiled when + * making a specific firmware. + * + */ + vTaskStartScheduler(); - /* Start the FreeRTOS scheduler, which should never return. - * - * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls - * (schedules) function files (modules). These functions never return from their - * while loops, which explains why each module has a while(1){} segment. Thus, - * the OpenPilot software actually starts at the vTaskStartScheduler() function, - * even though this is somewhat obscure. - * - * In addition, there are many main() functions in the OpenPilot firmware source tree - * This is because each main() refers to a separate hardware platform. Of course, - * C only allows one main(), so only the relevant main() function is compiled when - * making a specific firmware. - * - */ - vTaskStartScheduler(); + /* If all is well we will never reach here as the scheduler will now be running. */ - /* If all is well we will never reach here as the scheduler will now be running. */ - - /* Do some indication to user that something bad just happened */ - while (1) { + /* Do some indication to user that something bad just happened */ + while (1) { #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ - PIOS_DELAY_WaitmS(100); - } + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + PIOS_DELAY_WaitmS(100); + } - return 0; + return 0; } /** * @} * @} */ - diff --git a/flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h index 0ed32c74d..fb0a70d05 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h @@ -1,75 +1,74 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ /** - * @addtogroup PIOS PIOS - * @{ - * @addtogroup FreeRTOS FreeRTOS - * @{ - */ + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ /* Notes: We use 5 task priorities */ -#define configUSE_PREEMPTION 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configUSE_MALLOC_FAILED_HOOK 1 -#define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 ) -#define configTICK_RATE_HZ ( ( portTickType ) 1000 ) -#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) -#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 48 ) -#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 53 * 256) ) -#define configMAX_TASK_NAME_LEN ( 16 ) -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configIDLE_SHOULD_YIELD 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_MALLOC_FAILED_HOOK 1 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)48) +#define configTOTAL_HEAP_SIZE ((size_t)(53 * 256)) +#define configMAX_TASK_NAME_LEN (16) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configQUEUE_REGISTRY_SIZE 10 /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ + to exclude the API function. */ -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 0 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) #define CHECK_IRQ_STACK @@ -77,22 +76,23 @@ NVIC value of 255. */ /* Enable run time stats collection */ #ifdef DIAG_TASKS -#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 -#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\ -do {\ -(*(unsigned long *)0xe000edfc) |= (1<<24);/* DEMCR |= DEMCR_TRCENA */\ -(*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\ -} while(0) -#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */ +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ #else -#define configCHECK_FOR_STACK_OVERFLOW 1 +#define configCHECK_FOR_STACK_OVERFLOW 1 #endif /** - * @} - */ + * @} + */ #endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h b/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h index 0dc13c2a8..963014e21 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h @@ -10,19 +10,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h index 84eadbeba..16364d7fa 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file pios_board.h + * @file pios_board.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -28,57 +28,54 @@ #define PIOS_BOARD_H - - -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -//#define PIOS_LED_LED1_GPIO_PORT GPIOC -//#define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12 -//#define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC -//#define PIOS_LED_LED2_GPIO_PORT GPIOC -//#define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13 -//#define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC -#define PIOS_LED_NUM 2 -//#define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT } -//#define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN } -//#define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK } +// ------------------------ +// #define PIOS_LED_LED1_GPIO_PORT GPIOC +// #define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12 +// #define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC +// #define PIOS_LED_LED2_GPIO_PORT GPIOC +// #define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13 +// #define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC +#define PIOS_LED_NUM 2 +// #define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT } +// #define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN } +// #define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK } -//------------------------- +// ------------------------- // COM // // See also pios_board_posix.c -//------------------------- -//#define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +// ------------------------- +// #define PIOS_USART_TX_BUFFER_SIZE 256 +#define PIOS_COM_BUFFER_SIZE 1024 +#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_COM_TELEM_RF 0 -#define PIOS_COM_GPS 1 -#define PIOS_COM_TELEM_USB 2 +#define PIOS_COM_TELEM_RF 0 +#define PIOS_COM_GPS 1 +#define PIOS_COM_TELEM_USB 2 #ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX 3 -#define PIOS_COM_DEBUG PIOS_COM_AUX +#define PIOS_COM_AUX 3 +#define PIOS_COM_DEBUG PIOS_COM_AUX #endif /** * glue macros for file IO * STM32 uses DOSFS for file IO */ -#define PIOS_FOPEN_READ(filename,file) (file=fopen((char*)filename,"r"))==NULL +#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL -#define PIOS_FOPEN_WRITE(filename,file) (file=fopen((char*)filename,"w"))==NULL +#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL -#define PIOS_FREAD(file,bufferadr,length,resultadr) (*resultadr=fread((uint8_t*)bufferadr,1,length,*file)) != length +#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length -#define PIOS_FWRITE(file,bufferadr,length,resultadr) *resultadr=fwrite((uint8_t*)bufferadr,1,length,*file) +#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) +#define PIOS_FCLOSE(file) fclose(file) -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char*)filename) +#define PIOS_FUNLINK(file) unlink((char *)filename) #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index f35efa071..7d53b585e 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -142,26 +142,26 @@ /* #define PIOS_QUATERNION_STABILIZATION */ /* Performance counters */ -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 /* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 220 -#define HEAP_LIMIT_CRITICAL 40 -#define IRQSTACK_LIMIT_WARNING 100 -#define IRQSTACK_LIMIT_CRITICAL 60 -#define CPULOAD_LIMIT_WARNING 85 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 220 +#define HEAP_LIMIT_CRITICAL 40 +#define IRQSTACK_LIMIT_WARNING 100 +#define IRQSTACK_LIMIT_CRITICAL 60 +#define CPULOAD_LIMIT_WARNING 85 +#define CPULOAD_LIMIT_CRITICAL 95 /* Task stack sizes */ -#define PIOS_ACTUATOR_STACK_SIZE 1020 -#define PIOS_MANUAL_STACK_SIZE 800 -#define PIOS_SYSTEM_STACK_SIZE 660 -#define PIOS_STABILIZATION_STACK_SIZE 524 -#define PIOS_TELEM_STACK_SIZE 500 -#define PIOS_EVENTDISPATCHER_STACK_SIZE 130 +#define PIOS_ACTUATOR_STACK_SIZE 1020 +#define PIOS_MANUAL_STACK_SIZE 800 +#define PIOS_SYSTEM_STACK_SIZE 660 +#define PIOS_STABILIZATION_STACK_SIZE 524 +#define PIOS_TELEM_STACK_SIZE 500 +#define PIOS_EVENTDISPATCHER_STACK_SIZE 130 /* This can't be too high to stop eventdispatcher thread overflowing */ -#define PIOS_EVENTDISAPTCHER_QUEUE 10 +#define PIOS_EVENTDISAPTCHER_QUEUE 10 /* Revolution series */ /* #define REVOLUTION */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h index 49f94d7f0..eda0a2511 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,7 +41,7 @@ /* Defaults for Logging */ -#define LOG_FILENAME "PIOS.LOG" -#define STARTUP_LOG_ENABLED 1 +#define LOG_FILENAME "PIOS.LOG" +#define STARTUP_LOG_ENABLED 1 #endif /* PIOS_CONFIG_POSIX_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h index 63439dcbb..248ef57d7 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h @@ -35,12 +35,12 @@ #define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 4 +#define PIOS_USB_BOARD_EP_NUM 4 -#include /* USB_* macros */ +#include /* USB_* macros */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_FW) -#define PIOS_USB_BOARD_SN_SUFFIX "+FW" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index d4311d311..4ac37f26b 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -9,19 +9,19 @@ * @{ * @brief Defines board specific static initializers for hardware for the CopterControl board. *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -49,23 +49,23 @@ */ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 32 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 12 +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 32 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 12 -#define PIOS_COM_GPS_RX_BUF_LEN 32 +#define PIOS_COM_GPS_RX_BUF_LEN 32 -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 -#define PIOS_COM_HKOSD_TX_BUF_LEN 22 +#define PIOS_COM_HKOSD_TX_BUF_LEN 22 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) #define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 uint32_t pios_com_debug_id; -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ uint32_t pios_com_telem_rf_id; uint32_t pios_com_telem_usb_id; @@ -85,49 +85,49 @@ uintptr_t pios_uavo_settings_fs_id; #include "pios_mpu6000.h" #include "pios_mpu6000_config.h" static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { - .vector = PIOS_MPU6000_IRQHandler, - .line = EXTI_Line3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line3, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_MPU6000_IRQHandler, + .line = EXTI_Line3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line3, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { - .exti_cfg = &pios_exti_mpu6000_cfg, - .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, - // Clock at 8 khz, downsampled by 16 for 500 Hz - .Smpl_rate_div_no_dlp = 15, - // Clock at 1 khz, downsampled by 2 for 500 Hz - .Smpl_rate_div_dlp = 1, - .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, - .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, - .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, - .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, - .accel_range = PIOS_MPU6000_ACCEL_8G, - .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ, - .orientation = PIOS_MPU6000_TOP_180DEG + .exti_cfg = &pios_exti_mpu6000_cfg, + .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, + // Clock at 8 khz, downsampled by 16 for 500 Hz + .Smpl_rate_div_no_dlp = 15, + // Clock at 1 khz, downsampled by 2 for 500 Hz + .Smpl_rate_div_dlp = 1, + .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, + .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, + .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_TOP_180DEG }; #endif /* PIOS_INCLUDE_MPU6000 */ @@ -137,740 +137,738 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { * called from System/openpilot.c */ int32_t init_test; -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); + const struct pios_board_info *bdinfo = &pios_board_info_blob; - const struct pios_board_info * bdinfo = &pios_board_info_blob; - #if defined(PIOS_INCLUDE_LED) - const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); - PIOS_Assert(led_cfg); - PIOS_LED_Init(led_cfg); -#endif /* PIOS_INCLUDE_LED */ + const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_SPI) - /* Set up the SPI interface to the serial flash */ + /* Set up the SPI interface to the serial flash */ - switch(bdinfo->board_rev) { - case BOARD_REVISION_CC: - if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) { - PIOS_Assert(0); - } - break; - case BOARD_REVISION_CC3D: - if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) { - PIOS_Assert(0); - } - break; - default: - PIOS_Assert(0); - } + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) { + PIOS_Assert(0); + } + break; + case BOARD_REVISION_CC3D: + if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) { + PIOS_Assert(0); + } + break; + default: + PIOS_Assert(0); + } #endif - uintptr_t flash_id; - switch(bdinfo->board_rev) { - case BOARD_REVISION_CC: - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 1)) { - PIOS_DEBUG_Assert(0); - } - if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_w25x_cfg, &pios_jedec_flash_driver, flash_id)) { - PIOS_DEBUG_Assert(0); - } - break; - case BOARD_REVISION_CC3D: - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 0)) { - PIOS_DEBUG_Assert(0); - } - if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id)) { - PIOS_DEBUG_Assert(0); - } - break; - default: - PIOS_DEBUG_Assert(0); - } + uintptr_t flash_id; + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 1)) { + PIOS_DEBUG_Assert(0); + } + if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_w25x_cfg, &pios_jedec_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } + break; + case BOARD_REVISION_CC3D: + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 0)) { + PIOS_DEBUG_Assert(0); + } + if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } + break; + default: + PIOS_DEBUG_Assert(0); + } - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); #if defined(PIOS_INCLUDE_RTC) - /* Initialize the real-time clock and its associated tick */ - PIOS_RTC_Init(&pios_rtc_main_cfg); + /* Initialize the real-time clock and its associated tick */ + PIOS_RTC_Init(&pios_rtc_main_cfg); #endif - PIOS_IAP_Init(); - // check for safe mode commands from gcs - if(PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && - PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && - PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) - { - PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); - PIOS_IAP_WriteBootCmd(0,0); - PIOS_IAP_WriteBootCmd(1,0); - PIOS_IAP_WriteBootCmd(2,0); - } - - HwSettingsInitialize(); + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } + + HwSettingsInitialize(); #ifndef ERASE_FLASH - /* Initialize watchdog as early as possible to catch faults during init */ - PIOS_WDG_Init(); + /* Initialize watchdog as early as possible to catch faults during init */ + PIOS_WDG_Init(); #endif - /* Initialize the alarms library */ - AlarmsInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Check for repeated boot failures */ - uint16_t boot_count = PIOS_IAP_ReadBootCount(); - if (boot_count < 3) { - PIOS_IAP_WriteBootCount(++boot_count); - AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); - } else { - /* Too many failed boot attempts, force hwsettings to defaults */ - HwSettingsSetDefaults(HwSettingsHandle(), 0); - AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); - } + /* Check for repeated boot failures */ + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); - /* Set up pulse timers */ - PIOS_TIM_InitClock(&tim_1_cfg); - PIOS_TIM_InitClock(&tim_2_cfg); - PIOS_TIM_InitClock(&tim_3_cfg); - PIOS_TIM_InitClock(&tim_4_cfg); + /* Set up pulse timers */ + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_4_cfg); #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; + /* Flags to determine if various USB interfaces are advertised */ + bool usb_hid_present = false; + bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; + usb_cdc_present = true; #else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; #endif - uint32_t pios_usb_id; - - switch(bdinfo->board_rev) { - case BOARD_REVISION_CC: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); - break; - case BOARD_REVISION_CC3D: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); - break; - default: - PIOS_Assert(0); - } + uint32_t pios_usb_id; + + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); + break; + case BOARD_REVISION_CC3D: + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); + break; + default: + PIOS_Assert(0); + } #if defined(PIOS_INCLUDE_USB_CDC) - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); + uint8_t hwsettings_usb_vcpport; + /* Configure the USB VCP port */ + HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } + if (!usb_cdc_present) { + /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ + hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; + } - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: + switch (hwsettings_usb_vcpport) { + case HWSETTINGS_USB_VCPPORT_DISABLED: + break; + case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: + { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: + { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, + tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ + { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + NULL, 0, + tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_COM */ + break; + } +#endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); + /* Configure the usb HID port */ + uint8_t hwsettings_usb_hidport; + HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } + if (!usb_hid_present) { + /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ + hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; + } - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: + switch (hwsettings_usb_hidport) { + case HWSETTINGS_USB_HIDPORT_DISABLED: + break; + case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER: + { + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER: #if defined(PIOS_INCLUDE_USB_RCTX) - { - if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_USB_RCTX */ - break; - } + { + if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_USB_RCTX */ + break; + } -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ - /* Configure the main IO port */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); - uint8_t hwsettings_cc_mainport; - HwSettingsCC_MainPortGet(&hwsettings_cc_mainport); + /* Configure the main IO port */ + uint8_t hwsettings_DSMxBind; + HwSettingsDSMxBindGet(&hwsettings_DSMxBind); + uint8_t hwsettings_cc_mainport; + HwSettingsCC_MainPortGet(&hwsettings_cc_mainport); - switch (hwsettings_cc_mainport) { - case HWSETTINGS_CC_MAINPORT_DISABLED: - break; - case HWSETTINGS_CC_MAINPORT_TELEMETRY: + switch (hwsettings_cc_mainport) { + case HWSETTINGS_CC_MAINPORT_DISABLED: + break; + case HWSETTINGS_CC_MAINPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { - PIOS_Assert(0); - } + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { + PIOS_Assert(0); + } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_TELEMETRY_RF */ - break; - case HWSETTINGS_CC_MAINPORT_SBUS: -#if defined(PIOS_INCLUDE_SBUS) - { - uint32_t pios_usart_sbus_id; - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_id; - if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; - - } -#endif /* PIOS_INCLUDE_SBUS */ - break; - case HWSETTINGS_CC_MAINPORT_GPS: -#if defined(PIOS_INCLUDE_GPS) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { - PIOS_Assert(0); - } - - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_GPS */ - break; - case HWSETTINGS_CC_MAINPORT_DSM2: - case HWSETTINGS_CC_MAINPORT_DSMX10BIT: - case HWSETTINGS_CC_MAINPORT_DSMX11BIT: -#if defined(PIOS_INCLUDE_DSM) - { - enum pios_dsm_proto proto; - switch (hwsettings_cc_mainport) { - case HWSETTINGS_CC_MAINPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_CC_MAINPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_CC_MAINPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } - - uint32_t pios_usart_dsm_id; - if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, - &pios_dsm_main_cfg, - &pios_usart_com_driver, - pios_usart_dsm_id, - proto, 0)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT] = pios_dsm_rcvr_id; - } -#endif /* PIOS_INCLUDE_DSM */ - break; - case HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { - PIOS_Assert(0); - } - - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_CC_MAINPORT_COMBRIDGE: - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { - PIOS_Assert(0); - } - - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } - break; - case HWSETTINGS_CC_MAINPORT_OSDHK: - { - uint32_t pios_usart_hkosd_id; - if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_main_cfg)) { - PIOS_Assert(0); - } - - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_HKOSD_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id, - NULL, 0, - tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } - break; - } - - /* Configure the flexi port */ - uint8_t hwsettings_cc_flexiport; - HwSettingsCC_FlexiPortGet(&hwsettings_cc_flexiport); - - switch (hwsettings_cc_flexiport) { - case HWSETTINGS_CC_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_CC_FLEXIPORT_TELEMETRY: -#if defined(PIOS_INCLUDE_TELEMETRY_RF) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ - break; - case HWSETTINGS_CC_FLEXIPORT_COMBRIDGE: - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { - PIOS_Assert(0); - } + break; + case HWSETTINGS_CC_MAINPORT_SBUS: +#if defined(PIOS_INCLUDE_SBUS) + { + uint32_t pios_usart_sbus_id; + if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { + PIOS_Assert(0); + } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } - break; - case HWSETTINGS_CC_FLEXIPORT_GPS: + uint32_t pios_sbus_id; + if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { + PIOS_Assert(0); + } + + uint32_t pios_sbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; + } +#endif /* PIOS_INCLUDE_SBUS */ + break; + case HWSETTINGS_CC_MAINPORT_GPS: #if defined(PIOS_INCLUDE_GPS) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_GPS */ - break; - case HWSETTINGS_CC_FLEXIPORT_PPM: -#if defined(PIOS_INCLUDE_PPM_FLEXI) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { + PIOS_Assert(0); + } - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PPM_FLEXI */ - break; - case HWSETTINGS_CC_FLEXIPORT_DSM2: - case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT: - case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT: + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_GPS */ + break; + case HWSETTINGS_CC_MAINPORT_DSM2: + case HWSETTINGS_CC_MAINPORT_DSMX10BIT: + case HWSETTINGS_CC_MAINPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) - { - enum pios_dsm_proto proto; - switch (hwsettings_cc_flexiport) { - case HWSETTINGS_CC_FLEXIPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } + { + enum pios_dsm_proto proto; + switch (hwsettings_cc_mainport) { + case HWSETTINGS_CC_MAINPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_CC_MAINPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_CC_MAINPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } - uint32_t pios_usart_dsm_id; - if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_flexi_cfg)) { - PIOS_Assert(0); - } + uint32_t pios_usart_dsm_id; + if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_main_cfg)) { + PIOS_Assert(0); + } - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, - &pios_dsm_flexi_cfg, - &pios_usart_com_driver, - pios_usart_dsm_id, - proto, hwsettings_DSMxBind)) { - PIOS_Assert(0); - } + uint32_t pios_dsm_id; + if (PIOS_DSM_Init(&pios_dsm_id, + &pios_dsm_main_cfg, + &pios_usart_com_driver, + pios_usart_dsm_id, + proto, 0)) { + PIOS_Assert(0); + } - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT] = pios_dsm_rcvr_id; - } -#endif /* PIOS_INCLUDE_DSM */ - break; - case HWSETTINGS_CC_FLEXIPORT_DEBUGCONSOLE: + uint32_t pios_dsm_rcvr_id; + if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT] = pios_dsm_rcvr_id; + } +#endif /* PIOS_INCLUDE_DSM */ + break; + case HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint32_t pios_usart_generic_id; - if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { - PIOS_Assert(0); - } + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { + PIOS_Assert(0); + } - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_CC_FLEXIPORT_I2C: + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id, + NULL, 0, + tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_CC_MAINPORT_COMBRIDGE: + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, + tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + break; + case HWSETTINGS_CC_MAINPORT_OSDHK: + { + uint32_t pios_usart_hkosd_id; + if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_main_cfg)) { + PIOS_Assert(0); + } + + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_HKOSD_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id, + NULL, 0, + tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + break; + } + + /* Configure the flexi port */ + uint8_t hwsettings_cc_flexiport; + HwSettingsCC_FlexiPortGet(&hwsettings_cc_flexiport); + + switch (hwsettings_cc_flexiport) { + case HWSETTINGS_CC_FLEXIPORT_DISABLED: + break; + case HWSETTINGS_CC_FLEXIPORT_TELEMETRY: +#if defined(PIOS_INCLUDE_TELEMETRY_RF) + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_TELEMETRY_RF */ + break; + case HWSETTINGS_CC_FLEXIPORT_COMBRIDGE: + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, + tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + break; + case HWSETTINGS_CC_FLEXIPORT_GPS: +#if defined(PIOS_INCLUDE_GPS) + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_GPS */ + break; + case HWSETTINGS_CC_FLEXIPORT_PPM: +#if defined(PIOS_INCLUDE_PPM_FLEXI) + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); + + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PPM_FLEXI */ + break; + case HWSETTINGS_CC_FLEXIPORT_DSM2: + case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT: + case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT: +#if defined(PIOS_INCLUDE_DSM) + { + enum pios_dsm_proto proto; + switch (hwsettings_cc_flexiport) { + case HWSETTINGS_CC_FLEXIPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } + + uint32_t pios_usart_dsm_id; + if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_flexi_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_id; + if (PIOS_DSM_Init(&pios_dsm_id, + &pios_dsm_flexi_cfg, + &pios_usart_com_driver, + pios_usart_dsm_id, + proto, hwsettings_DSMxBind)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_rcvr_id; + if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT] = pios_dsm_rcvr_id; + } +#endif /* PIOS_INCLUDE_DSM */ + break; + case HWSETTINGS_CC_FLEXIPORT_DEBUGCONSOLE: +#if defined(PIOS_INCLUDE_COM) +#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { + PIOS_Assert(0); + } + + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id, + NULL, 0, + tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_CC_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) - { - if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_I2C */ - break; - case HWSETTINGS_CC_FLEXIPORT_OSDHK: - { - uint32_t pios_usart_hkosd_id; - if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_flexi_cfg)) { - PIOS_Assert(0); - } + { + if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_I2C */ + break; + case HWSETTINGS_CC_FLEXIPORT_OSDHK: + { + uint32_t pios_usart_hkosd_id; + if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_flexi_cfg)) { + PIOS_Assert(0); + } - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_HKOSD_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id, - NULL, 0, - tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } - break; - } + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_HKOSD_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id, + NULL, 0, + tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + break; + } - /* Configure the rcvr port */ - uint8_t hwsettings_rcvrport; - HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport); + /* Configure the rcvr port */ + uint8_t hwsettings_rcvrport; + HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport); - switch (hwsettings_rcvrport) { - case HWSETTINGS_CC_RCVRPORT_DISABLED: + switch (hwsettings_rcvrport) { + case HWSETTINGS_CC_RCVRPORT_DISABLED: #if defined(PIOS_INCLUDE_HCSR04) - { - uint32_t pios_hcsr04_id; - PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg); - } + { + uint32_t pios_hcsr04_id; + PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg); + } #endif - break; - case HWSETTINGS_CC_RCVRPORT_PWM: + break; + case HWSETTINGS_CC_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) - { - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); + { + uint32_t pios_pwm_id; + PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PWM */ - break; - case HWSETTINGS_CC_RCVRPORT_PPM: - case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS: + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PWM */ + break; + case HWSETTINGS_CC_RCVRPORT_PPM: + case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS: #if defined(PIOS_INCLUDE_PPM) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PPM */ - break; - case HWSETTINGS_CC_RCVRPORT_PPMPWM: - /* This is a combination of PPM and PWM inputs */ + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PPM */ + break; + case HWSETTINGS_CC_RCVRPORT_PPMPWM: + /* This is a combination of PPM and PWM inputs */ #if defined(PIOS_INCLUDE_PPM) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PPM */ + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PPM */ #if defined(PIOS_INCLUDE_PWM) - { - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg); + { + uint32_t pios_pwm_id; + PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg); - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PWM */ - break; - } + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PWM */ + break; + } #if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ + GCSReceiverInitialize(); + uint32_t pios_gcsrcvr_id; + PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_GCSRCVR */ - /* Remap AFIO pin for PB4 (Servo 5 Out)*/ - GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); + /* Remap AFIO pin for PB4 (Servo 5 Out)*/ + GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE); #ifndef PIOS_ENABLE_DEBUG_PINS - switch (hwsettings_rcvrport) { - case HWSETTINGS_CC_RCVRPORT_DISABLED: - case HWSETTINGS_CC_RCVRPORT_PWM: - case HWSETTINGS_CC_RCVRPORT_PPM: - case HWSETTINGS_CC_RCVRPORT_PPMPWM: - PIOS_Servo_Init(&pios_servo_cfg); - break; - case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS: - case HWSETTINGS_CC_RCVRPORT_OUTPUTS: - PIOS_Servo_Init(&pios_servo_rcvr_cfg); - break; - } + switch (hwsettings_rcvrport) { + case HWSETTINGS_CC_RCVRPORT_DISABLED: + case HWSETTINGS_CC_RCVRPORT_PWM: + case HWSETTINGS_CC_RCVRPORT_PPM: + case HWSETTINGS_CC_RCVRPORT_PPMPWM: + PIOS_Servo_Init(&pios_servo_cfg); + break; + case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS: + case HWSETTINGS_CC_RCVRPORT_OUTPUTS: + PIOS_Servo_Init(&pios_servo_rcvr_cfg); + break; + } #else - PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); -#endif /* PIOS_ENABLE_DEBUG_PINS */ + PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); +#endif /* PIOS_ENABLE_DEBUG_PINS */ - switch(bdinfo->board_rev) { - case BOARD_REVISION_CC: - // Revision 1 with invensense gyros, start the ADC + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + // Revision 1 with invensense gyros, start the ADC #if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); + PIOS_ADC_Init(&pios_adc_cfg); #endif #if defined(PIOS_INCLUDE_ADXL345) - PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0); + PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0); #endif - break; - case BOARD_REVISION_CC3D: - // Revision 2 with MPU6000 gyros, start a SPI interface and connect to it - GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); + break; + case BOARD_REVISION_CC3D: + // Revision 2 with MPU6000 gyros, start a SPI interface and connect to it + GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); #if defined(PIOS_INCLUDE_MPU6000) - // Set up the SPI interface to the serial flash - if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { - PIOS_Assert(0); - } - PIOS_MPU6000_Init(pios_spi_gyro_id,0,&pios_mpu6000_cfg); - PIOS_MPU6000_CONFIG_Configure(); - init_test = PIOS_MPU6000_Test(); + // Set up the SPI interface to the serial flash + if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { + PIOS_Assert(0); + } + PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); + PIOS_MPU6000_CONFIG_Configure(); + init_test = PIOS_MPU6000_Test(); #endif /* PIOS_INCLUDE_MPU6000 */ - break; - default: - PIOS_Assert(0); - } + break; + default: + PIOS_Assert(0); + } - PIOS_GPIO_Init(); + PIOS_GPIO_Init(); - /* Make sure we have at least one telemetry link configured or else fail initialization */ - PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); + /* Make sure we have at least one telemetry link configured or else fail initialization */ + PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); } /** diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c b/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c index f63cfa7fd..5750bed21 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c @@ -7,19 +7,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,43 +33,42 @@ * initializes all the core systems on this specific hardware * called from System/openpilot.c */ -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Initialize the alarms library */ - AlarmsInitialize(); + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } - - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); - - /* Initialize the PiOS library */ - PIOS_COM_Init(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize the PiOS library */ + PIOS_COM_Init(); } const struct pios_udp_cfg pios_udp0_cfg = { - .ip = "0.0.0.0", - .port = 9000, + .ip = "0.0.0.0", + .port = 9000, }; const struct pios_udp_cfg pios_udp1_cfg = { - .ip = "0.0.0.0", - .port = 9001, + .ip = "0.0.0.0", + .port = 9001, }; const struct pios_udp_cfg pios_udp2_cfg = { - .ip = "0.0.0.0", - .port = 9002, + .ip = "0.0.0.0", + .port = 9002, }; #ifdef PIOS_COM_AUX @@ -77,8 +76,8 @@ const struct pios_udp_cfg pios_udp2_cfg = { * AUX USART */ const struct pios_udp_cfg pios_udp3_cfg = { - .ip = "0.0.0.0", - .port = 9003, + .ip = "0.0.0.0", + .port = 9003, }; #endif @@ -86,23 +85,23 @@ const struct pios_udp_cfg pios_udp3_cfg = { * Board specific number of devices. */ struct pios_udp_dev pios_udp_devs[] = { -#define PIOS_UDP_TELEM 0 - { - .cfg = &pios_udp0_cfg, - }, -#define PIOS_UDP_GPS 1 - { - .cfg = &pios_udp1_cfg, - }, -#define PIOS_UDP_LOCAL 2 - { - .cfg = &pios_udp2_cfg, - }, +#define PIOS_UDP_TELEM 0 + { + .cfg = &pios_udp0_cfg, + }, +#define PIOS_UDP_GPS 1 + { + .cfg = &pios_udp1_cfg, + }, +#define PIOS_UDP_LOCAL 2 + { + .cfg = &pios_udp2_cfg, + }, #ifdef PIOS_COM_AUX -#define PIOS_UDP_AUX 3 - { - .cfg = &pios_udp3_cfg, - }, +#define PIOS_UDP_AUX 3 + { + .cfg = &pios_udp3_cfg, + }, #endif }; @@ -119,23 +118,23 @@ extern const struct pios_com_driver pios_serial_com_driver; extern const struct pios_com_driver pios_udp_com_driver; struct pios_com_dev pios_com_devs[] = { - { - .id = PIOS_UDP_TELEM, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_GPS, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_LOCAL, - .driver = &pios_udp_com_driver, - }, + { + .id = PIOS_UDP_TELEM, + .driver = &pios_udp_com_driver, + }, + { + .id = PIOS_UDP_GPS, + .driver = &pios_udp_com_driver, + }, + { + .id = PIOS_UDP_LOCAL, + .driver = &pios_udp_com_driver, + }, #ifdef PIOS_COM_AUX - { - .id = PIOS_UDP_AUX, - .driver = &pios_udp_com_driver, - }, + { + .id = PIOS_UDP_AUX, + .driver = &pios_udp_com_driver, + }, #endif }; diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index 9b19be3a7..0408ecfa8 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -26,45 +26,45 @@ #ifndef PIOS_BOARD_H #define PIOS_BOARD_H -//------------------------ +// ------------------------ // Timers and Channels Used -//------------------------ +// ------------------------ /* - Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 - ------+-----------+-----------+-----------+---------- - TIM1 | Servo 4 | | | - TIM2 | RC In 5 | RC In 6 | Servo 6 | - TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 - TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 - ------+-----------+-----------+-----------+---------- + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | Servo 4 | | | + TIM2 | RC In 5 | RC In 6 | Servo 6 | + TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 + TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 + ------+-----------+-----------+-----------+---------- */ -//------------------------ +// ------------------------ // DMA Channels Used -//------------------------ -/* Channel 1 - */ -/* Channel 2 - */ -/* Channel 3 - */ -/* Channel 4 - */ -/* Channel 5 - */ -/* Channel 6 - */ -/* Channel 7 - */ -/* Channel 8 - */ -/* Channel 9 - */ -/* Channel 10 - */ -/* Channel 11 - */ -/* Channel 12 - */ +// ------------------------ +/* Channel 1 - */ +/* Channel 2 - */ +/* Channel 3 - */ +/* Channel 4 - */ +/* Channel 5 - */ +/* Channel 6 - */ +/* Channel 7 - */ +/* Channel 8 - */ +/* Channel 9 - */ +/* Channel 10 - */ +/* Channel 11 - */ +/* Channel 12 - */ -//------------------------ +// ------------------------ // BOOTLOADER_SETTINGS -//------------------------ -#define BOARD_READABLE TRUE -#define BOARD_WRITABLE TRUE -#define MAX_DEL_RETRYS 3 +// ------------------------ +#define BOARD_READABLE TRUE +#define BOARD_WRITABLE TRUE +#define MAX_DEL_RETRYS 3 -//------------------------ +// ------------------------ // WATCHDOG_SETTINGS -//------------------------ +// ------------------------ #define PIOS_WATCHDOG_TIMEOUT 250 #define PIOS_WDG_REGISTER BKP_DR4 #define PIOS_WDG_ACTUATOR 0x0001 @@ -73,205 +73,205 @@ #define PIOS_WDG_MANUAL 0x0008 #define PIOS_WDG_AUTOTUNE 0x0010 -//------------------------ +// ------------------------ // TELEMETRY -//------------------------ +// ------------------------ #define TELEM_QUEUE_SIZE 20 -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -#define PIOS_LED_HEARTBEAT 0 +// ------------------------ +#define PIOS_LED_HEARTBEAT 0 -//------------------------- +// ------------------------- // System Settings -//------------------------- -#define PIOS_MASTER_CLOCK 72000000 -#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) +// ------------------------- +#define PIOS_MASTER_CLOCK 72000000 +#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) -//------------------------- +// ------------------------- // Interrupt Priorities -//------------------------- -#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS -#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS -#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... -#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... -//------------------------ +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------ // PIOS_I2C // See also pios_board.c -//------------------------ -#define PIOS_I2C_MAX_DEVS 1 +// ------------------------ +#define PIOS_I2C_MAX_DEVS 1 extern uint32_t pios_i2c_flexi_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexi_adapter_id) -#define PIOS_I2C_ESC_ADAPTER (pios_i2c_flexi_adapter_id) -#define PIOS_I2C_BMP085_ADAPTER (pios_i2c_flexi_adapter_id) +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexi_adapter_id) +#define PIOS_I2C_ESC_ADAPTER (pios_i2c_flexi_adapter_id) +#define PIOS_I2C_BMP085_ADAPTER (pios_i2c_flexi_adapter_id) -//------------------------ +// ------------------------ // PIOS_BMP085 -//------------------------ -#define PIOS_BMP085_OVERSAMPLING 3 +// ------------------------ +#define PIOS_BMP085_OVERSAMPLING 3 -//------------------------- +// ------------------------- // SPI // // See also pios_board.c -//------------------------- -#define PIOS_SPI_MAX_DEVS 2 +// ------------------------- +#define PIOS_SPI_MAX_DEVS 2 -//------------------------- +// ------------------------- // PIOS_USART -//------------------------- -#define PIOS_USART_MAX_DEVS 2 +// ------------------------- +#define PIOS_USART_MAX_DEVS 2 -//------------------------- +// ------------------------- // PIOS_COM // // See also pios_board.c -//------------------------- -#define PIOS_COM_MAX_DEVS 3 +// ------------------------- +#define PIOS_COM_MAX_DEVS 3 extern uint32_t pios_com_telem_rf_id; -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) #if defined(PIOS_INCLUDE_GPS) extern uint32_t pios_com_gps_id; -#define PIOS_COM_GPS (pios_com_gps_id) -#endif /* PIOS_INCLUDE_GPS */ +#define PIOS_COM_GPS (pios_com_gps_id) +#endif /* PIOS_INCLUDE_GPS */ extern uint32_t pios_com_bridge_id; -#define PIOS_COM_BRIDGE (pios_com_bridge_id) +#define PIOS_COM_BRIDGE (pios_com_bridge_id) extern uint32_t pios_com_vcp_id; -#define PIOS_COM_VCP (pios_com_vcp_id) +#define PIOS_COM_VCP (pios_com_vcp_id) extern uint32_t pios_com_telem_usb_id; -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) extern uint32_t pios_com_debug_id; -#define PIOS_COM_DEBUG (pios_com_debug_id) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#define PIOS_COM_DEBUG (pios_com_debug_id) +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ extern uint32_t pios_com_hkosd_id; -#define PIOS_COM_OSDHK (pios_com_hkosd_id) +#define PIOS_COM_OSDHK (pios_com_hkosd_id) -//------------------------- +// ------------------------- // ADC // PIOS_ADC_PinGet(0) = Gyro Z // PIOS_ADC_PinGet(1) = Gyro Y // PIOS_ADC_PinGet(2) = Gyro X -//------------------------- -//#define PIOS_ADC_OVERSAMPLING_RATE 1 -#define PIOS_ADC_USE_TEMP_SENSOR 1 -#define PIOS_ADC_TEMP_SENSOR_ADC ADC1 -#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 1 +// ------------------------- +// #define PIOS_ADC_OVERSAMPLING_RATE 1 +#define PIOS_ADC_USE_TEMP_SENSOR 1 +#define PIOS_ADC_TEMP_SENSOR_ADC ADC1 +#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 1 -#define PIOS_ADC_PIN1_GPIO_PORT GPIOA // PA4 (Gyro X) -#define PIOS_ADC_PIN1_GPIO_PIN GPIO_Pin_4 // ADC12_IN4 -#define PIOS_ADC_PIN1_GPIO_CHANNEL ADC_Channel_4 -#define PIOS_ADC_PIN1_ADC ADC2 -#define PIOS_ADC_PIN1_ADC_NUMBER 1 +#define PIOS_ADC_PIN1_GPIO_PORT GPIOA // PA4 (Gyro X) +#define PIOS_ADC_PIN1_GPIO_PIN GPIO_Pin_4 // ADC12_IN4 +#define PIOS_ADC_PIN1_GPIO_CHANNEL ADC_Channel_4 +#define PIOS_ADC_PIN1_ADC ADC2 +#define PIOS_ADC_PIN1_ADC_NUMBER 1 -#define PIOS_ADC_PIN2_GPIO_PORT GPIOA // PA5 (Gyro Y) -#define PIOS_ADC_PIN2_GPIO_PIN GPIO_Pin_5 // ADC123_IN5 -#define PIOS_ADC_PIN2_GPIO_CHANNEL ADC_Channel_5 -#define PIOS_ADC_PIN2_ADC ADC1 -#define PIOS_ADC_PIN2_ADC_NUMBER 2 +#define PIOS_ADC_PIN2_GPIO_PORT GPIOA // PA5 (Gyro Y) +#define PIOS_ADC_PIN2_GPIO_PIN GPIO_Pin_5 // ADC123_IN5 +#define PIOS_ADC_PIN2_GPIO_CHANNEL ADC_Channel_5 +#define PIOS_ADC_PIN2_ADC ADC1 +#define PIOS_ADC_PIN2_ADC_NUMBER 2 -#define PIOS_ADC_PIN3_GPIO_PORT GPIOA // PA3 (Gyro Z) -#define PIOS_ADC_PIN3_GPIO_PIN GPIO_Pin_3 // ADC12_IN3 -#define PIOS_ADC_PIN3_GPIO_CHANNEL ADC_Channel_3 -#define PIOS_ADC_PIN3_ADC ADC2 -#define PIOS_ADC_PIN3_ADC_NUMBER 2 +#define PIOS_ADC_PIN3_GPIO_PORT GPIOA // PA3 (Gyro Z) +#define PIOS_ADC_PIN3_GPIO_PIN GPIO_Pin_3 // ADC12_IN3 +#define PIOS_ADC_PIN3_GPIO_CHANNEL ADC_Channel_3 +#define PIOS_ADC_PIN3_ADC ADC2 +#define PIOS_ADC_PIN3_ADC_NUMBER 2 -#define PIOS_ADC_NUM_PINS 3 +#define PIOS_ADC_NUM_PINS 3 -#define PIOS_ADC_PORTS { PIOS_ADC_PIN1_GPIO_PORT, PIOS_ADC_PIN2_GPIO_PORT, PIOS_ADC_PIN3_GPIO_PORT } -#define PIOS_ADC_PINS { PIOS_ADC_PIN1_GPIO_PIN, PIOS_ADC_PIN2_GPIO_PIN, PIOS_ADC_PIN3_GPIO_PIN } -#define PIOS_ADC_CHANNELS { PIOS_ADC_PIN1_GPIO_CHANNEL, PIOS_ADC_PIN2_GPIO_CHANNEL, PIOS_ADC_PIN3_GPIO_CHANNEL } -#define PIOS_ADC_MAPPING { PIOS_ADC_PIN1_ADC, PIOS_ADC_PIN2_ADC, PIOS_ADC_PIN3_ADC } -#define PIOS_ADC_CHANNEL_MAPPING { PIOS_ADC_PIN1_ADC_NUMBER, PIOS_ADC_PIN2_ADC_NUMBER, PIOS_ADC_PIN3_ADC_NUMBER } -#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR) -#define PIOS_ADC_NUM_ADC_CHANNELS 2 -#define PIOS_ADC_USE_ADC2 1 -#define PIOS_ADC_CLOCK_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE) -#define PIOS_ADC_ADCCLK RCC_PCLK2_Div8 +#define PIOS_ADC_PORTS { PIOS_ADC_PIN1_GPIO_PORT, PIOS_ADC_PIN2_GPIO_PORT, PIOS_ADC_PIN3_GPIO_PORT } +#define PIOS_ADC_PINS { PIOS_ADC_PIN1_GPIO_PIN, PIOS_ADC_PIN2_GPIO_PIN, PIOS_ADC_PIN3_GPIO_PIN } +#define PIOS_ADC_CHANNELS { PIOS_ADC_PIN1_GPIO_CHANNEL, PIOS_ADC_PIN2_GPIO_CHANNEL, PIOS_ADC_PIN3_GPIO_CHANNEL } +#define PIOS_ADC_MAPPING { PIOS_ADC_PIN1_ADC, PIOS_ADC_PIN2_ADC, PIOS_ADC_PIN3_ADC } +#define PIOS_ADC_CHANNEL_MAPPING { PIOS_ADC_PIN1_ADC_NUMBER, PIOS_ADC_PIN2_ADC_NUMBER, PIOS_ADC_PIN3_ADC_NUMBER } +#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR) +#define PIOS_ADC_NUM_ADC_CHANNELS 2 +#define PIOS_ADC_USE_ADC2 1 +#define PIOS_ADC_CLOCK_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE) +#define PIOS_ADC_ADCCLK RCC_PCLK2_Div8 /* RCC_PCLK2_Div2: ADC clock = PCLK2/2 */ /* RCC_PCLK2_Div4: ADC clock = PCLK2/4 */ /* RCC_PCLK2_Div6: ADC clock = PCLK2/6 */ /* RCC_PCLK2_Div8: ADC clock = PCLK2/8 */ -#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_239Cycles5 +#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_239Cycles5 /* Sample time: */ /* With an ADCCLK = 14 MHz and a sampling time of 239.5 cycles: */ /* Tconv = 239.5 + 12.5 = 252 cycles = 18�s */ /* (1 / (ADCCLK / CYCLES)) = Sample Time (�S) */ -#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_LOW +#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_LOW // Currently analog acquistion hard coded at 480 Hz // PCKL2 = HCLK / 16 // ADCCLK = PCLK2 / 2 -#define PIOS_ADC_RATE (72.0e6f / 1.0f / 8.0f / 252.0f / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) -#define PIOS_ADC_MAX_OVERSAMPLING 36 +#define PIOS_ADC_RATE (72.0e6f / 1.0f / 8.0f / 252.0f / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) +#define PIOS_ADC_MAX_OVERSAMPLING 36 -//------------------------ +// ------------------------ // PIOS_RCVR // See also pios_board.c -//------------------------ -#define PIOS_RCVR_MAX_DEVS 3 -#define PIOS_RCVR_MAX_CHANNELS 12 -#define PIOS_GCSRCVR_TIMEOUT_MS 100 +// ------------------------ +#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_GCSRCVR_TIMEOUT_MS 100 -//------------------------- +// ------------------------- // Receiver PPM input -//------------------------- -#define PIOS_PPM_MAX_DEVS 1 -#define PIOS_PPM_NUM_INPUTS 12 +// ------------------------- +#define PIOS_PPM_MAX_DEVS 1 +#define PIOS_PPM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver PWM input -//------------------------- -#define PIOS_PWM_MAX_DEVS 1 -#define PIOS_PWM_NUM_INPUTS 6 +// ------------------------- +#define PIOS_PWM_MAX_DEVS 1 +#define PIOS_PWM_NUM_INPUTS 6 -//------------------------- +// ------------------------- // Receiver DSM input -//------------------------- -#define PIOS_DSM_MAX_DEVS 2 -#define PIOS_DSM_NUM_INPUTS 12 +// ------------------------- +#define PIOS_DSM_MAX_DEVS 2 +#define PIOS_DSM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver S.Bus input -//------------------------- -#define PIOS_SBUS_MAX_DEVS 1 -#define PIOS_SBUS_NUM_INPUTS (16+2) +// ------------------------- +#define PIOS_SBUS_MAX_DEVS 1 +#define PIOS_SBUS_NUM_INPUTS (16 + 2) -//------------------------- +// ------------------------- // Servo outputs -//------------------------- -#define PIOS_SERVO_UPDATE_HZ 50 -#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ -//-------------------------- +// -------------------------- // Timer controller settings -//-------------------------- -#define PIOS_TIM_MAX_DEVS 3 +// -------------------------- +#define PIOS_TIM_MAX_DEVS 3 -//------------------------- +// ------------------------- // GPIO -//------------------------- -#define PIOS_GPIO_PORTS { } -#define PIOS_GPIO_PINS { } -#define PIOS_GPIO_CLKS { } -#define PIOS_GPIO_NUM 0 +// ------------------------- +#define PIOS_GPIO_PORTS {} +#define PIOS_GPIO_PINS {} +#define PIOS_GPIO_CLKS {} +#define PIOS_GPIO_NUM 0 -//------------------------- +// ------------------------- // USB -//------------------------- -#define PIOS_USB_HID_MAX_DEVS 1 +// ------------------------- +#define PIOS_USB_HID_MAX_DEVS 1 -#define PIOS_USB_ENABLED 1 -#define PIOS_USB_DETECT_GPIO_PORT GPIOC -#define PIOS_USB_MAX_DEVS 1 -#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 +#define PIOS_USB_ENABLED 1 +#define PIOS_USB_DETECT_GPIO_PORT GPIOC +#define PIOS_USB_MAX_DEVS 1 +#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/coptercontrol/pios_usb_board_data.c b/flight/targets/boards/coptercontrol/pios_usb_board_data.c index 67c19e6c3..20c49fdcf 100644 --- a/flight/targets/boards/coptercontrol/pios_usb_board_data.c +++ b/flight/targets/boards/coptercontrol/pios_usb_board_data.c @@ -28,74 +28,75 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ -#include /* PIOS_SYS_SerialNumberGet */ -#include /* PIOS_USBHOOK_* */ -#include /* PIOS_USB_UTIL_AsciiToUtf8 */ +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ static const uint8_t usb_product_id[28] = { - sizeof(usb_product_id), - USB_DESC_TYPE_STRING, - 'C', 0, - 'o', 0, - 'p', 0, - 't', 0, - 'e', 0, - 'r', 0, - 'C', 0, - 'o', 0, - 'n', 0, - 't', 0, - 'r', 0, - 'o', 0, - 'l', 0, + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'C', 0, + 'o', 0, + 'p', 0, + 't', 0, + 'e', 0, + 'r', 0, + 'C', 0, + 'o', 0, + 'n', 0, + 't', 0, + 'r', 0, + 'o', 0, + 'l', 0, }; -static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = { - sizeof(usb_serial_number), - USB_DESC_TYPE_STRING, +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, }; static const struct usb_string_langid usb_lang_id = { - .bLength = sizeof(usb_lang_id), - .bDescriptorType = USB_DESC_TYPE_STRING, - .bLangID = htousbs(USB_LANGID_ENGLISH_US), + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), }; static const uint8_t usb_vendor_id[28] = { - sizeof(usb_vendor_id), - USB_DESC_TYPE_STRING, - 'o', 0, - 'p', 0, - 'e', 0, - 'n', 0, - 'p', 0, - 'i', 0, - 'l', 0, - 'o', 0, - 't', 0, - '.', 0, - 'o', 0, - 'r', 0, - 'g', 0 + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 }; int32_t PIOS_USB_BOARD_DATA_Init(void) { - /* Load device serial number into serial number string */ - uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - PIOS_SYS_SerialNumberGet((char *)sn); + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ - uint8_t * utf8 = &(usb_serial_number[2]); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1); + PIOS_SYS_SerialNumberGet((char *)sn); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); - return 0; + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; } diff --git a/flight/targets/boards/oplinkmini/board_hw_defs.c b/flight/targets/boards/oplinkmini/board_hw_defs.c index 2477af7d0..0939ef105 100644 --- a/flight/targets/boards/oplinkmini/board_hw_defs.c +++ b/flight/targets/boards/oplinkmini/board_hw_defs.c @@ -1,120 +1,118 @@ - #if defined(PIOS_INCLUDE_LED) #include static const struct pios_led pios_leds[] = { - [PIOS_LED_USB] = { - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, - [PIOS_LED_LINK] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, - [PIOS_LED_RX] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, - [PIOS_LED_TX] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, + [PIOS_LED_USB] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_LINK] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_RX] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_TX] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, #ifdef PIOS_RFM22B_DEBUG_ON_TELEM - [PIOS_LED_D1] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, - [PIOS_LED_D2] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, - [PIOS_LED_D3] = { - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, - [PIOS_LED_D4] = { - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_50MHz, - }, - }, - }, -#endif + [PIOS_LED_D1] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_D2] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_D3] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_D4] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, +#endif /* ifdef PIOS_RFM22B_DEBUG_ON_TELEM */ }; static const struct pios_led_cfg pios_led_cfg = { - .leds = pios_leds, - .num_leds = NELEMENTS(pios_leds), + .leds = pios_leds, + .num_leds = NELEMENTS(pios_leds), }; -const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (__attribute__((unused)) uint32_t board_revision) +const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision) { - return &pios_led_cfg; + return &pios_led_cfg; } -#endif /* PIOS_INCLUDE_LED */ +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_FLASH) #include "pios_flashfs_logfs_priv.h" #include "pios_flash_internal_priv.h" -static const struct pios_flash_internal_cfg flash_internal_cfg = { -}; +static const struct pios_flash_internal_cfg flash_internal_cfg = {}; static const struct flashfs_logfs_cfg flashfs_internal_cfg = { - .fs_magic = 0x99abcfef, + .fs_magic = 0x99abcfef, .total_fs_size = EE_BANK_SIZE, /* 2K bytes (2x1KB sectors) */ - .arena_size = 0x00002000, /* 4 * slot size = 1K bytes = 1 sector */ - .slot_size = 0x00000100, /* 256 bytes */ + .arena_size = 0x00002000, /* 4 * slot size = 1K bytes = 1 sector */ + .slot_size = 0x00000100, /* 256 bytes */ - .start_offset = EE_BANK_BASE, /* start after the bootloader */ - .sector_size = 0x00000400, /* 1K bytes */ - .page_size = 0x00000400, /* 1K bytes */ + .start_offset = EE_BANK_BASE, /* start after the bootloader */ + .sector_size = 0x00000400, /* 1K bytes */ + .page_size = 0x00000400, /* 1K bytes */ }; #include "pios_flash.h" @@ -132,118 +130,108 @@ static const struct flashfs_logfs_cfg flashfs_internal_cfg = { * .rodata section (ie. Flash) rather than in the .bss section (RAM). */ void PIOS_SPI_port_irq_handler(void); -void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_port_irq_handler"))); -void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_port_irq_handler"))); +void DMA1_Channel5_IRQHandler() __attribute__((alias("PIOS_SPI_port_irq_handler"))); +void DMA1_Channel4_IRQHandler() __attribute__((alias("PIOS_SPI_port_irq_handler"))); -static const struct pios_spi_cfg pios_spi_rfm22b_cfg = -{ - .regs = SPI1, +static const struct pios_spi_cfg pios_spi_rfm22b_cfg = { + .regs = SPI1, - .init = - { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 0, - .SPI_CPOL = SPI_CPOL_Low, - .SPI_CPHA = SPI_CPHA_1Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, // slowest SCLK - }, - .use_crc = FALSE, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 0, + .SPI_CPOL = SPI_CPOL_Low, + .SPI_CPHA = SPI_CPHA_1Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, // slowest SCLK + }, + .use_crc = FALSE, - .dma = - { - .ahb_clk = RCC_AHBPeriph_DMA1, - .irq = - { - .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), - .init = { - .NVIC_IRQChannel = DMA1_Channel2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + .irq = { + .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), + .init = { + .NVIC_IRQChannel = DMA1_Channel2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, - .rx = { - .channel = DMA1_Channel2, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel3, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .slave_count = 1, - .ssel = - {{ - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }}, - .sclk = - { - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = - { - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .mosi = - { - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, + .rx = { + .channel = DMA1_Channel2, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel3, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + } + }, + .sclk = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .mosi = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, }; uint32_t pios_spi_rfm22b_id; void PIOS_SPI_port_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_rfm22b_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_rfm22b_id); } #endif /* PIOS_INCLUDE_SPI */ @@ -253,47 +241,47 @@ void PIOS_SPI_port_irq_handler(void) #include static const struct pios_exti_cfg pios_exti_rfm22b_cfg __exti_config = { - .vector = PIOS_RFM22_EXT_Int, - .line = EXTI_Line2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line2, - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Falling, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_RFM22_EXT_Int, + .line = EXTI_Line2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line2, + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Falling, + .EXTI_LineCmd = ENABLE, + }, + }, }; #include struct pios_rfm22b_cfg pios_rfm22b_cfg = { - .spi_cfg = &pios_spi_rfm22b_cfg, - .exti_cfg = &pios_exti_rfm22b_cfg, - .RFXtalCap = 0x7f, - .slave_num = 0, - .gpio_direction = GPIO0_TX_GPIO1_RX, + .spi_cfg = &pios_spi_rfm22b_cfg, + .exti_cfg = &pios_exti_rfm22b_cfg, + .RFXtalCap = 0x7f, + .slave_num = 0, + .gpio_direction = GPIO0_TX_GPIO1_RX, }; -//! Compatibility layer for various hardware revisions -const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (__attribute__((unused)) uint32_t board_revision) +// ! Compatibility layer for various hardware revisions +const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22Cfg(__attribute__((unused)) uint32_t board_revision) { - return &pios_rfm22b_cfg; + return &pios_rfm22b_cfg; } #endif /* PIOS_INCLUDE_RFM22B */ @@ -305,146 +293,147 @@ const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (__attribute__((un */ #include "pios_adc_priv.h" extern void PIOS_ADC_handler(void); -void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); +void DMA1_Channel1_IRQHandler() __attribute__((alias("PIOS_ADC_handler"))); // Remap the ADC DMA handler to this one static const struct pios_adc_cfg pios_adc_cfg = { - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - .irq = { - .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), - .init = { - .NVIC_IRQChannel = DMA1_Channel1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA1_Channel1, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR, - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - } - }, - .half_flag = DMA1_IT_HT1, - .full_flag = DMA1_IT_TC1, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + .irq = { + .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), + .init = { + .NVIC_IRQChannel = DMA1_Channel1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA1_Channel1, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR, + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + } + }, + .half_flag = DMA1_IT_HT1, + .full_flag = DMA1_IT_TC1, }; struct pios_adc_dev pios_adc_devs[] = { - { - .cfg = &pios_adc_cfg, - .callback_function = NULL, - }, + { + .cfg = &pios_adc_cfg, + .callback_function = NULL, + }, }; uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs); -void PIOS_ADC_handler() { - PIOS_ADC_DMA_Handler(); +void PIOS_ADC_handler() +{ + PIOS_ADC_DMA_Handler(); } -#endif /* PIOS_INCLUDE_ADC */ +#endif /* PIOS_INCLUDE_ADC */ #if defined(PIOS_INCLUDE_TIM) #include "pios_tim_priv.h" static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, }; static const struct pios_tim_clock_cfg tim_1_cfg = { - .timer = TIM1, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM1, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_2_cfg = { - .timer = TIM2, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM2, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_3_cfg = { - .timer = TIM3, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM3, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_4_cfg = { - .timer = TIM4, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM4, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_channel pios_tim_ppm_flexi_port = { - .timer = TIM2, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap2_TIM2, + .timer = TIM2, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap2_TIM2, }; static const struct pios_tim_channel pios_tim_ppm_main_port = { - .timer = TIM1, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, + .timer = TIM1, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }; -#endif /* PIOS_INCLUDE_TIM */ +#endif /* PIOS_INCLUDE_TIM */ #if defined(PIOS_INCLUDE_USART) @@ -453,84 +442,76 @@ static const struct pios_tim_channel pios_tim_ppm_main_port = { /* * SERIAL USART */ -static const struct pios_usart_cfg pios_usart_serial_cfg = -{ - .regs = USART1, - .init = - { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = - { - .init = - { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = - { - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = - { - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, +static const struct pios_usart_cfg pios_usart_serial_cfg = { + .regs = USART1, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, }; static const struct pios_usart_cfg pios_usart_telem_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, + .regs = USART3, .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, }, - }, }; #endif /* PIOS_INCLUDE_USART */ @@ -547,27 +528,27 @@ static const struct pios_usart_cfg pios_usart_telem_flexi_cfg = { */ #include -void PIOS_RTC_IRQ_Handler (void); -void RTC_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler"))); +void PIOS_RTC_IRQ_Handler(void); +void RTC_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); static const struct pios_rtc_cfg pios_rtc_main_cfg = { - .clksrc = RCC_RTCCLKSource_HSE_Div128, - .prescaler = 100, - .irq = { - .init = { - .NVIC_IRQChannel = RTC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .clksrc = RCC_RTCCLKSource_HSE_Div128, + .prescaler = 100, + .irq = { + .init = { + .NVIC_IRQChannel = RTC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; -void PIOS_RTC_IRQ_Handler (void) +void PIOS_RTC_IRQ_Handler(void) { - PIOS_RTC_irq_handler (); + PIOS_RTC_irq_handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_RTC) */ /* * PPM Inputs @@ -576,28 +557,28 @@ void PIOS_RTC_IRQ_Handler (void) #include const struct pios_ppm_cfg pios_ppm_flexi_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = &pios_tim_ppm_flexi_port, - .num_channels = 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_ppm_flexi_port, + .num_channels = 1, }; const struct pios_ppm_cfg pios_ppm_main_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = &pios_tim_ppm_main_port, - .num_channels = 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_ppm_main_port, + .num_channels = 1, }; -#endif /* PIOS_INCLUDE_PPM */ +#endif /* PIOS_INCLUDE_PPM */ /* * PPM Output @@ -606,33 +587,33 @@ const struct pios_ppm_cfg pios_ppm_main_cfg = { #include static const struct pios_tim_channel pios_tim_ppmout[] = { - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_FullRemap_TIM2, - } + { + .timer = TIM2, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_FullRemap_TIM2, + } }; const struct pios_ppm_out_cfg pios_ppm_out_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_Low, - .TIM_OCNPolarity = TIM_OCPolarity_Low, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channel = pios_tim_ppmout, + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_Low, + .TIM_OCNPolarity = TIM_OCPolarity_Low, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channel = pios_tim_ppmout, }; #endif /* PIOS_INCLUDE_PPM_OUT */ @@ -646,30 +627,30 @@ const struct pios_ppm_out_cfg pios_ppm_out_cfg = { #include "pios_usb_priv.h" static const struct pios_usb_cfg pios_usb_main_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, }, - .vsense_active_low = false + }, + .vsense = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .vsense_active_low = false }; #include "pios_usb_board_data_priv.h" #include "pios_usb_desc_hid_cdc_priv.h" #include "pios_usb_desc_hid_only_priv.h" -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ #if defined(PIOS_INCLUDE_COM_MSG) @@ -681,9 +662,9 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { #include const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 2, - .data_rx_ep = 1, - .data_tx_ep = 1, + .data_if = 2, + .data_rx_ep = 1, + .data_tx_ep = 1, }; #endif /* PIOS_INCLUDE_USB_HID */ @@ -691,21 +672,21 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { #include const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 0, - .ctrl_tx_ep = 2, + .ctrl_if = 0, + .ctrl_tx_ep = 2, - .data_if = 1, - .data_rx_ep = 3, - .data_tx_ep = 3, + .data_if = 1, + .data_rx_ep = 3, + .data_tx_ep = 3, }; -#endif /* PIOS_INCLUDE_USB_CDC */ +#endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_FLASH_EEPROM) #include const struct pios_eeprom_cfg pios_eeprom_cfg = { - .base_address = PIOS_FLASH_EEPROM_ADDR, - .max_size = PIOS_FLASH_EEPROM_LEN, + .base_address = PIOS_FLASH_EEPROM_ADDR, + .max_size = PIOS_FLASH_EEPROM_LEN, }; #endif /* PIOS_INCLUDE_FLASH_EEPROM */ diff --git a/flight/targets/boards/oplinkmini/bootloader/inc/common.h b/flight/targets/boards/oplinkmini/bootloader/inc/common.h index 9973764e9..1212e06fd 100644 --- a/flight/targets/boards/oplinkmini/bootloader/inc/common.h +++ b/flight/targets/boards/oplinkmini/bootloader/inc/common.h @@ -28,10 +28,10 @@ #ifndef COMMON_H_ #define COMMON_H_ -//#include "board.h" +// #include "board.h" typedef enum { - start, keepgoing, + start, keepgoing, } DownloadAction; /**************************************************/ @@ -39,77 +39,77 @@ typedef enum { /**************************************************/ typedef enum { - DFUidle, //0 - uploading, //1 - wrong_packet_received, //2 - too_many_packets, //3 - too_few_packets, //4 - Last_operation_Success, //5 - downloading, //6 - BLidle, //7 - Last_operation_failed, //8 - uploadingStarting, //9 - outsideDevCapabilities, //10 - CRC_Fail,//11 - failed_jump, -//12 + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + BLidle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, +// 12 } DFUStates; /**************************************************/ /* OP_DFU commands */ /**************************************************/ typedef enum { - Reserved, //0 - Req_Capabilities, //1 - Rep_Capabilities, //2 - EnterDFU, //3 - JumpFW, //4 - Reset, //5 - Abort_Operation, //6 - Upload, //7 - Op_END, //8 - Download_Req, //9 - Download, //10 - Status_Request, //11 - Status_Rep -//12 + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep +// 12 } DFUCommands; typedef enum { - High_Density, Medium_Density + High_Density, Medium_Density } DeviceType; /**************************************************/ /* OP_DFU transfer types */ /**************************************************/ typedef enum { - FW, //0 - Descript -//2 + FW, // 0 + Descript +// 2 } DFUTransfer; /**************************************************/ /* OP_DFU transfer port */ /**************************************************/ typedef enum { - Usb, //0 - Serial -//2 + Usb, // 0 + Serial +// 2 } DFUPort; /**************************************************/ /* OP_DFU programable programable HW types */ /**************************************************/ typedef enum { - Self_flash, //0 - Remote_flash_via_spi -//1 + Self_flash, // 0 + Remote_flash_via_spi +// 1 } DFUProgType; /**************************************************/ /* OP_DFU programable sources */ /**************************************************/ -#define USB 0 -#define SPI 1 +#define USB 0 +#define SPI 1 -#define DownloadDelay 100000 +#define DownloadDelay 100000 -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 +#define MAX_DEL_RETRYS 3 +#define MAX_WRI_RETRYS 3 #endif /* COMMON_H_ */ diff --git a/flight/targets/boards/oplinkmini/bootloader/inc/pios_config.h b/flight/targets/boards/oplinkmini/bootloader/inc/pios_config.h index fbd493cc6..7a974ad83 100644 --- a/flight/targets/boards/oplinkmini/bootloader/inc/pios_config.h +++ b/flight/targets/boards/oplinkmini/bootloader/inc/pios_config.h @@ -2,28 +2,28 @@ ****************************************************************************** * @addtogroup OpenPilotBL OpenPilot BootLoader * @{ - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * In particular, pios_config.h is where you define which PiOS libraries * and features are included in the firmware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/oplinkmini/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/oplinkmini/bootloader/inc/pios_usb_board_data.h index ae6704b6e..e32e8fd9e 100644 --- a/flight/targets/boards/oplinkmini/bootloader/inc/pios_usb_board_data.h +++ b/flight/targets/boards/oplinkmini/bootloader/inc/pios_usb_board_data.h @@ -33,13 +33,13 @@ #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 2 +#define PIOS_USB_BOARD_EP_NUM 2 -#include /* struct usb_* */ +#include /* struct usb_* */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OPLINK -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OPLINK, USB_OP_BOARD_MODE_BL) -#define PIOS_USB_BOARD_SN_SUFFIX "+BL" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OPLINK +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OPLINK, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" /* * The bootloader uses a simplified report structure @@ -49,4 +49,4 @@ */ #define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/oplinkmini/bootloader/main.c b/flight/targets/boards/oplinkmini/bootloader/main.c index ae0fdd968..a243e8000 100644 --- a/flight/targets/boards/oplinkmini/bootloader/main.c +++ b/flight/targets/boards/oplinkmini/bootloader/main.c @@ -8,21 +8,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief This is the file with the main function of the OpenPilot BootLoader * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -59,10 +59,10 @@ uint8_t tempcount = 0; /* Extern variables ----------------------------------------------------------*/ DFUStates DeviceState; -int16_t status = 0; -uint8_t JumpToApp = FALSE; -uint8_t GO_dfu = FALSE; -uint8_t USB_connected = FALSE; +int16_t status = 0; +uint8_t JumpToApp = FALSE; +uint8_t GO_dfu = FALSE; +uint8_t USB_connected = FALSE; uint8_t User_DFU_request = FALSE; static uint8_t mReceive_Buffer[63]; /* Private function prototypes -----------------------------------------------*/ @@ -70,137 +70,150 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); uint8_t processRX(); void jump_to_app(); -int main() { - PIOS_SYS_Init(); - PIOS_Board_Init(); - PIOS_IAP_Init(); - - USB_connected = PIOS_USB_CableConnected(0); +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); - if (PIOS_IAP_CheckRequest() == TRUE) { - PIOS_DELAY_WaitmS(1000); - User_DFU_request = TRUE; - PIOS_IAP_ClearRequest(); - } + USB_connected = PIOS_USB_CableConnected(0); - GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); + if (PIOS_IAP_CheckRequest() == TRUE) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = TRUE; + PIOS_IAP_ClearRequest(); + } - if (GO_dfu == TRUE) { - PIOS_Board_Init(); - if (User_DFU_request == TRUE) - DeviceState = DFUidle; - else - DeviceState = BLidle; - } else - JumpToApp = TRUE; + GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); - uint32_t stopwatch = 0; - uint32_t prev_ticks = PIOS_DELAY_GetuS(); - while (TRUE) { - /* Update the stopwatch */ - uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); - prev_ticks += elapsed_ticks; - stopwatch += elapsed_ticks; + if (GO_dfu == TRUE) { + PIOS_Board_Init(); + if (User_DFU_request == TRUE) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = TRUE; + } - if (JumpToApp == TRUE) - jump_to_app(); + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (TRUE) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; - switch (DeviceState) { - case Last_operation_Success: - case uploadingStarting: - case DFUidle: - period1 = 5000; - sweep_steps1 = 100; - PIOS_LED_Off(PIOS_LED_ALARM); - period2 = 0; - break; - case uploading: - period1 = 5000; - sweep_steps1 = 100; - period2 = 2500; - sweep_steps2 = 50; - break; - case downloading: - period1 = 2500; - sweep_steps1 = 50; - PIOS_LED_Off(PIOS_LED_ALARM); - period2 = 0; - break; - case BLidle: - period1 = 0; - PIOS_LED_On(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - default://error - period1 = 5000; - sweep_steps1 = 100; - period2 = 5000; - sweep_steps2 = 100; - } + if (JumpToApp == TRUE) { + jump_to_app(); + } - if (period1 != 0) { - if (LedPWM(period1, sweep_steps1, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_On(PIOS_LED_HEARTBEAT); + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_ALARM); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_ALARM); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } - if (period2 != 0) { - if (LedPWM(period2, sweep_steps2, stopwatch)) - PIOS_LED_On(PIOS_LED_ALARM); - else - PIOS_LED_Off(PIOS_LED_ALARM); - } else - PIOS_LED_Off(PIOS_LED_ALARM); + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } - if (stopwatch > 50 * 1000 * 1000) - stopwatch = 0; - if ((stopwatch > 6 * 1000 * 1000) && (DeviceState - == BLidle)) - JumpToApp = TRUE; + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_ALARM); + } else { + PIOS_LED_Off(PIOS_LED_ALARM); + } + } else { + PIOS_LED_Off(PIOS_LED_ALARM); + } - processRX(); - DataDownload(start); - } + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && (DeviceState + == BLidle)) { + JumpToApp = TRUE; + } + + processRX(); + DataDownload(start); + } } -void jump_to_app() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; - if (((*(__IO uint32_t*) bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */ - FLASH_Lock(); - RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - _SetCNTR(0); // clear interrupt mask - _SetISTR(0); // clear all requests - JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); - Jump_To_Application = (pFunction) JumpAddress; - /* Initialize user application's Stack Pointer */ - __set_MSP(*(__IO uint32_t*) bdinfo->fw_base); - Jump_To_Application(); - } else { - DeviceState = failed_jump; - return; - } + if (((*(__IO uint32_t *)bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */ + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + _SetCNTR(0); // clear interrupt mask + _SetISTR(0); // clear all requests + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + return; + } } -uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { - uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ - uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ - uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ - if (curr_sweep & 1) { - pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ - } - return ((count % pwm_period) > pwm_duty) ? 1 : 0; + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; } -uint8_t processRX() { - if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { - processComand(mReceive_Buffer); - } - return TRUE; +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return TRUE; } - diff --git a/flight/targets/boards/oplinkmini/bootloader/pios_board.c b/flight/targets/boards/oplinkmini/bootloader/pios_board.c index f91f6a456..7bcd5ce8a 100644 --- a/flight/targets/boards/oplinkmini/bootloader/pios_board.c +++ b/flight/targets/boards/oplinkmini/bootloader/pios_board.c @@ -44,51 +44,52 @@ uint32_t pios_com_telem_usb_id; * called from System/openpilot.c */ static bool board_init_complete = false; -void PIOS_Board_Init(void) { - if (board_init_complete) { - return; - } +void PIOS_Board_Init(void) +{ + if (board_init_complete) { + return; + } - /* Enable Prefetch Buffer */ - FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); + /* Enable Prefetch Buffer */ + FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); - /* Flash 2 wait state */ - FLASH_SetLatency(FLASH_Latency_2); + /* Flash 2 wait state */ + FLASH_SetLatency(FLASH_Latency_2); - /* Delay system */ - PIOS_DELAY_Init(); + /* Delay system */ + PIOS_DELAY_Init(); - /* Initialize the PiOS library */ - PIOS_GPIO_Init(); + /* Initialize the PiOS library */ + PIOS_GPIO_Init(); #if defined(PIOS_INCLUDE_LED) - PIOS_LED_Init(&pios_led_cfg); -#endif /* PIOS_INCLUDE_LED */ + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Activate the HID-only USB configuration */ - PIOS_USB_DESC_HID_ONLY_Init(); + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); - uint32_t pios_usb_id; - if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) { - PIOS_Assert(0); - } + uint32_t pios_usb_id; + if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) { + PIOS_Assert(0); + } #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { - PIOS_Assert(0); - } -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); // TODO Tirar - board_init_complete = true; + board_init_complete = true; } diff --git a/flight/targets/boards/oplinkmini/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/oplinkmini/firmware/inc/FreeRTOSConfig.h index 38ff0bc7c..b40088c90 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/FreeRTOSConfig.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/FreeRTOSConfig.h @@ -1,75 +1,74 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ /** - * @addtogroup PIOS PIOS - * @{ - * @addtogroup FreeRTOS FreeRTOS - * @{ - */ + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ /* Notes: We use 5 task priorities */ -#define configUSE_PREEMPTION 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configUSE_MALLOC_FAILED_HOOK 1 -#define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 ) -#define configTICK_RATE_HZ ( ( portTickType ) 1000 ) -#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) -#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 48 ) -#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 54 * 256) ) -#define configMAX_TASK_NAME_LEN ( 16 ) -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configIDLE_SHOULD_YIELD 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_MALLOC_FAILED_HOOK 1 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)48) +#define configTOTAL_HEAP_SIZE ((size_t)(54 * 256)) +#define configMAX_TASK_NAME_LEN (16) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configQUEUE_REGISTRY_SIZE 10 /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ + to exclude the API function. */ -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 0 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) #define CHECK_IRQ_STACK @@ -77,22 +76,23 @@ NVIC value of 255. */ /* Enable run time stats collection */ #ifdef DIAG_TASKS -#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 -#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\ -do {\ -(*(unsigned long *)0xe000edfc) |= (1<<24);/* DEMCR |= DEMCR_TRCENA */\ -(*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\ -} while(0) -#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */ +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ #else -#define configCHECK_FOR_STACK_OVERFLOW 1 +#define configCHECK_FOR_STACK_OVERFLOW 1 #endif /** - * @} - */ + * @} + */ #endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h b/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h index 8c76835cd..3cceef152 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h @@ -10,19 +10,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_board_posix.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_board_posix.h index 84eadbeba..16364d7fa 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_board_posix.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_board_posix.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file pios_board.h + * @file pios_board.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -28,57 +28,54 @@ #define PIOS_BOARD_H - - -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -//#define PIOS_LED_LED1_GPIO_PORT GPIOC -//#define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12 -//#define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC -//#define PIOS_LED_LED2_GPIO_PORT GPIOC -//#define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13 -//#define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC -#define PIOS_LED_NUM 2 -//#define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT } -//#define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN } -//#define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK } +// ------------------------ +// #define PIOS_LED_LED1_GPIO_PORT GPIOC +// #define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12 +// #define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC +// #define PIOS_LED_LED2_GPIO_PORT GPIOC +// #define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13 +// #define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC +#define PIOS_LED_NUM 2 +// #define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT } +// #define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN } +// #define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK } -//------------------------- +// ------------------------- // COM // // See also pios_board_posix.c -//------------------------- -//#define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +// ------------------------- +// #define PIOS_USART_TX_BUFFER_SIZE 256 +#define PIOS_COM_BUFFER_SIZE 1024 +#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_COM_TELEM_RF 0 -#define PIOS_COM_GPS 1 -#define PIOS_COM_TELEM_USB 2 +#define PIOS_COM_TELEM_RF 0 +#define PIOS_COM_GPS 1 +#define PIOS_COM_TELEM_USB 2 #ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX 3 -#define PIOS_COM_DEBUG PIOS_COM_AUX +#define PIOS_COM_AUX 3 +#define PIOS_COM_DEBUG PIOS_COM_AUX #endif /** * glue macros for file IO * STM32 uses DOSFS for file IO */ -#define PIOS_FOPEN_READ(filename,file) (file=fopen((char*)filename,"r"))==NULL +#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL -#define PIOS_FOPEN_WRITE(filename,file) (file=fopen((char*)filename,"w"))==NULL +#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL -#define PIOS_FREAD(file,bufferadr,length,resultadr) (*resultadr=fread((uint8_t*)bufferadr,1,length,*file)) != length +#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length -#define PIOS_FWRITE(file,bufferadr,length,resultadr) *resultadr=fwrite((uint8_t*)bufferadr,1,length,*file) +#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) +#define PIOS_FCLOSE(file) fclose(file) -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char*)filename) +#define PIOS_FUNLINK(file) unlink((char *)filename) #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h index 5886c021e..69ab3ee05 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h @@ -110,7 +110,7 @@ #define PIOS_INCLUDE_FLASH_INTERNAL #define PIOS_INCLUDE_FLASH_LOGFS_SETTINGS /* #define FLASH_FREERTOS */ -//#define PIOS_INCLUDE_FLASH_EEPROM +// #define PIOS_INCLUDE_FLASH_EEPROM /* PIOS radio modules */ #define PIOS_INCLUDE_RFM22B @@ -142,26 +142,26 @@ /* #define PIOS_QUATERNION_STABILIZATION */ /* Performance counters */ -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 /* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 220 -#define HEAP_LIMIT_CRITICAL 40 -#define IRQSTACK_LIMIT_WARNING 100 -#define IRQSTACK_LIMIT_CRITICAL 60 -#define CPULOAD_LIMIT_WARNING 85 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 220 +#define HEAP_LIMIT_CRITICAL 40 +#define IRQSTACK_LIMIT_WARNING 100 +#define IRQSTACK_LIMIT_CRITICAL 60 +#define CPULOAD_LIMIT_WARNING 85 +#define CPULOAD_LIMIT_CRITICAL 95 /* Task stack sizes */ -#define PIOS_ACTUATOR_STACK_SIZE 1020 -#define PIOS_MANUAL_STACK_SIZE 724 -#define PIOS_SYSTEM_STACK_SIZE 460 -#define PIOS_STABILIZATION_STACK_SIZE 524 -#define PIOS_TELEM_STACK_SIZE 500 -#define PIOS_EVENTDISPATCHER_STACK_SIZE 130 +#define PIOS_ACTUATOR_STACK_SIZE 1020 +#define PIOS_MANUAL_STACK_SIZE 724 +#define PIOS_SYSTEM_STACK_SIZE 460 +#define PIOS_STABILIZATION_STACK_SIZE 524 +#define PIOS_TELEM_STACK_SIZE 500 +#define PIOS_EVENTDISPATCHER_STACK_SIZE 130 /* This can't be too high to stop eventdispatcher thread overflowing */ -#define PIOS_EVENTDISAPTCHER_QUEUE 10 +#define PIOS_EVENTDISAPTCHER_QUEUE 10 #endif /* PIOS_CONFIG_H */ /** diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h index 5e744a381..ecedcf3ce 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,14 +41,14 @@ /* Defaults for Logging */ -#define LOG_FILENAME "PIOS.LOG" -#define STARTUP_LOG_ENABLED 1 +#define LOG_FILENAME "PIOS.LOG" +#define STARTUP_LOG_ENABLED 1 /* COM Module */ -#define GPS_BAUDRATE 19200 -#define TELEM_BAUDRATE 19200 -#define AUXUART_ENABLED 0 -#define AUXUART_BAUDRATE 19200 +#define GPS_BAUDRATE 19200 +#define TELEM_BAUDRATE 19200 +#define AUXUART_ENABLED 0 +#define AUXUART_BAUDRATE 19200 #endif /* PIOS_CONFIG_POSIX_H */ diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_usb_board_data.h index 4ce97dfcc..736a7dbca 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_usb_board_data.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_usb_board_data.h @@ -35,12 +35,12 @@ #define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 4 +#define PIOS_USB_BOARD_EP_NUM 4 -#include /* USB_* macros */ +#include /* USB_* macros */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OPLINK -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OPLINK, USB_OP_BOARD_MODE_FW) -#define PIOS_USB_BOARD_SN_SUFFIX "+FW" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OPLINK +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OPLINK, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/oplinkmini/firmware/oplink.c b/flight/targets/boards/oplinkmini/firmware/oplink.c index dafbbacc3..07d4fa551 100644 --- a/flight/targets/boards/oplinkmini/firmware/oplink.c +++ b/flight/targets/boards/oplinkmini/firmware/oplink.c @@ -9,25 +9,25 @@ * @brief This is where the OP firmware starts. Those files also define the compile-time * options of the firmware. * @{ - * @file openpilot.c + * @file openpilot.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Sets up and runs main OpenPilot tasks. * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,7 +36,7 @@ #include /* Task Priorities */ -#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) /* Global Variables */ @@ -45,46 +45,45 @@ extern void PIOS_Board_Init(void); extern void Stack_Change(void); /** -* OpenPilot Main function: -* -* Initialize PiOS
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
-* Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) -* If something goes wrong, blink LED1 and LED2 every 100ms -* -*/ + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) + * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ int main() { - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ - /* Brings up System using CMSIS functions, enables the LEDs. */ - PIOS_SYS_Init(); + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); - /* Architecture dependant Hardware and - * core subsystem initialisation - * (see pios_board.c for your arch) - * */ - PIOS_Board_Init(); + /* Architecture dependant Hardware and + * core subsystem initialisation + * (see pios_board.c for your arch) + * */ + PIOS_Board_Init(); - /* Initialize modules */ - MODULE_INITIALISE_ALL + /* Initialize modules */ + MODULE_INITIALISE_ALL + /* swap the stack to use the IRQ stack */ + Stack_Change(); - /* swap the stack to use the IRQ stack */ - Stack_Change(); + /* Start the FreeRTOS scheduler which should never returns.*/ + vTaskStartScheduler(); - /* Start the FreeRTOS scheduler which should never returns.*/ - vTaskStartScheduler(); + /* If all is well we will never reach here as the scheduler will now be running. */ - /* If all is well we will never reach here as the scheduler will now be running. */ - - /* Do some indication to user that something bad just happened */ - while (1) { + /* Do some indication to user that something bad just happened */ + while (1) { #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ - PIOS_DELAY_WaitmS(100); - } + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + PIOS_DELAY_WaitmS(100); + } return 0; } @@ -93,4 +92,3 @@ int main() * @} * @} */ - diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index e7aabf709..aa90b4132 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -1,29 +1,29 @@ /** -****************************************************************************** -* @addtogroup OpenPilotSystem OpenPilot System -* @{ -* @addtogroup OpenPilotCore OpenPilot Core -* @{ -* -* @file pios_board.c -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. -* @brief Defines board specific static initializers for hardware for the OpenPilot board. -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * + * @file pios_board.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Defines board specific static initializers for hardware for the OpenPilot board. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -51,8 +51,8 @@ #define PIOS_COM_RFM22B_RF_RX_BUF_LEN 256 #define PIOS_COM_RFM22B_RF_TX_BUF_LEN 256 -#define PIOS_COM_TELEM_RX_BUF_LEN 256 -#define PIOS_COM_TELEM_TX_BUF_LEN 256 +#define PIOS_COM_TELEM_RX_BUF_LEN 256 +#define PIOS_COM_TELEM_TX_BUF_LEN 256 uint32_t pios_com_telem_usb_id = 0; uint32_t pios_com_telem_vcp_id = 0; @@ -60,15 +60,15 @@ uint32_t pios_com_telem_uart_main_id = 0; uint32_t pios_com_telem_uart_flexi_id = 0; uint32_t pios_com_telemetry_id = 0; #if defined(PIOS_INCLUDE_PPM) -uint32_t pios_ppm_rcvr_id = 0; +uint32_t pios_ppm_rcvr_id = 0; #endif #if defined(PIOS_INCLUDE_PPM_OUT) -uint32_t pios_ppm_out_id = 0; +uint32_t pios_ppm_out_id = 0; #endif #if defined(PIOS_INCLUDE_RFM22B) -uint32_t pios_rfm22b_id = 0; +uint32_t pios_rfm22b_id = 0; uint32_t pios_com_rfm22b_id = 0; -uint32_t pios_com_radio_id = 0; +uint32_t pios_com_radio_id = 0; #endif uint8_t *pios_uart_rx_buffer; uint8_t *pios_uart_tx_buffer; @@ -80,10 +80,10 @@ uintptr_t pios_uavo_settings_fs_id; * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ -void PIOS_Board_Init(void) { - - /* Delay system */ - PIOS_DELAY_Init(); +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); #ifdef PIOS_INCLUDE_FLASH_LOGFS_SETTINGS uintptr_t flash_id; @@ -91,235 +91,234 @@ void PIOS_Board_Init(void) { PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id); #endif - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - - /* Set up the SPI interface to the rfm22b */ - if (PIOS_SPI_Init(&pios_spi_rfm22b_id, &pios_spi_rfm22b_cfg)) { - PIOS_DEBUG_Assert(0); - } + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + + /* Set up the SPI interface to the rfm22b */ + if (PIOS_SPI_Init(&pios_spi_rfm22b_id, &pios_spi_rfm22b_cfg)) { + PIOS_DEBUG_Assert(0); + } #ifdef PIOS_INCLUDE_WDG - /* Initialize watchdog as early as possible to catch faults during init */ - PIOS_WDG_Init(); + /* Initialize watchdog as early as possible to catch faults during init */ + PIOS_WDG_Init(); #endif /* PIOS_INCLUDE_WDG */ #if defined(PIOS_INCLUDE_RTC) - /* Initialize the real-time clock and its associated tick */ - PIOS_RTC_Init(&pios_rtc_main_cfg); + /* Initialize the real-time clock and its associated tick */ + PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* PIOS_INCLUDE_RTC */ - OPLinkSettingsInitialize(); + OPLinkSettingsInitialize(); #if defined(PIOS_INCLUDE_LED) - PIOS_LED_Init(&pios_led_cfg); -#endif /* PIOS_INCLUDE_LED */ + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ - OPLinkSettingsData oplinkSettings; + OPLinkSettingsData oplinkSettings; - /* IAP System Setup */ - PIOS_IAP_Init(); - // check for safe mode commands from gcs - if(PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && - PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && - PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { - OPLinkSettingsGet(&oplinkSettings); - OPLinkSettingsSetDefaults(&oplinkSettings,0); - OPLinkSettingsSet(&oplinkSettings); - //PIOS_EEPROM_Save((uint8_t*)&oplinkSettings, sizeof(OPLinkSettingsData)); - for (uint32_t i = 0; i < 10; i++) { - PIOS_DELAY_WaitmS(100); - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); - } - PIOS_IAP_WriteBootCmd(0,0); - PIOS_IAP_WriteBootCmd(1,0); - PIOS_IAP_WriteBootCmd(2,0); - } - OPLinkSettingsGet(&oplinkSettings); + /* IAP System Setup */ + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + OPLinkSettingsGet(&oplinkSettings); + OPLinkSettingsSetDefaults(&oplinkSettings, 0); + OPLinkSettingsSet(&oplinkSettings); + // PIOS_EEPROM_Save((uint8_t*)&oplinkSettings, sizeof(OPLinkSettingsData)); + for (uint32_t i = 0; i < 10; i++) { + PIOS_DELAY_WaitmS(100); + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); + } + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } + OPLinkSettingsGet(&oplinkSettings); - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); #if defined(PIOS_INCLUDE_TIM) - /* Set up pulse timers */ - PIOS_TIM_InitClock(&tim_1_cfg); - PIOS_TIM_InitClock(&tim_2_cfg); - PIOS_TIM_InitClock(&tim_3_cfg); - PIOS_TIM_InitClock(&tim_4_cfg); -#endif /* PIOS_INCLUDE_TIM */ + /* Set up pulse timers */ + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_4_cfg); +#endif /* PIOS_INCLUDE_TIM */ - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); #if defined(PIOS_INCLUDE_USB_CDC) - /* Flags to determine if various USB interfaces are advertised */ - bool usb_cdc_present = false; + /* Flags to determine if various USB interfaces are advertised */ + bool usb_cdc_present = false; - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_cdc_present = true; + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } + usb_cdc_present = true; #else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } #endif - /*Initialize the USB device */ - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + /*Initialize the USB device */ + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); - /* Configure the USB HID port */ - { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + /* Configure the USB HID port */ + { + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } - /* Configure the USB virtual com port (VCP) */ + /* Configure the USB virtual com port (VCP) */ #if defined(PIOS_INCLUDE_USB_CDC) - if (usb_cdc_present) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_VCP_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_VCP_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_VCP_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_VCP_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + if (usb_cdc_present) { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_VCP_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_VCP_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + rx_buffer, PIOS_COM_TELEM_VCP_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_VCP_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } #endif - /* Initalize the RFM22B radio COM device. */ + /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) - { - const struct pios_board_info * bdinfo = &pios_board_info_blob; - const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); - if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { - PIOS_Assert(0); - } + { + const struct pios_board_info *bdinfo = &pios_board_info_blob; + const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); + if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { + PIOS_Assert(0); + } - uint8_t *rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_rfm22b_id, &pios_rfm22b_com_driver, pios_rfm22b_id, - rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_rfm22b_id, &pios_rfm22b_com_driver, pios_rfm22b_id, + rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } - /* Set the RFM22B bindings. */ - PIOS_RFM22B_SetBindings(pios_rfm22b_id, oplinkSettings.Bindings, oplinkSettings.RemoteMainPort, - oplinkSettings.RemoteFlexiPort, oplinkSettings.RemoteVCPPort, oplinkSettings.ComSpeed); - } + /* Set the RFM22B bindings. */ + PIOS_RFM22B_SetBindings(pios_rfm22b_id, oplinkSettings.Bindings, oplinkSettings.RemoteMainPort, + oplinkSettings.RemoteFlexiPort, oplinkSettings.RemoteVCPPort, oplinkSettings.ComSpeed); + } #endif /* PIOS_INCLUDE_RFM22B */ - /* Allocate the uart buffers. */ - pios_uart_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); - pios_uart_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); + /* Allocate the uart buffers. */ + pios_uart_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); + pios_uart_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); - /* Remap AFIO pin */ - GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); + /* Remap AFIO pin */ + GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE); #ifdef PIOS_INCLUDE_ADC - PIOS_ADC_Init(); + PIOS_ADC_Init(); #endif - PIOS_GPIO_Init(); + PIOS_GPIO_Init(); } void PIOS_InitUartMainPort() { #ifndef PIOS_RFM22B_DEBUG_ON_TELEM - uint32_t pios_usart1_id; - if (PIOS_USART_Init(&pios_usart1_id, &pios_usart_serial_cfg)) { - PIOS_Assert(0); - } - PIOS_Assert(pios_uart_rx_buffer); - PIOS_Assert(pios_uart_tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_uart_main_id, &pios_usart_com_driver, pios_usart1_id, - pios_uart_rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, - pios_uart_tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { - PIOS_Assert(0); - } + uint32_t pios_usart1_id; + if (PIOS_USART_Init(&pios_usart1_id, &pios_usart_serial_cfg)) { + PIOS_Assert(0); + } + PIOS_Assert(pios_uart_rx_buffer); + PIOS_Assert(pios_uart_tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_uart_main_id, &pios_usart_com_driver, pios_usart1_id, + pios_uart_rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, + pios_uart_tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { + PIOS_Assert(0); + } #endif } void PIOS_InitUartFlexiPort() { - uint32_t pios_usart3_id; - if (PIOS_USART_Init(&pios_usart3_id, &pios_usart_telem_flexi_cfg)) { - PIOS_Assert(0); - } - PIOS_Assert(pios_uart_rx_buffer); - PIOS_Assert(pios_uart_tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_uart_flexi_id, &pios_usart_com_driver, pios_usart3_id, - pios_uart_rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, - pios_uart_tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { - PIOS_Assert(0); - } + uint32_t pios_usart3_id; + + if (PIOS_USART_Init(&pios_usart3_id, &pios_usart_telem_flexi_cfg)) { + PIOS_Assert(0); + } + PIOS_Assert(pios_uart_rx_buffer); + PIOS_Assert(pios_uart_tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_uart_flexi_id, &pios_usart_com_driver, pios_usart3_id, + pios_uart_rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, + pios_uart_tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { + PIOS_Assert(0); + } } void PIOS_InitPPMMainPort(bool input) { #if defined(PIOS_INCLUDE_PPM) - /* PPM input is configured on the coordinator modem and output on the remote modem. */ - if (input) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_main_cfg); + /* PPM input is configured on the coordinator modem and output on the remote modem. */ + if (input) { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_main_cfg); - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) - PIOS_Assert(0); - } - // For some reason, PPM output on the main port doesn't work. -#endif /* PIOS_INCLUDE_PPM */ + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + } + // For some reason, PPM output on the main port doesn't work. +#endif /* PIOS_INCLUDE_PPM */ } void PIOS_InitPPMFlexiPort(bool input) { #if defined(PIOS_INCLUDE_PPM) - /* PPM input is configured on the coordinator modem and output on the remote modem. */ - if (input) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); + /* PPM input is configured on the coordinator modem and output on the remote modem. */ + if (input) { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) - PIOS_Assert(0); - } + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + } #if defined(PIOS_INCLUDE_PPM_OUT) - else - { - PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_ppm_out_cfg); - } -#endif /* PIOS_INCLUDE_PPM_OUT */ -#endif /* PIOS_INCLUDE_PPM */ + else { + PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_ppm_out_cfg); + } +#endif /* PIOS_INCLUDE_PPM_OUT */ +#endif /* PIOS_INCLUDE_PPM */ } /** diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c b/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c index f63cfa7fd..5750bed21 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c @@ -7,19 +7,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,43 +33,42 @@ * initializes all the core systems on this specific hardware * called from System/openpilot.c */ -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Initialize the alarms library */ - AlarmsInitialize(); + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } - - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); - - /* Initialize the PiOS library */ - PIOS_COM_Init(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + /* Initialize the PiOS library */ + PIOS_COM_Init(); } const struct pios_udp_cfg pios_udp0_cfg = { - .ip = "0.0.0.0", - .port = 9000, + .ip = "0.0.0.0", + .port = 9000, }; const struct pios_udp_cfg pios_udp1_cfg = { - .ip = "0.0.0.0", - .port = 9001, + .ip = "0.0.0.0", + .port = 9001, }; const struct pios_udp_cfg pios_udp2_cfg = { - .ip = "0.0.0.0", - .port = 9002, + .ip = "0.0.0.0", + .port = 9002, }; #ifdef PIOS_COM_AUX @@ -77,8 +76,8 @@ const struct pios_udp_cfg pios_udp2_cfg = { * AUX USART */ const struct pios_udp_cfg pios_udp3_cfg = { - .ip = "0.0.0.0", - .port = 9003, + .ip = "0.0.0.0", + .port = 9003, }; #endif @@ -86,23 +85,23 @@ const struct pios_udp_cfg pios_udp3_cfg = { * Board specific number of devices. */ struct pios_udp_dev pios_udp_devs[] = { -#define PIOS_UDP_TELEM 0 - { - .cfg = &pios_udp0_cfg, - }, -#define PIOS_UDP_GPS 1 - { - .cfg = &pios_udp1_cfg, - }, -#define PIOS_UDP_LOCAL 2 - { - .cfg = &pios_udp2_cfg, - }, +#define PIOS_UDP_TELEM 0 + { + .cfg = &pios_udp0_cfg, + }, +#define PIOS_UDP_GPS 1 + { + .cfg = &pios_udp1_cfg, + }, +#define PIOS_UDP_LOCAL 2 + { + .cfg = &pios_udp2_cfg, + }, #ifdef PIOS_COM_AUX -#define PIOS_UDP_AUX 3 - { - .cfg = &pios_udp3_cfg, - }, +#define PIOS_UDP_AUX 3 + { + .cfg = &pios_udp3_cfg, + }, #endif }; @@ -119,23 +118,23 @@ extern const struct pios_com_driver pios_serial_com_driver; extern const struct pios_com_driver pios_udp_com_driver; struct pios_com_dev pios_com_devs[] = { - { - .id = PIOS_UDP_TELEM, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_GPS, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_LOCAL, - .driver = &pios_udp_com_driver, - }, + { + .id = PIOS_UDP_TELEM, + .driver = &pios_udp_com_driver, + }, + { + .id = PIOS_UDP_GPS, + .driver = &pios_udp_com_driver, + }, + { + .id = PIOS_UDP_LOCAL, + .driver = &pios_udp_com_driver, + }, #ifdef PIOS_COM_AUX - { - .id = PIOS_UDP_AUX, - .driver = &pios_udp_com_driver, - }, + { + .id = PIOS_UDP_AUX, + .driver = &pios_udp_com_driver, + }, #endif }; diff --git a/flight/targets/boards/oplinkmini/pios_board.h b/flight/targets/boards/oplinkmini/pios_board.h index 786508844..bc0a9f321 100644 --- a/flight/targets/boards/oplinkmini/pios_board.h +++ b/flight/targets/boards/oplinkmini/pios_board.h @@ -1,4 +1,4 @@ - /** +/** ****************************************************************************** * * @file pios_board.h @@ -28,150 +28,150 @@ #define ADD_ONE_ADC -//------------------------ +// ------------------------ // Timers and Channels Used -//------------------------ +// ------------------------ /* -Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 -------+-----------+-----------+-----------+---------- -TIM1 | Servo 4 | | | -TIM2 | RC In 5 | RC In 6 | Servo 6 | -TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 -TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 -------+-----------+-----------+-----------+---------- -*/ + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | Servo 4 | | | + TIM2 | RC In 5 | RC In 6 | Servo 6 | + TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 + TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 + ------+-----------+-----------+-----------+---------- + */ -//------------------------ +// ------------------------ // DMA Channels Used -//------------------------ -/* Channel 1 - */ -/* Channel 2 - */ -/* Channel 3 - */ -/* Channel 4 - */ -/* Channel 5 - */ -/* Channel 6 - */ -/* Channel 7 - */ -/* Channel 8 - */ -/* Channel 9 - */ -/* Channel 10 - */ -/* Channel 11 - */ -/* Channel 12 - */ +// ------------------------ +/* Channel 1 - */ +/* Channel 2 - */ +/* Channel 3 - */ +/* Channel 4 - */ +/* Channel 5 - */ +/* Channel 6 - */ +/* Channel 7 - */ +/* Channel 8 - */ +/* Channel 9 - */ +/* Channel 10 - */ +/* Channel 11 - */ +/* Channel 12 - */ -//------------------------ +// ------------------------ // BOOTLOADER_SETTINGS -//------------------------ -#define BOARD_READABLE TRUE -#define BOARD_WRITABLE TRUE -#define MAX_DEL_RETRYS 3 +// ------------------------ +#define BOARD_READABLE TRUE +#define BOARD_WRITABLE TRUE +#define MAX_DEL_RETRYS 3 -//------------------------ +// ------------------------ // WATCHDOG_SETTINGS -//------------------------ -#define PIOS_WATCHDOG_TIMEOUT 500 -#define PIOS_WDG_REGISTER BKP_DR4 -#define PIOS_WDG_TELEMETRYTX 0x0001 -#define PIOS_WDG_TELEMETRYRX 0x0002 -#define PIOS_WDG_RADIOTX 0x0004 -#define PIOS_WDG_RADIORX 0x0008 -#define PIOS_WDG_RFM22B 0x0016 +// ------------------------ +#define PIOS_WATCHDOG_TIMEOUT 500 +#define PIOS_WDG_REGISTER BKP_DR4 +#define PIOS_WDG_TELEMETRYTX 0x0001 +#define PIOS_WDG_TELEMETRYRX 0x0002 +#define PIOS_WDG_RADIOTX 0x0004 +#define PIOS_WDG_RADIORX 0x0008 +#define PIOS_WDG_RFM22B 0x0016 -//------------------------ +// ------------------------ // TELEMETRY -//------------------------ -#define TELEM_QUEUE_SIZE 20 +// ------------------------ +#define TELEM_QUEUE_SIZE 20 -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -#define PIOS_LED_USB 0 -#define PIOS_LED_LINK 1 -#define PIOS_LED_RX 2 -#define PIOS_LED_TX 3 +// ------------------------ +#define PIOS_LED_USB 0 +#define PIOS_LED_LINK 1 +#define PIOS_LED_RX 2 +#define PIOS_LED_TX 3 #ifdef PIOS_RFM22B_DEBUG_ON_TELEM -#define PIOS_LED_D1 4 -#define PIOS_LED_D2 5 -#define PIOS_LED_D3 6 -#define PIOS_LED_D4 7 +#define PIOS_LED_D1 4 +#define PIOS_LED_D2 5 +#define PIOS_LED_D3 6 +#define PIOS_LED_D4 7 #endif -#define PIOS_LED_HEARTBEAT PIOS_LED_LINK -#define PIOS_LED_ALARM PIOS_LED_TX +#define PIOS_LED_HEARTBEAT PIOS_LED_LINK +#define PIOS_LED_ALARM PIOS_LED_TX -#define USB_LED_ON PIOS_LED_On(PIOS_LED_USB) -#define USB_LED_OFF PIOS_LED_Off(PIOS_LED_USB) -#define USB_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_USB) +#define USB_LED_ON PIOS_LED_On(PIOS_LED_USB) +#define USB_LED_OFF PIOS_LED_Off(PIOS_LED_USB) +#define USB_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_USB) -#define LINK_LED_ON PIOS_LED_On(PIOS_LED_LINK) -#define LINK_LED_OFF PIOS_LED_Off(PIOS_LED_LINK) -#define LINK_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_LINK) +#define LINK_LED_ON PIOS_LED_On(PIOS_LED_LINK) +#define LINK_LED_OFF PIOS_LED_Off(PIOS_LED_LINK) +#define LINK_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_LINK) -#define RX_LED_ON PIOS_LED_On(PIOS_LED_RX) -#define RX_LED_OFF PIOS_LED_Off(PIOS_LED_RX) -#define RX_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_RX) +#define RX_LED_ON PIOS_LED_On(PIOS_LED_RX) +#define RX_LED_OFF PIOS_LED_Off(PIOS_LED_RX) +#define RX_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_RX) -#define TX_LED_ON PIOS_LED_On(PIOS_LED_TX) -#define TX_LED_OFF PIOS_LED_Off(PIOS_LED_TX) -#define TX_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_TX) +#define TX_LED_ON PIOS_LED_On(PIOS_LED_TX) +#define TX_LED_OFF PIOS_LED_Off(PIOS_LED_TX) +#define TX_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_TX) #ifdef PIOS_RFM22B_DEBUG_ON_TELEM -#define D1_LED_ON PIOS_LED_On(PIOS_LED_D1) -#define D1_LED_OFF PIOS_LED_Off(PIOS_LED_D1) -#define D1_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D1) +#define D1_LED_ON PIOS_LED_On(PIOS_LED_D1) +#define D1_LED_OFF PIOS_LED_Off(PIOS_LED_D1) +#define D1_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D1) -#define D2_LED_ON PIOS_LED_On(PIOS_LED_D2) -#define D2_LED_OFF PIOS_LED_Off(PIOS_LED_D2) -#define D2_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D2) +#define D2_LED_ON PIOS_LED_On(PIOS_LED_D2) +#define D2_LED_OFF PIOS_LED_Off(PIOS_LED_D2) +#define D2_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D2) -#define D3_LED_ON PIOS_LED_On(PIOS_LED_D3) -#define D3_LED_OFF PIOS_LED_Off(PIOS_LED_D3) -#define D3_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D3) +#define D3_LED_ON PIOS_LED_On(PIOS_LED_D3) +#define D3_LED_OFF PIOS_LED_Off(PIOS_LED_D3) +#define D3_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D3) -#define D4_LED_ON PIOS_LED_On(PIOS_LED_D4) -#define D4_LED_OFF PIOS_LED_Off(PIOS_LED_D4) -#define D4_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D4) +#define D4_LED_ON PIOS_LED_On(PIOS_LED_D4) +#define D4_LED_OFF PIOS_LED_Off(PIOS_LED_D4) +#define D4_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D4) #endif -//------------------------- +// ------------------------- // System Settings -//------------------------- -#define PIOS_MASTER_CLOCK 72000000 -#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) +// ------------------------- +#define PIOS_MASTER_CLOCK 72000000 +#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) -//------------------------- +// ------------------------- // Interrupt Priorities -//------------------------- -#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS -#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS -#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... -#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... -//------------------------ +// ------------------------ // PIOS_I2C // See also pios_board.c -//------------------------ -#define PIOS_I2C_MAX_DEVS 1 +// ------------------------ +#define PIOS_I2C_MAX_DEVS 1 extern uint32_t pios_i2c_flexi_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexi_adapter_id) +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexi_adapter_id) -//------------------------ +// ------------------------ // PIOS_SPI // See also pios_board.c -//------------------------ -#define PIOS_SPI_MAX_DEVS 1 +// ------------------------ +#define PIOS_SPI_MAX_DEVS 1 -//------------------------- +// ------------------------- // PIOS_USART -//------------------------- -#define PIOS_USART_MAX_DEVS 3 +// ------------------------- +#define PIOS_USART_MAX_DEVS 3 -//------------------------- +// ------------------------- // PIOS_COM // // See also pios_board.c -//------------------------- -#define PIOS_COM_MAX_DEVS 5 +// ------------------------- +#define PIOS_COM_MAX_DEVS 5 extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_telem_vcp_id; @@ -182,135 +182,135 @@ extern uint32_t pios_com_rfm22b_id; extern uint32_t pios_com_radio_id; extern uint32_t pios_ppm_rcvr_id; extern uint32_t pios_ppm_out_id; -#define PIOS_COM_TELEM_USB_HID (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_USB PIOS_COM_TELEM_USB_HID -#define PIOS_COM_TELEM_USB_VCP (pios_com_telem_vcp_id) -#define PIOS_COM_TELEM_UART_MAIN (pios_com_telem_uart_main_id) -#define PIOS_COM_TELEM_UART_FLEXI (pios_com_telem_uart_flexi_id) -#define PIOS_COM_TELEMETRY (pios_com_telemetry_id) -#define PIOS_COM_RFM22B (pios_com_rfm22b_id) -#define PIOS_COM_RADIO (pios_com_radio_id) -#define PIOS_PPM_RECEIVER (pios_ppm_rcvr_id) -#define PIOS_PPM_OUTPUT (pios_ppm_out_id) +#define PIOS_COM_TELEM_USB_HID (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_USB PIOS_COM_TELEM_USB_HID +#define PIOS_COM_TELEM_USB_VCP (pios_com_telem_vcp_id) +#define PIOS_COM_TELEM_UART_MAIN (pios_com_telem_uart_main_id) +#define PIOS_COM_TELEM_UART_FLEXI (pios_com_telem_uart_flexi_id) +#define PIOS_COM_TELEMETRY (pios_com_telemetry_id) +#define PIOS_COM_RFM22B (pios_com_rfm22b_id) +#define PIOS_COM_RADIO (pios_com_radio_id) +#define PIOS_PPM_RECEIVER (pios_ppm_rcvr_id) +#define PIOS_PPM_OUTPUT (pios_ppm_out_id) -#define RFM22_DEBUG 1 +#define RFM22_DEBUG 1 -//------------------------- +// ------------------------- // ADC // None -//------------------------- -//#define PIOS_ADC_OVERSAMPLING_RATE 1 -#define PIOS_ADC_USE_TEMP_SENSOR 0 -#define PIOS_ADC_TEMP_SENSOR_ADC ADC1 -#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 1 +// ------------------------- +// #define PIOS_ADC_OVERSAMPLING_RATE 1 +#define PIOS_ADC_USE_TEMP_SENSOR 0 +#define PIOS_ADC_TEMP_SENSOR_ADC ADC1 +#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 1 -#define PIOS_ADC_NUM_PINS 0 +#define PIOS_ADC_NUM_PINS 0 -#define PIOS_ADC_PORTS { } -#define PIOS_ADC_PINS { } -#define PIOS_ADC_CHANNELS { } -#define PIOS_ADC_MAPPING { } -#define PIOS_ADC_CHANNEL_MAPPING { } -#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR) -#define PIOS_ADC_NUM_ADC_CHANNELS 0 -#define PIOS_ADC_USE_ADC2 0 -#define PIOS_ADC_CLOCK_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE) -#define PIOS_ADC_ADCCLK RCC_PCLK2_Div8 +#define PIOS_ADC_PORTS {} +#define PIOS_ADC_PINS {} +#define PIOS_ADC_CHANNELS {} +#define PIOS_ADC_MAPPING {} +#define PIOS_ADC_CHANNEL_MAPPING {} +#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR) +#define PIOS_ADC_NUM_ADC_CHANNELS 0 +#define PIOS_ADC_USE_ADC2 0 +#define PIOS_ADC_CLOCK_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE) +#define PIOS_ADC_ADCCLK RCC_PCLK2_Div8 /* RCC_PCLK2_Div2: ADC clock = PCLK2/2 */ /* RCC_PCLK2_Div4: ADC clock = PCLK2/4 */ /* RCC_PCLK2_Div6: ADC clock = PCLK2/6 */ /* RCC_PCLK2_Div8: ADC clock = PCLK2/8 */ -#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_239Cycles5 +#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_239Cycles5 /* Sample time: */ /* With an ADCCLK = 14 MHz and a sampling time of 239.5 cycles: */ /* Tconv = 239.5 + 12.5 = 252 cycles = 18�s */ /* (1 / (ADCCLK / CYCLES)) = Sample Time (�S) */ -#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_LOW +#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_LOW // Currently analog acquistion hard coded at 480 Hz // PCKL2 = HCLK / 16 // ADCCLK = PCLK2 / 2 -#define PIOS_ADC_RATE (72.0e6 / 1.0 / 8.0 / 252.0 / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) -#define PIOS_ADC_MAX_OVERSAMPLING 36 +#define PIOS_ADC_RATE (72.0e6 / 1.0 / 8.0 / 252.0 / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) +#define PIOS_ADC_MAX_OVERSAMPLING 36 -//------------------------ +// ------------------------ // PIOS_RCVR // See also pios_board.c -//------------------------ -#define PIOS_RCVR_MAX_DEVS 3 -#define PIOS_RCVR_MAX_CHANNELS 12 -#define PIOS_GCSRCVR_TIMEOUT_MS 100 +// ------------------------ +#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_GCSRCVR_TIMEOUT_MS 100 -//------------------------- +// ------------------------- // Receiver PPM input -//------------------------- -#define PIOS_PPM_MAX_DEVS 1 -#define PIOS_PPM_NUM_INPUTS 8 +// ------------------------- +#define PIOS_PPM_MAX_DEVS 1 +#define PIOS_PPM_NUM_INPUTS 8 -//------------------------- +// ------------------------- // Servo outputs -//------------------------- -#define PIOS_SERVO_UPDATE_HZ 50 -#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ -//-------------------------- +// -------------------------- // Timer controller settings -//-------------------------- -#define PIOS_TIM_MAX_DEVS 3 +// -------------------------- +#define PIOS_TIM_MAX_DEVS 3 -//------------------------- +// ------------------------- // GPIO -//------------------------- -#define PIOS_GPIO_PORTS { } -#define PIOS_GPIO_PINS { } -#define PIOS_GPIO_CLKS { } -#define PIOS_GPIO_NUM 0 +// ------------------------- +#define PIOS_GPIO_PORTS {} +#define PIOS_GPIO_PINS {} +#define PIOS_GPIO_CLKS {} +#define PIOS_GPIO_NUM 0 -//------------------------- +// ------------------------- // USB -//------------------------- -#define PIOS_USB_HID_MAX_DEVS 1 -#define PIOS_USB_ENABLED 1 -#define PIOS_USB_HID_MAX_DEVS 1 -#define PIOS_USB_MAX_DEVS 1 -#define PIOS_USB_DETECT_GPIO_PORT GPIOC -#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 -#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line15 +// ------------------------- +#define PIOS_USB_HID_MAX_DEVS 1 +#define PIOS_USB_ENABLED 1 +#define PIOS_USB_HID_MAX_DEVS 1 +#define PIOS_USB_MAX_DEVS 1 +#define PIOS_USB_DETECT_GPIO_PORT GPIOC +#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 +#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line15 -//------------------------- +// ------------------------- // RFM22 -//------------------------- +// ------------------------- #if defined(PIOS_INCLUDE_RFM22B) extern uint32_t pios_spi_rfm22b_id; -#define PIOS_RFM22_SPI_PORT (pios_spi_rfm22b_id) +#define PIOS_RFM22_SPI_PORT (pios_spi_rfm22b_id) extern uint32_t pios_rfm22b_id; #endif /* PIOS_INCLUDE_RFM22B */ -//------------------------- +// ------------------------- // Packet Handler -//------------------------- +// ------------------------- extern uint32_t pios_packet_handler; -#define PIOS_PACKET_HANDLER (pios_packet_handler) -#define PIOS_PH_MAX_PACKET 255 -#define PIOS_PH_WIN_SIZE 3 +#define PIOS_PACKET_HANDLER (pios_packet_handler) +#define PIOS_PH_MAX_PACKET 255 +#define PIOS_PH_WIN_SIZE 3 #define PIOS_PH_MAX_CONNECTIONS 1 -#define RS_ECC_NPARITY 4 +#define RS_ECC_NPARITY 4 -//------------------------- +// ------------------------- // Reed-Solomon ECC -//------------------------- +// ------------------------- #define RS_ECC_NPARITY 4 -//------------------------- +// ------------------------- // Flash EEPROM Emulation -//------------------------- +// ------------------------- -#define PIOS_FLASH_SIZE 0x20000 +#define PIOS_FLASH_SIZE 0x20000 #define PIOS_FLASH_EEPROM_START_ADDR 0x08000000 -#define PIOS_FLASH_PAGE_SIZE 1024 -#define PIOS_FLASH_EEPROM_ADDR (PIOS_FLASH_EEPROM_START_ADDR + PIOS_FLASH_SIZE - PIOS_FLASH_PAGE_SIZE) -#define PIOS_FLASH_EEPROM_LEN PIOS_FLASH_PAGE_SIZE +#define PIOS_FLASH_PAGE_SIZE 1024 +#define PIOS_FLASH_EEPROM_ADDR (PIOS_FLASH_EEPROM_START_ADDR + PIOS_FLASH_SIZE - PIOS_FLASH_PAGE_SIZE) +#define PIOS_FLASH_EEPROM_LEN PIOS_FLASH_PAGE_SIZE #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/oplinkmini/pios_usb_board_data.c b/flight/targets/boards/oplinkmini/pios_usb_board_data.c index 070d8c392..5aa5ab5f9 100644 --- a/flight/targets/boards/oplinkmini/pios_usb_board_data.c +++ b/flight/targets/boards/oplinkmini/pios_usb_board_data.c @@ -28,70 +28,71 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ -#include /* PIOS_SYS_SerialNumberGet */ -#include /* PIOS_USBHOOK_* */ -#include /* PIOS_USB_UTIL_AsciiToUtf8 */ +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ static const uint8_t usb_product_id[20] = { - sizeof(usb_product_id), - USB_DESC_TYPE_STRING, - 'P', 0, - 'i', 0, - 'p', 0, - 'X', 0, - 't', 0, - 'r', 0, - 'e', 0, - 'm', 0, - 'e', 0, + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'P', 0, + 'i', 0, + 'p', 0, + 'X', 0, + 't', 0, + 'r', 0, + 'e', 0, + 'm', 0, + 'e', 0, }; -static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = { - sizeof(usb_serial_number), - USB_DESC_TYPE_STRING, +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, }; static const struct usb_string_langid usb_lang_id = { - .bLength = sizeof(usb_lang_id), - .bDescriptorType = USB_DESC_TYPE_STRING, - .bLangID = htousbs(USB_LANGID_ENGLISH_US), + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), }; static const uint8_t usb_vendor_id[28] = { - sizeof(usb_vendor_id), - USB_DESC_TYPE_STRING, - 'o', 0, - 'p', 0, - 'e', 0, - 'n', 0, - 'p', 0, - 'i', 0, - 'l', 0, - 'o', 0, - 't', 0, - '.', 0, - 'o', 0, - 'r', 0, - 'g', 0 + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 }; int32_t PIOS_USB_BOARD_DATA_Init(void) { - /* Load device serial number into serial number string */ - uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - PIOS_SYS_SerialNumberGet((char *)sn); + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ - uint8_t * utf8 = &(usb_serial_number[2]); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1); + PIOS_SYS_SerialNumberGet((char *)sn); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); - return 0; + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; } diff --git a/flight/targets/boards/osd/board_hw_defs.c b/flight/targets/boards/osd/board_hw_defs.c index 95f142788..c00c4ba69 100644 --- a/flight/targets/boards/osd/board_hw_defs.c +++ b/flight/targets/boards/osd/board_hw_defs.c @@ -11,19 +11,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,61 +31,60 @@ #include static const struct pios_led pios_leds[] = { - [PIOS_LED_HEARTBEAT] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_ALARM] = { - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_ALARM] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, }; static const struct pios_led_cfg pios_led_cfg = { - .leds = pios_leds, - .num_leds = NELEMENTS(pios_leds), + .leds = pios_leds, + .num_leds = NELEMENTS(pios_leds), }; -const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (__attribute__((unused)) uint32_t board_revision) +const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision) { - return &pios_led_cfg; + return &pios_led_cfg; } -#endif /* PIOS_INCLUDE_LED */ +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_FLASH) #include "pios_flashfs_logfs_priv.h" #include "pios_flash_internal_priv.h" -static const struct pios_flash_internal_cfg flash_internal_cfg = { -}; +static const struct pios_flash_internal_cfg flash_internal_cfg = {}; static const struct flashfs_logfs_cfg flashfs_internal_cfg = { -.fs_magic = 0x99abcfef, -.total_fs_size = EE_BANK_SIZE, /* 32K bytes (2x16KB sectors) */ -.arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 1 sector */ -.slot_size = 0x00000100, /* 256 bytes */ + .fs_magic = 0x99abcfef, + .total_fs_size = EE_BANK_SIZE, /* 32K bytes (2x16KB sectors) */ + .arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 1 sector */ + .slot_size = 0x00000100, /* 256 bytes */ -.start_offset = EE_BANK_BASE, /* start after the bootloader */ -.sector_size = 0x00004000, /* 16K bytes */ -.page_size = 0x00004000, /* 16K bytes */ + .start_offset = EE_BANK_BASE, /* start after the bootloader */ + .sector_size = 0x00004000, /* 16K bytes */ + .page_size = 0x00004000, /* 16K bytes */ }; #include "pios_flash.h" @@ -103,122 +102,123 @@ static const struct flashfs_logfs_cfg flashfs_internal_cfg = { * .rodata section (ie. Flash) rather than in the .bss section (RAM). */ void PIOS_SPI_sdcard_irq_handler(void); -void DMA1_Channel2_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler"))); -void DMA1_Channel3_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler"))); +void DMA1_Channel2_IRQHandler() __attribute__((alias("PIOS_SPI_sdcard_irq_handler"))); +void DMA1_Channel3_IRQHandler() __attribute__((alias("PIOS_SPI_sdcard_irq_handler"))); static const struct pios_spi_cfg pios_spi_sdcard_cfg = { - .regs = SPI2, - .remap = GPIO_AF_SPI2, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* Maximum divider (ie. slowest clock rate) */ - }, - .use_crc = false, - .dma = { - .irq = { - .flags = (DMA_IT_TCIF3|DMA_IT_TEIF3|DMA_IT_HTIF3|DMA_IT_TCIF3), - .init = { - .NVIC_IRQChannel = DMA1_Stream3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA1_Stream3, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_Single, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA1_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_Single, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .slave_count = 1, - .ssel = {{ - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }}, + .regs = SPI2, + .remap = GPIO_AF_SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* Maximum divider (ie. slowest clock rate) */ + }, + .use_crc = false, + .dma = { + .irq = { + .flags = (DMA_IT_TCIF3 | DMA_IT_TEIF3 | DMA_IT_HTIF3 | DMA_IT_TCIF3), + .init = { + .NVIC_IRQChannel = DMA1_Stream3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA1_Stream3, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + .tx = { + .channel = DMA1_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, }; static uint32_t pios_spi_sdcard_id; void PIOS_SPI_sdcard_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_sdcard_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_sdcard_id); } -#endif - +#endif /* if defined(PIOS_INCLUDE_SPI) */ #include @@ -229,45 +229,45 @@ void PIOS_SPI_sdcard_irq_handler(void) * GPS USART */ static const struct pios_usart_cfg pios_usart_gps_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART1, + .remap = GPIO_AF_USART1, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_INCLUDE_GPS */ @@ -276,45 +276,45 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = { * AUX USART */ static const struct pios_usart_cfg pios_usart_aux_cfg = { - .regs = USART2, - .remap = GPIO_AF_USART2, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART2, + .remap = GPIO_AF_USART2, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_COM_AUX */ @@ -324,45 +324,45 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { * Telemetry on main USART */ static const struct pios_usart_cfg pios_usart_telem_main_cfg = { - .regs = UART4, - .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = UART4, + .remap = GPIO_AF_UART4, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = UART4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_COM_TELEM */ @@ -382,74 +382,74 @@ static const struct pios_usart_cfg pios_usart_telem_main_cfg = { */ void PIOS_I2C_flexiport_adapter_ev_irq_handler(void); void PIOS_I2C_flexiport_adapter_er_irq_handler(void); -void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexiport_adapter_ev_irq_handler"))); -void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexiport_adapter_er_irq_handler"))); +void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_ev_irq_handler"))); +void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_er_irq_handler"))); static const struct pios_i2c_adapter_cfg pios_i2c_flexiport_adapter_cfg = { - .regs = I2C2, - .remap = GPIO_AF_I2C2, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, + .regs = I2C2, + .remap = GPIO_AF_I2C2, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; uint32_t pios_i2c_flexiport_adapter_id; void PIOS_I2C_flexiport_adapter_ev_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexiport_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexiport_adapter_id); } void PIOS_I2C_flexiport_adapter_er_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexiport_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexiport_adapter_id); } -#endif +#endif /* if defined(PIOS_INCLUDE_I2C) */ #if defined(PIOS_INCLUDE_RTC) /* @@ -457,53 +457,53 @@ void PIOS_I2C_flexiport_adapter_er_irq_handler(void) */ #include -void PIOS_RTC_IRQ_Handler (void); -void RTC_WKUP_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler"))); +void PIOS_RTC_IRQ_Handler(void); +void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); static const struct pios_rtc_cfg pios_rtc_main_cfg = { - .clksrc = RCC_RTCCLKSource_HSE_Div8, // Divide 8 Mhz crystal down to 1 - // For some reason it's acting like crystal is 16 Mhz. This clock is then divided - // by another 16 to give a nominal 62.5 khz clock - .prescaler = 100, // Every 100 cycles gives 625 Hz - .irq = { - .init = { - .NVIC_IRQChannel = RTC_WKUP_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .clksrc = RCC_RTCCLKSource_HSE_Div8, // Divide 8 Mhz crystal down to 1 + // For some reason it's acting like crystal is 16 Mhz. This clock is then divided + // by another 16 to give a nominal 62.5 khz clock + .prescaler = 100, // Every 100 cycles gives 625 Hz + .irq = { + .init = { + .NVIC_IRQChannel = RTC_WKUP_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; -void PIOS_RTC_IRQ_Handler (void) +void PIOS_RTC_IRQ_Handler(void) { - PIOS_RTC_irq_handler (); + PIOS_RTC_irq_handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_RTC) */ #if defined(PIOS_INCLUDE_USB) #include "pios_usb_priv.h" static const struct pios_usb_cfg pios_usb_main_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = OTG_FS_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_25MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - }, + .irq = { + .init = { + .NVIC_IRQChannel = OTG_FS_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, }, - .vsense_active_low = false + }, + .vsense = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_25MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + }, + }, + .vsense_active_low = false }; #include "pios_usb_board_data_priv.h" @@ -511,7 +511,7 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { #include "pios_usb_desc_hid_only_priv.h" #include "pios_usbhook.h" -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ #if defined(PIOS_INCLUDE_COM_MSG) @@ -523,9 +523,9 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { #include const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, + .data_if = 0, + .data_rx_ep = 1, + .data_tx_ep = 1, }; #endif /* PIOS_INCLUDE_USB_HID */ @@ -533,36 +533,36 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { #include const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 1, - .ctrl_tx_ep = 2, + .ctrl_if = 1, + .ctrl_tx_ep = 2, - .data_if = 2, - .data_rx_ep = 3, - .data_tx_ep = 3, + .data_if = 2, + .data_rx_ep = 3, + .data_tx_ep = 3, }; -#endif /* PIOS_INCLUDE_USB_CDC */ +#endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_VIDEO) static const TIM_TimeBaseInitTypeDef tim_8_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / 50000) - 1), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / 50000) - 1), + .TIM_RepetitionCounter = 0x0000, }; static const struct pios_tim_clock_cfg tim_8_cfg = { - .timer = TIM8, - .time_base_init = &tim_8_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM8_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM8, + .time_base_init = &tim_8_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; /** @@ -570,404 +570,401 @@ static const struct pios_tim_clock_cfg tim_8_cfg = { */ #include static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - { - .timer = TIM8, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource8, - }, - .remap = GPIO_AF_TIM8, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource9, - }, - .remap = GPIO_AF_TIM8, - }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource8, + }, + .remap = GPIO_AF_TIM8, + }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource9, + }, + .remap = GPIO_AF_TIM8, + }, }; const struct pios_servo_cfg pios_servo_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = 0, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_all_pins, - .num_channels = NELEMENTS(pios_tim_servoport_all_pins), + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = 0, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_all_pins, + .num_channels = NELEMENTS(pios_tim_servoport_all_pins), }; - #include static const struct pios_exti_cfg pios_exti_hsync_cfg __exti_config = { - .vector = PIOS_Hsync_ISR, - .line = EXTI_Line7, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line7, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - //.EXTI_Trigger = EXTI_Trigger_Rising_Falling, - .EXTI_Trigger = EXTI_Trigger_Falling, - //.EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_Hsync_ISR, + .line = EXTI_Line7, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line7, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + // .EXTI_Trigger = EXTI_Trigger_Rising_Falling, + .EXTI_Trigger = EXTI_Trigger_Falling, + // .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_exti_cfg pios_exti_vsync_cfg __exti_config = { - .vector = PIOS_Vsync_ISR, - .line = EXTI_Line5, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line5, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Falling, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_Vsync_ISR, + .line = EXTI_Line5, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line5, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Falling, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_video_cfg pios_video_cfg = { - .mask = { - .regs = SPI3, - .remap = GPIO_AF_SPI3, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_1Line_Tx, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_Low, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, - }, - .use_crc = false, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case RX) - .flags = (DMA_IT_TCIF7), - .init = { - .NVIC_IRQChannel = DMA1_Stream7_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - /*.rx = {},*/ - .tx = { - .channel = DMA1_Stream7, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_BufferSize = BUFFER_LINE_LENGTH, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_VeryHigh, - .DMA_FIFOMode = DMA_FIFOMode_Enable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_INC4, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .miso = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - /*.mosi = {},*/ - .slave_count = 1, - }, - .level = { - .regs = SPI1, - .remap = GPIO_AF_SPI1, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_1Line_Tx, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_Low, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, - }, - .use_crc = false, - .dma = { - .irq = { - .flags = (DMA_IT_TCIF5), - .init = { - .NVIC_IRQChannel = DMA2_Stream5_IRQn, - .NVIC_IRQChannelPreemptionPriority = 0, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - /*.rx = {},*/ - .tx = { - .channel = DMA2_Stream5, - .init = { - .DMA_Channel = DMA_Channel_3, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_BufferSize = BUFFER_LINE_LENGTH, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_VeryHigh, - .DMA_FIFOMode = DMA_FIFOMode_Enable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_INC4, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - /*.mosi = {},*/ - .slave_count = 1, + .mask = { + .regs = SPI3, + .remap = GPIO_AF_SPI3, + .init = { + .SPI_Mode = SPI_Mode_Slave, + .SPI_Direction = SPI_Direction_1Line_Tx, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_Low, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + }, + .use_crc = false, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case RX) + .flags = (DMA_IT_TCIF7), + .init = { + .NVIC_IRQChannel = DMA1_Stream7_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + /*.rx = {},*/ + .tx = { + .channel = DMA1_Stream7, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_BufferSize = BUFFER_LINE_LENGTH, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_VeryHigh, + .DMA_FIFOMode = DMA_FIFOMode_Enable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_INC4, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .sclk = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .miso = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + /*.mosi = {},*/ + .slave_count = 1, + }, + .level = { + .regs = SPI1, + .remap = GPIO_AF_SPI1, + .init = { + .SPI_Mode = SPI_Mode_Slave, + .SPI_Direction = SPI_Direction_1Line_Tx, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_Low, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + }, + .use_crc = false, + .dma = { + .irq = { + .flags = (DMA_IT_TCIF5), + .init = { + .NVIC_IRQChannel = DMA2_Stream5_IRQn, + .NVIC_IRQChannelPreemptionPriority = 0, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + /*.rx = {},*/ + .tx = { + .channel = DMA2_Stream5, + .init = { + .DMA_Channel = DMA_Channel_3, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_BufferSize = BUFFER_LINE_LENGTH, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_VeryHigh, + .DMA_FIFOMode = DMA_FIFOMode_Enable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_INC4, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + /*.mosi = {},*/ + .slave_count = 1, + }, - }, + .hsync = &pios_exti_hsync_cfg, + .vsync = &pios_exti_vsync_cfg, - .hsync = &pios_exti_hsync_cfg, - .vsync = &pios_exti_vsync_cfg, - - .pixel_timer = { - .timer = TIM4, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource6, - }, - .remap = GPIO_AF_TIM4, - }, - .hsync_capture = { - .timer = TIM4, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource7, - }, - .remap = GPIO_AF_TIM4, - }, - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = 1, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, + .pixel_timer = { + .timer = TIM4, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource6, + }, + .remap = GPIO_AF_TIM4, + }, + .hsync_capture = { + .timer = TIM4, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource7, + }, + .remap = GPIO_AF_TIM4, + }, + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = 1, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, }; -#endif +#endif /* if defined(PIOS_INCLUDE_VIDEO) */ #if defined(PIOS_INCLUDE_WAVE) #include #include static const TIM_TimeBaseInitTypeDef tim_6_time_base = { - .TIM_Prescaler = 0, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = (PIOS_PERIPHERAL_APB1_CLOCK / 44100), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = 0, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = (PIOS_PERIPHERAL_APB1_CLOCK / 44100), + .TIM_RepetitionCounter = 0x0000, }; static const struct pios_tim_clock_cfg tim_6_cfg = { - .timer = TIM6, - .time_base_init = &tim_6_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM6_DAC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM6, + .time_base_init = &tim_6_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM6_DAC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_dac_cfg pios_dac_cfg = { - .timer = TIM6, - .time_base_init = { - .TIM_Prescaler = 0, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = (PIOS_PERIPHERAL_APB1_CLOCK / 44100), - .TIM_RepetitionCounter = 0x0000, - }, - .irq = { - .init = { - .NVIC_IRQChannel = TIM6_DAC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case RX) - .flags = (DMA_IT_TCIF5), - .init = { - .NVIC_IRQChannel = DMA1_Stream5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM6, + .time_base_init = { + .TIM_Prescaler = 0, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = (PIOS_PERIPHERAL_APB1_CLOCK / 44100), + .TIM_RepetitionCounter = 0x0000, + }, + .irq = { + .init = { + .NVIC_IRQChannel = TIM6_DAC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case RX) + .flags = (DMA_IT_TCIF5), + .init = { + .NVIC_IRQChannel = DMA1_Stream5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, - .rx = {}, - .tx = { - .channel = DMA1_Stream5, - .init = { - .DMA_Channel = DMA_Channel_7, - .DMA_PeripheralBaseAddr = (uint32_t)&DAC->DHR8R1, - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_BufferSize = BUFFERSIZE, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_High, - .DMA_FIFOMode = DMA_FIFOMode_Disable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_Single, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .channel = DAC_Channel_1, - .dac_init = { - .DAC_Trigger = DAC_Trigger_T6_TRGO, - .DAC_WaveGeneration = DAC_WaveGeneration_None, - .DAC_OutputBuffer = DAC_OutputBuffer_Enable, - }, - .dac_io = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AN, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, + .rx = {}, + .tx = { + .channel = DMA1_Stream5, + .init = { + .DMA_Channel = DMA_Channel_7, + .DMA_PeripheralBaseAddr = (uint32_t)&DAC->DHR8R1, + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_BufferSize = BUFFERSIZE, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_High, + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .channel = DAC_Channel_1, + .dac_init = { + .DAC_Trigger = DAC_Trigger_T6_TRGO, + .DAC_WaveGeneration = DAC_WaveGeneration_None, + .DAC_OutputBuffer = DAC_OutputBuffer_Enable, + }, + .dac_io = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AN, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, }; -#endif - +#endif /* if defined(PIOS_INCLUDE_WAVE) */ diff --git a/flight/targets/boards/osd/bootloader/inc/common.h b/flight/targets/boards/osd/bootloader/inc/common.h index 9010fddd9..e64b89014 100644 --- a/flight/targets/boards/osd/bootloader/inc/common.h +++ b/flight/targets/boards/osd/bootloader/inc/common.h @@ -28,10 +28,10 @@ #ifndef COMMON_H_ #define COMMON_H_ -//#include "board.h" +// #include "board.h" typedef enum { - start, keepgoing, + start, keepgoing, } DownloadAction; /**************************************************/ @@ -39,77 +39,77 @@ typedef enum { /**************************************************/ typedef enum { - DFUidle, //0 - uploading, //1 - wrong_packet_received, //2 - too_many_packets, //3 - too_few_packets, //4 - Last_operation_Success, //5 - downloading, //6 - BLidle, //7 - Last_operation_failed, //8 - uploadingStarting, //9 - outsideDevCapabilities, //10 - CRC_Fail,//11 - failed_jump, -//12 + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + BLidle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, +// 12 } DFUStates; /**************************************************/ /* OP_DFU commands */ /**************************************************/ typedef enum { - Reserved, //0 - Req_Capabilities, //1 - Rep_Capabilities, //2 - EnterDFU, //3 - JumpFW, //4 - Reset, //5 - Abort_Operation, //6 - Upload, //7 - Op_END, //8 - Download_Req, //9 - Download, //10 - Status_Request, //11 - Status_Rep -//12 + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep +// 12 } DFUCommands; typedef enum { - High_Density, Medium_Density + High_Density, Medium_Density } DeviceType; /**************************************************/ /* OP_DFU transfer types */ /**************************************************/ typedef enum { - FW, //0 - Descript -//2 + FW, // 0 + Descript +// 2 } DFUTransfer; /**************************************************/ /* OP_DFU transfer port */ /**************************************************/ typedef enum { - Usb, //0 - Serial -//2 + Usb, // 0 + Serial +// 2 } DFUPort; /**************************************************/ /* OP_DFU programable programable HW types */ /**************************************************/ typedef enum { - Self_flash, //0 - Remote_flash_via_spi -//1 + Self_flash, // 0 + Remote_flash_via_spi +// 1 } DFUProgType; /**************************************************/ /* OP_DFU programable sources */ /**************************************************/ -#define USB 0 -#define SPI 1 +#define USB 0 +#define SPI 1 -#define DownloadDelay 100000 +#define DownloadDelay 100000 -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 +#define MAX_DEL_RETRYS 3 +#define MAX_WRI_RETRYS 3 #endif /* COMMON_H_ */ diff --git a/flight/targets/boards/osd/bootloader/inc/pios_config.h b/flight/targets/boards/osd/bootloader/inc/pios_config.h index 1d121c321..cf7b48ee1 100644 --- a/flight/targets/boards/osd/bootloader/inc/pios_config.h +++ b/flight/targets/boards/osd/bootloader/inc/pios_config.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * - Central compile time config for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/osd/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/osd/bootloader/inc/pios_usb_board_data.h index fe9072b8c..d7b10fea5 100644 --- a/flight/targets/boards/osd/bootloader/inc/pios_usb_board_data.h +++ b/flight/targets/boards/osd/bootloader/inc/pios_usb_board_data.h @@ -33,13 +33,13 @@ #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 2 +#define PIOS_USB_BOARD_EP_NUM 2 -#include /* struct usb_* */ +#include /* struct usb_* */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OSD -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OSD, USB_OP_BOARD_MODE_BL) -#define PIOS_USB_BOARD_SN_SUFFIX "+BL" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OSD +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OSD, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" /* * The bootloader uses a simplified report structure @@ -49,4 +49,4 @@ */ #define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/osd/bootloader/main.c b/flight/targets/boards/osd/bootloader/main.c index 82878dfbd..12abfd8f0 100644 --- a/flight/targets/boards/osd/bootloader/main.c +++ b/flight/targets/boards/osd/bootloader/main.c @@ -8,21 +8,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief This is the file with the main function of the Revolution BootLoader * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,7 +32,7 @@ #include #include #include -#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USBHOOK_* */ #include /* Prototype of PIOS_Board_Init() function */ @@ -60,10 +60,10 @@ uint8_t tempcount = 0; /* Extern variables ----------------------------------------------------------*/ DFUStates DeviceState; -int16_t status = 0; -bool JumpToApp = false; -bool GO_dfu = false; -bool USB_connected = false; +int16_t status = 0; +bool JumpToApp = false; +bool GO_dfu = false; +bool USB_connected = false; bool User_DFU_request = false; static uint8_t mReceive_Buffer[63]; /* Private function prototypes -----------------------------------------------*/ @@ -71,143 +71,156 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); uint8_t processRX(); void jump_to_app(); -int main() { - PIOS_SYS_Init(); - PIOS_Board_Init(); - PIOS_IAP_Init(); +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); - USB_connected = PIOS_USB_CheckAvailable(0); + USB_connected = PIOS_USB_CheckAvailable(0); - if (PIOS_IAP_CheckRequest() == true) { - PIOS_DELAY_WaitmS(1000); - User_DFU_request = true; - PIOS_IAP_ClearRequest(); - } + if (PIOS_IAP_CheckRequest() == true) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = true; + PIOS_IAP_ClearRequest(); + } - GO_dfu = (USB_connected == true) || (User_DFU_request == true); + GO_dfu = (USB_connected == true) || (User_DFU_request == true); - if (GO_dfu == true) { - PIOS_Board_Init(); - if (User_DFU_request == true) - DeviceState = DFUidle; - else - DeviceState = BLidle; - } else - JumpToApp = true; + if (GO_dfu == true) { + PIOS_Board_Init(); + if (User_DFU_request == true) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = true; + } - uint32_t stopwatch = 0; - uint32_t prev_ticks = PIOS_DELAY_GetuS(); - while (true) { - /* Update the stopwatch */ - uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); - prev_ticks += elapsed_ticks; - stopwatch += elapsed_ticks; + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (true) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; - if (JumpToApp == true) - jump_to_app(); + if (JumpToApp == true) { + jump_to_app(); + } - switch (DeviceState) { - case Last_operation_Success: - case uploadingStarting: - case DFUidle: - period1 = 5000; - sweep_steps1 = 100; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case uploading: - period1 = 5000; - sweep_steps1 = 100; - period2 = 2500; - sweep_steps2 = 50; - break; - case downloading: - period1 = 2500; - sweep_steps1 = 50; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case BLidle: - period1 = 0; - PIOS_LED_On(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - default://error - period1 = 5000; - sweep_steps1 = 100; - period2 = 5000; - sweep_steps2 = 100; - } + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } - if (period1 != 0) { - if (LedPWM(period1, sweep_steps1, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_On(PIOS_LED_HEARTBEAT); + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } - if (period2 != 0) { - if (LedPWM(period2, sweep_steps2, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } - if (stopwatch > 50 * 1000 * 1000) - stopwatch = 0; - if ((stopwatch > 6 * 1000 * 1000) && (DeviceState - == BLidle)) - JumpToApp = true; + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && (DeviceState + == BLidle)) { + JumpToApp = true; + } - processRX(); - DataDownload(start); - } + processRX(); + DataDownload(start); + } } -void jump_to_app() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; - PIOS_LED_On(PIOS_LED_HEARTBEAT); + PIOS_LED_On(PIOS_LED_HEARTBEAT); // Look at cm3_vectors struct in startup. In a fw image the first uint32_t contains the address of the top of irqstack - uint32_t fwIrqStackBase = (*(__IO uint32_t*) bdinfo->fw_base) & 0xFFFE0000; + uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000; // Check for the two possible irqstack locations (sram or core coupled sram) - if ( fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { + if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { /* Jump to user application */ - FLASH_Lock(); - RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - PIOS_USBHOOK_Deactivate(); + PIOS_USBHOOK_Deactivate(); - JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); - Jump_To_Application = (pFunction) JumpAddress; - /* Initialize user application's Stack Pointer */ - __set_MSP(*(__IO uint32_t*) bdinfo->fw_base); - Jump_To_Application(); - } else { - DeviceState = failed_jump; - return; - } + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + return; + } } -uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { - uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ - uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ - uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ - if (curr_sweep & 1) { - pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ - } - return ((count % pwm_period) > pwm_duty) ? 1 : 0; + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; } -uint8_t processRX() { - if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { - processComand(mReceive_Buffer); - } - return true; +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return true; } - diff --git a/flight/targets/boards/osd/bootloader/pios_board.c b/flight/targets/boards/osd/bootloader/pios_board.c index bca178733..7e9858f18 100644 --- a/flight/targets/boards/osd/bootloader/pios_board.c +++ b/flight/targets/boards/osd/bootloader/pios_board.c @@ -7,19 +7,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,41 +39,42 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; -void PIOS_Board_Init() { - if (board_init_complete) { - return; - } +void PIOS_Board_Init() +{ + if (board_init_complete) { + return; + } + + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); - #if defined(PIOS_INCLUDE_LED) - PIOS_LED_Init(&pios_led_cfg); -#endif /* PIOS_INCLUDE_LED */ + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Activate the HID-only USB configuration */ - PIOS_USB_DESC_HID_ONLY_Init(); + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { - PIOS_Assert(0); - } -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ - PIOS_USBHOOK_Activate(); + PIOS_USBHOOK_Activate(); -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ - board_init_complete = true; + board_init_complete = true; } diff --git a/flight/targets/boards/osd/firmware/cm3_fault_handlers.c b/flight/targets/boards/osd/firmware/cm3_fault_handlers.c index e59e3b79a..e74ffa76a 100644 --- a/flight/targets/boards/osd/firmware/cm3_fault_handlers.c +++ b/flight/targets/boards/osd/firmware/cm3_fault_handlers.c @@ -14,34 +14,34 @@ # include #endif -#define FAULT_TRAMPOLINE(_vec) \ -__attribute__((naked, no_instrument_function)) \ -void \ -_vec##_Handler(void) \ -{ \ - __asm( ".syntax unified\n" \ - "MOVS R0, #4 \n" \ - "MOV R1, LR \n" \ - "TST R0, R1 \n" \ - "BEQ 1f \n" \ - "MRS R0, PSP \n" \ - "B " #_vec "_Handler2 \n" \ - "1: \n" \ - "MRS R0, MSP \n" \ - "B " #_vec "_Handler2 \n" \ - ".syntax divided\n"); \ -} \ -struct hack +#define FAULT_TRAMPOLINE(_vec) \ + __attribute__((naked, no_instrument_function)) \ + void \ + _vec##_Handler(void) \ + { \ + __asm(".syntax unified\n" \ + "MOVS R0, #4 \n" \ + "MOV R1, LR \n" \ + "TST R0, R1 \n" \ + "BEQ 1f \n" \ + "MRS R0, PSP \n" \ + "B " #_vec "_Handler2 \n" \ + "1: \n" \ + "MRS R0, MSP \n" \ + "B " #_vec "_Handler2 \n" \ + ".syntax divided\n"); \ + } \ + struct hack struct cm3_frame { - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t pc; - uint32_t psr; + uint32_t r0; + uint32_t r1; + uint32_t r2; + uint32_t r3; + uint32_t r12; + uint32_t lr; + uint32_t pc; + uint32_t psr; }; FAULT_TRAMPOLINE(HardFault); @@ -49,39 +49,42 @@ FAULT_TRAMPOLINE(BusFault); FAULT_TRAMPOLINE(UsageFault); /* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */ -#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) +#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) -void -HardFault_Handler2(struct cm3_frame *frame) +void HardFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nHARD FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(HFSR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nHARD FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(HFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } } -void -BusFault_Handler2(struct cm3_frame *frame) +void BusFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nBUS FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(BFAR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nBUS FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(BFAR)); + dbg_write_char('\n'); + for (;;) { + ; + } } -void -UsageFault_Handler2(struct cm3_frame *frame) +void UsageFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nUSAGE FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nUSAGE FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } } diff --git a/flight/targets/boards/osd/firmware/dcc_stdio.c b/flight/targets/boards/osd/firmware/dcc_stdio.c index 102dd70a1..1a522e9a0 100644 --- a/flight/targets/boards/osd/firmware/dcc_stdio.c +++ b/flight/targets/boards/osd/firmware/dcc_stdio.c @@ -1,33 +1,33 @@ /*************************************************************************** - * Copyright (C) 2008 by Dominic Rath * - * Dominic.Rath@gmx.de * - * Copyright (C) 2008 by Spencer Oliver * - * spen@spen-soft.co.uk * - * Copyright (C) 2008 by Frederik Kriewtz * - * frederik@kriewitz.eu * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* Copyright (C) 2008 by Frederik Kriewtz * +* frederik@kriewitz.eu * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ #include "inc/dcc_stdio.h" -#define TARGET_REQ_TRACEMSG 0x00 -#define TARGET_REQ_DEBUGMSG_ASCII 0x01 -#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) -#define TARGET_REQ_DEBUGCHAR 0x02 +#define TARGET_REQ_TRACEMSG 0x00 +#define TARGET_REQ_DEBUGMSG_ASCII 0x01 +#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) +#define TARGET_REQ_DEBUGCHAR 0x02 /* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel * DCRDR[7:0] is used by target for status @@ -35,115 +35,115 @@ * DCRDR[23:16] is used for by host for status * DCRDR[31:24] is used for by host for write buffer */ -#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) +#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) -#define BUSY 1 +#define BUSY 1 void dbg_write(unsigned long dcc_data) { - int len = 4; + int len = 4; - while (len--) - { - /* wait for data ready */ - while (NVIC_DBG_DATA_R & BUSY); + while (len--) { + /* wait for data ready */ + while (NVIC_DBG_DATA_R & BUSY) { + ; + } - /* write our data and set write flag - tell host there is data*/ - NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); - dcc_data >>= 8; - } + /* write our data and set write flag - tell host there is data*/ + NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); + dcc_data >>= 8; + } } void dbg_trace_point(unsigned long number) { - dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); + dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); } void dbg_write_u32(const unsigned long *val, long len) { - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); - while (len > 0) - { - dbg_write(*val); + while (len > 0) { + dbg_write(*val); - val++; - len--; - } + val++; + len--; + } } void dbg_write_u16(const unsigned short *val, long len) { - unsigned long dcc_data; + unsigned long dcc_data; - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = val[0] - | ((len > 1) ? val[1] << 16: 0x0000); + while (len > 0) { + dcc_data = val[0] + | ((len > 1) ? val[1] << 16 : 0x0000); - dbg_write(dcc_data); + dbg_write(dcc_data); - val += 2; - len -= 2; - } + val += 2; + len -= 2; + } } void dbg_write_u8(const unsigned char *val, long len) { - unsigned long dcc_data; + unsigned long dcc_data; - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = val[0] - | ((len > 1) ? val[1] << 8 : 0x00) - | ((len > 2) ? val[2] << 16 : 0x00) - | ((len > 3) ? val[3] << 24 : 0x00); + while (len > 0) { + dcc_data = val[0] + | ((len > 1) ? val[1] << 8 : 0x00) + | ((len > 2) ? val[2] << 16 : 0x00) + | ((len > 3) ? val[3] << 24 : 0x00); - dbg_write(dcc_data); + dbg_write(dcc_data); - val += 4; - len -= 4; - } + val += 4; + len -= 4; + } } void dbg_write_str(const char *msg) { - long len; - unsigned long dcc_data; + long len; + unsigned long dcc_data; - for (len = 0; msg[len] && (len < 65536); len++); + for (len = 0; msg[len] && (len < 65536); len++) { + ; + } - dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = msg[0] - | ((len > 1) ? msg[1] << 8 : 0x00) - | ((len > 2) ? msg[2] << 16 : 0x00) - | ((len > 3) ? msg[3] << 24 : 0x00); - dbg_write(dcc_data); + while (len > 0) { + dcc_data = msg[0] + | ((len > 1) ? msg[1] << 8 : 0x00) + | ((len > 2) ? msg[2] << 16 : 0x00) + | ((len > 3) ? msg[3] << 24 : 0x00); + dbg_write(dcc_data); - msg += 4; - len -= 4; - } + msg += 4; + len -= 4; + } } void dbg_write_char(char msg) { - dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); + dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); } void dbg_write_hex32(const unsigned long val) { - static const char hextab[] = { - '0', '1', '2', '3', '4', '5', '6', '7', - '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' - }; + static const char hextab[] = { + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' + }; - for (int shift = 28; shift >= 0; shift -= 4) - dbg_write_char(hextab[(val >> shift) & 0xf]); + for (int shift = 28; shift >= 0; shift -= 4) { + dbg_write_char(hextab[(val >> shift) & 0xf]); + } } diff --git a/flight/targets/boards/osd/firmware/font_outlined8x14.c b/flight/targets/boards/osd/firmware/font_outlined8x14.c index 55b9a66dd..6ace4c164 100644 --- a/flight/targets/boards/osd/firmware/font_outlined8x14.c +++ b/flight/targets/boards/osd/firmware/font_outlined8x14.c @@ -6,263 +6,263 @@ * Commons licenses (version 3.0 BY-SA) and *not* the GPL. */ -// This data is generated by a Python script from +// This data is generated by a Python script from // the original font .pngs. // 0xff = indicates character not present // any other byte contains the index of the character. const char font_lookup_outlined8x14[256] = { - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0x00, 0x01, 0x02, 0x03, 0xff, 0x04, 0x05, 0x06, - 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, - 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, - 0x17, 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, - 0x1f, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, - 0x27, 0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, - 0x2f, 0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, - 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, - 0x3f, 0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, - 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, - 0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, - 0x57, 0x58, 0x59, 0x5a, 0x5b, 0x5c, 0x5d, 0xff, - 0x5e, 0x5f, 0x60, 0x61, 0x62, 0x63, 0x64, 0x65, - 0x66, 0x67, 0x68, 0x69, 0x6a, 0x6b, 0x6c, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0x00, 0x01, 0x02, 0x03, 0xff, 0x04, 0x05, 0x06, + 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, + 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, + 0x17, 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, + 0x1f, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, + 0x27, 0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, + 0x2f, 0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, + 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, + 0x3f, 0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, + 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, + 0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, + 0x57, 0x58, 0x59, 0x5a, 0x5b, 0x5c, 0x5d, 0xff, + 0x5e, 0x5f, 0x60, 0x61, 0x62, 0x63, 0x64, 0x65, + 0x66, 0x67, 0x68, 0x69, 0x6a, 0x6b, 0x6c, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }; const char font_data_outlined8x14[] = { - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x20 - 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, - 0x00, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, // 0x21 - 0x66, 0x66, 0x66, 0x66, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x24, 0x24, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x22 - 0x7e, 0xff, 0xff, 0xff, 0x7e, 0xff, 0xff, 0xff, 0x7e, 0xff, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x24, 0x7e, 0x24, 0x24, 0x24, 0x7e, 0x24, 0x24, 0x24, 0x7e, 0x24, 0x00, 0x00, // 0x23 - 0x00, 0x70, 0x70, 0x72, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x4e, 0x0e, 0x0e, 0x00, - 0x00, 0x00, 0x20, 0x00, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x00, 0x04, 0x00, 0x00, // 0x25 - 0x38, 0x3c, 0x7c, 0x7c, 0x7c, 0xfc, 0xee, 0xef, 0xef, 0xff, 0xff, 0x7e, 0x00, 0x00, - 0x00, 0x18, 0x28, 0x10, 0x28, 0x28, 0x44, 0x44, 0x46, 0x44, 0x3a, 0x00, 0x00, 0x00, // 0x26 - 0x38, 0x38, 0x38, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x27 - 0x1e, 0x3e, 0x7e, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x7e, 0x3e, 0x1e, 0x00, - 0x00, 0x1c, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x1c, 0x00, 0x00, // 0x28 - 0x78, 0x7c, 0x7e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x7e, 0x7c, 0x78, 0x00, - 0x00, 0x38, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x38, 0x00, 0x00, // 0x29 - 0x38, 0x7c, 0xfe, 0xfe, 0xfe, 0xee, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x10, 0x7c, 0x28, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2a - 0x00, 0x00, 0x00, 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x10, 0x10, 0x7c, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2b - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x38, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x10, 0x00, 0x00, // 0x2c - 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2d - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, // 0x2e - 0x00, 0x07, 0x07, 0x0f, 0x0e, 0x1c, 0x1c, 0x38, 0x38, 0x70, 0xf0, 0xe0, 0xe0, 0x00, - 0x00, 0x00, 0x02, 0x04, 0x04, 0x08, 0x08, 0x10, 0x10, 0x20, 0x20, 0x40, 0x00, 0x00, // 0x2f - 0x7e, 0xff, 0xff, 0xe7, 0xef, 0xff, 0xff, 0xf7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x46, 0x4a, 0x52, 0x62, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x30 - 0x3c, 0x7c, 0xfc, 0xfc, 0x5c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x38, 0x68, 0x48, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x7e, 0x00, 0x00, // 0x31 - 0x3e, 0x7f, 0xff, 0xe7, 0x47, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x3c, 0x62, 0x42, 0x02, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x7e, 0x00, 0x00, // 0x32 - 0xfe, 0xff, 0xff, 0x07, 0x07, 0x7f, 0x7e, 0x7f, 0x07, 0x07, 0xff, 0xff, 0xfe, 0x00, - 0x00, 0x7c, 0x02, 0x02, 0x02, 0x04, 0x38, 0x04, 0x02, 0x02, 0x02, 0x7c, 0x00, 0x00, // 0x33 - 0x04, 0x0e, 0x1e, 0x3e, 0x7e, 0xff, 0xff, 0xff, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x00, - 0x00, 0x04, 0x0c, 0x14, 0x24, 0x44, 0x7e, 0x04, 0x04, 0x04, 0x04, 0x04, 0x00, 0x00, // 0x34 - 0xff, 0xff, 0xff, 0xe0, 0xfc, 0xfe, 0xff, 0x0f, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, - 0x00, 0x7e, 0x40, 0x40, 0x40, 0x78, 0x04, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x35 - 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x3c, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x36 - 0xff, 0xff, 0xff, 0x07, 0x07, 0x0e, 0x0e, 0x1c, 0x1c, 0x38, 0x38, 0x70, 0x70, 0x00, - 0x00, 0x7e, 0x02, 0x02, 0x02, 0x04, 0x04, 0x08, 0x08, 0x10, 0x10, 0x20, 0x00, 0x00, // 0x37 - 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0x7e, 0x3c, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x38 - 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x7f, 0x7f, 0x7f, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x02, 0x3c, 0x00, 0x00, // 0x39 - 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, // 0x3a - 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x38, 0x00, - 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x10, 0x00, 0x00, // 0x3b - 0x00, 0x00, 0x04, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x70, 0x38, 0x1c, 0x0e, 0x04, 0x00, - 0x00, 0x00, 0x00, 0x04, 0x08, 0x10, 0x20, 0x40, 0x20, 0x10, 0x08, 0x04, 0x00, 0x00, // 0x3c - 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x00, 0x7e, 0x7e, 0x7e, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x3d - 0x00, 0x00, 0x20, 0x70, 0x38, 0x1c, 0x0e, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0x20, 0x00, - 0x00, 0x00, 0x00, 0x20, 0x10, 0x08, 0x04, 0x02, 0x04, 0x08, 0x10, 0x20, 0x00, 0x00, // 0x3e - 0x3c, 0xfe, 0xff, 0xe7, 0xe7, 0x0e, 0x1c, 0x38, 0x38, 0x00, 0x38, 0x38, 0x38, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x02, 0x04, 0x08, 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, // 0x3f - 0x18, 0x3c, 0x7e, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x7c, 0x3e, 0x1e, 0x00, - 0x00, 0x18, 0x24, 0x42, 0x42, 0x42, 0x4a, 0x52, 0x52, 0x4c, 0x20, 0x1e, 0x00, 0x00, // 0x40 - 0x7e, 0xff, 0xff, 0xff, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x3c, 0x66, 0x42, 0x42, 0x42, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x41 - 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0x00, - 0x00, 0x7c, 0x46, 0x42, 0x42, 0x42, 0x7c, 0x42, 0x42, 0x42, 0x46, 0x7c, 0x00, 0x00, // 0x42 - 0x7f, 0xff, 0xff, 0xf7, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xf7, 0xff, 0xff, 0x7f, 0x00, - 0x00, 0x3e, 0x62, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x62, 0x3e, 0x00, 0x00, // 0x43 - 0xfe, 0xff, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xef, 0xff, 0xff, 0xfe, 0x00, - 0x00, 0x7c, 0x46, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x46, 0x7c, 0x00, 0x00, // 0x44 - 0xff, 0xff, 0xff, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x45 - 0xff, 0xff, 0xff, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, - 0x00, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x46 - 0x7f, 0xff, 0xff, 0xf0, 0xe0, 0xef, 0xef, 0xef, 0xe7, 0xf7, 0xff, 0xff, 0x7f, 0x00, - 0x00, 0x3e, 0x60, 0x40, 0x40, 0x40, 0x46, 0x42, 0x42, 0x42, 0x62, 0x3e, 0x00, 0x00, // 0x47 - 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x48 - 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x7c, 0x00, 0x00, // 0x49 - 0xff, 0xff, 0xff, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x3c, 0xfc, 0xfc, 0xf8, 0x00, - 0x00, 0x7e, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x18, 0x70, 0x00, 0x00, // 0x4a - 0xe7, 0xef, 0xff, 0xfe, 0xfc, 0xf8, 0xf0, 0xf8, 0xfc, 0xfe, 0xff, 0xef, 0xe7, 0x00, - 0x00, 0x42, 0x44, 0x48, 0x50, 0x60, 0x40, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x4b - 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x4c - 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x42, 0x66, 0x5a, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x4d - 0xe7, 0xf7, 0xff, 0xff, 0xff, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x42, 0x62, 0x52, 0x4a, 0x46, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x4e - 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x4f - 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, - 0x00, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x50 - 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x02, 0x02, 0x00, 0x00, // 0x51 - 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xf8, 0xfc, 0xee, 0xe7, 0xe3, 0x00, - 0x00, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x52 - 0xff, 0xff, 0xff, 0xe0, 0xf0, 0xfe, 0xff, 0x7f, 0x0f, 0x07, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x7e, 0x40, 0x40, 0x40, 0x60, 0x3c, 0x06, 0x02, 0x02, 0x02, 0x7e, 0x00, 0x00, // 0x53 - 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, - 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x54 - 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0x7e, 0x3c, 0x00, - 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x55 - 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xfe, 0x7e, 0x7e, 0x7e, 0x3c, 0x3c, 0x00, - 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x24, 0x24, 0x24, 0x18, 0x18, 0x00, 0x00, // 0x56 - 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0x00, - 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x5a, 0x66, 0x42, 0x00, 0x00, // 0x57 - 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0x7f, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x42, 0x42, 0x42, 0x66, 0x3c, 0x18, 0x3c, 0x66, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x58 - 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, - 0x00, 0x44, 0x44, 0x28, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x59 - 0xff, 0xff, 0xff, 0x07, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x7e, 0x02, 0x02, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x5a - 0x7e, 0x7e, 0x7e, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x3c, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x3c, 0x00, 0x00, // 0x5b - 0x00, 0xe0, 0xe0, 0xf0, 0x70, 0x38, 0x38, 0x1c, 0x1c, 0x0e, 0x0f, 0x07, 0x07, 0x00, - 0x00, 0x00, 0x40, 0x20, 0x20, 0x10, 0x10, 0x08, 0x08, 0x04, 0x04, 0x02, 0x00, 0x00, // 0x5c - 0x7e, 0x7e, 0x7e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x3c, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x3c, 0x00, 0x00, // 0x5d - 0x3c, 0x7e, 0x7e, 0x7e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x18, 0x24, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x5e - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x00, 0x00, // 0x5f - 0x30, 0x38, 0x1c, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x10, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x60 - 0x00, 0x00, 0x00, 0x00, 0x7c, 0xfc, 0xfe, 0xee, 0xfe, 0xfe, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x38, 0x44, 0x04, 0x04, 0x3c, 0x44, 0x7a, 0x00, 0x00, // 0x61 - 0x00, 0xe0, 0xe0, 0xe0, 0xe0, 0xfe, 0xff, 0xff, 0xf7, 0xe7, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x00, 0x40, 0x40, 0x40, 0x40, 0x5c, 0x62, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x62 - 0x00, 0x00, 0x00, 0x00, 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xf0, 0xff, 0x7f, 0x3f, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x20, 0x1e, 0x00, 0x00, // 0x63 - 0x00, 0x07, 0x07, 0x07, 0x07, 0x7f, 0xff, 0xff, 0xef, 0xe7, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x00, 0x02, 0x02, 0x02, 0x02, 0x3a, 0x46, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x64 - 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xff, 0xff, 0xff, 0xff, 0x7f, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x5c, 0x40, 0x3e, 0x00, 0x00, // 0x65 - 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, - 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x66 - 0x7c, 0xfe, 0xff, 0xef, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, - 0x00, 0x38, 0x44, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x67 - 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xfc, 0xfe, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x40, 0x40, 0x40, 0x40, 0x40, 0x78, 0x44, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x68 - 0x1c, 0x1c, 0x1c, 0x00, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x00, - 0x00, 0x08, 0x00, 0x00, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x00, 0x00, // 0x69 - 0x1c, 0x1c, 0x1c, 0x00, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x3c, 0xfc, 0xfc, 0xf0, 0x00, - 0x00, 0x08, 0x00, 0x00, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x10, 0x60, 0x00, 0x00, // 0x6a - 0x00, 0xe2, 0xe7, 0xee, 0xfc, 0xf8, 0xf0, 0xf0, 0xf8, 0xfc, 0xee, 0xe7, 0xe2, 0x00, - 0x00, 0x00, 0x42, 0x44, 0x48, 0x50, 0x60, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x6b - 0xf0, 0xf0, 0xf0, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x78, 0x7f, 0x3f, 0x1f, 0x00, - 0x00, 0x60, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x10, 0x0e, 0x00, 0x00, // 0x6c - 0x00, 0x00, 0x00, 0x00, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x66, 0x5a, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x6d - 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x6e - 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x6f - 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x70 - 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x00, - 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x03, 0x02, 0x00, 0x00, // 0x71 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x72 - 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xfe, 0xff, 0x7f, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x40, 0x40, 0x3c, 0x02, 0x02, 0x7e, 0x00, 0x00, // 0x73 - 0x00, 0xe0, 0xe0, 0xe0, 0xe0, 0xfc, 0xfc, 0xfc, 0xe0, 0xf0, 0xfe, 0xfe, 0x7e, 0x00, - 0x00, 0x00, 0x40, 0x40, 0x40, 0x40, 0x78, 0x40, 0x40, 0x40, 0x60, 0x3c, 0x00, 0x00, // 0x74 - 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x75 - 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x3c, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x24, 0x18, 0x00, 0x00, // 0x76 - 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x5a, 0x66, 0x42, 0x00, 0x00, // 0x77 - 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0xff, 0xe7, 0xe7, 0xe7, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x24, 0x18, 0x24, 0x42, 0x42, 0x00, 0x00, // 0x78 - 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0x07, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, - 0x00, 0x42, 0x42, 0x42, 0x42, 0x7c, 0x02, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x79 - 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x0e, 0x1c, 0x38, 0x70, 0xff, 0xff, 0xff, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x7e, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x7e, 0x00, 0x00, // 0x7a - 0x00, 0x3f, 0x7f, 0x7f, 0x70, 0xf0, 0xf0, 0xf0, 0xf0, 0x70, 0x7f, 0x7f, 0x3f, 0x00, - 0x00, 0x00, 0x1e, 0x20, 0x20, 0x20, 0x40, 0x40, 0x20, 0x20, 0x20, 0x1e, 0x00, 0x00, // 0x7b - 0x00, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, - 0x00, 0x00, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x7c - 0x00, 0xfc, 0xfe, 0xfe, 0x0e, 0x0f, 0x0f, 0x0f, 0x0f, 0x0e, 0xfe, 0xfe, 0xfc, 0x00, - 0x00, 0x00, 0x78, 0x04, 0x04, 0x04, 0x02, 0x02, 0x04, 0x04, 0x04, 0x78, 0x00, 0x00, // 0x7d - 0x00, 0x10, 0x3a, 0x7f, 0x2e, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x10, 0x2a, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x7e - 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x80 - 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x81 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x82 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x83 - 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x84 - 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x85 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x86 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x87 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x00, 0x00, // 0x88 - 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x7e, 0x7e, 0x3c, 0x00, - 0x00, 0x00, 0x18, 0x08, 0x18, 0x08, 0x18, 0x08, 0x18, 0x08, 0x3c, 0x3c, 0x00, 0x00, // 0x89 - 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x6e, 0x68, 0x08, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8a - 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x1f, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x69, 0x6a, 0x0c, 0x0c, 0x0a, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8b - 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x1c, 0x1c, 0x1c, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x6e, 0x68, 0x08, 0x0e, 0x08, 0x08, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8c - 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x7c, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x38, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x8d - 0x00, 0x00, 0x00, 0x3e, 0x7f, 0x7f, 0x7f, 0x7f, 0x7f, 0x3e, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x36, 0x36, 0x32, 0x3e, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00 // 0x8e + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x20 + 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, + 0x00, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, // 0x21 + 0x66, 0x66, 0x66, 0x66, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x24, 0x24, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x22 + 0x7e, 0xff, 0xff, 0xff, 0x7e, 0xff, 0xff, 0xff, 0x7e, 0xff, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x24, 0x7e, 0x24, 0x24, 0x24, 0x7e, 0x24, 0x24, 0x24, 0x7e, 0x24, 0x00, 0x00, // 0x23 + 0x00, 0x70, 0x70, 0x72, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x4e, 0x0e, 0x0e, 0x00, + 0x00, 0x00, 0x20, 0x00, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x00, 0x04, 0x00, 0x00, // 0x25 + 0x38, 0x3c, 0x7c, 0x7c, 0x7c, 0xfc, 0xee, 0xef, 0xef, 0xff, 0xff, 0x7e, 0x00, 0x00, + 0x00, 0x18, 0x28, 0x10, 0x28, 0x28, 0x44, 0x44, 0x46, 0x44, 0x3a, 0x00, 0x00, 0x00, // 0x26 + 0x38, 0x38, 0x38, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x27 + 0x1e, 0x3e, 0x7e, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x7e, 0x3e, 0x1e, 0x00, + 0x00, 0x1c, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x1c, 0x00, 0x00, // 0x28 + 0x78, 0x7c, 0x7e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x7e, 0x7c, 0x78, 0x00, + 0x00, 0x38, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x38, 0x00, 0x00, // 0x29 + 0x38, 0x7c, 0xfe, 0xfe, 0xfe, 0xee, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x10, 0x7c, 0x28, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2a + 0x00, 0x00, 0x00, 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x10, 0x10, 0x7c, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2b + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x38, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x10, 0x00, 0x00, // 0x2c + 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2d + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, // 0x2e + 0x00, 0x07, 0x07, 0x0f, 0x0e, 0x1c, 0x1c, 0x38, 0x38, 0x70, 0xf0, 0xe0, 0xe0, 0x00, + 0x00, 0x00, 0x02, 0x04, 0x04, 0x08, 0x08, 0x10, 0x10, 0x20, 0x20, 0x40, 0x00, 0x00, // 0x2f + 0x7e, 0xff, 0xff, 0xe7, 0xef, 0xff, 0xff, 0xf7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x46, 0x4a, 0x52, 0x62, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x30 + 0x3c, 0x7c, 0xfc, 0xfc, 0x5c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x38, 0x68, 0x48, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x7e, 0x00, 0x00, // 0x31 + 0x3e, 0x7f, 0xff, 0xe7, 0x47, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x3c, 0x62, 0x42, 0x02, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x7e, 0x00, 0x00, // 0x32 + 0xfe, 0xff, 0xff, 0x07, 0x07, 0x7f, 0x7e, 0x7f, 0x07, 0x07, 0xff, 0xff, 0xfe, 0x00, + 0x00, 0x7c, 0x02, 0x02, 0x02, 0x04, 0x38, 0x04, 0x02, 0x02, 0x02, 0x7c, 0x00, 0x00, // 0x33 + 0x04, 0x0e, 0x1e, 0x3e, 0x7e, 0xff, 0xff, 0xff, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x00, + 0x00, 0x04, 0x0c, 0x14, 0x24, 0x44, 0x7e, 0x04, 0x04, 0x04, 0x04, 0x04, 0x00, 0x00, // 0x34 + 0xff, 0xff, 0xff, 0xe0, 0xfc, 0xfe, 0xff, 0x0f, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, + 0x00, 0x7e, 0x40, 0x40, 0x40, 0x78, 0x04, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x35 + 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x3c, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x36 + 0xff, 0xff, 0xff, 0x07, 0x07, 0x0e, 0x0e, 0x1c, 0x1c, 0x38, 0x38, 0x70, 0x70, 0x00, + 0x00, 0x7e, 0x02, 0x02, 0x02, 0x04, 0x04, 0x08, 0x08, 0x10, 0x10, 0x20, 0x00, 0x00, // 0x37 + 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0x7e, 0x3c, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x38 + 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x7f, 0x7f, 0x7f, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x02, 0x3c, 0x00, 0x00, // 0x39 + 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, // 0x3a + 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x38, 0x00, + 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x10, 0x00, 0x00, // 0x3b + 0x00, 0x00, 0x04, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x70, 0x38, 0x1c, 0x0e, 0x04, 0x00, + 0x00, 0x00, 0x00, 0x04, 0x08, 0x10, 0x20, 0x40, 0x20, 0x10, 0x08, 0x04, 0x00, 0x00, // 0x3c + 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x00, 0x7e, 0x7e, 0x7e, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x3d + 0x00, 0x00, 0x20, 0x70, 0x38, 0x1c, 0x0e, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0x20, 0x00, + 0x00, 0x00, 0x00, 0x20, 0x10, 0x08, 0x04, 0x02, 0x04, 0x08, 0x10, 0x20, 0x00, 0x00, // 0x3e + 0x3c, 0xfe, 0xff, 0xe7, 0xe7, 0x0e, 0x1c, 0x38, 0x38, 0x00, 0x38, 0x38, 0x38, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x02, 0x04, 0x08, 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, // 0x3f + 0x18, 0x3c, 0x7e, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x7c, 0x3e, 0x1e, 0x00, + 0x00, 0x18, 0x24, 0x42, 0x42, 0x42, 0x4a, 0x52, 0x52, 0x4c, 0x20, 0x1e, 0x00, 0x00, // 0x40 + 0x7e, 0xff, 0xff, 0xff, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x3c, 0x66, 0x42, 0x42, 0x42, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x41 + 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0x00, + 0x00, 0x7c, 0x46, 0x42, 0x42, 0x42, 0x7c, 0x42, 0x42, 0x42, 0x46, 0x7c, 0x00, 0x00, // 0x42 + 0x7f, 0xff, 0xff, 0xf7, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xf7, 0xff, 0xff, 0x7f, 0x00, + 0x00, 0x3e, 0x62, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x62, 0x3e, 0x00, 0x00, // 0x43 + 0xfe, 0xff, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xef, 0xff, 0xff, 0xfe, 0x00, + 0x00, 0x7c, 0x46, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x46, 0x7c, 0x00, 0x00, // 0x44 + 0xff, 0xff, 0xff, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x45 + 0xff, 0xff, 0xff, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, + 0x00, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x46 + 0x7f, 0xff, 0xff, 0xf0, 0xe0, 0xef, 0xef, 0xef, 0xe7, 0xf7, 0xff, 0xff, 0x7f, 0x00, + 0x00, 0x3e, 0x60, 0x40, 0x40, 0x40, 0x46, 0x42, 0x42, 0x42, 0x62, 0x3e, 0x00, 0x00, // 0x47 + 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x48 + 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x7c, 0x00, 0x00, // 0x49 + 0xff, 0xff, 0xff, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x3c, 0xfc, 0xfc, 0xf8, 0x00, + 0x00, 0x7e, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x18, 0x70, 0x00, 0x00, // 0x4a + 0xe7, 0xef, 0xff, 0xfe, 0xfc, 0xf8, 0xf0, 0xf8, 0xfc, 0xfe, 0xff, 0xef, 0xe7, 0x00, + 0x00, 0x42, 0x44, 0x48, 0x50, 0x60, 0x40, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x4b + 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x4c + 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x42, 0x66, 0x5a, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x4d + 0xe7, 0xf7, 0xff, 0xff, 0xff, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x42, 0x62, 0x52, 0x4a, 0x46, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x4e + 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x4f + 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, + 0x00, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x50 + 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x02, 0x02, 0x00, 0x00, // 0x51 + 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xf8, 0xfc, 0xee, 0xe7, 0xe3, 0x00, + 0x00, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x52 + 0xff, 0xff, 0xff, 0xe0, 0xf0, 0xfe, 0xff, 0x7f, 0x0f, 0x07, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x7e, 0x40, 0x40, 0x40, 0x60, 0x3c, 0x06, 0x02, 0x02, 0x02, 0x7e, 0x00, 0x00, // 0x53 + 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, + 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x54 + 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0x7e, 0x3c, 0x00, + 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x55 + 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xfe, 0x7e, 0x7e, 0x7e, 0x3c, 0x3c, 0x00, + 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x24, 0x24, 0x24, 0x18, 0x18, 0x00, 0x00, // 0x56 + 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0x00, + 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x5a, 0x66, 0x42, 0x00, 0x00, // 0x57 + 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0x7f, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x42, 0x42, 0x42, 0x66, 0x3c, 0x18, 0x3c, 0x66, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x58 + 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, + 0x00, 0x44, 0x44, 0x28, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x59 + 0xff, 0xff, 0xff, 0x07, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x7e, 0x02, 0x02, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x5a + 0x7e, 0x7e, 0x7e, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x3c, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x3c, 0x00, 0x00, // 0x5b + 0x00, 0xe0, 0xe0, 0xf0, 0x70, 0x38, 0x38, 0x1c, 0x1c, 0x0e, 0x0f, 0x07, 0x07, 0x00, + 0x00, 0x00, 0x40, 0x20, 0x20, 0x10, 0x10, 0x08, 0x08, 0x04, 0x04, 0x02, 0x00, 0x00, // 0x5c + 0x7e, 0x7e, 0x7e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x3c, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x3c, 0x00, 0x00, // 0x5d + 0x3c, 0x7e, 0x7e, 0x7e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x18, 0x24, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x5e + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x00, 0x00, // 0x5f + 0x30, 0x38, 0x1c, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x10, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x60 + 0x00, 0x00, 0x00, 0x00, 0x7c, 0xfc, 0xfe, 0xee, 0xfe, 0xfe, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x38, 0x44, 0x04, 0x04, 0x3c, 0x44, 0x7a, 0x00, 0x00, // 0x61 + 0x00, 0xe0, 0xe0, 0xe0, 0xe0, 0xfe, 0xff, 0xff, 0xf7, 0xe7, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x00, 0x40, 0x40, 0x40, 0x40, 0x5c, 0x62, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x62 + 0x00, 0x00, 0x00, 0x00, 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xf0, 0xff, 0x7f, 0x3f, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x20, 0x1e, 0x00, 0x00, // 0x63 + 0x00, 0x07, 0x07, 0x07, 0x07, 0x7f, 0xff, 0xff, 0xef, 0xe7, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x00, 0x02, 0x02, 0x02, 0x02, 0x3a, 0x46, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x64 + 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xff, 0xff, 0xff, 0xff, 0x7f, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x5c, 0x40, 0x3e, 0x00, 0x00, // 0x65 + 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, + 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x66 + 0x7c, 0xfe, 0xff, 0xef, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, + 0x00, 0x38, 0x44, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x67 + 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xfc, 0xfe, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x40, 0x40, 0x40, 0x40, 0x40, 0x78, 0x44, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x68 + 0x1c, 0x1c, 0x1c, 0x00, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x00, + 0x00, 0x08, 0x00, 0x00, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x00, 0x00, // 0x69 + 0x1c, 0x1c, 0x1c, 0x00, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x3c, 0xfc, 0xfc, 0xf0, 0x00, + 0x00, 0x08, 0x00, 0x00, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x10, 0x60, 0x00, 0x00, // 0x6a + 0x00, 0xe2, 0xe7, 0xee, 0xfc, 0xf8, 0xf0, 0xf0, 0xf8, 0xfc, 0xee, 0xe7, 0xe2, 0x00, + 0x00, 0x00, 0x42, 0x44, 0x48, 0x50, 0x60, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x6b + 0xf0, 0xf0, 0xf0, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x78, 0x7f, 0x3f, 0x1f, 0x00, + 0x00, 0x60, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x10, 0x0e, 0x00, 0x00, // 0x6c + 0x00, 0x00, 0x00, 0x00, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x66, 0x5a, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x6d + 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x6e + 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x6f + 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x70 + 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x00, + 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x03, 0x02, 0x00, 0x00, // 0x71 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x72 + 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xfe, 0xff, 0x7f, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x40, 0x40, 0x3c, 0x02, 0x02, 0x7e, 0x00, 0x00, // 0x73 + 0x00, 0xe0, 0xe0, 0xe0, 0xe0, 0xfc, 0xfc, 0xfc, 0xe0, 0xf0, 0xfe, 0xfe, 0x7e, 0x00, + 0x00, 0x00, 0x40, 0x40, 0x40, 0x40, 0x78, 0x40, 0x40, 0x40, 0x60, 0x3c, 0x00, 0x00, // 0x74 + 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x75 + 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x3c, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x24, 0x18, 0x00, 0x00, // 0x76 + 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x5a, 0x66, 0x42, 0x00, 0x00, // 0x77 + 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0xff, 0xe7, 0xe7, 0xe7, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x24, 0x18, 0x24, 0x42, 0x42, 0x00, 0x00, // 0x78 + 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0x07, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, + 0x00, 0x42, 0x42, 0x42, 0x42, 0x7c, 0x02, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x79 + 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x0e, 0x1c, 0x38, 0x70, 0xff, 0xff, 0xff, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x7e, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x7e, 0x00, 0x00, // 0x7a + 0x00, 0x3f, 0x7f, 0x7f, 0x70, 0xf0, 0xf0, 0xf0, 0xf0, 0x70, 0x7f, 0x7f, 0x3f, 0x00, + 0x00, 0x00, 0x1e, 0x20, 0x20, 0x20, 0x40, 0x40, 0x20, 0x20, 0x20, 0x1e, 0x00, 0x00, // 0x7b + 0x00, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, + 0x00, 0x00, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x7c + 0x00, 0xfc, 0xfe, 0xfe, 0x0e, 0x0f, 0x0f, 0x0f, 0x0f, 0x0e, 0xfe, 0xfe, 0xfc, 0x00, + 0x00, 0x00, 0x78, 0x04, 0x04, 0x04, 0x02, 0x02, 0x04, 0x04, 0x04, 0x78, 0x00, 0x00, // 0x7d + 0x00, 0x10, 0x3a, 0x7f, 0x2e, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x10, 0x2a, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x7e + 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x80 + 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x81 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x82 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x83 + 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x84 + 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x85 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x86 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x87 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x00, 0x00, // 0x88 + 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x7e, 0x7e, 0x3c, 0x00, + 0x00, 0x00, 0x18, 0x08, 0x18, 0x08, 0x18, 0x08, 0x18, 0x08, 0x3c, 0x3c, 0x00, 0x00, // 0x89 + 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x6e, 0x68, 0x08, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8a + 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x1f, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x69, 0x6a, 0x0c, 0x0c, 0x0a, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8b + 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x1c, 0x1c, 0x1c, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x6e, 0x68, 0x08, 0x0e, 0x08, 0x08, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8c + 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x7c, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x38, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x8d + 0x00, 0x00, 0x00, 0x3e, 0x7f, 0x7f, 0x7f, 0x7f, 0x7f, 0x3e, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x36, 0x36, 0x32, 0x3e, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00 // 0x8e }; diff --git a/flight/targets/boards/osd/firmware/font_outlined8x8.c b/flight/targets/boards/osd/firmware/font_outlined8x8.c index 78561fb48..806374f25 100644 --- a/flight/targets/boards/osd/firmware/font_outlined8x8.c +++ b/flight/targets/boards/osd/firmware/font_outlined8x8.c @@ -6,177 +6,177 @@ * Commons licenses (version 3.0 BY-SA) and *not* the GPL. */ -// This data is generated by a Python script from +// This data is generated by a Python script from // the original font .pngs. // 0xff = indicates character not present // any other byte contains the index of the character. const char font_lookup_outlined8x8[256] = { - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0x00, 0x01, 0x02, 0x03, 0xff, 0xff, 0xff, 0x04, - 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, - 0x0d, 0x0e, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, - 0x15, 0x16, 0x17, 0xff, 0x18, 0x19, 0x1a, 0x1b, - 0xff, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, 0x21, 0x22, - 0x23, 0x24, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2a, - 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31, 0x32, - 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0xff, 0x39, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f, 0x40, 0x41, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0x00, 0x01, 0x02, 0x03, 0xff, 0xff, 0xff, 0x04, + 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, + 0x0d, 0x0e, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, + 0x15, 0x16, 0x17, 0xff, 0x18, 0x19, 0x1a, 0x1b, + 0xff, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, 0x21, 0x22, + 0x23, 0x24, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2a, + 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31, 0x32, + 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0xff, 0x39, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f, 0x40, 0x41, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }; const char font_data_outlined8x8[] = { - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x20 - 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, - 0x00, 0x10, 0x10, 0x10, 0x00, 0x10, 0x00, 0x00, // 0x21 - 0x44, 0xee, 0xee, 0x44, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x44, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x22 - 0x28, 0x7c, 0xfe, 0x7c, 0xfe, 0x7c, 0x28, 0x00, - 0x00, 0x28, 0x7c, 0x28, 0x7c, 0x28, 0x00, 0x00, // 0x23 - 0x10, 0x38, 0x38, 0x10, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x27 - 0x10, 0x38, 0x70, 0x70, 0x70, 0x38, 0x10, 0x00, - 0x00, 0x10, 0x20, 0x20, 0x20, 0x10, 0x00, 0x00, // 0x28 - 0x20, 0x70, 0x38, 0x38, 0x38, 0x70, 0x20, 0x00, - 0x00, 0x20, 0x10, 0x10, 0x10, 0x20, 0x00, 0x00, // 0x29 - 0x28, 0x7c, 0x38, 0x7c, 0x28, 0x00, 0x00, 0x00, - 0x00, 0x28, 0x10, 0x28, 0x00, 0x00, 0x00, 0x00, // 0x2a - 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x00, - 0x00, 0x10, 0x10, 0x7c, 0x10, 0x10, 0x00, 0x00, // 0x2b - 0x00, 0x00, 0x78, 0x78, 0x78, 0x78, 0x38, 0x00, - 0x00, 0x00, 0x00, 0x30, 0x30, 0x10, 0x00, 0x00, // 0x2c - 0x00, 0x00, 0xfe, 0xfe, 0xfe, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, 0x00, 0x00, // 0x2d - 0x00, 0x00, 0x00, 0x78, 0x78, 0x78, 0x78, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x00, 0x00, // 0x2e - 0x04, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x40, 0x00, - 0x00, 0x04, 0x08, 0x10, 0x20, 0x40, 0x00, 0x00, // 0x2f - 0x7c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7c, 0x00, - 0x00, 0x38, 0x44, 0x54, 0x44, 0x38, 0x00, 0x00, // 0x30 - 0x3c, 0x7c, 0xfc, 0xfc, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x18, 0x28, 0x48, 0x08, 0x7c, 0x00, 0x00, // 0x31 - 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x78, 0x04, 0x78, 0x40, 0x7c, 0x00, 0x00, // 0x32 - 0xfe, 0xfe, 0xfe, 0x7e, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x04, 0x3c, 0x04, 0x7c, 0x00, 0x00, // 0x33 - 0xee, 0xee, 0xfe, 0xfe, 0x7e, 0x0e, 0x0e, 0x00, - 0x00, 0x44, 0x44, 0x3c, 0x04, 0x04, 0x00, 0x00, // 0x34 - 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, - 0x00, 0x7c, 0x40, 0x78, 0x04, 0x78, 0x00, 0x00, // 0x35 - 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x40, 0x7c, 0x44, 0x7c, 0x00, 0x00, // 0x36 - 0xfe, 0xfe, 0xfe, 0x38, 0x70, 0xe0, 0xc0, 0x00, - 0x00, 0x7c, 0x08, 0x10, 0x20, 0x40, 0x00, 0x00, // 0x37 - 0x7c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7c, 0x00, - 0x00, 0x38, 0x44, 0x7c, 0x44, 0x38, 0x00, 0x00, // 0x38 - 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7e, 0x7e, 0x00, - 0x00, 0x7c, 0x44, 0x7c, 0x04, 0x3c, 0x00, 0x00, // 0x39 - 0x78, 0x78, 0x78, 0x78, 0x78, 0x78, 0x78, 0x00, - 0x00, 0x30, 0x30, 0x00, 0x30, 0x30, 0x00, 0x00, // 0x3a - 0x08, 0x1c, 0x38, 0x70, 0x38, 0x1c, 0x08, 0x00, - 0x00, 0x08, 0x10, 0x20, 0x10, 0x08, 0x00, 0x00, // 0x3c - 0xfe, 0xfe, 0xfe, 0x00, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, // 0x3d - 0x20, 0x70, 0x38, 0x1c, 0x38, 0x70, 0x20, 0x00, - 0x00, 0x20, 0x10, 0x08, 0x10, 0x20, 0x00, 0x00, // 0x3e - 0x7c, 0xfe, 0xfe, 0xfe, 0x3c, 0x38, 0x38, 0x00, - 0x00, 0x38, 0x44, 0x18, 0x00, 0x10, 0x00, 0x00, // 0x3f - 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0xee, 0x00, - 0x00, 0x7c, 0x44, 0x7c, 0x44, 0x44, 0x00, 0x00, // 0x41 - 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, - 0x00, 0x78, 0x44, 0x7c, 0x44, 0x78, 0x00, 0x00, // 0x42 - 0xfe, 0xfe, 0xfe, 0xe0, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x40, 0x40, 0x40, 0x7c, 0x00, 0x00, // 0x43 - 0xfc, 0xfe, 0xfe, 0xee, 0xfe, 0xfe, 0xfc, 0x00, - 0x00, 0x78, 0x44, 0x44, 0x44, 0x78, 0x00, 0x00, // 0x44 - 0xfe, 0xfe, 0xfe, 0xfc, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x40, 0x78, 0x40, 0x7c, 0x00, 0x00, // 0x45 - 0xfe, 0xfe, 0xfe, 0xfc, 0xfc, 0xe0, 0xe0, 0x00, - 0x00, 0x7c, 0x40, 0x78, 0x40, 0x40, 0x00, 0x00, // 0x46 - 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x40, 0x5c, 0x44, 0x7c, 0x00, 0x00, // 0x47 - 0xee, 0xee, 0xfe, 0xfe, 0xfe, 0xee, 0xee, 0x00, - 0x00, 0x44, 0x44, 0x7c, 0x44, 0x44, 0x00, 0x00, // 0x48 - 0xfe, 0xfe, 0xfe, 0x38, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x10, 0x10, 0x10, 0x7c, 0x00, 0x00, // 0x49 - 0xfe, 0xfe, 0xfe, 0x1c, 0xfc, 0xfc, 0xfc, 0x00, - 0x00, 0x7c, 0x08, 0x08, 0x08, 0x78, 0x00, 0x00, // 0x4a - 0xe6, 0xee, 0xfc, 0xf8, 0xfc, 0xee, 0xe6, 0x00, - 0x00, 0x44, 0x48, 0x70, 0x48, 0x44, 0x00, 0x00, // 0x4b - 0xe0, 0xe0, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x00, 0x00, // 0x4c - 0x6c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0x00, - 0x00, 0x28, 0x54, 0x54, 0x54, 0x44, 0x00, 0x00, // 0x4d - 0xee, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0x00, - 0x00, 0x44, 0x64, 0x54, 0x4c, 0x44, 0x00, 0x00, // 0x4e - 0xfe, 0xfe, 0xfe, 0xee, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x44, 0x44, 0x44, 0x7c, 0x00, 0x00, // 0x4f - 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0x00, - 0x00, 0x7c, 0x44, 0x7c, 0x40, 0x40, 0x00, 0x00, // 0x50 - 0xfe, 0xfe, 0xfe, 0xee, 0xfc, 0xfe, 0xf6, 0x00, - 0x00, 0x7c, 0x44, 0x44, 0x48, 0x74, 0x00, 0x00, // 0x51 - 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0xe8, 0x00, - 0x00, 0x7c, 0x44, 0x7c, 0x50, 0x48, 0x00, 0x00, // 0x52 - 0x7e, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, - 0x00, 0x3c, 0x40, 0x7c, 0x04, 0x78, 0x00, 0x00, // 0x53 - 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x00, - 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x54 - 0xee, 0xee, 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x00, - 0x00, 0x44, 0x44, 0x44, 0x44, 0x38, 0x00, 0x00, // 0x55 - 0xee, 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x00, - 0x00, 0x44, 0x44, 0x44, 0x28, 0x10, 0x00, 0x00, // 0x56 - 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x6c, 0x00, - 0x00, 0x54, 0x54, 0x54, 0x54, 0x28, 0x00, 0x00, // 0x57 - 0xee, 0xfe, 0xfe, 0x7c, 0xfe, 0xfe, 0xee, 0x00, - 0x00, 0x44, 0x28, 0x10, 0x28, 0x44, 0x00, 0x00, // 0x58 - 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x38, 0x38, 0x00, - 0x00, 0x44, 0x28, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x59 - 0xfe, 0xfe, 0xfe, 0x78, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x7c, 0x08, 0x10, 0x20, 0x7c, 0x00, 0x00, // 0x5a - 0x38, 0x7c, 0x78, 0x70, 0x78, 0x7c, 0x38, 0x00, - 0x00, 0x38, 0x20, 0x20, 0x20, 0x38, 0x00, 0x00, // 0x5b - 0x40, 0xe0, 0x70, 0x38, 0x1c, 0x0e, 0x04, 0x00, - 0x00, 0x40, 0x20, 0x10, 0x08, 0x04, 0x00, 0x00, // 0x5c - 0x38, 0x7c, 0x3c, 0x1c, 0x3c, 0x7c, 0x38, 0x00, - 0x00, 0x38, 0x08, 0x08, 0x08, 0x38, 0x00, 0x00, // 0x5d - 0x00, 0x00, 0x00, 0x00, 0xfe, 0xfe, 0xfe, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, // 0x5f - 0x10, 0x38, 0x7c, 0xfe, 0xfe, 0x38, 0x38, 0x00, - 0x00, 0x10, 0x38, 0x54, 0x10, 0x10, 0x00, 0x00, // 0x80 - 0x3e, 0x3e, 0x1e, 0x3e, 0x76, 0xe0, 0x40, 0x00, - 0x00, 0x1c, 0x0c, 0x14, 0x20, 0x40, 0x00, 0x00, // 0x81 - 0x10, 0x38, 0xfc, 0xfe, 0xfc, 0x38, 0x10, 0x00, - 0x00, 0x10, 0x08, 0x7c, 0x08, 0x10, 0x00, 0x00, // 0x82 - 0x40, 0xe0, 0x76, 0x3e, 0x1e, 0x3e, 0x3e, 0x00, - 0x00, 0x40, 0x20, 0x14, 0x0c, 0x1c, 0x00, 0x00, // 0x83 - 0x38, 0x38, 0xfe, 0xfe, 0x7c, 0x38, 0x10, 0x00, - 0x00, 0x10, 0x10, 0x54, 0x38, 0x10, 0x00, 0x00, // 0x84 - 0x04, 0x0e, 0xdc, 0xf8, 0xf0, 0xf8, 0xf8, 0x00, - 0x00, 0x04, 0x08, 0x50, 0x60, 0x70, 0x00, 0x00, // 0x85 - 0x10, 0x38, 0x7e, 0xfe, 0x7e, 0x38, 0x10, 0x00, - 0x00, 0x10, 0x20, 0x7c, 0x20, 0x10, 0x00, 0x00, // 0x86 - 0xf8, 0xf8, 0xf0, 0xf8, 0xdc, 0x0e, 0x04, 0x00, - 0x00, 0x70, 0x60, 0x50, 0x08, 0x04, 0x00, 0x00 // 0x87 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x20 + 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, + 0x00, 0x10, 0x10, 0x10, 0x00, 0x10, 0x00, 0x00, // 0x21 + 0x44, 0xee, 0xee, 0x44, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x44, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x22 + 0x28, 0x7c, 0xfe, 0x7c, 0xfe, 0x7c, 0x28, 0x00, + 0x00, 0x28, 0x7c, 0x28, 0x7c, 0x28, 0x00, 0x00, // 0x23 + 0x10, 0x38, 0x38, 0x10, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x27 + 0x10, 0x38, 0x70, 0x70, 0x70, 0x38, 0x10, 0x00, + 0x00, 0x10, 0x20, 0x20, 0x20, 0x10, 0x00, 0x00, // 0x28 + 0x20, 0x70, 0x38, 0x38, 0x38, 0x70, 0x20, 0x00, + 0x00, 0x20, 0x10, 0x10, 0x10, 0x20, 0x00, 0x00, // 0x29 + 0x28, 0x7c, 0x38, 0x7c, 0x28, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x10, 0x28, 0x00, 0x00, 0x00, 0x00, // 0x2a + 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x00, + 0x00, 0x10, 0x10, 0x7c, 0x10, 0x10, 0x00, 0x00, // 0x2b + 0x00, 0x00, 0x78, 0x78, 0x78, 0x78, 0x38, 0x00, + 0x00, 0x00, 0x00, 0x30, 0x30, 0x10, 0x00, 0x00, // 0x2c + 0x00, 0x00, 0xfe, 0xfe, 0xfe, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, 0x00, 0x00, // 0x2d + 0x00, 0x00, 0x00, 0x78, 0x78, 0x78, 0x78, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x00, 0x00, // 0x2e + 0x04, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x40, 0x00, + 0x00, 0x04, 0x08, 0x10, 0x20, 0x40, 0x00, 0x00, // 0x2f + 0x7c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7c, 0x00, + 0x00, 0x38, 0x44, 0x54, 0x44, 0x38, 0x00, 0x00, // 0x30 + 0x3c, 0x7c, 0xfc, 0xfc, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x18, 0x28, 0x48, 0x08, 0x7c, 0x00, 0x00, // 0x31 + 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x78, 0x04, 0x78, 0x40, 0x7c, 0x00, 0x00, // 0x32 + 0xfe, 0xfe, 0xfe, 0x7e, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x04, 0x3c, 0x04, 0x7c, 0x00, 0x00, // 0x33 + 0xee, 0xee, 0xfe, 0xfe, 0x7e, 0x0e, 0x0e, 0x00, + 0x00, 0x44, 0x44, 0x3c, 0x04, 0x04, 0x00, 0x00, // 0x34 + 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, + 0x00, 0x7c, 0x40, 0x78, 0x04, 0x78, 0x00, 0x00, // 0x35 + 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x40, 0x7c, 0x44, 0x7c, 0x00, 0x00, // 0x36 + 0xfe, 0xfe, 0xfe, 0x38, 0x70, 0xe0, 0xc0, 0x00, + 0x00, 0x7c, 0x08, 0x10, 0x20, 0x40, 0x00, 0x00, // 0x37 + 0x7c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7c, 0x00, + 0x00, 0x38, 0x44, 0x7c, 0x44, 0x38, 0x00, 0x00, // 0x38 + 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7e, 0x7e, 0x00, + 0x00, 0x7c, 0x44, 0x7c, 0x04, 0x3c, 0x00, 0x00, // 0x39 + 0x78, 0x78, 0x78, 0x78, 0x78, 0x78, 0x78, 0x00, + 0x00, 0x30, 0x30, 0x00, 0x30, 0x30, 0x00, 0x00, // 0x3a + 0x08, 0x1c, 0x38, 0x70, 0x38, 0x1c, 0x08, 0x00, + 0x00, 0x08, 0x10, 0x20, 0x10, 0x08, 0x00, 0x00, // 0x3c + 0xfe, 0xfe, 0xfe, 0x00, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, // 0x3d + 0x20, 0x70, 0x38, 0x1c, 0x38, 0x70, 0x20, 0x00, + 0x00, 0x20, 0x10, 0x08, 0x10, 0x20, 0x00, 0x00, // 0x3e + 0x7c, 0xfe, 0xfe, 0xfe, 0x3c, 0x38, 0x38, 0x00, + 0x00, 0x38, 0x44, 0x18, 0x00, 0x10, 0x00, 0x00, // 0x3f + 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0xee, 0x00, + 0x00, 0x7c, 0x44, 0x7c, 0x44, 0x44, 0x00, 0x00, // 0x41 + 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, + 0x00, 0x78, 0x44, 0x7c, 0x44, 0x78, 0x00, 0x00, // 0x42 + 0xfe, 0xfe, 0xfe, 0xe0, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x40, 0x40, 0x40, 0x7c, 0x00, 0x00, // 0x43 + 0xfc, 0xfe, 0xfe, 0xee, 0xfe, 0xfe, 0xfc, 0x00, + 0x00, 0x78, 0x44, 0x44, 0x44, 0x78, 0x00, 0x00, // 0x44 + 0xfe, 0xfe, 0xfe, 0xfc, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x40, 0x78, 0x40, 0x7c, 0x00, 0x00, // 0x45 + 0xfe, 0xfe, 0xfe, 0xfc, 0xfc, 0xe0, 0xe0, 0x00, + 0x00, 0x7c, 0x40, 0x78, 0x40, 0x40, 0x00, 0x00, // 0x46 + 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x40, 0x5c, 0x44, 0x7c, 0x00, 0x00, // 0x47 + 0xee, 0xee, 0xfe, 0xfe, 0xfe, 0xee, 0xee, 0x00, + 0x00, 0x44, 0x44, 0x7c, 0x44, 0x44, 0x00, 0x00, // 0x48 + 0xfe, 0xfe, 0xfe, 0x38, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x10, 0x10, 0x10, 0x7c, 0x00, 0x00, // 0x49 + 0xfe, 0xfe, 0xfe, 0x1c, 0xfc, 0xfc, 0xfc, 0x00, + 0x00, 0x7c, 0x08, 0x08, 0x08, 0x78, 0x00, 0x00, // 0x4a + 0xe6, 0xee, 0xfc, 0xf8, 0xfc, 0xee, 0xe6, 0x00, + 0x00, 0x44, 0x48, 0x70, 0x48, 0x44, 0x00, 0x00, // 0x4b + 0xe0, 0xe0, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x00, 0x00, // 0x4c + 0x6c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0x00, + 0x00, 0x28, 0x54, 0x54, 0x54, 0x44, 0x00, 0x00, // 0x4d + 0xee, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0x00, + 0x00, 0x44, 0x64, 0x54, 0x4c, 0x44, 0x00, 0x00, // 0x4e + 0xfe, 0xfe, 0xfe, 0xee, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x44, 0x44, 0x44, 0x7c, 0x00, 0x00, // 0x4f + 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0x00, + 0x00, 0x7c, 0x44, 0x7c, 0x40, 0x40, 0x00, 0x00, // 0x50 + 0xfe, 0xfe, 0xfe, 0xee, 0xfc, 0xfe, 0xf6, 0x00, + 0x00, 0x7c, 0x44, 0x44, 0x48, 0x74, 0x00, 0x00, // 0x51 + 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0xe8, 0x00, + 0x00, 0x7c, 0x44, 0x7c, 0x50, 0x48, 0x00, 0x00, // 0x52 + 0x7e, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, + 0x00, 0x3c, 0x40, 0x7c, 0x04, 0x78, 0x00, 0x00, // 0x53 + 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x00, + 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x54 + 0xee, 0xee, 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x00, + 0x00, 0x44, 0x44, 0x44, 0x44, 0x38, 0x00, 0x00, // 0x55 + 0xee, 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x00, + 0x00, 0x44, 0x44, 0x44, 0x28, 0x10, 0x00, 0x00, // 0x56 + 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x6c, 0x00, + 0x00, 0x54, 0x54, 0x54, 0x54, 0x28, 0x00, 0x00, // 0x57 + 0xee, 0xfe, 0xfe, 0x7c, 0xfe, 0xfe, 0xee, 0x00, + 0x00, 0x44, 0x28, 0x10, 0x28, 0x44, 0x00, 0x00, // 0x58 + 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x38, 0x38, 0x00, + 0x00, 0x44, 0x28, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x59 + 0xfe, 0xfe, 0xfe, 0x78, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x7c, 0x08, 0x10, 0x20, 0x7c, 0x00, 0x00, // 0x5a + 0x38, 0x7c, 0x78, 0x70, 0x78, 0x7c, 0x38, 0x00, + 0x00, 0x38, 0x20, 0x20, 0x20, 0x38, 0x00, 0x00, // 0x5b + 0x40, 0xe0, 0x70, 0x38, 0x1c, 0x0e, 0x04, 0x00, + 0x00, 0x40, 0x20, 0x10, 0x08, 0x04, 0x00, 0x00, // 0x5c + 0x38, 0x7c, 0x3c, 0x1c, 0x3c, 0x7c, 0x38, 0x00, + 0x00, 0x38, 0x08, 0x08, 0x08, 0x38, 0x00, 0x00, // 0x5d + 0x00, 0x00, 0x00, 0x00, 0xfe, 0xfe, 0xfe, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, // 0x5f + 0x10, 0x38, 0x7c, 0xfe, 0xfe, 0x38, 0x38, 0x00, + 0x00, 0x10, 0x38, 0x54, 0x10, 0x10, 0x00, 0x00, // 0x80 + 0x3e, 0x3e, 0x1e, 0x3e, 0x76, 0xe0, 0x40, 0x00, + 0x00, 0x1c, 0x0c, 0x14, 0x20, 0x40, 0x00, 0x00, // 0x81 + 0x10, 0x38, 0xfc, 0xfe, 0xfc, 0x38, 0x10, 0x00, + 0x00, 0x10, 0x08, 0x7c, 0x08, 0x10, 0x00, 0x00, // 0x82 + 0x40, 0xe0, 0x76, 0x3e, 0x1e, 0x3e, 0x3e, 0x00, + 0x00, 0x40, 0x20, 0x14, 0x0c, 0x1c, 0x00, 0x00, // 0x83 + 0x38, 0x38, 0xfe, 0xfe, 0x7c, 0x38, 0x10, 0x00, + 0x00, 0x10, 0x10, 0x54, 0x38, 0x10, 0x00, 0x00, // 0x84 + 0x04, 0x0e, 0xdc, 0xf8, 0xf0, 0xf8, 0xf8, 0x00, + 0x00, 0x04, 0x08, 0x50, 0x60, 0x70, 0x00, 0x00, // 0x85 + 0x10, 0x38, 0x7e, 0xfe, 0x7e, 0x38, 0x10, 0x00, + 0x00, 0x10, 0x20, 0x7c, 0x20, 0x10, 0x00, 0x00, // 0x86 + 0xf8, 0xf8, 0xf0, 0xf8, 0xdc, 0x0e, 0x04, 0x00, + 0x00, 0x70, 0x60, 0x50, 0x08, 0x04, 0x00, 0x00 // 0x87 }; diff --git a/flight/targets/boards/osd/firmware/fonts.c b/flight/targets/boards/osd/firmware/fonts.c index 5b0c22d75..96b6bec9d 100644 --- a/flight/targets/boards/osd/firmware/fonts.c +++ b/flight/targets/boards/osd/firmware/fonts.c @@ -6,12 +6,12 @@ * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. - * + * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * + * * You should have received a copy of the GNU General Public License along * with this program; if not, write to the Free Software Foundation, Inc., * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. @@ -21,15 +21,15 @@ // Font table. Add new fonts here. The table must end with a -1 for the id. struct FontEntry fonts[NUM_FONTS + 1] = { - { 0, 8, 14, "Outlined8x14", - font_lookup_outlined8x14, - font_data_outlined8x14, - 0 }, - { 1, 8, 8, "Outlined8x8", - font_lookup_outlined8x8, - font_data_outlined8x8, - FONT_UPPERCASE_ONLY }, - { 2, 8, 10, "font8x10", 0, 0, 0 }, - { 3, 12, 18, "font12x18", 0, 0, 0 }, - { -1, -1, -1, "", 0, 0, 0 } // ends font table + { 0, 8, 14, "Outlined8x14", + font_lookup_outlined8x14, + font_data_outlined8x14, + 0 }, + { 1, 8, 8, "Outlined8x8", + font_lookup_outlined8x8, + font_data_outlined8x8, + FONT_UPPERCASE_ONLY }, + { 2, 8, 10, "font8x10", 0, 0, 0 }, + { 3, 12, 18, "font12x18", 0, 0, 0 }, + { -1, -1, -1, "", 0, 0, 0 } // ends font table }; diff --git a/flight/targets/boards/osd/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/osd/firmware/inc/FreeRTOSConfig.h index 023d20476..62eed5553 100644 --- a/flight/targets/boards/osd/firmware/inc/FreeRTOSConfig.h +++ b/flight/targets/boards/osd/firmware/inc/FreeRTOSConfig.h @@ -1,85 +1,84 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ /** - * @addtogroup PIOS PIOS - * @{ - * @addtogroup FreeRTOS FreeRTOS - * @{ - */ + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ /* Notes: We use 5 task priorities */ -#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... -#define configTICK_RATE_HZ ((portTickType )1000) -#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) -#define configMINIMAL_STACK_SIZE ((unsigned short)512) -#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total -#define configMAX_TASK_NAME_LEN (16) +#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total +#define configMAX_TASK_NAME_LEN (16) -#define configUSE_PREEMPTION 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configIDLE_SHOULD_YIELD 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 -#define configUSE_TIMERS 1 -#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ -#define configTIMER_QUEUE_LENGTH 10 -#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE +#define configUSE_TIMERS 1 +#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ + to exclude the API function. */ -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 1 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 1 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 /* Enable run time stats collection */ -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 /* * Once we move to CMSIS2 we can at least use: @@ -88,16 +87,17 @@ NVIC value of 255. */ * * (still nothing for the DWT registers, surprisingly) */ -#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ - do { \ - (*(unsigned long *)0xe000edfc) |= (1<<24); /* DEMCR |= DEMCR_TRCENA */ \ - (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ - } while(0) -#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ /** - * @} - */ + * @} + */ #endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/osd/firmware/inc/dcc_stdio.h b/flight/targets/boards/osd/firmware/inc/dcc_stdio.h index dbfd39342..3d7354918 100644 --- a/flight/targets/boards/osd/firmware/inc/dcc_stdio.h +++ b/flight/targets/boards/osd/firmware/inc/dcc_stdio.h @@ -1,24 +1,24 @@ /*************************************************************************** - * Copyright (C) 2008 by Dominic Rath * - * Dominic.Rath@gmx.de * - * Copyright (C) 2008 by Spencer Oliver * - * spen@spen-soft.co.uk * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ #ifndef DCC_STDIO_H #define DCC_STDIO_H @@ -33,4 +33,4 @@ void dbg_write_str(const char *msg); void dbg_write_char(char msg); void dbg_write_hex32(const unsigned long val); -#endif /* DCC_STDIO_H */ +#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/osd/firmware/inc/font12x18.h b/flight/targets/boards/osd/firmware/inc/font12x18.h index ef0a45e27..11a599fed 100644 --- a/flight/targets/boards/osd/firmware/inc/font12x18.h +++ b/flight/targets/boards/osd/firmware/inc/font12x18.h @@ -9,9737 +9,9735 @@ #define FONT12X18_H_ static const uint16_t font_frame12x18[] = { - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x03C0, - 0x0660, - 0x0460, - 0x00C0, - 0x0180, - 0x0300, - 0x07E0, - 0x0000, - 0x0078, - 0x006C, - 0x0066, - 0x0066, - 0x0066, - 0x0066, - 0x006C, - 0x0078, - 0x0000, - - 0x0000, - 0x07C0, - 0x0060, - 0x0060, - 0x03C0, - 0x0060, - 0x0060, - 0x07C0, - 0x0000, - 0x0078, - 0x006C, - 0x0066, - 0x0066, - 0x0066, - 0x0066, - 0x006C, - 0x0078, - 0x0000, - - 0x0000, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0003, - 0x0007, - 0x001F, - 0x0038, - 0x0060, - 0x00C0, - 0x00C0, - 0x0180, - 0x0180, - 0x0181, - - 0x0000, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0C00, - 0x0E00, - 0x0F80, - 0x01C0, - 0x0060, - 0x0030, - 0x0030, - 0x0018, - 0x0018, - 0x0818, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x07FE, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0700, - 0x08C6, - 0x0826, - 0x0818, - 0x0C18, - 0x0C24, - 0x0642, - 0x0382, - 0x0381, - 0x03C1, - 0x00F1, - 0x00BE, - 0x0080, - 0x0080, - 0x01C0, - 0x03E0, - 0x07F0, - 0x0000, - - 0x0000, - 0x01F8, - 0x030C, - 0x0606, - 0x0402, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0000, - 0x07FE, - 0x0606, - 0x06FE, - 0x06C6, - 0x06F6, - 0x0606, - 0x07FE, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x030C, - 0x0606, - 0x0402, - 0x0402, - 0x0000, - 0x07FE, - 0x0606, - 0x06FE, - 0x06C6, - 0x06F6, - 0x0606, - 0x07FE, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0208, - 0x030C, - 0x0390, - 0x03E0, - 0x03E0, - 0x01F0, - 0x00F8, - 0x007C, - 0x0080, - 0x0180, - 0x0280, - 0x0480, - 0x0000, - 0x07F8, - 0x0000, - - 0x0181, - 0x0180, - 0x0180, - 0x00C0, - 0x00C0, - 0x0060, - 0x0038, - 0x001F, - 0x0007, - 0x0003, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0001, - 0x0000, - - 0x0818, - 0x0018, - 0x0018, - 0x0030, - 0x0030, - 0x0060, - 0x01C0, - 0x0F80, - 0x0E00, - 0x0C00, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0800, - 0x0000, - - 0x07FE, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x00F0, - 0x0108, - 0x0108, - 0x0008, - 0x0008, - 0x0008, - 0x0000, - 0x01F8, - 0x030C, - 0x037C, - 0x030C, - 0x037C, - 0x037C, - 0x01F8, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x0108, - 0x0108, - 0x0108, - 0x0000, - 0x01F8, - 0x030C, - 0x037C, - 0x030C, - 0x037C, - 0x037C, - 0x01F8, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x00FF, - 0x01FF, - 0x0387, - 0x0387, - 0x0387, - 0x03FF, - 0x03FF, - 0x03FF, - 0x03FF, - 0x03FF, - 0x03FF, - 0x01FF, - 0x00FF, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FF8, - 0x0FFC, - 0x0FF2, - 0x0C32, - 0x081E, - 0x000E, - 0x000E, - 0x000E, - 0x000E, - 0x081E, - 0x0C3E, - 0x0FFC, - 0x0FF8, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FF8, - 0x0FFC, - 0x0FF2, - 0x0C32, - 0x081E, - 0x000E, - 0x03CE, - 0x07EE, - 0x07EE, - 0x07EE, - 0x07EE, - 0x0BDC, - 0x0C38, - 0x0000, - 0x0000, - 0x0000, - - 0x03F8, - 0x0030, - 0x0060, - 0x01C0, - 0x03F8, - 0x0000, - 0x03F8, - 0x0318, - 0x03F8, - 0x0000, - 0x03F8, - 0x0318, - 0x03F8, - 0x0000, - 0x03B8, - 0x0358, - 0x0318, - 0x0318, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0001, - 0x0003, - 0x0007, - 0x000F, - 0x001F, - 0x003F, - 0x003F, - 0x003F, - 0x003F, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0000, - - 0x0000, - 0x0000, - 0x0800, - 0x0C00, - 0x0E00, - 0x0F00, - 0x0F80, - 0x00C0, - 0x00C0, - 0x00C0, - 0x0FC0, - 0x00C0, - 0x00C0, - 0x00C0, - 0x00C0, - 0x00C0, - 0x00C0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x00F0, - 0x01F8, - 0x03FC, - 0x0000, - 0x07FE, - 0x07E2, - 0x07E2, - 0x0422, - 0x043E, - 0x043E, - 0x043E, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0108, - 0x039C, - 0x018C, - 0x0084, - 0x0084, - 0x0108, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0198, - 0x0198, - 0x0198, - 0x07FE, - 0x07FE, - 0x0198, - 0x0198, - 0x07FE, - 0x07FE, - 0x0198, - 0x0198, - 0x0198, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x01F8, - 0x03FC, - 0x036C, - 0x0360, - 0x03F0, - 0x01F8, - 0x006C, - 0x036C, - 0x03FC, - 0x01F8, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x031C, - 0x0038, - 0x0070, - 0x00E0, - 0x01C0, - 0x038C, - 0x030C, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00E0, - 0x0190, - 0x0110, - 0x0110, - 0x0120, - 0x00C0, - 0x00C0, - 0x0120, - 0x0214, - 0x0208, - 0x0214, - 0x01E2, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0020, - 0x0070, - 0x0030, - 0x0010, - 0x0010, - 0x0020, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x000C, - 0x0018, - 0x0030, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0030, - 0x0018, - 0x000C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0180, - 0x00C0, - 0x0060, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0060, - 0x00C0, - 0x0180, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0402, - 0x0606, - 0x030C, - 0x0198, - 0x00F0, - 0x07FE, - 0x00F0, - 0x0198, - 0x030C, - 0x0606, - 0x0402, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x01F8, - 0x01F8, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x00C0, - 0x0180, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x01F8, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0006, - 0x000E, - 0x001C, - 0x0038, - 0x0070, - 0x00E0, - 0x01C0, - 0x0380, - 0x0700, - 0x0600, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x00E0, - 0x01E0, - 0x01E0, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x01F8, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x031C, - 0x0038, - 0x0070, - 0x00E0, - 0x01C0, - 0x03FC, - 0x03FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03F8, - 0x03FC, - 0x000C, - 0x000C, - 0x00F8, - 0x00F8, - 0x000C, - 0x000C, - 0x03FC, - 0x03F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0038, - 0x0078, - 0x00D8, - 0x0198, - 0x0318, - 0x03FC, - 0x03FC, - 0x0018, - 0x0018, - 0x0018, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x0300, - 0x03F8, - 0x03FC, - 0x000C, - 0x000C, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03F8, - 0x0300, - 0x0300, - 0x03F8, - 0x03FC, - 0x030C, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x000C, - 0x001C, - 0x0038, - 0x0070, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x030C, - 0x01F8, - 0x01F8, - 0x030C, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x030C, - 0x03FC, - 0x01FC, - 0x000C, - 0x000C, - 0x03FC, - 0x03F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x00C0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0018, - 0x0030, - 0x0060, - 0x00C0, - 0x0180, - 0x0180, - 0x00C0, - 0x0060, - 0x0030, - 0x0018, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x01F8, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x01F8, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0300, - 0x0180, - 0x00C0, - 0x0060, - 0x0030, - 0x0030, - 0x0060, - 0x00C0, - 0x0180, - 0x0300, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x070E, - 0x0606, - 0x0006, - 0x000E, - 0x001C, - 0x0038, - 0x0070, - 0x0060, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x0108, - 0x0204, - 0x04D2, - 0x0532, - 0x0512, - 0x0532, - 0x04D4, - 0x0408, - 0x0200, - 0x0104, - 0x00F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x01F8, - 0x03FC, - 0x039C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03F0, - 0x03F8, - 0x031C, - 0x030C, - 0x030C, - 0x031C, - 0x03F8, - 0x03F8, - 0x031C, - 0x030C, - 0x030C, - 0x031C, - 0x03F8, - 0x03F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x01F8, - 0x039C, - 0x030C, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x030C, - 0x039C, - 0x01F8, - 0x00F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03F0, - 0x03F8, - 0x031C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x031C, - 0x03F8, - 0x03F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x03F0, - 0x03F0, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x03FC, - 0x03FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x03F0, - 0x03F0, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x01F0, - 0x03F8, - 0x039C, - 0x030C, - 0x030C, - 0x0300, - 0x0300, - 0x033C, - 0x033C, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x01FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x01F8, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x01F8, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00FC, - 0x00FC, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x0330, - 0x0330, - 0x0330, - 0x0330, - 0x0330, - 0x03F0, - 0x01E0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0300, - 0x030C, - 0x031C, - 0x0338, - 0x0370, - 0x03E0, - 0x03C0, - 0x0380, - 0x03C0, - 0x03E0, - 0x0370, - 0x0338, - 0x031C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x03FC, - 0x03FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x039C, - 0x03FC, - 0x03FC, - 0x036C, - 0x036C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x038C, - 0x038C, - 0x03CC, - 0x03EC, - 0x037C, - 0x033C, - 0x031C, - 0x031C, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x01F8, - 0x039C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x039C, - 0x01F8, - 0x00F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03F0, - 0x03F8, - 0x031C, - 0x030C, - 0x030C, - 0x031C, - 0x03F8, - 0x03F0, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x01F8, - 0x039C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x036C, - 0x037C, - 0x03B8, - 0x01FC, - 0x00EC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03F0, - 0x03F8, - 0x031C, - 0x030C, - 0x030C, - 0x031C, - 0x03F8, - 0x03E0, - 0x03E0, - 0x0370, - 0x0338, - 0x031C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00F8, - 0x01FC, - 0x038C, - 0x0300, - 0x0300, - 0x0380, - 0x01F0, - 0x00F8, - 0x001C, - 0x000C, - 0x000C, - 0x031C, - 0x03F8, - 0x01F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x039C, - 0x01F8, - 0x00F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x039C, - 0x0198, - 0x01F8, - 0x00F0, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x036C, - 0x036C, - 0x036C, - 0x036C, - 0x036C, - 0x036C, - 0x03FC, - 0x01F8, - 0x0198, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x0198, - 0x00F0, - 0x0060, - 0x0060, - 0x00F0, - 0x0198, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x039C, - 0x01F8, - 0x00F0, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x000C, - 0x000C, - 0x001C, - 0x0038, - 0x0070, - 0x00E0, - 0x01C0, - 0x0380, - 0x0300, - 0x0300, - 0x03FC, - 0x03FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0078, - 0x0078, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0078, - 0x0078, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0600, - 0x0700, - 0x0380, - 0x01C0, - 0x00E0, - 0x0070, - 0x0038, - 0x001C, - 0x000E, - 0x0006, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x01E0, - 0x01E0, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x01E0, - 0x01E0, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x00F0, - 0x0198, - 0x0108, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x07FE, - 0x07FE, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0300, - 0x0180, - 0x00C0, - 0x0060, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x01F8, - 0x039C, - 0x030C, - 0x030C, - 0x03FC, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03F8, - 0x03FC, - 0x030C, - 0x030C, - 0x03F8, - 0x03F8, - 0x030C, - 0x030C, - 0x03FC, - 0x03F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03F0, - 0x03F8, - 0x031C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x031C, - 0x03F8, - 0x03F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x0300, - 0x0300, - 0x03F0, - 0x03F0, - 0x0300, - 0x0300, - 0x03FC, - 0x03FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x0300, - 0x0300, - 0x03F8, - 0x03F8, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x038C, - 0x0300, - 0x033C, - 0x033C, - 0x030C, - 0x038C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x00F0, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x00F0, - 0x00F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x007C, - 0x007C, - 0x0018, - 0x0018, - 0x0018, - 0x0018, - 0x0018, - 0x0198, - 0x01F8, - 0x00F0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x031C, - 0x0338, - 0x0370, - 0x03E0, - 0x03E0, - 0x03F0, - 0x0338, - 0x031C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x03FC, - 0x03FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x039C, - 0x03FC, - 0x036C, - 0x036C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x038C, - 0x03CC, - 0x03EC, - 0x037C, - 0x033C, - 0x031C, - 0x030C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03F8, - 0x03FC, - 0x030C, - 0x030C, - 0x03FC, - 0x03F8, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x033C, - 0x0338, - 0x03FC, - 0x01EC, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03F8, - 0x03FC, - 0x030C, - 0x030C, - 0x03F8, - 0x03E0, - 0x0370, - 0x0338, - 0x031C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x03FC, - 0x030C, - 0x0300, - 0x03F8, - 0x01FC, - 0x000C, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x039C, - 0x01F8, - 0x00F0, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x036C, - 0x036C, - 0x03FC, - 0x01F8, - 0x0198, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x039C, - 0x01F8, - 0x00F0, - 0x0060, - 0x0060, - 0x00F0, - 0x01F8, - 0x039C, - 0x030C, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x039C, - 0x01F8, - 0x00F0, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x03FC, - 0x001C, - 0x0038, - 0x0070, - 0x00E0, - 0x01C0, - 0x0380, - 0x03FC, - 0x03FC, - 0x0000, - - 0x0000, - 0x0000, - 0x0038, - 0x0038, - 0x0078, - 0x0060, - 0x0060, - 0x0060, - 0x00C0, - 0x00C0, - 0x0060, - 0x0060, - 0x0060, - 0x0078, - 0x0038, - 0x0038, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x01C0, - 0x01E0, - 0x01E0, - 0x0060, - 0x0060, - 0x0060, - 0x0030, - 0x0030, - 0x0060, - 0x0060, - 0x0060, - 0x01E0, - 0x01E0, - 0x01C0, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00C2, - 0x01E6, - 0x033C, - 0x0618, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0280, - 0x02C0, - 0x02E0, - 0x02F0, - 0x02F8, - 0x02F0, - 0x02E0, - 0x02C0, - 0x0280, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0700, - 0x0000, - - 0x0000, - 0x0660, - 0x06C0, - 0x0700, - 0x06C0, - 0x0660, - 0x0000, - 0x06BE, - 0x0688, - 0x0588, - 0x0588, - 0x0000, - 0x0066, - 0x0066, - 0x007E, - 0x0066, - 0x0066, - 0x0000, - - 0x0000, - 0x0660, - 0x06C0, - 0x0700, - 0x06C0, - 0x0660, - 0x0000, - 0x01DC, - 0x01AC, - 0x01AC, - 0x018C, - 0x0000, - 0x0066, - 0x0066, - 0x007E, - 0x0066, - 0x0066, - 0x0000, - - 0x0000, - 0x07F0, - 0x06B0, - 0x06B0, - 0x0630, - 0x0000, - 0x0078, - 0x0048, - 0x00FC, - 0x00CC, - 0x00CC, - 0x0000, - 0x0066, - 0x0066, - 0x007E, - 0x0066, - 0x0066, - 0x0000, - - 0x0000, - 0x0600, - 0x0600, - 0x0600, - 0x07E0, - 0x07E0, - 0x0000, - 0x00F8, - 0x018C, - 0x01FC, - 0x018C, - 0x0000, - 0x007E, - 0x007E, - 0x0018, - 0x0018, - 0x0018, - 0x0000, - - 0x0000, - 0x0600, - 0x0600, - 0x0600, - 0x07E0, - 0x07E0, - 0x0000, - 0x01F8, - 0x039C, - 0x039C, - 0x01F8, - 0x0000, - 0x00E6, - 0x00F6, - 0x00FE, - 0x00DE, - 0x00CE, - 0x0000, - - 0x0000, - 0x03E0, - 0x0630, - 0x07F0, - 0x0630, - 0x0000, - 0x0180, - 0x0180, - 0x0180, - 0x01F8, - 0x01F8, - 0x0000, - 0x007E, - 0x007E, - 0x0018, - 0x0018, - 0x0018, - 0x0000, - - 0x0000, - 0x0660, - 0x0660, - 0x03C0, - 0x0180, - 0x0000, - 0x01F8, - 0x0180, - 0x01F0, - 0x0180, - 0x01F8, - 0x0000, - 0x0030, - 0x0030, - 0x0030, - 0x003E, - 0x003E, - 0x0000, - - 0x0000, - 0x07E0, - 0x07E0, - 0x0180, - 0x0180, - 0x0180, - 0x0000, - 0x00D8, - 0x00F8, - 0x00D8, - 0x00D8, - 0x0000, - 0x003C, - 0x0022, - 0x0026, - 0x003C, - 0x0036, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0660, - 0x07E0, - 0x05A0, - 0x0420, - 0x0000, - 0x07E0, - 0x0000, - 0x07E0, - 0x0400, - 0x07E0, - 0x0020, - 0x07E0, - 0x0000, - - 0x0000, - 0x03E0, - 0x0630, - 0x07F0, - 0x0630, - 0x0000, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0000, - 0x007E, - 0x0046, - 0x0078, - 0x006C, - 0x0066, - 0x0000, - - 0x0000, - 0x0770, - 0x06B0, - 0x06B0, - 0x06B0, - 0x06B0, - 0x0000, - 0x00F8, - 0x018C, - 0x01FC, - 0x018C, - 0x0000, - 0x0066, - 0x0066, - 0x0066, - 0x003C, - 0x0018, - 0x0000, - - 0x0000, - 0x0200, - 0x020C, - 0x0212, - 0x0012, - 0x0712, - 0x0012, - 0x070C, - 0x0700, - 0x0700, - 0x0712, - 0x0712, - 0x0014, - 0x0218, - 0x0214, - 0x0212, - 0x0212, - 0x0200, - - 0x0000, - 0x0210, - 0x0210, - 0x0210, - 0x001E, - 0x0700, - 0x000C, - 0x0712, - 0x0712, - 0x0712, - 0x070C, - 0x0700, - 0x001E, - 0x0210, - 0x021E, - 0x0202, - 0x021E, - 0x0200, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0618, - 0x0738, - 0x07F8, - 0x06D8, - 0x06D8, - 0x0618, - 0x0618, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0630, - 0x0630, - 0x0630, - 0x0630, - 0x0360, - 0x03E0, - 0x01C0, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01C0, - 0x03E0, - 0x0220, - 0x0630, - 0x07F0, - 0x0630, - 0x0630, - 0x0000, - - 0x0000, - 0x0000, - 0x0003, - 0x001F, - 0x000F, - 0x000F, - 0x000F, - 0x000F, - 0x000F, - 0x000F, - 0x000F, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0800, - 0x0F00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0007, - 0x0003, - 0x0007, - 0x000F, - 0x001F, - 0x003F, - 0x003F, - 0x0007, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0E00, - 0x0F00, - 0x0F80, - 0x0E00, - 0x0E00, - 0x0C00, - 0x0C00, - 0x0800, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0001, - 0x0003, - 0x001F, - 0x007F, - 0x00FF, - 0x007F, - 0x001C, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0E60, - 0x0800, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0001, - 0x003F, - 0x00FF, - 0x007F, - 0x0078, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0700, - 0x0FF0, - 0x0FE0, - 0x0FE0, - 0x08C0, - 0x0040, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x007F, - 0x007F, - 0x007F, - 0x007F, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0300, - 0x0FC0, - 0x0FF8, - 0x0FF0, - 0x0FE0, - 0x0180, - 0x0100, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0010, - 0x003F, - 0x007F, - 0x001F, - 0x0001, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0880, - 0x0FE0, - 0x0FF0, - 0x0FF8, - 0x07F0, - 0x0700, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x000C, - 0x001F, - 0x003F, - 0x001F, - 0x0007, - 0x0001, - 0x0001, - 0x0003, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0C00, - 0x0FC0, - 0x0FE0, - 0x0FE0, - 0x0FF0, - 0x0FF0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x001F, - 0x000F, - 0x0007, - 0x0007, - 0x0003, - 0x0007, - 0x000F, - 0x0001, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0800, - 0x0C00, - 0x0E00, - 0x0F60, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0180, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0007, - 0x0007, - 0x0007, - 0x0007, - 0x000F, - 0x003F, - 0x000F, - 0x0007, - 0x0003, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0F00, - 0x0FC0, - 0x0E00, - 0x0C00, - 0x0800, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0001, - 0x0003, - 0x0007, - 0x006F, - 0x003F, - 0x003F, - 0x003F, - 0x003F, - 0x0018, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0F80, - 0x0F00, - 0x0E00, - 0x0E00, - 0x0C00, - 0x0E00, - 0x0F00, - 0x0800, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0003, - 0x003F, - 0x007F, - 0x007F, - 0x00FF, - 0x00FF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0300, - 0x0F80, - 0x0FC0, - 0x0F80, - 0x0E00, - 0x0800, - 0x0800, - 0x0C00, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0011, - 0x007F, - 0x00FF, - 0x01FF, - 0x00FE, - 0x000E, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0080, - 0x0FC0, - 0x0FE0, - 0x0F80, - 0x0800, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x000C, - 0x003F, - 0x01FF, - 0x00FF, - 0x007F, - 0x0018, - 0x0008, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FE0, - 0x0FE0, - 0x0FE0, - 0x0FE0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x000E, - 0x00FF, - 0x007F, - 0x007F, - 0x0031, - 0x0020, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0800, - 0x0FC0, - 0x0FF0, - 0x0FE0, - 0x01E0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x003F, - 0x003F, - 0x003F, - 0x0067, - 0x0001, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0800, - 0x0C00, - 0x0F80, - 0x0FE0, - 0x0FF0, - 0x0FE0, - 0x0380, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0007, - 0x000F, - 0x001F, - 0x0007, - 0x0007, - 0x0003, - 0x0003, - 0x0001, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0E00, - 0x0C00, - 0x0E00, - 0x0F00, - 0x0F80, - 0x0FC0, - 0x0FC0, - 0x0E00, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0180, - 0x0240, - 0x0240, - 0x0180, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0060, - 0x00F0, - 0x01F8, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x01F8, - 0x00F0, - 0x0060, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0100, - 0x0300, - 0x07C0, - 0x07F0, - 0x0330, - 0x0118, - 0x0018, - 0x0018, - 0x0118, - 0x0330, - 0x07F0, - 0x07C0, - 0x0300, - 0x0100, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x00F0, - 0x07FE, - 0x0606, - 0x0462, - 0x0462, - 0x0462, - 0x0462, - 0x0462, - 0x0462, - 0x0462, - 0x0402, - 0x0462, - 0x0462, - 0x0402, - 0x0606, - 0x07FE, - 0x0000, - - 0x0000, - 0x00F0, - 0x07FE, - 0x070E, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x07FE, - 0x07FE, - 0x0000, - - 0x0000, - 0x00F0, - 0x07FE, - 0x070E, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x0000, - - 0x0000, - 0x00F0, - 0x07FE, - 0x070E, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x0000, - - 0x0000, - 0x00F0, - 0x07FE, - 0x070E, - 0x0606, - 0x0606, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x0000, - - 0x0000, - 0x00F0, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x07FE, - 0x0000, - - 0x0000, - 0x0000, - 0x03FF, - 0x03FF, - 0x0300, - 0x0300, - 0x033C, - 0x033C, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - 0x0300, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x01F8, - 0x030C, - 0x0606, - 0x0606, - 0x0606, - 0x0606, - 0x030C, - 0x01F8, - 0x00F0, - - 0x0000, - 0x0000, - 0x0F00, - 0x0F00, - 0x033F, - 0x033F, - 0x0333, - 0x0333, - 0x0333, - 0x0333, - 0x03F3, - 0x03F3, - 0x0303, - 0x0303, - 0x0303, - 0x0303, - 0x0303, - 0x0303, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x000F, - 0x000F, - 0x03FC, - 0x03FC, - 0x030F, - 0x030F, - 0x0300, - 0x0300, - 0x0F00, - 0x0F00, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0F00, - 0x0F00, - 0x0000, - 0x0000, - 0x0F00, - 0x0F00, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0060, - 0x0060, - 0x00F0, - 0x00F0, - 0x01F8, - 0x01F8, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0040, - 0x0040, - 0x0E4F, - 0x0040, - 0x0040, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x038C, - 0x038C, - 0x03CC, - 0x036C, - 0x0B6D, - 0x033C, - 0x031C, - 0x030C, - 0x030C, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x00F0, - 0x01F8, - 0x0188, - 0x0180, - 0x01F0, - 0x0EFB, - 0x0018, - 0x0118, - 0x01F8, - 0x00F0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x01F8, - 0x01F8, - 0x0180, - 0x0180, - 0x01E0, - 0x0DEF, - 0x0180, - 0x0180, - 0x01F8, - 0x01F8, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x030C, - 0x030C, - 0x030C, - 0x030C, - 0x036C, - 0x0D6B, - 0x01F8, - 0x0198, - 0x0198, - 0x0198, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0E0F, - 0x0318, - 0x01B0, - 0x00E0, - 0x0040, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0070, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x007F, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00E0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FE0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0300, - 0x0300, - 0x03FF, - 0x03FF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0300, - 0x0300, - 0x0F00, - 0x0F00, - 0x0000, - 0x0000, - 0x0066, - 0x0066, - 0x0066, - 0x0066, - 0x0066, - 0x007E, - 0x003C, - 0x0018, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0180, - 0x0380, - 0x0380, - 0x0180, - 0x0180, - 0x0180, - 0x03C6, - 0x03C6, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03C0, - 0x0660, - 0x0660, - 0x03E0, - 0x0060, - 0x0260, - 0x07E0, - 0x03C0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x01F8, - 0x01F8, - 0x00F0, - 0x00F0, - 0x0060, - 0x0060, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - 0x0004, - - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - 0x0200, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0004, - 0x0004, - 0x0007, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0007, - 0x0004, - 0x0004, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0E00, - 0x0200, - 0x0200, - - 0x0200, - 0x0200, - 0x0E00, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0070, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0010, - 0x0018, - 0x001C, - 0x001E, - 0x001C, - 0x0018, - 0x0010, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x00E0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0080, - 0x0180, - 0x0380, - 0x0780, - 0x0380, - 0x0180, - 0x0080, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0030, - 0x0030, - 0x0030, - 0x0030, - 0x003F, - 0x003F, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x003F, - 0x003F, - 0x0030, - 0x0030, - 0x0030, - 0x0030, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FC0, - 0x0FC0, - 0x00C0, - 0x00C0, - 0x00C0, - 0x00C0, - - 0x00C0, - 0x00C0, - 0x00C0, - 0x00C0, - 0x0FC0, - 0x0FC0, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0660, - 0x07E0, - 0x05A0, - 0x0420, - 0x0004, - 0x03C4, - 0x0664, - 0x0660, - 0x0660, - 0x03C4, - 0x0004, - 0x07C4, - 0x0660, - 0x0660, - 0x0660, - 0x07C0, - 0x0000, - - 0x0000, - 0x0040, - 0x0040, - 0x0040, - 0x07FC, - 0x0444, - 0x0550, - 0x0446, - 0x07F4, - 0x0604, - 0x0014, - 0x0054, - 0x0054, - 0x0154, - 0x0154, - 0x0554, - 0x0556, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0660, - 0x0660, - 0x0660, - 0x03C0, - 0x0186, - 0x0004, - 0x0004, - 0x03C4, - 0x0664, - 0x0664, - 0x07E4, - 0x0664, - 0x0004, - 0x0006, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0660, - 0x0660, - 0x0660, - 0x03C0, - 0x0186, - 0x0004, - 0x0004, - 0x07C4, - 0x0664, - 0x0784, - 0x0664, - 0x07C4, - 0x0004, - 0x0006, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03C0, - 0x0600, - 0x0600, - 0x0600, - 0x03C6, - 0x0004, - 0x0004, - 0x03C4, - 0x0664, - 0x0664, - 0x07E4, - 0x0664, - 0x0004, - 0x0006, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x03C0, - 0x0600, - 0x0600, - 0x0600, - 0x03C6, - 0x0004, - 0x0004, - 0x07C4, - 0x0664, - 0x0784, - 0x0664, - 0x07C4, - 0x0004, - 0x0006, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0660, - 0x07E0, - 0x05A0, - 0x0420, - 0x0004, - 0x03C4, - 0x0664, - 0x0660, - 0x07E0, - 0x0664, - 0x0004, - 0x0664, - 0x0660, - 0x0660, - 0x03C0, - 0x0180, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x070E, - 0x0606, - 0x0402, - 0x0402, - 0x0402, - 0x0402, - 0x0606, - 0x070E, - 0x03FC, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x03FC, - 0x070E, - 0x0606, - 0x04F2, - 0x04F2, - 0x04F2, - 0x04F2, - 0x0606, - 0x070E, - 0x03FC, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0003, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0002, - 0x0003, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0FF8, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0FC0, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0E00, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0FFF, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0C00, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0400, - 0x0C00, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0FFE, - 0x0402, - 0x050A, - 0x058A, - 0x05CA, - 0x05EA, - 0x05FA, - 0x05FA, - 0x05EA, - 0x05CA, - 0x058A, - 0x050A, - 0x0402, - 0x0FFE, - 0x0000, - 0x0000, - - 0x0000, - 0x0001, - 0x0001, - 0x0030, - 0x0033, - 0x0007, - 0x000C, - 0x0018, - 0x00D8, - 0x00D8, - 0x0018, - 0x000C, - 0x0007, - 0x0033, - 0x0030, - 0x0001, - 0x0001, - 0x0000, - - 0x0000, - 0x0800, - 0x0800, - 0x00C0, - 0x0CC0, - 0x0E00, - 0x0300, - 0x0180, - 0x01B0, - 0x01B0, - 0x0180, - 0x0300, - 0x0E00, - 0x0CC0, - 0x00C0, - 0x0800, - 0x0800, - 0x0000, - - 0x0000, - 0x0000, - 0x0003, - 0x000F, - 0x001C, - 0x0030, - 0x0030, - 0x0060, - 0x0060, - 0x0060, - 0x0060, - 0x0030, - 0x0030, - 0x001C, - 0x000F, - 0x0003, - 0x0000, - 0x0000, - - 0x0000, - 0x0000, - 0x0C00, - 0x0F00, - 0x0F80, - 0x0FC0, - 0x0FC0, - 0x0FE0, - 0x0FE0, - 0x0FE0, - 0x0FE0, - 0x0FC0, - 0x0FC0, - 0x0F80, - 0x0F00, - 0x0C00, - 0x0000, - 0x0000, - - 0x0000, - 0x0006, - 0x000E, - 0x001A, - 0x0032, - 0x07E2, - 0x04C2, - 0x04C2, - 0x04C2, - 0x04C2, - 0x04C2, - 0x07E2, - 0x0032, - 0x001A, - 0x000E, - 0x0006, - 0x0000, - 0x0000, - - 0x0000, - 0x0060, - 0x0030, - 0x0010, - 0x0190, - 0x0098, - 0x04C8, - 0x0648, - 0x0248, - 0x0648, - 0x04C8, - 0x0098, - 0x0190, - 0x0010, - 0x0030, - 0x0060, - 0x0000, - 0x0000, - - 0x0000, - 0x07FE, - 0x03FC, - 0x030C, - 0x030C, - 0x030C, - 0x0198, - 0x00F0, - 0x0060, - 0x0060, - 0x00F0, - 0x0198, - 0x030C, - 0x030C, - 0x030C, - 0x03FC, - 0x07FE, - 0x0000, - - 0x0000, - 0x0000, - 0x0100, - 0x0180, - 0x01C0, - 0x01E0, - 0x01F0, - 0x01F8, - 0x01FC, - 0x01FE, - 0x01F0, - 0x01F0, - 0x0138, - 0x0038, - 0x001C, - 0x001C, - 0x0008, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0322, - 0x04B6, - 0x04AA, - 0x04AA, - 0x07A2, - 0x04A2, - 0x04A2, - 0x04A2, - 0x04A2, - 0x0000, - - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0722, - 0x04B6, - 0x04AA, - 0x04AA, - 0x0722, - 0x0422, - 0x0422, - 0x0422, - 0x0422, - 0x0000, - - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - 0x0FFF, - + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x03C0, + 0x0660, + 0x0460, + 0x00C0, + 0x0180, + 0x0300, + 0x07E0, + 0x0000, + 0x0078, + 0x006C, + 0x0066, + 0x0066, + 0x0066, + 0x0066, + 0x006C, + 0x0078, + 0x0000, + + 0x0000, + 0x07C0, + 0x0060, + 0x0060, + 0x03C0, + 0x0060, + 0x0060, + 0x07C0, + 0x0000, + 0x0078, + 0x006C, + 0x0066, + 0x0066, + 0x0066, + 0x0066, + 0x006C, + 0x0078, + 0x0000, + + 0x0000, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0003, + 0x0007, + 0x001F, + 0x0038, + 0x0060, + 0x00C0, + 0x00C0, + 0x0180, + 0x0180, + 0x0181, + + 0x0000, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0C00, + 0x0E00, + 0x0F80, + 0x01C0, + 0x0060, + 0x0030, + 0x0030, + 0x0018, + 0x0018, + 0x0818, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x07FE, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0700, + 0x08C6, + 0x0826, + 0x0818, + 0x0C18, + 0x0C24, + 0x0642, + 0x0382, + 0x0381, + 0x03C1, + 0x00F1, + 0x00BE, + 0x0080, + 0x0080, + 0x01C0, + 0x03E0, + 0x07F0, + 0x0000, + + 0x0000, + 0x01F8, + 0x030C, + 0x0606, + 0x0402, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0000, + 0x07FE, + 0x0606, + 0x06FE, + 0x06C6, + 0x06F6, + 0x0606, + 0x07FE, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x030C, + 0x0606, + 0x0402, + 0x0402, + 0x0000, + 0x07FE, + 0x0606, + 0x06FE, + 0x06C6, + 0x06F6, + 0x0606, + 0x07FE, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0208, + 0x030C, + 0x0390, + 0x03E0, + 0x03E0, + 0x01F0, + 0x00F8, + 0x007C, + 0x0080, + 0x0180, + 0x0280, + 0x0480, + 0x0000, + 0x07F8, + 0x0000, + + 0x0181, + 0x0180, + 0x0180, + 0x00C0, + 0x00C0, + 0x0060, + 0x0038, + 0x001F, + 0x0007, + 0x0003, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0001, + 0x0000, + + 0x0818, + 0x0018, + 0x0018, + 0x0030, + 0x0030, + 0x0060, + 0x01C0, + 0x0F80, + 0x0E00, + 0x0C00, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x0000, + + 0x07FE, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x00F0, + 0x0108, + 0x0108, + 0x0008, + 0x0008, + 0x0008, + 0x0000, + 0x01F8, + 0x030C, + 0x037C, + 0x030C, + 0x037C, + 0x037C, + 0x01F8, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x0108, + 0x0108, + 0x0108, + 0x0000, + 0x01F8, + 0x030C, + 0x037C, + 0x030C, + 0x037C, + 0x037C, + 0x01F8, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x00FF, + 0x01FF, + 0x0387, + 0x0387, + 0x0387, + 0x03FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x01FF, + 0x00FF, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FF8, + 0x0FFC, + 0x0FF2, + 0x0C32, + 0x081E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x081E, + 0x0C3E, + 0x0FFC, + 0x0FF8, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FF8, + 0x0FFC, + 0x0FF2, + 0x0C32, + 0x081E, + 0x000E, + 0x03CE, + 0x07EE, + 0x07EE, + 0x07EE, + 0x07EE, + 0x0BDC, + 0x0C38, + 0x0000, + 0x0000, + 0x0000, + + 0x03F8, + 0x0030, + 0x0060, + 0x01C0, + 0x03F8, + 0x0000, + 0x03F8, + 0x0318, + 0x03F8, + 0x0000, + 0x03F8, + 0x0318, + 0x03F8, + 0x0000, + 0x03B8, + 0x0358, + 0x0318, + 0x0318, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0001, + 0x0003, + 0x0007, + 0x000F, + 0x001F, + 0x003F, + 0x003F, + 0x003F, + 0x003F, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0000, + + 0x0000, + 0x0000, + 0x0800, + 0x0C00, + 0x0E00, + 0x0F00, + 0x0F80, + 0x00C0, + 0x00C0, + 0x00C0, + 0x0FC0, + 0x00C0, + 0x00C0, + 0x00C0, + 0x00C0, + 0x00C0, + 0x00C0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x01F8, + 0x03FC, + 0x0000, + 0x07FE, + 0x07E2, + 0x07E2, + 0x0422, + 0x043E, + 0x043E, + 0x043E, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0108, + 0x039C, + 0x018C, + 0x0084, + 0x0084, + 0x0108, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0198, + 0x0198, + 0x0198, + 0x07FE, + 0x07FE, + 0x0198, + 0x0198, + 0x07FE, + 0x07FE, + 0x0198, + 0x0198, + 0x0198, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x01F8, + 0x03FC, + 0x036C, + 0x0360, + 0x03F0, + 0x01F8, + 0x006C, + 0x036C, + 0x03FC, + 0x01F8, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x031C, + 0x0038, + 0x0070, + 0x00E0, + 0x01C0, + 0x038C, + 0x030C, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00E0, + 0x0190, + 0x0110, + 0x0110, + 0x0120, + 0x00C0, + 0x00C0, + 0x0120, + 0x0214, + 0x0208, + 0x0214, + 0x01E2, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0020, + 0x0070, + 0x0030, + 0x0010, + 0x0010, + 0x0020, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x000C, + 0x0018, + 0x0030, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0030, + 0x0018, + 0x000C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0180, + 0x00C0, + 0x0060, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0060, + 0x00C0, + 0x0180, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0402, + 0x0606, + 0x030C, + 0x0198, + 0x00F0, + 0x07FE, + 0x00F0, + 0x0198, + 0x030C, + 0x0606, + 0x0402, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x01F8, + 0x01F8, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x00C0, + 0x0180, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x01F8, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0006, + 0x000E, + 0x001C, + 0x0038, + 0x0070, + 0x00E0, + 0x01C0, + 0x0380, + 0x0700, + 0x0600, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00E0, + 0x01E0, + 0x01E0, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x01F8, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x031C, + 0x0038, + 0x0070, + 0x00E0, + 0x01C0, + 0x03FC, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x03FC, + 0x000C, + 0x000C, + 0x00F8, + 0x00F8, + 0x000C, + 0x000C, + 0x03FC, + 0x03F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0038, + 0x0078, + 0x00D8, + 0x0198, + 0x0318, + 0x03FC, + 0x03FC, + 0x0018, + 0x0018, + 0x0018, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x0300, + 0x03F8, + 0x03FC, + 0x000C, + 0x000C, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03F8, + 0x0300, + 0x0300, + 0x03F8, + 0x03FC, + 0x030C, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x000C, + 0x001C, + 0x0038, + 0x0070, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x030C, + 0x01F8, + 0x01F8, + 0x030C, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x030C, + 0x03FC, + 0x01FC, + 0x000C, + 0x000C, + 0x03FC, + 0x03F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x00C0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0018, + 0x0030, + 0x0060, + 0x00C0, + 0x0180, + 0x0180, + 0x00C0, + 0x0060, + 0x0030, + 0x0018, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x01F8, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x01F8, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0300, + 0x0180, + 0x00C0, + 0x0060, + 0x0030, + 0x0030, + 0x0060, + 0x00C0, + 0x0180, + 0x0300, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x070E, + 0x0606, + 0x0006, + 0x000E, + 0x001C, + 0x0038, + 0x0070, + 0x0060, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x0108, + 0x0204, + 0x04D2, + 0x0532, + 0x0512, + 0x0532, + 0x04D4, + 0x0408, + 0x0200, + 0x0104, + 0x00F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x039C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03F0, + 0x03F8, + 0x031C, + 0x030C, + 0x030C, + 0x031C, + 0x03F8, + 0x03F8, + 0x031C, + 0x030C, + 0x030C, + 0x031C, + 0x03F8, + 0x03F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x039C, + 0x030C, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x030C, + 0x039C, + 0x01F8, + 0x00F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03F0, + 0x03F8, + 0x031C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x031C, + 0x03F8, + 0x03F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x03F0, + 0x03F0, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x03FC, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x03F0, + 0x03F0, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x01F0, + 0x03F8, + 0x039C, + 0x030C, + 0x030C, + 0x0300, + 0x0300, + 0x033C, + 0x033C, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x01FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x01F8, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x01F8, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00FC, + 0x00FC, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x0330, + 0x0330, + 0x0330, + 0x0330, + 0x0330, + 0x03F0, + 0x01E0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0300, + 0x030C, + 0x031C, + 0x0338, + 0x0370, + 0x03E0, + 0x03C0, + 0x0380, + 0x03C0, + 0x03E0, + 0x0370, + 0x0338, + 0x031C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x03FC, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x039C, + 0x03FC, + 0x03FC, + 0x036C, + 0x036C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x038C, + 0x038C, + 0x03CC, + 0x03EC, + 0x037C, + 0x033C, + 0x031C, + 0x031C, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x039C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x039C, + 0x01F8, + 0x00F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03F0, + 0x03F8, + 0x031C, + 0x030C, + 0x030C, + 0x031C, + 0x03F8, + 0x03F0, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x039C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x036C, + 0x037C, + 0x03B8, + 0x01FC, + 0x00EC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03F0, + 0x03F8, + 0x031C, + 0x030C, + 0x030C, + 0x031C, + 0x03F8, + 0x03E0, + 0x03E0, + 0x0370, + 0x0338, + 0x031C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00F8, + 0x01FC, + 0x038C, + 0x0300, + 0x0300, + 0x0380, + 0x01F0, + 0x00F8, + 0x001C, + 0x000C, + 0x000C, + 0x031C, + 0x03F8, + 0x01F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x039C, + 0x01F8, + 0x00F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x039C, + 0x0198, + 0x01F8, + 0x00F0, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x036C, + 0x036C, + 0x036C, + 0x036C, + 0x036C, + 0x036C, + 0x03FC, + 0x01F8, + 0x0198, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x0198, + 0x00F0, + 0x0060, + 0x0060, + 0x00F0, + 0x0198, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x039C, + 0x01F8, + 0x00F0, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x000C, + 0x000C, + 0x001C, + 0x0038, + 0x0070, + 0x00E0, + 0x01C0, + 0x0380, + 0x0300, + 0x0300, + 0x03FC, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0078, + 0x0078, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0078, + 0x0078, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0600, + 0x0700, + 0x0380, + 0x01C0, + 0x00E0, + 0x0070, + 0x0038, + 0x001C, + 0x000E, + 0x0006, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x01E0, + 0x01E0, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x01E0, + 0x01E0, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x0198, + 0x0108, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x07FE, + 0x07FE, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0300, + 0x0180, + 0x00C0, + 0x0060, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x039C, + 0x030C, + 0x030C, + 0x03FC, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x03FC, + 0x030C, + 0x030C, + 0x03F8, + 0x03F8, + 0x030C, + 0x030C, + 0x03FC, + 0x03F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F0, + 0x03F8, + 0x031C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x031C, + 0x03F8, + 0x03F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x0300, + 0x0300, + 0x03F0, + 0x03F0, + 0x0300, + 0x0300, + 0x03FC, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x0300, + 0x0300, + 0x03F8, + 0x03F8, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x038C, + 0x0300, + 0x033C, + 0x033C, + 0x030C, + 0x038C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x00F0, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x00F0, + 0x00F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x007C, + 0x007C, + 0x0018, + 0x0018, + 0x0018, + 0x0018, + 0x0018, + 0x0198, + 0x01F8, + 0x00F0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x031C, + 0x0338, + 0x0370, + 0x03E0, + 0x03E0, + 0x03F0, + 0x0338, + 0x031C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x03FC, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x039C, + 0x03FC, + 0x036C, + 0x036C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x038C, + 0x03CC, + 0x03EC, + 0x037C, + 0x033C, + 0x031C, + 0x030C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x03FC, + 0x030C, + 0x030C, + 0x03FC, + 0x03F8, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x033C, + 0x0338, + 0x03FC, + 0x01EC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x03FC, + 0x030C, + 0x030C, + 0x03F8, + 0x03E0, + 0x0370, + 0x0338, + 0x031C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x030C, + 0x0300, + 0x03F8, + 0x01FC, + 0x000C, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x039C, + 0x01F8, + 0x00F0, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x036C, + 0x036C, + 0x03FC, + 0x01F8, + 0x0198, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x039C, + 0x01F8, + 0x00F0, + 0x0060, + 0x0060, + 0x00F0, + 0x01F8, + 0x039C, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x039C, + 0x01F8, + 0x00F0, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x03FC, + 0x001C, + 0x0038, + 0x0070, + 0x00E0, + 0x01C0, + 0x0380, + 0x03FC, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0038, + 0x0038, + 0x0078, + 0x0060, + 0x0060, + 0x0060, + 0x00C0, + 0x00C0, + 0x0060, + 0x0060, + 0x0060, + 0x0078, + 0x0038, + 0x0038, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x01C0, + 0x01E0, + 0x01E0, + 0x0060, + 0x0060, + 0x0060, + 0x0030, + 0x0030, + 0x0060, + 0x0060, + 0x0060, + 0x01E0, + 0x01E0, + 0x01C0, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00C2, + 0x01E6, + 0x033C, + 0x0618, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0280, + 0x02C0, + 0x02E0, + 0x02F0, + 0x02F8, + 0x02F0, + 0x02E0, + 0x02C0, + 0x0280, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0700, + 0x0000, + + 0x0000, + 0x0660, + 0x06C0, + 0x0700, + 0x06C0, + 0x0660, + 0x0000, + 0x06BE, + 0x0688, + 0x0588, + 0x0588, + 0x0000, + 0x0066, + 0x0066, + 0x007E, + 0x0066, + 0x0066, + 0x0000, + + 0x0000, + 0x0660, + 0x06C0, + 0x0700, + 0x06C0, + 0x0660, + 0x0000, + 0x01DC, + 0x01AC, + 0x01AC, + 0x018C, + 0x0000, + 0x0066, + 0x0066, + 0x007E, + 0x0066, + 0x0066, + 0x0000, + + 0x0000, + 0x07F0, + 0x06B0, + 0x06B0, + 0x0630, + 0x0000, + 0x0078, + 0x0048, + 0x00FC, + 0x00CC, + 0x00CC, + 0x0000, + 0x0066, + 0x0066, + 0x007E, + 0x0066, + 0x0066, + 0x0000, + + 0x0000, + 0x0600, + 0x0600, + 0x0600, + 0x07E0, + 0x07E0, + 0x0000, + 0x00F8, + 0x018C, + 0x01FC, + 0x018C, + 0x0000, + 0x007E, + 0x007E, + 0x0018, + 0x0018, + 0x0018, + 0x0000, + + 0x0000, + 0x0600, + 0x0600, + 0x0600, + 0x07E0, + 0x07E0, + 0x0000, + 0x01F8, + 0x039C, + 0x039C, + 0x01F8, + 0x0000, + 0x00E6, + 0x00F6, + 0x00FE, + 0x00DE, + 0x00CE, + 0x0000, + + 0x0000, + 0x03E0, + 0x0630, + 0x07F0, + 0x0630, + 0x0000, + 0x0180, + 0x0180, + 0x0180, + 0x01F8, + 0x01F8, + 0x0000, + 0x007E, + 0x007E, + 0x0018, + 0x0018, + 0x0018, + 0x0000, + + 0x0000, + 0x0660, + 0x0660, + 0x03C0, + 0x0180, + 0x0000, + 0x01F8, + 0x0180, + 0x01F0, + 0x0180, + 0x01F8, + 0x0000, + 0x0030, + 0x0030, + 0x0030, + 0x003E, + 0x003E, + 0x0000, + + 0x0000, + 0x07E0, + 0x07E0, + 0x0180, + 0x0180, + 0x0180, + 0x0000, + 0x00D8, + 0x00F8, + 0x00D8, + 0x00D8, + 0x0000, + 0x003C, + 0x0022, + 0x0026, + 0x003C, + 0x0036, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0660, + 0x07E0, + 0x05A0, + 0x0420, + 0x0000, + 0x07E0, + 0x0000, + 0x07E0, + 0x0400, + 0x07E0, + 0x0020, + 0x07E0, + 0x0000, + + 0x0000, + 0x03E0, + 0x0630, + 0x07F0, + 0x0630, + 0x0000, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0000, + 0x007E, + 0x0046, + 0x0078, + 0x006C, + 0x0066, + 0x0000, + + 0x0000, + 0x0770, + 0x06B0, + 0x06B0, + 0x06B0, + 0x06B0, + 0x0000, + 0x00F8, + 0x018C, + 0x01FC, + 0x018C, + 0x0000, + 0x0066, + 0x0066, + 0x0066, + 0x003C, + 0x0018, + 0x0000, + + 0x0000, + 0x0200, + 0x020C, + 0x0212, + 0x0012, + 0x0712, + 0x0012, + 0x070C, + 0x0700, + 0x0700, + 0x0712, + 0x0712, + 0x0014, + 0x0218, + 0x0214, + 0x0212, + 0x0212, + 0x0200, + + 0x0000, + 0x0210, + 0x0210, + 0x0210, + 0x001E, + 0x0700, + 0x000C, + 0x0712, + 0x0712, + 0x0712, + 0x070C, + 0x0700, + 0x001E, + 0x0210, + 0x021E, + 0x0202, + 0x021E, + 0x0200, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0618, + 0x0738, + 0x07F8, + 0x06D8, + 0x06D8, + 0x0618, + 0x0618, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0630, + 0x0630, + 0x0630, + 0x0630, + 0x0360, + 0x03E0, + 0x01C0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01C0, + 0x03E0, + 0x0220, + 0x0630, + 0x07F0, + 0x0630, + 0x0630, + 0x0000, + + 0x0000, + 0x0000, + 0x0003, + 0x001F, + 0x000F, + 0x000F, + 0x000F, + 0x000F, + 0x000F, + 0x000F, + 0x000F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0800, + 0x0F00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0007, + 0x0003, + 0x0007, + 0x000F, + 0x001F, + 0x003F, + 0x003F, + 0x0007, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0E00, + 0x0F00, + 0x0F80, + 0x0E00, + 0x0E00, + 0x0C00, + 0x0C00, + 0x0800, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0001, + 0x0003, + 0x001F, + 0x007F, + 0x00FF, + 0x007F, + 0x001C, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0E60, + 0x0800, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0001, + 0x003F, + 0x00FF, + 0x007F, + 0x0078, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0700, + 0x0FF0, + 0x0FE0, + 0x0FE0, + 0x08C0, + 0x0040, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0300, + 0x0FC0, + 0x0FF8, + 0x0FF0, + 0x0FE0, + 0x0180, + 0x0100, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0010, + 0x003F, + 0x007F, + 0x001F, + 0x0001, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0880, + 0x0FE0, + 0x0FF0, + 0x0FF8, + 0x07F0, + 0x0700, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x000C, + 0x001F, + 0x003F, + 0x001F, + 0x0007, + 0x0001, + 0x0001, + 0x0003, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0C00, + 0x0FC0, + 0x0FE0, + 0x0FE0, + 0x0FF0, + 0x0FF0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x001F, + 0x000F, + 0x0007, + 0x0007, + 0x0003, + 0x0007, + 0x000F, + 0x0001, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0800, + 0x0C00, + 0x0E00, + 0x0F60, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0180, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x000F, + 0x003F, + 0x000F, + 0x0007, + 0x0003, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0F00, + 0x0FC0, + 0x0E00, + 0x0C00, + 0x0800, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0001, + 0x0003, + 0x0007, + 0x006F, + 0x003F, + 0x003F, + 0x003F, + 0x003F, + 0x0018, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0F80, + 0x0F00, + 0x0E00, + 0x0E00, + 0x0C00, + 0x0E00, + 0x0F00, + 0x0800, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0003, + 0x003F, + 0x007F, + 0x007F, + 0x00FF, + 0x00FF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0300, + 0x0F80, + 0x0FC0, + 0x0F80, + 0x0E00, + 0x0800, + 0x0800, + 0x0C00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0011, + 0x007F, + 0x00FF, + 0x01FF, + 0x00FE, + 0x000E, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0080, + 0x0FC0, + 0x0FE0, + 0x0F80, + 0x0800, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x000C, + 0x003F, + 0x01FF, + 0x00FF, + 0x007F, + 0x0018, + 0x0008, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x000E, + 0x00FF, + 0x007F, + 0x007F, + 0x0031, + 0x0020, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0800, + 0x0FC0, + 0x0FF0, + 0x0FE0, + 0x01E0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x003F, + 0x003F, + 0x003F, + 0x0067, + 0x0001, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0800, + 0x0C00, + 0x0F80, + 0x0FE0, + 0x0FF0, + 0x0FE0, + 0x0380, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0007, + 0x000F, + 0x001F, + 0x0007, + 0x0007, + 0x0003, + 0x0003, + 0x0001, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0E00, + 0x0C00, + 0x0E00, + 0x0F00, + 0x0F80, + 0x0FC0, + 0x0FC0, + 0x0E00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0180, + 0x0240, + 0x0240, + 0x0180, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x01F8, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x01F8, + 0x00F0, + 0x0060, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0100, + 0x0300, + 0x07C0, + 0x07F0, + 0x0330, + 0x0118, + 0x0018, + 0x0018, + 0x0118, + 0x0330, + 0x07F0, + 0x07C0, + 0x0300, + 0x0100, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x00F0, + 0x07FE, + 0x0606, + 0x0462, + 0x0462, + 0x0462, + 0x0462, + 0x0462, + 0x0462, + 0x0462, + 0x0402, + 0x0462, + 0x0462, + 0x0402, + 0x0606, + 0x07FE, + 0x0000, + + 0x0000, + 0x00F0, + 0x07FE, + 0x070E, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x07FE, + 0x07FE, + 0x0000, + + 0x0000, + 0x00F0, + 0x07FE, + 0x070E, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x0000, + + 0x0000, + 0x00F0, + 0x07FE, + 0x070E, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x0000, + + 0x0000, + 0x00F0, + 0x07FE, + 0x070E, + 0x0606, + 0x0606, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x0000, + + 0x0000, + 0x00F0, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x0000, + + 0x0000, + 0x0000, + 0x03FF, + 0x03FF, + 0x0300, + 0x0300, + 0x033C, + 0x033C, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + 0x0300, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x030C, + 0x0606, + 0x0606, + 0x0606, + 0x0606, + 0x030C, + 0x01F8, + 0x00F0, + + 0x0000, + 0x0000, + 0x0F00, + 0x0F00, + 0x033F, + 0x033F, + 0x0333, + 0x0333, + 0x0333, + 0x0333, + 0x03F3, + 0x03F3, + 0x0303, + 0x0303, + 0x0303, + 0x0303, + 0x0303, + 0x0303, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x000F, + 0x000F, + 0x03FC, + 0x03FC, + 0x030F, + 0x030F, + 0x0300, + 0x0300, + 0x0F00, + 0x0F00, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0F00, + 0x0F00, + 0x0000, + 0x0000, + 0x0F00, + 0x0F00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x0060, + 0x00F0, + 0x00F0, + 0x01F8, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0040, + 0x0040, + 0x0E4F, + 0x0040, + 0x0040, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x038C, + 0x038C, + 0x03CC, + 0x036C, + 0x0B6D, + 0x033C, + 0x031C, + 0x030C, + 0x030C, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x0188, + 0x0180, + 0x01F0, + 0x0EFB, + 0x0018, + 0x0118, + 0x01F8, + 0x00F0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x01F8, + 0x0180, + 0x0180, + 0x01E0, + 0x0DEF, + 0x0180, + 0x0180, + 0x01F8, + 0x01F8, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x030C, + 0x030C, + 0x030C, + 0x036C, + 0x0D6B, + 0x01F8, + 0x0198, + 0x0198, + 0x0198, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0E0F, + 0x0318, + 0x01B0, + 0x00E0, + 0x0040, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0070, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x007F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00E0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FE0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0300, + 0x0300, + 0x03FF, + 0x03FF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0300, + 0x0300, + 0x0F00, + 0x0F00, + 0x0000, + 0x0000, + 0x0066, + 0x0066, + 0x0066, + 0x0066, + 0x0066, + 0x007E, + 0x003C, + 0x0018, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0180, + 0x0380, + 0x0380, + 0x0180, + 0x0180, + 0x0180, + 0x03C6, + 0x03C6, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03C0, + 0x0660, + 0x0660, + 0x03E0, + 0x0060, + 0x0260, + 0x07E0, + 0x03C0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x01F8, + 0x01F8, + 0x00F0, + 0x00F0, + 0x0060, + 0x0060, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + 0x0004, + + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + 0x0200, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0004, + 0x0004, + 0x0007, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0007, + 0x0004, + 0x0004, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0E00, + 0x0200, + 0x0200, + + 0x0200, + 0x0200, + 0x0E00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0070, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0010, + 0x0018, + 0x001C, + 0x001E, + 0x001C, + 0x0018, + 0x0010, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00E0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0080, + 0x0180, + 0x0380, + 0x0780, + 0x0380, + 0x0180, + 0x0080, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0030, + 0x0030, + 0x0030, + 0x0030, + 0x003F, + 0x003F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x003F, + 0x003F, + 0x0030, + 0x0030, + 0x0030, + 0x0030, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FC0, + 0x0FC0, + 0x00C0, + 0x00C0, + 0x00C0, + 0x00C0, + + 0x00C0, + 0x00C0, + 0x00C0, + 0x00C0, + 0x0FC0, + 0x0FC0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0660, + 0x07E0, + 0x05A0, + 0x0420, + 0x0004, + 0x03C4, + 0x0664, + 0x0660, + 0x0660, + 0x03C4, + 0x0004, + 0x07C4, + 0x0660, + 0x0660, + 0x0660, + 0x07C0, + 0x0000, + + 0x0000, + 0x0040, + 0x0040, + 0x0040, + 0x07FC, + 0x0444, + 0x0550, + 0x0446, + 0x07F4, + 0x0604, + 0x0014, + 0x0054, + 0x0054, + 0x0154, + 0x0154, + 0x0554, + 0x0556, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0660, + 0x0660, + 0x0660, + 0x03C0, + 0x0186, + 0x0004, + 0x0004, + 0x03C4, + 0x0664, + 0x0664, + 0x07E4, + 0x0664, + 0x0004, + 0x0006, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0660, + 0x0660, + 0x0660, + 0x03C0, + 0x0186, + 0x0004, + 0x0004, + 0x07C4, + 0x0664, + 0x0784, + 0x0664, + 0x07C4, + 0x0004, + 0x0006, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03C0, + 0x0600, + 0x0600, + 0x0600, + 0x03C6, + 0x0004, + 0x0004, + 0x03C4, + 0x0664, + 0x0664, + 0x07E4, + 0x0664, + 0x0004, + 0x0006, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03C0, + 0x0600, + 0x0600, + 0x0600, + 0x03C6, + 0x0004, + 0x0004, + 0x07C4, + 0x0664, + 0x0784, + 0x0664, + 0x07C4, + 0x0004, + 0x0006, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0660, + 0x07E0, + 0x05A0, + 0x0420, + 0x0004, + 0x03C4, + 0x0664, + 0x0660, + 0x07E0, + 0x0664, + 0x0004, + 0x0664, + 0x0660, + 0x0660, + 0x03C0, + 0x0180, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x070E, + 0x0606, + 0x0402, + 0x0402, + 0x0402, + 0x0402, + 0x0606, + 0x070E, + 0x03FC, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x070E, + 0x0606, + 0x04F2, + 0x04F2, + 0x04F2, + 0x04F2, + 0x0606, + 0x070E, + 0x03FC, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0003, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0002, + 0x0003, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0C00, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0400, + 0x0C00, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FFE, + 0x0402, + 0x050A, + 0x058A, + 0x05CA, + 0x05EA, + 0x05FA, + 0x05FA, + 0x05EA, + 0x05CA, + 0x058A, + 0x050A, + 0x0402, + 0x0FFE, + 0x0000, + 0x0000, + + 0x0000, + 0x0001, + 0x0001, + 0x0030, + 0x0033, + 0x0007, + 0x000C, + 0x0018, + 0x00D8, + 0x00D8, + 0x0018, + 0x000C, + 0x0007, + 0x0033, + 0x0030, + 0x0001, + 0x0001, + 0x0000, + + 0x0000, + 0x0800, + 0x0800, + 0x00C0, + 0x0CC0, + 0x0E00, + 0x0300, + 0x0180, + 0x01B0, + 0x01B0, + 0x0180, + 0x0300, + 0x0E00, + 0x0CC0, + 0x00C0, + 0x0800, + 0x0800, + 0x0000, + + 0x0000, + 0x0000, + 0x0003, + 0x000F, + 0x001C, + 0x0030, + 0x0030, + 0x0060, + 0x0060, + 0x0060, + 0x0060, + 0x0030, + 0x0030, + 0x001C, + 0x000F, + 0x0003, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0C00, + 0x0F00, + 0x0F80, + 0x0FC0, + 0x0FC0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FC0, + 0x0FC0, + 0x0F80, + 0x0F00, + 0x0C00, + 0x0000, + 0x0000, + + 0x0000, + 0x0006, + 0x000E, + 0x001A, + 0x0032, + 0x07E2, + 0x04C2, + 0x04C2, + 0x04C2, + 0x04C2, + 0x04C2, + 0x07E2, + 0x0032, + 0x001A, + 0x000E, + 0x0006, + 0x0000, + 0x0000, + + 0x0000, + 0x0060, + 0x0030, + 0x0010, + 0x0190, + 0x0098, + 0x04C8, + 0x0648, + 0x0248, + 0x0648, + 0x04C8, + 0x0098, + 0x0190, + 0x0010, + 0x0030, + 0x0060, + 0x0000, + 0x0000, + + 0x0000, + 0x07FE, + 0x03FC, + 0x030C, + 0x030C, + 0x030C, + 0x0198, + 0x00F0, + 0x0060, + 0x0060, + 0x00F0, + 0x0198, + 0x030C, + 0x030C, + 0x030C, + 0x03FC, + 0x07FE, + 0x0000, + + 0x0000, + 0x0000, + 0x0100, + 0x0180, + 0x01C0, + 0x01E0, + 0x01F0, + 0x01F8, + 0x01FC, + 0x01FE, + 0x01F0, + 0x01F0, + 0x0138, + 0x0038, + 0x001C, + 0x001C, + 0x0008, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0322, + 0x04B6, + 0x04AA, + 0x04AA, + 0x07A2, + 0x04A2, + 0x04A2, + 0x04A2, + 0x04A2, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0722, + 0x04B6, + 0x04AA, + 0x04AA, + 0x0722, + 0x0422, + 0x0422, + 0x0422, + 0x0422, + 0x0000, + + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, }; static const uint16_t font_mask12x18[] = { -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FFF, -0x0FFF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, - -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FFF, -0x0FFF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, - -0x0001, -0x0003, -0x0003, -0x0003, -0x0003, -0x0003, -0x0003, -0x0003, -0x0007, -0x001F, -0x003F, -0x007F, -0x00F8, -0x01E0, -0x01E0, -0x03C0, -0x03C1, -0x03C3, - -0x0800, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0E00, -0x0F80, -0x0FC0, -0x0FE0, -0x01F0, -0x0078, -0x0078, -0x003C, -0x083C, -0x0C3C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0FC6, -0x0FEF, -0x0FFF, -0x0FFE, -0x0FFC, -0x0FFE, -0x0FFF, -0x07FF, -0x07FF, -0x07FF, -0x03FF, -0x01FF, -0x01FE, -0x01C0, -0x03E0, -0x07F0, -0x0FFC, -0x0FFE, - -0x01F8, -0x03FC, -0x07FE, -0x0F0F, -0x0E07, -0x0407, -0x0007, -0x0007, -0x0007, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x0F0F, -0x0E07, -0x0E07, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x0000, -0x0000, -0x0208, -0x071C, -0x079E, -0x07FC, -0x07F0, -0x07F0, -0x03F8, -0x01FC, -0x00FE, -0x01FC, -0x03C0, -0x07C0, -0x0FC0, -0x0FF8, -0x0FFC, -0x0FFC, - -0x03C3, -0x03C1, -0x03C0, -0x01E0, -0x01E0, -0x00F8, -0x007F, -0x003F, -0x001F, -0x0007, -0x0003, -0x0003, -0x0003, -0x0003, -0x0003, -0x0003, -0x0003, -0x0001, - -0x0C3C, -0x083C, -0x003C, -0x0078, -0x0078, -0x01F0, -0x0FE0, -0x0FC0, -0x0F80, -0x0E00, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0C00, -0x0800, - -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x00F0, -0x03FC, -0x03FC, -0x039C, -0x011C, -0x001C, -0x001C, -0x01FC, -0x03FC, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x0000, - -0x0000, -0x0000, -0x0000, -0x00F0, -0x03FC, -0x03FC, -0x039C, -0x039C, -0x03FC, -0x03FC, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x0000, - -0x0000, -0x01FF, -0x03FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x03FF, -0x01FF, -0x0000, -0x0000, - -0x0000, -0x0FFC, -0x0FFE, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFE, -0x0FFC, -0x0000, -0x0000, - -0x0000, -0x0FFC, -0x0FFE, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFE, -0x0FFC, -0x0000, -0x0000, - -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0001, -0x0003, -0x0007, -0x000F, -0x001F, -0x003F, -0x007F, -0x00FF, -0x01FF, -0x037F, -0x027F, -0x007F, -0x007F, -0x007F, -0x007F, -0x007F, -0x007F, -0x007F, - -0x0800, -0x0C00, -0x0E00, -0x0F00, -0x0F80, -0x0FC0, -0x0FE0, -0x0FF0, -0x0FF8, -0x0FEC, -0x0FE4, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, - -0x0000, -0x0000, -0x0060, -0x01F8, -0x03FC, -0x07FE, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0060, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0060, -0x0060, -0x00F0, -0x00F0, -0x0060, -0x0000, - -0x0000, -0x0108, -0x039C, -0x07FE, -0x03DE, -0x01CE, -0x01CE, -0x039C, -0x0108, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0198, -0x03FC, -0x03FC, -0x07FE, -0x0FFF, -0x0FFF, -0x07FE, -0x07FE, -0x0FFF, -0x0FFF, -0x07FE, -0x03FC, -0x03FC, -0x0198, -0x0000, -0x0000, - -0x0000, -0x0060, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x07F8, -0x03FC, -0x03FE, -0x07FE, -0x07FE, -0x03FC, -0x01F8, -0x00F0, -0x0060, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x07BE, -0x037C, -0x00F8, -0x01F0, -0x03EC, -0x07DE, -0x079E, -0x030C, -0x0000, - -0x0000, -0x0000, -0x0000, -0x00E0, -0x01F0, -0x03F8, -0x03B8, -0x03B8, -0x03F0, -0x01E0, -0x01E0, -0x03FC, -0x073E, -0x071C, -0x07FE, -0x03F7, -0x01E2, -0x0000, - -0x0000, -0x0020, -0x0070, -0x00F8, -0x0078, -0x0038, -0x0038, -0x0070, -0x0020, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x000C, -0x001E, -0x003C, -0x0078, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0078, -0x003C, -0x001E, -0x000C, - -0x0000, -0x0000, -0x0000, -0x0180, -0x03C0, -0x01E0, -0x00F0, -0x0078, -0x0078, -0x0078, -0x0078, -0x0078, -0x0078, -0x0078, -0x00F0, -0x01E0, -0x03C0, -0x0180, - -0x0000, -0x0000, -0x0402, -0x0E07, -0x0F0F, -0x079E, -0x03FC, -0x07FE, -0x0FFF, -0x07FE, -0x03FC, -0x079E, -0x0F0F, -0x0E07, -0x0402, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x01F8, -0x03FC, -0x03FC, -0x01F8, -0x00F0, -0x0060, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x01E0, -0x03C0, -0x0180, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x03FC, -0x01F8, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0006, -0x000F, -0x001F, -0x003E, -0x007C, -0x00F8, -0x01F0, -0x03E0, -0x07C0, -0x0F80, -0x0F00, -0x0600, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x01F0, -0x03F0, -0x03F0, -0x01F0, -0x00F0, -0x00F0, -0x01F8, -0x03FC, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x07BE, -0x037C, -0x00F8, -0x01F0, -0x03FC, -0x07FE, -0x07FE, -0x03FC, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03F8, -0x07FC, -0x07FE, -0x03FE, -0x00FE, -0x01FC, -0x01FC, -0x00FE, -0x03FE, -0x07FE, -0x07FC, -0x03F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0038, -0x007C, -0x00FC, -0x01FC, -0x03FC, -0x07FC, -0x07FE, -0x07FE, -0x03FC, -0x003C, -0x003C, -0x0018, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x07FC, -0x07FE, -0x03FE, -0x031E, -0x07FE, -0x07FE, -0x07FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FC, -0x07F8, -0x07F8, -0x07FC, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x03FE, -0x003E, -0x007C, -0x00F8, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x03FC, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x03FE, -0x01FE, -0x03FE, -0x07FE, -0x07FC, -0x03F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x0060, -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x0060, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x0060, -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x01E0, -0x00C0, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0018, -0x003C, -0x0078, -0x00F0, -0x01E0, -0x03C0, -0x03C0, -0x01E0, -0x00F0, -0x0078, -0x003C, -0x0018, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x03FC, -0x01F8, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x03FC, -0x01F8, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0300, -0x0780, -0x03C0, -0x01E0, -0x00F0, -0x0078, -0x0078, -0x00F0, -0x01E0, -0x03C0, -0x0780, -0x0300, - -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x0FFF, -0x0F0F, -0x060F, -0x001F, -0x003E, -0x007C, -0x00F8, -0x00F0, -0x0060, -0x0060, -0x00F0, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x07DE, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFE, -0x0EDC, -0x070C, -0x03FE, -0x01FC, -0x00F8, - -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x03F0, -0x07F8, -0x07FC, -0x07FE, -0x079E, -0x079E, -0x07FE, -0x07FC, -0x07FC, -0x07FE, -0x079E, -0x079E, -0x07FE, -0x07FC, -0x07F8, -0x03F0, - -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x079E, -0x078C, -0x0780, -0x0780, -0x0780, -0x0780, -0x078C, -0x079E, -0x07FE, -0x03FC, -0x01F8, -0x00F0, - -0x0000, -0x0000, -0x03F0, -0x07F8, -0x07FC, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FC, -0x07F8, -0x03F0, - -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x0780, -0x0780, -0x07F0, -0x07F8, -0x07F8, -0x07F0, -0x0780, -0x0780, -0x07FC, -0x07FE, -0x07FE, -0x03FC, - -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x0780, -0x0780, -0x07F0, -0x07F8, -0x07F8, -0x07F0, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0300, - -0x0000, -0x0000, -0x01F0, -0x03F8, -0x07FC, -0x07FE, -0x079E, -0x079E, -0x078C, -0x07BC, -0x07FE, -0x07FE, -0x07BE, -0x079E, -0x07FE, -0x07FE, -0x03FE, -0x01FC, - -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x01F8, -0x03FC, -0x03FC, -0x01F8, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x01F8, -0x03FC, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x00FC, -0x01FE, -0x01FE, -0x00FC, -0x0078, -0x0078, -0x0078, -0x0378, -0x07F8, -0x07F8, -0x07F8, -0x07F8, -0x07F8, -0x07F8, -0x03F0, -0x01E0, - -0x0000, -0x0000, -0x0300, -0x078C, -0x079E, -0x07BE, -0x07FC, -0x07F8, -0x07F0, -0x07E0, -0x07C0, -0x07E0, -0x07F0, -0x07F8, -0x07FC, -0x07BE, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0300, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x07FC, -0x07FE, -0x07FE, -0x03FC, - -0x0000, -0x0000, -0x030C, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x07DE, -0x07DE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07BE, -0x07BE, -0x079E, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x03FC, -0x01F8, -0x00F0, - -0x0000, -0x0000, -0x03F0, -0x07F8, -0x07FC, -0x07FE, -0x079E, -0x079E, -0x07FE, -0x07FC, -0x07F8, -0x07F0, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0300, - -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FC, -0x03FE, -0x01FE, -0x00EC, - -0x0000, -0x0000, -0x03F0, -0x07F8, -0x07FC, -0x07FE, -0x079E, -0x079E, -0x07FE, -0x07FC, -0x07F8, -0x07F0, -0x07F8, -0x07FC, -0x07BE, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x00F8, -0x01FC, -0x03FE, -0x07FE, -0x078C, -0x0780, -0x07F0, -0x03F8, -0x01FC, -0x00FE, -0x001E, -0x031E, -0x07FE, -0x07FC, -0x03F8, -0x01F0, - -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x03FC, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x03FC, -0x01F8, -0x00F0, - -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x03FC, -0x03FC, -0x01F8, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x03FC, -0x0198, - -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x03FC, -0x01F8, -0x00F0, -0x00F0, -0x01F8, -0x03FC, -0x079E, -0x079E, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x03FC, -0x01F8, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x03FE, -0x001E, -0x003E, -0x007C, -0x00F8, -0x01F0, -0x03E0, -0x07C0, -0x0780, -0x07FC, -0x07FE, -0x07FE, -0x03FC, - -0x0000, -0x0000, -0x0078, -0x00FC, -0x00FC, -0x00F8, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F8, -0x00FC, -0x00FC, -0x0078, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0600, -0x0F00, -0x0F80, -0x07C0, -0x03E0, -0x01F0, -0x00F8, -0x007C, -0x003E, -0x001F, -0x000F, -0x0006, -0x0000, -0x0000, - -0x0000, -0x0000, -0x01E0, -0x03F0, -0x03F0, -0x01F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x01F0, -0x03F0, -0x03F0, -0x01E0, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x01F8, -0x03FC, -0x039C, -0x030C, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x07FE, -0x0FFF, -0x0FFF, -0x07FE, -0x0000, - -0x0000, -0x0300, -0x0780, -0x03C0, -0x01E0, -0x00F0, -0x0060, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03F8, -0x07FC, -0x07FE, -0x07FE, -0x07FE, -0x07FC, -0x07FC, -0x07FE, -0x07FE, -0x07FE, -0x07FC, -0x03F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x078C, -0x0780, -0x0780, -0x078C, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03F0, -0x07F8, -0x07FC, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FC, -0x07F8, -0x03F0, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x07F0, -0x07F8, -0x07F8, -0x07F0, -0x07FC, -0x07FE, -0x07FE, -0x03FC, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x07F8, -0x07FC, -0x07FC, -0x07F8, -0x0780, -0x0780, -0x0780, -0x0300, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x00F0, -0x01F8, -0x01F8, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x01F8, -0x01F8, -0x00F0, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x007C, -0x00FE, -0x00FE, -0x007C, -0x003C, -0x003C, -0x003C, -0x01BC, -0x03FC, -0x03FC, -0x01F8, -0x00F0, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x07BE, -0x07FC, -0x07F8, -0x07F0, -0x07F0, -0x07F8, -0x07FC, -0x07BE, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0300, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x07FC, -0x07FE, -0x07FE, -0x03FC, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x07DE, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07BE, -0x079E, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03F8, -0x07FC, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x07FC, -0x07F8, -0x0780, -0x0780, -0x0780, -0x0300, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x07BE, -0x07FE, -0x07FC, -0x07FE, -0x03FE, -0x01EC, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03F8, -0x07FC, -0x07FE, -0x07FE, -0x07FE, -0x07FC, -0x07F8, -0x07F8, -0x07FC, -0x07BE, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x07FE, -0x07FE, -0x07FC, -0x07FC, -0x03FE, -0x03FE, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x03FC, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x03FC, -0x01F8, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x03FC, -0x03FC, -0x0198, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x07FE, -0x03FC, -0x01F8, -0x00F0, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x079E, -0x030C, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x07FE, -0x03FC, -0x01F8, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0060, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x07FE, -0x03FE, -0x007C, -0x00F8, -0x01F0, -0x03E0, -0x07FC, -0x07FE, -0x07FE, -0x03FC, - -0x0000, -0x0038, -0x007C, -0x007C, -0x00FC, -0x00F8, -0x00F0, -0x00F0, -0x01E0, -0x01E0, -0x00F0, -0x00F0, -0x00F8, -0x00FC, -0x007C, -0x007C, -0x0038, -0x0000, - -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x0060, -0x0000, -0x0000, - -0x0000, -0x01C0, -0x03E0, -0x03F0, -0x03F0, -0x01F0, -0x00F0, -0x00F0, -0x0078, -0x0078, -0x00F0, -0x00F0, -0x01F0, -0x03F0, -0x03F0, -0x03E0, -0x01C0, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x00C2, -0x01E7, -0x03FF, -0x07FE, -0x0F3C, -0x0618, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0780, -0x07C0, -0x07E0, -0x07F0, -0x07F8, -0x07FC, -0x07F8, -0x07F0, -0x07E0, -0x07C0, -0x0780, -0x0700, -0x0700, -0x0700, -0x0F80, -0x0F80, -0x0F80, - -0x07E0, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF0, -0x07FE, -0x0FFF, -0x0FFE, -0x0FFC, -0x0FFC, -0x07FE, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x007E, - -0x0660, -0x0FF0, -0x0FE0, -0x0FC0, -0x0FE0, -0x0FF0, -0x07FC, -0x03FE, -0x03FE, -0x03FE, -0x03FE, -0x01FE, -0x00E7, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x0066, - -0x07F0, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x07F8, -0x00FC, -0x00FC, -0x01FE, -0x01FE, -0x01FE, -0x00FE, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x0066, - -0x0600, -0x0F00, -0x0F00, -0x0FE0, -0x0FF0, -0x0FF0, -0x07F8, -0x01FC, -0x03FE, -0x03FE, -0x03FE, -0x01FF, -0x00FF, -0x00FF, -0x007E, -0x003C, -0x003C, -0x0018, - -0x0600, -0x0F00, -0x0F00, -0x0FE0, -0x0FF0, -0x0FF0, -0x07F8, -0x03FC, -0x07FE, -0x07FE, -0x03FC, -0x01FE, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x00FE, - -0x07F0, -0x0FF0, -0x0FF8, -0x0FF8, -0x0FF8, -0x07F0, -0x03C0, -0x03C0, -0x03F8, -0x03FC, -0x03FC, -0x01FE, -0x00FF, -0x00FF, -0x007E, -0x003C, -0x003C, -0x0018, - -0x0660, -0x0FF0, -0x0FF0, -0x07E0, -0x03C0, -0x01F8, -0x03FC, -0x03F8, -0x03F0, -0x03F8, -0x03FC, -0x01F8, -0x0078, -0x0078, -0x007E, -0x007F, -0x007F, -0x003E, - -0x07E0, -0x0FF0, -0x0FF0, -0x07E0, -0x03C0, -0x03C0, -0x01D8, -0x01FC, -0x01FC, -0x01FC, -0x01FC, -0x00FC, -0x007E, -0x007F, -0x007F, -0x007F, -0x007F, -0x0037, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0660, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF0, -0x07E0, -0x0FF0, -0x07E0, -0x0FF0, -0x0FE0, -0x0FF0, -0x07F0, -0x0FF0, -0x07E0, - -0x03E0, -0x07F0, -0x0FF8, -0x0FF8, -0x0FF8, -0x06F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00FE, -0x00FF, -0x00FF, -0x00FE, -0x00FE, -0x00FF, -0x0066, - -0x0770, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x01FC, -0x03FE, -0x03FE, -0x03FE, -0x03FE, -0x00FF, -0x00FF, -0x00FF, -0x007E, -0x003C, -0x0018, - -0x023F, -0x073F, -0x073F, -0x073F, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x073F, -0x073F, -0x073F, -0x073F, -0x073F, -0x073F, - -0x023F, -0x073F, -0x073F, -0x073F, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x0FBF, -0x073F, -0x073F, -0x073F, -0x073F, -0x073F, -0x073F, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0618, -0x0F3C, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0F3C, -0x0618, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0630, -0x0F78, -0x0F78, -0x0F78, -0x0F78, -0x07F0, -0x07F0, -0x03E0, -0x01C0, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x01C0, -0x03E0, -0x07F0, -0x07F0, -0x0FF8, -0x0FF8, -0x0FF8, -0x0F78, -0x0630, - -0x0001, -0x0007, -0x001F, -0x007F, -0x01FF, -0x03FF, -0x03FF, -0x01FF, -0x003F, -0x003F, -0x003F, -0x003F, -0x003F, -0x003F, -0x001F, -0x0000, -0x0000, -0x0000, - -0x0C00, -0x0F80, -0x0FE0, -0x0FF8, -0x0FFC, -0x0FFC, -0x0FF8, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0001, -0x001F, -0x00FF, -0x00FF, -0x00FF, -0x003F, -0x007F, -0x00FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x001F, -0x0003, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0F80, -0x0FC0, -0x0FE0, -0x0FF0, -0x0FF8, -0x0FFC, -0x0FFC, -0x0FFC, -0x0F0C, -0x0F00, -0x0E00, -0x0E00, -0x0C00, -0x0800, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x001F, -0x001F, -0x001F, -0x007F, -0x01FF, -0x03FF, -0x03FF, -0x03FF, -0x01FF, -0x00FF, -0x007F, -0x003E, -0x0018, -0x0000, -0x0000, -0x0000, - -0x0000, -0x00E0, -0x0FE0, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0E78, -0x0818, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0003, -0x0007, -0x0007, -0x00FF, -0x03FF, -0x03FF, -0x03FF, -0x01FF, -0x01FF, -0x01FF, -0x00FE, -0x00F0, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0FC0, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FF8, -0x0FF8, -0x0FF0, -0x0FF0, -0x0FF0, -0x01E0, -0x00E0, -0x0040, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x00FF, -0x01FF, -0x01FF, -0x01FF, -0x03FF, -0x03FF, -0x03FF, -0x03FF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0400, -0x0780, -0x0FE0, -0x0FFC, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFE, -0x0FFC, -0x0FF0, -0x07E0, -0x07C0, -0x0700, -0x0600, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x003C, -0x007F, -0x01FF, -0x03FF, -0x03FF, -0x03FF, -0x03FF, -0x007F, -0x0007, -0x0007, -0x000F, -0x000F, -0x000E, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0180, -0x0FE0, -0x0FF0, -0x0FF8, -0x0FFC, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFC, -0x0FE0, -0x0E00, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x001E, -0x003F, -0x00FF, -0x01FF, -0x01FF, -0x01FF, -0x007F, -0x003F, -0x001F, -0x007F, -0x00FF, -0x00FF, -0x007F, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0800, -0x0E60, -0x0FF0, -0x0FF0, -0x0FF8, -0x0FF8, -0x0FFC, -0x0FFC, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFC, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0003, -0x003F, -0x007F, -0x007F, -0x003F, -0x003F, -0x003F, -0x01FF, -0x03FF, -0x03FF, -0x01FF, -0x003F, -0x0007, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0C00, -0x0C00, -0x0E00, -0x0F18, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FE0, -0x0FE0, -0x0FE0, -0x01E0, -0x0000, -0x0000, - -0x0000, -0x000F, -0x000F, -0x001F, -0x001F, -0x001F, -0x03FF, -0x03FF, -0x03FF, -0x01FF, -0x00FF, -0x007F, -0x003F, -0x001F, -0x0007, -0x0003, -0x0001, -0x0000, - -0x0000, -0x0F00, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FF8, -0x0FF0, -0x0FE0, -0x0FC0, -0x0F80, -0x0F00, -0x0E00, -0x0C00, -0x0000, - -0x0000, -0x0003, -0x0003, -0x0007, -0x018F, -0x01FF, -0x01FF, -0x01FF, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x007F, -0x007F, -0x007F, -0x0078, -0x0000, -0x0000, - -0x0000, -0x0C00, -0x0FC0, -0x0FE0, -0x0FE0, -0x0FC0, -0x0FC0, -0x0FC0, -0x0FF8, -0x0FFC, -0x0FFC, -0x0FF8, -0x0FC0, -0x0E00, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0001, -0x0067, -0x00FF, -0x00FF, -0x01FF, -0x01FF, -0x03FF, -0x03FF, -0x07FF, -0x07FF, -0x07FF, -0x03FF, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0780, -0x0FC0, -0x0FF0, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FE0, -0x0FC0, -0x0F80, -0x0FE0, -0x0FF0, -0x0FF0, -0x0FE0, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0018, -0x007F, -0x00FF, -0x01FF, -0x03FF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x03FF, -0x007F, -0x0007, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x03C0, -0x0FE0, -0x0FF8, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FE0, -0x0E00, -0x0E00, -0x0F00, -0x0F00, -0x0700, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0002, -0x001E, -0x007F, -0x03FF, -0x0FFF, -0x0FFF, -0x0FFF, -0x07FF, -0x03FF, -0x00FF, -0x007E, -0x003E, -0x000E, -0x0006, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0FF0, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x003F, -0x03FF, -0x03FF, -0x03FF, -0x01FF, -0x01FF, -0x00FF, -0x00FF, -0x00FF, -0x0078, -0x0070, -0x0020, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0C00, -0x0E00, -0x0E00, -0x0FF0, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FF8, -0x0FF8, -0x0FF8, -0x07F0, -0x00F0, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0070, -0x007F, -0x00FF, -0x00FF, -0x00FF, -0x01FF, -0x01FF, -0x01FF, -0x01FF, -0x01E7, -0x0181, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0F80, -0x0F80, -0x0F80, -0x0FE0, -0x0FF8, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FF8, -0x0FF0, -0x0FE0, -0x07C0, -0x0180, -0x0000, -0x0000, -0x0000, - -0x0000, -0x001F, -0x003F, -0x007F, -0x00FF, -0x01FF, -0x03FF, -0x03FF, -0x03FF, -0x030F, -0x000F, -0x0007, -0x0007, -0x0003, -0x0001, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0800, -0x0F80, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FC0, -0x0FE0, -0x0FF0, -0x0FF8, -0x0FF8, -0x0FF8, -0x0FF8, -0x0F80, -0x0C00, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0180, -0x03C0, -0x07E0, -0x07E0, -0x03C0, -0x0180, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0060, -0x00F0, -0x01F8, -0x03FC, -0x01F8, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x00F0, -0x01F8, -0x03FC, -0x01F8, -0x00F0, -0x0060, -0x0000, - -0x0000, -0x0100, -0x0380, -0x07C0, -0x0FF0, -0x0FF8, -0x07F8, -0x03BC, -0x013C, -0x013C, -0x033C, -0x07F8, -0x0FF8, -0x0FF0, -0x07C0, -0x0380, -0x0100, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x01F8, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x01F8, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x01F8, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x01F8, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x01F8, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x01F8, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - -0x0000, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FE, -0x07FE, -0x07FE, -0x07FE, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, -0x0780, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x07FE, -0x0F0F, -0x0F0F, -0x0F0F, -0x0F0F, -0x07FE, -0x03FC, -0x01F8, - -0x0000, -0x0F80, -0x0F80, -0x0FFF, -0x0FFF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x0787, -0x0787, -0x0787, -0x0787, -0x0787, - -0x0000, -0x0000, -0x0000, -0x0800, -0x0800, -0x0800, -0x0800, -0x081F, -0x081F, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0F9F, -0x0F9F, -0x0F80, -0x0F80, -0x0F80, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0060, -0x00F0, -0x00F0, -0x01F8, -0x01F8, -0x03FC, -0x03FC, -0x01F8, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0040, -0x00E0, -0x0EEF, -0x0FFF, -0x0EEF, -0x00E0, -0x0040, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x07DE, -0x07FE, -0x07FE, -0x0FFF, -0x0FFF, -0x0FFF, -0x07FE, -0x07BE, -0x079E, -0x030C, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x00F0, -0x01F8, -0x03FC, -0x03FC, -0x03C0, -0x0FFF, -0x0FFF, -0x0FFF, -0x03FC, -0x03FC, -0x03FC, -0x00F8, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x01F8, -0x03FC, -0x03FC, -0x03F8, -0x03C0, -0x0FFF, -0x0FFF, -0x0FFF, -0x03F0, -0x03FC, -0x03FC, -0x01F8, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x030C, -0x079E, -0x079E, -0x079E, -0x07FE, -0x0FFF, -0x0FFF, -0x0FFF, -0x03FC, -0x03FC, -0x03FC, -0x01F8, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0000, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0E0F, -0x0F1F, -0x0FBF, -0x03F8, -0x01F0, -0x00E0, -0x0040, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0070, -0x00F8, -0x0070, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x007F, -0x00FF, -0x007F, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x00E0, -0x01F0, -0x00E0, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FE0, -0x0FF0, -0x0FE0, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0780, -0x07FF, -0x07FF, -0x07FF, -0x07FF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x00F0, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0787, -0x0F80, -0x0F80, -0x0F80, -0x0F80, -0x0066, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x00FF, -0x007E, -0x003C, -0x0018, -0x0000, -0x0000, -0x0000, - -0x0F80, -0x0000, -0x0000, -0x0000, -0x0000, -0x0180, -0x03C0, -0x07C0, -0x07C0, -0x03C0, -0x03C0, -0x03C6, -0x07EF, -0x07EF, -0x03C6, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x03C0, -0x07E0, -0x0FF0, -0x0FF0, -0x07E0, -0x03F0, -0x0FF0, -0x0FF0, -0x07E0, -0x03C0, -0x0000, -0x0000, -0x0000, - -0x01F8, -0x03FC, -0x03FC, -0x01F8, -0x01F8, -0x00F0, -0x00F0, -0x0060, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, -0x000E, - -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, -0x0700, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x000E, -0x000F, -0x000F, -0x000F, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x000F, -0x000F, -0x000F, -0x000E, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0F00, -0x0F00, -0x0F00, -0x0700, - -0x0700, -0x0F00, -0x0F00, -0x0F00, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0070, -0x00F8, -0x0070, -0x0000, -0x0000, -0x0000, -0x0010, -0x0038, -0x003C, -0x003E, -0x003F, -0x003E, -0x003C, -0x0038, -0x0010, -0x0000, - -0x0000, -0x0000, -0x00E0, -0x01F0, -0x00E0, -0x0000, -0x0000, -0x0000, -0x0080, -0x01C0, -0x03C0, -0x07C0, -0x0FC0, -0x07C0, -0x03C0, -0x01C0, -0x0080, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0078, -0x0078, -0x0078, -0x007F, -0x007F, -0x007F, -0x007F, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x007F, -0x007F, -0x007F, -0x007F, -0x0078, -0x0078, -0x0078, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x01E0, -0x01E0, -0x01E0, - -0x01E0, -0x01E0, -0x01E0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0660, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF4, -0x07EE, -0x07EE, -0x0FFE, -0x0FF4, -0x0FF4, -0x07EE, -0x07EE, -0x0FEE, -0x0FF4, -0x0FF0, -0x0FF0, -0x0FE0, -0x07C0, - -0x0040, -0x00E0, -0x00E0, -0x07FC, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFF, -0x0FFE, -0x0FFE, -0x07FE, -0x00FE, -0x01FE, -0x03FE, -0x07FE, -0x0FFE, -0x0FFF, -0x0556, - -0x0000, -0x0000, -0x0660, -0x0FF0, -0x0FF0, -0x0FF0, -0x07E6, -0x03CF, -0x018E, -0x03CE, -0x07EE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x066E, -0x000F, -0x0006, - -0x0000, -0x0000, -0x0660, -0x0FF0, -0x0FF0, -0x0FF0, -0x07E6, -0x03CF, -0x018E, -0x07CE, -0x0FEE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FEE, -0x07CE, -0x000F, -0x0006, - -0x0000, -0x0000, -0x03C0, -0x07E0, -0x0FC0, -0x0F00, -0x0FC6, -0x07EF, -0x03CE, -0x03CE, -0x07EE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FFE, -0x07EE, -0x000F, -0x0006, - -0x0000, -0x0000, -0x03C0, -0x07E0, -0x0FC0, -0x0F00, -0x0FC6, -0x07EF, -0x03CE, -0x07CE, -0x0FEE, -0x0FFE, -0x0FFE, -0x0FFE, -0x0FEE, -0x07CE, -0x000F, -0x0006, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, - -0x0660, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF4, -0x07FE, -0x07FE, -0x0FFE, -0x0FF4, -0x0FF4, -0x0FFE, -0x067E, -0x0FFE, -0x0FF4, -0x0FF0, -0x07E0, -0x03C0, -0x0180, - -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x07FE, -0x03FC, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0000, -0x0000, -0x03FC, -0x07FE, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x07FE, -0x03FC, -0x0000, -0x0000, -0x0000, - -0x0000, -0x0003, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0007, -0x0003, -0x0000, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFC, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FE0, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0F00, -0x0F00, -0x0F00, -0x0F00, -0x0F00, -0x0F00, -0x0F00, -0x0F00, -0x0F00, -0x0F00, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, - -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0FFF, -0x0FFF, -0x0FFF, -0x0000, - -0x0000, -0x0C00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0E00, -0x0C00, -0x0000, - -0x0000, -0x0FFE, -0x0FFF, -0x0FFF, -0x0F9F, -0x0FDF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FDF, -0x0F9F, -0x0FFF, -0x0FFF, -0x0FFE, -0x0000, - -0x0001, -0x0003, -0x0033, -0x007B, -0x007F, -0x003F, -0x001F, -0x00FC, -0x01FC, -0x01FC, -0x00FC, -0x001F, -0x003F, -0x007F, -0x007B, -0x0033, -0x0003, -0x0003, - -0x0800, -0x0C00, -0x0CC0, -0x0DE0, -0x0FE0, -0x0FC0, -0x0F80, -0x03F0, -0x03F8, -0x03F8, -0x03F0, -0x0F80, -0x0FC0, -0x0FE0, -0x0DE0, -0x0CC0, -0x0C00, -0x0800, - -0x0000, -0x0003, -0x000F, -0x001F, -0x003F, -0x007D, -0x0079, -0x00F1, -0x00F1, -0x00F1, -0x00F1, -0x0079, -0x007D, -0x003F, -0x001F, -0x000F, -0x0003, -0x0000, - -0x0000, -0x0C00, -0x0F00, -0x0F80, -0x0FC0, -0x0FE0, -0x0FE0, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FF0, -0x0FE0, -0x0FE0, -0x0FC0, -0x0F80, -0x0F00, -0x0C00, -0x0000, - -0x0006, -0x000F, -0x001F, -0x003F, -0x07FF, -0x0FF7, -0x0FE7, -0x0FE7, -0x0FE7, -0x0FE7, -0x0FE7, -0x0FF7, -0x07FF, -0x003F, -0x001F, -0x000F, -0x0006, -0x0000, - -0x0020, -0x0070, -0x0038, -0x01B8, -0x03F8, -0x05FC, -0x0FFC, -0x0FFC, -0x07FC, -0x0FFC, -0x0FFC, -0x05FC, -0x03F8, -0x01B8, -0x0038, -0x0070, -0x0020, -0x0000, - -0x07FE, -0x0FFF, -0x07FE, -0x07FE, -0x079E, -0x079E, -0x03FC, -0x01F8, -0x00F0, -0x00F0, -0x01F8, -0x03FC, -0x079E, -0x079E, -0x07FE, -0x07FE, -0x0FFF, -0x07FE, - -0x0000, -0x0300, -0x0380, -0x03C0, -0x03E0, -0x03F0, -0x03F8, -0x03FC, -0x03FE, -0x03FF, -0x03FF, -0x03FC, -0x03FC, -0x03FE, -0x003E, -0x003E, -0x003E, -0x0008, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0322, -0x07F7, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FF7, -0x0FF7, -0x0FF7, -0x0FF7, -0x04A2, - -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0000, -0x0722, -0x0FF7, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0F77, -0x0E77, -0x0E77, -0x0E77, -0x0422, - -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, -0x0FFF, - + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FFF, + 0x0FFF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FFF, + 0x0FFF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + + 0x0001, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0007, + 0x001F, + 0x003F, + 0x007F, + 0x00F8, + 0x01E0, + 0x01E0, + 0x03C0, + 0x03C1, + 0x03C3, + + 0x0800, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0E00, + 0x0F80, + 0x0FC0, + 0x0FE0, + 0x01F0, + 0x0078, + 0x0078, + 0x003C, + 0x083C, + 0x0C3C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0FC6, + 0x0FEF, + 0x0FFF, + 0x0FFE, + 0x0FFC, + 0x0FFE, + 0x0FFF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x03FF, + 0x01FF, + 0x01FE, + 0x01C0, + 0x03E0, + 0x07F0, + 0x0FFC, + 0x0FFE, + + 0x01F8, + 0x03FC, + 0x07FE, + 0x0F0F, + 0x0E07, + 0x0407, + 0x0007, + 0x0007, + 0x0007, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x0F0F, + 0x0E07, + 0x0E07, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x0000, + 0x0000, + 0x0208, + 0x071C, + 0x079E, + 0x07FC, + 0x07F0, + 0x07F0, + 0x03F8, + 0x01FC, + 0x00FE, + 0x01FC, + 0x03C0, + 0x07C0, + 0x0FC0, + 0x0FF8, + 0x0FFC, + 0x0FFC, + + 0x03C3, + 0x03C1, + 0x03C0, + 0x01E0, + 0x01E0, + 0x00F8, + 0x007F, + 0x003F, + 0x001F, + 0x0007, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0003, + 0x0001, + + 0x0C3C, + 0x083C, + 0x003C, + 0x0078, + 0x0078, + 0x01F0, + 0x0FE0, + 0x0FC0, + 0x0F80, + 0x0E00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0C00, + 0x0800, + + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x00F0, + 0x03FC, + 0x03FC, + 0x039C, + 0x011C, + 0x001C, + 0x001C, + 0x01FC, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x03FC, + 0x03FC, + 0x039C, + 0x039C, + 0x03FC, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x0000, + + 0x0000, + 0x01FF, + 0x03FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x03FF, + 0x01FF, + 0x0000, + 0x0000, + + 0x0000, + 0x0FFC, + 0x0FFE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFE, + 0x0FFC, + 0x0000, + 0x0000, + + 0x0000, + 0x0FFC, + 0x0FFE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFE, + 0x0FFC, + 0x0000, + 0x0000, + + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0001, + 0x0003, + 0x0007, + 0x000F, + 0x001F, + 0x003F, + 0x007F, + 0x00FF, + 0x01FF, + 0x037F, + 0x027F, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + + 0x0800, + 0x0C00, + 0x0E00, + 0x0F00, + 0x0F80, + 0x0FC0, + 0x0FE0, + 0x0FF0, + 0x0FF8, + 0x0FEC, + 0x0FE4, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + + 0x0000, + 0x0000, + 0x0060, + 0x01F8, + 0x03FC, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0060, + 0x0060, + 0x00F0, + 0x00F0, + 0x0060, + 0x0000, + + 0x0000, + 0x0108, + 0x039C, + 0x07FE, + 0x03DE, + 0x01CE, + 0x01CE, + 0x039C, + 0x0108, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0198, + 0x03FC, + 0x03FC, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x07FE, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x07FE, + 0x03FC, + 0x03FC, + 0x0198, + 0x0000, + 0x0000, + + 0x0000, + 0x0060, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07F8, + 0x03FC, + 0x03FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x07BE, + 0x037C, + 0x00F8, + 0x01F0, + 0x03EC, + 0x07DE, + 0x079E, + 0x030C, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00E0, + 0x01F0, + 0x03F8, + 0x03B8, + 0x03B8, + 0x03F0, + 0x01E0, + 0x01E0, + 0x03FC, + 0x073E, + 0x071C, + 0x07FE, + 0x03F7, + 0x01E2, + 0x0000, + + 0x0000, + 0x0020, + 0x0070, + 0x00F8, + 0x0078, + 0x0038, + 0x0038, + 0x0070, + 0x0020, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x000C, + 0x001E, + 0x003C, + 0x0078, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0078, + 0x003C, + 0x001E, + 0x000C, + + 0x0000, + 0x0000, + 0x0000, + 0x0180, + 0x03C0, + 0x01E0, + 0x00F0, + 0x0078, + 0x0078, + 0x0078, + 0x0078, + 0x0078, + 0x0078, + 0x0078, + 0x00F0, + 0x01E0, + 0x03C0, + 0x0180, + + 0x0000, + 0x0000, + 0x0402, + 0x0E07, + 0x0F0F, + 0x079E, + 0x03FC, + 0x07FE, + 0x0FFF, + 0x07FE, + 0x03FC, + 0x079E, + 0x0F0F, + 0x0E07, + 0x0402, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + 0x00F0, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x01E0, + 0x03C0, + 0x0180, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0006, + 0x000F, + 0x001F, + 0x003E, + 0x007C, + 0x00F8, + 0x01F0, + 0x03E0, + 0x07C0, + 0x0F80, + 0x0F00, + 0x0600, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x01F0, + 0x03F0, + 0x03F0, + 0x01F0, + 0x00F0, + 0x00F0, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07BE, + 0x037C, + 0x00F8, + 0x01F0, + 0x03FC, + 0x07FE, + 0x07FE, + 0x03FC, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x07FC, + 0x07FE, + 0x03FE, + 0x00FE, + 0x01FC, + 0x01FC, + 0x00FE, + 0x03FE, + 0x07FE, + 0x07FC, + 0x03F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0038, + 0x007C, + 0x00FC, + 0x01FC, + 0x03FC, + 0x07FC, + 0x07FE, + 0x07FE, + 0x03FC, + 0x003C, + 0x003C, + 0x0018, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07FC, + 0x07FE, + 0x03FE, + 0x031E, + 0x07FE, + 0x07FE, + 0x07FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FC, + 0x07F8, + 0x07F8, + 0x07FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x03FE, + 0x003E, + 0x007C, + 0x00F8, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FE, + 0x01FE, + 0x03FE, + 0x07FE, + 0x07FC, + 0x03F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x0060, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x0060, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x0060, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x01E0, + 0x00C0, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0018, + 0x003C, + 0x0078, + 0x00F0, + 0x01E0, + 0x03C0, + 0x03C0, + 0x01E0, + 0x00F0, + 0x0078, + 0x003C, + 0x0018, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0300, + 0x0780, + 0x03C0, + 0x01E0, + 0x00F0, + 0x0078, + 0x0078, + 0x00F0, + 0x01E0, + 0x03C0, + 0x0780, + 0x0300, + + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x0FFF, + 0x0F0F, + 0x060F, + 0x001F, + 0x003E, + 0x007C, + 0x00F8, + 0x00F0, + 0x0060, + 0x0060, + 0x00F0, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07DE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFE, + 0x0EDC, + 0x070C, + 0x03FE, + 0x01FC, + 0x00F8, + + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x03F0, + 0x07F8, + 0x07FC, + 0x07FE, + 0x079E, + 0x079E, + 0x07FE, + 0x07FC, + 0x07FC, + 0x07FE, + 0x079E, + 0x079E, + 0x07FE, + 0x07FC, + 0x07F8, + 0x03F0, + + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x079E, + 0x078C, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x078C, + 0x079E, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + + 0x0000, + 0x0000, + 0x03F0, + 0x07F8, + 0x07FC, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FC, + 0x07F8, + 0x03F0, + + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x0780, + 0x0780, + 0x07F0, + 0x07F8, + 0x07F8, + 0x07F0, + 0x0780, + 0x0780, + 0x07FC, + 0x07FE, + 0x07FE, + 0x03FC, + + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x0780, + 0x0780, + 0x07F0, + 0x07F8, + 0x07F8, + 0x07F0, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0300, + + 0x0000, + 0x0000, + 0x01F0, + 0x03F8, + 0x07FC, + 0x07FE, + 0x079E, + 0x079E, + 0x078C, + 0x07BC, + 0x07FE, + 0x07FE, + 0x07BE, + 0x079E, + 0x07FE, + 0x07FE, + 0x03FE, + 0x01FC, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x00FC, + 0x01FE, + 0x01FE, + 0x00FC, + 0x0078, + 0x0078, + 0x0078, + 0x0378, + 0x07F8, + 0x07F8, + 0x07F8, + 0x07F8, + 0x07F8, + 0x07F8, + 0x03F0, + 0x01E0, + + 0x0000, + 0x0000, + 0x0300, + 0x078C, + 0x079E, + 0x07BE, + 0x07FC, + 0x07F8, + 0x07F0, + 0x07E0, + 0x07C0, + 0x07E0, + 0x07F0, + 0x07F8, + 0x07FC, + 0x07BE, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0300, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x07FC, + 0x07FE, + 0x07FE, + 0x03FC, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x07DE, + 0x07DE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07BE, + 0x07BE, + 0x079E, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + + 0x0000, + 0x0000, + 0x03F0, + 0x07F8, + 0x07FC, + 0x07FE, + 0x079E, + 0x079E, + 0x07FE, + 0x07FC, + 0x07F8, + 0x07F0, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0300, + + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FC, + 0x03FE, + 0x01FE, + 0x00EC, + + 0x0000, + 0x0000, + 0x03F0, + 0x07F8, + 0x07FC, + 0x07FE, + 0x079E, + 0x079E, + 0x07FE, + 0x07FC, + 0x07F8, + 0x07F0, + 0x07F8, + 0x07FC, + 0x07BE, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x00F8, + 0x01FC, + 0x03FE, + 0x07FE, + 0x078C, + 0x0780, + 0x07F0, + 0x03F8, + 0x01FC, + 0x00FE, + 0x001E, + 0x031E, + 0x07FE, + 0x07FC, + 0x03F8, + 0x01F0, + + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x03FC, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x03FC, + 0x03FC, + 0x01F8, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x03FC, + 0x0198, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x03FC, + 0x01F8, + 0x00F0, + 0x00F0, + 0x01F8, + 0x03FC, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x03FE, + 0x001E, + 0x003E, + 0x007C, + 0x00F8, + 0x01F0, + 0x03E0, + 0x07C0, + 0x0780, + 0x07FC, + 0x07FE, + 0x07FE, + 0x03FC, + + 0x0000, + 0x0000, + 0x0078, + 0x00FC, + 0x00FC, + 0x00F8, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F8, + 0x00FC, + 0x00FC, + 0x0078, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0600, + 0x0F00, + 0x0F80, + 0x07C0, + 0x03E0, + 0x01F0, + 0x00F8, + 0x007C, + 0x003E, + 0x001F, + 0x000F, + 0x0006, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x01E0, + 0x03F0, + 0x03F0, + 0x01F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x01F0, + 0x03F0, + 0x03F0, + 0x01E0, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x01F8, + 0x03FC, + 0x039C, + 0x030C, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x07FE, + 0x0000, + + 0x0000, + 0x0300, + 0x0780, + 0x03C0, + 0x01E0, + 0x00F0, + 0x0060, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x07FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FC, + 0x03F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x078C, + 0x0780, + 0x0780, + 0x078C, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F0, + 0x07F8, + 0x07FC, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FC, + 0x07F8, + 0x03F0, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07F0, + 0x07F8, + 0x07F8, + 0x07F0, + 0x07FC, + 0x07FE, + 0x07FE, + 0x03FC, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07F8, + 0x07FC, + 0x07FC, + 0x07F8, + 0x0780, + 0x0780, + 0x0780, + 0x0300, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x01F8, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x01F8, + 0x01F8, + 0x00F0, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x007C, + 0x00FE, + 0x00FE, + 0x007C, + 0x003C, + 0x003C, + 0x003C, + 0x01BC, + 0x03FC, + 0x03FC, + 0x01F8, + 0x00F0, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x07BE, + 0x07FC, + 0x07F8, + 0x07F0, + 0x07F0, + 0x07F8, + 0x07FC, + 0x07BE, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0300, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x07FC, + 0x07FE, + 0x07FE, + 0x03FC, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x07DE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07BE, + 0x079E, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x07FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07F8, + 0x0780, + 0x0780, + 0x0780, + 0x0300, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x07BE, + 0x07FE, + 0x07FC, + 0x07FE, + 0x03FE, + 0x01EC, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03F8, + 0x07FC, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07F8, + 0x07F8, + 0x07FC, + 0x07BE, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x07FC, + 0x07FC, + 0x03FE, + 0x03FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x03FC, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x03FC, + 0x03FC, + 0x0198, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x079E, + 0x030C, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x03FC, + 0x01F8, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0060, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x07FE, + 0x03FE, + 0x007C, + 0x00F8, + 0x01F0, + 0x03E0, + 0x07FC, + 0x07FE, + 0x07FE, + 0x03FC, + + 0x0000, + 0x0038, + 0x007C, + 0x007C, + 0x00FC, + 0x00F8, + 0x00F0, + 0x00F0, + 0x01E0, + 0x01E0, + 0x00F0, + 0x00F0, + 0x00F8, + 0x00FC, + 0x007C, + 0x007C, + 0x0038, + 0x0000, + + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x0060, + 0x0000, + 0x0000, + + 0x0000, + 0x01C0, + 0x03E0, + 0x03F0, + 0x03F0, + 0x01F0, + 0x00F0, + 0x00F0, + 0x0078, + 0x0078, + 0x00F0, + 0x00F0, + 0x01F0, + 0x03F0, + 0x03F0, + 0x03E0, + 0x01C0, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x00C2, + 0x01E7, + 0x03FF, + 0x07FE, + 0x0F3C, + 0x0618, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0780, + 0x07C0, + 0x07E0, + 0x07F0, + 0x07F8, + 0x07FC, + 0x07F8, + 0x07F0, + 0x07E0, + 0x07C0, + 0x0780, + 0x0700, + 0x0700, + 0x0700, + 0x0F80, + 0x0F80, + 0x0F80, + + 0x07E0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x07FE, + 0x0FFF, + 0x0FFE, + 0x0FFC, + 0x0FFC, + 0x07FE, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x007E, + + 0x0660, + 0x0FF0, + 0x0FE0, + 0x0FC0, + 0x0FE0, + 0x0FF0, + 0x07FC, + 0x03FE, + 0x03FE, + 0x03FE, + 0x03FE, + 0x01FE, + 0x00E7, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x0066, + + 0x07F0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x07F8, + 0x00FC, + 0x00FC, + 0x01FE, + 0x01FE, + 0x01FE, + 0x00FE, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x0066, + + 0x0600, + 0x0F00, + 0x0F00, + 0x0FE0, + 0x0FF0, + 0x0FF0, + 0x07F8, + 0x01FC, + 0x03FE, + 0x03FE, + 0x03FE, + 0x01FF, + 0x00FF, + 0x00FF, + 0x007E, + 0x003C, + 0x003C, + 0x0018, + + 0x0600, + 0x0F00, + 0x0F00, + 0x0FE0, + 0x0FF0, + 0x0FF0, + 0x07F8, + 0x03FC, + 0x07FE, + 0x07FE, + 0x03FC, + 0x01FE, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x00FE, + + 0x07F0, + 0x0FF0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x07F0, + 0x03C0, + 0x03C0, + 0x03F8, + 0x03FC, + 0x03FC, + 0x01FE, + 0x00FF, + 0x00FF, + 0x007E, + 0x003C, + 0x003C, + 0x0018, + + 0x0660, + 0x0FF0, + 0x0FF0, + 0x07E0, + 0x03C0, + 0x01F8, + 0x03FC, + 0x03F8, + 0x03F0, + 0x03F8, + 0x03FC, + 0x01F8, + 0x0078, + 0x0078, + 0x007E, + 0x007F, + 0x007F, + 0x003E, + + 0x07E0, + 0x0FF0, + 0x0FF0, + 0x07E0, + 0x03C0, + 0x03C0, + 0x01D8, + 0x01FC, + 0x01FC, + 0x01FC, + 0x01FC, + 0x00FC, + 0x007E, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + 0x0037, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0660, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x07E0, + 0x0FF0, + 0x07E0, + 0x0FF0, + 0x0FE0, + 0x0FF0, + 0x07F0, + 0x0FF0, + 0x07E0, + + 0x03E0, + 0x07F0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x06F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00FE, + 0x00FF, + 0x00FF, + 0x00FE, + 0x00FE, + 0x00FF, + 0x0066, + + 0x0770, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x01FC, + 0x03FE, + 0x03FE, + 0x03FE, + 0x03FE, + 0x00FF, + 0x00FF, + 0x00FF, + 0x007E, + 0x003C, + 0x0018, + + 0x023F, + 0x073F, + 0x073F, + 0x073F, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x073F, + 0x073F, + 0x073F, + 0x073F, + 0x073F, + 0x073F, + + 0x023F, + 0x073F, + 0x073F, + 0x073F, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x0FBF, + 0x073F, + 0x073F, + 0x073F, + 0x073F, + 0x073F, + 0x073F, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0618, + 0x0F3C, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0F3C, + 0x0618, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0630, + 0x0F78, + 0x0F78, + 0x0F78, + 0x0F78, + 0x07F0, + 0x07F0, + 0x03E0, + 0x01C0, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x01C0, + 0x03E0, + 0x07F0, + 0x07F0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0F78, + 0x0630, + + 0x0001, + 0x0007, + 0x001F, + 0x007F, + 0x01FF, + 0x03FF, + 0x03FF, + 0x01FF, + 0x003F, + 0x003F, + 0x003F, + 0x003F, + 0x003F, + 0x003F, + 0x001F, + 0x0000, + 0x0000, + 0x0000, + + 0x0C00, + 0x0F80, + 0x0FE0, + 0x0FF8, + 0x0FFC, + 0x0FFC, + 0x0FF8, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0001, + 0x001F, + 0x00FF, + 0x00FF, + 0x00FF, + 0x003F, + 0x007F, + 0x00FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x001F, + 0x0003, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0F80, + 0x0FC0, + 0x0FE0, + 0x0FF0, + 0x0FF8, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0F0C, + 0x0F00, + 0x0E00, + 0x0E00, + 0x0C00, + 0x0800, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x001F, + 0x001F, + 0x001F, + 0x007F, + 0x01FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x01FF, + 0x00FF, + 0x007F, + 0x003E, + 0x0018, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x00E0, + 0x0FE0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0E78, + 0x0818, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0003, + 0x0007, + 0x0007, + 0x00FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x00FE, + 0x00F0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0FC0, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FF8, + 0x0FF8, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x01E0, + 0x00E0, + 0x0040, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0400, + 0x0780, + 0x0FE0, + 0x0FFC, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFE, + 0x0FFC, + 0x0FF0, + 0x07E0, + 0x07C0, + 0x0700, + 0x0600, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x003C, + 0x007F, + 0x01FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x007F, + 0x0007, + 0x0007, + 0x000F, + 0x000F, + 0x000E, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0180, + 0x0FE0, + 0x0FF0, + 0x0FF8, + 0x0FFC, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFC, + 0x0FE0, + 0x0E00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x001E, + 0x003F, + 0x00FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x007F, + 0x003F, + 0x001F, + 0x007F, + 0x00FF, + 0x00FF, + 0x007F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0800, + 0x0E60, + 0x0FF0, + 0x0FF0, + 0x0FF8, + 0x0FF8, + 0x0FFC, + 0x0FFC, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFC, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0003, + 0x003F, + 0x007F, + 0x007F, + 0x003F, + 0x003F, + 0x003F, + 0x01FF, + 0x03FF, + 0x03FF, + 0x01FF, + 0x003F, + 0x0007, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0C00, + 0x0C00, + 0x0E00, + 0x0F18, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x01E0, + 0x0000, + 0x0000, + + 0x0000, + 0x000F, + 0x000F, + 0x001F, + 0x001F, + 0x001F, + 0x03FF, + 0x03FF, + 0x03FF, + 0x01FF, + 0x00FF, + 0x007F, + 0x003F, + 0x001F, + 0x0007, + 0x0003, + 0x0001, + 0x0000, + + 0x0000, + 0x0F00, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FF8, + 0x0FF0, + 0x0FE0, + 0x0FC0, + 0x0F80, + 0x0F00, + 0x0E00, + 0x0C00, + 0x0000, + + 0x0000, + 0x0003, + 0x0003, + 0x0007, + 0x018F, + 0x01FF, + 0x01FF, + 0x01FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x007F, + 0x007F, + 0x007F, + 0x0078, + 0x0000, + 0x0000, + + 0x0000, + 0x0C00, + 0x0FC0, + 0x0FE0, + 0x0FE0, + 0x0FC0, + 0x0FC0, + 0x0FC0, + 0x0FF8, + 0x0FFC, + 0x0FFC, + 0x0FF8, + 0x0FC0, + 0x0E00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0001, + 0x0067, + 0x00FF, + 0x00FF, + 0x01FF, + 0x01FF, + 0x03FF, + 0x03FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x03FF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0780, + 0x0FC0, + 0x0FF0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FE0, + 0x0FC0, + 0x0F80, + 0x0FE0, + 0x0FF0, + 0x0FF0, + 0x0FE0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0018, + 0x007F, + 0x00FF, + 0x01FF, + 0x03FF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x03FF, + 0x007F, + 0x0007, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x03C0, + 0x0FE0, + 0x0FF8, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FE0, + 0x0E00, + 0x0E00, + 0x0F00, + 0x0F00, + 0x0700, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0002, + 0x001E, + 0x007F, + 0x03FF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x07FF, + 0x03FF, + 0x00FF, + 0x007E, + 0x003E, + 0x000E, + 0x0006, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0FF0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x003F, + 0x03FF, + 0x03FF, + 0x03FF, + 0x01FF, + 0x01FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x0078, + 0x0070, + 0x0020, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0C00, + 0x0E00, + 0x0E00, + 0x0FF0, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x07F0, + 0x00F0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0070, + 0x007F, + 0x00FF, + 0x00FF, + 0x00FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01FF, + 0x01E7, + 0x0181, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0FE0, + 0x0FF8, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FF8, + 0x0FF0, + 0x0FE0, + 0x07C0, + 0x0180, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x001F, + 0x003F, + 0x007F, + 0x00FF, + 0x01FF, + 0x03FF, + 0x03FF, + 0x03FF, + 0x030F, + 0x000F, + 0x0007, + 0x0007, + 0x0003, + 0x0001, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0800, + 0x0F80, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FC0, + 0x0FE0, + 0x0FF0, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0FF8, + 0x0F80, + 0x0C00, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0180, + 0x03C0, + 0x07E0, + 0x07E0, + 0x03C0, + 0x0180, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0060, + 0x00F0, + 0x01F8, + 0x03FC, + 0x01F8, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x00F0, + 0x01F8, + 0x03FC, + 0x01F8, + 0x00F0, + 0x0060, + 0x0000, + + 0x0000, + 0x0100, + 0x0380, + 0x07C0, + 0x0FF0, + 0x0FF8, + 0x07F8, + 0x03BC, + 0x013C, + 0x013C, + 0x033C, + 0x07F8, + 0x0FF8, + 0x0FF0, + 0x07C0, + 0x0380, + 0x0100, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x01F8, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x01F8, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x01F8, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x01F8, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x01F8, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x01F8, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + + 0x0000, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FE, + 0x07FE, + 0x07FE, + 0x07FE, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + 0x0780, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x07FE, + 0x0F0F, + 0x0F0F, + 0x0F0F, + 0x0F0F, + 0x07FE, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0F80, + 0x0F80, + 0x0FFF, + 0x0FFF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x0787, + 0x0787, + 0x0787, + 0x0787, + 0x0787, + + 0x0000, + 0x0000, + 0x0000, + 0x0800, + 0x0800, + 0x0800, + 0x0800, + 0x081F, + 0x081F, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0F9F, + 0x0F9F, + 0x0F80, + 0x0F80, + 0x0F80, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0060, + 0x00F0, + 0x00F0, + 0x01F8, + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0040, + 0x00E0, + 0x0EEF, + 0x0FFF, + 0x0EEF, + 0x00E0, + 0x0040, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x07DE, + 0x07FE, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x07FE, + 0x07BE, + 0x079E, + 0x030C, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x00F0, + 0x01F8, + 0x03FC, + 0x03FC, + 0x03C0, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x03FC, + 0x03FC, + 0x03FC, + 0x00F8, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x01F8, + 0x03FC, + 0x03FC, + 0x03F8, + 0x03C0, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x03F0, + 0x03FC, + 0x03FC, + 0x01F8, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x030C, + 0x079E, + 0x079E, + 0x079E, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x03FC, + 0x03FC, + 0x03FC, + 0x01F8, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0000, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0E0F, + 0x0F1F, + 0x0FBF, + 0x03F8, + 0x01F0, + 0x00E0, + 0x0040, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0070, + 0x00F8, + 0x0070, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x007F, + 0x00FF, + 0x007F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x00E0, + 0x01F0, + 0x00E0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FE0, + 0x0FF0, + 0x0FE0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0780, + 0x07FF, + 0x07FF, + 0x07FF, + 0x07FF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x00F0, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0787, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0F80, + 0x0066, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x00FF, + 0x007E, + 0x003C, + 0x0018, + 0x0000, + 0x0000, + 0x0000, + + 0x0F80, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0180, + 0x03C0, + 0x07C0, + 0x07C0, + 0x03C0, + 0x03C0, + 0x03C6, + 0x07EF, + 0x07EF, + 0x03C6, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x03C0, + 0x07E0, + 0x0FF0, + 0x0FF0, + 0x07E0, + 0x03F0, + 0x0FF0, + 0x0FF0, + 0x07E0, + 0x03C0, + 0x0000, + 0x0000, + 0x0000, + + 0x01F8, + 0x03FC, + 0x03FC, + 0x01F8, + 0x01F8, + 0x00F0, + 0x00F0, + 0x0060, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + 0x000E, + + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + 0x0700, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x000E, + 0x000F, + 0x000F, + 0x000F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x000F, + 0x000F, + 0x000F, + 0x000E, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0700, + + 0x0700, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0070, + 0x00F8, + 0x0070, + 0x0000, + 0x0000, + 0x0000, + 0x0010, + 0x0038, + 0x003C, + 0x003E, + 0x003F, + 0x003E, + 0x003C, + 0x0038, + 0x0010, + 0x0000, + + 0x0000, + 0x0000, + 0x00E0, + 0x01F0, + 0x00E0, + 0x0000, + 0x0000, + 0x0000, + 0x0080, + 0x01C0, + 0x03C0, + 0x07C0, + 0x0FC0, + 0x07C0, + 0x03C0, + 0x01C0, + 0x0080, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0078, + 0x0078, + 0x0078, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x007F, + 0x007F, + 0x007F, + 0x007F, + 0x0078, + 0x0078, + 0x0078, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x01E0, + 0x01E0, + 0x01E0, + + 0x01E0, + 0x01E0, + 0x01E0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0660, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF4, + 0x07EE, + 0x07EE, + 0x0FFE, + 0x0FF4, + 0x0FF4, + 0x07EE, + 0x07EE, + 0x0FEE, + 0x0FF4, + 0x0FF0, + 0x0FF0, + 0x0FE0, + 0x07C0, + + 0x0040, + 0x00E0, + 0x00E0, + 0x07FC, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFF, + 0x0FFE, + 0x0FFE, + 0x07FE, + 0x00FE, + 0x01FE, + 0x03FE, + 0x07FE, + 0x0FFE, + 0x0FFF, + 0x0556, + + 0x0000, + 0x0000, + 0x0660, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x07E6, + 0x03CF, + 0x018E, + 0x03CE, + 0x07EE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x066E, + 0x000F, + 0x0006, + + 0x0000, + 0x0000, + 0x0660, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x07E6, + 0x03CF, + 0x018E, + 0x07CE, + 0x0FEE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FEE, + 0x07CE, + 0x000F, + 0x0006, + + 0x0000, + 0x0000, + 0x03C0, + 0x07E0, + 0x0FC0, + 0x0F00, + 0x0FC6, + 0x07EF, + 0x03CE, + 0x03CE, + 0x07EE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x07EE, + 0x000F, + 0x0006, + + 0x0000, + 0x0000, + 0x03C0, + 0x07E0, + 0x0FC0, + 0x0F00, + 0x0FC6, + 0x07EF, + 0x03CE, + 0x07CE, + 0x0FEE, + 0x0FFE, + 0x0FFE, + 0x0FFE, + 0x0FEE, + 0x07CE, + 0x000F, + 0x0006, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + + 0x0660, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF4, + 0x07FE, + 0x07FE, + 0x0FFE, + 0x0FF4, + 0x0FF4, + 0x0FFE, + 0x067E, + 0x0FFE, + 0x0FF4, + 0x0FF0, + 0x07E0, + 0x03C0, + 0x0180, + + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x07FE, + 0x03FC, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0000, + 0x0000, + 0x03FC, + 0x07FE, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x07FE, + 0x03FC, + 0x0000, + 0x0000, + 0x0000, + + 0x0000, + 0x0003, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0007, + 0x0003, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFC, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FE0, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0F00, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0000, + + 0x0000, + 0x0C00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0E00, + 0x0C00, + 0x0000, + + 0x0000, + 0x0FFE, + 0x0FFF, + 0x0FFF, + 0x0F9F, + 0x0FDF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FDF, + 0x0F9F, + 0x0FFF, + 0x0FFF, + 0x0FFE, + 0x0000, + + 0x0001, + 0x0003, + 0x0033, + 0x007B, + 0x007F, + 0x003F, + 0x001F, + 0x00FC, + 0x01FC, + 0x01FC, + 0x00FC, + 0x001F, + 0x003F, + 0x007F, + 0x007B, + 0x0033, + 0x0003, + 0x0003, + + 0x0800, + 0x0C00, + 0x0CC0, + 0x0DE0, + 0x0FE0, + 0x0FC0, + 0x0F80, + 0x03F0, + 0x03F8, + 0x03F8, + 0x03F0, + 0x0F80, + 0x0FC0, + 0x0FE0, + 0x0DE0, + 0x0CC0, + 0x0C00, + 0x0800, + + 0x0000, + 0x0003, + 0x000F, + 0x001F, + 0x003F, + 0x007D, + 0x0079, + 0x00F1, + 0x00F1, + 0x00F1, + 0x00F1, + 0x0079, + 0x007D, + 0x003F, + 0x001F, + 0x000F, + 0x0003, + 0x0000, + + 0x0000, + 0x0C00, + 0x0F00, + 0x0F80, + 0x0FC0, + 0x0FE0, + 0x0FE0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FF0, + 0x0FE0, + 0x0FE0, + 0x0FC0, + 0x0F80, + 0x0F00, + 0x0C00, + 0x0000, + + 0x0006, + 0x000F, + 0x001F, + 0x003F, + 0x07FF, + 0x0FF7, + 0x0FE7, + 0x0FE7, + 0x0FE7, + 0x0FE7, + 0x0FE7, + 0x0FF7, + 0x07FF, + 0x003F, + 0x001F, + 0x000F, + 0x0006, + 0x0000, + + 0x0020, + 0x0070, + 0x0038, + 0x01B8, + 0x03F8, + 0x05FC, + 0x0FFC, + 0x0FFC, + 0x07FC, + 0x0FFC, + 0x0FFC, + 0x05FC, + 0x03F8, + 0x01B8, + 0x0038, + 0x0070, + 0x0020, + 0x0000, + + 0x07FE, + 0x0FFF, + 0x07FE, + 0x07FE, + 0x079E, + 0x079E, + 0x03FC, + 0x01F8, + 0x00F0, + 0x00F0, + 0x01F8, + 0x03FC, + 0x079E, + 0x079E, + 0x07FE, + 0x07FE, + 0x0FFF, + 0x07FE, + + 0x0000, + 0x0300, + 0x0380, + 0x03C0, + 0x03E0, + 0x03F0, + 0x03F8, + 0x03FC, + 0x03FE, + 0x03FF, + 0x03FF, + 0x03FC, + 0x03FC, + 0x03FE, + 0x003E, + 0x003E, + 0x003E, + 0x0008, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0322, + 0x07F7, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FF7, + 0x0FF7, + 0x0FF7, + 0x0FF7, + 0x04A2, + + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0000, + 0x0722, + 0x0FF7, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0F77, + 0x0E77, + 0x0E77, + 0x0E77, + 0x0422, + + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, + 0x0FFF, }; #endif /* FONT12X18_H_ */ diff --git a/flight/targets/boards/osd/firmware/inc/font8x10.h b/flight/targets/boards/osd/firmware/inc/font8x10.h index f199499de..d0fe73306 100644 --- a/flight/targets/boards/osd/firmware/inc/font8x10.h +++ b/flight/targets/boards/osd/firmware/inc/font8x10.h @@ -9,5640 +9,5638 @@ #define FONT8X10_H_ static const uint8_t font_frame8x10[] = { -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x60, -0x10, -0x20, -0x4C, -0x7A, -0x0A, -0x0C, -0x00, -0x00, - -0x00, -0x60, -0x20, -0x60, -0x2C, -0x6A, -0x0A, -0x0C, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x10, -0x10, -0x10, -0x10, -0x10, -0x00, -0x10, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x28, -0x28, -0x7C, -0x28, -0x7C, -0x28, -0x28, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x7C, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x30, -0x30, -0x00, -0x00, - -0x00, -0x00, -0x04, -0x08, -0x10, -0x20, -0x40, -0x00, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x4C, -0x54, -0x64, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x10, -0x30, -0x10, -0x10, -0x10, -0x10, -0x38, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x04, -0x08, -0x10, -0x20, -0x7C, -0x00, -0x00, - -0x00, -0x7C, -0x08, -0x10, -0x08, -0x04, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x08, -0x18, -0x28, -0x48, -0x7C, -0x08, -0x08, -0x00, -0x00, - -0x00, -0x7C, -0x40, -0x78, -0x04, -0x04, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x18, -0x20, -0x40, -0x78, -0x44, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x7C, -0x04, -0x08, -0x10, -0x20, -0x20, -0x20, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x38, -0x44, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x3C, -0x04, -0x08, -0x30, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x30, -0x30, -0x00, -0x30, -0x30, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x7C, -0x00, -0x7C, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x04, -0x34, -0x54, -0x54, -0x38, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x44, -0x7C, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x78, -0x44, -0x44, -0x78, -0x44, -0x44, -0x78, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x40, -0x40, -0x40, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x70, -0x48, -0x44, -0x44, -0x44, -0x48, -0x70, -0x00, -0x00, - -0x00, -0x7C, -0x40, -0x40, -0x78, -0x40, -0x40, -0x7C, -0x00, -0x00, - -0x00, -0x7C, -0x40, -0x40, -0x78, -0x40, -0x40, -0x40, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x40, -0x58, -0x44, -0x44, -0x3C, -0x00, -0x00, - -0x00, -0x44, -0x44, -0x44, -0x7C, -0x44, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x38, -0x10, -0x10, -0x10, -0x10, -0x10, -0x38, -0x00, -0x00, - -0x00, -0x1C, -0x08, -0x08, -0x08, -0x08, -0x48, -0x30, -0x00, -0x00, - -0x00, -0x44, -0x48, -0x50, -0x60, -0x50, -0x48, -0x44, -0x00, -0x00, - -0x00, -0x40, -0x40, -0x40, -0x40, -0x40, -0x40, -0x7C, -0x00, -0x00, - -0x00, -0x44, -0x6C, -0x54, -0x54, -0x44, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x44, -0x44, -0x64, -0x54, -0x4C, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x44, -0x44, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x78, -0x44, -0x44, -0x78, -0x40, -0x40, -0x40, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x44, -0x54, -0x48, -0x34, -0x00, -0x00, - -0x00, -0x78, -0x44, -0x44, -0x78, -0x50, -0x48, -0x44, -0x00, -0x00, - -0x00, -0x38, -0x44, -0x40, -0x38, -0x04, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x7C, -0x10, -0x10, -0x10, -0x10, -0x10, -0x10, -0x00, -0x00, - -0x00, -0x44, -0x44, -0x44, -0x44, -0x44, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x44, -0x44, -0x44, -0x44, -0x44, -0x28, -0x10, -0x00, -0x00, - -0x00, -0x44, -0x44, -0x44, -0x54, -0x54, -0x54, -0x28, -0x00, -0x00, - -0x00, -0x44, -0x44, -0x28, -0x10, -0x28, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x44, -0x44, -0x44, -0x28, -0x10, -0x10, -0x10, -0x00, -0x00, - -0x00, -0x7C, -0x04, -0x08, -0x10, -0x20, -0x40, -0x7C, -0x00, -0x00, - -0x00, -0x38, -0x20, -0x20, -0x20, -0x20, -0x20, -0x38, -0x00, -0x00, - -0x00, -0x00, -0x40, -0x20, -0x10, -0x08, -0x04, -0x00, -0x00, -0x00, - -0x00, -0x38, -0x08, -0x08, -0x08, -0x08, -0x08, -0x38, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x38, -0x04, -0x3C, -0x44, -0x3C, -0x00, -0x00, - -0x00, -0x40, -0x40, -0x40, -0x78, -0x44, -0x44, -0x78, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x3C, -0x40, -0x40, -0x40, -0x3C, -0x00, -0x00, - -0x00, -0x04, -0x04, -0x04, -0x3C, -0x44, -0x44, -0x3C, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x38, -0x44, -0x7C, -0x40, -0x3C, -0x00, -0x00, - -0x00, -0x08, -0x14, -0x10, -0x7C, -0x10, -0x10, -0x10, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x3C, -0x44, -0x3C, -0x04, -0x78, -0x00, -0x00, - -0x00, -0x40, -0x40, -0x40, -0x78, -0x44, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x00, -0x10, -0x00, -0x10, -0x10, -0x10, -0x10, -0x00, -0x00, - -0x00, -0x08, -0x00, -0x08, -0x08, -0x08, -0x48, -0x30, -0x00, -0x00, - -0x00, -0x20, -0x20, -0x24, -0x28, -0x30, -0x28, -0x24, -0x00, -0x00, - -0x00, -0x30, -0x10, -0x10, -0x10, -0x10, -0x10, -0x38, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x68, -0x54, -0x54, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x58, -0x64, -0x44, -0x44, -0x44, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x38, -0x44, -0x44, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x78, -0x44, -0x78, -0x40, -0x40, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x3C, -0x44, -0x3C, -0x04, -0x04, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x58, -0x64, -0x40, -0x40, -0x40, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x3C, -0x40, -0x38, -0x04, -0x78, -0x00, -0x00, - -0x00, -0x10, -0x10, -0x7C, -0x10, -0x10, -0x14, -0x08, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x44, -0x44, -0x44, -0x44, -0x38, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x44, -0x44, -0x44, -0x28, -0x10, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x44, -0x44, -0x54, -0x54, -0x28, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x44, -0x28, -0x10, -0x28, -0x44, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x44, -0x28, -0x10, -0x10, -0x20, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x7C, -0x08, -0x10, -0x20, -0x7C, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x10, -0x10, -0x10, -0x10, -0x10, -0x10, -0x10, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x38, -0x54, -0x54, -0x54, -0x44, -0x54, -0x44, -0x7C, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x44, -0x44, -0x44, -0x44, -0x7C, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x44, -0x44, -0x7C, -0x7C, -0x7C, -0x00, - -0x00, -0x38, -0x44, -0x44, -0x7C, -0x7C, -0x7C, -0x7C, -0x7C, -0x00, - -0x00, -0x38, -0x44, -0x7C, -0x7C, -0x7C, -0x7C, -0x7C, -0x7C, -0x00, - -0x00, -0x38, -0x7C, -0x7C, -0x7C, -0x7C, -0x7C, -0x7C, -0x7C, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0xFF, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x18, -0x18, -0xFF, -0x18, -0x18, -0x00, -0x00, - -0x00, -0x42, -0x42, -0x62, -0x52, -0xCB, -0x46, -0x42, -0x42, -0x00, - -0x00, -0x3C, -0x42, -0x40, -0x30, -0xCF, -0x02, -0x42, -0x3C, -0x00, - -0x00, -0x3E, -0x20, -0x20, -0x38, -0xA3, -0x20, -0x20, -0x3E, -0x00, - -0x00, -0x42, -0x42, -0x42, -0x42, -0xDB, -0x5A, -0x5A, -0x24, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x81, -0x42, -0x24, -0x18, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x18, -0x7E, -0x5E, -0x9F, -0x8F, -0x4E, -0x6E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0x9F, -0x8F, -0x46, -0x66, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0x8F, -0x42, -0x66, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xF1, -0x42, -0x66, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xF9, -0xF1, -0x62, -0x66, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7A, -0xF9, -0xF1, -0x72, -0x76, -0x18, -0x00, - -0x00, -0x18, -0x76, -0x72, -0xF1, -0xF9, -0x7A, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x66, -0x62, -0xF1, -0xF9, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x66, -0x42, -0xF1, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x66, -0x42, -0x8F, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x66, -0x46, -0x8F, -0x9F, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x6E, -0x4E, -0x8F, -0x9F, -0x5E, -0x7E, -0x18, -0x00, - -0x00, -0x07, -0x06, -0x04, -0x44, -0x3C, -0x3C, -0x24, -0x24, -0x00, - -0x00, -0xE0, -0x60, -0x20, -0x22, -0x3C, -0x3C, -0x24, -0x24, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x60, + 0x10, + 0x20, + 0x4C, + 0x7A, + 0x0A, + 0x0C, + 0x00, + 0x00, + + 0x00, + 0x60, + 0x20, + 0x60, + 0x2C, + 0x6A, + 0x0A, + 0x0C, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x00, + 0x10, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x28, + 0x28, + 0x7C, + 0x28, + 0x7C, + 0x28, + 0x28, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x7C, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x30, + 0x30, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x04, + 0x08, + 0x10, + 0x20, + 0x40, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x4C, + 0x54, + 0x64, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x10, + 0x30, + 0x10, + 0x10, + 0x10, + 0x10, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x04, + 0x08, + 0x10, + 0x20, + 0x7C, + 0x00, + 0x00, + + 0x00, + 0x7C, + 0x08, + 0x10, + 0x08, + 0x04, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x08, + 0x18, + 0x28, + 0x48, + 0x7C, + 0x08, + 0x08, + 0x00, + 0x00, + + 0x00, + 0x7C, + 0x40, + 0x78, + 0x04, + 0x04, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x18, + 0x20, + 0x40, + 0x78, + 0x44, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x7C, + 0x04, + 0x08, + 0x10, + 0x20, + 0x20, + 0x20, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x38, + 0x44, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x3C, + 0x04, + 0x08, + 0x30, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x30, + 0x30, + 0x00, + 0x30, + 0x30, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x7C, + 0x00, + 0x7C, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x04, + 0x34, + 0x54, + 0x54, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x44, + 0x7C, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x78, + 0x44, + 0x44, + 0x78, + 0x44, + 0x44, + 0x78, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x40, + 0x40, + 0x40, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x70, + 0x48, + 0x44, + 0x44, + 0x44, + 0x48, + 0x70, + 0x00, + 0x00, + + 0x00, + 0x7C, + 0x40, + 0x40, + 0x78, + 0x40, + 0x40, + 0x7C, + 0x00, + 0x00, + + 0x00, + 0x7C, + 0x40, + 0x40, + 0x78, + 0x40, + 0x40, + 0x40, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x40, + 0x58, + 0x44, + 0x44, + 0x3C, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x44, + 0x44, + 0x7C, + 0x44, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x1C, + 0x08, + 0x08, + 0x08, + 0x08, + 0x48, + 0x30, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x48, + 0x50, + 0x60, + 0x50, + 0x48, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x40, + 0x40, + 0x40, + 0x40, + 0x40, + 0x40, + 0x7C, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x6C, + 0x54, + 0x54, + 0x44, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x44, + 0x64, + 0x54, + 0x4C, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x44, + 0x44, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x78, + 0x44, + 0x44, + 0x78, + 0x40, + 0x40, + 0x40, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x44, + 0x54, + 0x48, + 0x34, + 0x00, + 0x00, + + 0x00, + 0x78, + 0x44, + 0x44, + 0x78, + 0x50, + 0x48, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x40, + 0x38, + 0x04, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x7C, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x44, + 0x44, + 0x44, + 0x44, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x44, + 0x44, + 0x44, + 0x44, + 0x28, + 0x10, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x44, + 0x44, + 0x54, + 0x54, + 0x54, + 0x28, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x44, + 0x28, + 0x10, + 0x28, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x44, + 0x44, + 0x44, + 0x28, + 0x10, + 0x10, + 0x10, + 0x00, + 0x00, + + 0x00, + 0x7C, + 0x04, + 0x08, + 0x10, + 0x20, + 0x40, + 0x7C, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x20, + 0x20, + 0x20, + 0x20, + 0x20, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x40, + 0x20, + 0x10, + 0x08, + 0x04, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x08, + 0x08, + 0x08, + 0x08, + 0x08, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x38, + 0x04, + 0x3C, + 0x44, + 0x3C, + 0x00, + 0x00, + + 0x00, + 0x40, + 0x40, + 0x40, + 0x78, + 0x44, + 0x44, + 0x78, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x3C, + 0x40, + 0x40, + 0x40, + 0x3C, + 0x00, + 0x00, + + 0x00, + 0x04, + 0x04, + 0x04, + 0x3C, + 0x44, + 0x44, + 0x3C, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x38, + 0x44, + 0x7C, + 0x40, + 0x3C, + 0x00, + 0x00, + + 0x00, + 0x08, + 0x14, + 0x10, + 0x7C, + 0x10, + 0x10, + 0x10, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x3C, + 0x44, + 0x3C, + 0x04, + 0x78, + 0x00, + 0x00, + + 0x00, + 0x40, + 0x40, + 0x40, + 0x78, + 0x44, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x10, + 0x00, + 0x10, + 0x10, + 0x10, + 0x10, + 0x00, + 0x00, + + 0x00, + 0x08, + 0x00, + 0x08, + 0x08, + 0x08, + 0x48, + 0x30, + 0x00, + 0x00, + + 0x00, + 0x20, + 0x20, + 0x24, + 0x28, + 0x30, + 0x28, + 0x24, + 0x00, + 0x00, + + 0x00, + 0x30, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x68, + 0x54, + 0x54, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x58, + 0x64, + 0x44, + 0x44, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x38, + 0x44, + 0x44, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x78, + 0x44, + 0x78, + 0x40, + 0x40, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x3C, + 0x44, + 0x3C, + 0x04, + 0x04, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x58, + 0x64, + 0x40, + 0x40, + 0x40, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x3C, + 0x40, + 0x38, + 0x04, + 0x78, + 0x00, + 0x00, + + 0x00, + 0x10, + 0x10, + 0x7C, + 0x10, + 0x10, + 0x14, + 0x08, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x44, + 0x44, + 0x44, + 0x44, + 0x38, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x44, + 0x44, + 0x44, + 0x28, + 0x10, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x44, + 0x44, + 0x54, + 0x54, + 0x28, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x44, + 0x28, + 0x10, + 0x28, + 0x44, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x44, + 0x28, + 0x10, + 0x10, + 0x20, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x7C, + 0x08, + 0x10, + 0x20, + 0x7C, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x10, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x38, + 0x54, + 0x54, + 0x54, + 0x44, + 0x54, + 0x44, + 0x7C, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x44, + 0x44, + 0x44, + 0x44, + 0x7C, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x44, + 0x44, + 0x7C, + 0x7C, + 0x7C, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x44, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x00, + + 0x00, + 0x38, + 0x44, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x00, + + 0x00, + 0x38, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x7C, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0xFF, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x18, + 0x18, + 0xFF, + 0x18, + 0x18, + 0x00, + 0x00, + + 0x00, + 0x42, + 0x42, + 0x62, + 0x52, + 0xCB, + 0x46, + 0x42, + 0x42, + 0x00, + + 0x00, + 0x3C, + 0x42, + 0x40, + 0x30, + 0xCF, + 0x02, + 0x42, + 0x3C, + 0x00, + + 0x00, + 0x3E, + 0x20, + 0x20, + 0x38, + 0xA3, + 0x20, + 0x20, + 0x3E, + 0x00, + + 0x00, + 0x42, + 0x42, + 0x42, + 0x42, + 0xDB, + 0x5A, + 0x5A, + 0x24, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x81, + 0x42, + 0x24, + 0x18, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x5E, + 0x9F, + 0x8F, + 0x4E, + 0x6E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0x9F, + 0x8F, + 0x46, + 0x66, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0x8F, + 0x42, + 0x66, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xF1, + 0x42, + 0x66, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xF9, + 0xF1, + 0x62, + 0x66, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7A, + 0xF9, + 0xF1, + 0x72, + 0x76, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x76, + 0x72, + 0xF1, + 0xF9, + 0x7A, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x66, + 0x62, + 0xF1, + 0xF9, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x66, + 0x42, + 0xF1, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x66, + 0x42, + 0x8F, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x66, + 0x46, + 0x8F, + 0x9F, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x6E, + 0x4E, + 0x8F, + 0x9F, + 0x5E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x07, + 0x06, + 0x04, + 0x44, + 0x3C, + 0x3C, + 0x24, + 0x24, + 0x00, + + 0x00, + 0xE0, + 0x60, + 0x20, + 0x22, + 0x3C, + 0x3C, + 0x24, + 0x24, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, }; static const uint8_t font_mask8x10[] = { -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0xF0, -0xF8, -0xFC, -0xFE, -0xFF, -0xFF, -0x7F, -0x3F, -0x1F, -0x00, - -0xF0, -0xF8, -0xFC, -0xFE, -0xFF, -0xFF, -0xFF, -0x3F, -0x1F, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x10, -0x38, -0x38, -0x38, -0x38, -0x38, -0x10, -0x38, -0x10, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x28, -0x7C, -0x7C, -0xFE, -0x7C, -0xFE, -0x7C, -0x7C, -0x28, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x7C, -0xFE, -0x7C, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x30, -0x78, -0x78, -0x30, -0x00, - -0x00, -0x04, -0x0E, -0x1C, -0x38, -0x70, -0xE0, -0x40, -0x00, -0x00, - -0x38, -0x7C, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, -0x38, -0x00, - -0x10, -0x38, -0x78, -0x38, -0x38, -0x38, -0x38, -0x7C, -0x38, -0x00, - -0x38, -0x7C, -0xFE, -0x4E, -0x1C, -0x38, -0x7C, -0xFE, -0x7C, -0x00, - -0x7C, -0xFE, -0x7C, -0x38, -0x1C, -0x4E, -0xFE, -0x7C, -0x38, -0x00, - -0x08, -0x1C, -0x3C, -0x7C, -0xFC, -0xFE, -0x7C, -0x1C, -0x08, -0x00, - -0x7C, -0xFE, -0xFC, -0xFC, -0x7E, -0x4E, -0xFE, -0x7C, -0x38, -0x00, - -0x18, -0x3C, -0x78, -0xF8, -0xFC, -0xFE, -0xFE, -0x7C, -0x38, -0x00, - -0x7C, -0xFE, -0x7E, -0x1C, -0x38, -0x70, -0x70, -0x70, -0x20, -0x00, - -0x38, -0x7C, -0xFE, -0xFE, -0x7C, -0xFE, -0xFE, -0x7C, -0x38, -0x00, - -0x38, -0x7C, -0xFE, -0xFE, -0x7E, -0x3E, -0x3C, -0x78, -0x30, -0x00, - -0x00, -0x00, -0x30, -0x78, -0x78, -0x30, -0x78, -0x78, -0x30, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x7C, -0xFE, -0x7C, -0xFE, -0x7C, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x38, -0x7C, -0xFE, -0x7E, -0x7E, -0xFE, -0xFE, -0x7C, -0x38, -0x00, - -0x38, -0x7C, -0xFE, -0xEE, -0xFE, -0xFE, -0xFE, -0xEE, -0x44, -0x00, - -0x78, -0xFC, -0xFE, -0xFE, -0xFC, -0xFE, -0xFE, -0xFC, -0x78, -0x00, - -0x38, -0x7C, -0xFE, -0xE4, -0xE0, -0xE4, -0xFE, -0x7C, -0x38, -0x00, - -0x70, -0xF8, -0xFC, -0xEE, -0xEE, -0xEE, -0xFC, -0xF8, -0x70, -0x00, - -0x7C, -0xFE, -0xFC, -0xF8, -0xFC, -0xF8, -0xFC, -0xFE, -0x7C, -0x00, - -0x7C, -0xFE, -0xFC, -0xF8, -0xFC, -0xF8, -0xE0, -0xE0, -0x40, -0x00, - -0x38, -0x7C, -0xFE, -0xFC, -0xFC, -0xFE, -0xFE, -0x7E, -0x3C, -0x00, - -0x44, -0xEE, -0xEE, -0xFE, -0xFE, -0xFE, -0xEE, -0xEE, -0x44, -0x00, - -0x38, -0x7C, -0x38, -0x38, -0x38, -0x38, -0x38, -0x7C, -0x38, -0x00, - -0x1C, -0x3E, -0x1C, -0x1C, -0x1C, -0x5C, -0xFC, -0x78, -0x30, -0x00, - -0x44, -0xEE, -0xFC, -0xF8, -0xF0, -0xF8, -0xFC, -0xEE, -0x44, -0x00, - -0x40, -0xE0, -0xE0, -0xE0, -0xE0, -0xE0, -0xFC, -0xFE, -0x7C, -0x00, - -0x44, -0xEE, -0xFE, -0xFE, -0xFE, -0xFE, -0xEE, -0xEE, -0x44, -0x00, - -0x44, -0xEE, -0xEE, -0xFE, -0xFE, -0xFE, -0xEE, -0xEE, -0x44, -0x00, - -0x38, -0x7C, -0xFE, -0xEE, -0xEE, -0xEE, -0xFE, -0x7C, -0x38, -0x00, - -0x78, -0xFC, -0xFE, -0xFE, -0xFC, -0xF8, -0xE0, -0xE0, -0x40, -0x00, - -0x38, -0x7C, -0xFE, -0xEE, -0xEE, -0xFE, -0xFC, -0x7E, -0x34, -0x00, - -0x78, -0xFC, -0xFE, -0xFE, -0xFC, -0xF8, -0xFC, -0xEE, -0x44, -0x00, - -0x38, -0x7C, -0xFE, -0xFC, -0x7C, -0x7E, -0xFE, -0x7C, -0x38, -0x00, - -0x7C, -0xFE, -0x7C, -0x38, -0x38, -0x38, -0x38, -0x38, -0x10, -0x00, - -0x44, -0xEE, -0xEE, -0xEE, -0xEE, -0xEE, -0xFE, -0x7C, -0x38, -0x00, - -0x44, -0xEE, -0xEE, -0xEE, -0xEE, -0xEE, -0x7C, -0x38, -0x10, -0x00, - -0x44, -0xEE, -0xEE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, -0x28, -0x00, - -0x44, -0xEE, -0xEE, -0x7C, -0x38, -0x7C, -0xEE, -0xEE, -0x44, -0x00, - -0x44, -0xEE, -0xEE, -0xEE, -0x7C, -0x38, -0x38, -0x38, -0x10, -0x00, - -0x7C, -0xFE, -0x7E, -0x1C, -0x38, -0x70, -0xFC, -0xFE, -0x7C, -0x00, - -0x38, -0x7C, -0x78, -0x70, -0x70, -0x70, -0x78, -0x7C, -0x38, -0x00, - -0x00, -0x40, -0xE0, -0x70, -0x38, -0x1C, -0x0E, -0x04, -0x00, -0x00, - -0x38, -0x7C, -0x3C, -0x1C, -0x1C, -0x1C, -0x3C, -0x7C, -0x38, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x38, -0x7C, -0x3E, -0x7E, -0xFE, -0x7E, -0x3C, -0x00, - -0x40, -0xE0, -0xE0, -0xF8, -0xFC, -0xFE, -0xFE, -0xFC, -0x78, -0x00, - -0x00, -0x00, -0x3C, -0x7E, -0xFC, -0xE0, -0xFC, -0x7E, -0x3C, -0x00, - -0x04, -0x0E, -0x0E, -0x3E, -0x7E, -0xFE, -0xFE, -0x7E, -0x3C, -0x00, - -0x00, -0x00, -0x38, -0x7C, -0xFE, -0xFE, -0xFC, -0x7E, -0x3C, -0x00, - -0x08, -0x1C, -0x3E, -0x7C, -0xFE, -0x7C, -0x38, -0x38, -0x10, -0x00, - -0x00, -0x00, -0x3C, -0x7E, -0xFE, -0x7E, -0x7E, -0xFC, -0x78, -0x00, - -0x40, -0xE0, -0xE0, -0xF8, -0xFC, -0xFE, -0xEE, -0xEE, -0x44, -0x00, - -0x00, -0x10, -0x38, -0x10, -0x38, -0x38, -0x38, -0x38, -0x10, -0x00, - -0x08, -0x1C, -0x08, -0x1C, -0x1C, -0x5C, -0xFC, -0x78, -0x30, -0x00, - -0x20, -0x70, -0x74, -0x7E, -0x7C, -0x78, -0x7C, -0x7E, -0x24, -0x00, - -0x30, -0x78, -0x38, -0x38, -0x38, -0x38, -0x38, -0x7C, -0x38, -0x00, - -0x00, -0x00, -0x68, -0xFC, -0xFE, -0xFE, -0xFE, -0xEE, -0x6C, -0x00, - -0x00, -0x00, -0x58, -0xFC, -0xFE, -0xEE, -0xEE, -0xEE, -0x44, -0x00, - -0x00, -0x00, -0x38, -0x7C, -0xFE, -0xEE, -0xFE, -0x7C, -0x38, -0x00, - -0x00, -0x00, -0x78, -0xFC, -0xFE, -0xFC, -0xF8, -0xE0, -0x40, -0x00, - -0x00, -0x00, -0x3C, -0x7E, -0xFE, -0x7E, -0x3E, -0x0E, -0x04, -0x00, - -0x00, -0x00, -0x58, -0xFC, -0xFE, -0xE4, -0xE0, -0xE0, -0x40, -0x00, - -0x00, -0x00, -0x3C, -0x7E, -0xFC, -0x7C, -0x7E, -0xFC, -0x78, -0x00, - -0x10, -0x38, -0x7C, -0xFE, -0x7C, -0x3C, -0x3E, -0x1C, -0x08, -0x00, - -0x00, -0x00, -0x44, -0xEE, -0xEE, -0xEE, -0xFE, -0x7C, -0x38, -0x00, - -0x00, -0x00, -0x44, -0xEE, -0xEE, -0xEE, -0x7C, -0x38, -0x10, -0x00, - -0x00, -0x00, -0x44, -0xEE, -0xFE, -0xFE, -0xFE, -0x7C, -0x28, -0x00, - -0x00, -0x00, -0x44, -0xEE, -0x7C, -0x38, -0x7C, -0xEE, -0x44, -0x00, - -0x00, -0x00, -0x44, -0xEE, -0x7C, -0x38, -0x38, -0x70, -0x20, -0x00, - -0x00, -0x00, -0x7C, -0xFE, -0x7C, -0x38, -0x7C, -0xFE, -0x7C, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x10, -0x38, -0x38, -0x38, -0x38, -0x38, -0x38, -0x38, -0x10, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x38, -0x7C, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, - -0x38, -0x7C, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, - -0x38, -0x7C, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, - -0x38, -0x7C, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, - -0x38, -0x7C, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, - -0x38, -0x7C, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0xFE, -0x7C, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0xFF, -0xFF, -0xFF, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x18, -0x3C, -0xFF, -0xFF, -0xFF, -0x3C, -0x18, -0x00, - -0x42, -0xE7, -0xE7, -0xF7, -0xFF, -0xFF, -0xEF, -0xE7, -0xE7, -0x42, - -0x3C, -0x7E, -0xFF, -0xF2, -0xFD, -0xFF, -0xCF, -0xFF, -0x7E, -0x3C, - -0x3E, -0x7F, -0x7E, -0x78, -0xFF, -0xFF, -0xF3, -0x7E, -0x7F, -0x3E, - -0x42, -0xE7, -0xE7, -0xE7, -0xFF, -0xFF, -0xFF, -0xFF, -0x7E, -0x24, - -0x00, -0x00, -0x00, -0x00, -0xFF, -0x00, -0xFF, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x81, -0xC3, -0xE7, -0x7E, -0x3C, -0x18, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x00, -0x18, -0x7E, -0x7E, -0xFF, -0xFF, -0x7E, -0x7E, -0x18, -0x00, - -0x07, -0x0F, -0x0F, -0x4E, -0xFE, -0x7E, -0x7E, -0x7E, -0x7E, -0x24, - -0xE0, -0xF0, -0xF0, -0x72, -0x7F, -0x7E, -0x7E, -0x7E, -0x7E, -0x24, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, -0x00, - + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0xF0, + 0xF8, + 0xFC, + 0xFE, + 0xFF, + 0xFF, + 0x7F, + 0x3F, + 0x1F, + 0x00, + + 0xF0, + 0xF8, + 0xFC, + 0xFE, + 0xFF, + 0xFF, + 0xFF, + 0x3F, + 0x1F, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x10, + 0x38, + 0x38, + 0x38, + 0x38, + 0x38, + 0x10, + 0x38, + 0x10, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x28, + 0x7C, + 0x7C, + 0xFE, + 0x7C, + 0xFE, + 0x7C, + 0x7C, + 0x28, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x7C, + 0xFE, + 0x7C, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x30, + 0x78, + 0x78, + 0x30, + 0x00, + + 0x00, + 0x04, + 0x0E, + 0x1C, + 0x38, + 0x70, + 0xE0, + 0x40, + 0x00, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x10, + 0x38, + 0x78, + 0x38, + 0x38, + 0x38, + 0x38, + 0x7C, + 0x38, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0x4E, + 0x1C, + 0x38, + 0x7C, + 0xFE, + 0x7C, + 0x00, + + 0x7C, + 0xFE, + 0x7C, + 0x38, + 0x1C, + 0x4E, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x08, + 0x1C, + 0x3C, + 0x7C, + 0xFC, + 0xFE, + 0x7C, + 0x1C, + 0x08, + 0x00, + + 0x7C, + 0xFE, + 0xFC, + 0xFC, + 0x7E, + 0x4E, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x18, + 0x3C, + 0x78, + 0xF8, + 0xFC, + 0xFE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x7C, + 0xFE, + 0x7E, + 0x1C, + 0x38, + 0x70, + 0x70, + 0x70, + 0x20, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0x7C, + 0xFE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0x7E, + 0x3E, + 0x3C, + 0x78, + 0x30, + 0x00, + + 0x00, + 0x00, + 0x30, + 0x78, + 0x78, + 0x30, + 0x78, + 0x78, + 0x30, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x7C, + 0xFE, + 0x7C, + 0xFE, + 0x7C, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0x7E, + 0x7E, + 0xFE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xEE, + 0xFE, + 0xFE, + 0xFE, + 0xEE, + 0x44, + 0x00, + + 0x78, + 0xFC, + 0xFE, + 0xFE, + 0xFC, + 0xFE, + 0xFE, + 0xFC, + 0x78, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xE4, + 0xE0, + 0xE4, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x70, + 0xF8, + 0xFC, + 0xEE, + 0xEE, + 0xEE, + 0xFC, + 0xF8, + 0x70, + 0x00, + + 0x7C, + 0xFE, + 0xFC, + 0xF8, + 0xFC, + 0xF8, + 0xFC, + 0xFE, + 0x7C, + 0x00, + + 0x7C, + 0xFE, + 0xFC, + 0xF8, + 0xFC, + 0xF8, + 0xE0, + 0xE0, + 0x40, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xFC, + 0xFC, + 0xFE, + 0xFE, + 0x7E, + 0x3C, + 0x00, + + 0x44, + 0xEE, + 0xEE, + 0xFE, + 0xFE, + 0xFE, + 0xEE, + 0xEE, + 0x44, + 0x00, + + 0x38, + 0x7C, + 0x38, + 0x38, + 0x38, + 0x38, + 0x38, + 0x7C, + 0x38, + 0x00, + + 0x1C, + 0x3E, + 0x1C, + 0x1C, + 0x1C, + 0x5C, + 0xFC, + 0x78, + 0x30, + 0x00, + + 0x44, + 0xEE, + 0xFC, + 0xF8, + 0xF0, + 0xF8, + 0xFC, + 0xEE, + 0x44, + 0x00, + + 0x40, + 0xE0, + 0xE0, + 0xE0, + 0xE0, + 0xE0, + 0xFC, + 0xFE, + 0x7C, + 0x00, + + 0x44, + 0xEE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xEE, + 0xEE, + 0x44, + 0x00, + + 0x44, + 0xEE, + 0xEE, + 0xFE, + 0xFE, + 0xFE, + 0xEE, + 0xEE, + 0x44, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xEE, + 0xEE, + 0xEE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x78, + 0xFC, + 0xFE, + 0xFE, + 0xFC, + 0xF8, + 0xE0, + 0xE0, + 0x40, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xEE, + 0xEE, + 0xFE, + 0xFC, + 0x7E, + 0x34, + 0x00, + + 0x78, + 0xFC, + 0xFE, + 0xFE, + 0xFC, + 0xF8, + 0xFC, + 0xEE, + 0x44, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xFC, + 0x7C, + 0x7E, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x7C, + 0xFE, + 0x7C, + 0x38, + 0x38, + 0x38, + 0x38, + 0x38, + 0x10, + 0x00, + + 0x44, + 0xEE, + 0xEE, + 0xEE, + 0xEE, + 0xEE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x44, + 0xEE, + 0xEE, + 0xEE, + 0xEE, + 0xEE, + 0x7C, + 0x38, + 0x10, + 0x00, + + 0x44, + 0xEE, + 0xEE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + 0x28, + 0x00, + + 0x44, + 0xEE, + 0xEE, + 0x7C, + 0x38, + 0x7C, + 0xEE, + 0xEE, + 0x44, + 0x00, + + 0x44, + 0xEE, + 0xEE, + 0xEE, + 0x7C, + 0x38, + 0x38, + 0x38, + 0x10, + 0x00, + + 0x7C, + 0xFE, + 0x7E, + 0x1C, + 0x38, + 0x70, + 0xFC, + 0xFE, + 0x7C, + 0x00, + + 0x38, + 0x7C, + 0x78, + 0x70, + 0x70, + 0x70, + 0x78, + 0x7C, + 0x38, + 0x00, + + 0x00, + 0x40, + 0xE0, + 0x70, + 0x38, + 0x1C, + 0x0E, + 0x04, + 0x00, + 0x00, + + 0x38, + 0x7C, + 0x3C, + 0x1C, + 0x1C, + 0x1C, + 0x3C, + 0x7C, + 0x38, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x38, + 0x7C, + 0x3E, + 0x7E, + 0xFE, + 0x7E, + 0x3C, + 0x00, + + 0x40, + 0xE0, + 0xE0, + 0xF8, + 0xFC, + 0xFE, + 0xFE, + 0xFC, + 0x78, + 0x00, + + 0x00, + 0x00, + 0x3C, + 0x7E, + 0xFC, + 0xE0, + 0xFC, + 0x7E, + 0x3C, + 0x00, + + 0x04, + 0x0E, + 0x0E, + 0x3E, + 0x7E, + 0xFE, + 0xFE, + 0x7E, + 0x3C, + 0x00, + + 0x00, + 0x00, + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFC, + 0x7E, + 0x3C, + 0x00, + + 0x08, + 0x1C, + 0x3E, + 0x7C, + 0xFE, + 0x7C, + 0x38, + 0x38, + 0x10, + 0x00, + + 0x00, + 0x00, + 0x3C, + 0x7E, + 0xFE, + 0x7E, + 0x7E, + 0xFC, + 0x78, + 0x00, + + 0x40, + 0xE0, + 0xE0, + 0xF8, + 0xFC, + 0xFE, + 0xEE, + 0xEE, + 0x44, + 0x00, + + 0x00, + 0x10, + 0x38, + 0x10, + 0x38, + 0x38, + 0x38, + 0x38, + 0x10, + 0x00, + + 0x08, + 0x1C, + 0x08, + 0x1C, + 0x1C, + 0x5C, + 0xFC, + 0x78, + 0x30, + 0x00, + + 0x20, + 0x70, + 0x74, + 0x7E, + 0x7C, + 0x78, + 0x7C, + 0x7E, + 0x24, + 0x00, + + 0x30, + 0x78, + 0x38, + 0x38, + 0x38, + 0x38, + 0x38, + 0x7C, + 0x38, + 0x00, + + 0x00, + 0x00, + 0x68, + 0xFC, + 0xFE, + 0xFE, + 0xFE, + 0xEE, + 0x6C, + 0x00, + + 0x00, + 0x00, + 0x58, + 0xFC, + 0xFE, + 0xEE, + 0xEE, + 0xEE, + 0x44, + 0x00, + + 0x00, + 0x00, + 0x38, + 0x7C, + 0xFE, + 0xEE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x00, + 0x00, + 0x78, + 0xFC, + 0xFE, + 0xFC, + 0xF8, + 0xE0, + 0x40, + 0x00, + + 0x00, + 0x00, + 0x3C, + 0x7E, + 0xFE, + 0x7E, + 0x3E, + 0x0E, + 0x04, + 0x00, + + 0x00, + 0x00, + 0x58, + 0xFC, + 0xFE, + 0xE4, + 0xE0, + 0xE0, + 0x40, + 0x00, + + 0x00, + 0x00, + 0x3C, + 0x7E, + 0xFC, + 0x7C, + 0x7E, + 0xFC, + 0x78, + 0x00, + + 0x10, + 0x38, + 0x7C, + 0xFE, + 0x7C, + 0x3C, + 0x3E, + 0x1C, + 0x08, + 0x00, + + 0x00, + 0x00, + 0x44, + 0xEE, + 0xEE, + 0xEE, + 0xFE, + 0x7C, + 0x38, + 0x00, + + 0x00, + 0x00, + 0x44, + 0xEE, + 0xEE, + 0xEE, + 0x7C, + 0x38, + 0x10, + 0x00, + + 0x00, + 0x00, + 0x44, + 0xEE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + 0x28, + 0x00, + + 0x00, + 0x00, + 0x44, + 0xEE, + 0x7C, + 0x38, + 0x7C, + 0xEE, + 0x44, + 0x00, + + 0x00, + 0x00, + 0x44, + 0xEE, + 0x7C, + 0x38, + 0x38, + 0x70, + 0x20, + 0x00, + + 0x00, + 0x00, + 0x7C, + 0xFE, + 0x7C, + 0x38, + 0x7C, + 0xFE, + 0x7C, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x10, + 0x38, + 0x38, + 0x38, + 0x38, + 0x38, + 0x38, + 0x38, + 0x10, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + + 0x38, + 0x7C, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0xFE, + 0x7C, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0xFF, + 0xFF, + 0xFF, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x18, + 0x3C, + 0xFF, + 0xFF, + 0xFF, + 0x3C, + 0x18, + 0x00, + + 0x42, + 0xE7, + 0xE7, + 0xF7, + 0xFF, + 0xFF, + 0xEF, + 0xE7, + 0xE7, + 0x42, + + 0x3C, + 0x7E, + 0xFF, + 0xF2, + 0xFD, + 0xFF, + 0xCF, + 0xFF, + 0x7E, + 0x3C, + + 0x3E, + 0x7F, + 0x7E, + 0x78, + 0xFF, + 0xFF, + 0xF3, + 0x7E, + 0x7F, + 0x3E, + + 0x42, + 0xE7, + 0xE7, + 0xE7, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0x7E, + 0x24, + + 0x00, + 0x00, + 0x00, + 0x00, + 0xFF, + 0x00, + 0xFF, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x81, + 0xC3, + 0xE7, + 0x7E, + 0x3C, + 0x18, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x00, + 0x18, + 0x7E, + 0x7E, + 0xFF, + 0xFF, + 0x7E, + 0x7E, + 0x18, + 0x00, + + 0x07, + 0x0F, + 0x0F, + 0x4E, + 0xFE, + 0x7E, + 0x7E, + 0x7E, + 0x7E, + 0x24, + + 0xE0, + 0xF0, + 0xF0, + 0x72, + 0x7F, + 0x7E, + 0x7E, + 0x7E, + 0x7E, + 0x24, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, }; #endif /* FONT8X10_H_ */ diff --git a/flight/targets/boards/osd/firmware/inc/fonts.h b/flight/targets/boards/osd/firmware/inc/fonts.h index 8b7d35650..104392471 100644 --- a/flight/targets/boards/osd/firmware/inc/fonts.h +++ b/flight/targets/boards/osd/firmware/inc/fonts.h @@ -6,12 +6,12 @@ * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. - * + * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * + * * You should have received a copy of the GNU General Public License along * with this program; if not, write to the Free Software Foundation, Inc., * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. @@ -26,21 +26,20 @@ #include "font_outlined8x8.h" // This number must also be incremented for each new font. -#define NUM_FONTS 4 +#define NUM_FONTS 4 // Flags for fonts. -#define FONT_LOWERCASE_ONLY 1 -#define FONT_UPPERCASE_ONLY 2 +#define FONT_LOWERCASE_ONLY 1 +#define FONT_UPPERCASE_ONLY 2 // Font table. (Actual list of fonts in fonts.c.) -struct FontEntry -{ - int id; - unsigned char width, height; - const char *name; - const char *lookup; - const char *data; - int flags; +struct FontEntry { + int id; + unsigned char width, height; + const char *name; + const char *lookup; + const char *data; + int flags; }; extern struct FontEntry fonts[NUM_FONTS + 1]; diff --git a/flight/targets/boards/osd/firmware/inc/openpilot.h b/flight/targets/boards/osd/firmware/inc/openpilot.h index c0985e4c0..76cc3ccd6 100644 --- a/flight/targets/boards/osd/firmware/inc/openpilot.h +++ b/flight/targets/boards/osd/firmware/inc/openpilot.h @@ -10,19 +10,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/osd/firmware/inc/pios_config.h b/flight/targets/boards/osd/firmware/inc/pios_config.h index ee6aaec4c..a0c7d6362 100644 --- a/flight/targets/boards/osd/firmware/inc/pios_config.h +++ b/flight/targets/boards/osd/firmware/inc/pios_config.h @@ -142,15 +142,15 @@ #define PIOS_QUATERNION_STABILIZATION /* Performance counters */ -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 /* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 4000 -#define HEAP_LIMIT_CRITICAL 1000 -#define IRQSTACK_LIMIT_WARNING 150 -#define IRQSTACK_LIMIT_CRITICAL 80 -#define CPULOAD_LIMIT_WARNING 80 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 4000 +#define HEAP_LIMIT_CRITICAL 1000 +#define IRQSTACK_LIMIT_WARNING 150 +#define IRQSTACK_LIMIT_CRITICAL 80 +#define CPULOAD_LIMIT_WARNING 80 +#define CPULOAD_LIMIT_CRITICAL 95 /* Task stack sizes */ /* #define PIOS_ACTUATOR_STACK_SIZE 1020 */ diff --git a/flight/targets/boards/osd/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/osd/firmware/inc/pios_usb_board_data.h index 867849949..32e9f0ea6 100644 --- a/flight/targets/boards/osd/firmware/inc/pios_usb_board_data.h +++ b/flight/targets/boards/osd/firmware/inc/pios_usb_board_data.h @@ -35,12 +35,12 @@ #define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 4 +#define PIOS_USB_BOARD_EP_NUM 4 -#include /* USB_* macros */ +#include /* USB_* macros */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OSD -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OSD, USB_OP_BOARD_MODE_FW) -#define PIOS_USB_BOARD_SN_SUFFIX "+FW" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OSD +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OSD, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/osd/firmware/inc/splash.h b/flight/targets/boards/osd/firmware/inc/splash.h index 3ea31872c..1ba96618b 100644 --- a/flight/targets/boards/osd/firmware/inc/splash.h +++ b/flight/targets/boards/osd/firmware/inc/splash.h @@ -8,1442 +8,1448 @@ #ifndef SPLASH_H_ #define SPLASH_H_ -#define oplogo_width 144 +#define oplogo_width 144 #define oplogo_height 144 static const unsigned short oplogo_bits[] = { - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x001e, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0x01ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x07ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x0fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x7fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x0003, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x0007, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x000f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x001f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x07ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x0fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x1fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x3fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0x03ff, 0xffff, 0x0001, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0x03ff, 0xfffe, 0x0003, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0x01ff, 0xfff8, 0x000f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0x01ff, 0xffe0, 0x001f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0x01ff, 0xffc0, 0x003f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0x01ff, 0xff00, 0x007f, 0x0000, - 0x0000, 0x0000, 0x01f0, 0x0000, 0xff80, 0x00ff, 0xfe00, 0x01ff, 0x0000, - 0x0070, 0x0000, 0x07fc, 0x0000, 0xff80, 0x00ff, 0xf800, 0x03ff, 0x0000, - 0x07c0, 0x0000, 0x1fff, 0x0000, 0xffc0, 0x00ff, 0xf000, 0x07ff, 0x0000, - 0x7f80, 0xe000, 0x3fff, 0x0000, 0xffc0, 0x00ff, 0xc000, 0x0fff, 0x0000, - 0xfe00, 0xf803, 0x3fff, 0x0000, 0xffc0, 0x007f, 0x8000, 0x3fff, 0x0000, - 0xfc00, 0xfe3f, 0x1fff, 0x0000, 0xffc0, 0x007f, 0x0000, 0x7ffe, 0x0000, - 0xf000, 0xffff, 0x1fff, 0x0000, 0xffe0, 0x007f, 0x0000, 0xfffc, 0x0000, - 0xe000, 0xffff, 0x1fff, 0x0060, 0xffe0, 0x007f, 0x0000, 0xfff0, 0x0000, - 0x8000, 0xffff, 0x1fff, 0x01e0, 0xffe0, 0x003f, 0x0000, 0xffe0, 0x0001, - 0x0000, 0xffff, 0x0fff, 0x07f0, 0xfff0, 0x003f, 0x0000, 0xff80, 0x0003, - 0x0000, 0xfffc, 0x0fff, 0x1ff0, 0xfff0, 0x003f, 0x0000, 0xff00, 0x0003, - 0x0000, 0xfff8, 0x0fff, 0x7ff0, 0xfff0, 0x001f, 0x0000, 0xfe00, 0x0007, - 0x0000, 0xfff0, 0x0fff, 0xfff0, 0xfff0, 0x001f, 0x0000, 0xfc00, 0x0007, - 0x0000, 0xffc0, 0x0fff, 0xfff8, 0xffc3, 0x001f, 0x0000, 0xf800, 0x0007, - 0x0000, 0xffc0, 0x1fff, 0xfff8, 0xff0f, 0x001f, 0x0000, 0xf800, 0x0007, - 0x0000, 0xffc0, 0x3fff, 0xfff8, 0xfc3f, 0x000f, 0x0000, 0xf800, 0x0007, - 0x0000, 0xff80, 0x3fff, 0xfff8, 0xf8ff, 0x000f, 0x0000, 0xf800, 0x000f, - 0x0000, 0xff80, 0x3fff, 0xfffc, 0xe1ff, 0x000f, 0x0000, 0xf800, 0x000f, - 0x0000, 0xff00, 0x3fff, 0xfffc, 0x87ff, 0x000f, 0x0000, 0xf800, 0x000f, - 0x0000, 0xff00, 0x1fff, 0xfffc, 0x1fff, 0x0006, 0x0000, 0xf800, 0x000f, - 0x0000, 0xff00, 0x1fff, 0xfffc, 0x7fff, 0x0000, 0x0000, 0xf800, 0x000f, - 0x0000, 0xfe00, 0x1fff, 0xfffe, 0xffff, 0x0001, 0x0000, 0xf800, 0x000f, - 0x0000, 0xfe00, 0x0fff, 0xfffe, 0xffff, 0x0003, 0x0000, 0xf800, 0x000f, - 0x0000, 0xfc00, 0x01ff, 0xfffe, 0xffff, 0x000f, 0x0000, 0xf800, 0x000f, - 0x0000, 0xfc00, 0x01ff, 0xfffe, 0xffff, 0x003f, 0x0000, 0xf800, 0x000f, - 0x0000, 0xf800, 0x01ff, 0xffff, 0xffff, 0x00ff, 0x0000, 0xf800, 0x000f, - 0x0000, 0xf800, 0x00ff, 0xffff, 0xffff, 0x03ff, 0x0000, 0xf800, 0x000f, - 0x0000, 0xf800, 0x00ff, 0xffff, 0xffff, 0x0fff, 0x0000, 0xf800, 0x000f, - 0x0000, 0xf000, 0x00ff, 0xffff, 0xffff, 0x3fff, 0x0000, 0xf800, 0x000f, - 0x0000, 0xf000, 0x807f, 0xffff, 0xffff, 0x7fff, 0x0000, 0xf800, 0x000f, - 0x0000, 0xe000, 0x807f, 0xffff, 0xffff, 0xffff, 0x0001, 0xf800, 0x000f, - 0x0000, 0x8000, 0x807f, 0xffff, 0xffff, 0xffff, 0x0007, 0xfc00, 0x000f, - 0x0000, 0x0000, 0x807c, 0xffff, 0xffff, 0xffff, 0x001f, 0xfc00, 0x000f, - 0x0000, 0x0000, 0xc020, 0xffff, 0xffff, 0xffff, 0x007f, 0xfc00, 0x000f, - 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x00ff, 0xfc00, 0x000f, - 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x03ff, 0xfc00, 0x000f, - 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x0fff, 0xfc00, 0x000f, - 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0x3fff, 0xfc00, 0x000f, - 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xfc00, 0x000f, - 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xfc03, 0x000f, - 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xfe0f, 0x000f, - 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xfe1f, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0600, 0xc000, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x3e00, 0x0000, 0xfffe, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0xff00, 0x0001, 0xfff8, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0xff00, 0x000f, 0xffc0, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0xff00, 0x007f, 0xfe00, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0xff00, 0x03ff, 0xf000, 0xffff, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0xff80, 0x03ff, 0xc000, 0xffff, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0xff80, 0x03ff, 0x0000, 0xfffe, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0xff80, 0x03ff, 0x0000, 0xfff0, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0xffc0, 0x03ff, 0x0000, 0xff80, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0xffc0, 0x01ff, 0x0000, 0xfe00, 0xffff, 0x0003, - 0x0000, 0x0000, 0x0000, 0xffc0, 0x01ff, 0x0000, 0xf000, 0xffff, 0x0003, - 0x0000, 0x0000, 0x0000, 0xffc0, 0x01ff, 0x0000, 0x8000, 0xffff, 0x0001, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x01ff, 0x0000, 0x0000, 0xfffc, 0x0001, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x00ff, 0x0000, 0x0000, 0xfff0, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x00ff, 0x0000, 0x0000, 0x7f80, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x00ff, 0x0000, 0x0000, 0x0c00, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffc0, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff00, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfc00, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf000, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xc000, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x8000, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x000e, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1980, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x39c0, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3980, 0x0000, 0x0060, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3800, 0x0000, 0x0060, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3800, 0x0000, 0x00f0, - 0x0000, 0x03ff, 0x0ffc, 0x3ff0, 0xffc0, 0xff80, 0x3983, 0x7fe0, 0x07f8, - 0xc000, 0x07ff, 0x1fff, 0x7ffc, 0xfff0, 0xffc1, 0x39c7, 0xfff0, 0x07f9, - 0xe000, 0x8fff, 0x3fff, 0xfffe, 0xfff8, 0xffe3, 0x39cf, 0xfff8, 0x0063, - 0xe000, 0x9c00, 0x7003, 0xc00e, 0x8038, 0x00e3, 0x39ce, 0x801c, 0x0063, - 0x6000, 0x9c00, 0x7001, 0xc006, 0x0019, 0x0077, 0x39dc, 0x001c, 0x0063, - 0x6000, 0x9c00, 0x7001, 0xfffe, 0x0019, 0x0077, 0x39dc, 0x001c, 0x0063, - 0x6000, 0x9c00, 0x7001, 0xfffe, 0x0019, 0x0077, 0x39dc, 0x001c, 0x0063, - 0x6000, 0x9c00, 0x7001, 0x000e, 0x0018, 0x0077, 0x39dc, 0x001c, 0x0063, - 0xe000, 0x9c00, 0x7001, 0x000e, 0x0018, 0x0077, 0x39ce, 0x801c, 0x00e3, - 0xe000, 0xff8f, 0x3fff, 0x7ffe, 0x0018, 0xffff, 0xf1cf, 0xe0f8, 0x07e3, - 0xc000, 0xffff, 0x1fff, 0x7ffc, 0x0018, 0xffff, 0xf1c7, 0xfff0, 0x07c1, - 0x8000, 0xffff, 0x0fff, 0x7ff8, 0x0018, 0xffff, 0xc183, 0xffe0, 0x0780, - 0x0000, 0xc000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0060, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000 }; + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x001e, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0x01ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x07ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x0fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x7fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x0003, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x0007, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x000f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x001f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x07ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x0fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x1fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x3fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0x03ff, 0xffff, 0x0001, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0x03ff, 0xfffe, 0x0003, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0x01ff, 0xfff8, 0x000f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0x01ff, 0xffe0, 0x001f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0x01ff, 0xffc0, 0x003f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0x01ff, 0xff00, 0x007f, 0x0000, + 0x0000, 0x0000, 0x01f0, 0x0000, 0xff80, 0x00ff, 0xfe00, 0x01ff, 0x0000, + 0x0070, 0x0000, 0x07fc, 0x0000, 0xff80, 0x00ff, 0xf800, 0x03ff, 0x0000, + 0x07c0, 0x0000, 0x1fff, 0x0000, 0xffc0, 0x00ff, 0xf000, 0x07ff, 0x0000, + 0x7f80, 0xe000, 0x3fff, 0x0000, 0xffc0, 0x00ff, 0xc000, 0x0fff, 0x0000, + 0xfe00, 0xf803, 0x3fff, 0x0000, 0xffc0, 0x007f, 0x8000, 0x3fff, 0x0000, + 0xfc00, 0xfe3f, 0x1fff, 0x0000, 0xffc0, 0x007f, 0x0000, 0x7ffe, 0x0000, + 0xf000, 0xffff, 0x1fff, 0x0000, 0xffe0, 0x007f, 0x0000, 0xfffc, 0x0000, + 0xe000, 0xffff, 0x1fff, 0x0060, 0xffe0, 0x007f, 0x0000, 0xfff0, 0x0000, + 0x8000, 0xffff, 0x1fff, 0x01e0, 0xffe0, 0x003f, 0x0000, 0xffe0, 0x0001, + 0x0000, 0xffff, 0x0fff, 0x07f0, 0xfff0, 0x003f, 0x0000, 0xff80, 0x0003, + 0x0000, 0xfffc, 0x0fff, 0x1ff0, 0xfff0, 0x003f, 0x0000, 0xff00, 0x0003, + 0x0000, 0xfff8, 0x0fff, 0x7ff0, 0xfff0, 0x001f, 0x0000, 0xfe00, 0x0007, + 0x0000, 0xfff0, 0x0fff, 0xfff0, 0xfff0, 0x001f, 0x0000, 0xfc00, 0x0007, + 0x0000, 0xffc0, 0x0fff, 0xfff8, 0xffc3, 0x001f, 0x0000, 0xf800, 0x0007, + 0x0000, 0xffc0, 0x1fff, 0xfff8, 0xff0f, 0x001f, 0x0000, 0xf800, 0x0007, + 0x0000, 0xffc0, 0x3fff, 0xfff8, 0xfc3f, 0x000f, 0x0000, 0xf800, 0x0007, + 0x0000, 0xff80, 0x3fff, 0xfff8, 0xf8ff, 0x000f, 0x0000, 0xf800, 0x000f, + 0x0000, 0xff80, 0x3fff, 0xfffc, 0xe1ff, 0x000f, 0x0000, 0xf800, 0x000f, + 0x0000, 0xff00, 0x3fff, 0xfffc, 0x87ff, 0x000f, 0x0000, 0xf800, 0x000f, + 0x0000, 0xff00, 0x1fff, 0xfffc, 0x1fff, 0x0006, 0x0000, 0xf800, 0x000f, + 0x0000, 0xff00, 0x1fff, 0xfffc, 0x7fff, 0x0000, 0x0000, 0xf800, 0x000f, + 0x0000, 0xfe00, 0x1fff, 0xfffe, 0xffff, 0x0001, 0x0000, 0xf800, 0x000f, + 0x0000, 0xfe00, 0x0fff, 0xfffe, 0xffff, 0x0003, 0x0000, 0xf800, 0x000f, + 0x0000, 0xfc00, 0x01ff, 0xfffe, 0xffff, 0x000f, 0x0000, 0xf800, 0x000f, + 0x0000, 0xfc00, 0x01ff, 0xfffe, 0xffff, 0x003f, 0x0000, 0xf800, 0x000f, + 0x0000, 0xf800, 0x01ff, 0xffff, 0xffff, 0x00ff, 0x0000, 0xf800, 0x000f, + 0x0000, 0xf800, 0x00ff, 0xffff, 0xffff, 0x03ff, 0x0000, 0xf800, 0x000f, + 0x0000, 0xf800, 0x00ff, 0xffff, 0xffff, 0x0fff, 0x0000, 0xf800, 0x000f, + 0x0000, 0xf000, 0x00ff, 0xffff, 0xffff, 0x3fff, 0x0000, 0xf800, 0x000f, + 0x0000, 0xf000, 0x807f, 0xffff, 0xffff, 0x7fff, 0x0000, 0xf800, 0x000f, + 0x0000, 0xe000, 0x807f, 0xffff, 0xffff, 0xffff, 0x0001, 0xf800, 0x000f, + 0x0000, 0x8000, 0x807f, 0xffff, 0xffff, 0xffff, 0x0007, 0xfc00, 0x000f, + 0x0000, 0x0000, 0x807c, 0xffff, 0xffff, 0xffff, 0x001f, 0xfc00, 0x000f, + 0x0000, 0x0000, 0xc020, 0xffff, 0xffff, 0xffff, 0x007f, 0xfc00, 0x000f, + 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x00ff, 0xfc00, 0x000f, + 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x03ff, 0xfc00, 0x000f, + 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x0fff, 0xfc00, 0x000f, + 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0x3fff, 0xfc00, 0x000f, + 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xfc00, 0x000f, + 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xfc03, 0x000f, + 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xfe0f, 0x000f, + 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xfe1f, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0600, 0xc000, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x3e00, 0x0000, 0xfffe, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0xff00, 0x0001, 0xfff8, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0xff00, 0x000f, 0xffc0, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0xff00, 0x007f, 0xfe00, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0xff00, 0x03ff, 0xf000, 0xffff, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0xff80, 0x03ff, 0xc000, 0xffff, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0xff80, 0x03ff, 0x0000, 0xfffe, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0xff80, 0x03ff, 0x0000, 0xfff0, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0xffc0, 0x03ff, 0x0000, 0xff80, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0xffc0, 0x01ff, 0x0000, 0xfe00, 0xffff, 0x0003, + 0x0000, 0x0000, 0x0000, 0xffc0, 0x01ff, 0x0000, 0xf000, 0xffff, 0x0003, + 0x0000, 0x0000, 0x0000, 0xffc0, 0x01ff, 0x0000, 0x8000, 0xffff, 0x0001, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x01ff, 0x0000, 0x0000, 0xfffc, 0x0001, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x00ff, 0x0000, 0x0000, 0xfff0, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x00ff, 0x0000, 0x0000, 0x7f80, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x00ff, 0x0000, 0x0000, 0x0c00, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffc0, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff00, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfc00, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf000, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xc000, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x8000, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x000e, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1980, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x39c0, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3980, 0x0000, 0x0060, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3800, 0x0000, 0x0060, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3800, 0x0000, 0x00f0, + 0x0000, 0x03ff, 0x0ffc, 0x3ff0, 0xffc0, 0xff80, 0x3983, 0x7fe0, 0x07f8, + 0xc000, 0x07ff, 0x1fff, 0x7ffc, 0xfff0, 0xffc1, 0x39c7, 0xfff0, 0x07f9, + 0xe000, 0x8fff, 0x3fff, 0xfffe, 0xfff8, 0xffe3, 0x39cf, 0xfff8, 0x0063, + 0xe000, 0x9c00, 0x7003, 0xc00e, 0x8038, 0x00e3, 0x39ce, 0x801c, 0x0063, + 0x6000, 0x9c00, 0x7001, 0xc006, 0x0019, 0x0077, 0x39dc, 0x001c, 0x0063, + 0x6000, 0x9c00, 0x7001, 0xfffe, 0x0019, 0x0077, 0x39dc, 0x001c, 0x0063, + 0x6000, 0x9c00, 0x7001, 0xfffe, 0x0019, 0x0077, 0x39dc, 0x001c, 0x0063, + 0x6000, 0x9c00, 0x7001, 0x000e, 0x0018, 0x0077, 0x39dc, 0x001c, 0x0063, + 0xe000, 0x9c00, 0x7001, 0x000e, 0x0018, 0x0077, 0x39ce, 0x801c, 0x00e3, + 0xe000, 0xff8f, 0x3fff, 0x7ffe, 0x0018, 0xffff, 0xf1cf, 0xe0f8, 0x07e3, + 0xc000, 0xffff, 0x1fff, 0x7ffc, 0x0018, 0xffff, 0xf1c7, 0xfff0, 0x07c1, + 0x8000, 0xffff, 0x0fff, 0x7ff8, 0x0018, 0xffff, 0xc183, 0xffe0, 0x0780, + 0x0000, 0xc000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0070, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0x0001, 0x0000, 0x0000, 0x0060, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000 +}; static const unsigned short oplogo_mask_bits[] = { - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x003f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x03ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x0fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0x1fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0x7fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x0001, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x0007, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x000f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x001f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x003f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x03ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x0fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x1fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x3fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x7fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0001, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0003, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0007, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x001f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0x003f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0x8fff, 0xffff, 0x007f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0x07ff, 0xffff, 0x00ff, 0x0000, - 0x0000, 0x0000, 0x03f8, 0x0000, 0xffe0, 0x07ff, 0xfffc, 0x03ff, 0x0000, - 0x00f8, 0x0000, 0x0ffe, 0x0000, 0xffe0, 0x07ff, 0xfff0, 0x07ff, 0x0000, - 0x0ffc, 0x8000, 0x3fff, 0x0000, 0xffe0, 0x07ff, 0xffe0, 0x0fff, 0x0000, - 0xfffc, 0xf000, 0x7fff, 0x0000, 0xfff0, 0x03ff, 0xff80, 0x1fff, 0x0000, - 0xfffc, 0xfc07, 0xffff, 0x0000, 0xfff0, 0x03ff, 0xff00, 0x7fff, 0x0000, - 0xfff8, 0xff7f, 0xffff, 0x0000, 0xfff0, 0x03ff, 0xfc00, 0xffff, 0x0000, - 0xffe0, 0xffff, 0xffff, 0x0000, 0xfff0, 0x03ff, 0xf800, 0xffff, 0x0001, - 0xffc0, 0xffff, 0xffff, 0x00f0, 0xfff8, 0x01ff, 0xe000, 0xffff, 0x0003, - 0xff00, 0xffff, 0x7fff, 0x03f8, 0xfff8, 0x01ff, 0xc000, 0xffff, 0x0003, - 0xfe00, 0xffff, 0x7fff, 0x0ff8, 0xfff8, 0x01ff, 0x0000, 0xffff, 0x0007, - 0xf800, 0xffff, 0x7fff, 0x3ffc, 0xfffc, 0x01ff, 0x0000, 0xfffe, 0x000f, - 0xf000, 0xffff, 0x7fff, 0xfffc, 0xfffc, 0x00ff, 0x0000, 0xfff8, 0x000f, - 0xc000, 0xffff, 0x3fff, 0xfffc, 0xfffd, 0x00ff, 0x0000, 0xfff0, 0x001f, - 0x8000, 0xffff, 0x3fff, 0xfffc, 0xffff, 0x00ff, 0x0000, 0xffc0, 0x001f, - 0x0000, 0xfffe, 0x3fff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xff80, 0x001f, - 0x0000, 0xfffc, 0x7fff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xff00, 0x001f, - 0x0000, 0xfff8, 0xffff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xfe00, 0x001f, - 0x0000, 0xfff0, 0xffff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xff80, 0xbfff, 0xffff, 0xffff, 0x01ff, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xff00, 0xdfff, 0xffff, 0xffff, 0x07ff, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xff00, 0xc7ff, 0xffff, 0xffff, 0x1fff, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xfe00, 0xc7ff, 0xffff, 0xffff, 0x7fff, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xfe00, 0xc3ff, 0xffff, 0xffff, 0xffff, 0x0000, 0xfe00, 0x003f, - 0x0000, 0xfe00, 0xe3ff, 0xffff, 0xffff, 0xffff, 0x0003, 0xfe00, 0x003f, - 0x0000, 0xfc00, 0xe3ff, 0xffff, 0xffff, 0xffff, 0x000f, 0xfe00, 0x003f, - 0x0000, 0xfc00, 0xe1ff, 0xffff, 0xffff, 0xffff, 0x003f, 0xff00, 0x003f, - 0x0000, 0xf800, 0xe1ff, 0xffff, 0xffff, 0xffff, 0x00ff, 0xff00, 0x003f, - 0x0000, 0xf000, 0xf1ff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xff00, 0x003f, - 0x0000, 0xc000, 0xf1ff, 0xffff, 0xffff, 0xffff, 0x07ff, 0xff00, 0x003f, - 0x0000, 0x0000, 0xf0fe, 0xffff, 0xffff, 0xffff, 0x1fff, 0xff00, 0x003f, - 0x0000, 0x0000, 0xf070, 0xffff, 0xffff, 0xffff, 0x7fff, 0xff00, 0x003f, - 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0xff01, 0x003f, - 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0xff07, 0x003f, - 0x0000, 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, 0xff1f, 0x003f, - 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0xffff, 0xffbf, 0x003f, - 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0x0f00, 0xfffc, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0x7f80, 0xffe0, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xff80, 0xff83, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xffc0, 0xfc1f, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xffc0, 0xe0ff, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xffc0, 0x07ff, 0xffff, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xffc0, 0x0fff, 0xfffc, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x0fff, 0xffe0, 0xffff, 0xffff, 0x003f, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x0fff, 0xff00, 0xffff, 0xffff, 0x001f, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x0fff, 0xf800, 0xffff, 0xffff, 0x001f, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x0fff, 0xe000, 0xffff, 0xffff, 0x001f, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x0fff, 0x0000, 0xffff, 0xffff, 0x001f, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x0fff, 0x0000, 0xfff8, 0xffff, 0x001f, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x07ff, 0x0000, 0xffc0, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x07ff, 0x0000, 0xff00, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x07ff, 0x0000, 0xf800, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x07ff, 0x0000, 0xc000, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0xfffc, 0x03ff, 0x0000, 0x0000, 0xfffe, 0x0003, - 0x0000, 0x0000, 0x0000, 0xfffc, 0x03ff, 0x0000, 0x0000, 0xfff8, 0x0001, - 0x0000, 0x0000, 0x0000, 0xfffc, 0x03ff, 0x0000, 0x0000, 0xffc0, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x1e00, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffc, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffe0, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfe00, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf800, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe000, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xc000, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fc0, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7fe0, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x0000, 0x00f0, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x0000, 0x01f8, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x0000, 0x01f8, - 0x8000, 0x07ff, 0x1ffe, 0x7ff8, 0xffe0, 0xffc1, 0xffe7, 0xfff0, 0x0ffc, - 0xe000, 0x8fff, 0x3fff, 0xfffe, 0xfff8, 0xffe3, 0xffef, 0xfff8, 0x1fff, - 0xf000, 0xdfff, 0x7fff, 0xffff, 0xfffd, 0xfff7, 0xffff, 0xfffc, 0x1fff, - 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xfffe, 0x1fff, - 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, - 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, - 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, - 0xf800, 0xff01, 0xfc07, 0xffff, 0xc07f, 0x01ff, 0xffff, 0xc07f, 0x01ff, - 0xf800, 0xff01, 0xfc07, 0xffff, 0xc07f, 0x01ff, 0xffff, 0xc07f, 0x01ff, - 0xf800, 0xffdf, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xf1ff, 0x0fff, - 0xf800, 0xffff, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1fff, - 0xf800, 0xffff, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1fff, - 0xf800, 0xffff, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1fff, - 0xf000, 0xffff, 0x7fff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1ff7, - 0xe000, 0xffff, 0x3fff, 0xfffe, 0xc07f, 0xffff, 0xffef, 0xfffb, 0x1fe3, - 0xc000, 0xffff, 0x1fff, 0xfffc, 0x803c, 0xffff, 0xe3c7, 0xfff1, 0x0fc1, - 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01fc, 0x0000, 0x0000, 0x0000, - 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01fc, 0x0000, 0x0000, 0x0000, - 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01fc, 0x0000, 0x0000, 0x0000, - 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01f8, 0x0000, 0x0000, 0x0000, - 0x0000, 0xc000, 0x0003, 0x0000, 0x0000, 0x00f0, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000 }; + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x003f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x03ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x0fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0x1fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0x7fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x0001, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x0007, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x000f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x001f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x003f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x03ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x0fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x1fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x3fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x7fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0001, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0003, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0007, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x001f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0x003f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0x8fff, 0xffff, 0x007f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0x07ff, 0xffff, 0x00ff, 0x0000, + 0x0000, 0x0000, 0x03f8, 0x0000, 0xffe0, 0x07ff, 0xfffc, 0x03ff, 0x0000, + 0x00f8, 0x0000, 0x0ffe, 0x0000, 0xffe0, 0x07ff, 0xfff0, 0x07ff, 0x0000, + 0x0ffc, 0x8000, 0x3fff, 0x0000, 0xffe0, 0x07ff, 0xffe0, 0x0fff, 0x0000, + 0xfffc, 0xf000, 0x7fff, 0x0000, 0xfff0, 0x03ff, 0xff80, 0x1fff, 0x0000, + 0xfffc, 0xfc07, 0xffff, 0x0000, 0xfff0, 0x03ff, 0xff00, 0x7fff, 0x0000, + 0xfff8, 0xff7f, 0xffff, 0x0000, 0xfff0, 0x03ff, 0xfc00, 0xffff, 0x0000, + 0xffe0, 0xffff, 0xffff, 0x0000, 0xfff0, 0x03ff, 0xf800, 0xffff, 0x0001, + 0xffc0, 0xffff, 0xffff, 0x00f0, 0xfff8, 0x01ff, 0xe000, 0xffff, 0x0003, + 0xff00, 0xffff, 0x7fff, 0x03f8, 0xfff8, 0x01ff, 0xc000, 0xffff, 0x0003, + 0xfe00, 0xffff, 0x7fff, 0x0ff8, 0xfff8, 0x01ff, 0x0000, 0xffff, 0x0007, + 0xf800, 0xffff, 0x7fff, 0x3ffc, 0xfffc, 0x01ff, 0x0000, 0xfffe, 0x000f, + 0xf000, 0xffff, 0x7fff, 0xfffc, 0xfffc, 0x00ff, 0x0000, 0xfff8, 0x000f, + 0xc000, 0xffff, 0x3fff, 0xfffc, 0xfffd, 0x00ff, 0x0000, 0xfff0, 0x001f, + 0x8000, 0xffff, 0x3fff, 0xfffc, 0xffff, 0x00ff, 0x0000, 0xffc0, 0x001f, + 0x0000, 0xfffe, 0x3fff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xff80, 0x001f, + 0x0000, 0xfffc, 0x7fff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xff00, 0x001f, + 0x0000, 0xfff8, 0xffff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xfe00, 0x001f, + 0x0000, 0xfff0, 0xffff, 0xfffe, 0xffff, 0x007f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xff80, 0xbfff, 0xffff, 0xffff, 0x01ff, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xff00, 0xdfff, 0xffff, 0xffff, 0x07ff, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xff00, 0xc7ff, 0xffff, 0xffff, 0x1fff, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xfe00, 0xc7ff, 0xffff, 0xffff, 0x7fff, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xfe00, 0xc3ff, 0xffff, 0xffff, 0xffff, 0x0000, 0xfe00, 0x003f, + 0x0000, 0xfe00, 0xe3ff, 0xffff, 0xffff, 0xffff, 0x0003, 0xfe00, 0x003f, + 0x0000, 0xfc00, 0xe3ff, 0xffff, 0xffff, 0xffff, 0x000f, 0xfe00, 0x003f, + 0x0000, 0xfc00, 0xe1ff, 0xffff, 0xffff, 0xffff, 0x003f, 0xff00, 0x003f, + 0x0000, 0xf800, 0xe1ff, 0xffff, 0xffff, 0xffff, 0x00ff, 0xff00, 0x003f, + 0x0000, 0xf000, 0xf1ff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xff00, 0x003f, + 0x0000, 0xc000, 0xf1ff, 0xffff, 0xffff, 0xffff, 0x07ff, 0xff00, 0x003f, + 0x0000, 0x0000, 0xf0fe, 0xffff, 0xffff, 0xffff, 0x1fff, 0xff00, 0x003f, + 0x0000, 0x0000, 0xf070, 0xffff, 0xffff, 0xffff, 0x7fff, 0xff00, 0x003f, + 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0xff01, 0x003f, + 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0xff07, 0x003f, + 0x0000, 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, 0xff1f, 0x003f, + 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0xffff, 0xffbf, 0x003f, + 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0x0f00, 0xfffc, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0x7f80, 0xffe0, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xff80, 0xff83, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xffc0, 0xfc1f, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xffc0, 0xe0ff, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xffc0, 0x07ff, 0xffff, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xffc0, 0x0fff, 0xfffc, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x0fff, 0xffe0, 0xffff, 0xffff, 0x003f, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x0fff, 0xff00, 0xffff, 0xffff, 0x001f, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x0fff, 0xf800, 0xffff, 0xffff, 0x001f, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x0fff, 0xe000, 0xffff, 0xffff, 0x001f, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x0fff, 0x0000, 0xffff, 0xffff, 0x001f, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x0fff, 0x0000, 0xfff8, 0xffff, 0x001f, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x07ff, 0x0000, 0xffc0, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x07ff, 0x0000, 0xff00, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x07ff, 0x0000, 0xf800, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x07ff, 0x0000, 0xc000, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0xfffc, 0x03ff, 0x0000, 0x0000, 0xfffe, 0x0003, + 0x0000, 0x0000, 0x0000, 0xfffc, 0x03ff, 0x0000, 0x0000, 0xfff8, 0x0001, + 0x0000, 0x0000, 0x0000, 0xfffc, 0x03ff, 0x0000, 0x0000, 0xffc0, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x1e00, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffc, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffe0, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfe00, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf800, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe000, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xc000, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fc0, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7fe0, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x0000, 0x00f0, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x0000, 0x01f8, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x0000, 0x01f8, + 0x8000, 0x07ff, 0x1ffe, 0x7ff8, 0xffe0, 0xffc1, 0xffe7, 0xfff0, 0x0ffc, + 0xe000, 0x8fff, 0x3fff, 0xfffe, 0xfff8, 0xffe3, 0xffef, 0xfff8, 0x1fff, + 0xf000, 0xdfff, 0x7fff, 0xffff, 0xfffd, 0xfff7, 0xffff, 0xfffc, 0x1fff, + 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xfffe, 0x1fff, + 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, + 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, + 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, + 0xf800, 0xff01, 0xfc07, 0xffff, 0xc07f, 0x01ff, 0xffff, 0xc07f, 0x01ff, + 0xf800, 0xff01, 0xfc07, 0xffff, 0xc07f, 0x01ff, 0xffff, 0xc07f, 0x01ff, + 0xf800, 0xffdf, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xf1ff, 0x0fff, + 0xf800, 0xffff, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1fff, + 0xf800, 0xffff, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1fff, + 0xf800, 0xffff, 0xffff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1fff, + 0xf000, 0xffff, 0x7fff, 0xffff, 0xc07f, 0xffff, 0xffff, 0xffff, 0x1ff7, + 0xe000, 0xffff, 0x3fff, 0xfffe, 0xc07f, 0xffff, 0xffef, 0xfffb, 0x1fe3, + 0xc000, 0xffff, 0x1fff, 0xfffc, 0x803c, 0xffff, 0xe3c7, 0xfff1, 0x0fc1, + 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01fc, 0x0000, 0x0000, 0x0000, + 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01fc, 0x0000, 0x0000, 0x0000, + 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01fc, 0x0000, 0x0000, 0x0000, + 0x0000, 0xe000, 0x0007, 0x0000, 0x0000, 0x01f8, 0x0000, 0x0000, 0x0000, + 0x0000, 0xc000, 0x0003, 0x0000, 0x0000, 0x00f0, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000 +}; -#define level_width 144 +#define level_width 144 #define level_height 129 static const unsigned short level_bits[] = { - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf800, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xe000, 0xffff, 0x1e01, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfe00, 0x7fef, 0x7ff8, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff00, 0x83c0, 0xffff, 0x0001, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x3f80, 0xcffe, 0xfffd, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x9fc0, 0xffff, 0xb07f, 0x001f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xcfe0, 0xffff, 0xcfce, 0x007e, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xe7f8, 0xffff, 0x9ff7, 0x00fd, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf7fc, 0xffe7, 0xbff7, 0x01ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf3fe, 0xfffb, 0xb9ff, 0x03ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfbff, 0xfe03, 0xb9f7, 0x07ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0xf9ff, 0xebfd, 0xb9e7, 0x0fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xc000, 0xffff, 0xd7fe, 0xcfdf, 0x0fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xe000, 0x7fff, 0xf7ff, 0xf037, 0x1fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xe000, 0x7fff, 0xf7ff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf000, 0xefff, 0xf73e, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf000, 0xefff, 0xf73e, 0xefff, 0x7fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf000, 0xefff, 0xf939, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf800, 0x87ff, 0xfe07, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf800, 0xffff, 0xdffe, 0xffff, 0xffdf, 0x0001, 0x0000, 0x0000, - 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xefff, 0x1fff, 0xfff8, 0x0003, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0x81ff, 0xfff1, 0x0007, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0xfc3f, 0xfff3, 0x000f, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0xff9f, 0xfff0, 0x000f, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0x3fcf, 0xfff6, 0x001f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0x0ee7, 0xfff7, 0x003f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xe003, 0xfff7, 0x003f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xf371, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xfff9, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xfffe, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0x7fff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0x7fff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xdfff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xdfff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xfdff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xbfff, 0xffff, 0xfdff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0x7fff, 0xfffb, 0xfdff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xfeff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xf3ff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xcfff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xbeff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xfeff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x7fff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xbfff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xbeff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xdeff, 0x007f, 0x0000, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xdeff, 0x003f, 0x0000, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xefff, 0x003f, 0x0000, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xfb7f, 0x003f, 0x0000, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffbf, 0x003f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffbf, 0x003f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffdf, 0x013f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffef, 0x003f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0x3fff, 0xfff9, 0x003f, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, - 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, - 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, - 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, - 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000 }; + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf800, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xe000, 0xffff, 0x1e01, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfe00, 0x7fef, 0x7ff8, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff00, 0x83c0, 0xffff, 0x0001, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x3f80, 0xcffe, 0xfffd, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x9fc0, 0xffff, 0xb07f, 0x001f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xcfe0, 0xffff, 0xcfce, 0x007e, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xe7f8, 0xffff, 0x9ff7, 0x00fd, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf7fc, 0xffe7, 0xbff7, 0x01ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf3fe, 0xfffb, 0xb9ff, 0x03ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfbff, 0xfe03, 0xb9f7, 0x07ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0xf9ff, 0xebfd, 0xb9e7, 0x0fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xc000, 0xffff, 0xd7fe, 0xcfdf, 0x0fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xe000, 0x7fff, 0xf7ff, 0xf037, 0x1fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xe000, 0x7fff, 0xf7ff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf000, 0xefff, 0xf73e, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf000, 0xefff, 0xf73e, 0xefff, 0x7fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf000, 0xefff, 0xf939, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf800, 0x87ff, 0xfe07, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf800, 0xffff, 0xdffe, 0xffff, 0xffdf, 0x0001, 0x0000, 0x0000, + 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xefff, 0x1fff, 0xfff8, 0x0003, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0x81ff, 0xfff1, 0x0007, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0xfc3f, 0xfff3, 0x000f, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0xff9f, 0xfff0, 0x000f, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0x3fcf, 0xfff6, 0x001f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0x0ee7, 0xfff7, 0x003f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xe003, 0xfff7, 0x003f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xf371, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xfff9, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xfffe, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0x7fff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0x7fff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xdfff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xdfff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xfdff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xbfff, 0xffff, 0xfdff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0x7fff, 0xfffb, 0xfdff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xfeff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xf3ff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xcfff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xbeff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xfeff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x7fff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xbfff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xbeff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xdeff, 0x007f, 0x0000, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xdeff, 0x003f, 0x0000, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xefff, 0x003f, 0x0000, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xfb7f, 0x003f, 0x0000, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffbf, 0x003f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffbf, 0x003f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffdf, 0x013f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffef, 0x003f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0x3fff, 0xfff9, 0x003f, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, + 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, + 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, + 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, + 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000 +}; -#define level_mask_width 144 +#define level_mask_width 144 #define level_mask_height 129 static const unsigned short level_mask_bits[] = { - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7c00, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0200, 0xfe00, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0740, 0xdc00, 0x0001, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x03fe, 0xce00, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x01ff, 0xde1f, 0x0001, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x802e, 0xfe1f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x4000, 0x0044, 0xc000, 0xc007, 0x7c1d, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfee8, 0x00ff, 0x8000, 0xc00f, 0x0e38, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffff, 0x17ff, 0x0000, 0xc3df, 0x1c3f, 0x0000, - 0x0000, 0x0000, 0xa000, 0xffff, 0x3fff, 0x0000, 0xe3ff, 0x0e3f, 0x0000, - 0x0000, 0x0000, 0xff00, 0xffff, 0x7fff, 0x0000, 0xc07f, 0x1c7d, 0x0000, - 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0000, 0xe03e, 0x0e30, 0x0000, - 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0x0017, 0xc03e, 0x1c71, 0x0000, - 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0x001f, 0x8038, 0x0020, 0x0000, - 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0x007f, 0x003c, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0x00ff, 0x0020, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, - 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0150, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x03f0, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x1ff8, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x1ef8, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x147c, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0038, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x707c, 0x0054, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0xf0fc, 0x00fc, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xf1fc, 0x01fc, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xf80e, 0x00ee, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xf00e, 0x00fc, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0xf808, 0x00fe, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x7800, 0x007e, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xe000, 0x000e, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x4000, 0x0006, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x000e, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0004, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0020, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0054, 0x007c, - 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x00f8, 0x00f8, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x117c, 0x01fc, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x3818, 0x00fc, - 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x7c5c, 0x00fc, - 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0xfef8, 0x000c, - 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x7ff8, 0x001c, - 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0xfe38, 0x000c, - 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x7e30, 0x001c, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0xee30, 0x000c, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x5e70, 0x001c, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0e20, 0x000c, - 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0c60, 0x000c, - 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0800, 0x0000, - 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x3c00, 0xfffe, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, - 0xd000, 0x7e41, 0xfffc, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, - 0xf800, 0xeee3, 0xfff8, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, - 0xff00, 0x6cfd, 0xfff0, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, 0x0000, - 0x3f80, 0xecf8, 0xff80, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, - 0x3f00, 0x7fdc, 0xff00, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, - 0xbe00, 0x3ffb, 0xa000, 0xbfbb, 0x2bbf, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfe00, 0x1ffb, 0x0000, 0x1f00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfc00, 0x1fbb, 0x0000, 0x3e00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x7c00, 0x1d39, 0x0000, 0x7c00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x3800, 0x1838, 0x0000, 0xf800, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x7000, 0x0018, 0x0000, 0xfc00, 0x0001, 0x07c0, 0x0000, 0x0000, 0x0000, - 0x6000, 0x0000, 0x0000, 0xf800, 0x0003, 0x0e80, 0x0000, 0x0000, 0x0000, - 0x7000, 0x0000, 0x0100, 0xfc00, 0x0007, 0x1d00, 0x0000, 0x0000, 0x0000, - 0x6000, 0x0000, 0x0080, 0x7800, 0x000e, 0x3800, 0x0000, 0x0000, 0x0000, - 0x4000, 0x0000, 0x00c0, 0xfc00, 0x001c, 0x7000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x00e0, 0xc800, 0x0038, 0xe000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x1060, 0xdc00, 0x0071, 0x4000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x3860, 0x8800, 0x00e0, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x1c60, 0x9c00, 0x01c1, 0x1c00, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0860, 0x8800, 0x0383, 0x3800, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x9c40, 0x1c01, 0x1f03, 0x7100, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x8e00, 0x1801, 0x1a03, 0x6300, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xcc00, 0x1c00, 0x3c07, 0x4700, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xec00, 0x0800, 0x2002, 0xc600, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x7000, 0x1c00, 0x3007, 0xc600, 0x0001, 0x0000, 0x0000, - 0x0000, 0x0000, 0x6000, 0x0c00, 0x3002, 0x8c00, 0x0201, 0x0000, 0x0000, - 0x0000, 0x0000, 0x6000, 0x0c00, 0x3006, 0x9c00, 0x0701, 0x0000, 0x0000, - 0x0000, 0x0000, 0xe000, 0x0e00, 0x2002, 0x1800, 0x0600, 0x0000, 0x0000, - 0x0000, 0x0000, 0x4000, 0x0700, 0x3007, 0x3800, 0x0700, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0300, 0x3802, 0x3800, 0x0600, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0780, 0x1806, 0x1000, 0x0400, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0380, 0x0806, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x01c0, 0x1c06, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0180, 0x0806, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x05c0, 0x1c06, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0f80, 0x080e, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x1f00, 0x0406, 0x0000, 0x0000, 0x0000, 0x0000 }; + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7c00, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0200, 0xfe00, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0740, 0xdc00, 0x0001, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x03fe, 0xce00, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x01ff, 0xde1f, 0x0001, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x802e, 0xfe1f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x4000, 0x0044, 0xc000, 0xc007, 0x7c1d, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfee8, 0x00ff, 0x8000, 0xc00f, 0x0e38, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffff, 0x17ff, 0x0000, 0xc3df, 0x1c3f, 0x0000, + 0x0000, 0x0000, 0xa000, 0xffff, 0x3fff, 0x0000, 0xe3ff, 0x0e3f, 0x0000, + 0x0000, 0x0000, 0xff00, 0xffff, 0x7fff, 0x0000, 0xc07f, 0x1c7d, 0x0000, + 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x0000, 0xe03e, 0x0e30, 0x0000, + 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, 0x0017, 0xc03e, 0x1c71, 0x0000, + 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0x001f, 0x8038, 0x0020, 0x0000, + 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0x007f, 0x003c, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0x00ff, 0x0020, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, + 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0150, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x03f0, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x1ff8, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x1ef8, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x147c, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0038, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x707c, 0x0054, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0xf0fc, 0x00fc, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xf1fc, 0x01fc, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xf80e, 0x00ee, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xf00e, 0x00fc, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0xf808, 0x00fe, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x7800, 0x007e, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0xe000, 0x000e, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x4000, 0x0006, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x000e, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0004, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0020, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0054, 0x007c, + 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x00f8, 0x00f8, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x117c, 0x01fc, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x3818, 0x00fc, + 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x7c5c, 0x00fc, + 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0xfef8, 0x000c, + 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x7ff8, 0x001c, + 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0xfe38, 0x000c, + 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x7e30, 0x001c, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0xee30, 0x000c, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, 0x5e70, 0x001c, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0e20, 0x000c, + 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0c60, 0x000c, + 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0800, 0x0000, + 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x3c00, 0xfffe, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, + 0xd000, 0x7e41, 0xfffc, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, + 0xf800, 0xeee3, 0xfff8, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, + 0xff00, 0x6cfd, 0xfff0, 0xffff, 0xffff, 0x001f, 0x0000, 0x0000, 0x0000, + 0x3f80, 0xecf8, 0xff80, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, + 0x3f00, 0x7fdc, 0xff00, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, + 0xbe00, 0x3ffb, 0xa000, 0xbfbb, 0x2bbf, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfe00, 0x1ffb, 0x0000, 0x1f00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfc00, 0x1fbb, 0x0000, 0x3e00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x7c00, 0x1d39, 0x0000, 0x7c00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x3800, 0x1838, 0x0000, 0xf800, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x7000, 0x0018, 0x0000, 0xfc00, 0x0001, 0x07c0, 0x0000, 0x0000, 0x0000, + 0x6000, 0x0000, 0x0000, 0xf800, 0x0003, 0x0e80, 0x0000, 0x0000, 0x0000, + 0x7000, 0x0000, 0x0100, 0xfc00, 0x0007, 0x1d00, 0x0000, 0x0000, 0x0000, + 0x6000, 0x0000, 0x0080, 0x7800, 0x000e, 0x3800, 0x0000, 0x0000, 0x0000, + 0x4000, 0x0000, 0x00c0, 0xfc00, 0x001c, 0x7000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x00e0, 0xc800, 0x0038, 0xe000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x1060, 0xdc00, 0x0071, 0x4000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x3860, 0x8800, 0x00e0, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x1c60, 0x9c00, 0x01c1, 0x1c00, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0860, 0x8800, 0x0383, 0x3800, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x9c40, 0x1c01, 0x1f03, 0x7100, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x8e00, 0x1801, 0x1a03, 0x6300, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xcc00, 0x1c00, 0x3c07, 0x4700, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xec00, 0x0800, 0x2002, 0xc600, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x7000, 0x1c00, 0x3007, 0xc600, 0x0001, 0x0000, 0x0000, + 0x0000, 0x0000, 0x6000, 0x0c00, 0x3002, 0x8c00, 0x0201, 0x0000, 0x0000, + 0x0000, 0x0000, 0x6000, 0x0c00, 0x3006, 0x9c00, 0x0701, 0x0000, 0x0000, + 0x0000, 0x0000, 0xe000, 0x0e00, 0x2002, 0x1800, 0x0600, 0x0000, 0x0000, + 0x0000, 0x0000, 0x4000, 0x0700, 0x3007, 0x3800, 0x0700, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0300, 0x3802, 0x3800, 0x0600, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0780, 0x1806, 0x1000, 0x0400, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0380, 0x0806, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x01c0, 0x1c06, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0180, 0x0806, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x05c0, 0x1c06, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0f80, 0x080e, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x1f00, 0x0406, 0x0000, 0x0000, 0x0000, 0x0000 +}; -#define llama_width 240 +#define llama_width 240 #define llama_height 260 static const unsigned short llama_bits[] = { - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8400, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0800, 0x0000, 0x0004, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0680, 0xf000, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0600, 0xe000, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0700, 0xe000, 0x021f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x1780, 0xc000, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x07c0, 0x8000, 0x21ff, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x07c0, 0x8000, 0x07ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0fe0, 0x0000, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x07e4, 0x0000, 0x3ffe, - 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0ff0, 0x0000, 0x7ffc, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x07f0, 0x0000, 0xfff8, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2ff0, 0x0000, 0xfff1, - 0x0023, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0ff8, 0x0000, 0xfff0, 0x0007, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0ff8, 0x0000, 0xffe0, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0ff8, 0x0000, 0xffc0, - 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0ff8, 0x0000, 0xff80, 0x003f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0ffc, 0x0000, 0xff00, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0ffc, 0x0000, 0xfe00, - 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x4ff8, 0x0000, 0xfc00, 0x01ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0dfc, 0x0000, 0xfc80, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1bf8, 0x0000, 0xf800, - 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x93f8, 0x0000, 0xf000, 0x03ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x17f8, 0x0000, 0xe000, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0ff0, 0x0001, 0xc010, - 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x1ff0, 0x0000, 0xc000, 0x0fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x3fe0, 0x0800, 0x8000, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7fc4, 0x2082, 0x0040, - 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff88, 0x0c00, 0x0000, 0x0fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff00, 0x7103, 0x0000, 0x0ffe, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xe003, 0x0028, - 0x07fc, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x7800, 0x81f8, 0x0087, 0x07f8, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x3000, 0x9f00, 0x400f, 0x07f8, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x7e00, 0x004e, - 0x23fa, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfbfe, 0x005f, 0x03f6, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x8400, 0xf7ff, 0x003f, 0x03f6, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x80ff, - 0x11e6, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf080, 0xffff, 0x80ff, 0x01e6, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfc00, 0xffff, 0x01ff, 0x08ee, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x01ff, - 0x00ee, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x03ff, 0x056e, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0x07ff, 0x00ae, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xdfc0, 0xffff, 0x0fff, - 0x008e, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe1e0, 0xffff, 0x4ffe, 0x00ce, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf0e0, 0x07ff, 0x9ff0, 0x016e, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf874, 0x40ff, 0x3fe1, - 0x0066, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x3d30, 0x3f20, 0x7f8c, 0x0027, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0c08, 0x0f82, 0x6f90, 0x00b7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2208, 0x01c0, 0x1e40, - 0x0017, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0002, 0x00a8, 0xbe80, 0x0017, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0404, 0x0020, 0x7900, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe018, 0xf20f, - 0x000b, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf01c, 0xf63b, 0x000e, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x4002, 0x3c1e, 0xcc84, 0x0125, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2800, 0xdf1f, 0xd93f, - 0x0073, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xc000, 0xe7df, 0xb67f, 0x00f7, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xc000, 0xf3ff, 0x3cff, 0x04f7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe500, 0xfbff, 0x7dff, - 0x01f7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf200, 0xfdff, 0xfbff, 0x13fe, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf940, 0xfeff, 0xf7ff, 0x03fe, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc80, 0xff7f, 0xffff, - 0x27fd, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfe10, 0xff7f, 0xefff, 0x0ffb, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfe00, 0x3fbf, 0xeff8, 0x0ffb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0x01ff, 0xfe00, - 0x9fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0x007f, 0xd808, 0x1ff7, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xffc0, 0x0c07, 0xe7dc, 0x3ff7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe4, 0x0f0f, 0xdfc0, - 0x7fef, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff0, 0x1f7f, 0xfff0, 0x7fef, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfff0, 0x7fff, 0xfffc, 0xffef, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff9, 0xffff, 0xffff, - 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffc, 0xfeff, 0xffff, 0xffff, 0x0001, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfffc, 0xfdff, 0xffff, 0xffdf, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, - 0xffdf, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x2000, 0xffff, 0xfbff, 0xdfff, 0xffdf, 0x0003, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xffff, 0xfbff, 0xdfff, 0xfddf, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0xe7ff, 0xefff, - 0xffdf, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x8000, 0xffff, 0xdfff, 0xf7ff, 0xff9f, 0x004f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, - 0xffff, 0xbfff, 0xf1ff, 0xff9f, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0x6fff, 0xfc7c, - 0xff9f, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xfe01, 0xfb9f, 0x001f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe200, - 0xffff, 0x9fff, 0xffff, 0xfb9f, 0x011f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x7fff, 0xffff, - 0xfb9f, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf100, 0xffff, 0xffff, 0xfff8, 0xfbdf, 0x003f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, - 0xfbff, 0xffff, 0xfe03, 0xf7df, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xfe7f, 0xfff9, 0xffff, - 0xf7cf, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf800, 0xffbf, 0xfffb, 0xffff, 0xf7c7, 0x007f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, - 0xffcf, 0xffff, 0xffff, 0xf7e3, 0x04ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xfff7, 0xfff7, 0x3fff, - 0xf7e0, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xe200, 0xfffb, 0xfff7, 0x07ff, 0xf7f8, 0x01ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, - 0xfffc, 0xfff7, 0xc07f, 0xf7ff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xfffe, 0xfff7, 0xf81f, - 0xf7ff, 0x09ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x2400, 0xffff, 0xfffb, 0xff07, 0xe7ff, 0x01ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7000, - 0xffff, 0xfff9, 0xfff0, 0xf7ff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xb800, 0xffff, 0x7ff8, 0xfffc, - 0xe7ff, 0x13ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x9c00, 0x3fff, 0x9ffe, 0xffff, 0xf7ff, 0x07ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xdc00, - 0x1fff, 0xe7ff, 0xffff, 0xe7ff, 0x05ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xef00, 0xc1ff, 0xf9ff, 0xffff, - 0xf7ff, 0x05ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xef80, 0xf01f, 0xfcff, 0xffff, 0xf1ff, 0x03ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x37c0, - 0xfc00, 0xff5f, 0xffff, 0xf1ff, 0x0bff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffaf, 0xffff, - 0xf1ff, 0x0bfb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff80, 0xffff, 0xffd7, 0xffff, 0xf3ff, 0x1ffb, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8800, 0xffff, - 0xffff, 0x0077, 0xfff8, 0xf2ff, 0x17fb, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xf03b, 0xffef, - 0xf2ff, 0x97fb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8800, 0xffff, 0xffff, 0xfc01, 0xffff, 0xf2ff, 0x27fb, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, - 0xffff, 0xe001, 0xffff, 0xf97f, 0x2ff3, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xfffe, 0xffff, 0xc380, 0xffff, - 0xf97f, 0x0ff7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfff8, 0xffff, 0x3fa0, 0xffff, 0xfc7f, 0x4ff7, 0x0001, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, - 0x7fff, 0x7f80, 0xfffe, 0xfc3f, 0x0ff7, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc4, 0x3fff, 0xffc1, 0xfffd, - 0xfcbf, 0x1ff7, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff80, 0x3fff, 0xffc0, 0xffff, 0xfe9f, 0x1ff7, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff10, - 0xbfff, 0xffe0, 0xffff, 0xfe9f, 0x1ff7, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0x1fff, 0xfff0, 0xffff, - 0xffdf, 0x3ffb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf800, 0x4fff, 0xfff8, 0xffff, 0xffcf, 0x3fef, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe100, - 0x07ff, 0xfff8, 0xffff, 0xffcf, 0x3fe9, 0x0001, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x87ff, 0xffdc, 0xffff, - 0xffe7, 0x3fef, 0x0008, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x03ff, 0xfffe, 0xffff, 0xffe7, 0x3fe9, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x00f8, 0xfff7, 0xffff, 0xffe7, 0x7fe9, 0x0002, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2000, 0xfff7, 0xffff, - 0xffe7, 0x7fe9, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x8000, 0xfff9, 0xffff, 0xfff3, 0xffe9, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xc002, 0xfff8, 0xffff, 0xfff3, 0xffe9, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4008, 0xfffc, 0xffff, - 0xfff1, 0xffc9, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xfff9, 0xffc9, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffff, 0xffff, 0xfff8, 0xffc9, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0xfffe, 0xffff, - 0xfff8, 0x3fd9, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x4000, 0xfffe, 0xffff, 0xfffc, 0x07d9, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xa000, 0xffff, 0x7fff, 0xfffc, 0xf0d9, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xd000, 0xffff, 0x7fff, - 0xfffc, 0xf859, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xc200, 0xffff, 0x7fff, 0xfffe, 0x7e19, 0x0008, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf000, 0xffff, 0x3fff, 0xfffe, 0x1f81, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x3fff, - 0xfffe, 0x07d1, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x3fff, 0xffff, 0x01f9, 0x0000, - 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x7e80, 0xfffe, 0x3fff, 0xffff, 0x0078, 0x8000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3c00, 0xffff, 0x3fff, - 0x7fff, 0x0008, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xbe00, 0xffff, 0x9fff, 0x1fff, 0x0000, 0x8000, - 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x9e00, 0xffff, 0x9fff, 0x07ff, 0x0200, 0x2000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xce00, 0xffff, 0x9fff, - 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xce00, 0xffff, 0xcfff, 0x001f, 0x0002, 0x4000, - 0x8010, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xc600, 0xffff, 0x0fff, 0x0000, 0x0000, 0x4000, 0x1fc0, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xa200, 0xffff, 0x0fff, - 0x0000, 0x0000, 0x4000, 0x79f0, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xa000, 0xffff, 0x0fff, 0x0080, 0x0000, 0x6800, - 0x8ff0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xe000, 0xffff, 0x0fff, 0x0000, 0x0000, 0x6000, 0xfff2, 0x0004, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xd400, 0xffff, 0x0fff, - 0x0000, 0x0000, 0x6000, 0xfff9, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xd000, 0xfffb, 0x0fff, 0x0000, 0x0000, 0x6000, - 0xfbf8, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xc200, 0xfffd, 0x07ff, 0x0000, 0x0000, 0x6000, 0x81fc, 0x001f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xfffd, 0x07ff, - 0x0000, 0x0000, 0x6000, 0x007c, 0x001c, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xc000, 0xfffd, 0x17ff, 0x0000, 0x0000, 0x6000, - 0x023c, 0x0060, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x8000, 0xfffd, 0x17ff, 0x0000, 0x0000, 0xe000, 0x003e, 0x0084, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x07ff, - 0x0000, 0x0000, 0xc000, 0x001e, 0x0040, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x17ff, 0x0000, 0x0000, 0xc000, - 0x200e, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xe000, 0xfffe, 0x0fff, 0x0000, 0x0000, 0xc000, 0x000f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x0fff, - 0x0000, 0x0000, 0xc000, 0x07e7, 0x0801, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x0fff, 0x0000, 0x0000, 0xc000, - 0x1ff7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x6000, 0xfffa, 0x0fff, 0x0000, 0x0000, 0xc000, 0x7e3b, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xfffa, 0x5fff, - 0x0000, 0x0000, 0x8000, 0xf00f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfffa, 0x0fff, 0x0000, 0x0000, 0x8000, - 0xc007, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfffa, 0x1fff, 0x0000, 0x0000, 0x9000, 0x1003, 0x0013, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1000, 0xfffa, 0x1fff, - 0x0000, 0x0000, 0x8000, 0x000b, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, 0x1fff, 0x0000, 0x0000, 0x8000, - 0x0001, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x8000, 0xfff0, 0x9fff, 0x0000, 0x0000, 0xa000, 0x0001, 0x002a, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x1fff, - 0x0000, 0x0000, 0x0000, 0x0005, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x1fff, 0x0000, 0x0000, 0x0000, - 0x0005, 0x0010, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffe0, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0001, 0x0040, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0x1fff, - 0x0000, 0x0000, 0x4000, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffc4, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffc0, 0x3fff, 0x0001, 0x0000, 0x0000, 0x0003, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe88, 0x3fff, - 0x0000, 0x0000, 0x0000, 0x0100, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfd80, 0x3fff, 0x0000, 0x0000, 0x0000, - 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfb00, 0x3fff, 0x0000, 0x0000, 0x8000, 0x41ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfa00, 0x3fff, - 0x0000, 0x0000, 0xc800, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0x7fff, 0x0000, 0x0000, 0xe000, - 0x1fff, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xe000, 0x7fff, 0x0004, 0x0000, 0xf000, 0x7fff, 0x0010, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe080, 0xffff, - 0x0000, 0x0000, 0xf000, 0xffff, 0x0040, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xc900, 0xffff, 0x2000, 0x0000, 0xf000, - 0xffff, 0x0083, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8800, 0xffff, 0x0001, 0x0000, 0xf800, 0xffff, 0x0007, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, - 0x0001, 0x0002, 0xfc80, 0xffff, 0x041f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x2000, 0xffff, 0x0003, 0x0004, 0xfc00, - 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x7200, 0xffff, 0x1f87, 0x0000, 0xf440, 0xffff, 0x107f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7000, 0xfffe, - 0x3fc7, 0x0020, 0xf800, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x7800, 0xfffc, 0xffe3, 0x0001, 0xfc00, - 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x7c00, 0xfffc, 0xffe3, 0x0003, 0xfe00, 0xffff, 0x03ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7c00, 0xfffc, - 0xffe3, 0x000f, 0xff08, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x7e00, 0xfffa, 0xfff1, 0x083f, 0xffc0, - 0xffff, 0x07ff, 0x0010, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x7f00, 0xfffa, 0xfff9, 0x007f, 0xfff0, 0xffff, 0x07ff, 0x0040, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7f84, 0xfff2, - 0xfff8, 0x01ff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x7fc0, 0xfff6, 0xfffc, 0xc7ff, 0xffff, - 0xffff, 0x0fff, 0x0060, 0x0000, 0x2000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x7fe0, 0xfffe, 0xfffc, 0x1fff, 0xffff, 0xffff, 0x0fff, 0x00e0, - 0x0000, 0x0100, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7ff0, 0x7fee, - 0xfffe, 0xffff, 0xfffc, 0xffff, 0x0fff, 0x07c0, 0x0000, 0x0000, 0x0020, - 0x0000, 0x0000, 0x0000, 0x0000, 0x7ff8, 0x3fee, 0xfffe, 0xffff, 0xfff3, - 0xffff, 0x9fff, 0x07c0, 0x0000, 0xf020, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x7ffc, 0x3fde, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, 0x3fc1, - 0x0002, 0xff00, 0x0021, 0x0000, 0x0000, 0x0000, 0x0000, 0x7ffe, 0x3fff, - 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0x7ec0, 0x0000, 0x3f88, 0x0084, - 0x0000, 0x0000, 0x0000, 0x8000, 0x3fff, 0x9fff, 0xffff, 0xffff, 0xffff, - 0xffff, 0x7fff, 0x8006, 0x0001, 0x6080, 0x003e, 0x0000, 0x0000, 0x0000, - 0xc080, 0xbfff, 0x9fff, 0xffff, 0xffff, 0xffff, 0xbfff, 0x7fff, 0x00fe, - 0x0003, 0xbff0, 0x003f, 0x0000, 0x0000, 0x0000, 0xf000, 0x9fff, 0xdffd, - 0xffff, 0xffff, 0xffff, 0xe7ff, 0xffff, 0x1ffc, 0x0004, 0xdff8, 0x001f, - 0x0000, 0x0000, 0x0000, 0xfc00, 0x9fff, 0xcffd, 0xffff, 0xffff, 0xffff, - 0xf9ff, 0xffff, 0xfff9, 0x1019, 0xe7f8, 0x004f, 0x0000, 0x0000, 0x0000, - 0xfe04, 0xdfff, 0xeffe, 0xffff, 0xffff, 0xffff, 0xfcff, 0xffff, 0x0fd3, - 0x0070, 0xf7fc, 0x002f, 0x0000, 0x0000, 0x0000, 0xff80, 0x4fff, 0xf7fe, - 0xffff, 0xffff, 0xffff, 0xfe3f, 0xffff, 0xffe7, 0xf0fc, 0xfbfd, 0x0033, - 0x0000, 0x0000, 0x4000, 0xffe0, 0x6fff, 0xf7fe, 0xffff, 0xffff, 0xffff, - 0xfd9f, 0xffff, 0xff9f, 0xfffb, 0xfbfd, 0x003b, 0x0000, 0x0000, 0x0000, - 0xfff8, 0x27ff, 0xf3ff, 0xffff, 0xffff, 0xffff, 0xf9cf, 0xffff, 0xffbf, - 0xffe3, 0xfbfe, 0x001c, 0x0000, 0x0000, 0x0140, 0xffff, 0x13ff, 0xf9ff, - 0xffff, 0xffff, 0xffff, 0xe7e3, 0xffff, 0xffff, 0xffcf, 0x7bfe, 0x009f, - 0x0000, 0x0000, 0xe000, 0xffff, 0x8bff, 0xfdff, 0xffff, 0xffff, 0xffff, - 0xc7f1, 0xffff, 0xffff, 0x7f1f, 0x97fe, 0x000f, 0x0000, 0x8000, 0xff78, - 0xffff, 0x91ff, 0xfeff, 0xffff, 0xffff, 0xffff, 0xcff8, 0xffff, 0xffff, - 0xf87f, 0x30c3, 0x000f, 0x0000, 0x0000, 0xc1fc, 0xffff, 0xd9ff, 0xfe03, - 0xffff, 0xffff, 0x7fff, 0x9ff8, 0xffff, 0xffff, 0xe07f, 0xffbf, 0x0006, - 0x0000, 0x0000, 0x007e, 0xffff, 0xccff, 0xfc00, 0xffff, 0xffff, 0x7fff, - 0x9ffc, 0xffff, 0xffff, 0xc1ff, 0xffff, 0x0001, 0x0000, 0x9000, 0x3e1f, - 0xfffc, 0x07ff, 0xf0f8, 0xffff, 0xfff7, 0x3fff, 0x3ffe, 0xffff, 0xffff, - 0x01ff, 0xfffc, 0x0000, 0x0000, 0x8000, 0xff8f, 0xfff8, 0x07ff, 0xe1ff, - 0xffff, 0xffff, 0x1fff, 0x3ffe, 0xffff, 0xffff, 0x03ff, 0x7ff0, 0x0000, - 0x0000, 0xc000, 0xffe3, 0xffe3, 0xe07f, 0xc7ff, 0xffff, 0xfffb, 0x0fff, - 0x3dfb, 0xffff, 0xffff, 0x8fdf, 0x0000, 0x0000, 0x0000, 0xe000, 0xfff3, - 0xffc7, 0xf80f, 0x8fff, 0xffff, 0xfffb, 0x8fff, 0x33cf, 0xfffe, 0xffff, - 0x8fbf, 0x0000, 0x0004, 0x0000, 0xf000, 0xeff9, 0x0b1f, 0xffc0, 0x3fff, - 0xffff, 0xfffb, 0x07ff, 0x081c, 0xfffe, 0xffff, 0x18ff, 0x0001, 0x0001, - 0x0000, 0xf900, 0x9ffe, 0x007f, 0xfffe, 0x7fff, 0xfffe, 0xfffb, 0x07ff, - 0x0030, 0xfffe, 0xffff, 0x03ff, 0x2080, 0x0000, 0x0000, 0x7c80, 0x3ffe, - 0xfcff, 0xffff, 0x7fff, 0xfffe, 0xfffb, 0x03ff, 0x0001, 0xfffe, 0xffff, - 0x0f81, 0x0001, 0x0000, 0x0000, 0x7c00, 0x3801, 0xf3fc, 0xffff, 0xffff, - 0xfffc, 0xfffb, 0x03ff, 0x4020, 0xfffe, 0xffff, 0xe03f, 0x0002, 0x0000, - 0x0000, 0x3e00, 0x03f8, 0xcff9, 0xf77f, 0xffff, 0xfff9, 0xfffb, 0x11ff, - 0x0000, 0xffdf, 0xfffc, 0x00ff, 0x0002, 0x0000, 0x0000, 0x8f00, 0xbfff, - 0x3fe3, 0xfb7c, 0x3fff, 0xfffa, 0xfffb, 0x01ff, 0x4000, 0xf07e, 0x0003, - 0x1000, 0x0001, 0x0000, 0x0000, 0xc380, 0xffff, 0xffcf, 0xff82, 0xbfe5, - 0xfff1, 0xfff3, 0x00ff, 0x4000, 0x00fc, 0x0000, 0x0400, 0x0000, 0x0000, - 0x0000, 0xf3c0, 0xffff, 0xff8f, 0xfd9f, 0xbfde, 0xfff7, 0xfffb, 0x08ff, - 0x8000, 0x43e0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfce0, 0xffff, - 0xff00, 0xbcbf, 0xdfbf, 0xffc7, 0xfff3, 0x00ff, 0x0000, 0x0000, 0xa0d0, - 0x0006, 0x0000, 0x0000, 0x0000, 0xfe70, 0x1fff, 0xfe7c, 0xbc3f, 0xdfff, - 0x7fcf, 0xfff3, 0x047f, 0x0000, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xff3c, 0xc0ff, 0xf8fe, 0xddff, 0xde7f, 0x7f1f, 0xfff3, 0x003f, - 0x0000, 0x0480, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xff9e, 0xf81f, - 0xf1fe, 0xec07, 0xdeff, 0x7f3f, 0xfff3, 0x003f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x7fcd, 0x0ec0, 0x47fe, 0xe5e0, 0xdeff, - 0xfe3f, 0xfff2, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xc000, 0x07ee, 0x77f8, 0x0ffe, 0xf5fc, 0xddff, 0xfe7f, 0xfff6, 0x003f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x6000, 0x0007, 0x79ff, - 0x0ffe, 0xf0ff, 0xddff, 0xfcff, 0xfff4, 0x001f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xbe17, 0x7f7f, 0xe7ff, 0xf8ff, 0xd9ff, - 0xfcff, 0xfff4, 0x011f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xc000, 0xdf3b, 0x7f7f, 0x11ff, 0xf8fe, 0xdbff, 0xf8ff, 0xffe4, 0x001f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe200, 0xefbf, 0x3fbf, - 0xb1ff, 0xfcff, 0xd3ff, 0xf9ff, 0xffed, 0x001f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf800, 0xf79f, 0xbfdf, 0xcfff, 0xfcff, 0xd7ff, - 0xf1ff, 0xffe9, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfc40, 0xf79f, 0xdfdf, 0xe3ff, 0xfcff, 0xd7ff, 0xf3ff, 0xffe9, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff90, 0xfb9f, 0xcfe7, - 0xf800, 0xfeff, 0xd7ff, 0xf3ff, 0xffc3, 0x000f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfc00, 0xfd85, 0x07f3, 0xfc00, 0xfc7f, 0xafff, - 0xe3ff, 0xffd3, 0x0083, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0010, 0xfd80, 0x03f9, 0xde00, 0xfc7f, 0xafff, 0xe7ff, 0x7fc3, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0080, 0x7e80, 0x00fc, - 0xe700, 0xfc7f, 0xafff, 0xe7ff, 0x07c3, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x2000, 0x1e80, 0x203e, 0xf1c0, 0xfc7f, 0x8fff, - 0xe7ff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0300, 0x001e, 0xfc60, 0xf87f, 0x8fff, 0xe7ff, 0x0003, 0x0010, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0100, - 0xfe08, 0xf8ff, 0x4fff, 0xcfff, 0xc003, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe4, 0xe0ff, 0x4fff, - 0xcfff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff2, 0xc1ff, 0x4fff, 0xcfff, 0x0187, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0040, 0x0000, - 0xfffc, 0x83ff, 0x0fff, 0xcfff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xfffc, 0x0fff, 0x1ffe, - 0xcffe, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x1fff, 0x0ffc, 0xc7f8, 0x0007, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, - 0xf80f, 0x7fff, 0xcfe0, 0xe381, 0x004f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xffc0, 0xffff, 0xcf80, - 0xe007, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff00, 0xf7ff, 0xe007, 0xc00f, 0x0007, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0200, - 0xc000, 0xfc2f, 0xe01f, 0x303f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xf83d, - 0x003f, 0x0100, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0020, 0x8000, 0xff81, 0x0201, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x1fe0, 0x2040, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0001, - 0x0008, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x2000, 0x0000, 0x0002, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x4000, 0x2000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0202, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000 }; + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8400, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0800, 0x0000, 0x0004, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0680, 0xf000, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0600, 0xe000, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0700, 0xe000, 0x021f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x1780, 0xc000, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x07c0, 0x8000, 0x21ff, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x07c0, 0x8000, 0x07ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0fe0, 0x0000, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x07e4, 0x0000, 0x3ffe, + 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0ff0, 0x0000, 0x7ffc, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x07f0, 0x0000, 0xfff8, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2ff0, 0x0000, 0xfff1, + 0x0023, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0ff8, 0x0000, 0xfff0, 0x0007, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0ff8, 0x0000, 0xffe0, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0ff8, 0x0000, 0xffc0, + 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0ff8, 0x0000, 0xff80, 0x003f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0ffc, 0x0000, 0xff00, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0ffc, 0x0000, 0xfe00, + 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x4ff8, 0x0000, 0xfc00, 0x01ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0dfc, 0x0000, 0xfc80, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1bf8, 0x0000, 0xf800, + 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x93f8, 0x0000, 0xf000, 0x03ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x17f8, 0x0000, 0xe000, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0ff0, 0x0001, 0xc010, + 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x1ff0, 0x0000, 0xc000, 0x0fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x3fe0, 0x0800, 0x8000, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7fc4, 0x2082, 0x0040, + 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff88, 0x0c00, 0x0000, 0x0fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff00, 0x7103, 0x0000, 0x0ffe, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xe003, 0x0028, + 0x07fc, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x7800, 0x81f8, 0x0087, 0x07f8, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x3000, 0x9f00, 0x400f, 0x07f8, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0x7e00, 0x004e, + 0x23fa, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfbfe, 0x005f, 0x03f6, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x8400, 0xf7ff, 0x003f, 0x03f6, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x80ff, + 0x11e6, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf080, 0xffff, 0x80ff, 0x01e6, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfc00, 0xffff, 0x01ff, 0x08ee, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x01ff, + 0x00ee, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x03ff, 0x056e, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0x07ff, 0x00ae, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xdfc0, 0xffff, 0x0fff, + 0x008e, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe1e0, 0xffff, 0x4ffe, 0x00ce, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf0e0, 0x07ff, 0x9ff0, 0x016e, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf874, 0x40ff, 0x3fe1, + 0x0066, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x3d30, 0x3f20, 0x7f8c, 0x0027, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0c08, 0x0f82, 0x6f90, 0x00b7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2208, 0x01c0, 0x1e40, + 0x0017, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0002, 0x00a8, 0xbe80, 0x0017, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0404, 0x0020, 0x7900, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe018, 0xf20f, + 0x000b, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf01c, 0xf63b, 0x000e, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x4002, 0x3c1e, 0xcc84, 0x0125, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2800, 0xdf1f, 0xd93f, + 0x0073, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xc000, 0xe7df, 0xb67f, 0x00f7, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xc000, 0xf3ff, 0x3cff, 0x04f7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe500, 0xfbff, 0x7dff, + 0x01f7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf200, 0xfdff, 0xfbff, 0x13fe, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf940, 0xfeff, 0xf7ff, 0x03fe, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc80, 0xff7f, 0xffff, + 0x27fd, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfe10, 0xff7f, 0xefff, 0x0ffb, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfe00, 0x3fbf, 0xeff8, 0x0ffb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0x01ff, 0xfe00, + 0x9fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0x007f, 0xd808, 0x1ff7, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xffc0, 0x0c07, 0xe7dc, 0x3ff7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe4, 0x0f0f, 0xdfc0, + 0x7fef, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff0, 0x1f7f, 0xfff0, 0x7fef, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfff0, 0x7fff, 0xfffc, 0xffef, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff9, 0xffff, 0xffff, + 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffc, 0xfeff, 0xffff, 0xffff, 0x0001, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfffc, 0xfdff, 0xffff, 0xffdf, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, + 0xffdf, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x2000, 0xffff, 0xfbff, 0xdfff, 0xffdf, 0x0003, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xffff, 0xfbff, 0xdfff, 0xfddf, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0xe7ff, 0xefff, + 0xffdf, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x8000, 0xffff, 0xdfff, 0xf7ff, 0xff9f, 0x004f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, + 0xffff, 0xbfff, 0xf1ff, 0xff9f, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0x6fff, 0xfc7c, + 0xff9f, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xfe01, 0xfb9f, 0x001f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe200, + 0xffff, 0x9fff, 0xffff, 0xfb9f, 0x011f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x7fff, 0xffff, + 0xfb9f, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf100, 0xffff, 0xffff, 0xfff8, 0xfbdf, 0x003f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, + 0xfbff, 0xffff, 0xfe03, 0xf7df, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xfe7f, 0xfff9, 0xffff, + 0xf7cf, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf800, 0xffbf, 0xfffb, 0xffff, 0xf7c7, 0x007f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, + 0xffcf, 0xffff, 0xffff, 0xf7e3, 0x04ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xfff7, 0xfff7, 0x3fff, + 0xf7e0, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xe200, 0xfffb, 0xfff7, 0x07ff, 0xf7f8, 0x01ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, + 0xfffc, 0xfff7, 0xc07f, 0xf7ff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xfffe, 0xfff7, 0xf81f, + 0xf7ff, 0x09ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x2400, 0xffff, 0xfffb, 0xff07, 0xe7ff, 0x01ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7000, + 0xffff, 0xfff9, 0xfff0, 0xf7ff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xb800, 0xffff, 0x7ff8, 0xfffc, + 0xe7ff, 0x13ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x9c00, 0x3fff, 0x9ffe, 0xffff, 0xf7ff, 0x07ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xdc00, + 0x1fff, 0xe7ff, 0xffff, 0xe7ff, 0x05ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xef00, 0xc1ff, 0xf9ff, 0xffff, + 0xf7ff, 0x05ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xef80, 0xf01f, 0xfcff, 0xffff, 0xf1ff, 0x03ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x37c0, + 0xfc00, 0xff5f, 0xffff, 0xf1ff, 0x0bff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffaf, 0xffff, + 0xf1ff, 0x0bfb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff80, 0xffff, 0xffd7, 0xffff, 0xf3ff, 0x1ffb, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8800, 0xffff, + 0xffff, 0x0077, 0xfff8, 0xf2ff, 0x17fb, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xf03b, 0xffef, + 0xf2ff, 0x97fb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8800, 0xffff, 0xffff, 0xfc01, 0xffff, 0xf2ff, 0x27fb, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, + 0xffff, 0xe001, 0xffff, 0xf97f, 0x2ff3, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xfffe, 0xffff, 0xc380, 0xffff, + 0xf97f, 0x0ff7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfff8, 0xffff, 0x3fa0, 0xffff, 0xfc7f, 0x4ff7, 0x0001, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, + 0x7fff, 0x7f80, 0xfffe, 0xfc3f, 0x0ff7, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc4, 0x3fff, 0xffc1, 0xfffd, + 0xfcbf, 0x1ff7, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff80, 0x3fff, 0xffc0, 0xffff, 0xfe9f, 0x1ff7, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff10, + 0xbfff, 0xffe0, 0xffff, 0xfe9f, 0x1ff7, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0x1fff, 0xfff0, 0xffff, + 0xffdf, 0x3ffb, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf800, 0x4fff, 0xfff8, 0xffff, 0xffcf, 0x3fef, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe100, + 0x07ff, 0xfff8, 0xffff, 0xffcf, 0x3fe9, 0x0001, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x87ff, 0xffdc, 0xffff, + 0xffe7, 0x3fef, 0x0008, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x03ff, 0xfffe, 0xffff, 0xffe7, 0x3fe9, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x00f8, 0xfff7, 0xffff, 0xffe7, 0x7fe9, 0x0002, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2000, 0xfff7, 0xffff, + 0xffe7, 0x7fe9, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x8000, 0xfff9, 0xffff, 0xfff3, 0xffe9, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xc002, 0xfff8, 0xffff, 0xfff3, 0xffe9, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4008, 0xfffc, 0xffff, + 0xfff1, 0xffc9, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xfff9, 0xffc9, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffff, 0xffff, 0xfff8, 0xffc9, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0xfffe, 0xffff, + 0xfff8, 0x3fd9, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x4000, 0xfffe, 0xffff, 0xfffc, 0x07d9, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xa000, 0xffff, 0x7fff, 0xfffc, 0xf0d9, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xd000, 0xffff, 0x7fff, + 0xfffc, 0xf859, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xc200, 0xffff, 0x7fff, 0xfffe, 0x7e19, 0x0008, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf000, 0xffff, 0x3fff, 0xfffe, 0x1f81, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x3fff, + 0xfffe, 0x07d1, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x3fff, 0xffff, 0x01f9, 0x0000, + 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x7e80, 0xfffe, 0x3fff, 0xffff, 0x0078, 0x8000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3c00, 0xffff, 0x3fff, + 0x7fff, 0x0008, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xbe00, 0xffff, 0x9fff, 0x1fff, 0x0000, 0x8000, + 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x9e00, 0xffff, 0x9fff, 0x07ff, 0x0200, 0x2000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xce00, 0xffff, 0x9fff, + 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xce00, 0xffff, 0xcfff, 0x001f, 0x0002, 0x4000, + 0x8010, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xc600, 0xffff, 0x0fff, 0x0000, 0x0000, 0x4000, 0x1fc0, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xa200, 0xffff, 0x0fff, + 0x0000, 0x0000, 0x4000, 0x79f0, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xa000, 0xffff, 0x0fff, 0x0080, 0x0000, 0x6800, + 0x8ff0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xe000, 0xffff, 0x0fff, 0x0000, 0x0000, 0x6000, 0xfff2, 0x0004, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xd400, 0xffff, 0x0fff, + 0x0000, 0x0000, 0x6000, 0xfff9, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xd000, 0xfffb, 0x0fff, 0x0000, 0x0000, 0x6000, + 0xfbf8, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xc200, 0xfffd, 0x07ff, 0x0000, 0x0000, 0x6000, 0x81fc, 0x001f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xfffd, 0x07ff, + 0x0000, 0x0000, 0x6000, 0x007c, 0x001c, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xc000, 0xfffd, 0x17ff, 0x0000, 0x0000, 0x6000, + 0x023c, 0x0060, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x8000, 0xfffd, 0x17ff, 0x0000, 0x0000, 0xe000, 0x003e, 0x0084, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x07ff, + 0x0000, 0x0000, 0xc000, 0x001e, 0x0040, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x17ff, 0x0000, 0x0000, 0xc000, + 0x200e, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xe000, 0xfffe, 0x0fff, 0x0000, 0x0000, 0xc000, 0x000f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x0fff, + 0x0000, 0x0000, 0xc000, 0x07e7, 0x0801, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe000, 0xfffe, 0x0fff, 0x0000, 0x0000, 0xc000, + 0x1ff7, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x6000, 0xfffa, 0x0fff, 0x0000, 0x0000, 0xc000, 0x7e3b, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xfffa, 0x5fff, + 0x0000, 0x0000, 0x8000, 0xf00f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfffa, 0x0fff, 0x0000, 0x0000, 0x8000, + 0xc007, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfffa, 0x1fff, 0x0000, 0x0000, 0x9000, 0x1003, 0x0013, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1000, 0xfffa, 0x1fff, + 0x0000, 0x0000, 0x8000, 0x000b, 0x0002, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, 0x1fff, 0x0000, 0x0000, 0x8000, + 0x0001, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x8000, 0xfff0, 0x9fff, 0x0000, 0x0000, 0xa000, 0x0001, 0x002a, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x1fff, + 0x0000, 0x0000, 0x0000, 0x0005, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0x1fff, 0x0000, 0x0000, 0x0000, + 0x0005, 0x0010, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffe0, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0001, 0x0040, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0x1fff, + 0x0000, 0x0000, 0x4000, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffc4, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffc0, 0x3fff, 0x0001, 0x0000, 0x0000, 0x0003, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe88, 0x3fff, + 0x0000, 0x0000, 0x0000, 0x0100, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfd80, 0x3fff, 0x0000, 0x0000, 0x0000, + 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfb00, 0x3fff, 0x0000, 0x0000, 0x8000, 0x41ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfa00, 0x3fff, + 0x0000, 0x0000, 0xc800, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0x7fff, 0x0000, 0x0000, 0xe000, + 0x1fff, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xe000, 0x7fff, 0x0004, 0x0000, 0xf000, 0x7fff, 0x0010, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe080, 0xffff, + 0x0000, 0x0000, 0xf000, 0xffff, 0x0040, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xc900, 0xffff, 0x2000, 0x0000, 0xf000, + 0xffff, 0x0083, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8800, 0xffff, 0x0001, 0x0000, 0xf800, 0xffff, 0x0007, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, + 0x0001, 0x0002, 0xfc80, 0xffff, 0x041f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x2000, 0xffff, 0x0003, 0x0004, 0xfc00, + 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x7200, 0xffff, 0x1f87, 0x0000, 0xf440, 0xffff, 0x107f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7000, 0xfffe, + 0x3fc7, 0x0020, 0xf800, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x7800, 0xfffc, 0xffe3, 0x0001, 0xfc00, + 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x7c00, 0xfffc, 0xffe3, 0x0003, 0xfe00, 0xffff, 0x03ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7c00, 0xfffc, + 0xffe3, 0x000f, 0xff08, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x7e00, 0xfffa, 0xfff1, 0x083f, 0xffc0, + 0xffff, 0x07ff, 0x0010, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x7f00, 0xfffa, 0xfff9, 0x007f, 0xfff0, 0xffff, 0x07ff, 0x0040, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7f84, 0xfff2, + 0xfff8, 0x01ff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x7fc0, 0xfff6, 0xfffc, 0xc7ff, 0xffff, + 0xffff, 0x0fff, 0x0060, 0x0000, 0x2000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x7fe0, 0xfffe, 0xfffc, 0x1fff, 0xffff, 0xffff, 0x0fff, 0x00e0, + 0x0000, 0x0100, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7ff0, 0x7fee, + 0xfffe, 0xffff, 0xfffc, 0xffff, 0x0fff, 0x07c0, 0x0000, 0x0000, 0x0020, + 0x0000, 0x0000, 0x0000, 0x0000, 0x7ff8, 0x3fee, 0xfffe, 0xffff, 0xfff3, + 0xffff, 0x9fff, 0x07c0, 0x0000, 0xf020, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x7ffc, 0x3fde, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, 0x3fc1, + 0x0002, 0xff00, 0x0021, 0x0000, 0x0000, 0x0000, 0x0000, 0x7ffe, 0x3fff, + 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0x7ec0, 0x0000, 0x3f88, 0x0084, + 0x0000, 0x0000, 0x0000, 0x8000, 0x3fff, 0x9fff, 0xffff, 0xffff, 0xffff, + 0xffff, 0x7fff, 0x8006, 0x0001, 0x6080, 0x003e, 0x0000, 0x0000, 0x0000, + 0xc080, 0xbfff, 0x9fff, 0xffff, 0xffff, 0xffff, 0xbfff, 0x7fff, 0x00fe, + 0x0003, 0xbff0, 0x003f, 0x0000, 0x0000, 0x0000, 0xf000, 0x9fff, 0xdffd, + 0xffff, 0xffff, 0xffff, 0xe7ff, 0xffff, 0x1ffc, 0x0004, 0xdff8, 0x001f, + 0x0000, 0x0000, 0x0000, 0xfc00, 0x9fff, 0xcffd, 0xffff, 0xffff, 0xffff, + 0xf9ff, 0xffff, 0xfff9, 0x1019, 0xe7f8, 0x004f, 0x0000, 0x0000, 0x0000, + 0xfe04, 0xdfff, 0xeffe, 0xffff, 0xffff, 0xffff, 0xfcff, 0xffff, 0x0fd3, + 0x0070, 0xf7fc, 0x002f, 0x0000, 0x0000, 0x0000, 0xff80, 0x4fff, 0xf7fe, + 0xffff, 0xffff, 0xffff, 0xfe3f, 0xffff, 0xffe7, 0xf0fc, 0xfbfd, 0x0033, + 0x0000, 0x0000, 0x4000, 0xffe0, 0x6fff, 0xf7fe, 0xffff, 0xffff, 0xffff, + 0xfd9f, 0xffff, 0xff9f, 0xfffb, 0xfbfd, 0x003b, 0x0000, 0x0000, 0x0000, + 0xfff8, 0x27ff, 0xf3ff, 0xffff, 0xffff, 0xffff, 0xf9cf, 0xffff, 0xffbf, + 0xffe3, 0xfbfe, 0x001c, 0x0000, 0x0000, 0x0140, 0xffff, 0x13ff, 0xf9ff, + 0xffff, 0xffff, 0xffff, 0xe7e3, 0xffff, 0xffff, 0xffcf, 0x7bfe, 0x009f, + 0x0000, 0x0000, 0xe000, 0xffff, 0x8bff, 0xfdff, 0xffff, 0xffff, 0xffff, + 0xc7f1, 0xffff, 0xffff, 0x7f1f, 0x97fe, 0x000f, 0x0000, 0x8000, 0xff78, + 0xffff, 0x91ff, 0xfeff, 0xffff, 0xffff, 0xffff, 0xcff8, 0xffff, 0xffff, + 0xf87f, 0x30c3, 0x000f, 0x0000, 0x0000, 0xc1fc, 0xffff, 0xd9ff, 0xfe03, + 0xffff, 0xffff, 0x7fff, 0x9ff8, 0xffff, 0xffff, 0xe07f, 0xffbf, 0x0006, + 0x0000, 0x0000, 0x007e, 0xffff, 0xccff, 0xfc00, 0xffff, 0xffff, 0x7fff, + 0x9ffc, 0xffff, 0xffff, 0xc1ff, 0xffff, 0x0001, 0x0000, 0x9000, 0x3e1f, + 0xfffc, 0x07ff, 0xf0f8, 0xffff, 0xfff7, 0x3fff, 0x3ffe, 0xffff, 0xffff, + 0x01ff, 0xfffc, 0x0000, 0x0000, 0x8000, 0xff8f, 0xfff8, 0x07ff, 0xe1ff, + 0xffff, 0xffff, 0x1fff, 0x3ffe, 0xffff, 0xffff, 0x03ff, 0x7ff0, 0x0000, + 0x0000, 0xc000, 0xffe3, 0xffe3, 0xe07f, 0xc7ff, 0xffff, 0xfffb, 0x0fff, + 0x3dfb, 0xffff, 0xffff, 0x8fdf, 0x0000, 0x0000, 0x0000, 0xe000, 0xfff3, + 0xffc7, 0xf80f, 0x8fff, 0xffff, 0xfffb, 0x8fff, 0x33cf, 0xfffe, 0xffff, + 0x8fbf, 0x0000, 0x0004, 0x0000, 0xf000, 0xeff9, 0x0b1f, 0xffc0, 0x3fff, + 0xffff, 0xfffb, 0x07ff, 0x081c, 0xfffe, 0xffff, 0x18ff, 0x0001, 0x0001, + 0x0000, 0xf900, 0x9ffe, 0x007f, 0xfffe, 0x7fff, 0xfffe, 0xfffb, 0x07ff, + 0x0030, 0xfffe, 0xffff, 0x03ff, 0x2080, 0x0000, 0x0000, 0x7c80, 0x3ffe, + 0xfcff, 0xffff, 0x7fff, 0xfffe, 0xfffb, 0x03ff, 0x0001, 0xfffe, 0xffff, + 0x0f81, 0x0001, 0x0000, 0x0000, 0x7c00, 0x3801, 0xf3fc, 0xffff, 0xffff, + 0xfffc, 0xfffb, 0x03ff, 0x4020, 0xfffe, 0xffff, 0xe03f, 0x0002, 0x0000, + 0x0000, 0x3e00, 0x03f8, 0xcff9, 0xf77f, 0xffff, 0xfff9, 0xfffb, 0x11ff, + 0x0000, 0xffdf, 0xfffc, 0x00ff, 0x0002, 0x0000, 0x0000, 0x8f00, 0xbfff, + 0x3fe3, 0xfb7c, 0x3fff, 0xfffa, 0xfffb, 0x01ff, 0x4000, 0xf07e, 0x0003, + 0x1000, 0x0001, 0x0000, 0x0000, 0xc380, 0xffff, 0xffcf, 0xff82, 0xbfe5, + 0xfff1, 0xfff3, 0x00ff, 0x4000, 0x00fc, 0x0000, 0x0400, 0x0000, 0x0000, + 0x0000, 0xf3c0, 0xffff, 0xff8f, 0xfd9f, 0xbfde, 0xfff7, 0xfffb, 0x08ff, + 0x8000, 0x43e0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfce0, 0xffff, + 0xff00, 0xbcbf, 0xdfbf, 0xffc7, 0xfff3, 0x00ff, 0x0000, 0x0000, 0xa0d0, + 0x0006, 0x0000, 0x0000, 0x0000, 0xfe70, 0x1fff, 0xfe7c, 0xbc3f, 0xdfff, + 0x7fcf, 0xfff3, 0x047f, 0x0000, 0x0004, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xff3c, 0xc0ff, 0xf8fe, 0xddff, 0xde7f, 0x7f1f, 0xfff3, 0x003f, + 0x0000, 0x0480, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xff9e, 0xf81f, + 0xf1fe, 0xec07, 0xdeff, 0x7f3f, 0xfff3, 0x003f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x7fcd, 0x0ec0, 0x47fe, 0xe5e0, 0xdeff, + 0xfe3f, 0xfff2, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xc000, 0x07ee, 0x77f8, 0x0ffe, 0xf5fc, 0xddff, 0xfe7f, 0xfff6, 0x003f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x6000, 0x0007, 0x79ff, + 0x0ffe, 0xf0ff, 0xddff, 0xfcff, 0xfff4, 0x001f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xbe17, 0x7f7f, 0xe7ff, 0xf8ff, 0xd9ff, + 0xfcff, 0xfff4, 0x011f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xc000, 0xdf3b, 0x7f7f, 0x11ff, 0xf8fe, 0xdbff, 0xf8ff, 0xffe4, 0x001f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe200, 0xefbf, 0x3fbf, + 0xb1ff, 0xfcff, 0xd3ff, 0xf9ff, 0xffed, 0x001f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf800, 0xf79f, 0xbfdf, 0xcfff, 0xfcff, 0xd7ff, + 0xf1ff, 0xffe9, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfc40, 0xf79f, 0xdfdf, 0xe3ff, 0xfcff, 0xd7ff, 0xf3ff, 0xffe9, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff90, 0xfb9f, 0xcfe7, + 0xf800, 0xfeff, 0xd7ff, 0xf3ff, 0xffc3, 0x000f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfc00, 0xfd85, 0x07f3, 0xfc00, 0xfc7f, 0xafff, + 0xe3ff, 0xffd3, 0x0083, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0010, 0xfd80, 0x03f9, 0xde00, 0xfc7f, 0xafff, 0xe7ff, 0x7fc3, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0080, 0x7e80, 0x00fc, + 0xe700, 0xfc7f, 0xafff, 0xe7ff, 0x07c3, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x2000, 0x1e80, 0x203e, 0xf1c0, 0xfc7f, 0x8fff, + 0xe7ff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0300, 0x001e, 0xfc60, 0xf87f, 0x8fff, 0xe7ff, 0x0003, 0x0010, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0100, + 0xfe08, 0xf8ff, 0x4fff, 0xcfff, 0xc003, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe4, 0xe0ff, 0x4fff, + 0xcfff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff2, 0xc1ff, 0x4fff, 0xcfff, 0x0187, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0040, 0x0000, + 0xfffc, 0x83ff, 0x0fff, 0xcfff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xfffc, 0x0fff, 0x1ffe, + 0xcffe, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x1fff, 0x0ffc, 0xc7f8, 0x0007, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, + 0xf80f, 0x7fff, 0xcfe0, 0xe381, 0x004f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x4000, 0xffc0, 0xffff, 0xcf80, + 0xe007, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff00, 0xf7ff, 0xe007, 0xc00f, 0x0007, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0200, + 0xc000, 0xfc2f, 0xe01f, 0x303f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xf83d, + 0x003f, 0x0100, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0020, 0x8000, 0xff81, 0x0201, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x1fe0, 0x2040, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0001, + 0x0008, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x2000, 0x0000, 0x0002, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x4000, 0x2000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0202, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000 +}; -#define llama_mask_width 240 +#define llama_mask_width 240 #define llama_mask_height 260 static const unsigned short llama_mask_bits[] = { - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0e00, 0xfc00, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0f80, 0xfc00, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0f80, 0xfc00, 0x00ff, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0fc0, 0xfc00, 0x03ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x1fe0, 0xf800, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1ff0, 0xf000, 0x3fff, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x1ff0, 0xe000, 0x7fff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x1ff8, 0xe000, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1ffc, 0xc000, 0xffff, - 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x1ffc, 0x8000, 0xffff, 0x0007, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x1ffc, 0x8000, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3ffe, 0x0000, 0xffff, - 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x3ffe, 0x0000, 0xfffe, 0x007f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x3fff, 0x0000, 0xfffc, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fff, 0x0000, 0xfff8, - 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x3fff, 0x0000, 0xfff0, 0x03ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x3fff, 0x0000, 0xffe0, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fff, 0x0000, 0xffc0, - 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x7fff, 0x0000, 0xff80, 0x1fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x7fff, 0x0000, 0xff80, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7fff, 0x0000, 0xff00, - 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffff, 0x0000, 0xfe00, 0x7fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xffff, 0x0000, 0xfc20, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0x0001, 0xf830, - 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffe, 0x00c1, 0xf030, 0x7fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfffc, 0x6f81, 0xe030, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xff83, 0xe071, - 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0xff07, 0xc073, 0x7fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xffe0, 0xfdff, 0xc077, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xc07f, - 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xc0ff, 0x3fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfe00, 0xffff, 0xc0ff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x80ff, - 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x81ff, 0x1fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfc00, 0xffff, 0x81ff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x83ff, - 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0x83ff, 0x0fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0x07ff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0x0fff, - 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0x0fff, 0x07ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfff0, 0xffff, 0x1fff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0x3fff, - 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0x7fff, 0x01ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfff8, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, - 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff7c, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fbc, 0xfff0, 0xffff, - 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0f9e, 0xfff9, 0xffff, 0x003f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x878e, 0xfffb, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc186, 0xffff, 0xffff, - 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe082, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf002, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, - 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfe00, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, - 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xffc0, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, - 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfffc, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, - 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfff8, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, - 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfffe, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, - 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, - 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, - 0xffff, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, - 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, - 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, - 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, - 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, - 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, - 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, - 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, - 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x03ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, - 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, - 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, - 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, - 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, - 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, - 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, - 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, - 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, - 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, - 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffcf, 0xffff, - 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xffff, 0xffff, 0xffe3, 0xffff, 0xffff, 0xffff, 0x0001, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, - 0xffff, 0xffe1, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xfff1, 0xffff, - 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfff8, 0xffff, 0xfff8, 0xffff, 0xffff, 0xffff, 0x0003, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, - 0xffff, 0xfff8, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0x7fff, 0xfffc, 0xffff, - 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xff80, 0x7fff, 0xfffe, 0xffff, 0xffff, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, - 0x3fff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xbfff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x000d, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xf800, 0x9fff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0009, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, - 0xcfff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0013, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xefff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe7ff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf3fe, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf1f8, 0xffff, 0xffff, - 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, - 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, - 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, - 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, - 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0xffff, 0xffff, 0x7fff, 0x8000, 0x0001, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, - 0xffff, 0x3fff, 0x8000, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x07ff, 0xc000, - 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0xffff, 0xffff, 0x03ff, 0xe000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, - 0xffff, 0x003f, 0xe000, 0x3fc0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x0003, 0xf000, - 0xfff0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0xffff, 0x1fff, 0x0000, 0xf000, 0xfff8, 0x0001, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, - 0x07ff, 0x0000, 0xf000, 0xfff8, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x00ff, 0x0000, 0xf800, - 0xfffc, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0xfffe, 0x0007, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0x3fff, - 0x0000, 0x0000, 0xf800, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, - 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0xffff, 0x007f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfd80, 0xffff, 0x3fff, - 0x0000, 0x0000, 0xf800, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfd00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, - 0x03ff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfc00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0x01ff, 0x03fc, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x3fff, - 0x0000, 0x0000, 0xf800, 0x007f, 0x07c0, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, - 0x3fff, 0x0f00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfc00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0xffff, 0x0c00, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x3fff, - 0x0000, 0x0000, 0xf000, 0xffff, 0x0801, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf000, - 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf800, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf000, 0xffff, 0x0003, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x7fff, - 0x0000, 0x0000, 0xf000, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x7fff, 0x0000, 0x0000, 0xf000, - 0xfc3f, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf800, 0xffff, 0x7fff, 0x0000, 0x0000, 0xf000, 0xf00f, 0x001f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x7fff, - 0x0000, 0x0000, 0xe000, 0xc00f, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0x7fff, 0x0000, 0x0000, 0xe000, - 0x0007, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x8000, 0xffff, 0xffff, 0x0000, 0x0000, 0xe000, 0x0007, 0x003e, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, - 0x0000, 0x0000, 0xc000, 0x0007, 0x0038, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0x0000, 0x0000, 0xc000, - 0x0007, 0x0030, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfffe, 0xffff, 0x0000, 0x0000, 0xc000, 0x0007, 0x0060, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, - 0x0000, 0x0000, 0xc000, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0x0000, 0x0000, 0x8000, - 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfff8, 0xffff, 0x0001, 0x0000, 0x8000, 0x000f, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, - 0x0001, 0x0000, 0x8000, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0x0001, 0x0000, 0xe000, - 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffe0, 0xffff, 0x0001, 0x0000, 0xf000, 0x7fff, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, - 0x0003, 0x0000, 0xf800, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0x0003, 0x0000, 0xfc00, - 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffc0, 0xffff, 0x0007, 0x0000, 0xfc00, 0xffff, 0x001f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, - 0x0fc7, 0x0000, 0xfe00, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x3fe7, 0x0000, 0xff00, - 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfc00, 0xffff, 0x7fe7, 0x0000, 0xff00, 0xffff, 0x01ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, - 0xffff, 0x0003, 0xff80, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0x0007, 0xff80, - 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfe00, 0xffff, 0xffff, 0x000f, 0xffc0, 0xffff, 0x1fff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, - 0xffff, 0x003f, 0xffc0, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0x007f, 0xffc0, - 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xff80, 0xffff, 0xffff, 0x00ff, 0xffc0, 0xffff, 0x3fff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, - 0xffff, 0x01ff, 0xfff8, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0x7fff, 0x001c, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x007c, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x01fc, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0x7fff, 0x07fc, 0x0000, 0x2000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0ff8, - 0x0000, 0xff00, 0x0007, 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x3ff8, 0x0000, 0xffc0, 0x003f, - 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xfff8, 0x0000, 0xffe0, 0x003f, 0x0000, 0x0000, 0x0000, - 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xfff1, - 0x0003, 0xfff0, 0x003f, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0xfff8, 0x00ff, - 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0x001f, 0xfff8, 0x00ff, 0x0000, 0x0000, 0x0000, - 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0x003f, 0xfffc, 0x00ff, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0xfffc, 0x00ff, - 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xf1ff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, - 0xfffc, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, - 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0xe000, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0xff7c, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, - 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x8000, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x007f, 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, - 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xf000, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x001f, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, - 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0xfc00, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xfffe, 0x0007, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xfff1, 0x0001, - 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0x3f81, 0x0000, 0x0000, 0xff80, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0xf9ff, 0xffff, 0xffff, - 0xffff, 0x0001, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x1fff, 0xc1e0, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, - 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, - 0x8000, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0xffe0, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, 0xc000, 0xffff, 0xffff, - 0xffff, 0x0001, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x0fff, 0xc000, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, - 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, - 0x8000, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0xfffe, 0xe0f0, - 0x0007, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x07ff, 0x0000, 0x7ffc, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x03ff, - 0x0000, 0x0f80, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, - 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, - 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, - 0xffe7, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x3fff, 0xfff0, 0xffff, 0xffff, - 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffe0, 0x03ff, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0x01ff, - 0xfff8, 0xffff, 0xffff, 0xffff, 0xffef, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x8fe0, 0x007f, 0xfffc, 0xffff, 0xffff, - 0xffff, 0x00df, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x87e0, 0x003f, 0xfffe, 0xffff, 0xffff, 0xffff, 0x019f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8040, 0x0007, - 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, - 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, - 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, - 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1e00, - 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0200, 0xfffc, 0xffff, 0xffff, - 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xffff, 0xffff, 0xe07f, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, - 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0xbf00, 0xffff, 0x0003, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0xc000, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x03fe, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, - 0x0000, 0x0000, 0x0000 }; + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0e00, 0xfc00, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0f80, 0xfc00, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0f80, 0xfc00, 0x00ff, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0fc0, 0xfc00, 0x03ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x1fe0, 0xf800, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1ff0, 0xf000, 0x3fff, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x1ff0, 0xe000, 0x7fff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x1ff8, 0xe000, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1ffc, 0xc000, 0xffff, + 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x1ffc, 0x8000, 0xffff, 0x0007, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x1ffc, 0x8000, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3ffe, 0x0000, 0xffff, + 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x3ffe, 0x0000, 0xfffe, 0x007f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x3fff, 0x0000, 0xfffc, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fff, 0x0000, 0xfff8, + 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x3fff, 0x0000, 0xfff0, 0x03ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x3fff, 0x0000, 0xffe0, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fff, 0x0000, 0xffc0, + 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x7fff, 0x0000, 0xff80, 0x1fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x7fff, 0x0000, 0xff80, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x7fff, 0x0000, 0xff00, + 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffff, 0x0000, 0xfe00, 0x7fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xffff, 0x0000, 0xfc20, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0x0001, 0xf830, + 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffe, 0x00c1, 0xf030, 0x7fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfffc, 0x6f81, 0xe030, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xff83, 0xe071, + 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0xff07, 0xc073, 0x7fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xffe0, 0xfdff, 0xc077, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xc07f, + 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xc0ff, 0x3fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfe00, 0xffff, 0xc0ff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x80ff, + 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x81ff, 0x1fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfc00, 0xffff, 0x81ff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0x83ff, + 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0x83ff, 0x0fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0x07ff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0x0fff, + 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0x0fff, 0x07ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfff0, 0xffff, 0x1fff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0x3fff, + 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0x7fff, 0x01ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfff8, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, + 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff7c, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x3fbc, 0xfff0, 0xffff, + 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0f9e, 0xfff9, 0xffff, 0x003f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x878e, 0xfffb, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc186, 0xffff, 0xffff, + 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe082, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf002, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, + 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfe00, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, + 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xffc0, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, + 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfffc, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, + 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfff8, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, + 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfffe, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, + 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8000, + 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, + 0xffff, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, + 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, + 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, + 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, + 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, + 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, + 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, + 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, + 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x03ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, + 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, + 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, + 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, + 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, + 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, + 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, + 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, + 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, + 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, + 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffcf, 0xffff, + 0xffff, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xffff, 0xffff, 0xffe3, 0xffff, 0xffff, 0xffff, 0x0001, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, + 0xffff, 0xffe1, 0xffff, 0xffff, 0xffff, 0x0001, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0xfff1, 0xffff, + 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfff8, 0xffff, 0xfff8, 0xffff, 0xffff, 0xffff, 0x0003, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, + 0xffff, 0xfff8, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0x7fff, 0xfffc, 0xffff, + 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xff80, 0x7fff, 0xfffe, 0xffff, 0xffff, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, + 0x3fff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xbfff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x000d, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xf800, 0x9fff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0009, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, + 0xcfff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0013, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xefff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe7ff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0003, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf3fe, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf1f8, 0xffff, 0xffff, + 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, + 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, + 0xffff, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, + 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0x0000, + 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0xffff, 0xffff, 0x7fff, 0x8000, 0x0001, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, + 0xffff, 0x3fff, 0x8000, 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x07ff, 0xc000, + 0x0001, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0xffff, 0xffff, 0x03ff, 0xe000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, + 0xffff, 0x003f, 0xe000, 0x3fc0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0x0003, 0xf000, + 0xfff0, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0xffff, 0x1fff, 0x0000, 0xf000, 0xfff8, 0x0001, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, + 0x07ff, 0x0000, 0xf000, 0xfff8, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0x00ff, 0x0000, 0xf800, + 0xfffc, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0xfffe, 0x0007, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0x3fff, + 0x0000, 0x0000, 0xf800, 0xffff, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, + 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0xffff, 0x007f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfd80, 0xffff, 0x3fff, + 0x0000, 0x0000, 0xf800, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfd00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, + 0x03ff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfc00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0x01ff, 0x03fc, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x3fff, + 0x0000, 0x0000, 0xf800, 0x007f, 0x07c0, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, + 0x3fff, 0x0f00, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfc00, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf800, 0xffff, 0x0c00, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x3fff, + 0x0000, 0x0000, 0xf000, 0xffff, 0x0801, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf000, + 0xffff, 0x0003, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf800, 0xffff, 0x3fff, 0x0000, 0x0000, 0xf000, 0xffff, 0x0003, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x7fff, + 0x0000, 0x0000, 0xf000, 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0x7fff, 0x0000, 0x0000, 0xf000, + 0xfc3f, 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf800, 0xffff, 0x7fff, 0x0000, 0x0000, 0xf000, 0xf00f, 0x001f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, 0xffff, 0x7fff, + 0x0000, 0x0000, 0xe000, 0xc00f, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0x7fff, 0x0000, 0x0000, 0xe000, + 0x0007, 0x001f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x8000, 0xffff, 0xffff, 0x0000, 0x0000, 0xe000, 0x0007, 0x003e, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, + 0x0000, 0x0000, 0xc000, 0x0007, 0x0038, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0x0000, 0x0000, 0xc000, + 0x0007, 0x0030, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfffe, 0xffff, 0x0000, 0x0000, 0xc000, 0x0007, 0x0060, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, + 0x0000, 0x0000, 0xc000, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, 0x0000, 0x0000, 0x8000, + 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfff8, 0xffff, 0x0001, 0x0000, 0x8000, 0x000f, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, + 0x0001, 0x0000, 0x8000, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0x0001, 0x0000, 0xe000, + 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffe0, 0xffff, 0x0001, 0x0000, 0xf000, 0x7fff, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, + 0x0003, 0x0000, 0xf800, 0xffff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0x0003, 0x0000, 0xfc00, + 0xffff, 0x0007, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffc0, 0xffff, 0x0007, 0x0000, 0xfc00, 0xffff, 0x001f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, + 0x0fc7, 0x0000, 0xfe00, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0x3fe7, 0x0000, 0xff00, + 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfc00, 0xffff, 0x7fe7, 0x0000, 0xff00, 0xffff, 0x01ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, + 0xffff, 0x0003, 0xff80, 0xffff, 0x07ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0x0007, 0xff80, + 0xffff, 0x0fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfe00, 0xffff, 0xffff, 0x000f, 0xffc0, 0xffff, 0x1fff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, + 0xffff, 0x003f, 0xffc0, 0xffff, 0x1fff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0x007f, 0xffc0, + 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xff80, 0xffff, 0xffff, 0x00ff, 0xffc0, 0xffff, 0x3fff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, + 0xffff, 0x01ff, 0xfff8, 0xffff, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0x7fff, 0x001c, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x007c, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x01fc, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0x7fff, 0x07fc, 0x0000, 0x2000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x0ff8, + 0x0000, 0xff00, 0x0007, 0x0000, 0x0000, 0x0000, 0x8000, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0x7fff, 0x3ff8, 0x0000, 0xffc0, 0x003f, + 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xfff8, 0x0000, 0xffe0, 0x003f, 0x0000, 0x0000, 0x0000, + 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xfff1, + 0x0003, 0xfff0, 0x003f, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0007, 0xfff8, 0x00ff, + 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0x001f, 0xfff8, 0x00ff, 0x0000, 0x0000, 0x0000, + 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0x003f, 0xfffc, 0x00ff, 0x0000, 0x0000, 0x0000, 0xffc0, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0xfffc, 0x00ff, + 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xf1ff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, + 0xfffc, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, + 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0xe000, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0xff7c, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, + 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x8000, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x007f, 0x0000, 0x8000, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, + 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0xf000, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x001f, 0x0000, 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, + 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x000f, 0x0000, 0xfc00, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xfffe, 0x0007, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xfff1, 0x0001, + 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0x3f81, 0x0000, 0x0000, 0xff80, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x3fff, 0xf9ff, 0xffff, 0xffff, + 0xffff, 0x0001, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x1fff, 0xc1e0, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, + 0x0000, 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x1fff, + 0x8000, 0xffff, 0xffff, 0xffff, 0x0003, 0x0000, 0x0000, 0xffe0, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, 0xc000, 0xffff, 0xffff, + 0xffff, 0x0001, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x0fff, 0xc000, 0xffff, 0xffff, 0x07ff, 0x0000, 0x0000, + 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x0fff, + 0x8000, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0xfffc, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x07ff, 0x0000, 0xfffe, 0xe0f0, + 0x0007, 0x0000, 0x0000, 0x0000, 0xfffe, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x07ff, 0x0000, 0x7ffc, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x03ff, + 0x0000, 0x0f80, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x03ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xf000, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfe00, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xffc0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xfff0, 0xffff, 0xffff, + 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xfff8, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, + 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff80, 0xffff, 0xffff, + 0xffe7, 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xe000, 0xffff, 0x3fff, 0xfff0, 0xffff, 0xffff, + 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffe0, 0x03ff, 0xfff0, 0xffff, 0xffff, 0xffff, 0xffff, 0x001f, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xffe0, 0x01ff, + 0xfff8, 0xffff, 0xffff, 0xffff, 0xffef, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x8fe0, 0x007f, 0xfffc, 0xffff, 0xffff, + 0xffff, 0x00df, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x87e0, 0x003f, 0xfffe, 0xffff, 0xffff, 0xffff, 0x019f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8040, 0x0007, + 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0xffff, 0xffff, 0xffff, + 0xffff, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xe000, 0xffff, 0xffff, 0xffff, 0xffff, 0x003f, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf000, + 0xffff, 0xffff, 0xffff, 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xf800, 0xffff, 0xffff, 0xffff, + 0xffff, 0x007f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0xfc00, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1e00, + 0xffff, 0xffff, 0xffff, 0xffff, 0x00ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0200, 0xfffc, 0xffff, 0xffff, + 0xffff, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0xffe0, 0xffff, 0xffff, 0xffff, 0x01ff, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xffff, 0xffff, 0xe07f, 0x01ff, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xff00, 0xffff, + 0x000f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0xbf00, 0xffff, 0x0003, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0xc000, 0x3fff, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x03fe, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000 +}; #endif /* SPLASH_H_ */ diff --git a/flight/targets/boards/osd/firmware/osd.c b/flight/targets/boards/osd/firmware/osd.c index 499d1d8ca..047bbe01c 100644 --- a/flight/targets/boards/osd/firmware/osd.c +++ b/flight/targets/boards/osd/firmware/osd.c @@ -1,4 +1,3 @@ - /** ****************************************************************************** * @@ -27,7 +26,7 @@ // ***************************************************************************** -//#define USE_WATCHDOG // comment this out if you don't want to use the watchdog +// #define USE_WATCHDOG // comment this out if you don't want to use the watchdog // ***************************************************************************** @@ -35,21 +34,21 @@ #include /* Task Priorities */ -#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) /* Global Variables */ /* Prototype of PIOS_Board_Init() function */ extern void PIOS_Board_Init(void); extern void Stack_Change(void); -static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change"))); +static void Stack_Change_Weak() __attribute__((weakref("Stack_Change"))); /* Function Prototypes */ static void initTask(void *parameters); /* Local Variables */ -#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority -#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive +#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority +#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive static xTaskHandle initTaskHandle; // ***************************************************************************** @@ -59,49 +58,49 @@ static xTaskHandle initTaskHandle; // Local Variables #if defined(USE_WATCHDOG) - volatile uint16_t watchdog_timer; - uint16_t watchdog_delay; +volatile uint16_t watchdog_timer; +uint16_t watchdog_delay; #endif // ***************************************************************************** #if defined(USE_WATCHDOG) - void processWatchdog(void) - { - // random32 = UpdateCRC32(random32, IWDG->SR); +void processWatchdog(void) +{ + // random32 = UpdateCRC32(random32, IWDG->SR); - if (watchdog_timer < watchdog_delay) - return; + if (watchdog_timer < watchdog_delay) { + return; + } - // the watchdog needs resetting + // the watchdog needs resetting - watchdog_timer = 0; + watchdog_timer = 0; - watchdog_Clear(); - } + watchdog_Clear(); +} - void enableWatchdog(void) - { // enable a watchdog - watchdog_timer = 0; - watchdog_delay = watchdog_Init(1000); // 1 second watchdog timeout - } +void enableWatchdog(void) +{ // enable a watchdog + watchdog_timer = 0; + watchdog_delay = watchdog_Init(1000); // 1 second watchdog timeout +} -#endif +#endif /* if defined(USE_WATCHDOG) */ // ***************************************************************************** void sequenceLEDs(void) { - for (int i = 0; i < 2; i++) - { - //USB_LED_ON; + for (int i = 0; i < 2; i++) { + // USB_LED_ON; PIOS_DELAY_WaitmS(100); - //USB_LED_OFF; + // USB_LED_OFF; PIOS_DELAY_WaitmS(100); #if defined(USE_WATCHDOG) - processWatchdog(); // process the watchdog + processWatchdog(); // process the watchdog #endif } } @@ -111,109 +110,103 @@ void sequenceLEDs(void) void processReset(void) { - if (RCC_GetFlagStatus(RCC_FLAG_IWDGRST) != RESET) - { // Independant Watchdog Reset - + if (RCC_GetFlagStatus(RCC_FLAG_IWDGRST) != RESET) { // Independant Watchdog Reset #if defined(PIOS_COM_DEBUG_CONSOLE) - DEBUG_PRINTF(0, "\r\nINDEPENDANT WATCHDOG CAUSED A RESET\r\n"); + DEBUG_PRINTF(0, "\r\nINDEPENDANT WATCHDOG CAUSED A RESET\r\n"); #endif // all led's ON - //USB_LED_ON; + // USB_LED_ON; - PIOS_DELAY_WaitmS(500); // delay a bit + PIOS_DELAY_WaitmS(500); // delay a bit // all led's OFF - //USB_LED_OFF; - - + // USB_LED_OFF; } /* - if (RCC_GetFlagStatus(RCC_FLAG_WWDGRST) != RESET) - { // Window Watchdog Reset + if (RCC_GetFlagStatus(RCC_FLAG_WWDGRST) != RESET) + { // Window Watchdog Reset - DEBUG_PRINTF(0, "\r\nWINDOW WATCHDOG CAUSED A REBOOT\r\n"); + DEBUG_PRINTF(0, "\r\nWINDOW WATCHDOG CAUSED A REBOOT\r\n"); - // all led's ON - USB_LED_ON; - LINK_LED_ON; - RX_LED_ON; - TX_LED_ON; + // all led's ON + USB_LED_ON; + LINK_LED_ON; + RX_LED_ON; + TX_LED_ON; - PIOS_DELAY_WaitmS(500); // delay a bit + PIOS_DELAY_WaitmS(500); // delay a bit - // all led's OFF - USB_LED_OFF; - LINK_LED_OFF; - RX_LED_OFF; - TX_LED_OFF; - } -*/ - if (RCC_GetFlagStatus(RCC_FLAG_PORRST) != RESET) - { // Power-On Reset + // all led's OFF + USB_LED_OFF; + LINK_LED_OFF; + RX_LED_OFF; + TX_LED_OFF; + } + */ + if (RCC_GetFlagStatus(RCC_FLAG_PORRST) != RESET) { // Power-On Reset #if defined(PIOS_COM_DEBUG) - DEBUG_PRINTF(0, "\r\nPOWER-ON-RESET\r\n"); + DEBUG_PRINTF(0, "\r\nPOWER-ON-RESET\r\n"); #endif } - if (RCC_GetFlagStatus(RCC_FLAG_SFTRST) != RESET) - { // Software Reset + if (RCC_GetFlagStatus(RCC_FLAG_SFTRST) != RESET) { // Software Reset #if defined(PIOS_COM_DEBUG) - DEBUG_PRINTF(0, "\r\nSOFTWARE RESET\r\n"); + DEBUG_PRINTF(0, "\r\nSOFTWARE RESET\r\n"); #endif } - if (RCC_GetFlagStatus(RCC_FLAG_LPWRRST) != RESET) - { // Low-Power Reset + if (RCC_GetFlagStatus(RCC_FLAG_LPWRRST) != RESET) { // Low-Power Reset #if defined(PIOS_COM_DEBUG) - DEBUG_PRINTF(0, "\r\nLOW POWER RESET\r\n"); + DEBUG_PRINTF(0, "\r\nLOW POWER RESET\r\n"); #endif } - if (RCC_GetFlagStatus(RCC_FLAG_PINRST) != RESET) - { // Pin Reset + if (RCC_GetFlagStatus(RCC_FLAG_PINRST) != RESET) { // Pin Reset #if defined(PIOS_COM_DEBUG) - DEBUG_PRINTF("0, \r\nPIN RESET\r\n"); + DEBUG_PRINTF("0, \r\nPIN RESET\r\n"); #endif } // Clear reset flags - //RCC_ClearFlag(); + // RCC_ClearFlag(); } int main() { - int result; + int result; + // ************* // init various variables // ************* - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ - vPortInitialiseBlocks(); + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + vPortInitialiseBlocks(); - // Bring up System using CMSIS functions, enables the LEDs. - PIOS_SYS_Init(); + // Bring up System using CMSIS functions, enables the LEDs. + PIOS_SYS_Init(); - /* For Revolution we use a FreeRTOS task to bring up the system so we can */ - /* always rely on FreeRTOS primitive */ - result = xTaskCreate(initTask, (const signed char *)"init", - INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, - &initTaskHandle); - PIOS_Assert(result == pdPASS); + /* For Revolution we use a FreeRTOS task to bring up the system so we can */ + /* always rely on FreeRTOS primitive */ + result = xTaskCreate(initTask, (const signed char *)"init", + INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, + &initTaskHandle); + PIOS_Assert(result == pdPASS); - /* Start the FreeRTOS scheduler which should never returns.*/ - vTaskStartScheduler(); + /* Start the FreeRTOS scheduler which should never returns.*/ + vTaskStartScheduler(); - /* If all is well we will never reach here as the scheduler will now be running. */ - /* Do some indication to user that something bad just happened */ - PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ - for(;;) { \ - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ - PIOS_DELAY_WaitmS(100); \ - }; + /* If all is well we will never reach here as the scheduler will now be running. */ + /* Do some indication to user that something bad just happened */ + PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ + for (;;) { \ + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ + PIOS_DELAY_WaitmS(100); \ + } + ; - return 0; + return 0; } /** @@ -221,17 +214,16 @@ int main() * * Runs board and module initialisation, then terminates. */ -void -initTask(__attribute__((unused)) void *parameters) +void initTask(__attribute__((unused)) void *parameters) { - /* board driver init */ - PIOS_Board_Init(); + /* board driver init */ + PIOS_Board_Init(); - /* Initialize modules */ - MODULE_INITIALISE_ALL; + /* Initialize modules */ + MODULE_INITIALISE_ALL; - /* terminate this task */ - vTaskDelete(NULL); + /* terminate this task */ + vTaskDelete(NULL); } // ***************************************************************************** diff --git a/flight/targets/boards/osd/firmware/pios_board.c b/flight/targets/boards/osd/firmware/pios_board.c index cf65e3345..e725908c0 100644 --- a/flight/targets/boards/osd/firmware/pios_board.c +++ b/flight/targets/boards/osd/firmware/pios_board.c @@ -42,7 +42,7 @@ #include "../board_hw_defs.c" /* Private macro -------------------------------------------------------------*/ -#define countof(a) (sizeof(a) / sizeof(*(a))) +#define countof(a) (sizeof(a) / sizeof(*(a))) /* Private variables ---------------------------------------------------------*/ @@ -51,54 +51,53 @@ void PIOS_ADC_DMC_irq_handler(void); void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); struct pios_adc_cfg pios_adc_cfg = { - .adc_dev = ADC1, - .dma = { - .irq = { - .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), - .init = { - .NVIC_IRQChannel = DMA2_Stream4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA2_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR - }, - } - }, - .half_flag = DMA_IT_HTIF4, - .full_flag = DMA_IT_TCIF4, - + .adc_dev = ADC1, + .dma = { + .irq = { + .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), + .init = { + .NVIC_IRQChannel = DMA2_Stream4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR + }, + } + }, + .half_flag = DMA_IT_HTIF4, + .full_flag = DMA_IT_TCIF4, }; void PIOS_ADC_DMC_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_ADC_DMA_Handler(); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_ADC_DMA_Handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_ADC) */ static void Clock(uint32_t spektrum_id); -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 128 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 128 +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 128 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 128 -#define PIOS_COM_AUX_RX_BUF_LEN 512 -#define PIOS_COM_AUX_TX_BUF_LEN 512 +#define PIOS_COM_AUX_RX_BUF_LEN 512 +#define PIOS_COM_AUX_TX_BUF_LEN 512 -#define PIOS_COM_GPS_RX_BUF_LEN 32 +#define PIOS_COM_GPS_RX_BUF_LEN 32 #define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 uint32_t pios_com_aux_id; uint32_t pios_com_gps_id; @@ -108,387 +107,388 @@ uint32_t pios_com_telem_rf_id; uintptr_t pios_uavo_settings_fs_id; /** - * TIM3 is triggered by the HSYNC signal into its ETR line and will divide the + * TIM3 is triggered by the HSYNC signal into its ETR line and will divide the * APB1_CLOCK to generate a pixel clock that is used by the SPI CLK lines. * TIM4 will be synced to it and will divide by that times the pixel width to * fire an IRQ when the last pixel of the line has been output. Then the timer will - * be rearmed and wait for the next HSYNC signal. + * be rearmed and wait for the next HSYNC signal. * The critical timing detail is that the task be _DISABLED_ at the end of the line * before an extra pixel is clocked out * or we will need to configure the DMA task per line */ #include "pios_tim_priv.h" -#define NTSC_PX_CLOCK 6797088 -#define PAL_PX_CLOCK 6750130 -#define PX_PERIOD ((PIOS_PERIPHERAL_APB1_CLOCK / NTSC_PX_CLOCK) + 1) -#define LINE_PERIOD PX_PERIOD * GRAPHICS_WIDTH +#define NTSC_PX_CLOCK 6797088 +#define PAL_PX_CLOCK 6750130 +#define PX_PERIOD ((PIOS_PERIPHERAL_APB1_CLOCK / NTSC_PX_CLOCK) + 1) +#define LINE_PERIOD PX_PERIOD * GRAPHICS_WIDTH static const TIM_TimeBaseInitTypeDef tim_4_time_base = { - .TIM_Prescaler = 0, //PIOS_PERIPHERAL_APB1_CLOCK, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = LINE_PERIOD - 1, - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = 0, // PIOS_PERIPHERAL_APB1_CLOCK, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = LINE_PERIOD - 1, + .TIM_RepetitionCounter = 0x0000, }; static const struct pios_tim_clock_cfg pios_tim4_cfg = { - .timer = TIM4, - .time_base_init = &tim_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - } + .timer = TIM4, + .time_base_init = &tim_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + } }; -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + // Delay system + PIOS_DELAY_Init(); - // Delay system - PIOS_DELAY_Init(); - - PIOS_LED_Init(&pios_led_cfg); + PIOS_LED_Init(&pios_led_cfg); #if defined(PIOS_INCLUDE_SPI) - /* Set up the SPI interface to the SD card */ - if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) { - PIOS_Assert(0); - } + /* Set up the SPI interface to the SD card */ + if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) { + PIOS_Assert(0); + } #if defined(PIOS_INCLUDE_SDCARD) - /* Enable and mount the SDCard */ - PIOS_SDCARD_Init(pios_spi_sdcard_id); - PIOS_SDCARD_MountFS(0); + /* Enable and mount the SDCard */ + PIOS_SDCARD_Init(pios_spi_sdcard_id); + PIOS_SDCARD_MountFS(0); #endif #endif /* PIOS_INCLUDE_SPI */ #ifdef PIOS_INCLUDE_FLASH_LOGFS_SETTINGS -uintptr_t flash_id; -PIOS_Flash_Internal_Init(&flash_id, &flash_internal_cfg); -PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id); + uintptr_t flash_id; + PIOS_Flash_Internal_Init(&flash_id, &flash_internal_cfg); + PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id); #elif !defined(PIOS_USE_SETTINGS_ON_SDCARD) #error No setting storage specified. (define PIOS_USE_SETTINGS_ON_SDCARD or INCLUDE_FLASH_SECTOR_SETTINGS) #endif - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); - HwSettingsInitialize(); + HwSettingsInitialize(); #ifdef PIOS_INCLUDE_WDG - /* Initialize watchdog as early as possible to catch faults during init */ - PIOS_WDG_Init(); + /* Initialize watchdog as early as possible to catch faults during init */ + PIOS_WDG_Init(); #endif /* PIOS_INCLUDE_WDG */ - /* Initialize the alarms library */ - AlarmsInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); - /* IAP System Setup */ - PIOS_IAP_Init(); - uint16_t boot_count = PIOS_IAP_ReadBootCount(); - if (boot_count < 3) { - PIOS_IAP_WriteBootCount(++boot_count); - AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); - } else { - /* Too many failed boot attempts, force hwsettings to defaults */ - HwSettingsSetDefaults(HwSettingsHandle(), 0); - AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); - } + /* IAP System Setup */ + PIOS_IAP_Init(); + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } #if defined(PIOS_INCLUDE_RTC) - /* Initialize the real-time clock and its associated tick */ - PIOS_RTC_Init(&pios_rtc_main_cfg); - if (!PIOS_RTC_RegisterTickCallback(Clock, 0)) { - PIOS_DEBUG_Assert(0); - } + /* Initialize the real-time clock and its associated tick */ + PIOS_RTC_Init(&pios_rtc_main_cfg); + if (!PIOS_RTC_RegisterTickCallback(Clock, 0)) { + PIOS_DEBUG_Assert(0); + } #endif #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; + /* Flags to determine if various USB interfaces are advertised */ + bool usb_hid_present = false; + bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; + usb_cdc_present = true; #else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; #endif - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_CDC) - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); + uint8_t hwsettings_usb_vcpport; + /* Configure the USB VCP port */ + HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } + if (!usb_cdc_present) { + /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ + hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; + } - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: + switch (hwsettings_usb_vcpport) { + case HWSETTINGS_USB_VCPPORT_DISABLED: + break; + case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: + { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ + { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, + tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + } +#endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); + /* Configure the usb HID port */ + uint8_t hwsettings_usb_hidport; + HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } + if (!usb_hid_present) { + /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ + hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; + } - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: + switch (hwsettings_usb_hidport) { + case HWSETTINGS_USB_HIDPORT_DISABLED: + break; + case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } + { + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + } -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ - if (usb_hid_present || usb_cdc_present) { - PIOS_USBHOOK_Activate(); - } -#endif /* PIOS_INCLUDE_USB */ + if (usb_hid_present || usb_cdc_present) { + PIOS_USBHOOK_Activate(); + } +#endif /* PIOS_INCLUDE_USB */ #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_GPS) - uint32_t pios_usart_gps_id; - if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { - PIOS_Assert(0); - } + uint32_t pios_usart_gps_id; + if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { + PIOS_Assert(0); + } - uint8_t *gps_rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(gps_rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, - gps_rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); - } + uint8_t *gps_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(gps_rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, + gps_rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } -#endif /* PIOS_INCLUDE_GPS */ +#endif /* PIOS_INCLUDE_GPS */ #if defined(PIOS_INCLUDE_COM_AUX) - { - uint32_t pios_usart_aux_id; + { + uint32_t pios_usart_aux_id; - if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) { - PIOS_DEBUG_Assert(0); - } + if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) { + PIOS_DEBUG_Assert(0); + } - uint8_t *aux_rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_RX_BUF_LEN); - uint8_t *aux_tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_AUX_TX_BUF_LEN); - PIOS_Assert(aux_rx_buffer); - PIOS_Assert(aux_tx_buffer); + uint8_t *aux_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_AUX_RX_BUF_LEN); + uint8_t *aux_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_AUX_TX_BUF_LEN); + PIOS_Assert(aux_rx_buffer); + PIOS_Assert(aux_tx_buffer); - if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id, - aux_rx_buffer, PIOS_COM_AUX_RX_BUF_LEN, - aux_tx_buffer, PIOS_COM_AUX_TX_BUF_LEN)) { - PIOS_DEBUG_Assert(0); - } - } + if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id, + aux_rx_buffer, PIOS_COM_AUX_RX_BUF_LEN, + aux_tx_buffer, PIOS_COM_AUX_TX_BUF_LEN)) { + PIOS_DEBUG_Assert(0); + } + } #else - pios_com_aux_id = 0; -#endif /* PIOS_INCLUDE_COM_AUX */ + pios_com_aux_id = 0; +#endif /* PIOS_INCLUDE_COM_AUX */ #if defined(PIOS_INCLUDE_COM_TELEM) - { /* Eventually add switch for this port function */ - uint32_t pios_usart_telem_rf_id; - if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_main_cfg)) { - PIOS_Assert(0); - } + { /* Eventually add switch for this port function */ + uint32_t pios_usart_telem_rf_id; + if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_main_cfg)) { + PIOS_Assert(0); + } - uint8_t *telem_rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t *telem_tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(telem_rx_buffer); - PIOS_Assert(telem_tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id, - telem_rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - telem_tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + uint8_t *telem_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *telem_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(telem_rx_buffer); + PIOS_Assert(telem_tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id, + telem_rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + telem_tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } #else - pios_com_telem_rf_id = 0; -#endif /* PIOS_INCLUDE_COM_TELEM */ + pios_com_telem_rf_id = 0; +#endif /* PIOS_INCLUDE_COM_TELEM */ -#endif /* PIOS_INCLUDE_COM */ +#endif /* PIOS_INCLUDE_COM */ - /* Configure FlexiPort */ + /* Configure FlexiPort */ /* uint8_t hwsettings_rv_flexiport; - HwSettingsRV_FlexiPortGet(&hwsettings_rv_flexiport); + HwSettingsRV_FlexiPortGet(&hwsettings_rv_flexiport); - switch (hwsettings_rv_flexiport) { - case HWSETTINGS_RV_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_RV_FLEXIPORT_I2C:*/ + switch (hwsettings_rv_flexiport) { + case HWSETTINGS_RV_FLEXIPORT_DISABLED: + break; + case HWSETTINGS_RV_FLEXIPORT_I2C:*/ #if defined(PIOS_INCLUDE_I2C) - { - if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_I2C */ + { + if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_I2C */ /* break; - case HWSETTINGS_RV_FLEXIPORT_DSM2: - case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: - case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: - { - enum pios_dsm_proto proto; - switch (hwsettings_rv_flexiport) { - case HWSETTINGS_RV_FLEXIPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } - //TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, - &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hwsettings_DSMxBind); - } - break; - case HWSETTINGS_RV_FLEXIPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - case HWSETTINGS_RV_FLEXIPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - }*/ - /* hwsettings_rv_flexiport */ + case HWSETTINGS_RV_FLEXIPORT_DSM2: + case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: + case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: + { + enum pios_dsm_proto proto; + switch (hwsettings_rv_flexiport) { + case HWSETTINGS_RV_FLEXIPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } + //TODO: Define the various Channelgroup for Revo dsm inputs and handle here + PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, + &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hwsettings_DSMxBind); + } + break; + case HWSETTINGS_RV_FLEXIPORT_COMAUX: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); + break; + case HWSETTINGS_RV_FLEXIPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + }*/ + /* hwsettings_rv_flexiport */ #if defined(PIOS_INCLUDE_WAVE) - PIOS_WavPlay_Init(&pios_dac_cfg); + PIOS_WavPlay_Init(&pios_dac_cfg); #endif - // ADC system + // ADC system #if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); + PIOS_ADC_Init(&pios_adc_cfg); #endif #if defined(PIOS_INCLUDE_VIDEO) - PIOS_TIM_InitClock(&tim_8_cfg); - PIOS_Servo_Init(&pios_servo_cfg); - // Start the pixel and line clock counter - //PIOS_TIM_InitClock(&pios_tim4_cfg); - PIOS_Video_Init(&pios_video_cfg); + PIOS_TIM_InitClock(&tim_8_cfg); + PIOS_Servo_Init(&pios_servo_cfg); + // Start the pixel and line clock counter + // PIOS_TIM_InitClock(&pios_tim4_cfg); + PIOS_Video_Init(&pios_video_cfg); #endif } -uint16_t supv_timer=0; +uint16_t supv_timer = 0; -static void Clock(__attribute__((unused)) uint32_t spektrum_id) { - /* 125hz */ - ++supv_timer; - if(supv_timer >= 625) { - supv_timer = 0; - timex.sec++; - } - if (timex.sec >= 60) { - timex.sec = 0; - timex.min++; - } - if (timex.min >= 60) { - timex.min = 0; - timex.hour++; - } - if (timex.hour >= 99) { - timex.hour = 0; - } +static void Clock(__attribute__((unused)) uint32_t spektrum_id) +{ + /* 125hz */ + ++supv_timer; + if (supv_timer >= 625) { + supv_timer = 0; + timex.sec++; + } + if (timex.sec >= 60) { + timex.sec = 0; + timex.min++; + } + if (timex.min >= 60) { + timex.min = 0; + timex.hour++; + } + if (timex.hour >= 99) { + timex.hour = 0; + } } diff --git a/flight/targets/boards/osd/pios_board.h b/flight/targets/boards/osd/pios_board.h index c4d1e34b5..3a96a11d8 100644 --- a/flight/targets/boards/osd/pios_board.h +++ b/flight/targets/boards/osd/pios_board.h @@ -32,170 +32,170 @@ // Timers and Channels Used /* - Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 - ------+------------+------------+------------+------------ - TIM1 | DELAY | - TIM2 | | PPM Output | PPM Input | - TIM3 | TIMER INTERRUPT | - TIM4 | STOPWATCH | - ------+------------+------------+------------+------------ + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+------------+------------+------------+------------ + TIM1 | DELAY | + TIM2 | | PPM Output | PPM Input | + TIM3 | TIMER INTERRUPT | + TIM4 | STOPWATCH | + ------+------------+------------+------------+------------ */ -//------------------------ +// ------------------------ // DMA Channels Used -//------------------------ -/* Channel 1 - */ -/* Channel 2 - */ -/* Channel 3 - */ -/* Channel 4 - */ -/* Channel 5 - */ -/* Channel 6 - */ -/* Channel 7 - */ -/* Channel 8 - */ -/* Channel 9 - */ -/* Channel 10 - */ -/* Channel 11 - */ -/* Channel 12 - */ +// ------------------------ +/* Channel 1 - */ +/* Channel 2 - */ +/* Channel 3 - */ +/* Channel 4 - */ +/* Channel 5 - */ +/* Channel 6 - */ +/* Channel 7 - */ +/* Channel 8 - */ +/* Channel 9 - */ +/* Channel 10 - */ +/* Channel 11 - */ +/* Channel 12 - */ -//------------------------ +// ------------------------ // BOOTLOADER_SETTINGS -//------------------------ -#define BOARD_READABLE true -#define BOARD_WRITABLE true -#define MAX_DEL_RETRYS 3 +// ------------------------ +#define BOARD_READABLE true +#define BOARD_WRITABLE true +#define MAX_DEL_RETRYS 3 -//------------------------- +// ------------------------- // System Settings // // See also System_stm32f4xx.c -//------------------------- -//These macros are deprecated -//please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below -//#define PIOS_MASTER_CLOCK -//#define PIOS_PERIPHERAL_CLOCK -//#define PIOS_PERIPHERAL_CLOCK +// ------------------------- +// These macros are deprecated +// please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below +// #define PIOS_MASTER_CLOCK +// #define PIOS_PERIPHERAL_CLOCK +// #define PIOS_PERIPHERAL_CLOCK -#define PIOS_SYSCLK 168000000 -// Peripherals that belongs to APB1 are: -// DAC |PWR |CAN1,2 -// I2C1,2,3 |UART4,5 |USART3,2 -// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 -// I2S2Ext |IWDG |WWDG -// RTC/BKP reg +#define PIOS_SYSCLK 168000000 +// Peripherals that belongs to APB1 are: +// DAC |PWR |CAN1,2 +// I2C1,2,3 |UART4,5 |USART3,2 +// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 +// I2S2Ext |IWDG |WWDG +// RTC/BKP reg // TIM2,3,4,5,6,7,12,13,14 // Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) // Default APB1 Prescaler = 4 -#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) +#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) -// Peripherals belonging to APB2 -// SDIO |EXTI |SYSCFG |SPI1 -// ADC1,2,3 -// USART1,6 -// TIM1,8,9,10,11 +// Peripherals belonging to APB2 +// SDIO |EXTI |SYSCFG |SPI1 +// ADC1,2,3 +// USART1,6 +// TIM1,8,9,10,11 // // Default APB2 Prescaler = 2 // -#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK +#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK -//------------------------ +// ------------------------ // TELEMETRY -//------------------------ -#define TELEM_QUEUE_SIZE 20 -#define PIOS_TELEM_STACK_SIZE 624 +// ------------------------ +#define TELEM_QUEUE_SIZE 20 +#define PIOS_TELEM_STACK_SIZE 624 // ***************************************************************** // Interrupt Priorities -#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS -#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS -#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... -#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... -//------------------------ +// ------------------------ // WATCHDOG_SETTINGS -//------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -#define PIOS_WDG_REGISTER RTC_BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_OSDGEN 0x0010 +// ------------------------ +#define PIOS_WATCHDOG_TIMEOUT 250 +#define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_OSDGEN 0x0010 // ***************************************************************** // PIOS_LED -#define PIOS_LED_HEARTBEAT 0 -#define PIOS_LED_ALARM 1 +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_LED_ALARM 1 // ***************************************************************** // Delay Timer -//#define PIOS_DELAY_TIMER TIM2 -//#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE) -#define PIOS_DELAY_TIMER TIM1 -#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE) +// #define PIOS_DELAY_TIMER TIM2 +// #define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE) +#define PIOS_DELAY_TIMER TIM1 +#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE) // ***************************************************************** // Timer interrupt /*#define TIMER_INT_TIMER TIM3 - #define TIMER_INT_FUNC TIM3_IRQHandler - #define TIMER_INT_PRIORITY 2 + #define TIMER_INT_FUNC TIM3_IRQHandler + #define TIMER_INT_PRIORITY 2 - // ***************************************************************** - // Stop watch timer + // ***************************************************************** + // Stop watch timer - #define STOPWATCH_TIMER TIM4*/ +#define STOPWATCH_TIMER TIM4*/ -//------------------------ +// ------------------------ // PIOS_SPI // See also pios_board.c -//------------------------ -#define PIOS_SPI_MAX_DEVS 1 +// ------------------------ +#define PIOS_SPI_MAX_DEVS 1 extern uint32_t pios_spi_port_id; -#define PIOS_SPI_PORT (pios_spi_port_id) +#define PIOS_SPI_PORT (pios_spi_port_id) -//------------------------- +// ------------------------- // PIOS_USART // // See also pios_board.c -//------------------------- -#define PIOS_USART_MAX_DEVS 5 +// ------------------------- +#define PIOS_USART_MAX_DEVS 5 -//------------------------- +// ------------------------- // PIOS_COM // // See also pios_board.c -//------------------------- -#define PIOS_COM_MAX_DEVS 5 +// ------------------------- +#define PIOS_COM_MAX_DEVS 5 extern uint32_t pios_com_telem_rf_id; extern uint32_t pios_com_gps_id; extern uint32_t pios_com_aux_id; extern uint32_t pios_com_telem_usb_id; -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_DEBUG (PIOS_COM_AUX) -#define PIOS_COM_OSD (pios_com_aux_id) +#define PIOS_COM_AUX (pios_com_aux_id) +#define PIOS_COM_GPS (pios_com_gps_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_DEBUG (PIOS_COM_AUX) +#define PIOS_COM_OSD (pios_com_aux_id) -//extern uint32_t pios_com_serial_id; -//#define PIOS_COM_SERIAL (pios_com_serial_id) -//#define PIOS_COM_DEBUG PIOS_COM_SERIAL // uncomment this to send debug info out the serial port +// extern uint32_t pios_com_serial_id; +// #define PIOS_COM_SERIAL (pios_com_serial_id) +// #define PIOS_COM_DEBUG PIOS_COM_SERIAL // uncomment this to send debug info out the serial port -//extern uint32_t pios_com_gps_id; -//#define PIOS_COM_GPS (pios_com_gps_id) +// extern uint32_t pios_com_gps_id; +// #define PIOS_COM_GPS (pios_com_gps_id) #if defined(PIOS_INCLUDE_USB_HID) extern uint32_t pios_com_telem_usb_id; -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) #endif // ***************************************************************** // ADC -//------------------------- +// ------------------------- // ADC // PIOS_ADC_PinGet(0) = Current // PIOS_ADC_PinGet(1) = Voltage @@ -204,82 +204,82 @@ extern uint32_t pios_com_telem_usb_id; // PIOS_ADC_PinGet(4) = Video // PIOS_ADC_PinGet(5) = RSSI // PIOS_ADC_PinGet(6) = VREF -//------------------------- +// ------------------------- #define PIOS_DMA_PIN_CONFIG \ -{ \ -{GPIOC, GPIO_Pin_0, ADC_Channel_10}, \ -{GPIOC, GPIO_Pin_1, ADC_Channel_11}, \ -{GPIOC, GPIO_Pin_2, ADC_Channel_12}, \ -{NULL, 0, ADC_Channel_TempSensor}, /* Temperature sensor */\ -{GPIOC, GPIO_Pin_3, ADC_Channel_13}, \ -{GPIOA, GPIO_Pin_7, ADC_Channel_7}, \ -{NULL, 0, ADC_Channel_Vrefint} /* Voltage reference */\ -} + { \ + { GPIOC, GPIO_Pin_0, ADC_Channel_10 }, \ + { GPIOC, GPIO_Pin_1, ADC_Channel_11 }, \ + { GPIOC, GPIO_Pin_2, ADC_Channel_12 }, \ + { NULL, 0, ADC_Channel_TempSensor }, /* Temperature sensor */ \ + { GPIOC, GPIO_Pin_3, ADC_Channel_13 }, \ + { GPIOA, GPIO_Pin_7, ADC_Channel_7 }, \ + { NULL, 0, ADC_Channel_Vrefint } /* Voltage reference */ \ + } /* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */ /* which is annoying because this then determines the rate at which we generate buffer turnover events */ /* the objective here is to get enough buffer space to support 100Hz averaging rate */ -#define PIOS_ADC_NUM_CHANNELS 7 +#define PIOS_ADC_NUM_CHANNELS 7 #define PIOS_ADC_MAX_OVERSAMPLING 10 -#define PIOS_ADC_USE_ADC2 0 -#define PIOS_ADC_USE_TEMP_SENSOR 1 +#define PIOS_ADC_USE_ADC2 0 +#define PIOS_ADC_USE_TEMP_SENSOR 1 // ***************************************************************** // USB #if defined(PIOS_INCLUDE_USB_HID) -#define PIOS_USB_ENABLED 1 -#define PIOS_USB_DETECT_GPIO_PORT GPIO_IN_2_PORT -#define PIOS_USB_DETECT_GPIO_PIN GPIO_IN_2_PIN -#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4 -#define PIOS_IRQ_USB_PRIORITY 8 -#define PIOS_USB_RX_BUFFER_SIZE 512 -#define PIOS_USB_TX_BUFFER_SIZE 512 +#define PIOS_USB_ENABLED 1 +#define PIOS_USB_DETECT_GPIO_PORT GPIO_IN_2_PORT +#define PIOS_USB_DETECT_GPIO_PIN GPIO_IN_2_PIN +#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4 +#define PIOS_IRQ_USB_PRIORITY 8 +#define PIOS_USB_RX_BUFFER_SIZE 512 +#define PIOS_USB_TX_BUFFER_SIZE 512 #endif // ***************************************************************** -//-------------------------- +// -------------------------- // Timer controller settings -//-------------------------- -#define PIOS_TIM_MAX_DEVS 6 +// -------------------------- +#define PIOS_TIM_MAX_DEVS 6 -//------------------------- +// ------------------------- // USB -//------------------------- -#define PIOS_USB_MAX_DEVS 1 -#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ -#define PIOS_USB_HID_MAX_DEVS 1 +// ------------------------- +#define PIOS_USB_MAX_DEVS 1 +#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ +#define PIOS_USB_HID_MAX_DEVS 1 -//------------------------ +// ------------------------ // PIOS_I2C // See also pios_board.c -//------------------------ -#define PIOS_I2C_MAX_DEVS 1 +// ------------------------ +#define PIOS_I2C_MAX_DEVS 1 extern uint32_t pios_i2c_flexiport_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexiport_adapter_id) -#define PIOS_I2C_ESC_ADAPTER (pios_i2c_flexiport_adapter_id) -#define PIOS_I2C_BMP085_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_ESC_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_BMP085_ADAPTER (pios_i2c_flexiport_adapter_id) -//------------------------ +// ------------------------ // PIOS_BMP085 -//------------------------ -#define PIOS_BMP085_OVERSAMPLING 3 +// ------------------------ +#define PIOS_BMP085_OVERSAMPLING 3 /** * glue macros for file IO * STM32 uses DOSFS for file IO */ -#define PIOS_FOPEN_READ(filename,file) DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, DFS_READ, PIOS_SDCARD_Sector, &file) != DFS_OK +#define PIOS_FOPEN_READ(filename, file) DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, DFS_READ, PIOS_SDCARD_Sector, &file) != DFS_OK -#define PIOS_FOPEN_WRITE(filename,file) DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, DFS_WRITE, PIOS_SDCARD_Sector, &file) != DFS_OK +#define PIOS_FOPEN_WRITE(filename, file) DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, DFS_WRITE, PIOS_SDCARD_Sector, &file) != DFS_OK -#define PIOS_FREAD(file,bufferadr,length,resultadr) DFS_ReadFile(file, PIOS_SDCARD_Sector, (uint8_t*)bufferadr, resultadr, length) != DFS_OK +#define PIOS_FREAD(file, bufferadr, length, resultadr) DFS_ReadFile(file, PIOS_SDCARD_Sector, (uint8_t *)bufferadr, resultadr, length) != DFS_OK -#define PIOS_FWRITE(file,bufferadr,length,resultadr) DFS_WriteFile(file, PIOS_SDCARD_Sector, (uint8_t*)bufferadr, resultadr, length) +#define PIOS_FWRITE(file, bufferadr, length, resultadr) DFS_WriteFile(file, PIOS_SDCARD_Sector, (uint8_t *)bufferadr, resultadr, length) -#define PIOS_FCLOSE(file) DFS_Close(&file) +#define PIOS_FCLOSE(file) DFS_Close(&file) -#define PIOS_FUNLINK(filename) DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, PIOS_SDCARD_Sector) +#define PIOS_FUNLINK(filename) DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, PIOS_SDCARD_Sector) #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/osd/pios_usb_board_data.c b/flight/targets/boards/osd/pios_usb_board_data.c index 6e5e087cf..8d0ad46cf 100644 --- a/flight/targets/boards/osd/pios_usb_board_data.c +++ b/flight/targets/boards/osd/pios_usb_board_data.c @@ -28,64 +28,65 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ -#include /* PIOS_SYS_SerialNumberGet */ -#include /* PIOS_USBHOOK_* */ -#include /* PIOS_USB_UTIL_AsciiToUtf8 */ +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ static const uint8_t usb_product_id[8] = { - sizeof(usb_product_id), - USB_DESC_TYPE_STRING, - 'O', 0, - 'S', 0, - 'D', 0, + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'O', 0, + 'S', 0, + 'D', 0, }; -static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = { - sizeof(usb_serial_number), - USB_DESC_TYPE_STRING, +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, }; static const struct usb_string_langid usb_lang_id = { - .bLength = sizeof(usb_lang_id), - .bDescriptorType = USB_DESC_TYPE_STRING, - .bLangID = htousbs(USB_LANGID_ENGLISH_US), + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), }; static const uint8_t usb_vendor_id[28] = { - sizeof(usb_vendor_id), - USB_DESC_TYPE_STRING, - 'o', 0, - 'p', 0, - 'e', 0, - 'n', 0, - 'p', 0, - 'i', 0, - 'l', 0, - 'o', 0, - 't', 0, - '.', 0, - 'o', 0, - 'r', 0, - 'g', 0 + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 }; int32_t PIOS_USB_BOARD_DATA_Init(void) { - /* Load device serial number into serial number string */ - uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - PIOS_SYS_SerialNumberGet((char *)sn); + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ - uint8_t * utf8 = &(usb_serial_number[2]); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1); + PIOS_SYS_SerialNumberGet((char *)sn); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); - return 0; + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; } diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index 43d43f519..b5cdb57dd 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -9,19 +9,19 @@ * @{ * @brief Defines board specific static initializers for hardware for the Revolution board. *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -29,317 +29,321 @@ #include static const struct pios_led pios_leds[] = { - [PIOS_LED_HEARTBEAT] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_ALARM] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_ALARM] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, #ifdef PIOS_RFM22B_DEBUG_ON_TELEM - [PIOS_LED_D1] = { - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_D2] = { - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_D3] = { - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_D4] = { - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, -#endif + [PIOS_LED_D1] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D2] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D3] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D4] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, +#endif /* ifdef PIOS_RFM22B_DEBUG_ON_TELEM */ }; static const struct pios_led_cfg pios_led_cfg = { - .leds = pios_leds, - .num_leds = NELEMENTS(pios_leds), + .leds = pios_leds, + .num_leds = NELEMENTS(pios_leds), }; static const struct pios_led pios_leds_v2[] = { - [PIOS_LED_HEARTBEAT] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_ALARM] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_ALARM] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, #ifdef PIOS_RFM22B_DEBUG_ON_TELEM - [PIOS_LED_D1] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_D2] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_D3] = { - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_D4] = { - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, -#endif + [PIOS_LED_D1] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D2] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D3] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D4] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, +#endif /* ifdef PIOS_RFM22B_DEBUG_ON_TELEM */ }; static const struct pios_led_cfg pios_led_v2_cfg = { - .leds = pios_leds_v2, - .num_leds = NELEMENTS(pios_leds_v2), + .leds = pios_leds_v2, + .num_leds = NELEMENTS(pios_leds_v2), }; -const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (uint32_t board_revision) +const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(uint32_t board_revision) { - switch(board_revision) { - case 2: - return &pios_led_cfg; - break; - case 3: - return &pios_led_v2_cfg; - break; - default: - PIOS_DEBUG_Assert(0); - } - return NULL; + switch (board_revision) { + case 2: + return &pios_led_cfg; + + break; + case 3: + return &pios_led_v2_cfg; + + break; + default: + PIOS_DEBUG_Assert(0); + } + return NULL; } -#endif /* PIOS_INCLUDE_LED */ +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_SPI) #include #if defined(PIOS_OVERO_SPI) -/* SPI2 Interface +/* SPI2 Interface * - Used for Flexi/IO/Overo communications - 3: PB12 = SPI2 NSS, CAN2 RX - 4: PB13 = SPI2 SCK, CAN2 TX, USART3 CTS - 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS - 6: PB15 = SPI2 MOSI, TIM12 CH2 + 3: PB12 = SPI2 NSS, CAN2 RX + 4: PB13 = SPI2 SCK, CAN2 TX, USART3 CTS + 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS + 6: PB15 = SPI2 MOSI, TIM12 CH2 */ #include void PIOS_OVERO_irq_handler(void); void DMA1_Stream7_IRQHandler(void) __attribute__((alias("PIOS_OVERO_irq_handler"))); static const struct pios_overo_cfg pios_overo_cfg = { - .regs = SPI2, - .remap = GPIO_AF_SPI2, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Hard, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, - }, - .use_crc = false, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case TX) - .flags = (DMA_IT_TCIF7), - .init = { - .NVIC_IRQChannel = DMA1_Stream7_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .regs = SPI2, + .remap = GPIO_AF_SPI2, + .init = { + .SPI_Mode = SPI_Mode_Slave, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Hard, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + }, + .use_crc = false, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case TX) + .flags = (DMA_IT_TCIF7), + .init = { + .NVIC_IRQChannel = DMA1_Stream7_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, - .rx = { - .channel = DMA1_Stream0, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_Medium, - //TODO: Enable FIFO - .DMA_FIFOMode = DMA_FIFOMode_Disable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_Single, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA1_Stream7, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI2->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_Single, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .slave_count = 1, - .ssel = { { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - } }, + .rx = { + .channel = DMA1_Stream0, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_Medium, + // TODO: Enable FIFO + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + .tx = { + .channel = DMA1_Stream7, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, }; uint32_t pios_overo_id = 0; void PIOS_OVERO_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_OVERO_DMA_irq_handler(pios_overo_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_OVERO_DMA_irq_handler(pios_overo_id); } #endif /* PIOS_OVERO_SPI */ -/* +/* * SPI1 Interface * Used for MPU6000 gyro and accelerometer */ @@ -347,116 +351,118 @@ void PIOS_SPI_gyro_irq_handler(void); void DMA2_Stream0_IRQHandler(void) __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); void DMA2_Stream3_IRQHandler(void) __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); static const struct pios_spi_cfg pios_spi_gyro_cfg = { - .regs = SPI1, - .remap = GPIO_AF_SPI1, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, - }, - .use_crc = false, - .dma = { - .irq = { - .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), - .init = { - .NVIC_IRQChannel = DMA2_Stream0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA2_Stream0, - .init = { + .regs = SPI1, + .remap = GPIO_AF_SPI1, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, + }, + .use_crc = false, + .dma = { + .irq = { + .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), + .init = { + .NVIC_IRQChannel = DMA2_Stream0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA2_Stream0, + .init = { .DMA_Channel = DMA_Channel_3, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, /* .DMA_FIFOThreshold */ .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA2_Stream3, - .init = { + }, + }, + .tx = { + .channel = DMA2_Stream3, + .init = { .DMA_Channel = DMA_Channel_3, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_FIFOMode = DMA_FIFOMode_Disable, /* .DMA_FIFOThreshold */ .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .miso = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .mosi = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .slave_count = 1, - .ssel = { { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - } } - } + }, + }, + }, + .sclk = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .miso = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .mosi = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + } + } + } }; static uint32_t pios_spi_gyro_id; void PIOS_SPI_gyro_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); } @@ -468,128 +474,130 @@ void PIOS_SPI_telem_flash_irq_handler(void); void DMA1_Stream0_IRQHandler(void) __attribute__((alias("PIOS_SPI_telem_flash_irq_handler"))); void DMA1_Stream5_IRQHandler(void) __attribute__((alias("PIOS_SPI_telem_flash_irq_handler"))); static const struct pios_spi_cfg pios_spi_telem_flash_cfg = { - .regs = SPI3, - .remap = GPIO_AF_SPI3, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_Low, - .SPI_CPHA = SPI_CPHA_1Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, - }, - .use_crc = false, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case RX) - .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), - .init = { - .NVIC_IRQChannel = DMA1_Stream0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Stream0, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - //TODO: Enable FIFO - .DMA_FIFOMode = DMA_FIFOMode_Disable, + .regs = SPI3, + .remap = GPIO_AF_SPI3, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_Low, + .SPI_CPHA = SPI_CPHA_1Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, + }, + .use_crc = false, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case RX) + .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), + .init = { + .NVIC_IRQChannel = DMA1_Stream0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Stream0, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + // TODO: Enable FIFO + .DMA_FIFOMode = DMA_FIFOMode_Disable, .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA1_Stream5, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, + }, + }, + .tx = { + .channel = DMA1_Stream5, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .miso = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .mosi = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .slave_count = 2, - .ssel = { - { // RFM22b - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - } }, - { // Flash - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - } }, - }, + }, + }, + }, + .sclk = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .miso = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .mosi = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .slave_count = 2, + .ssel = { + { // RFM22b + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + } + }, + { // Flash + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + } + }, + }, }; uint32_t pios_spi_telem_flash_id; void PIOS_SPI_telem_flash_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_telem_flash_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_telem_flash_id); } @@ -597,65 +605,67 @@ void PIOS_SPI_telem_flash_irq_handler(void) #include static const struct pios_exti_cfg pios_exti_rfm22b_cfg __exti_config = { - .vector = PIOS_RFM22_EXT_Int, - .line = EXTI_Line2, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line2, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Falling, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_RFM22_EXT_Int, + .line = EXTI_Line2, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line2, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Falling, + .EXTI_LineCmd = ENABLE, + }, + }, }; const struct pios_rfm22b_cfg pios_rfm22b_rm1_cfg = { - .spi_cfg = &pios_spi_telem_flash_cfg, - .exti_cfg = &pios_exti_rfm22b_cfg, - .RFXtalCap = 0x7f, - .slave_num = 0, - .gpio_direction = GPIO0_RX_GPIO1_TX, + .spi_cfg = &pios_spi_telem_flash_cfg, + .exti_cfg = &pios_exti_rfm22b_cfg, + .RFXtalCap = 0x7f, + .slave_num = 0, + .gpio_direction = GPIO0_RX_GPIO1_TX, }; const struct pios_rfm22b_cfg pios_rfm22b_rm2_cfg = { - .spi_cfg = &pios_spi_telem_flash_cfg, - .exti_cfg = &pios_exti_rfm22b_cfg, - .RFXtalCap = 0x7f, - .slave_num = 0, - .gpio_direction = GPIO0_TX_GPIO1_RX, + .spi_cfg = &pios_spi_telem_flash_cfg, + .exti_cfg = &pios_exti_rfm22b_cfg, + .RFXtalCap = 0x7f, + .slave_num = 0, + .gpio_direction = GPIO0_TX_GPIO1_RX, }; -const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (uint32_t board_revision) +const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22Cfg(uint32_t board_revision) { - switch(board_revision) { - case 2: - return &pios_rfm22b_rm1_cfg; - break; - case 3: - return &pios_rfm22b_rm2_cfg; - break; - default: - PIOS_DEBUG_Assert(0); - } - return NULL; + switch (board_revision) { + case 2: + return &pios_rfm22b_rm1_cfg; + + break; + case 3: + return &pios_rfm22b_rm2_cfg; + + break; + default: + PIOS_DEBUG_Assert(0); + } + return NULL; } #endif /* PIOS_INCLUDE_RFM22B */ @@ -668,31 +678,31 @@ const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (uint32_t board_re #include "pios_flash_internal_priv.h" static const struct flashfs_logfs_cfg flashfs_external_cfg = { - .fs_magic = 0x99abceef, - .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ - .arena_size = 0x00010000, /* 256 * slot size */ - .slot_size = 0x00000100, /* 256 bytes */ + .fs_magic = 0x99abceef, + .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ + .arena_size = 0x00010000, /* 256 * slot size */ + .slot_size = 0x00000100, /* 256 bytes */ - .start_offset = 0, /* start at the beginning of the chip */ - .sector_size = 0x00010000, /* 64K bytes */ - .page_size = 0x00000100, /* 256 bytes */ + .start_offset = 0, /* start at the beginning of the chip */ + .sector_size = 0x00010000, /* 64K bytes */ + .page_size = 0x00000100, /* 256 bytes */ }; -static const struct pios_flash_internal_cfg flash_internal_cfg = { }; +static const struct pios_flash_internal_cfg flash_internal_cfg = {}; static const struct flashfs_logfs_cfg flashfs_internal_cfg = { -.fs_magic = 0x99abcfef, -.total_fs_size = EE_BANK_SIZE, /* 32K bytes (2x16KB sectors) */ -.arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 1 sector */ -.slot_size = 0x00000100, /* 256 bytes */ + .fs_magic = 0x99abcfef, + .total_fs_size = EE_BANK_SIZE, /* 32K bytes (2x16KB sectors) */ + .arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 1 sector */ + .slot_size = 0x00000100, /* 256 bytes */ -.start_offset = EE_BANK_BASE, /* start after the bootloader */ -.sector_size = 0x00004000, /* 16K bytes */ -.page_size = 0x00004000, /* 16K bytes */ + .start_offset = EE_BANK_BASE, /* start after the bootloader */ + .sector_size = 0x00004000, /* 16K bytes */ + .page_size = 0x00004000, /* 16K bytes */ }; -#endif /* PIOS_INCLUDE_FLASH */ +#endif /* PIOS_INCLUDE_FLASH */ #include @@ -702,44 +712,44 @@ static const struct flashfs_logfs_cfg flashfs_internal_cfg = { * MAIN USART */ static const struct pios_usart_cfg pios_usart_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART1, + .remap = GPIO_AF_USART1, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_INCLUDE_COM_TELEM */ @@ -747,131 +757,131 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { #include "pios_dsm_priv.h" static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART1, + .remap = GPIO_AF_USART1, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; // Because of the inverter on the main port this will not // work. Notice the mode is set to IN to maintain API // compatibility but protect the pins static const struct pios_dsm_cfg pios_dsm_main_cfg = { - .bind = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, + .bind = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, }; -#endif /* PIOS_INCLUDE_DSM */ +#endif /* PIOS_INCLUDE_DSM */ #include #if defined(PIOS_INCLUDE_SBUS) - /* - * S.Bus USART - */ +/* + * S.Bus USART + */ #include static const struct pios_usart_cfg pios_usart_sbus_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 100000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_Even, - .USART_StopBits = USART_StopBits_2, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, + .regs = USART1, + .remap = GPIO_AF_USART1, + .init = { + .USART_BaudRate = 100000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_Even, + .USART_StopBits = USART_StopBits_2, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, }; -#endif /* PIOS_INCLUDE_SBUS */ +#endif /* PIOS_INCLUDE_SBUS */ // Need this defined regardless to be able to turn it off static const struct pios_sbus_cfg pios_sbus_cfg = { - /* Inverter configuration */ - .inv = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .gpio_inv_enable = Bit_SET, - .gpio_inv_disable = Bit_RESET, - .gpio_clk_func = RCC_AHB1PeriphClockCmd, - .gpio_clk_periph = RCC_AHB1Periph_GPIOC, + /* Inverter configuration */ + .inv = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .gpio_inv_enable = Bit_SET, + .gpio_inv_disable = Bit_RESET, + .gpio_clk_func = RCC_AHB1PeriphClockCmd, + .gpio_clk_periph = RCC_AHB1Periph_GPIOC, }; @@ -880,45 +890,45 @@ static const struct pios_sbus_cfg pios_sbus_cfg = { * FLEXI PORT */ static const struct pios_usart_cfg pios_usart_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART3, + .remap = GPIO_AF_USART3, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_INCLUDE_COM_FLEXI */ @@ -927,144 +937,144 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { #include "pios_dsm_priv.h" static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART3, + .remap = GPIO_AF_USART3, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, + .bind = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, }; -#endif /* PIOS_INCLUDE_DSM */ +#endif /* PIOS_INCLUDE_DSM */ /* * HK OSD */ static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART1, + .remap = GPIO_AF_USART1, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART3, + .remap = GPIO_AF_USART3, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #if defined(PIOS_INCLUDE_COM) @@ -1083,144 +1093,144 @@ static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { void PIOS_I2C_mag_pressure_adapter_ev_irq_handler(void); void PIOS_I2C_mag_pressureadapter_er_irq_handler(void); void I2C1_EV_IRQHandler() - __attribute__ ((alias("PIOS_I2C_mag_pressure_adapter_ev_irq_handler"))); +__attribute__((alias("PIOS_I2C_mag_pressure_adapter_ev_irq_handler"))); void I2C1_ER_IRQHandler() - __attribute__ ((alias("PIOS_I2C_mag_pressure_adapter_er_irq_handler"))); +__attribute__((alias("PIOS_I2C_mag_pressure_adapter_er_irq_handler"))); static const struct pios_i2c_adapter_cfg pios_i2c_mag_pressure_adapter_cfg = { - .regs = I2C1, - .remap = GPIO_AF_I2C1, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, + .regs = I2C1, + .remap = GPIO_AF_I2C1, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_8, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C1_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C1_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; uint32_t pios_i2c_mag_pressure_adapter_id; void PIOS_I2C_mag_pressure_adapter_ev_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_mag_pressure_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_mag_pressure_adapter_id); } void PIOS_I2C_mag_pressure_adapter_er_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_mag_pressure_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_mag_pressure_adapter_id); } void PIOS_I2C_flexiport_adapter_ev_irq_handler(void); void PIOS_I2C_flexiport_adapter_er_irq_handler(void); -void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexiport_adapter_ev_irq_handler"))); -void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexiport_adapter_er_irq_handler"))); +void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_ev_irq_handler"))); +void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_er_irq_handler"))); static const struct pios_i2c_adapter_cfg pios_i2c_flexiport_adapter_cfg = { - .regs = I2C2, - .remap = GPIO_AF_I2C2, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, + .regs = I2C2, + .remap = GPIO_AF_I2C2, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; uint32_t pios_i2c_flexiport_adapter_id; void PIOS_I2C_flexiport_adapter_ev_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexiport_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexiport_adapter_id); } void PIOS_I2C_flexiport_adapter_er_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexiport_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexiport_adapter_id); } @@ -1235,182 +1245,182 @@ void PIOS_I2C_pressure_adapter_er_irq_handler(void); */ #include -void PIOS_RTC_IRQ_Handler (void); -void RTC_WKUP_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler"))); +void PIOS_RTC_IRQ_Handler(void); +void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); static const struct pios_rtc_cfg pios_rtc_main_cfg = { - .clksrc = RCC_RTCCLKSource_HSE_Div8, // Divide 8 Mhz crystal down to 1 - // For some reason it's acting like crystal is 16 Mhz. This clock is then divided - // by another 16 to give a nominal 62.5 khz clock - .prescaler = 100, // Every 100 cycles gives 625 Hz - .irq = { - .init = { - .NVIC_IRQChannel = RTC_WKUP_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .clksrc = RCC_RTCCLKSource_HSE_Div8, // Divide 8 Mhz crystal down to 1 + // For some reason it's acting like crystal is 16 Mhz. This clock is then divided + // by another 16 to give a nominal 62.5 khz clock + .prescaler = 100, // Every 100 cycles gives 625 Hz + .irq = { + .init = { + .NVIC_IRQChannel = RTC_WKUP_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; -void PIOS_RTC_IRQ_Handler (void) +void PIOS_RTC_IRQ_Handler(void) { - PIOS_RTC_irq_handler (); + PIOS_RTC_irq_handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_RTC) */ #include "pios_tim_priv.h" static const TIM_TimeBaseInitTypeDef tim_3_5_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, }; static const TIM_TimeBaseInitTypeDef tim_9_10_11_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, }; static const struct pios_tim_clock_cfg tim_3_cfg = { - .timer = TIM3, - .time_base_init = &tim_3_5_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM3, + .time_base_init = &tim_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_5_cfg = { - .timer = TIM5, - .time_base_init = &tim_3_5_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM5, + .time_base_init = &tim_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_9_cfg = { - .timer = TIM9, - .time_base_init = &tim_9_10_11_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_BRK_TIM9_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM9, + .time_base_init = &tim_9_10_11_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_BRK_TIM9_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_10_cfg = { - .timer = TIM10, - .time_base_init = &tim_9_10_11_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_UP_TIM10_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM10, + .time_base_init = &tim_9_10_11_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_UP_TIM10_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_11_cfg = { - .timer = TIM11, - .time_base_init = &tim_9_10_11_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_TRG_COM_TIM11_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM11, + .time_base_init = &tim_9_10_11_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_TRG_COM_TIM11_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; // Set up timers that only have inputs on APB1 // TIM2,3,4,5,6,7,12,13,14 static const TIM_TimeBaseInitTypeDef tim_apb1_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = 0xFFFF, + .TIM_RepetitionCounter = 0x0000, }; // Set up timers that only have inputs on APB2 // TIM1,8,9,10,11 static const TIM_TimeBaseInitTypeDef tim_apb2_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = 0xFFFF, + .TIM_RepetitionCounter = 0x0000, }; static const struct pios_tim_clock_cfg tim_1_cfg = { - .timer = TIM1, - .time_base_init = &tim_apb2_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM1, + .time_base_init = &tim_apb2_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_4_cfg = { - .timer = TIM4, - .time_base_init = &tim_apb1_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM4, + .time_base_init = &tim_apb1_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_8_cfg = { - .timer = TIM8, - .time_base_init = &tim_apb2_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM8_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM8, + .time_base_init = &tim_apb2_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_12_cfg = { - .timer = TIM12, - .time_base_init = &tim_apb1_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM8_BRK_TIM12_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM12, + .time_base_init = &tim_apb1_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_BRK_TIM12_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; @@ -1420,258 +1430,258 @@ static const struct pios_tim_clock_cfg tim_12_cfg = { */ #include static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource0, - }, - .remap = GPIO_AF_TIM3, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource1, - }, - .remap = GPIO_AF_TIM3, - }, - { - .timer = TIM9, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource3, - }, - .remap = GPIO_AF_TIM9, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource2, - }, - .remap = GPIO_AF_TIM2, - }, - { - .timer = TIM5, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource1, - }, - .remap = GPIO_AF_TIM5, - }, - { - .timer = TIM5, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource0, - }, - .remap = GPIO_AF_TIM5, - }, - // PWM pins on FlexiIO(receiver) port - { - // * 6: PB15 = SPI2 MOSI, TIM12 CH2 + { + .timer = TIM3, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource0, + }, + .remap = GPIO_AF_TIM3, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource1, + }, + .remap = GPIO_AF_TIM3, + }, + { + .timer = TIM9, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource3, + }, + .remap = GPIO_AF_TIM9, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource2, + }, + .remap = GPIO_AF_TIM2, + }, + { + .timer = TIM5, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource1, + }, + .remap = GPIO_AF_TIM5, + }, + { + .timer = TIM5, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource0, + }, + .remap = GPIO_AF_TIM5, + }, + // PWM pins on FlexiIO(receiver) port + { + // * 6: PB15 = SPI2 MOSI, TIM12 CH2 - .timer = TIM12, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource15, - }, - .remap = GPIO_AF_TIM12, - }, - { - // * 7: PC6 = TIM8 CH1, USART6 TX - .timer = TIM8, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource6, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 8: PC7 = TIM8 CH2, USART6 RX - .timer = TIM8, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource7, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 9: PC8 = TIM8 CH3 - .timer = TIM8, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource8, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 10: PC9 = TIM8 CH4 - .timer = TIM8, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource9, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS - .timer = TIM12, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource14, - }, - .remap = GPIO_AF_TIM12, - }, + .timer = TIM12, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource15, + }, + .remap = GPIO_AF_TIM12, + }, + { + // * 7: PC6 = TIM8 CH1, USART6 TX + .timer = TIM8, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource6, + }, + .remap = GPIO_AF_TIM8, + }, + + { + // * 8: PC7 = TIM8 CH2, USART6 RX + .timer = TIM8, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource7, + }, + .remap = GPIO_AF_TIM8, + }, + + { + // * 9: PC8 = TIM8 CH3 + .timer = TIM8, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource8, + }, + .remap = GPIO_AF_TIM8, + }, + + { + // * 10: PC9 = TIM8 CH4 + .timer = TIM8, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource9, + }, + .remap = GPIO_AF_TIM8, + }, + + { + // * 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS + .timer = TIM12, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource14, + }, + .remap = GPIO_AF_TIM12, + }, }; -#define PIOS_SERVOPORT_ALL_PINS_PWMOUT 6 +#define PIOS_SERVOPORT_ALL_PINS_PWMOUT 6 #define PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN_PPM 11 -#define PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN 12 +#define PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN 12 const struct pios_servo_cfg pios_servo_cfg_out = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_all_pins, - .num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT, + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_all_pins, + .num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT, }; // All servo outputs, servo input ch1 ppm, ch2-6 outputs const struct pios_servo_cfg pios_servo_cfg_out_in_ppm = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_all_pins, - .num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN_PPM, + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_all_pins, + .num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN_PPM, }; // All servo outputs, servo inputs ch1-6 Outputs const struct pios_servo_cfg pios_servo_cfg_out_in = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_all_pins, - .num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN, + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_all_pins, + .num_channels = PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN, }; @@ -1682,127 +1692,127 @@ const struct pios_servo_cfg pios_servo_cfg_out_in = { #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) #include static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { - { - .timer = TIM12, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource14, - }, - .remap = GPIO_AF_TIM12, - }, - { - .timer = TIM12, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource15, - }, - .remap = GPIO_AF_TIM12, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource6, - }, - .remap = GPIO_AF_TIM8, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource7, - }, - .remap = GPIO_AF_TIM8, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource8, - }, - .remap = GPIO_AF_TIM8, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource9, - }, - .remap = GPIO_AF_TIM8, - }, + { + .timer = TIM12, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource14, + }, + .remap = GPIO_AF_TIM12, + }, + { + .timer = TIM12, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource15, + }, + .remap = GPIO_AF_TIM12, + }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource6, + }, + .remap = GPIO_AF_TIM8, + }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource7, + }, + .remap = GPIO_AF_TIM8, + }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource8, + }, + .remap = GPIO_AF_TIM8, + }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource9, + }, + .remap = GPIO_AF_TIM8, + }, }; const struct pios_pwm_cfg pios_pwm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = pios_tim_rcvrport_all_channels, - .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_rcvrport_all_channels, + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), }; -// this configures outputs 2-6 as pwm inputs +// this configures outputs 2-6 as pwm inputs const struct pios_pwm_cfg pios_pwm_ppm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = &pios_tim_rcvrport_all_channels[1], - .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_rcvrport_all_channels[1], + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1, }; -#endif +#endif /* if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) */ /* * PPM Input @@ -1810,23 +1820,23 @@ const struct pios_pwm_cfg pios_pwm_ppm_cfg = { #if defined(PIOS_INCLUDE_PPM) #include static const struct pios_ppm_cfg pios_ppm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - .TIM_Channel = TIM_Channel_1, - }, - /* Use only the first channel for ppm */ - .channels = &pios_tim_rcvrport_all_channels[0], - .num_channels = 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + .TIM_Channel = TIM_Channel_1, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_rcvrport_all_channels[0], + .num_channels = 1, }; -#endif //PPM +#endif // PPM #if defined(PIOS_INCLUDE_GCSRCVR) #include "pios_gcsrcvr_priv.h" -#endif /* PIOS_INCLUDE_GCSRCVR */ +#endif /* PIOS_INCLUDE_GCSRCVR */ #if defined(PIOS_INCLUDE_RCVR) #include "pios_rcvr_priv.h" @@ -1836,60 +1846,62 @@ static const struct pios_ppm_cfg pios_ppm_cfg = { #include "pios_usb_priv.h" static const struct pios_usb_cfg pios_usb_main_rm1_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = OTG_FS_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 3, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_25MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - }, + .irq = { + .init = { + .NVIC_IRQChannel = OTG_FS_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 3, + .NVIC_IRQChannelCmd = ENABLE, }, - .vsense_active_low = false + }, + .vsense = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_25MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + }, + }, + .vsense_active_low = false }; static const struct pios_usb_cfg pios_usb_main_rm2_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = OTG_FS_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 3, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_25MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - }, + .irq = { + .init = { + .NVIC_IRQChannel = OTG_FS_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 3, + .NVIC_IRQChannelCmd = ENABLE, }, - .vsense_active_low = false + }, + .vsense = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_25MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + }, + }, + .vsense_active_low = false }; -const struct pios_usb_cfg * PIOS_BOARD_HW_DEFS_GetUsbCfg (uint32_t board_revision) +const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(uint32_t board_revision) { - switch(board_revision) { - case 2: - return &pios_usb_main_rm1_cfg; - break; - case 3: - return &pios_usb_main_rm2_cfg; - break; - default: - PIOS_DEBUG_Assert(0); - } - return NULL; + switch (board_revision) { + case 2: + return &pios_usb_main_rm1_cfg; + + break; + case 3: + return &pios_usb_main_rm2_cfg; + + break; + default: + PIOS_DEBUG_Assert(0); + } + return NULL; } #include "pios_usb_board_data_priv.h" @@ -1897,7 +1909,7 @@ const struct pios_usb_cfg * PIOS_BOARD_HW_DEFS_GetUsbCfg (uint32_t board_revisio #include "pios_usb_desc_hid_only_priv.h" #include "pios_usbhook.h" -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ #if defined(PIOS_INCLUDE_COM_MSG) @@ -1909,9 +1921,9 @@ const struct pios_usb_cfg * PIOS_BOARD_HW_DEFS_GetUsbCfg (uint32_t board_revisio #include const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, + .data_if = 0, + .data_rx_ep = 1, + .data_tx_ep = 1, }; #endif /* PIOS_INCLUDE_USB_HID */ @@ -1919,11 +1931,11 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { #include const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 1, - .ctrl_tx_ep = 2, + .ctrl_if = 1, + .ctrl_tx_ep = 2, - .data_if = 2, - .data_rx_ep = 3, - .data_tx_ep = 3, + .data_if = 2, + .data_rx_ep = 3, + .data_tx_ep = 3, }; -#endif /* PIOS_INCLUDE_USB_CDC */ +#endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/targets/boards/revolution/bootloader/inc/common.h b/flight/targets/boards/revolution/bootloader/inc/common.h index 9010fddd9..e64b89014 100644 --- a/flight/targets/boards/revolution/bootloader/inc/common.h +++ b/flight/targets/boards/revolution/bootloader/inc/common.h @@ -28,10 +28,10 @@ #ifndef COMMON_H_ #define COMMON_H_ -//#include "board.h" +// #include "board.h" typedef enum { - start, keepgoing, + start, keepgoing, } DownloadAction; /**************************************************/ @@ -39,77 +39,77 @@ typedef enum { /**************************************************/ typedef enum { - DFUidle, //0 - uploading, //1 - wrong_packet_received, //2 - too_many_packets, //3 - too_few_packets, //4 - Last_operation_Success, //5 - downloading, //6 - BLidle, //7 - Last_operation_failed, //8 - uploadingStarting, //9 - outsideDevCapabilities, //10 - CRC_Fail,//11 - failed_jump, -//12 + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + BLidle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, +// 12 } DFUStates; /**************************************************/ /* OP_DFU commands */ /**************************************************/ typedef enum { - Reserved, //0 - Req_Capabilities, //1 - Rep_Capabilities, //2 - EnterDFU, //3 - JumpFW, //4 - Reset, //5 - Abort_Operation, //6 - Upload, //7 - Op_END, //8 - Download_Req, //9 - Download, //10 - Status_Request, //11 - Status_Rep -//12 + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep +// 12 } DFUCommands; typedef enum { - High_Density, Medium_Density + High_Density, Medium_Density } DeviceType; /**************************************************/ /* OP_DFU transfer types */ /**************************************************/ typedef enum { - FW, //0 - Descript -//2 + FW, // 0 + Descript +// 2 } DFUTransfer; /**************************************************/ /* OP_DFU transfer port */ /**************************************************/ typedef enum { - Usb, //0 - Serial -//2 + Usb, // 0 + Serial +// 2 } DFUPort; /**************************************************/ /* OP_DFU programable programable HW types */ /**************************************************/ typedef enum { - Self_flash, //0 - Remote_flash_via_spi -//1 + Self_flash, // 0 + Remote_flash_via_spi +// 1 } DFUProgType; /**************************************************/ /* OP_DFU programable sources */ /**************************************************/ -#define USB 0 -#define SPI 1 +#define USB 0 +#define SPI 1 -#define DownloadDelay 100000 +#define DownloadDelay 100000 -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 +#define MAX_DEL_RETRYS 3 +#define MAX_WRI_RETRYS 3 #endif /* COMMON_H_ */ diff --git a/flight/targets/boards/revolution/bootloader/inc/pios_config.h b/flight/targets/boards/revolution/bootloader/inc/pios_config.h index 86fedc699..d3b203d2e 100644 --- a/flight/targets/boards/revolution/bootloader/inc/pios_config.h +++ b/flight/targets/boards/revolution/bootloader/inc/pios_config.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * - Central compile time config for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/revolution/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/revolution/bootloader/inc/pios_usb_board_data.h index eb00a55be..4e9c7c063 100644 --- a/flight/targets/boards/revolution/bootloader/inc/pios_usb_board_data.h +++ b/flight/targets/boards/revolution/bootloader/inc/pios_usb_board_data.h @@ -33,13 +33,13 @@ #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 2 +#define PIOS_USB_BOARD_EP_NUM 2 -#include /* struct usb_* */ +#include /* struct usb_* */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_BL) -#define PIOS_USB_BOARD_SN_SUFFIX "+BL" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" /* * The bootloader uses a simplified report structure @@ -49,4 +49,4 @@ */ #define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/revolution/bootloader/main.c b/flight/targets/boards/revolution/bootloader/main.c index b1e408c6a..033f14a6c 100644 --- a/flight/targets/boards/revolution/bootloader/main.c +++ b/flight/targets/boards/revolution/bootloader/main.c @@ -8,21 +8,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief This is the file with the main function of the Revolution BootLoader * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,7 +32,7 @@ #include #include #include -#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USBHOOK_* */ #include /* Prototype of PIOS_Board_Init() function */ @@ -61,10 +61,10 @@ uint8_t tempcount = 0; /* Extern variables ----------------------------------------------------------*/ DFUStates DeviceState; -int16_t status = 0; -bool JumpToApp = false; -bool GO_dfu = false; -bool USB_connected = false; +int16_t status = 0; +bool JumpToApp = false; +bool GO_dfu = false; +bool USB_connected = false; bool User_DFU_request = false; static uint8_t mReceive_Buffer[63]; /* Private function prototypes -----------------------------------------------*/ @@ -72,146 +72,160 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); uint8_t processRX(); void jump_to_app(); -int main() { - PIOS_SYS_Init(); - PIOS_Board_Init(); - PIOS_IAP_Init(); +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); - // Make sure the brown out reset value for this chip - // is 2.7 volts - check_bor(); + // Make sure the brown out reset value for this chip + // is 2.7 volts + check_bor(); - USB_connected = PIOS_USB_CheckAvailable(0); + USB_connected = PIOS_USB_CheckAvailable(0); - if (PIOS_IAP_CheckRequest() == true) { - PIOS_DELAY_WaitmS(1000); - User_DFU_request = true; - PIOS_IAP_ClearRequest(); - } + if (PIOS_IAP_CheckRequest() == true) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = true; + PIOS_IAP_ClearRequest(); + } - GO_dfu = (USB_connected == true) || (User_DFU_request == true); + GO_dfu = (USB_connected == true) || (User_DFU_request == true); - if (GO_dfu == true) { - if (User_DFU_request == true) - DeviceState = DFUidle; - else - DeviceState = BLidle; - } else - JumpToApp = true; + if (GO_dfu == true) { + if (User_DFU_request == true) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = true; + } - uint32_t stopwatch = 0; - uint32_t prev_ticks = PIOS_DELAY_GetuS(); - while (true) { - /* Update the stopwatch */ - uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); - prev_ticks += elapsed_ticks; - stopwatch += elapsed_ticks; + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (true) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; - if (JumpToApp == true) - jump_to_app(); + if (JumpToApp == true) { + jump_to_app(); + } - switch (DeviceState) { - case Last_operation_Success: - case uploadingStarting: - case DFUidle: - period1 = 5000; - sweep_steps1 = 100; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case uploading: - period1 = 5000; - sweep_steps1 = 100; - period2 = 2500; - sweep_steps2 = 50; - break; - case downloading: - period1 = 2500; - sweep_steps1 = 50; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case BLidle: - period1 = 0; - PIOS_LED_On(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - default://error - period1 = 5000; - sweep_steps1 = 100; - period2 = 5000; - sweep_steps2 = 100; - } + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } - if (period1 != 0) { - if (LedPWM(period1, sweep_steps1, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_On(PIOS_LED_HEARTBEAT); + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } - if (period2 != 0) { - if (LedPWM(period2, sweep_steps2, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } - if (stopwatch > 50 * 1000 * 1000) - stopwatch = 0; - if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) - JumpToApp = true; + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) { + JumpToApp = true; + } - processRX(); - DataDownload(start); - } + processRX(); + DataDownload(start); + } } -void jump_to_app() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; - PIOS_LED_On(PIOS_LED_HEARTBEAT); - // Look at cm3_vectors struct in startup. In a fw image the first uint32_t contains the address of the top of irqstack - uint32_t fwIrqStackBase = (*(__IO uint32_t*) bdinfo->fw_base) & 0xFFFE0000; - // Check for the two possible irqstack locations (sram or core coupled sram) - if ( fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { - /* Jump to user application */ - FLASH_Lock(); - RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + PIOS_LED_On(PIOS_LED_HEARTBEAT); + // Look at cm3_vectors struct in startup. In a fw image the first uint32_t contains the address of the top of irqstack + uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000; + // Check for the two possible irqstack locations (sram or core coupled sram) + if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { + /* Jump to user application */ + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - PIOS_USBHOOK_Deactivate(); + PIOS_USBHOOK_Deactivate(); - JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); - Jump_To_Application = (pFunction) JumpAddress; - /* Initialize user application's Stack Pointer */ - __set_MSP(*(__IO uint32_t*) bdinfo->fw_base); - Jump_To_Application(); - } else { - DeviceState = failed_jump; - return; - } + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + return; + } } -uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { - uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ - uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ - uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ - if (curr_sweep & 1) { - pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ - } - return ((count % pwm_period) > pwm_duty) ? 1 : 0; + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; } -uint8_t processRX() { - if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { - processComand(mReceive_Buffer); - } - return true; +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return true; } /** @@ -221,14 +235,18 @@ uint8_t processRX() { */ void check_bor() { - uint8_t bor = FLASH_OB_GetBOR(); - if(bor != OB_BOR_LEVEL3) { - FLASH_OB_Unlock(); - FLASH_OB_BORConfig(OB_BOR_LEVEL3); - FLASH_OB_Launch(); - while(FLASH_WaitForLastOperation() == FLASH_BUSY); - FLASH_OB_Lock(); - while(FLASH_WaitForLastOperation() == FLASH_BUSY); - } -} + uint8_t bor = FLASH_OB_GetBOR(); + if (bor != OB_BOR_LEVEL3) { + FLASH_OB_Unlock(); + FLASH_OB_BORConfig(OB_BOR_LEVEL3); + FLASH_OB_Launch(); + while (FLASH_WaitForLastOperation() == FLASH_BUSY) { + ; + } + FLASH_OB_Lock(); + while (FLASH_WaitForLastOperation() == FLASH_BUSY) { + ; + } + } +} diff --git a/flight/targets/boards/revolution/bootloader/pios_board.c b/flight/targets/boards/revolution/bootloader/pios_board.c index 5b2280389..2256f4785 100644 --- a/flight/targets/boards/revolution/bootloader/pios_board.c +++ b/flight/targets/boards/revolution/bootloader/pios_board.c @@ -7,19 +7,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,45 +39,46 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; -void PIOS_Board_Init() { - if (board_init_complete) { - return; - } +void PIOS_Board_Init() +{ + if (board_init_complete) { + return; + } + + /* Delay system */ + PIOS_DELAY_Init(); + + const struct pios_board_info *bdinfo = &pios_board_info_blob; - /* Delay system */ - PIOS_DELAY_Init(); - - const struct pios_board_info * bdinfo = &pios_board_info_blob; - #if defined(PIOS_INCLUDE_LED) - const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); - PIOS_Assert(led_cfg); - PIOS_LED_Init(led_cfg); -#endif /* PIOS_INCLUDE_LED */ + const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Activate the HID-only USB configuration */ - PIOS_USB_DESC_HID_ONLY_Init(); + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { - PIOS_Assert(0); - } -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ - PIOS_USBHOOK_Activate(); + PIOS_USBHOOK_Activate(); -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ - board_init_complete = true; + board_init_complete = true; } diff --git a/flight/targets/boards/revolution/firmware/cm3_fault_handlers.c b/flight/targets/boards/revolution/firmware/cm3_fault_handlers.c index e59e3b79a..e74ffa76a 100644 --- a/flight/targets/boards/revolution/firmware/cm3_fault_handlers.c +++ b/flight/targets/boards/revolution/firmware/cm3_fault_handlers.c @@ -14,34 +14,34 @@ # include #endif -#define FAULT_TRAMPOLINE(_vec) \ -__attribute__((naked, no_instrument_function)) \ -void \ -_vec##_Handler(void) \ -{ \ - __asm( ".syntax unified\n" \ - "MOVS R0, #4 \n" \ - "MOV R1, LR \n" \ - "TST R0, R1 \n" \ - "BEQ 1f \n" \ - "MRS R0, PSP \n" \ - "B " #_vec "_Handler2 \n" \ - "1: \n" \ - "MRS R0, MSP \n" \ - "B " #_vec "_Handler2 \n" \ - ".syntax divided\n"); \ -} \ -struct hack +#define FAULT_TRAMPOLINE(_vec) \ + __attribute__((naked, no_instrument_function)) \ + void \ + _vec##_Handler(void) \ + { \ + __asm(".syntax unified\n" \ + "MOVS R0, #4 \n" \ + "MOV R1, LR \n" \ + "TST R0, R1 \n" \ + "BEQ 1f \n" \ + "MRS R0, PSP \n" \ + "B " #_vec "_Handler2 \n" \ + "1: \n" \ + "MRS R0, MSP \n" \ + "B " #_vec "_Handler2 \n" \ + ".syntax divided\n"); \ + } \ + struct hack struct cm3_frame { - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t pc; - uint32_t psr; + uint32_t r0; + uint32_t r1; + uint32_t r2; + uint32_t r3; + uint32_t r12; + uint32_t lr; + uint32_t pc; + uint32_t psr; }; FAULT_TRAMPOLINE(HardFault); @@ -49,39 +49,42 @@ FAULT_TRAMPOLINE(BusFault); FAULT_TRAMPOLINE(UsageFault); /* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */ -#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) +#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) -void -HardFault_Handler2(struct cm3_frame *frame) +void HardFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nHARD FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(HFSR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nHARD FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(HFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } } -void -BusFault_Handler2(struct cm3_frame *frame) +void BusFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nBUS FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(BFAR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nBUS FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(BFAR)); + dbg_write_char('\n'); + for (;;) { + ; + } } -void -UsageFault_Handler2(struct cm3_frame *frame) +void UsageFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nUSAGE FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nUSAGE FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } } diff --git a/flight/targets/boards/revolution/firmware/dcc_stdio.c b/flight/targets/boards/revolution/firmware/dcc_stdio.c index 102dd70a1..1a522e9a0 100644 --- a/flight/targets/boards/revolution/firmware/dcc_stdio.c +++ b/flight/targets/boards/revolution/firmware/dcc_stdio.c @@ -1,33 +1,33 @@ /*************************************************************************** - * Copyright (C) 2008 by Dominic Rath * - * Dominic.Rath@gmx.de * - * Copyright (C) 2008 by Spencer Oliver * - * spen@spen-soft.co.uk * - * Copyright (C) 2008 by Frederik Kriewtz * - * frederik@kriewitz.eu * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* Copyright (C) 2008 by Frederik Kriewtz * +* frederik@kriewitz.eu * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ #include "inc/dcc_stdio.h" -#define TARGET_REQ_TRACEMSG 0x00 -#define TARGET_REQ_DEBUGMSG_ASCII 0x01 -#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) -#define TARGET_REQ_DEBUGCHAR 0x02 +#define TARGET_REQ_TRACEMSG 0x00 +#define TARGET_REQ_DEBUGMSG_ASCII 0x01 +#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) +#define TARGET_REQ_DEBUGCHAR 0x02 /* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel * DCRDR[7:0] is used by target for status @@ -35,115 +35,115 @@ * DCRDR[23:16] is used for by host for status * DCRDR[31:24] is used for by host for write buffer */ -#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) +#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) -#define BUSY 1 +#define BUSY 1 void dbg_write(unsigned long dcc_data) { - int len = 4; + int len = 4; - while (len--) - { - /* wait for data ready */ - while (NVIC_DBG_DATA_R & BUSY); + while (len--) { + /* wait for data ready */ + while (NVIC_DBG_DATA_R & BUSY) { + ; + } - /* write our data and set write flag - tell host there is data*/ - NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); - dcc_data >>= 8; - } + /* write our data and set write flag - tell host there is data*/ + NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); + dcc_data >>= 8; + } } void dbg_trace_point(unsigned long number) { - dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); + dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); } void dbg_write_u32(const unsigned long *val, long len) { - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); - while (len > 0) - { - dbg_write(*val); + while (len > 0) { + dbg_write(*val); - val++; - len--; - } + val++; + len--; + } } void dbg_write_u16(const unsigned short *val, long len) { - unsigned long dcc_data; + unsigned long dcc_data; - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = val[0] - | ((len > 1) ? val[1] << 16: 0x0000); + while (len > 0) { + dcc_data = val[0] + | ((len > 1) ? val[1] << 16 : 0x0000); - dbg_write(dcc_data); + dbg_write(dcc_data); - val += 2; - len -= 2; - } + val += 2; + len -= 2; + } } void dbg_write_u8(const unsigned char *val, long len) { - unsigned long dcc_data; + unsigned long dcc_data; - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = val[0] - | ((len > 1) ? val[1] << 8 : 0x00) - | ((len > 2) ? val[2] << 16 : 0x00) - | ((len > 3) ? val[3] << 24 : 0x00); + while (len > 0) { + dcc_data = val[0] + | ((len > 1) ? val[1] << 8 : 0x00) + | ((len > 2) ? val[2] << 16 : 0x00) + | ((len > 3) ? val[3] << 24 : 0x00); - dbg_write(dcc_data); + dbg_write(dcc_data); - val += 4; - len -= 4; - } + val += 4; + len -= 4; + } } void dbg_write_str(const char *msg) { - long len; - unsigned long dcc_data; + long len; + unsigned long dcc_data; - for (len = 0; msg[len] && (len < 65536); len++); + for (len = 0; msg[len] && (len < 65536); len++) { + ; + } - dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = msg[0] - | ((len > 1) ? msg[1] << 8 : 0x00) - | ((len > 2) ? msg[2] << 16 : 0x00) - | ((len > 3) ? msg[3] << 24 : 0x00); - dbg_write(dcc_data); + while (len > 0) { + dcc_data = msg[0] + | ((len > 1) ? msg[1] << 8 : 0x00) + | ((len > 2) ? msg[2] << 16 : 0x00) + | ((len > 3) ? msg[3] << 24 : 0x00); + dbg_write(dcc_data); - msg += 4; - len -= 4; - } + msg += 4; + len -= 4; + } } void dbg_write_char(char msg) { - dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); + dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); } void dbg_write_hex32(const unsigned long val) { - static const char hextab[] = { - '0', '1', '2', '3', '4', '5', '6', '7', - '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' - }; + static const char hextab[] = { + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' + }; - for (int shift = 28; shift >= 0; shift -= 4) - dbg_write_char(hextab[(val >> shift) & 0xf]); + for (int shift = 28; shift >= 0; shift -= 4) { + dbg_write_char(hextab[(val >> shift) & 0xf]); + } } diff --git a/flight/targets/boards/revolution/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/revolution/firmware/inc/FreeRTOSConfig.h index 023d20476..62eed5553 100644 --- a/flight/targets/boards/revolution/firmware/inc/FreeRTOSConfig.h +++ b/flight/targets/boards/revolution/firmware/inc/FreeRTOSConfig.h @@ -1,85 +1,84 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ /** - * @addtogroup PIOS PIOS - * @{ - * @addtogroup FreeRTOS FreeRTOS - * @{ - */ + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ /* Notes: We use 5 task priorities */ -#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... -#define configTICK_RATE_HZ ((portTickType )1000) -#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) -#define configMINIMAL_STACK_SIZE ((unsigned short)512) -#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total -#define configMAX_TASK_NAME_LEN (16) +#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total +#define configMAX_TASK_NAME_LEN (16) -#define configUSE_PREEMPTION 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configIDLE_SHOULD_YIELD 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 -#define configUSE_TIMERS 1 -#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ -#define configTIMER_QUEUE_LENGTH 10 -#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE +#define configUSE_TIMERS 1 +#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ + to exclude the API function. */ -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 1 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 1 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 /* Enable run time stats collection */ -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 /* * Once we move to CMSIS2 we can at least use: @@ -88,16 +87,17 @@ NVIC value of 255. */ * * (still nothing for the DWT registers, surprisingly) */ -#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ - do { \ - (*(unsigned long *)0xe000edfc) |= (1<<24); /* DEMCR |= DEMCR_TRCENA */ \ - (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ - } while(0) -#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ /** - * @} - */ + * @} + */ #endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/revolution/firmware/inc/dcc_stdio.h b/flight/targets/boards/revolution/firmware/inc/dcc_stdio.h index dbfd39342..3d7354918 100644 --- a/flight/targets/boards/revolution/firmware/inc/dcc_stdio.h +++ b/flight/targets/boards/revolution/firmware/inc/dcc_stdio.h @@ -1,24 +1,24 @@ /*************************************************************************** - * Copyright (C) 2008 by Dominic Rath * - * Dominic.Rath@gmx.de * - * Copyright (C) 2008 by Spencer Oliver * - * spen@spen-soft.co.uk * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ #ifndef DCC_STDIO_H #define DCC_STDIO_H @@ -33,4 +33,4 @@ void dbg_write_str(const char *msg); void dbg_write_char(char msg); void dbg_write_hex32(const unsigned long val); -#endif /* DCC_STDIO_H */ +#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/revolution/firmware/inc/openpilot.h b/flight/targets/boards/revolution/firmware/inc/openpilot.h index 8c76835cd..3cceef152 100644 --- a/flight/targets/boards/revolution/firmware/inc/openpilot.h +++ b/flight/targets/boards/revolution/firmware/inc/openpilot.h @@ -10,19 +10,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_board_sim.h b/flight/targets/boards/revolution/firmware/inc/pios_board_sim.h index e3625f583..68a32b116 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_board_sim.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_board_sim.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file pios_board.h + * @file pios_board.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -28,25 +28,23 @@ #define PIOS_BOARD_H - - -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -#define PIOS_LED_ALARM 0 +// ------------------------ +#define PIOS_LED_ALARM 0 #define PIOS_LED_HEARTBEAT 1 -#define PIOS_LED_NUM 2 +#define PIOS_LED_NUM 2 -//------------------------- +// ------------------------- // COM // // See also pios_board_posix.c -//------------------------- -//#define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_COM_MAX_DEVS 255 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +// ------------------------- +// #define PIOS_USART_TX_BUFFER_SIZE 256 +#define PIOS_COM_BUFFER_SIZE 1024 +#define PIOS_COM_MAX_DEVS 255 +#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE extern uint32_t pios_com_telem_rf_id; extern uint32_t pios_com_telem_usb_id; @@ -54,13 +52,13 @@ extern uint32_t pios_com_gps_id; extern uint32_t pios_com_aux_id; extern uint32_t pios_com_spectrum_id; -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_GPS (pios_com_gps_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_GPS (pios_com_gps_id) #ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_DEBUG (PIOS_COM_AUX) +#define PIOS_COM_AUX (pios_com_aux_id) +#define PIOS_COM_DEBUG (PIOS_COM_AUX) #endif #define PIOS_GCSRCVR_TIMEOUT_MS 200 @@ -68,18 +66,17 @@ extern uint32_t pios_com_spectrum_id; * glue macros for file IO * STM32 uses DOSFS for file IO */ -#define PIOS_FOPEN_READ(filename,file) (file=fopen((char*)filename,"r"))==NULL +#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL -#define PIOS_FOPEN_WRITE(filename,file) (file=fopen((char*)filename,"w"))==NULL +#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL -#define PIOS_FREAD(file,bufferadr,length,resultadr) (*resultadr=fread((uint8_t*)bufferadr,1,length,*file)) != length +#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length -#define PIOS_FWRITE(file,bufferadr,length,resultadr) *resultadr=fwrite((uint8_t*)bufferadr,1,length,*file) +#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) +#define PIOS_FCLOSE(file) fclose(file) -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char*)filename) +#define PIOS_FUNLINK(file) unlink((char *)filename) #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index b4b94cc84..e0b3fb62a 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -142,15 +142,15 @@ /* #define PIOS_QUATERNION_STABILIZATION */ /* Performance counters */ -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 /* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 1000 -#define HEAP_LIMIT_CRITICAL 500 -#define IRQSTACK_LIMIT_WARNING 150 -#define IRQSTACK_LIMIT_CRITICAL 80 -#define CPULOAD_LIMIT_WARNING 80 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 1000 +#define HEAP_LIMIT_CRITICAL 500 +#define IRQSTACK_LIMIT_WARNING 150 +#define IRQSTACK_LIMIT_CRITICAL 80 +#define CPULOAD_LIMIT_WARNING 80 +#define CPULOAD_LIMIT_CRITICAL 95 /* Task stack sizes */ /* #define PIOS_ACTUATOR_STACK_SIZE 1020 */ @@ -161,7 +161,7 @@ /* #define PIOS_EVENTDISPATCHER_STACK_SIZE 130 */ /* This can't be too high to stop eventdispatcher thread overflowing */ -#define PIOS_EVENTDISAPTCHER_QUEUE 10 +#define PIOS_EVENTDISAPTCHER_QUEUE 10 /* Revolution series */ #define REVOLUTION diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h b/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h index 763587c02..293516522 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,7 +37,7 @@ #define PIOS_INCLUDE_FREERTOS #define PIOS_INCLUDE_TASK_MONITOR #define PIOS_INCLUDE_COM -//#define PIOS_INCLUDE_GPS +// #define PIOS_INCLUDE_GPS #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_TELEMETRY_RF #define PIOS_INCLUDE_TCP @@ -46,21 +46,21 @@ #define PIOS_INCLUDE_RCVR #define PIOS_INCLUDE_GCSRCVR -#define PIOS_RCVR_MAX_CHANNELS 12 -#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_RCVR_MAX_DEVS 3 /* Defaults for Logging */ -#define LOG_FILENAME "PIOS.LOG" -#define STARTUP_LOG_ENABLED 1 +#define LOG_FILENAME "PIOS.LOG" +#define STARTUP_LOG_ENABLED 1 /* COM Module */ -#define GPS_BAUDRATE 19200 -#define TELEM_BAUDRATE 19200 -#define AUXUART_ENABLED 0 -#define AUXUART_BAUDRATE 19200 +#define GPS_BAUDRATE 19200 +#define TELEM_BAUDRATE 19200 +#define AUXUART_ENABLED 0 +#define AUXUART_BAUDRATE 19200 -#define TELEM_QUEUE_SIZE 20 -#define PIOS_TELEM_STACK_SIZE 2048 +#define TELEM_QUEUE_SIZE 20 +#define PIOS_TELEM_STACK_SIZE 2048 /* Stabilization options */ #define PIOS_QUATERNION_STABILIZATION @@ -68,12 +68,12 @@ /* GPS options */ #define PIOS_GPS_SETS_HOMELOCATION -#define HEAP_LIMIT_WARNING 4000 -#define HEAP_LIMIT_CRITICAL 1000 -#define IRQSTACK_LIMIT_WARNING 150 -#define IRQSTACK_LIMIT_CRITICAL 80 -#define CPULOAD_LIMIT_WARNING 80 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 4000 +#define HEAP_LIMIT_CRITICAL 1000 +#define IRQSTACK_LIMIT_WARNING 150 +#define IRQSTACK_LIMIT_CRITICAL 80 +#define CPULOAD_LIMIT_WARNING 80 +#define CPULOAD_LIMIT_CRITICAL 95 #define REVOLUTION diff --git a/flight/targets/boards/revolution/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/revolution/firmware/inc/pios_usb_board_data.h index b4b42536a..2e607ad2c 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_usb_board_data.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_usb_board_data.h @@ -35,12 +35,12 @@ #define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 4 +#define PIOS_USB_BOARD_EP_NUM 4 -#include /* USB_* macros */ +#include /* USB_* macros */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_FW) -#define PIOS_USB_BOARD_SN_SUFFIX "+FW" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index e72b2c7f5..996232d64 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -9,19 +9,19 @@ * @{ * @brief Defines board specific static initializers for hardware for the revomini board. *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -45,12 +45,12 @@ #if defined(PIOS_INCLUDE_RFM22B) // Forward declarations static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, - uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); #endif /** - * Sensor configurations + * Sensor configurations */ #if defined(PIOS_INCLUDE_ADC) @@ -58,77 +58,75 @@ static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, void PIOS_ADC_DMC_irq_handler(void); void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); struct pios_adc_cfg pios_adc_cfg = { - .adc_dev = ADC1, - .dma = { - .irq = { - .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), - .init = { - .NVIC_IRQChannel = DMA2_Stream4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA2_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR - }, - } - }, - .half_flag = DMA_IT_HTIF4, - .full_flag = DMA_IT_TCIF4, - + .adc_dev = ADC1, + .dma = { + .irq = { + .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), + .init = { + .NVIC_IRQChannel = DMA2_Stream4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR + }, + } + }, + .half_flag = DMA_IT_HTIF4, + .full_flag = DMA_IT_TCIF4, }; void PIOS_ADC_DMC_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_ADC_DMA_Handler(); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_ADC_DMA_Handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_ADC) */ #if defined(PIOS_INCLUDE_HMC5883) #include "pios_hmc5883.h" static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { - .vector = PIOS_HMC5883_IRQHandler, - .line = EXTI_Line7, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line7, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_HMC5883_IRQHandler, + .line = EXTI_Line7, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line7, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { - .exti_cfg = &pios_exti_hmc5883_cfg, - .M_ODR = PIOS_HMC5883_ODR_75, - .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, - .Gain = PIOS_HMC5883_GAIN_1_9, - .Mode = PIOS_HMC5883_MODE_CONTINUOUS, - + .exti_cfg = &pios_exti_hmc5883_cfg, + .M_ODR = PIOS_HMC5883_ODR_75, + .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, + .Gain = PIOS_HMC5883_GAIN_1_9, + .Mode = PIOS_HMC5883_MODE_CONTINUOUS, }; #endif /* PIOS_INCLUDE_HMC5883 */ @@ -138,7 +136,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { #if defined(PIOS_INCLUDE_MS5611) #include "pios_ms5611.h" static const struct pios_ms5611_cfg pios_ms5611_cfg = { - .oversampling = MS5611_OSR_512, + .oversampling = MS5611_OSR_512, }; #endif /* PIOS_INCLUDE_MS5611 */ @@ -150,51 +148,51 @@ static const struct pios_ms5611_cfg pios_ms5611_cfg = { #include "pios_mpu6000.h" #include "pios_mpu6000_config.h" static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { - .vector = PIOS_MPU6000_IRQHandler, - .line = EXTI_Line4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line4, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_MPU6000_IRQHandler, + .line = EXTI_Line4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line4, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { - .exti_cfg = &pios_exti_mpu6000_cfg, - .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, - // Clock at 8 khz, downsampled by 12 for 666Hz - .Smpl_rate_div_no_dlp = 11, - // with dlp on output rate is 500Hz - .Smpl_rate_div_dlp = 1, - .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, - .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, - .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, - .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, - .accel_range = PIOS_MPU6000_ACCEL_8G, - .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ, - .orientation = PIOS_MPU6000_TOP_180DEG + .exti_cfg = &pios_exti_mpu6000_cfg, + .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, + // Clock at 8 khz, downsampled by 12 for 666Hz + .Smpl_rate_div_no_dlp = 11, + // with dlp on output rate is 500Hz + .Smpl_rate_div_dlp = 1, + .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, + .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, + .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_TOP_180DEG }; #endif /* PIOS_INCLUDE_MPU6000 */ @@ -204,34 +202,34 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { */ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 -#define PIOS_COM_GPS_RX_BUF_LEN 32 +#define PIOS_COM_GPS_RX_BUF_LEN 32 -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 -#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 -#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512 +#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 +#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512 -#define PIOS_COM_HKOSD_RX_BUF_LEN 22 -#define PIOS_COM_HKOSD_TX_BUF_LEN 22 +#define PIOS_COM_HKOSD_RX_BUF_LEN 22 +#define PIOS_COM_HKOSD_TX_BUF_LEN 22 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) #define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 uint32_t pios_com_debug_id; -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -uint32_t pios_com_gps_id = 0; +uint32_t pios_com_gps_id = 0; uint32_t pios_com_telem_usb_id = 0; -uint32_t pios_com_telem_rf_id = 0; -uint32_t pios_com_bridge_id = 0; -uint32_t pios_com_overo_id = 0; -uint32_t pios_com_hkosd_id = 0; +uint32_t pios_com_telem_rf_id = 0; +uint32_t pios_com_bridge_id = 0; +uint32_t pios_com_overo_id = 0; +uint32_t pios_com_hkosd_id = 0; #if defined(PIOS_INCLUDE_RFM22B) uint32_t pios_rfm22b_id = 0; #endif @@ -239,83 +237,86 @@ uint32_t pios_rfm22b_id = 0; uintptr_t pios_uavo_settings_fs_id; uintptr_t pios_user_fs_id; -/* +/* * Setup a com port based on the passed cfg, driver and buffer sizes. tx size of -1 make the port rx only */ static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, size_t rx_buf_len, size_t tx_buf_len, - const struct pios_com_driver *com_driver, uint32_t *pios_com_id) + const struct pios_com_driver *com_driver, uint32_t *pios_com_id) { - uint32_t pios_usart_id; - if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { - PIOS_Assert(0); - } - - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(rx_buf_len); - PIOS_Assert(rx_buffer); - if(tx_buf_len!= (size_t)-1){ // this is the case for rx/tx ports - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(tx_buf_len); - PIOS_Assert(tx_buffer); - - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - tx_buffer, tx_buf_len)) { - PIOS_Assert(0); - } - } - else{ //rx only port - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - NULL, 0)) { - PIOS_Assert(0); - } - } + uint32_t pios_usart_id; + + if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(rx_buf_len); + PIOS_Assert(rx_buffer); + if (tx_buf_len != (size_t)-1) { // this is the case for rx/tx ports + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(tx_buf_len); + PIOS_Assert(tx_buffer); + + if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, + rx_buffer, rx_buf_len, + tx_buffer, tx_buf_len)) { + PIOS_Assert(0); + } + } else { // rx only port + if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, + rx_buffer, rx_buf_len, + NULL, 0)) { + PIOS_Assert(0); + } + } } -static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, - const struct pios_com_driver *usart_com_driver,enum pios_dsm_proto *proto, - ManualControlSettingsChannelGroupsOptions channelgroup,uint8_t *bind) +static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, + const struct pios_com_driver *usart_com_driver, enum pios_dsm_proto *proto, + ManualControlSettingsChannelGroupsOptions channelgroup, uint8_t *bind) { - uint32_t pios_usart_dsm_id; - if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, - pios_usart_dsm_id, *proto, *bind)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; + uint32_t pios_usart_dsm_id; + + if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_id; + if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, + pios_usart_dsm_id, *proto, *bind)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_rcvr_id; + if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; } static void PIOS_Board_configure_pwm(const struct pios_pwm_cfg *pwm_cfg) { - /* Set up the receiver port. Later this should be optional */ - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, pwm_cfg); + /* Set up the receiver port. Later this should be optional */ + uint32_t pios_pwm_id; - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; + PIOS_PWM_Init(&pios_pwm_id, pwm_cfg); + + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg) { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, ppm_cfg); + uint32_t pios_ppm_id; - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + PIOS_PPM_Init(&pios_ppm_id, ppm_cfg); + + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } /** @@ -326,514 +327,512 @@ static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg) #include -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); + const struct pios_board_info *bdinfo = &pios_board_info_blob; - const struct pios_board_info * bdinfo = &pios_board_info_blob; - #if defined(PIOS_INCLUDE_LED) - const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); - PIOS_Assert(led_cfg); - PIOS_LED_Init(led_cfg); -#endif /* PIOS_INCLUDE_LED */ + const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ - /* Set up the SPI interface to the gyro/acelerometer */ - if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { - PIOS_DEBUG_Assert(0); - } - - /* Set up the SPI interface to the flash and rfm22b */ - if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { - PIOS_DEBUG_Assert(0); - } + /* Set up the SPI interface to the gyro/acelerometer */ + if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { + PIOS_DEBUG_Assert(0); + } + + /* Set up the SPI interface to the flash and rfm22b */ + if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { + PIOS_DEBUG_Assert(0); + } #if defined(PIOS_INCLUDE_FLASH) - /* Connect flash to the appropriate interface and configure it */ - uintptr_t flash_id; + /* Connect flash to the appropriate interface and configure it */ + uintptr_t flash_id; - // initialize the internal settings storage flash - if (PIOS_Flash_Internal_Init(&flash_id, &flash_internal_cfg)) { - PIOS_DEBUG_Assert(0); - } + // initialize the internal settings storage flash + if (PIOS_Flash_Internal_Init(&flash_id, &flash_internal_cfg)) { + PIOS_DEBUG_Assert(0); + } - if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id)) { - PIOS_DEBUG_Assert(0); - } + if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } - // Initialize the external USER flash - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1)) { - PIOS_DEBUG_Assert(0); - } + // Initialize the external USER flash + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1)) { + PIOS_DEBUG_Assert(0); + } - if (PIOS_FLASHFS_Logfs_Init(&pios_user_fs_id, &flashfs_external_cfg, &pios_jedec_flash_driver, flash_id)) { - PIOS_DEBUG_Assert(0); - } + if (PIOS_FLASHFS_Logfs_Init(&pios_user_fs_id, &flashfs_external_cfg, &pios_jedec_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } + +#endif /* if defined(PIOS_INCLUDE_FLASH) */ -#endif - #if defined(PIOS_INCLUDE_RTC) - PIOS_RTC_Init(&pios_rtc_main_cfg); + PIOS_RTC_Init(&pios_rtc_main_cfg); #endif - /* IAP System Setup */ - PIOS_IAP_Init(); - // check for safe mode commands from gcs - if(PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && - PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && - PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) - { - PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); - PIOS_IAP_WriteBootCmd(0,0); - PIOS_IAP_WriteBootCmd(1,0); - PIOS_IAP_WriteBootCmd(2,0); - } - PIOS_WDG_Init(); - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - - HwSettingsInitialize(); - /* Initialize the alarms library */ - AlarmsInitialize(); + /* IAP System Setup */ + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } + PIOS_WDG_Init(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } + HwSettingsInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Set up pulse timers */ - PIOS_TIM_InitClock(&tim_1_cfg); - PIOS_TIM_InitClock(&tim_3_cfg); - PIOS_TIM_InitClock(&tim_4_cfg); - PIOS_TIM_InitClock(&tim_5_cfg); - PIOS_TIM_InitClock(&tim_8_cfg); - PIOS_TIM_InitClock(&tim_9_cfg); - PIOS_TIM_InitClock(&tim_10_cfg); - PIOS_TIM_InitClock(&tim_11_cfg); - PIOS_TIM_InitClock(&tim_12_cfg); - - uint16_t boot_count = PIOS_IAP_ReadBootCount(); - if (boot_count < 3) { - PIOS_IAP_WriteBootCount(++boot_count); - AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); - } else { - /* Too many failed boot attempts, force hwsettings to defaults */ - HwSettingsSetDefaults(HwSettingsHandle(), 0); - AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); - } + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); + + /* Set up pulse timers */ + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_4_cfg); + PIOS_TIM_InitClock(&tim_5_cfg); + PIOS_TIM_InitClock(&tim_8_cfg); + PIOS_TIM_InitClock(&tim_9_cfg); + PIOS_TIM_InitClock(&tim_10_cfg); + PIOS_TIM_InitClock(&tim_11_cfg); + PIOS_TIM_InitClock(&tim_12_cfg); + + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } - //PIOS_IAP_Init(); + // PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; + /* Flags to determine if various USB interfaces are advertised */ + bool usb_hid_present = false; + bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; + usb_cdc_present = true; #else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; #endif - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); #if defined(PIOS_INCLUDE_USB_CDC) - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); + uint8_t hwsettings_usb_vcpport; + /* Configure the USB VCP port */ + HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } + if (!usb_cdc_present) { + /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ + hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; + } - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: + switch (hwsettings_usb_vcpport) { + case HWSETTINGS_USB_VCPPORT_DISABLED: + break; + case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_telem_usb_id); -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_telem_usb_id); +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) - PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_vcp_id); -#endif /* PIOS_INCLUDE_COM */ - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ + PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_vcp_id); +#endif /* PIOS_INCLUDE_COM */ + break; + } +#endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); + /* Configure the usb HID port */ + uint8_t hwsettings_usb_hidport; + HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } + if (!usb_hid_present) { + /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ + hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; + } - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: + switch (hwsettings_usb_hidport) { + case HWSETTINGS_USB_HIDPORT_DISABLED: + break; + case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } + { + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + } -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ - if (usb_hid_present || usb_cdc_present) { - PIOS_USBHOOK_Activate(); - } -#endif /* PIOS_INCLUDE_USB */ + if (usb_hid_present || usb_cdc_present) { + PIOS_USBHOOK_Activate(); + } +#endif /* PIOS_INCLUDE_USB */ - /* Configure IO ports */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); - - /* Configure main USART port */ - uint8_t hwsettings_mainport; - HwSettingsRM_MainPortGet(&hwsettings_mainport); - switch (hwsettings_mainport) { - case HWSETTINGS_RM_MAINPORT_DISABLED: - break; - case HWSETTINGS_RM_MAINPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RM_MAINPORT_GPS: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); - break; - case HWSETTINGS_RM_MAINPORT_SBUS: + /* Configure IO ports */ + uint8_t hwsettings_DSMxBind; + HwSettingsDSMxBindGet(&hwsettings_DSMxBind); + + /* Configure main USART port */ + uint8_t hwsettings_mainport; + HwSettingsRM_MainPortGet(&hwsettings_mainport); + switch (hwsettings_mainport) { + case HWSETTINGS_RM_MAINPORT_DISABLED: + break; + case HWSETTINGS_RM_MAINPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); + break; + case HWSETTINGS_RM_MAINPORT_GPS: + PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); + break; + case HWSETTINGS_RM_MAINPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) - { - uint32_t pios_usart_sbus_id; - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { - PIOS_Assert(0); - } + { + uint32_t pios_usart_sbus_id; + if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { + PIOS_Assert(0); + } - uint32_t pios_sbus_id; - if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_Assert(0); - } + uint32_t pios_sbus_id; + if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { + PIOS_Assert(0); + } - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; - } + uint32_t pios_sbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; + } #endif - break; - case HWSETTINGS_RM_MAINPORT_DSM2: - case HWSETTINGS_RM_MAINPORT_DSMX10BIT: - case HWSETTINGS_RM_MAINPORT_DSMX11BIT: - { - enum pios_dsm_proto proto; - switch (hwsettings_mainport) { - case HWSETTINGS_RM_MAINPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_RM_MAINPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_RM_MAINPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } + break; + case HWSETTINGS_RM_MAINPORT_DSM2: + case HWSETTINGS_RM_MAINPORT_DSMX10BIT: + case HWSETTINGS_RM_MAINPORT_DSMX11BIT: + { + enum pios_dsm_proto proto; + switch (hwsettings_mainport) { + case HWSETTINGS_RM_MAINPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_RM_MAINPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_RM_MAINPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } - // Force binding to zero on the main port - hwsettings_DSMxBind = 0; + // Force binding to zero on the main port + hwsettings_DSMxBind = 0; - //TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, - &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hwsettings_DSMxBind); - } - break; - case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE: + // TODO: Define the various Channelgroup for Revo dsm inputs and handle here + PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, + &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); + } + break; + case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - break; - case HWSETTINGS_RM_MAINPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RM_MAINPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } /* hwsettings_rm_mainport */ + { + PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); + } +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ + break; + case HWSETTINGS_RM_MAINPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + case HWSETTINGS_RM_MAINPORT_OSDHK: + PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); + break; + } /* hwsettings_rm_mainport */ - if (hwsettings_mainport != HWSETTINGS_RM_MAINPORT_SBUS) { - GPIO_Init(pios_sbus_cfg.inv.gpio, &pios_sbus_cfg.inv.init); - GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); - } + if (hwsettings_mainport != HWSETTINGS_RM_MAINPORT_SBUS) { + GPIO_Init(pios_sbus_cfg.inv.gpio, &pios_sbus_cfg.inv.init); + GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); + } - /* Configure FlexiPort */ - uint8_t hwsettings_flexiport; - HwSettingsRM_FlexiPortGet(&hwsettings_flexiport); - switch (hwsettings_flexiport) { - case HWSETTINGS_RM_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_RM_FLEXIPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RM_FLEXIPORT_I2C: + /* Configure FlexiPort */ + uint8_t hwsettings_flexiport; + HwSettingsRM_FlexiPortGet(&hwsettings_flexiport); + switch (hwsettings_flexiport) { + case HWSETTINGS_RM_FLEXIPORT_DISABLED: + break; + case HWSETTINGS_RM_FLEXIPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); + break; + case HWSETTINGS_RM_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) - { - if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_I2C */ - break; - case HWSETTINGS_RM_FLEXIPORT_GPS: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); - break; - case HWSETTINGS_RM_FLEXIPORT_DSM2: - case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: - case HWSETTINGS_RM_FLEXIPORT_DSMX11BIT: - { - enum pios_dsm_proto proto; - switch (hwsettings_flexiport) { - case HWSETTINGS_RM_FLEXIPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_RM_FLEXIPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } - //TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, - &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT,&hwsettings_DSMxBind); - } - break; - case HWSETTINGS_RM_FLEXIPORT_DEBUGCONSOLE: + { + if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_I2C */ + break; + case HWSETTINGS_RM_FLEXIPORT_GPS: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); + break; + case HWSETTINGS_RM_FLEXIPORT_DSM2: + case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: + case HWSETTINGS_RM_FLEXIPORT_DSMX11BIT: + { + enum pios_dsm_proto proto; + switch (hwsettings_flexiport) { + case HWSETTINGS_RM_FLEXIPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_RM_FLEXIPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } + // TODO: Define the various Channelgroup for Revo dsm inputs and handle here + PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, + &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT, &hwsettings_DSMxBind); + } + break; + case HWSETTINGS_RM_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - break; - case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RM_FLEXIPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } /* hwsettings_rm_flexiport */ + { + PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); + } +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ + break; + case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + case HWSETTINGS_RM_FLEXIPORT_OSDHK: + PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); + break; + } /* hwsettings_rm_flexiport */ - /* Initalize the RFM22B radio COM device. */ + /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) - uint8_t hwsettings_radioport; - HwSettingsRadioPortGet(&hwsettings_radioport); - uint8_t hwsettings_maxrfpower; - HwSettingsMaxRFPowerGet(&hwsettings_maxrfpower); - uint32_t hwsettings_deffreq; - HwSettingsDefaultFrequencyGet(&hwsettings_deffreq); - switch (hwsettings_radioport) { - case HWSETTINGS_RADIOPORT_DISABLED: - break; - case HWSETTINGS_RADIOPORT_TELEMETRY: - { - const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); - if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { - PIOS_Assert(0); - } + uint8_t hwsettings_radioport; + HwSettingsRadioPortGet(&hwsettings_radioport); + uint8_t hwsettings_maxrfpower; + HwSettingsMaxRFPowerGet(&hwsettings_maxrfpower); + uint32_t hwsettings_deffreq; + HwSettingsDefaultFrequencyGet(&hwsettings_deffreq); + switch (hwsettings_radioport) { + case HWSETTINGS_RADIOPORT_DISABLED: + break; + case HWSETTINGS_RADIOPORT_TELEMETRY: + { + const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); + if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { + PIOS_Assert(0); + } - // Set the modem parameters and reinitilize the modem. - PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, hwsettings_deffreq); - switch (hwsettings_maxrfpower) - { - case OPLINKSETTINGS_MAXRFPOWER_125: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); - break; - case OPLINKSETTINGS_MAXRFPOWER_16: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); - break; - case OPLINKSETTINGS_MAXRFPOWER_316: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); - break; - case OPLINKSETTINGS_MAXRFPOWER_63: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); - break; - case OPLINKSETTINGS_MAXRFPOWER_126: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); - break; - case OPLINKSETTINGS_MAXRFPOWER_25: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); - break; - case OPLINKSETTINGS_MAXRFPOWER_50: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); - break; - case OPLINKSETTINGS_MAXRFPOWER_100: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); - break; - default: - // do nothing - break; - } - PIOS_RFM22B_Reinit(pios_rfm22b_id); + // Set the modem parameters and reinitilize the modem. + PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, hwsettings_deffreq); + switch (hwsettings_maxrfpower) { + case OPLINKSETTINGS_MAXRFPOWER_125: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); + break; + case OPLINKSETTINGS_MAXRFPOWER_16: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); + break; + case OPLINKSETTINGS_MAXRFPOWER_316: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); + break; + case OPLINKSETTINGS_MAXRFPOWER_63: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); + break; + case OPLINKSETTINGS_MAXRFPOWER_126: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); + break; + case OPLINKSETTINGS_MAXRFPOWER_25: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); + break; + case OPLINKSETTINGS_MAXRFPOWER_50: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); + break; + case OPLINKSETTINGS_MAXRFPOWER_100: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); + break; + default: + // do nothing + break; + } + PIOS_RFM22B_Reinit(pios_rfm22b_id); #ifdef PIOS_INCLUDE_RFM22B_COM - uint8_t *rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, - rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) - PIOS_Assert(0); + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, + rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } #endif #ifdef PIOS_INCLUDE_RFM22B_RCVR - if (PIOS_RFM22B_RCVR_Init(pios_rfm22b_id) != 0) - PIOS_Assert(0); - uint32_t pios_rfm22b_rcvr_id; - if (PIOS_RCVR_Init(&pios_rfm22b_rcvr_id, &pios_rfm22b_rcvr_driver, pios_rfm22b_id)) - PIOS_Assert(0); - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_rfm22b_rcvr_id; + if (PIOS_RFM22B_RCVR_Init(pios_rfm22b_id) != 0) { + PIOS_Assert(0); + } + uint32_t pios_rfm22b_rcvr_id; + if (PIOS_RCVR_Init(&pios_rfm22b_rcvr_id, &pios_rfm22b_rcvr_driver, pios_rfm22b_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_rfm22b_rcvr_id; #endif - // Set the com port configuration callback. - PIOS_RFM22B_SetComConfigCallback(pios_rfm22b_id, &configureComCallback); + // Set the com port configuration callback. + PIOS_RFM22B_SetComConfigCallback(pios_rfm22b_id, &configureComCallback); - break; - } - } + break; + } + } #endif /* PIOS_INCLUDE_RFM22B */ -#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PWM) - - const struct pios_servo_cfg *pios_servo_cfg; - // default to servo outputs only - pios_servo_cfg = &pios_servo_cfg_out; +#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PWM) + + const struct pios_servo_cfg *pios_servo_cfg; + // default to servo outputs only + pios_servo_cfg = &pios_servo_cfg_out; #endif - - /* Configure the receiver port*/ - uint8_t hwsettings_rcvrport; - HwSettingsRM_RcvrPortGet(&hwsettings_rcvrport); - // - switch (hwsettings_rcvrport){ - case HWSETTINGS_RM_RCVRPORT_DISABLED: - break; - case HWSETTINGS_RM_RCVRPORT_PWM: + + /* Configure the receiver port*/ + uint8_t hwsettings_rcvrport; + HwSettingsRM_RcvrPortGet(&hwsettings_rcvrport); + // + switch (hwsettings_rcvrport) { + case HWSETTINGS_RM_RCVRPORT_DISABLED: + break; + case HWSETTINGS_RM_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) - /* Set up the receiver port. Later this should be optional */ - PIOS_Board_configure_pwm(&pios_pwm_cfg); -#endif /* PIOS_INCLUDE_PWM */ - break; - case HWSETTINGS_RM_RCVRPORT_PPM: - case HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS: - case HWSETTINGS_RM_RCVRPORT_PPMPWM: + /* Set up the receiver port. Later this should be optional */ + PIOS_Board_configure_pwm(&pios_pwm_cfg); +#endif /* PIOS_INCLUDE_PWM */ + break; + case HWSETTINGS_RM_RCVRPORT_PPM: + case HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS: + case HWSETTINGS_RM_RCVRPORT_PPMPWM: #if defined(PIOS_INCLUDE_PPM) - if(hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) - { - // configure servo outputs and the remaining 5 inputs as outputs - pios_servo_cfg = &pios_servo_cfg_out_in_ppm; - } + if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) { + // configure servo outputs and the remaining 5 inputs as outputs + pios_servo_cfg = &pios_servo_cfg_out_in_ppm; + } - PIOS_Board_configure_ppm(&pios_ppm_cfg); + PIOS_Board_configure_ppm(&pios_ppm_cfg); - // enable pwm on the remaining channels - if(hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMPWM) - { - PIOS_Board_configure_pwm(&pios_pwm_ppm_cfg); - } + // enable pwm on the remaining channels + if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMPWM) { + PIOS_Board_configure_pwm(&pios_pwm_ppm_cfg); + } - break; -#endif /* PIOS_INCLUDE_PPM */ - case HWSETTINGS_RM_RCVRPORT_OUTPUTS: - // configure only the servo outputs - pios_servo_cfg = &pios_servo_cfg_out_in; - break; - } + break; +#endif /* PIOS_INCLUDE_PPM */ + case HWSETTINGS_RM_RCVRPORT_OUTPUTS: + // configure only the servo outputs + pios_servo_cfg = &pios_servo_cfg_out_in; + break; + } #if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ + GCSReceiverInitialize(); + uint32_t pios_gcsrcvr_id; + PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_GCSRCVR */ #ifndef PIOS_ENABLE_DEBUG_PINS - // pios_servo_cfg points to the correct configuration based on input port settings - PIOS_Servo_Init(pios_servo_cfg); + // pios_servo_cfg points to the correct configuration based on input port settings + PIOS_Servo_Init(pios_servo_cfg); #else - PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); + PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); #endif - - if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { - PIOS_DEBUG_Assert(0); - } - - PIOS_DELAY_WaitmS(50); + + if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { + PIOS_DEBUG_Assert(0); + } + + PIOS_DELAY_WaitmS(50); #if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); + PIOS_ADC_Init(&pios_adc_cfg); #endif #if defined(PIOS_INCLUDE_HMC5883) - PIOS_HMC5883_Init(&pios_hmc5883_cfg); + PIOS_HMC5883_Init(&pios_hmc5883_cfg); #endif - + #if defined(PIOS_INCLUDE_MS5611) - PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id); + PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id); #endif #if defined(PIOS_INCLUDE_MPU6000) - PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg); - PIOS_MPU6000_CONFIG_Configure(); + PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); + PIOS_MPU6000_CONFIG_Configure(); #endif - } #if defined(PIOS_INCLUDE_RFM22B) @@ -845,12 +844,13 @@ void PIOS_Board_Init(void) { * \param[in] com_speed The com port speed */ static void configureComCallback(__attribute__((unused)) OPLinkSettingsRemoteMainPortOptions main_port, - __attribute__((unused)) OPLinkSettingsRemoteFlexiPortOptions flexi_port, - __attribute__((unused)) OPLinkSettingsRemoteVCPPortOptions vcp_port, - OPLinkSettingsComSpeedOptions com_speed, - uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing) + __attribute__((unused)) OPLinkSettingsRemoteFlexiPortOptions flexi_port, + __attribute__((unused)) OPLinkSettingsRemoteVCPPortOptions vcp_port, + OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing) { uint32_t comBaud = 9600; + switch (com_speed) { case OPLINKSETTINGS_COMSPEED_2400: comBaud = 2400; @@ -882,10 +882,9 @@ static void configureComCallback(__attribute__((unused)) OPLinkSettingsRemoteMai PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, min_frequency, max_frequency, channel_spacing); } -#endif +#endif /* if defined(PIOS_INCLUDE_RFM22B) */ /** * @} * @} */ - diff --git a/flight/targets/boards/revolution/firmware/pios_board_sim.c b/flight/targets/boards/revolution/firmware/pios_board_sim.c index 22cc26d7d..8af566e93 100644 --- a/flight/targets/boards/revolution/firmware/pios_board_sim.c +++ b/flight/targets/boards/revolution/firmware/pios_board_sim.c @@ -7,19 +7,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,30 +39,28 @@ #include #include -void Stack_Change() { -} +void Stack_Change() {} -void Stack_Change_Weak() { -} +void Stack_Change_Weak() {} const struct pios_tcp_cfg pios_tcp_telem_cfg = { - .ip = "0.0.0.0", - .port = 9001, + .ip = "0.0.0.0", + .port = 9001, }; const struct pios_udp_cfg pios_udp_telem_cfg = { - .ip = "0.0.0.0", - .port = 9001, + .ip = "0.0.0.0", + .port = 9001, }; const struct pios_tcp_cfg pios_tcp_gps_cfg = { - .ip = "0.0.0.0", - .port = 9001, + .ip = "0.0.0.0", + .port = 9001, }; const struct pios_tcp_cfg pios_tcp_debug_cfg = { - .ip = "0.0.0.0", - .port = 9002, + .ip = "0.0.0.0", + .port = 9002, }; #ifdef PIOS_COM_AUX @@ -70,42 +68,42 @@ const struct pios_tcp_cfg pios_tcp_debug_cfg = { * AUX USART */ const struct pios_tcp_cfg pios_tcp_aux_cfg = { - .ip = "0.0.0.0", - .port = 9003, + .ip = "0.0.0.0", + .port = 9003, }; #endif #define PIOS_COM_TELEM_RF_RX_BUF_LEN 192 #define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 -#define PIOS_COM_GPS_RX_BUF_LEN 96 +#define PIOS_COM_GPS_RX_BUF_LEN 96 /* * Board specific number of devices. */ /* -struct pios_udp_dev pios_udp_devs[] = { -#define PIOS_UDP_TELEM 0 - { + struct pios_udp_dev pios_udp_devs[] = { + #define PIOS_UDP_TELEM 0 + { .cfg = &pios_udp0_cfg, - }, -#define PIOS_UDP_GPS 1 - { + }, + #define PIOS_UDP_GPS 1 + { .cfg = &pios_udp1_cfg, - }, -#define PIOS_UDP_LOCAL 2 - { + }, + #define PIOS_UDP_LOCAL 2 + { .cfg = &pios_udp2_cfg, - }, -#ifdef PIOS_COM_AUX -#define PIOS_UDP_AUX 3 - { + }, + #ifdef PIOS_COM_AUX + #define PIOS_UDP_AUX 3 + { .cfg = &pios_udp3_cfg, - }, -#endif -}; + }, + #endif + }; -uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs); -*/ + uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs); + */ /* * COM devices */ @@ -129,102 +127,101 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; * initializes all the core systems on this specific hardware * called from System/openpilot.c */ -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + UAVObjectsInitializeAll(); - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - UAVObjectsInitializeAll(); + AccelsInitialize(); + BaroAltitudeInitialize(); + MagnetometerInitialize(); + GPSPositionInitialize(); + GyrosInitialize(); + GyrosBiasInitialize(); - AccelsInitialize(); - BaroAltitudeInitialize(); - MagnetometerInitialize(); - GPSPositionInitialize(); - GyrosInitialize(); - GyrosBiasInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Initialize the alarms library */ - AlarmsInitialize(); + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } - - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1 - { - uint32_t pios_tcp_telem_rf_id; - if (PIOS_TCP_Init(&pios_tcp_telem_rf_id, &pios_tcp_telem_cfg)) { - PIOS_Assert(0); - } + { + uint32_t pios_tcp_telem_rf_id; + if (PIOS_TCP_Init(&pios_tcp_telem_rf_id, &pios_tcp_telem_cfg)) { + PIOS_Assert(0); + } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_tcp_com_driver, pios_tcp_telem_rf_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_tcp_com_driver, pios_tcp_telem_rf_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 0 - { - uint32_t pios_udp_telem_rf_id; - if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) { - PIOS_Assert(0); - } - - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + { + uint32_t pios_udp_telem_rf_id; + if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ #if defined(PIOS_INCLUDE_GPS) - { - uint32_t pios_tcp_gps_id; - if (PIOS_TCP_Init(&pios_tcp_gps_id, &pios_tcp_gps_cfg)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_tcp_com_driver, pios_tcp_gps_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_GPS */ -#endif + { + uint32_t pios_tcp_gps_id; + if (PIOS_TCP_Init(&pios_tcp_gps_id, &pios_tcp_gps_cfg)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_tcp_com_driver, pios_tcp_gps_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_GPS */ +#endif /* if defined(PIOS_INCLUDE_COM) */ #if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ - + GCSReceiverInitialize(); + uint32_t pios_gcsrcvr_id; + PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_GCSRCVR */ } /** diff --git a/flight/targets/boards/revolution/firmware/revolution.c b/flight/targets/boards/revolution/firmware/revolution.c index 02c7f0d01..42b4a995e 100644 --- a/flight/targets/boards/revolution/firmware/revolution.c +++ b/flight/targets/boards/revolution/firmware/revolution.c @@ -9,25 +9,25 @@ * @brief This is where the OP firmware starts. Those files also define the compile-time * options of the firmware. * @{ - * @file openpilot.c + * @file openpilot.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Sets up and runs main OpenPilot tasks. * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,12 +36,12 @@ #include /* Task Priorities */ -#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) /* Global Variables */ /* Local Variables */ -#define INCLUDE_TEST_TASKS 0 +#define INCLUDE_TEST_TASKS 0 #if INCLUDE_TEST_TASKS static uint8_t sdcard_available; #endif @@ -62,11 +62,11 @@ void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value); /* Prototype of PIOS_Board_Init() function */ extern void PIOS_Board_Init(void); extern void Stack_Change(void); -static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change"))); +static void Stack_Change_Weak() __attribute__((weakref("Stack_Change"))); /* Local Variables */ -#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority -#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive +#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority +#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive static xTaskHandle initTaskHandle; /* Function Prototypes */ @@ -76,65 +76,64 @@ static void initTask(void *parameters); extern void InitModules(void); /** -* OpenPilot Main function: -* -* Initialize PiOS
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
-* Start FreeRTOS Scheduler (vTaskStartScheduler)
-* If something goes wrong, blink LED1 and LED2 every 100ms -* -*/ + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler)
+ * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ int main() { - int result; + int result; - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ - vPortInitialiseBlocks(); + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + vPortInitialiseBlocks(); - /* Brings up System using CMSIS functions, enables the LEDs. */ - PIOS_SYS_Init(); - - /* For Revolution we use a FreeRTOS task to bring up the system so we can */ - /* always rely on FreeRTOS primitive */ - result = xTaskCreate(initTask, (const signed char *)"init", - INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, - &initTaskHandle); - PIOS_Assert(result == pdPASS); - - /* Start the FreeRTOS scheduler */ - vTaskStartScheduler(); + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); - /* If all is well we will never reach here as the scheduler will now be running. */ - /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ - PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ - for(;;) { \ - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ - PIOS_DELAY_WaitmS(100); \ - }; + /* For Revolution we use a FreeRTOS task to bring up the system so we can */ + /* always rely on FreeRTOS primitive */ + result = xTaskCreate(initTask, (const signed char *)"init", + INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, + &initTaskHandle); + PIOS_Assert(result == pdPASS); - return 0; + /* Start the FreeRTOS scheduler */ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ + PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ + for (;;) { \ + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ + PIOS_DELAY_WaitmS(100); \ + } + ; + + return 0; } /** * Initialisation task. * * Runs board and module initialisation, then terminates. */ -void -initTask(__attribute__((unused)) void *parameters) +void initTask(__attribute__((unused)) void *parameters) { - /* board driver init */ - PIOS_Board_Init(); - - /* Initialize modules */ - MODULE_INITIALISE_ALL; - - /* terminate this task */ - vTaskDelete(NULL); + /* board driver init */ + PIOS_Board_Init(); + + /* Initialize modules */ + MODULE_INITIALISE_ALL; + + /* terminate this task */ + vTaskDelete(NULL); } /** * @} * @} */ - diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index ef1434754..8bdebb671 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -31,26 +31,26 @@ #include -//------------------------ +// ------------------------ // Timers and Channels Used -//------------------------ +// ------------------------ /* - Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 - ------+-----------+-----------+-----------+---------- - TIM1 | | | | - TIM2 | --------------- PIOS_DELAY ----------------- - TIM3 | | | | - TIM4 | | | | - TIM5 | | | | - TIM6 | | | | - TIM7 | | | | - TIM8 | | | | - ------+-----------+-----------+-----------+---------- + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | | | | + TIM2 | --------------- PIOS_DELAY ----------------- + TIM3 | | | | + TIM4 | | | | + TIM5 | | | | + TIM6 | | | | + TIM7 | | | | + TIM8 | | | | + ------+-----------+-----------+-----------+---------- */ -//------------------------ +// ------------------------ // DMA Channels Used -//------------------------ +// ------------------------ /* Channel 1 - */ /* Channel 2 - SPI1 RX */ /* Channel 3 - SPI1 TX */ @@ -64,243 +64,243 @@ /* Channel 11 - */ /* Channel 12 - */ -//------------------------ +// ------------------------ // BOOTLOADER_SETTINGS -//------------------------ -#define BOARD_READABLE true -#define BOARD_WRITABLE true -#define MAX_DEL_RETRYS 3 +// ------------------------ +#define BOARD_READABLE true +#define BOARD_WRITABLE true +#define MAX_DEL_RETRYS 3 -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -#define PIOS_LED_HEARTBEAT 0 -#define PIOS_LED_ALARM 1 +// ------------------------ +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_LED_ALARM 1 #ifdef PIOS_RFM22B_DEBUG_ON_TELEM -#define PIOS_LED_D1 2 -#define PIOS_LED_D2 3 -#define PIOS_LED_D3 4 -#define PIOS_LED_D4 5 +#define PIOS_LED_D1 2 +#define PIOS_LED_D2 3 +#define PIOS_LED_D3 4 +#define PIOS_LED_D4 5 -#define D1_LED_ON PIOS_LED_On(PIOS_LED_D1) -#define D1_LED_OFF PIOS_LED_Off(PIOS_LED_D1) -#define D1_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D1) +#define D1_LED_ON PIOS_LED_On(PIOS_LED_D1) +#define D1_LED_OFF PIOS_LED_Off(PIOS_LED_D1) +#define D1_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D1) -#define D2_LED_ON PIOS_LED_On(PIOS_LED_D2) -#define D2_LED_OFF PIOS_LED_Off(PIOS_LED_D2) -#define D2_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D2) +#define D2_LED_ON PIOS_LED_On(PIOS_LED_D2) +#define D2_LED_OFF PIOS_LED_Off(PIOS_LED_D2) +#define D2_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D2) -#define D3_LED_ON PIOS_LED_On(PIOS_LED_D3) -#define D3_LED_OFF PIOS_LED_Off(PIOS_LED_D3) -#define D3_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D3) +#define D3_LED_ON PIOS_LED_On(PIOS_LED_D3) +#define D3_LED_OFF PIOS_LED_Off(PIOS_LED_D3) +#define D3_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D3) -#define D4_LED_ON PIOS_LED_On(PIOS_LED_D4) -#define D4_LED_OFF PIOS_LED_Off(PIOS_LED_D4) -#define D4_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D4) +#define D4_LED_ON PIOS_LED_On(PIOS_LED_D4) +#define D4_LED_OFF PIOS_LED_Off(PIOS_LED_D4) +#define D4_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D4) #endif /* PIOS_RFM22B_DEBUG_ON_TELEM */ -//------------------------ +// ------------------------ // PIOS_SPI // See also pios_board.c -//------------------------ -#define PIOS_SPI_MAX_DEVS 3 +// ------------------------ +#define PIOS_SPI_MAX_DEVS 3 -//------------------------ +// ------------------------ // PIOS_WDG -//------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -#define PIOS_WDG_REGISTER RTC_BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_SENSORS 0x0010 +// ------------------------ +#define PIOS_WATCHDOG_TIMEOUT 250 +#define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_SENSORS 0x0010 -//------------------------ +// ------------------------ // PIOS_I2C // See also pios_board.c -//------------------------ -#define PIOS_I2C_MAX_DEVS 3 +// ------------------------ +#define PIOS_I2C_MAX_DEVS 3 extern uint32_t pios_i2c_mag_pressure_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_pressure_adapter_id) +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_pressure_adapter_id) extern uint32_t pios_i2c_flexiport_adapter_id; -#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) -#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) -//------------------------- +// ------------------------- // PIOS_USART // // See also pios_board.c -//------------------------- -#define PIOS_USART_MAX_DEVS 5 +// ------------------------- +#define PIOS_USART_MAX_DEVS 5 -//------------------------- +// ------------------------- // PIOS_COM // // See also pios_board.c -//------------------------- -#define PIOS_COM_MAX_DEVS 4 +// ------------------------- +#define PIOS_COM_MAX_DEVS 4 extern uint32_t pios_com_telem_rf_id; extern uint32_t pios_com_gps_id; extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_bridge_id; extern uint32_t pios_com_vcp_id; extern uint32_t pios_com_hkosd_id; -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_OSDHK (pios_com_hkosd_id) +#define PIOS_COM_GPS (pios_com_gps_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_BRIDGE (pios_com_bridge_id) +#define PIOS_COM_VCP (pios_com_vcp_id) +#define PIOS_COM_OSDHK (pios_com_hkosd_id) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) extern uint32_t pios_com_debug_id; -#define PIOS_COM_DEBUG (pios_com_debug_id) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#define PIOS_COM_DEBUG (pios_com_debug_id) +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #if defined(PIOS_INCLUDE_RFM22B) extern uint32_t pios_rfm22b_id; extern uint32_t pios_spi_telem_flash_id; -#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id) +#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id) #endif /* PIOS_INCLUDE_RFM22B */ -//------------------------- +// ------------------------- // Packet Handler -//------------------------- -#define RS_ECC_NPARITY 4 -#define PIOS_PH_MAX_PACKET 255 -#define PIOS_PH_WIN_SIZE 3 +// ------------------------- +#define RS_ECC_NPARITY 4 +#define PIOS_PH_MAX_PACKET 255 +#define PIOS_PH_WIN_SIZE 3 #define PIOS_PH_MAX_CONNECTIONS 1 extern uint32_t pios_packet_handler; -#define PIOS_PACKET_HANDLER (pios_packet_handler) +#define PIOS_PACKET_HANDLER (pios_packet_handler) -//------------------------ -// TELEMETRY -//------------------------ -#define TELEM_QUEUE_SIZE 80 -#define PIOS_TELEM_STACK_SIZE 624 +// ------------------------ +// TELEMETRY +// ------------------------ +#define TELEM_QUEUE_SIZE 80 +#define PIOS_TELEM_STACK_SIZE 624 -//------------------------- +// ------------------------- // System Settings -// +// // See also System_stm32f4xx.c -//------------------------- -//These macros are deprecated -//please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below -//#define PIOS_MASTER_CLOCK -//#define PIOS_PERIPHERAL_CLOCK -//#define PIOS_PERIPHERAL_CLOCK +// ------------------------- +// These macros are deprecated +// please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below +// #define PIOS_MASTER_CLOCK +// #define PIOS_PERIPHERAL_CLOCK +// #define PIOS_PERIPHERAL_CLOCK -#define PIOS_SYSCLK 168000000 -// Peripherals that belongs to APB1 are: -// DAC |PWR |CAN1,2 -// I2C1,2,3 |UART4,5 |USART3,2 -// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 -// I2S2Ext |IWDG |WWDG -// RTC/BKP reg +#define PIOS_SYSCLK 168000000 +// Peripherals that belongs to APB1 are: +// DAC |PWR |CAN1,2 +// I2C1,2,3 |UART4,5 |USART3,2 +// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 +// I2S2Ext |IWDG |WWDG +// RTC/BKP reg // TIM2,3,4,5,6,7,12,13,14 -// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) -// Default APB1 Prescaler = 4 -#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) +// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) +// Default APB1 Prescaler = 4 +#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) -// Peripherals belonging to APB2 -// SDIO |EXTI |SYSCFG |SPI1 -// ADC1,2,3 -// USART1,6 -// TIM1,8,9,10,11 +// Peripherals belonging to APB2 +// SDIO |EXTI |SYSCFG |SPI1 +// ADC1,2,3 +// USART1,6 +// TIM1,8,9,10,11 // // Default APB2 Prescaler = 2 // -#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK +#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK -//------------------------- +// ------------------------- // Interrupt Priorities -//------------------------- -#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS -#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS -#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... -#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... -//------------------------ +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------ // PIOS_RCVR // See also pios_board.c -//------------------------ +// ------------------------ #define PIOS_RCVR_MAX_DEVS 3 #define PIOS_RCVR_MAX_CHANNELS 12 #define PIOS_GCSRCVR_TIMEOUT_MS 100 #define PIOS_RFM22B_RCVR_TIMEOUT_MS 200 -//------------------------- +// ------------------------- // Receiver PPM input -//------------------------- +// ------------------------- #define PIOS_PPM_MAX_DEVS 1 #define PIOS_PPM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver PWM input -//------------------------- +// ------------------------- #define PIOS_PWM_MAX_DEVS 1 #define PIOS_PWM_NUM_INPUTS 8 -//------------------------- +// ------------------------- // Receiver SPEKTRUM input -//------------------------- +// ------------------------- #define PIOS_SPEKTRUM_MAX_DEVS 2 #define PIOS_SPEKTRUM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver S.Bus input -//------------------------- +// ------------------------- #define PIOS_SBUS_MAX_DEVS 1 -#define PIOS_SBUS_NUM_INPUTS (16+2) +#define PIOS_SBUS_NUM_INPUTS (16 + 2) -//------------------------- +// ------------------------- // Receiver DSM input -//------------------------- -#define PIOS_DSM_MAX_DEVS 2 -#define PIOS_DSM_NUM_INPUTS 12 +// ------------------------- +#define PIOS_DSM_MAX_DEVS 2 +#define PIOS_DSM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Servo outputs -//------------------------- -#define PIOS_SERVO_UPDATE_HZ 50 -#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ -//-------------------------- +// -------------------------- // Timer controller settings -//-------------------------- -#define PIOS_TIM_MAX_DEVS 6 +// -------------------------- +#define PIOS_TIM_MAX_DEVS 6 -//------------------------- +// ------------------------- // ADC // PIOS_ADC_PinGet(0) = Current sensor // PIOS_ADC_PinGet(1) = Voltage sensor // PIOS_ADC_PinGet(4) = VREF // PIOS_ADC_PinGet(5) = Temperature sensor -//------------------------- +// ------------------------- #define PIOS_DMA_PIN_CONFIG \ -{ \ - {GPIOC, GPIO_Pin_1, ADC_Channel_11}, \ - {GPIOC, GPIO_Pin_2, ADC_Channel_12}, \ - {NULL, 0, ADC_Channel_Vrefint}, /* Voltage reference */ \ - {NULL, 0, ADC_Channel_TempSensor}, /* Temperature sensor */ \ -} + { \ + { GPIOC, GPIO_Pin_1, ADC_Channel_11 }, \ + { GPIOC, GPIO_Pin_2, ADC_Channel_12 }, \ + { NULL, 0, ADC_Channel_Vrefint }, /* Voltage reference */ \ + { NULL, 0, ADC_Channel_TempSensor }, /* Temperature sensor */ \ + } /* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */ /* which is annoying because this then determines the rate at which we generate buffer turnover events */ /* the objective here is to get enough buffer space to support 100Hz averaging rate */ -#define PIOS_ADC_NUM_CHANNELS 4 -#define PIOS_ADC_MAX_OVERSAMPLING 2 -#define PIOS_ADC_USE_ADC2 0 -#define PIOS_ADC_VOLTAGE_SCALE 3.30f/4096.0f -#define PIOS_ADC_USE_TEMP_SENSOR 1 +#define PIOS_ADC_NUM_CHANNELS 4 +#define PIOS_ADC_MAX_OVERSAMPLING 2 +#define PIOS_ADC_USE_ADC2 0 +#define PIOS_ADC_VOLTAGE_SCALE 3.30f / 4096.0f +#define PIOS_ADC_USE_TEMP_SENSOR 1 -//------------------------- +// ------------------------- // USB -//------------------------- -#define PIOS_USB_MAX_DEVS 1 -#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ -#define PIOS_USB_HID_MAX_DEVS 1 +// ------------------------- +#define PIOS_USB_MAX_DEVS 1 +#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ +#define PIOS_USB_HID_MAX_DEVS 1 #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/revolution/pios_usb_board_data.c b/flight/targets/boards/revolution/pios_usb_board_data.c index df136bab4..5a097b6bc 100644 --- a/flight/targets/boards/revolution/pios_usb_board_data.c +++ b/flight/targets/boards/revolution/pios_usb_board_data.c @@ -28,71 +28,72 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ -#include /* PIOS_SYS_SerialNumberGet */ -#include /* PIOS_USBHOOK_* */ -#include /* PIOS_USB_UTIL_AsciiToUtf8 */ +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ static const uint8_t usb_product_id[22] = { - sizeof(usb_product_id), - USB_DESC_TYPE_STRING, - 'R', 0, - 'e', 0, - 'v', 0, - 'o', 0, - 'l', 0, - 'u', 0, - 't', 0, - 'i', 0, - 'o', 0, - 'n', 0, + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'R', 0, + 'e', 0, + 'v', 0, + 'o', 0, + 'l', 0, + 'u', 0, + 't', 0, + 'i', 0, + 'o', 0, + 'n', 0, }; -static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = { - sizeof(usb_serial_number), - USB_DESC_TYPE_STRING, +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, }; static const struct usb_string_langid usb_lang_id = { - .bLength = sizeof(usb_lang_id), - .bDescriptorType = USB_DESC_TYPE_STRING, - .bLangID = htousbs(USB_LANGID_ENGLISH_US), + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), }; static const uint8_t usb_vendor_id[28] = { - sizeof(usb_vendor_id), - USB_DESC_TYPE_STRING, - 'o', 0, - 'p', 0, - 'e', 0, - 'n', 0, - 'p', 0, - 'i', 0, - 'l', 0, - 'o', 0, - 't', 0, - '.', 0, - 'o', 0, - 'r', 0, - 'g', 0 + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 }; int32_t PIOS_USB_BOARD_DATA_Init(void) { - /* Load device serial number into serial number string */ - uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - PIOS_SYS_SerialNumberGet((char *)sn); + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ - uint8_t * utf8 = &(usb_serial_number[2]); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1); + PIOS_SYS_SerialNumberGet((char *)sn); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); - return 0; + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; } diff --git a/flight/targets/boards/revoproto/board_hw_defs.c b/flight/targets/boards/revoproto/board_hw_defs.c index a07af9bc7..b5f060dc9 100644 --- a/flight/targets/boards/revoproto/board_hw_defs.c +++ b/flight/targets/boards/revoproto/board_hw_defs.c @@ -9,19 +9,19 @@ * @{ * @brief Defines board specific static initializers for hardware for the Revolution board. *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -29,43 +29,43 @@ #include static const struct pios_led pios_leds[] = { - [PIOS_LED_HEARTBEAT] = { - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, - [PIOS_LED_ALARM] = { - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - }, + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_ALARM] = { + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, }; static const struct pios_led_cfg pios_led_cfg = { - .leds = pios_leds, - .num_leds = NELEMENTS(pios_leds), + .leds = pios_leds, + .num_leds = NELEMENTS(pios_leds), }; -const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (__attribute__((unused)) uint32_t board_revision) +const struct pios_led_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision) { - return &pios_led_cfg; + return &pios_led_cfg; } -#endif /* PIOS_INCLUDE_LED */ +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_SPI) #include @@ -76,125 +76,128 @@ void PIOS_SPI_accel_irq_handler(void); void DMA2_Stream0_IRQHandler(void) __attribute__((alias("PIOS_SPI_accel_irq_handler"))); void DMA2_Stream3_IRQHandler(void) __attribute__((alias("PIOS_SPI_accel_irq_handler"))); static const struct pios_spi_cfg pios_spi_accel_cfg = { - .regs = SPI1, - .remap = GPIO_AF_SPI1, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, - }, - .use_crc = false, - .dma = { - .irq = { - .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), - .init = { - .NVIC_IRQChannel = DMA2_Stream0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA2_Stream0, - .init = { + .regs = SPI1, + .remap = GPIO_AF_SPI1, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, + }, + .use_crc = false, + .dma = { + .irq = { + .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), + .init = { + .NVIC_IRQChannel = DMA2_Stream0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA2_Stream0, + .init = { .DMA_Channel = DMA_Channel_3, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, /* .DMA_FIFOThreshold */ .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA2_Stream3, - .init = { + }, + }, + .tx = { + .channel = DMA2_Stream3, + .init = { .DMA_Channel = DMA_Channel_3, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_FIFOMode = DMA_FIFOMode_Disable, /* .DMA_FIFOThreshold */ .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .miso = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .mosi = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .slave_count = 2, - .ssel = { { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - } }, - { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - } }, - } + }, + }, + }, + .sclk = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .miso = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .mosi = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .slave_count = 2, + .ssel = { + { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + } + }, + { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + } + }, + } }; static uint32_t pios_spi_accel_id; void PIOS_SPI_accel_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_accel_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_accel_id); } @@ -205,118 +208,120 @@ void PIOS_SPI_GYRO_irq_handler(void); void DMA1_Stream3_IRQHandler(void) __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); void DMA1_Stream4_IRQHandler(void) __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); static const struct pios_spi_cfg pios_spi_gyro_cfg = { - .regs = SPI2, - .remap = GPIO_AF_SPI2, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, - }, - .use_crc = false, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case RX) - .flags = (DMA_IT_TCIF3 | DMA_IT_TEIF3 | DMA_IT_HTIF3), - .init = { - .NVIC_IRQChannel = DMA1_Stream3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Stream3, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - //TODO: Enable FIFO - .DMA_FIFOMode = DMA_FIFOMode_Disable, + .regs = SPI2, + .remap = GPIO_AF_SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, + }, + .use_crc = false, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case RX) + .flags = (DMA_IT_TCIF3 | DMA_IT_TEIF3 | DMA_IT_HTIF3), + .init = { + .NVIC_IRQChannel = DMA1_Stream3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Stream3, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + // TODO: Enable FIFO + .DMA_FIFOMode = DMA_FIFOMode_Disable, .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA1_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI2->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, + }, + }, + .tx = { + .channel = DMA1_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .slave_count = 1, - .ssel = { { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - } }, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, }; uint32_t pios_spi_gyro_id; void PIOS_SPI_gyro_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); } @@ -328,118 +333,120 @@ void PIOS_SPI_flash_irq_handler(void); void DMA1_Stream0_IRQHandler(void) __attribute__((alias("PIOS_SPI_flash_irq_handler"))); void DMA1_Stream5_IRQHandler(void) __attribute__((alias("PIOS_SPI_flash_irq_handler"))); static const struct pios_spi_cfg pios_spi_flash_cfg = { - .regs = SPI3, - .remap = GPIO_AF_SPI3, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, - }, - .use_crc = false, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case RX) - .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), - .init = { - .NVIC_IRQChannel = DMA1_Stream0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Stream0, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - //TODO: Enable FIFO - .DMA_FIFOMode = DMA_FIFOMode_Disable, + .regs = SPI3, + .remap = GPIO_AF_SPI3, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + }, + .use_crc = false, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case RX) + .flags = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0), + .init = { + .NVIC_IRQChannel = DMA1_Stream0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Stream0, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + // TODO: Enable FIFO + .DMA_FIFOMode = DMA_FIFOMode_Disable, .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA1_Stream5, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, + }, + }, + .tx = { + .channel = DMA1_Stream5, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, .DMA_MemoryBurst = DMA_MemoryBurst_Single, .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .miso = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .mosi = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .slave_count = 1, - .ssel = { { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - } }, + }, + }, + }, + .sclk = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .miso = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .mosi = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, }; uint32_t pios_spi_flash_id; void PIOS_SPI_flash_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_flash_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_flash_id); } #endif /* PIOS_FLASH_ON_ACCEL */ @@ -462,20 +469,20 @@ static const struct flashfs_logfs_cfg flashfs_external_cfg = { }; -static const struct pios_flash_internal_cfg flash_internal_cfg = { }; +static const struct pios_flash_internal_cfg flash_internal_cfg = {}; static const struct flashfs_logfs_cfg flashfs_internal_cfg = { -.fs_magic = 0x99abcfef, -.total_fs_size = EE_BANK_SIZE, /* 32K bytes (2x16KB sectors) */ -.arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 1 sector */ -.slot_size = 0x00000100, /* 256 bytes */ + .fs_magic = 0x99abcfef, + .total_fs_size = EE_BANK_SIZE, /* 32K bytes (2x16KB sectors) */ + .arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 1 sector */ + .slot_size = 0x00000100, /* 256 bytes */ -.start_offset = EE_BANK_BASE, /* start after the bootloader */ -.sector_size = 0x00004000, /* 16K bytes */ -.page_size = 0x00004000, /* 16K bytes */ + .start_offset = EE_BANK_BASE, /* start after the bootloader */ + .sector_size = 0x00004000, /* 16K bytes */ + .page_size = 0x00004000, /* 16K bytes */ }; -#endif /* PIOS_INCLUDE_FLASH */ +#endif /* PIOS_INCLUDE_FLASH */ #if defined(PIOS_OVERO_SPI) /* SPI3 Interface @@ -485,126 +492,125 @@ static const struct flashfs_logfs_cfg flashfs_internal_cfg = { void PIOS_OVERO_irq_handler(void); void DMA1_Stream7_IRQHandler(void) __attribute__((alias("PIOS_OVERO_irq_handler"))); static const struct pios_overo_cfg pios_overo_cfg = { - .regs = SPI3, - .remap = GPIO_AF_SPI3, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Hard, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, - }, - .use_crc = false, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case TX) - .flags = (DMA_IT_TCIF7), - .init = { - .NVIC_IRQChannel = DMA1_Stream7_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Stream0, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), - .DMA_DIR = DMA_DIR_PeripheralToMemory, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_Medium, - //TODO: Enable FIFO - .DMA_FIFOMode = DMA_FIFOMode_Disable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_Single, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - .tx = { - .channel = DMA1_Stream7, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_Medium, - .DMA_FIFOMode = DMA_FIFOMode_Disable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_Single, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .miso = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .mosi = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .slave_count = 1, - .ssel = { { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - } }, + .regs = SPI3, + .remap = GPIO_AF_SPI3, + .init = { + .SPI_Mode = SPI_Mode_Slave, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Hard, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + }, + .use_crc = false, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case TX) + .flags = (DMA_IT_TCIF7), + .init = { + .NVIC_IRQChannel = DMA1_Stream7_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Stream0, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_Medium, + // TODO: Enable FIFO + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + .tx = { + .channel = DMA1_Stream7, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .sclk = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .miso = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .mosi = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, }; uint32_t pios_overo_id = 0; void PIOS_OVERO_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_OVERO_DMA_irq_handler(pios_overo_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_OVERO_DMA_irq_handler(pios_overo_id); } -#else +#else /* if defined(PIOS_OVERO_SPI) */ #endif /* PIOS_OVERO_SPI */ - - - #include #ifdef PIOS_INCLUDE_COM_TELEM @@ -612,45 +618,45 @@ void PIOS_OVERO_irq_handler(void) * Telemetry on main USART */ static const struct pios_usart_cfg pios_usart_telem_cfg = { - .regs = USART2, - .remap = GPIO_AF_USART2, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART2, + .remap = GPIO_AF_USART2, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_COM_TELEM */ @@ -660,45 +666,45 @@ static const struct pios_usart_cfg pios_usart_telem_cfg = { * GPS USART */ static const struct pios_usart_cfg pios_usart_gps_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART1, + .remap = GPIO_AF_USART1, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_INCLUDE_GPS */ @@ -708,45 +714,45 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = { * AUX USART (UART label on rev2) */ static const struct pios_usart_cfg pios_usart_aux_cfg = { - .regs = USART6, - .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART6, + .remap = GPIO_AF_USART6, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART6_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_COM_AUX */ @@ -756,45 +762,45 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { * AUX USART SBUS ( UART/ S Bus label on rev2) */ static const struct pios_usart_cfg pios_usart_auxsbus_cfg = { - .regs = UART4, - .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = UART4, + .remap = GPIO_AF_UART4, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = UART4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_INCLUDE_COM_AUXSBUS */ @@ -804,45 +810,45 @@ static const struct pios_usart_cfg pios_usart_auxsbus_cfg = { * FLEXI PORT */ static const struct pios_usart_cfg pios_usart_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART3, + .remap = GPIO_AF_USART3, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = + USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #endif /* PIOS_INCLUDE_COM_FLEXI */ @@ -854,168 +860,168 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { #include static const struct pios_usart_cfg pios_usart_dsm_aux_cfg = { - .regs = USART6, - .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART6, + .remap = GPIO_AF_USART6, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART6_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; static const struct pios_dsm_cfg pios_dsm_aux_cfg = { - .bind = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, + .bind = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, }; static const struct pios_usart_cfg pios_usart_dsm_auxsbus_cfg = { - .regs = UART4, - .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = UART4, + .remap = GPIO_AF_UART4, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = UART4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; static const struct pios_dsm_cfg pios_dsm_auxsbus_cfg = { - .bind = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, + .bind = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, }; static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART3, + .remap = GPIO_AF_USART3, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, + .bind = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, }; -#endif /* PIOS_INCLUDE_DSM */ +#endif /* PIOS_INCLUDE_DSM */ #if defined(PIOS_INCLUDE_SBUS) /* @@ -1024,148 +1030,148 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { #include static const struct pios_usart_cfg pios_usart_sbus_auxsbus_cfg = { - .regs = UART4, - .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 100000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_Even, - .USART_StopBits = USART_StopBits_2, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, + .regs = UART4, + .remap = GPIO_AF_UART4, + .init = { + .USART_BaudRate = 100000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_Even, + .USART_StopBits = USART_StopBits_2, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = UART4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, }; static const struct pios_sbus_cfg pios_sbus_cfg = { - /* Inverter configuration */ - .inv = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .gpio_inv_enable = Bit_SET, - .gpio_inv_disable = Bit_RESET, - .gpio_clk_func = RCC_AHB1PeriphClockCmd, - .gpio_clk_periph = RCC_AHB1Periph_GPIOC, + /* Inverter configuration */ + .inv = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .gpio_inv_enable = Bit_SET, + .gpio_inv_disable = Bit_RESET, + .gpio_clk_func = RCC_AHB1PeriphClockCmd, + .gpio_clk_periph = RCC_AHB1Periph_GPIOC, }; -#endif /* PIOS_INCLUDE_SBUS */ +#endif /* PIOS_INCLUDE_SBUS */ /* * HK OSD */ static const struct pios_usart_cfg pios_usart_hkosd_auxsbus_cfg = { - .regs = UART4, - .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = UART4, + .remap = GPIO_AF_UART4, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = UART4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; static const struct pios_usart_cfg pios_usart_hkosd_aux_cfg = { - .regs = USART6, - .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, + .regs = USART6, + .remap = GPIO_AF_USART6, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART6_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, }; #if defined(PIOS_INCLUDE_COM) @@ -1184,215 +1190,215 @@ static const struct pios_usart_cfg pios_usart_hkosd_aux_cfg = { void PIOS_I2C_mag_adapter_ev_irq_handler(void); void PIOS_I2C_mag_adapter_er_irq_handler(void); void I2C1_EV_IRQHandler() -__attribute__ ((alias("PIOS_I2C_mag_adapter_ev_irq_handler"))); +__attribute__((alias("PIOS_I2C_mag_adapter_ev_irq_handler"))); void I2C1_ER_IRQHandler() -__attribute__ ((alias("PIOS_I2C_mag_adapter_er_irq_handler"))); +__attribute__((alias("PIOS_I2C_mag_adapter_er_irq_handler"))); static const struct pios_i2c_adapter_cfg pios_i2c_mag_adapter_cfg = { - .regs = I2C1, - .remap = GPIO_AF_I2C1, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, + .regs = I2C1, + .remap = GPIO_AF_I2C1, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C1_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C1_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; uint32_t pios_i2c_mag_adapter_id; void PIOS_I2C_mag_adapter_ev_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_mag_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_mag_adapter_id); } void PIOS_I2C_mag_adapter_er_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_mag_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_mag_adapter_id); } void PIOS_I2C_flexiport_adapter_ev_irq_handler(void); void PIOS_I2C_flexiport_adapter_er_irq_handler(void); -void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexiport_adapter_ev_irq_handler"))); -void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexiport_adapter_er_irq_handler"))); +void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_ev_irq_handler"))); +void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_er_irq_handler"))); static const struct pios_i2c_adapter_cfg pios_i2c_flexiport_adapter_cfg = { - .regs = I2C2, - .remap = GPIO_AF_I2C2, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, + .regs = I2C2, + .remap = GPIO_AF_I2C2, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; uint32_t pios_i2c_flexiport_adapter_id; void PIOS_I2C_flexiport_adapter_ev_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexiport_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexiport_adapter_id); } void PIOS_I2C_flexiport_adapter_er_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexiport_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexiport_adapter_id); } void PIOS_I2C_pressure_adapter_ev_irq_handler(void); void PIOS_I2C_pressure_adapter_er_irq_handler(void); -void I2C3_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_pressure_adapter_ev_irq_handler"))); -void I2C3_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_pressure_adapter_er_irq_handler"))); +void I2C3_EV_IRQHandler() __attribute__((alias("PIOS_I2C_pressure_adapter_ev_irq_handler"))); +void I2C3_ER_IRQHandler() __attribute__((alias("PIOS_I2C_pressure_adapter_er_irq_handler"))); static const struct pios_i2c_adapter_cfg pios_i2c_pressure_adapter_cfg = { - .regs = I2C3, - .remap = GPIO_AF_I2C3, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, + .regs = I2C3, + .remap = GPIO_AF_I2C3, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_8, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .sda = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, + }, + }, + .sda = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_9, .GPIO_Mode = GPIO_Mode_AF, .GPIO_Speed = GPIO_Speed_50MHz, .GPIO_OType = GPIO_OType_OD, .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C3_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C3_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C3_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C3_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; uint32_t pios_i2c_pressure_adapter_id; void PIOS_I2C_pressure_adapter_ev_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_pressure_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_pressure_adapter_id); } void PIOS_I2C_pressure_adapter_er_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_pressure_adapter_id); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_pressure_adapter_id); } #endif /* PIOS_INCLUDE_I2C */ @@ -1402,193 +1408,192 @@ void PIOS_I2C_pressure_adapter_er_irq_handler(void) */ #include -void PIOS_RTC_IRQ_Handler (void); -void RTC_WKUP_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler"))); +void PIOS_RTC_IRQ_Handler(void); +void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); static const struct pios_rtc_cfg pios_rtc_main_cfg = { - .clksrc = RCC_RTCCLKSource_HSE_Div16, // Divide 8 Mhz crystal down to 1 - // For some reason it's acting like crystal is 16 Mhz. This clock is then divided - // by another 16 to give a nominal 62.5 khz clock - .prescaler = 100, // Every 100 cycles gives 625 Hz - .irq = { - .init = { - .NVIC_IRQChannel = RTC_WKUP_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .clksrc = RCC_RTCCLKSource_HSE_Div16, // Divide 8 Mhz crystal down to 1 + // For some reason it's acting like crystal is 16 Mhz. This clock is then divided + // by another 16 to give a nominal 62.5 khz clock + .prescaler = 100, // Every 100 cycles gives 625 Hz + .irq = { + .init = { + .NVIC_IRQChannel = RTC_WKUP_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; -void PIOS_RTC_IRQ_Handler (void) +void PIOS_RTC_IRQ_Handler(void) { - PIOS_RTC_irq_handler (); + PIOS_RTC_irq_handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_RTC) */ #include "pios_tim_priv.h" static const TIM_TimeBaseInitTypeDef tim_2_3_5_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, }; static const TIM_TimeBaseInitTypeDef tim_9_10_11_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, }; // Set up timers that only have inputs on APB2 static const TIM_TimeBaseInitTypeDef tim_1_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = 0xFFFF, + .TIM_RepetitionCounter = 0x0000, }; static const TIM_TimeBaseInitTypeDef tim_8_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = 0xFFFF, + .TIM_RepetitionCounter = 0x0000, }; // Set up timers that only have inputs on APB1 static const TIM_TimeBaseInitTypeDef tim_4_time_base = { - .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, - .TIM_RepetitionCounter = 0x0000, + .TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = 0xFFFF, + .TIM_RepetitionCounter = 0x0000, }; - static const struct pios_tim_clock_cfg tim_2_cfg = { - .timer = TIM2, - .time_base_init = &tim_2_3_5_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM2, + .time_base_init = &tim_2_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_3_cfg = { - .timer = TIM3, - .time_base_init = &tim_2_3_5_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM3, + .time_base_init = &tim_2_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_5_cfg = { - .timer = TIM5, - .time_base_init = &tim_2_3_5_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM5, + .time_base_init = &tim_2_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_8_cfg = { - .timer = TIM8, - .time_base_init = &tim_8_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM8_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM8, + .time_base_init = &tim_8_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_9_cfg = { - .timer = TIM9, - .time_base_init = &tim_9_10_11_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_BRK_TIM9_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM9, + .time_base_init = &tim_9_10_11_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_BRK_TIM9_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_10_cfg = { - .timer = TIM10, - .time_base_init = &tim_9_10_11_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_UP_TIM10_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM10, + .time_base_init = &tim_9_10_11_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_UP_TIM10_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_11_cfg = { - .timer = TIM11, - .time_base_init = &tim_9_10_11_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_TRG_COM_TIM11_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM11, + .time_base_init = &tim_9_10_11_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_TRG_COM_TIM11_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_1_cfg = { - .timer = TIM1, - .time_base_init = &tim_1_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM1, + .time_base_init = &tim_1_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; static const struct pios_tim_clock_cfg tim_4_cfg = { - .timer = TIM4, - .time_base_init = &tim_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, + .timer = TIM4, + .time_base_init = &tim_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, }; /** @@ -1596,183 +1601,183 @@ static const struct pios_tim_clock_cfg tim_4_cfg = { */ #include static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - { - .timer = TIM9, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource5, - }, - .remap = GPIO_AF_TIM9, - }, - { - .timer = TIM9, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource6, - }, - .remap = GPIO_AF_TIM9, - }, - { - .timer = TIM11, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource9, - }, - .remap = GPIO_AF_TIM11, - }, - { - .timer = TIM10, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource8, - }, - .remap = GPIO_AF_TIM10, - }, - { - .timer = TIM5, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource2, - }, - .remap = GPIO_AF_TIM5, - }, - { - .timer = TIM5, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource3, - }, - .remap = GPIO_AF_TIM5, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource0, - }, - .remap = GPIO_AF_TIM3, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource1, - }, - .remap = GPIO_AF_TIM3, - }, - // PB3 - TIM2 CH2 LED1 - { - .timer = TIM2, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource3, - }, - .remap = GPIO_AF_TIM2, - }, - // PB4 - TIM3 CH1 LED2 - { - .timer = TIM3, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource4, - }, - .remap = GPIO_AF_TIM3, - }, + { + .timer = TIM9, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource5, + }, + .remap = GPIO_AF_TIM9, + }, + { + .timer = TIM9, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource6, + }, + .remap = GPIO_AF_TIM9, + }, + { + .timer = TIM11, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource9, + }, + .remap = GPIO_AF_TIM11, + }, + { + .timer = TIM10, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource8, + }, + .remap = GPIO_AF_TIM10, + }, + { + .timer = TIM5, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource2, + }, + .remap = GPIO_AF_TIM5, + }, + { + .timer = TIM5, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource3, + }, + .remap = GPIO_AF_TIM5, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource0, + }, + .remap = GPIO_AF_TIM3, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource1, + }, + .remap = GPIO_AF_TIM3, + }, + // PB3 - TIM2 CH2 LED1 + { + .timer = TIM2, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource3, + }, + .remap = GPIO_AF_TIM2, + }, + // PB4 - TIM3 CH1 LED2 + { + .timer = TIM3, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource4, + }, + .remap = GPIO_AF_TIM3, + }, }; const struct pios_servo_cfg pios_servo_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_all_pins, - .num_channels = NELEMENTS(pios_tim_servoport_all_pins), + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_all_pins, + .num_channels = NELEMENTS(pios_tim_servoport_all_pins), }; @@ -1782,147 +1787,147 @@ const struct pios_servo_cfg pios_servo_cfg = { #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) #include static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource15, - }, - .remap = GPIO_AF_TIM4, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource14, - }, - .remap = GPIO_AF_TIM4, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource13, - }, - .remap = GPIO_AF_TIM4, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource12, - }, - .remap = GPIO_AF_TIM4, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource14, - }, - .remap = GPIO_AF_TIM1, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource13, - }, - .remap = GPIO_AF_TIM1, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource11, - }, - .remap = GPIO_AF_TIM1, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOE, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource9, - }, - .remap = GPIO_AF_TIM1, - }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource15, + }, + .remap = GPIO_AF_TIM4, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource14, + }, + .remap = GPIO_AF_TIM4, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource13, + }, + .remap = GPIO_AF_TIM4, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource12, + }, + .remap = GPIO_AF_TIM4, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource14, + }, + .remap = GPIO_AF_TIM1, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource13, + }, + .remap = GPIO_AF_TIM1, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource11, + }, + .remap = GPIO_AF_TIM1, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOE, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + .pin_source = GPIO_PinSource9, + }, + .remap = GPIO_AF_TIM1, + }, }; const struct pios_pwm_cfg pios_pwm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = pios_tim_rcvrport_all_channels, - .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_rcvrport_all_channels, + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), }; -#endif +#endif /* if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) */ /* * PPM Input @@ -1930,23 +1935,23 @@ const struct pios_pwm_cfg pios_pwm_cfg = { #if defined(PIOS_INCLUDE_PPM) #include static const struct pios_ppm_cfg pios_ppm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - .TIM_Channel = TIM_Channel_2, - }, - /* Use only the first channel for ppm */ - .channels = &pios_tim_rcvrport_all_channels[0], - .num_channels = 1, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + .TIM_Channel = TIM_Channel_2, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_rcvrport_all_channels[0], + .num_channels = 1, }; -#endif //PPM +#endif // PPM #if defined(PIOS_INCLUDE_GCSRCVR) #include "pios_gcsrcvr_priv.h" -#endif /* PIOS_INCLUDE_GCSRCVR */ +#endif /* PIOS_INCLUDE_GCSRCVR */ #if defined(PIOS_INCLUDE_RCVR) #include "pios_rcvr_priv.h" @@ -1959,67 +1964,67 @@ static const struct pios_ppm_cfg pios_ppm_cfg = { #include static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = { -{ - .timer = TIM8, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_PuPd = GPIO_PuPd_DOWN - }, - .pin_source = GPIO_PinSource8, - }, - .remap = GPIO_AF_TIM8, -}, + { + .timer = TIM8, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_PuPd = GPIO_PuPd_DOWN + }, + .pin_source = GPIO_PinSource8, + }, + .remap = GPIO_AF_TIM8, + }, }; const struct pios_hcsr04_cfg pios_hcsr04_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = pios_tim_hcsr04_port_all_channels, - .num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels), - .trigger = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_hcsr04_port_all_channels, + .num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels), + .trigger = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }; -#endif +#endif /* if defined(PIOS_INCLUDE_HCSR04) */ #if defined(PIOS_INCLUDE_USB) #include "pios_usb_priv.h" static const struct pios_usb_cfg pios_usb_main_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = OTG_FS_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 3, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .vsense = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_25MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - }, + .irq = { + .init = { + .NVIC_IRQChannel = OTG_FS_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 3, + .NVIC_IRQChannelCmd = ENABLE, }, - .vsense_active_low = false + }, + .vsense = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_25MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + }, + }, + .vsense_active_low = false }; #include "pios_usb_board_data_priv.h" @@ -2027,7 +2032,7 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { #include "pios_usb_desc_hid_only_priv.h" #include "pios_usbhook.h" -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ #if defined(PIOS_INCLUDE_COM_MSG) @@ -2039,9 +2044,9 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { #include const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, + .data_if = 0, + .data_rx_ep = 1, + .data_tx_ep = 1, }; #endif /* PIOS_INCLUDE_USB_HID */ @@ -2049,11 +2054,11 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { #include const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 1, - .ctrl_tx_ep = 2, + .ctrl_if = 1, + .ctrl_tx_ep = 2, - .data_if = 2, - .data_rx_ep = 3, - .data_tx_ep = 3, + .data_if = 2, + .data_rx_ep = 3, + .data_tx_ep = 3, }; -#endif /* PIOS_INCLUDE_USB_CDC */ +#endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/targets/boards/revoproto/bootloader/inc/common.h b/flight/targets/boards/revoproto/bootloader/inc/common.h index 9010fddd9..e64b89014 100644 --- a/flight/targets/boards/revoproto/bootloader/inc/common.h +++ b/flight/targets/boards/revoproto/bootloader/inc/common.h @@ -28,10 +28,10 @@ #ifndef COMMON_H_ #define COMMON_H_ -//#include "board.h" +// #include "board.h" typedef enum { - start, keepgoing, + start, keepgoing, } DownloadAction; /**************************************************/ @@ -39,77 +39,77 @@ typedef enum { /**************************************************/ typedef enum { - DFUidle, //0 - uploading, //1 - wrong_packet_received, //2 - too_many_packets, //3 - too_few_packets, //4 - Last_operation_Success, //5 - downloading, //6 - BLidle, //7 - Last_operation_failed, //8 - uploadingStarting, //9 - outsideDevCapabilities, //10 - CRC_Fail,//11 - failed_jump, -//12 + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + BLidle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, +// 12 } DFUStates; /**************************************************/ /* OP_DFU commands */ /**************************************************/ typedef enum { - Reserved, //0 - Req_Capabilities, //1 - Rep_Capabilities, //2 - EnterDFU, //3 - JumpFW, //4 - Reset, //5 - Abort_Operation, //6 - Upload, //7 - Op_END, //8 - Download_Req, //9 - Download, //10 - Status_Request, //11 - Status_Rep -//12 + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep +// 12 } DFUCommands; typedef enum { - High_Density, Medium_Density + High_Density, Medium_Density } DeviceType; /**************************************************/ /* OP_DFU transfer types */ /**************************************************/ typedef enum { - FW, //0 - Descript -//2 + FW, // 0 + Descript +// 2 } DFUTransfer; /**************************************************/ /* OP_DFU transfer port */ /**************************************************/ typedef enum { - Usb, //0 - Serial -//2 + Usb, // 0 + Serial +// 2 } DFUPort; /**************************************************/ /* OP_DFU programable programable HW types */ /**************************************************/ typedef enum { - Self_flash, //0 - Remote_flash_via_spi -//1 + Self_flash, // 0 + Remote_flash_via_spi +// 1 } DFUProgType; /**************************************************/ /* OP_DFU programable sources */ /**************************************************/ -#define USB 0 -#define SPI 1 +#define USB 0 +#define SPI 1 -#define DownloadDelay 100000 +#define DownloadDelay 100000 -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 +#define MAX_DEL_RETRYS 3 +#define MAX_WRI_RETRYS 3 #endif /* COMMON_H_ */ diff --git a/flight/targets/boards/revoproto/bootloader/inc/pios_config.h b/flight/targets/boards/revoproto/bootloader/inc/pios_config.h index 1d121c321..cf7b48ee1 100644 --- a/flight/targets/boards/revoproto/bootloader/inc/pios_config.h +++ b/flight/targets/boards/revoproto/bootloader/inc/pios_config.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * - Central compile time config for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/revoproto/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/revoproto/bootloader/inc/pios_usb_board_data.h index eb00a55be..4e9c7c063 100644 --- a/flight/targets/boards/revoproto/bootloader/inc/pios_usb_board_data.h +++ b/flight/targets/boards/revoproto/bootloader/inc/pios_usb_board_data.h @@ -33,13 +33,13 @@ #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 2 +#define PIOS_USB_BOARD_EP_NUM 2 -#include /* struct usb_* */ +#include /* struct usb_* */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_BL) -#define PIOS_USB_BOARD_SN_SUFFIX "+BL" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" /* * The bootloader uses a simplified report structure @@ -49,4 +49,4 @@ */ #define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/revoproto/bootloader/main.c b/flight/targets/boards/revoproto/bootloader/main.c index 6d1ab5bf9..033f14a6c 100644 --- a/flight/targets/boards/revoproto/bootloader/main.c +++ b/flight/targets/boards/revoproto/bootloader/main.c @@ -8,21 +8,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief This is the file with the main function of the Revolution BootLoader * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,7 +32,7 @@ #include #include #include -#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USBHOOK_* */ #include /* Prototype of PIOS_Board_Init() function */ @@ -61,10 +61,10 @@ uint8_t tempcount = 0; /* Extern variables ----------------------------------------------------------*/ DFUStates DeviceState; -int16_t status = 0; -bool JumpToApp = false; -bool GO_dfu = false; -bool USB_connected = false; +int16_t status = 0; +bool JumpToApp = false; +bool GO_dfu = false; +bool USB_connected = false; bool User_DFU_request = false; static uint8_t mReceive_Buffer[63]; /* Private function prototypes -----------------------------------------------*/ @@ -72,146 +72,160 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); uint8_t processRX(); void jump_to_app(); -int main() { - PIOS_SYS_Init(); - PIOS_Board_Init(); - PIOS_IAP_Init(); +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); - // Make sure the brown out reset value for this chip - // is 2.7 volts - check_bor(); + // Make sure the brown out reset value for this chip + // is 2.7 volts + check_bor(); - USB_connected = PIOS_USB_CheckAvailable(0); + USB_connected = PIOS_USB_CheckAvailable(0); - if (PIOS_IAP_CheckRequest() == true) { - PIOS_DELAY_WaitmS(1000); - User_DFU_request = true; - PIOS_IAP_ClearRequest(); - } + if (PIOS_IAP_CheckRequest() == true) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = true; + PIOS_IAP_ClearRequest(); + } - GO_dfu = (USB_connected == true) || (User_DFU_request == true); + GO_dfu = (USB_connected == true) || (User_DFU_request == true); - if (GO_dfu == true) { - if (User_DFU_request == true) - DeviceState = DFUidle; - else - DeviceState = BLidle; - } else - JumpToApp = true; + if (GO_dfu == true) { + if (User_DFU_request == true) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = true; + } - uint32_t stopwatch = 0; - uint32_t prev_ticks = PIOS_DELAY_GetuS(); - while (true) { - /* Update the stopwatch */ - uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); - prev_ticks += elapsed_ticks; - stopwatch += elapsed_ticks; + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (true) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; - if (JumpToApp == true) - jump_to_app(); + if (JumpToApp == true) { + jump_to_app(); + } - switch (DeviceState) { - case Last_operation_Success: - case uploadingStarting: - case DFUidle: - period1 = 5000; - sweep_steps1 = 100; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case uploading: - period1 = 5000; - sweep_steps1 = 100; - period2 = 2500; - sweep_steps2 = 50; - break; - case downloading: - period1 = 2500; - sweep_steps1 = 50; - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - case BLidle: - period1 = 0; - PIOS_LED_On(PIOS_LED_HEARTBEAT); - period2 = 0; - break; - default://error - period1 = 5000; - sweep_steps1 = 100; - period2 = 5000; - sweep_steps2 = 100; - } + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } - if (period1 != 0) { - if (LedPWM(period1, sweep_steps1, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_On(PIOS_LED_HEARTBEAT); + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } - if (period2 != 0) { - if (LedPWM(period2, sweep_steps2, stopwatch)) - PIOS_LED_On(PIOS_LED_HEARTBEAT); - else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } else - PIOS_LED_Off(PIOS_LED_HEARTBEAT); + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } - if (stopwatch > 50 * 1000 * 1000) - stopwatch = 0; - if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) - JumpToApp = true; + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) { + JumpToApp = true; + } - processRX(); - DataDownload(start); - } + processRX(); + DataDownload(start); + } } -void jump_to_app() { - const struct pios_board_info * bdinfo = &pios_board_info_blob; +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; - PIOS_LED_On(PIOS_LED_HEARTBEAT); + PIOS_LED_On(PIOS_LED_HEARTBEAT); // Look at cm3_vectors struct in startup. In a fw image the first uint32_t contains the address of the top of irqstack - uint32_t fwIrqStackBase = (*(__IO uint32_t*) bdinfo->fw_base) & 0xFFFE0000; + uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000; // Check for the two possible irqstack locations (sram or core coupled sram) - if ( fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { + if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { /* Jump to user application */ - FLASH_Lock(); - RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); - RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); - RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - PIOS_USBHOOK_Deactivate(); + PIOS_USBHOOK_Deactivate(); - JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); - Jump_To_Application = (pFunction) JumpAddress; - /* Initialize user application's Stack Pointer */ - __set_MSP(*(__IO uint32_t*) bdinfo->fw_base); - Jump_To_Application(); - } else { - DeviceState = failed_jump; - return; - } + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + return; + } } -uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { - uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ - uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ - uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ - if (curr_sweep & 1) { - pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ - } - return ((count % pwm_period) > pwm_duty) ? 1 : 0; + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; } -uint8_t processRX() { - if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { - processComand(mReceive_Buffer); - } - return true; +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return true; } /** @@ -221,14 +235,18 @@ uint8_t processRX() { */ void check_bor() { - uint8_t bor = FLASH_OB_GetBOR(); - if(bor != OB_BOR_LEVEL3) { - FLASH_OB_Unlock(); - FLASH_OB_BORConfig(OB_BOR_LEVEL3); - FLASH_OB_Launch(); - while(FLASH_WaitForLastOperation() == FLASH_BUSY); - FLASH_OB_Lock(); - while(FLASH_WaitForLastOperation() == FLASH_BUSY); - } -} + uint8_t bor = FLASH_OB_GetBOR(); + if (bor != OB_BOR_LEVEL3) { + FLASH_OB_Unlock(); + FLASH_OB_BORConfig(OB_BOR_LEVEL3); + FLASH_OB_Launch(); + while (FLASH_WaitForLastOperation() == FLASH_BUSY) { + ; + } + FLASH_OB_Lock(); + while (FLASH_WaitForLastOperation() == FLASH_BUSY) { + ; + } + } +} diff --git a/flight/targets/boards/revoproto/bootloader/pios_board.c b/flight/targets/boards/revoproto/bootloader/pios_board.c index bca178733..7e9858f18 100644 --- a/flight/targets/boards/revoproto/bootloader/pios_board.c +++ b/flight/targets/boards/revoproto/bootloader/pios_board.c @@ -7,19 +7,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,41 +39,42 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; -void PIOS_Board_Init() { - if (board_init_complete) { - return; - } +void PIOS_Board_Init() +{ + if (board_init_complete) { + return; + } + + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); - #if defined(PIOS_INCLUDE_LED) - PIOS_LED_Init(&pios_led_cfg); -#endif /* PIOS_INCLUDE_LED */ + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Activate the HID-only USB configuration */ - PIOS_USB_DESC_HID_ONLY_Init(); + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { - PIOS_Assert(0); - } -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ - PIOS_USBHOOK_Activate(); + PIOS_USBHOOK_Activate(); -#endif /* PIOS_INCLUDE_USB */ +#endif /* PIOS_INCLUDE_USB */ - board_init_complete = true; + board_init_complete = true; } diff --git a/flight/targets/boards/revoproto/firmware/cm3_fault_handlers.c b/flight/targets/boards/revoproto/firmware/cm3_fault_handlers.c index e59e3b79a..e74ffa76a 100644 --- a/flight/targets/boards/revoproto/firmware/cm3_fault_handlers.c +++ b/flight/targets/boards/revoproto/firmware/cm3_fault_handlers.c @@ -14,34 +14,34 @@ # include #endif -#define FAULT_TRAMPOLINE(_vec) \ -__attribute__((naked, no_instrument_function)) \ -void \ -_vec##_Handler(void) \ -{ \ - __asm( ".syntax unified\n" \ - "MOVS R0, #4 \n" \ - "MOV R1, LR \n" \ - "TST R0, R1 \n" \ - "BEQ 1f \n" \ - "MRS R0, PSP \n" \ - "B " #_vec "_Handler2 \n" \ - "1: \n" \ - "MRS R0, MSP \n" \ - "B " #_vec "_Handler2 \n" \ - ".syntax divided\n"); \ -} \ -struct hack +#define FAULT_TRAMPOLINE(_vec) \ + __attribute__((naked, no_instrument_function)) \ + void \ + _vec##_Handler(void) \ + { \ + __asm(".syntax unified\n" \ + "MOVS R0, #4 \n" \ + "MOV R1, LR \n" \ + "TST R0, R1 \n" \ + "BEQ 1f \n" \ + "MRS R0, PSP \n" \ + "B " #_vec "_Handler2 \n" \ + "1: \n" \ + "MRS R0, MSP \n" \ + "B " #_vec "_Handler2 \n" \ + ".syntax divided\n"); \ + } \ + struct hack struct cm3_frame { - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t pc; - uint32_t psr; + uint32_t r0; + uint32_t r1; + uint32_t r2; + uint32_t r3; + uint32_t r12; + uint32_t lr; + uint32_t pc; + uint32_t psr; }; FAULT_TRAMPOLINE(HardFault); @@ -49,39 +49,42 @@ FAULT_TRAMPOLINE(BusFault); FAULT_TRAMPOLINE(UsageFault); /* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */ -#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) +#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) -void -HardFault_Handler2(struct cm3_frame *frame) +void HardFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nHARD FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(HFSR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nHARD FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(HFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } } -void -BusFault_Handler2(struct cm3_frame *frame) +void BusFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nBUS FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(BFAR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nBUS FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(BFAR)); + dbg_write_char('\n'); + for (;;) { + ; + } } -void -UsageFault_Handler2(struct cm3_frame *frame) +void UsageFault_Handler2(struct cm3_frame *frame) { - dbg_write_str("\nUSAGE FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - for (;;); + dbg_write_str("\nUSAGE FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } } diff --git a/flight/targets/boards/revoproto/firmware/dcc_stdio.c b/flight/targets/boards/revoproto/firmware/dcc_stdio.c index 102dd70a1..1a522e9a0 100644 --- a/flight/targets/boards/revoproto/firmware/dcc_stdio.c +++ b/flight/targets/boards/revoproto/firmware/dcc_stdio.c @@ -1,33 +1,33 @@ /*************************************************************************** - * Copyright (C) 2008 by Dominic Rath * - * Dominic.Rath@gmx.de * - * Copyright (C) 2008 by Spencer Oliver * - * spen@spen-soft.co.uk * - * Copyright (C) 2008 by Frederik Kriewtz * - * frederik@kriewitz.eu * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* Copyright (C) 2008 by Frederik Kriewtz * +* frederik@kriewitz.eu * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ #include "inc/dcc_stdio.h" -#define TARGET_REQ_TRACEMSG 0x00 -#define TARGET_REQ_DEBUGMSG_ASCII 0x01 -#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) -#define TARGET_REQ_DEBUGCHAR 0x02 +#define TARGET_REQ_TRACEMSG 0x00 +#define TARGET_REQ_DEBUGMSG_ASCII 0x01 +#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) +#define TARGET_REQ_DEBUGCHAR 0x02 /* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel * DCRDR[7:0] is used by target for status @@ -35,115 +35,115 @@ * DCRDR[23:16] is used for by host for status * DCRDR[31:24] is used for by host for write buffer */ -#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) +#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) -#define BUSY 1 +#define BUSY 1 void dbg_write(unsigned long dcc_data) { - int len = 4; + int len = 4; - while (len--) - { - /* wait for data ready */ - while (NVIC_DBG_DATA_R & BUSY); + while (len--) { + /* wait for data ready */ + while (NVIC_DBG_DATA_R & BUSY) { + ; + } - /* write our data and set write flag - tell host there is data*/ - NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); - dcc_data >>= 8; - } + /* write our data and set write flag - tell host there is data*/ + NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); + dcc_data >>= 8; + } } void dbg_trace_point(unsigned long number) { - dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); + dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); } void dbg_write_u32(const unsigned long *val, long len) { - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); - while (len > 0) - { - dbg_write(*val); + while (len > 0) { + dbg_write(*val); - val++; - len--; - } + val++; + len--; + } } void dbg_write_u16(const unsigned short *val, long len) { - unsigned long dcc_data; + unsigned long dcc_data; - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = val[0] - | ((len > 1) ? val[1] << 16: 0x0000); + while (len > 0) { + dcc_data = val[0] + | ((len > 1) ? val[1] << 16 : 0x0000); - dbg_write(dcc_data); + dbg_write(dcc_data); - val += 2; - len -= 2; - } + val += 2; + len -= 2; + } } void dbg_write_u8(const unsigned char *val, long len) { - unsigned long dcc_data; + unsigned long dcc_data; - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = val[0] - | ((len > 1) ? val[1] << 8 : 0x00) - | ((len > 2) ? val[2] << 16 : 0x00) - | ((len > 3) ? val[3] << 24 : 0x00); + while (len > 0) { + dcc_data = val[0] + | ((len > 1) ? val[1] << 8 : 0x00) + | ((len > 2) ? val[2] << 16 : 0x00) + | ((len > 3) ? val[3] << 24 : 0x00); - dbg_write(dcc_data); + dbg_write(dcc_data); - val += 4; - len -= 4; - } + val += 4; + len -= 4; + } } void dbg_write_str(const char *msg) { - long len; - unsigned long dcc_data; + long len; + unsigned long dcc_data; - for (len = 0; msg[len] && (len < 65536); len++); + for (len = 0; msg[len] && (len < 65536); len++) { + ; + } - dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); + dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); - while (len > 0) - { - dcc_data = msg[0] - | ((len > 1) ? msg[1] << 8 : 0x00) - | ((len > 2) ? msg[2] << 16 : 0x00) - | ((len > 3) ? msg[3] << 24 : 0x00); - dbg_write(dcc_data); + while (len > 0) { + dcc_data = msg[0] + | ((len > 1) ? msg[1] << 8 : 0x00) + | ((len > 2) ? msg[2] << 16 : 0x00) + | ((len > 3) ? msg[3] << 24 : 0x00); + dbg_write(dcc_data); - msg += 4; - len -= 4; - } + msg += 4; + len -= 4; + } } void dbg_write_char(char msg) { - dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); + dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); } void dbg_write_hex32(const unsigned long val) { - static const char hextab[] = { - '0', '1', '2', '3', '4', '5', '6', '7', - '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' - }; + static const char hextab[] = { + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' + }; - for (int shift = 28; shift >= 0; shift -= 4) - dbg_write_char(hextab[(val >> shift) & 0xf]); + for (int shift = 28; shift >= 0; shift -= 4) { + dbg_write_char(hextab[(val >> shift) & 0xf]); + } } diff --git a/flight/targets/boards/revoproto/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/revoproto/firmware/inc/FreeRTOSConfig.h index 023d20476..62eed5553 100644 --- a/flight/targets/boards/revoproto/firmware/inc/FreeRTOSConfig.h +++ b/flight/targets/boards/revoproto/firmware/inc/FreeRTOSConfig.h @@ -1,85 +1,84 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ /** - * @addtogroup PIOS PIOS - * @{ - * @addtogroup FreeRTOS FreeRTOS - * @{ - */ + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ /* Notes: We use 5 task priorities */ -#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... -#define configTICK_RATE_HZ ((portTickType )1000) -#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) -#define configMINIMAL_STACK_SIZE ((unsigned short)512) -#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total -#define configMAX_TASK_NAME_LEN (16) +#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total +#define configMAX_TASK_NAME_LEN (16) -#define configUSE_PREEMPTION 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configIDLE_SHOULD_YIELD 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 -#define configUSE_TIMERS 1 -#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ -#define configTIMER_QUEUE_LENGTH 10 -#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE +#define configUSE_TIMERS 1 +#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ + to exclude the API function. */ -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 1 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 1 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 /* Enable run time stats collection */ -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 /* * Once we move to CMSIS2 we can at least use: @@ -88,16 +87,17 @@ NVIC value of 255. */ * * (still nothing for the DWT registers, surprisingly) */ -#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ - do { \ - (*(unsigned long *)0xe000edfc) |= (1<<24); /* DEMCR |= DEMCR_TRCENA */ \ - (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ - } while(0) -#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ /** - * @} - */ + * @} + */ #endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/revoproto/firmware/inc/dcc_stdio.h b/flight/targets/boards/revoproto/firmware/inc/dcc_stdio.h index dbfd39342..3d7354918 100644 --- a/flight/targets/boards/revoproto/firmware/inc/dcc_stdio.h +++ b/flight/targets/boards/revoproto/firmware/inc/dcc_stdio.h @@ -1,24 +1,24 @@ /*************************************************************************** - * Copyright (C) 2008 by Dominic Rath * - * Dominic.Rath@gmx.de * - * Copyright (C) 2008 by Spencer Oliver * - * spen@spen-soft.co.uk * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ #ifndef DCC_STDIO_H #define DCC_STDIO_H @@ -33,4 +33,4 @@ void dbg_write_str(const char *msg); void dbg_write_char(char msg); void dbg_write_hex32(const unsigned long val); -#endif /* DCC_STDIO_H */ +#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/revoproto/firmware/inc/openpilot.h b/flight/targets/boards/revoproto/firmware/inc/openpilot.h index 8c76835cd..3cceef152 100644 --- a/flight/targets/boards/revoproto/firmware/inc/openpilot.h +++ b/flight/targets/boards/revoproto/firmware/inc/openpilot.h @@ -10,19 +10,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_board_sim.h b/flight/targets/boards/revoproto/firmware/inc/pios_board_sim.h index 3c960b03e..d1bcd16bc 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_board_sim.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_board_sim.h @@ -1,25 +1,25 @@ /** ****************************************************************************** * - * @file pios_board.h + * @file pios_board.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -28,25 +28,23 @@ #define PIOS_BOARD_H - - -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -#define PIOS_LED_ALARM 0 +// ------------------------ +#define PIOS_LED_ALARM 0 #define PIOS_LED_HEARTBEAT 1 -#define PIOS_LED_NUM 2 +#define PIOS_LED_NUM 2 -//------------------------- +// ------------------------- // COM // // See also pios_board_posix.c -//------------------------- -//#define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_COM_MAX_DEVS 255 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +// ------------------------- +// #define PIOS_USART_TX_BUFFER_SIZE 256 +#define PIOS_COM_BUFFER_SIZE 1024 +#define PIOS_COM_MAX_DEVS 255 +#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE extern uint32_t pios_com_telem_rf_id; extern uint32_t pios_com_telem_usb_id; @@ -54,13 +52,13 @@ extern uint32_t pios_com_gps_id; extern uint32_t pios_com_aux_id; extern uint32_t pios_com_spectrum_id; -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_GPS (pios_com_gps_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_GPS (pios_com_gps_id) #ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_DEBUG (PIOS_COM_AUX +#define PIOS_COM_AUX (pios_com_aux_id) +#define PIOS_COM_DEBUG (PIOS_COM_AUX #endif #define PIOS_GCSRCVR_TIMEOUT_MS 200 @@ -68,18 +66,17 @@ extern uint32_t pios_com_spectrum_id; * glue macros for file IO * STM32 uses DOSFS for file IO */ -#define PIOS_FOPEN_READ(filename,file) (file=fopen((char*)filename,"r"))==NULL +#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL -#define PIOS_FOPEN_WRITE(filename,file) (file=fopen((char*)filename,"w"))==NULL +#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL -#define PIOS_FREAD(file,bufferadr,length,resultadr) (*resultadr=fread((uint8_t*)bufferadr,1,length,*file)) != length +#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length -#define PIOS_FWRITE(file,bufferadr,length,resultadr) *resultadr=fwrite((uint8_t*)bufferadr,1,length,*file) +#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) +#define PIOS_FCLOSE(file) fclose(file) -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char*)filename) +#define PIOS_FUNLINK(file) unlink((char *)filename) #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index d15dd541e..eddc58534 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -142,15 +142,15 @@ /* #define PIOS_QUATERNION_STABILIZATION */ /* Performance counters */ -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 /* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 1000 -#define HEAP_LIMIT_CRITICAL 500 -#define IRQSTACK_LIMIT_WARNING 150 -#define IRQSTACK_LIMIT_CRITICAL 80 -#define CPULOAD_LIMIT_WARNING 80 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 1000 +#define HEAP_LIMIT_CRITICAL 500 +#define IRQSTACK_LIMIT_WARNING 150 +#define IRQSTACK_LIMIT_CRITICAL 80 +#define CPULOAD_LIMIT_WARNING 80 +#define CPULOAD_LIMIT_CRITICAL 95 /* Task stack sizes */ /* #define PIOS_ACTUATOR_STACK_SIZE 1020 */ @@ -161,7 +161,7 @@ /* #define PIOS_EVENTDISPATCHER_STACK_SIZE 130 */ /* This can't be too high to stop eventdispatcher thread overflowing */ -#define PIOS_EVENTDISAPTCHER_QUEUE 10 +#define PIOS_EVENTDISAPTCHER_QUEUE 10 /* Revolution series */ #define REVOLUTION diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h b/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h index 973ec4c72..56470b59b 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,7 +37,7 @@ #define PIOS_INCLUDE_FREERTOS #define PIOS_INCLUDE_TASK_MONITOR #define PIOS_INCLUDE_COM -//#define PIOS_INCLUDE_GPS +// #define PIOS_INCLUDE_GPS #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_TELEMETRY_RF #define PIOS_INCLUDE_TCP @@ -48,21 +48,21 @@ #define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_BL_HELPER -#define PIOS_RCVR_MAX_CHANNELS 12 -#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_RCVR_MAX_DEVS 3 /* Defaults for Logging */ -#define LOG_FILENAME "PIOS.LOG" -#define STARTUP_LOG_ENABLED 1 +#define LOG_FILENAME "PIOS.LOG" +#define STARTUP_LOG_ENABLED 1 /* COM Module */ -#define GPS_BAUDRATE 19200 -#define TELEM_BAUDRATE 19200 -#define AUXUART_ENABLED 0 -#define AUXUART_BAUDRATE 19200 +#define GPS_BAUDRATE 19200 +#define TELEM_BAUDRATE 19200 +#define AUXUART_ENABLED 0 +#define AUXUART_BAUDRATE 19200 -#define TELEM_QUEUE_SIZE 20 -#define PIOS_TELEM_STACK_SIZE 2048 +#define TELEM_QUEUE_SIZE 20 +#define PIOS_TELEM_STACK_SIZE 2048 /* Stabilization options */ #define PIOS_QUATERNION_STABILIZATION @@ -70,12 +70,12 @@ /* GPS options */ #define PIOS_GPS_SETS_HOMELOCATION -#define HEAP_LIMIT_WARNING 4000 -#define HEAP_LIMIT_CRITICAL 1000 -#define IRQSTACK_LIMIT_WARNING 150 -#define IRQSTACK_LIMIT_CRITICAL 80 -#define CPULOAD_LIMIT_WARNING 80 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 4000 +#define HEAP_LIMIT_CRITICAL 1000 +#define IRQSTACK_LIMIT_WARNING 150 +#define IRQSTACK_LIMIT_CRITICAL 80 +#define CPULOAD_LIMIT_WARNING 80 +#define CPULOAD_LIMIT_CRITICAL 95 #define REVOLUTION #define SIM_OSX diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/revoproto/firmware/inc/pios_usb_board_data.h index b4b42536a..2e607ad2c 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_usb_board_data.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_usb_board_data.h @@ -35,12 +35,12 @@ #define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 #define PIOS_USB_BOARD_HID_DATA_LENGTH 64 -#define PIOS_USB_BOARD_EP_NUM 4 +#define PIOS_USB_BOARD_EP_NUM 4 -#include /* USB_* macros */ +#include /* USB_* macros */ -#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION -#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_FW) -#define PIOS_USB_BOARD_SN_SUFFIX "+FW" +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_REVOLUTION +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_REVOLUTION, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" -#endif /* PIOS_USB_BOARD_DATA_H */ +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index b1b8fd6a1..f12b9ddb3 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -9,19 +9,19 @@ * @{ * @brief Defines board specific static initializers for hardware for the revolution board. *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,7 +43,7 @@ #include "../board_hw_defs.c" /** - * Sensor configurations + * Sensor configurations */ #if defined(PIOS_INCLUDE_ADC) @@ -51,77 +51,75 @@ void PIOS_ADC_DMC_irq_handler(void); void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); struct pios_adc_cfg pios_adc_cfg = { - .adc_dev = ADC1, - .dma = { - .irq = { - .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), - .init = { - .NVIC_IRQChannel = DMA2_Stream4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA2_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR - }, - } - }, - .half_flag = DMA_IT_HTIF4, - .full_flag = DMA_IT_TCIF4, - + .adc_dev = ADC1, + .dma = { + .irq = { + .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), + .init = { + .NVIC_IRQChannel = DMA2_Stream4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR + }, + } + }, + .half_flag = DMA_IT_HTIF4, + .full_flag = DMA_IT_TCIF4, }; void PIOS_ADC_DMC_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_ADC_DMA_Handler(); + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_ADC_DMA_Handler(); } -#endif +#endif /* if defined(PIOS_INCLUDE_ADC) */ #if defined(PIOS_INCLUDE_HMC5883) #include "pios_hmc5883.h" static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { - .vector = PIOS_HMC5883_IRQHandler, - .line = EXTI_Line5, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line5, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_HMC5883_IRQHandler, + .line = EXTI_Line5, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line5, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { - .exti_cfg = &pios_exti_hmc5883_cfg, - .M_ODR = PIOS_HMC5883_ODR_75, - .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, - .Gain = PIOS_HMC5883_GAIN_1_9, - .Mode = PIOS_HMC5883_MODE_CONTINUOUS, - + .exti_cfg = &pios_exti_hmc5883_cfg, + .M_ODR = PIOS_HMC5883_ODR_75, + .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, + .Gain = PIOS_HMC5883_GAIN_1_9, + .Mode = PIOS_HMC5883_MODE_CONTINUOUS, }; #endif /* PIOS_INCLUDE_HMC5883 */ @@ -131,7 +129,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { #if defined(PIOS_INCLUDE_MS5611) #include "pios_ms5611.h" static const struct pios_ms5611_cfg pios_ms5611_cfg = { - .oversampling = MS5611_OSR_512, + .oversampling = MS5611_OSR_512, }; #endif /* PIOS_INCLUDE_MS5611 */ @@ -141,39 +139,39 @@ static const struct pios_ms5611_cfg pios_ms5611_cfg = { #if defined(PIOS_INCLUDE_BMA180) #include "pios_bma180.h" static const struct pios_exti_cfg pios_exti_bma180_cfg __exti_config = { - .vector = PIOS_BMA180_IRQHandler, - .line = EXTI_Line4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line4, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_BMA180_IRQHandler, + .line = EXTI_Line4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line4, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_bma180_cfg pios_bma180_cfg = { - .exti_cfg = &pios_exti_bma180_cfg, - .bandwidth = BMA_BW_600HZ, - .range = BMA_RANGE_8G, + .exti_cfg = &pios_exti_bma180_cfg, + .bandwidth = BMA_BW_600HZ, + .range = BMA_RANGE_8G, }; #endif /* PIOS_INCLUDE_BMA180 */ @@ -184,51 +182,51 @@ static const struct pios_bma180_cfg pios_bma180_cfg = { #include "pios_mpu6000.h" #include "pios_mpu6000_config.h" static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { - .vector = PIOS_MPU6000_IRQHandler, - .line = EXTI_Line8, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line8, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_MPU6000_IRQHandler, + .line = EXTI_Line8, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line8, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { - .exti_cfg = &pios_exti_mpu6000_cfg, - .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, - // Clock at 8 khz, downsampled by 12 for 666Hz - .Smpl_rate_div_no_dlp = 11, - // with dlp on output rate is 500Hz - .Smpl_rate_div_dlp = 1, - .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, - .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, - .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, - .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, - .accel_range = PIOS_MPU6000_ACCEL_8G, - .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ, - .orientation = PIOS_MPU6000_TOP_0DEG + .exti_cfg = &pios_exti_mpu6000_cfg, + .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, + // Clock at 8 khz, downsampled by 12 for 666Hz + .Smpl_rate_div_no_dlp = 11, + // with dlp on output rate is 500Hz + .Smpl_rate_div_dlp = 1, + .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, + .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, + .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_TOP_0DEG }; #endif /* PIOS_INCLUDE_MPU6000 */ @@ -238,39 +236,39 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { #if defined(PIOS_INCLUDE_L3GD20) #include "pios_l3gd20.h" static const struct pios_exti_cfg pios_exti_l3gd20_cfg __exti_config = { - .vector = PIOS_L3GD20_IRQHandler, - .line = EXTI_Line8, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line8, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, + .vector = PIOS_L3GD20_IRQHandler, + .line = EXTI_Line8, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line8, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; static const struct pios_l3gd20_cfg pios_l3gd20_cfg = { - .exti_cfg = &pios_exti_l3gd20_cfg, - .range = PIOS_L3GD20_SCALE_500_DEG, + .exti_cfg = &pios_exti_l3gd20_cfg, + .range = PIOS_L3GD20_SCALE_500_DEG, }; #endif /* PIOS_INCLUDE_L3GD20 */ @@ -280,88 +278,89 @@ static const struct pios_l3gd20_cfg pios_l3gd20_cfg = { */ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 -#define PIOS_COM_GPS_RX_BUF_LEN 32 +#define PIOS_COM_GPS_RX_BUF_LEN 32 #define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 -#define PIOS_COM_AUX_RX_BUF_LEN 512 -#define PIOS_COM_AUX_TX_BUF_LEN 512 +#define PIOS_COM_AUX_RX_BUF_LEN 512 +#define PIOS_COM_AUX_TX_BUF_LEN 512 -#define PIOS_COM_HKOSD_RX_BUF_LEN 22 -#define PIOS_COM_HKOSD_TX_BUF_LEN 22 +#define PIOS_COM_HKOSD_RX_BUF_LEN 22 +#define PIOS_COM_HKOSD_TX_BUF_LEN 22 -uint32_t pios_com_aux_id = 0; -uint32_t pios_com_gps_id = 0; +uint32_t pios_com_aux_id = 0; +uint32_t pios_com_gps_id = 0; uint32_t pios_com_telem_usb_id = 0; -uint32_t pios_com_telem_rf_id = 0; -uint32_t pios_com_bridge_id = 0; -uint32_t pios_com_overo_id = 0; -uint32_t pios_com_hkosd_id = 0; +uint32_t pios_com_telem_rf_id = 0; +uint32_t pios_com_bridge_id = 0; +uint32_t pios_com_overo_id = 0; +uint32_t pios_com_hkosd_id = 0; uintptr_t pios_uavo_settings_fs_id; uintptr_t pios_user_fs_id; -/* +/* * Setup a com port based on the passed cfg, driver and buffer sizes. tx size of -1 make the port rx only */ static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, size_t rx_buf_len, size_t tx_buf_len, - const struct pios_com_driver *com_driver, uint32_t *pios_com_id) + const struct pios_com_driver *com_driver, uint32_t *pios_com_id) { - uint32_t pios_usart_id; - if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { - PIOS_Assert(0); - } - - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(rx_buf_len); - PIOS_Assert(rx_buffer); - if(tx_buf_len!= (size_t)-1){ // this is the case for rx/tx ports - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(tx_buf_len); - PIOS_Assert(tx_buffer); - - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - tx_buffer, tx_buf_len)) { - PIOS_Assert(0); - } - } - else{ //rx only port - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - NULL, 0)) { - PIOS_Assert(0); - } - } + uint32_t pios_usart_id; + + if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(rx_buf_len); + PIOS_Assert(rx_buffer); + if (tx_buf_len != (size_t)-1) { // this is the case for rx/tx ports + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(tx_buf_len); + PIOS_Assert(tx_buffer); + + if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, + rx_buffer, rx_buf_len, + tx_buffer, tx_buf_len)) { + PIOS_Assert(0); + } + } else { // rx only port + if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, + rx_buffer, rx_buf_len, + NULL, 0)) { + PIOS_Assert(0); + } + } } -static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, - const struct pios_com_driver *usart_com_driver,enum pios_dsm_proto *proto, - ManualControlSettingsChannelGroupsOptions channelgroup,uint8_t *bind) +static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, + const struct pios_com_driver *usart_com_driver, enum pios_dsm_proto *proto, + ManualControlSettingsChannelGroupsOptions channelgroup, uint8_t *bind) { - uint32_t pios_usart_dsm_id; - if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, - pios_usart_dsm_id, *proto, *bind)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; + uint32_t pios_usart_dsm_id; + + if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_id; + if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, + pios_usart_dsm_id, *proto, *bind)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_rcvr_id; + if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; } /** @@ -372,567 +371,561 @@ static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm #include -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; - const struct pios_board_info * bdinfo = &pios_board_info_blob; - - /* Delay system */ - PIOS_DELAY_Init(); + /* Delay system */ + PIOS_DELAY_Init(); - PIOS_LED_Init(&pios_led_cfg); + PIOS_LED_Init(&pios_led_cfg); /* Connect flash to the appropriate interface and configure it */ uintptr_t flash_id; // initialize the internal settings storage flash if (PIOS_Flash_Internal_Init(&flash_id, &flash_internal_cfg)) { - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); } if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id)) { - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); } - /* Set up the SPI interface to the accelerometer*/ - if (PIOS_SPI_Init(&pios_spi_accel_id, &pios_spi_accel_cfg)) { - PIOS_DEBUG_Assert(0); - } - - /* Set up the SPI interface to the gyro */ - if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { - PIOS_DEBUG_Assert(0); - } -#if !defined(PIOS_FLASH_ON_ACCEL) - /* Set up the SPI interface to the flash */ - if (PIOS_SPI_Init(&pios_spi_flash_id, &pios_spi_flash_cfg)) { - PIOS_DEBUG_Assert(0); - } + /* Set up the SPI interface to the accelerometer*/ + if (PIOS_SPI_Init(&pios_spi_accel_id, &pios_spi_accel_cfg)) { + PIOS_DEBUG_Assert(0); + } - /* Connect flash to the appropriate interface and configure it */ - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_id, 0)) { - PIOS_DEBUG_Assert(0); - } + /* Set up the SPI interface to the gyro */ + if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { + PIOS_DEBUG_Assert(0); + } +#if !defined(PIOS_FLASH_ON_ACCEL) + /* Set up the SPI interface to the flash */ + if (PIOS_SPI_Init(&pios_spi_flash_id, &pios_spi_flash_cfg)) { + PIOS_DEBUG_Assert(0); + } + + /* Connect flash to the appropriate interface and configure it */ + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_id, 0)) { + PIOS_DEBUG_Assert(0); + } #else - /* Connect flash to the appropriate interface and configure it */ - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_accel_id, 1)) { - PIOS_DEBUG_Assert(0); - } + /* Connect flash to the appropriate interface and configure it */ + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_accel_id, 1)) { + PIOS_DEBUG_Assert(0); + } #endif if (PIOS_FLASHFS_Logfs_Init(&pios_user_fs_id, &flashfs_external_cfg, &pios_jedec_flash_driver, flash_id)) { PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_RTC) - PIOS_RTC_Init(&pios_rtc_main_cfg); + PIOS_RTC_Init(&pios_rtc_main_cfg); #endif - /* IAP System Setup */ - PIOS_IAP_Init(); - // check for safe mode commands from gcs - if(PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && - PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && - PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) - { - PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); - PIOS_IAP_WriteBootCmd(0,0); - PIOS_IAP_WriteBootCmd(1,0); - PIOS_IAP_WriteBootCmd(2,0); - } + /* IAP System Setup */ + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - - HwSettingsInitialize(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); - /* Initialize the alarms library */ - AlarmsInitialize(); + HwSettingsInitialize(); - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Set up pulse timers */ - PIOS_TIM_InitClock(&tim_1_cfg); - PIOS_TIM_InitClock(&tim_2_cfg); - PIOS_TIM_InitClock(&tim_3_cfg); - PIOS_TIM_InitClock(&tim_4_cfg); - PIOS_TIM_InitClock(&tim_5_cfg); - PIOS_TIM_InitClock(&tim_9_cfg); - PIOS_TIM_InitClock(&tim_10_cfg); - PIOS_TIM_InitClock(&tim_11_cfg); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); - uint16_t boot_count = PIOS_IAP_ReadBootCount(); - if (boot_count < 3) { - PIOS_IAP_WriteBootCount(++boot_count); - AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); - } else { - /* Too many failed boot attempts, force hwsettings to defaults */ - HwSettingsSetDefaults(HwSettingsHandle(), 0); - AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); - } - - - //PIOS_IAP_Init(); + /* Set up pulse timers */ + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_4_cfg); + PIOS_TIM_InitClock(&tim_5_cfg); + PIOS_TIM_InitClock(&tim_9_cfg); + PIOS_TIM_InitClock(&tim_10_cfg); + PIOS_TIM_InitClock(&tim_11_cfg); + + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } + + + // PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; + /* Flags to determine if various USB interfaces are advertised */ + bool usb_hid_present = false; + bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; + usb_cdc_present = true; #else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; #endif - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_CDC) - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); + uint8_t hwsettings_usb_vcpport; + /* Configure the USB VCP port */ + HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } + if (!usb_cdc_present) { + /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ + hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; + } - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: + switch (hwsettings_usb_vcpport) { + case HWSETTINGS_USB_VCPPORT_DISABLED: + break; + case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_telem_usb_id); -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_telem_usb_id); +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) - PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_vcp_id); -#endif /* PIOS_INCLUDE_COM */ - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ + PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_vcp_id); +#endif /* PIOS_INCLUDE_COM */ + break; + } +#endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); + /* Configure the usb HID port */ + uint8_t hwsettings_usb_hidport; + HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } + if (!usb_hid_present) { + /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ + hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; + } - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: + switch (hwsettings_usb_hidport) { + case HWSETTINGS_USB_HIDPORT_DISABLED: + break; + case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } + { + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + } -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ - if (usb_hid_present || usb_cdc_present) { - PIOS_USBHOOK_Activate(); - } -#endif /* PIOS_INCLUDE_USB */ + if (usb_hid_present || usb_cdc_present) { + PIOS_USBHOOK_Activate(); + } +#endif /* PIOS_INCLUDE_USB */ - /* Configure IO ports */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); - - /* Configure Telemetry port */ - uint8_t hwsettings_rv_telemetryport; - HwSettingsRV_TelemetryPortGet(&hwsettings_rv_telemetryport); + /* Configure IO ports */ + uint8_t hwsettings_DSMxBind; + HwSettingsDSMxBindGet(&hwsettings_DSMxBind); - switch (hwsettings_rv_telemetryport){ - case HWSETTINGS_RV_TELEMETRYPORT_DISABLED: - break; - case HWSETTINGS_RV_TELEMETRYPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RV_TELEMETRYPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - case HWSETTINGS_RV_TELEMETRYPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - - } /* hwsettings_rv_telemetryport */ + /* Configure Telemetry port */ + uint8_t hwsettings_rv_telemetryport; + HwSettingsRV_TelemetryPortGet(&hwsettings_rv_telemetryport); - /* Configure GPS port */ - uint8_t hwsettings_rv_gpsport; - HwSettingsRV_GPSPortGet(&hwsettings_rv_gpsport); - switch (hwsettings_rv_gpsport){ - case HWSETTINGS_RV_GPSPORT_DISABLED: - break; - - case HWSETTINGS_RV_GPSPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - - case HWSETTINGS_RV_GPSPORT_GPS: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); - break; - - case HWSETTINGS_RV_GPSPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - - case HWSETTINGS_RV_GPSPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - }/* hwsettings_rv_gpsport */ + switch (hwsettings_rv_telemetryport) { + case HWSETTINGS_RV_TELEMETRYPORT_DISABLED: + break; + case HWSETTINGS_RV_TELEMETRYPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); + break; + case HWSETTINGS_RV_TELEMETRYPORT_COMAUX: + PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); + break; + case HWSETTINGS_RV_TELEMETRYPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + } /* hwsettings_rv_telemetryport */ - /* Configure AUXPort */ - uint8_t hwsettings_rv_auxport; - HwSettingsRV_AuxPortGet(&hwsettings_rv_auxport); + /* Configure GPS port */ + uint8_t hwsettings_rv_gpsport; + HwSettingsRV_GPSPortGet(&hwsettings_rv_gpsport); + switch (hwsettings_rv_gpsport) { + case HWSETTINGS_RV_GPSPORT_DISABLED: + break; - switch (hwsettings_rv_auxport) { - case HWSETTINGS_RV_AUXPORT_DISABLED: - break; - - case HWSETTINGS_RV_AUXPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - - case HWSETTINGS_RV_AUXPORT_DSM2: - case HWSETTINGS_RV_AUXPORT_DSMX10BIT: - case HWSETTINGS_RV_AUXPORT_DSMX11BIT: - { - enum pios_dsm_proto proto; - switch (hwsettings_rv_auxport) { - case HWSETTINGS_RV_AUXPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_RV_AUXPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_RV_AUXPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } - //TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_aux_cfg, &pios_dsm_aux_cfg, - &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hwsettings_DSMxBind); - } - break; - case HWSETTINGS_RV_AUXPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - case HWSETTINGS_RV_AUXPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RV_AUXPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_aux_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; + case HWSETTINGS_RV_GPSPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); + break; - } /* hwsettings_rv_auxport */ - /* Configure AUXSbusPort */ - //TODO: ensure that the serial invertion pin is setted correctly - uint8_t hwsettings_rv_auxsbusport; - HwSettingsRV_AuxSBusPortGet(&hwsettings_rv_auxsbusport); - - switch (hwsettings_rv_auxsbusport) { - case HWSETTINGS_RV_AUXSBUSPORT_DISABLED: - break; - case HWSETTINGS_RV_AUXSBUSPORT_SBUS: + case HWSETTINGS_RV_GPSPORT_GPS: + PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); + break; + + case HWSETTINGS_RV_GPSPORT_COMAUX: + PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); + break; + + case HWSETTINGS_RV_GPSPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + } /* hwsettings_rv_gpsport */ + + /* Configure AUXPort */ + uint8_t hwsettings_rv_auxport; + HwSettingsRV_AuxPortGet(&hwsettings_rv_auxport); + + switch (hwsettings_rv_auxport) { + case HWSETTINGS_RV_AUXPORT_DISABLED: + break; + + case HWSETTINGS_RV_AUXPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); + break; + + case HWSETTINGS_RV_AUXPORT_DSM2: + case HWSETTINGS_RV_AUXPORT_DSMX10BIT: + case HWSETTINGS_RV_AUXPORT_DSMX11BIT: + { + enum pios_dsm_proto proto; + switch (hwsettings_rv_auxport) { + case HWSETTINGS_RV_AUXPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_RV_AUXPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_RV_AUXPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } + // TODO: Define the various Channelgroup for Revo dsm inputs and handle here + PIOS_Board_configure_dsm(&pios_usart_dsm_aux_cfg, &pios_dsm_aux_cfg, + &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); + } + break; + case HWSETTINGS_RV_AUXPORT_COMAUX: + PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); + break; + case HWSETTINGS_RV_AUXPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + case HWSETTINGS_RV_AUXPORT_OSDHK: + PIOS_Board_configure_com(&pios_usart_hkosd_aux_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); + break; + } /* hwsettings_rv_auxport */ + /* Configure AUXSbusPort */ + // TODO: ensure that the serial invertion pin is setted correctly + uint8_t hwsettings_rv_auxsbusport; + HwSettingsRV_AuxSBusPortGet(&hwsettings_rv_auxsbusport); + + switch (hwsettings_rv_auxsbusport) { + case HWSETTINGS_RV_AUXSBUSPORT_DISABLED: + break; + case HWSETTINGS_RV_AUXSBUSPORT_SBUS: #ifdef PIOS_INCLUDE_SBUS - { - uint32_t pios_usart_sbus_id; - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_auxsbus_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_id; - if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; - - } + { + uint32_t pios_usart_sbus_id; + if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_auxsbus_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_sbus_id; + if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { + PIOS_Assert(0); + } + + uint32_t pios_sbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; + } #endif /* PIOS_INCLUDE_SBUS */ - break; + break; - case HWSETTINGS_RV_AUXSBUSPORT_DSM2: - case HWSETTINGS_RV_AUXSBUSPORT_DSMX10BIT: - case HWSETTINGS_RV_AUXSBUSPORT_DSMX11BIT: - { - enum pios_dsm_proto proto; - switch (hwsettings_rv_auxsbusport) { - case HWSETTINGS_RV_AUXSBUSPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_RV_AUXSBUSPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_RV_AUXSBUSPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } - //TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_auxsbus_cfg, &pios_dsm_auxsbus_cfg, - &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hwsettings_DSMxBind); - } - break; - case HWSETTINGS_RV_AUXSBUSPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - case HWSETTINGS_RV_AUXSBUSPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RV_AUXSBUSPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_auxsbus_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } /* hwsettings_rv_auxport */ - - /* Configure FlexiPort */ + case HWSETTINGS_RV_AUXSBUSPORT_DSM2: + case HWSETTINGS_RV_AUXSBUSPORT_DSMX10BIT: + case HWSETTINGS_RV_AUXSBUSPORT_DSMX11BIT: + { + enum pios_dsm_proto proto; + switch (hwsettings_rv_auxsbusport) { + case HWSETTINGS_RV_AUXSBUSPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_RV_AUXSBUSPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_RV_AUXSBUSPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } + // TODO: Define the various Channelgroup for Revo dsm inputs and handle here + PIOS_Board_configure_dsm(&pios_usart_dsm_auxsbus_cfg, &pios_dsm_auxsbus_cfg, + &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); + } + break; + case HWSETTINGS_RV_AUXSBUSPORT_COMAUX: + PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); + break; + case HWSETTINGS_RV_AUXSBUSPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + case HWSETTINGS_RV_AUXSBUSPORT_OSDHK: + PIOS_Board_configure_com(&pios_usart_hkosd_auxsbus_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); + break; + } /* hwsettings_rv_auxport */ - uint8_t hwsettings_rv_flexiport; - HwSettingsRV_FlexiPortGet(&hwsettings_rv_flexiport); - - switch (hwsettings_rv_flexiport) { - case HWSETTINGS_RV_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_RV_FLEXIPORT_I2C: + /* Configure FlexiPort */ + + uint8_t hwsettings_rv_flexiport; + HwSettingsRV_FlexiPortGet(&hwsettings_rv_flexiport); + + switch (hwsettings_rv_flexiport) { + case HWSETTINGS_RV_FLEXIPORT_DISABLED: + break; + case HWSETTINGS_RV_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) - { - if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_I2C */ - break; - - case HWSETTINGS_RV_FLEXIPORT_DSM2: - case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: - case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: - { - enum pios_dsm_proto proto; - switch (hwsettings_rv_flexiport) { - case HWSETTINGS_RV_FLEXIPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; - break; - case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: - proto = PIOS_DSM_PROTO_DSMX10BIT; - break; - case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: - proto = PIOS_DSM_PROTO_DSMX11BIT; - break; - default: - PIOS_Assert(0); - break; - } - //TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, - &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hwsettings_DSMxBind); - } - break; - case HWSETTINGS_RV_FLEXIPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - case HWSETTINGS_RV_FLEXIPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - } /* hwsettings_rv_flexiport */ - - - /* Configure the receiver port*/ - uint8_t hwsettings_rcvrport; - HwSettingsRV_RcvrPortGet(&hwsettings_rcvrport); - // - switch (hwsettings_rcvrport){ - case HWSETTINGS_RV_RCVRPORT_DISABLED: - break; - case HWSETTINGS_RV_RCVRPORT_PWM: + { + if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_I2C */ + break; + + case HWSETTINGS_RV_FLEXIPORT_DSM2: + case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: + case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: + { + enum pios_dsm_proto proto; + switch (hwsettings_rv_flexiport) { + case HWSETTINGS_RV_FLEXIPORT_DSM2: + proto = PIOS_DSM_PROTO_DSM2; + break; + case HWSETTINGS_RV_FLEXIPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_RV_FLEXIPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + PIOS_Assert(0); + break; + } + // TODO: Define the various Channelgroup for Revo dsm inputs and handle here + PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, + &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); + } + break; + case HWSETTINGS_RV_FLEXIPORT_COMAUX: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); + break; + case HWSETTINGS_RV_FLEXIPORT_COMBRIDGE: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); + break; + } /* hwsettings_rv_flexiport */ + + + /* Configure the receiver port*/ + uint8_t hwsettings_rcvrport; + HwSettingsRV_RcvrPortGet(&hwsettings_rcvrport); + // + switch (hwsettings_rcvrport) { + case HWSETTINGS_RV_RCVRPORT_DISABLED: + break; + case HWSETTINGS_RV_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) - { - /* Set up the receiver port. Later this should be optional */ - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); - - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PWM */ - break; - case HWSETTINGS_RV_RCVRPORT_PPM: - case HWSETTINGS_RV_RCVRPORT_PPMOUTPUTS: + { + /* Set up the receiver port. Later this should be optional */ + uint32_t pios_pwm_id; + PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); + + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PWM */ + break; + case HWSETTINGS_RV_RCVRPORT_PPM: + case HWSETTINGS_RV_RCVRPORT_PPMOUTPUTS: #if defined(PIOS_INCLUDE_PPM) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PPM */ - case HWSETTINGS_RV_RCVRPORT_OUTPUTS: - - break; - } + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PPM */ + case HWSETTINGS_RV_RCVRPORT_OUTPUTS: + + break; + } #if defined(PIOS_OVERO_SPI) - /* Set up the SPI based PIOS_COM interface to the overo */ - { - HwSettingsData hwSettings; - HwSettingsGet(&hwSettings); - if(hwSettings.OptionalModules[HWSETTINGS_OPTIONALMODULES_OVERO] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { - PIOS_DEBUG_Assert(0); - } - const uint32_t PACKET_SIZE = 1024; - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PACKET_SIZE); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PACKET_SIZE); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_overo_id, &pios_overo_com_driver, pios_overo_id, - rx_buffer, PACKET_SIZE, - tx_buffer, PACKET_SIZE)) { - PIOS_Assert(0); - } - } - } + /* Set up the SPI based PIOS_COM interface to the overo */ + { + HwSettingsData hwSettings; + HwSettingsGet(&hwSettings); + if (hwSettings.OptionalModules[HWSETTINGS_OPTIONALMODULES_OVERO] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { + PIOS_DEBUG_Assert(0); + } + const uint32_t PACKET_SIZE = 1024; + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PACKET_SIZE); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PACKET_SIZE); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_overo_id, &pios_overo_com_driver, pios_overo_id, + rx_buffer, PACKET_SIZE, + tx_buffer, PACKET_SIZE)) { + PIOS_Assert(0); + } + } + } -#endif +#endif /* if defined(PIOS_OVERO_SPI) */ #if defined(PIOS_INCLUDE_HCSR04) - { - PIOS_TIM_InitClock(&tim_8_cfg); - uint32_t pios_hcsr04_id; - PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg); - } + { + PIOS_TIM_InitClock(&tim_8_cfg); + uint32_t pios_hcsr04_id; + PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg); + } #endif #if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ - -#ifndef PIOS_ENABLE_DEBUG_PINS - switch (hwsettings_rcvrport) { - case HWSETTINGS_RV_RCVRPORT_DISABLED: - case HWSETTINGS_RV_RCVRPORT_PWM: - case HWSETTINGS_RV_RCVRPORT_PPM: - /* Set up the servo outputs */ - PIOS_Servo_Init(&pios_servo_cfg); - break; - case HWSETTINGS_RV_RCVRPORT_PPMOUTPUTS: - case HWSETTINGS_RV_RCVRPORT_OUTPUTS: - //PIOS_Servo_Init(&pios_servo_rcvr_cfg); - //TODO: Prepare the configurations on board_hw_defs and handle here: - PIOS_Servo_Init(&pios_servo_cfg); - break; - } -#else - PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); -#endif - - if (PIOS_I2C_Init(&pios_i2c_mag_adapter_id, &pios_i2c_mag_adapter_cfg)) { - PIOS_DEBUG_Assert(0); - } + GCSReceiverInitialize(); + uint32_t pios_gcsrcvr_id; + PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_GCSRCVR */ - if (PIOS_I2C_Init(&pios_i2c_pressure_adapter_id, &pios_i2c_pressure_adapter_cfg)) { - PIOS_DEBUG_Assert(0); - } - - PIOS_DELAY_WaitmS(50); +#ifndef PIOS_ENABLE_DEBUG_PINS + switch (hwsettings_rcvrport) { + case HWSETTINGS_RV_RCVRPORT_DISABLED: + case HWSETTINGS_RV_RCVRPORT_PWM: + case HWSETTINGS_RV_RCVRPORT_PPM: + /* Set up the servo outputs */ + PIOS_Servo_Init(&pios_servo_cfg); + break; + case HWSETTINGS_RV_RCVRPORT_PPMOUTPUTS: + case HWSETTINGS_RV_RCVRPORT_OUTPUTS: + // PIOS_Servo_Init(&pios_servo_rcvr_cfg); + // TODO: Prepare the configurations on board_hw_defs and handle here: + PIOS_Servo_Init(&pios_servo_cfg); + break; + } +#else + PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); +#endif + + if (PIOS_I2C_Init(&pios_i2c_mag_adapter_id, &pios_i2c_mag_adapter_cfg)) { + PIOS_DEBUG_Assert(0); + } + + if (PIOS_I2C_Init(&pios_i2c_pressure_adapter_id, &pios_i2c_pressure_adapter_cfg)) { + PIOS_DEBUG_Assert(0); + } + + PIOS_DELAY_WaitmS(50); #if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); + PIOS_ADC_Init(&pios_adc_cfg); #endif #if defined(PIOS_INCLUDE_HMC5883) - PIOS_HMC5883_Init(&pios_hmc5883_cfg); -#endif - -#if defined(PIOS_INCLUDE_MS5611) - PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id); + PIOS_HMC5883_Init(&pios_hmc5883_cfg); #endif - switch(bdinfo->board_rev) { - case 0x01: +#if defined(PIOS_INCLUDE_MS5611) + PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id); +#endif + + switch (bdinfo->board_rev) { + case 0x01: #if defined(PIOS_INCLUDE_L3GD20) - PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg); - PIOS_Assert(PIOS_L3GD20_Test() == 0); + PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg); + PIOS_Assert(PIOS_L3GD20_Test() == 0); #endif #if defined(PIOS_INCLUDE_BMA180) - PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg); - PIOS_Assert(PIOS_BMA180_Test() == 0); + PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg); + PIOS_Assert(PIOS_BMA180_Test() == 0); #endif - break; - case 0x02: + break; + case 0x02: #if defined(PIOS_INCLUDE_MPU6000) - PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg); - PIOS_MPU6000_CONFIG_Configure(); + PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); + PIOS_MPU6000_CONFIG_Configure(); #endif - break; - default: - PIOS_DEBUG_Assert(0); - } - + break; + default: + PIOS_DEBUG_Assert(0); + } } /** * @} * @} */ - diff --git a/flight/targets/boards/revoproto/firmware/pios_board_sim.c b/flight/targets/boards/revoproto/firmware/pios_board_sim.c index 970e69f3f..a73beba28 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board_sim.c +++ b/flight/targets/boards/revoproto/firmware/pios_board_sim.c @@ -7,19 +7,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,30 +39,28 @@ #include #include -void Stack_Change() { -} +void Stack_Change() {} -void Stack_Change_Weak() { -} +void Stack_Change_Weak() {} const struct pios_tcp_cfg pios_tcp_telem_cfg = { - .ip = "0.0.0.0", - .port = 9000, + .ip = "0.0.0.0", + .port = 9000, }; const struct pios_udp_cfg pios_udp_telem_cfg = { - .ip = "0.0.0.0", - .port = 9000, + .ip = "0.0.0.0", + .port = 9000, }; const struct pios_tcp_cfg pios_tcp_gps_cfg = { - .ip = "0.0.0.0", - .port = 9001, + .ip = "0.0.0.0", + .port = 9001, }; const struct pios_tcp_cfg pios_tcp_debug_cfg = { - .ip = "0.0.0.0", - .port = 9002, + .ip = "0.0.0.0", + .port = 9002, }; #ifdef PIOS_COM_AUX @@ -70,42 +68,42 @@ const struct pios_tcp_cfg pios_tcp_debug_cfg = { * AUX USART */ const struct pios_tcp_cfg pios_tcp_aux_cfg = { - .ip = "0.0.0.0", - .port = 9003, + .ip = "0.0.0.0", + .port = 9003, }; #endif #define PIOS_COM_TELEM_RF_RX_BUF_LEN 192 #define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 -#define PIOS_COM_GPS_RX_BUF_LEN 96 +#define PIOS_COM_GPS_RX_BUF_LEN 96 /* * Board specific number of devices. */ /* -struct pios_udp_dev pios_udp_devs[] = { -#define PIOS_UDP_TELEM 0 - { + struct pios_udp_dev pios_udp_devs[] = { + #define PIOS_UDP_TELEM 0 + { .cfg = &pios_udp0_cfg, - }, -#define PIOS_UDP_GPS 1 - { + }, + #define PIOS_UDP_GPS 1 + { .cfg = &pios_udp1_cfg, - }, -#define PIOS_UDP_LOCAL 2 - { + }, + #define PIOS_UDP_LOCAL 2 + { .cfg = &pios_udp2_cfg, - }, -#ifdef PIOS_COM_AUX -#define PIOS_UDP_AUX 3 - { + }, + #ifdef PIOS_COM_AUX + #define PIOS_UDP_AUX 3 + { .cfg = &pios_udp3_cfg, - }, -#endif -}; + }, + #endif + }; -uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs); -*/ + uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs); + */ /* * COM devices */ @@ -129,102 +127,101 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; * initializes all the core systems on this specific hardware * called from System/openpilot.c */ -void PIOS_Board_Init(void) { +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Delay system */ - PIOS_DELAY_Init(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + UAVObjectsInitializeAll(); - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - UAVObjectsInitializeAll(); + AccelsInitialize(); + BaroAltitudeInitialize(); + MagnetometerInitialize(); + GPSPositionInitialize(); + GyrosInitialize(); + GyrosBiasInitialize(); - AccelsInitialize(); - BaroAltitudeInitialize(); - MagnetometerInitialize(); - GPSPositionInitialize(); - GyrosInitialize(); - GyrosBiasInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Initialize the alarms library */ - AlarmsInitialize(); + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } - - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1 - { - uint32_t pios_tcp_telem_rf_id; - if (PIOS_TCP_Init(&pios_tcp_telem_rf_id, &pios_tcp_telem_cfg)) { - PIOS_Assert(0); - } + { + uint32_t pios_tcp_telem_rf_id; + if (PIOS_TCP_Init(&pios_tcp_telem_rf_id, &pios_tcp_telem_cfg)) { + PIOS_Assert(0); + } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_tcp_com_driver, pios_tcp_telem_rf_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_tcp_com_driver, pios_tcp_telem_rf_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 0 - { - uint32_t pios_udp_telem_rf_id; - if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) { - PIOS_Assert(0); - } - - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } + { + uint32_t pios_udp_telem_rf_id; + if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ #if defined(PIOS_INCLUDE_GPS) - { - uint32_t pios_tcp_gps_id; - if (PIOS_TCP_Init(&pios_tcp_gps_id, &pios_tcp_gps_cfg)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_tcp_com_driver, pios_tcp_gps_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_GPS */ -#endif + { + uint32_t pios_tcp_gps_id; + if (PIOS_TCP_Init(&pios_tcp_gps_id, &pios_tcp_gps_cfg)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_tcp_com_driver, pios_tcp_gps_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_GPS */ +#endif /* if defined(PIOS_INCLUDE_COM) */ #if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ - + GCSReceiverInitialize(); + uint32_t pios_gcsrcvr_id; + PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_GCSRCVR */ } /** diff --git a/flight/targets/boards/revoproto/firmware/revolution.c b/flight/targets/boards/revoproto/firmware/revolution.c index b1f771ddd..860cd34e2 100644 --- a/flight/targets/boards/revoproto/firmware/revolution.c +++ b/flight/targets/boards/revoproto/firmware/revolution.c @@ -9,25 +9,25 @@ * @brief This is where the OP firmware starts. Those files also define the compile-time * options of the firmware. * @{ - * @file openpilot.c + * @file openpilot.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Sets up and runs main OpenPilot tasks. * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,12 +35,12 @@ #include /* Task Priorities */ -#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) /* Global Variables */ /* Local Variables */ -#define INCLUDE_TEST_TASKS 0 +#define INCLUDE_TEST_TASKS 0 #if INCLUDE_TEST_TASKS static uint8_t sdcard_available; #endif @@ -61,11 +61,11 @@ void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value); /* Prototype of PIOS_Board_Init() function */ extern void PIOS_Board_Init(void); extern void Stack_Change(void); -static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change"))); +static void Stack_Change_Weak() __attribute__((weakref("Stack_Change"))); /* Local Variables */ -#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority -#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive +#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority +#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive static xTaskHandle initTaskHandle; /* Function Prototypes */ @@ -75,65 +75,64 @@ static void initTask(void *parameters); extern void InitModules(void); /** -* OpenPilot Main function: -* -* Initialize PiOS
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
-* Start FreeRTOS Scheduler (vTaskStartScheduler)
-* If something goes wrong, blink LED1 and LED2 every 100ms -* -*/ + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler)
+ * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ int main() { - int result; + int result; - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ - vPortInitialiseBlocks(); + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + vPortInitialiseBlocks(); - /* Brings up System using CMSIS functions, enables the LEDs. */ - PIOS_SYS_Init(); - - /* For Revolution we use a FreeRTOS task to bring up the system so we can */ - /* always rely on FreeRTOS primitive */ - result = xTaskCreate(initTask, (const signed char *)"init", - INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, - &initTaskHandle); - PIOS_Assert(result == pdPASS); - - /* Start the FreeRTOS scheduler */ - vTaskStartScheduler(); + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); - /* If all is well we will never reach here as the scheduler will now be running. */ - /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ - PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ - for(;;) { \ - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ - PIOS_DELAY_WaitmS(100); \ - }; + /* For Revolution we use a FreeRTOS task to bring up the system so we can */ + /* always rely on FreeRTOS primitive */ + result = xTaskCreate(initTask, (const signed char *)"init", + INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, + &initTaskHandle); + PIOS_Assert(result == pdPASS); - return 0; + /* Start the FreeRTOS scheduler */ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ + PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ + for (;;) { \ + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ + PIOS_DELAY_WaitmS(100); \ + } + ; + + return 0; } /** * Initialisation task. * * Runs board and module initialisation, then terminates. */ -void -initTask(__attribute__((unused)) void *parameters) +void initTask(__attribute__((unused)) void *parameters) { - /* board driver init */ - PIOS_Board_Init(); - - /* Initialize modules */ - MODULE_INITIALISE_ALL; - - /* terminate this task */ - vTaskDelete(NULL); + /* board driver init */ + PIOS_Board_Init(); + + /* Initialize modules */ + MODULE_INITIALISE_ALL; + + /* terminate this task */ + vTaskDelete(NULL); } /** * @} * @} */ - diff --git a/flight/targets/boards/revoproto/pios_board.h b/flight/targets/boards/revoproto/pios_board.h index 0467cd1fc..7920341eb 100644 --- a/flight/targets/boards/revoproto/pios_board.h +++ b/flight/targets/boards/revoproto/pios_board.h @@ -31,26 +31,26 @@ #include -//------------------------ +// ------------------------ // Timers and Channels Used -//------------------------ +// ------------------------ /* - Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 - ------+-----------+-----------+-----------+---------- - TIM1 | | | | - TIM2 | --------------- PIOS_DELAY ----------------- - TIM3 | | | | - TIM4 | | | | - TIM5 | | | | - TIM6 | | | | - TIM7 | | | | - TIM8 | | | | - ------+-----------+-----------+-----------+---------- + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | | | | + TIM2 | --------------- PIOS_DELAY ----------------- + TIM3 | | | | + TIM4 | | | | + TIM5 | | | | + TIM6 | | | | + TIM7 | | | | + TIM8 | | | | + ------+-----------+-----------+-----------+---------- */ -//------------------------ +// ------------------------ // DMA Channels Used -//------------------------ +// ------------------------ /* Channel 1 - */ /* Channel 2 - SPI1 RX */ /* Channel 3 - SPI1 TX */ @@ -64,61 +64,61 @@ /* Channel 11 - */ /* Channel 12 - */ -//------------------------ +// ------------------------ // BOOTLOADER_SETTINGS -//------------------------ -#define BOARD_READABLE true -#define BOARD_WRITABLE true -#define MAX_DEL_RETRYS 3 +// ------------------------ +#define BOARD_READABLE true +#define BOARD_WRITABLE true +#define MAX_DEL_RETRYS 3 -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -#define PIOS_LED_HEARTBEAT 0 -#define PIOS_LED_ALARM 1 +// ------------------------ +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_LED_ALARM 1 -//------------------------ +// ------------------------ // PIOS_SPI // See also pios_board.c -//------------------------ -#define PIOS_SPI_MAX_DEVS 3 +// ------------------------ +#define PIOS_SPI_MAX_DEVS 3 -//------------------------ +// ------------------------ // PIOS_WDG -//------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -#define PIOS_WDG_REGISTER RTC_BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_SENSORS 0x0010 -#define PIOS_WDG_AUTOTUNE 0x0020 +// ------------------------ +#define PIOS_WATCHDOG_TIMEOUT 250 +#define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_SENSORS 0x0010 +#define PIOS_WDG_AUTOTUNE 0x0020 -//------------------------ +// ------------------------ // PIOS_I2C // See also pios_board.c -//------------------------ -#define PIOS_I2C_MAX_DEVS 3 +// ------------------------ +#define PIOS_I2C_MAX_DEVS 3 extern uint32_t pios_i2c_mag_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_adapter_id) +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_adapter_id) extern uint32_t pios_i2c_flexiport_adapter_id; -#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) -#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) -//------------------------- +// ------------------------- // PIOS_USART // // See also pios_board.c -//------------------------- -#define PIOS_USART_MAX_DEVS 5 +// ------------------------- +#define PIOS_USART_MAX_DEVS 5 -//------------------------- +// ------------------------- // PIOS_COM // // See also pios_board.c -//------------------------- -#define PIOS_COM_MAX_DEVS 4 +// ------------------------- +#define PIOS_COM_MAX_DEVS 4 extern uint32_t pios_com_telem_rf_id; extern uint32_t pios_com_gps_id; extern uint32_t pios_com_aux_id; @@ -126,143 +126,143 @@ extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_bridge_id; extern uint32_t pios_com_vcp_id; extern uint32_t pios_com_hkosd_id; -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_DEBUG PIOS_COM_AUX -#define PIOS_COM_OSDHK (pios_com_hkosd_id) +#define PIOS_COM_AUX (pios_com_aux_id) +#define PIOS_COM_GPS (pios_com_gps_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_BRIDGE (pios_com_bridge_id) +#define PIOS_COM_VCP (pios_com_vcp_id) +#define PIOS_COM_DEBUG PIOS_COM_AUX +#define PIOS_COM_OSDHK (pios_com_hkosd_id) -//------------------------ -// TELEMETRY -//------------------------ -#define TELEM_QUEUE_SIZE 80 -#define PIOS_TELEM_STACK_SIZE 624 +// ------------------------ +// TELEMETRY +// ------------------------ +#define TELEM_QUEUE_SIZE 80 +#define PIOS_TELEM_STACK_SIZE 624 -//------------------------- +// ------------------------- // System Settings -// +// // See also System_stm32f4xx.c -//------------------------- -//These macros are deprecated -//please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below -//#define PIOS_MASTER_CLOCK -//#define PIOS_PERIPHERAL_CLOCK -//#define PIOS_PERIPHERAL_CLOCK +// ------------------------- +// These macros are deprecated +// please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below +// #define PIOS_MASTER_CLOCK +// #define PIOS_PERIPHERAL_CLOCK +// #define PIOS_PERIPHERAL_CLOCK -#define PIOS_SYSCLK 168000000 -// Peripherals that belongs to APB1 are: -// DAC |PWR |CAN1,2 -// I2C1,2,3 |UART4,5 |USART3,2 -// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 -// I2S2Ext |IWDG |WWDG -// RTC/BKP reg +#define PIOS_SYSCLK 168000000 +// Peripherals that belongs to APB1 are: +// DAC |PWR |CAN1,2 +// I2C1,2,3 |UART4,5 |USART3,2 +// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 +// I2S2Ext |IWDG |WWDG +// RTC/BKP reg // TIM2,3,4,5,6,7,12,13,14 -// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) -// Default APB1 Prescaler = 4 -#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) +// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) +// Default APB1 Prescaler = 4 +#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) -// Peripherals belonging to APB2 -// SDIO |EXTI |SYSCFG |SPI1 -// ADC1,2,3 -// USART1,6 -// TIM1,8,9,10,11 +// Peripherals belonging to APB2 +// SDIO |EXTI |SYSCFG |SPI1 +// ADC1,2,3 +// USART1,6 +// TIM1,8,9,10,11 // // Default APB2 Prescaler = 2 // -#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK +#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK -//------------------------- +// ------------------------- // Interrupt Priorities -//------------------------- -#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS -#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS -#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... -#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... -//------------------------ +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------ // PIOS_RCVR // See also pios_board.c -//------------------------ +// ------------------------ #define PIOS_RCVR_MAX_DEVS 3 #define PIOS_RCVR_MAX_CHANNELS 12 #define PIOS_GCSRCVR_TIMEOUT_MS 100 -//------------------------- +// ------------------------- // Receiver PPM input -//------------------------- +// ------------------------- #define PIOS_PPM_MAX_DEVS 1 #define PIOS_PPM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver PWM input -//------------------------- +// ------------------------- #define PIOS_PWM_MAX_DEVS 1 #define PIOS_PWM_NUM_INPUTS 8 -//------------------------- +// ------------------------- // Receiver SPEKTRUM input -//------------------------- +// ------------------------- #define PIOS_SPEKTRUM_MAX_DEVS 2 #define PIOS_SPEKTRUM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver S.Bus input -//------------------------- +// ------------------------- #define PIOS_SBUS_MAX_DEVS 1 -#define PIOS_SBUS_NUM_INPUTS (16+2) +#define PIOS_SBUS_NUM_INPUTS (16 + 2) -//------------------------- +// ------------------------- // Receiver DSM input -//------------------------- +// ------------------------- #define PIOS_DSM_MAX_DEVS 2 #define PIOS_DSM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Servo outputs -//------------------------- -#define PIOS_SERVO_UPDATE_HZ 50 -#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ -//-------------------------- +// -------------------------- // Timer controller settings -//-------------------------- -#define PIOS_TIM_MAX_DEVS 6 +// -------------------------- +#define PIOS_TIM_MAX_DEVS 6 -//------------------------- +// ------------------------- // ADC // PIOS_ADC_PinGet(0) = Current sensor // PIOS_ADC_PinGet(1) = Voltage sensor // PIOS_ADC_PinGet(2) = VREF // PIOS_ADC_PinGet(3) = Temperature sensor // PIOS_ADC_PinGet(4) = Board Power -//------------------------- +// ------------------------- #define PIOS_DMA_PIN_CONFIG \ -{ \ - {GPIOC, GPIO_Pin_0, ADC_Channel_10}, \ - {GPIOC, GPIO_Pin_1, ADC_Channel_11}, \ - {NULL, 0, ADC_Channel_Vrefint}, /* Voltage reference */ \ - {NULL, 0, ADC_Channel_TempSensor}, /* Temperature sensor */ \ - {GPIOC, GPIO_Pin_2, ADC_Channel_12} \ -} + { \ + { GPIOC, GPIO_Pin_0, ADC_Channel_10 }, \ + { GPIOC, GPIO_Pin_1, ADC_Channel_11 }, \ + { NULL, 0, ADC_Channel_Vrefint }, /* Voltage reference */ \ + { NULL, 0, ADC_Channel_TempSensor }, /* Temperature sensor */ \ + { GPIOC, GPIO_Pin_2, ADC_Channel_12 } \ + } /* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */ /* which is annoying because this then determines the rate at which we generate buffer turnover events */ /* the objective here is to get enough buffer space to support 100Hz averaging rate */ -#define PIOS_ADC_NUM_CHANNELS 4 -#define PIOS_ADC_MAX_OVERSAMPLING 2 -#define PIOS_ADC_USE_ADC2 0 -#define PIOS_ADC_VOLTAGE_SCALE 3.30f/4096.0f -#define PIOS_ADC_USE_TEMP_SENSOR 1 +#define PIOS_ADC_NUM_CHANNELS 4 +#define PIOS_ADC_MAX_OVERSAMPLING 2 +#define PIOS_ADC_USE_ADC2 0 +#define PIOS_ADC_VOLTAGE_SCALE 3.30f / 4096.0f +#define PIOS_ADC_USE_TEMP_SENSOR 1 -//------------------------- +// ------------------------- // USB -//------------------------- -#define PIOS_USB_MAX_DEVS 1 -#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ -#define PIOS_USB_HID_MAX_DEVS 1 +// ------------------------- +#define PIOS_USB_MAX_DEVS 1 +#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ +#define PIOS_USB_HID_MAX_DEVS 1 #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/revoproto/pios_usb_board_data.c b/flight/targets/boards/revoproto/pios_usb_board_data.c index df136bab4..5a097b6bc 100644 --- a/flight/targets/boards/revoproto/pios_usb_board_data.c +++ b/flight/targets/boards/revoproto/pios_usb_board_data.c @@ -28,71 +28,72 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ -#include /* PIOS_SYS_SerialNumberGet */ -#include /* PIOS_USBHOOK_* */ -#include /* PIOS_USB_UTIL_AsciiToUtf8 */ +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ static const uint8_t usb_product_id[22] = { - sizeof(usb_product_id), - USB_DESC_TYPE_STRING, - 'R', 0, - 'e', 0, - 'v', 0, - 'o', 0, - 'l', 0, - 'u', 0, - 't', 0, - 'i', 0, - 'o', 0, - 'n', 0, + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'R', 0, + 'e', 0, + 'v', 0, + 'o', 0, + 'l', 0, + 'u', 0, + 't', 0, + 'i', 0, + 'o', 0, + 'n', 0, }; -static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN*2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1)*2] = { - sizeof(usb_serial_number), - USB_DESC_TYPE_STRING, +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, }; static const struct usb_string_langid usb_lang_id = { - .bLength = sizeof(usb_lang_id), - .bDescriptorType = USB_DESC_TYPE_STRING, - .bLangID = htousbs(USB_LANGID_ENGLISH_US), + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), }; static const uint8_t usb_vendor_id[28] = { - sizeof(usb_vendor_id), - USB_DESC_TYPE_STRING, - 'o', 0, - 'p', 0, - 'e', 0, - 'n', 0, - 'p', 0, - 'i', 0, - 'l', 0, - 'o', 0, - 't', 0, - '.', 0, - 'o', 0, - 'r', 0, - 'g', 0 + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 }; int32_t PIOS_USB_BOARD_DATA_Init(void) { - /* Load device serial number into serial number string */ - uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - PIOS_SYS_SerialNumberGet((char *)sn); + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; - /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ - uint8_t * utf8 = &(usb_serial_number[2]); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); - utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX)-1); + PIOS_SYS_SerialNumberGet((char *)sn); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); - PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); - return 0; + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; } diff --git a/flight/targets/boards/simposix/board_hw_defs.c b/flight/targets/boards/simposix/board_hw_defs.c index 0e02eba0b..9971a8d30 100644 --- a/flight/targets/boards/simposix/board_hw_defs.c +++ b/flight/targets/boards/simposix/board_hw_defs.c @@ -11,19 +11,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,8 +36,8 @@ * Telemetry on main USART */ const struct pios_udp_cfg pios_udp_telem_cfg = { - .ip = "0.0.0.0", - .port = 9000, + .ip = "0.0.0.0", + .port = 9000, }; #endif /* PIOS_COM_TELEM */ @@ -46,8 +46,8 @@ const struct pios_udp_cfg pios_udp_telem_cfg = { * GPS USART */ const struct pios_udp_cfg pios_udp_gps_cfg = { - .ip = "0.0.0.0", - .port = 9001, + .ip = "0.0.0.0", + .port = 9001, }; #endif /* PIOS_INCLUDE_GPS */ @@ -57,8 +57,8 @@ const struct pios_udp_cfg pios_udp_gps_cfg = { * AUX USART (UART label on rev2) */ const struct pios_udp_cfg pios_udp_aux_cfg = { - .ip = "0.0.0.0", - .port = 9002, + .ip = "0.0.0.0", + .port = 9002, }; #endif /* PIOS_COM_AUX */ @@ -69,4 +69,3 @@ const struct pios_udp_cfg pios_udp_aux_cfg = { #include #endif /* PIOS_INCLUDE_COM */ - diff --git a/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig.h index c2a334f41..2be6e0658 100644 --- a/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig.h +++ b/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig.h @@ -1,107 +1,103 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ -/* Notes: We use 5 task priorities */ +/* Notes: We use 5 task priorities */ -#ifdef __APPLE__ - #define COND_SIGNALING - #define CHECK_TASK_RESUMES - #define RUNNING_THREAD_MUTEX -// #define TICK_SIGNAL -// #define TICK_SIGWAIT - #define IDLE_SLEEPS +#ifdef __APPLE__ + #define COND_SIGNALING + #define CHECK_TASK_RESUMES + #define RUNNING_THREAD_MUTEX +// #define TICK_SIGNAL +// #define TICK_SIGWAIT + #define IDLE_SLEEPS - #define configUSE_PREEMPTION 1 - #define configIDLE_SHOULD_YIELD 0 + #define configUSE_PREEMPTION 1 + #define configIDLE_SHOULD_YIELD 0 #endif #ifdef __CYGWIN__ - #define COND_SIGNALING - #define CHECK_TASK_RESUMES -// #define RUNNING_THREAD_MUTEX -// #define TICK_SIGNAL - #define TICK_SIGWAIT - #define IDLE_SLEEPS + #define COND_SIGNALING + #define CHECK_TASK_RESUMES +// #define RUNNING_THREAD_MUTEX +// #define TICK_SIGNAL + #define TICK_SIGWAIT + #define IDLE_SLEEPS - #define configUSE_PREEMPTION 0 - #define configIDLE_SHOULD_YIELD 1 + #define configUSE_PREEMPTION 0 + #define configIDLE_SHOULD_YIELD 1 #endif #ifdef __linux__ - #define COND_SIGNALING - #define CHECK_TASK_RESUMES - #define RUNNING_THREAD_MUTEX -// #define TICK_SIGNAL -// #define TICK_SIGWAIT - #define IDLE_SLEEPS + #define COND_SIGNALING + #define CHECK_TASK_RESUMES + #define RUNNING_THREAD_MUTEX +// #define TICK_SIGNAL +// #define TICK_SIGWAIT + #define IDLE_SLEEPS - #define configUSE_PREEMPTION 1 - #define configIDLE_SHOULD_YIELD 0 + #define configUSE_PREEMPTION 1 + #define configIDLE_SHOULD_YIELD 0 #endif -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 ) -#define configTICK_RATE_HZ ( ( portTickType ) 1000 ) -#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) -#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 256 ) -#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 45 * 1024 ) ) -#define configMAX_TASK_NAME_LEN ( 16 ) -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)256) +#define configTOTAL_HEAP_SIZE ((size_t)(45 * 1024)) +#define configMAX_TASK_NAME_LEN (16) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ - -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 0 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 0 - + to exclude the API function. */ +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 0 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 #endif /* FREERTOS_CONFIG_H */ - diff --git a/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig_bk.h b/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig_bk.h index 72d6e288a..3792f665b 100644 --- a/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig_bk.h +++ b/flight/targets/boards/simposix/firmware/inc/FreeRTOSConfig_bk.h @@ -1,85 +1,84 @@ - #ifndef FREERTOS_CONFIG_H #define FREERTOS_CONFIG_H /*----------------------------------------------------------- - * Application specific definitions. - * - * These definitions should be adjusted for your particular hardware and - * application requirements. - * - * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE - * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. - * - * See http://www.freertos.org/a00110.html. - *----------------------------------------------------------*/ +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ /** - * @addtogroup PIOS PIOS - * @{ - * @addtogroup FreeRTOS FreeRTOS - * @{ - */ + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ /* Notes: We use 5 task priorities */ -#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... -#define configTICK_RATE_HZ ((portTickType )1000) -#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) -#define configMINIMAL_STACK_SIZE ((unsigned short)512) -#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total -#define configMAX_TASK_NAME_LEN (16) +#define configCPU_CLOCK_HZ (SYSCLK_FREQ) // really the NVIC clock ... +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(180 * 1024)) // this is minimum, not total +#define configMAX_TASK_NAME_LEN (16) -#define configUSE_PREEMPTION 1 -#define configUSE_IDLE_HOOK 1 -#define configUSE_TICK_HOOK 0 -#define configUSE_TRACE_FACILITY 0 -#define configUSE_16_BIT_TICKS 0 -#define configIDLE_SHOULD_YIELD 0 -#define configUSE_MUTEXES 1 -#define configUSE_RECURSIVE_MUTEXES 1 -#define configUSE_COUNTING_SEMAPHORES 0 -#define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configCHECK_FOR_STACK_OVERFLOW 2 +#define configQUEUE_REGISTRY_SIZE 10 -#define configUSE_TIMERS 1 -#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ -#define configTIMER_QUEUE_LENGTH 10 -#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE +#define configUSE_TIMERS 1 +#define configTIMER_TASK_PRIORITY (configMAX_PRIORITIES - 1) /* run timers at max priority */ +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE /* Co-routine definitions. */ -#define configUSE_CO_ROUTINES 0 -//#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) +#define configUSE_CO_ROUTINES 0 +// #define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) /* Set the following definitions to 1 to include the API function, or zero -to exclude the API function. */ + to exclude the API function. */ -#define INCLUDE_vTaskPrioritySet 1 -#define INCLUDE_uxTaskPriorityGet 1 -#define INCLUDE_vTaskDelete 1 -#define INCLUDE_vTaskCleanUpResources 1 -#define INCLUDE_vTaskSuspend 1 -#define INCLUDE_vTaskDelayUntil 1 -#define INCLUDE_vTaskDelay 1 -#define INCLUDE_xTaskGetSchedulerState 1 -#define INCLUDE_xTaskGetCurrentTaskHandle 1 -#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 1 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 /* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 -(lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ -#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ -#define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ /* This is the value being used as per the ST library which permits 16 -priority values, 0 to 15. This must correspond to the -configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest -NVIC value of 255. */ -#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 /* Enable run time stats collection */ -#define configGENERATE_RUN_TIME_STATS 1 -#define INCLUDE_uxTaskGetRunTime 1 +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 /* * Once we move to CMSIS2 we can at least use: @@ -88,16 +87,17 @@ NVIC value of 255. */ * * (still nothing for the DWT registers, surprisingly) */ -#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ - do { \ - (*(unsigned long *)0xe000edfc) |= (1<<24); /* DEMCR |= DEMCR_TRCENA */ \ - (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ - } while(0) -#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ /** - * @} - */ + * @} + */ #endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/simposix/firmware/inc/dcc_stdio.h b/flight/targets/boards/simposix/firmware/inc/dcc_stdio.h index dbfd39342..3d7354918 100644 --- a/flight/targets/boards/simposix/firmware/inc/dcc_stdio.h +++ b/flight/targets/boards/simposix/firmware/inc/dcc_stdio.h @@ -1,24 +1,24 @@ /*************************************************************************** - * Copyright (C) 2008 by Dominic Rath * - * Dominic.Rath@gmx.de * - * Copyright (C) 2008 by Spencer Oliver * - * spen@spen-soft.co.uk * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ #ifndef DCC_STDIO_H #define DCC_STDIO_H @@ -33,4 +33,4 @@ void dbg_write_str(const char *msg); void dbg_write_char(char msg); void dbg_write_hex32(const unsigned long val); -#endif /* DCC_STDIO_H */ +#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/simposix/firmware/inc/openpilot.h b/flight/targets/boards/simposix/firmware/inc/openpilot.h index 8c76835cd..3cceef152 100644 --- a/flight/targets/boards/simposix/firmware/inc/openpilot.h +++ b/flight/targets/boards/simposix/firmware/inc/openpilot.h @@ -10,19 +10,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h index 3396f39e6..70e3ed2b6 100644 --- a/flight/targets/boards/simposix/firmware/inc/pios_config.h +++ b/flight/targets/boards/simposix/firmware/inc/pios_config.h @@ -5,28 +5,28 @@ * @addtogroup OpenPilotCore OpenPilot Core * @{ * - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * In particular, pios_config.h is where you define which PiOS libraries * and features are included in the firmware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,34 +39,34 @@ #define PIOS_INCLUDE_BL_HELPER /* Enable/Disable PiOS Modules */ -//#define PIOS_INCLUDE_ADC +// #define PIOS_INCLUDE_ADC #define PIOS_INCLUDE_DELAY -//#define PIOS_INCLUDE_I2C +// #define PIOS_INCLUDE_I2C #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_LED #define PIOS_INCLUDE_SDCARD -//#define PIOS_INCLUDE_IAP +// #define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_SERVO #define PIOS_INCLUDE_SPI #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_TASK_MONITOR #define PIOS_INCLUDE_USART -//#define PIOS_INCLUDE_USB +// #define PIOS_INCLUDE_USB #define PIOS_INCLUDE_USB_HID -//#define PIOS_INCLUDE_GPIO +// #define PIOS_INCLUDE_GPIO #define PIOS_INCLUDE_EXTI #define PIOS_INCLUDE_RTC #define PIOS_INCLUDE_WDG #define PIOS_INCLUDE_UDP /* Select the sensors to include */ -//#define PIOS_INCLUDE_BMA180 -//#define PIOS_INCLUDE_HMC5883 -//#define PIOS_INCLUDE_MPU6000 -//#define PIOS_MPU6000_ACCEL -//#define PIOS_INCLUDE_L3GD20 -//#define PIOS_INCLUDE_MS5611 -//#define PIOS_INCLUDE_HCSR04 +// #define PIOS_INCLUDE_BMA180 +// #define PIOS_INCLUDE_HMC5883 +// #define PIOS_INCLUDE_MPU6000 +// #define PIOS_MPU6000_ACCEL +// #define PIOS_INCLUDE_L3GD20 +// #define PIOS_INCLUDE_MS5611 +// #define PIOS_INCLUDE_HCSR04 #define PIOS_FLASH_ON_ACCEL /* true for second revo */ #define FLASH_FREERTOS /* Com systems to include */ @@ -81,34 +81,34 @@ /* Supported receiver interfaces */ #define PIOS_INCLUDE_RCVR #define PIOS_INCLUDE_DSM -//#define PIOS_INCLUDE_SBUS +// #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_PWM -//#define PIOS_INCLUDE_GCSRCVR +// #define PIOS_INCLUDE_GCSRCVR #define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_BL_HELPER #define PIOS_INCLUDE_SETTINGS #define PIOS_INCLUDE_FLASH /* A really shitty setting saving implementation */ -//#define PIOS_INCLUDE_FLASH_LOGFS_SETTINGS +// #define PIOS_INCLUDE_FLASH_LOGFS_SETTINGS /* Other Interfaces */ -//#define PIOS_INCLUDE_I2C_ESC +// #define PIOS_INCLUDE_I2C_ESC /* Flags that alter behaviors - mostly to lower resources for CC */ -#define PIOS_INCLUDE_INITCALL /* Include init call structures */ -#define PIOS_TELEM_PRIORITY_QUEUE /* Enable a priority queue in telemetry */ -#define PIOS_QUATERNION_STABILIZATION /* Stabilization options */ -//#define PIOS_GPS_SETS_HOMELOCATION /* GPS options */ +#define PIOS_INCLUDE_INITCALL /* Include init call structures */ +#define PIOS_TELEM_PRIORITY_QUEUE /* Enable a priority queue in telemetry */ +#define PIOS_QUATERNION_STABILIZATION /* Stabilization options */ +// #define PIOS_GPS_SETS_HOMELOCATION /* GPS options */ /* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 4000 -#define HEAP_LIMIT_CRITICAL 1000 -#define IRQSTACK_LIMIT_WARNING 150 -#define IRQSTACK_LIMIT_CRITICAL 80 -#define CPULOAD_LIMIT_WARNING 80 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 4000 +#define HEAP_LIMIT_CRITICAL 1000 +#define IRQSTACK_LIMIT_WARNING 150 +#define IRQSTACK_LIMIT_CRITICAL 80 +#define CPULOAD_LIMIT_WARNING 80 +#define CPULOAD_LIMIT_CRITICAL 95 // This actually needs calibrating #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD (8379692) diff --git a/flight/targets/boards/simposix/firmware/pios_board.c b/flight/targets/boards/simposix/firmware/pios_board.c index a86a2c639..06109fcbc 100644 --- a/flight/targets/boards/simposix/firmware/pios_board.c +++ b/flight/targets/boards/simposix/firmware/pios_board.c @@ -11,19 +11,19 @@ * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -45,7 +45,7 @@ #include "../board_hw_defs.c" /** - * Sensor configurations + * Sensor configurations */ /* One slot per selectable receiver group. @@ -54,58 +54,58 @@ */ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 -#define PIOS_COM_GPS_RX_BUF_LEN 32 +#define PIOS_COM_GPS_RX_BUF_LEN 32 #define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 -#define PIOS_COM_AUX_RX_BUF_LEN 512 -#define PIOS_COM_AUX_TX_BUF_LEN 512 +#define PIOS_COM_AUX_RX_BUF_LEN 512 +#define PIOS_COM_AUX_TX_BUF_LEN 512 -uint32_t pios_com_aux_id = 0; -uint32_t pios_com_gps_id = 0; +uint32_t pios_com_aux_id = 0; +uint32_t pios_com_gps_id = 0; uint32_t pios_com_telem_usb_id = 0; -uint32_t pios_com_telem_rf_id = 0; -uint32_t pios_com_bridge_id = 0; +uint32_t pios_com_telem_rf_id = 0; +uint32_t pios_com_bridge_id = 0; uintptr_t pios_uavo_settings_fs_id; -/* +/* * Setup a com port based on the passed cfg, driver and buffer sizes. tx size of -1 make the port rx only */ static void PIOS_Board_configure_com(const struct pios_udp_cfg *usart_port_cfg, size_t rx_buf_len, size_t tx_buf_len, - const struct pios_com_driver *com_driver, uint32_t *pios_com_id) + const struct pios_com_driver *com_driver, uint32_t *pios_com_id) { - uint32_t pios_usart_id; - if (PIOS_UDP_Init(&pios_usart_id, usart_port_cfg)) { - PIOS_Assert(0); - } - - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(rx_buf_len); - PIOS_Assert(rx_buffer); - if(tx_buf_len!= -1){ // this is the case for rx/tx ports - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(tx_buf_len); - PIOS_Assert(tx_buffer); - - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - tx_buffer, tx_buf_len)) { - PIOS_Assert(0); - } - } - else{ //rx only port - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - NULL, 0)) { - PIOS_Assert(0); - } - } + uint32_t pios_usart_id; + + if (PIOS_UDP_Init(&pios_usart_id, usart_port_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(rx_buf_len); + PIOS_Assert(rx_buffer); + if (tx_buf_len != -1) { // this is the case for rx/tx ports + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(tx_buf_len); + PIOS_Assert(tx_buffer); + + if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, + rx_buffer, rx_buf_len, + tx_buffer, tx_buf_len)) { + PIOS_Assert(0); + } + } else { // rx only port + if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, + rx_buffer, rx_buf_len, + NULL, 0)) { + PIOS_Assert(0); + } + } } /** @@ -113,90 +113,87 @@ static void PIOS_Board_configure_com(const struct pios_udp_cfg *usart_port_cfg, * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ -void PIOS_Board_Init(void) { - - /* Delay system */ - PIOS_DELAY_Init(); +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); - HwSettingsInitialize(); - - UAVObjectsInitializeAll(); - - /* Initialize the alarms library */ - AlarmsInitialize(); + HwSettingsInitialize(); - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } + UAVObjectsInitializeAll(); - /* Initialize the delayed callback library */ - CallbackSchedulerInitialize(); + /* Initialize the alarms library */ + AlarmsInitialize(); - /* Configure IO ports */ - - /* Configure Telemetry port */ - uint8_t hwsettings_rv_telemetryport; - HwSettingsRV_TelemetryPortGet(&hwsettings_rv_telemetryport); + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } - switch (hwsettings_rv_telemetryport){ - case HWSETTINGS_RV_TELEMETRYPORT_DISABLED: - break; - case HWSETTINGS_RV_TELEMETRYPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_udp_telem_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RV_TELEMETRYPORT_COMAUX: - PIOS_Board_configure_com(&pios_udp_telem_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); - break; - - } /* hwsettings_rv_telemetryport */ + /* Initialize the delayed callback library */ + CallbackSchedulerInitialize(); - /* Configure GPS port */ - uint8_t hwsettings_rv_gpsport; - HwSettingsRV_GPSPortGet(&hwsettings_rv_gpsport); - switch (hwsettings_rv_gpsport){ - case HWSETTINGS_RV_GPSPORT_DISABLED: - break; - - case HWSETTINGS_RV_GPSPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); - break; - - case HWSETTINGS_RV_GPSPORT_GPS: - PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_udp_com_driver, &pios_com_gps_id); - break; - - case HWSETTINGS_RV_GPSPORT_COMAUX: - PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); - break; - - }/* hwsettings_rv_gpsport */ + /* Configure IO ports */ - /* Configure AUXPort */ - uint8_t hwsettings_rv_auxport; - HwSettingsRV_AuxPortGet(&hwsettings_rv_auxport); + /* Configure Telemetry port */ + uint8_t hwsettings_rv_telemetryport; + HwSettingsRV_TelemetryPortGet(&hwsettings_rv_telemetryport); - switch (hwsettings_rv_auxport) { - case HWSETTINGS_RV_AUXPORT_DISABLED: - break; - - case HWSETTINGS_RV_AUXPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_udp_aux_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); - break; - - case HWSETTINGS_RV_AUXPORT_COMAUX: - PIOS_Board_configure_com(&pios_udp_aux_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); - break; - break; - } /* hwsettings_rv_auxport */ + switch (hwsettings_rv_telemetryport) { + case HWSETTINGS_RV_TELEMETRYPORT_DISABLED: + break; + case HWSETTINGS_RV_TELEMETRYPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_udp_telem_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); + break; + case HWSETTINGS_RV_TELEMETRYPORT_COMAUX: + PIOS_Board_configure_com(&pios_udp_telem_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); + break; + } /* hwsettings_rv_telemetryport */ + + /* Configure GPS port */ + uint8_t hwsettings_rv_gpsport; + HwSettingsRV_GPSPortGet(&hwsettings_rv_gpsport); + switch (hwsettings_rv_gpsport) { + case HWSETTINGS_RV_GPSPORT_DISABLED: + break; + + case HWSETTINGS_RV_GPSPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); + break; + + case HWSETTINGS_RV_GPSPORT_GPS: + PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_udp_com_driver, &pios_com_gps_id); + break; + + case HWSETTINGS_RV_GPSPORT_COMAUX: + PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); + break; + } /* hwsettings_rv_gpsport */ + + /* Configure AUXPort */ + uint8_t hwsettings_rv_auxport; + HwSettingsRV_AuxPortGet(&hwsettings_rv_auxport); + + switch (hwsettings_rv_auxport) { + case HWSETTINGS_RV_AUXPORT_DISABLED: + break; + + case HWSETTINGS_RV_AUXPORT_TELEMETRY: + PIOS_Board_configure_com(&pios_udp_aux_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_telem_rf_id); + break; + + case HWSETTINGS_RV_AUXPORT_COMAUX: + PIOS_Board_configure_com(&pios_udp_aux_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id); + break; + break; + } /* hwsettings_rv_auxport */ } /** * @} * @} */ - diff --git a/flight/targets/boards/simposix/firmware/simposix.c b/flight/targets/boards/simposix/firmware/simposix.c index 4b6620b9f..3b51a78bd 100644 --- a/flight/targets/boards/simposix/firmware/simposix.c +++ b/flight/targets/boards/simposix/firmware/simposix.c @@ -9,25 +9,25 @@ * @brief This is where the OP firmware starts. Those files also define the compile-time * options of the firmware. * @{ - * @file openpilot.c + * @file openpilot.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Sets up and runs main OpenPilot tasks. * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,12 +36,12 @@ #include /* Task Priorities */ -#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) /* Global Variables */ /* Local Variables */ -#define INCLUDE_TEST_TASKS 0 +#define INCLUDE_TEST_TASKS 0 #if INCLUDE_TEST_TASKS static uint8_t sdcard_available; #endif @@ -63,11 +63,11 @@ void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value); /* Prototype of PIOS_Board_Init() function */ extern void PIOS_Board_Init(void); extern void Stack_Change(void); -static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change"))); +static void Stack_Change_Weak() __attribute__((weakref("Stack_Change"))); /* Local Variables */ -#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority -#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive +#define INIT_TASK_PRIORITY (tskIDLE_PRIORITY + configMAX_PRIORITIES - 1) // max priority +#define INIT_TASK_STACK (1024 / 4) // XXX this seems excessive static xTaskHandle initTaskHandle; /* Function Prototypes */ @@ -77,64 +77,63 @@ static void initTask(void *parameters); extern void InitModules(void); /** -* OpenPilot Main function: -* -* Initialize PiOS
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
-* Start FreeRTOS Scheduler (vTaskStartScheduler)
-* If something goes wrong, blink LED1 and LED2 every 100ms -* -*/ + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler)
+ * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ int main() { - int result; + int result; - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ - /* Brings up System using CMSIS functions, enables the LEDs. */ - PIOS_SYS_Init(); - - /* For Revolution we use a FreeRTOS task to bring up the system so we can */ - /* always rely on FreeRTOS primitive */ - result = xTaskCreate(initTask, (const signed char *)"init", - INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, - &initTaskHandle); - PIOS_Assert(result == pdPASS); - - /* Start the FreeRTOS scheduler */ - vTaskStartScheduler(); + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); - /* If all is well we will never reach here as the scheduler will now be running. */ - /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ - PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ - for(;;) { \ - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ - PIOS_DELAY_WaitmS(100); \ - }; + /* For Revolution we use a FreeRTOS task to bring up the system so we can */ + /* always rely on FreeRTOS primitive */ + result = xTaskCreate(initTask, (const signed char *)"init", + INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, + &initTaskHandle); + PIOS_Assert(result == pdPASS); - return 0; + /* Start the FreeRTOS scheduler */ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ + PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ + for (;;) { \ + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ + PIOS_DELAY_WaitmS(100); \ + } + ; + + return 0; } /** * Initialisation task. * * Runs board and module initialisation, then terminates. */ -void -initTask(__attribute__((unused)) void *parameters) +void initTask(__attribute__((unused)) void *parameters) { - /* board driver init */ - PIOS_Board_Init(); - - /* Initialize modules */ - MODULE_INITIALISE_ALL; - - /* terminate this task */ - vTaskDelete(NULL); + /* board driver init */ + PIOS_Board_Init(); + + /* Initialize modules */ + MODULE_INITIALISE_ALL; + + /* terminate this task */ + vTaskDelete(NULL); } /** * @} * @} */ - diff --git a/flight/targets/boards/simposix/pios_board.h b/flight/targets/boards/simposix/pios_board.h index 196124ec6..c0d837f53 100644 --- a/flight/targets/boards/simposix/pios_board.h +++ b/flight/targets/boards/simposix/pios_board.h @@ -32,34 +32,34 @@ /** * glue macros for file IO **/ -#define FILEINFO FILE* -#define PIOS_FOPEN_READ(filename,file) (file=fopen((char*)filename,"r"))==NULL -#define PIOS_FOPEN_WRITE(filename,file) (file=fopen((char*)filename,"w"))==NULL -#define PIOS_FREAD(file,bufferadr,length,resultadr) (*resultadr=fread((uint8_t*)bufferadr,1,length,*file)) != length -#define PIOS_FWRITE(file,bufferadr,length,resultadr) *resultadr=fwrite((uint8_t*)bufferadr,1,length,*file) -#define PIOS_FCLOSE(file) fclose(file) -#define PIOS_FUNLINK(file) unlink((char*)filename) +#define FILEINFO FILE * +#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL +#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL +#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length +#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) +#define PIOS_FCLOSE(file) fclose(file) +#define PIOS_FUNLINK(file) unlink((char *)filename) -//------------------------ +// ------------------------ // Timers and Channels Used -//------------------------ +// ------------------------ /* -Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 -------+-----------+-----------+-----------+---------- -TIM1 | | | | -TIM2 | --------------- PIOS_DELAY ----------------- -TIM3 | | | | -TIM4 | | | | -TIM5 | | | | -TIM6 | | | | -TIM7 | | | | -TIM8 | | | | -------+-----------+-----------+-----------+---------- -*/ + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | | | | + TIM2 | --------------- PIOS_DELAY ----------------- + TIM3 | | | | + TIM4 | | | | + TIM5 | | | | + TIM6 | | | | + TIM7 | | | | + TIM8 | | | | + ------+-----------+-----------+-----------+---------- + */ -//------------------------ +// ------------------------ // DMA Channels Used -//------------------------ +// ------------------------ /* Channel 1 - */ /* Channel 2 - SPI1 RX */ /* Channel 3 - SPI1 TX */ @@ -73,186 +73,186 @@ TIM8 | | | | /* Channel 11 - */ /* Channel 12 - */ -//------------------------ +// ------------------------ // BOOTLOADER_SETTINGS -//------------------------ -//#define BOARD_READABLE true -//#define BOARD_WRITABLE true -//#define MAX_DEL_RETRYS 3 +// ------------------------ +// #define BOARD_READABLE true +// #define BOARD_WRITABLE true +// #define MAX_DEL_RETRYS 3 -//------------------------ +// ------------------------ // PIOS_LED -//------------------------ -#define PIOS_LED_NUM 3 -#define PIOS_LED_HEARTBEAT 0 -#define PIOS_LED_ALARM 1 +// ------------------------ +#define PIOS_LED_NUM 3 +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_LED_ALARM 1 -//------------------------ +// ------------------------ // PIOS_SPI // See also pios_board.c -//------------------------ -//#define PIOS_SPI_MAX_DEVS 3 +// ------------------------ +// #define PIOS_SPI_MAX_DEVS 3 -//------------------------ +// ------------------------ // PIOS_WDG -//------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -//#define PIOS_WDG_REGISTER RTC_BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_SENSORS 0x0010 +// ------------------------ +#define PIOS_WATCHDOG_TIMEOUT 250 +// #define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_SENSORS 0x0010 -//------------------------ +// ------------------------ // PIOS_I2C // See also pios_board.c -//------------------------ -//#define PIOS_I2C_MAX_DEVS 3 -//extern uint32_t pios_i2c_mag_adapter_id; -//#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_adapter_id) +// ------------------------ +// #define PIOS_I2C_MAX_DEVS 3 +// extern uint32_t pios_i2c_mag_adapter_id; +// #define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_adapter_id) -//------------------------- +// ------------------------- // PIOS_USART // // See also pios_board.c -//------------------------- -//#define PIOS_USART_MAX_DEVS 5 +// ------------------------- +// #define PIOS_USART_MAX_DEVS 5 -//------------------------- +// ------------------------- // PIOS_COM // // See also pios_board.c -//------------------------- -#define PIOS_COM_MAX_DEVS 25 +// ------------------------- +#define PIOS_COM_MAX_DEVS 25 extern uint32_t pios_com_telem_rf_id; extern uint32_t pios_com_gps_id; extern uint32_t pios_com_aux_id; extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_bridge_id; extern uint32_t pios_com_vcp_id; -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_DEBUG PIOS_COM_AUX +#define PIOS_COM_AUX (pios_com_aux_id) +#define PIOS_COM_GPS (pios_com_gps_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_BRIDGE (pios_com_bridge_id) +#define PIOS_COM_VCP (pios_com_vcp_id) +#define PIOS_COM_DEBUG PIOS_COM_AUX -//------------------------ -// TELEMETRY -//------------------------ -#define TELEM_QUEUE_SIZE 20 -#define PIOS_TELEM_STACK_SIZE 624 +// ------------------------ +// TELEMETRY +// ------------------------ +#define TELEM_QUEUE_SIZE 20 +#define PIOS_TELEM_STACK_SIZE 624 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_UDP_TX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +#define PIOS_COM_BUFFER_SIZE 1024 +#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE +#define PIOS_UDP_TX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -//------------------------- +// ------------------------- // System Settings -// +// // See also System_stm32f4xx.c -//------------------------- -//These macros are deprecated -//please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below -//#define PIOS_MASTER_CLOCK -//#define PIOS_PERIPHERAL_CLOCK -//#define PIOS_PERIPHERAL_CLOCK +// ------------------------- +// These macros are deprecated +// please use PIOS_PERIPHERAL_APBx_CLOCK According to the table below +// #define PIOS_MASTER_CLOCK +// #define PIOS_PERIPHERAL_CLOCK +// #define PIOS_PERIPHERAL_CLOCK -#define PIOS_SYSCLK 168000000 -// Peripherals that belongs to APB1 are: -// DAC |PWR |CAN1,2 -// I2C1,2,3 |UART4,5 |USART3,2 -// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 -// I2S2Ext |IWDG |WWDG -// RTC/BKP reg +#define PIOS_SYSCLK 168000000 +// Peripherals that belongs to APB1 are: +// DAC |PWR |CAN1,2 +// I2C1,2,3 |UART4,5 |USART3,2 +// I2S3Ext |SPI3/I2S3 |SPI2/I2S2 +// I2S2Ext |IWDG |WWDG +// RTC/BKP reg // TIM2,3,4,5,6,7,12,13,14 -// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) -// Default APB1 Prescaler = 4 -//#define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) +// Calculated as SYSCLK / APBPresc * (APBPre == 1 ? 1 : 2) +// Default APB1 Prescaler = 4 +// #define PIOS_PERIPHERAL_APB1_CLOCK (PIOS_SYSCLK / 2) -// Peripherals belonging to APB2 -// SDIO |EXTI |SYSCFG |SPI1 -// ADC1,2,3 -// USART1,6 -// TIM1,8,9,10,11 +// Peripherals belonging to APB2 +// SDIO |EXTI |SYSCFG |SPI1 +// ADC1,2,3 +// USART1,6 +// TIM1,8,9,10,11 // // Default APB2 Prescaler = 2 // -//#define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK +// #define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK -//------------------------- +// ------------------------- // Interrupt Priorities -//------------------------- -//#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS -//#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS -//#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... -//#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------- +// #define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +// #define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +// #define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +// #define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... -//------------------------ +// ------------------------ // PIOS_RCVR // See also pios_board.c -//------------------------ -#define PIOS_RCVR_MAX_DEVS 3 -#define PIOS_RCVR_MAX_CHANNELS 12 +// ------------------------ +#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 -//------------------------- +// ------------------------- // Receiver PPM input -//------------------------- -//#define PIOS_PPM_MAX_DEVS 1 -//#define PIOS_PPM_NUM_INPUTS 12 +// ------------------------- +// #define PIOS_PPM_MAX_DEVS 1 +// #define PIOS_PPM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver PWM input -//------------------------- -//#define PIOS_PWM_MAX_DEVS 1 -//#define PIOS_PWM_NUM_INPUTS 8 +// ------------------------- +// #define PIOS_PWM_MAX_DEVS 1 +// #define PIOS_PWM_NUM_INPUTS 8 -//------------------------- +// ------------------------- // Receiver SPEKTRUM input -//------------------------- -//#define PIOS_SPEKTRUM_MAX_DEVS 2 -//#define PIOS_SPEKTRUM_NUM_INPUTS 12 +// ------------------------- +// #define PIOS_SPEKTRUM_MAX_DEVS 2 +// #define PIOS_SPEKTRUM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Receiver S.Bus input -//------------------------- -//#define PIOS_SBUS_MAX_DEVS 1 -//#define PIOS_SBUS_NUM_INPUTS (16+2) +// ------------------------- +// #define PIOS_SBUS_MAX_DEVS 1 +// #define PIOS_SBUS_NUM_INPUTS (16+2) -//------------------------- +// ------------------------- // Receiver DSM input -//------------------------- -//#define PIOS_DSM_MAX_DEVS 2 -//#define PIOS_DSM_NUM_INPUTS 12 +// ------------------------- +// #define PIOS_DSM_MAX_DEVS 2 +// #define PIOS_DSM_NUM_INPUTS 12 -//------------------------- +// ------------------------- // Servo outputs -//------------------------- -//#define PIOS_SERVO_UPDATE_HZ 50 -//#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +// ------------------------- +// #define PIOS_SERVO_UPDATE_HZ 50 +// #define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ #define PIOS_SERVO_NUM_OUTPUTS 8 -#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS -//-------------------------- +#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS +// -------------------------- // Timer controller settings -//-------------------------- -//#define PIOS_TIM_MAX_DEVS 6 +// -------------------------- +// #define PIOS_TIM_MAX_DEVS 6 -//------------------------- +// ------------------------- // ADC // None. -//------------------------- +// ------------------------- -//------------------------- +// ------------------------- // USB -//------------------------- -//#define PIOS_USB_MAX_DEVS 1 -//#define PIOS_USB_ENABLED 1 /* Should remove all references to this */ -//#define PIOS_USB_HID_MAX_DEVS 1 +// ------------------------- +// #define PIOS_USB_MAX_DEVS 1 +// #define PIOS_USB_ENABLED 1 /* Should remove all references to this */ +// #define PIOS_USB_HID_MAX_DEVS 1 #endif /* PIOS_BOARD_H */ diff --git a/flight/targets/common/bootloader_updater/inc/pios_config.h b/flight/targets/common/bootloader_updater/inc/pios_config.h index 9fbd71501..8f54f402b 100644 --- a/flight/targets/common/bootloader_updater/inc/pios_config.h +++ b/flight/targets/common/bootloader_updater/inc/pios_config.h @@ -2,28 +2,28 @@ ****************************************************************************** * @addtogroup OpenPilotBL OpenPilot BootLoader * @{ - * @file pios_config.h + * @file pios_config.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. + * @brief PiOS configuration header. * Central compile time config for the project. * In particular, pios_config.h is where you define which PiOS libraries * and features are included in the firmware. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/flight/targets/common/bootloader_updater/main.c b/flight/targets/common/bootloader_updater/main.c index 4a55a7aed..385bbdd3d 100644 --- a/flight/targets/common/bootloader_updater/main.c +++ b/flight/targets/common/bootloader_updater/main.c @@ -8,21 +8,21 @@ * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief This is the file with the main function of the OpenPilot BootLoader * @see The GNU Public License (GPL) Version 3 - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,13 +44,12 @@ void error(int, int); extern uint32_t _binary_start; extern uint32_t _binary_end; extern uint32_t _binary_size; -const uint32_t *embedded_image_start = (uint32_t *) &(_binary_start); -const uint32_t *embedded_image_end = (uint32_t *) &(_binary_end); -const uint32_t embedded_image_size = (uint32_t) &(_binary_size); +const uint32_t *embedded_image_start = (uint32_t *)&(_binary_start); +const uint32_t *embedded_image_end = (uint32_t *)&(_binary_end); +const uint32_t embedded_image_size = (uint32_t)&(_binary_size); int main() { - PIOS_SYS_Init(); PIOS_Board_Init(); PIOS_LED_On(PIOS_LED_HEARTBEAT); @@ -58,9 +57,10 @@ int main() PIOS_LED_Off(PIOS_LED_HEARTBEAT); /// Self overwrite check - uint32_t base_address = SCB ->VTOR; - if ((BL_BANK_BASE + embedded_image_size) > base_address) + uint32_t base_address = SCB->VTOR; + if ((BL_BANK_BASE + embedded_image_size) > base_address) { error(PIOS_LED_HEARTBEAT, 1); + } /// /* @@ -72,10 +72,10 @@ int main() */ /* Calculate how far the board_info_blob is from the beginning of the bootloader */ - uint32_t board_info_blob_offset = (uint32_t) &pios_board_info_blob - (uint32_t) BL_BANK_BASE; + uint32_t board_info_blob_offset = (uint32_t)&pios_board_info_blob - (uint32_t)BL_BANK_BASE; /* Use the same offset into our embedded bootloader image */ - struct pios_board_info *new_board_info_blob = (struct pios_board_info *) ((uint32_t) embedded_image_start + board_info_blob_offset); + struct pios_board_info *new_board_info_blob = (struct pios_board_info *)((uint32_t)embedded_image_start + board_info_blob_offset); /* Compare the two board info blobs to make sure they're for the same HW revision */ if ((pios_board_info_blob.magic != new_board_info_blob->magic) || @@ -92,7 +92,7 @@ int main() fail = (PIOS_BL_HELPER_FLASH_Erase_Bootloader() != 1); - if (fail == true){ + if (fail == true) { error(PIOS_LED_HEARTBEAT, 3); } /// @@ -125,19 +125,18 @@ int main() for (;;) { PIOS_DELAY_WaitmS(1000); } - } void error(int led, int code) { - for (;;) { - PIOS_DELAY_WaitmS(1000); - for (int x = 0; x < code; x++) { - PIOS_LED_On(led); - PIOS_DELAY_WaitmS(200); - PIOS_LED_Off(led); - PIOS_DELAY_WaitmS(1000); - } - PIOS_DELAY_WaitmS(3000); - } + for (;;) { + PIOS_DELAY_WaitmS(1000); + for (int x = 0; x < code; x++) { + PIOS_LED_On(led); + PIOS_DELAY_WaitmS(200); + PIOS_LED_Off(led); + PIOS_DELAY_WaitmS(1000); + } + PIOS_DELAY_WaitmS(3000); + } } diff --git a/flight/targets/common/bootloader_updater/pios_board.c b/flight/targets/common/bootloader_updater/pios_board.c index accc300f9..8ef335e3f 100644 --- a/flight/targets/common/bootloader_updater/pios_board.c +++ b/flight/targets/common/bootloader_updater/pios_board.c @@ -35,19 +35,19 @@ */ #include -void PIOS_Board_Init(void) { - const struct pios_board_info * bdinfo = &pios_board_info_blob; +void PIOS_Board_Init(void) +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; - /* LEDs */ + /* LEDs */ #if defined(PIOS_INCLUDE_LED) - const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); - PIOS_Assert(led_cfg); - PIOS_LED_Init(led_cfg); -#endif /* PIOS_INCLUDE_LED */ + const struct pios_led_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ - /* Initialize the PiOS library */ + /* Initialize the PiOS library */ #if defined(PIOS_INCLUDE_GPIO) - PIOS_GPIO_Init(); -#endif /* PIOS_INCLUDE_GPIO */ - + PIOS_GPIO_Init(); +#endif /* PIOS_INCLUDE_GPIO */ } diff --git a/flight/tests/logfs/FreeRTOS.h b/flight/tests/logfs/FreeRTOS.h index 7f174088c..0c3db03fc 100644 --- a/flight/tests/logfs/FreeRTOS.h +++ b/flight/tests/logfs/FreeRTOS.h @@ -1,3 +1,3 @@ #include #define pvPortMalloc(xSize) (malloc(xSize)) -#define vPortFree(pv) (free(pv)) +#define vPortFree(pv) (free(pv)) diff --git a/flight/tests/logfs/openpilot.h b/flight/tests/logfs/openpilot.h index 69f7874da..4c2259d19 100644 --- a/flight/tests/logfs/openpilot.h +++ b/flight/tests/logfs/openpilot.h @@ -3,7 +3,9 @@ #include -#define PIOS_Assert(x) if (!(x)) { while (1) ; } +#define PIOS_Assert(x) \ + if (!(x)) { while (1) {; } \ + } #define PIOS_DEBUG_Assert(x) PIOS_Assert(x) #endif /* OPENPILOT_H */ diff --git a/flight/tests/logfs/pios_config.h b/flight/tests/logfs/pios_config.h index f365e39f6..66b2c8245 100644 --- a/flight/tests/logfs/pios_config.h +++ b/flight/tests/logfs/pios_config.h @@ -3,7 +3,7 @@ /* Enable/Disable PiOS modules */ #define PIOS_INCLUDE_FLASH -//#define PIOS_FLASHFS_LOGFS_MAX_DEVS 5 +// #define PIOS_FLASHFS_LOGFS_MAX_DEVS 5 #define PIOS_INCLUDE_FREERTOS #endif /* PIOS_CONFIG_H */ diff --git a/flight/tests/logfs/pios_flash_ut.c b/flight/tests/logfs/pios_flash_ut.c index b0d927fca..1c42a31fd 100644 --- a/flight/tests/logfs/pios_flash_ut.c +++ b/flight/tests/logfs/pios_flash_ut.c @@ -1,74 +1,75 @@ -#include /* abort */ -#include /* fopen/fread/fwrite/fseek */ -#include /* assert */ -#include /* memset */ +#include /* abort */ +#include /* fopen/fread/fwrite/fseek */ +#include /* assert */ +#include /* memset */ #include #include #include "pios_flash_ut_priv.h" enum flash_ut_magic { - FLASH_UT_MAGIC = 0x321dabc1, + FLASH_UT_MAGIC = 0x321dabc1, }; struct flash_ut_dev { - enum flash_ut_magic magic; - const struct pios_flash_ut_cfg * cfg; - bool transaction_in_progress; - FILE * flash_file; + enum flash_ut_magic magic; + const struct pios_flash_ut_cfg *cfg; + bool transaction_in_progress; + FILE *flash_file; }; -static struct flash_ut_dev * PIOS_Flash_UT_Alloc(void) +static struct flash_ut_dev *PIOS_Flash_UT_Alloc(void) { - struct flash_ut_dev * flash_dev = malloc(sizeof(struct flash_ut_dev)); + struct flash_ut_dev *flash_dev = malloc(sizeof(struct flash_ut_dev)); - flash_dev->magic = FLASH_UT_MAGIC; + flash_dev->magic = FLASH_UT_MAGIC; - return flash_dev; + return flash_dev; } -int32_t PIOS_Flash_UT_Init(uintptr_t * flash_id, const struct pios_flash_ut_cfg * cfg) +int32_t PIOS_Flash_UT_Init(uintptr_t *flash_id, const struct pios_flash_ut_cfg *cfg) { - /* Check inputs */ - assert(flash_id); - assert(cfg); - assert(cfg->size_of_flash); - assert(cfg->size_of_sector); - assert((cfg->size_of_flash % cfg->size_of_sector) == 0); - - struct flash_ut_dev * flash_dev = PIOS_Flash_UT_Alloc(); - assert(flash_dev); - - flash_dev->cfg = cfg; - flash_dev->transaction_in_progress = false; - - flash_dev->flash_file = fopen (FLASH_IMAGE_FILE, "rb+"); - if (flash_dev->flash_file == NULL) { - return -1; - } - - if (fseek (flash_dev->flash_file, flash_dev->cfg->size_of_flash, SEEK_SET) != 0) { - return -2; - } - - *flash_id = (uintptr_t)flash_dev; - - return 0; -} - -int32_t PIOS_Flash_UT_Destroy(uintptr_t flash_id) { /* Check inputs */ assert(flash_id); - struct flash_ut_dev * flash_dev = (void*)flash_id; + assert(cfg); + assert(cfg->size_of_flash); + assert(cfg->size_of_sector); + assert((cfg->size_of_flash % cfg->size_of_sector) == 0); + + struct flash_ut_dev *flash_dev = PIOS_Flash_UT_Alloc(); + assert(flash_dev); + + flash_dev->cfg = cfg; + flash_dev->transaction_in_progress = false; + + flash_dev->flash_file = fopen(FLASH_IMAGE_FILE, "rb+"); + if (flash_dev->flash_file == NULL) { + return -1; + } + + if (fseek(flash_dev->flash_file, flash_dev->cfg->size_of_flash, SEEK_SET) != 0) { + return -2; + } + + *flash_id = (uintptr_t)flash_dev; + + return 0; +} + +int32_t PIOS_Flash_UT_Destroy(uintptr_t flash_id) +{ + /* Check inputs */ + assert(flash_id); + struct flash_ut_dev *flash_dev = (void *)flash_id; if (flash_dev->flash_file == NULL) { - return -1; + return -1; } fclose(flash_dev->flash_file); free(flash_dev); - unlink (FLASH_IMAGE_FILE); + unlink(FLASH_IMAGE_FILE); return 0; } @@ -83,96 +84,95 @@ int32_t PIOS_Flash_UT_Destroy(uintptr_t flash_id) { static int32_t PIOS_Flash_UT_StartTransaction(uintptr_t flash_id) { - struct flash_ut_dev * flash_dev = (struct flash_ut_dev *)flash_id; + struct flash_ut_dev *flash_dev = (struct flash_ut_dev *)flash_id; - assert(!flash_dev->transaction_in_progress); + assert(!flash_dev->transaction_in_progress); - flash_dev->transaction_in_progress = true; + flash_dev->transaction_in_progress = true; - return 0; + return 0; } static int32_t PIOS_Flash_UT_EndTransaction(uintptr_t flash_id) { - struct flash_ut_dev * flash_dev = (struct flash_ut_dev *)flash_id; + struct flash_ut_dev *flash_dev = (struct flash_ut_dev *)flash_id; - assert(flash_dev->transaction_in_progress); + assert(flash_dev->transaction_in_progress); - flash_dev->transaction_in_progress = false; + flash_dev->transaction_in_progress = false; - return 0; + return 0; } static int32_t PIOS_Flash_UT_EraseSector(uintptr_t flash_id, uint32_t addr) { - struct flash_ut_dev * flash_dev = (struct flash_ut_dev *)flash_id; + struct flash_ut_dev *flash_dev = (struct flash_ut_dev *)flash_id; - assert(flash_dev->transaction_in_progress); + assert(flash_dev->transaction_in_progress); - if (fseek (flash_dev->flash_file, addr, SEEK_SET) != 0) { - assert(0); - } + if (fseek(flash_dev->flash_file, addr, SEEK_SET) != 0) { + assert(0); + } - unsigned char * buf = malloc(flash_dev->cfg->size_of_sector); - assert (buf); - memset((void *)buf, 0xFF, flash_dev->cfg->size_of_sector); + unsigned char *buf = malloc(flash_dev->cfg->size_of_sector); + assert(buf); + memset((void *)buf, 0xFF, flash_dev->cfg->size_of_sector); - size_t s; - s = fwrite (buf, 1, flash_dev->cfg->size_of_sector, flash_dev->flash_file); + size_t s; + s = fwrite(buf, 1, flash_dev->cfg->size_of_sector, flash_dev->flash_file); - assert (s == flash_dev->cfg->size_of_sector); + assert(s == flash_dev->cfg->size_of_sector); - return 0; + return 0; } -static int32_t PIOS_Flash_UT_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len) +static int32_t PIOS_Flash_UT_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) { - /* Check inputs */ - assert(data); + /* Check inputs */ + assert(data); - struct flash_ut_dev * flash_dev = (struct flash_ut_dev *)flash_id; + struct flash_ut_dev *flash_dev = (struct flash_ut_dev *)flash_id; - assert(flash_dev->transaction_in_progress); + assert(flash_dev->transaction_in_progress); - if (fseek (flash_dev->flash_file, addr, SEEK_SET) != 0) { - assert(0); - } + if (fseek(flash_dev->flash_file, addr, SEEK_SET) != 0) { + assert(0); + } - size_t s; - s = fwrite (data, 1, len, flash_dev->flash_file); + size_t s; + s = fwrite(data, 1, len, flash_dev->flash_file); - assert (s == len); + assert(s == len); - return 0; + return 0; } -static int32_t PIOS_Flash_UT_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len) +static int32_t PIOS_Flash_UT_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) { - /* Check inputs */ - assert(data); + /* Check inputs */ + assert(data); - struct flash_ut_dev * flash_dev = (struct flash_ut_dev *)flash_id; + struct flash_ut_dev *flash_dev = (struct flash_ut_dev *)flash_id; - assert(flash_dev->transaction_in_progress); + assert(flash_dev->transaction_in_progress); - if (fseek (flash_dev->flash_file, addr, SEEK_SET) != 0) { - assert(0); - } + if (fseek(flash_dev->flash_file, addr, SEEK_SET) != 0) { + assert(0); + } - size_t s; - s = fread (data, 1, len, flash_dev->flash_file); + size_t s; + s = fread(data, 1, len, flash_dev->flash_file); - assert (s == len); + assert(s == len); - return 0; + return 0; } /* Provide a flash driver to external drivers */ const struct pios_flash_driver pios_ut_flash_driver = { - .start_transaction = PIOS_Flash_UT_StartTransaction, - .end_transaction = PIOS_Flash_UT_EndTransaction, - .erase_sector = PIOS_Flash_UT_EraseSector, - .write_data = PIOS_Flash_UT_WriteData, - .read_data = PIOS_Flash_UT_ReadData, + .start_transaction = PIOS_Flash_UT_StartTransaction, + .end_transaction = PIOS_Flash_UT_EndTransaction, + .erase_sector = PIOS_Flash_UT_EraseSector, + .write_data = PIOS_Flash_UT_WriteData, + .read_data = PIOS_Flash_UT_ReadData, }; - diff --git a/flight/tests/logfs/pios_flash_ut_priv.h b/flight/tests/logfs/pios_flash_ut_priv.h index e6c87a800..cf2a83557 100644 --- a/flight/tests/logfs/pios_flash_ut_priv.h +++ b/flight/tests/logfs/pios_flash_ut_priv.h @@ -1,11 +1,11 @@ #include struct pios_flash_ut_cfg { - uint32_t size_of_flash; - uint32_t size_of_sector; + uint32_t size_of_flash; + uint32_t size_of_sector; }; -int32_t PIOS_Flash_UT_Init(uintptr_t * flash_id, const struct pios_flash_ut_cfg * cfg); +int32_t PIOS_Flash_UT_Init(uintptr_t *flash_id, const struct pios_flash_ut_cfg *cfg); int32_t PIOS_Flash_UT_Destroy(uintptr_t flash_id); extern const struct pios_flash_driver pios_ut_flash_driver; diff --git a/flight/tests/logfs/unittest.cpp b/flight/tests/logfs/unittest.cpp index 469a26768..ea4a56e33 100644 --- a/flight/tests/logfs/unittest.cpp +++ b/flight/tests/logfs/unittest.cpp @@ -1,12 +1,11 @@ #include "gtest/gtest.h" -#include /* printf */ -#include /* abort */ -#include /* memset */ +#include /* printf */ +#include /* abort */ +#include /* memset */ extern "C" { - -#include "pios_flash.h" /* PIOS_FLASH_* API */ +#include "pios_flash.h" /* PIOS_FLASH_* API */ #include "pios_flash_ut_priv.h" extern struct pios_flash_ut_cfg flash_config; @@ -16,370 +15,380 @@ extern struct pios_flash_ut_cfg flash_config; extern struct flashfs_logfs_cfg flashfs_config_partition_a; extern struct flashfs_logfs_cfg flashfs_config_partition_b; -#include "pios_flashfs.h" /* PIOS_FLASHFS_* */ - +#include "pios_flashfs.h" /* PIOS_FLASHFS_* */ } -#define OBJ0_ID 0xAA55AA55 +#define OBJ0_ID 0xAA55AA55 -#define OBJ1_ID 0x12345678 +#define OBJ1_ID 0x12345678 #define OBJ1_SIZE 76 -#define OBJ2_ID 0xABCDEFAB +#define OBJ2_ID 0xABCDEFAB #define OBJ2_SIZE 123 -#define OBJ3_ID 0x99999999 +#define OBJ3_ID 0x99999999 #define OBJ3_SIZE (256 - 12) // leave room for the slot header -#define OBJ4_ID 0x90901111 -#define OBJ4_SIZE (768) // only fits in partition b slots +#define OBJ4_ID 0x90901111 +#define OBJ4_SIZE (768) // only fits in partition b slots // To use a test fixture, derive a class from testing::Test. class LogfsTestRaw : public testing::Test { protected: - virtual void SetUp() { - /* create an empty, appropriately sized flash filesystem */ - FILE * theflash = fopen(FLASH_IMAGE_FILE, "wb"); - uint8_t sector[flash_config.size_of_sector]; - memset(sector, 0xFF, sizeof(sector)); - for (uint32_t i = 0; i < flash_config.size_of_flash / flash_config.size_of_sector; i++) { - fwrite(sector, sizeof(sector), 1, theflash); - } - fclose(theflash); + virtual void SetUp() + { + /* create an empty, appropriately sized flash filesystem */ + FILE *theflash = fopen(FLASH_IMAGE_FILE, "wb"); + uint8_t sector[flash_config.size_of_sector]; - /* Set up obj1 */ - for (uint32_t i = 0; i < sizeof(obj1); i++) { - obj1[i] = 0x10 + (i % 10); + memset(sector, 0xFF, sizeof(sector)); + for (uint32_t i = 0; i < flash_config.size_of_flash / flash_config.size_of_sector; i++) { + fwrite(sector, sizeof(sector), 1, theflash); + } + fclose(theflash); + + /* Set up obj1 */ + for (uint32_t i = 0; i < sizeof(obj1); i++) { + obj1[i] = 0x10 + (i % 10); + } + + /* Set up a second version of obj1 with different data */ + for (uint32_t i = 0; i < sizeof(obj1_alt); i++) { + obj1_alt[i] = 0xA0 + (i % 10); + } + + /* Set up obj2 */ + for (uint32_t i = 0; i < sizeof(obj2); i++) { + obj2[i] = 0x20 + (i % 10); + } + + /* Set up obj3 */ + for (uint32_t i = 0; i < sizeof(obj3); i++) { + obj3[i] = 0x30 + (i % 10); + } + + /* Set up obj4 */ + for (uint32_t i = 0; i < sizeof(obj4); i++) { + obj4[i] = 0x40 + (i % 10); + } } - /* Set up a second version of obj1 with different data */ - for (uint32_t i = 0; i < sizeof(obj1_alt); i++) { - obj1_alt[i] = 0xA0 + (i % 10); + virtual void TearDown() + { + unlink("theflash.bin"); } - /* Set up obj2 */ - for (uint32_t i = 0; i < sizeof(obj2); i++) { - obj2[i] = 0x20 + (i % 10); - } - - /* Set up obj3 */ - for (uint32_t i = 0; i < sizeof(obj3); i++) { - obj3[i] = 0x30 + (i % 10); - } - - /* Set up obj4 */ - for (uint32_t i = 0; i < sizeof(obj4); i++) { - obj4[i] = 0x40 + (i % 10); - } - } - - virtual void TearDown() { - unlink("theflash.bin"); - } - - unsigned char obj1[OBJ1_SIZE]; - unsigned char obj1_alt[OBJ1_SIZE]; - unsigned char obj2[OBJ2_SIZE]; - unsigned char obj3[OBJ3_SIZE]; - unsigned char obj4[OBJ4_SIZE]; + unsigned char obj1[OBJ1_SIZE]; + unsigned char obj1_alt[OBJ1_SIZE]; + unsigned char obj2[OBJ2_SIZE]; + unsigned char obj3[OBJ3_SIZE]; + unsigned char obj4[OBJ4_SIZE]; }; TEST_F(LogfsTestRaw, FlashInit) { - uintptr_t flash_id; - EXPECT_EQ(0, PIOS_Flash_UT_Init(&flash_id, &flash_config)); - PIOS_Flash_UT_Destroy(flash_id); + uintptr_t flash_id; + + EXPECT_EQ(0, PIOS_Flash_UT_Init(&flash_id, &flash_config)); + PIOS_Flash_UT_Destroy(flash_id); } TEST_F(LogfsTestRaw, LogfsInit) { - uintptr_t flash_id; - EXPECT_EQ(0, PIOS_Flash_UT_Init(&flash_id, &flash_config)); + uintptr_t flash_id; - uintptr_t fs_id; - EXPECT_EQ(0, PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_config_partition_a, &pios_ut_flash_driver, flash_id)); - PIOS_FLASHFS_Logfs_Destroy(fs_id); - PIOS_Flash_UT_Destroy(flash_id); + EXPECT_EQ(0, PIOS_Flash_UT_Init(&flash_id, &flash_config)); + + uintptr_t fs_id; + EXPECT_EQ(0, PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_config_partition_a, &pios_ut_flash_driver, flash_id)); + PIOS_FLASHFS_Logfs_Destroy(fs_id); + PIOS_Flash_UT_Destroy(flash_id); } class LogfsTestCooked : public LogfsTestRaw { protected: - virtual void SetUp() { - /* First, we need to set up the super fixture (LogfsTestRaw) */ - LogfsTestRaw::SetUp(); + virtual void SetUp() + { + /* First, we need to set up the super fixture (LogfsTestRaw) */ + LogfsTestRaw::SetUp(); - /* Init the flash and the flashfs so we don't need to repeat this in every test */ - EXPECT_EQ(0, PIOS_Flash_UT_Init(&flash_id, &flash_config)); - EXPECT_EQ(0, PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_config_partition_a, &pios_ut_flash_driver, flash_id)); - } + /* Init the flash and the flashfs so we don't need to repeat this in every test */ + EXPECT_EQ(0, PIOS_Flash_UT_Init(&flash_id, &flash_config)); + EXPECT_EQ(0, PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_config_partition_a, &pios_ut_flash_driver, flash_id)); + } - virtual void TearDown() { - PIOS_FLASHFS_Logfs_Destroy(fs_id); - PIOS_Flash_UT_Destroy(flash_id); - } + virtual void TearDown() + { + PIOS_FLASHFS_Logfs_Destroy(fs_id); + PIOS_Flash_UT_Destroy(flash_id); + } - uintptr_t flash_id; - uintptr_t fs_id; + uintptr_t flash_id; + uintptr_t fs_id; }; TEST_F(LogfsTestCooked, BadIdLogfsFormat) { - EXPECT_EQ(-1, PIOS_FLASHFS_Format(fs_id + 1)); + EXPECT_EQ(-1, PIOS_FLASHFS_Format(fs_id + 1)); } TEST_F(LogfsTestCooked, BadIdSave) { - EXPECT_EQ(-1, PIOS_FLASHFS_ObjSave(fs_id + 1, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(-1, PIOS_FLASHFS_ObjSave(fs_id + 1, OBJ1_ID, 0, obj1, sizeof(obj1))); } TEST_F(LogfsTestCooked, BadIdLoad) { - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(-1, PIOS_FLASHFS_ObjLoad(fs_id + 1, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + unsigned char obj1_check[OBJ1_SIZE]; + + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(-1, PIOS_FLASHFS_ObjLoad(fs_id + 1, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); } TEST_F(LogfsTestCooked, LogfsFormat) { - EXPECT_EQ(0, PIOS_FLASHFS_Format(fs_id)); + EXPECT_EQ(0, PIOS_FLASHFS_Format(fs_id)); } TEST_F(LogfsTestCooked, WriteOne) { - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); } TEST_F(LogfsTestCooked, WriteVerifyOne) { - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); } TEST_F(LogfsTestCooked, WriteVerifyDeleteVerifyOne) { - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjDelete(fs_id, OBJ1_ID, 0)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjDelete(fs_id, OBJ1_ID, 0)); - EXPECT_EQ(-3, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(-3, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); } TEST_F(LogfsTestCooked, WriteTwoVerifyOneA) { - /* Write obj1 then obj2 */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); + /* Write obj1 then obj2 */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); - /* Read back obj1 */ - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + /* Read back obj1 */ + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); } TEST_F(LogfsTestCooked, WriteTwoVerifyOneB) { - /* Write obj2 then obj1 */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); + /* Write obj2 then obj1 */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); - /* Read back obj1 */ - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + /* Read back obj1 */ + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); } TEST_F(LogfsTestCooked, WriteZeroSize) { - /* Write a zero length object */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ0_ID, 0, NULL, 0)); + /* Write a zero length object */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ0_ID, 0, NULL, 0)); } TEST_F(LogfsTestCooked, WriteVerifyZeroLength) { - /* Write a zero length object */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ0_ID, 0, NULL, 0)); + /* Write a zero length object */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ0_ID, 0, NULL, 0)); - /* Read back a zero length object -- effectively an existence check */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ0_ID, 0, NULL, 0)); + /* Read back a zero length object -- effectively an existence check */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ0_ID, 0, NULL, 0)); } TEST_F(LogfsTestCooked, WriteMaxSize) { - /* Write a max length object */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ3_ID, 0, obj3, sizeof(obj3))); + /* Write a max length object */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ3_ID, 0, obj3, sizeof(obj3))); } TEST_F(LogfsTestCooked, ReadNonexistent) { - /* Read back a zero length object -- basically an existence check */ - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(-3, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + /* Read back a zero length object -- basically an existence check */ + unsigned char obj1_check[OBJ1_SIZE]; + + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(-3, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); } TEST_F(LogfsTestCooked, WriteVerifyMultiInstance) { - /* Write instance zero */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); + /* Write instance zero */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); - /* Write a non-zero instance ID */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 123, obj1_alt, sizeof(obj1_alt))); + /* Write a non-zero instance ID */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 123, obj1_alt, sizeof(obj1_alt))); - unsigned char obj1_check[OBJ1_SIZE]; + unsigned char obj1_check[OBJ1_SIZE]; - /* Read back instance 123 */ - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 123, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1_alt, obj1_check, sizeof(obj1_alt))); + /* Read back instance 123 */ + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 123, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1_alt, obj1_check, sizeof(obj1_alt))); - /* Read back instance 0 */ - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + /* Read back instance 0 */ + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); } TEST_F(LogfsTestCooked, FillFilesystemAndGarbageCollect) { - /* Fill up the entire filesystem with multiple instances of obj1 */ - for (uint32_t i = 0; i < (flashfs_config_partition_a.arena_size / flashfs_config_partition_a.slot_size) - 1; i++) { - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, i, obj1, sizeof(obj1))); - } + /* Fill up the entire filesystem with multiple instances of obj1 */ + for (uint32_t i = 0; i < (flashfs_config_partition_a.arena_size / flashfs_config_partition_a.slot_size) - 1; i++) { + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, i, obj1, sizeof(obj1))); + } - /* Should fail to add a new object since the filesystem is full */ - EXPECT_EQ(-4, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); + /* Should fail to add a new object since the filesystem is full */ + EXPECT_EQ(-4, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); - /* Now save a new version of an existing object which should trigger gc and succeed */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1_alt, sizeof(obj1_alt))); + /* Now save a new version of an existing object which should trigger gc and succeed */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1_alt, sizeof(obj1_alt))); - /* Read back one of the original obj1 instances */ - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 1, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + /* Read back one of the original obj1 instances */ + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 1, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); - /* Read back the new version of obj1 written after gc */ - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1_alt, obj1_check, sizeof(obj1_alt))); + /* Read back the new version of obj1 written after gc */ + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1_alt, obj1_check, sizeof(obj1_alt))); } TEST_F(LogfsTestCooked, WriteManyVerify) { - for (uint32_t i = 0; i < 10000; i++) { - /* Write a collection of objects */ - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ0_ID, 0, NULL, 0)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 123, obj1_alt, sizeof(obj1_alt))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ3_ID, 0, obj3, sizeof(obj3))); - } + for (uint32_t i = 0; i < 10000; i++) { + /* Write a collection of objects */ + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ0_ID, 0, NULL, 0)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ1_ID, 123, obj1_alt, sizeof(obj1_alt))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ2_ID, 0, obj2, sizeof(obj2))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id, OBJ3_ID, 0, obj3, sizeof(obj3))); + } - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ0_ID, 0, NULL, 0)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ0_ID, 0, NULL, 0)); - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); - unsigned char obj1_alt_check[OBJ1_SIZE]; - memset(obj1_alt_check, 0, sizeof(obj1_alt_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 123, obj1_alt_check, sizeof(obj1_alt_check))); - EXPECT_EQ(0, memcmp(obj1_alt, obj1_alt_check, sizeof(obj1_alt))); + unsigned char obj1_alt_check[OBJ1_SIZE]; + memset(obj1_alt_check, 0, sizeof(obj1_alt_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ1_ID, 123, obj1_alt_check, sizeof(obj1_alt_check))); + EXPECT_EQ(0, memcmp(obj1_alt, obj1_alt_check, sizeof(obj1_alt))); - unsigned char obj2_check[OBJ2_SIZE]; - memset(obj2_check, 0, sizeof(obj2_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ2_ID, 0, obj2_check, sizeof(obj2_check))); - EXPECT_EQ(0, memcmp(obj2, obj2_check, sizeof(obj2))); + unsigned char obj2_check[OBJ2_SIZE]; + memset(obj2_check, 0, sizeof(obj2_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ2_ID, 0, obj2_check, sizeof(obj2_check))); + EXPECT_EQ(0, memcmp(obj2, obj2_check, sizeof(obj2))); - unsigned char obj3_check[OBJ3_SIZE]; - memset(obj3_check, 0, sizeof(obj3_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ3_ID, 0, obj3_check, sizeof(obj3_check))); - EXPECT_EQ(0, memcmp(obj3, obj3_check, sizeof(obj3))); + unsigned char obj3_check[OBJ3_SIZE]; + memset(obj3_check, 0, sizeof(obj3_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id, OBJ3_ID, 0, obj3_check, sizeof(obj3_check))); + EXPECT_EQ(0, memcmp(obj3, obj3_check, sizeof(obj3))); } class LogfsTestCookedMultiPart : public LogfsTestRaw { protected: - virtual void SetUp() { - /* First, we need to set up the super fixture (LogfsTestRaw) */ - LogfsTestRaw::SetUp(); + virtual void SetUp() + { + /* First, we need to set up the super fixture (LogfsTestRaw) */ + LogfsTestRaw::SetUp(); - /* Init the flash and the flashfs so we don't need to repeat this in every test */ - EXPECT_EQ(0, PIOS_Flash_UT_Init(&flash_id, &flash_config)); - EXPECT_EQ(0, PIOS_FLASHFS_Logfs_Init(&fs_id_a, &flashfs_config_partition_a, &pios_ut_flash_driver, flash_id)); - EXPECT_EQ(0, PIOS_FLASHFS_Logfs_Init(&fs_id_b, &flashfs_config_partition_b, &pios_ut_flash_driver, flash_id)); - } + /* Init the flash and the flashfs so we don't need to repeat this in every test */ + EXPECT_EQ(0, PIOS_Flash_UT_Init(&flash_id, &flash_config)); + EXPECT_EQ(0, PIOS_FLASHFS_Logfs_Init(&fs_id_a, &flashfs_config_partition_a, &pios_ut_flash_driver, flash_id)); + EXPECT_EQ(0, PIOS_FLASHFS_Logfs_Init(&fs_id_b, &flashfs_config_partition_b, &pios_ut_flash_driver, flash_id)); + } - virtual void TearDown() { - PIOS_FLASHFS_Logfs_Destroy(fs_id_b); - PIOS_FLASHFS_Logfs_Destroy(fs_id_a); - PIOS_Flash_UT_Destroy(flash_id); - } + virtual void TearDown() + { + PIOS_FLASHFS_Logfs_Destroy(fs_id_b); + PIOS_FLASHFS_Logfs_Destroy(fs_id_a); + PIOS_Flash_UT_Destroy(flash_id); + } - uintptr_t flash_id; - uintptr_t fs_id_a; - uintptr_t fs_id_b; + uintptr_t flash_id; + uintptr_t fs_id_a; + uintptr_t fs_id_b; }; TEST_F(LogfsTestCookedMultiPart, WriteOneWriteOneVerify) { - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_a, OBJ1_ID, 0, obj1, sizeof(obj1))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_b, OBJ2_ID, 0, obj2, sizeof(obj2))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_a, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_b, OBJ2_ID, 0, obj2, sizeof(obj2))); - /* OBJ1 found in A */ - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id_a, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); + /* OBJ1 found in A */ + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id_a, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + EXPECT_EQ(0, memcmp(obj1, obj1_check, sizeof(obj1))); - /* OBJ2 found in B */ - unsigned char obj2_check[OBJ2_SIZE]; - memset(obj2_check, 0, sizeof(obj2_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id_b, OBJ2_ID, 0, obj2_check, sizeof(obj2_check))); - EXPECT_EQ(0, memcmp(obj2, obj2_check, sizeof(obj2))); + /* OBJ2 found in B */ + unsigned char obj2_check[OBJ2_SIZE]; + memset(obj2_check, 0, sizeof(obj2_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id_b, OBJ2_ID, 0, obj2_check, sizeof(obj2_check))); + EXPECT_EQ(0, memcmp(obj2, obj2_check, sizeof(obj2))); } TEST_F(LogfsTestCookedMultiPart, WriteOneWriteOneFormatOneVerify) { - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_a, OBJ1_ID, 0, obj1, sizeof(obj1))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_b, OBJ2_ID, 0, obj2, sizeof(obj2))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_a, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_b, OBJ2_ID, 0, obj2, sizeof(obj2))); - EXPECT_EQ(0, PIOS_FLASHFS_Format(fs_id_a)); + EXPECT_EQ(0, PIOS_FLASHFS_Format(fs_id_a)); - /* OBJ2 still found in B */ - unsigned char obj2_check[OBJ2_SIZE]; - memset(obj2_check, 0, sizeof(obj2_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id_b, OBJ2_ID, 0, obj2_check, sizeof(obj2_check))); - EXPECT_EQ(0, memcmp(obj2, obj2_check, sizeof(obj2))); + /* OBJ2 still found in B */ + unsigned char obj2_check[OBJ2_SIZE]; + memset(obj2_check, 0, sizeof(obj2_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id_b, OBJ2_ID, 0, obj2_check, sizeof(obj2_check))); + EXPECT_EQ(0, memcmp(obj2, obj2_check, sizeof(obj2))); } TEST_F(LogfsTestCookedMultiPart, WriteOneWriteOneNoCross) { - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_a, OBJ1_ID, 0, obj1, sizeof(obj1))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_b, OBJ2_ID, 0, obj2, sizeof(obj2))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_a, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_b, OBJ2_ID, 0, obj2, sizeof(obj2))); - /* OBJ1 not found in B */ - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(-3, PIOS_FLASHFS_ObjLoad(fs_id_b, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + /* OBJ1 not found in B */ + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(-3, PIOS_FLASHFS_ObjLoad(fs_id_b, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - /* OBJ2 not found in A */ - unsigned char obj2_check[OBJ2_SIZE]; - memset(obj2_check, 0, sizeof(obj2_check)); - EXPECT_EQ(-3, PIOS_FLASHFS_ObjLoad(fs_id_a, OBJ2_ID, 0, obj2_check, sizeof(obj2_check))); + /* OBJ2 not found in A */ + unsigned char obj2_check[OBJ2_SIZE]; + memset(obj2_check, 0, sizeof(obj2_check)); + EXPECT_EQ(-3, PIOS_FLASHFS_ObjLoad(fs_id_a, OBJ2_ID, 0, obj2_check, sizeof(obj2_check))); } TEST_F(LogfsTestCookedMultiPart, WriteOneWriteOneDeleteOne) { - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_a, OBJ1_ID, 0, obj1, sizeof(obj1))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_b, OBJ2_ID, 0, obj2, sizeof(obj2))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_a, OBJ1_ID, 0, obj1, sizeof(obj1))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_b, OBJ2_ID, 0, obj2, sizeof(obj2))); - EXPECT_EQ(0, PIOS_FLASHFS_ObjDelete(fs_id_a, OBJ1_ID, 0)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjDelete(fs_id_a, OBJ1_ID, 0)); - /* OBJ1 not found in A */ - unsigned char obj1_check[OBJ1_SIZE]; - memset(obj1_check, 0, sizeof(obj1_check)); - EXPECT_EQ(-3, PIOS_FLASHFS_ObjLoad(fs_id_a, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); + /* OBJ1 not found in A */ + unsigned char obj1_check[OBJ1_SIZE]; + memset(obj1_check, 0, sizeof(obj1_check)); + EXPECT_EQ(-3, PIOS_FLASHFS_ObjLoad(fs_id_a, OBJ1_ID, 0, obj1_check, sizeof(obj1_check))); - /* OBJ2 still found in B */ - unsigned char obj2_check[OBJ2_SIZE]; - memset(obj2_check, 0, sizeof(obj2_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id_b, OBJ2_ID, 0, obj2_check, sizeof(obj2_check))); + /* OBJ2 still found in B */ + unsigned char obj2_check[OBJ2_SIZE]; + memset(obj2_check, 0, sizeof(obj2_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id_b, OBJ2_ID, 0, obj2_check, sizeof(obj2_check))); } TEST_F(LogfsTestCookedMultiPart, WriteLarge) { - EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_b, OBJ4_ID, 0, obj4, sizeof(obj4))); + EXPECT_EQ(0, PIOS_FLASHFS_ObjSave(fs_id_b, OBJ4_ID, 0, obj4, sizeof(obj4))); - /* OBJ4 found in B */ - unsigned char obj4_check[OBJ4_SIZE]; - memset(obj4_check, 0, sizeof(obj4_check)); - EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id_b, OBJ4_ID, 0, obj4_check, sizeof(obj4_check))); + /* OBJ4 found in B */ + unsigned char obj4_check[OBJ4_SIZE]; + memset(obj4_check, 0, sizeof(obj4_check)); + EXPECT_EQ(0, PIOS_FLASHFS_ObjLoad(fs_id_b, OBJ4_ID, 0, obj4_check, sizeof(obj4_check))); } diff --git a/flight/tests/logfs/unittest_init.c b/flight/tests/logfs/unittest_init.c index 9f6305839..bb351034d 100644 --- a/flight/tests/logfs/unittest_init.c +++ b/flight/tests/logfs/unittest_init.c @@ -1,4 +1,4 @@ -/* +/* * These need to be defined in a .c file so that we can use * designated initializer syntax which c++ doesn't support (yet). */ @@ -7,31 +7,30 @@ const struct pios_flash_ut_cfg flash_config = { - .size_of_flash = 0x00300000, - .size_of_sector = 0x00010000, + .size_of_flash = 0x00300000, + .size_of_sector = 0x00010000, }; #include "pios_flashfs_logfs_priv.h" const struct flashfs_logfs_cfg flashfs_config_partition_a = { - .fs_magic = 0x89abceef, - .total_fs_size = 0x00200000, /* 2M bytes (32 sectors) */ - .arena_size = 0x00010000, /* 256 * slot size */ - .slot_size = 0x00000100, /* 256 bytes */ + .fs_magic = 0x89abceef, + .total_fs_size = 0x00200000, /* 2M bytes (32 sectors) */ + .arena_size = 0x00010000, /* 256 * slot size */ + .slot_size = 0x00000100, /* 256 bytes */ - .start_offset = 0, /* start at the beginning of the chip */ - .sector_size = 0x00010000, /* 64K bytes */ - .page_size = 0x00000100, /* 256 bytes */ + .start_offset = 0, /* start at the beginning of the chip */ + .sector_size = 0x00010000, /* 64K bytes */ + .page_size = 0x00000100, /* 256 bytes */ }; const struct flashfs_logfs_cfg flashfs_config_partition_b = { - .fs_magic = 0x89abceef, - .total_fs_size = 0x00100000, /* 1M bytes (16 sectors) */ - .arena_size = 0x00010000, /* 64 * slot size */ - .slot_size = 0x00000400, /* 256 bytes */ + .fs_magic = 0x89abceef, + .total_fs_size = 0x00100000, /* 1M bytes (16 sectors) */ + .arena_size = 0x00010000, /* 64 * slot size */ + .slot_size = 0x00000400, /* 256 bytes */ - .start_offset = 0x00200000, /* start after partition a */ - .sector_size = 0x00010000, /* 64K bytes */ - .page_size = 0x00000100, /* 256 bytes */ + .start_offset = 0x00200000, /* start after partition a */ + .sector_size = 0x00010000, /* 64K bytes */ + .page_size = 0x00000100, /* 256 bytes */ }; - diff --git a/flight/uavobjects/callbackscheduler.c b/flight/uavobjects/callbackscheduler.c index 7a38cb551..f0f666a15 100644 --- a/flight/uavobjects/callbackscheduler.c +++ b/flight/uavobjects/callbackscheduler.c @@ -30,36 +30,35 @@ // Private constants #define STACK_SIZE 128 -#define MAX_SLEEP 1000 +#define MAX_SLEEP 1000 // Private types /** * task information */ struct DelayedCallbackTaskStruct { - DelayedCallbackInfo *callbackQueue[CALLBACK_PRIORITY_LOW + 1]; - DelayedCallbackInfo *queueCursor[CALLBACK_PRIORITY_LOW + 1]; - xTaskHandle callbackSchedulerTaskHandle; - signed char name[3]; - uint32_t stackSize; - DelayedCallbackPriorityTask priorityTask; - xSemaphoreHandle signal; - struct DelayedCallbackTaskStruct *next; + DelayedCallbackInfo *callbackQueue[CALLBACK_PRIORITY_LOW + 1]; + DelayedCallbackInfo *queueCursor[CALLBACK_PRIORITY_LOW + 1]; + xTaskHandle callbackSchedulerTaskHandle; + signed char name[3]; + uint32_t stackSize; + DelayedCallbackPriorityTask priorityTask; + xSemaphoreHandle signal; + struct DelayedCallbackTaskStruct *next; }; /** * callback information */ struct DelayedCallbackInfoStruct { - DelayedCallback cb; - bool volatile waiting; - uint32_t volatile scheduletime; - struct DelayedCallbackTaskStruct *task; - struct DelayedCallbackInfoStruct *next; + DelayedCallback cb; + bool volatile waiting; + uint32_t volatile scheduletime; + struct DelayedCallbackTaskStruct *task; + struct DelayedCallbackInfoStruct *next; }; - // Private variables static struct DelayedCallbackTaskStruct *schedulerTasks; static xSemaphoreHandle mutex; @@ -76,18 +75,18 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa */ int32_t CallbackSchedulerInitialize() { - // Initialize variables - schedulerTasks = NULL; - schedulerStarted = false; + // Initialize variables + schedulerTasks = NULL; + schedulerStarted = false; - // Create mutex - mutex = xSemaphoreCreateRecursiveMutex(); - if (mutex == NULL) { - return -1; - } + // Create mutex + mutex = xSemaphoreCreateRecursiveMutex(); + if (mutex == NULL) { + return -1; + } - // Done - return 0; + // Done + return 0; } /** @@ -102,34 +101,34 @@ int32_t CallbackSchedulerInitialize() */ int32_t CallbackSchedulerStart() { - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // only call once - PIOS_Assert(schedulerStarted == false); + // only call once + PIOS_Assert(schedulerStarted == false); - // start tasks - struct DelayedCallbackTaskStruct *cursor = NULL; - int t = 0; - LL_FOREACH(schedulerTasks, cursor) { - xTaskCreate( - CallbackSchedulerTask, - cursor->name, - cursor->stackSize/4, - cursor, - cursor->priorityTask, - &cursor->callbackSchedulerTaskHandle - ); - if (TASKINFO_RUNNING_CALLBACKSCHEDULER0+t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, cursor->callbackSchedulerTaskHandle); - } - t++; - } + // start tasks + struct DelayedCallbackTaskStruct *cursor = NULL; + int t = 0; + LL_FOREACH(schedulerTasks, cursor) { + xTaskCreate( + CallbackSchedulerTask, + cursor->name, + cursor->stackSize / 4, + cursor, + cursor->priorityTask, + &cursor->callbackSchedulerTaskHandle + ); + if (TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, cursor->callbackSchedulerTaskHandle); + } + t++; + } - schedulerStarted = true; + schedulerStarted = true; - xSemaphoreGiveRecursive(mutex); + xSemaphoreGiveRecursive(mutex); - return 0; + return 0; } /** @@ -145,47 +144,45 @@ int32_t CallbackSchedulerStart() * \return 0: not scheduled, previous schedule takes precedence, 1: new schedule, 2: previous schedule overridden */ int32_t DelayedCallbackSchedule( - DelayedCallbackInfo *cbinfo, - int32_t milliseconds, - DelayedCallbackUpdateMode updatemode - ) + DelayedCallbackInfo *cbinfo, + int32_t milliseconds, + DelayedCallbackUpdateMode updatemode) { - int32_t result = 0; + int32_t result = 0; - PIOS_Assert(cbinfo); + PIOS_Assert(cbinfo); - if (milliseconds <= 0) { - milliseconds = 0; // we can and will not schedule in the past since that ruins the wraparound of uint32_t - } + if (milliseconds <= 0) { + milliseconds = 0; // we can and will not schedule in the past since that ruins the wraparound of uint32_t + } - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - uint32_t new = xTaskGetTickCount() + (milliseconds/portTICK_RATE_MS); - if (!new) { - new = 1; // zero has a special meaning, schedule at time 1 instead - } + uint32_t new = xTaskGetTickCount() + (milliseconds / portTICK_RATE_MS); + if (!new) { + new = 1; // zero has a special meaning, schedule at time 1 instead + } - int32_t diff = new - cbinfo->scheduletime; - if ( (!cbinfo->scheduletime) - || ((updatemode & CALLBACK_UPDATEMODE_SOONER) && diff < 0) - || ((updatemode & CALLBACK_UPDATEMODE_LATER ) && diff > 0) - ) { + int32_t diff = new - cbinfo->scheduletime; + if ((!cbinfo->scheduletime) + || ((updatemode & CALLBACK_UPDATEMODE_SOONER) && diff < 0) + || ((updatemode & CALLBACK_UPDATEMODE_LATER) && diff > 0) + ) { + // the scheduletime may be updated + if (!cbinfo->scheduletime) { + result = 1; + } else { + result = 2; + } + cbinfo->scheduletime = new; - // the scheduletime may be updated - if (!cbinfo->scheduletime) { - result = 1; - } else { - result = 2; - } - cbinfo->scheduletime = new; + // scheduler needs to be notified to adapt sleep times + xSemaphoreGive(cbinfo->task->signal); + } - // scheduler needs to be notified to adapt sleep times - xSemaphoreGive(cbinfo->task->signal); - } + xSemaphoreGiveRecursive(mutex); - xSemaphoreGiveRecursive(mutex); - - return result; + return result; } /** @@ -196,12 +193,12 @@ int32_t DelayedCallbackSchedule( */ int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo) { - PIOS_Assert(cbinfo); + PIOS_Assert(cbinfo); - // no semaphore needed for the callback - cbinfo->waiting = true; - // but the scheduler as a whole needs to be notified - return xSemaphoreGive(cbinfo->task->signal); + // no semaphore needed for the callback + cbinfo->waiting = true; + // but the scheduler as a whole needs to be notified + return xSemaphoreGive(cbinfo->task->signal); } /** @@ -220,12 +217,12 @@ int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo) */ int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken) { - PIOS_Assert(cbinfo); + PIOS_Assert(cbinfo); - // no semaphore needed for the callback - cbinfo->waiting = true; - // but the scheduler as a whole needs to be notified - return xSemaphoreGiveFromISR(cbinfo->task->signal, pxHigherPriorityTaskWoken); + // no semaphore needed for the callback + cbinfo->waiting = true; + // but the scheduler as a whole needs to be notified + return xSemaphoreGiveFromISR(cbinfo->task->signal, pxHigherPriorityTaskWoken); } /** @@ -241,100 +238,98 @@ int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigh * \return CallbackInfo Pointer on success, NULL if failed. */ DelayedCallbackInfo *DelayedCallbackCreate( - DelayedCallback cb, - DelayedCallbackPriority priority, - DelayedCallbackPriorityTask priorityTask, - uint32_t stacksize - ) + DelayedCallback cb, + DelayedCallbackPriority priority, + DelayedCallbackPriorityTask priorityTask, + uint32_t stacksize) { + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // find appropriate scheduler task matching priorityTask + struct DelayedCallbackTaskStruct *task = NULL; + int t = 0; + LL_FOREACH(schedulerTasks, task) { + if (task->priorityTask == priorityTask) { + break; // found + } + t++; + } + // if given priorityTask does not exist, create it + if (!task) { + // allocate memory if possible + task = (struct DelayedCallbackTaskStruct *)pvPortMalloc(sizeof(struct DelayedCallbackTaskStruct)); + if (!task) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } - // find appropriate scheduler task matching priorityTask - struct DelayedCallbackTaskStruct *task = NULL; - int t = 0; - LL_FOREACH(schedulerTasks, task) { - if (task->priorityTask == priorityTask) { - break; // found - } - t++; - } - // if given priorityTask does not exist, create it - if (!task) { - // allocate memory if possible - task = (struct DelayedCallbackTaskStruct*)pvPortMalloc(sizeof(struct DelayedCallbackTaskStruct)); - if (!task) { - xSemaphoreGiveRecursive(mutex); - return NULL; - } + // initialize structure + for (DelayedCallbackPriority p = 0; p <= CALLBACK_PRIORITY_LOW; p++) { + task->callbackQueue[p] = NULL; + task->queueCursor[p] = NULL; + } + task->name[0] = 'C'; + task->name[1] = 'a' + t; + task->name[2] = 0; + task->stackSize = ((STACK_SIZE > stacksize) ? STACK_SIZE : stacksize); + task->priorityTask = priorityTask; + task->next = NULL; - // initialize structure - for (DelayedCallbackPriority p = 0; p <= CALLBACK_PRIORITY_LOW; p++) { - task->callbackQueue[p] = NULL; - task->queueCursor[p] = NULL; - } - task->name[0] = 'C'; - task->name[1] = 'a'+t; - task->name[2] = 0; - task->stackSize = ((STACK_SIZE>stacksize)?STACK_SIZE:stacksize); - task->priorityTask = priorityTask; - task->next = NULL; + // create the signaling semaphore + vSemaphoreCreateBinary(task->signal); + if (!task->signal) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } - // create the signaling semaphore - vSemaphoreCreateBinary( task->signal ); - if (!task->signal) { - xSemaphoreGiveRecursive(mutex); - return NULL; - } + // add to list of scheduler tasks + LL_APPEND(schedulerTasks, task); - // add to list of scheduler tasks - LL_APPEND(schedulerTasks, task); + // Previously registered tasks are spawned when CallbackSchedulerStart() is called. + // Tasks registered afterwards need to spawn upon creation. + if (schedulerStarted) { + xTaskCreate( + CallbackSchedulerTask, + task->name, + task->stackSize / 4, + task, + task->priorityTask, + &task->callbackSchedulerTaskHandle + ); + if (TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, task->callbackSchedulerTaskHandle); + } + } + } - // Previously registered tasks are spawned when CallbackSchedulerStart() is called. - // Tasks registered afterwards need to spawn upon creation. - if (schedulerStarted) { - xTaskCreate( - CallbackSchedulerTask, - task->name, - task->stackSize/4, - task, - task->priorityTask, - &task->callbackSchedulerTaskHandle - ); - if (TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, task->callbackSchedulerTaskHandle); - } - } - } - - if (!schedulerStarted && stacksize > task->stackSize) { - task->stackSize = stacksize; // previous to task initialisation we can still adapt to the maximum needed stack - } + if (!schedulerStarted && stacksize > task->stackSize) { + task->stackSize = stacksize; // previous to task initialisation we can still adapt to the maximum needed stack + } - if (stacksize > task->stackSize) { - xSemaphoreGiveRecursive(mutex); - return NULL; // error - not enough memory - } + if (stacksize > task->stackSize) { + xSemaphoreGiveRecursive(mutex); + return NULL; // error - not enough memory + } - // initialize callback scheduling info - DelayedCallbackInfo *info = (DelayedCallbackInfo*)pvPortMalloc(sizeof(DelayedCallbackInfo)); - if (!info) { - xSemaphoreGiveRecursive(mutex); - return NULL; // error - not enough memory - } - info->next = NULL; - info->waiting = false; - info->scheduletime = 0; - info->task = task; - info->cb = cb; + // initialize callback scheduling info + DelayedCallbackInfo *info = (DelayedCallbackInfo *)pvPortMalloc(sizeof(DelayedCallbackInfo)); + if (!info) { + xSemaphoreGiveRecursive(mutex); + return NULL; // error - not enough memory + } + info->next = NULL; + info->waiting = false; + info->scheduletime = 0; + info->task = task; + info->cb = cb; - // add to scheduling queue - LL_APPEND(task->callbackQueue[priority], info); + // add to scheduling queue + LL_APPEND(task->callbackQueue[priority], info); - xSemaphoreGiveRecursive(mutex); + xSemaphoreGiveRecursive(mutex); - return info; + return info; } /** @@ -345,60 +340,59 @@ DelayedCallbackInfo *DelayedCallbackCreate( */ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallbackPriority priority) { + int32_t result = MAX_SLEEP; + int32_t diff = 0; - int32_t result = MAX_SLEEP; - int32_t diff = 0; + // no such queue + if (priority > CALLBACK_PRIORITY_LOW) { + return result; + } - // no such queue - if (priority > CALLBACK_PRIORITY_LOW) { - return result; - } + // queue is empty, search a lower priority queue + if (task->callbackQueue[priority] == NULL) { + return runNextCallback(task, priority + 1); + } - // queue is empty, search a lower priority queue - if (task->callbackQueue[priority] == NULL) { - return runNextCallback(task, priority + 1); - } - - DelayedCallbackInfo *current = task->queueCursor[priority]; - DelayedCallbackInfo *next; - do { - if (current == NULL) { - next = task->callbackQueue[priority]; // loop around the end of the list - // also attempt to run a callback that has lower priority - // every time the queue is completely traversed - diff = runNextCallback(task, priority + 1); - if (!diff) { - task->queueCursor[priority] = next; // the recursive call has executed a callback - return 0; - } - if (diff < result) { - result = diff; // adjust sleep time - } - } else { - next = current->next; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // access to scheduletime should be mutex protected - if (current->scheduletime) { - diff = current->scheduletime - xTaskGetTickCount(); - if (diff <= 0) { - current->waiting = true; - } else if (diff < result) { - result = diff; // adjust sleep time - } - } - if (current->waiting) { - task->queueCursor[priority] = next; - current->scheduletime = 0; // any schedules are reset - current->waiting = false; // the flag is reset just before execution. - xSemaphoreGiveRecursive(mutex); - current->cb(); // call the callback - return 0; - } - xSemaphoreGiveRecursive(mutex); - } - current = next; - } while (current != task->queueCursor[priority]); - // once the list has been traversed entirely without finding any to be executed task, abort (nothing to do) - return result; + DelayedCallbackInfo *current = task->queueCursor[priority]; + DelayedCallbackInfo *next; + do { + if (current == NULL) { + next = task->callbackQueue[priority]; // loop around the end of the list + // also attempt to run a callback that has lower priority + // every time the queue is completely traversed + diff = runNextCallback(task, priority + 1); + if (!diff) { + task->queueCursor[priority] = next; // the recursive call has executed a callback + return 0; + } + if (diff < result) { + result = diff; // adjust sleep time + } + } else { + next = current->next; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // access to scheduletime should be mutex protected + if (current->scheduletime) { + diff = current->scheduletime - xTaskGetTickCount(); + if (diff <= 0) { + current->waiting = true; + } else if (diff < result) { + result = diff; // adjust sleep time + } + } + if (current->waiting) { + task->queueCursor[priority] = next; + current->scheduletime = 0; // any schedules are reset + current->waiting = false; // the flag is reset just before execution. + xSemaphoreGiveRecursive(mutex); + current->cb(); // call the callback + return 0; + } + xSemaphoreGiveRecursive(mutex); + } + current = next; + } while (current != task->queueCursor[priority]); + // once the list has been traversed entirely without finding any to be executed task, abort (nothing to do) + return result; } /** @@ -407,15 +401,13 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa */ static void CallbackSchedulerTask(void *task) { - uint32_t delay = 0; + uint32_t delay = 0; - while (1) { - - delay = runNextCallback((struct DelayedCallbackTaskStruct*)task, CALLBACK_PRIORITY_CRITICAL); - if (delay) { - // nothing to do but sleep - xSemaphoreTake(((struct DelayedCallbackTaskStruct*)task)->signal, delay); - } - } + while (1) { + delay = runNextCallback((struct DelayedCallbackTaskStruct *)task, CALLBACK_PRIORITY_CRITICAL); + if (delay) { + // nothing to do but sleep + xSemaphoreTake(((struct DelayedCallbackTaskStruct *)task)->signal, delay); + } + } } - diff --git a/flight/uavobjects/eventdispatcher.c b/flight/uavobjects/eventdispatcher.c index 962298980..503e9b1e5 100644 --- a/flight/uavobjects/eventdispatcher.c +++ b/flight/uavobjects/eventdispatcher.c @@ -4,7 +4,7 @@ * @file eventdispatcher.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Event dispatcher, distributes object events as callbacks. Alternative - * to using tasks and queues. All callbacks are invoked from the event task. + * to using tasks and queues. All callbacks are invoked from the event task. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ @@ -30,18 +30,18 @@ // Private constants #if defined(PIOS_EVENTDISAPTCHER_QUEUE) -#define MAX_QUEUE_SIZE PIOS_EVENTDISAPTCHER_QUEUE +#define MAX_QUEUE_SIZE PIOS_EVENTDISAPTCHER_QUEUE #else -#define MAX_QUEUE_SIZE 20 +#define MAX_QUEUE_SIZE 20 #endif #if defined(PIOS_EVENTDISPATCHER_STACK_SIZE) -#define STACK_SIZE PIOS_EVENTDISPATCHER_STACK_SIZE +#define STACK_SIZE PIOS_EVENTDISPATCHER_STACK_SIZE #else -#define STACK_SIZE configMINIMAL_STACK_SIZE +#define STACK_SIZE configMINIMAL_STACK_SIZE #endif /* PIOS_EVENTDISPATCHER_STACK_SIZE */ -#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) #define MAX_UPDATE_PERIOD_MS 1000 // Private types @@ -51,24 +51,24 @@ * Event callback information */ typedef struct { - UAVObjEvent ev; /** The actual event */ - UAVObjEventCallback cb; /** The callback function, or zero if none */ - xQueueHandle queue; /** The queue or zero if none */ + UAVObjEvent ev; /** The actual event */ + UAVObjEventCallback cb; /** The callback function, or zero if none */ + xQueueHandle queue; /** The queue or zero if none */ } EventCallbackInfo; /** * List of object properties that are needed for the periodic updates. */ struct PeriodicObjectListStruct { - EventCallbackInfo evInfo; /** Event callback information */ + EventCallbackInfo evInfo; /** Event callback information */ uint16_t updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */ - int32_t timeToNextUpdateMs; /** Time delay to the next update */ - struct PeriodicObjectListStruct* next; /** Needed by linked list library (utlist.h) */ + int32_t timeToNextUpdateMs; /** Time delay to the next update */ + struct PeriodicObjectListStruct *next; /** Needed by linked list library (utlist.h) */ }; typedef struct PeriodicObjectListStruct PeriodicObjectList; // Private variables -static PeriodicObjectList* mObjList; +static PeriodicObjectList *mObjList; static xQueueHandle mQueue; static xTaskHandle mEventTaskHandle; static xSemaphoreHandle mMutex; @@ -77,8 +77,8 @@ static EventStats mStats; // Private functions static int32_t processPeriodicUpdates(); static void eventTask(); -static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs); -static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs); +static int32_t eventPeriodicCreate(UAVObjEvent *ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs); +static int32_t eventPeriodicUpdate(UAVObjEvent *ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs); static uint16_t randomizePeriod(uint16_t periodMs); @@ -88,34 +88,35 @@ static uint16_t randomizePeriod(uint16_t periodMs); */ int32_t EventDispatcherInitialize() { - // Initialize variables - mObjList = NULL; - memset(&mStats, 0, sizeof(EventStats)); + // Initialize variables + mObjList = NULL; + memset(&mStats, 0, sizeof(EventStats)); - // Create mMutex - mMutex = xSemaphoreCreateRecursiveMutex(); - if (mMutex == NULL) - return -1; + // Create mMutex + mMutex = xSemaphoreCreateRecursiveMutex(); + if (mMutex == NULL) { + return -1; + } - // Create event queue - mQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo)); + // Create event queue + mQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo)); - // Create task - xTaskCreate(eventTask, (signed char*)"Event", STACK_SIZE, NULL, TASK_PRIORITY, &mEventTaskHandle); + // Create task + xTaskCreate(eventTask, (signed char *)"Event", STACK_SIZE, NULL, TASK_PRIORITY, &mEventTaskHandle); - // Done - return 0; + // Done + return 0; } /** * Get the statistics counters * @param[out] statsOut The statistics counters will be copied there */ -void EventGetStats(EventStats* statsOut) +void EventGetStats(EventStats *statsOut) { - xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); - memcpy(statsOut, &mStats, sizeof(EventStats)); - xSemaphoreGiveRecursive(mMutex); + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); + memcpy(statsOut, &mStats, sizeof(EventStats)); + xSemaphoreGiveRecursive(mMutex); } /** @@ -123,9 +124,9 @@ void EventGetStats(EventStats* statsOut) */ void EventClearStats() { - xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); - memset(&mStats, 0, sizeof(EventStats)); - xSemaphoreGiveRecursive(mMutex); + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); + memset(&mStats, 0, sizeof(EventStats)); + xSemaphoreGiveRecursive(mMutex); } /** @@ -135,15 +136,16 @@ void EventClearStats() * \param[in] cb The callback function * \return Success (0), failure (-1) */ -int32_t EventCallbackDispatch(UAVObjEvent* ev, UAVObjEventCallback cb) +int32_t EventCallbackDispatch(UAVObjEvent *ev, UAVObjEventCallback cb) { - EventCallbackInfo evInfo; - // Initialize event callback information - memcpy(&evInfo.ev, ev, sizeof(UAVObjEvent)); - evInfo.cb = cb; - evInfo.queue = 0; - // Push to queue - return xQueueSend(mQueue, &evInfo, 0); // will not block if queue is full + EventCallbackInfo evInfo; + + // Initialize event callback information + memcpy(&evInfo.ev, ev, sizeof(UAVObjEvent)); + evInfo.cb = cb; + evInfo.queue = 0; + // Push to queue + return xQueueSend(mQueue, &evInfo, 0); // will not block if queue is full } /** @@ -153,9 +155,9 @@ int32_t EventCallbackDispatch(UAVObjEvent* ev, UAVObjEventCallback cb) * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, uint16_t periodMs) +int32_t EventPeriodicCallbackCreate(UAVObjEvent *ev, UAVObjEventCallback cb, uint16_t periodMs) { - return eventPeriodicCreate(ev, cb, 0, periodMs); + return eventPeriodicCreate(ev, cb, 0, periodMs); } /** @@ -165,9 +167,9 @@ int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, uin * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, uint16_t periodMs) +int32_t EventPeriodicCallbackUpdate(UAVObjEvent *ev, UAVObjEventCallback cb, uint16_t periodMs) { - return eventPeriodicUpdate(ev, cb, 0, periodMs); + return eventPeriodicUpdate(ev, cb, 0, periodMs); } /** @@ -177,9 +179,9 @@ int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, uin * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, uint16_t periodMs) +int32_t EventPeriodicQueueCreate(UAVObjEvent *ev, xQueueHandle queue, uint16_t periodMs) { - return eventPeriodicCreate(ev, 0, queue, periodMs); + return eventPeriodicCreate(ev, 0, queue, periodMs); } /** @@ -189,9 +191,9 @@ int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, uint16_t p * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, uint16_t periodMs) +int32_t EventPeriodicQueueUpdate(UAVObjEvent *ev, xQueueHandle queue, uint16_t periodMs) { - return eventPeriodicUpdate(ev, 0, queue, periodMs); + return eventPeriodicUpdate(ev, 0, queue, periodMs); } /** @@ -202,39 +204,40 @@ int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, uint16_t p * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs) +static int32_t eventPeriodicCreate(UAVObjEvent *ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs) { - PeriodicObjectList *objEntry; - // Get lock - xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); - // Check that the object is not already connected - LL_FOREACH(mObjList, objEntry) { - if (objEntry->evInfo.cb == cb && - objEntry->evInfo.queue == queue && - objEntry->evInfo.ev.obj == ev->obj && - objEntry->evInfo.ev.instId == ev->instId && - objEntry->evInfo.ev.event == ev->event) { - // Already registered, do nothing - xSemaphoreGiveRecursive(mMutex); - return -1; - } - } + PeriodicObjectList *objEntry; + + // Get lock + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); + // Check that the object is not already connected + LL_FOREACH(mObjList, objEntry) { + if (objEntry->evInfo.cb == cb && + objEntry->evInfo.queue == queue && + objEntry->evInfo.ev.obj == ev->obj && + objEntry->evInfo.ev.instId == ev->instId && + objEntry->evInfo.ev.event == ev->event) { + // Already registered, do nothing + xSemaphoreGiveRecursive(mMutex); + return -1; + } + } // Create handle - objEntry = (PeriodicObjectList*)pvPortMalloc(sizeof(PeriodicObjectList)); - if (objEntry == NULL) { - return -1; - } - objEntry->evInfo.ev.obj = ev->obj; - objEntry->evInfo.ev.instId = ev->instId; - objEntry->evInfo.ev.event = ev->event; - objEntry->evInfo.cb = cb; - objEntry->evInfo.queue = queue; - objEntry->updatePeriodMs = periodMs; + objEntry = (PeriodicObjectList *)pvPortMalloc(sizeof(PeriodicObjectList)); + if (objEntry == NULL) { + return -1; + } + objEntry->evInfo.ev.obj = ev->obj; + objEntry->evInfo.ev.instId = ev->instId; + objEntry->evInfo.ev.event = ev->event; + objEntry->evInfo.cb = cb; + objEntry->evInfo.queue = queue; + objEntry->updatePeriodMs = periodMs; objEntry->timeToNextUpdateMs = randomizePeriod(periodMs); // avoid bunching of updates // Add to list LL_APPEND(mObjList, objEntry); - // Release lock - xSemaphoreGiveRecursive(mMutex); + // Release lock + xSemaphoreGiveRecursive(mMutex); return 0; } @@ -246,28 +249,29 @@ static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs) +static int32_t eventPeriodicUpdate(UAVObjEvent *ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs) { - PeriodicObjectList* objEntry; - // Get lock - xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); - // Find object - LL_FOREACH(mObjList, objEntry) { - if (objEntry->evInfo.cb == cb && - objEntry->evInfo.queue == queue && - objEntry->evInfo.ev.obj == ev->obj && - objEntry->evInfo.ev.instId == ev->instId && - objEntry->evInfo.ev.event == ev->event) { - // Object found, update period - objEntry->updatePeriodMs = periodMs; - objEntry->timeToNextUpdateMs = randomizePeriod(periodMs); // avoid bunching of updates - // Release lock - xSemaphoreGiveRecursive(mMutex); - return 0; - } - } + PeriodicObjectList *objEntry; + + // Get lock + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); + // Find object + LL_FOREACH(mObjList, objEntry) { + if (objEntry->evInfo.cb == cb && + objEntry->evInfo.queue == queue && + objEntry->evInfo.ev.obj == ev->obj && + objEntry->evInfo.ev.instId == ev->instId && + objEntry->evInfo.ev.event == ev->event) { + // Object found, update period + objEntry->updatePeriodMs = periodMs; + objEntry->timeToNextUpdateMs = randomizePeriod(periodMs); // avoid bunching of updates + // Release lock + xSemaphoreGiveRecursive(mMutex); + return 0; + } + } // If this point is reached the object was not found - xSemaphoreGiveRecursive(mMutex); + xSemaphoreGiveRecursive(mMutex); return -1; } @@ -276,38 +280,34 @@ static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue */ static void eventTask() { - uint32_t timeToNextUpdateMs; - uint32_t delayMs; - EventCallbackInfo evInfo; + uint32_t timeToNextUpdateMs; + uint32_t delayMs; + EventCallbackInfo evInfo; - /* Must do this in task context to ensure that TaskMonitor has already finished its init */ - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_EVENTDISPATCHER, mEventTaskHandle); + /* Must do this in task context to ensure that TaskMonitor has already finished its init */ + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_EVENTDISPATCHER, mEventTaskHandle); - // Initialize time - timeToNextUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS; + // Initialize time + timeToNextUpdateMs = xTaskGetTickCount() * portTICK_RATE_MS; - // Loop forever - while (1) - { - // Calculate delay time - delayMs = timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS); + // Loop forever + while (1) { + // Calculate delay time + delayMs = timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS); - // Wait for queue message - if ( xQueueReceive(mQueue, &evInfo, delayMs/portTICK_RATE_MS) == pdTRUE ) - { - // Invoke callback, if one - if ( evInfo.cb != 0) - { - evInfo.cb(&evInfo.ev); // the function is expected to copy the event information - } - } + // Wait for queue message + if (xQueueReceive(mQueue, &evInfo, delayMs / portTICK_RATE_MS) == pdTRUE) { + // Invoke callback, if one + if (evInfo.cb != 0) { + evInfo.cb(&evInfo.ev); // the function is expected to copy the event information + } + } - // Process periodic updates - if ((xTaskGetTickCount()*portTICK_RATE_MS) >= timeToNextUpdateMs ) - { - timeToNextUpdateMs = processPeriodicUpdates(); - } - } + // Process periodic updates + if ((xTaskGetTickCount() * portTICK_RATE_MS) >= timeToNextUpdateMs) { + timeToNextUpdateMs = processPeriodicUpdates(); + } + } } /** @@ -316,43 +316,43 @@ static void eventTask() */ static int32_t processPeriodicUpdates() { - PeriodicObjectList* objEntry; - int32_t timeNow; + PeriodicObjectList *objEntry; + int32_t timeNow; int32_t timeToNextUpdate; int32_t offset; - // Get lock - xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mMutex, portMAX_DELAY); // Iterate through each object and update its timer, if zero then transmit object. // Also calculate smallest delay to next update. - timeToNextUpdate = xTaskGetTickCount()*portTICK_RATE_MS + MAX_UPDATE_PERIOD_MS; + timeToNextUpdate = xTaskGetTickCount() * portTICK_RATE_MS + MAX_UPDATE_PERIOD_MS; LL_FOREACH(mObjList, objEntry) { // If object is configured for periodic updates if (objEntry->updatePeriodMs > 0) { // Check if time for the next update - timeNow = xTaskGetTickCount()*portTICK_RATE_MS; + timeNow = xTaskGetTickCount() * portTICK_RATE_MS; if (objEntry->timeToNextUpdateMs <= timeNow) { // Reset timer - offset = (timeNow - objEntry->timeToNextUpdateMs) % objEntry->updatePeriodMs; - objEntry->timeToNextUpdateMs = timeNow + objEntry->updatePeriodMs - offset; - // Invoke callback, if one - if (objEntry->evInfo.cb != 0) { - objEntry->evInfo.cb(&objEntry->evInfo.ev); // the function is expected to copy the event information - } - // Push event to queue, if one - if (objEntry->evInfo.queue != 0) { - if (xQueueSend(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0) != pdTRUE ) { // do not block if queue is full - if (objEntry->evInfo.ev.obj != NULL) { - mStats.lastErrorID = UAVObjGetID(objEntry->evInfo.ev.obj); - } - ++mStats.eventErrors; - } - } + offset = (timeNow - objEntry->timeToNextUpdateMs) % objEntry->updatePeriodMs; + objEntry->timeToNextUpdateMs = timeNow + objEntry->updatePeriodMs - offset; + // Invoke callback, if one + if (objEntry->evInfo.cb != 0) { + objEntry->evInfo.cb(&objEntry->evInfo.ev); // the function is expected to copy the event information + } + // Push event to queue, if one + if (objEntry->evInfo.queue != 0) { + if (xQueueSend(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0) != pdTRUE) { // do not block if queue is full + if (objEntry->evInfo.ev.obj != NULL) { + mStats.lastErrorID = UAVObjGetID(objEntry->evInfo.ev.obj); + } + ++mStats.eventErrors; + } + } } // Update minimum delay if (objEntry->timeToNextUpdateMs < timeToNextUpdate) { - timeToNextUpdate = objEntry->timeToNextUpdateMs; + timeToNextUpdate = objEntry->timeToNextUpdateMs; } } } @@ -369,14 +369,16 @@ static int32_t processPeriodicUpdates() */ static uint16_t randomizePeriod(uint16_t periodMs) { - static uint32_t seed = 1; - uint32_t hi, lo; - lo = 16807 * (seed & 0xFFFF); - hi = 16807 * (seed >> 16); - lo += (hi & 0x7FFF) << 16; - lo += hi >> 15; - if (lo > 0x7FFFFFFF) lo -= 0x7FFFFFFF; - seed = lo; - return (uint16_t)( ((float)periodMs * (float)lo) / (float)0x7FFFFFFF ); -} + static uint32_t seed = 1; + uint32_t hi, lo; + lo = 16807 * (seed & 0xFFFF); + hi = 16807 * (seed >> 16); + lo += (hi & 0x7FFF) << 16; + lo += hi >> 15; + if (lo > 0x7FFFFFFF) { + lo -= 0x7FFFFFFF; + } + seed = lo; + return (uint16_t)(((float)periodMs * (float)lo) / (float)0x7FFFFFFF); +} diff --git a/flight/uavobjects/inc/callbackscheduler.h b/flight/uavobjects/inc/callbackscheduler.h index e989d847f..8dfd7b682 100644 --- a/flight/uavobjects/inc/callbackscheduler.h +++ b/flight/uavobjects/inc/callbackscheduler.h @@ -27,11 +27,11 @@ #define CALLBACKSCHEDULER_H // Public types -typedef enum{ - CALLBACK_PRIORITY_CRITICAL = 0, - CALLBACK_PRIORITY_REGULAR = 1, - CALLBACK_PRIORITY_LOW = 2 - } DelayedCallbackPriority; +typedef enum { + CALLBACK_PRIORITY_CRITICAL = 0, + CALLBACK_PRIORITY_REGULAR = 1, + CALLBACK_PRIORITY_LOW = 2 +} DelayedCallbackPriority; // Use the CallbackPriority to define how frequent a callback needs to be // called in relation to others in the same callback scheduling task. // The scheduler will call callbacks waiting for execution with the same @@ -56,12 +56,12 @@ typedef enum{ // a low priority callback can block a critical one from being executed. // Callbacks MUST NOT block execution! -typedef enum{ - CALLBACK_TASK_AUXILIARY = (tskIDLE_PRIORITY + 1), - CALLBACK_TASK_NAVIGATION = (tskIDLE_PRIORITY + 2), - CALLBACK_TASK_FLIGHTCONTROL = (tskIDLE_PRIORITY + 3), - CALLBACK_TASK_DEVICEDRIVER = (tskIDLE_PRIORITY + 4), - } DelayedCallbackPriorityTask; +typedef enum { + CALLBACK_TASK_AUXILIARY = (tskIDLE_PRIORITY + 1), + CALLBACK_TASK_NAVIGATION = (tskIDLE_PRIORITY + 2), + CALLBACK_TASK_FLIGHTCONTROL = (tskIDLE_PRIORITY + 3), + CALLBACK_TASK_DEVICEDRIVER = (tskIDLE_PRIORITY + 4), +} DelayedCallbackPriorityTask; // Use the PriorityTask to define the global importance of callback execution // compared to other processes in the system. // Callbacks dispatched in a higher PriorityTasks will halt the execution of @@ -78,12 +78,12 @@ typedef enum{ // WARNING: Any higher priority task can prevent lower priority code from being // executed! (This does not only apply to callbacks but to all FreeRTOS tasks!) -typedef enum{ - CALLBACK_UPDATEMODE_NONE = 0, - CALLBACK_UPDATEMODE_SOONER = 1, - CALLBACK_UPDATEMODE_LATER = 2, - CALLBACK_UPDATEMODE_OVERRIDE = 3 - } DelayedCallbackUpdateMode; +typedef enum { + CALLBACK_UPDATEMODE_NONE = 0, + CALLBACK_UPDATEMODE_SOONER = 1, + CALLBACK_UPDATEMODE_LATER = 2, + CALLBACK_UPDATEMODE_OVERRIDE = 3 +} DelayedCallbackUpdateMode; // When scheduling a callback for execution at a time in the future, use the // update mode to define what should happen if the callback is already // scheduled. @@ -138,11 +138,10 @@ int32_t CallbackSchedulerStart(); * \return CallbackInfo Pointer on success, NULL if failed. */ DelayedCallbackInfo *DelayedCallbackCreate( - DelayedCallback cb, - DelayedCallbackPriority priority, - DelayedCallbackPriorityTask priorityTask, - uint32_t stacksize - ); + DelayedCallback cb, + DelayedCallbackPriority priority, + DelayedCallbackPriorityTask priorityTask, + uint32_t stacksize); /** * Schedule dispatching a callback at some point in the future. The function returns immediately. @@ -157,10 +156,9 @@ DelayedCallbackInfo *DelayedCallbackCreate( * \return 0: not scheduled, previous schedule takes precedence, 1: new schedule, 2: previous schedule overridden */ int32_t DelayedCallbackSchedule( - DelayedCallbackInfo *cbinfo, - int32_t milliseconds, - DelayedCallbackUpdateMode updatemode - ); + DelayedCallbackInfo *cbinfo, + int32_t milliseconds, + DelayedCallbackUpdateMode updatemode); /** * Dispatch an event by invoking the supplied callback. The function @@ -168,7 +166,7 @@ int32_t DelayedCallbackSchedule( * \param[in] *cbinfo the callback handle * \return Success (-1), failure (0) */ -int32_t DelayedCallbackDispatch( DelayedCallbackInfo *cbinfo ); +int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo); /** * Dispatch an event by invoking the supplied callback. The function @@ -184,6 +182,6 @@ int32_t DelayedCallbackDispatch( DelayedCallbackInfo *cbinfo ); * Check the demo task for your port to find the syntax required. * \return Success (-1), failure (0) */ -int32_t DelayedCallbackDispatchFromISR( DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken ); +int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken); #endif // CALLBACKSCHEDULER_H diff --git a/flight/uavobjects/inc/eventdispatcher.h b/flight/uavobjects/inc/eventdispatcher.h index 258312624..07895fccc 100644 --- a/flight/uavobjects/inc/eventdispatcher.h +++ b/flight/uavobjects/inc/eventdispatcher.h @@ -31,18 +31,18 @@ * Event dispatcher statistics */ typedef struct { - uint32_t lastErrorID; - uint32_t eventErrors; + uint32_t lastErrorID; + uint32_t eventErrors; } EventStats; // Public functions int32_t EventDispatcherInitialize(); -void EventGetStats(EventStats* statsOut); +void EventGetStats(EventStats *statsOut); void EventClearStats(); -int32_t EventCallbackDispatch(UAVObjEvent* ev, UAVObjEventCallback cb); -int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, uint16_t periodMs); -int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, uint16_t periodMs); -int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, uint16_t periodMs); -int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, uint16_t periodMs); +int32_t EventCallbackDispatch(UAVObjEvent *ev, UAVObjEventCallback cb); +int32_t EventPeriodicCallbackCreate(UAVObjEvent *ev, UAVObjEventCallback cb, uint16_t periodMs); +int32_t EventPeriodicCallbackUpdate(UAVObjEvent *ev, UAVObjEventCallback cb, uint16_t periodMs); +int32_t EventPeriodicQueueCreate(UAVObjEvent *ev, xQueueHandle queue, uint16_t periodMs); +int32_t EventPeriodicQueueUpdate(UAVObjEvent *ev, xQueueHandle queue, uint16_t periodMs); #endif // EVENTDISPATCHER_H diff --git a/flight/uavobjects/inc/uavobjectmanager.h b/flight/uavobjects/inc/uavobjectmanager.h index 53183f497..211b7bfc9 100644 --- a/flight/uavobjects/inc/uavobjectmanager.h +++ b/flight/uavobjects/inc/uavobjectmanager.h @@ -1,11 +1,11 @@ /** ****************************************************************************** * @addtogroup UAVObjects OpenPilot UAVObjects - * @{ - * @addtogroup UAV Object Manager + * @{ + * @addtogroup UAV Object Manager * @brief The core UAV Objects functions, most of which are wrappered by * autogenerated defines - * @{ + * @{ * * @file uavobjectmanager.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -32,30 +32,30 @@ #ifndef UAVOBJECTMANAGER_H #define UAVOBJECTMANAGER_H -#define UAVOBJ_ALL_INSTANCES 0xFFFF -#define UAVOBJ_MAX_INSTANCES 1000 +#define UAVOBJ_ALL_INSTANCES 0xFFFF +#define UAVOBJ_MAX_INSTANCES 1000 /* * Shifts and masks used to read/write metadata flags. */ -#define UAVOBJ_ACCESS_SHIFT 0 -#define UAVOBJ_GCS_ACCESS_SHIFT 1 -#define UAVOBJ_TELEMETRY_ACKED_SHIFT 2 -#define UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT 3 -#define UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT 4 +#define UAVOBJ_ACCESS_SHIFT 0 +#define UAVOBJ_GCS_ACCESS_SHIFT 1 +#define UAVOBJ_TELEMETRY_ACKED_SHIFT 2 +#define UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT 3 +#define UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT 4 #define UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT 6 -#define UAVOBJ_UPDATE_MODE_MASK 0x3 +#define UAVOBJ_UPDATE_MODE_MASK 0x3 -typedef void* UAVObjHandle; +typedef void *UAVObjHandle; /** * Object update mode, used by multiple modules (e.g. telemetry and logger) */ typedef enum { - UPDATEMODE_MANUAL = 0, /** Manually update object, by calling the updated() function */ - UPDATEMODE_PERIODIC = 1, /** Automatically update object at periodic intervals */ - UPDATEMODE_ONCHANGE = 2, /** Only update object when its data changes */ - UPDATEMODE_THROTTLED = 3 /** Object is updated on change, but not more often than the interval time */ + UPDATEMODE_MANUAL = 0, /** Manually update object, by calling the updated() function */ + UPDATEMODE_PERIODIC = 1, /** Automatically update object at periodic intervals */ + UPDATEMODE_ONCHANGE = 2, /** Only update object when its data changes */ + UPDATEMODE_THROTTLED = 3 /** Object is updated on change, but not more often than the interval time */ } UAVObjUpdateMode; /** @@ -75,45 +75,45 @@ typedef enum { * 6-7 gcsTelemetryUpdateMode Update mode used by the GCS (UAVObjUpdateMode) */ typedef struct { - uint8_t flags; /** Defines flags for update and logging modes and whether an update should be ACK'd (bits defined above) */ - uint16_t telemetryUpdatePeriod; /** Update period used by the telemetry module (only if telemetry mode is PERIODIC) */ - uint16_t gcsTelemetryUpdatePeriod; /** Update period used by the GCS (only if telemetry mode is PERIODIC) */ - uint16_t loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */ + uint8_t flags; /** Defines flags for update and logging modes and whether an update should be ACK'd (bits defined above) */ + uint16_t telemetryUpdatePeriod; /** Update period used by the telemetry module (only if telemetry mode is PERIODIC) */ + uint16_t gcsTelemetryUpdatePeriod; /** Update period used by the GCS (only if telemetry mode is PERIODIC) */ + uint16_t loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */ } __attribute__((packed)) UAVObjMetadata; /** * Event types generated by the objects. */ typedef enum { - EV_NONE = 0x00, /** No event */ - EV_UNPACKED = 0x01, /** Object data updated by unpacking */ - EV_UPDATED = 0x02, /** Object data updated by changing the data structure */ - EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */ - EV_UPDATED_PERIODIC = 0x08, /** Object update from periodic event */ - EV_UPDATE_REQ = 0x10 /** Request to update object data */ + EV_NONE = 0x00, /** No event */ + EV_UNPACKED = 0x01, /** Object data updated by unpacking */ + EV_UPDATED = 0x02, /** Object data updated by changing the data structure */ + EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */ + EV_UPDATED_PERIODIC = 0x08, /** Object update from periodic event */ + EV_UPDATE_REQ = 0x10 /** Request to update object data */ } UAVObjEventType; /** * Helper macros for event masks */ -#define EV_MASK_ALL 0 +#define EV_MASK_ALL 0 #define EV_MASK_ALL_UPDATES (EV_UNPACKED | EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATED_PERIODIC) /** * Access types */ typedef enum { - ACCESS_READWRITE = 0, - ACCESS_READONLY = 1 + ACCESS_READWRITE = 0, + ACCESS_READONLY = 1 } UAVObjAccessType; /** * Event message, this structure is sent in the event queue each time an event is generated */ typedef struct { - UAVObjHandle obj; - uint16_t instId; - UAVObjEventType event; + UAVObjHandle obj; + uint16_t instId; + UAVObjEventType event; } UAVObjEvent; /** @@ -121,7 +121,7 @@ typedef struct { * will be executed in the event task. The ev parameter should be copied if needed * after the function returns. */ -typedef void (*UAVObjEventCallback)(UAVObjEvent* ev); +typedef void (*UAVObjEventCallback)(UAVObjEvent *ev); /** * Callback used to initialize the object fields to their default values. @@ -132,17 +132,17 @@ typedef void (*UAVObjInitializeCallback)(UAVObjHandle obj_handle, uint16_t instI * Event manager statistics */ typedef struct { - uint32_t eventQueueErrors; - uint32_t eventCallbackErrors; - uint32_t lastCallbackErrorID; - uint32_t lastQueueErrorID; + uint32_t eventQueueErrors; + uint32_t eventCallbackErrors; + uint32_t lastCallbackErrorID; + uint32_t lastQueueErrorID; } UAVObjStats; int32_t UAVObjInitialize(); -void UAVObjGetStats(UAVObjStats* statsOut); +void UAVObjGetStats(UAVObjStats *statsOut); void UAVObjClearStats(); UAVObjHandle UAVObjRegister(uint32_t id, - int32_t isSingleInstance, int32_t isSettings, uint32_t numBytes, UAVObjInitializeCallback initCb); + int32_t isSingleInstance, int32_t isSettings, uint32_t numBytes, UAVObjInitializeCallback initCb); UAVObjHandle UAVObjGetByID(uint32_t id); uint32_t UAVObjGetID(UAVObjHandle obj); uint32_t UAVObjGetNumBytes(UAVObjHandle obj); @@ -152,14 +152,14 @@ uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle, UAVObjInitializeCallback bool UAVObjIsSingleInstance(UAVObjHandle obj); bool UAVObjIsMetaobject(UAVObjHandle obj); bool UAVObjIsSettings(UAVObjHandle obj); -int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t* dataIn); -int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t* dataOut); +int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn); +int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t *dataOut); int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId); int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId); int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId); #if defined(PIOS_INCLUDE_SDCARD) -int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId, FILEINFO* file); -UAVObjHandle UAVObjLoadFromFile(FILEINFO* file); +int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId, FILEINFO *file); +UAVObjHandle UAVObjLoadFromFile(FILEINFO *file); #endif int32_t UAVObjSaveSettings(); int32_t UAVObjLoadSettings(); @@ -167,29 +167,29 @@ int32_t UAVObjDeleteSettings(); int32_t UAVObjSaveMetaobjects(); int32_t UAVObjLoadMetaobjects(); int32_t UAVObjDeleteMetaobjects(); -int32_t UAVObjSetData(UAVObjHandle obj_handle, const void* dataIn); -int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void* dataIn, uint32_t offset, uint32_t size); -int32_t UAVObjGetData(UAVObjHandle obj_handle, void* dataOut); -int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void* dataOut, uint32_t offset, uint32_t size); -int32_t UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId, const void* dataIn); -int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size); -int32_t UAVObjGetInstanceData(UAVObjHandle obj_handle, uint16_t instId, void* dataOut); -int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size); -int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata* dataIn); -int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata* dataOut); -uint8_t UAVObjGetMetadataAccess(const UAVObjMetadata* dataOut); -UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* dataOut); -void UAVObjSetAccess(UAVObjMetadata* dataOut, UAVObjAccessType mode); -UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* dataOut); -void UAVObjSetGcsAccess(UAVObjMetadata* dataOut, UAVObjAccessType mode); -uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata* dataOut); -void UAVObjSetTelemetryAcked( UAVObjMetadata* dataOut, uint8_t val); -uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata* dataOut); -void UAVObjSetGcsTelemetryAcked(UAVObjMetadata* dataOut, uint8_t val); -UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata* dataOut); -void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* dataOut, UAVObjUpdateMode val); -UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* dataOut); -void UAVObjSetTelemetryGcsUpdateMode(UAVObjMetadata* dataOut, UAVObjUpdateMode val); +int32_t UAVObjSetData(UAVObjHandle obj_handle, const void *dataIn); +int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void *dataIn, uint32_t offset, uint32_t size); +int32_t UAVObjGetData(UAVObjHandle obj_handle, void *dataOut); +int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void *dataOut, uint32_t offset, uint32_t size); +int32_t UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId, const void *dataIn); +int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void *dataIn, uint32_t offset, uint32_t size); +int32_t UAVObjGetInstanceData(UAVObjHandle obj_handle, uint16_t instId, void *dataOut); +int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, void *dataOut, uint32_t offset, uint32_t size); +int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata *dataIn); +int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata *dataOut); +uint8_t UAVObjGetMetadataAccess(const UAVObjMetadata *dataOut); +UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata *dataOut); +void UAVObjSetAccess(UAVObjMetadata *dataOut, UAVObjAccessType mode); +UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata *dataOut); +void UAVObjSetGcsAccess(UAVObjMetadata *dataOut, UAVObjAccessType mode); +uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata *dataOut); +void UAVObjSetTelemetryAcked(UAVObjMetadata *dataOut, uint8_t val); +uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata *dataOut); +void UAVObjSetGcsTelemetryAcked(UAVObjMetadata *dataOut, uint8_t val); +UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata *dataOut); +void UAVObjSetTelemetryUpdateMode(UAVObjMetadata *dataOut, UAVObjUpdateMode val); +UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata *dataOut); +void UAVObjSetTelemetryGcsUpdateMode(UAVObjMetadata *dataOut, UAVObjUpdateMode val); int8_t UAVObjReadOnly(UAVObjHandle obj); int32_t UAVObjConnectQueue(UAVObjHandle obj_handle, xQueueHandle queue, uint8_t eventMask); int32_t UAVObjDisconnectQueue(UAVObjHandle obj_handle, xQueueHandle queue); diff --git a/flight/uavobjects/inc/utlist.h b/flight/uavobjects/inc/utlist.h index 40ebbc687..ef833289f 100644 --- a/flight/uavobjects/inc/utlist.h +++ b/flight/uavobjects/inc/utlist.h @@ -1,32 +1,32 @@ /* -Copyright (c) 2007-2009, Troy D. Hanson -All rights reserved. + Copyright (c) 2007-2009, Troy D. Hanson + All rights reserved. -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright + * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. -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. -*/ + 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. + */ #ifndef UTLIST_H #define UTLIST_H #define UTLIST_VERSION 1.8 -/* +/* * This file contains macros to manipulate singly and doubly-linked lists. * * 1. LL_ macros: singly-linked lists. @@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * To use singly-linked lists, your structure must have a "next" pointer. * To use doubly-linked lists, your structure must "prev" and "next" pointers. * Either way, the pointer to the head of the list must be initialized to NULL. - * + * * ----------------.EXAMPLE ------------------------- * struct item { * int id; @@ -62,288 +62,319 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * Unwieldy variable names used here to avoid shadowing passed-in variables. * *****************************************************************************/ #define LL_SORT(list, cmp) \ -do { \ - __typeof__(list) _ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \ - int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ - if (list) { \ - _ls_insize = 1; \ - _ls_looping = 1; \ - while (_ls_looping) { \ - _ls_p = list; \ - _ls_oldhead = list; \ - list = NULL; \ - _ls_tail = NULL; \ - _ls_nmerges = 0; \ - while (_ls_p) { \ - _ls_nmerges++; \ - _ls_q = _ls_p; \ - _ls_psize = 0; \ - for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ - _ls_psize++; \ - _ls_q = _ls_q->next; \ - if (!_ls_q) break; \ - } \ - _ls_qsize = _ls_insize; \ - while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ - if (_ls_psize == 0) { \ - _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ - } else if (_ls_qsize == 0 || !_ls_q) { \ - _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ - } else if (cmp(_ls_p,_ls_q) <= 0) { \ - _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ - } else { \ - _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ - } \ - if (_ls_tail) { \ - _ls_tail->next = _ls_e; \ - } else { \ - list = _ls_e; \ - } \ - _ls_tail = _ls_e; \ - } \ - _ls_p = _ls_q; \ - } \ - _ls_tail->next = NULL; \ - if (_ls_nmerges <= 1) { \ - _ls_looping=0; \ - } \ - _ls_insize *= 2; \ - } \ - } \ -} while (0) + do { \ + __typeof__(list)_ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \ + int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ + if (list) { \ + _ls_insize = 1; \ + _ls_looping = 1; \ + while (_ls_looping) { \ + _ls_p = list; \ + _ls_oldhead = list; \ + list = NULL; \ + _ls_tail = NULL; \ + _ls_nmerges = 0; \ + while (_ls_p) { \ + _ls_nmerges++; \ + _ls_q = _ls_p; \ + _ls_psize = 0; \ + for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ + _ls_psize++; \ + _ls_q = _ls_q->next; \ + if (!_ls_q) { break; } \ + } \ + _ls_qsize = _ls_insize; \ + while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ + if (_ls_psize == 0) { \ + _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ + } \ + else if (_ls_qsize == 0 || !_ls_q) { \ + _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ + } \ + else if (cmp(_ls_p, _ls_q) <= 0) { \ + _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ + } \ + else { \ + _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ + } \ + if (_ls_tail) { \ + _ls_tail->next = _ls_e; \ + } \ + else { \ + list = _ls_e; \ + } \ + _ls_tail = _ls_e; \ + } \ + _ls_p = _ls_q; \ + } \ + _ls_tail->next = NULL; \ + if (_ls_nmerges <= 1) { \ + _ls_looping = 0; \ + } \ + _ls_insize *= 2; \ + } \ + } \ + } \ + while (0) #define DL_SORT(list, cmp) \ -do { \ - __typeof__(list) _ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \ - int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ - if (list) { \ - _ls_insize = 1; \ - _ls_looping = 1; \ - while (_ls_looping) { \ - _ls_p = list; \ - _ls_oldhead = list; \ - list = NULL; \ - _ls_tail = NULL; \ - _ls_nmerges = 0; \ - while (_ls_p) { \ - _ls_nmerges++; \ - _ls_q = _ls_p; \ - _ls_psize = 0; \ - for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ - _ls_psize++; \ - _ls_q = _ls_q->next; \ - if (!_ls_q) break; \ - } \ - _ls_qsize = _ls_insize; \ - while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ - if (_ls_psize == 0) { \ - _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ - } else if (_ls_qsize == 0 || !_ls_q) { \ - _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ - } else if (cmp(_ls_p,_ls_q) <= 0) { \ - _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ - } else { \ - _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ - } \ - if (_ls_tail) { \ - _ls_tail->next = _ls_e; \ - } else { \ - list = _ls_e; \ - } \ - _ls_e->prev = _ls_tail; \ - _ls_tail = _ls_e; \ - } \ - _ls_p = _ls_q; \ - } \ - list->prev = _ls_tail; \ - _ls_tail->next = NULL; \ - if (_ls_nmerges <= 1) { \ - _ls_looping=0; \ - } \ - _ls_insize *= 2; \ - } \ - } \ -} while (0) + do { \ + __typeof__(list)_ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \ + int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ + if (list) { \ + _ls_insize = 1; \ + _ls_looping = 1; \ + while (_ls_looping) { \ + _ls_p = list; \ + _ls_oldhead = list; \ + list = NULL; \ + _ls_tail = NULL; \ + _ls_nmerges = 0; \ + while (_ls_p) { \ + _ls_nmerges++; \ + _ls_q = _ls_p; \ + _ls_psize = 0; \ + for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ + _ls_psize++; \ + _ls_q = _ls_q->next; \ + if (!_ls_q) { break; } \ + } \ + _ls_qsize = _ls_insize; \ + while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ + if (_ls_psize == 0) { \ + _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ + } \ + else if (_ls_qsize == 0 || !_ls_q) { \ + _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ + } \ + else if (cmp(_ls_p, _ls_q) <= 0) { \ + _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ + } \ + else { \ + _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ + } \ + if (_ls_tail) { \ + _ls_tail->next = _ls_e; \ + } \ + else { \ + list = _ls_e; \ + } \ + _ls_e->prev = _ls_tail; \ + _ls_tail = _ls_e; \ + } \ + _ls_p = _ls_q; \ + } \ + list->prev = _ls_tail; \ + _ls_tail->next = NULL; \ + if (_ls_nmerges <= 1) { \ + _ls_looping = 0; \ + } \ + _ls_insize *= 2; \ + } \ + } \ + } \ + while (0) #define CDL_SORT(list, cmp) \ -do { \ - __typeof__(list) _ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \ - int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ - if (list) { \ - _ls_insize = 1; \ - _ls_looping = 1; \ - while (_ls_looping) { \ - _ls_p = list; \ - _ls_oldhead = list; \ - list = NULL; \ - _ls_tail = NULL; \ - _ls_nmerges = 0; \ - while (_ls_p) { \ - _ls_nmerges++; \ - _ls_q = _ls_p; \ - _ls_psize = 0; \ - for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ - _ls_psize++; \ - _ls_q = ((_ls_q->next == _ls_oldhead) ? NULL : _ls_q->next); \ - if (!_ls_q) break; \ - } \ - _ls_qsize = _ls_insize; \ - while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ - if (_ls_psize == 0) { \ - _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ - if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \ - } else if (_ls_qsize == 0 || !_ls_q) { \ - _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ - if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \ - } else if (cmp(_ls_p,_ls_q) <= 0) { \ - _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ - if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \ - } else { \ - _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ - if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \ - } \ - if (_ls_tail) { \ - _ls_tail->next = _ls_e; \ - } else { \ - list = _ls_e; \ - } \ - _ls_e->prev = _ls_tail; \ - _ls_tail = _ls_e; \ - } \ - _ls_p = _ls_q; \ - } \ - list->prev = _ls_tail; \ - _ls_tail->next = list; \ - if (_ls_nmerges <= 1) { \ - _ls_looping=0; \ - } \ - _ls_insize *= 2; \ - } \ - } \ -} while (0) + do { \ + __typeof__(list)_ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \ + int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ + if (list) { \ + _ls_insize = 1; \ + _ls_looping = 1; \ + while (_ls_looping) { \ + _ls_p = list; \ + _ls_oldhead = list; \ + list = NULL; \ + _ls_tail = NULL; \ + _ls_nmerges = 0; \ + while (_ls_p) { \ + _ls_nmerges++; \ + _ls_q = _ls_p; \ + _ls_psize = 0; \ + for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ + _ls_psize++; \ + _ls_q = ((_ls_q->next == _ls_oldhead) ? NULL : _ls_q->next); \ + if (!_ls_q) { break; } \ + } \ + _ls_qsize = _ls_insize; \ + while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ + if (_ls_psize == 0) { \ + _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ + if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \ + } \ + else if (_ls_qsize == 0 || !_ls_q) { \ + _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ + if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \ + } \ + else if (cmp(_ls_p, _ls_q) <= 0) { \ + _ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \ + if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \ + } \ + else { \ + _ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \ + if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \ + } \ + if (_ls_tail) { \ + _ls_tail->next = _ls_e; \ + } \ + else { \ + list = _ls_e; \ + } \ + _ls_e->prev = _ls_tail; \ + _ls_tail = _ls_e; \ + } \ + _ls_p = _ls_q; \ + } \ + list->prev = _ls_tail; \ + _ls_tail->next = list; \ + if (_ls_nmerges <= 1) { \ + _ls_looping = 0; \ + } \ + _ls_insize *= 2; \ + } \ + } \ + } \ + while (0) /****************************************************************************** * singly linked list macros (non-circular) * *****************************************************************************/ -#define LL_PREPEND(head,add) \ -do { \ - (add)->next = head; \ - head = add; \ -} while (0) +#define LL_PREPEND(head, add) \ + do { \ + (add)->next = head; \ + head = add; \ + } \ + while (0) -#define LL_APPEND(head,add) \ -do { \ - __typeof__(head) _tmp; \ - (add)->next=NULL; \ - if (head) { \ - _tmp = head; \ - while (_tmp->next) { _tmp = _tmp->next; } \ - _tmp->next=(add); \ - } else { \ - (head)=(add); \ - } \ -} while (0) +#define LL_APPEND(head, add) \ + do { \ + __typeof__(head)_tmp; \ + (add)->next = NULL; \ + if (head) { \ + _tmp = head; \ + while (_tmp->next) { _tmp = _tmp->next; } \ + _tmp->next = (add); \ + } \ + else { \ + (head) = (add); \ + } \ + } \ + while (0) -#define LL_DELETE(head,del) \ -do { \ - __typeof__(head) _tmp; \ - if ((head) == (del)) { \ - (head)=(head)->next; \ - } else { \ - _tmp = head; \ - while (_tmp->next && (_tmp->next != (del))) { \ - _tmp = _tmp->next; \ - } \ - if (_tmp->next) { \ - _tmp->next = ((del)->next); \ - } \ - } \ -} while (0) +#define LL_DELETE(head, del) \ + do { \ + __typeof__(head)_tmp; \ + if ((head) == (del)) { \ + (head) = (head)->next; \ + } \ + else { \ + _tmp = head; \ + while (_tmp->next && (_tmp->next != (del))) { \ + _tmp = _tmp->next; \ + } \ + if (_tmp->next) { \ + _tmp->next = ((del)->next); \ + } \ + } \ + } \ + while (0) -#define LL_FOREACH(head,el) \ - for(el=head;el;el=el->next) +#define LL_FOREACH(head, el) \ + for (el = head; el; el = el->next) /****************************************************************************** * doubly linked list macros (non-circular) * *****************************************************************************/ -#define DL_PREPEND(head,add) \ -do { \ - (add)->next = head; \ - if (head) { \ - (add)->prev = (head)->prev; \ - (head)->prev = (add); \ - } else { \ - (add)->prev = (add); \ - } \ - (head) = (add); \ -} while (0) +#define DL_PREPEND(head, add) \ + do { \ + (add)->next = head; \ + if (head) { \ + (add)->prev = (head)->prev; \ + (head)->prev = (add); \ + } \ + else { \ + (add)->prev = (add); \ + } \ + (head) = (add); \ + } \ + while (0) -#define DL_APPEND(head,add) \ -do { \ - if (head) { \ - (add)->prev = (head)->prev; \ - (head)->prev->next = (add); \ - (head)->prev = (add); \ - (add)->next = NULL; \ - } else { \ - (head)=(add); \ - (head)->prev = (head); \ - (head)->next = NULL; \ - } \ -} while (0); +#define DL_APPEND(head, add) \ + do { \ + if (head) { \ + (add)->prev = (head)->prev; \ + (head)->prev->next = (add); \ + (head)->prev = (add); \ + (add)->next = NULL; \ + } \ + else { \ + (head) = (add); \ + (head)->prev = (head); \ + (head)->next = NULL; \ + } \ + } \ + while (0); -#define DL_DELETE(head,del) \ -do { \ - if ((del)->prev == (del)) { \ - (head)=NULL; \ - } else if ((del)==(head)) { \ - (del)->next->prev = (del)->prev; \ - (head) = (del)->next; \ - } else { \ - (del)->prev->next = (del)->next; \ - if ((del)->next) { \ - (del)->next->prev = (del)->prev; \ - } else { \ - (head)->prev = (del)->prev; \ - } \ - } \ -} while (0); +#define DL_DELETE(head, del) \ + do { \ + if ((del)->prev == (del)) { \ + (head) = NULL; \ + } \ + else if ((del) == (head)) { \ + (del)->next->prev = (del)->prev; \ + (head) = (del)->next; \ + } \ + else { \ + (del)->prev->next = (del)->next; \ + if ((del)->next) { \ + (del)->next->prev = (del)->prev; \ + } \ + else { \ + (head)->prev = (del)->prev; \ + } \ + } \ + } \ + while (0); -#define DL_FOREACH(head,el) \ - for(el=head;el;el=el->next) +#define DL_FOREACH(head, el) \ + for (el = head; el; el = el->next) /****************************************************************************** * circular doubly linked list macros * *****************************************************************************/ -#define CDL_PREPEND(head,add) \ -do { \ - if (head) { \ - (add)->prev = (head)->prev; \ - (add)->next = (head); \ - (head)->prev = (add); \ - (add)->prev->next = (add); \ - } else { \ - (add)->prev = (add); \ - (add)->next = (add); \ - } \ -(head)=(add); \ -} while (0) +#define CDL_PREPEND(head, add) \ + do { \ + if (head) { \ + (add)->prev = (head)->prev; \ + (add)->next = (head); \ + (head)->prev = (add); \ + (add)->prev->next = (add); \ + } \ + else { \ + (add)->prev = (add); \ + (add)->next = (add); \ + } \ + (head) = (add); \ + } \ + while (0) -#define CDL_DELETE(head,del) \ -do { \ - if ( ((head)==(del)) && ((head)->next == (head))) { \ - (head) = 0L; \ - } else { \ - (del)->next->prev = (del)->prev; \ - (del)->prev->next = (del)->next; \ - if ((del) == (head)) (head)=(del)->next; \ - } \ -} while (0); +#define CDL_DELETE(head, del) \ + do { \ + if (((head) == (del)) && ((head)->next == (head))) { \ + (head) = 0L; \ + } \ + else { \ + (del)->next->prev = (del)->prev; \ + (del)->prev->next = (del)->next; \ + if ((del) == (head)) { (head) = (del)->next; } \ + } \ + } \ + while (0); -#define CDL_FOREACH(head,el) \ - for(el=head;el;el= (el->next==head ? 0L : el->next)) +#define CDL_FOREACH(head, el) \ + for (el = head; el; el = (el->next == head ? 0L : el->next)) #endif /* UTLIST_H */ - diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c index f0044f951..d38c512ed 100644 --- a/flight/uavobjects/uavobjectmanager.c +++ b/flight/uavobjects/uavobjectmanager.c @@ -1,20 +1,20 @@ /** -****************************************************************************** -* @addtogroup UAVObjects OpenPilot UAVObjects -* @{ -* @addtogroup UAV Object Manager -* @brief The core UAV Objects functions, most of which are wrappered by -* autogenerated defines -* @{ -* -* -* @file uavobjectmanager.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. -* @brief Object manager library. This library holds a collection of all objects. -* It can be used by all modules/libraries to find an object reference. -* @see The GNU Public License (GPL) Version 3 -* -*****************************************************************************/ + ****************************************************************************** + * @addtogroup UAVObjects OpenPilot UAVObjects + * @{ + * @addtogroup UAV Object Manager + * @brief The core UAV Objects functions, most of which are wrappered by + * autogenerated defines + * @{ + * + * + * @file uavobjectmanager.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Object manager library. This library holds a collection of all objects. + * It can be used by all modules/libraries to find an object reference. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -55,43 +55,43 @@ static long _aslr_offset __attribute__((section("__DATA,_aslr"))); /* Table of UAVO handles */ #if (defined(__MACH__) && defined(__APPLE__)) /* Mach-o format */ -static struct UAVOData ** __start__uavo_handles; -static struct UAVOData ** __stop__uavo_handles; +static struct UAVOData * *__start__uavo_handles; +static struct UAVOData * *__stop__uavo_handles; #else /* ELF format: automagically defined at compile time */ -extern struct UAVOData * __start__uavo_handles[] __attribute__((weak)); -extern struct UAVOData * __stop__uavo_handles[] __attribute__((weak)); +extern struct UAVOData *__start__uavo_handles[] __attribute__((weak)); +extern struct UAVOData *__stop__uavo_handles[] __attribute__((weak)); #endif #define UAVO_LIST_ITERATE(_item) \ -for (struct UAVOData ** _uavo_slot = __start__uavo_handles; \ - _uavo_slot && _uavo_slot < __stop__uavo_handles; \ - _uavo_slot++) { \ - struct UAVOData * _item = *_uavo_slot; \ - if (_item == NULL) continue; + for (struct UAVOData * *_uavo_slot = __start__uavo_handles; \ + _uavo_slot && _uavo_slot < __stop__uavo_handles; \ + _uavo_slot++) { \ + struct UAVOData *_item = *_uavo_slot; \ + if (_item == NULL) { continue; } /** * List of event queues and the eventmask associated with the queue. */ /** opaque type for instances **/ -typedef void* InstanceHandle; +typedef void *InstanceHandle; struct ObjectEventEntry { - struct ObjectEventEntry * next; - xQueueHandle queue; - UAVObjEventCallback cb; - uint8_t eventMask; + struct ObjectEventEntry *next; + xQueueHandle queue; + UAVObjEventCallback cb; + uint8_t eventMask; }; /* - MetaInstance == [UAVOBase [UAVObjMetadata]] - SingleInstance == [UAVOBase [UAVOData [InstanceData]]] - MultiInstance == [UAVOBase [UAVOData [NumInstances [InstanceData0 [next]]]] + MetaInstance == [UAVOBase [UAVObjMetadata]] + SingleInstance == [UAVOBase [UAVOData [InstanceData]]] + MultiInstance == [UAVOBase [UAVOData [NumInstances [InstanceData0 [next]]]] ____________________/ - \-->[InstanceData1 [next]] + \-->[InstanceData1 [next]] _________...________/ - \-->[InstanceDataN [next]] + \-->[InstanceDataN [next]] */ /* @@ -100,111 +100,111 @@ struct ObjectEventEntry { * - The flags determine what type(s) this object */ struct UAVOBase { - /* Let these objects be added to an event queue */ - struct ObjectEventEntry * next_event; + /* Let these objects be added to an event queue */ + struct ObjectEventEntry *next_event; - /* Describe the type of object that follows this header */ - struct UAVOInfo { - bool isMeta : 1; - bool isSingle : 1; - bool isSettings : 1; - } flags; + /* Describe the type of object that follows this header */ + struct UAVOInfo { + bool isMeta : 1; + bool isSingle : 1; + bool isSettings : 1; + } flags; } __attribute__((packed)); /* Augmented type for Meta UAVO */ struct UAVOMeta { - struct UAVOBase base; - UAVObjMetadata instance0; + struct UAVOBase base; + UAVObjMetadata instance0; } __attribute__((packed)); /* Shared data structure for all data-carrying UAVObjects (UAVOSingle and UAVOMulti) */ struct UAVOData { - struct UAVOBase base; - uint32_t id; - /* - * Embed the Meta object as another complete UAVO - * inside the payload for this UAVO. - */ - struct UAVOMeta metaObj; - uint16_t instance_size; + struct UAVOBase base; + uint32_t id; + /* + * Embed the Meta object as another complete UAVO + * inside the payload for this UAVO. + */ + struct UAVOMeta metaObj; + uint16_t instance_size; } __attribute__((packed, aligned(4))); /* Augmented type for Single Instance Data UAVO */ struct UAVOSingle { - struct UAVOData uavo; + struct UAVOData uavo; - uint8_t instance0[]; - /* - * Additional space will be malloc'd here to hold the - * the data for this instance. - */ + uint8_t instance0[]; + /* + * Additional space will be malloc'd here to hold the + * the data for this instance. + */ } __attribute__((packed)); /* Part of a linked list of instances chained off of a multi instance UAVO. */ struct UAVOMultiInst { - struct UAVOMultiInst * next; - uint8_t instance[]; - /* - * Additional space will be malloc'd here to hold the - * the data for this instance. - */ + struct UAVOMultiInst *next; + uint8_t instance[]; + /* + * Additional space will be malloc'd here to hold the + * the data for this instance. + */ } __attribute__((packed)); /* Augmented type for Multi Instance Data UAVO */ struct UAVOMulti { - struct UAVOData uavo; - uint16_t num_instances; - struct UAVOMultiInst instance0 __attribute__((aligned(4))); - /* - * Additional space will be malloc'd here to hold the - * the data for instance 0. - */ + struct UAVOData uavo; + uint16_t num_instances; + struct UAVOMultiInst instance0 __attribute__((aligned(4))); + /* + * Additional space will be malloc'd here to hold the + * the data for instance 0. + */ } __attribute__((packed)); /** all information about a metaobject are hardcoded constants **/ #define MetaNumBytes sizeof(UAVObjMetadata) -#define MetaBaseObjectPtr(obj) ((struct UAVOData *)((obj)-offsetof(struct UAVOData, metaObj))) -#define MetaObjectPtr(obj) ((struct UAVODataMeta*) &((obj)->metaObj)) -#define MetaDataPtr(obj) ((UAVObjMetadata*)&((obj)->instance0)) -#define LinkedMetaDataPtr(obj) ((UAVObjMetadata*)&((obj)->metaObj.instance0)) -#define MetaObjectId(id) ((id)+1) +#define MetaBaseObjectPtr(obj) ((struct UAVOData *)((obj) - offsetof(struct UAVOData, metaObj))) +#define MetaObjectPtr(obj) ((struct UAVODataMeta *)&((obj)->metaObj)) +#define MetaDataPtr(obj) ((UAVObjMetadata *)&((obj)->instance0)) +#define LinkedMetaDataPtr(obj) ((UAVObjMetadata *)&((obj)->metaObj.instance0)) +#define MetaObjectId(id) ((id) + 1) /** all information about instances are dependant on object type **/ -#define ObjSingleInstanceDataOffset(obj) ((void*)(&(( (struct UAVOSingle*)obj )->instance0))) -#define InstanceDataOffset(inst) ((void*)&(( (struct UAVOMultiInst*)inst )->instance)) -#define InstanceData(instance) (void*)instance +#define ObjSingleInstanceDataOffset(obj) ((void *)(&(((struct UAVOSingle *)obj)->instance0))) +#define InstanceDataOffset(inst) ((void *)&(((struct UAVOMultiInst *)inst)->instance)) +#define InstanceData(instance) (void *)instance // Private functions -static int32_t sendEvent(struct UAVOBase * obj, uint16_t instId, - UAVObjEventType event); -static InstanceHandle createInstance(struct UAVOData * obj, uint16_t instId); -static InstanceHandle getInstance(struct UAVOData * obj, uint16_t instId); +static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, + UAVObjEventType event); +static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId); +static InstanceHandle getInstance(struct UAVOData *obj, uint16_t instId); static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, - UAVObjEventCallback cb, uint8_t eventMask); + UAVObjEventCallback cb, uint8_t eventMask); static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue, - UAVObjEventCallback cb); + UAVObjEventCallback cb); #if defined(PIOS_USE_SETTINGS_ON_SDCARD) && defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) #error Both PIOS_USE_SETTINGS_ON_SDCARD and PIOS_INCLUDE_FLASH_LOGFS_SETTINGS. Only one settings storage allowed. #endif #if defined(PIOS_USE_SETTINGS_ON_SDCARD) -static void objectFilename(UAVObjHandle obj_handle, uint8_t * filename); -static void customSPrintf(uint8_t * buffer, uint8_t * format, ...); +static void objectFilename(UAVObjHandle obj_handle, uint8_t *filename); +static void customSPrintf(uint8_t *buffer, uint8_t *format, ...); #endif // Private variables static xSemaphoreHandle mutex; static const UAVObjMetadata defMetadata = { - .flags = (ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT | - ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT | - 1 << UAVOBJ_TELEMETRY_ACKED_SHIFT | - 1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT | - UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT | - UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT), - .telemetryUpdatePeriod = 0, - .gcsTelemetryUpdatePeriod = 0, - .loggingUpdatePeriod = 0, + .flags = (ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT | + ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT | + 1 << UAVOBJ_TELEMETRY_ACKED_SHIFT | + 1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT | + UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT | + UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT), + .telemetryUpdatePeriod = 0, + .gcsTelemetryUpdatePeriod = 0, + .loggingUpdatePeriod = 0, }; static UAVObjStats stats; @@ -216,27 +216,28 @@ static UAVObjStats stats; */ int32_t UAVObjInitialize() { - // Initialize variables - memset(&stats, 0, sizeof(UAVObjStats)); + // Initialize variables + memset(&stats, 0, sizeof(UAVObjStats)); - /* Initialize _uavo_handles start/stop pointers */ - #if (defined(__MACH__) && defined(__APPLE__)) - uint64_t aslr_offset = (uint64_t) & _aslr_offset - getsectbyname("__DATA","_aslr")->addr; - __start__uavo_handles = (struct UAVOData **) (getsectbyname("__DATA","_uavo_handles")->addr + aslr_offset); - __stop__uavo_handles = (struct UAVOData **) ((uint64_t)__start__uavo_handles + getsectbyname("__DATA","_uavo_handles")->size); - #endif + /* Initialize _uavo_handles start/stop pointers */ + #if (defined(__MACH__) && defined(__APPLE__)) + uint64_t aslr_offset = (uint64_t)&_aslr_offset - getsectbyname("__DATA", "_aslr")->addr; + __start__uavo_handles = (struct UAVOData * *)(getsectbyname("__DATA", "_uavo_handles")->addr + aslr_offset); + __stop__uavo_handles = (struct UAVOData * *)((uint64_t)__start__uavo_handles + getsectbyname("__DATA", "_uavo_handles")->size); + #endif - // Initialize the uavo handle table - memset(__start__uavo_handles, 0, - (uintptr_t)__stop__uavo_handles - (uintptr_t)__start__uavo_handles); + // Initialize the uavo handle table + memset(__start__uavo_handles, 0, + (uintptr_t)__stop__uavo_handles - (uintptr_t)__start__uavo_handles); - // Create mutex - mutex = xSemaphoreCreateRecursiveMutex(); - if (mutex == NULL) - return -1; + // Create mutex + mutex = xSemaphoreCreateRecursiveMutex(); + if (mutex == NULL) { + return -1; + } - // Done - return 0; + // Done + return 0; } /***************** @@ -247,11 +248,11 @@ int32_t UAVObjInitialize() * Get the statistics counters * @param[out] statsOut The statistics counters will be copied there */ -void UAVObjGetStats(UAVObjStats * statsOut) +void UAVObjGetStats(UAVObjStats *statsOut) { - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - memcpy(statsOut, &stats, sizeof(UAVObjStats)); - xSemaphoreGiveRecursive(mutex); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + memcpy(statsOut, &stats, sizeof(UAVObjStats)); + xSemaphoreGiveRecursive(mutex); } /** @@ -259,75 +260,80 @@ void UAVObjGetStats(UAVObjStats * statsOut) */ void UAVObjClearStats() { - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - memset(&stats, 0, sizeof(UAVObjStats)); - xSemaphoreGiveRecursive(mutex); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + memset(&stats, 0, sizeof(UAVObjStats)); + xSemaphoreGiveRecursive(mutex); } /************************ * Object Initialization ***********************/ -static void UAVObjInitMetaData (struct UAVOMeta * obj_meta) +static void UAVObjInitMetaData(struct UAVOMeta *obj_meta) { - /* Fill in the common part of the UAVO */ - struct UAVOBase * uavo_base = &(obj_meta->base); - memset(uavo_base, 0, sizeof(*uavo_base)); - uavo_base->flags.isMeta = true; - uavo_base->flags.isSingle = true; - uavo_base->next_event = NULL; + /* Fill in the common part of the UAVO */ + struct UAVOBase *uavo_base = &(obj_meta->base); - /* Clear the instance data carried in the UAVO */ - memset(&(obj_meta->instance0), 0, sizeof(obj_meta->instance0)); + memset(uavo_base, 0, sizeof(*uavo_base)); + uavo_base->flags.isMeta = true; + uavo_base->flags.isSingle = true; + uavo_base->next_event = NULL; + + /* Clear the instance data carried in the UAVO */ + memset(&(obj_meta->instance0), 0, sizeof(obj_meta->instance0)); } -static struct UAVOData * UAVObjAllocSingle(uint32_t num_bytes) +static struct UAVOData *UAVObjAllocSingle(uint32_t num_bytes) { - /* Compute the complete size of the object, including the data for a single embedded instance */ - uint32_t object_size = sizeof(struct UAVOSingle) + num_bytes; + /* Compute the complete size of the object, including the data for a single embedded instance */ + uint32_t object_size = sizeof(struct UAVOSingle) + num_bytes; - /* Allocate the object from the heap */ - struct UAVOSingle * uavo_single = (struct UAVOSingle *) pvPortMalloc(object_size); - if (!uavo_single) - return (NULL); + /* Allocate the object from the heap */ + struct UAVOSingle *uavo_single = (struct UAVOSingle *)pvPortMalloc(object_size); - /* Fill in the common part of the UAVO */ - struct UAVOBase * uavo_base = &(uavo_single->uavo.base); - memset(uavo_base, 0, sizeof(*uavo_base)); - uavo_base->flags.isSingle = true; - uavo_base->next_event = NULL; + if (!uavo_single) { + return NULL; + } - /* Clear the instance data carried in the UAVO */ - memset(&(uavo_single->instance0), 0, num_bytes); + /* Fill in the common part of the UAVO */ + struct UAVOBase *uavo_base = &(uavo_single->uavo.base); + memset(uavo_base, 0, sizeof(*uavo_base)); + uavo_base->flags.isSingle = true; + uavo_base->next_event = NULL; - /* Give back the generic UAVO part */ - return (&(uavo_single->uavo)); + /* Clear the instance data carried in the UAVO */ + memset(&(uavo_single->instance0), 0, num_bytes); + + /* Give back the generic UAVO part */ + return &(uavo_single->uavo); } -static struct UAVOData * UAVObjAllocMulti(uint32_t num_bytes) +static struct UAVOData *UAVObjAllocMulti(uint32_t num_bytes) { - /* Compute the complete size of the object, including the data for a single embedded instance */ - uint32_t object_size = sizeof(struct UAVOMulti) + num_bytes; + /* Compute the complete size of the object, including the data for a single embedded instance */ + uint32_t object_size = sizeof(struct UAVOMulti) + num_bytes; - /* Allocate the object from the heap */ - struct UAVOMulti * uavo_multi = (struct UAVOMulti *) pvPortMalloc(object_size); - if (!uavo_multi) - return (NULL); + /* Allocate the object from the heap */ + struct UAVOMulti *uavo_multi = (struct UAVOMulti *)pvPortMalloc(object_size); - /* Fill in the common part of the UAVO */ - struct UAVOBase * uavo_base = &(uavo_multi->uavo.base); - memset(uavo_base, 0, sizeof(*uavo_base)); - uavo_base->flags.isSingle = false; - uavo_base->next_event = NULL; + if (!uavo_multi) { + return NULL; + } - /* Set up the type-specific part of the UAVO */ - uavo_multi->num_instances = 1; + /* Fill in the common part of the UAVO */ + struct UAVOBase *uavo_base = &(uavo_multi->uavo.base); + memset(uavo_base, 0, sizeof(*uavo_base)); + uavo_base->flags.isSingle = false; + uavo_base->next_event = NULL; - /* Clear the multi instance data carried in the UAVO */ - memset (&(uavo_multi->instance0), 0, sizeof(struct UAVOMultiInst) + num_bytes); + /* Set up the type-specific part of the UAVO */ + uavo_multi->num_instances = 1; - /* Give back the generic UAVO part */ - return (&(uavo_multi->uavo)); + /* Clear the multi instance data carried in the UAVO */ + memset(&(uavo_multi->instance0), 0, sizeof(struct UAVOMultiInst) + num_bytes); + + /* Give back the generic UAVO part */ + return &(uavo_multi->uavo); } /************************** @@ -344,57 +350,61 @@ static struct UAVOData * UAVObjAllocMulti(uint32_t num_bytes) * \return Object handle, or NULL if failure. * \return */ -UAVObjHandle UAVObjRegister(uint32_t id, - int32_t isSingleInstance, int32_t isSettings, - uint32_t num_bytes, - UAVObjInitializeCallback initCb) +UAVObjHandle UAVObjRegister(uint32_t id, + int32_t isSingleInstance, int32_t isSettings, + uint32_t num_bytes, + UAVObjInitializeCallback initCb) { - struct UAVOData * uavo_data = NULL; + struct UAVOData *uavo_data = NULL; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - /* Don't allow duplicate registrations */ - if (UAVObjGetByID(id)) - goto unlock_exit; + /* Don't allow duplicate registrations */ + if (UAVObjGetByID(id)) { + goto unlock_exit; + } - /* Map the various flags to one of the UAVO types we understand */ - if (isSingleInstance) { - uavo_data = UAVObjAllocSingle (num_bytes); - } else { - uavo_data = UAVObjAllocMulti (num_bytes); - } + /* Map the various flags to one of the UAVO types we understand */ + if (isSingleInstance) { + uavo_data = UAVObjAllocSingle(num_bytes); + } else { + uavo_data = UAVObjAllocMulti(num_bytes); + } - if (!uavo_data) - goto unlock_exit; + if (!uavo_data) { + goto unlock_exit; + } - /* Fill in the details about this UAVO */ - uavo_data->id = id; - uavo_data->instance_size = num_bytes; - if (isSettings) { - uavo_data->base.flags.isSettings = true; - } + /* Fill in the details about this UAVO */ + uavo_data->id = id; + uavo_data->instance_size = num_bytes; + if (isSettings) { + uavo_data->base.flags.isSettings = true; + } - /* Initialize the embedded meta UAVO */ - UAVObjInitMetaData (&uavo_data->metaObj); + /* Initialize the embedded meta UAVO */ + UAVObjInitMetaData(&uavo_data->metaObj); - /* Initialize object fields and metadata to default values */ - if (initCb) - initCb((UAVObjHandle) uavo_data, 0); + /* Initialize object fields and metadata to default values */ + if (initCb) { + initCb((UAVObjHandle)uavo_data, 0); + } - /* Always try to load the meta object from flash */ - UAVObjLoad((UAVObjHandle) &(uavo_data->metaObj), 0); + /* Always try to load the meta object from flash */ + UAVObjLoad((UAVObjHandle) & (uavo_data->metaObj), 0); - /* Attempt to load settings object from flash */ - if (uavo_data->base.flags.isSettings) - UAVObjLoad((UAVObjHandle) uavo_data, 0); + /* Attempt to load settings object from flash */ + if (uavo_data->base.flags.isSettings) { + UAVObjLoad((UAVObjHandle)uavo_data, 0); + } - // fire events for outer object and its embedded meta object - UAVObjInstanceUpdated((UAVObjHandle) uavo_data, 0); - UAVObjInstanceUpdated((UAVObjHandle) &(uavo_data->metaObj), 0); + // fire events for outer object and its embedded meta object + UAVObjInstanceUpdated((UAVObjHandle)uavo_data, 0); + UAVObjInstanceUpdated((UAVObjHandle) & (uavo_data->metaObj), 0); unlock_exit: - xSemaphoreGiveRecursive(mutex); - return (UAVObjHandle) uavo_data; + xSemaphoreGiveRecursive(mutex); + return (UAVObjHandle)uavo_data; } /** @@ -404,26 +414,26 @@ unlock_exit: */ UAVObjHandle UAVObjGetByID(uint32_t id) { - UAVObjHandle * found_obj = (UAVObjHandle *) NULL; + UAVObjHandle *found_obj = (UAVObjHandle *)NULL; - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Look for object - UAVO_LIST_ITERATE(tmp_obj) - if (tmp_obj->id == id) { - found_obj = (UAVObjHandle *)tmp_obj; - goto unlock_exit; - } - if (MetaObjectId(tmp_obj->id) == id) { - found_obj = (UAVObjHandle *)&(tmp_obj->metaObj); - goto unlock_exit; - } - } + // Look for object + UAVO_LIST_ITERATE(tmp_obj) + if (tmp_obj->id == id) { + found_obj = (UAVObjHandle *)tmp_obj; + goto unlock_exit; + } + if (MetaObjectId(tmp_obj->id) == id) { + found_obj = (UAVObjHandle *)&(tmp_obj->metaObj); + goto unlock_exit; + } +} unlock_exit: - xSemaphoreGiveRecursive(mutex); - return found_obj; +xSemaphoreGiveRecursive(mutex); +return found_obj; } /** @@ -433,22 +443,22 @@ unlock_exit: */ uint32_t UAVObjGetID(UAVObjHandle obj_handle) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - /* Recover the common object header */ - struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle; + /* Recover the common object header */ + struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle; - if (UAVObjIsMetaobject(obj_handle)) { - /* We have a meta object, find our containing UAVO */ - struct UAVOData * uavo_data = container_of ((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj); + if (UAVObjIsMetaobject(obj_handle)) { + /* We have a meta object, find our containing UAVO */ + struct UAVOData *uavo_data = container_of((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj); - return MetaObjectId (uavo_data->id); - } else { - /* We have a data object, augment our pointer */ - struct UAVOData * uavo_data = (struct UAVOData *) uavo_base; + return MetaObjectId(uavo_data->id); + } else { + /* We have a data object, augment our pointer */ + struct UAVOData *uavo_data = (struct UAVOData *)uavo_base; - return (uavo_data->id); - } + return uavo_data->id; + } } /** @@ -458,23 +468,23 @@ uint32_t UAVObjGetID(UAVObjHandle obj_handle) */ uint32_t UAVObjGetNumBytes(UAVObjHandle obj) { - PIOS_Assert(obj); + PIOS_Assert(obj); - uint32_t instance_size; + uint32_t instance_size; - /* Recover the common object header */ - struct UAVOBase * uavo_base = (struct UAVOBase *) obj; + /* Recover the common object header */ + struct UAVOBase *uavo_base = (struct UAVOBase *)obj; - if (uavo_base->flags.isMeta) { - instance_size = MetaNumBytes; - } else { - /* We have a data object, augment our pointer */ - struct UAVOData * uavo = (struct UAVOData *) uavo_base; + if (uavo_base->flags.isMeta) { + instance_size = MetaNumBytes; + } else { + /* We have a data object, augment our pointer */ + struct UAVOData *uavo = (struct UAVOData *)uavo_base; - instance_size = uavo->instance_size; - } + instance_size = uavo->instance_size; + } - return (instance_size); + return instance_size; } /** @@ -486,22 +496,22 @@ uint32_t UAVObjGetNumBytes(UAVObjHandle obj) */ UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj_handle) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - /* Recover the common object header */ - struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle; + /* Recover the common object header */ + struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle; - if (UAVObjIsMetaobject(obj_handle)) { - /* We have a meta object, find our containing UAVO. */ - struct UAVOData * uavo_data = container_of ((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj); + if (UAVObjIsMetaobject(obj_handle)) { + /* We have a meta object, find our containing UAVO. */ + struct UAVOData *uavo_data = container_of((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj); - return (UAVObjHandle) uavo_data; - } else { - /* We have a data object, augment our pointer */ - struct UAVOData * uavo_data = (struct UAVOData *) uavo_base; + return (UAVObjHandle)uavo_data; + } else { + /* We have a data object, augment our pointer */ + struct UAVOData *uavo_data = (struct UAVOData *)uavo_base; - return (UAVObjHandle) &(uavo_data->metaObj); - } + return (UAVObjHandle) & (uavo_data->metaObj); + } } /** @@ -511,17 +521,17 @@ UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj_handle) */ uint16_t UAVObjGetNumInstances(UAVObjHandle obj_handle) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - if (UAVObjIsSingleInstance(obj_handle)) { - /* Only one instance is allowed */ - return 1; - } else { - /* Multi-instance object. Inspect the object */ - /* Augment our pointer to reflect the proper type */ - struct UAVOMulti * uavo_multi = (struct UAVOMulti *) obj_handle; - return uavo_multi->num_instances; - } + if (UAVObjIsSingleInstance(obj_handle)) { + /* Only one instance is allowed */ + return 1; + } else { + /* Multi-instance object. Inspect the object */ + /* Augment our pointer to reflect the proper type */ + struct UAVOMulti *uavo_multi = (struct UAVOMulti *)obj_handle; + return uavo_multi->num_instances; + } } /** @@ -530,36 +540,36 @@ uint16_t UAVObjGetNumInstances(UAVObjHandle obj_handle) * \return The instance ID or 0 if an error */ uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle, - UAVObjInitializeCallback initCb) + UAVObjInitializeCallback initCb) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - if (UAVObjIsMetaobject(obj_handle)) { - return 0; - } + if (UAVObjIsMetaobject(obj_handle)) { + return 0; + } - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - InstanceHandle instEntry; - uint16_t instId = 0; + InstanceHandle instEntry; + uint16_t instId = 0; - // Create new instance - instId = UAVObjGetNumInstances(obj_handle); - instEntry = createInstance( (struct UAVOData *)obj_handle, instId); - if (instEntry == NULL) { - goto unlock_exit; - } + // Create new instance + instId = UAVObjGetNumInstances(obj_handle); + instEntry = createInstance((struct UAVOData *)obj_handle, instId); + if (instEntry == NULL) { + goto unlock_exit; + } - // Initialize instance data - if (initCb) { - initCb(obj_handle, instId); - } + // Initialize instance data + if (initCb) { + initCb(obj_handle, instId); + } unlock_exit: - xSemaphoreGiveRecursive(mutex); + xSemaphoreGiveRecursive(mutex); - return instId; + return instId; } /** @@ -569,12 +579,12 @@ unlock_exit: */ bool UAVObjIsSingleInstance(UAVObjHandle obj_handle) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - /* Recover the common object header */ - struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle; + /* Recover the common object header */ + struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle; - return uavo_base->flags.isSingle; + return uavo_base->flags.isSingle; } /** @@ -584,12 +594,12 @@ bool UAVObjIsSingleInstance(UAVObjHandle obj_handle) */ bool UAVObjIsMetaobject(UAVObjHandle obj_handle) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - /* Recover the common object header */ - struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle; + /* Recover the common object header */ + struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle; - return uavo_base->flags.isMeta; + return uavo_base->flags.isMeta; } /** @@ -599,12 +609,12 @@ bool UAVObjIsMetaobject(UAVObjHandle obj_handle) */ bool UAVObjIsSettings(UAVObjHandle obj_handle) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - /* Recover the common object header */ - struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle; + /* Recover the common object header */ + struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle; - return uavo_base->flags.isSettings; + return uavo_base->flags.isSettings; } /** @@ -615,48 +625,48 @@ bool UAVObjIsSettings(UAVObjHandle obj_handle) * \return 0 if success or -1 if failure */ int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, - const uint8_t * dataIn) + const uint8_t *dataIn) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - if (UAVObjIsMetaobject(obj_handle)) { - if (instId != 0) { - goto unlock_exit; - } - memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes); - } else { - struct UAVOData *obj; - InstanceHandle instEntry; + if (UAVObjIsMetaobject(obj_handle)) { + if (instId != 0) { + goto unlock_exit; + } + memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes); + } else { + struct UAVOData *obj; + InstanceHandle instEntry; - // Cast handle to object - obj = (struct UAVOData *) obj_handle; + // Cast handle to object + obj = (struct UAVOData *)obj_handle; - // Get the instance - instEntry = getInstance(obj, instId); + // Get the instance + instEntry = getInstance(obj, instId); - // If the instance does not exist create it and any other instances before it - if (instEntry == NULL) { - instEntry = createInstance(obj, instId); - if (instEntry == NULL) { - goto unlock_exit; - } - } - // Set the data - memcpy(InstanceData(instEntry), dataIn, obj->instance_size); - } + // If the instance does not exist create it and any other instances before it + if (instEntry == NULL) { + instEntry = createInstance(obj, instId); + if (instEntry == NULL) { + goto unlock_exit; + } + } + // Set the data + memcpy(InstanceData(instEntry), dataIn, obj->instance_size); + } - // Fire event - sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED); - rc = 0; + // Fire event + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UNPACKED); + rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; + xSemaphoreGiveRecursive(mutex); + return rc; } /** @@ -666,41 +676,41 @@ unlock_exit: * \param[out] dataOut The byte array * \return 0 if success or -1 if failure */ -int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t * dataOut) +int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t *dataOut) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - if (UAVObjIsMetaobject(obj_handle)) { - if (instId != 0) { - goto unlock_exit; - } - memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes); - } else { - struct UAVOData *obj; - InstanceHandle instEntry; + if (UAVObjIsMetaobject(obj_handle)) { + if (instId != 0) { + goto unlock_exit; + } + memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes); + } else { + struct UAVOData *obj; + InstanceHandle instEntry; - // Cast handle to object - obj = (struct UAVOData *) obj_handle; + // Cast handle to object + obj = (struct UAVOData *)obj_handle; - // Get the instance - instEntry = getInstance(obj, instId); - if (instEntry == NULL) { - goto unlock_exit; - } - // Pack data - memcpy(dataOut, InstanceData(instEntry), obj->instance_size); - } + // Get the instance + instEntry = getInstance(obj, instId); + if (instEntry == NULL) { + goto unlock_exit; + } + // Pack data + memcpy(dataOut, InstanceData(instEntry), obj->instance_size); + } - rc = 0; + rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; + xSemaphoreGiveRecursive(mutex); + return rc; } #if defined(PIOS_USE_SETTINGS_ON_SDCARD) @@ -714,69 +724,69 @@ unlock_exit: * @return 0 if success or -1 if failure */ int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId, - FILEINFO * file) + FILEINFO *file) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - uint32_t bytesWritten; - // Check for file system availability - if (PIOS_SDCARD_IsMounted() == 0) { - return -1; - } - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + uint32_t bytesWritten; + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return -1; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - if (UAVObjIsMetaobject(obj_handle)) { - // Get the instance information - if (instId != 0) { - xSemaphoreGiveRecursive(mutex); - return -1; - } - // Write the object ID - uint32_t objId = UAVObjGetID(obj_handle); - PIOS_FWRITE(file, &objId, sizeof(objId), - &bytesWritten); + if (UAVObjIsMetaobject(obj_handle)) { + // Get the instance information + if (instId != 0) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Write the object ID + uint32_t objId = UAVObjGetID(obj_handle); + PIOS_FWRITE(file, &objId, sizeof(objId), + &bytesWritten); - // Write the data and check that the write was successful - PIOS_FWRITE(file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes, - &bytesWritten); - if (bytesWritten != MetaNumBytes) { - xSemaphoreGiveRecursive(mutex); - return -1; - } - } else { - struct UAVOData * uavo; - InstanceHandle instEntry; + // Write the data and check that the write was successful + PIOS_FWRITE(file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes, + &bytesWritten); + if (bytesWritten != MetaNumBytes) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + } else { + struct UAVOData *uavo; + InstanceHandle instEntry; - // Cast to object - uavo = (struct UAVOData *) obj_handle; + // Cast to object + uavo = (struct UAVOData *)obj_handle; - // Get the instance information - instEntry = getInstance(uavo, instId); - if (instEntry == NULL) { - xSemaphoreGiveRecursive(mutex); - return -1; - } - // Write the object ID - PIOS_FWRITE(file, &uavo->id, sizeof(uavo->id), - &bytesWritten); + // Get the instance information + instEntry = getInstance(uavo, instId); + if (instEntry == NULL) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Write the object ID + PIOS_FWRITE(file, &uavo->id, sizeof(uavo->id), + &bytesWritten); - // Write the instance ID - if (!UAVObjIsSingleInstance(obj_handle)) { - PIOS_FWRITE(file, &instId, - sizeof(instId), &bytesWritten); - } - // Write the data and check that the write was successful - PIOS_FWRITE(file, InstanceData(instEntry), uavo->instance_size, - &bytesWritten); - if (bytesWritten != uavo->instance_size) { - xSemaphoreGiveRecursive(mutex); - return -1; - } - } - // Done - xSemaphoreGiveRecursive(mutex); - return 0; + // Write the instance ID + if (!UAVObjIsSingleInstance(obj_handle)) { + PIOS_FWRITE(file, &instId, + sizeof(instId), &bytesWritten); + } + // Write the data and check that the write was successful + PIOS_FWRITE(file, InstanceData(instEntry), uavo->instance_size, + &bytesWritten); + if (bytesWritten != uavo->instance_size) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + // Done + xSemaphoreGiveRecursive(mutex); + return 0; } #endif /* PIOS_USE_SETTINGS_ON_SDCARD */ @@ -792,58 +802,63 @@ int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId, */ int32_t UAVObjSave(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t instId) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); #if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) - if (UAVObjIsMetaobject(obj_handle)) { - if (instId != 0) - return -1; + if (UAVObjIsMetaobject(obj_handle)) { + if (instId != 0) { + return -1; + } - if (PIOS_FLASHFS_ObjSave(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId, (uint8_t*) MetaDataPtr((struct UAVOMeta *)obj_handle), UAVObjGetNumBytes(obj_handle)) != 0) - return -1; - } else { - InstanceHandle instEntry = getInstance( (struct UAVOData *)obj_handle, instId); + if (PIOS_FLASHFS_ObjSave(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId, (uint8_t *)MetaDataPtr((struct UAVOMeta *)obj_handle), UAVObjGetNumBytes(obj_handle)) != 0) { + return -1; + } + } else { + InstanceHandle instEntry = getInstance((struct UAVOData *)obj_handle, instId); - if (instEntry == NULL) - return -1; + if (instEntry == NULL) { + return -1; + } - if (InstanceData(instEntry) == NULL) - return -1; + if (InstanceData(instEntry) == NULL) { + return -1; + } - if (PIOS_FLASHFS_ObjSave(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId, InstanceData(instEntry), UAVObjGetNumBytes(obj_handle)) != 0) - return -1; - } -#endif + if (PIOS_FLASHFS_ObjSave(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId, InstanceData(instEntry), UAVObjGetNumBytes(obj_handle)) != 0) { + return -1; + } + } +#endif /* if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) */ #if defined(PIOS_USE_SETTINGS_ON_SDCARD) - FILEINFO file; - uint8_t filename[14]; + FILEINFO file; + uint8_t filename[14]; - // Check for file system availability - if (PIOS_SDCARD_IsMounted() == 0) { - return -1; - } - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return -1; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Get filename - objectFilename(obj_handle, filename); + // Get filename + objectFilename(obj_handle, filename); - // Open file - if (PIOS_FOPEN_WRITE(filename, file)) { - xSemaphoreGiveRecursive(mutex); - return -1; - } - // Append object - if (UAVObjSaveToFile(obj_handle, instId, &file) == -1) { - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); - return -1; - } - // Done, close file and unlock - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); + // Open file + if (PIOS_FOPEN_WRITE(filename, file)) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Append object + if (UAVObjSaveToFile(obj_handle, instId, &file) == -1) { + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Done, close file and unlock + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); #endif /* PIOS_USE_SETTINGS_ON_SDCARD */ - return 0; + return 0; } #if defined(PIOS_USE_SETTINGS_ON_SDCARD) @@ -852,87 +867,85 @@ int32_t UAVObjSave(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t ins * @param[in] file File to read from * @return The handle of the object loaded or NULL if a failure */ -UAVObjHandle UAVObjLoadFromFile(FILEINFO * file) +UAVObjHandle UAVObjLoadFromFile(FILEINFO *file) { - uint32_t bytesRead; - struct UAVOBase *objEntry; - InstanceHandle instEntry; - uint32_t objId; - uint16_t instId; - UAVObjHandle obj_handle; + uint32_t bytesRead; + struct UAVOBase *objEntry; + InstanceHandle instEntry; + uint32_t objId; + uint16_t instId; + UAVObjHandle obj_handle; - // Check for file system availability - if (PIOS_SDCARD_IsMounted() == 0) { - return NULL; - } - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return NULL; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Read the object ID - if (PIOS_FREAD(file, &objId, sizeof(objId), &bytesRead)) { - xSemaphoreGiveRecursive(mutex); - return NULL; - } - // Get the object - obj_handle = UAVObjGetByID(objId); - if (obj_handle == 0) { - xSemaphoreGiveRecursive(mutex); - return NULL; - } - objEntry = (struct UAVOBase *) obj_handle; + // Read the object ID + if (PIOS_FREAD(file, &objId, sizeof(objId), &bytesRead)) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + // Get the object + obj_handle = UAVObjGetByID(objId); + if (obj_handle == 0) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + objEntry = (struct UAVOBase *)obj_handle; - // Get the instance ID - instId = 0; - if (!UAVObjIsSingleInstance(obj_handle)) { - if (PIOS_FREAD - (file, &instId, sizeof(instId), &bytesRead)) { - xSemaphoreGiveRecursive(mutex); - return NULL; - } - } + // Get the instance ID + instId = 0; + if (!UAVObjIsSingleInstance(obj_handle)) { + if (PIOS_FREAD + (file, &instId, sizeof(instId), &bytesRead)) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + } - if (UAVObjIsMetaobject(obj_handle)) { - // If the instance does not exist create it and any other instances before it - if (instId != 0) { - // Error, unlock and return - xSemaphoreGiveRecursive(mutex); - return NULL; - } - // Read the instance data - if (PIOS_FREAD - (file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes, &bytesRead)) { - xSemaphoreGiveRecursive(mutex); - return NULL; - } - } else { + if (UAVObjIsMetaobject(obj_handle)) { + // If the instance does not exist create it and any other instances before it + if (instId != 0) { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return NULL; + } + // Read the instance data + if (PIOS_FREAD + (file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes, &bytesRead)) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + } else { + // Get the instance information + instEntry = getInstance((struct UAVOData *)objEntry, instId); - // Get the instance information - instEntry = getInstance((struct UAVOData *)objEntry, instId); + // If the instance does not exist create it and any other instances before it + if (instEntry == NULL) { + instEntry = createInstance((struct UAVOData *)objEntry, instId); + if (instEntry == NULL) { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return NULL; + } + } + // Read the instance data + if (PIOS_FREAD + (file, InstanceData(instEntry), ((struct UAVOData *)objEntry)->instance_size, &bytesRead)) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + } - // If the instance does not exist create it and any other instances before it - if (instEntry == NULL) { - instEntry = createInstance((struct UAVOData *)objEntry, instId); - if (instEntry == NULL) { - // Error, unlock and return - xSemaphoreGiveRecursive(mutex); - return NULL; - } - } - // Read the instance data - if (PIOS_FREAD - (file, InstanceData(instEntry), ((struct UAVOData *)objEntry)->instance_size, &bytesRead)) { - xSemaphoreGiveRecursive(mutex); - return NULL; - } + // Fire event + sendEvent(objEntry, instId, EV_UNPACKED); - } - - // Fire event - sendEvent(objEntry, instId, EV_UNPACKED); - - // Unlock - xSemaphoreGiveRecursive(mutex); - return obj_handle; + // Unlock + xSemaphoreGiveRecursive(mutex); + return obj_handle; } #endif /* PIOS_USE_SETTINGS_ON_SDCARD */ @@ -946,72 +959,75 @@ UAVObjHandle UAVObjLoadFromFile(FILEINFO * file) */ int32_t UAVObjLoad(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t instId) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); #if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) - if (UAVObjIsMetaobject(obj_handle)) { - if (instId != 0) - return -1; + if (UAVObjIsMetaobject(obj_handle)) { + if (instId != 0) { + return -1; + } - // Fire event on success - if (PIOS_FLASHFS_ObjLoad(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId, (uint8_t*) MetaDataPtr((struct UAVOMeta *)obj_handle), UAVObjGetNumBytes(obj_handle)) == 0) - sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED); - else - return -1; - } else { + // Fire event on success + if (PIOS_FLASHFS_ObjLoad(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId, (uint8_t *)MetaDataPtr((struct UAVOMeta *)obj_handle), UAVObjGetNumBytes(obj_handle)) == 0) { + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UNPACKED); + } else { + return -1; + } + } else { + InstanceHandle instEntry = getInstance((struct UAVOData *)obj_handle, instId); - InstanceHandle instEntry = getInstance( (struct UAVOData *)obj_handle, instId); + if (instEntry == NULL) { + return -1; + } - if (instEntry == NULL) - return -1; + // Fire event on success + if (PIOS_FLASHFS_ObjLoad(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId, InstanceData(instEntry), UAVObjGetNumBytes(obj_handle)) == 0) { + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UNPACKED); + } else { + return -1; + } + } - // Fire event on success - if (PIOS_FLASHFS_ObjLoad(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId, InstanceData(instEntry), UAVObjGetNumBytes(obj_handle)) == 0) - sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED); - else - return -1; - } - -#endif +#endif /* if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) */ #if defined(PIOS_USE_SETTINGS_ON_SDCARD) - FILEINFO file; - UAVObjHandle loadedObj; - uint8_t filename[14]; + FILEINFO file; + UAVObjHandle loadedObj; + uint8_t filename[14]; - // Check for file system availability - if (PIOS_SDCARD_IsMounted() == 0) { - return -1; - } - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return -1; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Get filename - objectFilename(obj_handle, filename); + // Get filename + objectFilename(obj_handle, filename); - // Open file - if (PIOS_FOPEN_READ(filename, file)) { - xSemaphoreGiveRecursive(mutex); - return -1; - } - // Load object - loadedObj = UAVObjLoadFromFile(&file); - if (loadedObj == 0) { - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); - return -1; - } - // Check that the IDs match - if (UAVObjGetID(loadedObj) != UAVObjGetID(obj_handle)) { - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); - return -1; - } - // Done, close file and unlock - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); + // Open file + if (PIOS_FOPEN_READ(filename, file)) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Load object + loadedObj = UAVObjLoadFromFile(&file); + if (loadedObj == 0) { + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Check that the IDs match + if (UAVObjGetID(loadedObj) != UAVObjGetID(obj_handle)) { + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Done, close file and unlock + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); #endif /* PIOS_USE_SETTINGS_ON_SDCARD */ - return 0; + return 0; } /** @@ -1022,30 +1038,30 @@ int32_t UAVObjLoad(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t ins */ int32_t UAVObjDelete(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t instId) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); #if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) - PIOS_FLASHFS_ObjDelete(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId); + PIOS_FLASHFS_ObjDelete(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId); #endif #if defined(PIOS_USE_SETTINGS_ON_SDCARD) - uint8_t filename[14]; + uint8_t filename[14]; - // Check for file system availability - if (PIOS_SDCARD_IsMounted() == 0) { - return -1; - } - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return -1; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Get filename - objectFilename(obj_handle, filename); + // Get filename + objectFilename(obj_handle, filename); - // Delete file - PIOS_FUNLINK(filename); + // Delete file + PIOS_FUNLINK(filename); - // Done - xSemaphoreGiveRecursive(mutex); + // Done + xSemaphoreGiveRecursive(mutex); #endif /* PIOS_USE_SETTINGS_ON_SDCARD */ - return 0; + return 0; } /** @@ -1054,28 +1070,28 @@ int32_t UAVObjDelete(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t i */ int32_t UAVObjSaveSettings() { - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - // Save all settings objects - UAVO_LIST_ITERATE(obj) - // Check if this is a settings object - if (UAVObjIsSettings(obj)) { - // Save object - if (UAVObjSave((UAVObjHandle) obj, 0) == - -1) { - goto unlock_exit; - } - } - } + // Save all settings objects + UAVO_LIST_ITERATE(obj) + // Check if this is a settings object + if (UAVObjIsSettings(obj)) { + // Save object + if (UAVObjSave((UAVObjHandle)obj, 0) == + -1) { + goto unlock_exit; + } + } +} - rc = 0; +rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; +xSemaphoreGiveRecursive(mutex); +return rc; } /** @@ -1084,28 +1100,28 @@ unlock_exit: */ int32_t UAVObjLoadSettings() { - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - // Load all settings objects - UAVO_LIST_ITERATE(obj) - // Check if this is a settings object - if (UAVObjIsSettings(obj)) { - // Load object - if (UAVObjLoad((UAVObjHandle) obj, 0) == - -1) { - goto unlock_exit; - } - } - } + // Load all settings objects + UAVO_LIST_ITERATE(obj) + // Check if this is a settings object + if (UAVObjIsSettings(obj)) { + // Load object + if (UAVObjLoad((UAVObjHandle)obj, 0) == + -1) { + goto unlock_exit; + } + } +} - rc = 0; +rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; +xSemaphoreGiveRecursive(mutex); +return rc; } /** @@ -1114,28 +1130,28 @@ unlock_exit: */ int32_t UAVObjDeleteSettings() { - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - // Save all settings objects - UAVO_LIST_ITERATE(obj) - // Check if this is a settings object - if (UAVObjIsSettings(obj)) { - // Save object - if (UAVObjDelete((UAVObjHandle) obj, 0) - == -1) { - goto unlock_exit; - } - } - } + // Save all settings objects + UAVO_LIST_ITERATE(obj) + // Check if this is a settings object + if (UAVObjIsSettings(obj)) { + // Save object + if (UAVObjDelete((UAVObjHandle)obj, 0) + == -1) { + goto unlock_exit; + } + } +} - rc = 0; +rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; +xSemaphoreGiveRecursive(mutex); +return rc; } /** @@ -1144,25 +1160,25 @@ unlock_exit: */ int32_t UAVObjSaveMetaobjects() { - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - // Save all settings objects - UAVO_LIST_ITERATE(obj) - // Save object - if (UAVObjSave( (UAVObjHandle) MetaObjectPtr(obj), 0) == - -1) { - goto unlock_exit; - } - } + // Save all settings objects + UAVO_LIST_ITERATE(obj) + // Save object + if (UAVObjSave((UAVObjHandle)MetaObjectPtr(obj), 0) == + -1) { + goto unlock_exit; + } +} - rc = 0; +rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; +xSemaphoreGiveRecursive(mutex); +return rc; } /** @@ -1171,25 +1187,25 @@ unlock_exit: */ int32_t UAVObjLoadMetaobjects() { - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - // Load all settings objects - UAVO_LIST_ITERATE(obj) - // Load object - if (UAVObjLoad((UAVObjHandle) MetaObjectPtr(obj), 0) == - -1) { - goto unlock_exit; - } - } + // Load all settings objects + UAVO_LIST_ITERATE(obj) + // Load object + if (UAVObjLoad((UAVObjHandle)MetaObjectPtr(obj), 0) == + -1) { + goto unlock_exit; + } +} - rc = 0; +rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; +xSemaphoreGiveRecursive(mutex); +return rc; } /** @@ -1198,25 +1214,25 @@ unlock_exit: */ int32_t UAVObjDeleteMetaobjects() { - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - // Load all settings objects - UAVO_LIST_ITERATE(obj) - // Load object - if (UAVObjDelete((UAVObjHandle) MetaObjectPtr(obj), 0) - == -1) { - goto unlock_exit; - } - } + // Load all settings objects + UAVO_LIST_ITERATE(obj) + // Load object + if (UAVObjDelete((UAVObjHandle)MetaObjectPtr(obj), 0) + == -1) { + goto unlock_exit; + } +} - rc = 0; +rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; +xSemaphoreGiveRecursive(mutex); +return rc; } /** @@ -1227,7 +1243,7 @@ unlock_exit: */ int32_t UAVObjSetData(UAVObjHandle obj_handle, const void *dataIn) { - return UAVObjSetInstanceData(obj_handle, 0, dataIn); + return UAVObjSetInstanceData(obj_handle, 0, dataIn); } /** @@ -1236,9 +1252,9 @@ int32_t UAVObjSetData(UAVObjHandle obj_handle, const void *dataIn) * \param[in] dataIn The object's data structure * \return 0 if success or -1 if failure */ -int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void* dataIn, uint32_t offset, uint32_t size) +int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void *dataIn, uint32_t offset, uint32_t size) { - return UAVObjSetInstanceDataField(obj_handle, 0, dataIn, offset, size); + return UAVObjSetInstanceDataField(obj_handle, 0, dataIn, offset, size); } /** @@ -1249,7 +1265,7 @@ int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void* dataIn, uint32_t */ int32_t UAVObjGetData(UAVObjHandle obj_handle, void *dataOut) { - return UAVObjGetInstanceData(obj_handle, 0, dataOut); + return UAVObjGetInstanceData(obj_handle, 0, dataOut); } /** @@ -1258,9 +1274,9 @@ int32_t UAVObjGetData(UAVObjHandle obj_handle, void *dataOut) * \param[out] dataOut The object's data structure * \return 0 if success or -1 if failure */ -int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void* dataOut, uint32_t offset, uint32_t size) +int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void *dataOut, uint32_t offset, uint32_t size) { - return UAVObjGetInstanceDataField(obj_handle, 0, dataOut, offset, size); + return UAVObjGetInstanceDataField(obj_handle, 0, dataOut, offset, size); } /** @@ -1271,47 +1287,47 @@ int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void* dataOut, uint32_t offs * \return 0 if success or -1 if failure */ int32_t UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId, - const void *dataIn) + const void *dataIn) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - if (UAVObjIsMetaobject(obj_handle)) { - if (instId != 0) { - goto unlock_exit; - } - memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes); - } else { - struct UAVOData *obj; - InstanceHandle instEntry; + if (UAVObjIsMetaobject(obj_handle)) { + if (instId != 0) { + goto unlock_exit; + } + memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes); + } else { + struct UAVOData *obj; + InstanceHandle instEntry; - // Cast to object info - obj = (struct UAVOData *) obj_handle; + // Cast to object info + obj = (struct UAVOData *)obj_handle; - // Check access level - if (UAVObjReadOnly(obj_handle)) { - goto unlock_exit; - } - // Get instance information - instEntry = getInstance(obj, instId); - if (instEntry == NULL) { - goto unlock_exit; - } - // Set data - memcpy(InstanceData(instEntry), dataIn, obj->instance_size); - } + // Check access level + if (UAVObjReadOnly(obj_handle)) { + goto unlock_exit; + } + // Get instance information + instEntry = getInstance(obj, instId); + if (instEntry == NULL) { + goto unlock_exit; + } + // Set data + memcpy(InstanceData(instEntry), dataIn, obj->instance_size); + } - // Fire event - sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED); - rc = 0; + // Fire event + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED); + rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; + xSemaphoreGiveRecursive(mutex); + return rc; } /** @@ -1321,63 +1337,63 @@ unlock_exit: * \param[in] dataIn The object's data structure * \return 0 if success or -1 if failure */ -int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size) +int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void *dataIn, uint32_t offset, uint32_t size) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - if (UAVObjIsMetaobject(obj_handle)) { - // Get instance information - if (instId != 0) { - goto unlock_exit; - } + if (UAVObjIsMetaobject(obj_handle)) { + // Get instance information + if (instId != 0) { + goto unlock_exit; + } - // Check for overrun - if ((size + offset) > MetaNumBytes) { - goto unlock_exit; - } + // Check for overrun + if ((size + offset) > MetaNumBytes) { + goto unlock_exit; + } - // Set data - memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, dataIn, size); - } else { - struct UAVOData * obj; - InstanceHandle instEntry; + // Set data + memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, dataIn, size); + } else { + struct UAVOData *obj; + InstanceHandle instEntry; - // Cast to object info - obj = (struct UAVOData *)obj_handle; + // Cast to object info + obj = (struct UAVOData *)obj_handle; - // Check access level - if (UAVObjReadOnly(obj_handle)) { - goto unlock_exit; - } + // Check access level + if (UAVObjReadOnly(obj_handle)) { + goto unlock_exit; + } - // Get instance information - instEntry = getInstance(obj, instId); - if (instEntry == NULL) { - goto unlock_exit; - } + // Get instance information + instEntry = getInstance(obj, instId); + if (instEntry == NULL) { + goto unlock_exit; + } - // Check for overrun - if ((size + offset) > obj->instance_size) { - goto unlock_exit; - } + // Check for overrun + if ((size + offset) > obj->instance_size) { + goto unlock_exit; + } - // Set data - memcpy(InstanceData(instEntry) + offset, dataIn, size); - } + // Set data + memcpy(InstanceData(instEntry) + offset, dataIn, size); + } - // Fire event - sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED); - rc = 0; + // Fire event + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED); + rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; + xSemaphoreGiveRecursive(mutex); + return rc; } /** @@ -1388,43 +1404,43 @@ unlock_exit: * \return 0 if success or -1 if failure */ int32_t UAVObjGetInstanceData(UAVObjHandle obj_handle, uint16_t instId, - void *dataOut) + void *dataOut) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - if (UAVObjIsMetaobject(obj_handle)) { - // Get instance information - if (instId != 0) { - goto unlock_exit; - } - // Set data - memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes); - } else { - struct UAVOData *obj; - InstanceHandle instEntry; + if (UAVObjIsMetaobject(obj_handle)) { + // Get instance information + if (instId != 0) { + goto unlock_exit; + } + // Set data + memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes); + } else { + struct UAVOData *obj; + InstanceHandle instEntry; - // Cast to object info - obj = (struct UAVOData *) obj_handle; + // Cast to object info + obj = (struct UAVOData *)obj_handle; - // Get instance information - instEntry = getInstance(obj, instId); - if (instEntry == NULL) { - goto unlock_exit; - } - // Set data - memcpy(dataOut, InstanceData(instEntry), obj->instance_size); - } + // Get instance information + instEntry = getInstance(obj, instId); + if (instEntry == NULL) { + goto unlock_exit; + } + // Set data + memcpy(dataOut, InstanceData(instEntry), obj->instance_size); + } - rc = 0; + rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; + xSemaphoreGiveRecursive(mutex); + return rc; } /** @@ -1434,55 +1450,55 @@ unlock_exit: * \param[out] dataOut The object's data structure * \return 0 if success or -1 if failure */ -int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size) +int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, void *dataOut, uint32_t offset, uint32_t size) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - int32_t rc = -1; + int32_t rc = -1; - if (UAVObjIsMetaobject(obj_handle)) { - // Get instance information - if (instId != 0) { - goto unlock_exit; - } + if (UAVObjIsMetaobject(obj_handle)) { + // Get instance information + if (instId != 0) { + goto unlock_exit; + } - // Check for overrun - if ((size + offset) > MetaNumBytes) { - goto unlock_exit; - } + // Check for overrun + if ((size + offset) > MetaNumBytes) { + goto unlock_exit; + } - // Set data - memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, size); - } else { - struct UAVOData * obj; - InstanceHandle instEntry; + // Set data + memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, size); + } else { + struct UAVOData *obj; + InstanceHandle instEntry; - // Cast to object info - obj = (struct UAVOData *)obj_handle; + // Cast to object info + obj = (struct UAVOData *)obj_handle; - // Get instance information - instEntry = getInstance(obj, instId); - if (instEntry == NULL) { - goto unlock_exit; - } + // Get instance information + instEntry = getInstance(obj, instId); + if (instEntry == NULL) { + goto unlock_exit; + } - // Check for overrun - if ((size + offset) > obj->instance_size) { - goto unlock_exit; - } - - // Set data - memcpy(dataOut, InstanceData(instEntry) + offset, size); - } + // Check for overrun + if ((size + offset) > obj->instance_size) { + goto unlock_exit; + } - rc = 0; + // Set data + memcpy(dataOut, InstanceData(instEntry) + offset, size); + } + + rc = 0; unlock_exit: - xSemaphoreGiveRecursive(mutex); - return rc; + xSemaphoreGiveRecursive(mutex); + return rc; } /** @@ -1491,21 +1507,21 @@ unlock_exit: * \param[in] dataIn The object's metadata structure * \return 0 if success or -1 if failure */ -int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata * dataIn) +int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata *dataIn) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Set metadata (metadata of metaobjects can not be modified) - if (UAVObjIsMetaobject(obj_handle)) { - return -1; - } + // Set metadata (metadata of metaobjects can not be modified) + if (UAVObjIsMetaobject(obj_handle)) { + return -1; + } - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - UAVObjSetData((UAVObjHandle) MetaObjectPtr((struct UAVOData *)obj_handle), dataIn); + UAVObjSetData((UAVObjHandle)MetaObjectPtr((struct UAVOData *)obj_handle), dataIn); - xSemaphoreGiveRecursive(mutex); - return 0; + xSemaphoreGiveRecursive(mutex); + return 0; } /** @@ -1514,24 +1530,24 @@ int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata * dataIn * \param[out] dataOut The object's metadata structure * \return 0 if success or -1 if failure */ -int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata * dataOut) +int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata *dataOut) { - PIOS_Assert(obj_handle); + PIOS_Assert(obj_handle); - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Get metadata - if (UAVObjIsMetaobject(obj_handle)) { - memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata)); - } else { - UAVObjGetData((UAVObjHandle) MetaObjectPtr( (struct UAVOData *)obj_handle ), - dataOut); - } + // Get metadata + if (UAVObjIsMetaobject(obj_handle)) { + memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata)); + } else { + UAVObjGetData((UAVObjHandle)MetaObjectPtr((struct UAVOData *)obj_handle), + dataOut); + } - // Unlock - xSemaphoreGiveRecursive(mutex); - return 0; + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; } /******************************* @@ -1543,10 +1559,10 @@ int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata * dataOut) * \param[in] metadata The metadata object * \return the access type */ -UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* metadata) +UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata *metadata) { - PIOS_Assert(metadata); - return (metadata->flags >> UAVOBJ_ACCESS_SHIFT) & 1; + PIOS_Assert(metadata); + return (metadata->flags >> UAVOBJ_ACCESS_SHIFT) & 1; } /** @@ -1554,10 +1570,10 @@ UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* metadata) * \param[in] metadata The metadata object * \param[in] mode The access mode */ -void UAVObjSetAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) +void UAVObjSetAccess(UAVObjMetadata *metadata, UAVObjAccessType mode) { - PIOS_Assert(metadata); - SET_BITS(metadata->flags, UAVOBJ_ACCESS_SHIFT, mode, 1); + PIOS_Assert(metadata); + SET_BITS(metadata->flags, UAVOBJ_ACCESS_SHIFT, mode, 1); } /** @@ -1565,10 +1581,10 @@ void UAVObjSetAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) * \param[in] metadata The metadata object * \return the GCS access type */ -UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* metadata) +UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata *metadata) { - PIOS_Assert(metadata); - return (metadata->flags >> UAVOBJ_GCS_ACCESS_SHIFT) & 1; + PIOS_Assert(metadata); + return (metadata->flags >> UAVOBJ_GCS_ACCESS_SHIFT) & 1; } /** @@ -1576,9 +1592,10 @@ UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* metadata) * \param[in] metadata The metadata object * \param[in] mode The access mode */ -void UAVObjSetGcsAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) { - PIOS_Assert(metadata); - SET_BITS(metadata->flags, UAVOBJ_GCS_ACCESS_SHIFT, mode, 1); +void UAVObjSetGcsAccess(UAVObjMetadata *metadata, UAVObjAccessType mode) +{ + PIOS_Assert(metadata); + SET_BITS(metadata->flags, UAVOBJ_GCS_ACCESS_SHIFT, mode, 1); } /** @@ -1586,9 +1603,10 @@ void UAVObjSetGcsAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) { * \param[in] metadata The metadata object * \return the telemetry acked boolean */ -uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata* metadata) { - PIOS_Assert(metadata); - return (metadata->flags >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 1; +uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata *metadata) +{ + PIOS_Assert(metadata); + return (metadata->flags >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 1; } /** @@ -1596,9 +1614,10 @@ uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata* metadata) { * \param[in] metadata The metadata object * \param[in] val The telemetry acked boolean */ -void UAVObjSetTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) { - PIOS_Assert(metadata); - SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_ACKED_SHIFT, val, 1); +void UAVObjSetTelemetryAcked(UAVObjMetadata *metadata, uint8_t val) +{ + PIOS_Assert(metadata); + SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_ACKED_SHIFT, val, 1); } /** @@ -1606,9 +1625,10 @@ void UAVObjSetTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) { * \param[in] metadata The metadata object * \return the telemetry acked boolean */ -uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata* metadata) { - PIOS_Assert(metadata); - return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 1; +uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata *metadata) +{ + PIOS_Assert(metadata); + return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 1; } /** @@ -1616,9 +1636,10 @@ uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata* metadata) { * \param[in] metadata The metadata object * \param[in] val The GCS telemetry acked boolean */ -void UAVObjSetGcsTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) { - PIOS_Assert(metadata); - SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, val, 1); +void UAVObjSetGcsTelemetryAcked(UAVObjMetadata *metadata, uint8_t val) +{ + PIOS_Assert(metadata); + SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, val, 1); } /** @@ -1626,9 +1647,10 @@ void UAVObjSetGcsTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) { * \param[in] metadata The metadata object * \return the telemetry update mode */ -UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata* metadata) { - PIOS_Assert(metadata); - return (metadata->flags >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK; +UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata *metadata) +{ + PIOS_Assert(metadata); + return (metadata->flags >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK; } /** @@ -1636,9 +1658,10 @@ UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata* metadata) { * \param[in] metadata The metadata object * \param[in] val The telemetry update mode */ -void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val) { - PIOS_Assert(metadata); - SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); +void UAVObjSetTelemetryUpdateMode(UAVObjMetadata *metadata, UAVObjUpdateMode val) +{ + PIOS_Assert(metadata); + SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); } /** @@ -1646,9 +1669,10 @@ void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val * \param[in] metadata The metadata object * \return the GCS telemetry update mode */ -UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* metadata) { - PIOS_Assert(metadata); - return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK; +UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata *metadata) +{ + PIOS_Assert(metadata); + return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK; } /** @@ -1656,27 +1680,28 @@ UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* metadata) * \param[in] metadata The metadata object * \param[in] val The GCS telemetry update mode */ -void UAVObjSetGcsTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val) { - PIOS_Assert(metadata); - SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); +void UAVObjSetGcsTelemetryUpdateMode(UAVObjMetadata *metadata, UAVObjUpdateMode val) +{ + PIOS_Assert(metadata); + SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); } /** * Check if an object is read only * \param[in] obj The object handle - * \return - * \arg 0 if not read only + * \return + * \arg 0 if not read only * \arg 1 if read only * \arg -1 if unable to get meta data */ int8_t UAVObjReadOnly(UAVObjHandle obj_handle) { - PIOS_Assert(obj_handle); - if (!UAVObjIsMetaobject(obj_handle)) { - return UAVObjGetAccess(LinkedMetaDataPtr( (struct UAVOData *)obj_handle)) == ACCESS_READONLY; - } - return -1; + PIOS_Assert(obj_handle); + if (!UAVObjIsMetaobject(obj_handle)) { + return UAVObjGetAccess(LinkedMetaDataPtr((struct UAVOData *)obj_handle)) == ACCESS_READONLY; + } + return -1; } /** @@ -1688,15 +1713,15 @@ int8_t UAVObjReadOnly(UAVObjHandle obj_handle) * \return 0 if success or -1 if failure */ int32_t UAVObjConnectQueue(UAVObjHandle obj_handle, xQueueHandle queue, - uint8_t eventMask) + uint8_t eventMask) { - PIOS_Assert(obj_handle); - PIOS_Assert(queue); - int32_t res; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - res = connectObj(obj_handle, queue, 0, eventMask); - xSemaphoreGiveRecursive(mutex); - return res; + PIOS_Assert(obj_handle); + PIOS_Assert(queue); + int32_t res; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + res = connectObj(obj_handle, queue, 0, eventMask); + xSemaphoreGiveRecursive(mutex); + return res; } /** @@ -1707,13 +1732,13 @@ int32_t UAVObjConnectQueue(UAVObjHandle obj_handle, xQueueHandle queue, */ int32_t UAVObjDisconnectQueue(UAVObjHandle obj_handle, xQueueHandle queue) { - PIOS_Assert(obj_handle); - PIOS_Assert(queue); - int32_t res; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - res = disconnectObj(obj_handle, queue, 0); - xSemaphoreGiveRecursive(mutex); - return res; + PIOS_Assert(obj_handle); + PIOS_Assert(queue); + int32_t res; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + res = disconnectObj(obj_handle, queue, 0); + xSemaphoreGiveRecursive(mutex); + return res; } /** @@ -1725,14 +1750,14 @@ int32_t UAVObjDisconnectQueue(UAVObjHandle obj_handle, xQueueHandle queue) * \return 0 if success or -1 if failure */ int32_t UAVObjConnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb, - uint8_t eventMask) + uint8_t eventMask) { - PIOS_Assert(obj_handle); - int32_t res; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - res = connectObj(obj_handle, 0, cb, eventMask); - xSemaphoreGiveRecursive(mutex); - return res; + PIOS_Assert(obj_handle); + int32_t res; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + res = connectObj(obj_handle, 0, cb, eventMask); + xSemaphoreGiveRecursive(mutex); + return res; } /** @@ -1743,12 +1768,12 @@ int32_t UAVObjConnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb, */ int32_t UAVObjDisconnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb) { - PIOS_Assert(obj_handle); - int32_t res; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - res = disconnectObj(obj_handle, 0, cb); - xSemaphoreGiveRecursive(mutex); - return res; + PIOS_Assert(obj_handle); + int32_t res; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + res = disconnectObj(obj_handle, 0, cb); + xSemaphoreGiveRecursive(mutex); + return res; } /** @@ -1758,7 +1783,7 @@ int32_t UAVObjDisconnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb */ void UAVObjRequestUpdate(UAVObjHandle obj_handle) { - UAVObjRequestInstanceUpdate(obj_handle, UAVOBJ_ALL_INSTANCES); + UAVObjRequestInstanceUpdate(obj_handle, UAVOBJ_ALL_INSTANCES); } /** @@ -1769,10 +1794,10 @@ void UAVObjRequestUpdate(UAVObjHandle obj_handle) */ void UAVObjRequestInstanceUpdate(UAVObjHandle obj_handle, uint16_t instId) { - PIOS_Assert(obj_handle); - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - sendEvent((struct UAVOBase *) obj_handle, instId, EV_UPDATE_REQ); - xSemaphoreGiveRecursive(mutex); + PIOS_Assert(obj_handle); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATE_REQ); + xSemaphoreGiveRecursive(mutex); } /** @@ -1781,7 +1806,7 @@ void UAVObjRequestInstanceUpdate(UAVObjHandle obj_handle, uint16_t instId) */ void UAVObjUpdated(UAVObjHandle obj_handle) { - UAVObjInstanceUpdated(obj_handle, UAVOBJ_ALL_INSTANCES); + UAVObjInstanceUpdated(obj_handle, UAVOBJ_ALL_INSTANCES); } /** @@ -1791,10 +1816,10 @@ void UAVObjUpdated(UAVObjHandle obj_handle) */ void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId) { - PIOS_Assert(obj_handle); - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - sendEvent((struct UAVOBase *) obj_handle, instId, EV_UPDATED_MANUAL); - xSemaphoreGiveRecursive(mutex); + PIOS_Assert(obj_handle); + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED_MANUAL); + xSemaphoreGiveRecursive(mutex); } /** @@ -1802,154 +1827,158 @@ void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId) * \param iterator This function will be called once for each object, * the object will be passed as a parameter */ -void UAVObjIterate(void (*iterator) (UAVObjHandle obj)) +void UAVObjIterate(void (*iterator)(UAVObjHandle obj)) { - PIOS_Assert(iterator); + PIOS_Assert(iterator); - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Iterate through the list and invoke iterator for each object - UAVO_LIST_ITERATE(obj) - (*iterator) ((UAVObjHandle) obj); - (*iterator) ((UAVObjHandle) &obj->metaObj); - } + // Iterate through the list and invoke iterator for each object + UAVO_LIST_ITERATE (obj) + (*iterator)((UAVObjHandle)obj); + (*iterator)((UAVObjHandle) &obj->metaObj); +} - // Release lock - xSemaphoreGiveRecursive(mutex); +// Release lock +xSemaphoreGiveRecursive(mutex); } /** * Send a triggered event to all event queues registered on the object. */ -static int32_t sendEvent(struct UAVOBase * obj, uint16_t instId, - UAVObjEventType triggered_event) +static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, + UAVObjEventType triggered_event) { - /* Set up the message that will be sent to all registered listeners */ - UAVObjEvent msg = { - .obj = (UAVObjHandle) obj, - .event = triggered_event, - .instId = instId, - }; + /* Set up the message that will be sent to all registered listeners */ + UAVObjEvent msg = { + .obj = (UAVObjHandle)obj, + .event = triggered_event, + .instId = instId, + }; - // Go through each object and push the event message in the queue (if event is activated for the queue) - struct ObjectEventEntry *event; - LL_FOREACH(obj->next_event, event) { - if (event->eventMask == 0 - || (event->eventMask & triggered_event) != 0) { - // Send to queue if a valid queue is registered - if (event->queue) { - // will not block - if (xQueueSend(event->queue, &msg, 0) != pdTRUE) { - stats.lastQueueErrorID = UAVObjGetID(obj); - ++stats.eventQueueErrors; - } - } + // Go through each object and push the event message in the queue (if event is activated for the queue) + struct ObjectEventEntry *event; - // Invoke callback (from event task) if a valid one is registered - if (event->cb) { - // invoke callback from the event task, will not block - if (EventCallbackDispatch(&msg, event->cb) != pdTRUE) { - ++stats.eventCallbackErrors; - stats.lastCallbackErrorID = UAVObjGetID(obj); - } - } - } - } + LL_FOREACH(obj->next_event, event) { + if (event->eventMask == 0 + || (event->eventMask & triggered_event) != 0) { + // Send to queue if a valid queue is registered + if (event->queue) { + // will not block + if (xQueueSend(event->queue, &msg, 0) != pdTRUE) { + stats.lastQueueErrorID = UAVObjGetID(obj); + ++stats.eventQueueErrors; + } + } - return 0; + // Invoke callback (from event task) if a valid one is registered + if (event->cb) { + // invoke callback from the event task, will not block + if (EventCallbackDispatch(&msg, event->cb) != pdTRUE) { + ++stats.eventCallbackErrors; + stats.lastCallbackErrorID = UAVObjGetID(obj); + } + } + } + } + + return 0; } /** * Create a new object instance, return the instance info or NULL if failure. */ -static InstanceHandle createInstance(struct UAVOData * obj, uint16_t instId) +static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId) { - struct UAVOMultiInst *instEntry; + struct UAVOMultiInst *instEntry; - /* Don't allow more than one instance for single instance objects */ - if (UAVObjIsSingleInstance(&(obj->base))) { - PIOS_Assert(0); - return NULL; - } + /* Don't allow more than one instance for single instance objects */ + if (UAVObjIsSingleInstance(&(obj->base))) { + PIOS_Assert(0); + return NULL; + } - /* Don't create more than the allowed number of instances */ - if (instId >= UAVOBJ_MAX_INSTANCES) { - return NULL; - } + /* Don't create more than the allowed number of instances */ + if (instId >= UAVOBJ_MAX_INSTANCES) { + return NULL; + } - /* Don't allow duplicate instances */ - if (instId < UAVObjGetNumInstances(&(obj->base))) { - return NULL; - } + /* Don't allow duplicate instances */ + if (instId < UAVObjGetNumInstances(&(obj->base))) { + return NULL; + } - // Create any missing instances (all instance IDs must be sequential) - for (uint16_t n = UAVObjGetNumInstances(&(obj->base)); n < instId; ++n) { - if (createInstance(obj, n) == NULL) { - return NULL; - } - } + // Create any missing instances (all instance IDs must be sequential) + for (uint16_t n = UAVObjGetNumInstances(&(obj->base)); n < instId; ++n) { + if (createInstance(obj, n) == NULL) { + return NULL; + } + } - /* Create the actual instance */ - uint32_t size = sizeof(struct UAVOMultiInst) + obj->instance_size; - instEntry = (struct UAVOMultiInst *) pvPortMalloc(size); - if (!instEntry) - return NULL; - memset(instEntry, 0, size); - LL_APPEND(( (struct UAVOMulti*)obj )->instance0.next, instEntry); + /* Create the actual instance */ + uint32_t size = sizeof(struct UAVOMultiInst) + obj->instance_size; + instEntry = (struct UAVOMultiInst *)pvPortMalloc(size); + if (!instEntry) { + return NULL; + } + memset(instEntry, 0, size); + LL_APPEND(((struct UAVOMulti *)obj)->instance0.next, instEntry); - ( (struct UAVOMulti*)obj )->num_instances++; + ((struct UAVOMulti *)obj)->num_instances++; - // Fire event - UAVObjInstanceUpdated((UAVObjHandle) obj, instId); + // Fire event + UAVObjInstanceUpdated((UAVObjHandle)obj, instId); - // Done - return InstanceDataOffset(instEntry); + // Done + return InstanceDataOffset(instEntry); } /** * Get the instance information or NULL if the instance does not exist */ -static InstanceHandle getInstance(struct UAVOData * obj, uint16_t instId) +static InstanceHandle getInstance(struct UAVOData *obj, uint16_t instId) { - if (UAVObjIsMetaobject(&obj->base)) { - /* Metadata Instance */ + if (UAVObjIsMetaobject(&obj->base)) { + /* Metadata Instance */ - if (instId != 0) - return NULL; + if (instId != 0) { + return NULL; + } - /* Augment our pointer to reflect the proper type */ - struct UAVOMeta * uavo_meta = (struct UAVOMeta *) obj; - return (&(uavo_meta->instance0)); + /* Augment our pointer to reflect the proper type */ + struct UAVOMeta *uavo_meta = (struct UAVOMeta *)obj; + return &(uavo_meta->instance0); + } else if (UAVObjIsSingleInstance(&(obj->base))) { + /* Single Instance */ - } else if (UAVObjIsSingleInstance(&(obj->base))) { - /* Single Instance */ + if (instId != 0) { + return NULL; + } - if (instId != 0) - return NULL; + /* Augment our pointer to reflect the proper type */ + struct UAVOSingle *uavo_single = (struct UAVOSingle *)obj; + return &(uavo_single->instance0); + } else { + /* Multi Instance */ + /* Augment our pointer to reflect the proper type */ + struct UAVOMulti *uavo_multi = (struct UAVOMulti *)obj; + if (instId >= uavo_multi->num_instances) { + return NULL; + } - /* Augment our pointer to reflect the proper type */ - struct UAVOSingle * uavo_single = (struct UAVOSingle *) obj; - return (&(uavo_single->instance0)); - } else { - /* Multi Instance */ - /* Augment our pointer to reflect the proper type */ - struct UAVOMulti * uavo_multi = (struct UAVOMulti *) obj; - if (instId >= uavo_multi->num_instances) - return NULL; - - // Look for specified instance ID - uint16_t instance = 0; - struct UAVOMultiInst *instEntry; - LL_FOREACH(&(uavo_multi->instance0), instEntry) { - if (instance++ == instId) { - /* Found it */ - return &(instEntry->instance); - } - } - /* Instance was not found */ - return NULL; - } + // Look for specified instance ID + uint16_t instance = 0; + struct UAVOMultiInst *instEntry; + LL_FOREACH(&(uavo_multi->instance0), instEntry) { + if (instance++ == instId) { + /* Found it */ + return &(instEntry->instance); + } + } + /* Instance was not found */ + return NULL; + } } /** @@ -1961,33 +1990,33 @@ static InstanceHandle getInstance(struct UAVOData * obj, uint16_t instId) * \return 0 if success or -1 if failure */ static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, - UAVObjEventCallback cb, uint8_t eventMask) + UAVObjEventCallback cb, uint8_t eventMask) { - struct ObjectEventEntry *event; - struct UAVOBase *obj; + struct ObjectEventEntry *event; + struct UAVOBase *obj; - // Check that the queue is not already connected, if it is simply update event mask - obj = (struct UAVOBase *) obj_handle; - LL_FOREACH(obj->next_event, event) { - if (event->queue == queue && event->cb == cb) { - // Already connected, update event mask and return - event->eventMask = eventMask; - return 0; - } - } + // Check that the queue is not already connected, if it is simply update event mask + obj = (struct UAVOBase *)obj_handle; + LL_FOREACH(obj->next_event, event) { + if (event->queue == queue && event->cb == cb) { + // Already connected, update event mask and return + event->eventMask = eventMask; + return 0; + } + } - // Add queue to list - event = (struct ObjectEventEntry *) pvPortMalloc(sizeof(struct ObjectEventEntry)); - if (event == NULL) { - return -1; - } - event->queue = queue; - event->cb = cb; - event->eventMask = eventMask; - LL_APPEND(obj->next_event, event); + // Add queue to list + event = (struct ObjectEventEntry *)pvPortMalloc(sizeof(struct ObjectEventEntry)); + if (event == NULL) { + return -1; + } + event->queue = queue; + event->cb = cb; + event->eventMask = eventMask; + LL_APPEND(obj->next_event, event); - // Done - return 0; + // Done + return 0; } /** @@ -1998,42 +2027,43 @@ static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, * \return 0 if success or -1 if failure */ static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue, - UAVObjEventCallback cb) + UAVObjEventCallback cb) { - struct ObjectEventEntry *event; - struct UAVOBase *obj; + struct ObjectEventEntry *event; + struct UAVOBase *obj; - // Find queue and remove it - obj = (struct UAVOBase *) obj_handle; - LL_FOREACH(obj->next_event, event) { - if ((event->queue == queue - && event->cb == cb)) { - LL_DELETE(obj->next_event, event); - vPortFree(event); - return 0; - } - } + // Find queue and remove it + obj = (struct UAVOBase *)obj_handle; + LL_FOREACH(obj->next_event, event) { + if ((event->queue == queue + && event->cb == cb)) { + LL_DELETE(obj->next_event, event); + vPortFree(event); + return 0; + } + } - // If this point is reached the queue was not found - return -1; + // If this point is reached the queue was not found + return -1; } #if defined(PIOS_USE_SETTINGS_ON_SDCARD) /** * Wrapper for the sprintf function */ -static void customSPrintf(uint8_t * buffer, uint8_t * format, ...) +static void customSPrintf(uint8_t *buffer, uint8_t *format, ...) { - va_list args; - va_start(args, format); - vsprintf((char *)buffer, (char *)format, args); + va_list args; + + va_start(args, format); + vsprintf((char *)buffer, (char *)format, args); } /** * Get an 8 character (plus extension) filename for the object. */ -static void objectFilename(UAVObjHandle obj_handle, uint8_t * filename) +static void objectFilename(UAVObjHandle obj_handle, uint8_t *filename) { - customSPrintf(filename, (uint8_t *) "%X.obj", UAVObjGetID(obj_handle)); + customSPrintf(filename, (uint8_t *)"%X.obj", UAVObjGetID(obj_handle)); } #endif /* PIOS_USE_SETTINGS_ON_SDCARD */ diff --git a/flight/uavtalk/inc/uavtalk.h b/flight/uavtalk/inc/uavtalk.h index a11979a95..cfbeef471 100644 --- a/flight/uavtalk/inc/uavtalk.h +++ b/flight/uavtalk/inc/uavtalk.h @@ -30,7 +30,7 @@ #define UAVTALK_H // Public types -typedef int32_t (*UAVTalkOutputStream)(uint8_t* data, int32_t length); +typedef int32_t (*UAVTalkOutputStream)(uint8_t *data, int32_t length); typedef struct { uint32_t txBytes; @@ -43,9 +43,9 @@ typedef struct { uint32_t rxErrors; } UAVTalkStats; -typedef void* UAVTalkConnection; +typedef void *UAVTalkConnection; -typedef enum {UAVTALK_STATE_ERROR=0, UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_TIMESTAMP, UAVTALK_STATE_DATA, UAVTALK_STATE_CS, UAVTALK_STATE_COMPLETE} UAVTalkRxState; +typedef enum { UAVTALK_STATE_ERROR = 0, UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_TIMESTAMP, UAVTALK_STATE_DATA, UAVTALK_STATE_CS, UAVTALK_STATE_COMPLETE } UAVTalkRxState; // Public functions UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream); diff --git a/flight/uavtalk/inc/uavtalk_priv.h b/flight/uavtalk/inc/uavtalk_priv.h index 84174e90a..310f9e351 100644 --- a/flight/uavtalk/inc/uavtalk_priv.h +++ b/flight/uavtalk/inc/uavtalk_priv.h @@ -33,40 +33,40 @@ // Private types and constants typedef struct { - uint8_t sync; - uint8_t type; - uint16_t size; - uint32_t objId; + uint8_t sync; + uint8_t type; + uint16_t size; + uint32_t objId; } uavtalk_min_header; -#define UAVTALK_MIN_HEADER_LENGTH sizeof(uavtalk_min_header) +#define UAVTALK_MIN_HEADER_LENGTH sizeof(uavtalk_min_header) typedef struct { - uint8_t sync; - uint8_t type; - uint16_t size; - uint32_t objId; - uint16_t instId; - uint16_t timestamp; + uint8_t sync; + uint8_t type; + uint16_t size; + uint32_t objId; + uint16_t instId; + uint16_t timestamp; } uavtalk_max_header; -#define UAVTALK_MAX_HEADER_LENGTH sizeof(uavtalk_max_header) +#define UAVTALK_MAX_HEADER_LENGTH sizeof(uavtalk_max_header) typedef uint8_t uavtalk_checksum; -#define UAVTALK_CHECKSUM_LENGTH sizeof(uavtalk_checksum) -#define UAVTALK_MAX_PAYLOAD_LENGTH (UAVOBJECTS_LARGEST + 1) -#define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH -#define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH +#define UAVTALK_CHECKSUM_LENGTH sizeof(uavtalk_checksum) +#define UAVTALK_MAX_PAYLOAD_LENGTH (UAVOBJECTS_LARGEST + 1) +#define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH +#define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH typedef struct { UAVObjHandle obj; uint8_t type; - uint16_t packet_size; - uint32_t objId; - uint16_t instId; - uint32_t length; - uint8_t instanceLength; - uint8_t timestampLength; - uint8_t cs; - uint16_t timestamp; + uint16_t packet_size; + uint32_t objId; + uint16_t instId; + uint32_t length; + uint8_t instanceLength; + uint8_t timestampLength; + uint8_t cs; + uint16_t timestamp; uint32_t rxCount; UAVTalkRxState state; uint16_t rxPacketLength; @@ -75,39 +75,39 @@ typedef struct { typedef struct { uint8_t canari; UAVTalkOutputStream outStream; - xSemaphoreHandle lock; - xSemaphoreHandle transLock; - xSemaphoreHandle respSema; + xSemaphoreHandle lock; + xSemaphoreHandle transLock; + xSemaphoreHandle respSema; UAVObjHandle respObj; - uint16_t respInstId; + uint16_t respInstId; UAVTalkStats stats; UAVTalkInputProcessor iproc; - uint8_t *rxBuffer; - uint32_t txSize; - uint8_t *txBuffer; + uint8_t *rxBuffer; + uint32_t txSize; + uint8_t *txBuffer; } UAVTalkConnectionData; -#define UAVTALK_CANARI 0xCA +#define UAVTALK_CANARI 0xCA #define UAVTALK_WAITFOREVER -1 #define UAVTALK_NOWAIT 0 -#define UAVTALK_SYNC_VAL 0x3C -#define UAVTALK_TYPE_MASK 0x78 -#define UAVTALK_TYPE_VER 0x20 -#define UAVTALK_TIMESTAMPED 0x80 -#define UAVTALK_TYPE_OBJ (UAVTALK_TYPE_VER | 0x00) -#define UAVTALK_TYPE_OBJ_REQ (UAVTALK_TYPE_VER | 0x01) -#define UAVTALK_TYPE_OBJ_ACK (UAVTALK_TYPE_VER | 0x02) -#define UAVTALK_TYPE_ACK (UAVTALK_TYPE_VER | 0x03) -#define UAVTALK_TYPE_NACK (UAVTALK_TYPE_VER | 0x04) -#define UAVTALK_TYPE_OBJ_TS (UAVTALK_TIMESTAMPED | UAVTALK_TYPE_OBJ) -#define UAVTALK_TYPE_OBJ_ACK_TS (UAVTALK_TIMESTAMPED | UAVTALK_TYPE_OBJ_ACK) +#define UAVTALK_SYNC_VAL 0x3C +#define UAVTALK_TYPE_MASK 0x78 +#define UAVTALK_TYPE_VER 0x20 +#define UAVTALK_TIMESTAMPED 0x80 +#define UAVTALK_TYPE_OBJ (UAVTALK_TYPE_VER | 0x00) +#define UAVTALK_TYPE_OBJ_REQ (UAVTALK_TYPE_VER | 0x01) +#define UAVTALK_TYPE_OBJ_ACK (UAVTALK_TYPE_VER | 0x02) +#define UAVTALK_TYPE_ACK (UAVTALK_TYPE_VER | 0x03) +#define UAVTALK_TYPE_NACK (UAVTALK_TYPE_VER | 0x04) +#define UAVTALK_TYPE_OBJ_TS (UAVTALK_TIMESTAMPED | UAVTALK_TYPE_OBJ) +#define UAVTALK_TYPE_OBJ_ACK_TS (UAVTALK_TIMESTAMPED | UAVTALK_TYPE_OBJ_ACK) -//macros -#define CHECKCONHANDLE(handle,variable,failcommand) \ - variable = (UAVTalkConnectionData*) handle; \ - if (variable == NULL || variable->canari != UAVTALK_CANARI) { \ - failcommand; \ - } +// macros +#define CHECKCONHANDLE(handle, variable, failcommand) \ + variable = (UAVTalkConnectionData *)handle; \ + if (variable == NULL || variable->canari != UAVTALK_CANARI) { \ + failcommand; \ + } #endif // UAVTALK__PRIV_H /** diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index eb060b735..bf19512cc 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -8,8 +8,8 @@ * @file uavtalk.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief UAVTalk library, implements to telemetry protocol. See the wiki for more details. - * This library should not be called directly by the application, it is only used by the - * Telemetry module. + * This library should not be called directly by the application, it is only used by the + * Telemetry module. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ @@ -38,7 +38,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type); static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type); static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId); -static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length); +static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data, int32_t length); static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId); /** @@ -50,24 +50,31 @@ static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint1 */ UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream) { - // allocate object - UAVTalkConnectionData * connection = pvPortMalloc(sizeof(UAVTalkConnectionData)); - if (!connection) return 0; - connection->canari = UAVTALK_CANARI; - connection->iproc.rxPacketLength = 0; - connection->iproc.state = UAVTALK_STATE_SYNC; - connection->outStream = outputStream; - connection->lock = xSemaphoreCreateRecursiveMutex(); - connection->transLock = xSemaphoreCreateRecursiveMutex(); - // allocate buffers - connection->rxBuffer = pvPortMalloc(UAVTALK_MAX_PACKET_LENGTH); - if (!connection->rxBuffer) return 0; - connection->txBuffer = pvPortMalloc(UAVTALK_MAX_PACKET_LENGTH); - if (!connection->txBuffer) return 0; - vSemaphoreCreateBinary(connection->respSema); - xSemaphoreTake(connection->respSema, 0); // reset to zero - UAVTalkResetStats( (UAVTalkConnection) connection ); - return (UAVTalkConnection) connection; + // allocate object + UAVTalkConnectionData *connection = pvPortMalloc(sizeof(UAVTalkConnectionData)); + + if (!connection) { + return 0; + } + connection->canari = UAVTALK_CANARI; + connection->iproc.rxPacketLength = 0; + connection->iproc.state = UAVTALK_STATE_SYNC; + connection->outStream = outputStream; + connection->lock = xSemaphoreCreateRecursiveMutex(); + connection->transLock = xSemaphoreCreateRecursiveMutex(); + // allocate buffers + connection->rxBuffer = pvPortMalloc(UAVTALK_MAX_PACKET_LENGTH); + if (!connection->rxBuffer) { + return 0; + } + connection->txBuffer = pvPortMalloc(UAVTALK_MAX_PACKET_LENGTH); + if (!connection->txBuffer) { + return 0; + } + vSemaphoreCreateBinary(connection->respSema); + xSemaphoreTake(connection->respSema, 0); // reset to zero + UAVTalkResetStats((UAVTalkConnection)connection); + return (UAVTalkConnection)connection; } /** @@ -79,21 +86,20 @@ UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream) */ int32_t UAVTalkSetOutputStream(UAVTalkConnection connectionHandle, UAVTalkOutputStream outputStream) { + UAVTalkConnectionData *connection; - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); + CHECKCONHANDLE(connectionHandle, connection, return -1); - // Lock - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - - // set output stream - connection->outStream = outputStream; - - // Release lock - xSemaphoreGiveRecursive(connection->lock); + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - return 0; + // set output stream + connection->outStream = outputStream; + // Release lock + xSemaphoreGiveRecursive(connection->lock); + + return 0; } /** @@ -103,9 +109,10 @@ int32_t UAVTalkSetOutputStream(UAVTalkConnection connectionHandle, UAVTalkOutput */ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connectionHandle) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return NULL); - return connection->outStream; + UAVTalkConnectionData *connection; + + CHECKCONHANDLE(connectionHandle, connection, return NULL); + return connection->outStream; } /** @@ -113,19 +120,20 @@ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connectionHandle) * \param[in] connection UAVTalkConnection to be used * @param[out] statsOut Statistics counters */ -void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats* statsOut) +void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return ); + UAVTalkConnectionData *connection; - // Lock - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - - // Copy stats - memcpy(statsOut, &connection->stats, sizeof(UAVTalkStats)); - - // Release lock - xSemaphoreGiveRecursive(connection->lock); + CHECKCONHANDLE(connectionHandle, connection, return ); + + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + + // Copy stats + memcpy(statsOut, &connection->stats, sizeof(UAVTalkStats)); + + // Release lock + xSemaphoreGiveRecursive(connection->lock); } /** @@ -134,17 +142,18 @@ void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats* statsOut) */ void UAVTalkResetStats(UAVTalkConnection connectionHandle) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return); + UAVTalkConnectionData *connection; - // Lock - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - - // Clear stats - memset(&connection->stats, 0, sizeof(UAVTalkStats)); - - // Release lock - xSemaphoreGiveRecursive(connection->lock); + CHECKCONHANDLE(connectionHandle, connection, return ); + + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + + // Clear stats + memset(&connection->stats, 0, sizeof(UAVTalkStats)); + + // Release lock + xSemaphoreGiveRecursive(connection->lock); } /** @@ -152,11 +161,12 @@ void UAVTalkResetStats(UAVTalkConnection connectionHandle) */ void UAVTalkGetLastTimestamp(UAVTalkConnection connectionHandle, uint16_t *timestamp) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return); + UAVTalkConnectionData *connection; - UAVTalkInputProcessor *iproc = &connection->iproc; - *timestamp = iproc->timestamp; + CHECKCONHANDLE(connectionHandle, connection, return ); + + UAVTalkInputProcessor *iproc = &connection->iproc; + *timestamp = iproc->timestamp; } @@ -172,9 +182,10 @@ void UAVTalkGetLastTimestamp(UAVTalkConnection connectionHandle, uint16_t *times */ int32_t UAVTalkSendObjectRequest(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, int32_t timeout) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ, timeout); + UAVTalkConnectionData *connection; + + CHECKCONHANDLE(connectionHandle, connection, return -1); + return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ, timeout); } /** @@ -189,17 +200,15 @@ int32_t UAVTalkSendObjectRequest(UAVTalkConnection connectionHandle, UAVObjHandl */ int32_t UAVTalkSendObject(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); - // Send object - if (acked == 1) - { - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK, timeoutMs); - } - else - { - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ, timeoutMs); - } + UAVTalkConnectionData *connection; + + CHECKCONHANDLE(connectionHandle, connection, return -1); + // Send object + if (acked == 1) { + return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK, timeoutMs); + } else { + return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ, timeoutMs); + } } /** @@ -214,17 +223,15 @@ int32_t UAVTalkSendObject(UAVTalkConnection connectionHandle, UAVObjHandle obj, */ int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); - // Send object - if (acked == 1) - { - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK_TS, timeoutMs); - } - else - { - return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_TS, timeoutMs); - } + UAVTalkConnectionData *connection; + + CHECKCONHANDLE(connectionHandle, connection, return -1); + // Send object + if (acked == 1) { + return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK_TS, timeoutMs); + } else { + return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_TS, timeoutMs); + } } /** @@ -233,57 +240,49 @@ int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjH * \param[in] obj Object * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] type Transaction type - * UAVTALK_TYPE_OBJ: send object, - * UAVTALK_TYPE_OBJ_REQ: request object update - * UAVTALK_TYPE_OBJ_ACK: send object with an ack + * UAVTALK_TYPE_OBJ: send object, + * UAVTALK_TYPE_OBJ_REQ: request object update + * UAVTALK_TYPE_OBJ_ACK: send object with an ack * \return 0 Success * \return -1 Failure */ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs) { - int32_t respReceived; - - // Send object depending on if a response is needed - if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS || type == UAVTALK_TYPE_OBJ_REQ) - { - // Get transaction lock (will block if a transaction is pending) - xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY); - // Send object - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - connection->respObj = obj; - connection->respInstId = instId; - sendObject(connection, obj, instId, type); - xSemaphoreGiveRecursive(connection->lock); - // Wait for response (or timeout) - respReceived = xSemaphoreTake(connection->respSema, timeoutMs/portTICK_RATE_MS); - // Check if a response was received - if (respReceived == pdFALSE) - { - // Cancel transaction - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - xSemaphoreTake(connection->respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema) - connection->respObj = 0; - xSemaphoreGiveRecursive(connection->lock); - xSemaphoreGiveRecursive(connection->transLock); - return -1; - } - else - { - xSemaphoreGiveRecursive(connection->transLock); - return 0; - } - } - else if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS) - { - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - sendObject(connection, obj, instId, type); - xSemaphoreGiveRecursive(connection->lock); - return 0; - } - else - { - return -1; - } + int32_t respReceived; + + // Send object depending on if a response is needed + if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS || type == UAVTALK_TYPE_OBJ_REQ) { + // Get transaction lock (will block if a transaction is pending) + xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY); + // Send object + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + connection->respObj = obj; + connection->respInstId = instId; + sendObject(connection, obj, instId, type); + xSemaphoreGiveRecursive(connection->lock); + // Wait for response (or timeout) + respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS); + // Check if a response was received + if (respReceived == pdFALSE) { + // Cancel transaction + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + xSemaphoreTake(connection->respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema) + connection->respObj = 0; + xSemaphoreGiveRecursive(connection->lock); + xSemaphoreGiveRecursive(connection->transLock); + return -1; + } else { + xSemaphoreGiveRecursive(connection->transLock); + return 0; + } + } else if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS) { + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + sendObject(connection, obj, instId, type); + xSemaphoreGiveRecursive(connection->lock); + return 0; + } else { + return -1; + } } /** @@ -294,246 +293,236 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle */ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t rxbyte) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); + UAVTalkConnectionData *connection; - UAVTalkInputProcessor *iproc = &connection->iproc; - ++connection->stats.rxBytes; + CHECKCONHANDLE(connectionHandle, connection, return -1); - if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) - iproc->state = UAVTALK_STATE_SYNC; - - if (iproc->rxPacketLength < 0xffff) - iproc->rxPacketLength++; // update packet byte count - - // Receive state machine - switch (iproc->state) - { - case UAVTALK_STATE_SYNC: - if (rxbyte != UAVTALK_SYNC_VAL) - break; - - // Initialize and update the CRC - iproc->cs = PIOS_CRC_updateByte(0, rxbyte); - - iproc->rxPacketLength = 1; - - iproc->state = UAVTALK_STATE_TYPE; - break; - - case UAVTALK_STATE_TYPE: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) - { - iproc->state = UAVTALK_STATE_ERROR; - break; - } - - iproc->type = rxbyte; - - iproc->packet_size = 0; - - iproc->state = UAVTALK_STATE_SIZE; - iproc->rxCount = 0; - break; - - case UAVTALK_STATE_SIZE: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - if (iproc->rxCount == 0) - { - iproc->packet_size += rxbyte; - iproc->rxCount++; - break; - } - - iproc->packet_size += rxbyte << 8; - - if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) - { // incorrect packet size - iproc->state = UAVTALK_STATE_ERROR; - break; - } - - iproc->rxCount = 0; - iproc->objId = 0; - iproc->state = UAVTALK_STATE_OBJID; - break; - - case UAVTALK_STATE_OBJID: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - iproc->objId += rxbyte << (8*(iproc->rxCount++)); + UAVTalkInputProcessor *iproc = &connection->iproc; + ++connection->stats.rxBytes; - if (iproc->rxCount < 4) - break; - - // Search for object. - iproc->obj = UAVObjGetByID(iproc->objId); - - // Determine data length - if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) - { - iproc->length = 0; - iproc->instanceLength = 0; - } - else - { - if (iproc->obj) - { - iproc->length = UAVObjGetNumBytes(iproc->obj); - iproc->instanceLength = (UAVObjIsSingleInstance(iproc->obj) ? 0 : 2); - iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0; - } - else - { - // We don't know if it's a multi-instance object, so just assume it's 0. - iproc->instanceLength = 0; - iproc->length = iproc->packet_size - iproc->rxPacketLength; - } - } - - // Check length and determine next state - if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) - { - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; - break; - } - - // Check the lengths match - if ((iproc->rxPacketLength + iproc->instanceLength + iproc->timestampLength + iproc->length) != iproc->packet_size) - { // packet error - mismatched packet size - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; - break; - } - - iproc->instId = 0; - if (iproc->type == UAVTALK_TYPE_NACK) - { - // If this is a NACK, we skip to Checksum - iproc->state = UAVTALK_STATE_CS; - } - // Check if this is a single instance object (i.e. if the instance ID field is coming next) - else if ((iproc->obj != 0) && !UAVObjIsSingleInstance(iproc->obj)) - { - iproc->state = UAVTALK_STATE_INSTID; - } - // Check if this is a single instance and has a timestamp in it - else if ((iproc->obj != 0) && (iproc->type & UAVTALK_TIMESTAMPED)) - { - iproc->timestamp = 0; - iproc->state = UAVTALK_STATE_TIMESTAMP; - } - else - { - // If there is a payload get it, otherwise receive checksum - if (iproc->length > 0) - iproc->state = UAVTALK_STATE_DATA; - else - iproc->state = UAVTALK_STATE_CS; - } - iproc->rxCount = 0; - - break; - - case UAVTALK_STATE_INSTID: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - iproc->instId += rxbyte << (8*(iproc->rxCount++)); + if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) { + iproc->state = UAVTALK_STATE_SYNC; + } - if (iproc->rxCount < 2) - break; - - iproc->rxCount = 0; - - // If there is a timestamp, get it - if ((iproc->length > 0) && (iproc->type & UAVTALK_TIMESTAMPED)) - { - iproc->timestamp = 0; - iproc->state = UAVTALK_STATE_TIMESTAMP; - } - // If there is a payload get it, otherwise receive checksum - else if (iproc->length > 0) - iproc->state = UAVTALK_STATE_DATA; - else - iproc->state = UAVTALK_STATE_CS; - - break; + if (iproc->rxPacketLength < 0xffff) { + iproc->rxPacketLength++; // update packet byte count + } + // Receive state machine + switch (iproc->state) { + case UAVTALK_STATE_SYNC: + if (rxbyte != UAVTALK_SYNC_VAL) { + break; + } - case UAVTALK_STATE_TIMESTAMP: - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + // Initialize and update the CRC + iproc->cs = PIOS_CRC_updateByte(0, rxbyte); - iproc->timestamp += rxbyte << (8*(iproc->rxCount++)); + iproc->rxPacketLength = 1; - if (iproc->rxCount < 2) - break; + iproc->state = UAVTALK_STATE_TYPE; + break; - iproc->rxCount = 0; + case UAVTALK_STATE_TYPE: - // If there is a payload get it, otherwise receive checksum - if (iproc->length > 0) - iproc->state = UAVTALK_STATE_DATA; - else - iproc->state = UAVTALK_STATE_CS; - break; + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - case UAVTALK_STATE_DATA: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - connection->rxBuffer[iproc->rxCount++] = rxbyte; - if (iproc->rxCount < iproc->length) - break; - - iproc->state = UAVTALK_STATE_CS; - iproc->rxCount = 0; - break; - - case UAVTALK_STATE_CS: - - // the CRC byte - if (rxbyte != iproc->cs) - { // packet error - faulty CRC - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; - break; - } - - if (iproc->rxPacketLength != (iproc->packet_size + 1)) - { // packet error - mismatched packet size - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; - break; - } + if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) { + iproc->state = UAVTALK_STATE_ERROR; + break; + } - connection->stats.rxObjectBytes += iproc->length; - connection->stats.rxObjects++; + iproc->type = rxbyte; - iproc->state = UAVTALK_STATE_COMPLETE; - break; - - default: - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; - } - - // Done - return iproc->state; + iproc->packet_size = 0; + + iproc->state = UAVTALK_STATE_SIZE; + iproc->rxCount = 0; + break; + + case UAVTALK_STATE_SIZE: + + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + + if (iproc->rxCount == 0) { + iproc->packet_size += rxbyte; + iproc->rxCount++; + break; + } + + iproc->packet_size += rxbyte << 8; + + if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) { // incorrect packet size + iproc->state = UAVTALK_STATE_ERROR; + break; + } + + iproc->rxCount = 0; + iproc->objId = 0; + iproc->state = UAVTALK_STATE_OBJID; + break; + + case UAVTALK_STATE_OBJID: + + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + + iproc->objId += rxbyte << (8 * (iproc->rxCount++)); + + if (iproc->rxCount < 4) { + break; + } + + // Search for object. + iproc->obj = UAVObjGetByID(iproc->objId); + + // Determine data length + if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) { + iproc->length = 0; + iproc->instanceLength = 0; + } else { + if (iproc->obj) { + iproc->length = UAVObjGetNumBytes(iproc->obj); + iproc->instanceLength = (UAVObjIsSingleInstance(iproc->obj) ? 0 : 2); + iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0; + } else { + // We don't know if it's a multi-instance object, so just assume it's 0. + iproc->instanceLength = 0; + iproc->length = iproc->packet_size - iproc->rxPacketLength; + } + } + + // Check length and determine next state + if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) { + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + break; + } + + // Check the lengths match + if ((iproc->rxPacketLength + iproc->instanceLength + iproc->timestampLength + iproc->length) != iproc->packet_size) { // packet error - mismatched packet size + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + break; + } + + iproc->instId = 0; + if (iproc->type == UAVTALK_TYPE_NACK) { + // If this is a NACK, we skip to Checksum + iproc->state = UAVTALK_STATE_CS; + } + // Check if this is a single instance object (i.e. if the instance ID field is coming next) + else if ((iproc->obj != 0) && !UAVObjIsSingleInstance(iproc->obj)) { + iproc->state = UAVTALK_STATE_INSTID; + } + // Check if this is a single instance and has a timestamp in it + else if ((iproc->obj != 0) && (iproc->type & UAVTALK_TIMESTAMPED)) { + iproc->timestamp = 0; + iproc->state = UAVTALK_STATE_TIMESTAMP; + } else { + // If there is a payload get it, otherwise receive checksum + if (iproc->length > 0) { + iproc->state = UAVTALK_STATE_DATA; + } else { + iproc->state = UAVTALK_STATE_CS; + } + } + iproc->rxCount = 0; + + break; + + case UAVTALK_STATE_INSTID: + + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + + iproc->instId += rxbyte << (8 * (iproc->rxCount++)); + + if (iproc->rxCount < 2) { + break; + } + + iproc->rxCount = 0; + + // If there is a timestamp, get it + if ((iproc->length > 0) && (iproc->type & UAVTALK_TIMESTAMPED)) { + iproc->timestamp = 0; + iproc->state = UAVTALK_STATE_TIMESTAMP; + } + // If there is a payload get it, otherwise receive checksum + else if (iproc->length > 0) { + iproc->state = UAVTALK_STATE_DATA; + } else { + iproc->state = UAVTALK_STATE_CS; + } + + break; + + case UAVTALK_STATE_TIMESTAMP: + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + + iproc->timestamp += rxbyte << (8 * (iproc->rxCount++)); + + if (iproc->rxCount < 2) { + break; + } + + iproc->rxCount = 0; + + // If there is a payload get it, otherwise receive checksum + if (iproc->length > 0) { + iproc->state = UAVTALK_STATE_DATA; + } else { + iproc->state = UAVTALK_STATE_CS; + } + break; + + case UAVTALK_STATE_DATA: + + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + + connection->rxBuffer[iproc->rxCount++] = rxbyte; + if (iproc->rxCount < iproc->length) { + break; + } + + iproc->state = UAVTALK_STATE_CS; + iproc->rxCount = 0; + break; + + case UAVTALK_STATE_CS: + + // the CRC byte + if (rxbyte != iproc->cs) { // packet error - faulty CRC + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + break; + } + + if (iproc->rxPacketLength != (iproc->packet_size + 1)) { // packet error - mismatched packet size + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + break; + } + + connection->stats.rxObjectBytes += iproc->length; + connection->stats.rxObjects++; + + iproc->state = UAVTALK_STATE_COMPLETE; + break; + + default: + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + } + + // Done + return iproc->state; } - + /** * Process an byte from the telemetry stream. * \param[in] connection UAVTalkConnection to be used @@ -542,20 +531,19 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle */ UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) { - UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte); + UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte); - if (state == UAVTALK_STATE_COMPLETE) - { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); - UAVTalkInputProcessor *iproc = &connection->iproc; + if (state == UAVTALK_STATE_COMPLETE) { + UAVTalkConnectionData *connection; + CHECKCONHANDLE(connectionHandle, connection, return -1); + UAVTalkInputProcessor *iproc = &connection->iproc; - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length); - xSemaphoreGiveRecursive(connection->lock); - } + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length); + xSemaphoreGiveRecursive(connection->lock); + } - return state; + return state; } /** @@ -570,59 +558,59 @@ UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uin */ UAVTalkRxState UAVTalkRelayInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) { - UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte); + UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte); - if (state == UAVTALK_STATE_COMPLETE) - { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); - UAVTalkInputProcessor *iproc = &connection->iproc; + if (state == UAVTALK_STATE_COMPLETE) { + UAVTalkConnectionData *connection; + CHECKCONHANDLE(connectionHandle, connection, return -1); + UAVTalkInputProcessor *iproc = &connection->iproc; - if (!connection->outStream) return -1; + if (!connection->outStream) { + return -1; + } - // Setup type and object id fields - connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte - connection->txBuffer[1] = iproc->type; - // data length inserted here below - connection->txBuffer[4] = (uint8_t)(iproc->objId & 0xFF); - connection->txBuffer[5] = (uint8_t)((iproc->objId >> 8) & 0xFF); - connection->txBuffer[6] = (uint8_t)((iproc->objId >> 16) & 0xFF); - connection->txBuffer[7] = (uint8_t)((iproc->objId >> 24) & 0xFF); - - // Setup instance ID if one is required - int32_t dataOffset = 8; - if (iproc->instanceLength > 0) - { - connection->txBuffer[8] = (uint8_t)(iproc->instId & 0xFF); - connection->txBuffer[9] = (uint8_t)((iproc->instId >> 8) & 0xFF); - dataOffset = 10; - } + // Setup type and object id fields + connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte + connection->txBuffer[1] = iproc->type; + // data length inserted here below + connection->txBuffer[4] = (uint8_t)(iproc->objId & 0xFF); + connection->txBuffer[5] = (uint8_t)((iproc->objId >> 8) & 0xFF); + connection->txBuffer[6] = (uint8_t)((iproc->objId >> 16) & 0xFF); + connection->txBuffer[7] = (uint8_t)((iproc->objId >> 24) & 0xFF); - // Add timestamp when the transaction type is appropriate - if (iproc->type & UAVTALK_TIMESTAMPED) - { - portTickType time = xTaskGetTickCount(); - connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); - connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); - dataOffset += 2; - } - - // Copy data (if any) - memcpy(&connection->txBuffer[dataOffset], connection->rxBuffer, iproc->length); - - // Store the packet length - connection->txBuffer[2] = (uint8_t)((dataOffset + iproc->length) & 0xFF); - connection->txBuffer[3] = (uint8_t)(((dataOffset + iproc->length) >> 8) & 0xFF); - - // Copy the checksum - connection->txBuffer[dataOffset + iproc->length] = iproc->cs; + // Setup instance ID if one is required + int32_t dataOffset = 8; + if (iproc->instanceLength > 0) { + connection->txBuffer[8] = (uint8_t)(iproc->instId & 0xFF); + connection->txBuffer[9] = (uint8_t)((iproc->instId >> 8) & 0xFF); + dataOffset = 10; + } - // Send the buffer. - if (UAVTalkSendBuf(connectionHandle, connection->txBuffer, iproc->rxPacketLength) < 0) - return UAVTALK_STATE_ERROR; - } + // Add timestamp when the transaction type is appropriate + if (iproc->type & UAVTALK_TIMESTAMPED) { + portTickType time = xTaskGetTickCount(); + connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); + connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); + dataOffset += 2; + } - return state; + // Copy data (if any) + memcpy(&connection->txBuffer[dataOffset], connection->rxBuffer, iproc->length); + + // Store the packet length + connection->txBuffer[2] = (uint8_t)((dataOffset + iproc->length) & 0xFF); + connection->txBuffer[3] = (uint8_t)(((dataOffset + iproc->length) >> 8) & 0xFF); + + // Copy the checksum + connection->txBuffer[dataOffset + iproc->length] = iproc->cs; + + // Send the buffer. + if (UAVTalkSendBuf(connectionHandle, connection->txBuffer, iproc->rxPacketLength) < 0) { + return UAVTALK_STATE_ERROR; + } + } + + return state; } /** @@ -634,18 +622,19 @@ UAVTalkRxState UAVTalkRelayInputStream(UAVTalkConnection connectionHandle, uint8 */ int32_t UAVTalkSendAck(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); + UAVTalkConnectionData *connection; - // Lock - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + CHECKCONHANDLE(connectionHandle, connection, return -1); - int32_t ret = sendObject(connection, obj, instId, UAVTALK_TYPE_ACK); - - // Release lock - xSemaphoreGiveRecursive(connection->lock); + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - return ret; + int32_t ret = sendObject(connection, obj, instId, UAVTALK_TYPE_ACK); + + // Release lock + xSemaphoreGiveRecursive(connection->lock); + + return ret; } /** @@ -657,18 +646,19 @@ int32_t UAVTalkSendAck(UAVTalkConnection connectionHandle, UAVObjHandle obj, uin */ int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection,return -1); + UAVTalkConnectionData *connection; - // Lock - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + CHECKCONHANDLE(connectionHandle, connection, return -1); - int32_t ret = sendNack(connection, objId); - - // Release lock - xSemaphoreGiveRecursive(connection->lock); + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - return ret; + int32_t ret = sendNack(connection, objId); + + // Release lock + xSemaphoreGiveRecursive(connection->lock); + + return ret; } /** @@ -682,25 +672,27 @@ int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId) */ int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_t len) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle,connection, return -1); + UAVTalkConnectionData *connection; - // Lock - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + CHECKCONHANDLE(connectionHandle, connection, return -1); - // Output the buffer - int32_t rc = (*connection->outStream)(buf, len); + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - // Update stats - connection->stats.txBytes += len; + // Output the buffer + int32_t rc = (*connection->outStream)(buf, len); - // Release lock - xSemaphoreGiveRecursive(connection->lock); + // Update stats + connection->stats.txBytes += len; - // Done - if (rc != len) - return -1; - return 0; + // Release lock + xSemaphoreGiveRecursive(connection->lock); + + // Done + if (rc != len) { + return -1; + } + return 0; } /** @@ -715,84 +707,73 @@ int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_ * \return -1 Failure */ static int32_t receiveObject(UAVTalkConnectionData *connection, - uint8_t type, - uint32_t objId, - uint16_t instId, - uint8_t* data, - __attribute__((unused))int32_t length) + uint8_t type, + uint32_t objId, + uint16_t instId, + uint8_t *data, + __attribute__((unused)) int32_t length) { - UAVObjHandle obj; - int32_t ret = 0; + UAVObjHandle obj; + int32_t ret = 0; - // Get the handle to the Object. Will be zero - // if object does not exist. - obj = UAVObjGetByID(objId); - - // Process message type - switch (type) { - case UAVTALK_TYPE_OBJ: - case UAVTALK_TYPE_OBJ_TS: - // All instances, not allowed for OBJ messages - if (obj && (instId != UAVOBJ_ALL_INSTANCES)) - { - // Unpack object, if the instance does not exist it will be created! - UAVObjUnpack(obj, instId, data); - // Check if an ack is pending - updateAck(connection, obj, instId); - } - else - { - ret = -1; - } - break; - case UAVTALK_TYPE_OBJ_ACK: - case UAVTALK_TYPE_OBJ_ACK_TS: - // All instances, not allowed for OBJ_ACK messages - if (obj && (instId != UAVOBJ_ALL_INSTANCES)) - { - // Unpack object, if the instance does not exist it will be created! - if ( UAVObjUnpack(obj, instId, data) == 0 ) - { - // Transmit ACK - sendObject(connection, obj, instId, UAVTALK_TYPE_ACK); - } - else - { - ret = -1; - } - } - else - { - ret = -1; - } - break; - case UAVTALK_TYPE_OBJ_REQ: - // Send requested object if message is of type OBJ_REQ - if (obj == 0) - sendNack(connection, objId); - else - sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ); - break; - case UAVTALK_TYPE_NACK: - // Do nothing on flight side, let it time out. - break; - case UAVTALK_TYPE_ACK: - // All instances, not allowed for ACK messages - if (obj && (instId != UAVOBJ_ALL_INSTANCES)) - { - // Check if an ack is pending - updateAck(connection, obj, instId); - } - else - { - ret = -1; - } - break; - default: - ret = -1; - } - // Done - return ret; + // Get the handle to the Object. Will be zero + // if object does not exist. + obj = UAVObjGetByID(objId); + + // Process message type + switch (type) { + case UAVTALK_TYPE_OBJ: + case UAVTALK_TYPE_OBJ_TS: + // All instances, not allowed for OBJ messages + if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { + // Unpack object, if the instance does not exist it will be created! + UAVObjUnpack(obj, instId, data); + // Check if an ack is pending + updateAck(connection, obj, instId); + } else { + ret = -1; + } + break; + case UAVTALK_TYPE_OBJ_ACK: + case UAVTALK_TYPE_OBJ_ACK_TS: + // All instances, not allowed for OBJ_ACK messages + if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { + // Unpack object, if the instance does not exist it will be created! + if (UAVObjUnpack(obj, instId, data) == 0) { + // Transmit ACK + sendObject(connection, obj, instId, UAVTALK_TYPE_ACK); + } else { + ret = -1; + } + } else { + ret = -1; + } + break; + case UAVTALK_TYPE_OBJ_REQ: + // Send requested object if message is of type OBJ_REQ + if (obj == 0) { + sendNack(connection, objId); + } else { + sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ); + } + break; + case UAVTALK_TYPE_NACK: + // Do nothing on flight side, let it time out. + break; + case UAVTALK_TYPE_ACK: + // All instances, not allowed for ACK messages + if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { + // Check if an ack is pending + updateAck(connection, obj, instId); + } else { + ret = -1; + } + break; + default: + ret = -1; + } + // Done + return ret; } /** @@ -803,11 +784,10 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, */ static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId) { - if (connection->respObj == obj && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES)) - { - xSemaphoreGive(connection->respSema); - connection->respObj = 0; - } + if (connection->respObj == obj && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES)) { + xSemaphoreGive(connection->respSema); + connection->respObj = 0; + } } /** @@ -821,53 +801,38 @@ static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint1 */ static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type) { - uint32_t numInst; - uint32_t n; - - // If all instances are requested and this is a single instance object, force instance ID to zero - if ( instId == UAVOBJ_ALL_INSTANCES && UAVObjIsSingleInstance(obj) ) - { - instId = 0; - } - - // Process message type - if ( type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS || type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS ) - { - if (instId == UAVOBJ_ALL_INSTANCES) - { - // Get number of instances - numInst = UAVObjGetNumInstances(obj); - // Send all instances - for (n = 0; n < numInst; ++n) - { - sendSingleObject(connection, obj, n, type); - } - return 0; - } - else - { - return sendSingleObject(connection, obj, instId, type); - } - } - else if (type == UAVTALK_TYPE_OBJ_REQ) - { - return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ); - } - else if (type == UAVTALK_TYPE_ACK) - { - if ( instId != UAVOBJ_ALL_INSTANCES ) - { - return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_ACK); - } - else - { - return -1; - } - } - else - { - return -1; - } + uint32_t numInst; + uint32_t n; + + // If all instances are requested and this is a single instance object, force instance ID to zero + if (instId == UAVOBJ_ALL_INSTANCES && UAVObjIsSingleInstance(obj)) { + instId = 0; + } + + // Process message type + if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS || type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS) { + if (instId == UAVOBJ_ALL_INSTANCES) { + // Get number of instances + numInst = UAVObjGetNumInstances(obj); + // Send all instances + for (n = 0; n < numInst; ++n) { + sendSingleObject(connection, obj, n, type); + } + return 0; + } else { + return sendSingleObject(connection, obj, instId, type); + } + } else if (type == UAVTALK_TYPE_OBJ_REQ) { + return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ); + } else if (type == UAVTALK_TYPE_ACK) { + if (instId != UAVOBJ_ALL_INSTANCES) { + return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_ACK); + } else { + return -1; + } + } else { + return -1; + } } /** @@ -881,87 +846,79 @@ static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, u */ static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type) { - int32_t length; - int32_t dataOffset; - uint32_t objId; + int32_t length; + int32_t dataOffset; + uint32_t objId; - if (!connection->outStream) return -1; + if (!connection->outStream) { + return -1; + } - // Setup type and object id fields - objId = UAVObjGetID(obj); - connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte - connection->txBuffer[1] = type; - // data length inserted here below - connection->txBuffer[4] = (uint8_t)(objId & 0xFF); - connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); - connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); - connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); - - // Setup instance ID if one is required - if (UAVObjIsSingleInstance(obj)) - { - dataOffset = 8; - } - else - { - connection->txBuffer[8] = (uint8_t)(instId & 0xFF); - connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF); - dataOffset = 10; - } + // Setup type and object id fields + objId = UAVObjGetID(obj); + connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte + connection->txBuffer[1] = type; + // data length inserted here below + connection->txBuffer[4] = (uint8_t)(objId & 0xFF); + connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); + connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); + connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); - // Add timestamp when the transaction type is appropriate - if (type & UAVTALK_TIMESTAMPED) - { - portTickType time = xTaskGetTickCount(); - connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); - connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); - dataOffset += 2; - } - - // Determine data length - if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK) - { - length = 0; - } - else - { - length = UAVObjGetNumBytes(obj); - } - - // Check length - if (length >= UAVTALK_MAX_PAYLOAD_LENGTH) - { - return -1; - } - - // Copy data (if any) - if (length > 0) - { - if ( UAVObjPack(obj, instId, &connection->txBuffer[dataOffset]) < 0 ) - { - return -1; - } - } - - // Store the packet length - connection->txBuffer[2] = (uint8_t)((dataOffset+length) & 0xFF); - connection->txBuffer[3] = (uint8_t)(((dataOffset+length) >> 8) & 0xFF); - - // Calculate checksum - connection->txBuffer[dataOffset+length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset+length); + // Setup instance ID if one is required + if (UAVObjIsSingleInstance(obj)) { + dataOffset = 8; + } else { + connection->txBuffer[8] = (uint8_t)(instId & 0xFF); + connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF); + dataOffset = 10; + } - uint16_t tx_msg_len = dataOffset+length+UAVTALK_CHECKSUM_LENGTH; - int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len); + // Add timestamp when the transaction type is appropriate + if (type & UAVTALK_TIMESTAMPED) { + portTickType time = xTaskGetTickCount(); + connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); + connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); + dataOffset += 2; + } - if (rc == tx_msg_len) { - // Update stats - ++connection->stats.txObjects; - connection->stats.txBytes += tx_msg_len; - connection->stats.txObjectBytes += length; - } - - // Done - return 0; + // Determine data length + if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK) { + length = 0; + } else { + length = UAVObjGetNumBytes(obj); + } + + // Check length + if (length >= UAVTALK_MAX_PAYLOAD_LENGTH) { + return -1; + } + + // Copy data (if any) + if (length > 0) { + if (UAVObjPack(obj, instId, &connection->txBuffer[dataOffset]) < 0) { + return -1; + } + } + + // Store the packet length + connection->txBuffer[2] = (uint8_t)((dataOffset + length) & 0xFF); + connection->txBuffer[3] = (uint8_t)(((dataOffset + length) >> 8) & 0xFF); + + // Calculate checksum + connection->txBuffer[dataOffset + length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset + length); + + uint16_t tx_msg_len = dataOffset + length + UAVTALK_CHECKSUM_LENGTH; + int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len); + + if (rc == tx_msg_len) { + // Update stats + ++connection->stats.txObjects; + connection->stats.txBytes += tx_msg_len; + connection->stats.txObjectBytes += length; + } + + // Done + return 0; } /** @@ -973,37 +930,39 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle */ static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId) { - int32_t dataOffset; + int32_t dataOffset; - if (!connection->outStream) return -1; + if (!connection->outStream) { + return -1; + } - connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte - connection->txBuffer[1] = UAVTALK_TYPE_NACK; - // data length inserted here below - connection->txBuffer[4] = (uint8_t)(objId & 0xFF); - connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); - connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); - connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); + connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte + connection->txBuffer[1] = UAVTALK_TYPE_NACK; + // data length inserted here below + connection->txBuffer[4] = (uint8_t)(objId & 0xFF); + connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); + connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); + connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); - dataOffset = 8; + dataOffset = 8; - // Store the packet length - connection->txBuffer[2] = (uint8_t)((dataOffset) & 0xFF); - connection->txBuffer[3] = (uint8_t)(((dataOffset) >> 8) & 0xFF); + // Store the packet length + connection->txBuffer[2] = (uint8_t)((dataOffset) & 0xFF); + connection->txBuffer[3] = (uint8_t)(((dataOffset) >> 8) & 0xFF); - // Calculate checksum - connection->txBuffer[dataOffset] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset); + // Calculate checksum + connection->txBuffer[dataOffset] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset); - uint16_t tx_msg_len = dataOffset+UAVTALK_CHECKSUM_LENGTH; - int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len); + uint16_t tx_msg_len = dataOffset + UAVTALK_CHECKSUM_LENGTH; + int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len); - if (rc == tx_msg_len) { - // Update stats - connection->stats.txBytes += tx_msg_len; - } + if (rc == tx_msg_len) { + // Update stats + connection->stats.txBytes += tx_msg_len; + } - // Done - return 0; + // Done + return 0; } /** diff --git a/ground/openpilotgcs/src/app/gcssplashscreen.cpp b/ground/openpilotgcs/src/app/gcssplashscreen.cpp index 568f2c683..84a0c4245 100644 --- a/ground/openpilotgcs/src/app/gcssplashscreen.cpp +++ b/ground/openpilotgcs/src/app/gcssplashscreen.cpp @@ -35,6 +35,7 @@ GCSSplashScreen::GCSSplashScreen() : { QString revision; QString year; + #ifdef GCS_REVISION revision = GCS_REVISION; #else @@ -48,7 +49,7 @@ GCSSplashScreen::GCSSplashScreen() : #endif setWindowFlags(windowFlags()); - m_pixmap = new QPixmap(":/app/splash.png"); + m_pixmap = new QPixmap(":/app/splash.png"); m_painter = new QPainter(m_pixmap); m_painter->setPen(Qt::lightGray); @@ -58,19 +59,19 @@ GCSSplashScreen::GCSSplashScreen() : QString(" 2010-") + year + QString(tr(" The OpenPilot Project - All Rights Reserved"))); - m_painter->drawText(406, 173, 310, 100, Qt::TextWordWrap|Qt::AlignTop|Qt::AlignLeft, + m_painter->drawText(406, 173, 310, 100, Qt::TextWordWrap | Qt::AlignTop | Qt::AlignLeft, QString(tr("GCS Revision - ")) + revision); setPixmap(*m_pixmap); } GCSSplashScreen::~GCSSplashScreen() -{ -} +{} void GCSSplashScreen::drawMessageText(const QString &message) { QPixmap pix(*m_pixmap); QPainter progressPainter(&pix); + progressPainter.setPen(Qt::lightGray); QFont font("Tahoma", 13); progressPainter.setFont(font); @@ -79,7 +80,8 @@ void GCSSplashScreen::drawMessageText(const QString &message) } void GCSSplashScreen::showPluginLoadingProgress(ExtensionSystem::PluginSpec *pluginSpec) -{ +{ QString message(tr("Loading ") + pluginSpec->name() + " plugin..."); + drawMessageText(message); } diff --git a/ground/openpilotgcs/src/app/gcssplashscreen.h b/ground/openpilotgcs/src/app/gcssplashscreen.h index 6c04e2367..bcf1ee9ad 100644 --- a/ground/openpilotgcs/src/app/gcssplashscreen.h +++ b/ground/openpilotgcs/src/app/gcssplashscreen.h @@ -35,22 +35,23 @@ #include "../gcs_version_info.h" -class GCSSplashScreen : public QSplashScreen -{ +class GCSSplashScreen : public QSplashScreen { Q_OBJECT public: explicit GCSSplashScreen(); ~GCSSplashScreen(); - + public slots: void showPluginLoadingProgress(ExtensionSystem::PluginSpec *pluginSpec); - void showProgressMessage(const QString &message) { drawMessageText(message); } + void showProgressMessage(const QString &message) + { + drawMessageText(message); + } private: QPixmap *m_pixmap; QPainter *m_painter; void drawMessageText(const QString &message); - }; #endif // GCSSPLASHSCREEN_H diff --git a/ground/openpilotgcs/src/app/main.cpp b/ground/openpilotgcs/src/app/main.cpp index a9183f0af..f58f46077 100644 --- a/ground/openpilotgcs/src/app/main.cpp +++ b/ground/openpilotgcs/src/app/main.cpp @@ -4,76 +4,76 @@ * @file main.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ /* - The GCS locale is set to the system locale by default unless the "hidden" setting General/Locale has a value. - The user can not change General/Locale from the Options dialog. + The GCS locale is set to the system locale by default unless the "hidden" setting General/Locale has a value. + The user can not change General/Locale from the Options dialog. - The GCS language will default to the GCS locale unless the General/OverrideLanguage has a value. - The user can change General/OverrideLanguage to any available language from the Options dialog. + The GCS language will default to the GCS locale unless the General/OverrideLanguage has a value. + The user can change General/OverrideLanguage to any available language from the Options dialog. - Both General/Locale and General/OverrideLanguage can be set from the command line or through the the factory defaults file. + Both General/Locale and General/OverrideLanguage can be set from the command line or through the the factory defaults file. - The -D option is used to permanently set a user setting. + The -D option is used to permanently set a user setting. - The -reset switch will clear all the user settings and will trigger a reload of the factory defaults. + The -reset switch will clear all the user settings and will trigger a reload of the factory defaults. - You can combine it with the -config-file= command line argument to quickly switch between multiple settings files. + You can combine it with the -config-file= command line argument to quickly switch between multiple settings files. - [code] - openpilotgcs -reset -config-file ./MyOpenPilotGCS.xml - [/code] + [code] + openpilotgcs -reset -config-file ./MyOpenPilotGCS.xml + [/code] - Relative paths are relative to /share/openpilotgcs/default_configurations/ + Relative paths are relative to /share/openpilotgcs/default_configurations/ - The specified file will be used to load the factory defaults from but only when the user settings are empty. - If the user settings are not empty the file will not be used. - This switch is useful on the 1st run when the user settings are empty or in combination with -reset. + The specified file will be used to load the factory defaults from but only when the user settings are empty. + If the user settings are not empty the file will not be used. + This switch is useful on the 1st run when the user settings are empty or in combination with -reset. - Quickly switch configurations + Quickly switch configurations - [code] - openpilotgcs -reset -config-file - [/code] + [code] + openpilotgcs -reset -config-file + [/code] - Configuring GCS from installer + Configuring GCS from installer - The -D option is used to permanently set a user setting. + The -D option is used to permanently set a user setting. - If the user chooses to start GCS at the end of the installer: + If the user chooses to start GCS at the end of the installer: - [code] - openpilotgcs -D General/OverrideLanguage=de - [/code] + [code] + openpilotgcs -D General/OverrideLanguage=de + [/code] - If the user chooses not to start GCS at the end of the installer, you still need to configure GCS. - In that case you can use -exit-after-config + If the user chooses not to start GCS at the end of the installer, you still need to configure GCS. + In that case you can use -exit-after-config - [code] - openpilotgcs -D General/OverrideLanguage=de -exit-after-config - [/code] + [code] + openpilotgcs -D General/OverrideLanguage=de -exit-after-config + [/code] */ @@ -102,307 +102,314 @@ #include namespace { +typedef QList PluginSpecSet; +typedef QMap AppOptions; +typedef QMap AppOptionValues; - typedef QList PluginSpecSet; - typedef QMap AppOptions; - typedef QMap AppOptionValues; +const int OptionIndent = 4; +const int DescriptionIndent = 24; - const int OptionIndent = 4; - const int DescriptionIndent = 24; +const QLatin1String APP_NAME("OpenPilot GCS"); - const QLatin1String APP_NAME("OpenPilot GCS"); +const QLatin1String CORE_PLUGIN_NAME("Core"); - const QLatin1String CORE_PLUGIN_NAME("Core"); - - const QLatin1String SETTINGS_ORG_NAME("OpenPilot"); - const QLatin1String SETTINGS_APP_NAME("OpenPilotGCS_config"); +const QLatin1String SETTINGS_ORG_NAME("OpenPilot"); +const QLatin1String SETTINGS_APP_NAME("OpenPilotGCS_config"); #ifdef Q_OS_MAC - const QLatin1String SHARE_PATH("/../Resources"); +const QLatin1String SHARE_PATH("/../Resources"); #else - const QLatin1String SHARE_PATH("/../share/openpilotgcs"); +const QLatin1String SHARE_PATH("/../share/openpilotgcs"); #endif - const char *DEFAULT_CONFIG_FILENAME = "OpenPilotGCS.xml"; +const char *DEFAULT_CONFIG_FILENAME = "OpenPilotGCS.xml"; - const char *fixedOptionsC = " [OPTION]... [FILE]...\n" - "Options:\n" - " -help Display this help\n" - " -version Display application version\n" - " -no-splash Don't display splash screen\n" - " -client Attempt to connect to already running instance\n" - " -D = Permanently set a user setting, e.g: -D General/OverrideLanguage=de\n" - " -reset Reset user settings to factory defaults.\n" - " -config-file Specify alternate factory defaults settings file (used with -reset)\n" - " -exit-after-config Exit after manipulating configuration settings\n"; +const char *fixedOptionsC = " [OPTION]... [FILE]...\n" + "Options:\n" + " -help Display this help\n" + " -version Display application version\n" + " -no-splash Don't display splash screen\n" + " -client Attempt to connect to already running instance\n" + " -D = Permanently set a user setting, e.g: -D General/OverrideLanguage=de\n" + " -reset Reset user settings to factory defaults.\n" + " -config-file Specify alternate factory defaults settings file (used with -reset)\n" + " -exit-after-config Exit after manipulating configuration settings\n"; - const QLatin1String HELP1_OPTION("-h"); - const QLatin1String HELP2_OPTION("-help"); - const QLatin1String HELP3_OPTION("/h"); - const QLatin1String HELP4_OPTION("--help"); - const QLatin1String VERSION_OPTION("-version"); - const QLatin1String NO_SPLASH_OPTION("-no-splash"); - const QLatin1String CLIENT_OPTION("-client"); - const QLatin1String CONFIG_OPTION("-D"); - const QLatin1String RESET_OPTION("-reset"); - const QLatin1String CONFIG_FILE_OPTION("-config-file"); - const QLatin1String EXIT_AFTER_CONFIG_OPTION("-exit-after-config"); +const QLatin1String HELP1_OPTION("-h"); +const QLatin1String HELP2_OPTION("-help"); +const QLatin1String HELP3_OPTION("/h"); +const QLatin1String HELP4_OPTION("--help"); +const QLatin1String VERSION_OPTION("-version"); +const QLatin1String NO_SPLASH_OPTION("-no-splash"); +const QLatin1String CLIENT_OPTION("-client"); +const QLatin1String CONFIG_OPTION("-D"); +const QLatin1String RESET_OPTION("-reset"); +const QLatin1String CONFIG_FILE_OPTION("-config-file"); +const QLatin1String EXIT_AFTER_CONFIG_OPTION("-exit-after-config"); - // Helpers for displaying messages. Note that there is no console on Windows. - void displayHelpText(QString t) - { +// Helpers for displaying messages. Note that there is no console on Windows. +void displayHelpText(QString t) +{ #ifdef Q_OS_WIN - // No console on Windows. (???) - // TODO there is a console on windows and popups are not always desired - t.replace(QLatin1Char('&'), QLatin1String("&")); - t.replace(QLatin1Char('<'), QLatin1String("<")); - t.replace(QLatin1Char('>'), QLatin1String(">")); - t.insert(0, QLatin1String("
"));
-        t.append(QLatin1String("
")); - QMessageBox::information(0, APP_NAME, t); + // No console on Windows. (???) + // TODO there is a console on windows and popups are not always desired + t.replace(QLatin1Char('&'), QLatin1String("&")); + t.replace(QLatin1Char('<'), QLatin1String("<")); + t.replace(QLatin1Char('>'), QLatin1String(">")); + t.insert(0, QLatin1String("
"));
+    t.append(QLatin1String("
")); + QMessageBox::information(0, APP_NAME, t); #else - qWarning("%s", qPrintable(t)); + qWarning("%s", qPrintable(t)); #endif - } +} - void displayError(const QString &t) - { +void displayError(const QString &t) +{ #ifdef Q_OS_WIN - // No console on Windows. (???) - // TODO there is a console on windows and popups are not always desired - QMessageBox::critical(0, APP_NAME, t); + // No console on Windows. (???) + // TODO there is a console on windows and popups are not always desired + QMessageBox::critical(0, APP_NAME, t); #else - qCritical("%s", qPrintable(t)); + qCritical("%s", qPrintable(t)); #endif - } +} - void printVersion(const ExtensionSystem::PluginSpec *corePlugin, const ExtensionSystem::PluginManager &pm) - { - QString version; - QTextStream str(&version); - str << '\n' << APP_NAME << ' ' << corePlugin->version() << " based on Qt " << qVersion() << "\n\n"; - pm.formatPluginVersions(str); - str << '\n' << corePlugin->copyright() << '\n'; - displayHelpText(version); - } +void printVersion(const ExtensionSystem::PluginSpec *corePlugin, const ExtensionSystem::PluginManager &pm) +{ + QString version; + QTextStream str(&version); - void printHelp(const QString &appExecName, const ExtensionSystem::PluginManager &pm) - { - QString help; - QTextStream str(&help); - str << "Usage: " << appExecName << fixedOptionsC; - ExtensionSystem::PluginManager::formatOptions(str, OptionIndent, DescriptionIndent); - pm.formatPluginOptions(str, OptionIndent, DescriptionIndent); - displayHelpText(help); - } + str << '\n' << APP_NAME << ' ' << corePlugin->version() << " based on Qt " << qVersion() << "\n\n"; + pm.formatPluginVersions(str); + str << '\n' << corePlugin->copyright() << '\n'; + displayHelpText(version); +} - inline QString msgCoreLoadFailure(const QString &reason) - { - return QCoreApplication::translate("Application", "Failed to load core plug-in, reason is: %1").arg(reason); - } +void printHelp(const QString &appExecName, const ExtensionSystem::PluginManager &pm) +{ + QString help; + QTextStream str(&help); - inline QString msgSendArgumentFailed() - { - return QCoreApplication::translate("Application", - "Unable to send command line arguments to the already running instance. It appears to be not responding."); - } + str << "Usage: " << appExecName << fixedOptionsC; + ExtensionSystem::PluginManager::formatOptions(str, OptionIndent, DescriptionIndent); + pm.formatPluginOptions(str, OptionIndent, DescriptionIndent); + displayHelpText(help); +} - // Prepare a remote argument: If it is a relative file, add the current directory - // since the the central instance might be running in a different directory. - inline QString prepareRemoteArgument(const QString &arg) - { - QFileInfo fi(arg); - if (!fi.exists()) { - return arg; - } - if (fi.isRelative()) { - return fi.absoluteFilePath(); - } +inline QString msgCoreLoadFailure(const QString &reason) +{ + return QCoreApplication::translate("Application", "Failed to load core plug-in, reason is: %1").arg(reason); +} + +inline QString msgSendArgumentFailed() +{ + return QCoreApplication::translate("Application", + "Unable to send command line arguments to the already running instance. It appears to be not responding."); +} + +// Prepare a remote argument: If it is a relative file, add the current directory +// since the the central instance might be running in a different directory. +inline QString prepareRemoteArgument(const QString &arg) +{ + QFileInfo fi(arg); + + if (!fi.exists()) { return arg; } + if (fi.isRelative()) { + return fi.absoluteFilePath(); + } + return arg; +} - // Send the arguments to an already running instance of application - bool sendArguments(SharedTools::QtSingleApplication &app, const QStringList &arguments) - { - if (!arguments.empty()) { - // Send off arguments - const QStringList::const_iterator acend = arguments.constEnd(); - for (QStringList::const_iterator it = arguments.constBegin(); it != acend; ++it) { - if (!app.sendMessage(prepareRemoteArgument(*it))) { - displayError(msgSendArgumentFailed()); - return false; - } +// Send the arguments to an already running instance of application +bool sendArguments(SharedTools::QtSingleApplication &app, const QStringList &arguments) +{ + if (!arguments.empty()) { + // Send off arguments + const QStringList::const_iterator acend = arguments.constEnd(); + for (QStringList::const_iterator it = arguments.constBegin(); it != acend; ++it) { + if (!app.sendMessage(prepareRemoteArgument(*it))) { + displayError(msgSendArgumentFailed()); + return false; } } - // Special empty argument means: Show and raise (the slot just needs to be triggered) - if (!app.sendMessage(QString())) { - displayError(msgSendArgumentFailed()); - return false; - } - return true; } + // Special empty argument means: Show and raise (the slot just needs to be triggered) + if (!app.sendMessage(QString())) { + displayError(msgSendArgumentFailed()); + return false; + } + return true; +} - void systemInit() - { +void systemInit() +{ #ifdef Q_OS_MAC - // increase the number of file that can be opened in application - struct rlimit rl; - getrlimit(RLIMIT_NOFILE, &rl); - rl.rlim_cur = rl.rlim_max; - setrlimit(RLIMIT_NOFILE, &rl); + // increase the number of file that can be opened in application + struct rlimit rl; + getrlimit(RLIMIT_NOFILE, &rl); + rl.rlim_cur = rl.rlim_max; + setrlimit(RLIMIT_NOFILE, &rl); #endif #ifdef Q_OS_LINUX - QApplication::setAttribute(Qt::AA_X11InitThreads, true); + QApplication::setAttribute(Qt::AA_X11InitThreads, true); #endif +} + +inline QStringList getPluginPaths() +{ + QStringList rc; + // Figure out root: Up one from 'bin' + QDir rootDir = QApplication::applicationDirPath(); + + rootDir.cdUp(); + const QString rootDirPath = rootDir.canonicalPath(); + // 1) "plugins" (Win/Linux) + QString pluginPath = rootDirPath; + pluginPath += QLatin1Char('/'); + pluginPath += QLatin1String(GCS_LIBRARY_BASENAME); + pluginPath += QLatin1Char('/'); + pluginPath += QLatin1String("openpilotgcs"); + pluginPath += QLatin1Char('/'); + pluginPath += QLatin1String("plugins"); + rc.push_back(pluginPath); + // 2) "PlugIns" (OS X) + pluginPath = rootDirPath; + pluginPath += QLatin1Char('/'); + pluginPath += QLatin1String("Plugins"); + rc.push_back(pluginPath); + return rc; +} + +AppOptions options() +{ + AppOptions appOptions; + + appOptions.insert(HELP1_OPTION, false); + appOptions.insert(HELP2_OPTION, false); + appOptions.insert(HELP3_OPTION, false); + appOptions.insert(HELP4_OPTION, false); + appOptions.insert(VERSION_OPTION, false); + appOptions.insert(NO_SPLASH_OPTION, false); + appOptions.insert(CLIENT_OPTION, false); + appOptions.insert(CONFIG_OPTION, true); + appOptions.insert(RESET_OPTION, false); + appOptions.insert(CONFIG_FILE_OPTION, true); + appOptions.insert(EXIT_AFTER_CONFIG_OPTION, false); + return appOptions; +} + +AppOptionValues parseCommandLine(SharedTools::QtSingleApplication &app, + ExtensionSystem::PluginManager &pluginManager, QString &errorMessage) +{ + AppOptionValues appOptionValues; + const QStringList arguments = app.arguments(); + + if (arguments.size() > 1) { + AppOptions appOptions = options(); + pluginManager.parseOptions(arguments, appOptions, &appOptionValues, &errorMessage); + } + return appOptionValues; +} + +void loadFactoryDefaults(QSettings &settings, AppOptionValues &appOptionValues) +{ + QDir directory(QCoreApplication::applicationDirPath() + SHARE_PATH + QString("/default_configurations")); + + qDebug() << "Looking for factory defaults configuration files in:" << directory.absolutePath(); + + QString fileName; + + // check if command line option -config-file contains a file name + QString commandLine = appOptionValues.value(CONFIG_FILE_OPTION); + if (!commandLine.isEmpty()) { + QFileInfo fi(commandLine); + if (fi.isRelative()) { + // file name specified on command line has a relative path + commandLine = directory.absolutePath() + QDir::separator() + commandLine; + } + if (QFile::exists(commandLine)) { + fileName = commandLine; + qDebug() << "Configuration file" << fileName << "specified on command line will be loaded."; + } else { + qWarning() << "Configuration file" << commandLine << "specified on command line does not exist."; + } } - inline QStringList getPluginPaths() - { - QStringList rc; - // Figure out root: Up one from 'bin' - QDir rootDir = QApplication::applicationDirPath(); - rootDir.cdUp(); - const QString rootDirPath = rootDir.canonicalPath(); - // 1) "plugins" (Win/Linux) - QString pluginPath = rootDirPath; - pluginPath += QLatin1Char('/'); - pluginPath += QLatin1String(GCS_LIBRARY_BASENAME); - pluginPath += QLatin1Char('/'); - pluginPath += QLatin1String("openpilotgcs"); - pluginPath += QLatin1Char('/'); - pluginPath += QLatin1String("plugins"); - rc.push_back(pluginPath); - // 2) "PlugIns" (OS X) - pluginPath = rootDirPath; - pluginPath += QLatin1Char('/'); - pluginPath += QLatin1String("Plugins"); - rc.push_back(pluginPath); - return rc; + if (fileName.isEmpty()) { + // check default file + if (QFile::exists(directory.absolutePath() + QDir::separator() + DEFAULT_CONFIG_FILENAME)) { + // use default file name + fileName = directory.absolutePath() + QDir::separator() + DEFAULT_CONFIG_FILENAME; + qDebug() << "Default configuration file" << fileName << "will be loaded."; + } else { + qWarning() << "No default configuration file found in" << directory.absolutePath(); + } } - AppOptions options() - { - AppOptions appOptions; - appOptions.insert(HELP1_OPTION, false); - appOptions.insert(HELP2_OPTION, false); - appOptions.insert(HELP3_OPTION, false); - appOptions.insert(HELP4_OPTION, false); - appOptions.insert(VERSION_OPTION, false); - appOptions.insert(NO_SPLASH_OPTION, false); - appOptions.insert(CLIENT_OPTION, false); - appOptions.insert(CONFIG_OPTION, true); - appOptions.insert(RESET_OPTION, false); - appOptions.insert(CONFIG_FILE_OPTION, true); - appOptions.insert(EXIT_AFTER_CONFIG_OPTION, false); - return appOptions; + if (fileName.isEmpty()) { + // TODO should we exit violently? + qCritical() << "No default configuration file found!"; + return; } - AppOptionValues parseCommandLine(SharedTools::QtSingleApplication &app, - ExtensionSystem::PluginManager &pluginManager, QString &errorMessage) - { - AppOptionValues appOptionValues; - const QStringList arguments = app.arguments(); - if (arguments.size() > 1) { - AppOptions appOptions = options(); - pluginManager.parseOptions(arguments, appOptions, &appOptionValues, &errorMessage); - } - return appOptionValues; + // create settings from file + QSettings qs(fileName, XmlConfig::XmlSettingsFormat); + + // transfer loaded settings to application settings + QStringList keys = qs.allKeys(); + foreach(QString key, keys) { + settings.setValue(key, qs.value(key)); } - void loadFactoryDefaults(QSettings &settings, AppOptionValues &appOptionValues) - { - QDir directory(QCoreApplication::applicationDirPath() + SHARE_PATH + QString("/default_configurations")); - qDebug() << "Looking for factory defaults configuration files in:" << directory.absolutePath(); + qDebug() << "Configuration file" << fileName << "was loaded."; +} - QString fileName; +void overrideSettings(QSettings &settings, int argc, char * *argv) +{ + // Options like -D My/setting=test + QRegExp rx("([^=]+)=(.*)"); - // check if command line option -config-file contains a file name - QString commandLine = appOptionValues.value(CONFIG_FILE_OPTION); - if (!commandLine.isEmpty()) { - QFileInfo fi(commandLine); - if (fi.isRelative()) { - // file name specified on command line has a relative path - commandLine = directory.absolutePath() + QDir::separator() + commandLine; - } - if (QFile::exists(commandLine)) { - fileName = commandLine; - qDebug() << "Configuration file" << fileName << "specified on command line will be loaded."; - } else { - qWarning() << "Configuration file" << commandLine << "specified on command line does not exist."; - } - } - - if (fileName.isEmpty()) { - // check default file - if (QFile::exists(directory.absolutePath() + QDir::separator() + DEFAULT_CONFIG_FILENAME)) { - // use default file name - fileName = directory.absolutePath() + QDir::separator() + DEFAULT_CONFIG_FILENAME; - qDebug() << "Default configuration file" << fileName << "will be loaded."; - } else { - qWarning() << "No default configuration file found in" << directory.absolutePath(); - } - } - - if (fileName.isEmpty()) { - // TODO should we exit violently? - qCritical() << "No default configuration file found!"; - return; - } - - // create settings from file - QSettings qs(fileName, XmlConfig::XmlSettingsFormat); - - // transfer loaded settings to application settings - QStringList keys = qs.allKeys(); - foreach(QString key, keys) { - settings.setValue(key, qs.value(key)); - } - - qDebug() << "Configuration file" << fileName << "was loaded."; - } - - void overrideSettings(QSettings &settings, int argc, char **argv) - { - // Options like -D My/setting=test - QRegExp rx("([^=]+)=(.*)"); - - for (int i = 0; i < argc; ++i) { - if (CONFIG_OPTION == QString(argv[i])) { - if (rx.indexIn(argv[++i]) > -1) { - QString key = rx.cap(1); - QString value = rx.cap(2); - qDebug() << "User setting" << key << "set to value" << value; - settings.setValue(key, value); - } - } - } - - settings.sync(); - } - - void loadTranslators(QString language, QTranslator &translator, QTranslator &qtTranslator) - { - const QString &creatorTrPath = QCoreApplication::applicationDirPath() + SHARE_PATH - + QLatin1String("/translations"); - if (translator.load(QLatin1String("openpilotgcs_") + language, creatorTrPath)) { - const QString &qtTrPath = QLibraryInfo::location(QLibraryInfo::TranslationsPath); - const QString &qtTrFile = QLatin1String("qt_") + language; - // Binary installer puts Qt tr files into creatorTrPath - if (qtTranslator.load(qtTrFile, qtTrPath) || qtTranslator.load(qtTrFile, creatorTrPath)) { - QCoreApplication::installTranslator(&translator); - QCoreApplication::installTranslator(&qtTranslator); - } else { - // unload() - translator.load(QString()); + for (int i = 0; i < argc; ++i) { + if (CONFIG_OPTION == QString(argv[i])) { + if (rx.indexIn(argv[++i]) > -1) { + QString key = rx.cap(1); + QString value = rx.cap(2); + qDebug() << "User setting" << key << "set to value" << value; + settings.setValue(key, value); } } } -} // namespace anonymous + settings.sync(); +} -int main(int argc, char **argv) +void loadTranslators(QString language, QTranslator &translator, QTranslator &qtTranslator) +{ + const QString &creatorTrPath = QCoreApplication::applicationDirPath() + SHARE_PATH + + QLatin1String("/translations"); + + if (translator.load(QLatin1String("openpilotgcs_") + language, creatorTrPath)) { + const QString &qtTrPath = QLibraryInfo::location(QLibraryInfo::TranslationsPath); + const QString &qtTrFile = QLatin1String("qt_") + language; + // Binary installer puts Qt tr files into creatorTrPath + if (qtTranslator.load(qtTrFile, qtTrPath) || qtTranslator.load(qtTrFile, creatorTrPath)) { + QCoreApplication::installTranslator(&translator); + QCoreApplication::installTranslator(&qtTranslator); + } else { + // unload() + translator.load(QString()); + } + } +} +} // namespace anonymous + +int main(int argc, char * *argv) { QElapsedTimer timer; + timer.start(); // low level init @@ -492,23 +499,23 @@ int main(int argc, char **argv) splash->showProgressMessage(QObject::tr("Application starting...")); splash->show(); // connect to track progress of plugin manager - QObject::connect(&pluginManager, SIGNAL(pluginAboutToBeLoaded(ExtensionSystem::PluginSpec*)), splash, - SLOT(showPluginLoadingProgress(ExtensionSystem::PluginSpec*))); + QObject::connect(&pluginManager, SIGNAL(pluginAboutToBeLoaded(ExtensionSystem::PluginSpec *)), splash, + SLOT(showPluginLoadingProgress(ExtensionSystem::PluginSpec *))); } // find and load core plugin const PluginSpecSet plugins = pluginManager.plugins(); ExtensionSystem::PluginSpec *coreplugin = 0; - foreach (ExtensionSystem::PluginSpec *spec, plugins) { + foreach(ExtensionSystem::PluginSpec * spec, plugins) { if (spec->name() == CORE_PLUGIN_NAME) { coreplugin = spec; break; } } if (!coreplugin) { - QString nativePaths = QDir::toNativeSeparators(getPluginPaths().join(QLatin1String(","))); + QString nativePaths = QDir::toNativeSeparators(getPluginPaths().join(QLatin1String(","))); const QString reason = QCoreApplication::translate("Application", "Could not find 'Core.pluginspec' in %1").arg( - nativePaths); + nativePaths); displayError(msgCoreLoadFailure(reason)); return 1; } @@ -522,7 +529,7 @@ int main(int argc, char **argv) return 0; } if (appOptionValues.contains(HELP1_OPTION) || appOptionValues.contains(HELP2_OPTION) - || appOptionValues.contains(HELP3_OPTION) || appOptionValues.contains(HELP4_OPTION)) { + || appOptionValues.contains(HELP3_OPTION) || appOptionValues.contains(HELP4_OPTION)) { printHelp(QFileInfo(app.applicationFilePath()).baseName(), pluginManager); return 0; } @@ -541,15 +548,15 @@ int main(int argc, char **argv) { QStringList errors; - foreach (ExtensionSystem::PluginSpec *p, pluginManager.plugins()) { + foreach(ExtensionSystem::PluginSpec * p, pluginManager.plugins()) { if (p->hasError()) { errors.append(p->errorString()); } } if (!errors.isEmpty()) { QMessageBox::warning(0, - QCoreApplication::translate("Application", "OpenPilot GCS - Plugin loader messages"), - errors.join(QString::fromLatin1("\n\n"))); + QCoreApplication::translate("Application", "OpenPilot GCS - Plugin loader messages"), + errors.join(QString::fromLatin1("\n\n"))); } } diff --git a/ground/openpilotgcs/src/experimental/HIDTest/main.cpp b/ground/openpilotgcs/src/experimental/HIDTest/main.cpp index e4c1b0cb9..6734ae38b 100644 --- a/ground/openpilotgcs/src/experimental/HIDTest/main.cpp +++ b/ground/openpilotgcs/src/experimental/HIDTest/main.cpp @@ -5,40 +5,40 @@ #define BUF_LEN 64 class MyThread : public QThread { +public: - public: - - void run() - { - qDebug() << "Hello"; - pjrc_rawhid hidHandle; - int numDevices = hidHandle.open(1,0x20a0,0x4117,0,0); //0xff9c,0x0001); - if( numDevices == 0 ) - numDevices = hidHandle.open(1,0x0483,0,0,0); - - qDebug() << numDevices << " device(s) opened"; - - //hidHandle.mytest(0); - - char buf[BUF_LEN]; - buf[0] = 2; - buf[1] = 0; - int result = hidHandle.send(0,buf, BUF_LEN, 500); - - qDebug() << result << " bytes sent"; - - int received = hidHandle.receive(0, buf, BUF_LEN, 3500); - - qDebug("%u bytes received. First value %x second %x", received,buf[0], buf[1]); + void run() + { + qDebug() << "Hello"; + pjrc_rawhid hidHandle; + int numDevices = hidHandle.open(1, 0x20a0, 0x4117, 0, 0); // 0xff9c,0x0001); + if (numDevices == 0) { + numDevices = hidHandle.open(1, 0x0483, 0, 0, 0); } - }; + qDebug() << numDevices << " device(s) opened"; + + // hidHandle.mytest(0); + + char buf[BUF_LEN]; + buf[0] = 2; + buf[1] = 0; + int result = hidHandle.send(0, buf, BUF_LEN, 500); + + qDebug() << result << " bytes sent"; + + int received = hidHandle.receive(0, buf, BUF_LEN, 3500); + + qDebug("%u bytes received. First value %x second %x", received, buf[0], buf[1]); + } +}; int main(int argc, char *argv[]) { QCoreApplication a(argc, argv); MyThread b; + b.start(); return a.exec(); diff --git a/ground/openpilotgcs/src/experimental/PowerLog6S/main.cpp b/ground/openpilotgcs/src/experimental/PowerLog6S/main.cpp index a95611270..5dbf9c4d8 100644 --- a/ground/openpilotgcs/src/experimental/PowerLog6S/main.cpp +++ b/ground/openpilotgcs/src/experimental/PowerLog6S/main.cpp @@ -31,197 +31,189 @@ using namespace std; -typedef unsigned long ULONG; // 4 Bytes +typedef unsigned long ULONG; // 4 Bytes typedef short SHORT; -typedef unsigned short USHORT; // 2 Bytes -typedef unsigned char BYTE; // 1 Byte -typedef unsigned short WORD; // 2 Bytes -typedef unsigned long DWORD; // 4 Bytes +typedef unsigned short USHORT; // 2 Bytes +typedef unsigned char BYTE; // 1 Byte +typedef unsigned short WORD; // 2 Bytes +typedef unsigned long DWORD; // 4 Bytes class MyParser : public QObject { + struct POWERLOG_HID_PACK { + BYTE Len; + BYTE Type; + DWORD Interval; + BYTE LogState; + SHORT Current; + USHORT Volt; + DWORD Cap; + SHORT Cell[6]; + USHORT RPM; + SHORT Temp[4]; + USHORT Period; + USHORT Pulse; + }; -struct POWERLOG_HID_PACK -{ - BYTE Len; - BYTE Type; - DWORD Interval; - BYTE LogState; - SHORT Current; - USHORT Volt; - DWORD Cap; - SHORT Cell[6]; - USHORT RPM; - SHORT Temp[4]; - USHORT Period; - USHORT Pulse; -}; - -enum -{ - TYPE_DATA_ONLINE = 0x10, - TYPE_DATA_OFFLINE = 0x11, - TYPE_ORDER = 0x20, -}; + enum { + TYPE_DATA_ONLINE = 0x10, + TYPE_DATA_OFFLINE = 0x11, + TYPE_ORDER = 0x20, + }; - public: +public: - void start() - { - qDebug() << "Connect a Junsi PowerLog 6S and watch the logging output"; - pjrc_rawhid hidHandle; - int numDevices = hidHandle.open(1, 0x0483,0x5750,0,0); //0xff9c,0x0001); - if( numDevices == 0 ) - numDevices = hidHandle.open(1,0x0483,0,0,0); - - qDebug() << numDevices << " device(s) opened"; - - //hidHandle.mytest(0); - - char buf[BUF_LEN]; - buf[0] = 2; - buf[1] = 0; - - cout << "Interval,Current,Volt,Cap,Cell1,Cell2,Cell3,Cell4,Cell5,Cell6,RPM,Temp1,Temp2,Temp3,Temp4,Period,Pulse" << endl; - - while (int received = hidHandle.receive(0, buf, BUF_LEN, 3500) ) { - ShowInf(buf); - } + void start() + { + qDebug() << "Connect a Junsi PowerLog 6S and watch the logging output"; + pjrc_rawhid hidHandle; + int numDevices = hidHandle.open(1, 0x0483, 0x5750, 0, 0); // 0xff9c,0x0001); + if (numDevices == 0) { + numDevices = hidHandle.open(1, 0x0483, 0, 0, 0); } + qDebug() << numDevices << " device(s) opened"; - void ShowInf(char *pBuf) - { - // qDebug() << "--------------------"; - POWERLOG_HID_PACK Inf; - int i; - int Count; - - Count=0; - Inf.Len = pBuf[Count]; - Count += sizeof(Inf.Len); - - Inf.Type = pBuf[Count]; - Count += sizeof(Inf.Type); + // hidHandle.mytest(0); - Inf.Interval = *((DWORD *)&pBuf[Count]); - printf("%ld,",Inf.Interval); + char buf[BUF_LEN]; + buf[0] = 2; + buf[1] = 0; - Count += sizeof(Inf.Interval); + cout << "Interval,Current,Volt,Cap,Cell1,Cell2,Cell3,Cell4,Cell5,Cell6,RPM,Temp1,Temp2,Temp3,Temp4,Period,Pulse" << endl; - Inf.LogState = pBuf[Count]; - Count += sizeof(Inf.LogState); - - if(((Inf.Type == TYPE_DATA_ONLINE)||(Inf.Type == TYPE_DATA_OFFLINE)) && (Inf.Len == 0x29))//0x27 - { - Inf.Current = *((SHORT *)&pBuf[Count]); - Count += sizeof(Inf.Current); - GetShowValue(QString("Current:"),Inf.Current,5,2); - - Inf.Volt = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.Volt); - GetShowValue(QString("Voltage:"),Inf.Volt,5,2); - - Inf.Cap = *((DWORD *)&pBuf[Count]); - Count += sizeof(Inf.Cap); - GetShowValue(QString("Cap:"),Inf.Cap,6,0); - - for(i=0;i<6;i++) - { - Inf.Cell[i] = *((SHORT *)&pBuf[Count]); - Count += sizeof(Inf.Cell[i]); - } - GetShowValue(QString("Cell 1:"),Inf.Cell[0],5,3); - GetShowValue(QString("Cell 2:"),Inf.Cell[1],5,3); - GetShowValue(QString("Cell 3:"),Inf.Cell[2],5,3); - GetShowValue(QString("Cell 4:"),Inf.Cell[3],5,3); - GetShowValue(QString("Cell 5:"),Inf.Cell[4],5,3); - GetShowValue(QString("Cell 6:"),Inf.Cell[5],5,3); - - Inf.RPM = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.RPM); - GetShowValue(QString("RPM:"),Inf.RPM,6,0); + while (int received = hidHandle.receive(0, buf, BUF_LEN, 3500)) { + ShowInf(buf); + } + } - for(i=0;i<4;i++) - { - Inf.Temp[i] = *((SHORT *)&pBuf[Count]); - Count += sizeof(Inf.Temp[i]); - - } - GetShowValue(QString("Int Temp1:"),Inf.Temp[0],4,1); - if(Inf.Temp[1]==0x7fff) - QString txtExtTemp1 = QString("NULL"); - else - GetShowValue(QString("Ext temp1:"),Inf.Temp[1],4,1); - if(Inf.Temp[2]==0x7fff) - QString txtExtTemp2 = QString("NULL"); - else - GetShowValue(QString("Ext temp2:"),Inf.Temp[2],4,1); - if(Inf.Temp[3]==0x7fff) - QString txtExtTemp3 = QString("NULL"); - else - GetShowValue(QString("Ext temp3:"),Inf.Temp[3],4,1); + void ShowInf(char *pBuf) + { + // qDebug() << "--------------------"; + POWERLOG_HID_PACK Inf; + int i; + int Count; - Inf.Period = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.Period); - GetShowValue(QString("Period:"),Inf.Period,6,0); + Count = 0; + Inf.Len = pBuf[Count]; + Count += sizeof(Inf.Len); - Inf.Pulse = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.Pulse); - GetShowValue(QString("Pulse:"),Inf.Pulse,6,0); + Inf.Type = pBuf[Count]; + Count += sizeof(Inf.Type); - printf("\n"); - } + Inf.Interval = *((DWORD *)&pBuf[Count]); + printf("%ld,", Inf.Interval); -} + Count += sizeof(Inf.Interval); -void GetShowValue(QString label,DWORD Value,WORD Len,WORD Dot) -{ - //cout << label .toStdString() << " "; + Inf.LogState = pBuf[Count]; + Count += sizeof(Inf.LogState); - if(Value<0) - { - Value = -Value; - if(Dot==1) - printf("-%ld.%01lu",Value/10,Value%10); - else if(Dot==2) - printf("-%ld.%02lu",Value/100,Value%100); - else if(Dot==3) - printf("-%ld.%03lu",Value/1000,Value%1000); - else if(Dot==4) - printf("-%ld.%04lu",Value/10000,Value%10000); - else - printf("-%ld",Value); - } - else - { - if(Dot==1) - printf("%ld.%01lu",Value/10,Value%10); - else if(Dot==2) - printf("%ld.%02lu",Value/100,Value%100); - else if(Dot==3) - printf("%ld.%03lu",Value/1000,Value%1000); - else if(Dot==4) - printf("%ld.%04lu",Value/10000,Value%10000); - else - printf("%ld",Value); - } - printf(","); + if (((Inf.Type == TYPE_DATA_ONLINE) || (Inf.Type == TYPE_DATA_OFFLINE)) && (Inf.Len == 0x29)) { // 0x27 + Inf.Current = *((SHORT *)&pBuf[Count]); + Count += sizeof(Inf.Current); + GetShowValue(QString("Current:"), Inf.Current, 5, 2); -} + Inf.Volt = *((USHORT *)&pBuf[Count]); + Count += sizeof(Inf.Volt); + GetShowValue(QString("Voltage:"), Inf.Volt, 5, 2); + + Inf.Cap = *((DWORD *)&pBuf[Count]); + Count += sizeof(Inf.Cap); + GetShowValue(QString("Cap:"), Inf.Cap, 6, 0); + + for (i = 0; i < 6; i++) { + Inf.Cell[i] = *((SHORT *)&pBuf[Count]); + Count += sizeof(Inf.Cell[i]); + } + GetShowValue(QString("Cell 1:"), Inf.Cell[0], 5, 3); + GetShowValue(QString("Cell 2:"), Inf.Cell[1], 5, 3); + GetShowValue(QString("Cell 3:"), Inf.Cell[2], 5, 3); + GetShowValue(QString("Cell 4:"), Inf.Cell[3], 5, 3); + GetShowValue(QString("Cell 5:"), Inf.Cell[4], 5, 3); + GetShowValue(QString("Cell 6:"), Inf.Cell[5], 5, 3); + + Inf.RPM = *((USHORT *)&pBuf[Count]); + Count += sizeof(Inf.RPM); + GetShowValue(QString("RPM:"), Inf.RPM, 6, 0); + for (i = 0; i < 4; i++) { + Inf.Temp[i] = *((SHORT *)&pBuf[Count]); + Count += sizeof(Inf.Temp[i]); + } + GetShowValue(QString("Int Temp1:"), Inf.Temp[0], 4, 1); + if (Inf.Temp[1] == 0x7fff) { + QString txtExtTemp1 = QString("NULL"); + } else { + GetShowValue(QString("Ext temp1:"), Inf.Temp[1], 4, 1); + } + if (Inf.Temp[2] == 0x7fff) { + QString txtExtTemp2 = QString("NULL"); + } else { + GetShowValue(QString("Ext temp2:"), Inf.Temp[2], 4, 1); + } + if (Inf.Temp[3] == 0x7fff) { + QString txtExtTemp3 = QString("NULL"); + } else { + GetShowValue(QString("Ext temp3:"), Inf.Temp[3], 4, 1); + } - }; + Inf.Period = *((USHORT *)&pBuf[Count]); + Count += sizeof(Inf.Period); + GetShowValue(QString("Period:"), Inf.Period, 6, 0); + + Inf.Pulse = *((USHORT *)&pBuf[Count]); + Count += sizeof(Inf.Pulse); + GetShowValue(QString("Pulse:"), Inf.Pulse, 6, 0); + + printf("\n"); + } + } + + void GetShowValue(QString label, DWORD Value, WORD Len, WORD Dot) + { + // cout << label .toStdString() << " "; + + if (Value < 0) { + Value = -Value; + if (Dot == 1) { + printf("-%ld.%01lu", Value / 10, Value % 10); + } else if (Dot == 2) { + printf("-%ld.%02lu", Value / 100, Value % 100); + } else if (Dot == 3) { + printf("-%ld.%03lu", Value / 1000, Value % 1000); + } else if (Dot == 4) { + printf("-%ld.%04lu", Value / 10000, Value % 10000); + } else { + printf("-%ld", Value); + } + } else { + if (Dot == 1) { + printf("%ld.%01lu", Value / 10, Value % 10); + } else if (Dot == 2) { + printf("%ld.%02lu", Value / 100, Value % 100); + } else if (Dot == 3) { + printf("%ld.%03lu", Value / 1000, Value % 1000); + } else if (Dot == 4) { + printf("%ld.%04lu", Value / 10000, Value % 10000); + } else { + printf("%ld", Value); + } + } + printf(","); + } +}; int main(int argc, char *argv[]) { QCoreApplication a(argc, argv); MyParser b; + b.start(); -// return a.exec(); +// return a.exec(); } diff --git a/ground/openpilotgcs/src/experimental/SerialLogger/main.cpp b/ground/openpilotgcs/src/experimental/SerialLogger/main.cpp index 03c91dfed..1d4e651c9 100644 --- a/ground/openpilotgcs/src/experimental/SerialLogger/main.cpp +++ b/ground/openpilotgcs/src/experimental/SerialLogger/main.cpp @@ -8,53 +8,49 @@ #include -class pollSerialPort : public QThread -{ +class pollSerialPort : public QThread { public: pollSerialPort(QString dn, QString fn) : device(dn), outFile(fn) - { - }; + {}; void run() { QByteArray dat; - //const char framingRaw[16] = {7,9,3,15,193,130,150,10,7,9,3,15,193,130,150,10}; - const char framingRaw[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15}; - QByteArray framing(framingRaw,16); + // const char framingRaw[16] = {7,9,3,15,193,130,150,10,7,9,3,15,193,130,150,10}; + const char framingRaw[16] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 }; + QByteArray framing(framingRaw, 16); PortSettings Settings; - Settings.BaudRate=BAUD115200; - Settings.DataBits=DATA_8; - Settings.Parity=PAR_NONE; - Settings.StopBits=STOP_1; - Settings.FlowControl=FLOW_OFF; - Settings.Timeout_Millisec=500; + + Settings.BaudRate = BAUD115200; + Settings.DataBits = DATA_8; + Settings.Parity = PAR_NONE; + Settings.StopBits = STOP_1; + Settings.FlowControl = FLOW_OFF; + Settings.Timeout_Millisec = 500; QextSerialPort serialPort(device, Settings); serialPort.open(QIODevice::ReadOnly); QFile file(outFile); - if( !file.open( QIODevice::WriteOnly ) ) - { + if (!file.open(QIODevice::WriteOnly)) { qDebug() << "Failed to open file: " << outFile; - return; + return; } - QTextStream ts( &file ); + QTextStream ts(&file); - while(1) - { + while (1) { dat = serialPort.read(500); - if(dat.contains(framing)) - { + if (dat.contains(framing)) { int start = dat.indexOf(framing); - int count = *((int *) (dat.data() + start+16)); + int count = *((int *)(dat.data() + start + 16)); qDebug() << "Found frame start at " << start << " count " << count; - } - else if (dat.size() == 0) + } else if (dat.size() == 0) { qDebug() << "No data"; - else + } else { qDebug() << "No frame start"; + } ts << dat; usleep(100000); } @@ -71,15 +67,17 @@ int main(int argc, char *argv[]) QString device; QString log; - if(argc < 2) + if (argc < 2) { device = "/dev/tty.usbserial-000014FAB"; - else + } else { device = QString(argv[1]); + } - if(argc < 3) + if (argc < 3) { log = "log.dat"; - else + } else { log = QString(argv[2]); + } pollSerialPort thread(device, log); thread.start(); diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/main.cpp b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/main.cpp index 61ca74b04..41891dbf9 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/main.cpp +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/main.cpp @@ -7,69 +7,60 @@ #include "qsspt.h" int main(int argc, char *argv[]) { -#define MAX_PACKET_DATA_LEN 255 -#define MAX_PACKET_BUF_SIZE (1+1+255+2) +#define MAX_PACKET_DATA_LEN 255 +#define MAX_PACKET_BUF_SIZE (1 + 1 + 255 + 2) - uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; - uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; + uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; + uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; - port * info; + port *info; PortSettings settings; - settings.BaudRate=BAUD57600; - settings.DataBits=DATA_8; - settings.FlowControl=FLOW_OFF; - settings.Parity=PAR_NONE; - settings.StopBits=STOP_1; - settings.Timeout_Millisec=5000; + settings.BaudRate = BAUD57600; + settings.DataBits = DATA_8; + settings.FlowControl = FLOW_OFF; + settings.Parity = PAR_NONE; + settings.StopBits = STOP_1; + settings.Timeout_Millisec = 5000; - info=new port(settings,"COM3"); - info->rxBuf = sspRxBuf; - info->rxBufSize = MAX_PACKET_DATA_LEN; - info->txBuf = sspTxBuf; - info->txBufSize = 255; - info->max_retry = 3; - info->timeoutLen = 5000; - //qssp b(info); + info = new port(settings, "COM3"); + info->rxBuf = sspRxBuf; + info->rxBufSize = MAX_PACKET_DATA_LEN; + info->txBuf = sspTxBuf; + info->txBufSize = 255; + info->max_retry = 3; + info->timeoutLen = 5000; + // qssp b(info); qsspt bb(info); uint8_t buf[1000]; QCoreApplication a(argc, argv); - while(!bb.ssp_Synchronise()) - { - qDebug()<<"trying sync"; + while (!bb.ssp_Synchronise()) { + qDebug() << "trying sync"; } - bb.start(); - qDebug()<<"sync complete"; - buf[0]=0; - buf[1]=1; - buf[2]=2; - while(true) - { - if(bb.sendData(buf,63)) - qDebug()<<"send OK"; -// else -// qDebug()<<"send NOK"; -// //bb.ssp_SendData(buf,63); - } - while(true) - { - - - - if(bb.packets_Available()>0) - { - + bb.start(); + qDebug() << "sync complete"; + buf[0] = 0; + buf[1] = 1; + buf[2] = 2; + while (true) { + if (bb.sendData(buf, 63)) { + qDebug() << "send OK"; + } +// else +// qDebug()<<"send NOK"; +////bb.ssp_SendData(buf,63); + } + while (true) { + if (bb.packets_Available() > 0) { bb.read_Packet(buf); - qDebug()<<"receive="<<(int)buf[0]<<(int)buf[1]<<(int)buf[2]; + qDebug() << "receive=" << (int)buf[0] << (int)buf[1] << (int)buf[2]; ++buf[0]; ++buf[1]; ++buf[2]; - //bb.ssp_SendData(buf,63); - bb.sendData(buf,63); + // bb.ssp_SendData(buf,63); + bb.sendData(buf, 63); } - //bb.ssp_ReceiveProcess(); - //bb.ssp_SendProcess(); - - + // bb.ssp_ReceiveProcess(); + // bb.ssp_SendProcess(); } return a.exec(); } diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.cpp b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.cpp index b247a642c..3cc4ed441 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.cpp +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.cpp @@ -1,16 +1,15 @@ #include "port.h" #include "delay.h" -port::port(PortSettings settings,QString name):mstatus(port::closed) +port::port(PortSettings settings, QString name) : mstatus(port::closed) { timer.start(); - sport = new QextSerialPort(name,settings, QextSerialPort::Polling); - if(sport->open(QIODevice::ReadWrite|QIODevice::Unbuffered)) - { - mstatus=port::open; - // sport->setDtr(); + sport = new QextSerialPort(name, settings, QextSerialPort::Polling); + if (sport->open(QIODevice::ReadWrite | QIODevice::Unbuffered)) { + mstatus = port::open; + // sport->setDtr(); + } else { + mstatus = port::error; } - else - mstatus=port::error; } port::portstatus port::status() { @@ -18,21 +17,20 @@ port::portstatus port::status() } int16_t port::pfSerialRead(void) { - char c[1]; - if(sport->bytesAvailable()) - { - sport->read(c,1); - } - else return -1; + + if (sport->bytesAvailable()) { + sport->read(c, 1); + } else { return -1; } return (uint8_t)c[0]; } void port::pfSerialWrite(uint8_t c) { char cc[1]; - cc[0]=c; - sport->write(cc,1); + + cc[0] = c; + sport->write(cc, 1); } uint32_t port::pfGetTime(void) diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.h b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.h index 6ffea1c11..4d3453958 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.h +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.h @@ -6,38 +6,37 @@ #include #include "common.h" -class port -{ +class port { public: - enum portstatus{open,closed,error}; - virtual int16_t pfSerialRead(void); // function to read a character from the serial input stream - virtual void pfSerialWrite( uint8_t ); // function to write a byte to be sent out the serial port + enum portstatus { open, closed, error }; + virtual int16_t pfSerialRead(void); // function to read a character from the serial input stream + virtual void pfSerialWrite(uint8_t); // function to write a byte to be sent out the serial port virtual uint32_t pfGetTime(void); - uint8_t retryCount; // how many times have we tried to transmit the 'send' packet - uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet - uint16_t max_retry; // Maximum number of retrys for a single transmit. - int32_t timeoutLen; // how long to wait for each retry to succeed - int32_t timeout; // current timeout. when 'time' reaches this point we have timed out - uint8_t txSeqNo; // current 'send' packet sequence number - uint16_t rxBufPos; // current buffer position in the receive packet - uint16_t rxBufLen; // number of 'data' bytes in the buffer - uint8_t rxSeqNo; // current 'receive' packet number - uint16_t rxBufSize; // size of the receive buffer. - uint16_t txBufSize; // size of the transmit buffer. - uint8_t *txBuf; // transmit buffer. REquired to store a copy of packet data in case a retry is needed. - uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received. - uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host + uint8_t retryCount; // how many times have we tried to transmit the 'send' packet + uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet + uint16_t max_retry; // Maximum number of retrys for a single transmit. + int32_t timeoutLen; // how long to wait for each retry to succeed + int32_t timeout; // current timeout. when 'time' reaches this point we have timed out + uint8_t txSeqNo; // current 'send' packet sequence number + uint16_t rxBufPos; // current buffer position in the receive packet + uint16_t rxBufLen; // number of 'data' bytes in the buffer + uint8_t rxSeqNo; // current 'receive' packet number + uint16_t rxBufSize; // size of the receive buffer. + uint16_t txBufSize; // size of the transmit buffer. + uint8_t *txBuf; // transmit buffer. REquired to store a copy of packet data in case a retry is needed. + uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received. + uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host // this is required when switching from the application to the bootloader // and vice-versa. This fixes the firwmare download timeout. // when this flag is set to true, the next time we send a packet we will first // send a synchronize packet. - ReceiveState InputState; - decodeState_ DecodeState; - uint16_t SendState; - uint16_t crc; - uint32_t RxError; - uint32_t TxError; - uint16_t flags; - port(PortSettings settings,QString name); + ReceiveState InputState; + decodeState_ DecodeState; + uint16_t SendState; + uint16_t crc; + uint32_t RxError; + uint32_t TxError; + uint16_t flags; + port(PortSettings settings, QString name); portstatus status(); private: portstatus mstatus; diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.cpp b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.cpp index fdb371e35..aadd01330 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.cpp +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.cpp @@ -7,48 +7,47 @@ /** PRIVATE DEFINITIONS **/ -#define SYNC 225 // Sync character used in Serial Protocol -#define ESC 224 // ESC character used in Serial Protocol -#define ESC_SYNC 1 // ESC_SYNC character used in Serial Protocol -#define ACK_BIT 0x80 // Ack bit, bit 7 of sequence number, 1 = Acknowledge, 0 = -// new packet +#define SYNC 225 // Sync character used in Serial Protocol +#define ESC 224 // ESC character used in Serial Protocol +#define ESC_SYNC 1 // ESC_SYNC character used in Serial Protocol +#define ACK_BIT 0x80 // Ack bit, bit 7 of sequence number, 1 = Acknowledge, 0 = +// new packet // packet location definitions. -#define LENGTH 0 -#define SEQNUM 1 -#define DATA 2 +#define LENGTH 0 +#define SEQNUM 1 +#define DATA 2 // Make larger sized integers from smaller sized integers -#define MAKEWORD16( ub, lb ) ((uint16_t)0x0000 | ((uint16_t)(ub) << 8) | (uint16_t)(lb) ) -#define MAKEWORD32( uw, lw ) ((uint32_t)(0x0UL | ((uint32_t)(uw) << 16) | (uint32_t)(lw)) ) -#define MAKEWORD32B( b3, b2, b1, b0 ) ((uint32_t)((uint32_t)(b3)<< 24) | ((uint32_t)(b2)<<16) | ((uint32_t)(b1)<<8) | ((uint32_t)(b0) ) +#define MAKEWORD16(ub, lb) ((uint16_t)0x0000 | ((uint16_t)(ub) << 8) | (uint16_t)(lb)) +#define MAKEWORD32(uw, lw) ((uint32_t)(0x0UL | ((uint32_t)(uw) << 16) | (uint32_t)(lw))) +#define MAKEWORD32B(b3, b2, b1, b0) ((uint32_t)((uint32_t)(b3) << 24) | ((uint32_t)(b2) << 16) | ((uint32_t)(b1) << 8) | ((uint32_t)(b0)) // Used to extract smaller integers from larger sized intergers -#define LOWERBYTE( w ) (uint8_t)((w) & 0x00ff ) -#define UPPERBYTE( w ) (uint8_t)(((w) & 0xff00) >> 8 ) -#define UPPERWORD(lw) (uint16_t)(((lw) & 0xffff0000) >> 16 ) -#define LOWERWORD(lw) (uint16_t)((lw) & 0x0000ffff) +#define LOWERBYTE(w) (uint8_t)((w) & 0x00ff) +#define UPPERBYTE(w) (uint8_t)(((w) & 0xff00) >> 8) +#define UPPERWORD(lw) (uint16_t)(((lw) & 0xffff0000) >> 16) +#define LOWERWORD(lw) (uint16_t)((lw) & 0x0000ffff) // Macros to operate on a target and bitmask. -#define CLEARBIT( a, b ) ((a) = (a) & ~(b)) -#define SETBIT( a, b ) ((a) = (a) | (b) ) -#define TOGGLEBIT(a,b) ((a) = (a) ^ (b) ) +#define CLEARBIT(a, b) ((a) = (a) & ~(b)) +#define SETBIT(a, b) ((a) = (a) | (b)) +#define TOGGLEBIT(a, b) ((a) = (a) ^ (b)) // test bit macros operate using a bit mask. -#define ISBITSET( a, b ) ( ((a) & (b)) == (b) ? TRUE : FALSE ) -#define ISBITCLEAR( a, b) ( (~(a) & (b)) == (b) ? TRUE : FALSE ) - +#define ISBITSET(a, b) (((a) & (b)) == (b) ? TRUE : FALSE) +#define ISBITCLEAR(a, b) ((~(a) & (b)) == (b) ? TRUE : FALSE) /* Flag bit masks...*/ -#define SENT_SYNCH (0x01) -#define ACK_RECEIVED (0x02) -#define ACK_EXPECTED (0x04) +#define SENT_SYNCH (0x01) +#define ACK_RECEIVED (0x02) +#define ACK_EXPECTED (0x04) -#define SSP_AWAITING_ACK 0 -#define SSP_ACKED 1 -#define SSP_IDLE 2 +#define SSP_AWAITING_ACK 0 +#define SSP_ACKED 1 +#define SSP_IDLE 2 /** PRIVATE DATA **/ static const uint16_t CRC_TABLE[] = { @@ -105,27 +104,25 @@ static const uint16_t CRC_TABLE[] = { * Must be called before calling the Send or REceive process functions. */ -void qssp::ssp_Init(const PortConfig_t* const info) +void qssp::ssp_Init(const PortConfig_t *const info) { - - - thisport->maxRetryCount = info->max_retry; - thisport->timeoutLen = info->timeoutLen; + thisport->maxRetryCount = info->max_retry; + thisport->timeoutLen = info->timeoutLen; thisport->txBufSize = info->txBufSize; - thisport->rxBufSize = info->rxBufSize; - thisport->txBuf = info->txBuf; - thisport->rxBuf = info->rxBuf; + thisport->rxBufSize = info->rxBufSize; + thisport->txBuf = info->txBuf; + thisport->rxBuf = info->rxBuf; thisport->retryCount = 0; - thisport->sendSynch = FALSE; //TRUE; - thisport->rxSeqNo = 255; - thisport->txSeqNo = 255; - thisport->SendState = SSP_IDLE; - thisport->InputState =(ReceiveState)0; - thisport->DecodeState =(decodeState_)0; - thisport->TxError =0; - thisport->RxError =0; - thisport->txSeqNo =0; - thisport->rxSeqNo =0; + thisport->sendSynch = FALSE; // TRUE; + thisport->rxSeqNo = 255; + thisport->txSeqNo = 255; + thisport->SendState = SSP_IDLE; + thisport->InputState = (ReceiveState)0; + thisport->DecodeState = (decodeState_)0; + thisport->TxError = 0; + thisport->RxError = 0; + thisport->txSeqNo = 0; + thisport->rxSeqNo = 0; } /*! @@ -139,11 +136,11 @@ void qssp::ssp_Init(const PortConfig_t* const info) * \note * */ -int16_t qssp::ssp_SendProcess( ) +int16_t qssp::ssp_SendProcess() { int16_t value = SSP_TX_WAITING; - if (thisport->SendState == SSP_AWAITING_ACK ) { + if (thisport->SendState == SSP_AWAITING_ACK) { if (sf_CheckTimeout() == TRUE) { if (thisport->retryCount < thisport->maxRetryCount) { // Try again @@ -153,16 +150,17 @@ int16_t qssp::ssp_SendProcess( ) } else { // Give up, # of trys has exceded the limit value = SSP_TX_TIMEOUT; - CLEARBIT( thisport->flags, ACK_RECEIVED); + CLEARBIT(thisport->flags, ACK_RECEIVED); thisport->SendState = SSP_IDLE; - if (debug) - qDebug()<<"Send TimeOut!"; + if (debug) { + qDebug() << "Send TimeOut!"; + } } } else { value = SSP_TX_WAITING; } - } else if( thisport->SendState == SSP_ACKED ) { - SETBIT( thisport->flags, ACK_RECEIVED); + } else if (thisport->SendState == SSP_ACKED) { + SETBIT(thisport->flags, ACK_RECEIVED); value = SSP_TX_ACKED; thisport->SendState = SSP_IDLE; } else { @@ -205,13 +203,13 @@ int16_t qssp::ssp_ReceiveProcess() * */ -int16_t qssp::ssp_ReceiveByte() +int16_t qssp::ssp_ReceiveByte() { int16_t b; int16_t packet_status = SSP_RX_IDLE; b = thisport->pfSerialRead(); - if( b != -1 ) { + if (b != -1) { packet_status = sf_ReceiveState(b); } return packet_status; @@ -228,17 +226,17 @@ int16_t qssp::ssp_ReceiveByte() * \note * */ -uint16_t qssp::ssp_SendDataBlock(uint8_t *data, uint16_t length ) +uint16_t qssp::ssp_SendDataBlock(uint8_t *data, uint16_t length) { - int16_t packet_status = SSP_TX_WAITING; - uint16_t retval = FALSE; + int16_t packet_status = SSP_TX_WAITING; + uint16_t retval = FALSE; - packet_status = ssp_SendData(data, length ); // send the data - while( packet_status == SSP_TX_WAITING ) { // check the status - (void)ssp_ReceiveProcess(); // process any bytes received. - packet_status = ssp_SendProcess(); // check the send status + packet_status = ssp_SendData(data, length); // send the data + while (packet_status == SSP_TX_WAITING) { // check the status + (void)ssp_ReceiveProcess(); // process any bytes received. + packet_status = ssp_SendProcess(); // check the send status } - if( packet_status == SSP_TX_ACKED ) { // figure out what happened to the packet + if (packet_status == SSP_TX_ACKED) { // figure out what happened to the packet retval = TRUE; } else { retval = FALSE; @@ -258,61 +256,62 @@ uint16_t qssp::ssp_SendDataBlock(uint8_t *data, uint16_t length ) * \note * */ -int16_t qssp::ssp_SendData(const uint8_t *data, const uint16_t length ) +int16_t qssp::ssp_SendData(const uint8_t *data, const uint16_t length) { - int16_t value = SSP_TX_WAITING; - if( (length + 2) > thisport->txBufSize ) { + if ((length + 2) > thisport->txBufSize) { // TRYING to send too much data. value = SSP_TX_BUFOVERRUN; - } else if( thisport->SendState == SSP_IDLE ) { + } else if (thisport->SendState == SSP_IDLE) { #ifdef ACTIVE_SYNCH - if( thisport->sendSynch == TRUE ) { + if (thisport->sendSynch == TRUE) { sf_SendSynchPacket(); } #endif #ifdef SYNCH_SEND - if( length == 0 ) { + if (length == 0) { // TODO this method could allow a task/user to start a synchronisation step if a zero is mistakenly passed to this function. - // could add a check for a NULL data pointer, or use some sort of static flag that can only be accessed by a static function - // that must be called before calling this function. + // could add a check for a NULL data pointer, or use some sort of static flag that can only be accessed by a static function + // that must be called before calling this function. // we are attempting to send a synch packet - thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us + thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us SETBIT(thisport->flags, SENT_SYNCH); } else { // we are sending a data packet - CLEARBIT( thisport->txSeqNo, ACK_BIT ); // make sure we are not sending a ACK packet - thisport->txSeqNo++; // update the sequence number. - if( thisport->txSeqNo > 0x7F) { // check for sequence number rollover - thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, + CLEARBIT(thisport->txSeqNo, ACK_BIT); // make sure we are not sending a ACK packet + thisport->txSeqNo++; // update the sequence number. + if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover + thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, // zero is reserviced for synchronization requests } } #else - CLEARBIT( thisport->txSeqNo, ACK_BIT ); // make sure we are not sending a ACK packet - thisport->txSeqNo++; // update the sequence number. - if( thisport->txSeqNo > 0x7F) { // check for sequence number rollover - thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, + CLEARBIT(thisport->txSeqNo, ACK_BIT); // make sure we are not sending a ACK packet + thisport->txSeqNo++; // update the sequence number. + if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover + thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, // zero is reserved for synchronization requests } -#endif - CLEARBIT( thisport->flags, ACK_RECEIVED); - thisport->SendState = SSP_AWAITING_ACK; +#endif // ifdef SYNCH_SEND + CLEARBIT(thisport->flags, ACK_RECEIVED); + thisport->SendState = SSP_AWAITING_ACK; value = SSP_TX_WAITING; - thisport->retryCount = 0; // zero out the retry counter for this transmission - sf_MakePacket( thisport->txBuf, data, length, thisport->txSeqNo ); - sf_SendPacket( ); // punch out the packet to the serial port - sf_SetSendTimeout( ); // do the timeout values - if (debug) - qDebug()<<"Sent DATA PACKET:"<txSeqNo; + thisport->retryCount = 0; // zero out the retry counter for this transmission + sf_MakePacket(thisport->txBuf, data, length, thisport->txSeqNo); + sf_SendPacket(); // punch out the packet to the serial port + sf_SetSendTimeout(); // do the timeout values + if (debug) { + qDebug() << "Sent DATA PACKET:" << thisport->txSeqNo; + } } else { // error we are already sending a packet. Need to wait for the current packet to be acked or timeout. value = SSP_TX_BUSY; - if (debug) - qDebug()<<"Error sending TX was busy"; + if (debug) { + qDebug() << "Error sending TX was busy"; + } } return value; } @@ -326,46 +325,47 @@ int16_t qssp::ssp_SendData(const uint8_t *data, const uint16_t length ) * \note * A. send a packet with a sequence number equal to zero * B. if timed out then: - * send synch packet again - * increment try counter - * if number of tries exceed maximum try limit then exit + * send synch packet again + * increment try counter + * if number of tries exceed maximum try limit then exit * C. goto A */ -uint16_t qssp::ssp_Synchronise( ) +uint16_t qssp::ssp_Synchronise() { - int16_t packet_status; - uint16_t retval = FALSE; + int16_t packet_status; + uint16_t retval = FALSE; #ifndef USE_SENDPACKET_DATA - thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us + thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us SETBIT(thisport->flags, SENT_SYNCH); // TODO - should this be using ssp_SendPacketData()?? - sf_MakePacket( thisport->txBuf, NULL, 0, thisport->txSeqNo ); // construct the packet - sf_SendPacket( ); - sf_SetSendTimeout( ); + sf_MakePacket(thisport->txBuf, NULL, 0, thisport->txSeqNo); // construct the packet + sf_SendPacket(); + sf_SetSendTimeout(); thisport->SendState = SSP_AWAITING_ACK; packet_status = SSP_TX_WAITING; #else - packet_status = ssp_SendData( NULL, 0 ); + packet_status = ssp_SendData(NULL, 0); #endif - while( packet_status == SSP_TX_WAITING ) { // we loop until we time out. - (void)ssp_ReceiveProcess( ); // do the receive process - packet_status = ssp_SendProcess( ); // do the send process + while (packet_status == SSP_TX_WAITING) { // we loop until we time out. + (void)ssp_ReceiveProcess(); // do the receive process + packet_status = ssp_SendProcess(); // do the send process } thisport->sendSynch = FALSE; - switch( packet_status ) { + switch (packet_status) { case SSP_TX_ACKED: retval = TRUE; break; - case SSP_TX_BUSY: // intentional fall through. - case SSP_TX_TIMEOUT: // intentional fall through. + case SSP_TX_BUSY: // intentional fall through. + case SSP_TX_TIMEOUT: // intentional fall through. case SSP_TX_BUFOVERRUN: retval = FALSE; break; default: retval = FALSE; break; - }; + } + ; return retval; } @@ -385,8 +385,8 @@ void qssp::sf_SendPacket() // use the raw serial write function so the SYNC byte does not get 'escaped' thisport->pfSerialWrite(SYNC); - for( uint8_t x = 0; x < packetLen; x++ ) { - sf_write_byte(thisport->txBuf[x] ); + for (uint8_t x = 0; x < packetLen; x++) { + sf_write_byte(thisport->txBuf[x]); } thisport->retryCount++; } @@ -408,26 +408,25 @@ void qssp::sf_SendPacket() * 3. TODO: Should this function return an error if data length to be sent is greater th tx buffer size? * */ -void qssp::sf_MakePacket( uint8_t *txBuf, const uint8_t * pdata, uint16_t length, uint8_t seqNo ) +void qssp::sf_MakePacket(uint8_t *txBuf, const uint8_t *pdata, uint16_t length, uint8_t seqNo) { - uint16_t crc = 0xffff; - uint16_t bufPos = 0; - uint8_t b; + uint16_t crc = 0xffff; + uint16_t bufPos = 0; + uint8_t b; // add 1 for the seq. number txBuf[LENGTH] = length + 1; txBuf[SEQNUM] = seqNo; - crc = sf_crc16( crc, seqNo ); + crc = sf_crc16(crc, seqNo); - length = length + 2; // add two for the length and seqno bytes which are added before the loop. - for( bufPos = 2; bufPos < length; bufPos++ ) { - b = *pdata++; + length = length + 2; // add two for the length and seqno bytes which are added before the loop. + for (bufPos = 2; bufPos < length; bufPos++) { + b = *pdata++; txBuf[bufPos] = b; - crc = sf_crc16( crc, b ); // update CRC value + crc = sf_crc16(crc, b); // update CRC value } txBuf[bufPos++] = LOWERBYTE(crc); txBuf[bufPos] = UPPERBYTE(crc); - } /*! @@ -442,13 +441,14 @@ void qssp::sf_MakePacket( uint8_t *txBuf, const uint8_t * pdata, uint16_t length void qssp::sf_SendAckPacket(uint8_t seqNumber) { - uint8_t AckSeqNumber = SETBIT( seqNumber, ACK_BIT ); + uint8_t AckSeqNumber = SETBIT(seqNumber, ACK_BIT); // create the packet, note we pass AckSequenceNumber directly - sf_MakePacket( thisport->txBuf, NULL, 0, AckSeqNumber ); - sf_SendPacket( ); - if (debug) - qDebug()<<"Sent ACK PACKET:"<txBuf, NULL, 0, AckSeqNumber); + sf_SendPacket(); + if (debug) { + qDebug() << "Sent ACK PACKET:" << seqNumber; + } // we don't set the timeout for an ACK because we don't ACK our ACKs in this protocol } @@ -461,30 +461,30 @@ void qssp::sf_SendAckPacket(uint8_t seqNumber) * \note * */ -void qssp::sf_write_byte(uint8_t c ) +void qssp::sf_write_byte(uint8_t c) { - if( c == SYNC ) { // check for SYNC byte - thisport->pfSerialWrite( ESC ); // since we are not starting a packet we must ESCAPE the SYNCH byte - thisport->pfSerialWrite( ESC_SYNC ); // now send the escaped synch char - } else if( c == ESC ) { // Check for ESC character - thisport->pfSerialWrite( ESC ); // if it is, we need to send it twice - thisport->pfSerialWrite( ESC ); + if (c == SYNC) { // check for SYNC byte + thisport->pfSerialWrite(ESC); // since we are not starting a packet we must ESCAPE the SYNCH byte + thisport->pfSerialWrite(ESC_SYNC); // now send the escaped synch char + } else if (c == ESC) { // Check for ESC character + thisport->pfSerialWrite(ESC); // if it is, we need to send it twice + thisport->pfSerialWrite(ESC); } else { - thisport->pfSerialWrite( c ); // otherwise write the byte to serial port + thisport->pfSerialWrite(c); // otherwise write the byte to serial port } } /************************************************************************************************************ -* -* NAME: uint16_t ssp_crc16( uint16_t crc, uint16_t data ) -* DESCRIPTION: Uses crc_table to calculate new crc -* ARGUMENTS: -* arg1: crc -* arg2: data - byte to calculate into CRC -* RETURN: New crc -* CREATED: 5/8/02 -* -*************************************************************************************************************/ + * + * NAME: uint16_t ssp_crc16( uint16_t crc, uint16_t data ) + * DESCRIPTION: Uses crc_table to calculate new crc + * ARGUMENTS: + * arg1: crc + * arg2: data - byte to calculate into CRC + * RETURN: New crc + * CREATED: 5/8/02 + * + *************************************************************************************************************/ /*! * \brief calculates the new CRC value for 'data' * \param crc = current CRC value @@ -495,9 +495,9 @@ void qssp::sf_write_byte(uint8_t c ) * */ -uint16_t qssp::sf_crc16( uint16_t crc, uint8_t data ) +uint16_t qssp::sf_crc16(uint16_t crc, uint8_t data) { - return (crc >> 8) ^ CRC_TABLE[( crc ^ data ) & 0x00FF ]; + return (crc >> 8) ^ CRC_TABLE[(crc ^ data) & 0x00FF]; } @@ -512,7 +512,8 @@ uint16_t qssp::sf_crc16( uint16_t crc, uint8_t data ) void qssp::sf_SetSendTimeout() { - uint32_t timeout; + uint32_t timeout; + timeout = thisport->pfGetTime() + thisport->timeoutLen; thisport->timeout = timeout; } @@ -526,32 +527,34 @@ void qssp::sf_SetSendTimeout() * \note * */ -uint16_t qssp::sf_CheckTimeout() +uint16_t qssp::sf_CheckTimeout() { - uint16_t retval = FALSE; - uint32_t current_time; + uint16_t retval = FALSE; + uint32_t current_time; current_time = thisport->pfGetTime(); - if( current_time > thisport->timeout ) { + if (current_time > thisport->timeout) { retval = TRUE; } - if(retval) - if (debug) - qDebug()<<"timeout"<timeout; + if (retval) { + if (debug) { + qDebug() << "timeout" << current_time << thisport->timeout; + } + } return retval; } /**************************************************************************** - * NAME: sf_ReceiveState - * DESC: Implements the receive state handling code for escaped and unescaped data - * ARGS: thisport - which port to operate on - * c - incoming byte - * RETURN: - * CREATED: - * NOTES: - * 1. change from using pointer to functions. - ****************************************************************************/ +* NAME: sf_ReceiveState +* DESC: Implements the receive state handling code for escaped and unescaped data +* ARGS: thisport - which port to operate on +* c - incoming byte +* RETURN: +* CREATED: +* NOTES: +* 1. change from using pointer to functions. +****************************************************************************/ /*! * \brief implements the receive state handling code for escaped and unescaped data * \param thisport = which port to use @@ -561,46 +564,46 @@ uint16_t qssp::sf_CheckTimeout() * \note * */ -int16_t qssp::sf_ReceiveState(uint8_t c ) +int16_t qssp::sf_ReceiveState(uint8_t c) { - int16_t retval = SSP_RX_RECEIVING; + int16_t retval = SSP_RX_RECEIVING; - switch( thisport->InputState ) { + switch (thisport->InputState) { case state_unescaped_e: - if( c == SYNC ) { + if (c == SYNC) { thisport->DecodeState = decode_len1_e; - } else if ( c == ESC ) { + } else if (c == ESC) { thisport->InputState = state_escaped_e; } else { retval = sf_DecodeState(c); } - break; // end of unescaped state. - case state_escaped_e: + break; // end of unescaped state. + case state_escaped_e: thisport->InputState = state_unescaped_e; - if( c == SYNC ) { + if (c == SYNC) { thisport->DecodeState = decode_len1_e; - } else if (c == ESC_SYNC ) { + } else if (c == ESC_SYNC) { retval = sf_DecodeState(SYNC); } else { retval = sf_DecodeState(c); } - break; // end of the escaped state. - default: + break; // end of the escaped state. + default: break; } return retval; } /**************************************************************************** - * NAME: sf_DecodeState - * DESC: Implements the receive state finite state machine - * ARGS: thisport - which port to operate on - * c - incoming byte - * RETURN: - * CREATED: - * NOTES: - * 1. change from using pointer to functions. - ****************************************************************************/ +* NAME: sf_DecodeState +* DESC: Implements the receive state finite state machine +* ARGS: thisport - which port to operate on +* c - incoming byte +* RETURN: +* CREATED: +* NOTES: +* 1. change from using pointer to functions. +****************************************************************************/ /*! * \brief implements the receiving decoding state machine @@ -611,19 +614,20 @@ int16_t qssp::sf_ReceiveState(uint8_t c ) * \note * */ -int16_t qssp::sf_DecodeState( uint8_t c ) +int16_t qssp::sf_DecodeState(uint8_t c) { - int16_t retval; - switch( thisport->DecodeState ) { + int16_t retval; + + switch (thisport->DecodeState) { case decode_idle_e: // 'c' is ignored in this state as the only way to leave the idle state is // recognition of the SYNC byte in the sf_ReceiveState function. retval = SSP_RX_IDLE; break; case decode_len1_e: - thisport->rxBuf[LENGTH]= c; + thisport->rxBuf[LENGTH] = c; thisport->rxBufLen = c; - if( thisport->rxBufLen <= thisport->rxBufSize ) { + if (thisport->rxBufLen <= thisport->rxBufSize) { thisport->DecodeState = decode_seqNo_e; retval = SSP_RX_RECEIVING; } else { @@ -631,37 +635,37 @@ int16_t qssp::sf_DecodeState( uint8_t c ) retval = SSP_RX_IDLE; } break; - case decode_seqNo_e: + case decode_seqNo_e: thisport->rxBuf[SEQNUM] = c; thisport->crc = 0xffff; - thisport->rxBufLen--; // subtract 1 for the seq. no. + thisport->rxBufLen--; // subtract 1 for the seq. no. thisport->rxBufPos = 2; - thisport->crc = sf_crc16( thisport->crc, c ); - if( thisport->rxBufLen > 0 ) { + thisport->crc = sf_crc16(thisport->crc, c); + if (thisport->rxBufLen > 0) { thisport->DecodeState = decode_data_e; } else { thisport->DecodeState = decode_crc1_e; } retval = SSP_RX_RECEIVING; break; - case decode_data_e: - thisport->rxBuf[ (thisport->rxBufPos)++] = c; - thisport->crc = sf_crc16( thisport->crc, c ); - if( thisport->rxBufPos == (thisport->rxBufLen+2) ) { + case decode_data_e: + thisport->rxBuf[(thisport->rxBufPos)++] = c; + thisport->crc = sf_crc16(thisport->crc, c); + if (thisport->rxBufPos == (thisport->rxBufLen + 2)) { thisport->DecodeState = decode_crc1_e; } retval = SSP_RX_RECEIVING; break; - case decode_crc1_e: - thisport->crc = sf_crc16( thisport->crc, c ); + case decode_crc1_e: + thisport->crc = sf_crc16(thisport->crc, c); thisport->DecodeState = decode_crc2_e; retval = SSP_RX_RECEIVING; break; - case decode_crc2_e: + case decode_crc2_e: thisport->DecodeState = decode_idle_e; // verify the CRC value for the packet - if( sf_crc16( thisport->crc, c) == 0) { + if (sf_crc16(thisport->crc, c) == 0) { // TODO shouldn't the return value of sf_ReceivePacket() be checked? sf_ReceivePacket(); retval = SSP_RX_COMPLETE; @@ -670,8 +674,8 @@ int16_t qssp::sf_DecodeState( uint8_t c ) retval = SSP_RX_IDLE; } break; - default: - thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet. + default: + thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet. retval = SSP_RX_IDLE; break; } @@ -679,18 +683,18 @@ int16_t qssp::sf_DecodeState( uint8_t c ) } /************************************************************************************************************ -* -* NAME: int16_t sf_ReceivePacket( ) -* DESCRIPTION: Receive one packet, assumed that data is in rec.buff[] -* ARGUMENTS: -* RETURN: 0 . no new packet was received, could be ack or same packet -* 1 . new packet received -* SSP_PACKET_? -* SSP_PACKET_COMPLETE -* SSP_PACKET_ACK -* CREATED: 5/8/02 -* -*************************************************************************************************************/ + * + * NAME: int16_t sf_ReceivePacket( ) + * DESCRIPTION: Receive one packet, assumed that data is in rec.buff[] + * ARGUMENTS: + * RETURN: 0 . no new packet was received, could be ack or same packet + * 1 . new packet received + * SSP_PACKET_? + * SSP_PACKET_COMPLETE + * SSP_PACKET_ACK + * CREATED: 5/8/02 + * + *************************************************************************************************************/ /*! * \brief receive one packet. calls the callback function if needed. * \param thisport = which port to use @@ -706,76 +710,79 @@ int16_t qssp::sf_ReceivePacket() { int16_t value = FALSE; - if( ISBITSET(thisport->rxBuf[SEQNUM], ACK_BIT ) ) { - // Received an ACK packet, need to check if it matches the previous sent packet - if( ( thisport->rxBuf[SEQNUM] & 0x7F) == (thisport->txSeqNo & 0x7f)) { - // It matches the last packet sent by us - SETBIT( thisport->txSeqNo, ACK_BIT ); + if (ISBITSET(thisport->rxBuf[SEQNUM], ACK_BIT)) { + // Received an ACK packet, need to check if it matches the previous sent packet + if ((thisport->rxBuf[SEQNUM] & 0x7F) == (thisport->txSeqNo & 0x7f)) { + // It matches the last packet sent by us + SETBIT(thisport->txSeqNo, ACK_BIT); thisport->SendState = SSP_ACKED; value = FALSE; - if (debug) - qDebug()<<"Received ACK:"<<(thisport->txSeqNo & 0x7F); + if (debug) { + qDebug() << "Received ACK:" << (thisport->txSeqNo & 0x7F); + } } // else ignore the ACK packet } else { - // Received a 'data' packet, figure out what type of packet we received... - if( thisport->rxBuf[SEQNUM] == 0 ) { - if (debug) - qDebug()<<"Received SYNC Request"; + // Received a 'data' packet, figure out what type of packet we received... + if (thisport->rxBuf[SEQNUM] == 0) { + if (debug) { + qDebug() << "Received SYNC Request"; + } // Synchronize sequence number with host #ifdef ACTIVE_SYNCH thisport->sendSynch = TRUE; #endif - sf_SendAckPacket(thisport->rxBuf[SEQNUM] ); - thisport->rxSeqNo = 0; + sf_SendAckPacket(thisport->rxBuf[SEQNUM]); + thisport->rxSeqNo = 0; value = FALSE; - } else if( thisport->rxBuf[SEQNUM] == thisport->rxSeqNo ) { + } else if (thisport->rxBuf[SEQNUM] == thisport->rxSeqNo) { // Already seen this packet, just ack it, don't act on the packet. - sf_SendAckPacket(thisport->rxBuf[SEQNUM] ); + sf_SendAckPacket(thisport->rxBuf[SEQNUM]); value = FALSE; } else { - //New Packet + // New Packet thisport->rxSeqNo = thisport->rxBuf[SEQNUM]; // Let the application do something with the data/packet. // skip the first two bytes (length and seq. no.) in the buffer. - if (debug) - qDebug()<<"Received DATA PACKET seq="<rxSeqNo<<"Data="<<(uint8_t)thisport->rxBuf[2]<<(uint8_t)thisport->rxBuf[3]<<(uint8_t)thisport->rxBuf[4]; - pfCallBack( &(thisport->rxBuf[2]), thisport->rxBufLen); + if (debug) { + qDebug() << "Received DATA PACKET seq=" << thisport->rxSeqNo << "Data=" << (uint8_t)thisport->rxBuf[2] << (uint8_t)thisport->rxBuf[3] << (uint8_t)thisport->rxBuf[4]; + } + pfCallBack(&(thisport->rxBuf[2]), thisport->rxBufLen); // after we send the ACK, it is possible for the host to send a new packet. // Thus the application needs to copy the data and reset the receive buffer // inside of thisport->pfCallBack() - sf_SendAckPacket(thisport->rxBuf[SEQNUM] ); + sf_SendAckPacket(thisport->rxBuf[SEQNUM]); value = TRUE; } } return value; } -qssp::qssp(port * info,bool debug):debug(debug) +qssp::qssp(port *info, bool debug) : debug(debug) { - - thisport=info; - thisport->maxRetryCount = info->max_retry; - thisport->timeoutLen = info->timeoutLen; + thisport = info; + thisport->maxRetryCount = info->max_retry; + thisport->timeoutLen = info->timeoutLen; thisport->txBufSize = info->txBufSize; - thisport->rxBufSize = info->rxBufSize; - thisport->txBuf = info->txBuf; - thisport->rxBuf = info->rxBuf; + thisport->rxBufSize = info->rxBufSize; + thisport->txBuf = info->txBuf; + thisport->rxBuf = info->rxBuf; thisport->retryCount = 0; - thisport->sendSynch = FALSE; //TRUE; - thisport->rxSeqNo = 255; - thisport->txSeqNo = 255; - thisport->SendState = SSP_IDLE; - thisport->InputState =(ReceiveState)0; - thisport->DecodeState =(decodeState_)0; - thisport->TxError =0; - thisport->RxError =0; - thisport->txSeqNo =0; - thisport->rxSeqNo =0; + thisport->sendSynch = FALSE; // TRUE; + thisport->rxSeqNo = 255; + thisport->txSeqNo = 255; + thisport->SendState = SSP_IDLE; + thisport->InputState = (ReceiveState)0; + thisport->DecodeState = (decodeState_)0; + thisport->TxError = 0; + thisport->RxError = 0; + thisport->txSeqNo = 0; + thisport->rxSeqNo = 0; } -void qssp::pfCallBack( uint8_t * buf, uint16_t size) +void qssp::pfCallBack(uint8_t *buf, uint16_t size) { - if (debug) - qDebug()<<"receive callback"<ssp_ReceiveProcess(); - sendstatus=this->ssp_SendProcess(); + while (!endthread) { + receivestatus = this->ssp_ReceiveProcess(); + sendstatus = this->ssp_SendProcess(); sendbufmutex.lock(); - if(datapending && receivestatus==SSP_TX_IDLE) - { - this->ssp_SendData(mbuf,msize); - datapending=false; + if (datapending && receivestatus == SSP_TX_IDLE) { + this->ssp_SendData(mbuf, msize); + datapending = false; } sendbufmutex.unlock(); - if(sendstatus==SSP_TX_ACKED) + if (sendstatus == SSP_TX_ACKED) { sendwait.wakeAll(); + } } - } -bool qsspt::sendData(uint8_t * buf,uint16_t size) +bool qsspt::sendData(uint8_t *buf, uint16_t size) { - if(datapending) + if (datapending) { return false; + } sendbufmutex.lock(); - datapending=true; - mbuf=buf; - msize=size; + datapending = true; + mbuf = buf; + msize = size; sendbufmutex.unlock(); msendwait.lock(); - sendwait.wait(&msendwait,100000); + sendwait.wait(&msendwait, 100000); msendwait.unlock(); return true; } -void qsspt::pfCallBack( uint8_t * buf, uint16_t size) +void qsspt::pfCallBack(uint8_t *buf, uint16_t size) { - if (debug) - qDebug()<<"receive callback"<> 8 ) -#define UPPERWORD(lw) (uint16_t)(((lw) & 0xffff0000) >> 16 ) -#define LOWERWORD(lw) (uint16_t)((lw) & 0x0000ffff) +#define LOWERBYTE(w) (uint8_t)((w) & 0x00ff) +#define UPPERBYTE(w) (uint8_t)(((w) & 0xff00) >> 8) +#define UPPERWORD(lw) (uint16_t)(((lw) & 0xffff0000) >> 16) +#define LOWERWORD(lw) (uint16_t)((lw) & 0x0000ffff) // Macros to operate on a target and bitmask. -#define CLEARBIT( a, b ) ((a) = (a) & ~(b)) -#define SETBIT( a, b ) ((a) = (a) | (b) ) -#define TOGGLEBIT(a,b) ((a) = (a) ^ (b) ) +#define CLEARBIT(a, b) ((a) = (a) & ~(b)) +#define SETBIT(a, b) ((a) = (a) | (b)) +#define TOGGLEBIT(a, b) ((a) = (a) ^ (b)) // test bit macros operate using a bit mask. -#define ISBITSET( a, b ) ( ((a) & (b)) == (b) ? TRUE : FALSE ) -#define ISBITCLEAR( a, b) ( (~(a) & (b)) == (b) ? TRUE : FALSE ) +#define ISBITSET(a, b) (((a) & (b)) == (b) ? TRUE : FALSE) +#define ISBITCLEAR(a, b) ((~(a) & (b)) == (b) ? TRUE : FALSE) /** PRIVATE FUNCTIONS **/ -//static void sf_SendSynchPacket( Port_t *thisport ); -static uint16_t sf_crc16( uint16_t crc, uint8_t data ); -static void sf_write_byte( Port_t *thisport, uint8_t c ); -static void sf_SetSendTimeout( Port_t *thisport ); -static uint16_t sf_CheckTimeout( Port_t *thisport ); -static int16_t sf_DecodeState( Port_t *thisport, uint8_t c ); -static int16_t sf_ReceiveState( Port_t *thisport, uint8_t c ); +// static void sf_SendSynchPacket( Port_t *thisport ); +static uint16_t sf_crc16(uint16_t crc, uint8_t data); +static void sf_write_byte(Port_t *thisport, uint8_t c); +static void sf_SetSendTimeout(Port_t *thisport); +static uint16_t sf_CheckTimeout(Port_t *thisport); +static int16_t sf_DecodeState(Port_t *thisport, uint8_t c); +static int16_t sf_ReceiveState(Port_t *thisport, uint8_t c); -static void sf_SendPacket( Port_t *thisport ); -static void sf_SendAckPacket( Port_t *thisport, uint8_t seqNumber); -static void sf_MakePacket( uint8_t *buf, const uint8_t * pdata, uint16_t length, uint8_t seqNo ); -static int16_t sf_ReceivePacket(Port_t *thisport); +static void sf_SendPacket(Port_t *thisport); +static void sf_SendAckPacket(Port_t *thisport, uint8_t seqNumber); +static void sf_MakePacket(uint8_t *buf, const uint8_t *pdata, uint16_t length, uint8_t seqNo); +static int16_t sf_ReceivePacket(Port_t *thisport); /* Flag bit masks...*/ -#define SENT_SYNCH (0x01) -#define ACK_RECEIVED (0x02) -#define ACK_EXPECTED (0x04) +#define SENT_SYNCH (0x01) +#define ACK_RECEIVED (0x02) +#define ACK_EXPECTED (0x04) -#define SSP_AWAITING_ACK 0 -#define SSP_ACKED 1 -#define SSP_IDLE 2 +#define SSP_AWAITING_ACK 0 +#define SSP_ACKED 1 +#define SSP_IDLE 2 /** PRIVATE DATA **/ static const uint16_t CRC_TABLE[] = { @@ -167,7 +166,7 @@ static const uint16_t CRC_TABLE[] = { 0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41, 0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641, 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040 - }; +}; /** EXTERNAL DATA **/ @@ -188,24 +187,24 @@ static const uint16_t CRC_TABLE[] = { * Must be called before calling the Send or REceive process functions. */ -void ssp_Init( Port_t *thisport, const PortConfig_t* const info) +void ssp_Init(Port_t *thisport, const PortConfig_t *const info) { - thisport->pfCallBack = info->pfCallBack; - thisport->pfSerialRead = info->pfSerialRead; - thisport->pfSerialWrite = info->pfSerialWrite; + thisport->pfCallBack = info->pfCallBack; + thisport->pfSerialRead = info->pfSerialRead; + thisport->pfSerialWrite = info->pfSerialWrite; thisport->pfGetTime = info->pfGetTime; - thisport->maxRetryCount = info->max_retry; - thisport->timeoutLen = info->timeoutLen; - thisport->txBufSize = info->txBufSize; - thisport->rxBufSize = info->rxBufSize; - thisport->txBuf = info->txBuf; - thisport->rxBuf = info->rxBuf; - thisport->retryCount = 0; - thisport->sendSynch = FALSE; //TRUE; - thisport->rxSeqNo = 255; - thisport->txSeqNo = 255; - thisport->SendState = SSP_IDLE; + thisport->maxRetryCount = info->max_retry; + thisport->timeoutLen = info->timeoutLen; + thisport->txBufSize = info->txBufSize; + thisport->rxBufSize = info->rxBufSize; + thisport->txBuf = info->txBuf; + thisport->rxBuf = info->rxBuf; + thisport->retryCount = 0; + thisport->sendSynch = FALSE; // TRUE; + thisport->rxSeqNo = 255; + thisport->txSeqNo = 255; + thisport->SendState = SSP_IDLE; } /*! @@ -219,35 +218,35 @@ void ssp_Init( Port_t *thisport, const PortConfig_t* const info) * \note * */ -int16_t ssp_SendProcess( Port_t *thisport ) +int16_t ssp_SendProcess(Port_t *thisport) { - int16_t value = SSP_TX_WAITING; + int16_t value = SSP_TX_WAITING; - if (thisport->SendState == SSP_AWAITING_ACK ) { - if (sf_CheckTimeout(thisport) == TRUE) { - if (thisport->retryCount < thisport->maxRetryCount) { - // Try again - sf_SendPacket(thisport); - sf_SetSendTimeout(thisport); - value = SSP_TX_WAITING; - } else { - // Give up, # of trys has exceded the limit - value = SSP_TX_TIMEOUT; - CLEARBIT( thisport->flags, ACK_RECEIVED); + if (thisport->SendState == SSP_AWAITING_ACK) { + if (sf_CheckTimeout(thisport) == TRUE) { + if (thisport->retryCount < thisport->maxRetryCount) { + // Try again + sf_SendPacket(thisport); + sf_SetSendTimeout(thisport); + value = SSP_TX_WAITING; + } else { + // Give up, # of trys has exceded the limit + value = SSP_TX_TIMEOUT; + CLEARBIT(thisport->flags, ACK_RECEIVED); thisport->SendState = SSP_IDLE; - } - } else { - value = SSP_TX_WAITING; - } - } else if( thisport->SendState == SSP_ACKED ) { - SETBIT( thisport->flags, ACK_RECEIVED); + } + } else { + value = SSP_TX_WAITING; + } + } else if (thisport->SendState == SSP_ACKED) { + SETBIT(thisport->flags, ACK_RECEIVED); value = SSP_TX_ACKED; thisport->SendState = SSP_IDLE; - } else { - thisport->SendState = SSP_IDLE; + } else { + thisport->SendState = SSP_IDLE; value = SSP_TX_IDLE; } - return value; + return value; } /*! @@ -260,17 +259,17 @@ int16_t ssp_SendProcess( Port_t *thisport ) */ int16_t ssp_ReceiveProcess(Port_t *thisport) { - int16_t b; - int16_t packet_status = SSP_RX_IDLE; + int16_t b; + int16_t packet_status = SSP_RX_IDLE; - do { - b = thisport->pfSerialRead(); // attempt to read a char from the serial buffer - if (b != -1) { - packet_status = sf_ReceiveState(thisport, b); // process the newly received byte in the receive state machine - } - // keep going until either we received a full packet or there are no more bytes to process - } while (packet_status != SSP_RX_COMPLETE && b != -1); - return packet_status; + do { + b = thisport->pfSerialRead(); // attempt to read a char from the serial buffer + if (b != -1) { + packet_status = sf_ReceiveState(thisport, b); // process the newly received byte in the receive state machine + } + // keep going until either we received a full packet or there are no more bytes to process + } while (packet_status != SSP_RX_COMPLETE && b != -1); + return packet_status; } /*! @@ -282,13 +281,13 @@ int16_t ssp_ReceiveProcess(Port_t *thisport) * */ -int16_t ssp_ReceiveByte(Port_t *thisport ) +int16_t ssp_ReceiveByte(Port_t *thisport) { int16_t b; int16_t packet_status = SSP_RX_IDLE; b = thisport->pfSerialRead(); - if( b != -1 ) { + if (b != -1) { packet_status = sf_ReceiveState(thisport, b); } return packet_status; @@ -305,17 +304,17 @@ int16_t ssp_ReceiveByte(Port_t *thisport ) * \note * */ -uint16_t ssp_SendDataBlock( Port_t *thisport, uint8_t *data, uint16_t length ) +uint16_t ssp_SendDataBlock(Port_t *thisport, uint8_t *data, uint16_t length) { - int16_t packet_status = SSP_TX_WAITING; - uint16_t retval = FALSE; + int16_t packet_status = SSP_TX_WAITING; + uint16_t retval = FALSE; - packet_status = ssp_SendData( thisport, data, length ); // send the data - while( packet_status == SSP_TX_WAITING ) { // check the status - (void)ssp_ReceiveProcess( thisport ); // process any bytes received. - packet_status = ssp_SendProcess( thisport ); // check the send status + packet_status = ssp_SendData(thisport, data, length); // send the data + while (packet_status == SSP_TX_WAITING) { // check the status + (void)ssp_ReceiveProcess(thisport); // process any bytes received. + packet_status = ssp_SendProcess(thisport); // check the send status } - if( packet_status == SSP_TX_ACKED ) { // figure out what happened to the packet + if (packet_status == SSP_TX_ACKED) { // figure out what happened to the packet retval = TRUE; } else { retval = FALSE; @@ -335,54 +334,53 @@ uint16_t ssp_SendDataBlock( Port_t *thisport, uint8_t *data, uint16_t length ) * \note * */ -int16_t ssp_SendData( Port_t *thisport, const uint8_t *data, const uint16_t length ) +int16_t ssp_SendData(Port_t *thisport, const uint8_t *data, const uint16_t length) { - int16_t value = SSP_TX_WAITING; - if( (length + 2) > thisport->txBufSize ) { + if ((length + 2) > thisport->txBufSize) { // TRYING to send too much data. value = SSP_TX_BUFOVERRUN; - } else if( thisport->SendState == SSP_IDLE ) { + } else if (thisport->SendState == SSP_IDLE) { #ifdef ACTIVE_SYNCH - if( thisport->sendSynch == TRUE ) { + if (thisport->sendSynch == TRUE) { sf_SendSynchPacket(thisport); } #endif #ifdef SYNCH_SEND - if( length == 0 ) { + if (length == 0) { // TODO this method could allow a task/user to start a synchronisation step if a zero is mistakenly passed to this function. - // could add a check for a NULL data pointer, or use some sort of static flag that can only be accessed by a static function - // that must be called before calling this function. + // could add a check for a NULL data pointer, or use some sort of static flag that can only be accessed by a static function + // that must be called before calling this function. // we are attempting to send a synch packet - thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us + thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us SETBIT(thisport->flags, SENT_SYNCH); } else { // we are sending a data packet - CLEARBIT( thisport->txSeqNo, ACK_BIT ); // make sure we are not sending a ACK packet - thisport->txSeqNo++; // update the sequence number. - if( thisport->txSeqNo > 0x7F) { // check for sequence number rollover - thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, - // zero is reserviced for synchronization requests + CLEARBIT(thisport->txSeqNo, ACK_BIT); // make sure we are not sending a ACK packet + thisport->txSeqNo++; // update the sequence number. + if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover + thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, + // zero is reserviced for synchronization requests } } #else - CLEARBIT( thisport->txSeqNo, ACK_BIT ); // make sure we are not sending a ACK packet - thisport->txSeqNo++; // update the sequence number. - if( thisport->txSeqNo > 0x7F) { // check for sequence number rollover - thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, - // zero is reserved for synchronization requests + CLEARBIT(thisport->txSeqNo, ACK_BIT); // make sure we are not sending a ACK packet + thisport->txSeqNo++; // update the sequence number. + if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover + thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, + // zero is reserved for synchronization requests } -#endif - CLEARBIT( thisport->flags, ACK_RECEIVED); - thisport->SendState = SSP_AWAITING_ACK; +#endif // ifdef SYNCH_SEND + CLEARBIT(thisport->flags, ACK_RECEIVED); + thisport->SendState = SSP_AWAITING_ACK; value = SSP_TX_WAITING; - thisport->retryCount = 0; // zero out the retry counter for this transmission - sf_MakePacket( thisport->txBuf, data, length, thisport->txSeqNo ); - sf_SendPacket( thisport ); // punch out the packet to the serial port - sf_SetSendTimeout( thisport ); // do the timeout values + thisport->retryCount = 0; // zero out the retry counter for this transmission + sf_MakePacket(thisport->txBuf, data, length, thisport->txSeqNo); + sf_SendPacket(thisport); // punch out the packet to the serial port + sf_SetSendTimeout(thisport); // do the timeout values } else { // error we are already sending a packet. Need to wait for the current packet to be acked or timeout. value = SSP_TX_BUSY; @@ -399,46 +397,47 @@ int16_t ssp_SendData( Port_t *thisport, const uint8_t *data, const uint16_t leng * \note * A. send a packet with a sequence number equal to zero * B. if timed out then: - * send synch packet again - * increment try counter - * if number of tries exceed maximum try limit then exit + * send synch packet again + * increment try counter + * if number of tries exceed maximum try limit then exit * C. goto A */ -uint16_t ssp_Synchronise( Port_t *thisport ) +uint16_t ssp_Synchronise(Port_t *thisport) { - int16_t packet_status; - uint16_t retval = FALSE; + int16_t packet_status; + uint16_t retval = FALSE; #ifndef USE_SENDPACKET_DATA - thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us + thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us SETBIT(thisport->flags, SENT_SYNCH); // TODO - should this be using ssp_SendPacketData()?? - sf_MakePacket( thisport->txBuf, NULL, 0, thisport->txSeqNo ); // construct the packet - sf_SendPacket( thisport ); - sf_SetSendTimeout( thisport ); + sf_MakePacket(thisport->txBuf, NULL, 0, thisport->txSeqNo); // construct the packet + sf_SendPacket(thisport); + sf_SetSendTimeout(thisport); thisport->SendState = SSP_AWAITING_ACK; packet_status = SSP_TX_WAITING; #else - packet_status = ssp_SendData( thisport, NULL, 0 ); + packet_status = ssp_SendData(thisport, NULL, 0); #endif - while( packet_status == SSP_TX_WAITING ) { // we loop until we time out. - (void)ssp_ReceiveProcess( thisport ); // do the receive process - packet_status = ssp_SendProcess( thisport ); // do the send process + while (packet_status == SSP_TX_WAITING) { // we loop until we time out. + (void)ssp_ReceiveProcess(thisport); // do the receive process + packet_status = ssp_SendProcess(thisport); // do the send process } thisport->sendSynch = FALSE; - switch( packet_status ) { + switch (packet_status) { case SSP_TX_ACKED: retval = TRUE; break; - case SSP_TX_BUSY: // intentional fall through. - case SSP_TX_TIMEOUT: // intentional fall through. + case SSP_TX_BUSY: // intentional fall through. + case SSP_TX_TIMEOUT: // intentional fall through. case SSP_TX_BUFOVERRUN: retval = FALSE; break; default: retval = FALSE; break; - }; + } + ; return retval; } @@ -452,14 +451,14 @@ uint16_t ssp_Synchronise( Port_t *thisport ) * Packet should be formed through the use of sf_MakePacket before calling this function. */ static void sf_SendPacket(Port_t *thisport) - { - // add 3 to packet data length for: 1 length + 2 CRC (packet overhead) - uint8_t packetLen = thisport->txBuf[LENGTH] + 3; +{ + // add 3 to packet data length for: 1 length + 2 CRC (packet overhead) + uint8_t packetLen = thisport->txBuf[LENGTH] + 3; // use the raw serial write function so the SYNC byte does not get 'escaped' - thisport->pfSerialWrite(SYNC); - for( uint8_t x = 0; x < packetLen; x++ ) { - sf_write_byte(thisport, thisport->txBuf[x] ); + thisport->pfSerialWrite(SYNC); + for (uint8_t x = 0; x < packetLen; x++) { + sf_write_byte(thisport, thisport->txBuf[x]); } thisport->retryCount++; } @@ -481,26 +480,25 @@ static void sf_SendPacket(Port_t *thisport) * 3. TODO: Should this function return an error if data length to be sent is greater th tx buffer size? * */ -void sf_MakePacket( uint8_t *txBuf, const uint8_t * pdata, uint16_t length, uint8_t seqNo ) +void sf_MakePacket(uint8_t *txBuf, const uint8_t *pdata, uint16_t length, uint8_t seqNo) { - uint16_t crc = 0xffff; - uint16_t bufPos = 0; - uint8_t b; + uint16_t crc = 0xffff; + uint16_t bufPos = 0; + uint8_t b; // add 1 for the seq. number txBuf[LENGTH] = length + 1; txBuf[SEQNUM] = seqNo; - crc = sf_crc16( crc, seqNo ); + crc = sf_crc16(crc, seqNo); - length = length + 2; // add two for the length and seqno bytes which are added before the loop. - for( bufPos = 2; bufPos < length; bufPos++ ) { - b = *pdata++; + length = length + 2; // add two for the length and seqno bytes which are added before the loop. + for (bufPos = 2; bufPos < length; bufPos++) { + b = *pdata++; txBuf[bufPos] = b; - crc = sf_crc16( crc, b ); // update CRC value + crc = sf_crc16(crc, b); // update CRC value } txBuf[bufPos++] = LOWERBYTE(crc); txBuf[bufPos] = UPPERBYTE(crc); - } /*! @@ -513,13 +511,13 @@ void sf_MakePacket( uint8_t *txBuf, const uint8_t * pdata, uint16_t length, uint * */ -static void sf_SendAckPacket( Port_t *thisport, uint8_t seqNumber) +static void sf_SendAckPacket(Port_t *thisport, uint8_t seqNumber) { - uint8_t AckSeqNumber = SETBIT( seqNumber, ACK_BIT ); + uint8_t AckSeqNumber = SETBIT(seqNumber, ACK_BIT); // create the packet, note we pass AckSequenceNumber directly - sf_MakePacket( thisport->txBuf, NULL, 0, AckSeqNumber ); - sf_SendPacket( thisport ); + sf_MakePacket(thisport->txBuf, NULL, 0, AckSeqNumber); + sf_SendPacket(thisport); // we don't set the timeout for an ACK because we don't ACK our ACKs in this protocol } @@ -532,30 +530,30 @@ static void sf_SendAckPacket( Port_t *thisport, uint8_t seqNumber) * \note * */ -static void sf_write_byte( Port_t *thisport, uint8_t c ) +static void sf_write_byte(Port_t *thisport, uint8_t c) { - if( c == SYNC ) { // check for SYNC byte - thisport->pfSerialWrite( ESC ); // since we are not starting a packet we must ESCAPE the SYNCH byte - thisport->pfSerialWrite( ESC_SYNC ); // now send the escaped synch char - } else if( c == ESC ) { // Check for ESC character - thisport->pfSerialWrite( ESC ); // if it is, we need to send it twice - thisport->pfSerialWrite( ESC ); - } else { - thisport->pfSerialWrite( c ); // otherwise write the byte to serial port - } + if (c == SYNC) { // check for SYNC byte + thisport->pfSerialWrite(ESC); // since we are not starting a packet we must ESCAPE the SYNCH byte + thisport->pfSerialWrite(ESC_SYNC); // now send the escaped synch char + } else if (c == ESC) { // Check for ESC character + thisport->pfSerialWrite(ESC); // if it is, we need to send it twice + thisport->pfSerialWrite(ESC); + } else { + thisport->pfSerialWrite(c); // otherwise write the byte to serial port + } } /************************************************************************************************************ -* -* NAME: uint16_t ssp_crc16( uint16_t crc, uint16_t data ) -* DESCRIPTION: Uses crc_table to calculate new crc -* ARGUMENTS: -* arg1: crc -* arg2: data - byte to calculate into CRC -* RETURN: New crc -* CREATED: 5/8/02 -* -*************************************************************************************************************/ + * + * NAME: uint16_t ssp_crc16( uint16_t crc, uint16_t data ) + * DESCRIPTION: Uses crc_table to calculate new crc + * ARGUMENTS: + * arg1: crc + * arg2: data - byte to calculate into CRC + * RETURN: New crc + * CREATED: 5/8/02 + * + *************************************************************************************************************/ /*! * \brief calculates the new CRC value for 'data' * \param crc = current CRC value @@ -566,9 +564,9 @@ static void sf_write_byte( Port_t *thisport, uint8_t c ) * */ -static uint16_t sf_crc16( uint16_t crc, uint8_t data ) +static uint16_t sf_crc16(uint16_t crc, uint8_t data) { - return (crc >> 8) ^ CRC_TABLE[( crc ^ data ) & 0x00FF ]; + return (crc >> 8) ^ CRC_TABLE[(crc ^ data) & 0x00FF]; } @@ -581,11 +579,12 @@ static uint16_t sf_crc16( uint16_t crc, uint8_t data ) * */ -static void sf_SetSendTimeout( Port_t *thisport ) +static void sf_SetSendTimeout(Port_t *thisport) { - uint32_t timeout; + uint32_t timeout; + timeout = thisport->pfGetTime() + thisport->timeoutLen; - thisport->timeout = timeout; + thisport->timeout = timeout; } /*! @@ -597,13 +596,13 @@ static void sf_SetSendTimeout( Port_t *thisport ) * \note * */ -static uint16_t sf_CheckTimeout( Port_t *thisport ) +static uint16_t sf_CheckTimeout(Port_t *thisport) { - uint16_t retval = FALSE; - uint32_t current_time; + uint16_t retval = FALSE; + uint32_t current_time; current_time = thisport->pfGetTime(); - if( current_time > thisport->timeout ) { + if (current_time > thisport->timeout) { retval = TRUE; } return retval; @@ -611,15 +610,15 @@ static uint16_t sf_CheckTimeout( Port_t *thisport ) /**************************************************************************** - * NAME: sf_ReceiveState - * DESC: Implements the receive state handling code for escaped and unescaped data - * ARGS: thisport - which port to operate on - * c - incoming byte - * RETURN: - * CREATED: - * NOTES: - * 1. change from using pointer to functions. - ****************************************************************************/ +* NAME: sf_ReceiveState +* DESC: Implements the receive state handling code for escaped and unescaped data +* ARGS: thisport - which port to operate on +* c - incoming byte +* RETURN: +* CREATED: +* NOTES: +* 1. change from using pointer to functions. +****************************************************************************/ /*! * \brief implements the receive state handling code for escaped and unescaped data * \param thisport = which port to use @@ -629,46 +628,46 @@ static uint16_t sf_CheckTimeout( Port_t *thisport ) * \note * */ -static int16_t sf_ReceiveState( Port_t *thisport, uint8_t c ) +static int16_t sf_ReceiveState(Port_t *thisport, uint8_t c) { - int16_t retval = SSP_RX_RECEIVING; + int16_t retval = SSP_RX_RECEIVING; - switch( thisport->InputState ) { - case state_unescaped_e: - if( c == SYNC ) { - thisport->DecodeState = decode_len1_e; - } else if ( c == ESC ) { - thisport->InputState = state_escaped_e; - } else { - retval = sf_DecodeState( thisport, c); - } - break; // end of unescaped state. - case state_escaped_e: - thisport->InputState = state_unescaped_e; - if( c == SYNC ) { - thisport->DecodeState = decode_len1_e; - } else if (c == ESC_SYNC ) { - retval = sf_DecodeState( thisport, SYNC); - } else { - retval = sf_DecodeState( thisport, c); - } - break; // end of the escaped state. - default: - break; - } - return retval; + switch (thisport->InputState) { + case state_unescaped_e: + if (c == SYNC) { + thisport->DecodeState = decode_len1_e; + } else if (c == ESC) { + thisport->InputState = state_escaped_e; + } else { + retval = sf_DecodeState(thisport, c); + } + break; // end of unescaped state. + case state_escaped_e: + thisport->InputState = state_unescaped_e; + if (c == SYNC) { + thisport->DecodeState = decode_len1_e; + } else if (c == ESC_SYNC) { + retval = sf_DecodeState(thisport, SYNC); + } else { + retval = sf_DecodeState(thisport, c); + } + break; // end of the escaped state. + default: + break; + } + return retval; } /**************************************************************************** - * NAME: sf_DecodeState - * DESC: Implements the receive state finite state machine - * ARGS: thisport - which port to operate on - * c - incoming byte - * RETURN: - * CREATED: - * NOTES: - * 1. change from using pointer to functions. - ****************************************************************************/ +* NAME: sf_DecodeState +* DESC: Implements the receive state finite state machine +* ARGS: thisport - which port to operate on +* c - incoming byte +* RETURN: +* CREATED: +* NOTES: +* 1. change from using pointer to functions. +****************************************************************************/ /*! * \brief implements the receiving decoding state machine @@ -679,86 +678,87 @@ static int16_t sf_ReceiveState( Port_t *thisport, uint8_t c ) * \note * */ -static int16_t sf_DecodeState( Port_t *thisport, uint8_t c ) +static int16_t sf_DecodeState(Port_t *thisport, uint8_t c) { - int16_t retval; - switch( thisport->DecodeState ) { - case decode_idle_e: + int16_t retval; + + switch (thisport->DecodeState) { + case decode_idle_e: // 'c' is ignored in this state as the only way to leave the idle state is // recognition of the SYNC byte in the sf_ReceiveState function. retval = SSP_RX_IDLE; - break; - case decode_len1_e: - thisport->rxBuf[LENGTH]= c; - thisport->rxBufLen = c; - if( thisport->rxBufLen <= thisport->rxBufSize ) { - thisport->DecodeState = decode_seqNo_e; - retval = SSP_RX_RECEIVING; - } else { - thisport->DecodeState = decode_idle_e; - retval = SSP_RX_IDLE; - } - break; - case decode_seqNo_e: - thisport->rxBuf[SEQNUM] = c; - thisport->crc = 0xffff; - thisport->rxBufLen--; // subtract 1 for the seq. no. - thisport->rxBufPos = 2; + break; + case decode_len1_e: + thisport->rxBuf[LENGTH] = c; + thisport->rxBufLen = c; + if (thisport->rxBufLen <= thisport->rxBufSize) { + thisport->DecodeState = decode_seqNo_e; + retval = SSP_RX_RECEIVING; + } else { + thisport->DecodeState = decode_idle_e; + retval = SSP_RX_IDLE; + } + break; + case decode_seqNo_e: + thisport->rxBuf[SEQNUM] = c; + thisport->crc = 0xffff; + thisport->rxBufLen--; // subtract 1 for the seq. no. + thisport->rxBufPos = 2; - thisport->crc = sf_crc16( thisport->crc, c ); - if( thisport->rxBufLen > 0 ) { - thisport->DecodeState = decode_data_e; - } else { - thisport->DecodeState = decode_crc1_e; - } - retval = SSP_RX_RECEIVING; - break; - case decode_data_e: - thisport->rxBuf[ (thisport->rxBufPos)++] = c; - thisport->crc = sf_crc16( thisport->crc, c ); - if( thisport->rxBufPos == (thisport->rxBufLen+2) ) { + thisport->crc = sf_crc16(thisport->crc, c); + if (thisport->rxBufLen > 0) { + thisport->DecodeState = decode_data_e; + } else { thisport->DecodeState = decode_crc1_e; - } - retval = SSP_RX_RECEIVING; - break; - case decode_crc1_e: - thisport->crc = sf_crc16( thisport->crc, c ); - thisport->DecodeState = decode_crc2_e; - retval = SSP_RX_RECEIVING; - break; - case decode_crc2_e: - thisport->DecodeState = decode_idle_e; + } + retval = SSP_RX_RECEIVING; + break; + case decode_data_e: + thisport->rxBuf[(thisport->rxBufPos)++] = c; + thisport->crc = sf_crc16(thisport->crc, c); + if (thisport->rxBufPos == (thisport->rxBufLen + 2)) { + thisport->DecodeState = decode_crc1_e; + } + retval = SSP_RX_RECEIVING; + break; + case decode_crc1_e: + thisport->crc = sf_crc16(thisport->crc, c); + thisport->DecodeState = decode_crc2_e; + retval = SSP_RX_RECEIVING; + break; + case decode_crc2_e: + thisport->DecodeState = decode_idle_e; // verify the CRC value for the packet - if( sf_crc16( thisport->crc, c) == 0) { - // TODO shouldn't the return value of sf_ReceivePacket() be checked? - sf_ReceivePacket( thisport ); - retval = SSP_RX_COMPLETE; - } else { - thisport->RxError++; - retval = SSP_RX_IDLE; - } - break; - default: - thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet. - retval = SSP_RX_IDLE; - break; - } - return retval; + if (sf_crc16(thisport->crc, c) == 0) { + // TODO shouldn't the return value of sf_ReceivePacket() be checked? + sf_ReceivePacket(thisport); + retval = SSP_RX_COMPLETE; + } else { + thisport->RxError++; + retval = SSP_RX_IDLE; + } + break; + default: + thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet. + retval = SSP_RX_IDLE; + break; + } + return retval; } /************************************************************************************************************ -* -* NAME: int16_t sf_ReceivePacket( ) -* DESCRIPTION: Receive one packet, assumed that data is in rec.buff[] -* ARGUMENTS: -* RETURN: 0 . no new packet was received, could be ack or same packet -* 1 . new packet received -* SSP_PACKET_? -* SSP_PACKET_COMPLETE -* SSP_PACKET_ACK -* CREATED: 5/8/02 -* -*************************************************************************************************************/ + * + * NAME: int16_t sf_ReceivePacket( ) + * DESCRIPTION: Receive one packet, assumed that data is in rec.buff[] + * ARGUMENTS: + * RETURN: 0 . no new packet was received, could be ack or same packet + * 1 . new packet received + * SSP_PACKET_? + * SSP_PACKET_COMPLETE + * SSP_PACKET_ACK + * CREATED: 5/8/02 + * + *************************************************************************************************************/ /*! * \brief receive one packet. calls the callback function if needed. * \param thisport = which port to use @@ -772,46 +772,45 @@ static int16_t sf_DecodeState( Port_t *thisport, uint8_t c ) static int16_t sf_ReceivePacket(Port_t *thisport) { - int16_t value = FALSE; + int16_t value = FALSE; - if( ISBITSET(thisport->rxBuf[SEQNUM], ACK_BIT ) ) { - // Received an ACK packet, need to check if it matches the previous sent packet - if( ( thisport->rxBuf[SEQNUM] & 0x7F) == (thisport->txSeqNo & 0x7f)) { - // It matches the last packet sent by us - SETBIT( thisport->txSeqNo, ACK_BIT ); - thisport->SendState = SSP_ACKED; - value = FALSE; - } + if (ISBITSET(thisport->rxBuf[SEQNUM], ACK_BIT)) { + // Received an ACK packet, need to check if it matches the previous sent packet + if ((thisport->rxBuf[SEQNUM] & 0x7F) == (thisport->txSeqNo & 0x7f)) { + // It matches the last packet sent by us + SETBIT(thisport->txSeqNo, ACK_BIT); + thisport->SendState = SSP_ACKED; + value = FALSE; + } // else ignore the ACK packet - } else { - // Received a 'data' packet, figure out what type of packet we received... - if( thisport->rxBuf[SEQNUM] == 0 ) { - // Synchronize sequence number with host + } else { + // Received a 'data' packet, figure out what type of packet we received... + if (thisport->rxBuf[SEQNUM] == 0) { + // Synchronize sequence number with host #ifdef ACTIVE_SYNCH thisport->sendSynch = TRUE; #endif - sf_SendAckPacket( thisport, thisport->rxBuf[SEQNUM] ); - thisport->rxSeqNo = 0; - value = FALSE; - } else if( thisport->rxBuf[SEQNUM] == thisport->rxSeqNo ) { - // Already seen this packet, just ack it, don't act on the packet. - sf_SendAckPacket( thisport, thisport->rxBuf[SEQNUM] ); - value = FALSE; - } else { - //New Packet - thisport->rxSeqNo = thisport->rxBuf[SEQNUM]; - // Let the application do something with the data/packet. - if( thisport->pfCallBack != NULL ) { - // skip the first two bytes (length and seq. no.) in the buffer. - thisport->pfCallBack( &(thisport->rxBuf[2]), thisport->rxBufLen); - } - // after we send the ACK, it is possible for the host to send a new packet. - // Thus the application needs to copy the data and reset the receive buffer - // inside of thisport->pfCallBack() - sf_SendAckPacket( thisport, thisport->rxBuf[SEQNUM] ); - value = TRUE; - } - } - return value; + sf_SendAckPacket(thisport, thisport->rxBuf[SEQNUM]); + thisport->rxSeqNo = 0; + value = FALSE; + } else if (thisport->rxBuf[SEQNUM] == thisport->rxSeqNo) { + // Already seen this packet, just ack it, don't act on the packet. + sf_SendAckPacket(thisport, thisport->rxBuf[SEQNUM]); + value = FALSE; + } else { + // New Packet + thisport->rxSeqNo = thisport->rxBuf[SEQNUM]; + // Let the application do something with the data/packet. + if (thisport->pfCallBack != NULL) { + // skip the first two bytes (length and seq. no.) in the buffer. + thisport->pfCallBack(&(thisport->rxBuf[2]), thisport->rxBufLen); + } + // after we send the ACK, it is possible for the host to send a new packet. + // Thus the application needs to copy the data and reset the receive buffer + // inside of thisport->pfCallBack() + sf_SendAckPacket(thisport, thisport->rxBuf[SEQNUM]); + value = TRUE; + } + } + return value; } - diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/ssp.h b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/ssp.h index e269043ea..2a2f4bff1 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/ssp.h +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/SSP/ssp.h @@ -1,9 +1,9 @@ /******************************************************************* - * - * NAME: ssp.h - * - * - *******************************************************************/ +* +* NAME: ssp.h +* +* +*******************************************************************/ #ifndef SSP_H #define SSP_H /** INCLUDE FILES **/ @@ -11,111 +11,108 @@ /** LOCAL DEFINITIONS **/ #ifndef TRUE -#define TRUE 1 +#define TRUE 1 #endif #ifndef FALSE -#define FALSE 0 +#define FALSE 0 #endif -#define SSP_TX_IDLE 0 // not expecting a ACK packet (no current transmissions in progress) -#define SSP_TX_WAITING 1 // waiting for a valid ACK to arrive -#define SSP_TX_TIMEOUT 2 // failed to receive a valid ACK in the timeout period, after retrying. -#define SSP_TX_ACKED 3 // valid ACK received before timeout period. -#define SSP_TX_BUFOVERRUN 4 // amount of data to send execeds the transmission buffer sizeof -#define SSP_TX_BUSY 5 // Attempted to start a transmission while a transmission was already in progress. -//#define SSP_TX_FAIL - failure... +#define SSP_TX_IDLE 0 // not expecting a ACK packet (no current transmissions in progress) +#define SSP_TX_WAITING 1 // waiting for a valid ACK to arrive +#define SSP_TX_TIMEOUT 2 // failed to receive a valid ACK in the timeout period, after retrying. +#define SSP_TX_ACKED 3 // valid ACK received before timeout period. +#define SSP_TX_BUFOVERRUN 4 // amount of data to send execeds the transmission buffer sizeof +#define SSP_TX_BUSY 5 // Attempted to start a transmission while a transmission was already in progress. +// #define SSP_TX_FAIL - failure... -#define SSP_RX_IDLE 0 -#define SSP_RX_RECEIVING 1 -#define SSP_RX_COMPLETE 2 +#define SSP_RX_IDLE 0 +#define SSP_RX_RECEIVING 1 +#define SSP_RX_COMPLETE 2 // types of packet that can be received -#define SSP_RX_DATA 5 -#define SSP_RX_ACK 6 -#define SSP_RX_SYNCH 7 +#define SSP_RX_DATA 5 +#define SSP_RX_ACK 6 +#define SSP_RX_SYNCH 7 typedef enum decodeState_ { - decode_len1_e = 0, - decode_seqNo_e, - decode_data_e, - decode_crc1_e, - decode_crc2_e, - decode_idle_e + decode_len1_e = 0, + decode_seqNo_e, + decode_data_e, + decode_crc1_e, + decode_crc2_e, + decode_idle_e } DecodeState_t; typedef enum ReceiveState { - state_escaped_e = 0, - state_unescaped_e + state_escaped_e = 0, + state_unescaped_e } ReceiveState_t; -typedef struct -{ - uint8_t *pbuff; - uint16_t length; - uint16_t crc; - uint8_t seqNo; +typedef struct { + uint8_t *pbuff; + uint16_t length; + uint16_t crc; + uint8_t seqNo; } Packet_t; typedef struct { - - uint8_t *rxBuf; // Buffer used to store rcv data - uint16_t rxBufSize; // rcv buffer size. - uint8_t *txBuf; // Length of data in buffer - uint16_t txBufSize; // CRC for data in Packet buff - uint16_t max_retry; // Maximum number of retrys for a single transmit. - int32_t timeoutLen; // how long to wait for each retry to succeed - void (*pfCallBack)( uint8_t *, uint16_t); // call back function that is called when a full packet has been received - int16_t (*pfSerialRead)(void); // function to call to read a byte from serial hardware - void (*pfSerialWrite)( uint8_t ); // function used to write a byte to serial hardware for transmission - uint32_t (*pfGetTime)(void); // function returns time in number of seconds that has elapsed from a given reference point -} PortConfig_t; + uint8_t *rxBuf; // Buffer used to store rcv data + uint16_t rxBufSize; // rcv buffer size. + uint8_t *txBuf; // Length of data in buffer + uint16_t txBufSize; // CRC for data in Packet buff + uint16_t max_retry; // Maximum number of retrys for a single transmit. + int32_t timeoutLen; // how long to wait for each retry to succeed + void (*pfCallBack)(uint8_t *, uint16_t); // call back function that is called when a full packet has been received + int16_t (*pfSerialRead)(void); // function to call to read a byte from serial hardware + void (*pfSerialWrite)(uint8_t); // function used to write a byte to serial hardware for transmission + uint32_t (*pfGetTime)(void); // function returns time in number of seconds that has elapsed from a given reference point +} PortConfig_t; typedef struct Port_tag { - void (*pfCallBack)( uint8_t *, uint16_t); // call back function that is called when a full packet has been received - int16_t (*pfSerialRead)(void); // function to read a character from the serial input stream - void (*pfSerialWrite)( uint8_t ); // function to write a byte to be sent out the serial port - uint32_t (*pfGetTime)(void); // function returns time in number of seconds that has elapsed from a given reference point - uint8_t retryCount; // how many times have we tried to transmit the 'send' packet - uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet - int32_t timeoutLen; // how long to wait for each retry to succeed - int32_t timeout; // current timeout. when 'time' reaches this point we have timed out - uint8_t txSeqNo; // current 'send' packet sequence number - uint16_t rxBufPos; // current buffer position in the receive packet - uint16_t rxBufLen; // number of 'data' bytes in the buffer - uint8_t rxSeqNo; // current 'receive' packet number - uint16_t rxBufSize; // size of the receive buffer. - uint16_t txBufSize; // size of the transmit buffer. - uint8_t *txBuf; // transmit buffer. REquired to store a copy of packet data in case a retry is needed. - uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received. - uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host - // this is required when switching from the application to the bootloader - // and vice-versa. This fixes the firwmare download timeout. - // when this flag is set to true, the next time we send a packet we will first - // send a synchronize packet. - ReceiveState_t InputState; - DecodeState_t DecodeState; - uint16_t SendState; - uint16_t crc; - uint32_t RxError; - uint32_t TxError; - uint16_t flags; + void (*pfCallBack)(uint8_t *, uint16_t); // call back function that is called when a full packet has been received + int16_t (*pfSerialRead)(void); // function to read a character from the serial input stream + void (*pfSerialWrite)(uint8_t); // function to write a byte to be sent out the serial port + uint32_t (*pfGetTime)(void); // function returns time in number of seconds that has elapsed from a given reference point + uint8_t retryCount; // how many times have we tried to transmit the 'send' packet + uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet + int32_t timeoutLen; // how long to wait for each retry to succeed + int32_t timeout; // current timeout. when 'time' reaches this point we have timed out + uint8_t txSeqNo; // current 'send' packet sequence number + uint16_t rxBufPos; // current buffer position in the receive packet + uint16_t rxBufLen; // number of 'data' bytes in the buffer + uint8_t rxSeqNo; // current 'receive' packet number + uint16_t rxBufSize; // size of the receive buffer. + uint16_t txBufSize; // size of the transmit buffer. + uint8_t *txBuf; // transmit buffer. REquired to store a copy of packet data in case a retry is needed. + uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received. + uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host + // this is required when switching from the application to the bootloader + // and vice-versa. This fixes the firwmare download timeout. + // when this flag is set to true, the next time we send a packet we will first + // send a synchronize packet. + ReceiveState_t InputState; + DecodeState_t DecodeState; + uint16_t SendState; + uint16_t crc; + uint32_t RxError; + uint32_t TxError; + uint16_t flags; } Port_t; - /** Public Data **/ /** PUBLIC FUNCTIONS **/ -int16_t ssp_ReceiveProcess( Port_t *thisport ); -int16_t ssp_SendProcess( Port_t *thisport ); -uint16_t ssp_SendString( Port_t *thisport, char *str ); -int16_t ssp_SendData(Port_t *thisport, const uint8_t * data,const uint16_t length ); -void ssp_Init( Port_t *thisport, const PortConfig_t* const info); -int16_t ssp_ReceiveByte(Port_t *thisport ); -uint16_t ssp_Synchronise( Port_t *thisport ); +int16_t ssp_ReceiveProcess(Port_t *thisport); +int16_t ssp_SendProcess(Port_t *thisport); +uint16_t ssp_SendString(Port_t *thisport, char *str); +int16_t ssp_SendData(Port_t *thisport, const uint8_t *data, const uint16_t length); +void ssp_Init(Port_t *thisport, const PortConfig_t *const info); +int16_t ssp_ReceiveByte(Port_t *thisport); +uint16_t ssp_Synchronise(Port_t *thisport); /** EXTERNAL FUNCTIONS **/ -#endif +#endif // ifndef SSP_H diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.cpp b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.cpp index 3ef75e8ee..2d6335dfe 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.cpp +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.cpp @@ -1,3 +1 @@ - #include "delay.h" - diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.h b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.h index 7119a770d..6862e7a9d 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.h +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/delay.h @@ -2,8 +2,7 @@ #define DELAY_H #include -class delay : public QThread -{ +class delay : public QThread { public: static void msleep(unsigned long msecs) { diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/main.cpp b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/main.cpp index c9c687986..e5c98386b 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/main.cpp +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/main.cpp @@ -11,314 +11,243 @@ int main(int argc, char *argv[]) ///SSP///////////////////////////////// - ///////////////////////////////////////////// QCoreApplication a(argc, argv); -// argc=4; -// argv[1]="-ls"; -// argv[2]="-t"; -// argv[3]="COM3"; - if(argc>1||!PRIVATE) - { - bool use_serial=false; + +// argc=4; +// argv[1]="-ls"; +// argv[2]="-t"; +// argv[3]="COM3"; + if (argc > 1 || !PRIVATE) { + bool use_serial = false; bool verify; - bool debug=false; - bool umodereset=false; + bool debug = false; + bool umodereset = false; OP_DFU::Actions action; QString file; QString serialport; QString description; - int device=-1; + int device = -1; QStringList args; - for(int i=0;i : program hw (requires:-d - optionals:-v,-w) |\n"; - cout<<"| -v : verify (requires:-d) |\n"; - cout<<"| -dn : download firmware to file (requires:-d) |\n"; - // cout<<"| -dd : download discription (requires:-d) |\n"; - cout<<"| -d : (requires: -p) |\n"; - cout<<"| -ca : compares byte by byte current firmware with file|\n"; - cout<<"| -cc : compares CRC of current firmware with file |\n"; - cout<<"| -s : requests status of device |\n"; - cout<<"| -r : resets the device |\n"; - cout<<"| -j : exits bootloader and jumps to user FW |\n"; - cout<<"| -debug : prints debug information |\n"; - cout<<"| -t : uses serial port(requires:-ur) |\n"; - cout<<"| -ur : user mode reset* |\n"; - cout<<"| |\n"; - cout<<"| examples: |\n"; - cout<<"| |\n"; - cout<<"| program and verify device #0 |\n"; - cout<<"| OPUploadTool -p c:/OpenPilot.bin -w \"Openpilot Firmware\" -v -d 0 |\n"; - cout<<"| |\n"; - cout<<"| Perform a quick compare of FW in file with FW in device #1 |\n"; - cout<<"| OPUploadTool -ch c:/OpenPilot2.bin -d 2 |\n"; - cout<<"| |\n"; - cout<<"| *requires valid user space firmwares already running |\n"; - cout<<"|________________________________________________________________________|\n"; + if (args.contains("-debug")) { + debug = true; + } + if (args.contains("-ur")) { + umodereset = true; + } + if (args.contains("-?") || (!PRIVATE && argc == 1)) { + cout << "_________________________________________________________________________\n"; + cout << "| Commands |\n"; + cout << "| |\n"; + cout << "| -ls : lists available devices |\n"; + cout << "| -p : program hw (requires:-d - optionals:-v,-w) |\n"; + cout << "| -v : verify (requires:-d) |\n"; + cout << "| -dn : download firmware to file (requires:-d) |\n"; + // cout<<"| -dd : download discription (requires:-d) |\n"; + cout << "| -d : (requires: -p) |\n"; + cout << "| -ca : compares byte by byte current firmware with file|\n"; + cout << "| -cc : compares CRC of current firmware with file |\n"; + cout << "| -s : requests status of device |\n"; + cout << "| -r : resets the device |\n"; + cout << "| -j : exits bootloader and jumps to user FW |\n"; + cout << "| -debug : prints debug information |\n"; + cout << "| -t : uses serial port(requires:-ur) |\n"; + cout << "| -ur : user mode reset* |\n"; + cout << "| |\n"; + cout << "| examples: |\n"; + cout << "| |\n"; + cout << "| program and verify device #0 |\n"; + cout << "| OPUploadTool -p c:/OpenPilot.bin -w \"Openpilot Firmware\" -v -d 0 |\n"; + cout << "| |\n"; + cout << "| Perform a quick compare of FW in file with FW in device #1 |\n"; + cout << "| OPUploadTool -ch c:/OpenPilot2.bin -d 2 |\n"; + cout << "| |\n"; + cout << "| *requires valid user space firmwares already running |\n"; + cout << "|________________________________________________________________________|\n"; return 0; - - - } - - else if(args.contains(PROGRAMFW)) - { - if(args.contains(VERIFY)) - { - verify=true; + } else if (args.contains(PROGRAMFW)) { + if (args.contains(VERIFY)) { + verify = true; + } else { + verify = false; } - else - verify=false; - if(args.contains(PROGRAMDESC)) - { - if(args.indexOf(PROGRAMDESC)+1dfu.numberOfDevices-1) - { - cout<<"Error:Invalid Device"; + if (device > dfu.numberOfDevices - 1) { + cout << "Error:Invalid Device"; return -1; } -// if(dfu.numberOfDevices==1) -// dfu.use_delay=false; - if(!dfu.enterDFU(device)) - { - cout<<"Error:Could not enter DFU mode\n"; +// if(dfu.numberOfDevices==1) +// dfu.use_delay=false; + if (!dfu.enterDFU(device)) { + cout << "Error:Could not enter DFU mode\n"; return -1; } - if (action==OP_DFU::actionProgram) - { - if(((OP_DFU::device)dfu.devices[device]).Writable==false) - { - cout<<"ERROR device not Writable\n"; + if (action == OP_DFU::actionProgram) { + if (((OP_DFU::device)dfu.devices[device]).Writable == false) { + cout << "ERROR device not Writable\n"; return false; } - OP_DFU::Status retstatus=dfu.UploadFirmware(file.toAscii(),verify, device); - if(retstatus!=OP_DFU::Last_operation_Success) - { - cout<<"Upload failed with code:"< #include -OP_DFU::OP_DFU(bool _debug,bool _use_serial,QString portname,bool umodereset): debug(_debug),use_serial(_use_serial),mready(true) +OP_DFU::OP_DFU(bool _debug, bool _use_serial, QString portname, bool umodereset) : debug(_debug), use_serial(_use_serial), mready(true) { - if(use_serial) - { - cout<<"Connect hw now and press any key"; - // getch(); + if (use_serial) { + cout << "Connect hw now and press any key"; + // getch(); // delay::msleep(2000); PortSettings settings; - settings.BaudRate=BAUD57600; - settings.DataBits=DATA_8; - settings.FlowControl=FLOW_OFF; - settings.Parity=PAR_NONE; - settings.StopBits=STOP_1; - settings.Timeout_Millisec=1000; - info=new port(settings,portname); - info->rxBuf = sspRxBuf; - info->rxBufSize = MAX_PACKET_DATA_LEN; - info->txBuf = sspTxBuf; - info->txBufSize = MAX_PACKET_DATA_LEN; - info->max_retry = 10; - info->timeoutLen = 1000; - if(info->status()!=port::open) - { - cout<<"Could not open serial port\n"; - mready=false; + settings.BaudRate = BAUD57600; + settings.DataBits = DATA_8; + settings.FlowControl = FLOW_OFF; + settings.Parity = PAR_NONE; + settings.StopBits = STOP_1; + settings.Timeout_Millisec = 1000; + info = new port(settings, portname); + info->rxBuf = sspRxBuf; + info->rxBufSize = MAX_PACKET_DATA_LEN; + info->txBuf = sspTxBuf; + info->txBufSize = MAX_PACKET_DATA_LEN; + info->max_retry = 10; + info->timeoutLen = 1000; + if (info->status() != port::open) { + cout << "Could not open serial port\n"; + mready = false; return; } - if(umodereset) + if (umodereset) { sendReset(); - delay::msleep(5000); - serialhandle=new qsspt(info,debug); - - while(serialhandle->ssp_Synchronise()==false) - { - if (debug) - qDebug()<<"SYNC failed, resending"; } - qDebug()<<"SYNC Succeded"; - serialhandle->start(); - // serialhandle->start(); - } - else - { + delay::msleep(5000); + serialhandle = new qsspt(info, debug); - send_delay=10; - use_delay=true; - int numDevices=0; - cout<<"Please connect device now\n"; - int count=0; - while(numDevices==0) - { - cout<<"."; - delay::msleep(500); - numDevices = hidHandle.open(1,0x20a0,0x415A,0,0); //0xff9c,0x0001); - if(numDevices==0) - numDevices = hidHandle.open(1,0x20a0,0x415B,0,0); //0xff9c,0x0001); - if(++count==10) - { - cout<<"\r"; - cout<<" "; - cout<<"\r"; - count=0; + while (serialhandle->ssp_Synchronise() == false) { + if (debug) { + qDebug() << "SYNC failed, resending"; } } - if(debug) + qDebug() << "SYNC Succeded"; + serialhandle->start(); + // serialhandle->start(); + } else { + send_delay = 10; + use_delay = true; + int numDevices = 0; + cout << "Please connect device now\n"; + int count = 0; + while (numDevices == 0) { + cout << "."; + delay::msleep(500); + numDevices = hidHandle.open(1, 0x20a0, 0x415A, 0, 0); // 0xff9c,0x0001); + if (numDevices == 0) { + numDevices = hidHandle.open(1, 0x20a0, 0x415B, 0, 0); // 0xff9c,0x0001); + } + if (++count == 10) { + cout << "\r"; + cout << " "; + cout << "\r"; + count = 0; + } + } + if (debug) { qDebug() << numDevices << " device(s) opened"; - if(umodereset) - { + } + if (umodereset) { sendReset(); - qDebug()<<"before delay"; + qDebug() << "before delay"; delay::msleep(5000); - qDebug()<<"after delay"; - if(hidHandle.open(1,0x20a0,0x4117,0,0)==0) - mready=false; + qDebug() << "after delay"; + if (hidHandle.open(1, 0x20a0, 0x4117, 0, 0) == 0) { + mready = false; + } } } } void OP_DFU::sendReset(void) { - qDebug()<<"Requesting user mode reset"; - char aa[255]= {0x02,0x3E,0x3C,0x20,0x75,0x00,0x20,0x4F,0x67,0x34,0x62,0x04,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};//63 - char ba[255]= {0x02,0x3E,0x3C,0x20,0x75,0x00,0x20,0x4F,0x67,0x34,0xB9,0x08,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; - char ca[255]= {0x02,0x3E,0x3C,0x20,0x75,0x00,0x20,0x4F,0x67,0x34,0x10,0x0D,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; - char bb[255]={0x02,0x3D,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x28,0x8D,0x19,0x00,0x00,0x13}; - char ab[255]={0x02,0x3D,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x47,0x1B,0x19,0x00,0x00,0x76}; - char cb[255]={0x02,0x3D,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x66,0x78,0x1C,0x00,0x00,0x25}; - //125 - if(!use_serial) - { - hidHandle.send(0,aa, 64, 5000); - hidHandle.send(0,ab, 64, 5000); - delay::msleep(600); - hidHandle.send(0,ba, 64, 5000); - hidHandle.send(0,bb, 64, 5000); - delay::msleep(600); - hidHandle.send(0,ca, 64, 5000); - hidHandle.send(0,cb, 64, 5000); - delay::msleep(100); - hidHandle.close(1); -} - else - { + qDebug() << "Requesting user mode reset"; + char aa[255] = { 0x02, 0x3E, 0x3C, 0x20, 0x75, 0x00, 0x20, 0x4F, 0x67, 0x34, 0x62, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; // 63 + char ba[255] = { 0x02, 0x3E, 0x3C, 0x20, 0x75, 0x00, 0x20, 0x4F, 0x67, 0x34, 0xB9, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + char ca[255] = { 0x02, 0x3E, 0x3C, 0x20, 0x75, 0x00, 0x20, 0x4F, 0x67, 0x34, 0x10, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + char bb[255] = { 0x02, 0x3D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x28, 0x8D, 0x19, 0x00, 0x00, 0x13 }; + char ab[255] = { 0x02, 0x3D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x1B, 0x19, 0x00, 0x00, 0x76 }; + char cb[255] = { 0x02, 0x3D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x66, 0x78, 0x1C, 0x00, 0x00, 0x25 }; + // 125 + if (!use_serial) { + hidHandle.send(0, aa, 64, 5000); + hidHandle.send(0, ab, 64, 5000); + delay::msleep(600); + hidHandle.send(0, ba, 64, 5000); + hidHandle.send(0, bb, 64, 5000); + delay::msleep(600); + hidHandle.send(0, ca, 64, 5000); + hidHandle.send(0, cb, 64, 5000); + delay::msleep(100); + hidHandle.close(1); + } else { char a[255]; char b[255]; char c[255]; - memcpy (a,aa+2,62); - memcpy (a+62,ab+2,60); - memcpy (b,ba+2,62); - memcpy (b+62,bb+2,60); - memcpy (c,ca+2,62); - memcpy (c+62,cb+2,60); - for(int x=0;x<123;++x) + memcpy(a, aa + 2, 62); + memcpy(a + 62, ab + 2, 60); + memcpy(b, ba + 2, 62); + memcpy(b + 62, bb + 2, 60); + memcpy(c, ca + 2, 62); + memcpy(c + 62, cb + 2, 60); + for (int x = 0; x < 123; ++x) { info->pfSerialWrite(a[x]); + } delay::msleep(600); - for(int x=0;x<123;++x) + for (int x = 0; x < 123; ++x) { info->pfSerialWrite(b[x]); + } delay::msleep(600); - for(int x=0;x<123;++x) + for (int x = 0; x < 123; ++x) { info->pfSerialWrite(c[x]); + } } } bool OP_DFU::SaveByteArrayToFile(QString const & sfile, const QByteArray &array) { QFile file(sfile); - //QFile file("in.txt"); - if (!file.open(QIODevice::WriteOnly)) - { - if(debug) - qDebug()<<"Cant open file"; + + // QFile file("in.txt"); + if (!file.open(QIODevice::WriteOnly)) { + if (debug) { + qDebug() << "Cant open file"; + } return false; } file.write(array); @@ -144,246 +141,248 @@ bool OP_DFU::SaveByteArrayToFile(QString const & sfile, const QByteArray &array) bool OP_DFU::enterDFU(int const &devNumber) { char buf[BUF_LEN]; - buf[0] =0x02;//reportID - buf[1] = OP_DFU::EnterDFU;//DFU Command - buf[2] = 0;//DFU Count - buf[3] = 0;//DFU Count - buf[4] = 0;//DFU Count - buf[5] = 0;//DFU Count - buf[6] = devNumber;//DFU Data0 - buf[7] = 1;//DFU Data1 - buf[8] = 1;//DFU Data2 - buf[9] = 1;//DFU Data3 + + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::EnterDFU; // DFU Command + buf[2] = 0; // DFU Count + buf[3] = 0; // DFU Count + buf[4] = 0; // DFU Count + buf[5] = 0; // DFU Count + buf[6] = devNumber; // DFU Data0 + buf[7] = 1; // DFU Data1 + buf[8] = 1; // DFU Data2 + buf[9] = 1; // DFU Data3 int result = sendData(buf, BUF_LEN); - if(result<1) + if (result < 1) { return false; - if(debug) + } + if (debug) { qDebug() << result << " bytes sent"; + } return true; } -bool OP_DFU::StartUpload(qint32 const & numberOfBytes, TransferTypes const & type,quint32 crc) +bool OP_DFU::StartUpload(qint32 const & numberOfBytes, TransferTypes const & type, quint32 crc) { int lastPacketCount; - qint32 numberOfPackets=numberOfBytes/4/14; - int pad=(numberOfBytes-numberOfPackets*4*14)/4; - if(pad==0) - { - lastPacketCount=14; - } - else - { + qint32 numberOfPackets = numberOfBytes / 4 / 14; + int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4; + + if (pad == 0) { + lastPacketCount = 14; + } else { ++numberOfPackets; - lastPacketCount=pad; + lastPacketCount = pad; } char buf[BUF_LEN]; - buf[0] =0x02;//reportID - buf[1] = setStartBit(OP_DFU::Upload);//DFU Command - buf[2] = numberOfPackets>>24;//DFU Count - buf[3] = numberOfPackets>>16;//DFU Count - buf[4] = numberOfPackets>>8;//DFU Count - buf[5] = numberOfPackets;//DFU Count - buf[6] = (int)type;//DFU Data0 - buf[7] = lastPacketCount;//DFU Data1 - buf[8] = crc>>24; - buf[9] = crc>>16; - buf[10] = crc>>8; + buf[0] = 0x02; // reportID + buf[1] = setStartBit(OP_DFU::Upload); // DFU Command + buf[2] = numberOfPackets >> 24; // DFU Count + buf[3] = numberOfPackets >> 16; // DFU Count + buf[4] = numberOfPackets >> 8; // DFU Count + buf[5] = numberOfPackets; // DFU Count + buf[6] = (int)type; // DFU Data0 + buf[7] = lastPacketCount; // DFU Data1 + buf[8] = crc >> 24; + buf[9] = crc >> 16; + buf[10] = crc >> 8; buf[11] = crc; - if(debug) - qDebug()<<"Number of packets:"<0) - { + } + if (result > 0) { return true; } return false; } -bool OP_DFU::UploadData(qint32 const & numberOfBytes, QByteArray & data) +bool OP_DFU::UploadData(qint32 const & numberOfBytes, QByteArray & data) { int lastPacketCount; - qint32 numberOfPackets=numberOfBytes/4/14; - int pad=(numberOfBytes-numberOfPackets*4*14)/4; - if(pad==0) - { - lastPacketCount=14; - } - else - { + qint32 numberOfPackets = numberOfBytes / 4 / 14; + int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4; + + if (pad == 0) { + lastPacketCount = 14; + } else { ++numberOfPackets; - lastPacketCount=pad; + lastPacketCount = pad; + } + if (debug) { + qDebug() << "Start Uploading:" << numberOfPackets << "4Bytes"; } - if(debug) - qDebug()<<"Start Uploading:"<>24;//DFU Count - buf[3] = packetcount>>16;//DFU Count - buf[4] = packetcount>>8;//DFU Count - buf[5] = packetcount;//DFU Count - char *pointer=data.data(); - pointer=pointer+4*14*packetcount; - // qDebug()<<"Packet Number="<>24;//DFU Count - buf[3] = numberOfPackets>>16;//DFU Count - buf[4] = numberOfPackets>>8;//DFU Count - buf[5] = numberOfPackets;//DFU Count - buf[6] = (int)type;//DFU Data0 - buf[7] = lastPacketCount;//DFU Data1 - buf[8] = 1;//DFU Data2 - buf[9] = 1;//DFU Data3 + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::Download_Req; // DFU Command + buf[2] = numberOfPackets >> 24; // DFU Count + buf[3] = numberOfPackets >> 16; // DFU Count + buf[4] = numberOfPackets >> 8; // DFU Count + buf[5] = numberOfPackets; // DFU Count + buf[6] = (int)type; // DFU Data0 + buf[7] = lastPacketCount; // DFU Data1 + buf[8] = 1; // DFU Data2 + buf[9] = 1; // DFU Data3 int result = sendData(buf, BUF_LEN); - if(debug) - qDebug() << "StartDownload:"<>(x*2) & 1); - dev.Writable=(bool)(RWFlags>>(x*2+1) & 1); + dev.Readable = (bool)(RWFlags >> (x * 2) & 1); + dev.Writable = (bool)(RWFlags >> (x * 2 + 1) & 1); devices.append(dev); - buf[0] =0x02;//reportID - buf[1] = OP_DFU::Req_Capabilities;//DFU Command + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::Req_Capabilities; // DFU Command buf[2] = 0; buf[3] = 0; buf[4] = 0; buf[5] = 0; - buf[6] = x+1; + buf[6] = x + 1; buf[7] = 0; buf[8] = 0; buf[9] = 0; int result = sendData(buf, BUF_LEN); - result = receiveData(buf,BUF_LEN); - // devices[x].ID=buf[9]; - devices[x].ID=buf[14]; - devices[x].ID=devices[x].ID<<8 | (quint8)buf[15]; - devices[x].BL_Version=buf[7]; - devices[x].SizeOfDesc=buf[8]; + result = receiveData(buf, BUF_LEN); + // devices[x].ID=buf[9]; + devices[x].ID = buf[14]; + devices[x].ID = devices[x].ID << 8 | (quint8)buf[15]; + devices[x].BL_Version = buf[7]; + devices[x].SizeOfDesc = buf[8]; quint32 aux; - aux=(quint8)buf[10]; - aux=aux<<8 |(quint8)buf[11]; - aux=aux<<8 |(quint8)buf[12]; - aux=aux<<8 |(quint8)buf[13]; + aux = (quint8)buf[10]; + aux = aux << 8 | (quint8)buf[11]; + aux = aux << 8 | (quint8)buf[12]; + aux = aux << 8 | (quint8)buf[13]; - devices[x].FW_CRC=aux; + devices[x].FW_CRC = aux; - aux=(quint8)buf[2]; - aux=aux<<8 |(quint8)buf[3]; - aux=aux<<8 |(quint8)buf[4]; - aux=aux<<8 |(quint8)buf[5]; - devices[x].SizeOfCode=aux; + aux = (quint8)buf[2]; + aux = aux << 8 | (quint8)buf[3]; + aux = aux << 8 | (quint8)buf[4]; + aux = aux << 8 | (quint8)buf[5]; + devices[x].SizeOfCode = aux; } - if(debug) - { - qDebug()<<"Found "<0) + } + if (result > 0) { return true; + } return false; } -OP_DFU::Status OP_DFU::UploadFirmware(const QString &sfile, const bool &verify,int device) +OP_DFU::Status OP_DFU::UploadFirmware(const QString &sfile, const bool &verify, int device) { OP_DFU::Status ret; - cout<<"Starting Firmware Uploading...\n"; + + cout << "Starting Firmware Uploading...\n"; QFile file(sfile); - //QFile file("in.txt"); - if (!file.open(QIODevice::ReadOnly)) - { - if(debug) - qDebug()<<"Cant open file"; + // QFile file("in.txt"); + if (!file.open(QIODevice::ReadOnly)) { + if (debug) { + qDebug() << "Cant open file"; + } return OP_DFU::abort; } - QByteArray arr=file.readAll(); + QByteArray arr = file.readAll(); - if(debug) - qDebug()<<"Bytes Loaded="<"); - }else{ - bar.replace(i,1," "); + for (int i = 0; i < 50; i++) { + if (i < (percent / 2)) { + bar.replace(i, 1, "="); + } else if (i == (percent / 2)) { + bar.replace(i, 1, ">"); + } else { + bar.replace(i, 1, " "); } } - std::cout<< "\r"<> 2; // /4 Size passed in as a byte count, assumed to be a multiple of 4 + // Size = Size >> 2; // /4 Size passed in as a byte count, assumed to be a multiple of 4 - while(Size--) - { + while (Size--) { static const quint32 CrcTable[16] = { // Nibble lookup table for 0x04C11DB7 polynomial - 0x00000000,0x04C11DB7,0x09823B6E,0x0D4326D9,0x130476DC,0x17C56B6B,0x1A864DB2,0x1E475005, - 0x2608EDB8,0x22C9F00F,0x2F8AD6D6,0x2B4BCB61,0x350C9B64,0x31CD86D3,0x3C8EA00A,0x384FBDBD }; + 0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005, + 0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61, 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD + }; - Crc = Crc ^ *((quint32 *)Buffer); // Apply all 32-bits + Crc = Crc ^ *((quint32 *)Buffer); // Apply all 32-bits Buffer += 1; // Process 32-bits, 4 at a time, or 8 rounds Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // Assumes 32-bit reg, masking index to 4-bits - Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // 0x04C11DB7 Polynomial used in STM32 + Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // 0x04C11DB7 Polynomial used in STM32 Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; @@ -805,62 +795,56 @@ quint32 OP_DFU::CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer) Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; } - return(Crc); + return Crc; } quint32 OP_DFU::CRCFromQBArray(QByteArray array, quint32 Size) { - int pad=Size-array.length(); - array.append(QByteArray(pad,255)); - quint32 t[Size/4]; - for(int x=0;xsendData((uint8_t*)data+1,size-1)) - { - if (debug) - qDebug()<<"packet sent"<<"data0"<<((uint8_t*)data+1)[0]; + if (!use_serial) { + return hidHandle.send(0, data, size, 5000); + } else { + if (serialhandle->sendData((uint8_t *)data + 1, size - 1)) { + if (debug) { + qDebug() << "packet sent" << "data0" << ((uint8_t *)data + 1)[0]; + } return size; } - if(debug) - qDebug()<<"Serial send OVERRUN"; + if (debug) { + qDebug() << "Serial send OVERRUN"; + } } } -int OP_DFU::receiveData(void * data,int size) +int OP_DFU::receiveData(void *data, int size) { - if(!use_serial) - { - return hidHandle.receive(0,data, size, 10000); - } - else - { + if (!use_serial) { + return hidHandle.receive(0, data, size, 10000); + } else { int x; QTime time; time.start(); - while(true) - { - if(x=serialhandle->read_Packet(((char *) data)+1)!=-1||time.elapsed()>10000) - { - if(time.elapsed()>10000) - qDebug()<<"____timeout"; + while (true) { + if (x = serialhandle->read_Packet(((char *)data) + 1) != -1 || time.elapsed() > 10000) { + if (time.elapsed() > 10000) { + qDebug() << "____timeout"; + } return x; } } diff --git a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.h b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.h index f5b82f7e7..e3a0593ec 100644 --- a/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.h +++ b/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.h @@ -15,61 +15,55 @@ #include "SSP/port.h" #include "SSP/qsspt.h" using namespace std; -#define BUF_LEN 64 +#define BUF_LEN 64 -//Command Line Options -#define DOWNLOAD "-dn" //done -#define DEVICE "-d" //done -//#define DOWNDESCRIPTION "-dd" //done -#define PROGRAMFW "-p" //done -#define PROGRAMDESC "-w" //done -#define VERIFY "-v" //done -#define COMPARECRC "-cc" +// Command Line Options +#define DOWNLOAD "-dn" // done +#define DEVICE "-d" // done +// #define DOWNDESCRIPTION "-dd" //done +#define PROGRAMFW "-p" // done +#define PROGRAMDESC "-w" // done +#define VERIFY "-v" // done +#define COMPARECRC "-cc" #define COMPAREALL "-ca" -#define STATUSREQUEST "-s" //done -#define LISTDEVICES "-ls" //done +#define STATUSREQUEST "-s" // done +#define LISTDEVICES "-ls" // done #define RESET "-r" #define JUMP "-j" #define USE_SERIAL "-t" -#define MAX_PACKET_DATA_LEN 255 -#define MAX_PACKET_BUF_SIZE (1+1+MAX_PACKET_DATA_LEN+2) +#define MAX_PACKET_DATA_LEN 255 +#define MAX_PACKET_BUF_SIZE (1 + 1 + MAX_PACKET_DATA_LEN + 2) -class OP_DFU -{ +class OP_DFU { public: - enum TransferTypes - { + enum TransferTypes { FW, Descript }; - enum CompareType - { + enum CompareType { crccompare, bytetobytecompare }; - enum Status - { - DFUidle,//0 - uploading,//1 - wrong_packet_received,//2 - too_many_packets,//3 - too_few_packets,//4 - Last_operation_Success,//5 - downloading,//6 - idle,//7 - Last_operation_failed,//8 - uploadingStarting,//9 - outsideDevCapabilities,//10 - CRC_Fail,//11 - failed_jump,//12 - abort//13 - + enum Status { + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + idle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, // 12 + abort // 13 }; - enum Actions - { + enum Actions { actionProgram, actionProgramAndVerify, actionDownload, @@ -81,78 +75,80 @@ public: actionJump }; - enum Commands - { - Reserved,//0 - Req_Capabilities,//1 - Rep_Capabilities,//2 - EnterDFU,//3 - JumpFW,//4 - Reset,//5 - Abort_Operation,//6 - Upload,//7 - Op_END,//8 - Download_Req,//9 - Download,//10 - Status_Request,//11 - Status_Rep,//12 - + enum Commands { + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep, // 12 }; - struct device - { - int ID; + struct device { + int ID; quint32 FW_CRC; - int BL_Version; - int SizeOfDesc; + int BL_Version; + int SizeOfDesc; quint32 SizeOfCode; - bool Readable; - bool Writable; + bool Readable; + bool Writable; }; void JumpToApp(); void ResetDevice(void); bool enterDFU(int const &devNumber); - bool StartUpload(qint32 const &numberOfBytes, TransferTypes const & type,quint32 crc); - bool UploadData(qint32 const & numberOfPackets,QByteArray & data); - Status UploadDescription(QString & description); - Status UploadFirmware(const QString &sfile, const bool &verify,int device); + bool StartUpload(qint32 const &numberOfBytes, TransferTypes const & type, quint32 crc); + bool UploadData(qint32 const & numberOfPackets, QByteArray & data); + Status UploadDescription(QString & description); + Status UploadFirmware(const QString &sfile, const bool &verify, int device); Status StatusRequest(); bool EndOperation(); - void printProgBar( int const & percent,QString const& label); + void printProgBar(int const & percent, QString const & label); QString DownloadDescription(int const & numberOfChars); QByteArray StartDownload(qint32 const & numberOfBytes, TransferTypes const & type); - bool SaveByteArrayToFile(QString const & file,QByteArray const &array); - void CopyWords(char * source, char* destination, int count); + bool SaveByteArrayToFile(QString const & file, QByteArray const &array); + void CopyWords(char *source, char *destination, int count); // QByteArray DownloadData(int devNumber,int numberOfPackets); - OP_DFU(bool debug,bool use_serial,QString port,bool umodereset); + OP_DFU(bool debug, bool use_serial, QString port, bool umodereset); void sendReset(void); bool findDevices(); QList devices; int numberOfDevices; - QString StatusToString(OP_DFU::Status const & status); - OP_DFU::Status CompareFirmware(const QString &sfile, const CompareType &type,int device); + QString StatusToString(OP_DFU::Status const & status); + OP_DFU::Status CompareFirmware(const QString &sfile, const CompareType &type, int device); quint32 CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer); quint32 CRCFromQBArray(QByteArray array, quint32 Size); void test(); int send_delay; bool use_delay; - bool ready(){return mready;} + bool ready() + { + return mready; + } void AbortOperation(void); private: bool mready; bool debug; int RWFlags; bool use_serial; - qsspt * serialhandle; - int sendData(void*,int); - int receiveData(void * data,int size); + qsspt *serialhandle; + int sendData(void *, int); + int receiveData(void *data, int size); pjrc_rawhid hidHandle; - int setStartBit(int command){return command|0x20;} - uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; - uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; + int setStartBit(int command) + { + return command | 0x20; + } + uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; + uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; - port * info; + port *info; }; - #endif // OP_DFU_H diff --git a/ground/openpilotgcs/src/experimental/finaltest/main.cpp b/ground/openpilotgcs/src/experimental/finaltest/main.cpp index 6e7efd975..1bd2ce51c 100644 --- a/ground/openpilotgcs/src/experimental/finaltest/main.cpp +++ b/ground/openpilotgcs/src/experimental/finaltest/main.cpp @@ -5,6 +5,7 @@ int main(int argc, char *argv[]) { QApplication a(argc, argv); MainWindow w; + w.show(); return a.exec(); } diff --git a/ground/openpilotgcs/src/experimental/finaltest/mainwindow.cpp b/ground/openpilotgcs/src/experimental/finaltest/mainwindow.cpp index 6cd849d10..039df5ef9 100644 --- a/ground/openpilotgcs/src/experimental/finaltest/mainwindow.cpp +++ b/ground/openpilotgcs/src/experimental/finaltest/mainwindow.cpp @@ -1,28 +1,28 @@ #include "mainwindow.h" #include "ui_mainwindow.h" -//#include "../mapwidget/mapgraphicitem.h" +// #include "../mapwidget/mapgraphicitem.h" MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent), ui(new Ui::MainWindow) { - map=new mapcontrol::OPMapWidget(); + map = new mapcontrol::OPMapWidget(); map->SetShowUAV(true); map->SetShowHome(true); ui->setupUi(this); ui->comboBox->addItems(mapcontrol::Helper::MapTypes()); ui->comboBox->setCurrentIndex(mapcontrol::Helper::MapTypes().indexOf("GoogleHybrid")); - QHBoxLayout *layout=new QHBoxLayout(parent); + QHBoxLayout *layout = new QHBoxLayout(parent); layout->addWidget(map); layout->addWidget(ui->widget); ui->centralWidget->setLayout(layout); - QTimer * timer=new QTimer; + QTimer *timer = new QTimer; timer->setInterval(500); timer->start(); - connect(map,SIGNAL(zoomChanged(double,double,double)),this,SLOT(zoomChanged(double,double,double))); - connect(map,SIGNAL(OnTilesStillToLoad(int)),this,SLOT(ttl(int))); - connect(timer,SIGNAL(timeout()),this,SLOT(time())); - map->WPCreate(internals::PointLatLng(38.42,-9.5),0,"lisbon"); + connect(map, SIGNAL(zoomChanged(double, double, double)), this, SLOT(zoomChanged(double, double, double))); + connect(map, SIGNAL(OnTilesStillToLoad(int)), this, SLOT(ttl(int))); + connect(timer, SIGNAL(timeout()), this, SLOT(time())); + map->WPCreate(internals::PointLatLng(38.42, -9.5), 0, "lisbon"); map->SetMouseWheelZoomType(internals::MouseWheelZoomType::MousePositionAndCenter); } @@ -40,19 +40,19 @@ void MainWindow::ttl(int value) void MainWindow::time() { internals::PointLatLng ll; + ll.SetLat(map->currentMousePosition().Lat()); ll.SetLng(map->currentMousePosition().Lng()); - // map->UAV->SetUAVPos(ll,10); + // map->UAV->SetUAVPos(ll,10); ui->label_2->setText(map->currentMousePosition().ToString()); - QGraphicsItem* itemm=map->itemAt(map->mapFromGlobal(QCursor::pos())); - mapcontrol::WayPointItem* w=qgraphicsitem_cast(itemm); - //if(w) - // qDebug()<Type; - if (itemm->Type==mapcontrol::WayPointItem::Type) - { - int x=itemm->Type; - int xx= x; - QLabel* l=new QLabel(this); + QGraphicsItem *itemm = map->itemAt(map->mapFromGlobal(QCursor::pos())); + mapcontrol::WayPointItem *w = qgraphicsitem_cast(itemm); + // if(w) + // qDebug()<Type; + if (itemm->Type == mapcontrol::WayPointItem::Type) { + int x = itemm->Type; + int xx = x; + QLabel *l = new QLabel(this); l->show(); } } @@ -60,6 +60,7 @@ void MainWindow::time() void MainWindow::changeEvent(QEvent *e) { QMainWindow::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: ui->retranslateUi(this); @@ -71,48 +72,49 @@ void MainWindow::changeEvent(QEvent *e) void MainWindow::on_pushButtonZoomP_clicked() { - double x=map->ZoomTotal(); - double y=ui->doubleSpinBox->value(); - double z=x+y; - map->SetZoom(map->ZoomTotal()+ui->doubleSpinBox->value()); + double x = map->ZoomTotal(); + double y = ui->doubleSpinBox->value(); + double z = x + y; + + map->SetZoom(map->ZoomTotal() + ui->doubleSpinBox->value()); } void MainWindow::on_pushButtonZoomM_clicked() { - map->SetZoom(map->ZoomTotal()-ui->doubleSpinBox->value()); + map->SetZoom(map->ZoomTotal() - ui->doubleSpinBox->value()); } void MainWindow::on_checkBox_clicked(bool checked) { map->SetShowTileGridLines(checked); } -void MainWindow::zoomChanged(double zoomt,double zoom,double zoomdigi) +void MainWindow::zoomChanged(double zoomt, double zoom, double zoomdigi) { - ui->label_5->setText("CurrentZoom="+QString::number(zoomt)+QString::number(zoom)+QString::number(zoomdigi)); + ui->label_5->setText("CurrentZoom=" + QString::number(zoomt) + QString::number(zoom) + QString::number(zoomdigi)); } void MainWindow::on_pushButtonRL_clicked() { map->UAV->DeleteTrail(); - // map->UAV->SetUAVHeading(10); - //map->SetRotate(map->Rotate()-1); - //map->WPCreate(); - // map->WPCreate(internals::PointLatLng(20,20),100); + // map->UAV->SetUAVHeading(10); + // map->SetRotate(map->Rotate()-1); + // map->WPCreate(); + // map->WPCreate(internals::PointLatLng(20,20),100); } void MainWindow::on_pushButtonRC_clicked() { - map->SetRotate(0); -// wp=map->WPInsert(1); + map->SetRotate(0); +// wp=map->WPInsert(1); } void MainWindow::on_pushButtonRR_clicked() { - map->SetRotate(map->Rotate()+1); - // map->WPDeleteAll(); - // map->WPDelete(wp); -// QList list= map->WPSelected(); -// int x=list.at(0)->Number(); + map->SetRotate(map->Rotate() + 1); + // map->WPDeleteAll(); + // map->WPDelete(wp); +// QList list= map->WPSelected(); +// int x=list.at(0)->Number(); } void MainWindow::on_pushButton_clicked() @@ -122,9 +124,9 @@ void MainWindow::on_pushButton_clicked() void MainWindow::on_pushButtonGO_clicked() { - core::GeoCoderStatusCode::Types x=map->SetCurrentPositionByKeywords(ui->lineEdit->text()); - ui->label->setText( mapcontrol::Helper::StrFromGeoCoderStatusCode(x)); + core::GeoCoderStatusCode::Types x = map->SetCurrentPositionByKeywords(ui->lineEdit->text()); + ui->label->setText(mapcontrol::Helper::StrFromGeoCoderStatusCode(x)); } void MainWindow::on_checkBox_2_clicked(bool checked) @@ -134,15 +136,16 @@ void MainWindow::on_checkBox_2_clicked(bool checked) void MainWindow::on_comboBox_currentIndexChanged(QString value) { - if (map->isStarted()) + if (map->isStarted()) { map->SetMapType(mapcontrol::Helper::MapTypeFromString(value)); + } } void MainWindow::on_pushButton_2_clicked() { map->RipMap(); -// QLabel *x=new QLabel(); -// ima=map->X(); -// x->setPixmap(QPixmap::fromImage(ima)); -// x->show(); +// QLabel *x=new QLabel(); +// ima=map->X(); +// x->setPixmap(QPixmap::fromImage(ima)); +// x->show(); } diff --git a/ground/openpilotgcs/src/experimental/finaltest/mainwindow.h b/ground/openpilotgcs/src/experimental/finaltest/mainwindow.h index 9dd7e509a..4a74848b6 100644 --- a/ground/openpilotgcs/src/experimental/finaltest/mainwindow.h +++ b/ground/openpilotgcs/src/experimental/finaltest/mainwindow.h @@ -5,7 +5,7 @@ #include namespace Ui { - class MainWindow; +class MainWindow; } class MainWindow : public QMainWindow { @@ -20,13 +20,13 @@ protected: private: Ui::MainWindow *ui; mapcontrol::OPMapWidget *map; - mapcontrol::WayPointItem* wp; + mapcontrol::WayPointItem *wp; QImage ima; QLabel l; private slots: void on_pushButton_2_clicked(); - void on_comboBox_currentIndexChanged(QString ); + void on_comboBox_currentIndexChanged(QString); void on_checkBox_2_clicked(bool checked); void on_pushButtonGO_clicked(); void on_pushButton_clicked(); @@ -36,7 +36,7 @@ private slots: void on_checkBox_clicked(bool checked); void on_pushButtonZoomM_clicked(); void on_pushButtonZoomP_clicked(); - void zoomChanged(double zoomt,double zoom,double zoomdigi); + void zoomChanged(double zoomt, double zoom, double zoomdigi); void ttl(int value); void time(); }; diff --git a/ground/openpilotgcs/src/experimental/finaltest/ui_mainwindow.h b/ground/openpilotgcs/src/experimental/finaltest/ui_mainwindow.h index 4a65bd5c0..7d141209d 100644 --- a/ground/openpilotgcs/src/experimental/finaltest/ui_mainwindow.h +++ b/ground/openpilotgcs/src/experimental/finaltest/ui_mainwindow.h @@ -32,8 +32,7 @@ QT_BEGIN_NAMESPACE -class Ui_MainWindow -{ +class Ui_MainWindow { public: QWidget *centralWidget; QWidget *widget; @@ -75,8 +74,9 @@ public: void setupUi(QMainWindow *MainWindow) { - if (MainWindow->objectName().isEmpty()) + if (MainWindow->objectName().isEmpty()) { MainWindow->setObjectName(QString::fromUtf8("MainWindow")); + } MainWindow->resize(524, 551); centralWidget = new QWidget(MainWindow); centralWidget->setObjectName(QString::fromUtf8("centralWidget")); @@ -174,7 +174,7 @@ public: verticalLayout_3->addWidget(groupBox_2); - groupBox_3 = new QGroupBox(widget); + groupBox_3 = new QGroupBox(widget); groupBox_3->setObjectName(QString::fromUtf8("groupBox_3")); verticalLayout = new QVBoxLayout(groupBox_3); verticalLayout->setSpacing(6); @@ -183,7 +183,7 @@ public: horizontalLayout_3 = new QHBoxLayout(); horizontalLayout_3->setSpacing(6); horizontalLayout_3->setObjectName(QString::fromUtf8("horizontalLayout_3")); - pushButtonZoomP = new QPushButton(groupBox_3); + pushButtonZoomP = new QPushButton(groupBox_3); pushButtonZoomP->setObjectName(QString::fromUtf8("pushButtonZoomP")); horizontalLayout_3->addWidget(pushButtonZoomP); @@ -260,7 +260,7 @@ public: mainToolBar = new QToolBar(MainWindow); mainToolBar->setObjectName(QString::fromUtf8("mainToolBar")); MainWindow->addToolBar(Qt::TopToolBarArea, mainToolBar); - statusBar = new QStatusBar(MainWindow); + statusBar = new QStatusBar(MainWindow); statusBar->setObjectName(QString::fromUtf8("statusBar")); MainWindow->setStatusBar(statusBar); @@ -292,11 +292,10 @@ public: checkBox_2->setText(QApplication::translate("MainWindow", "UseOpenGL", 0, QApplication::UnicodeUTF8)); pushButton->setText(QApplication::translate("MainWindow", "ReloadMap", 0, QApplication::UnicodeUTF8)); } // retranslateUi - }; namespace Ui { - class MainWindow: public Ui_MainWindow {}; +class MainWindow : public Ui_MainWindow {}; } // namespace Ui QT_END_NAMESPACE diff --git a/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/main.cpp b/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/main.cpp index 256051ba7..1a9701d74 100644 --- a/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/main.cpp +++ b/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/main.cpp @@ -1,38 +1,38 @@ /** -****************************************************************************** -* -* @file main.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup DocumentationHelper -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file main.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup DocumentationHelper + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include #include "mainwindow.h" - + int main(int argc, char *argv[]) { QApplication a(argc, argv); MainWindow w; + w.show(); return a.exec(); } - diff --git a/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.cpp b/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.cpp index 1c0b61203..c312bb0ac 100644 --- a/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.cpp +++ b/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.cpp @@ -1,66 +1,65 @@ /** -****************************************************************************** -* -* @file mainwindow.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup DocumentationHelper -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file mainwindow.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup DocumentationHelper + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "mainwindow.h" #include "ui_mainwindow.h" #include "QFileDialog" MainWindow::MainWindow(QWidget *parent) : - QMainWindow(parent), - ui(new Ui::MainWindow) + QMainWindow(parent), + ui(new Ui::MainWindow) { ui->setupUi(this); - license<<"/**"; - license<<"******************************************************************************"; - license<<"*"; - license<<"* @file %file"; - license<<"* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010."; - license<<"* @brief "; - license<<"* @see The GNU Public License (GPL) Version 3"; - license<<"* @defgroup %defgroup"; - license<<"* @{"; - license<<"* "; - license<<"*****************************************************************************/"; - license<<"/* "; - license<<"* This program is free software; you can redistribute it and/or modify "; - license<<"* it under the terms of the GNU General Public License as published by "; - license<<"* the Free Software Foundation; either version 3 of the License, or "; - license<<"* (at your option) any later version."; - license<<"* "; - license<<"* This program is distributed in the hope that it will be useful, but "; - license<<"* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY "; - license<<"* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License "; - license<<"* for more details."; - license<<"* "; - license<<"* You should have received a copy of the GNU General Public License along "; - license<<"* with this program; if not, write to the Free Software Foundation, Inc., "; - license<<"* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA"; - license<<"*/"; - + license << "/**"; + license << "******************************************************************************"; + license << "*"; + license << "* @file %file"; + license << "* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010."; + license << "* @brief "; + license << "* @see The GNU Public License (GPL) Version 3"; + license << "* @defgroup %defgroup"; + license << "* @{"; + license << "* "; + license << "*****************************************************************************/"; + license << "/* "; + license << "* This program is free software; you can redistribute it and/or modify "; + license << "* it under the terms of the GNU General Public License as published by "; + license << "* the Free Software Foundation; either version 3 of the License, or "; + license << "* (at your option) any later version."; + license << "* "; + license << "* This program is distributed in the hope that it will be useful, but "; + license << "* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY "; + license << "* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License "; + license << "* for more details."; + license << "* "; + license << "* You should have received a copy of the GNU General Public License along "; + license << "* with this program; if not, write to the Free Software Foundation, Inc., "; + license << "* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA"; + license << "*/"; } MainWindow::~MainWindow() @@ -71,6 +70,7 @@ MainWindow::~MainWindow() void MainWindow::changeEvent(QEvent *e) { QMainWindow::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: ui->retranslateUi(this); @@ -82,119 +82,110 @@ void MainWindow::changeEvent(QEvent *e) void MainWindow::on_cpathBT_clicked() { - wpath=QFileDialog::getExistingDirectory(this,"Choose Work Path"); - ui->cpathL->setText("Current Path:"+wpath); + wpath = QFileDialog::getExistingDirectory(this, "Choose Work Path"); + ui->cpathL->setText("Current Path:" + wpath); } void MainWindow::on_goBT_clicked() { QDir myDir(wpath); QStringList filter; - filter<<"*.cpp"<<"*.h"; - QStringList list = myDir.entryList (filter); - foreach(QString str,list) - { - bool go=true; - if(ui->confirmCB->isChecked()) - if(QMessageBox::question(this,"Process File?:",str,QMessageBox::Yes,QMessageBox::No)==QMessageBox::No)go=false; - if(go){ - ui->cfileL->setText("Current File:"+str); - QFile file(wpath+QDir::separator()+str); - if (!file.open(QIODevice::ReadWrite)) { - ui->output->append("Cannot open file "+str+" for writing"); + filter << "*.cpp" << "*.h"; + QStringList list = myDir.entryList(filter); + + foreach(QString str, list) { + bool go = true; + + if (ui->confirmCB->isChecked()) { + if (QMessageBox::question(this, "Process File?:", str, QMessageBox::Yes, QMessageBox::No) == QMessageBox::No) { + go = false; } - else - { - ui->output->append("Processing file "+str); + } + if (go) { + ui->cfileL->setText("Current File:" + str); + QFile file(wpath + QDir::separator() + str); + if (!file.open(QIODevice::ReadWrite)) { + ui->output->append("Cannot open file " + str + " for writing"); + } else { + ui->output->append("Processing file " + str); QTextStream out(&file); QStringList filestr; out.seek(0); - while(!out.atEnd()) filestr<licenseCB->isChecked()) - { - - bool haslicense=false; - foreach(QString str,filestr) - { - if(str.contains("The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010")) - { - haslicense=true; + if (ui->licenseCB->isChecked()) { + bool haslicense = false; + foreach(QString str, filestr) { + if (str.contains("The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010")) { + haslicense = true; ui->output->append("File Already has License Info"); } } - if(!haslicense) - { - bool process=true; - if(ui->confirmCB->isChecked()) - if(QMessageBox::question(this,"Add license Info?:",str,QMessageBox::Yes,QMessageBox::No)==QMessageBox::No) process=false; - if(process) - { + if (!haslicense) { + bool process = true; + if (ui->confirmCB->isChecked()) { + if (QMessageBox::question(this, "Add license Info?:", str, QMessageBox::Yes, QMessageBox::No) == QMessageBox::No) { + process = false; + } + } + if (process) { ui->output->append("Added License info to file"); out.seek(0); - foreach(QString line,license) - { - line.replace("%file",str); - line.replace("%defgroup",ui->defgroup->text()); - out<defgroup->text()); + out << line << "\r\n"; } - foreach(QString str,filestr) out<nameCB->isChecked()) - { + if (ui->nameCB->isChecked()) { filestr.clear(); out.seek(0); - while(!out.atEnd()) filestr<output->append("File Already has Namespace"); } } - if(ui->confirmCB->isChecked() && process) - if(QMessageBox::question(this,"Create Namespace?:",str,QMessageBox::Yes,QMessageBox::No)==QMessageBox::No) process=false; - if(process) - { + if (ui->confirmCB->isChecked() && process) { + if (QMessageBox::question(this, "Create Namespace?:", str, QMessageBox::Yes, QMessageBox::No) == QMessageBox::No) { + process = false; + } + } + if (process) { ui->output->append("Added NameSpace to file"); out.seek(0); - bool initdone=false; - for(int x=0;xnamespa->text()+" {"); - filestr.insert(x," "); - initdone=true; + bool initdone = false; + for (int x = 0; x < filestr.count(); ++x) { + QString line = filestr.at(x); + if (!initdone) { + if (!(line.trimmed().startsWith("#") || line.trimmed().startsWith("/") || line.trimmed().startsWith("*") || line.trimmed().isEmpty())) { + filestr.insert(x, "namespace " + ui->namespa->text() + " {"); + filestr.insert(x, " "); + initdone = true; } - } - else - { - if((QString)str.split(".").at(1)=="cpp") - { + } else { + if ((QString)str.split(".").at(1) == "cpp") { filestr.append("}"); break; - } - else - { - for(int y=filestr.count()-1;y>1;--y) - { - QString s=filestr.at(y); - if(s.contains("#endif")) - { - filestr.insert(y,"}"); + } else { + for (int y = filestr.count() - 1; y > 1; --y) { + QString s = filestr.at(y); + if (s.contains("#endif")) { + filestr.insert(y, "}"); break; } } @@ -202,43 +193,40 @@ void MainWindow::on_goBT_clicked() } } } - foreach(QString str,filestr) out<bockifCB->isChecked()) - { + if (ui->bockifCB->isChecked()) { QString genif; ui->output->append("Creating block ifs"); filestr.clear(); out.seek(0); - while(!out.atEnd()) filestr<textEdit->append(genif); - foreach(QString str,filestr) out<textEdit->append(genif); + } + foreach(QString str, filestr) out << str + "\r\n"; out.flush(); - } file.close(); } - } } } diff --git a/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.h b/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.h index feebc949b..4b482cd80 100644 --- a/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.h +++ b/ground/openpilotgcs/src/experimental/tools/DocumentationHelper/mainwindow.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file mainwindow.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup DocumentationHelper -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file mainwindow.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup DocumentationHelper + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef MAINWINDOW_H #define MAINWINDOW_H @@ -34,7 +34,7 @@ #include namespace Ui { - class MainWindow; +class MainWindow; } class MainWindow : public QMainWindow { diff --git a/ground/openpilotgcs/src/libs/aggregation/aggregate.cpp b/ground/openpilotgcs/src/libs/aggregation/aggregate.cpp index 81fc794b1..0a98eb5c3 100644 --- a/ground/openpilotgcs/src/libs/aggregation/aggregate.cpp +++ b/ground/openpilotgcs/src/libs/aggregation/aggregate.cpp @@ -4,25 +4,25 @@ * @file aggregate.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,7 +39,7 @@ Components that are bundled to an Aggregate can be "cast" to each other and have a coupled life cycle. See the documentation of Aggregation::Aggregate for details and examples. -*/ + */ /*! \class Aggregation::Aggregate @@ -97,7 +97,7 @@ Aggregation aware code never uses qobject_cast, but always uses Aggregation::query which behaves like a qobject_cast as a fallback. -*/ + */ /*! \fn T *Aggregate::component() @@ -107,7 +107,7 @@ \sa Aggregate::components() \sa Aggregate::add() -*/ + */ /*! \fn QList Aggregate::components() @@ -116,17 +116,17 @@ \sa Aggregate::component() \sa Aggregate::add() -*/ + */ /*! \fn T *Aggregation::query(Aggregate *obj) \internal -*/ + */ /*! \fn QList Aggregation::query_all(Aggregate *obj) \internal -*/ + */ /*! \relates Aggregation::Aggregate @@ -138,7 +138,7 @@ checked, or if it doesn't belong to an Aggregate null is returned. \sa Aggregate::component() -*/ + */ /*! \relates Aggregation::Aggregate @@ -148,7 +148,7 @@ type are returned. Otherwise, \a obj is returned if it is of the requested type. \sa Aggregate::components() -*/ + */ using namespace Aggregation; @@ -156,26 +156,29 @@ using namespace Aggregation; \fn Aggregate *Aggregate::parentAggregate(QObject *obj) Returns the Aggregate object of \a obj if there is one. Otherwise returns 0. -*/ + */ Aggregate *Aggregate::parentAggregate(QObject *obj) { QReadLocker locker(&lock()); + return aggregateMap().value(obj); } QHash &Aggregate::aggregateMap() { static QHash map; + return map; } /*! \fn QReadWriteLock &Aggregate::lock() \internal -*/ + */ QReadWriteLock &Aggregate::lock() { static QReadWriteLock lock; + return lock; } @@ -185,11 +188,12 @@ QReadWriteLock &Aggregate::lock() Creates a new Aggregate with the given \a parent. The \a parent is passed directly passed to the QObject part of the class and is not used beside that. -*/ + */ Aggregate::Aggregate(QObject *parent) : QObject(parent) { QWriteLocker locker(&lock()); + aggregateMap().insert(this, this); } @@ -197,12 +201,13 @@ Aggregate::Aggregate(QObject *parent) \fn Aggregate::~Aggregate() Deleting the aggregate automatically deletes all its components. -*/ + */ Aggregate::~Aggregate() { QWriteLocker locker(&lock()); - foreach (QObject *component, m_components) { - disconnect(component, SIGNAL(destroyed(QObject*)), this, SLOT(deleteSelf(QObject*))); + + foreach(QObject * component, m_components) { + disconnect(component, SIGNAL(destroyed(QObject *)), this, SLOT(deleteSelf(QObject *))); aggregateMap().remove(component); } qDeleteAll(m_components); @@ -226,19 +231,22 @@ void Aggregate::deleteSelf(QObject *obj) Adds the \a component to the aggregate. \sa Aggregate::remove() -*/ + */ void Aggregate::add(QObject *component) { - if (!component) + if (!component) { return; + } QWriteLocker locker(&lock()); Aggregate *parentAggregation = aggregateMap().value(component); - if (parentAggregation == this) + if (parentAggregation == this) { return; - if (parentAggregation) + } + if (parentAggregation) { parentAggregation->remove(component); + } m_components.append(component); - connect(component, SIGNAL(destroyed(QObject*)), this, SLOT(deleteSelf(QObject*))); + connect(component, SIGNAL(destroyed(QObject *)), this, SLOT(deleteSelf(QObject *))); aggregateMap().insert(component, this); } @@ -248,13 +256,14 @@ void Aggregate::add(QObject *component) Removes the \a component from the aggregate. \sa Aggregate::add() -*/ + */ void Aggregate::remove(QObject *component) { - if (!component) + if (!component) { return; + } QWriteLocker locker(&lock()); aggregateMap().remove(component); m_components.removeAll(component); - disconnect(component, SIGNAL(destroyed(QObject*)), this, SLOT(deleteSelf(QObject*))); + disconnect(component, SIGNAL(destroyed(QObject *)), this, SLOT(deleteSelf(QObject *))); } diff --git a/ground/openpilotgcs/src/libs/aggregation/aggregate.h b/ground/openpilotgcs/src/libs/aggregation/aggregate.h index da225977f..d150f2089 100644 --- a/ground/openpilotgcs/src/libs/aggregation/aggregate.h +++ b/ground/openpilotgcs/src/libs/aggregation/aggregate.h @@ -4,25 +4,25 @@ * @file aggregate.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,9 +38,7 @@ #include namespace Aggregation { - -class AGGREGATION_EXPORT Aggregate : public QObject -{ +class AGGREGATION_EXPORT Aggregate : public QObject { Q_OBJECT public: @@ -50,20 +48,23 @@ public: void add(QObject *component); void remove(QObject *component); - template T *component() { + template T *component() + { QReadLocker(&lock()); - foreach (QObject *component, m_components) { - if (T *result = qobject_cast(component)) + foreach(QObject * component, m_components) { + if (T * result = qobject_cast(component)) { return result; + } } return (T *)0; } - template QList components() { + template QList components() + { QReadLocker(&lock()); QList results; - foreach (QObject *component, m_components) { - if (T *result = qobject_cast(component)) { + foreach(QObject * component, m_components) { + if (T * result = qobject_cast(component)) { results << result; } } @@ -85,15 +86,17 @@ private: // get a component via global template function template T *query(Aggregate *obj) { - if (!obj) + if (!obj) { return (T *)0; + } return obj->template component(); } template T *query(QObject *obj) { - if (!obj) + if (!obj) { return (T *)0; + } T *result = qobject_cast(obj); if (!result) { QReadLocker(&lock()); @@ -106,25 +109,27 @@ template T *query(QObject *obj) // get all components of a specific type via template function template QList query_all(Aggregate *obj) { - if (!obj) + if (!obj) { return QList(); + } return obj->template components(); } template QList query_all(QObject *obj) { - if (!obj) + if (!obj) { return QList(); + } QReadLocker(&lock()); Aggregate *parentAggregation = Aggregate::parentAggregate(obj); QList results; - if (parentAggregation) + if (parentAggregation) { results = query_all(parentAggregation); - else if (T *result = qobject_cast(obj)) + } else if (T * result = qobject_cast(obj)) { results.append(result); + } return results; } - } // namespace Aggregation #endif // QAGGREGATION_H diff --git a/ground/openpilotgcs/src/libs/aggregation/aggregation_global.h b/ground/openpilotgcs/src/libs/aggregation/aggregation_global.h index e8bca0029..63f69680e 100644 --- a/ground/openpilotgcs/src/libs/aggregation/aggregation_global.h +++ b/ground/openpilotgcs/src/libs/aggregation/aggregation_global.h @@ -4,25 +4,25 @@ * @file aggregation_global.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/libs/aggregation/examples/text/main.cpp b/ground/openpilotgcs/src/libs/aggregation/examples/text/main.cpp index 525f1575e..c937ef413 100644 --- a/ground/openpilotgcs/src/libs/aggregation/examples/text/main.cpp +++ b/ground/openpilotgcs/src/libs/aggregation/examples/text/main.cpp @@ -4,25 +4,25 @@ * @file main.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -51,6 +51,7 @@ void MyMain::select(int index) IText1 *t1 = Aggregation::query(entry); IText2 *t2 = Aggregation::query(entry); IText3 *t3 = Aggregation::query(entry); + // set the label texts and enable/disable, depending on whether // the respective interface implementations exist ui.text1->setText(t1 ? t1->text() : tr("N/A")); @@ -81,6 +82,7 @@ int main(int argc, char *argv[]) // the second additionally provides an IText2 implementation // adding a component to the aggregation is done by setting the aggregation as the parent of the component Aggregation::Aggregate *obj2 = new Aggregation::Aggregate; + obj2->add(new IComboEntry("Entry with text 2")); obj2->add(new IText2("This is a text for label 2")); diff --git a/ground/openpilotgcs/src/libs/aggregation/examples/text/main.h b/ground/openpilotgcs/src/libs/aggregation/examples/text/main.h index 92e85abae..ae2585641 100644 --- a/ground/openpilotgcs/src/libs/aggregation/examples/text/main.h +++ b/ground/openpilotgcs/src/libs/aggregation/examples/text/main.h @@ -4,25 +4,25 @@ * @file main.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,8 +36,7 @@ #include -class MyMain : public QWidget -{ +class MyMain : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/libs/aggregation/examples/text/myinterfaces.h b/ground/openpilotgcs/src/libs/aggregation/examples/text/myinterfaces.h index 63cce9a79..95a47a3a2 100644 --- a/ground/openpilotgcs/src/libs/aggregation/examples/text/myinterfaces.h +++ b/ground/openpilotgcs/src/libs/aggregation/examples/text/myinterfaces.h @@ -4,25 +4,25 @@ * @file myinterfaces.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,52 +33,60 @@ #include -class IComboEntry : public QObject -{ +class IComboEntry : public QObject { Q_OBJECT public: IComboEntry(QString title) : m_title(title) {} virtual ~IComboEntry() {} - QString title() const { return m_title; } + QString title() const + { + return m_title; + } private: QString m_title; }; -class IText1 : public QObject -{ +class IText1 : public QObject { Q_OBJECT public: IText1(QString text) : m_text(text) {} virtual ~IText1() {} - QString text() const { return m_text; } + QString text() const + { + return m_text; + } private: QString m_text; }; -class IText2 : public QObject -{ +class IText2 : public QObject { Q_OBJECT public: IText2(QString text) : m_text(text) {} - QString text() const { return m_text; } + QString text() const + { + return m_text; + } private: QString m_text; }; -class IText3 : public QObject -{ +class IText3 : public QObject { Q_OBJECT public: IText3(QString text) : m_text(text) {} virtual ~IText3() {} - QString text() const { return m_text; } + QString text() const + { + return m_text; + } private: QString m_text; diff --git a/ground/openpilotgcs/src/libs/extensionsystem/extensionsystem_global.h b/ground/openpilotgcs/src/libs/extensionsystem/extensionsystem_global.h index dbe6b9178..a06821486 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/extensionsystem_global.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/extensionsystem_global.h @@ -4,25 +4,25 @@ * @file extensionsystem_global.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/libs/extensionsystem/iplugin.cpp b/ground/openpilotgcs/src/libs/extensionsystem/iplugin.cpp index 65720561c..f15247ee0 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/iplugin.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/iplugin.cpp @@ -4,25 +4,25 @@ * @file iplugin.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -212,7 +212,7 @@ Plugins have access to the plugin manager (and its object pool) via the PluginManager::instance() method. -*/ + */ /*! \fn bool IPlugin::initialize(const QStringList &arguments, QString *errorString) @@ -224,7 +224,7 @@ the \a errorString should be set to a user-readable message describing the reason. \sa extensionsInitialized() -*/ + */ /*! \fn void IPlugin::extensionsInitialized() @@ -236,7 +236,7 @@ look in the plugin manager's object pool for objects that have been provided by dependent plugins. \sa initialize() -*/ + */ /*! \fn void IPlugin::shutdown() @@ -246,28 +246,28 @@ disconnect from the PluginManager::aboutToRemoveObject() signal if getting the signal (and probably doing lots of stuff to update the internal and visible state) doesn't make sense during shutdown. -*/ + */ using namespace ExtensionSystem; /*! \fn IPlugin::IPlugin() \internal -*/ + */ IPlugin::IPlugin() : d(new Internal::IPluginPrivate()) -{ -} +{} /*! \fn IPlugin::~IPlugin() \internal -*/ + */ IPlugin::~IPlugin() { PluginManager *pm = PluginManager::instance(); - foreach (QObject *obj, d->addedObjectsInReverseOrder) - pm->removeObject(obj); + + foreach(QObject * obj, d->addedObjectsInReverseOrder) + pm->removeObject(obj); qDeleteAll(d->addedObjectsInReverseOrder); d->addedObjectsInReverseOrder.clear(); delete d; @@ -278,7 +278,7 @@ IPlugin::~IPlugin() \fn PluginSpec *IPlugin::pluginSpec() const Returns the PluginSpec corresponding to this plugin. This is not available in the constructor. -*/ + */ PluginSpec *IPlugin::pluginSpec() const { return d->pluginSpec; @@ -288,7 +288,7 @@ PluginSpec *IPlugin::pluginSpec() const \fn void IPlugin::addObject(QObject *obj) Convenience method that registers \a obj in the plugin manager's plugin pool by just calling PluginManager::addObject(). -*/ + */ void IPlugin::addObject(QObject *obj) { PluginManager::instance()->addObject(obj); @@ -303,7 +303,7 @@ void IPlugin::addObject(QObject *obj) removed and deleted in reverse order of registration when the IPlugin instance is destroyed. \sa PluginManager::addObject() -*/ + */ void IPlugin::addAutoReleasedObject(QObject *obj) { d->addedObjectsInReverseOrder.prepend(obj); @@ -314,9 +314,8 @@ void IPlugin::addAutoReleasedObject(QObject *obj) \fn void IPlugin::removeObject(QObject *obj) Convenience method that unregisters \a obj from the plugin manager's plugin pool by just calling PluginManager::removeObject(). -*/ + */ void IPlugin::removeObject(QObject *obj) { PluginManager::instance()->removeObject(obj); } - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/iplugin.h b/ground/openpilotgcs/src/libs/extensionsystem/iplugin.h index 957153645..b713a3b55 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/iplugin.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/iplugin.h @@ -4,25 +4,25 @@ * @file iplugin.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,17 +34,15 @@ #include namespace ExtensionSystem { - namespace Internal { - class IPluginPrivate; - class PluginSpecPrivate; +class IPluginPrivate; +class PluginSpecPrivate; } class PluginManager; class PluginSpec; -class EXTENSIONSYSTEM_EXPORT IPlugin : public QObject -{ +class EXTENSIONSYSTEM_EXPORT IPlugin : public QObject { Q_OBJECT public: @@ -53,7 +51,7 @@ public: virtual bool initialize(const QStringList &arguments, QString *errorString) = 0; virtual void extensionsInitialized() = 0; - virtual void shutdown() { } + virtual void shutdown() {} PluginSpec *pluginSpec() const; @@ -66,7 +64,6 @@ private: friend class Internal::PluginSpecPrivate; }; - } // namespace ExtensionSystem #endif // IPLUGIN_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/iplugin_p.h b/ground/openpilotgcs/src/libs/extensionsystem/iplugin_p.h index 1b8845d07..cc3486fb0 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/iplugin_p.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/iplugin_p.h @@ -4,25 +4,25 @@ * @file iplugin_p.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,20 +34,16 @@ #include namespace ExtensionSystem { - class PluginManager; class PluginSpec; namespace Internal { - -class IPluginPrivate -{ +class IPluginPrivate { public: PluginSpec *pluginSpec; QList addedObjectsInReverseOrder; }; - } // namespace Internal } // namespace ExtensionSystem diff --git a/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.cpp b/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.cpp index 24979c2ca..aa01196bc 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.cpp @@ -4,25 +4,25 @@ * @file optionsparser.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,59 +35,70 @@ using namespace ExtensionSystem::Internal; static const char *END_OF_OPTIONS = "--"; const char *OptionsParser::NO_LOAD_OPTION = "-noload"; -const char *OptionsParser::TEST_OPTION = "-test"; +const char *OptionsParser::TEST_OPTION = "-test"; OptionsParser::OptionsParser(const QStringList &args, - const QMap &appOptions, - QMap *foundAppOptions, - QString *errorString, - PluginManagerPrivate *pmPrivate) + const QMap &appOptions, + QMap *foundAppOptions, + QString *errorString, + PluginManagerPrivate *pmPrivate) : m_args(args), m_appOptions(appOptions), - m_foundAppOptions(foundAppOptions), - m_errorString(errorString), - m_pmPrivate(pmPrivate), - m_it(m_args.constBegin()), - m_end(m_args.constEnd()), - m_isDependencyRefreshNeeded(false), - m_hasError(false) + m_foundAppOptions(foundAppOptions), + m_errorString(errorString), + m_pmPrivate(pmPrivate), + m_it(m_args.constBegin()), + m_end(m_args.constEnd()), + m_isDependencyRefreshNeeded(false), + m_hasError(false) { ++m_it; // jump over program name - if (m_errorString) + if (m_errorString) { m_errorString->clear(); - if (m_foundAppOptions) + } + if (m_foundAppOptions) { m_foundAppOptions->clear(); + } m_pmPrivate->arguments.clear(); } bool OptionsParser::parse() { while (!m_hasError) { - if (!nextToken()) // move forward + if (!nextToken()) { // move forward break; - if (checkForEndOfOptions()) + } + if (checkForEndOfOptions()) { break; - if (checkForNoLoadOption()) + } + if (checkForNoLoadOption()) { continue; - if (checkForTestOption()) + } + if (checkForTestOption()) { continue; - if (checkForAppOption()) + } + if (checkForAppOption()) { continue; - if (checkForPluginOption()) + } + if (checkForPluginOption()) { continue; - if (checkForUnknownOption()) + } + if (checkForUnknownOption()) { break; + } // probably a file or something m_pmPrivate->arguments << m_currentArg; } - if (m_isDependencyRefreshNeeded) + if (m_isDependencyRefreshNeeded) { m_pmPrivate->resolveDependencies(); + } return !m_hasError; } bool OptionsParser::checkForEndOfOptions() { - if (m_currentArg != QLatin1String(END_OF_OPTIONS)) + if (m_currentArg != QLatin1String(END_OF_OPTIONS)) { return false; + } while (nextToken()) { m_pmPrivate->arguments << m_currentArg; } @@ -96,14 +107,16 @@ bool OptionsParser::checkForEndOfOptions() bool OptionsParser::checkForTestOption() { - if (m_currentArg != QLatin1String(TEST_OPTION)) + if (m_currentArg != QLatin1String(TEST_OPTION)) { return false; + } if (nextToken(RequiredToken)) { PluginSpec *spec = m_pmPrivate->pluginByName(m_currentArg); if (!spec) { - if (m_errorString) + if (m_errorString) { *m_errorString = QCoreApplication::translate("PluginManager", "The plugin '%1' does not exist.").arg(m_currentArg); + } m_hasError = true; } else { m_pmPrivate->testSpecs.append(spec); @@ -114,14 +127,16 @@ bool OptionsParser::checkForTestOption() bool OptionsParser::checkForNoLoadOption() { - if (m_currentArg != QLatin1String(NO_LOAD_OPTION)) + if (m_currentArg != QLatin1String(NO_LOAD_OPTION)) { return false; + } if (nextToken(RequiredToken)) { PluginSpec *spec = m_pmPrivate->pluginByName(m_currentArg); if (!spec) { - if (m_errorString) + if (m_errorString) { *m_errorString = QCoreApplication::translate("PluginManager", "The plugin '%1' does not exist.").arg(m_currentArg); + } m_hasError = true; } else { m_pmPrivate->pluginSpecs.removeAll(spec); @@ -134,16 +149,18 @@ bool OptionsParser::checkForNoLoadOption() bool OptionsParser::checkForAppOption() { - if (!m_appOptions.contains(m_currentArg)) + if (!m_appOptions.contains(m_currentArg)) { return false; + } QString option = m_currentArg; QString argument; if (m_appOptions.value(m_currentArg) && nextToken(RequiredToken)) { - //argument required + // argument required argument = m_currentArg; } - if (m_foundAppOptions) + if (m_foundAppOptions) { m_foundAppOptions->insert(option, argument); + } return true; } @@ -151,8 +168,10 @@ bool OptionsParser::checkForPluginOption() { bool requiresParameter; PluginSpec *spec = m_pmPrivate->pluginForOption(m_currentArg, &requiresParameter); - if (!spec) + + if (!spec) { return false; + } spec->addArgument(m_currentArg); if (requiresParameter && nextToken(RequiredToken)) { spec->addArgument(m_currentArg); @@ -162,11 +181,13 @@ bool OptionsParser::checkForPluginOption() bool OptionsParser::checkForUnknownOption() { - if (!m_currentArg.startsWith(QLatin1Char('-'))) + if (!m_currentArg.startsWith(QLatin1Char('-'))) { return false; - if (m_errorString) + } + if (m_errorString) { *m_errorString = QCoreApplication::translate("PluginManager", "Unknown option %1").arg(m_currentArg); + } m_hasError = true; return true; } @@ -176,9 +197,10 @@ bool OptionsParser::nextToken(OptionsParser::TokenType type) if (m_it == m_end) { if (type == OptionsParser::RequiredToken) { m_hasError = true; - if (m_errorString) + if (m_errorString) { *m_errorString = QCoreApplication::translate("PluginManager", "The option %1 requires an argument.").arg(m_currentArg); + } } return false; } diff --git a/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.h b/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.h index 665c0cb71..6132a1ba2 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/optionsparser.h @@ -4,25 +4,25 @@ * @file optionsparser.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,16 +36,14 @@ namespace ExtensionSystem { namespace Internal { - -class OptionsParser -{ +class OptionsParser { public: OptionsParser(const QStringList &args, - const QMap &appOptions, - QMap *foundAppOptions, - QString *errorString, - PluginManagerPrivate *pmPrivate); - + const QMap &appOptions, + QMap *foundAppOptions, + QString *errorString, + PluginManagerPrivate *pmPrivate); + bool parse(); static const char *NO_LOAD_OPTION; @@ -62,13 +60,13 @@ private: enum TokenType { OptionalToken, RequiredToken }; bool nextToken(TokenType type = OptionalToken); - + const QStringList &m_args; const QMap &m_appOptions; QMap *m_foundAppOptions; QString *m_errorString; PluginManagerPrivate *m_pmPrivate; - + // state QString m_currentArg; QStringList::const_iterator m_it; @@ -76,7 +74,6 @@ private: bool m_isDependencyRefreshNeeded; bool m_hasError; }; - } // namespace Internal } // namespace ExtensionSystem diff --git a/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.cpp b/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.cpp index 7bff8e54c..aad82582d 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.cpp @@ -4,25 +4,25 @@ * @file plugindetailsview.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,17 +40,17 @@ uses the plugin manager. \sa ExtensionSystem::PluginView -*/ + */ using namespace ExtensionSystem; /*! \fn PluginDetailsView::PluginDetailsView(QWidget *parent) Constructs a new view with given \a parent widget. -*/ + */ PluginDetailsView::PluginDetailsView(QWidget *parent) - : QWidget(parent), - m_ui(new Internal::Ui::PluginDetailsView()) + : QWidget(parent), + m_ui(new Internal::Ui::PluginDetailsView()) { m_ui->setupUi(this); } @@ -58,7 +58,7 @@ PluginDetailsView::PluginDetailsView(QWidget *parent) /*! \fn PluginDetailsView::~PluginDetailsView() \internal -*/ + */ PluginDetailsView::~PluginDetailsView() { delete m_ui; @@ -68,7 +68,7 @@ PluginDetailsView::~PluginDetailsView() \fn void PluginDetailsView::update(PluginSpec *spec) Reads the given \a spec and displays its values in this PluginDetailsView. -*/ + */ void PluginDetailsView::update(PluginSpec *spec) { m_ui->name->setText(spec->name()); @@ -82,7 +82,7 @@ void PluginDetailsView::update(PluginSpec *spec) m_ui->copyright->setText(spec->copyright()); m_ui->license->setText(spec->license()); QStringList depStrings; - foreach (PluginDependency dep, spec->dependencies()) { + foreach(PluginDependency dep, spec->dependencies()) { depStrings << QString("%1 (%2)").arg(dep.name).arg(dep.version); } m_ui->dependencies->addItems(depStrings); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.h b/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.h index 26e0baf13..1d6161b2c 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/plugindetailsview.h @@ -4,25 +4,25 @@ * @file plugindetailsview.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,30 +34,27 @@ #include namespace ExtensionSystem { - class PluginSpec; namespace Internal { namespace Ui { - class PluginDetailsView; +class PluginDetailsView; } // namespace Ui } // namespace Internal -class EXTENSIONSYSTEM_EXPORT PluginDetailsView : public QWidget -{ +class EXTENSIONSYSTEM_EXPORT PluginDetailsView : public QWidget { Q_OBJECT public: PluginDetailsView(QWidget *parent = 0); ~PluginDetailsView(); - + void update(PluginSpec *spec); private: Internal::Ui::PluginDetailsView *m_ui; }; - } // namespace ExtensionSystem -#endif +#endif // ifndef PLUGINDETAILSVIEW_H_ diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.cpp b/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.cpp index 3ab6db8e5..df177d361 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.cpp @@ -4,25 +4,25 @@ * @file pluginerrorview.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,17 +40,17 @@ uses the plugin manager. \sa ExtensionSystem::PluginView -*/ + */ using namespace ExtensionSystem; /*! \fn PluginErrorView::PluginErrorView(QWidget *parent) Constructs a new error view with given \a parent widget. -*/ + */ PluginErrorView::PluginErrorView(QWidget *parent) : QWidget(parent), - m_ui(new Internal::Ui::PluginErrorView()) + m_ui(new Internal::Ui::PluginErrorView()) { m_ui->setupUi(this); } @@ -58,7 +58,7 @@ PluginErrorView::PluginErrorView(QWidget *parent) /*! \fn PluginErrorView::~PluginErrorView() \internal -*/ + */ PluginErrorView::~PluginErrorView() { delete m_ui; @@ -68,41 +68,42 @@ PluginErrorView::~PluginErrorView() \fn void PluginErrorView::update(PluginSpec *spec) Reads the given \a spec and displays its state and error information in this PluginErrorView. -*/ + */ void PluginErrorView::update(PluginSpec *spec) { QString text; QString tooltip; + switch (spec->state()) { case PluginSpec::Invalid: - text = tr("Invalid"); + text = tr("Invalid"); tooltip = tr("Description file found, but error on read"); break; case PluginSpec::Read: - text = tr("Read"); + text = tr("Read"); tooltip = tr("Description successfully read"); break; case PluginSpec::Resolved: - text = tr("Resolved"); + text = tr("Resolved"); tooltip = tr("Dependencies are successfully resolved"); break; case PluginSpec::Loaded: - text = tr("Loaded"); + text = tr("Loaded"); tooltip = tr("Library is loaded"); break; case PluginSpec::Initialized: - text = tr("Initialized"); + text = tr("Initialized"); tooltip = tr("Plugin's initialization method succeeded"); break; case PluginSpec::Running: - text = tr("Running"); + text = tr("Running"); tooltip = tr("Plugin successfully loaded and running"); break; case PluginSpec::Stopped: - text = tr("Stopped"); + text = tr("Stopped"); tooltip = tr("Plugin was shut down"); case PluginSpec::Deleted: - text = tr("Deleted"); + text = tr("Deleted"); tooltip = tr("Plugin ended its life cycle and was deleted"); } m_ui->state->setText(text); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.h index 62daf093d..b33aaabf6 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginerrorview.h @@ -4,25 +4,25 @@ * @file pluginerrorview.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,16 +34,14 @@ #include namespace ExtensionSystem { - class PluginSpec; namespace Internal { namespace Ui { - class PluginErrorView; +class PluginErrorView; } // namespace Ui } // namespace Internal -class EXTENSIONSYSTEM_EXPORT PluginErrorView : public QWidget -{ +class EXTENSIONSYSTEM_EXPORT PluginErrorView : public QWidget { Q_OBJECT public: @@ -55,7 +53,6 @@ public: private: Internal::Ui::PluginErrorView *m_ui; }; - } // namespace ExtensionSystem #endif // PLUGINERRORVIEW_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.cpp b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.cpp index 09730f2d5..8ff664205 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.cpp @@ -4,25 +4,25 @@ * @file pluginmanager.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -52,12 +52,12 @@ enum { debugLeaks = 0 }; The basic extension system contains of the plugin manager and its supporting classes, and the IPlugin interface that must be implemented by plugin providers. -*/ + */ /*! \namespace ExtensionSystem::Internal \internal -*/ + */ /*! \class ExtensionSystem::PluginManager @@ -112,24 +112,24 @@ enum { debugLeaks = 0 }; \endcode \bold Note: The object pool manipulating functions are thread-safe. -*/ + */ /*! \fn void PluginManager::objectAdded(QObject *obj) Signal that \a obj has been added to the object pool. -*/ + */ /*! \fn void PluginManager::aboutToRemoveObject(QObject *obj) Signal that \a obj will be removed from the object pool. -*/ + */ /*! \fn void PluginManager::pluginsChanged() Signal that the list of available plugins has changed. \sa plugins() -*/ + */ /*! \fn T *PluginManager::getObject() const @@ -141,7 +141,7 @@ enum { debugLeaks = 0 }; the object pool, this method will choose an arbitrary one of them. \sa addObject() -*/ + */ /*! \fn QList PluginManager::getObjects() const @@ -151,7 +151,7 @@ enum { debugLeaks = 0 }; determine the type of an object. \sa addObject() -*/ + */ using namespace ExtensionSystem; using namespace ExtensionSystem::Internal; @@ -166,7 +166,7 @@ PluginManager *PluginManager::m_instance = 0; /*! \fn PluginManager *PluginManager::instance() Get the unique plugin manager instance. -*/ + */ PluginManager *PluginManager::instance() { return m_instance; @@ -175,9 +175,9 @@ PluginManager *PluginManager::instance() /*! \fn PluginManager::PluginManager() Create a plugin manager. Should be done only once per application. -*/ + */ PluginManager::PluginManager() - : d(new PluginManagerPrivate(this)),m_allPluginsLoaded(false) + : d(new PluginManagerPrivate(this)), m_allPluginsLoaded(false) { m_instance = this; } @@ -185,7 +185,7 @@ PluginManager::PluginManager() /*! \fn PluginManager::~PluginManager() \internal -*/ + */ PluginManager::~PluginManager() { delete d; @@ -203,7 +203,7 @@ PluginManager::~PluginManager() \sa PluginManager::removeObject() \sa PluginManager::getObject() \sa PluginManager::getObjects() -*/ + */ void PluginManager::addObject(QObject *obj) { d->addObject(obj); @@ -213,7 +213,7 @@ void PluginManager::addObject(QObject *obj) \fn void PluginManager::removeObject(QObject *obj) Emits aboutToRemoveObject() and removes the object \a obj from the object pool. \sa PluginManager::addObject() -*/ + */ void PluginManager::removeObject(QObject *obj) { d->removeObject(obj); @@ -225,7 +225,7 @@ void PluginManager::removeObject(QObject *obj) Usually clients do not need to call this. \sa PluginManager::getObject() \sa PluginManager::getObjects() -*/ + */ QList PluginManager::allObjects() const { return d->allObjects; @@ -239,7 +239,7 @@ QList PluginManager::allObjects() const \sa setPluginPaths() \sa plugins() -*/ + */ void PluginManager::loadPlugins() { return d->loadPlugins(); @@ -250,7 +250,7 @@ void PluginManager::loadPlugins() The list of paths were the plugin manager searches for plugins. \sa setPluginPaths() -*/ + */ QStringList PluginManager::pluginPaths() const { return d->pluginPaths; @@ -264,7 +264,7 @@ QStringList PluginManager::pluginPaths() const \sa pluginPaths() \sa loadPlugins() -*/ + */ void PluginManager::setPluginPaths(const QStringList &paths) { d->setPluginPaths(paths); @@ -276,7 +276,7 @@ void PluginManager::setPluginPaths(const QStringList &paths) The default is "xml". \sa setFileExtension() -*/ + */ QString PluginManager::fileExtension() const { return d->extension; @@ -288,7 +288,7 @@ QString PluginManager::fileExtension() const The default is "xml". At the moment this must be called before setPluginPaths() is called. // ### TODO let this + setPluginPaths read the plugin specs lazyly whenever loadPlugins() or plugins() is called. -*/ + */ void PluginManager::setFileExtension(const QString &extension) { d->extension = extension; @@ -298,7 +298,7 @@ void PluginManager::setFileExtension(const QString &extension) \fn QStringList PluginManager::arguments() const The arguments left over after parsing (Neither startup nor plugin arguments). Typically, this will be the list of files to open. -*/ + */ QStringList PluginManager::arguments() const { return d->arguments; @@ -313,7 +313,7 @@ QStringList PluginManager::arguments() const the plugin specification has a reference to the created plugin instance as well. \sa setPluginPaths() -*/ + */ QList PluginManager::plugins() const { return d->pluginSpecs; @@ -327,30 +327,32 @@ QList PluginManager::plugins() const The caller (the application) may register itself for options via the \a appOptions list, containing pairs of "option string" and a bool that indicates if the option requires an argument. Application options always override any plugin's options. - + \a foundAppOptions is set to pairs of ("option string", "argument") for any application options that were found. The command line options that were not processed can be retrieved via the arguments() method. If an error occurred (like missing argument for an option that requires one), \a errorString contains a descriptive message of the error. - + Returns if there was an error. */ bool PluginManager::parseOptions(const QStringList &args, - const QMap &appOptions, - QMap *foundAppOptions, - QString *errorString) + const QMap &appOptions, + QMap *foundAppOptions, + QString *errorString) { OptionsParser options(args, appOptions, foundAppOptions, errorString, d); + return options.parse(); } - static inline void indent(QTextStream &str, int indent) { const QChar blank = QLatin1Char(' '); - for (int i = 0 ; i < indent; i++) + + for (int i = 0; i < indent; i++) { str << blank; + } } static inline void formatOption(QTextStream &str, @@ -358,6 +360,7 @@ static inline void formatOption(QTextStream &str, int optionIndentation, int descriptionIndentation) { int remainingIndent = descriptionIndentation - optionIndentation - opt.size(); + indent(str, optionIndentation); str << opt; if (!parm.isEmpty()) { @@ -372,7 +375,7 @@ static inline void formatOption(QTextStream &str, \fn static PluginManager::formatOptions(QTextStream &str, int optionIndentation, int descriptionIndentation) Format the startup options of the plugin manager for command line help. -*/ + */ void PluginManager::formatOptions(QTextStream &str, int optionIndentation, int descriptionIndentation) { @@ -385,7 +388,7 @@ void PluginManager::formatOptions(QTextStream &str, int optionIndentation, int d \fn PluginManager::formatPluginOptions(QTextStream &str, int optionIndentation, int descriptionIndentation) const Format the plugin options of the plugin specs for command line help. -*/ + */ void PluginManager::formatPluginOptions(QTextStream &str, int optionIndentation, int descriptionIndentation) const { @@ -395,10 +398,11 @@ void PluginManager::formatPluginOptions(QTextStream &str, int optionIndentation, for (PluginSpecSet::const_iterator pit = d->pluginSpecs.constBegin(); pit != pcend; ++pit) { const PluginArgumentDescriptions pargs = (*pit)->argumentDescriptions(); if (!pargs.empty()) { - str << "\nPlugin: " << (*pit)->name() << '\n'; + str << "\nPlugin: " << (*pit)->name() << '\n'; const PluginArgumentDescriptions::const_iterator acend = pargs.constEnd(); - for (PluginArgumentDescriptions::const_iterator ait =pargs.constBegin(); ait != acend; ++ait) + for (PluginArgumentDescriptions::const_iterator ait = pargs.constBegin(); ait != acend; ++ait) { formatOption(str, ait->name, ait->parameter, ait->description, optionIndentation, descriptionIndentation); + } } } } @@ -407,33 +411,34 @@ void PluginManager::formatPluginOptions(QTextStream &str, int optionIndentation, \fn PluginManager::formatPluginVersions(QTextStream &str) const Format the version of the plugin specs for command line help. -*/ + */ void PluginManager::formatPluginVersions(QTextStream &str) const { const PluginSpecSet::const_iterator cend = d->pluginSpecs.constEnd(); + for (PluginSpecSet::const_iterator it = d->pluginSpecs.constBegin(); it != cend; ++it) { const PluginSpec *ps = *it; - str << " " << ps->name() << ' ' << ps->version() << ' ' << ps->description() << '\n'; + str << " " << ps->name() << ' ' << ps->version() << ' ' << ps->description() << '\n'; } } void PluginManager::startTests() { #ifdef WITH_TESTS - foreach (PluginSpec *pluginSpec, d->testSpecs) { + foreach(PluginSpec * pluginSpec, d->testSpecs) { const QMetaObject *mo = pluginSpec->plugin()->metaObject(); QStringList methods; + methods.append("arg0"); // We only want slots starting with "test" for (int i = mo->methodOffset(); i < mo->methodCount(); ++i) { if (QByteArray(mo->method(i).signature()).startsWith("test")) { QString method = QString::fromLatin1(mo->method(i).signature()); - methods.append(method.left(method.size()-2)); + methods.append(method.left(method.size() - 2)); } } QTest::qExec(pluginSpec->plugin(), methods); - } #endif } @@ -454,7 +459,8 @@ bool PluginManager::runningTests() const QString PluginManager::testDataDirectory() const { QByteArray ba = qgetenv("QTCREATOR_TEST_DIR"); - QString s = QString::fromLocal8Bit(ba.constData(), ba.size()); + QString s = QString::fromLocal8Bit(ba.constData(), ba.size()); + if (s.isEmpty()) { s = GCS_TEST_DIR; s.append("/tests"); @@ -463,12 +469,12 @@ QString PluginManager::testDataDirectory() const return s; } -//============PluginManagerPrivate=========== +// ============PluginManagerPrivate=========== /*! \fn PluginSpec *PluginManagerPrivate::createSpec() \internal -*/ + */ PluginSpec *PluginManagerPrivate::createSpec() { return new PluginSpec(); @@ -477,7 +483,7 @@ PluginSpec *PluginManagerPrivate::createSpec() /*! \fn PluginSpecPrivate *PluginManagerPrivate::privateSpec(PluginSpec *spec) \internal -*/ + */ PluginSpecPrivate *PluginManagerPrivate::privateSpec(PluginSpec *spec) { return spec->d; @@ -486,16 +492,15 @@ PluginSpecPrivate *PluginManagerPrivate::privateSpec(PluginSpec *spec) /*! \fn PluginManagerPrivate::PluginManagerPrivate(PluginManager *pluginManager) \internal -*/ + */ PluginManagerPrivate::PluginManagerPrivate(PluginManager *pluginManager) : extension("xml"), q(pluginManager) -{ -} +{} /*! \fn PluginManagerPrivate::~PluginManagerPrivate() \internal -*/ + */ PluginManagerPrivate::~PluginManagerPrivate() { stopAll(); @@ -508,7 +513,7 @@ PluginManagerPrivate::~PluginManagerPrivate() void PluginManagerPrivate::stopAll() { QList queue = loadQueue(); - foreach (PluginSpec *spec, queue) { + foreach(PluginSpec * spec, queue) { loadPlugin(spec, PluginSpec::Stopped); } QListIterator it(queue); @@ -521,7 +526,7 @@ void PluginManagerPrivate::stopAll() /*! \fn void PluginManagerPrivate::addObject(QObject *obj) \internal -*/ + */ void PluginManagerPrivate::addObject(QObject *obj) { { @@ -535,8 +540,9 @@ void PluginManagerPrivate::addObject(QObject *obj) return; } - if (debugLeaks) + if (debugLeaks) { qDebug() << "PluginManagerPrivate::addObject" << obj << obj->objectName(); + } allObjects.append(obj); } @@ -546,7 +552,7 @@ void PluginManagerPrivate::addObject(QObject *obj) /*! \fn void PluginManagerPrivate::removeObject(QObject *obj) \internal -*/ + */ void PluginManagerPrivate::removeObject(QObject *obj) { if (obj == 0) { @@ -556,11 +562,12 @@ void PluginManagerPrivate::removeObject(QObject *obj) if (!allObjects.contains(obj)) { qWarning() << "PluginManagerPrivate::removeObject(): object not in list:" - << obj << obj->objectName(); + << obj << obj->objectName(); return; } - if (debugLeaks) + if (debugLeaks) { qDebug() << "PluginManagerPrivate::removeObject" << obj << obj->objectName(); + } emit q->aboutToRemoveObject(obj); QWriteLocker lock(&(q->m_lock)); @@ -570,36 +577,36 @@ void PluginManagerPrivate::removeObject(QObject *obj) /*! \fn void PluginManagerPrivate::loadPlugins() \internal -*/ + */ void PluginManagerPrivate::loadPlugins() { QList queue = loadQueue(); - foreach (PluginSpec *spec, queue) { + foreach(PluginSpec * spec, queue) { loadPlugin(spec, PluginSpec::Loaded); } - foreach (PluginSpec *spec, queue) { + foreach(PluginSpec * spec, queue) { loadPlugin(spec, PluginSpec::Initialized); } QListIterator it(queue); it.toBack(); while (it.hasPrevious()) { - PluginSpec* plugin = it.previous(); + PluginSpec *plugin = it.previous(); emit q->pluginAboutToBeLoaded(plugin); loadPlugin(plugin, PluginSpec::Running); } emit q->pluginsChanged(); - q->m_allPluginsLoaded=true; + q->m_allPluginsLoaded = true; emit q->pluginsLoadEnded(); } /*! \fn void PluginManagerPrivate::loadQueue() \internal -*/ + */ QList PluginManagerPrivate::loadQueue() { QList queue; - foreach (PluginSpec *spec, pluginSpecs) { + foreach(PluginSpec * spec, pluginSpecs) { QList circularityCheckQueue; loadQueue(spec, queue, circularityCheckQueue); } @@ -609,20 +616,21 @@ QList PluginManagerPrivate::loadQueue() /*! \fn bool PluginManagerPrivate::loadQueue(PluginSpec *spec, QList &queue, QList &circularityCheckQueue) \internal -*/ + */ bool PluginManagerPrivate::loadQueue(PluginSpec *spec, QList &queue, - QList &circularityCheckQueue) + QList &circularityCheckQueue) { - if (queue.contains(spec)) + if (queue.contains(spec)) { return true; + } // check for circular dependencies if (circularityCheckQueue.contains(spec)) { - spec->d->hasError = true; + spec->d->hasError = true; spec->d->errorString = PluginManager::tr("Circular dependency detected:\n"); int index = circularityCheckQueue.indexOf(spec); for (int i = index; i < circularityCheckQueue.size(); ++i) { spec->d->errorString.append(PluginManager::tr("%1(%2) depends on\n") - .arg(circularityCheckQueue.at(i)->name()).arg(circularityCheckQueue.at(i)->version())); + .arg(circularityCheckQueue.at(i)->name()).arg(circularityCheckQueue.at(i)->version())); } spec->d->errorString.append(PluginManager::tr("%1(%2)").arg(spec->name()).arg(spec->version())); return false; @@ -630,18 +638,18 @@ bool PluginManagerPrivate::loadQueue(PluginSpec *spec, QList &queu circularityCheckQueue.append(spec); // check if we have the dependencies if (spec->state() == PluginSpec::Invalid || spec->state() == PluginSpec::Read) { - spec->d->hasError = true; + spec->d->hasError = true; spec->d->errorString += "\n"; spec->d->errorString += PluginManager::tr("Cannot load plugin because dependencies are not resolved"); return false; } // add dependencies - foreach (PluginSpec *depSpec, spec->dependencySpecs()) { + foreach(PluginSpec * depSpec, spec->dependencySpecs()) { if (!loadQueue(depSpec, queue, circularityCheckQueue)) { - spec->d->hasError = true; + spec->d->hasError = true; spec->d->errorString = PluginManager::tr("Cannot load plugin because dependency failed to load: %1(%2)\nReason: %3") - .arg(depSpec->name()).arg(depSpec->version()).arg(depSpec->errorString()); + .arg(depSpec->name()).arg(depSpec->version()).arg(depSpec->errorString()); return false; } } @@ -653,11 +661,12 @@ bool PluginManagerPrivate::loadQueue(PluginSpec *spec, QList &queu /*! \fn void PluginManagerPrivate::loadPlugin(PluginSpec *spec, PluginSpec::State destState) \internal -*/ + */ void PluginManagerPrivate::loadPlugin(PluginSpec *spec, PluginSpec::State destState) { - if (spec->hasError()) + if (spec->hasError()) { return; + } if (destState == PluginSpec::Running) { spec->d->initializeExtensions(); return; @@ -665,27 +674,28 @@ void PluginManagerPrivate::loadPlugin(PluginSpec *spec, PluginSpec::State destSt spec->d->kill(); return; } - foreach (PluginSpec *depSpec, spec->dependencySpecs()) { + foreach(PluginSpec * depSpec, spec->dependencySpecs()) { if (depSpec->state() != destState) { - spec->d->hasError = true; + spec->d->hasError = true; spec->d->errorString = PluginManager::tr("Cannot load plugin because dependency failed to load: %1(%2)\nReason: %3") - .arg(depSpec->name()).arg(depSpec->version()).arg(depSpec->errorString()); + .arg(depSpec->name()).arg(depSpec->version()).arg(depSpec->errorString()); return; } } - if (destState == PluginSpec::Loaded) + if (destState == PluginSpec::Loaded) { spec->d->loadLibrary(); - else if (destState == PluginSpec::Initialized) + } else if (destState == PluginSpec::Initialized) { spec->d->initializePlugin(); - else if (destState == PluginSpec::Stopped) + } else if (destState == PluginSpec::Stopped) { spec->d->stop(); + } } /*! \fn void PluginManagerPrivate::setPluginPaths(const QStringList &paths) \internal -*/ + */ void PluginManagerPrivate::setPluginPaths(const QStringList &paths) { pluginPaths = paths; @@ -695,7 +705,7 @@ void PluginManagerPrivate::setPluginPaths(const QStringList &paths) /*! \fn void PluginManagerPrivate::readPluginPaths() \internal -*/ + */ void PluginManagerPrivate::readPluginPaths() { qDeleteAll(pluginSpecs); @@ -706,14 +716,15 @@ void PluginManagerPrivate::readPluginPaths() while (!searchPaths.isEmpty()) { const QDir dir(searchPaths.takeFirst()); const QFileInfoList files = dir.entryInfoList(QStringList() << QString("*.%1").arg(extension), QDir::Files); - foreach (const QFileInfo &file, files) - specFiles << file.absoluteFilePath(); - const QFileInfoList dirs = dir.entryInfoList(QDir::Dirs|QDir::NoDotAndDotDot); - foreach (const QFileInfo &subdir, dirs) - searchPaths << subdir.absoluteFilePath(); + foreach(const QFileInfo &file, files) + specFiles << file.absoluteFilePath(); + const QFileInfoList dirs = dir.entryInfoList(QDir::Dirs | QDir::NoDotAndDotDot); + foreach(const QFileInfo &subdir, dirs) + searchPaths << subdir.absoluteFilePath(); } - foreach (const QString &specFile, specFiles) { + foreach(const QString &specFile, specFiles) { PluginSpec *spec = new PluginSpec; + spec->d->read(specFile); pluginSpecs.append(spec); } @@ -725,12 +736,12 @@ void PluginManagerPrivate::readPluginPaths() void PluginManagerPrivate::resolveDependencies() { - foreach (PluginSpec *spec, pluginSpecs) { + foreach(PluginSpec * spec, pluginSpecs) { spec->d->resolveDependencies(pluginSpecs); } } - // Look in argument descriptions of the specs for the option. +// Look in argument descriptions of the specs for the option. PluginSpec *PluginManagerPrivate::pluginForOption(const QString &option, bool *requiresArgument) const { // Look in the plugins for an option @@ -756,9 +767,9 @@ PluginSpec *PluginManagerPrivate::pluginForOption(const QString &option, bool *r PluginSpec *PluginManagerPrivate::pluginByName(const QString &name) const { - foreach (PluginSpec *spec, pluginSpecs) - if (spec->name() == name) - return spec; + foreach(PluginSpec * spec, pluginSpecs) + if (spec->name() == name) { + return spec; + } return 0; } - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.h index c194c20fd..215ae4e0f 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.h @@ -4,25 +4,25 @@ * @file pluginmanager.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,22 +41,23 @@ class QTextStream; QT_END_NAMESPACE namespace ExtensionSystem { - namespace Internal { - class PluginManagerPrivate; +class PluginManagerPrivate; } class IPlugin; class PluginSpec; -class EXTENSIONSYSTEM_EXPORT PluginManager : public QObject -{ +class EXTENSIONSYSTEM_EXPORT PluginManager : public QObject { Q_DISABLE_COPY(PluginManager) Q_OBJECT public: static PluginManager *instance(); - bool allPluginsLoaded(){return m_allPluginsLoaded;} + bool allPluginsLoaded() + { + return m_allPluginsLoaded; + } PluginManager(); virtual ~PluginManager(); @@ -67,24 +68,28 @@ public: template QList getObjects() const { QReadLocker lock(&m_lock); + QList results; QList all = allObjects(); QList result; - foreach (QObject *obj, all) { + foreach(QObject * obj, all) { result = Aggregation::query_all(obj); - if (!result.isEmpty()) + if (!result.isEmpty()) { results += result; + } } return results; } template T *getObject() const { QReadLocker lock(&m_lock); + QList all = allObjects(); T *result = 0; - foreach (QObject *obj, all) { - if ((result = Aggregation::query(obj)) != 0) + foreach(QObject * obj, all) { + if ((result = Aggregation::query(obj)) != 0) { break; + } } return result; } @@ -100,9 +105,9 @@ public: // command line arguments QStringList arguments() const; bool parseOptions(const QStringList &args, - const QMap &appOptions, - QMap *foundAppOptions, - QString *errorString); + const QMap &appOptions, + QMap *foundAppOptions, + QString *errorString); static void formatOptions(QTextStream &str, int optionIndentation, int descriptionIndentation); void formatPluginOptions(QTextStream &str, int optionIndentation, int descriptionIndentation) const; void formatPluginVersions(QTextStream &str) const; @@ -114,7 +119,7 @@ signals: void objectAdded(QObject *obj); void aboutToRemoveObject(QObject *obj); - void pluginAboutToBeLoaded(ExtensionSystem::PluginSpec* pluginSpec); + void pluginAboutToBeLoaded(ExtensionSystem::PluginSpec *pluginSpec); void pluginsChanged(); void pluginsLoadEnded(); private slots: @@ -128,7 +133,6 @@ private: friend class Internal::PluginManagerPrivate; }; - } // namespace ExtensionSystem #endif // EXTENSIONSYSTEM_PLUGINMANAGER_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager_p.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager_p.h index cea427806..29b8e78f0 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager_p.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager_p.h @@ -4,25 +4,25 @@ * @file pluginmanager_p.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,15 +37,12 @@ #include namespace ExtensionSystem { - class PluginManager; namespace Internal { - class PluginSpecPrivate; -class EXTENSIONSYSTEM_EXPORT PluginManagerPrivate -{ +class EXTENSIONSYSTEM_EXPORT PluginManagerPrivate { public: PluginManagerPrivate(PluginManager *pluginManager); virtual ~PluginManagerPrivate(); @@ -81,11 +78,10 @@ private: void readPluginPaths(); bool loadQueue(PluginSpec *spec, - QList &queue, - QList &circularityCheckQueue); + QList &queue, + QList &circularityCheckQueue); void stopAll(); }; - } // namespace Internal } // namespace ExtensionSystem diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.cpp b/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.cpp index f3615595e..1cc6dd8f8 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.cpp @@ -4,25 +4,25 @@ * @file pluginspec.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -50,12 +50,12 @@ #if USE_UNPATCHED_QPLUGINLOADER # include - typedef QT_PREPEND_NAMESPACE(QPluginLoader) PluginLoader; +typedef QT_PREPEND_NAMESPACE (QPluginLoader) PluginLoader; #else # include "patchedpluginloader.cpp" - typedef PatchedPluginLoader PluginLoader; +typedef PatchedPluginLoader PluginLoader; #endif @@ -69,17 +69,17 @@ See also ExtensionSystem::IPlugin for more information about plugin dependencies and version matching. -*/ + */ /*! \variable ExtensionSystem::PluginDependency::name String identifier of the plugin. -*/ + */ /*! \variable ExtensionSystem::PluginDependency::version Version string that a plugin must match to fill this dependency. -*/ + */ /*! \class ExtensionSystem::PluginSpec @@ -90,7 +90,7 @@ goes through its loading process (see PluginSpec::State). If an error occurs, the plugin spec is the place to look for the error details. -*/ + */ /*! \enum ExtensionSystem::PluginSpec::State @@ -120,14 +120,14 @@ The plugin has been shut down, i.e. the plugin's IPlugin::shutdown() method has been called. \value Deleted The plugin instance has been deleted. -*/ + */ using namespace ExtensionSystem; using namespace ExtensionSystem::Internal; /*! \fn bool PluginDependency::operator==(const PluginDependency &other) \internal -*/ + */ bool PluginDependency::operator==(const PluginDependency &other) { return name == other.name && version == other.version; @@ -136,16 +136,15 @@ bool PluginDependency::operator==(const PluginDependency &other) /*! \fn PluginSpec::PluginSpec() \internal -*/ + */ PluginSpec::PluginSpec() : d(new PluginSpecPrivate(this)) -{ -} +{} /*! \fn PluginSpec::~PluginSpec() \internal -*/ + */ PluginSpec::~PluginSpec() { delete d; @@ -155,7 +154,7 @@ PluginSpec::~PluginSpec() /*! \fn QString PluginSpec::name() const The plugin name. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::name() const { return d->name; @@ -164,7 +163,7 @@ QString PluginSpec::name() const /*! \fn QString PluginSpec::version() const The plugin version. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::version() const { return d->version; @@ -173,7 +172,7 @@ QString PluginSpec::version() const /*! \fn QString PluginSpec::compatVersion() const The plugin compatibility version. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::compatVersion() const { return d->compatVersion; @@ -182,7 +181,7 @@ QString PluginSpec::compatVersion() const /*! \fn QString PluginSpec::vendor() const The plugin vendor. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::vendor() const { return d->vendor; @@ -191,7 +190,7 @@ QString PluginSpec::vendor() const /*! \fn QString PluginSpec::copyright() const The plugin copyright. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::copyright() const { return d->copyright; @@ -200,7 +199,7 @@ QString PluginSpec::copyright() const /*! \fn QString PluginSpec::license() const The plugin license. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::license() const { return d->license; @@ -209,7 +208,7 @@ QString PluginSpec::license() const /*! \fn QString PluginSpec::description() const The plugin description. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::description() const { return d->description; @@ -218,7 +217,7 @@ QString PluginSpec::description() const /*! \fn QString PluginSpec::url() const The plugin url where you can find more information about the plugin. This is valid after the PluginSpec::Read state is reached. -*/ + */ QString PluginSpec::url() const { return d->url; @@ -227,7 +226,7 @@ QString PluginSpec::url() const /*! \fn QList PluginSpec::dependencies() const The plugin dependencies. This is valid after the PluginSpec::Read state is reached. -*/ + */ QList PluginSpec::dependencies() const { return d->dependencies; @@ -236,7 +235,7 @@ QList PluginSpec::dependencies() const /*! \fn PluginSpec::PluginArgumentDescriptions PluginSpec::argumentDescriptions() const Returns a list of descriptions of command line arguments the plugin processes. -*/ + */ PluginSpec::PluginArgumentDescriptions PluginSpec::argumentDescriptions() const { @@ -247,7 +246,7 @@ PluginSpec::PluginArgumentDescriptions PluginSpec::argumentDescriptions() const \fn QString PluginSpec::location() const The absolute path to the directory containing the plugin xml description file this PluginSpec corresponds to. -*/ + */ QString PluginSpec::location() const { return d->location; @@ -257,7 +256,7 @@ QString PluginSpec::location() const \fn QString PluginSpec::filePath() const The absolute path to the plugin xml description file (including the file name) this PluginSpec corresponds to. -*/ + */ QString PluginSpec::filePath() const { return d->filePath; @@ -266,7 +265,7 @@ QString PluginSpec::filePath() const /*! \fn QStringList PluginSpec::arguments() const Command line arguments specific to that plugin. Set at startup -*/ + */ QStringList PluginSpec::arguments() const { @@ -276,7 +275,7 @@ QStringList PluginSpec::arguments() const /*! \fn void PluginSpec::setArguments(const QStringList &arguments) Set the command line arguments specific to that plugin to \a arguments. -*/ + */ void PluginSpec::setArguments(const QStringList &arguments) { @@ -286,7 +285,7 @@ void PluginSpec::setArguments(const QStringList &arguments) /*! \fn PluginSpec::addArgument(const QString &argument) Adds \a argument to the command line arguments specific to that plugin. -*/ + */ void PluginSpec::addArgument(const QString &argument) { @@ -298,7 +297,7 @@ void PluginSpec::addArgument(const QString &argument) \fn PluginSpec::State PluginSpec::state() const The state in which the plugin currently is. See the description of the PluginSpec::State enum for details. -*/ + */ PluginSpec::State PluginSpec::state() const { return d->state; @@ -307,7 +306,7 @@ PluginSpec::State PluginSpec::state() const /*! \fn bool PluginSpec::hasError() const Returns whether an error occurred while reading/starting the plugin. -*/ + */ bool PluginSpec::hasError() const { return d->hasError; @@ -316,7 +315,7 @@ bool PluginSpec::hasError() const /*! \fn QString PluginSpec::errorString() const Detailed, possibly multi-line, error description in case of an error. -*/ + */ QString PluginSpec::errorString() const { return d->errorString; @@ -328,7 +327,7 @@ QString PluginSpec::errorString() const \a pluginName and \a version. \sa PluginSpec::dependencies() -*/ + */ bool PluginSpec::provides(const QString &pluginName, const QString &version) const { return d->provides(pluginName, version); @@ -338,7 +337,7 @@ bool PluginSpec::provides(const QString &pluginName, const QString &version) con \fn IPlugin *PluginSpec::plugin() const The corresponding IPlugin instance, if the plugin library has already been successfully loaded, i.e. the PluginSpec::Loaded state is reached. -*/ + */ IPlugin *PluginSpec::plugin() const { return d->plugin; @@ -350,70 +349,71 @@ IPlugin *PluginSpec::plugin() const Valid if PluginSpec::Resolved state is reached. \sa PluginSpec::dependencies() -*/ + */ QList PluginSpec::dependencySpecs() const { return d->dependencySpecs; } -//==========PluginSpecPrivate================== +// ==========PluginSpecPrivate================== namespace { - const char * const PLUGIN = "plugin"; - const char * const PLUGIN_NAME = "name"; - const char * const PLUGIN_VERSION = "version"; - const char * const PLUGIN_COMPATVERSION = "compatVersion"; - const char * const VENDOR = "vendor"; - const char * const COPYRIGHT = "copyright"; - const char * const LICENSE = "license"; - const char * const DESCRIPTION = "description"; - const char * const URL = "url"; - const char * const DEPENDENCYLIST = "dependencyList"; - const char * const DEPENDENCY = "dependency"; - const char * const DEPENDENCY_NAME = "name"; - const char * const DEPENDENCY_VERSION = "version"; - const char * const ARGUMENTLIST = "argumentList"; - const char * const ARGUMENT = "argument"; - const char * const ARGUMENT_NAME = "name"; - const char * const ARGUMENT_PARAMETER = "parameter"; +const char *const PLUGIN = "plugin"; +const char *const PLUGIN_NAME = "name"; +const char *const PLUGIN_VERSION = "version"; +const char *const PLUGIN_COMPATVERSION = "compatVersion"; +const char *const VENDOR = "vendor"; +const char *const COPYRIGHT = "copyright"; +const char *const LICENSE = "license"; +const char *const DESCRIPTION = "description"; +const char *const URL = "url"; +const char *const DEPENDENCYLIST = "dependencyList"; +const char *const DEPENDENCY = "dependency"; +const char *const DEPENDENCY_NAME = "name"; +const char *const DEPENDENCY_VERSION = "version"; +const char *const ARGUMENTLIST = "argumentList"; +const char *const ARGUMENT = "argument"; +const char *const ARGUMENT_NAME = "name"; +const char *const ARGUMENT_PARAMETER = "parameter"; } /*! \fn PluginSpecPrivate::PluginSpecPrivate(PluginSpec *spec) \internal -*/ + */ PluginSpecPrivate::PluginSpecPrivate(PluginSpec *spec) : plugin(0), state(PluginSpec::Invalid), hasError(false), q(spec) -{ -} +{} /*! \fn bool PluginSpecPrivate::read(const QString &fileName) \internal -*/ + */ bool PluginSpecPrivate::read(const QString &fileName) { name = version - = compatVersion - = vendor - = copyright - = license - = description - = url - = location - = ""; - state = PluginSpec::Invalid; - hasError = false; + = compatVersion + = vendor + = copyright + = license + = description + = url + = location + = ""; + state = PluginSpec::Invalid; + hasError = false; errorString = ""; dependencies.clear(); QFile file(fileName); - if (!file.exists()) + if (!file.exists()) { return reportError(tr("File does not exist: %1").arg(file.fileName())); - if (!file.open(QIODevice::ReadOnly)) + } + if (!file.open(QIODevice::ReadOnly)) { return reportError(tr("Could not open file for read: %1").arg(file.fileName())); + } QFileInfo fileInfo(file); location = fileInfo.absolutePath(); filePath = fileInfo.absoluteFilePath(); @@ -428,12 +428,13 @@ bool PluginSpecPrivate::read(const QString &fileName) break; } } - if (reader.hasError()) + if (reader.hasError()) { return reportError(tr("Error parsing file %1: %2, at line %3, column %4") - .arg(file.fileName()) - .arg(reader.errorString()) - .arg(reader.lineNumber()) - .arg(reader.columnNumber())); + .arg(file.fileName()) + .arg(reader.errorString()) + .arg(reader.lineNumber()) + .arg(reader.columnNumber())); + } state = PluginSpec::Read; return true; } @@ -441,11 +442,11 @@ bool PluginSpecPrivate::read(const QString &fileName) /*! \fn bool PluginSpecPrivate::reportError(const QString &err) \internal -*/ + */ bool PluginSpecPrivate::reportError(const QString &err) { errorString = err; - hasError = true; + hasError = true; return false; } @@ -477,10 +478,11 @@ static inline QString msgUnexpectedToken() /*! \fn void PluginSpecPrivate::readPluginSpec(QXmlStreamReader &reader) \internal -*/ + */ void PluginSpecPrivate::readPluginSpec(QXmlStreamReader &reader) { QString element = reader.name().toString(); + if (element != QString(PLUGIN)) { reader.raiseError(QCoreApplication::translate("PluginSpec", "Expected element '%1' as top level element").arg(PLUGIN)); return; @@ -511,22 +513,23 @@ void PluginSpecPrivate::readPluginSpec(QXmlStreamReader &reader) switch (reader.tokenType()) { case QXmlStreamReader::StartElement: element = reader.name().toString(); - if (element == VENDOR) + if (element == VENDOR) { vendor = reader.readElementText().trimmed(); - else if (element == COPYRIGHT) + } else if (element == COPYRIGHT) { copyright = reader.readElementText().trimmed(); - else if (element == LICENSE) + } else if (element == LICENSE) { license = reader.readElementText().trimmed(); - else if (element == DESCRIPTION) + } else if (element == DESCRIPTION) { description = reader.readElementText().trimmed(); - else if (element == URL) + } else if (element == URL) { url = reader.readElementText().trimmed(); - else if (element == DEPENDENCYLIST) + } else if (element == DEPENDENCYLIST) { readDependencies(reader); - else if (element == ARGUMENTLIST) + } else if (element == ARGUMENTLIST) { readArgumentDescriptions(reader); - else + } else { reader.raiseError(msgInvalidElement(name)); + } break; case QXmlStreamReader::EndDocument: case QXmlStreamReader::Comment: @@ -543,11 +546,12 @@ void PluginSpecPrivate::readPluginSpec(QXmlStreamReader &reader) /*! \fn void PluginSpecPrivate::readArgumentDescriptions(QXmlStreamReader &reader) \internal -*/ + */ void PluginSpecPrivate::readArgumentDescriptions(QXmlStreamReader &reader) { QString element; + while (!reader.atEnd()) { reader.readNext(); switch (reader.tokenType()) { @@ -564,8 +568,9 @@ void PluginSpecPrivate::readArgumentDescriptions(QXmlStreamReader &reader) break; case QXmlStreamReader::EndElement: element = reader.name().toString(); - if (element == ARGUMENTLIST) + if (element == ARGUMENTLIST) { return; + } reader.raiseError(msgUnexpectedClosing(element)); break; default: @@ -578,29 +583,32 @@ void PluginSpecPrivate::readArgumentDescriptions(QXmlStreamReader &reader) /*! \fn void PluginSpecPrivate::readArgumentDescription(QXmlStreamReader &reader) \internal -*/ + */ void PluginSpecPrivate::readArgumentDescription(QXmlStreamReader &reader) { PluginArgumentDescription arg; + arg.name = reader.attributes().value(ARGUMENT_NAME).toString(); if (arg.name.isEmpty()) { reader.raiseError(msgAttributeMissing(ARGUMENT, ARGUMENT_NAME)); return; } - arg.parameter = reader.attributes().value(ARGUMENT_PARAMETER).toString(); + arg.parameter = reader.attributes().value(ARGUMENT_PARAMETER).toString(); arg.description = reader.readElementText(); - if (reader.tokenType() != QXmlStreamReader::EndElement) + if (reader.tokenType() != QXmlStreamReader::EndElement) { reader.raiseError(msgUnexpectedToken()); + } argumentDescriptions.push_back(arg); } /*! \fn void PluginSpecPrivate::readDependencies(QXmlStreamReader &reader) \internal -*/ + */ void PluginSpecPrivate::readDependencies(QXmlStreamReader &reader) { QString element; + while (!reader.atEnd()) { reader.readNext(); switch (reader.tokenType()) { @@ -617,8 +625,9 @@ void PluginSpecPrivate::readDependencies(QXmlStreamReader &reader) break; case QXmlStreamReader::EndElement: element = reader.name().toString(); - if (element == DEPENDENCYLIST) + if (element == DEPENDENCYLIST) { return; + } reader.raiseError(msgUnexpectedClosing(element)); break; default: @@ -631,10 +640,11 @@ void PluginSpecPrivate::readDependencies(QXmlStreamReader &reader) /*! \fn void PluginSpecPrivate::readDependencyEntry(QXmlStreamReader &reader) \internal -*/ + */ void PluginSpecPrivate::readDependencyEntry(QXmlStreamReader &reader) { PluginDependency dep; + dep.name = reader.attributes().value(DEPENDENCY_NAME).toString(); if (dep.name.isEmpty()) { reader.raiseError(msgAttributeMissing(DEPENDENCY, DEPENDENCY_NAME)); @@ -647,34 +657,37 @@ void PluginSpecPrivate::readDependencyEntry(QXmlStreamReader &reader) } dependencies.append(dep); reader.readNext(); - if (reader.tokenType() != QXmlStreamReader::EndElement) + if (reader.tokenType() != QXmlStreamReader::EndElement) { reader.raiseError(msgUnexpectedToken()); + } } /*! \fn bool PluginSpecPrivate::provides(const QString &pluginName, const QString &pluginVersion) const \internal -*/ + */ bool PluginSpecPrivate::provides(const QString &pluginName, const QString &pluginVersion) const { - if (QString::compare(pluginName, name, Qt::CaseInsensitive) != 0) + if (QString::compare(pluginName, name, Qt::CaseInsensitive) != 0) { return false; + } return (versionCompare(version, pluginVersion) >= 0) && (versionCompare(compatVersion, pluginVersion) <= 0); } /*! \fn QRegExp &PluginSpecPrivate::versionRegExp() \internal -*/ + */ QRegExp &PluginSpecPrivate::versionRegExp() { static QRegExp reg("([0-9]+)(?:[.]([0-9]+))?(?:[.]([0-9]+))?(?:_([0-9]+))?"); + return reg; } /*! \fn bool PluginSpecPrivate::isValidVersion(const QString &version) \internal -*/ + */ bool PluginSpecPrivate::isValidVersion(const QString &version) { return versionRegExp().exactMatch(version); @@ -683,24 +696,29 @@ bool PluginSpecPrivate::isValidVersion(const QString &version) /*! \fn int PluginSpecPrivate::versionCompare(const QString &version1, const QString &version2) \internal -*/ + */ int PluginSpecPrivate::versionCompare(const QString &version1, const QString &version2) { QRegExp reg1 = versionRegExp(); QRegExp reg2 = versionRegExp(); - if (!reg1.exactMatch(version1)) + + if (!reg1.exactMatch(version1)) { return 0; - if (!reg2.exactMatch(version2)) + } + if (!reg2.exactMatch(version2)) { return 0; + } int number1; int number2; for (int i = 0; i < 4; ++i) { - number1 = reg1.cap(i+1).toInt(); - number2 = reg2.cap(i+1).toInt(); - if (number1 < number2) + number1 = reg1.cap(i + 1).toInt(); + number2 = reg2.cap(i + 1).toInt(); + if (number1 < number2) { return -1; - if (number1 > number2) + } + if (number1 > number2) { return 1; + } } return 0; } @@ -708,22 +726,25 @@ int PluginSpecPrivate::versionCompare(const QString &version1, const QString &ve /*! \fn bool PluginSpecPrivate::resolveDependencies(const QList &specs) \internal -*/ + */ bool PluginSpecPrivate::resolveDependencies(const QList &specs) { - if (hasError) + if (hasError) { return false; - if (state == PluginSpec::Resolved) + } + if (state == PluginSpec::Resolved) { state = PluginSpec::Read; // Go back, so we just re-resolve the dependencies. + } if (state != PluginSpec::Read) { errorString = QCoreApplication::translate("PluginSpec", "Resolving dependencies failed because state != Read"); - hasError = true; + hasError = true; return false; } QList resolvedDependencies; - foreach (const PluginDependency &dependency, dependencies) { + foreach(const PluginDependency &dependency, dependencies) { PluginSpec *found = 0; - foreach (PluginSpec *spec, specs) { + + foreach(PluginSpec * spec, specs) { if (spec->provides(dependency.name, dependency.version)) { found = spec; break; @@ -731,16 +752,18 @@ bool PluginSpecPrivate::resolveDependencies(const QList &specs) } if (!found) { hasError = true; - if (!errorString.isEmpty()) + if (!errorString.isEmpty()) { errorString.append("\n"); + } errorString.append(QCoreApplication::translate("PluginSpec", "Could not resolve dependency '%1(%2)'") - .arg(dependency.name).arg(dependency.version)); + .arg(dependency.name).arg(dependency.version)); continue; } resolvedDependencies.append(found); } - if (hasError) + if (hasError) { return false; + } dependencySpecs = resolvedDependencies; state = PluginSpec::Resolved; return true; @@ -749,16 +772,18 @@ bool PluginSpecPrivate::resolveDependencies(const QList &specs) /*! \fn bool PluginSpecPrivate::loadLibrary() \internal -*/ + */ bool PluginSpecPrivate::loadLibrary() { - if (hasError) + if (hasError) { return false; + } if (state != PluginSpec::Resolved) { - if (state == PluginSpec::Loaded) + if (state == PluginSpec::Loaded) { return true; + } errorString = QCoreApplication::translate("PluginSpec", "Loading the library failed because state != Resolved"); - hasError = true; + hasError = true; return false; } #ifdef QT_NO_DEBUG @@ -771,7 +796,7 @@ bool PluginSpecPrivate::loadLibrary() QString libName = QString("%1/lib%2.so").arg(location).arg(name); #endif -#else //Q_NO_DEBUG +#else // Q_NO_DEBUG #ifdef Q_OS_WIN QString libName = QString("%1/%2d.dll").arg(location).arg(name); @@ -785,18 +810,18 @@ bool PluginSpecPrivate::loadLibrary() PluginLoader loader(libName); if (!loader.load()) { - hasError = true; + hasError = true; errorString = libName + QString::fromLatin1(": ") + loader.errorString(); return false; } - IPlugin *pluginObject = qobject_cast(loader.instance()); + IPlugin *pluginObject = qobject_cast(loader.instance()); if (!pluginObject) { - hasError = true; + hasError = true; errorString = QCoreApplication::translate("PluginSpec", "Plugin is not valid (does not derive from IPlugin)"); loader.unload(); return false; } - state = PluginSpec::Loaded; + state = PluginSpec::Loaded; plugin = pluginObject; plugin->d->pluginSpec = q; return true; @@ -805,27 +830,29 @@ bool PluginSpecPrivate::loadLibrary() /*! \fn bool PluginSpecPrivate::initializePlugin() \internal -*/ + */ bool PluginSpecPrivate::initializePlugin() { - if (hasError) + if (hasError) { return false; + } if (state != PluginSpec::Loaded) { - if (state == PluginSpec::Initialized) + if (state == PluginSpec::Initialized) { return true; + } errorString = QCoreApplication::translate("PluginSpec", "Initializing the plugin failed because state != Loaded"); - hasError = true; + hasError = true; return false; } if (!plugin) { errorString = QCoreApplication::translate("PluginSpec", "Internal error: have no plugin instance to initialize"); - hasError = true; + hasError = true; return false; } QString err; if (!plugin->initialize(arguments, &err)) { errorString = QCoreApplication::translate("PluginSpec", "Plugin initialization failed: %1").arg(err); - hasError = true; + hasError = true; return false; } state = PluginSpec::Initialized; @@ -835,21 +862,23 @@ bool PluginSpecPrivate::initializePlugin() /*! \fn bool PluginSpecPrivate::initializeExtensions() \internal -*/ + */ bool PluginSpecPrivate::initializeExtensions() { - if (hasError) + if (hasError) { return false; + } if (state != PluginSpec::Initialized) { - if (state == PluginSpec::Running) + if (state == PluginSpec::Running) { return true; + } errorString = QCoreApplication::translate("PluginSpec", "Cannot perform extensionsInitialized because state != Initialized"); - hasError = true; + hasError = true; return false; } if (!plugin) { errorString = QCoreApplication::translate("PluginSpec", "Internal error: have no plugin instance to perform extensionsInitialized"); - hasError = true; + hasError = true; return false; } plugin->extensionsInitialized(); @@ -860,11 +889,12 @@ bool PluginSpecPrivate::initializeExtensions() /*! \fn bool PluginSpecPrivate::stop() \internal -*/ + */ void PluginSpecPrivate::stop() { - if (!plugin) + if (!plugin) { return; + } plugin->shutdown(); state = PluginSpec::Stopped; } @@ -872,13 +902,13 @@ void PluginSpecPrivate::stop() /*! \fn bool PluginSpecPrivate::kill() \internal -*/ + */ void PluginSpecPrivate::kill() { - if (!plugin) + if (!plugin) { return; + } delete plugin; plugin = 0; - state = PluginSpec::Deleted; + state = PluginSpec::Deleted; } - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.h index f226d6021..846bb4647 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginspec.h @@ -4,25 +4,25 @@ * @file pluginspec.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,32 +39,28 @@ class QStringList; QT_END_NAMESPACE namespace ExtensionSystem { - namespace Internal { - class PluginSpecPrivate; - class PluginManagerPrivate; +class PluginSpecPrivate; +class PluginManagerPrivate; } class IPlugin; -struct EXTENSIONSYSTEM_EXPORT PluginDependency -{ +struct EXTENSIONSYSTEM_EXPORT PluginDependency { QString name; QString version; bool operator==(const PluginDependency &other); }; -struct EXTENSIONSYSTEM_EXPORT PluginArgumentDescription -{ +struct EXTENSIONSYSTEM_EXPORT PluginArgumentDescription { QString name; QString parameter; QString description; }; -class EXTENSIONSYSTEM_EXPORT PluginSpec -{ +class EXTENSIONSYSTEM_EXPORT PluginSpec { public: - enum State { Invalid, Read, Resolved, Loaded, Initialized, Running, Stopped, Deleted}; + enum State { Invalid, Read, Resolved, Loaded, Initialized, Running, Stopped, Deleted }; ~PluginSpec(); @@ -109,8 +105,6 @@ private: Internal::PluginSpecPrivate *d; friend class Internal::PluginManagerPrivate; }; - } // namespace ExtensionSystem #endif // PLUGINSPEC_H - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginspec_p.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginspec_p.h index 912364572..228ed29f2 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginspec_p.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginspec_p.h @@ -4,25 +4,25 @@ * @file pluginspec_p.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,14 +36,11 @@ #include namespace ExtensionSystem { - class IPlugin; class PluginManager; namespace Internal { - -class EXTENSIONSYSTEM_EXPORT PluginSpecPrivate : public QObject -{ +class EXTENSIONSYSTEM_EXPORT PluginSpecPrivate : public QObject { Q_OBJECT public: @@ -95,7 +92,6 @@ private: static QRegExp &versionRegExp(); }; - } // namespace Internal } // namespace ExtensionSystem diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginview.cpp b/ground/openpilotgcs/src/libs/extensionsystem/pluginview.cpp index 4fe0decef..6d5daffc2 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginview.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginview.cpp @@ -4,25 +4,25 @@ * @file pluginview.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -47,33 +47,32 @@ \sa ExtensionSystem::PluginDetailsView \sa ExtensionSystem::PluginErrorView -*/ + */ /*! \fn void PluginView::currentPluginChanged(ExtensionSystem::PluginSpec *spec) The current selection in the plugin list has changed to the plugin corresponding to \a spec. -*/ + */ /*! \fn void PluginView::pluginActivated(ExtensionSystem::PluginSpec *spec) The plugin list entry corresponding to \a spec has been activated, e.g. by a double-click. -*/ + */ using namespace ExtensionSystem; -Q_DECLARE_METATYPE(ExtensionSystem::PluginSpec*) - +Q_DECLARE_METATYPE(ExtensionSystem::PluginSpec *) /*! \fn PluginView::PluginView(PluginManager *manager, QWidget *parent) Constructs a PluginView that gets the list of plugins from the given plugin \a manager with a given \a parent widget. -*/ + */ PluginView::PluginView(PluginManager *manager, QWidget *parent) - : QWidget(parent), - m_ui(new Internal::Ui::PluginView), - p(new Internal::PluginViewPrivate) + : QWidget(parent), + m_ui(new Internal::Ui::PluginView), + p(new Internal::PluginViewPrivate) { m_ui->setupUi(this); QHeaderView *header = m_ui->pluginWidget->header(); @@ -83,17 +82,17 @@ PluginView::PluginView(PluginManager *manager, QWidget *parent) m_ui->pluginWidget->sortItems(1, Qt::AscendingOrder); p->manager = manager; connect(p->manager, SIGNAL(pluginsChanged()), this, SLOT(updateList())); - connect(m_ui->pluginWidget, SIGNAL(currentItemChanged(QTreeWidgetItem*,QTreeWidgetItem*)), - this, SLOT(selectPlugin(QTreeWidgetItem*))); - connect(m_ui->pluginWidget, SIGNAL(itemActivated(QTreeWidgetItem*,int)), - this, SLOT(activatePlugin(QTreeWidgetItem*))); + connect(m_ui->pluginWidget, SIGNAL(currentItemChanged(QTreeWidgetItem *, QTreeWidgetItem *)), + this, SLOT(selectPlugin(QTreeWidgetItem *))); + connect(m_ui->pluginWidget, SIGNAL(itemActivated(QTreeWidgetItem *, int)), + this, SLOT(activatePlugin(QTreeWidgetItem *))); updateList(); } /*! \fn PluginView::~PluginView() \internal -*/ + */ PluginView::~PluginView() { delete p; @@ -103,11 +102,12 @@ PluginView::~PluginView() /*! \fn PluginSpec *PluginView::currentPlugin() const Returns the current selection in the list of plugins. -*/ + */ PluginSpec *PluginView::currentPlugin() const { - if (!m_ui->pluginWidget->currentItem()) + if (!m_ui->pluginWidget->currentItem()) { return 0; + } return m_ui->pluginWidget->currentItem()->data(0, Qt::UserRole).value(); } @@ -115,42 +115,47 @@ void PluginView::updateList() { static QIcon okIcon(":/extensionsystem/images/ok.png"); static QIcon errorIcon(":/extensionsystem/images/error.png"); + QList items; QTreeWidgetItem *currentItem = 0; PluginSpec *currPlugin = currentPlugin(); - foreach (PluginSpec *spec, p->manager->plugins()) { + foreach(PluginSpec * spec, p->manager->plugins()) { QTreeWidgetItem *item = new QTreeWidgetItem(QStringList() - << "" - << spec->name() - << QString("%1 (%2)").arg(spec->version()).arg(spec->compatVersion()) - << spec->vendor() - << QDir::toNativeSeparators(spec->filePath())); + << "" + << spec->name() + << QString("%1 (%2)").arg(spec->version()).arg(spec->compatVersion()) + << spec->vendor() + << QDir::toNativeSeparators(spec->filePath())); + item->setToolTip(4, QDir::toNativeSeparators(spec->filePath())); item->setIcon(0, spec->hasError() ? errorIcon : okIcon); item->setData(0, Qt::UserRole, qVariantFromValue(spec)); items.append(item); - if (currPlugin == spec) + if (currPlugin == spec) { currentItem = item; + } } m_ui->pluginWidget->clear(); - if (!items.isEmpty()) + if (!items.isEmpty()) { m_ui->pluginWidget->addTopLevelItems(items); - if (currentItem) + } + if (currentItem) { m_ui->pluginWidget->setCurrentItem(currentItem); - else if (!items.isEmpty()) + } else if (!items.isEmpty()) { m_ui->pluginWidget->setCurrentItem(items.first()); + } } void PluginView::selectPlugin(QTreeWidgetItem *current) { - if (!current) + if (!current) { emit currentPluginChanged(0); - else + } else { emit currentPluginChanged(current->data(0, Qt::UserRole).value()); + } } void PluginView::activatePlugin(QTreeWidgetItem *item) { emit pluginActivated(item->data(0, Qt::UserRole).value()); } - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginview.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginview.h index b070218a9..f1d49eb6f 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginview.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginview.h @@ -4,25 +4,25 @@ * @file pluginview.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,19 +38,17 @@ class QTreeWidgetItem; QT_END_NAMESPACE namespace ExtensionSystem { - class PluginManager; class PluginSpec; namespace Internal { - class PluginViewPrivate; +class PluginViewPrivate; namespace Ui { - class PluginView; +class PluginView; } // namespace Ui } // namespace Internal -class EXTENSIONSYSTEM_EXPORT PluginView : public QWidget -{ +class EXTENSIONSYSTEM_EXPORT PluginView : public QWidget { Q_OBJECT public: @@ -72,7 +70,6 @@ private: Internal::Ui::PluginView *m_ui; Internal::PluginViewPrivate *p; }; - } // namespae ExtensionSystem #endif // PLUGIN_VIEW_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginview_p.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginview_p.h index c0bbee9c4..d00eedbcf 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginview_p.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginview_p.h @@ -4,25 +4,25 @@ * @file pluginview_p.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,17 +30,13 @@ #define PLUGINVIEW_P_H namespace ExtensionSystem { - class PluginManager; namespace Internal { - -class PluginViewPrivate -{ +class PluginViewPrivate { public: PluginManager *manager; }; - } // namespace Internal } // namespace ExtensionSystem diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.cpp index 6bbc210b5..67188bae9 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.cpp @@ -4,25 +4,25 @@ * @file plugin1.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,8 +33,7 @@ using namespace Plugin1; MyPlugin1::MyPlugin1() -{ -} +{} bool MyPlugin1::initialize(const QStringList &arguments, QString *errorString) { @@ -44,8 +43,6 @@ bool MyPlugin1::initialize(const QStringList &arguments, QString *errorString) } void MyPlugin1::extensionsInitialized() -{ -} +{} Q_EXPORT_PLUGIN(MyPlugin1) - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.h index b765ec016..6d656c3bf 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin1/plugin1.h @@ -4,25 +4,25 @@ * @file plugin1.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ #include namespace Plugin1 { - -class MyPlugin1 : public ExtensionSystem::IPlugin -{ +class MyPlugin1 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -45,7 +43,6 @@ public: bool initialize(const QStringList &arguments, QString *errorString); void extensionsInitialized(); }; - } // namespace Plugin1 #endif // PLUGIN1_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.cpp index f2ce38e8f..4bbf5513a 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.cpp @@ -4,25 +4,25 @@ * @file plugin2.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,8 +33,7 @@ using namespace Plugin2; MyPlugin2::MyPlugin2() -{ -} +{} bool MyPlugin2::initialize(const QStringList &, QString *) { @@ -42,8 +41,6 @@ bool MyPlugin2::initialize(const QStringList &, QString *) } void MyPlugin2::extensionsInitialized() -{ -} +{} Q_EXPORT_PLUGIN(MyPlugin2) - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.h index c15e0c458..b91731569 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin2/plugin2.h @@ -4,25 +4,25 @@ * @file plugin2.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ #include namespace Plugin2 { - -class MyPlugin2 : public ExtensionSystem::IPlugin -{ +class MyPlugin2 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -45,7 +43,6 @@ public: bool initialize(const QStringList &arguments, QString *errorString); void extensionsInitialized(); }; - } // Plugin2 #endif // PLUGIN2_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.cpp index 2761bd5d2..f74cc322f 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.cpp @@ -4,25 +4,25 @@ * @file plugin3.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "plugin3.h" @@ -32,8 +32,7 @@ using namespace Plugin3; MyPlugin3::MyPlugin3() -{ -} +{} bool MyPlugin3::initialize(const QStringList &, QString *) { @@ -41,8 +40,6 @@ bool MyPlugin3::initialize(const QStringList &, QString *) } void MyPlugin3::extensionsInitialized() -{ -} +{} Q_EXPORT_PLUGIN(MyPlugin3) - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.h index aa40fa1fb..69b2ec6b5 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/circularplugins/plugin3/plugin3.h @@ -4,25 +4,25 @@ * @file plugin3.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ #include namespace Plugin3 { - -class MyPlugin3 : public ExtensionSystem::IPlugin -{ +class MyPlugin3 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -45,7 +43,6 @@ public: bool initialize(const QStringList &arguments, QString *errorString); void extensionsInitialized(); }; - } // namespace Plugin3 #endif // PLUGIN3_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.cpp index 8eaf2cab5..8a59a392a 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.cpp @@ -4,25 +4,25 @@ * @file plugin1.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,8 +37,7 @@ using namespace Plugin1; MyPlugin1::MyPlugin1() : initializeCalled(false) -{ -} +{} bool MyPlugin1::initialize(const QStringList & /*arguments*/, QString *errorString) { @@ -49,20 +48,24 @@ bool MyPlugin1::initialize(const QStringList & /*arguments*/, QString *errorStri bool found2 = false; bool found3 = false; - foreach (QObject *object, ExtensionSystem::PluginManager::instance()->allObjects()) { - if (object->objectName() == "MyPlugin2") + foreach(QObject * object, ExtensionSystem::PluginManager::instance()->allObjects()) { + if (object->objectName() == "MyPlugin2") { found2 = true; - else if (object->objectName() == "MyPlugin3") + } else if (object->objectName() == "MyPlugin3") { found3 = true; + } } - if (found2 && found3) + if (found2 && found3) { return true; + } if (errorString) { QString error = "object(s) missing from plugin(s):"; - if (!found2) + if (!found2) { error.append(" plugin2"); - if (!found3) + } + if (!found3) { error.append(" plugin3"); + } *errorString = error; } return false; @@ -70,8 +73,9 @@ bool MyPlugin1::initialize(const QStringList & /*arguments*/, QString *errorStri void MyPlugin1::extensionsInitialized() { - if (!initializeCalled) + if (!initializeCalled) { return; + } // don't do this at home, it's just done here for the test QObject *obj = new QObject; obj->setObjectName("MyPlugin1_running"); @@ -79,4 +83,3 @@ void MyPlugin1::extensionsInitialized() } Q_EXPORT_PLUGIN(MyPlugin1) - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.h index 61b01b3b4..ffe1a8583 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin1/plugin1.h @@ -4,25 +4,25 @@ * @file plugin1.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ #include namespace Plugin1 { - -class MyPlugin1 : public ExtensionSystem::IPlugin -{ +class MyPlugin1 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -48,7 +46,6 @@ public: private: bool initializeCalled; }; - } // namespace Plugin1 #endif // PLUGIN1_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.cpp index e50167aeb..59d5da91e 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.cpp @@ -4,25 +4,25 @@ * @file plugin2.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,8 +37,7 @@ using namespace Plugin2; MyPlugin2::MyPlugin2() : initializeCalled(false) -{ -} +{} bool MyPlugin2::initialize(const QStringList &, QString *) { @@ -52,8 +51,9 @@ bool MyPlugin2::initialize(const QStringList &, QString *) void MyPlugin2::extensionsInitialized() { - if (!initializeCalled) + if (!initializeCalled) { return; + } // don't do this at home, it's just done here for the test QObject *obj = new QObject; obj->setObjectName("MyPlugin2_running"); @@ -61,4 +61,3 @@ void MyPlugin2::extensionsInitialized() } Q_EXPORT_PLUGIN(MyPlugin2) - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.h index 262d4ce9e..e7ce45485 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin2/plugin2.h @@ -4,25 +4,25 @@ * @file plugin2.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ #include namespace Plugin2 { - -class MyPlugin2 : public ExtensionSystem::IPlugin -{ +class MyPlugin2 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -48,7 +46,6 @@ public: private: bool initializeCalled; }; - } // namespace Plugin2 #endif // PLUGIN2_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.cpp index a55d7e51c..7b38e607b 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.cpp @@ -4,25 +4,25 @@ * @file plugin3.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,8 +37,7 @@ using namespace Plugin3; MyPlugin3::MyPlugin3() : initializeCalled(false) -{ -} +{} bool MyPlugin3::initialize(const QStringList & /*arguments*/, QString *errorString) { @@ -48,21 +47,25 @@ bool MyPlugin3::initialize(const QStringList & /*arguments*/, QString *errorStri addAutoReleasedObject(obj); bool found2 = false; - foreach (QObject *object, ExtensionSystem::PluginManager::instance()->allObjects()) { - if (object->objectName() == "MyPlugin2") + foreach(QObject * object, ExtensionSystem::PluginManager::instance()->allObjects()) { + if (object->objectName() == "MyPlugin2") { found2 = true; + } } - if (found2) + if (found2) { return true; - if (errorString) + } + if (errorString) { *errorString = "object from plugin2 could not be found"; + } return false; } void MyPlugin3::extensionsInitialized() { - if (!initializeCalled) + if (!initializeCalled) { return; + } // don't do this at home, it's just done here for the test QObject *obj = new QObject; obj->setObjectName("MyPlugin3_running"); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.h index f7c1f2cf8..17194f5b3 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/correctplugins1/plugin3/plugin3.h @@ -4,25 +4,25 @@ * @file plugin3.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef PLUGIN3_H @@ -33,9 +33,7 @@ #include namespace Plugin3 { - -class MyPlugin3 : public ExtensionSystem::IPlugin -{ +class MyPlugin3 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -47,7 +45,6 @@ public: private: bool initializeCalled; }; - } // namespace Plugin3 #endif // PLUGIN3_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/tst_pluginmanager.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/tst_pluginmanager.cpp index b81e5892c..515cc3409 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/tst_pluginmanager.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginmanager/tst_pluginmanager.cpp @@ -4,25 +4,25 @@ * @file tst_pluginmanager.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,8 +38,7 @@ using namespace ExtensionSystem; class SignalReceiver; -class tst_PluginManager : public QObject -{ +class tst_PluginManager : public QObject { Q_OBJECT private slots: @@ -57,8 +56,7 @@ private: SignalReceiver *m_sr; }; -class SignalReceiver : public QObject -{ +class SignalReceiver : public QObject { Q_OBJECT public: @@ -68,30 +66,36 @@ public: pluginsChangedCount(0), objectAddedObj(0), aboutToRemoveObjectObj(0) - { } + {} int objectAddedCount; int aboutToRemoveObjectCount; int pluginsChangedCount; QObject *objectAddedObj; QObject *aboutToRemoveObjectObj; public slots: - void objectAdded(QObject *obj) { objectAddedCount++; objectAddedObj = obj; } - void aboutToRemoveObject(QObject *obj) { aboutToRemoveObjectCount++; aboutToRemoveObjectObj = obj; } - void pluginsChanged() { pluginsChangedCount++; } + void objectAdded(QObject *obj) + { + objectAddedCount++; objectAddedObj = obj; + } + void aboutToRemoveObject(QObject *obj) + { + aboutToRemoveObjectCount++; aboutToRemoveObjectObj = obj; + } + void pluginsChanged() + { + pluginsChangedCount++; + } }; -class MyClass1 : public QObject -{ +class MyClass1 : public QObject { Q_OBJECT }; -class MyClass2 : public QObject -{ +class MyClass2 : public QObject { Q_OBJECT }; -class MyClass11 : public MyClass1 -{ +class MyClass11 : public MyClass1 { Q_OBJECT }; @@ -99,8 +103,8 @@ void tst_PluginManager::init() { m_pm = new PluginManager; m_sr = new SignalReceiver; - connect(m_pm, SIGNAL(objectAdded(QObject*)), m_sr, SLOT(objectAdded(QObject*))); - connect(m_pm, SIGNAL(aboutToRemoveObject(QObject*)), m_sr, SLOT(aboutToRemoveObject(QObject*))); + connect(m_pm, SIGNAL(objectAdded(QObject *)), m_sr, SLOT(objectAdded(QObject *))); + connect(m_pm, SIGNAL(aboutToRemoveObject(QObject *)), m_sr, SLOT(aboutToRemoveObject(QObject *))); connect(m_pm, SIGNAL(pluginsChanged()), m_sr, SLOT(pluginsChanged())); } @@ -114,6 +118,7 @@ void tst_PluginManager::addRemoveObjects() { QObject *object1 = new QObject; QObject *object2 = new QObject; + QCOMPARE(m_pm->allObjects().size(), 0); m_pm->addObject(object1); QCOMPARE(m_sr->objectAddedCount, 1); @@ -149,15 +154,16 @@ void tst_PluginManager::addRemoveObjects() void tst_PluginManager::getObject() { - MyClass2 *object2 = new MyClass2; + MyClass2 *object2 = new MyClass2; MyClass11 *object11 = new MyClass11; + m_pm->addObject(object2); - QCOMPARE(m_pm->getObject(), (MyClass11*)0); - QCOMPARE(m_pm->getObject(), (MyClass1*)0); + QCOMPARE(m_pm->getObject(), (MyClass11 *)0); + QCOMPARE(m_pm->getObject(), (MyClass1 *)0); QCOMPARE(m_pm->getObject(), object2); m_pm->addObject(object11); QCOMPARE(m_pm->getObject(), object11); - QCOMPARE(m_pm->getObject(), qobject_cast(object11)); + QCOMPARE(m_pm->getObject(), qobject_cast(object11)); QCOMPARE(m_pm->getObject(), object2); m_pm->removeObject(object2); m_pm->removeObject(object11); @@ -167,24 +173,25 @@ void tst_PluginManager::getObject() void tst_PluginManager::getObjects() { - MyClass1 *object1 = new MyClass1; - MyClass2 *object2 = new MyClass2; + MyClass1 *object1 = new MyClass1; + MyClass2 *object2 = new MyClass2; MyClass11 *object11 = new MyClass11; + m_pm->addObject(object2); - QCOMPARE(m_pm->getObjects(), QList()); - QCOMPARE(m_pm->getObjects(), QList()); - QCOMPARE(m_pm->getObjects(), QList() << object2); - QCOMPARE(m_pm->allObjects(), QList() << object2); + QCOMPARE(m_pm->getObjects(), QList()); + QCOMPARE(m_pm->getObjects(), QList()); + QCOMPARE(m_pm->getObjects(), QList() << object2); + QCOMPARE(m_pm->allObjects(), QList() << object2); m_pm->addObject(object11); - QCOMPARE(m_pm->getObjects(), QList() << object11); - QCOMPARE(m_pm->getObjects(), QList() << object11); - QCOMPARE(m_pm->getObjects(), QList() << object2); - QCOMPARE(m_pm->allObjects(), QList() << object2 << object11); + QCOMPARE(m_pm->getObjects(), QList() << object11); + QCOMPARE(m_pm->getObjects(), QList() << object11); + QCOMPARE(m_pm->getObjects(), QList() << object2); + QCOMPARE(m_pm->allObjects(), QList() << object2 << object11); m_pm->addObject(object1); - QCOMPARE(m_pm->getObjects(), QList() << object11); - QCOMPARE(m_pm->getObjects(), QList() << object11 << object1); - QCOMPARE(m_pm->getObjects(), QList() << object2); - QCOMPARE(m_pm->allObjects(), QList() << object2 << object11 << object1); + QCOMPARE(m_pm->getObjects(), QList() << object11); + QCOMPARE(m_pm->getObjects(), QList() << object11 << object1); + QCOMPARE(m_pm->getObjects(), QList() << object2); + QCOMPARE(m_pm->allObjects(), QList() << object2 << object11 << object1); m_pm->removeObject(object2); m_pm->removeObject(object11); m_pm->removeObject(object1); @@ -199,9 +206,10 @@ void tst_PluginManager::plugins() QCOMPARE(m_sr->pluginsChangedCount, 1); QSet plugins = m_pm->plugins(); QCOMPARE(plugins.count(), 3); - foreach (const QString &expected, QStringList() << "helloworld" << "MyPlugin" << "dummyPlugin") { + foreach(const QString &expected, QStringList() << "helloworld" << "MyPlugin" << "dummyPlugin") { bool found = false; - foreach (PluginSpec *spec, plugins) { + + foreach(PluginSpec * spec, plugins) { if (spec->name() == expected) { found = true; break; @@ -215,18 +223,18 @@ void tst_PluginManager::circularPlugins() { m_pm->setPluginPaths(QStringList() << "circularplugins"); m_pm->loadPlugins(); - foreach (PluginSpec *spec, m_pm->plugins()) { + foreach(PluginSpec * spec, m_pm->plugins()) { if (spec->name() == "plugin1") { QVERIFY(spec->hasError()); QCOMPARE(spec->state(), PluginSpec::Resolved); - QCOMPARE(spec->plugin(), (IPlugin*)0); + QCOMPARE(spec->plugin(), (IPlugin *)0); } else if (spec->name() == "plugin2") { QVERIFY(!spec->hasError()); QCOMPARE(spec->state(), PluginSpec::Running); } else if (spec->name() == "plugin3") { QVERIFY(spec->hasError()); QCOMPARE(spec->state(), PluginSpec::Resolved); - QCOMPARE(spec->plugin(), (IPlugin*)0); + QCOMPARE(spec->plugin(), (IPlugin *)0); } } } @@ -236,22 +244,24 @@ void tst_PluginManager::correctPlugins1() m_pm->setFileExtension("spec"); m_pm->setPluginPaths(QStringList() << "correctplugins1"); m_pm->loadPlugins(); - foreach (PluginSpec *spec, m_pm->plugins()) { - if (spec->hasError()) + foreach(PluginSpec * spec, m_pm->plugins()) { + if (spec->hasError()) { qDebug() << spec->errorString(); + } QVERIFY(!spec->hasError()); QCOMPARE(spec->state(), PluginSpec::Running); } bool plugin1running = false; bool plugin2running = false; bool plugin3running = false; - foreach (QObject *obj, m_pm->allObjects()) { - if (obj->objectName() == "MyPlugin1_running") + foreach(QObject * obj, m_pm->allObjects()) { + if (obj->objectName() == "MyPlugin1_running") { plugin1running = true; - else if (obj->objectName() == "MyPlugin2_running") + } else if (obj->objectName() == "MyPlugin2_running") { plugin2running = true; - else if (obj->objectName() == "MyPlugin3_running") + } else if (obj->objectName() == "MyPlugin3_running") { plugin3running = true; + } } QVERIFY(plugin1running); QVERIFY(plugin2running); @@ -261,4 +271,3 @@ void tst_PluginManager::correctPlugins1() QTEST_MAIN(tst_PluginManager) #include "tst_pluginmanager.moc" - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.cpp index 4343c1564..aa28c9726 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.cpp @@ -4,25 +4,25 @@ * @file testplugin.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,8 +34,7 @@ using namespace MyPlugin; MyPluginImpl::MyPluginImpl() : m_isInitialized(false), m_isExtensionsInitialized(false) -{ -} +{} bool MyPluginImpl::initialize(const QStringList &, QString *) { @@ -49,4 +48,3 @@ void MyPluginImpl::extensionsInitialized() } Q_EXPORT_PLUGIN(MyPluginImpl) - diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.h index b8824fd6b..1e1f48584 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin.h @@ -4,25 +4,25 @@ * @file testplugin.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include namespace MyPlugin { - -class MYPLUGIN_EXPORT MyPluginImpl : public ExtensionSystem::IPlugin -{ +class MYPLUGIN_EXPORT MyPluginImpl : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -46,13 +44,18 @@ public: bool initialize(const QStringList &arguments, QString *errorString); void extensionsInitialized(); - bool isInitialized() { return m_isInitialized; } - bool isExtensionsInitialized() { return m_isExtensionsInitialized; } + bool isInitialized() + { + return m_isInitialized; + } + bool isExtensionsInitialized() + { + return m_isExtensionsInitialized; + } private: bool m_isInitialized; bool m_isExtensionsInitialized; }; - } // namespace #endif // TESTPLUGIN_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin_global.h b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin_global.h index 08da14ba3..129dd0ecd 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin_global.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/testplugin/testplugin_global.h @@ -4,25 +4,25 @@ * @file testplugin_global.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/tst_pluginspec.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/tst_pluginspec.cpp index 0591217f0..6eed2fa67 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/tst_pluginspec.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/auto/pluginspec/tst_pluginspec.cpp @@ -4,25 +4,25 @@ * @file tst_pluginspec.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,8 +38,7 @@ using namespace ExtensionSystem; -class tst_PluginSpec : public QObject -{ +class tst_PluginSpec : public QObject { Q_OBJECT private slots: @@ -58,6 +57,7 @@ private slots: void tst_PluginSpec::read() { Internal::PluginSpecPrivate spec(0); + QCOMPARE(spec.state, PluginSpec::Invalid); QVERIFY(spec.read("testspecs/spec1.xml")); QCOMPARE(spec.state, PluginSpec::Read); @@ -72,10 +72,10 @@ void tst_PluginSpec::read() QCOMPARE(spec.description, QString("This plugin is just a test.\n it demonstrates the great use of the plugin spec.")); QCOMPARE(spec.url, QString("http://www.trolltech.com")); PluginDependency dep1; - dep1.name = QString("SomeOtherPlugin"); + dep1.name = QString("SomeOtherPlugin"); dep1.version = QString("2.3.0_2"); PluginDependency dep2; - dep2.name = QString("EvenOther"); + dep2.name = QString("EvenOther"); dep2.version = QString("1.0.0"); QCOMPARE(spec.dependencies, QList() << dep1 << dep2); @@ -88,6 +88,7 @@ void tst_PluginSpec::read() void tst_PluginSpec::readError() { Internal::PluginSpecPrivate spec(0); + QCOMPARE(spec.state, PluginSpec::Invalid); QVERIFY(!spec.read("non-existing-file.xml")); QCOMPARE(spec.state, PluginSpec::Invalid); @@ -155,6 +156,7 @@ void tst_PluginSpec::versionCompare() void tst_PluginSpec::provides() { Internal::PluginSpecPrivate spec(0); + QVERIFY(spec.read("testspecs/simplespec.xml")); QVERIFY(!spec.provides("SomeOtherPlugin", "2.2.3_9")); QVERIFY(!spec.provides("MyPlugin", "2.2.3_10")); @@ -180,15 +182,16 @@ void tst_PluginSpec::provides() void tst_PluginSpec::locationAndPath() { Internal::PluginSpecPrivate spec(0); + QVERIFY(spec.read("testspecs/simplespec.xml")); - QCOMPARE(spec.location, QDir::currentPath()+"/testspecs"); - QCOMPARE(spec.filePath, QDir::currentPath()+"/testspecs/simplespec.xml"); + QCOMPARE(spec.location, QDir::currentPath() + "/testspecs"); + QCOMPARE(spec.filePath, QDir::currentPath() + "/testspecs/simplespec.xml"); QVERIFY(spec.read("testdir/../testspecs/simplespec.xml")); - QCOMPARE(spec.location, QDir::currentPath()+"/testspecs"); - QCOMPARE(spec.filePath, QDir::currentPath()+"/testspecs/simplespec.xml"); + QCOMPARE(spec.location, QDir::currentPath() + "/testspecs"); + QCOMPARE(spec.filePath, QDir::currentPath() + "/testspecs/simplespec.xml"); QVERIFY(spec.read("testdir/spec.xml")); - QCOMPARE(spec.location, QDir::currentPath()+"/testdir"); - QCOMPARE(spec.filePath, QDir::currentPath()+"/testdir/spec.xml"); + QCOMPARE(spec.location, QDir::currentPath() + "/testdir"); + QCOMPARE(spec.filePath, QDir::currentPath() + "/testdir/spec.xml"); } void tst_PluginSpec::resolveDependencies() @@ -226,10 +229,11 @@ void tst_PluginSpec::loadLibrary() PluginSpec *ps = Internal::PluginManagerPrivate::createSpec(); Internal::PluginSpecPrivate *spec = Internal::PluginManagerPrivate::privateSpec(ps); PluginManager *manager = new PluginManager(); + QVERIFY(spec->read("testplugin/testplugin.xml")); QVERIFY(spec->resolveDependencies(QSet())); QVERIFY(spec->loadLibrary()); - QVERIFY(qobject_cast(spec->plugin) != 0); + QVERIFY(qobject_cast(spec->plugin) != 0); QCOMPARE(spec->state, PluginSpec::Loaded); QVERIFY(!spec->hasError); QCOMPARE(spec->plugin->pluginSpec(), ps); @@ -241,10 +245,11 @@ void tst_PluginSpec::initializePlugin() { Internal::PluginSpecPrivate spec(0); MyPlugin::MyPluginImpl *impl; + QVERIFY(spec.read("testplugin/testplugin.xml")); QVERIFY(spec.resolveDependencies(QSet())); QVERIFY(spec.loadLibrary()); - impl = qobject_cast(spec.plugin); + impl = qobject_cast(spec.plugin); QVERIFY(impl != 0); QVERIFY(!impl->isInitialized()); QVERIFY(spec.initializePlugin()); @@ -257,10 +262,11 @@ void tst_PluginSpec::initializeExtensions() { Internal::PluginSpecPrivate spec(0); MyPlugin::MyPluginImpl *impl; + QVERIFY(spec.read("testplugin/testplugin.xml")); QVERIFY(spec.resolveDependencies(QSet())); QVERIFY(spec.loadLibrary()); - impl = qobject_cast(spec.plugin); + impl = qobject_cast(spec.plugin); QVERIFY(impl != 0); QVERIFY(spec.initializePlugin()); QVERIFY(spec.initializeExtensions()); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.cpp index eb352547a..6894393b1 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.cpp @@ -4,25 +4,25 @@ * @file plugindialog.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,6 +43,7 @@ PluginDialog::PluginDialog(ExtensionSystem::PluginManager *manager) : m_view(new ExtensionSystem::PluginView(manager, this)) { QVBoxLayout *vl = new QVBoxLayout(this); + vl->setMargin(0); vl->setSpacing(0); vl->addWidget(m_view); @@ -61,10 +62,10 @@ PluginDialog::PluginDialog(ExtensionSystem::PluginManager *manager) resize(650, 300); setWindowTitle(tr("Installed Plugins")); - connect(m_view, SIGNAL(currentPluginChanged(ExtensionSystem::PluginSpec*)), - this, SLOT(updateButtons())); - connect(m_view, SIGNAL(pluginActivated(ExtensionSystem::PluginSpec*)), - this, SLOT(openDetails(ExtensionSystem::PluginSpec*))); + connect(m_view, SIGNAL(currentPluginChanged(ExtensionSystem::PluginSpec *)), + this, SLOT(updateButtons())); + connect(m_view, SIGNAL(pluginActivated(ExtensionSystem::PluginSpec *)), + this, SLOT(openDetails(ExtensionSystem::PluginSpec *))); connect(m_detailsButton, SIGNAL(clicked()), this, SLOT(openDetails())); connect(m_errorDetailsButton, SIGNAL(clicked()), this, SLOT(openErrorDetails())); } @@ -72,6 +73,7 @@ PluginDialog::PluginDialog(ExtensionSystem::PluginManager *manager) void PluginDialog::updateButtons() { ExtensionSystem::PluginSpec *selectedSpec = m_view->currentPlugin(); + if (selectedSpec) { m_detailsButton->setEnabled(true); m_errorDetailsButton->setEnabled(selectedSpec->hasError()); @@ -84,13 +86,14 @@ void PluginDialog::updateButtons() void PluginDialog::openDetails() { - openDetails(m_view->currentPlugin()); + openDetails(m_view->currentPlugin()); } void PluginDialog::openDetails(ExtensionSystem::PluginSpec *spec) { - if (!spec) + if (!spec) { return; + } QDialog dialog(this); dialog.setWindowTitle(tr("Plugin Details of %1").arg(spec->name())); QVBoxLayout *layout = new QVBoxLayout; @@ -109,8 +112,10 @@ void PluginDialog::openDetails(ExtensionSystem::PluginSpec *spec) void PluginDialog::openErrorDetails() { ExtensionSystem::PluginSpec *spec = m_view->currentPlugin(); - if (!spec) + + if (!spec) { return; + } QDialog dialog(this); dialog.setWindowTitle(tr("Plugin Errors of %1").arg(spec->name())); QVBoxLayout *layout = new QVBoxLayout; @@ -131,6 +136,7 @@ int main(int argc, char *argv[]) ExtensionSystem::PluginManager manager; QApplication app(argc, argv); PluginDialog dialog(&manager); + manager.setPluginPaths(QStringList() << "plugins"); manager.loadPlugins(); dialog.show(); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.h b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.h index 3ee89e5ae..77cf5acd3 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugindialog.h @@ -4,25 +4,25 @@ * @file plugindialog.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,8 +35,7 @@ #include #include -class PluginDialog : public QWidget -{ +class PluginDialog : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.cpp index 11c8b4ebd..f2932b60b 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.cpp @@ -4,25 +4,25 @@ * @file plugin1.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,8 +37,7 @@ using namespace Plugin1; MyPlugin1::MyPlugin1() : initializeCalled(false) -{ -} +{} bool MyPlugin1::initialize(const QStringList & /*arguments*/, QString *errorString) { @@ -49,20 +48,24 @@ bool MyPlugin1::initialize(const QStringList & /*arguments*/, QString *errorStri bool found2 = false; bool found3 = false; - foreach (QObject *object, ExtensionSystem::PluginManager::instance()->allObjects()) { - if (object->objectName() == "MyPlugin2") + foreach(QObject * object, ExtensionSystem::PluginManager::instance()->allObjects()) { + if (object->objectName() == "MyPlugin2") { found2 = true; - else if (object->objectName() == "MyPlugin3") + } else if (object->objectName() == "MyPlugin3") { found3 = true; + } } - if (found2 && found3) + if (found2 && found3) { return true; + } if (errorString) { QString error = "object(s) missing from plugin(s):"; - if (!found2) + if (!found2) { error.append(" plugin2"); - if (!found3) + } + if (!found3) { error.append(" plugin3"); + } *errorString = error; } return false; @@ -70,8 +73,9 @@ bool MyPlugin1::initialize(const QStringList & /*arguments*/, QString *errorStri void MyPlugin1::extensionsInitialized() { - if (!initializeCalled) + if (!initializeCalled) { return; + } // don't do this at home, it's just done here for the test QObject *obj = new QObject(this); obj->setObjectName("MyPlugin1_running"); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.h b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.h index 085cca8f8..4ad65894f 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin1/plugin1.h @@ -4,25 +4,25 @@ * @file plugin1.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include namespace Plugin1 { - -class MyPlugin1 : public ExtensionSystem::IPlugin -{ +class MyPlugin1 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -49,7 +47,6 @@ public: private: bool initializeCalled; }; - } // namespace Plugin1 #endif // PLUGIN1_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.cpp index 52c67cbf9..44409ad83 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.cpp @@ -4,25 +4,25 @@ * @file plugin2.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,8 +37,7 @@ using namespace Plugin2; MyPlugin2::MyPlugin2() : initializeCalled(false) -{ -} +{} bool MyPlugin2::initialize(const QStringList & /*arguments*/, QString *errorString) { @@ -53,8 +52,9 @@ bool MyPlugin2::initialize(const QStringList & /*arguments*/, QString *errorStri void MyPlugin2::extensionsInitialized() { - if (!initializeCalled) + if (!initializeCalled) { return; + } // don't do this at home, it's just done here for the test QObject *obj = new QObject(this); obj->setObjectName("MyPlugin2_running"); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.h b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.h index f524546a3..0a4d6f165 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin2/plugin2.h @@ -4,25 +4,25 @@ * @file plugin2.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include namespace Plugin2 { - -class MyPlugin2 : public ExtensionSystem::IPlugin -{ +class MyPlugin2 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -49,7 +47,6 @@ public: private: bool initializeCalled; }; - } // namespace Plugin2 #endif // PLUGIN2_H diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.cpp b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.cpp index 118b13198..a8b434de7 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.cpp @@ -4,25 +4,25 @@ * @file plugin3.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,8 +37,7 @@ using namespace Plugin3; MyPlugin3::MyPlugin3() : initializeCalled(false) -{ -} +{} bool MyPlugin3::initialize(const QStringList & /*arguments*/, QString *errorString) { @@ -48,21 +47,25 @@ bool MyPlugin3::initialize(const QStringList & /*arguments*/, QString *errorStri addAutoReleasedObject(obj); bool found2 = false; - foreach (QObject *object, ExtensionSystem::PluginManager::instance()->allObjects()) { - if (object->objectName() == "MyPlugin2") + foreach(QObject * object, ExtensionSystem::PluginManager::instance()->allObjects()) { + if (object->objectName() == "MyPlugin2") { found2 = true; + } } - if (found2) + if (found2) { return true; - if (errorString) + } + if (errorString) { *errorString = "object from plugin2 could not be found"; + } return false; } void MyPlugin3::extensionsInitialized() { - if (!initializeCalled) + if (!initializeCalled) { return; + } // don't do this at home, it's just done here for the test QObject *obj = new QObject(this); obj->setObjectName("MyPlugin3_running"); diff --git a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.h b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.h index cdc7f4df9..3fff58b4d 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/test/manual/pluginview/plugins/plugin3/plugin3.h @@ -4,25 +4,25 @@ * @file plugin3.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include namespace Plugin3 { - -class MyPlugin3 : public ExtensionSystem::IPlugin -{ +class MyPlugin3 : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -49,7 +47,6 @@ public: private: bool initializeCalled; }; - } // namespace Plugin3 #endif // PLUGIN3_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.h b/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.h index aae8fd5ff..f7a8361dd 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.h @@ -1,20 +1,17 @@ #ifndef OPMAP_CONTROL_H_ #define OPMAP_CONTROL_H_ #include "src/mapwidget/opmapwidget.h" -namespace mapcontrol -{ - struct customData - { - float velocity; - int mode; - float mode_params[4]; - int condition; - float condition_params[4]; - int command; - int jumpdestination; - int errordestination; - }; - +namespace mapcontrol { +struct customData { + float velocity; + int mode; + float mode_params[4]; + int condition; + float condition_params[4]; + int command; + int jumpdestination; + int errordestination; +}; } Q_DECLARE_METATYPE(mapcontrol::customData) #endif diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/accessmode.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/accessmode.h index a2022e653..673d14444 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/accessmode.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/accessmode.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file accessmode.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file accessmode.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef ACCESSMODE_H #define ACCESSMODE_H @@ -33,54 +33,52 @@ #include #include namespace core { - class AccessMode:public QObject - { - Q_OBJECT - Q_ENUMS(Types) - public: - enum Types - { - /// - /// access only server - /// - ServerOnly, +class AccessMode : public QObject { + Q_OBJECT Q_ENUMS(Types) +public: + enum Types { + /// + /// access only server + /// + ServerOnly, - /// - /// access first server and caches localy - /// - ServerAndCache, - - /// - /// access only cache - /// - CacheOnly - }; - static QString StrByType(Types const& value) - { - QMetaObject metaObject = AccessMode().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - QString s=metaEnum.valueToKey(value); - return s; - } - static Types TypeByStr(QString const& value) - { - QMetaObject metaObject = AccessMode().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - Types s=(Types)metaEnum.keyToValue(value.toLatin1()); - return s; - } - static QStringList TypesList() - { - QStringList ret; - QMetaObject metaObject = AccessMode().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - for(int x=0;x + /// access first server and caches localy + /// + ServerAndCache, + /// + /// access only cache + /// + CacheOnly }; + static QString StrByType(Types const & value) + { + QMetaObject metaObject = AccessMode().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + QString s = metaEnum.valueToKey(value); + + return s; + } + static Types TypeByStr(QString const & value) + { + QMetaObject metaObject = AccessMode().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + Types s = (Types)metaEnum.keyToValue(value.toLatin1()); + + return s; + } + static QStringList TypesList() + { + QStringList ret; + QMetaObject metaObject = AccessMode().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + + for (int x = 0; x < metaEnum.keyCount(); ++x) { + ret.append(metaEnum.key(x)); + } + return ret; + } +}; } #endif // ACCESSMODE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.cpp index a6946eb7d..ca615f94b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.cpp @@ -1,87 +1,82 @@ /** -****************************************************************************** -* -* @file alllayersoftype.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file alllayersoftype.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "alllayersoftype.h" namespace core { - AllLayersOfType::AllLayersOfType() +AllLayersOfType::AllLayersOfType() +{} +QVector AllLayersOfType::GetAllLayersOfType(const MapType::Types &type) +{ + QVector types; { - - } - QVector AllLayersOfType::GetAllLayersOfType(const MapType::Types &type) - { - QVector types; + switch (type) { + case MapType::GoogleHybrid: { - switch(type) - { - case MapType::GoogleHybrid: - { - - types.append(MapType::GoogleSatellite); - types.append(MapType::GoogleLabels); - } - break; - - case MapType::GoogleHybridChina: - { - types.append(MapType::GoogleSatelliteChina); - types.append(MapType::GoogleLabelsChina); - } - break; - - case MapType::GoogleHybridKorea: - { - types.append(MapType::GoogleSatelliteKorea); - types.append(MapType::GoogleLabelsKorea); - } - break; - - case MapType::YahooHybrid: - { - types.append(MapType::YahooSatellite); - types.append(MapType::YahooLabels); - } - break; - - case MapType::ArcGIS_MapsLT_Map_Hybrid: - { - types.append(MapType::ArcGIS_MapsLT_OrtoFoto); - types.append(MapType::ArcGIS_MapsLT_Map_Labels); - } - break; - - default: - { - types.append(type); - } - break; - } + types.append(MapType::GoogleSatellite); + types.append(MapType::GoogleLabels); } + break; - return types; + case MapType::GoogleHybridChina: + { + types.append(MapType::GoogleSatelliteChina); + types.append(MapType::GoogleLabelsChina); + } + break; + case MapType::GoogleHybridKorea: + { + types.append(MapType::GoogleSatelliteKorea); + types.append(MapType::GoogleLabelsKorea); + } + break; + + case MapType::YahooHybrid: + { + types.append(MapType::YahooSatellite); + types.append(MapType::YahooLabels); + } + break; + + case MapType::ArcGIS_MapsLT_Map_Hybrid: + { + types.append(MapType::ArcGIS_MapsLT_OrtoFoto); + types.append(MapType::ArcGIS_MapsLT_Map_Labels); + } + break; + + default: + { + types.append(type); + } + break; + } } + + return types; +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.h index 0c45bc7fe..16ba1b70e 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file alllayersoftype.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file alllayersoftype.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef ALLLAYERSOFTYPE_H #define ALLLAYERSOFTYPE_H @@ -32,12 +32,10 @@ #include namespace core { - class AllLayersOfType - { - public: - AllLayersOfType(); - QVector GetAllLayersOfType(const MapType::Types &type); - }; - +class AllLayersOfType { +public: + AllLayersOfType(); + QVector GetAllLayersOfType(const MapType::Types &type); +}; } #endif // ALLLAYERSOFTYPE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp index e4059af55..47704b63a 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.cpp @@ -1,197 +1,189 @@ /** -****************************************************************************** -* -* @file cache.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file cache.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "cache.h" #include "utils/pathutils.h" #include namespace core { - Cache* Cache::m_pInstance=0; +Cache *Cache::m_pInstance = 0; - Cache* Cache::Instance() - { - if(!m_pInstance) - m_pInstance=new Cache; - return m_pInstance; +Cache *Cache::Instance() +{ + if (!m_pInstance) { + m_pInstance = new Cache; } + return m_pInstance; +} - void Cache::setCacheLocation(const QString& value) - { - cache=value; - routeCache = cache + "RouteCache" + QDir::separator(); - geoCache = cache + "GeocoderCache"+ QDir::separator(); - placemarkCache = cache + "PlacemarkCache" + QDir::separator(); - ImageCache.setGtileCache(value); - } - QString Cache::CacheLocation() - { - return cache; - } - Cache::Cache() - { - if(cache.isNull()|cache.isEmpty()) - { - cache= Utils::PathUtils().GetStoragePath()+"mapscache"+QDir::separator(); - setCacheLocation(cache); - } - } - QString Cache::GetGeocoderFromCache(const QString &urlEnd) - { -#ifdef DEBUG_GetGeocoderFromCache - qDebug()<<"Entered GetGeocoderFromCache"; -#endif - QString ret=QString::null; - QString filename=geoCache+QString(urlEnd)+".geo"; -#ifdef DEBUG_GetGeocoderFromCache - qDebug()<<"GetGeocoderFromCache: Does file exist?:"<>ret; - } - } -#ifdef DEBUG_GetGeocoderFromCache - qDebug()<<"GetGeocoderFromCache:Returning:"<>ret; - } - } -#ifdef DEBUG_CACHE - qDebug()<<"GetPlacemarkFromCache:Returning:"<> ret; + } + } +#ifdef DEBUG_GetGeocoderFromCache + qDebug() << "GetGeocoderFromCache:Returning:" << ret; +#endif + return ret; +} +void Cache::CacheGeocoder(const QString &urlEnd, const QString &content) +{ + QString ret = QString::null; + QString filename = geoCache + QString(urlEnd) + ".geo"; + +#ifdef DEBUG_CACHE + qDebug() << "CacheGeocoder: Filename:" << filename; +#endif // DEBUG_CACHE + QFileInfo File(filename);; + QDir dir = File.absoluteDir(); + QString path = dir.absolutePath(); +#ifdef DEBUG_CACHE + qDebug() << "CacheGeocoder: Path:" << path; +#endif // DEBUG_CACHE + if (!dir.exists()) { +#ifdef DEBUG_CACHE + qDebug() << "CacheGeocoder: Cache path doesn't exist, try to create"; +#endif // DEBUG_CACHE + if (!dir.mkpath(path)) { +#ifdef DEBUG_CACHE + qDebug() << "GetGeocoderFromCache: Could not create path"; +#endif // DEBUG_CACHE + } + } +#ifdef DEBUG_CACHE + qDebug() << "CacheGeocoder: OpenFile:" << filename; +#endif // DEBUG_CACHE + QFile file(filename); + if (file.open(QIODevice::WriteOnly)) { +#ifdef DEBUG_CACHE + qDebug() << "CacheGeocoder: File Opened!!!:" << filename; +#endif // DEBUG_CACHE + QTextStream stream(&file); + stream.setCodec("UTF-8"); + stream << content; + } +} +QString Cache::GetPlacemarkFromCache(const QString &urlEnd) +{ +#ifdef DEBUG_CACHE + qDebug() << "Entered GetPlacemarkFromCache"; +#endif // DEBUG_CACHE + QString ret = QString::null; + QString filename = placemarkCache + QString(urlEnd) + ".plc"; +#ifdef DEBUG_CACHE + qDebug() << "GetPlacemarkFromCache: Does file exist?:" << filename; +#endif // DEBUG_CACHE + QFileInfo File(filename); + if (File.exists()) { +#ifdef DEBUG_CACHE + qDebug() << "GetPlacemarkFromCache:File exists!!"; +#endif // DEBUG_CACHE + QFile file(filename); + if (file.open(QIODevice::ReadOnly)) { + QTextStream stream(&file); + stream.setCodec("UTF-8"); + stream >> ret; + } + } +#ifdef DEBUG_CACHE + qDebug() << "GetPlacemarkFromCache:Returning:" << ret; +#endif // DEBUG_CACHE + return ret; +} +void Cache::CachePlacemark(const QString &urlEnd, const QString &content) +{ + QString ret = QString::null; + QString filename = placemarkCache + QString(urlEnd) + ".plc"; + +#ifdef DEBUG_CACHE + qDebug() << "CachePlacemark: Filename:" << filename; +#endif // DEBUG_CACHE + QFileInfo File(filename);; + QDir dir = File.absoluteDir(); + QString path = dir.absolutePath(); +#ifdef DEBUG_CACHE + qDebug() << "CachePlacemark: Path:" << path; +#endif // DEBUG_CACHE + if (!dir.exists()) { +#ifdef DEBUG_CACHE + qDebug() << "CachePlacemark: Cache path doesn't exist, try to create"; +#endif // DEBUG_CACHE + if (!dir.mkpath(path)) { +#ifdef DEBUG_CACHE + qDebug() << "CachePlacemark: Could not create path"; +#endif // DEBUG_CACHE + } + } +#ifdef DEBUG_CACHE + qDebug() << "CachePlacemark: OpenFile:" << filename; +#endif // DEBUG_CACHE + QFile file(filename); + if (file.open(QIODevice::WriteOnly)) { +#ifdef DEBUG_CACHE + qDebug() << "CachePlacemark: File Opened!!!:" << filename; +#endif // DEBUG_CACHE + QTextStream stream(&file); + stream.setCodec("UTF-8"); + stream << content; + } +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.h index 3cbddf34f..df4e42077 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cache.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file cache.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file cache.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef CACHE_H #define CACHE_H @@ -31,32 +31,33 @@ #include "debugheader.h" namespace core { - class Cache +class Cache { +public: + static Cache *Instance(); + + + PureImageCache ImageCache; + QString CacheLocation(); + void setCacheLocation(const QString & value); + void CacheGeocoder(const QString &urlEnd, const QString &content); + QString GetGeocoderFromCache(const QString &urlEnd); + void CachePlacemark(const QString &urlEnd, const QString &content); + QString GetPlacemarkFromCache(const QString &urlEnd); + void CacheRoute(const QString &urlEnd, const QString &content); + QString GetRouteFromCache(const QString &urlEnd); + +private: + Cache(); + Cache(Cache const &) {} + Cache & operator=(Cache const &) { - public: - static Cache* Instance(); - - - PureImageCache ImageCache; - QString CacheLocation(); - void setCacheLocation(const QString& value); - void CacheGeocoder(const QString &urlEnd,const QString &content); - QString GetGeocoderFromCache(const QString &urlEnd); - void CachePlacemark(const QString &urlEnd,const QString &content); - QString GetPlacemarkFromCache(const QString &urlEnd); - void CacheRoute(const QString &urlEnd,const QString &content); - QString GetRouteFromCache(const QString &urlEnd); - - private: - Cache(); - Cache(Cache const&){} - Cache& operator=(Cache const&){ return *this; } - static Cache* m_pInstance; - QString cache; - QString routeCache; - QString geoCache; - QString placemarkCache; - }; - + return *this; + } + static Cache *m_pInstance; + QString cache; + QString routeCache; + QString geoCache; + QString placemarkCache; +}; } #endif // CACHE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.cpp index 7d1f1d661..59cec8169 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.cpp @@ -1,79 +1,79 @@ /** -****************************************************************************** -* -* @file cacheitemqueue.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file cacheitemqueue.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "cacheitemqueue.h" namespace core { - CacheItemQueue::CacheItemQueue(const MapType::Types &Type, const Point &Pos, const QByteArray &Img, const int &Zoom) - { - type=Type; - pos=Pos; - img=Img; - zoom=Zoom; - - } - - QByteArray CacheItemQueue::GetImg() - { - return img; - } - - MapType::Types CacheItemQueue::GetMapType() - { - return type; - } - Point CacheItemQueue::GetPosition() - { - return pos; - } - void CacheItemQueue::SetImg(const QByteArray &value) - { - img=value; - } - void CacheItemQueue::SetMapType(const MapType::Types &value) - { - type=value; - } - void CacheItemQueue::SetPosition(const Point &value) - { - pos=value; - } - - CacheItemQueue& CacheItemQueue::operator =(const CacheItemQueue &cSource) - { - img=cSource.img; - pos=cSource.pos; - type=cSource.type; - zoom=cSource.zoom; - return *this; - } - bool CacheItemQueue::operator ==(const CacheItemQueue &cSource) - { - bool b=(img==cSource.img)&& (pos==cSource.pos) && (type==cSource.type) && (zoom==cSource.zoom); - return b; - } +CacheItemQueue::CacheItemQueue(const MapType::Types &Type, const Point &Pos, const QByteArray &Img, const int &Zoom) +{ + type = Type; + pos = Pos; + img = Img; + zoom = Zoom; +} + +QByteArray CacheItemQueue::GetImg() +{ + return img; +} + +MapType::Types CacheItemQueue::GetMapType() +{ + return type; +} +Point CacheItemQueue::GetPosition() +{ + return pos; +} +void CacheItemQueue::SetImg(const QByteArray &value) +{ + img = value; +} +void CacheItemQueue::SetMapType(const MapType::Types &value) +{ + type = value; +} +void CacheItemQueue::SetPosition(const Point &value) +{ + pos = value; +} + +CacheItemQueue & CacheItemQueue::operator =(const CacheItemQueue &cSource) +{ + img = cSource.img; + pos = cSource.pos; + type = cSource.type; + zoom = cSource.zoom; + return *this; +} +bool CacheItemQueue::operator ==(const CacheItemQueue &cSource) +{ + bool b = (img == cSource.img) && (pos == cSource.pos) && (type == cSource.type) && (zoom == cSource.zoom); + + return b; +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.h index 786fb5d5c..1d610faee 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/cacheitemqueue.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file cacheitemqueue.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file cacheitemqueue.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef CACHEITEMQUEUE_H #define CACHEITEMQUEUE_H @@ -32,38 +32,41 @@ #include - namespace core { - class CacheItemQueue +class CacheItemQueue { +public: + CacheItemQueue(const MapType::Types &Type, const core::Point &Pos, const QByteArray &Img, const int &Zoom); + CacheItemQueue() {}; + CacheItemQueue(const CacheItemQueue &cSource) { - public: - CacheItemQueue(const MapType::Types &Type,const core::Point &Pos,const QByteArray &Img,const int &Zoom); - CacheItemQueue(){}; - CacheItemQueue(const CacheItemQueue &cSource) - { - img=cSource.img; - pos=cSource.pos; - type=cSource.type; - zoom=cSource.zoom; - } - CacheItemQueue& operator= (const CacheItemQueue &cSource); - bool operator== (const CacheItemQueue &cSource); - void SetMapType(const MapType::Types &value); - void SetPosition(const core::Point &value); - void SetImg(const QByteArray &value); - MapType::Types GetMapType(); - core::Point GetPosition(); - QByteArray GetImg(); - int GetZoom(){return zoom;}; - void SetZoom(const int &value) {zoom=value;}; - private: - - - MapType::Types type; - core::Point pos; - QByteArray img; - int zoom; + img = cSource.img; + pos = cSource.pos; + type = cSource.type; + zoom = cSource.zoom; + } + CacheItemQueue & operator=(const CacheItemQueue &cSource); + bool operator==(const CacheItemQueue &cSource); + void SetMapType(const MapType::Types &value); + void SetPosition(const core::Point &value); + void SetImg(const QByteArray &value); + MapType::Types GetMapType(); + core::Point GetPosition(); + QByteArray GetImg(); + int GetZoom() + { + return zoom; }; + void SetZoom(const int &value) + { + zoom = value; + }; +private: + + MapType::Types type; + core::Point pos; + QByteArray img; + int zoom; +}; } #endif // CACHEITEMQUEUE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/debugheader.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/debugheader.h index 9b410c9c2..d4590fb59 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/debugheader.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/debugheader.h @@ -1,13 +1,13 @@ #ifndef DEBUGHEADER_H #define DEBUGHEADER_H -//#define DEBUG_MEMORY_CACHE -//#define DEBUG_CACHE -//#define DEBUG_GMAPS -//#define DEBUG_PUREIMAGECACHE -//#define DEBUG_TILECACHEQUEUE -//#define DEBUG_URLFACTORY -//#define DEBUG_MEMORY_CACHE -//#define DEBUG_GetGeocoderFromCache +// #define DEBUG_MEMORY_CACHE +// #define DEBUG_CACHE +// #define DEBUG_GMAPS +// #define DEBUG_PUREIMAGECACHE +// #define DEBUG_TILECACHEQUEUE +// #define DEBUG_URLFACTORY +// #define DEBUG_MEMORY_CACHE +// #define DEBUG_GetGeocoderFromCache #endif // DEBUGHEADER_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.cpp index ea2242651..fd4554c7d 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.cpp @@ -1,31 +1,30 @@ /** -****************************************************************************** -* -* @file diagnostics.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file diagnostics.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "diagnostics.h" -diagnostics::diagnostics():networkerrors(0),emptytiles(0),timeouts(0),runningThreads(0),tilesFromMem(0),tilesFromNet(0),tilesFromDB(0) -{ -} +diagnostics::diagnostics() : networkerrors(0), emptytiles(0), timeouts(0), runningThreads(0), tilesFromMem(0), tilesFromNet(0), tilesFromDB(0) +{} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.h index f9c503b97..bad0af01f 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/diagnostics.h @@ -1,46 +1,46 @@ /** -****************************************************************************** -* -* @file diagnostics.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file diagnostics.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef DIAGNOSTICS_H #define DIAGNOSTICS_H #include -struct diagnostics -{ +struct diagnostics { diagnostics(); - int networkerrors; - int emptytiles; - int timeouts; - int runningThreads; - int tilesFromMem; - int tilesFromNet; - int tilesFromDB; + int networkerrors; + int emptytiles; + int timeouts; + int runningThreads; + int tilesFromMem; + int tilesFromNet; + int tilesFromDB; QString toString() { return QString("Network errors:%1\nEmpty Tiles:%2\nTimeOuts:%3\nRunningThreads:%4\nTilesFromMem:%5\nTilesFromNet:%6\nTilesFromDB:%7").arg(networkerrors).arg(emptytiles).arg(timeouts).arg(runningThreads).arg(tilesFromMem).arg(tilesFromNet).arg(tilesFromDB); - ; + + ; } }; diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/geodecoderstatus.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/geodecoderstatus.h index 66fe1e56e..86930ab5b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/geodecoderstatus.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/geodecoderstatus.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file geodecoderstatus.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file geodecoderstatus.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef GEODECODERSTATUS_H #define GEODECODERSTATUS_H @@ -32,100 +32,97 @@ #include #include namespace core { - class GeoCoderStatusCode:public QObject - { - Q_OBJECT - Q_ENUMS(Types) - public: - enum Types - { - /// - /// unknow response - /// - Unknow = -1, +class GeoCoderStatusCode : public QObject { + Q_OBJECT Q_ENUMS(Types) +public: + enum Types { + /// + /// unknow response + /// + Unknow = -1, - /// - /// No errors occurred; the address was successfully parsed and its geocode has been returned. - /// - G_GEO_SUCCESS=200, + /// + /// No errors occurred; the address was successfully parsed and its geocode has been returned. + /// + G_GEO_SUCCESS = 200, - /// - /// A directions request could not be successfully parsed. - /// For example, the request may have been rejected if it contained more than the maximum number of waypoints allowed. - /// - G_GEO_BAD_REQUEST=400, + /// + /// A directions request could not be successfully parsed. + /// For example, the request may have been rejected if it contained more than the maximum number of waypoints allowed. + /// + G_GEO_BAD_REQUEST = 400, - /// - /// A geocoding or directions request could not be successfully processed, yet the exact reason for the failure is not known. - /// - G_GEO_SERVER_ERROR=500, + /// + /// A geocoding or directions request could not be successfully processed, yet the exact reason for the failure is not known. + /// + G_GEO_SERVER_ERROR = 500, - /// - /// The HTTP q parameter was either missing or had no value. - /// For geocoding requests, this means that an empty address was specified as input. For directions requests, this means that no query was specified in the input. - /// - G_GEO_MISSING_QUERY=601, + /// + /// The HTTP q parameter was either missing or had no value. + /// For geocoding requests, this means that an empty address was specified as input. For directions requests, this means that no query was specified in the input. + /// + G_GEO_MISSING_QUERY = 601, - /// - /// Synonym for G_GEO_MISSING_QUERY. - /// - G_GEO_MISSING_ADDRESS=601, + /// + /// Synonym for G_GEO_MISSING_QUERY. + /// + G_GEO_MISSING_ADDRESS = 601, - /// - /// No corresponding geographic location could be found for the specified address. - /// This may be due to the fact that the address is relatively new, or it may be incorrect. - /// - G_GEO_UNKNOWN_ADDRESS=602, + /// + /// No corresponding geographic location could be found for the specified address. + /// This may be due to the fact that the address is relatively new, or it may be incorrect. + /// + G_GEO_UNKNOWN_ADDRESS = 602, - /// - /// The geocode for the given address or the route for the given directions query cannot be returned due to legal or contractual reasons. - /// - G_GEO_UNAVAILABLE_ADDRESS=603, + /// + /// The geocode for the given address or the route for the given directions query cannot be returned due to legal or contractual reasons. + /// + G_GEO_UNAVAILABLE_ADDRESS = 603, - /// - /// The GDirections object could not compute directions between the points mentioned in the query. - /// This is usually because there is no route available between the two points, or because we do not have data for routing in that region. - /// - G_GEO_UNKNOWN_DIRECTIONS=604, + /// + /// The GDirections object could not compute directions between the points mentioned in the query. + /// This is usually because there is no route available between the two points, or because we do not have data for routing in that region. + /// + G_GEO_UNKNOWN_DIRECTIONS = 604, - /// - /// The given key is either invalid or does not match the domain for which it was given. - /// - G_GEO_BAD_KEY=610, - - /// - /// The given key has gone over the requests limit in the 24 hour period or has submitted too many requests in too short a period of time. - /// If you're sending multiple requests in parallel or in a tight loop, use a timer or pause in your code to make sure you don't send the requests too quickly. - /// - G_GEO_TOO_MANY_QUERIES=620 - - }; - static QString StrByType(Types const& value) - { - QMetaObject metaObject = GeoCoderStatusCode().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - QString s=metaEnum.valueToKey(value); - return s; - } - static Types TypeByStr(QString const& value) - { - QMetaObject metaObject = GeoCoderStatusCode().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - Types s=(Types)metaEnum.keyToValue(value.toLatin1()); - return s; - } - static QStringList TypesList() - { - QStringList ret; - QMetaObject metaObject = GeoCoderStatusCode().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - for(int x=0;x + /// The given key is either invalid or does not match the domain for which it was given. + /// + G_GEO_BAD_KEY = 610, + /// + /// The given key has gone over the requests limit in the 24 hour period or has submitted too many requests in too short a period of time. + /// If you're sending multiple requests in parallel or in a tight loop, use a timer or pause in your code to make sure you don't send the requests too quickly. + /// + G_GEO_TOO_MANY_QUERIES = 620 }; + static QString StrByType(Types const & value) + { + QMetaObject metaObject = GeoCoderStatusCode().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + QString s = metaEnum.valueToKey(value); + + return s; + } + static Types TypeByStr(QString const & value) + { + QMetaObject metaObject = GeoCoderStatusCode().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + Types s = (Types)metaEnum.keyToValue(value.toLatin1()); + + return s; + } + static QStringList TypesList() + { + QStringList ret; + QMetaObject metaObject = GeoCoderStatusCode().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + + for (int x = 0; x < metaEnum.keyCount(); ++x) { + ret.append(metaEnum.key(x)); + } + return ret; + } +}; } #endif // GEODECODERSTATUS_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.cpp index b674f4f46..235ba02d1 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.cpp @@ -1,69 +1,68 @@ /** -****************************************************************************** -* -* @file kibertilecache.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file kibertilecache.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "kibertilecache.h" -//TODO add readwrite lock +// TODO add readwrite lock namespace core { - KiberTileCache::KiberTileCache() - { - memoryCacheSize = 0; - _MemoryCacheCapacity = 22; - } - - void KiberTileCache::setMemoryCacheCapacity(const int &value) - { - kiberCacheLock.lockForWrite(); - _MemoryCacheCapacity=value; - kiberCacheLock.unlock(); - } - int KiberTileCache::MemoryCacheCapacity() - { - kiberCacheLock.lockForRead(); - return _MemoryCacheCapacity; - kiberCacheLock.unlock(); - } - - void KiberTileCache::RemoveMemoryOverload() - { - while(MemoryCacheSize()>MemoryCacheCapacity()) - { - if(cachequeue.count()>0 && list.count()>0) - { -#ifdef DEBUG_MEMORY_CACHE - qDebug()<<"Cleaning Memory cache="<<" started with "< MemoryCacheCapacity()) { + if (cachequeue.count() > 0 && list.count() > 0) { +#ifdef DEBUG_MEMORY_CACHE + qDebug() << "Cleaning Memory cache=" << " started with " << cachequeue.count() << " tile " << "ocupying " << memoryCacheSize << " bytes"; +#endif + RawTile first = list.dequeue(); + memoryCacheSize -= cachequeue.value(first).size(); + cachequeue.remove(first); + } + } +#ifdef DEBUG_MEMORY_CACHE + qDebug() << "Cleaning Memory cache=" << " ended with " << cachequeue.count() << " tile " << "ocupying " << memoryCacheSize << " bytes"; +#endif +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.h index 5fe2292de..7102d112a 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/kibertilecache.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file kibertilecache.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file kibertilecache.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef KIBERTILECACHE_H #define KIBERTILECACHE_H @@ -34,25 +34,23 @@ #include #include "debugheader.h" namespace core { - class KiberTileCache +class KiberTileCache { +public: + KiberTileCache(); + + void setMemoryCacheCapacity(const int &value); + int MemoryCacheCapacity(); + double MemoryCacheSize() { - public: - KiberTileCache(); - - void setMemoryCacheCapacity(const int &value); - int MemoryCacheCapacity(); - double MemoryCacheSize(){return memoryCacheSize/1048576.0;} - void RemoveMemoryOverload(); - QReadWriteLock kiberCacheLock; - QHash cachequeue; - QQueue list; - long memoryCacheSize; - private: - int _MemoryCacheCapacity; - - }; - - - + return memoryCacheSize / 1048576.0; + } + void RemoveMemoryOverload(); + QReadWriteLock kiberCacheLock; + QHash cachequeue; + QQueue list; + long memoryCacheSize; +private: + int _MemoryCacheCapacity; +}; } #endif // KIBERTILECACHE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.cpp index 52da98b80..a9856b983 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.cpp @@ -1,100 +1,97 @@ /** -****************************************************************************** -* -* @file languagetype.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file languagetype.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "languagetype.h" - namespace core { - LanguageType::LanguageType() - { - list - <<"ar" - <<"bg" - <<"bn" - <<"ca" - <<"cs" - <<"da" - <<"de" - <<"el" - <<"en" - <<"en-AU" - <<"en-GB" - <<"es" - <<"eu" - <<"fi" - <<"fil" - <<"fr" - <<"gl" - <<"gu" - <<"hi" - <<"hr" - <<"hu" - <<"id" - <<"it" - <<"iw" - <<"ja" - <<"kn" - <<"ko" - <<"lt" - <<"lv" - <<"ml" - <<"mr" - <<"nl" - <<"nn" - <<"no" - <<"or" - <<"pl" - <<"pt" - <<"pt-BR" - <<"pt-PT" - <<"rm" - <<"ro" - <<"ru" - <<"sk" - <<"sl" - <<"sr" - <<"sv" - <<"ta" - <<"te" - <<"th" - <<"tr" - <<"uk" - <<"vi" - <<"zh-CN" - <<"zh-TW"; - - } - QString LanguageType::toShortString(Types type) - { - return list[type]; - } - LanguageType::~LanguageType() - { - list.clear(); - } - +LanguageType::LanguageType() +{ + list + << "ar" + << "bg" + << "bn" + << "ca" + << "cs" + << "da" + << "de" + << "el" + << "en" + << "en-AU" + << "en-GB" + << "es" + << "eu" + << "fi" + << "fil" + << "fr" + << "gl" + << "gu" + << "hi" + << "hr" + << "hu" + << "id" + << "it" + << "iw" + << "ja" + << "kn" + << "ko" + << "lt" + << "lv" + << "ml" + << "mr" + << "nl" + << "nn" + << "no" + << "or" + << "pl" + << "pt" + << "pt-BR" + << "pt-PT" + << "rm" + << "ro" + << "ru" + << "sk" + << "sl" + << "sr" + << "sv" + << "ta" + << "te" + << "th" + << "tr" + << "uk" + << "vi" + << "zh-CN" + << "zh-TW"; +} +QString LanguageType::toShortString(Types type) +{ + return list[type]; +} +LanguageType::~LanguageType() +{ + list.clear(); +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.h index 17cfe3b67..1a6a963e1 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/languagetype.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file languagetype.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file languagetype.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef LANGUAGETYPE_H #define LANGUAGETYPE_H @@ -34,100 +34,98 @@ namespace core { - class LanguageType:public QObject - { - Q_OBJECT - Q_ENUMS(Types) - public: - enum Types - { - Arabic, - Bulgarian, - Bengali, - Catalan, - Czech, - Danish, - German, - Greek, - English, - EnglishAustralian, - EnglishGreatBritain, - Spanish, - Basque, - Finnish, - Filipino, - French, - Galician, - Gujarati, - Hindi, - Croatian, - Hungarian, - Indonesian, - Italian, - Hebrew, - Japanese, - Kannada, - Korean, - Lithuanian, - Latvian, - Malayalam, - Marathi, - Dutch, - NorwegianNynorsk, - Norwegian, - Oriya, - Polish, - Portuguese, - PortugueseBrazil, - PortuguesePortugal, - Romansch, - Romanian, - Russian, - Slovak, - Slovenian, - Serbian, - Swedish, - Tamil, - Telugu, - Thai, - Turkish, - Ukrainian, - Vietnamese, - ChineseSimplified, - ChineseTraditional - }; - - static QString StrByType(Types const& value) - { - QMetaObject metaObject = LanguageType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - QString s=metaEnum.valueToKey(value); - return s; - } - static Types TypeByStr(QString const& value) - { - QMetaObject metaObject = LanguageType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - Types s=(Types)metaEnum.keyToValue(value.toLatin1()); - return s; - } - static QStringList TypesList() - { - QStringList ret; - QMetaObject metaObject = LanguageType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - for(int x=0;x @@ -31,99 +31,97 @@ #include namespace core { - class MapType:public QObject - { - Q_OBJECT - Q_ENUMS(Types) - public: - enum Types - { - GoogleMap=1, - GoogleSatellite=4, - GoogleLabels=8, - GoogleTerrain=16, - GoogleHybrid=20, +class MapType : public QObject { + Q_OBJECT Q_ENUMS(Types) +public: + enum Types { + GoogleMap = 1, + GoogleSatellite = 4, + GoogleLabels = 8, + GoogleTerrain = 16, + GoogleHybrid = 20, - GoogleMapChina=22, - GoogleSatelliteChina=24, - GoogleLabelsChina=26, - GoogleTerrainChina=28, - GoogleHybridChina=29, + GoogleMapChina = 22, + GoogleSatelliteChina = 24, + GoogleLabelsChina = 26, + GoogleTerrainChina = 28, + GoogleHybridChina = 29, - OpenStreetMap=32, - OpenStreetOsm=33, - OpenStreetMapSurfer=34, - OpenStreetMapSurferTerrain=35, + OpenStreetMap = 32, + OpenStreetOsm = 33, + OpenStreetMapSurfer = 34, + OpenStreetMapSurferTerrain = 35, - YahooMap=64, - YahooSatellite=128, - YahooLabels=256, - YahooHybrid=333, + YahooMap = 64, + YahooSatellite = 128, + YahooLabels = 256, + YahooHybrid = 333, - BingMap=444, - BingSatellite=555, - BingHybrid=666, + BingMap = 444, + BingSatellite = 555, + BingHybrid = 666, - ArcGIS_Map=777, - ArcGIS_Satellite=788, - ArcGIS_ShadedRelief=799, - ArcGIS_Terrain=811, + ArcGIS_Map = 777, + ArcGIS_Satellite = 788, + ArcGIS_ShadedRelief = 799, + ArcGIS_Terrain = 811, - // use these numbers to clean up old stuff - //ArcGIS_MapsLT_Map_Old= 877, - //ArcGIS_MapsLT_OrtoFoto_Old = 888, - //ArcGIS_MapsLT_Map_Labels_Old = 890, - //ArcGIS_MapsLT_Map_Hybrid_Old = 899, - //ArcGIS_MapsLT_Map=977, - //ArcGIS_MapsLT_OrtoFoto=988, - //ArcGIS_MapsLT_Map_Labels=990, - //ArcGIS_MapsLT_Map_Hybrid=999, - //ArcGIS_MapsLT_Map=978, - //ArcGIS_MapsLT_OrtoFoto=989, - //ArcGIS_MapsLT_Map_Labels=991, - //ArcGIS_MapsLT_Map_Hybrid=998, + // use these numbers to clean up old stuff + // ArcGIS_MapsLT_Map_Old= 877, + // ArcGIS_MapsLT_OrtoFoto_Old = 888, + // ArcGIS_MapsLT_Map_Labels_Old = 890, + // ArcGIS_MapsLT_Map_Hybrid_Old = 899, + // ArcGIS_MapsLT_Map=977, + // ArcGIS_MapsLT_OrtoFoto=988, + // ArcGIS_MapsLT_Map_Labels=990, + // ArcGIS_MapsLT_Map_Hybrid=999, + // ArcGIS_MapsLT_Map=978, + // ArcGIS_MapsLT_OrtoFoto=989, + // ArcGIS_MapsLT_Map_Labels=991, + // ArcGIS_MapsLT_Map_Hybrid=998, - ArcGIS_MapsLT_Map=1000, - ArcGIS_MapsLT_OrtoFoto=1001, - ArcGIS_MapsLT_Map_Labels=1002, - ArcGIS_MapsLT_Map_Hybrid=1003, + ArcGIS_MapsLT_Map = 1000, + ArcGIS_MapsLT_OrtoFoto = 1001, + ArcGIS_MapsLT_Map_Labels = 1002, + ArcGIS_MapsLT_Map_Hybrid = 1003, - PergoTurkeyMap = 2001, - SigPacSpainMap = 3001, + PergoTurkeyMap = 2001, + SigPacSpainMap = 3001, - GoogleMapKorea=4001, - GoogleSatelliteKorea=4002, - GoogleLabelsKorea=4003, - GoogleHybridKorea=4005, + GoogleMapKorea = 4001, + GoogleSatelliteKorea = 4002, + GoogleLabelsKorea = 4003, + GoogleHybridKorea = 4005, - YandexMapRu = 5000 - }; - static QString StrByType(Types const& value) - { - QMetaObject metaObject = MapType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - QString s=metaEnum.valueToKey(value); - return s; - } - static Types TypeByStr(QString const& value) - { - QMetaObject metaObject = MapType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - Types s=(Types)metaEnum.keyToValue(value.toLatin1()); - return s; - } - static QStringList TypesList() - { - QStringList ret; - QMetaObject metaObject = MapType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - for(int x=0;x #include "debugheader.h" namespace core { - class MemoryCache - { - public: - MemoryCache(); - - KiberTileCache TilesInMemory; - QByteArray GetTileFromMemoryCache(const RawTile &tile); - void AddTileToMemoryCache(const RawTile &tile, const QByteArray &pic); - QReadWriteLock kiberCacheLock; - }; - +class MemoryCache { +public: + MemoryCache(); + KiberTileCache TilesInMemory; + QByteArray GetTileFromMemoryCache(const RawTile &tile); + void AddTileToMemoryCache(const RawTile &tile, const QByteArray &pic); + QReadWriteLock kiberCacheLock; +}; } #endif // MEMORYCACHE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp index 86660fd3e..fd249f66d 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp @@ -1,286 +1,270 @@ /** -****************************************************************************** -* -* @file OPMaps.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file OPMaps.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "opmaps.h" namespace core { - OPMaps* OPMaps::m_pInstance=0; +OPMaps *OPMaps::m_pInstance = 0; - OPMaps* OPMaps::Instance() - { - if(!m_pInstance) - m_pInstance=new OPMaps; - return m_pInstance; - } - OPMaps::OPMaps():RetryLoadTile(2),useMemoryCache(true) - { - accessmode=AccessMode::ServerAndCache; - Language=LanguageType::PortuguesePortugal; - LanguageStr=LanguageType().toShortString(Language); - Cache::Instance(); - - } - - - OPMaps::~OPMaps() - { - TileDBcacheQueue.wait(); - } - - - - QByteArray OPMaps::GetImageFrom(const MapType::Types &type,const Point &pos,const int &zoom) - { -#ifdef DEBUG_TIMINGS - QTime time; - time.restart(); -#endif -#ifdef DEBUG_GMAPS - qDebug()<<"Entered GetImageFrom"; -#endif //DEBUG_GMAPS - QByteArray ret; - - if(useMemoryCache) - { -#ifdef DEBUG_GMAPS - qDebug()<<"Try Tile from memory:Size="<ImageCache.GetImageFromCache(type,pos,zoom); - if(!ret.isEmpty()) - { - errorvars.lock(); - ++diag.tilesFromDB; - errorvars.unlock(); -#ifdef DEBUG_GMAPS - qDebug()<<"Tile found in Database"; -#endif //DEBUG_GMAPS - if(useMemoryCache) - { -#ifdef DEBUG_GMAPS - qDebug()<<"Add Tile to memory"; -#endif //DEBUG_GMAPS - AddTileToMemoryCache(RawTile(type,pos,zoom),ret); - } - return ret; - } - } - if(accessmode!=AccessMode::CacheOnly) - { - QEventLoop q; - QNetworkReply *reply; - QNetworkRequest qheader; - QNetworkAccessManager network; - QTimer tT; - tT.setSingleShot(true); - connect(&network, SIGNAL(finished(QNetworkReply*)), - &q, SLOT(quit())); - connect(&tT, SIGNAL(timeout()), &q, SLOT(quit())); - network.setProxy(Proxy); -#ifdef DEBUG_GMAPS - qDebug()<<"Try Tile from the Internet"; -#endif //DEBUG_GMAPS -#ifdef DEBUG_TIMINGS - qDebug()<<"opmaps before make image url"<error()!=QNetworkReply::NoError)) - { - errorvars.lock(); - ++diag.networkerrors; - errorvars.unlock(); - reply->deleteLater(); - return ret; - } - ret=reply->readAll(); - reply->deleteLater();//TODO can't this be global?? - if(ret.isEmpty()) - { -#ifdef DEBUG_GMAPS - qDebug()<<"Invalid Tile"; -#endif //DEBUG_GMAPS - errorvars.lock(); - ++diag.emptytiles; - errorvars.unlock(); - return ret; - } -#ifdef DEBUG_GMAPS - qDebug()<<"Received Tile from the Internet"; -#endif //DEBUG_GMAPS - errorvars.lock(); - ++diag.tilesFromNet; - errorvars.unlock(); - if (useMemoryCache) - { -#ifdef DEBUG_GMAPS - qDebug()<<"Add Tile to memory cache"; -#endif //DEBUG_GMAPS - AddTileToMemoryCache(RawTile(type,pos,zoom),ret); - } - if(accessmode!=AccessMode::ServerOnly) - { -#ifdef DEBUG_GMAPS - qDebug()<<"Add tile to DataBase"; -#endif //DEBUG_GMAPS - CacheItemQueue * item=new CacheItemQueue(type,pos,ret,zoom); - TileDBcacheQueue.EnqueueCacheTask(item); - } - - - } - } -#ifdef DEBUG_GMAPS - qDebug()<<"Entered GetImageFrom"; -#endif //DEBUG_GMAPS - return ret; - } - - bool OPMaps::ExportToGMDB(const QString &file) - { - return Cache::Instance()->ImageCache.ExportMapDataToDB(Cache::Instance()->ImageCache.GtileCache()+QDir::separator()+"Data.qmdb",file); - } - bool OPMaps::ImportFromGMDB(const QString &file) - { - return Cache::Instance()->ImageCache.ExportMapDataToDB(file,Cache::Instance()->ImageCache.GtileCache()+QDir::separator()+"Data.qmdb"); - } - - diagnostics OPMaps::GetDiagnostics() - { - diagnostics i; - errorvars.lock(); - i=diag; - errorvars.unlock(); - return i; +OPMaps *OPMaps::Instance() +{ + if (!m_pInstance) { + m_pInstance = new OPMaps; } + return m_pInstance; +} +OPMaps::OPMaps() : RetryLoadTile(2), useMemoryCache(true) +{ + accessmode = AccessMode::ServerAndCache; + Language = LanguageType::PortuguesePortugal; + LanguageStr = LanguageType().toShortString(Language); + Cache::Instance(); } + +OPMaps::~OPMaps() +{ + TileDBcacheQueue.wait(); +} + + +QByteArray OPMaps::GetImageFrom(const MapType::Types &type, const Point &pos, const int &zoom) +{ +#ifdef DEBUG_TIMINGS + QTime time; + time.restart(); +#endif +#ifdef DEBUG_GMAPS + qDebug() << "Entered GetImageFrom"; +#endif // DEBUG_GMAPS + QByteArray ret; + + if (useMemoryCache) { +#ifdef DEBUG_GMAPS + qDebug() << "Try Tile from memory:Size=" << TilesInMemory.MemoryCacheSize(); +#endif // DEBUG_GMAPS + ret = GetTileFromMemoryCache(RawTile(type, pos, zoom)); + if (!ret.isEmpty()) { + errorvars.lock(); + ++diag.tilesFromMem; + errorvars.unlock(); + } + } + if (ret.isEmpty()) { +#ifdef DEBUG_GMAPS + qDebug() << "Tile not in memory"; +#endif // DEBUG_GMAPS + if (accessmode != (AccessMode::ServerOnly)) { +#ifdef DEBUG_GMAPS + qDebug() << "Try tile from DataBase"; +#endif // DEBUG_GMAPS + ret = Cache::Instance()->ImageCache.GetImageFromCache(type, pos, zoom); + if (!ret.isEmpty()) { + errorvars.lock(); + ++diag.tilesFromDB; + errorvars.unlock(); +#ifdef DEBUG_GMAPS + qDebug() << "Tile found in Database"; +#endif // DEBUG_GMAPS + if (useMemoryCache) { +#ifdef DEBUG_GMAPS + qDebug() << "Add Tile to memory"; +#endif // DEBUG_GMAPS + AddTileToMemoryCache(RawTile(type, pos, zoom), ret); + } + return ret; + } + } + if (accessmode != AccessMode::CacheOnly) { + QEventLoop q; + QNetworkReply *reply; + QNetworkRequest qheader; + QNetworkAccessManager network; + QTimer tT; + tT.setSingleShot(true); + connect(&network, SIGNAL(finished(QNetworkReply *)), + &q, SLOT(quit())); + connect(&tT, SIGNAL(timeout()), &q, SLOT(quit())); + network.setProxy(Proxy); +#ifdef DEBUG_GMAPS + qDebug() << "Try Tile from the Internet"; +#endif // DEBUG_GMAPS +#ifdef DEBUG_TIMINGS + qDebug() << "opmaps before make image url" << time.elapsed(); +#endif + QString url = MakeImageUrl(type, pos, zoom, LanguageStr); +#ifdef DEBUG_TIMINGS + qDebug() << "opmaps after make image url" << time.elapsed(); +#endif // url "http://vec02.maps.yandex.ru/tiles?l=map&v=2.10.2&x=7&y=5&z=3" string + // "http://map3.pergo.com.tr/tile/02/000/000/007/000/000/002.png" + qheader.setUrl(QUrl(url)); + qheader.setRawHeader("User-Agent", UserAgent); + qheader.setRawHeader("Accept", "*/*"); + switch (type) { + case MapType::GoogleMap: + case MapType::GoogleSatellite: + case MapType::GoogleLabels: + case MapType::GoogleTerrain: + case MapType::GoogleHybrid: + { + qheader.setRawHeader("Referrer", "http://maps.google.com/"); + } + break; + + case MapType::GoogleMapChina: + case MapType::GoogleSatelliteChina: + case MapType::GoogleLabelsChina: + case MapType::GoogleTerrainChina: + case MapType::GoogleHybridChina: + { + qheader.setRawHeader("Referrer", "http://ditu.google.cn/"); + } + break; + + case MapType::BingHybrid: + case MapType::BingMap: + case MapType::BingSatellite: + { + qheader.setRawHeader("Referrer", "http://www.bing.com/maps/"); + } + break; + + case MapType::YahooHybrid: + case MapType::YahooLabels: + case MapType::YahooMap: + case MapType::YahooSatellite: + { + qheader.setRawHeader("Referrer", "http://maps.yahoo.com/"); + } + break; + + case MapType::ArcGIS_MapsLT_Map_Labels: + case MapType::ArcGIS_MapsLT_Map: + case MapType::ArcGIS_MapsLT_OrtoFoto: + case MapType::ArcGIS_MapsLT_Map_Hybrid: + { + qheader.setRawHeader("Referrer", "http://www.maps.lt/map_beta/"); + } + break; + + case MapType::OpenStreetMapSurfer: + case MapType::OpenStreetMapSurferTerrain: + { + qheader.setRawHeader("Referrer", "http://www.mapsurfer.net/"); + } + break; + + case MapType::OpenStreetMap: + case MapType::OpenStreetOsm: + { + qheader.setRawHeader("Referrer", "http://www.openstreetmap.org/"); + } + break; + + case MapType::YandexMapRu: + { + qheader.setRawHeader("Referrer", "http://maps.yandex.ru/"); + } + break; + default: + break; + } + reply = network.get(qheader); + tT.start(Timeout); + q.exec(); + + if (!tT.isActive()) { + errorvars.lock(); + ++diag.timeouts; + errorvars.unlock(); + return ret; + } + tT.stop(); + if ((reply->error() != QNetworkReply::NoError)) { + errorvars.lock(); + ++diag.networkerrors; + errorvars.unlock(); + reply->deleteLater(); + return ret; + } + ret = reply->readAll(); + reply->deleteLater(); // TODO can't this be global?? + if (ret.isEmpty()) { +#ifdef DEBUG_GMAPS + qDebug() << "Invalid Tile"; +#endif // DEBUG_GMAPS + errorvars.lock(); + ++diag.emptytiles; + errorvars.unlock(); + return ret; + } +#ifdef DEBUG_GMAPS + qDebug() << "Received Tile from the Internet"; +#endif // DEBUG_GMAPS + errorvars.lock(); + ++diag.tilesFromNet; + errorvars.unlock(); + if (useMemoryCache) { +#ifdef DEBUG_GMAPS + qDebug() << "Add Tile to memory cache"; +#endif // DEBUG_GMAPS + AddTileToMemoryCache(RawTile(type, pos, zoom), ret); + } + if (accessmode != AccessMode::ServerOnly) { +#ifdef DEBUG_GMAPS + qDebug() << "Add tile to DataBase"; +#endif // DEBUG_GMAPS + CacheItemQueue *item = new CacheItemQueue(type, pos, ret, zoom); + TileDBcacheQueue.EnqueueCacheTask(item); + } + } + } +#ifdef DEBUG_GMAPS + qDebug() << "Entered GetImageFrom"; +#endif // DEBUG_GMAPS + return ret; +} + +bool OPMaps::ExportToGMDB(const QString &file) +{ + return Cache::Instance()->ImageCache.ExportMapDataToDB(Cache::Instance()->ImageCache.GtileCache() + QDir::separator() + "Data.qmdb", file); +} +bool OPMaps::ImportFromGMDB(const QString &file) +{ + return Cache::Instance()->ImageCache.ExportMapDataToDB(file, Cache::Instance()->ImageCache.GtileCache() + QDir::separator() + "Data.qmdb"); +} + +diagnostics OPMaps::GetDiagnostics() +{ + diagnostics i; + + errorvars.lock(); + i = diag; + errorvars.unlock(); + return i; +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.h index 81b7daf2c..f5cf9dd43 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file OPMaps.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file OPMaps.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef OPMaps_H #define OPMaps_H @@ -41,54 +41,66 @@ #include "urlfactory.h" #include "diagnostics.h" -//#include "point.h" +// #include "point.h" namespace core { - class OPMaps: public MemoryCache,public AllLayersOfType,public UrlFactory +class OPMaps : public MemoryCache, public AllLayersOfType, public UrlFactory { +public: ~OPMaps(); + + static OPMaps *Instance(); + bool ImportFromGMDB(const QString &file); + bool ExportToGMDB(const QString &file); + /// + /// timeout for map connections + /// + + + QByteArray GetImageFrom(const MapType::Types &type, const core::Point &pos, const int &zoom); + bool UseMemoryCache() { + return useMemoryCache; + } // TODO + void setUseMemoryCache(const bool & value) + { + useMemoryCache = value; + } + void setLanguage(const LanguageType::Types & language) + { + Language = language; + } // TODO + LanguageType::Types GetLanguage() + { + return Language; + } // TODO + AccessMode::Types GetAccessMode() const + { + return accessmode; + } + void setAccessMode(const AccessMode::Types & mode) + { + accessmode = mode; + } + int RetryLoadTile; + diagnostics GetDiagnostics(); - - public: - - ~OPMaps(); - - static OPMaps* Instance(); - bool ImportFromGMDB(const QString &file); - bool ExportToGMDB(const QString &file); - /// - /// timeout for map connections - /// - - - QByteArray GetImageFrom(const MapType::Types &type,const core::Point &pos,const int &zoom); - bool UseMemoryCache(){return useMemoryCache;}//TODO - void setUseMemoryCache(const bool& value){useMemoryCache=value;} - void setLanguage(const LanguageType::Types& language){Language=language;}//TODO - LanguageType::Types GetLanguage(){return Language;}//TODO - AccessMode::Types GetAccessMode()const{return accessmode;} - void setAccessMode(const AccessMode::Types& mode){accessmode=mode;} - int RetryLoadTile; - diagnostics GetDiagnostics(); - - private: - bool useMemoryCache; - LanguageType::Types Language; - AccessMode::Types accessmode; - // PureImageCache ImageCacheLocal;//TODO Criar acesso Get Set - TileCacheQueue TileDBcacheQueue; - OPMaps(); - OPMaps(OPMaps const&){} - OPMaps& operator=(OPMaps const&){ return *this; } - static OPMaps* m_pInstance; - diagnostics diag; - QMutex errorvars; - protected: - // MemoryCache TilesInMemory; - - - - }; - +private: + bool useMemoryCache; + LanguageType::Types Language; + AccessMode::Types accessmode; + // PureImageCache ImageCacheLocal;//TODO Criar acesso Get Set + TileCacheQueue TileDBcacheQueue; + OPMaps(); + OPMaps(OPMaps const &) {} + OPMaps & operator=(OPMaps const &) + { + return *this; + } + static OPMaps *m_pInstance; + diagnostics diag; + QMutex errorvars; +protected: + // MemoryCache TilesInMemory; +}; } #endif // OPMaps_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.cpp index 72d664137..ce94cebfc 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.cpp @@ -1,28 +1,27 @@ /** -****************************************************************************** -* -* @file placemark.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file placemark.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "placemark.h" - diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.h index 7639bc3ca..d4e15d6cc 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/placemark.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file placemark.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file placemark.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef PLACEMARK_H #define PLACEMARK_H @@ -31,24 +31,33 @@ namespace core { - class Placemark +class Placemark { +public: + Placemark(const QString &address) { - public: - Placemark(const QString &address) - { - this->address = address; - } - QString Address(){return address;} - int Accuracy(){return accuracy;} - void SetAddress(const QString &adr){address=adr;} - void SetAccuracy(const int &value){accuracy=value;} - private: + this->address = address; + } + QString Address() + { + return address; + } + int Accuracy() + { + return accuracy; + } + void SetAddress(const QString &adr) + { + address = adr; + } + void SetAccuracy(const int &value) + { + accuracy = value; + } +private: - QString address; - int accuracy; - protected: - - - }; + QString address; + int accuracy; +protected: +}; } #endif // PLACEMARK_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.cpp index c41a66880..81bda5271 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.cpp @@ -1,74 +1,73 @@ /** -****************************************************************************** -* -* @file point.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file point.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "point.h" #include "size.h" namespace core { - Point::Point(int dw) - { - this->x=(short)Point::LOWORD(dw); - this->y=(short)Point::HIWORD(dw); - empty=false; - } - Point::Point(Size sz) - { - this->x=sz.Width(); - this->y=sz.Height(); - empty=false; - } - Point::Point(int x, int y) - { - this->x=x; - this->y=y; - empty=false; - } - Point::Point():x(0),y(0),empty(true) - {} - uint qHash(Point const& point) - { - return point.x^point.y; - } - bool operator==(Point const &lhs,Point const &rhs) - { - return (lhs.x==rhs.x && lhs.y==rhs.y); - } - bool operator!=(Point const &lhs,Point const &rhs) - { - return !(lhs==rhs); - } - int Point::HIWORD(int n) - { - return (n >> 16) & 0xffff; - } - - int Point::LOWORD(int n) - { - return n & 0xffff; - } - Point Point::Empty=Point(); - +Point::Point(int dw) +{ + this->x = (short)Point::LOWORD(dw); + this->y = (short)Point::HIWORD(dw); + empty = false; +} +Point::Point(Size sz) +{ + this->x = sz.Width(); + this->y = sz.Height(); + empty = false; +} +Point::Point(int x, int y) +{ + this->x = x; + this->y = y; + empty = false; +} +Point::Point() : x(0), y(0), empty(true) +{} +uint qHash(Point const & point) +{ + return point.x ^ point.y; +} +bool operator==(Point const &lhs, Point const &rhs) +{ + return lhs.x == rhs.x && lhs.y == rhs.y; +} +bool operator!=(Point const &lhs, Point const &rhs) +{ + return !(lhs == rhs); +} +int Point::HIWORD(int n) +{ + return (n >> 16) & 0xffff; +} + +int Point::LOWORD(int n) +{ + return n & 0xffff; +} +Point Point::Empty = Point(); } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.h index 318922aa2..b23d2a7f7 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/point.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file point.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file point.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef OPOINT_H #define OPOINT_H @@ -31,42 +31,59 @@ #include namespace core { - struct Size; - struct Point +struct Size; +struct Point { + friend uint qHash(Point const & point); + friend bool operator==(Point const & lhs, Point const & rhs); + friend bool operator!=(Point const & lhs, Point const & rhs); +public: + + Point(); + Point(int x, int y); + Point(Size sz); + Point(int dw); + bool IsEmpty() { - friend uint qHash(Point const& point); - friend bool operator==(Point const& lhs,Point const& rhs); - friend bool operator!=(Point const& lhs,Point const& rhs); - public: + return empty; + } + int X() const + { + return this->x; + } + int Y() const + { + return this->y; + } + void SetX(const int &value) + { + x = value; empty = false; + } + void SetY(const int &value) + { + y = value; empty = false; + } + QString ToString() const + { + return "{" + QString::number(x) + "," + QString::number(y) + "}"; + } - Point(); - Point(int x,int y); - Point(Size sz); - Point(int dw); - bool IsEmpty(){return empty;} - int X()const{return this->x;} - int Y()const{return this->y;} - void SetX(const int &value){x=value;empty=false;} - void SetY(const int &value){y=value;empty=false;} - QString ToString()const{return "{"+QString::number(x)+","+QString::number(y)+"}";} + static Point Empty; + void Offset(const int &dx, const int &dy) + { + x += dx; + y += dy; + } + void Offset(Point p) + { + Offset(p.x, p.y); + } + static int HIWORD(int n); + static int LOWORD(int n); - static Point Empty; - void Offset(const int &dx,const int &dy) - { - x += dx; - y += dy; - } - void Offset(Point p) - { - Offset(p.x, p.y); - } - static int HIWORD(int n); - static int LOWORD(int n); - - private: - int x; - int y; - bool empty; - }; +private: + int x; + int y; + bool empty; +}; } #endif // POINT_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.cpp index 7e87503c8..a4c913b9e 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.cpp @@ -1,88 +1,87 @@ /** -****************************************************************************** -* -* @file providerstrings.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file providerstrings.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "providerstrings.h" namespace core { - const QString ProviderStrings::levelsForSigPacSpainMap[] = {"0", "1", "2", "3", "4", - "MTNSIGPAC", - "MTN2000", "MTN2000", "MTN2000", "MTN2000", "MTN2000", - "MTN200", "MTN200", "MTN200", - "MTN25", "MTN25", - "ORTOFOTOS","ORTOFOTOS","ORTOFOTOS","ORTOFOTOS"}; +const QString ProviderStrings::levelsForSigPacSpainMap[] = { "0", "1", "2", "3", "4", + "MTNSIGPAC", + "MTN2000", "MTN2000", "MTN2000", "MTN2000", "MTN2000", + "MTN200", "MTN200", "MTN200", + "MTN25", "MTN25", + "ORTOFOTOS", "ORTOFOTOS", "ORTOFOTOS", "ORTOFOTOS" }; - ProviderStrings::ProviderStrings() - { -// VersionGoogleMap = "m@132"; -// VersionGoogleSatellite = "71"; -// VersionGoogleLabels = "h@132"; -// VersionGoogleTerrain = "t@125,r@132"; - // Google version strings - VersionGoogleMap = "m@132"; - VersionGoogleSatellite = "71"; - VersionGoogleLabels = "h@132"; - VersionGoogleTerrain = "t@125,r@132"; - SecGoogleWord = "Galileo"; +ProviderStrings::ProviderStrings() +{ +// VersionGoogleMap = "m@132"; +// VersionGoogleSatellite = "71"; +// VersionGoogleLabels = "h@132"; +// VersionGoogleTerrain = "t@125,r@132"; + // Google version strings + VersionGoogleMap = "m@132"; + VersionGoogleSatellite = "71"; + VersionGoogleLabels = "h@132"; + VersionGoogleTerrain = "t@125,r@132"; + SecGoogleWord = "Galileo"; - // Google (China) version strings - VersionGoogleMapChina = "m@132"; - VersionGoogleSatelliteChina = "s@71"; - VersionGoogleLabelsChina = "h@132"; - VersionGoogleTerrainChina = "t@125,r@132"; + // Google (China) version strings + VersionGoogleMapChina = "m@132"; + VersionGoogleSatelliteChina = "s@71"; + VersionGoogleLabelsChina = "h@132"; + VersionGoogleTerrainChina = "t@125,r@132"; - // Google (Korea) version strings - VersionGoogleMapKorea = "kr1.12"; - VersionGoogleSatelliteKorea = "66"; - VersionGoogleLabelsKorea = "kr1t.12"; + // Google (Korea) version strings + VersionGoogleMapKorea = "kr1.12"; + VersionGoogleSatelliteKorea = "66"; + VersionGoogleLabelsKorea = "kr1t.12"; - /// - /// Google Maps API generated using http://greatmaps.codeplex.com/ - /// from http://code.google.com/intl/en-us/apis/maps/signup.html - /// - GoogleMapsAPIKey = "ABQIAAAAWaQgWiEBF3lW97ifKnAczhRAzBk5Igf8Z5n2W3hNnMT0j2TikxTLtVIGU7hCLLHMAuAMt-BO5UrEWA"; + /// + /// Google Maps API generated using http://greatmaps.codeplex.com/ + /// from http://code.google.com/intl/en-us/apis/maps/signup.html + /// + GoogleMapsAPIKey = "ABQIAAAAWaQgWiEBF3lW97ifKnAczhRAzBk5Igf8Z5n2W3hNnMT0j2TikxTLtVIGU7hCLLHMAuAMt-BO5UrEWA"; - // Yahoo version strings - VersionYahooMap = "4.3"; - VersionYahooSatellite = "1.9"; - VersionYahooLabels = "4.3"; + // Yahoo version strings + VersionYahooMap = "4.3"; + VersionYahooSatellite = "1.9"; + VersionYahooLabels = "4.3"; - // BingMaps - VersionBingMaps = "563"; + // BingMaps + VersionBingMaps = "563"; - // YandexMap - VersionYandexMap = "2.16.0"; - //VersionYandexSatellite = "1.19.0"; - //////////////////// + // YandexMap + VersionYandexMap = "2.16.0"; + // VersionYandexSatellite = "1.19.0"; + //////////////////// - /// - /// Bing Maps Customer Identification, more info here - /// http://msdn.microsoft.com/en-us/library/bb924353.aspx - /// - BingMapsClientToken = ""; - - } + /// + /// Bing Maps Customer Identification, more info here + /// http://msdn.microsoft.com/en-us/library/bb924353.aspx + /// + BingMapsClientToken = ""; +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.h index c24ccd795..1d349e7c2 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file providerstrings.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file providerstrings.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef PROVIDERSTRINGS_H #define PROVIDERSTRINGS_H @@ -31,55 +31,52 @@ namespace core { - class ProviderStrings - { - public: - ProviderStrings(); - static const QString levelsForSigPacSpainMap[]; - QString GoogleMapsAPIKey; - // Google version strings - QString VersionGoogleMap; - QString VersionGoogleSatellite; - QString VersionGoogleLabels; - QString VersionGoogleTerrain; - QString SecGoogleWord; +class ProviderStrings { +public: + ProviderStrings(); + static const QString levelsForSigPacSpainMap[]; + QString GoogleMapsAPIKey; + // Google version strings + QString VersionGoogleMap; + QString VersionGoogleSatellite; + QString VersionGoogleLabels; + QString VersionGoogleTerrain; + QString SecGoogleWord; - // Google (China) version strings - QString VersionGoogleMapChina; - QString VersionGoogleSatelliteChina; - QString VersionGoogleLabelsChina; - QString VersionGoogleTerrainChina; + // Google (China) version strings + QString VersionGoogleMapChina; + QString VersionGoogleSatelliteChina; + QString VersionGoogleLabelsChina; + QString VersionGoogleTerrainChina; - // Google (Korea) version strings - QString VersionGoogleMapKorea; - QString VersionGoogleSatelliteKorea; - QString VersionGoogleLabelsKorea; + // Google (Korea) version strings + QString VersionGoogleMapKorea; + QString VersionGoogleSatelliteKorea; + QString VersionGoogleLabelsKorea; - /// - /// Google Maps API generated using http://greatmaps.codeplex.com/ - /// from http://code.google.com/intl/en-us/apis/maps/signup.html - /// + /// + /// Google Maps API generated using http://greatmaps.codeplex.com/ + /// from http://code.google.com/intl/en-us/apis/maps/signup.html + /// - // Yahoo version strings - QString VersionYahooMap; - QString VersionYahooSatellite; - QString VersionYahooLabels; + // Yahoo version strings + QString VersionYahooMap; + QString VersionYahooSatellite; + QString VersionYahooLabels; - // BingMaps - QString VersionBingMaps; + // BingMaps + QString VersionBingMaps; - // YandexMap - QString VersionYandexMap; + // YandexMap + QString VersionYandexMap; - - /// - /// Bing Maps Customer Identification, more info here - /// http://msdn.microsoft.com/en-us/library/bb924353.aspx - /// - QString BingMapsClientToken; - }; - + /// + /// Bing Maps Customer Identification, more info here + /// http://msdn.microsoft.com/en-us/library/bb924353.aspx + /// + QString BingMapsClientToken; +}; } #endif // PROVIDERSTRINGS_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.cpp index c64f3c111..f6c28e8a6 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.cpp @@ -1,38 +1,35 @@ /** -****************************************************************************** -* -* @file pureimage.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file pureimage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "pureimage.h" - namespace core { PureImageProxy::PureImageProxy() -{ - -} +{} QPixmap PureImageProxy::FromStream(const QByteArray &array) { @@ -40,7 +37,7 @@ QPixmap PureImageProxy::FromStream(const QByteArray &array) } bool PureImageProxy::Save(const QByteArray &array, QPixmap &pic) { - pic=QPixmap::fromImage(QImage::fromData(array)); + pic = QPixmap::fromImage(QImage::fromData(array)); return true; } } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.h index edc6bd85c..8606c40d4 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimage.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file pureimage.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file pureimage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef PUREIMAGE_H #define PUREIMAGE_H @@ -32,13 +32,11 @@ namespace core { - class PureImageProxy - { - public: - PureImageProxy(); - static QPixmap FromStream(const QByteArray &array); - static bool Save(const QByteArray &array,QPixmap &pic); - }; - +class PureImageProxy { +public: + PureImageProxy(); + static QPixmap FromStream(const QByteArray &array); + static bool Save(const QByteArray &array, QPixmap &pic); +}; } #endif // PUREIMAGE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.cpp index eb16755ea..3b6a44aac 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.cpp @@ -1,354 +1,331 @@ /** -****************************************************************************** -* -* @file pureimagecache.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file pureimagecache.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "pureimagecache.h" #include #include -//#define DEBUG_PUREIMAGECACHE +// #define DEBUG_PUREIMAGECACHE namespace core { - qlonglong PureImageCache::ConnCounter=0; +qlonglong PureImageCache::ConnCounter = 0; - PureImageCache::PureImageCache() - { +PureImageCache::PureImageCache() +{} +void PureImageCache::setGtileCache(const QString &value) +{ + lock.lockForWrite(); + gtilecache = value; + QDir d; + if (!d.exists(gtilecache)) { + d.mkdir(gtilecache); +#ifdef DEBUG_PUREIMAGECACHE + qDebug() << "Create Cache directory"; +#endif // DEBUG_PUREIMAGECACHE } - - void PureImageCache::setGtileCache(const QString &value) { - lock.lockForWrite(); - gtilecache=value; - QDir d; - if(!d.exists(gtilecache)) - { - d.mkdir(gtilecache); + QString db = gtilecache + "Data.qmdb"; + if (!QFileInfo(db).exists()) { #ifdef DEBUG_PUREIMAGECACHE - qDebug()<<"Create Cache directory"; -#endif //DEBUG_PUREIMAGECACHE + qDebug() << "Try to create EmptyDB"; +#endif // DEBUG_PUREIMAGECACHE + CreateEmptyDB(db); } - { - QString db=gtilecache+"Data.qmdb"; - if(!QFileInfo(db).exists()) - { -#ifdef DEBUG_PUREIMAGECACHE - qDebug()<<"Try to create EmptyDB"; -#endif //DEBUG_PUREIMAGECACHE - CreateEmptyDB(db); - } - } - lock.unlock(); } - QString PureImageCache::GtileCache() - { - return gtilecache; + lock.unlock(); +} +QString PureImageCache::GtileCache() +{ + return gtilecache; +} + + +bool PureImageCache::CreateEmptyDB(const QString &file) +{ +#ifdef DEBUG_PUREIMAGECACHE + qDebug() << "Create database at!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!:" << file; +#endif // DEBUG_PUREIMAGECACHE + QFileInfo File(file); + QDir dir = File.absoluteDir(); + QString path = dir.absolutePath(); + QString filename = File.fileName(); + if (File.exists()) { + QFile(filename).remove(); } + if (!dir.exists()) { +#ifdef DEBUG_PUREIMAGECACHE + qDebug() << "CreateEmptyDB: Cache path doesn't exist, try to create"; +#endif // DEBUG_PUREIMAGECACHE + if (!dir.mkpath(path)) { +#ifdef DEBUG_PUREIMAGECACHE + qDebug() << "CreateEmptyDB: Could not create path"; +#endif // DEBUG_PUREIMAGECACHE + return false; + } + } + QSqlDatabase db; + db = QSqlDatabase::addDatabase("QSQLITE", QLatin1String("CreateConn")); + db.setDatabaseName(file); + if (!db.open()) { +#ifdef DEBUG_PUREIMAGECACHE + qDebug() << "CreateEmptyDB: Unable to create database"; +#endif // DEBUG_PUREIMAGECACHE - bool PureImageCache::CreateEmptyDB(const QString &file) - { + return false; + } + QSqlQuery query(db); + query.exec("CREATE TABLE IF NOT EXISTS Tiles (id INTEGER NOT NULL PRIMARY KEY, X INTEGER NOT NULL, Y INTEGER NOT NULL, Zoom INTEGER NOT NULL, Type INTEGER NOT NULL,Date TEXT)"); + if (query.numRowsAffected() == -1) { #ifdef DEBUG_PUREIMAGECACHE - qDebug()<<"Create database at!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!:"< add; - bool ret=true; - QString dir=gtilecache; - { - QString db=dir+"Data.qmdb"; - ret=QFileInfo(db).exists(); - if(ret) - { - QSqlDatabase cn; - Mcounter.lock(); - qlonglong id=++ConnCounter; - Mcounter.unlock(); - cn = QSqlDatabase::addDatabase("QSQLITE",QString::number(id)); - cn.setDatabaseName(db); - cn.setConnectOptions("QSQLITE_ENABLE_SHARED_CACHE"); - if(cn.open()) - { - { - QSqlQuery query(cn); - query.exec(QString("SELECT id, X, Y, Zoom, Type, Date FROM Tiles")); - while(query.next()) - { - if(QDateTime::fromString(query.value(5).toString()).daysTo(QDateTime::currentDateTime())>days) - add.append(query.value(0).toLongLong()); - } - foreach(long i,add) - { - query.exec(QString("DELETE FROM Tiles WHERE id = %1;").arg(i)); - } - } - - cn.close(); - } - QSqlDatabase::removeDatabase(QString::number(id)); - } - } - } - // PureImageCache::ExportMapDataToDB("C:/Users/Xapo/Documents/mapcontrol/debug/mapscache/data.qmdb","C:/Users/Xapo/Documents/mapcontrol/debug/mapscache/data2.qmdb"); - bool PureImageCache::ExportMapDataToDB(QString sourceFile, QString destFile) - { - bool ret=true; - QList add; - if(!QFileInfo(destFile).exists()) - { -#ifdef DEBUG_PUREIMAGECACHE - qDebug()<<"Try to create EmptyDB"; -#endif //DEBUG_PUREIMAGECACHE - ret=CreateEmptyDB(destFile); - } - if(!ret) return false; - QSqlDatabase ca = QSqlDatabase::addDatabase("QSQLITE","ca"); - ca.setDatabaseName(sourceFile); - - if(ca.open()) - { - QSqlDatabase cb = QSqlDatabase::addDatabase("QSQLITE","cb"); - cb.setDatabaseName(destFile); - if(cb.open()) - { - QSqlQuery queryb(cb); - queryb.exec(QString("ATTACH DATABASE \"%1\" AS Source").arg(sourceFile)); - QSqlQuery querya(ca); - querya.exec("SELECT id, X, Y, Zoom, Type, Date FROM Tiles"); - while(querya.next()) - { - long id=querya.value(0).toLongLong(); - queryb.exec(QString("SELECT id FROM Tiles WHERE X=%1 AND Y=%2 AND Zoom=%3 AND Type=%4;").arg(querya.value(1).toLongLong()).arg(querya.value(2).toLongLong()).arg(querya.value(3).toLongLong()).arg(querya.value(4).toLongLong())); - if(!queryb.next()) - { - add.append(id); - } - - } - long f; - foreach(f,add) - { - queryb.exec(QString("INSERT INTO Tiles(X, Y, Zoom, Type, Date) SELECT X, Y, Zoom, Type, Date FROM Source.Tiles WHERE id=%1").arg(f)); - queryb.exec(QString("INSERT INTO TilesData(id, Tile) Values((SELECT last_insert_rowid()), (SELECT Tile FROM Source.TilesData WHERE id=%1))").arg(f)); - } - add.clear(); - ca.close(); - cb.close(); - - } - else return false; - } - else return false; - QSqlDatabase::removeDatabase("ca"); - QSqlDatabase::removeDatabase("cb"); - return true; - - } - + QSqlDatabase::removeDatabase(QString::number(id)); + lock.unlock(); + return ar; +} +void PureImageCache::deleteOlderTiles(int const & days) +{ + if (gtilecache.isEmpty() | gtilecache.isNull()) { + return; + } + QList add; + bool ret = true; + QString dir = gtilecache; + { + QString db = dir + "Data.qmdb"; + ret = QFileInfo(db).exists(); + if (ret) { + QSqlDatabase cn; + Mcounter.lock(); + qlonglong id = ++ConnCounter; + Mcounter.unlock(); + cn = QSqlDatabase::addDatabase("QSQLITE", QString::number(id)); + cn.setDatabaseName(db); + cn.setConnectOptions("QSQLITE_ENABLE_SHARED_CACHE"); + if (cn.open()) { + { + QSqlQuery query(cn); + query.exec(QString("SELECT id, X, Y, Zoom, Type, Date FROM Tiles")); + while (query.next()) { + if (QDateTime::fromString(query.value(5).toString()).daysTo(QDateTime::currentDateTime()) > days) { + add.append(query.value(0).toLongLong()); + } + } + foreach(long i, add) { + query.exec(QString("DELETE FROM Tiles WHERE id = %1;").arg(i)); + } + } + + cn.close(); + } + QSqlDatabase::removeDatabase(QString::number(id)); + } + } +} +// PureImageCache::ExportMapDataToDB("C:/Users/Xapo/Documents/mapcontrol/debug/mapscache/data.qmdb","C:/Users/Xapo/Documents/mapcontrol/debug/mapscache/data2.qmdb"); +bool PureImageCache::ExportMapDataToDB(QString sourceFile, QString destFile) +{ + bool ret = true; + + QList add; + if (!QFileInfo(destFile).exists()) { +#ifdef DEBUG_PUREIMAGECACHE + qDebug() << "Try to create EmptyDB"; +#endif // DEBUG_PUREIMAGECACHE + ret = CreateEmptyDB(destFile); + } + if (!ret) { + return false; + } + QSqlDatabase ca = QSqlDatabase::addDatabase("QSQLITE", "ca"); + ca.setDatabaseName(sourceFile); + + if (ca.open()) { + QSqlDatabase cb = QSqlDatabase::addDatabase("QSQLITE", "cb"); + cb.setDatabaseName(destFile); + if (cb.open()) { + QSqlQuery queryb(cb); + queryb.exec(QString("ATTACH DATABASE \"%1\" AS Source").arg(sourceFile)); + QSqlQuery querya(ca); + querya.exec("SELECT id, X, Y, Zoom, Type, Date FROM Tiles"); + while (querya.next()) { + long id = querya.value(0).toLongLong(); + queryb.exec(QString("SELECT id FROM Tiles WHERE X=%1 AND Y=%2 AND Zoom=%3 AND Type=%4;").arg(querya.value(1).toLongLong()).arg(querya.value(2).toLongLong()).arg(querya.value(3).toLongLong()).arg(querya.value(4).toLongLong())); + if (!queryb.next()) { + add.append(id); + } + } + long f; + foreach(f, add) { + queryb.exec(QString("INSERT INTO Tiles(X, Y, Zoom, Type, Date) SELECT X, Y, Zoom, Type, Date FROM Source.Tiles WHERE id=%1").arg(f)); + queryb.exec(QString("INSERT INTO TilesData(id, Tile) Values((SELECT last_insert_rowid()), (SELECT Tile FROM Source.TilesData WHERE id=%1))").arg(f)); + } + add.clear(); + ca.close(); + cb.close(); + } else { return false; } + } else { return false; } + QSqlDatabase::removeDatabase("ca"); + QSqlDatabase::removeDatabase("cb"); + return true; +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.h index d92d58090..de324b629 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/pureimagecache.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file pureimagecache.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file pureimagecache.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef PUREIMAGECACHE_H #define PUREIMAGECACHE_H @@ -43,25 +43,21 @@ #include #include namespace core { - class PureImageCache - { - - public: - PureImageCache(); - static bool CreateEmptyDB(const QString &file); - bool PutImageToCache(const QByteArray &tile,const MapType::Types &type,const core::Point &pos, const int &zoom); - QByteArray GetImageFromCache(MapType::Types type, core::Point pos, int zoom); - QString GtileCache(); - void setGtileCache(const QString &value); - static bool ExportMapDataToDB(QString sourceFile, QString destFile); - void deleteOlderTiles(int const& days); - private: - QString gtilecache; - QMutex Mcounter; - QReadWriteLock lock; - static qlonglong ConnCounter; - - }; - +class PureImageCache { +public: + PureImageCache(); + static bool CreateEmptyDB(const QString &file); + bool PutImageToCache(const QByteArray &tile, const MapType::Types &type, const core::Point &pos, const int &zoom); + QByteArray GetImageFromCache(MapType::Types type, core::Point pos, int zoom); + QString GtileCache(); + void setGtileCache(const QString &value); + static bool ExportMapDataToDB(QString sourceFile, QString destFile); + void deleteOlderTiles(int const & days); +private: + QString gtilecache; + QMutex Mcounter; + QReadWriteLock lock; + static qlonglong ConnCounter; +}; } #endif // PUREIMAGECACHE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.cpp index 836d1c6a6..abcf68e04 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.cpp @@ -1,38 +1,38 @@ /** -****************************************************************************** -* -* @file rawtile.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file rawtile.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "rawtile.h" - + namespace core { RawTile::RawTile(const MapType::Types &Type, const Point &Pos, const int &Zoom) { - zoom=Zoom; - type=Type; - pos=Pos; + zoom = Zoom; + type = Type; + pos = Pos; } QString RawTile::ToString() { @@ -52,25 +52,26 @@ int RawTile::Zoom() } void RawTile::setType(const MapType::Types &value) { - type=value; + type = value; } void RawTile::setPos(const Point &value) { - pos=value; + pos = value; } void RawTile::setZoom(const int &value) { - zoom=value; + zoom = value; } -uint qHash(RawTile const& tile) +uint qHash(RawTile const & tile) { // RawTile tile=tilee; - quint64 tmp=(((quint64)(tile.zoom))<<54)+(((quint64)(tile.type))<<36)+(((quint64)(tile.pos.X()))<<18)+(((quint64)(tile.pos.Y()))); - // quint64 tmp5=tmp+tmp2+tmp3+tmp4; + quint64 tmp = (((quint64)(tile.zoom)) << 54) + (((quint64)(tile.type)) << 36) + (((quint64)(tile.pos.X())) << 18) + (((quint64)(tile.pos.Y()))); + + // quint64 tmp5=tmp+tmp2+tmp3+tmp4; return ::qHash(tmp); } -bool operator==(RawTile const &lhs,RawTile const &rhs) +bool operator==(RawTile const &lhs, RawTile const &rhs) { - return (lhs.pos==rhs.pos && lhs.zoom==rhs.zoom && lhs.type==rhs.type); + return lhs.pos == rhs.pos && lhs.zoom == rhs.zoom && lhs.type == rhs.type; } } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.h index ae0796f6c..93cb1c34f 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/rawtile.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file rawtile.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file rawtile.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef RAWTILE_H #define RAWTILE_H @@ -33,24 +33,23 @@ #include namespace core { - class RawTile - { - friend uint qHash(RawTile const& tile); - friend bool operator==(RawTile const& lhs,RawTile const& rhs); +class RawTile { + friend uint qHash(RawTile const & tile); + friend bool operator==(RawTile const & lhs, RawTile const & rhs); - public: - RawTile(const MapType::Types &Type,const core::Point &Pos,const int &Zoom); - QString ToString(void); - MapType::Types Type(); - core::Point Pos(); - int Zoom(); - void setType(const MapType::Types &value); - void setPos(const core::Point &value); - void setZoom(const int &value); - private: - MapType::Types type; - core::Point pos; - int zoom; - }; +public: + RawTile(const MapType::Types &Type, const core::Point &Pos, const int &Zoom); + QString ToString(void); + MapType::Types Type(); + core::Point Pos(); + int Zoom(); + void setType(const MapType::Types &value); + void setPos(const core::Point &value); + void setZoom(const int &value); +private: + MapType::Types type; + core::Point pos; + int zoom; +}; } #endif // RAWTILE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.cpp index 3d663cde7..b3988e844 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.cpp @@ -1,33 +1,33 @@ /** -****************************************************************************** -* -* @file size.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file size.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "size.h" - + namespace core { -Size::Size():width(0),height(0) +Size::Size() : width(0), height(0) {} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.h index 6949ae28a..7a97079b9 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/size.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file size.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file size.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef SIZE_H #define SIZE_H @@ -32,27 +32,58 @@ #include namespace core { - struct Size +struct Size { + Size(); + Size(Point pt) { - - Size(); - Size(Point pt){width=pt.X(); height=pt.Y();}; - Size(int Width,int Height){width=Width; height=Height;}; - friend uint qHash(Size const& size); - // friend bool operator==(Size const& lhs,Size const& rhs); - Size operator-(const Size &sz1){return Size(width-sz1.width,height-sz1.height);} - Size operator+(const Size &sz1){return Size(sz1.width+width,sz1.height+height);} - - int GetHashCode(){return width^height;} - uint qHash(Size const& /*rect*/){return width^height;} - QString ToString(){return "With="+QString::number(width)+" ,Height="+QString::number(height);} - int Width()const {return width;} - int Height()const {return height;} - void SetWidth(int const& value){width=value;} - void SetHeight(int const& value){height=value;} - private: - int width; - int height; + width = pt.X(); height = pt.Y(); }; + Size(int Width, int Height) + { + width = Width; height = Height; + }; + friend uint qHash(Size const & size); + // friend bool operator==(Size const& lhs,Size const& rhs); + Size operator-(const Size &sz1) + { + return Size(width - sz1.width, height - sz1.height); + } + Size operator+(const Size &sz1) + { + return Size(sz1.width + width, sz1.height + height); + } + + int GetHashCode() + { + return width ^ height; + } + uint qHash(Size const & /*rect*/) + { + return width ^ height; + } + QString ToString() + { + return "With=" + QString::number(width) + " ,Height=" + QString::number(height); + } + int Width() const + { + return width; + } + int Height() const + { + return height; + } + void SetWidth(int const & value) + { + width = value; + } + void SetHeight(int const & value) + { + height = value; + } +private: + int width; + int height; +}; } #endif // SIZE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.cpp index 9843f2f45..a47716517 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.cpp @@ -1,128 +1,112 @@ /** -****************************************************************************** -* -* @file tilecachequeue.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file tilecachequeue.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "tilecachequeue.h" -//#define DEBUG_TILECACHEQUEUE - +// #define DEBUG_TILECACHEQUEUE + namespace core { TileCacheQueue::TileCacheQueue() -{ - -} +{} TileCacheQueue::~TileCacheQueue() { - // QThread::wait(10000); + // QThread::wait(10000); } void TileCacheQueue::EnqueueCacheTask(CacheItemQueue *task) { #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"DB Do I EnqueueCacheTask"<GetPosition().X()<<","<GetPosition().Y(); -#endif //DEBUG_TILECACHEQUEUE - if(!tileCacheQueue.contains(task)) - { + qDebug() << "DB Do I EnqueueCacheTask" << task->GetPosition().X() << "," << task->GetPosition().Y(); +#endif // DEBUG_TILECACHEQUEUE + if (!tileCacheQueue.contains(task)) { #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"EnqueueCacheTask"<GetPosition().X()<<","<GetPosition().Y(); -#endif //DEBUG_TILECACHEQUEUE + qDebug() << "EnqueueCacheTask" << task->GetPosition().X() << "," << task->GetPosition().Y(); +#endif // DEBUG_TILECACHEQUEUE mutex.lock(); tileCacheQueue.enqueue(task); mutex.unlock(); - if(this->isRunning()) - { + if (this->isRunning()) { #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"Wake Thread"; -#endif //DEBUG_TILECACHEQUEUE - //this->start(QThread::NormalPriority); - //waitmutex.lock(); + qDebug() << "Wake Thread"; +#endif // DEBUG_TILECACHEQUEUE + // this->start(QThread::NormalPriority); + // waitmutex.lock(); waitc.wakeAll(); - //waitmutex.unlock(); - } - else - { + // waitmutex.unlock(); + } else { #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"Start Thread"; -#endif //DEBUG_TILECACHEQUEUE + qDebug() << "Start Thread"; +#endif // DEBUG_TILECACHEQUEUE this->start(QThread::NormalPriority); } } - } void TileCacheQueue::run() { #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"Cache Engine Start"; -#endif //DEBUG_TILECACHEQUEUE - while(true) - { + qDebug() << "Cache Engine Start"; +#endif // DEBUG_TILECACHEQUEUE + while (true) { CacheItemQueue *task; #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"Cache"; -#endif //DEBUG_TILECACHEQUEUE - if(tileCacheQueue.count()>0) - { + qDebug() << "Cache"; +#endif // DEBUG_TILECACHEQUEUE + if (tileCacheQueue.count() > 0) { mutex.lock(); - task=tileCacheQueue.dequeue(); + task = tileCacheQueue.dequeue(); mutex.unlock(); #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"Cache engine Put:"<GetPosition().X()<<","<GetPosition().Y(); -#endif //DEBUG_TILECACHEQUEUE - Cache::Instance()->ImageCache.PutImageToCache(task->GetImg(),task->GetMapType(),task->GetPosition(),task->GetZoom()); + qDebug() << "Cache engine Put:" << task->GetPosition().X() << "," << task->GetPosition().Y(); +#endif // DEBUG_TILECACHEQUEUE + Cache::Instance()->ImageCache.PutImageToCache(task->GetImg(), task->GetMapType(), task->GetPosition(), task->GetZoom()); usleep(44); delete task; - } - - else - { - qDebug()<<"Cache engine BEGIN WAIT"; + } else { + qDebug() << "Cache engine BEGIN WAIT"; waitmutex.lock(); - int tout=4000; - if(!waitc.wait(&waitmutex,tout)) - { + int tout = 4000; + if (!waitc.wait(&waitmutex, tout)) { waitmutex.unlock(); #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"Cache Engine TimeOut"; -#endif //DEBUG_TILECACHEQUEUE + qDebug() << "Cache Engine TimeOut"; +#endif // DEBUG_TILECACHEQUEUE mutex.lock(); - if(tileCacheQueue.count()==0) - { + if (tileCacheQueue.count() == 0) { mutex.unlock(); break; } mutex.unlock(); } - qDebug()<<"Cache Engine DID NOT TimeOut"; + qDebug() << "Cache Engine DID NOT TimeOut"; waitmutex.unlock(); } } #ifdef DEBUG_TILECACHEQUEUE - qDebug()<<"Cache Engine Stopped"; -#endif //DEBUG_TILECACHEQUEUE + qDebug() << "Cache Engine Stopped"; +#endif // DEBUG_TILECACHEQUEUE } - - } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.h index 2885fcdfe..3ded54303 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/tilecachequeue.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file tilecachequeue.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file tilecachequeue.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef TILECACHEQUEUE_H #define TILECACHEQUEUE_H @@ -39,21 +39,20 @@ namespace core { - class TileCacheQueue:public QThread - { - Q_OBJECT - public: - TileCacheQueue(); - ~TileCacheQueue(); - void EnqueueCacheTask(CacheItemQueue *task); +class TileCacheQueue : public QThread { + Q_OBJECT +public: + TileCacheQueue(); + ~TileCacheQueue(); + void EnqueueCacheTask(CacheItemQueue *task); - protected: - QQueue tileCacheQueue; - private: - void run(); - QMutex mutex; - QMutex waitmutex; - QWaitCondition waitc; - }; +protected: + QQueue tileCacheQueue; +private: + void run(); + QMutex mutex; + QMutex waitmutex; + QWaitCondition waitc; +}; } #endif // TILECACHEQUEUE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp index 2ebdeffc5..cd4e4d0d0 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp @@ -1,694 +1,675 @@ /** -****************************************************************************** -* -* @file urlfactory.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file urlfactory.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "urlfactory.h" #include namespace core { +const double UrlFactory::EarthRadiusKm = 6378.137; // WGS-84 - const double UrlFactory::EarthRadiusKm = 6378.137; // WGS-84 +UrlFactory::UrlFactory() +{ + /// + /// timeout for map connections + /// - UrlFactory::UrlFactory() - { - /// - /// timeout for map connections - /// + Proxy.setType(QNetworkProxy::NoProxy); - Proxy.setType(QNetworkProxy::NoProxy); + /// + /// Gets or sets the value of the User-agent HTTP header. + /// + UserAgent = "Mozilla/5.0 (Windows; U; Windows NT 6.0; en-US; rv:1.9.1.7) Gecko/20091221 Firefox/3.5.7"; - /// - /// Gets or sets the value of the User-agent HTTP header. - /// - UserAgent = "Mozilla/5.0 (Windows; U; Windows NT 6.0; en-US; rv:1.9.1.7) Gecko/20091221 Firefox/3.5.7"; + Timeout = 5 * 1000; + CorrectGoogleVersions = true; + isCorrectedGoogleVersions = false; + UseGeocoderCache = true; + UsePlacemarkCache = true; +} +UrlFactory::~UrlFactory() +{} +QString UrlFactory::TileXYToQuadKey(const int &tileX, const int &tileY, const int &levelOfDetail) const +{ + QString quadKey; - Timeout = 5 * 1000; - CorrectGoogleVersions=true; - isCorrectedGoogleVersions = false; - UseGeocoderCache=true; - UsePlacemarkCache=true; - } - UrlFactory::~UrlFactory() - { - } - QString UrlFactory::TileXYToQuadKey(const int &tileX,const int &tileY,const int &levelOfDetail) const - { - QString quadKey; - for(int i = levelOfDetail; i > 0; i--) - { - char digit = '0'; - int mask = 1 << (i - 1); - if((tileX & mask) != 0) - { - digit++; - } - if((tileY & mask) != 0) - { - digit++; - digit++; - } - quadKey.append(digit); + for (int i = levelOfDetail; i > 0; i--) { + char digit = '0'; + int mask = 1 << (i - 1); + if ((tileX & mask) != 0) { + digit++; } - return quadKey; - } - int UrlFactory::GetServerNum(const Point &pos,const int &max) const - { - return (pos.X() + 2 * pos.Y()) % max; - } - void UrlFactory::setIsCorrectGoogleVersions(bool value) - { - isCorrectedGoogleVersions=value; + if ((tileY & mask) != 0) { + digit++; + digit++; + } + quadKey.append(digit); } + return quadKey; +} +int UrlFactory::GetServerNum(const Point &pos, const int &max) const +{ + return (pos.X() + 2 * pos.Y()) % max; +} +void UrlFactory::setIsCorrectGoogleVersions(bool value) +{ + isCorrectedGoogleVersions = value; +} - bool UrlFactory::IsCorrectGoogleVersions() - { - return isCorrectedGoogleVersions; - } +bool UrlFactory::IsCorrectGoogleVersions() +{ + return isCorrectedGoogleVersions; +} - void UrlFactory::TryCorrectGoogleVersions() - { - QMutexLocker locker(&mutex); - if(CorrectGoogleVersions && !IsCorrectGoogleVersions()) - { - QNetworkReply *reply; - QNetworkRequest qheader; - QNetworkAccessManager network; - QEventLoop q; - QTimer tT; - tT.setSingleShot(true); - connect(&network, SIGNAL(finished(QNetworkReply*)), - &q, SLOT(quit())); - connect(&tT, SIGNAL(timeout()), &q, SLOT(quit())); - network.setProxy(Proxy); +void UrlFactory::TryCorrectGoogleVersions() +{ + QMutexLocker locker(&mutex); + + if (CorrectGoogleVersions && !IsCorrectGoogleVersions()) { + QNetworkReply *reply; + QNetworkRequest qheader; + QNetworkAccessManager network; + QEventLoop q; + QTimer tT; + tT.setSingleShot(true); + connect(&network, SIGNAL(finished(QNetworkReply *)), + &q, SLOT(quit())); + connect(&tT, SIGNAL(timeout()), &q, SLOT(quit())); + network.setProxy(Proxy); #ifdef DEBUG_URLFACTORY - qDebug()<<"Correct GoogleVersion"; -#endif //DEBUG_URLFACTORY - setIsCorrectGoogleVersions(true); - QString url = "http://maps.google.com"; + qDebug() << "Correct GoogleVersion"; +#endif // DEBUG_URLFACTORY + setIsCorrectGoogleVersions(true); + QString url = "http://maps.google.com"; - qheader.setUrl(QUrl(url)); - qheader.setRawHeader("User-Agent",UserAgent); - reply=network.get(qheader); - tT.start(Timeout); - q.exec(); - if(!tT.isActive()) - return; - tT.stop(); - if( (reply->error()!=QNetworkReply::NoError)) - { + qheader.setUrl(QUrl(url)); + qheader.setRawHeader("User-Agent", UserAgent); + reply = network.get(qheader); + tT.start(Timeout); + q.exec(); + if (!tT.isActive()) { + return; + } + tT.stop(); + if ((reply->error() != QNetworkReply::NoError)) { #ifdef DEBUG_URLFACTORY - qDebug()<<"Try corrected version withou abort or error:"<errorString(); -#endif //DEBUG_URLFACTORY - return; - } - QString html=QString(reply->readAll()); - QRegExp reg("\"*http://mt0.google.com/vt/lyrs=m@(\\d*)",Qt::CaseInsensitive); - if(reg.indexIn(html)!=-1) - { - QStringList gc=reg.capturedTexts(); - VersionGoogleMap = QString("m@%1").arg(gc[1]); - VersionGoogleMapChina = VersionGoogleMap; + qDebug() << "Try corrected version withou abort or error:" << reply->errorString(); +#endif // DEBUG_URLFACTORY + return; + } + QString html = QString(reply->readAll()); + QRegExp reg("\"*http://mt0.google.com/vt/lyrs=m@(\\d*)", Qt::CaseInsensitive); + if (reg.indexIn(html) != -1) { + QStringList gc = reg.capturedTexts(); + VersionGoogleMap = QString("m@%1").arg(gc[1]); + VersionGoogleMapChina = VersionGoogleMap; #ifdef DEBUG_URLFACTORY - qDebug()<<"TryCorrectGoogleVersions, VersionGoogleMap: "<deleteLater(); - + qDebug() << "TryCorrectGoogleVersions, VersionGoogleMap: " << VersionGoogleMap; +#endif // DEBUG_URLFACTORY } - } - - QString UrlFactory::MakeImageUrl(const MapType::Types &type,const Point &pos,const int &zoom,const QString &language) - { + reg = QRegExp("\"*http://mt0.google.com/vt/lyrs=h@(\\d*)", Qt::CaseInsensitive); + if (reg.indexIn(html) != -1) { + QStringList gc = reg.capturedTexts(); + VersionGoogleLabels = QString("h@%1").arg(gc[1]); + VersionGoogleLabelsChina = VersionGoogleLabels; #ifdef DEBUG_URLFACTORY - qDebug()<<"Entered MakeImageUrl"; -#endif //DEBUG_URLFACTORY - switch(type) - { - case MapType::GoogleMap: - { - QString server = "mt"; - QString request = "vt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - TryCorrectGoogleVersions(); - - return QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleMap).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleSatellite: - { - QString server = "khm"; - QString request = "kh"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - TryCorrectGoogleVersions(); - return QString("http://%1%2.google.com/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatellite).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleLabels: - { - QString server = "mt"; - QString request = "vt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - TryCorrectGoogleVersions(); - - return QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabels).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleTerrain: - { - QString server = "mt"; - QString request = "vt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - TryCorrectGoogleVersions(); - return QString("http://%1%2.google.com/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleTerrain).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleMapChina: - { - QString server = "mt"; - QString request = "vt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - TryCorrectGoogleVersions(); - // http://mt0.google.cn/vt/v=w2.101&hl=zh-CN&gl=cn&x=12&y=6&z=4&s=Ga - - return QString("http://%1%2.google.cn/%3/lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleMapChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleSatelliteChina: - { - QString server = "mt"; - QString request = "vt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - // TryCorrectGoogleVersions(); - // http://khm0.google.cn/kh/v=46&x=12&y=6&z=4&s=Ga - - return QString("http://%1%2.google.cn/%3/lyrs=%4&gl=cn&x=%5%6&y=%7&z=%8&s=%9").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatelliteChina).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleLabelsChina: - { - QString server = "mt"; - QString request = "vt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - TryCorrectGoogleVersions(); - // http://mt0.google.cn/vt/v=w2t.110&hl=zh-CN&gl=cn&x=12&y=6&z=4&s=Ga - - return QString("http://%1%2.google.cn/%3/imgtp=png32&lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabelsChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleTerrainChina: - { - QString server = "mt"; - QString request = "vt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - TryCorrectGoogleVersions(); - // http://mt0.google.cn/vt/v=w2p.110&hl=zh-CN&gl=cn&x=12&y=6&z=4&s=Ga - - return QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleTerrainChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleMapKorea: - { - QString server = "mt"; - QString request = "mt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - - //http://mt3.gmaptiles.co.kr/mt/v=kr1.11&hl=lt&x=109&y=49&z=7&s= - - QString ret = QString("http://%1%2.gmaptiles.co.kr/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleMapKorea).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - return ret; - } - break; - case MapType::GoogleSatelliteKorea: - { - QString server = "khm"; - QString request = "kh"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - - // http://khm1.google.co.kr/kh/v=54&x=109&y=49&z=7&s= - - return QString("http://%1%2.google.co.kr/%3/v=%4&x=%5%6&y=%7&z=%8&s=%9").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatelliteKorea).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::GoogleLabelsKorea: - { - QString server = "mt"; - QString request = "mt"; - QString sec1 = ""; // after &x=... - QString sec2 = ""; // after &zoom=... - GetSecGoogleWords(pos, sec1, sec2); - - // http://mt1.gmaptiles.co.kr/mt/v=kr1t.11&hl=lt&x=109&y=50&z=7&s=G - - return QString("http://%1%2.gmaptiles.co.kr/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabelsKorea).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); - } - break; - case MapType::YahooMap: - { - return QString("http://maps%1.yimg.com/hx/tl?v=%2&.intl=%3&x=%4&y=%5&z=%6&r=1").arg(((GetServerNum(pos, 2)) + 1)).arg(VersionYahooMap).arg(language).arg(pos.X()).arg((((1 << zoom) >> 1) - 1 - pos.Y())).arg((zoom + 1)); - } - - case MapType::YahooSatellite: - { - return QString("http://maps%1.yimg.com/ae/ximg?v=%2&t=a&s=256&.intl=%3&x=%4&y=%5&z=%6&r=1").arg("3").arg(VersionYahooSatellite).arg(language).arg(pos.X()).arg(((1 << zoom) >> 1) - 1 - pos.Y()).arg(zoom + 1); - } - break; - case MapType::YahooLabels: - { - return QString("http://maps%1.yimg.com/hx/tl?v=%2&t=h&.intl=%3&x=%4&y=%5&z=%6&r=1").arg("1").arg(VersionYahooLabels).arg(language).arg(pos.X()).arg(((1 << zoom) >> 1) - 1 - pos.Y()).arg(zoom + 1); - } - break; - case MapType::OpenStreetMap: - { - char letter= "abc"[GetServerNum(pos, 3)]; - return QString("http://%1.tile.openstreetmap.org/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y()); - } - break; - case MapType::OpenStreetOsm: - { - char letter = "abc"[GetServerNum(pos, 3)]; - return QString("http://%1.tah.openstreetmap.org/Tiles/tile/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y()); - } - break; - case MapType::OpenStreetMapSurfer: - { - // http://tiles1.mapsurfer.net/tms_r.ashx?x=37378&y=20826&z=16 - - return QString("http://tiles1.mapsurfer.net/tms_r.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom); - } - break; - case MapType::OpenStreetMapSurferTerrain: - { - // http://tiles2.mapsurfer.net/tms_t.ashx?x=9346&y=5209&z=14 - - return QString("http://tiles2.mapsurfer.net/tms_t.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom); - } - break; - case MapType::BingMap: - { - QString key = TileXYToQuadKey(pos.X(), pos.Y(), zoom); - return QString("http://ecn.t%1.tiles.virtualearth.net/tiles/r%2.png?g=%3&mkt=%4%5").arg(GetServerNum(pos, 4)).arg(key).arg(VersionBingMaps).arg(language).arg(!(BingMapsClientToken.isNull()|BingMapsClientToken.isEmpty()) ? "&token=" + BingMapsClientToken : QString("")); - } - break; - case MapType::BingSatellite: - { - QString key = TileXYToQuadKey(pos.X(), pos.Y(), zoom); - return QString("http://ecn.t%1.tiles.virtualearth.net/tiles/a%2.jpeg?g=%3&mkt=%4%5").arg(GetServerNum(pos, 4)).arg(key).arg(VersionBingMaps).arg(language).arg(!(BingMapsClientToken.isNull()|BingMapsClientToken.isEmpty()) ? "&token=" + BingMapsClientToken : QString("")); - } - break; - case MapType::BingHybrid: - { - QString key = TileXYToQuadKey(pos.X(), pos.Y(), zoom); - return QString("http://ecn.t%1.tiles.virtualearth.net/tiles/h%2.jpeg?g=%3&mkt=%4%5").arg(GetServerNum(pos, 4)).arg(key).arg(VersionBingMaps).arg(language).arg(!(BingMapsClientToken.isNull()|BingMapsClientToken.isEmpty()) ? "&token=" + BingMapsClientToken : QString("")); - } - - case MapType::ArcGIS_Map: - { - // http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_StreetMap_World_2D/MapServer/tile/0/0/0.jpg - - return QString("http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_StreetMap_World_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); - } - break; - case MapType::ArcGIS_Satellite: - { - // http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_Imagery_World_2D/MapServer/tile/1/0/1.jpg - - return QString("http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_Imagery_World_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); - } - break; - case MapType::ArcGIS_ShadedRelief: - { - // http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_ShadedRelief_World_2D/MapServer/tile/1/0/1.jpg - - return QString("http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_ShadedRelief_World_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); - } - break; - case MapType::ArcGIS_Terrain: - { - // http://server.arcgisonline.com/ArcGIS/rest/services/NGS_Topo_US_2D/MapServer/tile/4/3/15 - - return QString("http://server.arcgisonline.com/ArcGIS/rest/services/NGS_Topo_US_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); - } - break; - case MapType::ArcGIS_MapsLT_OrtoFoto: - { - // http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L02/R0000001b/C00000028.jpg - // http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto/MapServer/tile/0/9/13 - // return string.Format("http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L{0:00}/R{1:x8}/C{2:x8}.jpg", zoom, pos.Y(), pos.X()); - // http://dc1.maps.lt/cache/mapslt_ortofoto_512/map/_alllayers/L03/R0000001c/C00000029.jpg - // return string.Format("http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto/MapServer/tile/{0}/{1}/{2}", zoom, pos.Y(), pos.X()); - // http://dc1.maps.lt/cache/mapslt_ortofoto_512/map/_alllayers/L03/R0000001d/C0000002a.jpg - //TODO verificar - return QString("http://dc1.maps.lt/cache/mapslt_ortofoto/map/_alllayers/L%1/R%2/C%3.jpg").arg(zoom,2,10,(QChar)'0').arg(pos.Y(),8,16,(QChar)'0').arg(pos.X(),8,16,(QChar)'0'); - } - break; - case MapType::ArcGIS_MapsLT_Map: - { - // http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L02/R0000001b/C00000028.jpg - // http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto/MapServer/tile/0/9/13 - // return string.Format("http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L{0:00}/R{1:x8}/C{2:x8}.jpg", zoom, pos.Y(), pos.X()); - // http://arcgis.maps.lt/ArcGIS/rest/services/mapslt/MapServer/tile/7/1162/1684.png - // http://dc1.maps.lt/cache/mapslt_512/map/_alllayers/L03/R0000001b/C00000029.png - //TODO verificar - // http://dc1.maps.lt/cache/mapslt/map/_alllayers/L02/R0000001c/C00000029.png - return QString("http://dc1.maps.lt/cache/mapslt/map/_alllayers/L%1/R%2/C%3.png").arg(zoom,2,10,(QChar)'0').arg(pos.Y(),8,16,(QChar)'0').arg(pos.X(),8,16,(QChar)'0'); - } - break; - case MapType::ArcGIS_MapsLT_Map_Labels: - { - //http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto_overlay/MapServer/tile/0/9/13 - //return string.Format("http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto_overlay/MapServer/tile/{0}/{1}/{2}", zoom, pos.Y(), pos.X()); - //http://dc1.maps.lt/cache/mapslt_ortofoto_overlay_512/map/_alllayers/L03/R0000001d/C00000029.png - //TODO verificar - return QString("http://dc1.maps.lt/cache/mapslt_ortofoto_overlay/map/_alllayers/L%1/R%2/C%3.png").arg(zoom,2,10,(QChar)'0').arg(pos.Y(),8,16,(QChar)'0').arg(pos.X(),8,16,(QChar)'0'); - } - break; - case MapType::PergoTurkeyMap: - { - // http://{domain}/{layerName}/{zoomLevel}/{first3LetterOfTileX}/{second3LetterOfTileX}/{third3LetterOfTileX}/{first3LetterOfTileY}/{second3LetterOfTileY}/{third3LetterOfTileXY}.png - - // http://map3.pergo.com.tr/tile/00/000/000/001/000/000/000.png - // That means: Zoom Level: 0 TileX: 1 TileY: 0 - - // http://domain/tile/14/000/019/371/000/011/825.png - // That means: Zoom Level: 14 TileX: 19371 TileY:11825 - - // string x = pos.X().ToString("000000000").Insert(3, "/").Insert(7, "/"); // - 000/000/001 - // string y = pos.Y().ToString("000000000").Insert(3, "/").Insert(7, "/"); // - 000/000/000 - QString x=QString("%1").arg(QString::number(pos.X()),9,(QChar)'0'); - x.insert(3,"/").insert(7,"/"); - QString y=QString("%1").arg(QString::number(pos.Y()),9,(QChar)'0'); - y.insert(3,"/").insert(7,"/"); - //"http://map03.pergo.com.tr/tile/2/000/000/003/000/000/002.png" - return QString("http://map%1.pergo.com.tr/tile/%2/%3/%4.png").arg(GetServerNum(pos, 4)).arg(zoom,2,10,(QChar)'0').arg(x).arg(y); - } - break; - case MapType::SigPacSpainMap: - { - return QString("http://sigpac.mapa.es/kmlserver/raster/%1@3785/%2.%3.%4.img").arg(levelsForSigPacSpainMap[zoom]).arg(zoom).arg(pos.X()).arg((2 << (zoom - 1)) - pos.Y() - 1); - } - break; - - case MapType::YandexMapRu: - { - QString server = "vec"; - - //http://vec01.maps.yandex.ru/tiles?l=map&v=2.10.2&x=1494&y=650&z=11 - - return QString("http://%1").arg(server)+QString("0%2.maps.yandex.ru/tiles?l=map&v=%3&x=%4&y=%5&z=%6").arg(GetServerNum(pos, 4)+1).arg(VersionYandexMap).arg(pos.X()).arg(pos.Y()).arg(zoom); - } - break; - default: - break; + qDebug() << "TryCorrectGoogleVersions, VersionGoogleLabels: " << VersionGoogleLabels; +#endif // DEBUG_URLFACTORY } + reg = QRegExp("\"*http://khm0.google.com/kh/v=(\\d*)", Qt::CaseInsensitive); + if (reg.indexIn(html) != -1) { + QStringList gc = reg.capturedTexts(); + VersionGoogleSatellite = gc[1]; + VersionGoogleSatelliteKorea = VersionGoogleSatellite; + VersionGoogleSatelliteChina = "s@" + VersionGoogleSatellite; - return QString::null; - } - void UrlFactory::GetSecGoogleWords(const Point &pos, QString &sec1, QString &sec2) - { - sec1 = ""; // after &x=... - sec2 = ""; // after &zoom=... - int seclen = ((pos.X() * 3) + pos.Y()) % 8; - sec2 = SecGoogleWord.left(seclen); - if(pos.Y() >= 10000 && pos.Y() < 100000) - { - sec1 = "&s="; + qDebug() << "TryCorrectGoogleVersions, VersionGoogleSatellite: " << VersionGoogleSatellite; } - } - QString UrlFactory::MakeGeocoderUrl(QString keywords) - { - QString key = keywords.replace(' ', '+'); - return QString("http://maps.google.com/maps/geo?q=%1&output=csv&key=%2").arg(key).arg(GoogleMapsAPIKey); - } - QString UrlFactory::MakeReverseGeocoderUrl(internals::PointLatLng &pt,const QString &language) - { - - return QString("http://maps.google.com/maps/geo?hl=%1&ll=%2,%3&output=csv&key=%4").arg(language).arg(QString::number(pt.Lat())).arg(QString::number(pt.Lng())).arg(GoogleMapsAPIKey); - - } - internals::PointLatLng UrlFactory::GetLatLngFromGeodecoder(const QString &keywords, GeoCoderStatusCode::Types &status) - { - return GetLatLngFromGeocoderUrl(MakeGeocoderUrl(keywords),UseGeocoderCache,status); - } - internals::PointLatLng UrlFactory::GetLatLngFromGeocoderUrl(const QString &url, const bool &useCache, GeoCoderStatusCode::Types &status) - { + reg = QRegExp("\"*http://mt0.google.com/vt/lyrs=t@(\\d*),r@(\\d*)", Qt::CaseInsensitive); + if (reg.indexIn(html) != -1) { + QStringList gc = reg.capturedTexts(); + VersionGoogleTerrain = QString("t@%1,r@%2").arg(gc[1]).arg(gc[2]); + VersionGoogleTerrainChina = VersionGoogleTerrain; #ifdef DEBUG_URLFACTORY - qDebug()<<"Entered GetLatLngFromGeocoderUrl:"; -#endif //DEBUG_URLFACTORY - status = GeoCoderStatusCode::Unknow; - internals::PointLatLng ret(0,0); - QString urlEnd = url.mid(url.indexOf("geo?q=")+6); - urlEnd.replace( QRegExp( - "[^" - "A-Z,a-z,0-9," - "\\^,\\&,\\',\\@," - "\\{,\\},\\[,\\]," - "\\,,\\$,\\=,\\!," - "\\-,\\#,\\(,\\)," - "\\%,\\.,\\+,\\~,\\_" - "]"), "_" ); - - QString geo = useCache ? Cache::Instance()->GetGeocoderFromCache(urlEnd) : ""; - - if(geo.isNull()|geo.isEmpty()) - { -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl:Not in cache going internet"; -#endif //DEBUG_URLFACTORY - QNetworkReply *reply; - QNetworkRequest qheader; - QNetworkAccessManager network; - network.setProxy(Proxy); - qheader.setUrl(QUrl(url)); - qheader.setRawHeader("User-Agent",UserAgent); - reply=network.get(qheader); -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl:URL="<isFinished()) || (time.elapsed()>(6*Timeout))) ){QCoreApplication::processEvents(QEventLoop::AllEvents);} -#ifdef DEBUG_URLFACTORY - qDebug()<<"Finished?"<error()<<" abort?"<<(time.elapsed()>Timeout*6); -#endif //DEBUG_URLFACTORY - if( (reply->error()!=QNetworkReply::NoError) | (time.elapsed()>Timeout*6)) - { -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl::Network error"; -#endif //DEBUG_URLFACTORY - return internals::PointLatLng(0,0); - } - { -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl:Reply ok"; -#endif //DEBUG_URLFACTORY - geo=reply->readAll(); - - - // cache geocoding - if(useCache && geo.startsWith("200")) - { - Cache::Instance()->CacheGeocoder(urlEnd, geo); - } - } - reply->deleteLater(); + qDebug() << "TryCorrectGoogleVersions, VersionGoogleTerrain: " << VersionGoogleTerrain; +#endif // DEBUG_URLFACTORY } - - - // parse values - // true : 200,4,56.1451640,22.0681787 - // false: 602,0,0,0 - { - QStringList values = geo.split(','); - if(values.count() == 4) - { - status = (GeoCoderStatusCode::Types) QString(values[0]).toInt(); - if(status == GeoCoderStatusCode::G_GEO_SUCCESS) - { - double lat = QString(values[2]).toDouble(); - double lng = QString(values[3]).toDouble(); - - ret = internals::PointLatLng(lat, lng); -#ifdef DEBUG_URLFACTORY - qDebug()<<"Lat="<GetPlacemarkFromCache(urlEnd) : ""; - - if(reverse.isNull()|reverse.isEmpty()) - { -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl:Not in cache going internet"; -#endif //DEBUG_URLFACTORY - QNetworkReply *reply; - QNetworkRequest qheader; - QNetworkAccessManager network; - network.setProxy(Proxy); - qheader.setUrl(QUrl(url)); - qheader.setRawHeader("User-Agent",UserAgent); - reply=network.get(qheader); -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl:URL="<isFinished()) || (time.elapsed()>(6*Timeout))) ){QCoreApplication::processEvents(QEventLoop::AllEvents);} -#ifdef DEBUG_URLFACTORY - qDebug()<<"Finished?"<error()<<" abort?"<<(time.elapsed()>Timeout*6); -#endif //DEBUG_URLFACTORY - if( (reply->error()!=QNetworkReply::NoError) | (time.elapsed()>Timeout*6)) - { -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl::Network error"; -#endif //DEBUG_URLFACTORY - return ret; - } - { -#ifdef DEBUG_URLFACTORY - qDebug()<<"GetLatLngFromGeocoderUrl:Reply ok"; -#endif //DEBUG_URLFACTORY - QByteArray a=(reply->readAll()); - QTextCodec *codec = QTextCodec::codecForName("UTF-8"); - reverse = codec->toUnicode(a); -#ifdef DEBUG_URLFACTORY - qDebug()<CachePlacemark(urlEnd, reverse); - } - } - reply->deleteLater(); - } - - - // parse values - // true : 200,4,56.1451640,22.0681787 - // false: 602,0,0,0 - if(reverse.startsWith("200")) - { - QString acc = reverse.left(reverse.indexOf('\"')); - ret = Placemark(reverse.remove(reverse.indexOf('\"'))); - ret.SetAccuracy ((int) (( (QString) acc.split(',')[1]).toInt()) ); - - } - return ret; - } - double UrlFactory::GetDistance(internals::PointLatLng p1, internals::PointLatLng p2) - { - double dLat1InRad = p1.Lat() * (M_PI / 180); - double dLong1InRad = p1.Lng() * (M_PI / 180); - double dLat2InRad = p2.Lat() * (M_PI / 180); - double dLong2InRad = p2.Lng() * (M_PI / 180); - double dLongitude = dLong2InRad - dLong1InRad; - double dLatitude = dLat2InRad - dLat1InRad; - double a = pow(sin(dLatitude / 2), 2) + cos(dLat1InRad) * cos(dLat2InRad) * pow(sin(dLongitude / 2), 2); - double c = 2 * atan2(sqrt(a), sqrt(1 - a)); - double dDistance = EarthRadiusKm * c; - return dDistance; + reply->deleteLater(); } } + +QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, const int &zoom, const QString &language) +{ +#ifdef DEBUG_URLFACTORY + qDebug() << "Entered MakeImageUrl"; +#endif // DEBUG_URLFACTORY + switch (type) { + case MapType::GoogleMap: + { + QString server = "mt"; + QString request = "vt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + TryCorrectGoogleVersions(); + + return QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleMap).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleSatellite: + { + QString server = "khm"; + QString request = "kh"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + TryCorrectGoogleVersions(); + return QString("http://%1%2.google.com/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatellite).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleLabels: + { + QString server = "mt"; + QString request = "vt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + TryCorrectGoogleVersions(); + + return QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabels).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleTerrain: + { + QString server = "mt"; + QString request = "vt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + TryCorrectGoogleVersions(); + return QString("http://%1%2.google.com/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleTerrain).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleMapChina: + { + QString server = "mt"; + QString request = "vt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + TryCorrectGoogleVersions(); + // http://mt0.google.cn/vt/v=w2.101&hl=zh-CN&gl=cn&x=12&y=6&z=4&s=Ga + + return QString("http://%1%2.google.cn/%3/lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleMapChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleSatelliteChina: + { + QString server = "mt"; + QString request = "vt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + // TryCorrectGoogleVersions(); + // http://khm0.google.cn/kh/v=46&x=12&y=6&z=4&s=Ga + + return QString("http://%1%2.google.cn/%3/lyrs=%4&gl=cn&x=%5%6&y=%7&z=%8&s=%9").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatelliteChina).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleLabelsChina: + { + QString server = "mt"; + QString request = "vt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + TryCorrectGoogleVersions(); + // http://mt0.google.cn/vt/v=w2t.110&hl=zh-CN&gl=cn&x=12&y=6&z=4&s=Ga + + return QString("http://%1%2.google.cn/%3/imgtp=png32&lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabelsChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleTerrainChina: + { + QString server = "mt"; + QString request = "vt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + TryCorrectGoogleVersions(); + // http://mt0.google.cn/vt/v=w2p.110&hl=zh-CN&gl=cn&x=12&y=6&z=4&s=Ga + + return QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleTerrainChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleMapKorea: + { + QString server = "mt"; + QString request = "mt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + + // http://mt3.gmaptiles.co.kr/mt/v=kr1.11&hl=lt&x=109&y=49&z=7&s= + + QString ret = QString("http://%1%2.gmaptiles.co.kr/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleMapKorea).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + return ret; + } + break; + case MapType::GoogleSatelliteKorea: + { + QString server = "khm"; + QString request = "kh"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + + // http://khm1.google.co.kr/kh/v=54&x=109&y=49&z=7&s= + + return QString("http://%1%2.google.co.kr/%3/v=%4&x=%5%6&y=%7&z=%8&s=%9").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatelliteKorea).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::GoogleLabelsKorea: + { + QString server = "mt"; + QString request = "mt"; + QString sec1 = ""; // after &x=... + QString sec2 = ""; // after &zoom=... + GetSecGoogleWords(pos, sec1, sec2); + + // http://mt1.gmaptiles.co.kr/mt/v=kr1t.11&hl=lt&x=109&y=50&z=7&s=G + + return QString("http://%1%2.gmaptiles.co.kr/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabelsKorea).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); + } + break; + case MapType::YahooMap: + { + return QString("http://maps%1.yimg.com/hx/tl?v=%2&.intl=%3&x=%4&y=%5&z=%6&r=1").arg(((GetServerNum(pos, 2)) + 1)).arg(VersionYahooMap).arg(language).arg(pos.X()).arg((((1 << zoom) >> 1) - 1 - pos.Y())).arg((zoom + 1)); + } + + case MapType::YahooSatellite: + { + return QString("http://maps%1.yimg.com/ae/ximg?v=%2&t=a&s=256&.intl=%3&x=%4&y=%5&z=%6&r=1").arg("3").arg(VersionYahooSatellite).arg(language).arg(pos.X()).arg(((1 << zoom) >> 1) - 1 - pos.Y()).arg(zoom + 1); + } + break; + case MapType::YahooLabels: + { + return QString("http://maps%1.yimg.com/hx/tl?v=%2&t=h&.intl=%3&x=%4&y=%5&z=%6&r=1").arg("1").arg(VersionYahooLabels).arg(language).arg(pos.X()).arg(((1 << zoom) >> 1) - 1 - pos.Y()).arg(zoom + 1); + } + break; + case MapType::OpenStreetMap: + { + char letter = "abc"[GetServerNum(pos, 3)]; + return QString("http://%1.tile.openstreetmap.org/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y()); + } + break; + case MapType::OpenStreetOsm: + { + char letter = "abc"[GetServerNum(pos, 3)]; + return QString("http://%1.tah.openstreetmap.org/Tiles/tile/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y()); + } + break; + case MapType::OpenStreetMapSurfer: + { + // http://tiles1.mapsurfer.net/tms_r.ashx?x=37378&y=20826&z=16 + + return QString("http://tiles1.mapsurfer.net/tms_r.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom); + } + break; + case MapType::OpenStreetMapSurferTerrain: + { + // http://tiles2.mapsurfer.net/tms_t.ashx?x=9346&y=5209&z=14 + + return QString("http://tiles2.mapsurfer.net/tms_t.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom); + } + break; + case MapType::BingMap: + { + QString key = TileXYToQuadKey(pos.X(), pos.Y(), zoom); + return QString("http://ecn.t%1.tiles.virtualearth.net/tiles/r%2.png?g=%3&mkt=%4%5").arg(GetServerNum(pos, 4)).arg(key).arg(VersionBingMaps).arg(language).arg(!(BingMapsClientToken.isNull() | BingMapsClientToken.isEmpty()) ? "&token=" + BingMapsClientToken : QString("")); + } + break; + case MapType::BingSatellite: + { + QString key = TileXYToQuadKey(pos.X(), pos.Y(), zoom); + return QString("http://ecn.t%1.tiles.virtualearth.net/tiles/a%2.jpeg?g=%3&mkt=%4%5").arg(GetServerNum(pos, 4)).arg(key).arg(VersionBingMaps).arg(language).arg(!(BingMapsClientToken.isNull() | BingMapsClientToken.isEmpty()) ? "&token=" + BingMapsClientToken : QString("")); + } + break; + case MapType::BingHybrid: + { + QString key = TileXYToQuadKey(pos.X(), pos.Y(), zoom); + return QString("http://ecn.t%1.tiles.virtualearth.net/tiles/h%2.jpeg?g=%3&mkt=%4%5").arg(GetServerNum(pos, 4)).arg(key).arg(VersionBingMaps).arg(language).arg(!(BingMapsClientToken.isNull() | BingMapsClientToken.isEmpty()) ? "&token=" + BingMapsClientToken : QString("")); + } + + case MapType::ArcGIS_Map: + { + // http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_StreetMap_World_2D/MapServer/tile/0/0/0.jpg + + return QString("http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_StreetMap_World_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); + } + break; + case MapType::ArcGIS_Satellite: + { + // http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_Imagery_World_2D/MapServer/tile/1/0/1.jpg + + return QString("http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_Imagery_World_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); + } + break; + case MapType::ArcGIS_ShadedRelief: + { + // http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_ShadedRelief_World_2D/MapServer/tile/1/0/1.jpg + + return QString("http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_ShadedRelief_World_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); + } + break; + case MapType::ArcGIS_Terrain: + { + // http://server.arcgisonline.com/ArcGIS/rest/services/NGS_Topo_US_2D/MapServer/tile/4/3/15 + + return QString("http://server.arcgisonline.com/ArcGIS/rest/services/NGS_Topo_US_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X()); + } + break; + case MapType::ArcGIS_MapsLT_OrtoFoto: + { + // http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L02/R0000001b/C00000028.jpg + // http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto/MapServer/tile/0/9/13 + // return string.Format("http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L{0:00}/R{1:x8}/C{2:x8}.jpg", zoom, pos.Y(), pos.X()); + // http://dc1.maps.lt/cache/mapslt_ortofoto_512/map/_alllayers/L03/R0000001c/C00000029.jpg + // return string.Format("http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto/MapServer/tile/{0}/{1}/{2}", zoom, pos.Y(), pos.X()); + // http://dc1.maps.lt/cache/mapslt_ortofoto_512/map/_alllayers/L03/R0000001d/C0000002a.jpg + // TODO verificar + return QString("http://dc1.maps.lt/cache/mapslt_ortofoto/map/_alllayers/L%1/R%2/C%3.jpg").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0'); + } + break; + case MapType::ArcGIS_MapsLT_Map: + { + // http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L02/R0000001b/C00000028.jpg + // http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto/MapServer/tile/0/9/13 + // return string.Format("http://www.maps.lt/ortofoto/mapslt_ortofoto_vector_512/map/_alllayers/L{0:00}/R{1:x8}/C{2:x8}.jpg", zoom, pos.Y(), pos.X()); + // http://arcgis.maps.lt/ArcGIS/rest/services/mapslt/MapServer/tile/7/1162/1684.png + // http://dc1.maps.lt/cache/mapslt_512/map/_alllayers/L03/R0000001b/C00000029.png + // TODO verificar + // http://dc1.maps.lt/cache/mapslt/map/_alllayers/L02/R0000001c/C00000029.png + return QString("http://dc1.maps.lt/cache/mapslt/map/_alllayers/L%1/R%2/C%3.png").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0'); + } + break; + case MapType::ArcGIS_MapsLT_Map_Labels: + { + // http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto_overlay/MapServer/tile/0/9/13 + // return string.Format("http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto_overlay/MapServer/tile/{0}/{1}/{2}", zoom, pos.Y(), pos.X()); + // http://dc1.maps.lt/cache/mapslt_ortofoto_overlay_512/map/_alllayers/L03/R0000001d/C00000029.png + // TODO verificar + return QString("http://dc1.maps.lt/cache/mapslt_ortofoto_overlay/map/_alllayers/L%1/R%2/C%3.png").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0'); + } + break; + case MapType::PergoTurkeyMap: + { + // http://{domain}/{layerName}/{zoomLevel}/{first3LetterOfTileX}/{second3LetterOfTileX}/{third3LetterOfTileX}/{first3LetterOfTileY}/{second3LetterOfTileY}/{third3LetterOfTileXY}.png + + // http://map3.pergo.com.tr/tile/00/000/000/001/000/000/000.png + // That means: Zoom Level: 0 TileX: 1 TileY: 0 + + // http://domain/tile/14/000/019/371/000/011/825.png + // That means: Zoom Level: 14 TileX: 19371 TileY:11825 + + // string x = pos.X().ToString("000000000").Insert(3, "/").Insert(7, "/"); // - 000/000/001 + // string y = pos.Y().ToString("000000000").Insert(3, "/").Insert(7, "/"); // - 000/000/000 + QString x = QString("%1").arg(QString::number(pos.X()), 9, (QChar)'0'); + x.insert(3, "/").insert(7, "/"); + QString y = QString("%1").arg(QString::number(pos.Y()), 9, (QChar)'0'); + y.insert(3, "/").insert(7, "/"); + // "http://map03.pergo.com.tr/tile/2/000/000/003/000/000/002.png" + return QString("http://map%1.pergo.com.tr/tile/%2/%3/%4.png").arg(GetServerNum(pos, 4)).arg(zoom, 2, 10, (QChar)'0').arg(x).arg(y); + } + break; + case MapType::SigPacSpainMap: + { + return QString("http://sigpac.mapa.es/kmlserver/raster/%1@3785/%2.%3.%4.img").arg(levelsForSigPacSpainMap[zoom]).arg(zoom).arg(pos.X()).arg((2 << (zoom - 1)) - pos.Y() - 1); + } + break; + + case MapType::YandexMapRu: + { + QString server = "vec"; + + // http://vec01.maps.yandex.ru/tiles?l=map&v=2.10.2&x=1494&y=650&z=11 + + return QString("http://%1").arg(server) + QString("0%2.maps.yandex.ru/tiles?l=map&v=%3&x=%4&y=%5&z=%6").arg(GetServerNum(pos, 4) + 1).arg(VersionYandexMap).arg(pos.X()).arg(pos.Y()).arg(zoom); + } + break; + default: + break; + } + + return QString::null; +} +void UrlFactory::GetSecGoogleWords(const Point &pos, QString &sec1, QString &sec2) +{ + sec1 = ""; // after &x=... + sec2 = ""; // after &zoom=... + int seclen = ((pos.X() * 3) + pos.Y()) % 8; + sec2 = SecGoogleWord.left(seclen); + if (pos.Y() >= 10000 && pos.Y() < 100000) { + sec1 = "&s="; + } +} +QString UrlFactory::MakeGeocoderUrl(QString keywords) +{ + QString key = keywords.replace(' ', '+'); + + return QString("http://maps.google.com/maps/geo?q=%1&output=csv&key=%2").arg(key).arg(GoogleMapsAPIKey); +} +QString UrlFactory::MakeReverseGeocoderUrl(internals::PointLatLng &pt, const QString &language) +{ + return QString("http://maps.google.com/maps/geo?hl=%1&ll=%2,%3&output=csv&key=%4").arg(language).arg(QString::number(pt.Lat())).arg(QString::number(pt.Lng())).arg(GoogleMapsAPIKey); +} +internals::PointLatLng UrlFactory::GetLatLngFromGeodecoder(const QString &keywords, GeoCoderStatusCode::Types &status) +{ + return GetLatLngFromGeocoderUrl(MakeGeocoderUrl(keywords), UseGeocoderCache, status); +} +internals::PointLatLng UrlFactory::GetLatLngFromGeocoderUrl(const QString &url, const bool &useCache, GeoCoderStatusCode::Types &status) +{ +#ifdef DEBUG_URLFACTORY + qDebug() << "Entered GetLatLngFromGeocoderUrl:"; +#endif // DEBUG_URLFACTORY + status = GeoCoderStatusCode::Unknow; + internals::PointLatLng ret(0, 0); + QString urlEnd = url.mid(url.indexOf("geo?q=") + 6); + urlEnd.replace(QRegExp( + "[^" + "A-Z,a-z,0-9," + "\\^,\\&,\\',\\@," + "\\{,\\},\\[,\\]," + "\\,,\\$,\\=,\\!," + "\\-,\\#,\\(,\\)," + "\\%,\\.,\\+,\\~,\\_" + "]"), "_"); + + QString geo = useCache ? Cache::Instance()->GetGeocoderFromCache(urlEnd) : ""; + + if (geo.isNull() | geo.isEmpty()) { +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl:Not in cache going internet"; +#endif // DEBUG_URLFACTORY + QNetworkReply *reply; + QNetworkRequest qheader; + QNetworkAccessManager network; + network.setProxy(Proxy); + qheader.setUrl(QUrl(url)); + qheader.setRawHeader("User-Agent", UserAgent); + reply = network.get(qheader); +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl:URL=" << url; +#endif // DEBUG_URLFACTORY + QTime time; + time.start(); + while ((!(reply->isFinished()) || (time.elapsed() > (6 * Timeout)))) { + QCoreApplication::processEvents(QEventLoop::AllEvents); + } +#ifdef DEBUG_URLFACTORY + qDebug() << "Finished?" << reply->error() << " abort?" << (time.elapsed() > Timeout * 6); +#endif // DEBUG_URLFACTORY + if ((reply->error() != QNetworkReply::NoError) | (time.elapsed() > Timeout * 6)) { +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl::Network error"; +#endif // DEBUG_URLFACTORY + return internals::PointLatLng(0, 0); + } + { +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl:Reply ok"; +#endif // DEBUG_URLFACTORY + geo = reply->readAll(); + + + // cache geocoding + if (useCache && geo.startsWith("200")) { + Cache::Instance()->CacheGeocoder(urlEnd, geo); + } + } + reply->deleteLater(); + } + + + // parse values + // true : 200,4,56.1451640,22.0681787 + // false: 602,0,0,0 + { + QStringList values = geo.split(','); + if (values.count() == 4) { + status = (GeoCoderStatusCode::Types)QString(values[0]).toInt(); + if (status == GeoCoderStatusCode::G_GEO_SUCCESS) { + double lat = QString(values[2]).toDouble(); + double lng = QString(values[3]).toDouble(); + + ret = internals::PointLatLng(lat, lng); +#ifdef DEBUG_URLFACTORY + qDebug() << "Lat=" << lat << " Lng=" << lng; +#endif // DEBUG_URLFACTORY + } + } + } + return ret; +} + +Placemark UrlFactory::GetPlacemarkFromGeocoder(internals::PointLatLng location) +{ + return GetPlacemarkFromReverseGeocoderUrl(MakeReverseGeocoderUrl(location, LanguageStr), UsePlacemarkCache); +} + +Placemark UrlFactory::GetPlacemarkFromReverseGeocoderUrl(const QString &url, const bool &useCache) +{ + Placemark ret(""); + +#ifdef DEBUG_URLFACTORY + qDebug() << "Entered GetPlacemarkFromReverseGeocoderUrl:"; +#endif // DEBUG_URLFACTORY + // status = GeoCoderStatusCode::Unknow; + QString urlEnd = url.right(url.indexOf("geo?hl=")); + urlEnd.replace(QRegExp( + "[^" + "A-Z,a-z,0-9," + "\\^,\\&,\\',\\@," + "\\{,\\},\\[,\\]," + "\\,,\\$,\\=,\\!," + "\\-,\\#,\\(,\\)," + "\\%,\\.,\\+,\\~,\\_" + "]"), "_"); + + QString reverse = useCache ? Cache::Instance()->GetPlacemarkFromCache(urlEnd) : ""; + + if (reverse.isNull() | reverse.isEmpty()) { +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl:Not in cache going internet"; +#endif // DEBUG_URLFACTORY + QNetworkReply *reply; + QNetworkRequest qheader; + QNetworkAccessManager network; + network.setProxy(Proxy); + qheader.setUrl(QUrl(url)); + qheader.setRawHeader("User-Agent", UserAgent); + reply = network.get(qheader); +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl:URL=" << url; +#endif // DEBUG_URLFACTORY + QTime time; + time.start(); + while ((!(reply->isFinished()) || (time.elapsed() > (6 * Timeout)))) { + QCoreApplication::processEvents(QEventLoop::AllEvents); + } +#ifdef DEBUG_URLFACTORY + qDebug() << "Finished?" << reply->error() << " abort?" << (time.elapsed() > Timeout * 6); +#endif // DEBUG_URLFACTORY + if ((reply->error() != QNetworkReply::NoError) | (time.elapsed() > Timeout * 6)) { +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl::Network error"; +#endif // DEBUG_URLFACTORY + return ret; + } + { +#ifdef DEBUG_URLFACTORY + qDebug() << "GetLatLngFromGeocoderUrl:Reply ok"; +#endif // DEBUG_URLFACTORY + QByteArray a = (reply->readAll()); + QTextCodec *codec = QTextCodec::codecForName("UTF-8"); + reverse = codec->toUnicode(a); +#ifdef DEBUG_URLFACTORY + qDebug() << reverse; +#endif // DEBUG_URLFACTORY + // cache geocoding + if (useCache && reverse.startsWith("200")) { + Cache::Instance()->CachePlacemark(urlEnd, reverse); + } + } + reply->deleteLater(); + } + + + // parse values + // true : 200,4,56.1451640,22.0681787 + // false: 602,0,0,0 + if (reverse.startsWith("200")) { + QString acc = reverse.left(reverse.indexOf('\"')); + ret = Placemark(reverse.remove(reverse.indexOf('\"'))); + ret.SetAccuracy((int)(((QString)acc.split(',')[1]).toInt())); + } + return ret; +} +double UrlFactory::GetDistance(internals::PointLatLng p1, internals::PointLatLng p2) +{ + double dLat1InRad = p1.Lat() * (M_PI / 180); + double dLong1InRad = p1.Lng() * (M_PI / 180); + double dLat2InRad = p2.Lat() * (M_PI / 180); + double dLong2InRad = p2.Lng() * (M_PI / 180); + double dLongitude = dLong2InRad - dLong1InRad; + double dLatitude = dLat2InRad - dLat1InRad; + double a = pow(sin(dLatitude / 2), 2) + cos(dLat1InRad) * cos(dLat2InRad) * pow(sin(dLongitude / 2), 2); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + double dDistance = EarthRadiusKm * c; + + return dDistance; +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.h index a40947cc2..ace8b52d3 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file urlfactory.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file urlfactory.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef URLFACTORY_H #define URLFACTORY_H @@ -45,44 +45,42 @@ #include "cmath" namespace core { - class UrlFactory: public QObject,public ProviderStrings - { - Q_OBJECT - public: - /// - /// Gets or sets the value of the User-agent HTTP header. - /// - QByteArray UserAgent; - QNetworkProxy Proxy; - UrlFactory(); - ~UrlFactory(); - QString MakeImageUrl(const MapType::Types &type,const core::Point &pos,const int &zoom,const QString &language); - internals::PointLatLng GetLatLngFromGeodecoder(const QString &keywords,GeoCoderStatusCode::Types &status); - Placemark GetPlacemarkFromGeocoder(internals::PointLatLng location); - int Timeout; - private: - void GetSecGoogleWords(const core::Point &pos, QString &sec1, QString &sec2); - int GetServerNum(const core::Point &pos,const int &max) const; - void TryCorrectGoogleVersions(); - bool isCorrectedGoogleVersions; - QString TileXYToQuadKey(const int &tileX,const int &tileY,const int &levelOfDetail) const; - bool CorrectGoogleVersions; - bool UseGeocoderCache; //TODO GetSet - bool UsePlacemarkCache;//TODO GetSet - static const double EarthRadiusKm; - double GetDistance(internals::PointLatLng p1,internals::PointLatLng p2); - QMutex mutex; - - protected: - static short timelapse; - QString LanguageStr; - bool IsCorrectGoogleVersions(); - void setIsCorrectGoogleVersions(bool value); - QString MakeGeocoderUrl(QString keywords); - QString MakeReverseGeocoderUrl(internals::PointLatLng &pt,const QString &language); - internals::PointLatLng GetLatLngFromGeocoderUrl(const QString &url,const bool &useCache, GeoCoderStatusCode::Types &status); - Placemark GetPlacemarkFromReverseGeocoderUrl(const QString &url,const bool &useCache); - }; +class UrlFactory : public QObject, public ProviderStrings { + Q_OBJECT +public: + /// + /// Gets or sets the value of the User-agent HTTP header. + /// + QByteArray UserAgent; + QNetworkProxy Proxy; + UrlFactory(); + ~UrlFactory(); + QString MakeImageUrl(const MapType::Types &type, const core::Point &pos, const int &zoom, const QString &language); + internals::PointLatLng GetLatLngFromGeodecoder(const QString &keywords, GeoCoderStatusCode::Types &status); + Placemark GetPlacemarkFromGeocoder(internals::PointLatLng location); + int Timeout; +private: + void GetSecGoogleWords(const core::Point &pos, QString &sec1, QString &sec2); + int GetServerNum(const core::Point &pos, const int &max) const; + void TryCorrectGoogleVersions(); + bool isCorrectedGoogleVersions; + QString TileXYToQuadKey(const int &tileX, const int &tileY, const int &levelOfDetail) const; + bool CorrectGoogleVersions; + bool UseGeocoderCache; // TODO GetSet + bool UsePlacemarkCache; // TODO GetSet + static const double EarthRadiusKm; + double GetDistance(internals::PointLatLng p1, internals::PointLatLng p2); + QMutex mutex; +protected: + static short timelapse; + QString LanguageStr; + bool IsCorrectGoogleVersions(); + void setIsCorrectGoogleVersions(bool value); + QString MakeGeocoderUrl(QString keywords); + QString MakeReverseGeocoderUrl(internals::PointLatLng &pt, const QString &language); + internals::PointLatLng GetLatLngFromGeocoderUrl(const QString &url, const bool &useCache, GeoCoderStatusCode::Types &status); + Placemark GetPlacemarkFromReverseGeocoderUrl(const QString &url, const bool &useCache); +}; } #endif // URLFACTORY_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/MouseWheelZoomType.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/MouseWheelZoomType.cpp index 17fe3b1cd..efdbd2ba7 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/MouseWheelZoomType.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/MouseWheelZoomType.cpp @@ -1,32 +1,30 @@ /** -****************************************************************************** -* -* @file MouseWheelZoomType.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file MouseWheelZoomType.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "mousewheelzoomtype.h" - -namespace internals { -} +namespace internals {} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/copyrightstrings.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/copyrightstrings.h index 36336bf83..59ac8085f 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/copyrightstrings.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/copyrightstrings.h @@ -1,41 +1,40 @@ /** -****************************************************************************** -* -* @file copyrightstrings.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file copyrightstrings.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef COPYRIGHTSTRINGS_H #define COPYRIGHTSTRINGS_H #include #include - -namespace internals { -static const QString googleCopyright = QString("%1 Google - Map data %1 Tele Atlas, Imagery %1 TerraMetrics").arg(QDate::currentDate().year()); -static const QString openStreetMapCopyright = QString(" OpenStreetMap - Map data %1 OpenStreetMap").arg(QDate::currentDate().year()); -static const QString yahooMapCopyright = QString(" Yahoo! Inc. - Map data & Imagery %1 NAVTEQ").arg(QDate::currentDate().year()); -static const QString virtualEarthCopyright = QString("%1 Microsoft Corporation, %1 NAVTEQ, %1 Image courtesy of NASA").arg(QDate::currentDate().year()); -static const QString arcGisCopyright = QString("%1 ESRI - Map data %1 ArcGIS").arg(QDate::currentDate().year()); +namespace internals { +static const QString googleCopyright = QString("%1 Google - Map data %1 Tele Atlas, Imagery %1 TerraMetrics").arg(QDate::currentDate().year()); +static const QString openStreetMapCopyright = QString(" OpenStreetMap - Map data %1 OpenStreetMap").arg(QDate::currentDate().year()); +static const QString yahooMapCopyright = QString(" Yahoo! Inc. - Map data & Imagery %1 NAVTEQ").arg(QDate::currentDate().year()); +static const QString virtualEarthCopyright = QString("%1 Microsoft Corporation, %1 NAVTEQ, %1 Image courtesy of NASA").arg(QDate::currentDate().year()); +static const QString arcGisCopyright = QString("%1 ESRI - Map data %1 ArcGIS").arg(QDate::currentDate().year()); } #endif // COPYRIGHTSTRINGS_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.cpp index b484f2537..0d81a8bdf 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.cpp @@ -1,180 +1,163 @@ /** -****************************************************************************** -* -* @file core.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file core.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "core.h" #ifdef DEBUG_CORE -qlonglong internals::Core::debugcounter=0; +qlonglong internals::Core::debugcounter = 0; #endif using namespace projections; namespace internals { - Core::Core():MouseWheelZooming(false),currentPosition(0,0),currentPositionPixel(0,0),LastLocationInBounds(-1,-1),sizeOfMapArea(0,0) - ,minOfTiles(0,0),maxOfTiles(0,0),zoom(0),isDragging(false),TooltipTextPadding(10,10),loaderLimit(5),maxzoom(21),started(false),runningThreads(0) - { - mousewheelzoomtype=MouseWheelZoomType::MousePositionAndCenter; - SetProjection(new MercatorProjection()); - this->setAutoDelete(false); - ProcessLoadTaskCallback.setMaxThreadCount(10); - renderOffset=Point(0,0); - dragPoint=Point(0,0); - CanDragMap=true; - tilesToload=0; - OPMaps::Instance(); - } - Core::~Core() - { - ProcessLoadTaskCallback.waitForDone(); - } +Core::Core() : MouseWheelZooming(false), currentPosition(0, 0), currentPositionPixel(0, 0), LastLocationInBounds(-1, -1), sizeOfMapArea(0, 0) + , minOfTiles(0, 0), maxOfTiles(0, 0), zoom(0), isDragging(false), TooltipTextPadding(10, 10), loaderLimit(5), maxzoom(21), started(false), runningThreads(0) +{ + mousewheelzoomtype = MouseWheelZoomType::MousePositionAndCenter; + SetProjection(new MercatorProjection()); + this->setAutoDelete(false); + ProcessLoadTaskCallback.setMaxThreadCount(10); + renderOffset = Point(0, 0); + dragPoint = Point(0, 0); + CanDragMap = true; + tilesToload = 0; + OPMaps::Instance(); +} +Core::~Core() +{ + ProcessLoadTaskCallback.waitForDone(); +} - void Core::run() - { - MrunningThreads.lock(); - ++runningThreads; - MrunningThreads.unlock(); +void Core::run() +{ + MrunningThreads.lock(); + ++runningThreads; + MrunningThreads.unlock(); #ifdef DEBUG_CORE - qlonglong debug; - Mdebug.lock(); - debug=++debugcounter; - Mdebug.unlock(); - qDebug()<<"core:run"<<" ID="<GetPos().ToString()<<" now has "<Overlays.count()<<" overlays"<<" ID="<GetPos().ToString() << " now has " << t->Overlays.count() << " overlays" << " ID=" << debug; +#endif // DEBUG_CORE } Moverlays.unlock(); break; - } - else if(OPMaps::Instance()->RetryLoadTile > 0) - { + } else if (OPMaps::Instance()->RetryLoadTile > 0) { #ifdef DEBUG_CORE - qDebug()<<"ProcessLoadTask: " << task.ToString()<< " -> empty tile, retry " << retry<<" ID="< empty tile, retry " << retry << " ID=" << debug;; +#endif // DEBUG_CORE { QWaitCondition wait; QMutex m; m.lock(); - wait.wait(&m,500); + wait.wait(&m, 500); } } - } - while(++retry < OPMaps::Instance()->RetryLoadTile); + } while (++retry < OPMaps::Instance()->RetryLoadTile); } - if(t->Overlays.count() > 0) - { - Matrix.SetTileAt(task.Pos,t); + if (t->Overlays.count() > 0) { + Matrix.SetTileAt(task.Pos, t); emit OnNeedInvalidation(); #ifdef DEBUG_CORE - qDebug()<<"Core::run add tile "<GetPos().ToString()<<" to matrix index "<Overlays.count()<<" ID="<GetPos().ToString() << " to matrix index " << task.Pos.ToString() << " ID=" << debug; + qDebug() << "Core::run matrix index " << task.Pos.ToString() << " as tile with " << Matrix.TileAt(task.Pos)->Overlays.count() << " ID=" << debug; +#endif // DEBUG_CORE + } else { // emit OnTilesStillToLoad(tilesToload); delete t; @@ -189,8 +172,7 @@ namespace internals { { // last buddy cleans stuff ;} - if(last) - { + if (last) { OPMaps::Instance()->kiberCacheLock.lockForWrite(); OPMaps::Instance()->TilesInMemory.RemoveMemoryOverload(); OPMaps::Instance()->kiberCacheLock.unlock(); @@ -206,505 +188,460 @@ namespace internals { emit OnNeedInvalidation(); - } } - - - } #ifdef DEBUG_CORE - qDebug()<<"loaderLimit release:"+loaderLimit.available()<<" ID="<Type()!="LKS94Projection") - { - SetProjection(new LKS94Projection()); - maxzoom=11; - } - } - break; - - case MapType::PergoTurkeyMap: - { - if(Projection()->Type()!="PlateCarreeProjectionPergo") - { - SetProjection(new PlateCarreeProjectionPergo()); - maxzoom=17; - } - } - break; - - case MapType::YandexMapRu: - { - if(Projection()->Type()!="MercatorProjectionYandex") - { - SetProjection(new MercatorProjectionYandex()); - maxzoom=13; - } - } - break; - - default: - { - if(Projection()->Type()!="MercatorProjection") - { - SetProjection(new MercatorProjection()); - maxzoom=21; - } - } - break; - } - - minOfTiles = Projection()->GetTileMatrixMinXY(Zoom()); - maxOfTiles = Projection()->GetTileMatrixMaxXY(Zoom()); - SetCurrentPositionGPixel(Projection()->FromLatLngToPixel(CurrentPosition(), Zoom())); - - if(started) - { - CancelAsyncTasks(); - OnMapSizeChanged(Width, Height); - GoToCurrentPosition(); - ReloadMap(); - GoToCurrentPosition(); - emit OnMapTypeChanged(value); - - } - } - - } - void Core::StartSystem() - { - if(!started) - { - started = true; - - ReloadMap(); - GoToCurrentPosition(); - } - } - - void Core::UpdateCenterTileXYLocation() - { - PointLatLng center = FromLocalToLatLng(Width/2, Height/2); - Point centerPixel = Projection()->FromLatLngToPixel(center, Zoom()); - centerTileXYLocation = Projection()->FromPixelToTileXY(centerPixel); - } - - void Core::OnMapSizeChanged(int const& width, int const& height) - { - Width = width; - Height = height; - - sizeOfMapArea.SetWidth(1 + (Width/Projection()->TileSize().Width())/2); - sizeOfMapArea.SetHeight(1 + (Height/Projection()->TileSize().Height())/2); - - UpdateCenterTileXYLocation(); - - if(started) - { +void Core::SetZoom(const int &value) +{ + if (!isDragging) { + zoom = value; + minOfTiles = Projection()->GetTileMatrixMinXY(value); + maxOfTiles = Projection()->GetTileMatrixMaxXY(value); + currentPositionPixel = Projection()->FromLatLngToPixel(currentPosition, value); + if (started) { + MtileLoadQueue.lock(); + tileLoadQueue.clear(); + MtileLoadQueue.unlock(); + MtileToload.lock(); + tilesToload = 0; + MtileToload.unlock(); + Matrix.Clear(); + GoToCurrentPositionOnZoom(); UpdateBounds(); + keepInBounds(); + emit OnMapDrag(); + emit OnMapZoomChanged(); + emit OnNeedInvalidation(); + } + } +} +void Core::SetCurrentPosition(const PointLatLng &value) +{ + if (!IsDragging()) { + currentPosition = value; + SetCurrentPositionGPixel(Projection()->FromLatLngToPixel(value, Zoom())); + + if (started) { + GoToCurrentPosition(); + emit OnCurrentPositionChanged(currentPosition); + } + } else { + currentPosition = value; + SetCurrentPositionGPixel(Projection()->FromLatLngToPixel(value, Zoom())); + + if (started) { emit OnCurrentPositionChanged(currentPosition); } } - void Core::OnMapClose() - { - // if(waitOnEmptyTasks != null) - // { - // try - // { - // waitOnEmptyTasks.Set(); - // waitOnEmptyTasks.Close(); - // } - // catch - // { - // } - // } +} +void Core::SetMapType(const MapType::Types &value) +{ + if (value != GetMapType()) { + mapType = value; - CancelAsyncTasks(); - } - GeoCoderStatusCode::Types Core::SetCurrentPositionByKeywords(QString const& keys) - { - GeoCoderStatusCode::Types status = GeoCoderStatusCode::Unknow; - PointLatLng pos = OPMaps::Instance()->GetLatLngFromGeodecoder(keys, status); - if(!pos.IsEmpty() && (status == GeoCoderStatusCode::G_GEO_SUCCESS)) + switch (value) { + case MapType::ArcGIS_Map: + case MapType::ArcGIS_Satellite: + case MapType::ArcGIS_ShadedRelief: + case MapType::ArcGIS_Terrain: { - SetCurrentPosition(pos); - } - - return status; - } - RectLatLng Core::CurrentViewArea() - { - PointLatLng p = Projection()->FromPixelToLatLng(-renderOffset.X(), -renderOffset.Y(), Zoom()); - double rlng = Projection()->FromPixelToLatLng(-renderOffset.X() + Width, -renderOffset.Y(), Zoom()).Lng(); - double blat = Projection()->FromPixelToLatLng(-renderOffset.X(), -renderOffset.Y() + Height, Zoom()).Lat(); - return RectLatLng::FromLTRB(p.Lng(), p.Lat(), rlng, blat); - - } - PointLatLng Core::FromLocalToLatLng(int const& x, int const& y) - { - return Projection()->FromPixelToLatLng(Point(x - renderOffset.X(), y - renderOffset.Y()), Zoom()); - } - - - Point Core::FromLatLngToLocal(PointLatLng const& latlng) - { - Point pLocal = Projection()->FromLatLngToPixel(latlng, Zoom()); - pLocal.Offset(renderOffset); - return pLocal; - } - int Core::GetMaxZoomToFitRect(RectLatLng const& rect) - { - int zoom = 0; - - for(int i = 1; i <= MaxZoom(); i++) - { - Point p1 = Projection()->FromLatLngToPixel(rect.LocationTopLeft(), i); - Point p2 = Projection()->FromLatLngToPixel(rect.Bottom(), rect.Right(), i); - - if(((p2.X() - p1.X()) <= Width+10) && (p2.Y() - p1.Y()) <= Height+10) - { - zoom = i; - } - else - { - break; + if (Projection()->Type() != "PlateCarreeProjection") { + SetProjection(new PlateCarreeProjection()); + maxzoom = 13; } } + break; - return zoom; - } - void Core::BeginDrag(Point const& pt) - { - dragPoint.SetX(pt.X() - renderOffset.X()); - dragPoint.SetY(pt.Y() - renderOffset.Y()); - isDragging = true; - } - void Core::EndDrag() - { - isDragging = false; - emit OnNeedInvalidation(); - - } - void Core::ReloadMap() - { - if(started) + case MapType::ArcGIS_MapsLT_Map_Hybrid: + case MapType::ArcGIS_MapsLT_Map_Labels: + case MapType::ArcGIS_MapsLT_Map: + case MapType::ArcGIS_MapsLT_OrtoFoto: { -#ifdef DEBUG_CORE - qDebug()<<"------------------"; -#endif //DEBUG_CORE - - MtileLoadQueue.lock(); - { - tileLoadQueue.clear(); - } - MtileLoadQueue.unlock(); - MtileToload.lock(); - tilesToload=0; - MtileToload.unlock(); - Matrix.Clear(); - - emit OnNeedInvalidation(); - - } - } - void Core::GoToCurrentPosition() - { - // reset stuff - renderOffset = Point::Empty; - centerTileXYLocationLast = Point::Empty; - dragPoint = Point::Empty; - - // goto location - Drag(Point(-(GetcurrentPositionGPixel().X() - Width/2), -(GetcurrentPositionGPixel().Y() - Height/2))); - } - void Core::GoToCurrentPositionOnZoom() - { - // reset stuff - renderOffset = Point::Empty; - centerTileXYLocationLast = Point::Empty; - dragPoint = Point::Empty; - - // goto location and centering - if(MouseWheelZooming) - { - if(mousewheelzoomtype != MouseWheelZoomType::MousePositionWithoutCenter) - { - Point pt = Point(-(GetcurrentPositionGPixel().X() - Width/2), -(GetcurrentPositionGPixel().Y() - Height/2)); - renderOffset.SetX(pt.X() - dragPoint.X()); - renderOffset.SetY(pt.Y() - dragPoint.Y()); - } - else // without centering - { - renderOffset.SetX(-GetcurrentPositionGPixel().X() - dragPoint.X()); - renderOffset.SetY(-GetcurrentPositionGPixel().Y() - dragPoint.Y()); - renderOffset.Offset(mouseLastZoom); + if (Projection()->Type() != "LKS94Projection") { + SetProjection(new LKS94Projection()); + maxzoom = 11; } } - else // use current map center + break; + + case MapType::PergoTurkeyMap: { - mouseLastZoom = Point::Empty; - - Point pt = Point(-(GetcurrentPositionGPixel().X() - Width/2), -(GetcurrentPositionGPixel().Y() - Height/2)); - renderOffset.SetX(pt.X() - dragPoint.X()); - renderOffset.SetY(pt.Y() - dragPoint.Y()); - } - - UpdateCenterTileXYLocation(); - } - void Core::DragOffset(Point const& offset) - { - renderOffset.Offset(offset); - - UpdateCenterTileXYLocation(); - - if(centerTileXYLocation != centerTileXYLocationLast) - { - centerTileXYLocationLast = centerTileXYLocation; - UpdateBounds(); - } - - { - LastLocationInBounds = CurrentPosition(); - SetCurrentPosition (FromLocalToLatLng((int) Width/2, (int) Height/2)); - } - - emit OnNeedInvalidation(); - emit OnMapDrag(); - } - void Core::Drag(Point const& pt) - { - renderOffset.SetX(pt.X() - dragPoint.X()); - renderOffset.SetY(pt.Y() - dragPoint.Y()); - keepInBounds(); - UpdateCenterTileXYLocation(); - - if(centerTileXYLocation != centerTileXYLocationLast) - { - centerTileXYLocationLast = centerTileXYLocation; - UpdateBounds(); - } - - if(IsDragging()) - { - LastLocationInBounds = CurrentPosition(); - SetCurrentPosition(FromLocalToLatLng((int) Width/2, (int) Height/2)); - } - - emit OnNeedInvalidation(); - - - emit OnMapDrag(); - - } - void Core::CancelAsyncTasks() - { - if(started) - { - ProcessLoadTaskCallback.waitForDone(); - MtileLoadQueue.lock(); - { - tileLoadQueue.clear(); - //tilesToload=0; - } - MtileLoadQueue.unlock(); - MtileToload.lock(); - tilesToload=0; - MtileToload.unlock(); - // ProcessLoadTaskCallback.waitForDone(); - } - } - void Core::UpdateBounds() - { - MtileDrawingList.lock(); - { - FindTilesAround(tileDrawingList); - -#ifdef DEBUG_CORE - qDebug()<<"OnTileLoadStart: " << tileDrawingList.count() << " tiles to load at zoom " << Zoom() << ", time: " << QDateTime::currentDateTime().date(); -#endif //DEBUG_CORE - - emit OnTileLoadStart(); - - - foreach(Point p,tileDrawingList) - { - LoadTask task = LoadTask(p, Zoom()); - { - MtileLoadQueue.lock(); - { - if(!tileLoadQueue.contains(task)) - { - MtileToload.lock(); - ++tilesToload; - MtileToload.unlock(); - tileLoadQueue.enqueue(task); -#ifdef DEBUG_CORE - qDebug()<<"Core::UpdateBounds new Task"<Type() != "PlateCarreeProjectionPergo") { + SetProjection(new PlateCarreeProjectionPergo()); + maxzoom = 17; } } - MtileDrawingList.unlock(); - UpdateGroundResolution(); - } - void Core::FindTilesAround(QList &list) - { - list.clear();; - for(int i = -sizeOfMapArea.Width(); i <= sizeOfMapArea.Width(); i++) + break; + + case MapType::YandexMapRu: { - for(int j = -sizeOfMapArea.Height(); j <= sizeOfMapArea.Height(); j++) - { - Point p = centerTileXYLocation; - p.SetX(p.X() + i); - p.SetY(p.Y() + j); - - //if(p.X < minOfTiles.Width) - //{ - // p.X += (maxOfTiles.Width + 1); - //} - - //if(p.X > maxOfTiles.Width) - //{ - // p.X -= (maxOfTiles.Width + 1); - //} - - if(p.X() >= minOfTiles.Width() && p.Y() >= minOfTiles.Height() && p.X() <= maxOfTiles.Width() && p.Y() <= maxOfTiles.Height()) - { - if(!list.contains(p)) - { - list.append(p); - } - } + if (Projection()->Type() != "MercatorProjectionYandex") { + SetProjection(new MercatorProjectionYandex()); + maxzoom = 13; } } + break; + default: + { + if (Projection()->Type() != "MercatorProjection") { + SetProjection(new MercatorProjection()); + maxzoom = 21; + } + } + break; + } - } - void Core::UpdateGroundResolution() - { - double rez = Projection()->GetGroundResolution(Zoom(), CurrentPosition().Lat()); - pxRes100m = (int) (100.0 / rez); // 100 meters - pxRes1000m = (int) (1000.0 / rez); // 1km - pxRes10km = (int) (10000.0 / rez); // 10km - pxRes100km = (int) (100000.0 / rez); // 100km - pxRes1000km = (int) (1000000.0 / rez); // 1000km - pxRes5000km = (int) (5000000.0 / rez); // 5000km - } - void Core::keepInBounds() - { - if(renderOffset.X()>0) - renderOffset.SetX(0); - if(renderOffset.Y()>0) - renderOffset.SetY(0); - int maxDragY=GetCurrentRegion().Height()-GettileRect().Height()*(maxOfTiles.Height()-minOfTiles.Height()+1); - int maxDragX=GetCurrentRegion().Width()-GettileRect().Width()*(maxOfTiles.Width()-minOfTiles.Width()+1); + minOfTiles = Projection()->GetTileMatrixMinXY(Zoom()); + maxOfTiles = Projection()->GetTileMatrixMaxXY(Zoom()); + SetCurrentPositionGPixel(Projection()->FromLatLngToPixel(CurrentPosition(), Zoom())); - if(maxDragY>renderOffset.Y()) - renderOffset.SetY(maxDragY); - if(maxDragX>renderOffset.X()) - renderOffset.SetX(maxDragX); + if (started) { + CancelAsyncTasks(); + OnMapSizeChanged(Width, Height); + GoToCurrentPosition(); + ReloadMap(); + GoToCurrentPosition(); + emit OnMapTypeChanged(value); + } } } +void Core::StartSystem() +{ + if (!started) { + started = true; + + ReloadMap(); + GoToCurrentPosition(); + } +} + +void Core::UpdateCenterTileXYLocation() +{ + PointLatLng center = FromLocalToLatLng(Width / 2, Height / 2); + Point centerPixel = Projection()->FromLatLngToPixel(center, Zoom()); + + centerTileXYLocation = Projection()->FromPixelToTileXY(centerPixel); +} + +void Core::OnMapSizeChanged(int const & width, int const & height) +{ + Width = width; + Height = height; + + sizeOfMapArea.SetWidth(1 + (Width / Projection()->TileSize().Width()) / 2); + sizeOfMapArea.SetHeight(1 + (Height / Projection()->TileSize().Height()) / 2); + + UpdateCenterTileXYLocation(); + + if (started) { + UpdateBounds(); + + emit OnCurrentPositionChanged(currentPosition); + } +} +void Core::OnMapClose() +{ + // if(waitOnEmptyTasks != null) + // { + // try + // { + // waitOnEmptyTasks.Set(); + // waitOnEmptyTasks.Close(); + // } + // catch + // { + // } + // } + + CancelAsyncTasks(); +} +GeoCoderStatusCode::Types Core::SetCurrentPositionByKeywords(QString const & keys) +{ + GeoCoderStatusCode::Types status = GeoCoderStatusCode::Unknow; + PointLatLng pos = OPMaps::Instance()->GetLatLngFromGeodecoder(keys, status); + + if (!pos.IsEmpty() && (status == GeoCoderStatusCode::G_GEO_SUCCESS)) { + SetCurrentPosition(pos); + } + + return status; +} +RectLatLng Core::CurrentViewArea() +{ + PointLatLng p = Projection()->FromPixelToLatLng(-renderOffset.X(), -renderOffset.Y(), Zoom()); + double rlng = Projection()->FromPixelToLatLng(-renderOffset.X() + Width, -renderOffset.Y(), Zoom()).Lng(); + double blat = Projection()->FromPixelToLatLng(-renderOffset.X(), -renderOffset.Y() + Height, Zoom()).Lat(); + + return RectLatLng::FromLTRB(p.Lng(), p.Lat(), rlng, blat); +} +PointLatLng Core::FromLocalToLatLng(int const & x, int const & y) +{ + return Projection()->FromPixelToLatLng(Point(x - renderOffset.X(), y - renderOffset.Y()), Zoom()); +} + + +Point Core::FromLatLngToLocal(PointLatLng const & latlng) +{ + Point pLocal = Projection()->FromLatLngToPixel(latlng, Zoom()); + + pLocal.Offset(renderOffset); + return pLocal; +} +int Core::GetMaxZoomToFitRect(RectLatLng const & rect) +{ + int zoom = 0; + + for (int i = 1; i <= MaxZoom(); i++) { + Point p1 = Projection()->FromLatLngToPixel(rect.LocationTopLeft(), i); + Point p2 = Projection()->FromLatLngToPixel(rect.Bottom(), rect.Right(), i); + + if (((p2.X() - p1.X()) <= Width + 10) && (p2.Y() - p1.Y()) <= Height + 10) { + zoom = i; + } else { + break; + } + } + + return zoom; +} +void Core::BeginDrag(Point const & pt) +{ + dragPoint.SetX(pt.X() - renderOffset.X()); + dragPoint.SetY(pt.Y() - renderOffset.Y()); + isDragging = true; +} +void Core::EndDrag() +{ + isDragging = false; + emit OnNeedInvalidation(); +} +void Core::ReloadMap() +{ + if (started) { +#ifdef DEBUG_CORE + qDebug() << "------------------"; +#endif // DEBUG_CORE + + MtileLoadQueue.lock(); + { + tileLoadQueue.clear(); + } + MtileLoadQueue.unlock(); + MtileToload.lock(); + tilesToload = 0; + MtileToload.unlock(); + Matrix.Clear(); + + emit OnNeedInvalidation(); + } +} +void Core::GoToCurrentPosition() +{ + // reset stuff + renderOffset = Point::Empty; + centerTileXYLocationLast = Point::Empty; + dragPoint = Point::Empty; + + // goto location + Drag(Point(-(GetcurrentPositionGPixel().X() - Width / 2), -(GetcurrentPositionGPixel().Y() - Height / 2))); +} +void Core::GoToCurrentPositionOnZoom() +{ + // reset stuff + renderOffset = Point::Empty; + centerTileXYLocationLast = Point::Empty; + dragPoint = Point::Empty; + + // goto location and centering + if (MouseWheelZooming) { + if (mousewheelzoomtype != MouseWheelZoomType::MousePositionWithoutCenter) { + Point pt = Point(-(GetcurrentPositionGPixel().X() - Width / 2), -(GetcurrentPositionGPixel().Y() - Height / 2)); + renderOffset.SetX(pt.X() - dragPoint.X()); + renderOffset.SetY(pt.Y() - dragPoint.Y()); + } else { // without centering + renderOffset.SetX(-GetcurrentPositionGPixel().X() - dragPoint.X()); + renderOffset.SetY(-GetcurrentPositionGPixel().Y() - dragPoint.Y()); + renderOffset.Offset(mouseLastZoom); + } + } else { // use current map center + mouseLastZoom = Point::Empty; + + Point pt = Point(-(GetcurrentPositionGPixel().X() - Width / 2), -(GetcurrentPositionGPixel().Y() - Height / 2)); + renderOffset.SetX(pt.X() - dragPoint.X()); + renderOffset.SetY(pt.Y() - dragPoint.Y()); + } + + UpdateCenterTileXYLocation(); +} +void Core::DragOffset(Point const & offset) +{ + renderOffset.Offset(offset); + + UpdateCenterTileXYLocation(); + + if (centerTileXYLocation != centerTileXYLocationLast) { + centerTileXYLocationLast = centerTileXYLocation; + UpdateBounds(); + } + + { + LastLocationInBounds = CurrentPosition(); + SetCurrentPosition(FromLocalToLatLng((int)Width / 2, (int)Height / 2)); + } + + emit OnNeedInvalidation(); + emit OnMapDrag(); +} +void Core::Drag(Point const & pt) +{ + renderOffset.SetX(pt.X() - dragPoint.X()); + renderOffset.SetY(pt.Y() - dragPoint.Y()); + keepInBounds(); + UpdateCenterTileXYLocation(); + + if (centerTileXYLocation != centerTileXYLocationLast) { + centerTileXYLocationLast = centerTileXYLocation; + UpdateBounds(); + } + + if (IsDragging()) { + LastLocationInBounds = CurrentPosition(); + SetCurrentPosition(FromLocalToLatLng((int)Width / 2, (int)Height / 2)); + } + + emit OnNeedInvalidation(); + + + emit OnMapDrag(); +} +void Core::CancelAsyncTasks() +{ + if (started) { + ProcessLoadTaskCallback.waitForDone(); + MtileLoadQueue.lock(); + { + tileLoadQueue.clear(); + // tilesToload=0; + } + MtileLoadQueue.unlock(); + MtileToload.lock(); + tilesToload = 0; + MtileToload.unlock(); + // ProcessLoadTaskCallback.waitForDone(); + } +} +void Core::UpdateBounds() +{ + MtileDrawingList.lock(); + { + FindTilesAround(tileDrawingList); + +#ifdef DEBUG_CORE + qDebug() << "OnTileLoadStart: " << tileDrawingList.count() << " tiles to load at zoom " << Zoom() << ", time: " << QDateTime::currentDateTime().date(); +#endif // DEBUG_CORE + + emit OnTileLoadStart(); + + + foreach(Point p, tileDrawingList) { + LoadTask task = LoadTask(p, Zoom()); + { + MtileLoadQueue.lock(); + { + if (!tileLoadQueue.contains(task)) { + MtileToload.lock(); + ++tilesToload; + MtileToload.unlock(); + tileLoadQueue.enqueue(task); +#ifdef DEBUG_CORE + qDebug() << "Core::UpdateBounds new Task" << task.Pos.ToString(); +#endif // DEBUG_CORE + ProcessLoadTaskCallback.start(this); + } + } + MtileLoadQueue.unlock(); + } + } + } + MtileDrawingList.unlock(); + UpdateGroundResolution(); +} +void Core::FindTilesAround(QList &list) +{ + list.clear();; + for (int i = -sizeOfMapArea.Width(); i <= sizeOfMapArea.Width(); i++) { + for (int j = -sizeOfMapArea.Height(); j <= sizeOfMapArea.Height(); j++) { + Point p = centerTileXYLocation; + p.SetX(p.X() + i); + p.SetY(p.Y() + j); + + // if(p.X < minOfTiles.Width) + // { + // p.X += (maxOfTiles.Width + 1); + // } + + // if(p.X > maxOfTiles.Width) + // { + // p.X -= (maxOfTiles.Width + 1); + // } + + if (p.X() >= minOfTiles.Width() && p.Y() >= minOfTiles.Height() && p.X() <= maxOfTiles.Width() && p.Y() <= maxOfTiles.Height()) { + if (!list.contains(p)) { + list.append(p); + } + } + } + } +} +void Core::UpdateGroundResolution() +{ + double rez = Projection()->GetGroundResolution(Zoom(), CurrentPosition().Lat()); + + pxRes100m = (int)(100.0 / rez); // 100 meters + pxRes1000m = (int)(1000.0 / rez); // 1km + pxRes10km = (int)(10000.0 / rez); // 10km + pxRes100km = (int)(100000.0 / rez); // 100km + pxRes1000km = (int)(1000000.0 / rez); // 1000km + pxRes5000km = (int)(5000000.0 / rez); // 5000km +} +void Core::keepInBounds() +{ + if (renderOffset.X() > 0) { + renderOffset.SetX(0); + } + if (renderOffset.Y() > 0) { + renderOffset.SetY(0); + } + int maxDragY = GetCurrentRegion().Height() - GettileRect().Height() * (maxOfTiles.Height() - minOfTiles.Height() + 1); + int maxDragX = GetCurrentRegion().Width() - GettileRect().Width() * (maxOfTiles.Width() - minOfTiles.Width() + 1); + + if (maxDragY > renderOffset.Y()) { + renderOffset.SetY(maxDragY); + } + if (maxDragX > renderOffset.X()) { + renderOffset.SetX(maxDragX); + } +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.h index dca51d646..9998f0408 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file core.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file core.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef CORE_H #define CORE_H @@ -57,228 +57,340 @@ #include -namespace mapcontrol -{ - class OPMapControl; - class MapGraphicItem; +namespace mapcontrol { +class OPMapControl; +class MapGraphicItem; } namespace internals { +class Core : public QObject, public QRunnable { + Q_OBJECT - class Core:public QObject,public QRunnable + friend class mapcontrol::OPMapControl; + friend class mapcontrol::MapGraphicItem; +public: + Core(); + ~Core(); + void run(); + PointLatLng CurrentPosition() const { - Q_OBJECT + return currentPosition; + } - friend class mapcontrol::OPMapControl; - friend class mapcontrol::MapGraphicItem; - public: - Core(); - ~Core(); - void run(); - PointLatLng CurrentPosition()const{return currentPosition;} + void SetCurrentPosition(const PointLatLng &value); - void SetCurrentPosition(const PointLatLng &value); + core::Point GetcurrentPositionGPixel() + { + return currentPositionPixel; + } + void SetcurrentPositionGPixel(const core::Point &value) + { + currentPositionPixel = value; + } - core::Point GetcurrentPositionGPixel(){return currentPositionPixel;} - void SetcurrentPositionGPixel(const core::Point &value){currentPositionPixel=value;} + core::Point GetrenderOffset() + { + return renderOffset; + } + void SetrenderOffset(const core::Point &value) + { + renderOffset = value; + } - core::Point GetrenderOffset(){return renderOffset;} - void SetrenderOffset(const core::Point &value){renderOffset=value;} + core::Point GetcenterTileXYLocation() + { + return centerTileXYLocation; + } + void SetcenterTileXYLocation(const core::Point &value) + { + centerTileXYLocation = value; + } - core::Point GetcenterTileXYLocation(){return centerTileXYLocation;} - void SetcenterTileXYLocation(const core::Point &value){centerTileXYLocation=value;} + core::Point GetcenterTileXYLocationLast() + { + return centerTileXYLocationLast; + } + void SetcenterTileXYLocationLast(const core::Point &value) + { + centerTileXYLocationLast = value; + } - core::Point GetcenterTileXYLocationLast(){return centerTileXYLocationLast;} - void SetcenterTileXYLocationLast(const core::Point &value){centerTileXYLocationLast=value;} + core::Point GetdragPoint() + { + return dragPoint; + } + void SetdragPoint(const core::Point &value) + { + dragPoint = value; + } - core::Point GetdragPoint(){return dragPoint;} - void SetdragPoint(const core::Point &value){dragPoint=value;} + core::Point GetmouseDown() + { + return mouseDown; + } + void SetmouseDown(const core::Point &value) + { + mouseDown = value; + } - core::Point GetmouseDown(){return mouseDown;} - void SetmouseDown(const core::Point &value){mouseDown=value;} + core::Point GetmouseCurrent() + { + return mouseCurrent; + } + void SetmouseCurrent(const core::Point &value) + { + mouseCurrent = value; + } - core::Point GetmouseCurrent(){return mouseCurrent;} - void SetmouseCurrent(const core::Point &value){mouseCurrent=value;} + core::Point GetmouseLastZoom() + { + return mouseLastZoom; + } + void SetmouseLastZoom(const core::Point &value) + { + mouseLastZoom = value; + } - core::Point GetmouseLastZoom(){return mouseLastZoom;} - void SetmouseLastZoom(const core::Point &value){mouseLastZoom=value;} + MouseWheelZoomType::Types GetMouseWheelZoomType() + { + return mousewheelzoomtype; + } + void SetMouseWheelZoomType(const MouseWheelZoomType::Types &value) + { + mousewheelzoomtype = value; + } - MouseWheelZoomType::Types GetMouseWheelZoomType(){return mousewheelzoomtype;} - void SetMouseWheelZoomType(const MouseWheelZoomType::Types &value){mousewheelzoomtype=value;} + PointLatLng GetLastLocationInBounds() + { + return LastLocationInBounds; + } + void SetLastLocationInBounds(const PointLatLng &value) + { + LastLocationInBounds = value; + } - PointLatLng GetLastLocationInBounds(){return LastLocationInBounds;} - void SetLastLocationInBounds(const PointLatLng &value){LastLocationInBounds=value;} + Size GetsizeOfMapArea() + { + return sizeOfMapArea; + } + void SetsizeOfMapArea(const Size &value) + { + sizeOfMapArea = value; + } - Size GetsizeOfMapArea(){return sizeOfMapArea;} - void SetsizeOfMapArea(const Size &value){sizeOfMapArea=value;} + Size GetminOfTiles() + { + return minOfTiles; + } + void SetminOfTiles(const Size &value) + { + minOfTiles = value; + } - Size GetminOfTiles(){return minOfTiles;} - void SetminOfTiles(const Size &value){minOfTiles=value;} + Size GetmaxOfTiles() + { + return maxOfTiles; + } + void SetmaxOfTiles(const Size &value) + { + maxOfTiles = value; + } - Size GetmaxOfTiles(){return maxOfTiles;} - void SetmaxOfTiles(const Size &value){maxOfTiles=value;} + Rectangle GettileRect() + { + return tileRect; + } + void SettileRect(const Rectangle &value) + { + tileRect = value; + } - Rectangle GettileRect(){return tileRect;} - void SettileRect(const Rectangle &value){tileRect=value;} + core::Point GettilePoint() + { + return tilePoint; + } + void SettilePoint(const core::Point &value) + { + tilePoint = value; + } - core::Point GettilePoint(){return tilePoint;} - void SettilePoint(const core::Point &value){tilePoint=value;} + Rectangle GetCurrentRegion() + { + return CurrentRegion; + } + void SetCurrentRegion(const Rectangle &value) + { + CurrentRegion = value; + } - Rectangle GetCurrentRegion(){return CurrentRegion;} - void SetCurrentRegion(const Rectangle &value){CurrentRegion=value;} + QList tileDrawingList; - QList tileDrawingList; + PureProjection *Projection() + { + return projection; + } + void SetProjection(PureProjection *value) + { + projection = value; + tileRect = Rectangle(core::Point(0, 0), value->TileSize()); + } + bool IsDragging() const + { + return isDragging; + } - PureProjection* Projection() - { - return projection; - } - void SetProjection(PureProjection* value) - { - projection=value; - tileRect=Rectangle(core::Point(0,0),value->TileSize()); - } - bool IsDragging()const{return isDragging;} + int Zoom() const + { + return zoom; + } + void SetZoom(int const & value); - int Zoom()const{return zoom;} - void SetZoom(int const& value); + int MaxZoom() const + { + return maxzoom; + } - int MaxZoom()const{return maxzoom;} + void UpdateBounds(); - void UpdateBounds(); + MapType::Types GetMapType() + { + return mapType; + } + void SetMapType(MapType::Types const & value); - MapType::Types GetMapType(){return mapType;} - void SetMapType(MapType::Types const& value); + void StartSystem(); - void StartSystem(); + void UpdateCenterTileXYLocation(); - void UpdateCenterTileXYLocation(); + void OnMapSizeChanged(int const & width, int const & height); // TODO had as slot - void OnMapSizeChanged(int const& width, int const& height);//TODO had as slot + void OnMapClose(); // TODO had as slot - void OnMapClose();//TODO had as slot + GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const & keys); - GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const& keys); + RectLatLng CurrentViewArea(); - RectLatLng CurrentViewArea(); + PointLatLng FromLocalToLatLng(int const & x, int const & y); - PointLatLng FromLocalToLatLng(int const& x, int const& y); + Point FromLatLngToLocal(PointLatLng const & latlng); - Point FromLatLngToLocal(PointLatLng const& latlng); + int GetMaxZoomToFitRect(RectLatLng const & rect); - int GetMaxZoomToFitRect(RectLatLng const& rect); + void BeginDrag(core::Point const & pt); - void BeginDrag(core::Point const& pt); + void EndDrag(); - void EndDrag(); + void ReloadMap(); - void ReloadMap(); + void GoToCurrentPosition(); - void GoToCurrentPosition(); + bool MouseWheelZooming; - bool MouseWheelZooming; + void DragOffset(core::Point const & offset); - void DragOffset(core::Point const& offset); + void Drag(core::Point const & pt); - void Drag(core::Point const& pt); + void CancelAsyncTasks(); - void CancelAsyncTasks(); + void FindTilesAround(QList &list); - void FindTilesAround(QList &list); + void UpdateGroundResolution(); - void UpdateGroundResolution(); + TileMatrix Matrix; - TileMatrix Matrix; + bool isStarted() + { + return started; + } - bool isStarted(){return started;} + diagnostics GetDiagnostics(); - diagnostics GetDiagnostics(); +signals: + void OnCurrentPositionChanged(internals::PointLatLng point); + void OnTileLoadComplete(); + void OnTilesStillToLoad(int number); + void OnTileLoadStart(); + void OnMapDrag(); + void OnMapZoomChanged(); + void OnMapTypeChanged(MapType::Types type); + void OnEmptyTileError(int zoom, core::Point pos); + void OnNeedInvalidation(); - signals: - void OnCurrentPositionChanged(internals::PointLatLng point); - void OnTileLoadComplete(); - void OnTilesStillToLoad(int number); - void OnTileLoadStart(); - void OnMapDrag(); - void OnMapZoomChanged(); - void OnMapTypeChanged(MapType::Types type); - void OnEmptyTileError(int zoom, core::Point pos); - void OnNeedInvalidation(); +private: - private: + void keepInBounds(); + PointLatLng currentPosition; + core::Point currentPositionPixel; + core::Point renderOffset; + core::Point centerTileXYLocation; + core::Point centerTileXYLocationLast; + core::Point dragPoint; + Rectangle tileRect; + core::Point mouseDown; + bool CanDragMap; + core::Point mouseCurrent; + PointLatLng LastLocationInBounds; + core::Point mouseLastZoom; - void keepInBounds(); - PointLatLng currentPosition; - core::Point currentPositionPixel; - core::Point renderOffset; - core::Point centerTileXYLocation; - core::Point centerTileXYLocationLast; - core::Point dragPoint; - Rectangle tileRect; - core::Point mouseDown; - bool CanDragMap; - core::Point mouseCurrent; - PointLatLng LastLocationInBounds; - core::Point mouseLastZoom; - - MouseWheelZoomType::Types mousewheelzoomtype; + MouseWheelZoomType::Types mousewheelzoomtype; - Size sizeOfMapArea; - Size minOfTiles; - Size maxOfTiles; + Size sizeOfMapArea; + Size minOfTiles; + Size maxOfTiles; - core::Point tilePoint; + core::Point tilePoint; - Rectangle CurrentRegion; + Rectangle CurrentRegion; - QQueue tileLoadQueue; + QQueue tileLoadQueue; - int zoom; + int zoom; - PureProjection* projection; + PureProjection *projection; - bool isDragging; + bool isDragging; - QMutex MtileLoadQueue; + QMutex MtileLoadQueue; - QMutex Moverlays; + QMutex Moverlays; - QMutex MtileDrawingList; + QMutex MtileDrawingList; #ifdef DEBUG_CORE - QMutex Mdebug; - static qlonglong debugcounter; + QMutex Mdebug; + static qlonglong debugcounter; #endif - Size TooltipTextPadding; + Size TooltipTextPadding; - MapType::Types mapType; + MapType::Types mapType; - QSemaphore loaderLimit; + QSemaphore loaderLimit; - QThreadPool ProcessLoadTaskCallback; - QMutex MtileToload; - int tilesToload; + QThreadPool ProcessLoadTaskCallback; + QMutex MtileToload; + int tilesToload; - int maxzoom; - QMutex MrunningThreads; - int runningThreads; - diagnostics diag; + int maxzoom; + QMutex MrunningThreads; + int runningThreads; + diagnostics diag; - protected: - bool started; - - int Width; - int Height; - int pxRes100m; // 100 meters - int pxRes1000m; // 1km - int pxRes10km; // 10km - int pxRes100km; // 100km - int pxRes1000km; // 1000km - int pxRes5000km; // 5000km - void SetCurrentPositionGPixel(core::Point const& value){currentPositionPixel = value;} - void GoToCurrentPositionOnZoom(); - - }; +protected: + bool started; + int Width; + int Height; + int pxRes100m; // 100 meters + int pxRes1000m; // 1km + int pxRes10km; // 10km + int pxRes100km; // 100km + int pxRes1000km; // 1000km + int pxRes5000km; // 5000km + void SetCurrentPositionGPixel(core::Point const & value) + { + currentPositionPixel = value; + } + void GoToCurrentPositionOnZoom(); +}; } #endif // CORE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/debugheader.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/debugheader.h index 50ae028f8..70623712f 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/debugheader.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/debugheader.h @@ -1,8 +1,8 @@ #ifndef DEBUGHEADER_H #define DEBUGHEADER_H -//#define DEBUG_CORE -//#define DEBUG_TILE -//#define DEBUG_TILEMATRIX +// #define DEBUG_CORE +// #define DEBUG_TILE +// #define DEBUG_TILEMATRIX #endif // DEBUGHEADER_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.cpp index 535c7e615..3f0fa7225 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.cpp @@ -1,35 +1,35 @@ /** -****************************************************************************** -* -* @file loadtask.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file loadtask.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "loadtask.h" - + namespace internals { -bool operator==(LoadTask const& lhs,LoadTask const& rhs) +bool operator==(LoadTask const & lhs, LoadTask const & rhs) { - return ((lhs.Pos==rhs.Pos)&&(lhs.Zoom==rhs.Zoom)); + return (lhs.Pos == rhs.Pos) && (lhs.Zoom == rhs.Zoom); } } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.h index 47df78a65..2810c56b5 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/loadtask.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file loadtask.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file loadtask.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef LOADTASK_H #define LOADTASK_H @@ -31,35 +31,33 @@ #include "../core/point.h" using namespace core; -namespace internals -{ -struct LoadTask - { - friend bool operator==(LoadTask const& lhs,LoadTask const& rhs); - public: +namespace internals { +struct LoadTask { + friend bool operator==(LoadTask const & lhs, LoadTask const & rhs); +public: core::Point Pos; int Zoom; LoadTask(Point pos, int zoom) - { - Pos = pos; + { + Pos = pos; Zoom = zoom; } LoadTask() { - Pos=core::Point(-1,-1); - Zoom=-1; + Pos = core::Point(-1, -1); + Zoom = -1; } bool HasValue() { - return !(Zoom==-1); + return !(Zoom == -1); } - QString ToString()const - { + QString ToString() const + { return QString::number(Zoom) + " - " + Pos.ToString(); - } - }; + } +}; } #endif // LOADTASK_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/mousewheelzoomtype.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/mousewheelzoomtype.h index d3fe648b9..b947564fb 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/mousewheelzoomtype.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/mousewheelzoomtype.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file mousewheelzoomtype.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file mousewheelzoomtype.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef MOUSEWHEELZOOMTYPE_H #define MOUSEWHEELZOOMTYPE_H #include @@ -32,13 +32,10 @@ #include #include namespace internals { - class MouseWheelZoomType:public QObject - { - Q_OBJECT - Q_ENUMS(Types) - public: - enum Types - { +class MouseWheelZoomType : public QObject { + Q_OBJECT Q_ENUMS(Types) +public: + enum Types { /// /// zooms map to current mouse position and makes it map center /// @@ -55,35 +52,34 @@ namespace internals { /// ViewCenter }; - static QString StrByType(Types const& value) + static QString StrByType(Types const & value) { QMetaObject metaObject = MouseWheelZoomType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - QString s=metaEnum.valueToKey(value); + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + QString s = metaEnum.valueToKey(value); + return s; } - static Types TypeByStr(QString const& value) + static Types TypeByStr(QString const & value) { QMetaObject metaObject = MouseWheelZoomType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - Types s=(Types)metaEnum.keyToValue(value.toLatin1()); + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + Types s = (Types)metaEnum.keyToValue(value.toLatin1()); + return s; } static QStringList TypesList() { QStringList ret; QMetaObject metaObject = MouseWheelZoomType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - for(int x=0;x #include #include "sizelatlng.h" - + namespace internals { -struct PointLatLng -{ - //friend uint qHash(PointLatLng const& point); - friend bool operator==(PointLatLng const& lhs,PointLatLng const& rhs); - friend bool operator!=(PointLatLng const& left, PointLatLng const& right); +struct PointLatLng { + // friend uint qHash(PointLatLng const& point); + friend bool operator==(PointLatLng const & lhs, PointLatLng const & rhs); + friend bool operator!=(PointLatLng const & left, PointLatLng const & right); friend PointLatLng operator+(PointLatLng pt, SizeLatLng sz); friend PointLatLng operator-(PointLatLng pt, SizeLatLng sz); - //TODO Sizelatlng friend PointLatLng operator+(PointLatLng pt, SizeLatLng sz); + // TODO Sizelatlng friend PointLatLng operator+(PointLatLng pt, SizeLatLng sz); - private: +private: double lat; double lng; - bool empty; - public: + bool empty; +public: PointLatLng(); static PointLatLng Empty; - PointLatLng(const double &lat,const double &lng) - { - this->lat = lat; - this->lng = lng; - empty=false; - } + PointLatLng(const double &lat, const double &lng) + { + this->lat = lat; + this->lng = lng; + empty = false; + } - bool IsEmpty() - { - return empty; - } + bool IsEmpty() + { + return empty; + } - double Lat()const - { - return this->lat; - } + double Lat() const + { + return this->lat; + } - void SetLat(const double &value) - { - this->lat = value; - empty=false; - } + void SetLat(const double &value) + { + this->lat = value; + empty = false; + } - double Lng()const - { - return this->lng; - } - void SetLng(const double &value) - { - this->lng = value; - empty=false; - } + double Lng() const + { + return this->lng; + } + void SetLng(const double &value) + { + this->lng = value; + empty = false; + } + static PointLatLng Add(PointLatLng const & pt, SizeLatLng const & sz) + { + return PointLatLng(pt.Lat() - sz.HeightLat(), pt.Lng() + sz.WidthLng()); + } + + static PointLatLng Subtract(PointLatLng const & pt, SizeLatLng const & sz) + { + return PointLatLng(pt.Lat() + sz.HeightLat(), pt.Lng() - sz.WidthLng()); + } + void Offset(PointLatLng const & pos) + { + this->Offset(pos.Lat(), pos.Lng()); + } - static PointLatLng Add(PointLatLng const& pt, SizeLatLng const& sz) - { - return PointLatLng(pt.Lat() - sz.HeightLat(), pt.Lng() + sz.WidthLng()); - } - - static PointLatLng Subtract(PointLatLng const& pt, SizeLatLng const& sz) - { - return PointLatLng(pt.Lat() + sz.HeightLat(), pt.Lng() - sz.WidthLng()); - } + void Offset(double const & lat, double const & lng) + { + this->lng += lng; + this->lat -= lat; + } - void Offset(PointLatLng const& pos) - { - this->Offset(pos.Lat(), pos.Lng()); - } - - void Offset(double const& lat, double const& lng) - { - this->lng += lng; - this->lat -= lat; - } - - - QString ToString()const - { - return QString("{Lat=%1, Lng=%2}").arg(this->lat).arg(this->lng); - } + QString ToString() const + { + return QString("{Lat=%1, Lng=%2}").arg(this->lat).arg(this->lng); + } //// static PointLatLng() //// { //// Empty = new PointLatLng(); //// } - }; +}; // diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.cpp index 0273a5bda..415bbf7f6 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.cpp @@ -1,38 +1,36 @@ /** -****************************************************************************** -* -* @file lks94projection.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file lks94projection.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "lks94projection.h" - namespace projections { -LKS94Projection::LKS94Projection():MinLatitude (53.33 ), MaxLatitude (56.55 ), MinLongitude (20.22 ), -MaxLongitude (27.11 ), orignX (5122000 ), orignY (10000100 ),tileSize(256, 256) -{ -} +LKS94Projection::LKS94Projection() : MinLatitude(53.33), MaxLatitude(56.55), MinLongitude(20.22), + MaxLongitude(27.11), orignX(5122000), orignY(10000100), tileSize(256, 256) +{} Size LKS94Projection::TileSize() const { @@ -44,198 +42,174 @@ double LKS94Projection::Axis() const } double LKS94Projection::Flattening() const { - - return (1.0 / 298.257222101); - + return 1.0 / 298.257222101; } -Point LKS94Projection::FromLatLngToPixel(double lat, double lng, int const& zoom) +Point LKS94Projection::FromLatLngToPixel(double lat, double lng, int const & zoom) { Point ret; - lat = Clip(lat, MinLatitude, MaxLatitude); - lng = Clip(lng, MinLongitude, MaxLongitude); + lat = Clip(lat, MinLatitude, MaxLatitude); + lng = Clip(lng, MinLongitude, MaxLongitude); QVector lks(3); - lks[0]=lng; - lks[1]=lat; - lks = DTM10(lks); - lks = MTD10(lks); - lks = DTM00(lks); + lks[0] = lng; + lks[1] = lat; + lks = DTM10(lks); + lks = MTD10(lks); + lks = DTM00(lks); double res = GetTileMatrixResolution(zoom); - ret.SetX((int) floor((lks[0] + orignX) / res)); - ret.SetY((int) floor((orignY - lks[1]) / res)); + ret.SetX((int)floor((lks[0] + orignX) / res)); + ret.SetY((int)floor((orignY - lks[1]) / res)); return ret; } -internals::PointLatLng LKS94Projection::FromPixelToLatLng(int const& x, int const& y, int const& zoom) +internals::PointLatLng LKS94Projection::FromPixelToLatLng(int const & x, int const & y, int const & zoom) { - internals::PointLatLng ret;// = internals::PointLatLng::Empty; + internals::PointLatLng ret; // = internals::PointLatLng::Empty; double res = GetTileMatrixResolution(zoom); QVector lks(2); - lks[0]=(x * res) - orignX; - lks[1]=-(y * res) + orignY; - lks = MTD11(lks); - lks = DTM10(lks); - lks = MTD10(lks); + lks[0] = (x * res) - orignX; + lks[1] = -(y * res) + orignY; + lks = MTD11(lks); + lks = DTM10(lks); + lks = MTD10(lks); ret.SetLat(Clip(lks[1], MinLatitude, MaxLatitude)); ret.SetLng(Clip(lks[0], MinLongitude, MaxLongitude)); return ret; } -QVector LKS94Projection::DTM10(const QVector & lonlat) +QVector LKS94Projection::DTM10(const QVector & lonlat) { - double es; // Eccentricity squared : (a^2 - b^2)/a^2 - double semiMajor = 6378137.0; // major axis - double semiMinor = 6356752.3142451793; // minor axis - double ab; // Semi_major / semi_minor - double ba; // Semi_minor / semi_major - double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 + double es; // Eccentricity squared : (a^2 - b^2)/a^2 + double semiMajor = 6378137.0; // major axis + double semiMinor = 6356752.3142451793; // minor axis + double ab; // Semi_major / semi_minor + double ba; // Semi_minor / semi_major + double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 - es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); //e^2 + es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); // e^2 ses = (pow(semiMajor, 2) - pow(semiMinor, 2)) / pow(semiMinor, 2); - ba = semiMinor / semiMajor; - ab = semiMajor / semiMinor; + ba = semiMinor / semiMajor; + ab = semiMajor / semiMinor; // ... double lon = DegreesToRadians(lonlat[0]); double lat = DegreesToRadians(lonlat[1]); - double h = lonlat.count() < 3 ? 0 : std::isnan(lonlat[2]) ? 0 : lonlat[2];//TODO NAN - double v = semiMajor / sqrt(1 - es * pow(sin(lat), 2)); - double x = (v + h) * cos(lat) * cos(lon); - double y = (v + h) * cos(lat) * sin(lon); - double z = ((1 - es) * v + h) * sin(lat); + double h = lonlat.count() < 3 ? 0 : std::isnan(lonlat[2]) ? 0 : lonlat[2]; // TODO NAN + double v = semiMajor / sqrt(1 - es * pow(sin(lat), 2)); + double x = (v + h) * cos(lat) * cos(lon); + double y = (v + h) * cos(lat) * sin(lon); + double z = ((1 - es) * v + h) * sin(lat); QVector ret(3); - ret[0]=x; - ret[1]=y; - ret[2]=z; - return ret; + ret[0] = x; + ret[1] = y; + ret[2] = z; + return ret; } -QVector LKS94Projection::MTD10(QVector & pnt) +QVector LKS94Projection::MTD10(QVector & pnt) { QVector ret(3); - const double COS_67P5 = 0.38268343236508977; // cosine of 67.5 degrees - const double AD_C = 1.0026000; // Toms region 1 constant + const double COS_67P5 = 0.38268343236508977; // cosine of 67.5 degrees + const double AD_C = 1.0026000; // Toms region 1 constant - double es; // Eccentricity squared : (a^2 - b^2)/a^2 - double semiMajor = 6378137.0; // major axis - double semiMinor = 6356752.3141403561; // minor axis - double ab; // Semi_major / semi_minor - double ba; // Semi_minor / semi_major - double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 + double es; // Eccentricity squared : (a^2 - b^2)/a^2 + double semiMajor = 6378137.0; // major axis + double semiMinor = 6356752.3141403561; // minor axis + double ab; // Semi_major / semi_minor + double ba; // Semi_minor / semi_major + double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 - es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); //e^2 + es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); // e^2 ses = (pow(semiMajor, 2) - pow(semiMinor, 2)) / pow(semiMinor, 2); - ba = semiMinor / semiMajor; - ab = semiMajor / semiMinor; + ba = semiMinor / semiMajor; + ab = semiMajor / semiMinor; // ... - bool AtPole = false; // is location in polar region - double Z = pnt.count() < 3 ? 0 : std::isnan(pnt[2]) ? 0 : pnt[2];//TODO NaN + bool AtPole = false; // is location in polar region + double Z = pnt.count() < 3 ? 0 : std::isnan(pnt[2]) ? 0 : pnt[2]; // TODO NaN - double lon = 0; - double lat = 0; + double lon = 0; + double lat = 0; double Height = 0; - if(pnt[0] != 0.0) - { + if (pnt[0] != 0.0) { lon = atan2(pnt[1], pnt[0]); - } - else - { - if(pnt[1] > 0) - { + } else { + if (pnt[1] > 0) { lon = M_PI / 2; - } - else - if(pnt[1] < 0) - { + } else if (pnt[1] < 0) { lon = -M_PI * 0.5; - } - else - { + } else { AtPole = true; - lon = 0.0; - if(Z > 0.0) // north pole - { + lon = 0.0; + if (Z > 0.0) { // north pole lat = M_PI * 0.5; - } - else - if(Z < 0.0) // south pole - { + } else if (Z < 0.0) { // south pole lat = -M_PI * 0.5; - } - else // center of earth - { - ret[0]=RadiansToDegrees(lon); - ret[1]=RadiansToDegrees(M_PI * 0.5); - ret[2]=-semiMinor; + } else { // center of earth + ret[0] = RadiansToDegrees(lon); + ret[1] = RadiansToDegrees(M_PI * 0.5); + ret[2] = -semiMinor; return ret; } } } - double W2 = pnt[0] * pnt[0] + pnt[1] * pnt[1]; // Square of distance from Z axis - double W = sqrt(W2); // distance from Z axis - double T0 = Z * AD_C; // initial estimate of vertical component - double S0 = sqrt(T0 * T0 + W2); // initial estimate of horizontal component - double Sin_B0 = T0 / S0; // sin(B0), B0 is estimate of Bowring aux variable - double Cos_B0 = W / S0; // cos(B0) + double W2 = pnt[0] * pnt[0] + pnt[1] * pnt[1]; // Square of distance from Z axis + double W = sqrt(W2); // distance from Z axis + double T0 = Z * AD_C; // initial estimate of vertical component + double S0 = sqrt(T0 * T0 + W2); // initial estimate of horizontal component + double Sin_B0 = T0 / S0; // sin(B0), B0 is estimate of Bowring aux variable + double Cos_B0 = W / S0; // cos(B0) double Sin3_B0 = pow(Sin_B0, 3); - double T1 = Z + semiMinor * ses * Sin3_B0; // corrected estimate of vertical component - double Sum = W - semiMajor * es * Cos_B0 * Cos_B0 * Cos_B0; // numerator of cos(phi1) - double S1 = sqrt(T1 * T1 + Sum * Sum); // corrected estimate of horizontal component - double Sin_p1 = T1 / S1; // sin(phi1), phi1 is estimated latitude - double Cos_p1 = Sum / S1; // cos(phi1) - double Rn = semiMajor / sqrt(1.0 - es * Sin_p1 * Sin_p1); // Earth radius at location - if(Cos_p1 >= COS_67P5) - { + double T1 = Z + semiMinor * ses * Sin3_B0; // corrected estimate of vertical component + double Sum = W - semiMajor * es * Cos_B0 * Cos_B0 * Cos_B0; // numerator of cos(phi1) + double S1 = sqrt(T1 * T1 + Sum * Sum); // corrected estimate of horizontal component + double Sin_p1 = T1 / S1; // sin(phi1), phi1 is estimated latitude + double Cos_p1 = Sum / S1; // cos(phi1) + double Rn = semiMajor / sqrt(1.0 - es * Sin_p1 * Sin_p1); // Earth radius at location + if (Cos_p1 >= COS_67P5) { Height = W / Cos_p1 - Rn; - } - else - if(Cos_p1 <= -COS_67P5) - { + } else if (Cos_p1 <= -COS_67P5) { Height = W / -Cos_p1 - Rn; - } - else - { + } else { Height = Z / Sin_p1 + Rn * (es - 1.0); } - if(!AtPole) - { + if (!AtPole) { lat = atan(Sin_p1 / Cos_p1); } - ret[0]=RadiansToDegrees(lon); - ret[1]=RadiansToDegrees(lat); - ret[2]=Height; + ret[0] = RadiansToDegrees(lon); + ret[1] = RadiansToDegrees(lat); + ret[2] = Height; return ret; } -QVector LKS94Projection::DTM00(QVector & lonlat) +QVector LKS94Projection::DTM00(QVector & lonlat) { - double scaleFactor = 0.9998; // scale factor - double centralMeridian = 0.41887902047863912; // Center qlonglongitude (projection center) */ - double latOrigin = 0.0; // center latitude - double falseNorthing = 0.0; // y offset in meters - double falseEasting = 500000.0; // x offset in meters - double semiMajor = 6378137.0; // major axis - double semiMinor = 6356752.3141403561; // minor axis - double metersPerUnit = 1.0; + double scaleFactor = 0.9998; // scale factor + double centralMeridian = 0.41887902047863912; // Center qlonglongitude (projection center) */ + double latOrigin = 0.0; // center latitude + double falseNorthing = 0.0; // y offset in meters + double falseEasting = 500000.0; // x offset in meters + double semiMajor = 6378137.0; // major axis + double semiMinor = 6356752.3141403561; // minor axis + double metersPerUnit = 1.0; - double e0, e1, e2, e3; // eccentricity constants - double e, es, esp; // eccentricity constants - double ml0; // small value m + double e0, e1, e2, e3; // eccentricity constants + double e, es, esp; // eccentricity constants + double ml0; // small value m - es = 1.0 - pow(semiMinor / semiMajor, 2); - e = sqrt(es); - e0 = e0fn(es); - e1 = e1fn(es); - e2 = e2fn(es); - e3 = e3fn(es); + es = 1.0 - pow(semiMinor / semiMajor, 2); + e = sqrt(es); + e0 = e0fn(es); + e1 = e1fn(es); + e2 = e2fn(es); + e3 = e3fn(es); ml0 = semiMajor * mlfn(e0, e1, e2, e3, latOrigin); esp = es / (1.0 - es); @@ -244,23 +218,23 @@ QVector LKS94Projection::DTM00(QVector & lonlat) double lon = DegreesToRadians(lonlat[0]); double lat = DegreesToRadians(lonlat[1]); - double delta_lon = 0.0; // Delta qlonglongitude (Given qlonglongitude - center) + double delta_lon = 0.0; // Delta qlonglongitude (Given qlonglongitude - center) double sin_phi, cos_phi; // sin and cos value - double al, als; // temporary values - double c, t, tq; // temporary values - double con, n, ml; // cone constant, small m + double al, als; // temporary values + double c, t, tq; // temporary values + double con, n, ml; // cone constant, small m delta_lon = LKS94Projection::AdjustLongitude(lon - centralMeridian); - LKS94Projection::SinCos(lat, sin_phi, cos_phi); + LKS94Projection::SinCos(lat, sin_phi, cos_phi); - al = cos_phi * delta_lon; + al = cos_phi * delta_lon; als = pow(al, 2); - c = pow(cos_phi, 2); - tq = tan(lat); - t = pow(tq, 2); + c = pow(cos_phi, 2); + tq = tan(lat); + t = pow(tq, 2); con = 1.0 - es * pow(sin_phi, 2); - n = semiMajor / sqrt(con); - ml = semiMajor * mlfn(e0, e1, e2, e3, lat); + n = semiMajor / sqrt(con); + ml = semiMajor * mlfn(e0, e1, e2, e3, lat); double x = scaleFactor * n * al * (1.0 + als / 6.0 * (1.0 - t + c + als / 20.0 * (5.0 - 18.0 * t + pow(t, 2) + 72.0 * c - 58.0 * esp))) + falseEasting; @@ -269,176 +243,151 @@ QVector LKS94Projection::DTM00(QVector & lonlat) (5.0 - t + 9.0 * c + 4.0 * pow(c, 2) + als / 30.0 * (61.0 - 58.0 * t + pow(t, 2) + 600.0 * c - 330.0 * esp))))) + falseNorthing; - if(lonlat.count() < 3) - { + if (lonlat.count() < 3) { QVector ret(2); - ret[0]= x / metersPerUnit; - ret[1]= y / metersPerUnit; + ret[0] = x / metersPerUnit; + ret[1] = y / metersPerUnit; return ret; - } - else - { + } else { QVector ret(3); - ret[0]= x / metersPerUnit; - ret[1]= y / metersPerUnit; - ret[2]=lonlat[2]; + ret[0] = x / metersPerUnit; + ret[1] = y / metersPerUnit; + ret[2] = lonlat[2]; return ret; } } -QVector LKS94Projection::DTM01(QVector & lonlat) +QVector LKS94Projection::DTM01(QVector & lonlat) { - double es; // Eccentricity squared : (a^2 - b^2)/a^2 - double semiMajor = 6378137.0; // major axis - double semiMinor = 6356752.3141403561; // minor axis - double ab; // Semi_major / semi_minor - double ba; // Semi_minor / semi_major - double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 + double es; // Eccentricity squared : (a^2 - b^2)/a^2 + double semiMajor = 6378137.0; // major axis + double semiMinor = 6356752.3141403561; // minor axis + double ab; // Semi_major / semi_minor + double ba; // Semi_minor / semi_major + double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 - es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); - ses = (pow(semiMajor, 2) -pow(semiMinor, 2)) / pow(semiMinor, 2); - ba = semiMinor / semiMajor; - ab = semiMajor / semiMinor; + es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); + ses = (pow(semiMajor, 2) - pow(semiMinor, 2)) / pow(semiMinor, 2); + ba = semiMinor / semiMajor; + ab = semiMajor / semiMinor; // ... double lon = DegreesToRadians(lonlat[0]); double lat = DegreesToRadians(lonlat[1]); - double h = lonlat.count() < 3 ? 0 : std::isnan(lonlat[2]) ? 0 : lonlat[2];//TODO NaN - double v = semiMajor / sqrt(1 - es * pow(sin(lat), 2)); - double x = (v + h) * cos(lat) * cos(lon); - double y = (v + h) * cos(lat) * sin(lon); - double z = ((1 - es) * v + h) * sin(lat); + double h = lonlat.count() < 3 ? 0 : std::isnan(lonlat[2]) ? 0 : lonlat[2]; // TODO NaN + double v = semiMajor / sqrt(1 - es * pow(sin(lat), 2)); + double x = (v + h) * cos(lat) * cos(lon); + double y = (v + h) * cos(lat) * sin(lon); + double z = ((1 - es) * v + h) * sin(lat); QVector ret(3); - ret[0]=x; - ret[1]=y; - ret[2]=z; + ret[0] = x; + ret[1] = y; + ret[2] = z; return ret; } -QVector LKS94Projection::MTD01(QVector & pnt) +QVector LKS94Projection::MTD01(QVector & pnt) { const double COS_67P5 = 0.38268343236508977; // cosine of 67.5 degrees - const double AD_C = 1.0026000; // Toms region 1 constant + const double AD_C = 1.0026000; // Toms region 1 constant - double es; // Eccentricity squared : (a^2 - b^2)/a^2 - double semiMajor = 6378137.0; // major axis - double semiMinor = 6356752.3142451793; // minor axis - double ab; // Semi_major / semi_minor - double ba; // Semi_minor / semi_major - double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 + double es; // Eccentricity squared : (a^2 - b^2)/a^2 + double semiMajor = 6378137.0; // major axis + double semiMinor = 6356752.3142451793; // minor axis + double ab; // Semi_major / semi_minor + double ba; // Semi_minor / semi_major + double ses; // Second eccentricity squared : (a^2 - b^2)/b^2 - es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); + es = 1.0 - (semiMinor * semiMinor) / (semiMajor * semiMajor); ses = (pow(semiMajor, 2) - pow(semiMinor, 2)) / pow(semiMinor, 2); - ba = semiMinor / semiMajor; - ab = semiMajor / semiMinor; + ba = semiMinor / semiMajor; + ab = semiMajor / semiMinor; // ... - bool At_Pole = false; // is location in polar region - double Z = pnt.count() < 3 ? 0 : std::isnan(pnt[2]) ? 0 : pnt[2];//TODO NaN + bool At_Pole = false; // is location in polar region + double Z = pnt.count() < 3 ? 0 : std::isnan(pnt[2]) ? 0 : pnt[2]; // TODO NaN - double lon = 0; - double lat = 0; + double lon = 0; + double lat = 0; double Height = 0; - if(pnt[0] != 0.0) - { + if (pnt[0] != 0.0) { lon = atan2(pnt[1], pnt[0]); - } - else - { - if(pnt[1] > 0) - { + } else { + if (pnt[1] > 0) { lon = M_PI / 2; - } - else - if(pnt[1] < 0) - { + } else if (pnt[1] < 0) { lon = -M_PI * 0.5; - } - else - { + } else { At_Pole = true; - lon = 0.0; - if(Z > 0.0) // north pole - { + lon = 0.0; + if (Z > 0.0) { // north pole lat = M_PI * 0.5; - } - else - if(Z < 0.0) // south pole - { + } else if (Z < 0.0) { // south pole lat = -M_PI * 0.5; - } - else // center of earth - { + } else { // center of earth QVector ret(3); - ret[0]=RadiansToDegrees(lon); - ret[1]=RadiansToDegrees(M_PI * 0.5); - ret[2]=-semiMinor; + ret[0] = RadiansToDegrees(lon); + ret[1] = RadiansToDegrees(M_PI * 0.5); + ret[2] = -semiMinor; return ret; } } } - double W2 = pnt[0] * pnt[0] + pnt[1] * pnt[1]; // Square of distance from Z axis - double W = sqrt(W2); // distance from Z axis - double T0 = Z * AD_C; // initial estimate of vertical component - double S0 = sqrt(T0 * T0 + W2); //initial estimate of horizontal component - double Sin_B0 = T0 / S0; // sin(B0), B0 is estimate of Bowring aux variable - double Cos_B0 = W / S0; // cos(B0) + double W2 = pnt[0] * pnt[0] + pnt[1] * pnt[1]; // Square of distance from Z axis + double W = sqrt(W2); // distance from Z axis + double T0 = Z * AD_C; // initial estimate of vertical component + double S0 = sqrt(T0 * T0 + W2); // initial estimate of horizontal component + double Sin_B0 = T0 / S0; // sin(B0), B0 is estimate of Bowring aux variable + double Cos_B0 = W / S0; // cos(B0) double Sin3_B0 = pow(Sin_B0, 3); - double T1 = Z + semiMinor * ses * Sin3_B0; //corrected estimate of vertical component - double Sum = W - semiMajor * es * Cos_B0 * Cos_B0 * Cos_B0; // numerator of cos(phi1) - double S1 = sqrt(T1 * T1 + Sum * Sum); // corrected estimate of horizontal component - double Sin_p1 = T1 / S1; // sin(phi1), phi1 is estimated latitude - double Cos_p1 = Sum / S1; // cos(phi1) - double Rn = semiMajor / sqrt(1.0 - es * Sin_p1 * Sin_p1); // Earth radius at location + double T1 = Z + semiMinor * ses * Sin3_B0; // corrected estimate of vertical component + double Sum = W - semiMajor * es * Cos_B0 * Cos_B0 * Cos_B0; // numerator of cos(phi1) + double S1 = sqrt(T1 * T1 + Sum * Sum); // corrected estimate of horizontal component + double Sin_p1 = T1 / S1; // sin(phi1), phi1 is estimated latitude + double Cos_p1 = Sum / S1; // cos(phi1) + double Rn = semiMajor / sqrt(1.0 - es * Sin_p1 * Sin_p1); // Earth radius at location - if(Cos_p1 >= COS_67P5) - { + if (Cos_p1 >= COS_67P5) { Height = W / Cos_p1 - Rn; - } - else - if(Cos_p1 <= -COS_67P5) - { + } else if (Cos_p1 <= -COS_67P5) { Height = W / -Cos_p1 - Rn; - } - else - { + } else { Height = Z / Sin_p1 + Rn * (es - 1.0); } - if(!At_Pole) - { + if (!At_Pole) { lat = atan(Sin_p1 / Cos_p1); } QVector ret(3); - ret[0]=RadiansToDegrees(lon); - ret[1]=RadiansToDegrees(lat); - ret[2]=Height; + ret[0] = RadiansToDegrees(lon); + ret[1] = RadiansToDegrees(lat); + ret[2] = Height; return ret; } -QVector LKS94Projection::MTD11(QVector & p) +QVector LKS94Projection::MTD11(QVector & p) { - double scaleFactor = 0.9998; // scale factor - double centralMeridian = 0.41887902047863912; // Center qlonglongitude (projection center) - double latOrigin = 0.0; // center latitude - double falseNorthing = 0.0; // y offset in meters - double falseEasting = 500000.0; // x offset in meters - double semiMajor = 6378137.0; // major axis - double semiMinor = 6356752.3141403561; // minor axis - double metersPerUnit = 1.0; + double scaleFactor = 0.9998; // scale factor + double centralMeridian = 0.41887902047863912; // Center qlonglongitude (projection center) + double latOrigin = 0.0; // center latitude + double falseNorthing = 0.0; // y offset in meters + double falseEasting = 500000.0; // x offset in meters + double semiMajor = 6378137.0; // major axis + double semiMinor = 6356752.3141403561; // minor axis + double metersPerUnit = 1.0; - double e0, e1, e2, e3; // eccentricity constants - double e, es, esp; // eccentricity constants - double ml0; // small value m + double e0, e1, e2, e3; // eccentricity constants + double e, es, esp; // eccentricity constants + double ml0; // small value m - es =(semiMinor * semiMinor) / (semiMajor * semiMajor); - es=1.0-es; - e = sqrt(es); - e0 = e0fn(es); - e1 = e1fn(es); - e2 = e2fn(es); - e3 = e3fn(es); + es = (semiMinor * semiMinor) / (semiMajor * semiMajor); + es = 1.0 - es; + e = sqrt(es); + e0 = e0fn(es); + e1 = e1fn(es); + e2 = e2fn(es); + e3 = e3fn(es); ml0 = semiMajor * mlfn(e0, e1, e2, e3, latOrigin); esp = es / (1.0 - es); @@ -456,30 +405,30 @@ QVector LKS94Projection::MTD11(QVector & p) con = (ml0 + y / scaleFactor) / semiMajor; phi = con; - for(i = 0; ; i++) - { + for (i = 0;; i++) { delta_phi = ((con + e1 * sin(2.0 * phi) - e2 * sin(4.0 * phi) + e3 * sin(6.0 * phi)) / e0) - phi; phi += delta_phi; - if(fabs(delta_phi) <= EPSLoN) + if (fabs(delta_phi) <= EPSLoN) { break; + } - if(i >= max_iter) + if (i >= max_iter) { throw "Latitude failed to converge"; + } } - if(fabs(phi) < HALF_PI) - { - SinCos(phi, sin_phi, cos_phi); + if (fabs(phi) < HALF_PI) { + SinCos(phi, sin_phi, cos_phi); tan_phi = tan(phi); - c = esp * pow(cos_phi, 2); - cs = pow(c, 2); - t = pow(tan_phi, 2); - ts = pow(t, 2); + c = esp * pow(cos_phi, 2); + cs = pow(c, 2); + t = pow(tan_phi, 2); + ts = pow(t, 2); con = 1.0 - es * pow(sin_phi, 2); - n = semiMajor / sqrt(con); - r = n * (1.0 - es) / con; - d = x / (n * scaleFactor); - ds = pow(d, 2); + n = semiMajor / sqrt(con); + r = n * (1.0 - es) / con; + d = x / (n * scaleFactor); + ds = pow(d, 2); double lat = phi - (n * tan_phi * ds / r) * (0.5 - ds / 24.0 * (5.0 + 3.0 * t + 10.0 * c - 4.0 * cs - 9.0 * esp - ds / 30.0 * (61.0 + 90.0 * t + @@ -489,127 +438,115 @@ QVector LKS94Projection::MTD11(QVector & p) c - ds / 20.0 * (5.0 - 2.0 * c + 28.0 * t - 3.0 * cs + 8.0 * esp + 24.0 * ts))) / cos_phi)); - if(p.count() < 3) - { + if (p.count() < 3) { QVector ret(2); - ret[0]= RadiansToDegrees(lon); - ret[1]= RadiansToDegrees(lat); + ret[0] = RadiansToDegrees(lon); + ret[1] = RadiansToDegrees(lat); return ret; - } - else - { + } else { QVector ret(3); - ret[0]= RadiansToDegrees(lon); - ret[1]= RadiansToDegrees(lat); - ret[2]=p[2]; + ret[0] = RadiansToDegrees(lon); + ret[1] = RadiansToDegrees(lat); + ret[2] = p[2]; return ret; - //return new double[] { RadiansToDegrees(lon), RadiansToDegrees(lat), p[2] }; + // return new double[] { RadiansToDegrees(lon), RadiansToDegrees(lat), p[2] }; } - } - else - { - if(p.count() < 3) - { + } else { + if (p.count() < 3) { QVector ret(2); - ret[0]= RadiansToDegrees(HALF_PI * Sign(y)); - ret[1]= RadiansToDegrees(centralMeridian); + ret[0] = RadiansToDegrees(HALF_PI * Sign(y)); + ret[1] = RadiansToDegrees(centralMeridian); return ret; - } - - else - { + } else { QVector ret(3); - ret[0]= RadiansToDegrees(HALF_PI * Sign(y)); - ret[1]= RadiansToDegrees(centralMeridian); - ret[2]=p[2]; + ret[0] = RadiansToDegrees(HALF_PI * Sign(y)); + ret[1] = RadiansToDegrees(centralMeridian); + ret[2] = p[2]; return ret; } - } } -double LKS94Projection::Clip(double const& n, double const& minValue, double const& maxValue) +double LKS94Projection::Clip(double const & n, double const & minValue, double const & maxValue) { return qMin(qMax(n, minValue), maxValue); } -double LKS94Projection::GetTileMatrixResolution(int const& zoom) +double LKS94Projection::GetTileMatrixResolution(int const & zoom) { double ret = 0; - switch(zoom) - { + switch (zoom) { case 0: - { - ret = 1587.50317500635; - } - break; + { + ret = 1587.50317500635; + } + break; case 1: - { - ret = 793.751587503175; - } - break; + { + ret = 793.751587503175; + } + break; case 2: - { - ret = 529.167725002117; - } - break; + { + ret = 529.167725002117; + } + break; case 3: - { - ret = 264.583862501058; - } - break; + { + ret = 264.583862501058; + } + break; case 4: - { - ret = 132.291931250529; - } - break; + { + ret = 132.291931250529; + } + break; case 5: - { - ret = 52.9167725002117; - } - break; + { + ret = 52.9167725002117; + } + break; case 6: - { - ret = 26.4583862501058; - } - break; + { + ret = 26.4583862501058; + } + break; case 7: - { - ret = 13.2291931250529; - } - break; + { + ret = 13.2291931250529; + } + break; case 8: - { - ret = 6.61459656252646; - } - break; + { + ret = 6.61459656252646; + } + break; case 9: - { - ret = 2.64583862501058; - } - break; + { + ret = 2.64583862501058; + } + break; case 10: - { - ret = 1.32291931250529; - } - break; + { + ret = 1.32291931250529; + } + break; case 11: - { - ret = 0.529167725002117; - } - break; - + { + ret = 0.529167725002117; + } + break; } return ret; @@ -617,175 +554,171 @@ double LKS94Projection::GetTileMatrixResolution(int const& zoom) /* * Returns the conversion from pixels to meters */ -double LKS94Projection::GetGroundResolution(int const& zoom, double const& latitude) +double LKS94Projection::GetGroundResolution(int const & zoom, double const & latitude) { Q_UNUSED(zoom); Q_UNUSED(latitude); return GetTileMatrixResolution(zoom); } -Size LKS94Projection::GetTileMatrixMinXY(int const& zoom) +Size LKS94Projection::GetTileMatrixMinXY(int const & zoom) { Size ret; - switch(zoom) - { - + switch (zoom) { case 0: - { - ret = Size(12, 8); - } - break; + { + ret = Size(12, 8); + } + break; case 1: - { - ret = Size(24, 17); - } - break; + { + ret = Size(24, 17); + } + break; case 2: - { - ret = Size(37, 25); - } - break; + { + ret = Size(37, 25); + } + break; case 3: - { - ret = Size(74, 51); - } - break; + { + ret = Size(74, 51); + } + break; case 4: - { - ret = Size(149, 103); - } - break; + { + ret = Size(149, 103); + } + break; case 5: - { - ret = Size(374, 259); - } - break; + { + ret = Size(374, 259); + } + break; case 6: - { - ret = Size(749, 519); - } - break; + { + ret = Size(749, 519); + } + break; case 7: - { - ret = Size(1594, 1100); - } - break; + { + ret = Size(1594, 1100); + } + break; case 8: - { - ret = Size(3188, 2201); - } - break; + { + ret = Size(3188, 2201); + } + break; case 9: - { - ret = Size(7971, 5502); - } - break; + { + ret = Size(7971, 5502); + } + break; case 10: - { - ret = Size(15943, 11005); - } - break; + { + ret = Size(15943, 11005); + } + break; case 11: - { - ret = Size(39858, 27514); - } - break; + { + ret = Size(39858, 27514); + } + break; } return ret; } -Size LKS94Projection::GetTileMatrixMaxXY(int const& zoom) +Size LKS94Projection::GetTileMatrixMaxXY(int const & zoom) { Size ret; - switch(zoom) - { + switch (zoom) { case 0: - { - ret = Size(14, 10); - } - break; + { + ret = Size(14, 10); + } + break; case 1: - { - ret = Size(30, 20); - } - break; + { + ret = Size(30, 20); + } + break; case 2: - { - ret = Size(45, 31); - } - break; + { + ret = Size(45, 31); + } + break; case 3: - { - ret = Size(90, 62); - } - break; + { + ret = Size(90, 62); + } + break; case 4: - { - ret = Size(181, 125); - } - break; + { + ret = Size(181, 125); + } + break; case 5: - { - ret = Size(454, 311); - } - break; + { + ret = Size(454, 311); + } + break; case 6: - { - ret = Size(903, 623); - } - break; + { + ret = Size(903, 623); + } + break; case 7: - { - ret = Size(1718, 1193); - } - break; + { + ret = Size(1718, 1193); + } + break; case 8: - { - ret = Size(3437, 2386); - } - break; + { + ret = Size(3437, 2386); + } + break; case 9: - { - ret = Size(8594, 5966); - } - break; + { + ret = Size(8594, 5966); + } + break; case 10: - { - ret = Size(17189, 11932); - } - break; + { + ret = Size(17189, 11932); + } + break; case 11: - { - ret = Size(42972, 29831); - } - break; + { + ret = Size(42972, 29831); + } + break; } return ret; } - } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h index af230eca7..e7bd633ae 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file lks94projection.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file lks94projection.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef LKS94PROJECTION_H #define LKS94PROJECTION_H #include @@ -32,41 +32,38 @@ namespace projections { -class LKS94Projection:public internals::PureProjection -{ +class LKS94Projection : public internals::PureProjection { public: LKS94Projection(); - double GetTileMatrixResolution(int const& zoom); - virtual QString Type(){return "LKS94Projection";} + double GetTileMatrixResolution(int const & zoom); + virtual QString Type() + { + return "LKS94Projection"; + } virtual Size TileSize() const; virtual double Axis() const; virtual double Flattening() const; - virtual core::Point FromLatLngToPixel(double lat, double lng, int const& zoom); - virtual internals::PointLatLng FromPixelToLatLng(int const& x, int const& y, int const& zoom); - virtual double GetGroundResolution(int const& zoom, double const& latitude); - virtual Size GetTileMatrixMinXY(int const& zoom); - virtual Size GetTileMatrixMaxXY(int const& zoom); + virtual core::Point FromLatLngToPixel(double lat, double lng, int const & zoom); + virtual internals::PointLatLng FromPixelToLatLng(int const & x, int const & y, int const & zoom); + virtual double GetGroundResolution(int const & zoom, double const & latitude); + virtual Size GetTileMatrixMinXY(int const & zoom); + virtual Size GetTileMatrixMaxXY(int const & zoom); private: - const double MinLatitude; - const double MaxLatitude; - const double MinLongitude; - const double MaxLongitude; - const double orignX; - const double orignY; - Size tileSize; - QVector DTM10(const QVector & lonlat); - QVector MTD10(QVector & pnt); - QVector DTM00(QVector & lonlat); - QVector DTM01(QVector & lonlat); - QVector MTD01(QVector & pnt); - QVector MTD11(QVector & p); - double Clip(double const& n, double const& minValue, double const& maxValue); + const double MinLatitude; + const double MaxLatitude; + const double MinLongitude; + const double MaxLongitude; + const double orignX; + const double orignY; + Size tileSize; + QVector DTM10(const QVector & lonlat); + QVector MTD10(QVector & pnt); + QVector DTM00(QVector & lonlat); + QVector DTM01(QVector & lonlat); + QVector MTD01(QVector & pnt); + QVector MTD11(QVector & p); + double Clip(double const & n, double const & minValue, double const & maxValue); }; - } #endif // LKS94PROJECTION_H - - - - diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.cpp index 9f3021672..ad518031c 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.cpp @@ -1,60 +1,59 @@ /** -****************************************************************************** -* -* @file mercatorprojection.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file mercatorprojection.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "mercatorprojection.h" - + namespace projections { -MercatorProjection::MercatorProjection():MinLatitude(-85.05112878), MaxLatitude(85.05112878),MinLongitude(-177), -MaxLongitude(177), tileSize(256, 256) -{ -} +MercatorProjection::MercatorProjection() : MinLatitude(-85.05112878), MaxLatitude(85.05112878), MinLongitude(-177), + MaxLongitude(177), tileSize(256, 256) +{} Point MercatorProjection::FromLatLngToPixel(double lat, double lng, const int &zoom) { - Point ret;// = Point.Empty; + Point ret; // = Point.Empty; lat = Clip(lat, MinLatitude, MaxLatitude); lng = Clip(lng, MinLongitude, MaxLongitude); - double x = (lng + 180) / 360; + double x = (lng + 180) / 360; double sinLatitude = sin(lat * M_PI / 180); - double y = 0.5 - log((1 + sinLatitude) / (1 - sinLatitude)) / (4 * M_PI); + double y = 0.5 - log((1 + sinLatitude) / (1 - sinLatitude)) / (4 * M_PI); - Size s = GetTileMatrixSizePixel(zoom); + Size s = GetTileMatrixSizePixel(zoom); int mapSizeX = s.Width(); int mapSizeY = s.Height(); - ret.SetX((int) Clip(x * mapSizeX + 0.5, 0, mapSizeX - 1)); - ret.SetY((int) Clip(y * mapSizeY + 0.5, 0, mapSizeY - 1)); + ret.SetX((int)Clip(x * mapSizeX + 0.5, 0, mapSizeX - 1)); + ret.SetY((int)Clip(y * mapSizeY + 0.5, 0, mapSizeY - 1)); return ret; } internals::PointLatLng MercatorProjection::FromPixelToLatLng(const int &x, const int &y, const int &zoom) { - internals::PointLatLng ret;// = internals::PointLatLng.Empty; + internals::PointLatLng ret; // = internals::PointLatLng.Empty; Size s = GetTileMatrixSizePixel(zoom); double mapSizeX = s.Width(); @@ -82,13 +81,13 @@ double MercatorProjection::Axis() const } double MercatorProjection::Flattening() const { - return (1.0 / 298.257223563); + return 1.0 / 298.257223563; } Size MercatorProjection::GetTileMatrixMaxXY(const int &zoom) { Q_UNUSED(zoom); int xy = (1 << zoom); - return Size(xy - 1, xy - 1); + return Size(xy - 1, xy - 1); } Size MercatorProjection::GetTileMatrixMinXY(const int &zoom) { diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.h index d36b45c25..06c0b253b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojection.h @@ -1,55 +1,56 @@ /** -****************************************************************************** -* -* @file mercatorprojection.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file mercatorprojection.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef MERCATORPROJECTION_H #define MERCATORPROJECTION_H #include "../pureprojection.h" namespace projections { - class MercatorProjection:public internals::PureProjection -{ +class MercatorProjection : public internals::PureProjection { public: MercatorProjection(); - virtual QString Type(){return "MercatorProjection";} + virtual QString Type() + { + return "MercatorProjection"; + } virtual Size TileSize() const; virtual double Axis() const; - virtual double Flattening()const; - virtual core::Point FromLatLngToPixel(double lat, double lng, int const& zoom); - virtual internals::PointLatLng FromPixelToLatLng(const int &x,const int &y,const int &zoom); - virtual Size GetTileMatrixMinXY(const int &zoom); - virtual Size GetTileMatrixMaxXY(const int &zoom); + virtual double Flattening() const; + virtual core::Point FromLatLngToPixel(double lat, double lng, int const & zoom); + virtual internals::PointLatLng FromPixelToLatLng(const int &x, const int &y, const int &zoom); + virtual Size GetTileMatrixMinXY(const int &zoom); + virtual Size GetTileMatrixMaxXY(const int &zoom); private: const double MinLatitude; const double MaxLatitude; const double MinLongitude; const double MaxLongitude; - double Clip(double const& n, double const& minValue, double const& maxValue)const; + double Clip(double const & n, double const & minValue, double const & maxValue) const; Size tileSize; }; - } #endif // MERCATORPROJECTION_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.cpp index f2495c1f4..16dd65272 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.cpp @@ -1,38 +1,36 @@ /** -****************************************************************************** -* -* @file mercatorprojectionyandex.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file mercatorprojectionyandex.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "mercatorprojectionyandex.h" - namespace projections { -MercatorProjectionYandex::MercatorProjectionYandex():MinLatitude(-85.05112878), MaxLatitude(85.05112878),MinLongitude(-177), -MaxLongitude(177), RAD_DEG(180 / M_PI),DEG_RAD(M_PI / 180),MathPiDiv4(M_PI / 4),tileSize(256, 256) -{ -} +MercatorProjectionYandex::MercatorProjectionYandex() : MinLatitude(-85.05112878), MaxLatitude(85.05112878), MinLongitude(-177), + MaxLongitude(177), RAD_DEG(180 / M_PI), DEG_RAD(M_PI / 180), MathPiDiv4(M_PI / 4), tileSize(256, 256) +{} Point MercatorProjectionYandex::FromLatLngToPixel(double lat, double lng, const int &zoom) { lat = Clip(lat, MinLatitude, MaxLatitude); @@ -41,44 +39,44 @@ Point MercatorProjectionYandex::FromLatLngToPixel(double lat, double lng, const double rLon = lng * DEG_RAD; // Math.PI / 180; double rLat = lat * DEG_RAD; // Math.PI / 180; - double a = 6378137; - double k = 0.0818191908426; + double a = 6378137; + double k = 0.0818191908426; - double z = tan(MathPiDiv4 + rLat / 2) / pow((tan(MathPiDiv4 + asin(k * sin(rLat)) / 2)), k); - double z1 = pow(2, 23 - zoom); + double z = tan(MathPiDiv4 + rLat / 2) / pow((tan(MathPiDiv4 + asin(k * sin(rLat)) / 2)), k); + double z1 = pow(2, 23 - zoom); - double DX = ((20037508.342789 + a * rLon) * 53.5865938 / z1); - double DY = ((20037508.342789 - a * log(z)) * 53.5865938 / z1); + double DX = ((20037508.342789 + a * rLon) * 53.5865938 / z1); + double DY = ((20037508.342789 - a * log(z)) * 53.5865938 / z1); - Point ret;// = Point.Empty; - ret.SetX((int) DX); - ret.SetY((int) DY); + Point ret; // = Point.Empty; + ret.SetX((int)DX); + ret.SetY((int)DY); return ret; - } internals::PointLatLng MercatorProjectionYandex::FromPixelToLatLng(const int &x, const int &y, const int &zoom) { Size s = GetTileMatrixSizePixel(zoom); - //double mapSizeX = s.Width(); - //double mapSizeY = s.Height(); + // double mapSizeX = s.Width(); + // double mapSizeY = s.Height(); - double a = 6378137; - double c1 = 0.00335655146887969; - double c2 = 0.00000657187271079536; - double c3 = 0.00000001764564338702; - double c4 = 0.00000000005328478445; - double z1 = (23 - zoom); + double a = 6378137; + double c1 = 0.00335655146887969; + double c2 = 0.00000657187271079536; + double c3 = 0.00000001764564338702; + double c4 = 0.00000000005328478445; + double z1 = (23 - zoom); double mercX = (x * pow(2, z1)) / 53.5865938 - 20037508.342789; - double mercY = 20037508.342789 - (y *pow(2, z1)) / 53.5865938; + double mercY = 20037508.342789 - (y * pow(2, z1)) / 53.5865938; - double g = M_PI /2 - 2 *atan(1 / exp(mercY /a)); - double z = g + c1 * sin(2 * g) + c2 * sin(4 * g) + c3 * sin(6 * g) + c4 * sin(8 * g); + double g = M_PI / 2 - 2 * atan(1 / exp(mercY / a)); + double z = g + c1 * sin(2 * g) + c2 * sin(4 * g) + c3 * sin(6 * g) + c4 * sin(8 * g); + + internals::PointLatLng ret; // = internals::PointLatLng.Empty; - internals::PointLatLng ret;// = internals::PointLatLng.Empty; ret.SetLat(z * RAD_DEG); - ret.SetLng (mercX / a * RAD_DEG); + ret.SetLng(mercX / a * RAD_DEG); return ret; } @@ -96,12 +94,13 @@ double MercatorProjectionYandex::Axis() const } double MercatorProjectionYandex::Flattening() const { - return (1.0 / 298.257223563); + return 1.0 / 298.257223563; } Size MercatorProjectionYandex::GetTileMatrixMaxXY(const int &zoom) { int xy = (1 << zoom); - return Size(xy - 1, xy - 1); + + return Size(xy - 1, xy - 1); } Size MercatorProjectionYandex::GetTileMatrixMinXY(const int &zoom) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.h index f7be2c786..425bcfe7a 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/mercatorprojectionyandex.h @@ -1,48 +1,50 @@ /** -****************************************************************************** -* -* @file mercatorprojectionyandex.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file mercatorprojectionyandex.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef MERCATORPROJECTIONYANDEX_H #define MERCATORPROJECTIONYANDEX_H #include "../pureprojection.h" - + namespace projections { - class MercatorProjectionYandex:public internals::PureProjection -{ +class MercatorProjectionYandex : public internals::PureProjection { public: MercatorProjectionYandex(); - virtual QString Type(){return "MercatorProjectionYandex";} + virtual QString Type() + { + return "MercatorProjectionYandex"; + } virtual Size TileSize() const; virtual double Axis() const; - virtual double Flattening()const; - virtual core::Point FromLatLngToPixel(double lat, double lng, int const& zoom); - virtual internals::PointLatLng FromPixelToLatLng(const int &x,const int &y,const int &zoom); - virtual Size GetTileMatrixMinXY(const int &zoom); - virtual Size GetTileMatrixMaxXY(const int &zoom); + virtual double Flattening() const; + virtual core::Point FromLatLngToPixel(double lat, double lng, int const & zoom); + virtual internals::PointLatLng FromPixelToLatLng(const int &x, const int &y, const int &zoom); + virtual Size GetTileMatrixMinXY(const int &zoom); + virtual Size GetTileMatrixMaxXY(const int &zoom); private: const double MinLatitude; const double MaxLatitude; @@ -51,9 +53,8 @@ private: const double RAD_DEG; const double DEG_RAD; const double MathPiDiv4; - double Clip(double const& n, double const& minValue, double const& maxValue)const; + double Clip(double const & n, double const & minValue, double const & maxValue) const; Size tileSize; }; - } #endif // MERCATORPROJECTIONYANDEX_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp index 16b0f0400..30fedd2b7 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp @@ -1,66 +1,63 @@ /** -****************************************************************************** -* -* @file platecarreeprojection.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file platecarreeprojection.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "platecarreeprojection.h" - namespace projections { -PlateCarreeProjection::PlateCarreeProjection():MinLatitude(-85.05112878), MaxLatitude(85.05112878),MinLongitude(-180), -MaxLongitude(180), tileSize(512, 512) -{ -} +PlateCarreeProjection::PlateCarreeProjection() : MinLatitude(-85.05112878), MaxLatitude(85.05112878), MinLongitude(-180), + MaxLongitude(180), tileSize(512, 512) +{} Point PlateCarreeProjection::FromLatLngToPixel(double lat, double lng, const int &zoom) { - Point ret;// = Point.Empty; + Point ret; // = Point.Empty; lat = Clip(lat, MinLatitude, MaxLatitude); lng = Clip(lng, MinLongitude, MaxLongitude); Size s = GetTileMatrixSizePixel(zoom); double mapSizeX = s.Width(); - //double mapSizeY = s.Height(); + // double mapSizeY = s.Height(); - double scale = 360.0 / mapSizeX; + double scale = 360.0 / mapSizeX; - ret.SetY((int) ((90.0 - lat) / scale)); - ret.SetX((int) ((lng + 180.0) / scale)); + ret.SetY((int)((90.0 - lat) / scale)); + ret.SetX((int)((lng + 180.0) / scale)); return ret; - } internals::PointLatLng PlateCarreeProjection::FromPixelToLatLng(const int &x, const int &y, const int &zoom) { - internals::PointLatLng ret;// = internals::PointLatLng.Empty; + internals::PointLatLng ret; // = internals::PointLatLng.Empty; Size s = GetTileMatrixSizePixel(zoom); double mapSizeX = s.Width(); - //double mapSizeY = s.Height(); + // double mapSizeY = s.Height(); - double scale = 360.0 / mapSizeX; + double scale = 360.0 / mapSizeX; ret.SetLat(90 - (y * scale)); ret.SetLng((x * scale) - 180); @@ -81,12 +78,13 @@ double PlateCarreeProjection::Axis() const } double PlateCarreeProjection::Flattening() const { - return (1.0 / 298.257223563); + return 1.0 / 298.257223563; } Size PlateCarreeProjection::GetTileMatrixMaxXY(const int &zoom) { - int y = (int) pow(2, zoom); - return Size((2*y) - 1, y - 1); + int y = (int)pow(2, zoom); + + return Size((2 * y) - 1, y - 1); } Size PlateCarreeProjection::GetTileMatrixMinXY(const int &zoom) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.h index 010cf430a..d65816484 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.h @@ -1,55 +1,57 @@ /** -****************************************************************************** -* -* @file platecarreeprojection.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file platecarreeprojection.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef PLATECARREEPROJECTION_H #define PLATECARREEPROJECTION_H #include "../pureprojection.h" - + namespace projections { -class PlateCarreeProjection:public internals::PureProjection -{ +class PlateCarreeProjection : public internals::PureProjection { public: PlateCarreeProjection(); - virtual QString Type(){return "PlateCarreeProjection";} + virtual QString Type() + { + return "PlateCarreeProjection"; + } virtual Size TileSize() const; virtual double Axis() const; - virtual double Flattening()const; - virtual core::Point FromLatLngToPixel(double lat, double lng, int const& zoom); - virtual internals::PointLatLng FromPixelToLatLng(const int &x,const int &y,const int &zoom); - virtual Size GetTileMatrixMinXY(const int &zoom); - virtual Size GetTileMatrixMaxXY(const int &zoom); + virtual double Flattening() const; + virtual core::Point FromLatLngToPixel(double lat, double lng, int const & zoom); + virtual internals::PointLatLng FromPixelToLatLng(const int &x, const int &y, const int &zoom); + virtual Size GetTileMatrixMinXY(const int &zoom); + virtual Size GetTileMatrixMaxXY(const int &zoom); private: const double MinLatitude; const double MaxLatitude; const double MinLongitude; const double MaxLongitude; - double Clip(double const& n, double const& minValue, double const& maxValue)const; + double Clip(double const & n, double const & minValue, double const & maxValue) const; Size tileSize; }; } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp index e746036c0..3c657aea4 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp @@ -1,64 +1,63 @@ /** -****************************************************************************** -* -* @file platecarreeprojectionpergo.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file platecarreeprojectionpergo.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "platecarreeprojectionpergo.h" - + namespace projections { -PlateCarreeProjectionPergo::PlateCarreeProjectionPergo():MinLatitude(-85.05112878), MaxLatitude(85.05112878),MinLongitude(-180), -MaxLongitude(180), tileSize(256, 256) -{ -} +PlateCarreeProjectionPergo::PlateCarreeProjectionPergo() : MinLatitude(-85.05112878), MaxLatitude(85.05112878), MinLongitude(-180), + MaxLongitude(180), tileSize(256, 256) +{} Point PlateCarreeProjectionPergo::FromLatLngToPixel(double lat, double lng, const int &zoom) { - Point ret;// = Point.Empty; + Point ret; // = Point.Empty; lat = Clip(lat, MinLatitude, MaxLatitude); lng = Clip(lng, MinLongitude, MaxLongitude); Size s = GetTileMatrixSizePixel(zoom); double mapSizeX = s.Width(); - //double mapSizeY = s.Height(); + // double mapSizeY = s.Height(); - double scale = 360.0 / mapSizeX; + double scale = 360.0 / mapSizeX; - ret.SetY((int) ((90.0 - lat) / scale)); - ret.SetX((int) ((lng + 180.0) / scale)); + ret.SetY((int)((90.0 - lat) / scale)); + ret.SetX((int)((lng + 180.0) / scale)); return ret; } internals::PointLatLng PlateCarreeProjectionPergo::FromPixelToLatLng(const int &x, const int &y, const int &zoom) { - internals::PointLatLng ret;// = internals::PointLatLng.Empty; + internals::PointLatLng ret; // = internals::PointLatLng.Empty; Size s = GetTileMatrixSizePixel(zoom); double mapSizeX = s.Width(); - //double mapSizeY = s.Height(); + // double mapSizeY = s.Height(); - double scale = 360.0 / mapSizeX; + double scale = 360.0 / mapSizeX; ret.SetLat(90 - (y * scale)); ret.SetLng((x * scale) - 180); @@ -80,12 +79,13 @@ double PlateCarreeProjectionPergo::Axis() const } double PlateCarreeProjectionPergo::Flattening() const { - return (1.0 / 298.257223563); + return 1.0 / 298.257223563; } Size PlateCarreeProjectionPergo::GetTileMatrixMaxXY(const int &zoom) { - int y = (int) pow(2, zoom); - return Size((2*y) - 1, y - 1); + int y = (int)pow(2, zoom); + + return Size((2 * y) - 1, y - 1); } Size PlateCarreeProjectionPergo::GetTileMatrixMinXY(const int &zoom) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.h index f5f80c316..baa6811b3 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.h @@ -1,55 +1,57 @@ /** -****************************************************************************** -* -* @file platecarreeprojectionpergo.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file platecarreeprojectionpergo.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef PLATECARREEPROJECTIONPERGO_H #define PLATECARREEPROJECTIONPERGO_H #include "../pureprojection.h" - + namespace projections { -class PlateCarreeProjectionPergo:public internals::PureProjection -{ +class PlateCarreeProjectionPergo : public internals::PureProjection { public: PlateCarreeProjectionPergo(); - virtual QString Type(){return "PlateCarreeProjectionPergo";} + virtual QString Type() + { + return "PlateCarreeProjectionPergo"; + } virtual Size TileSize() const; virtual double Axis() const; - virtual double Flattening()const; - virtual core::Point FromLatLngToPixel(double lat, double lng, int const& zoom); - virtual internals::PointLatLng FromPixelToLatLng(const int &x,const int &y,const int &zoom); - virtual Size GetTileMatrixMinXY(const int &zoom); - virtual Size GetTileMatrixMaxXY(const int &zoom); + virtual double Flattening() const; + virtual core::Point FromLatLngToPixel(double lat, double lng, int const & zoom); + virtual internals::PointLatLng FromPixelToLatLng(const int &x, const int &y, const int &zoom); + virtual Size GetTileMatrixMinXY(const int &zoom); + virtual Size GetTileMatrixMaxXY(const int &zoom); private: const double MinLatitude; const double MaxLatitude; const double MinLongitude; const double MaxLongitude; - double Clip(double const& n, double const& minValue, double const& maxValue)const; + double Clip(double const & n, double const & minValue, double const & maxValue) const; Size tileSize; }; } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.cpp index 2eab113a1..6762241d8 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.cpp @@ -1,277 +1,264 @@ /** -****************************************************************************** -* -* @file pureprojection.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file pureprojection.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "pureprojection.h" - - - - namespace internals { - -const double PureProjection::PI = M_PI; +const double PureProjection::PI = M_PI; const double PureProjection::HALF_PI = (M_PI * 0.5); -const double PureProjection::TWO_PI= (M_PI * 2.0); -const double PureProjection::EPSLoN= 1.0e-10; -const double PureProjection::MAX_VAL= 4; -const double PureProjection::MAXLONG= 2147483647; -const double PureProjection::DBLLONG= 4.61168601e18; -const double PureProjection::R2D=180/M_PI; -const double PureProjection::D2R=M_PI/180; - -Point PureProjection::FromLatLngToPixel(const PointLatLng &p,const int &zoom) - { - return FromLatLngToPixel(p.Lat(), p.Lng(), zoom); - } - - - PointLatLng PureProjection::FromPixelToLatLng(const Point &p,const int &zoom) - { - return FromPixelToLatLng(p.X(), p.Y(), zoom); - } - - Point PureProjection::FromPixelToTileXY(const Point &p) - { - return Point((int) (p.X() / TileSize().Width()), (int) (p.Y() / TileSize().Height())); - } - - Point PureProjection::FromTileXYToPixel(const Point &p) - { - return Point((p.X() * TileSize().Width()), (p.Y() * TileSize().Height())); - } - - Size PureProjection::GetTileMatrixSizeXY(const int &zoom) - { - Size sMin = GetTileMatrixMinXY(zoom); - Size sMax = GetTileMatrixMaxXY(zoom); - - return Size(sMax.Width() - sMin.Width() + 1, sMax.Height() - sMin.Height() + 1); - } - int PureProjection::GetTileMatrixItemCount(const int &zoom) - { - Size s = GetTileMatrixSizeXY(zoom); - return (s.Width() * s.Height()); - } - Size PureProjection::GetTileMatrixSizePixel(const int &zoom) - { - Size s = GetTileMatrixSizeXY(zoom); - return Size(s.Width() * TileSize().Width(), s.Height() * TileSize().Height()); - } - QList PureProjection::GetAreaTileList(const RectLatLng &rect,const int &zoom,const int &padding) - { - QList ret; - - Point topLeft = FromPixelToTileXY(FromLatLngToPixel(rect.LocationTopLeft(), zoom)); - Point rightBottom = FromPixelToTileXY(FromLatLngToPixel(rect.Bottom(), rect.Right(), zoom)); - - for(int x = (topLeft.X() - padding); x <= (rightBottom.X() + padding); x++) - { - for(int y = (topLeft.Y() - padding); y <= (rightBottom.Y() + padding); y++) - { - Point p = Point(x, y); - if(!ret.contains(p) && p.X() >= 0 && p.Y() >= 0) - { - ret.append(p); - } - } - } - //ret.TrimExcess(); - - return ret; - } - /* - * Returns the conversion from pixels to meters - */ - double PureProjection::GetGroundResolution(const int &zoom,const double &latitude) - { - return (cos(latitude * (PI / 180)) * 2 * PI * Axis()) / GetTileMatrixSizePixel(zoom).Width(); - } - - double PureProjection::Sign(const double &x) - { - if(x < 0.0) - return (-1); - else - return (1); - } - - double PureProjection::AdjustLongitude(double x) - { - qlonglong count = 0; - while(true) - { - if(qAbs(x) <= PI) - break; - else - if(((qlonglong) qAbs(x / PI)) < 2) - x = x - (Sign(x) * TWO_PI); - - else - if(((qlonglong) qAbs(x / TWO_PI)) < MAXLONG) - { - x = x - (((qlonglong) (x / TWO_PI)) * TWO_PI); - } - else - if(((qlonglong) qAbs(x / (MAXLONG * TWO_PI))) < MAXLONG) - { - x = x - (((qlonglong) (x / (MAXLONG * TWO_PI))) * (TWO_PI * MAXLONG)); - } - else - if(((qlonglong) qAbs(x / (DBLLONG * TWO_PI))) < MAXLONG) - { - x = x - (((qlonglong) (x / (DBLLONG * TWO_PI))) * (TWO_PI * DBLLONG)); - } - else - x = x - (Sign(x) * TWO_PI); - count++; - if(count > MAX_VAL) - break; - } - return (x); - } - - void PureProjection::SinCos(const double &val, double &si, double &co) - { - si = sin(val); - co = cos(val); - } - - double PureProjection::e0fn(const double &x) - { - return (1.0 - 0.25 * x * (1.0 + x / 16.0 * (3.0 + 1.25 * x))); - } - - double PureProjection::e1fn(const double &x) - { - return (0.375 * x * (1.0 + 0.25 * x * (1.0 + 0.46875 * x))); - } - - double PureProjection::e2fn(const double &x) - { - return (0.05859375 * x * x * (1.0 + 0.75 * x)); - } - - double PureProjection::e3fn(const double &x) - { - return (x * x * x * (35.0 / 3072.0)); - } - - double PureProjection::mlfn(const double &e0,const double &e1,const double &e2,const double &e3,const double &phi) - { - return (e0 * phi - e1 * sin(2.0 * phi) + e2 * sin(4.0 * phi) - e3 * sin(6.0 * phi)); - } - - qlonglong PureProjection::GetUTMzone(const double &lon) - { - return ((qlonglong) (((lon + 180.0) / 6.0) + 1.0)); - } - - - void PureProjection::FromGeodeticToCartesian(double Lat,double Lng,double Height, double &X, double &Y, double &Z) - { - Lat = (PI / 180) * Lat; - Lng = (PI / 180) * Lng; - - double B = Axis() * (1.0 - Flattening()); - double ee = 1.0 - (B / Axis()) * (B / Axis()); - double N = (Axis() / sqrt(1.0 - ee * sin(Lat) * sin(Lat))); - - X = (N + Height) * cos(Lat) * cos(Lng); - Y = (N + Height) * cos(Lat) * sin(Lng); - Z = (N * (B / Axis()) * (B / Axis()) + Height) * sin(Lat); - } - void PureProjection::FromCartesianTGeodetic(const double &X,const double &Y,const double &Z, double &Lat, double &Lng) - { - double E = Flattening() * (2.0 - Flattening()); - Lng = atan2(Y, X); - - double P = sqrt(X * X + Y * Y); - double Theta = atan2(Z, (P * (1.0 - Flattening()))); - double st = sin(Theta); - double ct = cos(Theta); - Lat = atan2(Z + E / (1.0 - Flattening()) * Axis() * st * st * st, P - E * Axis() * ct * ct * ct); - - Lat /= (PI / 180); - Lng /= (PI / 180); - } - double PureProjection::courseBetweenLatLng(PointLatLng const& p1,PointLatLng const& p2) - { - - double lon1=p1.Lng()* (M_PI / 180); - double lat1=p1.Lat()* (M_PI / 180); - double lon2=p2.Lng()* (M_PI / 180); - double lat2=p2.Lat()* (M_PI / 180); - - return 2*M_PI-myfmod(atan2(sin(lon1-lon2)*cos(lat2), - cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon1-lon2)), 2*M_PI); - } - - double PureProjection::DistanceBetweenLatLng(PointLatLng const& p1,PointLatLng const& p2) - { - double R = 6371; // km - double lat1=p1.Lat(); - double lat2=p2.Lat(); - double lon1=p1.Lng(); - double lon2=p2.Lng(); - double dLat = (lat2-lat1)* (PI / 180); - double dLon = (lon2-lon1)* (PI / 180); - double a = sin(dLat/2) * sin(dLat/2) + cos(lat1* (PI / 180)) * cos(lat2* (PI / 180)) * sin(dLon/2) * sin(dLon/2); - double c = 2 * atan2(sqrt(a), sqrt(1-a)); - double d = R * c; - return d; - } - - void PureProjection::offSetFromLatLngs(PointLatLng p1,PointLatLng p2,double &distance,double &bearing) - { - distance=DistanceBetweenLatLng(p1,p2)*1000; - bearing=courseBetweenLatLng(p1,p2); - } - - double PureProjection::myfmod(double x,double y) - { - return x - y*floor(x/y); - } - - PointLatLng PureProjection::translate(PointLatLng p1,double distance,double bearing) - { - PointLatLng ret; - double d=distance; - double tc=bearing; - double lat1=p1.Lat()*M_PI/180; - double lon1=p1.Lng()*M_PI/180; - double R=6378137; - double lat2 = asin(sin(lat1)*cos(d/R) + cos(lat1)*sin(d/R)*cos(tc) ); - double lon2 = lon1 + atan2(sin(tc)*sin(d/R)*cos(lat1), - cos(d/R)-sin(lat1)*sin(lat2)); - lat2=lat2*180/M_PI; - lon2=lon2*180/M_PI; - ret.SetLat(lat2); - ret.SetLng(lon2); - return ret; - } +const double PureProjection::TWO_PI = (M_PI * 2.0); +const double PureProjection::EPSLoN = 1.0e-10; +const double PureProjection::MAX_VAL = 4; +const double PureProjection::MAXLONG = 2147483647; +const double PureProjection::DBLLONG = 4.61168601e18; +const double PureProjection::R2D = 180 / M_PI; +const double PureProjection::D2R = M_PI / 180; +Point PureProjection::FromLatLngToPixel(const PointLatLng &p, const int &zoom) +{ + return FromLatLngToPixel(p.Lat(), p.Lng(), zoom); +} + + +PointLatLng PureProjection::FromPixelToLatLng(const Point &p, const int &zoom) +{ + return FromPixelToLatLng(p.X(), p.Y(), zoom); +} + +Point PureProjection::FromPixelToTileXY(const Point &p) +{ + return Point((int)(p.X() / TileSize().Width()), (int)(p.Y() / TileSize().Height())); +} + +Point PureProjection::FromTileXYToPixel(const Point &p) +{ + return Point((p.X() * TileSize().Width()), (p.Y() * TileSize().Height())); +} + +Size PureProjection::GetTileMatrixSizeXY(const int &zoom) +{ + Size sMin = GetTileMatrixMinXY(zoom); + Size sMax = GetTileMatrixMaxXY(zoom); + + return Size(sMax.Width() - sMin.Width() + 1, sMax.Height() - sMin.Height() + 1); +} +int PureProjection::GetTileMatrixItemCount(const int &zoom) +{ + Size s = GetTileMatrixSizeXY(zoom); + + return s.Width() * s.Height(); +} +Size PureProjection::GetTileMatrixSizePixel(const int &zoom) +{ + Size s = GetTileMatrixSizeXY(zoom); + + return Size(s.Width() * TileSize().Width(), s.Height() * TileSize().Height()); +} +QList PureProjection::GetAreaTileList(const RectLatLng &rect, const int &zoom, const int &padding) +{ + QList ret; + + Point topLeft = FromPixelToTileXY(FromLatLngToPixel(rect.LocationTopLeft(), zoom)); + Point rightBottom = FromPixelToTileXY(FromLatLngToPixel(rect.Bottom(), rect.Right(), zoom)); + + for (int x = (topLeft.X() - padding); x <= (rightBottom.X() + padding); x++) { + for (int y = (topLeft.Y() - padding); y <= (rightBottom.Y() + padding); y++) { + Point p = Point(x, y); + if (!ret.contains(p) && p.X() >= 0 && p.Y() >= 0) { + ret.append(p); + } + } + } + // ret.TrimExcess(); + + return ret; +} +/* + * Returns the conversion from pixels to meters + */ +double PureProjection::GetGroundResolution(const int &zoom, const double &latitude) +{ + return (cos(latitude * (PI / 180)) * 2 * PI * Axis()) / GetTileMatrixSizePixel(zoom).Width(); +} + +double PureProjection::Sign(const double &x) +{ + if (x < 0.0) { + return -1; + } else { + return 1; + } +} + +double PureProjection::AdjustLongitude(double x) +{ + qlonglong count = 0; + + while (true) { + if (qAbs(x) <= PI) { + break; + } else if (((qlonglong)qAbs(x / PI)) < 2) { + x = x - (Sign(x) * TWO_PI); + } else if (((qlonglong)qAbs(x / TWO_PI)) < MAXLONG) { + x = x - (((qlonglong)(x / TWO_PI)) * TWO_PI); + } else if (((qlonglong)qAbs(x / (MAXLONG * TWO_PI))) < MAXLONG) { + x = x - (((qlonglong)(x / (MAXLONG * TWO_PI))) * (TWO_PI * MAXLONG)); + } else if (((qlonglong)qAbs(x / (DBLLONG * TWO_PI))) < MAXLONG) { + x = x - (((qlonglong)(x / (DBLLONG * TWO_PI))) * (TWO_PI * DBLLONG)); + } else { + x = x - (Sign(x) * TWO_PI); + } + count++; + if (count > MAX_VAL) { + break; + } + } + return x; +} + +void PureProjection::SinCos(const double &val, double &si, double &co) +{ + si = sin(val); + co = cos(val); +} + +double PureProjection::e0fn(const double &x) +{ + return 1.0 - 0.25 * x * (1.0 + x / 16.0 * (3.0 + 1.25 * x)); +} + +double PureProjection::e1fn(const double &x) +{ + return 0.375 * x * (1.0 + 0.25 * x * (1.0 + 0.46875 * x)); +} + +double PureProjection::e2fn(const double &x) +{ + return 0.05859375 * x * x * (1.0 + 0.75 * x); +} + +double PureProjection::e3fn(const double &x) +{ + return x * x * x * (35.0 / 3072.0); +} + +double PureProjection::mlfn(const double &e0, const double &e1, const double &e2, const double &e3, const double &phi) +{ + return e0 * phi - e1 * sin(2.0 * phi) + e2 * sin(4.0 * phi) - e3 * sin(6.0 * phi); +} + +qlonglong PureProjection::GetUTMzone(const double &lon) +{ + return (qlonglong)(((lon + 180.0) / 6.0) + 1.0); +} + + +void PureProjection::FromGeodeticToCartesian(double Lat, double Lng, double Height, double &X, double &Y, double &Z) +{ + Lat = (PI / 180) * Lat; + Lng = (PI / 180) * Lng; + + double B = Axis() * (1.0 - Flattening()); + double ee = 1.0 - (B / Axis()) * (B / Axis()); + double N = (Axis() / sqrt(1.0 - ee * sin(Lat) * sin(Lat))); + + X = (N + Height) * cos(Lat) * cos(Lng); + Y = (N + Height) * cos(Lat) * sin(Lng); + Z = (N * (B / Axis()) * (B / Axis()) + Height) * sin(Lat); +} +void PureProjection::FromCartesianTGeodetic(const double &X, const double &Y, const double &Z, double &Lat, double &Lng) +{ + double E = Flattening() * (2.0 - Flattening()); + + Lng = atan2(Y, X); + + double P = sqrt(X * X + Y * Y); + double Theta = atan2(Z, (P * (1.0 - Flattening()))); + double st = sin(Theta); + double ct = cos(Theta); + Lat = atan2(Z + E / (1.0 - Flattening()) * Axis() * st * st * st, P - E * Axis() * ct * ct * ct); + + Lat /= (PI / 180); + Lng /= (PI / 180); +} +double PureProjection::courseBetweenLatLng(PointLatLng const & p1, PointLatLng const & p2) +{ + double lon1 = p1.Lng() * (M_PI / 180); + double lat1 = p1.Lat() * (M_PI / 180); + double lon2 = p2.Lng() * (M_PI / 180); + double lat2 = p2.Lat() * (M_PI / 180); + + return 2 * M_PI - myfmod(atan2(sin(lon1 - lon2) * cos(lat2), + cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon1 - lon2)), 2 * M_PI); +} + +double PureProjection::DistanceBetweenLatLng(PointLatLng const & p1, PointLatLng const & p2) +{ + double R = 6371; // km + double lat1 = p1.Lat(); + double lat2 = p2.Lat(); + double lon1 = p1.Lng(); + double lon2 = p2.Lng(); + double dLat = (lat2 - lat1) * (PI / 180); + double dLon = (lon2 - lon1) * (PI / 180); + double a = sin(dLat / 2) * sin(dLat / 2) + cos(lat1 * (PI / 180)) * cos(lat2 * (PI / 180)) * sin(dLon / 2) * sin(dLon / 2); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + double d = R * c; + + return d; +} + +void PureProjection::offSetFromLatLngs(PointLatLng p1, PointLatLng p2, double &distance, double &bearing) +{ + distance = DistanceBetweenLatLng(p1, p2) * 1000; + bearing = courseBetweenLatLng(p1, p2); +} + +double PureProjection::myfmod(double x, double y) +{ + return x - y * floor(x / y); +} + +PointLatLng PureProjection::translate(PointLatLng p1, double distance, double bearing) +{ + PointLatLng ret; + double d = distance; + double tc = bearing; + double lat1 = p1.Lat() * M_PI / 180; + double lon1 = p1.Lng() * M_PI / 180; + double R = 6378137; + double lat2 = asin(sin(lat1) * cos(d / R) + cos(lat1) * sin(d / R) * cos(tc)); + double lon2 = lon1 + atan2(sin(tc) * sin(d / R) * cos(lat1), + cos(d / R) - sin(lat1) * sin(lat2)); + + lat2 = lat2 * 180 / M_PI; + lon2 = lon2 * 180 / M_PI; + ret.SetLat(lat2); + ret.SetLng(lon2); + return ret; +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.h index 6c41db846..7ebc11d1a 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file pureprojection.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file pureprojection.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef PUREPROJECTION_H #define PUREPROJECTION_H @@ -36,56 +36,54 @@ #include using namespace core; -namespace internals -{ - -class PureProjection -{ - - +namespace internals { +class PureProjection { public: - virtual Size TileSize()const=0; + virtual Size TileSize() const = 0; - virtual double Axis()const=0; + virtual double Axis() const = 0; - virtual double Flattening()const=0; + virtual double Flattening() const = 0; - virtual core::Point FromLatLngToPixel(double lat, double lng, int const& zoom)=0; + virtual core::Point FromLatLngToPixel(double lat, double lng, int const & zoom) = 0; - virtual PointLatLng FromPixelToLatLng(const int &x,const int &y,const int &zoom)=0; + virtual PointLatLng FromPixelToLatLng(const int &x, const int &y, const int &zoom) = 0; - virtual QString Type(){return "PureProjection";} - core::Point FromLatLngToPixel(const PointLatLng &p,const int &zoom); + virtual QString Type() + { + return "PureProjection"; + } + core::Point FromLatLngToPixel(const PointLatLng &p, const int &zoom); - PointLatLng FromPixelToLatLng(const Point &p,const int &zoom); + PointLatLng FromPixelToLatLng(const Point &p, const int &zoom); virtual core::Point FromPixelToTileXY(const core::Point &p); virtual core::Point FromTileXYToPixel(const core::Point &p); - virtual Size GetTileMatrixMinXY(const int &zoom)=0; - virtual Size GetTileMatrixMaxXY(const int &zoom)=0; + virtual Size GetTileMatrixMinXY(const int &zoom) = 0; + virtual Size GetTileMatrixMaxXY(const int &zoom) = 0; virtual Size GetTileMatrixSizeXY(const int &zoom); int GetTileMatrixItemCount(const int &zoom); virtual Size GetTileMatrixSizePixel(const int &zoom); - QList GetAreaTileList(const RectLatLng &rect,const int &zoom,const int &padding); - virtual double GetGroundResolution(const int &zoom,const double &latitude); + QList GetAreaTileList(const RectLatLng &rect, const int &zoom, const int &padding); + virtual double GetGroundResolution(const int &zoom, const double &latitude); - double DegreesToRadians(const double °)const + double DegreesToRadians(const double °) const { - return (D2R * deg); + return D2R * deg; } - double RadiansToDegrees(const double &rad)const + double RadiansToDegrees(const double &rad) const { - return (R2D * rad); + return R2D * rad; } - void FromGeodeticToCartesian(double Lat,double Lng,double Height, double &X, double &Y, double &Z); - void FromCartesianTGeodetic(const double &X,const double &Y,const double &Z, double &Lat, double &Lng); - static double DistanceBetweenLatLng(PointLatLng const& p1,PointLatLng const& p2); + void FromGeodeticToCartesian(double Lat, double Lng, double Height, double &X, double &Y, double &Z); + void FromCartesianTGeodetic(const double &X, const double &Y, const double &Z, double &Lat, double &Lng); + static double DistanceBetweenLatLng(PointLatLng const & p1, PointLatLng const & p2); PointLatLng translate(PointLatLng p1, double distance, double bearing); double courseBetweenLatLng(const PointLatLng &p1, const PointLatLng &p2); void offSetFromLatLngs(PointLatLng p1, PointLatLng p2, double &dX, double &dY); protected: - + static const double PI; static const double HALF_PI; static const double TWO_PI; @@ -99,12 +97,12 @@ protected: static double Sign(const double &x); static double AdjustLongitude(double x); - static void SinCos(const double &val, double &sin, double &cos); + static void SinCos(const double &val, double &sin, double &cos); static double e0fn(const double &x); static double e1fn(const double &x); static double e2fn(const double &x); static double e3fn(const double &x); - static double mlfn(const double &e0,const double &e1,const double &e2,const double &e3,const double &phi); + static double mlfn(const double &e0, const double &e1, const double &e2, const double &e3, const double &phi); static qlonglong GetUTMzone(const double &lon); private: double myfmod(double x, double y); diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.cpp index 461a914bb..a9f94a809 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.cpp @@ -1,79 +1,78 @@ /** -****************************************************************************** -* -* @file rectangle.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file rectangle.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "rectangle.h" namespace internals { -Rectangle Rectangle::Empty=Rectangle(); +Rectangle Rectangle::Empty = Rectangle(); Rectangle Rectangle::FromLTRB(int left, int top, int right, int bottom) - { - return Rectangle(left, - top, - right - left, - bottom - top); - } +{ + return Rectangle(left, + top, + right - left, + bottom - top); +} Rectangle Rectangle::Inflate(Rectangle rect, int x, int y) - { - Rectangle r = rect; - r.Inflate(x, y); - return r; - } +{ + Rectangle r = rect; + + r.Inflate(x, y); + return r; +} Rectangle Rectangle::Intersect(Rectangle a, Rectangle b) - { - int x1 = std::max(a.X(), b.X()); - int x2 = std::min(a.X() + a.Width(), b.X() + b.Width()); - int y1 = std::max(a.Y(), b.Y()); - int y2 = std::min(a.Y() + a.Height(), b.Y() + b.Height()); +{ + int x1 = std::max(a.X(), b.X()); + int x2 = std::min(a.X() + a.Width(), b.X() + b.Width()); + int y1 = std::max(a.Y(), b.Y()); + int y2 = std::min(a.Y() + a.Height(), b.Y() + b.Height()); - if(x2 >= x1 - && y2 >= y1) - { - - return Rectangle(x1, y1, x2 - x1, y2 - y1); - } - return Rectangle::Empty; - } -Rectangle Rectangle::Union(const Rectangle &a,const Rectangle &b) - { + if (x2 >= x1 + && y2 >= y1) { + return Rectangle(x1, y1, x2 - x1, y2 - y1); + } + return Rectangle::Empty; +} +Rectangle Rectangle::Union(const Rectangle &a, const Rectangle &b) +{ int x1 = std::min(a.x, b.x); int x2 = std::max(a.x + a.width, b.x + b.width); int y1 = std::min(a.y, b.y); int y2 = std::max(a.y + a.height, b.y + b.height); - return Rectangle(x1, y1, x2 - x1, y2 - y1); - } -bool operator==(Rectangle const& lhs,Rectangle const& rhs) + return Rectangle(x1, y1, x2 - x1, y2 - y1); +} +bool operator==(Rectangle const & lhs, Rectangle const & rhs) { - return (lhs.x == rhs.x && lhs.y == rhs.y && lhs.width == rhs.width && lhs.height == rhs.height); + return lhs.x == rhs.x && lhs.y == rhs.y && lhs.width == rhs.width && lhs.height == rhs.height; +} +uint qHash(Rectangle const & rect) +{ + return (int)((quint32)rect.x ^ + (((quint32)rect.y << 13) | ((quint32)rect.y >> 19)) ^ + (((quint32)rect.width << 26) | ((quint32)rect.width >> 6)) ^ + (((quint32)rect.height << 7) | ((quint32)rect.height >> 25))); } -uint qHash(Rectangle const& rect) - { - return (int) ((quint32) rect.x ^ - (((quint32) rect.y << 13) | ((quint32) rect.y >> 19)) ^ - (((quint32) rect.width << 26) | ((quint32) rect.width >> 6)) ^ - (((quint32) rect.height << 7) | ((quint32) rect.height >> 25))); - } } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.h index bc70ea28c..67d0f636b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectangle.h @@ -1,61 +1,62 @@ /** -****************************************************************************** -* -* @file rectangle.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file rectangle.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef RECTANGLE_H #define RECTANGLE_H -//#include +// #include #include "../core/size.h" #include "math.h" using namespace core; -namespace internals -{ -struct Rectangle -{ - - friend uint qHash(Rectangle const& rect); - friend bool operator==(Rectangle const& lhs,Rectangle const& rhs); +namespace internals { +struct Rectangle { + friend uint qHash(Rectangle const & rect); + friend bool operator==(Rectangle const & lhs, Rectangle const & rhs); public: static Rectangle Empty; static Rectangle FromLTRB(int left, int top, int right, int bottom); - Rectangle(){x=0; y=0; width=0; height=0; }; + Rectangle() + { + x = 0; y = 0; width = 0; height = 0; + }; Rectangle(int x, int y, int width, int height) { - this->x = x; - this->y = y; - this->width = width; + this->x = x; + this->y = y; + this->width = width; this->height = height; } Rectangle(core::Point location, core::Size size) { - this->x = location.X(); - this->y = location.Y(); - this->width = size.Width(); + this->x = location.X(); + this->y = location.Y(); + this->width = size.Width(); this->height = size.Height(); } - core::Point GetLocation() { + core::Point GetLocation() + { return core::Point(x, y); } void SetLocation(const core::Point &value) @@ -63,91 +64,132 @@ public: x = value.X(); y = value.Y(); } - int X(){return x;} - int Y(){return y;} - void SetX(const int &value){x=value;} - void SetY(const int &value){y=value;} - int Width(){return width;} - void SetWidth(const int &value){width=value;} - int Height(){return height;} - void SetHeight(const int &value){height=value;} - int Left(){return x;} - int Top(){return y;} - int Right(){return x+width;} - int Bottom(){return y+height;} - bool IsEmpty(){return (height==0 && width==0 && x==0 && y==0);} + int X() + { + return x; + } + int Y() + { + return y; + } + void SetX(const int &value) + { + x = value; + } + void SetY(const int &value) + { + y = value; + } + int Width() + { + return width; + } + void SetWidth(const int &value) + { + width = value; + } + int Height() + { + return height; + } + void SetHeight(const int &value) + { + height = value; + } + int Left() + { + return x; + } + int Top() + { + return y; + } + int Right() + { + return x + width; + } + int Bottom() + { + return y + height; + } + bool IsEmpty() + { + return height == 0 && width == 0 && x == 0 && y == 0; + } bool operator==(const Rectangle &cSource) { - return (cSource.x == x && cSource.y == y && cSource.width == width && cSource.height == height); + return cSource.x == x && cSource.y == y && cSource.width == width && cSource.height == height; } - bool operator!=(const Rectangle &cSource){return !(*this==cSource);} - bool Contains(const int &x,const int &y) + bool operator!=(const Rectangle &cSource) { - return this->x<=x && xx+this->width && this->y<=y && yy+this->height; + return !(*this == cSource); + } + bool Contains(const int &x, const int &y) + { + return this->x <= x && x < this->x + this->width && this->y <= y && y < this->y + this->height; } bool Contains(const core::Point &pt) { - return Contains(pt.X(),pt.Y()); + return Contains(pt.X(), pt.Y()); } bool Contains(const Rectangle &rect) { return (this->x <= rect.x) && - ((rect.x + rect.width) <= (this->x + this->width)) && - (this->y <= rect.y) && - ((rect.y + rect.height) <= (this->y + this->height)); + ((rect.x + rect.width) <= (this->x + this->width)) && + (this->y <= rect.y) && + ((rect.y + rect.height) <= (this->y + this->height)); } - void Inflate(const int &width,const int &height) - { - this->x -= width; - this->y -= height; - this->width += 2*width; - this->height += 2*height; - } + void Inflate(const int &width, const int &height) + { + this->x -= width; + this->y -= height; + this->width += 2 * width; + this->height += 2 * height; + } void Inflate(Size &size) - { - - Inflate(size.Width(), size.Height()); - } + { + Inflate(size.Width(), size.Height()); + } static Rectangle Inflate(Rectangle rect, int x, int y); void Intersect(const Rectangle &rect) - { + { Rectangle result = Rectangle::Intersect(rect, *this); - this->x = result.X(); - this->y = result.Y(); - this->width = result.Width(); - this->height = result.Height(); - } + this->x = result.X(); + this->y = result.Y(); + this->width = result.Width(); + this->height = result.Height(); + } static Rectangle Intersect(Rectangle a, Rectangle b); bool IntersectsWith(const Rectangle &rect) - { - return (rect.x < this->x + this->width) && + { + return (rect.x < this->x + this->width) && (this->x < (rect.x + rect.width)) && (rect.y < this->y + this->height) && (this->y < rect.y + rect.height); - } - static Rectangle Union(const Rectangle &a,const Rectangle &b); + } + static Rectangle Union(const Rectangle &a, const Rectangle &b); void Offset(const core::Point &pos) { Offset(pos.X(), pos.Y()); } - void Offset(const int &x,const int &y) + void Offset(const int &x, const int &y) { this->x += x; this->y += y; } QString ToString() - { + { return "{X=" + QString::number(x) + ",Y=" + QString::number(y) + - ",Width=" + QString::number(width) + - ",Height=" +QString::number(height) +"}"; - } + ",Width=" + QString::number(width) + + ",Height=" + QString::number(height) + "}"; + } private: int x; int y; diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.cpp index 1d8529623..30221f048 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.cpp @@ -1,48 +1,46 @@ /** -****************************************************************************** -* -* @file rectlatlng.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file rectlatlng.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "rectlatlng.h" - namespace internals { -RectLatLng RectLatLng::Empty=RectLatLng(); -uint qHash(RectLatLng const& rect) +RectLatLng RectLatLng::Empty = RectLatLng(); +uint qHash(RectLatLng const & rect) { - return (int) (((((uint) rect.Lng()) ^ ((((uint) rect.Lat()) << 13) | (((uint) rect.Lat()) >> 0x13))) ^ ((((uint) rect.WidthLng()) << 0x1a) | (((uint) rect.WidthLng()) >> 6))) ^ ((((uint) rect.HeightLat()) << 7) | (((uint) rect.HeightLat()) >> 0x19))); + return (int)(((((uint)rect.Lng()) ^ ((((uint)rect.Lat()) << 13) | (((uint)rect.Lat()) >> 0x13))) ^ ((((uint)rect.WidthLng()) << 0x1a) | (((uint)rect.WidthLng()) >> 6))) ^ ((((uint)rect.HeightLat()) << 7) | (((uint)rect.HeightLat()) >> 0x19))); } -bool operator==(RectLatLng const& left,RectLatLng const& right) +bool operator==(RectLatLng const & left, RectLatLng const & right) { - return ((((left.Lng() == right.Lng()) && (left.Lat() == right.Lat())) && (left.WidthLng() == right.WidthLng())) && (left.HeightLat() == right.HeightLat())); + return (((left.Lng() == right.Lng()) && (left.Lat() == right.Lat())) && (left.WidthLng() == right.WidthLng())) && (left.HeightLat() == right.HeightLat()); } -bool operator!=(RectLatLng const& left,RectLatLng const& right) +bool operator!=(RectLatLng const & left, RectLatLng const & right) { return !(left == right); } - } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.h index 1e3f56fa2..730530f5e 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/rectlatlng.h @@ -1,61 +1,60 @@ /** -****************************************************************************** -* -* @file rectlatlng.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file rectlatlng.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef RECTLATLNG_H #define RECTLATLNG_H -//#include "pointlatlng.h" +// #include "pointlatlng.h" #include "../internals/pointlatlng.h" #include "math.h" #include #include "sizelatlng.h" - + namespace internals { -struct RectLatLng -{ +struct RectLatLng { public: static RectLatLng Empty; - friend uint qHash(RectLatLng const& rect); - friend bool operator==(RectLatLng const& left,RectLatLng const& right); - friend bool operator!=(RectLatLng const& left,RectLatLng const& right); - RectLatLng(double const& lat, double const& lng, double const& widthLng, double const& heightLat) + friend uint qHash(RectLatLng const & rect); + friend bool operator==(RectLatLng const & left, RectLatLng const & right); + friend bool operator!=(RectLatLng const & left, RectLatLng const & right); + RectLatLng(double const & lat, double const & lng, double const & widthLng, double const & heightLat) { this->lng = lng; this->lat = lat; this->widthLng = widthLng; this->heightLat = heightLat; - isempty=false; + isempty = false; } - RectLatLng(PointLatLng const& location, SizeLatLng const& size) + RectLatLng(PointLatLng const & location, SizeLatLng const & size) { this->lng = location.Lng(); this->lat = location.Lat(); this->widthLng = size.WidthLng(); this->heightLat = size.HeightLat(); - isempty=false; + isempty = false; } RectLatLng() { @@ -63,27 +62,27 @@ public: this->lat = 0; this->widthLng = 0; this->heightLat = 0; - isempty=true; + isempty = true; } - static RectLatLng FromLTRB(double const& lng, double const& lat, double const& rightLng, double const& bottomLat) + static RectLatLng FromLTRB(double const & lng, double const & lat, double const & rightLng, double const & bottomLat) { return RectLatLng(lat, lng, rightLng - lng, lat - bottomLat); } - PointLatLng LocationTopLeft()const + PointLatLng LocationTopLeft() const { - return PointLatLng(this->lat, this->lng); + return PointLatLng(this->lat, this->lng); } - void SetLocationTopLeft(PointLatLng const& value) + void SetLocationTopLeft(PointLatLng const & value) { this->lng = value.Lng(); this->lat = value.Lat(); - isempty=false; + isempty = false; } PointLatLng LocationRightBottom() { + PointLatLng ret = PointLatLng(this->lat, this->lng); - PointLatLng ret = PointLatLng(this->lat, this->lng); ret.Offset(HeightLat(), WidthLng()); return ret; } @@ -91,174 +90,174 @@ public: { return SizeLatLng(this->HeightLat(), this->WidthLng()); } - void SetSize(SizeLatLng const& value) + void SetSize(SizeLatLng const & value) { - this->widthLng = value.WidthLng(); + this->widthLng = value.WidthLng(); this->heightLat = value.HeightLat(); - isempty=false; + isempty = false; } - double Lng()const + double Lng() const { return this->lng; } - void SetLng(double const& value) + void SetLng(double const & value) { this->lng = value; - isempty=false; + isempty = false; } - double Lat()const + double Lat() const { return this->lat; } - void SetLat(double const& value) + void SetLat(double const & value) { this->lat = value; - isempty=false; + isempty = false; } - double WidthLng()const + double WidthLng() const { return this->widthLng; } - void SetWidthLng(double const& value) + void SetWidthLng(double const & value) { this->widthLng = value; - isempty=false; + isempty = false; } - double HeightLat()const + double HeightLat() const { return this->heightLat; } - void SetHeightLat(double const& value) + void SetHeightLat(double const & value) { this->heightLat = value; - isempty=false; + isempty = false; } - double Left()const + double Left() const { return this->Lng(); } - double Top()const + double Top() const { return this->Lat(); } - double Right()const + double Right() const { - return (this->Lng() + this->WidthLng()); + return this->Lng() + this->WidthLng(); } - double Bottom()const + double Bottom() const { - return (this->Lat() - this->HeightLat()); + return this->Lat() - this->HeightLat(); } - bool IsEmpty()const - { + bool IsEmpty() const + { return isempty; } - bool Contains(double const& lat, double const& lng) + bool Contains(double const & lat, double const & lng) { - return ((((this->Lng() <= lng) && (lng < (this->Lng() + this->WidthLng()))) && (this->Lat() >= lat)) && (lat > (this->Lat() - this->HeightLat()))); + return (((this->Lng() <= lng) && (lng < (this->Lng() + this->WidthLng()))) && (this->Lat() >= lat)) && (lat > (this->Lat() - this->HeightLat())); } - bool Contains(PointLatLng const& pt) + bool Contains(PointLatLng const & pt) { return this->Contains(pt.Lat(), pt.Lng()); } - bool Contains(RectLatLng const& rect) + bool Contains(RectLatLng const & rect) { - return ((((this->Lng() <= rect.Lng()) && ((rect.Lng() + rect.WidthLng()) <= (this->Lng() + this->WidthLng()))) && (this->Lat() >= rect.Lat())) && ((rect.Lat() - rect.HeightLat()) >= (this->Lat() - this->HeightLat()))); + return (((this->Lng() <= rect.Lng()) && ((rect.Lng() + rect.WidthLng()) <= (this->Lng() + this->WidthLng()))) && (this->Lat() >= rect.Lat())) && ((rect.Lat() - rect.HeightLat()) >= (this->Lat() - this->HeightLat())); } - void Inflate(double const& lat, double const& lng) + void Inflate(double const & lat, double const & lng) { this->lng -= lng; this->lat += lat; this->widthLng += (double)2 * lng; - this->heightLat +=(double)2 * lat; + this->heightLat += (double)2 * lat; } - void Inflate(SizeLatLng const& size) + void Inflate(SizeLatLng const & size) { this->Inflate(size.HeightLat(), size.WidthLng()); } - static RectLatLng Inflate(RectLatLng const& rect, double const& lat, double const& lng) + static RectLatLng Inflate(RectLatLng const & rect, double const & lat, double const & lng) { RectLatLng ef = rect; + ef.Inflate(lat, lng); return ef; } - void Intersect(RectLatLng const& rect) + void Intersect(RectLatLng const & rect) { RectLatLng ef = Intersect(rect, *this); + this->lng = ef.Lng(); this->lat = ef.Lat(); this->widthLng = ef.WidthLng(); this->heightLat = ef.HeightLat(); } - static RectLatLng Intersect(RectLatLng const& a, RectLatLng const& b) + static RectLatLng Intersect(RectLatLng const & a, RectLatLng const & b) { - double lng = std::max(a.Lng(), b.Lng()); - double num2 = std::min((double) (a.Lng() + a.WidthLng()), (double) (b.Lng() + b.WidthLng())); + double lng = std::max(a.Lng(), b.Lng()); + double num2 = std::min((double)(a.Lng() + a.WidthLng()), (double)(b.Lng() + b.WidthLng())); - double lat = std::max(a.Lat(), b.Lat()); - double num4 = std::min((double) (a.Lat() + a.HeightLat()), (double) (b.Lat() + b.HeightLat())); + double lat = std::max(a.Lat(), b.Lat()); + double num4 = std::min((double)(a.Lat() + a.HeightLat()), (double)(b.Lat() + b.HeightLat())); - if((num2 >= lng) && (num4 >= lat)) - { + if ((num2 >= lng) && (num4 >= lat)) { return RectLatLng(lng, lat, num2 - lng, num4 - lat); } return Empty; } - bool IntersectsWith(RectLatLng const& rect) - { - return ((((rect.Lng() < (this->Lng() + this->WidthLng())) && (this->Lng() < (rect.Lng() + rect.WidthLng()))) && (rect.Lat() < (this->Lat() + this->HeightLat()))) && (this->Lat() < (rect.Lat() + rect.HeightLat()))); - } + bool IntersectsWith(RectLatLng const & rect) + { + return (((rect.Lng() < (this->Lng() + this->WidthLng())) && (this->Lng() < (rect.Lng() + rect.WidthLng()))) && (rect.Lat() < (this->Lat() + this->HeightLat()))) && (this->Lat() < (rect.Lat() + rect.HeightLat())); + } - static RectLatLng Union(RectLatLng const& a, RectLatLng const& b) - { - double lng = std::min(a.Lng(), b.Lng()); - double num2 = std::max((double) (a.Lng() + a.WidthLng()), (double) (b.Lng() + b.WidthLng())); - double lat = std::min(a.Lat(), b.Lat()); - double num4 = std::max((double) (a.Lat() + a.HeightLat()), (double) (b.Lat() + b.HeightLat())); - return RectLatLng(lng, lat, num2 - lng, num4 - lat); - } - void Offset(PointLatLng const& pos) - { - this->Offset(pos.Lat(), pos.Lng()); - } + static RectLatLng Union(RectLatLng const & a, RectLatLng const & b) + { + double lng = std::min(a.Lng(), b.Lng()); + double num2 = std::max((double)(a.Lng() + a.WidthLng()), (double)(b.Lng() + b.WidthLng())); + double lat = std::min(a.Lat(), b.Lat()); + double num4 = std::max((double)(a.Lat() + a.HeightLat()), (double)(b.Lat() + b.HeightLat())); - void Offset(double const& lat, double const& lng) - { - this->lng += lng; - this->lat -= lat; - } + return RectLatLng(lng, lat, num2 - lng, num4 - lat); + } + void Offset(PointLatLng const & pos) + { + this->Offset(pos.Lat(), pos.Lng()); + } - QString ToString() const - { - return ("{Lat=" + QString::number(this->Lat()) + ",Lng=" + QString::number(this->Lng()) + ",WidthLng=" + QString::number(this->WidthLng()) + ",HeightLat=" + QString::number(this->HeightLat()) + "}"); - } + void Offset(double const & lat, double const & lng) + { + this->lng += lng; + this->lat -= lat; + } + + QString ToString() const + { + return "{Lat=" + QString::number(this->Lat()) + ",Lng=" + QString::number(this->Lng()) + ",WidthLng=" + QString::number(this->WidthLng()) + ",HeightLat=" + QString::number(this->HeightLat()) + "}"; + } private: double lng; double lat; double widthLng; double heightLat; - bool isempty; + bool isempty; }; - } #endif // RECTLATLNG_H - -// static RectLatLng() -// { -// Empty = new RectLatLng(); -// } -// } +// static RectLatLng() +// { +// Empty = new RectLatLng(); +// } +// } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.cpp index f66b7a104..d1163ac6d 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.cpp @@ -1,60 +1,58 @@ /** -****************************************************************************** -* -* @file sizelatlng.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file sizelatlng.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "sizelatlng.h" #include "pointlatlng.h" - -namespace internals { -SizeLatLng::SizeLatLng():heightLat(0),widthLng(0) -{ -} -SizeLatLng::SizeLatLng(PointLatLng const& pt) +namespace internals { +SizeLatLng::SizeLatLng() : heightLat(0), widthLng(0) +{} +SizeLatLng::SizeLatLng(PointLatLng const & pt) { - this->heightLat = pt.Lat(); - this->widthLng = pt.Lng(); + this->heightLat = pt.Lat(); + this->widthLng = pt.Lng(); } -SizeLatLng operator+(SizeLatLng const& sz1, SizeLatLng const& sz2) +SizeLatLng operator+(SizeLatLng const & sz1, SizeLatLng const & sz2) { return SizeLatLng::Add(sz1, sz2); } -SizeLatLng operator-(SizeLatLng const& sz1, SizeLatLng const& sz2) +SizeLatLng operator-(SizeLatLng const & sz1, SizeLatLng const & sz2) { - return SizeLatLng::Subtract(sz1, sz2); + return SizeLatLng::Subtract(sz1, sz2); } -bool operator==(SizeLatLng const& sz1, SizeLatLng const& sz2) +bool operator==(SizeLatLng const & sz1, SizeLatLng const & sz2) { - return ((sz1.WidthLng() == sz2.WidthLng()) && (sz1.HeightLat() == sz2.HeightLat())); + return (sz1.WidthLng() == sz2.WidthLng()) && (sz1.HeightLat() == sz2.HeightLat()); } -bool operator!=(SizeLatLng const& sz1, SizeLatLng const& sz2) +bool operator!=(SizeLatLng const & sz1, SizeLatLng const & sz2) { - return !(sz1 == sz2); + return !(sz1 == sz2); } -SizeLatLng SizeLatLng::Empty=SizeLatLng(); +SizeLatLng SizeLatLng::Empty = SizeLatLng(); } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.h index 7a4463567..e7897cfab 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/sizelatlng.h @@ -1,128 +1,127 @@ /** -****************************************************************************** -* -* @file sizelatlng.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file sizelatlng.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef SIZELATLNG_H #define SIZELATLNG_H #include - + namespace internals { struct PointLatLng; -struct SizeLatLng -{ +struct SizeLatLng { public: SizeLatLng(); - static SizeLatLng Empty; + static SizeLatLng Empty; - SizeLatLng(SizeLatLng const& size) + SizeLatLng(SizeLatLng const & size) { - this->widthLng = size.widthLng; - this->heightLat = size.heightLat; + this->widthLng = size.widthLng; + this->heightLat = size.heightLat; } - SizeLatLng(PointLatLng const& pt); + SizeLatLng(PointLatLng const & pt); - SizeLatLng(double const& heightLat, double const& widthLng) + SizeLatLng(double const & heightLat, double const & widthLng) { - this->heightLat = heightLat; - this->widthLng = widthLng; + this->heightLat = heightLat; + this->widthLng = widthLng; } - friend SizeLatLng operator+(SizeLatLng const& sz1, SizeLatLng const& sz2); - friend SizeLatLng operator-(SizeLatLng const& sz1, SizeLatLng const& sz2); - friend bool operator==(SizeLatLng const& sz1, SizeLatLng const& sz2); - friend bool operator!=(SizeLatLng const& sz1, SizeLatLng const& sz2); + friend SizeLatLng operator+(SizeLatLng const & sz1, SizeLatLng const & sz2); + friend SizeLatLng operator-(SizeLatLng const & sz1, SizeLatLng const & sz2); + friend bool operator==(SizeLatLng const & sz1, SizeLatLng const & sz2); + friend bool operator!=(SizeLatLng const & sz1, SizeLatLng const & sz2); -// static explicit operator PointLatLng(SizeLatLng size) -// { -// return new PointLatLng(size.HeightLat(), size.WidthLng()); -// } +// static explicit operator PointLatLng(SizeLatLng size) +// { +// return new PointLatLng(size.HeightLat(), size.WidthLng()); +// } - bool IsEmpty()const - { - return ((this->widthLng == 0) && (this->heightLat == 0)); - } - - double WidthLng()const - { - return this->widthLng; - } - void SetWidthLng(double const& value) - { - this->widthLng = value; - } - - - double HeightLat()const - { - return this->heightLat; - } - void SetHeightLat(double const& value) - { - this->heightLat = value; - } - - static SizeLatLng Add(SizeLatLng const& sz1, SizeLatLng const& sz2) + bool IsEmpty() const { - return SizeLatLng(sz1.HeightLat() + sz2.HeightLat(), sz1.WidthLng() + sz2.WidthLng()); + return (this->widthLng == 0) && (this->heightLat == 0); } - static SizeLatLng Subtract(SizeLatLng const& sz1, SizeLatLng const& sz2) + double WidthLng() const { - return SizeLatLng(sz1.HeightLat() - sz2.HeightLat(), sz1.WidthLng() - sz2.WidthLng()); + return this->widthLng; + } + void SetWidthLng(double const & value) + { + this->widthLng = value; } -// override bool Equals(object obj) -// { -// if(!(obj is SizeLatLng)) -// { -// return false; -// } -// SizeLatLng ef = (SizeLatLng) obj; -// return (((ef.WidthLng == this->WidthLng) && (ef.HeightLat == this->HeightLat)) && ef.GetType().Equals(base.GetType())); -// } -// override int GetHashCode() -// { -// return base.GetHashCode(); -// } - -// PointLatLng ToPointLatLng() -// { -// return (PointLatLng) this; -// } - - QString ToString() + double HeightLat() const { - return ("{WidthLng=" + QString::number(this->widthLng) + ", HeightLng=" + QString::number(this->heightLat) + "}"); + return this->heightLat; + } + void SetHeightLat(double const & value) + { + this->heightLat = value; + } + + static SizeLatLng Add(SizeLatLng const & sz1, SizeLatLng const & sz2) + { + return SizeLatLng(sz1.HeightLat() + sz2.HeightLat(), sz1.WidthLng() + sz2.WidthLng()); + } + + static SizeLatLng Subtract(SizeLatLng const & sz1, SizeLatLng const & sz2) + { + return SizeLatLng(sz1.HeightLat() - sz2.HeightLat(), sz1.WidthLng() - sz2.WidthLng()); + } + +// override bool Equals(object obj) +// { +// if(!(obj is SizeLatLng)) +// { +// return false; +// } +// SizeLatLng ef = (SizeLatLng) obj; +// return (((ef.WidthLng == this->WidthLng) && (ef.HeightLat == this->HeightLat)) && ef.GetType().Equals(base.GetType())); +// } + +// override int GetHashCode() +// { +// return base.GetHashCode(); +// } + +// PointLatLng ToPointLatLng() +// { +// return (PointLatLng) this; +// } + + QString ToString() + { + return "{WidthLng=" + QString::number(this->widthLng) + ", HeightLng=" + QString::number(this->heightLat) + "}"; } @@ -130,7 +129,5 @@ private: double heightLat; double widthLng; }; - } #endif // SIZELATLNG_H - diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.cpp index e63ccc4be..c07ca63b3 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.cpp @@ -1,60 +1,56 @@ /** -****************************************************************************** -* -* @file tile.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file tile.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "tile.h" - + namespace internals { Tile::Tile(int zoom, Point pos) { - this->zoom=zoom; - this->pos=pos; + this->zoom = zoom; + this->pos = pos; } void Tile::Clear() { #ifdef DEBUG_TILE - qDebug()<<"Tile:Clear Overlays"; -#endif //DEBUG_TILE + qDebug() << "Tile:Clear Overlays"; +#endif // DEBUG_TILE mutex.lock(); - foreach(QByteArray img, Overlays) - { + foreach(QByteArray img, Overlays) { img.~QByteArray(); } Overlays.clear(); mutex.unlock(); } -Tile::Tile():zoom(0),pos(0,0) +Tile::Tile() : zoom(0), pos(0, 0) +{} +Tile & Tile::operator =(const Tile &cSource) { - -} -Tile& Tile::operator =(const Tile &cSource) -{ - this->zoom=cSource.zoom; - this->pos=cSource.pos; + this->zoom = cSource.zoom; + this->pos = cSource.pos; return *this; } - } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.h index 0f67ad205..84872ccb8 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tile.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file tile.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file tile.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef TILE_H #define TILE_H @@ -34,25 +34,38 @@ #include #include "debugheader.h" using namespace core; -namespace internals -{ -class Tile -{ +namespace internals { +class Tile { public: - Tile(int zoom,core::Point pos); + Tile(int zoom, core::Point pos); Tile(); void Clear(); - int GetZoom(){return zoom;} - core::Point GetPos(){return pos;} - void SetZoom(const int &value){zoom=value;} - void SetPos(const core::Point &value){pos=value;} - Tile& operator= (const Tile &cSource); + int GetZoom() + { + return zoom; + } + core::Point GetPos() + { + return pos; + } + void SetZoom(const int &value) + { + zoom = value; + } + void SetPos(const core::Point &value) + { + pos = value; + } + Tile & operator=(const Tile &cSource); Tile(const Tile &cSource) { - this->zoom=cSource.zoom; - this->pos=cSource.pos; + this->zoom = cSource.zoom; + this->pos = cSource.pos; + } + bool HasValue() + { + return !(zoom == 0); } - bool HasValue(){return !(zoom==0);} QList Overlays; protected: @@ -60,8 +73,6 @@ protected: private: int zoom; core::Point pos; - - }; } #endif // TILE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tilematrix.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tilematrix.cpp index b9ad2b003..8a637d75f 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tilematrix.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/tilematrix.cpp @@ -1,148 +1,142 @@ /** -****************************************************************************** -* -* @file tilematrix.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + ****************************************************************************** + * + * @file tilematrix.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "tilematrix.h" - + namespace internals { TileMatrix::TileMatrix() -{ -} +{} void TileMatrix::Clear() { mutex.lock(); - foreach(Tile* t,matrix.values()) - { + foreach(Tile * t, matrix.values()) { delete t; - t=0; + t = 0; } matrix.clear(); mutex.unlock(); } -//void TileMatrix::RebuildToUpperZoom() -//{ -// mutex.lock(); -// QList newtiles; -// foreach(Tile* t,matrix.values()) -// { -// Point point=Point(t->GetPos().X()*2,t->GetPos().Y()*2); -// Tile* tile1=new Tile(t->GetZoom()+1,point); -// Tile* tile2=new Tile(t->GetZoom()+1,Point(point.X()+1,point.Y()+0)); -// Tile* tile3=new Tile(t->GetZoom()+1,Point(point.X()+0,point.Y()+1)); -// Tile* tile4=new Tile(t->GetZoom()+1,Point(point.X()+1,point.Y()+1)); +// void TileMatrix::RebuildToUpperZoom() +// { +// mutex.lock(); +// QList newtiles; +// foreach(Tile* t,matrix.values()) +// { +// Point point=Point(t->GetPos().X()*2,t->GetPos().Y()*2); +// Tile* tile1=new Tile(t->GetZoom()+1,point); +// Tile* tile2=new Tile(t->GetZoom()+1,Point(point.X()+1,point.Y()+0)); +// Tile* tile3=new Tile(t->GetZoom()+1,Point(point.X()+0,point.Y()+1)); +// Tile* tile4=new Tile(t->GetZoom()+1,Point(point.X()+1,point.Y()+1)); // -// foreach(QByteArray arr, t->Overlays) -// { -// QImage ima=QImage::fromData(arr); -// QImage ima1=ima.copy(0,0,ima.width()/2,ima.height()/2); -// QImage ima2=ima.copy(ima.width()/2,0,ima.width()/2,ima.height()/2); -// QImage ima3=ima.copy(0,ima.height()/2,ima.width()/2,ima.height()/2); -// QImage ima4=ima.copy(ima.width()/2,ima.height()/2,ima.width()/2,ima.height()/2); -// QByteArray ba; -// QBuffer buf(&ba); -// ima1.scaled(QSize(ima.width(),ima.height())).save(&buf,"PNG"); -// tile1->Overlays.append(ba); -// QByteArray ba1; -// QBuffer buf1(&ba1); -// ima2.scaled(QSize(ima.width(),ima.height())).save(&buf1,"PNG"); -// tile2->Overlays.append(ba1); -// QByteArray ba2; -// QBuffer buf2(&ba2); -// ima3.scaled(QSize(ima.width(),ima.height())).save(&buf2,"PNG"); -// tile3->Overlays.append(ba2); -// QByteArray ba3; -// QBuffer buf3(&ba3); -// ima4.scaled(QSize(ima.width(),ima.height())).save(&buf3,"PNG"); -// tile4->Overlays.append(ba3); -// newtiles.append(tile1); -// newtiles.append(tile2); -// newtiles.append(tile3); -// newtiles.append(tile4); -// } -// } -// foreach(Tile* t,matrix.values()) -// { -// delete t; -// t=0; -// } -// matrix.clear(); -// foreach(Tile* t,newtiles) -// { -// matrix.insert(t->GetPos(),t); -// } +// foreach(QByteArray arr, t->Overlays) +// { +// QImage ima=QImage::fromData(arr); +// QImage ima1=ima.copy(0,0,ima.width()/2,ima.height()/2); +// QImage ima2=ima.copy(ima.width()/2,0,ima.width()/2,ima.height()/2); +// QImage ima3=ima.copy(0,ima.height()/2,ima.width()/2,ima.height()/2); +// QImage ima4=ima.copy(ima.width()/2,ima.height()/2,ima.width()/2,ima.height()/2); +// QByteArray ba; +// QBuffer buf(&ba); +// ima1.scaled(QSize(ima.width(),ima.height())).save(&buf,"PNG"); +// tile1->Overlays.append(ba); +// QByteArray ba1; +// QBuffer buf1(&ba1); +// ima2.scaled(QSize(ima.width(),ima.height())).save(&buf1,"PNG"); +// tile2->Overlays.append(ba1); +// QByteArray ba2; +// QBuffer buf2(&ba2); +// ima3.scaled(QSize(ima.width(),ima.height())).save(&buf2,"PNG"); +// tile3->Overlays.append(ba2); +// QByteArray ba3; +// QBuffer buf3(&ba3); +// ima4.scaled(QSize(ima.width(),ima.height())).save(&buf3,"PNG"); +// tile4->Overlays.append(ba3); +// newtiles.append(tile1); +// newtiles.append(tile2); +// newtiles.append(tile3); +// newtiles.append(tile4); +// } +// } +// foreach(Tile* t,matrix.values()) +// { +// delete t; +// t=0; +// } +// matrix.clear(); +// foreach(Tile* t,newtiles) +// { +// matrix.insert(t->GetPos(),t); +// } // -// mutex.unlock(); -//} +// mutex.unlock(); +// } void TileMatrix::ClearPointsNotIn(QListlist) { removals.clear(); mutex.lock(); - foreach(Point p, matrix.keys()) - { - if(!list.contains(p)) - { + foreach(Point p, matrix.keys()) { + if (!list.contains(p)) { removals.append(p); } } mutex.unlock(); - foreach(Point p,removals) - { - Tile* t=TileAt(p); - if(t!=0) - { + foreach(Point p, removals) { + Tile *t = TileAt(p); + + if (t != 0) { mutex.lock(); delete t; - t=0; + t = 0; matrix.remove(p); mutex.unlock(); } - } removals.clear(); } -Tile* TileMatrix::TileAt(const Point &p) +Tile *TileMatrix::TileAt(const Point &p) { - #ifdef DEBUG_TILEMATRIX - qDebug()<<"TileMatrix:TileAt:"< namespace internals { -class TileMatrix -{ +class TileMatrix { public: TileMatrix(); void Clear(); void ClearPointsNotIn(QList list); - Tile* TileAt(const core::Point &p); - void SetTileAt(const core::Point &p,Tile* tile); - int count()const{return matrix.count();} - // void RebuildToUpperZoom(); + Tile *TileAt(const core::Point &p); + void SetTileAt(const core::Point &p, Tile *tile); + int count() const + { + return matrix.count(); + } + // void RebuildToUpperZoom(); protected: - QHash matrix; + QHash matrix; QList removals; QMutex mutex; }; - } #endif // TILEMATRIX_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.cpp index 38a855040..28fb01bd6 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.cpp @@ -1,43 +1,42 @@ /** -****************************************************************************** -* -* @file configuration.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A class that centralizes most of the mapcontrol configurations -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file configuration.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A class that centralizes most of the mapcontrol configurations + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "configuration.h" -namespace mapcontrol -{ +namespace mapcontrol { Configuration::Configuration() { - EmptytileBrush = Qt::cyan; - MissingDataFont =QFont ("Times",10,QFont::Bold); - EmptyTileText = "We are sorry, but we don't\nhave imagery at this zoom\nlevel for this region."; + EmptytileBrush = Qt::cyan; + MissingDataFont = QFont("Times", 10, QFont::Bold); + EmptyTileText = "We are sorry, but we don't\nhave imagery at this zoom\nlevel for this region."; EmptyTileBorders = QPen(Qt::white); ScalePen = QPen(Qt::blue); - SelectionPen = QPen(Qt::blue); + SelectionPen = QPen(Qt::blue); DragButton = Qt::LeftButton; } -void Configuration::SetAccessMode(core::AccessMode::Types const& type) +void Configuration::SetAccessMode(core::AccessMode::Types const & type) { core::OPMaps::Instance()->setAccessMode(type); } @@ -45,7 +44,7 @@ core::AccessMode::Types Configuration::AccessMode() { return core::OPMaps::Instance()->GetAccessMode(); } -void Configuration::SetLanguage(core::LanguageType::Types const& type) +void Configuration::SetLanguage(core::LanguageType::Types const & type) { core::OPMaps::Instance()->setLanguage(type); } @@ -53,7 +52,7 @@ core::LanguageType::Types Configuration::Language() { return core::OPMaps::Instance()->GetLanguage(); } -void Configuration::SetUseMemoryCache(bool const& value) +void Configuration::SetUseMemoryCache(bool const & value) { core::OPMaps::Instance()->setUseMemoryCache(value); } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.h index 6c3987654..c1edc793c 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/configuration.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file configuration.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A class that centralizes most of the mapcontrol configurations -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file configuration.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A class that centralizes most of the mapcontrol configurations + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef CONFIGURATION_H #define CONFIGURATION_H @@ -35,152 +35,164 @@ #include "../core/opmaps.h" #include "../core/accessmode.h" #include "../core/cache.h" -namespace mapcontrol -{ - +namespace mapcontrol { /** -* @brief A class that centralizes most of the mapcontrol configurations -* -* @class Configuration configuration.h "configuration.h" -*/ -class Configuration -{ + * @brief A class that centralizes most of the mapcontrol configurations + * + * @class Configuration configuration.h "configuration.h" + */ +class Configuration { public: Configuration(); /** - * @brief Used to draw empty map tiles - * - * @var EmptytileBrush - */ + * @brief Used to draw empty map tiles + * + * @var EmptytileBrush + */ QBrush EmptytileBrush; /** - * @brief Used for empty tiles text - * - * @var EmptyTileText - */ + * @brief Used for empty tiles text + * + * @var EmptyTileText + */ QString EmptyTileText; /** - * @brief Used to draw empty tile borders - * - * @var EmptyTileBorders - */ + * @brief Used to draw empty tile borders + * + * @var EmptyTileBorders + */ QPen EmptyTileBorders; /** - * @brief Used to Draw the maps scale - * - * @var ScalePen - */ + * @brief Used to Draw the maps scale + * + * @var ScalePen + */ QPen ScalePen; /** - * @brief Used to draw selection box - * - * @var SelectionPen - */ + * @brief Used to draw selection box + * + * @var SelectionPen + */ QPen SelectionPen; /** - * @brief - * - * @var MissingDataFont - */ + * @brief + * + * @var MissingDataFont + */ QFont MissingDataFont; /** - * @brief Button used for dragging - * - * @var DragButton - */ + * @brief Button used for dragging + * + * @var DragButton + */ Qt::MouseButton DragButton; /** - * @brief Sets the access mode for the map (cache only, server and cache...) - * - * @param type access mode - */ - void SetAccessMode(core::AccessMode::Types const& type); + * @brief Sets the access mode for the map (cache only, server and cache...) + * + * @param type access mode + */ + void SetAccessMode(core::AccessMode::Types const & type); /** - * @brief Returns the access mode for the map (cache only, server and cache...) - * - * @return core::AccessMode::Types access mode for the map - */ + * @brief Returns the access mode for the map (cache only, server and cache...) + * + * @return core::AccessMode::Types access mode for the map + */ core::AccessMode::Types AccessMode(); /** - * @brief Sets the language used for geocaching - * - * @param type The language to be used - */ - void SetLanguage(core::LanguageType::Types const& type); + * @brief Sets the language used for geocaching + * + * @param type The language to be used + */ + void SetLanguage(core::LanguageType::Types const & type); /** - * @brief Returns the language used for geocaching - * - * @return core::LanguageType::Types - */ + * @brief Returns the language used for geocaching + * + * @return core::LanguageType::Types + */ core::LanguageType::Types Language(); /** - * @brief Used to allow disallow use of memory caching - * - * @param value - * @return - */ - void SetUseMemoryCache(bool const& value); + * @brief Used to allow disallow use of memory caching + * + * @param value + * @return + */ + void SetUseMemoryCache(bool const & value); /** - * @brief Return if memory caching is in use - * - * @return - */ - bool UseMemoryCache(){return core::OPMaps::Instance()->UseMemoryCache();} - - /** - * @brief Returns the currently used memory for tiles - * - * @return - */ - double TileMemoryUsed()const{return core::OPMaps::Instance()->TilesInMemory.MemoryCacheSize();} - - /** - * @brief Sets the size of the memory for tiles - * - * @param value size in Mb to use for tiles - * @return - */ - void SetTileMemorySize(int const& value){core::OPMaps::Instance()->TilesInMemory.setMemoryCacheCapacity(value);} - - /** - * @brief Sets the location for the SQLite Database used for caching and the geocoding cache files - * - * @param dir The path location for the cache file-IMPORTANT Must end with closing slash "/" - */ - void SetCacheLocation(QString const& dir) + * @brief Return if memory caching is in use + * + * @return + */ + bool UseMemoryCache() { - core::Cache::Instance()->setCacheLocation(dir); - + return core::OPMaps::Instance()->UseMemoryCache(); } /** - * @brief Deletes tiles in DataBase older than "days" days - * - * @param days - * @return - */ - void DeleteTilesOlderThan(int const& days){core::Cache::Instance()->ImageCache.deleteOlderTiles(days);} + * @brief Returns the currently used memory for tiles + * + * @return + */ + double TileMemoryUsed() const + { + return core::OPMaps::Instance()->TilesInMemory.MemoryCacheSize(); + } /** - * @brief Exports tiles from one DB to another. Only new tiles are added. - * - * @param sourceDB the source DB - * @param destDB the destination DB. If it doesnt exhist it will be created. - * @return - */ - void ExportMapDataToDB(QString const& sourceDB, QString const& destDB)const{core::PureImageCache::ExportMapDataToDB(sourceDB,destDB);} + * @brief Sets the size of the memory for tiles + * + * @param value size in Mb to use for tiles + * @return + */ + void SetTileMemorySize(int const & value) + { + core::OPMaps::Instance()->TilesInMemory.setMemoryCacheCapacity(value); + } + /** - * @brief Returns the location for the SQLite Database used for caching and the geocoding cache files - * - * @return - */ - QString CacheLocation(){return core::Cache::Instance()->CacheLocation();} + * @brief Sets the location for the SQLite Database used for caching and the geocoding cache files + * + * @param dir The path location for the cache file-IMPORTANT Must end with closing slash "/" + */ + void SetCacheLocation(QString const & dir) + { + core::Cache::Instance()->setCacheLocation(dir); + } + /** + * @brief Deletes tiles in DataBase older than "days" days + * + * @param days + * @return + */ + void DeleteTilesOlderThan(int const & days) + { + core::Cache::Instance()->ImageCache.deleteOlderTiles(days); + } + /** + * @brief Exports tiles from one DB to another. Only new tiles are added. + * + * @param sourceDB the source DB + * @param destDB the destination DB. If it doesnt exhist it will be created. + * @return + */ + void ExportMapDataToDB(QString const & sourceDB, QString const & destDB) const + { + core::PureImageCache::ExportMapDataToDB(sourceDB, destDB); + } + /** + * @brief Returns the location for the SQLite Database used for caching and the geocoding cache files + * + * @return + */ + QString CacheLocation() + { + return core::Cache::Instance()->CacheLocation(); + } }; } #endif // CONFIGURATION_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp index 358ba5ed7..cf6f656e2 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp @@ -1,186 +1,171 @@ /** -****************************************************************************** -* -* @file gpsitem.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a UAV -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file gpsitem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a UAV + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "../internals/pureprojection.h" #include "gpsitem.h" -namespace mapcontrol +namespace mapcontrol { +GPSItem::GPSItem(MapGraphicItem *map, OPMapWidget *parent, QString uavPic) : map(map), mapwidget(parent), showtrail(true), showtrailline(true), trailtime(1), traildistance(2), autosetreached(true) + , autosetdistance(100) { - GPSItem::GPSItem(MapGraphicItem* map,OPMapWidget* parent,QString uavPic):map(map),mapwidget(parent),showtrail(true),showtrailline(true),trailtime(1),traildistance(2),autosetreached(true) - ,autosetdistance(100) - { - pic.load(uavPic); - // Don't scale but trust the image we are given - // pic=pic.scaled(50,33,Qt::IgnoreAspectRatio); - localposition=map->FromLatLngToLocal(mapwidget->CurrentPosition()); - this->setPos(localposition.X(),localposition.Y()); - this->setZValue(4); - trail=new QGraphicsItemGroup(this); - trail->setParentItem(map); - trailLine=new QGraphicsItemGroup(this); - trailLine->setParentItem(map); - this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); - setCacheMode(QGraphicsItem::ItemCoordinateCache); - mapfollowtype=UAVMapFollowType::None; - //trailtype=UAVTrailType::ByDistance; - trailtype=UAVTrailType::ByTimeElapsed; - timer.start(); - connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); - } - GPSItem::~GPSItem() - { + pic.load(uavPic); + // Don't scale but trust the image we are given + // pic=pic.scaled(50,33,Qt::IgnoreAspectRatio); + localposition = map->FromLatLngToLocal(mapwidget->CurrentPosition()); + this->setPos(localposition.X(), localposition.Y()); + this->setZValue(4); + trail = new QGraphicsItemGroup(this); + trail->setParentItem(map); + trailLine = new QGraphicsItemGroup(this); + trailLine->setParentItem(map); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + setCacheMode(QGraphicsItem::ItemCoordinateCache); + mapfollowtype = UAVMapFollowType::None; + // trailtype=UAVTrailType::ByDistance; + trailtype = UAVTrailType::ByTimeElapsed; + timer.start(); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); +} +GPSItem::~GPSItem() +{} +void GPSItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) +{ + Q_UNUSED(option); + Q_UNUSED(widget); + painter->drawPixmap(-pic.width() / 2, -pic.height() / 2, pic); +} +QRectF GPSItem::boundingRect() const +{ + return QRectF(-pic.width() / 2, -pic.height() / 2, pic.width(), pic.height()); +} +void GPSItem::SetUAVPos(const internals::PointLatLng &position, const int &altitude) +{ + if (coord.IsEmpty()) { + lastcoord = coord; } - - void GPSItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) - { - Q_UNUSED(option); - Q_UNUSED(widget); - painter->drawPixmap(-pic.width()/2,-pic.height()/2,pic); - } - QRectF GPSItem::boundingRect()const - { - return QRectF(-pic.width()/2,-pic.height()/2,pic.width(),pic.height()); - } - void GPSItem::SetUAVPos(const internals::PointLatLng &position, const int &altitude) - { - if(coord.IsEmpty()) - lastcoord=coord; - if(coord!=position) - { - - if(trailtype==UAVTrailType::ByTimeElapsed) - { - if(timer.elapsed()>trailtime*1000) - { - TrailItem * ob=new TrailItem(position,altitude,Qt::red,map); - trail->addToGroup(ob); - connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT())); - if(!lasttrailline.IsEmpty()) - { - TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::green,map); - trailLine->addToGroup(obj); - connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot())); - } - lasttrailline=position; - timer.restart(); + if (coord != position) { + if (trailtype == UAVTrailType::ByTimeElapsed) { + if (timer.elapsed() > trailtime * 1000) { + TrailItem *ob = new TrailItem(position, altitude, Qt::red, map); + trail->addToGroup(ob); + connect(this, SIGNAL(setChildPosition()), ob, SLOT(setPosSLOT())); + if (!lasttrailline.IsEmpty()) { + TrailLineItem *obj = new TrailLineItem(lasttrailline, position, Qt::green, map); + trailLine->addToGroup(obj); + connect(this, SIGNAL(setChildLine()), obj, SLOT(setLineSlot())); } - + lasttrailline = position; + timer.restart(); } - else if(trailtype==UAVTrailType::ByDistance) - { - if(qAbs(internals::PureProjection::DistanceBetweenLatLng(lastcoord,position)*1000)>traildistance) - { - TrailItem * ob=new TrailItem(position,altitude,Qt::red,map); - trail->addToGroup(ob); - connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT())); - if(!lasttrailline.IsEmpty()) - { - TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::green,map); - trailLine->addToGroup(obj); - connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot())); - } - lasttrailline=position; - lastcoord=position; + } else if (trailtype == UAVTrailType::ByDistance) { + if (qAbs(internals::PureProjection::DistanceBetweenLatLng(lastcoord, position) * 1000) > traildistance) { + TrailItem *ob = new TrailItem(position, altitude, Qt::red, map); + trail->addToGroup(ob); + connect(this, SIGNAL(setChildPosition()), ob, SLOT(setPosSLOT())); + if (!lasttrailline.IsEmpty()) { + TrailLineItem *obj = new TrailLineItem(lasttrailline, position, Qt::green, map); + trailLine->addToGroup(obj); + connect(this, SIGNAL(setChildLine()), obj, SLOT(setLineSlot())); } + lasttrailline = position; + lastcoord = position; } - coord=position; - this->altitude=altitude; - RefreshPos(); } - } - - /** - * Rotate the UAV Icon on the map, or rotate the map - * depending on the display mode - */ - void GPSItem::SetUAVHeading(const qreal &value) - { - if(mapfollowtype==UAVMapFollowType::CenterAndRotateMap) - { - mapwidget->SetRotate(-value); - } - else { - if (this->rotation() != value) - this->setRotation(value); - } - } - - - int GPSItem::type()const - { - return Type; - } - - - void GPSItem::RefreshPos() - { - localposition=map->FromLatLngToLocal(coord); - this->setPos(localposition.X(),localposition.Y()); - emit setChildPosition(); - emit setChildLine(); - - } - - void GPSItem::setOpacitySlot(qreal opacity) - { - setOpacity(opacity); - } - void GPSItem::SetTrailType(const UAVTrailType::Types &value) - { - trailtype=value; - if(trailtype==UAVTrailType::ByTimeElapsed) - timer.restart(); - } - void GPSItem::SetShowTrail(const bool &value) - { - showtrail=value; - trail->setVisible(value); - - } - void GPSItem::SetShowTrailLine(const bool &value) - { - showtrailline=value; - trailLine->setVisible(value); - } - void GPSItem::DeleteTrail()const - { - foreach(QGraphicsItem* i,trail->childItems()) - delete i; - foreach(QGraphicsItem* i,trailLine->childItems()) - delete i; - } - double GPSItem::Distance3D(const internals::PointLatLng &coord, const int &altitude) - { - return sqrt(pow(internals::PureProjection::DistanceBetweenLatLng(this->coord,coord)*1000,2)+ - pow(this->altitude-altitude,2)); - - } - void GPSItem::SetUavPic(QString UAVPic) - { - pic.load(":/uavs/images/"+UAVPic); + coord = position; + this->altitude = altitude; + RefreshPos(); } } + +/** + * Rotate the UAV Icon on the map, or rotate the map + * depending on the display mode + */ +void GPSItem::SetUAVHeading(const qreal &value) +{ + if (mapfollowtype == UAVMapFollowType::CenterAndRotateMap) { + mapwidget->SetRotate(-value); + } else { + if (this->rotation() != value) { + this->setRotation(value); + } + } +} + + +int GPSItem::type() const +{ + return Type; +} + + +void GPSItem::RefreshPos() +{ + localposition = map->FromLatLngToLocal(coord); + this->setPos(localposition.X(), localposition.Y()); + emit setChildPosition(); + emit setChildLine(); +} + +void GPSItem::setOpacitySlot(qreal opacity) +{ + setOpacity(opacity); +} +void GPSItem::SetTrailType(const UAVTrailType::Types &value) +{ + trailtype = value; + if (trailtype == UAVTrailType::ByTimeElapsed) { + timer.restart(); + } +} +void GPSItem::SetShowTrail(const bool &value) +{ + showtrail = value; + trail->setVisible(value); +} +void GPSItem::SetShowTrailLine(const bool &value) +{ + showtrailline = value; + trailLine->setVisible(value); +} +void GPSItem::DeleteTrail() const +{ + foreach(QGraphicsItem * i, trail->childItems()) + delete i; + foreach(QGraphicsItem * i, trailLine->childItems()) + delete i; +} +double GPSItem::Distance3D(const internals::PointLatLng &coord, const int &altitude) +{ + return sqrt(pow(internals::PureProjection::DistanceBetweenLatLng(this->coord, coord) * 1000, 2) + + pow(this->altitude - altitude, 2)); +} +void GPSItem::SetUavPic(QString UAVPic) +{ + pic.load(":/uavs/images/" + UAVPic); +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.h index c78c734aa..8e760b27e 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file gpsitem.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a WayPoint -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file gpsitem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef GPSITEM_H #define GPSITEM_H @@ -40,190 +40,229 @@ #include "opmapwidget.h" #include "trailitem.h" #include "traillineitem.h" -namespace mapcontrol -{ - class WayPointItem; - class OPMapWidget; +namespace mapcontrol { +class WayPointItem; +class OPMapWidget; +/** + * @brief A QGraphicsItem representing the UAV + * + * @class UAVItem gpsitem.h "mapwidget/gpsitem.h" + */ +class GPSItem : public QObject, public QGraphicsItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) +public: + enum { Type = UserType + 6 }; + GPSItem(MapGraphicItem *map, OPMapWidget *parent, QString uavPic = QString::fromUtf8(":/uavs/images/mapquad.png")); + ~GPSItem(); /** -* @brief A QGraphicsItem representing the UAV -* -* @class UAVItem gpsitem.h "mapwidget/gpsitem.h" -*/ - class GPSItem:public QObject,public QGraphicsItem + * @brief Sets the UAV position + * + * @param position LatLng point + * @param altitude altitude in meters + */ + void SetUAVPos(internals::PointLatLng const & position, int const & altitude); + /** + * @brief Sets the UAV heading + * + * @param value heading angle (north=0deg) + */ + void SetUAVHeading(qreal const & value); + /** + * @brief Returns the UAV position + * + * @return internals::PointLatLng + */ + internals::PointLatLng UAVPos() const { - Q_OBJECT - Q_INTERFACES(QGraphicsItem) - public: - enum { Type = UserType + 6 }; - GPSItem(MapGraphicItem* map,OPMapWidget* parent, QString uavPic=QString::fromUtf8(":/uavs/images/mapquad.png")); - ~GPSItem(); - /** - * @brief Sets the UAV position - * - * @param position LatLng point - * @param altitude altitude in meters - */ - void SetUAVPos(internals::PointLatLng const& position,int const& altitude); - /** - * @brief Sets the UAV heading - * - * @param value heading angle (north=0deg) - */ - void SetUAVHeading(qreal const& value); - /** - * @brief Returns the UAV position - * - * @return internals::PointLatLng - */ - internals::PointLatLng UAVPos()const{return coord;} - /** - * @brief Sets the Map follow type - * - * @param value can be "none"(map doesnt follow UAV), "CenterMap"(map moves to keep UAV centered) or "CenterAndRotateMap"(map moves and rotates to keep UAV centered and straight) - */ - void SetMapFollowType(UAVMapFollowType::Types const& value){mapfollowtype=value;} - /** - * @brief Sets the trail type - * - * @param value can be "NoTrail"(no trail is plotted), "ByTimeElapsed"(a trail point is plotted each TrailTime()) or ByDistance(a trail point is plotted if the distance between the UAV and the last trail point is bigger than TrailDistance()) - */ - void SetTrailType(UAVTrailType::Types const& value); - /** - * @brief Returns the map follow method used - * - * @return UAVMapFollowType::Types - */ - UAVMapFollowType::Types GetMapFollowType()const{return mapfollowtype;} - /** - * @brief Returns the UAV trail type. It can be plotted by time elapsed or distance - * - * @return UAVTrailType::Types - */ - UAVTrailType::Types GetTrailType()const{return trailtype;} + return coord; + } + /** + * @brief Sets the Map follow type + * + * @param value can be "none"(map doesnt follow UAV), "CenterMap"(map moves to keep UAV centered) or "CenterAndRotateMap"(map moves and rotates to keep UAV centered and straight) + */ + void SetMapFollowType(UAVMapFollowType::Types const & value) + { + mapfollowtype = value; + } + /** + * @brief Sets the trail type + * + * @param value can be "NoTrail"(no trail is plotted), "ByTimeElapsed"(a trail point is plotted each TrailTime()) or ByDistance(a trail point is plotted if the distance between the UAV and the last trail point is bigger than TrailDistance()) + */ + void SetTrailType(UAVTrailType::Types const & value); + /** + * @brief Returns the map follow method used + * + * @return UAVMapFollowType::Types + */ + UAVMapFollowType::Types GetMapFollowType() const + { + return mapfollowtype; + } + /** + * @brief Returns the UAV trail type. It can be plotted by time elapsed or distance + * + * @return UAVTrailType::Types + */ + UAVTrailType::Types GetTrailType() const + { + return trailtype; + } - void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, - QWidget *widget); - QRectF boundingRect() const; - /** - * @brief Sets the trail time to be used if TrailType is ByTimeElapsed - * - * @param seconds the UAV trail time elapsed value. If the trail type is time elapsed - * a trail point will be plotted each "value returned" seconds. - */ - void SetTrailTime(int const& seconds){trailtime=seconds;} - /** - * @brief Returns the UAV trail time elapsed value. If the trail type is time elapsed - * a trail point will be plotted each "value returned" seconds. - * - * @return int - */ - int TrailTime()const{return trailtime;} - /** - * @brief Sets the trail distance to be used if TrailType is ByDistance - * - * @param distance the UAV trail plot distance. - * If the trail type is ByDistance a trail dot is plotted if - * the distance between the current UAV position and the last trail point - * is bigger than the returned value - */ - void SetTrailDistance(int const& distance){traildistance=distance;} - /** - * @brief Returns the UAV trail plot distance. - * If the trail type is distance diference a trail dot is plotted if - * the distance between the current UAV position and the last trail point - * is bigger than the returned value - * - * @return int - */ - int TrailDistance()const{return traildistance;} - /** - * @brief Returns true if UAV trail is shown - * - * @return bool - */ - bool ShowTrail()const{return showtrail;} - /** - * @brief Returns true if UAV trail line is shown - * - * @return bool - */ - bool ShowTrailLine()const{return showtrailline;} - /** - * @brief Used to define if the UAV displays a trail - * - * @param value - */ - void SetShowTrail(bool const& value); - /** - * @brief Used to define if the UAV displays a trail line - * - * @param value - */ - void SetShowTrailLine(bool const& value); - /** - * @brief Deletes all the trail points - */ - void DeleteTrail()const; - /** - * @brief Returns true if the UAV automaticaly sets WP reached value (changing its color) - * - * @return bool - */ - bool AutoSetReached()const{return autosetreached;} - /** - * @brief Defines if the UAV can set the WP's "reached" value automaticaly. - * - * @param value - */ - void SetAutoSetReached(bool const& value){autosetreached=value;} - /** - * @brief Returns the 3D distance in meters necessary for the UAV to set WP's to "reached" - * - * @return double - */ - double AutoSetDistance()const{return autosetdistance;} - /** - * @brief Sets the the 3D distance in meters necessary for the UAV to set WP's to "reached" - * - * @param value - */ - void SetAutoSetDistance(double const& value){autosetdistance=value;} + void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, + QWidget *widget); + QRectF boundingRect() const; + /** + * @brief Sets the trail time to be used if TrailType is ByTimeElapsed + * + * @param seconds the UAV trail time elapsed value. If the trail type is time elapsed + * a trail point will be plotted each "value returned" seconds. + */ + void SetTrailTime(int const & seconds) + { + trailtime = seconds; + } + /** + * @brief Returns the UAV trail time elapsed value. If the trail type is time elapsed + * a trail point will be plotted each "value returned" seconds. + * + * @return int + */ + int TrailTime() const + { + return trailtime; + } + /** + * @brief Sets the trail distance to be used if TrailType is ByDistance + * + * @param distance the UAV trail plot distance. + * If the trail type is ByDistance a trail dot is plotted if + * the distance between the current UAV position and the last trail point + * is bigger than the returned value + */ + void SetTrailDistance(int const & distance) + { + traildistance = distance; + } + /** + * @brief Returns the UAV trail plot distance. + * If the trail type is distance diference a trail dot is plotted if + * the distance between the current UAV position and the last trail point + * is bigger than the returned value + * + * @return int + */ + int TrailDistance() const + { + return traildistance; + } + /** + * @brief Returns true if UAV trail is shown + * + * @return bool + */ + bool ShowTrail() const + { + return showtrail; + } + /** + * @brief Returns true if UAV trail line is shown + * + * @return bool + */ + bool ShowTrailLine() const + { + return showtrailline; + } + /** + * @brief Used to define if the UAV displays a trail + * + * @param value + */ + void SetShowTrail(bool const & value); + /** + * @brief Used to define if the UAV displays a trail line + * + * @param value + */ + void SetShowTrailLine(bool const & value); + /** + * @brief Deletes all the trail points + */ + void DeleteTrail() const; + /** + * @brief Returns true if the UAV automaticaly sets WP reached value (changing its color) + * + * @return bool + */ + bool AutoSetReached() const + { + return autosetreached; + } + /** + * @brief Defines if the UAV can set the WP's "reached" value automaticaly. + * + * @param value + */ + void SetAutoSetReached(bool const & value) + { + autosetreached = value; + } + /** + * @brief Returns the 3D distance in meters necessary for the UAV to set WP's to "reached" + * + * @return double + */ + double AutoSetDistance() const + { + return autosetdistance; + } + /** + * @brief Sets the the 3D distance in meters necessary for the UAV to set WP's to "reached" + * + * @param value + */ + void SetAutoSetDistance(double const & value) + { + autosetdistance = value; + } - int type() const; + int type() const; - void SetUavPic(QString UAVPic); - private: - MapGraphicItem* map; + void SetUavPic(QString UAVPic); +private: + MapGraphicItem *map; - int altitude; - UAVMapFollowType::Types mapfollowtype; - UAVTrailType::Types trailtype; - internals::PointLatLng coord; - internals::PointLatLng lastcoord; - QPixmap pic; - core::Point localposition; - OPMapWidget* mapwidget; - QGraphicsItemGroup* trail; - QGraphicsItemGroup * trailLine; - internals::PointLatLng lasttrailline; - QTime timer; - bool showtrail; - bool showtrailline; - int trailtime; - int traildistance; - bool autosetreached; - double Distance3D(internals::PointLatLng const& coord, int const& altitude); - double autosetdistance; - // QRectF rect; + int altitude; + UAVMapFollowType::Types mapfollowtype; + UAVTrailType::Types trailtype; + internals::PointLatLng coord; + internals::PointLatLng lastcoord; + QPixmap pic; + core::Point localposition; + OPMapWidget *mapwidget; + QGraphicsItemGroup *trail; + QGraphicsItemGroup *trailLine; + internals::PointLatLng lasttrailline; + QTime timer; + bool showtrail; + bool showtrailline; + int trailtime; + int traildistance; + bool autosetreached; + double Distance3D(internals::PointLatLng const & coord, int const & altitude); + double autosetdistance; + // QRectF rect; - public slots: - void RefreshPos(); - void setOpacitySlot(qreal opacity); - signals: - void UAVReachedWayPoint(int const& waypointnumber,WayPointItem* waypoint); - void UAVLeftSafetyBouble(internals::PointLatLng const& position); - void setChildPosition(); - void setChildLine(); - }; +public slots: + void RefreshPos(); + void setOpacitySlot(qreal opacity); +signals: + void UAVReachedWayPoint(int const & waypointnumber, WayPointItem *waypoint); + void UAVLeftSafetyBouble(internals::PointLatLng const & position); + void setChildPosition(); + void setChildLine(); +}; } #endif // GPSITEM_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.cpp index fa8bddb48..311ac4b50 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.cpp @@ -1,155 +1,148 @@ /** -****************************************************************************** -* -* @file homeitem.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a trail point -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file homeitem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a trail point + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "homeitem.h" -namespace mapcontrol +namespace mapcontrol { +HomeItem::HomeItem(MapGraphicItem *map, OPMapWidget *parent) : safe(true), map(map), mapwidget(parent), + showsafearea(true), toggleRefresh(true), safearea(1000), altitude(0), isDragging(false) { - HomeItem::HomeItem(MapGraphicItem* map,OPMapWidget* parent):safe(true),map(map),mapwidget(parent), - showsafearea(true),toggleRefresh(true),safearea(1000),altitude(0),isDragging(false) - { - pic.load(QString::fromUtf8(":/markers/images/home2.svg")); - pic=pic.scaled(30,30,Qt::IgnoreAspectRatio); - this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); - this->setFlag(QGraphicsItem::ItemIsMovable,false); - this->setFlag(QGraphicsItem::ItemIsSelectable,true); - localposition=map->FromLatLngToLocal(mapwidget->CurrentPosition()); - this->setPos(localposition.X(),localposition.Y()); - this->setZValue(4); - coord=internals::PointLatLng(50,50); - RefreshToolTip(); - setCacheMode(QGraphicsItem::DeviceCoordinateCache); - connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); - } - - void HomeItem::RefreshToolTip() - { - QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); - - setToolTip(QString("Waypoint: Home\nCoordinate:%1\nAltitude:%2\n").arg(coord_str).arg(QString::number(altitude))); - } - - - void HomeItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) - { - Q_UNUSED(option); - Q_UNUSED(widget); - painter->drawPixmap(-pic.width()/2,-pic.height()/2,pic); - if(showsafearea) - { - if(safe) - painter->setPen(Qt::green); - else - painter->setPen(Qt::red); - painter->drawEllipse(QPointF(0,0),localsafearea,localsafearea); - // painter->drawRect(QRectF(-localsafearea,-localsafearea,localsafearea*2,localsafearea*2)); - } - - } - QRectF HomeItem::boundingRect()const - { - if(pic.width()>localsafearea*2 && !toggleRefresh) - return QRectF(-pic.width()/2,-pic.height()/2,pic.width(),pic.height()); - else - return QRectF(-localsafearea,-localsafearea,localsafearea*2,localsafearea*2); - } - - - int HomeItem::type()const - { - return Type; - } - - void HomeItem::RefreshPos() - { - prepareGeometryChange(); - localposition=map->FromLatLngToLocal(coord); - this->setPos(localposition.X(),localposition.Y()); - if(showsafearea) - localsafearea=safearea/map->Projection()->GetGroundResolution(map->ZoomTotal(),coord.Lat()); - - RefreshToolTip(); - - this->update(); - toggleRefresh=false; - - } - - void HomeItem::setOpacitySlot(qreal opacity) - { - setOpacity(opacity); - } - - void HomeItem::mousePressEvent(QGraphicsSceneMouseEvent *event) - { - if(event->button()==Qt::LeftButton) - { - isDragging=true; - } - QGraphicsItem::mousePressEvent(event); - } - - void HomeItem::mouseReleaseEvent(QGraphicsSceneMouseEvent *event) - { - if(event->button()==Qt::LeftButton) - { - coord=map->FromLocalToLatLng(this->pos().x(),this->pos().y()); - isDragging=false; - - emit homePositionChanged(coord,Altitude()); - } - QGraphicsItem::mouseReleaseEvent(event); - - RefreshToolTip(); - } - void HomeItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent *event) - { - if(event->button()==Qt::LeftButton) - { - emit homedoubleclick(this); - } - } - void HomeItem::mouseMoveEvent(QGraphicsSceneMouseEvent *event) - { - - if(isDragging) - { - coord=map->FromLocalToLatLng(this->pos().x(),this->pos().y()); - emit homePositionChanged(coord,Altitude()); - } - QGraphicsItem::mouseMoveEvent(event); - } - - //Set clickable area as smaller than the bounding rect. - QPainterPath HomeItem::shape() const - { - QPainterPath path; - path.addEllipse(QRectF(-12, -25, 24, 50)); - return path; - } - + pic.load(QString::fromUtf8(":/markers/images/home2.svg")); + pic = pic.scaled(30, 30, Qt::IgnoreAspectRatio); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + this->setFlag(QGraphicsItem::ItemIsMovable, false); + this->setFlag(QGraphicsItem::ItemIsSelectable, true); + localposition = map->FromLatLngToLocal(mapwidget->CurrentPosition()); + this->setPos(localposition.X(), localposition.Y()); + this->setZValue(4); + coord = internals::PointLatLng(50, 50); + RefreshToolTip(); + setCacheMode(QGraphicsItem::DeviceCoordinateCache); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); } +void HomeItem::RefreshToolTip() +{ + QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); + + setToolTip(QString("Waypoint: Home\nCoordinate:%1\nAltitude:%2\n").arg(coord_str).arg(QString::number(altitude))); +} + + +void HomeItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) +{ + Q_UNUSED(option); + Q_UNUSED(widget); + painter->drawPixmap(-pic.width() / 2, -pic.height() / 2, pic); + if (showsafearea) { + if (safe) { + painter->setPen(Qt::green); + } else { + painter->setPen(Qt::red); + } + painter->drawEllipse(QPointF(0, 0), localsafearea, localsafearea); + // painter->drawRect(QRectF(-localsafearea,-localsafearea,localsafearea*2,localsafearea*2)); + } +} +QRectF HomeItem::boundingRect() const +{ + if (pic.width() > localsafearea * 2 && !toggleRefresh) { + return QRectF(-pic.width() / 2, -pic.height() / 2, pic.width(), pic.height()); + } else { + return QRectF(-localsafearea, -localsafearea, localsafearea * 2, localsafearea * 2); + } +} + + +int HomeItem::type() const +{ + return Type; +} + +void HomeItem::RefreshPos() +{ + prepareGeometryChange(); + localposition = map->FromLatLngToLocal(coord); + this->setPos(localposition.X(), localposition.Y()); + if (showsafearea) { + localsafearea = safearea / map->Projection()->GetGroundResolution(map->ZoomTotal(), coord.Lat()); + } + + RefreshToolTip(); + + this->update(); + toggleRefresh = false; +} + +void HomeItem::setOpacitySlot(qreal opacity) +{ + setOpacity(opacity); +} + +void HomeItem::mousePressEvent(QGraphicsSceneMouseEvent *event) +{ + if (event->button() == Qt::LeftButton) { + isDragging = true; + } + QGraphicsItem::mousePressEvent(event); +} + +void HomeItem::mouseReleaseEvent(QGraphicsSceneMouseEvent *event) +{ + if (event->button() == Qt::LeftButton) { + coord = map->FromLocalToLatLng(this->pos().x(), this->pos().y()); + isDragging = false; + + emit homePositionChanged(coord, Altitude()); + } + QGraphicsItem::mouseReleaseEvent(event); + + RefreshToolTip(); +} +void HomeItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent *event) +{ + if (event->button() == Qt::LeftButton) { + emit homedoubleclick(this); + } +} +void HomeItem::mouseMoveEvent(QGraphicsSceneMouseEvent *event) +{ + if (isDragging) { + coord = map->FromLocalToLatLng(this->pos().x(), this->pos().y()); + emit homePositionChanged(coord, Altitude()); + } + QGraphicsItem::mouseMoveEvent(event); +} + +// Set clickable area as smaller than the bounding rect. +QPainterPath HomeItem::shape() const +{ + QPainterPath path; + + path.addEllipse(QRectF(-12, -25, 24, 50)); + return path; +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.h index 2cf34b135..783b30a2c 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file homeitem.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a WayPoint -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file homeitem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef HOMEITEM_H #define HOMEITEM_H @@ -33,56 +33,79 @@ #include "../internals/pointlatlng.h" #include #include "opmapwidget.h" -namespace mapcontrol -{ - - class HomeItem:public QObject,public QGraphicsItem +namespace mapcontrol { +class HomeItem : public QObject, public QGraphicsItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) +public: + enum { Type = UserType + 4 }; + HomeItem(MapGraphicItem *map, OPMapWidget *parent); + void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, + QWidget *widget); + QRectF boundingRect() const; + int type() const; + bool ShowSafeArea() const { - Q_OBJECT - Q_INTERFACES(QGraphicsItem) - public: - enum { Type = UserType + 4 }; - HomeItem(MapGraphicItem* map,OPMapWidget* parent); - void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, - QWidget *widget); - QRectF boundingRect() const; - int type() const; - bool ShowSafeArea()const{return showsafearea;} - void SetShowSafeArea(bool const& value){showsafearea=value;} - void SetToggleRefresh(bool const& value){toggleRefresh=value;} - int SafeArea()const{return safearea;} - void SetSafeArea(int const& value){safearea=value;} - bool safe; - void SetCoord(internals::PointLatLng const& value){coord=value;emit homePositionChanged(value,Altitude());} - internals::PointLatLng Coord()const{return coord;} - void SetAltitude(float const& value){altitude=value;emit homePositionChanged(Coord(),Altitude());} - float Altitude()const{return altitude;} - void RefreshToolTip(); - private: + return showsafearea; + } + void SetShowSafeArea(bool const & value) + { + showsafearea = value; + } + void SetToggleRefresh(bool const & value) + { + toggleRefresh = value; + } + int SafeArea() const + { + return safearea; + } + void SetSafeArea(int const & value) + { + safearea = value; + } + bool safe; + void SetCoord(internals::PointLatLng const & value) + { + coord = value; emit homePositionChanged(value, Altitude()); + } + internals::PointLatLng Coord() const + { + return coord; + } + void SetAltitude(float const & value) + { + altitude = value; emit homePositionChanged(Coord(), Altitude()); + } + float Altitude() const + { + return altitude; + } + void RefreshToolTip(); +private: - MapGraphicItem* map; - OPMapWidget* mapwidget; - QPixmap pic; - core::Point localposition; - internals::PointLatLng coord; - bool showsafearea; - bool toggleRefresh; - int safearea; - int localsafearea; - float altitude; - bool isDragging; - protected: - void mouseMoveEvent ( QGraphicsSceneMouseEvent * event ); - void mousePressEvent ( QGraphicsSceneMouseEvent * event ); - void mouseReleaseEvent ( QGraphicsSceneMouseEvent * event ); - void mouseDoubleClickEvent(QGraphicsSceneMouseEvent *event); - QPainterPath shape() const; - public slots: - void RefreshPos(); - void setOpacitySlot(qreal opacity); - signals: - void homePositionChanged(internals::PointLatLng coord,float); - void homedoubleclick(HomeItem* waypoint); - }; + MapGraphicItem *map; + OPMapWidget *mapwidget; + QPixmap pic; + core::Point localposition; + internals::PointLatLng coord; + bool showsafearea; + bool toggleRefresh; + int safearea; + int localsafearea; + float altitude; + bool isDragging; +protected: + void mouseMoveEvent(QGraphicsSceneMouseEvent *event); + void mousePressEvent(QGraphicsSceneMouseEvent *event); + void mouseReleaseEvent(QGraphicsSceneMouseEvent *event); + void mouseDoubleClickEvent(QGraphicsSceneMouseEvent *event); + QPainterPath shape() const; +public slots: + void RefreshPos(); + void setOpacitySlot(qreal opacity); +signals: + void homePositionChanged(internals::PointLatLng coord, float); + void homedoubleclick(HomeItem *waypoint); +}; } #endif // HOMEITEM_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp index 587303d6d..c4ff05c69 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp @@ -1,563 +1,495 @@ /** -****************************************************************************** -* -* @file mapgraphicitem.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief The main graphicsItem used on the widget, contains the map and map logic -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file mapgraphicitem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief The main graphicsItem used on the widget, contains the map and map logic + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "uavitem.h" #include "gpsitem.h" #include "homeitem.h" #include "mapgraphicitem.h" -namespace mapcontrol +namespace mapcontrol { +MapGraphicItem::MapGraphicItem(internals::Core *core, Configuration *configuration) : core(core), config(configuration), MapRenderTransform(1), maxZoom(17), minZoom(2), zoomReal(0), isSelected(false), rotation(0), zoomDigi(0) { - MapGraphicItem::MapGraphicItem(internals::Core *core, Configuration *configuration):core(core),config(configuration),MapRenderTransform(1), maxZoom(17),minZoom(2),zoomReal(0),isSelected(false),rotation(0),zoomDigi(0) + dragons.load(QString::fromUtf8(":/markers/images/dragons1.jpg")); + showTileGridLines = false; + isMouseOverMarker = false; + maprect = QRectF(0, 0, 1022, 680); + core->SetCurrentRegion(internals::Rectangle(0, 0, maprect.width(), maprect.height())); + core->SetMapType(MapType::GoogleHybrid); + this->SetZoom(2); + this->setFlag(ItemIsFocusable); + connect(core, SIGNAL(OnNeedInvalidation()), this, SLOT(Core_OnNeedInvalidation())); + connect(core, SIGNAL(OnMapDrag()), this, SLOT(childPosRefresh())); + connect(core, SIGNAL(OnMapZoomChanged()), this, SLOT(childPosRefresh())); + setCacheMode(QGraphicsItem::ItemCoordinateCache); +} + +void MapGraphicItem::start() +{ + core->StartSystem(); +} + +void MapGraphicItem::resize(const QRectF &rect) +{ + Q_UNUSED(rect); { - dragons.load(QString::fromUtf8(":/markers/images/dragons1.jpg")); - showTileGridLines=false; - isMouseOverMarker=false; - maprect=QRectF(0,0,1022,680); - core->SetCurrentRegion(internals::Rectangle(0, 0, maprect.width(), maprect.height())); - core->SetMapType(MapType::GoogleHybrid); - this->SetZoom(2); - this->setFlag(ItemIsFocusable); - connect(core,SIGNAL(OnNeedInvalidation()),this,SLOT(Core_OnNeedInvalidation())); - connect(core,SIGNAL(OnMapDrag()),this,SLOT(childPosRefresh())); - connect(core,SIGNAL(OnMapZoomChanged()),this,SLOT(childPosRefresh())); - setCacheMode(QGraphicsItem::ItemCoordinateCache); + this->prepareGeometryChange(); + maprect = boundingBox(scene()->sceneRect(), rotation); + this->setTransform(QTransform().translate(-(maprect.width() - scene()->width()) / 2, -(maprect.height() - scene()->height()) / 2)); + this->setTransformOriginPoint(maprect.center().x(), maprect.center().y()); + this->setRotation(rotation); } - void MapGraphicItem::start() - { - core->StartSystem(); + core->OnMapSizeChanged(maprect.width(), maprect.height()); + core->SetCurrentRegion(internals::Rectangle(0, 0, maprect.width(), maprect.height())); + if (isVisible()) { + core->GoToCurrentPosition(); } +} - void MapGraphicItem::resize(const QRectF &rect) - { - Q_UNUSED(rect); - { - this->prepareGeometryChange(); - maprect=boundingBox(scene()->sceneRect(),rotation); - this->setTransform(QTransform().translate(-(maprect.width()-scene()->width())/2,-(maprect.height()-scene()->height())/2)); - this->setTransformOriginPoint(maprect.center().x(),maprect.center().y()); - this->setRotation(rotation); - } +QRectF MapGraphicItem::boundingRect() const +{ + const int Margin = 1; - core->OnMapSizeChanged(maprect.width(),maprect.height()); - core->SetCurrentRegion(internals::Rectangle(0, 0, maprect.width(), maprect.height())); - if(isVisible()) - { - core->GoToCurrentPosition(); - } - } + return maprect.adjusted(-Margin, -Margin, +Margin, +Margin); +} +void MapGraphicItem::Core_OnNeedInvalidation() +{ + this->update(); + emit childRefreshPosition(); +} +void MapGraphicItem::childPosRefresh() +{ + emit childRefreshPosition(); +} +void MapGraphicItem::setOverlayOpacity(qreal value) +{ + emit childSetOpacity(value); +} +void MapGraphicItem::ConstructLastImage(int const & zoomdiff) +{ + QImage temp; + QSize size = boundingRect().size().toSize(); - QRectF MapGraphicItem::boundingRect() const - { - const int Margin = 1; - return maprect.adjusted(-Margin, -Margin, +Margin, +Margin); - } - void MapGraphicItem::Core_OnNeedInvalidation() - { - this->update(); - emit childRefreshPosition(); - } - void MapGraphicItem::childPosRefresh() - { - emit childRefreshPosition(); - } - void MapGraphicItem::setOverlayOpacity(qreal value) - { - emit childSetOpacity(value); - } - void MapGraphicItem::ConstructLastImage(int const& zoomdiff) - { - QImage temp; - QSize size=boundingRect().size().toSize(); - size.setWidth(size.width()*2*zoomdiff); - size.setHeight(size.height()*2*zoomdiff); - temp=QImage(size,QImage::Format_ARGB32_Premultiplied); - temp.fill(0); - QPainter imagePainter(&temp); - imagePainter.translate(-boundingRect().topLeft()); - imagePainter.scale(2*zoomdiff,2*zoomdiff); - paintImage(&imagePainter); - imagePainter.end(); - lastimagepoint=Point(core->GetrenderOffset().X()*2*zoomdiff,core->GetrenderOffset().Y()*2*zoomdiff); - lastimage=temp; - } - void MapGraphicItem::paintImage(QPainter *painter) - { - - if(MapRenderTransform!=1) - { - QTransform transform; - transform.translate(-((boundingRect().width()*MapRenderTransform)-(boundingRect().width()))/2,-((boundingRect().height()*MapRenderTransform)-(boundingRect().height()))/2); - transform.scale(MapRenderTransform,MapRenderTransform); - painter->setWorldTransform(transform); - { - DrawMap2D(painter); - } - painter->resetTransform(); - } - else + size.setWidth(size.width() * 2 * zoomdiff); + size.setHeight(size.height() * 2 * zoomdiff); + temp = QImage(size, QImage::Format_ARGB32_Premultiplied); + temp.fill(0); + QPainter imagePainter(&temp); + imagePainter.translate(-boundingRect().topLeft()); + imagePainter.scale(2 * zoomdiff, 2 * zoomdiff); + paintImage(&imagePainter); + imagePainter.end(); + lastimagepoint = Point(core->GetrenderOffset().X() * 2 * zoomdiff, core->GetrenderOffset().Y() * 2 * zoomdiff); + lastimage = temp; +} +void MapGraphicItem::paintImage(QPainter *painter) +{ + if (MapRenderTransform != 1) { + QTransform transform; + transform.translate(-((boundingRect().width() * MapRenderTransform) - (boundingRect().width())) / 2, -((boundingRect().height() * MapRenderTransform) - (boundingRect().height())) / 2); + transform.scale(MapRenderTransform, MapRenderTransform); + painter->setWorldTransform(transform); { DrawMap2D(painter); } - //painter->drawRect(maprect); + painter->resetTransform(); + } else { + DrawMap2D(painter); } - void MapGraphicItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) - { - Q_UNUSED(option); - Q_UNUSED(widget); + // painter->drawRect(maprect); +} +void MapGraphicItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) +{ + Q_UNUSED(option); + Q_UNUSED(widget); - if(MapRenderTransform!=1) - { - QTransform transform; - transform.translate(-((boundingRect().width()*MapRenderTransform)-(boundingRect().width()))/2,-((boundingRect().height()*MapRenderTransform)-(boundingRect().height()))/2); - transform.scale(MapRenderTransform,MapRenderTransform); + if (MapRenderTransform != 1) { + QTransform transform; + transform.translate(-((boundingRect().width() * MapRenderTransform) - (boundingRect().width())) / 2, -((boundingRect().height() * MapRenderTransform) - (boundingRect().height())) / 2); + transform.scale(MapRenderTransform, MapRenderTransform); - painter->setWorldTransform(transform); - painter->setRenderHint(QPainter::SmoothPixmapTransform,true); - painter->setRenderHint(QPainter::HighQualityAntialiasing,true); + painter->setWorldTransform(transform); + painter->setRenderHint(QPainter::SmoothPixmapTransform, true); + painter->setRenderHint(QPainter::HighQualityAntialiasing, true); - { - DrawMap2D(painter); - } - painter->resetTransform(); - } - else { DrawMap2D(painter); } + painter->resetTransform(); + } else { + DrawMap2D(painter); } - void MapGraphicItem::mouseMoveEvent(QGraphicsSceneMouseEvent *event) - { - if(core->IsDragging()) - { - if(MapRenderTransform!=1) - { - qreal dx= (event->pos().x()-core->mouseDown.X())/(MapRenderTransform); - qreal dy= (event->pos().y()-core->mouseDown.Y())/(MapRenderTransform); - qreal nx=core->mouseDown.X()+dx; - qreal ny=core->mouseDown.Y()+dy; - core->mouseCurrent.SetX(nx); - core->mouseCurrent.SetY(ny); - } - else - { - core->mouseCurrent.SetX(event->pos().x()); - core->mouseCurrent.SetY(event->pos().y()); - } - { - core->Drag(core->mouseCurrent); - } - +} +void MapGraphicItem::mouseMoveEvent(QGraphicsSceneMouseEvent *event) +{ + if (core->IsDragging()) { + if (MapRenderTransform != 1) { + qreal dx = (event->pos().x() - core->mouseDown.X()) / (MapRenderTransform); + qreal dy = (event->pos().y() - core->mouseDown.Y()) / (MapRenderTransform); + qreal nx = core->mouseDown.X() + dx; + qreal ny = core->mouseDown.Y() + dy; + core->mouseCurrent.SetX(nx); + core->mouseCurrent.SetY(ny); + } else { + core->mouseCurrent.SetX(event->pos().x()); + core->mouseCurrent.SetY(event->pos().y()); } - else if(isSelected && !selectionStart.IsEmpty() && (event->modifiers() == Qt::ControlModifier || event->modifiers() == Qt::ShiftModifier)) { - selectionEnd = FromLocalToLatLng(event->pos().x(), event->pos().y()); - { - internals::PointLatLng p1 = selectionStart; - internals::PointLatLng p2 = selectionEnd; - - double x1 = qMin(p1.Lng(), p2.Lng()); - double y1 = qMax(p1.Lat(), p2.Lat()); - double x2 = qMax(p1.Lng(), p2.Lng()); - double y2 = qMin(p1.Lat(), p2.Lat()); - - SetSelectedArea(internals::RectLatLng(y1, x1, x2 - x1, y1 - y2)); - } + core->Drag(core->mouseCurrent); } - QGraphicsItem::mouseMoveEvent(event); - } - void MapGraphicItem::mousePressEvent(QGraphicsSceneMouseEvent *event) - { - if(!IsMouseOverMarker()) + } else if (isSelected && !selectionStart.IsEmpty() && (event->modifiers() == Qt::ControlModifier || event->modifiers() == Qt::ShiftModifier)) { + selectionEnd = FromLocalToLatLng(event->pos().x(), event->pos().y()); { - if(event->button() == config->DragButton && CanDragMap()&& !((event->modifiers()==Qt::ShiftModifier)||(event->modifiers()==Qt::ControlModifier))) - { - core->mouseDown.SetX(event->pos().x()); - core->mouseDown.SetY(event->pos().y()); + internals::PointLatLng p1 = selectionStart; + internals::PointLatLng p2 = selectionEnd; + double x1 = qMin(p1.Lng(), p2.Lng()); + double y1 = qMax(p1.Lat(), p2.Lat()); + double x2 = qMax(p1.Lng(), p2.Lng()); + double y2 = qMin(p1.Lat(), p2.Lat()); - this->setCursor(Qt::SizeAllCursor); - - core->BeginDrag(core->mouseDown); - this->update(); - - } - else if(!isSelected && ((event->modifiers()==Qt::ControlModifier)||(event->modifiers()==Qt::ShiftModifier))) - { - isSelected = true; - SetSelectedArea (internals::RectLatLng::Empty); - selectionEnd = internals::PointLatLng::Empty; - selectionStart = FromLocalToLatLng(event->pos().x(), event->pos().y()); - } - } - } - void MapGraphicItem::mouseReleaseEvent(QGraphicsSceneMouseEvent *event) - { - if(isSelected) - { - isSelected = false; - } - - if(core->IsDragging()) - { - core->EndDrag(); - - this->setCursor(Qt::ArrowCursor); - if(!BoundsOfMap.IsEmpty() && !BoundsOfMap.Contains(core->CurrentPosition())) - { - if(!core->LastLocationInBounds.IsEmpty()) - { - core->SetCurrentPosition(core->LastLocationInBounds); - } - } - } - else - { - if(!selectionEnd.IsEmpty() && !selectionStart.IsEmpty()) - { - if(!selectedArea.IsEmpty() && event->modifiers() == Qt::ShiftModifier) - { - SetZoomToFitRect(SelectedArea()); - selectedArea=internals::RectLatLng::Empty; - } - } - + SetSelectedArea(internals::RectLatLng(y1, x1, x2 - x1, y1 - y2)); } } - void MapGraphicItem::keyPressEvent(QKeyEvent *event) - { - if(event->modifiers()&(Qt::ShiftModifier|Qt::ControlModifier)) - this->setCursor(Qt::CrossCursor); - if(event->key()==Qt::Key_Escape) - selectedArea=internals::RectLatLng::Empty; - QGraphicsItem::keyPressEvent(event); - } - void MapGraphicItem::keyReleaseEvent(QKeyEvent *event) - { - if((event->modifiers()&(Qt::ShiftModifier|Qt::ControlModifier))==0) - this->setCursor(Qt::ArrowCursor); - } - bool MapGraphicItem::SetZoomToFitRect(internals::RectLatLng const& rect) - { - int maxZoom = core->GetMaxZoomToFitRect(rect); - if(maxZoom > 0) - { - internals::PointLatLng center=internals::PointLatLng(rect.Lat()-(rect.HeightLat()/2), rect.Lng()+(rect.WidthLng()/2)); - core->SetCurrentPosition(center); + QGraphicsItem::mouseMoveEvent(event); +} +void MapGraphicItem::mousePressEvent(QGraphicsSceneMouseEvent *event) +{ + if (!IsMouseOverMarker()) { + if (event->button() == config->DragButton && CanDragMap() && !((event->modifiers() == Qt::ShiftModifier) || (event->modifiers() == Qt::ControlModifier))) { + core->mouseDown.SetX(event->pos().x()); + core->mouseDown.SetY(event->pos().y()); - if(maxZoom > MaxZoom()) - { - maxZoom = MaxZoom(); - } - if((int) Zoom() != maxZoom) - { - SetZoom(maxZoom); - } + this->setCursor(Qt::SizeAllCursor); - return true; - } - return false; - } - - void MapGraphicItem::wheelEvent(QGraphicsSceneWheelEvent *event) - { - - if(!IsMouseOverMarker() && !IsDragging()) - { - if(core->GetmouseLastZoom().X() != event->pos().x() && core->mouseLastZoom.Y() != event->pos().y()) - { - if(GetMouseWheelZoomType() == internals::MouseWheelZoomType::MousePositionAndCenter) - { - core->SetCurrentPosition(FromLocalToLatLng(event->pos().x(), event->pos().y())); - } - else if(GetMouseWheelZoomType() == internals::MouseWheelZoomType::ViewCenter) - { - core->SetCurrentPosition(FromLocalToLatLng((int) maprect.width()/2, (int) maprect.height()/2)); - } - else if(GetMouseWheelZoomType() == internals::MouseWheelZoomType::MousePositionWithoutCenter) - { - core->SetCurrentPosition(FromLocalToLatLng(event->pos().x(), event->pos().y())); - - } - - core->mouseLastZoom.SetX((event->pos().x())); - core->mouseLastZoom.SetY((event->pos().y())); - } - - // set mouse position to map center - if(GetMouseWheelZoomType() != internals::MouseWheelZoomType::MousePositionWithoutCenter) - { - { - // System.Drawing.Point p = PointToScreen(new System.Drawing.Point(Width/2, Height/2)); - // Stuff.SetCursorPos((int) p.X, (int) p.Y); - } - } - - core->MouseWheelZooming = true; - - if(event->delta() > 0) - { - SetZoom(ZoomTotal()+1); - } - else if(event->delta() < 0) - { - SetZoom(ZoomTotal()-1); - } - - core->MouseWheelZooming = false; + core->BeginDrag(core->mouseDown); + this->update(); + } else if (!isSelected && ((event->modifiers() == Qt::ControlModifier) || (event->modifiers() == Qt::ShiftModifier))) { + isSelected = true; + SetSelectedArea(internals::RectLatLng::Empty); + selectionEnd = internals::PointLatLng::Empty; + selectionStart = FromLocalToLatLng(event->pos().x(), event->pos().y()); } } - void MapGraphicItem::DrawMap2D(QPainter *painter) - { - painter->drawImage(this->boundingRect(),dragons.toImage()); - if(!lastimage.isNull()) - painter->drawImage(core->GetrenderOffset().X()-lastimagepoint.X(),core->GetrenderOffset().Y()-lastimagepoint.Y(),lastimage); +} +void MapGraphicItem::mouseReleaseEvent(QGraphicsSceneMouseEvent *event) +{ + if (isSelected) { + isSelected = false; + } - for(int i = -core->GetsizeOfMapArea().Width(); i <= core->GetsizeOfMapArea().Width(); i++) - { - for(int j = -core->GetsizeOfMapArea().Height(); j <= core->GetsizeOfMapArea().Height(); j++) + if (core->IsDragging()) { + core->EndDrag(); + + this->setCursor(Qt::ArrowCursor); + if (!BoundsOfMap.IsEmpty() && !BoundsOfMap.Contains(core->CurrentPosition())) { + if (!core->LastLocationInBounds.IsEmpty()) { + core->SetCurrentPosition(core->LastLocationInBounds); + } + } + } else { + if (!selectionEnd.IsEmpty() && !selectionStart.IsEmpty()) { + if (!selectedArea.IsEmpty() && event->modifiers() == Qt::ShiftModifier) { + SetZoomToFitRect(SelectedArea()); + selectedArea = internals::RectLatLng::Empty; + } + } + } +} +void MapGraphicItem::keyPressEvent(QKeyEvent *event) +{ + if (event->modifiers() & (Qt::ShiftModifier | Qt::ControlModifier)) { + this->setCursor(Qt::CrossCursor); + } + if (event->key() == Qt::Key_Escape) { + selectedArea = internals::RectLatLng::Empty; + } + QGraphicsItem::keyPressEvent(event); +} +void MapGraphicItem::keyReleaseEvent(QKeyEvent *event) +{ + if ((event->modifiers() & (Qt::ShiftModifier | Qt::ControlModifier)) == 0) { + this->setCursor(Qt::ArrowCursor); + } +} +bool MapGraphicItem::SetZoomToFitRect(internals::RectLatLng const & rect) +{ + int maxZoom = core->GetMaxZoomToFitRect(rect); + + if (maxZoom > 0) { + internals::PointLatLng center = internals::PointLatLng(rect.Lat() - (rect.HeightLat() / 2), rect.Lng() + (rect.WidthLng() / 2)); + core->SetCurrentPosition(center); + + if (maxZoom > MaxZoom()) { + maxZoom = MaxZoom(); + } + + if ((int)Zoom() != maxZoom) { + SetZoom(maxZoom); + } + + return true; + } + return false; +} + +void MapGraphicItem::wheelEvent(QGraphicsSceneWheelEvent *event) +{ + if (!IsMouseOverMarker() && !IsDragging()) { + if (core->GetmouseLastZoom().X() != event->pos().x() && core->mouseLastZoom.Y() != event->pos().y()) { + if (GetMouseWheelZoomType() == internals::MouseWheelZoomType::MousePositionAndCenter) { + core->SetCurrentPosition(FromLocalToLatLng(event->pos().x(), event->pos().y())); + } else if (GetMouseWheelZoomType() == internals::MouseWheelZoomType::ViewCenter) { + core->SetCurrentPosition(FromLocalToLatLng((int)maprect.width() / 2, (int)maprect.height() / 2)); + } else if (GetMouseWheelZoomType() == internals::MouseWheelZoomType::MousePositionWithoutCenter) { + core->SetCurrentPosition(FromLocalToLatLng(event->pos().x(), event->pos().y())); + } + + core->mouseLastZoom.SetX((event->pos().x())); + core->mouseLastZoom.SetY((event->pos().y())); + } + + // set mouse position to map center + if (GetMouseWheelZoomType() != internals::MouseWheelZoomType::MousePositionWithoutCenter) { { - core->SettilePoint (core->GetcenterTileXYLocation()); - core->SettilePoint(Point(core->GettilePoint().X()+ i,core->GettilePoint().Y()+j)); - { - internals::Tile* t = core->Matrix.TileAt(core->GettilePoint()); - if(true) - { - core->tileRect.SetX(core->GettilePoint().X()*core->tileRect.Width()); - core->tileRect.SetY(core->GettilePoint().Y()*core->tileRect.Height()); - core->tileRect.Offset(core->GetrenderOffset()); - if(core->GetCurrentRegion().IntersectsWith(core->tileRect)) - { - bool found = false; + // System.Drawing.Point p = PointToScreen(new System.Drawing.Point(Width/2, Height/2)); + // Stuff.SetCursorPos((int) p.X, (int) p.Y); + } + } - // render tile - //lock(t.Overlays) - if(t!=0) - { - foreach(QByteArray img,t->Overlays) - { - if(img.count()!=0) + core->MouseWheelZooming = true; + + if (event->delta() > 0) { + SetZoom(ZoomTotal() + 1); + } else if (event->delta() < 0) { + SetZoom(ZoomTotal() - 1); + } + + core->MouseWheelZooming = false; + } +} +void MapGraphicItem::DrawMap2D(QPainter *painter) +{ + painter->drawImage(this->boundingRect(), dragons.toImage()); + if (!lastimage.isNull()) { + painter->drawImage(core->GetrenderOffset().X() - lastimagepoint.X(), core->GetrenderOffset().Y() - lastimagepoint.Y(), lastimage); + } + + for (int i = -core->GetsizeOfMapArea().Width(); i <= core->GetsizeOfMapArea().Width(); i++) { + for (int j = -core->GetsizeOfMapArea().Height(); j <= core->GetsizeOfMapArea().Height(); j++) { + core->SettilePoint(core->GetcenterTileXYLocation()); + core->SettilePoint(Point(core->GettilePoint().X() + i, core->GettilePoint().Y() + j)); + { + internals::Tile *t = core->Matrix.TileAt(core->GettilePoint()); + if (true) { + core->tileRect.SetX(core->GettilePoint().X() * core->tileRect.Width()); + core->tileRect.SetY(core->GettilePoint().Y() * core->tileRect.Height()); + core->tileRect.Offset(core->GetrenderOffset()); + if (core->GetCurrentRegion().IntersectsWith(core->tileRect)) { + bool found = false; + + // render tile + // lock(t.Overlays) + if (t != 0) { + foreach(QByteArray img, t->Overlays) { + if (img.count() != 0) { + if (!found) { + found = true; + } { - if(!found) - found = true; - { - painter->drawPixmap(core->tileRect.X(),core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height(),PureImageProxy::FromStream(img)); - } + painter->drawPixmap(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height(), PureImageProxy::FromStream(img)); } } } + } - if(showTileGridLines) + if (showTileGridLines) { + painter->setPen(config->EmptyTileBorders); + painter->drawRect(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()); { - painter->setPen(config->EmptyTileBorders); - painter->drawRect(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()); - { - painter->setFont(config->MissingDataFont); - painter->setPen(Qt::red); - painter->drawText(QRectF(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()),Qt::AlignCenter,(core->GettilePoint() == core->GetcenterTileXYLocation()? "CENTER: " :"TILE: ")+core->GettilePoint().ToString()); - } - } - - // add text if tile is missing - if(false) - { - - painter->fillRect(QRectF(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()),config->EmptytileBrush); painter->setFont(config->MissingDataFont); - painter->drawText(QRectF(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()),config->EmptyTileText); - - - - painter->setPen(config->EmptyTileBorders); - painter->drawRect(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()); - - // raise error - - } - if(!SelectedArea().IsEmpty()) - { - core::Point p1 = FromLatLngToLocal(SelectedArea().LocationTopLeft()); - core::Point p2 = FromLatLngToLocal(SelectedArea().LocationRightBottom()); - int x1 = p1.X(); - int y1 = p1.Y(); - int x2 = p2.X(); - int y2 = p2.Y(); - painter->setPen(Qt::black); - painter->setBrush(QBrush(QColor(50,50,100,20))); - painter->drawRect(x1,y1,x2-x1,y2-y1); + painter->setPen(Qt::red); + painter->drawText(QRectF(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()), Qt::AlignCenter, (core->GettilePoint() == core->GetcenterTileXYLocation() ? "CENTER: " : "TILE: ") + core->GettilePoint().ToString()); } } + + // add text if tile is missing + if (false) { + painter->fillRect(QRectF(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()), config->EmptytileBrush); + painter->setFont(config->MissingDataFont); + painter->drawText(QRectF(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()), config->EmptyTileText); + + + painter->setPen(config->EmptyTileBorders); + painter->drawRect(core->tileRect.X(), core->tileRect.Y(), core->tileRect.Width(), core->tileRect.Height()); + + // raise error + } + if (!SelectedArea().IsEmpty()) { + core::Point p1 = FromLatLngToLocal(SelectedArea().LocationTopLeft()); + core::Point p2 = FromLatLngToLocal(SelectedArea().LocationRightBottom()); + int x1 = p1.X(); + int y1 = p1.Y(); + int x2 = p2.X(); + int y2 = p2.Y(); + painter->setPen(Qt::black); + painter->setBrush(QBrush(QColor(50, 50, 100, 20))); + painter->drawRect(x1, y1, x2 - x1, y2 - y1); + } } } } } } +} - core::Point MapGraphicItem::FromLatLngToLocal(internals::PointLatLng const& point) - { - core::Point ret = core->FromLatLngToLocal(point); - if(MapRenderTransform!=1) - { - ret.SetX((int) (ret.X() * MapRenderTransform)); - ret.SetY((int) (ret.Y() * MapRenderTransform)); - ret.SetX(ret.X()-((boundingRect().width()*MapRenderTransform)-(boundingRect().width()))/2); - ret.SetY(ret.Y()-((boundingRect().height()*MapRenderTransform)-(boundingRect().height()))/2); +core::Point MapGraphicItem::FromLatLngToLocal(internals::PointLatLng const & point) +{ + core::Point ret = core->FromLatLngToLocal(point); + if (MapRenderTransform != 1) { + ret.SetX((int)(ret.X() * MapRenderTransform)); + ret.SetY((int)(ret.Y() * MapRenderTransform)); + ret.SetX(ret.X() - ((boundingRect().width() * MapRenderTransform) - (boundingRect().width())) / 2); + ret.SetY(ret.Y() - ((boundingRect().height() * MapRenderTransform) - (boundingRect().height())) / 2); + } + return ret; +} +internals::PointLatLng MapGraphicItem::FromLocalToLatLng(int x, int y) +{ + if (MapRenderTransform != 1) { + x = x + ((boundingRect().width() * MapRenderTransform) - (boundingRect().width())) / 2; + y = y + ((boundingRect().height() * MapRenderTransform) - (boundingRect().height())) / 2; + x = (int)(x / MapRenderTransform); + y = (int)(y / MapRenderTransform); + } + return core->FromLocalToLatLng(x, y); +} + +double MapGraphicItem::Zoom() +{ + return zoomReal; +} +double MapGraphicItem::ZoomDigi() +{ + return zoomDigi; +} +double MapGraphicItem::ZoomTotal() +{ + return zoomDigi + zoomReal; +} + +void MapGraphicItem::SetZoom(double const & value) +{ + if (ZoomTotal() != value) { + if (value > MaxZoom()) { + zoomReal = MaxZoom(); + zoomDigi = value - MaxZoom(); + } else if (value < MinZoom()) { + zoomDigi = 0; + zoomReal = MinZoom(); + } else { + zoomDigi = 0; + zoomReal = value; } - return ret; - } - internals::PointLatLng MapGraphicItem::FromLocalToLatLng(int x, int y) - { - if(MapRenderTransform!=1) - { - x=x+((boundingRect().width()*MapRenderTransform)-(boundingRect().width()))/2; - y=y+((boundingRect().height()*MapRenderTransform)-(boundingRect().height()))/2; - - x = (int) (x / MapRenderTransform); - y = (int) (y / MapRenderTransform); - } - return core->FromLocalToLatLng(x, y); - } - - double MapGraphicItem::Zoom() - { - return zoomReal; - } - double MapGraphicItem::ZoomDigi() - { - return zoomDigi; - } - double MapGraphicItem::ZoomTotal() - { - return zoomDigi+zoomReal; - } - - void MapGraphicItem::SetZoom(double const& value) - { - if(ZoomTotal() != value) - { - if(value > MaxZoom()) + double integer; + double remainder = modf(value, &integer); + if (zoomDigi != 0 || remainder != 0) { + float scaleValue = zoomDigi + remainder + 1; { - zoomReal = MaxZoom(); - zoomDigi =value-MaxZoom(); + MapRenderTransform = scaleValue; } - else - if(value < MinZoom()) - { - zoomDigi=0; - zoomReal = MinZoom(); + if (integer > MaxZoom()) { + integer = MaxZoom(); } - else - { - zoomDigi=0; - zoomReal = value; - } - double integer; - double remainder = modf (value , &integer); - if(zoomDigi!=0||remainder != 0) - { - float scaleValue = zoomDigi+remainder + 1; - { - MapRenderTransform = scaleValue; - } - if(integer>MaxZoom()) - integer=MaxZoom(); - SetZoomStep((qint32)(integer)); - // core->GoToCurrentPositionOnZoom(); - this->update(); + SetZoomStep((qint32)(integer)); + // core->GoToCurrentPositionOnZoom(); + this->update(); + } else { + MapRenderTransform = 1; - } - else - { - - MapRenderTransform = 1; - - SetZoomStep ((qint32)(value)); - zoomReal = ZoomStep(); - this->update(); - } + SetZoomStep((qint32)(value)); + zoomReal = ZoomStep(); + this->update(); } } - int MapGraphicItem::ZoomStep()const - { - return core->Zoom(); - } - void MapGraphicItem::SetZoomStep(int const& value) - { - if(value-core->Zoom()>0 && value<= MaxZoom()) - ConstructLastImage(value-core->Zoom()); - else if(value!=MaxZoom()) - lastimage=QImage(); - if(value > MaxZoom()) - { - core->SetZoom(MaxZoom()); - emit zoomChanged(MaxZoom()+ZoomDigi(),Zoom(),ZoomDigi()); - } - else if(value < MinZoom()) - { - core->SetZoom(MinZoom()); - emit zoomChanged(MinZoom()+ZoomDigi(),Zoom(),ZoomDigi()); - } - else - { - core->SetZoom(value); - emit zoomChanged(value+ZoomDigi(),Zoom(),ZoomDigi());; - } - - } - - void MapGraphicItem::Offset(int const& x, int const& y) - { - core->DragOffset(Point(x, y)); - } - void MapGraphicItem::mapRotate(qreal angle) - { - if (rotation != angle) { - rotation=angle; - resize(scene()->sceneRect()); - } - } - QRectF MapGraphicItem::boundingBox(const QRectF &rect, const qreal &angle) - { - QRectF ret(rect); - float c=cos(angle*2*M_PI/360); - float s=sin(angle*2*M_PI/360); - ret.setHeight(rect.height()*fabs(c)+rect.width()*fabs(s)); - ret.setWidth(rect.width()*fabs(c)+rect.height()*fabs(s)); - return ret; - } - QSize MapGraphicItem::sizeHint()const - { - core::Size size=core->projection->GetTileMatrixMaxXY(MinZoom()); - core::Size tilesize=core->projection->TileSize(); - QSize rsize((size.Width()+1)*tilesize.Width(),(size.Height()+1)*tilesize.Height()); - return rsize; - } +} +int MapGraphicItem::ZoomStep() const +{ + return core->Zoom(); +} +void MapGraphicItem::SetZoomStep(int const & value) +{ + if (value - core->Zoom() > 0 && value <= MaxZoom()) { + ConstructLastImage(value - core->Zoom()); + } else if (value != MaxZoom()) { + lastimage = QImage(); + } + if (value > MaxZoom()) { + core->SetZoom(MaxZoom()); + emit zoomChanged(MaxZoom() + ZoomDigi(), Zoom(), ZoomDigi()); + } else if (value < MinZoom()) { + core->SetZoom(MinZoom()); + emit zoomChanged(MinZoom() + ZoomDigi(), Zoom(), ZoomDigi()); + } else { + core->SetZoom(value); + emit zoomChanged(value + ZoomDigi(), Zoom(), ZoomDigi());; + } +} + +void MapGraphicItem::Offset(int const & x, int const & y) +{ + core->DragOffset(Point(x, y)); +} +void MapGraphicItem::mapRotate(qreal angle) +{ + if (rotation != angle) { + rotation = angle; + resize(scene()->sceneRect()); + } +} +QRectF MapGraphicItem::boundingBox(const QRectF &rect, const qreal &angle) +{ + QRectF ret(rect); + float c = cos(angle * 2 * M_PI / 360); + float s = sin(angle * 2 * M_PI / 360); + + ret.setHeight(rect.height() * fabs(c) + rect.width() * fabs(s)); + ret.setWidth(rect.width() * fabs(c) + rect.height() * fabs(s)); + return ret; +} +QSize MapGraphicItem::sizeHint() const +{ + core::Size size = core->projection->GetTileMatrixMaxXY(MinZoom()); + core::Size tilesize = core->projection->TileSize(); + QSize rsize((size.Width() + 1) * tilesize.Width(), (size.Height() + 1) * tilesize.Height()); + + return rsize; +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h index a87450dd1..e1b2c5d88 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h @@ -1,35 +1,35 @@ /** -****************************************************************************** -* -* @file mapgraphicitem.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief The main graphicsItem used on the widget, contains the map and map logic -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file mapgraphicitem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief The main graphicsItem used on the widget, contains the map and map logic + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef MAPGRAPHICITEM_H #define MAPGRAPHICITEM_H #include #include "../internals/core.h" -//#include "../internals/point.h" +// #include "../internals/point.h" #include "../core/diagnostics.h" #include "configuration.h" #include @@ -39,185 +39,228 @@ #include #include #include "waypointitem.h" -//#include "uavitem.h" +// #include "uavitem.h" -namespace mapcontrol -{ - class WayPointItem; - class OPMapWidget; - /** - * @brief The main graphicsItem used on the widget, contains the map and map logic - * - * @class MapGraphicItem mapgraphicitem.h "mapgraphicitem.h" - */ - class MapGraphicItem:public QObject,public QGraphicsItem - { - friend class mapcontrol::OPMapWidget; - Q_OBJECT +namespace mapcontrol { +class WayPointItem; +class OPMapWidget; +/** + * @brief The main graphicsItem used on the widget, contains the map and map logic + * + * @class MapGraphicItem mapgraphicitem.h "mapgraphicitem.h" + */ +class MapGraphicItem : public QObject, public QGraphicsItem { + friend class mapcontrol::OPMapWidget; + Q_OBJECT Q_INTERFACES(QGraphicsItem) - public: +public: - /** - * @brief Contructer - * - * @param core - * @param configuration the configuration to be used - * @return - */ - MapGraphicItem(internals::Core *core,Configuration *configuration); - QRectF boundingRect() const; - void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, - QWidget *widget); + /** + * @brief Contructer + * + * @param core + * @param configuration the configuration to be used + * @return + */ + MapGraphicItem(internals::Core *core, Configuration *configuration); + QRectF boundingRect() const; + void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, + QWidget *widget); - QSize sizeHint()const; - /** - * @brief Convertes LatLong coordinates to local item coordinates - * - * @param point LatLong point to be converted - * @return core::Point Local item point - */ - core::Point FromLatLngToLocal(internals::PointLatLng const& point); - /** - * @brief Converts from local item coordinates to LatLong point - * - * @param x x local coordinate - * @param y y local coordinate - * @return internals::PointLatLng LatLng coordinate - */ - internals::PointLatLng FromLocalToLatLng(int x, int y); - /** - * @brief Returns true if map is being dragged - * - * @return - */ - bool IsDragging()const{return core->IsDragging();} + QSize sizeHint() const; + /** + * @brief Convertes LatLong coordinates to local item coordinates + * + * @param point LatLong point to be converted + * @return core::Point Local item point + */ + core::Point FromLatLngToLocal(internals::PointLatLng const & point); + /** + * @brief Converts from local item coordinates to LatLong point + * + * @param x x local coordinate + * @param y y local coordinate + * @return internals::PointLatLng LatLng coordinate + */ + internals::PointLatLng FromLocalToLatLng(int x, int y); + /** + * @brief Returns true if map is being dragged + * + * @return + */ + bool IsDragging() const + { + return core->IsDragging(); + } - QImage lastimage; - core::Point lastimagepoint; - void paintImage(QPainter* painter); - void ConstructLastImage(int const& zoomdiff); - internals::PureProjection* Projection()const{return core->Projection();} - double Zoom(); - double ZoomDigi(); - double ZoomTotal(); - void setOverlayOpacity(qreal value); - protected: - void mouseMoveEvent ( QGraphicsSceneMouseEvent * event ); - void mousePressEvent ( QGraphicsSceneMouseEvent * event ); - void wheelEvent ( QGraphicsSceneWheelEvent * event ); - void mouseReleaseEvent(QGraphicsSceneMouseEvent *event); - bool IsMouseOverMarker()const{return isMouseOverMarker;} - void keyPressEvent ( QKeyEvent * event ); - void keyReleaseEvent ( QKeyEvent * event ); + QImage lastimage; + core::Point lastimagepoint; + void paintImage(QPainter *painter); + void ConstructLastImage(int const & zoomdiff); + internals::PureProjection *Projection() const + { + return core->Projection(); + } + double Zoom(); + double ZoomDigi(); + double ZoomTotal(); + void setOverlayOpacity(qreal value); +protected: + void mouseMoveEvent(QGraphicsSceneMouseEvent *event); + void mousePressEvent(QGraphicsSceneMouseEvent *event); + void wheelEvent(QGraphicsSceneWheelEvent *event); + void mouseReleaseEvent(QGraphicsSceneMouseEvent *event); + bool IsMouseOverMarker() const + { + return isMouseOverMarker; + } + void keyPressEvent(QKeyEvent *event); + void keyReleaseEvent(QKeyEvent *event); - /** - * @brief Returns current map zoom - * - * @return int Current map zoom - */ - int ZoomStep()const; - /** - * @brief Sets map zoom - * - * @param value zoom value - */ - void SetZoomStep(int const& value); + /** + * @brief Returns current map zoom + * + * @return int Current map zoom + */ + int ZoomStep() const; + /** + * @brief Sets map zoom + * + * @param value zoom value + */ + void SetZoomStep(int const & value); - /** - * @brief Ask Stacey - * - * @param value - */ - void SetShowDragons(bool const& value); - private: - bool showDragons; - bool SetZoomToFitRect(internals::RectLatLng const& rect); - internals::Core *core; - Configuration *config; - bool showTileGridLines; - qreal MapRenderTransform; - void DrawMap2D(QPainter *painter); - /** - * @brief Maximum possible zoom - * - * @var maxZoom - */ - int maxZoom; - /** - * @brief Minimum possible zoom - * - * @var minZoom - */ - int minZoom; - internals::RectLatLng selectedArea; - internals::PointLatLng selectionStart; - internals::PointLatLng selectionEnd; - double zoomReal; - double zoomDigi; - QRectF maprect; - bool isSelected; - bool isMouseOverMarker; - QPixmap dragons; - void SetIsMouseOverMarker(bool const& value){isMouseOverMarker = value;} + /** + * @brief Ask Stacey + * + * @param value + */ + void SetShowDragons(bool const & value); +private: + bool showDragons; + bool SetZoomToFitRect(internals::RectLatLng const & rect); + internals::Core *core; + Configuration *config; + bool showTileGridLines; + qreal MapRenderTransform; + void DrawMap2D(QPainter *painter); + /** + * @brief Maximum possible zoom + * + * @var maxZoom + */ + int maxZoom; + /** + * @brief Minimum possible zoom + * + * @var minZoom + */ + int minZoom; + internals::RectLatLng selectedArea; + internals::PointLatLng selectionStart; + internals::PointLatLng selectionEnd; + double zoomReal; + double zoomDigi; + QRectF maprect; + bool isSelected; + bool isMouseOverMarker; + QPixmap dragons; + void SetIsMouseOverMarker(bool const & value) + { + isMouseOverMarker = value; + } - qreal rotation; - /** - * @brief Creates a rectangle that represents the "view" of the cuurent map, to compensate - * rotation - * - * @param rect original rectangle - * @param angle angle of rotation - * @return QRectF - */ - QRectF boundingBox(QRectF const& rect, qreal const& angle); - /** - * @brief Returns the maximum allowed zoom - * - * @return int - */ - int MaxZoom()const{return core->MaxZoom();} - /** - * @brief Returns the minimum allowed zoom - * - * @return int - */ - int MinZoom()const{return minZoom;} - internals::MouseWheelZoomType::Types GetMouseWheelZoomType(){return core->GetMouseWheelZoomType();} - void SetSelectedArea(internals::RectLatLng const& value){selectedArea = value;this->update();} - internals::RectLatLng SelectedArea()const{return selectedArea;} - internals::RectLatLng BoundsOfMap; - void Offset(int const& x, int const& y); - bool CanDragMap()const{return core->CanDragMap;} - void SetCanDragMap(bool const& value){core->CanDragMap = value;} + qreal rotation; + /** + * @brief Creates a rectangle that represents the "view" of the cuurent map, to compensate + * rotation + * + * @param rect original rectangle + * @param angle angle of rotation + * @return QRectF + */ + QRectF boundingBox(QRectF const & rect, qreal const & angle); + /** + * @brief Returns the maximum allowed zoom + * + * @return int + */ + int MaxZoom() const + { + return core->MaxZoom(); + } + /** + * @brief Returns the minimum allowed zoom + * + * @return int + */ + int MinZoom() const + { + return minZoom; + } + internals::MouseWheelZoomType::Types GetMouseWheelZoomType() + { + return core->GetMouseWheelZoomType(); + } + void SetSelectedArea(internals::RectLatLng const & value) + { + selectedArea = value; this->update(); + } + internals::RectLatLng SelectedArea() const + { + return selectedArea; + } + internals::RectLatLng BoundsOfMap; + void Offset(int const & x, int const & y); + bool CanDragMap() const + { + return core->CanDragMap; + } + void SetCanDragMap(bool const & value) + { + core->CanDragMap = value; + } - void SetZoom(double const& value); - void mapRotate ( qreal angle ); - void start(); - void ReloadMap(){core->ReloadMap();} - GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const& keys){return core->SetCurrentPositionByKeywords(keys);} - MapType::Types GetMapType(){return core->GetMapType();} - void SetMapType(MapType::Types const& value){core->SetMapType(value);} - private slots: - void Core_OnNeedInvalidation(); - void childPosRefresh(); - public slots: - /** - * @brief To be called when the scene size changes - * - * @param rect - */ - void resize ( QRectF const &rect=QRectF() ); - signals: - /** - * @brief Fired when the current zoom is changed - * - * @param zoom - */ - void wpdoubleclicked(WayPointItem * wp); - void zoomChanged(double zoomtotal,double zoomreal,double zoomdigi); - void childRefreshPosition(); - void childSetOpacity(qreal value); - }; + void SetZoom(double const & value); + void mapRotate(qreal angle); + void start(); + void ReloadMap() + { + core->ReloadMap(); + } + GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const & keys) + { + return core->SetCurrentPositionByKeywords(keys); + } + MapType::Types GetMapType() + { + return core->GetMapType(); + } + void SetMapType(MapType::Types const & value) + { + core->SetMapType(value); + } +private slots: + void Core_OnNeedInvalidation(); + void childPosRefresh(); +public slots: + /** + * @brief To be called when the scene size changes + * + * @param rect + */ + void resize(QRectF const &rect = QRectF()); +signals: + /** + * @brief Fired when the current zoom is changed + * + * @param zoom + */ + void wpdoubleclicked(WayPointItem *wp); + void zoomChanged(double zoomtotal, double zoomreal, double zoomdigi); + void childRefreshPosition(); + void childSetOpacity(qreal value); +}; } #endif // MAPGRAPHICITEM_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.cpp index 78256bd35..955818a7b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.cpp @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file mapripform.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief Form to be used with the MapRipper class -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file mapripform.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Form to be used with the MapRipper class + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "mapripform.h" #include "ui_mapripform.h" @@ -33,7 +33,7 @@ MapRipForm::MapRipForm(QWidget *parent) : ui(new Ui::MapRipForm) { ui->setupUi(this); - connect(ui->cancelButton,SIGNAL(clicked()),this,SIGNAL(cancelRequest())); + connect(ui->cancelButton, SIGNAL(clicked()), this, SIGNAL(cancelRequest())); } MapRipForm::~MapRipForm() @@ -44,7 +44,7 @@ void MapRipForm::SetPercentage(const int &perc) { ui->progressBar->setValue(perc); } -void MapRipForm::SetProvider(const QString &prov,const int &zoom) +void MapRipForm::SetProvider(const QString &prov, const int &zoom) { ui->mainlabel->setText(QString("Currently ripping from:%1 at Zoom level %2").arg(prov).arg(zoom)); } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.h index dc0ceb56c..0912acbdc 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripform.h @@ -1,49 +1,48 @@ /** -****************************************************************************** -* -* @file mapripform.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief Form to be used with the MapRipper class -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file mapripform.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Form to be used with the MapRipper class + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef MAPRIPFORM_H #define MAPRIPFORM_H #include namespace Ui { - class MapRipForm; +class MapRipForm; } -class MapRipForm : public QWidget -{ +class MapRipForm : public QWidget { Q_OBJECT public: explicit MapRipForm(QWidget *parent = 0); ~MapRipForm(); public slots: - void SetPercentage(int const& perc); - void SetProvider(QString const& prov,int const& zoom); - void SetNumberOfTiles(int const& total,int const& actual); + void SetPercentage(int const & perc); + void SetProvider(QString const & prov, int const & zoom); + void SetNumberOfTiles(int const & total, int const & actual); signals: void cancelRequest(); private: diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.cpp index 26778d5cb..59600a21f 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.cpp @@ -1,100 +1,87 @@ /** -****************************************************************************** -* -* @file mapripper.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A class that allows ripping of a selection of the map -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file mapripper.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A class that allows ripping of a selection of the map + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "mapripper.h" -namespace mapcontrol +namespace mapcontrol { +MapRipper::MapRipper(internals::Core *core, const internals::RectLatLng & rect) : sleep(100), cancel(false), progressForm(0), core(core), yesToAll(false) { - -MapRipper::MapRipper(internals::Core * core, const internals::RectLatLng & rect):sleep(100),cancel(false),progressForm(0),core(core),yesToAll(false) - { - if(!rect.IsEmpty()) - { - type=core->GetMapType(); - progressForm=new MapRipForm; - connect(progressForm,SIGNAL(cancelRequest()),this,SLOT(stopFetching())); - area=rect; - zoom=core->Zoom(); - maxzoom=core->MaxZoom(); - points=core->Projection()->GetAreaTileList(area,zoom,0); - this->start(); - cancel=false; - progressForm->show(); - connect(this,SIGNAL(percentageChanged(int)),progressForm,SLOT(SetPercentage(int))); - connect(this,SIGNAL(numberOfTilesChanged(int,int)),progressForm,SLOT(SetNumberOfTiles(int,int))); - connect(this,SIGNAL(providerChanged(QString,int)),progressForm,SLOT(SetProvider(QString,int))); - connect(this,SIGNAL(finished()),this,SLOT(finish())); - emit numberOfTilesChanged(0,0); - } - else + if (!rect.IsEmpty()) { + type = core->GetMapType(); + progressForm = new MapRipForm; + connect(progressForm, SIGNAL(cancelRequest()), this, SLOT(stopFetching())); + area = rect; + zoom = core->Zoom(); + maxzoom = core->MaxZoom(); + points = core->Projection()->GetAreaTileList(area, zoom, 0); + this->start(); + cancel = false; + progressForm->show(); + connect(this, SIGNAL(percentageChanged(int)), progressForm, SLOT(SetPercentage(int))); + connect(this, SIGNAL(numberOfTilesChanged(int, int)), progressForm, SLOT(SetNumberOfTiles(int, int))); + connect(this, SIGNAL(providerChanged(QString, int)), progressForm, SLOT(SetProvider(QString, int))); + connect(this, SIGNAL(finished()), this, SLOT(finish())); + emit numberOfTilesChanged(0, 0); + } else #ifdef Q_OS_DARWIN - QMessageBox::information(new QWidget(),"No valid selection","This pre-caches map data.\n\nPlease first select the area of the map to rip with +Left mouse click"); + { QMessageBox::information(new QWidget(), "No valid selection", "This pre-caches map data.\n\nPlease first select the area of the map to rip with +Left mouse click"); } #else - QMessageBox::information(new QWidget(),"No valid selection","This pre-caches map data.\n\nPlease first select the area of the map to rip with +Left mouse click"); + { QMessageBox::information(new QWidget(), "No valid selection", "This pre-caches map data.\n\nPlease first select the area of the map to rip with +Left mouse click"); } #endif - } +} void MapRipper::finish() { - if(zoomProjection()->GetAreaTileList(area,zoom,0); + points = core->Projection()->GetAreaTileList(area, zoom, 0); this->start(); - } - else if(ret==QMessageBox::YesAll) - { - yesToAll=true; + } else if (ret == QMessageBox::YesAll) { + yesToAll = true; points.clear(); - points=core->Projection()->GetAreaTileList(area,zoom,0); + points = core->Projection()->GetAreaTileList(area, zoom, 0); this->start(); - } - else - { + } else { progressForm->close(); delete progressForm; this->deleteLater(); } - } - else - { - yesToAll=false; + } else { + yesToAll = false; progressForm->close(); delete progressForm; this->deleteLater(); @@ -102,55 +89,53 @@ void MapRipper::finish() } - void MapRipper::run() - { - int countOk = 0; - bool goodtile=false; - // Stuff.Shuffle(ref list); - QVector types = OPMaps::Instance()->GetAllLayersOfType(type); - int all=points.count(); - for(int i = 0; i < all; i++) - { - emit numberOfTilesChanged(all,i+1); - if(cancel) - break; +void MapRipper::run() +{ + int countOk = 0; + bool goodtile = false; - core::Point p = points[i]; - { - //qDebug()<<"offline fetching:"<GetImageFrom(type, p, zoom); - if(img.length()!=0) - { - goodtile=true; - img=NULL; - } - else - goodtile=false; - } - if(goodtile) - { - countOk++; - } - else - { - i--; - QThread::msleep(1000); - continue; + // Stuff.Shuffle(ref list); + QVector types = OPMaps::Instance()->GetAllLayersOfType(type); + int all = points.count(); + for (int i = 0; i < all; i++) { + emit numberOfTilesChanged(all, i + 1); + if (cancel) { + break; + } + + core::Point p = points[i]; + { + // qDebug()<<"offline fetching:"<GetImageFrom(type, p, zoom); + + if (img.length() != 0) { + goodtile = true; + img = NULL; + } else { + goodtile = false; } } - emit percentageChanged((int) ((i+1)*100/all));//, i+1); - // worker.ReportProgress((int) ((i+1)*100/all), i+1); - - QThread::msleep(sleep); + if (goodtile) { + countOk++; + } else { + i--; + QThread::msleep(1000); + continue; + } } - } + emit percentageChanged((int)((i + 1) * 100 / all)); // , i+1); + // worker.ReportProgress((int) ((i+1)*100/all), i+1); - void MapRipper::stopFetching() - { - QMutexLocker locker(&mutex); - cancel=true; + QThread::msleep(sleep); } } + +void MapRipper::stopFetching() +{ + QMutexLocker locker(&mutex); + + cancel = true; +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.h index e6958a1a2..4929a7e93 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapripper.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file mapripper.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A class that allows ripping of a selection of the map -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file mapripper.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A class that allows ripping of a selection of the map + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef MAPRIPPER_H #define MAPRIPPER_H @@ -32,36 +32,34 @@ #include "mapripform.h" #include #include -namespace mapcontrol -{ - class MapRipper:public QThread - { - Q_OBJECT - public: - MapRipper(internals::Core *,internals::RectLatLng const&); - void run(); - private: - QList points; - int zoom; - core::MapType::Types type; - int sleep; - internals::RectLatLng area; - bool cancel; - MapRipForm * progressForm; - int maxzoom; - internals::Core * core; - bool yesToAll; - QMutex mutex; +namespace mapcontrol { +class MapRipper : public QThread { + Q_OBJECT +public: + MapRipper(internals::Core *, internals::RectLatLng const &); + void run(); +private: + QList points; + int zoom; + core::MapType::Types type; + int sleep; + internals::RectLatLng area; + bool cancel; + MapRipForm *progressForm; + int maxzoom; + internals::Core *core; + bool yesToAll; + QMutex mutex; - signals: - void percentageChanged(int const& perc); - void numberOfTilesChanged(int const& total,int const& actual); - void providerChanged(QString const& prov,int const& zoom); +signals: + void percentageChanged(int const & perc); + void numberOfTilesChanged(int const & total, int const & actual); + void providerChanged(QString const & prov, int const & zoom); - public slots: - void stopFetching(); - void finish(); - }; +public slots: + void stopFetching(); + void finish(); +}; } #endif // MAPRIPPER_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp index ea477adde..f9ede780e 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp @@ -1,553 +1,549 @@ /** -****************************************************************************** -* -* @file opmapwidget.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief The Map Widget, this is the part exposed to the user -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file opmapwidget.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief The Map Widget, this is the part exposed to the user + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "opmapwidget.h" #include #include #include "waypointitem.h" -namespace mapcontrol +namespace mapcontrol { +OPMapWidget::OPMapWidget(QWidget *parent, Configuration *config) : QGraphicsView(parent), configuration(config), UAV(0), GPS(0), Home(0) + , followmouse(true), compass(0), showuav(false), showhome(false), diagTimer(0), diagGraphItem(0), showDiag(false), overlayOpacity(1) { - - OPMapWidget::OPMapWidget(QWidget *parent, Configuration *config):QGraphicsView(parent),configuration(config),UAV(0),GPS(0),Home(0) - ,followmouse(true),compass(0),showuav(false),showhome(false),diagTimer(0),diagGraphItem(0),showDiag(false),overlayOpacity(1) - { - setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); - core=new internals::Core; - map=new MapGraphicItem(core,config); - mscene.addItem(map); - this->setScene(&mscene); - Home=new HomeItem(map,this); - Home->setParentItem(map); - Home->setZValue(-1); - setStyleSheet("QToolTip {font-size:8pt; color:blue;opacity: 223; padding:2px; border-width:2px; border-style:solid; border-color: rgb(170, 170, 127);border-radius:4px }"); - this->adjustSize(); - connect(map,SIGNAL(zoomChanged(double,double,double)),this,SIGNAL(zoomChanged(double,double,double))); - connect(map->core,SIGNAL(OnCurrentPositionChanged(internals::PointLatLng)),this,SIGNAL(OnCurrentPositionChanged(internals::PointLatLng))); - connect(map->core,SIGNAL(OnEmptyTileError(int,core::Point)),this,SIGNAL(OnEmptyTileError(int,core::Point))); - connect(map->core,SIGNAL(OnMapDrag()),this,SIGNAL(OnMapDrag())); - connect(map->core,SIGNAL(OnMapTypeChanged(MapType::Types)),this,SIGNAL(OnMapTypeChanged(MapType::Types))); - connect(map->core,SIGNAL(OnMapZoomChanged()),this,SIGNAL(OnMapZoomChanged())); - connect(map->core,SIGNAL(OnTileLoadComplete()),this,SIGNAL(OnTileLoadComplete())); - connect(map->core,SIGNAL(OnTileLoadStart()),this,SIGNAL(OnTileLoadStart())); - connect(map->core,SIGNAL(OnTilesStillToLoad(int)),this,SIGNAL(OnTilesStillToLoad(int))); - connect(map,SIGNAL(wpdoubleclicked(WayPointItem*)),this,SIGNAL(OnWayPointDoubleClicked(WayPointItem*))); - connect(&mscene,SIGNAL(selectionChanged()),this,SLOT(OnSelectionChanged())); - SetShowDiagnostics(showDiag); - this->setMouseTracking(followmouse); - SetShowCompass(true); - QPixmapCache::setCacheLimit(64*1024); - } - void OPMapWidget::SetShowDiagnostics(bool const& value) - { - showDiag=value; - if(!showDiag) - { - if(diagGraphItem!=0) - { - delete diagGraphItem; - diagGraphItem=0; - } - if(diagTimer!=0) - { - delete diagTimer; - diagTimer=0; - } - - if(GPS!=0) - { - GPS->DeleteTrail(); - delete GPS; - GPS=0; - } - } - else - { - diagTimer=new QTimer(); - connect(diagTimer,SIGNAL(timeout()),this,SLOT(diagRefresh())); - diagTimer->start(500); - if(GPS==0) - { - GPS=new GPSItem(map,this); - GPS->setParentItem(map); - GPS->setOpacity(overlayOpacity); - setOverlayOpacity(overlayOpacity); - } - } - - } - void OPMapWidget::SetUavPic(QString UAVPic) - { - if(UAV!=0) - UAV->SetUavPic(UAVPic); - if(GPS!=0) - GPS->SetUavPic(UAVPic); - } - - WayPointLine * OPMapWidget::WPLineCreate(WayPointItem *from, WayPointItem *to,QColor color) - { - if(!from|!to) - return NULL; - WayPointLine* ret= new WayPointLine(from,to,map,color); - ret->setOpacity(overlayOpacity); - return ret; - } - WayPointLine * OPMapWidget::WPLineCreate(HomeItem *from, WayPointItem *to,QColor color) - { - if(!from|!to) - return NULL; - WayPointLine* ret= new WayPointLine(from,to,map,color); - ret->setOpacity(overlayOpacity); - return ret; - } - WayPointCircle * OPMapWidget::WPCircleCreate(WayPointItem *center, WayPointItem *radius, bool clockwise,QColor color) - { - if(!center|!radius) - return NULL; - WayPointCircle* ret= new WayPointCircle(center,radius,clockwise,map,color); - ret->setOpacity(overlayOpacity); - return ret; - } - - WayPointCircle *OPMapWidget::WPCircleCreate(HomeItem *center, WayPointItem *radius, bool clockwise,QColor color) - { - if(!center|!radius) - return NULL; - WayPointCircle* ret= new WayPointCircle(center,radius,clockwise,map,color); - ret->setOpacity(overlayOpacity); - return ret; - } - void OPMapWidget::SetShowUAV(const bool &value) - { - if(value && UAV==0) - { - UAV=new UAVItem(map,this); - UAV->setParentItem(map); - connect(this,SIGNAL(UAVLeftSafetyBouble(internals::PointLatLng)),UAV,SIGNAL(UAVLeftSafetyBouble(internals::PointLatLng))); - connect(this,SIGNAL(UAVReachedWayPoint(int,WayPointItem*)),UAV,SIGNAL(UAVReachedWayPoint(int,WayPointItem*))); - UAV->setOpacity(overlayOpacity); - } - else if(!value) - { - if(UAV!=0) - { - delete UAV; - UAV=NULL; - } - - } - } - void OPMapWidget::SetShowHome(const bool &value) - { - Home->setVisible(value); - } - - void OPMapWidget::resizeEvent(QResizeEvent *event) - { - if (scene()) - scene()->setSceneRect( - QRect(QPoint(0, 0), event->size())); - QGraphicsView::resizeEvent(event); - if(compass) - compass->setScale(0.1+0.05*(qreal)(event->size().width())/1000*(qreal)(event->size().height())/600); - - } - QSize OPMapWidget::sizeHint() const - { - return map->sizeHint(); - } - void OPMapWidget::showEvent(QShowEvent *event) - { - connect(&mscene,SIGNAL(sceneRectChanged(QRectF)),map,SLOT(resize(QRectF))); - map->start(); - QGraphicsView::showEvent(event); - } - OPMapWidget::~OPMapWidget() - { - if(UAV) - delete UAV; - if(Home) - delete Home; - if(map) - delete map; - if(core) - delete core; - if(configuration) - delete configuration; - foreach(QGraphicsItem* i,this->items()) - { - if(i) - delete i; - } - } - void OPMapWidget::closeEvent(QCloseEvent *event) - { - core->OnMapClose(); - event->accept(); - } - void OPMapWidget::SetUseOpenGL(const bool &value) - { - useOpenGL=value; - if (useOpenGL) - setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); - else - setupViewport(new QWidget()); - update(); - } - internals::PointLatLng OPMapWidget::currentMousePosition() - { - return currentmouseposition; - } - - void OPMapWidget::mouseMoveEvent(QMouseEvent *event) - { - QGraphicsView::mouseMoveEvent(event); - QPointF p=event->posF(); - p=map->mapFromParent(p); - currentmouseposition=map->FromLocalToLatLng(p.x(),p.y()); - } - ////////////////WAYPOINT//////////////////////// - WayPointItem* OPMapWidget::WPCreate() - { - WayPointItem* item=new WayPointItem(this->CurrentPosition(),0,map); - ConnectWP(item); - item->setParentItem(map); - int position=item->Number(); - emit WPCreated(position,item); - return item; - } - WayPointItem* OPMapWidget::magicWPCreate() - { - WayPointItem* item=new WayPointItem(map,true); - item->SetShowNumber(false); - item->setParentItem(map); - return item; - } - void OPMapWidget::WPCreate(WayPointItem* item) - { - ConnectWP(item); - item->setParentItem(map); - int position=item->Number(); - emit WPCreated(position,item); - setOverlayOpacity(overlayOpacity); - } - WayPointItem* OPMapWidget::WPCreate(internals::PointLatLng const& coord,int const& altitude) - { - WayPointItem* item=new WayPointItem(coord,altitude,map); - ConnectWP(item); - item->setParentItem(map); - int position=item->Number(); - emit WPCreated(position,item); - setOverlayOpacity(overlayOpacity); - return item; - } - WayPointItem* OPMapWidget::WPCreate(internals::PointLatLng const& coord,int const& altitude, QString const& description) - { - WayPointItem* item=new WayPointItem(coord,altitude,description,map); - ConnectWP(item); - item->setParentItem(map); - int position=item->Number(); - emit WPCreated(position,item); - setOverlayOpacity(overlayOpacity); - return item; - } - WayPointItem* OPMapWidget::WPCreate(const distBearingAltitude &relativeCoord, const QString &description) - { - WayPointItem* item=new WayPointItem(relativeCoord,description,map); - ConnectWP(item); - item->setParentItem(map); - int position=item->Number(); - emit WPCreated(position,item); - setOverlayOpacity(overlayOpacity); - return item; - } - WayPointItem* OPMapWidget::WPInsert(const int &position) - { - WayPointItem* item=new WayPointItem(this->CurrentPosition(),0,map); - item->SetNumber(position); - ConnectWP(item); - item->setParentItem(map); - emit WPInserted(position,item); - setOverlayOpacity(overlayOpacity); - return item; - } - void OPMapWidget::WPInsert(WayPointItem* item,const int &position) - { - item->SetNumber(position); - ConnectWP(item); - item->setParentItem(map); - emit WPInserted(position,item); - setOverlayOpacity(overlayOpacity); - } - WayPointItem* OPMapWidget::WPInsert(internals::PointLatLng const& coord,int const& altitude,const int &position) - { - WayPointItem* item=new WayPointItem(coord,altitude,map); - item->SetNumber(position); - ConnectWP(item); - item->setParentItem(map); - emit WPInserted(position,item); - setOverlayOpacity(overlayOpacity); - return item; - } - WayPointItem* OPMapWidget::WPInsert(internals::PointLatLng const& coord,int const& altitude, QString const& description,const int &position) - { - internals::PointLatLng mcoord; - bool reloc=false; - if(mcoord==internals::PointLatLng(0,0)) - { - mcoord=CurrentPosition(); - reloc=true; - } - else - mcoord=coord; - WayPointItem* item=new WayPointItem(mcoord,altitude,description,map); - item->SetNumber(position); - ConnectWP(item); - item->setParentItem(map); - emit WPInserted(position,item); - if(reloc) - emit WPValuesChanged(item); - setOverlayOpacity(overlayOpacity); - return item; - } - WayPointItem* OPMapWidget::WPInsert(distBearingAltitude const& relative, QString const& description,const int &position) - { - WayPointItem* item=new WayPointItem(relative,description,map); - item->SetNumber(position); - ConnectWP(item); - item->setParentItem(map); - emit WPInserted(position,item); - setOverlayOpacity(overlayOpacity); - return item; - } - void OPMapWidget::WPDelete(WayPointItem *item) - { - emit WPDeleted(item->Number(),item); - delete item; - } - void OPMapWidget::WPDelete(int number) - { - foreach(QGraphicsItem* i,map->childItems()) - { - WayPointItem* w=qgraphicsitem_cast(i); - if(w) - { - if(w->Number()==number) - { - emit WPDeleted(w->Number(),w); - delete w; - return; - } - } - } - } - WayPointItem * OPMapWidget::WPFind(int number) - { - foreach(QGraphicsItem* i,map->childItems()) - { - WayPointItem* w=qgraphicsitem_cast(i); - if(w) - { - if(w->Number()==number) - { - return w; - } - } - } - return NULL; - } - void OPMapWidget::WPSetVisibleAll(bool value) - { - foreach(QGraphicsItem* i,map->childItems()) - { - WayPointItem* w=qgraphicsitem_cast(i); - if(w) - { - if(w->Number()!=-1) - w->setVisible(value); - } - } - } - void OPMapWidget::WPDeleteAll() - { - foreach(QGraphicsItem* i,map->childItems()) - { - WayPointItem* w=qgraphicsitem_cast(i); - if(w) - { - if(w->Number()!=-1) - { - emit WPDeleted(w->Number(),w); - delete w; - } - } - } - } - bool OPMapWidget::WPPresent() - { - foreach(QGraphicsItem* i,map->childItems()) - { - WayPointItem* w=qgraphicsitem_cast(i); - if(w) - { - if(w->Number()!=-1) - { - return true; - } - } - } - return false; - } - - void OPMapWidget::deleteAllOverlays() - { - foreach(QGraphicsItem* i,map->childItems()) - { - WayPointLine* w=qgraphicsitem_cast(i); - if(w) - w->deleteLater(); - else - { - WayPointCircle* ww=qgraphicsitem_cast(i); - if(ww) - ww->deleteLater(); - } - } - } - QList OPMapWidget::WPSelected() - { - QList list; - foreach(QGraphicsItem* i,mscene.selectedItems()) - { - WayPointItem* w=qgraphicsitem_cast(i); - if(w) - list.append(w); - } - return list; - } - void OPMapWidget::WPRenumber(WayPointItem *item, const int &newnumber) - { - item->SetNumber(newnumber); - } - - void OPMapWidget::ConnectWP(WayPointItem *item) - { - connect(item,SIGNAL(WPNumberChanged(int,int,WayPointItem*)),this,SIGNAL(WPNumberChanged(int,int,WayPointItem*)),Qt::DirectConnection); - connect(item,SIGNAL(WPValuesChanged(WayPointItem*)),this,SIGNAL(WPValuesChanged(WayPointItem*)),Qt::DirectConnection); - connect(item,SIGNAL(localPositionChanged(QPointF,WayPointItem*)),this,SIGNAL(WPLocalPositionChanged(QPointF,WayPointItem*)),Qt::DirectConnection); - connect(item,SIGNAL(manualCoordChange(WayPointItem*)),this,SIGNAL(WPManualCoordChange(WayPointItem*)),Qt::DirectConnection); - connect(this,SIGNAL(WPInserted(int,WayPointItem*)),item,SLOT(WPInserted(int,WayPointItem*)),Qt::DirectConnection); - connect(this,SIGNAL(WPNumberChanged(int,int,WayPointItem*)),item,SLOT(WPRenumbered(int,int,WayPointItem*)),Qt::DirectConnection); - connect(this,SIGNAL(WPDeleted(int,WayPointItem*)),item,SLOT(WPDeleted(int,WayPointItem*)),Qt::DirectConnection); - } - void OPMapWidget::diagRefresh() - { - if(showDiag) - { - if(diagGraphItem==0) - { - diagGraphItem=new QGraphicsTextItem(); - mscene.addItem(diagGraphItem); - diagGraphItem->setPos(10,100); - diagGraphItem->setZValue(3); - diagGraphItem->setFlag(QGraphicsItem::ItemIsMovable,true); - diagGraphItem->setDefaultTextColor(Qt::yellow); - } - diagGraphItem->setPlainText(core->GetDiagnostics().toString()); - } - else - if(diagGraphItem!=0) - { + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); + core = new internals::Core; + map = new MapGraphicItem(core, config); + mscene.addItem(map); + this->setScene(&mscene); + Home = new HomeItem(map, this); + Home->setParentItem(map); + Home->setZValue(-1); + setStyleSheet("QToolTip {font-size:8pt; color:blue;opacity: 223; padding:2px; border-width:2px; border-style:solid; border-color: rgb(170, 170, 127);border-radius:4px }"); + this->adjustSize(); + connect(map, SIGNAL(zoomChanged(double, double, double)), this, SIGNAL(zoomChanged(double, double, double))); + connect(map->core, SIGNAL(OnCurrentPositionChanged(internals::PointLatLng)), this, SIGNAL(OnCurrentPositionChanged(internals::PointLatLng))); + connect(map->core, SIGNAL(OnEmptyTileError(int, core::Point)), this, SIGNAL(OnEmptyTileError(int, core::Point))); + connect(map->core, SIGNAL(OnMapDrag()), this, SIGNAL(OnMapDrag())); + connect(map->core, SIGNAL(OnMapTypeChanged(MapType::Types)), this, SIGNAL(OnMapTypeChanged(MapType::Types))); + connect(map->core, SIGNAL(OnMapZoomChanged()), this, SIGNAL(OnMapZoomChanged())); + connect(map->core, SIGNAL(OnTileLoadComplete()), this, SIGNAL(OnTileLoadComplete())); + connect(map->core, SIGNAL(OnTileLoadStart()), this, SIGNAL(OnTileLoadStart())); + connect(map->core, SIGNAL(OnTilesStillToLoad(int)), this, SIGNAL(OnTilesStillToLoad(int))); + connect(map, SIGNAL(wpdoubleclicked(WayPointItem *)), this, SIGNAL(OnWayPointDoubleClicked(WayPointItem *))); + connect(&mscene, SIGNAL(selectionChanged()), this, SLOT(OnSelectionChanged())); + SetShowDiagnostics(showDiag); + this->setMouseTracking(followmouse); + SetShowCompass(true); + QPixmapCache::setCacheLimit(64 * 1024); +} +void OPMapWidget::SetShowDiagnostics(bool const & value) +{ + showDiag = value; + if (!showDiag) { + if (diagGraphItem != 0) { delete diagGraphItem; - diagGraphItem=0; + diagGraphItem = 0; + } + if (diagTimer != 0) { + delete diagTimer; + diagTimer = 0; } - } - ////////////////////////////////////////////// - void OPMapWidget::SetShowCompass(const bool &value) - { - if(value && !compass) - { - compass=new QGraphicsSvgItem(QString::fromUtf8(":/markers/images/compas.svg")); - compass->setScale(0.1+0.05*(qreal)(this->size().width())/1000*(qreal)(this->size().height())/600); - // compass->setTransformOriginPoint(compass->boundingRect().width(),compass->boundingRect().height()); - compass->setFlag(QGraphicsItem::ItemIsMovable,true); - compass->setFlag(QGraphicsItem::ItemIsSelectable,true); - mscene.addItem(compass); - compass->setTransformOriginPoint(compass->boundingRect().width()/2,compass->boundingRect().height()/2); - compass->setPos(55-compass->boundingRect().width()/2,55-compass->boundingRect().height()/2); - compass->setZValue(3); - compass->setOpacity(0.7); + if (GPS != 0) { + GPS->DeleteTrail(); + delete GPS; + GPS = 0; } - if(!value && compass) - { - delete compass; - compass=0; + } else { + diagTimer = new QTimer(); + connect(diagTimer, SIGNAL(timeout()), this, SLOT(diagRefresh())); + diagTimer->start(500); + if (GPS == 0) { + GPS = new GPSItem(map, this); + GPS->setParentItem(map); + GPS->setOpacity(overlayOpacity); + setOverlayOpacity(overlayOpacity); } } - - void OPMapWidget::setOverlayOpacity(qreal value) - { - map->setOverlayOpacity(value); - overlayOpacity=value; - } - void OPMapWidget::SetRotate(qreal const& value) - { - map->mapRotate(value); - if(compass && (compass->rotation() != value)) { - compass->setRotation(value); - } - } - void OPMapWidget::RipMap() - { - new MapRipper(core,map->SelectedArea()); - } - - void OPMapWidget::setSelectedWP(QListlist) - { - this->scene()->clearSelection(); - foreach(WayPointItem * wp,list) - { - wp->setSelected(true); - } - } - - void OPMapWidget::OnSelectionChanged() - { - QList list; - QList wplist; - list=this->scene()->selectedItems(); - foreach(QGraphicsItem* item,list) - { - WayPointItem * wp=qgraphicsitem_cast(item); - if(wp) - wplist.append(wp); - } - if(wplist.length()>0) - emit selectedWPChanged(wplist); - } +} +void OPMapWidget::SetUavPic(QString UAVPic) +{ + if (UAV != 0) { + UAV->SetUavPic(UAVPic); + } + if (GPS != 0) { + GPS->SetUavPic(UAVPic); + } +} + +WayPointLine *OPMapWidget::WPLineCreate(WayPointItem *from, WayPointItem *to, QColor color) +{ + if (!from | !to) { + return NULL; + } + WayPointLine *ret = new WayPointLine(from, to, map, color); + ret->setOpacity(overlayOpacity); + return ret; +} +WayPointLine *OPMapWidget::WPLineCreate(HomeItem *from, WayPointItem *to, QColor color) +{ + if (!from | !to) { + return NULL; + } + WayPointLine *ret = new WayPointLine(from, to, map, color); + ret->setOpacity(overlayOpacity); + return ret; +} +WayPointCircle *OPMapWidget::WPCircleCreate(WayPointItem *center, WayPointItem *radius, bool clockwise, QColor color) +{ + if (!center | !radius) { + return NULL; + } + WayPointCircle *ret = new WayPointCircle(center, radius, clockwise, map, color); + ret->setOpacity(overlayOpacity); + return ret; +} + +WayPointCircle *OPMapWidget::WPCircleCreate(HomeItem *center, WayPointItem *radius, bool clockwise, QColor color) +{ + if (!center | !radius) { + return NULL; + } + WayPointCircle *ret = new WayPointCircle(center, radius, clockwise, map, color); + ret->setOpacity(overlayOpacity); + return ret; +} +void OPMapWidget::SetShowUAV(const bool &value) +{ + if (value && UAV == 0) { + UAV = new UAVItem(map, this); + UAV->setParentItem(map); + connect(this, SIGNAL(UAVLeftSafetyBouble(internals::PointLatLng)), UAV, SIGNAL(UAVLeftSafetyBouble(internals::PointLatLng))); + connect(this, SIGNAL(UAVReachedWayPoint(int, WayPointItem *)), UAV, SIGNAL(UAVReachedWayPoint(int, WayPointItem *))); + UAV->setOpacity(overlayOpacity); + } else if (!value) { + if (UAV != 0) { + delete UAV; + UAV = NULL; + } + } +} +void OPMapWidget::SetShowHome(const bool &value) +{ + Home->setVisible(value); +} + +void OPMapWidget::resizeEvent(QResizeEvent *event) +{ + if (scene()) { + scene()->setSceneRect( + QRect(QPoint(0, 0), event->size())); + } + QGraphicsView::resizeEvent(event); + if (compass) { + compass->setScale(0.1 + 0.05 * (qreal)(event->size().width()) / 1000 * (qreal)(event->size().height()) / 600); + } +} +QSize OPMapWidget::sizeHint() const +{ + return map->sizeHint(); +} +void OPMapWidget::showEvent(QShowEvent *event) +{ + connect(&mscene, SIGNAL(sceneRectChanged(QRectF)), map, SLOT(resize(QRectF))); + map->start(); + QGraphicsView::showEvent(event); +} +OPMapWidget::~OPMapWidget() +{ + if (UAV) { + delete UAV; + } + if (Home) { + delete Home; + } + if (map) { + delete map; + } + if (core) { + delete core; + } + if (configuration) { + delete configuration; + } + foreach(QGraphicsItem * i, this->items()) { + if (i) { + delete i; + } + } +} +void OPMapWidget::closeEvent(QCloseEvent *event) +{ + core->OnMapClose(); + event->accept(); +} +void OPMapWidget::SetUseOpenGL(const bool &value) +{ + useOpenGL = value; + if (useOpenGL) { + setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); + } else { + setupViewport(new QWidget()); + } + update(); +} +internals::PointLatLng OPMapWidget::currentMousePosition() +{ + return currentmouseposition; +} + +void OPMapWidget::mouseMoveEvent(QMouseEvent *event) +{ + QGraphicsView::mouseMoveEvent(event); + QPointF p = event->posF(); + + p = map->mapFromParent(p); + currentmouseposition = map->FromLocalToLatLng(p.x(), p.y()); +} +////////////////WAYPOINT//////////////////////// +WayPointItem *OPMapWidget::WPCreate() +{ + WayPointItem *item = new WayPointItem(this->CurrentPosition(), 0, map); + + ConnectWP(item); + item->setParentItem(map); + int position = item->Number(); + emit WPCreated(position, item); + return item; +} +WayPointItem *OPMapWidget::magicWPCreate() +{ + WayPointItem *item = new WayPointItem(map, true); + + item->SetShowNumber(false); + item->setParentItem(map); + return item; +} +void OPMapWidget::WPCreate(WayPointItem *item) +{ + ConnectWP(item); + item->setParentItem(map); + int position = item->Number(); + emit WPCreated(position, item); + setOverlayOpacity(overlayOpacity); +} +WayPointItem *OPMapWidget::WPCreate(internals::PointLatLng const & coord, int const & altitude) +{ + WayPointItem *item = new WayPointItem(coord, altitude, map); + + ConnectWP(item); + item->setParentItem(map); + int position = item->Number(); + emit WPCreated(position, item); + setOverlayOpacity(overlayOpacity); + return item; +} +WayPointItem *OPMapWidget::WPCreate(internals::PointLatLng const & coord, int const & altitude, QString const & description) +{ + WayPointItem *item = new WayPointItem(coord, altitude, description, map); + + ConnectWP(item); + item->setParentItem(map); + int position = item->Number(); + emit WPCreated(position, item); + setOverlayOpacity(overlayOpacity); + return item; +} +WayPointItem *OPMapWidget::WPCreate(const distBearingAltitude &relativeCoord, const QString &description) +{ + WayPointItem *item = new WayPointItem(relativeCoord, description, map); + + ConnectWP(item); + item->setParentItem(map); + int position = item->Number(); + emit WPCreated(position, item); + setOverlayOpacity(overlayOpacity); + return item; +} +WayPointItem *OPMapWidget::WPInsert(const int &position) +{ + WayPointItem *item = new WayPointItem(this->CurrentPosition(), 0, map); + + item->SetNumber(position); + ConnectWP(item); + item->setParentItem(map); + emit WPInserted(position, item); + setOverlayOpacity(overlayOpacity); + return item; +} +void OPMapWidget::WPInsert(WayPointItem *item, const int &position) +{ + item->SetNumber(position); + ConnectWP(item); + item->setParentItem(map); + emit WPInserted(position, item); + setOverlayOpacity(overlayOpacity); +} +WayPointItem *OPMapWidget::WPInsert(internals::PointLatLng const & coord, int const & altitude, const int &position) +{ + WayPointItem *item = new WayPointItem(coord, altitude, map); + + item->SetNumber(position); + ConnectWP(item); + item->setParentItem(map); + emit WPInserted(position, item); + setOverlayOpacity(overlayOpacity); + return item; +} +WayPointItem *OPMapWidget::WPInsert(internals::PointLatLng const & coord, int const & altitude, QString const & description, const int &position) +{ + internals::PointLatLng mcoord; + bool reloc = false; + + if (mcoord == internals::PointLatLng(0, 0)) { + mcoord = CurrentPosition(); + reloc = true; + } else { + mcoord = coord; + } + WayPointItem *item = new WayPointItem(mcoord, altitude, description, map); + item->SetNumber(position); + ConnectWP(item); + item->setParentItem(map); + emit WPInserted(position, item); + if (reloc) { + emit WPValuesChanged(item); + } + setOverlayOpacity(overlayOpacity); + return item; +} +WayPointItem *OPMapWidget::WPInsert(distBearingAltitude const & relative, QString const & description, const int &position) +{ + WayPointItem *item = new WayPointItem(relative, description, map); + + item->SetNumber(position); + ConnectWP(item); + item->setParentItem(map); + emit WPInserted(position, item); + setOverlayOpacity(overlayOpacity); + return item; +} +void OPMapWidget::WPDelete(WayPointItem *item) +{ + emit WPDeleted(item->Number(), item); + + delete item; +} +void OPMapWidget::WPDelete(int number) +{ + foreach(QGraphicsItem * i, map->childItems()) { + WayPointItem *w = qgraphicsitem_cast(i); + + if (w) { + if (w->Number() == number) { + emit WPDeleted(w->Number(), w); + delete w; + return; + } + } + } +} +WayPointItem *OPMapWidget::WPFind(int number) +{ + foreach(QGraphicsItem * i, map->childItems()) { + WayPointItem *w = qgraphicsitem_cast(i); + + if (w) { + if (w->Number() == number) { + return w; + } + } + } + return NULL; +} +void OPMapWidget::WPSetVisibleAll(bool value) +{ + foreach(QGraphicsItem * i, map->childItems()) { + WayPointItem *w = qgraphicsitem_cast(i); + + if (w) { + if (w->Number() != -1) { + w->setVisible(value); + } + } + } +} +void OPMapWidget::WPDeleteAll() +{ + foreach(QGraphicsItem * i, map->childItems()) { + WayPointItem *w = qgraphicsitem_cast(i); + + if (w) { + if (w->Number() != -1) { + emit WPDeleted(w->Number(), w); + delete w; + } + } + } +} +bool OPMapWidget::WPPresent() +{ + foreach(QGraphicsItem * i, map->childItems()) { + WayPointItem *w = qgraphicsitem_cast(i); + + if (w) { + if (w->Number() != -1) { + return true; + } + } + } + return false; +} + +void OPMapWidget::deleteAllOverlays() +{ + foreach(QGraphicsItem * i, map->childItems()) { + WayPointLine *w = qgraphicsitem_cast(i); + + if (w) { + w->deleteLater(); + } else { + WayPointCircle *ww = qgraphicsitem_cast(i); + if (ww) { + ww->deleteLater(); + } + } + } +} +QList OPMapWidget::WPSelected() +{ + QList list; + foreach(QGraphicsItem * i, mscene.selectedItems()) { + WayPointItem *w = qgraphicsitem_cast(i); + + if (w) { + list.append(w); + } + } + return list; +} +void OPMapWidget::WPRenumber(WayPointItem *item, const int &newnumber) +{ + item->SetNumber(newnumber); +} + +void OPMapWidget::ConnectWP(WayPointItem *item) +{ + connect(item, SIGNAL(WPNumberChanged(int, int, WayPointItem *)), this, SIGNAL(WPNumberChanged(int, int, WayPointItem *)), Qt::DirectConnection); + connect(item, SIGNAL(WPValuesChanged(WayPointItem *)), this, SIGNAL(WPValuesChanged(WayPointItem *)), Qt::DirectConnection); + connect(item, SIGNAL(localPositionChanged(QPointF, WayPointItem *)), this, SIGNAL(WPLocalPositionChanged(QPointF, WayPointItem *)), Qt::DirectConnection); + connect(item, SIGNAL(manualCoordChange(WayPointItem *)), this, SIGNAL(WPManualCoordChange(WayPointItem *)), Qt::DirectConnection); + connect(this, SIGNAL(WPInserted(int, WayPointItem *)), item, SLOT(WPInserted(int, WayPointItem *)), Qt::DirectConnection); + connect(this, SIGNAL(WPNumberChanged(int, int, WayPointItem *)), item, SLOT(WPRenumbered(int, int, WayPointItem *)), Qt::DirectConnection); + connect(this, SIGNAL(WPDeleted(int, WayPointItem *)), item, SLOT(WPDeleted(int, WayPointItem *)), Qt::DirectConnection); +} +void OPMapWidget::diagRefresh() +{ + if (showDiag) { + if (diagGraphItem == 0) { + diagGraphItem = new QGraphicsTextItem(); + mscene.addItem(diagGraphItem); + diagGraphItem->setPos(10, 100); + diagGraphItem->setZValue(3); + diagGraphItem->setFlag(QGraphicsItem::ItemIsMovable, true); + diagGraphItem->setDefaultTextColor(Qt::yellow); + } + diagGraphItem->setPlainText(core->GetDiagnostics().toString()); + } else if (diagGraphItem != 0) { + delete diagGraphItem; + diagGraphItem = 0; + } +} + +////////////////////////////////////////////// +void OPMapWidget::SetShowCompass(const bool &value) +{ + if (value && !compass) { + compass = new QGraphicsSvgItem(QString::fromUtf8(":/markers/images/compas.svg")); + compass->setScale(0.1 + 0.05 * (qreal)(this->size().width()) / 1000 * (qreal)(this->size().height()) / 600); + // compass->setTransformOriginPoint(compass->boundingRect().width(),compass->boundingRect().height()); + compass->setFlag(QGraphicsItem::ItemIsMovable, true); + compass->setFlag(QGraphicsItem::ItemIsSelectable, true); + mscene.addItem(compass); + compass->setTransformOriginPoint(compass->boundingRect().width() / 2, compass->boundingRect().height() / 2); + compass->setPos(55 - compass->boundingRect().width() / 2, 55 - compass->boundingRect().height() / 2); + compass->setZValue(3); + compass->setOpacity(0.7); + } + if (!value && compass) { + delete compass; + compass = 0; + } +} + +void OPMapWidget::setOverlayOpacity(qreal value) +{ + map->setOverlayOpacity(value); + overlayOpacity = value; +} +void OPMapWidget::SetRotate(qreal const & value) +{ + map->mapRotate(value); + if (compass && (compass->rotation() != value)) { + compass->setRotation(value); + } +} +void OPMapWidget::RipMap() +{ + new MapRipper(core, map->SelectedArea()); +} + +void OPMapWidget::setSelectedWP(QListlist) +{ + this->scene()->clearSelection(); + foreach(WayPointItem * wp, list) { + wp->setSelected(true); + } +} + +void OPMapWidget::OnSelectionChanged() +{ + QList list; + QList wplist; + list = this->scene()->selectedItems(); + foreach(QGraphicsItem * item, list) { + WayPointItem *wp = qgraphicsitem_cast(item); + + if (wp) { + wplist.append(wp); + } + } + if (wplist.length() > 0) { + emit selectedWPChanged(wplist); + } +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h index 020f3831b..f427a560c 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file opmapwidget.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief The Map Widget, this is the part exposed to the user -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file opmapwidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief The Map Widget, this is the part exposed to the user + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef OPMAPWIDGET_H #define OPMAPWIDGET_H @@ -44,473 +44,618 @@ #include "waypointline.h" #include "waypointcircle.h" #include "waypointitem.h" -namespace mapcontrol -{ - class UAVItem; - class GPSItem; - class HomeItem; +namespace mapcontrol { +class UAVItem; +class GPSItem; +class HomeItem; +/** + * @brief Collection of static functions to help dealing with various enums used + * Contains functions for enumToString conversio, StringToEnum, QStringList of enum values... + * + * @class Helper opmapwidget.h "opmapwidget.h" + */ +class Helper { +public: /** - * @brief Collection of static functions to help dealing with various enums used - * Contains functions for enumToString conversio, StringToEnum, QStringList of enum values... - * - * @class Helper opmapwidget.h "opmapwidget.h" - */ - class Helper + * @brief Converts from String to Type + * + * @param value String to convert + * @return + */ + static MapType::Types MapTypeFromString(QString const & value) { - public: - /** - * @brief Converts from String to Type - * - * @param value String to convert - * @return - */ - static MapType::Types MapTypeFromString(QString const& value){return MapType::TypeByStr(value);} - /** - * @brief Converts from Type to String - */ - static QString StrFromMapType(MapType::Types const& value){return MapType::StrByType(value);} - /** - * @brief Returns QStringList with string representing all the enum values - */ - static QStringList MapTypes(){return MapType::TypesList();} - - /** - * @brief Converts from String to Type - */ - static GeoCoderStatusCode::Types GeoCoderStatusCodeFromString(QString const& value){return GeoCoderStatusCode::TypeByStr(value);} - /** - * @brief Converts from Type to String - */ - static QString StrFromGeoCoderStatusCode(GeoCoderStatusCode::Types const& value){return GeoCoderStatusCode::StrByType(value);} - /** - * @brief Returns QStringList with string representing all the enum values - */ - static QStringList GeoCoderTypes(){return GeoCoderStatusCode::TypesList();} - - /** - * @brief Converts from String to Type - */ - static internals::MouseWheelZoomType::Types MouseWheelZoomTypeFromString(QString const& value){return internals::MouseWheelZoomType::TypeByStr(value);} - /** - * @brief Converts from Type to String - */ - static QString StrFromMouseWheelZoomType(internals::MouseWheelZoomType::Types const& value){return internals::MouseWheelZoomType::StrByType(value);} - /** - * @brief Returns QStringList with string representing all the enum values - */ - static QStringList MouseWheelZoomTypes(){return internals::MouseWheelZoomType::TypesList();} - /** - * @brief Converts from String to Type - */ - static core::LanguageType::Types LanguageTypeFromString(QString const& value){return core::LanguageType::TypeByStr(value);} - /** - * @brief Converts from Type to String - */ - static QString StrFromLanguageType(core::LanguageType::Types const& value){return core::LanguageType::StrByType(value);} - /** - * @brief Returns QStringList with string representing all the enum values - */ - static QStringList LanguageTypes(){return core::LanguageType::TypesList();} - /** - * @brief Converts from String to Type - */ - static core::AccessMode::Types AccessModeFromString(QString const& value){return core::AccessMode::TypeByStr(value);} - /** - * @brief Converts from Type to String - */ - static QString StrFromAccessMode(core::AccessMode::Types const& value){return core::AccessMode::StrByType(value);} - /** - * @brief Returns QStringList with string representing all the enum values - */ - static QStringList AccessModeTypes(){return core::AccessMode::TypesList();} - - /** - * @brief Converts from String to Type - */ - static UAVMapFollowType::Types UAVMapFollowFromString(QString const& value){return UAVMapFollowType::TypeByStr(value);} - /** - * @brief Converts from Type to String - */ - static QString StrFromUAVMapFollow(UAVMapFollowType::Types const& value){return UAVMapFollowType::StrByType(value);} - /** - * @brief Returns QStringList with string representing all the enum values - */ - static QStringList UAVMapFollowTypes(){return UAVMapFollowType::TypesList();} - /** - * @brief Converts from String to Type - */ - static UAVTrailType::Types UAVTrailTypeFromString(QString const& value){return UAVTrailType::TypeByStr(value);} - /** - * @brief Converts from Type to String - */ - static QString StrFromUAVTrailType(UAVTrailType::Types const& value){return UAVTrailType::StrByType(value);} - /** - * @brief Returns QStringList with string representing all the enum values - */ - static QStringList UAVTrailTypes(){return UAVTrailType::TypesList();} - }; - - class OPMapWidget:public QGraphicsView + return MapType::TypeByStr(value); + } + /** + * @brief Converts from Type to String + */ + static QString StrFromMapType(MapType::Types const & value) { - Q_OBJECT + return MapType::StrByType(value); + } + /** + * @brief Returns QStringList with string representing all the enum values + */ + static QStringList MapTypes() + { + return MapType::TypesList(); + } - // Q_PROPERTY(int MaxZoom READ MaxZoom WRITE SetMaxZoom) - Q_PROPERTY(int MinZoom READ MinZoom WRITE SetMinZoom) - Q_PROPERTY(bool ShowTileGridLines READ ShowTileGridLines WRITE SetShowTileGridLines) - Q_PROPERTY(double Zoom READ ZoomTotal WRITE SetZoom) - Q_PROPERTY(qreal Rotate READ Rotate WRITE SetRotate) - Q_ENUMS(internals::MouseWheelZoomType::Types) - Q_ENUMS(internals::GeoCoderStatusCode::Types) + /** + * @brief Converts from String to Type + */ + static GeoCoderStatusCode::Types GeoCoderStatusCodeFromString(QString const & value) + { + return GeoCoderStatusCode::TypeByStr(value); + } + /** + * @brief Converts from Type to String + */ + static QString StrFromGeoCoderStatusCode(GeoCoderStatusCode::Types const & value) + { + return GeoCoderStatusCode::StrByType(value); + } + /** + * @brief Returns QStringList with string representing all the enum values + */ + static QStringList GeoCoderTypes() + { + return GeoCoderStatusCode::TypesList(); + } - public: - QSize sizeHint() const; - /** - * @brief Constructor - * - * @param parent parent widget - * @param config pointer to configuration classed to be used - * @return - */ - OPMapWidget(QWidget *parent=0,Configuration *config=new Configuration); - ~OPMapWidget(); + /** + * @brief Converts from String to Type + */ + static internals::MouseWheelZoomType::Types MouseWheelZoomTypeFromString(QString const & value) + { + return internals::MouseWheelZoomType::TypeByStr(value); + } + /** + * @brief Converts from Type to String + */ + static QString StrFromMouseWheelZoomType(internals::MouseWheelZoomType::Types const & value) + { + return internals::MouseWheelZoomType::StrByType(value); + } + /** + * @brief Returns QStringList with string representing all the enum values + */ + static QStringList MouseWheelZoomTypes() + { + return internals::MouseWheelZoomType::TypesList(); + } + /** + * @brief Converts from String to Type + */ + static core::LanguageType::Types LanguageTypeFromString(QString const & value) + { + return core::LanguageType::TypeByStr(value); + } + /** + * @brief Converts from Type to String + */ + static QString StrFromLanguageType(core::LanguageType::Types const & value) + { + return core::LanguageType::StrByType(value); + } + /** + * @brief Returns QStringList with string representing all the enum values + */ + static QStringList LanguageTypes() + { + return core::LanguageType::TypesList(); + } + /** + * @brief Converts from String to Type + */ + static core::AccessMode::Types AccessModeFromString(QString const & value) + { + return core::AccessMode::TypeByStr(value); + } + /** + * @brief Converts from Type to String + */ + static QString StrFromAccessMode(core::AccessMode::Types const & value) + { + return core::AccessMode::StrByType(value); + } + /** + * @brief Returns QStringList with string representing all the enum values + */ + static QStringList AccessModeTypes() + { + return core::AccessMode::TypesList(); + } - /** - * @brief Returns true if map is showing gridlines - * - * @return bool - */ - bool ShowTileGridLines()const {return map->showTileGridLines;} + /** + * @brief Converts from String to Type + */ + static UAVMapFollowType::Types UAVMapFollowFromString(QString const & value) + { + return UAVMapFollowType::TypeByStr(value); + } + /** + * @brief Converts from Type to String + */ + static QString StrFromUAVMapFollow(UAVMapFollowType::Types const & value) + { + return UAVMapFollowType::StrByType(value); + } + /** + * @brief Returns QStringList with string representing all the enum values + */ + static QStringList UAVMapFollowTypes() + { + return UAVMapFollowType::TypesList(); + } + /** + * @brief Converts from String to Type + */ + static UAVTrailType::Types UAVTrailTypeFromString(QString const & value) + { + return UAVTrailType::TypeByStr(value); + } + /** + * @brief Converts from Type to String + */ + static QString StrFromUAVTrailType(UAVTrailType::Types const & value) + { + return UAVTrailType::StrByType(value); + } + /** + * @brief Returns QStringList with string representing all the enum values + */ + static QStringList UAVTrailTypes() + { + return UAVTrailType::TypesList(); + } +}; - /** - * @brief Defines if map is to show gridlines - * - * @param value - * @return - */ - void SetShowTileGridLines(bool const& value){map->showTileGridLines=value;map->update();} +class OPMapWidget : public QGraphicsView { + Q_OBJECT + // Q_PROPERTY(int MaxZoom READ MaxZoom WRITE SetMaxZoom) + Q_PROPERTY(int MinZoom READ MinZoom WRITE SetMinZoom) + Q_PROPERTY(bool ShowTileGridLines READ ShowTileGridLines WRITE SetShowTileGridLines) + Q_PROPERTY(double Zoom READ ZoomTotal WRITE SetZoom) + Q_PROPERTY(qreal Rotate READ Rotate WRITE SetRotate) + Q_ENUMS(internals::MouseWheelZoomType::Types) + Q_ENUMS(internals::GeoCoderStatusCode::Types) - /** - * @brief Returns the maximum zoom for the map - * - */ - int MaxZoom()const{return map->MaxZoom();} +public: + QSize sizeHint() const; + /** + * @brief Constructor + * + * @param parent parent widget + * @param config pointer to configuration classed to be used + * @return + */ + OPMapWidget(QWidget *parent = 0, Configuration *config = new Configuration); + ~OPMapWidget(); - // void SetMaxZoom(int const& value){map->maxZoom = value;} + /** + * @brief Returns true if map is showing gridlines + * + * @return bool + */ + bool ShowTileGridLines() const + { + return map->showTileGridLines; + } - /** - * @brief - * - */ - int MinZoom()const{return map->minZoom;} - /** - * @brief - * - * @param value - */ - void SetMinZoom(int const& value){map->minZoom = value;} + /** + * @brief Defines if map is to show gridlines + * + * @param value + * @return + */ + void SetShowTileGridLines(bool const & value) + { + map->showTileGridLines = value; map->update(); + } - internals::MouseWheelZoomType::Types GetMouseWheelZoomType(){return map->core->GetMouseWheelZoomType();} - void SetMouseWheelZoomType(internals::MouseWheelZoomType::Types const& value){map->core->SetMouseWheelZoomType(value);} - // void SetMouseWheelZoomTypeByStr(const QString &value){map->core->SetMouseWheelZoomType(internals::MouseWheelZoomType::TypeByStr(value));} - // QString GetMouseWheelZoomTypeStr(){return map->GetMouseWheelZoomTypeStr();} + /** + * @brief Returns the maximum zoom for the map + * + */ + int MaxZoom() const + { + return map->MaxZoom(); + } - internals::RectLatLng SelectedArea()const{return map->selectedArea;} - void SetSelectedArea(internals::RectLatLng const& value){ map->selectedArea = value;this->update();} + // void SetMaxZoom(int const& value){map->maxZoom = value;} - bool CanDragMap()const{return map->CanDragMap();} - void SetCanDragMap(bool const& value){map->SetCanDragMap(value);} + /** + * @brief + * + */ + int MinZoom() const + { + return map->minZoom; + } + /** + * @brief + * + * @param value + */ + void SetMinZoom(int const & value) + { + map->minZoom = value; + } - internals::PointLatLng CurrentPosition()const{return map->core->CurrentPosition();} - void SetCurrentPosition(internals::PointLatLng const& value){map->core->SetCurrentPosition(value);} + internals::MouseWheelZoomType::Types GetMouseWheelZoomType() + { + return map->core->GetMouseWheelZoomType(); + } + void SetMouseWheelZoomType(internals::MouseWheelZoomType::Types const & value) + { + map->core->SetMouseWheelZoomType(value); + } + // void SetMouseWheelZoomTypeByStr(const QString &value){map->core->SetMouseWheelZoomType(internals::MouseWheelZoomType::TypeByStr(value));} + // QString GetMouseWheelZoomTypeStr(){return map->GetMouseWheelZoomTypeStr();} - double ZoomReal(){return map->Zoom();} - double ZoomDigi(){return map->ZoomDigi();} - double ZoomTotal(){return map->ZoomTotal();} - void SetZoom(double const& value){map->SetZoom(value);} + internals::RectLatLng SelectedArea() const + { + return map->selectedArea; + } + void SetSelectedArea(internals::RectLatLng const & value) + { + map->selectedArea = value; this->update(); + } - qreal Rotate(){return map->rotation;} - void SetRotate(qreal const& value); + bool CanDragMap() const + { + return map->CanDragMap(); + } + void SetCanDragMap(bool const & value) + { + map->SetCanDragMap(value); + } - void ReloadMap(){map->ReloadMap(); map->resize();} + internals::PointLatLng CurrentPosition() const + { + return map->core->CurrentPosition(); + } + void SetCurrentPosition(internals::PointLatLng const & value) + { + map->core->SetCurrentPosition(value); + } - GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const& keys){return map->SetCurrentPositionByKeywords(keys);} + double ZoomReal() + { + return map->Zoom(); + } + double ZoomDigi() + { + return map->ZoomDigi(); + } + double ZoomTotal() + { + return map->ZoomTotal(); + } + void SetZoom(double const & value) + { + map->SetZoom(value); + } - bool UseOpenGL(){return useOpenGL;} - void SetUseOpenGL(bool const& value); + qreal Rotate() + { + return map->rotation; + } + void SetRotate(qreal const & value); - MapType::Types GetMapType(){return map->core->GetMapType();} - void SetMapType(MapType::Types const& value){map->lastimage=QImage(); map->core->SetMapType(value);} + void ReloadMap() + { + map->ReloadMap(); map->resize(); + } - bool isStarted(){return map->core->isStarted();} + GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const & keys) + { + return map->SetCurrentPositionByKeywords(keys); + } - Configuration* configuration; + bool UseOpenGL() + { + return useOpenGL; + } + void SetUseOpenGL(bool const & value); - internals::PointLatLng currentMousePosition(); + MapType::Types GetMapType() + { + return map->core->GetMapType(); + } + void SetMapType(MapType::Types const & value) + { + map->lastimage = QImage(); map->core->SetMapType(value); + } - void SetFollowMouse(bool const& value){followmouse=value;this->setMouseTracking(followmouse);} - bool FollowMouse(){return followmouse;} + bool isStarted() + { + return map->core->isStarted(); + } - internals::PointLatLng GetFromLocalToLatLng(QPointF p) {return map->FromLocalToLatLng(p.x(),p.y());} + Configuration *configuration; - /** - * @brief Creates a new WayPoint on the center of the map - * - * @return WayPointItem a pointer to the WayPoint created - */ - WayPointItem* WPCreate(); - /** - * @brief Creates a new WayPoint - * - * @param item the WayPoint to create - */ - void WPCreate(WayPointItem* item); - /** - * @brief Creates a new WayPoint - * - * @param coord the coordinates in LatLng of the WayPoint - * @param altitude the Altitude of the WayPoint - * @return WayPointItem a pointer to the WayPoint created - */ - WayPointItem* WPCreate(internals::PointLatLng const& coord,int const& altitude); - /** - * @brief Creates a new WayPoint - * - * @param coord the coordinates in LatLng of the WayPoint - * @param altitude the Altitude of the WayPoint - * @param description the description of the WayPoint - * @return WayPointItem a pointer to the WayPoint created - */ - WayPointItem* WPCreate(internals::PointLatLng const& coord,int const& altitude, QString const& description); - /** - * @brief Creates a new WayPoint - * - * @param coord the offset in meters to the home position - * @param altitude the Altitude of the WayPoint - * @param description the description of the WayPoint - * @return WayPointItem a pointer to the WayPoint created - */ - WayPointItem *WPCreate(const distBearingAltitude &relativeCoord, const QString &description); - /** - * @brief Inserts a new WayPoint on the specified position - * - * @param position index of the WayPoint - * @return WayPointItem a pointer to the WayPoint created - */ - WayPointItem* WPInsert(int const& position); - /** - * @brief Inserts a new WayPoint on the specified position - * - * @param item the WayPoint to Insert - * @param position index of the WayPoint - */ - void WPInsert(WayPointItem* item,int const& position); - /** - * @brief Inserts a new WayPoint on the specified position - * - * @param coord the coordinates in LatLng of the WayPoint - * @param altitude the Altitude of the WayPoint - * @param position index of the WayPoint - * @return WayPointItem a pointer to the WayPoint Inserted - */ - WayPointItem* WPInsert(internals::PointLatLng const& coord,int const& altitude,int const& position); - /** - * @brief Inserts a new WayPoint on the specified position - * - * @param coord the coordinates in LatLng of the WayPoint - * @param altitude the Altitude of the WayPoint - * @param description the description of the WayPoint - * @param position index of the WayPoint - * @return WayPointItem a pointer to the WayPoint Inserted - */ - WayPointItem* WPInsert(internals::PointLatLng const& coord,int const& altitude, QString const& description,int const& position); - WayPointItem *WPInsert(const distBearingAltitude &relative, const QString &description, const int &position); + internals::PointLatLng currentMousePosition(); - /** - * @brief Deletes the WayPoint - * - * @param item the WayPoint to delete - */ - void WPDelete(WayPointItem* item); - /** - * @brief deletes all WayPoints - * - */ - void WPDeleteAll(); - /** - * @brief Returns the currently selected WayPoints - * - * @return @return QList - */ - QList WPSelected(); + void SetFollowMouse(bool const & value) + { + followmouse = value; this->setMouseTracking(followmouse); + } + bool FollowMouse() + { + return followmouse; + } - /** - * @brief Renumbers the WayPoint and all others as needed - * - * @param item the WayPoint to renumber - * @param newnumber the WayPoint's new number - */ - void WPRenumber(WayPointItem* item,int const& newnumber); + internals::PointLatLng GetFromLocalToLatLng(QPointF p) + { + return map->FromLocalToLatLng(p.x(), p.y()); + } - void SetShowCompass(bool const& value); + /** + * @brief Creates a new WayPoint on the center of the map + * + * @return WayPointItem a pointer to the WayPoint created + */ + WayPointItem *WPCreate(); + /** + * @brief Creates a new WayPoint + * + * @param item the WayPoint to create + */ + void WPCreate(WayPointItem *item); + /** + * @brief Creates a new WayPoint + * + * @param coord the coordinates in LatLng of the WayPoint + * @param altitude the Altitude of the WayPoint + * @return WayPointItem a pointer to the WayPoint created + */ + WayPointItem *WPCreate(internals::PointLatLng const & coord, int const & altitude); + /** + * @brief Creates a new WayPoint + * + * @param coord the coordinates in LatLng of the WayPoint + * @param altitude the Altitude of the WayPoint + * @param description the description of the WayPoint + * @return WayPointItem a pointer to the WayPoint created + */ + WayPointItem *WPCreate(internals::PointLatLng const & coord, int const & altitude, QString const & description); + /** + * @brief Creates a new WayPoint + * + * @param coord the offset in meters to the home position + * @param altitude the Altitude of the WayPoint + * @param description the description of the WayPoint + * @return WayPointItem a pointer to the WayPoint created + */ + WayPointItem *WPCreate(const distBearingAltitude &relativeCoord, const QString &description); + /** + * @brief Inserts a new WayPoint on the specified position + * + * @param position index of the WayPoint + * @return WayPointItem a pointer to the WayPoint created + */ + WayPointItem *WPInsert(int const & position); + /** + * @brief Inserts a new WayPoint on the specified position + * + * @param item the WayPoint to Insert + * @param position index of the WayPoint + */ + void WPInsert(WayPointItem *item, int const & position); + /** + * @brief Inserts a new WayPoint on the specified position + * + * @param coord the coordinates in LatLng of the WayPoint + * @param altitude the Altitude of the WayPoint + * @param position index of the WayPoint + * @return WayPointItem a pointer to the WayPoint Inserted + */ + WayPointItem *WPInsert(internals::PointLatLng const & coord, int const & altitude, int const & position); + /** + * @brief Inserts a new WayPoint on the specified position + * + * @param coord the coordinates in LatLng of the WayPoint + * @param altitude the Altitude of the WayPoint + * @param description the description of the WayPoint + * @param position index of the WayPoint + * @return WayPointItem a pointer to the WayPoint Inserted + */ + WayPointItem *WPInsert(internals::PointLatLng const & coord, int const & altitude, QString const & description, int const & position); + WayPointItem *WPInsert(const distBearingAltitude &relative, const QString &description, const int &position); - void setOverlayOpacity(qreal value); + /** + * @brief Deletes the WayPoint + * + * @param item the WayPoint to delete + */ + void WPDelete(WayPointItem *item); + /** + * @brief deletes all WayPoints + * + */ + void WPDeleteAll(); + /** + * @brief Returns the currently selected WayPoints + * + * @return @return QList + */ + QList WPSelected(); - UAVItem* UAV; - GPSItem* GPS; - HomeItem* Home; - void SetShowUAV(bool const& value); - bool ShowUAV()const{return showuav;} - void SetShowHome(bool const& value); - bool ShowHome()const{return showhome;} - void SetShowDiagnostics(bool const& value); - void SetUavPic(QString UAVPic); - WayPointLine * WPLineCreate(WayPointItem *from,WayPointItem *to, QColor color); - WayPointLine * WPLineCreate(HomeItem *from,WayPointItem *to, QColor color); - WayPointCircle *WPCircleCreate(WayPointItem *center, WayPointItem *radius,bool clockwise,QColor color); - WayPointCircle *WPCircleCreate(HomeItem *center, WayPointItem *radius,bool clockwise,QColor color); - void deleteAllOverlays(); - void WPSetVisibleAll(bool value); - WayPointItem *magicWPCreate(); - bool WPPresent(); - void WPDelete(int number); - WayPointItem *WPFind(int number); - void setSelectedWP(QList list); - private: - internals::Core *core; - MapGraphicItem *map; - QGraphicsScene mscene; - bool useOpenGL; - GeoCoderStatusCode x; - MapType y; - core::AccessMode xx; - internals::PointLatLng currentmouseposition; - bool followmouse; - void ConnectWP(WayPointItem* item); - QGraphicsSvgItem *compass; - bool showuav; - bool showhome; - QTimer * diagTimer; - QGraphicsTextItem * diagGraphItem; - bool showDiag; - qreal overlayOpacity; - private slots: - void diagRefresh(); - // WayPointItem* item;//apagar - protected: - void resizeEvent(QResizeEvent *event); - void showEvent ( QShowEvent * event ); - void closeEvent(QCloseEvent *event); - void mouseMoveEvent ( QMouseEvent * event ); - // private slots: - signals: - void zoomChanged(double zoomt,double zoom, double zoomd); - /** - * @brief fires when one of the WayPoints numbers changes (not fired if due to a auto-renumbering) - * - * @param oldnumber WayPoint old number - * @param newnumber WayPoint new number - * @param waypoint a pointer to the WayPoint that was renumbered - */ - void WPNumberChanged(int const& oldnumber,int const& newnumber,WayPointItem* waypoint); - /** - * @brief Fired when the description, altitude or coordinates of a WayPoint changed - * - * @param waypoint a pointer to the WayPoint - */ - void WPValuesChanged(WayPointItem* waypoint); - /** - * @brief Fires when a new WayPoint is inserted - * - * @param number new WayPoint number - * @param waypoint WayPoint inserted - */ - void WPReached(WayPointItem* waypoint); + /** + * @brief Renumbers the WayPoint and all others as needed + * + * @param item the WayPoint to renumber + * @param newnumber the WayPoint's new number + */ + void WPRenumber(WayPointItem *item, int const & newnumber); - void WPCreated(int const& number,WayPointItem* waypoint); + void SetShowCompass(bool const & value); - /** - * @brief Fires when a new WayPoint is inserted - * - * @param number new WayPoint number - * @param waypoint WayPoint inserted - */ - void WPInserted(int const& number,WayPointItem* waypoint); - /** - * @brief Fires When a WayPoint is deleted - * - * @param number number of the deleted WayPoint - */ - void WPDeleted(int const& number,WayPointItem* waypoint); + void setOverlayOpacity(qreal value); - void WPLocalPositionChanged(QPointF,WayPointItem*); - void WPManualCoordChange(WayPointItem*); - /** - * @brief Fires When a WayPoint is Reached - * - * @param number number of the Reached WayPoint - */ - void UAVReachedWayPoint(int const& waypointnumber,WayPointItem* waypoint); - /** - * @brief Fires When the UAV lives the safety bouble - * - * @param position the position of the UAV - */ - void UAVLeftSafetyBouble(internals::PointLatLng const& position); + UAVItem *UAV; + GPSItem *GPS; + HomeItem *Home; + void SetShowUAV(bool const & value); + bool ShowUAV() const + { + return showuav; + } + void SetShowHome(bool const & value); + bool ShowHome() const + { + return showhome; + } + void SetShowDiagnostics(bool const & value); + void SetUavPic(QString UAVPic); + WayPointLine *WPLineCreate(WayPointItem *from, WayPointItem *to, QColor color); + WayPointLine *WPLineCreate(HomeItem *from, WayPointItem *to, QColor color); + WayPointCircle *WPCircleCreate(WayPointItem *center, WayPointItem *radius, bool clockwise, QColor color); + WayPointCircle *WPCircleCreate(HomeItem *center, WayPointItem *radius, bool clockwise, QColor color); + void deleteAllOverlays(); + void WPSetVisibleAll(bool value); + WayPointItem *magicWPCreate(); + bool WPPresent(); + void WPDelete(int number); + WayPointItem *WPFind(int number); + void setSelectedWP(QList list); +private: + internals::Core *core; + MapGraphicItem *map; + QGraphicsScene mscene; + bool useOpenGL; + GeoCoderStatusCode x; + MapType y; + core::AccessMode xx; + internals::PointLatLng currentmouseposition; + bool followmouse; + void ConnectWP(WayPointItem *item); + QGraphicsSvgItem *compass; + bool showuav; + bool showhome; + QTimer *diagTimer; + QGraphicsTextItem *diagGraphItem; + bool showDiag; + qreal overlayOpacity; +private slots: + void diagRefresh(); + // WayPointItem* item;//apagar +protected: + void resizeEvent(QResizeEvent *event); + void showEvent(QShowEvent *event); + void closeEvent(QCloseEvent *event); + void mouseMoveEvent(QMouseEvent *event); + // private slots: +signals: + void zoomChanged(double zoomt, double zoom, double zoomd); + /** + * @brief fires when one of the WayPoints numbers changes (not fired if due to a auto-renumbering) + * + * @param oldnumber WayPoint old number + * @param newnumber WayPoint new number + * @param waypoint a pointer to the WayPoint that was renumbered + */ + void WPNumberChanged(int const & oldnumber, int const & newnumber, WayPointItem *waypoint); + /** + * @brief Fired when the description, altitude or coordinates of a WayPoint changed + * + * @param waypoint a pointer to the WayPoint + */ + void WPValuesChanged(WayPointItem *waypoint); + /** + * @brief Fires when a new WayPoint is inserted + * + * @param number new WayPoint number + * @param waypoint WayPoint inserted + */ + void WPReached(WayPointItem *waypoint); - /** - * @brief Fires when map position changes - * - * @param point the point in LatLng of the new center of the map - */ - void OnCurrentPositionChanged(internals::PointLatLng point); - /** - * @brief Fires when there are no more tiles to load - * - */ - void OnTileLoadComplete(); - /** - * @brief Fires when tiles loading begins - * - */ - void OnTileLoadStart(); - /** - * @brief Fires when the map is dragged - * - */ - void OnMapDrag(); - /** - * @brief Fires when map zoom changes - * - */ - void OnMapZoomChanged(); - /** - * @brief Fires when map type changes - * - * @param type The maps new type - */ - void OnMapTypeChanged(MapType::Types type); - /** - * @brief Fires when an error ocurred while loading a tile - * - * @param zoom tile zoom - * @param pos tile position - */ - void OnEmptyTileError(int zoom, core::Point pos); - /** - * @brief Fires when the number of tiles in the load queue changes - * - * @param number the number of tiles still in the queue - */ - void OnTilesStillToLoad(int number); - void OnWayPointDoubleClicked(WayPointItem * waypoint); - void selectedWPChanged(QList); - public slots: - /** - * @brief Ripps the current selection to the DB - */ - void RipMap(); - void OnSelectionChanged(); + void WPCreated(int const & number, WayPointItem *waypoint); - }; + /** + * @brief Fires when a new WayPoint is inserted + * + * @param number new WayPoint number + * @param waypoint WayPoint inserted + */ + void WPInserted(int const & number, WayPointItem *waypoint); + /** + * @brief Fires When a WayPoint is deleted + * + * @param number number of the deleted WayPoint + */ + void WPDeleted(int const & number, WayPointItem *waypoint); + + void WPLocalPositionChanged(QPointF, WayPointItem *); + void WPManualCoordChange(WayPointItem *); + /** + * @brief Fires When a WayPoint is Reached + * + * @param number number of the Reached WayPoint + */ + void UAVReachedWayPoint(int const & waypointnumber, WayPointItem *waypoint); + /** + * @brief Fires When the UAV lives the safety bouble + * + * @param position the position of the UAV + */ + void UAVLeftSafetyBouble(internals::PointLatLng const & position); + + /** + * @brief Fires when map position changes + * + * @param point the point in LatLng of the new center of the map + */ + void OnCurrentPositionChanged(internals::PointLatLng point); + /** + * @brief Fires when there are no more tiles to load + * + */ + void OnTileLoadComplete(); + /** + * @brief Fires when tiles loading begins + * + */ + void OnTileLoadStart(); + /** + * @brief Fires when the map is dragged + * + */ + void OnMapDrag(); + /** + * @brief Fires when map zoom changes + * + */ + void OnMapZoomChanged(); + /** + * @brief Fires when map type changes + * + * @param type The maps new type + */ + void OnMapTypeChanged(MapType::Types type); + /** + * @brief Fires when an error ocurred while loading a tile + * + * @param zoom tile zoom + * @param pos tile position + */ + void OnEmptyTileError(int zoom, core::Point pos); + /** + * @brief Fires when the number of tiles in the load queue changes + * + * @param number the number of tiles still in the queue + */ + void OnTilesStillToLoad(int number); + void OnWayPointDoubleClicked(WayPointItem *waypoint); + void selectedWPChanged(QList); +public slots: + /** + * @brief Ripps the current selection to the DB + */ + void RipMap(); + void OnSelectionChanged(); +}; } #endif // OPMAPWIDGET_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.cpp index 69b2dbbd2..866cfc842 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.cpp @@ -1,61 +1,61 @@ /** -****************************************************************************** -* -* @file trailitem.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a trail point -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file trailitem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a trail point + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "trailitem.h" #include -namespace mapcontrol +namespace mapcontrol { +TrailItem::TrailItem(internals::PointLatLng const & coord, int const & altitude, QBrush color, MapGraphicItem *map) : QGraphicsItem(map), coord(coord), m_brush(color), m_map(map) { -TrailItem::TrailItem(internals::PointLatLng const& coord,int const& altitude, QBrush color, MapGraphicItem *map):QGraphicsItem(map),coord(coord),m_brush(color),m_map(map) - { - QDateTime time=QDateTime::currentDateTime(); - QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); - setToolTip(QString(tr("Position:")+"%1\n"+tr("Altitude:")+"%2\n"+tr("Time:")+"%3").arg(coord_str).arg(QString::number(altitude)).arg(time.toString())); - } + QDateTime time = QDateTime::currentDateTime(); + QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); - void TrailItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) - { - Q_UNUSED(option); - Q_UNUSED(widget); - - painter->setBrush(m_brush); - painter->drawEllipse(-2,-2,4,4); - } - QRectF TrailItem::boundingRect()const - { - return QRectF(-2,-2,4,4); - } - - - int TrailItem::type()const - { - return Type; - } - - void TrailItem::setPosSLOT() - { - setPos(m_map->FromLatLngToLocal(this->coord).X(),m_map->FromLatLngToLocal(this->coord).Y()); - } + setToolTip(QString(tr("Position:") + "%1\n" + tr("Altitude:") + "%2\n" + tr("Time:") + "%3").arg(coord_str).arg(QString::number(altitude)).arg(time.toString())); +} + +void TrailItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) +{ + Q_UNUSED(option); + Q_UNUSED(widget); + + painter->setBrush(m_brush); + painter->drawEllipse(-2, -2, 4, 4); +} +QRectF TrailItem::boundingRect() const +{ + return QRectF(-2, -2, 4, 4); +} + + +int TrailItem::type() const +{ + return Type; +} + +void TrailItem::setPosSLOT() +{ + setPos(m_map->FromLatLngToLocal(this->coord).X(), m_map->FromLatLngToLocal(this->coord).Y()); +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.h index bfc58b97d..e3a8a4b23 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file trailitem.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a WayPoint -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file trailitem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef TRAILITEM_H #define TRAILITEM_H @@ -34,30 +34,23 @@ #include #include "mapgraphicitem.h" -namespace mapcontrol -{ - - class TrailItem:public QObject,public QGraphicsItem - { - Q_OBJECT - Q_INTERFACES(QGraphicsItem) - public: - enum { Type = UserType + 3 }; - TrailItem(internals::PointLatLng const& coord,int const& altitude, QBrush color,MapGraphicItem * map); - void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, - QWidget *widget); - QRectF boundingRect() const; - int type() const; - internals::PointLatLng coord; - private: - QBrush m_brush; - MapGraphicItem * m_map; - public slots: - void setPosSLOT(); - signals: - - }; +namespace mapcontrol { +class TrailItem : public QObject, public QGraphicsItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) +public: + enum { Type = UserType + 3 }; + TrailItem(internals::PointLatLng const & coord, int const & altitude, QBrush color, MapGraphicItem *map); + void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, + QWidget *widget); + QRectF boundingRect() const; + int type() const; + internals::PointLatLng coord; +private: + QBrush m_brush; + MapGraphicItem *m_map; +public slots: + void setPosSLOT(); +signals: +}; } #endif // TRAILITEM_H - - diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.cpp index 7ad63109e..0e5a9dcd9 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.cpp @@ -1,48 +1,48 @@ /** -****************************************************************************** -* -* @file trailitem.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a trail point -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file trailitem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a trail point + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "traillineitem.h" -namespace mapcontrol +namespace mapcontrol { +TrailLineItem::TrailLineItem(internals::PointLatLng const & coord1, internals::PointLatLng const & coord2, QBrush color, MapGraphicItem *map) : QGraphicsLineItem(map), coord1(coord1), coord2(coord2), m_brush(color), m_map(map) { -TrailLineItem::TrailLineItem(internals::PointLatLng const& coord1,internals::PointLatLng const& coord2, QBrush color, MapGraphicItem * map):QGraphicsLineItem(map),coord1(coord1),coord2(coord2),m_brush(color),m_map(map) - { - QPen pen; - pen.setBrush(m_brush); - pen.setWidth(1); - this->setPen(pen); - } + QPen pen; - int TrailLineItem::type()const - { - return Type; - } - - void TrailLineItem::setLineSlot() - { - setLine(m_map->FromLatLngToLocal(this->coord1).X(),m_map->FromLatLngToLocal(this->coord1).Y(),m_map->FromLatLngToLocal(this->coord2).X(),m_map->FromLatLngToLocal(this->coord2).Y()); - } + pen.setBrush(m_brush); + pen.setWidth(1); + this->setPen(pen); +} + +int TrailLineItem::type() const +{ + return Type; +} + +void TrailLineItem::setLineSlot() +{ + setLine(m_map->FromLatLngToLocal(this->coord1).X(), m_map->FromLatLngToLocal(this->coord1).Y(), m_map->FromLatLngToLocal(this->coord2).X(), m_map->FromLatLngToLocal(this->coord2).Y()); +} } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.h index db195108f..c61412014 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/traillineitem.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file traillineitem.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a WayPoint -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file traillineitem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef TAILLINEITEM_H #define TAILLINEITEM_H @@ -34,27 +34,22 @@ #include #include "mapgraphicitem.h" -namespace mapcontrol -{ - - class TrailLineItem:public QObject,public QGraphicsLineItem - { - Q_OBJECT - Q_INTERFACES(QGraphicsItem) - public: - enum { Type = UserType + 7 }; - TrailLineItem(internals::PointLatLng const& coord1,internals::PointLatLng const& coord2, QBrush color,MapGraphicItem * map); - int type() const; - internals::PointLatLng coord1; - internals::PointLatLng coord2; - private: - QBrush m_brush; - MapGraphicItem * m_map; - public slots: - void setLineSlot(); - signals: - - }; +namespace mapcontrol { +class TrailLineItem : public QObject, public QGraphicsLineItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) +public: + enum { Type = UserType + 7 }; + TrailLineItem(internals::PointLatLng const & coord1, internals::PointLatLng const & coord2, QBrush color, MapGraphicItem *map); + int type() const; + internals::PointLatLng coord1; + internals::PointLatLng coord2; +private: + QBrush m_brush; + MapGraphicItem *m_map; +public slots: + void setLineSlot(); +signals: +}; } #endif // TAILLINEITEM_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp index 6fc3d237b..58190c735 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp @@ -1,452 +1,430 @@ /** -****************************************************************************** -* -* @file uavitem.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a UAV -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file uavitem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a UAV + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "../internals/pureprojection.h" #include "uavitem.h" #include -namespace mapcontrol +namespace mapcontrol { +double UAVItem::groundspeed_mps_filt = 0; + +UAVItem::UAVItem(MapGraphicItem *map, OPMapWidget *parent, QString uavPic) : map(map), mapwidget(parent), showtrail(true), showtrailline(true), trailtime(5), traildistance(50), autosetreached(true) + , autosetdistance(100), altitude(0), showUAVInfo(false) { + pic.load(uavPic); + this->setFlag(QGraphicsItem::ItemIsMovable, false); + this->setFlag(QGraphicsItem::ItemIsSelectable, false); + localposition = map->FromLatLngToLocal(mapwidget->CurrentPosition()); + this->setPos(localposition.X(), localposition.Y()); + this->setZValue(4); + trail = new QGraphicsItemGroup(this); + trail->setParentItem(map); + trailLine = new QGraphicsItemGroup(this); + trailLine->setParentItem(map); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + setCacheMode(QGraphicsItem::ItemCoordinateCache); + mapfollowtype = UAVMapFollowType::None; + trailtype = UAVTrailType::ByDistance; + timer.start(); + generateArrowhead(); + double pixels2meters = map->Projection()->GetGroundResolution(map->ZoomTotal(), coord.Lat()); + meters2pixels = 1.0 / pixels2meters; + setCacheMode(QGraphicsItem::DeviceCoordinateCache); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); + connect(map, SIGNAL(zoomChanged(double, double, double)), this, SLOT(zoomChangedSlot())); +} +UAVItem::~UAVItem() +{} - double UAVItem::groundspeed_mps_filt = 0; +void UAVItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) +{ + Q_UNUSED(option); + Q_UNUSED(widget); - UAVItem::UAVItem(MapGraphicItem* map,OPMapWidget* parent,QString uavPic):map(map),mapwidget(parent),showtrail(true),showtrailline(true),trailtime(5),traildistance(50),autosetreached(true) - ,autosetdistance(100),altitude(0),showUAVInfo(false) - { - pic.load(uavPic); - this->setFlag(QGraphicsItem::ItemIsMovable,false); - this->setFlag(QGraphicsItem::ItemIsSelectable,false); - localposition=map->FromLatLngToLocal(mapwidget->CurrentPosition()); - this->setPos(localposition.X(),localposition.Y()); - this->setZValue(4); - trail=new QGraphicsItemGroup(this); - trail->setParentItem(map); - trailLine=new QGraphicsItemGroup(this); - trailLine->setParentItem(map); - this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); - setCacheMode(QGraphicsItem::ItemCoordinateCache); - mapfollowtype=UAVMapFollowType::None; - trailtype=UAVTrailType::ByDistance; - timer.start(); - generateArrowhead(); - double pixels2meters = map->Projection()->GetGroundResolution(map->ZoomTotal(),coord.Lat()); - meters2pixels=1.0 / pixels2meters; - setCacheMode(QGraphicsItem::DeviceCoordinateCache); - connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); - connect(map,SIGNAL(zoomChanged(double,double,double)),this,SLOT(zoomChangedSlot())); - } - UAVItem::~UAVItem() - { + // Draw plane + painter->drawPixmap(-pic.width() / 2, -pic.height() / 2, pic); + // Return if UAV Info context menu is turned off + if (!showUAVInfo) { + return; } - void UAVItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) - { - Q_UNUSED(option); - Q_UNUSED(widget); + QPen myPen; - //Draw plane - painter->drawPixmap(-pic.width()/2,-pic.height()/2,pic); + // Turn on anti-aliasing so the fonts don't look terrible + painter->setRenderHint(QPainter::Antialiasing, true); - //Return if UAV Info context menu is turned off - if(!showUAVInfo){ - return; - } + // Set pen attributes + QColor myColor(Qt::red); + myPen.setWidth(3); + myPen.setColor(myColor); + painter->setPen(myPen); + painter->drawPolygon(arrowHead); + painter->setPen(myPen); + painter->drawLine(arrowShaft); - QPen myPen; + // Set trend arc's color + myPen.setColor(Qt::magenta); + painter->setPen(myPen); - //Turn on anti-aliasing so the fonts don't look terrible - painter->setRenderHint(QPainter::Antialiasing, true); - - //Set pen attributes - QColor myColor(Qt::red); - myPen.setWidth(3); - myPen.setColor(myColor); - painter->setPen(myPen); - painter->drawPolygon(arrowHead); - painter->setPen(myPen); - painter->drawLine(arrowShaft); - - //Set trend arc's color - myPen.setColor(Qt::magenta); - painter->setPen(myPen); - - if (trendSpanAngle > 0){ - QRectF rect(0, -trendRadius, trendRadius*2, trendRadius*2); - painter->drawArc(rect, 180*16, -trendSpanAngle*16); - } - else{ - QRectF rect(-2*trendRadius, -trendRadius, trendRadius*2, trendRadius*2); - painter->drawArc(rect, 0*16, -trendSpanAngle*16); - } - - //*********** Create time rings - if(groundspeed_mps_filt > 0){ //Don't clutter the display with rings that are only one pixel wide - myPen.setWidth(2); - - myPen.setColor(QColor(0, 0, 0, 100)); - painter->setPen(myPen); - painter->drawEllipse(QPointF(0,0),precalcRings,precalcRings); - - myPen.setColor(QColor(0, 0, 0, 110)); - painter->setPen(myPen); - painter->drawEllipse(QPointF(0,0),precalcRings*2,precalcRings*2); - - myPen.setColor(QColor(0, 0, 0, 120)); - painter->setPen(myPen); - painter->drawEllipse(QPointF(0,0),precalcRings*4,precalcRings*4); - } - //Rotate the text back to vertical - qreal rot=this->rotation(); - painter->rotate(-1*rot); - - myPen.setWidth(1); - myPen.setColor(Qt::white); - painter->setBrush(Qt::white); - painter->setPen(myPen); - painter->drawPath(textPath); + if (trendSpanAngle > 0) { + QRectF rect(0, -trendRadius, trendRadius * 2, trendRadius * 2); + painter->drawArc(rect, 180 * 16, -trendSpanAngle * 16); + } else { + QRectF rect(-2 * trendRadius, -trendRadius, trendRadius * 2, trendRadius * 2); + painter->drawArc(rect, 0 * 16, -trendSpanAngle * 16); } - void UAVItem::updateTextOverlay() - { - QPainterPath temp; - if(!showUAVInfo) - { - temp.swap(textPath); - return; - } + // *********** Create time rings + if (groundspeed_mps_filt > 0) { // Don't clutter the display with rings that are only one pixel wide + myPen.setWidth(2); - QFont borderfont( "Arial", 14, QFont::Normal, false ); + myPen.setColor(QColor(0, 0, 0, 100)); + painter->setPen(myPen); + painter->drawEllipse(QPointF(0, 0), precalcRings, precalcRings); - //Top left corner of text - int textAnchorX = 20; - int textAnchorY = 20; + myPen.setColor(QColor(0, 0, 0, 110)); + painter->setPen(myPen); + painter->drawEllipse(QPointF(0, 0), precalcRings * 2, precalcRings * 2); - QString uavoInfoStrLine1, uavoInfoStrLine2; - QString uavoInfoStrLine3, uavoInfoStrLine4; - QString uavoInfoStrLine5; + myPen.setColor(QColor(0, 0, 0, 120)); + painter->setPen(myPen); + painter->drawEllipse(QPointF(0, 0), precalcRings * 4, precalcRings * 4); + } + // Rotate the text back to vertical + qreal rot = this->rotation(); + painter->rotate(-1 * rot); - uavoInfoStrLine1.append(QString("CAS: %1 kph").arg(CAS_mps * 3.6)); - uavoInfoStrLine2.append(QString("Groundspeed: %1 kph").arg(groundspeed_kph, 0, 'f',1)); - uavoInfoStrLine3.append(QString("Lat-Lon: %1, %2").arg(coord.Lat(), 0, 'f',7).arg(coord.Lng(), 0, 'f',7)); - uavoInfoStrLine4.append(QString("North-East: %1 m, %2 m").arg(NED[0], 0, 'f',1).arg(NED[1], 0, 'f',1)); - uavoInfoStrLine5.append(QString("Altitude: %1 m").arg(-NED[2], 0, 'f',1)); - temp.addText(textAnchorX, textAnchorY+16*0, borderfont, uavoInfoStrLine1); - temp.addText(textAnchorX, textAnchorY+16*1, borderfont, uavoInfoStrLine2); - temp.addText(textAnchorX, textAnchorY+16*2, borderfont, uavoInfoStrLine3); - temp.addText(textAnchorX, textAnchorY+16*3, borderfont, uavoInfoStrLine4); - temp.addText(textAnchorX, textAnchorY+16*4, borderfont, uavoInfoStrLine5); + myPen.setWidth(1); + myPen.setColor(Qt::white); + painter->setBrush(Qt::white); + painter->setPen(myPen); + painter->drawPath(textPath); +} - //Add text for time rings. - if(groundspeed_mps > 0){ - //Always add the left side... - temp.addText(-(groundspeed_mps_filt*ringTime*1*meters2pixels+10), 0, borderfont, QString("%1 s").arg(ringTime,0,'f',0)); - temp.addText(-(groundspeed_mps_filt*ringTime*2*meters2pixels+10), 0, borderfont, QString("%1 s").arg(ringTime*2,0,'f',0)); - temp.addText(-(groundspeed_mps_filt*ringTime*4*meters2pixels+10), 0, borderfont, QString("%1 s").arg(ringTime*4,0,'f',0)); - //... and add the right side, only if it doesn't interfere with the uav info text - if(groundspeed_mps_filt*ringTime*4*meters2pixels > 200){ - if(groundspeed_mps_filt*ringTime*2*meters2pixels > 200){ - if(groundspeed_mps_filt*ringTime*1*meters2pixels > 200){ - temp.addText(groundspeed_mps_filt*ringTime*1*meters2pixels-8, 0, borderfont, QString("%1 s").arg(ringTime,0,'f',0)); - } - temp.addText(groundspeed_mps_filt*ringTime*2*meters2pixels-8, 0, borderfont, QString("%1 s").arg(ringTime*2,0,'f',0)); - } - temp.addText(groundspeed_mps_filt*ringTime*4*meters2pixels-8, 0, borderfont, QString("%1 s").arg(ringTime*4,0,'f',0)); - } - } +void UAVItem::updateTextOverlay() +{ + QPainterPath temp; + + if (!showUAVInfo) { temp.swap(textPath); + return; } - QRectF UAVItem::boundingRect()const - { - if(showUAVInfo){ - if (boundingRectSize < 220){ - //In case the bounding rectangle isn't big enough to get the whole of the UAV Info graphic - return QRectF(-boundingRectSize,-80,boundingRectSize+220,180); - } - else{ - return QRectF(-boundingRectSize,-boundingRectSize,2*boundingRectSize,2*boundingRectSize); - } - } - else{ - return QRectF(-pic.width()/2,-pic.height()/2,pic.width(),pic.height()); - } - } + QFont borderfont("Arial", 14, QFont::Normal, false); - void UAVItem::SetNED(double NED[3]){ - this->NED[0] = NED[0]; - this->NED[1] = NED[1]; - this->NED[2] = NED[2]; - } + // Top left corner of text + int textAnchorX = 20; + int textAnchorY = 20; - void UAVItem::SetYawRate(double yawRate_dps){ - this->yawRate_dps=yawRate_dps; + QString uavoInfoStrLine1, uavoInfoStrLine2; + QString uavoInfoStrLine3, uavoInfoStrLine4; + QString uavoInfoStrLine5; - if (fabs(this->yawRate_dps) < 5e-1){ //This number is really the smallest we can go. Any smaller, and it might have problems if we forecast a shorter distance into the future - this->yawRate_dps=5e-1; - } + uavoInfoStrLine1.append(QString("CAS: %1 kph").arg(CAS_mps * 3.6)); + uavoInfoStrLine2.append(QString("Groundspeed: %1 kph").arg(groundspeed_kph, 0, 'f', 1)); + uavoInfoStrLine3.append(QString("Lat-Lon: %1, %2").arg(coord.Lat(), 0, 'f', 7).arg(coord.Lng(), 0, 'f', 7)); + uavoInfoStrLine4.append(QString("North-East: %1 m, %2 m").arg(NED[0], 0, 'f', 1).arg(NED[1], 0, 'f', 1)); + uavoInfoStrLine5.append(QString("Altitude: %1 m").arg(-NED[2], 0, 'f', 1)); + temp.addText(textAnchorX, textAnchorY + 16 * 0, borderfont, uavoInfoStrLine1); + temp.addText(textAnchorX, textAnchorY + 16 * 1, borderfont, uavoInfoStrLine2); + temp.addText(textAnchorX, textAnchorY + 16 * 2, borderfont, uavoInfoStrLine3); + temp.addText(textAnchorX, textAnchorY + 16 * 3, borderfont, uavoInfoStrLine4); + temp.addText(textAnchorX, textAnchorY + 16 * 4, borderfont, uavoInfoStrLine5); - //*********** Create trend arc - trendSpanAngle = this->yawRate_dps * 5; //Forecast 5 seconds into the future - - //Calculate radius in [m], and then convert to pixels in local frame (not the same frame as is displayed on the map widget) - trendRadius=fabs(groundspeed_mps/(this->yawRate_dps*M_PI/180))*meters2pixels; - } - - void UAVItem::SetCAS(double CAS_mps){ - this->CAS_mps=CAS_mps; - } - - void UAVItem::SetGroundspeed(double vNED[3], int m_maxUpdateRate_ms){ - this->vNED[0] = vNED[0]; - this->vNED[1] = vNED[1]; - this->vNED[2] = vNED[2]; - groundspeed_kph=sqrt(vNED[0]*vNED[0] + vNED[1]*vNED[1] + vNED[2]*vNED[2])*3.6; - groundspeed_mps=groundspeed_kph/3.6; - //On the first pass, set the filtered speed to the reported speed. - static bool firstGroundspeed=true; - if (firstGroundspeed){ - groundspeed_mps_filt=groundspeed_kph/3.6; - firstGroundspeed=false; - } - else{ - int riseTime_ms=1000; - double alpha= m_maxUpdateRate_ms/(double)(m_maxUpdateRate_ms+riseTime_ms); - groundspeed_mps_filt= alpha*groundspeed_mps_filt + (1-alpha)*(groundspeed_kph/3.6); - } - ringTime=10*pow(2,17-map->ZoomTotal()); //Basic ring is 10 seconds wide at zoom level 17 - precalcRings=groundspeed_mps_filt*ringTime*meters2pixels; - boundingRectSize=groundspeed_mps_filt*ringTime*4*meters2pixels+20; - prepareGeometryChange(); - } - - - void UAVItem::SetUAVPos(const internals::PointLatLng &position, const int &altitude) - { - if(coord.IsEmpty()) - lastcoord=coord; - if(coord!=position) - { - - if(trailtype==UAVTrailType::ByTimeElapsed) - { - if(timer.elapsed()>trailtime*1000) - { - TrailItem * ob=new TrailItem(position,altitude,Qt::green,map); - trail->addToGroup(ob); - connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT())); - if(!lasttrailline.IsEmpty()) - { - TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::red,map); - trailLine->addToGroup(obj); - connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot())); - } - lasttrailline=position; - timer.restart(); + // Add text for time rings. + if (groundspeed_mps > 0) { + // Always add the left side... + temp.addText(-(groundspeed_mps_filt * ringTime * 1 * meters2pixels + 10), 0, borderfont, QString("%1 s").arg(ringTime, 0, 'f', 0)); + temp.addText(-(groundspeed_mps_filt * ringTime * 2 * meters2pixels + 10), 0, borderfont, QString("%1 s").arg(ringTime * 2, 0, 'f', 0)); + temp.addText(-(groundspeed_mps_filt * ringTime * 4 * meters2pixels + 10), 0, borderfont, QString("%1 s").arg(ringTime * 4, 0, 'f', 0)); + // ... and add the right side, only if it doesn't interfere with the uav info text + if (groundspeed_mps_filt * ringTime * 4 * meters2pixels > 200) { + if (groundspeed_mps_filt * ringTime * 2 * meters2pixels > 200) { + if (groundspeed_mps_filt * ringTime * 1 * meters2pixels > 200) { + temp.addText(groundspeed_mps_filt * ringTime * 1 * meters2pixels - 8, 0, borderfont, QString("%1 s").arg(ringTime, 0, 'f', 0)); } - - } - else if(trailtype==UAVTrailType::ByDistance) - { - if(qAbs(internals::PureProjection::DistanceBetweenLatLng(lastcoord,position)*1000)>traildistance) - { - TrailItem * ob=new TrailItem(position,altitude,Qt::green,map); - trail->addToGroup(ob); - connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT())); - if(!lasttrailline.IsEmpty()) - { - TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::red,map); - trailLine->addToGroup(obj); - connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot())); - } - lasttrailline=position; - lastcoord=position; - } - } - coord=position; - this->altitude=altitude; - RefreshPos(); - if(mapfollowtype==UAVMapFollowType::CenterAndRotateMap||mapfollowtype==UAVMapFollowType::CenterMap) - { - mapwidget->SetCurrentPosition(coord); - } - if(autosetreached) - { - foreach(QGraphicsItem* i,map->childItems()) - { - WayPointItem* wp=qgraphicsitem_cast(i); - if(wp) - { - if(Distance3D(wp->Coord(),wp->Altitude())SetReached(true); - emit UAVReachedWayPoint(wp->Number(),wp); - } - } - } - } - if(mapwidget->Home!=0) - { - //verify if the UAV is inside the safety bouble - if(Distance3D(mapwidget->Home->Coord(),mapwidget->Home->Altitude())>mapwidget->Home->SafeArea()) - { - if(mapwidget->Home->safe!=false) - { - mapwidget->Home->safe=false; - mapwidget->Home->update(); - emit UAVLeftSafetyBouble(this->coord); - } - } - else - { - if(mapwidget->Home->safe!=true) - { - mapwidget->Home->safe=true; - mapwidget->Home->update(); - } - } - + temp.addText(groundspeed_mps_filt * ringTime * 2 * meters2pixels - 8, 0, borderfont, QString("%1 s").arg(ringTime * 2, 0, 'f', 0)); } + temp.addText(groundspeed_mps_filt * ringTime * 4 * meters2pixels - 8, 0, borderfont, QString("%1 s").arg(ringTime * 4, 0, 'f', 0)); } } + temp.swap(textPath); +} - /** - * Rotate the UAV Icon on the map, or rotate the map - * depending on the display mode - */ - void UAVItem::SetUAVHeading(const qreal &value) - { - if(mapfollowtype==UAVMapFollowType::CenterAndRotateMap) - { - mapwidget->SetRotate(-value); +QRectF UAVItem::boundingRect() const +{ + if (showUAVInfo) { + if (boundingRectSize < 220) { + // In case the bounding rectangle isn't big enough to get the whole of the UAV Info graphic + return QRectF(-boundingRectSize, -80, boundingRectSize + 220, 180); + } else { + return QRectF(-boundingRectSize, -boundingRectSize, 2 * boundingRectSize, 2 * boundingRectSize); } - else { - if (this->rotation() != value) - this->setRotation(value); - } - } - - - int UAVItem::type()const - { - return Type; - } - - - void UAVItem::RefreshPos() - { - localposition=map->FromLatLngToLocal(coord); - this->setPos(localposition.X(),localposition.Y()); - emit setChildPosition(); - emit setChildLine(); - updateTextOverlay(); - } - - void UAVItem::setOpacitySlot(qreal opacity) - { - this->setOpacity(opacity); - } - - void UAVItem::zoomChangedSlot() - { - double pixels2meters = map->Projection()->GetGroundResolution(map->ZoomTotal(),coord.Lat()); - meters2pixels=1.0 / pixels2meters; - boundingRectSize=groundspeed_mps_filt*ringTime*4*meters2pixels+20; - prepareGeometryChange(); - updateTextOverlay(); - update(); - } - void UAVItem::SetTrailType(const UAVTrailType::Types &value) - { - trailtype=value; - if(trailtype==UAVTrailType::ByTimeElapsed) - timer.restart(); - } - void UAVItem::SetShowTrail(const bool &value) - { - showtrail=value; - trail->setVisible(value); - } - void UAVItem::SetShowTrailLine(const bool &value) - { - showtrailline=value; - trailLine->setVisible(value); - } - - void UAVItem::DeleteTrail()const - { - foreach(QGraphicsItem* i,trail->childItems()) - delete i; - foreach(QGraphicsItem* i,trailLine->childItems()) - delete i; - } - double UAVItem::Distance3D(const internals::PointLatLng &coord, const int &altitude) - { - return sqrt(pow(internals::PureProjection::DistanceBetweenLatLng(this->coord,coord)*1000,2)+ - pow(this->altitude-altitude,2)); - } - void UAVItem::SetUavPic(QString UAVPic) - { - pic.load(":/uavs/images/"+UAVPic); - } - - void UAVItem::SetShowUAVInfo(bool const& value) - { - showUAVInfo=value; - showJustChanged=true; - update(); - } - - void UAVItem::generateArrowhead(){ - qreal arrowSize = 10; - - //Create line from (0,0), to (1,1). Later, we'll scale and rotate it - arrowShaft=QLineF(0,0,1.0,1.0); - - //Set the starting point to (0,0) - arrowShaft.setP1(QPointF(0,0)); - - //Set angle and length - arrowShaft.setLength(60.0); - arrowShaft.setAngle(90.0); - - //Form arrowhead - double angle = ::acos(arrowShaft.dx() / arrowShaft.length()); - if (arrowShaft.dy() <= 0) - angle = (M_PI * 2) - angle; - - QPointF arrowP1 = arrowShaft.pointAt(1) + QPointF(sin(angle + M_PI / 3) * arrowSize, - cos(angle + M_PI / 3) * arrowSize); - QPointF arrowP2 = arrowShaft.pointAt(1) + QPointF(sin(angle + M_PI - M_PI / 3) * arrowSize, - cos(angle + M_PI - M_PI / 3) * arrowSize); - - //Assemble arrowhead - arrowHead.clear(); - arrowHead << arrowShaft.pointAt(1) << arrowP1 << arrowP2; - + } else { + return QRectF(-pic.width() / 2, -pic.height() / 2, pic.width(), pic.height()); } } + +void UAVItem::SetNED(double NED[3]) +{ + this->NED[0] = NED[0]; + this->NED[1] = NED[1]; + this->NED[2] = NED[2]; +} + +void UAVItem::SetYawRate(double yawRate_dps) +{ + this->yawRate_dps = yawRate_dps; + + if (fabs(this->yawRate_dps) < 5e-1) { // This number is really the smallest we can go. Any smaller, and it might have problems if we forecast a shorter distance into the future + this->yawRate_dps = 5e-1; + } + + // *********** Create trend arc + trendSpanAngle = this->yawRate_dps * 5; // Forecast 5 seconds into the future + + // Calculate radius in [m], and then convert to pixels in local frame (not the same frame as is displayed on the map widget) + trendRadius = fabs(groundspeed_mps / (this->yawRate_dps * M_PI / 180)) * meters2pixels; +} + +void UAVItem::SetCAS(double CAS_mps) +{ + this->CAS_mps = CAS_mps; +} + +void UAVItem::SetGroundspeed(double vNED[3], int m_maxUpdateRate_ms) +{ + this->vNED[0] = vNED[0]; + this->vNED[1] = vNED[1]; + this->vNED[2] = vNED[2]; + groundspeed_kph = sqrt(vNED[0] * vNED[0] + vNED[1] * vNED[1] + vNED[2] * vNED[2]) * 3.6; + groundspeed_mps = groundspeed_kph / 3.6; + // On the first pass, set the filtered speed to the reported speed. + static bool firstGroundspeed = true; + if (firstGroundspeed) { + groundspeed_mps_filt = groundspeed_kph / 3.6; + firstGroundspeed = false; + } else { + int riseTime_ms = 1000; + double alpha = m_maxUpdateRate_ms / (double)(m_maxUpdateRate_ms + riseTime_ms); + groundspeed_mps_filt = alpha * groundspeed_mps_filt + (1 - alpha) * (groundspeed_kph / 3.6); + } + ringTime = 10 * pow(2, 17 - map->ZoomTotal()); // Basic ring is 10 seconds wide at zoom level 17 + precalcRings = groundspeed_mps_filt * ringTime * meters2pixels; + boundingRectSize = groundspeed_mps_filt * ringTime * 4 * meters2pixels + 20; + prepareGeometryChange(); +} + + +void UAVItem::SetUAVPos(const internals::PointLatLng &position, const int &altitude) +{ + if (coord.IsEmpty()) { + lastcoord = coord; + } + if (coord != position) { + if (trailtype == UAVTrailType::ByTimeElapsed) { + if (timer.elapsed() > trailtime * 1000) { + TrailItem *ob = new TrailItem(position, altitude, Qt::green, map); + trail->addToGroup(ob); + connect(this, SIGNAL(setChildPosition()), ob, SLOT(setPosSLOT())); + if (!lasttrailline.IsEmpty()) { + TrailLineItem *obj = new TrailLineItem(lasttrailline, position, Qt::red, map); + trailLine->addToGroup(obj); + connect(this, SIGNAL(setChildLine()), obj, SLOT(setLineSlot())); + } + lasttrailline = position; + timer.restart(); + } + } else if (trailtype == UAVTrailType::ByDistance) { + if (qAbs(internals::PureProjection::DistanceBetweenLatLng(lastcoord, position) * 1000) > traildistance) { + TrailItem *ob = new TrailItem(position, altitude, Qt::green, map); + trail->addToGroup(ob); + connect(this, SIGNAL(setChildPosition()), ob, SLOT(setPosSLOT())); + if (!lasttrailline.IsEmpty()) { + TrailLineItem *obj = new TrailLineItem(lasttrailline, position, Qt::red, map); + trailLine->addToGroup(obj); + connect(this, SIGNAL(setChildLine()), obj, SLOT(setLineSlot())); + } + lasttrailline = position; + lastcoord = position; + } + } + coord = position; + this->altitude = altitude; + RefreshPos(); + if (mapfollowtype == UAVMapFollowType::CenterAndRotateMap || mapfollowtype == UAVMapFollowType::CenterMap) { + mapwidget->SetCurrentPosition(coord); + } + if (autosetreached) { + foreach(QGraphicsItem * i, map->childItems()) { + WayPointItem *wp = qgraphicsitem_cast(i); + + if (wp) { + if (Distance3D(wp->Coord(), wp->Altitude()) < autosetdistance) { + wp->SetReached(true); + emit UAVReachedWayPoint(wp->Number(), wp); + } + } + } + } + if (mapwidget->Home != 0) { + // verify if the UAV is inside the safety bouble + if (Distance3D(mapwidget->Home->Coord(), mapwidget->Home->Altitude()) > mapwidget->Home->SafeArea()) { + if (mapwidget->Home->safe != false) { + mapwidget->Home->safe = false; + mapwidget->Home->update(); + emit UAVLeftSafetyBouble(this->coord); + } + } else { + if (mapwidget->Home->safe != true) { + mapwidget->Home->safe = true; + mapwidget->Home->update(); + } + } + } + } +} + +/** + * Rotate the UAV Icon on the map, or rotate the map + * depending on the display mode + */ +void UAVItem::SetUAVHeading(const qreal &value) +{ + if (mapfollowtype == UAVMapFollowType::CenterAndRotateMap) { + mapwidget->SetRotate(-value); + } else { + if (this->rotation() != value) { + this->setRotation(value); + } + } +} + + +int UAVItem::type() const +{ + return Type; +} + + +void UAVItem::RefreshPos() +{ + localposition = map->FromLatLngToLocal(coord); + this->setPos(localposition.X(), localposition.Y()); + emit setChildPosition(); + emit setChildLine(); + updateTextOverlay(); +} + +void UAVItem::setOpacitySlot(qreal opacity) +{ + this->setOpacity(opacity); +} + +void UAVItem::zoomChangedSlot() +{ + double pixels2meters = map->Projection()->GetGroundResolution(map->ZoomTotal(), coord.Lat()); + + meters2pixels = 1.0 / pixels2meters; + boundingRectSize = groundspeed_mps_filt * ringTime * 4 * meters2pixels + 20; + prepareGeometryChange(); + updateTextOverlay(); + update(); +} +void UAVItem::SetTrailType(const UAVTrailType::Types &value) +{ + trailtype = value; + if (trailtype == UAVTrailType::ByTimeElapsed) { + timer.restart(); + } +} +void UAVItem::SetShowTrail(const bool &value) +{ + showtrail = value; + trail->setVisible(value); +} +void UAVItem::SetShowTrailLine(const bool &value) +{ + showtrailline = value; + trailLine->setVisible(value); +} + +void UAVItem::DeleteTrail() const +{ + foreach(QGraphicsItem * i, trail->childItems()) + delete i; + foreach(QGraphicsItem * i, trailLine->childItems()) + delete i; +} +double UAVItem::Distance3D(const internals::PointLatLng &coord, const int &altitude) +{ + return sqrt(pow(internals::PureProjection::DistanceBetweenLatLng(this->coord, coord) * 1000, 2) + + pow(this->altitude - altitude, 2)); +} +void UAVItem::SetUavPic(QString UAVPic) +{ + pic.load(":/uavs/images/" + UAVPic); +} + +void UAVItem::SetShowUAVInfo(bool const & value) +{ + showUAVInfo = value; + showJustChanged = true; + update(); +} + +void UAVItem::generateArrowhead() +{ + qreal arrowSize = 10; + + // Create line from (0,0), to (1,1). Later, we'll scale and rotate it + arrowShaft = QLineF(0, 0, 1.0, 1.0); + + // Set the starting point to (0,0) + arrowShaft.setP1(QPointF(0, 0)); + + // Set angle and length + arrowShaft.setLength(60.0); + arrowShaft.setAngle(90.0); + + // Form arrowhead + double angle = ::acos(arrowShaft.dx() / arrowShaft.length()); + if (arrowShaft.dy() <= 0) { + angle = (M_PI * 2) - angle; + } + + QPointF arrowP1 = arrowShaft.pointAt(1) + QPointF(sin(angle + M_PI / 3) * arrowSize, + cos(angle + M_PI / 3) * arrowSize); + QPointF arrowP2 = arrowShaft.pointAt(1) + QPointF(sin(angle + M_PI - M_PI / 3) * arrowSize, + cos(angle + M_PI - M_PI / 3) * arrowSize); + + // Assemble arrowhead + arrowHead.clear(); + arrowHead << arrowShaft.pointAt(1) << arrowP1 << arrowP2; +} +} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.h index 472563f37..e137ceac1 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file uavitem.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a WayPoint -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file uavitem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef UAVITEM_H #define UAVITEM_H @@ -40,238 +40,277 @@ #include "opmapwidget.h" #include "trailitem.h" #include "traillineitem.h" -namespace mapcontrol -{ - class WayPointItem; - class OPMapWidget; +namespace mapcontrol { +class WayPointItem; +class OPMapWidget; +/** + * @brief A QGraphicsItem representing the UAV + * + * @class UAVItem uavitem.h "mapwidget/uavitem.h" + */ +class UAVItem : public QObject, public QGraphicsItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) +public: + enum { Type = UserType + 2 }; + UAVItem(MapGraphicItem *map, OPMapWidget *parent, QString uavPic = QString::fromUtf8(":/uavs/images/mapquad.png")); + ~UAVItem(); + /** -* @brief A QGraphicsItem representing the UAV -* -* @class UAVItem uavitem.h "mapwidget/uavitem.h" -*/ - class UAVItem:public QObject,public QGraphicsItem + * @brief Sets the UAV NED position + * + * @param NED + */ + void SetNED(double NED[3]); + /** + * @brief Sets the UAV groundspeed + * + * @param NED + */ + void SetGroundspeed(double vNED[3], int m_maxUpdateRate); + /** + * @brief Sets the UAV Calibrated Airspeed + * + * @param NED + */ + void SetCAS(double CAS); + /** + * @brief Sets the UAV yaw rate + * + * @param NED + */ + void SetYawRate(double yawRate_dps); + /** + * @brief Sets the UAV position + * + * @param position LatLng point + * @param altitude altitude in meters + */ + void SetUAVPos(internals::PointLatLng const & position, int const & altitude); + /** + * @brief Sets the UAV heading + * + * @param value heading angle (north=0deg) + */ + void SetUAVHeading(qreal const & value); + /** + * @brief Returns the UAV position + * + * @return internals::PointLatLng + */ + internals::PointLatLng UAVPos() const { - Q_OBJECT - Q_INTERFACES(QGraphicsItem) - public: - enum { Type = UserType + 2 }; - UAVItem(MapGraphicItem* map,OPMapWidget* parent, QString uavPic=QString::fromUtf8(":/uavs/images/mapquad.png")); - ~UAVItem(); + return coord; + } + /** + * @brief Sets the Map follow type + * + * @param value can be "none"(map doesnt follow UAV), "CenterMap"(map moves to keep UAV centered) or "CenterAndRotateMap"(map moves and rotates to keep UAV centered and straight) + */ + void SetMapFollowType(UAVMapFollowType::Types const & value) + { + mapfollowtype = value; + } + /** + * @brief Sets the trail type + * + * @param value can be "NoTrail"(no trail is plotted), "ByTimeElapsed"(a trail point is plotted each TrailTime()) or ByDistance(a trail point is plotted if the distance between the UAV and the last trail point is bigger than TrailDistance()) + */ + void SetTrailType(UAVTrailType::Types const & value); + /** + * @brief Returns the map follow method used + * + * @return UAVMapFollowType::Types + */ + UAVMapFollowType::Types GetMapFollowType() const + { + return mapfollowtype; + } + /** + * @brief Returns the UAV trail type. It can be plotted by time elapsed or distance + * + * @return UAVTrailType::Types + */ + UAVTrailType::Types GetTrailType() const + { + return trailtype; + } - /** - * @brief Sets the UAV NED position - * - * @param NED - */ - void SetNED(double NED[3]); - /** - * @brief Sets the UAV groundspeed - * - * @param NED - */ - void SetGroundspeed(double vNED[3], int m_maxUpdateRate); - /** - * @brief Sets the UAV Calibrated Airspeed - * - * @param NED - */ - void SetCAS(double CAS); - /** - * @brief Sets the UAV yaw rate - * - * @param NED - */ - void SetYawRate(double yawRate_dps); - /** - * @brief Sets the UAV position - * - * @param position LatLng point - * @param altitude altitude in meters - */ - void SetUAVPos(internals::PointLatLng const& position,int const& altitude); - /** - * @brief Sets the UAV heading - * - * @param value heading angle (north=0deg) - */ - void SetUAVHeading(qreal const& value); - /** - * @brief Returns the UAV position - * - * @return internals::PointLatLng - */ - internals::PointLatLng UAVPos()const{return coord;} - /** - * @brief Sets the Map follow type - * - * @param value can be "none"(map doesnt follow UAV), "CenterMap"(map moves to keep UAV centered) or "CenterAndRotateMap"(map moves and rotates to keep UAV centered and straight) - */ - void SetMapFollowType(UAVMapFollowType::Types const& value){mapfollowtype=value;} - /** - * @brief Sets the trail type - * - * @param value can be "NoTrail"(no trail is plotted), "ByTimeElapsed"(a trail point is plotted each TrailTime()) or ByDistance(a trail point is plotted if the distance between the UAV and the last trail point is bigger than TrailDistance()) - */ - void SetTrailType(UAVTrailType::Types const& value); - /** - * @brief Returns the map follow method used - * - * @return UAVMapFollowType::Types - */ - UAVMapFollowType::Types GetMapFollowType()const{return mapfollowtype;} - /** - * @brief Returns the UAV trail type. It can be plotted by time elapsed or distance - * - * @return UAVTrailType::Types - */ - UAVTrailType::Types GetTrailType()const{return trailtype;} + void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, + QWidget *widget); + QRectF boundingRect() const; + /** + * @brief Sets the trail time to be used if TrailType is ByTimeElapsed + * + * @param seconds the UAV trail time elapsed value. If the trail type is time elapsed + * a trail point will be plotted each "value returned" seconds. + */ + void SetTrailTime(int const & seconds) + { + trailtime = seconds; + } + /** + * @brief Returns the UAV trail time elapsed value. If the trail type is time elapsed + * a trail point will be plotted each "value returned" seconds. + * + * @return int + */ + int TrailTime() const + { + return trailtime; + } + /** + * @brief Sets the trail distance to be used if TrailType is ByDistance + * + * @param distance the UAV trail plot distance. + * If the trail type is ByDistance a trail dot is plotted if + * the distance between the current UAV position and the last trail point + * is bigger than the returned value + */ + void SetTrailDistance(int const & distance) + { + traildistance = distance; + } + /** + * @brief Returns the UAV trail plot distance. + * If the trail type is distance diference a trail dot is plotted if + * the distance between the current UAV position and the last trail point + * is bigger than the returned value + * + * @return int + */ + int TrailDistance() const + { + return traildistance; + } + /** + * @brief Returns true if UAV trail is shown + * + * @return bool + */ + bool ShowTrail() const + { + return showtrail; + } + /** + * @brief Returns true if UAV trail line is shown + * + * @return bool + */ + bool ShowTrailLine() const + { + return showtrailline; + } + /** + * @brief Used to define if the UAV displays a trail + * + * @param value + */ + void SetShowTrail(bool const & value); + /** + * @brief Used to define if the UAV displays a trail line + * + * @param value + */ + void SetShowTrailLine(bool const & value); + /** + * @brief Deletes all the trail points + */ + void DeleteTrail() const; + /** + * @brief Returns true if the UAV automaticaly sets WP reached value (changing its color) + * + * @return bool + */ + bool AutoSetReached() const + { + return autosetreached; + } + /** + * @brief Defines if the UAV can set the WP's "reached" value automaticaly. + * + * @param value + */ + void SetAutoSetReached(bool const & value) + { + autosetreached = value; + } + /** + * @brief Returns the 3D distance in meters necessary for the UAV to set WP's to "reached" + * + * @return double + */ + double AutoSetDistance() const + { + return autosetdistance; + } + /** + * @brief Sets the the 3D distance in meters necessary for the UAV to set WP's to "reached" + * + * @param value + */ + void SetAutoSetDistance(double const & value) + { + autosetdistance = value; + } - void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, - QWidget *widget); - QRectF boundingRect() const; - /** - * @brief Sets the trail time to be used if TrailType is ByTimeElapsed - * - * @param seconds the UAV trail time elapsed value. If the trail type is time elapsed - * a trail point will be plotted each "value returned" seconds. - */ - void SetTrailTime(int const& seconds){trailtime=seconds;} - /** - * @brief Returns the UAV trail time elapsed value. If the trail type is time elapsed - * a trail point will be plotted each "value returned" seconds. - * - * @return int - */ - int TrailTime()const{return trailtime;} - /** - * @brief Sets the trail distance to be used if TrailType is ByDistance - * - * @param distance the UAV trail plot distance. - * If the trail type is ByDistance a trail dot is plotted if - * the distance between the current UAV position and the last trail point - * is bigger than the returned value - */ - void SetTrailDistance(int const& distance){traildistance=distance;} - /** - * @brief Returns the UAV trail plot distance. - * If the trail type is distance diference a trail dot is plotted if - * the distance between the current UAV position and the last trail point - * is bigger than the returned value - * - * @return int - */ - int TrailDistance()const{return traildistance;} - /** - * @brief Returns true if UAV trail is shown - * - * @return bool - */ - bool ShowTrail()const{return showtrail;} - /** - * @brief Returns true if UAV trail line is shown - * - * @return bool - */ - bool ShowTrailLine()const{return showtrailline;} - /** - * @brief Used to define if the UAV displays a trail - * - * @param value - */ - void SetShowTrail(bool const& value); - /** - * @brief Used to define if the UAV displays a trail line - * - * @param value - */ - void SetShowTrailLine(bool const& value); - /** - * @brief Deletes all the trail points - */ - void DeleteTrail()const; - /** - * @brief Returns true if the UAV automaticaly sets WP reached value (changing its color) - * - * @return bool - */ - bool AutoSetReached()const{return autosetreached;} - /** - * @brief Defines if the UAV can set the WP's "reached" value automaticaly. - * - * @param value - */ - void SetAutoSetReached(bool const& value){autosetreached=value;} - /** - * @brief Returns the 3D distance in meters necessary for the UAV to set WP's to "reached" - * - * @return double - */ - double AutoSetDistance()const{return autosetdistance;} - /** - * @brief Sets the the 3D distance in meters necessary for the UAV to set WP's to "reached" - * - * @param value - */ - void SetAutoSetDistance(double const& value){autosetdistance=value;} + int type() const; - int type() const; + void SetUavPic(QString UAVPic); + void SetShowUAVInfo(bool const & value); + void updateTextOverlay(); +private: + void generateArrowhead(); + MapGraphicItem *map; + OPMapWidget *mapwidget; + QPolygonF arrowHead; + QLineF arrowShaft; + int altitude; + UAVMapFollowType::Types mapfollowtype; + UAVTrailType::Types trailtype; + internals::PointLatLng coord; + internals::PointLatLng lastcoord; + double NED[3]; + double vNED[3]; + double CAS_mps; + double groundspeed_kph; + double groundspeed_mps; + double yawRate_dps; + double trendRadius; + double trendSpanAngle; + float meters2pixels; + double precalcRings; + double ringTime; + QPixmap pic; + core::Point localposition; + QGraphicsItemGroup *trail; + QGraphicsItemGroup *trailLine; + internals::PointLatLng lasttrailline; + QTime timer; + bool showtrail; + bool showtrailline; + int trailtime; + int traildistance; + bool autosetreached; + double Distance3D(internals::PointLatLng const & coord, int const & altitude); + double autosetdistance; + bool showUAVInfo; + static double groundspeed_mps_filt; + float boundingRectSize; + bool showJustChanged; - void SetUavPic(QString UAVPic); - void SetShowUAVInfo(bool const& value); - void updateTextOverlay(); - private: - void generateArrowhead(); - MapGraphicItem* map; - OPMapWidget* mapwidget; - QPolygonF arrowHead; - QLineF arrowShaft; - int altitude; - UAVMapFollowType::Types mapfollowtype; - UAVTrailType::Types trailtype; - internals::PointLatLng coord; - internals::PointLatLng lastcoord; - double NED[3]; - double vNED[3]; - double CAS_mps; - double groundspeed_kph; - double groundspeed_mps; - double yawRate_dps; - double trendRadius; - double trendSpanAngle; - float meters2pixels; - double precalcRings; - double ringTime; - QPixmap pic; - core::Point localposition; - QGraphicsItemGroup* trail; - QGraphicsItemGroup * trailLine; - internals::PointLatLng lasttrailline; - QTime timer; - bool showtrail; - bool showtrailline; - int trailtime; - int traildistance; - bool autosetreached; - double Distance3D(internals::PointLatLng const& coord, int const& altitude); - double autosetdistance; - bool showUAVInfo; - static double groundspeed_mps_filt; - float boundingRectSize; - bool showJustChanged; + bool refreshPaint_flag; - bool refreshPaint_flag; + QPainterPath textPath; - QPainterPath textPath; - - public slots: - void RefreshPos(); - void setOpacitySlot(qreal opacity); - void zoomChangedSlot(); - signals: - void UAVReachedWayPoint(int const& waypointnumber,WayPointItem* waypoint); - void UAVLeftSafetyBouble(internals::PointLatLng const& position); - void setChildPosition(); - void setChildLine(); - }; +public slots: + void RefreshPos(); + void setOpacitySlot(qreal opacity); + void zoomChangedSlot(); +signals: + void UAVReachedWayPoint(int const & waypointnumber, WayPointItem *waypoint); + void UAVLeftSafetyBouble(internals::PointLatLng const & position); + void setChildPosition(); + void setChildLine(); +}; } #endif // UAVITEM_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavmapfollowtype.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavmapfollowtype.h index b4c870e87..9f7c06279 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavmapfollowtype.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavmapfollowtype.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file uavmapfollowtype.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief An enum representing the various map follow modes -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file uavmapfollowtype.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief An enum representing the various map follow modes + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef UAVMAPFOLLOWTYPE_H #define UAVMAPFOLLOWTYPE_H @@ -32,55 +32,53 @@ #include #include namespace mapcontrol { - class UAVMapFollowType:public QObject - { - Q_OBJECT - Q_ENUMS(Types) - public: - enum Types - { - /// - /// only centers the map on the UAV - /// - CenterMap, +class UAVMapFollowType : public QObject { + Q_OBJECT Q_ENUMS(Types) +public: + enum Types { + /// + /// only centers the map on the UAV + /// + CenterMap, - /// - /// centers and rotates map on the UAV - /// - CenterAndRotateMap, - - /// - /// map is not connected to UAV position or heading - /// - None - }; - static QString StrByType(Types const& value) - { - QMetaObject metaObject = UAVMapFollowType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - QString s=metaEnum.valueToKey(value); - return s; - } - static Types TypeByStr(QString const& value) - { - QMetaObject metaObject = UAVMapFollowType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - Types s=(Types)metaEnum.keyToValue(value.toLatin1()); - return s; - } - static QStringList TypesList() - { - QStringList ret; - QMetaObject metaObject = UAVMapFollowType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - for(int x=0;x + /// centers and rotates map on the UAV + /// + CenterAndRotateMap, + /// + /// map is not connected to UAV position or heading + /// + None }; + static QString StrByType(Types const & value) + { + QMetaObject metaObject = UAVMapFollowType().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + QString s = metaEnum.valueToKey(value); + + return s; + } + static Types TypeByStr(QString const & value) + { + QMetaObject metaObject = UAVMapFollowType().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + Types s = (Types)metaEnum.keyToValue(value.toLatin1()); + + return s; + } + static QStringList TypesList() + { + QStringList ret; + QMetaObject metaObject = UAVMapFollowType().staticMetaObject; + QMetaEnum metaEnum = metaObject.enumerator(metaObject.indexOfEnumerator("Types")); + + for (int x = 0; x < metaEnum.keyCount(); ++x) { + ret.append(metaEnum.key(x)); + } + return ret; + } +}; } #endif // UAVMAPFOLLOWTYPE_H diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavtrailtype.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavtrailtype.h index 2a17d4b61..3dc5a18bf 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavtrailtype.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavtrailtype.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file uavtrailtype.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief An enum representing the uav trailing modes -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file uavtrailtype.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief An enum representing the uav trailing modes + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef UAVTRAILTYPE_H #define UAVTRAILTYPE_H @@ -32,59 +32,57 @@ #include #include namespace mapcontrol { - class UAVTrailType:public QObject - { - Q_OBJECT - Q_ENUMS(Types) - public: - enum Types - { - /** - * @brief UAV does not plot a trail - * - * @var NoTrail - */ - NoTrail, - /** - * @brief UAV plots a trail point every 'x' seconds - * - * @var ByTimeElapsed - */ - ByTimeElapsed, - /** - * @brief UAV plots a trail point every 'x' meters (ground distance) - * - * @var ByDistance - */ - ByDistance - }; - static QString StrByType(Types const& value) - { - QMetaObject metaObject = UAVTrailType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - QString s=metaEnum.valueToKey(value); - return s; - } - static Types TypeByStr(QString const& value) - { - QMetaObject metaObject = UAVTrailType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - Types s=(Types)metaEnum.keyToValue(value.toLatin1()); - return s; - } - static QStringList TypesList() - { - QStringList ret; - QMetaObject metaObject = UAVTrailType().staticMetaObject; - QMetaEnum metaEnum= metaObject.enumerator( metaObject.indexOfEnumerator("Types")); - for(int x=0;x #include "homeitem.h" -namespace mapcontrol +namespace mapcontrol { +WayPointCircle::WayPointCircle(WayPointItem *center, WayPointItem *radius, bool clockwise, MapGraphicItem *map, QColor color) : my_center(center), + my_radius(radius), my_map(map), QGraphicsEllipseItem(map), myColor(color), myClockWise(clockwise) { -WayPointCircle::WayPointCircle(WayPointItem *center, WayPointItem *radius,bool clockwise, MapGraphicItem *map,QColor color):my_center(center), - my_radius(radius),my_map(map),QGraphicsEllipseItem(map),myColor(color),myClockWise(clockwise) -{ - connect(center,SIGNAL(localPositionChanged(QPointF,WayPointItem*)),this,SLOT(refreshLocations())); - connect(radius,SIGNAL(localPositionChanged(QPointF,WayPointItem*)),this,SLOT(refreshLocations())); - connect(center,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); - connect(radius,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); + connect(center, SIGNAL(localPositionChanged(QPointF, WayPointItem *)), this, SLOT(refreshLocations())); + connect(radius, SIGNAL(localPositionChanged(QPointF, WayPointItem *)), this, SLOT(refreshLocations())); + connect(center, SIGNAL(aboutToBeDeleted(WayPointItem *)), this, SLOT(waypointdeleted())); + connect(radius, SIGNAL(aboutToBeDeleted(WayPointItem *)), this, SLOT(waypointdeleted())); refreshLocations(); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); - + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); } -WayPointCircle::WayPointCircle(HomeItem *radius, WayPointItem *center, bool clockwise, MapGraphicItem *map, QColor color):my_center(center), - my_radius(radius),my_map(map),QGraphicsEllipseItem(map),myColor(color),myClockWise(clockwise) +WayPointCircle::WayPointCircle(HomeItem *radius, WayPointItem *center, bool clockwise, MapGraphicItem *map, QColor color) : my_center(center), + my_radius(radius), my_map(map), QGraphicsEllipseItem(map), myColor(color), myClockWise(clockwise) { - connect(radius,SIGNAL(homePositionChanged(internals::PointLatLng,float)),this,SLOT(refreshLocations())); - connect(center,SIGNAL(localPositionChanged(QPointF)),this,SLOT(refreshLocations())); - connect(center,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); + connect(radius, SIGNAL(homePositionChanged(internals::PointLatLng, float)), this, SLOT(refreshLocations())); + connect(center, SIGNAL(localPositionChanged(QPointF)), this, SLOT(refreshLocations())); + connect(center, SIGNAL(aboutToBeDeleted(WayPointItem *)), this, SLOT(waypointdeleted())); refreshLocations(); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); } int WayPointCircle::type() const @@ -65,44 +63,44 @@ void WayPointCircle::paint(QPainter *painter, const QStyleOptionGraphicsItem *op QPointF p1; QPointF p2; - p1=QPointF(line.p1().x(),line.p1().y()+line.length()); - p2=QPointF(line.p1().x(),line.p1().y()-line.length()); + p1 = QPointF(line.p1().x(), line.p1().y() + line.length()); + p2 = QPointF(line.p1().x(), line.p1().y() - line.length()); QPen myPen = pen(); myPen.setColor(myColor); qreal arrowSize = 10; painter->setPen(myPen); - QBrush brush=painter->brush(); + QBrush brush = painter->brush(); painter->setBrush(myColor); - double angle =0; - if(!myClockWise) - angle+=M_PI; + double angle = 0; + if (!myClockWise) { + angle += M_PI; + } - QPointF arrowP1 = p1 + QPointF(sin(angle + M_PI / 3) * arrowSize, - cos(angle + M_PI / 3) * arrowSize); - QPointF arrowP2 = p1 + QPointF(sin(angle + M_PI - M_PI / 3) * arrowSize, - cos(angle + M_PI - M_PI / 3) * arrowSize); + QPointF arrowP1 = p1 + QPointF(sin(angle + M_PI / 3) * arrowSize, + cos(angle + M_PI / 3) * arrowSize); + QPointF arrowP2 = p1 + QPointF(sin(angle + M_PI - M_PI / 3) * arrowSize, + cos(angle + M_PI - M_PI / 3) * arrowSize); - QPointF arrowP21 = p2 + QPointF(sin(angle + M_PI + M_PI / 3) * arrowSize, - cos(angle + M_PI + M_PI / 3) * arrowSize); - QPointF arrowP22 = p2 + QPointF(sin(angle + M_PI + M_PI - M_PI / 3) * arrowSize, - cos(angle + M_PI + M_PI - M_PI / 3) * arrowSize); - - arrowHead.clear(); - arrowHead << p1 << arrowP1 << arrowP2; - painter->drawPolygon(arrowHead); - arrowHead.clear(); - arrowHead << p2 << arrowP21 << arrowP22; - painter->drawPolygon(arrowHead); - painter->translate(-line.length(),-line.length()); - painter->setBrush(brush); - painter->drawEllipse(this->rect()); + QPointF arrowP21 = p2 + QPointF(sin(angle + M_PI + M_PI / 3) * arrowSize, + cos(angle + M_PI + M_PI / 3) * arrowSize); + QPointF arrowP22 = p2 + QPointF(sin(angle + M_PI + M_PI - M_PI / 3) * arrowSize, + cos(angle + M_PI + M_PI - M_PI / 3) * arrowSize); + arrowHead.clear(); + arrowHead << p1 << arrowP1 << arrowP2; + painter->drawPolygon(arrowHead); + arrowHead.clear(); + arrowHead << p2 << arrowP21 << arrowP22; + painter->drawPolygon(arrowHead); + painter->translate(-line.length(), -line.length()); + painter->setBrush(brush); + painter->drawEllipse(this->rect()); } void WayPointCircle::refreshLocations() { - line=QLineF(my_center->pos(),my_radius->pos()); - this->setRect(my_center->pos().x(),my_center->pos().y(),2*line.length(),2*line.length()); + line = QLineF(my_center->pos(), my_radius->pos()); + this->setRect(my_center->pos().x(), my_center->pos().y(), 2 * line.length(), 2 * line.length()); this->update(); } @@ -115,5 +113,4 @@ void WayPointCircle::setOpacitySlot(qreal opacity) { setOpacity(opacity); } - } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.h index db839819d..ec6ac5a13 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file waypointcircle.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a circle connecting 2 waypoints -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file waypointcircle.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a circle connecting 2 waypoints + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef WAYPOINTCIRCLE_H #define WAYPOINTCIRCLE_H #include @@ -35,24 +35,22 @@ #include #include -namespace mapcontrol -{ - -class WayPointCircle:public QObject,public QGraphicsEllipseItem -{ - Q_OBJECT - Q_INTERFACES(QGraphicsItem) +namespace mapcontrol { +class WayPointCircle : public QObject, public QGraphicsEllipseItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) public: enum { Type = UserType + 9 }; - WayPointCircle(WayPointItem * center, WayPointItem * radius,bool clockwise,MapGraphicItem * map,QColor color=Qt::green); - WayPointCircle(HomeItem * center, WayPointItem * radius,bool clockwise,MapGraphicItem * map,QColor color=Qt::green); + WayPointCircle(WayPointItem *center, WayPointItem *radius, bool clockwise, MapGraphicItem *map, QColor color = Qt::green); + WayPointCircle(HomeItem *center, WayPointItem *radius, bool clockwise, MapGraphicItem *map, QColor color = Qt::green); int type() const; void setColor(const QColor &color) - { myColor = color; } + { + myColor = color; + } private: - QGraphicsItem * my_center; - QGraphicsItem * my_radius; - MapGraphicItem * my_map; + QGraphicsItem *my_center; + QGraphicsItem *my_radius; + MapGraphicItem *my_map; QPolygonF arrowHead; QColor myColor; bool myClockWise; diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp index 09e6c4d12..59c2ecb25 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp @@ -1,505 +1,485 @@ /** -****************************************************************************** -* -* @file waypointitem.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a WayPoint -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file waypointitem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "waypointitem.h" #include "homeitem.h" -namespace mapcontrol +namespace mapcontrol { +WayPointItem::WayPointItem(const internals::PointLatLng &coord, int const & altitude, MapGraphicItem *map, wptype type) : coord(coord), reached(false), description(""), shownumber(true), isDragging(false), altitude(altitude), map(map), myType(type) { -WayPointItem::WayPointItem(const internals::PointLatLng &coord,int const& altitude, MapGraphicItem *map,wptype type):coord(coord),reached(false),description(""),shownumber(true),isDragging(false),altitude(altitude),map(map),myType(type) - { - text=0; - numberI=0; - isMagic=false; - picture.load(QString::fromUtf8(":/markers/images/marker.png")); - number=WayPointItem::snumber; - ++WayPointItem::snumber; - this->setFlag(QGraphicsItem::ItemIsMovable,true); - this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); - this->setFlag(QGraphicsItem::ItemIsSelectable,true); - SetShowNumber(shownumber); - RefreshToolTip(); - RefreshPos(); - myHome=NULL; - QList list=map->childItems(); - foreach(QGraphicsItem * obj,list) - { - HomeItem* h=qgraphicsitem_cast (obj); - if(h) - myHome=h; - } - - if(myHome) - { - map->Projection()->offSetFromLatLngs(myHome->Coord(),coord,relativeCoord.distance,relativeCoord.bearing); - relativeCoord.altitudeRelative=Altitude()-myHome->Altitude(); - connect(myHome,SIGNAL(homePositionChanged(internals::PointLatLng,float)),this,SLOT(onHomePositionChanged(internals::PointLatLng,float))); - } - connect(this,SIGNAL(waypointdoubleclick(WayPointItem*)),map,SIGNAL(wpdoubleclicked(WayPointItem*))); - emit manualCoordChange(this); - connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); -} - -WayPointItem::WayPointItem(MapGraphicItem *map, bool magicwaypoint):reached(false),description(""),shownumber(true),isDragging(false),altitude(0),map(map) -{ - relativeCoord.bearing=0; - relativeCoord.distance=0; - relativeCoord.altitudeRelative=0; - myType=relative; - if(magicwaypoint) - { - isMagic=true; - picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker3.png")); - number=-1; - } - else - { - isMagic=false; - number=WayPointItem::snumber; - ++WayPointItem::snumber; - } - text=0; - numberI=0; - this->setFlag(QGraphicsItem::ItemIsMovable,true); - this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); - this->setFlag(QGraphicsItem::ItemIsSelectable,true); + text = 0; + numberI = 0; + isMagic = false; + picture.load(QString::fromUtf8(":/markers/images/marker.png")); + number = WayPointItem::snumber; + ++WayPointItem::snumber; + this->setFlag(QGraphicsItem::ItemIsMovable, true); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + this->setFlag(QGraphicsItem::ItemIsSelectable, true); SetShowNumber(shownumber); RefreshToolTip(); RefreshPos(); - myHome=NULL; - QList list=map->childItems(); - foreach(QGraphicsItem * obj,list) - { - HomeItem* h=qgraphicsitem_cast (obj); - if(h) - myHome=h; + myHome = NULL; + QList list = map->childItems(); + foreach(QGraphicsItem * obj, list) { + HomeItem *h = qgraphicsitem_cast (obj); + + if (h) { + myHome = h; + } } - if(myHome) - { - coord=map->Projection()->translate(myHome->Coord(),relativeCoord.distance,relativeCoord.bearing); - SetAltitude(myHome->Altitude()+relativeCoord.altitudeRelative); - connect(myHome,SIGNAL(homePositionChanged(internals::PointLatLng,float)),this,SLOT(onHomePositionChanged(internals::PointLatLng,float))); + if (myHome) { + map->Projection()->offSetFromLatLngs(myHome->Coord(), coord, relativeCoord.distance, relativeCoord.bearing); + relativeCoord.altitudeRelative = Altitude() - myHome->Altitude(); + connect(myHome, SIGNAL(homePositionChanged(internals::PointLatLng, float)), this, SLOT(onHomePositionChanged(internals::PointLatLng, float))); } - connect(this,SIGNAL(waypointdoubleclick(WayPointItem*)),map,SIGNAL(wpdoubleclicked(WayPointItem*))); + connect(this, SIGNAL(waypointdoubleclick(WayPointItem *)), map, SIGNAL(wpdoubleclicked(WayPointItem *))); emit manualCoordChange(this); - connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); } - WayPointItem::WayPointItem(const internals::PointLatLng &coord,int const& altitude, const QString &description, MapGraphicItem *map,wptype type):coord(coord),reached(false),description(description),shownumber(true),isDragging(false),altitude(altitude),map(map),myType(type) - { - text=0; - numberI=0; - isMagic=false; - picture.load(QString::fromUtf8(":/markers/images/marker.png")); - number=WayPointItem::snumber; + +WayPointItem::WayPointItem(MapGraphicItem *map, bool magicwaypoint) : reached(false), description(""), shownumber(true), isDragging(false), altitude(0), map(map) +{ + relativeCoord.bearing = 0; + relativeCoord.distance = 0; + relativeCoord.altitudeRelative = 0; + myType = relative; + if (magicwaypoint) { + isMagic = true; + picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker3.png")); + number = -1; + } else { + isMagic = false; + number = WayPointItem::snumber; ++WayPointItem::snumber; - this->setFlag(QGraphicsItem::ItemIsMovable,true); - this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); - this->setFlag(QGraphicsItem::ItemIsSelectable,true); - SetShowNumber(shownumber); - RefreshToolTip(); - RefreshPos(); - myHome=NULL; - QList list=map->childItems(); - foreach(QGraphicsItem * obj,list) - { - HomeItem* h=qgraphicsitem_cast (obj); - if(h) - myHome=h; - } - if(myHome) - { - map->Projection()->offSetFromLatLngs(myHome->Coord(),coord,relativeCoord.distance,relativeCoord.bearing); - relativeCoord.altitudeRelative=Altitude()-myHome->Altitude(); - connect(myHome,SIGNAL(homePositionChanged(internals::PointLatLng,float)),this,SLOT(onHomePositionChanged(internals::PointLatLng,float))); - } - connect(this,SIGNAL(waypointdoubleclick(WayPointItem*)),map,SIGNAL(wpdoubleclicked(WayPointItem*))); - emit manualCoordChange(this); - connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); } + text = 0; + numberI = 0; + this->setFlag(QGraphicsItem::ItemIsMovable, true); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + this->setFlag(QGraphicsItem::ItemIsSelectable, true); + SetShowNumber(shownumber); + RefreshToolTip(); + RefreshPos(); + myHome = NULL; + QList list = map->childItems(); + foreach(QGraphicsItem * obj, list) { + HomeItem *h = qgraphicsitem_cast (obj); - WayPointItem::WayPointItem(const distBearingAltitude &relativeCoordenate, const QString &description, MapGraphicItem *map):relativeCoord(relativeCoordenate),reached(false),description(description),shownumber(true),isDragging(false),map(map) - { - myHome=NULL; - QList list=map->childItems(); - foreach(QGraphicsItem * obj,list) - { - HomeItem* h=qgraphicsitem_cast (obj); - if(h) - myHome=h; - } - if(myHome) - { - connect(myHome,SIGNAL(homePositionChanged(internals::PointLatLng,float)),this,SLOT(onHomePositionChanged(internals::PointLatLng,float))); - coord=map->Projection()->translate(myHome->Coord(),relativeCoord.distance,relativeCoord.bearing); - SetAltitude(myHome->Altitude()+relativeCoord.altitudeRelative); - } - myType=relative; - text=0; - numberI=0; - isMagic=false; - picture.load(QString::fromUtf8(":/markers/images/marker.png")); - number=WayPointItem::snumber; - ++WayPointItem::snumber; - this->setFlag(QGraphicsItem::ItemIsMovable,true); - this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); - this->setFlag(QGraphicsItem::ItemIsSelectable,true); - SetShowNumber(shownumber); - RefreshToolTip(); - RefreshPos(); - connect(this,SIGNAL(waypointdoubleclick(WayPointItem*)),map,SIGNAL(wpdoubleclicked(WayPointItem*))); - emit manualCoordChange(this); - connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); - } - - void WayPointItem::setWPType(wptype type) - { - myType=type; - emit WPValuesChanged(this); - RefreshPos(); - RefreshToolTip(); - this->update(); - } - - QRectF WayPointItem::boundingRect() const - { - return QRectF(-picture.width()/2,-picture.height(),picture.width(),picture.height()); - } - void WayPointItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) - { - Q_UNUSED(option); - Q_UNUSED(widget); - painter->drawPixmap(-picture.width()/2,-picture.height(),picture); - painter->setPen(Qt::green); - if(this->isSelected()) - painter->drawRect(QRectF(-picture.width()/2,-picture.height(),picture.width()-1,picture.height()-1)); - } - void WayPointItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent *event) - { - if(event->button()==Qt::LeftButton) - { - emit waypointdoubleclick(this); + if (h) { + myHome = h; } } - void WayPointItem::mousePressEvent(QGraphicsSceneMouseEvent *event) - { - if(event->button()==Qt::LeftButton) - { - text=new QGraphicsSimpleTextItem(this); - textBG=new QGraphicsRectItem(this); + if (myHome) { + coord = map->Projection()->translate(myHome->Coord(), relativeCoord.distance, relativeCoord.bearing); + SetAltitude(myHome->Altitude() + relativeCoord.altitudeRelative); + connect(myHome, SIGNAL(homePositionChanged(internals::PointLatLng, float)), this, SLOT(onHomePositionChanged(internals::PointLatLng, float))); + } + connect(this, SIGNAL(waypointdoubleclick(WayPointItem *)), map, SIGNAL(wpdoubleclicked(WayPointItem *))); + emit manualCoordChange(this); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); +} +WayPointItem::WayPointItem(const internals::PointLatLng &coord, int const & altitude, const QString &description, MapGraphicItem *map, wptype type) : coord(coord), reached(false), description(description), shownumber(true), isDragging(false), altitude(altitude), map(map), myType(type) +{ + text = 0; + numberI = 0; + isMagic = false; + picture.load(QString::fromUtf8(":/markers/images/marker.png")); + number = WayPointItem::snumber; + ++WayPointItem::snumber; + this->setFlag(QGraphicsItem::ItemIsMovable, true); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + this->setFlag(QGraphicsItem::ItemIsSelectable, true); + SetShowNumber(shownumber); + RefreshToolTip(); + RefreshPos(); + myHome = NULL; + QList list = map->childItems(); + foreach(QGraphicsItem * obj, list) { + HomeItem *h = qgraphicsitem_cast (obj); + + if (h) { + myHome = h; + } + } + if (myHome) { + map->Projection()->offSetFromLatLngs(myHome->Coord(), coord, relativeCoord.distance, relativeCoord.bearing); + relativeCoord.altitudeRelative = Altitude() - myHome->Altitude(); + connect(myHome, SIGNAL(homePositionChanged(internals::PointLatLng, float)), this, SLOT(onHomePositionChanged(internals::PointLatLng, float))); + } + connect(this, SIGNAL(waypointdoubleclick(WayPointItem *)), map, SIGNAL(wpdoubleclicked(WayPointItem *))); + emit manualCoordChange(this); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); +} + +WayPointItem::WayPointItem(const distBearingAltitude &relativeCoordenate, const QString &description, MapGraphicItem *map) : relativeCoord(relativeCoordenate), reached(false), description(description), shownumber(true), isDragging(false), map(map) +{ + myHome = NULL; + QList list = map->childItems(); + foreach(QGraphicsItem * obj, list) { + HomeItem *h = qgraphicsitem_cast (obj); + + if (h) { + myHome = h; + } + } + if (myHome) { + connect(myHome, SIGNAL(homePositionChanged(internals::PointLatLng, float)), this, SLOT(onHomePositionChanged(internals::PointLatLng, float))); + coord = map->Projection()->translate(myHome->Coord(), relativeCoord.distance, relativeCoord.bearing); + SetAltitude(myHome->Altitude() + relativeCoord.altitudeRelative); + } + myType = relative; + text = 0; + numberI = 0; + isMagic = false; + picture.load(QString::fromUtf8(":/markers/images/marker.png")); + number = WayPointItem::snumber; + ++WayPointItem::snumber; + this->setFlag(QGraphicsItem::ItemIsMovable, true); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + this->setFlag(QGraphicsItem::ItemIsSelectable, true); + SetShowNumber(shownumber); + RefreshToolTip(); + RefreshPos(); + connect(this, SIGNAL(waypointdoubleclick(WayPointItem *)), map, SIGNAL(wpdoubleclicked(WayPointItem *))); + emit manualCoordChange(this); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); +} + +void WayPointItem::setWPType(wptype type) +{ + myType = type; + emit WPValuesChanged(this); + RefreshPos(); + RefreshToolTip(); + this->update(); +} + +QRectF WayPointItem::boundingRect() const +{ + return QRectF(-picture.width() / 2, -picture.height(), picture.width(), picture.height()); +} +void WayPointItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) +{ + Q_UNUSED(option); + Q_UNUSED(widget); + painter->drawPixmap(-picture.width() / 2, -picture.height(), picture); + painter->setPen(Qt::green); + if (this->isSelected()) { + painter->drawRect(QRectF(-picture.width() / 2, -picture.height(), picture.width() - 1, picture.height() - 1)); + } +} +void WayPointItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent *event) +{ + if (event->button() == Qt::LeftButton) { + emit waypointdoubleclick(this); + } +} + +void WayPointItem::mousePressEvent(QGraphicsSceneMouseEvent *event) +{ + if (event->button() == Qt::LeftButton) { + text = new QGraphicsSimpleTextItem(this); + textBG = new QGraphicsRectItem(this); textBG->setBrush(Qt::yellow); text->setPen(QPen(Qt::red)); - text->setPos(10,-picture.height()); - textBG->setPos(10,-picture.height()); + text->setPos(10, -picture.height()); + textBG->setPos(10, -picture.height()); text->setZValue(3); RefreshToolTip(); - isDragging=true; + isDragging = true; } - QGraphicsItem::mousePressEvent(event); - } - void WayPointItem::mouseReleaseEvent(QGraphicsSceneMouseEvent *event) - { - if(event->button()==Qt::LeftButton) - { - if(text) - { - delete text; - text=NULL; - } - if(textBG) - { - delete textBG; - textBG=NULL; - } - - isDragging=false; - RefreshToolTip(); - emit manualCoordChange(this); - emit localPositionChanged(this->pos(),this); - emit WPValuesChanged(this); - } - QGraphicsItem::mouseReleaseEvent(event); - } - void WayPointItem::mouseMoveEvent(QGraphicsSceneMouseEvent *event) - { - - if(isDragging) - { - coord=map->FromLocalToLatLng(this->pos().x(),this->pos().y()); - QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); - if(myHome) - { - map->Projection()->offSetFromLatLngs(myHome->Coord(),coord,relativeCoord.distance,relativeCoord.bearing); - } - QString relativeCoord_str = QString::number(relativeCoord.distance) + "m " + QString::number(relativeCoord.bearing*180/M_PI)+"deg"; - text->setText(coord_str+"\n"+relativeCoord_str); - textBG->setRect(text->boundingRect()); - emit localPositionChanged(this->pos(),this); - emit WPValuesChanged(this); - } - QGraphicsItem::mouseMoveEvent(event); - } - void WayPointItem::SetAltitude(const float &value) - { - if(altitude==value) - return; - altitude=value; - if(myHome) - { - relativeCoord.altitudeRelative=altitude-myHome->Altitude(); - } - RefreshToolTip(); - emit WPValuesChanged(this); - this->update(); - } - - void WayPointItem::setRelativeCoord(distBearingAltitude value) - { - if(qAbs(value.distance-relativeCoord.distance)<0.1 - && qAbs(value.bearing-relativeCoord.bearing)<0.01 && value.altitudeRelative==relativeCoord.altitudeRelative) - return; - relativeCoord=value; - if(myHome) - { - SetCoord(map->Projection()->translate(myHome->Coord(),relativeCoord.distance,relativeCoord.bearing)); - SetAltitude(myHome->Altitude()+relativeCoord.altitudeRelative); - } - RefreshPos(); - RefreshToolTip(); - emit WPValuesChanged(this); - this->update(); - } - - void WayPointItem::SetCoord(const internals::PointLatLng &value) - { - qDebug()<<"1 SetCoord("<Projection()->offSetFromLatLngs(myHome->Coord(),Coord(),back.distance,back.bearing); - if(qAbs(back.bearing-relativeCoord.bearing)>0.01 || qAbs(back.distance-relativeCoord.distance)>0.1) - { - qDebug()<<"4 setCoord-relative coordinates where also updated"; - relativeCoord=back; - } - emit WPValuesChanged(this); - RefreshPos(); - RefreshToolTip(); - this->update(); - qDebug()<<"5 setCoord EXIT"; - } - void WayPointItem::SetDescription(const QString &value) - { - if(description==value) - return; - description=value; - RefreshToolTip(); - emit WPValuesChanged(this); - this->update(); - } - void WayPointItem::SetNumber(const int &value) - { - int oldnumber=number; - number=value; - RefreshToolTip(); - numberI->setText(QString::number(numberAdjusted())); - numberIBG->setRect(numberI->boundingRect().adjusted(-2,0,1,0)); - this->update(); - emit WPNumberChanged(oldnumber,value,this); - } - void WayPointItem::SetReached(const bool &value) - { - reached=value; - emit WPValuesChanged(this); - if(value) - picture.load(QString::fromUtf8(":/markers/images/bigMarkerGreen.png")); - else - { - if(!isMagic) - { - if(this->flags() & QGraphicsItem::ItemIsMovable==QGraphicsItem::ItemIsMovable) - picture.load(QString::fromUtf8(":/markers/images/marker.png")); - else - picture.load(QString::fromUtf8(":/markers/images/waypoint_marker2.png")); - } - else - { - picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker3.png")); - } - } - this->update(); - - } - void WayPointItem::SetShowNumber(const bool &value) - { - shownumber=value; - if((numberI==0) && value) - { - numberI=new QGraphicsSimpleTextItem(this); - numberIBG=new QGraphicsRectItem(this); - numberIBG->setBrush(Qt::white); - numberIBG->setOpacity(0.5); - numberI->setZValue(3); - numberI->setPen(QPen(Qt::blue)); - numberI->setPos(0,-13-picture.height()); - numberIBG->setPos(0,-13-picture.height()); - numberI->setText(QString::number(numberAdjusted())); - numberIBG->setRect(numberI->boundingRect().adjusted(-2,0,1,0)); - } - else if (!value && numberI) - { - delete numberI; - delete numberIBG; - } - this->update(); - } - void WayPointItem::WPDeleted(const int &onumber,WayPointItem *waypoint) - { - Q_UNUSED(waypoint); - int n=number; - if(number>onumber) SetNumber(--n); - } - void WayPointItem::WPInserted(const int &onumber, WayPointItem *waypoint) - { - if(Number()==-1) - return; - - if(waypoint!=this) - { - if(onumber<=number) SetNumber(++number); - } - } - - void WayPointItem::onHomePositionChanged(internals::PointLatLng homepos, float homeAltitude) - { - if(myType==relative) - { - coord=map->Projection()->translate(homepos,relativeCoord.distance,relativeCoord.bearing); - SetAltitude(relativeCoord.altitudeRelative+homeAltitude); - emit WPValuesChanged(this); - RefreshPos(); - RefreshToolTip(); - this->update(); - } - else - { - if(myHome) - { - map->Projection()->offSetFromLatLngs(myHome->Coord(),coord,relativeCoord.distance,relativeCoord.bearing); - relativeCoord.altitudeRelative=Altitude()-homeAltitude; - } - emit WPValuesChanged(this); - } - } - - void WayPointItem::WPRenumbered(const int &oldnumber, const int &newnumber, WayPointItem *waypoint) - { - if (waypoint!=this) - { - if(((oldnumber>number) && (newnumber<=number))) - { - SetNumber(++number); - } - else if (((oldnumbernumber))) - { - SetNumber(--number); - } - else if (newnumber==number) - { - SetNumber(++number); - } - } - } - int WayPointItem::type() const - { - // Enable the use of qgraphicsitem_cast with this item. - return Type; - } - - WayPointItem::~WayPointItem() - { - emit aboutToBeDeleted(this); - --WayPointItem::snumber; - } - void WayPointItem::RefreshPos() - { - core::Point point=map->FromLatLngToLocal(coord); - this->setPos(point.X(),point.Y()); - emit localPositionChanged(this->pos(),this); - } - - void WayPointItem::setOpacitySlot(qreal opacity) - { - setOpacity(opacity); - } - void WayPointItem::RefreshToolTip() - { - QString type_str; - if(myType==relative) - type_str="Relative"; - else - type_str="Absolute"; - QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); - QString relativeCoord_str = " Distance:" + QString::number(relativeCoord.distance) + " Bearing:" + QString::number(relativeCoord.bearing*180/M_PI); - QString relativeAltitude_str=QString::number(relativeCoord.altitudeRelative); - if(Number()!=-1) - setToolTip(QString("WayPoint Number:%1\nDescription:%2\nCoordinate:%4\nFrom Home:%5\nRelative altitude:%6\nAltitude:%7\nType:%8\n%9").arg(QString::number(numberAdjusted())).arg(description).arg(coord_str).arg(relativeCoord_str).arg(relativeAltitude_str).arg(QString::number(altitude)).arg(type_str).arg(myCustomString)); - else - setToolTip(QString("Magic WayPoint\nCoordinate:%1\nFrom Home:%2\nAltitude:%3\nType:%4\n%5").arg(coord_str).arg(relativeCoord_str).arg(QString::number(altitude)).arg(type_str).arg(myCustomString)); - } - - void WayPointItem::setFlag(QGraphicsItem::GraphicsItemFlag flag, bool enabled) - { - if(isMagic) - { - QGraphicsItem::setFlag(flag,enabled); - return; - } - else if(flag==QGraphicsItem::ItemIsMovable) - { - if(enabled) - picture.load(QString::fromUtf8(":/markers/images/marker.png")); - else - picture.load(QString::fromUtf8(":/markers/images/waypoint_marker2.png")); - } - QGraphicsItem::setFlag(flag,enabled); - } - - int WayPointItem::snumber=0; + QGraphicsItem::mousePressEvent(event); +} +void WayPointItem::mouseReleaseEvent(QGraphicsSceneMouseEvent *event) +{ + if (event->button() == Qt::LeftButton) { + if (text) { + delete text; + text = NULL; + } + if (textBG) { + delete textBG; + textBG = NULL; + } + + isDragging = false; + RefreshToolTip(); + emit manualCoordChange(this); + emit localPositionChanged(this->pos(), this); + emit WPValuesChanged(this); + } + QGraphicsItem::mouseReleaseEvent(event); +} +void WayPointItem::mouseMoveEvent(QGraphicsSceneMouseEvent *event) +{ + if (isDragging) { + coord = map->FromLocalToLatLng(this->pos().x(), this->pos().y()); + QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); + if (myHome) { + map->Projection()->offSetFromLatLngs(myHome->Coord(), coord, relativeCoord.distance, relativeCoord.bearing); + } + QString relativeCoord_str = QString::number(relativeCoord.distance) + "m " + QString::number(relativeCoord.bearing * 180 / M_PI) + "deg"; + text->setText(coord_str + "\n" + relativeCoord_str); + textBG->setRect(text->boundingRect()); + emit localPositionChanged(this->pos(), this); + emit WPValuesChanged(this); + } + QGraphicsItem::mouseMoveEvent(event); +} +void WayPointItem::SetAltitude(const float &value) +{ + if (altitude == value) { + return; + } + altitude = value; + if (myHome) { + relativeCoord.altitudeRelative = altitude - myHome->Altitude(); + } + RefreshToolTip(); + emit WPValuesChanged(this); + this->update(); +} + +void WayPointItem::setRelativeCoord(distBearingAltitude value) +{ + if (qAbs(value.distance - relativeCoord.distance) < 0.1 + && qAbs(value.bearing - relativeCoord.bearing) < 0.01 && value.altitudeRelative == relativeCoord.altitudeRelative) { + return; + } + relativeCoord = value; + if (myHome) { + SetCoord(map->Projection()->translate(myHome->Coord(), relativeCoord.distance, relativeCoord.bearing)); + SetAltitude(myHome->Altitude() + relativeCoord.altitudeRelative); + } + RefreshPos(); + RefreshToolTip(); + emit WPValuesChanged(this); + this->update(); +} + +void WayPointItem::SetCoord(const internals::PointLatLng &value) +{ + qDebug() << "1 SetCoord(" << value.Lat() << "," << value.Lng() << ")" << "OLD:" << Coord().Lat() << "," << Coord().Lng(); + if (qAbs(Coord().Lat() - value.Lat()) < 0.0001 && qAbs(Coord().Lng() - value.Lng()) < 0.0001) { + qDebug() << "2 SetCoord nothing changed returning"; + return; + } + qDebug() << "3 setCoord there were changes"; + coord = value; + distBearingAltitude back = relativeCoord; + if (myHome) { + map->Projection()->offSetFromLatLngs(myHome->Coord(), Coord(), back.distance, back.bearing); + } + if (qAbs(back.bearing - relativeCoord.bearing) > 0.01 || qAbs(back.distance - relativeCoord.distance) > 0.1) { + qDebug() << "4 setCoord-relative coordinates where also updated"; + relativeCoord = back; + } + emit WPValuesChanged(this); + RefreshPos(); + RefreshToolTip(); + this->update(); + qDebug() << "5 setCoord EXIT"; +} +void WayPointItem::SetDescription(const QString &value) +{ + if (description == value) { + return; + } + description = value; + RefreshToolTip(); + emit WPValuesChanged(this); + this->update(); +} +void WayPointItem::SetNumber(const int &value) +{ + int oldnumber = number; + + number = value; + RefreshToolTip(); + numberI->setText(QString::number(numberAdjusted())); + numberIBG->setRect(numberI->boundingRect().adjusted(-2, 0, 1, 0)); + this->update(); + emit WPNumberChanged(oldnumber, value, this); +} +void WayPointItem::SetReached(const bool &value) +{ + reached = value; + emit WPValuesChanged(this); + if (value) { + picture.load(QString::fromUtf8(":/markers/images/bigMarkerGreen.png")); + } else { + if (!isMagic) { + if (this->flags() & QGraphicsItem::ItemIsMovable == QGraphicsItem::ItemIsMovable) { + picture.load(QString::fromUtf8(":/markers/images/marker.png")); + } else { + picture.load(QString::fromUtf8(":/markers/images/waypoint_marker2.png")); + } + } else { + picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker3.png")); + } + } + this->update(); +} +void WayPointItem::SetShowNumber(const bool &value) +{ + shownumber = value; + if ((numberI == 0) && value) { + numberI = new QGraphicsSimpleTextItem(this); + numberIBG = new QGraphicsRectItem(this); + numberIBG->setBrush(Qt::white); + numberIBG->setOpacity(0.5); + numberI->setZValue(3); + numberI->setPen(QPen(Qt::blue)); + numberI->setPos(0, -13 - picture.height()); + numberIBG->setPos(0, -13 - picture.height()); + numberI->setText(QString::number(numberAdjusted())); + numberIBG->setRect(numberI->boundingRect().adjusted(-2, 0, 1, 0)); + } else if (!value && numberI) { + delete numberI; + delete numberIBG; + } + this->update(); +} +void WayPointItem::WPDeleted(const int &onumber, WayPointItem *waypoint) +{ + Q_UNUSED(waypoint); + int n = number; + if (number > onumber) { + SetNumber(--n); + } +} +void WayPointItem::WPInserted(const int &onumber, WayPointItem *waypoint) +{ + if (Number() == -1) { + return; + } + + if (waypoint != this) { + if (onumber <= number) { + SetNumber(++number); + } + } +} + +void WayPointItem::onHomePositionChanged(internals::PointLatLng homepos, float homeAltitude) +{ + if (myType == relative) { + coord = map->Projection()->translate(homepos, relativeCoord.distance, relativeCoord.bearing); + SetAltitude(relativeCoord.altitudeRelative + homeAltitude); + emit WPValuesChanged(this); + RefreshPos(); + RefreshToolTip(); + this->update(); + } else { + if (myHome) { + map->Projection()->offSetFromLatLngs(myHome->Coord(), coord, relativeCoord.distance, relativeCoord.bearing); + relativeCoord.altitudeRelative = Altitude() - homeAltitude; + } + emit WPValuesChanged(this); + } +} + +void WayPointItem::WPRenumbered(const int &oldnumber, const int &newnumber, WayPointItem *waypoint) +{ + if (waypoint != this) { + if (((oldnumber > number) && (newnumber <= number))) { + SetNumber(++number); + } else if (((oldnumber < number) && (newnumber > number))) { + SetNumber(--number); + } else if (newnumber == number) { + SetNumber(++number); + } + } +} +int WayPointItem::type() const +{ + // Enable the use of qgraphicsitem_cast with this item. + return Type; +} + +WayPointItem::~WayPointItem() +{ + emit aboutToBeDeleted(this); + + --WayPointItem::snumber; +} +void WayPointItem::RefreshPos() +{ + core::Point point = map->FromLatLngToLocal(coord); + + this->setPos(point.X(), point.Y()); + emit localPositionChanged(this->pos(), this); +} + +void WayPointItem::setOpacitySlot(qreal opacity) +{ + setOpacity(opacity); +} +void WayPointItem::RefreshToolTip() +{ + QString type_str; + + if (myType == relative) { + type_str = "Relative"; + } else { + type_str = "Absolute"; + } + QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); + QString relativeCoord_str = " Distance:" + QString::number(relativeCoord.distance) + " Bearing:" + QString::number(relativeCoord.bearing * 180 / M_PI); + QString relativeAltitude_str = QString::number(relativeCoord.altitudeRelative); + if (Number() != -1) { + setToolTip(QString("WayPoint Number:%1\nDescription:%2\nCoordinate:%4\nFrom Home:%5\nRelative altitude:%6\nAltitude:%7\nType:%8\n%9").arg(QString::number(numberAdjusted())).arg(description).arg(coord_str).arg(relativeCoord_str).arg(relativeAltitude_str).arg(QString::number(altitude)).arg(type_str).arg(myCustomString)); + } else { + setToolTip(QString("Magic WayPoint\nCoordinate:%1\nFrom Home:%2\nAltitude:%3\nType:%4\n%5").arg(coord_str).arg(relativeCoord_str).arg(QString::number(altitude)).arg(type_str).arg(myCustomString)); + } +} + +void WayPointItem::setFlag(QGraphicsItem::GraphicsItemFlag flag, bool enabled) +{ + if (isMagic) { + QGraphicsItem::setFlag(flag, enabled); + return; + } else if (flag == QGraphicsItem::ItemIsMovable) { + if (enabled) { + picture.load(QString::fromUtf8(":/markers/images/marker.png")); + } else { + picture.load(QString::fromUtf8(":/markers/images/waypoint_marker2.png")); + } + } + QGraphicsItem::setFlag(flag, enabled); +} + +int WayPointItem::snumber = 0; } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h index d974a78fc..e97c76255 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file waypointitem.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a WayPoint -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file waypointitem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef WAYPOINTITEM_H #define WAYPOINTITEM_H @@ -35,208 +35,243 @@ #include #include -namespace mapcontrol -{ -struct distBearingAltitude -{ +namespace mapcontrol { +struct distBearingAltitude { double distance; double bearing; - float altitudeRelative; - double bearingToDegrees(){return bearing*180/M_PI;} - void setBearingFromDegrees(double degrees){bearing=degrees*M_PI/180;} + float altitudeRelative; + double bearingToDegrees() + { + return bearing * 180 / M_PI; + } + void setBearingFromDegrees(double degrees) + { + bearing = degrees * M_PI / 180; + } }; class HomeItem; /** -* @brief A QGraphicsItem representing a WayPoint -* -* @class WayPointItem waypointitem.h "waypointitem.h" -*/ -class WayPointItem:public QObject,public QGraphicsItem -{ - Q_OBJECT - Q_INTERFACES(QGraphicsItem) + * @brief A QGraphicsItem representing a WayPoint + * + * @class WayPointItem waypointitem.h "waypointitem.h" + */ +class WayPointItem : public QObject, public QGraphicsItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) public: enum { Type = UserType + 1 }; - enum wptype {absolute,relative}; + enum wptype { absolute, relative }; /** - * @brief Constructer - * - * @param coord coordinates in LatLng of the Waypoint - * @param altitude altitude of the WayPoint - * @param map pointer to map to use - * @return - */ - WayPointItem(internals::PointLatLng const& coord,int const& altitude,MapGraphicItem* map,wptype type=absolute); - WayPointItem(MapGraphicItem* map,bool magicwaypoint); + * @brief Constructer + * + * @param coord coordinates in LatLng of the Waypoint + * @param altitude altitude of the WayPoint + * @param map pointer to map to use + * @return + */ + WayPointItem(internals::PointLatLng const & coord, int const & altitude, MapGraphicItem *map, wptype type = absolute); + WayPointItem(MapGraphicItem *map, bool magicwaypoint); /** - * @brief Constructer - * - * @param coord coordinates in LatLng of the WayPoint - * @param altitude altitude of the WayPoint - * @param description description fo the WayPoint - * @param map pointer to map to use - * @return - */ - WayPointItem(internals::PointLatLng const& coord,int const& altitude,QString const& description,MapGraphicItem* map,wptype type=absolute); - WayPointItem(distBearingAltitude const& relativeCoord,QString const& description,MapGraphicItem* map); + * @brief Constructer + * + * @param coord coordinates in LatLng of the WayPoint + * @param altitude altitude of the WayPoint + * @param description description fo the WayPoint + * @param map pointer to map to use + * @return + */ + WayPointItem(internals::PointLatLng const & coord, int const & altitude, QString const & description, MapGraphicItem *map, wptype type = absolute); + WayPointItem(distBearingAltitude const & relativeCoord, QString const & description, MapGraphicItem *map); /** - * @brief Returns the WayPoint description - * - * @return QString - */ - QString Description(){return description;} + * @brief Returns the WayPoint description + * + * @return QString + */ + QString Description() + { + return description; + } /** - * @brief Sets the WayPoint description - * - * @param value - */ - void SetDescription(QString const& value); + * @brief Sets the WayPoint description + * + * @param value + */ + void SetDescription(QString const & value); /** - * @brief Returns true if WayPoint is Reached - * - * @return bool - */ - bool Reached(){return reached;} + * @brief Returns true if WayPoint is Reached + * + * @return bool + */ + bool Reached() + { + return reached; + } /** - * @brief Sets if WayPoint is Reached - * - * @param value - */ - void SetReached(bool const& value); + * @brief Sets if WayPoint is Reached + * + * @param value + */ + void SetReached(bool const & value); /** - * @brief Returns the WayPoint number - * - */ - int Number(){return number;} - int numberAdjusted(){return number+1;} + * @brief Returns the WayPoint number + * + */ + int Number() + { + return number; + } + int numberAdjusted() + { + return number + 1; + } /** - * @brief Sets WayPoint number - * - * @param value - */ - void SetNumber(int const& value); + * @brief Sets WayPoint number + * + * @param value + */ + void SetNumber(int const & value); /** - * @brief Returns WayPoint LatLng coordinate - * - */ - internals::PointLatLng Coord(){return coord;} + * @brief Returns WayPoint LatLng coordinate + * + */ + internals::PointLatLng Coord() + { + return coord; + } /** - * @brief Sets WayPoint LatLng coordinate - * - * @param value - */ - void SetCoord(internals::PointLatLng const& value); + * @brief Sets WayPoint LatLng coordinate + * + * @param value + */ + void SetCoord(internals::PointLatLng const & value); /** - * @brief Used if WayPoint number is to be drawn on screen - * - */ - bool ShowNumber(){return shownumber;} + * @brief Used if WayPoint number is to be drawn on screen + * + */ + bool ShowNumber() + { + return shownumber; + } /** - * @brief Used to set if WayPoint number is to be drawn on screen - * - * @param value - */ - void SetShowNumber(bool const& value); + * @brief Used to set if WayPoint number is to be drawn on screen + * + * @param value + */ + void SetShowNumber(bool const & value); /** - * @brief Returns the WayPoint altitude - * - * @return int - */ - float Altitude(){return altitude;} + * @brief Returns the WayPoint altitude + * + * @return int + */ + float Altitude() + { + return altitude; + } /** - * @brief Sets the WayPoint Altitude - * - * @param value - */ + * @brief Sets the WayPoint Altitude + * + * @param value + */ void SetAltitude(const float &value); void setRelativeCoord(distBearingAltitude value); - distBearingAltitude getRelativeCoord(){return relativeCoord;} + distBearingAltitude getRelativeCoord() + { + return relativeCoord; + } int type() const; QRectF boundingRect() const; void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, - QWidget *widget); + QWidget *widget); void RefreshToolTip(); QPixmap picture; - QString customString(){return myCustomString;} - void setCustomString(QString arg){myCustomString=arg;} + QString customString() + { + return myCustomString; + } + void setCustomString(QString arg) + { + myCustomString = arg; + } void setFlag(GraphicsItemFlag flag, bool enabled); -~WayPointItem(); + ~WayPointItem(); static int snumber; void setWPType(wptype type); - wptype WPType(){return myType;} + wptype WPType() + { + return myType; + } protected: - void mouseMoveEvent ( QGraphicsSceneMouseEvent * event ); - void mousePressEvent ( QGraphicsSceneMouseEvent * event ); - void mouseReleaseEvent ( QGraphicsSceneMouseEvent * event ); + void mouseMoveEvent(QGraphicsSceneMouseEvent *event); + void mousePressEvent(QGraphicsSceneMouseEvent *event); + void mouseReleaseEvent(QGraphicsSceneMouseEvent *event); void mouseDoubleClickEvent(QGraphicsSceneMouseEvent *event); private: - internals::PointLatLng coord;//coordinates of this WayPoint + internals::PointLatLng coord; // coordinates of this WayPoint distBearingAltitude relativeCoord; bool reached; QString description; bool shownumber; bool isDragging; float altitude; - MapGraphicItem* map; + MapGraphicItem *map; int number; bool isMagic; - QGraphicsSimpleTextItem* text; - QGraphicsRectItem* textBG; - QGraphicsSimpleTextItem* numberI; - QGraphicsRectItem* numberIBG; + QGraphicsSimpleTextItem *text; + QGraphicsRectItem *textBG; + QGraphicsSimpleTextItem *numberI; + QGraphicsRectItem *numberIBG; QTransform transf; - HomeItem * myHome; + HomeItem *myHome; wptype myType; QString myCustomString; public slots: /** - * @brief Called when a WayPoint is deleted - * - * @param number number of the WayPoint that was deleted - */ - void WPDeleted(int const& number,WayPointItem *waypoint); + * @brief Called when a WayPoint is deleted + * + * @param number number of the WayPoint that was deleted + */ + void WPDeleted(int const & number, WayPointItem *waypoint); /** - * @brief Called when a WayPoint is renumbered - * - * @param oldnumber the old WayPoint number - * @param newnumber the new WayPoint number - * @param waypoint a pointer to the WayPoint renumbered - */ - void WPRenumbered(int const& oldnumber,int const& newnumber,WayPointItem* waypoint); + * @brief Called when a WayPoint is renumbered + * + * @param oldnumber the old WayPoint number + * @param newnumber the new WayPoint number + * @param waypoint a pointer to the WayPoint renumbered + */ + void WPRenumbered(int const & oldnumber, int const & newnumber, WayPointItem *waypoint); /** - * @brief Called when a WayPoint is inserted - * - * @param number the number of the WayPoint - * @param waypoint a pointer to the WayPoint inserted - */ - void WPInserted(int const& number,WayPointItem* waypoint); + * @brief Called when a WayPoint is inserted + * + * @param number the number of the WayPoint + * @param waypoint a pointer to the WayPoint inserted + */ + void WPInserted(int const & number, WayPointItem *waypoint); - void onHomePositionChanged(internals::PointLatLng,float altitude); + void onHomePositionChanged(internals::PointLatLng, float altitude); void RefreshPos(); void setOpacitySlot(qreal opacity); signals: /** - * @brief fires when this WayPoint number changes (not fired if due to a auto-renumbering) - * - * @param oldnumber this WayPoint old number - * @param newnumber this WayPoint new number - * @param waypoint a pointer to this WayPoint - */ - void WPNumberChanged(int const& oldnumber,int const& newnumber,WayPointItem* waypoint); + * @brief fires when this WayPoint number changes (not fired if due to a auto-renumbering) + * + * @param oldnumber this WayPoint old number + * @param newnumber this WayPoint new number + * @param waypoint a pointer to this WayPoint + */ + void WPNumberChanged(int const & oldnumber, int const & newnumber, WayPointItem *waypoint); /** - * @brief Fired when the description, altitude or coordinates change - * - * @param waypoint a pointer to this WayPoint - */ + * @brief Fired when the description, altitude or coordinates change + * + * @param waypoint a pointer to this WayPoint + */ - void WPValuesChanged(WayPointItem* waypoint); - void waypointdoubleclick(WayPointItem* waypoint); - void localPositionChanged(QPointF point,WayPointItem* waypoint); + void WPValuesChanged(WayPointItem *waypoint); + void waypointdoubleclick(WayPointItem *waypoint); + void localPositionChanged(QPointF point, WayPointItem *waypoint); void manualCoordChange(WayPointItem *); void aboutToBeDeleted(WayPointItem *); }; diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.cpp index ad245cbc1..eb750a1e6 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.cpp @@ -1,66 +1,67 @@ /** -****************************************************************************** -* -* @file waypointline.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a line connecting 2 waypoints -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file waypointline.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a line connecting 2 waypoints + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #include "waypointline.h" #include #include "homeitem.h" -namespace mapcontrol +namespace mapcontrol { +WayPointLine::WayPointLine(WayPointItem *from, WayPointItem *to, MapGraphicItem *map, QColor color) : source(from), + destination(to), my_map(map), QGraphicsLineItem(map), myColor(color) { -WayPointLine::WayPointLine(WayPointItem *from, WayPointItem *to, MapGraphicItem *map,QColor color):source(from), - destination(to),my_map(map),QGraphicsLineItem(map),myColor(color) -{ - this->setLine(to->pos().x(),to->pos().y(),from->pos().x(),from->pos().y()); - connect(from,SIGNAL(localPositionChanged(QPointF,WayPointItem*)),this,SLOT(refreshLocations())); - connect(to,SIGNAL(localPositionChanged(QPointF,WayPointItem*)),this,SLOT(refreshLocations())); - connect(from,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); - connect(to,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); - if(myColor==Qt::green) + this->setLine(to->pos().x(), to->pos().y(), from->pos().x(), from->pos().y()); + connect(from, SIGNAL(localPositionChanged(QPointF, WayPointItem *)), this, SLOT(refreshLocations())); + connect(to, SIGNAL(localPositionChanged(QPointF, WayPointItem *)), this, SLOT(refreshLocations())); + connect(from, SIGNAL(aboutToBeDeleted(WayPointItem *)), this, SLOT(waypointdeleted())); + connect(to, SIGNAL(aboutToBeDeleted(WayPointItem *)), this, SLOT(waypointdeleted())); + if (myColor == Qt::green) { this->setZValue(10); - else if(myColor==Qt::yellow) + } else if (myColor == Qt::yellow) { this->setZValue(9); - else if(myColor==Qt::red) + } else if (myColor == Qt::red) { this->setZValue(8); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); + } + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); } -WayPointLine::WayPointLine(HomeItem *from, WayPointItem *to, MapGraphicItem *map, QColor color):source(from), - destination(to),my_map(map),QGraphicsLineItem(map),myColor(color) +WayPointLine::WayPointLine(HomeItem *from, WayPointItem *to, MapGraphicItem *map, QColor color) : source(from), + destination(to), my_map(map), QGraphicsLineItem(map), myColor(color) { - this->setLine(to->pos().x(),to->pos().y(),from->pos().x(),from->pos().y()); - connect(from,SIGNAL(homePositionChanged(internals::PointLatLng,float)),this,SLOT(refreshLocations())); - connect(to,SIGNAL(localPositionChanged(QPointF,WayPointItem*)),this,SLOT(refreshLocations())); - connect(to,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); - if(myColor==Qt::green) + this->setLine(to->pos().x(), to->pos().y(), from->pos().x(), from->pos().y()); + connect(from, SIGNAL(homePositionChanged(internals::PointLatLng, float)), this, SLOT(refreshLocations())); + connect(to, SIGNAL(localPositionChanged(QPointF, WayPointItem *)), this, SLOT(refreshLocations())); + connect(to, SIGNAL(aboutToBeDeleted(WayPointItem *)), this, SLOT(waypointdeleted())); + if (myColor == Qt::green) { this->setZValue(10); - else if(myColor==Qt::yellow) + } else if (myColor == Qt::yellow) { this->setZValue(9); - else if(myColor==Qt::red) + } else if (myColor == Qt::red) { this->setZValue(8); - connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); + } + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); } int WayPointLine::type() const { @@ -70,6 +71,7 @@ int WayPointLine::type() const QPainterPath WayPointLine::shape() const { QPainterPath path = QGraphicsLineItem::shape(); + path.addPolygon(arrowHead); return path; } @@ -85,30 +87,31 @@ void WayPointLine::paint(QPainter *painter, const QStyleOptionGraphicsItem *opti painter->setBrush(myColor); double angle = ::acos(line().dx() / line().length()); - if (line().dy() >= 0) + if (line().dy() >= 0) { angle = (M_PI * 2) - angle; + } - QPointF arrowP1 = line().pointAt(0.5) + QPointF(sin(angle + M_PI / 3) * arrowSize, - cos(angle + M_PI / 3) * arrowSize); - QPointF arrowP2 = line().pointAt(0.5) + QPointF(sin(angle + M_PI - M_PI / 3) * arrowSize, - cos(angle + M_PI - M_PI / 3) * arrowSize); - arrowHead.clear(); - arrowHead << line().pointAt(0.5) << arrowP1 << arrowP2; - painter->drawPolygon(arrowHead); - if(myColor==Qt::red) - myPen.setWidth(3); - else if(myColor==Qt::yellow) - myPen.setWidth(2); - else if(myColor==Qt::green) - myPen.setWidth(1); - painter->setPen(myPen); - painter->drawLine(line()); - + QPointF arrowP1 = line().pointAt(0.5) + QPointF(sin(angle + M_PI / 3) * arrowSize, + cos(angle + M_PI / 3) * arrowSize); + QPointF arrowP2 = line().pointAt(0.5) + QPointF(sin(angle + M_PI - M_PI / 3) * arrowSize, + cos(angle + M_PI - M_PI / 3) * arrowSize); + arrowHead.clear(); + arrowHead << line().pointAt(0.5) << arrowP1 << arrowP2; + painter->drawPolygon(arrowHead); + if (myColor == Qt::red) { + myPen.setWidth(3); + } else if (myColor == Qt::yellow) { + myPen.setWidth(2); + } else if (myColor == Qt::green) { + myPen.setWidth(1); + } + painter->setPen(myPen); + painter->drawLine(line()); } void WayPointLine::refreshLocations() { - this->setLine(destination->pos().x(),destination->pos().y(),source->pos().x(),source->pos().y()); + this->setLine(destination->pos().x(), destination->pos().y(), source->pos().x(), source->pos().y()); } void WayPointLine::waypointdeleted() @@ -120,5 +123,4 @@ void WayPointLine::setOpacitySlot(qreal opacity) { setOpacity(opacity); } - } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.h index 6b6bc4133..cc1cc3b93 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.h @@ -1,29 +1,29 @@ /** -****************************************************************************** -* -* @file waypointline.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief A graphicsItem representing a line connecting 2 waypoints -* @see The GNU Public License (GPL) Version 3 -* @defgroup OPMapWidget -* @{ -* -*****************************************************************************/ + ****************************************************************************** + * + * @file waypointline.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief A graphicsItem representing a line connecting 2 waypoints + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ /* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 3 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, but -* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY -* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -* for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ #ifndef WAYPOINTLINE_H #define WAYPOINTLINE_H #include @@ -35,24 +35,23 @@ #include #include -namespace mapcontrol -{ -class WayPointLine:public QObject,public QGraphicsLineItem -{ - Q_OBJECT - Q_INTERFACES(QGraphicsItem) +namespace mapcontrol { +class WayPointLine : public QObject, public QGraphicsLineItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) public: enum { Type = UserType + 8 }; - WayPointLine(WayPointItem * from, WayPointItem * to,MapGraphicItem * map,QColor color=Qt::green); - WayPointLine(HomeItem * from, WayPointItem * to,MapGraphicItem * map,QColor color=Qt::green); + WayPointLine(WayPointItem *from, WayPointItem *to, MapGraphicItem *map, QColor color = Qt::green); + WayPointLine(HomeItem *from, WayPointItem *to, MapGraphicItem *map, QColor color = Qt::green); int type() const; QPainterPath shape() const; void setColor(const QColor &color) - { myColor = color; } + { + myColor = color; + } private: - QGraphicsItem * source; - QGraphicsItem * destination; - MapGraphicItem * my_map; + QGraphicsItem *source; + QGraphicsItem *destination; + MapGraphicItem *my_map; QPolygonF arrowHead; QColor myColor; protected: diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/posix_qextserialport.cpp b/ground/openpilotgcs/src/libs/qextserialport/src/posix_qextserialport.cpp index c7f923cd5..e672122c5 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/posix_qextserialport.cpp +++ b/ground/openpilotgcs/src/libs/qextserialport/src/posix_qextserialport.cpp @@ -1,5 +1,3 @@ - - #include #include #include "qextserialport.h" @@ -13,699 +11,691 @@ void QextSerialPort::platformSpecificInit() } /*! -Standard destructor. -*/ + Standard destructor. + */ void QextSerialPort::platformSpecificDestruct() {} /*! -Sets the baud rate of the serial port. Note that not all rates are applicable on -all platforms. The following table shows translations of the various baud rate -constants on Windows(including NT/2000) and POSIX platforms. Speeds marked with an * -are speeds that are usable on both Windows and POSIX. + Sets the baud rate of the serial port. Note that not all rates are applicable on + all platforms. The following table shows translations of the various baud rate + constants on Windows(including NT/2000) and POSIX platforms. Speeds marked with an * + are speeds that are usable on both Windows and POSIX. -\note -BAUD76800 may not be supported on all POSIX systems. SGI/IRIX systems do not support -BAUD1800. + \note + BAUD76800 may not be supported on all POSIX systems. SGI/IRIX systems do not support + BAUD1800. -\verbatim + \verbatim - RATE Windows Speed POSIX Speed - ----------- ------------- ----------- + RATE Windows Speed POSIX Speed + ----------- ------------- ----------- BAUD50 110 50 BAUD75 110 75 - *BAUD110 110 110 + * BAUD110 110 110 BAUD134 110 134.5 BAUD150 110 150 BAUD200 110 200 - *BAUD300 300 300 - *BAUD600 600 600 - *BAUD1200 1200 1200 + * BAUD300 300 300 + * BAUD600 600 600 + * BAUD1200 1200 1200 BAUD1800 1200 1800 - *BAUD2400 2400 2400 - *BAUD4800 4800 4800 - *BAUD9600 9600 9600 + * BAUD2400 2400 2400 + * BAUD4800 4800 4800 + * BAUD9600 9600 9600 BAUD14400 14400 9600 - *BAUD19200 19200 19200 - *BAUD38400 38400 38400 + * BAUD19200 19200 19200 + * BAUD38400 38400 38400 BAUD56000 56000 38400 - *BAUD57600 57600 57600 + * BAUD57600 57600 57600 BAUD76800 57600 76800 - *BAUD115200 115200 115200 + * BAUD115200 115200 115200 BAUD128000 128000 115200 BAUD256000 256000 115200 -\endverbatim -*/ + \endverbatim + */ void QextSerialPort::setBaudRate(BaudRateType baudRate) { QMutexLocker lock(mutex); - if (Settings.BaudRate!=baudRate) { + + if (Settings.BaudRate != baudRate) { switch (baudRate) { - case BAUD14400: - Settings.BaudRate=BAUD9600; - break; + case BAUD14400: + Settings.BaudRate = BAUD9600; + break; - case BAUD56000: - Settings.BaudRate=BAUD38400; - break; + case BAUD56000: + Settings.BaudRate = BAUD38400; + break; - case BAUD76800: + case BAUD76800: #ifndef B76800 - Settings.BaudRate=BAUD57600; + Settings.BaudRate = BAUD57600; #else - Settings.BaudRate=baudRate; + Settings.BaudRate = baudRate; #endif - break; + break; - case BAUD128000: - case BAUD256000: - Settings.BaudRate=BAUD115200; - break; + case BAUD128000: + case BAUD256000: + Settings.BaudRate = BAUD115200; + break; - default: - Settings.BaudRate=baudRate; - break; + default: + Settings.BaudRate = baudRate; + break; } } if (isOpen()) { switch (baudRate) { - - /*50 baud*/ - case BAUD50: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 50 baud operation."); + /*50 baud*/ + case BAUD50: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 50 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B50; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B50; #else - cfsetispeed(&Posix_CommConfig, B50); - cfsetospeed(&Posix_CommConfig, B50); + cfsetispeed(&Posix_CommConfig, B50); + cfsetospeed(&Posix_CommConfig, B50); #endif - break; + break; - /*75 baud*/ - case BAUD75: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 75 baud operation."); + /*75 baud*/ + case BAUD75: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 75 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B75; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B75; #else - cfsetispeed(&Posix_CommConfig, B75); - cfsetospeed(&Posix_CommConfig, B75); + cfsetispeed(&Posix_CommConfig, B75); + cfsetospeed(&Posix_CommConfig, B75); #endif - break; + break; - /*110 baud*/ - case BAUD110: + /*110 baud*/ + case BAUD110: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B110; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B110; #else - cfsetispeed(&Posix_CommConfig, B110); - cfsetospeed(&Posix_CommConfig, B110); + cfsetispeed(&Posix_CommConfig, B110); + cfsetospeed(&Posix_CommConfig, B110); #endif - break; + break; - /*134.5 baud*/ - case BAUD134: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 134.5 baud operation."); + /*134.5 baud*/ + case BAUD134: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 134.5 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B134; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B134; #else - cfsetispeed(&Posix_CommConfig, B134); - cfsetospeed(&Posix_CommConfig, B134); + cfsetispeed(&Posix_CommConfig, B134); + cfsetospeed(&Posix_CommConfig, B134); #endif - break; + break; - /*150 baud*/ - case BAUD150: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 150 baud operation."); + /*150 baud*/ + case BAUD150: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 150 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B150; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B150; #else - cfsetispeed(&Posix_CommConfig, B150); - cfsetospeed(&Posix_CommConfig, B150); + cfsetispeed(&Posix_CommConfig, B150); + cfsetospeed(&Posix_CommConfig, B150); #endif - break; + break; - /*200 baud*/ - case BAUD200: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 200 baud operation."); + /*200 baud*/ + case BAUD200: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows does not support 200 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B200; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B200; #else - cfsetispeed(&Posix_CommConfig, B200); - cfsetospeed(&Posix_CommConfig, B200); + cfsetispeed(&Posix_CommConfig, B200); + cfsetospeed(&Posix_CommConfig, B200); #endif - break; + break; - /*300 baud*/ - case BAUD300: + /*300 baud*/ + case BAUD300: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B300; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B300; #else - cfsetispeed(&Posix_CommConfig, B300); - cfsetospeed(&Posix_CommConfig, B300); + cfsetispeed(&Posix_CommConfig, B300); + cfsetospeed(&Posix_CommConfig, B300); #endif - break; + break; - /*600 baud*/ - case BAUD600: + /*600 baud*/ + case BAUD600: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B600; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B600; #else - cfsetispeed(&Posix_CommConfig, B600); - cfsetospeed(&Posix_CommConfig, B600); + cfsetispeed(&Posix_CommConfig, B600); + cfsetospeed(&Posix_CommConfig, B600); #endif - break; + break; - /*1200 baud*/ - case BAUD1200: + /*1200 baud*/ + case BAUD1200: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B1200; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B1200; #else - cfsetispeed(&Posix_CommConfig, B1200); - cfsetospeed(&Posix_CommConfig, B1200); + cfsetispeed(&Posix_CommConfig, B1200); + cfsetospeed(&Posix_CommConfig, B1200); #endif - break; + break; - /*1800 baud*/ - case BAUD1800: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows and IRIX do not support 1800 baud operation."); + /*1800 baud*/ + case BAUD1800: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows and IRIX do not support 1800 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B1800; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B1800; #else - cfsetispeed(&Posix_CommConfig, B1800); - cfsetospeed(&Posix_CommConfig, B1800); + cfsetispeed(&Posix_CommConfig, B1800); + cfsetospeed(&Posix_CommConfig, B1800); #endif - break; + break; - /*2400 baud*/ - case BAUD2400: + /*2400 baud*/ + case BAUD2400: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B2400; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B2400; #else - cfsetispeed(&Posix_CommConfig, B2400); - cfsetospeed(&Posix_CommConfig, B2400); + cfsetispeed(&Posix_CommConfig, B2400); + cfsetospeed(&Posix_CommConfig, B2400); #endif - break; + break; - /*4800 baud*/ - case BAUD4800: + /*4800 baud*/ + case BAUD4800: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B4800; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B4800; #else - cfsetispeed(&Posix_CommConfig, B4800); - cfsetospeed(&Posix_CommConfig, B4800); + cfsetispeed(&Posix_CommConfig, B4800); + cfsetospeed(&Posix_CommConfig, B4800); #endif - break; + break; - /*9600 baud*/ - case BAUD9600: + /*9600 baud*/ + case BAUD9600: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B9600; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B9600; #else - cfsetispeed(&Posix_CommConfig, B9600); - cfsetospeed(&Posix_CommConfig, B9600); + cfsetispeed(&Posix_CommConfig, B9600); + cfsetospeed(&Posix_CommConfig, B9600); #endif - break; + break; - /*14400 baud*/ - case BAUD14400: - TTY_WARNING("QextSerialPort: POSIX does not support 14400 baud operation. Switching to 9600 baud."); + /*14400 baud*/ + case BAUD14400: + TTY_WARNING("QextSerialPort: POSIX does not support 14400 baud operation. Switching to 9600 baud."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B9600; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B9600; #else - cfsetispeed(&Posix_CommConfig, B9600); - cfsetospeed(&Posix_CommConfig, B9600); + cfsetispeed(&Posix_CommConfig, B9600); + cfsetospeed(&Posix_CommConfig, B9600); #endif - break; + break; - /*19200 baud*/ - case BAUD19200: + /*19200 baud*/ + case BAUD19200: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B19200; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B19200; #else - cfsetispeed(&Posix_CommConfig, B19200); - cfsetospeed(&Posix_CommConfig, B19200); + cfsetispeed(&Posix_CommConfig, B19200); + cfsetospeed(&Posix_CommConfig, B19200); #endif - break; + break; - /*38400 baud*/ - case BAUD38400: + /*38400 baud*/ + case BAUD38400: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B38400; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B38400; #else - cfsetispeed(&Posix_CommConfig, B38400); - cfsetospeed(&Posix_CommConfig, B38400); + cfsetispeed(&Posix_CommConfig, B38400); + cfsetospeed(&Posix_CommConfig, B38400); #endif - break; + break; - /*56000 baud*/ - case BAUD56000: - TTY_WARNING("QextSerialPort: POSIX does not support 56000 baud operation. Switching to 38400 baud."); + /*56000 baud*/ + case BAUD56000: + TTY_WARNING("QextSerialPort: POSIX does not support 56000 baud operation. Switching to 38400 baud."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B38400; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B38400; #else - cfsetispeed(&Posix_CommConfig, B38400); - cfsetospeed(&Posix_CommConfig, B38400); + cfsetispeed(&Posix_CommConfig, B38400); + cfsetospeed(&Posix_CommConfig, B38400); #endif - break; + break; - /*57600 baud*/ - case BAUD57600: + /*57600 baud*/ + case BAUD57600: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B57600; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B57600; #else - cfsetispeed(&Posix_CommConfig, B57600); - cfsetospeed(&Posix_CommConfig, B57600); + cfsetispeed(&Posix_CommConfig, B57600); + cfsetospeed(&Posix_CommConfig, B57600); #endif - break; + break; - /*76800 baud*/ - case BAUD76800: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows and some POSIX systems do not support 76800 baud operation."); + /*76800 baud*/ + case BAUD76800: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Windows and some POSIX systems do not support 76800 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag &= (~CBAUD); #ifdef B76800 - Posix_CommConfig.c_cflag|=B76800; + Posix_CommConfig.c_cflag |= B76800; #else - TTY_WARNING("QextSerialPort: QextSerialPort was compiled without 76800 baud support. Switching to 57600 baud."); - Posix_CommConfig.c_cflag|=B57600; -#endif //B76800 -#else //CBAUD + TTY_WARNING("QextSerialPort: QextSerialPort was compiled without 76800 baud support. Switching to 57600 baud."); + Posix_CommConfig.c_cflag |= B57600; +#endif // B76800 +#else // CBAUD #ifdef B76800 - cfsetispeed(&Posix_CommConfig, B76800); - cfsetospeed(&Posix_CommConfig, B76800); + cfsetispeed(&Posix_CommConfig, B76800); + cfsetospeed(&Posix_CommConfig, B76800); #else - TTY_WARNING("QextSerialPort: QextSerialPort was compiled without 76800 baud support. Switching to 57600 baud."); - cfsetispeed(&Posix_CommConfig, B57600); - cfsetospeed(&Posix_CommConfig, B57600); -#endif //B76800 -#endif //CBAUD - break; + TTY_WARNING("QextSerialPort: QextSerialPort was compiled without 76800 baud support. Switching to 57600 baud."); + cfsetispeed(&Posix_CommConfig, B57600); + cfsetospeed(&Posix_CommConfig, B57600); +#endif // B76800 +#endif // CBAUD + break; - /*115200 baud*/ - case BAUD115200: + /*115200 baud*/ + case BAUD115200: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B115200; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B115200; #else - cfsetispeed(&Posix_CommConfig, B115200); - cfsetospeed(&Posix_CommConfig, B115200); + cfsetispeed(&Posix_CommConfig, B115200); + cfsetospeed(&Posix_CommConfig, B115200); #endif - break; + break; - /*128000 baud*/ - case BAUD128000: - TTY_WARNING("QextSerialPort: POSIX does not support 128000 baud operation. Switching to 115200 baud."); + /*128000 baud*/ + case BAUD128000: + TTY_WARNING("QextSerialPort: POSIX does not support 128000 baud operation. Switching to 115200 baud."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B115200; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B115200; #else - cfsetispeed(&Posix_CommConfig, B115200); - cfsetospeed(&Posix_CommConfig, B115200); + cfsetispeed(&Posix_CommConfig, B115200); + cfsetospeed(&Posix_CommConfig, B115200); #endif - break; + break; - /*256000 baud*/ - case BAUD256000: - TTY_WARNING("QextSerialPort: POSIX does not support 256000 baud operation. Switching to 115200 baud."); + /*256000 baud*/ + case BAUD256000: + TTY_WARNING("QextSerialPort: POSIX does not support 256000 baud operation. Switching to 115200 baud."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B115200; + Posix_CommConfig.c_cflag &= (~CBAUD); + Posix_CommConfig.c_cflag |= B115200; #else - cfsetispeed(&Posix_CommConfig, B115200); - cfsetospeed(&Posix_CommConfig, B115200); + cfsetispeed(&Posix_CommConfig, B115200); + cfsetospeed(&Posix_CommConfig, B115200); #endif - break; + break; } tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); } } /*! -Sets the number of data bits used by the serial port. Possible values of dataBits are: -\verbatim + Sets the number of data bits used by the serial port. Possible values of dataBits are: + \verbatim DATA_5 5 data bits DATA_6 6 data bits DATA_7 7 data bits DATA_8 8 data bits -\endverbatim + \endverbatim -\note -This function is subject to the following restrictions: -\par + \note + This function is subject to the following restrictions: + \par 5 data bits cannot be used with 2 stop bits. -\par + \par 8 data bits cannot be used with space parity on POSIX systems. -*/ + */ void QextSerialPort::setDataBits(DataBitsType dataBits) { QMutexLocker lock(mutex); - if (Settings.DataBits!=dataBits) { - if ((Settings.StopBits==STOP_2 && dataBits==DATA_5) || - (Settings.StopBits==STOP_1_5 && dataBits!=DATA_5) || - (Settings.Parity==PAR_SPACE && dataBits==DATA_8)) { - } - else { - Settings.DataBits=dataBits; + + if (Settings.DataBits != dataBits) { + if ((Settings.StopBits == STOP_2 && dataBits == DATA_5) || + (Settings.StopBits == STOP_1_5 && dataBits != DATA_5) || + (Settings.Parity == PAR_SPACE && dataBits == DATA_8)) {} else { + Settings.DataBits = dataBits; } } if (isOpen()) { - switch(dataBits) { + switch (dataBits) { + /*5 data bits*/ + case DATA_5: + if (Settings.StopBits == STOP_2) { + TTY_WARNING("QextSerialPort: 5 Data bits cannot be used with 2 stop bits."); + } else { + Settings.DataBits = dataBits; + Posix_CommConfig.c_cflag &= (~CSIZE); + Posix_CommConfig.c_cflag |= CS5; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + } + break; - /*5 data bits*/ - case DATA_5: - if (Settings.StopBits==STOP_2) { - TTY_WARNING("QextSerialPort: 5 Data bits cannot be used with 2 stop bits."); - } - else { - Settings.DataBits=dataBits; - Posix_CommConfig.c_cflag&=(~CSIZE); - Posix_CommConfig.c_cflag|=CS5; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; + /*6 data bits*/ + case DATA_6: + if (Settings.StopBits == STOP_1_5) { + TTY_WARNING("QextSerialPort: 6 Data bits cannot be used with 1.5 stop bits."); + } else { + Settings.DataBits = dataBits; + Posix_CommConfig.c_cflag &= (~CSIZE); + Posix_CommConfig.c_cflag |= CS6; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + } + break; - /*6 data bits*/ - case DATA_6: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("QextSerialPort: 6 Data bits cannot be used with 1.5 stop bits."); - } - else { - Settings.DataBits=dataBits; - Posix_CommConfig.c_cflag&=(~CSIZE); - Posix_CommConfig.c_cflag|=CS6; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; + /*7 data bits*/ + case DATA_7: + if (Settings.StopBits == STOP_1_5) { + TTY_WARNING("QextSerialPort: 7 Data bits cannot be used with 1.5 stop bits."); + } else { + Settings.DataBits = dataBits; + Posix_CommConfig.c_cflag &= (~CSIZE); + Posix_CommConfig.c_cflag |= CS7; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + } + break; - /*7 data bits*/ - case DATA_7: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("QextSerialPort: 7 Data bits cannot be used with 1.5 stop bits."); - } - else { - Settings.DataBits=dataBits; - Posix_CommConfig.c_cflag&=(~CSIZE); - Posix_CommConfig.c_cflag|=CS7; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; - - /*8 data bits*/ - case DATA_8: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("QextSerialPort: 8 Data bits cannot be used with 1.5 stop bits."); - } - else { - Settings.DataBits=dataBits; - Posix_CommConfig.c_cflag&=(~CSIZE); - Posix_CommConfig.c_cflag|=CS8; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; + /*8 data bits*/ + case DATA_8: + if (Settings.StopBits == STOP_1_5) { + TTY_WARNING("QextSerialPort: 8 Data bits cannot be used with 1.5 stop bits."); + } else { + Settings.DataBits = dataBits; + Posix_CommConfig.c_cflag &= (~CSIZE); + Posix_CommConfig.c_cflag |= CS8; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + } + break; } } } /*! -Sets the parity associated with the serial port. The possible values of parity are: -\verbatim + Sets the parity associated with the serial port. The possible values of parity are: + \verbatim PAR_SPACE Space Parity PAR_MARK Mark Parity PAR_NONE No Parity PAR_EVEN Even Parity PAR_ODD Odd Parity -\endverbatim + \endverbatim -\note -This function is subject to the following limitations: -\par -POSIX systems do not support mark parity. -\par -POSIX systems support space parity only if tricked into doing so, and only with + \note + This function is subject to the following limitations: + \par + POSIX systems do not support mark parity. + \par + POSIX systems support space parity only if tricked into doing so, and only with fewer than 8 data bits. Use space parity very carefully with POSIX systems. -*/ + */ void QextSerialPort::setParity(ParityType parity) { QMutexLocker lock(mutex); - if (Settings.Parity!=parity) { - if (parity==PAR_MARK || (parity==PAR_SPACE && Settings.DataBits==DATA_8)) { - } - else { - Settings.Parity=parity; + + if (Settings.Parity != parity) { + if (parity == PAR_MARK || (parity == PAR_SPACE && Settings.DataBits == DATA_8)) {} else { + Settings.Parity = parity; } } if (isOpen()) { switch (parity) { + /*space parity*/ + case PAR_SPACE: + if (Settings.DataBits == DATA_8) { + TTY_PORTABILITY_WARNING("QextSerialPort: Space parity is only supported in POSIX with 7 or fewer data bits"); + } else { + /*space parity not directly supported - add an extra data bit to simulate it*/ + Posix_CommConfig.c_cflag &= ~(PARENB | CSIZE); + switch (Settings.DataBits) { + case DATA_5: + Settings.DataBits = DATA_6; + Posix_CommConfig.c_cflag |= CS6; + break; - /*space parity*/ - case PAR_SPACE: - if (Settings.DataBits==DATA_8) { - TTY_PORTABILITY_WARNING("QextSerialPort: Space parity is only supported in POSIX with 7 or fewer data bits"); + case DATA_6: + Settings.DataBits = DATA_7; + Posix_CommConfig.c_cflag |= CS7; + break; + + case DATA_7: + Settings.DataBits = DATA_8; + Posix_CommConfig.c_cflag |= CS8; + break; + + case DATA_8: + break; } - else { - - /*space parity not directly supported - add an extra data bit to simulate it*/ - Posix_CommConfig.c_cflag&=~(PARENB|CSIZE); - switch(Settings.DataBits) { - case DATA_5: - Settings.DataBits=DATA_6; - Posix_CommConfig.c_cflag|=CS6; - break; - - case DATA_6: - Settings.DataBits=DATA_7; - Posix_CommConfig.c_cflag|=CS7; - break; - - case DATA_7: - Settings.DataBits=DATA_8; - Posix_CommConfig.c_cflag|=CS8; - break; - - case DATA_8: - break; - } - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; - - /*mark parity - WINDOWS ONLY*/ - case PAR_MARK: - TTY_WARNING("QextSerialPort: Mark parity is not supported by POSIX."); - break; - - /*no parity*/ - case PAR_NONE: - Posix_CommConfig.c_cflag&=(~PARENB); tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + } + break; - /*even parity*/ - case PAR_EVEN: - Posix_CommConfig.c_cflag&=(~PARODD); - Posix_CommConfig.c_cflag|=PARENB; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + /*mark parity - WINDOWS ONLY*/ + case PAR_MARK: + TTY_WARNING("QextSerialPort: Mark parity is not supported by POSIX."); + break; - /*odd parity*/ - case PAR_ODD: - Posix_CommConfig.c_cflag|=(PARENB|PARODD); - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + /*no parity*/ + case PAR_NONE: + Posix_CommConfig.c_cflag &= (~PARENB); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; + + /*even parity*/ + case PAR_EVEN: + Posix_CommConfig.c_cflag &= (~PARODD); + Posix_CommConfig.c_cflag |= PARENB; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; + + /*odd parity*/ + case PAR_ODD: + Posix_CommConfig.c_cflag |= (PARENB | PARODD); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; } } } /*! -Sets the number of stop bits used by the serial port. Possible values of stopBits are: -\verbatim + Sets the number of stop bits used by the serial port. Possible values of stopBits are: + \verbatim STOP_1 1 stop bit STOP_1_5 1.5 stop bits STOP_2 2 stop bits -\endverbatim -\note -This function is subject to the following restrictions: -\par + \endverbatim + \note + This function is subject to the following restrictions: + \par 2 stop bits cannot be used with 5 data bits. -\par + \par POSIX does not support 1.5 stop bits. -*/ + */ void QextSerialPort::setStopBits(StopBitsType stopBits) { QMutexLocker lock(mutex); - if (Settings.StopBits!=stopBits) { - if ((Settings.DataBits==DATA_5 && stopBits==STOP_2) || stopBits==STOP_1_5) {} - else { - Settings.StopBits=stopBits; + + if (Settings.StopBits != stopBits) { + if ((Settings.DataBits == DATA_5 && stopBits == STOP_2) || stopBits == STOP_1_5) {} else { + Settings.StopBits = stopBits; } } if (isOpen()) { switch (stopBits) { + /*one stop bit*/ + case STOP_1: + Settings.StopBits = stopBits; + Posix_CommConfig.c_cflag &= (~CSTOPB); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; - /*one stop bit*/ - case STOP_1: - Settings.StopBits=stopBits; - Posix_CommConfig.c_cflag&=(~CSTOPB); + /*1.5 stop bits*/ + case STOP_1_5: + TTY_WARNING("QextSerialPort: 1.5 stop bit operation is not supported by POSIX."); + break; + + /*two stop bits*/ + case STOP_2: + if (Settings.DataBits == DATA_5) { + TTY_WARNING("QextSerialPort: 2 stop bits cannot be used with 5 data bits"); + } else { + Settings.StopBits = stopBits; + Posix_CommConfig.c_cflag |= CSTOPB; tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; - - /*1.5 stop bits*/ - case STOP_1_5: - TTY_WARNING("QextSerialPort: 1.5 stop bit operation is not supported by POSIX."); - break; - - /*two stop bits*/ - case STOP_2: - if (Settings.DataBits==DATA_5) { - TTY_WARNING("QextSerialPort: 2 stop bits cannot be used with 5 data bits"); - } - else { - Settings.StopBits=stopBits; - Posix_CommConfig.c_cflag|=CSTOPB; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; + } + break; } } } /*! -Sets the flow control used by the port. Possible values of flow are: -\verbatim + Sets the flow control used by the port. Possible values of flow are: + \verbatim FLOW_OFF No flow control FLOW_HARDWARE Hardware (RTS/CTS) flow control FLOW_XONXOFF Software (XON/XOFF) flow control -\endverbatim -\note -FLOW_HARDWARE may not be supported on all versions of UNIX. In cases where it is -unsupported, FLOW_HARDWARE is the same as FLOW_OFF. + \endverbatim + \note + FLOW_HARDWARE may not be supported on all versions of UNIX. In cases where it is + unsupported, FLOW_HARDWARE is the same as FLOW_OFF. -*/ + */ void QextSerialPort::setFlowControl(FlowType flow) { QMutexLocker lock(mutex); - if (Settings.FlowControl!=flow) { - Settings.FlowControl=flow; + + if (Settings.FlowControl != flow) { + Settings.FlowControl = flow; } if (isOpen()) { - switch(flow) { + switch (flow) { + /*no flow control*/ + case FLOW_OFF: + Posix_CommConfig.c_cflag &= (~CRTSCTS); + Posix_CommConfig.c_iflag &= (~(IXON | IXOFF | IXANY)); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; - /*no flow control*/ - case FLOW_OFF: - Posix_CommConfig.c_cflag&=(~CRTSCTS); - Posix_CommConfig.c_iflag&=(~(IXON|IXOFF|IXANY)); - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + /*software (XON/XOFF) flow control*/ + case FLOW_XONXOFF: + Posix_CommConfig.c_cflag &= (~CRTSCTS); + Posix_CommConfig.c_iflag |= (IXON | IXOFF | IXANY); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; - /*software (XON/XOFF) flow control*/ - case FLOW_XONXOFF: - Posix_CommConfig.c_cflag&=(~CRTSCTS); - Posix_CommConfig.c_iflag|=(IXON|IXOFF|IXANY); - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; - - case FLOW_HARDWARE: - Posix_CommConfig.c_cflag|=CRTSCTS; - Posix_CommConfig.c_iflag&=(~(IXON|IXOFF|IXANY)); - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + case FLOW_HARDWARE: + Posix_CommConfig.c_cflag |= CRTSCTS; + Posix_CommConfig.c_iflag &= (~(IXON | IXOFF | IXANY)); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; } } } /*! -Sets the read and write timeouts for the port to millisec milliseconds. -Note that this is a per-character timeout, i.e. the port will wait this long for each -individual character, not for the whole read operation. This timeout also applies to the -bytesWaiting() function. + Sets the read and write timeouts for the port to millisec milliseconds. + Note that this is a per-character timeout, i.e. the port will wait this long for each + individual character, not for the whole read operation. This timeout also applies to the + bytesWaiting() function. -\note -POSIX does not support millisecond-level control for I/O timeout values. Any -timeout set using this function will be set to the next lowest tenth of a second for -the purposes of detecting read or write timeouts. For example a timeout of 550 milliseconds -will be seen by the class as a timeout of 500 milliseconds for the purposes of reading and -writing the port. However millisecond-level control is allowed by the select() system call, -so for example a 550-millisecond timeout will be seen as 550 milliseconds on POSIX systems for -the purpose of detecting available bytes in the read buffer. + \note + POSIX does not support millisecond-level control for I/O timeout values. Any + timeout set using this function will be set to the next lowest tenth of a second for + the purposes of detecting read or write timeouts. For example a timeout of 550 milliseconds + will be seen by the class as a timeout of 500 milliseconds for the purposes of reading and + writing the port. However millisecond-level control is allowed by the select() system call, + so for example a 550-millisecond timeout will be seen as 550 milliseconds on POSIX systems for + the purpose of detecting available bytes in the read buffer. -*/ + */ void QextSerialPort::setTimeout(long millisec) { QMutexLocker lock(mutex); - Settings.Timeout_Millisec = millisec; - Posix_Copy_Timeout.tv_sec = millisec / 1000; + + Settings.Timeout_Millisec = millisec; + Posix_Copy_Timeout.tv_sec = millisec / 1000; Posix_Copy_Timeout.tv_usec = millisec % 1000; if (isOpen()) { - if (millisec == -1) + if (millisec == -1) { fcntl(fd, F_SETFL, O_NDELAY); - else - //O_SYNC should enable blocking ::write() - //however this seems not working on Linux 2.6.21 (works on OpenBSD 4.2) + } else { + // O_SYNC should enable blocking ::write() + // however this seems not working on Linux 2.6.21 (works on OpenBSD 4.2) fcntl(fd, F_SETFL, O_SYNC); - tcgetattr(fd, & Posix_CommConfig); - Posix_CommConfig.c_cc[VTIME] = millisec/100; - tcsetattr(fd, TCSAFLUSH, & Posix_CommConfig); + } + tcgetattr(fd, &Posix_CommConfig); + Posix_CommConfig.c_cc[VTIME] = millisec / 100; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); } } /*! -Opens the serial port associated to this class. -This function has no effect if the port associated with the class is already open. -The port is also configured to the current settings, as stored in the Settings structure. -*/ + Opens the serial port associated to this class. + This function has no effect if the port associated with the class is already open. + The port is also configured to the current settings, as stored in the Settings structure. + */ bool QextSerialPort::open(OpenMode mode) { QMutexLocker lock(mutex); - if (mode == QIODevice::NotOpen) + + if (mode == QIODevice::NotOpen) { return isOpen(); + } if (!isOpen()) { qDebug() << "trying to open file" << port.toAscii(); - //note: linux 2.6.21 seems to ignore O_NDELAY flag - if ((fd = ::open(port.toAscii() ,O_RDWR | O_NOCTTY | O_NDELAY)) != -1) { + // note: linux 2.6.21 seems to ignore O_NDELAY flag + if ((fd = ::open(port.toAscii(), O_RDWR | O_NOCTTY | O_NDELAY)) != -1) { qDebug("file opened succesfully"); - setOpenMode(mode); // Flag the port as opened - tcgetattr(fd, &old_termios); // Save the old termios + setOpenMode(mode); // Flag the port as opened + tcgetattr(fd, &old_termios); // Save the old termios Posix_CommConfig = old_termios; // Make a working copy - cfmakeraw(&Posix_CommConfig); // Enable raw access + cfmakeraw(&Posix_CommConfig); // Enable raw access /*set up other port settings*/ - Posix_CommConfig.c_cflag|=CREAD|CLOCAL; - Posix_CommConfig.c_lflag&=(~(ICANON|ECHO|ECHOE|ECHOK|ECHONL|ISIG)); - Posix_CommConfig.c_iflag&=(~(INPCK|IGNPAR|PARMRK|ISTRIP|ICRNL|IXANY)); - Posix_CommConfig.c_oflag&=(~OPOST); - Posix_CommConfig.c_cc[VMIN]= 0; -#ifdef _POSIX_VDISABLE // Is a disable character available on this system? + Posix_CommConfig.c_cflag |= CREAD | CLOCAL; + Posix_CommConfig.c_lflag &= (~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG)); + Posix_CommConfig.c_iflag &= (~(INPCK | IGNPAR | PARMRK | ISTRIP | ICRNL | IXANY)); + Posix_CommConfig.c_oflag &= (~OPOST); + Posix_CommConfig.c_cc[VMIN] = 0; +#ifdef _POSIX_VDISABLE // Is a disable character available on this system? // Some systems allow for per-device disable-characters, so get the - // proper value for the configured device + // proper value for the configured device const long vdisable = fpathconf(fd, _PC_VDISABLE); - Posix_CommConfig.c_cc[VINTR] = vdisable; - Posix_CommConfig.c_cc[VQUIT] = vdisable; + Posix_CommConfig.c_cc[VINTR] = vdisable; + Posix_CommConfig.c_cc[VQUIT] = vdisable; Posix_CommConfig.c_cc[VSTART] = vdisable; - Posix_CommConfig.c_cc[VSTOP] = vdisable; - Posix_CommConfig.c_cc[VSUSP] = vdisable; -#endif //_POSIX_VDISABLE + Posix_CommConfig.c_cc[VSTOP] = vdisable; + Posix_CommConfig.c_cc[VSUSP] = vdisable; +#endif // _POSIX_VDISABLE setBaudRate(Settings.BaudRate); setDataBits(Settings.DataBits); setParity(Settings.Parity); @@ -727,24 +717,24 @@ bool QextSerialPort::open(OpenMode mode) } /*! -Closes a serial port. This function has no effect if the serial port associated with the class -is not currently open. -*/ + Closes a serial port. This function has no effect if the serial port associated with the class + is not currently open. + */ void QextSerialPort::close() { QMutexLocker lock(mutex); - if( isOpen() ) - { + + if (isOpen()) { // Force a flush and then restore the original termios flush(); // Using both TCSAFLUSH and TCSANOW here discards any pending input - tcsetattr(fd, TCSAFLUSH | TCSANOW, &old_termios); // Restore termios + tcsetattr(fd, TCSAFLUSH | TCSANOW, &old_termios); // Restore termios // Be a good QIODevice and call QIODevice::close() before POSIX close() - // so the aboutToClose() signal is emitted at the proper time - QIODevice::close(); // Flag the device as closed + // so the aboutToClose() signal is emitted at the proper time + QIODevice::close(); // Flag the device as closed // QIODevice::close() doesn't actually close the port, so do that here ::close(fd); - if(readNotifier) { + if (readNotifier) { delete readNotifier; readNotifier = 0; } @@ -752,42 +742,46 @@ void QextSerialPort::close() } /*! -Flushes all pending I/O to the serial port. This function has no effect if the serial port -associated with the class is not currently open. -*/ + Flushes all pending I/O to the serial port. This function has no effect if the serial port + associated with the class is not currently open. + */ void QextSerialPort::flush() { QMutexLocker lock(mutex); - if (isOpen()) + + if (isOpen()) { tcflush(fd, TCIOFLUSH); + } } /*! -This function will return the number of bytes waiting in the receive queue of the serial port. -It is included primarily to provide a complete QIODevice interface, and will not record errors -in the lastErr member (because it is const). This function is also not thread-safe - in -multithreading situations, use QextSerialPort::bytesWaiting() instead. -*/ + This function will return the number of bytes waiting in the receive queue of the serial port. + It is included primarily to provide a complete QIODevice interface, and will not record errors + in the lastErr member (because it is const). This function is also not thread-safe - in + multithreading situations, use QextSerialPort::bytesWaiting() instead. + */ qint64 QextSerialPort::size() const { int numBytes; - if (ioctl(fd, FIONREAD, &numBytes)<0) { + + if (ioctl(fd, FIONREAD, &numBytes) < 0) { numBytes = 0; } return (qint64)numBytes; } /*! -Returns the number of bytes waiting in the port's receive queue. This function will return 0 if -the port is not currently open, or -1 on error. -*/ + Returns the number of bytes waiting in the port's receive queue. This function will return 0 if + the port is not currently open, or -1 on error. + */ qint64 QextSerialPort::bytesAvailable() const { QMutexLocker lock(mutex); + if (isOpen()) { int bytesQueued; if (ioctl(fd, FIONREAD, &bytesQueued) == -1) { - return (qint64)-1; + return (qint64) - 1; } return bytesQueued + QIODevice::bytesAvailable(); } @@ -795,10 +789,10 @@ qint64 QextSerialPort::bytesAvailable() const } /*! -This function is included to implement the full QIODevice interface, and currently has no -purpose within this class. This function is meaningless on an unbuffered device and currently -only prints a warning message to that effect. -*/ + This function is included to implement the full QIODevice interface, and currently has no + purpose within this class. This function is meaningless on an unbuffered device and currently + only prints a warning message to that effect. + */ void QextSerialPort::ungetChar(char) { /*meaningless on unbuffered sequential device - return error and print a warning*/ @@ -806,154 +800,159 @@ void QextSerialPort::ungetChar(char) } /*! -Translates a system-specific error code to a QextSerialPort error code. Used internally. -*/ + Translates a system-specific error code to a QextSerialPort error code. Used internally. + */ void QextSerialPort::translateError(ulong error) { switch (error) { - case EBADF: - case ENOTTY: - lastErr=E_INVALID_FD; - break; + case EBADF: + case ENOTTY: + lastErr = E_INVALID_FD; + break; - case EINTR: - lastErr=E_CAUGHT_NON_BLOCKED_SIGNAL; - break; + case EINTR: + lastErr = E_CAUGHT_NON_BLOCKED_SIGNAL; + break; - case ENOMEM: - lastErr=E_NO_MEMORY; - break; + case ENOMEM: + lastErr = E_NO_MEMORY; + break; } } /*! -Sets DTR line to the requested state (high by default). This function will have no effect if -the port associated with the class is not currently open. -*/ + Sets DTR line to the requested state (high by default). This function will have no effect if + the port associated with the class is not currently open. + */ void QextSerialPort::setDtr(bool set) { QMutexLocker lock(mutex); + if (isOpen()) { int status; ioctl(fd, TIOCMGET, &status); if (set) { - status|=TIOCM_DTR; - } - else { - status&=~TIOCM_DTR; + status |= TIOCM_DTR; + } else { + status &= ~TIOCM_DTR; } ioctl(fd, TIOCMSET, &status); } } /*! -Sets RTS line to the requested state (high by default). This function will have no effect if -the port associated with the class is not currently open. -*/ + Sets RTS line to the requested state (high by default). This function will have no effect if + the port associated with the class is not currently open. + */ void QextSerialPort::setRts(bool set) { QMutexLocker lock(mutex); + if (isOpen()) { int status; ioctl(fd, TIOCMGET, &status); if (set) { - status|=TIOCM_RTS; - } - else { - status&=~TIOCM_RTS; + status |= TIOCM_RTS; + } else { + status &= ~TIOCM_RTS; } ioctl(fd, TIOCMSET, &status); } } /*! -Returns the line status as stored by the port function. This function will retrieve the states -of the following lines: DCD, CTS, DSR, and RI. On POSIX systems, the following additional lines -can be monitored: DTR, RTS, Secondary TXD, and Secondary RXD. The value returned is an unsigned -long with specific bits indicating which lines are high. The following constants should be used -to examine the states of individual lines: + Returns the line status as stored by the port function. This function will retrieve the states + of the following lines: DCD, CTS, DSR, and RI. On POSIX systems, the following additional lines + can be monitored: DTR, RTS, Secondary TXD, and Secondary RXD. The value returned is an unsigned + long with specific bits indicating which lines are high. The following constants should be used + to examine the states of individual lines: -\verbatim -Mask Line ------- ---- -LS_CTS CTS -LS_DSR DSR -LS_DCD DCD -LS_RI RI -LS_RTS RTS (POSIX only) -LS_DTR DTR (POSIX only) -LS_ST Secondary TXD (POSIX only) -LS_SR Secondary RXD (POSIX only) -\endverbatim + \verbatim + Mask Line + ------ ---- + LS_CTS CTS + LS_DSR DSR + LS_DCD DCD + LS_RI RI + LS_RTS RTS (POSIX only) + LS_DTR DTR (POSIX only) + LS_ST Secondary TXD (POSIX only) + LS_SR Secondary RXD (POSIX only) + \endverbatim -This function will return 0 if the port associated with the class is not currently open. -*/ + This function will return 0 if the port associated with the class is not currently open. + */ unsigned long QextSerialPort::lineStatus() { - unsigned long Status=0, Temp=0; + unsigned long Status = 0, Temp = 0; QMutexLocker lock(mutex); + if (isOpen()) { ioctl(fd, TIOCMGET, &Temp); - if (Temp&TIOCM_CTS) { - Status|=LS_CTS; + if (Temp & TIOCM_CTS) { + Status |= LS_CTS; } - if (Temp&TIOCM_DSR) { - Status|=LS_DSR; + if (Temp & TIOCM_DSR) { + Status |= LS_DSR; } - if (Temp&TIOCM_RI) { - Status|=LS_RI; + if (Temp & TIOCM_RI) { + Status |= LS_RI; } - if (Temp&TIOCM_CD) { - Status|=LS_DCD; + if (Temp & TIOCM_CD) { + Status |= LS_DCD; } - if (Temp&TIOCM_DTR) { - Status|=LS_DTR; + if (Temp & TIOCM_DTR) { + Status |= LS_DTR; } - if (Temp&TIOCM_RTS) { - Status|=LS_RTS; + if (Temp & TIOCM_RTS) { + Status |= LS_RTS; } - if (Temp&TIOCM_ST) { - Status|=LS_ST; + if (Temp & TIOCM_ST) { + Status |= LS_ST; } - if (Temp&TIOCM_SR) { - Status|=LS_SR; + if (Temp & TIOCM_SR) { + Status |= LS_SR; } } return Status; } /*! -Reads a block of data from the serial port. This function will read at most maxSize bytes from -the serial port and place them in the buffer pointed to by data. Return value is the number of -bytes actually read, or -1 on error. + Reads a block of data from the serial port. This function will read at most maxSize bytes from + the serial port and place them in the buffer pointed to by data. Return value is the number of + bytes actually read, or -1 on error. -\warning before calling this function ensure that serial port associated with this class -is currently open (use isOpen() function to check if port is open). -*/ -qint64 QextSerialPort::readData(char * data, qint64 maxSize) + \warning before calling this function ensure that serial port associated with this class + is currently open (use isOpen() function to check if port is open). + */ +qint64 QextSerialPort::readData(char *data, qint64 maxSize) { QMutexLocker lock(mutex); int retVal = ::read(fd, data, maxSize); - if (retVal == -1) + + if (retVal == -1) { lastErr = E_READ_FAILED; + } return retVal; } /*! -Writes a block of data to the serial port. This function will write maxSize bytes -from the buffer pointed to by data to the serial port. Return value is the number -of bytes actually written, or -1 on error. + Writes a block of data to the serial port. This function will write maxSize bytes + from the buffer pointed to by data to the serial port. Return value is the number + of bytes actually written, or -1 on error. -\warning before calling this function ensure that serial port associated with this class -is currently open (use isOpen() function to check if port is open). -*/ -qint64 QextSerialPort::writeData(const char * data, qint64 maxSize) + \warning before calling this function ensure that serial port associated with this class + is currently open (use isOpen() function to check if port is open). + */ +qint64 QextSerialPort::writeData(const char *data, qint64 maxSize) { QMutexLocker lock(mutex); int retVal = ::write(fd, data, maxSize); - if (retVal == -1) - lastErr = E_WRITE_FAILED; + + if (retVal == -1) { + lastErr = E_WRITE_FAILED; + } return (qint64)retVal; } diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator.h b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator.h index de173715c..cf9143ca9 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator.h +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator.h @@ -27,12 +27,12 @@ * Structure containing port information. */ struct QextPortInfo { - QString portName; ///< Port name. - QString physName; ///< Physical name. + QString portName; ///< Port name. + QString physName; ///< Physical name. QString friendName; ///< Friendly name. - QString enumName; ///< Enumerator name. - int vendorID; ///< Vendor ID. - int productID; ///< Product ID + QString enumName; ///< Enumerator name. + int vendorID; ///< Vendor ID. + int productID; ///< Product ID }; #ifdef Q_OS_WIN @@ -40,160 +40,159 @@ struct QextPortInfo { #include class QextSerialEnumerator; -class QextSerialRegistrationWidget : public QWidget -{ +class QextSerialRegistrationWidget : public QWidget { Q_OBJECT - public: - QextSerialRegistrationWidget( QextSerialEnumerator* qese ) { - this->qese = qese; - } - ~QextSerialRegistrationWidget( ) { } +public: + QextSerialRegistrationWidget(QextSerialEnumerator *qese) + { + this->qese = qese; + } + ~QextSerialRegistrationWidget() {} - protected: - QextSerialEnumerator* qese; - bool winEvent( MSG* message, long* result ); +protected: + QextSerialEnumerator *qese; + bool winEvent(MSG *message, long *result); }; #endif // QT_GUI_LIB #endif // Q_OS_WIN /*! - Provides list of ports available in the system. + Provides list of ports available in the system. - \section Usage - To poll the system for a list of connected devices, simply use getPorts(). Each - QextPortInfo structure will populated with information about the corresponding device. + \section Usage + To poll the system for a list of connected devices, simply use getPorts(). Each + QextPortInfo structure will populated with information about the corresponding device. - \b Example - \code - QList ports = QextSerialEnumerator::getPorts(); - foreach( QextPortInfo port, ports ) { + \b Example + \code + QList ports = QextSerialEnumerator::getPorts(); + foreach( QextPortInfo port, ports ) { // inspect port... - } - \endcode + } + \endcode - To enable event-driven notification of device connection events, first call - setUpNotifications() and then connect to the deviceDiscovered() and deviceRemoved() - signals. Event-driven behavior is currently available only on Windows and OS X. + To enable event-driven notification of device connection events, first call + setUpNotifications() and then connect to the deviceDiscovered() and deviceRemoved() + signals. Event-driven behavior is currently available only on Windows and OS X. - \b Example - \code - QextSerialEnumerator* enumerator = new QextSerialEnumerator(); - connect(enumerator, SIGNAL(deviceDiscovered(const QextPortInfo &)), + \b Example + \code + QextSerialEnumerator* enumerator = new QextSerialEnumerator(); + connect(enumerator, SIGNAL(deviceDiscovered(const QextPortInfo &)), myClass, SLOT(onDeviceDiscovered(const QextPortInfo &))); - connect(enumerator, SIGNAL(deviceRemoved(const QextPortInfo &)), + connect(enumerator, SIGNAL(deviceRemoved(const QextPortInfo &)), myClass, SLOT(onDeviceRemoved(const QextPortInfo &))); - \endcode + \endcode - \section Credits - Windows implementation is based on Zach Gorman's work from - The Code Project (http://www.codeproject.com/system/setupdi.asp). + \section Credits + Windows implementation is based on Zach Gorman's work from + The Code Project (http://www.codeproject.com/system/setupdi.asp). - OS X implementation, see - http://developer.apple.com/documentation/DeviceDrivers/Conceptual/AccessingHardware/AH_Finding_Devices/chapter_4_section_2.html + OS X implementation, see + http://developer.apple.com/documentation/DeviceDrivers/Conceptual/AccessingHardware/AH_Finding_Devices/chapter_4_section_2.html - \author Michal Policht, Liam Staskawicz -*/ -class QEXTSERIALPORT_EXPORT QextSerialEnumerator : public QObject -{ -Q_OBJECT - public: - QextSerialEnumerator( ); - ~QextSerialEnumerator( ); + \author Michal Policht, Liam Staskawicz + */ +class QEXTSERIALPORT_EXPORT QextSerialEnumerator : public QObject { + Q_OBJECT +public: + QextSerialEnumerator(); + ~QextSerialEnumerator(); #ifdef Q_OS_WIN - LRESULT onDeviceChangeWin( WPARAM wParam, LPARAM lParam ); - private: - /*! - * Get value of specified property from the registry. - * \param key handle to an open key. - * \param property property name. - * \return property value. - */ - static QString getRegKeyValue(HKEY key, LPCTSTR property); + LRESULT onDeviceChangeWin(WPARAM wParam, LPARAM lParam); +private: + /*! + * Get value of specified property from the registry. + * \param key handle to an open key. + * \param property property name. + * \return property value. + */ + static QString getRegKeyValue(HKEY key, LPCTSTR property); - /*! - * Get specific property from registry. - * \param devInfo pointer to the device information set that contains the interface - * and its underlying device. Returned by SetupDiGetClassDevs() function. - * \param devData pointer to an SP_DEVINFO_DATA structure that defines the device instance. - * this is returned by SetupDiGetDeviceInterfaceDetail() function. - * \param property registry property. One of defined SPDRP_* constants. - * \return property string. - */ - static QString getDeviceProperty(HDEVINFO devInfo, PSP_DEVINFO_DATA devData, DWORD property); + /*! + * Get specific property from registry. + * \param devInfo pointer to the device information set that contains the interface + * and its underlying device. Returned by SetupDiGetClassDevs() function. + * \param devData pointer to an SP_DEVINFO_DATA structure that defines the device instance. + * this is returned by SetupDiGetDeviceInterfaceDetail() function. + * \param property registry property. One of defined SPDRP_* constants. + * \return property string. + */ + static QString getDeviceProperty(HDEVINFO devInfo, PSP_DEVINFO_DATA devData, DWORD property); - /*! - * Search for serial ports using setupapi. - * \param infoList list with result. - */ - static void setupAPIScan(QList & infoList); - void setUpNotificationWin( ); - static bool getDeviceDetailsWin( QextPortInfo* portInfo, HDEVINFO devInfo, - PSP_DEVINFO_DATA devData, WPARAM wParam = DBT_DEVICEARRIVAL ); - static void enumerateDevicesWin( const GUID & guidDev, QList* infoList ); - bool matchAndDispatchChangedDevice(const QString & deviceID, const GUID & guid, WPARAM wParam); + /*! + * Search for serial ports using setupapi. + * \param infoList list with result. + */ + static void setupAPIScan(QList & infoList); + void setUpNotificationWin(); + static bool getDeviceDetailsWin(QextPortInfo *portInfo, HDEVINFO devInfo, + PSP_DEVINFO_DATA devData, WPARAM wParam = DBT_DEVICEARRIVAL); + static void enumerateDevicesWin(const GUID & guidDev, QList *infoList); + bool matchAndDispatchChangedDevice(const QString & deviceID, const GUID & guid, WPARAM wParam); #ifdef QT_GUI_LIB - QextSerialRegistrationWidget* notificationWidget; + QextSerialRegistrationWidget *notificationWidget; #endif #endif /*Q_OS_WIN*/ #ifdef Q_OS_UNIX #ifdef Q_OS_MAC - private: - /*! - * Search for serial ports using IOKit. - * \param infoList list with result. - */ - static void scanPortsOSX(QList & infoList); - static void iterateServicesOSX(io_object_t service, QList & infoList); - static bool getServiceDetailsOSX( io_object_t service, QextPortInfo* portInfo ); +private: + /*! + * Search for serial ports using IOKit. + * \param infoList list with result. + */ + static void scanPortsOSX(QList & infoList); + static void iterateServicesOSX(io_object_t service, QList & infoList); + static bool getServiceDetailsOSX(io_object_t service, QextPortInfo *portInfo); - void setUpNotificationOSX( ); - void onDeviceDiscoveredOSX( io_object_t service ); - void onDeviceTerminatedOSX( io_object_t service ); - friend void deviceDiscoveredCallbackOSX( void *ctxt, io_iterator_t serialPortIterator ); - friend void deviceTerminatedCallbackOSX( void *ctxt, io_iterator_t serialPortIterator ); + void setUpNotificationOSX(); + void onDeviceDiscoveredOSX(io_object_t service); + void onDeviceTerminatedOSX(io_object_t service); + friend void deviceDiscoveredCallbackOSX(void *ctxt, io_iterator_t serialPortIterator); + friend void deviceTerminatedCallbackOSX(void *ctxt, io_iterator_t serialPortIterator); - IONotificationPortRef notificationPortRef; + IONotificationPortRef notificationPortRef; #else // Q_OS_MAC - private: - /*! - * Search for serial ports on unix. - * \param infoList list with result. - */ - static void scanPortsNix(QList & infoList); +private: + /*! + * Search for serial ports on unix. + * \param infoList list with result. + */ + static void scanPortsNix(QList & infoList); #endif // Q_OS_MAC #endif /* Q_OS_UNIX */ - public: - /*! - Get list of ports. - \return list of ports currently available in the system. - */ - static QList getPorts(); - /*! - Enable event-driven notifications of board discovery/removal. - */ - void setUpNotifications( ); +public: + /*! + Get list of ports. + \return list of ports currently available in the system. + */ + static QList getPorts(); + /*! + Enable event-driven notifications of board discovery/removal. + */ + void setUpNotifications(); - signals: - /*! - A new device has been connected to the system. +signals: + /*! + A new device has been connected to the system. - setUpNotifications() must be called first to enable event-driven device notifications. - Currently only implemented on Windows and OS X. - \param info The device that has been discovered. - */ - void deviceDiscovered( const QextPortInfo & info ); - /*! - A device has been disconnected from the system. + setUpNotifications() must be called first to enable event-driven device notifications. + Currently only implemented on Windows and OS X. + \param info The device that has been discovered. + */ + void deviceDiscovered(const QextPortInfo & info); + /*! + A device has been disconnected from the system. - setUpNotifications() must be called first to enable event-driven device notifications. - Currently only implemented on Windows and OS X. - \param info The device that was disconnected. - */ - void deviceRemoved( const QextPortInfo & info ); + setUpNotifications() must be called first to enable event-driven device notifications. + Currently only implemented on Windows and OS X. + \param info The device that was disconnected. + */ + void deviceRemoved(const QextPortInfo & info); }; #endif /*_QEXTSERIALENUMERATOR_H_*/ diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_osx.cpp b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_osx.cpp index 9cf05e69c..65f67ba4e 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_osx.cpp +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_osx.cpp @@ -1,6 +1,3 @@ - - - #include "qextserialenumerator.h" #include #include @@ -9,33 +6,35 @@ #include #include -QextSerialEnumerator::QextSerialEnumerator( ) +QextSerialEnumerator::QextSerialEnumerator() { - if( !QMetaType::isRegistered( QMetaType::type("QextPortInfo") ) ) + if (!QMetaType::isRegistered(QMetaType::type("QextPortInfo"))) { qRegisterMetaType("QextPortInfo"); + } } -QextSerialEnumerator::~QextSerialEnumerator( ) +QextSerialEnumerator::~QextSerialEnumerator() { - IONotificationPortDestroy( notificationPortRef ); + IONotificationPortDestroy(notificationPortRef); } // static QList QextSerialEnumerator::getPorts() -{ QList infoList; +{ + QList infoList; io_iterator_t serialPortIterator = 0; kern_return_t kernResult = KERN_FAILURE; CFMutableDictionaryRef matchingDictionary; // first try to get any serialbsd devices, then try any USBCDC devices - if( !(matchingDictionary = IOServiceMatching(kIOSerialBSDServiceValue) ) ) { + if (!(matchingDictionary = IOServiceMatching(kIOSerialBSDServiceValue))) { qWarning("IOServiceMatching returned a NULL dictionary."); return infoList; } CFDictionaryAddValue(matchingDictionary, CFSTR(kIOSerialBSDTypeKey), CFSTR(kIOSerialBSDAllTypes)); // then create the iterator with all the matching devices - if( IOServiceGetMatchingServices(kIOMasterPortDefault, matchingDictionary, &serialPortIterator) != KERN_SUCCESS ) { + if (IOServiceGetMatchingServices(kIOMasterPortDefault, matchingDictionary, &serialPortIterator) != KERN_SUCCESS) { qCritical() << "IOServiceGetMatchingServices failed, returned" << kernResult; return infoList; } @@ -50,23 +49,24 @@ void QextSerialEnumerator::iterateServicesOSX(io_object_t service, QListportName = ioPathName; - if( bsdPathAsCFString ) - { + if (bsdPathAsCFString) { char path[MAXPATHLEN]; - if( CFStringGetCString((CFStringRef)bsdPathAsCFString, path, - PATH_MAX, kCFStringEncodingUTF8) ) { - portInfo->physName = path; + if (CFStringGetCString((CFStringRef)bsdPathAsCFString, path, + PATH_MAX, kCFStringEncodingUTF8)) { + portInfo->physName = path; portInfo->friendName = path; } CFRelease(bsdPathAsCFString); } - if(productNameAsCFString) - { + if (productNameAsCFString) { char productName[MAXPATHLEN]; - if( CFStringGetCString((CFStringRef)productNameAsCFString, productName, - PATH_MAX, kCFStringEncodingUTF8) ) + if (CFStringGetCString((CFStringRef)productNameAsCFString, productName, + PATH_MAX, kCFStringEncodingUTF8)) { portInfo->friendName = productName; + } CFRelease(productNameAsCFString); } - if(vendorIdAsCFNumber) - { + if (vendorIdAsCFNumber) { SInt32 vID; - if(CFNumberGetValue((CFNumberRef)vendorIdAsCFNumber, kCFNumberSInt32Type, &vID)) + if (CFNumberGetValue((CFNumberRef)vendorIdAsCFNumber, kCFNumberSInt32Type, &vID)) { portInfo->vendorID = vID; + } CFRelease(vendorIdAsCFNumber); } - if(productIdAsCFNumber) - { + if (productIdAsCFNumber) { SInt32 pID; - if(CFNumberGetValue((CFNumberRef)productIdAsCFNumber, kCFNumberSInt32Type, &pID)) + if (CFNumberGetValue((CFNumberRef)productIdAsCFNumber, kCFNumberSInt32Type, &pID)) { portInfo->productID = pID; + } CFRelease(productIdAsCFNumber); } IOObjectRelease(service); @@ -140,58 +139,66 @@ bool QextSerialEnumerator::getServiceDetailsOSX( io_object_t service, QextPortIn } // IOKit callbacks registered via setupNotifications() -void deviceDiscoveredCallbackOSX( void *ctxt, io_iterator_t serialPortIterator ); -void deviceTerminatedCallbackOSX( void *ctxt, io_iterator_t serialPortIterator ); +void deviceDiscoveredCallbackOSX(void *ctxt, io_iterator_t serialPortIterator); +void deviceTerminatedCallbackOSX(void *ctxt, io_iterator_t serialPortIterator); -void deviceDiscoveredCallbackOSX( void *ctxt, io_iterator_t serialPortIterator ) +void deviceDiscoveredCallbackOSX(void *ctxt, io_iterator_t serialPortIterator) { - QextSerialEnumerator* qese = (QextSerialEnumerator*)ctxt; + QextSerialEnumerator *qese = (QextSerialEnumerator *)ctxt; io_object_t serialService; - while ((serialService = IOIteratorNext(serialPortIterator))) + + while ((serialService = IOIteratorNext(serialPortIterator))) { qese->onDeviceDiscoveredOSX(serialService); + } } -void deviceTerminatedCallbackOSX( void *ctxt, io_iterator_t serialPortIterator ) +void deviceTerminatedCallbackOSX(void *ctxt, io_iterator_t serialPortIterator) { - QextSerialEnumerator* qese = (QextSerialEnumerator*)ctxt; + QextSerialEnumerator *qese = (QextSerialEnumerator *)ctxt; io_object_t serialService; - while ((serialService = IOIteratorNext(serialPortIterator))) + + while ((serialService = IOIteratorNext(serialPortIterator))) { qese->onDeviceTerminatedOSX(serialService); + } } /* - A device has been discovered via IOKit. - Create a QextPortInfo if possible, and emit the signal indicating that we've found it. -*/ -void QextSerialEnumerator::onDeviceDiscoveredOSX( io_object_t service ) + A device has been discovered via IOKit. + Create a QextPortInfo if possible, and emit the signal indicating that we've found it. + */ +void QextSerialEnumerator::onDeviceDiscoveredOSX(io_object_t service) { QextPortInfo info; - info.vendorID = 0; + + info.vendorID = 0; info.productID = 0; - if( getServiceDetailsOSX( service, &info ) ) - emit deviceDiscovered( info ); + if (getServiceDetailsOSX(service, &info)) { + emit deviceDiscovered(info); + } } /* - Notification via IOKit that a device has been removed. - Create a QextPortInfo if possible, and emit the signal indicating that it's gone. -*/ -void QextSerialEnumerator::onDeviceTerminatedOSX( io_object_t service ) + Notification via IOKit that a device has been removed. + Create a QextPortInfo if possible, and emit the signal indicating that it's gone. + */ +void QextSerialEnumerator::onDeviceTerminatedOSX(io_object_t service) { QextPortInfo info; - info.vendorID = 0; + + info.vendorID = 0; info.productID = 0; - if( getServiceDetailsOSX( service, &info ) ) - emit deviceRemoved( info ); + if (getServiceDetailsOSX(service, &info)) { + emit deviceRemoved(info); + } } /* - Create matching dictionaries for the devices we want to get notifications for, - and add them to the current run loop. Invoke the callbacks that will be responding - to these notifications once to arm them, and discover any devices that - are currently connected at the time notifications are setup. -*/ -void QextSerialEnumerator::setUpNotifications( ) + Create matching dictionaries for the devices we want to get notifications for, + and add them to the current run loop. Invoke the callbacks that will be responding + to these notifications once to arm them, and discover any devices that + are currently connected at the time notifications are setup. + */ +void QextSerialEnumerator::setUpNotifications() { kern_return_t kernResult; mach_port_t masterPort; @@ -207,22 +214,23 @@ void QextSerialEnumerator::setUpNotifications( ) } classesToMatch = IOServiceMatching(kIOSerialBSDServiceValue); - if (classesToMatch == NULL) + if (classesToMatch == NULL) { qDebug("IOServiceMatching returned a NULL dictionary."); - else + } else { CFDictionarySetValue(classesToMatch, CFSTR(kIOSerialBSDTypeKey), CFSTR(kIOSerialBSDAllTypes)); + } - if( !(cdcClassesToMatch = IOServiceNameMatching("AppleUSBCDC") ) ) { + if (!(cdcClassesToMatch = IOServiceNameMatching("AppleUSBCDC"))) { qWarning("couldn't create cdc matching dict"); return; } // Retain an additional reference since each call to IOServiceAddMatchingNotification consumes one. - classesToMatch = (CFMutableDictionaryRef) CFRetain(classesToMatch); - cdcClassesToMatch = (CFMutableDictionaryRef) CFRetain(cdcClassesToMatch); + classesToMatch = (CFMutableDictionaryRef)CFRetain(classesToMatch); + cdcClassesToMatch = (CFMutableDictionaryRef)CFRetain(cdcClassesToMatch); notificationPortRef = IONotificationPortCreate(masterPort); - if(notificationPortRef == NULL) { + if (notificationPortRef == NULL) { qDebug("IONotificationPortCreate return a NULL IONotificationPortRef."); return; } @@ -243,7 +251,7 @@ void QextSerialEnumerator::setUpNotifications( ) } // arm the callback, and grab any devices that are already connected - deviceDiscoveredCallbackOSX( this, portIterator ); + deviceDiscoveredCallbackOSX(this, portIterator); kernResult = IOServiceAddMatchingNotification(notificationPortRef, kIOMatchedNotification, cdcClassesToMatch, deviceDiscoveredCallbackOSX, this, &portIterator); @@ -253,7 +261,7 @@ void QextSerialEnumerator::setUpNotifications( ) } // arm the callback, and grab any devices that are already connected - deviceDiscoveredCallbackOSX( this, portIterator ); + deviceDiscoveredCallbackOSX(this, portIterator); kernResult = IOServiceAddMatchingNotification(notificationPortRef, kIOTerminatedNotification, classesToMatch, deviceTerminatedCallbackOSX, this, &portIterator); @@ -263,7 +271,7 @@ void QextSerialEnumerator::setUpNotifications( ) } // arm the callback, and clear any devices that are terminated - deviceTerminatedCallbackOSX( this, portIterator ); + deviceTerminatedCallbackOSX(this, portIterator); kernResult = IOServiceAddMatchingNotification(notificationPortRef, kIOTerminatedNotification, cdcClassesToMatch, deviceTerminatedCallbackOSX, this, &portIterator); @@ -273,6 +281,5 @@ void QextSerialEnumerator::setUpNotifications( ) } // arm the callback, and clear any devices that are terminated - deviceTerminatedCallbackOSX( this, portIterator ); + deviceTerminatedCallbackOSX(this, portIterator); } - diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_unix.cpp b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_unix.cpp index fd5af0d25..f60127279 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_unix.cpp +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_unix.cpp @@ -1,21 +1,18 @@ - - - #include "qextserialenumerator.h" #include #include #include #include -QextSerialEnumerator::QextSerialEnumerator( ) +QextSerialEnumerator::QextSerialEnumerator() { - if( !QMetaType::isRegistered( QMetaType::type("QextPortInfo") ) ) + if (!QMetaType::isRegistered(QMetaType::type("QextPortInfo"))) { qRegisterMetaType("QextPortInfo"); + } } -QextSerialEnumerator::~QextSerialEnumerator( ) -{ -} +QextSerialEnumerator::~QextSerialEnumerator() +{} QList QextSerialEnumerator::getPorts() { @@ -32,7 +29,7 @@ QList QextSerialEnumerator::getPorts() bool ok; QString current = portNameList.at(i); // remove the ttyS part, and check, if the other part is a number - current.remove(0,4).toInt(&ok, 10); + current.remove(0, 4).toInt(&ok, 10); if (!ok) { portNameList.removeAt(i); i--; @@ -46,33 +43,31 @@ QList QextSerialEnumerator::getPorts() portNamePrefixes << "ttyACM*" << "ttyUSB*" << "rfcomm*"; portNameList.append(dir.entryList(portNamePrefixes, (QDir::System | QDir::Files), QDir::Name)); - foreach (QString str , portNameList) { + foreach(QString str, portNameList) { QextPortInfo inf; - inf.physName = "/dev/"+str; + + inf.physName = "/dev/" + str; inf.portName = str; if (str.contains("ttyS")) { - inf.friendName = "Serial port "+str.remove(0, 4); + inf.friendName = "Serial port " + str.remove(0, 4); + } else if (str.contains("ttyUSB")) { + inf.friendName = "USB-serial adapter " + str.remove(0, 6); + } else if (str.contains("rfcomm")) { + inf.friendName = "Bluetooth-serial adapter " + str.remove(0, 6); + } else if (str.contains("ttyACM")) { + inf.friendName = "USB VCP adapter " + str.remove(0, 6); } - else if (str.contains("ttyUSB")) { - inf.friendName = "USB-serial adapter "+str.remove(0, 6); - } - else if (str.contains("rfcomm")) { - inf.friendName = "Bluetooth-serial adapter "+str.remove(0, 6); - } - else if (str.contains("ttyACM")) { - inf.friendName = "USB VCP adapter "+str.remove(0, 6); - } inf.enumName = "/dev"; // is there a more helpful name for this? infoList.append(inf); } -#else +#else // ifdef Q_OS_LINUX qCritical("Enumeration for POSIX systems (except Linux) is not implemented yet."); -#endif +#endif // ifdef Q_OS_LINUX return infoList; } -void QextSerialEnumerator::setUpNotifications( ) +void QextSerialEnumerator::setUpNotifications() { qCritical("Notifications for *Nix/FreeBSD are not implemented yet"); } diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_win.cpp b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_win.cpp index 6026b5458..550b91a40 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_win.cpp +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_win.cpp @@ -1,6 +1,3 @@ - - - #include "qextserialenumerator.h" #include #include @@ -10,68 +7,72 @@ #include "qextserialport.h" #include -QextSerialEnumerator::QextSerialEnumerator( ) +QextSerialEnumerator::QextSerialEnumerator() { - if( !QMetaType::isRegistered( QMetaType::type("QextPortInfo") ) ) + if (!QMetaType::isRegistered(QMetaType::type("QextPortInfo"))) { qRegisterMetaType("QextPortInfo"); + } #if (defined QT_GUI_LIB) notificationWidget = 0; #endif // Q_OS_WIN } -QextSerialEnumerator::~QextSerialEnumerator( ) +QextSerialEnumerator::~QextSerialEnumerator() { #if (defined QT_GUI_LIB) - if( notificationWidget ) + if (notificationWidget) { delete notificationWidget; + } #endif } - // see http://msdn.microsoft.com/en-us/library/ms791134.aspx for list of GUID classes #ifndef GUID_DEVCLASS_PORTS - DEFINE_GUID(GUID_DEVCLASS_PORTS, 0x4D36E978, 0xE325, 0x11CE, 0xBF, 0xC1, 0x08, 0x00, 0x2B, 0xE1, 0x03, 0x18 ); +DEFINE_GUID(GUID_DEVCLASS_PORTS, 0x4D36E978, 0xE325, 0x11CE, 0xBF, 0xC1, 0x08, 0x00, 0x2B, 0xE1, 0x03, 0x18); #endif /* Gordon Schumacher's macros for TCHAR -> QString conversions and vice versa */ #ifdef UNICODE - #define QStringToTCHAR(x) (wchar_t*) x.utf16() - #define PQStringToTCHAR(x) (wchar_t*) x->utf16() - #define TCHARToQString(x) QString::fromUtf16((ushort*)(x)) - #define TCHARToQStringN(x,y) QString::fromUtf16((ushort*)(x),(y)) + #define QStringToTCHAR(x) (wchar_t *)x.utf16() + #define PQStringToTCHAR(x) (wchar_t *)x->utf16() + #define TCHARToQString(x) QString::fromUtf16((ushort *)(x)) + #define TCHARToQStringN(x, y) QString::fromUtf16((ushort *)(x), (y)) #else #define QStringToTCHAR(x) x.local8Bit().constData() #define PQStringToTCHAR(x) x->local8Bit().constData() #define TCHARToQString(x) QString::fromLocal8Bit((x)) - #define TCHARToQStringN(x,y) QString::fromLocal8Bit((x),(y)) + #define TCHARToQStringN(x, y) QString::fromLocal8Bit((x), (y)) #endif /*UNICODE*/ -//static +// static QString QextSerialEnumerator::getRegKeyValue(HKEY key, LPCTSTR property) { DWORD size = 0; DWORD type; - RegQueryValueEx(key, property, NULL, NULL, NULL, & size); - BYTE* buff = new BYTE[size]; + + RegQueryValueEx(key, property, NULL, NULL, NULL, &size); + BYTE *buff = new BYTE[size]; QString result; - if( RegQueryValueEx(key, property, NULL, &type, buff, & size) == ERROR_SUCCESS ) + if (RegQueryValueEx(key, property, NULL, &type, buff, &size) == ERROR_SUCCESS) { result = TCHARToQString(buff); + } RegCloseKey(key); - delete [] buff; + delete[] buff; return result; } -//static +// static QString QextSerialEnumerator::getDeviceProperty(HDEVINFO devInfo, PSP_DEVINFO_DATA devData, DWORD property) { DWORD buffSize = 0; - SetupDiGetDeviceRegistryProperty(devInfo, devData, property, NULL, NULL, 0, & buffSize); - BYTE* buff = new BYTE[buffSize]; + + SetupDiGetDeviceRegistryProperty(devInfo, devData, property, NULL, NULL, 0, &buffSize); + BYTE *buff = new BYTE[buffSize]; SetupDiGetDeviceRegistryProperty(devInfo, devData, property, NULL, buff, buffSize, NULL); QString result = TCHARToQString(buff); - delete [] buff; + delete[] buff; return result; } @@ -82,18 +83,17 @@ QList QextSerialEnumerator::getPorts() return ports; } -void QextSerialEnumerator::enumerateDevicesWin( const GUID & guid, QList* infoList ) +void QextSerialEnumerator::enumerateDevicesWin(const GUID & guid, QList *infoList) { HDEVINFO devInfo; - if( (devInfo = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT)) != INVALID_HANDLE_VALUE) - { + + if ((devInfo = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT)) != INVALID_HANDLE_VALUE) { SP_DEVINFO_DATA devInfoData; devInfoData.cbSize = sizeof(SP_DEVINFO_DATA); - for(int i = 0; SetupDiEnumDeviceInfo(devInfo, i, &devInfoData); i++) - { + for (int i = 0; SetupDiEnumDeviceInfo(devInfo, i, &devInfoData); i++) { QextPortInfo info; info.productID = info.vendorID = 0; - getDeviceDetailsWin( &info, devInfo, &devInfoData ); + getDeviceDetailsWin(&info, devInfo, &devInfoData); infoList->append(info); } SetupDiDestroyDeviceInfoList(devInfo); @@ -101,10 +101,10 @@ void QextSerialEnumerator::enumerateDevicesWin( const GUID & guid, QListmessage == WM_DEVICECHANGE ) { - qese->onDeviceChangeWin( message->wParam, message->lParam ); + if (message->message == WM_DEVICECHANGE) { + qese->onDeviceChangeWin(message->wParam, message->lParam); *result = 1; return true; } @@ -112,11 +112,12 @@ bool QextSerialRegistrationWidget::winEvent( MSG* message, long* result ) } #endif -void QextSerialEnumerator::setUpNotifications( ) +void QextSerialEnumerator::setUpNotifications() { #ifdef QT_GUI_LIB - if(notificationWidget) + if (notificationWidget) { return; + } notificationWidget = new QextSerialRegistrationWidget(this); DEV_BROADCAST_DEVICEINTERFACE dbh; @@ -124,26 +125,25 @@ void QextSerialEnumerator::setUpNotifications( ) dbh.dbcc_size = sizeof(dbh); dbh.dbcc_devicetype = DBT_DEVTYP_DEVICEINTERFACE; CopyMemory(&dbh.dbcc_classguid, &GUID_DEVCLASS_PORTS, sizeof(GUID)); - if( RegisterDeviceNotification( notificationWidget->winId( ), &dbh, DEVICE_NOTIFY_WINDOW_HANDLE ) == NULL) + if (RegisterDeviceNotification(notificationWidget->winId(), &dbh, DEVICE_NOTIFY_WINDOW_HANDLE) == NULL) { qWarning() << "RegisterDeviceNotification failed:" << GetLastError(); + } // setting up notifications doesn't tell us about devices already connected // so get those manually - foreach( QextPortInfo port, getPorts() ) - emit deviceDiscovered( port ); + foreach(QextPortInfo port, getPorts()) + emit deviceDiscovered(port); #else qWarning("QextSerialEnumerator: GUI not enabled - can't register for device notifications."); #endif // QT_GUI_LIB } -LRESULT QextSerialEnumerator::onDeviceChangeWin( WPARAM wParam, LPARAM lParam ) +LRESULT QextSerialEnumerator::onDeviceChangeWin(WPARAM wParam, LPARAM lParam) { - if ( DBT_DEVICEARRIVAL == wParam || DBT_DEVICEREMOVECOMPLETE == wParam ) - { + if (DBT_DEVICEARRIVAL == wParam || DBT_DEVICEREMOVECOMPLETE == wParam) { PDEV_BROADCAST_HDR pHdr = (PDEV_BROADCAST_HDR)lParam; - if( pHdr->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE ) - { + if (pHdr->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE) { PDEV_BROADCAST_DEVICEINTERFACE pDevInf = (PDEV_BROADCAST_DEVICEINTERFACE)pHdr; - // delimiters are different across APIs...change to backslash. ugh. + // delimiters are different across APIs...change to backslash. ugh. QString deviceID = TCHARToQString(pDevInf->dbcc_name).toUpper().replace("#", "\\"); matchAndDispatchChangedDevice(deviceID, GUID_DEVCLASS_PORTS, wParam); @@ -157,25 +157,24 @@ bool QextSerialEnumerator::matchAndDispatchChangedDevice(const QString & deviceI bool rv = false; DWORD dwFlag = (DBT_DEVICEARRIVAL == wParam) ? DIGCF_PRESENT : DIGCF_ALLCLASSES; HDEVINFO devInfo; - if( (devInfo = SetupDiGetClassDevs(&guid,NULL,NULL,dwFlag)) != INVALID_HANDLE_VALUE ) - { + + if ((devInfo = SetupDiGetClassDevs(&guid, NULL, NULL, dwFlag)) != INVALID_HANDLE_VALUE) { SP_DEVINFO_DATA spDevInfoData; spDevInfoData.cbSize = sizeof(SP_DEVINFO_DATA); - for(int i=0; SetupDiEnumDeviceInfo(devInfo, i, &spDevInfoData); i++) - { - DWORD nSize=0 ; + for (int i = 0; SetupDiEnumDeviceInfo(devInfo, i, &spDevInfoData); i++) { + DWORD nSize = 0; TCHAR buf[MAX_PATH]; - if ( SetupDiGetDeviceInstanceId(devInfo, &spDevInfoData, buf, MAX_PATH, &nSize) && - deviceID.contains(TCHARToQString(buf))) // we found a match - { + if (SetupDiGetDeviceInstanceId(devInfo, &spDevInfoData, buf, MAX_PATH, &nSize) && + deviceID.contains(TCHARToQString(buf))) { // we found a match rv = true; QextPortInfo info; info.productID = info.vendorID = 0; - getDeviceDetailsWin( &info, devInfo, &spDevInfoData, wParam ); - if( wParam == DBT_DEVICEARRIVAL ) + getDeviceDetailsWin(&info, devInfo, &spDevInfoData, wParam); + if (wParam == DBT_DEVICEARRIVAL) { emit deviceDiscovered(info); - else if( wParam == DBT_DEVICEREMOVECOMPLETE ) + } else if (wParam == DBT_DEVICEREMOVECOMPLETE) { emit deviceRemoved(info); + } break; } } @@ -184,23 +183,22 @@ bool QextSerialEnumerator::matchAndDispatchChangedDevice(const QString & deviceI return rv; } -bool QextSerialEnumerator::getDeviceDetailsWin( QextPortInfo* portInfo, HDEVINFO devInfo, PSP_DEVINFO_DATA devData, WPARAM wParam ) +bool QextSerialEnumerator::getDeviceDetailsWin(QextPortInfo *portInfo, HDEVINFO devInfo, PSP_DEVINFO_DATA devData, WPARAM wParam) { portInfo->friendName = getDeviceProperty(devInfo, devData, SPDRP_FRIENDLYNAME); - if( wParam == DBT_DEVICEARRIVAL) + if (wParam == DBT_DEVICEARRIVAL) { portInfo->physName = getDeviceProperty(devInfo, devData, SPDRP_PHYSICAL_DEVICE_OBJECT_NAME); + } portInfo->enumName = getDeviceProperty(devInfo, devData, SPDRP_ENUMERATOR_NAME); QString hardwareIDs = getDeviceProperty(devInfo, devData, SPDRP_HARDWAREID); HKEY devKey = SetupDiOpenDevRegKey(devInfo, devData, DICS_FLAG_GLOBAL, 0, DIREG_DEV, KEY_READ); - portInfo->portName = QextSerialPort::fullPortNameWin( getRegKeyValue(devKey, TEXT("PortName")) ); + portInfo->portName = QextSerialPort::fullPortNameWin(getRegKeyValue(devKey, TEXT("PortName"))); QRegExp idRx("VID_(\\w+)&PID_(\\w+)"); - if( hardwareIDs.toUpper().contains(idRx) ) - { + if (hardwareIDs.toUpper().contains(idRx)) { bool dummy; - portInfo->vendorID = idRx.cap(1).toInt(&dummy, 16); + portInfo->vendorID = idRx.cap(1).toInt(&dummy, 16); portInfo->productID = idRx.cap(2).toInt(&dummy, 16); - //qDebug() << "got vid:" << vid << "pid:" << pid; + // qDebug() << "got vid:" << vid << "pid:" << pid; } return true; } - diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.cpp b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.cpp index ccf359839..c13b7e466 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.cpp +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.cpp @@ -1,35 +1,32 @@ - - #include #include "qextserialport.h" /*! -Default constructor. Note that the name of the device used by a QextSerialPort constructed with -this constructor will be determined by #defined constants, or lack thereof - the default behavior -is the same as _TTY_LINUX_. Possible naming conventions and their associated constants are: + Default constructor. Note that the name of the device used by a QextSerialPort constructed with + this constructor will be determined by #defined constants, or lack thereof - the default behavior + is the same as _TTY_LINUX_. Possible naming conventions and their associated constants are: -\verbatim + \verbatim -Constant Used By Naming Convention ----------- ------------- ------------------------ -Q_OS_WIN Windows COM1, COM2 -_TTY_IRIX_ SGI/IRIX /dev/ttyf1, /dev/ttyf2 -_TTY_HPUX_ HP-UX /dev/tty1p0, /dev/tty2p0 -_TTY_SUN_ SunOS/Solaris /dev/ttya, /dev/ttyb -_TTY_DIGITAL_ Digital UNIX /dev/tty01, /dev/tty02 -_TTY_FREEBSD_ FreeBSD /dev/ttyd0, /dev/ttyd1 -_TTY_OPENBSD_ OpenBSD /dev/tty00, /dev/tty01 -_TTY_LINUX_ Linux /dev/ttyS0, /dev/ttyS1 - Linux /dev/ttyS0, /dev/ttyS1 -\endverbatim + Constant Used By Naming Convention + ---------- ------------- ------------------------ + Q_OS_WIN Windows COM1, COM2 + _TTY_IRIX_ SGI/IRIX /dev/ttyf1, /dev/ttyf2 + _TTY_HPUX_ HP-UX /dev/tty1p0, /dev/tty2p0 + _TTY_SUN_ SunOS/Solaris /dev/ttya, /dev/ttyb + _TTY_DIGITAL_ Digital UNIX /dev/tty01, /dev/tty02 + _TTY_FREEBSD_ FreeBSD /dev/ttyd0, /dev/ttyd1 + _TTY_OPENBSD_ OpenBSD /dev/tty00, /dev/tty01 + _TTY_LINUX_ Linux /dev/ttyS0, /dev/ttyS1 + Linux /dev/ttyS0, /dev/ttyS1 + \endverbatim -This constructor assigns the device name to the name of the first port on the specified system. -See the other constructors if you need to open a different port. -*/ + This constructor assigns the device name to the name of the first port on the specified system. + See the other constructors if you need to open a different port. + */ QextSerialPort::QextSerialPort(QextSerialPort::QueryMode mode) - : QIODevice() + : QIODevice() { - #ifdef Q_OS_WIN setPortName("COM1"); @@ -51,9 +48,9 @@ QextSerialPort::QextSerialPort(QextSerialPort::QueryMode mode) #elif defined(_TTY_OPENBSD_) setPortName("/dev/tty00"); -#else +#else // ifdef Q_OS_WIN setPortName("/dev/ttyS0"); -#endif +#endif // ifdef Q_OS_WIN construct(); setQueryMode(mode); @@ -61,10 +58,10 @@ QextSerialPort::QextSerialPort(QextSerialPort::QueryMode mode) } /*! -Constructs a serial port attached to the port specified by name. -name is the name of the device, which is windowsystem-specific, -e.g."COM1" or "/dev/ttyS0". -*/ + Constructs a serial port attached to the port specified by name. + name is the name of the device, which is windowsystem-specific, + e.g."COM1" or "/dev/ttyS0". + */ QextSerialPort::QextSerialPort(const QString & name, QextSerialPort::QueryMode mode) : QIODevice() { @@ -75,9 +72,9 @@ QextSerialPort::QextSerialPort(const QString & name, QextSerialPort::QueryMode m } /*! -Constructs a port with default name and specified settings. -*/ -QextSerialPort::QextSerialPort(const PortSettings& settings, QextSerialPort::QueryMode mode) + Constructs a port with default name and specified settings. + */ +QextSerialPort::QextSerialPort(const PortSettings & settings, QextSerialPort::QueryMode mode) : QIODevice() { construct(); @@ -92,9 +89,9 @@ QextSerialPort::QextSerialPort(const PortSettings& settings, QextSerialPort::Que } /*! -Constructs a port with specified name and settings. -*/ -QextSerialPort::QextSerialPort(const QString & name, const PortSettings& settings, QextSerialPort::QueryMode mode) + Constructs a port with specified name and settings. + */ +QextSerialPort::QextSerialPort(const QString & name, const PortSettings & settings, QextSerialPort::QueryMode mode) : QIODevice() { construct(); @@ -110,19 +107,19 @@ QextSerialPort::QextSerialPort(const QString & name, const PortSettings& setting } /*! -Common constructor function for setting up default port settings. -(115200 Baud, 8N1, Hardware flow control where supported, otherwise no flow control, and 0 ms timeout). -*/ + Common constructor function for setting up default port settings. + (115200 Baud, 8N1, Hardware flow control where supported, otherwise no flow control, and 0 ms timeout). + */ void QextSerialPort::construct() { lastErr = E_NO_ERROR; - Settings.BaudRate=BAUD115200; - Settings.DataBits=DATA_8; - Settings.Parity=PAR_NONE; - Settings.StopBits=STOP_1; - Settings.FlowControl=FLOW_HARDWARE; - Settings.Timeout_Millisec=500; - mutex = new QMutex( QMutex::Recursive ); + Settings.BaudRate = BAUD115200; + Settings.DataBits = DATA_8; + Settings.Parity = PAR_NONE; + Settings.StopBits = STOP_1; + Settings.FlowControl = FLOW_HARDWARE; + Settings.Timeout_Millisec = 500; + mutex = new QMutex(QMutex::Recursive); setOpenMode(QIODevice::NotOpen); } @@ -132,86 +129,87 @@ void QextSerialPort::setQueryMode(QueryMode mechanism) } /*! -Sets the name of the device associated with the object, e.g. "COM1", or "/dev/ttyS0". -*/ + Sets the name of the device associated with the object, e.g. "COM1", or "/dev/ttyS0". + */ void QextSerialPort::setPortName(const QString & name) { #ifdef Q_OS_WIN - port = fullPortNameWin( name ); + port = fullPortNameWin(name); #else port = name; #endif } /*! -Returns the name set by setPortName(). -*/ + Returns the name set by setPortName(). + */ QString QextSerialPort::portName() const { return port; } /*! - Reads all available data from the device, and returns it as a QByteArray. - This function has no way of reporting errors; returning an empty QByteArray() - can mean either that no data was currently available for reading, or that an error occurred. -*/ + Reads all available data from the device, and returns it as a QByteArray. + This function has no way of reporting errors; returning an empty QByteArray() + can mean either that no data was currently available for reading, or that an error occurred. + */ QByteArray QextSerialPort::readAll() { int avail = this->bytesAvailable(); + return (avail > 0) ? this->read(avail) : QByteArray(); } /*! -Returns the baud rate of the serial port. For a list of possible return values see -the definition of the enum BaudRateType. -*/ + Returns the baud rate of the serial port. For a list of possible return values see + the definition of the enum BaudRateType. + */ BaudRateType QextSerialPort::baudRate(void) const { return Settings.BaudRate; } /*! -Returns the number of data bits used by the port. For a list of possible values returned by -this function, see the definition of the enum DataBitsType. -*/ + Returns the number of data bits used by the port. For a list of possible values returned by + this function, see the definition of the enum DataBitsType. + */ DataBitsType QextSerialPort::dataBits() const { return Settings.DataBits; } /*! -Returns the type of parity used by the port. For a list of possible values returned by -this function, see the definition of the enum ParityType. -*/ + Returns the type of parity used by the port. For a list of possible values returned by + this function, see the definition of the enum ParityType. + */ ParityType QextSerialPort::parity() const { return Settings.Parity; } /*! -Returns the number of stop bits used by the port. For a list of possible return values, see -the definition of the enum StopBitsType. -*/ + Returns the number of stop bits used by the port. For a list of possible return values, see + the definition of the enum StopBitsType. + */ StopBitsType QextSerialPort::stopBits() const { return Settings.StopBits; } /*! -Returns the type of flow control used by the port. For a list of possible values returned -by this function, see the definition of the enum FlowType. -*/ + Returns the type of flow control used by the port. For a list of possible values returned + by this function, see the definition of the enum FlowType. + */ FlowType QextSerialPort::flowControl() const { return Settings.FlowControl; } /*! -Returns true if device is sequential, otherwise returns false. Serial port is sequential device -so this function always returns true. Check QIODevice::isSequential() documentation for more -information. -*/ + Returns true if device is sequential, otherwise returns false. Serial port is sequential device + so this function always returns true. Check QIODevice::isSequential() documentation for more + information. + */ bool QextSerialPort::isSequential() const { return true; @@ -219,31 +217,46 @@ bool QextSerialPort::isSequential() const QString QextSerialPort::errorString() { - switch(lastErr) - { - case E_NO_ERROR: return "No Error has occurred"; - case E_INVALID_FD: return "Invalid file descriptor (port was not opened correctly)"; - case E_NO_MEMORY: return "Unable to allocate memory tables (POSIX)"; - case E_CAUGHT_NON_BLOCKED_SIGNAL: return "Caught a non-blocked signal (POSIX)"; - case E_PORT_TIMEOUT: return "Operation timed out (POSIX)"; - case E_INVALID_DEVICE: return "The file opened by the port is not a valid device"; - case E_BREAK_CONDITION: return "The port detected a break condition"; - case E_FRAMING_ERROR: return "The port detected a framing error (usually caused by incorrect baud rate settings)"; - case E_IO_ERROR: return "There was an I/O error while communicating with the port"; - case E_BUFFER_OVERRUN: return "Character buffer overrun"; - case E_RECEIVE_OVERFLOW: return "Receive buffer overflow"; - case E_RECEIVE_PARITY_ERROR: return "The port detected a parity error in the received data"; - case E_TRANSMIT_OVERFLOW: return "Transmit buffer overflow"; - case E_READ_FAILED: return "General read operation failure"; - case E_WRITE_FAILED: return "General write operation failure"; - case E_FILE_NOT_FOUND: return "The "+this->portName()+" file doesn't exists"; - default: return QString("Unknown error: %1").arg(lastErr); + switch (lastErr) { + case E_NO_ERROR: return "No Error has occurred"; + + case E_INVALID_FD: return "Invalid file descriptor (port was not opened correctly)"; + + case E_NO_MEMORY: return "Unable to allocate memory tables (POSIX)"; + + case E_CAUGHT_NON_BLOCKED_SIGNAL: return "Caught a non-blocked signal (POSIX)"; + + case E_PORT_TIMEOUT: return "Operation timed out (POSIX)"; + + case E_INVALID_DEVICE: return "The file opened by the port is not a valid device"; + + case E_BREAK_CONDITION: return "The port detected a break condition"; + + case E_FRAMING_ERROR: return "The port detected a framing error (usually caused by incorrect baud rate settings)"; + + case E_IO_ERROR: return "There was an I/O error while communicating with the port"; + + case E_BUFFER_OVERRUN: return "Character buffer overrun"; + + case E_RECEIVE_OVERFLOW: return "Receive buffer overflow"; + + case E_RECEIVE_PARITY_ERROR: return "The port detected a parity error in the received data"; + + case E_TRANSMIT_OVERFLOW: return "Transmit buffer overflow"; + + case E_READ_FAILED: return "General read operation failure"; + + case E_WRITE_FAILED: return "General write operation failure"; + + case E_FILE_NOT_FOUND: return "The " + this->portName() + " file doesn't exists"; + + default: return QString("Unknown error: %1").arg(lastErr); } } /*! -Standard destructor. -*/ + Standard destructor. + */ QextSerialPort::~QextSerialPort() { if (isOpen()) { diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.h b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.h index 8d3ac29a6..57d3a6931 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.h +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.h @@ -1,4 +1,3 @@ - #ifndef _QEXTSERIALPORT_H_ #define _QEXTSERIALPORT_H_ @@ -18,31 +17,31 @@ #ifdef _TTY_NOWARN_ #define TTY_WARNING(s) #else -#define TTY_WARNING(s) qWarning(s) +#define TTY_WARNING(s) qWarning(s) #endif /*_TTY_NOWARN_*/ /*line status constants*/ -#define LS_CTS 0x01 -#define LS_DSR 0x02 -#define LS_DCD 0x04 -#define LS_RI 0x08 -#define LS_RTS 0x10 -#define LS_DTR 0x20 -#define LS_ST 0x40 -#define LS_SR 0x80 +#define LS_CTS 0x01 +#define LS_DSR 0x02 +#define LS_DCD 0x04 +#define LS_RI 0x08 +#define LS_RTS 0x10 +#define LS_DTR 0x20 +#define LS_ST 0x40 +#define LS_SR 0x80 /*error constants*/ -#define E_NO_ERROR 0 -#define E_INVALID_FD 1 -#define E_NO_MEMORY 2 -#define E_CAUGHT_NON_BLOCKED_SIGNAL 3 -#define E_PORT_TIMEOUT 4 -#define E_INVALID_DEVICE 5 -#define E_BREAK_CONDITION 6 -#define E_FRAMING_ERROR 7 -#define E_IO_ERROR 8 -#define E_BUFFER_OVERRUN 9 +#define E_NO_ERROR 0 +#define E_INVALID_FD 1 +#define E_NO_MEMORY 2 +#define E_CAUGHT_NON_BLOCKED_SIGNAL 3 +#define E_PORT_TIMEOUT 4 +#define E_INVALID_DEVICE 5 +#define E_BREAK_CONDITION 6 +#define E_FRAMING_ERROR 7 +#define E_IO_ERROR 8 +#define E_BUFFER_OVERRUN 9 #define E_RECEIVE_OVERFLOW 10 #define E_RECEIVE_PARITY_ERROR 11 #define E_TRANSMIT_OVERFLOW 12 @@ -50,61 +49,56 @@ #define E_WRITE_FAILED 14 #define E_FILE_NOT_FOUND 15 -enum BaudRateType -{ - BAUD50, //POSIX ONLY - BAUD75, //POSIX ONLY +enum BaudRateType { + BAUD50, // POSIX ONLY + BAUD75, // POSIX ONLY BAUD110, - BAUD134, //POSIX ONLY - BAUD150, //POSIX ONLY - BAUD200, //POSIX ONLY + BAUD134, // POSIX ONLY + BAUD150, // POSIX ONLY + BAUD200, // POSIX ONLY BAUD300, BAUD600, BAUD1200, - BAUD1800, //POSIX ONLY + BAUD1800, // POSIX ONLY BAUD2400, BAUD4800, BAUD9600, - BAUD14400, //WINDOWS ONLY + BAUD14400, // WINDOWS ONLY BAUD19200, BAUD38400, - BAUD56000, //WINDOWS ONLY + BAUD56000, // WINDOWS ONLY BAUD57600, - BAUD76800, //POSIX ONLY + BAUD76800, // POSIX ONLY BAUD115200, - BAUD128000, //WINDOWS ONLY - BAUD230400, //WINDOWS ONLY - BAUD256000, //WINDOWS ONLY - BAUD460800, //WINDOWS ONLY - BAUD921600 //WINDOWS ONLY + BAUD128000, // WINDOWS ONLY + BAUD230400, // WINDOWS ONLY + BAUD256000, // WINDOWS ONLY + BAUD460800, // WINDOWS ONLY + BAUD921600 // WINDOWS ONLY }; -enum DataBitsType -{ +enum DataBitsType { DATA_5, DATA_6, DATA_7, DATA_8 }; -enum ParityType -{ +enum ParityType { PAR_NONE, PAR_ODD, PAR_EVEN, - PAR_MARK, //WINDOWS ONLY + PAR_MARK, // WINDOWS ONLY PAR_SPACE }; -enum StopBitsType -{ +enum StopBitsType { STOP_1, - STOP_1_5, //WINDOWS ONLY + STOP_1_5, // WINDOWS ONLY STOP_2 }; -enum FlowType -{ +enum FlowType { FLOW_OFF, FLOW_HARDWARE, FLOW_XONXOFF @@ -113,13 +107,12 @@ enum FlowType /** * structure to contain port settings */ -struct PortSettings -{ +struct PortSettings { BaudRateType BaudRate; DataBitsType DataBits; - ParityType Parity; + ParityType Parity; StopBitsType StopBits; - FlowType FlowControl; + FlowType FlowControl; long Timeout_Millisec; }; @@ -143,23 +136,23 @@ struct PortSettings #endif /*! -Encapsulates a serial port on both POSIX and Windows systems. + Encapsulates a serial port on both POSIX and Windows systems. -\note -Be sure to check the full list of members, as QIODevice provides quite a lot of -functionality for QextSerialPort. + \note + Be sure to check the full list of members, as QIODevice provides quite a lot of + functionality for QextSerialPort. -\section Usage -QextSerialPort offers both a polling and event driven API. Event driven is typically easier -to use, since you never have to worry about checking for new data. + \section Usage + QextSerialPort offers both a polling and event driven API. Event driven is typically easier + to use, since you never have to worry about checking for new data. -\b Example -\code -QextSerialPort* port = new QextSerialPort("COM1", QextSerialPort::EventDriven); -connect(port, SIGNAL(readyRead()), myClass, SLOT(onDataAvailable())); -port->open(); + \b Example + \code + QextSerialPort* port = new QextSerialPort("COM1", QextSerialPort::EventDriven); + connect(port, SIGNAL(readyRead()), myClass, SLOT(onDataAvailable())); + port->open(); -void MyClass::onDataAvailable() { + void MyClass::onDataAvailable() { int avail = port->bytesAvailable(); if( avail > 0 ) { QByteArray usbdata; @@ -169,172 +162,173 @@ void MyClass::onDataAvailable() { processNewData(usbdata); } } -} -\endcode + } + \endcode -\section Compatibility -The user will be notified of errors and possible portability conflicts at run-time -by default - this behavior can be turned off by defining _TTY_NOWARN_ -(to turn off all warnings) or _TTY_NOWARN_PORT_ (to turn off portability warnings) in the project. + \section Compatibility + The user will be notified of errors and possible portability conflicts at run-time + by default - this behavior can be turned off by defining _TTY_NOWARN_ + (to turn off all warnings) or _TTY_NOWARN_PORT_ (to turn off portability warnings) in the project. -On Windows NT/2000/XP this class uses Win32 serial port functions by default. The user may -select POSIX behavior under NT, 2000, or XP ONLY by defining Q_OS_UNIX in the project. -No guarantees are made as to the quality of POSIX support under NT/2000 however. + On Windows NT/2000/XP this class uses Win32 serial port functions by default. The user may + select POSIX behavior under NT, 2000, or XP ONLY by defining Q_OS_UNIX in the project. + No guarantees are made as to the quality of POSIX support under NT/2000 however. -\author Stefan Sander, Michal Policht, Brandon Fosdick, Liam Staskawicz -*/ -class QEXTSERIALPORT_EXPORT QextSerialPort: public QIODevice -{ + \author Stefan Sander, Michal Policht, Brandon Fosdick, Liam Staskawicz + */ +class QEXTSERIALPORT_EXPORT QextSerialPort : public QIODevice { Q_OBJECT - public: - enum QueryMode { - Polling, - EventDriven - }; +public: + enum QueryMode { + Polling, + EventDriven + }; - QextSerialPort(QueryMode mode = EventDriven); - QextSerialPort(const QString & name, QueryMode mode = EventDriven); - QextSerialPort(PortSettings const& s, QueryMode mode = EventDriven); - QextSerialPort(const QString & name, PortSettings const& s, QueryMode mode = EventDriven); - ~QextSerialPort(); + QextSerialPort(QueryMode mode = EventDriven); + QextSerialPort(const QString & name, QueryMode mode = EventDriven); + QextSerialPort(PortSettings const & s, QueryMode mode = EventDriven); + QextSerialPort(const QString & name, PortSettings const & s, QueryMode mode = EventDriven); + ~QextSerialPort(); - void setPortName(const QString & name); - QString portName() const; + void setPortName(const QString & name); + QString portName() const; - /**! - * Get query mode. - * \return query mode. - */ - inline QueryMode queryMode() const { return _queryMode; } + /**! + * Get query mode. + * \return query mode. + */ + inline QueryMode queryMode() const + { + return _queryMode; + } - /*! - * Set desired serial communication handling style. You may choose from polling - * or event driven approach. This function does nothing when port is open; to - * apply changes port must be reopened. - * - * In event driven approach read() and write() functions are acting - * asynchronously. They return immediately and the operation is performed in - * the background, so they doesn't freeze the calling thread. - * To determine when operation is finished, QextSerialPort runs separate thread - * and monitors serial port events. Whenever the event occurs, adequate signal - * is emitted. - * - * When polling is set, read() and write() are acting synchronously. Signals are - * not working in this mode and some functions may not be available. The advantage - * of polling is that it generates less overhead due to lack of signals emissions - * and it doesn't start separate thread to monitor events. - * - * Generally event driven approach is more capable and friendly, although some - * applications may need as low overhead as possible and then polling comes. - * - * \param mode query mode. - */ - void setQueryMode(QueryMode mode); + /*! + * Set desired serial communication handling style. You may choose from polling + * or event driven approach. This function does nothing when port is open; to + * apply changes port must be reopened. + * + * In event driven approach read() and write() functions are acting + * asynchronously. They return immediately and the operation is performed in + * the background, so they doesn't freeze the calling thread. + * To determine when operation is finished, QextSerialPort runs separate thread + * and monitors serial port events. Whenever the event occurs, adequate signal + * is emitted. + * + * When polling is set, read() and write() are acting synchronously. Signals are + * not working in this mode and some functions may not be available. The advantage + * of polling is that it generates less overhead due to lack of signals emissions + * and it doesn't start separate thread to monitor events. + * + * Generally event driven approach is more capable and friendly, although some + * applications may need as low overhead as possible and then polling comes. + * + * \param mode query mode. + */ + void setQueryMode(QueryMode mode); - void setBaudRate(BaudRateType); - BaudRateType baudRate() const; + void setBaudRate(BaudRateType); + BaudRateType baudRate() const; - void setDataBits(DataBitsType); - DataBitsType dataBits() const; + void setDataBits(DataBitsType); + DataBitsType dataBits() const; - void setParity(ParityType); - ParityType parity() const; + void setParity(ParityType); + ParityType parity() const; - void setStopBits(StopBitsType); - StopBitsType stopBits() const; + void setStopBits(StopBitsType); + StopBitsType stopBits() const; - void setFlowControl(FlowType); - FlowType flowControl() const; + void setFlowControl(FlowType); + FlowType flowControl() const; - void setTimeout(long); + void setTimeout(long); - bool open(OpenMode mode); - bool isSequential() const; - void close(); - void flush(); + bool open(OpenMode mode); + bool isSequential() const; + void close(); + void flush(); - qint64 size() const; - qint64 bytesAvailable() const; - QByteArray readAll(); + qint64 size() const; + qint64 bytesAvailable() const; + QByteArray readAll(); - void ungetChar(char c); + void ungetChar(char c); - ulong lastError() const; - void translateError(ulong error); + ulong lastError() const; + void translateError(ulong error); - void setDtr(bool set=true); - void setRts(bool set=true); - ulong lineStatus(); - QString errorString(); + void setDtr(bool set = true); + void setRts(bool set = true); + ulong lineStatus(); + QString errorString(); #ifdef Q_OS_WIN - virtual bool waitForReadyRead(int msecs); ///< @todo implement. - virtual qint64 bytesToWrite() const; - static QString fullPortNameWin(const QString & name); + virtual bool waitForReadyRead(int msecs); ///< @todo implement. + virtual qint64 bytesToWrite() const; + static QString fullPortNameWin(const QString & name); #endif - protected: - QMutex* mutex; - QString port; - PortSettings Settings; - ulong lastErr; - QueryMode _queryMode; +protected: + QMutex *mutex; + QString port; + PortSettings Settings; + ulong lastErr; + QueryMode _queryMode; - // platform specific members + // platform specific members #ifdef Q_OS_UNIX - int fd; - QSocketNotifier *readNotifier; - struct termios Posix_CommConfig; - struct termios old_termios; - struct timeval Posix_Timeout; - struct timeval Posix_Copy_Timeout; + int fd; + QSocketNotifier *readNotifier; + struct termios Posix_CommConfig; + struct termios old_termios; + struct timeval Posix_Timeout; + struct timeval Posix_Copy_Timeout; #elif (defined Q_OS_WIN) - HANDLE Win_Handle; - OVERLAPPED overlap; - COMMCONFIG Win_CommConfig; - COMMTIMEOUTS Win_CommTimeouts; - QWinEventNotifier *winEventNotifier; - DWORD eventMask; - QList pendingWrites; - QReadWriteLock* bytesToWriteLock; - qint64 _bytesToWrite; + HANDLE Win_Handle; + OVERLAPPED overlap; + COMMCONFIG Win_CommConfig; + COMMTIMEOUTS Win_CommTimeouts; + QWinEventNotifier *winEventNotifier; + DWORD eventMask; + QList pendingWrites; + QReadWriteLock *bytesToWriteLock; + qint64 _bytesToWrite; #endif - void construct(); // common construction - void platformSpecificDestruct(); - void platformSpecificInit(); - qint64 readData(char * data, qint64 maxSize); - qint64 writeData(const char * data, qint64 maxSize); + void construct(); // common construction + void platformSpecificDestruct(); + void platformSpecificInit(); + qint64 readData(char *data, qint64 maxSize); + qint64 writeData(const char *data, qint64 maxSize); #ifdef Q_OS_WIN - private slots: - void onWinEvent(HANDLE h); - void triggerTxEmpty(); +private slots: + void onWinEvent(HANDLE h); + void triggerTxEmpty(); private: - QTimer fakeTxEmpty; + QTimer fakeTxEmpty; #endif - private: - Q_DISABLE_COPY(QextSerialPort) +private: + Q_DISABLE_COPY(QextSerialPort) - signals: -// /** -// * This signal is emitted whenever port settings are updated. -// * \param valid \p true if settings are valid, \p false otherwise. -// * -// * @todo implement. -// */ -// // void validSettings(bool valid); - - /*! - * This signal is emitted whenever dsr line has changed its state. You may - * use this signal to check if device is connected. - * \param status \p true when DSR signal is on, \p false otherwise. - * - * \see lineStatus(). - */ - void dsrChanged(bool status); +signals: +///** +// * This signal is emitted whenever port settings are updated. +// * \param valid \p true if settings are valid, \p false otherwise. +// * +// * @todo implement. +// */ +//// void validSettings(bool valid); + /*! + * This signal is emitted whenever dsr line has changed its state. You may + * use this signal to check if device is connected. + * \param status \p true when DSR signal is on, \p false otherwise. + * + * \see lineStatus(). + */ + void dsrChanged(bool status); }; -#endif +#endif // ifndef _QEXTSERIALPORT_H_ diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport_global.h b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport_global.h index a2e65346b..c8210867b 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport_global.h +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport_global.h @@ -1,5 +1,3 @@ - - #ifndef QEXTSERIALPORT_GLOBAL_H #define QEXTSERIALPORT_GLOBAL_H @@ -12,4 +10,3 @@ #endif #endif // QEXTSERIALPORT_GLOBAL_H - diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/win_qextserialport.cpp b/ground/openpilotgcs/src/libs/qextserialport/src/win_qextserialport.cpp index 1fe0ff3ba..d44cbf03c 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/win_qextserialport.cpp +++ b/ground/openpilotgcs/src/libs/qextserialport/src/win_qextserialport.cpp @@ -1,30 +1,29 @@ - - #include #include #include #include "qextserialport.h" #if (defined Q_OS_WIN) - #define CBR_230400 230400 - #define CBR_460800 460800 - #define CBR_921600 921600 + #define CBR_230400 230400 + #define CBR_460800 460800 + #define CBR_921600 921600 #endif void QextSerialPort::platformSpecificInit() { - Win_Handle=INVALID_HANDLE_VALUE; + Win_Handle = INVALID_HANDLE_VALUE; ZeroMemory(&overlap, sizeof(OVERLAPPED)); - overlap.hEvent = CreateEvent(NULL, true, false, NULL); + overlap.hEvent = CreateEvent(NULL, true, false, NULL); winEventNotifier = 0; bytesToWriteLock = new QReadWriteLock; - _bytesToWrite = 0; + _bytesToWrite = 0; } /*! -Standard destructor. -*/ -void QextSerialPort::platformSpecificDestruct() { + Standard destructor. + */ +void QextSerialPort::platformSpecificDestruct() +{ CloseHandle(overlap.hEvent); delete bytesToWriteLock; } @@ -33,45 +32,51 @@ QString QextSerialPort::fullPortNameWin(const QString & name) { QRegExp rx("^COM(\\d+)"); QString fullName(name); - if(fullName.contains(rx)) { + + if (fullName.contains(rx)) { int portnum = rx.cap(1).toInt(); - if(portnum > 9) // COM ports greater than 9 need \\.\ prepended + if (portnum > 9) { // COM ports greater than 9 need \\.\ prepended fullName.prepend("\\\\.\\"); + } } return fullName; } /*! -Opens a serial port. Note that this function does not specify which device to open. If you need -to open a device by name, see QextSerialPort::open(const char*). This function has no effect -if the port associated with the class is already open. The port is also configured to the current -settings, as stored in the Settings structure. -*/ -bool QextSerialPort::open(OpenMode mode) { - unsigned long confSize = sizeof(COMMCONFIG); + Opens a serial port. Note that this function does not specify which device to open. If you need + to open a device by name, see QextSerialPort::open(const char*). This function has no effect + if the port associated with the class is already open. The port is also configured to the current + settings, as stored in the Settings structure. + */ +bool QextSerialPort::open(OpenMode mode) +{ + unsigned long confSize = sizeof(COMMCONFIG); + Win_CommConfig.dwSize = confSize; DWORD dwFlagsAndAttributes = 0; - if (queryMode() == QextSerialPort::EventDriven) + if (queryMode() == QextSerialPort::EventDriven) { dwFlagsAndAttributes += FILE_FLAG_OVERLAPPED; + } QMutexLocker lock(mutex); - if (mode == QIODevice::NotOpen) + if (mode == QIODevice::NotOpen) { return isOpen(); + } if (!isOpen()) { /*open the port*/ - Win_Handle=CreateFileA(port.toAscii(), GENERIC_READ|GENERIC_WRITE, - 0, NULL, OPEN_EXISTING, dwFlagsAndAttributes, NULL); - if (Win_Handle!=INVALID_HANDLE_VALUE) { + Win_Handle = CreateFileA(port.toAscii(), GENERIC_READ | GENERIC_WRITE, + 0, NULL, OPEN_EXISTING, dwFlagsAndAttributes, NULL); + if (Win_Handle != INVALID_HANDLE_VALUE) { QIODevice::open(mode); /*configure port settings*/ GetCommConfig(Win_Handle, &Win_CommConfig, &confSize); GetCommState(Win_Handle, &(Win_CommConfig.dcb)); /*set up parameters*/ - Win_CommConfig.dcb.fBinary=TRUE; - Win_CommConfig.dcb.fInX=FALSE; - Win_CommConfig.dcb.fOutX=FALSE; - Win_CommConfig.dcb.fAbortOnError=FALSE; - Win_CommConfig.dcb.fNull=FALSE; + Win_CommConfig.dcb.fBinary = TRUE; + Win_CommConfig.dcb.fInX = FALSE; + Win_CommConfig.dcb.fOutX = FALSE; + Win_CommConfig.dcb.fAbortOnError = FALSE; + Win_CommConfig.dcb.fNull = FALSE; setBaudRate(Settings.BaudRate); setDataBits(Settings.DataBits); setStopBits(Settings.StopBits); @@ -80,24 +85,23 @@ bool QextSerialPort::open(OpenMode mode) { setTimeout(Settings.Timeout_Millisec); SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - //init event driven approach + // init event driven approach if (queryMode() == QextSerialPort::EventDriven) { Win_CommTimeouts.ReadIntervalTimeout = MAXDWORD; - Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; - Win_CommTimeouts.ReadTotalTimeoutConstant = 0; + Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; + Win_CommTimeouts.ReadTotalTimeoutConstant = 0; Win_CommTimeouts.WriteTotalTimeoutMultiplier = 0; - Win_CommTimeouts.WriteTotalTimeoutConstant = 0; + Win_CommTimeouts.WriteTotalTimeoutConstant = 0; SetCommTimeouts(Win_Handle, &Win_CommTimeouts); - if (!SetCommMask( Win_Handle, EV_TXEMPTY | EV_RXCHAR | EV_DSR)) { + if (!SetCommMask(Win_Handle, EV_TXEMPTY | EV_RXCHAR | EV_DSR)) { qWarning() << "failed to set Comm Mask. Error code:", GetLastError(); return false; } winEventNotifier = new QWinEventNotifier(overlap.hEvent, this); connect(winEventNotifier, SIGNAL(activated(HANDLE)), this, SLOT(onWinEvent(HANDLE))); - connect(&fakeTxEmpty,SIGNAL(timeout()),this,SLOT(triggerTxEmpty())); + connect(&fakeTxEmpty, SIGNAL(timeout()), this, SLOT(triggerTxEmpty())); fakeTxEmpty.start(10000); WaitCommEvent(Win_Handle, &eventMask, &overlap); - } } } else { @@ -107,26 +111,29 @@ bool QextSerialPort::open(OpenMode mode) { } /*! -Closes a serial port. This function has no effect if the serial port associated with the class -is not currently open. -*/ + Closes a serial port. This function has no effect if the serial port associated with the class + is not currently open. + */ void QextSerialPort::close() { QMutexLocker lock(mutex); + fakeTxEmpty.stop(); - disconnect(&fakeTxEmpty,SIGNAL(timeout()),this,SLOT(triggerTxEmpty())); + disconnect(&fakeTxEmpty, SIGNAL(timeout()), this, SLOT(triggerTxEmpty())); if (isOpen()) { - flush(); + flush(); QIODevice::close(); // mark ourselves as closed CancelIo(Win_Handle); - if (CloseHandle(Win_Handle)) + if (CloseHandle(Win_Handle)) { Win_Handle = INVALID_HANDLE_VALUE; - if (winEventNotifier) + } + if (winEventNotifier) { winEventNotifier->deleteLater(); + } _bytesToWrite = 0; - foreach(OVERLAPPED* o, pendingWrites) { + foreach(OVERLAPPED * o, pendingWrites) { CloseHandle(o->hEvent); delete o; } @@ -135,667 +142,668 @@ void QextSerialPort::close() } /*! -Flushes all pending I/O to the serial port. This function has no effect if the serial port -associated with the class is not currently open. -*/ -void QextSerialPort::flush() { + Flushes all pending I/O to the serial port. This function has no effect if the serial port + associated with the class is not currently open. + */ +void QextSerialPort::flush() +{ QMutexLocker lock(mutex); + if (isOpen()) { FlushFileBuffers(Win_Handle); } } /*! -This function will return the number of bytes waiting in the receive queue of the serial port. -It is included primarily to provide a complete QIODevice interface, and will not record errors -in the lastErr member (because it is const). This function is also not thread-safe - in -multithreading situations, use QextSerialPort::bytesAvailable() instead. -*/ -qint64 QextSerialPort::size() const { + This function will return the number of bytes waiting in the receive queue of the serial port. + It is included primarily to provide a complete QIODevice interface, and will not record errors + in the lastErr member (because it is const). This function is also not thread-safe - in + multithreading situations, use QextSerialPort::bytesAvailable() instead. + */ +qint64 QextSerialPort::size() const +{ int availBytes; COMSTAT Win_ComStat; - DWORD Win_ErrorMask=0; + DWORD Win_ErrorMask = 0; + ClearCommError(Win_Handle, &Win_ErrorMask, &Win_ComStat); availBytes = Win_ComStat.cbInQue; return (qint64)availBytes; } /*! -Returns the number of bytes waiting in the port's receive queue. This function will return 0 if -the port is not currently open, or -1 on error. -*/ -qint64 QextSerialPort::bytesAvailable() const { + Returns the number of bytes waiting in the port's receive queue. This function will return 0 if + the port is not currently open, or -1 on error. + */ +qint64 QextSerialPort::bytesAvailable() const +{ QMutexLocker lock(mutex); + if (isOpen()) { DWORD Errors; COMSTAT Status; if (ClearCommError(Win_Handle, &Errors, &Status)) { return Status.cbInQue + QIODevice::bytesAvailable(); } - return (qint64)-1; + return (qint64) - 1; } return 0; } /*! -Translates a system-specific error code to a QextSerialPort error code. Used internally. -*/ -void QextSerialPort::translateError(ulong error) { - if (error&CE_BREAK) { - lastErr=E_BREAK_CONDITION; - } - else if (error&CE_FRAME) { - lastErr=E_FRAMING_ERROR; - } - else if (error&CE_IOE) { - lastErr=E_IO_ERROR; - } - else if (error&CE_MODE) { - lastErr=E_INVALID_FD; - } - else if (error&CE_OVERRUN) { - lastErr=E_BUFFER_OVERRUN; - } - else if (error&CE_RXPARITY) { - lastErr=E_RECEIVE_PARITY_ERROR; - } - else if (error&CE_RXOVER) { - lastErr=E_RECEIVE_OVERFLOW; - } - else if (error&CE_TXFULL) { - lastErr=E_TRANSMIT_OVERFLOW; + Translates a system-specific error code to a QextSerialPort error code. Used internally. + */ +void QextSerialPort::translateError(ulong error) +{ + if (error & CE_BREAK) { + lastErr = E_BREAK_CONDITION; + } else if (error & CE_FRAME) { + lastErr = E_FRAMING_ERROR; + } else if (error & CE_IOE) { + lastErr = E_IO_ERROR; + } else if (error & CE_MODE) { + lastErr = E_INVALID_FD; + } else if (error & CE_OVERRUN) { + lastErr = E_BUFFER_OVERRUN; + } else if (error & CE_RXPARITY) { + lastErr = E_RECEIVE_PARITY_ERROR; + } else if (error & CE_RXOVER) { + lastErr = E_RECEIVE_OVERFLOW; + } else if (error & CE_TXFULL) { + lastErr = E_TRANSMIT_OVERFLOW; } } /*! -Reads a block of data from the serial port. This function will read at most maxlen bytes from -the serial port and place them in the buffer pointed to by data. Return value is the number of -bytes actually read, or -1 on error. + Reads a block of data from the serial port. This function will read at most maxlen bytes from + the serial port and place them in the buffer pointed to by data. Return value is the number of + bytes actually read, or -1 on error. -\warning before calling this function ensure that serial port associated with this class -is currently open (use isOpen() function to check if port is open). -*/ + \warning before calling this function ensure that serial port associated with this class + is currently open (use isOpen() function to check if port is open). + */ qint64 QextSerialPort::readData(char *data, qint64 maxSize) { DWORD retVal; QMutexLocker lock(mutex); + retVal = 0; if (queryMode() == QextSerialPort::EventDriven) { OVERLAPPED overlapRead; ZeroMemory(&overlapRead, sizeof(OVERLAPPED)); - if (!ReadFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, & overlapRead)) { - if (GetLastError() == ERROR_IO_PENDING) - GetOverlappedResult(Win_Handle, & overlapRead, & retVal, true); - else { + if (!ReadFile(Win_Handle, (void *)data, (DWORD)maxSize, &retVal, &overlapRead)) { + if (GetLastError() == ERROR_IO_PENDING) { + GetOverlappedResult(Win_Handle, &overlapRead, &retVal, true); + } else { lastErr = E_READ_FAILED; - retVal = (DWORD)-1; + retVal = (DWORD)-1; } } - } else if (!ReadFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, NULL)) { + } else if (!ReadFile(Win_Handle, (void *)data, (DWORD)maxSize, &retVal, NULL)) { lastErr = E_READ_FAILED; - retVal = (DWORD)-1; + retVal = (DWORD)-1; } return (qint64)retVal; } /*! -Writes a block of data to the serial port. This function will write len bytes -from the buffer pointed to by data to the serial port. Return value is the number -of bytes actually written, or -1 on error. + Writes a block of data to the serial port. This function will write len bytes + from the buffer pointed to by data to the serial port. Return value is the number + of bytes actually written, or -1 on error. -\warning before calling this function ensure that serial port associated with this class -is currently open (use isOpen() function to check if port is open). -*/ + \warning before calling this function ensure that serial port associated with this class + is currently open (use isOpen() function to check if port is open). + */ qint64 QextSerialPort::writeData(const char *data, qint64 maxSize) { - QMutexLocker lock( mutex ); + QMutexLocker lock(mutex); DWORD retVal = 0; + if (queryMode() == QextSerialPort::EventDriven) { - OVERLAPPED* newOverlapWrite = new OVERLAPPED; + OVERLAPPED *newOverlapWrite = new OVERLAPPED; ZeroMemory(newOverlapWrite, sizeof(OVERLAPPED)); newOverlapWrite->hEvent = CreateEvent(NULL, true, false, NULL); - if (WriteFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, newOverlapWrite)) { + if (WriteFile(Win_Handle, (void *)data, (DWORD)maxSize, &retVal, newOverlapWrite)) { CloseHandle(newOverlapWrite->hEvent); delete newOverlapWrite; - } - else if (GetLastError() == ERROR_IO_PENDING) { + } else if (GetLastError() == ERROR_IO_PENDING) { // writing asynchronously...not an error QWriteLocker writelocker(bytesToWriteLock); _bytesToWrite += maxSize; pendingWrites.append(newOverlapWrite); - } - else { + } else { qDebug() << "serialport write error:" << GetLastError(); lastErr = E_WRITE_FAILED; - retVal = (DWORD)-1; - if(!CancelIo(newOverlapWrite->hEvent)) + retVal = (DWORD)-1; + if (!CancelIo(newOverlapWrite->hEvent)) { qDebug() << "serialport: couldn't cancel IO"; - if(!CloseHandle(newOverlapWrite->hEvent)) + } + if (!CloseHandle(newOverlapWrite->hEvent)) { qDebug() << "serialport: couldn't close OVERLAPPED handle"; + } delete newOverlapWrite; } - } else if (!WriteFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, NULL)) { + } else if (!WriteFile(Win_Handle, (void *)data, (DWORD)maxSize, &retVal, NULL)) { lastErr = E_WRITE_FAILED; - retVal = (DWORD)-1; + retVal = (DWORD)-1; } return (qint64)retVal; } /*! -This function is included to implement the full QIODevice interface, and currently has no -purpose within this class. This function is meaningless on an unbuffered device and currently -only prints a warning message to that effect. -*/ -void QextSerialPort::ungetChar(char c) { - + This function is included to implement the full QIODevice interface, and currently has no + purpose within this class. This function is meaningless on an unbuffered device and currently + only prints a warning message to that effect. + */ +void QextSerialPort::ungetChar(char c) +{ /*meaningless on unbuffered sequential device - return error and print a warning*/ TTY_WARNING("QextSerialPort: ungetChar() called on an unbuffered sequential device - operation is meaningless"); } /*! -Sets the flow control used by the port. Possible values of flow are: -\verbatim + Sets the flow control used by the port. Possible values of flow are: + \verbatim FLOW_OFF No flow control FLOW_HARDWARE Hardware (RTS/CTS) flow control FLOW_XONXOFF Software (XON/XOFF) flow control -\endverbatim -*/ -void QextSerialPort::setFlowControl(FlowType flow) { + \endverbatim + */ +void QextSerialPort::setFlowControl(FlowType flow) +{ QMutexLocker lock(mutex); - if (Settings.FlowControl!=flow) { - Settings.FlowControl=flow; + + if (Settings.FlowControl != flow) { + Settings.FlowControl = flow; } if (isOpen()) { - switch(flow) { + switch (flow) { + /*no flow control*/ + case FLOW_OFF: + Win_CommConfig.dcb.fOutxCtsFlow = FALSE; + Win_CommConfig.dcb.fRtsControl = RTS_CONTROL_DISABLE; + Win_CommConfig.dcb.fInX = FALSE; + Win_CommConfig.dcb.fOutX = FALSE; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; - /*no flow control*/ - case FLOW_OFF: - Win_CommConfig.dcb.fOutxCtsFlow=FALSE; - Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_DISABLE; - Win_CommConfig.dcb.fInX=FALSE; - Win_CommConfig.dcb.fOutX=FALSE; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; + /*software (XON/XOFF) flow control*/ + case FLOW_XONXOFF: + Win_CommConfig.dcb.fOutxCtsFlow = FALSE; + Win_CommConfig.dcb.fRtsControl = RTS_CONTROL_DISABLE; + Win_CommConfig.dcb.fInX = TRUE; + Win_CommConfig.dcb.fOutX = TRUE; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; - /*software (XON/XOFF) flow control*/ - case FLOW_XONXOFF: - Win_CommConfig.dcb.fOutxCtsFlow=FALSE; - Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_DISABLE; - Win_CommConfig.dcb.fInX=TRUE; - Win_CommConfig.dcb.fOutX=TRUE; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; - - case FLOW_HARDWARE: - Win_CommConfig.dcb.fOutxCtsFlow=TRUE; - Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_HANDSHAKE; - Win_CommConfig.dcb.fInX=FALSE; - Win_CommConfig.dcb.fOutX=FALSE; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; + case FLOW_HARDWARE: + Win_CommConfig.dcb.fOutxCtsFlow = TRUE; + Win_CommConfig.dcb.fRtsControl = RTS_CONTROL_HANDSHAKE; + Win_CommConfig.dcb.fInX = FALSE; + Win_CommConfig.dcb.fOutX = FALSE; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; } } } /*! -Sets the parity associated with the serial port. The possible values of parity are: -\verbatim + Sets the parity associated with the serial port. The possible values of parity are: + \verbatim PAR_SPACE Space Parity PAR_MARK Mark Parity PAR_NONE No Parity PAR_EVEN Even Parity PAR_ODD Odd Parity -\endverbatim -*/ -void QextSerialPort::setParity(ParityType parity) { + \endverbatim + */ +void QextSerialPort::setParity(ParityType parity) +{ QMutexLocker lock(mutex); - if (Settings.Parity!=parity) { - Settings.Parity=parity; + + if (Settings.Parity != parity) { + Settings.Parity = parity; } if (isOpen()) { - Win_CommConfig.dcb.Parity=(unsigned char)parity; + Win_CommConfig.dcb.Parity = (unsigned char)parity; switch (parity) { + /*space parity*/ + case PAR_SPACE: + if (Settings.DataBits == DATA_8) { + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Space parity with 8 data bits is not supported by POSIX systems."); + } + Win_CommConfig.dcb.fParity = TRUE; + break; - /*space parity*/ - case PAR_SPACE: - if (Settings.DataBits==DATA_8) { - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Space parity with 8 data bits is not supported by POSIX systems."); - } - Win_CommConfig.dcb.fParity=TRUE; - break; + /*mark parity - WINDOWS ONLY*/ + case PAR_MARK: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Mark parity is not supported by POSIX systems"); + Win_CommConfig.dcb.fParity = TRUE; + break; - /*mark parity - WINDOWS ONLY*/ - case PAR_MARK: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: Mark parity is not supported by POSIX systems"); - Win_CommConfig.dcb.fParity=TRUE; - break; + /*no parity*/ + case PAR_NONE: + Win_CommConfig.dcb.fParity = FALSE; + break; - /*no parity*/ - case PAR_NONE: - Win_CommConfig.dcb.fParity=FALSE; - break; + /*even parity*/ + case PAR_EVEN: + Win_CommConfig.dcb.fParity = TRUE; + break; - /*even parity*/ - case PAR_EVEN: - Win_CommConfig.dcb.fParity=TRUE; - break; - - /*odd parity*/ - case PAR_ODD: - Win_CommConfig.dcb.fParity=TRUE; - break; + /*odd parity*/ + case PAR_ODD: + Win_CommConfig.dcb.fParity = TRUE; + break; } SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); } } /*! -Sets the number of data bits used by the serial port. Possible values of dataBits are: -\verbatim + Sets the number of data bits used by the serial port. Possible values of dataBits are: + \verbatim DATA_5 5 data bits DATA_6 6 data bits DATA_7 7 data bits DATA_8 8 data bits -\endverbatim + \endverbatim -\note -This function is subject to the following restrictions: -\par + \note + This function is subject to the following restrictions: + \par 5 data bits cannot be used with 2 stop bits. -\par + \par 1.5 stop bits can only be used with 5 data bits. -\par + \par 8 data bits cannot be used with space parity on POSIX systems. -*/ -void QextSerialPort::setDataBits(DataBitsType dataBits) { + */ +void QextSerialPort::setDataBits(DataBitsType dataBits) +{ QMutexLocker lock(mutex); - if (Settings.DataBits!=dataBits) { - if ((Settings.StopBits==STOP_2 && dataBits==DATA_5) || - (Settings.StopBits==STOP_1_5 && dataBits!=DATA_5)) { - } - else { - Settings.DataBits=dataBits; + + if (Settings.DataBits != dataBits) { + if ((Settings.StopBits == STOP_2 && dataBits == DATA_5) || + (Settings.StopBits == STOP_1_5 && dataBits != DATA_5)) {} else { + Settings.DataBits = dataBits; } } if (isOpen()) { - switch(dataBits) { + switch (dataBits) { + /*5 data bits*/ + case DATA_5: + if (Settings.StopBits == STOP_2) { + TTY_WARNING("QextSerialPort: 5 Data bits cannot be used with 2 stop bits."); + } else { + Win_CommConfig.dcb.ByteSize = 5; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; - /*5 data bits*/ - case DATA_5: - if (Settings.StopBits==STOP_2) { - TTY_WARNING("QextSerialPort: 5 Data bits cannot be used with 2 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=5; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; + /*6 data bits*/ + case DATA_6: + if (Settings.StopBits == STOP_1_5) { + TTY_WARNING("QextSerialPort: 6 Data bits cannot be used with 1.5 stop bits."); + } else { + Win_CommConfig.dcb.ByteSize = 6; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; - /*6 data bits*/ - case DATA_6: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("QextSerialPort: 6 Data bits cannot be used with 1.5 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=6; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; + /*7 data bits*/ + case DATA_7: + if (Settings.StopBits == STOP_1_5) { + TTY_WARNING("QextSerialPort: 7 Data bits cannot be used with 1.5 stop bits."); + } else { + Win_CommConfig.dcb.ByteSize = 7; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; - /*7 data bits*/ - case DATA_7: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("QextSerialPort: 7 Data bits cannot be used with 1.5 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=7; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; - - /*8 data bits*/ - case DATA_8: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("QextSerialPort: 8 Data bits cannot be used with 1.5 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=8; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; + /*8 data bits*/ + case DATA_8: + if (Settings.StopBits == STOP_1_5) { + TTY_WARNING("QextSerialPort: 8 Data bits cannot be used with 1.5 stop bits."); + } else { + Win_CommConfig.dcb.ByteSize = 8; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; } } } /*! -Sets the number of stop bits used by the serial port. Possible values of stopBits are: -\verbatim + Sets the number of stop bits used by the serial port. Possible values of stopBits are: + \verbatim STOP_1 1 stop bit STOP_1_5 1.5 stop bits STOP_2 2 stop bits -\endverbatim + \endverbatim -\note -This function is subject to the following restrictions: -\par + \note + This function is subject to the following restrictions: + \par 2 stop bits cannot be used with 5 data bits. -\par + \par 1.5 stop bits cannot be used with 6 or more data bits. -\par + \par POSIX does not support 1.5 stop bits. -*/ -void QextSerialPort::setStopBits(StopBitsType stopBits) { + */ +void QextSerialPort::setStopBits(StopBitsType stopBits) +{ QMutexLocker lock(mutex); - if (Settings.StopBits!=stopBits) { - if ((Settings.DataBits==DATA_5 && stopBits==STOP_2) || - (stopBits==STOP_1_5 && Settings.DataBits!=DATA_5)) { - } - else { - Settings.StopBits=stopBits; + + if (Settings.StopBits != stopBits) { + if ((Settings.DataBits == DATA_5 && stopBits == STOP_2) || + (stopBits == STOP_1_5 && Settings.DataBits != DATA_5)) {} else { + Settings.StopBits = stopBits; } } if (isOpen()) { switch (stopBits) { + /*one stop bit*/ + case STOP_1: + Win_CommConfig.dcb.StopBits = ONESTOPBIT; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; - /*one stop bit*/ - case STOP_1: - Win_CommConfig.dcb.StopBits=ONESTOPBIT; + /*1.5 stop bits*/ + case STOP_1_5: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: 1.5 stop bit operation is not supported by POSIX."); + if (Settings.DataBits != DATA_5) { + TTY_WARNING("QextSerialPort: 1.5 stop bits can only be used with 5 data bits"); + } else { + Win_CommConfig.dcb.StopBits = ONE5STOPBITS; SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; + } + break; - /*1.5 stop bits*/ - case STOP_1_5: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: 1.5 stop bit operation is not supported by POSIX."); - if (Settings.DataBits!=DATA_5) { - TTY_WARNING("QextSerialPort: 1.5 stop bits can only be used with 5 data bits"); - } - else { - Win_CommConfig.dcb.StopBits=ONE5STOPBITS; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; - - /*two stop bits*/ - case STOP_2: - if (Settings.DataBits==DATA_5) { - TTY_WARNING("QextSerialPort: 2 stop bits cannot be used with 5 data bits"); - } - else { - Win_CommConfig.dcb.StopBits=TWOSTOPBITS; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; + /*two stop bits*/ + case STOP_2: + if (Settings.DataBits == DATA_5) { + TTY_WARNING("QextSerialPort: 2 stop bits cannot be used with 5 data bits"); + } else { + Win_CommConfig.dcb.StopBits = TWOSTOPBITS; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; } } } /*! -Sets the baud rate of the serial port. Note that not all rates are applicable on -all platforms. The following table shows translations of the various baud rate -constants on Windows(including NT/2000) and POSIX platforms. Speeds marked with an * -are speeds that are usable on both Windows and POSIX. -\verbatim + Sets the baud rate of the serial port. Note that not all rates are applicable on + all platforms. The following table shows translations of the various baud rate + constants on Windows(including NT/2000) and POSIX platforms. Speeds marked with an * + are speeds that are usable on both Windows and POSIX. + \verbatim - RATE Windows Speed POSIX Speed - ----------- ------------- ----------- + RATE Windows Speed POSIX Speed + ----------- ------------- ----------- BAUD50 110 50 BAUD75 110 75 - *BAUD110 110 110 + * BAUD110 110 110 BAUD134 110 134.5 BAUD150 110 150 BAUD200 110 200 - *BAUD300 300 300 - *BAUD600 600 600 - *BAUD1200 1200 1200 + * BAUD300 300 300 + * BAUD600 600 600 + * BAUD1200 1200 1200 BAUD1800 1200 1800 - *BAUD2400 2400 2400 - *BAUD4800 4800 4800 - *BAUD9600 9600 9600 + * BAUD2400 2400 2400 + * BAUD4800 4800 4800 + * BAUD9600 9600 9600 BAUD14400 14400 9600 - *BAUD19200 19200 19200 - *BAUD38400 38400 38400 + * BAUD19200 19200 19200 + * BAUD38400 38400 38400 BAUD56000 56000 38400 - *BAUD57600 57600 57600 + * BAUD57600 57600 57600 BAUD76800 57600 76800 - *BAUD115200 115200 115200 + * BAUD115200 115200 115200 BAUD128000 128000 115200 BAUD256000 256000 115200 -\endverbatim -*/ -void QextSerialPort::setBaudRate(BaudRateType baudRate) { + \endverbatim + */ +void QextSerialPort::setBaudRate(BaudRateType baudRate) +{ QMutexLocker lock(mutex); - if (Settings.BaudRate!=baudRate) { + + if (Settings.BaudRate != baudRate) { switch (baudRate) { - case BAUD50: - case BAUD75: - case BAUD134: - case BAUD150: - case BAUD200: - Settings.BaudRate=BAUD110; - break; + case BAUD50: + case BAUD75: + case BAUD134: + case BAUD150: + case BAUD200: + Settings.BaudRate = BAUD110; + break; - case BAUD1800: - Settings.BaudRate=BAUD1200; - break; + case BAUD1800: + Settings.BaudRate = BAUD1200; + break; - case BAUD76800: - Settings.BaudRate=BAUD57600; - break; + case BAUD76800: + Settings.BaudRate = BAUD57600; + break; - default: - Settings.BaudRate=baudRate; - break; + default: + Settings.BaudRate = baudRate; + break; } } if (isOpen()) { switch (baudRate) { + /*50 baud*/ + case BAUD50: + TTY_WARNING("QextSerialPort: Windows does not support 50 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate = CBR_110; + break; - /*50 baud*/ - case BAUD50: - TTY_WARNING("QextSerialPort: Windows does not support 50 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + /*75 baud*/ + case BAUD75: + TTY_WARNING("QextSerialPort: Windows does not support 75 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate = CBR_110; + break; - /*75 baud*/ - case BAUD75: - TTY_WARNING("QextSerialPort: Windows does not support 75 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + /*110 baud*/ + case BAUD110: + Win_CommConfig.dcb.BaudRate = CBR_110; + break; - /*110 baud*/ - case BAUD110: - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + /*134.5 baud*/ + case BAUD134: + TTY_WARNING("QextSerialPort: Windows does not support 134.5 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate = CBR_110; + break; - /*134.5 baud*/ - case BAUD134: - TTY_WARNING("QextSerialPort: Windows does not support 134.5 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + /*150 baud*/ + case BAUD150: + TTY_WARNING("QextSerialPort: Windows does not support 150 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate = CBR_110; + break; - /*150 baud*/ - case BAUD150: - TTY_WARNING("QextSerialPort: Windows does not support 150 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + /*200 baud*/ + case BAUD200: + TTY_WARNING("QextSerialPort: Windows does not support 200 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate = CBR_110; + break; - /*200 baud*/ - case BAUD200: - TTY_WARNING("QextSerialPort: Windows does not support 200 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + /*300 baud*/ + case BAUD300: + Win_CommConfig.dcb.BaudRate = CBR_300; + break; - /*300 baud*/ - case BAUD300: - Win_CommConfig.dcb.BaudRate=CBR_300; - break; + /*600 baud*/ + case BAUD600: + Win_CommConfig.dcb.BaudRate = CBR_600; + break; - /*600 baud*/ - case BAUD600: - Win_CommConfig.dcb.BaudRate=CBR_600; - break; + /*1200 baud*/ + case BAUD1200: + Win_CommConfig.dcb.BaudRate = CBR_1200; + break; - /*1200 baud*/ - case BAUD1200: - Win_CommConfig.dcb.BaudRate=CBR_1200; - break; + /*1800 baud*/ + case BAUD1800: + TTY_WARNING("QextSerialPort: Windows does not support 1800 baud operation. Switching to 1200 baud."); + Win_CommConfig.dcb.BaudRate = CBR_1200; + break; - /*1800 baud*/ - case BAUD1800: - TTY_WARNING("QextSerialPort: Windows does not support 1800 baud operation. Switching to 1200 baud."); - Win_CommConfig.dcb.BaudRate=CBR_1200; - break; + /*2400 baud*/ + case BAUD2400: + Win_CommConfig.dcb.BaudRate = CBR_2400; + break; - /*2400 baud*/ - case BAUD2400: - Win_CommConfig.dcb.BaudRate=CBR_2400; - break; + /*4800 baud*/ + case BAUD4800: + Win_CommConfig.dcb.BaudRate = CBR_4800; + break; - /*4800 baud*/ - case BAUD4800: - Win_CommConfig.dcb.BaudRate=CBR_4800; - break; + /*9600 baud*/ + case BAUD9600: + Win_CommConfig.dcb.BaudRate = CBR_9600; + break; - /*9600 baud*/ - case BAUD9600: - Win_CommConfig.dcb.BaudRate=CBR_9600; - break; + /*14400 baud*/ + case BAUD14400: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 14400 baud operation."); + Win_CommConfig.dcb.BaudRate = CBR_14400; + break; - /*14400 baud*/ - case BAUD14400: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 14400 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_14400; - break; + /*19200 baud*/ + case BAUD19200: + Win_CommConfig.dcb.BaudRate = CBR_19200; + break; - /*19200 baud*/ - case BAUD19200: - Win_CommConfig.dcb.BaudRate=CBR_19200; - break; + /*38400 baud*/ + case BAUD38400: + Win_CommConfig.dcb.BaudRate = CBR_38400; + break; - /*38400 baud*/ - case BAUD38400: - Win_CommConfig.dcb.BaudRate=CBR_38400; - break; + /*56000 baud*/ + case BAUD56000: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 56000 baud operation."); + Win_CommConfig.dcb.BaudRate = CBR_56000; + break; - /*56000 baud*/ - case BAUD56000: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 56000 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_56000; - break; + /*57600 baud*/ + case BAUD57600: + Win_CommConfig.dcb.BaudRate = CBR_57600; + break; - /*57600 baud*/ - case BAUD57600: - Win_CommConfig.dcb.BaudRate=CBR_57600; - break; + /*76800 baud*/ + case BAUD76800: + TTY_WARNING("QextSerialPort: Windows does not support 76800 baud operation. Switching to 57600 baud."); + Win_CommConfig.dcb.BaudRate = CBR_57600; + break; - /*76800 baud*/ - case BAUD76800: - TTY_WARNING("QextSerialPort: Windows does not support 76800 baud operation. Switching to 57600 baud."); - Win_CommConfig.dcb.BaudRate=CBR_57600; - break; + /*115200 baud*/ + case BAUD115200: + Win_CommConfig.dcb.BaudRate = CBR_115200; + break; - /*115200 baud*/ - case BAUD115200: - Win_CommConfig.dcb.BaudRate=CBR_115200; - break; + /*128000 baud*/ + case BAUD128000: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 128000 baud operation."); + Win_CommConfig.dcb.BaudRate = CBR_128000; + break; - /*128000 baud*/ - case BAUD128000: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 128000 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_128000; - break; + /*230400 baud*/ + case BAUD230400: + Win_CommConfig.dcb.BaudRate = CBR_230400; + break; - /*230400 baud*/ - case BAUD230400: - Win_CommConfig.dcb.BaudRate=CBR_230400; - break; + /*256000 baud*/ + case BAUD256000: + TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 256000 baud operation."); + Win_CommConfig.dcb.BaudRate = CBR_256000; + break; - /*256000 baud*/ - case BAUD256000: - TTY_PORTABILITY_WARNING("QextSerialPort Portability Warning: POSIX does not support 256000 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_256000; - break; + /*460800 baud*/ + case BAUD460800: + Win_CommConfig.dcb.BaudRate = CBR_460800; + break; - /*460800 baud*/ - case BAUD460800: - Win_CommConfig.dcb.BaudRate=CBR_460800; - break; - - /*921600 baud*/ - case BAUD921600: - Win_CommConfig.dcb.BaudRate=CBR_921600; - break; - } + /*921600 baud*/ + case BAUD921600: + Win_CommConfig.dcb.BaudRate = CBR_921600; + break; + } SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); } } /*! -Sets DTR line to the requested state (high by default). This function will have no effect if -the port associated with the class is not currently open. -*/ -void QextSerialPort::setDtr(bool set) { + Sets DTR line to the requested state (high by default). This function will have no effect if + the port associated with the class is not currently open. + */ +void QextSerialPort::setDtr(bool set) +{ QMutexLocker lock(mutex); + if (isOpen()) { if (set) { EscapeCommFunction(Win_Handle, SETDTR); - } - else { + } else { EscapeCommFunction(Win_Handle, CLRDTR); } } } /*! -Sets RTS line to the requested state (high by default). This function will have no effect if -the port associated with the class is not currently open. -*/ -void QextSerialPort::setRts(bool set) { + Sets RTS line to the requested state (high by default). This function will have no effect if + the port associated with the class is not currently open. + */ +void QextSerialPort::setRts(bool set) +{ QMutexLocker lock(mutex); + if (isOpen()) { if (set) { EscapeCommFunction(Win_Handle, SETRTS); - } - else { + } else { EscapeCommFunction(Win_Handle, CLRRTS); } } } /*! -Returns the line status as stored by the port function. This function will retrieve the states -of the following lines: DCD, CTS, DSR, and RI. On POSIX systems, the following additional lines -can be monitored: DTR, RTS, Secondary TXD, and Secondary RXD. The value returned is an unsigned -long with specific bits indicating which lines are high. The following constants should be used -to examine the states of individual lines: + Returns the line status as stored by the port function. This function will retrieve the states + of the following lines: DCD, CTS, DSR, and RI. On POSIX systems, the following additional lines + can be monitored: DTR, RTS, Secondary TXD, and Secondary RXD. The value returned is an unsigned + long with specific bits indicating which lines are high. The following constants should be used + to examine the states of individual lines: -\verbatim -Mask Line ------- ---- -LS_CTS CTS -LS_DSR DSR -LS_DCD DCD -LS_RI RI -\endverbatim + \verbatim + Mask Line + ------ ---- + LS_CTS CTS + LS_DSR DSR + LS_DCD DCD + LS_RI RI + \endverbatim -This function will return 0 if the port associated with the class is not currently open. -*/ -ulong QextSerialPort::lineStatus(void) { - unsigned long Status=0, Temp=0; + This function will return 0 if the port associated with the class is not currently open. + */ +ulong QextSerialPort::lineStatus(void) +{ + unsigned long Status = 0, Temp = 0; QMutexLocker lock(mutex); + if (isOpen()) { GetCommModemStatus(Win_Handle, &Temp); - if (Temp&MS_CTS_ON) { - Status|=LS_CTS; + if (Temp & MS_CTS_ON) { + Status |= LS_CTS; } - if (Temp&MS_DSR_ON) { - Status|=LS_DSR; + if (Temp & MS_DSR_ON) { + Status |= LS_DSR; } - if (Temp&MS_RING_ON) { - Status|=LS_RI; + if (Temp & MS_RING_ON) { + Status |= LS_RI; } - if (Temp&MS_RLSD_ON) { - Status|=LS_DCD; + if (Temp & MS_RLSD_ON) { + Status |= LS_DCD; } } return Status; @@ -803,41 +811,45 @@ ulong QextSerialPort::lineStatus(void) { bool QextSerialPort::waitForReadyRead(int msecs) { - //@todo implement + // @todo implement return false; } qint64 QextSerialPort::bytesToWrite() const { QReadLocker rl(bytesToWriteLock); + return _bytesToWrite; } /* - Triggered when there's activity on our HANDLE. -*/ + Triggered when there's activity on our HANDLE. + */ void QextSerialPort::onWinEvent(HANDLE h) { QMutexLocker lock(mutex); - if(h == overlap.hEvent) { + + if (h == overlap.hEvent) { if (eventMask & EV_RXCHAR) { - if (sender() != this && bytesAvailable() > 0) + if (sender() != this && bytesAvailable() > 0) { emit readyRead(); + } } if (eventMask & EV_TXEMPTY) { /* - A write completed. Run through the list of OVERLAPPED writes, and if - they completed successfully, take them off the list and delete them. - Otherwise, leave them on there so they can finish. - */ + A write completed. Run through the list of OVERLAPPED writes, and if + they completed successfully, take them off the list and delete them. + Otherwise, leave them on there so they can finish. + */ qint64 totalBytesWritten = 0; - QList overlapsToDelete; - foreach(OVERLAPPED* o, pendingWrites) { + QList overlapsToDelete; + foreach(OVERLAPPED * o, pendingWrites) { DWORD numBytes = 0; - if (GetOverlappedResult(Win_Handle, o, & numBytes, false)) { + + if (GetOverlappedResult(Win_Handle, o, &numBytes, false)) { overlapsToDelete.append(o); totalBytesWritten += numBytes; - } else if( GetLastError() != ERROR_IO_INCOMPLETE ) { + } else if (GetLastError() != ERROR_IO_INCOMPLETE) { overlapsToDelete.append(o); qWarning() << "CommEvent overlapped write error:" << GetLastError(); } @@ -849,32 +861,36 @@ void QextSerialPort::onWinEvent(HANDLE h) _bytesToWrite = 0; } - foreach(OVERLAPPED* o, overlapsToDelete) { + foreach(OVERLAPPED * o, overlapsToDelete) { OVERLAPPED *toDelete = pendingWrites.takeAt(pendingWrites.indexOf(o)); + CloseHandle(toDelete->hEvent); delete toDelete; } } if (eventMask & EV_DSR) { - if (lineStatus() & LS_DSR) + if (lineStatus() & LS_DSR) { emit dsrChanged(true); - else + } else { emit dsrChanged(false); + } } } WaitCommEvent(Win_Handle, &eventMask, &overlap); } /*! -Sets the read and write timeouts for the port to millisec milliseconds. -Setting 0 indicates that timeouts are not used for read nor write operations; -however read() and write() functions will still block. Set -1 to provide -non-blocking behaviour (read() and write() will return immediately). + Sets the read and write timeouts for the port to millisec milliseconds. + Setting 0 indicates that timeouts are not used for read nor write operations; + however read() and write() functions will still block. Set -1 to provide + non-blocking behaviour (read() and write() will return immediately). -\note this function does nothing in event driven mode. -*/ -void QextSerialPort::setTimeout(long millisec) { + \note this function does nothing in event driven mode. + */ +void QextSerialPort::setTimeout(long millisec) +{ QMutexLocker lock(mutex); + Settings.Timeout_Millisec = millisec; if (millisec == -1) { @@ -884,28 +900,29 @@ void QextSerialPort::setTimeout(long millisec) { Win_CommTimeouts.ReadIntervalTimeout = millisec; Win_CommTimeouts.ReadTotalTimeoutConstant = millisec; } - Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; + Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; Win_CommTimeouts.WriteTotalTimeoutMultiplier = millisec; - Win_CommTimeouts.WriteTotalTimeoutConstant = 0; - if (queryMode() != QextSerialPort::EventDriven) + Win_CommTimeouts.WriteTotalTimeoutConstant = 0; + if (queryMode() != QextSerialPort::EventDriven) { SetCommTimeouts(Win_Handle, &Win_CommTimeouts); + } } /*! -emulates the EV_TXEMPTY system event not present on some BT interfaces -*/ + emulates the EV_TXEMPTY system event not present on some BT interfaces + */ void QextSerialPort::triggerTxEmpty() { - if (bytesToWrite()>500) - { + if (bytesToWrite() > 500) { QMutexLocker lock(mutex); qint64 totalBytesWritten = 0; - QList overlapsToDelete; - foreach(OVERLAPPED* o, pendingWrites) { + QList overlapsToDelete; + foreach(OVERLAPPED * o, pendingWrites) { DWORD numBytes = 0; - if (GetOverlappedResult(Win_Handle, o, & numBytes, false)) { + + if (GetOverlappedResult(Win_Handle, o, &numBytes, false)) { overlapsToDelete.append(o); totalBytesWritten += numBytes; - } else if( GetLastError() != ERROR_IO_INCOMPLETE ) { + } else if (GetLastError() != ERROR_IO_INCOMPLETE) { overlapsToDelete.append(o); qWarning() << "CommEvent overlapped write error:" << GetLastError(); } @@ -914,14 +931,14 @@ void QextSerialPort::triggerTxEmpty() if (totalBytesWritten > 0) { QWriteLocker writelocker(bytesToWriteLock); _bytesToWrite = 0; - //qDebug()<<"zeroed bytesToWrite"; + // qDebug()<<"zeroed bytesToWrite"; } - //qDebug()<<"overlapsToDelete"<hEvent); delete toDelete; } } - } diff --git a/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.cpp b/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.cpp index dda8b3cbb..aba3b968d 100644 --- a/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.cpp +++ b/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.cpp @@ -5,39 +5,42 @@ #include -//#define QSPINBOX_QSBDEBUG +// #define QSPINBOX_QSBDEBUG #ifdef QSPINBOX_QSBDEBUG # define QSBDEBUG qDebug #else -# define QSBDEBUG if (false) qDebug +# define QSBDEBUG \ + if (false) \ + qDebug #endif -QScienceSpinBox::QScienceSpinBox(QWidget * parent) +QScienceSpinBox::QScienceSpinBox(QWidget *parent) : QDoubleSpinBox(parent) { - initLocalValues(parent); - setDecimals(8); - QDoubleSpinBox::setDecimals(1000); - - // set Range to maximum possible values - double doubleMax = std::numeric_limits::max(); - setRange(-doubleMax, doubleMax); + initLocalValues(parent); + setDecimals(8); + QDoubleSpinBox::setDecimals(1000); - v = new QDoubleValidator(this); - v->setDecimals(1000); // (standard anyway) - v->setNotation(QDoubleValidator::ScientificNotation); - this->lineEdit()->setValidator(v); + // set Range to maximum possible values + double doubleMax = std::numeric_limits::max(); + setRange(-doubleMax, doubleMax); + + v = new QDoubleValidator(this); + v->setDecimals(1000); // (standard anyway) + v->setNotation(QDoubleValidator::ScientificNotation); + this->lineEdit()->setValidator(v); } void QScienceSpinBox::initLocalValues(QWidget *parent) { const QString str = (parent ? parent->locale() : QLocale()).toString(4567.1); + if (str.size() == 6) { delimiter = str.at(4); - thousand = QChar((ushort)0); + thousand = QChar((ushort)0); } else if (str.size() == 7) { - thousand = str.at(1); + thousand = str.at(1); delimiter = str.at(5); } Q_ASSERT(!delimiter.isNull()); @@ -45,33 +48,34 @@ void QScienceSpinBox::initLocalValues(QWidget *parent) int QScienceSpinBox::decimals() const { - return dispDecimals; + return dispDecimals; } void QScienceSpinBox::setDecimals(int value) { - dispDecimals = value; + dispDecimals = value; } // overwritten virtual function of QAbstractSpinBox void QScienceSpinBox::stepBy(int steps) { - if (steps < 0) - stepDown(); - else - stepUp(); + if (steps < 0) { + stepDown(); + } else { + stepUp(); + } } void QScienceSpinBox::stepDown() { - QSBDEBUG() << "stepDown()"; - setValue(value()/10.0); + QSBDEBUG() << "stepDown()"; + setValue(value() / 10.0); } void QScienceSpinBox::stepUp() { - QSBDEBUG() << "stepUp()"; - setValue(value()*10.0); + QSBDEBUG() << "stepUp()"; + setValue(value() * 10.0); } /*! @@ -79,11 +83,11 @@ void QScienceSpinBox::stepUp() */ QString QScienceSpinBox::textFromValue(double value) const { + // convert to string -> Using exponetial display with internal decimals + QString str = locale().toString(value, 'e', dispDecimals); - // convert to string -> Using exponetial display with internal decimals - QString str = locale().toString(value, 'e', dispDecimals); // remove thousand sign - if (qAbs(value) >= 1000.0) { + if (qAbs(value) >= 1000.0) { str.remove(thousand); } return str; @@ -94,13 +98,15 @@ double QScienceSpinBox::valueFromText(const QString &text) const QString copy = text; int pos = this->lineEdit()->cursorPosition(); QValidator::State state = QValidator::Acceptable; + return validateAndInterpret(copy, pos, state).toDouble(); } // this function is never used...? double QScienceSpinBox::round(double value) const { - const QString strDbl = locale().toString(value, 'g', dispDecimals); + const QString strDbl = locale().toString(value, 'g', dispDecimals); + return locale().toDouble(strDbl); } @@ -108,6 +114,7 @@ double QScienceSpinBox::round(double value) const QValidator::State QScienceSpinBox::validate(QString &text, int &pos) const { QValidator::State state; + validateAndInterpret(text, pos, state); return state; } @@ -122,57 +129,58 @@ void QScienceSpinBox::fixup(QString &input) const bool QScienceSpinBox::isIntermediateValue(const QString &str) const { QSBDEBUG() << "input is" << str << minimum() << maximum(); - qint64 dec = 1; - - for (int i=0; i < decimals(); ++i) + qint64 dec = 1; + + for (int i = 0; i < decimals(); ++i) { dec *= 10; + } const QLatin1Char dot('.'); /*! - * determine minimum possible values on left and right of Decimal-char - */ - // I know QString::number() uses CLocale so I use dot - const QString minstr = QString::number(minimum(), 'f', QDoubleSpinBox::decimals()); + * determine minimum possible values on left and right of Decimal-char + */ + // I know QString::number() uses CLocale so I use dot + const QString minstr = QString::number(minimum(), 'f', QDoubleSpinBox::decimals()); qint64 min_left = minstr.left(minstr.indexOf(dot)).toLongLong(); - qint64 min_right = minstr.mid(minstr.indexOf(dot) + 1).toLongLong(); + qint64 min_right = minstr.mid(minstr.indexOf(dot) + 1).toLongLong(); - const QString maxstr = QString::number(maximum(), 'f', QDoubleSpinBox::decimals()); + const QString maxstr = QString::number(maximum(), 'f', QDoubleSpinBox::decimals()); qint64 max_left = maxstr.left(maxstr.indexOf(dot)).toLongLong(); - qint64 max_right = maxstr.mid(maxstr.indexOf(dot) + 1).toLongLong(); - - /*! - * determine left and right long values (left and right of delimiter) - */ - const int dotindex = str.indexOf(delimiter); - const bool negative = maximum() < 0; - qint64 left = 0, right = 0; - bool doleft = true; + qint64 max_right = maxstr.mid(maxstr.indexOf(dot) + 1).toLongLong(); + + /*! + * determine left and right long values (left and right of delimiter) + */ + const int dotindex = str.indexOf(delimiter); + const bool negative = maximum() < 0; + qint64 left = 0, right = 0; + bool doleft = true; bool doright = true; - // no separator -> everthing in left + // no separator -> everthing in left if (dotindex == -1) { - left = str.toLongLong(); + left = str.toLongLong(); doright = false; } - // separator on left or contains '+' - else if (dotindex == 0 || (dotindex == 1 && str.at(0) == QLatin1Char('+'))) { + // separator on left or contains '+' + else if (dotindex == 0 || (dotindex == 1 && str.at(0) == QLatin1Char('+'))) { // '+' at negative max - if (negative) { + if (negative) { QSBDEBUG() << __FILE__ << __LINE__ << "returns false"; return false; } doleft = false; - right = str.mid(dotindex + 1).toLongLong(); + right = str.mid(dotindex + 1).toLongLong(); } - // contains '-' - else if (dotindex == 1 && str.at(0) == QLatin1Char('-')) { - // '-' at positiv max + // contains '-' + else if (dotindex == 1 && str.at(0) == QLatin1Char('-')) { + // '-' at positiv max if (!negative) { QSBDEBUG() << __FILE__ << __LINE__ << "returns false"; return false; } doleft = false; - right = str.mid(dotindex + 1).toLongLong(); + right = str.mid(dotindex + 1).toLongLong(); } else { left = str.left(dotindex).toLongLong(); if (dotindex == str.size() - 1) { // nothing right of Separator @@ -181,11 +189,10 @@ bool QScienceSpinBox::isIntermediateValue(const QString &str) const right = str.mid(dotindex + 1).toLongLong(); } } - // left > 0, with max < 0 and no '-' - if ((left >= 0 && max_left < 0 && !str.startsWith(QLatin1Char('-'))) - // left > 0, with min > 0 - || (left < 0 && min_left >= 0)) - { + // left > 0, with max < 0 and no '-' + if ((left >= 0 && max_left < 0 && !str.startsWith(QLatin1Char('-'))) + // left > 0, with min > 0 + || (left < 0 && min_left >= 0)) { QSBDEBUG("returns false"); return false; } @@ -221,8 +228,8 @@ bool QScienceSpinBox::isIntermediateValue(const QString &str) const if (match != max_left) { max_right = negative ? 0 : dec; } - qint64 tmpl = negative ? max_right : min_right; - qint64 tmpr = negative ? min_right : max_right; + qint64 tmpl = negative ? max_right : min_right; + qint64 tmpr = negative ? min_right : max_right; const bool ret = isIntermediateValueHelper(right, tmpl, tmpr); QSBDEBUG() << __FILE__ << __LINE__ << "returns" << ret; return ret; @@ -236,19 +243,19 @@ bool QScienceSpinBox::isIntermediateValue(const QString &str) const \internal Multi purpose function that parses input, sets state to the appropriate state and returns the value it will be interpreted as. -*/ + */ // reimplemented function, copied from QDoubleSpinBoxPrivate::validateAndInterpret QVariant QScienceSpinBox::validateAndInterpret( - QString &input, - int &pos, + QString &input, + int &pos, QValidator::State &state) const { - /*! return 'cachedText' if - * input = cachedText, or input Empty + /*! return 'cachedText' if + * input = cachedText, or input Empty */ - static QString cachedText; - static QValidator::State cachedState; + static QString cachedText; + static QValidator::State cachedState; static QVariant cachedValue; if (cachedText == input && !input.isEmpty()) { @@ -260,133 +267,129 @@ QVariant QScienceSpinBox::validateAndInterpret( const double max = maximum(); const double min = minimum(); - // removes prefix & suffix - QString copy = stripped(input, &pos); + // removes prefix & suffix + QString copy = stripped(input, &pos); QSBDEBUG() << "input" << input << "copy" << copy; - int len = copy.size(); + int len = copy.size(); double num = min; - const bool plus = max >= 0; + const bool plus = max >= 0; const bool minus = min <= 0; - // Test possible 'Intermediate' reasons - switch (len) - { - case 0: - // Length 0 is always 'Intermediate', except for min=max - if (max != min) { - state = QValidator::Intermediate; - } else { - state = QValidator::Invalid; - } - goto end; - case 1: - // if only char is '+' or '-' - if (copy.at(0) == delimiter - || (plus && copy.at(0) == QLatin1Char('+')) - || (minus && copy.at(0) == QLatin1Char('-'))) { - state = QValidator::Intermediate; - goto end; - } - break; - case 2: - // if only chars are '+' or '-' followed by Comma seperator (delimiter) - if (copy.at(1) == delimiter - && ((plus && copy.at(0) == QLatin1Char('+')) || (minus && copy.at(0) == QLatin1Char('-')))) { - state = QValidator::Intermediate; - goto end; - } - break; - default: break; + // Test possible 'Intermediate' reasons + switch (len) { + case 0: + // Length 0 is always 'Intermediate', except for min=max + if (max != min) { + state = QValidator::Intermediate; + } else { + state = QValidator::Invalid; + } + goto end; + case 1: + // if only char is '+' or '-' + if (copy.at(0) == delimiter + || (plus && copy.at(0) == QLatin1Char('+')) + || (minus && copy.at(0) == QLatin1Char('-'))) { + state = QValidator::Intermediate; + goto end; + } + break; + case 2: + // if only chars are '+' or '-' followed by Comma seperator (delimiter) + if (copy.at(1) == delimiter + && ((plus && copy.at(0) == QLatin1Char('+')) || (minus && copy.at(0) == QLatin1Char('-')))) { + state = QValidator::Intermediate; + goto end; + } + break; + default: break; } // end switch // First char must not be thousand-char - if (copy.at(0) == thousand) - { - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to Invalid"; + if (copy.at(0) == thousand) { + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to Invalid"; state = QValidator::Invalid; goto end; } - // Test possible 'Invalid' reasons - else if (len > 1) - { + // Test possible 'Invalid' reasons + else if (len > 1) { const int dec = copy.indexOf(delimiter); // position of delimiter // if decimal separator (delimiter) exists - if (dec != -1) { - // not two delimiters after one other (meaning something like ',,') + if (dec != -1) { + // not two delimiters after one other (meaning something like ',,') if (dec + 1 < copy.size() && copy.at(dec + 1) == delimiter && pos == dec + 1) { copy.remove(dec + 1, 1); // typing a delimiter when you are on the delimiter - } // should be treated as typing right arrow - // too many decimal points - if (copy.size() - dec > QDoubleSpinBox::decimals() + 1) { - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to Invalid"; + } // should be treated as typing right arrow + // too many decimal points + if (copy.size() - dec > QDoubleSpinBox::decimals() + 1) { + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to Invalid"; state = QValidator::Invalid; goto end; } - // after decimal separator no thousand char - for (int i=dec + 1; i 1) + } // end if (len > 1) - // block of remaining test before 'end' mark - { + // block of remaining test before 'end' mark + { bool ok = false; - bool notAcceptable = false; + bool notAcceptable = false; - // convert 'copy' to double, and check if that was 'ok' + // convert 'copy' to double, and check if that was 'ok' QLocale loc(locale()); num = loc.toDouble(copy, &ok); QSBDEBUG() << __FILE__ << __LINE__ << loc << copy << num << ok; - - // conversion to double did fail + + // conversion to double did fail if (!ok) { - // maybe thousand char was responsable - if (thousand.isPrint()) - { - // if no thousand sign is possible, then - // something else is responable -> Invalid + // maybe thousand char was responsable + if (thousand.isPrint()) { + // if no thousand sign is possible, then + // something else is responable -> Invalid if (max < 1000 && min > -1000 && copy.contains(thousand)) { state = QValidator::Invalid; - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to Invalid"; + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to Invalid"; goto end; } - // two thousand-chars after one other are not valid + // two thousand-chars after one other are not valid const int len = copy.size(); - for (int i=0; i Invalid + // if conversion still not valid, then reason unknown -> Invalid if (!ok) { state = QValidator::Invalid; - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to Invalid"; + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to Invalid"; goto end; } notAcceptable = true; // -> state = Intermediate } // endif: (thousand.isPrint()) } - // no thousand sign, but still invalid for unknown reason + // no thousand sign, but still invalid for unknown reason if (!ok) { state = QValidator::Invalid; - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to Invalid"; - } - // number valid and within valid range - else if (num >= min && num <= max) { - if (notAcceptable) { - state = QValidator::Intermediate; // conversion to num initially failed - } else { - state = QValidator::Acceptable; - } - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to " + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to Invalid"; + } + // number valid and within valid range + else if (num >= min && num <= max) { + if (notAcceptable) { + state = QValidator::Intermediate; // conversion to num initially failed + } else { + state = QValidator::Acceptable; + } + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to " << (state == QValidator::Intermediate ? "Intermediate" : "Acceptable"); - } - // when max and min is the same the only non-Invalid input is max (or min) - else if (max == min) { + } + // when max and min is the same the only non-Invalid input is max (or min) + else if (max == min) { state = QValidator::Invalid; - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to Invalid"; + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to Invalid"; } else { - // value out of valid range (coves only special cases) + // value out of valid range (coves only special cases) if ((num >= 0 && num > max) || (num < 0 && num < min)) { state = QValidator::Invalid; - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to Invalid"; + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to Invalid"; } else { - // invalid range, further test with 'isIntermediateValue' - if (isIntermediateValue(copy)) { - state = QValidator::Intermediate; - } else { - state = QValidator::Invalid; - } - QSBDEBUG() << __FILE__ << __LINE__<< "state is set to " + // invalid range, further test with 'isIntermediateValue' + if (isIntermediateValue(copy)) { + state = QValidator::Intermediate; + } else { + state = QValidator::Invalid; + } + QSBDEBUG() << __FILE__ << __LINE__ << "state is set to " << (state == QValidator::Intermediate ? "Intermediate" : "Acceptable"); } } @@ -443,15 +446,15 @@ QVariant QScienceSpinBox::validateAndInterpret( end: // if something went wrong, set num to something valid - if (state != QValidator::Acceptable) { + if (state != QValidator::Acceptable) { num = max > 0 ? min : max; } - // save (private) cache values - cachedText = prefix() + copy + suffix(); + // save (private) cache values + cachedText = prefix() + copy + suffix(); cachedState = state; cachedValue = QVariant(num); - // return resulting valid num + // return resulting valid num return QVariant(num); } @@ -459,35 +462,37 @@ end: /*! \internal Strips any prefix/suffix from \a text. -*/ + */ // reimplemented function, copied from QAbstractSpinBoxPrivate::stripped QString QScienceSpinBox::stripped(const QString &t, int *pos) const { QString text = t; - QString prefixtext = prefix(); - QString suffixtext = suffix(); - + QString prefixtext = prefix(); + QString suffixtext = suffix(); + if (specialValueText().size() == 0 || text != specialValueText()) { - int from = 0; - int size = text.size(); + int from = 0; + int size = text.size(); bool changed = false; if (prefixtext.size() && text.startsWith(prefixtext)) { - from += prefixtext.size(); - size -= from; + from += prefixtext.size(); + size -= from; changed = true; } if (suffixtext.size() && text.endsWith(suffixtext)) { - size -= suffixtext.size(); + size -= suffixtext.size(); changed = true; } - if (changed) + if (changed) { text = text.mid(from, size); + } } const int s = text.size(); text = text.trimmed(); - if (pos) + if (pos) { (*pos) -= (s - text.size()); + } return text; } @@ -497,12 +502,13 @@ bool QScienceSpinBox::isIntermediateValueHelper(qint64 num, qint64 min, qint64 m QSBDEBUG("%lld %lld %lld", num, min, max); if (num >= min && num <= max) { - if (match) + if (match) { *match = num; + } QSBDEBUG("returns true 0"); return true; } - qint64 tmp = num; + qint64 tmp = num; int numDigits = 0; int digits[10]; @@ -511,7 +517,7 @@ bool QScienceSpinBox::isIntermediateValueHelper(qint64 num, qint64 min, qint64 m digits[0] = 0; } else { tmp = qAbs(num); - for (int i=0; tmp > 0; ++i) { + for (int i = 0; tmp > 0; ++i) { digits[numDigits++] = tmp % 10; tmp /= 10; } @@ -519,22 +525,24 @@ bool QScienceSpinBox::isIntermediateValueHelper(qint64 num, qint64 min, qint64 m int failures = 0; qint64 number; - for (number=max; number>=min; --number) { + for (number = max; number >= min; --number) { tmp = qAbs(number); - for (int i=0; tmp > 0;) { + for (int i = 0; tmp > 0;) { if (digits[i] == (tmp % 10)) { if (++i == numDigits) { - if (match) + if (match) { *match = number; + } QSBDEBUG("returns true 1"); return true; } } tmp /= 10; } - if (failures++ == 500000) { //upper bound - if (match) + if (failures++ == 500000) { // upper bound + if (match) { *match = num; + } QSBDEBUG("returns true 2"); return true; } diff --git a/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.h b/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.h index faecd0df1..99b849e66 100644 --- a/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.h +++ b/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.h @@ -12,39 +12,37 @@ #include -class QScienceSpinBox : public QDoubleSpinBox -{ -Q_OBJECT +class QScienceSpinBox : public QDoubleSpinBox { + Q_OBJECT public: - QScienceSpinBox(QWidget * parent = 0); + QScienceSpinBox(QWidget *parent = 0); - int decimals() const; - void setDecimals(int value); + int decimals() const; + void setDecimals(int value); - QString textFromValue ( double value ) const; - double valueFromText ( const QString & text ) const; + QString textFromValue(double value) const; + double valueFromText(const QString & text) const; static bool isIntermediateValueHelper(qint64 num, qint64 minimum, qint64 maximum, qint64 *match = 0); private: - int dispDecimals; + int dispDecimals; QChar delimiter, thousand; - QDoubleValidator * v; + QDoubleValidator *v; private: - void initLocalValues(QWidget *parent); + void initLocalValues(QWidget *parent); bool isIntermediateValue(const QString &str) const; QVariant validateAndInterpret(QString &input, int &pos, QValidator::State &state) const; - QValidator::State validate(QString &text, int &pos) const; - void fixup(QString &input) const; - QString stripped(const QString &t, int *pos) const; - double round(double value) const; - void stepBy(int steps); + QValidator::State validate(QString &text, int &pos) const; + void fixup(QString &input) const; + QString stripped(const QString &t, int *pos) const; + double round(double value) const; + void stepBy(int steps); public slots: - void stepDown(); - void stepUp(); - + void stepDown(); + void stepUp(); }; -#endif +#endif // ifndef __QScienceSpinBox_H__ diff --git a/ground/openpilotgcs/src/libs/qtconcurrent/multitask.h b/ground/openpilotgcs/src/libs/qtconcurrent/multitask.h index 18a748c0e..8e59ada83 100644 --- a/ground/openpilotgcs/src/libs/qtconcurrent/multitask.h +++ b/ground/openpilotgcs/src/libs/qtconcurrent/multitask.h @@ -4,25 +4,25 @@ * @file multitask.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,27 +44,24 @@ QT_BEGIN_NAMESPACE namespace QtConcurrent { - -class QTCONCURRENT_EXPORT MultiTaskBase : public QObject, public QRunnable -{ +class QTCONCURRENT_EXPORT MultiTaskBase : public QObject, public QRunnable { Q_OBJECT protected slots: - virtual void cancelSelf() = 0; + virtual void cancelSelf() = 0; virtual void setFinished() = 0; virtual void setProgressRange(int min, int max) = 0; virtual void setProgressValue(int value) = 0; - virtual void setProgressText(QString value) = 0; + virtual void setProgressText(QString value) = 0; }; template -class MultiTask : public MultiTaskBase -{ +class MultiTask : public MultiTaskBase { public: - MultiTask(void (Class::*fn)(QFutureInterface &), const QList &objects) + MultiTask(void(Class::*fn)(QFutureInterface &), const QList &objects) : fn(fn), objects(objects) { - maxProgress = 100*objects.size(); + maxProgress = 100 * objects.size(); } QFuture future() @@ -76,13 +73,14 @@ public: void run() { QThreadPool::globalInstance()->releaseThread(); + futureInterface.setProgressRange(0, maxProgress); - foreach (Class *object, objects) { + foreach(Class * object, objects) { QFutureWatcher *watcher = new QFutureWatcher(); watchers.insert(object, watcher); finished.insert(watcher, false); connect(watcher, SIGNAL(finished()), this, SLOT(setFinished())); - connect(watcher, SIGNAL(progressRangeChanged(int,int)), this, SLOT(setProgressRange(int,int))); + connect(watcher, SIGNAL(progressRangeChanged(int, int)), this, SLOT(setProgressRange(int, int))); connect(watcher, SIGNAL(progressValueChanged(int)), this, SLOT(setProgressValue(int))); connect(watcher, SIGNAL(progressTextChanged(QString)), this, SLOT(setProgressText(QString))); watcher->setFuture(QtConcurrent::run(fn, object)); @@ -101,26 +99,28 @@ public: protected: void cancelSelf() { - foreach (QFutureWatcher *watcher, watchers) - watcher->future().cancel(); + foreach(QFutureWatcher *watcher, watchers) + watcher->future().cancel(); } void setFinished() { updateProgress(); QFutureWatcher *watcher = static_cast *>(sender()); - if (finished.contains(watcher)) + if (finished.contains(watcher)) { finished[watcher] = true; + } bool allFinished = true; const QList finishedValues = finished.values(); - foreach (bool isFinished, finishedValues) { + foreach(bool isFinished, finishedValues) { if (!isFinished) { allFinished = false; break; } } - if (allFinished) + if (allFinished) { loop->quit(); + } } void setProgressRange(int min, int max) @@ -146,12 +146,14 @@ private: { int progressSum = 0; const QList *> watchersValues = watchers.values(); - foreach (QFutureWatcher *watcher, watchersValues) { + + foreach(QFutureWatcher *watcher, watchersValues) { if (watcher->progressMinimum() == watcher->progressMaximum()) { - if (watcher->future().isFinished() && !watcher->future().isCanceled()) + if (watcher->future().isFinished() && !watcher->future().isCanceled()) { progressSum += 100; + } } else { - progressSum += 100*(watcher->progressValue()-watcher->progressMinimum())/(watcher->progressMaximum()-watcher->progressMinimum()); + progressSum += 100 * (watcher->progressValue() - watcher->progressMinimum()) / (watcher->progressMaximum() - watcher->progressMinimum()); } } futureInterface.setProgressValue(progressSum); @@ -161,9 +163,11 @@ private: { QString text; const QList *> watchersValues = watchers.values(); - foreach (QFutureWatcher *watcher, watchersValues) { - if (!watcher->progressText().isEmpty()) + + foreach(QFutureWatcher *watcher, watchersValues) { + if (!watcher->progressText().isEmpty()) { text += watcher->progressText() + "\n"; + } } text = text.trimmed(); futureInterface.setProgressValueAndText(futureInterface.progressValue(), text); @@ -188,7 +192,6 @@ QFuture run(void (Class::*fn)(QFutureInterface &), const QList &o QThreadPool::globalInstance()->start(task, priority); return future; } - } // namespace QtConcurrent QT_END_NAMESPACE diff --git a/ground/openpilotgcs/src/libs/qtconcurrent/qtconcurrent_global.h b/ground/openpilotgcs/src/libs/qtconcurrent/qtconcurrent_global.h index ba8c071f3..f1c048b57 100644 --- a/ground/openpilotgcs/src/libs/qtconcurrent/qtconcurrent_global.h +++ b/ground/openpilotgcs/src/libs/qtconcurrent/qtconcurrent_global.h @@ -4,25 +4,25 @@ * @file qtconcurrent_global.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/libs/qtconcurrent/runextensions.h b/ground/openpilotgcs/src/libs/qtconcurrent/runextensions.h index 96f5c7dab..01c8cd533 100644 --- a/ground/openpilotgcs/src/libs/qtconcurrent/runextensions.h +++ b/ground/openpilotgcs/src/libs/qtconcurrent/runextensions.h @@ -4,25 +4,25 @@ * @file runextensions.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,13 +36,11 @@ QT_BEGIN_NAMESPACE namespace QtConcurrent { - -template -class StoredInterfaceFunctionCall0 : public QRunnable -{ +template +class StoredInterfaceFunctionCall0 : public QRunnable { public: - StoredInterfaceFunctionCall0(void (fn)(QFutureInterface &)) - : fn(fn) { } + StoredInterfaceFunctionCall0(void(fn)(QFutureInterface &)) + : fn(fn) {} QFuture start() { @@ -60,14 +58,12 @@ public: private: QFutureInterface futureInterface; FunctionPointer fn; - }; -template -class StoredInterfaceMemberFunctionCall0 : public QRunnable -{ +template +class StoredInterfaceMemberFunctionCall0 : public QRunnable { public: - StoredInterfaceMemberFunctionCall0(void (Class::*fn)(QFutureInterface &), Class *object) - : fn(fn), object(object) { } + StoredInterfaceMemberFunctionCall0(void(Class::*fn)(QFutureInterface &), Class *object) + : fn(fn), object(object) {} QFuture start() { @@ -86,15 +82,13 @@ private: QFutureInterface futureInterface; FunctionPointer fn; Class *object; - }; -template -class StoredInterfaceFunctionCall1 : public QRunnable -{ +template +class StoredInterfaceFunctionCall1 : public QRunnable { public: - StoredInterfaceFunctionCall1(void (fn)(QFutureInterface &, Arg1), Arg1 arg1) - : fn(fn), arg1(arg1) { } + StoredInterfaceFunctionCall1(void(fn)(QFutureInterface &, Arg1), Arg1 arg1) + : fn(fn), arg1(arg1) {} QFuture start() { @@ -114,12 +108,11 @@ private: FunctionPointer fn; Arg1 arg1; }; -template -class StoredInterfaceMemberFunctionCall1 : public QRunnable -{ +template +class StoredInterfaceMemberFunctionCall1 : public QRunnable { public: - StoredInterfaceMemberFunctionCall1(void (Class::*fn)(QFutureInterface &, Arg1), Class *object, Arg1 arg1) - : fn(fn), object(object), arg1(arg1) { } + StoredInterfaceMemberFunctionCall1(void(Class::*fn)(QFutureInterface &, Arg1), Class *object, Arg1 arg1) + : fn(fn), object(object), arg1(arg1) {} QFuture start() { @@ -141,12 +134,11 @@ private: Arg1 arg1; }; -template -class StoredInterfaceFunctionCall2 : public QRunnable -{ +template +class StoredInterfaceFunctionCall2 : public QRunnable { public: - StoredInterfaceFunctionCall2(void (fn)(QFutureInterface &, Arg1, Arg2), Arg1 arg1, Arg2 arg2) - : fn(fn), arg1(arg1), arg2(arg2) { } + StoredInterfaceFunctionCall2(void(fn)(QFutureInterface &, Arg1, Arg2), Arg1 arg1, Arg2 arg2) + : fn(fn), arg1(arg1), arg2(arg2) {} QFuture start() { @@ -166,12 +158,11 @@ private: FunctionPointer fn; Arg1 arg1; Arg2 arg2; }; -template -class StoredInterfaceMemberFunctionCall2 : public QRunnable -{ +template +class StoredInterfaceMemberFunctionCall2 : public QRunnable { public: - StoredInterfaceMemberFunctionCall2(void (Class::*fn)(QFutureInterface &, Arg1, Arg2), Class *object, Arg1 arg1, Arg2 arg2) - : fn(fn), object(object), arg1(arg1), arg2(arg2) { } + StoredInterfaceMemberFunctionCall2(void(Class::*fn)(QFutureInterface &, Arg1, Arg2), Class *object, Arg1 arg1, Arg2 arg2) + : fn(fn), object(object), arg1(arg1), arg2(arg2) {} QFuture start() { @@ -193,12 +184,11 @@ private: Arg1 arg1; Arg2 arg2; }; -template -class StoredInterfaceFunctionCall3 : public QRunnable -{ +template +class StoredInterfaceFunctionCall3 : public QRunnable { public: - StoredInterfaceFunctionCall3(void (fn)(QFutureInterface &, Arg1, Arg2, Arg3), Arg1 arg1, Arg2 arg2, Arg3 arg3) - : fn(fn), arg1(arg1), arg2(arg2), arg3(arg3) { } + StoredInterfaceFunctionCall3(void(fn)(QFutureInterface &, Arg1, Arg2, Arg3), Arg1 arg1, Arg2 arg2, Arg3 arg3) + : fn(fn), arg1(arg1), arg2(arg2), arg3(arg3) {} QFuture start() { @@ -218,12 +208,11 @@ private: FunctionPointer fn; Arg1 arg1; Arg2 arg2; Arg3 arg3; }; -template -class StoredInterfaceMemberFunctionCall3 : public QRunnable -{ +template +class StoredInterfaceMemberFunctionCall3 : public QRunnable { public: - StoredInterfaceMemberFunctionCall3(void (Class::*fn)(QFutureInterface &, Arg1, Arg2, Arg3), Class *object, Arg1 arg1, Arg2 arg2, Arg3 arg3) - : fn(fn), object(object), arg1(arg1), arg2(arg2), arg3(arg3) { } + StoredInterfaceMemberFunctionCall3(void(Class::*fn)(QFutureInterface &, Arg1, Arg2, Arg3), Class *object, Arg1 arg1, Arg2 arg2, Arg3 arg3) + : fn(fn), object(object), arg1(arg1), arg2(arg2), arg3(arg3) {} QFuture start() { @@ -245,12 +234,11 @@ private: Arg1 arg1; Arg2 arg2; Arg3 arg3; }; -template -class StoredInterfaceFunctionCall4 : public QRunnable -{ +template +class StoredInterfaceFunctionCall4 : public QRunnable { public: - StoredInterfaceFunctionCall4(void (fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4), Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4) - : fn(fn), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4) { } + StoredInterfaceFunctionCall4(void(fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4), Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4) + : fn(fn), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4) {} QFuture start() { @@ -270,12 +258,11 @@ private: FunctionPointer fn; Arg1 arg1; Arg2 arg2; Arg3 arg3; Arg4 arg4; }; -template -class StoredInterfaceMemberFunctionCall4 : public QRunnable -{ +template +class StoredInterfaceMemberFunctionCall4 : public QRunnable { public: - StoredInterfaceMemberFunctionCall4(void (Class::*fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4), Class *object, Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4) - : fn(fn), object(object), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4) { } + StoredInterfaceMemberFunctionCall4(void(Class::*fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4), Class *object, Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4) + : fn(fn), object(object), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4) {} QFuture start() { @@ -297,12 +284,11 @@ private: Arg1 arg1; Arg2 arg2; Arg3 arg3; Arg4 arg4; }; -template -class StoredInterfaceFunctionCall5 : public QRunnable -{ +template +class StoredInterfaceFunctionCall5 : public QRunnable { public: - StoredInterfaceFunctionCall5(void (fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4, Arg5), Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4, Arg5 arg5) - : fn(fn), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4), arg5(arg5) { } + StoredInterfaceFunctionCall5(void(fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4, Arg5), Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4, Arg5 arg5) + : fn(fn), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4), arg5(arg5) {} QFuture start() { @@ -322,12 +308,11 @@ private: FunctionPointer fn; Arg1 arg1; Arg2 arg2; Arg3 arg3; Arg4 arg4; Arg5 arg5; }; -template -class StoredInterfaceMemberFunctionCall5 : public QRunnable -{ +template +class StoredInterfaceMemberFunctionCall5 : public QRunnable { public: - StoredInterfaceMemberFunctionCall5(void (Class::*fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4, Arg5), Class *object, Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4, Arg5 arg5) - : fn(fn), object(object), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4), arg5(arg5) { } + StoredInterfaceMemberFunctionCall5(void(Class::*fn)(QFutureInterface &, Arg1, Arg2, Arg3, Arg4, Arg5), Class *object, Arg1 arg1, Arg2 arg2, Arg3 arg3, Arg4 arg4, Arg5 arg5) + : fn(fn), object(object), arg1(arg1), arg2(arg2), arg3(arg3), arg4(arg4), arg5(arg5) {} QFuture start() { @@ -383,9 +368,8 @@ QFuture run(void (*functionPointer)(QFutureInterface &, Arg1, Arg2, Arg3, template QFuture run(void (Class::*fn)(QFutureInterface &), Class *object) { - return (new StoredInterfaceMemberFunctionCall0 &), Class>(fn, object))->start(); + return (new StoredInterfaceMemberFunctionCall0 &), Class>(fn, object))->start(); } - } // namespace QtConcurrent QT_END_NAMESPACE diff --git a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.cpp b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.cpp index 53a36ccf9..a2fb03ed5 100644 --- a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.cpp +++ b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.cpp @@ -24,166 +24,158 @@ /**********************************************************************/ SDLGamepad::SDLGamepad() { - buttons = -1; - axes = -1; - index = -1; - loop = false; - tick = MIN_RATE; - gamepad = 0; + buttons = -1; + axes = -1; + index = -1; + loop = false; + tick = MIN_RATE; + gamepad = 0; } /**********************************************************************/ SDLGamepad::~SDLGamepad() { - loop = false; + loop = false; - if(gamepad) - SDL_JoystickClose(gamepad); + if (gamepad) { + SDL_JoystickClose(gamepad); + } - SDL_Quit(); + SDL_Quit(); } /**********************************************************************/ bool SDLGamepad::init() { - if(SDL_Init(SDL_INIT_JOYSTICK) < 0) - return false; + if (SDL_Init(SDL_INIT_JOYSTICK) < 0) { + return false; + } - if(SDL_NumJoysticks() > 0) - { - emit gamepads(SDL_NumJoysticks()); + if (SDL_NumJoysticks() > 0) { + emit gamepads(SDL_NumJoysticks()); - if(!setGamepad(0)) - return false; + if (!setGamepad(0)) { + return false; + } - for(qint8 i = 0; i < buttons; i++) - buttonStates.append(0); - } - else - { - return false; - } + for (qint8 i = 0; i < buttons; i++) { + buttonStates.append(0); + } + } else { + return false; + } - loop = true; - return true; + loop = true; + return true; } /**********************************************************************/ void SDLGamepad::run() { - while(loop) - { - updateAxes(); - updateButtons(); - msleep(tick); - } + while (loop) { + updateAxes(); + updateButtons(); + msleep(tick); + } } /**********************************************************************/ bool SDLGamepad::setGamepad(qint16 index) { - if (index != this->index) - { - if(SDL_JoystickOpened(this->index)) - SDL_JoystickClose(gamepad); + if (index != this->index) { + if (SDL_JoystickOpened(this->index)) { + SDL_JoystickClose(gamepad); + } - gamepad = SDL_JoystickOpen(index); + gamepad = SDL_JoystickOpen(index); - if(gamepad) - { - buttons = SDL_JoystickNumButtons(gamepad); - axes = SDL_JoystickNumAxes(gamepad); + if (gamepad) { + buttons = SDL_JoystickNumButtons(gamepad); + axes = SDL_JoystickNumAxes(gamepad); - if (axes >= 4) { - this->index = index; - return true; - } - else - { - buttons = -1; - axes = -1; - this->index = -1; - qCritical("Gamepad has less than 4 axes"); - } + if (axes >= 4) { + this->index = index; + return true; + } else { + buttons = -1; + axes = -1; + this->index = -1; + qCritical("Gamepad has less than 4 axes"); + } + } else { + buttons = -1; + axes = -1; + this->index = -1; + qCritical("Unable to open Gamepad!"); + } } - else - { - buttons = -1; - axes = -1; - this->index = -1; - qCritical("Unable to open Gamepad!"); - } - } - return false; + return false; } /**********************************************************************/ void SDLGamepad::setTickRate(qint16 ms) { - tick = ms; + tick = ms; } /**********************************************************************/ void SDLGamepad::updateAxes() { - if(gamepad) - { - QListInt16 values; - SDL_JoystickUpdate(); + if (gamepad) { + QListInt16 values; + SDL_JoystickUpdate(); - for(qint8 i = 0; i < axes; i++) - { - qint16 value = SDL_JoystickGetAxis(gamepad, i); + for (qint8 i = 0; i < axes; i++) { + qint16 value = SDL_JoystickGetAxis(gamepad, i); - if(value > -NULL_RANGE && value < NULL_RANGE) - value = 0; + if (value > -NULL_RANGE && value < NULL_RANGE) { + value = 0; + } - values.append(value); + values.append(value); + } + + emit axesValues(values); } - - emit axesValues(values); - } } /**********************************************************************/ void SDLGamepad::updateButtons() { - if(gamepad) - { - SDL_JoystickUpdate(); + if (gamepad) { + SDL_JoystickUpdate(); - for(qint8 i = 0; i < buttons; i++) - { - qint16 state = SDL_JoystickGetButton(gamepad, i); + for (qint8 i = 0; i < buttons; i++) { + qint16 state = SDL_JoystickGetButton(gamepad, i); - if(buttonStates.at(i) != state) - { - if(state > 0) - emit buttonState((ButtonNumber)i, true); - else - emit buttonState((ButtonNumber)i, false); + if (buttonStates.at(i) != state) { + if (state > 0) { + emit buttonState((ButtonNumber)i, true); + } else { + emit buttonState((ButtonNumber)i, false); + } - buttonStates.replace(i, state); - } + buttonStates.replace(i, state); + } + } } - } } /**********************************************************************/ void SDLGamepad::quit() { - loop = false; + loop = false; } /**********************************************************************/ qint16 SDLGamepad::getAxes() { - return axes; + return axes; } /**********************************************************************/ qint16 SDLGamepad::getButtons() { - return buttons; + return buttons; } diff --git a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.h b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.h index 033f8ff96..217f9465f 100644 --- a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.h +++ b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.h @@ -49,7 +49,7 @@ * * @see SDLGamepad::setTickRate() */ -#define MIN_RATE 10 +#define MIN_RATE 10 /** * Axis enumeration. @@ -58,17 +58,16 @@ * dealing with axes numbers. Up to 10 axes are supported. That should * be more than enough for every gamepad out there. */ -enum AxisNumber -{ - AXIS_1, - AXIS_2, - AXIS_3, - AXIS_4, - AXIS_5, - AXIS_6, - AXIS_7, - AXIS_8, - AXIS_9 +enum AxisNumber { + AXIS_1, + AXIS_2, + AXIS_3, + AXIS_4, + AXIS_5, + AXIS_6, + AXIS_7, + AXIS_8, + AXIS_9 }; /** @@ -81,28 +80,27 @@ enum AxisNumber * * @see SDLGamepad::buttonState() */ -enum ButtonNumber -{ - BUTTON_01, - BUTTON_02, - BUTTON_03, - BUTTON_04, - BUTTON_05, - BUTTON_06, - BUTTON_07, - BUTTON_08, - BUTTON_09, - BUTTON_10, - BUTTON_11, - BUTTON_12, - BUTTON_13, - BUTTON_14, - BUTTON_15, - BUTTON_16, - BUTTON_17, - BUTTON_18, - BUTTON_19, - BUTTON_20 +enum ButtonNumber { + BUTTON_01, + BUTTON_02, + BUTTON_03, + BUTTON_04, + BUTTON_05, + BUTTON_06, + BUTTON_07, + BUTTON_08, + BUTTON_09, + BUTTON_10, + BUTTON_11, + BUTTON_12, + BUTTON_13, + BUTTON_14, + BUTTON_15, + BUTTON_16, + BUTTON_17, + BUTTON_18, + BUTTON_19, + BUTTON_20 }; /** @@ -129,11 +127,10 @@ typedef QList QListInt16; * @version 1.0 * @date 2009 */ -class SDLGAMEPADSHARED_EXPORT SDLGamepad : public QThread -{ - Q_OBJECT +class SDLGAMEPADSHARED_EXPORT SDLGamepad : public QThread { + Q_OBJECT - public: +public: /** * Class constructor. @@ -182,7 +179,7 @@ class SDLGAMEPADSHARED_EXPORT SDLGamepad : public QThread */ qint16 getButtons(); - public slots: +public slots: /** * Init the SDL system and set up gamepad. @@ -227,7 +224,7 @@ class SDLGAMEPADSHARED_EXPORT SDLGamepad : public QThread */ bool setGamepad(qint16 index); - private: +private: /** * Variable to control thread. @@ -312,7 +309,7 @@ class SDLGAMEPADSHARED_EXPORT SDLGamepad : public QThread */ QList buttonStates; - signals: +signals: /** * A signal that emitts the number of gamepads present in SDL. diff --git a/ground/openpilotgcs/src/libs/utils/abstractprocess.h b/ground/openpilotgcs/src/libs/utils/abstractprocess.h index 68f6a45d1..bfbb7e557 100644 --- a/ground/openpilotgcs/src/libs/utils/abstractprocess.h +++ b/ground/openpilotgcs/src/libs/utils/abstractprocess.h @@ -4,25 +4,25 @@ * @file abstractprocess.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,27 +34,37 @@ #include namespace Utils { - -class QTCREATOR_UTILS_EXPORT AbstractProcess -{ +class QTCREATOR_UTILS_EXPORT AbstractProcess { public: AbstractProcess() {} virtual ~AbstractProcess() {} - QString workingDirectory() const { return m_workingDir; } - void setWorkingDirectory(const QString &dir) { m_workingDir = dir; } + QString workingDirectory() const + { + return m_workingDir; + } + void setWorkingDirectory(const QString &dir) + { + m_workingDir = dir; + } - QStringList environment() const { return m_environment; } - void setEnvironment(const QStringList &env) { m_environment = env; } + QStringList environment() const + { + return m_environment; + } + void setEnvironment(const QStringList &env) + { + m_environment = env; + } virtual bool start(const QString &program, const QStringList &args) = 0; virtual void stop() = 0; virtual bool isRunning() const = 0; virtual qint64 applicationPID() const = 0; - virtual int exitCode() const = 0; + virtual int exitCode() const = 0; -//signals: +// signals: virtual void processError(const QString &error) = 0; #ifdef Q_OS_WIN @@ -71,8 +81,6 @@ private: QString m_workingDir; QStringList m_environment; }; - -} //namespace Utils +} // namespace Utils #endif // ABSTRACTPROCESS_H - diff --git a/ground/openpilotgcs/src/libs/utils/abstractprocess_win.cpp b/ground/openpilotgcs/src/libs/utils/abstractprocess_win.cpp index 861ca061f..ed60465e3 100644 --- a/ground/openpilotgcs/src/libs/utils/abstractprocess_win.cpp +++ b/ground/openpilotgcs/src/libs/utils/abstractprocess_win.cpp @@ -4,25 +4,25 @@ * @file abstractprocess_win.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,22 +30,24 @@ #include -namespace Utils { - +namespace Utils { QStringList AbstractProcess::fixWinEnvironment(const QStringList &env) { QStringList envStrings = env; + // add PATH if necessary (for DLL loading) - if (envStrings.filter(QRegExp(QLatin1String("^PATH="),Qt::CaseInsensitive)).isEmpty()) { + if (envStrings.filter(QRegExp(QLatin1String("^PATH="), Qt::CaseInsensitive)).isEmpty()) { QByteArray path = qgetenv("PATH"); - if (!path.isEmpty()) + if (!path.isEmpty()) { envStrings.prepend(QString(QLatin1String("PATH=%1")).arg(QString::fromLocal8Bit(path))); + } } // add systemroot if needed - if (envStrings.filter(QRegExp(QLatin1String("^SystemRoot="),Qt::CaseInsensitive)).isEmpty()) { + if (envStrings.filter(QRegExp(QLatin1String("^SystemRoot="), Qt::CaseInsensitive)).isEmpty()) { QByteArray systemRoot = qgetenv("SystemRoot"); - if (!systemRoot.isEmpty()) + if (!systemRoot.isEmpty()) { envStrings.prepend(QString(QLatin1String("SystemRoot=%1")).arg(QString::fromLocal8Bit(systemRoot))); + } } return envStrings; } @@ -54,9 +56,10 @@ QString AbstractProcess::createWinCommandline(const QString &program, const QStr { const QChar doubleQuote = QLatin1Char('"'); const QChar blank = QLatin1Char(' '); - const QChar backSlash = QLatin1Char('\\'); + const QChar backSlash = QLatin1Char('\\'); + + QString programName = program; - QString programName = program; if (!programName.startsWith(doubleQuote) && !programName.endsWith(doubleQuote) && programName.contains(blank)) { programName.insert(0, doubleQuote); programName.append(doubleQuote); @@ -64,8 +67,9 @@ QString AbstractProcess::createWinCommandline(const QString &program, const QStr // add the prgram as the first arrg ... it works better programName.replace(QLatin1Char('/'), backSlash); QString cmdLine = programName; - if (args.empty()) + if (args.empty()) { return cmdLine; + } cmdLine += blank; for (int i = 0; i < args.size(); ++i) { @@ -99,8 +103,10 @@ QByteArray AbstractProcess::createWinEnvironment(const QStringList &env) { QByteArray envlist; int pos = 0; - foreach (const QString &tmp, env) { + + foreach(const QString &tmp, env) { const uint tmpSize = sizeof(TCHAR) * (tmp.length() + 1); + envlist.resize(envlist.size() + tmpSize); memcpy(envlist.data() + pos, tmp.utf16(), tmpSize); pos += tmpSize; @@ -110,5 +116,4 @@ QByteArray AbstractProcess::createWinEnvironment(const QStringList &env) envlist[pos++] = 0; return envlist; } - -} //namespace Utils +} // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.cpp b/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.cpp index fd3c85c5c..894cccc0b 100644 --- a/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.cpp +++ b/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.cpp @@ -4,25 +4,25 @@ * @file basevalidatinglineedit.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,17 +33,16 @@ enum { debug = 0 }; namespace Utils { - struct BaseValidatingLineEditPrivate { explicit BaseValidatingLineEditPrivate(const QWidget *w); const QColor m_okTextColor; - QColor m_errorTextColor; + QColor m_errorTextColor; BaseValidatingLineEdit::State m_state; QString m_errorMessage; QString m_initialText; - bool m_firstChange; + bool m_firstChange; }; BaseValidatingLineEditPrivate::BaseValidatingLineEditPrivate(const QWidget *w) : @@ -51,8 +50,7 @@ BaseValidatingLineEditPrivate::BaseValidatingLineEditPrivate(const QWidget *w) : m_errorTextColor(Qt::red), m_state(BaseValidatingLineEdit::Invalid), m_firstChange(true) -{ -} +{} BaseValidatingLineEdit::BaseValidatingLineEdit(QWidget *parent) : QLineEdit(parent), @@ -87,9 +85,9 @@ QColor BaseValidatingLineEdit::errorColor() const return m_bd->m_errorTextColor; } -void BaseValidatingLineEdit::setErrorColor(const QColor &c) +void BaseValidatingLineEdit::setErrorColor(const QColor &c) { - m_bd->m_errorTextColor = c; + m_bd->m_errorTextColor = c; } QColor BaseValidatingLineEdit::textColor(const QWidget *w) @@ -100,6 +98,7 @@ QColor BaseValidatingLineEdit::textColor(const QWidget *w) void BaseValidatingLineEdit::setTextColor(QWidget *w, const QColor &c) { QPalette palette = w->palette(); + palette.setColor(QPalette::Active, QPalette::Text, c); w->setPalette(palette); } @@ -125,11 +124,12 @@ void BaseValidatingLineEdit::slotChanged(const QString &t) // Are we displaying the initial text? const bool isDisplayingInitialText = !m_bd->m_initialText.isEmpty() && t == m_bd->m_initialText; const State newState = isDisplayingInitialText ? - DisplayingInitialText : - (validate(t, &m_bd->m_errorMessage) ? Valid : Invalid); + DisplayingInitialText : + (validate(t, &m_bd->m_errorMessage) ? Valid : Invalid); setToolTip(m_bd->m_errorMessage); - if (debug) - qDebug() << Q_FUNC_INFO << t << "State" << m_bd->m_state << "->" << newState << m_bd->m_errorMessage; + if (debug) { + qDebug() << Q_FUNC_INFO << t << "State" << m_bd->m_state << "->" << newState << m_bd->m_errorMessage; + } // Changed..figure out if valid changed. DisplayingInitialText is not valid, // but should not show error color. Also trigger on the first change. if (newState != m_bd->m_state || m_bd->m_firstChange) { @@ -146,13 +146,13 @@ void BaseValidatingLineEdit::slotChanged(const QString &t) void BaseValidatingLineEdit::slotReturnPressed() { - if (isValid()) + if (isValid()) { emit validReturnPressed(); + } } void BaseValidatingLineEdit::triggerChanged() { slotChanged(text()); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.h b/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.h index 2e66a7b58..9d4cd67b7 100644 --- a/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.h +++ b/ground/openpilotgcs/src/libs/utils/basevalidatinglineedit.h @@ -4,25 +4,25 @@ * @file basevalidatinglineedit.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,6 @@ #include namespace Utils { - struct BaseValidatingLineEditPrivate; /** @@ -48,10 +47,8 @@ struct BaseValidatingLineEditPrivate; * "". This results in state 'DisplayingInitialText', which * is not valid, but is not marked red. */ -class QTCREATOR_UTILS_EXPORT BaseValidatingLineEdit : public QLineEdit -{ - Q_OBJECT - Q_DISABLE_COPY(BaseValidatingLineEdit) +class QTCREATOR_UTILS_EXPORT BaseValidatingLineEdit : public QLineEdit { + Q_OBJECT Q_DISABLE_COPY(BaseValidatingLineEdit) Q_PROPERTY(QString initialText READ initialText WRITE setInitialText DESIGNABLE true) Q_PROPERTY(QColor errorColor READ errorColor WRITE setErrorColor DESIGNABLE true) @@ -70,7 +67,7 @@ public: void setInitialText(const QString &); QColor errorColor() const; - void setErrorColor(const QColor &); + void setErrorColor(const QColor &); // Trigger an update (after changing settings) void triggerChanged(); @@ -95,7 +92,6 @@ protected slots: private: BaseValidatingLineEditPrivate *m_bd; }; - } // namespace Utils #endif // BASEVALIDATINGLINEEDIT_H diff --git a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp index 2edef8ddf..6cf8d7643 100644 --- a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp +++ b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp @@ -30,7 +30,7 @@ #define GL_CLAMP_TO_EDGE 0x812F #endif -CachedSvgItem::CachedSvgItem(QGraphicsItem * parent) : +CachedSvgItem::CachedSvgItem(QGraphicsItem *parent) : QGraphicsSvgItem(parent), m_context(0), m_texture(0), @@ -39,7 +39,7 @@ CachedSvgItem::CachedSvgItem(QGraphicsItem * parent) : setCacheMode(NoCache); } -CachedSvgItem::CachedSvgItem(const QString & fileName, QGraphicsItem * parent): +CachedSvgItem::CachedSvgItem(const QString & fileName, QGraphicsItem *parent) : QGraphicsSvgItem(fileName, parent), m_context(0), m_texture(0), @@ -58,46 +58,47 @@ CachedSvgItem::~CachedSvgItem() void CachedSvgItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) { - if (painter->paintEngine()->type() != QPaintEngine::OpenGL && - painter->paintEngine()->type() != QPaintEngine::OpenGL2) { - //Fallback to direct painting + painter->paintEngine()->type() != QPaintEngine::OpenGL2) { + // Fallback to direct painting QGraphicsSvgItem::paint(painter, option, widget); return; } QRectF br = boundingRect(); - QTransform transform = painter->worldTransform(); - qreal sceneScale = transform.map(QLineF(0,0,1,0)).length(); + QTransform transform = painter->worldTransform(); + qreal sceneScale = transform.map(QLineF(0, 0, 1, 0)).length(); bool stencilTestEnabled = glIsEnabled(GL_STENCIL_TEST); bool scissorTestEnabled = glIsEnabled(GL_SCISSOR_TEST); painter->beginNativePainting(); - if (stencilTestEnabled) + if (stencilTestEnabled) { glEnable(GL_STENCIL_TEST); - if (scissorTestEnabled) + } + if (scissorTestEnabled) { glEnable(GL_SCISSOR_TEST); + } bool dirty = false; if (!m_texture) { glGenTextures(1, &m_texture); - m_context = const_cast(QGLContext::currentContext()); + m_context = const_cast(QGLContext::currentContext()); - dirty = true; + dirty = true; } if (!qFuzzyCompare(sceneScale, m_scale)) { m_scale = sceneScale; - dirty = true; + dirty = true; } - int textureWidth = (int(br.width()*m_scale) + 3) & ~3; - int textureHeight = (int(br.height()*m_scale) + 3) & ~3; + int textureWidth = (int(br.width() * m_scale) + 3) & ~3; + int textureHeight = (int(br.height() * m_scale) + 3) & ~3; if (dirty) { - //qDebug() << "re-render image"; + // qDebug() << "re-render image"; QImage img(textureWidth, textureHeight, QImage::Format_ARGB32); { @@ -117,15 +118,15 @@ void CachedSvgItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *opt glBindTexture(GL_TEXTURE_2D, m_texture); glTexImage2D( - GL_TEXTURE_2D, - 0, - GL_RGBA, - textureWidth, - textureHeight, - 0, - GL_RGBA, - GL_UNSIGNED_BYTE, - img.bits()); + GL_TEXTURE_2D, + 0, + GL_RGBA, + textureWidth, + textureHeight, + 0, + GL_RGBA, + GL_UNSIGNED_BYTE, + img.bits()); glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); @@ -143,15 +144,15 @@ void CachedSvgItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *opt glBindTexture(GL_TEXTURE_2D, m_texture); - //texture may be slightly large than svn image, ensure only used area is rendered - qreal tw = br.width()*m_scale/textureWidth; - qreal th = br.height()*m_scale/textureHeight; + // texture may be slightly large than svn image, ensure only used area is rendered + qreal tw = br.width() * m_scale / textureWidth; + qreal th = br.height() * m_scale / textureHeight; glBegin(GL_QUADS); - glTexCoord2d(0, 0 ); glVertex3d(br.left(), br.top(), -1); - glTexCoord2d(tw, 0 ); glVertex3d(br.right(), br.top(), -1); + glTexCoord2d(0, 0); glVertex3d(br.left(), br.top(), -1); + glTexCoord2d(tw, 0); glVertex3d(br.right(), br.top(), -1); glTexCoord2d(tw, th); glVertex3d(br.right(), br.bottom(), -1); - glTexCoord2d(0, th); glVertex3d(br.left(), br.bottom(), -1); + glTexCoord2d(0, th); glVertex3d(br.left(), br.bottom(), -1); glEnd(); glDisable(GL_TEXTURE_2D); diff --git a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h index 747ef391c..de88d5281 100644 --- a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h +++ b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h @@ -32,15 +32,14 @@ class QGLContext; -//Cache Svg item as GL Texture. -//Texture is regenerated each time item is scaled -//but it's reused during rotation, unlike DeviceCoordinateCache mode -class QTCREATOR_UTILS_EXPORT CachedSvgItem: public QGraphicsSvgItem -{ +// Cache Svg item as GL Texture. +// Texture is regenerated each time item is scaled +// but it's reused during rotation, unlike DeviceCoordinateCache mode +class QTCREATOR_UTILS_EXPORT CachedSvgItem : public QGraphicsSvgItem { Q_OBJECT public: - CachedSvgItem(QGraphicsItem * parent = 0); - CachedSvgItem(const QString & fileName, QGraphicsItem * parent = 0); + CachedSvgItem(QGraphicsItem *parent = 0); + CachedSvgItem(const QString & fileName, QGraphicsItem *parent = 0); ~CachedSvgItem(); void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget); @@ -51,4 +50,4 @@ private: qreal m_scale; }; -#endif +#endif // ifndef CACHEDSVGITEM_H diff --git a/ground/openpilotgcs/src/libs/utils/checkablemessagebox.cpp b/ground/openpilotgcs/src/libs/utils/checkablemessagebox.cpp index d176d7ee6..83c344cbc 100644 --- a/ground/openpilotgcs/src/libs/utils/checkablemessagebox.cpp +++ b/ground/openpilotgcs/src/libs/utils/checkablemessagebox.cpp @@ -4,25 +4,25 @@ * @file checkablemessagebox.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,7 +33,6 @@ #include namespace Utils { - struct CheckableMessageBoxPrivate { CheckableMessageBoxPrivate() : clickedButton(0) {} @@ -51,7 +50,7 @@ CheckableMessageBox::CheckableMessageBox(QWidget *parent) : m_d->ui.pixmapLabel->setVisible(false); connect(m_d->ui.buttonBox, SIGNAL(accepted()), this, SLOT(accept())); connect(m_d->ui.buttonBox, SIGNAL(rejected()), this, SLOT(reject())); - connect(m_d->ui.buttonBox, SIGNAL(clicked(QAbstractButton*)), this, SLOT(slotClicked(QAbstractButton*))); + connect(m_d->ui.buttonBox, SIGNAL(clicked(QAbstractButton *)), this, SLOT(slotClicked(QAbstractButton *))); } CheckableMessageBox::~CheckableMessageBox() @@ -71,8 +70,9 @@ QAbstractButton *CheckableMessageBox::clickedButton() const QDialogButtonBox::StandardButton CheckableMessageBox::clickedStandardButton() const { - if (m_d->clickedButton) + if (m_d->clickedButton) { return m_d->ui.buttonBox->standardButton(m_d->clickedButton); + } return QDialogButtonBox::NoButton; } @@ -88,8 +88,9 @@ void CheckableMessageBox::setText(const QString &t) QPixmap CheckableMessageBox::iconPixmap() const { - if (const QPixmap *p = m_d->ui.pixmapLabel->pixmap()) + if (const QPixmap * p = m_d->ui.pixmapLabel->pixmap()) { return QPixmap(*p); + } return QPixmap(); } @@ -131,31 +132,33 @@ void CheckableMessageBox::setStandardButtons(QDialogButtonBox::StandardButtons s QDialogButtonBox::StandardButton CheckableMessageBox::defaultButton() const { - foreach (QAbstractButton *b, m_d->ui.buttonBox->buttons()) - if (QPushButton *pb = qobject_cast(b)) - if (pb->isDefault()) - return m_d->ui.buttonBox->standardButton(pb); + foreach(QAbstractButton * b, m_d->ui.buttonBox->buttons()) + if (QPushButton * pb = qobject_cast(b)) { + if (pb->isDefault()) { + return m_d->ui.buttonBox->standardButton(pb); + } + } return QDialogButtonBox::NoButton; } void CheckableMessageBox::setDefaultButton(QDialogButtonBox::StandardButton s) { - if (QPushButton *b = m_d->ui.buttonBox->button(s)) { + if (QPushButton * b = m_d->ui.buttonBox->button(s)) { b->setDefault(true); b->setFocus(); } } -QDialogButtonBox::StandardButton - CheckableMessageBox::question(QWidget *parent, - const QString &title, - const QString &question, - const QString &checkBoxText, - bool *checkBoxSetting, - QDialogButtonBox::StandardButtons buttons, - QDialogButtonBox::StandardButton defaultButton) +QDialogButtonBox::StandardButton CheckableMessageBox::question(QWidget *parent, + const QString &title, + const QString &question, + const QString &checkBoxText, + bool *checkBoxSetting, + QDialogButtonBox::StandardButtons buttons, + QDialogButtonBox::StandardButton defaultButton) { CheckableMessageBox mb(parent); + mb.setWindowTitle(title); mb.setIconPixmap(QMessageBox::standardIcon(QMessageBox::Question)); mb.setText(question); @@ -172,5 +175,4 @@ QMessageBox::StandardButton CheckableMessageBox::dialogButtonBoxToMessageBoxButt { return static_cast(int(db)); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/checkablemessagebox.h b/ground/openpilotgcs/src/libs/utils/checkablemessagebox.h index f9af481dd..d1565fe80 100644 --- a/ground/openpilotgcs/src/libs/utils/checkablemessagebox.h +++ b/ground/openpilotgcs/src/libs/utils/checkablemessagebox.h @@ -4,25 +4,25 @@ * @file checkablemessagebox.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,17 +36,14 @@ #include namespace Utils { - struct CheckableMessageBoxPrivate; /* A messagebox suitable for questions with a * "Do not ask me again" checkbox. Emulates the QMessageBox API with * static conveniences. */ -class QTCREATOR_UTILS_EXPORT CheckableMessageBox : public QDialog -{ - Q_OBJECT - Q_PROPERTY(QString text READ text WRITE setText) +class QTCREATOR_UTILS_EXPORT CheckableMessageBox : public QDialog { + Q_OBJECT Q_PROPERTY(QString text READ text WRITE setText) Q_PROPERTY(QPixmap iconPixmap READ iconPixmap WRITE setIconPixmap) Q_PROPERTY(bool isChecked READ isChecked WRITE setChecked) Q_PROPERTY(QString checkBoxText READ checkBoxText WRITE setCheckBoxText) @@ -56,14 +53,13 @@ public: explicit CheckableMessageBox(QWidget *parent); virtual ~CheckableMessageBox(); - static QDialogButtonBox::StandardButton - question(QWidget *parent, - const QString &title, - const QString &question, - const QString &checkBoxText, - bool *checkBoxSetting, - QDialogButtonBox::StandardButtons buttons = QDialogButtonBox::Yes|QDialogButtonBox::No, - QDialogButtonBox::StandardButton defaultButton = QDialogButtonBox::No); + static QDialogButtonBox::StandardButton question(QWidget *parent, + const QString &title, + const QString &question, + const QString &checkBoxText, + bool *checkBoxSetting, + QDialogButtonBox::StandardButtons buttons = QDialogButtonBox::Yes | QDialogButtonBox::No, + QDialogButtonBox::StandardButton defaultButton = QDialogButtonBox::No); QString text() const; void setText(const QString &); @@ -74,30 +70,29 @@ public: QString checkBoxText() const; void setCheckBoxText(const QString &); - QDialogButtonBox::StandardButtons standardButtons() const; - void setStandardButtons(QDialogButtonBox::StandardButtons s); + QDialogButtonBox::StandardButtons standardButtons() const; + void setStandardButtons(QDialogButtonBox::StandardButtons s); - QDialogButtonBox::StandardButton defaultButton() const; - void setDefaultButton(QDialogButtonBox::StandardButton s); + QDialogButtonBox::StandardButton defaultButton() const; + void setDefaultButton(QDialogButtonBox::StandardButton s); // see static QMessageBox::standardPixmap() QPixmap iconPixmap() const; - void setIconPixmap (const QPixmap &p); + void setIconPixmap(const QPixmap &p); - // Query the result - QAbstractButton *clickedButton() const; - QDialogButtonBox::StandardButton clickedStandardButton() const; + // Query the result + QAbstractButton *clickedButton() const; + QDialogButtonBox::StandardButton clickedStandardButton() const; - // Conversion convenience - static QMessageBox::StandardButton dialogButtonBoxToMessageBoxButton(QDialogButtonBox::StandardButton); + // Conversion convenience + static QMessageBox::StandardButton dialogButtonBoxToMessageBoxButton(QDialogButtonBox::StandardButton); private slots: - void slotClicked(QAbstractButton *b); + void slotClicked(QAbstractButton *b); private: - CheckableMessageBoxPrivate *m_d; + CheckableMessageBoxPrivate *m_d; }; - } // namespace Utils #endif // CHECKABLEMESSAGEBOX_H diff --git a/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.cpp b/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.cpp index 1f23c9be5..d1a1e7d9d 100644 --- a/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.cpp +++ b/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.cpp @@ -4,25 +4,25 @@ * @file classnamevalidatinglineedit.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,6 @@ #include namespace Utils { - struct ClassNameValidatingLineEditPrivate { ClassNameValidatingLineEditPrivate(); @@ -45,21 +44,20 @@ struct ClassNameValidatingLineEditPrivate { }; // Match something like "Namespace1::Namespace2::ClassName". -ClassNameValidatingLineEditPrivate:: ClassNameValidatingLineEditPrivate() : +ClassNameValidatingLineEditPrivate::ClassNameValidatingLineEditPrivate() : m_nameRegexp(QLatin1String("[a-zA-Z_][a-zA-Z0-9_]*(::[a-zA-Z_][a-zA-Z0-9_]*)*")), m_namespaceDelimiter(QLatin1String("::")), m_namespacesEnabled(false), m_lowerCaseFileName(true) { - QTC_ASSERT(m_nameRegexp.isValid(), return); + QTC_ASSERT(m_nameRegexp.isValid(), return ); } // --------------------- ClassNameValidatingLineEdit ClassNameValidatingLineEdit::ClassNameValidatingLineEdit(QWidget *parent) : Utils::BaseValidatingLineEdit(parent), m_d(new ClassNameValidatingLineEditPrivate) -{ -} +{} ClassNameValidatingLineEdit::~ClassNameValidatingLineEdit() { @@ -79,16 +77,19 @@ void ClassNameValidatingLineEdit::setNamespacesEnabled(bool b) bool ClassNameValidatingLineEdit::validate(const QString &value, QString *errorMessage) const { if (!m_d->m_namespacesEnabled && value.contains(QLatin1Char(':'))) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The class name must not contain namespace delimiters."); + } return false; } else if (value.isEmpty()) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("Please enter a class name."); + } return false; } else if (!m_d->m_nameRegexp.exactMatch(value)) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The class name contains invalid characters."); + } return false; } return true; @@ -97,13 +98,15 @@ bool ClassNameValidatingLineEdit::validate(const QString &value, QString *errorM void ClassNameValidatingLineEdit::slotChanged(const QString &t) { Utils::BaseValidatingLineEdit::slotChanged(t); + if (isValid()) { // Suggest file names, strip namespaces QString fileName = m_d->m_lowerCaseFileName ? t.toLower() : t; if (m_d->m_namespacesEnabled) { const int namespaceIndex = fileName.lastIndexOf(m_d->m_namespaceDelimiter); - if (namespaceIndex != -1) + if (namespaceIndex != -1) { fileName.remove(0, namespaceIndex + m_d->m_namespaceDelimiter.size()); + } } emit updateFileName(fileName); } @@ -114,6 +117,7 @@ QString ClassNameValidatingLineEdit::createClassName(const QString &name) // Remove spaces and convert the adjacent characters to uppercase QString className = name; QRegExp spaceMatcher(QLatin1String(" +(\\w)"), Qt::CaseSensitive, QRegExp::RegExp2); + QTC_ASSERT(spaceMatcher.isValid(), /**/); int pos; while ((pos = spaceMatcher.indexIn(className)) != -1) { @@ -144,5 +148,4 @@ void ClassNameValidatingLineEdit::setLowerCaseFileName(bool v) { m_d->m_lowerCaseFileName = v; } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.h b/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.h index 83890f31e..e656c6079 100644 --- a/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.h +++ b/ground/openpilotgcs/src/libs/utils/classnamevalidatinglineedit.h @@ -4,25 +4,25 @@ * @file classnamevalidatinglineedit.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,15 +33,13 @@ #include "basevalidatinglineedit.h" namespace Utils { - struct ClassNameValidatingLineEditPrivate; /* A Line edit that validates a C++ class name and emits a signal * to derive suggested file names from it. */ class QTCREATOR_UTILS_EXPORT ClassNameValidatingLineEdit - : public Utils::BaseValidatingLineEdit -{ + : public Utils::BaseValidatingLineEdit { Q_DISABLE_COPY(ClassNameValidatingLineEdit) Q_PROPERTY(bool namespacesEnabled READ namespacesEnabled WRITE setNamespacesEnabled DESIGNABLE true) Q_PROPERTY(bool lowerCaseFileName READ lowerCaseFileName WRITE setLowerCaseFileName) @@ -72,7 +70,6 @@ protected: private: ClassNameValidatingLineEditPrivate *m_d; }; - } // namespace Utils #endif // CLASSNAMEVALIDATINGLINEEDIT_H diff --git a/ground/openpilotgcs/src/libs/utils/codegeneration.cpp b/ground/openpilotgcs/src/libs/utils/codegeneration.cpp index a8b93c6f2..db9e640ea 100644 --- a/ground/openpilotgcs/src/libs/utils/codegeneration.cpp +++ b/ground/openpilotgcs/src/libs/utils/codegeneration.cpp @@ -4,25 +4,25 @@ * @file codegeneration.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,20 +33,20 @@ #include namespace Utils { - QTCREATOR_UTILS_EXPORT QString fileNameToCppIdentifier(const QString &s) { QString rc; - const int len = s.size(); - const QChar underscore = QLatin1Char('_'); - const QChar dot = QLatin1Char('.'); + const int len = s.size(); + const QChar underscore = QLatin1Char('_'); + const QChar dot = QLatin1Char('.'); for (int i = 0; i < len; i++) { const QChar c = s.at(i); - if (c == underscore || c.isLetterOrNumber()) + if (c == underscore || c.isLetterOrNumber()) { rc += c; - else if (c == dot) + } else if (c == dot) { rc += underscore; + } } return rc; } @@ -55,6 +55,7 @@ QTCREATOR_UTILS_EXPORT QString headerGuard(const QString &file) { const QFileInfo fi(file); QString rc = fileNameToCppIdentifier(fi.completeBaseName()).toUpper(); + rc += QLatin1Char('_'); rc += fileNameToCppIdentifier(fi.suffix()).toUpper(); return rc; @@ -64,9 +65,10 @@ QTCREATOR_UTILS_EXPORT void writeIncludeFileDirective(const QString &file, bool globalInclude, QTextStream &str) { - const QChar opening = globalInclude ? QLatin1Char('<') : QLatin1Char('"'); - const QChar closing = globalInclude ? QLatin1Char('>') : QLatin1Char('"'); - str << QLatin1String("#include ") << opening << file << closing << QLatin1Char('\n'); + const QChar opening = globalInclude ? QLatin1Char('<') : QLatin1Char('"'); + const QChar closing = globalInclude ? QLatin1Char('>') : QLatin1Char('"'); + + str << QLatin1String("#include ") << opening << file << closing << QLatin1Char('\n'); } QTCREATOR_UTILS_EXPORT @@ -75,6 +77,7 @@ QString writeOpeningNameSpaces(const QStringList &l, const QString &indent, { const int count = l.size(); QString rc; + if (count) { str << '\n'; for (int i = 0; i < count; i++) { @@ -89,13 +92,14 @@ QTCREATOR_UTILS_EXPORT void writeClosingNameSpaces(const QStringList &l, const QString &indent, QTextStream &str) { - if (!l.empty()) + if (!l.empty()) { str << '\n'; + } for (int i = l.size() - 1; i >= 0; i--) { - if (i) + if (i) { str << QString(indent.size() * i, QLatin1Char(' ')); - str << "} // namespace " << l.at(i) << '\n'; + } + str << "} // namespace " << l.at(i) << '\n'; } } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/codegeneration.h b/ground/openpilotgcs/src/libs/utils/codegeneration.h index 80debe585..bba0f8b3a 100644 --- a/ground/openpilotgcs/src/libs/utils/codegeneration.h +++ b/ground/openpilotgcs/src/libs/utils/codegeneration.h @@ -4,25 +4,25 @@ * @file codegeneration.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,7 +38,6 @@ class QStringList; QT_END_NAMESPACE namespace Utils { - // Convert a file name to a Cpp identifier (stripping invalid characters // or replacing them by an underscore). QTCREATOR_UTILS_EXPORT QString fileNameToCppIdentifier(const QString &s); @@ -62,7 +61,6 @@ QTCREATOR_UTILS_EXPORT void writeClosingNameSpaces(const QStringList &namespaces, const QString &indent, QTextStream &str); - } // namespace Utils #endif // CODEGENERATION_H diff --git a/ground/openpilotgcs/src/libs/utils/consoleprocess.cpp b/ground/openpilotgcs/src/libs/utils/consoleprocess.cpp index 596e45977..44bc23fbd 100644 --- a/ground/openpilotgcs/src/libs/utils/consoleprocess.cpp +++ b/ground/openpilotgcs/src/libs/utils/consoleprocess.cpp @@ -4,40 +4,41 @@ * @file consoleprocess.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "consoleprocess.h" namespace Utils { - QString ConsoleProcess::modeOption(Mode m) { switch (m) { - case Debug: + case Debug: return QLatin1String("debug"); - case Suspend: + + case Suspend: return QLatin1String("suspend"); - case Run: + + case Run: break; } return QLatin1String("run"); @@ -50,8 +51,8 @@ QString ConsoleProcess::msgCommChannelFailed(const QString &error) QString ConsoleProcess::msgPromptToClose() { - //! Showed in a terminal which might have - //! a different character set on Windows. + // ! Showed in a terminal which might have + // ! a different character set on Windows. return tr("Press to close this window..."); } @@ -79,5 +80,4 @@ QString ConsoleProcess::msgCannotExecute(const QString & p, const QString &why) { return tr("Cannot execute '%1': %2").arg(p, why); } - } diff --git a/ground/openpilotgcs/src/libs/utils/consoleprocess.h b/ground/openpilotgcs/src/libs/utils/consoleprocess.h index 389a14c38..79322f23e 100644 --- a/ground/openpilotgcs/src/libs/utils/consoleprocess.h +++ b/ground/openpilotgcs/src/libs/utils/consoleprocess.h @@ -4,25 +4,25 @@ * @file consoleprocess.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -51,9 +51,7 @@ class QTemporaryFile; QT_END_NAMESPACE namespace Utils { - -class QTCREATOR_UTILS_EXPORT ConsoleProcess : public QObject, public AbstractProcess -{ +class QTCREATOR_UTILS_EXPORT ConsoleProcess : public QObject, public AbstractProcess { Q_OBJECT public: @@ -64,16 +62,34 @@ public: bool start(const QString &program, const QStringList &args); void stop(); - void setMode(Mode m) { m_mode = m; } - Mode mode() const { return m_mode; } + void setMode(Mode m) + { + m_mode = m; + } + Mode mode() const + { + return m_mode; + } bool isRunning() const; // This reflects the state of the console+stub - qint64 applicationPID() const { return m_appPid; } - int exitCode() const { return m_appCode; } // This will be the signal number if exitStatus == CrashExit - QProcess::ExitStatus exitStatus() const { return m_appStatus; } + qint64 applicationPID() const + { + return m_appPid; + } + int exitCode() const + { + return m_appCode; + } // This will be the signal number if exitStatus == CrashExit + QProcess::ExitStatus exitStatus() const + { + return m_appStatus; + } #ifdef Q_OS_UNIX - void setSettings(QSettings *settings) { m_settings = settings; } + void setSettings(QSettings *settings) + { + m_settings = settings; + } static QString defaultTerminalEmulator(); static QString terminalEmulator(const QSettings *settings); static void setTerminalEmulator(QSettings *settings, const QString &term); @@ -132,9 +148,7 @@ private: QByteArray m_stubServerDir; QSettings *m_settings; #endif - }; +} // namespace Utils -} //namespace Utils - -#endif +#endif // ifndef CONSOLEPROCESS_H diff --git a/ground/openpilotgcs/src/libs/utils/consoleprocess_unix.cpp b/ground/openpilotgcs/src/libs/utils/consoleprocess_unix.cpp index 6a66d6d78..fb7165046 100644 --- a/ground/openpilotgcs/src/libs/utils/consoleprocess_unix.cpp +++ b/ground/openpilotgcs/src/libs/utils/consoleprocess_unix.cpp @@ -4,25 +4,25 @@ * @file consoleprocess_unix.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -64,8 +64,9 @@ ConsoleProcess::~ConsoleProcess() bool ConsoleProcess::start(const QString &program, const QStringList &args) { - if (isRunning()) + if (isRunning()) { return false; + } const QString err = stubServerListen(); if (!err.isEmpty()) { @@ -82,7 +83,7 @@ bool ConsoleProcess::start(const QString &program, const QStringList &args) m_tempFile = 0; return false; } - foreach (const QString &var, environment()) { + foreach(const QString &var, environment()) { m_tempFile->write(var.toLocal8Bit()); m_tempFile->write("", 1); } @@ -92,16 +93,16 @@ bool ConsoleProcess::start(const QString &program, const QStringList &args) QStringList xtermArgs = terminalEmulator(m_settings).split(QLatin1Char(' ')); // FIXME: quoting xtermArgs #ifdef Q_OS_MAC - << (QCoreApplication::applicationDirPath() + QLatin1String("/../Resources/qtcreator_process_stub")) -#else - << (QCoreApplication::applicationDirPath() + QLatin1String("/qtcreator_process_stub")) -#endif - << modeOption(m_mode) - << m_stubServer.fullServerName() - << msgPromptToClose() - << workingDirectory() - << (m_tempFile ? m_tempFile->fileName() : 0) - << program << args; + << (QCoreApplication::applicationDirPath() + QLatin1String("/../Resources/qtcreator_process_stub")) + #else + << (QCoreApplication::applicationDirPath() + QLatin1String("/qtcreator_process_stub")) + #endif + << modeOption(m_mode) + << m_stubServer.fullServerName() + << msgPromptToClose() + << workingDirectory() + << (m_tempFile ? m_tempFile->fileName() : 0) + << program << args; QString xterm = xtermArgs.takeFirst(); m_process.start(xterm, xtermArgs); @@ -119,13 +120,15 @@ bool ConsoleProcess::start(const QString &program, const QStringList &args) void ConsoleProcess::stop() { - if (!isRunning()) + if (!isRunning()) { return; + } stubServerShutdown(); m_appPid = 0; m_process.terminate(); - if (!m_process.waitForFinished(1000)) + if (!m_process.waitForFinished(1000)) { m_process.kill(); + } m_process.waitForFinished(); } @@ -139,21 +142,25 @@ QString ConsoleProcess::stubServerListen() // We need to put the socket in a private directory, as some systems simply do not // check the file permissions of sockets. QString stubFifoDir; + forever { { QTemporaryFile tf; - if (!tf.open()) + if (!tf.open()) { return msgCannotCreateTempFile(tf.errorString()); + } stubFifoDir = QFile::encodeName(tf.fileName()); } // By now the temp file was deleted again m_stubServerDir = QFile::encodeName(stubFifoDir); - if (!::mkdir(m_stubServerDir.constData(), 0700)) + if (!::mkdir(m_stubServerDir.constData(), 0700)) { break; - if (errno != EEXIST) + } + if (errno != EEXIST) { return msgCannotCreateTempDir(stubFifoDir, QString::fromLocal8Bit(strerror(errno))); + } } - const QString stubServer = stubFifoDir + "/stub-socket"; + const QString stubServer = stubFifoDir + "/stub-socket"; if (!m_stubServer.listen(stubServer)) { ::rmdir(m_stubServerDir.constData()); return tr("Cannot create socket '%1': %2").arg(stubServer, m_stubServer.errorString()); @@ -196,17 +203,17 @@ void ConsoleProcess::readStubOutput() delete m_tempFile; m_tempFile = 0; - m_appPid = out.mid(4).toInt(); + m_appPid = out.mid(4).toInt(); emit processStarted(); } else if (out.startsWith("exit ")) { m_appStatus = QProcess::NormalExit; - m_appCode = out.mid(5).toInt(); - m_appPid = 0; + m_appCode = out.mid(5).toInt(); + m_appPid = 0; emit processStopped(); } else if (out.startsWith("crash ")) { m_appStatus = QProcess::CrashExit; - m_appCode = out.mid(6).toInt(); - m_appPid = 0; + m_appCode = out.mid(6).toInt(); + m_appPid = 0; emit processStopped(); } else { emit processError(msgUnexpectedOutput()); @@ -219,15 +226,16 @@ void ConsoleProcess::readStubOutput() void ConsoleProcess::stubExited() { // The stub exit might get noticed before we read the error status. - if (m_stubSocket && m_stubSocket->state() == QLocalSocket::ConnectedState) + if (m_stubSocket && m_stubSocket->state() == QLocalSocket::ConnectedState) { m_stubSocket->waitForDisconnected(); + } stubServerShutdown(); delete m_tempFile; m_tempFile = 0; if (m_appPid) { m_appStatus = QProcess::CrashExit; - m_appCode = -1; - m_appPid = 0; + m_appCode = -1; + m_appPid = 0; emit processStopped(); // Maybe it actually did not, but keep state consistent } emit wrapperStopped(); @@ -236,19 +244,23 @@ void ConsoleProcess::stubExited() QString ConsoleProcess::defaultTerminalEmulator() { // FIXME: enable this once runInTerminal works nicely -#if 0 //def Q_OS_MAC +#if 0 // def Q_OS_MAC return QDir::cleanPath(QCoreApplication::applicationDirPath() + QLatin1String("/../Resources/runInTerminal.command")); + #else return QLatin1String("xterm"); + #endif } QString ConsoleProcess::terminalEmulator(const QSettings *settings) { const QString dflt = defaultTerminalEmulator() + QLatin1String(" -e"); - if (!settings) + + if (!settings) { return dflt; + } return settings->value(QLatin1String("General/TerminalEmulator"), dflt).toString(); } diff --git a/ground/openpilotgcs/src/libs/utils/consoleprocess_win.cpp b/ground/openpilotgcs/src/libs/utils/consoleprocess_win.cpp index 15eec8361..a4a6bfb9d 100644 --- a/ground/openpilotgcs/src/libs/utils/consoleprocess_win.cpp +++ b/ground/openpilotgcs/src/libs/utils/consoleprocess_win.cpp @@ -4,25 +4,25 @@ * @file consoleprocess_win.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -62,8 +62,9 @@ ConsoleProcess::~ConsoleProcess() bool ConsoleProcess::start(const QString &program, const QStringList &args) { - if (isRunning()) + if (isRunning()) { return false; + } const QString err = stubServerListen(); if (!err.isEmpty()) { @@ -83,8 +84,8 @@ bool ConsoleProcess::start(const QString &program, const QStringList &args) QTextStream out(m_tempFile); out.setCodec("UTF-16LE"); out.setGenerateByteOrderMark(false); - foreach (const QString &var, fixWinEnvironment(environment())) - out << var << QChar(0); + foreach(const QString &var, fixWinEnvironment(environment())) + out << var << QChar(0); out << QChar(0); } @@ -96,8 +97,9 @@ bool ConsoleProcess::start(const QString &program, const QStringList &args) ZeroMemory(m_pid, sizeof(PROCESS_INFORMATION)); QString workDir = QDir::toNativeSeparators(workingDirectory()); - if (!workDir.isEmpty() && !workDir.endsWith('\\')) + if (!workDir.isEmpty() && !workDir.endsWith('\\')) { workDir.append('\\'); + } QStringList stubArgs; stubArgs << modeOption(m_mode) @@ -108,9 +110,9 @@ bool ConsoleProcess::start(const QString &program, const QStringList &args) << msgPromptToClose(); const QString cmdLine = createWinCommandline( - QCoreApplication::applicationDirPath() + QLatin1String("/qtcreator_process_stub.exe"), stubArgs); + QCoreApplication::applicationDirPath() + QLatin1String("/qtcreator_process_stub.exe"), stubArgs); - bool success = CreateProcessW(0, (WCHAR*)cmdLine.utf16(), + bool success = CreateProcessW(0, (WCHAR *)cmdLine.utf16(), 0, 0, FALSE, CREATE_NEW_CONSOLE, 0, 0, &si, m_pid); @@ -153,8 +155,9 @@ QString ConsoleProcess::stubServerListen() { if (m_stubServer.listen(QString::fromLatin1("creator-%1-%2") .arg(QCoreApplication::applicationPid()) - .arg(rand()))) + .arg(rand()))) { return QString(); + } return m_stubServer.errorString(); } @@ -162,8 +165,9 @@ void ConsoleProcess::stubServerShutdown() { delete m_stubSocket; m_stubSocket = 0; - if (m_stubServer.isListening()) + if (m_stubServer.isListening()) { m_stubServer.close(); + } } void ConsoleProcess::stubConnectionAvailable() @@ -184,12 +188,12 @@ void ConsoleProcess::readStubOutput() } else if (out.startsWith("pid ")) { // Wil not need it any more delete m_tempFile; - m_tempFile = 0; + m_tempFile = 0; - m_appPid = out.mid(4).toInt(); + m_appPid = out.mid(4).toInt(); m_hInferior = OpenProcess( - SYNCHRONIZE | PROCESS_QUERY_INFORMATION | PROCESS_TERMINATE, - FALSE, m_appPid); + SYNCHRONIZE | PROCESS_QUERY_INFORMATION | PROCESS_TERMINATE, + FALSE, m_appPid); if (m_hInferior == NULL) { emit processError(tr("Cannot obtain a handle to the inferior: %1") .arg(winErrorMessage(GetLastError()))); @@ -213,19 +217,20 @@ void ConsoleProcess::cleanupInferior() inferiorFinishedNotifier = 0; CloseHandle(m_hInferior); m_hInferior = NULL; - m_appPid = 0; + m_appPid = 0; } void ConsoleProcess::inferiorExited() { DWORD chldStatus; - if (!GetExitCodeProcess(m_hInferior, &chldStatus)) + if (!GetExitCodeProcess(m_hInferior, &chldStatus)) { emit processError(tr("Cannot obtain exit status from inferior: %1") .arg(winErrorMessage(GetLastError()))); + } cleanupInferior(); m_appStatus = QProcess::NormalExit; - m_appCode = chldStatus; + m_appCode = chldStatus; emit processStopped(); } @@ -245,16 +250,16 @@ void ConsoleProcess::cleanupStub() void ConsoleProcess::stubExited() { // The stub exit might get noticed before we read the pid for the kill. - if (m_stubSocket && m_stubSocket->state() == QLocalSocket::ConnectedState) + if (m_stubSocket && m_stubSocket->state() == QLocalSocket::ConnectedState) { m_stubSocket->waitForDisconnected(); + } cleanupStub(); if (m_hInferior != NULL) { TerminateProcess(m_hInferior, (unsigned)-1); cleanupInferior(); m_appStatus = QProcess::CrashExit; - m_appCode = -1; + m_appCode = -1; emit processStopped(); } emit wrapperStopped(); } - diff --git a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp index bf5b987e8..4de1f311b 100644 --- a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp +++ b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp @@ -32,138 +32,139 @@ #include #include -#define RAD2DEG (180.0/M_PI) -#define DEG2RAD (M_PI/180.0) +#define RAD2DEG (180.0 / M_PI) +#define DEG2RAD (M_PI / 180.0) namespace Utils { - CoordinateConversions::CoordinateConversions() -{ - -} +{} /** - * Get rotation matrix from ECEF to NED for that LLA - * @param[in] LLA Longitude latitude altitude for this location - * @param[out] Rne[3][3] Rotation matrix - */ -void CoordinateConversions::RneFromLLA(double LLA[3], double Rne[3][3]){ + * Get rotation matrix from ECEF to NED for that LLA + * @param[in] LLA Longitude latitude altitude for this location + * @param[out] Rne[3][3] Rotation matrix + */ +void CoordinateConversions::RneFromLLA(double LLA[3], double Rne[3][3]) +{ float sinLat, sinLon, cosLat, cosLon; - sinLat=(float)sin(DEG2RAD*LLA[0]); - sinLon=(float)sin(DEG2RAD*LLA[1]); - cosLat=(float)cos(DEG2RAD*LLA[0]); - cosLon=(float)cos(DEG2RAD*LLA[1]); + sinLat = (float)sin(DEG2RAD * LLA[0]); + sinLon = (float)sin(DEG2RAD * LLA[1]); + cosLat = (float)cos(DEG2RAD * LLA[0]); + cosLon = (float)cos(DEG2RAD * LLA[1]); - Rne[0][0] = -sinLat*cosLon; Rne[0][1] = -sinLat*sinLon; Rne[0][2] = cosLat; - Rne[1][0] = -sinLon; Rne[1][1] = cosLon; Rne[1][2] = 0; - Rne[2][0] = -cosLat*cosLon; Rne[2][1] = -cosLat*sinLon; Rne[2][2] = -sinLat; + Rne[0][0] = -sinLat * cosLon; Rne[0][1] = -sinLat * sinLon; Rne[0][2] = cosLat; + Rne[1][0] = -sinLon; Rne[1][1] = cosLon; Rne[1][2] = 0; + Rne[2][0] = -cosLat * cosLon; Rne[2][1] = -cosLat * sinLon; Rne[2][2] = -sinLat; } /** - * Convert from LLA coordinates to ECEF coordinates - * @param[in] LLA[3] latitude longitude alititude coordinates in - * @param[out] ECEF[3] location in ECEF coordinates - */ -void CoordinateConversions::LLA2ECEF(double LLA[3], double ECEF[3]){ - const double a = 6378137.0; // Equatorial Radius - const double e = 8.1819190842622e-2; // Eccentricity - double sinLat, sinLon, cosLat, cosLon; - double N; + * Convert from LLA coordinates to ECEF coordinates + * @param[in] LLA[3] latitude longitude alititude coordinates in + * @param[out] ECEF[3] location in ECEF coordinates + */ +void CoordinateConversions::LLA2ECEF(double LLA[3], double ECEF[3]) +{ + const double a = 6378137.0; // Equatorial Radius + const double e = 8.1819190842622e-2; // Eccentricity + double sinLat, sinLon, cosLat, cosLon; + double N; - sinLat=sin(DEG2RAD*LLA[0]); - sinLon=sin(DEG2RAD*LLA[1]); - cosLat=cos(DEG2RAD*LLA[0]); - cosLon=cos(DEG2RAD*LLA[1]); + sinLat = sin(DEG2RAD * LLA[0]); + sinLon = sin(DEG2RAD * LLA[1]); + cosLat = cos(DEG2RAD * LLA[0]); + cosLon = cos(DEG2RAD * LLA[1]); - N = a / sqrt(1.0 - e*e*sinLat*sinLat); //prime vertical radius of curvature + N = a / sqrt(1.0 - e * e * sinLat * sinLat); // prime vertical radius of curvature - ECEF[0] = (N+LLA[2])*cosLat*cosLon; - ECEF[1] = (N+LLA[2])*cosLat*sinLon; - ECEF[2] = ((1-e*e)*N + LLA[2]) * sinLat; + ECEF[0] = (N + LLA[2]) * cosLat * cosLon; + ECEF[1] = (N + LLA[2]) * cosLat * sinLon; + ECEF[2] = ((1 - e * e) * N + LLA[2]) * sinLat; } /** - * Convert from ECEF coordinates to LLA coordinates - * @param[in] ECEF[3] location in ECEF coordinates - * @param[out] LLA[3] latitude longitude alititude coordinates - */ + * Convert from ECEF coordinates to LLA coordinates + * @param[in] ECEF[3] location in ECEF coordinates + * @param[out] LLA[3] latitude longitude alititude coordinates + */ int CoordinateConversions::ECEF2LLA(double ECEF[3], double LLA[3]) { - const double a = 6378137.0; // Equatorial Radius - const double e = 8.1819190842622e-2; // Eccentricity - double x=ECEF[0], y=ECEF[1], z=ECEF[2]; + const double a = 6378137.0; // Equatorial Radius + const double e = 8.1819190842622e-2; // Eccentricity + double x = ECEF[0], y = ECEF[1], z = ECEF[2]; double Lat, N, NplusH, delta, esLat; uint16_t iter; - LLA[1] = RAD2DEG*atan2(y,x); + LLA[1] = RAD2DEG * atan2(y, x); N = a; NplusH = N; - delta = 1; - Lat = 1; - iter=0; + delta = 1; + Lat = 1; + iter = 0; - while (((delta > 1.0e-14)||(delta < -1.0e-14)) && (iter < 100)) - { - delta = Lat - atan(z / (sqrt(x*x + y*y)*(1-(N*e*e/NplusH)))); - Lat = Lat-delta; - esLat = e*sin(Lat); - N = a / sqrt(1 - esLat*esLat); - NplusH = sqrt(x*x + y*y)/cos(Lat); - iter += 1; + while (((delta > 1.0e-14) || (delta < -1.0e-14)) && (iter < 100)) { + delta = Lat - atan(z / (sqrt(x * x + y * y) * (1 - (N * e * e / NplusH)))); + Lat = Lat - delta; + esLat = e * sin(Lat); + N = a / sqrt(1 - esLat * esLat); + NplusH = sqrt(x * x + y * y) / cos(Lat); + iter += 1; } - LLA[0] = RAD2DEG*Lat; + LLA[0] = RAD2DEG * Lat; LLA[2] = NplusH - N; - if (iter==500) return (0); - else return (1); + if (iter == 500) { + return 0; + } else { return 1; } } /** - * Get the current location in Longitude, Latitude Altitude (above WSG-84 ellipsoid) - * @param[in] BaseECEF the ECEF of the home location (in m) - * @param[in] NED the offset from the home location (in m) - * @param[out] position three element double for position in decimal degrees and altitude in meters - * @returns - * @arg 0 success - * @arg -1 for failure - */ + * Get the current location in Longitude, Latitude Altitude (above WSG-84 ellipsoid) + * @param[in] BaseECEF the ECEF of the home location (in m) + * @param[in] NED the offset from the home location (in m) + * @param[out] position three element double for position in decimal degrees and altitude in meters + * @returns + * @arg 0 success + * @arg -1 for failure + */ int CoordinateConversions::NED2LLA_HomeECEF(double BaseECEFm[3], double NED[3], double position[3]) { int i; // stored value is in cm, convert to m double BaseLLA[3]; double ECEF[3]; - double Rne [3][3]; + double Rne[3][3]; // Get LLA address to compute conversion matrix ECEF2LLA(BaseECEFm, BaseLLA); RneFromLLA(BaseLLA, Rne); /* P = ECEF + Rne' * NED */ - for(i = 0; i < 3; i++) - ECEF[i] = BaseECEFm[i] + Rne[0][i]*NED[0] + Rne[1][i]*NED[1] + Rne[2][i]*NED[2]; + for (i = 0; i < 3; i++) { + ECEF[i] = BaseECEFm[i] + Rne[0][i] * NED[0] + Rne[1][i] * NED[1] + Rne[2][i] * NED[2]; + } - ECEF2LLA(ECEF,position); + ECEF2LLA(ECEF, position); return 0; } /** - * Get the current location in Longitude, Latitude, Altitude (above WSG-84 ellipsoid) - * @param[in] homeLLA the latitude, longitude, and altitude of the home location (in [m]) - * @param[in] NED the offset from the home location (in [m]) - * @param[out] position three element double for position in decimal degrees and altitude in meters - * @returns - * @arg 0 success - * @arg -1 for failure - */ + * Get the current location in Longitude, Latitude, Altitude (above WSG-84 ellipsoid) + * @param[in] homeLLA the latitude, longitude, and altitude of the home location (in [m]) + * @param[in] NED the offset from the home location (in [m]) + * @param[out] position three element double for position in decimal degrees and altitude in meters + * @returns + * @arg 0 success + * @arg -1 for failure + */ int CoordinateConversions::NED2LLA_HomeLLA(double homeLLA[3], double NED[3], double position[3]) { double T[3]; - T[0] = homeLLA[2]+6.378137E6f * M_PI / 180.0; - T[1] = cosf(homeLLA[0] * M_PI / 180.0)*(homeLLA[2]+6.378137E6f) * M_PI / 180.0; + + T[0] = homeLLA[2] + 6.378137E6f * M_PI / 180.0; + T[1] = cosf(homeLLA[0] * M_PI / 180.0) * (homeLLA[2] + 6.378137E6f) * M_PI / 180.0; T[2] = -1.0f; position[0] = homeLLA[0] + NED[0] / T[0]; @@ -175,108 +176,105 @@ int CoordinateConversions::NED2LLA_HomeLLA(double homeLLA[3], double NED[3], dou void CoordinateConversions::LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]) { - double ECEF[3]; - float diff[3]; + double ECEF[3]; + float diff[3]; - LLA2ECEF(LLA, ECEF); + LLA2ECEF(LLA, ECEF); - diff[0] = (float)(ECEF[0] - BaseECEF[0]); - diff[1] = (float)(ECEF[1] - BaseECEF[1]); - diff[2] = (float)(ECEF[2] - BaseECEF[2]); + diff[0] = (float)(ECEF[0] - BaseECEF[0]); + diff[1] = (float)(ECEF[1] - BaseECEF[1]); + diff[2] = (float)(ECEF[2] - BaseECEF[2]); - NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; - NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; - NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; + NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; + NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; + NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; } // ****** find roll, pitch, yaw from quaternion ******** void CoordinateConversions::Quaternion2RPY(const float q[4], float rpy[3]) { - float R13, R11, R12, R23, R33; - float q0s = q[0] * q[0]; - float q1s = q[1] * q[1]; - float q2s = q[2] * q[2]; - float q3s = q[3] * q[3]; + float R13, R11, R12, R23, R33; + float q0s = q[0] * q[0]; + float q1s = q[1] * q[1]; + float q2s = q[2] * q[2]; + float q3s = q[3] * q[3]; - R13 = 2 * (q[1] * q[3] - q[0] * q[2]); - R11 = q0s + q1s - q2s - q3s; - R12 = 2 * (q[1] * q[2] + q[0] * q[3]); - R23 = 2 * (q[2] * q[3] + q[0] * q[1]); - R33 = q0s - q1s - q2s + q3s; + R13 = 2 * (q[1] * q[3] - q[0] * q[2]); + R11 = q0s + q1s - q2s - q3s; + R12 = 2 * (q[1] * q[2] + q[0] * q[3]); + R23 = 2 * (q[2] * q[3] + q[0] * q[1]); + R33 = q0s - q1s - q2s + q3s; - rpy[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 - rpy[2] = RAD2DEG * atan2f(R12, R11); - rpy[0] = RAD2DEG * atan2f(R23, R33); + rpy[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 + rpy[2] = RAD2DEG * atan2f(R12, R11); + rpy[0] = RAD2DEG * atan2f(R23, R33); - //TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2 + // TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2 } // ****** find quaternion from roll, pitch, yaw ******** void CoordinateConversions::RPY2Quaternion(const float rpy[3], float q[4]) { - float phi, theta, psi; - float cphi, sphi, ctheta, stheta, cpsi, spsi; + float phi, theta, psi; + float cphi, sphi, ctheta, stheta, cpsi, spsi; - phi = DEG2RAD * rpy[0] / 2; - theta = DEG2RAD * rpy[1] / 2; - psi = DEG2RAD * rpy[2] / 2; - cphi = cosf(phi); - sphi = sinf(phi); - ctheta = cosf(theta); - stheta = sinf(theta); - cpsi = cosf(psi); - spsi = sinf(psi); + phi = DEG2RAD * rpy[0] / 2; + theta = DEG2RAD * rpy[1] / 2; + psi = DEG2RAD * rpy[2] / 2; + cphi = cosf(phi); + sphi = sinf(phi); + ctheta = cosf(theta); + stheta = sinf(theta); + cpsi = cosf(psi); + spsi = sinf(psi); - q[0] = cphi * ctheta * cpsi + sphi * stheta * spsi; - q[1] = sphi * ctheta * cpsi - cphi * stheta * spsi; - q[2] = cphi * stheta * cpsi + sphi * ctheta * spsi; - q[3] = cphi * ctheta * spsi - sphi * stheta * cpsi; + q[0] = cphi * ctheta * cpsi + sphi * stheta * spsi; + q[1] = sphi * ctheta * cpsi - cphi * stheta * spsi; + q[2] = cphi * stheta * cpsi + sphi * ctheta * spsi; + q[3] = cphi * ctheta * spsi - sphi * stheta * cpsi; - if (q[0] < 0) { // q0 always positive for uniqueness - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; - } + if (q[0] < 0) { // q0 always positive for uniqueness + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } } -//** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** void CoordinateConversions::Quaternion2R(const float q[4], float Rbe[3][3]) { + float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3]; - float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3]; - - Rbe[0][0] = q0s + q1s - q2s - q3s; - Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]); - Rbe[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]); - Rbe[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]); - Rbe[1][1] = q0s - q1s + q2s - q3s; - Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]); - Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]); - Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]); - Rbe[2][2] = q0s - q1s - q2s + q3s; + Rbe[0][0] = q0s + q1s - q2s - q3s; + Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]); + Rbe[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]); + Rbe[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]); + Rbe[1][1] = q0s - q1s + q2s - q3s; + Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]); + Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]); + Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]); + Rbe[2][2] = q0s - q1s - q2s + q3s; } -//** Find quaternion vector from a rotation matrix, Rbe, a matrix which rotates a vector from earth frame to body frame ** +// ** Find quaternion vector from a rotation matrix, Rbe, a matrix which rotates a vector from earth frame to body frame ** void CoordinateConversions::R2Quaternion(float const Rbe[3][3], float q[4]) { qreal w, x, y, z; // w always >= 0 - w = sqrt(std::max(0.0, 1.0 + Rbe[0][0] + Rbe[1][1] + Rbe[2][2])) / 2.0; - x = sqrt(std::max(0.0, 1.0 + Rbe[0][0] - Rbe[1][1] - Rbe[2][2])) / 2.0; - y = sqrt(std::max(0.0, 1.0 - Rbe[0][0] + Rbe[1][1] - Rbe[2][2])) / 2.0; - z = sqrt(std::max(0.0, 1.0 - Rbe[0][0] - Rbe[1][1] + Rbe[2][2])) / 2.0; + w = sqrt(std::max(0.0, 1.0 + Rbe[0][0] + Rbe[1][1] + Rbe[2][2])) / 2.0; + x = sqrt(std::max(0.0, 1.0 + Rbe[0][0] - Rbe[1][1] - Rbe[2][2])) / 2.0; + y = sqrt(std::max(0.0, 1.0 - Rbe[0][0] + Rbe[1][1] - Rbe[2][2])) / 2.0; + z = sqrt(std::max(0.0, 1.0 - Rbe[0][0] - Rbe[1][1] + Rbe[2][2])) / 2.0; - x = copysign(x, (Rbe[1][2] - Rbe[2][1])); - y = copysign(y, (Rbe[2][0] - Rbe[0][2])); - z = copysign(z, (Rbe[0][1] - Rbe[1][0])); + x = copysign(x, (Rbe[1][2] - Rbe[2][1])); + y = copysign(y, (Rbe[2][0] - Rbe[0][2])); + z = copysign(z, (Rbe[0][1] - Rbe[1][0])); - q[0]=w; - q[1]=x; - q[2]=y; - q[3]=z; + q[0] = w; + q[1] = x; + q[2] = y; + q[3] = z; } - - } diff --git a/ground/openpilotgcs/src/libs/utils/coordinateconversions.h b/ground/openpilotgcs/src/libs/utils/coordinateconversions.h index 5f515c147..de6ac7568 100644 --- a/ground/openpilotgcs/src/libs/utils/coordinateconversions.h +++ b/ground/openpilotgcs/src/libs/utils/coordinateconversions.h @@ -36,9 +36,7 @@ #include "math.h" namespace Utils { - -class QTCREATOR_UTILS_EXPORT CoordinateConversions -{ +class QTCREATOR_UTILS_EXPORT CoordinateConversions { public: CoordinateConversions(); int NED2LLA_HomeECEF(double BaseECEFcm[3], double NED[3], double position[3]); @@ -52,7 +50,6 @@ public: void Quaternion2R(const float q[4], float Rbe[3][3]); void R2Quaternion(float const Rbe[3][3], float q[4]); }; - } #endif /* COORDINATECONVERSIONS_H */ diff --git a/ground/openpilotgcs/src/libs/utils/detailsbutton.cpp b/ground/openpilotgcs/src/libs/utils/detailsbutton.cpp index 0424e2bb0..edccfcb8f 100644 --- a/ground/openpilotgcs/src/libs/utils/detailsbutton.cpp +++ b/ground/openpilotgcs/src/libs/utils/detailsbutton.cpp @@ -4,25 +4,25 @@ * @file detailsbutton.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/libs/utils/detailsbutton.h b/ground/openpilotgcs/src/libs/utils/detailsbutton.h index c4b5d9bd9..efe36a8d3 100644 --- a/ground/openpilotgcs/src/libs/utils/detailsbutton.h +++ b/ground/openpilotgcs/src/libs/utils/detailsbutton.h @@ -4,25 +4,25 @@ * @file detailsbutton.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,7 +35,6 @@ #include "utils_global.h" namespace Utils { - class QTCREATOR_UTILS_EXPORT DetailsButton #ifdef Q_OS_MAC : public QPushButton @@ -45,7 +44,7 @@ class QTCREATOR_UTILS_EXPORT DetailsButton { Q_OBJECT public: - DetailsButton(QWidget *parent=0); + DetailsButton(QWidget *parent = 0); bool isToggled(); public slots: void onClicked(); diff --git a/ground/openpilotgcs/src/libs/utils/detailswidget.cpp b/ground/openpilotgcs/src/libs/utils/detailswidget.cpp index 0f8a4ad18..2de40e972 100644 --- a/ground/openpilotgcs/src/libs/utils/detailswidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/detailswidget.cpp @@ -4,25 +4,25 @@ * @file detailswidget.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,12 +39,11 @@ using namespace Utils; DetailsWidget::DetailsWidget(QWidget *parent) : QWidget(parent), - m_summaryLabel(new QLabel(this)), - m_detailsButton(new DetailsButton(this)), - m_widget(0), - m_toolWidget(0), - m_grid(new QGridLayout(this)) - + m_summaryLabel(new QLabel(this)), + m_detailsButton(new DetailsButton(this)), + m_widget(0), + m_toolWidget(0), + m_grid(new QGridLayout(this)) { m_grid->setContentsMargins(4, 3, 4, 3); @@ -65,26 +64,25 @@ DetailsWidget::DetailsWidget(QWidget *parent) } DetailsWidget::~DetailsWidget() -{ - -} +{} void DetailsWidget::paintEvent(QPaintEvent *paintEvent) { - //TL--> ___________ <-- TR - // | | - //ML-> ______________| <--MM | <--MR - // | | - //BL-> |_________________________| <-- BR + // TL--> ___________ <-- TR + // | | + // ML-> ______________| <--MM | <--MR + // | | + // BL-> |_________________________| <-- BR QWidget::paintEvent(paintEvent); - if (!m_detailsButton->isToggled()) + if (!m_detailsButton->isToggled()) { return; + } const QRect detailsGeometry = m_detailsButton->geometry(); - const QRect widgetGeometry = m_widget ? m_widget->geometry() : QRect(x(), y() + height(), width(), 0); + const QRect widgetGeometry = m_widget ? m_widget->geometry() : QRect(x(), y() + height(), width(), 0); QPoint tl(detailsGeometry.topLeft()); tl += QPoint(-3, -3); @@ -114,8 +112,10 @@ void DetailsWidget::paintEvent(QPaintEvent *paintEvent) void DetailsWidget::detailsButtonClicked() { bool visible = m_detailsButton->isToggled(); - if (m_widget) + + if (m_widget) { m_widget->setVisible(visible); + } m_dummyWidget->setVisible(visible); fixUpLayout(); } @@ -137,8 +137,9 @@ bool DetailsWidget::expanded() const void DetailsWidget::setExpanded(bool v) { - if (expanded() != v) + if (expanded() != v) { m_detailsButton->animateClick(); + } } QWidget *DetailsWidget::widget() const @@ -148,8 +149,9 @@ QWidget *DetailsWidget::widget() const void DetailsWidget::setWidget(QWidget *widget) { - if (m_widget == widget) + if (m_widget == widget) { return; + } if (m_widget) { m_grid->removeWidget(m_widget); m_widget = 0; @@ -165,8 +167,9 @@ void DetailsWidget::setWidget(QWidget *widget) void DetailsWidget::setToolWidget(QWidget *widget) { - if (m_toolWidget == widget) + if (m_toolWidget == widget) { return; + } if (m_toolWidget) { m_grid->removeWidget(m_toolWidget); m_toolWidget = 0; @@ -184,16 +187,17 @@ QWidget *DetailsWidget::toolWidget() const void DetailsWidget::fixUpLayout() { - if (!m_widget) + if (!m_widget) { return; + } QWidget *parent = m_widget; QStack widgets; - while((parent = parent->parentWidget()) && parent && parent->layout()) { + while ((parent = parent->parentWidget()) && parent && parent->layout()) { widgets.push(parent); parent->layout()->update(); } - while(!widgets.isEmpty()) { + while (!widgets.isEmpty()) { widgets.pop()->layout()->activate(); } } diff --git a/ground/openpilotgcs/src/libs/utils/detailswidget.h b/ground/openpilotgcs/src/libs/utils/detailswidget.h index c5d6345e1..71ea2f23e 100644 --- a/ground/openpilotgcs/src/libs/utils/detailswidget.h +++ b/ground/openpilotgcs/src/libs/utils/detailswidget.h @@ -4,25 +4,25 @@ * @file detailswidget.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,10 +41,8 @@ QT_END_NAMESPACE namespace Utils { class DetailsButton; -class QTCREATOR_UTILS_EXPORT DetailsWidget : public QWidget -{ - Q_OBJECT - Q_PROPERTY(QString summaryText READ summaryText WRITE setSummaryText DESIGNABLE true) +class QTCREATOR_UTILS_EXPORT DetailsWidget : public QWidget { + Q_OBJECT Q_PROPERTY(QString summaryText READ summaryText WRITE setSummaryText DESIGNABLE true) Q_PROPERTY(bool expanded READ expanded WRITE setExpanded DESIGNABLE true) public: DetailsWidget(QWidget *parent = 0); diff --git a/ground/openpilotgcs/src/libs/utils/fancylineedit.cpp b/ground/openpilotgcs/src/libs/utils/fancylineedit.cpp index 83d92fa88..a0c7f40c6 100644 --- a/ground/openpilotgcs/src/libs/utils/fancylineedit.cpp +++ b/ground/openpilotgcs/src/libs/utils/fancylineedit.cpp @@ -4,25 +4,25 @@ * @file fancylineedit.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,7 +39,6 @@ enum { margin = 6 }; namespace Utils { - static inline QString sideToStyleSheetString(FancyLineEdit::Side side) { return side == FancyLineEdit::Left ? QLatin1String("left") : QLatin1String("right"); @@ -50,6 +49,7 @@ static inline QString sideToStyleSheetString(FancyLineEdit::Side side) static QString labelStyleSheet(FancyLineEdit::Side side) { QString rc = QLatin1String("QLabel { margin-"); + rc += sideToStyleSheetString(side); rc += QLatin1String(": "); rc += QString::number(margin); @@ -58,7 +58,7 @@ static QString labelStyleSheet(FancyLineEdit::Side side) } // --------- FancyLineEditPrivate as QObject with label -// event filter +// event filter class FancyLineEditPrivate : public QObject { public: @@ -92,16 +92,17 @@ FancyLineEditPrivate::FancyLineEditPrivate(QLineEdit *parent) : m_useLayoutDirection(false), m_menuTabFocusTrigger(false), m_showingHintText(false) -{ -} +{} bool FancyLineEditPrivate::eventFilter(QObject *obj, QEvent *event) { - if (!m_menu || obj != m_menuLabel) + if (!m_menu || obj != m_menuLabel) { return QObject::eventFilter(obj, event); + } switch (event->type()) { - case QEvent::MouseButtonPress: { + case QEvent::MouseButtonPress: + { const QMouseEvent *me = static_cast(event); m_menu->exec(me->globalPos()); return true; @@ -130,8 +131,7 @@ FancyLineEdit::FancyLineEdit(QWidget *parent) : } FancyLineEdit::~FancyLineEdit() -{ -} +{} // Position the menu label left or right according to size. // Called when switching side and from resizeEvent. @@ -139,11 +139,11 @@ void FancyLineEdit::positionMenuLabel() { switch (side()) { case Left: - m_d->m_menuLabel->setGeometry(0, 0, m_d->m_pixmap.width()+margin, height()); + m_d->m_menuLabel->setGeometry(0, 0, m_d->m_pixmap.width() + margin, height()); break; case Right: m_d->m_menuLabel->setGeometry(width() - m_d->m_pixmap.width() - margin, 0, - m_d->m_pixmap.width()+margin, height()); + m_d->m_pixmap.width() + margin, height()); break; } } @@ -154,12 +154,14 @@ void FancyLineEdit::updateStyleSheet(Side side) // respective side and set color according to whether we are showing the // hint text QString sheet = QLatin1String("QLineEdit{ padding-"); + sheet += sideToStyleSheetString(side); sheet += QLatin1String(": "); sheet += QString::number(m_d->m_pixmap.width() + margin); sheet += QLatin1Char(';'); - if (m_d->m_showingHintText) + if (m_d->m_showingHintText) { sheet += QLatin1String(" color: #BBBBBB;"); + } sheet += QLatin1Char('}'); setStyleSheet(sheet); } @@ -190,9 +192,10 @@ void FancyLineEdit::setSide(Side side) FancyLineEdit::Side FancyLineEdit::side() const { - if (m_d->m_useLayoutDirection) + if (m_d->m_useLayoutDirection) { return qApp->layoutDirection() == Qt::LeftToRight ? Left : Right; - return m_d->m_side; + } + return m_d->m_side; } void FancyLineEdit::resizeEvent(QResizeEvent *) @@ -213,12 +216,12 @@ QPixmap FancyLineEdit::pixmap() const void FancyLineEdit::setMenu(QMenu *menu) { - m_d->m_menu = menu; + m_d->m_menu = menu; } QMenu *FancyLineEdit::menu() const { - return m_d->m_menu; + return m_d->m_menu; } bool FancyLineEdit::useLayoutDirection() const @@ -243,8 +246,9 @@ bool FancyLineEdit::hasMenuTabFocusTrigger() const void FancyLineEdit::setMenuTabFocusTrigger(bool v) { - if (m_d->m_menuTabFocusTrigger == v) + if (m_d->m_menuTabFocusTrigger == v) { return; + } m_d->m_menuTabFocusTrigger = v; m_d->m_menuLabel->setFocusPolicy(v ? Qt::TabFocus : Qt::NoFocus); @@ -258,12 +262,14 @@ QString FancyLineEdit::hintText() const void FancyLineEdit::setHintText(const QString &ht) { // Updating magic to make the property work in Designer. - if (ht == m_d->m_hintText) + if (ht == m_d->m_hintText) { return; + } hideHintText(); m_d->m_hintText = ht; - if (!hasFocus() && !ht.isEmpty()) + if (!hasFocus() && !ht.isEmpty()) { showHintText(); + } } void FancyLineEdit::showHintText() @@ -307,5 +313,4 @@ QString FancyLineEdit::typedText() const { return m_d->m_showingHintText ? QString() : text(); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/fancylineedit.h b/ground/openpilotgcs/src/libs/utils/fancylineedit.h index f25c8507e..09eceabe7 100644 --- a/ground/openpilotgcs/src/libs/utils/fancylineedit.h +++ b/ground/openpilotgcs/src/libs/utils/fancylineedit.h @@ -4,25 +4,25 @@ * @file fancylineedit.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,6 @@ #include namespace Utils { - class FancyLineEditPrivate; /* A line edit with an embedded pixmap on one side that is connected to @@ -44,19 +43,17 @@ class FancyLineEditPrivate; * text if isShowingHintText() returns true (that is, does not contain * valid user input). */ -class QTCREATOR_UTILS_EXPORT FancyLineEdit : public QLineEdit -{ +class QTCREATOR_UTILS_EXPORT FancyLineEdit : public QLineEdit { Q_DISABLE_COPY(FancyLineEdit) - Q_OBJECT - Q_ENUMS(Side) + Q_OBJECT Q_ENUMS(Side) Q_PROPERTY(QPixmap pixmap READ pixmap WRITE setPixmap DESIGNABLE true) Q_PROPERTY(Side side READ side WRITE setSide DESIGNABLE isSideStored STORED isSideStored) Q_PROPERTY(bool useLayoutDirection READ useLayoutDirection WRITE setUseLayoutDirection DESIGNABLE true) - Q_PROPERTY(bool menuTabFocusTrigger READ hasMenuTabFocusTrigger WRITE setMenuTabFocusTrigger DESIGNABLE true) + Q_PROPERTY(bool menuTabFocusTrigger READ hasMenuTabFocusTrigger WRITE setMenuTabFocusTrigger DESIGNABLE true) Q_PROPERTY(QString hintText READ hintText WRITE setHintText DESIGNABLE true) public: - enum Side {Left, Right}; + enum Side { Left, Right }; explicit FancyLineEdit(QWidget *parent = 0); ~FancyLineEdit(); @@ -103,7 +100,6 @@ private: FancyLineEditPrivate *m_d; }; - } // namespace Utils #endif // FANCYLINEEDIT_H diff --git a/ground/openpilotgcs/src/libs/utils/fancymainwindow.cpp b/ground/openpilotgcs/src/libs/utils/fancymainwindow.cpp index 870904334..ea03c92c8 100644 --- a/ground/openpilotgcs/src/libs/utils/fancymainwindow.cpp +++ b/ground/openpilotgcs/src/libs/utils/fancymainwindow.cpp @@ -4,25 +4,25 @@ * @file fancymainwindow.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,16 +40,16 @@ FancyMainWindow::FancyMainWindow(QWidget *parent) : QMainWindow(parent), m_locked(true), m_handleDockVisibilityChanges(true) -{ -} +{} QDockWidget *FancyMainWindow::addDockForWidget(QWidget *widget) { QDockWidget *dockWidget = new QDockWidget(widget->windowTitle(), this); + dockWidget->setObjectName(widget->windowTitle()); dockWidget->setWidget(widget); connect(dockWidget->toggleViewAction(), SIGNAL(triggered()), - this, SLOT(onDockActionTriggered()), Qt::QueuedConnection); + this, SLOT(onDockActionTriggered()), Qt::QueuedConnection); connect(dockWidget, SIGNAL(visibilityChanged(bool)), this, SLOT(onDockVisibilityChange(bool))); connect(dockWidget, SIGNAL(topLevelChanged(bool)), @@ -63,12 +63,13 @@ QDockWidget *FancyMainWindow::addDockForWidget(QWidget *widget) void FancyMainWindow::updateDockWidget(QDockWidget *dockWidget) { const QDockWidget::DockWidgetFeatures features = - (m_locked) ? QDockWidget::DockWidgetClosable | QDockWidget::DockWidgetFloatable - : QDockWidget::DockWidgetMovable | QDockWidget::DockWidgetClosable | QDockWidget::DockWidgetFloatable; + (m_locked) ? QDockWidget::DockWidgetClosable | QDockWidget::DockWidgetFloatable + : QDockWidget::DockWidgetMovable | QDockWidget::DockWidgetClosable | QDockWidget::DockWidgetFloatable; QWidget *titleBarWidget = dockWidget->titleBarWidget(); - if (m_locked && !titleBarWidget && !dockWidget->isFloating()) + + if (m_locked && !titleBarWidget && !dockWidget->isFloating()) { titleBarWidget = new QWidget(dockWidget); - else if ((!m_locked || dockWidget->isFloating()) && titleBarWidget) { + } else if ((!m_locked || dockWidget->isFloating()) && titleBarWidget) { delete titleBarWidget; titleBarWidget = 0; } @@ -79,16 +80,19 @@ void FancyMainWindow::updateDockWidget(QDockWidget *dockWidget) void FancyMainWindow::onDockActionTriggered() { QDockWidget *dw = qobject_cast(sender()->parent()); + if (dw) { - if (dw->isVisible()) + if (dw->isVisible()) { dw->raise(); + } } } void FancyMainWindow::onDockVisibilityChange(bool visible) { - if (!m_handleDockVisibilityChanges) + if (!m_handleDockVisibilityChanges) { return; + } QDockWidget *dockWidget = qobject_cast(sender()); int index = m_dockWidgets.indexOf(dockWidget); m_dockWidgetActiveState[index] = visible; @@ -96,15 +100,16 @@ void FancyMainWindow::onDockVisibilityChange(bool visible) void FancyMainWindow::onTopLevelChanged() { - updateDockWidget(qobject_cast(sender())); + updateDockWidget(qobject_cast(sender())); } void FancyMainWindow::setTrackingEnabled(bool enabled) { if (enabled) { m_handleDockVisibilityChanges = true; - for (int i = 0; i < m_dockWidgets.size(); ++i) + for (int i = 0; i < m_dockWidgets.size(); ++i) { m_dockWidgetActiveState[i] = m_dockWidgets[i]->isVisible(); + } } else { m_handleDockVisibilityChanges = false; } @@ -113,7 +118,7 @@ void FancyMainWindow::setTrackingEnabled(bool enabled) void FancyMainWindow::setLocked(bool locked) { m_locked = locked; - foreach (QDockWidget *dockWidget, m_dockWidgets) { + foreach(QDockWidget * dockWidget, m_dockWidgets) { updateDockWidget(dockWidget); } } @@ -139,8 +144,9 @@ void FancyMainWindow::handleVisibilityChanged(bool visible) dockWidget->setVisible(visible && m_dockWidgetActiveState.at(i)); } } - if (visible) + if (visible) { m_handleDockVisibilityChanges = true; + } } void FancyMainWindow::saveSettings(QSettings *settings) const @@ -156,7 +162,7 @@ void FancyMainWindow::saveSettings(QSettings *settings) const void FancyMainWindow::restoreSettings(QSettings *settings) { QHash hash; - foreach (const QString &key, settings->childKeys()) { + foreach(const QString &key, settings->childKeys()) { hash.insert(key, settings->value(key)); } restoreSettings(hash); @@ -165,11 +171,11 @@ void FancyMainWindow::restoreSettings(QSettings *settings) QHash FancyMainWindow::saveSettings() const { QHash settings; - settings["State"] = saveState(); + settings["State"] = saveState(); settings["Locked"] = m_locked; for (int i = 0; i < m_dockWidgetActiveState.count(); ++i) { settings[m_dockWidgets.at(i)->objectName()] = - m_dockWidgetActiveState.at(i); + m_dockWidgetActiveState.at(i); } return settings; } @@ -177,8 +183,10 @@ QHash FancyMainWindow::saveSettings() const void FancyMainWindow::restoreSettings(const QHash &settings) { QByteArray ba = settings.value("State", QByteArray()).toByteArray(); - if (!ba.isEmpty()) + + if (!ba.isEmpty()) { restoreState(ba); + } m_locked = settings.value("Locked", true).toBool(); for (int i = 0; i < m_dockWidgetActiveState.count(); ++i) { m_dockWidgetActiveState[i] = settings.value(m_dockWidgets.at(i)->objectName(), false).toBool(); diff --git a/ground/openpilotgcs/src/libs/utils/fancymainwindow.h b/ground/openpilotgcs/src/libs/utils/fancymainwindow.h index fb8e03e33..0d2abafeb 100644 --- a/ground/openpilotgcs/src/libs/utils/fancymainwindow.h +++ b/ground/openpilotgcs/src/libs/utils/fancymainwindow.h @@ -4,25 +4,25 @@ * @file fancymainwindow.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,19 +41,23 @@ class QSettings; QT_END_NAMESPACE namespace Utils { - -class QTCREATOR_UTILS_EXPORT FancyMainWindow : public QMainWindow -{ +class QTCREATOR_UTILS_EXPORT FancyMainWindow : public QMainWindow { Q_OBJECT public: FancyMainWindow(QWidget *parent = 0); QDockWidget *addDockForWidget(QWidget *widget); - QList dockWidgets() const { return m_dockWidgets; } + QList dockWidgets() const + { + return m_dockWidgets; + } void setTrackingEnabled(bool enabled); - bool isLocked() const { return m_locked; } + bool isLocked() const + { + return m_locked; + } void saveSettings(QSettings *settings) const; void restoreSettings(QSettings *settings); @@ -79,9 +83,8 @@ private: QList m_dockWidgets; QList m_dockWidgetActiveState; bool m_locked; - bool m_handleDockVisibilityChanges; //todo + bool m_handleDockVisibilityChanges; // todo }; - } // namespace Utils #endif // FANCYMAINWINDOW_H diff --git a/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.cpp b/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.cpp index 2111f0afe..3f53447f4 100644 --- a/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.cpp +++ b/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.cpp @@ -4,25 +4,25 @@ * @file filenamevalidatinglineedit.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,16 +33,16 @@ #include namespace Utils { - #define WINDOWS_DEVICES "CON|AUX|PRN|COM1|COM2|LPT1|LPT2|NUL" // Naming a file like a device name will break on Windows, even if it is // "com1.txt". Since we are cross-platform, we generally disallow such file -// names. +// names. static const QRegExp &windowsDeviceNoSubDirPattern() { static const QRegExp rc(QLatin1String(WINDOWS_DEVICES), - Qt::CaseInsensitive); + Qt::CaseInsensitive); + QTC_ASSERT(rc.isValid(), return rc); return rc; } @@ -50,6 +50,7 @@ static const QRegExp &windowsDeviceNoSubDirPattern() static const QRegExp &windowsDeviceSubDirPattern() { static const QRegExp rc(QLatin1String(".*[/\\\\](" WINDOWS_DEVICES ")"), Qt::CaseInsensitive); + QTC_ASSERT(rc.isValid(), return rc); return rc; } @@ -59,8 +60,7 @@ FileNameValidatingLineEdit::FileNameValidatingLineEdit(QWidget *parent) : BaseValidatingLineEdit(parent), m_allowDirectories(false), m_unused(0) -{ -} +{} bool FileNameValidatingLineEdit::allowDirectories() const { @@ -83,51 +83,56 @@ void FileNameValidatingLineEdit::setAllowDirectories(bool v) static const char *notAllowedCharsSubDir = "?:&*\"|#%<> "; static const char *notAllowedCharsNoSubDir = "?:&*\"|#%<> "SLASHES; -static const char *notAllowedSubStrings[] = {".."}; +static const char *notAllowedSubStrings[] = { ".." }; bool FileNameValidatingLineEdit::validateFileName(const QString &name, bool allowDirectories, QString *errorMessage /* = 0*/) { if (name.isEmpty()) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The name must not be empty"); + } return false; } // Characters const char *notAllowedChars = allowDirectories ? notAllowedCharsSubDir : notAllowedCharsNoSubDir; - for (const char *c = notAllowedChars; *c; c++) + for (const char *c = notAllowedChars; *c; c++) { if (name.contains(QLatin1Char(*c))) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The name must not contain any of the characters '%1'.").arg(QLatin1String(notAllowedChars)); + } return false; } + } // Substrings - const int notAllowedSubStringCount = sizeof(notAllowedSubStrings)/sizeof(const char *); + const int notAllowedSubStringCount = sizeof(notAllowedSubStrings) / sizeof(const char *); for (int s = 0; s < notAllowedSubStringCount; s++) { const QLatin1String notAllowedSubString(notAllowedSubStrings[s]); if (name.contains(notAllowedSubString)) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The name must not contain '%1'.").arg(QString(notAllowedSubString)); + } return false; } } // Windows devices bool matchesWinDevice = windowsDeviceNoSubDirPattern().exactMatch(name); - if (!matchesWinDevice && allowDirectories) + if (!matchesWinDevice && allowDirectories) { matchesWinDevice = windowsDeviceSubDirPattern().exactMatch(name); + } if (matchesWinDevice) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The name must not match that of a MS Windows device. (%1)."). arg(windowsDeviceNoSubDirPattern().pattern().replace(QLatin1Char('|'), QLatin1Char(','))); + } return false; } return true; } -bool FileNameValidatingLineEdit::validate(const QString &value, QString *errorMessage) const +bool FileNameValidatingLineEdit::validate(const QString &value, QString *errorMessage) const { return validateFileName(value, m_allowDirectories, errorMessage); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.h b/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.h index 7036ac332..81e0436d7 100644 --- a/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.h +++ b/ground/openpilotgcs/src/libs/utils/filenamevalidatinglineedit.h @@ -4,25 +4,25 @@ * @file filenamevalidatinglineedit.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,15 +32,12 @@ #include "basevalidatinglineedit.h" namespace Utils { - /** * A control that let's the user choose a file name, based on a QLineEdit. Has * some validation logic for embedding into QWizardPage. */ -class QTCREATOR_UTILS_EXPORT FileNameValidatingLineEdit : public BaseValidatingLineEdit -{ - Q_OBJECT - Q_DISABLE_COPY(FileNameValidatingLineEdit) +class QTCREATOR_UTILS_EXPORT FileNameValidatingLineEdit : public BaseValidatingLineEdit { + Q_OBJECT Q_DISABLE_COPY(FileNameValidatingLineEdit) Q_PROPERTY(bool allowDirectories READ allowDirectories WRITE setAllowDirectories) public: explicit FileNameValidatingLineEdit(QWidget *parent = 0); @@ -63,7 +60,6 @@ private: bool m_allowDirectories; void *m_unused; }; - } // namespace Utils #endif // FILENAMEVALIDATINGLINEEDIT_H diff --git a/ground/openpilotgcs/src/libs/utils/filesearch.cpp b/ground/openpilotgcs/src/libs/utils/filesearch.cpp index b2d5a647e..c728aeb78 100644 --- a/ground/openpilotgcs/src/libs/utils/filesearch.cpp +++ b/ground/openpilotgcs/src/libs/utils/filesearch.cpp @@ -4,25 +4,25 @@ * @file filesearch.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -46,7 +46,7 @@ static inline QString msgCanceled(const QString &searchTerm, int numMatches, int return QCoreApplication::translate("Utils::FileSearch", "%1: canceled. %n occurrences found in %2 files.", 0, QCoreApplication::CodecForTr, numMatches). - arg(searchTerm).arg(numFilesSearched); + arg(searchTerm).arg(numFilesSearched); } static inline QString msgFound(const QString &searchTerm, int numMatches, int numFilesSearched) @@ -54,7 +54,7 @@ static inline QString msgFound(const QString &searchTerm, int numMatches, int nu return QCoreApplication::translate("Utils::FileSearch", "%1: %n occurrences found in %2 files.", 0, QCoreApplication::CodecForTr, numMatches). - arg(searchTerm).arg(numFilesSearched); + arg(searchTerm).arg(numFilesSearched); } static inline QString msgFound(const QString &searchTerm, int numMatches, int numFilesSearched, int filesSize) @@ -62,11 +62,10 @@ static inline QString msgFound(const QString &searchTerm, int numMatches, int nu return QCoreApplication::translate("Utils::FileSearch", "%1: %n occurrences found in %2 of %3 files.", 0, QCoreApplication::CodecForTr, numMatches). - arg(searchTerm).arg(numFilesSearched).arg(filesSize); + arg(searchTerm).arg(numFilesSearched).arg(filesSize); } namespace { - void runFileSearch(QFutureInterface &future, QString searchTerm, QStringList files, @@ -78,25 +77,26 @@ void runFileSearch(QFutureInterface &future, int numMatches = 0; bool caseInsensitive = !(flags & QTextDocument::FindCaseSensitively); - bool wholeWord = (flags & QTextDocument::FindWholeWords); + bool wholeWord = (flags & QTextDocument::FindWholeWords); - QByteArray sa = searchTerm.toUtf8(); - int scMaxIndex = sa.length()-1; - const char *sc = sa.constData(); + QByteArray sa = searchTerm.toUtf8(); + int scMaxIndex = sa.length() - 1; + const char *sc = sa.constData(); - QByteArray sal = searchTerm.toLower().toUtf8(); + QByteArray sal = searchTerm.toLower().toUtf8(); const char *scl = sal.constData(); - QByteArray sau = searchTerm.toUpper().toUtf8(); + QByteArray sau = searchTerm.toUpper().toUtf8(); const char *scu = sau.constData(); - int chunkSize = qMax(100000, sa.length()); + int chunkSize = qMax(100000, sa.length()); QFile file; QBuffer buffer; - foreach (QString s, files) { - if (future.isPaused()) + foreach(QString s, files) { + if (future.isPaused()) { future.waitForResume(); + } if (future.isCanceled()) { future.setProgressValueAndText(numFilesSearched, msgCanceled(searchTerm, numMatches, numFilesSearched)); break; @@ -109,54 +109,55 @@ void runFileSearch(QFutureInterface &future, file.setFileName(s); device = &file; } - if (!device->open(QIODevice::ReadOnly)) + if (!device->open(QIODevice::ReadOnly)) { continue; + } int lineNr = 1; const char *startOfLastLine = NULL; bool firstChunk = true; while (!device->atEnd()) { - if (!firstChunk) - device->seek(device->pos()-sa.length()+1); + if (!firstChunk) { + device->seek(device->pos() - sa.length() + 1); + } const QByteArray chunk = device->read(chunkSize); - const char *chunkPtr = chunk.constData(); + const char *chunkPtr = chunk.constData(); startOfLastLine = chunkPtr; - for (const char *regionPtr = chunkPtr; regionPtr < chunkPtr + chunk.length()-scMaxIndex; ++regionPtr) { + for (const char *regionPtr = chunkPtr; regionPtr < chunkPtr + chunk.length() - scMaxIndex; ++regionPtr) { const char *regionEnd = regionPtr + scMaxIndex; if (*regionPtr == '\n') { startOfLastLine = regionPtr + 1; ++lineNr; - } - else if ( - // case sensitive - (!caseInsensitive && *regionPtr == sc[0] && *regionEnd == sc[scMaxIndex]) - || - // case insensitive - (caseInsensitive && (*regionPtr == scl[0] || *regionPtr == scu[0]) - && (*regionEnd == scl[scMaxIndex] || *regionEnd == scu[scMaxIndex])) - ) { - const char *afterRegion = regionEnd + 1; + } else if ( + // case sensitive + (!caseInsensitive && *regionPtr == sc[0] && *regionEnd == sc[scMaxIndex]) + || + // case insensitive + (caseInsensitive && (*regionPtr == scl[0] || *regionPtr == scu[0]) + && (*regionEnd == scl[scMaxIndex] || *regionEnd == scu[scMaxIndex])) + ) { + const char *afterRegion = regionEnd + 1; const char *beforeRegion = regionPtr - 1; bool equal = true; if (wholeWord && - ( isalnum(*beforeRegion) - || (*beforeRegion == '_') - || isalnum(*afterRegion) - || (*afterRegion == '_'))) { + (isalnum(*beforeRegion) + || (*beforeRegion == '_') + || isalnum(*afterRegion) + || (*afterRegion == '_'))) { equal = false; } int regionIndex = 1; for (const char *regionCursor = regionPtr + 1; regionCursor < regionEnd; ++regionCursor, ++regionIndex) { - if ( // case sensitive - (!caseInsensitive && equal && *regionCursor != sc[regionIndex]) - || - // case insensitive - (caseInsensitive && equal && *regionCursor != sc[regionIndex] && *regionCursor != scl[regionIndex] && *regionCursor != scu[regionIndex]) - ) { - equal = false; + if ( // case sensitive + (!caseInsensitive && equal && *regionCursor != sc[regionIndex]) + || + // case insensitive + (caseInsensitive && equal && *regionCursor != sc[regionIndex] && *regionCursor != scl[regionIndex] && *regionCursor != scu[regionIndex]) + ) { + equal = false; } } if (equal) { @@ -166,10 +167,11 @@ void runFileSearch(QFutureInterface &future, res.reserve(256); int i = 0; int n = 0; - while (startOfLastLine[i] != '\n' && startOfLastLine[i] != '\r' && i < textLength && n++ < 256) + while (startOfLastLine[i] != '\n' && startOfLastLine[i] != '\r' && i < textLength && n++ < 256) { res.append(startOfLastLine[i++]); + } future.reportResult(FileSearchResult(s, lineNr, QString(res), - regionPtr - startOfLastLine, sa.length())); + regionPtr - startOfLastLine, sa.length())); ++numMatches; } } @@ -181,30 +183,33 @@ void runFileSearch(QFutureInterface &future, future.setProgressValueAndText(numFilesSearched, msgFound(searchTerm, numMatches, numFilesSearched, files.size())); device->close(); } - if (!future.isCanceled()) + if (!future.isCanceled()) { future.setProgressValueAndText(numFilesSearched, msgFound(searchTerm, numMatches, numFilesSearched)); + } } void runFileSearchRegExp(QFutureInterface &future, - QString searchTerm, - QStringList files, - QTextDocument::FindFlags flags, - QMap fileToContentsMap) + QString searchTerm, + QStringList files, + QTextDocument::FindFlags flags, + QMap fileToContentsMap) { future.setProgressRange(0, files.size()); int numFilesSearched = 0; int numMatches = 0; - if (flags & QTextDocument::FindWholeWords) + if (flags & QTextDocument::FindWholeWords) { searchTerm = QString::fromLatin1("\\b%1\\b").arg(searchTerm); + } const Qt::CaseSensitivity caseSensitivity = (flags & QTextDocument::FindCaseSensitively) ? Qt::CaseSensitive : Qt::CaseInsensitive; const QRegExp expression(searchTerm, caseSensitivity); QFile file; QString str; QTextStream stream; - foreach (const QString &s, files) { - if (future.isPaused()) + foreach(const QString &s, files) { + if (future.isPaused()) { future.waitForResume(); + } if (future.isCanceled()) { future.setProgressValueAndText(numFilesSearched, msgCanceled(searchTerm, numMatches, numFilesSearched)); break; @@ -216,8 +221,9 @@ void runFileSearchRegExp(QFutureInterface &future, stream.setString(&str); } else { file.setFileName(s); - if (!file.open(QIODevice::ReadOnly)) + if (!file.open(QIODevice::ReadOnly)) { continue; + } needsToCloseFile = true; stream.setDevice(&file); } @@ -228,33 +234,34 @@ void runFileSearchRegExp(QFutureInterface &future, int pos = 0; while ((pos = expression.indexIn(line, pos)) != -1) { future.reportResult(FileSearchResult(s, lineNr, line, - pos, expression.matchedLength())); + pos, expression.matchedLength())); pos += expression.matchedLength(); } ++lineNr; } ++numFilesSearched; future.setProgressValueAndText(numFilesSearched, msgFound(searchTerm, numMatches, numFilesSearched, files.size())); - if (needsToCloseFile) + if (needsToCloseFile) { file.close(); + } } - if (!future.isCanceled()) + if (!future.isCanceled()) { future.setProgressValueAndText(numFilesSearched, msgFound(searchTerm, numMatches, numFilesSearched)); + } } - } // namespace QFuture Utils::findInFiles(const QString &searchTerm, const QStringList &files, - QTextDocument::FindFlags flags, QMap fileToContentsMap) + QTextDocument::FindFlags flags, QMap fileToContentsMap) { return QtConcurrent::run > - (runFileSearch, searchTerm, files, flags, fileToContentsMap); + (runFileSearch, searchTerm, files, flags, fileToContentsMap); } QFuture Utils::findInFilesRegExp(const QString &searchTerm, const QStringList &files, - QTextDocument::FindFlags flags, QMap fileToContentsMap) + QTextDocument::FindFlags flags, QMap fileToContentsMap) { return QtConcurrent::run > - (runFileSearchRegExp, searchTerm, files, flags, fileToContentsMap); + (runFileSearchRegExp, searchTerm, files, flags, fileToContentsMap); } diff --git a/ground/openpilotgcs/src/libs/utils/filesearch.h b/ground/openpilotgcs/src/libs/utils/filesearch.h index c7a046b0f..09802336e 100644 --- a/ground/openpilotgcs/src/libs/utils/filesearch.h +++ b/ground/openpilotgcs/src/libs/utils/filesearch.h @@ -4,25 +4,25 @@ * @file filesearch.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,15 +37,12 @@ #include namespace Utils { - -class QTCREATOR_UTILS_EXPORT FileSearchResult -{ +class QTCREATOR_UTILS_EXPORT FileSearchResult { public: FileSearchResult() {} FileSearchResult(QString fileName, int lineNumber, QString matchingLine, int matchStart, int matchLength) - : fileName(fileName), lineNumber(lineNumber), matchingLine(matchingLine), matchStart(matchStart), matchLength(matchLength) - { - } + : fileName(fileName), lineNumber(lineNumber), matchingLine(matchingLine), matchStart(matchStart), matchLength(matchLength) + {} QString fileName; int lineNumber; QString matchingLine; @@ -54,11 +51,10 @@ public: }; QTCREATOR_UTILS_EXPORT QFuture findInFiles(const QString &searchTerm, const QStringList &files, - QTextDocument::FindFlags flags, QMap fileToContentsMap = QMap()); + QTextDocument::FindFlags flags, QMap fileToContentsMap = QMap()); QTCREATOR_UTILS_EXPORT QFuture findInFilesRegExp(const QString &searchTerm, const QStringList &files, - QTextDocument::FindFlags flags, QMap fileToContentsMap = QMap()); - + QTextDocument::FindFlags flags, QMap fileToContentsMap = QMap()); } // namespace Utils #endif // FILESEARCH_H diff --git a/ground/openpilotgcs/src/libs/utils/filewizarddialog.cpp b/ground/openpilotgcs/src/libs/utils/filewizarddialog.cpp index 615b3b0ca..552e212fd 100644 --- a/ground/openpilotgcs/src/libs/utils/filewizarddialog.cpp +++ b/ground/openpilotgcs/src/libs/utils/filewizarddialog.cpp @@ -4,25 +4,25 @@ * @file filewizarddialog.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,7 +32,6 @@ #include namespace Utils { - FileWizardDialog::FileWizardDialog(QWidget *parent) : QWizard(parent), m_filePage(new FileWizardPage) @@ -58,12 +57,10 @@ QString FileWizardDialog::path() const void FileWizardDialog::setPath(const QString &path) { m_filePage->setPath(path); - } void FileWizardDialog::setName(const QString &name) { m_filePage->setName(name); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/filewizarddialog.h b/ground/openpilotgcs/src/libs/utils/filewizarddialog.h index f68e203ce..5e10a2aa2 100644 --- a/ground/openpilotgcs/src/libs/utils/filewizarddialog.h +++ b/ground/openpilotgcs/src/libs/utils/filewizarddialog.h @@ -4,25 +4,25 @@ * @file filewizarddialog.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,17 +34,15 @@ #include namespace Utils { - class FileWizardPage; /* Standard wizard for a single file letting the user choose name and path. Custom pages can be added via Core::IWizardExtension. -*/ + */ class QTCREATOR_UTILS_EXPORT FileWizardDialog : public QWizard { - Q_OBJECT - Q_DISABLE_COPY(FileWizardDialog) + Q_OBJECT Q_DISABLE_COPY(FileWizardDialog) public: explicit FileWizardDialog(QWidget *parent = 0); @@ -58,7 +56,6 @@ public slots: private: FileWizardPage *m_filePage; }; - } // namespace Utils #endif // FILEWIZARDDIALOG_H diff --git a/ground/openpilotgcs/src/libs/utils/filewizardpage.cpp b/ground/openpilotgcs/src/libs/utils/filewizardpage.cpp index 17f16e065..f25fb3d4b 100644 --- a/ground/openpilotgcs/src/libs/utils/filewizardpage.cpp +++ b/ground/openpilotgcs/src/libs/utils/filewizardpage.cpp @@ -4,25 +4,25 @@ * @file filewizardpage.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,9 +30,7 @@ #include "ui_filewizardpage.h" namespace Utils { - -struct FileWizardPagePrivate -{ +struct FileWizardPagePrivate { FileWizardPagePrivate(); Ui::WizardPage m_ui; bool m_complete; @@ -40,8 +38,7 @@ struct FileWizardPagePrivate FileWizardPagePrivate::FileWizardPagePrivate() : m_complete(false) -{ -} +{} FileWizardPage::FileWizardPage(QWidget *parent) : QWizardPage(parent), @@ -83,6 +80,7 @@ void FileWizardPage::setName(const QString &name) void FileWizardPage::changeEvent(QEvent *e) { QWizardPage::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: m_d->m_ui.retranslateUi(this); @@ -110,6 +108,7 @@ void FileWizardPage::setPathLabel(const QString &label) void FileWizardPage::slotValidChanged() { const bool newComplete = m_d->m_ui.pathChooser->isValid() && m_d->m_ui.nameLineEdit->isValid(); + if (newComplete != m_d->m_complete) { m_d->m_complete = newComplete; emit completeChanged(); @@ -118,13 +117,13 @@ void FileWizardPage::slotValidChanged() void FileWizardPage::slotActivated() { - if (m_d->m_complete) + if (m_d->m_complete) { emit activated(); + } } bool FileWizardPage::validateBaseName(const QString &name, QString *errorMessage /* = 0*/) { return FileNameValidatingLineEdit::validateFileName(name, false, errorMessage); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/filewizardpage.h b/ground/openpilotgcs/src/libs/utils/filewizardpage.h index 88d8b01e1..7fa559711 100644 --- a/ground/openpilotgcs/src/libs/utils/filewizardpage.h +++ b/ground/openpilotgcs/src/libs/utils/filewizardpage.h @@ -4,25 +4,25 @@ * @file filewizardpage.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,6 @@ #include namespace Utils { - struct FileWizardPagePrivate; /** @@ -44,10 +43,8 @@ struct FileWizardPagePrivate; * The name and path labels can be changed. By default they are simply "Name:" * and "Path:". */ -class QTCREATOR_UTILS_EXPORT FileWizardPage : public QWizardPage -{ - Q_OBJECT - Q_DISABLE_COPY(FileWizardPage) +class QTCREATOR_UTILS_EXPORT FileWizardPage : public QWizardPage { + Q_OBJECT Q_DISABLE_COPY(FileWizardPage) Q_PROPERTY(QString path READ path WRITE setPath DESIGNABLE true) Q_PROPERTY(QString name READ name WRITE setName DESIGNABLE true) public: @@ -83,7 +80,6 @@ protected: private: FileWizardPagePrivate *m_d; }; - } // namespace Utils #endif // FILEWIZARDPAGE_H diff --git a/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp b/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp index 67a32b0e8..44bf8c079 100644 --- a/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp +++ b/ground/openpilotgcs/src/libs/utils/homelocationutil.cpp @@ -35,40 +35,45 @@ #include "worldmagmodel.h" namespace Utils { +HomeLocationUtil::HomeLocationUtil() +{} - HomeLocationUtil::HomeLocationUtil() - { +/** + * @brief Get local magnetic field + * @param[in] LLA The longitude-latitude-altitude coordinate to compute the magnetic field at + * @param[out] Be The resulting magnetic field at that location and time in [mGau](?) + * @returns 0 if successful, negative otherwise. + */ +int HomeLocationUtil::getDetails(double LLA[3], double Be[3]) +{ + // ************* + // check input parms + + double latitude = LLA[0]; + double longitude = LLA[1]; + double altitude = LLA[2]; + + if (latitude != latitude) { + return -1; // prevent nan error } - - /** - * @brief Get local magnetic field - * @param[in] LLA The longitude-latitude-altitude coordinate to compute the magnetic field at - * @param[out] Be The resulting magnetic field at that location and time in [mGau](?) - * @returns 0 if successful, negative otherwise. - */ - int HomeLocationUtil::getDetails(double LLA[3], double Be[3]) - { - // ************* - // check input parms - - double latitude = LLA[0]; - double longitude = LLA[1]; - double altitude = LLA[2]; - - if (latitude != latitude) return -1; // prevent nan error - if (longitude != longitude) return -2; // prevent nan error - if (altitude != altitude) return -3; // prevent nan error - - if (latitude < -90 || latitude > 90) return -4; // range checking - if (longitude < -180 || longitude > 180) return -5; // range checking - - QDateTime dt = QDateTime::currentDateTime().toUTC(); - - // Fetch world magnetic model - int result = WorldMagModel().GetMagVector(LLA, dt.date().month(), dt.date().day(), dt.date().year(), Be); - Q_ASSERT(result == 0); - - return result; + if (longitude != longitude) { + return -2; // prevent nan error } + if (altitude != altitude) { + return -3; // prevent nan error + } + if (latitude < -90 || latitude > 90) { + return -4; // range checking + } + if (longitude < -180 || longitude > 180) { + return -5; // range checking + } + QDateTime dt = QDateTime::currentDateTime().toUTC(); + // Fetch world magnetic model + int result = WorldMagModel().GetMagVector(LLA, dt.date().month(), dt.date().day(), dt.date().year(), Be); + Q_ASSERT(result == 0); + + return result; +} } diff --git a/ground/openpilotgcs/src/libs/utils/homelocationutil.h b/ground/openpilotgcs/src/libs/utils/homelocationutil.h index 3688787ee..8dde442d2 100644 --- a/ground/openpilotgcs/src/libs/utils/homelocationutil.h +++ b/ground/openpilotgcs/src/libs/utils/homelocationutil.h @@ -34,18 +34,14 @@ // ****************************** namespace Utils { +class QTCREATOR_UTILS_EXPORT HomeLocationUtil { +public: + HomeLocationUtil(); - class QTCREATOR_UTILS_EXPORT HomeLocationUtil - { - public: - HomeLocationUtil(); - - int getDetails(double LLA[3], double Be[3]); - - private: - - }; + int getDetails(double LLA[3], double Be[3]); +private: +}; } // ****************************** diff --git a/ground/openpilotgcs/src/libs/utils/iwelcomepage.cpp b/ground/openpilotgcs/src/libs/utils/iwelcomepage.cpp index c6cac187f..8654a8bc3 100644 --- a/ground/openpilotgcs/src/libs/utils/iwelcomepage.cpp +++ b/ground/openpilotgcs/src/libs/utils/iwelcomepage.cpp @@ -4,25 +4,25 @@ * @file iwelcomepage.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,11 +31,7 @@ using namespace Utils; IWelcomePage::IWelcomePage() -{ - -} +{} IWelcomePage::~IWelcomePage() -{ - -} +{} diff --git a/ground/openpilotgcs/src/libs/utils/iwelcomepage.h b/ground/openpilotgcs/src/libs/utils/iwelcomepage.h index 34d910bbb..9439c1a9a 100644 --- a/ground/openpilotgcs/src/libs/utils/iwelcomepage.h +++ b/ground/openpilotgcs/src/libs/utils/iwelcomepage.h @@ -4,25 +4,25 @@ * @file iwelcomepage.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,11 +35,9 @@ #include namespace Utils { - class IWelcomePagePrivate; -class QTCREATOR_UTILS_EXPORT IWelcomePage : public QObject -{ +class QTCREATOR_UTILS_EXPORT IWelcomePage : public QObject { Q_OBJECT public: @@ -48,13 +46,15 @@ public: virtual QWidget *page() = 0; virtual QString title() const = 0; - virtual int priority() const { return 0; } + virtual int priority() const + { + return 0; + } private: // not used atm IWelcomePagePrivate *m_d; }; - } #endif // IWELCOMEPAGE_H diff --git a/ground/openpilotgcs/src/libs/utils/linecolumnlabel.cpp b/ground/openpilotgcs/src/libs/utils/linecolumnlabel.cpp index 1c59b1a0d..4992b17fa 100644 --- a/ground/openpilotgcs/src/libs/utils/linecolumnlabel.cpp +++ b/ground/openpilotgcs/src/libs/utils/linecolumnlabel.cpp @@ -4,44 +4,42 @@ * @file linecolumnlabel.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "linecolumnlabel.h" namespace Utils { - LineColumnLabel::LineColumnLabel(QWidget *parent) - : QLabel(parent), m_unused(0) -{ -} + : QLabel(parent), m_unused(0) +{} LineColumnLabel::~LineColumnLabel() -{ -} +{} void LineColumnLabel::setText(const QString &text, const QString &maxText) { QLabel::setText(text); + m_maxText = maxText; } QSize LineColumnLabel::sizeHint() const @@ -56,7 +54,6 @@ QString LineColumnLabel::maxText() const void LineColumnLabel::setMaxText(const QString &maxText) { - m_maxText = maxText; + m_maxText = maxText; } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/linecolumnlabel.h b/ground/openpilotgcs/src/libs/utils/linecolumnlabel.h index 836598da2..10ff22117 100644 --- a/ground/openpilotgcs/src/libs/utils/linecolumnlabel.h +++ b/ground/openpilotgcs/src/libs/utils/linecolumnlabel.h @@ -4,25 +4,25 @@ * @file linecolumnlabel.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,15 +33,12 @@ #include namespace Utils { - /* A label suitable for displaying cursor positions, etc. with a fixed * with derived from a sample text. */ -class QTCREATOR_UTILS_EXPORT LineColumnLabel : public QLabel -{ +class QTCREATOR_UTILS_EXPORT LineColumnLabel : public QLabel { Q_DISABLE_COPY(LineColumnLabel) - Q_OBJECT - Q_PROPERTY(QString maxText READ maxText WRITE setMaxText DESIGNABLE true) + Q_OBJECT Q_PROPERTY(QString maxText READ maxText WRITE setMaxText DESIGNABLE true) public: explicit LineColumnLabel(QWidget *parent = 0); @@ -57,7 +54,6 @@ private: QString m_maxText; void *m_unused; }; - } // namespace Utils #endif // LINECOLUMNLABEL_H diff --git a/ground/openpilotgcs/src/libs/utils/listutils.h b/ground/openpilotgcs/src/libs/utils/listutils.h index b3c079c02..a842ecd09 100644 --- a/ground/openpilotgcs/src/libs/utils/listutils.h +++ b/ground/openpilotgcs/src/libs/utils/listutils.h @@ -4,25 +4,25 @@ * @file listutils.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,17 +32,15 @@ #include namespace Utils { - template QList qwConvertList(const QList &list) { QList convertedList; - foreach (T2 listEntry, list) { + foreach(T2 listEntry, list) { convertedList << qobject_cast(listEntry); } return convertedList; } - } // namespace Utils #endif // LISTUTILS_H diff --git a/ground/openpilotgcs/src/libs/utils/mylistwidget.cpp b/ground/openpilotgcs/src/libs/utils/mylistwidget.cpp index f5fbfb639..f8a6de992 100644 --- a/ground/openpilotgcs/src/libs/utils/mylistwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/mylistwidget.cpp @@ -30,9 +30,10 @@ QStyleOptionViewItem MyListWidget::viewOptions() const { QStyleOptionViewItem option = QListWidget::viewOptions(); + if (m_iconAbove) { option.decorationPosition = QStyleOptionViewItem::Top; - option.displayAlignment = Qt::AlignCenter; + option.displayAlignment = Qt::AlignCenter; } return option; } diff --git a/ground/openpilotgcs/src/libs/utils/mylistwidget.h b/ground/openpilotgcs/src/libs/utils/mylistwidget.h index 176063681..7463db68f 100644 --- a/ground/openpilotgcs/src/libs/utils/mylistwidget.h +++ b/ground/openpilotgcs/src/libs/utils/mylistwidget.h @@ -38,12 +38,14 @@ * to place the icon above the label in ListMode. This is achieved * the easiest by subclassing QListWidget and overriding viewOptions(). */ -class QTCREATOR_UTILS_EXPORT MyListWidget : public QListWidget -{ +class QTCREATOR_UTILS_EXPORT MyListWidget : public QListWidget { Q_OBJECT public: MyListWidget(QWidget *parent) : QListWidget(parent), m_iconAbove(false) {} - void setIconAbove(bool iconAbove) { m_iconAbove = iconAbove; } + void setIconAbove(bool iconAbove) + { + m_iconAbove = iconAbove; + } protected: QStyleOptionViewItem viewOptions() const; private: diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp index a9610c0e5..20f45b08e 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp @@ -33,10 +33,10 @@ MyTabbedStackWidget::MyTabbedStackWidget(QWidget *parent, bool isVertical, bool iconAbove) : QWidget(parent), - m_vertical(isVertical), - m_iconAbove(iconAbove) + m_vertical(isVertical), + m_iconAbove(iconAbove) { - m_listWidget = new MyListWidget(this); + m_listWidget = new MyListWidget(this); m_listWidget->setIconAbove(m_iconAbove); m_stackWidget = new QStackedWidget(); m_stackWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); @@ -69,7 +69,7 @@ MyTabbedStackWidget::MyTabbedStackWidget(QWidget *parent, bool isVertical, bool m_stackWidget->setContentsMargins(0, 0, 0, 0); setLayout(toplevelLayout); - connect(m_listWidget, SIGNAL(currentRowChanged(int)), this, SLOT(showWidget(int)),Qt::QueuedConnection); + connect(m_listWidget, SIGNAL(currentRowChanged(int)), this, SLOT(showWidget(int)), Qt::QueuedConnection); } void MyTabbedStackWidget::insertTab(const int index, QWidget *tab, const QIcon &icon, const QString &label) @@ -83,7 +83,8 @@ void MyTabbedStackWidget::insertTab(const int index, QWidget *tab, const QIcon & void MyTabbedStackWidget::removeTab(int index) { - QWidget * wid=m_stackWidget->widget(index); + QWidget *wid = m_stackWidget->widget(index); + m_stackWidget->removeWidget(wid); delete wid; QListWidgetItem *item = m_listWidget->item(index); @@ -93,7 +94,7 @@ void MyTabbedStackWidget::removeTab(int index) void MyTabbedStackWidget::setWidgetsEnabled(bool enabled) { - for(int i = 0; i < m_stackWidget->count(); i++) { + for (int i = 0; i < m_stackWidget->count(); i++) { m_stackWidget->widget(i)->setEnabled(enabled); } } @@ -110,18 +111,16 @@ void MyTabbedStackWidget::setCurrentIndex(int index) void MyTabbedStackWidget::showWidget(int index) { - if(m_stackWidget->currentIndex()==index) + if (m_stackWidget->currentIndex() == index) { return; - bool proceed=false; - emit currentAboutToShow(index,&proceed); - if(proceed) - { + } + bool proceed = false; + emit currentAboutToShow(index, &proceed); + if (proceed) { m_stackWidget->setCurrentIndex(index); emit currentChanged(index); - } - else - { - m_listWidget->setCurrentRow(m_stackWidget->currentIndex(),QItemSelectionModel::ClearAndSelect); + } else { + m_listWidget->setCurrentRow(m_stackWidget->currentIndex(), QItemSelectionModel::ClearAndSelect); } } @@ -131,4 +130,3 @@ void MyTabbedStackWidget::insertCornerWidget(int index, QWidget *widget) widget->hide(); } - diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h index 7307673b1..6862ee426 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h @@ -36,8 +36,7 @@ * MyTabbedStackWidget is a MyListWidget combined with a QStackedWidget, * similar in function to QTabWidget. */ -class QTCREATOR_UTILS_EXPORT MyTabbedStackWidget : public QWidget -{ +class QTCREATOR_UTILS_EXPORT MyTabbedStackWidget : public QWidget { Q_OBJECT public: @@ -45,19 +44,31 @@ public: void insertTab(int index, QWidget *tab, const QIcon &icon, const QString &label); void removeTab(int index); - void setIconSize(int size) { m_listWidget->setIconSize(QSize(size, size)); } + void setIconSize(int size) + { + m_listWidget->setIconSize(QSize(size, size)); + } void setWidgetsEnabled(bool enabled); int currentIndex() const; void insertCornerWidget(int index, QWidget *widget); - int cornerWidgetCount() { return m_cornerWidgetCount; } - QWidget * currentWidget(){return m_stackWidget->currentWidget();} - QWidget * getWidget(int index) {return m_stackWidget->widget(index);} + int cornerWidgetCount() + { + return m_cornerWidgetCount; + } + QWidget *currentWidget() + { + return m_stackWidget->currentWidget(); + } + QWidget *getWidget(int index) + { + return m_stackWidget->widget(index); + } signals: - void currentAboutToShow(int index,bool * proceed); + void currentAboutToShow(int index, bool *proceed); void currentChanged(int index); public slots: diff --git a/ground/openpilotgcs/src/libs/utils/mytabwidget.cpp b/ground/openpilotgcs/src/libs/utils/mytabwidget.cpp index 2a1a7bee4..330312777 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/mytabwidget.cpp @@ -32,12 +32,14 @@ MyTabWidget::MyTabWidget(QWidget *parent) : QTabWidget(parent) { QTabBar *tabBar = QTabWidget::tabBar(); - connect(tabBar, SIGNAL(tabMoved(int, int)), this, SLOT(myTabMoved(int,int))); + + connect(tabBar, SIGNAL(tabMoved(int, int)), this, SLOT(myTabMoved(int, int))); } void MyTabWidget::moveTab(int from, int to) { QTabBar *tabBar = QTabWidget::tabBar(); + tabBar->moveTab(from, to); } diff --git a/ground/openpilotgcs/src/libs/utils/mytabwidget.h b/ground/openpilotgcs/src/libs/utils/mytabwidget.h index 8f941110b..6110c13c6 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabwidget.h +++ b/ground/openpilotgcs/src/libs/utils/mytabwidget.h @@ -37,8 +37,7 @@ * tabMoved(int, int) which QTabBar has but for some reason is * not made available from QTabWidget. */ -class QTCREATOR_UTILS_EXPORT MyTabWidget : public QTabWidget -{ +class QTCREATOR_UTILS_EXPORT MyTabWidget : public QTabWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/libs/utils/newclasswidget.cpp b/ground/openpilotgcs/src/libs/utils/newclasswidget.cpp index 61d7c52a4..844dbd68c 100644 --- a/ground/openpilotgcs/src/libs/utils/newclasswidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/newclasswidget.cpp @@ -4,25 +4,25 @@ * @file newclasswidget.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,7 +41,6 @@ enum { debugNewClassWidget = 0 }; namespace Utils { - struct NewClassWidgetPrivate { NewClassWidgetPrivate(); @@ -49,18 +48,18 @@ struct NewClassWidgetPrivate { QString m_headerExtension; QString m_sourceExtension; QString m_formExtension; - bool m_valid; - bool m_classEdited; + bool m_valid; + bool m_classEdited; // Store the "visible" values to prevent the READ accessors from being // fooled by a temporarily hidden widget - bool m_baseClassInputVisible; - bool m_formInputVisible; - bool m_pathInputVisible; - bool m_qobjectCheckBoxVisible; - bool m_formInputCheckable; + bool m_baseClassInputVisible; + bool m_formInputVisible; + bool m_pathInputVisible; + bool m_qobjectCheckBoxVisible; + bool m_formInputCheckable; }; -NewClassWidgetPrivate:: NewClassWidgetPrivate() : +NewClassWidgetPrivate::NewClassWidgetPrivate() : m_headerExtension(QLatin1String("h")), m_sourceExtension(QLatin1String("cpp")), m_formExtension(QLatin1String("ui")), @@ -72,8 +71,7 @@ NewClassWidgetPrivate:: NewClassWidgetPrivate() : m_qobjectCheckBoxVisible(false), m_formInputCheckable(false) -{ -} +{} // --------------------- NewClassWidget NewClassWidget::NewClassWidget(QWidget *parent) : @@ -114,7 +112,7 @@ NewClassWidget::NewClassWidget(QWidget *parent) : connect(m_d->m_ui.formFileLineEdit, SIGNAL(validReturnPressed()), this, SLOT(slotActivated())); connect(m_d->m_ui.pathChooser, SIGNAL(returnPressed()), - this, SLOT(slotActivated())); + this, SLOT(slotActivated())); connect(m_d->m_ui.generateFormCheckBox, SIGNAL(stateChanged(int)), this, SLOT(slotFormInputChecked())); @@ -131,17 +129,20 @@ NewClassWidget::~NewClassWidget() void NewClassWidget::classNameEdited() { - if (debugNewClassWidget) + if (debugNewClassWidget) { qDebug() << Q_FUNC_INFO << m_d->m_headerExtension << m_d->m_sourceExtension; + } m_d->m_classEdited = true; } void NewClassWidget::suggestClassNameFromBase() { - if (debugNewClassWidget) + if (debugNewClassWidget) { qDebug() << Q_FUNC_INFO << m_d->m_headerExtension << m_d->m_sourceExtension; - if (m_d->m_classEdited) + } + if (m_d->m_classEdited) { return; + } // Suggest a class unless edited ("QMainWindow"->"MainWindow") QString base = baseClassName(); if (base.startsWith(QLatin1Char('Q'))) { @@ -154,8 +155,10 @@ QStringList NewClassWidget::baseClassChoices() const { QStringList rc; const int count = m_d->m_ui.baseClassComboBox->count(); - for (int i = 0; i < count; i++) + + for (int i = 0; i < count; i++) { rc.push_back(m_d->m_ui.baseClassComboBox->itemText(i)); + } return rc; } @@ -179,12 +182,12 @@ void NewClassWidget::setBaseClassEditable(bool editable) bool NewClassWidget::isBaseClassInputVisible() const { - return m_d->m_baseClassInputVisible; + return m_d->m_baseClassInputVisible; } bool NewClassWidget::isBaseClassEditable() const { - return m_d->m_ui.baseClassComboBox->isEditable(); + return m_d->m_ui.baseClassComboBox->isEditable(); } void NewClassWidget::setFormInputVisible(bool visible) @@ -206,8 +209,9 @@ void NewClassWidget::setFormInputCheckable(bool checkable) void NewClassWidget::setFormInputCheckable(bool checkable, bool force) { - if (!force && checkable == m_d->m_formInputCheckable) + if (!force && checkable == m_d->m_formInputCheckable) { return; + } m_d->m_formInputCheckable = checkable; m_d->m_ui.generateFormLabel->setVisible(checkable); m_d->m_ui.generateFormCheckBox->setVisible(checkable); @@ -231,6 +235,7 @@ bool NewClassWidget::formInputChecked() const void NewClassWidget::slotFormInputChecked() { const bool checked = formInputChecked(); + m_d->m_ui.formLabel->setEnabled(checked); m_d->m_ui.formFileLineEdit->setEnabled(checked); } @@ -249,8 +254,9 @@ bool NewClassWidget::isPathInputVisible() const void NewClassWidget::setClassName(const QString &suggestedName) { - if (debugNewClassWidget) + if (debugNewClassWidget) { qDebug() << Q_FUNC_INFO << suggestedName << m_d->m_headerExtension << m_d->m_sourceExtension; + } m_d->m_ui.classLineEdit->setText(ClassNameValidatingLineEdit::createClassName(suggestedName)); } @@ -267,6 +273,7 @@ QString NewClassWidget::baseClassName() const void NewClassWidget::setBaseClassName(const QString &c) { const int index = m_d->m_ui.baseClassComboBox->findText(c); + if (index != -1) { m_d->m_ui.baseClassComboBox->setCurrentIndex(index); suggestClassNameFromBase(); @@ -295,12 +302,12 @@ QString NewClassWidget::path() const void NewClassWidget::setPath(const QString &path) { - m_d->m_ui.pathChooser->setPath(path); + m_d->m_ui.pathChooser->setPath(path); } bool NewClassWidget::namespacesEnabled() const { - return m_d->m_ui.classLineEdit->namespacesEnabled(); + return m_d->m_ui.classLineEdit->namespacesEnabled(); } void NewClassWidget::setNamespacesEnabled(bool b) @@ -315,8 +322,9 @@ QString NewClassWidget::sourceExtension() const void NewClassWidget::setSourceExtension(const QString &e) { - if (debugNewClassWidget) + if (debugNewClassWidget) { qDebug() << Q_FUNC_INFO << e; + } m_d->m_sourceExtension = fixSuffix(e); } @@ -327,8 +335,9 @@ QString NewClassWidget::headerExtension() const void NewClassWidget::setHeaderExtension(const QString &e) { - if (debugNewClassWidget) + if (debugNewClassWidget) { qDebug() << Q_FUNC_INFO << e; + } m_d->m_headerExtension = fixSuffix(e); } @@ -339,8 +348,9 @@ QString NewClassWidget::formExtension() const void NewClassWidget::setFormExtension(const QString &e) { - if (debugNewClassWidget) + if (debugNewClassWidget) { qDebug() << Q_FUNC_INFO << e; + } m_d->m_formExtension = fixSuffix(e); } @@ -393,6 +403,7 @@ void NewClassWidget::setClassTypeComboVisible(bool v) void NewClassWidget::slotValidChanged() { const bool newValid = isValid(); + if (newValid != m_d->m_valid) { m_d->m_valid = newValid; emit validChanged(); @@ -402,8 +413,9 @@ void NewClassWidget::slotValidChanged() bool NewClassWidget::isValid(QString *error) const { if (!m_d->m_ui.classLineEdit->isValid()) { - if (error) + if (error) { *error = m_d->m_ui.classLineEdit->errorMessage(); + } return false; } @@ -412,36 +424,41 @@ bool NewClassWidget::isValid(QString *error) const QRegExp classNameValidator(QLatin1String("[a-zA-Z_][a-zA-Z0-9_]*(::[a-zA-Z_][a-zA-Z0-9_]*)*")); const QString baseClass = m_d->m_ui.baseClassComboBox->currentText().trimmed(); if (!baseClass.isEmpty() && !classNameValidator.exactMatch(baseClass)) { - if (error) + if (error) { *error = tr("Invalid base class name"); + } return false; } } if (!m_d->m_ui.headerFileLineEdit->isValid()) { - if (error) + if (error) { *error = tr("Invalid header file name: '%1'").arg(m_d->m_ui.headerFileLineEdit->errorMessage()); + } return false; } if (!m_d->m_ui.sourceFileLineEdit->isValid()) { - if (error) + if (error) { *error = tr("Invalid source file name: '%1'").arg(m_d->m_ui.sourceFileLineEdit->errorMessage()); + } return false; } if (isFormInputVisible()) { if (!m_d->m_ui.formFileLineEdit->isValid()) { - if (error) + if (error) { *error = tr("Invalid form file name: '%1'").arg(m_d->m_ui.formFileLineEdit->errorMessage()); + } return false; } } if (isPathInputVisible()) { if (!m_d->m_ui.pathChooser->isValid()) { - if (error) - *error = m_d->m_ui.pathChooser->errorMessage(); + if (error) { + *error = m_d->m_ui.pathChooser->errorMessage(); + } return false; } } @@ -455,8 +472,9 @@ void NewClassWidget::triggerUpdateFileNames() void NewClassWidget::slotUpdateFileNames(const QString &baseName) { - if (debugNewClassWidget) + if (debugNewClassWidget) { qDebug() << Q_FUNC_INFO << baseName << m_d->m_headerExtension << m_d->m_sourceExtension; + } const QChar dot = QLatin1Char('.'); m_d->m_ui.sourceFileLineEdit->setText(baseName + dot + m_d->m_sourceExtension); m_d->m_ui.headerFileLineEdit->setText(baseName + dot + m_d->m_headerExtension); @@ -465,15 +483,18 @@ void NewClassWidget::slotUpdateFileNames(const QString &baseName) void NewClassWidget::slotActivated() { - if (m_d->m_valid) + if (m_d->m_valid) { emit activated(); + } } QString NewClassWidget::fixSuffix(const QString &suffix) { QString s = suffix; - if (s.startsWith(QLatin1Char('.'))) + + if (s.startsWith(QLatin1Char('.'))) { s.remove(0, 1); + } return s; } @@ -481,8 +502,10 @@ QString NewClassWidget::fixSuffix(const QString &suffix) static QString ensureSuffix(QString f, const QString &extension) { const QChar dot = QLatin1Char('.'); - if (f.contains(dot)) + + if (f.contains(dot)) { return f; + } f += dot; f += extension; return f; @@ -491,8 +514,9 @@ static QString ensureSuffix(QString f, const QString &extension) // If a non-empty name was passed, expand to directory and suffix static QString expandFileName(const QDir &dir, const QString name, const QString &extension) { - if (name.isEmpty()) + if (name.isEmpty()) { return QString(); + } return dir.absoluteFilePath(ensureSuffix(name, extension)); } @@ -500,11 +524,12 @@ QStringList NewClassWidget::files() const { QStringList rc; const QDir dir = QDir(path()); + rc.push_back(expandFileName(dir, headerFileName(), headerExtension())); rc.push_back(expandFileName(dir, sourceFileName(), sourceExtension())); - if (isFormInputVisible()) + if (isFormInputVisible()) { rc.push_back(expandFileName(dir, formFileName(), formExtension())); + } return rc; } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/newclasswidget.h b/ground/openpilotgcs/src/libs/utils/newclasswidget.h index 6b36553d1..eff073719 100644 --- a/ground/openpilotgcs/src/libs/utils/newclasswidget.h +++ b/ground/openpilotgcs/src/libs/utils/newclasswidget.h @@ -4,25 +4,25 @@ * @file newclasswidget.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,7 +38,6 @@ class QStringList; QT_END_NAMESPACE namespace Utils { - struct NewClassWidgetPrivate; /** @@ -47,11 +46,9 @@ struct NewClassWidgetPrivate; * names for header, source and form files. Has some smart logic to derive * the file names from the class name. */ -class QTCREATOR_UTILS_EXPORT NewClassWidget : public QWidget -{ +class QTCREATOR_UTILS_EXPORT NewClassWidget : public QWidget { Q_DISABLE_COPY(NewClassWidget) - Q_OBJECT - Q_PROPERTY(bool namespacesEnabled READ namespacesEnabled WRITE setNamespacesEnabled DESIGNABLE true) + Q_OBJECT Q_PROPERTY(bool namespacesEnabled READ namespacesEnabled WRITE setNamespacesEnabled DESIGNABLE true) Q_PROPERTY(bool baseClassInputVisible READ isBaseClassInputVisible WRITE setBaseClassInputVisible DESIGNABLE true) Q_PROPERTY(bool baseClassEditable READ isBaseClassEditable WRITE setBaseClassEditable DESIGNABLE false) Q_PROPERTY(bool formInputVisible READ isFormInputVisible WRITE setFormInputVisible DESIGNABLE true) @@ -161,7 +158,6 @@ private: QString fixSuffix(const QString &suffix); NewClassWidgetPrivate *m_d; }; - } // namespace Utils #endif // NEWCLASSWIDGET_H diff --git a/ground/openpilotgcs/src/libs/utils/parameteraction.cpp b/ground/openpilotgcs/src/libs/utils/parameteraction.cpp index 88a145c6b..74e5cde8d 100644 --- a/ground/openpilotgcs/src/libs/utils/parameteraction.cpp +++ b/ground/openpilotgcs/src/libs/utils/parameteraction.cpp @@ -4,42 +4,40 @@ * @file parameteraction.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "parameteraction.h" namespace Utils { - ParameterAction::ParameterAction(const QString &emptyText, - const QString ¶meterText, - EnablingMode mode, - QObject* parent) : + const QString ¶meterText, + EnablingMode mode, + QObject *parent) : QAction(emptyText, parent), m_emptyText(emptyText), m_parameterText(parameterText), m_enablingMode(mode) -{ -} +{} QString ParameterAction::emptyText() const { @@ -74,13 +72,14 @@ void ParameterAction::setEnablingMode(EnablingMode m) void ParameterAction::setParameter(const QString &p) { const bool enabled = !p.isEmpty(); + if (enabled) { setText(m_parameterText.arg(p)); } else { setText(m_emptyText); } - if (m_enablingMode == EnabledWithParameter) + if (m_enablingMode == EnabledWithParameter) { setEnabled(enabled); + } } - } diff --git a/ground/openpilotgcs/src/libs/utils/parameteraction.h b/ground/openpilotgcs/src/libs/utils/parameteraction.h index 599c53a5f..5c36f84d9 100644 --- a/ground/openpilotgcs/src/libs/utils/parameteraction.h +++ b/ground/openpilotgcs/src/libs/utils/parameteraction.h @@ -4,25 +4,25 @@ * @file parameteraction.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,6 @@ #include namespace Utils { - /* ParameterAction: Intended for actions that act on a 'current', * string-type parameter (typically file name) and have 2 states: * 1) displaying "Do XX" (empty text) @@ -44,8 +43,7 @@ namespace Utils { * The text passed in should already be translated; parameterText * should contain a %1 where the parameter is to be inserted. */ -class QTCREATOR_UTILS_EXPORT ParameterAction : public QAction -{ +class QTCREATOR_UTILS_EXPORT ParameterAction : public QAction { Q_ENUMS(EnablingMode) Q_PROPERTY(QString emptyText READ emptyText WRITE setEmptyText) Q_PROPERTY(QString parameterText READ parameterText WRITE setParameterText) @@ -57,7 +55,7 @@ public: explicit ParameterAction(const QString &emptyText, const QString ¶meterText, EnablingMode em = AlwaysEnabled, - QObject* parent = 0); + QObject *parent = 0); QString emptyText() const; void setEmptyText(const QString &); @@ -76,7 +74,6 @@ private: QString m_parameterText; EnablingMode m_enablingMode; }; - } #endif // PARAMETERACTION_H diff --git a/ground/openpilotgcs/src/libs/utils/pathchooser.cpp b/ground/openpilotgcs/src/libs/utils/pathchooser.cpp index 426b8b845..e166c66da 100644 --- a/ground/openpilotgcs/src/libs/utils/pathchooser.cpp +++ b/ground/openpilotgcs/src/libs/utils/pathchooser.cpp @@ -4,25 +4,25 @@ * @file pathchooser.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,18 +43,16 @@ #include #include -/*static*/ const char * const Utils::PathChooser::browseButtonLabel = +/*static*/ const char *const Utils::PathChooser::browseButtonLabel = #ifdef Q_WS_MAC - QT_TRANSLATE_NOOP("Utils::PathChooser", "Choose..."); + QT_TRANSLATE_NOOP("Utils::PathChooser", "Choose..."); #else - QT_TRANSLATE_NOOP("Utils::PathChooser", "Browse..."); + QT_TRANSLATE_NOOP("Utils::PathChooser", "Browse..."); #endif namespace Utils { - // ------------------ PathValidatingLineEdit -class PathValidatingLineEdit : public BaseValidatingLineEdit -{ +class PathValidatingLineEdit : public BaseValidatingLineEdit { public: explicit PathValidatingLineEdit(PathChooser *chooser, QWidget *parent = 0); @@ -69,7 +67,7 @@ PathValidatingLineEdit::PathValidatingLineEdit(PathChooser *chooser, QWidget *pa BaseValidatingLineEdit(parent), m_chooser(chooser) { - QTC_ASSERT(chooser, return); + QTC_ASSERT(chooser, return ); } bool PathValidatingLineEdit::validate(const QString &value, QString *errorMessage) const @@ -78,30 +76,27 @@ bool PathValidatingLineEdit::validate(const QString &value, QString *errorMessag } // ------------------ PathChooserPrivate -struct PathChooserPrivate -{ +struct PathChooserPrivate { PathChooserPrivate(PathChooser *chooser); QHBoxLayout *m_hLayout; PathValidatingLineEdit *m_lineEdit; PathChooser::Kind m_acceptingKind; - QString m_dialogTitleOverride; - QString m_dialogFilter; - QString m_initialBrowsePathOverride; + QString m_dialogTitleOverride; + QString m_dialogFilter; + QString m_initialBrowsePathOverride; }; PathChooserPrivate::PathChooserPrivate(PathChooser *chooser) : m_hLayout(new QHBoxLayout), m_lineEdit(new PathValidatingLineEdit(chooser)), m_acceptingKind(PathChooser::Directory) -{ -} +{} PathChooser::PathChooser(QWidget *parent) : QWidget(parent), m_d(new PathChooserPrivate(this)) { - m_d->m_hLayout->setContentsMargins(0, 0, 0, 0); connect(m_d->m_lineEdit, SIGNAL(validReturnPressed()), this, SIGNAL(returnPressed())); @@ -139,7 +134,7 @@ void PathChooser::addButton(const QString &text, QObject *receiver, const char * QAbstractButton *PathChooser::buttonAtIndex(int index) const { - return findChildren().at(index); + return findChildren().at(index); } QString PathChooser::path() const @@ -157,15 +152,19 @@ void PathChooser::slotBrowse() emit beforeBrowsing(); QString predefined = path(); + if ((predefined.isEmpty() || !QFileInfo(predefined).isDir()) - && !m_d->m_initialBrowsePathOverride.isNull()) { + && !m_d->m_initialBrowsePathOverride.isNull()) { predefined = m_d->m_initialBrowsePathOverride; - if (!QFileInfo(predefined).isDir()) + + if (!QFileInfo(predefined).isDir()) { predefined.clear(); + } } - if (predefined.startsWith(":")) + if (predefined.startsWith(":")) { predefined.clear(); + } // Prompt for a file/dir QString dialogTitle; @@ -173,14 +172,14 @@ void PathChooser::slotBrowse() switch (m_d->m_acceptingKind) { case PathChooser::Directory: newPath = QFileDialog::getExistingDirectory(this, - makeDialogTitle(tr("Choose a directory")), predefined); + makeDialogTitle(tr("Choose a directory")), predefined); break; case PathChooser::File: // fall through case PathChooser::Command: newPath = QFileDialog::getOpenFileName(this, - makeDialogTitle(tr("Choose a file")), predefined, - m_d->m_dialogFilter); + makeDialogTitle(tr("Choose a file")), predefined, + m_d->m_dialogFilter); break; default: @@ -190,8 +189,9 @@ void PathChooser::slotBrowse() // Delete trailing slashes unless it is "/"|"\\", only if (!newPath.isEmpty()) { newPath = QDir::toNativeSeparators(newPath); - if (newPath.size() > 1 && newPath.endsWith(QDir::separator())) + if (newPath.size() > 1 && newPath.endsWith(QDir::separator())) { newPath.truncate(newPath.size() - 1); + } setPath(newPath); } @@ -211,8 +211,9 @@ QString PathChooser::errorMessage() const bool PathChooser::validatePath(const QString &path, QString *errorMessage) { if (path.isEmpty()) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The path must not be empty."); + } return false; } @@ -224,8 +225,9 @@ bool PathChooser::validatePath(const QString &path, QString *errorMessage) case PathChooser::Directory: // fall through case PathChooser::File: if (!fi.exists()) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The path '%1' does not exist.").arg(path); + } return false; } break; @@ -239,16 +241,18 @@ bool PathChooser::validatePath(const QString &path, QString *errorMessage) switch (m_d->m_acceptingKind) { case PathChooser::Directory: if (!isDir) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The path '%1' is not a directory.").arg(path); + } return false; } break; case PathChooser::File: if (isDir) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The path '%1' is not a file.").arg(path); + } return false; } break; @@ -277,8 +281,10 @@ QString PathChooser::homePath() // does not let people actually display the contents of their home // directory. Alternatively, create a QtCreator-specific directory? return QDesktopServices::storageLocation(QDesktopServices::DocumentsLocation); + #else return QDir::homePath(); + #endif } @@ -319,10 +325,10 @@ void PathChooser::setInitialBrowsePathBackup(const QString &path) QString PathChooser::makeDialogTitle(const QString &title) { - if (m_d->m_dialogTitleOverride.isNull()) + if (m_d->m_dialogTitleOverride.isNull()) { return title; - else + } else { return m_d->m_dialogTitleOverride; + } } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/pathchooser.h b/ground/openpilotgcs/src/libs/utils/pathchooser.h index 54a85ed34..3da12a7a0 100644 --- a/ground/openpilotgcs/src/libs/utils/pathchooser.h +++ b/ground/openpilotgcs/src/libs/utils/pathchooser.h @@ -4,25 +4,25 @@ * @file pathchooser.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,24 +35,22 @@ #include namespace Utils { - struct PathChooserPrivate; /** * A control that let's the user choose a path, consisting of a QLineEdit and * a "Browse" button. Has some validation logic for embedding into QWizardPage. */ -class QTCREATOR_UTILS_EXPORT PathChooser : public QWidget -{ +class QTCREATOR_UTILS_EXPORT PathChooser : public QWidget { Q_DISABLE_COPY(PathChooser) Q_OBJECT - Q_ENUMS(Kind) + Q_ENUMS(Kind) Q_PROPERTY(QString path READ path WRITE setPath DESIGNABLE true) Q_PROPERTY(QString promptDialogTitle READ promptDialogTitle WRITE setPromptDialogTitle DESIGNABLE true) Q_PROPERTY(Kind expectedKind READ expectedKind WRITE setExpectedKind DESIGNABLE true) public: - static const char * const browseButtonLabel; + static const char *const browseButtonLabel; explicit PathChooser(QWidget *parent = 0); virtual ~PathChooser(); @@ -114,7 +112,6 @@ private slots: private: PathChooserPrivate *m_d; }; - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/pathlisteditor.cpp b/ground/openpilotgcs/src/libs/utils/pathlisteditor.cpp index 9f1459727..944dd6195 100644 --- a/ground/openpilotgcs/src/libs/utils/pathlisteditor.cpp +++ b/ground/openpilotgcs/src/libs/utils/pathlisteditor.cpp @@ -4,25 +4,25 @@ * @file pathlisteditor.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -46,7 +46,6 @@ #include namespace Utils { - // ------------ PathListPlainTextEdit: // Replaces the platform separator ';',':' by '\n' // when inserting, allowing for pasting in paths @@ -56,7 +55,7 @@ class PathListPlainTextEdit : public QPlainTextEdit { public: explicit PathListPlainTextEdit(QWidget *parent = 0); protected: - virtual void insertFromMimeData (const QMimeData *source); + virtual void insertFromMimeData(const QMimeData *source); }; PathListPlainTextEdit::PathListPlainTextEdit(QWidget *parent) : @@ -85,22 +84,22 @@ void PathListPlainTextEdit::insertFromMimeData(const QMimeData *source) struct PathListEditorPrivate { PathListEditorPrivate(); - QHBoxLayout *layout; - QVBoxLayout *buttonLayout; - QToolButton *toolButton; + QHBoxLayout *layout; + QVBoxLayout *buttonLayout; + QToolButton *toolButton; QMenu *buttonMenu; QPlainTextEdit *edit; - QSignalMapper *envVarMapper; + QSignalMapper *envVarMapper; QString fileDialogTitle; }; PathListEditorPrivate::PathListEditorPrivate() : - layout(new QHBoxLayout), - buttonLayout(new QVBoxLayout), - toolButton(new QToolButton), - buttonMenu(new QMenu), - edit(new PathListPlainTextEdit), - envVarMapper(0) + layout(new QHBoxLayout), + buttonLayout(new QVBoxLayout), + toolButton(new QToolButton), + buttonMenu(new QMenu), + edit(new PathListPlainTextEdit), + envVarMapper(0) { layout->setMargin(0); layout->addWidget(edit); @@ -110,8 +109,8 @@ PathListEditorPrivate::PathListEditorPrivate() : } PathListEditor::PathListEditor(QWidget *parent) : - QWidget(parent), - m_d(new PathListEditorPrivate) + QWidget(parent), + m_d(new PathListEditorPrivate) { setLayout(m_d->layout); m_d->toolButton->setPopupMode(QToolButton::MenuButtonPopup); @@ -129,28 +128,32 @@ PathListEditor::~PathListEditor() delete m_d; } -static inline QAction *createAction(QObject *parent, const QString &text, QObject * receiver, const char *slotFunc) +static inline QAction *createAction(QObject *parent, const QString &text, QObject *receiver, const char *slotFunc) { QAction *rc = new QAction(text, parent); QObject::connect(rc, SIGNAL(triggered()), receiver, slotFunc); + return rc; } -QAction *PathListEditor::addAction(const QString &text, QObject * receiver, const char *slotFunc) +QAction *PathListEditor::addAction(const QString &text, QObject *receiver, const char *slotFunc) { QAction *rc = createAction(this, text, receiver, slotFunc); + m_d->buttonMenu->addAction(rc); return rc; } -QAction *PathListEditor::insertAction(int index /* -1 */, const QString &text, QObject * receiver, const char *slotFunc) +QAction *PathListEditor::insertAction(int index /* -1 */, const QString &text, QObject *receiver, const char *slotFunc) { // Find the 'before' action QAction *beforeAction = 0; + if (index >= 0) { - const QList actions = m_d->buttonMenu->actions(); - if (index < actions.size()) + const QList actions = m_d->buttonMenu->actions(); + if (index < actions.size()) { beforeAction = actions.at(index); + } } QAction *rc = createAction(this, text, receiver, slotFunc); if (beforeAction) { @@ -174,13 +177,16 @@ QString PathListEditor::pathListString() const QStringList PathListEditor::pathList() const { const QString text = m_d->edit->toPlainText().trimmed(); - if (text.isEmpty()) + + if (text.isEmpty()) { return QStringList(); + } // trim each line QStringList rc = text.split(QLatin1Char('\n'), QString::SkipEmptyParts); const QStringList::iterator end = rc.end(); - for (QStringList::iterator it = rc.begin(); it != end; ++it) + for (QStringList::iterator it = rc.begin(); it != end; ++it) { *it = it->trimmed(); + } return rc; } @@ -221,15 +227,19 @@ void PathListEditor::clear() void PathListEditor::slotAdd() { const QString dir = QFileDialog::getExistingDirectory(this, m_d->fileDialogTitle); - if (!dir.isEmpty()) + + if (!dir.isEmpty()) { appendPath(QDir::toNativeSeparators(dir)); + } } void PathListEditor::slotInsert() { const QString dir = QFileDialog::getExistingDirectory(this, m_d->fileDialogTitle); - if (!dir.isEmpty()) + + if (!dir.isEmpty()) { insertPathAtCursor(QDir::toNativeSeparators(dir)); + } } QChar PathListEditor::separator() @@ -269,9 +279,10 @@ void PathListEditor::insertPathAtCursor(const QString &path) { // If the cursor is at an empty line or at end(), // just insert. Else insert line before - QTextCursor cursor = m_d->edit->textCursor(); - QTextBlock block = cursor.block(); + QTextCursor cursor = m_d->edit->textCursor(); + QTextBlock block = cursor.block(); const bool needNewLine = !block.text().isEmpty(); + if (needNewLine) { cursor.movePosition(QTextCursor::StartOfLine, QTextCursor::MoveAnchor); cursor.insertBlock(); @@ -287,8 +298,10 @@ void PathListEditor::insertPathAtCursor(const QString &path) void PathListEditor::appendPath(const QString &path) { QString paths = text().trimmed(); - if (!paths.isEmpty()) + + if (!paths.isEmpty()) { paths += QLatin1Char('\n'); + } paths += path; setText(paths); } @@ -297,14 +310,15 @@ void PathListEditor::deletePathAtCursor() { // Delete current line QTextCursor cursor = m_d->edit->textCursor(); + if (cursor.block().isValid()) { cursor.movePosition(QTextCursor::StartOfLine, QTextCursor::MoveAnchor); // Select down or until end of [last] line - if (!cursor.movePosition(QTextCursor::Down, QTextCursor::KeepAnchor)) + if (!cursor.movePosition(QTextCursor::Down, QTextCursor::KeepAnchor)) { cursor.movePosition(QTextCursor::EndOfLine, QTextCursor::KeepAnchor); + } cursor.removeSelectedText(); m_d->edit->setTextCursor(cursor); } } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/pathlisteditor.h b/ground/openpilotgcs/src/libs/utils/pathlisteditor.h index edd3dbbcb..8bdec3c01 100644 --- a/ground/openpilotgcs/src/libs/utils/pathlisteditor.h +++ b/ground/openpilotgcs/src/libs/utils/pathlisteditor.h @@ -4,25 +4,25 @@ * @file pathlisteditor.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,7 +39,6 @@ class QAction; QT_END_NAMESPACE namespace Utils { - struct PathListEditorPrivate; /** @@ -53,11 +52,9 @@ struct PathListEditorPrivate; * by new line characters for convenience. */ -class QTCREATOR_UTILS_EXPORT PathListEditor : public QWidget -{ +class QTCREATOR_UTILS_EXPORT PathListEditor : public QWidget { Q_DISABLE_COPY(PathListEditor) - Q_OBJECT - Q_PROPERTY(QStringList pathList READ pathList WRITE setPathList DESIGNABLE true) + Q_OBJECT Q_PROPERTY(QStringList pathList READ pathList WRITE setPathList DESIGNABLE true) Q_PROPERTY(QString fileDialogTitle READ fileDialogTitle WRITE setFileDialogTitle DESIGNABLE true) public: @@ -83,8 +80,8 @@ public slots: protected: // Index after which to insert further "Add" actions static int lastAddActionIndex(); - QAction *insertAction(int index /* -1 */, const QString &text, QObject * receiver, const char *slotFunc); - QAction *addAction(const QString &text, QObject * receiver, const char *slotFunc); + QAction *insertAction(int index /* -1 */, const QString &text, QObject *receiver, const char *slotFunc); + QAction *addAction(const QString &text, QObject *receiver, const char *slotFunc); QString text() const; void setText(const QString &); @@ -101,7 +98,6 @@ private slots: private: PathListEditorPrivate *m_d; }; - } // namespace Utils #endif // PATHLISTEDITOR_H diff --git a/ground/openpilotgcs/src/libs/utils/pathutils.cpp b/ground/openpilotgcs/src/libs/utils/pathutils.cpp index 3c06cd497..f3cc27344 100644 --- a/ground/openpilotgcs/src/libs/utils/pathutils.cpp +++ b/ground/openpilotgcs/src/libs/utils/pathutils.cpp @@ -32,112 +32,111 @@ namespace Utils { +PathUtils::PathUtils() +{} - PathUtils::PathUtils() - { +/** + Returns the base path of the share directory. - } - - /** - Returns the base path of the share directory. - - Path is in Qt/Unix conventions, separated by "/". - */ + Path is in Qt/Unix conventions, separated by "/". + */ QString PathUtils::GetDataPath() { // This routine works with "/" as the standard: // Figure out root: Up one from 'bin' QDir rootDir = QApplication::applicationDirPath(); + rootDir.cdUp(); const QString rootDirPath = rootDir.canonicalPath(); QString dataPath = rootDirPath; dataPath += QLatin1Char('/'); dataPath += QLatin1String(GCS_DATA_BASENAME); dataPath += QLatin1Char('/'); - return dataPath; + return dataPath; } /** - Cuts the standard data path from the 'path' argument. If path does not start -with the standard data path, then do nothing. + Cuts the standard data path from the 'path' argument. If path does not start + with the standard data path, then do nothing. Always returns a path converted to "/". - */ + */ QString PathUtils::RemoveDataPath(QString path) { // Depending on the platform, we might get either "/" or "\" // so we need to go to the standard ("/") QString goodPath = QDir::fromNativeSeparators(path); + if (goodPath.startsWith(GetDataPath())) { - int i = goodPath.length()- GetDataPath().length(); + int i = goodPath.length() - GetDataPath().length(); return QString("%%DATAPATH%%") + goodPath.right(i); - } else + } else { return goodPath; + } } /** - Inserts the data path (only if the path starts with %%DATAPATH%%) + Inserts the data path (only if the path starts with %%DATAPATH%%) - Returns a "/" or "\" separated path depending on platform conventions. - */ + Returns a "/" or "\" separated path depending on platform conventions. + */ QString PathUtils::InsertDataPath(QString path) { - if (path.startsWith(QString("%%DATAPATH%%"))) - { + if (path.startsWith(QString("%%DATAPATH%%"))) { QString newPath = GetDataPath(); - newPath += path.right(path.length()-12); + newPath += path.right(path.length() - 12); return QDir::toNativeSeparators(newPath); } return QDir::toNativeSeparators(path); } /** - Gets a standard user-writable location for the system - */ + Gets a standard user-writable location for the system + */ QString PathUtils::GetStoragePath() { // This routine works with "/" as the standard: // Work out where the settings are stored on the machine - QSettings set(XmlConfig::XmlSettingsFormat, QSettings::UserScope,QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS_config")); + QSettings set(XmlConfig::XmlSettingsFormat, QSettings::UserScope, QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS_config")); QFileInfo f(set.fileName()); QDir dir(f.absoluteDir()); const QString homeDirPath = dir.canonicalPath(); QString storagePath = homeDirPath; + storagePath += QLatin1Char('/'); // storagePath += QLatin1String("OpenPilot"); // storagePath += QLatin1Char('/'); - return storagePath; + return storagePath; } /** - Removes the standard storage path and replace with a tag - */ + Removes the standard storage path and replace with a tag + */ QString PathUtils::RemoveStoragePath(QString path) { // Depending on the platform, we might get either "/" or "\" // so we need to go to the standard ("/") QString goodPath = QDir::fromNativeSeparators(path); + if (goodPath.startsWith(GetStoragePath())) { - int i = goodPath.length()- GetStoragePath().length(); + int i = goodPath.length() - GetStoragePath().length(); return QString("%%STOREPATH%%") + goodPath.right(i); - } else + } else { return goodPath; + } } /** - Inserts the standard storage path is there is a storage path tag - */ + Inserts the standard storage path is there is a storage path tag + */ QString PathUtils::InsertStoragePath(QString path) { - if (path.startsWith(QString("%%STOREPATH%%"))) - { + if (path.startsWith(QString("%%STOREPATH%%"))) { QString newPath = GetStoragePath(); - newPath += path.right(path.length()-13); + newPath += path.right(path.length() - 13); return QDir::toNativeSeparators(newPath); } return QDir::toNativeSeparators(path); - } - } diff --git a/ground/openpilotgcs/src/libs/utils/pathutils.h b/ground/openpilotgcs/src/libs/utils/pathutils.h index 6cd8b36c3..0a0b82f09 100644 --- a/ground/openpilotgcs/src/libs/utils/pathutils.h +++ b/ground/openpilotgcs/src/libs/utils/pathutils.h @@ -36,9 +36,7 @@ #include namespace Utils { - -class QTCREATOR_UTILS_EXPORT PathUtils -{ +class QTCREATOR_UTILS_EXPORT PathUtils { public: PathUtils(); QString GetDataPath(); @@ -48,9 +46,7 @@ public: QString GetStoragePath(); QString RemoveStoragePath(QString path); QString InsertStoragePath(QString path); - }; - } #endif /* PATHUTILS_H */ diff --git a/ground/openpilotgcs/src/libs/utils/projectintropage.cpp b/ground/openpilotgcs/src/libs/utils/projectintropage.cpp index 93be86a8c..2eafc29d9 100644 --- a/ground/openpilotgcs/src/libs/utils/projectintropage.cpp +++ b/ground/openpilotgcs/src/libs/utils/projectintropage.cpp @@ -4,25 +4,25 @@ * @file projectintropage.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include namespace Utils { - -struct ProjectIntroPagePrivate -{ +struct ProjectIntroPagePrivate { ProjectIntroPagePrivate(); Ui::ProjectIntroPage m_ui; bool m_complete; @@ -47,13 +45,12 @@ struct ProjectIntroPagePrivate const QString m_hintStyleSheet; }; -ProjectIntroPagePrivate:: ProjectIntroPagePrivate() : +ProjectIntroPagePrivate::ProjectIntroPagePrivate() : m_complete(false), m_errorStyleSheet(QLatin1String("background : red;")), m_warningStyleSheet(QLatin1String("background : yellow;")), m_hintStyleSheet() -{ -} +{} ProjectIntroPage::ProjectIntroPage(QWidget *parent) : QWizardPage(parent), @@ -112,6 +109,7 @@ void ProjectIntroPage::setDescription(const QString &description) void ProjectIntroPage::changeEvent(QEvent *e) { QWizardPage::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: m_d->m_ui.retranslateUi(this); @@ -140,6 +138,7 @@ bool ProjectIntroPage::validate() case BaseValidatingLineEdit::Invalid: displayStatusMessage(Error, m_d->m_ui.nameLineEdit->errorMessage()); return false; + case BaseValidatingLineEdit::DisplayingInitialText: break; case BaseValidatingLineEdit::Valid: @@ -169,6 +168,7 @@ bool ProjectIntroPage::validate() void ProjectIntroPage::slotChanged() { const bool newComplete = validate(); + if (newComplete != m_d->m_complete) { m_d->m_complete = newComplete; emit completeChanged(); @@ -177,8 +177,9 @@ void ProjectIntroPage::slotChanged() void ProjectIntroPage::slotActivated() { - if (m_d->m_complete) + if (m_d->m_complete) { emit activated(); + } } bool ProjectIntroPage::validateProjectDirectory(const QString &name, QString *errorMessage) @@ -206,5 +207,4 @@ void ProjectIntroPage::hideStatusLabel() { displayStatusMessage(Hint, QString()); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/projectintropage.h b/ground/openpilotgcs/src/libs/utils/projectintropage.h index eee66c806..03a2495e1 100644 --- a/ground/openpilotgcs/src/libs/utils/projectintropage.h +++ b/ground/openpilotgcs/src/libs/utils/projectintropage.h @@ -4,25 +4,25 @@ * @file projectintropage.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,6 @@ #include namespace Utils { - struct ProjectIntroPagePrivate; /* Standard wizard page for a single file letting the user choose name @@ -50,13 +49,11 @@ struct ProjectIntroPagePrivate; * layout, otherwise, QWizard will squeeze it due to its strange expanding * hacks. */ -class QTCREATOR_UTILS_EXPORT ProjectIntroPage : public QWizardPage -{ - Q_OBJECT - Q_DISABLE_COPY(ProjectIntroPage) +class QTCREATOR_UTILS_EXPORT ProjectIntroPage : public QWizardPage { + Q_OBJECT Q_DISABLE_COPY(ProjectIntroPage) Q_PROPERTY(QString description READ description WRITE setPath DESIGNABLE true) Q_PROPERTY(QString path READ path WRITE setPath DESIGNABLE true) - Q_PROPERTY(QString name READ name WRITE setName DESIGNABLE true) + Q_PROPERTY(QString name READ name WRITE setName DESIGNABLE true) public: explicit ProjectIntroPage(QWidget *parent = 0); virtual ~ProjectIntroPage(); @@ -97,7 +94,6 @@ private: ProjectIntroPagePrivate *m_d; }; - } // namespace Utils #endif // PROJECTINTROPAGE_H diff --git a/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.cpp b/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.cpp index 5819c384d..56f9ebc4a 100644 --- a/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.cpp +++ b/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.cpp @@ -4,25 +4,25 @@ * @file projectnamevalidatinglineedit.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,24 +30,24 @@ #include "filenamevalidatinglineedit.h" namespace Utils { - ProjectNameValidatingLineEdit::ProjectNameValidatingLineEdit(QWidget *parent) - : BaseValidatingLineEdit(parent) -{ -} + : BaseValidatingLineEdit(parent) +{} bool ProjectNameValidatingLineEdit::validateProjectName(const QString &name, QString *errorMessage /* = 0*/) { // Validation is file name + checking for dots - if (!FileNameValidatingLineEdit::validateFileName(name, false, errorMessage)) + if (!FileNameValidatingLineEdit::validateFileName(name, false, errorMessage)) { return false; + } // We don't want dots in the directory name for some legacy Windows // reason. Since we are cross-platform, we generally disallow it. if (name.contains(QLatin1Char('.'))) { - if (errorMessage) + if (errorMessage) { *errorMessage = tr("The name must not contain the '.'-character."); - return false; + } + return false; } return true; } @@ -56,5 +56,4 @@ bool ProjectNameValidatingLineEdit::validate(const QString &value, QString *erro { return validateProjectName(value, errorMessage); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.h b/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.h index 1e587c50a..0ddde939e 100644 --- a/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.h +++ b/ground/openpilotgcs/src/libs/utils/projectnamevalidatinglineedit.h @@ -4,25 +4,25 @@ * @file projectnamevalidatinglineedit.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,11 +32,8 @@ #include "basevalidatinglineedit.h" namespace Utils { - -class QTCREATOR_UTILS_EXPORT ProjectNameValidatingLineEdit : public BaseValidatingLineEdit -{ - Q_OBJECT - Q_DISABLE_COPY(ProjectNameValidatingLineEdit) +class QTCREATOR_UTILS_EXPORT ProjectNameValidatingLineEdit : public BaseValidatingLineEdit { + Q_OBJECT Q_DISABLE_COPY(ProjectNameValidatingLineEdit) public: explicit ProjectNameValidatingLineEdit(QWidget *parent = 0); @@ -46,7 +43,6 @@ public: protected: virtual bool validate(const QString &value, QString *errorMessage) const; }; - } // namespace Utils #endif // PROJECTNAMEVALIDATINGLINEEDIT_H diff --git a/ground/openpilotgcs/src/libs/utils/qtcassert.h b/ground/openpilotgcs/src/libs/utils/qtcassert.h index 01b10345b..e793fb268 100644 --- a/ground/openpilotgcs/src/libs/utils/qtcassert.h +++ b/ground/openpilotgcs/src/libs/utils/qtcassert.h @@ -4,25 +4,25 @@ * @file qtcassert.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,13 +32,13 @@ #include #define QTC_ASSERT_STRINGIFY_INTERNAL(x) #x -#define QTC_ASSERT_STRINGIFY(x) QTC_ASSERT_STRINGIFY_INTERNAL(x) +#define QTC_ASSERT_STRINGIFY(x) QTC_ASSERT_STRINGIFY_INTERNAL(x) // we do not use the 'do {...} while (0)' idiom here to be able to use // 'break' and 'continue' as 'actions'. #define QTC_ASSERT(cond, action) \ - if(cond){}else{qDebug()<<"ASSERTION " #cond " FAILED AT " __FILE__ ":" QTC_ASSERT_STRINGIFY(__LINE__);action;} + if (cond) {} \ + else { qDebug() << "ASSERTION " #cond " FAILED AT " __FILE__ ":" QTC_ASSERT_STRINGIFY(__LINE__); action; } #endif // QTC_ASSERT_H - diff --git a/ground/openpilotgcs/src/libs/utils/qtcolorbutton.cpp b/ground/openpilotgcs/src/libs/utils/qtcolorbutton.cpp index aab8d71c0..d05368ca6 100644 --- a/ground/openpilotgcs/src/libs/utils/qtcolorbutton.cpp +++ b/ground/openpilotgcs/src/libs/utils/qtcolorbutton.cpp @@ -4,25 +4,25 @@ * @file qtcolorbutton.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include namespace Utils { - -class QtColorButtonPrivate -{ +class QtColorButtonPrivate { QtColorButton *q_ptr; Q_DECLARE_PUBLIC(QtColorButton) public: @@ -58,19 +56,23 @@ public: void QtColorButtonPrivate::slotEditColor() { QColor newColor; + if (m_alphaAllowed) { bool ok; const QRgb rgba = QColorDialog::getRgba(m_color.rgba(), &ok, q_ptr); - if (!ok) + if (!ok) { return; + } newColor = QColor::fromRgba(rgba); } else { newColor = QColorDialog::getColor(m_color, q_ptr); - if (!newColor.isValid()) + if (!newColor.isValid()) { return; + } } - if (newColor == q_ptr->color()) + if (newColor == q_ptr->color()) { return; + } q_ptr->setColor(newColor); emit q_ptr->colorChanged(m_color); } @@ -78,8 +80,9 @@ void QtColorButtonPrivate::slotEditColor() QColor QtColorButtonPrivate::shownColor() const { #ifndef QT_NO_DRAGANDDROP - if (m_dragging) + if (m_dragging) { return m_dragColor; + } #endif return m_color; } @@ -93,6 +96,7 @@ QPixmap QtColorButtonPrivate::generatePixmap() const QPixmap pm(2 * pixSize, 2 * pixSize); QPainter pmp(&pm); + pmp.fillRect(0, 0, pixSize, pixSize, Qt::lightGray); pmp.fillRect(pixSize, pixSize, pixSize, pixSize, Qt::lightGray); pmp.fillRect(0, pixSize, pixSize, pixSize, Qt::darkGray); @@ -102,7 +106,7 @@ QPixmap QtColorButtonPrivate::generatePixmap() const QPainter p(&pix); int corr = 1; - QRect r = pix.rect().adjusted(corr, corr, -corr, -corr); + QRect r = pix.rect().adjusted(corr, corr, -corr, -corr); p.setBrushOrigin((r.width() % pixSize + pixSize) / 2 + corr, (r.height() % pixSize + pixSize) / 2 + corr); p.fillRect(r, br); @@ -121,7 +125,7 @@ QtColorButton::QtColorButton(QWidget *parent) { d_ptr = new QtColorButtonPrivate; d_ptr->q_ptr = this; - d_ptr->m_dragging = false; + d_ptr->m_dragging = false; d_ptr->m_backgroundCheckered = true; d_ptr->m_alphaAllowed = true; @@ -138,8 +142,9 @@ QtColorButton::~QtColorButton() void QtColorButton::setColor(const QColor &color) { - if (d_ptr->m_color == color) + if (d_ptr->m_color == color) { return; + } d_ptr->m_color = color; update(); } @@ -151,8 +156,9 @@ QColor QtColorButton::color() const void QtColorButton::setBackgroundCheckered(bool checkered) { - if (d_ptr->m_backgroundCheckered == checkered) + if (d_ptr->m_backgroundCheckered == checkered) { return; + } d_ptr->m_backgroundCheckered = checkered; update(); } @@ -175,8 +181,10 @@ bool QtColorButton::isAlphaAllowed() const void QtColorButton::paintEvent(QPaintEvent *event) { QToolButton::paintEvent(event); - if (!isEnabled()) + + if (!isEnabled()) { return; + } const int pixSize = 10; QBrush br(d_ptr->shownColor()); @@ -197,21 +205,21 @@ void QtColorButton::paintEvent(QPaintEvent *event) p.setBrushOrigin((r.width() % pixSize + pixSize) / 2 + corr, (r.height() % pixSize + pixSize) / 2 + corr); p.fillRect(r, br); - //const int adjX = qRound(r.width() / 4.0); - //const int adjY = qRound(r.height() / 4.0); - //p.fillRect(r.adjusted(adjX, adjY, -adjX, -adjY), - // QColor(d_ptr->shownColor().rgb())); + // const int adjX = qRound(r.width() / 4.0); + // const int adjY = qRound(r.height() / 4.0); + // p.fillRect(r.adjusted(adjX, adjY, -adjX, -adjY), + // QColor(d_ptr->shownColor().rgb())); /* - p.fillRect(r.adjusted(0, r.height() * 3 / 4, 0, 0), + p.fillRect(r.adjusted(0, r.height() * 3 / 4, 0, 0), QColor(d_ptr->shownColor().rgb())); - p.fillRect(r.adjusted(0, 0, 0, -r.height() * 3 / 4), + p.fillRect(r.adjusted(0, 0, 0, -r.height() * 3 / 4), QColor(d_ptr->shownColor().rgb())); - */ + */ /* - const QColor frameColor0(0, 0, 0, qRound(0.2 * (0xFF - d_ptr->shownColor().alpha()))); - p.setPen(frameColor0); - p.drawRect(r.adjusted(adjX, adjY, -adjX - 1, -adjY - 1)); - */ + const QColor frameColor0(0, 0, 0, qRound(0.2 * (0xFF - d_ptr->shownColor().alpha()))); + p.setPen(frameColor0); + p.drawRect(r.adjusted(adjX, adjY, -adjX - 1, -adjY - 1)); + */ const QColor frameColor1(0, 0, 0, 26); p.setPen(frameColor1); @@ -224,8 +232,9 @@ void QtColorButton::paintEvent(QPaintEvent *event) void QtColorButton::mousePressEvent(QMouseEvent *event) { #ifndef QT_NO_DRAGANDDROP - if (event->button() == Qt::LeftButton) + if (event->button() == Qt::LeftButton) { d_ptr->m_dragStart = event->pos(); + } #endif QToolButton::mousePressEvent(event); } @@ -234,7 +243,7 @@ void QtColorButton::mouseMoveEvent(QMouseEvent *event) { #ifndef QT_NO_DRAGANDDROP if (event->buttons() & Qt::LeftButton && - (d_ptr->m_dragStart - event->pos()).manhattanLength() > QApplication::startDragDistance()) { + (d_ptr->m_dragStart - event->pos()).manhattanLength() > QApplication::startDragDistance()) { QMimeData *mime = new QMimeData; mime->setColorData(color()); QDrag *drg = new QDrag(this); @@ -253,12 +262,14 @@ void QtColorButton::mouseMoveEvent(QMouseEvent *event) void QtColorButton::dragEnterEvent(QDragEnterEvent *event) { const QMimeData *mime = event->mimeData(); - if (!mime->hasColor()) + + if (!mime->hasColor()) { return; + } event->accept(); d_ptr->m_dragColor = qvariant_cast(mime->colorData()); - d_ptr->m_dragging = true; + d_ptr->m_dragging = true; update(); } @@ -273,13 +284,13 @@ void QtColorButton::dropEvent(QDropEvent *event) { event->accept(); d_ptr->m_dragging = false; - if (d_ptr->m_dragColor == color()) + if (d_ptr->m_dragColor == color()) { return; + } setColor(d_ptr->m_dragColor); emit colorChanged(color()); } -#endif - +#endif // ifndef QT_NO_DRAGANDDROP } // namespace Utils #include "moc_qtcolorbutton.cpp" diff --git a/ground/openpilotgcs/src/libs/utils/qtcolorbutton.h b/ground/openpilotgcs/src/libs/utils/qtcolorbutton.h index 6dd5f5068..8773bcd59 100644 --- a/ground/openpilotgcs/src/libs/utils/qtcolorbutton.h +++ b/ground/openpilotgcs/src/libs/utils/qtcolorbutton.h @@ -4,25 +4,25 @@ * @file qtcolorbutton.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,11 +34,8 @@ #include namespace Utils { - -class QTCREATOR_UTILS_EXPORT QtColorButton : public QToolButton -{ - Q_OBJECT - Q_PROPERTY(bool backgroundCheckered READ isBackgroundCheckered WRITE setBackgroundCheckered) +class QTCREATOR_UTILS_EXPORT QtColorButton : public QToolButton { + Q_OBJECT Q_PROPERTY(bool backgroundCheckered READ isBackgroundCheckered WRITE setBackgroundCheckered) Q_PROPERTY(bool alphaAllowed READ isAlphaAllowed WRITE setAlphaAllowed) public: QtColorButton(QWidget *parent = 0); @@ -72,7 +69,6 @@ private: Q_DISABLE_COPY(QtColorButton) Q_PRIVATE_SLOT(d_ptr, void slotEditColor()) }; - } // namespace Utils #endif // QTCOLORBUTTON_H diff --git a/ground/openpilotgcs/src/libs/utils/qwineventnotifier_p.h b/ground/openpilotgcs/src/libs/utils/qwineventnotifier_p.h index 459cd6730..1db97c6bc 100644 --- a/ground/openpilotgcs/src/libs/utils/qwineventnotifier_p.h +++ b/ground/openpilotgcs/src/libs/utils/qwineventnotifier_p.h @@ -43,8 +43,8 @@ #define QWINEVENTNOTIFIER_P_H // -// W A R N I N G -// ------------- +// W A R N I N G +// ------------- // // This file is not part of the Qt API. It exists for the convenience // of other Qt classes. This header file may change from version to @@ -58,10 +58,9 @@ QT_BEGIN_NAMESPACE -class Q_CORE_EXPORT QWinEventNotifier : public QObject -{ +class Q_CORE_EXPORT QWinEventNotifier : public QObject { Q_OBJECT - Q_DECLARE_PRIVATE(QObject) + Q_DECLARE_PRIVATE(QObject) public: explicit QWinEventNotifier(QObject *parent = 0); @@ -80,7 +79,7 @@ Q_SIGNALS: void activated(HANDLE hEvent); protected: - bool event(QEvent * e); + bool event(QEvent *e); private: Q_DISABLE_COPY(QWinEventNotifier) diff --git a/ground/openpilotgcs/src/libs/utils/reloadpromptutils.cpp b/ground/openpilotgcs/src/libs/utils/reloadpromptutils.cpp index 80b237d6b..c7532f7c6 100644 --- a/ground/openpilotgcs/src/libs/utils/reloadpromptutils.cpp +++ b/ground/openpilotgcs/src/libs/utils/reloadpromptutils.cpp @@ -4,25 +4,25 @@ * @file reloadpromptutils.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,34 +34,35 @@ using namespace Utils; -QTCREATOR_UTILS_EXPORT Utils::ReloadPromptAnswer - Utils::reloadPrompt(const QString &fileName, bool modified, QWidget *parent) +QTCREATOR_UTILS_EXPORT Utils::ReloadPromptAnswer Utils::reloadPrompt(const QString &fileName, bool modified, QWidget *parent) { - const QString title = QCoreApplication::translate("Utils::reloadPrompt", "File Changed"); QString msg; - if (modified) + if (modified) { msg = QCoreApplication::translate("Utils::reloadPrompt", "The unsaved file %1 has been changed outside Qt Creator. Do you want to reload it and discard your changes?").arg(QDir::toNativeSeparators(fileName)); - else + } else { msg = QCoreApplication::translate("Utils::reloadPrompt", "The file %1 has changed outside Qt Creator. Do you want to reload it?").arg(QDir::toNativeSeparators(fileName)); + } return reloadPrompt(title, msg, parent); } -QTCREATOR_UTILS_EXPORT Utils::ReloadPromptAnswer - Utils::reloadPrompt(const QString &title, const QString &prompt, QWidget *parent) +QTCREATOR_UTILS_EXPORT Utils::ReloadPromptAnswer Utils::reloadPrompt(const QString &title, const QString &prompt, QWidget *parent) { switch (QMessageBox::question(parent, title, prompt, - QMessageBox::Yes|QMessageBox::YesToAll|QMessageBox::No|QMessageBox::NoToAll, + QMessageBox::Yes | QMessageBox::YesToAll | QMessageBox::No | QMessageBox::NoToAll, QMessageBox::YesToAll)) { case QMessageBox::Yes: - return ReloadCurrent; + return ReloadCurrent; + case QMessageBox::YesToAll: return ReloadAll; + case QMessageBox::No: return ReloadSkipCurrent; + default: break; } diff --git a/ground/openpilotgcs/src/libs/utils/reloadpromptutils.h b/ground/openpilotgcs/src/libs/utils/reloadpromptutils.h index 4c6e328d4..e5bd2f54c 100644 --- a/ground/openpilotgcs/src/libs/utils/reloadpromptutils.h +++ b/ground/openpilotgcs/src/libs/utils/reloadpromptutils.h @@ -4,25 +4,25 @@ * @file reloadpromptutils.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,12 +37,10 @@ class QWidget; QT_END_NAMESPACE namespace Utils { - enum ReloadPromptAnswer { ReloadCurrent, ReloadAll, ReloadSkipCurrent, ReloadNone }; QTCREATOR_UTILS_EXPORT ReloadPromptAnswer reloadPrompt(const QString &fileName, bool modified, QWidget *parent); QTCREATOR_UTILS_EXPORT ReloadPromptAnswer reloadPrompt(const QString &title, const QString &prompt, QWidget *parent); - } // namespace Utils #endif // RELOADPROMPTUTILS_H diff --git a/ground/openpilotgcs/src/libs/utils/savedaction.cpp b/ground/openpilotgcs/src/libs/utils/savedaction.cpp index b3b49fc54..b0e551896 100644 --- a/ground/openpilotgcs/src/libs/utils/savedaction.cpp +++ b/ground/openpilotgcs/src/libs/utils/savedaction.cpp @@ -4,25 +4,25 @@ * @file savedaction.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -54,16 +54,16 @@ using namespace Utils; /*! \class Utils::SavedAction - + \brief The SavedAction class is a helper class for actions with persistent state. \ingroup utils -*/ + */ SavedAction::SavedAction(QObject *parent) - : QAction(parent) + : QAction(parent) { m_widget = 0; connect(this, SIGNAL(triggered(bool)), this, SLOT(actionTriggered(bool))); @@ -74,7 +74,7 @@ SavedAction::SavedAction(QObject *parent) Returns the current value of the object. \sa setValue() -*/ + */ QVariant SavedAction::value() const { return m_value; @@ -86,16 +86,19 @@ QVariant SavedAction::value() const \a doemit is true, the \c valueChanged() signal will be emitted. \sa value() -*/ + */ void SavedAction::setValue(const QVariant &value, bool doemit) { - if (value == m_value) + if (value == m_value) { return; + } m_value = value; - if (this->isCheckable()) + if (this->isCheckable()) { this->setChecked(m_value.toBool()); - if (doemit) + } + if (doemit) { emit valueChanged(m_value); + } } @@ -104,7 +107,7 @@ void SavedAction::setValue(const QVariant &value, bool doemit) in the settings. \sa setDefaultValue() -*/ + */ QVariant SavedAction::defaultValue() const { return m_defaultValue; @@ -116,7 +119,7 @@ QVariant SavedAction::defaultValue() const in the settings. \sa defaultValue() -*/ + */ void SavedAction::setDefaultValue(const QVariant &value) { m_defaultValue = value; @@ -127,7 +130,7 @@ void SavedAction::setDefaultValue(const QVariant &value) Returns the key to be used when accessing the settings. \sa settingsKey() -*/ + */ QString SavedAction::settingsKey() const { return m_settingsKey; @@ -138,7 +141,7 @@ QString SavedAction::settingsKey() const Sets the key to be used when accessing the settings. \sa settingsKey() -*/ + */ void SavedAction::setSettingsKey(const QString &key) { m_settingsKey = key; @@ -149,10 +152,10 @@ void SavedAction::setSettingsKey(const QString &key) Sets the key and group to be used when accessing the settings. \sa settingsKey() -*/ + */ void SavedAction::setSettingsKey(const QString &group, const QString &key) { - m_settingsKey = key; + m_settingsKey = key; m_settingsGroup = group; } @@ -161,7 +164,7 @@ void SavedAction::setSettingsKey(const QString &group, const QString &key) Sets the key to be used when accessing the settings. \sa settingsKey() -*/ + */ QString SavedAction::settingsGroup() const { return m_settingsGroup; @@ -171,7 +174,7 @@ QString SavedAction::settingsGroup() const Sets the group to be used when accessing the settings. \sa settingsGroup() -*/ + */ void SavedAction::setSettingsGroup(const QString &group) { m_settingsGroup = group; @@ -190,9 +193,9 @@ void SavedAction::setTextPattern(const QString &value) QString SavedAction::toString() const { return QLatin1String("value: ") + m_value.toString() - + QLatin1String(" defaultvalue: ") + m_defaultValue.toString() - + QLatin1String(" settingskey: ") + m_settingsGroup - + '/' + m_settingsKey; + + QLatin1String(" defaultvalue: ") + m_defaultValue.toString() + + QLatin1String(" settingskey: ") + m_settingsGroup + + '/' + m_settingsKey; } /*! @@ -210,14 +213,15 @@ QString SavedAction::toString() const with the "%1" placeholder removed will be used. \sa textPattern(), setTextPattern() -*/ + */ QAction *SavedAction::updatedAction(const QString &text0) { QString text = text0; bool enabled = true; + if (!m_textPattern.isEmpty()) { if (text.isEmpty()) { - text = m_textPattern; + text = m_textPattern; text.remove("\"%1\""); text.remove("%1"); enabled = false; @@ -232,94 +236,98 @@ QAction *SavedAction::updatedAction(const QString &text0) } /* - Uses \c settingsGroup() and \c settingsKey() to restore the + Uses \c settingsGroup() and \c settingsKey() to restore the item from \a settings, \sa settingsKey(), settingsGroup(), writeSettings() -*/ + */ void SavedAction::readSettings(QSettings *settings) { - if (m_settingsGroup.isEmpty() || m_settingsKey.isEmpty()) + if (m_settingsGroup.isEmpty() || m_settingsKey.isEmpty()) { return; + } settings->beginGroup(m_settingsGroup); QVariant var = settings->value(m_settingsKey, m_defaultValue); // work around old ini files containing @Invalid() entries - if (isCheckable() && !var.isValid()) + if (isCheckable() && !var.isValid()) { var = false; + } setValue(var); - //qDebug() << "READING: " << var.isValid() << m_settingsKey << " -> " << m_value - // << " (default: " << m_defaultValue << ")" << var; + // qDebug() << "READING: " << var.isValid() << m_settingsKey << " -> " << m_value + // << " (default: " << m_defaultValue << ")" << var; settings->endGroup(); } /* - Uses \c settingsGroup() and \c settingsKey() to write the + Uses \c settingsGroup() and \c settingsKey() to write the item to \a settings, \sa settingsKey(), settingsGroup(), readSettings() -*/ + */ void SavedAction::writeSettings(QSettings *settings) { - if (m_settingsGroup.isEmpty() || m_settingsKey.isEmpty()) + if (m_settingsGroup.isEmpty() || m_settingsKey.isEmpty()) { return; + } settings->beginGroup(m_settingsGroup); settings->setValue(m_settingsKey, m_value); - //qDebug() << "WRITING: " << m_settingsKey << " -> " << toString(); + // qDebug() << "WRITING: " << m_settingsKey << " -> " << toString(); settings->endGroup(); } - + /* A \c SavedAction can be connected to a widget, typically a checkbox, radiobutton, or a lineedit in some configuration dialog. - The widget will retrieve its contents from the SavedAction's + The widget will retrieve its contents from the SavedAction's value, and - depending on the \a ApplyMode - either write changes back immediately, or when \s SavedAction::apply() is called explicitly. \sa apply(), disconnectWidget() -*/ + */ void SavedAction::connectWidget(QWidget *widget, ApplyMode applyMode) { QTC_ASSERT(!m_widget, - qDebug() << "ALREADY CONNECTED: " << widget << m_widget << toString(); return); - m_widget = widget; + qDebug() << "ALREADY CONNECTED: " << widget << m_widget << toString(); return ); + m_widget = widget; m_applyMode = applyMode; - - if (QAbstractButton *button = qobject_cast(widget)) { + + if (QAbstractButton * button = qobject_cast(widget)) { if (button->isCheckable()) { button->setChecked(m_value.toBool()); connect(button, SIGNAL(clicked(bool)), - this, SLOT(checkableButtonClicked(bool))); + this, SLOT(checkableButtonClicked(bool))); } else { connect(button, SIGNAL(clicked()), - this, SLOT(uncheckableButtonClicked())); + this, SLOT(uncheckableButtonClicked())); } - } else if (QSpinBox *spinBox = qobject_cast(widget)) { + } else if (QSpinBox * spinBox = qobject_cast(widget)) { spinBox->setValue(m_value.toInt()); - //qDebug() << "SETTING VALUE" << spinBox->value(); + + // qDebug() << "SETTING VALUE" << spinBox->value(); connect(spinBox, SIGNAL(valueChanged(int)), - this, SLOT(spinBoxValueChanged(int))); + this, SLOT(spinBoxValueChanged(int))); connect(spinBox, SIGNAL(valueChanged(QString)), - this, SLOT(spinBoxValueChanged(QString))); - } else if (QDoubleSpinBox *doubleSpinBox = qobject_cast(widget)) { + this, SLOT(spinBoxValueChanged(QString))); + } else if (QDoubleSpinBox * doubleSpinBox = qobject_cast(widget)) { doubleSpinBox->setValue(m_value.toDouble()); - //qDebug() << "SETTING VALUE" << doubleSpinBox->value(); + // qDebug() << "SETTING VALUE" << doubleSpinBox->value(); connect(doubleSpinBox, SIGNAL(valueChanged(double)), - this, SLOT(doubleSpinBoxValueChanged(double))); + this, SLOT(doubleSpinBoxValueChanged(double))); connect(doubleSpinBox, SIGNAL(valueChanged(QString)), - this, SLOT(doubleSpinBoxValueChanged(QString))); - } else if (QLineEdit *lineEdit = qobject_cast(widget)) { + this, SLOT(doubleSpinBoxValueChanged(QString))); + } else if (QLineEdit * lineEdit = qobject_cast(widget)) { lineEdit->setText(m_value.toString()); - //qDebug() << "SETTING TEXT" << lineEdit->text(); + // qDebug() << "SETTING TEXT" << lineEdit->text(); connect(lineEdit, SIGNAL(editingFinished()), - this, SLOT(lineEditEditingFinished())); - } else if (PathChooser *pathChooser = qobject_cast(widget)) { + this, SLOT(lineEditEditingFinished())); + } else if (PathChooser * pathChooser = qobject_cast(widget)) { pathChooser->setPath(m_value.toString()); connect(pathChooser, SIGNAL(editingFinished()), - this, SLOT(pathChooserEditingFinished())); + this, SLOT(pathChooserEditingFinished())); connect(pathChooser, SIGNAL(browsingFinished()), - this, SLOT(pathChooserEditingFinished())); + this, SLOT(pathChooserEditingFinished())); } else { qDebug() << "Cannot connect widget " << widget << toString(); } @@ -329,7 +337,7 @@ void SavedAction::connectWidget(QWidget *widget, ApplyMode applyMode) Disconnects the \c SavedAction from a widget. \sa apply(), connectWidget() -*/ + */ void SavedAction::disconnectWidget() { m_widget = 0; @@ -337,94 +345,113 @@ void SavedAction::disconnectWidget() void SavedAction::apply(QSettings *s) { - if (QAbstractButton *button = qobject_cast(m_widget)) + if (QAbstractButton * button = qobject_cast(m_widget)) { setValue(button->isChecked()); - else if (QLineEdit *lineEdit = qobject_cast(m_widget)) + } else if (QLineEdit * lineEdit = qobject_cast(m_widget)) { setValue(lineEdit->text()); - else if (QSpinBox *spinBox = qobject_cast(m_widget)) + } else if (QSpinBox * spinBox = qobject_cast(m_widget)) { setValue(spinBox->value()); - else if (QDoubleSpinBox *doubleSpinBox = qobject_cast(m_widget)) + } else if (QDoubleSpinBox * doubleSpinBox = qobject_cast(m_widget)) { setValue(doubleSpinBox->value()); - else if (PathChooser *pathChooser = qobject_cast(m_widget)) + } else if (PathChooser * pathChooser = qobject_cast(m_widget)) { setValue(pathChooser->path()); - if (s) - writeSettings(s); + } + if (s) { + writeSettings(s); + } } void SavedAction::uncheckableButtonClicked() { QAbstractButton *button = qobject_cast(sender()); - QTC_ASSERT(button, return); - //qDebug() << "UNCHECKABLE BUTTON: " << sender(); + + QTC_ASSERT(button, return ); + // qDebug() << "UNCHECKABLE BUTTON: " << sender(); QAction::trigger(); } void SavedAction::checkableButtonClicked(bool) { QAbstractButton *button = qobject_cast(sender()); - QTC_ASSERT(button, return); - //qDebug() << "CHECKABLE BUTTON: " << sender(); - if (m_applyMode == ImmediateApply) + + QTC_ASSERT(button, return ); + // qDebug() << "CHECKABLE BUTTON: " << sender(); + if (m_applyMode == ImmediateApply) { setValue(button->isChecked()); + } } void SavedAction::lineEditEditingFinished() { QLineEdit *lineEdit = qobject_cast(sender()); - QTC_ASSERT(lineEdit, return); - if (m_applyMode == ImmediateApply) + + QTC_ASSERT(lineEdit, return ); + if (m_applyMode == ImmediateApply) { setValue(lineEdit->text()); + } } void SavedAction::spinBoxValueChanged(int value) { QSpinBox *spinBox = qobject_cast(sender()); - QTC_ASSERT(spinBox, return); - if (m_applyMode == ImmediateApply) + + QTC_ASSERT(spinBox, return ); + if (m_applyMode == ImmediateApply) { setValue(value); + } } void SavedAction::spinBoxValueChanged(QString value) { QSpinBox *spinBox = qobject_cast(sender()); - QTC_ASSERT(spinBox, return); - if (m_applyMode == ImmediateApply) + + QTC_ASSERT(spinBox, return ); + if (m_applyMode == ImmediateApply) { setValue(value); + } } void SavedAction::doubleSpinBoxValueChanged(double value) { QDoubleSpinBox *doubleSpinBox = qobject_cast(sender()); - QTC_ASSERT(doubleSpinBox, return); - if (m_applyMode == ImmediateApply) + + QTC_ASSERT(doubleSpinBox, return ); + if (m_applyMode == ImmediateApply) { setValue(value); + } } void SavedAction::doubleSpinBoxValueChanged(QString value) { QDoubleSpinBox *doubleSpinBox = qobject_cast(sender()); - QTC_ASSERT(doubleSpinBox, return); - if (m_applyMode == ImmediateApply) + + QTC_ASSERT(doubleSpinBox, return ); + if (m_applyMode == ImmediateApply) { setValue(value); + } } void SavedAction::pathChooserEditingFinished() { PathChooser *pathChooser = qobject_cast(sender()); - QTC_ASSERT(pathChooser, return); - if (m_applyMode == ImmediateApply) + + QTC_ASSERT(pathChooser, return ); + if (m_applyMode == ImmediateApply) { setValue(pathChooser->path()); + } } void SavedAction::actionTriggered(bool) { - if (isCheckable()) + if (isCheckable()) { setValue(isChecked()); + } if (actionGroup() && actionGroup()->isExclusive()) { // FIXME: should be taken care of more directly - foreach (QAction *act, actionGroup()->actions()) - if (SavedAction *dact = qobject_cast(act)) - dact->setValue(bool(act == this)); + foreach(QAction * act, actionGroup()->actions()) + if (SavedAction * dact = qobject_cast(act)) { + dact->setValue(bool(act == this)); + } } } @@ -444,19 +471,19 @@ void SavedAction::trigger(const QVariant &data) void SavedActionSet::insert(SavedAction *action, QWidget *widget) { m_list.append(action); - if (widget) + if (widget) { action->connectWidget(widget); + } } void SavedActionSet::apply(QSettings *settings) { - foreach (SavedAction *action, m_list) - action->apply(settings); + foreach(SavedAction * action, m_list) + action->apply(settings); } void SavedActionSet::finish() { - foreach (SavedAction *action, m_list) - action->disconnectWidget(); + foreach(SavedAction * action, m_list) + action->disconnectWidget(); } - diff --git a/ground/openpilotgcs/src/libs/utils/savedaction.h b/ground/openpilotgcs/src/libs/utils/savedaction.h index a3eaf0c7f..df624c637 100644 --- a/ground/openpilotgcs/src/libs/utils/savedaction.h +++ b/ground/openpilotgcs/src/libs/utils/savedaction.h @@ -4,25 +4,25 @@ * @file savedaction.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,11 +42,9 @@ class QSettings; QT_END_NAMESPACE namespace Utils { - enum ApplyMode { ImmediateApply, DeferedApply }; -class QTCREATOR_UTILS_EXPORT SavedAction : public QAction -{ +class QTCREATOR_UTILS_EXPORT SavedAction : public QAction { Q_OBJECT public: @@ -71,7 +69,7 @@ public: virtual void readSettings(QSettings *settings); Q_SLOT virtual void writeSettings(QSettings *settings); - + virtual void connectWidget(QWidget *widget, ApplyMode applyMode = DeferedApply); virtual void disconnectWidget(); Q_SLOT virtual void apply(QSettings *settings); @@ -105,8 +103,7 @@ private: ApplyMode m_applyMode; }; -class QTCREATOR_UTILS_EXPORT SavedActionSet -{ +class QTCREATOR_UTILS_EXPORT SavedActionSet { public: SavedActionSet() {} ~SavedActionSet() {} @@ -114,12 +111,14 @@ public: void insert(SavedAction *action, QWidget *widget); void apply(QSettings *settings); void finish(); - void clear() { m_list.clear(); } + void clear() + { + m_list.clear(); + } private: QList m_list; }; - } // namespace Utils #endif // SAVED_ACTION_H diff --git a/ground/openpilotgcs/src/libs/utils/settingsutils.cpp b/ground/openpilotgcs/src/libs/utils/settingsutils.cpp index 4f3180254..c10b9ab42 100644 --- a/ground/openpilotgcs/src/libs/utils/settingsutils.cpp +++ b/ground/openpilotgcs/src/libs/utils/settingsutils.cpp @@ -4,25 +4,25 @@ * @file settingsutils.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,18 +31,18 @@ #include namespace Utils { - QTCREATOR_UTILS_EXPORT QString settingsKey(const QString &category) { QString rc(category); const QChar underscore = QLatin1Char('_'); const int size = rc.size(); + for (int i = 0; i < size; i++) { const QChar c = rc.at(i); - if (!c.isLetterOrNumber() && c != underscore) + if (!c.isLetterOrNumber() && c != underscore) { rc[i] = underscore; + } } return rc; } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/settingsutils.h b/ground/openpilotgcs/src/libs/utils/settingsutils.h index 49b93cb69..be85a69e7 100644 --- a/ground/openpilotgcs/src/libs/utils/settingsutils.h +++ b/ground/openpilotgcs/src/libs/utils/settingsutils.h @@ -4,25 +4,25 @@ * @file settingsutils.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,11 +32,9 @@ #include "utils_global.h" namespace Utils { - // Create a usable settings key from a category, // for example Editor|C++ -> Editor_C__ QTCREATOR_UTILS_EXPORT QString settingsKey(const QString &category); - } // namespace Utils #endif // SETTINGSTUTILS_H diff --git a/ground/openpilotgcs/src/libs/utils/styledbar.cpp b/ground/openpilotgcs/src/libs/utils/styledbar.cpp index 63a27b776..7ceb6709b 100644 --- a/ground/openpilotgcs/src/libs/utils/styledbar.cpp +++ b/ground/openpilotgcs/src/libs/utils/styledbar.cpp @@ -4,25 +4,25 @@ * @file styledbar.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -60,7 +60,7 @@ void StyledBar::paintEvent(QPaintEvent *event) Q_UNUSED(event) QPainter painter(this); QStyleOption option; - option.rect = rect(); + option.rect = rect(); option.state = QStyle::State_Horizontal; style()->drawControl(QStyle::CE_ToolBar, &option, &painter, this); } @@ -76,8 +76,8 @@ void StyledSeparator::paintEvent(QPaintEvent *event) Q_UNUSED(event) QPainter painter(this); QStyleOption option; - option.rect = rect(); - option.state = QStyle::State_Horizontal; + option.rect = rect(); + option.state = QStyle::State_Horizontal; option.palette = palette(); style()->drawPrimitive(QStyle::PE_IndicatorToolBarSeparator, &option, &painter, this); } diff --git a/ground/openpilotgcs/src/libs/utils/styledbar.h b/ground/openpilotgcs/src/libs/utils/styledbar.h index a0144d0a9..e564eb1b6 100644 --- a/ground/openpilotgcs/src/libs/utils/styledbar.h +++ b/ground/openpilotgcs/src/libs/utils/styledbar.h @@ -4,25 +4,25 @@ * @file styledbar.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ #include namespace Utils { - -class QTCREATOR_UTILS_EXPORT StyledBar : public QWidget -{ +class QTCREATOR_UTILS_EXPORT StyledBar : public QWidget { Q_OBJECT public: StyledBar(QWidget *parent = 0); @@ -46,14 +44,12 @@ protected: void paintEvent(QPaintEvent *event); }; -class QTCREATOR_UTILS_EXPORT StyledSeparator : public QWidget -{ +class QTCREATOR_UTILS_EXPORT StyledSeparator : public QWidget { public: StyledSeparator(QWidget *parent = 0); protected: void paintEvent(QPaintEvent *event); }; - } // Utils #endif // STYLEDBAR_H diff --git a/ground/openpilotgcs/src/libs/utils/stylehelper.cpp b/ground/openpilotgcs/src/libs/utils/stylehelper.cpp index 879a0b9b3..5b22e39c5 100644 --- a/ground/openpilotgcs/src/libs/utils/stylehelper.cpp +++ b/ground/openpilotgcs/src/libs/utils/stylehelper.cpp @@ -4,25 +4,25 @@ * @file stylehelper.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,24 +39,25 @@ static int clamp(float x) { const int val = x > 255 ? 255 : static_cast(x); + return val < 0 ? 0 : val; } // Clamps float color values within (0, 255) /* -static int range(float x, int min, int max) -{ + static int range(float x, int min, int max) + { int val = x > max ? max : x; return val < min ? min : val; -} -*/ + } + */ namespace Utils { - QColor StyleHelper::mergedColors(const QColor &colorA, const QColor &colorB, int factor) { const int maxFactor = 100; QColor tmp = colorA; + tmp.setRed((tmp.red() * factor) / maxFactor + (colorB.red() * (maxFactor - factor)) / maxFactor); tmp.setGreen((tmp.green() * factor) / maxFactor + (colorB.green() * (maxFactor - factor)) / maxFactor); tmp.setBlue((tmp.blue() * factor) / maxFactor + (colorB.blue() * (maxFactor - factor)) / maxFactor); @@ -67,14 +68,17 @@ qreal StyleHelper::sidebarFontSize() { #if defined(Q_WS_MAC) return 9; + #else return 7.5; + #endif } QPalette StyleHelper::sidebarFontPalette(const QPalette &original) { QPalette palette = original; + palette.setColor(QPalette::Active, QPalette::Text, panelTextColor()); palette.setColor(QPalette::Active, QPalette::WindowText, panelTextColor()); palette.setColor(QPalette::Inactive, QPalette::Text, panelTextColor().darker()); @@ -84,7 +88,7 @@ QPalette StyleHelper::sidebarFontPalette(const QPalette &original) QColor StyleHelper::panelTextColor() { - //qApp->palette().highlightedText().color(); + // qApp->palette().highlightedText().color(); return Qt::white; } @@ -98,6 +102,7 @@ QColor StyleHelper::baseColor() QColor StyleHelper::highlightColor() { QColor result = baseColor(); + result.setHsv(result.hue(), clamp(result.saturation()), clamp(result.value() * 1.16)); @@ -107,6 +112,7 @@ QColor StyleHelper::highlightColor() QColor StyleHelper::shadowColor() { QColor result = baseColor(); + result.setHsv(result.hue(), clamp(result.saturation() * 1.1), clamp(result.value() * 0.70)); @@ -116,6 +122,7 @@ QColor StyleHelper::shadowColor() QColor StyleHelper::borderColor() { QColor result = baseColor(); + result.setHsv(result.hue(), result.saturation(), result.value() / 2); @@ -126,8 +133,8 @@ void StyleHelper::setBaseColor(const QColor &color) { if (color.isValid() && color != m_baseColor) { m_baseColor = color; - foreach (QWidget *w, QApplication::topLevelWidgets()) - w->update(); + foreach(QWidget * w, QApplication::topLevelWidgets()) + w->update(); } } @@ -135,6 +142,7 @@ static void verticalGradientHelper(QPainter *p, const QRect &spanRect, const QRe { QColor base = StyleHelper::baseColor(); QLinearGradient grad(spanRect.topRight(), spanRect.topLeft()); + grad.setColorAt(0, StyleHelper::highlightColor()); grad.setColorAt(0.301, base); grad.setColorAt(1, StyleHelper::shadowColor()); @@ -150,8 +158,8 @@ void StyleHelper::verticalGradient(QPainter *painter, const QRect &spanRect, con if (StyleHelper::usePixmapCache()) { QString key; key.sprintf("mh_vertical %d %d %d %d %d", - spanRect.width(), spanRect.height(), clipRect.width(), - clipRect.height(), StyleHelper::baseColor().rgb());; + spanRect.width(), spanRect.height(), clipRect.width(), + clipRect.height(), StyleHelper::baseColor().rgb());; QPixmap pixmap; if (!QPixmapCache::find(key, pixmap)) { @@ -170,10 +178,11 @@ void StyleHelper::verticalGradient(QPainter *painter, const QRect &spanRect, con } static void horizontalGradientHelper(QPainter *p, const QRect &spanRect, const -QRect &rect) + QRect &rect) { QColor base = StyleHelper::baseColor(); QLinearGradient grad(rect.topLeft(), rect.bottomLeft()); + grad.setColorAt(0, StyleHelper::highlightColor().lighter(120)); if (rect.height() == StyleHelper::navigationWidgetHeight()) { grad.setColorAt(0.4, StyleHelper::highlightColor()); @@ -189,7 +198,6 @@ QRect &rect) shadowGradient.setColorAt(0.7, highlight); shadowGradient.setColorAt(1, QColor(0, 0, 0, 40)); p->fillRect(rect, shadowGradient); - } void StyleHelper::horizontalGradient(QPainter *painter, const QRect &spanRect, const QRect &clipRect) @@ -197,8 +205,8 @@ void StyleHelper::horizontalGradient(QPainter *painter, const QRect &spanRect, c if (StyleHelper::usePixmapCache()) { QString key; key.sprintf("mh_horizontal %d %d %d %d %d", - spanRect.width(), spanRect.height(), clipRect.width(), - clipRect.height(), StyleHelper::baseColor().rgb()); + spanRect.width(), spanRect.height(), clipRect.width(), + clipRect.height(), StyleHelper::baseColor().rgb()); QPixmap pixmap; if (!QPixmapCache::find(key, pixmap)) { @@ -211,7 +219,6 @@ void StyleHelper::horizontalGradient(QPainter *painter, const QRect &spanRect, c } painter->drawPixmap(clipRect.topLeft(), pixmap); - } else { horizontalGradientHelper(painter, spanRect, clipRect); } @@ -221,6 +228,7 @@ static void menuGradientHelper(QPainter *p, const QRect &spanRect, const QRect & { QLinearGradient grad(spanRect.topLeft(), spanRect.bottomLeft()); QColor menuColor = StyleHelper::mergedColors(StyleHelper::baseColor(), QColor(244, 244, 244), 25); + grad.setColorAt(0, menuColor.lighter(112)); grad.setColorAt(1, menuColor); p->fillRect(rect, grad); @@ -231,8 +239,8 @@ void StyleHelper::menuGradient(QPainter *painter, const QRect &spanRect, const Q if (StyleHelper::usePixmapCache()) { QString key; key.sprintf("mh_menu %d %d %d %d %d", - spanRect.width(), spanRect.height(), clipRect.width(), - clipRect.height(), StyleHelper::baseColor().rgb()); + spanRect.width(), spanRect.height(), clipRect.width(), + clipRect.height(), StyleHelper::baseColor().rgb()); QPixmap pixmap; if (!QPixmapCache::find(key, pixmap)) { @@ -249,5 +257,4 @@ void StyleHelper::menuGradient(QPainter *painter, const QRect &spanRect, const Q menuGradientHelper(painter, spanRect, clipRect); } } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/stylehelper.h b/ground/openpilotgcs/src/libs/utils/stylehelper.h index c0973827e..fb8384555 100644 --- a/ground/openpilotgcs/src/libs/utils/stylehelper.h +++ b/ground/openpilotgcs/src/libs/utils/stylehelper.h @@ -4,25 +4,25 @@ * @file stylehelper.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,11 +42,13 @@ QT_END_NAMESPACE // Helper class holding all custom color values namespace Utils { -class QTCREATOR_UTILS_EXPORT StyleHelper -{ +class QTCREATOR_UTILS_EXPORT StyleHelper { public: // Height of the project explorer navigation bar - static int navigationWidgetHeight() { return 24; } + static int navigationWidgetHeight() + { + return 24; + } static qreal sidebarFontSize(); static QPalette sidebarFontPalette(const QPalette &original); @@ -56,7 +58,10 @@ public: static QColor highlightColor(); static QColor shadowColor(); static QColor borderColor(); - static QColor buttonTextColor() { return QColor(0x4c4c4c); } + static QColor buttonTextColor() + { + return QColor(0x4c4c4c); + } static QColor mergedColors(const QColor &colorA, const QColor &colorB, int factor = 50); // Sets the base color and makes sure all top level widgets are updated @@ -68,11 +73,13 @@ public: static void menuGradient(QPainter *painter, const QRect &spanRect, const QRect &clipRect); // Pixmap cache should only be enabled for X11 due to slow gradients - static bool usePixmapCache() { return true; } + static bool usePixmapCache() + { + return true; + } private: static QColor m_baseColor; }; - } // namespace Utils #endif // STYLEHELPER_H diff --git a/ground/openpilotgcs/src/libs/utils/submiteditorwidget.cpp b/ground/openpilotgcs/src/libs/utils/submiteditorwidget.cpp index 44cdb1d90..76131e276 100644 --- a/ground/openpilotgcs/src/libs/utils/submiteditorwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/submiteditorwidget.cpp @@ -4,25 +4,25 @@ * @file submiteditorwidget.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,11 +44,9 @@ enum { debug = 0 }; enum { defaultLineWidth = 72 }; namespace Utils { - // QActionPushButton: A push button tied to an action // (similar to a QToolButton) -class QActionPushButton : public QPushButton -{ +class QActionPushButton : public QPushButton { Q_OBJECT public: explicit QActionPushButton(QAction *a); @@ -58,7 +56,7 @@ private slots: }; QActionPushButton::QActionPushButton(QAction *a) : - QPushButton(a->icon(), a->text()) + QPushButton(a->icon(), a->text()) { connect(a, SIGNAL(changed()), this, SLOT(actionChanged())); connect(this, SIGNAL(clicked()), a, SLOT(trigger())); @@ -67,20 +65,23 @@ QActionPushButton::QActionPushButton(QAction *a) : void QActionPushButton::actionChanged() { - if (const QAction *a = qobject_cast(sender())) + if (const QAction * a = qobject_cast(sender())) { setEnabled(a->isEnabled()); + } } // Helpers to retrieve model data static inline bool listModelChecked(const QAbstractItemModel *model, int row, int column = 0) { const QModelIndex checkableIndex = model->index(row, column, QModelIndex()); + return model->data(checkableIndex, Qt::CheckStateRole).toInt() == Qt::Checked; } static inline QString listModelText(const QAbstractItemModel *model, int row, int column) { const QModelIndex index = model->index(row, column, QModelIndex()); + return model->data(index, Qt::DisplayRole).toString(); } @@ -88,9 +89,12 @@ static inline QString listModelText(const QAbstractItemModel *model, int row, in static bool listModelContainsCheckedItem(const QAbstractItemModel *model) { const int count = model->rowCount(); - for (int i = 0; i < count; i++) - if (listModelChecked(model, i, 0)) + + for (int i = 0; i < count; i++) { + if (listModelChecked(model, i, 0)) { return true; + } + } return false; } @@ -98,19 +102,21 @@ static bool listModelContainsCheckedItem(const QAbstractItemModel *model) QList selectedRows(const QAbstractItemView *view) { const QModelIndexList indexList = view->selectionModel()->selectedRows(0); - if (indexList.empty()) + + if (indexList.empty()) { return QList(); + } QList rc; const QModelIndexList::const_iterator cend = indexList.constEnd(); - for (QModelIndexList::const_iterator it = indexList.constBegin(); it != cend; ++it) + for (QModelIndexList::const_iterator it = indexList.constBegin(); it != cend; ++it) { rc.push_back(it->row()); + } return rc; } // ----------- SubmitEditorWidgetPrivate -struct SubmitEditorWidgetPrivate -{ +struct SubmitEditorWidgetPrivate { // A pair of position/action to extend context menus typedef QPair > AdditionalContextMenuAction; @@ -119,8 +125,8 @@ struct SubmitEditorWidgetPrivate Ui::SubmitEditorWidget m_ui; bool m_filesSelected; bool m_filesChecked; - int m_fileNameColumn; - int m_activatedRow; + int m_fileNameColumn; + int m_activatedRow; QList descriptionEditContextMenuActions; QVBoxLayout *m_fieldLayout; @@ -135,8 +141,7 @@ SubmitEditorWidgetPrivate::SubmitEditorWidgetPrivate() : m_activatedRow(-1), m_fieldLayout(0), m_lineWidth(defaultLineWidth) -{ -} +{} SubmitEditorWidget::SubmitEditorWidget(QWidget *parent) : QWidget(parent), @@ -165,7 +170,7 @@ SubmitEditorWidget::~SubmitEditorWidget() } void SubmitEditorWidget::registerActions(QAction *editorUndoAction, QAction *editorRedoAction, - QAction *submitAction, QAction *diffAction) + QAction *submitAction, QAction *diffAction) { if (editorUndoAction) { editorUndoAction->setEnabled(m_d->m_ui.description->document()->isUndoAvailable()); @@ -181,8 +186,9 @@ void SubmitEditorWidget::registerActions(QAction *editorUndoAction, QAction *edi if (submitAction) { if (debug) { int count = 0; - if (const QAbstractItemModel *model = m_d->m_ui.fileView->model()) + if (const QAbstractItemModel * model = m_d->m_ui.fileView->model()) { count = model->rowCount(); + } qDebug() << Q_FUNC_INFO << submitAction << count << "items" << m_d->m_filesChecked; } submitAction->setEnabled(m_d->m_filesChecked); @@ -190,8 +196,9 @@ void SubmitEditorWidget::registerActions(QAction *editorUndoAction, QAction *edi m_d->m_ui.buttonLayout->addWidget(new QActionPushButton(submitAction)); } if (diffAction) { - if (debug) + if (debug) { qDebug() << diffAction << m_d->m_filesSelected; + } diffAction->setEnabled(m_d->m_filesSelected); connect(this, SIGNAL(fileSelectionChanged(bool)), diffAction, SLOT(setEnabled(bool))); connect(diffAction, SIGNAL(triggered()), this, SLOT(triggerDiffSelected())); @@ -199,7 +206,7 @@ void SubmitEditorWidget::registerActions(QAction *editorUndoAction, QAction *edi } } -void SubmitEditorWidget::unregisterActions(QAction *editorUndoAction, QAction *editorRedoAction, +void SubmitEditorWidget::unregisterActions(QAction *editorUndoAction, QAction *editorRedoAction, QAction *submitAction, QAction *diffAction) { if (editorUndoAction) { @@ -211,12 +218,13 @@ void SubmitEditorWidget::unregisterActions(QAction *editorUndoAction, QAction * disconnect(editorRedoAction, SIGNAL(triggered()), m_d->m_ui.description, SLOT(redo())); } - if (submitAction) + if (submitAction) { disconnect(this, SIGNAL(fileCheckStateChanged(bool)), submitAction, SLOT(setEnabled(bool))); + } if (diffAction) { - disconnect(this, SIGNAL(fileSelectionChanged(bool)), diffAction, SLOT(setEnabled(bool))); - disconnect(diffAction, SIGNAL(triggered()), this, SLOT(triggerDiffSelected())); + disconnect(this, SIGNAL(fileSelectionChanged(bool)), diffAction, SLOT(setEnabled(bool))); + disconnect(diffAction, SIGNAL(triggered()), this, SLOT(triggerDiffSelected())); } } @@ -224,6 +232,7 @@ void SubmitEditorWidget::unregisterActions(QAction *editorUndoAction, QAction * static inline QString trimMessageText(const QString &t) { QString rc = t.trimmed(); + rc += QLatin1Char('\n'); return rc; } @@ -235,7 +244,8 @@ static QString wrappedText(const QTextEdit *e) const QChar newLine = QLatin1Char('\n'); QString rc; QTextCursor cursor(e->document()); - cursor.movePosition(QTextCursor::Start); + + cursor.movePosition(QTextCursor::Start); while (!cursor.atEnd()) { cursor.select(QTextCursor::LineUnderCursor); rc += cursor.selectedText(); @@ -249,9 +259,10 @@ static QString wrappedText(const QTextEdit *e) QString SubmitEditorWidget::descriptionText() const { QString rc = trimMessageText(lineWrap() ? wrappedText(m_d->m_ui.description) : m_d->m_ui.description->toPlainText()); + // append field entries - foreach(const SubmitFieldWidget *fw, m_d->m_fieldWidgets) - rc += fw->fieldValues(); + foreach(const SubmitFieldWidget * fw, m_d->m_fieldWidgets) + rc += fw->fieldValues(); return rc; } @@ -267,11 +278,12 @@ bool SubmitEditorWidget::lineWrap() const void SubmitEditorWidget::setLineWrap(bool v) { - if (debug) + if (debug) { qDebug() << Q_FUNC_INFO << v; + } if (v) { m_d->m_ui.description->setLineWrapColumnOrWidth(m_d->m_lineWidth); - m_d->m_ui.description->setLineWrapMode(QTextEdit::FixedColumnWidth); + m_d->m_ui.description->setLineWrapMode(QTextEdit::FixedColumnWidth); } else { m_d->m_ui.description->setLineWrapMode(QTextEdit::NoWrap); } @@ -284,13 +296,16 @@ int SubmitEditorWidget::lineWrapWidth() const void SubmitEditorWidget::setLineWrapWidth(int v) { - if (debug) + if (debug) { qDebug() << Q_FUNC_INFO << v << lineWrap(); - if (m_d->m_lineWidth == v) + } + if (m_d->m_lineWidth == v) { return; + } m_d->m_lineWidth = v; - if (lineWrap()) + if (lineWrap()) { m_d->m_ui.description->setLineWrapColumnOrWidth(v); + } } int SubmitEditorWidget::fileNameColumn() const @@ -321,17 +336,18 @@ void SubmitEditorWidget::setFileModel(QAbstractItemModel *model) if (model->rowCount()) { const int columnCount = model->columnCount(); - for (int c = 0; c < columnCount; c++) + for (int c = 0; c < columnCount; c++) { m_d->m_ui.fileView->resizeColumnToContents(c); + } } - connect(model, SIGNAL(dataChanged(QModelIndex,QModelIndex)), + connect(model, SIGNAL(dataChanged(QModelIndex, QModelIndex)), this, SLOT(updateSubmitAction())); connect(model, SIGNAL(modelReset()), this, SLOT(updateSubmitAction())); - connect(model, SIGNAL(rowsInserted(QModelIndex,int,int)), + connect(model, SIGNAL(rowsInserted(QModelIndex, int, int)), this, SLOT(updateSubmitAction())); - connect(model, SIGNAL(rowsRemoved(QModelIndex,int,int)), + connect(model, SIGNAL(rowsRemoved(QModelIndex, int, int)), this, SLOT(updateSubmitAction())); connect(m_d->m_ui.fileView->selectionModel(), SIGNAL(selectionChanged(QItemSelection, QItemSelection)), this, SLOT(updateDiffAction())); @@ -346,14 +362,17 @@ QAbstractItemModel *SubmitEditorWidget::fileModel() const QStringList SubmitEditorWidget::selectedFiles() const { const QList selection = selectedRows(m_d->m_ui.fileView); - if (selection.empty()) + + if (selection.empty()) { return QStringList(); + } QStringList rc; const QAbstractItemModel *model = m_d->m_ui.fileView->model(); const int count = selection.size(); - for (int i = 0; i < count; i++) + for (int i = 0; i < count; i++) { rc.push_back(listModelText(model, selection.at(i), fileNameColumn())); + } return rc; } @@ -361,12 +380,16 @@ QStringList SubmitEditorWidget::checkedFiles() const { QStringList rc; const QAbstractItemModel *model = m_d->m_ui.fileView->model(); - if (!model) + + if (!model) { return rc; + } const int count = model->rowCount(); - for (int i = 0; i < count; i++) - if (listModelChecked(model, i, 0)) + for (int i = 0; i < count; i++) { + if (listModelChecked(model, i, 0)) { rc.push_back(listModelText(model, i, fileNameColumn())); + } + } return rc; } @@ -378,8 +401,10 @@ QTextEdit *SubmitEditorWidget::descriptionEdit() const void SubmitEditorWidget::triggerDiffSelected() { const QStringList sel = selectedFiles(); - if (!sel.empty()) + + if (!sel.empty()) { emit diffSelected(sel); + } } void SubmitEditorWidget::diffActivatedDelayed() @@ -406,6 +431,7 @@ void SubmitEditorWidget::updateActions() void SubmitEditorWidget::updateSubmitAction() { const bool newFilesCheckedState = hasCheckedFiles(); + if (m_d->m_filesChecked != newFilesCheckedState) { m_d->m_filesChecked = newFilesCheckedState; emit fileCheckStateChanged(m_d->m_filesChecked); @@ -416,6 +442,7 @@ void SubmitEditorWidget::updateSubmitAction() void SubmitEditorWidget::updateDiffAction() { const bool filesSelected = hasSelection(); + if (m_d->m_filesSelected != filesSelected) { m_d->m_filesSelected = filesSelected; emit fileSelectionChanged(m_d->m_filesSelected); @@ -425,21 +452,24 @@ void SubmitEditorWidget::updateDiffAction() bool SubmitEditorWidget::hasSelection() const { // Not present until model is set - if (const QItemSelectionModel *sm = m_d->m_ui.fileView->selectionModel()) + if (const QItemSelectionModel * sm = m_d->m_ui.fileView->selectionModel()) { return sm->hasSelection(); + } return false; } bool SubmitEditorWidget::hasCheckedFiles() const { - if (const QAbstractItemModel *model = m_d->m_ui.fileView->model()) + if (const QAbstractItemModel * model = m_d->m_ui.fileView->model()) { return listModelContainsCheckedItem(model); + } return false; } void SubmitEditorWidget::changeEvent(QEvent *e) { QWidget::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: m_d->m_ui.retranslateUi(this); @@ -462,7 +492,7 @@ void SubmitEditorWidget::addSubmitFieldWidget(SubmitFieldWidget *f) QHBoxLayout *outerLayout = new QHBoxLayout; outerLayout->addLayout(m_d->m_fieldLayout); outerLayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Ignored)); - QBoxLayout *descrLayout = qobject_cast(m_d->m_ui.descriptionBox->layout()); + QBoxLayout *descrLayout = qobject_cast(m_d->m_ui.descriptionBox->layout()); Q_ASSERT(descrLayout); descrLayout->addLayout(outerLayout); } @@ -487,9 +517,10 @@ void SubmitEditorWidget::insertDescriptionEditContextMenuAction(int pos, QAction void SubmitEditorWidget::editorCustomContextMenuRequested(const QPoint &pos) { - QMenu *menu = m_d->m_ui.description->createStandardContextMenu(); + QMenu *menu = m_d->m_ui.description->createStandardContextMenu(); + // Extend - foreach (const SubmitEditorWidgetPrivate::AdditionalContextMenuAction &a, m_d->descriptionEditContextMenuActions) { + foreach(const SubmitEditorWidgetPrivate::AdditionalContextMenuAction & a, m_d->descriptionEditContextMenuActions) { if (a.second) { if (a.first >= 0) { menu->insertAction(menu->actions().at(a.first), a.second); @@ -501,7 +532,6 @@ void SubmitEditorWidget::editorCustomContextMenuRequested(const QPoint &pos) menu->exec(m_d->m_ui.description->mapToGlobal(pos)); delete menu; } - } // namespace Utils #include "submiteditorwidget.moc" diff --git a/ground/openpilotgcs/src/libs/utils/submiteditorwidget.h b/ground/openpilotgcs/src/libs/utils/submiteditorwidget.h index 969b562ba..61c639b7e 100644 --- a/ground/openpilotgcs/src/libs/utils/submiteditorwidget.h +++ b/ground/openpilotgcs/src/libs/utils/submiteditorwidget.h @@ -4,25 +4,25 @@ * @file submiteditorwidget.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,7 +44,6 @@ class QLineEdit; QT_END_NAMESPACE namespace Utils { - class SubmitFieldWidget; struct SubmitEditorWidgetPrivate; @@ -66,10 +65,8 @@ struct SubmitEditorWidgetPrivate; * Care should be taken to ensure the widget is deleted properly when the * editor closes. */ -class QTCREATOR_UTILS_EXPORT SubmitEditorWidget : public QWidget -{ - Q_OBJECT - Q_DISABLE_COPY(SubmitEditorWidget) +class QTCREATOR_UTILS_EXPORT SubmitEditorWidget : public QWidget { + Q_OBJECT Q_DISABLE_COPY(SubmitEditorWidget) Q_PROPERTY(QString descriptionText READ descriptionText WRITE setDescriptionText DESIGNABLE true) Q_PROPERTY(int fileNameColumn READ fileNameColumn WRITE setFileNameColumn DESIGNABLE false) Q_PROPERTY(QAbstractItemView::SelectionMode fileListSelectionMode READ fileListSelectionMode WRITE setFileListSelectionMode DESIGNABLE true) @@ -79,9 +76,9 @@ public: explicit SubmitEditorWidget(QWidget *parent = 0); virtual ~SubmitEditorWidget(); - void registerActions(QAction *editorUndoAction, QAction *editorRedoAction, + void registerActions(QAction *editorUndoAction, QAction *editorRedoAction, QAction *submitAction = 0, QAction *diffAction = 0); - void unregisterActions(QAction *editorUndoAction, QAction *editorRedoAction, + void unregisterActions(QAction *editorUndoAction, QAction *editorRedoAction, QAction *submitAction = 0, QAction *diffAction = 0); QString descriptionText() const; @@ -140,7 +137,6 @@ private: SubmitEditorWidgetPrivate *m_d; }; - } // namespace Utils #endif // SUBMITEDITORWIDGET_H diff --git a/ground/openpilotgcs/src/libs/utils/submitfieldwidget.cpp b/ground/openpilotgcs/src/libs/utils/submitfieldwidget.cpp index 93b73f195..3e39238b2 100644 --- a/ground/openpilotgcs/src/libs/utils/submitfieldwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/submitfieldwidget.cpp @@ -4,25 +4,25 @@ * @file submitfieldwidget.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -46,22 +46,22 @@ enum { spacing = 2 }; static void inline setComboBlocked(QComboBox *cb, int index) { const bool blocked = cb->blockSignals(true); + cb->setCurrentIndex(index); cb->blockSignals(blocked); } namespace Utils { - // Field/Row entry struct FieldEntry { FieldEntry(); void createGui(const QIcon &removeIcon); void deleteGuiLater(); - QComboBox *combo; + QComboBox *combo; QHBoxLayout *layout; - QLineEdit *lineEdit; - QToolBar *toolBar; + QLineEdit *lineEdit; + QToolBar *toolBar; QToolButton *clearButton; QToolButton *browseButton; int comboIndex; @@ -75,22 +75,21 @@ FieldEntry::FieldEntry() : clearButton(0), browseButton(0), comboIndex(0) -{ -} +{} void FieldEntry::createGui(const QIcon &removeIcon) { - layout = new QHBoxLayout; + layout = new QHBoxLayout; layout->setMargin(0); - layout ->setSpacing(spacing); - combo = new QComboBox; + layout->setSpacing(spacing); + combo = new QComboBox; layout->addWidget(combo); - lineEdit = new QLineEdit; + lineEdit = new QLineEdit; layout->addWidget(lineEdit); - toolBar = new QToolBar; + toolBar = new QToolBar; toolBar->setProperty("_q_custom_style_disabled", QVariant(true)); layout->addWidget(toolBar); - clearButton = new QToolButton; + clearButton = new QToolButton; clearButton->setIcon(removeIcon); toolBar->addWidget(clearButton); browseButton = new QToolButton; @@ -103,7 +102,7 @@ void FieldEntry::deleteGuiLater() clearButton->deleteLater(); browseButton->deleteLater(); toolBar->deleteLater(); - lineEdit->deleteLater(); + lineEdit->deleteLater(); combo->deleteLater(); layout->deleteLater(); } @@ -116,11 +115,11 @@ struct SubmitFieldWidgetPrivate { int findField(const QString &f, int excluded = -1) const; inline QString fieldText(int) const; inline QString fieldValue(int) const; - inline void focusField(int); + inline void focusField(int); - const QIcon removeFieldIcon; - QStringList fields; - QCompleter *completer; + const QIcon removeFieldIcon; + QStringList fields; + QCompleter *completer; bool hasBrowseButton; bool allowDuplicateFields; @@ -129,21 +128,22 @@ struct SubmitFieldWidgetPrivate { }; SubmitFieldWidgetPrivate::SubmitFieldWidgetPrivate() : - removeFieldIcon(QLatin1String(":/utils/images/removesubmitfield.png")), - completer(0), - hasBrowseButton(false), - allowDuplicateFields(false), - layout(0) -{ -} + removeFieldIcon(QLatin1String(":/utils/images/removesubmitfield.png")), + completer(0), + hasBrowseButton(false), + allowDuplicateFields(false), + layout(0) +{} int SubmitFieldWidgetPrivate::findSender(const QObject *o) const { const int count = fieldEntries.size(); + for (int i = 0; i < count; i++) { const FieldEntry &fe = fieldEntries.at(i); - if (fe.combo == o || fe.browseButton == o || fe.clearButton == o || fe.lineEdit == o) + if (fe.combo == o || fe.browseButton == o || fe.clearButton == o || fe.lineEdit == o) { return i; + } } return -1; } @@ -151,9 +151,12 @@ int SubmitFieldWidgetPrivate::findSender(const QObject *o) const int SubmitFieldWidgetPrivate::findField(const QString &ft, int excluded) const { const int count = fieldEntries.size(); - for (int i = 0; i < count; i++) - if (i != excluded && fieldText(i) == ft) + + for (int i = 0; i < count; i++) { + if (i != excluded && fieldText(i) == ft) { return i; + } + } return -1; } @@ -174,8 +177,8 @@ void SubmitFieldWidgetPrivate::focusField(int pos) // SubmitFieldWidget SubmitFieldWidget::SubmitFieldWidget(QWidget *parent) : - QWidget(parent), - m_d(new SubmitFieldWidgetPrivate) + QWidget(parent), + m_d(new SubmitFieldWidgetPrivate) { m_d->layout = new QVBoxLayout; m_d->layout->setMargin(0); @@ -191,12 +194,14 @@ SubmitFieldWidget::~SubmitFieldWidget() void SubmitFieldWidget::setFields(const QStringList & f) { // remove old fields - for (int i = m_d->fieldEntries.size() - 1 ; i >= 0 ; i--) - removeField(i); + for (int i = m_d->fieldEntries.size() - 1; i >= 0; i--) { + removeField(i); + } m_d->fields = f; - if (!f.empty()) + if (!f.empty()) { createField(f.front()); + } } QStringList SubmitFieldWidget::fields() const @@ -211,11 +216,12 @@ bool SubmitFieldWidget::hasBrowseButton() const void SubmitFieldWidget::setHasBrowseButton(bool d) { - if (m_d->hasBrowseButton == d) + if (m_d->hasBrowseButton == d) { return; + } m_d->hasBrowseButton = d; foreach(const FieldEntry &fe, m_d->fieldEntries) - fe.browseButton->setVisible(d); + fe.browseButton->setVisible(d); } bool SubmitFieldWidget::allowDuplicateFields() const @@ -235,11 +241,12 @@ QCompleter *SubmitFieldWidget::completer() const void SubmitFieldWidget::setCompleter(QCompleter *c) { - if (c == m_d->completer) + if (c == m_d->completer) { return; + } m_d->completer = c; foreach(const FieldEntry &fe, m_d->fieldEntries) - fe.lineEdit->setCompleter(c); + fe.lineEdit->setCompleter(c); } QString SubmitFieldWidget::fieldValue(int pos) const @@ -254,12 +261,14 @@ void SubmitFieldWidget::setFieldValue(int pos, const QString &value) QString SubmitFieldWidget::fieldValues() const { - const QChar blank = QLatin1Char(' '); + const QChar blank = QLatin1Char(' '); const QChar newLine = QLatin1Char('\n'); // Format as "RevBy: value\nSigned-Off: value\n" QString rc; + foreach(const FieldEntry &fe, m_d->fieldEntries) { const QString value = fe.lineEdit->text().trimmed(); + if (!value.isEmpty()) { rc += fe.combo->currentText(); rc += blank; @@ -273,6 +282,7 @@ QString SubmitFieldWidget::fieldValues() const void SubmitFieldWidget::createField(const QString &f) { FieldEntry fe; + fe.createGui(m_d->removeFieldIcon); fe.combo->addItems(m_d->fields); if (!f.isEmpty()) { @@ -284,11 +294,13 @@ void SubmitFieldWidget::createField(const QString &f) } connect(fe.browseButton, SIGNAL(clicked()), this, SLOT(slotBrowseButtonClicked())); - if (!m_d->hasBrowseButton) + if (!m_d->hasBrowseButton) { fe.browseButton->setVisible(false); + } - if (m_d->completer) + if (m_d->completer) { fe.lineEdit->setCompleter(m_d->completer); + } connect(fe.combo, SIGNAL(currentIndexChanged(int)), this, SLOT(slotComboIndexChanged(int))); @@ -302,6 +314,7 @@ void SubmitFieldWidget::slotRemove() { // Never remove first entry const int index = m_d->findSender(sender()); + switch (index) { case -1: break; @@ -316,8 +329,9 @@ void SubmitFieldWidget::slotRemove() void SubmitFieldWidget::removeField(int index) { - FieldEntry fe = m_d->fieldEntries.takeAt(index); - QLayoutItem * item = m_d->layout->takeAt(index); + FieldEntry fe = m_d->fieldEntries.takeAt(index); + QLayoutItem *item = m_d->layout->takeAt(index); + fe.deleteGuiLater(); delete item; } @@ -325,10 +339,13 @@ void SubmitFieldWidget::removeField(int index) void SubmitFieldWidget::slotComboIndexChanged(int comboIndex) { const int pos = m_d->findSender(sender()); - if (debug) + + if (debug) { qDebug() << '>' << Q_FUNC_INFO << pos; - if (pos == -1) + } + if (pos == -1) { return; + } // Accept new index or reset combo to previous value? int &previousIndex = m_d->fieldEntries[pos].comboIndex; if (comboIndexChange(pos, comboIndex)) { @@ -336,8 +353,9 @@ void SubmitFieldWidget::slotComboIndexChanged(int comboIndex) } else { setComboBlocked(m_d->fieldEntries.at(pos).combo, previousIndex); } - if (debug) + if (debug) { qDebug() << '<' << Q_FUNC_INFO << pos; + } } // Handle change of a combo. Return "false" if the combo @@ -345,6 +363,7 @@ void SubmitFieldWidget::slotComboIndexChanged(int comboIndex) bool SubmitFieldWidget::comboIndexChange(int pos, int index) { const QString newField = m_d->fieldEntries.at(pos).combo->itemText(index); + // If the field is visible elsewhere: focus the existing one and refuse if (!m_d->allowDuplicateFields) { const int existingFieldIndex = m_d->findField(newField, pos); @@ -354,8 +373,9 @@ bool SubmitFieldWidget::comboIndexChange(int pos, int index) } } // Empty value: just change the field - if (m_d->fieldValue(pos).isEmpty()) + if (m_d->fieldValue(pos).isEmpty()) { return true; + } // Non-empty: Create a new field and reset the triggering combo createField(newField); return false; @@ -366,5 +386,4 @@ void SubmitFieldWidget::slotBrowseButtonClicked() const int pos = m_d->findSender(sender()); emit browseButtonClicked(pos, m_d->fieldText(pos)); } - } diff --git a/ground/openpilotgcs/src/libs/utils/submitfieldwidget.h b/ground/openpilotgcs/src/libs/utils/submitfieldwidget.h index 10154fffd..8bc912c27 100644 --- a/ground/openpilotgcs/src/libs/utils/submitfieldwidget.h +++ b/ground/openpilotgcs/src/libs/utils/submitfieldwidget.h @@ -4,25 +4,25 @@ * @file submitfieldwidget.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,7 +38,6 @@ class QCompleter; QT_END_NAMESPACE namespace Utils { - struct SubmitFieldWidgetPrivate; /* A widget for editing submit message fields like "reviewed-by:", @@ -47,10 +46,8 @@ struct SubmitFieldWidgetPrivate; * When choosing a different field in the combo, a new row is opened if text * has been entered for the current field. Optionally, a "Browse..." button and * completer can be added. */ -class QTCREATOR_UTILS_EXPORT SubmitFieldWidget : public QWidget -{ - Q_OBJECT - Q_PROPERTY(QStringList fields READ fields WRITE setFields DESIGNABLE true) +class QTCREATOR_UTILS_EXPORT SubmitFieldWidget : public QWidget { + Q_OBJECT Q_PROPERTY(QStringList fields READ fields WRITE setFields DESIGNABLE true) Q_PROPERTY(bool hasBrowseButton READ hasBrowseButton WRITE setHasBrowseButton DESIGNABLE true) Q_PROPERTY(bool allowDuplicateFields READ allowDuplicateFields WRITE setAllowDuplicateFields DESIGNABLE true) @@ -59,7 +56,7 @@ public: virtual ~SubmitFieldWidget(); QStringList fields() const; - void setFields(const QStringList&); + void setFields(const QStringList &); bool hasBrowseButton() const; void setHasBrowseButton(bool d); @@ -91,7 +88,6 @@ private: SubmitFieldWidgetPrivate *m_d; }; - } #endif // SUBMITFIELDWIDGET_H diff --git a/ground/openpilotgcs/src/libs/utils/svgimageprovider.cpp b/ground/openpilotgcs/src/libs/utils/svgimageprovider.cpp index 6e454002b..2b9a8c79a 100644 --- a/ground/openpilotgcs/src/libs/utils/svgimageprovider.cpp +++ b/ground/openpilotgcs/src/libs/utils/svgimageprovider.cpp @@ -30,12 +30,11 @@ #include #include -SvgImageProvider::SvgImageProvider(const QString &basePath): +SvgImageProvider::SvgImageProvider(const QString &basePath) : QObject(), QDeclarativeImageProvider(QDeclarativeImageProvider::Image), m_basePath(basePath) -{ -} +{} SvgImageProvider::~SvgImageProvider() { @@ -45,14 +44,16 @@ SvgImageProvider::~SvgImageProvider() QSvgRenderer *SvgImageProvider::loadRenderer(const QString &svgFile) { QSvgRenderer *renderer = m_renderers.value(svgFile); + if (!renderer) { renderer = new QSvgRenderer(svgFile); QString fn = QUrl::fromLocalFile(m_basePath).resolved(svgFile).toLocalFile(); - //convert path to be relative to base - if (!renderer->isValid()) + // convert path to be relative to base + if (!renderer->isValid()) { renderer->load(fn); + } if (!renderer->isValid()) { qWarning() << "Failed to load svg file:" << svgFile << fn; @@ -79,7 +80,7 @@ QSvgRenderer *SvgImageProvider::loadRenderer(const QString &svgFile) Image { source: "image://svg/pfd.svg!world" } -*/ + */ QImage SvgImageProvider::requestImage(const QString &id, QSize *size, const QSize &requestedSize) { QString svgFile = id; @@ -87,15 +88,16 @@ QImage SvgImageProvider::requestImage(const QString &id, QSize *size, const QSiz QString parameters; int separatorPos = id.indexOf('!'); + if (separatorPos != -1) { svgFile = id.left(separatorPos); - element = id.mid(separatorPos+1); + element = id.mid(separatorPos + 1); } int parametersPos = element.indexOf('?'); if (parametersPos != -1) { - parameters = element.mid(parametersPos+1); - element = element.left(parametersPos); + parameters = element.mid(parametersPos + 1); + element = element.left(parametersPos); } int hSlicesCount = 0; @@ -120,31 +122,33 @@ QImage SvgImageProvider::requestImage(const QString &id, QSize *size, const QSiz } } - if (size) + if (size) { *size = QSize(); + } QSvgRenderer *renderer = loadRenderer(svgFile); - if (!renderer) + if (!renderer) { return QImage(); + } - qreal xScale = 1.0; - qreal yScale = 1.0; + qreal xScale = 1.0; + qreal yScale = 1.0; QSize docSize = renderer->defaultSize(); if (!requestedSize.isEmpty()) { if (!element.isEmpty()) { QRectF elementBounds = renderer->boundsOnElement(element); - xScale = qreal(requestedSize.width())/elementBounds.width(); - yScale = qreal(requestedSize.height())/elementBounds.height(); + xScale = qreal(requestedSize.width()) / elementBounds.width(); + yScale = qreal(requestedSize.height()) / elementBounds.height(); } else if (!docSize.isEmpty()) { - xScale = qreal(requestedSize.width())/docSize.width(); - yScale = qreal(requestedSize.height())/docSize.height(); + xScale = qreal(requestedSize.width()) / docSize.width(); + yScale = qreal(requestedSize.height()) / docSize.height(); } } - //keep the aspect ratio - //TODO: how to configure it? as a part of image path? + // keep the aspect ratio + // TODO: how to configure it? as a part of image path? xScale = yScale = qMin(xScale, yScale); if (!element.isEmpty()) { @@ -154,40 +158,41 @@ QImage SvgImageProvider::requestImage(const QString &id, QSize *size, const QSiz } QRectF elementBounds = renderer->boundsOnElement(element); - int elementWidth = qRound(elementBounds.width() * xScale); - int elementHeigh = qRound(elementBounds.height() * yScale); + int elementWidth = qRound(elementBounds.width() * xScale); + int elementHeigh = qRound(elementBounds.height() * yScale); int w = elementWidth; int h = elementHeigh; int x = 0; int y = 0; if (hSlicesCount > 1) { - x = (w*hSlice)/hSlicesCount; - w = (w*(hSlice+1))/hSlicesCount - x; + x = (w * hSlice) / hSlicesCount; + w = (w * (hSlice + 1)) / hSlicesCount - x; } if (vSlicesCount > 1) { - y = (h*(vSlice))/vSlicesCount; - h = (h*(vSlice+1))/vSlicesCount - y; + y = (h * (vSlice)) / vSlicesCount; + h = (h * (vSlice + 1)) / vSlicesCount - y; } - QImage img(w+border*2, h+border*2, QImage::Format_ARGB32_Premultiplied); + QImage img(w + border * 2, h + border * 2, QImage::Format_ARGB32_Premultiplied); img.fill(0); QPainter p(&img); p.setRenderHints(QPainter::TextAntialiasing | QPainter::Antialiasing | QPainter::SmoothPixmapTransform); - p.translate(-x+border,-y+border); + p.translate(-x + border, -y + border); renderer->render(&p, element, QRectF(0, 0, elementWidth, elementHeigh)); - if (size) + if (size) { *size = QSize(w, h); + } - //img.save(element+parameters+".png"); + // img.save(element+parameters+".png"); return img; } else { - //render the whole svg file + // render the whole svg file int w = qRound(docSize.width() * xScale); int h = qRound(docSize.height() * yScale); @@ -201,8 +206,9 @@ QImage SvgImageProvider::requestImage(const QString &id, QSize *size, const QSiz p.scale(xScale, yScale); renderer->render(&p, QRectF(QPointF(), QSizeF(docSize))); - if (size) + if (size) { *size = QSize(w, h); + } return img; } } @@ -213,17 +219,18 @@ QPixmap SvgImageProvider::requestPixmap(const QString &id, QSize *size, const QS } /*! - \fn SvgImageProvider::scaledElementBounds(const QString &svgFile, const QString &element) + \fn SvgImageProvider::scaledElementBounds(const QString &svgFile, const QString &element) - Returns the bound of \a element in logical coordinates, - scalled to the default size of svg document (so the bounds of whole doc would be (0,0,1,1) ). -*/ + Returns the bound of \a element in logical coordinates, + scalled to the default size of svg document (so the bounds of whole doc would be (0,0,1,1) ). + */ QRectF SvgImageProvider::scaledElementBounds(const QString &svgFile, const QString &elementName) { QSvgRenderer *renderer = loadRenderer(svgFile); - if (!renderer) + if (!renderer) { return QRectF(); + } if (!renderer->elementExists(elementName)) { qWarning() << "invalid element:" << elementName << "of" << svgFile; @@ -234,9 +241,9 @@ QRectF SvgImageProvider::scaledElementBounds(const QString &svgFile, const QStri QMatrix matrix = renderer->matrixForElement(elementName); elementBounds = matrix.mapRect(elementBounds); - QSize docSize = renderer->defaultSize(); - return QRectF(elementBounds.x()/docSize.width(), - elementBounds.y()/docSize.height(), - elementBounds.width()/docSize.width(), - elementBounds.height()/docSize.height()); + QSize docSize = renderer->defaultSize(); + return QRectF(elementBounds.x() / docSize.width(), + elementBounds.y() / docSize.height(), + elementBounds.width() / docSize.width(), + elementBounds.height() / docSize.height()); } diff --git a/ground/openpilotgcs/src/libs/utils/svgimageprovider.h b/ground/openpilotgcs/src/libs/utils/svgimageprovider.h index c13c610ff..e24f51e5a 100644 --- a/ground/openpilotgcs/src/libs/utils/svgimageprovider.h +++ b/ground/openpilotgcs/src/libs/utils/svgimageprovider.h @@ -35,23 +35,22 @@ #include "utils_global.h" -class QTCREATOR_UTILS_EXPORT SvgImageProvider : public QObject, public QDeclarativeImageProvider -{ +class QTCREATOR_UTILS_EXPORT SvgImageProvider : public QObject, public QDeclarativeImageProvider { Q_OBJECT public: SvgImageProvider(const QString &basePath); - ~SvgImageProvider(); + ~SvgImageProvider(); QSvgRenderer *loadRenderer(const QString &svgFile); - QImage requestImage(const QString &id, QSize *size, const QSize& requestedSize); - QPixmap requestPixmap(const QString &id, QSize *size, const QSize& requestedSize); + QImage requestImage(const QString &id, QSize *size, const QSize & requestedSize); + QPixmap requestPixmap(const QString &id, QSize *size, const QSize & requestedSize); Q_INVOKABLE QRectF scaledElementBounds(const QString &svgFile, const QString &elementName); private: - QMap m_renderers; + QMap m_renderers; QString m_basePath; }; -#endif +#endif // ifndef SVGIMAGEPROVIDER_H_ diff --git a/ground/openpilotgcs/src/libs/utils/synchronousprocess.cpp b/ground/openpilotgcs/src/libs/utils/synchronousprocess.cpp index 332c60f64..02a9dfff9 100644 --- a/ground/openpilotgcs/src/libs/utils/synchronousprocess.cpp +++ b/ground/openpilotgcs/src/libs/utils/synchronousprocess.cpp @@ -4,25 +4,25 @@ * @file synchronousprocess.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,25 +44,24 @@ enum { debug = 0 }; enum { defaultMaxHangTimerCount = 10 }; namespace Utils { - // ----------- SynchronousProcessResponse SynchronousProcessResponse::SynchronousProcessResponse() : - result(StartFailed), - exitCode(-1) -{ -} + result(StartFailed), + exitCode(-1) +{} void SynchronousProcessResponse::clear() { - result = StartFailed; + result = StartFailed; exitCode = -1; stdOut.clear(); stdErr.clear(); } -QTCREATOR_UTILS_EXPORT QDebug operator<<(QDebug str, const SynchronousProcessResponse& r) +QTCREATOR_UTILS_EXPORT QDebug operator<<(QDebug str, const SynchronousProcessResponse & r) { QDebug nsp = str.nospace(); + nsp << "SynchronousProcessResponse: result=" << r.result << " ex=" << r.exitCode << '\n' << r.stdOut.size() << " bytes stdout, stderr=" << r.stdErr << '\n'; return str; @@ -78,7 +77,7 @@ struct ChannelBuffer { bool firstData; bool bufferedSignalsEnabled; bool firstBuffer; - int bufferPos; + int bufferPos; }; ChannelBuffer::ChannelBuffer() : @@ -86,14 +85,13 @@ ChannelBuffer::ChannelBuffer() : bufferedSignalsEnabled(false), firstBuffer(true), bufferPos(0) -{ -} +{} void ChannelBuffer::clearForRun() { - firstData = true; + firstData = true; firstBuffer = true; - bufferPos = 0; + bufferPos = 0; } /* Check for complete lines read from the device and return them, moving the @@ -103,10 +101,12 @@ QByteArray ChannelBuffer::linesRead() { // Any new lines? const int lastLineIndex = data.lastIndexOf('\n'); - if (lastLineIndex == -1 || lastLineIndex <= bufferPos) + + if (lastLineIndex == -1 || lastLineIndex <= bufferPos) { return QByteArray(); + } const int nextBufferPos = lastLineIndex + 1; - const QByteArray lines = data.mid(bufferPos, nextBufferPos - bufferPos); + const QByteArray lines = data.mid(bufferPos, nextBufferPos - bufferPos); bufferPos = nextBufferPos; return lines; } @@ -114,15 +114,15 @@ QByteArray ChannelBuffer::linesRead() // ----------- SynchronousProcessPrivate struct SynchronousProcessPrivate { SynchronousProcessPrivate(); - void clearForRun(); + void clearForRun(); QTextCodec *m_stdOutCodec; - QProcess m_process; - QTimer m_timer; + QProcess m_process; + QTimer m_timer; QEventLoop m_eventLoop; SynchronousProcessResponse m_result; - int m_hangTimerCount; - int m_maxHangTimerCount; + int m_hangTimerCount; + int m_maxHangTimerCount; bool m_startFailure; ChannelBuffer m_stdOut; @@ -134,8 +134,7 @@ SynchronousProcessPrivate::SynchronousProcessPrivate() : m_hangTimerCount(0), m_maxHangTimerCount(defaultMaxHangTimerCount), m_startFailure(false) -{ -} +{} void SynchronousProcessPrivate::clearForRun() { @@ -152,7 +151,7 @@ SynchronousProcess::SynchronousProcess() : { m_d->m_timer.setInterval(1000); connect(&m_d->m_timer, SIGNAL(timeout()), this, SLOT(slotTimeout())); - connect(&m_d->m_process, SIGNAL(finished(int,QProcess::ExitStatus)), this, SLOT(finished(int,QProcess::ExitStatus))); + connect(&m_d->m_process, SIGNAL(finished(int, QProcess::ExitStatus)), this, SLOT(finished(int, QProcess::ExitStatus))); connect(&m_d->m_process, SIGNAL(error(QProcess::ProcessError)), this, SLOT(error(QProcess::ProcessError))); connect(&m_d->m_process, SIGNAL(readyReadStandardOutput()), this, SLOT(stdOutReady())); @@ -229,7 +228,7 @@ QString SynchronousProcess::workingDirectory() const return m_d->m_process.workingDirectory(); } -QProcess::ProcessChannelMode SynchronousProcess::processChannelMode () const +QProcess::ProcessChannelMode SynchronousProcess::processChannelMode() const { return m_d->m_process.processChannelMode(); } @@ -240,10 +239,11 @@ void SynchronousProcess::setProcessChannelMode(QProcess::ProcessChannelMode m) } SynchronousProcessResponse SynchronousProcess::run(const QString &binary, - const QStringList &args) + const QStringList &args) { - if (debug) + if (debug) { qDebug() << '>' << Q_FUNC_INFO << binary << args; + } m_d->clearForRun(); @@ -267,38 +267,43 @@ SynchronousProcessResponse SynchronousProcess::run(const QString &binary, QApplication::restoreOverrideCursor(); } - if (debug) + if (debug) { qDebug() << '<' << Q_FUNC_INFO << binary << m_d->m_result; - return m_d->m_result; + } + return m_d->m_result; } void SynchronousProcess::slotTimeout() { if (++m_d->m_hangTimerCount > m_d->m_maxHangTimerCount) { - if (debug) + if (debug) { qDebug() << Q_FUNC_INFO << "HANG detected, killing"; + } m_d->m_process.kill(); m_d->m_result.result = SynchronousProcessResponse::Hang; } else { - if (debug) + if (debug) { qDebug() << Q_FUNC_INFO << m_d->m_hangTimerCount; + } } } void SynchronousProcess::finished(int exitCode, QProcess::ExitStatus e) { - if (debug) + if (debug) { qDebug() << Q_FUNC_INFO << exitCode << e; + } m_d->m_hangTimerCount = 0; switch (e) { case QProcess::NormalExit: - m_d->m_result.result = exitCode ? SynchronousProcessResponse::FinishedError : SynchronousProcessResponse::Finished; + m_d->m_result.result = exitCode ? SynchronousProcessResponse::FinishedError : SynchronousProcessResponse::Finished; m_d->m_result.exitCode = exitCode; break; case QProcess::CrashExit: // Was hang detected before and killed? - if (m_d->m_result.result != SynchronousProcessResponse::Hang) + if (m_d->m_result.result != SynchronousProcessResponse::Hang) { m_d->m_result.result = SynchronousProcessResponse::TerminatedAbnormally; + } m_d->m_result.exitCode = -1; break; } @@ -308,11 +313,13 @@ void SynchronousProcess::finished(int exitCode, QProcess::ExitStatus e) void SynchronousProcess::error(QProcess::ProcessError e) { m_d->m_hangTimerCount = 0; - if (debug) + if (debug) { qDebug() << Q_FUNC_INFO << e; + } // Was hang detected before and killed? - if (m_d->m_result.result != SynchronousProcessResponse::Hang) + if (m_d->m_result.result != SynchronousProcessResponse::Hang) { m_d->m_result.result = SynchronousProcessResponse::StartFailed; + } m_d->m_startFailure = true; m_d->m_eventLoop.quit(); } @@ -337,6 +344,7 @@ QString SynchronousProcess::convertStdErr(const QByteArray &ba) QString SynchronousProcess::convertStdOut(const QByteArray &ba) const { QString stdOut = m_d->m_stdOutCodec ? m_d->m_stdOutCodec->toUnicode(ba) : QString::fromLocal8Bit(ba.constData(), ba.size()); + return stdOut.remove(QLatin1Char('\r')); } @@ -344,8 +352,10 @@ void SynchronousProcess::processStdOut(bool emitSignals) { // Handle binary data const QByteArray ba = m_d->m_process.readAllStandardOutput(); - if (debug > 1) + + if (debug > 1) { qDebug() << Q_FUNC_INFO << emitSignals << ba; + } if (!ba.isEmpty()) { m_d->m_stdOut.data += ba; if (emitSignals) { @@ -368,8 +378,10 @@ void SynchronousProcess::processStdErr(bool emitSignals) { // Handle binary data const QByteArray ba = m_d->m_process.readAllStandardError(); - if (debug > 1) + + if (debug > 1) { qDebug() << Q_FUNC_INFO << emitSignals << ba; + } if (!ba.isEmpty()) { m_d->m_stdErr.data += ba; if (emitSignals) { @@ -408,44 +420,52 @@ static QString checkBinary(const QDir &dir, const QString &binary) { // naive UNIX approach const QFileInfo info(dir.filePath(binary)); - if (info.isFile() && info.isExecutable()) + + if (info.isFile() && info.isExecutable()) { return info.absoluteFilePath(); + } // Does the OS have some weird extension concept or does the // binary have a 3 letter extension? - if (pathOS == OS_Unix) + if (pathOS == OS_Unix) { return QString(); + } const int dotIndex = binary.lastIndexOf(QLatin1Char('.')); - if (dotIndex != -1 && dotIndex == binary.size() - 4) - return QString(); + if (dotIndex != -1 && dotIndex == binary.size() - 4) { + return QString(); + } switch (pathOS) { case OS_Unix: break; - case OS_Windows: { - static const char *windowsExtensions[] = {".cmd", ".bat", ".exe", ".com" }; - // Check the Windows extensions using the order - const int windowsExtensionCount = sizeof(windowsExtensions)/sizeof(const char*); - for (int e = 0; e < windowsExtensionCount; e ++) { - const QFileInfo windowsBinary(dir.filePath(binary + QLatin1String(windowsExtensions[e]))); - if (windowsBinary.isFile() && windowsBinary.isExecutable()) - return windowsBinary.absoluteFilePath(); + case OS_Windows: + { + static const char *windowsExtensions[] = { ".cmd", ".bat", ".exe", ".com" }; + // Check the Windows extensions using the order + const int windowsExtensionCount = sizeof(windowsExtensions) / sizeof(const char *); + for (int e = 0; e < windowsExtensionCount; e++) { + const QFileInfo windowsBinary(dir.filePath(binary + QLatin1String(windowsExtensions[e]))); + if (windowsBinary.isFile() && windowsBinary.isExecutable()) { + return windowsBinary.absoluteFilePath(); } } - break; - case OS_Mac: { - // Check for Mac app folders - const QFileInfo appFolder(dir.filePath(binary + QLatin1String(".app"))); - if (appFolder.isDir()) { - QString macBinaryPath = appFolder.absoluteFilePath(); - macBinaryPath += QLatin1String("/Contents/MacOS/"); - macBinaryPath += binary; - const QFileInfo macBinary(macBinaryPath); - if (macBinary.isFile() && macBinary.isExecutable()) - return macBinary.absoluteFilePath(); + } + break; + case OS_Mac: + { + // Check for Mac app folders + const QFileInfo appFolder(dir.filePath(binary + QLatin1String(".app"))); + if (appFolder.isDir()) { + QString macBinaryPath = appFolder.absoluteFilePath(); + macBinaryPath += QLatin1String("/Contents/MacOS/"); + macBinaryPath += binary; + const QFileInfo macBinary(macBinaryPath); + if (macBinary.isFile() && macBinary.isExecutable()) { + return macBinary.absoluteFilePath(); } } - break; + } + break; } return QString(); } @@ -454,25 +474,30 @@ QString SynchronousProcess::locateBinary(const QString &path, const QString &bin { // Absolute file? const QFileInfo absInfo(binary); - if (absInfo.isAbsolute()) + + if (absInfo.isAbsolute()) { return checkBinary(absInfo.dir(), absInfo.fileName()); + } // Windows finds binaries in the current directory if (pathOS == OS_Windows) { const QString currentDirBinary = checkBinary(QDir::current(), binary); - if (!currentDirBinary.isEmpty()) + if (!currentDirBinary.isEmpty()) { return currentDirBinary; + } } const QStringList paths = path.split(pathSeparator()); - if (paths.empty()) + if (paths.empty()) { return QString(); + } const QStringList::const_iterator cend = paths.constEnd(); for (QStringList::const_iterator it = paths.constBegin(); it != cend; ++it) { const QDir dir(*it); const QString rc = checkBinary(dir, binary); - if (!rc.isEmpty()) + if (!rc.isEmpty()) { return rc; + } } return QString(); } @@ -480,14 +505,15 @@ QString SynchronousProcess::locateBinary(const QString &path, const QString &bin QString SynchronousProcess::locateBinary(const QString &binary) { const QByteArray path = qgetenv("PATH"); + return locateBinary(QString::fromLocal8Bit(path), binary); } QChar SynchronousProcess::pathSeparator() { - if (pathOS == OS_Windows) + if (pathOS == OS_Windows) { return QLatin1Char(';'); + } return QLatin1Char(':'); } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/synchronousprocess.h b/ground/openpilotgcs/src/libs/utils/synchronousprocess.h index a3cf2df44..db0ea01a1 100644 --- a/ground/openpilotgcs/src/libs/utils/synchronousprocess.h +++ b/ground/openpilotgcs/src/libs/utils/synchronousprocess.h @@ -4,25 +4,25 @@ * @file synchronousprocess.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,12 +42,10 @@ class QByteArray; QT_END_NAMESPACE namespace Utils { - struct SynchronousProcessPrivate; /* Result of SynchronousProcess execution */ -struct QTCREATOR_UTILS_EXPORT SynchronousProcessResponse -{ +struct QTCREATOR_UTILS_EXPORT SynchronousProcessResponse { enum Result { // Finished with return code 0 Finished, @@ -58,13 +56,14 @@ struct QTCREATOR_UTILS_EXPORT SynchronousProcessResponse // Executable could not be started StartFailed, // Hang, no output after time out - Hang }; + Hang + }; SynchronousProcessResponse(); void clear(); - Result result; - int exitCode; + Result result; + int exitCode; QString stdOut; QString stdErr; }; @@ -83,8 +82,7 @@ QTCREATOR_UTILS_EXPORT QDebug operator<<(QDebug str, const SynchronousProcessRes * stdOutBufferedSignalsEnabled()/setStdErrBufferedSignalsEnabled(). * They would typically be used for log windows. */ -class QTCREATOR_UTILS_EXPORT SynchronousProcess : public QObject -{ +class QTCREATOR_UTILS_EXPORT SynchronousProcess : public QObject { Q_OBJECT public: SynchronousProcess(); @@ -97,7 +95,7 @@ public: void setStdOutCodec(QTextCodec *c); QTextCodec *stdOutCodec() const; - QProcess::ProcessChannelMode processChannelMode () const; + QProcess::ProcessChannelMode processChannelMode() const; void setProcessChannelMode(QProcess::ProcessChannelMode m); bool stdOutBufferedSignalsEnabled() const; @@ -142,7 +140,6 @@ private: SynchronousProcessPrivate *m_d; }; - } // namespace Utils -#endif +#endif // ifndef SYNCHRONOUSPROCESS_H diff --git a/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.cpp b/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.cpp index c5dc21a15..6efdd0d48 100644 --- a/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.cpp +++ b/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.cpp @@ -4,25 +4,25 @@ * @file treewidgetcolumnstretcher.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,7 +33,7 @@ using namespace Utils; TreeWidgetColumnStretcher::TreeWidgetColumnStretcher(QTreeWidget *treeWidget, int columnToStretch) - : QObject(treeWidget->header()), m_columnToStretch(columnToStretch) + : QObject(treeWidget->header()), m_columnToStretch(columnToStretch) { parent()->installEventFilter(this); QHideEvent fake; @@ -44,18 +44,20 @@ bool TreeWidgetColumnStretcher::eventFilter(QObject *obj, QEvent *ev) { if (obj == parent()) { if (ev->type() == QEvent::Show) { - QHeaderView *hv = qobject_cast(obj); - for (int i = 0; i < hv->count(); ++i) + QHeaderView *hv = qobject_cast(obj); + for (int i = 0; i < hv->count(); ++i) { hv->setResizeMode(i, QHeaderView::Interactive); + } } else if (ev->type() == QEvent::Hide) { - QHeaderView *hv = qobject_cast(obj); - for (int i = 0; i < hv->count(); ++i) + QHeaderView *hv = qobject_cast(obj); + for (int i = 0; i < hv->count(); ++i) { hv->setResizeMode(i, i == m_columnToStretch ? QHeaderView::Stretch : QHeaderView::ResizeToContents); + } } else if (ev->type() == QEvent::Resize) { - QHeaderView *hv = qobject_cast(obj); + QHeaderView *hv = qobject_cast(obj); if (hv->resizeMode(m_columnToStretch) == QHeaderView::Interactive) { - QResizeEvent *re = static_cast(ev); - int diff = re->size().width() - re->oldSize().width() ; + QResizeEvent *re = static_cast(ev); + int diff = re->size().width() - re->oldSize().width(); hv->resizeSection(m_columnToStretch, qMax(32, hv->sectionSize(1) + diff)); } } diff --git a/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.h b/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.h index 7d6619b2c..da3659c43 100644 --- a/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.h +++ b/ground/openpilotgcs/src/libs/utils/treewidgetcolumnstretcher.h @@ -4,25 +4,25 @@ * @file treewidgetcolumnstretcher.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,24 +37,21 @@ class QTreeWidget; QT_END_NAMESPACE namespace Utils { - /* -The class fixes QTreeWidget to resize all columns to contents, except one -stretching column. As opposed to standard QTreeWidget, all columns are -still interactively resizable. + The class fixes QTreeWidget to resize all columns to contents, except one + stretching column. As opposed to standard QTreeWidget, all columns are + still interactively resizable. -*/ + */ -class QTCREATOR_UTILS_EXPORT TreeWidgetColumnStretcher : public QObject -{ +class QTCREATOR_UTILS_EXPORT TreeWidgetColumnStretcher : public QObject { int m_columnToStretch; public: TreeWidgetColumnStretcher(QTreeWidget *treeWidget, int columnToStretch); bool eventFilter(QObject *obj, QEvent *ev); }; - } // namespace Utils #endif // TREEWIDGETCOLUMNSTRETCHER_H diff --git a/ground/openpilotgcs/src/libs/utils/uncommentselection.cpp b/ground/openpilotgcs/src/libs/utils/uncommentselection.cpp index 64f7ddcf9..1c3669f4c 100644 --- a/ground/openpilotgcs/src/libs/utils/uncommentselection.cpp +++ b/ground/openpilotgcs/src/libs/utils/uncommentselection.cpp @@ -4,25 +4,25 @@ * @file uncommentselection.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,24 +36,25 @@ void Utils::unCommentSelection(QPlainTextEdit *edit) { QTextCursor cursor = edit->textCursor(); QTextDocument *doc = cursor.document(); + cursor.beginEditBlock(); - int pos = cursor.position(); + int pos = cursor.position(); int anchor = cursor.anchor(); - int start = qMin(anchor, pos); - int end = qMax(anchor, pos); - bool anchorIsStart = (anchor == start); + int start = qMin(anchor, pos); + int end = qMax(anchor, pos); + bool anchorIsStart = (anchor == start); QTextBlock startBlock = doc->findBlock(start); - QTextBlock endBlock = doc->findBlock(end); + QTextBlock endBlock = doc->findBlock(end); if (end > start && endBlock.position() == end) { --end; endBlock = endBlock.previous(); } - bool doCStyleUncomment = false; - bool doCStyleComment = false; + bool doCStyleUncomment = false; + bool doCStyleComment = false; bool doCppStyleUncomment = false; bool hasSelection = cursor.hasSelection(); @@ -63,15 +64,15 @@ void Utils::unCommentSelection(QPlainTextEdit *edit) int startPos = start - startBlock.position(); bool hasLeadingCharacters = !startText.left(startPos).trimmed().isEmpty(); if ((startPos >= 2 - && startText.at(startPos-2) == QLatin1Char('/') - && startText.at(startPos-1) == QLatin1Char('*'))) { + && startText.at(startPos - 2) == QLatin1Char('/') + && startText.at(startPos - 1) == QLatin1Char('*'))) { startPos -= 2; - start -= 2; + start -= 2; } bool hasSelStart = (startPos < startText.length() - 2 && startText.at(startPos) == QLatin1Char('/') - && startText.at(startPos+1) == QLatin1Char('*')); + && startText.at(startPos + 1) == QLatin1Char('*')); QString endText = endBlock.text(); @@ -79,18 +80,18 @@ void Utils::unCommentSelection(QPlainTextEdit *edit) bool hasTrailingCharacters = !endText.left(endPos).remove(QLatin1String("//")).trimmed().isEmpty() && !endText.mid(endPos).trimmed().isEmpty(); if ((endPos <= endText.length() - 2 - && endText.at(endPos) == QLatin1Char('*') - && endText.at(endPos+1) == QLatin1Char('/'))) { + && endText.at(endPos) == QLatin1Char('*') + && endText.at(endPos + 1) == QLatin1Char('/'))) { endPos += 2; - end += 2; + end += 2; } bool hasSelEnd = (endPos >= 2 - && endText.at(endPos-2) == QLatin1Char('*') - && endText.at(endPos-1) == QLatin1Char('/')); + && endText.at(endPos - 2) == QLatin1Char('*') + && endText.at(endPos - 1) == QLatin1Char('/')); doCStyleUncomment = hasSelStart && hasSelEnd; - doCStyleComment = !doCStyleUncomment && (hasLeadingCharacters || hasTrailingCharacters); + doCStyleComment = !doCStyleUncomment && (hasLeadingCharacters || hasTrailingCharacters); } if (doCStyleUncomment) { @@ -127,8 +128,9 @@ void Utils::unCommentSelection(QPlainTextEdit *edit) cursor.removeSelectedText(); break; } - if (!text.at(i).isSpace()) + if (!text.at(i).isSpace()) { break; + } ++i; } } else { @@ -141,8 +143,9 @@ void Utils::unCommentSelection(QPlainTextEdit *edit) // adjust selection when commenting out if (hasSelection && !doCStyleUncomment && !doCppStyleUncomment) { cursor = edit->textCursor(); - if (!doCStyleComment) + if (!doCStyleComment) { start = startBlock.position(); // move the double slashes into the selection + } int lastSelPos = anchorIsStart ? cursor.position() : cursor.anchor(); if (anchorIsStart) { cursor.setPosition(start); @@ -156,4 +159,3 @@ void Utils::unCommentSelection(QPlainTextEdit *edit) cursor.endEditBlock(); } - diff --git a/ground/openpilotgcs/src/libs/utils/uncommentselection.h b/ground/openpilotgcs/src/libs/utils/uncommentselection.h index 6be0dca08..56c9f376d 100644 --- a/ground/openpilotgcs/src/libs/utils/uncommentselection.h +++ b/ground/openpilotgcs/src/libs/utils/uncommentselection.h @@ -4,25 +4,25 @@ * @file uncommentselection.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,9 +36,7 @@ class QPlainTextEdit; QT_END_NAMESPACE namespace Utils { - QTCREATOR_UTILS_EXPORT void unCommentSelection(QPlainTextEdit *edit); - } // end of namespace Utils #endif // UNCOMMENTSELECTION_H diff --git a/ground/openpilotgcs/src/libs/utils/utils_global.h b/ground/openpilotgcs/src/libs/utils/utils_global.h index 45bbf5b45..dce6149a7 100644 --- a/ground/openpilotgcs/src/libs/utils/utils_global.h +++ b/ground/openpilotgcs/src/libs/utils/utils_global.h @@ -4,25 +4,25 @@ * @file utils_global.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.cpp b/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.cpp index 1cb0d1e73..723316038 100644 --- a/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.cpp @@ -4,25 +4,25 @@ * @file welcomemodetreewidget.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,27 +34,26 @@ #include namespace Utils { - void WelcomeModeLabel::setStyledText(const QString &text) { - QString rc = QLatin1String( - "" - "" - "

" - ""); + QString rc = QLatin1String( + "" + "" + "

" + ""); + rc += text; rc += QLatin1String("


"); setText(rc); } -struct WelcomeModeTreeWidgetPrivate -{ +struct WelcomeModeTreeWidgetPrivate { WelcomeModeTreeWidgetPrivate() {} QIcon bullet; }; WelcomeModeTreeWidget::WelcomeModeTreeWidget(QWidget *parent) : - QTreeWidget(parent), m_d(new WelcomeModeTreeWidgetPrivate) + QTreeWidget(parent), m_d(new WelcomeModeTreeWidgetPrivate) { m_d->bullet = QIcon(QLatin1String(":/welcome/images/list_bullet_arrow.png")); connect(this, SIGNAL(itemClicked(QTreeWidgetItem *, int)), @@ -81,6 +80,7 @@ QSize WelcomeModeTreeWidget::sizeHint() const QTreeWidgetItem *WelcomeModeTreeWidget::addItem(const QString &label, const QString &data, const QString &toolTip) { QTreeWidgetItem *item = new QTreeWidgetItem(this); + item->setIcon(0, m_d->bullet); item->setSizeHint(0, QSize(24, 30)); QLabel *lbl = new QLabel(label); @@ -90,21 +90,22 @@ QTreeWidgetItem *WelcomeModeTreeWidget::addItem(const QString &label, const QStr QBoxLayout *lay = new QVBoxLayout; lay->setContentsMargins(3, 2, 0, 0); lay->addWidget(lbl); - QWidget *wdg = new QWidget; + QWidget *wdg = new QWidget; wdg->setLayout(lay); setItemWidget(item, 1, wdg); item->setData(0, Qt::UserRole, data); - if (!toolTip.isEmpty()) + if (!toolTip.isEmpty()) { wdg->setToolTip(toolTip); + } return item; - } void WelcomeModeTreeWidget::slotAddNewsItem(const QString &title, const QString &description, const QString &link) { - int itemWidth = width()-header()->sectionSize(0); + int itemWidth = width() - header()->sectionSize(0); QFont f = font(); - QString elidedText = QFontMetrics(f).elidedText(description, Qt::ElideRight, itemWidth); + QString elidedText = QFontMetrics(f).elidedText(description, Qt::ElideRight, itemWidth); + f.setBold(true); QString elidedTitle = QFontMetrics(f).elidedText(title, Qt::ElideRight, itemWidth); QString data = QString::fromLatin1("%1
%2").arg(elidedTitle).arg(elidedText); @@ -115,5 +116,4 @@ void WelcomeModeTreeWidget::slotItemClicked(QTreeWidgetItem *item) { emit activated(item->data(0, Qt::UserRole).toString()); } - } diff --git a/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.h b/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.h index d97cfd0c1..67374c469 100644 --- a/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.h +++ b/ground/openpilotgcs/src/libs/utils/welcomemodetreewidget.h @@ -4,25 +4,25 @@ * @file welcomemodetreewidget.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,12 +35,10 @@ #include namespace Utils { - struct WelcomeModeTreeWidgetPrivate; struct WelcomeModeLabelPrivate; -class QTCREATOR_UTILS_EXPORT WelcomeModeLabel : public QLabel -{ +class QTCREATOR_UTILS_EXPORT WelcomeModeLabel : public QLabel { Q_OBJECT public: WelcomeModeLabel(QWidget *parent) : QLabel(parent) {}; @@ -48,14 +46,13 @@ public: WelcomeModeLabelPrivate *m_d; }; -class QTCREATOR_UTILS_EXPORT WelcomeModeTreeWidget : public QTreeWidget -{ +class QTCREATOR_UTILS_EXPORT WelcomeModeTreeWidget : public QTreeWidget { Q_OBJECT public: WelcomeModeTreeWidget(QWidget *parent = 0); ~WelcomeModeTreeWidget(); - QTreeWidgetItem *addItem(const QString &label, const QString &data,const QString &toolTip = QString::null); + QTreeWidgetItem *addItem(const QString &label, const QString &data, const QString &toolTip = QString::null); public slots: void slotAddNewsItem(const QString &title, const QString &description, const QString &link); @@ -73,7 +70,6 @@ private slots: private: WelcomeModeTreeWidgetPrivate *m_d; }; - } #endif // WELCOMEMODETREEWIDGET_H diff --git a/ground/openpilotgcs/src/libs/utils/winutils.cpp b/ground/openpilotgcs/src/libs/utils/winutils.cpp index ae80f26ef..67dba1bb6 100644 --- a/ground/openpilotgcs/src/libs/utils/winutils.cpp +++ b/ground/openpilotgcs/src/libs/utils/winutils.cpp @@ -4,25 +4,25 @@ * @file winutils.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,15 +36,15 @@ #include namespace Utils { - QTCREATOR_UTILS_EXPORT QString winErrorMessage(unsigned long error) { - QString rc = QString::fromLatin1("#%1: ").arg(error); + QString rc = QString::fromLatin1("#%1: ").arg(error); ushort *lpMsgBuf; const int len = FormatMessage( - FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, - NULL, error, 0, (LPTSTR)&lpMsgBuf, 0, NULL); + FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, + NULL, error, 0, (LPTSTR)&lpMsgBuf, 0, NULL); + if (len) { rc = QString::fromUtf16(lpMsgBuf, len); LocalFree(lpMsgBuf); @@ -59,9 +59,9 @@ QTCREATOR_UTILS_EXPORT QString winGetDLLVersion(WinDLLVersionType t, QString *errorMessage) { // Resolve required symbols from the version.dll - typedef DWORD (APIENTRY *GetFileVersionInfoSizeProtoType)(LPCTSTR, LPDWORD); - typedef BOOL (APIENTRY *GetFileVersionInfoWProtoType)(LPCWSTR, DWORD, DWORD, LPVOID); - typedef BOOL (APIENTRY *VerQueryValueWProtoType)(const LPVOID, LPWSTR lpSubBlock, LPVOID, PUINT); + typedef DWORD (APIENTRY * GetFileVersionInfoSizeProtoType)(LPCTSTR, LPDWORD); + typedef BOOL (APIENTRY * GetFileVersionInfoWProtoType)(LPCWSTR, DWORD, DWORD, LPVOID); + typedef BOOL (APIENTRY * VerQueryValueWProtoType)(const LPVOID, LPWSTR lpSubBlock, LPVOID, PUINT); const char *versionDLLC = "version.dll"; QLibrary versionLib(QLatin1String(versionDLLC), 0); @@ -81,7 +81,7 @@ QTCREATOR_UTILS_EXPORT QString winGetDLLVersion(WinDLLVersionType t, // Now go ahead, read version info resource DWORD dummy = 0; const LPCTSTR fileName = reinterpret_cast(name.utf16()); // MinGWsy - const DWORD infoSize = (*getFileVersionInfoSizeW)(fileName, &dummy); + const DWORD infoSize = (*getFileVersionInfoSizeW)(fileName, &dummy); if (infoSize == 0) { *errorMessage = QString::fromLatin1("Unable to determine the size of the version information of %1: %2").arg(name, winErrorMessage(GetLastError())); return QString(); @@ -92,12 +92,12 @@ QTCREATOR_UTILS_EXPORT QString winGetDLLVersion(WinDLLVersionType t, *errorMessage = QString::fromLatin1("Unable to determine the version information of %1: %2").arg(name, winErrorMessage(GetLastError())); return QString(); } - VS_FIXEDFILEINFO *versionInfo; + VS_FIXEDFILEINFO *versionInfo; UINT len = 0; if (!(*verQueryValueW)(data, TEXT("\\"), &versionInfo, &len)) { *errorMessage = QString::fromLatin1("Unable to determine version string of %1: %2").arg(name, winErrorMessage(GetLastError())); return QString(); - } + } QString rc; switch (t) { case WinDLLFileVersion: @@ -109,5 +109,4 @@ QTCREATOR_UTILS_EXPORT QString winGetDLLVersion(WinDLLVersionType t, } return rc; } - } // namespace Utils diff --git a/ground/openpilotgcs/src/libs/utils/winutils.h b/ground/openpilotgcs/src/libs/utils/winutils.h index acd8fe506..7d268e011 100644 --- a/ground/openpilotgcs/src/libs/utils/winutils.h +++ b/ground/openpilotgcs/src/libs/utils/winutils.h @@ -4,25 +4,25 @@ * @file winutils.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,7 +36,6 @@ class QString; QT_END_NAMESPACE namespace Utils { - // Helper to format a Windows error message, taking the // code as returned by the GetLastError()-API. QTCREATOR_UTILS_EXPORT QString winErrorMessage(unsigned long error); diff --git a/ground/openpilotgcs/src/libs/utils/worldmagmodel.cpp b/ground/openpilotgcs/src/libs/utils/worldmagmodel.cpp index d93588684..37a6f356f 100644 --- a/ground/openpilotgcs/src/libs/utils/worldmagmodel.cpp +++ b/ground/openpilotgcs/src/libs/utils/worldmagmodel.cpp @@ -49,1037 +49,1008 @@ #include #include -#define RAD2DEG(rad) ((rad) * (180.0 / M_PI)) -#define DEG2RAD(deg) ((deg) * (M_PI / 180.0)) +#define RAD2DEG(rad) ((rad) * (180.0 / M_PI)) +#define DEG2RAD(deg) ((deg) * (M_PI / 180.0)) // updated coeffs available from http://www.ngdc.noaa.gov/geomag/WMM/wmm_ddownload.shtml const double CoeffFile[91][6] = { - {0, 0, 0, 0, 0, 0}, - {1, 0, -29496.6, 0.0, 11.6, 0.0}, - {1, 1, -1586.3, 4944.4, 16.5, -25.9}, - {2, 0, -2396.6, 0.0, -12.1, 0.0}, - {2, 1, 3026.1, -2707.7, -4.4, -22.5}, - {2, 2, 1668.6, -576.1, 1.9, -11.8}, - {3, 0, 1340.1, 0.0, 0.4, 0.0}, - {3, 1, -2326.2, -160.2, -4.1, 7.3}, - {3, 2, 1231.9, 251.9, -2.9, -3.9}, - {3, 3, 634.0, -536.6, -7.7, -2.6}, - {4, 0, 912.6, 0.0, -1.8, 0.0}, - {4, 1, 808.9, 286.4, 2.3, 1.1}, - {4, 2, 166.7, -211.2, -8.7, 2.7}, - {4, 3, -357.1, 164.3, 4.6, 3.9}, - {4, 4, 89.4, -309.1, -2.1, -0.8}, - {5, 0, -230.9, 0.0, -1.0, 0.0}, - {5, 1, 357.2, 44.6, 0.6, 0.4}, - {5, 2, 200.3, 188.9, -1.8, 1.8}, - {5, 3, -141.1, -118.2, -1.0, 1.2}, - {5, 4, -163.0, 0.0, 0.9, 4.0}, - {5, 5, -7.8, 100.9, 1.0, -0.6}, - {6, 0, 72.8, 0.0, -0.2, 0.0}, - {6, 1, 68.6, -20.8, -0.2, -0.2}, - {6, 2, 76.0, 44.1, -0.1, -2.1}, - {6, 3, -141.4, 61.5, 2.0, -0.4}, - {6, 4, -22.8, -66.3, -1.7, -0.6}, - {6, 5, 13.2, 3.1, -0.3, 0.5}, - {6, 6, -77.9, 55.0, 1.7, 0.9}, - {7, 0, 80.5, 0.0, 0.1, 0.0}, - {7, 1, -75.1, -57.9, -0.1, 0.7}, - {7, 2, -4.7, -21.1, -0.6, 0.3}, - {7, 3, 45.3, 6.5, 1.3, -0.1}, - {7, 4, 13.9, 24.9, 0.4, -0.1}, - {7, 5, 10.4, 7.0, 0.3, -0.8}, - {7, 6, 1.7, -27.7, -0.7, -0.3}, - {7, 7, 4.9, -3.3, 0.6, 0.3}, - {8, 0, 24.4, 0.0, -0.1, 0.0}, - {8, 1, 8.1, 11.0, 0.1, -0.1}, - {8, 2, -14.5, -20.0, -0.6, 0.2}, - {8, 3, -5.6, 11.9, 0.2, 0.4}, - {8, 4, -19.3, -17.4, -0.2, 0.4}, - {8, 5, 11.5, 16.7, 0.3, 0.1}, - {8, 6, 10.9, 7.0, 0.3, -0.1}, - {8, 7, -14.1, -10.8, -0.6, 0.4}, - {8, 8, -3.7, 1.7, 0.2, 0.3}, - {9, 0, 5.4, 0.0, 0.0, 0.0}, - {9, 1, 9.4, -20.5, -0.1, 0.0}, - {9, 2, 3.4, 11.5, 0.0, -0.2}, - {9, 3, -5.2, 12.8, 0.3, 0.0}, - {9, 4, 3.1, -7.2, -0.4, -0.1}, - {9, 5, -12.4, -7.4, -0.3, 0.1}, - {9, 6, -0.7, 8.0, 0.1, 0.0}, - {9, 7, 8.4, 2.1, -0.1, -0.2}, - {9, 8, -8.5, -6.1, -0.4, 0.3}, - {9, 9, -10.1, 7.0, -0.2, 0.2}, - {10, 0, -2.0, 0.0, 0.0, 0.0}, - {10, 1, -6.3, 2.8, 0.0, 0.1}, - {10, 2, 0.9, -0.1, -0.1, -0.1}, - {10, 3, -1.1, 4.7, 0.2, 0.0}, - {10, 4, -0.2, 4.4, 0.0, -0.1}, - {10, 5, 2.5, -7.2, -0.1, -0.1}, - {10, 6, -0.3, -1.0, -0.2, 0.0}, - {10, 7, 2.2, -3.9, 0.0, -0.1}, - {10, 8, 3.1, -2.0, -0.1, -0.2}, - {10, 9, -1.0, -2.0, -0.2, 0.0}, - {10, 10, -2.8, -8.3, -0.2, -0.1}, - {11, 0, 3.0, 0.0, 0.0, 0.0}, - {11, 1, -1.5, 0.2, 0.0, 0.0}, - {11, 2, -2.1, 1.7, 0.0, 0.1}, - {11, 3, 1.7, -0.6, 0.1, 0.0}, - {11, 4, -0.5, -1.8, 0.0, 0.1}, - {11, 5, 0.5, 0.9, 0.0, 0.0}, - {11, 6, -0.8, -0.4, 0.0, 0.1}, - {11, 7, 0.4, -2.5, 0.0, 0.0}, - {11, 8, 1.8, -1.3, 0.0, -0.1}, - {11, 9, 0.1, -2.1, 0.0, -0.1}, - {11, 10, 0.7, -1.9, -0.1, 0.0}, - {11, 11, 3.8, -1.8, 0.0, -0.1}, - {12, 0, -2.2, 0.0, 0.0, 0.0}, - {12, 1, -0.2, -0.9, 0.0, 0.0}, - {12, 2, 0.3, 0.3, 0.1, 0.0}, - {12, 3, 1.0, 2.1, 0.1, 0.0}, - {12, 4, -0.6, -2.5, -0.1, 0.0}, - {12, 5, 0.9, 0.5, 0.0, 0.0}, - {12, 6, -0.1, 0.6, 0.0, 0.1}, - {12, 7, 0.5, 0.0, 0.0, 0.0}, - {12, 8, -0.4, 0.1, 0.0, 0.0}, - {12, 9, -0.4, 0.3, 0.0, 0.0}, - {12, 10, 0.2, -0.9, 0.0, 0.0}, - {12, 11, -0.8, -0.2, -0.1, 0.0}, - {12, 12, 0.0, 0.9, 0.1, 0.0} + { 0, 0, 0, 0, 0, 0 }, + { 1, 0, -29496.6, 0.0, 11.6, 0.0 }, + { 1, 1, -1586.3, 4944.4, 16.5, -25.9 }, + { 2, 0, -2396.6, 0.0, -12.1, 0.0 }, + { 2, 1, 3026.1, -2707.7, -4.4, -22.5 }, + { 2, 2, 1668.6, -576.1, 1.9, -11.8 }, + { 3, 0, 1340.1, 0.0, 0.4, 0.0 }, + { 3, 1, -2326.2, -160.2, -4.1, 7.3 }, + { 3, 2, 1231.9, 251.9, -2.9, -3.9 }, + { 3, 3, 634.0, -536.6, -7.7, -2.6 }, + { 4, 0, 912.6, 0.0, -1.8, 0.0 }, + { 4, 1, 808.9, 286.4, 2.3, 1.1 }, + { 4, 2, 166.7, -211.2, -8.7, 2.7 }, + { 4, 3, -357.1, 164.3, 4.6, 3.9 }, + { 4, 4, 89.4, -309.1, -2.1, -0.8 }, + { 5, 0, -230.9, 0.0, -1.0, 0.0 }, + { 5, 1, 357.2, 44.6, 0.6, 0.4 }, + { 5, 2, 200.3, 188.9, -1.8, 1.8 }, + { 5, 3, -141.1, -118.2, -1.0, 1.2 }, + { 5, 4, -163.0, 0.0, 0.9, 4.0 }, + { 5, 5, -7.8, 100.9, 1.0, -0.6 }, + { 6, 0, 72.8, 0.0, -0.2, 0.0 }, + { 6, 1, 68.6, -20.8, -0.2, -0.2 }, + { 6, 2, 76.0, 44.1, -0.1, -2.1 }, + { 6, 3, -141.4, 61.5, 2.0, -0.4 }, + { 6, 4, -22.8, -66.3, -1.7, -0.6 }, + { 6, 5, 13.2, 3.1, -0.3, 0.5 }, + { 6, 6, -77.9, 55.0, 1.7, 0.9 }, + { 7, 0, 80.5, 0.0, 0.1, 0.0 }, + { 7, 1, -75.1, -57.9, -0.1, 0.7 }, + { 7, 2, -4.7, -21.1, -0.6, 0.3 }, + { 7, 3, 45.3, 6.5, 1.3, -0.1 }, + { 7, 4, 13.9, 24.9, 0.4, -0.1 }, + { 7, 5, 10.4, 7.0, 0.3, -0.8 }, + { 7, 6, 1.7, -27.7, -0.7, -0.3 }, + { 7, 7, 4.9, -3.3, 0.6, 0.3 }, + { 8, 0, 24.4, 0.0, -0.1, 0.0 }, + { 8, 1, 8.1, 11.0, 0.1, -0.1 }, + { 8, 2, -14.5, -20.0, -0.6, 0.2 }, + { 8, 3, -5.6, 11.9, 0.2, 0.4 }, + { 8, 4, -19.3, -17.4, -0.2, 0.4 }, + { 8, 5, 11.5, 16.7, 0.3, 0.1 }, + { 8, 6, 10.9, 7.0, 0.3, -0.1 }, + { 8, 7, -14.1, -10.8, -0.6, 0.4 }, + { 8, 8, -3.7, 1.7, 0.2, 0.3 }, + { 9, 0, 5.4, 0.0, 0.0, 0.0 }, + { 9, 1, 9.4, -20.5, -0.1, 0.0 }, + { 9, 2, 3.4, 11.5, 0.0, -0.2 }, + { 9, 3, -5.2, 12.8, 0.3, 0.0 }, + { 9, 4, 3.1, -7.2, -0.4, -0.1 }, + { 9, 5, -12.4, -7.4, -0.3, 0.1 }, + { 9, 6, -0.7, 8.0, 0.1, 0.0 }, + { 9, 7, 8.4, 2.1, -0.1, -0.2 }, + { 9, 8, -8.5, -6.1, -0.4, 0.3 }, + { 9, 9, -10.1, 7.0, -0.2, 0.2 }, + { 10, 0, -2.0, 0.0, 0.0, 0.0 }, + { 10, 1, -6.3, 2.8, 0.0, 0.1 }, + { 10, 2, 0.9, -0.1, -0.1, -0.1 }, + { 10, 3, -1.1, 4.7, 0.2, 0.0 }, + { 10, 4, -0.2, 4.4, 0.0, -0.1 }, + { 10, 5, 2.5, -7.2, -0.1, -0.1 }, + { 10, 6, -0.3, -1.0, -0.2, 0.0 }, + { 10, 7, 2.2, -3.9, 0.0, -0.1 }, + { 10, 8, 3.1, -2.0, -0.1, -0.2 }, + { 10, 9, -1.0, -2.0, -0.2, 0.0 }, + { 10, 10, -2.8, -8.3, -0.2, -0.1 }, + { 11, 0, 3.0, 0.0, 0.0, 0.0 }, + { 11, 1, -1.5, 0.2, 0.0, 0.0 }, + { 11, 2, -2.1, 1.7, 0.0, 0.1 }, + { 11, 3, 1.7, -0.6, 0.1, 0.0 }, + { 11, 4, -0.5, -1.8, 0.0, 0.1 }, + { 11, 5, 0.5, 0.9, 0.0, 0.0 }, + { 11, 6, -0.8, -0.4, 0.0, 0.1 }, + { 11, 7, 0.4, -2.5, 0.0, 0.0 }, + { 11, 8, 1.8, -1.3, 0.0, -0.1 }, + { 11, 9, 0.1, -2.1, 0.0, -0.1 }, + { 11, 10, 0.7, -1.9, -0.1, 0.0 }, + { 11, 11, 3.8, -1.8, 0.0, -0.1 }, + { 12, 0, -2.2, 0.0, 0.0, 0.0 }, + { 12, 1, -0.2, -0.9, 0.0, 0.0 }, + { 12, 2, 0.3, 0.3, 0.1, 0.0 }, + { 12, 3, 1.0, 2.1, 0.1, 0.0 }, + { 12, 4, -0.6, -2.5, -0.1, 0.0 }, + { 12, 5, 0.9, 0.5, 0.0, 0.0 }, + { 12, 6, -0.1, 0.6, 0.0, 0.1 }, + { 12, 7, 0.5, 0.0, 0.0, 0.0 }, + { 12, 8, -0.4, 0.1, 0.0, 0.0 }, + { 12, 9, -0.4, 0.3, 0.0, 0.0 }, + { 12, 10, 0.2, -0.9, 0.0, 0.0 }, + { 12, 11, -0.8, -0.2, -0.1, 0.0 }, + { 12, 12, 0.0, 0.9, 0.1, 0.0 } }; namespace Utils { +WorldMagModel::WorldMagModel() +{ + Initialize(); +} - WorldMagModel::WorldMagModel() - { - Initialize(); +/** + * @brief + * @param[in] LLA The longitude-latitude-altitude coordinate to compute the magnetic field at + * @param[in] Month + * @param[in] Day + * @param[in] Year + * @param[out] Be The resulting magnetic field at that location and time in [mGau](?) + * @returns 0 if successful, negative otherwise. + */ +int WorldMagModel::GetMagVector(double LLA[3], int Month, int Day, int Year, double Be[3]) +{ + double Lat = LLA[0]; + double Lon = LLA[1]; + double AltEllipsoid = LLA[2] / 1000.0; // convert to km + + // *********** + // range check supplied params + + if (Lat < -90) { + return -1; // error } + if (Lat > 90) { + return -2; // error + } + if (Lon < -180) { + return -3; // error + } + if (Lon > 180) { + return -4; // error + } + // *********** - /** - * @brief - * @param[in] LLA The longitude-latitude-altitude coordinate to compute the magnetic field at - * @param[in] Month - * @param[in] Day - * @param[in] Year - * @param[out] Be The resulting magnetic field at that location and time in [mGau](?) - * @returns 0 if successful, negative otherwise. + WMMtype_CoordSpherical CoordSpherical; + WMMtype_CoordGeodetic CoordGeodetic; + WMMtype_GeoMagneticElements GeoMagneticElements; + + Initialize(); + + CoordGeodetic.lambda = Lon; + CoordGeodetic.phi = Lat; + CoordGeodetic.HeightAboveEllipsoid = AltEllipsoid; + + // Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report + GeodeticToSpherical(&CoordGeodetic, &CoordSpherical); + + if (DateToYear(Month, Day, Year) < 0) { + return -5; // error + } + // Compute the geoMagnetic field elements and their time change + if (Geomag(&CoordSpherical, &CoordGeodetic, &GeoMagneticElements) < 0) { + return -6; // error + } + // set the returned values + Be[0] = GeoMagneticElements.X * 1e-2; + Be[1] = GeoMagneticElements.Y * 1e-2; + Be[2] = GeoMagneticElements.Z * 1e-2; + + // *********** + + return 0; // OK +} + +void WorldMagModel::Initialize() +{ // Sets default values for WMM subroutines. + // UPDATES : Ellip and MagneticModel + + // Sets WGS-84 parameters + Ellip.a = 6378.137; // semi-major axis of the ellipsoid in km + Ellip.b = 6356.7523142; // semi-minor axis of the ellipsoid in km + Ellip.fla = 1 / 298.257223563; // flattening + Ellip.eps = sqrt(1 - (Ellip.b * Ellip.b) / (Ellip.a * Ellip.a)); // first eccentricity + Ellip.epssq = (Ellip.eps * Ellip.eps); // first eccentricity squared + Ellip.re = 6371.2; // Earth's radius in km + + // Sets Magnetic Model parameters + MagneticModel.nMax = WMM_MAX_MODEL_DEGREES; + MagneticModel.nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES; + MagneticModel.SecularVariationUsed = 0; + + // Really, Really needs to be read from a file - out of date in 2015 at latest + MagneticModel.EditionDate = 5.7863328170559505e-307; + MagneticModel.epoch = 2010.0; + sprintf(MagneticModel.ModelName, "WMM-2010"); +} + + +int WorldMagModel::Geomag(WMMtype_CoordSpherical *CoordSpherical, WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_GeoMagneticElements *GeoMagneticElements) +/* + The main subroutine that calls a sequence of WMM sub-functions to calculate the magnetic field elements for a single point. + The function expects the model coefficients and point coordinates as input and returns the magnetic field elements and + their rate of change. Though, this subroutine can be called successively to calculate a time series, profile or grid + of magnetic field, these are better achieved by the subroutine WMM_Grid. + + INPUT: Ellip + CoordSpherical + CoordGeodetic + TimedMagneticModel + + OUTPUT : GeoMagneticElements + */ +{ + WMMtype_MagneticResults MagneticResultsSph; + WMMtype_MagneticResults MagneticResultsGeo; + WMMtype_MagneticResults MagneticResultsSphVar; + WMMtype_MagneticResults MagneticResultsGeoVar; + WMMtype_LegendreFunction LegendreFunction; + WMMtype_SphericalHarmonicVariables SphVariables; + + // Compute Spherical Harmonic variables + ComputeSphericalHarmonicVariables(CoordSpherical, MagneticModel.nMax, &SphVariables); + + // Compute ALF + if (AssociatedLegendreFunction(CoordSpherical, MagneticModel.nMax, &LegendreFunction) < 0) { + return -1; // error + } + // Accumulate the spherical harmonic coefficients + Summation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSph); + + // Sum the Secular Variation Coefficients + SecVarSummation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSphVar); + + // Map the computed Magnetic fields to Geodeitic coordinates + RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSph, &MagneticResultsGeo); + + // Map the secular variation field components to Geodetic coordinates + RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSphVar, &MagneticResultsGeoVar); + + // Calculate the Geomagnetic elements, Equation 18 , WMM Technical report + CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); + + // Calculate the secular variation of each of the Geomagnetic elements + CalculateSecularVariation(&MagneticResultsGeoVar, GeoMagneticElements); + + return 0; // OK +} + +void WorldMagModel::ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_SphericalHarmonicVariables *SphVariables) +{ + /* Computes Spherical variables + Variables computed are (a/r)^(n+2), cos_m(lamda) and sin_m(lambda) for spherical harmonic + summations. (Equations 10-12 in the WMM Technical Report) + INPUT Ellip data structure with the following elements + float a; semi-major axis of the ellipsoid + float b; semi-minor axis of the ellipsoid + float fla; flattening + float epssq; first eccentricity squared + float eps; first eccentricity + float re; mean radius of ellipsoid + CoordSpherical A data structure with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) + nMax integer ( Maxumum degree of spherical harmonic secular model)\ + + OUTPUT SphVariables Pointer to the data structure with the following elements + float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES+1]; [earth_reference_radius_km sph. radius ]^n + float cos_mlambda[WMM_MAX_MODEL_DEGREES+1]; cp(m) - cosine of (mspherical coord. longitude) + float sin_mlambda[WMM_MAX_MODEL_DEGREES+1]; sp(m) - sine of (mspherical coord. longitude) */ - int WorldMagModel::GetMagVector(double LLA[3], int Month, int Day, int Year, double Be[3]) - { - double Lat = LLA[0]; - double Lon = LLA[1]; - double AltEllipsoid = LLA[2]/1000.0; // convert to km + double cos_lambda = cos(DEG2RAD(CoordSpherical->lambda)); + double sin_lambda = sin(DEG2RAD(CoordSpherical->lambda)); - // *********** - // range check supplied params + /* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2) + for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ - if (Lat < -90) return -1; // error - if (Lat > 90) return -2; // error - - if (Lon < -180) return -3; // error - if (Lon > 180) return -4; // error - - // *********** - - WMMtype_CoordSpherical CoordSpherical; - WMMtype_CoordGeodetic CoordGeodetic; - WMMtype_GeoMagneticElements GeoMagneticElements; - - Initialize(); - - CoordGeodetic.lambda = Lon; - CoordGeodetic.phi = Lat; - CoordGeodetic.HeightAboveEllipsoid = AltEllipsoid; - - // Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report - GeodeticToSpherical(&CoordGeodetic, &CoordSpherical); - - if (DateToYear(Month, Day, Year) < 0) - return -5; // error - - // Compute the geoMagnetic field elements and their time change - if (Geomag(&CoordSpherical, &CoordGeodetic, &GeoMagneticElements) < 0) - return -6; // error - - // set the returned values - Be[0] = GeoMagneticElements.X * 1e-2; - Be[1] = GeoMagneticElements.Y * 1e-2; - Be[2] = GeoMagneticElements.Z * 1e-2; - - // *********** - - return 0; // OK + SphVariables->RelativeRadiusPower[0] = (Ellip.re / CoordSpherical->r) * (Ellip.re / CoordSpherical->r); + for (int n = 1; n <= nMax; n++) { + SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n - 1] * (Ellip.re / CoordSpherical->r); } - void WorldMagModel::Initialize() - { // Sets default values for WMM subroutines. - // UPDATES : Ellip and MagneticModel - - // Sets WGS-84 parameters - Ellip.a = 6378.137; // semi-major axis of the ellipsoid in km - Ellip.b = 6356.7523142; // semi-minor axis of the ellipsoid in km - Ellip.fla = 1 / 298.257223563; // flattening - Ellip.eps = sqrt(1 - (Ellip.b * Ellip.b) / (Ellip.a * Ellip.a)); // first eccentricity - Ellip.epssq = (Ellip.eps * Ellip.eps); // first eccentricity squared - Ellip.re = 6371.2; // Earth's radius in km - - // Sets Magnetic Model parameters - MagneticModel.nMax = WMM_MAX_MODEL_DEGREES; - MagneticModel.nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES; - MagneticModel.SecularVariationUsed = 0; - - // Really, Really needs to be read from a file - out of date in 2015 at latest - MagneticModel.EditionDate = 5.7863328170559505e-307; - MagneticModel.epoch = 2010.0; - sprintf(MagneticModel.ModelName, "WMM-2010"); - } - - - int WorldMagModel::Geomag(WMMtype_CoordSpherical *CoordSpherical, WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_GeoMagneticElements *GeoMagneticElements) /* - The main subroutine that calls a sequence of WMM sub-functions to calculate the magnetic field elements for a single point. - The function expects the model coefficients and point coordinates as input and returns the magnetic field elements and - their rate of change. Though, this subroutine can be called successively to calculate a time series, profile or grid - of magnetic field, these are better achieved by the subroutine WMM_Grid. + Compute cos(m*lambda), sin(m*lambda) for m = 0 ... nMax + cos(a + b) = cos(a)*cos(b) - sin(a)*sin(b) + sin(a + b) = cos(a)*sin(b) + sin(a)*cos(b) + */ + SphVariables->cos_mlambda[0] = 1.0; + SphVariables->sin_mlambda[0] = 0.0; - INPUT: Ellip - CoordSpherical - CoordGeodetic - TimedMagneticModel + SphVariables->cos_mlambda[1] = cos_lambda; + SphVariables->sin_mlambda[1] = sin_lambda; + for (int m = 2; m <= nMax; m++) { + SphVariables->cos_mlambda[m] = SphVariables->cos_mlambda[m - 1] * cos_lambda - SphVariables->sin_mlambda[m - 1] * sin_lambda; + SphVariables->sin_mlambda[m] = SphVariables->cos_mlambda[m - 1] * sin_lambda + SphVariables->sin_mlambda[m - 1] * cos_lambda; + } +} - OUTPUT : GeoMagneticElements - */ - { - WMMtype_MagneticResults MagneticResultsSph; - WMMtype_MagneticResults MagneticResultsGeo; - WMMtype_MagneticResults MagneticResultsSphVar; - WMMtype_MagneticResults MagneticResultsGeoVar; - WMMtype_LegendreFunction LegendreFunction; - WMMtype_SphericalHarmonicVariables SphVariables; +int WorldMagModel::AssociatedLegendreFunction(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_LegendreFunction *LegendreFunction) +{ + /* Computes all of the Schmidt-semi normalized associated Legendre + functions up to degree nMax. If nMax <= 16, function WMM_PcupLow is used. + Otherwise WMM_PcupHigh is called. + INPUT CoordSpherical A data structure with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) + nMax integer ( Maxumum degree of spherical harmonic secular model) + LegendreFunction Pointer to data structure with the following elements + float *Pcup; ( pointer to store Legendre Function ) + float *dPcup; ( pointer to store Derivative of Lagendre function ) - // Compute Spherical Harmonic variables - ComputeSphericalHarmonicVariables(CoordSpherical, MagneticModel.nMax, &SphVariables); + OUTPUT LegendreFunction Calculated Legendre variables in the data structure + */ - // Compute ALF - if (AssociatedLegendreFunction(CoordSpherical, MagneticModel.nMax, &LegendreFunction) < 0) + double sin_phi = sin(DEG2RAD(CoordSpherical->phig)); // sin (geocentric latitude) + + if (nMax <= 16 || (1 - fabs(sin_phi)) < 1.0e-10) { /* If nMax is less tha 16 or at the poles */ + PcupLow(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax); + } else { + if (PcupHigh(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0) { return -1; // error - - // Accumulate the spherical harmonic coefficients - Summation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSph); - - // Sum the Secular Variation Coefficients - SecVarSummation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSphVar); - - // Map the computed Magnetic fields to Geodeitic coordinates - RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSph, &MagneticResultsGeo); - - // Map the secular variation field components to Geodetic coordinates - RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSphVar, &MagneticResultsGeoVar); - - // Calculate the Geomagnetic elements, Equation 18 , WMM Technical report - CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); - - // Calculate the secular variation of each of the Geomagnetic elements - CalculateSecularVariation(&MagneticResultsGeoVar, GeoMagneticElements); - - return 0; // OK - } - - void WorldMagModel::ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_SphericalHarmonicVariables *SphVariables) - { - /* Computes Spherical variables - Variables computed are (a/r)^(n+2), cos_m(lamda) and sin_m(lambda) for spherical harmonic - summations. (Equations 10-12 in the WMM Technical Report) - INPUT Ellip data structure with the following elements - float a; semi-major axis of the ellipsoid - float b; semi-minor axis of the ellipsoid - float fla; flattening - float epssq; first eccentricity squared - float eps; first eccentricity - float re; mean radius of ellipsoid - CoordSpherical A data structure with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) - nMax integer ( Maxumum degree of spherical harmonic secular model)\ - - OUTPUT SphVariables Pointer to the data structure with the following elements - float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES+1]; [earth_reference_radius_km sph. radius ]^n - float cos_mlambda[WMM_MAX_MODEL_DEGREES+1]; cp(m) - cosine of (mspherical coord. longitude) - float sin_mlambda[WMM_MAX_MODEL_DEGREES+1]; sp(m) - sine of (mspherical coord. longitude) - */ - double cos_lambda = cos(DEG2RAD(CoordSpherical->lambda)); - double sin_lambda = sin(DEG2RAD(CoordSpherical->lambda)); - - /* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2) - for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ - - SphVariables->RelativeRadiusPower[0] = (Ellip.re / CoordSpherical->r) * (Ellip.re / CoordSpherical->r); - for (int n = 1; n <= nMax; n++) - SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n - 1] * (Ellip.re / CoordSpherical->r); - - /* - Compute cos(m*lambda), sin(m*lambda) for m = 0 ... nMax - cos(a + b) = cos(a)*cos(b) - sin(a)*sin(b) - sin(a + b) = cos(a)*sin(b) + sin(a)*cos(b) - */ - SphVariables->cos_mlambda[0] = 1.0; - SphVariables->sin_mlambda[0] = 0.0; - - SphVariables->cos_mlambda[1] = cos_lambda; - SphVariables->sin_mlambda[1] = sin_lambda; - for (int m = 2; m <= nMax; m++) - { - SphVariables->cos_mlambda[m] = SphVariables->cos_mlambda[m - 1] * cos_lambda - SphVariables->sin_mlambda[m - 1] * sin_lambda; - SphVariables->sin_mlambda[m] = SphVariables->cos_mlambda[m - 1] * sin_lambda + SphVariables->sin_mlambda[m - 1] * cos_lambda; } } - int WorldMagModel::AssociatedLegendreFunction(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_LegendreFunction *LegendreFunction) - { - /* Computes all of the Schmidt-semi normalized associated Legendre - functions up to degree nMax. If nMax <= 16, function WMM_PcupLow is used. - Otherwise WMM_PcupHigh is called. - INPUT CoordSpherical A data structure with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) - nMax integer ( Maxumum degree of spherical harmonic secular model) - LegendreFunction Pointer to data structure with the following elements - float *Pcup; ( pointer to store Legendre Function ) - float *dPcup; ( pointer to store Derivative of Lagendre function ) + return 0; // OK +} - OUTPUT LegendreFunction Calculated Legendre variables in the data structure - */ +void WorldMagModel::Summation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables *SphVariables, + WMMtype_CoordSpherical *CoordSpherical, + WMMtype_MagneticResults *MagneticResults) +{ + /* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using spherical harmonic summation. - double sin_phi = sin(DEG2RAD(CoordSpherical->phig)); // sin (geocentric latitude) + The vector Magnetic field is given by -grad V, where V is Geomagnetic scalar potential + The gradient in spherical coordinates is given by: - if (nMax <= 16 || (1 - fabs(sin_phi)) < 1.0e-10) /* If nMax is less tha 16 or at the poles */ - PcupLow(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax); - else - { - if (PcupHigh(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0) - return -1; // error - } + dV ^ 1 dV ^ 1 dV ^ + grad V = -- r + - -- t + -------- -- p + dr r dt r sin(t) dp - return 0; // OK - } + INPUT : LegendreFunction + MagneticModel + SphVariables + CoordSpherical + OUTPUT : MagneticResults - void WorldMagModel::Summation( WMMtype_LegendreFunction *LegendreFunction, - WMMtype_SphericalHarmonicVariables *SphVariables, - WMMtype_CoordSpherical *CoordSpherical, - WMMtype_MagneticResults *MagneticResults) - { - /* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using spherical harmonic summation. + Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov + */ - The vector Magnetic field is given by -grad V, where V is Geomagnetic scalar potential - The gradient in spherical coordinates is given by: + MagneticResults->Bz = 0.0; + MagneticResults->By = 0.0; + MagneticResults->Bx = 0.0; - dV ^ 1 dV ^ 1 dV ^ - grad V = -- r + - -- t + -------- -- p - dr r dt r sin(t) dp + for (int n = 1; n <= MagneticModel.nMax; n++) { + for (int m = 0; m <= n; m++) { + int index = (n * (n + 1) / 2 + m); - INPUT : LegendreFunction - MagneticModel - SphVariables - CoordSpherical - OUTPUT : MagneticResults - - Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov - */ - - MagneticResults->Bz = 0.0; - MagneticResults->By = 0.0; - MagneticResults->Bx = 0.0; - - for (int n = 1; n <= MagneticModel.nMax; n++) - { - for (int m = 0; m <= n; m++) - { - int index = (n * (n + 1) / 2 + m); - -/* nMax (n+2) n m m m +/* nMax (n+2) n m m m Bz = -SUM (a/r) (n+1) SUM [g cos(m p) + h sin(m p)] P (sin(phi)) - n=1 m=0 n n n */ + n=1 m=0 n n n */ /* Equation 12 in the WMM Technical report. Derivative with respect to radius.*/ - MagneticResults->Bz -= - SphVariables->RelativeRadiusPower[n] * - (get_main_field_coeff_g(index) * - SphVariables->cos_mlambda[m] + get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) - * (double)(n + 1) * LegendreFunction->Pcup[index]; + MagneticResults->Bz -= + SphVariables->RelativeRadiusPower[n] * + (get_main_field_coeff_g(index) * + SphVariables->cos_mlambda[m] + get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) + * (double)(n + 1) * LegendreFunction->Pcup[index]; /* 1 nMax (n+2) n m m m By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (get_main_field_coeff_g(index) * - SphVariables->sin_mlambda[m] - get_main_field_coeff_h(index) * SphVariables->cos_mlambda[m]) - * (double)(m) * LegendreFunction->Pcup[index]; + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (get_main_field_coeff_g(index) * + SphVariables->sin_mlambda[m] - get_main_field_coeff_h(index) * SphVariables->cos_mlambda[m]) + * (double)(m) * LegendreFunction->Pcup[index]; /* nMax (n+2) n m m m Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */ - MagneticResults->Bx -= - SphVariables->RelativeRadiusPower[n] * - (get_main_field_coeff_g(index) * - SphVariables->cos_mlambda[m] + get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) - * LegendreFunction->dPcup[index]; - - } - } - - double cos_phi = cos(DEG2RAD(CoordSpherical->phig)); - if (fabs(cos_phi) > 1.0e-10) - { - MagneticResults->By = MagneticResults->By / cos_phi; - } - else - { - /* Special calculation for component - By - at Geographic poles. - * If the user wants to avoid using this function, please make sure that - * the latitude is not exactly +/-90. An option is to make use the function - * WMM_CheckGeographicPoles. - */ - SummationSpecial(SphVariables, CoordSpherical, MagneticResults); + MagneticResults->Bx -= + SphVariables->RelativeRadiusPower[n] * + (get_main_field_coeff_g(index) * + SphVariables->cos_mlambda[m] + get_main_field_coeff_h(index) * SphVariables->sin_mlambda[m]) + * LegendreFunction->dPcup[index]; } } - void WorldMagModel::SecVarSummation( WMMtype_LegendreFunction *LegendreFunction, - WMMtype_SphericalHarmonicVariables *SphVariables, - WMMtype_CoordSpherical *CoordSpherical, - WMMtype_MagneticResults *MagneticResults) - { - /*This Function sums the secular variation coefficients to get the secular variation of the Magnetic vector. - INPUT : LegendreFunction - MagneticModel - SphVariables - CoordSpherical - OUTPUT : MagneticResults + double cos_phi = cos(DEG2RAD(CoordSpherical->phig)); + if (fabs(cos_phi) > 1.0e-10) { + MagneticResults->By = MagneticResults->By / cos_phi; + } else { + /* Special calculation for component - By - at Geographic poles. + * If the user wants to avoid using this function, please make sure that + * the latitude is not exactly +/-90. An option is to make use the function + * WMM_CheckGeographicPoles. */ + SummationSpecial(SphVariables, CoordSpherical, MagneticResults); + } +} - MagneticModel.SecularVariationUsed = true; +void WorldMagModel::SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables *SphVariables, + WMMtype_CoordSpherical *CoordSpherical, + WMMtype_MagneticResults *MagneticResults) +{ + /*This Function sums the secular variation coefficients to get the secular variation of the Magnetic vector. + INPUT : LegendreFunction + MagneticModel + SphVariables + CoordSpherical + OUTPUT : MagneticResults + */ - MagneticResults->Bz = 0.0; - MagneticResults->By = 0.0; - MagneticResults->Bx = 0.0; + MagneticModel.SecularVariationUsed = true; - for (int n = 1; n <= MagneticModel.nMaxSecVar; n++) - { - for (int m = 0; m <= n; m++) - { - int index = (n * (n + 1) / 2 + m); + MagneticResults->Bz = 0.0; + MagneticResults->By = 0.0; + MagneticResults->Bx = 0.0; -/* nMax (n+2) n m m m + for (int n = 1; n <= MagneticModel.nMaxSecVar; n++) { + for (int m = 0; m <= n; m++) { + int index = (n * (n + 1) / 2 + m); + +/* nMax (n+2) n m m m Bz = -SUM (a/r) (n+1) SUM [g cos(m p) + h sin(m p)] P (sin(phi)) - n=1 m=0 n n n */ + n=1 m=0 n n n */ /* Derivative with respect to radius.*/ - MagneticResults->Bz -= - SphVariables->RelativeRadiusPower[n] * - (get_secular_var_coeff_g(index) * - SphVariables->cos_mlambda[m] + get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) - * (double)(n + 1) * LegendreFunction->Pcup[index]; + MagneticResults->Bz -= + SphVariables->RelativeRadiusPower[n] * + (get_secular_var_coeff_g(index) * + SphVariables->cos_mlambda[m] + get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) + * (double)(n + 1) * LegendreFunction->Pcup[index]; /* 1 nMax (n+2) n m m m By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (get_secular_var_coeff_g(index) * - SphVariables->sin_mlambda[m] - get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[m]) - * (double)(m) * LegendreFunction->Pcup[index]; + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (get_secular_var_coeff_g(index) * + SphVariables->sin_mlambda[m] - get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[m]) + * (double)(m) * LegendreFunction->Pcup[index]; /* nMax (n+2) n m m m Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Derivative with respect to latitude, divided by radius. */ - MagneticResults->Bx -= - SphVariables->RelativeRadiusPower[n] * - (get_secular_var_coeff_g(index) * - SphVariables->cos_mlambda[m] + get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) - * LegendreFunction->dPcup[index]; - } - } - - double cos_phi = cos(DEG2RAD(CoordSpherical->phig)); - if (fabs(cos_phi) > 1.0e-10) - { - MagneticResults->By = MagneticResults->By / cos_phi; - } - else - { /* Special calculation for component By at Geographic poles */ - SecVarSummationSpecial(SphVariables, CoordSpherical, MagneticResults); + MagneticResults->Bx -= + SphVariables->RelativeRadiusPower[n] * + (get_secular_var_coeff_g(index) * + SphVariables->cos_mlambda[m] + get_secular_var_coeff_h(index) * SphVariables->sin_mlambda[m]) + * LegendreFunction->dPcup[index]; } } - void WorldMagModel::RotateMagneticVector( WMMtype_CoordSpherical *CoordSpherical, - WMMtype_CoordGeodetic *CoordGeodetic, - WMMtype_MagneticResults *MagneticResultsSph, - WMMtype_MagneticResults *MagneticResultsGeo) - { - /* Rotate the Magnetic Vectors to Geodetic Coordinates - Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov - Equation 16, WMM Technical report + double cos_phi = cos(DEG2RAD(CoordSpherical->phig)); + if (fabs(cos_phi) > 1.0e-10) { + MagneticResults->By = MagneticResults->By / cos_phi; + } else { /* Special calculation for component By at Geographic poles */ + SecVarSummationSpecial(SphVariables, CoordSpherical, MagneticResults); + } +} - INPUT : CoordSpherical : Data structure WMMtype_CoordSpherical with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) +void WorldMagModel::RotateMagneticVector(WMMtype_CoordSpherical *CoordSpherical, + WMMtype_CoordGeodetic *CoordGeodetic, + WMMtype_MagneticResults *MagneticResultsSph, + WMMtype_MagneticResults *MagneticResultsGeo) +{ + /* Rotate the Magnetic Vectors to Geodetic Coordinates + Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov + Equation 16, WMM Technical report - CoordGeodetic : Data structure WMMtype_CoordGeodetic with the following elements - float lambda; (longitude) - float phi; ( geodetic latitude) - float HeightAboveEllipsoid; (height above the ellipsoid (HaE) ) - float HeightAboveGeoid;(height above the Geoid ) + INPUT : CoordSpherical : Data structure WMMtype_CoordSpherical with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) - MagneticResultsSph : Data structure WMMtype_MagneticResults with the following elements - float Bx; North - float By; East - float Bz; Down + CoordGeodetic : Data structure WMMtype_CoordGeodetic with the following elements + float lambda; (longitude) + float phi; ( geodetic latitude) + float HeightAboveEllipsoid; (height above the ellipsoid (HaE) ) + float HeightAboveGeoid;(height above the Geoid ) - OUTPUT: MagneticResultsGeo Pointer to the data structure WMMtype_MagneticResults, with the following elements - float Bx; North - float By; East - float Bz; Down - */ + MagneticResultsSph : Data structure WMMtype_MagneticResults with the following elements + float Bx; North + float By; East + float Bz; Down - /* Difference between the spherical and Geodetic latitudes */ - double Psi = DEG2RAD(CoordSpherical->phig - CoordGeodetic->phi); + OUTPUT: MagneticResultsGeo Pointer to the data structure WMMtype_MagneticResults, with the following elements + float Bx; North + float By; East + float Bz; Down + */ - /* Rotate spherical field components to the Geodeitic system */ - MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sin(Psi) + MagneticResultsSph->Bz * cos(Psi); - MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cos(Psi) - MagneticResultsSph->Bz * sin(Psi); - MagneticResultsGeo->By = MagneticResultsSph->By; + /* Difference between the spherical and Geodetic latitudes */ + double Psi = DEG2RAD(CoordSpherical->phig - CoordGeodetic->phi); + + /* Rotate spherical field components to the Geodeitic system */ + MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sin(Psi) + MagneticResultsSph->Bz * cos(Psi); + MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cos(Psi) - MagneticResultsSph->Bz * sin(Psi); + MagneticResultsGeo->By = MagneticResultsSph->By; +} + +void WorldMagModel::CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements) +{ + /* Calculate all the Geomagnetic elements from X,Y and Z components + INPUT MagneticResultsGeo Pointer to data structure with the following elements + float Bx; ( North ) + float By; ( East ) + float Bz; ( Down ) + OUTPUT GeoMagneticElements Pointer to data structure with the following elements + float Decl; (Angle between the magnetic field vector and true north, positive east) + float Incl; Angle between the magnetic field vector and the horizontal plane, positive down + float F; Magnetic Field Strength + float H; Horizontal Magnetic Field Strength + float X; Northern component of the magnetic field vector + float Y; Eastern component of the magnetic field vector + float Z; Downward component of the magnetic field vector + */ + + GeoMagneticElements->X = MagneticResultsGeo->Bx; + GeoMagneticElements->Y = MagneticResultsGeo->By; + GeoMagneticElements->Z = MagneticResultsGeo->Bz; + + GeoMagneticElements->H = sqrt(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By); + GeoMagneticElements->F = sqrt(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz); + GeoMagneticElements->Decl = RAD2DEG(atan2(GeoMagneticElements->Y, GeoMagneticElements->X)); + GeoMagneticElements->Incl = RAD2DEG(atan2(GeoMagneticElements->Z, GeoMagneticElements->H)); +} + +void WorldMagModel::CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements) +{ + /* This takes the Magnetic Variation in x, y, and z and uses it to calculate the secular variation of each of the Geomagnetic elements. + INPUT MagneticVariation Data structure with the following elements + float Bx; ( North ) + float By; ( East ) + float Bz; ( Down ) + OUTPUT MagneticElements Pointer to the data structure with the following elements updated + float Decldot; Yearly Rate of change in declination + float Incldot; Yearly Rate of change in inclination + float Fdot; Yearly rate of change in Magnetic field strength + float Hdot; Yearly rate of change in horizontal field strength + float Xdot; Yearly rate of change in the northern component + float Ydot; Yearly rate of change in the eastern component + float Zdot; Yearly rate of change in the downward component + float GVdot;Yearly rate of chnage in grid variation + */ + + MagneticElements->Xdot = MagneticVariation->Bx; + MagneticElements->Ydot = MagneticVariation->By; + MagneticElements->Zdot = MagneticVariation->Bz; + MagneticElements->Hdot = (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot) / MagneticElements->H; // See equation 19 in the WMM technical report + MagneticElements->Fdot = + (MagneticElements->X * MagneticElements->Xdot + + MagneticElements->Y * MagneticElements->Ydot + MagneticElements->Z * MagneticElements->Zdot) / MagneticElements->F; + MagneticElements->Decldot = + 180.0 / M_PI * (MagneticElements->X * MagneticElements->Ydot - + MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H); + MagneticElements->Incldot = + 180.0 / M_PI * (MagneticElements->H * MagneticElements->Zdot - + MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F); + MagneticElements->GVdot = MagneticElements->Decldot; +} + +int WorldMagModel::PcupHigh(double *Pcup, double *dPcup, double x, int nMax) +{ + /* This function evaluates all of the Schmidt-semi normalized associated Legendre + functions up to degree nMax. The functions are initially scaled by + 10^280 sin^m in order to minimize the effects of underflow at large m + near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299). + Note that this function performs the same operation as WMM_PcupLow. + However this function also can be used for high degree (large nMax) models. + + Calling Parameters: + INPUT + nMax: Maximum spherical harmonic degree to compute. + x: cos(colatitude) or sin(latitude). + + OUTPUT + Pcup: A vector of all associated Legendgre polynomials evaluated at + x up to nMax. The lenght must by greater or equal to (nMax+1)*(nMax+2)/2. + dPcup: Derivative of Pcup(x) with respect to latitude + Notes: + + Adopted from the FORTRAN code written by Mark Wieczorek September 25, 2005. + + Manoj Nair, Nov, 2009 Manoj.C.Nair@Noaa.Gov + + Change from the previous version + The prevous version computes the derivatives as + dP(n,m)(x)/dx, where x = sin(latitude) (or cos(colatitude) ). + However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude. + Hence the derivatives are multiplied by sin(latitude). + Removed the options for CS phase and normalizations. + + Note: In geomagnetism, the derivatives of ALF are usually found with + respect to the colatitudes. Here the derivatives are found with respect + to the latitude. The difference is a sign reversal for the derivative of + the Associated Legendre Functions. + + The derivates can't be computed for latitude = |90| degrees. + */ + double f1[WMM_NUMPCUP]; + double f2[WMM_NUMPCUP]; + double PreSqr[WMM_NUMPCUP]; + int m; + + if (fabs(x) == 1.0) { + // printf("Error in PcupHigh: derivative cannot be calculated at poles\n"); + return -2; } - void WorldMagModel::CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements) - { - /* Calculate all the Geomagnetic elements from X,Y and Z components - INPUT MagneticResultsGeo Pointer to data structure with the following elements - float Bx; ( North ) - float By; ( East ) - float Bz; ( Down ) - OUTPUT GeoMagneticElements Pointer to data structure with the following elements - float Decl; (Angle between the magnetic field vector and true north, positive east) - float Incl; Angle between the magnetic field vector and the horizontal plane, positive down - float F; Magnetic Field Strength - float H; Horizontal Magnetic Field Strength - float X; Northern component of the magnetic field vector - float Y; Eastern component of the magnetic field vector - float Z; Downward component of the magnetic field vector - */ + double scalef = 1.0e-280; - GeoMagneticElements->X = MagneticResultsGeo->Bx; - GeoMagneticElements->Y = MagneticResultsGeo->By; - GeoMagneticElements->Z = MagneticResultsGeo->Bz; - - GeoMagneticElements->H = sqrt(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By); - GeoMagneticElements->F = sqrt(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz); - GeoMagneticElements->Decl = RAD2DEG(atan2(GeoMagneticElements->Y, GeoMagneticElements->X)); - GeoMagneticElements->Incl = RAD2DEG(atan2(GeoMagneticElements->Z, GeoMagneticElements->H)); + for (int n = 0; n <= 2 * nMax + 1; ++n) { + PreSqr[n] = sqrt((double)(n)); } - void WorldMagModel::CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements) - { - /* This takes the Magnetic Variation in x, y, and z and uses it to calculate the secular variation of each of the Geomagnetic elements. - INPUT MagneticVariation Data structure with the following elements - float Bx; ( North ) - float By; ( East ) - float Bz; ( Down ) - OUTPUT MagneticElements Pointer to the data structure with the following elements updated - float Decldot; Yearly Rate of change in declination - float Incldot; Yearly Rate of change in inclination - float Fdot; Yearly rate of change in Magnetic field strength - float Hdot; Yearly rate of change in horizontal field strength - float Xdot; Yearly rate of change in the northern component - float Ydot; Yearly rate of change in the eastern component - float Zdot; Yearly rate of change in the downward component - float GVdot;Yearly rate of chnage in grid variation - */ + int k = 2; - MagneticElements->Xdot = MagneticVariation->Bx; - MagneticElements->Ydot = MagneticVariation->By; - MagneticElements->Zdot = MagneticVariation->Bz; - MagneticElements->Hdot = (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot) / MagneticElements->H; //See equation 19 in the WMM technical report - MagneticElements->Fdot = - (MagneticElements->X * MagneticElements->Xdot + - MagneticElements->Y * MagneticElements->Ydot + MagneticElements->Z * MagneticElements->Zdot) / MagneticElements->F; - MagneticElements->Decldot = - 180.0 / M_PI * (MagneticElements->X * MagneticElements->Ydot - - MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H); - MagneticElements->Incldot = - 180.0 / M_PI * (MagneticElements->H * MagneticElements->Zdot - - MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F); - MagneticElements->GVdot = MagneticElements->Decldot; - } - - int WorldMagModel::PcupHigh(double *Pcup, double *dPcup, double x, int nMax) - { - /* This function evaluates all of the Schmidt-semi normalized associated Legendre - functions up to degree nMax. The functions are initially scaled by - 10^280 sin^m in order to minimize the effects of underflow at large m - near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299). - Note that this function performs the same operation as WMM_PcupLow. - However this function also can be used for high degree (large nMax) models. - - Calling Parameters: - INPUT - nMax: Maximum spherical harmonic degree to compute. - x: cos(colatitude) or sin(latitude). - - OUTPUT - Pcup: A vector of all associated Legendgre polynomials evaluated at - x up to nMax. The lenght must by greater or equal to (nMax+1)*(nMax+2)/2. - dPcup: Derivative of Pcup(x) with respect to latitude - Notes: - - Adopted from the FORTRAN code written by Mark Wieczorek September 25, 2005. - - Manoj Nair, Nov, 2009 Manoj.C.Nair@Noaa.Gov - - Change from the previous version - The prevous version computes the derivatives as - dP(n,m)(x)/dx, where x = sin(latitude) (or cos(colatitude) ). - However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude. - Hence the derivatives are multiplied by sin(latitude). - Removed the options for CS phase and normalizations. - - Note: In geomagnetism, the derivatives of ALF are usually found with - respect to the colatitudes. Here the derivatives are found with respect - to the latitude. The difference is a sign reversal for the derivative of - the Associated Legendre Functions. - - The derivates can't be computed for latitude = |90| degrees. - */ - double f1[WMM_NUMPCUP]; - double f2[WMM_NUMPCUP]; - double PreSqr[WMM_NUMPCUP]; - int m; - - if (fabs(x) == 1.0) - { - // printf("Error in PcupHigh: derivative cannot be calculated at poles\n"); - return -2; + for (int n = 2; n <= nMax; n++) { + k = k + 1; + f1[k] = (double)(2 * n - 1) / n; + f2[k] = (double)(n - 1) / n; + for (int m = 1; m <= n - 2; m++) { + k = k + 1; + f1[k] = (double)(2 * n - 1) / PreSqr[n + m] / PreSqr[n - m]; + f2[k] = PreSqr[n - m - 1] * PreSqr[n + m - 1] / PreSqr[n + m] / PreSqr[n - m]; } + k = k + 2; + } - double scalef = 1.0e-280; + /*z = sin (geocentric latitude) */ + double z = sqrt((1.0 - x) * (1.0 + x)); + double pm2 = 1.0; + Pcup[0] = 1.0; + dPcup[0] = 0.0; + if (nMax == 0) { + return -3; + } + double pm1 = x; + Pcup[1] = pm1; + dPcup[1] = z; + k = 1; - for (int n = 0; n <= 2 * nMax + 1; ++n) - PreSqr[n] = sqrt((double)(n)); + for (int n = 2; n <= nMax; n++) { + k = k + n; + double plm = f1[k] * x * pm1 - f2[k] * pm2; + Pcup[k] = plm; + dPcup[k] = (double)(n) * (pm1 - x * plm) / z; + pm2 = pm1; + pm1 = plm; + } - int k = 2; + double pmm = PreSqr[2] * scalef; + double rescalem = 1.0 / scalef; + int kstart = 0; - for (int n = 2; n <= nMax; n++) - { - k = k + 1; - f1[k] = (double)(2 * n - 1) / n; - f2[k] = (double)(n - 1) / n; - for (int m = 1; m <= n - 2; m++) - { - k = k + 1; - f1[k] = (double)(2 * n - 1) / PreSqr[n + m] / PreSqr[n - m]; - f2[k] = PreSqr[n - m - 1] * PreSqr[n + m - 1] / PreSqr[n + m] / PreSqr[n - m]; - } - k = k + 2; - } + for (m = 1; m <= nMax - 1; ++m) { + rescalem = rescalem * z; - /*z = sin (geocentric latitude) */ - double z = sqrt((1.0 - x) * (1.0 + x)); - double pm2 = 1.0; - Pcup[0] = 1.0; - dPcup[0] = 0.0; - if (nMax == 0) - return -3; - double pm1 = x; - Pcup[1] = pm1; - dPcup[1] = z; - k = 1; - - for (int n = 2; n <= nMax; n++) - { + /* Calculate Pcup(m,m) */ + kstart = kstart + m + 1; + pmm = pmm * PreSqr[2 * m + 1] / PreSqr[2 * m]; + Pcup[kstart] = pmm * rescalem / PreSqr[2 * m + 1]; + dPcup[kstart] = -((double)(m) * x * Pcup[kstart] / z); + pm2 = pmm / PreSqr[2 * m + 1]; + /* Calculate Pcup(m+1,m) */ + k = kstart + m + 1; + pm1 = x * PreSqr[2 * m + 1] * pm2; + Pcup[k] = pm1 * rescalem; + dPcup[k] = ((pm2 * rescalem) * PreSqr[2 * m + 1] - x * (double)(m + 1) * Pcup[k]) / z; + /* Calculate Pcup(n,m) */ + for (int n = m + 2; n <= nMax; ++n) { k = k + n; - double plm = f1[k] * x * pm1 - f2[k] * pm2; - Pcup[k] = plm; - dPcup[k] = (double)(n) * (pm1 - x * plm) / z; + double plm = x * f1[k] * pm1 - f2[k] * pm2; + Pcup[k] = plm * rescalem; + dPcup[k] = (PreSqr[n + m] * PreSqr[n - m] * (pm1 * rescalem) - (double)(n) * x * Pcup[k]) / z; pm2 = pm1; pm1 = plm; } - - double pmm = PreSqr[2] * scalef; - double rescalem = 1.0 / scalef; - int kstart = 0; - - for (m = 1; m <= nMax - 1; ++m) - { - rescalem = rescalem * z; - - /* Calculate Pcup(m,m) */ - kstart = kstart + m + 1; - pmm = pmm * PreSqr[2 * m + 1] / PreSqr[2 * m]; - Pcup[kstart] = pmm * rescalem / PreSqr[2 * m + 1]; - dPcup[kstart] = -((double)(m) * x * Pcup[kstart] / z); - pm2 = pmm / PreSqr[2 * m + 1]; - /* Calculate Pcup(m+1,m) */ - k = kstart + m + 1; - pm1 = x * PreSqr[2 * m + 1] * pm2; - Pcup[k] = pm1 * rescalem; - dPcup[k] = ((pm2 * rescalem) * PreSqr[2 * m + 1] - x * (double)(m + 1) * Pcup[k]) / z; - /* Calculate Pcup(n,m) */ - for (int n = m + 2; n <= nMax; ++n) - { - k = k + n; - double plm = x * f1[k] * pm1 - f2[k] * pm2; - Pcup[k] = plm * rescalem; - dPcup[k] = (PreSqr[n + m] * PreSqr[n - m] * (pm1 * rescalem) - (double)(n) * x * Pcup[k]) / z; - pm2 = pm1; - pm1 = plm; - } - } - - /* Calculate Pcup(nMax,nMax) */ - rescalem = rescalem * z; - kstart = kstart + m + 1; - pmm = pmm / PreSqr[2 * nMax]; - Pcup[kstart] = pmm * rescalem; - dPcup[kstart] = -(double)(nMax) * x * Pcup[kstart] / z; - - // ********* - - return 0; // OK } - void WorldMagModel::PcupLow(double *Pcup, double *dPcup, double x, int nMax) - { - /* This function evaluates all of the Schmidt-semi normalized associated Legendre functions up to degree nMax. + /* Calculate Pcup(nMax,nMax) */ + rescalem = rescalem * z; + kstart = kstart + m + 1; + pmm = pmm / PreSqr[2 * nMax]; + Pcup[kstart] = pmm * rescalem; + dPcup[kstart] = -(double)(nMax) * x * Pcup[kstart] / z; - Calling Parameters: - INPUT - nMax: Maximum spherical harmonic degree to compute. - x: cos(colatitude) or sin(latitude). + // ********* - OUTPUT - Pcup: A vector of all associated Legendgre polynomials evaluated at - x up to nMax. - dPcup: Derivative of Pcup(x) with respect to latitude + return 0; // OK +} - Notes: Overflow may occur if nMax > 20 , especially for high-latitudes. - Use WMM_PcupHigh for large nMax. +void WorldMagModel::PcupLow(double *Pcup, double *dPcup, double x, int nMax) +{ + /* This function evaluates all of the Schmidt-semi normalized associated Legendre functions up to degree nMax. - Writted by Manoj Nair, June, 2009 . Manoj.C.Nair@Noaa.Gov. + Calling Parameters: + INPUT + nMax: Maximum spherical harmonic degree to compute. + x: cos(colatitude) or sin(latitude). - Note: In geomagnetism, the derivatives of ALF are usually found with - respect to the colatitudes. Here the derivatives are found with respect - to the latitude. The difference is a sign reversal for the derivative of - the Associated Legendre Functions. - */ + OUTPUT + Pcup: A vector of all associated Legendgre polynomials evaluated at + x up to nMax. + dPcup: Derivative of Pcup(x) with respect to latitude - double schmidtQuasiNorm[WMM_NUMPCUP]; + Notes: Overflow may occur if nMax > 20 , especially for high-latitudes. + Use WMM_PcupHigh for large nMax. - Pcup[0] = 1.0; - dPcup[0] = 0.0; + Writted by Manoj Nair, June, 2009 . Manoj.C.Nair@Noaa.Gov. - /*sin (geocentric latitude) - sin_phi */ - double z = sqrt((1.0 - x) * (1.0 + x)); + Note: In geomagnetism, the derivatives of ALF are usually found with + respect to the colatitudes. Here the derivatives are found with respect + to the latitude. The difference is a sign reversal for the derivative of + the Associated Legendre Functions. + */ - /* First, Compute the Gauss-normalized associated Legendre functions */ - for (int n = 1; n <= nMax; n++) - { - for (int m = 0; m <= n; m++) - { - int index = (n * (n + 1) / 2 + m); - if (n == m) - { - int index1 = (n - 1) * n / 2 + m - 1; - Pcup[index] = z * Pcup[index1]; - dPcup[index] = z * dPcup[index1] + x * Pcup[index1]; - } - else - if (n == 1 && m == 0) - { - int index1 = (n - 1) * n / 2 + m; - Pcup[index] = x * Pcup[index1]; - dPcup[index] = x * dPcup[index1] - z * Pcup[index1]; - } - else - if (n > 1 && n != m) - { - int index1 = (n - 2) * (n - 1) / 2 + m; - int index2 = (n - 1) * n / 2 + m; - if (m > n - 2) - { - Pcup[index] = x * Pcup[index2]; - dPcup[index] = x * dPcup[index2] - z * Pcup[index2]; - } - else - { - double k = (double)(((n - 1) * (n - 1)) - (m * m)) / (double)((2 * n - 1) * (2 * n - 3)); - Pcup[index] = x * Pcup[index2] - k * Pcup[index1]; - dPcup[index] = x * dPcup[index2] - z * Pcup[index2] - k * dPcup[index1]; - } + double schmidtQuasiNorm[WMM_NUMPCUP]; + + Pcup[0] = 1.0; + dPcup[0] = 0.0; + + /*sin (geocentric latitude) - sin_phi */ + double z = sqrt((1.0 - x) * (1.0 + x)); + + /* First, Compute the Gauss-normalized associated Legendre functions */ + for (int n = 1; n <= nMax; n++) { + for (int m = 0; m <= n; m++) { + int index = (n * (n + 1) / 2 + m); + if (n == m) { + int index1 = (n - 1) * n / 2 + m - 1; + Pcup[index] = z * Pcup[index1]; + dPcup[index] = z * dPcup[index1] + x * Pcup[index1]; + } else if (n == 1 && m == 0) { + int index1 = (n - 1) * n / 2 + m; + Pcup[index] = x * Pcup[index1]; + dPcup[index] = x * dPcup[index1] - z * Pcup[index1]; + } else if (n > 1 && n != m) { + int index1 = (n - 2) * (n - 1) / 2 + m; + int index2 = (n - 1) * n / 2 + m; + if (m > n - 2) { + Pcup[index] = x * Pcup[index2]; + dPcup[index] = x * dPcup[index2] - z * Pcup[index2]; + } else { + double k = (double)(((n - 1) * (n - 1)) - (m * m)) / (double)((2 * n - 1) * (2 * n - 3)); + Pcup[index] = x * Pcup[index2] - k * Pcup[index1]; + dPcup[index] = x * dPcup[index2] - z * Pcup[index2] - k * dPcup[index1]; } } } + } + /*Compute the ration between the Gauss-normalized associated Legendre + functions and the Schmidt quasi-normalized version. This is equivalent to + sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ + + schmidtQuasiNorm[0] = 1.0; + for (int n = 1; n <= nMax; n++) { + int index = (n * (n + 1) / 2); + int index1 = (n - 1) * n / 2; + /* for m = 0 */ + schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (double)(2 * n - 1) / (double)n; + + for (int m = 1; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + index1 = (n * (n + 1) / 2 + m - 1); + schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * sqrt((double)((n - m + 1) * (m == 1 ? 2 : 1)) / (double)(n + m)); + } + } + + /* Converts the Gauss-normalized associated Legendre + functions to the Schmidt quasi-normalized version using pre-computed + relation stored in the variable schmidtQuasiNorm */ + + for (int n = 1; n <= nMax; n++) { + for (int m = 0; m <= n; m++) { + int index = (n * (n + 1) / 2 + m); + Pcup[index] = Pcup[index] * schmidtQuasiNorm[index]; + dPcup[index] = -dPcup[index] * schmidtQuasiNorm[index]; + /* The sign is changed since the new WMM routines use derivative with respect to latitude insted of co-latitude */ + } + } +} + +void WorldMagModel::SummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) +{ + /* Special calculation for the component By at Geographic poles. + Manoj Nair, June, 2009 manoj.c.nair@noaa.gov + INPUT: MagneticModel + SphVariables + CoordSpherical + OUTPUT: MagneticResults + CALLS : none + See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report + */ + + double PcupS[WMM_NUMPCUPS]; + + PcupS[0] = 1; + double schmidtQuasiNorm1 = 1.0; + + MagneticResults->By = 0.0; + double sin_phi = sin(DEG2RAD(CoordSpherical->phig)); + + for (int n = 1; n <= MagneticModel.nMax; n++) { /*Compute the ration between the Gauss-normalized associated Legendre - functions and the Schmidt quasi-normalized version. This is equivalent to - sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ + functions and the Schmidt quasi-normalized version. This is equivalent to + sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ - schmidtQuasiNorm[0] = 1.0; - for (int n = 1; n <= nMax; n++) - { - int index = (n * (n + 1) / 2); - int index1 = (n - 1) * n / 2; - /* for m = 0 */ - schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (double)(2 * n - 1) / (double)n; - - for (int m = 1; m <= n; m++) - { - index = (n * (n + 1) / 2 + m); - index1 = (n * (n + 1) / 2 + m - 1); - schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * sqrt((double)((n - m + 1) * (m == 1 ? 2 : 1)) / (double)(n + m)); - } + int index = (n * (n + 1) / 2 + 1); + double schmidtQuasiNorm2 = schmidtQuasiNorm1 * (double)(2 * n - 1) / (double)n; + double schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((double)(n * 2) / (double)(n + 1)); + schmidtQuasiNorm1 = schmidtQuasiNorm2; + if (n == 1) { + PcupS[n] = PcupS[n - 1]; + } else { + double k = (double)(((n - 1) * (n - 1)) - 1) / (double)((2 * n - 1) * (2 * n - 3)); + PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; } - /* Converts the Gauss-normalized associated Legendre - functions to the Schmidt quasi-normalized version using pre-computed - relation stored in the variable schmidtQuasiNorm */ - - for (int n = 1; n <= nMax; n++) - { - for (int m = 0; m <= n; m++) - { - int index = (n * (n + 1) / 2 + m); - Pcup[index] = Pcup[index] * schmidtQuasiNorm[index]; - dPcup[index] = -dPcup[index] * schmidtQuasiNorm[index]; - /* The sign is changed since the new WMM routines use derivative with respect to latitude insted of co-latitude */ - } - } - } - - void WorldMagModel::SummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) - { - /* Special calculation for the component By at Geographic poles. - Manoj Nair, June, 2009 manoj.c.nair@noaa.gov - INPUT: MagneticModel - SphVariables - CoordSpherical - OUTPUT: MagneticResults - CALLS : none - See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report - */ - - double PcupS[WMM_NUMPCUPS]; - - PcupS[0] = 1; - double schmidtQuasiNorm1 = 1.0; - - MagneticResults->By = 0.0; - double sin_phi = sin(DEG2RAD(CoordSpherical->phig)); - - for (int n = 1; n <= MagneticModel.nMax; n++) - { - /*Compute the ration between the Gauss-normalized associated Legendre - functions and the Schmidt quasi-normalized version. This is equivalent to - sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ - - int index = (n * (n + 1) / 2 + 1); - double schmidtQuasiNorm2 = schmidtQuasiNorm1 * (double)(2 * n - 1) / (double)n; - double schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((double)(n * 2) / (double)(n + 1)); - schmidtQuasiNorm1 = schmidtQuasiNorm2; - if (n == 1) - { - PcupS[n] = PcupS[n - 1]; - } - else - { - double k = (double)(((n - 1) * (n - 1)) - 1) / (double)((2 * n - 1) * (2 * n - 3)); - PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; - } - /* 1 nMax (n+2) n m m m By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (get_main_field_coeff_g(index) * - SphVariables->sin_mlambda[1] - get_main_field_coeff_h(index) * SphVariables->cos_mlambda[1]) - * PcupS[n] * schmidtQuasiNorm3; - } + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (get_main_field_coeff_g(index) * + SphVariables->sin_mlambda[1] - get_main_field_coeff_h(index) * SphVariables->cos_mlambda[1]) + * PcupS[n] * schmidtQuasiNorm3; } +} - void WorldMagModel::SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) - { - /*Special calculation for the secular variation summation at the poles. +void WorldMagModel::SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults) +{ + /*Special calculation for the secular variation summation at the poles. - INPUT: MagneticModel - SphVariables - CoordSpherical - OUTPUT: MagneticResults - */ + INPUT: MagneticModel + SphVariables + CoordSpherical + OUTPUT: MagneticResults + */ - double PcupS[WMM_NUMPCUPS]; + double PcupS[WMM_NUMPCUPS]; - PcupS[0] = 1; - double schmidtQuasiNorm1 = 1.0; + PcupS[0] = 1; + double schmidtQuasiNorm1 = 1.0; - MagneticResults->By = 0.0; - double sin_phi = sin(DEG2RAD(CoordSpherical->phig)); + MagneticResults->By = 0.0; + double sin_phi = sin(DEG2RAD(CoordSpherical->phig)); - for (int n = 1; n <= MagneticModel.nMaxSecVar; n++) - { - int index = (n * (n + 1) / 2 + 1); - double schmidtQuasiNorm2 = schmidtQuasiNorm1 * (double)(2 * n - 1) / (double)n; - double schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((double)(n * 2) / (double)(n + 1)); - schmidtQuasiNorm1 = schmidtQuasiNorm2; - if (n == 1) - { - PcupS[n] = PcupS[n - 1]; - } - else - { - double k = (double)(((n - 1) * (n - 1)) - 1) / (double)((2 * n - 1) * (2 * n - 3)); - PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; - } + for (int n = 1; n <= MagneticModel.nMaxSecVar; n++) { + int index = (n * (n + 1) / 2 + 1); + double schmidtQuasiNorm2 = schmidtQuasiNorm1 * (double)(2 * n - 1) / (double)n; + double schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((double)(n * 2) / (double)(n + 1)); + schmidtQuasiNorm1 = schmidtQuasiNorm2; + if (n == 1) { + PcupS[n] = PcupS[n - 1]; + } else { + double k = (double)(((n - 1) * (n - 1)) - 1) / (double)((2 * n - 1) * (2 * n - 3)); + PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; + } /* 1 nMax (n+2) n m m m By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += - SphVariables->RelativeRadiusPower[n] * - (get_secular_var_coeff_g(index) * - SphVariables->sin_mlambda[1] - get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[1]) - * PcupS[n] * schmidtQuasiNorm3; - } + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (get_secular_var_coeff_g(index) * + SphVariables->sin_mlambda[1] - get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[1]) + * PcupS[n] * schmidtQuasiNorm3; } - - // brief Comput the MainFieldCoeffH accounting for the date - double WorldMagModel::get_main_field_coeff_g(int index) - { - if (index >= WMM_NUMTERMS) - return 0; - - double coeff = CoeffFile[index][2]; - - int a = MagneticModel.nMaxSecVar; - int b = (a * (a + 1) / 2 + a); - for (int n = 1; n <= MagneticModel.nMax; n++) - { - for (int m = 0; m <= n; m++) - { - int sum_index = (n * (n + 1) / 2 + m); - - /* Hacky for now, will solve for which conditions need summing analytically */ - if (sum_index != index) - continue; - - if (index <= b) - coeff += (decimal_date - MagneticModel.epoch) * get_secular_var_coeff_g(sum_index); - } - } - - return coeff; - } - - double WorldMagModel::get_main_field_coeff_h(int index) - { - if (index >= WMM_NUMTERMS) - return 0; - - double coeff = CoeffFile[index][3]; - - int a = MagneticModel.nMaxSecVar; - int b = (a * (a + 1) / 2 + a); - for (int n = 1; n <= MagneticModel.nMax; n++) - { - for (int m = 0; m <= n; m++) - { - int sum_index = (n * (n + 1) / 2 + m); - - /* Hacky for now, will solve for which conditions need summing analytically */ - if (sum_index != index) - continue; - - if (index <= b) - coeff += (decimal_date - MagneticModel.epoch) * get_secular_var_coeff_h(sum_index); - } - } - - return coeff; - } - - double WorldMagModel::get_secular_var_coeff_g(int index) - { - if (index >= WMM_NUMTERMS) - return 0; - - return CoeffFile[index][4]; - } - - double WorldMagModel::get_secular_var_coeff_h(int index) - { - if (index >= WMM_NUMTERMS) - return 0; - - return CoeffFile[index][5]; - } - - int WorldMagModel::DateToYear(int month, int day, int year) - { - // Converts a given calendar date into a decimal year - - int temp = 0; // Total number of days - int MonthDays[13] = { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; - int ExtraDay = 0; - - if ((year % 4 == 0 && year % 100 != 0) || (year % 400 == 0)) - ExtraDay = 1; - MonthDays[2] += ExtraDay; - - /******************Validation********************************/ - - if (month <= 0 || month > 12) - return -1; // error - - if (day <= 0 || day > MonthDays[month]) - return -2; // error - - /****************Calculation of t***************************/ - for (int i = 1; i <= month; i++) - temp += MonthDays[i - 1]; - temp += day; - - decimal_date = year + (temp - 1) / (365.0 + ExtraDay); - - return 0; // OK - } - - void WorldMagModel::GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical) - { - // Converts Geodetic coordinates to Spherical coordinates - // Convert geodetic coordinates, (defined by the WGS-84 - // reference ellipsoid), to Earth Centered Earth Fixed Cartesian - // coordinates, and then to spherical coordinates. - - double CosLat = cos(DEG2RAD(CoordGeodetic->phi)); - double SinLat = sin(DEG2RAD(CoordGeodetic->phi)); - - // compute the local radius of curvature on the WGS-84 reference ellipsoid - double rc = Ellip.a / sqrt(1.0 - Ellip.epssq * SinLat * SinLat); - - // compute ECEF Cartesian coordinates of specified point (for longitude=0) - double xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat; - double zp = (rc * (1.0 - Ellip.epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat; - - // compute spherical radius and angle lambda and phi of specified point - CoordSpherical->r = sqrt(xp * xp + zp * zp); - CoordSpherical->phig = RAD2DEG(asin(zp / CoordSpherical->r)); // geocentric latitude - CoordSpherical->lambda = CoordGeodetic->lambda; // longitude - } - +} + +// brief Comput the MainFieldCoeffH accounting for the date +double WorldMagModel::get_main_field_coeff_g(int index) +{ + if (index >= WMM_NUMTERMS) { + return 0; + } + + double coeff = CoeffFile[index][2]; + + int a = MagneticModel.nMaxSecVar; + int b = (a * (a + 1) / 2 + a); + for (int n = 1; n <= MagneticModel.nMax; n++) { + for (int m = 0; m <= n; m++) { + int sum_index = (n * (n + 1) / 2 + m); + + /* Hacky for now, will solve for which conditions need summing analytically */ + if (sum_index != index) { + continue; + } + + if (index <= b) { + coeff += (decimal_date - MagneticModel.epoch) * get_secular_var_coeff_g(sum_index); + } + } + } + + return coeff; +} + +double WorldMagModel::get_main_field_coeff_h(int index) +{ + if (index >= WMM_NUMTERMS) { + return 0; + } + + double coeff = CoeffFile[index][3]; + + int a = MagneticModel.nMaxSecVar; + int b = (a * (a + 1) / 2 + a); + for (int n = 1; n <= MagneticModel.nMax; n++) { + for (int m = 0; m <= n; m++) { + int sum_index = (n * (n + 1) / 2 + m); + + /* Hacky for now, will solve for which conditions need summing analytically */ + if (sum_index != index) { + continue; + } + + if (index <= b) { + coeff += (decimal_date - MagneticModel.epoch) * get_secular_var_coeff_h(sum_index); + } + } + } + + return coeff; +} + +double WorldMagModel::get_secular_var_coeff_g(int index) +{ + if (index >= WMM_NUMTERMS) { + return 0; + } + + return CoeffFile[index][4]; +} + +double WorldMagModel::get_secular_var_coeff_h(int index) +{ + if (index >= WMM_NUMTERMS) { + return 0; + } + + return CoeffFile[index][5]; +} + +int WorldMagModel::DateToYear(int month, int day, int year) +{ + // Converts a given calendar date into a decimal year + + int temp = 0; // Total number of days + int MonthDays[13] = { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; + int ExtraDay = 0; + + if ((year % 4 == 0 && year % 100 != 0) || (year % 400 == 0)) { + ExtraDay = 1; + } + MonthDays[2] += ExtraDay; + + /******************Validation********************************/ + + if (month <= 0 || month > 12) { + return -1; // error + } + if (day <= 0 || day > MonthDays[month]) { + return -2; // error + } + /****************Calculation of t***************************/ + for (int i = 1; i <= month; i++) { + temp += MonthDays[i - 1]; + } + temp += day; + + decimal_date = year + (temp - 1) / (365.0 + ExtraDay); + + return 0; // OK +} + +void WorldMagModel::GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical) +{ + // Converts Geodetic coordinates to Spherical coordinates + // Convert geodetic coordinates, (defined by the WGS-84 + // reference ellipsoid), to Earth Centered Earth Fixed Cartesian + // coordinates, and then to spherical coordinates. + + double CosLat = cos(DEG2RAD(CoordGeodetic->phi)); + double SinLat = sin(DEG2RAD(CoordGeodetic->phi)); + + // compute the local radius of curvature on the WGS-84 reference ellipsoid + double rc = Ellip.a / sqrt(1.0 - Ellip.epssq * SinLat * SinLat); + + // compute ECEF Cartesian coordinates of specified point (for longitude=0) + double xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat; + double zp = (rc * (1.0 - Ellip.epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat; + + // compute spherical radius and angle lambda and phi of specified point + CoordSpherical->r = sqrt(xp * xp + zp * zp); + CoordSpherical->phig = RAD2DEG(asin(zp / CoordSpherical->r)); // geocentric latitude + CoordSpherical->lambda = CoordGeodetic->lambda; // longitude +} } diff --git a/ground/openpilotgcs/src/libs/utils/worldmagmodel.h b/ground/openpilotgcs/src/libs/utils/worldmagmodel.h index 2d9ca2840..447dc001d 100644 --- a/ground/openpilotgcs/src/libs/utils/worldmagmodel.h +++ b/ground/openpilotgcs/src/libs/utils/worldmagmodel.h @@ -34,147 +34,135 @@ // ****************************** // internal structure definitions -#define WMM_MAX_MODEL_DEGREES 12 -#define WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES 12 -#define WMM_NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES + 1) * (WMM_MAX_MODEL_DEGREES + 2) / 2); -#define WMM_NUMPCUP 92 // NUMTERMS + 1 -#define WMM_NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES + 1 +#define WMM_MAX_MODEL_DEGREES 12 +#define WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES 12 +#define WMM_NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES + 1) * (WMM_MAX_MODEL_DEGREES + 2) / 2); +#define WMM_NUMPCUP 92 // NUMTERMS + 1 +#define WMM_NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES + 1 -typedef struct -{ +typedef struct { double EditionDate; - double epoch; //Base time of Geomagnetic model epoch (yrs) - char ModelName[20]; -// double Main_Field_Coeff_G[WMM_NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) -// double Main_Field_Coeff_H[WMM_NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) -// double Secular_Var_Coeff_G[WMM_NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) -// double Secular_Var_Coeff_H[WMM_NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) - int nMax; // Maximum degree of spherical harmonic model - int nMaxSecVar; // Maxumum degree of spherical harmonic secular model - int SecularVariationUsed; // Whether or not the magnetic secular variation vector will be needed by program + double epoch; // Base time of Geomagnetic model epoch (yrs) + char ModelName[20]; +// double Main_Field_Coeff_G[WMM_NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) +// double Main_Field_Coeff_H[WMM_NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) +// double Secular_Var_Coeff_G[WMM_NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) +// double Secular_Var_Coeff_H[WMM_NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) + int nMax; // Maximum degree of spherical harmonic model + int nMaxSecVar; // Maxumum degree of spherical harmonic secular model + int SecularVariationUsed; // Whether or not the magnetic secular variation vector will be needed by program } WMMtype_MagneticModel; -typedef struct -{ - double a; // semi-major axis of the ellipsoid - double b; // semi-minor axis of the ellipsoid - double fla; // flattening - double epssq; // first eccentricity squared - double eps; // first eccentricity - double re; // mean radius of ellipsoid +typedef struct { + double a; // semi-major axis of the ellipsoid + double b; // semi-minor axis of the ellipsoid + double fla; // flattening + double epssq; // first eccentricity squared + double eps; // first eccentricity + double re; // mean radius of ellipsoid } WMMtype_Ellipsoid; -typedef struct -{ - double lambda; // longitude - double phi; // geodetic latitude - double HeightAboveEllipsoid; // height above the ellipsoid (HaE) +typedef struct { + double lambda; // longitude + double phi; // geodetic latitude + double HeightAboveEllipsoid; // height above the ellipsoid (HaE) } WMMtype_CoordGeodetic; -typedef struct -{ - double lambda; // longitude - double phig; // geocentric latitude - double r; // distance from the center of the ellipsoid +typedef struct { + double lambda; // longitude + double phig; // geocentric latitude + double r; // distance from the center of the ellipsoid } WMMtype_CoordSpherical; -typedef struct -{ - int Year; - int Month; - int Day; +typedef struct { + int Year; + int Month; + int Day; double DecimalYear; } WMMtype_Date; -typedef struct -{ - double Pcup[WMM_NUMPCUP]; // Legendre Function - double dPcup[WMM_NUMPCUP]; // Derivative of Lagendre fn +typedef struct { + double Pcup[WMM_NUMPCUP]; // Legendre Function + double dPcup[WMM_NUMPCUP]; // Derivative of Lagendre fn } WMMtype_LegendreFunction; -typedef struct -{ - double Bx; // North - double By; // East - double Bz; // Down +typedef struct { + double Bx; // North + double By; // East + double Bz; // Down } WMMtype_MagneticResults; -typedef struct -{ - double RelativeRadiusPower[WMM_MAX_MODEL_DEGREES + 1]; // [earth_reference_radius_km / sph. radius ]^n - double cos_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // cp(m) - cosine of (m*spherical coord. longitude - double sin_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // sp(m) - sine of (m*spherical coord. longitude) +typedef struct { + double RelativeRadiusPower[WMM_MAX_MODEL_DEGREES + 1]; // [earth_reference_radius_km / sph. radius ]^n + double cos_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // cp(m) - cosine of (m*spherical coord. longitude + double sin_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // sp(m) - sine of (m*spherical coord. longitude) } WMMtype_SphericalHarmonicVariables; -typedef struct -{ - double Decl; /*1. Angle between the magnetic field vector and true north, positive east */ - double Incl; /*2. Angle between the magnetic field vector and the horizontal plane, positive down */ - double F; /*3. Magnetic Field Strength */ - double H; /*4. Horizontal Magnetic Field Strength */ - double X; /*5. Northern component of the magnetic field vector */ - double Y; /*6. Eastern component of the magnetic field vector */ - double Z; /*7. Downward component of the magnetic field vector */ - double GV; /*8. The Grid Variation */ - double Decldot; /*9. Yearly Rate of change in declination */ - double Incldot; /*10. Yearly Rate of change in inclination */ - double Fdot; /*11. Yearly rate of change in Magnetic field strength */ - double Hdot; /*12. Yearly rate of change in horizontal field strength */ - double Xdot; /*13. Yearly rate of change in the northern component */ - double Ydot; /*14. Yearly rate of change in the eastern component */ - double Zdot; /*15. Yearly rate of change in the downward component */ - double GVdot; /*16. Yearly rate of chnage in grid variation */ +typedef struct { + double Decl; /*1. Angle between the magnetic field vector and true north, positive east */ + double Incl; /*2. Angle between the magnetic field vector and the horizontal plane, positive down */ + double F; /*3. Magnetic Field Strength */ + double H; /*4. Horizontal Magnetic Field Strength */ + double X; /*5. Northern component of the magnetic field vector */ + double Y; /*6. Eastern component of the magnetic field vector */ + double Z; /*7. Downward component of the magnetic field vector */ + double GV; /*8. The Grid Variation */ + double Decldot; /*9. Yearly Rate of change in declination */ + double Incldot; /*10. Yearly Rate of change in inclination */ + double Fdot; /*11. Yearly rate of change in Magnetic field strength */ + double Hdot; /*12. Yearly rate of change in horizontal field strength */ + double Xdot; /*13. Yearly rate of change in the northern component */ + double Ydot; /*14. Yearly rate of change in the eastern component */ + double Zdot; /*15. Yearly rate of change in the downward component */ + double GVdot; /*16. Yearly rate of chnage in grid variation */ } WMMtype_GeoMagneticElements; // ****************************** namespace Utils { +class QTCREATOR_UTILS_EXPORT WorldMagModel { +public: + WorldMagModel(); - class QTCREATOR_UTILS_EXPORT WorldMagModel - { - public: - WorldMagModel(); + int GetMagVector(double LLA[3], int Month, int Day, int Year, double Be[3]); - int GetMagVector(double LLA[3], int Month, int Day, int Year, double Be[3]); +private: + WMMtype_Ellipsoid Ellip; + WMMtype_MagneticModel MagneticModel; - private: - WMMtype_Ellipsoid Ellip; - WMMtype_MagneticModel MagneticModel; - - double decimal_date; - - void Initialize(); - int Geomag(WMMtype_CoordSpherical *CoordSpherical, WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_GeoMagneticElements *GeoMagneticElements); - void ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_SphericalHarmonicVariables *SphVariables); - int AssociatedLegendreFunction(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_LegendreFunction *LegendreFunction); - void Summation( WMMtype_LegendreFunction *LegendreFunction, - WMMtype_SphericalHarmonicVariables *SphVariables, - WMMtype_CoordSpherical *CoordSpherical, - WMMtype_MagneticResults *MagneticResults); - void SecVarSummation( WMMtype_LegendreFunction *LegendreFunction, - WMMtype_SphericalHarmonicVariables *SphVariables, - WMMtype_CoordSpherical *CoordSpherical, - WMMtype_MagneticResults *MagneticResults); - void RotateMagneticVector( WMMtype_CoordSpherical *CoordSpherical, - WMMtype_CoordGeodetic *CoordGeodetic, - WMMtype_MagneticResults *MagneticResultsSph, - WMMtype_MagneticResults *MagneticResultsGeo); - void CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements); - void CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements); - int PcupHigh(double *Pcup, double *dPcup, double x, int nMax); - void PcupLow(double *Pcup, double *dPcup, double x, int nMax); - void SummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); - void SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); - double get_main_field_coeff_g(int index); - double get_main_field_coeff_h(int index); - double get_secular_var_coeff_g(int index); - double get_secular_var_coeff_h(int index); - int DateToYear(int month, int day, int year); - void GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical); - }; + double decimal_date; + void Initialize(); + int Geomag(WMMtype_CoordSpherical *CoordSpherical, WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_GeoMagneticElements *GeoMagneticElements); + void ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_SphericalHarmonicVariables *SphVariables); + int AssociatedLegendreFunction(WMMtype_CoordSpherical *CoordSpherical, int nMax, WMMtype_LegendreFunction *LegendreFunction); + void Summation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables *SphVariables, + WMMtype_CoordSpherical *CoordSpherical, + WMMtype_MagneticResults *MagneticResults); + void SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, + WMMtype_SphericalHarmonicVariables *SphVariables, + WMMtype_CoordSpherical *CoordSpherical, + WMMtype_MagneticResults *MagneticResults); + void RotateMagneticVector(WMMtype_CoordSpherical *CoordSpherical, + WMMtype_CoordGeodetic *CoordGeodetic, + WMMtype_MagneticResults *MagneticResultsSph, + WMMtype_MagneticResults *MagneticResultsGeo); + void CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements); + void CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements); + int PcupHigh(double *Pcup, double *dPcup, double x, int nMax); + void PcupLow(double *Pcup, double *dPcup, double x, int nMax); + void SummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); + void SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *SphVariables, WMMtype_CoordSpherical *CoordSpherical, WMMtype_MagneticResults *MagneticResults); + double get_main_field_coeff_g(int index); + double get_main_field_coeff_h(int index); + double get_secular_var_coeff_g(int index); + double get_secular_var_coeff_h(int index); + int DateToYear(int month, int day, int year); + void GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical); +}; } // ****************************** -#endif +#endif // ifndef WORLDMAGMODEL_H diff --git a/ground/openpilotgcs/src/libs/utils/xmlconfig.cpp b/ground/openpilotgcs/src/libs/utils/xmlconfig.cpp index 7b4cc9d5a..3170cd6ca 100644 --- a/ground/openpilotgcs/src/libs/utils/xmlconfig.cpp +++ b/ground/openpilotgcs/src/libs/utils/xmlconfig.cpp @@ -45,7 +45,7 @@ QString XmlConfig::rootName = "gcs"; const QSettings::Format XmlConfig::XmlSettingsFormat = - QSettings::registerFormat("xml", XmlConfig::readXmlFile, XmlConfig::writeXmlFile); + QSettings::registerFormat("xml", XmlConfig::readXmlFile, XmlConfig::writeXmlFile); bool XmlConfig::readXmlFile(QIODevice &device, QSettings::SettingsMap &map) @@ -57,7 +57,7 @@ bool XmlConfig::readXmlFile(QIODevice &device, QSettings::SettingsMap &map) int errorColumn; if (!domDoc.setContent(&device, true, &errorStr, &errorLine, - &errorColumn)) { + &errorColumn)) { QString err = QString(tr("GCS config")) + tr("Parse error at line %1, column %2:\n%3") .arg(errorLine) @@ -72,17 +72,17 @@ bool XmlConfig::readXmlFile(QIODevice &device, QSettings::SettingsMap &map) return true; } -void XmlConfig::handleNode(QDomElement* node, QSettings::SettingsMap &map, QString path) +void XmlConfig::handleNode(QDomElement *node, QSettings::SettingsMap &map, QString path) { - if ( !node ){ + if (!node) { return; } - // qDebug() << "XmlConfig::handleNode start"; + // qDebug() << "XmlConfig::handleNode start"; QString nodeName = node->nodeName(); // For arrays, QT will use simple numbers as keys, which is not a valid element in XML. // Therefore we prefixed these. - if ( nodeName.startsWith(NUM_PREFIX) ){ + if (nodeName.startsWith(NUM_PREFIX)) { nodeName.replace(NUM_PREFIX, ""); } // Xml tags are restrictive with allowed characters, @@ -90,42 +90,42 @@ void XmlConfig::handleNode(QDomElement* node, QSettings::SettingsMap &map, QStri nodeName = nodeName.replace("__PCT__", "%"); nodeName = QUrl::fromPercentEncoding(nodeName.toAscii()); - if ( nodeName == XmlConfig::rootName ) + if (nodeName == XmlConfig::rootName) { ; - else if ( path == "" ) + } else if (path == "") { path = nodeName; - else + } else { path += "/" + nodeName; + } -// qDebug() << "Node: " << ": " << path << " Children: " << node->childNodes().length(); - for ( uint i = 0; i < node->childNodes().length(); ++i ){ +// qDebug() << "Node: " << ": " << path << " Children: " << node->childNodes().length(); + for (uint i = 0; i < node->childNodes().length(); ++i) { QDomNode child = node->childNodes().item(i); - if ( child.isElement() ){ - handleNode( static_cast(&child), map, path); - } - else if ( child.isText() ){ -// qDebug() << "Key: " << path << " Value:" << node->text(); + if (child.isElement()) { + handleNode(static_cast(&child), map, path); + } else if (child.isText()) { +// qDebug() << "Key: " << path << " Value:" << node->text(); map.insert(path, stringToVariant(node->text())); - } - else{ + } else { qDebug() << "Child not Element or text!" << child.nodeType(); } } -// qDebug() << "XmlConfig::handleNode end"; +// qDebug() << "XmlConfig::handleNode end"; } bool XmlConfig::writeXmlFile(QIODevice &device, const QSettings::SettingsMap &map) { QDomDocument outDocument; -// qDebug() << "writeXmlFile start"; - outDocument.appendChild( outDocument.createElement(XmlConfig::rootName)); + +// qDebug() << "writeXmlFile start"; + outDocument.appendChild(outDocument.createElement(XmlConfig::rootName)); QMapIterator iter(map); while (iter.hasNext()) { iter.next(); -// qDebug() << "Entry: " << iter.key() << ": " << iter.value().toString() << endl; +// qDebug() << "Entry: " << iter.key() << ": " << iter.value().toString() << endl; QDomNode node = outDocument.firstChild(); - foreach ( QString elem, iter.key().split('/')){ - if ( elem == "" ){ + foreach(QString elem, iter.key().split('/')) { + if (elem == "") { continue; } // Xml tags are restrictive with allowed characters, @@ -134,14 +134,14 @@ bool XmlConfig::writeXmlFile(QIODevice &device, const QSettings::SettingsMap &ma elem = elem.replace("%", "__PCT__"); // For arrays, QT will use simple numbers as keys, which is not a valid element in XML. // Therefore we prefixed these. - if ( elem.startsWith(NUM_PREFIX) ){ + if (elem.startsWith(NUM_PREFIX)) { qWarning() << "ERROR: Settings must not start with " << NUM_PREFIX - << " in: " + iter.key(); + << " in: " + iter.key(); } - if ( QRegExp("[0-9]").exactMatch(elem.left(1)) ){ + if (QRegExp("[0-9]").exactMatch(elem.left(1))) { elem.prepend(NUM_PREFIX); } - if ( node.firstChildElement(elem).isNull() ){ + if (node.firstChildElement(elem).isNull()) { node.appendChild(outDocument.createElement(elem)); } node = node.firstChildElement(elem); @@ -149,18 +149,20 @@ bool XmlConfig::writeXmlFile(QIODevice &device, const QSettings::SettingsMap &ma node.appendChild(outDocument.createTextNode(variantToString(iter.value()))); } device.write(outDocument.toByteArray(2).constData()); -// qDebug() << "Dokument:\n" << outDocument.toByteArray(2).constData(); -// qDebug() << "writeXmlFile end"; +// qDebug() << "Dokument:\n" << outDocument.toByteArray(2).constData(); +// qDebug() << "writeXmlFile end"; return true; } -QSettings::SettingsMap XmlConfig::settingsToMap(QSettings& qs){ +QSettings::SettingsMap XmlConfig::settingsToMap(QSettings & qs) +{ qDebug() << "settingsToMap:---------------"; QSettings::SettingsMap map; QStringList keys = qs.allKeys(); - foreach (QString key, keys) { + foreach(QString key, keys) { QVariant val = qs.value(key); + qDebug() << key << val.toString(); map.insert(key, val); } @@ -173,34 +175,38 @@ QString XmlConfig::variantToString(const QVariant &v) QString result; switch (v.type()) { - case QVariant::Invalid: - result = QLatin1String("@Invalid()"); - break; + case QVariant::Invalid: + result = QLatin1String("@Invalid()"); + break; - case QVariant::ByteArray: { - QByteArray a = v.toByteArray().toBase64(); - result = QLatin1String("@ByteArray("); - result += QString::fromLatin1(a.constData(), a.size()); - result += QLatin1Char(')'); - break; - } + case QVariant::ByteArray: + { + QByteArray a = v.toByteArray().toBase64(); + result = QLatin1String("@ByteArray("); + result += QString::fromLatin1(a.constData(), a.size()); + result += QLatin1Char(')'); + break; + } - case QVariant::String: - case QVariant::LongLong: - case QVariant::ULongLong: - case QVariant::Int: - case QVariant::UInt: - case QVariant::Bool: - case QVariant::Double: - case QVariant::KeySequence: - case QVariant::Color: { - result = v.toString(); - if (result.startsWith(QLatin1Char('@'))) - result.prepend(QLatin1Char('@')); - break; + case QVariant::String: + case QVariant::LongLong: + case QVariant::ULongLong: + case QVariant::Int: + case QVariant::UInt: + case QVariant::Bool: + case QVariant::Double: + case QVariant::KeySequence: + case QVariant::Color: + { + result = v.toString(); + if (result.startsWith(QLatin1Char('@'))) { + result.prepend(QLatin1Char('@')); } + break; + } #ifndef QT_NO_GEOM_VARIANT - case QVariant::Rect: { + case QVariant::Rect: + { QRect r = qvariant_cast(v); result += QLatin1String("@Rect("); result += QString::number(r.x()); @@ -213,7 +219,8 @@ QString XmlConfig::variantToString(const QVariant &v) result += QLatin1Char(')'); break; } - case QVariant::Size: { + case QVariant::Size: + { QSize s = qvariant_cast(v); result += QLatin1String("@Size("); result += QString::number(s.width()); @@ -222,7 +229,8 @@ QString XmlConfig::variantToString(const QVariant &v) result += QLatin1Char(')'); break; } - case QVariant::Point: { + case QVariant::Point: + { QPoint p = qvariant_cast(v); result += QLatin1String("@Point("); result += QString::number(p.x()); @@ -233,7 +241,8 @@ QString XmlConfig::variantToString(const QVariant &v) } #endif // !QT_NO_GEOM_VARIANT - default: { + default: + { #ifndef QT_NO_DATASTREAM QByteArray a; { @@ -242,20 +251,20 @@ QString XmlConfig::variantToString(const QVariant &v) s << v; } - result = QLatin1String("@Variant("); + result = QLatin1String("@Variant("); result += QString::fromLatin1(a.toBase64().constData()); result += QLatin1Char(')'); - // These were being much too noisy!! - //qDebug() << "Variant Type: " << v.type(); - //qDebug()<< "Variant: " << result; + // These were being much too noisy!! + // qDebug() << "Variant Type: " << v.type(); + // qDebug()<< "Variant: " << result; #else Q_ASSERT(!"QSettings: Cannot save custom types without QDataStream support"); #endif break; } -} + } -return result; + return result; } QVariant XmlConfig::stringToVariant(const QString &s) @@ -272,30 +281,34 @@ QVariant XmlConfig::stringToVariant(const QString &s) QVariant result; stream >> result; return result; + #else Q_ASSERT(!"QSettings: Cannot load custom types without QDataStream support"); #endif #ifndef QT_NO_GEOM_VARIANT } else if (s.startsWith(QLatin1String("@Rect("))) { QStringList args = splitArgs(s, 5); - if (args.size() == 4) + if (args.size() == 4) { return QVariant(QRect(args[0].toInt(), args[1].toInt(), args[2].toInt(), args[3].toInt())); + } } else if (s.startsWith(QLatin1String("@Size("))) { QStringList args = splitArgs(s, 5); - if (args.size() == 2) + if (args.size() == 2) { return QVariant(QSize(args[0].toInt(), args[1].toInt())); + } } else if (s.startsWith(QLatin1String("@Point("))) { QStringList args = splitArgs(s, 6); - if (args.size() == 2) + if (args.size() == 2) { return QVariant(QPoint(args[0].toInt(), args[1].toInt())); + } #endif } else if (s == QLatin1String("@Invalid()")) { return QVariant(); } - } - if (s.startsWith(QLatin1String("@@"))) + if (s.startsWith(QLatin1String("@@"))) { return QVariant(s.mid(1)); + } } return QVariant(s); @@ -304,6 +317,7 @@ QVariant XmlConfig::stringToVariant(const QString &s) QStringList XmlConfig::splitArgs(const QString &s, int idx) { int l = s.length(); + Q_ASSERT(l > 0); Q_ASSERT(s.at(idx) == QLatin1Char('(')); Q_ASSERT(s.at(l - 1) == QLatin1Char(')')); diff --git a/ground/openpilotgcs/src/libs/utils/xmlconfig.h b/ground/openpilotgcs/src/libs/utils/xmlconfig.h index 447b2b6d4..98345e38d 100644 --- a/ground/openpilotgcs/src/libs/utils/xmlconfig.h +++ b/ground/openpilotgcs/src/libs/utils/xmlconfig.h @@ -37,9 +37,7 @@ #include #include -class XMLCONFIG_EXPORT XmlConfig : QObject -{ - +class XMLCONFIG_EXPORT XmlConfig : QObject { public: static const QSettings::Format XmlSettingsFormat; @@ -49,8 +47,8 @@ public: private: static QString rootName; - static void handleNode(QDomElement* node, QSettings::SettingsMap &map, QString path = ""); - static QSettings::SettingsMap settingsToMap(QSettings& qs); + static void handleNode(QDomElement *node, QSettings::SettingsMap &map, QString path = ""); + static QSettings::SettingsMap settingsToMap(QSettings & qs); static QString variantToString(const QVariant &v); static QVariant stringToVariant(const QString &s); static QStringList splitArgs(const QString &s, int idx); diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.cpp b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.cpp index 33cc84c6b..7f10d6029 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.cpp @@ -30,61 +30,59 @@ #include "antennatrackgadgetconfiguration.h" AntennaTrackGadget::AntennaTrackGadget(QString classId, AntennaTrackWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget), - connected(FALSE) + IUAVGadget(classId, parent), + m_widget(widget), + connected(FALSE) { - connect(m_widget->connectButton, SIGNAL(clicked(bool)), this,SLOT(onConnect())); - connect(m_widget->disconnectButton, SIGNAL(clicked(bool)), this,SLOT(onDisconnect())); + connect(m_widget->connectButton, SIGNAL(clicked(bool)), this, SLOT(onConnect())); + connect(m_widget->disconnectButton, SIGNAL(clicked(bool)), this, SLOT(onDisconnect())); } AntennaTrackGadget::~AntennaTrackGadget() -{ -} +{} /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void AntennaTrackGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void AntennaTrackGadget::loadConfiguration(IUAVGadgetConfiguration *config) { // Delete the (old)port, this also closes it. - if(port) { + if (port) { delete port; } // Delete the (old)parser, this also disconnects all signals. - if(parser) { + if (parser) { delete parser; } - AntennaTrackGadgetConfiguration *AntennaTrackConfig = qobject_cast< AntennaTrackGadgetConfiguration*>(config); + AntennaTrackGadgetConfiguration *AntennaTrackConfig = qobject_cast< AntennaTrackGadgetConfiguration *>(config); PortSettings portsettings; - portsettings.BaudRate=AntennaTrackConfig->speed(); - portsettings.DataBits=AntennaTrackConfig->dataBits(); - portsettings.FlowControl=AntennaTrackConfig->flow(); - portsettings.Parity=AntennaTrackConfig->parity(); - portsettings.StopBits=AntennaTrackConfig->stopBits(); - portsettings.Timeout_Millisec=AntennaTrackConfig->timeOut(); + portsettings.BaudRate = AntennaTrackConfig->speed(); + portsettings.DataBits = AntennaTrackConfig->dataBits(); + portsettings.FlowControl = AntennaTrackConfig->flow(); + portsettings.Parity = AntennaTrackConfig->parity(); + portsettings.StopBits = AntennaTrackConfig->stopBits(); + portsettings.Timeout_Millisec = AntennaTrackConfig->timeOut(); // In case we find no port, buttons disabled m_widget->connectButton->setEnabled(false); m_widget->disconnectButton->setEnabled(false); QList ports = QextSerialEnumerator::getPorts(); - foreach( QextPortInfo nport, ports ) { - if(nport.friendName == AntennaTrackConfig->port()) - { + foreach(QextPortInfo nport, ports) { + if (nport.friendName == AntennaTrackConfig->port()) { qDebug() << "Using Serial port"; - //parser = new NMEAParser(); + // parser = new NMEAParser(); #ifdef Q_OS_WIN - port=new QextSerialPort(nport.portName,portsettings,QextSerialPort::EventDriven); + port = new QextSerialPort(nport.portName, portsettings, QextSerialPort::EventDriven); #else - port=new QextSerialPort(nport.physName,portsettings,QextSerialPort::EventDriven); + port = new QextSerialPort(nport.physName, portsettings, QextSerialPort::EventDriven); #endif m_widget->setPort(port); m_widget->connectButton->setEnabled(true); @@ -99,32 +97,33 @@ void AntennaTrackGadget::loadConfiguration(IUAVGadgetConfiguration* config) qDebug() << "Using Telemetry parser"; parser = new TelemetryParser(); - connect(parser, SIGNAL(position(double,double,double)), m_widget,SLOT(setPosition(double,double,double))); - connect(parser, SIGNAL(home(double,double,double)), m_widget,SLOT(setHomePosition(double,double,double))); + connect(parser, SIGNAL(position(double, double, double)), m_widget, SLOT(setPosition(double, double, double))); + connect(parser, SIGNAL(home(double, double, double)), m_widget, SLOT(setHomePosition(double, double, double))); connect(parser, SIGNAL(packet(QString)), m_widget, SLOT(dumpPacket(QString))); } -void AntennaTrackGadget::onConnect() { +void AntennaTrackGadget::onConnect() +{ m_widget->textBrowser->append(QString("Connecting to Tracker ...\n")); // TODO: Somehow mark that we're running, and disable connect button while so? if (port) { - qDebug() << "Opening: " << port->portName() << "."; - bool isOpen = port->open(QIODevice::ReadWrite); - qDebug() << "Open: " << isOpen; - if(isOpen) { + qDebug() << "Opening: " << port->portName() << "."; + bool isOpen = port->open(QIODevice::ReadWrite); + qDebug() << "Open: " << isOpen; + if (isOpen) { m_widget->connectButton->setEnabled(false); m_widget->disconnectButton->setEnabled(true); } } else { qDebug() << "Port undefined or invalid."; } - } -void AntennaTrackGadget::onDisconnect() { +void AntennaTrackGadget::onDisconnect() +{ if (port) { - qDebug() << "Closing: " << port->portName() << "."; + qDebug() << "Closing: " << port->portName() << "."; port->close(); m_widget->connectButton->setEnabled(true); m_widget->disconnectButton->setEnabled(false); @@ -133,23 +132,27 @@ void AntennaTrackGadget::onDisconnect() { } } -void AntennaTrackGadget::onDataAvailable() { +void AntennaTrackGadget::onDataAvailable() +{ int avail = port->bytesAvailable(); - if( avail > 0 ) { + + if (avail > 0) { QByteArray serialData; serialData.resize(avail); int bytesRead = port->read(serialData.data(), serialData.size()); - if( bytesRead > 0 ) { + if (bytesRead > 0) { processNewSerialData(serialData); } } } -void AntennaTrackGadget::processNewSerialData(QByteArray serialData) { - int dataLength = serialData.size(); - const char* data = serialData.constData(); +void AntennaTrackGadget::processNewSerialData(QByteArray serialData) +{ + int dataLength = serialData.size(); + const char *data = serialData.constData(); + m_widget->textBrowser->append(QString(serialData)); - for(int pos = 0; pos < dataLength; pos++) { - //parser->processInputStream(data[pos]); + for (int pos = 0; pos < dataLength; pos++) { + // parser->processInputStream(data[pos]); } } diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.h b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.h index 9363abaf2..682bda768 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadget.h @@ -41,18 +41,20 @@ class AntennaTrackWidget; using namespace Core; -class AntennaTrackGadget : public Core::IUAVGadget -{ +class AntennaTrackGadget : public Core::IUAVGadget { Q_OBJECT public: AntennaTrackGadget(QString classId, AntennaTrackWidget *widget, QWidget *parent = 0); ~AntennaTrackGadget(); - QWidget *widget() { return m_widget; } + QWidget *widget() + { + return m_widget; + } - // void setMode(QString mode); // Either UAVTalk or serial port + // void setMode(QString mode); // Either UAVTalk or serial port - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); public slots: void onConnect(); void onDisconnect(); diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.cpp index f4c8ed23e..e346db875 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.cpp @@ -32,7 +32,7 @@ * Loads a saved configuration or defaults if non exist. * */ -AntennaTrackGadgetConfiguration::AntennaTrackGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +AntennaTrackGadgetConfiguration::AntennaTrackGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_connectionMode("Serial"), m_defaultPort("Unknown"), @@ -43,36 +43,35 @@ AntennaTrackGadgetConfiguration::AntennaTrackGadgetConfiguration(QString classId m_defaultStopBits(STOP_1), m_defaultTimeOut(5000) { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { BaudRateType speed; DataBitsType databits; FlowType flow; ParityType parity; StopBitsType stopbits; - int ispeed = qSettings->value("defaultSpeed").toInt(); - int idatabits = qSettings->value("defaultDataBits").toInt(); - int iflow = qSettings->value("defaultFlow").toInt(); - int iparity = qSettings->value("defaultParity").toInt(); - int istopbits = qSettings->value("defaultStopBits").toInt(); - QString port = qSettings->value("defaultPort").toString(); + int ispeed = qSettings->value("defaultSpeed").toInt(); + int idatabits = qSettings->value("defaultDataBits").toInt(); + int iflow = qSettings->value("defaultFlow").toInt(); + int iparity = qSettings->value("defaultParity").toInt(); + int istopbits = qSettings->value("defaultStopBits").toInt(); + QString port = qSettings->value("defaultPort").toString(); QString conMode = qSettings->value("connectionMode").toString(); - databits = (DataBitsType) idatabits; - flow = (FlowType)iflow; - parity = (ParityType)iparity; + databits = (DataBitsType)idatabits; + flow = (FlowType)iflow; + parity = (ParityType)iparity; stopbits = (StopBitsType)istopbits; - speed = (BaudRateType)ispeed; - m_defaultPort = port; - m_defaultSpeed = speed; + speed = (BaudRateType)ispeed; + m_defaultPort = port; + m_defaultSpeed = speed; m_defaultDataBits = databits; - m_defaultFlow = flow; - m_defaultParity = parity; + m_defaultFlow = flow; + m_defaultParity = parity; m_defaultStopBits = stopbits; - m_connectionMode = conMode; + m_connectionMode = conMode; } - } /** @@ -83,13 +82,13 @@ IUAVGadgetConfiguration *AntennaTrackGadgetConfiguration::clone() { AntennaTrackGadgetConfiguration *m = new AntennaTrackGadgetConfiguration(this->classId()); - m->m_defaultSpeed = m_defaultSpeed; + m->m_defaultSpeed = m_defaultSpeed; m->m_defaultDataBits = m_defaultDataBits; - m->m_defaultFlow = m_defaultFlow; - m->m_defaultParity = m_defaultParity; + m->m_defaultFlow = m_defaultFlow; + m->m_defaultParity = m_defaultParity; m->m_defaultStopBits = m_defaultStopBits; - m->m_defaultPort = m_defaultPort; - m->m_connectionMode = m_connectionMode; + m->m_defaultPort = m_defaultPort; + m->m_connectionMode = m_connectionMode; return m; } @@ -97,13 +96,13 @@ IUAVGadgetConfiguration *AntennaTrackGadgetConfiguration::clone() * Saves a configuration. * */ -void AntennaTrackGadgetConfiguration::saveConfig(QSettings* settings) const { - settings->setValue("defaultSpeed", m_defaultSpeed); - settings->setValue("defaultDataBits", m_defaultDataBits); - settings->setValue("defaultFlow", m_defaultFlow); - settings->setValue("defaultParity", m_defaultParity); - settings->setValue("defaultStopBits", m_defaultStopBits); - settings->setValue("defaultPort", m_defaultPort); - settings->setValue("connectionMode", m_connectionMode); - +void AntennaTrackGadgetConfiguration::saveConfig(QSettings *settings) const +{ + settings->setValue("defaultSpeed", m_defaultSpeed); + settings->setValue("defaultDataBits", m_defaultDataBits); + settings->setValue("defaultFlow", m_defaultFlow); + settings->setValue("defaultParity", m_defaultParity); + settings->setValue("defaultStopBits", m_defaultStopBits); + settings->setValue("defaultPort", m_defaultPort); + settings->setValue("connectionMode", m_connectionMode); } diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.h index 2521607ff..ad7c171f0 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetconfiguration.h @@ -33,46 +33,92 @@ using namespace Core; -class AntennaTrackGadgetConfiguration : public IUAVGadgetConfiguration -{ +class AntennaTrackGadgetConfiguration : public IUAVGadgetConfiguration { Q_OBJECT - public: - explicit AntennaTrackGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); +public: + explicit AntennaTrackGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void setConnectionMode(QString mode) { m_connectionMode = mode; } - QString connectionMode() { return m_connectionMode; } + void setConnectionMode(QString mode) + { + m_connectionMode = mode; + } + QString connectionMode() + { + return m_connectionMode; + } - //set port configuration functions - void setSpeed(BaudRateType speed) {m_defaultSpeed=speed;} - void setDataBits(DataBitsType databits) {m_defaultDataBits=databits;} - void setFlow(FlowType flow) {m_defaultFlow=flow;} - void setParity(ParityType parity) {m_defaultParity=parity;} - void setStopBits(StopBitsType stopbits) {m_defaultStopBits=stopbits;} - void setPort(QString port){m_defaultPort=port;} - void setTimeOut(long timeout){m_defaultTimeOut=timeout;} + // set port configuration functions + void setSpeed(BaudRateType speed) + { + m_defaultSpeed = speed; + } + void setDataBits(DataBitsType databits) + { + m_defaultDataBits = databits; + } + void setFlow(FlowType flow) + { + m_defaultFlow = flow; + } + void setParity(ParityType parity) + { + m_defaultParity = parity; + } + void setStopBits(StopBitsType stopbits) + { + m_defaultStopBits = stopbits; + } + void setPort(QString port) + { + m_defaultPort = port; + } + void setTimeOut(long timeout) + { + m_defaultTimeOut = timeout; + } - //get port configuration functions - QString port(){return m_defaultPort;} - BaudRateType speed() {return m_defaultSpeed;} - FlowType flow() {return m_defaultFlow;} - DataBitsType dataBits() {return m_defaultDataBits;} - StopBitsType stopBits() {return m_defaultStopBits;} - ParityType parity() {return m_defaultParity;} - long timeOut(){return m_defaultTimeOut;} + // get port configuration functions + QString port() + { + return m_defaultPort; + } + BaudRateType speed() + { + return m_defaultSpeed; + } + FlowType flow() + { + return m_defaultFlow; + } + DataBitsType dataBits() + { + return m_defaultDataBits; + } + StopBitsType stopBits() + { + return m_defaultStopBits; + } + ParityType parity() + { + return m_defaultParity; + } + long timeOut() + { + return m_defaultTimeOut; + } - void saveConfig(QSettings* settings) const; - IUAVGadgetConfiguration *clone(); - - private: - QString m_connectionMode; - QString m_defaultPort; - BaudRateType m_defaultSpeed; - DataBitsType m_defaultDataBits; - FlowType m_defaultFlow; - ParityType m_defaultParity; - StopBitsType m_defaultStopBits; - long m_defaultTimeOut; + void saveConfig(QSettings *settings) const; + IUAVGadgetConfiguration *clone(); +private: + QString m_connectionMode; + QString m_defaultPort; + BaudRateType m_defaultSpeed; + DataBitsType m_defaultDataBits; + FlowType m_defaultFlow; + ParityType m_defaultParity; + StopBitsType m_defaultStopBits; + long m_defaultTimeOut; }; #endif // ANTENNATRACKGADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.cpp index 7f369e033..14818b1cc 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.cpp @@ -32,29 +32,27 @@ #include AntennaTrackGadgetFactory::AntennaTrackGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("AntennaTrackGadget"), - tr("Antenna Track Gadget"), - parent) -{ -} + IUAVGadgetFactory(QString("AntennaTrackGadget"), + tr("Antenna Track Gadget"), + parent) +{} AntennaTrackGadgetFactory::~AntennaTrackGadgetFactory() -{ -} +{} -Core::IUAVGadget* AntennaTrackGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *AntennaTrackGadgetFactory::createGadget(QWidget *parent) { - AntennaTrackWidget* gadgetWidget = new AntennaTrackWidget(parent); + AntennaTrackWidget *gadgetWidget = new AntennaTrackWidget(parent); + return new AntennaTrackGadget(QString("AntennaTrackGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *AntennaTrackGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *AntennaTrackGadgetFactory::createConfiguration(QSettings *qSettings) { return new AntennaTrackGadgetConfiguration(QString("AntennaTrackGadget"), qSettings); } IOptionsPage *AntennaTrackGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new AntennaTrackGadgetOptionsPage(qobject_cast(config)); + return new AntennaTrackGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.h b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.h index e3efd07d5..f8e1333ba 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetfactory.h @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class AntennaTrackGadgetFactory : public IUAVGadgetFactory -{ +class AntennaTrackGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: AntennaTrackGadgetFactory(QObject *parent = 0); ~AntennaTrackGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.cpp index eb7c37b21..46f6df650 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.cpp @@ -34,135 +34,135 @@ #include AntennaTrackGadgetOptionsPage::AntennaTrackGadgetOptionsPage(AntennaTrackGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) + IOptionsPage(parent), + m_config(config) { -// Taken from the uploader gadget, since we also can use a serial port for this -// Gadget +// Taken from the uploader gadget, since we also can use a serial port for this +// Gadget - //the begining of some ugly code -//diferent OS's have diferent serial port capabilities + // the begining of some ugly code +// diferent OS's have diferent serial port capabilities #ifdef Q_OS_WIN -//load windows port capabilities -BaudRateTypeString - <<"BAUD110" - <<"BAUD300" - <<"BAUD600" - <<"BAUD1200" - <<"BAUD2400" - <<"BAUD4800" - <<"BAUD9600" - <<"BAUD14400" - <<"BAUD19200" - <<"BAUD38400" - <<"BAUD56000" - <<"BAUD57600" - <<"BAUD115200" - <<"BAUD128000" - <<"BAUD256000"; -DataBitsTypeString - <<"DATA_5" - <<"DATA_6" - <<"DATA_7" - <<"DATA_8"; -ParityTypeString - <<"PAR_NONE" - <<"PAR_ODD" - <<"PAR_EVEN" - <<"PAR_MARK" //WINDOWS ONLY - <<"PAR_SPACE"; -StopBitsTypeString - <<"STOP_1" - <<"STOP_1_5" //WINDOWS ONLY - <<"STOP_2"; -#else -//load POSIX port capabilities -BaudRateTypeString +// load windows port capabilities + BaudRateTypeString + << "BAUD110" + << "BAUD300" + << "BAUD600" + << "BAUD1200" + << "BAUD2400" + << "BAUD4800" + << "BAUD9600" + << "BAUD14400" + << "BAUD19200" + << "BAUD38400" + << "BAUD56000" + << "BAUD57600" + << "BAUD115200" + << "BAUD128000" + << "BAUD256000"; + DataBitsTypeString + << "DATA_5" + << "DATA_6" + << "DATA_7" + << "DATA_8"; + ParityTypeString + << "PAR_NONE" + << "PAR_ODD" + << "PAR_EVEN" + << "PAR_MARK" // WINDOWS ONLY + << "PAR_SPACE"; + StopBitsTypeString + << "STOP_1" + << "STOP_1_5" // WINDOWS ONLY + << "STOP_2"; +#else // ifdef Q_OS_WIN +// load POSIX port capabilities + BaudRateTypeString - <<"BAUD50" //POSIX ONLY - <<"BAUD75" //POSIX ONLY - <<"BAUD110" - <<"BAUD134" //POSIX ONLY - <<"BAUD150" //POSIX ONLY - <<"BAUD200" //POSIX ONLY - <<"BAUD300" - <<"BAUD600" - <<"BAUD1200" - <<"BAUD1800" //POSIX ONLY - <<"BAUD2400" - <<"BAUD4800" - <<"BAUD9600" - <<"BAUD19200" - <<"BAUD38400" - <<"BAUD57600" - <<"BAUD76800" //POSIX ONLY - <<"BAUD115200"; -DataBitsTypeString - <<"DATA_5" - <<"DATA_6" - <<"DATA_7" - <<"DATA_8"; -ParityTypeString - <<"PAR_NONE" - <<"PAR_ODD" - <<"PAR_EVEN" - <<"PAR_SPACE"; -StopBitsTypeString - <<"STOP_1" - <<"STOP_2"; -#endif -//load all OS's capabilities -BaudRateTypeStringALL - <<"BAUD50" //POSIX ONLY - <<"BAUD75" //POSIX ONLY - <<"BAUD110" - <<"BAUD134" //POSIX ONLY - <<"BAUD150" //POSIX ONLY - <<"BAUD200" //POSIX ONLY - <<"BAUD300" - <<"BAUD600" - <<"BAUD1200" - <<"BAUD1800" //POSIX ONLY - <<"BAUD2400" - <<"BAUD4800" - <<"BAUD9600" - <<"BAUD14400" - <<"BAUD19200" - <<"BAUD38400" - <<"BAUD56000" - <<"BAUD57600" - <<"BAUD76800" //POSIX ONLY - <<"BAUD115200" - <<"BAUD128000" - <<"BAUD256000"; -DataBitsTypeStringALL - <<"DATA_5" - <<"DATA_6" - <<"DATA_7" - <<"DATA_8"; -ParityTypeStringALL - <<"PAR_NONE" - <<"PAR_ODD" - <<"PAR_EVEN" - <<"PAR_MARK" //WINDOWS ONLY - <<"PAR_SPACE"; -StopBitsTypeStringALL - <<"STOP_1" - <<"STOP_1_5" //WINDOWS ONLY - <<"STOP_2"; + << "BAUD50" // POSIX ONLY + << "BAUD75" // POSIX ONLY + << "BAUD110" + << "BAUD134" // POSIX ONLY + << "BAUD150" // POSIX ONLY + << "BAUD200" // POSIX ONLY + << "BAUD300" + << "BAUD600" + << "BAUD1200" + << "BAUD1800" // POSIX ONLY + << "BAUD2400" + << "BAUD4800" + << "BAUD9600" + << "BAUD19200" + << "BAUD38400" + << "BAUD57600" + << "BAUD76800" // POSIX ONLY + << "BAUD115200"; + DataBitsTypeString + << "DATA_5" + << "DATA_6" + << "DATA_7" + << "DATA_8"; + ParityTypeString + << "PAR_NONE" + << "PAR_ODD" + << "PAR_EVEN" + << "PAR_SPACE"; + StopBitsTypeString + << "STOP_1" + << "STOP_2"; +#endif // ifdef Q_OS_WIN +// load all OS's capabilities + BaudRateTypeStringALL + << "BAUD50" // POSIX ONLY + << "BAUD75" // POSIX ONLY + << "BAUD110" + << "BAUD134" // POSIX ONLY + << "BAUD150" // POSIX ONLY + << "BAUD200" // POSIX ONLY + << "BAUD300" + << "BAUD600" + << "BAUD1200" + << "BAUD1800" // POSIX ONLY + << "BAUD2400" + << "BAUD4800" + << "BAUD9600" + << "BAUD14400" + << "BAUD19200" + << "BAUD38400" + << "BAUD56000" + << "BAUD57600" + << "BAUD76800" // POSIX ONLY + << "BAUD115200" + << "BAUD128000" + << "BAUD256000"; + DataBitsTypeStringALL + << "DATA_5" + << "DATA_6" + << "DATA_7" + << "DATA_8"; + ParityTypeStringALL + << "PAR_NONE" + << "PAR_ODD" + << "PAR_EVEN" + << "PAR_MARK" // WINDOWS ONLY + << "PAR_SPACE"; + StopBitsTypeStringALL + << "STOP_1" + << "STOP_1_5" // WINDOWS ONLY + << "STOP_2"; -FlowTypeString - <<"FLOW_OFF" - <<"FLOW_HARDWARE" - <<"FLOW_XONXOFF"; + FlowTypeString + << "FLOW_OFF" + << "FLOW_HARDWARE" + << "FLOW_XONXOFF"; } -bool sortPorts(QextPortInfo const& s1,QextPortInfo const& s2) +bool sortPorts(QextPortInfo const & s1, QextPortInfo const & s2) { -return s1.portName ports = QextSerialEnumerator::getPorts(); - qSort(ports.begin(), ports.end(),sortPorts); - foreach( QextPortInfo port, ports ) { + qSort(ports.begin(), ports.end(), sortPorts); + foreach(QextPortInfo port, ports) { qDebug() << "Adding port: " << port.friendName << " (" << port.portName << ")"; options_page->portComboBox->addItem(port.friendName, port.friendName); } int portIndex = options_page->portComboBox->findData(m_config->port()); - if(portIndex!=-1){ + if (portIndex != -1) { qDebug() << "createPage(): port is " << m_config->port(); options_page->portComboBox->setCurrentIndex(portIndex); } @@ -188,40 +188,40 @@ QWidget *AntennaTrackGadgetOptionsPage::createPage(QWidget *parent) options_page->portSpeedComboBox->addItems(BaudRateTypeString); int portSpeedIndex = options_page->portSpeedComboBox->findText(BaudRateTypeStringALL.at((int)m_config->speed())); - if(portSpeedIndex != -1){ - options_page->portSpeedComboBox->setCurrentIndex(portSpeedIndex); + if (portSpeedIndex != -1) { + options_page->portSpeedComboBox->setCurrentIndex(portSpeedIndex); } // FLOW CONTROL options_page->flowControlComboBox->addItems(FlowTypeString); int flowControlIndex = options_page->flowControlComboBox->findText(FlowTypeString.at((int)m_config->flow())); - if(flowControlIndex != -1){ - options_page->flowControlComboBox->setCurrentIndex(flowControlIndex); + if (flowControlIndex != -1) { + options_page->flowControlComboBox->setCurrentIndex(flowControlIndex); } // DATABITS options_page->dataBitsComboBox->addItems(DataBitsTypeString); int dataBitsIndex = options_page->dataBitsComboBox->findText(DataBitsTypeStringALL.at((int)m_config->dataBits())); - if(dataBitsIndex != -1){ - options_page->dataBitsComboBox->setCurrentIndex(dataBitsIndex); + if (dataBitsIndex != -1) { + options_page->dataBitsComboBox->setCurrentIndex(dataBitsIndex); } // STOPBITS options_page->stopBitsComboBox->addItems(StopBitsTypeString); int stopBitsIndex = options_page->stopBitsComboBox->findText(StopBitsTypeStringALL.at((int)m_config->stopBits())); - if(stopBitsIndex != -1){ - options_page->stopBitsComboBox->setCurrentIndex(stopBitsIndex); + if (stopBitsIndex != -1) { + options_page->stopBitsComboBox->setCurrentIndex(stopBitsIndex); } // PARITY options_page->parityComboBox->addItems(ParityTypeString); int parityIndex = options_page->parityComboBox->findText(ParityTypeStringALL.at((int)m_config->parity())); - if(parityIndex != -1){ - options_page->parityComboBox->setCurrentIndex(parityIndex); + if (parityIndex != -1) { + options_page->parityComboBox->setCurrentIndex(parityIndex); } // TIMEOUT @@ -231,8 +231,9 @@ QWidget *AntennaTrackGadgetOptionsPage::createPage(QWidget *parent) connectionModes << "Serial"; options_page->connectionMode->addItems(connectionModes); int conMode = options_page->connectionMode->findText(m_config->connectionMode()); - if (conMode != -1) - options_page->connectionMode->setCurrentIndex(conMode); + if (conMode != -1) { + options_page->connectionMode->setCurrentIndex(conMode); + } return optionsPageWidget; @@ -247,6 +248,7 @@ QWidget *AntennaTrackGadgetOptionsPage::createPage(QWidget *parent) void AntennaTrackGadgetOptionsPage::apply() { int portIndex = options_page->portComboBox->currentIndex(); + m_config->setPort(options_page->portComboBox->itemData(portIndex).toString()); qDebug() << "apply(): port is " << m_config->port(); @@ -255,9 +257,8 @@ void AntennaTrackGadgetOptionsPage::apply() m_config->setDataBits((DataBitsType)DataBitsTypeStringALL.indexOf(options_page->dataBitsComboBox->currentText())); m_config->setStopBits((StopBitsType)StopBitsTypeStringALL.indexOf(options_page->stopBitsComboBox->currentText())); m_config->setParity((ParityType)ParityTypeStringALL.indexOf(options_page->parityComboBox->currentText())); - m_config->setTimeOut( options_page->timeoutSpinBox->value()); + m_config->setTimeOut(options_page->timeoutSpinBox->value()); m_config->setConnectionMode(options_page->connectionMode->currentText()); - } void AntennaTrackGadgetOptionsPage::finish() diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.h index 063869b91..07210dc6e 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackgadgetoptionspage.h @@ -41,14 +41,13 @@ class IUAVGadgetConfiguration; class AntennaTrackGadgetConfiguration; namespace Ui { - class AntennaTrackGadgetOptionsPage; +class AntennaTrackGadgetOptionsPage; } using namespace Core; -class AntennaTrackGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class AntennaTrackGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit AntennaTrackGadgetOptionsPage(AntennaTrackGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.cpp b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.cpp index f63a17ddf..b1593639a 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.cpp @@ -5,15 +5,18 @@ #include #include -AntennaTrackPlugin::AntennaTrackPlugin() { - // Do nothing - } - -AntennaTrackPlugin::~AntennaTrackPlugin() { +AntennaTrackPlugin::AntennaTrackPlugin() +{ // Do nothing } -bool AntennaTrackPlugin::initialize(const QStringList& args, QString *errMsg) { +AntennaTrackPlugin::~AntennaTrackPlugin() +{ + // Do nothing +} + +bool AntennaTrackPlugin::initialize(const QStringList & args, QString *errMsg) +{ Q_UNUSED(args); Q_UNUSED(errMsg); @@ -23,11 +26,13 @@ bool AntennaTrackPlugin::initialize(const QStringList& args, QString *errMsg) { return true; } -void AntennaTrackPlugin::extensionsInitialized() { +void AntennaTrackPlugin::extensionsInitialized() +{ // Do nothing } -void AntennaTrackPlugin::shutdown() { +void AntennaTrackPlugin::shutdown() +{ // Do nothing } diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.h b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.h index 71cf7392c..17c1f37b2 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackplugin.h @@ -5,14 +5,13 @@ class AntennaTrackGadgetFactory; -class AntennaTrackPlugin : public ExtensionSystem::IPlugin -{ +class AntennaTrackPlugin : public ExtensionSystem::IPlugin { public: AntennaTrackPlugin(); ~AntennaTrackPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); private: AntennaTrackGadgetFactory *mf; diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.cpp b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.cpp index 90ad37aac..9306951d2 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.cpp @@ -41,22 +41,21 @@ AntennaTrackWidget::AntennaTrackWidget(QWidget *parent) : QWidget(parent) { setupUi(this); - azimuth_old=0; - elevation_old=0; + azimuth_old = 0; + elevation_old = 0; } AntennaTrackWidget::~AntennaTrackWidget() -{ -} +{} void AntennaTrackWidget::setPort(QPointer portx) { - port=portx; + port = portx; } void AntennaTrackWidget::dumpPacket(const QString &packet) { textBrowser->append(packet); - if(textBrowser->document()->lineCount() > 200) { + if (textBrowser->document()->lineCount() > 200) { QTextCursor tc = textBrowser->textCursor(); tc.movePosition(QTextCursor::Start); tc.movePosition(QTextCursor::Down, QTextCursor::KeepAnchor); @@ -67,63 +66,69 @@ void AntennaTrackWidget::dumpPacket(const QString &packet) void AntennaTrackWidget::setPosition(double lat, double lon, double alt) { - //lat *= 1E-7; - //lon *= 1E-7; + // lat *= 1E-7; + // lon *= 1E-7; double deg = floor(fabs(lat)); - double min = (fabs(lat)-deg)*60; + double min = (fabs(lat) - deg) * 60; QString str1; - str1.sprintf("%.0f%c%.3f' ", deg,0x00b0, min); - if (lat>0) + + str1.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min); + if (lat > 0) { str1.append("N"); - else + } else { str1.append("S"); + } coord_value->setText(str1); deg = floor(fabs(lon)); - min = (fabs(lon)-deg)*60; + min = (fabs(lon) - deg) * 60; QString str2; - str2.sprintf("%.0f%c%.3f' ", deg,0x00b0, min); - if (lon>0) + str2.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min); + if (lon > 0) { str2.append("E"); - else + } else { str2.append("W"); + } coord_value_2->setText(str2); QString str3; str3.sprintf("%.2f m", alt); coord_value_3->setText(str3); - TrackData.Latitude=lat; - TrackData.Longitude=lon; - TrackData.Altitude=alt; + TrackData.Latitude = lat; + TrackData.Longitude = lon; + TrackData.Altitude = alt; calcAntennaPosition(); } void AntennaTrackWidget::setHomePosition(double lat, double lon, double alt) { - //lat *= 1E-7; - //lon *= 1E-7; + // lat *= 1E-7; + // lon *= 1E-7; double deg = floor(fabs(lat)); - double min = (fabs(lat)-deg)*60; + double min = (fabs(lat) - deg) * 60; QString str1; - str1.sprintf("%.0f%c%.3f' ", deg,0x00b0, min); - if (lat>0) + + str1.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min); + if (lat > 0) { str1.append("N"); - else + } else { str1.append("S"); + } speed_value->setText(str1); deg = floor(fabs(lon)); - min = (fabs(lon)-deg)*60; + min = (fabs(lon) - deg) * 60; QString str2; - str2.sprintf("%.0f%c%.3f' ", deg,0x00b0, min); - if (lon>0) + str2.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min); + if (lon > 0) { str2.append("E"); - else + } else { str2.append("W"); + } bear_label->setText(str2); QString str3; str3.sprintf("%.2f m", alt); bear_value->setText(str3); - TrackData.HomeLatitude=lat; - TrackData.HomeLongitude=lon; - TrackData.HomeAltitude=alt; + TrackData.HomeLatitude = lat; + TrackData.HomeLongitude = lon; + TrackData.HomeAltitude = alt; calcAntennaPosition(); } @@ -132,55 +137,57 @@ void AntennaTrackWidget::calcAntennaPosition(void) /** http://www.movable-type.co.uk/scripts/latlong.html **/ double lat1, lat2, lon1, lon2, a, c, d, x, y, brng; double azimuth, elevation; - double gcsAlt=TrackData.HomeAltitude; // Home MSL altitude - double uavAlt=TrackData.Altitude; // UAV MSL altitude - double dAlt=uavAlt-gcsAlt; // Altitude difference + double gcsAlt = TrackData.HomeAltitude; // Home MSL altitude + double uavAlt = TrackData.Altitude; // UAV MSL altitude + double dAlt = uavAlt - gcsAlt; // Altitude difference // Convert to radians - lat1 = TrackData.HomeLatitude*(M_PI/180); // Home lat - lon1 = TrackData.HomeLongitude*(M_PI/180); // Home lon - lat2 = TrackData.Latitude*(M_PI/180); // UAV lat - lon2 = TrackData.Longitude*(M_PI/180); // UAV lon + lat1 = TrackData.HomeLatitude * (M_PI / 180); // Home lat + lon1 = TrackData.HomeLongitude * (M_PI / 180); // Home lon + lat2 = TrackData.Latitude * (M_PI / 180); // UAV lat + lon2 = TrackData.Longitude * (M_PI / 180); // UAV lon // Bearing /** - var y = Math.sin(dLon) * Math.cos(lat2); - var x = Math.cos(lat1)*Math.sin(lat2) - + var y = Math.sin(dLon) * Math.cos(lat2); + var x = Math.cos(lat1)*Math.sin(lat2) - Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon); - var brng = Math.atan2(y, x).toDeg(); - **/ - y = sin(lon2-lon1) * cos(lat2); - x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon2-lon1); - brng = atan2((sin(lon2-lon1)*cos(lat2)),(cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)))*(180/M_PI); - if(brng<0) - brng+=360; + var brng = Math.atan2(y, x).toDeg(); + **/ + y = sin(lon2 - lon1) * cos(lat2); + x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon2 - lon1); + brng = atan2((sin(lon2 - lon1) * cos(lat2)), (cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon2 - lon1))) * (180 / M_PI); + if (brng < 0) { + brng += 360; + } // bearing to stepper azimuth = brng; // Haversine formula for distance /** - var R = 6371; // km - var dLat = (lat2-lat1).toRad(); - var dLon = (lon2-lon1).toRad(); - var a = Math.sin(dLat/2) * Math.sin(dLat/2) + + var R = 6371; // km + var dLat = (lat2-lat1).toRad(); + var dLon = (lon2-lon1).toRad(); + var a = Math.sin(dLat/2) * Math.sin(dLat/2) + Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) * Math.sin(dLon/2) * Math.sin(dLon/2); - var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)); - var d = R * c; - **/ - a = sin((lat2-lat1)/2) * sin((lat2-lat1)/2) + - cos(lat1) * cos(lat2) * - sin((lon2-lon1)/2) * sin((lon2-lon1)/2); - c = 2 * atan2(sqrt(a), sqrt(1-a)); + var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)); + var d = R * c; + **/ + a = sin((lat2 - lat1) / 2) * sin((lat2 - lat1) / 2) + + cos(lat1) * cos(lat2) * + sin((lon2 - lon1) / 2) * sin((lon2 - lon1) / 2); + c = 2 * atan2(sqrt(a), sqrt(1 - a)); d = 6371 * 1000 * c; // Elevation v depends servo direction - if(d!=0) - elevation = 90-(atan(dAlt/d)*(180/M_PI)); - else + if (d != 0) { + elevation = 90 - (atan(dAlt / d) * (180 / M_PI)); + } else { elevation = 0; - //! TODO: sanity check + } + // ! TODO: sanity check QString str3; str3.sprintf("%.0f deg", azimuth); @@ -189,18 +196,17 @@ void AntennaTrackWidget::calcAntennaPosition(void) str3.sprintf("%.0f deg", elevation); elevation_value->setText(str3); - //servo value 2000-4000 - int servo = (int)(2000.0/180*elevation+2000); - int stepper = (int)(400.0/360*(azimuth-azimuth_old)); + // servo value 2000-4000 + int servo = (int)(2000.0 / 180 * elevation + 2000); + int stepper = (int)(400.0 / 360 * (azimuth - azimuth_old)); // send azimuth and elevation to tracker hardware - str3.sprintf("move %d 2000 2000 2000 %d\r", stepper,servo); - if(port->isOpen()) - { - if(azimuth_old!=azimuth || elevation!=elevation_old) + str3.sprintf("move %d 2000 2000 2000 %d\r", stepper, servo); + if (port->isOpen()) { + if (azimuth_old != azimuth || elevation != elevation_old) { port->write(str3.toAscii()); - azimuth_old = azimuth; + } + azimuth_old = azimuth; elevation_old = elevation; } - } diff --git a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.h b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.h index 8b1bea216..17f9abe65 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/antennatrackwidget.h @@ -39,37 +39,34 @@ class Ui_AntennaTrackWidget; -typedef struct struct_TrackData -{ - double Latitude; - double Longitude; - double Altitude; - double HomeLatitude; - double HomeLongitude; - double HomeAltitude; +typedef struct struct_TrackData { + double Latitude; + double Longitude; + double Altitude; + double HomeLatitude; + double HomeLongitude; + double HomeAltitude; +} TrackData_t; -}TrackData_t; - -class AntennaTrackWidget : public QWidget, public Ui_AntennaTrackWidget -{ +class AntennaTrackWidget : public QWidget, public Ui_AntennaTrackWidget { Q_OBJECT public: AntennaTrackWidget(QWidget *parent = 0); - ~AntennaTrackWidget(); - TrackData_t TrackData; - void setPort(QPointer portx); + ~AntennaTrackWidget(); + TrackData_t TrackData; + void setPort(QPointer portx); private slots: - void setPosition(double, double, double); - void setHomePosition(double, double, double); - void dumpPacket(const QString &packet); + void setPosition(double, double, double); + void setHomePosition(double, double, double); + void dumpPacket(const QString &packet); private: - void calcAntennaPosition(void); - QGraphicsSvgItem * marker; - QPointer port; - double azimuth_old; - double elevation_old; + void calcAntennaPosition(void); + QGraphicsSvgItem *marker; + QPointer port; + double azimuth_old; + double elevation_old; }; #endif /* ANTENNATRACKWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.cpp b/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.cpp index da5a3bcfd..a13af7376 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.cpp @@ -33,11 +33,11 @@ GPSParser::GPSParser(QObject *parent) : QObject(parent) } GPSParser::~GPSParser() -{ +{} -} - -void GPSParser::processInputStream(char c) { +void GPSParser::processInputStream(char c) { - Q_UNUSED(c)} + { + Q_UNUSED(c) + } } diff --git a/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.h b/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.h index e189e9ceb..db6765e3b 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/gpsparser.h @@ -32,28 +32,26 @@ #include #include -class GPSParser: public QObject -{ +class GPSParser : public QObject { Q_OBJECT -public: - ~GPSParser(); +public: ~GPSParser(); virtual void processInputStream(char c); protected: GPSParser(QObject *parent = 0); signals: - void sv(int); // Satellites in view - void position(double,double,double); // Lat, Lon, Alt - void home(double,double,double); // Lat, Lon, Alt - void datetime(double,double); // Date then time - void speedheading(double,double); - void packet(QString); // Raw NMEA Packet (or just info) - void satellite(int,int,int,int,int); // Index, PRN, Elevation, Azimuth, SNR - void fixmode(QString); // Mode of fix: "Auto", "Manual". - void fixtype(QString); // Type of fix: "NoGPS", "NoFix", "Fix2D", "Fix3D". - void dop(double, double, double); // HDOP, VDOP, PDOP - void fixSVs(QList); // SV's used for fix. + void sv(int); // Satellites in view + void position(double, double, double); // Lat, Lon, Alt + void home(double, double, double); // Lat, Lon, Alt + void datetime(double, double); // Date then time + void speedheading(double, double); + void packet(QString); // Raw NMEA Packet (or just info) + void satellite(int, int, int, int, int); // Index, PRN, Elevation, Azimuth, SNR + void fixmode(QString); // Mode of fix: "Auto", "Manual". + void fixtype(QString); // Type of fix: "NoGPS", "NoFix", "Fix2D", "Fix3D". + void dop(double, double, double); // HDOP, VDOP, PDOP + void fixSVs(QList); // SV's used for fix. }; #endif // GPSPARSER_H diff --git a/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp b/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp index fc72735e2..6bbdef701 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp @@ -39,43 +39,44 @@ TelemetryParser::TelemetryParser(QObject *parent) : GPSParser(parent) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject *gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + UAVDataObject *gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + if (gpsObj != NULL) { - connect(gpsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateGPS(UAVObject*))); + connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGPS(UAVObject *))); } else { qDebug() << "Error: Object is unknown (GPSPosition)."; } - gpsObj = dynamic_cast(objManager->getObject("HomeLocation")); + gpsObj = dynamic_cast(objManager->getObject("HomeLocation")); if (gpsObj != NULL) { - connect(gpsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateHome(UAVObject*))); + connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateHome(UAVObject *))); } else { qDebug() << "Error: Object is unknown (HomeLocation)."; } - } TelemetryParser::~TelemetryParser() +{} + +void TelemetryParser::updateHome(UAVObject *object1) { - -} - -void TelemetryParser::updateHome( UAVObject* object1) { double lat = object1->getField(QString("Latitude"))->getDouble(); double lon = object1->getField(QString("Longitude"))->getDouble(); double alt = object1->getField(QString("Altitude"))->getDouble(); + lat *= 1E-7; lon *= 1E-7; - emit home(lat,lon,alt); + emit home(lat, lon, alt); } -void TelemetryParser::updateGPS( UAVObject* object1) { +void TelemetryParser::updateGPS(UAVObject *object1) +{ double lat = object1->getField(QString("Latitude"))->getDouble(); double lon = object1->getField(QString("Longitude"))->getDouble(); double alt = object1->getField(QString("Altitude"))->getDouble(); + lat *= 1E-7; lon *= 1E-7; - emit position(lat,lon,alt); + emit position(lat, lon, alt); } - diff --git a/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.h b/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.h index c3e37fa77..b054a80d4 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.h +++ b/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.h @@ -36,17 +36,15 @@ #include "gpsparser.h" -class TelemetryParser: public GPSParser -{ - +class TelemetryParser : public GPSParser { Q_OBJECT - + public: - TelemetryParser(QObject *parent = 0); - ~TelemetryParser(); + TelemetryParser(QObject *parent = 0); + ~TelemetryParser(); public slots: - void updateGPS(UAVObject* object1); - void updateHome(UAVObject* object1); + void updateGPS(UAVObject *object1); + void updateHome(UAVObject *object1); }; #endif // TELEMETRYPARSER_H diff --git a/ground/openpilotgcs/src/plugins/config/alignment-calibration.cpp b/ground/openpilotgcs/src/plugins/config/alignment-calibration.cpp index c6b880a77..fde1c0f4b 100644 --- a/ground/openpilotgcs/src/plugins/config/alignment-calibration.cpp +++ b/ground/openpilotgcs/src/plugins/config/alignment-calibration.cpp @@ -20,32 +20,31 @@ using namespace Eigen; * reference frame. * @param n_samples The number of samples. */ -void -calibration_misalignment(Vector3f& rotationVector, - const Vector3f samples0[], - const Vector3f& reference0, - const Vector3f samples1[], - const Vector3f& reference1, - size_t n_samples) +void calibration_misalignment(Vector3f & rotationVector, + const Vector3f samples0[], + const Vector3f & reference0, + const Vector3f samples1[], + const Vector3f & reference1, + size_t n_samples) { - // Note that this implementation makes the assumption that the angular - // misalignment is small. Something based on QUEST would be needed to - // account for errors larger than a few degrees. - Matrix X(n_samples, 3); - Matrix y(n_samples, 1); + // Note that this implementation makes the assumption that the angular + // misalignment is small. Something based on QUEST would be needed to + // account for errors larger than a few degrees. + Matrix X(n_samples, 3); + Matrix y(n_samples, 1); - AngleAxisd reference(Quaterniond().setFromTwoVectors( - reference0.cast(), reference1.cast())); - for (size_t i = 0; i < n_samples; ++i) { - AngleAxisd observation(Quaterniond().setFromTwoVectors( - samples0[i].cast(), samples1[i].cast())); + AngleAxisd reference(Quaterniond().setFromTwoVectors( + reference0.cast(), reference1.cast())); + for (size_t i = 0; i < n_samples; ++i) { + AngleAxisd observation(Quaterniond().setFromTwoVectors( + samples0[i].cast(), samples1[i].cast())); - X.row(i) = observation.axis(); - y[i] = reference.angle() - observation.angle(); - } + X.row(i) = observation.axis(); + y[i] = reference.angle() - observation.angle(); + } - // Run linear least squares over the result. - Vector3d result; - (X.transpose() * X).ldlt().solve(X.transpose()*y, &result); - rotationVector = result.cast(); + // Run linear least squares over the result. + Vector3d result; + (X.transpose() * X).ldlt().solve(X.transpose() * y, &result); + rotationVector = result.cast(); } diff --git a/ground/openpilotgcs/src/plugins/config/assertions.h b/ground/openpilotgcs/src/plugins/config/assertions.h index 6b8c0c8fe..fc865ec55 100644 --- a/ground/openpilotgcs/src/plugins/config/assertions.h +++ b/ground/openpilotgcs/src/plugins/config/assertions.h @@ -28,51 +28,52 @@ #include template -bool hasNaN(const MatrixBase& expr); +bool hasNaN(const MatrixBase & expr); template -bool hasInf(const MatrixBase& expr); +bool hasInf(const MatrixBase & expr); template -bool perpendicular(const MatrixBase& expl, const MatrixBase& expr); +bool perpendicular(const MatrixBase & expl, const MatrixBase & expr); template -bool hasNaN(const MatrixBase& expr) +bool hasNaN(const MatrixBase & expr) { - for (int j = 0; j != expr.cols(); ++j) { - for (int i = 0; i != expr.rows(); ++i) { - if (std::isnan(expr.coeff(i, j))) - return true; - } - } - return false; + for (int j = 0; j != expr.cols(); ++j) { + for (int i = 0; i != expr.rows(); ++i) { + if (std::isnan(expr.coeff(i, j))) { + return true; + } + } + } + return false; } template -bool hasInf(const MatrixBase& expr) +bool hasInf(const MatrixBase & expr) { - for (int i = 0; i != expr.cols(); ++i) { - for (int j = 0; j != expr.rows(); ++j) { - if (std::isinf(expr.coeff(j, i))) - return true; - } - } - return false; + for (int i = 0; i != expr.cols(); ++i) { + for (int j = 0; j != expr.rows(); ++j) { + if (std::isinf(expr.coeff(j, i))) { + return true; + } + } + } + return false; } template -bool isReal(const MatrixBase& exp) +bool isReal(const MatrixBase & exp) { - return !hasNaN(exp) && ! hasInf(exp); + return !hasNaN(exp) && !hasInf(exp); } template -bool perpendicular(const MatrixBase& lhs, const MatrixBase& rhs) +bool perpendicular(const MatrixBase & lhs, const MatrixBase & rhs) { - // A really weak check for "almost perpendicular". Use it for debugging - // purposes only. - return fabs(rhs.dot(lhs)) < 0.0001; + // A really weak check for "almost perpendicular". Use it for debugging + // purposes only. + return fabs(rhs.dot(lhs)) < 0.0001; } #endif /* ASSERTIONS_HPP_ */ - diff --git a/ground/openpilotgcs/src/plugins/config/calibration.h b/ground/openpilotgcs/src/plugins/config/calibration.h index 8fa40167d..47805d105 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration.h +++ b/ground/openpilotgcs/src/plugins/config/calibration.h @@ -6,49 +6,42 @@ using std::size_t; using namespace Eigen; -void -calibration_misalignment(Vector3f& rotationVector, - const Vector3f samples0[], - const Vector3f& reference0, - const Vector3f samples1[], - const Vector3f& reference1, - size_t n_samples); +void calibration_misalignment(Vector3f & rotationVector, + const Vector3f samples0[], + const Vector3f & reference0, + const Vector3f samples1[], + const Vector3f & reference1, + size_t n_samples); -Vector3f -twostep_bias_only(const Vector3f samples[], - size_t n_samples, - const Vector3f& referenceField, - const float noise); +Vector3f twostep_bias_only(const Vector3f samples[], + size_t n_samples, + const Vector3f & referenceField, + const float noise); -void -twostep_bias_scale(Vector3f& bias, - Vector3f& scale, - const Vector3f samples[], - const size_t n_samples, - const Vector3f& referenceField, - const float noise); +void twostep_bias_scale(Vector3f & bias, + Vector3f & scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f & referenceField, + const float noise); -void -twostep_bias_scale(Vector3f& bias, - Matrix3f& scale, - const Vector3f samples[], - const size_t n_samples, - const Vector3f& referenceField, - const float noise); +void twostep_bias_scale(Vector3f & bias, + Matrix3f & scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f & referenceField, + const float noise); -void -openpilot_bias_scale(Vector3f& bias, - Vector3f& scale, - const Vector3f samples[], - const size_t n_samples, - const Vector3f& referenceField); +void openpilot_bias_scale(Vector3f & bias, + Vector3f & scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f & referenceField); -void -gyroscope_calibration(Vector3f& bias, - Matrix3f& accelSensitivity, - Vector3f gyroSamples[], - Vector3f accelSamples[], - size_t n_samples); +void gyroscope_calibration(Vector3f & bias, + Matrix3f & accelSensitivity, + Vector3f gyroSamples[], + Vector3f accelSamples[], + size_t n_samples); #endif // !defined AHRS_CALIBRATION_HPP - diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp index abab410fd..166d8c9a1 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp @@ -46,13 +46,14 @@ QStringList ConfigCcpmWidget::getChannelDescriptions() { // init a channel_numelem list of channel desc defaults QStringList channelDesc; - for (int i = 0; i < (int) ConfigCcpmWidget::CHANNEL_NUMELEM; i++) { + + for (int i = 0; i < (int)ConfigCcpmWidget::CHANNEL_NUMELEM; i++) { channelDesc.append(QString("-")); } // get the gui config data GUIConfigDataUnion configData = getConfigData(); - heliGUISettingsStruct heli = configData.heli; + heliGUISettingsStruct heli = configData.heli; if (heli.Throttle > 0) { channelDesc[heli.Throttle - 1] = QString("Throttle"); @@ -118,26 +119,26 @@ QStringList ConfigCcpmWidget::getChannelDescriptions() } ConfigCcpmWidget::ConfigCcpmWidget(QWidget *parent) : - VehicleConfig(parent), m_aircraft(new Ui_CcpmConfigWidget()) + VehicleConfig(parent), m_aircraft(new Ui_CcpmConfigWidget()) { m_aircraft->setupUi(this); SwashLvlConfigurationInProgress = 0; SwashLvlState = 0; SwashLvlServoInterlock = 0; - updatingFromHardware = FALSE; - updatingToHardware = FALSE; + updatingFromHardware = FALSE; + updatingToHardware = FALSE; // Initialization of the swashplaye widget m_aircraft->SwashplateImage->setScene(new QGraphicsScene(this)); m_aircraft->SwashLvlSwashplateImage->setScene(m_aircraft->SwashplateImage->scene()); m_aircraft->SwashLvlSwashplateImage->setSceneRect(-50, -50, 500, 500); - //m_aircraft->SwashLvlSwashplateImage->scale(.85,.85); + // m_aircraft->SwashLvlSwashplateImage->scale(.85,.85); - //m_aircraft->SwashplateImage->setSceneRect(SwashplateImg->boundingRect()); + // m_aircraft->SwashplateImage->setSceneRect(SwashplateImg->boundingRect()); m_aircraft->SwashplateImage->setSceneRect(-50, -30, 500, 500); - //m_aircraft->SwashplateImage->scale(.85,.85); + // m_aircraft->SwashplateImage->scale(.85,.85); QSvgRenderer *renderer = new QSvgRenderer(); renderer->load(QString(":/configgadget/images/ccpm_setup.svg")); @@ -146,7 +147,7 @@ ConfigCcpmWidget::ConfigCcpmWidget(QWidget *parent) : SwashplateImg->setSharedRenderer(renderer); SwashplateImg->setElementId("Swashplate"); SwashplateImg->setObjectName("Swashplate"); - //SwashplateImg->setScale(0.75); + // SwashplateImg->setScale(0.75); m_aircraft->SwashplateImage->scene()->addItem(SwashplateImg); QFont serifFont("Times", 24, QFont::Bold); @@ -163,13 +164,13 @@ ConfigCcpmWidget::ConfigCcpmWidget(QWidget *parent) : // creates a default pen QPen pen2; - //pen2.setStyle(Qt::DotLine); + // pen2.setStyle(Qt::DotLine); pen2.setWidth(1); pen2.setBrush(Qt::blue); - //pen2.setCapStyle(Qt::RoundCap); - //pen2.setJoinStyle(Qt::RoundJoin); + // pen2.setCapStyle(Qt::RoundCap); + // pen2.setJoinStyle(Qt::RoundJoin); - //brush.setStyle(Qt::RadialGradientPattern); + // brush.setStyle(Qt::RadialGradientPattern); QList ServoNames; ServoNames << "ServoW" << "ServoX" << "ServoY" << "ServoZ"; @@ -177,7 +178,7 @@ ConfigCcpmWidget::ConfigCcpmWidget(QWidget *parent) : for (int i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { ServoLines[i] = m_aircraft->SwashLvlSwashplateImage->scene()->addLine(0, 0, 100 * i, i * i * 100, pen); - Servos[i] = new QGraphicsSvgItem(); + Servos[i] = new QGraphicsSvgItem(); Servos[i]->setSharedRenderer(renderer); Servos[i]->setElementId(ServoNames.at(i)); m_aircraft->SwashplateImage->scene()->addItem(Servos[i]); @@ -193,12 +194,11 @@ ConfigCcpmWidget::ConfigCcpmWidget(QWidget *parent) : m_aircraft->SwashplateImage->scene()->addItem(ServosTextCircles[i]); m_aircraft->SwashplateImage->scene()->addItem(ServosText[i]); - SwashLvlSpinBoxes[i] = new QSpinBox(m_aircraft->SwashLvlSwashplateImage); // use QGraphicsView + SwashLvlSpinBoxes[i] = new QSpinBox(m_aircraft->SwashLvlSwashplateImage); // use QGraphicsView m_aircraft->SwashLvlSwashplateImage->scene()->addWidget(SwashLvlSpinBoxes[i]); SwashLvlSpinBoxes[i]->setMaximum(10000); SwashLvlSpinBoxes[i]->setMinimum(0); SwashLvlSpinBoxes[i]->setValue(0); - } // initialize our two mixer curves @@ -225,14 +225,14 @@ ConfigCcpmWidget::ConfigCcpmWidget(QWidget *parent) : QStringList Types; Types << QString::fromUtf8("CCPM 2 Servo 90º") << QString::fromUtf8("CCPM 3 Servo 90º") - << QString::fromUtf8("CCPM 4 Servo 90º") << QString::fromUtf8("CCPM 3 Servo 120º") - << QString::fromUtf8("CCPM 3 Servo 140º") << QString::fromUtf8("FP 2 Servo 90º") - << QString::fromUtf8("Coax 2 Servo 90º") << QString::fromUtf8("Custom - User Angles") - << QString::fromUtf8("Custom - Advanced Settings"); + << QString::fromUtf8("CCPM 4 Servo 90º") << QString::fromUtf8("CCPM 3 Servo 120º") + << QString::fromUtf8("CCPM 3 Servo 140º") << QString::fromUtf8("FP 2 Servo 90º") + << QString::fromUtf8("Coax 2 Servo 90º") << QString::fromUtf8("Custom - User Angles") + << QString::fromUtf8("Custom - Advanced Settings"); m_aircraft->ccpmType->addItems(Types); m_aircraft->ccpmType->setCurrentIndex(m_aircraft->ccpmType->count() - 1); - //refreshWidgetsValues(QString("HeliCP")); + // refreshWidgetsValues(QString("HeliCP")); UpdateType(); @@ -253,7 +253,7 @@ ConfigCcpmWidget::ConfigCcpmWidget(QWidget *parent) : connect(m_aircraft->ccpmCollectivespinBox, SIGNAL(valueChanged(int)), this, SLOT(UpdateMixer())); connect(m_aircraft->ccpmType, SIGNAL(currentIndexChanged(int)), this, SLOT(UpdateType())); connect(m_aircraft->ccpmSingleServo, SIGNAL(currentIndexChanged(int)), this, SLOT(UpdateType())); - connect(m_aircraft->TabObject, SIGNAL(currentChanged ( QWidget * )), this, SLOT(UpdateType())); + connect(m_aircraft->TabObject, SIGNAL(currentChanged(QWidget *)), this, SLOT(UpdateType())); connect(m_aircraft->SwashLvlStartButton, SIGNAL(clicked()), this, SLOT(SwashLvlStartButtonPressed())); connect(m_aircraft->SwashLvlNextButton, SIGNAL(clicked()), this, SLOT(SwashLvlNextButtonPressed())); @@ -277,7 +277,8 @@ void ConfigCcpmWidget::setupUI(QString frameType) Q_UNUSED(frameType); } -void ConfigCcpmWidget::registerWidgets(ConfigTaskWidget &parent) { +void ConfigCcpmWidget::registerWidgets(ConfigTaskWidget &parent) +{ parent.addWidget(m_aircraft->ccpmType); parent.addWidget(m_aircraft->ccpmTailChannel); parent.addWidget(m_aircraft->ccpmEngineChannel); @@ -316,8 +317,8 @@ void ConfigCcpmWidget::registerWidgets(ConfigTaskWidget &parent) { void ConfigCcpmWidget::resetActuators(GUIConfigDataUnion *configData) { - configData->heli.Throttle = 0; - configData->heli.Tail = 0; + configData->heli.Throttle = 0; + configData->heli.Tail = 0; configData->heli.ServoIndexW = 0; configData->heli.ServoIndexX = 0; configData->heli.ServoIndexY = 0; @@ -358,14 +359,14 @@ void ConfigCcpmWidget::refreshWidgetsValues(QString frameType) // servo assignments setComboCurrentIndex(m_aircraft->ccpmServoWChannel, config.heli.ServoIndexW); - setComboCurrentIndex( m_aircraft->ccpmServoXChannel,config.heli.ServoIndexX); - setComboCurrentIndex( m_aircraft->ccpmServoYChannel,config.heli.ServoIndexY); - setComboCurrentIndex( m_aircraft->ccpmServoZChannel,config.heli.ServoIndexZ); + setComboCurrentIndex(m_aircraft->ccpmServoXChannel, config.heli.ServoIndexX); + setComboCurrentIndex(m_aircraft->ccpmServoYChannel, config.heli.ServoIndexY); + setComboCurrentIndex(m_aircraft->ccpmServoZChannel, config.heli.ServoIndexZ); // throttle - setComboCurrentIndex( m_aircraft->ccpmEngineChannel, config.heli.Throttle); + setComboCurrentIndex(m_aircraft->ccpmEngineChannel, config.heli.Throttle); // tail - setComboCurrentIndex( m_aircraft->ccpmTailChannel, config.heli.Tail); + setComboCurrentIndex(m_aircraft->ccpmTailChannel, config.heli.Tail); getMixer(); } @@ -386,11 +387,11 @@ void ConfigCcpmWidget::UpdateType() SetUIComponentVisibilities(); - TypeInt = m_aircraft->ccpmType->count() - m_aircraft->ccpmType->currentIndex() - 1; + TypeInt = m_aircraft->ccpmType->count() - m_aircraft->ccpmType->currentIndex() - 1; TypeText = m_aircraft->ccpmType->currentText(); SingleServoIndex = m_aircraft->ccpmSingleServo->currentIndex(); - //set visibility of user settings + // set visibility of user settings m_aircraft->ccpmAdvancedSettingsTable->setEnabled(TypeInt == 0); m_aircraft->ccpmAdvancedSettingsTable->clearFocus(); @@ -418,7 +419,7 @@ void ConfigCcpmWidget::UpdateType() m_aircraft->PitchCurve->setVisible(1); NumServosDefined = 4; - //set values for pre defined heli types + // set values for pre defined heli types if (TypeText.compare(QString::fromUtf8("CCPM 2 Servo 90º"), Qt::CaseInsensitive) == 0) { m_aircraft->ccpmAngleW->setValue(AdjustmentAngle + 0); m_aircraft->ccpmAngleX->setValue(fmod(AdjustmentAngle + 90, 360)); @@ -504,7 +505,7 @@ void ConfigCcpmWidget::UpdateType() NumServosDefined = 2; } - //Set the text of the motor boxes + // Set the text of the motor boxes if (TypeText.compare(QString::fromUtf8("Coax 2 Servo 90º"), Qt::CaseInsensitive) == 0) { m_aircraft->ccpmEngineLabel->setText("CW motor"); m_aircraft->ccpmTailLabel->setText("CCW motor"); @@ -513,7 +514,7 @@ void ConfigCcpmWidget::UpdateType() m_aircraft->ccpmTailLabel->setText("Tail rotor"); } - //set the visibility of the swashplate servo selection boxes + // set the visibility of the swashplate servo selection boxes m_aircraft->ccpmServoWLabel->setVisible(NumServosDefined >= 1); m_aircraft->ccpmServoXLabel->setVisible(NumServosDefined >= 2); m_aircraft->ccpmServoYLabel->setVisible(NumServosDefined >= 3); @@ -523,7 +524,7 @@ void ConfigCcpmWidget::UpdateType() m_aircraft->ccpmServoYChannel->setVisible(NumServosDefined >= 3); m_aircraft->ccpmServoZChannel->setVisible(NumServosDefined >= 4); - //set the visibility of the swashplate angle selection boxes + // set the visibility of the swashplate angle selection boxes m_aircraft->ccpmServoWLabel_2->setVisible(NumServosDefined >= 1); m_aircraft->ccpmServoXLabel_2->setVisible(NumServosDefined >= 2); m_aircraft->ccpmServoYLabel_2->setVisible(NumServosDefined >= 3); @@ -539,109 +540,109 @@ void ConfigCcpmWidget::UpdateType() table->setColumnWidth(i, (table->width() - table->verticalHeader()->width()) / 6); } - //update UI + // update UI ccpmSwashplateUpdate(); } void ConfigCcpmWidget::ccpmSwashplateRedraw() { - double angle[CCPM_MAX_SWASH_SERVOS],CorrectionAngle,x,y,w,h,radius,CenterX,CenterY; - int used[CCPM_MAX_SWASH_SERVOS],defined[CCPM_MAX_SWASH_SERVOS],i; + double angle[CCPM_MAX_SWASH_SERVOS], CorrectionAngle, x, y, w, h, radius, CenterX, CenterY; + int used[CCPM_MAX_SWASH_SERVOS], defined[CCPM_MAX_SWASH_SERVOS], i; QRectF bounds; QRect size; - double scale,xscale,yscale; + double scale, xscale, yscale; - size = m_aircraft->SwashplateImage->rect(); - xscale=size.width(); - yscale=size.height(); - scale=xscale; - if (yscaleSwashplateImage->resetTransform (); - m_aircraft->SwashplateImage->scale(scale,scale); - - size = m_aircraft->SwashLvlSwashplateImage->rect(); - xscale=size.width(); - yscale=size.height(); - scale=xscale; - if (yscaleSwashLvlSwashplateImage->resetTransform (); - m_aircraft->SwashLvlSwashplateImage->scale(scale,scale); - - CorrectionAngle=m_aircraft->ccpmCorrectionAngle->value(); + size = m_aircraft->SwashplateImage->rect(); + xscale = size.width(); + yscale = size.height(); + scale = xscale; + if (yscale < scale) { + scale = yscale; + } + scale /= 460.00; + m_aircraft->SwashplateImage->resetTransform(); + m_aircraft->SwashplateImage->scale(scale, scale); - CenterX=200; - CenterY=200; + size = m_aircraft->SwashLvlSwashplateImage->rect(); + xscale = size.width(); + yscale = size.height(); + scale = xscale; + if (yscale < scale) { + scale = yscale; + } + scale /= 590.00; + m_aircraft->SwashLvlSwashplateImage->resetTransform(); + m_aircraft->SwashLvlSwashplateImage->scale(scale, scale); - bounds=SwashplateImg->boundingRect(); - - SwashplateImg->setPos(CenterX-bounds.width()/2,CenterY-bounds.height()/2); + CorrectionAngle = m_aircraft->ccpmCorrectionAngle->value(); - defined[0]=(m_aircraft->ccpmServoWChannel->isEnabled()); - defined[1]=(m_aircraft->ccpmServoXChannel->isEnabled()); - defined[2]=(m_aircraft->ccpmServoYChannel->isEnabled()); - defined[3]=(m_aircraft->ccpmServoZChannel->isEnabled()); - used[0]=((m_aircraft->ccpmServoWChannel->currentIndex()>0)&&(m_aircraft->ccpmServoWChannel->isEnabled())); - used[1]=((m_aircraft->ccpmServoXChannel->currentIndex()>0)&&(m_aircraft->ccpmServoXChannel->isEnabled())); - used[2]=((m_aircraft->ccpmServoYChannel->currentIndex()>0)&&(m_aircraft->ccpmServoYChannel->isEnabled())); - used[3]=((m_aircraft->ccpmServoZChannel->currentIndex()>0)&&(m_aircraft->ccpmServoZChannel->isEnabled())); - angle[0]=(CorrectionAngle+180+m_aircraft->ccpmAngleW->value())*Pi/180.00; - angle[1]=(CorrectionAngle+180+m_aircraft->ccpmAngleX->value())*Pi/180.00; - angle[2]=(CorrectionAngle+180+m_aircraft->ccpmAngleY->value())*Pi/180.00; - angle[3]=(CorrectionAngle+180+m_aircraft->ccpmAngleZ->value())*Pi/180.00; + CenterX = 200; + CenterY = 200; + + bounds = SwashplateImg->boundingRect(); + + SwashplateImg->setPos(CenterX - bounds.width() / 2, CenterY - bounds.height() / 2); + + defined[0] = (m_aircraft->ccpmServoWChannel->isEnabled()); + defined[1] = (m_aircraft->ccpmServoXChannel->isEnabled()); + defined[2] = (m_aircraft->ccpmServoYChannel->isEnabled()); + defined[3] = (m_aircraft->ccpmServoZChannel->isEnabled()); + used[0] = ((m_aircraft->ccpmServoWChannel->currentIndex() > 0) && (m_aircraft->ccpmServoWChannel->isEnabled())); + used[1] = ((m_aircraft->ccpmServoXChannel->currentIndex() > 0) && (m_aircraft->ccpmServoXChannel->isEnabled())); + used[2] = ((m_aircraft->ccpmServoYChannel->currentIndex() > 0) && (m_aircraft->ccpmServoYChannel->isEnabled())); + used[3] = ((m_aircraft->ccpmServoZChannel->currentIndex() > 0) && (m_aircraft->ccpmServoZChannel->isEnabled())); + angle[0] = (CorrectionAngle + 180 + m_aircraft->ccpmAngleW->value()) * Pi / 180.00; + angle[1] = (CorrectionAngle + 180 + m_aircraft->ccpmAngleX->value()) * Pi / 180.00; + angle[2] = (CorrectionAngle + 180 + m_aircraft->ccpmAngleY->value()) * Pi / 180.00; + angle[3] = (CorrectionAngle + 180 + m_aircraft->ccpmAngleZ->value()) * Pi / 180.00; - for (i=0;isetPos(x, y); - Servos[i]->setVisible(used[i]!=0); + Servos[i]->setVisible(used[i] != 0); + + radius = 150; + bounds = ServosText[i]->boundingRect(); + x = CenterX - (radius * sin(angle[i])) - bounds.width() / 2; + y = CenterY + (radius * cos(angle[i])) - bounds.height() / 2; - radius=150; - bounds=ServosText[i]->boundingRect(); - x=CenterX-(radius*sin(angle[i]))-bounds.width()/2; - y=CenterY+(radius*cos(angle[i]))-bounds.height()/2; - ServosText[i]->setPos(x, y); - ServosText[i]->setVisible(used[i]!=0); - - if (bounds.width()>bounds.height()) - { + ServosText[i]->setVisible(used[i] != 0); + + if (bounds.width() > bounds.height()) { bounds.setHeight(bounds.width()); - } - else - { + } else { bounds.setWidth(bounds.height()); } - x=CenterX-(radius*sin(angle[i]))-bounds.width()/2; - y=CenterY+(radius*cos(angle[i]))-bounds.height()/2; - + x = CenterX - (radius * sin(angle[i])) - bounds.width() / 2; + y = CenterY + (radius * cos(angle[i])) - bounds.height() / 2; + ServosTextCircles[i]->setRect(bounds); ServosTextCircles[i]->setPos(x, y); - ServosTextCircles[i]->setVisible(used[i]!=0); + ServosTextCircles[i]->setVisible(used[i] != 0); - w=SwashLvlSpinBoxes[i]->width()/2; - h=SwashLvlSpinBoxes[i]->height()/2; - radius = (215.00+w+h); - x=CenterX-(radius*sin(angle[i]))-w; - y=CenterY+(radius*cos(angle[i]))-h; - SwashLvlSpinBoxes[i]->move(m_aircraft->SwashLvlSwashplateImage->mapFromScene (x, y)); - SwashLvlSpinBoxes[i]->setVisible(used[i]!=0); + w = SwashLvlSpinBoxes[i]->width() / 2; + h = SwashLvlSpinBoxes[i]->height() / 2; + radius = (215.00 + w + h); + x = CenterX - (radius * sin(angle[i])) - w; + y = CenterY + (radius * cos(angle[i])) - h; + SwashLvlSpinBoxes[i]->move(m_aircraft->SwashLvlSwashplateImage->mapFromScene(x, y)); + SwashLvlSpinBoxes[i]->setVisible(used[i] != 0); - radius=220; - x=CenterX-(radius*sin(angle[i])); - y=CenterY+(radius*cos(angle[i])); - ServoLines[i]->setLine(CenterX,CenterY,x,y); - ServoLines[i]->setVisible(defined[i]!=0); + radius = 220; + x = CenterX - (radius * sin(angle[i])); + y = CenterY + (radius * cos(angle[i])); + ServoLines[i]->setLine(CenterX, CenterY, x, y); + ServoLines[i]->setVisible(defined[i] != 0); } - //m_aircraft->SwashplateImage->centerOn (CenterX, CenterY); + // m_aircraft->SwashplateImage->centerOn (CenterX, CenterY); - //m_aircraft->SwashplateImage->fitInView(SwashplateImg, Qt::KeepAspectRatio); + // m_aircraft->SwashplateImage->fitInView(SwashplateImg, Qt::KeepAspectRatio); } void ConfigCcpmWidget::ccpmSwashplateUpdate() @@ -660,32 +661,33 @@ void ConfigCcpmWidget::UpdateMixer() float ThisAngle[6]; QString Channel; - if (throwConfigError(QString("HeliCP"))) + if (throwConfigError(QString("HeliCP"))) { return; + } GUIConfigDataUnion config = getConfigData(); - useCCPM = !(config.heli.ccpmCollectivePassthroughState || !config.heli.ccpmLinkCyclicState); + useCCPM = !(config.heli.ccpmCollectivePassthroughState || !config.heli.ccpmLinkCyclicState); useCyclic = config.heli.ccpmLinkRollState; - CollectiveConstant = (float) config.heli.SliderValue0 / 100.00; + CollectiveConstant = (float)config.heli.SliderValue0 / 100.00; - if (useCCPM) { //cyclic = 1 - collective + if (useCCPM) { // cyclic = 1 - collective PitchConstant = 1 - CollectiveConstant; - RollConstant = PitchConstant; + RollConstant = PitchConstant; } else { - PitchConstant = (float) config.heli.SliderValue1 / 100.00; + PitchConstant = (float)config.heli.SliderValue1 / 100.00; ; if (useCyclic) { RollConstant = PitchConstant; } else { - RollConstant = (float) config.heli.SliderValue2 / 100.00; + RollConstant = (float)config.heli.SliderValue2 / 100.00; ; } } - if (config.heli.SwashplateType > 0) { //not advanced settings - //get the channel data from the ui + if (config.heli.SwashplateType > 0) { // not advanced settings + // get the channel data from the ui MixerChannelData[0] = m_aircraft->ccpmEngineChannel->currentIndex(); MixerChannelData[1] = m_aircraft->ccpmTailChannel->currentIndex(); MixerChannelData[2] = m_aircraft->ccpmServoWChannel->currentIndex(); @@ -693,13 +695,13 @@ void ConfigCcpmWidget::UpdateMixer() MixerChannelData[4] = m_aircraft->ccpmServoYChannel->currentIndex(); MixerChannelData[5] = m_aircraft->ccpmServoZChannel->currentIndex(); - //get the angle data from the ui - ThisAngle[2] = m_aircraft->ccpmAngleW->value(); - ThisAngle[3] = m_aircraft->ccpmAngleX->value(); - ThisAngle[4] = m_aircraft->ccpmAngleY->value(); - ThisAngle[5] = m_aircraft->ccpmAngleZ->value(); + // get the angle data from the ui + ThisAngle[2] = m_aircraft->ccpmAngleW->value(); + ThisAngle[3] = m_aircraft->ccpmAngleX->value(); + ThisAngle[4] = m_aircraft->ccpmAngleY->value(); + ThisAngle[5] = m_aircraft->ccpmAngleZ->value(); - //get the angle data from the ui + // get the angle data from the ui ThisEnable[2] = m_aircraft->ccpmServoWChannel->isEnabled(); ThisEnable[3] = m_aircraft->ccpmServoXChannel->isEnabled(); ThisEnable[4] = m_aircraft->ccpmServoYChannel->isEnabled(); @@ -710,18 +712,18 @@ void ConfigCcpmWidget::UpdateMixer() ServosText[2]->setPlainText(QString("%1").arg(MixerChannelData[4])); ServosText[3]->setPlainText(QString("%1").arg(MixerChannelData[5])); - //go through the user data and update the mixer matrix + // go through the user data and update the mixer matrix QTableWidget *table = m_aircraft->ccpmAdvancedSettingsTable; for (int i = 0; i < 6; i++) { if ((MixerChannelData[i] > 0) && ((ThisEnable[i]) || (i < 2))) { table->item(i, 0)->setText(QString("%1").arg(MixerChannelData[i])); - //Generate the mixer vector - if (i == 0) { //main motor-engine - table->item(i, 1)->setText(QString("%1").arg(127)); //ThrottleCurve1 - table->item(i, 2)->setText(QString("%1").arg(0)); //ThrottleCurve2 - table->item(i, 3)->setText(QString("%1").arg(0)); //Roll - table->item(i, 4)->setText(QString("%1").arg(0)); //Pitch + // Generate the mixer vector + if (i == 0) { // main motor-engine + table->item(i, 1)->setText(QString("%1").arg(127)); // ThrottleCurve1 + table->item(i, 2)->setText(QString("%1").arg(0)); // ThrottleCurve2 + table->item(i, 3)->setText(QString("%1").arg(0)); // Roll + table->item(i, 4)->setText(QString("%1").arg(0)); // Pitch if (TypeText.compare(QString::fromUtf8("Coax 2 Servo 90º"), Qt::CaseInsensitive) == 0) { // Yaw @@ -730,7 +732,6 @@ void ConfigCcpmWidget::UpdateMixer() // Yaw table->item(i, 5)->setText(QString("%1").arg(0)); } - } if (i == 1) { // tailrotor --or-- counter-clockwise motor @@ -745,38 +746,35 @@ void ConfigCcpmWidget::UpdateMixer() // Yaw table->item(i, 5)->setText(QString("%1").arg(127)); } - //ThrottleCurve2 + // ThrottleCurve2 table->item(i, 2)->setText(QString("%1").arg(0)); // Roll table->item(i, 3)->setText(QString("%1").arg(0)); // Pitch table->item(i, 4)->setText(QString("%1").arg(0)); - } if (i > 1) { // Swashplate - //ThrottleCurve1 + // ThrottleCurve1 table->item(i, 1)->setText(QString("%1").arg(0)); - //ThrottleCurve2 - table->item(i, 2)->setText(QString("%1").arg((int) (127.0 * CollectiveConstant))); + // ThrottleCurve2 + table->item(i, 2)->setText(QString("%1").arg((int)(127.0 * CollectiveConstant))); table->item(i, 3)->setText( - QString("%1").arg( - (int) (127.0 * (RollConstant) - * sin((180 + config.heli.CorrectionAngle + ThisAngle[i]) * Pi / 180.00)))); //Roll + QString("%1").arg( + (int)(127.0 * (RollConstant) + * sin((180 + config.heli.CorrectionAngle + ThisAngle[i]) * Pi / 180.00)))); // Roll table->item(i, 4)->setText( - QString("%1").arg( - (int) (127.0 * (PitchConstant) - * cos((config.heli.CorrectionAngle + ThisAngle[i]) * Pi / 180.00)))); //Pitch + QString("%1").arg( + (int)(127.0 * (PitchConstant) + * cos((config.heli.CorrectionAngle + ThisAngle[i]) * Pi / 180.00)))); // Pitch // Yaw table->item(i, 5)->setText(QString("%1").arg(0)); - } } else { for (int j = 0; j < 6; j++) { table->item(i, j)->setText(QString("-")); } } - } } else { // advanced settings @@ -798,29 +796,30 @@ QString ConfigCcpmWidget::updateConfigObjects() bool useCCPM; bool useCyclic; - if (updatingFromHardware == TRUE) + if (updatingFromHardware == TRUE) { return airframeType; + } updatingFromHardware = TRUE; - //get the user options + // get the user options GUIConfigDataUnion config = getConfigData(); - //swashplate config - config.heli.SwashplateType = m_aircraft->ccpmType->count() - m_aircraft->ccpmType->currentIndex() - 1; + // swashplate config + config.heli.SwashplateType = m_aircraft->ccpmType->count() - m_aircraft->ccpmType->currentIndex() - 1; config.heli.FirstServoIndex = m_aircraft->ccpmSingleServo->currentIndex(); - //ccpm mixing options + // ccpm mixing options config.heli.ccpmCollectivePassthroughState = m_aircraft->ccpmCollectivePassthrough->isChecked(); config.heli.ccpmLinkCyclicState = m_aircraft->ccpmLinkCyclic->isChecked(); - config.heli.ccpmLinkRollState = m_aircraft->ccpmLinkRoll->isChecked(); - useCCPM = !(config.heli.ccpmCollectivePassthroughState || !config.heli.ccpmLinkCyclicState); + config.heli.ccpmLinkRollState = m_aircraft->ccpmLinkRoll->isChecked(); + useCCPM = !(config.heli.ccpmCollectivePassthroughState || !config.heli.ccpmLinkCyclicState); useCyclic = config.heli.ccpmLinkRollState; - //correction angle + // correction angle config.heli.CorrectionAngle = m_aircraft->ccpmCorrectionAngle->value(); - //update sliders + // update sliders if (useCCPM) { config.heli.SliderValue0 = m_aircraft->ccpmCollectiveSlider->value(); } else { @@ -833,15 +832,15 @@ QString ConfigCcpmWidget::updateConfigObjects() } config.heli.SliderValue2 = m_aircraft->ccpmRollScale->value(); - //servo assignments - config.heli.ServoIndexW = m_aircraft->ccpmServoWChannel->currentIndex(); - config.heli.ServoIndexX = m_aircraft->ccpmServoXChannel->currentIndex(); - config.heli.ServoIndexY = m_aircraft->ccpmServoYChannel->currentIndex(); - config.heli.ServoIndexZ = m_aircraft->ccpmServoZChannel->currentIndex(); + // servo assignments + config.heli.ServoIndexW = m_aircraft->ccpmServoWChannel->currentIndex(); + config.heli.ServoIndexX = m_aircraft->ccpmServoXChannel->currentIndex(); + config.heli.ServoIndexY = m_aircraft->ccpmServoYChannel->currentIndex(); + config.heli.ServoIndexZ = m_aircraft->ccpmServoZChannel->currentIndex(); - //throttle - config.heli.Throttle = m_aircraft->ccpmEngineChannel->currentIndex(); - //tail + // throttle + config.heli.Throttle = m_aircraft->ccpmEngineChannel->currentIndex(); + // tail config.heli.Tail = m_aircraft->ccpmTailChannel->currentIndex(); setConfigData(config); @@ -855,22 +854,21 @@ void ConfigCcpmWidget::SetUIComponentVisibilities() m_aircraft->ccpmRevoMixingBox->setVisible(0); m_aircraft->ccpmPitchMixingBox->setVisible( - !m_aircraft->ccpmCollectivePassthrough->isChecked() && m_aircraft->ccpmLinkCyclic->isChecked()); + !m_aircraft->ccpmCollectivePassthrough->isChecked() && m_aircraft->ccpmLinkCyclic->isChecked()); m_aircraft->ccpmCollectiveScalingBox->setVisible( - m_aircraft->ccpmCollectivePassthrough->isChecked() || !m_aircraft->ccpmLinkCyclic->isChecked()); + m_aircraft->ccpmCollectivePassthrough->isChecked() || !m_aircraft->ccpmLinkCyclic->isChecked()); m_aircraft->ccpmLinkCyclic->setVisible(!m_aircraft->ccpmCollectivePassthrough->isChecked()); m_aircraft->ccpmCyclicScalingBox->setVisible( - (m_aircraft->ccpmCollectivePassthrough->isChecked() || !m_aircraft->ccpmLinkCyclic->isChecked()) - && m_aircraft->ccpmLinkRoll->isChecked()); + (m_aircraft->ccpmCollectivePassthrough->isChecked() || !m_aircraft->ccpmLinkCyclic->isChecked()) + && m_aircraft->ccpmLinkRoll->isChecked()); if (!m_aircraft->ccpmCollectivePassthrough->checkState() && m_aircraft->ccpmLinkCyclic->isChecked()) { m_aircraft->ccpmPitchScalingBox->setVisible(0); m_aircraft->ccpmRollScalingBox->setVisible(0); m_aircraft->ccpmLinkRoll->setVisible(0); - } else { m_aircraft->ccpmPitchScalingBox->setVisible(!m_aircraft->ccpmLinkRoll->isChecked()); m_aircraft->ccpmRollScalingBox->setVisible(!m_aircraft->ccpmLinkRoll->isChecked()); @@ -879,16 +877,20 @@ void ConfigCcpmWidget::SetUIComponentVisibilities() } /** - Request the current value of the SystemSettings which holds the ccpm type - */ + Request the current value of the SystemSettings which holds the ccpm type + */ void ConfigCcpmWidget::getMixer() { - if (SwashLvlConfigurationInProgress)return; - if (updatingToHardware)return; + if (SwashLvlConfigurationInProgress) { + return; + } + if (updatingToHardware) { + return; + } updatingFromHardware = TRUE; - - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); QList curveValues; @@ -897,8 +899,7 @@ void ConfigCcpmWidget::getMixer() // is at least one of the curve values != 0? if (isValidThrottleCurve(&curveValues)) { m_aircraft->ThrottleCurve->setCurve(&curveValues); - } - else { + } else { m_aircraft->ThrottleCurve->ResetCurve(); } @@ -906,46 +907,48 @@ void ConfigCcpmWidget::getMixer() // is at least one of the curve values != 0? if (isValidThrottleCurve(&curveValues)) { m_aircraft->PitchCurve->setCurve(&curveValues); - } - else { + } else { m_aircraft->PitchCurve->ResetCurve(); } - updatingFromHardware=FALSE; + updatingFromHardware = FALSE; ccpmSwashplateUpdate(); } /** - Sends the config to the board (ccpm type) - */ + Sends the config to the board (ccpm type) + */ void ConfigCcpmWidget::setMixer() { - int i,j; + int i, j; - if (SwashLvlConfigurationInProgress)return; - if (updatingToHardware == TRUE) return; + if (SwashLvlConfigurationInProgress) { + return; + } + if (updatingToHardware == TRUE) { + return; + } - updatingToHardware=TRUE; + updatingToHardware = TRUE; - MixerSettings * mixerSettings = MixerSettings::GetInstance(getObjectManager()); + MixerSettings *mixerSettings = MixerSettings::GetInstance(getObjectManager()); Q_ASSERT(mixerSettings); MixerSettings::DataFields mixerSettingsData = mixerSettings->getData(); UpdateMixer(); // Set up some helper pointers - qint8 * mixers[8] = {mixerSettingsData.Mixer1Vector, + qint8 *mixers[8] = { mixerSettingsData.Mixer1Vector, mixerSettingsData.Mixer2Vector, mixerSettingsData.Mixer3Vector, mixerSettingsData.Mixer4Vector, mixerSettingsData.Mixer5Vector, mixerSettingsData.Mixer6Vector, mixerSettingsData.Mixer7Vector, - mixerSettingsData.Mixer8Vector - }; + mixerSettingsData.Mixer8Vector }; - quint8 * mixerTypes[8] = { + quint8 *mixerTypes[8] = { &mixerSettingsData.Mixer1Type, &mixerSettingsData.Mixer2Type, &mixerSettingsData.Mixer3Type, @@ -956,90 +959,89 @@ void ConfigCcpmWidget::setMixer() &mixerSettingsData.Mixer8Type }; - //reset all to Disabled - for (i=0; i<8; i++) + // reset all to Disabled + for (i = 0; i < 8; i++) { *mixerTypes[i] = 0; + } - //go through the user data and update the mixer matrix - for (i=0;i<6;i++) - { - if (MixerChannelData[i]>0) - { - //Set the mixer type. If Coax, then first two are motors. Otherwise, only first is motor - if (TypeText.compare(QString::fromUtf8("Coax 2 Servo 90º"), Qt::CaseInsensitive)==0) - { + // go through the user data and update the mixer matrix + for (i = 0; i < 6; i++) { + if (MixerChannelData[i] > 0) { + // Set the mixer type. If Coax, then first two are motors. Otherwise, only first is motor + if (TypeText.compare(QString::fromUtf8("Coax 2 Servo 90º"), Qt::CaseInsensitive) == 0) { *(mixerTypes[MixerChannelData[i] - 1]) = i > 1 ? - MixerSettings::MIXER1TYPE_SERVO : - MixerSettings::MIXER1TYPE_MOTOR; - } - else{ + MixerSettings::MIXER1TYPE_SERVO : + MixerSettings::MIXER1TYPE_MOTOR; + } else { *(mixerTypes[MixerChannelData[i] - 1]) = i > 0 ? - MixerSettings::MIXER1TYPE_SERVO : - MixerSettings::MIXER1TYPE_MOTOR; + MixerSettings::MIXER1TYPE_SERVO : + MixerSettings::MIXER1TYPE_MOTOR; } - //Configure the vector - for (j=0;j<5;j++) - mixers[MixerChannelData[i] - 1][j] = m_aircraft->ccpmAdvancedSettingsTable->item(i,j+1)->text().toInt(); + // Configure the vector + for (j = 0; j < 5; j++) { + mixers[MixerChannelData[i] - 1][j] = m_aircraft->ccpmAdvancedSettingsTable->item(i, j + 1)->text().toInt(); + } } } - //get the user data for the curve into the mixer settings + // get the user data for the curve into the mixer settings QList curve1 = m_aircraft->ThrottleCurve->getCurve(); QList curve2 = m_aircraft->PitchCurve->getCurve(); - for (i=0;i<5;i++) { + for (i = 0; i < 5; i++) { mixerSettingsData.ThrottleCurve1[i] = curve1.at(i); mixerSettingsData.ThrottleCurve2[i] = curve2.at(i); } - - //mapping of collective input to curve 2... - //MixerSettings.Curve2Source = Throttle,Roll,Pitch,Yaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5 - //check if we are using throttle or directly from a channel... - if (m_aircraft->ccpmCollectivePassthrough->isChecked()) + + // mapping of collective input to curve 2... + // MixerSettings.Curve2Source = Throttle,Roll,Pitch,Yaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5 + // check if we are using throttle or directly from a channel... + if (m_aircraft->ccpmCollectivePassthrough->isChecked()) { mixerSettingsData.Curve2Source = MixerSettings::CURVE2SOURCE_COLLECTIVE; - else + } else { mixerSettingsData.Curve2Source = MixerSettings::CURVE2SOURCE_THROTTLE; - + } + mixerSettings->setData(mixerSettingsData); mixerSettings->updated(); - updatingToHardware=FALSE; - + updatingToHardware = FALSE; } /** - Send ccpm type to the board and request saving to SD card - */ + Send ccpm type to the board and request saving to SD card + */ void ConfigCcpmWidget::saveccpmUpdate() { - if (SwashLvlConfigurationInProgress)return; + if (SwashLvlConfigurationInProgress) { + return; + } ShowDisclaimer(0); // Send update so that the latest value is saved - //sendccpmUpdate(); + // sendccpmUpdate(); setMixer(); - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(obj); saveObjectToSD(obj); } -void ConfigCcpmWidget::resizeEvent(QResizeEvent* event) +void ConfigCcpmWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); // Make the custom table columns autostretch: m_aircraft->ccpmAdvancedSettingsTable->resizeColumnsToContents(); - for (int i=0;i<6;i++) { - m_aircraft->ccpmAdvancedSettingsTable->setColumnWidth(i,(m_aircraft->ccpmAdvancedSettingsTable->width()- - m_aircraft->ccpmAdvancedSettingsTable->verticalHeader()->width())/6); + for (int i = 0; i < 6; i++) { + m_aircraft->ccpmAdvancedSettingsTable->setColumnWidth(i, (m_aircraft->ccpmAdvancedSettingsTable->width() - + m_aircraft->ccpmAdvancedSettingsTable->verticalHeader()->width()) / 6); } ccpmSwashplateRedraw(); - } void ConfigCcpmWidget::showEvent(QShowEvent *event) { Q_UNUSED(event) m_aircraft->ccpmAdvancedSettingsTable->resizeColumnsToContents(); - for (int i=0;i<6;i++) { - m_aircraft->ccpmAdvancedSettingsTable->setColumnWidth(i,(m_aircraft->ccpmAdvancedSettingsTable->width()- - m_aircraft->ccpmAdvancedSettingsTable->verticalHeader()->width())/6); + for (int i = 0; i < 6; i++) { + m_aircraft->ccpmAdvancedSettingsTable->setColumnWidth(i, (m_aircraft->ccpmAdvancedSettingsTable->width() - + m_aircraft->ccpmAdvancedSettingsTable->verticalHeader()->width()) / 6); } ccpmSwashplateRedraw(); } @@ -1049,181 +1051,181 @@ void ConfigCcpmWidget::SwashLvlStartButtonPressed() { QMessageBox msgBox; int i; - msgBox.setText("

Swashplate Leveling Routine

"); - msgBox.setInformativeText("You are about to start the Swashplate levelling routine.

This process will start by downloading the current configuration from the GCS to the OP hardware and will adjust your configuration at various stages.

The final state of your system should match the current configuration in the GCS config gadget.

Please ensure all ccpm settings in the GCS are correct before continuing.

If this process is interrupted, then the state of your OP board may not match the GCS configuration.

After completing this process, please check all settings before attempting to fly.

Please disconnect your motor to ensure it will not spin up.


Do you wish to proceed?"); - msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); - msgBox.setDefaultButton(QMessageBox::Cancel); - msgBox.setIcon(QMessageBox::Information); - int ret = msgBox.exec(); - UAVObjectField* MinField; - UAVObjectField* NeutralField; - UAVObjectField* MaxField; - UAVDataObject* obj; - ExtensionSystem::PluginManager *pm; - UAVObjectManager *objManager; + msgBox.setText("

Swashplate Leveling Routine

"); + msgBox.setInformativeText("You are about to start the Swashplate levelling routine.

This process will start by downloading the current configuration from the GCS to the OP hardware and will adjust your configuration at various stages.

The final state of your system should match the current configuration in the GCS config gadget.

Please ensure all ccpm settings in the GCS are correct before continuing.

If this process is interrupted, then the state of your OP board may not match the GCS configuration.

After completing this process, please check all settings before attempting to fly.

Please disconnect your motor to ensure it will not spin up.


Do you wish to proceed?"); + msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); + msgBox.setDefaultButton(QMessageBox::Cancel); + msgBox.setIcon(QMessageBox::Information); + int ret = msgBox.exec(); - switch (ret) { - case QMessageBox::Yes: - // Yes was clicked - SwashLvlState=0; - //remove Flight control of ActuatorCommand - enableSwashplateLevellingControl(true); + UAVObjectField *MinField; + UAVObjectField *NeutralField; + UAVObjectField *MaxField; + UAVDataObject *obj; + ExtensionSystem::PluginManager *pm; + UAVObjectManager *objManager; - m_aircraft->SwashLvlStartButton->setEnabled(false); - m_aircraft->SwashLvlNextButton->setEnabled(true); - m_aircraft->SwashLvlCancelButton->setEnabled(true); - m_aircraft->SwashLvlFinishButton->setEnabled(false); - //clear status check boxes - m_aircraft->SwashLvlStepList->item(0)->setCheckState(Qt::Unchecked); - m_aircraft->SwashLvlStepList->item(1)->setCheckState(Qt::Unchecked); - m_aircraft->SwashLvlStepList->item(2)->setCheckState(Qt::Unchecked); - m_aircraft->SwashLvlStepList->item(3)->setCheckState(Qt::Unchecked); + switch (ret) { + case QMessageBox::Yes: + // Yes was clicked + SwashLvlState = 0; + // remove Flight control of ActuatorCommand + enableSwashplateLevellingControl(true); + + m_aircraft->SwashLvlStartButton->setEnabled(false); + m_aircraft->SwashLvlNextButton->setEnabled(true); + m_aircraft->SwashLvlCancelButton->setEnabled(true); + m_aircraft->SwashLvlFinishButton->setEnabled(false); + // clear status check boxes + m_aircraft->SwashLvlStepList->item(0)->setCheckState(Qt::Unchecked); + m_aircraft->SwashLvlStepList->item(1)->setCheckState(Qt::Unchecked); + m_aircraft->SwashLvlStepList->item(2)->setCheckState(Qt::Unchecked); + m_aircraft->SwashLvlStepList->item(3)->setCheckState(Qt::Unchecked); - //download the current settings to the OP hw - //sendccpmUpdate(); - setMixer(); + // download the current settings to the OP hw + // sendccpmUpdate(); + setMixer(); - //change control mode to gcs control / disarmed - //set throttle to 0 + // change control mode to gcs control / disarmed + // set throttle to 0 - //save off the old ActuatorSettings for the swashplate servos - pm = ExtensionSystem::PluginManager::instance(); - objManager = pm->getObject(); + // save off the old ActuatorSettings for the swashplate servos + pm = ExtensionSystem::PluginManager::instance(); + objManager = pm->getObject(); - // Get the channel assignements: - obj = dynamic_cast(objManager->getObject(QString("ActuatorSettings"))); - Q_ASSERT(obj); - // obj->requestUpdate(); - MinField = obj->getField(QString("ChannelMin")); - NeutralField = obj->getField(QString("ChannelNeutral")); - MaxField = obj->getField(QString("ChannelMax")); + // Get the channel assignements: + obj = dynamic_cast(objManager->getObject(QString("ActuatorSettings"))); + Q_ASSERT(obj); + // obj->requestUpdate(); + MinField = obj->getField(QString("ChannelMin")); + NeutralField = obj->getField(QString("ChannelNeutral")); + MaxField = obj->getField(QString("ChannelMax")); - //channel assignments - oldSwashLvlConfiguration.ServoChannels[0]=m_aircraft->ccpmServoWChannel->currentIndex(); - oldSwashLvlConfiguration.ServoChannels[1]=m_aircraft->ccpmServoXChannel->currentIndex(); - oldSwashLvlConfiguration.ServoChannels[2]=m_aircraft->ccpmServoYChannel->currentIndex(); - oldSwashLvlConfiguration.ServoChannels[3]=m_aircraft->ccpmServoZChannel->currentIndex(); - //if servos are used - oldSwashLvlConfiguration.Used[0]=((m_aircraft->ccpmServoWChannel->currentIndex()>0)&&(m_aircraft->ccpmServoWChannel->isEnabled())); - oldSwashLvlConfiguration.Used[1]=((m_aircraft->ccpmServoXChannel->currentIndex()>0)&&(m_aircraft->ccpmServoXChannel->isEnabled())); - oldSwashLvlConfiguration.Used[2]=((m_aircraft->ccpmServoYChannel->currentIndex()>0)&&(m_aircraft->ccpmServoYChannel->isEnabled())); - oldSwashLvlConfiguration.Used[3]=((m_aircraft->ccpmServoZChannel->currentIndex()>0)&&(m_aircraft->ccpmServoZChannel->isEnabled())); - //min,neutral,max values for the servos - for (i=0;igetValue(oldSwashLvlConfiguration.ServoChannels[i]).toInt(); - oldSwashLvlConfiguration.Neutral[i]=NeutralField->getValue(oldSwashLvlConfiguration.ServoChannels[i]).toInt(); - oldSwashLvlConfiguration.Max[i]=MaxField->getValue(oldSwashLvlConfiguration.ServoChannels[i]).toInt(); - } + // channel assignments + oldSwashLvlConfiguration.ServoChannels[0] = m_aircraft->ccpmServoWChannel->currentIndex(); + oldSwashLvlConfiguration.ServoChannels[1] = m_aircraft->ccpmServoXChannel->currentIndex(); + oldSwashLvlConfiguration.ServoChannels[2] = m_aircraft->ccpmServoYChannel->currentIndex(); + oldSwashLvlConfiguration.ServoChannels[3] = m_aircraft->ccpmServoZChannel->currentIndex(); + // if servos are used + oldSwashLvlConfiguration.Used[0] = ((m_aircraft->ccpmServoWChannel->currentIndex() > 0) && (m_aircraft->ccpmServoWChannel->isEnabled())); + oldSwashLvlConfiguration.Used[1] = ((m_aircraft->ccpmServoXChannel->currentIndex() > 0) && (m_aircraft->ccpmServoXChannel->isEnabled())); + oldSwashLvlConfiguration.Used[2] = ((m_aircraft->ccpmServoYChannel->currentIndex() > 0) && (m_aircraft->ccpmServoYChannel->isEnabled())); + oldSwashLvlConfiguration.Used[3] = ((m_aircraft->ccpmServoZChannel->currentIndex() > 0) && (m_aircraft->ccpmServoZChannel->isEnabled())); + // min,neutral,max values for the servos + for (i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { + oldSwashLvlConfiguration.Min[i] = MinField->getValue(oldSwashLvlConfiguration.ServoChannels[i]).toInt(); + oldSwashLvlConfiguration.Neutral[i] = NeutralField->getValue(oldSwashLvlConfiguration.ServoChannels[i]).toInt(); + oldSwashLvlConfiguration.Max[i] = MaxField->getValue(oldSwashLvlConfiguration.ServoChannels[i]).toInt(); + } - //copy to new Actuator settings. - memcpy((void*)&newSwashLvlConfiguration,(void*)&oldSwashLvlConfiguration,sizeof(SwashplateServoSettingsStruct)); + // copy to new Actuator settings. + memcpy((void *)&newSwashLvlConfiguration, (void *)&oldSwashLvlConfiguration, sizeof(SwashplateServoSettingsStruct)); - //goto the first step - SwashLvlNextButtonPressed(); + // goto the first step + SwashLvlNextButtonPressed(); break; - case QMessageBox::Cancel: - // Cancel was clicked - SwashLvlState=0; - //restore Flight control of ActuatorCommand - enableSwashplateLevellingControl(false); + case QMessageBox::Cancel: + // Cancel was clicked + SwashLvlState = 0; + // restore Flight control of ActuatorCommand + enableSwashplateLevellingControl(false); - m_aircraft->SwashLvlStartButton->setEnabled(true); - m_aircraft->SwashLvlNextButton->setEnabled(false); - m_aircraft->SwashLvlCancelButton->setEnabled(false); - m_aircraft->SwashLvlFinishButton->setEnabled(false); - break; - default: - // should never be reached - break; - } + m_aircraft->SwashLvlStartButton->setEnabled(true); + m_aircraft->SwashLvlNextButton->setEnabled(false); + m_aircraft->SwashLvlCancelButton->setEnabled(false); + m_aircraft->SwashLvlFinishButton->setEnabled(false); + break; + default: + // should never be reached + break; + } } void ConfigCcpmWidget::SwashLvlNextButtonPressed() { - //ShowDisclaimer(2); + // ShowDisclaimer(2); SwashLvlState++; switch (SwashLvlState) { case 0: break; - case 1: //Neutral levelling + case 1: // Neutral levelling m_aircraft->SwashLvlStepList->setCurrentRow(0); - //set spin boxes and swashplate servos to Neutral values + // set spin boxes and swashplate servos to Neutral values setSwashplateLevel(50); - //disable position slider + // disable position slider m_aircraft->SwashLvlPositionSlider->setEnabled(false); m_aircraft->SwashLvlPositionSpinBox->setEnabled(false); - //set position slider to 50% + // set position slider to 50% m_aircraft->SwashLvlPositionSlider->setValue(50); m_aircraft->SwashLvlPositionSpinBox->setValue(50); - //connect spinbox signals to slots and ebnable them + // connect spinbox signals to slots and ebnable them for (int i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { connect(SwashLvlSpinBoxes[i], SIGNAL(valueChanged(int)), this, SLOT(SwashLvlSpinBoxChanged(int))); SwashLvlSpinBoxes[i]->setEnabled(true); } - //issue user instructions + // issue user instructions m_aircraft->SwashLvlStepInstruction->setHtml( - "

Neutral levelling

Using adjustment of:

  • servo horns
  • link lengths and
  • Neutral timing spinboxes to the right

ensure that the swashplate is in the center of desired travel range and is level."); + "

Neutral levelling

Using adjustment of:

  • servo horns
  • link lengths and
  • Neutral timing spinboxes to the right

ensure that the swashplate is in the center of desired travel range and is level."); break; - case 2: //Max levelling - //check Neutral status as complete + case 2: // Max levelling + // check Neutral status as complete m_aircraft->SwashLvlStepList->item(0)->setCheckState(Qt::Checked); m_aircraft->SwashLvlStepList->setCurrentRow(1); - //set spin boxes and swashplate servos to Max values + // set spin boxes and swashplate servos to Max values setSwashplateLevel(100); - //set position slider to 100% + // set position slider to 100% m_aircraft->SwashLvlPositionSlider->setValue(100); m_aircraft->SwashLvlPositionSpinBox->setValue(100); - //issue user instructions + // issue user instructions m_aircraft->SwashLvlStepInstruction->setText( - "

Max levelling

Using adjustment of:

  • Max timing spinboxes to the right ONLY

ensure that the swashplate is at the top of desired travel range and is level."); + "

Max levelling

Using adjustment of:

  • Max timing spinboxes to the right ONLY

ensure that the swashplate is at the top of desired travel range and is level."); break; - case 3: //Min levelling - //check Max status as complete + case 3: // Min levelling + // check Max status as complete m_aircraft->SwashLvlStepList->item(1)->setCheckState(Qt::Checked); m_aircraft->SwashLvlStepList->setCurrentRow(2); - //set spin boxes and swashplate servos to Min values + // set spin boxes and swashplate servos to Min values setSwashplateLevel(0); - //set position slider to 0% + // set position slider to 0% m_aircraft->SwashLvlPositionSlider->setValue(0); m_aircraft->SwashLvlPositionSpinBox->setValue(0); - //issue user instructions + // issue user instructions m_aircraft->SwashLvlStepInstruction->setText( - "

Min levelling

Using adjustment of:

  • Min timing spinboxes to the right ONLY

ensure that the swashplate is at the bottom of desired travel range and is level."); + "

Min levelling

Using adjustment of:

  • Min timing spinboxes to the right ONLY

ensure that the swashplate is at the bottom of desired travel range and is level."); break; - case 4: //levelling verification - //check Min status as complete + case 4: // levelling verification + // check Min status as complete m_aircraft->SwashLvlStepList->item(2)->setCheckState(Qt::Checked); m_aircraft->SwashLvlStepList->setCurrentRow(3); - //enable position slider + // enable position slider m_aircraft->SwashLvlPositionSlider->setEnabled(true); m_aircraft->SwashLvlPositionSpinBox->setEnabled(true); - //make heli respond to slider movement + // make heli respond to slider movement connect(m_aircraft->SwashLvlPositionSlider, SIGNAL(valueChanged(int)), this, SLOT(setSwashplateLevel(int))); - //disable spin boxes + // disable spin boxes for (int i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { SwashLvlSpinBoxes[i]->setEnabled(false); } - //issue user instructions + // issue user instructions m_aircraft->SwashLvlStepInstruction->setText( - "

levelling verification

Adjust the slider to the right over it's full range and observe the swashplate motion. It should remain level over the entire range of travel."); + "

levelling verification

Adjust the slider to the right over it's full range and observe the swashplate motion. It should remain level over the entire range of travel."); break; - case 5: //levelling complete - //check verify status as complete + case 5: // levelling complete + // check verify status as complete m_aircraft->SwashLvlStepList->item(3)->setCheckState(Qt::Checked); - //issue user instructions + // issue user instructions m_aircraft->SwashLvlStepInstruction->setText( - "

levelling complete

Press the Finish button to save these settings to the SD card

Press the cancel button to return to the pre-levelling settings"); - //disable position slider + "

levelling complete

Press the Finish button to save these settings to the SD card

Press the cancel button to return to the pre-levelling settings"); + // disable position slider m_aircraft->SwashLvlPositionSlider->setEnabled(false); m_aircraft->SwashLvlPositionSpinBox->setEnabled(false); - //disconnect levelling slots from signals + // disconnect levelling slots from signals disconnect(m_aircraft->SwashLvlPositionSlider, SIGNAL(valueChanged(int)), this, SLOT(setSwashplateLevel(int))); for (int i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { disconnect(SwashLvlSpinBoxes[i], SIGNAL(valueChanged(int)), this, SLOT(SwashLvlSpinBoxChanged(int))); @@ -1235,11 +1237,11 @@ void ConfigCcpmWidget::SwashLvlNextButtonPressed() m_aircraft->SwashLvlFinishButton->setEnabled(true); default: - //restore collective/cyclic setting - //restore pitch curve - //clear spin boxes - //change control mode to gcs control (OFF) / disarmed - //issue user confirmation + // restore collective/cyclic setting + // restore pitch curve + // clear spin boxes + // change control mode to gcs control (OFF) / disarmed + // issue user confirmation break; } } @@ -1262,15 +1264,15 @@ void ConfigCcpmWidget::SwashLvlCancelButtonPressed() m_aircraft->SwashLvlStepList->item(2)->setCheckState(Qt::Unchecked); m_aircraft->SwashLvlStepList->item(3)->setCheckState(Qt::Unchecked); - //restore old Actuator Settings + // restore old Actuator Settings ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast(objManager->getObject(QString("ActuatorSettings"))); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("ActuatorSettings"))); Q_ASSERT(obj); - //update settings to match our changes. - MinField = obj->getField(QString("ChannelMin")); + // update settings to match our changes. + MinField = obj->getField(QString("ChannelMin")); NeutralField = obj->getField(QString("ChannelNeutral")); - MaxField = obj->getField(QString("ChannelMax")); + MaxField = obj->getField(QString("ChannelMax")); // min,neutral,max values for the servos for (int i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { @@ -1285,7 +1287,7 @@ void ConfigCcpmWidget::SwashLvlCancelButtonPressed() enableSwashplateLevellingControl(false); m_aircraft->SwashLvlStepInstruction->setText( - "

Levelling Cancelled

Previous settings have been restored."); + "

Levelling Cancelled

Previous settings have been restored."); } @@ -1303,13 +1305,13 @@ void ConfigCcpmWidget::SwashLvlFinishButtonPressed() // save new Actuator Settings to memory and SD card ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast(objManager->getObject(QString("ActuatorSettings"))); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("ActuatorSettings"))); Q_ASSERT(obj); // update settings to match our changes. - MinField = obj->getField(QString("ChannelMin")); + MinField = obj->getField(QString("ChannelMin")); NeutralField = obj->getField(QString("ChannelNeutral")); - MaxField = obj->getField(QString("ChannelMax")); + MaxField = obj->getField(QString("ChannelMax")); // min,neutral,max values for the servos for (int i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { @@ -1325,32 +1327,34 @@ void ConfigCcpmWidget::SwashLvlFinishButtonPressed() enableSwashplateLevellingControl(false); m_aircraft->SwashLvlStepInstruction->setText( - "

Levelling Completed

New settings have been saved to the SD card"); + "

Levelling Completed

New settings have been saved to the SD card"); ShowDisclaimer(0); - //ShowDisclaimer(2); + // ShowDisclaimer(2); } int ConfigCcpmWidget::ShowDisclaimer(int messageID) { QMessageBox msgBox; + msgBox.setText("

Warning!!!

"); int ret; switch (messageID) { case 0: // Basic disclaimer msgBox.setInformativeText( - "

This code has many configurations.

Please double check all settings before attempting flight!"); + "

This code has many configurations.

Please double check all settings before attempting flight!"); msgBox.setStandardButtons(QMessageBox::Ok); msgBox.setDefaultButton(QMessageBox::Ok); msgBox.setIcon(QMessageBox::Information); ret = msgBox.exec(); return 0; + break; case 1: // Not Tested disclaimer msgBox.setInformativeText( - "

The CCPM mixer code needs more testing!

Use it at your own risk!

Do you wish to continue?"); + "

The CCPM mixer code needs more testing!

Use it at your own risk!

Do you wish to continue?"); msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); msgBox.setDefaultButton(QMessageBox::Cancel); msgBox.setIcon(QMessageBox::Warning); @@ -1358,6 +1362,7 @@ int ConfigCcpmWidget::ShowDisclaimer(int messageID) switch (ret) { case QMessageBox::Cancel: return -1; + case QMessageBox::Yes: return 0; } @@ -1365,12 +1370,13 @@ int ConfigCcpmWidget::ShowDisclaimer(int messageID) case 2: // DO NOT use msgBox.setInformativeText( - "

The CCPM swashplate levelling code is NOT complete!

DO NOT use it for flight!"); + "

The CCPM swashplate levelling code is NOT complete!

DO NOT use it for flight!"); msgBox.setStandardButtons(QMessageBox::Ok); msgBox.setDefaultButton(QMessageBox::Ok); msgBox.setIcon(QMessageBox::Critical); ret = msgBox.exec(); return 0; + break; default: // should never be reached @@ -1381,23 +1387,24 @@ int ConfigCcpmWidget::ShowDisclaimer(int messageID) /** - Toggles the channel testing mode by making the GCS take over - the ActuatorCommand objects - */ + Toggles the channel testing mode by making the GCS take over + the ActuatorCommand objects + */ void ConfigCcpmWidget::enableSwashplateLevellingControl(bool state) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast(objManager->getObject(QString("ActuatorCommand"))); - UAVObject::Metadata mdata = obj->getMetadata(); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("ActuatorCommand"))); + UAVObject::Metadata mdata = obj->getMetadata(); + if (state) { SwashLvlaccInitialData = mdata; UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); UAVObject::SetGcsTelemetryAcked(mdata, false); UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); - mdata.gcsTelemetryUpdatePeriod = 100; + mdata.gcsTelemetryUpdatePeriod = 100; SwashLvlConfigurationInProgress = 1; m_aircraft->TabObject->setTabEnabled(0, 0); m_aircraft->TabObject->setTabEnabled(2, 0); @@ -1412,15 +1419,14 @@ void ConfigCcpmWidget::enableSwashplateLevellingControl(bool state) m_aircraft->TabObject->setTabEnabled(2, 1); m_aircraft->TabObject->setTabEnabled(3, 1); m_aircraft->ccpmType->setEnabled(1); - } obj->setMetadata(mdata); } /** - Sets the swashplate level to a given value based on current settings for Max, Neutral and Min values. - level ranges -1 to +1 - */ + Sets the swashplate level to a given value based on current settings for Max, Neutral and Min values. + level ranges -1 to +1 + */ void ConfigCcpmWidget::setSwashplateLevel(int percent) { if (percent < 0) { @@ -1433,7 +1439,7 @@ void ConfigCcpmWidget::setSwashplateLevel(int percent) return; // -1; } - double level = ((double) percent / 50.00) - 1.00; + double level = ((double)percent / 50.00) - 1.00; SwashLvlServoInterlock = 1; @@ -1504,12 +1510,10 @@ void ConfigCcpmWidget::SwashLvlSpinBoxChanged(int value) actuatorCommand->setData(actuatorCommandData); actuatorCommand->updated(); - - return; } /** - This function displays text and color formatting in order to help the user understand what channels have not yet been configured. + This function displays text and color formatting in order to help the user understand what channels have not yet been configured. */ bool ConfigCcpmWidget::throwConfigError(QString airframeType) { @@ -1556,7 +1560,6 @@ bool ConfigCcpmWidget::throwConfigError(QString airframeType) error = true; } else { m_aircraft->ccpmTailLabel->setText(QTextEdit(m_aircraft->ccpmTailLabel->text()).toPlainText()); - } return error; diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.h index a0e4fa094..921a6c414 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.h @@ -52,8 +52,7 @@ typedef struct { int Min[CCPM_MAX_SWASH_SERVOS]; } SwashplateServoSettingsStruct; -class ConfigCcpmWidget: public VehicleConfig -{ +class ConfigCcpmWidget : public VehicleConfig { Q_OBJECT public: @@ -101,7 +100,10 @@ private: virtual void resetActuators(GUIConfigDataUnion *configData); int ShowDisclaimer(int messageID); - virtual void enableControls(bool enable) { Q_UNUSED(enable) }; // Not used by this widget + virtual void enableControls(bool enable) + { + Q_UNUSED(enable) + }; // Not used by this widget bool updatingFromHardware; bool updatingToHardware; @@ -122,8 +124,8 @@ private slots: void SwashLvlCancelButtonPressed(); void SwashLvlFinishButtonPressed(); - //void UpdateCCPMOptionsFromUI(); - //void UpdateCCPMUIFromOptions(); + // void UpdateCCPMOptionsFromUI(); + // void UpdateCCPMUIFromOptions(); void SetUIComponentVisibilities(); diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.cpp index 192a20ed1..248ea1889 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.cpp @@ -41,31 +41,32 @@ QStringList ConfigCustomWidget::getChannelDescriptions() { QStringList channelDesc; - for (int i = 0; i < (int) VehicleConfig::CHANNEL_NUMELEM; i++) { + + for (int i = 0; i < (int)VehicleConfig::CHANNEL_NUMELEM; i++) { channelDesc.append(QString("-")); } return channelDesc; } ConfigCustomWidget::ConfigCustomWidget(QWidget *parent) : - VehicleConfig(parent), m_aircraft(new Ui_CustomConfigWidget()) + VehicleConfig(parent), m_aircraft(new Ui_CustomConfigWidget()) { m_aircraft->setupUi(this); // Put combo boxes in line one of the custom mixer table: - UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); - UAVObjectField* field = mixer->getField(QString("Mixer1Type")); + UAVObjectField *field = mixer->getField(QString("Mixer1Type")); QStringList list = field->getOptions(); - for (int i = 0; i < (int) VehicleConfig::CHANNEL_NUMELEM; i++) { - QComboBox* qb = new QComboBox(m_aircraft->customMixerTable); + for (int i = 0; i < (int)VehicleConfig::CHANNEL_NUMELEM; i++) { + QComboBox *qb = new QComboBox(m_aircraft->customMixerTable); qb->addItems(list); m_aircraft->customMixerTable->setCellWidget(0, i, qb); } SpinBoxDelegate *sbd = new SpinBoxDelegate(); - for (int i = 1; i < (int) VehicleConfig::CHANNEL_NUMELEM; i++) { + for (int i = 1; i < (int)VehicleConfig::CHANNEL_NUMELEM; i++) { m_aircraft->customMixerTable->setItemDelegateForRow(i, sbd); } } @@ -80,7 +81,8 @@ void ConfigCustomWidget::setupUI(QString frameType) Q_ASSERT(m_aircraft); } -void ConfigCustomWidget::registerWidgets(ConfigTaskWidget &parent) { +void ConfigCustomWidget::registerWidgets(ConfigTaskWidget &parent) +{ parent.addWidget(m_aircraft->customMixerTable); parent.addWidget(m_aircraft->customThrottle1Curve->getCurveWidget()); parent.addWidget(m_aircraft->customThrottle1Curve); @@ -89,11 +91,10 @@ void ConfigCustomWidget::registerWidgets(ConfigTaskWidget &parent) { } void ConfigCustomWidget::resetActuators(GUIConfigDataUnion *configData) -{ -} +{} /** - Helper function to refresh the UI widget values + Helper function to refresh the UI widget values */ void ConfigCustomWidget::refreshWidgetsValues(QString frameType) { @@ -111,17 +112,16 @@ void ConfigCustomWidget::refreshWidgetsValues(QString frameType) if (isValidThrottleCurve(&curveValues)) { // yes, use the curve we just read from mixersettings m_aircraft->customThrottle1Curve->initCurve(&curveValues); - } - else { + } else { // no, init a straight curve m_aircraft->customThrottle1Curve->initLinearCurve(curveValues.count(), 1.0); } - if (MixerSettings *mxr = qobject_cast(mixer)) { + if (MixerSettings * mxr = qobject_cast(mixer)) { MixerSettings::DataFields mixerSettingsData = mxr->getData(); - if (mixerSettingsData.Curve2Source == MixerSettings::CURVE2SOURCE_THROTTLE) + if (mixerSettingsData.Curve2Source == MixerSettings::CURVE2SOURCE_THROTTLE) { m_aircraft->customThrottle2Curve->setMixerType(MixerCurve::MIXERCURVE_THROTTLE); - else { + } else { m_aircraft->customThrottle2Curve->setMixerType(MixerCurve::MIXERCURVE_PITCH); } } @@ -131,49 +131,49 @@ void ConfigCustomWidget::refreshWidgetsValues(QString frameType) if (isValidThrottleCurve(&curveValues)) { m_aircraft->customThrottle2Curve->initCurve(&curveValues); - } - else { + } else { m_aircraft->customThrottle2Curve->initLinearCurve(curveValues.count(), 1.0, m_aircraft->customThrottle2Curve->getMin()); } // Update the mixer table: for (int channel = 0; channel < m_aircraft->customMixerTable->columnCount(); channel++) { - UAVObjectField* field = mixer->getField(mixerTypes.at(channel)); + UAVObjectField *field = mixer->getField(mixerTypes.at(channel)); if (field) { - QComboBox* q = (QComboBox*) m_aircraft->customMixerTable->cellWidget(0, channel); + QComboBox *q = (QComboBox *)m_aircraft->customMixerTable->cellWidget(0, channel); if (q) { QString s = field->getValue().toString(); setComboCurrentIndex(q, q->findText(s)); } m_aircraft->customMixerTable->item(1, channel)->setText( - QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1))); + QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1))); m_aircraft->customMixerTable->item(2, channel)->setText( - QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2))); + QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2))); m_aircraft->customMixerTable->item(3, channel)->setText( - QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL))); + QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL))); m_aircraft->customMixerTable->item(4, channel)->setText( - QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH))); + QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH))); m_aircraft->customMixerTable->item(5, channel)->setText( - QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW))); + QString::number(getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW))); } } } /** - Helper function to + Helper function to */ QString ConfigCustomWidget::updateConfigObjectsFromWidgets() { UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + Q_ASSERT(mixer); setThrottleCurve(mixer, VehicleConfig::MIXER_THROTTLECURVE1, m_aircraft->customThrottle1Curve->getCurve()); setThrottleCurve(mixer, VehicleConfig::MIXER_THROTTLECURVE2, m_aircraft->customThrottle2Curve->getCurve()); // Update the table: - for (int channel = 0; channel < (int) VehicleConfig::CHANNEL_NUMELEM; channel++) { - QComboBox* q = (QComboBox*) m_aircraft->customMixerTable->cellWidget(0, channel); + for (int channel = 0; channel < (int)VehicleConfig::CHANNEL_NUMELEM; channel++) { + QComboBox *q = (QComboBox *)m_aircraft->customMixerTable->cellWidget(0, channel); if (q->currentText() == "Disabled") { setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_DISABLED); } else if (q->currentText() == "Motor") { @@ -200,73 +200,73 @@ QString ConfigCustomWidget::updateConfigObjectsFromWidgets() setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_ACCESSORY5); } setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, - m_aircraft->customMixerTable->item(1, channel)->text().toDouble()); + m_aircraft->customMixerTable->item(1, channel)->text().toDouble()); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2, - m_aircraft->customMixerTable->item(2, channel)->text().toDouble()); + m_aircraft->customMixerTable->item(2, channel)->text().toDouble()); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, - m_aircraft->customMixerTable->item(3, channel)->text().toDouble()); + m_aircraft->customMixerTable->item(3, channel)->text().toDouble()); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, - m_aircraft->customMixerTable->item(4, channel)->text().toDouble()); + m_aircraft->customMixerTable->item(4, channel)->text().toDouble()); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, - m_aircraft->customMixerTable->item(5, channel)->text().toDouble()); + m_aircraft->customMixerTable->item(5, channel)->text().toDouble()); } return "Custom"; } /** - This function displays text and color formatting in order to help the user understand what channels have not yet been configured. + This function displays text and color formatting in order to help the user understand what channels have not yet been configured. */ bool ConfigCustomWidget::throwConfigError(int numMotors) -{ +{ return false; } /** - WHAT DOES THIS DO??? + WHAT DOES THIS DO??? */ void ConfigCustomWidget::showEvent(QShowEvent *event) { Q_UNUSED(event); // Make the custom table columns autostretch: m_aircraft->customMixerTable->resizeColumnsToContents(); - int channelCount = (int) VehicleConfig::CHANNEL_NUMELEM; + int channelCount = (int)VehicleConfig::CHANNEL_NUMELEM; for (int i = 0; i < channelCount; i++) { m_aircraft->customMixerTable->setColumnWidth(i, - (m_aircraft->customMixerTable->width() - m_aircraft->customMixerTable->verticalHeader()->width()) - / channelCount); + (m_aircraft->customMixerTable->width() - m_aircraft->customMixerTable->verticalHeader()->width()) + / channelCount); } } /** - Resize the GUI contents when the user changes the window size + Resize the GUI contents when the user changes the window size */ void ConfigCustomWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); // Make the custom table columns autostretch: m_aircraft->customMixerTable->resizeColumnsToContents(); - int channelCount = (int) VehicleConfig::CHANNEL_NUMELEM; + int channelCount = (int)VehicleConfig::CHANNEL_NUMELEM; for (int i = 0; i < channelCount; i++) { m_aircraft->customMixerTable->setColumnWidth(i, - (m_aircraft->customMixerTable->width() - m_aircraft->customMixerTable->verticalHeader()->width()) - / channelCount); + (m_aircraft->customMixerTable->width() - m_aircraft->customMixerTable->verticalHeader()->width()) + / channelCount); } } /** - Helper delegate for the custom mixer editor table. - Taken straight from Qt examples, thanks! -*/ + Helper delegate for the custom mixer editor table. + Taken straight from Qt examples, thanks! + */ SpinBoxDelegate::SpinBoxDelegate(QObject *parent) : - QItemDelegate(parent) -{ -} + QItemDelegate(parent) +{} QWidget *SpinBoxDelegate::createEditor(QWidget *parent, const QStyleOptionViewItem &option, - const QModelIndex &index) const + const QModelIndex &index) const { QSpinBox *editor = new QSpinBox(parent); + editor->setMinimum(-127); editor->setMaximum(127); @@ -277,13 +277,15 @@ void SpinBoxDelegate::setEditorData(QWidget *editor, const QModelIndex &index) c { int value = index.model()->data(index, Qt::EditRole).toInt(); - QSpinBox *spinBox = static_cast(editor); + QSpinBox *spinBox = static_cast(editor); + spinBox->setValue(value); } void SpinBoxDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, const QModelIndex &index) const { - QSpinBox *spinBox = static_cast(editor); + QSpinBox *spinBox = static_cast(editor); + spinBox->interpretText(); int value = spinBox->value(); @@ -291,7 +293,7 @@ void SpinBoxDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, c } void SpinBoxDelegate::updateEditorGeometry(QWidget *editor, const QStyleOptionViewItem &option, - const QModelIndex &index) const + const QModelIndex &index) const { editor->setGeometry(option.rect); } diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.h index 1bf7bbf75..3364522d2 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.h @@ -41,8 +41,7 @@ class Ui_Widget; -class ConfigCustomWidget: public VehicleConfig -{ +class ConfigCustomWidget : public VehicleConfig { Q_OBJECT public: @@ -67,11 +66,9 @@ private: private slots: virtual void setupUI(QString airframeType); virtual bool throwConfigError(int numMotors); - }; -class SpinBoxDelegate : public QItemDelegate -{ +class SpinBoxDelegate : public QItemDelegate { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index da2575637..1e2068f93 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -44,7 +44,8 @@ QStringList ConfigFixedWingWidget::getChannelDescriptions() { // init a channel_numelem list of channel desc defaults QStringList channelDesc; - for (int i = 0; i < (int) ConfigFixedWingWidget::CHANNEL_NUMELEM; i++) { + + for (int i = 0; i < (int)ConfigFixedWingWidget::CHANNEL_NUMELEM; i++) { channelDesc.append(QString("-")); } @@ -76,7 +77,7 @@ QStringList ConfigFixedWingWidget::getChannelDescriptions() } ConfigFixedWingWidget::ConfigFixedWingWidget(QWidget *parent) : - VehicleConfig(parent), m_aircraft(new Ui_FixedWingConfigWidget()) + VehicleConfig(parent), m_aircraft(new Ui_FixedWingConfigWidget()) { m_aircraft->setupUi(this); @@ -98,13 +99,13 @@ ConfigFixedWingWidget::~ConfigFixedWingWidget() } /** - Virtual function to setup the UI + Virtual function to setup the UI */ void ConfigFixedWingWidget::setupUI(QString frameType) { Q_ASSERT(m_aircraft); - if (frameType == "FixedWing" || frameType == "Elevator aileron rudder") { + if (frameType == "FixedWing" || frameType == "Elevator aileron rudder") { setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Elevator aileron rudder")); m_aircraft->fwRudder1ChannelBox->setEnabled(true); m_aircraft->fwRudder2ChannelBox->setEnabled(true); @@ -112,7 +113,7 @@ void ConfigFixedWingWidget::setupUI(QString frameType) m_aircraft->fwElevator2ChannelBox->setEnabled(true); m_aircraft->fwAileron1ChannelBox->setEnabled(true); m_aircraft->fwAileron2ChannelBox->setEnabled(true); - + m_aircraft->fwAileron1Label->setText("Aileron 1"); m_aircraft->fwAileron2Label->setText("Aileron 2"); m_aircraft->fwElevator1Label->setText("Elevator 1"); @@ -120,7 +121,6 @@ void ConfigFixedWingWidget::setupUI(QString frameType) m_aircraft->elevonSlider1->setEnabled(false); m_aircraft->elevonSlider2->setEnabled(false); - } else if (frameType == "FixedWingElevon" || frameType == "Elevon") { setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Elevon")); m_aircraft->fwAileron1Label->setText("Elevon 1"); @@ -137,8 +137,7 @@ void ConfigFixedWingWidget::setupUI(QString frameType) m_aircraft->elevonSlider1->setEnabled(true); m_aircraft->elevonSlider2->setEnabled(true); - - } else if (frameType == "FixedWingVtail" || frameType == "Vtail") { + } else if (frameType == "FixedWingVtail" || frameType == "Vtail") { setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Vtail")); m_aircraft->fwRudder1ChannelBox->setEnabled(false); m_aircraft->fwRudder2ChannelBox->setEnabled(false); @@ -159,7 +158,8 @@ void ConfigFixedWingWidget::setupUI(QString frameType) } } -void ConfigFixedWingWidget::registerWidgets(ConfigTaskWidget &parent) { +void ConfigFixedWingWidget::registerWidgets(ConfigTaskWidget &parent) +{ parent.addWidget(m_aircraft->fixedWingThrottle->getCurveWidget()); parent.addWidget(m_aircraft->fixedWingThrottle); parent.addWidget(m_aircraft->fixedWingType); @@ -177,17 +177,17 @@ void ConfigFixedWingWidget::registerWidgets(ConfigTaskWidget &parent) { void ConfigFixedWingWidget::resetActuators(GUIConfigDataUnion *configData) { - configData->fixedwing.FixedWingPitch1 = 0; - configData->fixedwing.FixedWingPitch2 = 0; - configData->fixedwing.FixedWingRoll1 = 0; - configData->fixedwing.FixedWingRoll2 = 0; - configData->fixedwing.FixedWingYaw1 = 0; - configData->fixedwing.FixedWingYaw2 = 0; + configData->fixedwing.FixedWingPitch1 = 0; + configData->fixedwing.FixedWingPitch2 = 0; + configData->fixedwing.FixedWingRoll1 = 0; + configData->fixedwing.FixedWingRoll2 = 0; + configData->fixedwing.FixedWingYaw1 = 0; + configData->fixedwing.FixedWingYaw2 = 0; configData->fixedwing.FixedWingThrottle = 0; } /** - Virtual function to refresh the UI widget values + Virtual function to refresh the UI widget values */ void ConfigFixedWingWidget::refreshWidgetsValues(QString frameType) { @@ -205,13 +205,12 @@ void ConfigFixedWingWidget::refreshWidgetsValues(QString frameType) if (isValidThrottleCurve(&curveValues)) { // yes, use the curve we just read from mixersettings m_aircraft->fixedWingThrottle->initCurve(&curveValues); - } - else { + } else { // no, init a straight curve m_aircraft->fixedWingThrottle->initLinearCurve(curveValues.count(), 1.0); } - GUIConfigDataUnion config = getConfigData(); + GUIConfigDataUnion config = getConfigData(); fixedGUISettingsStruct fixed = config.fixedwing; // Then retrieve how channels are setup @@ -230,61 +229,61 @@ void ConfigFixedWingWidget::refreshWidgetsValues(QString frameType) if (channel > -1) { // If for some reason the actuators were incoherent, we might fail here, hence the check. m_aircraft->elevonSlider1->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL) * 100); + getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL) * 100); m_aircraft->elevonSlider2->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); + getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); } - } - else if (frameType == "FixedWingVtail") { + } else if (frameType == "FixedWingVtail") { int channel = m_aircraft->fwElevator1ChannelBox->currentIndex() - 1; if (channel > -1) { // If for some reason the actuators were incoherent, we might fail here, hence the check. m_aircraft->elevonSlider1->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW) * 100); + getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW) * 100); m_aircraft->elevonSlider2->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); + getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); } } } /** - Virtual function to update the UI widget objects + Virtual function to update the UI widget objects */ QString ConfigFixedWingWidget::updateConfigObjectsFromWidgets() { - QString airframeType = "FixedWing"; + QString airframeType = "FixedWing"; + + // Save the curve (common to all Fixed wing frames) + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); - // Save the curve (common to all Fixed wing frames) - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); - // Remove Feed Forward, it is pointless on a plane: + // Remove Feed Forward, it is pointless on a plane: setMixerValue(mixer, "FeedForward", 0.0); // Set the throttle curve - setThrottleCurve(mixer,VehicleConfig::MIXER_THROTTLECURVE1, m_aircraft->fixedWingThrottle->getCurve()); + setThrottleCurve(mixer, VehicleConfig::MIXER_THROTTLECURVE1, m_aircraft->fixedWingThrottle->getCurve()); - // All airframe types must start with "FixedWing" - if (m_aircraft->fixedWingType->currentText() == "Elevator aileron rudder" ) { - airframeType = "FixedWing"; - setupFrameFixedWing( airframeType ); - } else if (m_aircraft->fixedWingType->currentText() == "Elevon") { - airframeType = "FixedWingElevon"; - setupFrameElevon( airframeType ); - } else { // "Vtail" - airframeType = "FixedWingVtail"; - setupFrameVtail( airframeType ); - } + // All airframe types must start with "FixedWing" + if (m_aircraft->fixedWingType->currentText() == "Elevator aileron rudder") { + airframeType = "FixedWing"; + setupFrameFixedWing(airframeType); + } else if (m_aircraft->fixedWingType->currentText() == "Elevon") { + airframeType = "FixedWingElevon"; + setupFrameElevon(airframeType); + } else { // "Vtail" + airframeType = "FixedWingVtail"; + setupFrameVtail(airframeType); + } - return airframeType; + return airframeType; } /** - Setup Elevator/Aileron/Rudder airframe. - - If both Aileron channels are set to 'None' (EasyStar), do Pitch/Rudder mixing - - Returns False if impossible to create the mixer. + Setup Elevator/Aileron/Rudder airframe. + + If both Aileron channels are set to 'None' (EasyStar), do Pitch/Rudder mixing + + Returns False if impossible to create the mixer. */ bool ConfigFixedWingWidget::setupFrameFixedWing(QString airframeType) { @@ -298,16 +297,16 @@ bool ConfigFixedWingWidget::setupFrameFixedWing(QString airframeType) GUIConfigDataUnion config = getConfigData(); resetActuators(&config); - config.fixedwing.FixedWingPitch1 = m_aircraft->fwElevator1ChannelBox->currentIndex(); - config.fixedwing.FixedWingPitch2 = m_aircraft->fwElevator2ChannelBox->currentIndex(); - config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); - config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); - config.fixedwing.FixedWingYaw1 = m_aircraft->fwRudder1ChannelBox->currentIndex(); + config.fixedwing.FixedWingPitch1 = m_aircraft->fwElevator1ChannelBox->currentIndex(); + config.fixedwing.FixedWingPitch2 = m_aircraft->fwElevator2ChannelBox->currentIndex(); + config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); + config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); + config.fixedwing.FixedWingYaw1 = m_aircraft->fwRudder1ChannelBox->currentIndex(); config.fixedwing.FixedWingThrottle = m_aircraft->fwEngineChannelBox->currentIndex(); setConfigData(config); - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); @@ -355,7 +354,7 @@ bool ConfigFixedWingWidget::setupFrameFixedWing(QString airframeType) } /** - Setup Elevon + Setup Elevon */ bool ConfigFixedWingWidget::setupFrameElevon(QString airframeType) { @@ -368,15 +367,15 @@ bool ConfigFixedWingWidget::setupFrameElevon(QString airframeType) GUIConfigDataUnion config = getConfigData(); resetActuators(&config); - config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); - config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); - config.fixedwing.FixedWingYaw1 = m_aircraft->fwRudder1ChannelBox->currentIndex(); - config.fixedwing.FixedWingYaw2 = m_aircraft->fwRudder2ChannelBox->currentIndex(); + config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); + config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); + config.fixedwing.FixedWingYaw1 = m_aircraft->fwRudder1ChannelBox->currentIndex(); + config.fixedwing.FixedWingYaw2 = m_aircraft->fwRudder2ChannelBox->currentIndex(); config.fixedwing.FixedWingThrottle = m_aircraft->fwEngineChannelBox->currentIndex(); setConfigData(config); - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); @@ -407,16 +406,16 @@ bool ConfigFixedWingWidget::setupFrameElevon(QString airframeType) channel = m_aircraft->fwAileron1ChannelBox->currentIndex() - 1; if (channel > -1) { setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - value = (double) (m_aircraft->elevonSlider2->value() * 1.27); + value = (double)(m_aircraft->elevonSlider2->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value); - value = (double) (m_aircraft->elevonSlider1->value() * 1.27); + value = (double)(m_aircraft->elevonSlider1->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, value); channel = m_aircraft->fwAileron2ChannelBox->currentIndex() - 1; setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - value = (double) (m_aircraft->elevonSlider2->value() * 1.27); + value = (double)(m_aircraft->elevonSlider2->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value); - value = (double) (m_aircraft->elevonSlider1->value() * 1.27); + value = (double)(m_aircraft->elevonSlider1->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, -value); } @@ -425,28 +424,28 @@ bool ConfigFixedWingWidget::setupFrameElevon(QString airframeType) } /** - Setup VTail + Setup VTail */ bool ConfigFixedWingWidget::setupFrameVtail(QString airframeType) { // Check coherence: - // Show any config errors in GUI + // Show any config errors in GUI if (throwConfigError(airframeType)) { return false; } - + GUIConfigDataUnion config = getConfigData(); resetActuators(&config); - config.fixedwing.FixedWingPitch1 = m_aircraft->fwElevator1ChannelBox->currentIndex(); - config.fixedwing.FixedWingPitch2 = m_aircraft->fwElevator2ChannelBox->currentIndex(); - config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); - config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); + config.fixedwing.FixedWingPitch1 = m_aircraft->fwElevator1ChannelBox->currentIndex(); + config.fixedwing.FixedWingPitch2 = m_aircraft->fwElevator2ChannelBox->currentIndex(); + config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); + config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); config.fixedwing.FixedWingThrottle = m_aircraft->fwEngineChannelBox->currentIndex(); setConfigData(config); - - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); @@ -458,44 +457,44 @@ bool ConfigFixedWingWidget::setupFrameVtail(QString airframeType) // 1. Assign the servo/motor/none for each channel // motor - int channel = m_aircraft->fwEngineChannelBox->currentIndex()-1; - setMixerType(mixer,channel,VehicleConfig::MIXERTYPE_MOTOR); - setMixerVectorValue(mixer,channel,VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127); + int channel = m_aircraft->fwEngineChannelBox->currentIndex() - 1; + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_MOTOR); + setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127); // rudders - channel = m_aircraft->fwRudder1ChannelBox->currentIndex()-1; - setMixerType(mixer,channel,VehicleConfig::MIXERTYPE_SERVO); + channel = m_aircraft->fwRudder1ChannelBox->currentIndex() - 1; + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, 127); - channel = m_aircraft->fwRudder2ChannelBox->currentIndex()-1; - setMixerType(mixer,channel,VehicleConfig::MIXERTYPE_SERVO); + channel = m_aircraft->fwRudder2ChannelBox->currentIndex() - 1; + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -127); // ailerons - channel = m_aircraft->fwAileron1ChannelBox->currentIndex()-1; + channel = m_aircraft->fwAileron1ChannelBox->currentIndex() - 1; if (channel > -1) { - setMixerType(mixer,channel,VehicleConfig::MIXERTYPE_SERVO); + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, 127); - channel = m_aircraft->fwAileron2ChannelBox->currentIndex()-1; - setMixerType(mixer,channel,VehicleConfig::MIXERTYPE_SERVO); + channel = m_aircraft->fwAileron2ChannelBox->currentIndex() - 1; + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, -127); } // vtail - channel = m_aircraft->fwElevator1ChannelBox->currentIndex()-1; + channel = m_aircraft->fwElevator1ChannelBox->currentIndex() - 1; if (channel > -1) { - setMixerType(mixer,channel,VehicleConfig::MIXERTYPE_SERVO); - double value = (double)(m_aircraft->elevonSlider2->value()*1.27); + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); + double value = (double)(m_aircraft->elevonSlider2->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value); - value = (double)(m_aircraft->elevonSlider1->value()*1.27); + value = (double)(m_aircraft->elevonSlider1->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, value); - channel = m_aircraft->fwElevator2ChannelBox->currentIndex()-1; - setMixerType(mixer,channel,VehicleConfig::MIXERTYPE_SERVO); - value = (double)(m_aircraft->elevonSlider2->value()*1.27); + channel = m_aircraft->fwElevator2ChannelBox->currentIndex() - 1; + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); + value = (double)(m_aircraft->elevonSlider2->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value); - value = (double)(m_aircraft->elevonSlider1->value()*1.27); + value = (double)(m_aircraft->elevonSlider1->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -value); } @@ -506,13 +505,14 @@ bool ConfigFixedWingWidget::setupFrameVtail(QString airframeType) void ConfigFixedWingWidget::enableControls(bool enable) { ConfigTaskWidget::enableControls(enable); - if(enable) { + + if (enable) { setupUI(m_aircraft->fixedWingType->currentText()); } } /** - This function displays text and color formatting in order to help the user understand what channels have not yet been configured. + This function displays text and color formatting in order to help the user understand what channels have not yet been configured. */ bool ConfigFixedWingWidget::throwConfigError(QString airframeType) { @@ -520,79 +520,78 @@ bool ConfigFixedWingWidget::throwConfigError(QString airframeType) bool error = false; // Create a red block. All combo boxes are the same size, so any one should do as a model - int size = m_aircraft->fwEngineChannelBox->style()->pixelMetric(QStyle::PM_SmallIconSize); + int size = m_aircraft->fwEngineChannelBox->style()->pixelMetric(QStyle::PM_SmallIconSize); QPixmap pixmap(size, size); + pixmap.fill(QColor("red")); if (airframeType == "FixedWing") { if (m_aircraft->fwEngineChannelBox->currentText() == "None") { - m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if (m_aircraft->fwElevator1ChannelBox->currentText() == "None") { - m_aircraft->fwElevator1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwElevator1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwElevator1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwElevator1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if ((m_aircraft->fwAileron1ChannelBox->currentText() == "None") - && (m_aircraft->fwRudder1ChannelBox->currentText() == "None")) { + && (m_aircraft->fwRudder1ChannelBox->currentText() == "None")) { pixmap.fill(QColor("green")); - m_aircraft->fwAileron1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes - m_aircraft->fwRudder1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwAileron1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + m_aircraft->fwRudder1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwAileron1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - m_aircraft->fwRudder1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwAileron1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->fwRudder1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } } else if (airframeType == "FixedWingElevon") { if (m_aircraft->fwEngineChannelBox->currentText() == "None") { - m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if (m_aircraft->fwAileron1ChannelBox->currentText() == "None") { - m_aircraft->fwAileron1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwAileron1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwAileron1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwAileron1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if (m_aircraft->fwAileron2ChannelBox->currentText() == "None") { - m_aircraft->fwAileron2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwAileron2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwAileron2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwAileron2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } - } else if (airframeType == "FixedWingVtail") { if (m_aircraft->fwEngineChannelBox->currentText() == "None") { - m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if (m_aircraft->fwElevator1ChannelBox->currentText() == "None") { - m_aircraft->fwElevator1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwElevator1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwElevator1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwElevator1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if (m_aircraft->fwElevator2ChannelBox->currentText() == "None") { - m_aircraft->fwElevator2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + m_aircraft->fwElevator2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwElevator2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->fwElevator2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } - } if (error) { diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h index 3a0d45883..6716334b5 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h @@ -41,8 +41,7 @@ class Ui_Widget; -class ConfigFixedWingWidget: public VehicleConfig -{ +class ConfigFixedWingWidget : public VehicleConfig { Q_OBJECT public: @@ -70,7 +69,6 @@ protected: private slots: virtual void setupUI(QString airframeType); virtual bool throwConfigError(QString airframeType); - }; #endif // CONFIGFIXEDWINGWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp index d37b58695..8941c7e35 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp @@ -45,7 +45,8 @@ QStringList ConfigGroundVehicleWidget::getChannelDescriptions() { // init a channel_numelem list of channel desc defaults QStringList channelDesc; - for (int i = 0; i < (int) ConfigGroundVehicleWidget::CHANNEL_NUMELEM; i++) { + + for (int i = 0; i < (int)ConfigGroundVehicleWidget::CHANNEL_NUMELEM; i++) { channelDesc.append(QString("-")); } @@ -68,7 +69,7 @@ QStringList ConfigGroundVehicleWidget::getChannelDescriptions() } ConfigGroundVehicleWidget::ConfigGroundVehicleWidget(QWidget *parent) : - VehicleConfig(parent), m_aircraft(new Ui_GroundConfigWidget()) + VehicleConfig(parent), m_aircraft(new Ui_GroundConfigWidget()) { m_aircraft->setupUi(this); @@ -90,7 +91,7 @@ ConfigGroundVehicleWidget::~ConfigGroundVehicleWidget() } /** - Virtual function to setup the UI + Virtual function to setup the UI */ void ConfigGroundVehicleWidget::setupUI(QString frameType) { @@ -106,7 +107,7 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType) if (frameType == "GroundVehicleDifferential" || frameType == "Differential (tank)") { // Tank setComboCurrentIndex(m_aircraft->groundVehicleType, - m_aircraft->groundVehicleType->findText("Differential (tank)")); + m_aircraft->groundVehicleType->findText("Differential (tank)")); m_aircraft->gvMotor1ChannelBox->setEnabled(true); m_aircraft->gvMotor2ChannelBox->setEnabled(true); @@ -123,7 +124,6 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType) m_aircraft->gvThrottleCurve1GroupBox->setTitle("Left throttle curve"); m_aircraft->gvThrottleCurve2GroupBox->setTitle("Right throttle curve"); - } else if (frameType == "GroundVehicleMotorcycle" || frameType == "Motorcycle") { // Motorcycle setComboCurrentIndex(m_aircraft->groundVehicleType, m_aircraft->groundVehicleType->findText("Motorcycle")); @@ -161,12 +161,14 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType) void ConfigGroundVehicleWidget::enableControls(bool enable) { ConfigTaskWidget::enableControls(enable); - if(enable) { + + if (enable) { setupUI(m_aircraft->groundVehicleType->currentText()); } } -void ConfigGroundVehicleWidget::registerWidgets(ConfigTaskWidget &parent) { +void ConfigGroundVehicleWidget::registerWidgets(ConfigTaskWidget &parent) +{ parent.addWidget(m_aircraft->groundVehicleThrottle1->getCurveWidget()); parent.addWidget(m_aircraft->groundVehicleThrottle1); parent.addWidget(m_aircraft->groundVehicleThrottle2->getCurveWidget()); @@ -190,7 +192,7 @@ void ConfigGroundVehicleWidget::resetActuators(GUIConfigDataUnion *configData) } /** - Virtual function to refresh the UI widget values + Virtual function to refresh the UI widget values */ void ConfigGroundVehicleWidget::refreshWidgetsValues(QString frameType) { @@ -229,45 +231,45 @@ void ConfigGroundVehicleWidget::refreshWidgetsValues(QString frameType) setComboCurrentIndex(m_aircraft->gvSteering2ChannelBox, config.ground.GroundVehicleSteering2); if (frameType == "GroundVehicleDifferential") { - //CURRENTLY BROKEN UNTIL WE DECIDE HOW DIFFERENTIAL SHOULD BEHAVE + // CURRENTLY BROKEN UNTIL WE DECIDE HOW DIFFERENTIAL SHOULD BEHAVE // If the vehicle type is "differential", restore the slider setting // Find the channel number for Motor1 - //obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); - //Q_ASSERT(obj); + // obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + // Q_ASSERT(obj); int channel = m_aircraft->gvMotor1ChannelBox->currentIndex() - 1; if (channel > -1) { // If for some reason the actuators were incoherent, we might fail here, hence the check. m_aircraft->differentialSteeringSlider1->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL) * 100); + getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL) * 100); m_aircraft->differentialSteeringSlider2->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); + getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); } } else if (frameType == "GroundVehicleMotorcycle") { // CURRENTLY BROKEN UNTIL WE DECIDE HOW MOTORCYCLE SHOULD BEHAVE -// obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); -// Q_ASSERT(obj); -// int chMixerNumber = m_aircraft->gvMotor1ChannelBox->currentIndex()-1; -// if (chMixerNumber >=0) { -// field = obj->getField(mixerVectors.at(chMixerNumber)); -// int ti = field->getElementNames().indexOf("Yaw"); -// m_aircraft->differentialSteeringSlider1->setValue(field->getDouble(ti)*100); +// obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); +// Q_ASSERT(obj); +// int chMixerNumber = m_aircraft->gvMotor1ChannelBox->currentIndex()-1; +// if (chMixerNumber >=0) { +// field = obj->getField(mixerVectors.at(chMixerNumber)); +// int ti = field->getElementNames().indexOf("Yaw"); +// m_aircraft->differentialSteeringSlider1->setValue(field->getDouble(ti)*100); // -// ti = field->getElementNames().indexOf("Pitch"); -// m_aircraft->differentialSteeringSlider2->setValue(field->getDouble(ti)*100); -// } +// ti = field->getElementNames().indexOf("Pitch"); +// m_aircraft->differentialSteeringSlider2->setValue(field->getDouble(ti)*100); +// } } } /** - Virtual function to update the UI widget objects + Virtual function to update the UI widget objects */ QString ConfigGroundVehicleWidget::updateConfigObjectsFromWidgets() { QString airframeType = "GroundVehicleCar"; // Save the curve (common to all ground vehicle frames) - UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); // Remove Feed Forward, it is pointless on a ground vehicle: setMixerValue(mixer, "FeedForward", 0.0); @@ -292,9 +294,9 @@ QString ConfigGroundVehicleWidget::updateConfigObjectsFromWidgets() } /** - Setup balancing ground vehicle. - - Returns False if impossible to create the mixer. + Setup balancing ground vehicle. + + Returns False if impossible to create the mixer. */ bool ConfigGroundVehicleWidget::setupGroundVehicleMotorcycle(QString airframeType) { @@ -313,7 +315,7 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleMotorcycle(QString airframeTyp setConfigData(config); - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); @@ -341,9 +343,9 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleMotorcycle(QString airframeTyp } /** - Setup differentially steered ground vehicle. - - Returns False if impossible to create the mixer. + Setup differentially steered ground vehicle. + + Returns False if impossible to create the mixer. */ bool ConfigGroundVehicleWidget::setupGroundVehicleDifferential(QString airframeType) { @@ -363,7 +365,7 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleDifferential(QString airframeT setConfigData(config); - UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); @@ -386,9 +388,9 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleDifferential(QString airframeT } /** - Setup steerable ground vehicle. - - Returns False if impossible to create the mixer. + Setup steerable ground vehicle. + + Returns False if impossible to create the mixer. */ bool ConfigGroundVehicleWidget::setupGroundVehicleCar(QString airframeType) { @@ -409,7 +411,7 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleCar(QString airframeType) setConfigData(config); - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); @@ -436,7 +438,7 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleCar(QString airframeType) } /** - This function displays text and color formatting in order to help the user understand what channels have not yet been configured. + This function displays text and color formatting in order to help the user understand what channels have not yet been configured. */ bool ConfigGroundVehicleWidget::throwConfigError(QString airframeType) { @@ -444,83 +446,83 @@ bool ConfigGroundVehicleWidget::throwConfigError(QString airframeType) bool error = false; // Create a red block. All combo boxes are the same size, so any one should do as a model - int size = m_aircraft->gvEngineChannelBox->style()->pixelMetric(QStyle::PM_SmallIconSize); + int size = m_aircraft->gvEngineChannelBox->style()->pixelMetric(QStyle::PM_SmallIconSize); QPixmap pixmap(size, size); + pixmap.fill(QColor("red")); - if (airframeType == "GroundVehicleCar") { //Car + if (airframeType == "GroundVehicleCar") { // Car if (m_aircraft->gvMotor1ChannelBox->currentText() == "None" - && m_aircraft->gvMotor2ChannelBox->currentText() == "None") { + && m_aircraft->gvMotor2ChannelBox->currentText() == "None") { pixmap.fill(QColor("green")); - m_aircraft->gvMotor1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes - m_aircraft->gvMotor2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes - //m_aircraft->gvMotor1Label->setText("" + m_aircraft->gvMotor1Label->text() + ""); - //m_aircraft->gvMotor2Label->setText("" + m_aircraft->gvMotor2Label->text() + ""); + m_aircraft->gvMotor1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + m_aircraft->gvMotor2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + // m_aircraft->gvMotor1Label->setText("" + m_aircraft->gvMotor1Label->text() + ""); + // m_aircraft->gvMotor2Label->setText("" + m_aircraft->gvMotor2Label->text() + ""); error = true; - } else { - m_aircraft->gvMotor1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - m_aircraft->gvMotor2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - //QTextEdit* htmlText=new QTextEdit(m_aircraft->gvMotor1Label->text()); // HtmlText is any QString with html tags. - //m_aircraft->gvMotor1Label->setText(htmlText->toPlainText()); - //delete htmlText; + m_aircraft->gvMotor1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->gvMotor2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + // QTextEdit* htmlText=new QTextEdit(m_aircraft->gvMotor1Label->text()); // HtmlText is any QString with html tags. + // m_aircraft->gvMotor1Label->setText(htmlText->toPlainText()); + // delete htmlText; - //htmlText=new QTextEdit(m_aircraft->gvMotor2Label->text()); // HtmlText is any QString with html tags. - //m_aircraft->gvMotor2Label->setText(htmlText->toPlainText()); + // htmlText=new QTextEdit(m_aircraft->gvMotor2Label->text()); // HtmlText is any QString with html tags. + // m_aircraft->gvMotor2Label->setText(htmlText->toPlainText()); } if (m_aircraft->gvSteering1ChannelBox->currentText() == "None" - && m_aircraft->gvSteering2ChannelBox->currentText() == "None") { - m_aircraft->gvSteering1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes - m_aircraft->gvSteering2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes - //m_aircraft->gvStatusLabel->setText("ERROR: check steering channel assignment"); - //m_aircraft->gvSteering1Label->setText("" + m_aircraft->gvSteering1Label->text() + ""); - //m_aircraft->gvSteering2Label->setText("" + m_aircraft->gvSteering2Label->text() + ""); + && m_aircraft->gvSteering2ChannelBox->currentText() == "None") { + m_aircraft->gvSteering1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + m_aircraft->gvSteering2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + // m_aircraft->gvStatusLabel->setText("ERROR: check steering channel assignment"); + // m_aircraft->gvSteering1Label->setText("" + m_aircraft->gvSteering1Label->text() + ""); + // m_aircraft->gvSteering2Label->setText("" + m_aircraft->gvSteering2Label->text() + ""); error = true; } else { - m_aircraft->gvSteering1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - m_aircraft->gvSteering2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - //QTextEdit* htmlText=new QTextEdit(m_aircraft->gvSteering1Label->text()); // HtmlText is any QString with html tags. - //m_aircraft->gvSteering1Label->setText(htmlText->toPlainText()); - //delete htmlText; + m_aircraft->gvSteering1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->gvSteering2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + // QTextEdit* htmlText=new QTextEdit(m_aircraft->gvSteering1Label->text()); // HtmlText is any QString with html tags. + // m_aircraft->gvSteering1Label->setText(htmlText->toPlainText()); + // delete htmlText; - //htmlText=new QTextEdit(m_aircraft->gvSteering2Label->text()); // HtmlText is any QString with html tags. - //m_aircraft->gvSteering2Label->setText(htmlText->toPlainText()); + // htmlText=new QTextEdit(m_aircraft->gvSteering2Label->text()); // HtmlText is any QString with html tags. + // m_aircraft->gvSteering2Label->setText(htmlText->toPlainText()); } - } else if (airframeType == "GroundVehicleDifferential") { //Tank + } else if (airframeType == "GroundVehicleDifferential") { // Tank if (m_aircraft->gvMotor1ChannelBox->currentText() == "None" - || m_aircraft->gvMotor2ChannelBox->currentText() == "None") { - m_aircraft->gvMotor1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes - m_aircraft->gvMotor2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + || m_aircraft->gvMotor2ChannelBox->currentText() == "None") { + m_aircraft->gvMotor1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + m_aircraft->gvMotor2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->gvMotor1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - m_aircraft->gvMotor2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->gvMotor1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->gvMotor2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } // Always reset - m_aircraft->gvSteering1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - m_aircraft->gvSteering2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - } else if (airframeType == "GroundVehicleMotorcycle") { //Motorcycle + m_aircraft->gvSteering1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->gvSteering2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + } else if (airframeType == "GroundVehicleMotorcycle") { // Motorcycle if (m_aircraft->gvMotor1ChannelBox->currentText() == "None" - && m_aircraft->gvMotor2ChannelBox->currentText() == "None") { - m_aircraft->gvMotor2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + && m_aircraft->gvMotor2ChannelBox->currentText() == "None") { + m_aircraft->gvMotor2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->gvMotor2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->gvMotor2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if (m_aircraft->gvSteering1ChannelBox->currentText() == "None" - && m_aircraft->gvSteering2ChannelBox->currentText() == "None") { - m_aircraft->gvSteering1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + && m_aircraft->gvSteering2ChannelBox->currentText() == "None") { + m_aircraft->gvSteering1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->gvSteering1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->gvSteering1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } // Always reset - m_aircraft->gvMotor1ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes - m_aircraft->gvSteering2ChannelBox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + m_aircraft->gvMotor1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->gvSteering2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } if (error) { diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.h index 0a7abd858..c17dcea9d 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.h @@ -41,8 +41,7 @@ class Ui_Widget; -class ConfigGroundVehicleWidget: public VehicleConfig -{ +class ConfigGroundVehicleWidget : public VehicleConfig { Q_OBJECT public: @@ -70,7 +69,6 @@ private: private slots: virtual void setupUI(QString airframeType); virtual bool throwConfigError(QString airframeType); - }; #endif // CONFIGGROUNDVEHICLEWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp index a51cf370b..8aaa0c175 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp @@ -47,13 +47,14 @@ QStringList ConfigMultiRotorWidget::getChannelDescriptions() { // init a channel_numelem list of channel desc defaults QStringList channelDesc; - for (int i = 0; i < (int) ConfigMultiRotorWidget::CHANNEL_NUMELEM; i++) { + + for (int i = 0; i < (int)ConfigMultiRotorWidget::CHANNEL_NUMELEM; i++) { channelDesc.append(QString("-")); } // get the gui config data GUIConfigDataUnion configData = getConfigData(); - multiGUISettingsStruct multi = configData.multi; + multiGUISettingsStruct multi = configData.multi; if (multi.VTOLMotorN > 0 && multi.VTOLMotorN <= ConfigMultiRotorWidget::CHANNEL_NUMELEM) { channelDesc[multi.VTOLMotorN - 1] = QString("VTOLMotorN"); @@ -86,7 +87,7 @@ QStringList ConfigMultiRotorWidget::getChannelDescriptions() } ConfigMultiRotorWidget::ConfigMultiRotorWidget(QWidget *parent) : - VehicleConfig(parent), m_aircraft(new Ui_MultiRotorConfigWidget()), invertMotors(false) + VehicleConfig(parent), m_aircraft(new Ui_MultiRotorConfigWidget()), invertMotors(false) { m_aircraft->setupUi(this); @@ -110,13 +111,13 @@ ConfigMultiRotorWidget::ConfigMultiRotorWidget(QWidget *parent) : QStringList multiRotorTypes; multiRotorTypes << "Tricopter Y" << "Quad +" << "Quad X" << "Hexacopter" << "Hexacopter X" << "Hexacopter Y6" - << "Octocopter" << "Octocopter V" << "Octo Coax +" << "Octo Coax X"; + << "Octocopter" << "Octocopter V" << "Octo Coax +" << "Octo Coax X"; m_aircraft->multirotorFrameType->addItems(multiRotorTypes); // Set default model to "Quad X" m_aircraft->multirotorFrameType->setCurrentIndex(m_aircraft->multirotorFrameType->findText("Quad X")); - //setupUI(m_aircraft->multirotorFrameType->currentText()); + // setupUI(m_aircraft->multirotorFrameType->currentText()); connect(m_aircraft->multirotorFrameType, SIGNAL(currentIndexChanged(QString)), this, SLOT(setupUI(QString))); @@ -162,14 +163,14 @@ void ConfigMultiRotorWidget::setupUI(QString frameType) setYawMixLevel(33); } else if (frameType == "HexaX" || frameType == "Hexacopter X") { setComboCurrentIndex(m_aircraft->multirotorFrameType, - m_aircraft->multirotorFrameType->findText("Hexacopter X")); + m_aircraft->multirotorFrameType->findText("Hexacopter X")); m_aircraft->mrRollMixLevel->setValue(33); m_aircraft->mrPitchMixLevel->setValue(50); setYawMixLevel(33); } else if (frameType == "HexaCoax" || frameType == "Hexacopter Y6") { setComboCurrentIndex(m_aircraft->multirotorFrameType, - m_aircraft->multirotorFrameType->findText("Hexacopter Y6")); + m_aircraft->multirotorFrameType->findText("Hexacopter Y6")); m_aircraft->mrRollMixLevel->setValue(100); m_aircraft->mrPitchMixLevel->setValue(50); @@ -182,7 +183,7 @@ void ConfigMultiRotorWidget::setupUI(QString frameType) setYawMixLevel(25); } else if (frameType == "OctoV" || frameType == "Octocopter V") { setComboCurrentIndex(m_aircraft->multirotorFrameType, - m_aircraft->multirotorFrameType->findText("Octocopter V")); + m_aircraft->multirotorFrameType->findText("Octocopter V")); m_aircraft->mrRollMixLevel->setValue(25); m_aircraft->mrPitchMixLevel->setValue(25); @@ -216,7 +217,7 @@ void ConfigMultiRotorWidget::setupEnabledControls(QString frameType) // disable all motor channel boxes for (int i = 1; i <= 8; i++) { // do it manually so we can turn off any error decorations - QComboBox *combobox = qFindChild(this, "multiMotorChannelBox" + QString::number(i)); + QComboBox *combobox = qFindChild(this, "multiMotorChannelBox" + QString::number(i)); if (combobox) { combobox->setEnabled(false); combobox->setItemData(0, 0, Qt::DecorationRole); @@ -247,7 +248,8 @@ void ConfigMultiRotorWidget::setupEnabledControls(QString frameType) } } -void ConfigMultiRotorWidget::registerWidgets(ConfigTaskWidget &parent) { +void ConfigMultiRotorWidget::registerWidgets(ConfigTaskWidget &parent) +{ parent.addWidget(m_aircraft->multiThrottleCurve->getCurveWidget()); parent.addWidget(m_aircraft->multiThrottleCurve); parent.addWidget(m_aircraft->multirotorFrameType); @@ -268,19 +270,19 @@ void ConfigMultiRotorWidget::registerWidgets(ConfigTaskWidget &parent) { void ConfigMultiRotorWidget::resetActuators(GUIConfigDataUnion *configData) { - configData->multi.VTOLMotorN = 0; + configData->multi.VTOLMotorN = 0; configData->multi.VTOLMotorNE = 0; - configData->multi.VTOLMotorE = 0; + configData->multi.VTOLMotorE = 0; configData->multi.VTOLMotorSE = 0; - configData->multi.VTOLMotorS = 0; + configData->multi.VTOLMotorS = 0; configData->multi.VTOLMotorSW = 0; - configData->multi.VTOLMotorW = 0; + configData->multi.VTOLMotorW = 0; configData->multi.VTOLMotorNW = 0; configData->multi.TRIYaw = 0; } /** - Helper function to refresh the UI widget values + Helper function to refresh the UI widget values */ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) { @@ -303,7 +305,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->multiThrottleCurve->initLinearCurve(curveValues.count(), 0.9); } - GUIConfigDataUnion config = getConfigData(); + GUIConfigDataUnion config = getConfigData(); multiGUISettingsStruct multi = config.multi; if (frameType == "QuadP") { @@ -321,11 +323,11 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) double value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH); m_aircraft->mrPitchMixLevel->setValue(qRound(value / 1.27)); - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); } } else if (frameType == "QuadX") { @@ -370,9 +372,9 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); - //change channels + // change channels channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); } } else if (frameType == "HexaX") { @@ -393,11 +395,11 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) double value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH); m_aircraft->mrPitchMixLevel->setValue(qRound(value / 1.27)); - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); } } else if (frameType == "HexaCoax") { @@ -418,10 +420,10 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->mrPitchMixLevel->setValue(qRound(2 * value / 1.27)); channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(qRound(value / 1.27)); - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(qRound(value / 1.27)); } } else if (frameType == "Octo" || frameType == "OctoV" || frameType == "OctoCoaxP") { @@ -447,9 +449,9 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); - //change channels + // change channels channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); } else if (frameType == "OctoV") { double value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH); @@ -458,9 +460,9 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); - //change channels + // change channels channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); } else if (frameType == "OctoCoaxP") { double value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH); @@ -469,9 +471,9 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); - //change channels + // change channels channel = m_aircraft->multiMotorChannelBox3->currentIndex() - 1; - value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); } } @@ -515,7 +517,6 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(qRound(value / 1.27)); - } } @@ -523,15 +524,16 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) } /** - Helper function to update the UI widget objects + Helper function to update the UI widget objects */ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() { UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + Q_ASSERT(mixer); // Curve is also common to all quads: - setThrottleCurve(mixer, VehicleConfig::MIXER_THROTTLECURVE1, m_aircraft->multiThrottleCurve->getCurve() ); + setThrottleCurve(mixer, VehicleConfig::MIXER_THROTTLECURVE1, m_aircraft->multiThrottleCurve->getCurve()); QString airframeType; QList motorList; @@ -558,46 +560,43 @@ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() setupMotors(motorList); // Motor 1 to 6, Y6 Layout: - // pitch roll yaw + // pitch roll yaw double mixerMatrix[8][3] = { - { 0.5, 1, -1 }, - { 0.5, 1, 1 }, - { 0.5, -1, -1 }, - { 0.5, -1, 1 }, - { -1, 0, -1 }, - { -1, 0, 1 }, - { 0, 0, 0 }, - { 0, 0, 0 } + { 0.5, 1, -1 }, + { 0.5, 1, 1 }, + { 0.5, -1, -1 }, + { 0.5, -1, 1 }, + { -1, 0, -1 }, + { -1, 0, 1 }, + { 0, 0, 0 }, + { 0, 0, 0 } }; setupMultiRotorMixer(mixerMatrix); m_aircraft->mrStatusLabel->setText("Configuration OK"); - } else if (m_aircraft->multirotorFrameType->currentText() == "Octocopter") { airframeType = "Octo"; // Show any config errors in GUI if (throwConfigError(8)) { return airframeType; - } motorList << "VTOLMotorN" << "VTOLMotorNE" << "VTOLMotorE" << "VTOLMotorSE" << "VTOLMotorS" << "VTOLMotorSW" - << "VTOLMotorW" << "VTOLMotorNW"; + << "VTOLMotorW" << "VTOLMotorNW"; setupMotors(motorList); // Motor 1 to 8: - // pitch roll yaw + // pitch roll yaw double mixerMatrix[8][3] = { - { 1, 0, -1 }, - { 1, -1, 1 }, - { 0, -1, -1 }, - { -1, -1, 1 }, - { -1, 0, -1 }, - { -1, 1, 1 }, - { 0, 1, -1 }, - { 1, 1, 1 } + { 1, 0, -1 }, + { 1, -1, 1 }, + { 0, -1, -1 }, + { -1, -1, 1 }, + { -1, 0, -1 }, + { -1, 1, 1 }, + { 0, 1, -1 }, + { 1, 1, 1 } }; setupMultiRotorMixer(mixerMatrix); m_aircraft->mrStatusLabel->setText("Configuration OK"); - } else if (m_aircraft->multirotorFrameType->currentText() == "Octocopter V") { airframeType = "OctoV"; @@ -606,24 +605,23 @@ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() return airframeType; } motorList << "VTOLMotorN" << "VTOLMotorNE" << "VTOLMotorE" << "VTOLMotorSE" << "VTOLMotorS" << "VTOLMotorSW" - << "VTOLMotorW" << "VTOLMotorNW"; + << "VTOLMotorW" << "VTOLMotorNW"; setupMotors(motorList); // Motor 1 to 8: // IMPORTANT: Assumes evenly spaced engines - // pitch roll yaw + // pitch roll yaw double mixerMatrix[8][3] = { - { 0.33, -1, -1 }, - { 1 , -1, 1 }, - { -1 , -1, -1 }, - { -0.33, -1, 1 }, - { -0.33, 1, -1 }, - { -1 , 1, 1 }, - { 1 , 1, -1 }, - { 0.33, 1, 1 } + { 0.33, -1, -1 }, + { 1, -1, 1 }, + { -1, -1, -1 }, + { -0.33, -1, 1 }, + { -0.33, 1, -1 }, + { -1, 1, 1 }, + { 1, 1, -1 }, + { 0.33, 1, 1 } }; setupMultiRotorMixer(mixerMatrix); m_aircraft->mrStatusLabel->setText("Configuration OK"); - } else if (m_aircraft->multirotorFrameType->currentText() == "Octo Coax +") { airframeType = "OctoCoaxP"; @@ -632,23 +630,22 @@ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() return airframeType; } motorList << "VTOLMotorN" << "VTOLMotorNE" << "VTOLMotorE" << "VTOLMotorSE" << "VTOLMotorS" << "VTOLMotorSW" - << "VTOLMotorW" << "VTOLMotorNW"; + << "VTOLMotorW" << "VTOLMotorNW"; setupMotors(motorList); // Motor 1 to 8: - // pitch roll yaw + // pitch roll yaw double mixerMatrix[8][3] = { - { 1, 0, -1 }, - { 1, 0, 1 }, - { 0, -1, -1 }, - { 0, -1, 1 }, - { -1, 0, -1 }, - { -1, 0, 1 }, - { 0, 1, -1 }, - { 0, 1, 1 } + { 1, 0, -1 }, + { 1, 0, 1 }, + { 0, -1, -1 }, + { 0, -1, 1 }, + { -1, 0, -1 }, + { -1, 0, 1 }, + { 0, 1, -1 }, + { 0, 1, 1 } }; setupMultiRotorMixer(mixerMatrix); m_aircraft->mrStatusLabel->setText("Configuration OK"); - } else if (m_aircraft->multirotorFrameType->currentText() == "Octo Coax X") { airframeType = "OctoCoaxX"; @@ -657,30 +654,28 @@ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() return airframeType; } motorList << "VTOLMotorNW" << "VTOLMotorN" << "VTOLMotorNE" << "VTOLMotorE" << "VTOLMotorSE" << "VTOLMotorS" - << "VTOLMotorSW" << "VTOLMotorW"; + << "VTOLMotorSW" << "VTOLMotorW"; setupMotors(motorList); // Motor 1 to 8: - // pitch roll yaw + // pitch roll yaw double mixerMatrix[8][3] = { - { 1, 1, -1 }, - { 1, 1, 1 }, - { 1, -1, -1 }, - { 1, -1, 1 }, + { 1, 1, -1 }, + { 1, 1, 1 }, + { 1, -1, -1 }, + { 1, -1, 1 }, { -1, -1, -1 }, - { -1, -1, 1 }, - { -1, 1, -1 }, - { -1, 1, 1 } + { -1, -1, 1 }, + { -1, 1, -1 }, + { -1, 1, 1 } }; setupMultiRotorMixer(mixerMatrix); m_aircraft->mrStatusLabel->setText("Configuration OK"); - } else if (m_aircraft->multirotorFrameType->currentText() == "Tricopter Y") { airframeType = "Tri"; // Show any config errors in GUI if (throwConfigError(3)) { return airframeType; - } if (m_aircraft->triYawChannelBox->currentText() == "None") { m_aircraft->mrStatusLabel->setText("Error: Assign a Yaw channel"); @@ -694,16 +689,16 @@ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() setConfigData(config); // Motor 1 to 6, Y6 Layout: - // pitch roll yaw + // pitch roll yaw double mixerMatrix[8][3] = { - { 0.5, 1, 0 }, - { 0.5, -1, 0 }, - { -1, 0, 0 }, - { 0, 0, 0 }, - { 0, 0, 0 }, - { 0, 0, 0 }, - { 0, 0, 0 }, - { 0, 0, 0 } + { 0.5, 1, 0 }, + { 0.5, -1, 0 }, + { -1, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 } }; setupMultiRotorMixer(mixerMatrix); @@ -716,7 +711,6 @@ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() } m_aircraft->mrStatusLabel->setText(tr("Configuration OK")); - } return airframeType; @@ -731,11 +725,12 @@ void ConfigMultiRotorWidget::setYawMixLevel(int value) m_aircraft->mrYawMixLevel->setValue(value); m_aircraft->MultirotorRevMixerCheckBox->setChecked(false); } - } -void ConfigMultiRotorWidget::reverseMultirotorMotor(){ +void ConfigMultiRotorWidget::reverseMultirotorMotor() +{ QString frameType = m_aircraft->multirotorFrameType->currentText(); + updateAirframe(frameType); } @@ -779,11 +774,12 @@ void ConfigMultiRotorWidget::updateAirframe(QString frameType) } /** - Helper function: setupQuadMotor + Helper function: setupQuadMotor */ void ConfigMultiRotorWidget::setupQuadMotor(int channel, double pitch, double roll, double yaw) { - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + Q_ASSERT(mixer); setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_MOTOR); @@ -796,21 +792,22 @@ void ConfigMultiRotorWidget::setupQuadMotor(int channel, double pitch, double ro } /** - Helper function: setup motors. Takes a list of channel names in input. + Helper function: setup motors. Takes a list of channel names in input. */ void ConfigMultiRotorWidget::setupMotors(QList motorList) { - QList mmList; + QList mmList; mmList << m_aircraft->multiMotorChannelBox1 << m_aircraft->multiMotorChannelBox2 - << m_aircraft->multiMotorChannelBox3 << m_aircraft->multiMotorChannelBox4 - << m_aircraft->multiMotorChannelBox5 << m_aircraft->multiMotorChannelBox6 - << m_aircraft->multiMotorChannelBox7 << m_aircraft->multiMotorChannelBox8; + << m_aircraft->multiMotorChannelBox3 << m_aircraft->multiMotorChannelBox4 + << m_aircraft->multiMotorChannelBox5 << m_aircraft->multiMotorChannelBox6 + << m_aircraft->multiMotorChannelBox7 << m_aircraft->multiMotorChannelBox8; GUIConfigDataUnion configData = getConfigData(); resetActuators(&configData); - foreach (QString motor, motorList) { + foreach(QString motor, motorList) { int index = mmList.takeFirst()->currentIndex(); + if (motor == QString("VTOLMotorN")) { configData.multi.VTOLMotorN = index; } else if (motor == QString("VTOLMotorNE")) { @@ -833,7 +830,7 @@ void ConfigMultiRotorWidget::setupMotors(QList motorList) } /** - Set up a Quad-X or Quad-P mixer + Set up a Quad-X or Quad-P mixer */ bool ConfigMultiRotorWidget::setupQuad(bool pLayout) { @@ -854,37 +851,37 @@ bool ConfigMultiRotorWidget::setupQuad(bool pLayout) // Now, setup the mixer: // Motor 1 to 4, X Layout: - // pitch roll yaw - // {0.5 ,0.5 ,-0.5 //Front left motor (CW) - // {0.5 ,-0.5 ,0.5 //Front right motor(CCW) - // {-0.5 ,-0.5 ,-0.5 //rear right motor (CW) - // {-0.5 ,0.5 ,0.5 //Rear left motor (CCW) - double xMixer [8][3] = { - { 1, 1, -1}, - { 1, -1, 1}, - {-1, -1, -1}, - {-1, 1, 1}, - { 0, 0, 0}, - { 0, 0, 0}, - { 0, 0, 0}, - { 0, 0, 0} + // pitch roll yaw + // {0.5 ,0.5 ,-0.5 //Front left motor (CW) + // {0.5 ,-0.5 ,0.5 //Front right motor(CCW) + // {-0.5 ,-0.5 ,-0.5 //rear right motor (CW) + // {-0.5 ,0.5 ,0.5 //Rear left motor (CCW) + double xMixer[8][3] = { + { 1, 1, -1 }, + { 1, -1, 1 }, + { -1, -1, -1 }, + { -1, 1, 1 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 } }; // Motor 1 to 4, P Layout: // pitch roll yaw - // {1 ,0 ,-0.5 //Front motor (CW) - // {0 ,-1 ,0.5 //Right motor(CCW) - // {-1 ,0 ,-0.5 //Rear motor (CW) - // {0 ,1 ,0.5 //Left motor (CCW) - double pMixer [8][3] = { - { 1, 0, -1}, - { 0, -1, 1}, - {-1, 0, -1}, - { 0, 1, 1}, - { 0, 0, 0}, - { 0, 0, 0}, - { 0, 0, 0}, - { 0, 0, 0} + // {1 ,0 ,-0.5 //Front motor (CW) + // {0 ,-1 ,0.5 //Right motor(CCW) + // {-1 ,0 ,-0.5 //Rear motor (CW) + // {0 ,1 ,0.5 //Left motor (CCW) + double pMixer[8][3] = { + { 1, 0, -1 }, + { 0, -1, 1 }, + { -1, 0, -1 }, + { 0, 1, 1 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 } }; if (pLayout) { @@ -897,7 +894,7 @@ bool ConfigMultiRotorWidget::setupQuad(bool pLayout) } /** - Set up a Hexa-X or Hexa-P mixer + Set up a Hexa-X or Hexa-P mixer */ bool ConfigMultiRotorWidget::setupHexa(bool pLayout) { @@ -918,22 +915,22 @@ bool ConfigMultiRotorWidget::setupHexa(bool pLayout) // and set only the relevant channels: // Motor 1 to 6, P Layout: - // pitch roll yaw - // 1 { 0.3 , 0 ,-0.3 // N CW - // 2 { 0.3 ,-0.5 , 0.3 // NE CCW - // 3 {-0.3 ,-0.5 ,-0.3 // SE CW - // 4 {-0.3 , 0 , 0.3 // S CCW - // 5 {-0.3 , 0.5 ,-0.3 // SW CW - // 6 { 0.3 , 0.5 , 0.3 // NW CCW - double pMixer [8][3] = { - { 1, 0, -1}, - { 1, -1, 1}, - {-1, -1, -1}, - {-1, 0, 1}, - {-1, 1, -1}, - { 1, 1, 1}, - { 0, 0, 0}, - { 0, 0, 0} + // pitch roll yaw + // 1 { 0.3 , 0 ,-0.3 // N CW + // 2 { 0.3 ,-0.5 , 0.3 // NE CCW + // 3 {-0.3 ,-0.5 ,-0.3 // SE CW + // 4 {-0.3 , 0 , 0.3 // S CCW + // 5 {-0.3 , 0.5 ,-0.3 // SW CW + // 6 { 0.3 , 0.5 , 0.3 // NW CCW + double pMixer[8][3] = { + { 1, 0, -1 }, + { 1, -1, 1 }, + { -1, -1, -1 }, + { -1, 0, 1 }, + { -1, 1, -1 }, + { 1, 1, 1 }, + { 0, 0, 0 }, + { 0, 0, 0 } }; // Motor 1 to 6, X Layout: @@ -943,15 +940,15 @@ bool ConfigMultiRotorWidget::setupHexa(bool pLayout) // 4 [ -0.5, 0.3, 0.3 ] SW // 5 [ 0 , 0.3, -0.3 ] W // 6 [ 0.5, 0.3, 0.3 ] NW - double xMixer [8][3] = { - { 1, -1, -1}, - { 0, -1, 1}, - { -1, -1, -1}, - { -1, 1, 1}, - { 0, 1, -1}, - { 1, 1, 1}, - { 0, 0, 0}, - { 0, 0, 0} + double xMixer[8][3] = { + { 1, -1, -1 }, + { 0, -1, 1 }, + { -1, -1, -1 }, + { -1, 1, 1 }, + { 0, 1, -1 }, + { 1, 1, 1 }, + { 0, 0, 0 }, + { 0, 0, 0 } }; if (pLayout) { @@ -965,31 +962,32 @@ bool ConfigMultiRotorWidget::setupHexa(bool pLayout) /** - This function sets up the multirotor mixer values. + This function sets up the multirotor mixer values. */ bool ConfigMultiRotorWidget::setupMultiRotorMixer(double mixerFactors[8][3]) { - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); // and enable only the relevant channels: - double pFactor = (double) m_aircraft->mrPitchMixLevel->value() / 100.0; - double rFactor = (double) m_aircraft->mrRollMixLevel->value() / 100.0; + double pFactor = (double)m_aircraft->mrPitchMixLevel->value() / 100.0; + double rFactor = (double)m_aircraft->mrRollMixLevel->value() / 100.0; invertMotors = m_aircraft->MultirotorRevMixerCheckBox->isChecked(); - double yFactor = (invertMotors ? -1.0 : 1.0) * (double) m_aircraft->mrYawMixLevel->value() / 100.0; + double yFactor = (invertMotors ? -1.0 : 1.0) * (double)m_aircraft->mrYawMixLevel->value() / 100.0; - QList mmList; + QList mmList; mmList << m_aircraft->multiMotorChannelBox1 << m_aircraft->multiMotorChannelBox2 - << m_aircraft->multiMotorChannelBox3 << m_aircraft->multiMotorChannelBox4 - << m_aircraft->multiMotorChannelBox5 << m_aircraft->multiMotorChannelBox6 - << m_aircraft->multiMotorChannelBox7 << m_aircraft->multiMotorChannelBox8; + << m_aircraft->multiMotorChannelBox3 << m_aircraft->multiMotorChannelBox4 + << m_aircraft->multiMotorChannelBox5 << m_aircraft->multiMotorChannelBox6 + << m_aircraft->multiMotorChannelBox7 << m_aircraft->multiMotorChannelBox8; for (int i = 0; i < 8; i++) { if (mmList.at(i)->isEnabled()) { int channel = mmList.at(i)->currentIndex() - 1; if (channel > -1) { setupQuadMotor(channel, mixerFactors[i][0] * pFactor, rFactor * mixerFactors[i][1], - yFactor * mixerFactors[i][2]); + yFactor * mixerFactors[i][2]); } } } @@ -997,7 +995,7 @@ bool ConfigMultiRotorWidget::setupMultiRotorMixer(double mixerFactors[8][3]) } /** - This function displays text and color formatting in order to help the user understand what channels have not yet been configured. + This function displays text and color formatting in order to help the user understand what channels have not yet been configured. */ bool ConfigMultiRotorWidget::throwConfigError(int numMotors) { @@ -1006,30 +1004,30 @@ bool ConfigMultiRotorWidget::throwConfigError(int numMotors) // Iterate through all instances of multiMotorChannelBox for (int i = 0; i < numMotors; i++) { - //Fine widgets with text "multiMotorChannelBox.x", where x is an integer - QComboBox *combobox = qFindChild(this, "multiMotorChannelBox" + QString::number(i + 1)); + // Fine widgets with text "multiMotorChannelBox.x", where x is an integer + QComboBox *combobox = qFindChild(this, "multiMotorChannelBox" + QString::number(i + 1)); if (combobox) { if (combobox->currentText() == "None") { int size = combobox->style()->pixelMetric(QStyle::PM_SmallIconSize); QPixmap pixmap(size, size); pixmap.fill(QColor("red")); - combobox->setItemData(0, pixmap, Qt::DecorationRole); //Set color palettes + combobox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - combobox->setItemData(0, 0, Qt::DecorationRole); //Reset color palettes + combobox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } } } if (error) { m_aircraft->mrStatusLabel->setText( - QString("ERROR: Assign all %1 motor channels").arg(numMotors)); + QString("ERROR: Assign all %1 motor channels").arg(numMotors)); } return error; } /** - WHAT DOES THIS DO??? + WHAT DOES THIS DO??? */ void ConfigMultiRotorWidget::showEvent(QShowEvent *event) { @@ -1041,7 +1039,7 @@ void ConfigMultiRotorWidget::showEvent(QShowEvent *event) } /** - Resize the GUI contents when the user changes the window size + Resize the GUI contents when the user changes the window size */ void ConfigMultiRotorWidget::resizeEvent(QResizeEvent *event) { @@ -1052,7 +1050,8 @@ void ConfigMultiRotorWidget::resizeEvent(QResizeEvent *event) void ConfigMultiRotorWidget::enableControls(bool enable) { ConfigTaskWidget::enableControls(enable); - if(enable) { + + if (enable) { setupEnabledControls(m_aircraft->multirotorFrameType->currentText()); } } diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h index 75c251777..e9c2c2c70 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h @@ -41,8 +41,7 @@ class Ui_Widget; -class ConfigMultiRotorWidget: public VehicleConfig -{ +class ConfigMultiRotorWidget : public VehicleConfig { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp index e70ee0835..21c892aed 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp @@ -1,4 +1,3 @@ - /** ****************************************************************************** * @@ -37,38 +36,39 @@ VehicleConfig::VehicleConfig(QWidget *parent) : ConfigTaskWidget(parent) { // Generate lists of mixerTypeNames, mixerVectorNames, channelNames channelNames << "None"; - for (int i = 0; i < (int) VehicleConfig::CHANNEL_NUMELEM; i++) { + for (int i = 0; i < (int)VehicleConfig::CHANNEL_NUMELEM; i++) { mixerTypes << QString("Mixer%1Type").arg(i + 1); mixerVectors << QString("Mixer%1Vector").arg(i + 1); channelNames << QString("Channel%1").arg(i + 1); } mixerTypeDescriptions << "Disabled" << "Motor" << "Servo" << "CameraRoll" << "CameraPitch" << "CameraYaw" - << "Accessory0" << "Accessory1" << "Accessory2" << "Accessory3" << "Accessory4" << "Accessory5"; + << "Accessory0" << "Accessory1" << "Accessory2" << "Accessory3" << "Accessory4" << "Accessory5"; // This is needed because new style tries to compact things as much as possible in grid // and on OSX the widget sizes of PushButtons is reported incorrectly: // https://bugreports.qt-project.org/browse/QTBUG-14591 - foreach(QPushButton *btn, findChildren()) { + foreach(QPushButton * btn, findChildren()) { btn->setAttribute(Qt::WA_LayoutUsesWidgetRect); } } VehicleConfig::~VehicleConfig() { - // Do nothing + // Do nothing } GUIConfigDataUnion VehicleConfig::getConfigData() { // get an instance of systemsettings SystemSettings *systemSettings = SystemSettings::GetInstance(getUAVObjectManager()); + Q_ASSERT(systemSettings); SystemSettings::DataFields systemSettingsData = systemSettings->getData(); // copy systemsettings -> local configData GUIConfigDataUnion configData; - for (int i = 0; i < (int) SystemSettings::GUICONFIGDATA_NUMELEM; i++) { + for (int i = 0; i < (int)SystemSettings::GUICONFIGDATA_NUMELEM; i++) { configData.UAVObject[i] = systemSettingsData.GUIConfigData[i]; } @@ -95,7 +95,7 @@ void VehicleConfig::setConfigData(GUIConfigDataUnion configData) } // copy parameter configData -> systemsettings - for (int i = 0; i < (int) SystemSettings::GUICONFIGDATA_NUMELEM; i++) { + for (int i = 0; i < (int)SystemSettings::GUICONFIGDATA_NUMELEM; i++) { guiConfig->setValue(configData.UAVObject[i], i); } } @@ -121,9 +121,7 @@ void VehicleConfig::refreshWidgetsValues(UAVObject *o) } void VehicleConfig::updateObjectsFromWidgets() -{ - -} +{} void VehicleConfig::resetActuators(GUIConfigDataUnion *configData) { @@ -131,7 +129,8 @@ void VehicleConfig::resetActuators(GUIConfigDataUnion *configData) } -void VehicleConfig::registerWidgets(ConfigTaskWidget &parent) { +void VehicleConfig::registerWidgets(ConfigTaskWidget &parent) +{ Q_UNUSED(parent); } @@ -141,15 +140,15 @@ void VehicleConfig::registerWidgets(ConfigTaskWidget &parent) { void VehicleConfig::populateChannelComboBoxes() { QList l = findChildren(QRegExp("\\S+ChannelBo\\S+")); - foreach(QComboBox *combobox, l) { + foreach(QComboBox * combobox, l) { combobox->addItems(channelNames); } } /** - Helper function: - Sets the current index on supplied combobox to index - if it is within bounds 0 <= index < combobox.count() + Helper function: + Sets the current index on supplied combobox to index + if it is within bounds 0 <= index < combobox.count() */ void VehicleConfig::setComboCurrentIndex(QComboBox *box, int index) { @@ -160,13 +159,13 @@ void VehicleConfig::setComboCurrentIndex(QComboBox *box, int index) } /** - Helper function: - enables/disables the named comboboxes within supplied owner + Helper function: + enables/disables the named comboboxes within supplied owner */ void VehicleConfig::enableComboBoxes(QWidget *owner, QString boxName, int boxCount, bool enable) { for (int i = 1; i <= boxCount; i++) { - QComboBox* box = qFindChild(owner, QString("%0%1").arg(boxName).arg(i)); + QComboBox *box = qFindChild(owner, QString("%0%1").arg(boxName).arg(i)); if (box) { box->setEnabled(enable); } @@ -224,7 +223,7 @@ void VehicleConfig::resetMixerVector(UAVDataObject *mixer, int channel) // Disable all servo/motor mixers (but keep camera and accessory ones) void VehicleConfig::resetMotorAndServoMixers(UAVDataObject *mixer) { - for (int channel = 0; channel < (int) VehicleConfig::CHANNEL_NUMELEM; channel++) { + for (int channel = 0; channel < (int)VehicleConfig::CHANNEL_NUMELEM; channel++) { QString type = getMixerType(mixer, channel); if ((type == "Disabled") || (type == "Motor") || (type == "Servo")) { setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_DISABLED); @@ -298,7 +297,7 @@ void VehicleConfig::setThrottleCurve(UAVDataObject *mixer, MixerThrottleCurveEle break; } - if (field && (field->getNumElements() == (unsigned int) curve.length())) { + if (field && (field->getNumElements() == (unsigned int)curve.length())) { for (int i = 0; i < curve.length(); i++) { field->setValue(curve.at(i), i); } @@ -335,8 +334,9 @@ bool VehicleConfig::isValidThrottleCurve(QList *curve) if (curve) { for (int i = 0; i < curve->count(); i++) { - if (curve->at(i) != 0) + if (curve->at(i) != 0) { return true; + } } } return false; @@ -346,6 +346,7 @@ double VehicleConfig::getCurveMin(QList *curve) { // TODO initialize to max double double min = 0; + for (int i = 0; i < curve->count(); i++) { min = std::min(min, curve->at(i)); } @@ -356,6 +357,7 @@ double VehicleConfig::getCurveMax(QList *curve) { // TODO initialize to min double double max = 0; + for (int i = 0; i < curve->count(); i++) { max = std::max(max, curve->at(i)); } @@ -363,9 +365,9 @@ double VehicleConfig::getCurveMax(QList *curve) } /** - Reset the contents of a field - */ -void VehicleConfig::resetField(UAVObjectField * field) + Reset the contents of a field + */ +void VehicleConfig::resetField(UAVObjectField *field) { for (unsigned int i = 0; i < field->getNumElements(); i++) { field->setValue(0, i); @@ -380,6 +382,7 @@ UAVObjectManager *VehicleConfig::getUAVObjectManager() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objMngr = pm->getObject(); + Q_ASSERT(objMngr); return objMngr; } diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h index f116d05b9..b767bc081 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h @@ -34,71 +34,70 @@ #include "actuatorcommand.h" typedef struct { - uint VTOLMotorN:4; - uint VTOLMotorS:4; - uint VTOLMotorE:4; - uint VTOLMotorW:4; - uint VTOLMotorNW:4; - uint VTOLMotorNE:4; - uint VTOLMotorSW:4; - uint VTOLMotorSE:4; // 32 bits - uint TRIYaw:4; - quint32 padding:28; // 64 bits + uint VTOLMotorN : 4; + uint VTOLMotorS : 4; + uint VTOLMotorE : 4; + uint VTOLMotorW : 4; + uint VTOLMotorNW : 4; + uint VTOLMotorNE : 4; + uint VTOLMotorSW : 4; + uint VTOLMotorSE : 4; // 32 bits + uint TRIYaw : 4; + quint32 padding : 28; // 64 bits quint32 padding1; quint32 padding2; // 128 bits } __attribute__((packed)) multiGUISettingsStruct; typedef struct { - uint SwashplateType:3; - uint FirstServoIndex:2; - uint CorrectionAngle:9; - uint ccpmCollectivePassthroughState:1; - uint ccpmLinkCyclicState:1; - uint ccpmLinkRollState:1; - uint SliderValue0:7; - uint SliderValue1:7; - uint SliderValue2:7; // 41 bits - uint ServoIndexW:4; - uint ServoIndexX:4; - uint ServoIndexY:4; - uint ServoIndexZ:4; // 57 bits - uint Throttle:4; - uint Tail:4; // 65bits - quint32 padding:31; // 96 bits + uint SwashplateType : 3; + uint FirstServoIndex : 2; + uint CorrectionAngle : 9; + uint ccpmCollectivePassthroughState : 1; + uint ccpmLinkCyclicState : 1; + uint ccpmLinkRollState : 1; + uint SliderValue0 : 7; + uint SliderValue1 : 7; + uint SliderValue2 : 7; // 41 bits + uint ServoIndexW : 4; + uint ServoIndexX : 4; + uint ServoIndexY : 4; + uint ServoIndexZ : 4; // 57 bits + uint Throttle : 4; + uint Tail : 4; // 65bits + quint32 padding : 31; // 96 bits quint32 padding1; // 128 bits } __attribute__((packed)) heliGUISettingsStruct; typedef struct { - uint FixedWingThrottle:4; - uint FixedWingRoll1:4; - uint FixedWingRoll2:4; - uint FixedWingPitch1:4; - uint FixedWingPitch2:4; - uint FixedWingYaw1:4; - uint FixedWingYaw2:4; - uint padding:4; // 32 bits + uint FixedWingThrottle : 4; + uint FixedWingRoll1 : 4; + uint FixedWingRoll2 : 4; + uint FixedWingPitch1 : 4; + uint FixedWingPitch2 : 4; + uint FixedWingYaw1 : 4; + uint FixedWingYaw2 : 4; + uint padding : 4; // 32 bits quint32 padding1; quint32 padding2; quint32 padding3; // 128 bits } __attribute__((packed)) fixedGUISettingsStruct; typedef struct { - uint GroundVehicleThrottle1:4; - uint GroundVehicleThrottle2:4; - uint GroundVehicleSteering1:4; - uint GroundVehicleSteering2:4; - uint padding:16; // 32 bits + uint GroundVehicleThrottle1 : 4; + uint GroundVehicleThrottle2 : 4; + uint GroundVehicleSteering1 : 4; + uint GroundVehicleSteering2 : 4; + uint padding : 16; // 32 bits quint32 padding1; quint32 padding2; quint32 padding3; // 128 bits } __attribute__((packed)) groundGUISettingsStruct; -typedef union -{ +typedef union { uint UAVObject[4]; // 32 bits * 4 - heliGUISettingsStruct heli; // 128 bits - fixedGUISettingsStruct fixedwing; - multiGUISettingsStruct multi; + heliGUISettingsStruct heli; // 128 bits + fixedGUISettingsStruct fixedwing; + multiGUISettingsStruct multi; groundGUISettingsStruct ground; } GUIConfigDataUnion; @@ -110,9 +109,8 @@ class ConfigTaskWidget; * This class derives from ConfigTaskWidget and overrides its the default "binding" mechanism. * It does not use the "dirty" state management directlyand registers its relevant widgets with ConfigTaskWidget to do so. */ -class VehicleConfig: public ConfigTaskWidget -{ -Q_OBJECT +class VehicleConfig : public ConfigTaskWidget { + Q_OBJECT public: @@ -123,27 +121,27 @@ public: /* Enumeration options for field MixerType */ typedef enum { - MIXERTYPE_DISABLED = 0, - MIXERTYPE_MOTOR = 1, - MIXERTYPE_SERVO = 2, - MIXERTYPE_CAMERAROLL = 3, + MIXERTYPE_DISABLED = 0, + MIXERTYPE_MOTOR = 1, + MIXERTYPE_SERVO = 2, + MIXERTYPE_CAMERAROLL = 3, MIXERTYPE_CAMERAPITCH = 4, - MIXERTYPE_CAMERAYAW = 5, - MIXERTYPE_ACCESSORY0 = 6, - MIXERTYPE_ACCESSORY1 = 7, - MIXERTYPE_ACCESSORY2 = 8, - MIXERTYPE_ACCESSORY3 = 9, - MIXERTYPE_ACCESSORY4 = 10, - MIXERTYPE_ACCESSORY5 = 11 + MIXERTYPE_CAMERAYAW = 5, + MIXERTYPE_ACCESSORY0 = 6, + MIXERTYPE_ACCESSORY1 = 7, + MIXERTYPE_ACCESSORY2 = 8, + MIXERTYPE_ACCESSORY3 = 9, + MIXERTYPE_ACCESSORY4 = 10, + MIXERTYPE_ACCESSORY5 = 11 } MixerTypeElem; /* Array element names for field MixerVector */ typedef enum { MIXERVECTOR_THROTTLECURVE1 = 0, MIXERVECTOR_THROTTLECURVE2 = 1, - MIXERVECTOR_ROLL = 2, + MIXERVECTOR_ROLL = 2, MIXERVECTOR_PITCH = 3, - MIXERVECTOR_YAW = 4 + MIXERVECTOR_YAW = 4 } MixerVectorElem; static const quint32 CHANNEL_NUMELEM = ActuatorCommand::CHANNEL_NUMELEM;; @@ -183,7 +181,7 @@ protected: QString getMixerType(UAVDataObject *mixer, int channel); void setMixerType(UAVDataObject *mixer, int channel, MixerTypeElem mixerType); void setThrottleCurve(UAVDataObject *mixer, MixerThrottleCurveElem curveType, QList curve); - void getThrottleCurve(UAVDataObject *mixer, MixerThrottleCurveElem curveType, QList* curve); + void getThrottleCurve(UAVDataObject *mixer, MixerThrottleCurveElem curveType, QList *curve); bool isValidThrottleCurve(QList *curve); double getCurveMin(QList *curve); double getCurveMax(QList *curve); @@ -199,7 +197,6 @@ private: private slots: virtual void setupUI(QString airframeType); - }; #endif // VEHICLECONFIG_H diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp index d6dc18dde..f0762ff51 100644 --- a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp @@ -44,13 +44,14 @@ ConfigCCHWWidget::ConfigCCHWWidget(QWidget *parent) : ConfigTaskWidget(parent) m_telemetry = new Ui_CC_HW_Widget(); m_telemetry->setupUi(this); - ExtensionSystem::PluginManager *pm=ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); - if(!settings->useExpertMode()) + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { m_telemetry->saveTelemetryToRAM->setVisible(false); + } - UAVObjectUtilManager* utilMngr = pm->getObject(); + UAVObjectUtilManager *utilMngr = pm->getObject(); int id = utilMngr->getBoardModel(); switch (id) { @@ -73,16 +74,16 @@ ConfigCCHWWidget::ConfigCCHWWidget(QWidget *parent) : ConfigTaskWidget(parent) m_telemetry->label_2->setPixmap(QPixmap(":/configgadget/images/coptercontrol.svg")); break; } - addApplySaveButtons(m_telemetry->saveTelemetryToRAM,m_telemetry->saveTelemetryToSD); - addUAVObjectToWidgetRelation("HwSettings","CC_FlexiPort",m_telemetry->cbFlexi); - addUAVObjectToWidgetRelation("HwSettings","CC_MainPort",m_telemetry->cbTele); - addUAVObjectToWidgetRelation("HwSettings","CC_RcvrPort",m_telemetry->cbRcvr); - addUAVObjectToWidgetRelation("HwSettings","USB_HIDPort",m_telemetry->cbUsbHid); - addUAVObjectToWidgetRelation("HwSettings","USB_VCPPort",m_telemetry->cbUsbVcp); - addUAVObjectToWidgetRelation("HwSettings","TelemetrySpeed",m_telemetry->telemetrySpeed); - addUAVObjectToWidgetRelation("HwSettings","GPSSpeed",m_telemetry->gpsSpeed); - addUAVObjectToWidgetRelation("HwSettings","ComUsbBridgeSpeed",m_telemetry->comUsbBridgeSpeed); - connect(m_telemetry->cchwHelp,SIGNAL(clicked()),this,SLOT(openHelp())); + addApplySaveButtons(m_telemetry->saveTelemetryToRAM, m_telemetry->saveTelemetryToSD); + addUAVObjectToWidgetRelation("HwSettings", "CC_FlexiPort", m_telemetry->cbFlexi); + addUAVObjectToWidgetRelation("HwSettings", "CC_MainPort", m_telemetry->cbTele); + addUAVObjectToWidgetRelation("HwSettings", "CC_RcvrPort", m_telemetry->cbRcvr); + addUAVObjectToWidgetRelation("HwSettings", "USB_HIDPort", m_telemetry->cbUsbHid); + addUAVObjectToWidgetRelation("HwSettings", "USB_VCPPort", m_telemetry->cbUsbVcp); + addUAVObjectToWidgetRelation("HwSettings", "TelemetrySpeed", m_telemetry->telemetrySpeed); + addUAVObjectToWidgetRelation("HwSettings", "GPSSpeed", m_telemetry->gpsSpeed); + addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_telemetry->comUsbBridgeSpeed); + connect(m_telemetry->cchwHelp, SIGNAL(clicked()), this, SLOT(openHelp())); enableControls(false); populateWidgets(); refreshWidgetsValues(); @@ -95,42 +96,33 @@ ConfigCCHWWidget::~ConfigCCHWWidget() } void ConfigCCHWWidget::refreshValues() -{ -} +{} void ConfigCCHWWidget::widgetsContentsChanged() { ConfigTaskWidget::widgetsContentsChanged(); + if (((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE) && - (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE)) || - ((m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE) && - (m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE)) || - ((m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) && - (m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE))) - { + (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE)) || + ((m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE) && + (m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE)) || + ((m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) && + (m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE))) { enableControls(false); m_telemetry->problems->setText(tr("Warning: you have configured more than one DebugConsole, this currently is not supported")); - } - else if (((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_TELEMETRY) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_TELEMETRY)) || - ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_GPS) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_GPS)) || - ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE)) || - ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_COMBRIDGE) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_COMBRIDGE))) - { + } else if (((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_TELEMETRY) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_TELEMETRY)) || + ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_GPS) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_GPS)) || + ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE)) || + ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_COMBRIDGE) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_COMBRIDGE))) { enableControls(false); m_telemetry->problems->setText(tr("Warning: you have configured both MainPort and FlexiPort for the same function, this currently is not supported")); - } - else if ((m_telemetry->cbUsbHid->currentIndex() == HwSettings::USB_HIDPORT_USBTELEMETRY) && (m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_USBTELEMETRY)) - { + } else if ((m_telemetry->cbUsbHid->currentIndex() == HwSettings::USB_HIDPORT_USBTELEMETRY) && (m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_USBTELEMETRY)) { enableControls(false); m_telemetry->problems->setText(tr("Warning: you have configured both USB HID Port and USB VCP Port for the same function, this currently is not supported")); - } - else if ((m_telemetry->cbUsbHid->currentIndex() != HwSettings::USB_HIDPORT_USBTELEMETRY) && (m_telemetry->cbUsbVcp->currentIndex() != HwSettings::USB_VCPPORT_USBTELEMETRY)) - { + } else if ((m_telemetry->cbUsbHid->currentIndex() != HwSettings::USB_HIDPORT_USBTELEMETRY) && (m_telemetry->cbUsbVcp->currentIndex() != HwSettings::USB_VCPPORT_USBTELEMETRY)) { enableControls(false); m_telemetry->problems->setText(tr("Warning: you have disabled USB Telemetry on both USB HID Port and USB VCP Port, this currently is not supported")); - } - else - { + } else { m_telemetry->problems->setText(""); enableControls(true); } @@ -138,10 +130,10 @@ void ConfigCCHWWidget::widgetsContentsChanged() void ConfigCCHWWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/D4AUAQ", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/D4AUAQ", QUrl::StrictMode)); } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h index 7c4945281..503955f0b 100644 --- a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h @@ -36,8 +36,7 @@ #include #include "smartsavebutton.h" -class ConfigCCHWWidget: public ConfigTaskWidget -{ +class ConfigCCHWWidget : public ConfigTaskWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp index 63de056ad..e395aaecf 100644 --- a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp @@ -1,4 +1,3 @@ - #include "configautotunewidget.h" #include @@ -34,22 +33,25 @@ ConfigAutotuneWidget::ConfigAutotuneWidget(QWidget *parent) : RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager()); Q_ASSERT(relayTuning); - if(relayTuning) - connect(relayTuning, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(recomputeStabilization())); + if (relayTuning) { + connect(relayTuning, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(recomputeStabilization())); + } // Connect the apply button for the stabilization settings connect(m_autotune->useComputedValues, SIGNAL(pressed()), this, SLOT(saveStabilization())); } /** - * Apply the stabilization settings computed - */ + * Apply the stabilization settings computed + */ void ConfigAutotuneWidget::saveStabilization() { StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager()); + Q_ASSERT(stabilizationSettings); - if(!stabilizationSettings) + if (!stabilizationSettings) { return; + } // Make sure to recompute in case the other stab settings changed since // the last time @@ -61,25 +63,29 @@ void ConfigAutotuneWidget::saveStabilization() } /** - * Called whenever the gain ratios or measured values - * are changed - */ + * Called whenever the gain ratios or measured values + * are changed + */ void ConfigAutotuneWidget::recomputeStabilization() { RelayTuningSettings *relayTuningSettings = RelayTuningSettings::GetInstance(getObjectManager()); + Q_ASSERT(relayTuningSettings); - if (!relayTuningSettings) + if (!relayTuningSettings) { return; + } RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager()); Q_ASSERT(relayTuning); - if(!relayTuning) + if (!relayTuning) { return; + } StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager()); Q_ASSERT(stabilizationSettings); - if(!stabilizationSettings) + if (!stabilizationSettings) { return; + } RelayTuning::DataFields relayTuningData = relayTuning->getData(); RelayTuningSettings::DataFields tuningSettingsData = relayTuningSettings->getData(); @@ -94,24 +100,25 @@ void ConfigAutotuneWidget::recomputeStabilization() // For now just run over roll and pitch for (int i = 0; i < 2; i++) { - if (relayTuningData.Period[i] == 0 || relayTuningData.Gain[i] == 0) + if (relayTuningData.Period[i] == 0 || relayTuningData.Gain[i] == 0) { continue; + } - double wu = 1000.0 * 2 * M_PI / relayTuningData.Period[i]; // ultimate freq = output osc freq (rad/s) + double wu = 1000.0 * 2 * M_PI / relayTuningData.Period[i]; // ultimate freq = output osc freq (rad/s) - double wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s) - double zc = wc * zero_ratio_r; // controller zero location (rad/s) - double kpu = 4.0f / M_PI / relayTuningData.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity - double kp = kpu * gain_ratio_r; // proportional gain - double ki = zc * kp; // integral gain + double wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s) + double zc = wc * zero_ratio_r; // controller zero location (rad/s) + double kpu = 4.0f / M_PI / relayTuningData.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity + double kp = kpu * gain_ratio_r; // proportional gain + double ki = zc * kp; // integral gain // Now calculate gains for the next loop out knowing it is the integral of // the inner loop -- the plant is position/velocity = scale*1/s - double wc2 = wc * gain_ratio_p; // crossover of the attitude loop - double kp2 = wc2; // kp of attitude - double ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude + double wc2 = wc * gain_ratio_p; // crossover of the attitude loop + double kp2 = wc2; // kp of attitude + double ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude - switch(i) { + switch (i) { case 0: // Roll stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KP] = kp; @@ -141,9 +148,9 @@ void ConfigAutotuneWidget::recomputeStabilization() void ConfigAutotuneWidget::refreshWidgetsValues(UAVObject *obj) { HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); - if(obj==hwSettings) - { - bool dirtyBack=isDirty(); + + if (obj == hwSettings) { + bool dirtyBack = isDirty(); HwSettings::DataFields hwSettingsData = hwSettings->getData(); m_autotune->enableAutoTune->setChecked( hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_AUTOTUNE] == HwSettings::OPTIONALMODULES_ENABLED); @@ -155,8 +162,9 @@ void ConfigAutotuneWidget::updateObjectsFromWidgets() { HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); HwSettings::DataFields hwSettingsData = hwSettings->getData(); + hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_AUTOTUNE] = - m_autotune->enableAutoTune->isChecked() ? HwSettings::OPTIONALMODULES_ENABLED : HwSettings::OPTIONALMODULES_DISABLED; + m_autotune->enableAutoTune->isChecked() ? HwSettings::OPTIONALMODULES_ENABLED : HwSettings::OPTIONALMODULES_DISABLED; hwSettings->setData(hwSettingsData); ConfigTaskWidget::updateObjectsFromWidgets(); } diff --git a/ground/openpilotgcs/src/plugins/config/configautotunewidget.h b/ground/openpilotgcs/src/plugins/config/configautotunewidget.h index 466335d04..777c24b3f 100644 --- a/ground/openpilotgcs/src/plugins/config/configautotunewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.h @@ -38,8 +38,7 @@ #include #include -class ConfigAutotuneWidget : public ConfigTaskWidget -{ +class ConfigAutotuneWidget : public ConfigTaskWidget { Q_OBJECT public: explicit ConfigAutotuneWidget(QWidget *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp index 4b8e445e5..829a40608 100644 --- a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp @@ -45,15 +45,15 @@ ConfigCameraStabilizationWidget::ConfigCameraStabilizationWidget(QWidget *parent { ui = new Ui_CameraStabilizationWidget(); ui->setupUi(this); - - addApplySaveButtons(ui->camerastabilizationSaveRAM,ui->camerastabilizationSaveSD); - - ExtensionSystem::PluginManager *pm=ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); - if(!settings->useExpertMode()) + + addApplySaveButtons(ui->camerastabilizationSaveRAM, ui->camerastabilizationSaveSD); + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { ui->camerastabilizationSaveRAM->setVisible(false); - - + } + // These widgets don't have direct relation to UAVObjects // and need special processing @@ -61,7 +61,6 @@ ConfigCameraStabilizationWidget::ConfigCameraStabilizationWidget(QWidget *parent ui->rollChannel, ui->pitchChannel, ui->yawChannel, - }; const int NUM_OUTPUTS = sizeof(outputs) / sizeof(outputs[0]); @@ -69,8 +68,9 @@ ConfigCameraStabilizationWidget::ConfigCameraStabilizationWidget(QWidget *parent for (int i = 0; i < NUM_OUTPUTS; i++) { outputs[i]->clear(); outputs[i]->addItem("None"); - for (quint32 j = 0; j < ActuatorCommand::CHANNEL_NUMELEM; j++) - outputs[i]->addItem(QString("Channel %1").arg(j+1)); + for (quint32 j = 0; j < ActuatorCommand::CHANNEL_NUMELEM; j++) { + outputs[i]->addItem(QString("Channel %1").arg(j + 1)); + } } // Load UAVObjects to widget relations from UI file @@ -102,7 +102,7 @@ ConfigCameraStabilizationWidget::ConfigCameraStabilizationWidget(QWidget *parent ConfigCameraStabilizationWidget::~ConfigCameraStabilizationWidget() { - // Do nothing + // Do nothing } /* @@ -155,10 +155,12 @@ void ConfigCameraStabilizationWidget::refreshWidgetsValues(UAVObject *obj) // Default to none if not found. // Then search for any mixer channels set to this outputs[i]->setCurrentIndex(0); - for (int j = 0; j < NUM_MIXERS; j++) + for (int j = 0; j < NUM_MIXERS; j++) { if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i) && - outputs[i]->currentIndex() != (j + 1)) + outputs[i]->currentIndex() != (j + 1)) { outputs[i]->setCurrentIndex(j + 1); + } + } } setDirty(dirty); @@ -176,9 +178,10 @@ void ConfigCameraStabilizationWidget::updateObjectsFromWidgets() // Save state of the module enable checkbox first. // Do not use setData() member on whole object, if possible, since it triggers // unnessesary UAVObect update. - quint8 enableModule = ui->enableCameraStabilization->isChecked() ? - HwSettings::OPTIONALMODULES_ENABLED : HwSettings::OPTIONALMODULES_DISABLED; + quint8 enableModule = ui->enableCameraStabilization->isChecked() ? + HwSettings::OPTIONALMODULES_ENABLED : HwSettings::OPTIONALMODULES_DISABLED; HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); + hwSettings->setOptionalModules(HwSettings::OPTIONALMODULES_CAMERASTAB, enableModule); // Update mixer channels which were mapped to camera outputs in case they are @@ -220,7 +223,7 @@ void ConfigCameraStabilizationWidget::updateObjectsFromWidgets() if ((mixerNum >= 0) && // Short circuit in case of none (*mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_DISABLED) && - (*mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i) ) { + (*mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i)) { // If the mixer channel already mapped to something, it should not be // used for camera output, we reset it to none outputs[i]->setCurrentIndex(0); @@ -230,17 +233,20 @@ void ConfigCameraStabilizationWidget::updateObjectsFromWidgets() widgetUpdated = true; } else { // Make sure no other channels have this output set - for (int j = 0; j < NUM_MIXERS; j++) - if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i)) + for (int j = 0; j < NUM_MIXERS; j++) { + if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i)) { *mixerTypes[j] = MixerSettings::MIXER1TYPE_DISABLED; + } + } // If this channel is assigned to one of the outputs that is not disabled // set it - if ((mixerNum >= 0) && (mixerNum < NUM_MIXERS)) + if ((mixerNum >= 0) && (mixerNum < NUM_MIXERS)) { *mixerTypes[mixerNum] = MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i; + } } } - } while(widgetUpdated); + } while (widgetUpdated); // FIXME: Should not use setData() to prevent double updates. // It should be refactored after the reformatting of MixerSettings UAVObject. @@ -263,11 +269,11 @@ void ConfigCameraStabilizationWidget::defaultRequestedSlot(int group) // But if you want, you could use the dirtyClone() function to get default // values of an object and then use them to set a widget state. // - //HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); - //HwSettings *hwSettingsDefault=(HwSettings*)hwSettings->dirtyClone(); - //HwSettings::DataFields hwSettingsData = hwSettingsDefault->getData(); - //m_camerastabilization->enableCameraStabilization->setChecked( - // hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_CAMERASTAB] == HwSettings::OPTIONALMODULES_ENABLED); + // HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); + // HwSettings *hwSettingsDefault=(HwSettings*)hwSettings->dirtyClone(); + // HwSettings::DataFields hwSettingsData = hwSettingsDefault->getData(); + // m_camerastabilization->enableCameraStabilization->setChecked( + // hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_CAMERASTAB] == HwSettings::OPTIONALMODULES_ENABLED); // For outputs we set them all to none, so don't use any UAVObject to get defaults QComboBox *outputs[] = { diff --git a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h index 2715daa77..8c656f8ff 100644 --- a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h @@ -34,8 +34,7 @@ #include "uavobject.h" #include "camerastabsettings.h" -class ConfigCameraStabilizationWidget: public ConfigTaskWidget -{ +class ConfigCameraStabilizationWidget : public ConfigTaskWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index cd840e93e..79d69382e 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -39,30 +39,31 @@ #include ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) : - ConfigTaskWidget(parent), - ui(new Ui_ccattitude) + ConfigTaskWidget(parent), + ui(new Ui_ccattitude) { ui->setupUi(this); - forceConnectedState(); //dynamic widgets don't recieve the connected signal - connect(ui->zeroBias,SIGNAL(clicked()),this,SLOT(startAccelCalibration())); + forceConnectedState(); // dynamic widgets don't recieve the connected signal + connect(ui->zeroBias, SIGNAL(clicked()), this, SLOT(startAccelCalibration())); - ExtensionSystem::PluginManager *pm=ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); - if(!settings->useExpertMode()) + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { ui->applyButton->setVisible(false); - - addApplySaveButtons(ui->applyButton,ui->saveButton); + } + + addApplySaveButtons(ui->applyButton, ui->saveButton); addUAVObject("AttitudeSettings"); // Connect the help button connect(ui->ccAttitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp())); - addUAVObjectToWidgetRelation("AttitudeSettings","ZeroDuringArming",ui->zeroGyroBiasOnArming); + addUAVObjectToWidgetRelation("AttitudeSettings", "ZeroDuringArming", ui->zeroGyroBiasOnArming); addUAVObjectToWidgetRelation("AttitudeSettings", "AccelTau", ui->accelTauSpinbox); - addUAVObjectToWidgetRelation("AttitudeSettings","BoardRotation",ui->rollBias,AttitudeSettings::BOARDROTATION_ROLL); - addUAVObjectToWidgetRelation("AttitudeSettings","BoardRotation",ui->pitchBias,AttitudeSettings::BOARDROTATION_PITCH); - addUAVObjectToWidgetRelation("AttitudeSettings","BoardRotation",ui->yawBias,AttitudeSettings::BOARDROTATION_YAW); + addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", ui->rollBias, AttitudeSettings::BOARDROTATION_ROLL); + addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", ui->pitchBias, AttitudeSettings::BOARDROTATION_PITCH); + addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", ui->yawBias, AttitudeSettings::BOARDROTATION_YAW); addWidget(ui->zeroBias); refreshWidgetsValues(); } @@ -72,22 +73,22 @@ ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget() delete ui; } -void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { - - if (!timer.isActive()) { +void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj) +{ + if (!timer.isActive()) { // ignore updates that come in after the timer has expired return; } - Accels * accels = Accels::GetInstance(getObjectManager()); - Gyros * gyros = Gyros::GetInstance(getObjectManager()); + Accels *accels = Accels::GetInstance(getObjectManager()); + Gyros *gyros = Gyros::GetInstance(getObjectManager()); // Accumulate samples until we have _at least_ NUM_SENSOR_UPDATES samples // for both gyros and accels. // Note that, at present, we stash the samples and then compute the bias // at the end, even though the mean could be accumulated as we go. - // In future, a better algorithm could be used. - if(obj->getObjID() == Accels::OBJID) { + // In future, a better algorithm could be used. + if (obj->getObjID() == Accels::OBJID) { accelUpdates++; Accels::DataFields accelsData = accels->getData(); x_accum.append(accelsData.x); @@ -99,16 +100,16 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { x_gyro_accum.append(gyrosData.x); y_gyro_accum.append(gyrosData.y); z_gyro_accum.append(gyrosData.z); - } + } // update the progress indicator - ui->zeroBiasProgress->setValue((float) qMin(accelUpdates, gyroUpdates) / NUM_SENSOR_UPDATES * 100); + ui->zeroBiasProgress->setValue((float)qMin(accelUpdates, gyroUpdates) / NUM_SENSOR_UPDATES * 100); // If we have enough samples, then stop sampling and compute the biases if (accelUpdates >= NUM_SENSOR_UPDATES && gyroUpdates >= NUM_SENSOR_UPDATES) { timer.stop(); - disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); - disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); + disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); + disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); float x_bias = listMean(x_accum) / ACCEL_SCALE; float y_bias = listMean(y_accum) / ACCEL_SCALE; @@ -122,12 +123,12 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData(); // We offset the gyro bias by current bias to help precision - attitudeSettingsData.AccelBias[0] += x_bias; - attitudeSettingsData.AccelBias[1] += y_bias; - attitudeSettingsData.AccelBias[2] += z_bias; - attitudeSettingsData.GyroBias[0] = -x_gyro_bias; - attitudeSettingsData.GyroBias[1] = -y_gyro_bias; - attitudeSettingsData.GyroBias[2] = -z_gyro_bias; + attitudeSettingsData.AccelBias[0] += x_bias; + attitudeSettingsData.AccelBias[1] += y_bias; + attitudeSettingsData.AccelBias[2] += z_bias; + attitudeSettingsData.GyroBias[0] = -x_gyro_bias; + attitudeSettingsData.GyroBias[1] = -y_gyro_bias; + attitudeSettingsData.GyroBias[2] = -z_gyro_bias; attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); this->setDirty(true); @@ -137,13 +138,15 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { } } -void ConfigCCAttitudeWidget::timeout() { - UAVDataObject * obj = Accels::GetInstance(getObjectManager()); - disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); - disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); +void ConfigCCAttitudeWidget::timeout() +{ + UAVDataObject *obj = Accels::GetInstance(getObjectManager()); - Accels * accels = Accels::GetInstance(getObjectManager()); - Gyros * gyros = Gyros::GetInstance(getObjectManager()); + disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); + disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); + + Accels *accels = Accels::GetInstance(getObjectManager()); + Gyros *gyros = Gyros::GetInstance(getObjectManager()); accels->setMetadata(initialAccelsMdata); gyros->setMetadata(initialGyrosMdata); @@ -154,17 +157,18 @@ void ConfigCCAttitudeWidget::timeout() { msgBox.exec(); // reset progress indicator - ui->zeroBiasProgress->setValue(0); + ui->zeroBiasProgress->setValue(0); // reenable controls enableControls(true); } -void ConfigCCAttitudeWidget::startAccelCalibration() { +void ConfigCCAttitudeWidget::startAccelCalibration() +{ // disable controls during sampling enableControls(false); accelUpdates = 0; - gyroUpdates = 0; + gyroUpdates = 0; x_accum.clear(); y_accum.clear(); z_accum.clear(); @@ -178,10 +182,10 @@ void ConfigCCAttitudeWidget::startAccelCalibration() { AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); // Set up to receive updates - UAVDataObject * accels = Accels::GetInstance(getObjectManager()); - UAVDataObject * gyros = Gyros::GetInstance(getObjectManager()); - connect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); - connect(gyros,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); + UAVDataObject *accels = Accels::GetInstance(getObjectManager()); + UAVDataObject *gyros = Gyros::GetInstance(getObjectManager()); + connect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); + connect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); // Speed up updates initialAccelsMdata = accels->getMetadata(); @@ -200,13 +204,12 @@ void ConfigCCAttitudeWidget::startAccelCalibration() { timer.setSingleShot(true); timer.start(5000 + (NUM_SENSOR_UPDATES * qMax(accelsMdata.flightTelemetryUpdatePeriod, gyrosMdata.flightTelemetryUpdatePeriod))); - connect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); + connect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); } void ConfigCCAttitudeWidget::openHelp() { - - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/44Cf", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/44Cf", QUrl::StrictMode)); } void ConfigCCAttitudeWidget::setAccelFiltering(bool active) @@ -225,5 +228,6 @@ void ConfigCCAttitudeWidget::enableControls(bool enable) void ConfigCCAttitudeWidget::updateObjectsFromWidgets() { ConfigTaskWidget::updateObjectsFromWidgets(); + ui->zeroBiasProgress->setValue(0); } diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h index b171035d1..c9b835050 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h @@ -37,8 +37,7 @@ class Ui_Widget; -class ConfigCCAttitudeWidget : public ConfigTaskWidget -{ +class ConfigCCAttitudeWidget : public ConfigTaskWidget { Q_OBJECT public: @@ -48,7 +47,7 @@ public: virtual void updateObjectsFromWidgets(); private slots: - void sensorsUpdated(UAVObject * obj); + void sensorsUpdated(UAVObject *obj); void timeout(); void startAccelCalibration(); void openHelp(); @@ -71,7 +70,6 @@ private: static const float ACCEL_SCALE = 0.004f * 9.81f; protected: virtual void enableControls(bool enable); - }; #endif // CCATTITUDEWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configgadget.cpp b/ground/openpilotgcs/src/plugins/config/configgadget.cpp index dc1ca4721..013309e27 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadget.cpp @@ -28,9 +28,8 @@ #include "configgadgetwidget.h" ConfigGadget::ConfigGadget(QString classId, ConfigGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), m_widget(widget) -{ -} + IUAVGadget(classId, parent), m_widget(widget) +{} ConfigGadget::~ConfigGadget() { @@ -39,5 +38,5 @@ ConfigGadget::~ConfigGadget() void ConfigGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - Q_UNUSED(config); + Q_UNUSED(config); } diff --git a/ground/openpilotgcs/src/plugins/config/configgadget.h b/ground/openpilotgcs/src/plugins/config/configgadget.h index 6e617000b..23184b74e 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadget.h +++ b/ground/openpilotgcs/src/plugins/config/configgadget.h @@ -31,7 +31,7 @@ #include "../uavobjectwidgetutils/configtaskwidget.h" class IUAVGadget; -//class QList; +// class QList; class QWidget; class QString; class ConfigGadgetWidget; @@ -39,15 +39,17 @@ class Ui_ConfigGadget; using namespace Core; -class ConfigGadget : public Core::IUAVGadget -{ +class ConfigGadget : public Core::IUAVGadget { Q_OBJECT public: ConfigGadget(QString classId, ConfigGadgetWidget *widget, QWidget *parent = 0); ~ConfigGadget(); - QWidget *widget() { return (QWidget*)m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return (QWidget *)m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: ConfigGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetfactory.cpp index aef634008..a9add1da0 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetfactory.cpp @@ -34,23 +34,21 @@ ConfigGadgetFactory::ConfigGadgetFactory(QObject *parent) : IUAVGadgetFactory(QString("ConfigGadget"), tr("Config"), parent), gadgetWidget(0) -{ -} +{} ConfigGadgetFactory::~ConfigGadgetFactory() -{ -} +{} Core::IUAVGadget *ConfigGadgetFactory::createGadget(QWidget *parent) { gadgetWidget = new ConfigGadgetWidget(parent); // Add Menu entry - Core::ActionManager *am = Core::ICore::instance()->actionManager(); + Core::ActionManager *am = Core::ICore::instance()->actionManager(); Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_TOOLS); Core::Command *cmd = am->registerAction(new QAction(this), "ConfigPlugin.ShowInputWizard", - QList() << Core::Constants::C_GLOBAL_ID); + QList() << Core::Constants::C_GLOBAL_ID); cmd->setDefaultKeySequence(QKeySequence("Ctrl+R")); cmd->action()->setText(tr("Radio Setup Wizard")); diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetfactory.h b/ground/openpilotgcs/src/plugins/config/configgadgetfactory.h index a6bd08de6..936cb41b4 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetfactory.h @@ -38,9 +38,8 @@ class IUAVGadgetFactory; using namespace Core; -class CONFIG_EXPORT ConfigGadgetFactory: public Core::IUAVGadgetFactory -{ - Q_OBJECT +class CONFIG_EXPORT ConfigGadgetFactory : public Core::IUAVGadgetFactory { + Q_OBJECT public: ConfigGadgetFactory(QObject *parent = 0); ~ConfigGadgetFactory(); diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index 5f2f268ba..db3900e69 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -192,7 +192,7 @@ void ConfigGadgetWidget::onAutopilotConnect() // First of all, check what Board type we are talking to, and // if necessary, remove/add tabs in the config gadget: ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectUtilManager *utilMngr = pm->getObject(); + UAVObjectUtilManager *utilMngr = pm->getObject(); if (utilMngr) { int selectedIndex = ftw->currentIndex(); int board = utilMngr->getBoardModel(); diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h index 24209e1cf..3827f7e7e 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h @@ -40,21 +40,20 @@ #include "utils/mytabbedstackwidget.h" #include "../uavobjectwidgetutils/configtaskwidget.h" -class ConfigGadgetWidget: public QWidget -{ +class ConfigGadgetWidget : public QWidget { Q_OBJECT - QTextBrowser* help; + QTextBrowser *help; public: ConfigGadgetWidget(QWidget *parent = 0); ~ConfigGadgetWidget(); - enum widgetTabs {hardware=0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, oplink}; + enum widgetTabs { hardware = 0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, oplink }; void startInputWizard(); public slots: void onAutopilotConnect(); void onAutopilotDisconnect(); - void tabAboutToChange(int i,bool *); + void tabAboutToChange(int i, bool *); void updateOPLinkStatus(UAVObject *object); void onOPLinkDisconnect(); @@ -65,15 +64,15 @@ signals: void oplinkDisconnect(); protected: - void resizeEvent(QResizeEvent * event); - MyTabbedStackWidget *ftw; + void resizeEvent(QResizeEvent *event); + MyTabbedStackWidget *ftw; private: - UAVDataObject* oplinkStatusObj; + UAVDataObject *oplinkStatusObj; - // A timer that timesout the connction to the OPLink. - QTimer *oplinkTimeout; - bool oplinkConnected; + // A timer that timesout the connction to the OPLink. + QTimer *oplinkTimeout; + bool oplinkConnected; }; #endif // CONFIGGADGETWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 505c3a125..646130d9b 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -46,49 +46,49 @@ #define ACCESS_MIN_MOVE -3 #define ACCESS_MAX_MOVE 3 -#define STICK_MIN_MOVE -8 -#define STICK_MAX_MOVE 8 +#define STICK_MIN_MOVE -8 +#define STICK_MAX_MOVE 8 -ConfigInputWidget::ConfigInputWidget(QWidget *parent) : +ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent), wizardStep(wizardNone), // not currently stored in the settings UAVO - transmitterMode(mode2), - transmitterType(acro), + transmitterMode(mode2), + transmitterType(acro), // loop(NULL), skipflag(false) { - manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); - manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); - flightStatusObj = FlightStatus::GetInstance(getObjectManager()); - receiverActivityObj=ReceiverActivity::GetInstance(getObjectManager()); + manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); + manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); + flightStatusObj = FlightStatus::GetInstance(getObjectManager()); + receiverActivityObj = ReceiverActivity::GetInstance(getObjectManager()); ui = new Ui_InputWidget(); ui->setupUi(this); - - addApplySaveButtons(ui->saveRCInputToRAM,ui->saveRCInputToSD); - ExtensionSystem::PluginManager *pm=ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); - if(!settings->useExpertMode()) + addApplySaveButtons(ui->saveRCInputToRAM, ui->saveRCInputToSD); + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { ui->saveRCInputToRAM->setVisible(false); - - addApplySaveButtons(ui->saveRCInputToRAM,ui->saveRCInputToSD); + } - //Generate the rows of buttons in the input channel form GUI - unsigned int index=0; + addApplySaveButtons(ui->saveRCInputToRAM, ui->saveRCInputToSD); + + // Generate the rows of buttons in the input channel form GUI + unsigned int index = 0; unsigned int indexRT = 0; - foreach (QString name, manualSettingsObj->getField("ChannelNumber")->getElementNames()) - { + foreach(QString name, manualSettingsObj->getField("ChannelNumber")->getElementNames()) { Q_ASSERT(index < ManualControlSettings::CHANNELGROUPS_NUMELEM); - inputChannelForm * inpForm=new inputChannelForm(this,index==0); - ui->channelSettings->layout()->addWidget(inpForm); //Add the row to the UI + inputChannelForm *inpForm = new inputChannelForm(this, index == 0); + ui->channelSettings->layout()->addWidget(inpForm); // Add the row to the UI inpForm->setName(name); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelGroups",inpForm->ui->channelGroup,index); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNumber",inpForm->ui->channelNumber,index); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMin",inpForm->ui->channelMin,index); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNeutral",inpForm->ui->channelNeutral,index); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMax",inpForm->ui->channelMax,index); + addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelGroups", inpForm->ui->channelGroup, index); + addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelNumber", inpForm->ui->channelNumber, index); + addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelMin", inpForm->ui->channelMin, index); + addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelNeutral", inpForm->ui->channelNeutral, index); + addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelMax", inpForm->ui->channelMax, index); addWidget(inpForm->ui->channelNumberDropdown); addWidget(inpForm->ui->channelRev); addWidget(inpForm->ui->channelResponseTime); @@ -119,13 +119,13 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : addUAVObjectToWidgetRelation("ManualControlSettings", "Deadband", ui->deadband, 0, 0.01f); - connect(ui->configurationWizard,SIGNAL(clicked()),this,SLOT(goToWizard())); - connect(ui->stackedWidget,SIGNAL(currentChanged(int)),this,SLOT(disableWizardButton(int))); - connect(ui->runCalibration,SIGNAL(toggled(bool)),this, SLOT(simpleCalibration(bool))); + connect(ui->configurationWizard, SIGNAL(clicked()), this, SLOT(goToWizard())); + connect(ui->stackedWidget, SIGNAL(currentChanged(int)), this, SLOT(disableWizardButton(int))); + connect(ui->runCalibration, SIGNAL(toggled(bool)), this, SLOT(simpleCalibration(bool))); - connect(ui->wzNext,SIGNAL(clicked()),this,SLOT(wzNext())); - connect(ui->wzCancel,SIGNAL(clicked()),this,SLOT(wzCancel())); - connect(ui->wzBack,SIGNAL(clicked()),this,SLOT(wzBack())); + connect(ui->wzNext, SIGNAL(clicked()), this, SLOT(wzNext())); + connect(ui->wzCancel, SIGNAL(clicked()), this, SLOT(wzCancel())); + connect(ui->wzBack, SIGNAL(clicked()), this, SLOT(wzBack())); ui->stackedWidget->setCurrentIndex(0); addUAVObjectToWidgetRelation("ManualControlSettings", "FlightModePosition", ui->fmsModePos1, 0, 1, true); @@ -146,10 +146,10 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : addUAVObjectToWidgetRelation("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Yaw, "Yaw", 1, true); addUAVObjectToWidgetRelation("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Yaw, "Yaw", 1, true); - addUAVObjectToWidgetRelation("ManualControlSettings","Arming",ui->armControl); - addUAVObjectToWidgetRelation("ManualControlSettings","ArmedTimeout",ui->armTimeout,0,1000); - connect( ManualControlCommand::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(moveFMSlider())); - connect( ManualControlSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updatePositionSlider())); + addUAVObjectToWidgetRelation("ManualControlSettings", "Arming", ui->armControl); + addUAVObjectToWidgetRelation("ManualControlSettings", "ArmedTimeout", ui->armTimeout, 0, 1000); + connect(ManualControlCommand::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveFMSlider())); + connect(ManualControlSettings::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updatePositionSlider())); addWidget(ui->configurationWizard); addWidget(ui->runCalibration); @@ -164,13 +164,12 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : m_renderer = new QSvgRenderer(); QGraphicsScene *l_scene = ui->graphicsView->scene(); ui->graphicsView->setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); - if (QFile::exists(":/configgadget/images/TX2.svg") && m_renderer->load(QString(":/configgadget/images/TX2.svg")) && m_renderer->isValid()) - { + if (QFile::exists(":/configgadget/images/TX2.svg") && m_renderer->load(QString(":/configgadget/images/TX2.svg")) && m_renderer->isValid()) { l_scene->clear(); // Deletes all items contained in the scene as well. m_txBackground = new QGraphicsSvgItem(); // All other items will be clipped to the shape of the background - m_txBackground->setFlags(QGraphicsItem::ItemClipsChildrenToShape| + m_txBackground->setFlags(QGraphicsItem::ItemClipsChildrenToShape | QGraphicsItem::ItemClipsToShape); m_txBackground->setSharedRenderer(m_renderer); m_txBackground->setElementId("background"); @@ -219,112 +218,109 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : m_txArrows->setElementId("arrows"); m_txArrows->setVisible(false); - QRectF orig=m_renderer->boundsOnElement("ljoy"); + QRectF orig = m_renderer->boundsOnElement("ljoy"); QMatrix Matrix = m_renderer->matrixForElement("ljoy"); - orig=Matrix.mapRect(orig); - m_txLeftStickOrig.translate(orig.x(),orig.y()); - m_txLeftStick->setTransform(m_txLeftStickOrig,false); + orig = Matrix.mapRect(orig); + m_txLeftStickOrig.translate(orig.x(), orig.y()); + m_txLeftStick->setTransform(m_txLeftStickOrig, false); - orig=m_renderer->boundsOnElement("arrows"); + orig = m_renderer->boundsOnElement("arrows"); Matrix = m_renderer->matrixForElement("arrows"); - orig=Matrix.mapRect(orig); - m_txArrowsOrig.translate(orig.x(),orig.y()); - m_txArrows->setTransform(m_txArrowsOrig,false); + orig = Matrix.mapRect(orig); + m_txArrowsOrig.translate(orig.x(), orig.y()); + m_txArrows->setTransform(m_txArrowsOrig, false); - orig=m_renderer->boundsOnElement("body"); + orig = m_renderer->boundsOnElement("body"); Matrix = m_renderer->matrixForElement("body"); - orig=Matrix.mapRect(orig); - m_txMainBodyOrig.translate(orig.x(),orig.y()); - m_txMainBody->setTransform(m_txMainBodyOrig,false); + orig = Matrix.mapRect(orig); + m_txMainBodyOrig.translate(orig.x(), orig.y()); + m_txMainBody->setTransform(m_txMainBodyOrig, false); - orig=m_renderer->boundsOnElement("flightModeCenter"); + orig = m_renderer->boundsOnElement("flightModeCenter"); Matrix = m_renderer->matrixForElement("flightModeCenter"); - orig=Matrix.mapRect(orig); - m_txFlightModeCOrig.translate(orig.x(),orig.y()); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + orig = Matrix.mapRect(orig); + m_txFlightModeCOrig.translate(orig.x(), orig.y()); + m_txFlightMode->setTransform(m_txFlightModeCOrig, false); - orig=m_renderer->boundsOnElement("flightModeLeft"); + orig = m_renderer->boundsOnElement("flightModeLeft"); Matrix = m_renderer->matrixForElement("flightModeLeft"); - orig=Matrix.mapRect(orig); - m_txFlightModeLOrig.translate(orig.x(),orig.y()); - orig=m_renderer->boundsOnElement("flightModeRight"); + orig = Matrix.mapRect(orig); + m_txFlightModeLOrig.translate(orig.x(), orig.y()); + orig = m_renderer->boundsOnElement("flightModeRight"); Matrix = m_renderer->matrixForElement("flightModeRight"); - orig=Matrix.mapRect(orig); - m_txFlightModeROrig.translate(orig.x(),orig.y()); + orig = Matrix.mapRect(orig); + m_txFlightModeROrig.translate(orig.x(), orig.y()); - orig=m_renderer->boundsOnElement("rjoy"); + orig = m_renderer->boundsOnElement("rjoy"); Matrix = m_renderer->matrixForElement("rjoy"); - orig=Matrix.mapRect(orig); - m_txRightStickOrig.translate(orig.x(),orig.y()); - m_txRightStick->setTransform(m_txRightStickOrig,false); + orig = Matrix.mapRect(orig); + m_txRightStickOrig.translate(orig.x(), orig.y()); + m_txRightStick->setTransform(m_txRightStickOrig, false); - orig=m_renderer->boundsOnElement("access0"); + orig = m_renderer->boundsOnElement("access0"); Matrix = m_renderer->matrixForElement("access0"); - orig=Matrix.mapRect(orig); - m_txAccess0Orig.translate(orig.x(),orig.y()); - m_txAccess0->setTransform(m_txAccess0Orig,false); + orig = Matrix.mapRect(orig); + m_txAccess0Orig.translate(orig.x(), orig.y()); + m_txAccess0->setTransform(m_txAccess0Orig, false); - orig=m_renderer->boundsOnElement("access1"); + orig = m_renderer->boundsOnElement("access1"); Matrix = m_renderer->matrixForElement("access1"); - orig=Matrix.mapRect(orig); - m_txAccess1Orig.translate(orig.x(),orig.y()); - m_txAccess1->setTransform(m_txAccess1Orig,false); + orig = Matrix.mapRect(orig); + m_txAccess1Orig.translate(orig.x(), orig.y()); + m_txAccess1->setTransform(m_txAccess1Orig, false); - orig=m_renderer->boundsOnElement("access2"); + orig = m_renderer->boundsOnElement("access2"); Matrix = m_renderer->matrixForElement("access2"); - orig=Matrix.mapRect(orig); - m_txAccess2Orig.translate(orig.x(),orig.y()); - m_txAccess2->setTransform(m_txAccess2Orig,true); + orig = Matrix.mapRect(orig); + m_txAccess2Orig.translate(orig.x(), orig.y()); + m_txAccess2->setTransform(m_txAccess2Orig, true); } - ui->graphicsView->fitInView(m_txMainBody, Qt::KeepAspectRatio ); - animate=new QTimer(this); - connect(animate,SIGNAL(timeout()),this,SLOT(moveTxControls())); + ui->graphicsView->fitInView(m_txMainBody, Qt::KeepAspectRatio); + animate = new QTimer(this); + connect(animate, SIGNAL(timeout()), this, SLOT(moveTxControls())); heliChannelOrder << ManualControlSettings::CHANNELGROUPS_COLLECTIVE << - ManualControlSettings::CHANNELGROUPS_THROTTLE << - ManualControlSettings::CHANNELGROUPS_ROLL << - ManualControlSettings::CHANNELGROUPS_PITCH << - ManualControlSettings::CHANNELGROUPS_YAW << - ManualControlSettings::CHANNELGROUPS_FLIGHTMODE << - ManualControlSettings::CHANNELGROUPS_ACCESSORY0 << - ManualControlSettings::CHANNELGROUPS_ACCESSORY1 << - ManualControlSettings::CHANNELGROUPS_ACCESSORY2; + ManualControlSettings::CHANNELGROUPS_THROTTLE << + ManualControlSettings::CHANNELGROUPS_ROLL << + ManualControlSettings::CHANNELGROUPS_PITCH << + ManualControlSettings::CHANNELGROUPS_YAW << + ManualControlSettings::CHANNELGROUPS_FLIGHTMODE << + ManualControlSettings::CHANNELGROUPS_ACCESSORY0 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY1 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY2; acroChannelOrder << ManualControlSettings::CHANNELGROUPS_THROTTLE << - ManualControlSettings::CHANNELGROUPS_ROLL << - ManualControlSettings::CHANNELGROUPS_PITCH << - ManualControlSettings::CHANNELGROUPS_YAW << - ManualControlSettings::CHANNELGROUPS_FLIGHTMODE << - ManualControlSettings::CHANNELGROUPS_ACCESSORY0 << - ManualControlSettings::CHANNELGROUPS_ACCESSORY1 << - ManualControlSettings::CHANNELGROUPS_ACCESSORY2; + ManualControlSettings::CHANNELGROUPS_ROLL << + ManualControlSettings::CHANNELGROUPS_PITCH << + ManualControlSettings::CHANNELGROUPS_YAW << + ManualControlSettings::CHANNELGROUPS_FLIGHTMODE << + ManualControlSettings::CHANNELGROUPS_ACCESSORY0 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY1 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY2; updateEnableControls(); } void ConfigInputWidget::resetTxControls() { - - m_txLeftStick->setTransform(m_txLeftStickOrig,false); - m_txRightStick->setTransform(m_txRightStickOrig,false); - m_txAccess0->setTransform(m_txAccess0Orig,false); - m_txAccess1->setTransform(m_txAccess1Orig,false); - m_txAccess2->setTransform(m_txAccess2Orig,false); + m_txLeftStick->setTransform(m_txLeftStickOrig, false); + m_txRightStick->setTransform(m_txRightStickOrig, false); + m_txAccess0->setTransform(m_txAccess0Orig, false); + m_txAccess1->setTransform(m_txAccess1Orig, false); + m_txAccess2->setTransform(m_txAccess2Orig, false); m_txFlightMode->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + m_txFlightMode->setTransform(m_txFlightModeCOrig, false); m_txArrows->setVisible(false); } ConfigInputWidget::~ConfigInputWidget() -{ - -} +{} void ConfigInputWidget::enableControls(bool enable) { ConfigTaskWidget::enableControls(enable); - if(enable) { + if (enable) { updatePositionSlider(); } } @@ -332,17 +328,19 @@ void ConfigInputWidget::enableControls(bool enable) void ConfigInputWidget::resizeEvent(QResizeEvent *event) { QWidget::resizeEvent(event); - ui->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio ); + + ui->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio); } void ConfigInputWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/04Cf", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/04Cf", QUrl::StrictMode)); } void ConfigInputWidget::goToWizard() { QMessageBox msgBox; + msgBox.setText(tr("Arming Settings are now set to 'Always Disarmed' for your safety.")); msgBox.setDetailedText(tr("You will have to reconfigure the arming settings manually " "when the wizard is finished. After the last step of the " @@ -352,20 +350,20 @@ void ConfigInputWidget::goToWizard() msgBox.exec(); // Set correct tab visible before starting wizard. - if(ui->tabWidget->currentIndex() != 0) { + if (ui->tabWidget->currentIndex() != 0) { ui->tabWidget->setCurrentIndex(0); } - - // Stash current manual settings data in case the wizard is + + // Stash current manual settings data in case the wizard is // cancelled or the user proceeds far enough into the wizard such // that the UAVO is changed, but then backs out to the start and // chooses a different TX type (which could otherwise result in // unexpected TX channels being enabled) - manualSettingsData=manualSettingsObj->getData(); + manualSettingsData = manualSettingsObj->getData(); previousManualSettingsData = manualSettingsData; - manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; + manualSettingsData.Arming = ManualControlSettings::ARMING_ALWAYSDISARMED; manualSettingsObj->setData(manualSettingsData); - + // start the wizard wizardSetUpStep(wizardWelcome); ui->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio); @@ -373,10 +371,11 @@ void ConfigInputWidget::goToWizard() void ConfigInputWidget::disableWizardButton(int value) { - if(value!=0) + if (value != 0) { ui->groupBox_3->setVisible(false); - else + } else { ui->groupBox_3->setVisible(true); + } } void ConfigInputWidget::wzCancel() @@ -385,9 +384,10 @@ void ConfigInputWidget::wzCancel() manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); ui->stackedWidget->setCurrentIndex(0); - if(wizardStep != wizardNone) + if (wizardStep != wizardNone) { wizardTearDownStep(wizardStep); - wizardStep=wizardNone; + } + wizardStep = wizardNone; ui->stackedWidget->setCurrentIndex(0); // Load settings back from beginning of wizard @@ -398,12 +398,13 @@ void ConfigInputWidget::wzNext() { // In identify sticks mode the next button can indicate // channel advance - if(wizardStep != wizardNone && - wizardStep != wizardIdentifySticks) + if (wizardStep != wizardNone && + wizardStep != wizardIdentifySticks) { wizardTearDownStep(wizardStep); + } // State transitions for next button - switch(wizardStep) { + switch (wizardStep) { case wizardWelcome: wizardSetUpStep(wizardChooseType); break; @@ -415,7 +416,7 @@ void ConfigInputWidget::wzNext() break; case wizardIdentifySticks: nextChannel(); - if(currentChannelNum==-1) { // Gone through all channels + if (currentChannelNum == -1) { // Gone through all channels wizardTearDownStep(wizardIdentifySticks); wizardSetUpStep(wizardIdentifyCenter); } @@ -430,23 +431,22 @@ void ConfigInputWidget::wzNext() wizardSetUpStep(wizardFinish); break; case wizardFinish: - wizardStep=wizardNone; + wizardStep = wizardNone; // Leave setting the throttle neutral until the final Next press, // else the throttle scaling causes the graphical stick movement to not // match the tx stick - manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE]= - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]+ - ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE]- - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); - if((abs(manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE] - - manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE])<100) || - (abs(manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE] - - manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE])<100)) - { - manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]= - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]+ + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE] = + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE] + + ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE] - + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]) * 0.02); + if ((abs(manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE] - + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]) < 100) || + (abs(manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE] - + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]) < 100)) { + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE] = + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE] + (manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE] - - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE])/2; + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]) / 2; } manualSettingsObj->setData(manualSettingsData); // move to Arming Settings tab @@ -460,12 +460,13 @@ void ConfigInputWidget::wzNext() void ConfigInputWidget::wzBack() { - if(wizardStep != wizardNone && - wizardStep != wizardIdentifySticks) + if (wizardStep != wizardNone && + wizardStep != wizardIdentifySticks) { wizardTearDownStep(wizardStep); + } // State transitions for next button - switch(wizardStep) { + switch (wizardStep) { case wizardChooseType: wizardSetUpStep(wizardWelcome); break; @@ -474,7 +475,7 @@ void ConfigInputWidget::wzBack() break; case wizardIdentifySticks: prevChannel(); - if(currentChannelNum == -1) { + if (currentChannelNum == -1) { wizardTearDownStep(wizardIdentifySticks); wizardSetUpStep(wizardChooseMode); } @@ -499,21 +500,21 @@ void ConfigInputWidget::wzBack() void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) { ui->wzText2->clear(); - - switch(step) { + + switch (step) { case wizardWelcome: - foreach(QPointer wd,extraWidgets) - { - if(!wd.isNull()) + foreach(QPointer wd, extraWidgets) { + if (!wd.isNull()) { delete wd; + } } extraWidgets.clear(); ui->graphicsView->setVisible(false); setTxMovement(nothing); ui->wzText->setText(tr("Welcome to the inputs configuration wizard.\n\n" - "Please follow the instructions on the screen and only move your controls when asked to.\n" - "Make sure you already configured your hardware settings on the proper tab and restarted your board.\n\n" - "You can press 'back' at any time to return to the previous screen or press 'Cancel' to quit the wizard.\n")); + "Please follow the instructions on the screen and only move your controls when asked to.\n" + "Make sure you already configured your hardware settings on the proper tab and restarted your board.\n\n" + "You can press 'back' at any time to return to the previous screen or press 'Cancel' to quit the wizard.\n")); ui->stackedWidget->setCurrentIndex(1); ui->wzBack->setEnabled(false); break; @@ -524,12 +525,11 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) setTxMovement(nothing); ui->wzText->setText(tr("Please choose your transmitter type:")); ui->wzBack->setEnabled(true); - QRadioButton * typeAcro=new QRadioButton(tr("Acro: normal transmitter for fixed-wing or quad"),this); - QRadioButton * typeHeli=new QRadioButton(tr("Helicopter: has collective pitch and throttle input"),this); + QRadioButton *typeAcro = new QRadioButton(tr("Acro: normal transmitter for fixed-wing or quad"), this); + QRadioButton *typeHeli = new QRadioButton(tr("Helicopter: has collective pitch and throttle input"), this); if (transmitterType == heli) { typeHeli->setChecked(true); - } - else { + } else { typeAcro->setChecked(true); } ui->wzText2->setText(tr("If selecting the Helicopter option, please engage throttle hold now.")); @@ -540,7 +540,7 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) ui->radioButtonsLayout->layout()->addWidget(typeAcro); ui->radioButtonsLayout->layout()->addWidget(typeHeli); } - break; + break; case wizardChooseMode: { ui->wzBack->setEnabled(true); @@ -551,23 +551,23 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) txMode mode = static_cast(i); if (transmitterType == heli) { switch (mode) { - case mode1: label = tr("Mode 1: Fore/Aft Cyclic and Yaw on the left, Throttle/Collective and Left/Right Cyclic on the right"); break; - case mode2: label = tr("Mode 2: Throttle/Collective and Yaw on the left, Cyclic on the right"); break; - case mode3: label = tr("Mode 3: Cyclic on the left, Throttle/Collective and Yaw on the right"); break; - case mode4: label = tr("Mode 4: Throttle/Collective and Left/Right Cyclic on the left, Fore/Aft Cyclic and Yaw on the right"); break; - default: Q_ASSERT(0); break; - } } - else { + case mode1: label = tr("Mode 1: Fore/Aft Cyclic and Yaw on the left, Throttle/Collective and Left/Right Cyclic on the right"); break; + case mode2: label = tr("Mode 2: Throttle/Collective and Yaw on the left, Cyclic on the right"); break; + case mode3: label = tr("Mode 3: Cyclic on the left, Throttle/Collective and Yaw on the right"); break; + case mode4: label = tr("Mode 4: Throttle/Collective and Left/Right Cyclic on the left, Fore/Aft Cyclic and Yaw on the right"); break; + default: Q_ASSERT(0); break; + } + } else { switch (mode) { - case mode1: label = tr("Mode 1: Elevator and Rudder on the left, Throttle and Ailerons on the right"); break; - case mode2: label = tr("Mode 2: Throttle and Rudder on the left, Elevator and Ailerons on the right"); break; - case mode3: label = tr("Mode 3: Elevator and Ailerons on the left, Throttle and Rudder on the right"); break; - case mode4: label = tr("Mode 4: Throttle and Ailerons on the left, Elevator and Rudder on the right"); break; - default: Q_ASSERT(0); break; - } + case mode1: label = tr("Mode 1: Elevator and Rudder on the left, Throttle and Ailerons on the right"); break; + case mode2: label = tr("Mode 2: Throttle and Rudder on the left, Elevator and Ailerons on the right"); break; + case mode3: label = tr("Mode 3: Elevator and Ailerons on the left, Throttle and Rudder on the right"); break; + case mode4: label = tr("Mode 4: Throttle and Ailerons on the left, Elevator and Rudder on the right"); break; + default: Q_ASSERT(0); break; + } ui->wzText2->setText(tr("For a Quad: Elevator is Pitch, Ailerons are Roll, and Rudder is Yaw.")); } - QRadioButton * modeButton = new QRadioButton(label, this); + QRadioButton *modeButton = new QRadioButton(label, this); if (transmitterMode == mode) { modeButton->setChecked(true); } @@ -575,77 +575,74 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) ui->radioButtonsLayout->layout()->addWidget(modeButton); } } - break; + break; case wizardIdentifySticks: usedChannels.clear(); - currentChannelNum=-1; + currentChannelNum = -1; nextChannel(); - manualSettingsData=manualSettingsObj->getData(); - connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); + manualSettingsData = manualSettingsObj->getData(); + connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyControls())); ui->wzNext->setEnabled(false); break; case wizardIdentifyCenter: setTxMovement(centerAll); ui->wzText->setText(QString(tr("Please center all controls and trims and press Next when ready.\n\n" - "If your FlightMode switch has only two positions, leave it in either position."))); + "If your FlightMode switch has only two positions, leave it in either position."))); break; case wizardIdentifyLimits: { - accessoryDesiredObj0 = AccessoryDesired::GetInstance(getObjectManager(),0); - accessoryDesiredObj1 = AccessoryDesired::GetInstance(getObjectManager(),1); - accessoryDesiredObj2 = AccessoryDesired::GetInstance(getObjectManager(),2); + accessoryDesiredObj0 = AccessoryDesired::GetInstance(getObjectManager(), 0); + accessoryDesiredObj1 = AccessoryDesired::GetInstance(getObjectManager(), 1); + accessoryDesiredObj2 = AccessoryDesired::GetInstance(getObjectManager(), 2); setTxMovement(nothing); ui->wzText->setText(QString(tr("Please move all controls to their maximum extents on both directions.\n\nPress Next when ready."))); fastMdata(); - manualSettingsData=manualSettingsObj->getData(); - for(uint i=0;igetData(); + for (uint i = 0; i < ManualControlSettings::CHANNELMAX_NUMELEM; ++i) { // Preserve the inverted status - if(manualSettingsData.ChannelMin[i] <= manualSettingsData.ChannelMax[i]) { - manualSettingsData.ChannelMin[i]=manualSettingsData.ChannelNeutral[i]; - manualSettingsData.ChannelMax[i]=manualSettingsData.ChannelNeutral[i]; + if (manualSettingsData.ChannelMin[i] <= manualSettingsData.ChannelMax[i]) { + manualSettingsData.ChannelMin[i] = manualSettingsData.ChannelNeutral[i]; + manualSettingsData.ChannelMax[i] = manualSettingsData.ChannelNeutral[i]; } else { // Make this detect as still inverted - manualSettingsData.ChannelMin[i]=manualSettingsData.ChannelNeutral[i] + 1; - manualSettingsData.ChannelMax[i]=manualSettingsData.ChannelNeutral[i]; + manualSettingsData.ChannelMin[i] = manualSettingsData.ChannelNeutral[i] + 1; + manualSettingsData.ChannelMax[i] = manualSettingsData.ChannelNeutral[i]; } } - connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits())); - connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyLimits())); + connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); } - break; + break; case wizardIdentifyInverted: dimOtherControls(true); setTxMovement(nothing); extraWidgets.clear(); - for (int index = 0; index < manualSettingsObj->getField("ChannelMax")->getElementNames().length(); index++) - { + for (int index = 0; index < manualSettingsObj->getField("ChannelMax")->getElementNames().length(); index++) { QString name = manualSettingsObj->getField("ChannelMax")->getElementNames().at(index); - if(!name.contains("Access") && !name.contains("Flight") && - (!name.contains("Collective") || transmitterType == heli)) - { - QCheckBox * cb=new QCheckBox(name,this); + if (!name.contains("Access") && !name.contains("Flight") && + (!name.contains("Collective") || transmitterType == heli)) { + QCheckBox *cb = new QCheckBox(name, this); // Make sure checked status matches current one cb->setChecked(manualSettingsData.ChannelMax[index] < manualSettingsData.ChannelMin[index]); - dynamic_cast(ui->checkBoxesLayout->layout())->addWidget(cb, extraWidgets.size()/4, extraWidgets.size()%4); + dynamic_cast(ui->checkBoxesLayout->layout())->addWidget(cb, extraWidgets.size() / 4, extraWidgets.size() % 4); extraWidgets.append(cb); - connect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); + connect(cb, SIGNAL(toggled(bool)), this, SLOT(invertControls())); } } - connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); ui->wzText->setText(QString(tr("Please check the picture below and correct all the sticks which show an inverted movement. Press Next when ready."))); fastMdata(); break; case wizardFinish: dimOtherControls(false); - connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); ui->wzText->setText(QString(tr("You have completed this wizard, please check below if the picture mimics your sticks movement.\n\n" - "IMPORTANT: These new settings have not been saved to the board yet. After pressing Next you will go to the Arming Settings " - "tab where you can set your desired arming sequence and save the configuration."))); + "IMPORTANT: These new settings have not been saved to the board yet. After pressing Next you will go to the Arming Settings " + "tab where you can set your desired arming sequence and save the configuration."))); fastMdata(); break; default: @@ -656,76 +653,76 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) void ConfigInputWidget::wizardTearDownStep(enum wizardSteps step) { - QRadioButton * mode, * type; + QRadioButton *mode, *type; + Q_ASSERT(step == wizardStep); - switch(step) { + switch (step) { case wizardWelcome: break; - case wizardChooseType: - type=qobject_cast(extraWidgets.at(0)); - if(type->isChecked()) - transmitterType=acro; - else - transmitterType=heli; + case wizardChooseType: + type = qobject_cast(extraWidgets.at(0)); + if (type->isChecked()) { + transmitterType = acro; + } else { + transmitterType = heli; + } delete extraWidgets.at(0); delete extraWidgets.at(1); extraWidgets.clear(); break; - case wizardChooseMode: - for (int i = mode1; i <= mode4; ++i) { - mode=qobject_cast(extraWidgets.first()); - if(mode->isChecked()) { - transmitterMode=static_cast(i); + case wizardChooseMode: + for (int i = mode1; i <= mode4; ++i) { + mode = qobject_cast(extraWidgets.first()); + if (mode->isChecked()) { + transmitterMode = static_cast(i); } delete mode; extraWidgets.removeFirst(); } break; case wizardIdentifySticks: - disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); + disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyControls())); ui->wzNext->setEnabled(true); setTxMovement(nothing); break; case wizardIdentifyCenter: - manualCommandData=manualCommandObj->getData(); - manualSettingsData=manualSettingsObj->getData(); - for(unsigned int i=0;igetData(); + manualSettingsData = manualSettingsObj->getData(); + for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; ++i) { + manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; } manualSettingsObj->setData(manualSettingsData); setTxMovement(nothing); break; case wizardIdentifyLimits: - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits())); - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - disconnect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - disconnect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyLimits())); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + disconnect(flightStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + disconnect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); manualSettingsObj->setData(manualSettingsData); restoreMdata(); setTxMovement(nothing); break; case wizardIdentifyInverted: dimOtherControls(false); - foreach(QWidget * wd,extraWidgets) - { - QCheckBox * cb=qobject_cast(wd); - if(cb) - { - disconnect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); + foreach(QWidget * wd, extraWidgets) { + QCheckBox *cb = qobject_cast(wd); + + if (cb) { + disconnect(cb, SIGNAL(toggled(bool)), this, SLOT(invertControls())); delete cb; } } extraWidgets.clear(); - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); restoreMdata(); break; case wizardFinish: dimOtherControls(false); setTxMovement(nothing); - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - disconnect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - disconnect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + disconnect(flightStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); + disconnect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); restoreMdata(); break; default: @@ -734,8 +731,8 @@ void ConfigInputWidget::wizardTearDownStep(enum wizardSteps step) } /** - * Set manual control command to fast updates - */ + * Set manual control command to fast updates + */ void ConfigInputWidget::fastMdata() { manualControlMdata = manualCommandObj->getMetadata(); @@ -746,56 +743,58 @@ void ConfigInputWidget::fastMdata() } /** - * Restore previous update settings for manual control data - */ + * Restore previous update settings for manual control data + */ void ConfigInputWidget::restoreMdata() { manualCommandObj->setMetadata(manualControlMdata); } /** - * Set the display to indicate which channel the person should move - */ + * Set the display to indicate which channel the person should move + */ void ConfigInputWidget::setChannel(int newChan) { - if(newChan == ManualControlSettings::CHANNELGROUPS_COLLECTIVE) + if (newChan == ManualControlSettings::CHANNELGROUPS_COLLECTIVE) { ui->wzText->setText(QString(tr("Please enable throttle hold mode.\n\nMove the Collective Pitch stick."))); - else if (newChan == ManualControlSettings::CHANNELGROUPS_FLIGHTMODE) + } else if (newChan == ManualControlSettings::CHANNELGROUPS_FLIGHTMODE) { ui->wzText->setText(QString(tr("Please toggle the Flight Mode switch.\n\nFor switches you may have to repeat this rapidly."))); - else if((transmitterType == heli) && (newChan == ManualControlSettings::CHANNELGROUPS_THROTTLE)) + } else if ((transmitterType == heli) && (newChan == ManualControlSettings::CHANNELGROUPS_THROTTLE)) { ui->wzText->setText(QString(tr("Please disable throttle hold mode.\n\nMove the Throttle stick."))); - else + } else { ui->wzText->setText(QString(tr("Please move each control one at a time according to the instructions and picture below.\n\n" - "Move the %1 stick.")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan))); + "Move the %1 stick.")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan))); + } - if(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("Accessory") || - manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("FlightMode")) { + if (manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("Accessory") || + manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("FlightMode")) { ui->wzNext->setEnabled(true); ui->wzText->setText(ui->wzText->text() + tr(" Alternatively, click Next to skip this channel.")); - } else + } else { ui->wzNext->setEnabled(false); + } setMoveFromCommand(newChan); currentChannelNum = newChan; - channelDetected = false; + channelDetected = false; } /** - * Unfortunately order of channel should be different in different conditions. Selects - * next channel based on heli or acro mode - */ + * Unfortunately order of channel should be different in different conditions. Selects + * next channel based on heli or acro mode + */ void ConfigInputWidget::nextChannel() { QList order = (transmitterType == heli) ? heliChannelOrder : acroChannelOrder; - if(currentChannelNum == -1) { + if (currentChannelNum == -1) { setChannel(order[0]); return; } for (int i = 0; i < order.length() - 1; i++) { - if(order[i] == currentChannelNum) { - setChannel(order[i+1]); + if (order[i] == currentChannelNum) { + setChannel(order[i + 1]); return; } } @@ -803,24 +802,25 @@ void ConfigInputWidget::nextChannel() } /** - * Unfortunately order of channel should be different in different conditions. Selects - * previous channel based on heli or acro mode - */ + * Unfortunately order of channel should be different in different conditions. Selects + * previous channel based on heli or acro mode + */ void ConfigInputWidget::prevChannel() { QList order = transmitterType == heli ? heliChannelOrder : acroChannelOrder; // No previous from unset channel or next state - if(currentChannelNum == -1) + if (currentChannelNum == -1) { return; + } for (int i = 1; i < order.length(); i++) { - if(order[i] == currentChannelNum) { - if (!usedChannels.isEmpty() && + if (order[i] == currentChannelNum) { + if (!usedChannels.isEmpty() && usedChannels.back().channelIndex == currentChannelNum) { usedChannels.removeLast(); } - setChannel(order[i-1]); + setChannel(order[i - 1]); return; } } @@ -829,38 +829,38 @@ void ConfigInputWidget::prevChannel() void ConfigInputWidget::identifyControls() { - static int debounce=0; + static int debounce = 0; - receiverActivityData=receiverActivityObj->getData(); - if(receiverActivityData.ActiveChannel==255) + receiverActivityData = receiverActivityObj->getData(); + if (receiverActivityData.ActiveChannel == 255) { return; - if(channelDetected) + } + if (channelDetected) { return; - else - { - receiverActivityData=receiverActivityObj->getData(); - currentChannel.group=receiverActivityData.ActiveGroup; - currentChannel.number=receiverActivityData.ActiveChannel; - if(currentChannel==lastChannel) + } else { + receiverActivityData = receiverActivityObj->getData(); + currentChannel.group = receiverActivityData.ActiveGroup; + currentChannel.number = receiverActivityData.ActiveChannel; + if (currentChannel == lastChannel) { ++debounce; - lastChannel.group= currentChannel.group; - lastChannel.number=currentChannel.number; - lastChannel.channelIndex = currentChannelNum; - if(!usedChannels.contains(lastChannel) && debounce>1) - { - channelDetected = true; - debounce=0; - usedChannels.append(lastChannel); - manualSettingsData=manualSettingsObj->getData(); - manualSettingsData.ChannelGroups[currentChannelNum]=currentChannel.group; - manualSettingsData.ChannelNumber[currentChannelNum]=currentChannel.number; - manualSettingsObj->setData(manualSettingsData); } - else + lastChannel.group = currentChannel.group; + lastChannel.number = currentChannel.number; + lastChannel.channelIndex = currentChannelNum; + if (!usedChannels.contains(lastChannel) && debounce > 1) { + channelDetected = true; + debounce = 0; + usedChannels.append(lastChannel); + manualSettingsData = manualSettingsObj->getData(); + manualSettingsData.ChannelGroups[currentChannelNum] = currentChannel.group; + manualSettingsData.ChannelNumber[currentChannelNum] = currentChannel.number; + manualSettingsObj->setData(manualSettingsData); + } else { return; + } } - //m_config->wzText->clear(); + // m_config->wzText->clear(); setTxMovement(nothing); QTimer::singleShot(2500, this, SLOT(wzNext())); @@ -868,21 +868,24 @@ void ConfigInputWidget::identifyControls() void ConfigInputWidget::identifyLimits() { - manualCommandData=manualCommandObj->getData(); - for(uint i=0;igetData(); + for (uint i = 0; i < ManualControlSettings::CHANNELMAX_NUMELEM; ++i) { + if (manualSettingsData.ChannelMin[i] <= manualSettingsData.ChannelMax[i]) { // Non inverted channel - if(manualSettingsData.ChannelMin[i]>manualCommandData.Channel[i]) - manualSettingsData.ChannelMin[i]=manualCommandData.Channel[i]; - if(manualSettingsData.ChannelMax[i] manualCommandData.Channel[i]) { + manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; + } + if (manualSettingsData.ChannelMax[i] < manualCommandData.Channel[i]) { + manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; + } } else { // Inverted channel - if(manualSettingsData.ChannelMax[i]>manualCommandData.Channel[i]) - manualSettingsData.ChannelMax[i]=manualCommandData.Channel[i]; - if(manualSettingsData.ChannelMin[i] manualCommandData.Channel[i]) { + manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; + } + if (manualSettingsData.ChannelMin[i] < manualCommandData.Channel[i]) { + manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; + } } } manualSettingsObj->setData(manualSettingsData); @@ -890,123 +893,122 @@ void ConfigInputWidget::identifyLimits() void ConfigInputWidget::setMoveFromCommand(int command) { // ManualControlSettings::ChannelNumberElem: - // CHANNELNUMBER_ROLL=0, - // CHANNELNUMBER_PITCH=1, - // CHANNELNUMBER_YAW=2, - // CHANNELNUMBER_THROTTLE=3, - // CHANNELNUMBER_FLIGHTMODE=4, - // CHANNELNUMBER_ACCESSORY0=5, - // CHANNELNUMBER_ACCESSORY1=6, - // CHANNELNUMBER_ACCESSORY2=7 - + // CHANNELNUMBER_ROLL=0, + // CHANNELNUMBER_PITCH=1, + // CHANNELNUMBER_YAW=2, + // CHANNELNUMBER_THROTTLE=3, + // CHANNELNUMBER_FLIGHTMODE=4, + // CHANNELNUMBER_ACCESSORY0=5, + // CHANNELNUMBER_ACCESSORY1=6, + // CHANNELNUMBER_ACCESSORY2=7 + txMovements movement; - + switch (command) { - case ManualControlSettings::CHANNELNUMBER_ROLL: - movement = ((transmitterMode == mode3 || transmitterMode == mode4) ? - moveLeftHorizontalStick: moveRightHorizontalStick); - break; - case ManualControlSettings::CHANNELNUMBER_PITCH: - movement = (transmitterMode == mode1 || transmitterMode == mode3) ? - moveLeftVerticalStick: moveRightVerticalStick; - break; - case ManualControlSettings::CHANNELNUMBER_YAW: - movement = ((transmitterMode == mode1 || transmitterMode == mode2) ? - moveLeftHorizontalStick: moveRightHorizontalStick); - break; - case ManualControlSettings::CHANNELNUMBER_THROTTLE: - movement = (transmitterMode == mode2 || transmitterMode == mode4) ? - moveLeftVerticalStick: moveRightVerticalStick; - break; - case ManualControlSettings::CHANNELNUMBER_COLLECTIVE: - movement = (transmitterMode == mode2 || transmitterMode == mode4) ? - moveLeftVerticalStick: moveRightVerticalStick; - break; - case ManualControlSettings::CHANNELNUMBER_FLIGHTMODE: - movement = moveFlightMode; - break; - case ManualControlSettings::CHANNELNUMBER_ACCESSORY0: - movement = moveAccess0; - break; - case ManualControlSettings::CHANNELNUMBER_ACCESSORY1: - movement = moveAccess1; - break; - case ManualControlSettings::CHANNELNUMBER_ACCESSORY2: - movement = moveAccess2; - break; - default: - Q_ASSERT(0); - break; - } + case ManualControlSettings::CHANNELNUMBER_ROLL: + movement = ((transmitterMode == mode3 || transmitterMode == mode4) ? + moveLeftHorizontalStick : moveRightHorizontalStick); + break; + case ManualControlSettings::CHANNELNUMBER_PITCH: + movement = (transmitterMode == mode1 || transmitterMode == mode3) ? + moveLeftVerticalStick : moveRightVerticalStick; + break; + case ManualControlSettings::CHANNELNUMBER_YAW: + movement = ((transmitterMode == mode1 || transmitterMode == mode2) ? + moveLeftHorizontalStick : moveRightHorizontalStick); + break; + case ManualControlSettings::CHANNELNUMBER_THROTTLE: + movement = (transmitterMode == mode2 || transmitterMode == mode4) ? + moveLeftVerticalStick : moveRightVerticalStick; + break; + case ManualControlSettings::CHANNELNUMBER_COLLECTIVE: + movement = (transmitterMode == mode2 || transmitterMode == mode4) ? + moveLeftVerticalStick : moveRightVerticalStick; + break; + case ManualControlSettings::CHANNELNUMBER_FLIGHTMODE: + movement = moveFlightMode; + break; + case ManualControlSettings::CHANNELNUMBER_ACCESSORY0: + movement = moveAccess0; + break; + case ManualControlSettings::CHANNELNUMBER_ACCESSORY1: + movement = moveAccess1; + break; + case ManualControlSettings::CHANNELNUMBER_ACCESSORY2: + movement = moveAccess2; + break; + default: + Q_ASSERT(0); + break; + } setTxMovement(movement); } void ConfigInputWidget::setTxMovement(txMovements movement) { resetTxControls(); - switch(movement) - { + switch (movement) { case moveLeftVerticalStick: - movePos=0; - growing=true; - currentMovement=moveLeftVerticalStick; + movePos = 0; + growing = true; + currentMovement = moveLeftVerticalStick; animate->start(100); break; case moveRightVerticalStick: - movePos=0; - growing=true; - currentMovement=moveRightVerticalStick; + movePos = 0; + growing = true; + currentMovement = moveRightVerticalStick; animate->start(100); break; case moveLeftHorizontalStick: - movePos=0; - growing=true; - currentMovement=moveLeftHorizontalStick; + movePos = 0; + growing = true; + currentMovement = moveLeftHorizontalStick; animate->start(100); break; case moveRightHorizontalStick: - movePos=0; - growing=true; - currentMovement=moveRightHorizontalStick; + movePos = 0; + growing = true; + currentMovement = moveRightHorizontalStick; animate->start(100); break; case moveAccess0: - movePos=0; - growing=true; - currentMovement=moveAccess0; + movePos = 0; + growing = true; + currentMovement = moveAccess0; animate->start(100); break; case moveAccess1: - movePos=0; - growing=true; - currentMovement=moveAccess1; + movePos = 0; + growing = true; + currentMovement = moveAccess1; animate->start(100); break; case moveAccess2: - movePos=0; - growing=true; - currentMovement=moveAccess2; + movePos = 0; + growing = true; + currentMovement = moveAccess2; animate->start(100); break; case moveFlightMode: - movePos=0; - growing=true; - currentMovement=moveFlightMode; + movePos = 0; + growing = true; + currentMovement = moveFlightMode; animate->start(1000); break; case centerAll: - movePos=0; - currentMovement=centerAll; + movePos = 0; + currentMovement = centerAll; animate->start(1000); break; case moveAll: - movePos=0; - growing=true; - currentMovement=moveAll; + movePos = 0; + growing = true; + currentMovement = moveAll; animate->start(50); break; case nothing: - movePos=0; + movePos = 0; animate->stop(); break; default: @@ -1018,179 +1020,155 @@ void ConfigInputWidget::setTxMovement(txMovements movement) void ConfigInputWidget::moveTxControls() { QTransform trans; - QGraphicsItem * item; + QGraphicsItem *item; txMovementType move; int limitMax; int limitMin; - static bool auxFlag=false; - switch(currentMovement) - { + static bool auxFlag = false; + + switch (currentMovement) { case moveLeftVerticalStick: - item=m_txLeftStick; - trans=m_txLeftStickOrig; - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=vertical; + item = m_txLeftStick; + trans = m_txLeftStickOrig; + limitMax = STICK_MAX_MOVE; + limitMin = STICK_MIN_MOVE; + move = vertical; break; case moveRightVerticalStick: - item=m_txRightStick; - trans=m_txRightStickOrig; - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=vertical; + item = m_txRightStick; + trans = m_txRightStickOrig; + limitMax = STICK_MAX_MOVE; + limitMin = STICK_MIN_MOVE; + move = vertical; break; case moveLeftHorizontalStick: - item=m_txLeftStick; - trans=m_txLeftStickOrig; - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=horizontal; + item = m_txLeftStick; + trans = m_txLeftStickOrig; + limitMax = STICK_MAX_MOVE; + limitMin = STICK_MIN_MOVE; + move = horizontal; break; case moveRightHorizontalStick: - item=m_txRightStick; - trans=m_txRightStickOrig; - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=horizontal; + item = m_txRightStick; + trans = m_txRightStickOrig; + limitMax = STICK_MAX_MOVE; + limitMin = STICK_MIN_MOVE; + move = horizontal; break; case moveAccess0: - item=m_txAccess0; - trans=m_txAccess0Orig; - limitMax=ACCESS_MAX_MOVE; - limitMin=ACCESS_MIN_MOVE; - move=horizontal; + item = m_txAccess0; + trans = m_txAccess0Orig; + limitMax = ACCESS_MAX_MOVE; + limitMin = ACCESS_MIN_MOVE; + move = horizontal; break; case moveAccess1: - item=m_txAccess1; - trans=m_txAccess1Orig; - limitMax=ACCESS_MAX_MOVE; - limitMin=ACCESS_MIN_MOVE; - move=horizontal; + item = m_txAccess1; + trans = m_txAccess1Orig; + limitMax = ACCESS_MAX_MOVE; + limitMin = ACCESS_MIN_MOVE; + move = horizontal; break; case moveAccess2: - item=m_txAccess2; - trans=m_txAccess2Orig; - limitMax=ACCESS_MAX_MOVE; - limitMin=ACCESS_MIN_MOVE; - move=horizontal; + item = m_txAccess2; + trans = m_txAccess2Orig; + limitMax = ACCESS_MAX_MOVE; + limitMin = ACCESS_MIN_MOVE; + move = horizontal; break; case moveFlightMode: - item=m_txFlightMode; - move=jump; + item = m_txFlightMode; + move = jump; break; case centerAll: - item=m_txArrows; - move=jump; + item = m_txArrows; + move = jump; break; case moveAll: - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=mix; + limitMax = STICK_MAX_MOVE; + limitMin = STICK_MIN_MOVE; + move = mix; break; default: break; } - if(move==vertical) - item->setTransform(trans.translate(0,movePos*10),false); - else if(move==horizontal) - item->setTransform(trans.translate(movePos*10,0),false); - else if(move==jump) - { - if(item==m_txArrows) - { + if (move == vertical) { + item->setTransform(trans.translate(0, movePos * 10), false); + } else if (move == horizontal) { + item->setTransform(trans.translate(movePos * 10, 0), false); + } else if (move == jump) { + if (item == m_txArrows) { m_txArrows->setVisible(!m_txArrows->isVisible()); - } - else if(item==m_txFlightMode) - { - QGraphicsSvgItem * svg; - svg=(QGraphicsSvgItem *)item; - if (svg) - { - if(svg->elementId()=="flightModeCenter") - { - if(growing) - { + } else if (item == m_txFlightMode) { + QGraphicsSvgItem *svg; + svg = (QGraphicsSvgItem *)item; + if (svg) { + if (svg->elementId() == "flightModeCenter") { + if (growing) { svg->setElementId("flightModeRight"); - m_txFlightMode->setTransform(m_txFlightModeROrig,false); - } - else - { + m_txFlightMode->setTransform(m_txFlightModeROrig, false); + } else { svg->setElementId("flightModeLeft"); - m_txFlightMode->setTransform(m_txFlightModeLOrig,false); + m_txFlightMode->setTransform(m_txFlightModeLOrig, false); } - } - else if(svg->elementId()=="flightModeRight") - { - growing=false; + } else if (svg->elementId() == "flightModeRight") { + growing = false; svg->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); - } - else if(svg->elementId()=="flightModeLeft") - { - growing=true; + m_txFlightMode->setTransform(m_txFlightModeCOrig, false); + } else if (svg->elementId() == "flightModeLeft") { + growing = true; svg->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + m_txFlightMode->setTransform(m_txFlightModeCOrig, false); } } } - } - else if(move==mix) - { - trans=m_txAccess0Orig; - m_txAccess0->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); - trans=m_txAccess1Orig; - m_txAccess1->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); - trans=m_txAccess2Orig; - m_txAccess2->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); + } else if (move == mix) { + trans = m_txAccess0Orig; + m_txAccess0->setTransform(trans.translate(movePos * 10 * ACCESS_MAX_MOVE / STICK_MAX_MOVE, 0), false); + trans = m_txAccess1Orig; + m_txAccess1->setTransform(trans.translate(movePos * 10 * ACCESS_MAX_MOVE / STICK_MAX_MOVE, 0), false); + trans = m_txAccess2Orig; + m_txAccess2->setTransform(trans.translate(movePos * 10 * ACCESS_MAX_MOVE / STICK_MAX_MOVE, 0), false); - if(auxFlag) - { - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(0,movePos*10),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(0,movePos*10),false); - } - else - { - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(movePos*10,0),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(movePos*10,0),false); + if (auxFlag) { + trans = m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(0, movePos * 10), false); + trans = m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(0, movePos * 10), false); + } else { + trans = m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(movePos * 10, 0), false); + trans = m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(movePos * 10, 0), false); } - if(movePos==0) - { + if (movePos == 0) { m_txFlightMode->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); - } - else if(movePos==ACCESS_MAX_MOVE/2) - { + m_txFlightMode->setTransform(m_txFlightModeCOrig, false); + } else if (movePos == ACCESS_MAX_MOVE / 2) { m_txFlightMode->setElementId("flightModeRight"); - m_txFlightMode->setTransform(m_txFlightModeROrig,false); - } - else if(movePos==ACCESS_MIN_MOVE/2) - { + m_txFlightMode->setTransform(m_txFlightModeROrig, false); + } else if (movePos == ACCESS_MIN_MOVE / 2) { m_txFlightMode->setElementId("flightModeLeft"); - m_txFlightMode->setTransform(m_txFlightModeLOrig,false); + m_txFlightMode->setTransform(m_txFlightModeLOrig, false); } } - if(move==horizontal || move==vertical ||move==mix) - { - if(movePos==0 && growing) - auxFlag=!auxFlag; - if(growing) - ++movePos; - else - --movePos; - if(movePos>limitMax) - { - movePos=movePos-2; - growing=false; + if (move == horizontal || move == vertical || move == mix) { + if (movePos == 0 && growing) { + auxFlag = !auxFlag; } - if(movePos limitMax) { + movePos = movePos - 2; + growing = false; + } + if (movePos < limitMin) { + movePos = movePos + 2; + growing = true; } } } @@ -1198,68 +1176,66 @@ void ConfigInputWidget::moveTxControls() void ConfigInputWidget::moveSticks() { QTransform trans; - manualCommandData=manualCommandObj->getData(); - flightStatusData=flightStatusObj->getData(); - accessoryDesiredData0=accessoryDesiredObj0->getData(); - accessoryDesiredData1=accessoryDesiredObj1->getData(); - accessoryDesiredData2=accessoryDesiredObj2->getData(); + + manualCommandData = manualCommandObj->getData(); + flightStatusData = flightStatusObj->getData(); + accessoryDesiredData0 = accessoryDesiredObj0->getData(); + accessoryDesiredData1 = accessoryDesiredObj1->getData(); + accessoryDesiredData2 = accessoryDesiredObj2->getData(); switch (transmitterMode) { - case mode1: - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); - break; - case mode2: - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); - break; - case mode3: - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); - break; - case mode4: - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); - break; - default: - Q_ASSERT(0); - break; + case mode1: + trans = m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw * STICK_MAX_MOVE * 10, manualCommandData.Pitch * STICK_MAX_MOVE * 10), false); + trans = m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(manualCommandData.Roll * STICK_MAX_MOVE * 10, -manualCommandData.Throttle * STICK_MAX_MOVE * 10), false); + break; + case mode2: + trans = m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw * STICK_MAX_MOVE * 10, -manualCommandData.Throttle * STICK_MAX_MOVE * 10), false); + trans = m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(manualCommandData.Roll * STICK_MAX_MOVE * 10, manualCommandData.Pitch * STICK_MAX_MOVE * 10), false); + break; + case mode3: + trans = m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(manualCommandData.Roll * STICK_MAX_MOVE * 10, manualCommandData.Pitch * STICK_MAX_MOVE * 10), false); + trans = m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(manualCommandData.Yaw * STICK_MAX_MOVE * 10, -manualCommandData.Throttle * STICK_MAX_MOVE * 10), false); + break; + case mode4: + trans = m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(manualCommandData.Roll * STICK_MAX_MOVE * 10, -manualCommandData.Throttle * STICK_MAX_MOVE * 10), false); + trans = m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(manualCommandData.Yaw * STICK_MAX_MOVE * 10, manualCommandData.Pitch * STICK_MAX_MOVE * 10), false); + break; + default: + Q_ASSERT(0); + break; } - if(flightStatusData.FlightMode==manualSettingsData.FlightModePosition[0]) - { + if (flightStatusData.FlightMode == manualSettingsData.FlightModePosition[0]) { m_txFlightMode->setElementId("flightModeLeft"); - m_txFlightMode->setTransform(m_txFlightModeLOrig,false); - } - else if (flightStatusData.FlightMode==manualSettingsData.FlightModePosition[1]) - { + m_txFlightMode->setTransform(m_txFlightModeLOrig, false); + } else if (flightStatusData.FlightMode == manualSettingsData.FlightModePosition[1]) { m_txFlightMode->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); - } - else if (flightStatusData.FlightMode==manualSettingsData.FlightModePosition[2]) - { + m_txFlightMode->setTransform(m_txFlightModeCOrig, false); + } else if (flightStatusData.FlightMode == manualSettingsData.FlightModePosition[2]) { m_txFlightMode->setElementId("flightModeRight"); - m_txFlightMode->setTransform(m_txFlightModeROrig,false); + m_txFlightMode->setTransform(m_txFlightModeROrig, false); } - m_txAccess0->setTransform(QTransform(m_txAccess0Orig).translate(accessoryDesiredData0.AccessoryVal*ACCESS_MAX_MOVE*10,0),false); - m_txAccess1->setTransform(QTransform(m_txAccess1Orig).translate(accessoryDesiredData1.AccessoryVal*ACCESS_MAX_MOVE*10,0),false); - m_txAccess2->setTransform(QTransform(m_txAccess2Orig).translate(accessoryDesiredData2.AccessoryVal*ACCESS_MAX_MOVE*10,0),false); + m_txAccess0->setTransform(QTransform(m_txAccess0Orig).translate(accessoryDesiredData0.AccessoryVal * ACCESS_MAX_MOVE * 10, 0), false); + m_txAccess1->setTransform(QTransform(m_txAccess1Orig).translate(accessoryDesiredData1.AccessoryVal * ACCESS_MAX_MOVE * 10, 0), false); + m_txAccess2->setTransform(QTransform(m_txAccess2Orig).translate(accessoryDesiredData2.AccessoryVal * ACCESS_MAX_MOVE * 10, 0), false); } void ConfigInputWidget::dimOtherControls(bool value) { qreal opac; - if(value) - opac=0.1; - else - opac=1; + + if (value) { + opac = 0.1; + } else { + opac = 1; + } m_txAccess0->setOpacity(opac); m_txAccess1->setOpacity(opac); m_txAccess2->setOpacity(opac); @@ -1268,20 +1244,18 @@ void ConfigInputWidget::dimOtherControls(bool value) void ConfigInputWidget::invertControls() { - manualSettingsData=manualSettingsObj->getData(); - foreach(QWidget * wd,extraWidgets) - { - QCheckBox * cb=qobject_cast(wd); - if(cb) - { + manualSettingsData = manualSettingsObj->getData(); + foreach(QWidget * wd, extraWidgets) { + QCheckBox *cb = qobject_cast(wd); + + if (cb) { int index = manualSettingsObj->getField("ChannelNumber")->getElementNames().indexOf(cb->text()); - if((cb->isChecked() && (manualSettingsData.ChannelMax[index]>manualSettingsData.ChannelMin[index])) || - (!cb->isChecked() && (manualSettingsData.ChannelMax[index]isChecked() && (manualSettingsData.ChannelMax[index] > manualSettingsData.ChannelMin[index])) || + (!cb->isChecked() && (manualSettingsData.ChannelMax[index] < manualSettingsData.ChannelMin[index]))) { qint16 aux; - aux=manualSettingsData.ChannelMax[index]; - manualSettingsData.ChannelMax[index]=manualSettingsData.ChannelMin[index]; - manualSettingsData.ChannelMin[index]=aux; + aux = manualSettingsData.ChannelMax[index]; + manualSettingsData.ChannelMax[index] = manualSettingsData.ChannelMin[index]; + manualSettingsData.ChannelMin[index] = aux; } } } @@ -1291,41 +1265,42 @@ void ConfigInputWidget::invertControls() void ConfigInputWidget::moveFMSlider() { ManualControlSettings::DataFields manualSettingsDataPriv = manualSettingsObj->getData(); - ManualControlCommand::DataFields manualCommandDataPriv = manualCommandObj->getData(); + ManualControlCommand::DataFields manualCommandDataPriv = manualCommandObj->getData(); float valueScaled; - int chMin = manualSettingsDataPriv.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; - int chMax = manualSettingsDataPriv.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]; + int chMin = manualSettingsDataPriv.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; + int chMax = manualSettingsDataPriv.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]; int chNeutral = manualSettingsDataPriv.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]; - int value = manualCommandDataPriv.Channel[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; - if ((chMax > chMin && value >= chNeutral) || (chMin > chMax && value <= chNeutral)) - { - if (chMax != chNeutral) + int value = manualCommandDataPriv.Channel[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; + + if ((chMax > chMin && value >= chNeutral) || (chMin > chMax && value <= chNeutral)) { + if (chMax != chNeutral) { valueScaled = (float)(value - chNeutral) / (float)(chMax - chNeutral); - else + } else { valueScaled = 0; - } - else - { - if (chMin != chNeutral) + } + } else { + if (chMin != chNeutral) { valueScaled = (float)(value - chNeutral) / (float)(chNeutral - chMin); - else + } else { valueScaled = 0; + } } // Bound and scale FlightMode from [-1..+1] to [0..1] range - if (valueScaled < -1.0) + if (valueScaled < -1.0) { valueScaled = -1.0; - else - if (valueScaled > 1.0) - valueScaled = 1.0; + } else if (valueScaled > 1.0) { + valueScaled = 1.0; + } // Convert flightMode value into the switch position in the range [0..N-1] // This uses the same optimized computation as flight code to be consistent uint8_t pos = ((int16_t)(valueScaled * 256) + 256) * manualSettingsDataPriv.FlightModeNumber >> 9; - if (pos >= manualSettingsDataPriv.FlightModeNumber) + if (pos >= manualSettingsDataPriv.FlightModeNumber) { pos = manualSettingsDataPriv.FlightModeNumber - 1; + } ui->fmsSlider->setValue(pos); } @@ -1333,66 +1308,67 @@ void ConfigInputWidget::updatePositionSlider() { ManualControlSettings::DataFields manualSettingsDataPriv = manualSettingsObj->getData(); - switch(manualSettingsDataPriv.FlightModeNumber) { + switch (manualSettingsDataPriv.FlightModeNumber) { default: case 6: ui->fmsModePos6->setEnabled(true); - // pass through + // pass through case 5: ui->fmsModePos5->setEnabled(true); - // pass through + // pass through case 4: ui->fmsModePos4->setEnabled(true); - // pass through + // pass through case 3: ui->fmsModePos3->setEnabled(true); - // pass through + // pass through case 2: ui->fmsModePos2->setEnabled(true); - // pass through + // pass through case 1: ui->fmsModePos1->setEnabled(true); - // pass through + // pass through case 0: - break; + break; } - switch(manualSettingsDataPriv.FlightModeNumber) { + switch (manualSettingsDataPriv.FlightModeNumber) { case 0: ui->fmsModePos1->setEnabled(false); - // pass through + // pass through case 1: ui->fmsModePos2->setEnabled(false); - // pass through + // pass through case 2: ui->fmsModePos3->setEnabled(false); - // pass through + // pass through case 3: ui->fmsModePos4->setEnabled(false); - // pass through + // pass through case 4: ui->fmsModePos5->setEnabled(false); - // pass through + // pass through case 5: ui->fmsModePos6->setEnabled(false); - // pass through + // pass through case 6: default: - break; + break; } } void ConfigInputWidget::updateCalibration() { - manualCommandData=manualCommandObj->getData(); - for(uint i=0;imanualCommandData.Channel[i]) || - (reverse[i] && manualSettingsData.ChannelMin[i]manualCommandData.Channel[i])) - manualSettingsData.ChannelMax[i]=manualCommandData.Channel[i]; + manualCommandData = manualCommandObj->getData(); + for (uint i = 0; i < ManualControlSettings::CHANNELMAX_NUMELEM; ++i) { + if ((!reverse[i] && manualSettingsData.ChannelMin[i] > manualCommandData.Channel[i]) || + (reverse[i] && manualSettingsData.ChannelMin[i] < manualCommandData.Channel[i])) { + manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; + } + if ((!reverse[i] && manualSettingsData.ChannelMax[i] < manualCommandData.Channel[i]) || + (reverse[i] && manualSettingsData.ChannelMax[i] > manualCommandData.Channel[i])) { + manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; + } manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; } @@ -1412,46 +1388,47 @@ void ConfigInputWidget::simpleCalibration(bool enable) msgBox.setDefaultButton(QMessageBox::Ok); msgBox.exec(); - manualCommandData = manualCommandObj->getData(); + manualCommandData = manualCommandObj->getData(); - manualSettingsData=manualSettingsObj->getData(); - manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; + manualSettingsData = manualSettingsObj->getData(); + manualSettingsData.Arming = ManualControlSettings::ARMING_ALWAYSDISARMED; manualSettingsObj->setData(manualSettingsData); for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) { reverse[i] = manualSettingsData.ChannelMax[i] < manualSettingsData.ChannelMin[i]; - manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; + manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; - manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; + manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; } fastMdata(); - connect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(updateCalibration())); + connect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(updateCalibration())); } else { ui->configurationWizard->setEnabled(true); - manualCommandData = manualCommandObj->getData(); + manualCommandData = manualCommandObj->getData(); manualSettingsData = manualSettingsObj->getData(); restoreMdata(); - for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) + for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) { manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; + } // Force flight mode neutral to middle manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE] = - (manualSettingsData.ChannelMax[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE] + - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE]) / 2; + (manualSettingsData.ChannelMax[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE] + + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE]) / 2; // Force throttle to be near min - manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE]= - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]+ - ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE]- - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE] = + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE] + + ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE] - + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]) * 0.02); manualSettingsObj->setData(manualSettingsData); - disconnect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(updateCalibration())); + disconnect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(updateCalibration())); } } diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index be8adedb8..18c6b4209 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -49,124 +49,125 @@ class Ui_InputWidget; -class ConfigInputWidget: public ConfigTaskWidget -{ - Q_OBJECT +class ConfigInputWidget : public ConfigTaskWidget { + Q_OBJECT public: - ConfigInputWidget(QWidget *parent = 0); - ~ConfigInputWidget(); - enum wizardSteps{wizardWelcome,wizardChooseMode,wizardChooseType,wizardIdentifySticks,wizardIdentifyCenter,wizardIdentifyLimits,wizardIdentifyInverted,wizardFinish,wizardNone}; - enum txMode{mode1,mode2,mode3,mode4}; - enum txMovements{moveLeftVerticalStick,moveRightVerticalStick,moveLeftHorizontalStick,moveRightHorizontalStick,moveAccess0,moveAccess1,moveAccess2,moveFlightMode,centerAll,moveAll,nothing}; - enum txMovementType{vertical,horizontal,jump,mix}; - enum txType {acro, heli}; - void startInputWizard() { goToWizard(); } - void enableControls(bool enable); + ConfigInputWidget(QWidget *parent = 0); + ~ConfigInputWidget(); + enum wizardSteps { wizardWelcome, wizardChooseMode, wizardChooseType, wizardIdentifySticks, wizardIdentifyCenter, wizardIdentifyLimits, wizardIdentifyInverted, wizardFinish, wizardNone }; + enum txMode { mode1, mode2, mode3, mode4 }; + enum txMovements { moveLeftVerticalStick, moveRightVerticalStick, moveLeftHorizontalStick, moveRightHorizontalStick, moveAccess0, moveAccess1, moveAccess2, moveFlightMode, centerAll, moveAll, nothing }; + enum txMovementType { vertical, horizontal, jump, mix }; + enum txType { acro, heli }; + void startInputWizard() + { + goToWizard(); + } + void enableControls(bool enable); private: - bool growing; - bool reverse[ManualControlSettings::CHANNELNEUTRAL_NUMELEM]; - txMovements currentMovement; - int movePos; - void setTxMovement(txMovements movement); - Ui_InputWidget *ui; - wizardSteps wizardStep; - QList > extraWidgets; - txMode transmitterMode; - txType transmitterType; - struct channelsStruct + bool growing; + bool reverse[ManualControlSettings::CHANNELNEUTRAL_NUMELEM]; + txMovements currentMovement; + int movePos; + void setTxMovement(txMovements movement); + Ui_InputWidget *ui; + wizardSteps wizardStep; + QList > extraWidgets; + txMode transmitterMode; + txType transmitterType; + struct channelsStruct { + bool operator ==(const channelsStruct & rhs) const { - bool operator ==(const channelsStruct& rhs) const - { - return((group==rhs.group) &&(number==rhs.number)); - } - int group; - int number; - int channelIndex; - }lastChannel; - channelsStruct currentChannel; - QList usedChannels; - bool channelDetected; - QEventLoop * loop; - bool skipflag; + return (group == rhs.group) && (number == rhs.number); + } + int group; + int number; + int channelIndex; + } lastChannel; + channelsStruct currentChannel; + QList usedChannels; + bool channelDetected; + QEventLoop *loop; + bool skipflag; - int currentChannelNum; - QList heliChannelOrder; - QList acroChannelOrder; + int currentChannelNum; + QList heliChannelOrder; + QList acroChannelOrder; - ManualControlCommand * manualCommandObj; - ManualControlCommand::DataFields manualCommandData; - FlightStatus * flightStatusObj; - FlightStatus::DataFields flightStatusData; - AccessoryDesired * accessoryDesiredObj0; - AccessoryDesired * accessoryDesiredObj1; - AccessoryDesired * accessoryDesiredObj2; - AccessoryDesired::DataFields accessoryDesiredData0; - AccessoryDesired::DataFields accessoryDesiredData1; - AccessoryDesired::DataFields accessoryDesiredData2; - UAVObject::Metadata manualControlMdata; - ManualControlSettings * manualSettingsObj; - ManualControlSettings::DataFields manualSettingsData; - ManualControlSettings::DataFields previousManualSettingsData; - ReceiverActivity * receiverActivityObj; - ReceiverActivity::DataFields receiverActivityData; + ManualControlCommand *manualCommandObj; + ManualControlCommand::DataFields manualCommandData; + FlightStatus *flightStatusObj; + FlightStatus::DataFields flightStatusData; + AccessoryDesired *accessoryDesiredObj0; + AccessoryDesired *accessoryDesiredObj1; + AccessoryDesired *accessoryDesiredObj2; + AccessoryDesired::DataFields accessoryDesiredData0; + AccessoryDesired::DataFields accessoryDesiredData1; + AccessoryDesired::DataFields accessoryDesiredData2; + UAVObject::Metadata manualControlMdata; + ManualControlSettings *manualSettingsObj; + ManualControlSettings::DataFields manualSettingsData; + ManualControlSettings::DataFields previousManualSettingsData; + ReceiverActivity *receiverActivityObj; + ReceiverActivity::DataFields receiverActivityData; - QSvgRenderer *m_renderer; + QSvgRenderer *m_renderer; - // Background: background - QGraphicsSvgItem *m_txMainBody; - QGraphicsSvgItem *m_txLeftStick; - QGraphicsSvgItem *m_txRightStick; - QGraphicsSvgItem *m_txAccess0; - QGraphicsSvgItem *m_txAccess1; - QGraphicsSvgItem *m_txAccess2; - QGraphicsSvgItem *m_txFlightMode; - QGraphicsSvgItem *m_txBackground; - QGraphicsSvgItem *m_txArrows; - QTransform m_txLeftStickOrig; - QTransform m_txRightStickOrig; - QTransform m_txAccess0Orig; - QTransform m_txAccess1Orig; - QTransform m_txAccess2Orig; - QTransform m_txFlightModeCOrig; - QTransform m_txFlightModeLOrig; - QTransform m_txFlightModeROrig; - QTransform m_txMainBodyOrig; - QTransform m_txArrowsOrig; - QTimer * animate; - void resetTxControls(); - void setMoveFromCommand(int command); + // Background: background + QGraphicsSvgItem *m_txMainBody; + QGraphicsSvgItem *m_txLeftStick; + QGraphicsSvgItem *m_txRightStick; + QGraphicsSvgItem *m_txAccess0; + QGraphicsSvgItem *m_txAccess1; + QGraphicsSvgItem *m_txAccess2; + QGraphicsSvgItem *m_txFlightMode; + QGraphicsSvgItem *m_txBackground; + QGraphicsSvgItem *m_txArrows; + QTransform m_txLeftStickOrig; + QTransform m_txRightStickOrig; + QTransform m_txAccess0Orig; + QTransform m_txAccess1Orig; + QTransform m_txAccess2Orig; + QTransform m_txFlightModeCOrig; + QTransform m_txFlightModeLOrig; + QTransform m_txFlightModeROrig; + QTransform m_txMainBodyOrig; + QTransform m_txArrowsOrig; + QTimer *animate; + void resetTxControls(); + void setMoveFromCommand(int command); - void fastMdata(); - void restoreMdata(); + void fastMdata(); + void restoreMdata(); - void setChannel(int); - void nextChannel(); - void prevChannel(); + void setChannel(int); + void nextChannel(); + void prevChannel(); - void wizardSetUpStep(enum wizardSteps); - void wizardTearDownStep(enum wizardSteps); + void wizardSetUpStep(enum wizardSteps); + void wizardTearDownStep(enum wizardSteps); private slots: - void wzNext(); - void wzBack(); - void wzCancel(); - void goToWizard(); - void disableWizardButton(int); - void openHelp(); - void identifyControls(); - void identifyLimits(); - void moveTxControls(); - void moveSticks(); - void dimOtherControls(bool value); - void moveFMSlider(); - void updatePositionSlider(); - void invertControls(); - void simpleCalibration(bool state); - void updateCalibration(); + void wzNext(); + void wzBack(); + void wzCancel(); + void goToWizard(); + void disableWizardButton(int); + void openHelp(); + void identifyControls(); + void identifyLimits(); + void moveTxControls(); + void moveSticks(); + void dimOtherControls(bool value); + void moveFMSlider(); + void updatePositionSlider(); + void invertControls(); + void simpleCalibration(bool state); + void updateCalibration(); protected: - void resizeEvent(QResizeEvent *event); + void resizeEvent(QResizeEvent *event); }; -#endif +#endif // ifndef CONFIGINPUTWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp index d71918aba..65a9851af 100644 --- a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp @@ -49,18 +49,18 @@ #include #include -ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(parent),wasItMe(false) +ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(parent), wasItMe(false) { ui = new Ui_OutputWidget(); ui->setupUi(this); - + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Core::Internal::GeneralSettings *settings = pm->getObject(); - if(!settings->useExpertMode()) { + if (!settings->useExpertMode()) { ui->saveRCOutputToRAM->setVisible(false); } - UAVSettingsImportExportFactory *importexportplugin = pm->getObject(); + UAVSettingsImportExportFactory *importexportplugin = pm->getObject(); connect(importexportplugin, SIGNAL(importAboutToBegin()), this, SLOT(stopTests())); connect(ui->channelOutTest, SIGNAL(toggled(bool)), this, SLOT(runChannelTests(bool))); @@ -79,7 +79,7 @@ ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(paren for (unsigned int i = 0; i < ActuatorCommand::CHANNEL_NUMELEM; i++) { OutputChannelForm *form = new OutputChannelForm(i, this, i == 0); connect(ui->channelOutTest, SIGNAL(toggled(bool)), form, SLOT(enableChannelTest(bool))); - connect(form, SIGNAL(channelChanged(int,int)), this, SLOT(sendChannelTest(int,int))); + connect(form, SIGNAL(channelChanged(int, int)), this, SLOT(sendChannelTest(int, int))); ui->channelLayout->addWidget(form); addWidget(form->ui.actuatorMin); addWidget(form->ui.actuatorNeutral); @@ -97,14 +97,14 @@ ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(paren addWidget(ui->cb_outputRate1); addWidget(ui->spinningArmed); - disconnect(this, SLOT(refreshWidgetsValues(UAVObject*))); + disconnect(this, SLOT(refreshWidgetsValues(UAVObject *))); UAVObjectManager *objManager = pm->getObject(); - UAVObject* obj = objManager->getObject(QString("ActuatorCommand")); - if(UAVObject::GetGcsTelemetryUpdateMode(obj->getMetadata()) == UAVObject::UPDATEMODE_ONCHANGE) { + UAVObject *obj = objManager->getObject(QString("ActuatorCommand")); + if (UAVObject::GetGcsTelemetryUpdateMode(obj->getMetadata()) == UAVObject::UPDATEMODE_ONCHANGE) { this->setEnabled(false); } - connect(obj,SIGNAL(objectUpdated(UAVObject*)), this, SLOT(disableIfNotMe(UAVObject*))); + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(disableIfNotMe(UAVObject *))); refreshWidgetsValues(); updateEnableControls(); @@ -113,7 +113,8 @@ ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(paren void ConfigOutputWidget::enableControls(bool enable) { ConfigTaskWidget::enableControls(enable); - if(!enable) { + + if (!enable) { ui->channelOutTest->setChecked(false); } ui->channelOutTest->setEnabled(enable); @@ -121,19 +122,19 @@ void ConfigOutputWidget::enableControls(bool enable) ConfigOutputWidget::~ConfigOutputWidget() { - // Do nothing + // Do nothing } /** - Toggles the channel testing mode by making the GCS take over - the ActuatorCommand objects - */ + Toggles the channel testing mode by making the GCS take over + the ActuatorCommand objects + */ void ConfigOutputWidget::runChannelTests(bool state) { - SystemAlarms * systemAlarmsObj = SystemAlarms::GetInstance(getObjectManager()); + SystemAlarms *systemAlarmsObj = SystemAlarms::GetInstance(getObjectManager()); SystemAlarms::DataFields systemAlarms = systemAlarmsObj->getData(); - if(state && systemAlarms.Alarm[SystemAlarms::ALARM_ACTUATOR] != SystemAlarms::ALARM_OK) { + if (state && systemAlarms.Alarm[SystemAlarms::ALARM_ACTUATOR] != SystemAlarms::ALARM_OK) { QMessageBox mbox; mbox.setText(QString(tr("The actuator module is in an error state. This can also occur because there are no inputs. Please fix these before testing outputs."))); mbox.setStandardButtons(QMessageBox::Ok); @@ -147,12 +148,12 @@ void ConfigOutputWidget::runChannelTests(bool state) } // Confirm this is definitely what they want - if(state) { + if (state) { QMessageBox mbox; mbox.setText(QString(tr("This option will start your motors by the amount selected on the sliders regardless of transmitter. It is recommended to remove any blades from motors. Are you sure you want to do this?"))); mbox.setStandardButtons(QMessageBox::Yes | QMessageBox::No); int retval = mbox.exec(); - if(retval != QMessageBox::Yes) { + if (retval != QMessageBox::Yes) { state = false; qDebug() << "Cancelled"; ui->channelOutTest->setChecked(false); @@ -160,7 +161,7 @@ void ConfigOutputWidget::runChannelTests(bool state) } } - ActuatorCommand * obj = ActuatorCommand::GetInstance(getObjectManager()); + ActuatorCommand *obj = ActuatorCommand::GetInstance(getObjectManager()); UAVObject::Metadata mdata = obj->getMetadata(); if (state) { wasItMe = true; @@ -170,21 +171,19 @@ void ConfigOutputWidget::runChannelTests(bool state) UAVObject::SetGcsTelemetryAcked(mdata, false); UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); mdata.gcsTelemetryUpdatePeriod = 100; - } - else { + } else { wasItMe = false; - mdata = accInitialData; // Restore metadata + mdata = accInitialData; // Restore metadata } obj->setMetadata(mdata); obj->updated(); - } -OutputChannelForm* ConfigOutputWidget::getOutputChannelForm(const int index) const +OutputChannelForm *ConfigOutputWidget::getOutputChannelForm(const int index) const { - QList outputChannelForms = findChildren(); - foreach(OutputChannelForm *outputChannelForm, outputChannelForms) { - if(outputChannelForm->index() == index) { + QList outputChannelForms = findChildren(); + foreach(OutputChannelForm * outputChannelForm, outputChannelForms) { + if (outputChannelForm->index() == index) { return outputChannelForm; } } @@ -194,32 +193,33 @@ OutputChannelForm* ConfigOutputWidget::getOutputChannelForm(const int index) con } /** - * Set the label for a channel output assignement - */ + * Set the label for a channel output assignement + */ void ConfigOutputWidget::assignOutputChannel(UAVDataObject *obj, QString str) { - //FIXME: use signal/ slot approach - UAVObjectField* field = obj->getField(str); - QStringList options = field->getOptions(); + // FIXME: use signal/ slot approach + UAVObjectField *field = obj->getField(str); + QStringList options = field->getOptions(); int index = options.indexOf(field->getValue().toString()); OutputChannelForm *outputChannelForm = getOutputChannelForm(index); - if(outputChannelForm) { + + if (outputChannelForm) { outputChannelForm->setAssignment(str); } } /** - Sends the channel value to the UAV to move the servo. - Returns immediately if we are not in testing mode - */ + Sends the channel value to the UAV to move the servo. + Returns immediately if we are not in testing mode + */ void ConfigOutputWidget::sendChannelTest(int index, int value) { if (!ui->channelOutTest->isChecked()) { return; } - if(index < 0 || (unsigned)index >= ActuatorCommand::CHANNEL_NUMELEM) { + if (index < 0 || (unsigned)index >= ActuatorCommand::CHANNEL_NUMELEM) { return; } @@ -231,15 +231,14 @@ void ConfigOutputWidget::sendChannelTest(int index, int value) } - /******************************** - * Output settings - *******************************/ + * Output settings + *******************************/ /** - Request the current config from the board (RC Output) - */ -void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) + Request the current config from the board (RC Output) + */ +void ConfigOutputWidget::refreshWidgetsValues(UAVObject *obj) { Q_UNUSED(obj); @@ -252,8 +251,8 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) QStringList ChannelDesc = ConfigVehicleTypeWidget::getChannelDescriptions(); // Initialize output forms - QList outputChannelForms = findChildren(); - foreach(OutputChannelForm *outputChannelForm, outputChannelForms) { + QList outputChannelForms = findChildren(); + foreach(OutputChannelForm * outputChannelForm, outputChannelForms) { outputChannelForm->setAssignment(ChannelDesc[outputChannelForm->index()]); // init min,max,neutral @@ -261,7 +260,7 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) int maxValue = actuatorSettingsData.ChannelMax[outputChannelForm->index()]; outputChannelForm->minmax(minValue, maxValue); - int neutral = actuatorSettingsData.ChannelNeutral[outputChannelForm->index()]; + int neutral = actuatorSettingsData.ChannelNeutral[outputChannelForm->index()]; outputChannelForm->neutral(neutral); } @@ -269,22 +268,22 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) ui->spinningArmed->setChecked(actuatorSettingsData.MotorsSpinWhileArmed == ActuatorSettings::MOTORSSPINWHILEARMED_TRUE); // Setup output rates for all banks - if(ui->cb_outputRate1->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[0])) == -1) { + if (ui->cb_outputRate1->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[0])) == -1) { ui->cb_outputRate1->addItem(QString::number(actuatorSettingsData.ChannelUpdateFreq[0])); } - if(ui->cb_outputRate2->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[1])) == -1) { + if (ui->cb_outputRate2->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[1])) == -1) { ui->cb_outputRate2->addItem(QString::number(actuatorSettingsData.ChannelUpdateFreq[1])); } - if(ui->cb_outputRate3->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[2]) )== -1) { + if (ui->cb_outputRate3->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[2])) == -1) { ui->cb_outputRate3->addItem(QString::number(actuatorSettingsData.ChannelUpdateFreq[2])); } - if(ui->cb_outputRate4->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[3])) == -1) { + if (ui->cb_outputRate4->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[3])) == -1) { ui->cb_outputRate4->addItem(QString::number(actuatorSettingsData.ChannelUpdateFreq[3])); } - if(ui->cb_outputRate5->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[4])) == -1) { + if (ui->cb_outputRate5->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[4])) == -1) { ui->cb_outputRate5->addItem(QString::number(actuatorSettingsData.ChannelUpdateFreq[4])); } - if(ui->cb_outputRate6->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[5])) == -1) { + if (ui->cb_outputRate6->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[5])) == -1) { ui->cb_outputRate6->addItem(QString::number(actuatorSettingsData.ChannelUpdateFreq[5])); } ui->cb_outputRate1->setCurrentIndex(ui->cb_outputRate1->findText(QString::number(actuatorSettingsData.ChannelUpdateFreq[0]))); @@ -311,10 +310,10 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) // Get connected board model ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Q_ASSERT(pm); - UAVObjectUtilManager *utilMngr = pm->getObject(); + UAVObjectUtilManager *utilMngr = pm->getObject(); Q_ASSERT(utilMngr); - if(utilMngr) { + if (utilMngr) { int board = utilMngr->getBoardModel(); // Setup labels and combos for banks according to board type if ((board & 0xff00) == 0x0400) { @@ -327,8 +326,7 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) ui->cb_outputRate2->setEnabled(true); ui->cb_outputRate3->setEnabled(true); ui->cb_outputRate4->setEnabled(true); - } - else if((board & 0xff00) == 0x0900) { + } else if ((board & 0xff00) == 0x0900) { // Revolution family of boards 6 timer banks ui->chBank1->setText("1-2"); ui->chBank2->setText("3"); @@ -346,9 +344,10 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) } // Get Channel ranges: - foreach(OutputChannelForm *outputChannelForm, outputChannelForms) { - int minValue = actuatorSettingsData.ChannelMin[outputChannelForm->index()]; + foreach(OutputChannelForm * outputChannelForm, outputChannelForms) { + int minValue = actuatorSettingsData.ChannelMin[outputChannelForm->index()]; int maxValue = actuatorSettingsData.ChannelMax[outputChannelForm->index()]; + outputChannelForm->minmax(minValue, maxValue); int neutral = actuatorSettingsData.ChannelNeutral[outputChannelForm->index()]; @@ -357,22 +356,23 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject * obj) } /** - * Sends the config to the board, without saving to the SD card (RC Output) - */ + * Sends the config to the board, without saving to the SD card (RC Output) + */ void ConfigOutputWidget::updateObjectsFromWidgets() { emit updateObjectsFromWidgetsRequested(); ActuatorSettings *actuatorSettings = ActuatorSettings::GetInstance(getObjectManager()); + Q_ASSERT(actuatorSettings); - if(actuatorSettings) { + if (actuatorSettings) { ActuatorSettings::DataFields actuatorSettingsData = actuatorSettings->getData(); // Set channel ranges - QList outputChannelForms = findChildren(); - foreach(OutputChannelForm *outputChannelForm, outputChannelForms) { - actuatorSettingsData.ChannelMax[outputChannelForm->index()] = outputChannelForm->max(); - actuatorSettingsData.ChannelMin[outputChannelForm->index()] = outputChannelForm->min(); + QList outputChannelForms = findChildren(); + foreach(OutputChannelForm * outputChannelForm, outputChannelForms) { + actuatorSettingsData.ChannelMax[outputChannelForm->index()] = outputChannelForm->max(); + actuatorSettingsData.ChannelMin[outputChannelForm->index()] = outputChannelForm->min(); actuatorSettingsData.ChannelNeutral[outputChannelForm->index()] = outputChannelForm->neutral(); } @@ -385,8 +385,8 @@ void ConfigOutputWidget::updateObjectsFromWidgets() actuatorSettingsData.ChannelUpdateFreq[5] = ui->cb_outputRate6->currentText().toUInt(); actuatorSettingsData.MotorsSpinWhileArmed = ui->spinningArmed->isChecked() ? - ActuatorSettings::MOTORSSPINWHILEARMED_TRUE : - ActuatorSettings::MOTORSSPINWHILEARMED_FALSE; + ActuatorSettings::MOTORSSPINWHILEARMED_TRUE : + ActuatorSettings::MOTORSSPINWHILEARMED_FALSE; // Apply settings actuatorSettings->setData(actuatorSettingsData); @@ -395,7 +395,7 @@ void ConfigOutputWidget::updateObjectsFromWidgets() void ConfigOutputWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/WIGf", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/WIGf", QUrl::StrictMode)); } void ConfigOutputWidget::stopTests() @@ -403,14 +403,13 @@ void ConfigOutputWidget::stopTests() ui->channelOutTest->setChecked(false); } -void ConfigOutputWidget::disableIfNotMe(UAVObject* obj) +void ConfigOutputWidget::disableIfNotMe(UAVObject *obj) { - if(UAVObject::GetGcsTelemetryUpdateMode(obj->getMetadata()) == UAVObject::UPDATEMODE_ONCHANGE) { - if(!wasItMe) { + if (UAVObject::GetGcsTelemetryUpdateMode(obj->getMetadata()) == UAVObject::UPDATEMODE_ONCHANGE) { + if (!wasItMe) { this->setEnabled(false); } - } - else { + } else { this->setEnabled(true); } } diff --git a/ground/openpilotgcs/src/plugins/config/configoutputwidget.h b/ground/openpilotgcs/src/plugins/config/configoutputwidget.h index 1d15c8de5..1c12f64c0 100644 --- a/ground/openpilotgcs/src/plugins/config/configoutputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configoutputwidget.h @@ -40,40 +40,39 @@ class Ui_OutputWidget; class OutputChannelForm; -class ConfigOutputWidget: public ConfigTaskWidget -{ - Q_OBJECT +class ConfigOutputWidget : public ConfigTaskWidget { + Q_OBJECT public: - ConfigOutputWidget(QWidget *parent = 0); - ~ConfigOutputWidget(); + ConfigOutputWidget(QWidget *parent = 0); + ~ConfigOutputWidget(); private: Ui_OutputWidget *ui; - QList sliders; + QList sliders; - void updateChannelInSlider(QSlider *slider, QLabel *min, QLabel *max, QCheckBox *rev, int value); + void updateChannelInSlider(QSlider *slider, QLabel *min, QLabel *max, QCheckBox *rev, int value); - void assignChannel(UAVDataObject *obj, QString str); - void assignOutputChannel(UAVDataObject *obj, QString str); - OutputChannelForm* getOutputChannelForm(const int index) const; - int mccDataRate; + void assignChannel(UAVDataObject *obj, QString str); + void assignOutputChannel(UAVDataObject *obj, QString str); + OutputChannelForm *getOutputChannelForm(const int index) const; + int mccDataRate; - UAVObject::Metadata accInitialData; + UAVObject::Metadata accInitialData; - bool wasItMe; + bool wasItMe; private slots: - void stopTests(); - void disableIfNotMe(UAVObject *obj); - virtual void refreshWidgetsValues(UAVObject * obj=NULL); - void updateObjectsFromWidgets(); - void runChannelTests(bool state); - void sendChannelTest(int index, int value); - void openHelp(); + void stopTests(); + void disableIfNotMe(UAVObject *obj); + virtual void refreshWidgetsValues(UAVObject *obj = NULL); + void updateObjectsFromWidgets(); + void runChannelTests(bool state); + void sendChannelTest(int index, int value); + void openHelp(); protected: - void enableControls(bool enable); + void enableControls(bool enable); }; -#endif +#endif // ifndef CONFIGOUTPUTWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index a0e4c43de..9bbbab0d9 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -1,14 +1,14 @@ /** -****************************************************************************** -* -* @file configtxpidswidget.cpp -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup ConfigPlugin Config Plugin -* @{ -* @brief The Configuration Gadget used to configure the PipXtreme -*****************************************************************************/ + ****************************************************************************** + * + * @file configtxpidswidget.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief The Configuration Gadget used to configure the PipXtreme + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -33,148 +33,149 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget(parent) { - m_oplink = new Ui_PipXtremeWidget(); - m_oplink->setupUi(this); + m_oplink = new Ui_PipXtremeWidget(); + m_oplink->setupUi(this); - // Connect to the OPLinkStatus object updates - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - oplinkStatusObj = dynamic_cast(objManager->getObject("OPLinkStatus")); - if (oplinkStatusObj != NULL ) { - connect(oplinkStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateStatus(UAVObject*))); - } else { - qDebug() << "Error: Object is unknown (OPLinkStatus)."; - } + // Connect to the OPLinkStatus object updates + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + oplinkStatusObj = dynamic_cast(objManager->getObject("OPLinkStatus")); + if (oplinkStatusObj != NULL) { + connect(oplinkStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateStatus(UAVObject *))); + } else { + qDebug() << "Error: Object is unknown (OPLinkStatus)."; + } - // Connect to the OPLinkSettings object updates - oplinkSettingsObj = dynamic_cast(objManager->getObject("OPLinkSettings")); - if (oplinkSettingsObj != NULL ) { - connect(oplinkSettingsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateSettings(UAVObject*))); - } else { - qDebug() << "Error: Object is unknown (OPLinkSettings)."; - } - autoLoadWidgets(); - Core::Internal::GeneralSettings *settings = pm->getObject(); - if(!settings->useExpertMode()) - m_oplink->Apply->setVisible(false); - addApplySaveButtons(m_oplink->Apply, m_oplink->Save); + // Connect to the OPLinkSettings object updates + oplinkSettingsObj = dynamic_cast(objManager->getObject("OPLinkSettings")); + if (oplinkSettingsObj != NULL) { + connect(oplinkSettingsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateSettings(UAVObject *))); + } else { + qDebug() << "Error: Object is unknown (OPLinkSettings)."; + } + autoLoadWidgets(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { + m_oplink->Apply->setVisible(false); + } + addApplySaveButtons(m_oplink->Apply, m_oplink->Save); - addUAVObjectToWidgetRelation("OPLinkSettings", "MainPort", m_oplink->MainPort); - addUAVObjectToWidgetRelation("OPLinkSettings", "FlexiPort", m_oplink->FlexiPort); - addUAVObjectToWidgetRelation("OPLinkSettings", "VCPPort", m_oplink->VCPPort); - addUAVObjectToWidgetRelation("OPLinkSettings", "MaxRFPower", m_oplink->MaxRFTxPower); - addUAVObjectToWidgetRelation("OPLinkSettings", "MinFrequency", m_oplink->MinimumFrequency); - addUAVObjectToWidgetRelation("OPLinkSettings", "MaxFrequency", m_oplink->MaximumFrequency); - addUAVObjectToWidgetRelation("OPLinkSettings", "InitFrequency", m_oplink->InitFrequency); - addUAVObjectToWidgetRelation("OPLinkSettings", "ChannelSpacing", m_oplink->StepSize); + addUAVObjectToWidgetRelation("OPLinkSettings", "MainPort", m_oplink->MainPort); + addUAVObjectToWidgetRelation("OPLinkSettings", "FlexiPort", m_oplink->FlexiPort); + addUAVObjectToWidgetRelation("OPLinkSettings", "VCPPort", m_oplink->VCPPort); + addUAVObjectToWidgetRelation("OPLinkSettings", "MaxRFPower", m_oplink->MaxRFTxPower); + addUAVObjectToWidgetRelation("OPLinkSettings", "MinFrequency", m_oplink->MinimumFrequency); + addUAVObjectToWidgetRelation("OPLinkSettings", "MaxFrequency", m_oplink->MaximumFrequency); + addUAVObjectToWidgetRelation("OPLinkSettings", "InitFrequency", m_oplink->InitFrequency); + addUAVObjectToWidgetRelation("OPLinkSettings", "ChannelSpacing", m_oplink->StepSize); - addUAVObjectToWidgetRelation("OPLinkStatus", "DeviceID", m_oplink->DeviceID); - addUAVObjectToWidgetRelation("OPLinkStatus", "RxGood", m_oplink->Good); - addUAVObjectToWidgetRelation("OPLinkStatus", "RxCorrected", m_oplink->Corrected); - addUAVObjectToWidgetRelation("OPLinkStatus", "RxErrors", m_oplink->Errors); - addUAVObjectToWidgetRelation("OPLinkStatus", "RxMissed", m_oplink->Missed); - addUAVObjectToWidgetRelation("OPLinkStatus", "RxFailure", m_oplink->RxFailure); - addUAVObjectToWidgetRelation("OPLinkStatus", "UAVTalkErrors", m_oplink->UAVTalkErrors); - addUAVObjectToWidgetRelation("OPLinkStatus", "TxDropped", m_oplink->Dropped); - addUAVObjectToWidgetRelation("OPLinkStatus", "TxResent", m_oplink->Resent); - addUAVObjectToWidgetRelation("OPLinkStatus", "TxFailure", m_oplink->TxFailure); - addUAVObjectToWidgetRelation("OPLinkStatus", "Resets", m_oplink->Resets); - addUAVObjectToWidgetRelation("OPLinkStatus", "Timeouts", m_oplink->Timeouts); - addUAVObjectToWidgetRelation("OPLinkStatus", "RSSI", m_oplink->RSSI); - addUAVObjectToWidgetRelation("OPLinkStatus", "HeapRemaining", m_oplink->FreeHeap); - addUAVObjectToWidgetRelation("OPLinkStatus", "LinkQuality", m_oplink->LinkQuality); - addUAVObjectToWidgetRelation("OPLinkStatus", "RXSeq", m_oplink->RXSeq); - addUAVObjectToWidgetRelation("OPLinkStatus", "TXSeq", m_oplink->TXSeq); - addUAVObjectToWidgetRelation("OPLinkStatus", "RXRate", m_oplink->RXRate); - addUAVObjectToWidgetRelation("OPLinkStatus", "TXRate", m_oplink->TXRate); + addUAVObjectToWidgetRelation("OPLinkStatus", "DeviceID", m_oplink->DeviceID); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxGood", m_oplink->Good); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxCorrected", m_oplink->Corrected); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxErrors", m_oplink->Errors); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxMissed", m_oplink->Missed); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxFailure", m_oplink->RxFailure); + addUAVObjectToWidgetRelation("OPLinkStatus", "UAVTalkErrors", m_oplink->UAVTalkErrors); + addUAVObjectToWidgetRelation("OPLinkStatus", "TxDropped", m_oplink->Dropped); + addUAVObjectToWidgetRelation("OPLinkStatus", "TxResent", m_oplink->Resent); + addUAVObjectToWidgetRelation("OPLinkStatus", "TxFailure", m_oplink->TxFailure); + addUAVObjectToWidgetRelation("OPLinkStatus", "Resets", m_oplink->Resets); + addUAVObjectToWidgetRelation("OPLinkStatus", "Timeouts", m_oplink->Timeouts); + addUAVObjectToWidgetRelation("OPLinkStatus", "RSSI", m_oplink->RSSI); + addUAVObjectToWidgetRelation("OPLinkStatus", "HeapRemaining", m_oplink->FreeHeap); + addUAVObjectToWidgetRelation("OPLinkStatus", "LinkQuality", m_oplink->LinkQuality); + addUAVObjectToWidgetRelation("OPLinkStatus", "RXSeq", m_oplink->RXSeq); + addUAVObjectToWidgetRelation("OPLinkStatus", "TXSeq", m_oplink->TXSeq); + addUAVObjectToWidgetRelation("OPLinkStatus", "RXRate", m_oplink->RXRate); + addUAVObjectToWidgetRelation("OPLinkStatus", "TXRate", m_oplink->TXRate); - signalMapperAddBinding = new QSignalMapper(this); - signalMapperRemBinding = new QSignalMapper(this); - connect(m_oplink->BindingAdd_1, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_1, (QWidget*)(m_oplink->BindingID_1)); - connect(m_oplink->BindingRemove_1, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_1, (QWidget*)(m_oplink->BindingID_1)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_1, "0"); - connect(m_oplink->BindingAdd_2, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_2, (QWidget*)(m_oplink->BindingID_2)); - connect(m_oplink->BindingRemove_2, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_2, (QWidget*)(m_oplink->BindingID_2)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_2, "1"); - connect(m_oplink->BindingAdd_3, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_3, (QWidget*)(m_oplink->BindingID_3)); - connect(m_oplink->BindingRemove_3, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_3, (QWidget*)(m_oplink->BindingID_3)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_3, "2"); - connect(m_oplink->BindingAdd_4, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_4, (QWidget*)(m_oplink->BindingID_4)); - connect(m_oplink->BindingRemove_4, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_4, (QWidget*)(m_oplink->BindingID_4)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_4, "3"); - connect(m_oplink->BindingAdd_5, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_5, (QWidget*)(m_oplink->BindingID_5)); - connect(m_oplink->BindingRemove_5, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_5, (QWidget*)(m_oplink->BindingID_5)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_5, "4"); - connect(m_oplink->BindingAdd_6, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_6, (QWidget*)(m_oplink->BindingID_6)); - connect(m_oplink->BindingRemove_6, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_6, (QWidget*)(m_oplink->BindingID_6)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_6, "5"); - connect(m_oplink->BindingAdd_7, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_7, (QWidget*)(m_oplink->BindingID_7)); - connect(m_oplink->BindingRemove_7, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_7, (QWidget*)(m_oplink->BindingID_7)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_7, "6"); - connect(m_oplink->BindingAdd_8, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_8, (QWidget*)(m_oplink->BindingID_8)); - connect(m_oplink->BindingRemove_8, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_8, (QWidget*)(m_oplink->BindingID_8)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_8, "7"); - connect(signalMapperAddBinding, SIGNAL(mapped(QWidget *)), this, SLOT(addBinding(QWidget *))); - connect(signalMapperRemBinding, SIGNAL(mapped(QWidget *)), this, SLOT(removeBinding(QWidget *))); + signalMapperAddBinding = new QSignalMapper(this); + signalMapperRemBinding = new QSignalMapper(this); + connect(m_oplink->BindingAdd_1, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_1, (QWidget *)(m_oplink->BindingID_1)); + connect(m_oplink->BindingRemove_1, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_1, (QWidget *)(m_oplink->BindingID_1)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_1, "0"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_1, "0"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_1, "0"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_1, "0"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_1, "0"); + connect(m_oplink->BindingAdd_2, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_2, (QWidget *)(m_oplink->BindingID_2)); + connect(m_oplink->BindingRemove_2, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_2, (QWidget *)(m_oplink->BindingID_2)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_2, "1"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_2, "1"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_2, "1"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_2, "1"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_2, "1"); + connect(m_oplink->BindingAdd_3, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_3, (QWidget *)(m_oplink->BindingID_3)); + connect(m_oplink->BindingRemove_3, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_3, (QWidget *)(m_oplink->BindingID_3)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_3, "2"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_3, "2"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_3, "2"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_3, "2"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_3, "2"); + connect(m_oplink->BindingAdd_4, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_4, (QWidget *)(m_oplink->BindingID_4)); + connect(m_oplink->BindingRemove_4, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_4, (QWidget *)(m_oplink->BindingID_4)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_4, "3"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_4, "3"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_4, "3"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_4, "3"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_4, "3"); + connect(m_oplink->BindingAdd_5, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_5, (QWidget *)(m_oplink->BindingID_5)); + connect(m_oplink->BindingRemove_5, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_5, (QWidget *)(m_oplink->BindingID_5)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_5, "4"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_5, "4"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_5, "4"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_5, "4"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_5, "4"); + connect(m_oplink->BindingAdd_6, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_6, (QWidget *)(m_oplink->BindingID_6)); + connect(m_oplink->BindingRemove_6, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_6, (QWidget *)(m_oplink->BindingID_6)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_6, "5"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_6, "5"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_6, "5"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_6, "5"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_6, "5"); + connect(m_oplink->BindingAdd_7, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_7, (QWidget *)(m_oplink->BindingID_7)); + connect(m_oplink->BindingRemove_7, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_7, (QWidget *)(m_oplink->BindingID_7)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_7, "6"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_7, "6"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_7, "6"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_7, "6"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_7, "6"); + connect(m_oplink->BindingAdd_8, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_8, (QWidget *)(m_oplink->BindingID_8)); + connect(m_oplink->BindingRemove_8, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_8, (QWidget *)(m_oplink->BindingID_8)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_8, "7"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_8, "7"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_8, "7"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_8, "7"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_8, "7"); + connect(signalMapperAddBinding, SIGNAL(mapped(QWidget *)), this, SLOT(addBinding(QWidget *))); + connect(signalMapperRemBinding, SIGNAL(mapped(QWidget *)), this, SLOT(removeBinding(QWidget *))); - //Add scroll bar when necessary - QScrollArea *scroll = new QScrollArea; - scroll->setWidget(m_oplink->frame_3); - scroll->setWidgetResizable(true); - m_oplink->verticalLayout_3->addWidget(scroll); + // Add scroll bar when necessary + QScrollArea *scroll = new QScrollArea; + scroll->setWidget(m_oplink->frame_3); + scroll->setWidgetResizable(true); + m_oplink->verticalLayout_3->addWidget(scroll); - // Request and update of the setting object. - settingsUpdated = false; + // Request and update of the setting object. + settingsUpdated = false; - disableMouseWheelEvents(); + disableMouseWheelEvents(); } ConfigPipXtremeWidget::~ConfigPipXtremeWidget() @@ -184,145 +185,143 @@ ConfigPipXtremeWidget::~ConfigPipXtremeWidget() } /*! - \brief Called by updates to @OPLinkStatus - */ + \brief Called by updates to @OPLinkStatus + */ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) { + // Request and update of the setting object if we haven't received it yet. + if (!settingsUpdated) { + oplinkSettingsObj->requestUpdate(); + } - // Request and update of the setting object if we haven't received it yet. - if (!settingsUpdated) - oplinkSettingsObj->requestUpdate(); + // Update the detected devices. + UAVObjectField *pairIdField = object->getField("PairIDs"); + if (pairIdField) { + quint32 pairid1 = pairIdField->getValue(0).toUInt(); + m_oplink->PairID1->setText(QString::number(pairid1, 16).toUpper()); + m_oplink->PairID1->setEnabled(false); + m_oplink->PairSelect1->setEnabled(pairid1); + quint32 pairid2 = pairIdField->getValue(1).toUInt(); + m_oplink->PairID2->setText(QString::number(pairIdField->getValue(1).toUInt(), 16).toUpper()); + m_oplink->PairID2->setEnabled(false); + m_oplink->PairSelect2->setEnabled(pairid2); + quint32 pairid3 = pairIdField->getValue(2).toUInt(); + m_oplink->PairID3->setText(QString::number(pairIdField->getValue(2).toUInt(), 16).toUpper()); + m_oplink->PairID3->setEnabled(false); + m_oplink->PairSelect3->setEnabled(pairid3); + quint32 pairid4 = pairIdField->getValue(3).toUInt(); + m_oplink->PairID4->setText(QString::number(pairIdField->getValue(3).toUInt(), 16).toUpper()); + m_oplink->PairID4->setEnabled(false); + m_oplink->PairSelect4->setEnabled(pairid4); + } else { + qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; + } + UAVObjectField *pairRssiField = object->getField("PairSignalStrengths"); + if (pairRssiField) { + m_oplink->PairSignalStrengthBar1->setValue(pairRssiField->getValue(0).toInt()); + m_oplink->PairSignalStrengthBar2->setValue(pairRssiField->getValue(1).toInt()); + m_oplink->PairSignalStrengthBar3->setValue(pairRssiField->getValue(2).toInt()); + m_oplink->PairSignalStrengthBar4->setValue(pairRssiField->getValue(3).toInt()); + m_oplink->PairSignalStrengthLabel1->setText(QString("%1dB").arg(pairRssiField->getValue(0).toInt())); + m_oplink->PairSignalStrengthLabel2->setText(QString("%1dB").arg(pairRssiField->getValue(1).toInt())); + m_oplink->PairSignalStrengthLabel3->setText(QString("%1dB").arg(pairRssiField->getValue(2).toInt())); + m_oplink->PairSignalStrengthLabel4->setText(QString("%1dB").arg(pairRssiField->getValue(3).toInt())); + } else { + qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; + } - // Update the detected devices. - UAVObjectField* pairIdField = object->getField("PairIDs"); - if (pairIdField) { - quint32 pairid1 = pairIdField->getValue(0).toUInt(); - m_oplink->PairID1->setText(QString::number(pairid1, 16).toUpper()); - m_oplink->PairID1->setEnabled(false); - m_oplink->PairSelect1->setEnabled(pairid1); - quint32 pairid2 = pairIdField->getValue(1).toUInt(); - m_oplink->PairID2->setText(QString::number(pairIdField->getValue(1).toUInt(), 16).toUpper()); - m_oplink->PairID2->setEnabled(false); - m_oplink->PairSelect2->setEnabled(pairid2); - quint32 pairid3 = pairIdField->getValue(2).toUInt(); - m_oplink->PairID3->setText(QString::number(pairIdField->getValue(2).toUInt(), 16).toUpper()); - m_oplink->PairID3->setEnabled(false); - m_oplink->PairSelect3->setEnabled(pairid3); - quint32 pairid4 = pairIdField->getValue(3).toUInt(); - m_oplink->PairID4->setText(QString::number(pairIdField->getValue(3).toUInt(), 16).toUpper()); - m_oplink->PairID4->setEnabled(false); - m_oplink->PairSelect4->setEnabled(pairid4); - } else { - qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; - } - UAVObjectField* pairRssiField = object->getField("PairSignalStrengths"); - if (pairRssiField) { - m_oplink->PairSignalStrengthBar1->setValue(pairRssiField->getValue(0).toInt()); - m_oplink->PairSignalStrengthBar2->setValue(pairRssiField->getValue(1).toInt()); - m_oplink->PairSignalStrengthBar3->setValue(pairRssiField->getValue(2).toInt()); - m_oplink->PairSignalStrengthBar4->setValue(pairRssiField->getValue(3).toInt()); - m_oplink->PairSignalStrengthLabel1->setText(QString("%1dB").arg(pairRssiField->getValue(0).toInt())); - m_oplink->PairSignalStrengthLabel2->setText(QString("%1dB").arg(pairRssiField->getValue(1).toInt())); - m_oplink->PairSignalStrengthLabel3->setText(QString("%1dB").arg(pairRssiField->getValue(2).toInt())); - m_oplink->PairSignalStrengthLabel4->setText(QString("%1dB").arg(pairRssiField->getValue(3).toInt())); - } else { - qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; - } + // Update the Description field + UAVObjectField *descField = object->getField("Description"); + if (descField) { + /* + * This looks like a binary with a description at the end + * 4 bytes: header: "OpFw" + * 4 bytes: git commit hash (short version of SHA1) + * 4 bytes: Unix timestamp of last git commit + * 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files. + * 26 bytes: commit tag if it is there, otherwise "Unreleased". Zero-padded + * ---- 40 bytes limit --- + * 20 bytes: SHA1 sum of the firmware. + * 40 bytes: free for now. + */ + char buf[OPLinkStatus::DESCRIPTION_NUMELEM]; + for (unsigned int i = 0; i < 26; ++i) { + buf[i] = descField->getValue(i + 14).toChar().toAscii(); + } + buf[26] = '\0'; + QString descstr(buf); + quint32 gitDate = descField->getValue(11).toChar().toAscii() & 0xFF; + for (int i = 1; i < 4; i++) { + gitDate = gitDate << 8; + gitDate += descField->getValue(11 - i).toChar().toAscii() & 0xFF; + } + QString date = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyy-MM-dd HH:mm"); + m_oplink->FirmwareVersion->setText(descstr + " " + date); + } else { + qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; + } - // Update the Description field - UAVObjectField* descField = object->getField("Description"); - if (descField) { - /* - * This looks like a binary with a description at the end - * 4 bytes: header: "OpFw" - * 4 bytes: git commit hash (short version of SHA1) - * 4 bytes: Unix timestamp of last git commit - * 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files. - * 26 bytes: commit tag if it is there, otherwise "Unreleased". Zero-padded - * ---- 40 bytes limit --- - * 20 bytes: SHA1 sum of the firmware. - * 40 bytes: free for now. - */ - char buf[OPLinkStatus::DESCRIPTION_NUMELEM]; - for (unsigned int i = 0; i < 26; ++i) - buf[i] = descField->getValue(i + 14).toChar().toAscii(); - buf[26] = '\0'; - QString descstr(buf); - quint32 gitDate = descField->getValue(11).toChar().toAscii() & 0xFF; - for (int i = 1; i < 4; i++) { - gitDate = gitDate << 8; - gitDate += descField->getValue(11-i).toChar().toAscii() & 0xFF; - } - QString date = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyy-MM-dd HH:mm"); - m_oplink->FirmwareVersion->setText(descstr + " " + date); - } else { - qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; - } + // Update the serial number field + UAVObjectField *serialField = object->getField("CPUSerial"); + if (serialField) { + char buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2 + 1]; + for (unsigned int i = 0; i < OPLinkStatus::CPUSERIAL_NUMELEM; ++i) { + unsigned char val = serialField->getValue(i).toUInt() >> 4; + buf[i * 2] = ((val < 10) ? '0' : '7') + val; + val = serialField->getValue(i).toUInt() & 0xf; + buf[i * 2 + 1] = ((val < 10) ? '0' : '7') + val; + } + buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2] = '\0'; + m_oplink->SerialNumber->setText(buf); + } else { + qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; + } - // Update the serial number field - UAVObjectField* serialField = object->getField("CPUSerial"); - if (serialField) { - char buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2 + 1]; - for (unsigned int i = 0; i < OPLinkStatus::CPUSERIAL_NUMELEM; ++i) - { - unsigned char val = serialField->getValue(i).toUInt() >> 4; - buf[i * 2] = ((val < 10) ? '0' : '7') + val; - val = serialField->getValue(i).toUInt() & 0xf; - buf[i * 2 + 1] = ((val < 10) ? '0' : '7') + val; - } - buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2] = '\0'; - m_oplink->SerialNumber->setText(buf); - } else { - qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; - } - - // Update the link state - UAVObjectField* linkField = object->getField("LinkState"); - if (linkField) { - m_oplink->LinkState->setText(linkField->getValue().toString()); - } else { - qDebug() << "PipXtremeGadgetWidget: Count not read link state field."; - } + // Update the link state + UAVObjectField *linkField = object->getField("LinkState"); + if (linkField) { + m_oplink->LinkState->setText(linkField->getValue().toString()); + } else { + qDebug() << "PipXtremeGadgetWidget: Count not read link state field."; + } } /*! - \brief Called by updates to @OPLinkSettings - */ + \brief Called by updates to @OPLinkSettings + */ void ConfigPipXtremeWidget::updateSettings(UAVObject *object) { - Q_UNUSED(object); + Q_UNUSED(object); - if (!settingsUpdated) - { - settingsUpdated = true; - enableControls(true); - } + if (!settingsUpdated) { + settingsUpdated = true; + enableControls(true); + } } void ConfigPipXtremeWidget::disconnected() { - if (settingsUpdated) - { - settingsUpdated = false; - enableControls(false); - } + if (settingsUpdated) { + settingsUpdated = false; + enableControls(false); + } } void ConfigPipXtremeWidget::addBinding(QWidget *w) { - if(QLineEdit *le = qobject_cast(w)) { - + if (QLineEdit * le = qobject_cast(w)) { // Get the pair ID out of the selection widget quint32 pairID = 0; bool okay; - if (m_oplink->PairSelect1->isChecked()) + if (m_oplink->PairSelect1->isChecked()) { pairID = m_oplink->PairID1->text().toUInt(&okay, 16); - else if (m_oplink->PairSelect2->isChecked()) + } else if (m_oplink->PairSelect2->isChecked()) { pairID = m_oplink->PairID2->text().toUInt(&okay, 16); - else if (m_oplink->PairSelect3->isChecked()) + } else if (m_oplink->PairSelect3->isChecked()) { pairID = m_oplink->PairID3->text().toUInt(&okay, 16); - else if (m_oplink->PairSelect4->isChecked()) + } else if (m_oplink->PairSelect4->isChecked()) { pairID = m_oplink->PairID4->text().toUInt(&okay, 16); + } // Store the ID in the first open slot (or the last slot if all are full). le->setText(QString::number(pairID, 16).toUpper()); @@ -331,7 +330,7 @@ void ConfigPipXtremeWidget::addBinding(QWidget *w) void ConfigPipXtremeWidget::removeBinding(QWidget *w) { - if(QLineEdit *le = qobject_cast(w)) { + if (QLineEdit * le = qobject_cast(w)) { le->setText(QString::number(0, 16).toUpper()); } } @@ -339,4 +338,4 @@ void ConfigPipXtremeWidget::removeBinding(QWidget *w) /** @} @} -*/ + */ diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h index 9a612bf94..304ebc478 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h @@ -1,14 +1,14 @@ /** -****************************************************************************** -* -* @file configpipxtremewidget.h -* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup ConfigPlugin Config Plugin -* @{ -* @brief The Configuration Gadget used to configure PipXtreme -*****************************************************************************/ + ****************************************************************************** + * + * @file configpipxtremewidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief The Configuration Gadget used to configure PipXtreme + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -32,38 +32,37 @@ #include "ui_pipxtreme.h" #include "configtaskwidget.h" -class ConfigPipXtremeWidget : public ConfigTaskWidget -{ - Q_OBJECT +class ConfigPipXtremeWidget : public ConfigTaskWidget { + Q_OBJECT public: - ConfigPipXtremeWidget(QWidget *parent = 0); - ~ConfigPipXtremeWidget(); - + ConfigPipXtremeWidget(QWidget *parent = 0); + ~ConfigPipXtremeWidget(); + public slots: - void updateStatus(UAVObject *object1); - void updateSettings(UAVObject *object1); + void updateStatus(UAVObject *object1); + void updateSettings(UAVObject *object1); private: - Ui_PipXtremeWidget *m_oplink; + Ui_PipXtremeWidget *m_oplink; - // The OPLink status UAVObject - UAVDataObject *oplinkStatusObj; + // The OPLink status UAVObject + UAVDataObject *oplinkStatusObj; - // The OPLink ssettins UAVObject - OPLinkSettings* oplinkSettingsObj; + // The OPLink ssettins UAVObject + OPLinkSettings *oplinkSettingsObj; - // Are the settings current? - bool settingsUpdated; + // Are the settings current? + bool settingsUpdated; - // Signal mappers to add arguments to signals. - QSignalMapper *signalMapperAddBinding; - QSignalMapper *signalMapperRemBinding; + // Signal mappers to add arguments to signals. + QSignalMapper *signalMapperAddBinding; + QSignalMapper *signalMapperRemBinding; private slots: - void disconnected(); - void addBinding(QWidget *w); - void removeBinding(QWidget *w); + void disconnected(); + void addBinding(QWidget *w); + void removeBinding(QWidget *w); }; #endif // CONFIGTXPIDWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configplugin.cpp b/ground/openpilotgcs/src/plugins/config/configplugin.cpp index fd3a30544..21f696abf 100644 --- a/ground/openpilotgcs/src/plugins/config/configplugin.cpp +++ b/ground/openpilotgcs/src/plugins/config/configplugin.cpp @@ -34,59 +34,61 @@ ConfigPlugin::ConfigPlugin() { - // Do nothing + // Do nothing } ConfigPlugin::~ConfigPlugin() { - // Do nothing + // Do nothing } -bool ConfigPlugin::initialize(const QStringList& args, QString *errMsg) +bool ConfigPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - cf = new ConfigGadgetFactory(this); - addAutoReleasedObject(cf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + cf = new ConfigGadgetFactory(this); + addAutoReleasedObject(cf); - // Add Menu entry to erase all settings - Core::ActionManager* am = Core::ICore::instance()->actionManager(); - Core::ActionContainer* ac = am->actionContainer(Core::Constants::M_TOOLS); + // Add Menu entry to erase all settings + Core::ActionManager *am = Core::ICore::instance()->actionManager(); + Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_TOOLS); - // Command to erase all settings from the board - cmd = am->registerAction(new QAction(this), - "ConfigPlugin.EraseAll", - QList() << - Core::Constants::C_GLOBAL_ID); - cmd->action()->setText(tr("Erase all settings from board...")); + // Command to erase all settings from the board + cmd = am->registerAction(new QAction(this), + "ConfigPlugin.EraseAll", + QList() << + Core::Constants::C_GLOBAL_ID); + cmd->action()->setText(tr("Erase all settings from board...")); - ac->menu()->addSeparator(); - ac->appendGroup("Utilities"); - ac->addAction(cmd, "Utilities"); + ac->menu()->addSeparator(); + ac->appendGroup("Utilities"); + ac->addAction(cmd, "Utilities"); - connect(cmd->action(), SIGNAL(triggered(bool)), this, SLOT(eraseAllSettings())); + connect(cmd->action(), SIGNAL(triggered(bool)), this, SLOT(eraseAllSettings())); - // ********************* - // Listen to autopilot connection events - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - TelemetryManager* telMngr = pm->getObject(); - connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); - connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); + // ********************* + // Listen to autopilot connection events + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + TelemetryManager *telMngr = pm->getObject(); + connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); + connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); - // And check whether by any chance we are not already connected - if (telMngr->isConnected()) - onAutopilotConnect(); + // And check whether by any chance we are not already connected + if (telMngr->isConnected()) { + onAutopilotConnect(); + } - return true; + return true; } /** - * @brief Return handle to object manager - */ -UAVObjectManager * ConfigPlugin::getObjectManager() + * @brief Return handle to object manager + */ +UAVObjectManager *ConfigPlugin::getObjectManager() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * objMngr = pm->getObject(); + UAVObjectManager *objMngr = pm->getObject(); + Q_ASSERT(objMngr); return objMngr; } @@ -94,7 +96,6 @@ UAVObjectManager * ConfigPlugin::getObjectManager() void ConfigPlugin::extensionsInitialized() { cmd->action()->setEnabled(false); - } void ConfigPlugin::shutdown() @@ -103,16 +104,16 @@ void ConfigPlugin::shutdown() } /** - * Enable the menu entry when the autopilot connects - */ + * Enable the menu entry when the autopilot connects + */ void ConfigPlugin::onAutopilotConnect() { cmd->action()->setEnabled(true); } /** - * Enable the menu entry when the autopilot connects - */ + * Enable the menu entry when the autopilot connects + */ void ConfigPlugin::onAutopilotDisconnect() { cmd->action()->setEnabled(false); @@ -120,23 +121,25 @@ void ConfigPlugin::onAutopilotDisconnect() /** - * Erase all settings from the board - */ + * Erase all settings from the board + */ void ConfigPlugin::eraseAllSettings() { QMessageBox msgBox; + msgBox.setText(tr("Are you sure you want to erase all board settings?.")); msgBox.setInformativeText(tr("All settings stored in your board flash will be deleted.")); msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); msgBox.setDefaultButton(QMessageBox::Ok); - if (msgBox.exec() != QMessageBox::Ok) - return; + if (msgBox.exec() != QMessageBox::Ok) { + return; + } settingsErased = false; - ObjectPersistence* objper = ObjectPersistence::GetInstance(getObjectManager()); + ObjectPersistence *objper = ObjectPersistence::GetInstance(getObjectManager()); Q_ASSERT(objper); - connect(objper, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(eraseDone(UAVObject *))); + connect(objper, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(eraseDone(UAVObject *))); ObjectPersistence::DataFields data = objper->getData(); data.Operation = ObjectPersistence::OPERATION_FULLERASE; @@ -145,27 +148,27 @@ void ConfigPlugin::eraseAllSettings() // based on UAVO meta data objper->setData(data); objper->updated(); - QTimer::singleShot(FLASH_ERASE_TIMEOUT_MS,this,SLOT(eraseFailed())); - + QTimer::singleShot(FLASH_ERASE_TIMEOUT_MS, this, SLOT(eraseFailed())); } void ConfigPlugin::eraseFailed() { - if (settingsErased) + if (settingsErased) { return; + } - ObjectPersistence* objper = ObjectPersistence::GetInstance(getObjectManager()); + ObjectPersistence *objper = ObjectPersistence::GetInstance(getObjectManager()); ObjectPersistence::DataFields data = objper->getData(); - if(data.Operation == ObjectPersistence::OPERATION_FULLERASE) { + if (data.Operation == ObjectPersistence::OPERATION_FULLERASE) { // First attempt via flash erase failed. Fall back on erase all settings data.Operation = ObjectPersistence::OPERATION_DELETE; data.Selection = ObjectPersistence::SELECTION_ALLSETTINGS; objper->setData(data); objper->updated(); - QTimer::singleShot(FLASH_ERASE_TIMEOUT_MS,this,SLOT(eraseFailed())); + QTimer::singleShot(FLASH_ERASE_TIMEOUT_MS, this, SLOT(eraseFailed())); } else { - disconnect(objper, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(eraseDone(UAVObject *))); + disconnect(objper, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(eraseDone(UAVObject *))); QMessageBox msgBox; msgBox.setText(tr("Error trying to erase settings.")); msgBox.setInformativeText(tr("Power-cycle your board after removing all blades. Settings might be inconsistent.")); @@ -175,18 +178,19 @@ void ConfigPlugin::eraseFailed() } } -void ConfigPlugin::eraseDone(UAVObject * obj) +void ConfigPlugin::eraseDone(UAVObject *obj) { QMessageBox msgBox; - ObjectPersistence* objper = ObjectPersistence::GetInstance(getObjectManager()); + ObjectPersistence *objper = ObjectPersistence::GetInstance(getObjectManager()); ObjectPersistence::DataFields data = objper->getData(); + Q_ASSERT(obj->getInstID() == objper->getInstID()); - if(data.Operation != ObjectPersistence::OPERATION_COMPLETED) { + if (data.Operation != ObjectPersistence::OPERATION_COMPLETED) { return; } - disconnect(objper, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(eraseDone(UAVObject *))); + disconnect(objper, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(eraseDone(UAVObject *))); if (data.Operation == ObjectPersistence::OPERATION_COMPLETED) { settingsErased = true; msgBox.setText(tr("Settings are now erased.")); diff --git a/ground/openpilotgcs/src/plugins/config/configplugin.h b/ground/openpilotgcs/src/plugins/config/configplugin.h index d0d6ec09a..254f2cdc5 100644 --- a/ground/openpilotgcs/src/plugins/config/configplugin.h +++ b/ground/openpilotgcs/src/plugins/config/configplugin.h @@ -40,17 +40,16 @@ class ConfigGadgetFactory; -class ConfigPlugin : public ExtensionSystem::IPlugin -{ +class ConfigPlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: ConfigPlugin(); ~ConfigPlugin(); - UAVObjectManager * getObjectManager(); + UAVObjectManager *getObjectManager(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); private slots: @@ -60,13 +59,12 @@ private slots: void eraseDone(UAVObject *); void eraseFailed(); - private: +private: ConfigGadgetFactory *cf; - Core::Command* cmd; + Core::Command *cmd; bool settingsErased; static const int FLASH_ERASE_TIMEOUT_MS = 45000; - }; #endif // CONFIGPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp index b4416eb93..f04b3b493 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp @@ -41,34 +41,34 @@ ConfigRevoHWWidget::ConfigRevoHWWidget(QWidget *parent) : ConfigTaskWidget(paren ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Core::Internal::GeneralSettings *settings = pm->getObject(); - if(!settings->useExpertMode()) { + if (!settings->useExpertMode()) { m_ui->saveTelemetryToRAM->setEnabled(false); m_ui->saveTelemetryToRAM->setVisible(false); } addApplySaveButtons(m_ui->saveTelemetryToRAM, m_ui->saveTelemetryToSD); - addUAVObjectToWidgetRelation("HwSettings","RM_FlexiPort",m_ui->cbFlexi); - addUAVObjectToWidgetRelation("HwSettings","RM_MainPort",m_ui->cbMain); - addUAVObjectToWidgetRelation("HwSettings","RM_RcvrPort",m_ui->cbRcvr); + addUAVObjectToWidgetRelation("HwSettings", "RM_FlexiPort", m_ui->cbFlexi); + addUAVObjectToWidgetRelation("HwSettings", "RM_MainPort", m_ui->cbMain); + addUAVObjectToWidgetRelation("HwSettings", "RM_RcvrPort", m_ui->cbRcvr); - addUAVObjectToWidgetRelation("HwSettings","USB_HIDPort",m_ui->cbUSBHIDFunction); - addUAVObjectToWidgetRelation("HwSettings","USB_VCPPort",m_ui->cbUSBVCPFunction); - addUAVObjectToWidgetRelation("HwSettings","ComUsbBridgeSpeed",m_ui->cbUSBVCPSpeed); + addUAVObjectToWidgetRelation("HwSettings", "USB_HIDPort", m_ui->cbUSBHIDFunction); + addUAVObjectToWidgetRelation("HwSettings", "USB_VCPPort", m_ui->cbUSBVCPFunction); + addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_ui->cbUSBVCPSpeed); - addUAVObjectToWidgetRelation("HwSettings","TelemetrySpeed",m_ui->cbFlexiTelemSpeed); - addUAVObjectToWidgetRelation("HwSettings","GPSSpeed",m_ui->cbFlexiGPSSpeed); - addUAVObjectToWidgetRelation("HwSettings","ComUsbBridgeSpeed",m_ui->cbFlexiComSpeed); + addUAVObjectToWidgetRelation("HwSettings", "TelemetrySpeed", m_ui->cbFlexiTelemSpeed); + addUAVObjectToWidgetRelation("HwSettings", "GPSSpeed", m_ui->cbFlexiGPSSpeed); + addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_ui->cbFlexiComSpeed); - addUAVObjectToWidgetRelation("HwSettings","TelemetrySpeed",m_ui->cbMainTelemSpeed); - addUAVObjectToWidgetRelation("HwSettings","GPSSpeed",m_ui->cbMainGPSSpeed); - addUAVObjectToWidgetRelation("HwSettings","ComUsbBridgeSpeed",m_ui->cbMainComSpeed); + addUAVObjectToWidgetRelation("HwSettings", "TelemetrySpeed", m_ui->cbMainTelemSpeed); + addUAVObjectToWidgetRelation("HwSettings", "GPSSpeed", m_ui->cbMainGPSSpeed); + addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_ui->cbMainComSpeed); addUAVObjectToWidgetRelation("HwSettings", "RadioPort", m_ui->cbModem); addUAVObjectToWidgetRelation("HwSettings", "MaxRFPower", m_ui->cbTxPower); addUAVObjectToWidgetRelation("HwSettings", "DefaultFrequency", m_ui->leInitFreq); - connect(m_ui->cchwHelp,SIGNAL(clicked()),this,SLOT(openHelp())); + connect(m_ui->cchwHelp, SIGNAL(clicked()), this, SLOT(openHelp())); setupCustomCombos(); enableControls(true); @@ -94,12 +94,12 @@ void ConfigRevoHWWidget::setupCustomCombos() connect(m_ui->cbFlexi, SIGNAL(currentIndexChanged(int)), this, SLOT(flexiPortChanged(int))); connect(m_ui->cbMain, SIGNAL(currentIndexChanged(int)), this, SLOT(mainPortChanged(int))); connect(m_ui->cbModem, SIGNAL(currentIndexChanged(int)), this, SLOT(modemPortChanged(int))); - } void ConfigRevoHWWidget::refreshWidgetsValues(UAVObject *obj) { ConfigTaskWidget::refreshWidgetsValues(obj); + usbVCPPortChanged(0); mainPortChanged(0); flexiPortChanged(0); @@ -115,20 +115,18 @@ void ConfigRevoHWWidget::updateObjectsFromWidgets() // If any port is configured to be GPS port, enable GPS module if it is not enabled. // Otherwise disable GPS module. - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_GPS || m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_GPS) { + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_GPS || m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_GPS) { data.OptionalModules[HwSettings::OPTIONALMODULES_GPS] = HwSettings::OPTIONALMODULES_ENABLED; - } - else { + } else { data.OptionalModules[HwSettings::OPTIONALMODULES_GPS] = HwSettings::OPTIONALMODULES_DISABLED; } // If any port is configured to be ComBridge port, enable UsbComBridge module if it is not enabled. // Otherwise disable UsbComBridge module. - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_COMBRIDGE || m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_COMBRIDGE || - m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_COMBRIDGE) { + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_COMBRIDGE || m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_COMBRIDGE || + m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_COMBRIDGE) { data.OptionalModules[HwSettings::OPTIONALMODULES_COMUSBBRIDGE] = HwSettings::OPTIONALMODULES_ENABLED; - } - else { + } else { data.OptionalModules[HwSettings::OPTIONALMODULES_COMUSBBRIDGE] = HwSettings::OPTIONALMODULES_DISABLED; } @@ -144,31 +142,31 @@ void ConfigRevoHWWidget::usbVCPPortChanged(int index) m_ui->lblUSBVCPSpeed->setVisible(vcpComBridgeEnabled); m_ui->cbUSBVCPSpeed->setVisible(vcpComBridgeEnabled); - if(!vcpComBridgeEnabled && m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_COMBRIDGE) { + if (!vcpComBridgeEnabled && m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_COMBRIDGE) { m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); } m_ui->cbFlexi->model()->setData(m_ui->cbFlexi->model()->index(HwSettings::RM_FLEXIPORT_COMBRIDGE, 0), - !vcpComBridgeEnabled ? QVariant(0) : QVariant(1|32), Qt::UserRole - 1); + !vcpComBridgeEnabled ? QVariant(0) : QVariant(1 | 32), Qt::UserRole - 1); - if(!vcpComBridgeEnabled && m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_COMBRIDGE) { + if (!vcpComBridgeEnabled && m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_COMBRIDGE) { m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); } m_ui->cbMain->model()->setData(m_ui->cbMain->model()->index(HwSettings::RM_MAINPORT_COMBRIDGE, 0), - !vcpComBridgeEnabled ? QVariant(0) : QVariant(1|32), Qt::UserRole - 1); + !vcpComBridgeEnabled ? QVariant(0) : QVariant(1 | 32), Qt::UserRole - 1); - //_DEBUGCONSOLE modes are mutual exclusive - if(m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) { - if(m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_DEBUGCONSOLE) { + // _DEBUGCONSOLE modes are mutual exclusive + if (m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) { + if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_DEBUGCONSOLE) { m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); } - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_DEBUGCONSOLE) { + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_DEBUGCONSOLE) { m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); } } - //_USBTELEMETRY modes are mutual exclusive - if(m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_USBTELEMETRY) { - if(m_ui->cbUSBHIDFunction->currentIndex() == HwSettings::USB_HIDPORT_USBTELEMETRY) { + // _USBTELEMETRY modes are mutual exclusive + if (m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_USBTELEMETRY) { + if (m_ui->cbUSBHIDFunction->currentIndex() == HwSettings::USB_HIDPORT_USBTELEMETRY) { m_ui->cbUSBHIDFunction->setCurrentIndex(HwSettings::USB_HIDPORT_DISABLED); } } @@ -178,9 +176,9 @@ void ConfigRevoHWWidget::usbHIDPortChanged(int index) { Q_UNUSED(index); - //_USBTELEMETRY modes are mutual exclusive - if(m_ui->cbUSBHIDFunction->currentIndex() == HwSettings::USB_HIDPORT_USBTELEMETRY) { - if(m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_USBTELEMETRY) { + // _USBTELEMETRY modes are mutual exclusive + if (m_ui->cbUSBHIDFunction->currentIndex() == HwSettings::USB_HIDPORT_USBTELEMETRY) { + if (m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_USBTELEMETRY) { m_ui->cbUSBVCPFunction->setCurrentIndex(HwSettings::USB_VCPPORT_DISABLED); } } @@ -195,42 +193,41 @@ void ConfigRevoHWWidget::flexiPortChanged(int index) m_ui->cbFlexiComSpeed->setVisible(false); m_ui->lblFlexiSpeed->setVisible(true); - switch(m_ui->cbFlexi->currentIndex()) - { - case HwSettings::RM_FLEXIPORT_TELEMETRY: - m_ui->cbFlexiTelemSpeed->setVisible(true); - if(m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_TELEMETRY) { - m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); - } - if(m_ui->cbModem->currentIndex() == HwSettings::RADIOPORT_TELEMETRY) { - m_ui->cbModem->setCurrentIndex(HwSettings::RADIOPORT_DISABLED); - } - break; - case HwSettings::RM_FLEXIPORT_GPS: - m_ui->cbFlexiGPSSpeed->setVisible(true); - if(m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_GPS) { - m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); - } - break; - case HwSettings::RM_FLEXIPORT_COMBRIDGE: - m_ui->cbFlexiComSpeed->setVisible(true); - if(m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_COMBRIDGE) { - m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); - } - break; - case HwSettings::RM_FLEXIPORT_DEBUGCONSOLE: - m_ui->cbFlexiComSpeed->setVisible(true); - if(m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_DEBUGCONSOLE) { - m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); - } - if(m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) { - m_ui->cbUSBVCPFunction->setCurrentIndex(HwSettings::USB_VCPPORT_DISABLED); - } - break; - default: - m_ui->lblFlexiSpeed->setVisible(false); - break; + switch (m_ui->cbFlexi->currentIndex()) { + case HwSettings::RM_FLEXIPORT_TELEMETRY: + m_ui->cbFlexiTelemSpeed->setVisible(true); + if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_TELEMETRY) { + m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); } + if (m_ui->cbModem->currentIndex() == HwSettings::RADIOPORT_TELEMETRY) { + m_ui->cbModem->setCurrentIndex(HwSettings::RADIOPORT_DISABLED); + } + break; + case HwSettings::RM_FLEXIPORT_GPS: + m_ui->cbFlexiGPSSpeed->setVisible(true); + if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_GPS) { + m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); + } + break; + case HwSettings::RM_FLEXIPORT_COMBRIDGE: + m_ui->cbFlexiComSpeed->setVisible(true); + if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_COMBRIDGE) { + m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); + } + break; + case HwSettings::RM_FLEXIPORT_DEBUGCONSOLE: + m_ui->cbFlexiComSpeed->setVisible(true); + if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_DEBUGCONSOLE) { + m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); + } + if (m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) { + m_ui->cbUSBVCPFunction->setCurrentIndex(HwSettings::USB_VCPPORT_DISABLED); + } + break; + default: + m_ui->lblFlexiSpeed->setVisible(false); + break; + } } void ConfigRevoHWWidget::mainPortChanged(int index) @@ -242,41 +239,40 @@ void ConfigRevoHWWidget::mainPortChanged(int index) m_ui->cbMainComSpeed->setVisible(false); m_ui->lblMainSpeed->setVisible(true); - switch(m_ui->cbMain->currentIndex()) - { - case HwSettings::RM_MAINPORT_TELEMETRY: - m_ui->cbMainTelemSpeed->setVisible(true); - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_TELEMETRY) { - m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); - } - if(m_ui->cbModem->currentIndex() == HwSettings::RADIOPORT_TELEMETRY) { - m_ui->cbModem->setCurrentIndex(HwSettings::RADIOPORT_DISABLED); - } - break; - case HwSettings::RM_MAINPORT_GPS: - m_ui->cbMainGPSSpeed->setVisible(true); - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_GPS) { - m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); - } - break; - case HwSettings::RM_MAINPORT_COMBRIDGE: - m_ui->cbMainComSpeed->setVisible(true); - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_COMBRIDGE) { - m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); - } - break; - case HwSettings::RM_MAINPORT_DEBUGCONSOLE: - m_ui->cbMainComSpeed->setVisible(true); - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_DEBUGCONSOLE) { - m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); - } - if(m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) { - m_ui->cbUSBVCPFunction->setCurrentIndex(HwSettings::USB_VCPPORT_DISABLED); - } - break; - default: - m_ui->lblMainSpeed->setVisible(false); - break; + switch (m_ui->cbMain->currentIndex()) { + case HwSettings::RM_MAINPORT_TELEMETRY: + m_ui->cbMainTelemSpeed->setVisible(true); + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_TELEMETRY) { + m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); + } + if (m_ui->cbModem->currentIndex() == HwSettings::RADIOPORT_TELEMETRY) { + m_ui->cbModem->setCurrentIndex(HwSettings::RADIOPORT_DISABLED); + } + break; + case HwSettings::RM_MAINPORT_GPS: + m_ui->cbMainGPSSpeed->setVisible(true); + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_GPS) { + m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); + } + break; + case HwSettings::RM_MAINPORT_COMBRIDGE: + m_ui->cbMainComSpeed->setVisible(true); + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_COMBRIDGE) { + m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); + } + break; + case HwSettings::RM_MAINPORT_DEBUGCONSOLE: + m_ui->cbMainComSpeed->setVisible(true); + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_DEBUGCONSOLE) { + m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); + } + if (m_ui->cbUSBVCPFunction->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) { + m_ui->cbUSBVCPFunction->setCurrentIndex(HwSettings::USB_VCPPORT_DISABLED); + } + break; + default: + m_ui->lblMainSpeed->setVisible(false); + break; } } @@ -284,11 +280,11 @@ void ConfigRevoHWWidget::modemPortChanged(int index) { Q_UNUSED(index); - if(m_ui->cbModem->currentIndex()== HwSettings::RADIOPORT_TELEMETRY) { - if(m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_TELEMETRY) { + if (m_ui->cbModem->currentIndex() == HwSettings::RADIOPORT_TELEMETRY) { + if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_TELEMETRY) { m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); } - if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_TELEMETRY) { + if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_TELEMETRY) { m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); } m_ui->lblTxPower->setVisible(true); @@ -305,5 +301,5 @@ void ConfigRevoHWWidget::modemPortChanged(int index) void ConfigRevoHWWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/GgDBAQ", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/GgDBAQ", QUrl::StrictMode)); } diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.h b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.h index 7e6ed51f4..bfabea5b4 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.h @@ -36,8 +36,7 @@ #include -class ConfigRevoHWWidget: public ConfigTaskWidget -{ +class ConfigRevoHWWidget : public ConfigTaskWidget { Q_OBJECT public: @@ -49,7 +48,7 @@ private: void setupCustomCombos(); protected slots: - void refreshWidgetsValues(UAVObject * obj = NULL); + void refreshWidgetsValues(UAVObject *obj = NULL); void updateObjectsFromWidgets(); private slots: @@ -59,7 +58,6 @@ private slots: void mainPortChanged(int index); void modemPortChanged(int index); void openHelp(); - }; #endif // CONFIGREVOHWWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index b28693c09..5859a299f 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -837,35 +837,35 @@ void ConfigRevoWidget::computeScaleBias() void ConfigRevoWidget::storeAndClearBoardRotation() { - if(!isBoardRotationStored) { + if (!isBoardRotationStored) { // Store current board rotation isBoardRotationStored = true; AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); Q_ASSERT(attitudeSettings); - AttitudeSettings::DataFields data = attitudeSettings->getData(); - storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW] = data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]; - storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]; + AttitudeSettings::DataFields data = attitudeSettings->getData(); + storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW] = data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]; + storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]; storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH]; // Set board rotation to no rotation - data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = 0; - data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = 0; - data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = 0; + data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = 0; + data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = 0; + data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = 0; attitudeSettings->setData(data); } } void ConfigRevoWidget::recallBoardRotation() { - if(isBoardRotationStored) { + if (isBoardRotationStored) { // Recall current board rotation isBoardRotationStored = false; AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); Q_ASSERT(attitudeSettings); - AttitudeSettings::DataFields data = attitudeSettings->getData(); - data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW]; - data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL]; + AttitudeSettings::DataFields data = attitudeSettings->getData(); + data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW]; + data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL]; data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH]; attitudeSettings->setData(data); } @@ -1016,7 +1016,6 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject *obj) if (mag_accum_x.length() >= NOISE_SAMPLES && gyro_accum_x.length() >= NOISE_SAMPLES && accel_accum_x.length() >= NOISE_SAMPLES) { - // No need to for more updates Magnetometer *mags = Magnetometer::GetInstance(getObjectManager()); Accels *accels = Accels::GetInstance(getObjectManager()); diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h index d728a877b..7f6324779 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h @@ -37,8 +37,7 @@ #include -class ConfigStabilizationWidget: public ConfigTaskWidget -{ +class ConfigStabilizationWidget : public ConfigTaskWidget { Q_OBJECT public: @@ -47,7 +46,7 @@ public: private: Ui_StabilizationWidget *ui; - QTimer * realtimeUpdates; + QTimer *realtimeUpdates; // Milliseconds between automatic 'Instant Updates' static const int AUTOMATIC_UPDATE_RATE = 500; @@ -58,7 +57,7 @@ protected slots: private slots: void realtimeUpdatesSlot(bool value); void linkCheckBoxes(bool value); - void processLinkedWidgets(QWidget*); + void processLinkedWidgets(QWidget *); }; #endif // ConfigStabilizationWidget_H diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp index b279135bb..02902f5c8 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp @@ -35,11 +35,12 @@ ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent) { m_txpid = new Ui_TxPIDWidget(); m_txpid->setupUi(this); - - ExtensionSystem::PluginManager *pm=ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); - if(!settings->useExpertMode()) + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { m_txpid->Apply->setVisible(false); + } autoLoadWidgets(); addApplySaveButtons(m_txpid->Apply, m_txpid->Save); @@ -82,13 +83,14 @@ ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent) ConfigTxPIDWidget::~ConfigTxPIDWidget() { - // Do nothing + // Do nothing } void ConfigTxPIDWidget::refreshValues() { HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); HwSettings::DataFields hwSettingsData = hwSettings->getData(); + m_txpid->TxPIDEnable->setChecked( hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_TXPID] == HwSettings::OPTIONALMODULES_ENABLED); } @@ -97,6 +99,7 @@ void ConfigTxPIDWidget::applySettings() { HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); HwSettings::DataFields hwSettingsData = hwSettings->getData(); + hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_TXPID] = m_txpid->TxPIDEnable->isChecked() ? HwSettings::OPTIONALMODULES_ENABLED : HwSettings::OPTIONALMODULES_DISABLED; hwSettings->setData(hwSettingsData); diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h index 14eb5925d..38d2aa318 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h @@ -30,8 +30,7 @@ #include "ui_txpid.h" #include "configtaskwidget.h" -class ConfigTxPIDWidget : public ConfigTaskWidget -{ +class ConfigTxPIDWidget : public ConfigTaskWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp index af316585a..2e9a5f0fe 100644 --- a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp @@ -45,14 +45,15 @@ #include /** - Static function to get currently assigned channelDescriptions - for all known vehicle types; instantiates the appropriate object - then asks it to supply channel descs - */ + Static function to get currently assigned channelDescriptions + for all known vehicle types; instantiates the appropriate object + then asks it to supply channel descs + */ QStringList ConfigVehicleTypeWidget::getChannelDescriptions() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objMngr = pm->getObject(); + Q_ASSERT(objMngr); // get an instance of systemsettings @@ -104,9 +105,9 @@ ConfigVehicleTypeWidget::ConfigVehicleTypeWidget(QWidget *parent) : ConfigTaskWi { m_aircraft = new Ui_AircraftWidget(); m_aircraft->setupUi(this); - - ExtensionSystem::PluginManager *pm=ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); if (!settings->useExpertMode()) { m_aircraft->saveAircraftToRAM->setVisible(false); } @@ -125,11 +126,11 @@ ConfigVehicleTypeWidget::ConfigVehicleTypeWidget(QWidget *parent) : ConfigTaskWi m_aircraft->aircraftType->addItems(airframeTypes); // Set default vehicle to MultiRotor - //m_aircraft->aircraftType->setCurrentIndex(3); + // m_aircraft->aircraftType->setCurrentIndex(3); - // Connect aircraft type selection dropbox to callback function + // Connect aircraft type selection dropbox to callback function connect(m_aircraft->aircraftType, SIGNAL(currentIndexChanged(int)), this, SLOT(switchAirframeType(int))); - + // Connect the three feed forward test checkboxes connect(m_aircraft->ffTestBox1, SIGNAL(clicked(bool)), this, SLOT(enableFFTest())); connect(m_aircraft->ffTestBox2, SIGNAL(clicked(bool)), this, SLOT(enableFFTest())); @@ -157,25 +158,26 @@ ConfigVehicleTypeWidget::ConfigVehicleTypeWidget(QWidget *parent) : ConfigTaskWi } /** - Destructor + Destructor */ ConfigVehicleTypeWidget::~ConfigVehicleTypeWidget() { - // Do nothing + // Do nothing } void ConfigVehicleTypeWidget::switchAirframeType(int index) { // TODO not safe w/r to translation!!! QString frameCategory = m_aircraft->aircraftType->currentText(); + m_aircraft->airframesWidget->setCurrentWidget(getVehicleConfigWidget(frameCategory)); } /** - Refreshes the current value of the SystemSettings which holds the aircraft type - Note: The default behavior of ConfigTaskWidget is bypassed. - Therefore no automatic synchronization of UAV Objects to UI is done. - */ + Refreshes the current value of the SystemSettings which holds the aircraft type + Note: The default behavior of ConfigTaskWidget is bypassed. + Therefore no automatic synchronization of UAV Objects to UI is done. + */ void ConfigVehicleTypeWidget::refreshWidgetsValues(UAVObject *o) { Q_UNUSED(o); @@ -183,11 +185,11 @@ void ConfigVehicleTypeWidget::refreshWidgetsValues(UAVObject *o) if (!allObjectsUpdated()) { return; } - + bool dirty = isDirty(); - + // Get the Airframe type from the system settings: - UAVDataObject *system = dynamic_cast(getObjectManager()->getObject(QString("SystemSettings"))); + UAVDataObject *system = dynamic_cast(getObjectManager()->getObject(QString("SystemSettings"))); Q_ASSERT(system); UAVObjectField *field = system->getField(QString("AirframeType")); @@ -198,14 +200,14 @@ void ConfigVehicleTypeWidget::refreshWidgetsValues(UAVObject *o) QString frameType = field->getValue().toString(); qDebug() << "ConfigVehicleTypeWidget::refreshWidgetsValues - frame type:" << frameType; - QString category = frameCategory(frameType); + QString category = frameCategory(frameType); setComboCurrentIndex(m_aircraft->aircraftType, m_aircraft->aircraftType->findText(category)); VehicleConfig *vehicleConfig = getVehicleConfigWidget(category); if (vehicleConfig) { vehicleConfig->refreshWidgetsValues(frameType); } - + updateFeedForwardUI(); setDirty(dirty); @@ -214,21 +216,22 @@ void ConfigVehicleTypeWidget::refreshWidgetsValues(UAVObject *o) } /** - Sends the config to the board (airframe type) + Sends the config to the board (airframe type) - We do all the tasks common to all airframes, or family of airframes, and - we call additional methods for specific frames, so that we do not have a code - that is too heavy. + We do all the tasks common to all airframes, or family of airframes, and + we call additional methods for specific frames, so that we do not have a code + that is too heavy. - Note: The default behavior of ConfigTaskWidget is bypassed. - Therefore no automatic synchronization of UI to UAV Objects is done. -*/ + Note: The default behavior of ConfigTaskWidget is bypassed. + Therefore no automatic synchronization of UI to UAV Objects is done. + */ void ConfigVehicleTypeWidget::updateObjectsFromWidgets() { // Airframe type defaults to Custom QString airframeType = "Custom"; - VehicleConfig *vehicleConfig = (VehicleConfig *) m_aircraft->airframesWidget->currentWidget(); + VehicleConfig *vehicleConfig = (VehicleConfig *)m_aircraft->airframesWidget->currentWidget(); + if (vehicleConfig) { airframeType = vehicleConfig->updateConfigObjectsFromWidgets(); } @@ -260,21 +263,22 @@ void ConfigVehicleTypeWidget::updateObjectsFromWidgets() QString ConfigVehicleTypeWidget::frameCategory(QString frameType) { QString category; + if (frameType == "FixedWing" || frameType == "Elevator aileron rudder" || frameType == "FixedWingElevon" - || frameType == "Elevon" || frameType == "FixedWingVtail" || frameType == "Vtail") { + || frameType == "Elevon" || frameType == "FixedWingVtail" || frameType == "Vtail") { category = "Fixed Wing"; } else if (frameType == "Tri" || frameType == "Tricopter Y" || frameType == "QuadX" || frameType == "Quad X" - || frameType == "QuadP" || frameType == "Quad +" || frameType == "Hexa" || frameType == "Hexacopter" - || frameType == "HexaX" || frameType == "Hexacopter X" || frameType == "HexaCoax" - || frameType == "Hexacopter Y6" || frameType == "Octo" || frameType == "Octocopter" || frameType == "OctoV" - || frameType == "Octocopter V" || frameType == "OctoCoaxP" || frameType == "Octo Coax +" - || frameType == "OctoCoaxX" || frameType == "Octo Coax X") { + || frameType == "QuadP" || frameType == "Quad +" || frameType == "Hexa" || frameType == "Hexacopter" + || frameType == "HexaX" || frameType == "Hexacopter X" || frameType == "HexaCoax" + || frameType == "Hexacopter Y6" || frameType == "Octo" || frameType == "Octocopter" || frameType == "OctoV" + || frameType == "Octocopter V" || frameType == "OctoCoaxP" || frameType == "Octo Coax +" + || frameType == "OctoCoaxX" || frameType == "Octo Coax X") { category = "Multirotor"; } else if (frameType == "HeliCP") { category = "Helicopter"; } else if (frameType == "GroundVehicleCar" || frameType == "Turnable (car)" - || frameType == "GroundVehicleDifferential" || frameType == "Differential (tank)" - || frameType == "GroundVehicleMotorcyle" || frameType == "Motorcycle") { + || frameType == "GroundVehicleDifferential" || frameType == "Differential (tank)" + || frameType == "GroundVehicleMotorcyle" || frameType == "Motorcycle") { category = "Ground"; } else { category = "Custom"; @@ -285,6 +289,7 @@ QString ConfigVehicleTypeWidget::frameCategory(QString frameType) VehicleConfig *ConfigVehicleTypeWidget::getVehicleConfigWidget(QString frameCategory) { VehicleConfig *vehiculeConfig; + if (!vehicleIndexMap.contains(frameCategory)) { // create config widget vehiculeConfig = createVehicleConfigWidget(frameCategory); @@ -296,7 +301,7 @@ VehicleConfig *ConfigVehicleTypeWidget::getVehicleConfigWidget(QString frameCate vehicleIndexMap[frameCategory] = index; } int index = vehicleIndexMap.value(frameCategory); - vehiculeConfig = (VehicleConfig *) m_aircraft->airframesWidget->widget(index); + vehiculeConfig = (VehicleConfig *)m_aircraft->airframesWidget->widget(index); return vehiculeConfig; } @@ -318,8 +323,8 @@ VehicleConfig *ConfigVehicleTypeWidget::createVehicleConfigWidget(QString frameC } /** - Enables and runs feed forward testing - */ + Enables and runs feed forward testing + */ void ConfigVehicleTypeWidget::enableFFTest() { // Role: @@ -327,11 +332,11 @@ void ConfigVehicleTypeWidget::enableFFTest() // - Every other timer event: toggle engine from 45% to 55% // - Every other time event: send FF settings to flight FW if (m_aircraft->ffTestBox1->isChecked() && m_aircraft->ffTestBox2->isChecked() - && m_aircraft->ffTestBox3->isChecked()) { + && m_aircraft->ffTestBox3->isChecked()) { if (!ffTuningInProgress) { // Initiate tuning: - UAVDataObject *obj = dynamic_cast(getObjectManager()->getObject( - QString("ManualControlCommand"))); + UAVDataObject *obj = dynamic_cast(getObjectManager()->getObject( + QString("ManualControlCommand"))); UAVObject::Metadata mdata = obj->getMetadata(); accInitialData = mdata; UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY); @@ -340,7 +345,7 @@ void ConfigVehicleTypeWidget::enableFFTest() // Depending on phase, either move actuator or send FF settings: if (ffTuningPhase) { // Send FF settings to the board - UAVDataObject* mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); QPointer vconfig = new VehicleConfig(); @@ -353,9 +358,9 @@ void ConfigVehicleTypeWidget::enableFFTest() mixer->updated(); } else { // Toggle motor state - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject( - QString("ManualControlCommand"))); - double value = obj->getField("Throttle")->getDouble(); + UAVDataObject *obj = dynamic_cast(getObjectManager()->getObject( + QString("ManualControlCommand"))); + double value = obj->getField("Throttle")->getDouble(); double target = (value < 0.5) ? 0.55 : 0.45; obj->getField("Throttle")->setValue(target); obj->updated(); @@ -368,8 +373,8 @@ void ConfigVehicleTypeWidget::enableFFTest() // Disarm! if (ffTuningInProgress) { ffTuningInProgress = false; - UAVDataObject *obj = dynamic_cast(getObjectManager()->getObject( - QString("ManualControlCommand"))); + UAVDataObject *obj = dynamic_cast(getObjectManager()->getObject( + QString("ManualControlCommand"))); UAVObject::Metadata mdata = obj->getMetadata(); mdata = accInitialData; // Restore metadata obj->setMetadata(mdata); @@ -378,13 +383,14 @@ void ConfigVehicleTypeWidget::enableFFTest() } /** - Updates the custom airframe settings based on the current airframe. + Updates the custom airframe settings based on the current airframe. - Note: does NOT ask for an object refresh itself! - */ + Note: does NOT ask for an object refresh itself! + */ void ConfigVehicleTypeWidget::updateFeedForwardUI() { - UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + Q_ASSERT(mixer); QPointer vconfig = new VehicleConfig(); @@ -397,19 +403,19 @@ void ConfigVehicleTypeWidget::updateFeedForwardUI() } /** - Opens the wiki from the user's default browser + Opens the wiki from the user's default browser */ void ConfigVehicleTypeWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/44Cf", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/44Cf", QUrl::StrictMode)); } /** - Helper function: - Sets the current index on supplied combobox to index - if it is within bounds 0 <= index < combobox.count() + Helper function: + Sets the current index on supplied combobox to index + if it is within bounds 0 <= index < combobox.count() */ -void ConfigVehicleTypeWidget::setComboCurrentIndex(QComboBox* box, int index) +void ConfigVehicleTypeWidget::setComboCurrentIndex(QComboBox *box, int index) { if (index >= 0 && index < box->count()) { box->setCurrentIndex(index); diff --git a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.h b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.h index 6184fd6fb..5a065865c 100644 --- a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.h @@ -53,8 +53,7 @@ * TODO consider to call "super" to benefit from default logic... * TODO improve handling of relationship with VehicleConfig derived classes (i.e. ConfigTaskWidget within ConfigTaskWidget) */ -class ConfigVehicleTypeWidget: public ConfigTaskWidget -{ +class ConfigVehicleTypeWidget : public ConfigTaskWidget { Q_OBJECT public: @@ -90,7 +89,6 @@ private slots: void switchAirframeType(int index); void openHelp(); void enableFFTest(); - }; #endif // CONFIGVEHICLETYPEWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/dblspindelegate.cpp b/ground/openpilotgcs/src/plugins/config/dblspindelegate.cpp index 21af3f1d2..8c7eda9b1 100644 --- a/ground/openpilotgcs/src/plugins/config/dblspindelegate.cpp +++ b/ground/openpilotgcs/src/plugins/config/dblspindelegate.cpp @@ -28,45 +28,48 @@ #include "dblspindelegate.h" /** - Helper delegate for the custom mixer editor table. - */ + Helper delegate for the custom mixer editor table. + */ DoubleSpinDelegate::DoubleSpinDelegate(QObject *parent) - : QItemDelegate(parent) - { - m_min = 0.0; - m_max = 1.0; + : QItemDelegate(parent) +{ + m_min = 0.0; + m_max = 1.0; m_decimals = 2; - m_step = 0.01; - } + m_step = 0.01; +} QWidget *DoubleSpinDelegate::createEditor(QWidget *parent, - const QStyleOptionViewItem &/* option */, - const QModelIndex &/* index */) const + const QStyleOptionViewItem & /* option */, + const QModelIndex & /* index */) const { QDoubleSpinBox *editor = new QDoubleSpinBox(parent); + editor->setMinimum(m_min); editor->setMaximum(m_max); editor->setDecimals(m_decimals); editor->setSingleStep(m_step); - connect(editor,SIGNAL(valueChanged(double)), this, SLOT(valueChanged())); + connect(editor, SIGNAL(valueChanged(double)), this, SLOT(valueChanged())); return editor; } void DoubleSpinDelegate::setEditorData(QWidget *editor, - const QModelIndex &index) const + const QModelIndex &index) const { double value = index.model()->data(index, Qt::EditRole).toDouble(); - QDoubleSpinBox *spinBox = static_cast(editor); + QDoubleSpinBox *spinBox = static_cast(editor); + spinBox->setValue(value); } void DoubleSpinDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, - const QModelIndex &index) const + const QModelIndex &index) const { - QDoubleSpinBox *spinBox = static_cast(editor); + QDoubleSpinBox *spinBox = static_cast(editor); + spinBox->interpretText(); double value = spinBox->value(); @@ -74,7 +77,7 @@ void DoubleSpinDelegate::setModelData(QWidget *editor, QAbstractItemModel *model } void DoubleSpinDelegate::updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &/* index */) const + const QStyleOptionViewItem &option, const QModelIndex & /* index */) const { editor->setGeometry(option.rect); } @@ -83,4 +86,3 @@ void DoubleSpinDelegate::valueChanged() { emit ValueChanged(); } - diff --git a/ground/openpilotgcs/src/plugins/config/dblspindelegate.h b/ground/openpilotgcs/src/plugins/config/dblspindelegate.h index 482a07649..0781a7847 100644 --- a/ground/openpilotgcs/src/plugins/config/dblspindelegate.h +++ b/ground/openpilotgcs/src/plugins/config/dblspindelegate.h @@ -34,8 +34,7 @@ namespace Ui { class DoubleSpinDelegate; } -class DoubleSpinDelegate : public QItemDelegate -{ +class DoubleSpinDelegate : public QItemDelegate { Q_OBJECT public: @@ -49,19 +48,34 @@ public: const QModelIndex &index) const; void updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &index) const; + const QStyleOptionViewItem &option, const QModelIndex &index) const; - void setMin(double min) { m_min = min; } - void setMax(double max) { m_max = max; } - void setRange(double min, double max) { m_min = min; m_max = max; } - void setStep(double step) { m_step = step; } - void setDecimals(int decimals) { m_decimals = decimals; } + void setMin(double min) + { + m_min = min; + } + void setMax(double max) + { + m_max = max; + } + void setRange(double min, double max) + { + m_min = min; m_max = max; + } + void setStep(double step) + { + m_step = step; + } + void setDecimals(int decimals) + { + m_decimals = decimals; + } private: - double m_min; - double m_max; - double m_step; - int m_decimals; + double m_min; + double m_max; + double m_step; + int m_decimals; signals: void ValueChanged(); diff --git a/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.cpp index 4705bb02e..e221aebe2 100644 --- a/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.cpp @@ -31,8 +31,8 @@ #include DefaultAttitudeWidget::DefaultAttitudeWidget(QWidget *parent) : - QWidget(parent), - ui(new Ui_defaultattitude) + QWidget(parent), + ui(new Ui_defaultattitude) { ui->setupUi(this); } @@ -41,4 +41,3 @@ DefaultAttitudeWidget::~DefaultAttitudeWidget() { delete ui; } - diff --git a/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.h b/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.h index 8eb5cfa34..a21541d83 100644 --- a/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.h +++ b/ground/openpilotgcs/src/plugins/config/defaultattitudewidget.h @@ -38,8 +38,7 @@ class Ui_Widget; -class DefaultAttitudeWidget : public QWidget -{ +class DefaultAttitudeWidget : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.cpp b/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.cpp index 69eef9117..21b9274e4 100644 --- a/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.cpp @@ -31,8 +31,8 @@ #include DefaultHwSettingsWidget::DefaultHwSettingsWidget(QWidget *parent) : - QWidget(parent), - ui(new Ui_defaulthwsettings) + QWidget(parent), + ui(new Ui_defaulthwsettings) { ui->setupUi(this); } @@ -41,4 +41,3 @@ DefaultHwSettingsWidget::~DefaultHwSettingsWidget() { delete ui; } - diff --git a/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.h b/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.h index 1d498c35a..604a7f019 100644 --- a/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.h +++ b/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.h @@ -38,8 +38,7 @@ class Ui_Widget; -class DefaultHwSettingsWidget : public QWidget -{ +class DefaultHwSettingsWidget : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/fancytabwidget.cpp b/ground/openpilotgcs/src/plugins/config/fancytabwidget.cpp index bb0b65b09..6a5629ec3 100644 --- a/ground/openpilotgcs/src/plugins/config/fancytabwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/fancytabwidget.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,15 +44,15 @@ #include #include -const int FancyTabBar::m_rounding = 22; +const int FancyTabBar::m_rounding = 22; const int FancyTabBar::m_textPadding = 4; FancyTabBar::FancyTabBar(QWidget *parent, bool isVertical) : QWidget(parent) { - verticalTabs = isVertical; + verticalTabs = isVertical; setIconSize(16); - m_hoverIndex = -1; + m_hoverIndex = -1; m_currentIndex = 0; if (isVertical) { setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); @@ -78,11 +78,12 @@ FancyTabBar::~FancyTabBar() QSize FancyTabBar::tabSizeHint(bool minimum) const { QFont boldFont(font()); + boldFont.setPointSizeF(Utils::StyleHelper::sidebarFontSize()); boldFont.setBold(true); QFontMetrics fm(boldFont); - int spacing = 6; - int width = 90 + spacing + 2; + int spacing = 6; + int width = 90 + spacing + 2; int iconHeight = minimum ? 0 : iconSize; return QSize(width, iconHeight + spacing + fm.height()); @@ -93,9 +94,11 @@ void FancyTabBar::paintEvent(QPaintEvent *event) Q_UNUSED(event) QPainter p(this); - for (int i = 0; i < count(); ++i) - if (i != currentIndex()) + for (int i = 0; i < count(); ++i) { + if (i != currentIndex()) { paintTab(&p, i); + } + } // paint active tab last, since it overlaps the neighbors paintTab(&p, currentIndex()); @@ -117,9 +120,9 @@ void FancyTabBar::mouseMoveEvent(QMouseEvent *e) m_hoverControl.stop(); m_hoverIndex = newHover; update(m_hoverRect); - m_hoverRect = QRect(); + m_hoverRect = QRect(); - if (m_hoverIndex >=0) { + if (m_hoverIndex >= 0) { QRect oldHoverRect = m_hoverRect; m_hoverRect = tabRect(m_hoverIndex); m_hoverControl.start(); @@ -133,7 +136,7 @@ bool FancyTabBar::event(QEvent *event) if (m_hoverIndex >= 0 && m_hoverIndex < m_tabs.count()) { QString tt = tabToolTip(m_hoverIndex); if (!tt.isEmpty()) { - QToolTip::showText(static_cast(event)->globalPos(), tt, this); + QToolTip::showText(static_cast(event)->globalPos(), tt, this); return true; } } @@ -150,7 +153,7 @@ void FancyTabBar::updateHover() void FancyTabBar::enterEvent(QEvent *e) { Q_UNUSED(e) - m_hoverRect = QRect(); + m_hoverRect = QRect(); m_hoverIndex = -1; } @@ -161,7 +164,7 @@ void FancyTabBar::leaveEvent(QEvent *e) if (m_hoverIndex >= 0) { m_hoverIndex = -1; update(m_hoverRect); - m_hoverRect = QRect(); + m_hoverRect = QRect(); } } @@ -174,16 +177,20 @@ void FancyTabBar::updateTabNameIcon(int index, const QIcon &icon, const QString QSize FancyTabBar::sizeHint() const { QSize sh = tabSizeHint(); - if (verticalTabs) + + if (verticalTabs) { return QSize(sh.width(), sh.height() * m_tabs.count()); + } return QSize(sh.width() * m_tabs.count(), sh.height()); } QSize FancyTabBar::minimumSizeHint() const { QSize sh = tabSizeHint(true); - if (verticalTabs) + + if (verticalTabs) { return QSize(sh.width(), sh.height() * m_tabs.count()); + } return QSize(sh.width() * m_tabs.count(), sh.height()); } @@ -192,18 +199,18 @@ QRect FancyTabBar::tabRect(int index) const QSize sh = tabSizeHint(); if (verticalTabs) { - if (sh.height() * m_tabs.count() > height()) + if (sh.height() * m_tabs.count() > height()) { sh.setHeight(height() / m_tabs.count()); + } return QRect(0, index * sh.height(), sh.width(), sh.height()); - } - if(sh.width() * m_tabs.count() > width()) + if (sh.width() * m_tabs.count() > width()) { sh.setWidth(width() / m_tabs.count()); + } return QRect(index * sh.width(), 0, sh.width(), sh.height()); - } void FancyTabBar::mousePressEvent(QMouseEvent *e) @@ -221,10 +228,10 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const { painter->save(); - QRect rect = tabRect(tabIndex); + QRect rect = tabRect(tabIndex); bool selected = (tabIndex == m_currentIndex); - bool hover = (tabIndex == m_hoverIndex); + bool hover = (tabIndex == m_hoverIndex); #ifdef Q_WS_MAC hover = false; // Dont hover on Mac @@ -238,13 +245,13 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const } QColor light = QColor(255, 255, 255, 40); - QColor dark = QColor(0, 0, 0, 60); + QColor dark = QColor(0, 0, 0, 60); if (selected) { QLinearGradient selectedGradient(rect.bottomRight(), QPoint(rect.center().x(), rect.top())); selectedGradient.setColorAt(0, Qt::white); selectedGradient.setColorAt(0.3, Qt::white); - selectedGradient.setColorAt(0.7, QColor(210, 210, 220)); //give a blue-ish color + selectedGradient.setColorAt(0.7, QColor(210, 210, 220)); // give a blue-ish color painter->fillRect(rect, selectedGradient); painter->setPen(QColor(200, 200, 200)); @@ -253,8 +260,9 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const painter->drawLine(rect.topRight(), rect.bottomRight()); } else { painter->fillRect(rect, background); - if (hover) + if (hover) { painter->fillRect(rect, hoverColor); + } painter->setPen(QPen(light, 0)); painter->drawLine(rect.topLeft(), rect.bottomLeft()); painter->setPen(QPen(dark, 0)); @@ -269,25 +277,29 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const boldFont.setBold(true); painter->setFont(boldFont); painter->setPen(selected ? Utils::StyleHelper::panelTextColor() : QColor(30, 30, 30, 80)); - int textFlags = Qt::AlignCenter | Qt::AlignBottom | Qt::ElideRight | Qt::TextWordWrap; + int textFlags = Qt::AlignCenter | Qt::AlignBottom | Qt::ElideRight | Qt::TextWordWrap; painter->drawText(tabTextRect, textFlags, tabText); painter->setPen(selected ? QColor(60, 60, 60) : Utils::StyleHelper::panelTextColor()); int textHeight = painter->fontMetrics().boundingRect(QRect(0, 0, width(), height()), Qt::TextWordWrap, tabText).height(); tabIconRect.adjust(0, 4, 0, -textHeight); - int iconSize = qMin(tabIconRect.width(), tabIconRect.height()); - if (iconSize > 4) + int iconSize = qMin(tabIconRect.width(), tabIconRect.height()); + if (iconSize > 4) { style()->drawItemPixmap(painter, tabIconRect, Qt::AlignCenter | Qt::AlignVCenter, tabIcon(tabIndex).pixmap(tabIconRect.size())); + } painter->translate(0, -1); painter->drawText(tabTextRect, textFlags, tabText); painter->restore(); } -void FancyTabBar::setCurrentIndex(int index) { - bool proceed=true; +void FancyTabBar::setCurrentIndex(int index) +{ + bool proceed = true; emit aboutToChange(&proceed); - if(!proceed) + + if (!proceed) { return; + } m_currentIndex = index; update(); emit currentChanged(index); @@ -297,19 +309,19 @@ void FancyTabBar::setCurrentIndex(int index) { // FancyColorButton ////// -class FancyColorButton : public QWidget -{ +class FancyColorButton : public QWidget { public: FancyColorButton(QWidget *parent) - : m_parent(parent) + : m_parent(parent) { setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Preferred); } void mousePressEvent(QMouseEvent *ev) { - if (ev->modifiers() & Qt::ShiftModifier) + if (ev->modifiers() & Qt::ShiftModifier) { Utils::StyleHelper::setBaseColor(QColorDialog::getColor(Utils::StyleHelper::baseColor(), m_parent)); + } } private: QWidget *m_parent; @@ -375,22 +387,24 @@ FancyTabWidget::FancyTabWidget(QWidget *parent, bool isVertical) selectionLayout->addWidget(m_cornerWidgetContainer, 0); m_modesStack = new QStackedLayout; - m_statusBar = new QStatusBar; + m_statusBar = new QStatusBar; m_statusBar->setSizePolicy(QSizePolicy::Ignored, QSizePolicy::Fixed); QVBoxLayout *vlayout = new QVBoxLayout; vlayout->setMargin(0); vlayout->setSpacing(0); vlayout->addLayout(m_modesStack); - if (!isVertical) + if (!isVertical) { vlayout->addWidget(m_selectionWidget); -// vlayout->addWidget(m_statusBar); //status bar is not used for now + } +// vlayout->addWidget(m_statusBar); //status bar is not used for now QHBoxLayout *mainLayout = new QHBoxLayout; mainLayout->setMargin(0); mainLayout->setSpacing(1); - if (isVertical) + if (isVertical) { mainLayout->addWidget(m_selectionWidget); + } mainLayout->addLayout(vlayout); setLayout(mainLayout); @@ -419,6 +433,7 @@ void FancyTabWidget::updateTabNameIcon(int index, const QIcon &icon, const QStri void FancyTabWidget::setBackgroundBrush(const QBrush &brush) { QPalette pal = m_tabBar->palette(); + pal.setBrush(QPalette::Mid, brush); m_tabBar->setPalette(pal); pal = m_cornerWidgetContainer->palette(); @@ -441,6 +456,7 @@ void FancyTabWidget::paintEvent(QPaintEvent *event) void FancyTabWidget::insertCornerWidget(int pos, QWidget *widget) { QHBoxLayout *layout = static_cast(m_cornerWidgetContainer->layout()); + layout->insertWidget(pos, widget); } @@ -472,6 +488,7 @@ void FancyTabWidget::setCurrentIndex(int index) void FancyTabWidget::showWidget(int index) { emit currentAboutToShow(index); + m_modesStack->setCurrentIndex(index); emit currentChanged(index); } @@ -480,7 +497,7 @@ void FancyTabWidget::setTabToolTip(int index, const QString &toolTip) { m_tabBar->setTabToolTip(index, toolTip); } -QWidget * FancyTabWidget::currentWidget() +QWidget *FancyTabWidget::currentWidget() { return m_modesStack->currentWidget(); } diff --git a/ground/openpilotgcs/src/plugins/config/fancytabwidget.h b/ground/openpilotgcs/src/plugins/config/fancytabwidget.h index 73da2aae8..e0bcad74a 100644 --- a/ground/openpilotgcs/src/plugins/config/fancytabwidget.h +++ b/ground/openpilotgcs/src/plugins/config/fancytabwidget.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,18 +40,17 @@ class QStackedLayout; class QStatusBar; QT_END_NAMESPACE - struct FancyTab { - QIcon icon; - QString text; - QString toolTip; - }; +struct FancyTab { + QIcon icon; + QString text; + QString toolTip; +}; -class FancyTabBar : public QWidget -{ +class FancyTabBar : public QWidget { Q_OBJECT public: - FancyTabBar(QWidget *parent = 0, bool isVertical=false); + FancyTabBar(QWidget *parent = 0, bool isVertical = false); ~FancyTabBar(); bool event(QEvent *event); @@ -66,26 +65,50 @@ public: QSize sizeHint() const; QSize minimumSizeHint() const; - void insertTab(int index, const QIcon &icon, const QString &label) { + void insertTab(int index, const QIcon &icon, const QString &label) + { FancyTab tab; + tab.icon = icon; tab.text = label; m_tabs.insert(index, tab); } - void removeTab(int index) { + void removeTab(int index) + { m_tabs.removeAt(index); } void updateTabNameIcon(int index, const QIcon &icon, const QString &label); void setCurrentIndex(int index); - int currentIndex() const { return m_currentIndex; } + int currentIndex() const + { + return m_currentIndex; + } - void setTabToolTip(int index, QString toolTip) { m_tabs[index].toolTip = toolTip; } - QString tabToolTip(int index) const { return m_tabs.at(index).toolTip; } + void setTabToolTip(int index, QString toolTip) + { + m_tabs[index].toolTip = toolTip; + } + QString tabToolTip(int index) const + { + return m_tabs.at(index).toolTip; + } - void setIconSize(int s) { iconSize = s; } - QIcon tabIcon(int index) const {return m_tabs.at(index).icon; } - QString tabText(int index) const { return m_tabs.at(index).text; } - int count() const {return m_tabs.count(); } + void setIconSize(int s) + { + iconSize = s; + } + QIcon tabIcon(int index) const + { + return m_tabs.at(index).icon; + } + QString tabText(int index) const + { + return m_tabs.at(index).text; + } + int count() const + { + return m_tabs.count(); + } QRect tabRect(int index) const; @@ -108,11 +131,9 @@ private: bool verticalTabs; QSize tabSizeHint(bool minimum = false) const; - }; -class FancyTabWidget : public QWidget -{ +class FancyTabWidget : public QWidget { Q_OBJECT public: @@ -126,14 +147,17 @@ public: int cornerWidgetCount() const; void setTabToolTip(int index, const QString &toolTip); void updateTabNameIcon(int index, const QIcon &icon, const QString &label); - void setIconSize(int s) { m_tabBar->setIconSize(s); } + void setIconSize(int s) + { + m_tabBar->setIconSize(s); + } void paintEvent(QPaintEvent *event); int currentIndex() const; QStatusBar *statusBar() const; - QWidget * currentWidget(); + QWidget *currentWidget(); signals: void currentAboutToShow(int index); void currentChanged(int index); diff --git a/ground/openpilotgcs/src/plugins/config/gyro-calibration.cpp b/ground/openpilotgcs/src/plugins/config/gyro-calibration.cpp index 20c12c252..9f35fbcdd 100644 --- a/ground/openpilotgcs/src/plugins/config/gyro-calibration.cpp +++ b/ground/openpilotgcs/src/plugins/config/gyro-calibration.cpp @@ -1,4 +1,3 @@ - #include #include #include @@ -9,79 +8,78 @@ * The measurement equation is * gyro_k = accelSensitivity * \tilde{accel}_k + bias + omega * - * where, omega is the true angular rate (assumed to be zero) - * bias is the sensor bias - * accelSensitivity is the 3x3 matrix of sensitivity scale factors - * \tilde{accel}_k is the calibrated measurement of the accelerometer - * at time k + * where, omega is the true angular rate (assumed to be zero) + * bias is the sensor bias + * accelSensitivity is the 3x3 matrix of sensitivity scale factors + * \tilde{accel}_k is the calibrated measurement of the accelerometer + * at time k * * After calibration, the optimized gyro measurement is given by * \tilde{gyro}_k = gyro_k - bias - accelSensitivity * \tilde{accel}_k */ -void -gyroscope_calibration(Vector3f& bias, - Matrix3f& accelSensitivity, - Vector3f gyroSamples[], - Vector3f accelSamples[], - size_t n_samples) +void gyroscope_calibration(Vector3f & bias, + Matrix3f & accelSensitivity, + Vector3f gyroSamples[], + Vector3f accelSamples[], + size_t n_samples) { - // Assume the following measurement model: - // y = H*x - // where x is the vector of unknowns, and y is the measurement vector. - // the vector of unknowns is - // [a_x a_y a_z b_x] - // The measurement vector is - // [gyro_x] - // and the measurement matrix H is - // [accelSample_x accelSample_y accelSample_z 1] + // Assume the following measurement model: + // y = H*x + // where x is the vector of unknowns, and y is the measurement vector. + // the vector of unknowns is + // [a_x a_y a_z b_x] + // The measurement vector is + // [gyro_x] + // and the measurement matrix H is + // [accelSample_x accelSample_y accelSample_z 1] - // Note that the individual solutions for x are given by - // (H^T \times H)^-1 \times H^T y - // Everything is identical except for y and x. So, transform it - // into block form X = (H^T \times H)^-1 \times H^T Y - // where each column of X contains the partial solution for each - // column of y. + // Note that the individual solutions for x are given by + // (H^T \times H)^-1 \times H^T y + // Everything is identical except for y and x. So, transform it + // into block form X = (H^T \times H)^-1 \times H^T Y + // where each column of X contains the partial solution for each + // column of y. - // Construct the matrix of accelerometer samples. Intermediate results - // are computed in "twice the precision that the source provides and the - // result deserves" by Kahan's thumbrule to prevent numerical problems. - Matrix H(n_samples, 4); - // And the matrix of gyro samples. - Matrix Y(n_samples, 3); - for (unsigned i = 0; i < n_samples; ++i) { - H.row(i) << accelSamples[i].transpose().cast(), 1.0; - Y.row(i) << gyroSamples[i].transpose().cast(); - } + // Construct the matrix of accelerometer samples. Intermediate results + // are computed in "twice the precision that the source provides and the + // result deserves" by Kahan's thumbrule to prevent numerical problems. + Matrix H(n_samples, 4); + // And the matrix of gyro samples. + Matrix Y(n_samples, 3); + for (unsigned i = 0; i < n_samples; ++i) { + H.row(i) << accelSamples[i].transpose().cast(), 1.0; + Y.row(i) << gyroSamples[i].transpose().cast(); + } #if 1 - Matrix result; - // Use the cholesky-based Penrose pseudoinverse method. - (H.transpose() * H).ldlt().solve(H.transpose()*Y, &result); + Matrix result; + // Use the cholesky-based Penrose pseudoinverse method. + (H.transpose() * H).ldlt().solve(H.transpose() * Y, &result); - // Transpose the result and return it to the caller. Cast back to float - // since there really isn't that much accuracy in the result. - bias = result.row(3).transpose().cast(); - accelSensitivity = result.block<3, 3>(0, 0).transpose().cast(); + // Transpose the result and return it to the caller. Cast back to float + // since there really isn't that much accuracy in the result. + bias = result.row(3).transpose().cast(); + accelSensitivity = result.block<3, 3>(0, 0).transpose().cast(); #else - // TODO: Compare this result with a total-least-squares model. - size_t n = 4; - Matrix C; - C << H, Y; - SVD > usv(C); - Matrix V_ab = usv.matrixV().block<4, 3>(0, n); - Matrix V_bb = usv.matrixV().corner(BottomRight, n_samples-4, 3); - // X = -V_ab/V_bb; - // X^T = (A * B^-1)^T - // X^T = (B^-1^T * A^T) - // X^T = (B^T^-1 * A^T) - // V_bb is orthgonal but not orthonormal. QR decomposition - // should be very fast in this case. - Matrix result; - V_bb.transpose().qr().solve(-V_ab.transpose(), &result); + // TODO: Compare this result with a total-least-squares model. + size_t n = 4; + Matrix C; + C << H, Y; + SVD > usv(C); + Matrix V_ab = usv.matrixV().block<4, 3>(0, n); + Matrix V_bb = usv.matrixV().corner(BottomRight, n_samples - 4, 3); + // X = -V_ab/V_bb; + // X^T = (A * B^-1)^T + // X^T = (B^-1^T * A^T) + // X^T = (B^T^-1 * A^T) + // V_bb is orthgonal but not orthonormal. QR decomposition + // should be very fast in this case. + Matrix result; + V_bb.transpose().qr().solve(-V_ab.transpose(), &result); - // Results only deserve single precision. - bias = result.col(3).cast(); - accelSensitivity = result.block<3, 3>(0, 0).cast(); + // Results only deserve single precision. + bias = result.col(3).cast(); + accelSensitivity = result.block<3, 3>(0, 0).cast(); -#endif +#endif // if 1 } diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.h b/ground/openpilotgcs/src/plugins/config/inputchannelform.h index 606ef2d78..ffdb9898b 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.h +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.h @@ -4,15 +4,14 @@ #include #include "configinputwidget.h" namespace Ui { - class inputChannelForm; +class inputChannelForm; } -class inputChannelForm : public ConfigTaskWidget -{ +class inputChannelForm : public ConfigTaskWidget { Q_OBJECT public: - explicit inputChannelForm(QWidget *parent = 0,bool showlegend=false); + explicit inputChannelForm(QWidget *parent = 0, bool showlegend = false); ~inputChannelForm(); friend class ConfigInputWidget; void setName(QString &name); diff --git a/ground/openpilotgcs/src/plugins/config/legacy-calibration.cpp b/ground/openpilotgcs/src/plugins/config/legacy-calibration.cpp index 92e34efb2..efaf3fc06 100644 --- a/ground/openpilotgcs/src/plugins/config/legacy-calibration.cpp +++ b/ground/openpilotgcs/src/plugins/config/legacy-calibration.cpp @@ -34,8 +34,8 @@ * The measurement equation is * B_k = D^-1 * (A_k * H_k - b) * Where B_k is the measurement of the field at time k - * D is the diagonal matrix of scale factors - * A_k is the attitude direction cosine matrix at time k + * D is the diagonal matrix of scale factors + * A_k is the attitude direction cosine matrix at time k * H_k is the reference field at the kth sample * b is the vector of scale factors. * @@ -45,70 +45,67 @@ * @param bias[out] The computed bias of the sensor * @param scale[out] The computed scale factor of the sensor * @param samples An array of sample data points. Only the first 6 are - * used. + * used. * @param n_samples The number of sample data points. Must be at least 6. * @param referenceField The field being measured by the sensor. */ -void -openpilot_bias_scale(Vector3f& bias, - Vector3f& scale, - const Vector3f samples[], - const size_t n_samples, - const Vector3f& referenceField) +void openpilot_bias_scale(Vector3f & bias, + Vector3f & scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f & referenceField) { - // TODO: Add error handling, and return error codes through the return - // value. - if (n_samples < 6) { - bias = Vector3f::Zero(); - scale = Vector3f::Zero(); - return; - } - // mag = (S*x + b)**2 - // mag = (S**2 * x**2 + 2*S*b*x + b*b) - // 0 = S**2 * (x1**2 - x2**2) + 2*S*B*(x1 - x2)) - // Fill in matrix A - - // write six difference-in-magnitude equations of the form - // Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + - // 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0 - // - // or in other words - // 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + - // 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = - // (x1^2-x2^2) - Matrix A; - Matrix f; - for (unsigned i=0; i<5; i++){ - A.row(i)[0] = 2.0 * (samples[i+1].x() - samples[i].x()); - A.row(i)[1] = samples[i+1].y()* samples[i+1].y() - samples[i].y()*samples[i].y(); - A.row(i)[2] = 2.0 * (samples[i+1].y() - samples[i].y()); - A.row(i)[3] = samples[i+1].z()*samples[i+1].z() - samples[i].z()*samples[i].z(); - A.row(i)[4] = 2.0 * (samples[i+1].z() - samples[i].z()); - f[i] = samples[i].x()*samples[i].x() - samples[i+1].x()*samples[i+1].x(); - } - Matrix c; - A.lu().solve(f, &c); + // TODO: Add error handling, and return error codes through the return + // value. + if (n_samples < 6) { + bias = Vector3f::Zero(); + scale = Vector3f::Zero(); + return; + } + // mag = (S*x + b)**2 + // mag = (S**2 * x**2 + 2*S*b*x + b*b) + // 0 = S**2 * (x1**2 - x2**2) + 2*S*B*(x1 - x2)) + // Fill in matrix A - + // write six difference-in-magnitude equations of the form + // Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + + // 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0 + // + // or in other words + // 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + + // 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = + // (x1^2-x2^2) + Matrix A; + Matrix f; + for (unsigned i = 0; i < 5; i++) { + A.row(i)[0] = 2.0 * (samples[i + 1].x() - samples[i].x()); + A.row(i)[1] = samples[i + 1].y() * samples[i + 1].y() - samples[i].y() * samples[i].y(); + A.row(i)[2] = 2.0 * (samples[i + 1].y() - samples[i].y()); + A.row(i)[3] = samples[i + 1].z() * samples[i + 1].z() - samples[i].z() * samples[i].z(); + A.row(i)[4] = 2.0 * (samples[i + 1].z() - samples[i].z()); + f[i] = samples[i].x() * samples[i].x() - samples[i + 1].x() * samples[i + 1].x(); + } + Matrix c; + A.lu().solve(f, &c); - // use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer - float xp = samples[0].x(); + // use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer + float xp = samples[0].x(); float yp = samples[0].y(); - float zp = samples[0].z(); + float zp = samples[0].z(); - float Sx = sqrt(referenceField.squaredNorm() / - (xp*xp + 2*c[0]*xp + c[0]*c[0] + c[1]*yp*yp + 2*c[2]*yp + c[2]*c[2]/c[1] + c[3]*zp*zp + 2*c[4]*zp + c[4]*c[4]/c[3])); + float Sx = sqrt(referenceField.squaredNorm() / + (xp * xp + 2 * c[0] * xp + c[0] * c[0] + c[1] * yp * yp + 2 * c[2] * yp + c[2] * c[2] / c[1] + c[3] * zp * zp + 2 * c[4] * zp + c[4] * c[4] / c[3])); - scale[0] = Sx; - bias[0] = Sx*c[0]; - scale[1] = sqrt(c[1]*Sx*Sx); - bias[1] = c[2]*Sx*Sx/scale[1]; - scale[2] = sqrt(c[3]*Sx*Sx); - bias[2] = c[4]*Sx*Sx/scale[2]; + scale[0] = Sx; + bias[0] = Sx * c[0]; + scale[1] = sqrt(c[1] * Sx * Sx); + bias[1] = c[2] * Sx * Sx / scale[1]; + scale[2] = sqrt(c[3] * Sx * Sx); + bias[2] = c[4] * Sx * Sx / scale[2]; - for (int i = 0; i < 3; ++i) { - // Fixup difference between openpilot measurement model and twostep - // version - bias[i] = -bias[i] / scale[i]; - } + for (int i = 0; i < 3; ++i) { + // Fixup difference between openpilot measurement model and twostep + // version + bias[i] = -bias[i] / scale[i]; + } } - - diff --git a/ground/openpilotgcs/src/plugins/config/mixercurve.h b/ground/openpilotgcs/src/plugins/config/mixercurve.h index 58e0c2f8b..bd4b0acec 100644 --- a/ground/openpilotgcs/src/plugins/config/mixercurve.h +++ b/ground/openpilotgcs/src/plugins/config/mixercurve.h @@ -43,23 +43,22 @@ namespace Ui { class MixerCurve; } -class MixerCurve : public QFrame -{ +class MixerCurve : public QFrame { Q_OBJECT - + public: explicit MixerCurve(QWidget *parent = 0); ~MixerCurve(); /* Enumeration options for ThrottleCurves */ - typedef enum { MIXERCURVE_THROTTLE=0, MIXERCURVE_PITCH=1 } MixerCurveType; + typedef enum { MIXERCURVE_THROTTLE = 0, MIXERCURVE_PITCH = 1 } MixerCurveType; void setMixerType(MixerCurveType curveType); - void initCurve (const QList* points); + void initCurve(const QList *points); QList getCurve(); void initLinearCurve(int numPoints, double maxValue = 1, double minValue = 0); - void setCurve(const QList* points); + void setCurve(const QList *points); void setMin(double value); double getMin(); void setMax(double value); @@ -69,7 +68,10 @@ public: double getCurveStep(); double setRange(double min, double max); - MixerCurveWidget* getCurveWidget() { return m_curve; } + MixerCurveWidget *getCurveWidget() + { + return m_curve; + } signals: @@ -91,12 +93,11 @@ private slots: void UpdateCurveUI(); private: - Ui::MixerCurve* m_mixerUI; - MixerCurveWidget* m_curve; - QTableWidget* m_settings; + Ui::MixerCurve *m_mixerUI; + MixerCurveWidget *m_curve; + QTableWidget *m_settings; MixerCurveType m_curveType; - DoubleSpinDelegate* m_spinDelegate; - + DoubleSpinDelegate *m_spinDelegate; }; #endif // MIXERCURVE_H diff --git a/ground/openpilotgcs/src/plugins/config/outputchannelform.cpp b/ground/openpilotgcs/src/plugins/config/outputchannelform.cpp index f2d039edb..a69a88bf9 100644 --- a/ground/openpilotgcs/src/plugins/config/outputchannelform.cpp +++ b/ground/openpilotgcs/src/plugins/config/outputchannelform.cpp @@ -29,24 +29,26 @@ #include "configoutputwidget.h" OutputChannelForm::OutputChannelForm(const int index, QWidget *parent, const bool showLegend) : - ConfigTaskWidget(parent), - ui(), - m_index(index), - m_inChannelTest(false) + ConfigTaskWidget(parent), + ui(), + m_index(index), + m_inChannelTest(false) { ui.setupUi(this); - if(!showLegend) - { + if (!showLegend) { // Remove legend - QGridLayout *grid_layout = dynamic_cast(layout()); + QGridLayout *grid_layout = dynamic_cast(layout()); Q_ASSERT(grid_layout); - for (int col = 0; col < grid_layout->columnCount(); col++) - { // remove every item in first row + for (int col = 0; col < grid_layout->columnCount(); col++) { // remove every item in first row QLayoutItem *item = grid_layout->itemAtPosition(0, col); - if (!item) continue; + if (!item) { + continue; + } // get widget from layout item QWidget *legend_widget = item->widget(); - if (!legend_widget) continue; + if (!legend_widget) { + continue; + } // delete widget grid_layout->removeWidget(legend_widget); delete legend_widget; @@ -54,7 +56,7 @@ OutputChannelForm::OutputChannelForm(const int index, QWidget *parent, const boo } // The convention for OP is Channel 1 to Channel 10. - ui.actuatorNumber->setText(QString("%1:").arg(m_index+1)); + ui.actuatorNumber->setText(QString("%1:").arg(m_index + 1)); // Register for ActuatorSettings changes: connect(ui.actuatorMin, SIGNAL(editingFinished()), @@ -84,19 +86,18 @@ OutputChannelForm::~OutputChannelForm() */ void OutputChannelForm::enableChannelTest(bool state) { - if (m_inChannelTest == state) return; + if (m_inChannelTest == state) { + return; + } m_inChannelTest = state; - if(m_inChannelTest) - { + if (m_inChannelTest) { // Prevent stupid users from touching the minimum & maximum ranges while // moving the sliders. Thanks Ivan for the tip :) ui.actuatorMin->setEnabled(false); ui.actuatorMax->setEnabled(false); ui.actuatorRev->setEnabled(false); - } - else - { + } else { ui.actuatorMin->setEnabled(true); ui.actuatorMax->setEnabled(true); ui.actuatorRev->setEnabled(true); @@ -111,34 +112,39 @@ void OutputChannelForm::linkToggled(bool state) { Q_UNUSED(state) - if (!m_inChannelTest) - return; // we are not in Test Output mode - + if (!m_inChannelTest) { + return; // we are not in Test Output mode + } // find the minimum slider value for the linked ones - if (!parent()) return; + if (!parent()) { + return; + } int min = 10000; int linked_count = 0; - QList outputChannelForms = parent()->findChildren(); + QList outputChannelForms = parent()->findChildren(); // set the linked channels of the parent widget to the same value - foreach(OutputChannelForm *outputChannelForm, outputChannelForms) - { - if (!outputChannelForm->ui.actuatorLink->checkState()) + foreach(OutputChannelForm * outputChannelForm, outputChannelForms) { + if (!outputChannelForm->ui.actuatorLink->checkState()) { continue; - if (this == outputChannelForm) + } + if (this == outputChannelForm) { continue; + } int value = outputChannelForm->ui.actuatorNeutral->value(); - if(min > value) min = value; + if (min > value) { + min = value; + } linked_count++; } - if (linked_count <= 0) - return; // no linked channels - + if (linked_count <= 0) { + return; // no linked channels + } // set the linked channels to the same value - foreach(OutputChannelForm *outputChannelForm, outputChannelForms) - { - if (!outputChannelForm->ui.actuatorLink->checkState()) + foreach(OutputChannelForm * outputChannelForm, outputChannelForms) { + if (!outputChannelForm->ui.actuatorLink->checkState()) { continue; + } outputChannelForm->ui.actuatorNeutral->setValue(min); } } @@ -167,10 +173,11 @@ void OutputChannelForm::minmax(int minimum, int maximum) ui.actuatorMin->setValue(minimum); ui.actuatorMax->setValue(maximum); setChannelRange(); - if(ui.actuatorMin->value() > ui.actuatorMax->value()) + if (ui.actuatorMin->value() > ui.actuatorMax->value()) { ui.actuatorRev->setChecked(true); - else + } else { ui.actuatorRev->setChecked(false); + } } /** @@ -188,17 +195,18 @@ void OutputChannelForm::setAssignment(const QString &assignment) { ui.actuatorName->setText(assignment); QFontMetrics metrics(ui.actuatorName->font()); - int width=metrics.width(assignment)+1; - foreach(OutputChannelForm * form,parent()->findChildren()) - { - if(form==this) + int width = metrics.width(assignment) + 1; + foreach(OutputChannelForm * form, parent()->findChildren()) { + if (form == this) { continue; - if(form->ui.actuatorName->minimumSize().width()ui.actuatorName->setMinimumSize(width,0); - else - width=form->ui.actuatorName->minimumSize().width(); + } + if (form->ui.actuatorName->minimumSize().width() < width) { + form->ui.actuatorName->setMinimumSize(width, 0); + } else { + width = form->ui.actuatorName->minimumSize().width(); + } } - ui.actuatorName->setMinimumSize(width,0); + ui.actuatorName->setMinimumSize(width, 0); } /** @@ -211,24 +219,23 @@ void OutputChannelForm::setAssignment(const QString &assignment) void OutputChannelForm::setChannelRange() { int oldMini = ui.actuatorNeutral->minimum(); -// int oldMaxi = ui.actuatorNeutral->maximum(); - if (ui.actuatorMin->value() < ui.actuatorMax->value()) - { +// int oldMaxi = ui.actuatorNeutral->maximum(); + + if (ui.actuatorMin->value() < ui.actuatorMax->value()) { ui.actuatorNeutral->setRange(ui.actuatorMin->value(), ui.actuatorMax->value()); ui.actuatorRev->setChecked(false); - } - else - { + } else { ui.actuatorNeutral->setRange(ui.actuatorMax->value(), ui.actuatorMin->value()); ui.actuatorRev->setChecked(true); } - if (ui.actuatorNeutral->value() == oldMini) + if (ui.actuatorNeutral->value() == oldMini) { ui.actuatorNeutral->setValue(ui.actuatorNeutral->minimum()); + } -// if (ui.actuatorNeutral->value() == oldMaxi) -// ui.actuatorNeutral->setValue(ui.actuatorNeutral->maximum()); // this can be dangerous if it happens to be controlling a motor at the time! +// if (ui.actuatorNeutral->value() == oldMaxi) +// ui.actuatorNeutral->setValue(ui.actuatorNeutral->maximum()); // this can be dangerous if it happens to be controlling a motor at the time! } /** @@ -238,10 +245,12 @@ void OutputChannelForm::reverseChannel(bool state) { // Sanity check: if state became true, make sure the Maxvalue was higher than Minvalue // the situations below can happen! - if (state && (ui.actuatorMax->value() < ui.actuatorMin->value())) + if (state && (ui.actuatorMax->value() < ui.actuatorMin->value())) { return; - if (!state && (ui.actuatorMax->value() > ui.actuatorMin->value())) + } + if (!state && (ui.actuatorMax->value() > ui.actuatorMin->value())) { return; + } // Now, swap the min & max values (only on the spinboxes, the slider // does not change! @@ -253,15 +262,12 @@ void OutputChannelForm::reverseChannel(bool state) // This is a trick to force the slider to update its value and // emit the right signal itself, because our sendChannelTest(int) method // relies on the object sender's identity. - if (ui.actuatorNeutral->value() < ui.actuatorNeutral->maximum()) - { - ui.actuatorNeutral->setValue(ui.actuatorNeutral->value()+1); - ui.actuatorNeutral->setValue(ui.actuatorNeutral->value()-1); - } - else - { - ui.actuatorNeutral->setValue(ui.actuatorNeutral->value()-1); - ui.actuatorNeutral->setValue(ui.actuatorNeutral->value()+1); + if (ui.actuatorNeutral->value() < ui.actuatorNeutral->maximum()) { + ui.actuatorNeutral->setValue(ui.actuatorNeutral->value() + 1); + ui.actuatorNeutral->setValue(ui.actuatorNeutral->value() - 1); + } else { + ui.actuatorNeutral->setValue(ui.actuatorNeutral->value() - 1); + ui.actuatorNeutral->setValue(ui.actuatorNeutral->value() + 1); } } @@ -273,39 +279,48 @@ void OutputChannelForm::sendChannelTest(int value) { int in_value = value; - QSlider *ob = (QSlider *)QObject::sender(); - if (!ob) return; + QSlider *ob = (QSlider *)QObject::sender(); - if (ui.actuatorRev->isChecked()) - value = ui.actuatorMin->value() - value + ui.actuatorMax->value(); // the channel is reversed + if (!ob) { + return; + } + if (ui.actuatorRev->isChecked()) { + value = ui.actuatorMin->value() - value + ui.actuatorMax->value(); // the channel is reversed + } // update the label ui.actuatorValue->setText(QString::number(value)); - if (ui.actuatorLink->checkState() && parent()) - { // the channel is linked to other channels - QList outputChannelForms = parent()->findChildren(); + if (ui.actuatorLink->checkState() && parent()) { // the channel is linked to other channels + QList outputChannelForms = parent()->findChildren(); // set the linked channels of the parent widget to the same value - foreach(OutputChannelForm *outputChannelForm, outputChannelForms) - { - if (this == outputChannelForm) continue; - if (!outputChannelForm->ui.actuatorLink->checkState()) continue; + foreach(OutputChannelForm * outputChannelForm, outputChannelForms) { + if (this == outputChannelForm) { + continue; + } + if (!outputChannelForm->ui.actuatorLink->checkState()) { + continue; + } int val = in_value; - if (val < outputChannelForm->ui.actuatorNeutral->minimum()) + if (val < outputChannelForm->ui.actuatorNeutral->minimum()) { val = outputChannelForm->ui.actuatorNeutral->minimum(); - if (val > outputChannelForm->ui.actuatorNeutral->maximum()) + } + if (val > outputChannelForm->ui.actuatorNeutral->maximum()) { val = outputChannelForm->ui.actuatorNeutral->maximum(); + } - if (outputChannelForm->ui.actuatorNeutral->value() == val) continue; + if (outputChannelForm->ui.actuatorNeutral->value() == val) { + continue; + } outputChannelForm->ui.actuatorNeutral->setValue(val); outputChannelForm->ui.actuatorValue->setText(QString::number(val)); } } - if (!m_inChannelTest) - return; // we are not in Test Output mode - + if (!m_inChannelTest) { + return; // we are not in Test Output mode + } emit channelChanged(index(), value); } diff --git a/ground/openpilotgcs/src/plugins/config/outputchannelform.h b/ground/openpilotgcs/src/plugins/config/outputchannelform.h index 434f9517a..a669fc2f8 100644 --- a/ground/openpilotgcs/src/plugins/config/outputchannelform.h +++ b/ground/openpilotgcs/src/plugins/config/outputchannelform.h @@ -31,8 +31,7 @@ #include "ui_outputchannelform.h" #include "configtaskwidget.h" -class OutputChannelForm : public ConfigTaskWidget -{ +class OutputChannelForm : public ConfigTaskWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/config/twostep.cpp b/ground/openpilotgcs/src/plugins/config/twostep.cpp index fa9e95f58..e1562af29 100644 --- a/ground/openpilotgcs/src/plugins/config/twostep.cpp +++ b/ground/openpilotgcs/src/plugins/config/twostep.cpp @@ -3,7 +3,7 @@ * * @file twostep.cpp * @author Jonathan Brandmeyer - * Copyright (C) 2010. + * Copyright (C) 2010. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup Config plugin @@ -40,12 +40,12 @@ #include #if defined(__APPLE__) || defined(_WIN32) - // Qt bug work around +// Qt bug work around #ifndef isnan - extern "C" int isnan(double); +extern "C" int isnan(double); #endif #ifndef isinf - extern "C" int isinf(double); +extern "C" int isinf(double); #endif #endif @@ -66,31 +66,32 @@ */ namespace { -Vector3f center(const Vector3f samples[], - size_t n_samples) +Vector3f center(const Vector3f samples[], + size_t n_samples) { - Vector3f summation(Vector3f::Zero()); - for (size_t i = 0; i < n_samples; ++i) { - summation += samples[i]; - } - return summation / float(n_samples); + Vector3f summation(Vector3f::Zero()); + + for (size_t i = 0; i < n_samples; ++i) { + summation += samples[i]; + } + return summation / float(n_samples); } -void inv_fisher_information_matrix(Matrix3f& fisherInv, - Matrix3f& fisher, - const Vector3f samples[], - size_t n_samples, - const float noise) +void inv_fisher_information_matrix(Matrix3f & fisherInv, + Matrix3f & fisher, + const Vector3f samples[], + size_t n_samples, + const float noise) { - MatrixXf column_samples(3, n_samples); - for (size_t i = 0; i < n_samples; ++i) { - column_samples.col(i) = samples[i]; - } - fisher = 4 / noise * - column_samples * column_samples.transpose(); - // Compute the inverse by taking advantage of the results symmetricness - fisher.ldlt().solve(Matrix3f::Identity(), &fisherInv); - return; + MatrixXf column_samples(3, n_samples); + + for (size_t i = 0; i < n_samples; ++i) { + column_samples.col(i) = samples[i]; + } + fisher = 4 / noise * + column_samples * column_samples.transpose(); + // Compute the inverse by taking advantage of the results symmetricness + fisher.ldlt().solve(Matrix3f::Identity(), &fisherInv); } // Eq 14 of the original reference. Computes the gradiant of the cost function // with respect to the bias offset. @@ -100,96 +101,95 @@ void inv_fisher_information_matrix(Matrix3f& fisherInv, // @param mu: The trace of the noise matrix (3*noise) // @return the negative gradiant of the cost function with respect to the // current value of the estimate, b -Vector3f -neg_dJdb(const Vector3f samples[], - const float mags[], - size_t n_samples, - const Vector3f& b, - float mu, - float noise) +Vector3f neg_dJdb(const Vector3f samples[], + const float mags[], + size_t n_samples, + const Vector3f & b, + float mu, + float noise) { - Vector3f summation(Vector3f::Zero()); - float b_norm2 = b.squaredNorm(); - - for (size_t i = 0; i < n_samples; ++i) { - summation += (mags[i] - 2*samples[i].dot(b) + b_norm2 - mu) - *2* (samples[i] - b); - } - - return (1.0 / noise)*summation; + Vector3f summation(Vector3f::Zero()); + float b_norm2 = b.squaredNorm(); + + for (size_t i = 0; i < n_samples; ++i) { + summation += (mags[i] - 2 * samples[i].dot(b) + b_norm2 - mu) + * 2 * (samples[i] - b); + } + + return (1.0 / noise) * summation; } } // !namespace (anon) -Vector3f twostep_bias_only(const Vector3f samples[], - size_t n_samples, - const Vector3f& referenceField, - const float noise) +Vector3f twostep_bias_only(const Vector3f samples[], + size_t n_samples, + const Vector3f & referenceField, + const float noise) { - // \tilde{H} - Vector3f centeredSamples[n_samples]; - // z_k - float sampleDeltaMag[n_samples]; - // eq 7 and 8 applied to samples - Vector3f avg = center(samples, n_samples); - float refSquaredNorm = referenceField.squaredNorm(); - float sampleDeltaMagCenter = 0; - for (size_t i = 0; i < n_samples; ++i) - { - // eq 9 applied to samples - centeredSamples[i] = samples[i] - avg; - // eqn 2a - sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; - sampleDeltaMagCenter += sampleDeltaMag[i]; - } - sampleDeltaMagCenter /= n_samples; + // \tilde{H} + Vector3f centeredSamples[n_samples]; + // z_k + float sampleDeltaMag[n_samples]; + // eq 7 and 8 applied to samples + Vector3f avg = center(samples, n_samples); + float refSquaredNorm = referenceField.squaredNorm(); + float sampleDeltaMagCenter = 0; - Matrix3f P_bb; - Matrix3f P_bb_inv; - // Due to eq 12b - inv_fisher_information_matrix(P_bb, P_bb_inv, centeredSamples, n_samples, noise); - // Compute centered magnitudes - float sampleDeltaMagCentered[n_samples]; - for (size_t i = 0; i < n_samples; ++i) { - sampleDeltaMagCentered[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; - } + for (size_t i = 0; i < n_samples; ++i) { + // eq 9 applied to samples + centeredSamples[i] = samples[i] - avg; + // eqn 2a + sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; + sampleDeltaMagCenter += sampleDeltaMag[i]; + } + sampleDeltaMagCenter /= n_samples; - // From eq 12a - Vector3f estimate( Vector3f::Zero()); - for (size_t i = 0; i < n_samples; ++i) { - estimate += sampleDeltaMagCentered[i] * centeredSamples[i]; - } - estimate = P_bb * ((2 / noise) * estimate); + Matrix3f P_bb; + Matrix3f P_bb_inv; + // Due to eq 12b + inv_fisher_information_matrix(P_bb, P_bb_inv, centeredSamples, n_samples, noise); + // Compute centered magnitudes + float sampleDeltaMagCentered[n_samples]; + for (size_t i = 0; i < n_samples; ++i) { + sampleDeltaMagCentered[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; + } - // Newton-Raphson gradient descent to the optimal solution - // eq 14a and 14b - float mu = -3*noise; - for (int i = 0; i < 6; ++i) { - Vector3f neg_gradiant = neg_dJdb(samples, sampleDeltaMag, n_samples, - estimate, mu, noise); - Matrix3f scale = P_bb_inv + 4/noise*(avg - estimate)*(avg - estimate).transpose(); - Vector3f neg_increment; - scale.ldlt().solve(neg_gradiant, &neg_increment); - // Note that the negative has been done twice - estimate += neg_increment; - } - return estimate; + // From eq 12a + Vector3f estimate(Vector3f::Zero()); + for (size_t i = 0; i < n_samples; ++i) { + estimate += sampleDeltaMagCentered[i] * centeredSamples[i]; + } + estimate = P_bb * ((2 / noise) * estimate); + + // Newton-Raphson gradient descent to the optimal solution + // eq 14a and 14b + float mu = -3 * noise; + for (int i = 0; i < 6; ++i) { + Vector3f neg_gradiant = neg_dJdb(samples, sampleDeltaMag, n_samples, + estimate, mu, noise); + Matrix3f scale = P_bb_inv + 4 / noise * (avg - estimate) * (avg - estimate).transpose(); + Vector3f neg_increment; + scale.ldlt().solve(neg_gradiant, &neg_increment); + // Note that the negative has been done twice + estimate += neg_increment; + } + return estimate; } namespace { // Private utility functions for the version of TWOSTEP that computes bias and // scale factor vectors alone. -/** Compute the gradiant of the norm of b with respect to the estimate vector. +/** Compute the gradiant of the norm of b with respect to the estimate vector. */ Matrix -dnormb_dtheta(const Matrix& theta) +dnormb_dtheta(const Matrix & theta) { - return (Matrix() << 2*theta.coeff(0)/(1+theta.coeff(3)), - 2*theta.coeff(1)/(1+theta.coeff(4)), - 2*theta.coeff(2)/(1+theta.coeff(5)), - -pow(theta.coeff(0), 2)/pow(1+theta.coeff(3), 2), - -pow(theta.coeff(1), 2)/pow(1+theta.coeff(4), 2), - -pow(theta.coeff(2), 2)/pow(1+theta.coeff(5), 2)).finished(); + return (Matrix() << 2 * theta.coeff(0) / (1 + theta.coeff(3)), + 2 * theta.coeff(1) / (1 + theta.coeff(4)), + 2 * theta.coeff(2) / (1 + theta.coeff(5)), + -pow(theta.coeff(0), 2) / pow(1 + theta.coeff(3), 2), + -pow(theta.coeff(1), 2) / pow(1 + theta.coeff(4), 2), + -pow(theta.coeff(2), 2) / pow(1 + theta.coeff(5), 2)).finished(); } /** @@ -197,41 +197,40 @@ dnormb_dtheta(const Matrix& theta) * estimate. */ Matrix -dJdb(const Matrix& centerL, - double centerMag, - const Matrix& theta, - const Matrix& dbdtheta, - double mu, - double noise) +dJdb(const Matrix & centerL, + double centerMag, + const Matrix & theta, + const Matrix & dbdtheta, + double mu, + double noise) { - // \frac{\delta}{\delta \theta'} |b(\theta')|^2 - Matrix vectorPart = dbdtheta - centerL; + // \frac{\delta}{\delta \theta'} |b(\theta')|^2 + Matrix vectorPart = dbdtheta - centerL; - // By equation 35 - double normbthetaSquared = 0; - for (unsigned i = 0; i < 3; ++i) { - normbthetaSquared += theta.coeff(i)*theta.coeff(i)/(1+theta.coeff(3+i)); - } - double scalarPart = (centerMag - centerL.dot(theta) + normbthetaSquared - - mu)/noise; - return scalarPart * vectorPart; + // By equation 35 + double normbthetaSquared = 0; + for (unsigned i = 0; i < 3; ++i) { + normbthetaSquared += theta.coeff(i) * theta.coeff(i) / (1 + theta.coeff(3 + i)); + } + double scalarPart = (centerMag - centerL.dot(theta) + normbthetaSquared + - mu) / noise; + return scalarPart * vectorPart; } /** Reconstruct the scale factor and bias offset vectors from the transformed * estimate vector. */ Matrix -theta_to_sane(const Matrix& theta) +theta_to_sane(const Matrix & theta) { - return (Matrix() << - theta.coeff(0) / sqrt(1 + theta.coeff(3)), - theta.coeff(1) / sqrt(1 + theta.coeff(4)), - theta.coeff(2) / sqrt(1 + theta.coeff(5)), - -1 + sqrt(1 + theta.coeff(3)), - -1 + sqrt(1 + theta.coeff(4)), - -1 + sqrt(1 + theta.coeff(5))).finished(); + return (Matrix() << + theta.coeff(0) / sqrt(1 + theta.coeff(3)), + theta.coeff(1) / sqrt(1 + theta.coeff(4)), + theta.coeff(2) / sqrt(1 + theta.coeff(5)), + -1 + sqrt(1 + theta.coeff(3)), + -1 + sqrt(1 + theta.coeff(4)), + -1 + sqrt(1 + theta.coeff(5))).finished(); } - } // !namespace (anon) /** @@ -255,143 +254,142 @@ theta_to_sane(const Matrix& theta) * @param scale[out] The computed scale factor, in the sensor frame * @param samples[in] An array of measurement samples. * @param n_samples The number of samples in the array. - * @param referenceField The magnetic field vector at this location. + * @param referenceField The magnetic field vector at this location. * @param noise The one-sigma squared standard deviation of the observed noise * in the sensor. */ -void twostep_bias_scale(Vector3f& bias, - Vector3f& scale, - const Vector3f samples[], - const size_t n_samples, - const Vector3f& referenceField, - const float noise) +void twostep_bias_scale(Vector3f & bias, + Vector3f & scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f & referenceField, + const float noise) { - // Initial estimate for gradiant descent starts at eq 37a of ref 2. + // Initial estimate for gradiant descent starts at eq 37a of ref 2. - // Define L_k by eq 30 and 28 for k = 1 .. n_samples - Matrix fullSamples(n_samples, 6); - // \hbar{L} by eq 33, simplified by obesrving that the - Matrix centerSample = Matrix::Zero(); - // Define the sample differences z_k by eq 23 a) - double sampleDeltaMag[n_samples]; - // The center value \hbar{z} - double sampleDeltaMagCenter = 0; - double refSquaredNorm = referenceField.squaredNorm(); + // Define L_k by eq 30 and 28 for k = 1 .. n_samples + Matrix fullSamples(n_samples, 6); + // \hbar{L} by eq 33, simplified by obesrving that the + Matrix centerSample = Matrix::Zero(); + // Define the sample differences z_k by eq 23 a) + double sampleDeltaMag[n_samples]; + // The center value \hbar{z} + double sampleDeltaMagCenter = 0; + double refSquaredNorm = referenceField.squaredNorm(); - for (size_t i = 0; i < n_samples; ++i) { - fullSamples.row(i) << 2*samples[i].transpose().cast(), - -(samples[i][0]*samples[i][0]), - -(samples[i][1]*samples[i][1]), - -(samples[i][2]*samples[i][2]); - centerSample += fullSamples.row(i); + for (size_t i = 0; i < n_samples; ++i) { + fullSamples.row(i) << 2 * samples[i].transpose().cast(), + -(samples[i][0] * samples[i][0]), + -(samples[i][1] * samples[i][1]), + -(samples[i][2] * samples[i][2]); + centerSample += fullSamples.row(i); - sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; - sampleDeltaMagCenter += sampleDeltaMag[i]; - } - sampleDeltaMagCenter /= n_samples; - centerSample /= n_samples; + sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; + sampleDeltaMagCenter += sampleDeltaMag[i]; + } + sampleDeltaMagCenter /= n_samples; + centerSample /= n_samples; - // True for all k. - // double mu = -3*noise; - // The center value of mu, \bar{mu} - // double centerMu = mu; - // The center value of mu, \tilde{mu} - // double centeredMu = 0; + // True for all k. + // double mu = -3*noise; + // The center value of mu, \bar{mu} + // double centerMu = mu; + // The center value of mu, \tilde{mu} + // double centeredMu = 0; - // Define \tilde{L}_k for k = 0 .. n_samples - Matrix centeredSamples(n_samples, 6); - // Define \tilde{z}_k for k = 0 .. n_samples - double centeredMags[n_samples]; - // Compute the term under the summation of eq 37a - Matrix estimateSummation = Matrix::Zero(); - for (size_t i = 0; i < n_samples; ++i) { - centeredSamples.row(i) = fullSamples.row(i) - centerSample; - centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; - estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose(); - } - estimateSummation /= noise; // note: paper supplies 1/noise + // Define \tilde{L}_k for k = 0 .. n_samples + Matrix centeredSamples(n_samples, 6); + // Define \tilde{z}_k for k = 0 .. n_samples + double centeredMags[n_samples]; + // Compute the term under the summation of eq 37a + Matrix estimateSummation = Matrix::Zero(); + for (size_t i = 0; i < n_samples; ++i) { + centeredSamples.row(i) = fullSamples.row(i) - centerSample; + centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; + estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose(); + } + estimateSummation /= noise; // note: paper supplies 1/noise - // By eq 37 b). Note, paper supplies 1/noise here - Matrix P_theta_theta_inv = (1.0f/noise)* - centeredSamples.transpose()*centeredSamples; + // By eq 37 b). Note, paper supplies 1/noise here + Matrix P_theta_theta_inv = (1.0f / noise) * + centeredSamples.transpose() * centeredSamples; #ifdef PRINTF_DEBUGGING - SelfAdjointEigenSolver > eig(P_theta_theta_inv); - std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n"; - std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose() - << "\n"; - std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n"; + SelfAdjointEigenSolver > eig(P_theta_theta_inv); + std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n"; + std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose() + << "\n"; + std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n"; #endif - // The Fisher information matrix has a small eigenvalue, with a - // corresponding eigenvector of about [1e-2 1e-2 1e-2 0.55, 0.55, - // 0.55]. This means that there is relatively little information - // about the common gain on the scale factor, which has a - // corresponding effect on the bias, too. The fixup is performed by - // the first iteration of the second stage of TWOSTEP, as documented in - // [1]. - Matrix estimate; - // By eq 37 a), \tilde{Fisher}^-1 - P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate); + // The Fisher information matrix has a small eigenvalue, with a + // corresponding eigenvector of about [1e-2 1e-2 1e-2 0.55, 0.55, + // 0.55]. This means that there is relatively little information + // about the common gain on the scale factor, which has a + // corresponding effect on the bias, too. The fixup is performed by + // the first iteration of the second stage of TWOSTEP, as documented in + // [1]. + Matrix estimate; + // By eq 37 a), \tilde{Fisher}^-1 + P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate); #ifdef PRINTF_DEBUGGING - Matrix P_theta_theta; - P_theta_theta_inv.ldlt().solve(Matrix::Identity(), &P_theta_theta); - SelfAdjointEigenSolver > eig2(P_theta_theta); - std::cout << "eigenvalues: " << eig2.eigenvalues().transpose() << "\n"; - std::cout << "eigenvectors: \n" << eig2.eigenvectors() << "\n"; - std::cout << "estimate summation: \n" << estimateSummation.normalized() - << "\n"; + Matrix P_theta_theta; + P_theta_theta_inv.ldlt().solve(Matrix::Identity(), &P_theta_theta); + SelfAdjointEigenSolver > eig2(P_theta_theta); + std::cout << "eigenvalues: " << eig2.eigenvalues().transpose() << "\n"; + std::cout << "eigenvectors: \n" << eig2.eigenvectors() << "\n"; + std::cout << "estimate summation: \n" << estimateSummation.normalized() + << "\n"; #endif - // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradiant(theta) - // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1} - size_t count = 3; - while (count --> 0) { - // eq 40 - Matrix db_dtheta = dnormb_dtheta(estimate); + // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradiant(theta) + // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1} + size_t count = 3; + while (count-- > 0) { + // eq 40 + Matrix db_dtheta = dnormb_dtheta(estimate); - Matrix dJ_dtheta = dJdb(centerSample, - sampleDeltaMagCenter, - estimate, - db_dtheta, - -3*noise, - noise/n_samples); + Matrix dJ_dtheta = dJdb(centerSample, + sampleDeltaMagCenter, + estimate, + db_dtheta, + -3 * noise, + noise / n_samples); - // Eq 39 (with double-negation on term inside parens) - db_dtheta = centerSample - db_dtheta; - Matrix scale = P_theta_theta_inv + - (double(n_samples)/noise)*db_dtheta.transpose() * db_dtheta; + // Eq 39 (with double-negation on term inside parens) + db_dtheta = centerSample - db_dtheta; + Matrix scale = P_theta_theta_inv + + (double(n_samples) / noise) * db_dtheta.transpose() * db_dtheta; - // eq 14b, mutatis mutandis (grumble, grumble) - Matrix increment; - scale.ldlt().solve(dJ_dtheta, &increment); - estimate -= increment; - } - - // Transform the estimated parameters from [c | e] back into [b | d]. - for (size_t i = 0; i < 3; ++i) { - scale.coeffRef(i) = -1 + sqrt(1 + estimate.coeff(3+i)); - bias.coeffRef(i) = estimate.coeff(i)/sqrt(1 + estimate.coeff(3+i)); - } + // eq 14b, mutatis mutandis (grumble, grumble) + Matrix increment; + scale.ldlt().solve(dJ_dtheta, &increment); + estimate -= increment; + } + + // Transform the estimated parameters from [c | e] back into [b | d]. + for (size_t i = 0; i < 3; ++i) { + scale.coeffRef(i) = -1 + sqrt(1 + estimate.coeff(3 + i)); + bias.coeffRef(i) = estimate.coeff(i) / sqrt(1 + estimate.coeff(3 + i)); + } } namespace { -// Private functions specific to the scale factor and orthogonal calibration +// Private functions specific to the scale factor and orthogonal calibration // version of TWOSTEP -/** +/** * Reconstruct the symmetric E matrix from the last 6 elements of the estimate * vector */ -Matrix3d -E_theta(const Matrix& theta) +Matrix3d E_theta(const Matrix & theta) { - // By equation 49b - return (Matrix3d() << - theta.coeff(3), theta.coeff(6), theta.coeff(7), - theta.coeff(6), theta.coeff(4), theta.coeff(8), - theta.coeff(7), theta.coeff(8), theta.coeff(5)).finished(); + // By equation 49b + return (Matrix3d() << + theta.coeff(3), theta.coeff(6), theta.coeff(7), + theta.coeff(6), theta.coeff(4), theta.coeff(8), + theta.coeff(7), theta.coeff(8), theta.coeff(5)).finished(); } /** @@ -401,22 +399,23 @@ E_theta(const Matrix& theta) * From eq 55 of [2]. */ Matrix -dnormb_dtheta(const Matrix& theta) +dnormb_dtheta(const Matrix & theta) { - // Re-form the matrix E from the vector of theta' - Matrix3d E = E_theta(theta); - // Compute (I + E)^-1 * c - Vector3d IE_inv_c; - E.ldlt().solve(theta.end<3>(), &IE_inv_c); + // Re-form the matrix E from the vector of theta' + Matrix3d E = E_theta(theta); + // Compute (I + E)^-1 * c + Vector3d IE_inv_c; - return (Matrix() << 2*IE_inv_c, - -IE_inv_c.coeff(0)*IE_inv_c.coeff(0), - -IE_inv_c.coeff(1)*IE_inv_c.coeff(1), - -IE_inv_c.coeff(2)*IE_inv_c.coeff(2), + E.ldlt().solve(theta.end<3>(), &IE_inv_c); - -2*IE_inv_c.coeff(0)*IE_inv_c.coeff(1), - -2*IE_inv_c.coeff(0)*IE_inv_c.coeff(2), - -2*IE_inv_c.coeff(1)*IE_inv_c.coeff(2)).finished(); + return (Matrix() << 2 * IE_inv_c, + -IE_inv_c.coeff(0) * IE_inv_c.coeff(0), + -IE_inv_c.coeff(1) * IE_inv_c.coeff(1), + -IE_inv_c.coeff(2) * IE_inv_c.coeff(2), + + -2 * IE_inv_c.coeff(0) * IE_inv_c.coeff(1), + -2 * IE_inv_c.coeff(0) * IE_inv_c.coeff(2), + -2 * IE_inv_c.coeff(1) * IE_inv_c.coeff(2)).finished(); } // The gradient of the cost function with respect to theta, at a particular @@ -427,25 +426,25 @@ dnormb_dtheta(const Matrix& theta) // @return the gradient of the cost function with respect to the // current value of the estimate, theta' Matrix -dJ_dtheta(const Matrix& centerL, - double centerMag, - const Matrix& theta, - const Matrix& dbdtheta, - double mu, - double noise) +dJ_dtheta(const Matrix & centerL, + double centerMag, + const Matrix & theta, + const Matrix & dbdtheta, + double mu, + double noise) { - // \frac{\delta}{\delta \theta'} |b(\theta')|^2 - Matrix vectorPart = dbdtheta - centerL; + // \frac{\delta}{\delta \theta'} |b(\theta')|^2 + Matrix vectorPart = dbdtheta - centerL; - // |b(\theta')|^2 - Matrix3d E = E_theta(theta); - Vector3d rhs; - (Matrix3d::Identity() + E).ldlt().solve(theta.start<3>(), &rhs); - double normbthetaSquared = theta.start<3>().dot(rhs); + // |b(\theta')|^2 + Matrix3d E = E_theta(theta); + Vector3d rhs; + (Matrix3d::Identity() + E).ldlt().solve(theta.start<3>(), &rhs); + double normbthetaSquared = theta.start<3>().dot(rhs); - double scalarPart = (centerMag - centerL.dot(theta) + normbthetaSquared - - mu)/noise; - return scalarPart * vectorPart; + double scalarPart = (centerMag - centerL.dot(theta) + normbthetaSquared + - mu) / noise; + return scalarPart * vectorPart; } } // !namespace (anonymous) @@ -463,7 +462,7 @@ dJ_dtheta(const Matrix& centerL, * \epsilon_k is the noise at the kth sample * * After computing the scale factor and bias matrices, the optimal estimate is - * given by + * given by * \tilde{B}_k = (I_{3x3} + D)B_k - b * * This implementation makes the assumption that \epsilon is a constant white, @@ -474,145 +473,143 @@ dJ_dtheta(const Matrix& centerL, * @param scale[out] The computed scale factor matrix, in the sensor frame. * @param samples[in] An array of measurement samples. * @param n_samples The number of samples in the array. - * @param referenceField The magnetic field vector at this location. + * @param referenceField The magnetic field vector at this location. * @param noise The one-sigma squared standard deviation of the observed noise * in the sensor. */ -void twostep_bias_scale(Vector3f& bias, - Matrix3f& scale, - const Vector3f samples[], - const size_t n_samples, - const Vector3f& referenceField, - const float noise) +void twostep_bias_scale(Vector3f & bias, + Matrix3f & scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f & referenceField, + const float noise) { - // Define L_k by eq 51 for k = 1 .. n_samples - Matrix fullSamples(n_samples, 9); - // \hbar{L} by eq 52, simplified by observing that the common noise term - // makes this a simple average. - Matrix centerSample = Matrix::Zero(); - // Define the sample differences z_k by eq 23 a) - double sampleDeltaMag[n_samples]; - // The center value \hbar{z} - double sampleDeltaMagCenter = 0; - // The squared norm of the reference vector - double refSquaredNorm = referenceField.squaredNorm(); + // Define L_k by eq 51 for k = 1 .. n_samples + Matrix fullSamples(n_samples, 9); + // \hbar{L} by eq 52, simplified by observing that the common noise term + // makes this a simple average. + Matrix centerSample = Matrix::Zero(); + // Define the sample differences z_k by eq 23 a) + double sampleDeltaMag[n_samples]; + // The center value \hbar{z} + double sampleDeltaMagCenter = 0; + // The squared norm of the reference vector + double refSquaredNorm = referenceField.squaredNorm(); - for (size_t i = 0; i < n_samples; ++i) { - fullSamples.row(i) << 2*samples[i].transpose().cast(), - -(samples[i][0]*samples[i][0]), - -(samples[i][1]*samples[i][1]), - -(samples[i][2]*samples[i][2]), - -2*(samples[i][0]*samples[i][1]), - -2*(samples[i][0]*samples[i][2]), - -2*(samples[i][1]*samples[i][2]); + for (size_t i = 0; i < n_samples; ++i) { + fullSamples.row(i) << 2 * samples[i].transpose().cast(), + -(samples[i][0] * samples[i][0]), + -(samples[i][1] * samples[i][1]), + -(samples[i][2] * samples[i][2]), + -2 * (samples[i][0] * samples[i][1]), + -2 * (samples[i][0] * samples[i][2]), + -2 * (samples[i][1] * samples[i][2]); - centerSample += fullSamples.row(i); + centerSample += fullSamples.row(i); - sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; - sampleDeltaMagCenter += sampleDeltaMag[i]; - } - sampleDeltaMagCenter /= n_samples; - centerSample /= n_samples; + sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; + sampleDeltaMagCenter += sampleDeltaMag[i]; + } + sampleDeltaMagCenter /= n_samples; + centerSample /= n_samples; - // Define \tilde{L}_k for k = 0 .. n_samples - Matrix centeredSamples(n_samples, 9); - // Define \tilde{z}_k for k = 0 .. n_samples - double centeredMags[n_samples]; - // Compute the term under the summation of eq 57a - Matrix estimateSummation = Matrix::Zero(); - for (size_t i = 0; i < n_samples; ++i) { - centeredSamples.row(i) = fullSamples.row(i) - centerSample; - centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; - estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose(); - } - estimateSummation /= noise; + // Define \tilde{L}_k for k = 0 .. n_samples + Matrix centeredSamples(n_samples, 9); + // Define \tilde{z}_k for k = 0 .. n_samples + double centeredMags[n_samples]; + // Compute the term under the summation of eq 57a + Matrix estimateSummation = Matrix::Zero(); + for (size_t i = 0; i < n_samples; ++i) { + centeredSamples.row(i) = fullSamples.row(i) - centerSample; + centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; + estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose(); + } + estimateSummation /= noise; - // By eq 57b - Matrix P_theta_theta_inv = (1.0f/noise)* - centeredSamples.transpose()*centeredSamples; + // By eq 57b + Matrix P_theta_theta_inv = (1.0f / noise) * + centeredSamples.transpose() * centeredSamples; #ifdef PRINTF_DEBUGGING - SelfAdjointEigenSolver > eig(P_theta_theta_inv); - std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n"; - std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose() - << "\n"; - std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n"; + SelfAdjointEigenSolver > eig(P_theta_theta_inv); + std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n"; + std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose() + << "\n"; + std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n"; #endif - // The current value of the estimate. Initialized to \tilde{\theta}^* - Matrix estimate; - // By eq 57a - P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate); + // The current value of the estimate. Initialized to \tilde{\theta}^* + Matrix estimate; + // By eq 57a + P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate); - // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradient(theta) - // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1} - size_t count = 0; - double eta = 10000; - while (count++ < 200 && eta > 1e-8) { - static bool warned = false; - if (hasNaN(estimate)) { - std::cout << "WARNING: found NaN at time " << count << "\n"; - warned = true; - } + // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradient(theta) + // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1} + size_t count = 0; + double eta = 10000; + while (count++ < 200 && eta > 1e-8) { + static bool warned = false; + if (hasNaN(estimate)) { + std::cout << "WARNING: found NaN at time " << count << "\n"; + warned = true; + } #if 0 - SelfAdjointEigenSolver eig_E(E_theta(estimate)); - Vector3d S = eig_E.eigenvalues(); - Vector3d W; W << -1 + sqrt(1 + S.coeff(0)), - -1 + sqrt(1 + S.coeff(1)), - -1 + sqrt(1 + S.coeff(2)); - scale = (eig_E.eigenvectors() * W.asDiagonal() * - eig_E.eigenvectors().transpose()) .cast(); + SelfAdjointEigenSolver eig_E(E_theta(estimate)); + Vector3d S = eig_E.eigenvalues(); + Vector3d W; W << -1 + sqrt(1 + S.coeff(0)), + -1 + sqrt(1 + S.coeff(1)), + -1 + sqrt(1 + S.coeff(2)); + scale = (eig_E.eigenvectors() * W.asDiagonal() * + eig_E.eigenvectors().transpose()).cast(); - (Matrix3f::Identity() + scale).ldlt().solve( - estimate.start<3>().cast(), &bias); - std::cout << "\n\nestimated bias: " << bias.transpose() - << "\nestimated scale:\n" << scale; + (Matrix3f::Identity() + scale).ldlt().solve( + estimate.start<3>().cast(), &bias); + std::cout << "\n\nestimated bias: " << bias.transpose() + << "\nestimated scale:\n" << scale; #endif - - Matrix db_dtheta = dnormb_dtheta(estimate); - Matrix dJ_dtheta = ::dJ_dtheta(centerSample, - sampleDeltaMagCenter, - estimate, - db_dtheta, - -3*noise, - noise/n_samples); + Matrix db_dtheta = dnormb_dtheta(estimate); - // Eq 59, with reused storage on db_dtheta - db_dtheta = centerSample - db_dtheta; - Matrix scale = P_theta_theta_inv + - (double(n_samples)/noise)*db_dtheta.transpose() * db_dtheta; + Matrix dJ_dtheta = ::dJ_dtheta(centerSample, + sampleDeltaMagCenter, + estimate, + db_dtheta, + -3 * noise, + noise / n_samples); - // eq 14b, mutatis mutandis (grumble, grumble) - Matrix increment; - scale.ldlt().solve(dJ_dtheta, &increment); - estimate -= increment; - eta = increment.dot(scale * increment); - std::cout << "eta: " << eta << "\n"; - } - std::cout << "terminated at eta = " << eta - << " after " << count << " iterations\n"; - - if (!isnan(eta) && !isinf(eta)) { - // Transform the estimated parameters from [c | E] back into [b | D]. - // See eq 63-65 - SelfAdjointEigenSolver eig_E(E_theta(estimate)); - Vector3d S = eig_E.eigenvalues(); - Vector3d W; W << -1 + sqrt(1 + S.coeff(0)), - -1 + sqrt(1 + S.coeff(1)), - -1 + sqrt(1 + S.coeff(2)); - scale = (eig_E.eigenvectors() * W.asDiagonal() * - eig_E.eigenvectors().transpose()) .cast(); + // Eq 59, with reused storage on db_dtheta + db_dtheta = centerSample - db_dtheta; + Matrix scale = P_theta_theta_inv + + (double(n_samples) / noise) * db_dtheta.transpose() * db_dtheta; - (Matrix3f::Identity() + scale).ldlt().solve( - estimate.start<3>().cast(), &bias); - } - else { - // return nonsense data. The eigensolver can fall ingo - // an infinite loop otherwise. - // TODO: Add error code return - scale = Matrix3f::Ones()*std::numeric_limits::quiet_NaN(); - bias = Vector3f::Zero(); - } + // eq 14b, mutatis mutandis (grumble, grumble) + Matrix increment; + scale.ldlt().solve(dJ_dtheta, &increment); + estimate -= increment; + eta = increment.dot(scale * increment); + std::cout << "eta: " << eta << "\n"; + } + std::cout << "terminated at eta = " << eta + << " after " << count << " iterations\n"; + + if (!isnan(eta) && !isinf(eta)) { + // Transform the estimated parameters from [c | E] back into [b | D]. + // See eq 63-65 + SelfAdjointEigenSolver eig_E(E_theta(estimate)); + Vector3d S = eig_E.eigenvalues(); + Vector3d W; W << -1 + sqrt(1 + S.coeff(0)), + -1 + sqrt(1 + S.coeff(1)), + -1 + sqrt(1 + S.coeff(2)); + scale = (eig_E.eigenvectors() * W.asDiagonal() * + eig_E.eigenvectors().transpose()).cast(); + + (Matrix3f::Identity() + scale).ldlt().solve( + estimate.start<3>().cast(), &bias); + } else { + // return nonsense data. The eigensolver can fall ingo + // an infinite loop otherwise. + // TODO: Add error code return + scale = Matrix3f::Ones() * std::numeric_limits::quiet_NaN(); + bias = Vector3f::Zero(); + } } - diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.cpp b/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.cpp index b3af24ace..645b9f547 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.cpp +++ b/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -29,8 +29,8 @@ #include "texteditloggerengine.h" ConsoleGadget::ConsoleGadget(QString classId, ConsoleGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) + IUAVGadget(classId, parent), + m_widget(widget) { m_logger = new TextEditLoggerEngine(widget); bool suitableName = false; @@ -38,8 +38,9 @@ ConsoleGadget::ConsoleGadget(QString classId, ConsoleGadgetWidget *widget, QWidg QString loggerName; while (!suitableName) { loggerName = "TextEditLogger" + QVariant(i).toString(); - if (!qxtLog->isLoggerEngine(loggerName)) + if (!qxtLog->isLoggerEngine(loggerName)) { suitableName = true; + } ++i; } qxtLog->addLoggerEngine(loggerName, m_logger); diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.h b/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.h index 50c572cf6..8495e4ef5 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.h +++ b/ground/openpilotgcs/src/plugins/consolegadget/consolegadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,16 +38,24 @@ class TextEditLoggerEngine; using namespace Core; -class ConsoleGadget : public Core::IUAVGadget -{ +class ConsoleGadget : public Core::IUAVGadget { Q_OBJECT public: ConsoleGadget(QString classId, ConsoleGadgetWidget *widget, QWidget *parent = 0); ~ConsoleGadget(); - QList context() const { return m_context; } - QWidget *widget() { return m_widget; } - QString contextHelpId() const { return QString(); } + QList context() const + { + return m_context; + } + QWidget *widget() + { + return m_widget; + } + QString contextHelpId() const + { + return QString(); + } private: QWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.cpp b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.cpp index 1989c3e5e..89df10e3b 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,18 +30,17 @@ #include ConsoleGadgetFactory::ConsoleGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("ConsoleGadget"), - tr("Console"), - parent) -{ -} + IUAVGadgetFactory(QString("ConsoleGadget"), + tr("Console"), + parent) +{} ConsoleGadgetFactory::~ConsoleGadgetFactory() +{} + +IUAVGadget *ConsoleGadgetFactory::createGadget(QWidget *parent) { + ConsoleGadgetWidget *gadgetWidget = new ConsoleGadgetWidget(parent); -} - -IUAVGadget* ConsoleGadgetFactory::createGadget(QWidget *parent) { - ConsoleGadgetWidget* gadgetWidget = new ConsoleGadgetWidget(parent); return new ConsoleGadget(QString("ConsoleGadget"), gadgetWidget, parent); } diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.h b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.h index 183c8c17e..9d21946c1 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,8 +37,7 @@ class IUAVGadgetFactory; using namespace Core; -class ConsoleGadgetFactory : public IUAVGadgetFactory -{ +class ConsoleGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: ConsoleGadgetFactory(QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.cpp b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.cpp index 5267131a1..8218ed34e 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,13 +32,12 @@ ConsoleGadgetWidget::ConsoleGadgetWidget(QWidget *parent) : QTextEdit(parent) { - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setReadOnly(true); } ConsoleGadgetWidget::~ConsoleGadgetWidget() { - // Do nothing + // Do nothing } - diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.h b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.h index 65fe3d18a..f72665a47 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/consolegadget/consolegadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,13 +30,12 @@ #include -class ConsoleGadgetWidget : public QTextEdit -{ +class ConsoleGadgetWidget : public QTextEdit { Q_OBJECT public: ConsoleGadgetWidget(QWidget *parent = 0); - ~ConsoleGadgetWidget(); + ~ConsoleGadgetWidget(); private: }; diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.cpp b/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.cpp index 5461b90a8..7e1806cb5 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.cpp +++ b/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,32 +34,31 @@ ConsolePlugin::ConsolePlugin() { - // Do nothing + // Do nothing } ConsolePlugin::~ConsolePlugin() { - // Do nothing + // Do nothing } -bool ConsolePlugin::initialize(const QStringList& args, QString *errMsg) +bool ConsolePlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new ConsoleGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new ConsoleGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void ConsolePlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void ConsolePlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(ConsolePlugin) - diff --git a/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.h b/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.h index 243de47d0..5e859d0da 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.h +++ b/ground/openpilotgcs/src/plugins/consolegadget/consoleplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class ConsoleGadgetFactory; -class ConsolePlugin : public ExtensionSystem::IPlugin -{ +class ConsolePlugin : public ExtensionSystem::IPlugin { public: - ConsolePlugin(); - ~ConsolePlugin(); + ConsolePlugin(); + ~ConsolePlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - ConsoleGadgetFactory *mf; + ConsoleGadgetFactory *mf; }; #endif /* CONSOLEPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.cpp b/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.cpp index 136260557..df9acadeb 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.cpp +++ b/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.cpp @@ -8,21 +8,21 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -45,17 +45,16 @@ TextEditLoggerEngine::TextEditLoggerEngine(QTextEdit *textEdit) : m_textEdit(tex } TextEditLoggerEngine::~TextEditLoggerEngine() -{ -} +{} void TextEditLoggerEngine::initLoggerEngine() { - return; // Should work out of the box! + // Should work out of the box! } void TextEditLoggerEngine::killLoggerEngine() { - return; // I do nothing. + // I do nothing. } bool TextEditLoggerEngine::isInitialized() const @@ -66,13 +65,15 @@ bool TextEditLoggerEngine::isInitialized() const void TextEditLoggerEngine::setLogLevelEnabled(QxtLogger::LogLevels level, bool enable) { QxtLoggerEngine::setLogLevelsEnabled(level | QXT_REQUIRED_LEVELS, enable); - if (!enable) QxtLoggerEngine::setLogLevelsEnabled(QXT_REQUIRED_LEVELS); + + if (!enable) { + QxtLoggerEngine::setLogLevelsEnabled(QXT_REQUIRED_LEVELS); + } } void TextEditLoggerEngine::writeFormatted(QxtLogger::LogLevel level, const QList &msgs) { - switch (level) - { + switch (level) { case QxtLogger::ErrorLevel: writeToTextEdit("Error", msgs, Qt::red); break; @@ -100,29 +101,31 @@ void TextEditLoggerEngine::writeFormatted(QxtLogger::LogLevel level, const QList } } -void TextEditLoggerEngine::writeToTextEdit(const QString& level, const QList &msgs, QColor color) +void TextEditLoggerEngine::writeToTextEdit(const QString & level, const QList &msgs, QColor color) { /* Message format... [time] [error level] First message..... second message third message - */ - if (msgs.isEmpty()) + */ + if (msgs.isEmpty()) { return; + } QScrollBar *sb = m_textEdit->verticalScrollBar(); - bool scroll = sb->value() == sb->maximum(); + bool scroll = sb->value() == sb->maximum(); QString header = '[' + QTime::currentTime().toString("hh:mm:ss.zzz") + "] [" + level + "] "; QString padding; QString appendText; appendText.append(header); - for (int i = 0; i < header.size(); i++) padding.append(' '); + for (int i = 0; i < header.size(); i++) { + padding.append(' '); + } int count = 0; - Q_FOREACH(const QVariant& out, msgs) - { - if (!out.isNull()) - { - if (count != 0) + Q_FOREACH(const QVariant &out, msgs) { + if (!out.isNull()) { + if (count != 0) { appendText.append(padding); + } appendText.append(out.toString()); } count++; @@ -130,6 +133,7 @@ void TextEditLoggerEngine::writeToTextEdit(const QString& level, const QList%2").arg(color.name()).arg(appendText); m_textEdit->append(appendText); - if (scroll) + if (scroll) { sb->setValue(sb->maximum()); + } } diff --git a/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.h b/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.h index 6f4b4d22a..6ba73b479 100644 --- a/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.h +++ b/ground/openpilotgcs/src/plugins/consolegadget/texteditloggerengine.h @@ -8,21 +8,21 @@ * @{ * @addtogroup ConsolePlugin Console Plugin * @{ - * @brief The Console Gadget impliments a console view + * @brief The Console Gadget impliments a console view *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include class QTextEdit; -class TextEditLoggerEngine : public QxtLoggerEngine -{ - +class TextEditLoggerEngine : public QxtLoggerEngine { public: TextEditLoggerEngine(QTextEdit *textEdit); ~TextEditLoggerEngine(); @@ -50,7 +48,7 @@ public: bool isInitialized() const; private: - virtual void writeToTextEdit(const QString& str_level, const QList &msgs, QColor color = QColor(0,0,0)); + virtual void writeToTextEdit(const QString & str_level, const QList &msgs, QColor color = QColor(0, 0, 0)); QTextEdit *m_textEdit; }; diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.cpp b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.cpp index 0755db0fa..bf8742d75 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,7 +38,7 @@ #include #include -Q_DECLARE_METATYPE(Core::Internal::MenuActionContainer*) +Q_DECLARE_METATYPE(Core::Internal::MenuActionContainer *) using namespace Core; using namespace Core::Internal; @@ -64,7 +64,7 @@ using namespace Core::Internal; You can define if the menu represented by this action container should automatically disable or hide whenever it only contains disabled items and submenus by setting the corresponding \l{ActionContainer::setEmptyAction()}{EmptyAction}. -*/ + */ /*! \enum ActionContainer::EmptyAction @@ -76,37 +76,37 @@ using namespace Core::Internal; The menu will be visible but disabled. \value EA_Hide The menu will not be visible until the state of the subitems change. -*/ + */ /*! \fn ActionContainer::setEmptyAction(EmptyAction disableOrHide) Defines if the menu represented by this action container should automatically \a disableOrHide whenever it only contains disabled items and submenus. \sa ActionContainer::EmptyAction -*/ + */ /*! \fn int ActionContainer::id() const \internal -*/ + */ /*! \fn QMenu *ActionContainer::menu() const Returns the QMenu instance that is represented by this action container, or 0 if this action container represents a menu bar. -*/ + */ /*! \fn QMenuBar *ActionContainer::menuBar() const Returns the QMenuBar instance that is represented by this action container, or 0 if this action container represents a menu. -*/ + */ /*! \fn QAction *ActionContainer::insertLocation(const QString &group) const Returns an action representing the \a group, that could be used with \c{QWidget::insertAction}. -*/ + */ /*! \fn void ActionContainer::appendGroup(const QString &identifier) @@ -115,7 +115,7 @@ using namespace Core::Internal; menus directly to these parts. \sa addAction() \sa addMenu() -*/ + */ /*! \fn void ActionContainer::addAction(Core::Command *action, const QString &group) @@ -123,7 +123,7 @@ using namespace Core::Internal; last item of the specified \a group. \sa appendGroup() \sa addMenu() -*/ + */ /*! \fn void ActionContainer::addMenu(Core::ActionContainer *menu, const QString &group) @@ -131,30 +131,28 @@ using namespace Core::Internal; last item of the specified \a group. \sa appendGroup() \sa addAction() -*/ + */ /*! \fn bool ActionContainer::update() \internal -*/ + */ /*! \fn ActionContainer::~ActionContainer() \internal -*/ + */ // ---------- ActionContainerPrivate ------------ /*! \class Core::Internal::ActionContainerPrivate \internal -*/ + */ ActionContainerPrivate::ActionContainerPrivate(int id) : m_data(0), m_id(id) -{ - -} +{} void ActionContainerPrivate::setEmptyAction(EmptyAction ea) { @@ -169,29 +167,34 @@ bool ActionContainerPrivate::hasEmptyAction(EmptyAction ea) const void ActionContainerPrivate::appendGroup(const QString &group) { int gid = UniqueIDManager::instance()->uniqueIdentifier(group); + m_groups << gid; } QAction *ActionContainerPrivate::insertLocation(const QString &group) const { - int grpid = UniqueIDManager::instance()->uniqueIdentifier(group); + int grpid = UniqueIDManager::instance()->uniqueIdentifier(group); int prevKey = 0; - int pos = ((grpid << 16) | 0xFFFF); + int pos = ((grpid << 16) | 0xFFFF); + return beforeAction(pos, &prevKey); } void ActionContainerPrivate::addAction(Command *action, const QString &group) { - if (!canAddAction(action)) + if (!canAddAction(action)) { return; + } - ActionManagerPrivate *am = ActionManagerPrivate::instance(); + ActionManagerPrivate *am = ActionManagerPrivate::instance(); UniqueIDManager *idmanager = UniqueIDManager::instance(); int grpid = idmanager->uniqueIdentifier(Constants::G_DEFAULT_TWO); - if (!group.isEmpty()) + if (!group.isEmpty()) { grpid = idmanager->uniqueIdentifier(group); - if (!m_groups.contains(grpid) && !am->defaultGroups().contains(grpid)) + } + if (!m_groups.contains(grpid) && !am->defaultGroups().contains(grpid)) { qWarning() << "*** addAction(): Unknown group: " << group; + } int pos = ((grpid << 16) | 0xFFFF); addAction(action, pos, true); } @@ -199,16 +202,20 @@ void ActionContainerPrivate::addAction(Command *action, const QString &group) void ActionContainerPrivate::addMenu(ActionContainer *menu, const QString &group) { ActionContainerPrivate *container = static_cast(menu); - if (!container->canBeAddedToMenu()) - return; - ActionManagerPrivate *am = ActionManagerPrivate::instance(); + if (!container->canBeAddedToMenu()) { + return; + } + + ActionManagerPrivate *am = ActionManagerPrivate::instance(); UniqueIDManager *idmanager = UniqueIDManager::instance(); int grpid = idmanager->uniqueIdentifier(Constants::G_DEFAULT_TWO); - if (!group.isEmpty()) + if (!group.isEmpty()) { grpid = idmanager->uniqueIdentifier(group); - if (!m_groups.contains(grpid) && !am->defaultGroups().contains(grpid)) + } + if (!m_groups.contains(grpid) && !am->defaultGroups().contains(grpid)) { qWarning() << "*** addMenu(): Unknown group: " << group; + } int pos = ((grpid << 16) | 0xFFFF); addMenu(menu, pos, true); } @@ -230,12 +237,12 @@ QMenuBar *ActionContainerPrivate::menuBar() const bool ActionContainerPrivate::canAddAction(Command *action) const { - return (action->action() != 0); + return action->action() != 0; } void ActionContainerPrivate::addAction(Command *action, int pos, bool setpos) { - Action *a = static_cast(action); + Action *a = static_cast(action); int prevKey = 0; QAction *ba = beforeAction(pos, &prevKey); @@ -244,7 +251,7 @@ void ActionContainerPrivate::addAction(Command *action, int pos, bool setpos) pos = calcPosition(pos, prevKey); CommandLocation loc; loc.m_container = m_id; - loc.m_position = pos; + loc.m_position = pos; QList locs = a->locations(); locs.append(loc); a->setLocations(locs); @@ -266,7 +273,7 @@ void ActionContainerPrivate::addMenu(ActionContainer *menu, int pos, bool setpos pos = calcPosition(pos, prevKey); CommandLocation loc; loc.m_container = m_id; - loc.m_position = pos; + loc.m_position = pos; mc->setLocation(loc); } @@ -293,14 +300,18 @@ QAction *ActionContainerPrivate::beforeAction(int pos, int *prevKey) const ++i; } - if (baId == -1) + if (baId == -1) { return 0; + } - if (Command *cmd = am->command(baId)) + if (Command * cmd = am->command(baId)) { return cmd->action(); - if (ActionContainer *container = am->actionContainer(baId)) - if (QMenu *menu = container->menu()) + } + if (ActionContainer * container = am->actionContainer(baId)) { + if (QMenu * menu = container->menu()) { return menu->menuAction(); + } + } return 0; } @@ -308,13 +319,16 @@ QAction *ActionContainerPrivate::beforeAction(int pos, int *prevKey) const int ActionContainerPrivate::calcPosition(int pos, int prevKey) const { int grp = (pos & 0xFFFF0000); - if (prevKey == -1) + + if (prevKey == -1) { return grp; + } int prevgrp = (prevKey & 0xFFFF0000); - if (grp != prevgrp) + if (grp != prevgrp) { return grp; + } return grp + (prevKey & 0xFFFF) + 10; } @@ -324,7 +338,7 @@ int ActionContainerPrivate::calcPosition(int pos, int prevKey) const /*! \class Core::Internal::MenuActionContainer \internal -*/ + */ MenuActionContainer::MenuActionContainer(int id) : ActionContainerPrivate(id), m_menu(0) @@ -337,7 +351,7 @@ void MenuActionContainer::setMenu(QMenu *menu) m_menu = menu; QVariant v; - qVariantSetValue(v, this); + qVariantSetValue(v, this); m_menu->menuAction()->setData(v); } @@ -369,14 +383,15 @@ CommandLocation MenuActionContainer::location() const bool MenuActionContainer::update() { - if (hasEmptyAction(EA_None)) + if (hasEmptyAction(EA_None)) { return true; + } bool hasitems = false; - foreach (ActionContainer *container, subContainers()) { + foreach(ActionContainer * container, subContainers()) { if (container == this) { - qWarning() << Q_FUNC_INFO << "container" << (this->menu() ? this->menu()->title() : "") << "contains itself as subcontainer"; + qWarning() << Q_FUNC_INFO << "container" << (this->menu() ? this->menu()->title() : "") << "contains itself as subcontainer"; continue; } if (container->update()) { @@ -385,7 +400,7 @@ bool MenuActionContainer::update() } } if (!hasitems) { - foreach (Command *command, commands()) { + foreach(Command * command, commands()) { if (command->isActive()) { hasitems = true; break; @@ -393,10 +408,11 @@ bool MenuActionContainer::update() } } - if (hasEmptyAction(EA_Hide)) + if (hasEmptyAction(EA_Hide)) { m_menu->setVisible(hasitems); - else if (hasEmptyAction(EA_Disable)) + } else if (hasEmptyAction(EA_Disable)) { m_menu->setEnabled(hasitems); + } return hasitems; } @@ -412,7 +428,7 @@ bool MenuActionContainer::canBeAddedToMenu() const /*! \class Core::Internal::MenuBarActionContainer \internal -*/ + */ MenuBarActionContainer::MenuBarActionContainer(int id) : ActionContainerPrivate(id), m_menuBar(0) @@ -442,22 +458,24 @@ void MenuBarActionContainer::insertMenu(QAction *before, QMenu *menu) bool MenuBarActionContainer::update() { - if (hasEmptyAction(EA_None)) + if (hasEmptyAction(EA_None)) { return true; + } bool hasitems = false; QList actions = m_menuBar->actions(); - for (int i=0; iisVisible()) { hasitems = true; break; } } - if (hasEmptyAction(EA_Hide)) + if (hasEmptyAction(EA_Hide)) { m_menuBar->setVisible(hasitems); - else if (hasEmptyAction(EA_Disable)) + } else if (hasEmptyAction(EA_Disable)) { m_menuBar->setEnabled(hasitems); + } return hasitems; } @@ -466,4 +484,3 @@ bool MenuBarActionContainer::canBeAddedToMenu() const { return false; } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.h b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.h index a8e20272f..c29f269af 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,17 +35,15 @@ #include namespace Core { - class Command; -class ActionContainer : public QObject -{ +class ActionContainer : public QObject { public: enum EmptyAction { - EA_Mask = 0xFF00, - EA_None = 0x0100, - EA_Hide = 0x0200, - EA_Disable = 0x0300 + EA_Mask = 0xFF00, + EA_None = 0x0100, + EA_Hide = 0x0200, + EA_Disable = 0x0300 }; virtual void setEmptyAction(EmptyAction ea) = 0; @@ -57,13 +55,12 @@ public: virtual QAction *insertLocation(const QString &group) const = 0; virtual void appendGroup(const QString &group) = 0; - virtual void addAction(Core::Command *action, const QString &group = QString()) = 0; + virtual void addAction(Core::Command *action, const QString &group = QString()) = 0; virtual void addMenu(Core::ActionContainer *menu, const QString &group = QString()) = 0; virtual bool update() = 0; virtual ~ActionContainer() {} }; - } // namespace Core #endif // ACTIONCONTAINER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer_p.h b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer_p.h index 47cb6f83d..cb906269e 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer_p.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actioncontainer_p.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,9 +36,7 @@ namespace Core { namespace Internal { - -class ActionContainerPrivate : public Core::ActionContainer -{ +class ActionContainerPrivate : public Core::ActionContainer { public: ActionContainerPrivate(int id); virtual ~ActionContainerPrivate() {} @@ -59,8 +57,14 @@ public: virtual void insertAction(QAction *before, QAction *action) = 0; virtual void insertMenu(QAction *before, QMenu *menu) = 0; - QList commands() const { return m_commands; } - QList subContainers() const { return m_subContainers; } + QList commands() const + { + return m_commands; + } + QList subContainers() const + { + return m_subContainers; + } protected: bool canAddAction(Command *action) const; bool canAddMenu(ActionContainer *menu) const; @@ -81,8 +85,7 @@ private: QList m_commands; }; -class MenuActionContainer : public ActionContainerPrivate -{ +class MenuActionContainer : public ActionContainerPrivate { public: MenuActionContainer(int id); @@ -103,8 +106,7 @@ private: CommandLocation m_location; }; -class MenuBarActionContainer : public ActionContainerPrivate -{ +class MenuBarActionContainer : public ActionContainerPrivate { public: MenuBarActionContainer(int id); @@ -120,7 +122,6 @@ protected: private: QMenuBar *m_menuBar; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.cpp index 844780df5..a86a70f0c 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,7 +42,7 @@ #include namespace { - enum { warnAboutFindFailures = 0 }; +enum { warnAboutFindFailures = 0 }; } /*! @@ -136,7 +136,7 @@ namespace { \sa Core::Command \sa Core::ActionContainer \sa Core::IContext -*/ + */ /*! \fn ActionContainer *ActionManager::createMenu(const QString &id) @@ -147,7 +147,7 @@ namespace { the returned ActionContainer. Add your menu to some other menu or a menu bar via the ActionManager::actionContainer and ActionContainer::addMenu methods. -*/ + */ /*! \fn ActionContainer *ActionManager::createMenuBar(const QString &id) @@ -156,7 +156,7 @@ namespace { Returns a new ActionContainer that you can use to get the QMenuBar instance or to add menus to the menu bar. The ActionManager owns the returned ActionContainer. -*/ + */ /*! \fn Command *ActionManager::registerAction(QAction *action, const QString &id, const QList &context) @@ -167,7 +167,7 @@ namespace { same \a id as long as the \a context is different. In this case a trigger of the actual action is forwarded to the registered QAction for the currently active context. -*/ + */ /*! \fn Command *ActionManager::registerShortcut(QShortcut *shortcut, const QString &id, const QList &context) @@ -178,7 +178,7 @@ namespace { same \a id as long as the \a context is different. In this case a trigger of the actual shortcut is forwarded to the registered QShortcut for the currently active context. -*/ + */ /*! \fn Command *ActionManager::command(const QString &id) const @@ -186,7 +186,7 @@ namespace { under the given string \a id. \sa ActionManager::registerAction() -*/ + */ /*! \fn ActionContainer *ActionManager::actionContainer(const QString &id) const @@ -195,37 +195,37 @@ namespace { \sa ActionManager::createMenu() \sa ActionManager::createMenuBar() -*/ + */ /*! \fn ActionManager::ActionManager(QObject *parent) \internal -*/ + */ /*! \fn ActionManager::~ActionManager() \internal -*/ + */ using namespace Core; using namespace Core::Internal; -ActionManagerPrivate* ActionManagerPrivate::m_instance = 0; +ActionManagerPrivate *ActionManagerPrivate::m_instance = 0; /*! \class ActionManagerPrivate \inheaderfile actionmanager_p.h \internal -*/ + */ ActionManagerPrivate::ActionManagerPrivate(MainWindow *mainWnd) - : ActionManager(mainWnd), + : ActionManager(mainWnd), m_mainWnd(mainWnd) { UniqueIDManager *uidmgr = UniqueIDManager::instance(); + m_defaultGroups << uidmgr->uniqueIdentifier(Constants::G_DEFAULT_ONE); m_defaultGroups << uidmgr->uniqueIdentifier(Constants::G_DEFAULT_TWO); m_defaultGroups << uidmgr->uniqueIdentifier(Constants::G_DEFAULT_THREE); m_instance = this; - } ActionManagerPrivate::~ActionManagerPrivate() @@ -266,19 +266,22 @@ void ActionManagerPrivate::setContext(const QList &context) // and only update commands that are either in old or new contexts m_context = context; const IdCmdMap::const_iterator cmdcend = m_idCmdMap.constEnd(); - for (IdCmdMap::const_iterator it = m_idCmdMap.constBegin(); it != cmdcend; ++it) + for (IdCmdMap::const_iterator it = m_idCmdMap.constBegin(); it != cmdcend; ++it) { it.value()->setCurrentContext(m_context); + } const IdContainerMap::const_iterator acend = m_idContainerMap.constEnd(); - for (IdContainerMap::const_iterator it = m_idContainerMap.constBegin(); it != acend; ++it) + for (IdContainerMap::const_iterator it = m_idContainerMap.constBegin(); it != acend; ++it) { it.value()->update(); + } } bool ActionManagerPrivate::hasContext(QList context) const { - for (int i=0; iuniqueIdentifier(id); const IdContainerMap::const_iterator it = m_idContainerMap.constFind(uid); - if (it != m_idContainerMap.constEnd()) + + if (it != m_idContainerMap.constEnd()) { return it.value(); + } QMenu *m = new QMenu(m_mainWnd); m->setObjectName(id); @@ -305,8 +310,10 @@ ActionContainer *ActionManagerPrivate::createMenuBar(const QString &id) { const int uid = UniqueIDManager::instance()->uniqueIdentifier(id); const IdContainerMap::const_iterator it = m_idContainerMap.constFind(uid); - if (it != m_idContainerMap.constEnd()) + + if (it != m_idContainerMap.constEnd()) { return it.value(); + } QMenuBar *mb = new QMenuBar; // No parent (System menu bar on Mac OS X) mb->setObjectName(id); @@ -323,9 +330,11 @@ Command *ActionManagerPrivate::registerAction(QAction *action, const QString &id { OverrideableAction *a = 0; Command *c = registerOverridableAction(action, id, false); + a = static_cast(c); - if (a) + if (a) { a->addOverrideAction(action, context); + } return a; } @@ -333,7 +342,8 @@ Command *ActionManagerPrivate::registerOverridableAction(QAction *action, const { OverrideableAction *a = 0; const int uid = UniqueIDManager::instance()->uniqueIdentifier(id); - if (CommandPrivate *c = m_idCmdMap.value(uid, 0)) { + + if (CommandPrivate * c = m_idCmdMap.value(uid, 0)) { a = qobject_cast(c); if (!a) { qWarning() << "registerAction: id" << id << "is registered with a different command type."; @@ -366,7 +376,7 @@ Command *ActionManagerPrivate::registerOverridableAction(QAction *action, const m_mainWnd->addAction(baseAction); a->setKeySequence(a->keySequence()); a->setDefaultKeySequence(QKeySequence()); - } else if (checkUnique) { + } else if (checkUnique) { qWarning() << "registerOverridableAction: id" << id << "is already registered."; } @@ -377,7 +387,8 @@ Command *ActionManagerPrivate::registerShortcut(QShortcut *shortcut, const QStri { Shortcut *sc = 0; int uid = UniqueIDManager::instance()->uniqueIdentifier(id); - if (CommandPrivate *c = m_idCmdMap.value(uid, 0)) { + + if (CommandPrivate * c = m_idCmdMap.value(uid, 0)) { sc = qobject_cast(c); if (!sc) { qWarning() << "registerShortcut: id" << id << "is registered with a different command type."; @@ -393,16 +404,18 @@ Command *ActionManagerPrivate::registerShortcut(QShortcut *shortcut, const QStri return sc; } - if (!hasContext(context)) + if (!hasContext(context)) { shortcut->setEnabled(false); + } shortcut->setObjectName(id); shortcut->setParent(m_mainWnd); sc->setShortcut(shortcut); - if (context.isEmpty()) + if (context.isEmpty()) { sc->setContext(QList() << 0); - else + } else { sc->setContext(context); + } sc->setKeySequence(shortcut->key()); sc->setDefaultKeySequence(QKeySequence()); @@ -414,9 +427,11 @@ Command *ActionManagerPrivate::command(const QString &id) const { const int uid = UniqueIDManager::instance()->uniqueIdentifier(id); const IdCmdMap::const_iterator it = m_idCmdMap.constFind(uid); + if (it == m_idCmdMap.constEnd()) { - if (warnAboutFindFailures) + if (warnAboutFindFailures) { qWarning() << "ActionManagerPrivate::command(): failed to find :" << id << '/' << uid; + } return 0; } return it.value(); @@ -426,9 +441,11 @@ ActionContainer *ActionManagerPrivate::actionContainer(const QString &id) const { const int uid = UniqueIDManager::instance()->uniqueIdentifier(id); const IdContainerMap::const_iterator it = m_idContainerMap.constFind(uid); + if (it == m_idContainerMap.constEnd()) { - if (warnAboutFindFailures) + if (warnAboutFindFailures) { qWarning() << "ActionManagerPrivate::actionContainer(): failed to find :" << id << '/' << uid; + } return 0; } return it.value(); @@ -437,9 +454,11 @@ ActionContainer *ActionManagerPrivate::actionContainer(const QString &id) const Command *ActionManagerPrivate::command(int uid) const { const IdCmdMap::const_iterator it = m_idCmdMap.constFind(uid); + if (it == m_idCmdMap.constEnd()) { - if (warnAboutFindFailures) - qWarning() << "ActionManagerPrivate::command(): failed to find :" << UniqueIDManager::instance()->stringForUniqueIdentifier(uid) << '/' << uid; + if (warnAboutFindFailures) { + qWarning() << "ActionManagerPrivate::command(): failed to find :" << UniqueIDManager::instance()->stringForUniqueIdentifier(uid) << '/' << uid; + } return 0; } return it.value(); @@ -448,9 +467,11 @@ Command *ActionManagerPrivate::command(int uid) const ActionContainer *ActionManagerPrivate::actionContainer(int uid) const { const IdContainerMap::const_iterator it = m_idContainerMap.constFind(uid); + if (it == m_idContainerMap.constEnd()) { - if (warnAboutFindFailures) + if (warnAboutFindFailures) { qWarning() << "ActionManagerPrivate::actionContainer(): failed to find :" << UniqueIDManager::instance()->stringForUniqueIdentifier(uid) << uid; + } return 0; } return it.value(); @@ -458,20 +479,22 @@ ActionContainer *ActionManagerPrivate::actionContainer(int uid) const static const char *settingsGroup = "KeyBindings"; static const char *idKey = "ID"; -static const char *sequenceKey = "Keysequence"; +static const char *sequenceKey = "Keysequence"; void ActionManagerPrivate::readSettings(QSettings *settings) { const int shortcuts = settings->beginReadArray(QLatin1String(settingsGroup)); - for (int i=0; isetArrayIndex(i); const QString sid = settings->value(QLatin1String(idKey)).toString(); const QKeySequence key(settings->value(QLatin1String(sequenceKey)).toString()); const int id = UniqueIDManager::instance()->uniqueIdentifier(sid); Command *cmd = command(id); - if (cmd) + if (cmd) { cmd->setKeySequence(key); + } } settings->endArray(); } @@ -483,9 +506,9 @@ void ActionManagerPrivate::saveSettings(QSettings *settings) const IdCmdMap::const_iterator cmdcend = m_idCmdMap.constEnd(); for (IdCmdMap::const_iterator j = m_idCmdMap.constBegin(); j != cmdcend; ++j) { - const int id = j.key(); + const int id = j.key(); CommandPrivate *cmd = j.value(); - QKeySequence key = cmd->keySequence(); + QKeySequence key = cmd->keySequence(); if (key != cmd->defaultKeySequence()) { const QString sid = UniqueIDManager::instance()->stringForUniqueIdentifier(id); settings->setArrayIndex(count); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.h index eae782693..57350188d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,15 +44,13 @@ class QString; QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT ActionManager : public QObject -{ +class CORE_EXPORT ActionManager : public QObject { Q_OBJECT public: ActionManager(QObject *parent = 0) : QObject(parent) {} virtual ~ActionManager() {} - virtual ActionContainer *createMenu(const QString &id) = 0; + virtual ActionContainer *createMenu(const QString &id) = 0; virtual ActionContainer *createMenuBar(const QString &id) = 0; virtual Command *registerAction(QAction *action, const QString &id, const QList &context) = 0; @@ -61,7 +59,6 @@ public: virtual Command *command(const QString &id) const = 0; virtual ActionContainer *actionContainer(const QString &id) const = 0; }; - } // namespace Core #endif // ACTIONMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager_p.h b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager_p.h index 6573f7031..2c2f440bf 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager_p.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/actionmanager_p.h @@ -10,19 +10,19 @@ * @{ * @brief The Core GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,24 +39,20 @@ QT_BEGIN_NAMESPACE class QSettings; QT_END_NAMESPACE -struct CommandLocation -{ +struct CommandLocation { int m_container; int m_position; }; namespace Core { - class UniqueIDManager; namespace Internal { - class ActionContainerPrivate; class MainWindow; class CommandPrivate; -class ActionManagerPrivate : public Core::ActionManager -{ +class ActionManagerPrivate : public Core::ActionManager { Q_OBJECT public: @@ -79,14 +75,14 @@ public: void readSettings(QSettings *settings); - //ActionManager Interface + // ActionManager Interface ActionContainer *createMenu(const QString &id); ActionContainer *createMenuBar(const QString &id); Command *registerAction(QAction *action, const QString &id, - const QList &context); + const QList &context); Command *registerShortcut(QShortcut *shortcut, const QString &id, - const QList &context); + const QList &context); Core::Command *command(const QString &id) const; Core::ActionContainer *actionContainer(const QString &id) const; @@ -94,9 +90,9 @@ public: private: bool hasContext(QList context) const; Command *registerOverridableAction(QAction *action, const QString &id, - bool checkUnique); + bool checkUnique); - static ActionManagerPrivate* m_instance; + static ActionManagerPrivate *m_instance; QList m_defaultGroups; typedef QHash IdCmdMap; @@ -105,14 +101,13 @@ private: typedef QHash IdContainerMap; IdContainerMap m_idContainerMap; -// typedef QMap GlobalGroupMap; -// GlobalGroupMap m_globalgroups; +// typedef QMap GlobalGroupMap; +// GlobalGroupMap m_globalgroups; // QList m_context; MainWindow *m_mainWnd; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.cpp b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.cpp index 9f0c84c2d..de5379423 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -56,7 +56,7 @@ If there is no active action, the default behavior of the visible action is to be disabled. You can change that behavior to make the visible action hide instead via the Command's \l{Command::CommandAttribute}{attributes}. -*/ + */ /*! \enum Command::CommandAttribute @@ -74,37 +74,37 @@ \value CA_NonConfigureable Flag to indicate that the keyboard shortcut of this Command should not be configurable by the user. -*/ + */ /*! \fn void Command::setDefaultKeySequence(const QKeySequence &key) Set the default keyboard shortcut that can be used to activate this command to \a key. This is used if the user didn't customize the shortcut, or resets the shortcut to the default one. -*/ + */ /*! \fn void Command::defaultKeySequence() const Returns the default keyboard shortcut that can be used to activate this command. \sa setDefaultKeySequence() -*/ + */ /*! \fn void Command::keySequenceChanged() Sent when the keyboard shortcut assigned to this Command changes, e.g. when the user sets it in the keyboard shortcut settings dialog. -*/ + */ /*! \fn QKeySequence Command::keySequence() const Returns the current keyboard shortcut assigned to this Command. \sa defaultKeySequence() -*/ + */ /*! \fn void Command::setKeySequence(const QKeySequence &key) \internal -*/ + */ /*! \fn void Command::setDefaultText(const QString &text) @@ -112,24 +112,24 @@ keyboard shortcut settings dialog. If you don't set this, the current text from the user visible action is taken (which is ok in many cases). -*/ + */ /*! \fn QString Command::defaultText() const Returns the text that is used to present this Command to the user. \sa setDefaultText() -*/ + */ /*! \fn int Command::id() const \internal -*/ + */ /*! \fn QString Command::stringWithAppendedShortcut(const QString &string) const Returns the \a string with an appended representation of the keyboard shortcut that is currently assigned to this Command. -*/ + */ /*! \fn QAction *Command::action() const @@ -142,13 +142,13 @@ tool tip (in addition to the tool tip of the active action) and gets disabled/hidden when there is no active action for the current context. -*/ + */ /*! \fn QShortcut *Command::shortcut() const Returns the shortcut for this Command. If the Command represents an action, it returns null. -*/ + */ /*! \fn void Command::setAttribute(CommandAttribute attribute) @@ -156,14 +156,14 @@ \sa CommandAttribute \sa removeAttribute() \sa hasAttribute() -*/ + */ /*! \fn void Command::removeAttribute(CommandAttribute attribute) Remove the \a attribute from the attributes of this Command. \sa CommandAttribute \sa setAttribute() -*/ + */ /*! \fn bool Command::hasAttribute(CommandAttribute attribute) const @@ -171,30 +171,29 @@ \sa CommandAttribute \sa removeAttribute() \sa setAttribute() -*/ + */ /*! \fn bool Command::isActive() const Returns if the Command has an active action/shortcut for the current context. -*/ + */ /*! \fn Command::~Command() \internal -*/ + */ using namespace Core::Internal; /*! \class CommandPrivate \internal -*/ + */ CommandPrivate::CommandPrivate(int id) : m_attributes(0), m_id(id) -{ -} +{} void CommandPrivate::setDefaultKeySequence(const QKeySequence &key) { @@ -243,13 +242,13 @@ void CommandPrivate::removeAttribute(CommandAttribute attr) bool CommandPrivate::hasAttribute(CommandAttribute attr) const { - return (m_attributes & attr); + return m_attributes & attr; } QString CommandPrivate::stringWithAppendedShortcut(const QString &str) const { return QString("%1 %2").arg(str).arg( - keySequence().toString(QKeySequence::NativeText)); + keySequence().toString(QKeySequence::NativeText)); } // ---------- Shortcut ------------ @@ -257,18 +256,17 @@ QString CommandPrivate::stringWithAppendedShortcut(const QString &str) const /*! \class Shortcut \internal -*/ + */ Shortcut::Shortcut(int id) : CommandPrivate(id), m_shortcut(0) -{ - -} +{} QString Shortcut::name() const { - if (!m_shortcut) + if (!m_shortcut) { return QString(); + } return m_shortcut->whatsThis(); } @@ -295,8 +293,9 @@ QList Shortcut::context() const void Shortcut::setDefaultKeySequence(const QKeySequence &key) { - if (m_shortcut->key().isEmpty()) + if (m_shortcut->key().isEmpty()) { setKeySequence(key); + } CommandPrivate::setDefaultKeySequence(key); } @@ -323,7 +322,7 @@ QString Shortcut::defaultText() const bool Shortcut::setCurrentContext(const QList &context) { - foreach (int ctxt, m_context) { + foreach(int ctxt, m_context) { if (context.contains(ctxt)) { m_shortcut->setEnabled(true); return true; @@ -341,19 +340,18 @@ bool Shortcut::isActive() const // ---------- Action ------------ /*! - \class Action - \internal -*/ + \class Action + \internal + */ Action::Action(int id) : CommandPrivate(id), m_action(0) -{ - -} +{} QString Action::name() const { - if (!m_action) + if (!m_action) { return QString(); + } return m_action->text(); } @@ -384,8 +382,9 @@ QList Action::locations() const void Action::setDefaultKeySequence(const QKeySequence &key) { - if (m_action->shortcut().isEmpty()) + if (m_action->shortcut().isEmpty()) { setKeySequence(key); + } CommandPrivate::setDefaultKeySequence(key); } @@ -398,10 +397,11 @@ void Action::setKeySequence(const QKeySequence &key) void Action::updateToolTipWithKeySequence() { - if (m_action->shortcut().isEmpty()) + if (m_action->shortcut().isEmpty()) { m_action->setToolTip(m_toolTip); - else + } else { m_action->setToolTip(stringWithAppendedShortcut(m_toolTip)); + } } QKeySequence Action::keySequence() const @@ -414,13 +414,12 @@ QKeySequence Action::keySequence() const /*! \class OverrideableAction \internal -*/ + */ OverrideableAction::OverrideableAction(int id) : Action(id), m_currentAction(0), m_active(false), m_contextInitialized(false) -{ -} +{} void OverrideableAction::setAction(QAction *action) { @@ -434,14 +433,15 @@ bool OverrideableAction::setCurrentContext(const QList &context) QAction *oldAction = m_currentAction; m_currentAction = 0; for (int i = 0; i < m_context.size(); ++i) { - if (QAction *a = m_contextActionMap.value(m_context.at(i), 0)) { + if (QAction * a = m_contextActionMap.value(m_context.at(i), 0)) { m_currentAction = a; break; } } - if (m_currentAction == oldAction && m_contextInitialized) + if (m_currentAction == oldAction && m_contextInitialized) { return true; + } m_contextInitialized = true; if (oldAction) { @@ -459,8 +459,9 @@ bool OverrideableAction::setCurrentContext(const QList &context) m_active = true; return true; } - if (hasAttribute(CA_Hide)) + if (hasAttribute(CA_Hide)) { m_action->setVisible(false); + } m_action->setEnabled(false); m_active = false; return false; @@ -471,10 +472,11 @@ void OverrideableAction::addOverrideAction(QAction *action, const QList &co if (context.isEmpty()) { m_contextActionMap.insert(0, action); } else { - for (int i=0; itext()); + } m_contextActionMap.insert(k, action); } } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.h b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.h index d83110127..6f4b394bc 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,32 +41,30 @@ QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT Command : public QObject -{ +class CORE_EXPORT Command : public QObject { Q_OBJECT public: enum CommandAttribute { - CA_Hide = 0x0100, - CA_UpdateText = 0x0200, - CA_UpdateIcon = 0x0400, + CA_Hide = 0x0100, + CA_UpdateText = 0x0200, + CA_UpdateIcon = 0x0400, CA_NonConfigureable = 0x8000, - CA_Mask = 0xFF00 + CA_Mask = 0xFF00 }; virtual void setDefaultKeySequence(const QKeySequence &key) = 0; - virtual QKeySequence defaultKeySequence() const = 0; + virtual QKeySequence defaultKeySequence() const = 0; virtual QKeySequence keySequence() const = 0; virtual void setDefaultText(const QString &text) = 0; virtual QString defaultText() const = 0; virtual int id() const = 0; - virtual QAction *action() const = 0; + virtual QAction *action() const = 0; virtual QShortcut *shortcut() const = 0; - virtual void setAttribute(CommandAttribute attr) = 0; - virtual void removeAttribute(CommandAttribute attr) = 0; + virtual void setAttribute(CommandAttribute attr) = 0; + virtual void removeAttribute(CommandAttribute attr) = 0; virtual bool hasAttribute(CommandAttribute attr) const = 0; virtual bool isActive() const = 0; @@ -80,7 +78,6 @@ public: signals: void keySequenceChanged(); }; - } // namespace Core #endif // COMMAND_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command_p.h b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command_p.h index ce024768f..a2a80fae8 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command_p.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/command_p.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,9 +39,7 @@ namespace Core { namespace Internal { - -class CommandPrivate : public Core::Command -{ +class CommandPrivate : public Core::Command { Q_OBJECT public: CommandPrivate(int id); @@ -76,8 +74,7 @@ protected: QString m_defaultText; }; -class Shortcut : public CommandPrivate -{ +class Shortcut : public CommandPrivate { Q_OBJECT public: Shortcut(int id); @@ -105,8 +102,7 @@ private: QString m_defaultText; }; -class Action : public CommandPrivate -{ +class Action : public CommandPrivate { Q_OBJECT public: Action(int id); @@ -125,14 +121,13 @@ public: protected: void updateToolTipWithKeySequence(); - + QAction *m_action; QList m_locations; QString m_toolTip; }; -class OverrideableAction : public Action -{ +class OverrideableAction : public Action { Q_OBJECT public: @@ -153,7 +148,6 @@ private: bool m_active; bool m_contextInitialized; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.cpp b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.cpp index bc7840b01..9554a817b 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,35 +42,36 @@ using namespace Core::Internal; \class CommandsFile \brief The CommandsFile class provides a collection of import and export commands. \inheaderfile commandsfile.h -*/ + */ /*! ... -*/ + */ CommandsFile::CommandsFile(const QString &filename) : m_filename(filename) -{ - -} +{} /*! ... -*/ + */ QMap CommandsFile::importCommands() const { QMap result; QFile file(m_filename); - if (!file.open(QIODevice::ReadOnly)) + if (!file.open(QIODevice::ReadOnly)) { return result; + } QDomDocument doc("KeyboardMappingScheme"); - if (!doc.setContent(&file)) + if (!doc.setContent(&file)) { return result; + } QDomElement root = doc.documentElement(); - if (root.nodeName() != QLatin1String("mapping")) + if (root.nodeName() != QLatin1String("mapping")) { return result; + } QDomElement ks = root.firstChildElement(); for (; !ks.isNull(); ks = ks.nextSiblingElement()) { @@ -78,8 +79,9 @@ QMap CommandsFile::importCommands() const QString id = ks.attribute(QLatin1String("id")); QKeySequence shortcutkey; QDomElement keyelem = ks.firstChildElement("key"); - if (!keyelem.isNull()) + if (!keyelem.isNull()) { shortcutkey = QKeySequence(keyelem.attribute("value")); + } result.insert(id, shortcutkey); } } @@ -90,21 +92,24 @@ QMap CommandsFile::importCommands() const /*! ... -*/ + */ bool CommandsFile::exportCommands(const QList &items) { UniqueIDManager *idmanager = UniqueIDManager::instance(); QFile file(m_filename); - if (!file.open(QIODevice::WriteOnly)) + + if (!file.open(QIODevice::WriteOnly)) { return false; + } QDomDocument doc("KeyboardMappingScheme"); QDomElement root = doc.createElement("mapping"); doc.appendChild(root); - foreach (const ShortcutItem *item, items) { + foreach(const ShortcutItem * item, items) { QDomElement ctag = doc.createElement("shortcut"); + ctag.setAttribute(QLatin1String("id"), idmanager->stringForUniqueIdentifier(item->m_cmd->id())); root.appendChild(ctag); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.h b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.h index bdfeccac2..8ca75b071 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/actionmanager/commandsfile.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,11 +37,9 @@ namespace Core { namespace Internal { - struct ShortcutItem; - -class CommandsFile : public QObject -{ + +class CommandsFile : public QObject { Q_OBJECT public: @@ -53,8 +51,7 @@ public: private: QString m_filename; }; - } // namespace Internal } // namespace Core -#endif //COMMANDSFILE_H +#endif // COMMANDSFILE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.cpp b/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.cpp index 8bf76b4ea..7b7c12935 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,7 +43,7 @@ #include #include #include - + #include #include #include @@ -62,71 +62,69 @@ AuthorsDialog::AuthorsDialog(QWidget *parent) setWindowIcon(QIcon(":/core/images/openpilot_logo_32.png")); setWindowTitle(tr("About OpenPilot")); setWindowFlags(windowFlags() & ~Qt::WindowContextHelpButtonHint); - // This loads a QML doc containing a Tabbed view - QDeclarativeView *view = new QDeclarativeView(this); - view->setSource(QUrl("qrc:/core/qml/AboutDialog.qml")); + // This loads a QML doc containing a Tabbed view + QDeclarativeView *view = new QDeclarativeView(this); + view->setSource(QUrl("qrc:/core/qml/AboutDialog.qml")); + QString version = QLatin1String(GCS_VERSION_LONG); + version += QDate(2007, 25, 10).toString(Qt::SystemLocaleDate); - QString version = QLatin1String(GCS_VERSION_LONG); - version += QDate(2007, 25, 10).toString(Qt::SystemLocaleDate); - - QString ideRev; + QString ideRev; #ifdef GCS_REVISION - //: This gets conditionally inserted as argument %8 into the description string. - ideRev = tr("From revision %1
").arg(QString::fromLatin1(GCS_REVISION_STR).left(10)); + // : This gets conditionally inserted as argument %8 into the description string. + ideRev = tr("From revision %1
").arg(QString::fromLatin1(GCS_REVISION_STR).left(10)); #endif #ifdef UAVO_HASH - //: This gets conditionally inserted as argument %11 into the description string. - QByteArray uavoHashArray; - QString uavoHash = QString::fromLatin1(Core::Constants::UAVOSHA1_STR); - uavoHash.chop(2); - uavoHash.remove(0, 2); - uavoHash = uavoHash.trimmed(); - bool ok; - foreach(QString str, uavoHash.split(",")) { - uavoHashArray.append(str.toInt(&ok, 16)); - } - QString gcsUavoHashStr; - foreach(char i, uavoHashArray) { - gcsUavoHashStr.append(QString::number(i, 16).right(2)); - } - QString uavoHashStr = gcsUavoHashStr; + // : This gets conditionally inserted as argument %11 into the description string. + QByteArray uavoHashArray; + QString uavoHash = QString::fromLatin1(Core::Constants::UAVOSHA1_STR); + uavoHash.chop(2); + uavoHash.remove(0, 2); + uavoHash = uavoHash.trimmed(); + bool ok; + foreach(QString str, uavoHash.split(",")) { + uavoHashArray.append(str.toInt(&ok, 16)); + } + QString gcsUavoHashStr; + foreach(char i, uavoHashArray) { + gcsUavoHashStr.append(QString::number(i, 16).right(2)); + } + QString uavoHashStr = gcsUavoHashStr; #else - QString uavoHashStr = "N/A"; + QString uavoHashStr = "N/A"; #endif - const QString description = tr( - "

OpenPilot Ground Control Station

" - "GCS Revision: %1
" - "UAVO Hash: %2
" - "
" - "Built from %3
" - "Built on %4 at %5
" - "Based on Qt %6 (%7 bit)
" - "
" - "© %8, 2010-%9. All rights reserved.
" - "
" - "This program is free software; you can redistribute it and/or modify
" - "it under the terms of the GNU General Public License as published by
" - "the Free Software Foundation; either version 3 of the License, or
" - "(at your option) any later version.
" - "
" - "The program is provided AS IS with NO WARRANTY OF ANY KIND, " - "INCLUDING THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A " - "PARTICULAR PURPOSE.
" - ).arg( - QString::fromLatin1(GCS_REVISION_STR).left(60), // %1 - uavoHashStr, // %2 - QLatin1String(GCS_ORIGIN_STR), // $3 - QLatin1String(__DATE__), // %4 - QLatin1String(__TIME__), // %5 - QLatin1String(QT_VERSION_STR), // %6 - QString::number(QSysInfo::WordSize), // %7 - QLatin1String(GCS_AUTHOR), // %8 - QLatin1String(GCS_YEAR_STR) // %9 - ); - // Expose the version description to the QML doc - view->rootContext()->setContextProperty("version", description); - + const QString description = tr( + "

OpenPilot Ground Control Station

" + "GCS Revision: %1
" + "UAVO Hash: %2
" + "
" + "Built from %3
" + "Built on %4 at %5
" + "Based on Qt %6 (%7 bit)
" + "
" + "© %8, 2010-%9. All rights reserved.
" + "
" + "This program is free software; you can redistribute it and/or modify
" + "it under the terms of the GNU General Public License as published by
" + "the Free Software Foundation; either version 3 of the License, or
" + "(at your option) any later version.
" + "
" + "The program is provided AS IS with NO WARRANTY OF ANY KIND, " + "INCLUDING THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A " + "PARTICULAR PURPOSE.
" + ).arg( + QString::fromLatin1(GCS_REVISION_STR).left(60), // %1 + uavoHashStr, // %2 + QLatin1String(GCS_ORIGIN_STR), // $3 + QLatin1String(__DATE__), // %4 + QLatin1String(__TIME__), // %5 + QLatin1String(QT_VERSION_STR), // %6 + QString::number(QSysInfo::WordSize), // %7 + QLatin1String(GCS_AUTHOR), // %8 + QLatin1String(GCS_YEAR_STR) // %9 + ); + // Expose the version description to the QML doc + view->rootContext()->setContextProperty("version", description); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.h b/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.h index f1521bf14..fadac688e 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/authorsdialog.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,14 +33,11 @@ namespace Core { namespace Internal { - -class AuthorsDialog : public QDialog -{ +class AuthorsDialog : public QDialog { Q_OBJECT public: explicit AuthorsDialog(QWidget *parent); }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/basemode.cpp b/ground/openpilotgcs/src/plugins/coreplugin/basemode.cpp index f50e4dfdd..63d84a679 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/basemode.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/basemode.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -59,7 +59,7 @@ using namespace Core; [...] } \endcode -*/ + */ /*! \fn BaseMode::BaseMode(QObject *parent) @@ -68,17 +68,16 @@ using namespace Core; setter functions to give the mode a meaning. \a parent -*/ -BaseMode::BaseMode(QObject *parent): + */ +BaseMode::BaseMode(QObject *parent) : IMode(parent), m_priority(0), m_widget(0) -{ -} +{} /*! \fn BaseMode::~BaseMode() -*/ + */ BaseMode::~BaseMode() { delete m_widget; diff --git a/ground/openpilotgcs/src/plugins/coreplugin/basemode.h b/ground/openpilotgcs/src/plugins/coreplugin/basemode.h index 786c97bed..c2204c7c5 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/basemode.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/basemode.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,10 +37,8 @@ #include namespace Core { - class CORE_EXPORT BaseMode - : public IMode -{ + : public IMode { Q_OBJECT public: @@ -48,29 +46,64 @@ public: ~BaseMode(); // IMode - QString name() const { return m_name; } - QIcon icon() const { return m_icon; } - int priority() const { return m_priority; } - QWidget *widget() { return m_widget; } - const char *uniqueModeName() const { return m_uniqueModeName; } - QList context() const { return m_context; } + QString name() const + { + return m_name; + } + QIcon icon() const + { + return m_icon; + } + int priority() const + { + return m_priority; + } + QWidget *widget() + { + return m_widget; + } + const char *uniqueModeName() const + { + return m_uniqueModeName; + } + QList context() const + { + return m_context; + } - void setName(const QString &name) { m_name = name; } - void setIcon(const QIcon &icon) { m_icon = icon; } - void setPriority(int priority) { m_priority = priority; } - void setWidget(QWidget *widget) { m_widget = widget; } - void setUniqueModeName(const char *uniqueModeName) { m_uniqueModeName = uniqueModeName; } - void setContext(const QList &context) { m_context = context; } + void setName(const QString &name) + { + m_name = name; + } + void setIcon(const QIcon &icon) + { + m_icon = icon; + } + void setPriority(int priority) + { + m_priority = priority; + } + void setWidget(QWidget *widget) + { + m_widget = widget; + } + void setUniqueModeName(const char *uniqueModeName) + { + m_uniqueModeName = uniqueModeName; + } + void setContext(const QList &context) + { + m_context = context; + } private: QString m_name; QIcon m_icon; int m_priority; QWidget *m_widget; - const char * m_uniqueModeName; + const char *m_uniqueModeName; QList m_context; }; - } // namespace Core #endif // BASEMODE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/baseview.cpp b/ground/openpilotgcs/src/plugins/coreplugin/baseview.cpp index 0a87a9476..f659188d5 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/baseview.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/baseview.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,13 +33,12 @@ using namespace Core; BaseView::BaseView(QObject *parent) - : IView(parent), + : IView(parent), m_viewName(""), m_widget(0), m_context(QList()), m_defaultPosition(IView::First) -{ -} +{} BaseView::~BaseView() { @@ -75,6 +74,7 @@ void BaseView::setUniqueViewName(const char *name) QWidget *BaseView::setWidget(QWidget *widget) { QWidget *oldWidget = m_widget; + m_widget = widget; return oldWidget; } @@ -88,4 +88,3 @@ void BaseView::setDefaultPosition(IView::ViewPosition position) { m_defaultPosition = position; } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/baseview.h b/ground/openpilotgcs/src/plugins/coreplugin/baseview.h index 8f54f3c0f..2bbef0a8d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/baseview.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/baseview.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ #include namespace Core { - -class CORE_EXPORT BaseView : public IView -{ +class CORE_EXPORT BaseView : public IView { Q_OBJECT public: @@ -59,7 +57,6 @@ private: QList m_context; IView::ViewPosition m_defaultPosition; }; - } // namespace Core #endif // BASEVIEW_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp index 2580c2dab..db684e4c1 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp @@ -40,7 +40,6 @@ #include namespace Core { - ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow, QTabWidget *modeStack) : QWidget(mainWindow), m_availableDevList(0), @@ -50,7 +49,7 @@ ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow, QTabWidge m_mainWindow(mainWindow) { // monitor widget - m_monitorWidget = new TelemetryMonitorWidget(this); + m_monitorWidget = new TelemetryMonitorWidget(this); // device list m_availableDevList = new QComboBox; @@ -90,31 +89,35 @@ ConnectionManager::~ConnectionManager() { disconnectDevice(); suspendPolling(); - if (m_monitorWidget) + if (m_monitorWidget) { delete m_monitorWidget; + } } void ConnectionManager::init() { - //register to the plugin manager so we can receive - //new connection object from plugins - QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(objectAdded(QObject*)), this, SLOT(objectAdded(QObject*))); - QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(aboutToRemoveObject(QObject*)), this, SLOT(aboutToRemoveObject(QObject*))); + // register to the plugin manager so we can receive + // new connection object from plugins + QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(objectAdded(QObject *)), this, SLOT(objectAdded(QObject *))); + QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(aboutToRemoveObject(QObject *)), this, SLOT(aboutToRemoveObject(QObject *))); } /** -* Method called when the user clicks the "Connect" button -*/ + * Method called when the user clicks the "Connect" button + */ bool ConnectionManager::connectDevice(DevListItem device) { QString deviceName = m_availableDevList->itemData(m_availableDevList->currentIndex(), Qt::ToolTipRole).toString(); DevListItem connection_device = findDevice(deviceName); - if (!connection_device.connection) + + if (!connection_device.connection) { return false; + } QIODevice *io_dev = connection_device.connection->openDevice(connection_device.device.name); - if (!io_dev) + if (!io_dev) { return false; + } io_dev->open(QIODevice::ReadWrite); @@ -142,9 +145,9 @@ bool ConnectionManager::connectDevice(DevListItem device) } /** -* Method called by plugins who want to force a disconnection. -* Used by Uploader gadget for instance. -*/ + * Method called by plugins who want to force a disconnection. + * Used by Uploader gadget for instance. + */ bool ConnectionManager::disconnectDevice() { // tell the monitor widget we're disconnected @@ -160,10 +163,12 @@ bool ConnectionManager::disconnectDevice() // We are connected - disconnect from the device // stop our timers - if(reconnect->isActive()) + if (reconnect->isActive()) { reconnect->stop(); - if(reconnectCheck->isActive()) + } + if (reconnectCheck->isActive()) { reconnectCheck->stop(); + } // signal interested plugins that user is disconnecting his device emit deviceAboutToDisconnect(); @@ -172,7 +177,7 @@ bool ConnectionManager::disconnectDevice() if (m_connectionDevice.connection) { m_connectionDevice.connection->closeDevice(m_connectionDevice.getConName()); } - } catch (...) { // handle exception + } catch(...) { // handle exception qDebug() << "Exception: m_connectionDevice.connection->closeDevice(" << m_connectionDevice.getConName() << ")"; } @@ -187,15 +192,18 @@ bool ConnectionManager::disconnectDevice() } /** -* Slot called when a plugin added an object to the core pool -*/ + * Slot called when a plugin added an object to the core pool + */ void ConnectionManager::objectAdded(QObject *obj) { - //Check if a plugin added a connection object to the pool + // Check if a plugin added a connection object to the pool IConnection *connection = Aggregation::query(obj); - if (!connection) return; - //register devices and populate CB + if (!connection) { + return; + } + + // register devices and populate CB devChanged(connection); // Keep track of the registration to be able to tell plugins @@ -207,19 +215,22 @@ void ConnectionManager::objectAdded(QObject *obj) void ConnectionManager::aboutToRemoveObject(QObject *obj) { - //Check if a plugin added a connection object to the pool + // Check if a plugin added a connection object to the pool IConnection *connection = Aggregation::query(obj); - if (!connection) return; - if (m_connectionDevice.connection && m_connectionDevice.connection == connection) - { // we are currently using the one that is about to be removed + if (!connection) { + return; + } + + if (m_connectionDevice.connection && m_connectionDevice.connection == connection) { // we are currently using the one that is about to be removed disconnectDevice(); m_connectionDevice.connection = NULL; m_ioDev = NULL; } - if (m_connectionsList.contains(connection)) + if (m_connectionsList.contains(connection)) { m_connectionsList.removeAt(m_connectionsList.indexOf(connection)); + } } @@ -230,8 +241,8 @@ void ConnectionManager::onConnectionDestroyed(QObject *obj) } /** -* Slot called when the user selects a device from the combo box -*/ + * Slot called when the user selects a device from the combo box + */ void ConnectionManager::onDeviceSelectionChanged(int index) { if (index >= 0) { @@ -241,8 +252,8 @@ void ConnectionManager::onDeviceSelectionChanged(int index) } /** -* Slot called when the user clicks the connect/disconnect button -*/ + * Slot called when the user clicks the connect/disconnect button + */ void ConnectionManager::onConnectClicked() { // Check if we have a ioDev already created: @@ -253,48 +264,49 @@ void ConnectionManager::onConnectClicked() if (device.connection) { connectDevice(device); } - } - else { + } else { // disconnecting disconnectDevice(); } } /** -* Slot called when the telemetry is connected -*/ + * Slot called when the telemetry is connected + */ void ConnectionManager::telemetryConnected() { qDebug() << "TelemetryMonitor: connected"; - if(reconnectCheck->isActive()) + if (reconnectCheck->isActive()) { reconnectCheck->stop(); + } - //tell the monitor we're connected + // tell the monitor we're connected m_monitorWidget->connect(); } /** -* Slot called when the telemetry is disconnected -*/ + * Slot called when the telemetry is disconnected + */ void ConnectionManager::telemetryDisconnected() { qDebug() << "TelemetryMonitor: disconnected"; - if (m_ioDev){ - if(m_connectionDevice.connection->shortName()=="Serial") { - if(!reconnect->isActive()) + if (m_ioDev) { + if (m_connectionDevice.connection->shortName() == "Serial") { + if (!reconnect->isActive()) { reconnect->start(1000); + } } } - //tell the monitor we're disconnected + // tell the monitor we're disconnected m_monitorWidget->disconnect(); } /** -* Slot called when the telemetry rates are updated -*/ + * Slot called when the telemetry rates are updated + */ void ConnectionManager::telemetryUpdated(double txRate, double rxRate) { m_monitorWidget->updateTelemetry(txRate, rxRate); @@ -302,17 +314,18 @@ void ConnectionManager::telemetryUpdated(double txRate, double rxRate) void ConnectionManager::reconnectSlot() { - qDebug()<<"reconnect"; - if(m_ioDev->isOpen()) + qDebug() << "reconnect"; + if (m_ioDev->isOpen()) { m_ioDev->close(); + } - if(m_ioDev->open(QIODevice::ReadWrite)) { - qDebug()<<"reconnect successfull"; + if (m_ioDev->open(QIODevice::ReadWrite)) { + qDebug() << "reconnect successfull"; reconnect->stop(); reconnectCheck->start(20000); + } else { + qDebug() << "reconnect NOT successfull"; } - else - qDebug()<<"reconnect NOT successfull"; } void ConnectionManager::reconnectCheckSlot() @@ -322,14 +335,14 @@ void ConnectionManager::reconnectCheckSlot() } /** -* Find a device by its displayed (visible on screen) name -*/ + * Find a device by its displayed (visible on screen) name + */ DevListItem ConnectionManager::findDevice(const QString &devName) { - foreach (DevListItem d, m_devList) - { - if (d.getConName() == devName) + foreach(DevListItem d, m_devList) { + if (d.getConName() == devName) { return d; + } } qDebug() << "findDevice: cannot find " << devName << " in device list"; @@ -340,14 +353,13 @@ DevListItem ConnectionManager::findDevice(const QString &devName) } /** -* Tells every connection plugin to stop polling for devices if they -* are doing that. -* -*/ + * Tells every connection plugin to stop polling for devices if they + * are doing that. + * + */ void ConnectionManager::suspendPolling() { - foreach (IConnection *cnx, m_connectionsList) - { + foreach(IConnection * cnx, m_connectionsList) { cnx->suspendPolling(); } @@ -357,13 +369,12 @@ void ConnectionManager::suspendPolling() } /** -* Tells every connection plugin to resume polling for devices if they -* are doing that. -*/ + * Tells every connection plugin to resume polling for devices if they + * are doing that. + */ void ConnectionManager::resumePolling() { - foreach (IConnection *cnx, m_connectionsList) - { + foreach(IConnection * cnx, m_connectionsList) { cnx->resumePolling(); } @@ -385,8 +396,7 @@ void ConnectionManager::updateConnectionList(IConnection *connection) // Go through the list of connections of that type. If they are not in the // available device list then remove them. If they are connected, then // disconnect them. - for (QLinkedList::iterator iter = m_devList.begin(); iter != m_devList.end(); ) - { + for (QLinkedList::iterator iter = m_devList.begin(); iter != m_devList.end();) { if (iter->connection != connection) { ++iter; continue; @@ -401,49 +411,50 @@ void ConnectionManager::updateConnectionList(IConnection *connection) } iter = m_devList.erase(iter); - } else + } else { ++iter; + } } // Add any back to list that don't exist - foreach (IConnection::device dev, availableDev) - { + foreach(IConnection::device dev, availableDev) { bool found = m_devList.contains(DevListItem(connection, dev)); + if (!found) { - registerDevice(connection,dev); + registerDevice(connection, dev); } } } /** -* Register a device from a specific connection plugin -* @param devN contains the connection shortname + device name which is diplayed in the tooltip -* @param disp is the name that is displayed in the dropdown menu -* @param name is the actual device name -*/ + * Register a device from a specific connection plugin + * @param devN contains the connection shortname + device name which is diplayed in the tooltip + * @param disp is the name that is displayed in the dropdown menu + * @param name is the actual device name + */ void ConnectionManager::registerDevice(IConnection *conn, IConnection::device device) { - DevListItem d(conn,device); + DevListItem d(conn, device); + m_devList.append(d); } /** -* A device plugin notified us that its device list has changed -* (eg: user plug/unplug a usb device) -* \param[in] connection Connection type which signaled the update -*/ + * A device plugin notified us that its device list has changed + * (eg: user plug/unplug a usb device) + * \param[in] connection Connection type which signaled the update + */ void ConnectionManager::devChanged(IConnection *connection) { - if(!ExtensionSystem::PluginManager::instance()->allPluginsLoaded()) - { + if (!ExtensionSystem::PluginManager::instance()->allPluginsLoaded()) { connectionBackup.append(connection); - connect(ExtensionSystem::PluginManager::instance(),SIGNAL(pluginsLoadEnded()),this,SLOT(connectionsCallBack()),Qt::UniqueConnection); + connect(ExtensionSystem::PluginManager::instance(), SIGNAL(pluginsLoadEnded()), this, SLOT(connectionsCallBack()), Qt::UniqueConnection); return; } - //clear device list combobox + // clear device list combobox m_availableDevList->clear(); - //remove registered devices of this IConnection from the list + // remove registered devices of this IConnection from the list updateConnectionList(connection); updateConnectionDropdown(); @@ -452,17 +463,18 @@ void ConnectionManager::devChanged(IConnection *connection) emit availableDevicesChanged(m_devList); - //disable connection button if the liNameif (m_availableDevList->count() > 0) - if (m_availableDevList->count() > 0) + // disable connection button if the liNameif (m_availableDevList->count() > 0) + if (m_availableDevList->count() > 0) { m_connectBtn->setEnabled(true); - else + } else { m_connectBtn->setEnabled(false); + } } void ConnectionManager::updateConnectionDropdown() { // add all the list again to the combobox - foreach (DevListItem d, m_devList) { + foreach(DevListItem d, m_devList) { m_availableDevList->addItem(d.getConName()); m_availableDevList->setItemData(m_availableDevList->count() - 1, d.getConName(), Qt::ToolTipRole); if (!m_ioDev && d.getConName().startsWith("USB")) { @@ -491,12 +503,10 @@ void ConnectionManager::updateConnectionDropdown() void Core::ConnectionManager::connectionsCallBack() { - foreach(IConnection * con,connectionBackup) - { + foreach(IConnection * con, connectionBackup) { devChanged(con); } connectionBackup.clear(); - disconnect(ExtensionSystem::PluginManager::instance(),SIGNAL(pluginsLoadEnded()),this,SLOT(connectionsCallBack())); + disconnect(ExtensionSystem::PluginManager::instance(), SIGNAL(pluginsLoadEnded()), this, SLOT(connectionsCallBack())); } - -} //namespace Core +} // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h index 8a8f49eff..71eee425f 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h @@ -48,31 +48,32 @@ class QTabWidget; QT_END_NAMESPACE namespace Core { - - class IConnection; +class IConnection; namespace Internal { - class FancyTabWidget; - class FancyActionBar; - class MainWindow; +class FancyTabWidget; +class FancyActionBar; +class MainWindow; } // namespace Internal -class DevListItem -{ +class DevListItem { public: DevListItem(IConnection *c, IConnection::device d) : - connection(c), device(d) { } + connection(c), device(d) {} - DevListItem() : connection(NULL) { } + DevListItem() : connection(NULL) {} - QString getConName() { - if (connection == NULL) + QString getConName() + { + if (connection == NULL) { return ""; + } return connection->shortName() + ": " + device.displayName; } - bool operator==(const DevListItem &rhs) { + bool operator==(const DevListItem &rhs) + { return connection == rhs.connection && device == rhs.device; } @@ -81,8 +82,7 @@ public: }; -class CORE_EXPORT ConnectionManager : public QWidget -{ +class CORE_EXPORT ConnectionManager : public QWidget { Q_OBJECT public: @@ -91,13 +91,25 @@ public: void init(); - QIODevice* getCurrentConnection() { return m_ioDev; } - DevListItem getCurrentDevice() { return m_connectionDevice; } + QIODevice *getCurrentConnection() + { + return m_ioDev; + } + DevListItem getCurrentDevice() + { + return m_connectionDevice; + } DevListItem findDevice(const QString &devName); - QLinkedList getAvailableDevices() { return m_devList; } + QLinkedList getAvailableDevices() + { + return m_devList; + } - bool isConnected() { return m_ioDev != 0; } + bool isConnected() + { + return m_ioDev != 0; + } bool connectDevice(DevListItem device); bool disconnectDevice(); @@ -129,7 +141,7 @@ private slots: void devChanged(IConnection *connection); void onConnectionDestroyed(QObject *obj); - void connectionsCallBack(); //used to call devChange after all the plugins are loaded + void connectionsCallBack(); // used to call devChange after all the plugins are loaded void reconnectSlot(); void reconnectCheckSlot(); @@ -137,27 +149,25 @@ protected: QComboBox *m_availableDevList; QPushButton *m_connectBtn; QLinkedList m_devList; - QList m_connectionsList; + QList m_connectionsList; - //tx/rx telemetry monitor - TelemetryMonitorWidget* m_monitorWidget; + // tx/rx telemetry monitor + TelemetryMonitorWidget *m_monitorWidget; - //currently connected connection plugin + // currently connected connection plugin DevListItem m_connectionDevice; - //currently connected QIODevice + // currently connected QIODevice QIODevice *m_ioDev; private: - bool connectDevice(); + bool connectDevice(); bool polling; Internal::MainWindow *m_mainWindow; QList connectionBackup; QTimer *reconnect; QTimer *reconnectCheck; - }; - -} //namespace Core +} // namespace Core #endif // CONNECTIONMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/core_global.h b/ground/openpilotgcs/src/plugins/coreplugin/core_global.h index 3c9b668f2..2297ec6e4 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/core_global.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/core_global.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreconstants.h b/ground/openpilotgcs/src/plugins/coreplugin/coreconstants.h index 6373fc28b..799c8c2e2 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreconstants.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreconstants.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,218 +31,217 @@ namespace Core { namespace Constants { - -#define GCS_VERSION_MAJOR 1 -#define GCS_VERSION_MINOR 0 +#define GCS_VERSION_MAJOR 1 +#define GCS_VERSION_MINOR 0 #define GCS_VERSION_RELEASE 0 #define STRINGIFY_INTERNAL(x) #x -#define STRINGIFY(x) STRINGIFY_INTERNAL(x) +#define STRINGIFY(x) STRINGIFY_INTERNAL(x) -#define GCS_VERSION STRINGIFY(GCS_VERSION_MAJOR) \ +#define GCS_VERSION \ + STRINGIFY(GCS_VERSION_MAJOR) \ "." STRINGIFY(GCS_VERSION_MINOR) \ "." STRINGIFY(GCS_VERSION_RELEASE) -const char * const GCS_VERSION_LONG = GCS_VERSION; +const char *const GCS_VERSION_LONG = GCS_VERSION; #ifdef GCS_REVISION -const char * const GCS_REVISION_STR = GCS_REVISION; +const char *const GCS_REVISION_STR = GCS_REVISION; #else -const char * const GCS_REVISION_STR = "N/A"; +const char *const GCS_REVISION_STR = "N/A"; #endif #ifdef GCS_YEAR -const char * const GCS_YEAR_STR = GCS_YEAR; +const char *const GCS_YEAR_STR = GCS_YEAR; #else -const char * const GCS_YEAR_STR = "2013"; +const char *const GCS_YEAR_STR = "2013"; #endif #ifdef GCS_ORIGIN -const char * const GCS_ORIGIN_STR = GCS_ORIGIN; +const char *const GCS_ORIGIN_STR = GCS_ORIGIN; #else -const char * const GCS_ORIGIN_STR = "unknown repository"; +const char *const GCS_ORIGIN_STR = "unknown repository"; #endif #ifdef UAVO_HASH -const char * const UAVOSHA1_STR = STRINGIFY(UAVO_HASH); +const char *const UAVOSHA1_STR = STRINGIFY(UAVO_HASH); #else -const char * const UAVOSHA1_STR = ""; +const char *const UAVOSHA1_STR = ""; #endif -const char * const GCS_AUTHOR = "The OpenPilot Project"; -const char * const GCS_HELP = "http://wiki.openpilot.org"; +const char *const GCS_AUTHOR = "The OpenPilot Project"; +const char *const GCS_HELP = "http://wiki.openpilot.org"; #undef GCS_VERSION #undef STRINGIFY #undef STRINGIFY_INTERNAL -//modes -const char * const MODE_WELCOME = "Welcome"; -const char * const MODE_UAVGADGET = "Mode 1"; -const int P_MODE_WELCOME = 100; -const int P_MODE_UAVGADGET = 90; +// modes +const char *const MODE_WELCOME = "Welcome"; +const char *const MODE_UAVGADGET = "Mode 1"; +const int P_MODE_WELCOME = 100; +const int P_MODE_UAVGADGET = 90; -//menubar -const char * const MENU_BAR = "GCS.MenuBar"; +// menubar +const char *const MENU_BAR = "GCS.MenuBar"; -//menus -const char * const M_FILE = "GCS.Menu.File"; -const char * const M_FILE_OPEN = "GCS.Menu.File.Open"; -const char * const M_FILE_NEW = "GCS.Menu.File.New"; -const char * const M_FILE_RECENTFILES = "GCS.Menu.File.RecentFiles"; -const char * const M_EDIT = "GCS.Menu.Edit"; -const char * const M_EDIT_ADVANCED = "GCS.Menu.Edit.Advanced"; -const char * const M_TOOLS = "GCS.Menu.Tools"; -const char * const M_WINDOW = "GCS.Menu.Window"; -const char * const M_WINDOW_PANES = "GCS.Menu.Window.Panes"; -const char * const M_HELP = "GCS.Menu.Help"; +// menus +const char *const M_FILE = "GCS.Menu.File"; +const char *const M_FILE_OPEN = "GCS.Menu.File.Open"; +const char *const M_FILE_NEW = "GCS.Menu.File.New"; +const char *const M_FILE_RECENTFILES = "GCS.Menu.File.RecentFiles"; +const char *const M_EDIT = "GCS.Menu.Edit"; +const char *const M_EDIT_ADVANCED = "GCS.Menu.Edit.Advanced"; +const char *const M_TOOLS = "GCS.Menu.Tools"; +const char *const M_WINDOW = "GCS.Menu.Window"; +const char *const M_WINDOW_PANES = "GCS.Menu.Window.Panes"; +const char *const M_HELP = "GCS.Menu.Help"; -//contexts -const char * const C_GLOBAL = "Global Context"; -const int C_GLOBAL_ID = 0; -const char * const C_WELCOME_MODE = "Core.WelcomeMode"; -const char * const C_UAVGADGET_MODE = "Core.UAVGadgetMode"; -const char * const C_UAVGADGETMANAGER = "Core.UAVGadgetManager"; -const char * const C_NAVIGATION_PANE = "Core.NavigationPane"; -const char * const C_PROBLEM_PANE = "Core.ProblemPane"; +// contexts +const char *const C_GLOBAL = "Global Context"; +const int C_GLOBAL_ID = 0; +const char *const C_WELCOME_MODE = "Core.WelcomeMode"; +const char *const C_UAVGADGET_MODE = "Core.UAVGadgetMode"; +const char *const C_UAVGADGETMANAGER = "Core.UAVGadgetManager"; +const char *const C_NAVIGATION_PANE = "Core.NavigationPane"; +const char *const C_PROBLEM_PANE = "Core.ProblemPane"; -//default editor kind -const char * const K_DEFAULT_TEXT_EDITOR = QT_TRANSLATE_NOOP("OpenWith::Editors", "Plain Text Editor"); -const char * const K_DEFAULT_BINARY_EDITOR = QT_TRANSLATE_NOOP("OpenWith::Editors", "Binary Editor"); +// default editor kind +const char *const K_DEFAULT_TEXT_EDITOR = QT_TRANSLATE_NOOP("OpenWith::Editors", "Plain Text Editor"); +const char *const K_DEFAULT_BINARY_EDITOR = QT_TRANSLATE_NOOP("OpenWith::Editors", "Binary Editor"); -//actions -const char * const UNDO = "GCS.Undo"; -const char * const REDO = "GCS.Redo"; -const char * const COPY = "GCS.Copy"; -const char * const PASTE = "GCS.Paste"; -const char * const CUT = "GCS.Cut"; -const char * const SELECTALL = "GCS.SelectAll"; +// actions +const char *const UNDO = "GCS.Undo"; +const char *const REDO = "GCS.Redo"; +const char *const COPY = "GCS.Copy"; +const char *const PASTE = "GCS.Paste"; +const char *const CUT = "GCS.Cut"; +const char *const SELECTALL = "GCS.SelectAll"; -const char * const NEW = "GCS.New"; -const char * const OPEN = "GCS.Open"; -const char * const OPEN_WITH = "GCS.OpenWith"; -const char * const REVERTTOSAVED = "GCS.RevertToSaved"; -const char * const SAVE = "GCS.Save"; -const char * const SAVEAS = "GCS.SaveAs"; -const char * const SAVEALL = "GCS.SaveAll"; -const char * const EXIT = "GCS.Exit"; +const char *const NEW = "GCS.New"; +const char *const OPEN = "GCS.Open"; +const char *const OPEN_WITH = "GCS.OpenWith"; +const char *const REVERTTOSAVED = "GCS.RevertToSaved"; +const char *const SAVE = "GCS.Save"; +const char *const SAVEAS = "GCS.SaveAs"; +const char *const SAVEALL = "GCS.SaveAll"; +const char *const EXIT = "GCS.Exit"; -const char * const OPTIONS = "GCS.Options"; -const char * const TOGGLE_SIDEBAR = "GCS.ToggleSidebar"; -const char * const TOGGLE_FULLSCREEN = "GCS.ToggleFullScreen"; +const char *const OPTIONS = "GCS.Options"; +const char *const TOGGLE_SIDEBAR = "GCS.ToggleSidebar"; +const char *const TOGGLE_FULLSCREEN = "GCS.ToggleFullScreen"; -const char * const MINIMIZE_WINDOW = "GCS.MinimizeWindow"; -const char * const ZOOM_WINDOW = "GCS.ZoomWindow"; +const char *const MINIMIZE_WINDOW = "GCS.MinimizeWindow"; +const char *const ZOOM_WINDOW = "GCS.ZoomWindow"; -const char * const SPLIT = "GCS.Split"; -const char * const SPLIT_SIDE_BY_SIDE = "GCS.SplitSideBySide"; -const char * const REMOVE_CURRENT_SPLIT = "GCS.RemoveCurrentSplit"; -const char * const REMOVE_ALL_SPLITS = "GCS.RemoveAllSplits"; -const char * const GOTO_OTHER_SPLIT = "GCS.GotoOtherSplit"; -const char * const SAVEASDEFAULT = "GCS.SaveAsDefaultLayout"; -const char * const RESTOREDEFAULT = "GCS.RestoreDefaultLayout"; -const char * const HIDE_TOOLBARS = "GCS.HideToolbars"; -const char * const CLOSE = "GCS.Close"; -const char * const CLOSEALL = "GCS.CloseAll"; -const char * const CLOSEOTHERS = "GCS.CloseOthers"; -const char * const GOTONEXT = "GCS.GotoNext"; -const char * const GOTOPREV = "GCS.GotoPrevious"; -const char * const GOTONEXTINHISTORY = "GCS.GotoNextInHistory"; -const char * const GOTOPREVINHISTORY = "GCS.GotoPreviousInHistory"; -const char * const GO_BACK = "GCS.GoBack"; -const char * const GO_FORWARD = "GCS.GoForward"; -const char * const GOTOPREVIOUSGROUP = "GCS.GotoPreviousTabGroup"; -const char * const GOTONEXTGROUP = "GCS.GotoNextTabGroup"; -const char * const WINDOWSLIST = "GCS.WindowsList"; -const char * const ABOUT_OPENPILOTGCS = "GCS.AboutOpenPilotGCS"; -const char * const ABOUT_PLUGINS = "GCS.AboutPlugins"; -const char * const ABOUT_AUTHORS = "GCS.AboutAuthors"; -const char * const ABOUT_QT = "GCS.AboutQt"; -const char * const S_RETURNTOEDITOR = "GCS.ReturnToEditor"; -const char * const OPEN_IN_EXTERNAL_EDITOR = "GCS.OpenInExternalEditor"; +const char *const SPLIT = "GCS.Split"; +const char *const SPLIT_SIDE_BY_SIDE = "GCS.SplitSideBySide"; +const char *const REMOVE_CURRENT_SPLIT = "GCS.RemoveCurrentSplit"; +const char *const REMOVE_ALL_SPLITS = "GCS.RemoveAllSplits"; +const char *const GOTO_OTHER_SPLIT = "GCS.GotoOtherSplit"; +const char *const SAVEASDEFAULT = "GCS.SaveAsDefaultLayout"; +const char *const RESTOREDEFAULT = "GCS.RestoreDefaultLayout"; +const char *const HIDE_TOOLBARS = "GCS.HideToolbars"; +const char *const CLOSE = "GCS.Close"; +const char *const CLOSEALL = "GCS.CloseAll"; +const char *const CLOSEOTHERS = "GCS.CloseOthers"; +const char *const GOTONEXT = "GCS.GotoNext"; +const char *const GOTOPREV = "GCS.GotoPrevious"; +const char *const GOTONEXTINHISTORY = "GCS.GotoNextInHistory"; +const char *const GOTOPREVINHISTORY = "GCS.GotoPreviousInHistory"; +const char *const GO_BACK = "GCS.GoBack"; +const char *const GO_FORWARD = "GCS.GoForward"; +const char *const GOTOPREVIOUSGROUP = "GCS.GotoPreviousTabGroup"; +const char *const GOTONEXTGROUP = "GCS.GotoNextTabGroup"; +const char *const WINDOWSLIST = "GCS.WindowsList"; +const char *const ABOUT_OPENPILOTGCS = "GCS.AboutOpenPilotGCS"; +const char *const ABOUT_PLUGINS = "GCS.AboutPlugins"; +const char *const ABOUT_AUTHORS = "GCS.AboutAuthors"; +const char *const ABOUT_QT = "GCS.AboutQt"; +const char *const S_RETURNTOEDITOR = "GCS.ReturnToEditor"; +const char *const OPEN_IN_EXTERNAL_EDITOR = "GCS.OpenInExternalEditor"; // default groups -const char * const G_DEFAULT_ONE = "GCS.Group.Default.One"; -const char * const G_DEFAULT_TWO = "GCS.Group.Default.Two"; -const char * const G_DEFAULT_THREE = "GCS.Group.Default.Three"; +const char *const G_DEFAULT_ONE = "GCS.Group.Default.One"; +const char *const G_DEFAULT_TWO = "GCS.Group.Default.Two"; +const char *const G_DEFAULT_THREE = "GCS.Group.Default.Three"; // main menu bar groups -const char * const G_FILE = "GCS.Group.File"; -const char * const G_EDIT = "GCS.Group.Edit"; -const char * const G_VIEW = "GCS.Group.View"; -const char * const G_TOOLS = "GCS.Group.Tools"; -const char * const G_WINDOW = "GCS.Group.Window"; -const char * const G_HELP = "GCS.Group.Help"; +const char *const G_FILE = "GCS.Group.File"; +const char *const G_EDIT = "GCS.Group.Edit"; +const char *const G_VIEW = "GCS.Group.View"; +const char *const G_TOOLS = "GCS.Group.Tools"; +const char *const G_WINDOW = "GCS.Group.Window"; +const char *const G_HELP = "GCS.Group.Help"; // file menu groups -const char * const G_FILE_NEW = "GCS.Group.File.New"; -const char * const G_FILE_OPEN = "GCS.Group.File.Open"; -const char * const G_FILE_PROJECT = "GCS.Group.File.Project"; -const char * const G_FILE_SAVE = "GCS.Group.File.Save"; -const char * const G_FILE_CLOSE = "GCS.Group.File.Close"; -const char * const G_FILE_OTHER = "GCS.Group.File.Other"; +const char *const G_FILE_NEW = "GCS.Group.File.New"; +const char *const G_FILE_OPEN = "GCS.Group.File.Open"; +const char *const G_FILE_PROJECT = "GCS.Group.File.Project"; +const char *const G_FILE_SAVE = "GCS.Group.File.Save"; +const char *const G_FILE_CLOSE = "GCS.Group.File.Close"; +const char *const G_FILE_OTHER = "GCS.Group.File.Other"; // edit menu groups -const char * const G_EDIT_UNDOREDO = "GCS.Group.Edit.UndoRedo"; -const char * const G_EDIT_COPYPASTE = "GCS.Group.Edit.CopyPaste"; -const char * const G_EDIT_SELECTALL = "GCS.Group.Edit.SelectAll"; -const char * const G_EDIT_ADVANCED = "GCS.Group.Edit.Advanced"; +const char *const G_EDIT_UNDOREDO = "GCS.Group.Edit.UndoRedo"; +const char *const G_EDIT_COPYPASTE = "GCS.Group.Edit.CopyPaste"; +const char *const G_EDIT_SELECTALL = "GCS.Group.Edit.SelectAll"; +const char *const G_EDIT_ADVANCED = "GCS.Group.Edit.Advanced"; -const char * const G_EDIT_FIND = "GCS.Group.Edit.Find"; -const char * const G_EDIT_OTHER = "GCS.Group.Edit.Other"; +const char *const G_EDIT_FIND = "GCS.Group.Edit.Find"; +const char *const G_EDIT_OTHER = "GCS.Group.Edit.Other"; // advanced edit menu groups -const char * const G_EDIT_FORMAT = "GCS.Group.Edit.Format"; -const char * const G_EDIT_COLLAPSING = "GCS.Group.Edit.Collapsing"; -const char * const G_EDIT_BLOCKS = "GCS.Group.Edit.Blocks"; -const char * const G_EDIT_FONT = "GCS.Group.Edit.Font"; -const char * const G_EDIT_EDITOR = "GCS.Group.Edit.Editor"; +const char *const G_EDIT_FORMAT = "GCS.Group.Edit.Format"; +const char *const G_EDIT_COLLAPSING = "GCS.Group.Edit.Collapsing"; +const char *const G_EDIT_BLOCKS = "GCS.Group.Edit.Blocks"; +const char *const G_EDIT_FONT = "GCS.Group.Edit.Font"; +const char *const G_EDIT_EDITOR = "GCS.Group.Edit.Editor"; // window menu groups -const char * const G_WINDOW_SIZE = "GCS.Group.Window.Size"; -const char * const G_WINDOW_PANES = "GCS.Group.Window.Panes"; -const char * const G_WINDOW_SPLIT = "GCS.Group.Window.Split"; -const char * const G_WINDOW_NAVIGATE = "GCS.Group.Window.Navigate"; -const char * const G_WINDOW_OTHER = "GCS.Group.Window.Other"; -const char * const G_WINDOW_HIDE_TOOLBAR = "GCS.Group.Window.Hide"; +const char *const G_WINDOW_SIZE = "GCS.Group.Window.Size"; +const char *const G_WINDOW_PANES = "GCS.Group.Window.Panes"; +const char *const G_WINDOW_SPLIT = "GCS.Group.Window.Split"; +const char *const G_WINDOW_NAVIGATE = "GCS.Group.Window.Navigate"; +const char *const G_WINDOW_OTHER = "GCS.Group.Window.Other"; +const char *const G_WINDOW_HIDE_TOOLBAR = "GCS.Group.Window.Hide"; // help groups (global) -const char * const G_HELP_HELP = "GCS.Group.Help.Help"; -const char * const G_HELP_ABOUT = "GCS.Group.Help.About"; +const char *const G_HELP_HELP = "GCS.Group.Help.Help"; +const char *const G_HELP_ABOUT = "GCS.Group.Help.About"; -const char * const ICON_MINUS = ":/core/images/minus.png"; -const char * const ICON_PLUS = ":/core/images/plus.png"; -const char * const ICON_NEWFILE = ":/core/images/filenew.png"; -const char * const ICON_OPENFILE = ":/core/images/fileopen.png"; -const char * const ICON_SAVEFILE = ":/core/images/filesave.png"; -const char * const ICON_UNDO = ":/core/images/undo.png"; -const char * const ICON_REDO = ":/core/images/redo.png"; -const char * const ICON_COPY = ":/core/images/editcopy.png"; -const char * const ICON_PASTE = ":/core/images/editpaste.png"; -const char * const ICON_CUT = ":/core/images/editcut.png"; -const char * const ICON_NEXT = ":/core/images/next.png"; -const char * const ICON_PREV = ":/core/images/prev.png"; -const char * const ICON_DIR = ":/core/images/dir.png"; -const char * const ICON_CLEAN_PANE = ":/core/images/clean_pane_small.png"; -const char * const ICON_CLEAR = ":/core/images/clear.png"; -const char * const ICON_FIND = ":/core/images/find.png"; -const char * const ICON_FINDNEXT = ":/core/images/findnext.png"; -const char * const ICON_REPLACE = ":/core/images/replace.png"; -const char * const ICON_RESET = ":/core/images/reset.png"; -const char * const ICON_MAGNIFIER = ":/core/images/magnifier.png"; -const char * const ICON_TOGGLE_SIDEBAR = ":/core/images/sidebaricon.png"; -const char * const ICON_PLUGIN = ":/core/images/pluginicon.png"; -const char * const ICON_EXIT = ":/core/images/exiticon.png"; -const char * const ICON_OPTIONS = ":/core/images/optionsicon.png"; -const char * const ICON_HELP = ":/core/images/helpicon.png"; -const char * const ICON_OPENPILOT = ":/core/images/openpiloticon.png"; +const char *const ICON_MINUS = ":/core/images/minus.png"; +const char *const ICON_PLUS = ":/core/images/plus.png"; +const char *const ICON_NEWFILE = ":/core/images/filenew.png"; +const char *const ICON_OPENFILE = ":/core/images/fileopen.png"; +const char *const ICON_SAVEFILE = ":/core/images/filesave.png"; +const char *const ICON_UNDO = ":/core/images/undo.png"; +const char *const ICON_REDO = ":/core/images/redo.png"; +const char *const ICON_COPY = ":/core/images/editcopy.png"; +const char *const ICON_PASTE = ":/core/images/editpaste.png"; +const char *const ICON_CUT = ":/core/images/editcut.png"; +const char *const ICON_NEXT = ":/core/images/next.png"; +const char *const ICON_PREV = ":/core/images/prev.png"; +const char *const ICON_DIR = ":/core/images/dir.png"; +const char *const ICON_CLEAN_PANE = ":/core/images/clean_pane_small.png"; +const char *const ICON_CLEAR = ":/core/images/clear.png"; +const char *const ICON_FIND = ":/core/images/find.png"; +const char *const ICON_FINDNEXT = ":/core/images/findnext.png"; +const char *const ICON_REPLACE = ":/core/images/replace.png"; +const char *const ICON_RESET = ":/core/images/reset.png"; +const char *const ICON_MAGNIFIER = ":/core/images/magnifier.png"; +const char *const ICON_TOGGLE_SIDEBAR = ":/core/images/sidebaricon.png"; +const char *const ICON_PLUGIN = ":/core/images/pluginicon.png"; +const char *const ICON_EXIT = ":/core/images/exiticon.png"; +const char *const ICON_OPTIONS = ":/core/images/optionsicon.png"; +const char *const ICON_HELP = ":/core/images/helpicon.png"; +const char *const ICON_OPENPILOT = ":/core/images/openpiloticon.png"; // wizard kind -const char * const WIZARD_TYPE_FILE = "GCS::WizardType::File"; -const char * const WIZARD_TYPE_CLASS = "GCS::WizardType::Class"; - +const char *const WIZARD_TYPE_FILE = "GCS::WizardType::File"; +const char *const WIZARD_TYPE_CLASS = "GCS::WizardType::Class"; } // namespace Constants } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.cpp b/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.cpp index b45582628..5ee027096 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.cpp @@ -10,19 +10,19 @@ * @{ * @brief The Core GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,10 +33,8 @@ namespace Core { namespace Internal { - // The Core Singleton static CoreImpl *m_instance = 0; - } // namespace Internal } // namespace Core @@ -45,14 +43,14 @@ using namespace Core; using namespace Core::Internal; -ICore* ICore::instance() +ICore *ICore::instance() { return m_instance; } CoreImpl::CoreImpl(MainWindow *mainwindow) { - m_instance = this; + m_instance = this; m_mainwindow = mainwindow; } @@ -184,24 +182,24 @@ void CoreImpl::updateContext() void CoreImpl::openFiles(const QStringList &arguments) { Q_UNUSED(arguments) - //m_mainwindow->openFiles(arguments); + // m_mainwindow->openFiles(arguments); } -void CoreImpl::readMainSettings(QSettings* qs, bool workspaceDiffOnly) +void CoreImpl::readMainSettings(QSettings *qs, bool workspaceDiffOnly) { m_mainwindow->readSettings(qs, workspaceDiffOnly); } -void CoreImpl::saveMainSettings(QSettings* qs) +void CoreImpl::saveMainSettings(QSettings *qs) { m_mainwindow->saveSettings(qs); } -void CoreImpl::readSettings(IConfigurablePlugin* plugin, QSettings* qs) +void CoreImpl::readSettings(IConfigurablePlugin *plugin, QSettings *qs) { m_mainwindow->readSettings(plugin, qs); } -void CoreImpl::saveSettings(IConfigurablePlugin* plugin, QSettings* qs) +void CoreImpl::saveSettings(IConfigurablePlugin *plugin, QSettings *qs) { m_mainwindow->saveSettings(plugin, qs); } @@ -209,4 +207,3 @@ void CoreImpl::deleteSettings() { m_mainwindow->deleteSettings(); } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.h b/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.h index cd0dfd928..671c248a2 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,9 +34,7 @@ namespace Core { namespace Internal { - -class CoreImpl : public ICore -{ +class CoreImpl : public ICore { Q_OBJECT public: @@ -64,10 +62,10 @@ public: QSettings *settings(QSettings::Scope scope = QSettings::UserScope) const; SettingsDatabase *settingsDatabase() const; - void readMainSettings(QSettings* qs, bool workspaceDiffOnly); - void saveMainSettings(QSettings* qs); - void readSettings(IConfigurablePlugin* plugin, QSettings* qs = 0 ); - void saveSettings(IConfigurablePlugin* plugin, QSettings* qs = 0 ); + void readMainSettings(QSettings *qs, bool workspaceDiffOnly); + void saveMainSettings(QSettings *qs); + void readSettings(IConfigurablePlugin *plugin, QSettings *qs = 0); + void saveSettings(IConfigurablePlugin *plugin, QSettings *qs = 0); void deleteSettings(); QString resourcePath() const; @@ -92,7 +90,6 @@ private: MainWindow *m_mainwindow; friend class MainWindow; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.cpp b/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.cpp index 511077ee7..fd5a058cf 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,8 +38,7 @@ using namespace Core::Internal; CorePlugin::CorePlugin() : m_mainWindow(new MainWindow) -{ -} +{} CorePlugin::~CorePlugin() { @@ -61,7 +60,7 @@ void CorePlugin::extensionsInitialized() m_mainWindow->extensionsInitialized(); } -void CorePlugin::remoteArgument(const QString& arg) +void CorePlugin::remoteArgument(const QString & arg) { // An empty argument is sent to trigger activation // of the window via QtSingleApplication. It should be @@ -69,7 +68,7 @@ void CorePlugin::remoteArgument(const QString& arg) if (arg.isEmpty()) { m_mainWindow->activateWindow(); } else { - //m_mainWindow->openFiles(QStringList(arg)); + // m_mainWindow->openFiles(QStringList(arg)); } } @@ -78,4 +77,4 @@ void CorePlugin::shutdown() m_mainWindow->shutdown(); } -Q_EXPORT_PLUGIN2(Core,CorePlugin) +Q_EXPORT_PLUGIN2(Core, CorePlugin) diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.h b/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.h index ccd80c69c..8c4431fee 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,11 +33,9 @@ namespace Core { namespace Internal { - class MainWindow; -class CorePlugin : public ExtensionSystem::IPlugin -{ +class CorePlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -49,12 +47,11 @@ public: virtual void shutdown(); public slots: - void remoteArgument(const QString&); + void remoteArgument(const QString &); private: MainWindow *m_mainWindow; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.cpp b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.cpp index 93ad2eb07..91ec41530 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.cpp @@ -11,35 +11,35 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "ioptionspage.h" /*! - \class Core::IOptionsPage - \mainclass - \brief The IOptionsPage is an interface for providing options pages. + \class Core::IOptionsPage + \mainclass + \brief The IOptionsPage is an interface for providing options pages. - Guidelines for implementing: - \list - \o id() is an id used for filtering when calling ICore:: showOptionsDialog() - \o trName() is the (translated) name for display. - \o category() is the category used for filtering when calling ICore:: showOptionsDialog() - \o trCategory() is the translated category - \o apply() is called to store the settings. It should detect if any changes have been + Guidelines for implementing: + \list + \o id() is an id used for filtering when calling ICore:: showOptionsDialog() + \o trName() is the (translated) name for display. + \o category() is the category used for filtering when calling ICore:: showOptionsDialog() + \o trCategory() is the translated category + \o apply() is called to store the settings. It should detect if any changes have been made and store those. - \endlist -*/ + \endlist + */ diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.h b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.h index cd87f4bba..1060d072d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/ioptionspage.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,9 +39,7 @@ class QWidget; QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT IOptionsPage : public QObject -{ +class CORE_EXPORT IOptionsPage : public QObject { Q_OBJECT public: IOptionsPage(QObject *parent = 0) : @@ -49,24 +47,41 @@ public: m_icon(QIcon()) {} virtual ~IOptionsPage() {} - void setIcon(QIcon icon) { m_icon = icon; } - QIcon icon() { return m_icon; } + void setIcon(QIcon icon) + { + m_icon = icon; + } + QIcon icon() + { + return m_icon; + } /* gadget options pages can leave these 4 functions as is, since they are decorated by UAVGadgetOptionsPageDecorator, all other options pages must override these */ - virtual QString id() const { return ""; }; - virtual QString trName() const { return ""; }; - virtual QString category() const { return "DefaultCategory"; }; - virtual QString trCategory() const { return "DefaultCategory"; }; + virtual QString id() const + { + return ""; + }; + virtual QString trName() const + { + return ""; + }; + virtual QString category() const + { + return "DefaultCategory"; + }; + virtual QString trCategory() const + { + return "DefaultCategory"; + }; virtual QWidget *createPage(QWidget *parent) = 0; - virtual void apply() = 0; + virtual void apply() = 0; virtual void finish() = 0; private: QIcon m_icon; }; - } // namespace Core #endif // IOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.cpp b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.cpp index 122927f63..202f3996d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -57,7 +57,7 @@ \endcode \sa Core::BaseFileWizard \sa Core::StandardFileWizard -*/ + */ /*! \enum Core::IWizard::Kind @@ -70,51 +70,51 @@ The wizard creates a new class (e.g. source+header files). \value ProjectWizard The wizard creates a new project. -*/ + */ /*! \fn IWizard::IWizard(QObject *parent) \internal -*/ + */ /*! \fn IWizard::~IWizard() \internal -*/ + */ /*! \fn Kind IWizard::kind() const Returns what kind of objects are created by the wizard. \sa Kind -*/ + */ /*! \fn QIcon IWizard::icon() const Returns an icon to show in the wizard selection dialog. -*/ + */ /*! \fn QString IWizard::description() const Returns a translated description to show when this wizard is selected in the dialog. -*/ + */ /*! \fn QString IWizard::name() const Returns the translated name of the wizard, how it should appear in the dialog. -*/ + */ /*! \fn QString IWizard::category() const Returns a category ID to add the wizard to. -*/ + */ /*! \fn QString IWizard::trCategory() const Returns the translated string of the category, how it should appear in the dialog. -*/ + */ /*! \fn QStringList IWizard::runWizard(const QString &path, QWidget *parent) @@ -124,25 +124,28 @@ created. The wizard should fill this in its path selection elements as a default path. Returns a list of files (absolute paths) that have been created, if any. -*/ + */ using namespace Core; /* A utility to find all wizards supporting a view mode and matching a predicate */ template - QList findWizards(Predicate predicate) +QList findWizards(Predicate predicate) { // Filter all wizards - const QList allWizards = IWizard::allWizards(); - QList rc; - const QList::const_iterator cend = allWizards.constEnd(); - for (QList::const_iterator it = allWizards.constBegin(); it != cend; ++it) - if (predicate(*(*it))) + const QList allWizards = IWizard::allWizards(); + + QList rc; + const QList::const_iterator cend = allWizards.constEnd(); + for (QList::const_iterator it = allWizards.constBegin(); it != cend; ++it) { + if (predicate(*(*it))) { rc.push_back(*it); + } + } return rc; } -QList IWizard::allWizards() +QList IWizard::allWizards() { return ExtensionSystem::PluginManager::instance()->getObjects(); } @@ -152,13 +155,15 @@ QList IWizard::allWizards() class WizardKindPredicate { public: WizardKindPredicate(IWizard::Kind kind) : m_kind(kind) {} - bool operator()(const IWizard &w) const { return w.kind() == m_kind; } + bool operator()(const IWizard &w) const + { + return w.kind() == m_kind; + } private: const IWizard::Kind m_kind; }; -QList IWizard::wizardsOfKind(Kind kind) +QList IWizard::wizardsOfKind(Kind kind) { return findWizards(WizardKindPredicate(kind)); } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.h b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.h index 0ce03b8fc..b46e5b768 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/iwizard.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,10 +37,8 @@ class QIcon; QT_END_NAMESPACE namespace Core { - class CORE_EXPORT IWizard - : public QObject -{ + : public QObject { Q_OBJECT public: enum Kind { @@ -52,22 +50,21 @@ public: IWizard(QObject *parent = 0) : QObject(parent) {} virtual ~IWizard() {} - virtual Kind kind() const = 0; - virtual QIcon icon() const = 0; + virtual Kind kind() const = 0; + virtual QIcon icon() const = 0; virtual QString description() const = 0; - virtual QString name() const = 0; + virtual QString name() const = 0; - virtual QString category() const = 0; - virtual QString trCategory() const = 0; + virtual QString category() const = 0; + virtual QString trCategory() const = 0; virtual QStringList runWizard(const QString &path, QWidget *parent) = 0; // Utility to find all registered wizards - static QList allWizards(); + static QList allWizards(); // Utility to find all registered wizards of a certain kind - static QList wizardsOfKind(Kind kind); + static QList wizardsOfKind(Kind kind); }; - } // namespace Core #endif // IWIZARD_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.cpp b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.cpp index fc45184d5..fd772b0bb 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,7 +32,7 @@ #include "icore.h" #include "coreplugin/uavgadgetinstancemanager.h" #include "coreplugin/uavgadgetoptionspagedecorator.h" -//#include "coreimpl.h" +// #include "coreimpl.h" #include #include @@ -41,11 +41,11 @@ #include namespace { - struct PageData { - int index; - QString category; - QString id; - }; +struct PageData { + int index; + QString category; + QString id; +}; } Q_DECLARE_METATYPE(::PageData) @@ -56,23 +56,27 @@ using namespace Core::Internal; // Helpers to sort by category. id bool optionsPageLessThan(const IOptionsPage *p1, const IOptionsPage *p2) { - const UAVGadgetOptionsPageDecorator *gp1 = qobject_cast(p1); - const UAVGadgetOptionsPageDecorator *gp2 = qobject_cast(p2); - if (gp1 && (gp2 == NULL)) + const UAVGadgetOptionsPageDecorator *gp1 = qobject_cast(p1); + const UAVGadgetOptionsPageDecorator *gp2 = qobject_cast(p2); + + if (gp1 && (gp2 == NULL)) { return false; + } - if (gp2 && (gp1 == NULL)) + if (gp2 && (gp1 == NULL)) { return true; + } - if (const int cc = QString::localeAwareCompare(p1->trCategory(), p2->trCategory())) + if (const int cc = QString::localeAwareCompare(p1->trCategory(), p2->trCategory())) { return cc < 0; + } return QString::localeAwareCompare(p1->trName(), p2->trName()) < 0; } -static inline QList sortedOptionsPages() +static inline QList sortedOptionsPages() { - QList rc = ExtensionSystem::PluginManager::instance()->getObjects(); + QList rc = ExtensionSystem::PluginManager::instance()->getObjects(); qStableSort(rc.begin(), rc.end(), optionsPageLessThan); return rc; } @@ -93,15 +97,15 @@ SettingsDialog::SettingsDialog(QWidget *parent, const QString &categoryId, const // restore last displayed category and page // this is done only if no category or page was provided through the constructor QString initialCategory = categoryId; - QString initialPage = pageId; + QString initialPage = pageId; qDebug() << "SettingsDialog constructor initial category:" << initialCategory << ", initial page:" << initialPage; if (initialCategory.isEmpty() && initialPage.isEmpty()) { initialCategory = settings->value("LastPreferenceCategory", QVariant(QString())).toString(); - initialPage = settings->value("LastPreferencePage", QVariant(QString())).toString(); + initialPage = settings->value("LastPreferencePage", QVariant(QString())).toString(); qDebug() << "SettingsDialog settings initial category:" << initialCategory << ", initial page: " << initialPage; } // restore window size - int windowWidth = settings->value("SettingsWindowWidth", 0).toInt(); + int windowWidth = settings->value("SettingsWindowWidth", 0).toInt(); int windowHeight = settings->value("SettingsWindowHeight", 0).toInt(); qDebug() << "SettingsDialog window width :" << windowWidth << ", height:" << windowHeight; if (windowWidth > 0 && windowHeight > 0) { @@ -112,31 +116,32 @@ SettingsDialog::SettingsDialog(QWidget *parent, const QString &categoryId, const buttonBox->button(QDialogButtonBox::Ok)->setDefault(true); connect(buttonBox->button(QDialogButtonBox::Apply), SIGNAL(clicked()), this, SLOT(apply())); - + m_instanceManager = Core::ICore::instance()->uavGadgetInstanceManager(); - - connect(this, SIGNAL(settingsDialogShown(Core::Internal::SettingsDialog*)), m_instanceManager, SLOT(settingsDialogShown(Core::Internal::SettingsDialog*))); + + connect(this, SIGNAL(settingsDialogShown(Core::Internal::SettingsDialog *)), m_instanceManager, SLOT(settingsDialogShown(Core::Internal::SettingsDialog *))); connect(this, SIGNAL(settingsDialogRemoved()), m_instanceManager, SLOT(settingsDialogRemoved())); connect(this, SIGNAL(categoryItemSelected()), this, SLOT(categoryItemSelectedShowChildInstead()), Qt::QueuedConnection); splitter->setCollapsible(0, false); splitter->setCollapsible(1, false); pageTree->header()->setVisible(false); -// pageTree->setIconSize(QSize(24, 24)); +// pageTree->setIconSize(QSize(24, 24)); connect(pageTree, SIGNAL(currentItemChanged(QTreeWidgetItem *, QTreeWidgetItem *)), - this, SLOT(pageSelected())); + this, SLOT(pageSelected())); QMap categories; - QList pages = sortedOptionsPages(); + QList pages = sortedOptionsPages(); int index = 0; bool firstUavGadgetOptionsPageFound = false; QTreeWidgetItem *initialItem = 0; - foreach (IOptionsPage *page, pages) { + foreach(IOptionsPage * page, pages) { PageData pageData; - pageData.index = index; + + pageData.index = index; pageData.category = page->category(); pageData.id = page->id(); @@ -144,14 +149,14 @@ SettingsDialog::SettingsDialog(QWidget *parent, const QString &categoryId, const item->setText(0, page->trName()); item->setData(0, Qt::UserRole, qVariantFromValue(pageData)); - QString trCategories = page->trCategory(); + QString trCategories = page->trCategory(); QString currentCategory = page->category(); QTreeWidgetItem *categoryItem; if (!categories.contains(currentCategory)) { // Above the first gadget option we insert a separator if (!firstUavGadgetOptionsPageFound) { - UAVGadgetOptionsPageDecorator *pd = qobject_cast(page); + UAVGadgetOptionsPageDecorator *pd = qobject_cast(page); if (pd) { firstUavGadgetOptionsPageFound = true; QTreeWidgetItem *separator = new QTreeWidgetItem(pageTree); @@ -168,8 +173,8 @@ SettingsDialog::SettingsDialog(QWidget *parent, const QString &categoryId, const QList *categoryItemList = m_categoryItemsMap.value(currentCategory); if (!categoryItemList) { - categoryItemList = new QList(); - m_categoryItemsMap.insert(currentCategory, categoryItemList); + categoryItemList = new QList(); + m_categoryItemsMap.insert(currentCategory, categoryItemList); } categoryItemList->append(item); @@ -178,7 +183,7 @@ SettingsDialog::SettingsDialog(QWidget *parent, const QString &categoryId, const // creating all option pages upfront is slow, so we create place holder widgets instead // the real option page widget will be created later when the user selects it // the place holder is a QLabel and we assume that no option page will be a QLabel... - QLabel * placeholderWidget = new QLabel(stackedPages); + QLabel *placeholderWidget = new QLabel(stackedPages); stackedPages->addWidget(placeholderWidget); if (page->id() == initialPage && currentCategory == initialCategory) { @@ -191,8 +196,9 @@ SettingsDialog::SettingsDialog(QWidget *parent, const QString &categoryId, const foreach(QString category, m_categoryItemsMap.keys()) { QList *categoryItemList = m_categoryItemsMap.value(category); if (categoryItemList->size() > 1) { - foreach (QTreeWidgetItem *item, *categoryItemList) { + foreach(QTreeWidgetItem * item, *categoryItemList) { QTreeWidgetItem *categoryItem = categories.value(category); + categoryItem->addChild(item); } } @@ -223,7 +229,7 @@ SettingsDialog::~SettingsDialog() } // delete place holders for (int i = 0; i < stackedPages->count(); i++) { - QLabel * widget = dynamic_cast(stackedPages->widget(i)); + QLabel *widget = dynamic_cast(stackedPages->widget(i)); if (widget) { delete widget; } @@ -233,15 +239,17 @@ SettingsDialog::~SettingsDialog() void SettingsDialog::pageSelected() { QTreeWidgetItem *item = pageTree->currentItem(); - if (!item) + + if (!item) { return; + } PageData data = item->data(0, Qt::UserRole).value(); - int index = data.index; + int index = data.index; m_currentCategory = data.category; - m_currentPage = data.id; + m_currentPage = data.id; // check if we are looking at a place holder or not - QWidget *widget = dynamic_cast(stackedPages->widget(index)); + QWidget *widget = dynamic_cast(stackedPages->widget(index)); if (widget) { // place holder found, get rid of it... stackedPages->removeWidget(widget); @@ -261,6 +269,7 @@ void SettingsDialog::pageSelected() void SettingsDialog::categoryItemSelectedShowChildInstead() { QTreeWidgetItem *item = pageTree->currentItem(); + item->setExpanded(true); pageTree->setCurrentItem(item->child(0), 0, QItemSelectionModel::SelectCurrent); } @@ -268,8 +277,9 @@ void SettingsDialog::categoryItemSelectedShowChildInstead() void SettingsDialog::deletePage() { QTreeWidgetItem *item = pageTree->currentItem(); - PageData data = item->data(0, Qt::UserRole).value(); + PageData data = item->data(0, Qt::UserRole).value(); QString category = data.category; + QList *categoryItemList = m_categoryItemsMap.value(category); QTreeWidgetItem *parentItem = item->parent(); parentItem->removeChild(item); @@ -280,10 +290,11 @@ void SettingsDialog::deletePage() pageSelected(); } -void SettingsDialog::insertPage(IOptionsPage* page) +void SettingsDialog::insertPage(IOptionsPage *page) { PageData pageData; - pageData.index = m_pages.count(); + + pageData.index = m_pages.count(); pageData.category = page->category(); pageData.id = page->id(); @@ -296,8 +307,9 @@ void SettingsDialog::insertPage(IOptionsPage* page) break; } } - if (!categoryItem) + if (!categoryItem) { return; + } // If this category has no child right now // we need to add the "default child" @@ -324,6 +336,7 @@ void SettingsDialog::insertPage(IOptionsPage* page) void SettingsDialog::updateText(QString text) { QTreeWidgetItem *item = pageTree->currentItem(); + item->setText(0, text); } @@ -337,9 +350,9 @@ void SettingsDialog::accept() { m_applied = true; for (int i = 0; i < m_pages.size(); i++) { - QWidget * widget = dynamic_cast(stackedPages->widget(i)); + QWidget *widget = dynamic_cast(stackedPages->widget(i)); if (!widget) { - IOptionsPage * page = m_pages.at(i); + IOptionsPage *page = m_pages.at(i); page->apply(); page->finish(); } @@ -350,9 +363,9 @@ void SettingsDialog::accept() void SettingsDialog::reject() { for (int i = 0; i < m_pages.size(); i++) { - QWidget * widget = dynamic_cast(stackedPages->widget(i)); + QWidget *widget = dynamic_cast(stackedPages->widget(i)); if (!widget) { - IOptionsPage * page = m_pages.at(i); + IOptionsPage *page = m_pages.at(i); page->finish(); } } @@ -362,9 +375,9 @@ void SettingsDialog::reject() void SettingsDialog::apply() { for (int i = 0; i < m_pages.size(); i++) { - QWidget * widget = dynamic_cast(stackedPages->widget(i)); + QWidget *widget = dynamic_cast(stackedPages->widget(i)); if (!widget) { - IOptionsPage * page = m_pages.at(i); + IOptionsPage *page = m_pages.at(i); page->apply(); } } @@ -383,6 +396,7 @@ bool SettingsDialog::execDialog() void SettingsDialog::done(int val) { QSettings *settings = ICore::instance()->settings(); + settings->setValue("General/LastPreferenceCategory", m_currentCategory); settings->setValue("General/LastPreferencePage", m_currentPage); settings->setValue("General/SettingsWindowWidth", this->width()); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.h b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.h index 5ce5d424b..ade61b525 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/settingsdialog.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,13 +36,10 @@ #include "coreplugin/dialogs/ioptionspage.h" namespace Core { - class UAVGadgetInstanceManager; namespace Internal { - -class SettingsDialog : public QDialog, public ::Ui::SettingsDialog -{ +class SettingsDialog : public QDialog, public::Ui::SettingsDialog { Q_OBJECT public: @@ -57,7 +54,7 @@ public: void disableApplyOk(bool disable); signals: - void settingsDialogShown(Core::Internal::SettingsDialog*); + void settingsDialogShown(Core::Internal::SettingsDialog *); void settingsDialogRemoved(); void categoryItemSelected(); @@ -72,14 +69,13 @@ private slots: void categoryItemSelectedShowChildInstead(); private: - QList m_pages; + QList m_pages; QMap *> m_categoryItemsMap; UAVGadgetInstanceManager *m_instanceManager; bool m_applied; QString m_currentCategory; QString m_currentPage; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.cpp b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.cpp index fb4bce83c..6566cf9bd 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,19 +44,17 @@ #include #include -Q_DECLARE_METATYPE(Core::Internal::ShortcutItem*) +Q_DECLARE_METATYPE(Core::Internal::ShortcutItem *) using namespace Core; using namespace Core::Internal; ShortcutSettings::ShortcutSettings(QObject *parent) : IOptionsPage(parent) -{ -} +{} ShortcutSettings::~ShortcutSettings() -{ -} +{} // IOptionsPage @@ -84,7 +82,7 @@ QWidget *ShortcutSettings::createPage(QWidget *parent) { m_keyNum = m_key[0] = m_key[1] = m_key[2] = m_key[3] = 0; - m_page = new Ui_ShortcutSettings(); + m_page = new Ui_ShortcutSettings(); QWidget *w = new QWidget(parent); m_page->setupUi(w); @@ -92,15 +90,15 @@ QWidget *ShortcutSettings::createPage(QWidget *parent) m_page->shortcutEdit->installEventFilter(this); connect(m_page->resetButton, SIGNAL(clicked()), - this, SLOT(resetKeySequence())); + this, SLOT(resetKeySequence())); connect(m_page->removeButton, SIGNAL(clicked()), - this, SLOT(removeKeySequence())); + this, SLOT(removeKeySequence())); connect(m_page->exportButton, SIGNAL(clicked()), - this, SLOT(exportAction())); + this, SLOT(exportAction())); connect(m_page->importButton, SIGNAL(clicked()), - this, SLOT(importAction())); + this, SLOT(importAction())); connect(m_page->defaultButton, SIGNAL(clicked()), - this, SLOT(defaultAction())); + this, SLOT(defaultAction())); initialize(); @@ -108,7 +106,7 @@ QWidget *ShortcutSettings::createPage(QWidget *parent) connect(m_page->filterEdit, SIGNAL(textChanged(QString)), this, SLOT(filterChanged(QString))); connect(m_page->commandList, SIGNAL(currentItemChanged(QTreeWidgetItem *, QTreeWidgetItem *)), - this, SLOT(commandChanged(QTreeWidgetItem *))); + this, SLOT(commandChanged(QTreeWidgetItem *))); connect(m_page->shortcutEdit, SIGNAL(textChanged(QString)), this, SLOT(keyChanged())); new Utils::TreeWidgetColumnStretcher(m_page->commandList, 1); @@ -120,8 +118,8 @@ QWidget *ShortcutSettings::createPage(QWidget *parent) void ShortcutSettings::apply() { - foreach (ShortcutItem *item, m_scitems) - item->m_cmd->setKeySequence(item->m_key); + foreach(ShortcutItem * item, m_scitems) + item->m_cmd->setKeySequence(item->m_key); } void ShortcutSettings::finish() @@ -136,16 +134,17 @@ bool ShortcutSettings::eventFilter(QObject *o, QEvent *e) { Q_UNUSED(o) - if ( e->type() == QEvent::KeyPress ) { - QKeyEvent *k = static_cast(e); + if (e->type() == QEvent::KeyPress) { + QKeyEvent *k = static_cast(e); handleKeyEvent(k); return true; } - if ( e->type() == QEvent::Shortcut || - e->type() == QEvent::ShortcutOverride || - e->type() == QEvent::KeyRelease ) + if (e->type() == QEvent::Shortcut || + e->type() == QEvent::ShortcutOverride || + e->type() == QEvent::KeyRelease) { return true; + } return false; } @@ -164,7 +163,7 @@ void ShortcutSettings::commandChanged(QTreeWidgetItem *current) void ShortcutSettings::filterChanged(const QString &f) { - for (int i=0; icommandList->topLevelItemCount(); ++i) { + for (int i = 0; i < m_page->commandList->topLevelItemCount(); ++i) { QTreeWidgetItem *item = m_page->commandList->topLevelItem(i); item->setHidden(filter(f, item)); } @@ -173,6 +172,7 @@ void ShortcutSettings::filterChanged(const QString &f) void ShortcutSettings::keyChanged() { QTreeWidgetItem *current = m_page->commandList->currentItem(); + if (current && current->data(0, Qt::UserRole).isValid()) { ShortcutItem *scitem = qVariantValue(current->data(0, Qt::UserRole)); scitem->m_key = QKeySequence(m_key[0], m_key[1], m_key[2], m_key[3]); @@ -193,11 +193,13 @@ void ShortcutSettings::setKeySequence(const QKeySequence &key) bool ShortcutSettings::filter(const QString &f, const QTreeWidgetItem *item) { if (item->childCount() == 0) { - if (f.isEmpty()) + if (f.isEmpty()) { return false; + } for (int i = 0; i < item->columnCount(); ++i) { - if (item->text(i).contains(f, Qt::CaseInsensitive)) + if (item->text(i).contains(f, Qt::CaseInsensitive)) { return false; + } } return true; } @@ -218,6 +220,7 @@ bool ShortcutSettings::filter(const QString &f, const QTreeWidgetItem *item) void ShortcutSettings::resetKeySequence() { QTreeWidgetItem *current = m_page->commandList->currentItem(); + if (current && current->data(0, Qt::UserRole).isValid()) { ShortcutItem *scitem = qVariantValue(current->data(0, Qt::UserRole)); setKeySequence(scitem->m_cmd->defaultKeySequence()); @@ -235,19 +238,22 @@ void ShortcutSettings::importAction() UniqueIDManager *uidm = UniqueIDManager::instance(); QString fileName = QFileDialog::getOpenFileName(0, tr("Import Keyboard Mapping Scheme"), - ICore::instance()->resourcePath() + "/schemes/", - tr("Keyboard Mapping Scheme (*.kms)")); + ICore::instance()->resourcePath() + "/schemes/", + tr("Keyboard Mapping Scheme (*.kms)")); + if (!fileName.isEmpty()) { CommandsFile cf(fileName); QMap mapping = cf.importCommands(); - foreach (ShortcutItem *item, m_scitems) { + foreach(ShortcutItem * item, m_scitems) { QString sid = uidm->stringForUniqueIdentifier(item->m_cmd->id()); + if (mapping.contains(sid)) { item->m_key = mapping.value(sid); item->m_item->setText(2, item->m_key); - if (item->m_item == m_page->commandList->currentItem()) + if (item->m_item == m_page->commandList->currentItem()) { commandChanged(item->m_item); + } } } } @@ -255,11 +261,12 @@ void ShortcutSettings::importAction() void ShortcutSettings::defaultAction() { - foreach (ShortcutItem *item, m_scitems) { + foreach(ShortcutItem * item, m_scitems) { item->m_key = item->m_cmd->defaultKeySequence(); item->m_item->setText(2, item->m_key); - if (item->m_item == m_page->commandList->currentItem()) + if (item->m_item == m_page->commandList->currentItem()) { commandChanged(item->m_item); + } } } @@ -283,17 +290,19 @@ void ShortcutSettings::initialize() m_am = ActionManagerPrivate::instance(); UniqueIDManager *uidm = UniqueIDManager::instance(); - foreach (Command *c, m_am->commands()) { - if (c->hasAttribute(Command::CA_NonConfigureable)) + foreach(Command * c, m_am->commands()) { + if (c->hasAttribute(Command::CA_NonConfigureable)) { continue; - if (c->action() && c->action()->isSeparator()) + } + if (c->action() && c->action()->isSeparator()) { continue; + } QTreeWidgetItem *item = 0; ShortcutItem *s = new ShortcutItem; m_scitems << s; - item = new QTreeWidgetItem(m_page->commandList); - s->m_cmd = c; + item = new QTreeWidgetItem(m_page->commandList); + s->m_cmd = c; s->m_item = item; item->setText(0, uidm->stringForUniqueIdentifier(c->id())); @@ -316,29 +325,31 @@ void ShortcutSettings::initialize() void ShortcutSettings::handleKeyEvent(QKeyEvent *e) { int nextKey = e->key(); - if ( m_keyNum > 3 || - nextKey == Qt::Key_Control || - nextKey == Qt::Key_Shift || - nextKey == Qt::Key_Meta || - nextKey == Qt::Key_Alt ) - return; + + if (m_keyNum > 3 || + nextKey == Qt::Key_Control || + nextKey == Qt::Key_Shift || + nextKey == Qt::Key_Meta || + nextKey == Qt::Key_Alt) { + return; + } nextKey |= translateModifiers(e->modifiers(), e->text()); switch (m_keyNum) { - case 0: - m_key[0] = nextKey; - break; - case 1: - m_key[1] = nextKey; - break; - case 2: - m_key[2] = nextKey; - break; - case 3: - m_key[3] = nextKey; - break; - default: - break; + case 0: + m_key[0] = nextKey; + break; + case 1: + m_key[1] = nextKey; + break; + case 2: + m_key[2] = nextKey; + break; + case 3: + m_key[3] = nextKey; + break; + default: + break; } m_keyNum++; QKeySequence ks(m_key[0], m_key[1], m_key[2], m_key[3]); @@ -350,18 +361,23 @@ int ShortcutSettings::translateModifiers(Qt::KeyboardModifiers state, const QString &text) { int result = 0; + // The shift modifier only counts when it is not used to type a symbol // that is only reachable using the shift key anyway if ((state & Qt::ShiftModifier) && (text.size() == 0 || !text.at(0).isPrint() || text.at(0).isLetter() - || text.at(0).isSpace())) + || text.at(0).isSpace())) { result |= Qt::SHIFT; - if (state & Qt::ControlModifier) + } + if (state & Qt::ControlModifier) { result |= Qt::CTRL; - if (state & Qt::MetaModifier) + } + if (state & Qt::MetaModifier) { result |= Qt::META; - if (state & Qt::AltModifier) + } + if (state & Qt::AltModifier) { result |= Qt::ALT; + } return result; } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.h b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.h index 762e397fd..7aac4c6d9 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/dialogs/shortcutsettings.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,24 +41,20 @@ class Ui_ShortcutSettings; QT_END_NAMESPACE namespace Core { - class Command; namespace Internal { - class ActionManagerPrivate; class MainWindow; -struct ShortcutItem -{ +struct ShortcutItem { Command *m_cmd; QKeySequence m_key; QTreeWidgetItem *m_item; }; -class ShortcutSettings : public Core::IOptionsPage -{ +class ShortcutSettings : public Core::IOptionsPage { Q_OBJECT public: @@ -101,7 +97,6 @@ private: int m_key[4], m_keyNum; Ui_ShortcutSettings *m_page; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.cpp b/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.cpp index 0b11fee49..6b2fc4179 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,8 +37,7 @@ using namespace Core::Internal; EventFilteringMainWindow::EventFilteringMainWindow() -{ -} +{} #ifdef Q_OS_WIN bool EventFilteringMainWindow::winEvent(MSG *msg, long *result) diff --git a/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.h b/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.h index 16ca657f2..3f0ddef43 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/eventfilteringmainwindow.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,14 +33,12 @@ namespace Core { namespace Internal { - /*! * This class only exists because we can't include windows.h in mainwindow.cpp * because windows defines an IContext... */ -class EventFilteringMainWindow : public QMainWindow -{ +class EventFilteringMainWindow : public QMainWindow { Q_OBJECT public: EventFilteringMainWindow(); @@ -52,9 +50,7 @@ protected: signals: void deviceChange(); - }; - } // Internal } // Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.cpp b/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.cpp index fea7d26fd..f6ce52c7a 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,15 +38,15 @@ using namespace Core; using namespace Internal; -static const char* const svgIdButtonBase = "ButtonBase"; -static const char* const svgIdButtonNormalBase = "ButtonNormalBase"; -static const char* const svgIdButtonNormalOverlay = "ButtonNormalOverlay"; -static const char* const svgIdButtonPressedBase = "ButtonPressedBase"; -static const char* const svgIdButtonPressedOverlay = "ButtonPressedOverlay"; -static const char* const svgIdButtonDisabledOverlay = "ButtonDisabledOverlay"; -static const char* const svgIdButtonHoverOverlay = "ButtonHoverOverlay"; +static const char *const svgIdButtonBase = "ButtonBase"; +static const char *const svgIdButtonNormalBase = "ButtonNormalBase"; +static const char *const svgIdButtonNormalOverlay = "ButtonNormalOverlay"; +static const char *const svgIdButtonPressedBase = "ButtonPressedBase"; +static const char *const svgIdButtonPressedOverlay = "ButtonPressedOverlay"; +static const char *const svgIdButtonDisabledOverlay = "ButtonDisabledOverlay"; +static const char *const svgIdButtonHoverOverlay = "ButtonHoverOverlay"; -static const char* const elementsSvgIds[] = { +static const char *const elementsSvgIds[] = { svgIdButtonBase, svgIdButtonNormalBase, svgIdButtonNormalOverlay, @@ -59,9 +59,10 @@ static const char* const elementsSvgIds[] = { const QMap &buttonElementsMap() { static QMap result; + if (result.isEmpty()) { QSvgRenderer renderer(QLatin1String(":/fancyactionbar/images/fancytoolbutton.svg")); - for (size_t i = 0; i < sizeof(elementsSvgIds)/sizeof(elementsSvgIds[0]); i++) { + for (size_t i = 0; i < sizeof(elementsSvgIds) / sizeof(elementsSvgIds[0]); i++) { QString elementId(elementsSvgIds[i]); QPicture elementPicture; QPainter elementPainter(&elementPicture); @@ -93,19 +94,20 @@ void FancyToolButton::paintEvent(QPaintEvent *event) p.drawPicture(0, 0, m_buttonElements.value(svgIdButtonBase)); p.drawPicture(0, 0, m_buttonElements.value(isDown() ? svgIdButtonPressedBase : svgIdButtonNormalBase)); #ifndef Q_WS_MAC // Mac UIs usually don't hover - if (underMouse() && isEnabled()) + if (underMouse() && isEnabled()) { p.drawPicture(0, 0, m_buttonElements.value(svgIdButtonHoverOverlay)); + } #endif - if (scale < 1) + if (scale < 1) { p.restore(); + } if (!icon().isNull()) { icon().paint(&p, rect()); } else { const int margin = 4; p.drawText(rect().adjusted(margin, margin, -margin, -margin), Qt::AlignCenter | Qt::TextWordWrap, text()); - } if (scale < 1) { @@ -150,29 +152,31 @@ FancyActionBar::FancyActionBar(QWidget *parent) void FancyActionBar::insertAction(int index, QAction *action, QMenu *menu) { FancyToolButton *toolButton = new FancyToolButton(this); + toolButton->setDefaultAction(action); if (menu) { toolButton->setMenu(menu); toolButton->setPopupMode(QToolButton::DelayedPopup); // execute action also if a context menu item is select - connect(toolButton, SIGNAL(triggered(QAction*)), - this, SLOT(toolButtonContextMenuActionTriggered(QAction*)), + connect(toolButton, SIGNAL(triggered(QAction *)), + this, SLOT(toolButtonContextMenuActionTriggered(QAction *)), Qt::QueuedConnection); } m_actionsLayout->insertWidget(index, toolButton); } /* - This slot is invoked when a context menu action of a tool button is triggered. - In this case we also want to trigger the default action of the button. + This slot is invoked when a context menu action of a tool button is triggered. + In this case we also want to trigger the default action of the button. - This allows the user e.g. to select and run a specific run configuration with one click. - */ -void FancyActionBar::toolButtonContextMenuActionTriggered(QAction* action) + This allows the user e.g. to select and run a specific run configuration with one click. + */ +void FancyActionBar::toolButtonContextMenuActionTriggered(QAction *action) { - if (QToolButton *button = qobject_cast(sender())) { - if (action != button->defaultAction()) + if (QToolButton * button = qobject_cast(sender())) { + if (action != button->defaultAction()) { button->defaultAction()->trigger(); + } } } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.h b/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.h index 1b61edba2..147474ef4 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/fancyactionbar.h @@ -4,7 +4,7 @@ * @file fancyactionbar.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup CorePlugin Core Plugin @@ -12,18 +12,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,9 +40,7 @@ QT_END_NAMESPACE namespace Core { namespace Internal { - -class FancyToolButton : public QToolButton -{ +class FancyToolButton : public QToolButton { public: FancyToolButton(QWidget *parent = 0); @@ -54,8 +52,7 @@ private: const QMap &m_buttonElements; }; -class FancyActionBar : public QWidget -{ +class FancyActionBar : public QWidget { Q_OBJECT public: @@ -65,11 +62,10 @@ public: void insertAction(int index, QAction *action, QMenu *menu = 0); private slots: - void toolButtonContextMenuActionTriggered(QAction*); + void toolButtonContextMenuActionTriggered(QAction *); private: QVBoxLayout *m_actionsLayout; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.cpp b/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.cpp index 1a4ca10a5..3e82d40c5 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -47,15 +47,15 @@ using namespace Core; using namespace Internal; -const int FancyTabBar::m_rounding = 22; +const int FancyTabBar::m_rounding = 22; const int FancyTabBar::m_textPadding = 4; FancyTabBar::FancyTabBar(QWidget *parent, bool isVertical) : QWidget(parent) { - verticalTabs = isVertical; + verticalTabs = isVertical; setIconSize(16); - m_hoverIndex = -1; + m_hoverIndex = -1; m_currentIndex = 0; if (isVertical) { setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); @@ -81,11 +81,12 @@ FancyTabBar::~FancyTabBar() QSize FancyTabBar::tabSizeHint(bool minimum) const { QFont boldFont(font()); + boldFont.setPointSizeF(Utils::StyleHelper::sidebarFontSize()); boldFont.setBold(true); QFontMetrics fm(boldFont); - int spacing = 6; - int width = 90 + spacing + 2; + int spacing = 6; + int width = 90 + spacing + 2; int iconHeight = minimum ? 0 : iconSize; return QSize(width, iconHeight + spacing + fm.height()); @@ -96,9 +97,11 @@ void FancyTabBar::paintEvent(QPaintEvent *event) Q_UNUSED(event) QPainter p(this); - for (int i = 0; i < count(); ++i) - if (i != currentIndex()) + for (int i = 0; i < count(); ++i) { + if (i != currentIndex()) { paintTab(&p, i); + } + } // paint active tab last, since it overlaps the neighbors paintTab(&p, currentIndex()); @@ -120,9 +123,9 @@ void FancyTabBar::mouseMoveEvent(QMouseEvent *e) m_hoverControl.stop(); m_hoverIndex = newHover; update(m_hoverRect); - m_hoverRect = QRect(); + m_hoverRect = QRect(); - if (m_hoverIndex >=0) { + if (m_hoverIndex >= 0) { QRect oldHoverRect = m_hoverRect; m_hoverRect = tabRect(m_hoverIndex); m_hoverControl.start(); @@ -136,7 +139,7 @@ bool FancyTabBar::event(QEvent *event) if (m_hoverIndex >= 0 && m_hoverIndex < m_tabs.count()) { QString tt = tabToolTip(m_hoverIndex); if (!tt.isEmpty()) { - QToolTip::showText(static_cast(event)->globalPos(), tt, this); + QToolTip::showText(static_cast(event)->globalPos(), tt, this); return true; } } @@ -153,7 +156,7 @@ void FancyTabBar::updateHover() void FancyTabBar::enterEvent(QEvent *e) { Q_UNUSED(e) - m_hoverRect = QRect(); + m_hoverRect = QRect(); m_hoverIndex = -1; } @@ -164,7 +167,7 @@ void FancyTabBar::leaveEvent(QEvent *e) if (m_hoverIndex >= 0) { m_hoverIndex = -1; update(m_hoverRect); - m_hoverRect = QRect(); + m_hoverRect = QRect(); } } @@ -177,16 +180,20 @@ void FancyTabBar::updateTabNameIcon(int index, const QIcon &icon, const QString QSize FancyTabBar::sizeHint() const { QSize sh = tabSizeHint(); - if (verticalTabs) + + if (verticalTabs) { return QSize(sh.width(), sh.height() * m_tabs.count()); + } return QSize(sh.width() * m_tabs.count(), sh.height()); } QSize FancyTabBar::minimumSizeHint() const { QSize sh = tabSizeHint(true); - if (verticalTabs) + + if (verticalTabs) { return QSize(sh.width(), sh.height() * m_tabs.count()); + } return QSize(sh.width() * m_tabs.count(), sh.height()); } @@ -195,18 +202,18 @@ QRect FancyTabBar::tabRect(int index) const QSize sh = tabSizeHint(); if (verticalTabs) { - if (sh.height() * m_tabs.count() > height()) + if (sh.height() * m_tabs.count() > height()) { sh.setHeight(height() / m_tabs.count()); + } return QRect(0, index * sh.height(), sh.width(), sh.height()); - } - if(sh.width() * m_tabs.count() > width()) + if (sh.width() * m_tabs.count() > width()) { sh.setWidth(width() / m_tabs.count()); + } return QRect(index * sh.width(), 0, sh.width(), sh.height()); - } void FancyTabBar::mousePressEvent(QMouseEvent *e) @@ -224,10 +231,10 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const { painter->save(); - QRect rect = tabRect(tabIndex); + QRect rect = tabRect(tabIndex); bool selected = (tabIndex == m_currentIndex); - bool hover = (tabIndex == m_hoverIndex); + bool hover = (tabIndex == m_hoverIndex); #ifdef Q_WS_MAC hover = false; // Dont hover on Mac @@ -241,13 +248,13 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const } QColor light = QColor(255, 255, 255, 40); - QColor dark = QColor(0, 0, 0, 60); + QColor dark = QColor(0, 0, 0, 60); if (selected) { QLinearGradient selectedGradient(rect.bottomRight(), QPoint(rect.center().x(), rect.top())); selectedGradient.setColorAt(0, Qt::white); selectedGradient.setColorAt(0.3, Qt::white); - selectedGradient.setColorAt(0.7, QColor(210, 210, 220)); //give a blue-ish color + selectedGradient.setColorAt(0.7, QColor(210, 210, 220)); // give a blue-ish color painter->fillRect(rect, selectedGradient); painter->setPen(QColor(200, 200, 200)); @@ -256,8 +263,9 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const painter->drawLine(rect.topRight(), rect.bottomRight()); } else { painter->fillRect(rect, background); - if (hover) + if (hover) { painter->fillRect(rect, hoverColor); + } painter->setPen(QPen(light, 0)); painter->drawLine(rect.topLeft(), rect.bottomLeft()); painter->setPen(QPen(dark, 0)); @@ -272,21 +280,23 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const boldFont.setBold(true); painter->setFont(boldFont); painter->setPen(selected ? Utils::StyleHelper::panelTextColor() : QColor(30, 30, 30, 80)); - int textFlags = Qt::AlignCenter | Qt::AlignBottom | Qt::ElideRight | Qt::TextWordWrap; + int textFlags = Qt::AlignCenter | Qt::AlignBottom | Qt::ElideRight | Qt::TextWordWrap; painter->drawText(tabTextRect, textFlags, tabText); painter->setPen(selected ? QColor(60, 60, 60) : Utils::StyleHelper::panelTextColor()); int textHeight = painter->fontMetrics().boundingRect(QRect(0, 0, width(), height()), Qt::TextWordWrap, tabText).height(); tabIconRect.adjust(0, 4, 0, -textHeight); - int iconSize = qMin(tabIconRect.width(), tabIconRect.height()); - if (iconSize > 4) + int iconSize = qMin(tabIconRect.width(), tabIconRect.height()); + if (iconSize > 4) { style()->drawItemPixmap(painter, tabIconRect, Qt::AlignCenter | Qt::AlignVCenter, tabIcon(tabIndex).pixmap(tabIconRect.size())); + } painter->translate(0, -1); painter->drawText(tabTextRect, textFlags, tabText); painter->restore(); } -void FancyTabBar::setCurrentIndex(int index) { +void FancyTabBar::setCurrentIndex(int index) +{ m_currentIndex = index; update(); emit currentChanged(index); @@ -296,19 +306,19 @@ void FancyTabBar::setCurrentIndex(int index) { // FancyColorButton ////// -class FancyColorButton : public QWidget -{ +class FancyColorButton : public QWidget { public: FancyColorButton(QWidget *parent) - : m_parent(parent) + : m_parent(parent) { setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Preferred); } void mousePressEvent(QMouseEvent *ev) { - if (ev->modifiers() & Qt::ShiftModifier) + if (ev->modifiers() & Qt::ShiftModifier) { Utils::StyleHelper::setBaseColor(QColorDialog::getColor(Utils::StyleHelper::baseColor(), m_parent)); + } } private: QWidget *m_parent; @@ -375,22 +385,24 @@ FancyTabWidget::FancyTabWidget(QWidget *parent, bool isVertical) selectionLayout->addWidget(m_cornerWidgetContainer, 0); m_modesStack = new QStackedLayout; - m_statusBar = new QStatusBar; + m_statusBar = new QStatusBar; m_statusBar->setSizePolicy(QSizePolicy::Ignored, QSizePolicy::Fixed); QVBoxLayout *vlayout = new QVBoxLayout; vlayout->setMargin(0); vlayout->setSpacing(0); vlayout->addLayout(m_modesStack); - if (!isVertical) + if (!isVertical) { vlayout->addWidget(m_selectionWidget); -// vlayout->addWidget(m_statusBar); //status bar is not used for now + } +// vlayout->addWidget(m_statusBar); //status bar is not used for now QHBoxLayout *mainLayout = new QHBoxLayout; mainLayout->setMargin(0); mainLayout->setSpacing(1); - if (isVertical) + if (isVertical) { mainLayout->addWidget(m_selectionWidget); + } mainLayout->addLayout(vlayout); setLayout(mainLayout); @@ -419,6 +431,7 @@ void FancyTabWidget::updateTabNameIcon(int index, const QIcon &icon, const QStri void FancyTabWidget::setBackgroundBrush(const QBrush &brush) { QPalette pal = m_tabBar->palette(); + pal.setBrush(QPalette::Mid, brush); m_tabBar->setPalette(pal); pal = m_cornerWidgetContainer->palette(); @@ -441,6 +454,7 @@ void FancyTabWidget::paintEvent(QPaintEvent *event) void FancyTabWidget::insertCornerWidget(int pos, QWidget *widget) { QHBoxLayout *layout = static_cast(m_cornerWidgetContainer->layout()); + layout->insertWidget(pos, widget); } @@ -472,6 +486,7 @@ void FancyTabWidget::setCurrentIndex(int index) void FancyTabWidget::showWidget(int index) { emit currentAboutToShow(index); + m_modesStack->setCurrentIndex(index); emit currentChanged(index); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.h b/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.h index 136d1b28e..d7ef0860e 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/fancytabwidget.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,19 +42,17 @@ QT_END_NAMESPACE namespace Core { namespace Internal { +struct FancyTab { + QIcon icon; + QString text; + QString toolTip; +}; - struct FancyTab { - QIcon icon; - QString text; - QString toolTip; - }; - -class FancyTabBar : public QWidget -{ +class FancyTabBar : public QWidget { Q_OBJECT public: - FancyTabBar(QWidget *parent = 0, bool isVertical=false); + FancyTabBar(QWidget *parent = 0, bool isVertical = false); ~FancyTabBar(); bool event(QEvent *event); @@ -69,28 +67,53 @@ public: QSize sizeHint() const; QSize minimumSizeHint() const; - void insertTab(int index, const QIcon &icon, const QString &label) { + void insertTab(int index, const QIcon &icon, const QString &label) + { FancyTab tab; + tab.icon = icon; tab.text = label; m_tabs.insert(index, tab); } - void removeTab(int index) { + void removeTab(int index) + { m_tabs.removeAt(index); - if ( m_currentIndex >= m_tabs.length() ) + if (m_currentIndex >= m_tabs.length()) { m_currentIndex = m_tabs.length() - 1; + } } void updateTabNameIcon(int index, const QIcon &icon, const QString &label); void setCurrentIndex(int index); - int currentIndex() const { return m_currentIndex; } + int currentIndex() const + { + return m_currentIndex; + } - void setTabToolTip(int index, QString toolTip) { m_tabs[index].toolTip = toolTip; } - QString tabToolTip(int index) const { return m_tabs.at(index).toolTip; } + void setTabToolTip(int index, QString toolTip) + { + m_tabs[index].toolTip = toolTip; + } + QString tabToolTip(int index) const + { + return m_tabs.at(index).toolTip; + } - void setIconSize(int s) { iconSize = s; } - QIcon tabIcon(int index) const {return m_tabs.at(index).icon; } - QString tabText(int index) const { return m_tabs.at(index).text; } - int count() const {return m_tabs.count(); } + void setIconSize(int s) + { + iconSize = s; + } + QIcon tabIcon(int index) const + { + return m_tabs.at(index).icon; + } + QString tabText(int index) const + { + return m_tabs.at(index).text; + } + int count() const + { + return m_tabs.count(); + } QRect tabRect(int index) const; @@ -112,11 +135,9 @@ private: bool verticalTabs; QSize tabSizeHint(bool minimum = false) const; - }; -class FancyTabWidget : public QWidget -{ +class FancyTabWidget : public QWidget { Q_OBJECT public: @@ -130,7 +151,10 @@ public: int cornerWidgetCount() const; void setTabToolTip(int index, const QString &toolTip); void updateTabNameIcon(int index, const QIcon &icon, const QString &label); - void setIconSize(int s) { m_tabBar->setIconSize(s); } + void setIconSize(int s) + { + m_tabBar->setIconSize(s); + } void paintEvent(QPaintEvent *event); @@ -154,7 +178,6 @@ private: QWidget *m_selectionWidget; QStatusBar *m_statusBar; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.cpp b/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.cpp index 432c33c7a..c5eea24fe 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,21 +36,20 @@ using namespace Core; /*! - \class FileIconProvider + \class FileIconProvider - Provides icons based on file suffixes. + Provides icons based on file suffixes. - The class is a singleton: It's instance can be accessed via the static instance() method. - Plugins can register custom icons via registerIconSuffix(), and retrieve icons via the icon() - method. - */ + The class is a singleton: It's instance can be accessed via the static instance() method. + Plugins can register custom icons via registerIconSuffix(), and retrieve icons via the icon() + method. + */ FileIconProvider *FileIconProvider::m_instance = 0; FileIconProvider::FileIconProvider() : m_unknownFileIcon(qApp->style()->standardIcon(QStyle::SP_FileIcon)) -{ -} +{} FileIconProvider::~FileIconProvider() { @@ -58,9 +57,9 @@ FileIconProvider::~FileIconProvider() } /*! - Returns the icon associated with the file suffix in fileInfo. If there is none, - the default icon of the operating system is returned. - */ + Returns the icon associated with the file suffix in fileInfo. If there is none, + the default icon of the operating system is returned. + */ QIcon FileIconProvider::icon(const QFileInfo &fileInfo) { const QString suffix = fileInfo.suffix(); @@ -75,8 +74,9 @@ QIcon FileIconProvider::icon(const QFileInfo &fileInfo) // This is incorrect if the OS does not always return the same icon for the // same suffix (Mac OS X), but should speed up the retrieval a lot ... icon = m_systemIconProvider.icon(fileInfo); - if (!suffix.isEmpty()) + if (!suffix.isEmpty()) { registerIconOverlayForSuffix(icon, suffix); + } #else if (fileInfo.isDir()) { icon = m_systemIconProvider.icon(fileInfo); @@ -90,25 +90,27 @@ QIcon FileIconProvider::icon(const QFileInfo &fileInfo) } /*! - Creates a pixmap with baseicon at size and overlays overlayIcon over it. - */ + Creates a pixmap with baseicon at size and overlays overlayIcon over it. + */ QPixmap FileIconProvider::overlayIcon(QStyle::StandardPixmap baseIcon, const QIcon &overlayIcon, const QSize &size) const { QPixmap iconPixmap = qApp->style()->standardIcon(baseIcon).pixmap(size); QPainter painter(&iconPixmap); + painter.drawPixmap(0, 0, overlayIcon.pixmap(size)); painter.end(); return iconPixmap; } /*! - Registers an icon for a given suffix, overlaying the system file icon. - */ + Registers an icon for a given suffix, overlaying the system file icon. + */ void FileIconProvider::registerIconOverlayForSuffix(const QIcon &icon, const QString &suffix) { QPixmap fileIconPixmap = overlayIcon(QStyle::SP_FileIcon, icon, QSize(16, 16)); + // delete old icon, if it exists - QList >::iterator iter = m_cache.begin(); + QList >::iterator iter = m_cache.begin(); for (; iter != m_cache.end(); ++iter) { if ((*iter).first == suffix) { iter = m_cache.erase(iter); @@ -116,32 +118,34 @@ void FileIconProvider::registerIconOverlayForSuffix(const QIcon &icon, const QSt } } - QPair newEntry(suffix, fileIconPixmap); + QPair newEntry(suffix, fileIconPixmap); m_cache.append(newEntry); } /*! - Registers an icon for all the suffixes of a given mime type, overlaying the system file icon. - */ + Registers an icon for all the suffixes of a given mime type, overlaying the system file icon. + */ void FileIconProvider::registerIconOverlayForMimeType(const QIcon &icon, const MimeType &mimeType) { - foreach (const QString &suffix, mimeType.suffixes()) - registerIconOverlayForSuffix(icon, suffix); + foreach(const QString &suffix, mimeType.suffixes()) + registerIconOverlayForSuffix(icon, suffix); } /*! - Returns an icon for the given suffix, or an empty one if none registered. - */ + Returns an icon for the given suffix, or an empty one if none registered. + */ QIcon FileIconProvider::iconForSuffix(const QString &suffix) const { QIcon icon; + #if defined(Q_WS_WIN) || defined(Q_WS_MAC) // On Windows and Mac we use the file system icons Q_UNUSED(suffix) #else - if (suffix.isEmpty()) + if (suffix.isEmpty()) { return icon; + } - QList >::const_iterator iter = m_cache.constBegin(); + QList >::const_iterator iter = m_cache.constBegin(); for (; iter != m_cache.constEnd(); ++iter) { if ((*iter).first == suffix) { icon = (*iter).second; @@ -153,11 +157,12 @@ QIcon FileIconProvider::iconForSuffix(const QString &suffix) const } /*! - Returns the sole instance of FileIconProvider. - */ + Returns the sole instance of FileIconProvider. + */ FileIconProvider *FileIconProvider::instance() { - if (!m_instance) + if (!m_instance) { m_instance = new FileIconProvider; + } return m_instance; } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.h b/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.h index a8ea8d776..0e06b7378 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/fileiconprovider.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,13 +38,10 @@ #include namespace Core { - class MimeType; -class CORE_EXPORT FileIconProvider -{ -public: - ~FileIconProvider(); // used to clear the cache +class CORE_EXPORT FileIconProvider { +public: ~FileIconProvider(); // used to clear the cache QIcon icon(const QFileInfo &fileInfo); QPixmap overlayIcon(QStyle::StandardPixmap baseIcon, const QIcon &overlayIcon, const QSize &size) const; @@ -67,7 +64,6 @@ private: FileIconProvider(); static FileIconProvider *m_instance; }; - } // namespace Core #endif // FILEICONPROVIDER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp index 016004bc6..a383264e4 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,15 +43,14 @@ using namespace Utils; using namespace Core::Internal; -GeneralSettings::GeneralSettings(): +GeneralSettings::GeneralSettings() : m_saveSettingsOnExit(true), m_autoConnect(true), m_autoSelect(true), m_useUDPMirror(false), m_useExpertMode(false), m_dialog(0) -{ -} +{} QString GeneralSettings::id() const { @@ -78,7 +77,8 @@ static bool hasQmFilesForLocale(const QString &locale, const QString &creatorTrP static const QString qtTrPath = QLibraryInfo::location(QLibraryInfo::TranslationsPath); const QString trFile = QLatin1String("qt_") + locale + QLatin1String(".qm"); - return QFile::exists(qtTrPath+'/'+trFile) || QFile::exists(creatorTrPath+'/'+trFile); + + return QFile::exists(qtTrPath + '/' + trFile) || QFile::exists(creatorTrPath + '/' + trFile); } void GeneralSettings::fillLanguageBox() const @@ -92,13 +92,14 @@ void GeneralSettings::fillLanguageBox() const m_page->languageBox->setCurrentIndex(m_page->languageBox->count() - 1); } - const QString creatorTrPath = Core::ICore::instance()->resourcePath() + QLatin1String("/translations"); + const QString creatorTrPath = Core::ICore::instance()->resourcePath() + QLatin1String("/translations"); const QStringList languageFiles = QDir(creatorTrPath).entryList(QStringList(QLatin1String("openpilotgcs*.qm"))); foreach(QString languageFile, languageFiles) { int start = languageFile.indexOf(QLatin1Char('_')) + 1; - int end = languageFile.lastIndexOf(QLatin1Char('.')); + int end = languageFile.lastIndexOf(QLatin1Char('.')); const QString locale = languageFile.mid(start, end - start); + // no need to show a language that creator will not load anyway if (hasQmFilesForLocale(locale, creatorTrPath)) { m_page->languageBox->addItem(QLocale::languageToString(QLocale(locale).language()), locale); @@ -134,15 +135,16 @@ QWidget *GeneralSettings::createPage(QWidget *parent) void GeneralSettings::apply() { int currentIndex = m_page->languageBox->currentIndex(); + setLanguage(m_page->languageBox->itemData(currentIndex, Qt::UserRole).toString()); // Apply the new base color if accepted StyleHelper::setBaseColor(m_page->colorButton->color()); m_saveSettingsOnExit = m_page->checkBoxSaveOnExit->isChecked(); - m_useUDPMirror=m_page->cbUseUDPMirror->isChecked(); - m_useExpertMode=m_page->cbExpertMode->isChecked(); - m_autoConnect = m_page->checkAutoConnect->isChecked(); - m_autoSelect = m_page->checkAutoSelect->isChecked(); + m_useUDPMirror = m_page->cbUseUDPMirror->isChecked(); + m_useExpertMode = m_page->cbExpertMode->isChecked(); + m_autoConnect = m_page->checkAutoConnect->isChecked(); + m_autoSelect = m_page->checkAutoSelect->isChecked(); } void GeneralSettings::finish() @@ -153,11 +155,11 @@ void GeneralSettings::finish() void GeneralSettings::readSettings(QSettings *qs) { qs->beginGroup(QLatin1String("General")); - m_language = qs->value(QLatin1String("OverrideLanguage"), QLocale::system().name()).toString(); + m_language = qs->value(QLatin1String("OverrideLanguage"), QLocale::system().name()).toString(); m_saveSettingsOnExit = qs->value(QLatin1String("SaveSettingsOnExit"), m_saveSettingsOnExit).toBool(); - m_autoConnect = qs->value(QLatin1String("AutoConnect"), m_autoConnect).toBool(); - m_autoSelect = qs->value(QLatin1String("AutoSelect"), m_autoSelect).toBool(); - m_useUDPMirror = qs->value(QLatin1String("UDPMirror"), m_useUDPMirror).toBool(); + m_autoConnect = qs->value(QLatin1String("AutoConnect"), m_autoConnect).toBool(); + m_autoSelect = qs->value(QLatin1String("AutoSelect"), m_autoSelect).toBool(); + m_useUDPMirror = qs->value(QLatin1String("UDPMirror"), m_useUDPMirror).toBool(); m_useExpertMode = qs->value(QLatin1String("ExpertMode"), m_useExpertMode).toBool(); qs->endGroup(); } @@ -168,8 +170,7 @@ void GeneralSettings::saveSettings(QSettings *qs) if (m_language.isEmpty()) { qs->remove(QLatin1String("OverrideLanguage")); - } - else { + } else { qs->setValue(QLatin1String("OverrideLanguage"), m_language); } @@ -196,10 +197,10 @@ void GeneralSettings::showHelpForExternalEditor() } #if 0 QMessageBox *mb = new QMessageBox(QMessageBox::Information, - tr("Variables"), - EditorManager::instance()->externalEditorHelpText(), - QMessageBox::Cancel, - m_page->helpExternalEditorButton); + tr("Variables"), + EditorManager::instance()->externalEditorHelpText(), + QMessageBox::Cancel, + m_page->helpExternalEditorButton); mb->setWindowModality(Qt::NonModal); m_dialog = mb; mb->show(); @@ -221,8 +222,8 @@ void GeneralSettings::setLanguage(const QString &locale) { if (m_language != locale) { if (!locale.isEmpty()) { - QMessageBox::information((QWidget*) Core::ICore::instance()->mainWindow(), tr("Restart required"), - tr("The language change will take effect after a restart of the OpenPilot GCS.")); + QMessageBox::information((QWidget *)Core::ICore::instance()->mainWindow(), tr("Restart required"), + tr("The language change will take effect after a restart of the OpenPilot GCS.")); } m_language = locale; } @@ -255,10 +256,9 @@ bool GeneralSettings::useExpertMode() const void GeneralSettings::slotAutoConnect(int value) { - if (value==Qt::Checked) { + if (value == Qt::Checked) { m_page->checkAutoSelect->setEnabled(false); - } - else { + } else { m_page->checkAutoSelect->setEnabled(true); } } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h index dd585c304..f9b909333 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,13 +36,11 @@ namespace Core { namespace Internal { - namespace Ui { - class GeneralSettings; +class GeneralSettings; } -class CORE_EXPORT GeneralSettings : public IOptionsPage -{ +class CORE_EXPORT GeneralSettings : public IOptionsPage { Q_OBJECT public: @@ -52,15 +50,15 @@ public: QString trName() const; QString category() const; QString trCategory() const; - QWidget* createPage(QWidget *parent); + QWidget *createPage(QWidget *parent); void apply(); void finish(); bool saveSettingsOnExit() const; bool autoConnect() const; bool autoSelect() const; bool useUDPMirror() const; - void readSettings(QSettings* qs); - void saveSettings(QSettings* qs); + void readSettings(QSettings *qs); + void saveSettings(QSettings *qs); bool useExpertMode() const; signals: @@ -73,7 +71,7 @@ private slots: private: void fillLanguageBox() const; QString language() const; - void setLanguage(const QString&); + void setLanguage(const QString &); Ui::GeneralSettings *m_page; QString m_language; bool m_saveSettingsOnExit; @@ -83,9 +81,7 @@ private: bool m_useExpertMode; QPointer m_dialog; QList m_codecs; - }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iconfigurableplugin.h b/ground/openpilotgcs/src/plugins/coreplugin/iconfigurableplugin.h index 951530062..2dd4816a0 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iconfigurableplugin.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/iconfigurableplugin.h @@ -9,17 +9,14 @@ #include namespace Core { - -class CORE_EXPORT IConfigurablePlugin : public ExtensionSystem::IPlugin -{ +class CORE_EXPORT IConfigurablePlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: // IConfigurablePlugin(QObject *parent = 0){} virtual ~IConfigurablePlugin() {} - virtual void readConfig( QSettings* qSettings, UAVConfigInfo *configInfo) = 0; - virtual void saveConfig(QSettings* qSettings, Core::UAVConfigInfo *configInfo) = 0; + virtual void readConfig(QSettings *qSettings, UAVConfigInfo *configInfo) = 0; + virtual void saveConfig(QSettings *qSettings, Core::UAVConfigInfo *configInfo) = 0; }; - } // namespace Core #endif // ICONFIGURABLEPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iconnection.cpp b/ground/openpilotgcs/src/plugins/coreplugin/iconnection.cpp index 445591327..f89813214 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iconnection.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/iconnection.cpp @@ -27,4 +27,3 @@ */ #include "iconnection.h" - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iconnection.h b/ground/openpilotgcs/src/plugins/coreplugin/iconnection.h index 6bc35c18e..a5ebbc17d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iconnection.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/iconnection.h @@ -36,46 +36,52 @@ #include "core_global.h" namespace Core { - /** -* An IConnection object define a "type of connection", -* for instance USB, Serial, Network, ... -*/ -class CORE_EXPORT IConnection : public QObject -{ + * An IConnection object define a "type of connection", + * for instance USB, Serial, Network, ... + */ +class CORE_EXPORT IConnection : public QObject { Q_OBJECT public: /** - * Return the list of devices found on the system - */ - struct device - { + * Return the list of devices found on the system + */ + struct device { QString name; QString displayName; - bool operator==(device i){return this->name==i.name;} + bool operator==(device i) + { + return this->name == i.name; + } }; virtual QList availableDevices() = 0; /** - * Open a device, and return a QIODevice interface from it - * It should be a dynamically created object as it will be - * deleted by the connection manager. - */ + * Open a device, and return a QIODevice interface from it + * It should be a dynamically created object as it will be + * deleted by the connection manager. + */ virtual QIODevice *openDevice(const QString &deviceName) = 0; - virtual void closeDevice(const QString &deviceName) { Q_UNUSED(deviceName) }; + virtual void closeDevice(const QString &deviceName) + { + Q_UNUSED(deviceName) + }; /** - * Connection type name "USB HID" - */ + * Connection type name "USB HID" + */ virtual QString connectionName() = 0; /** - * Short name to display in a combo box - */ - virtual QString shortName() {return connectionName();} + * Short name to display in a combo box + */ + virtual QString shortName() + { + return connectionName(); + } /** * Manage whether the plugin is allowed to poll for devices @@ -86,11 +92,10 @@ public: signals: /** - * Available devices list has changed, signal it to connection manager (and whoever wants to know) - */ + * Available devices list has changed, signal it to connection manager (and whoever wants to know) + */ void availableDevChanged(IConnection *); }; - -} //namespace Core +} // namespace Core #endif // ICONNECTION_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/icontext.h b/ground/openpilotgcs/src/plugins/coreplugin/icontext.h index ca03b6902..d1d43e8aa 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/icontext.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/icontext.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,9 +37,7 @@ class QWidget; QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT IContext : public QObject -{ +class CORE_EXPORT IContext : public QObject { Q_OBJECT public: IContext(QObject *parent = 0) : QObject(parent) {} @@ -47,28 +45,34 @@ public: virtual QList context() const = 0; virtual QWidget *widget() = 0; - virtual QString contextHelpId() const { return QString(); } + virtual QString contextHelpId() const + { + return QString(); + } }; -class BaseContext : public Core::IContext -{ +class BaseContext : public Core::IContext { public: BaseContext(QWidget *widget, const QList &context, QObject *parent = 0) : Core::IContext(parent), m_widget(widget), m_context(context) + {} + + QList context() const { + return m_context; } - QList context() const { return m_context; } - - QWidget *widget() { return m_widget; } + QWidget *widget() + { + return m_widget; + } private: QWidget *m_widget; QList m_context; }; - } // namespace Core -#endif //ICONTEXT_H +#endif // ICONTEXT_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/icore.cpp b/ground/openpilotgcs/src/plugins/coreplugin/icore.cpp index 35030426c..8f4717fb2 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/icore.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/icore.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,12 +32,12 @@ \namespace Core \brief The Core namespace contains all classes that make up the Core plugin which constitute the basic functionality of the OpenPilot GCS. -*/ + */ /*! \namespace Core::Internal \internal -*/ + */ /*! \class Core::ICore @@ -49,7 +49,7 @@ from your plugin through \c{Core::instance()}. \mainclass -*/ + */ /*! \fn QStringList ICore::showNewItemDialog(const QString &title, @@ -64,7 +64,7 @@ current file. \sa Core::FileManager -*/ + */ /*! \fn bool ICore::showOptionsDialog(const QString &group = QString(), @@ -73,7 +73,7 @@ \a page in a specified \a group. The arguments refer to the string IDs of the corresponding IOptionsPage. -*/ + */ /*! \fn bool ICore::showWarningWithOptions(const QString &title, const QString &text, @@ -86,7 +86,7 @@ Should be used to display configuration errors and point users to the setting. Returns true if the settings dialog was accepted. -*/ + */ /*! @@ -95,21 +95,21 @@ The action manager is responsible for registration of menus and menu items and keyboard shortcuts. -*/ + */ /*! \fn FileManager *ICore::fileManager() const \brief Returns the application's file manager. The file manager keeps track of files for changes outside the application. -*/ + */ /*! \fn UniqueIDManager *ICore::uniqueIDManager() const \brief Returns the application's id manager. The unique ID manager transforms strings in unique integers and the other way round. -*/ + */ /*! \fn MessageManager *ICore::messageManager() const @@ -117,7 +117,7 @@ The message manager is the interface to the "General" output pane for general application debug messages. -*/ + */ /*! \fn ExtensionSystem::PluginManager *ICore::pluginManager() const @@ -125,7 +125,7 @@ The plugin manager handles the plugin life cycles and manages the common object pool. -*/ + */ /*! \fn EditorManager *ICore::editorManager() const @@ -134,8 +134,7 @@ The editor manager handles all editor related tasks like opening documents, the stack of currently open documents and the currently active document. -*/ - + */ /*! @@ -145,7 +144,7 @@ The variable manager is used to register application wide string variables like \c MY_PROJECT_DIR such that strings like \c{somecommand ${MY_PROJECT_DIR}/sub} can be resolved/expanded from anywhere in the application. -*/ + */ /*! \fn ThreadManager *ICore::threadManager() const @@ -154,7 +153,7 @@ The thread manager is used to manage application wide QThread objects, allowing certain critical objects to synchronize directly within the same real time thread - anywhere in the application. -*/ + */ /*! \fn ModeManager *ICore::modeManager() const @@ -164,14 +163,14 @@ that were added to the plugin manager's object pool as well as their buttons and the tool bar with the round buttons in the lower left corner of the OpenPilot GCS. -*/ + */ /*! \fn MimeDatabase *ICore::mimeDatabase() const \brief Returns the application's mime database. Use the mime database to manage mime types. -*/ + */ /*! \fn QSettings *ICore::settings(QSettings::UserScope scope) const @@ -189,7 +188,7 @@ functionality exists for internal purposes only. \see settingsDatabase() -*/ + */ /*! \fn SettingsDatabase *ICore::settingsDatabase() const @@ -200,7 +199,7 @@ are application wide. \see SettingsDatabase -*/ + */ /*! \fn QString ICore::resourcePath() const @@ -209,14 +208,14 @@ This abstraction is needed to avoid platform-specific code all over the place, since e.g. on Mac the resources are part of the application bundle. -*/ + */ /*! \fn QMainWindow *ICore::mainWindow() const \brief Returns the main application window. For use as dialog parent etc. -*/ + */ /*! \fn IContext *ICore::currentContextObject() const @@ -224,7 +223,7 @@ \sa ICore::addAdditionalContext() \sa ICore::addContextObject() -*/ + */ /*! \fn void ICore::addAdditionalContext(int context) @@ -237,7 +236,7 @@ \sa ICore::removeAdditionalContext() \sa ICore::hasContext() \sa ICore::updateContext() -*/ + */ /*! \fn void ICore::removeAdditionalContext(int context) @@ -249,7 +248,7 @@ \sa ICore::addAdditionalContext() \sa ICore::hasContext() \sa ICore::updateContext() -*/ + */ /*! \fn bool ICore::hasContext(int context) const @@ -257,7 +256,7 @@ \sa ICore::addAdditionalContext() \sa ICore::addContextObject() -*/ + */ /*! \fn void ICore::addContextObject(IContext *context) @@ -269,7 +268,7 @@ \sa ICore::removeContextObject() \sa ICore::addAdditionalContext() \sa ICore::currentContextObject() -*/ + */ /*! \fn void ICore::removeContextObject(IContext *context) @@ -278,8 +277,8 @@ \sa ICore::addContextObject() \sa ICore::addAdditionalContext() \sa ICore::currentContextObject() -} -*/ + } + */ /*! \fn void ICore::updateContext() @@ -287,29 +286,29 @@ \sa ICore::addAdditionalContext() \sa ICore::removeAdditionalContext() -*/ + */ /*! \fn void ICore::openFiles(const QStringList &fileNames) \brief Open all files from a list of \a fileNames like it would be done if they were given to the OpenPilot GCS on the command line, or they were opened via \gui{File|Open}. -*/ + */ /*! \fn ICore::ICore() \internal -*/ + */ /*! \fn ICore::~ICore() \internal -*/ + */ /*! \fn void ICore::coreOpened() \brief Emitted after all plugins have been loaded and the main window shown. -*/ + */ /*! \fn void ICore::saveSettingsRequested() @@ -317,13 +316,13 @@ should be saved to disk. At the moment that happens when the application is closed, and on \gui{Save All}. -*/ + */ /*! \fn void ICore::optionsDialogRequested() \brief Signal that allows plugins to perform actions just before the \gui{Tools|Options} dialog is shown. -*/ + */ /*! \fn void ICore::coreAboutToClose() @@ -332,16 +331,16 @@ The application is guaranteed to shut down after this signal is emitted. It's there as an addition to the usual plugin lifecycle methods, namely IPlugin::shutdown(), just for convenience. -*/ + */ /*! \fn void ICore::contextAboutToChange(Core::IContext *context) \brief Sent just before a new \a context becomes the current context (meaning that its widget got focus). -*/ + */ /*! \fn void ICore::contextChanged(Core::IContext *context) \brief Sent just after a new \a context became the current context (meaning that its widget got focus). -*/ + */ diff --git a/ground/openpilotgcs/src/plugins/coreplugin/icore.h b/ground/openpilotgcs/src/plugins/coreplugin/icore.h index f06479883..a0b67e820 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/icore.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/icore.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,7 +40,6 @@ template class QList; QT_END_NAMESPACE namespace Core { - class ActionManager; class IContext; class IWizard; @@ -56,8 +55,7 @@ class UAVGadgetManager; class UAVGadgetInstanceManager; class IConfigurablePlugin; -class CORE_EXPORT ICore : public QObject -{ +class CORE_EXPORT ICore : public QObject { Q_OBJECT public: @@ -71,16 +69,16 @@ public: QWidget *parent = 0) = 0; virtual bool showWarningWithOptions(const QString &title, const QString &text, - const QString &details = QString(), - const QString &settingsCategory = QString(), - const QString &settingsId = QString(), - QWidget *parent = 0) = 0; + const QString &details = QString(), + const QString &settingsCategory = QString(), + const QString &settingsId = QString(), + QWidget *parent = 0) = 0; - virtual ActionManager *actionManager() const = 0; - virtual UniqueIDManager *uniqueIDManager() const = 0; - virtual MessageManager *messageManager() const = 0; - virtual VariableManager *variableManager() const = 0; - virtual ThreadManager *threadManager() const = 0; + virtual ActionManager *actionManager() const = 0; + virtual UniqueIDManager *uniqueIDManager() const = 0; + virtual MessageManager *messageManager() const = 0; + virtual VariableManager *variableManager() const = 0; + virtual ThreadManager *threadManager() const = 0; virtual ModeManager *modeManager() const = 0; virtual ConnectionManager *connectionManager() const = 0; virtual UAVGadgetInstanceManager *uavGadgetInstanceManager() const = 0; @@ -88,23 +86,23 @@ public: virtual QSettings *settings(QSettings::Scope scope = QSettings::UserScope) const = 0; virtual SettingsDatabase *settingsDatabase() const = 0; - virtual void readMainSettings(QSettings* qs, bool workspaceDiffOnly = false) = 0; - virtual void saveMainSettings(QSettings* qs) = 0; - virtual void readSettings(IConfigurablePlugin* plugin, QSettings* qs = 0) = 0; - virtual void saveSettings(IConfigurablePlugin* plugin, QSettings* qs = 0) = 0; + virtual void readMainSettings(QSettings *qs, bool workspaceDiffOnly = false) = 0; + virtual void saveMainSettings(QSettings *qs) = 0; + virtual void readSettings(IConfigurablePlugin *plugin, QSettings *qs = 0) = 0; + virtual void saveSettings(IConfigurablePlugin *plugin, QSettings *qs = 0) = 0; virtual void deleteSettings() = 0; - virtual QString resourcePath() const = 0; + virtual QString resourcePath() const = 0; virtual QMainWindow *mainWindow() const = 0; // adds and removes additional active contexts, this context is appended to the // currently active contexts. call updateContext after changing - virtual IContext *currentContextObject() const = 0; - virtual void addAdditionalContext(int context) = 0; - virtual void removeAdditionalContext(int context) = 0; + virtual IContext *currentContextObject() const = 0; + virtual void addAdditionalContext(int context) = 0; + virtual void removeAdditionalContext(int context) = 0; virtual bool hasContext(int context) const = 0; - virtual void addContextObject(IContext *context) = 0; + virtual void addContextObject(IContext *context) = 0; virtual void removeContextObject(IContext *context) = 0; virtual void updateContext() = 0; @@ -120,7 +118,6 @@ signals: void contextAboutToChange(Core::IContext *context); void contextChanged(Core::IContext *context); }; - } // namespace Core #endif // ICORE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/icorelistener.h b/ground/openpilotgcs/src/plugins/coreplugin/icorelistener.h index 8f94fca4b..92c203fbb 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/icorelistener.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/icorelistener.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,39 +34,40 @@ namespace Core { /*! - \class Core::ICoreListener + \class Core::ICoreListener - \brief Provides a hook for plugins to veto on certain events emitted from -the core plugin. + \brief Provides a hook for plugins to veto on certain events emitted from + the core plugin. - You implement this interface if you want to prevent certain events from - occurring, e.g. if you want to prevent the closing of the whole application - or to prevent the closing of an editor window under certain conditions. + You implement this interface if you want to prevent certain events from + occurring, e.g. if you want to prevent the closing of the whole application + or to prevent the closing of an editor window under certain conditions. - If e.g. the application window requests a close, then first - ICoreListener::coreAboutToClose() is called (in arbitrary order) on all - registered objects implementing this interface. If one if these calls returns - false, the process is aborted and the event is ignored. If all calls return - true, the corresponding signal is emitted and the event is accepted/performed. + If e.g. the application window requests a close, then first + ICoreListener::coreAboutToClose() is called (in arbitrary order) on all + registered objects implementing this interface. If one if these calls returns + false, the process is aborted and the event is ignored. If all calls return + true, the corresponding signal is emitted and the event is accepted/performed. - Guidelines for implementing: - \list - \o Return false from the implemented method if you want to prevent the event. - \o You need to add your implementing object to the plugin managers objects: + Guidelines for implementing: + \list + \o Return false from the implemented method if you want to prevent the event. + \o You need to add your implementing object to the plugin managers objects: ExtensionSystem::PluginManager::instance()->addObject(yourImplementingObject); - \o Don't forget to remove the object again at deconstruction (e.g. in the destructor of + \o Don't forget to remove the object again at deconstruction (e.g. in the destructor of your plugin). -*/ -class CORE_EXPORT ICoreListener : public QObject -{ + */ +class CORE_EXPORT ICoreListener : public QObject { Q_OBJECT public: ICoreListener(QObject *parent = 0) : QObject(parent) {} virtual ~ICoreListener() {} - virtual bool coreAboutToClose() { return true; } + virtual bool coreAboutToClose() + { + return true; + } }; - } // namespace Core #endif // ICORELISTENER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/imode.h b/ground/openpilotgcs/src/plugins/coreplugin/imode.h index b5c7eeb1b..44d970677 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/imode.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/imode.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,21 +38,18 @@ class QIcon; QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT IMode : public IContext -{ +class CORE_EXPORT IMode : public IContext { Q_OBJECT public: IMode(QObject *parent = 0) : IContext(parent) {} virtual ~IMode() {} virtual QString name() const = 0; - virtual QIcon icon() const = 0; + virtual QIcon icon() const = 0; virtual int priority() const = 0; virtual void setPriority(int priority) = 0; virtual const char *uniqueModeName() const = 0; }; - } // namespace Core #endif // IMODE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/ioutputpane.h b/ground/openpilotgcs/src/plugins/coreplugin/ioutputpane.h index af166f4c2..aeb518b93 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/ioutputpane.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/ioutputpane.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,16 +40,14 @@ class QWidget; QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT IOutputPane : public QObject -{ +class CORE_EXPORT IOutputPane : public QObject { Q_OBJECT public: IOutputPane(QObject *parent = 0) : QObject(parent) {} virtual ~IOutputPane() {} virtual QWidget *outputWidget(QWidget *parent) = 0; - virtual QList toolBarWidgets() const = 0; + virtual QList toolBarWidgets() const = 0; virtual QString name() const = 0; // -1 don't show in statusBar @@ -60,18 +58,18 @@ public: virtual void visibilityChanged(bool visible) = 0; // This function is called to give the outputwindow focus - virtual void setFocus() = 0; + virtual void setFocus() = 0; // Wheter the outputpane has focus - virtual bool hasFocus() = 0; + virtual bool hasFocus() = 0; // Wheter the outputpane can be focused at the moment. // (E.g. the search result window doesn't want to be focussed if the are no results.) - virtual bool canFocus() = 0; + virtual bool canFocus() = 0; virtual bool canNavigate() = 0; - virtual bool canNext() = 0; + virtual bool canNext() = 0; virtual bool canPrevious() = 0; - virtual void goToNext() = 0; - virtual void goToPrev() = 0; + virtual void goToNext() = 0; + virtual void goToPrev() = 0; public slots: void popup() { @@ -108,7 +106,6 @@ signals: void togglePage(bool withFocusIfShown); void navigateStateUpdate(); }; - } // namespace Core #endif // IOUTPUTPANE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.cpp b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.cpp index d91877b54..4863c4a53 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.cpp @@ -25,4 +25,3 @@ * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.h b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.h index 6fbce59a0..98bc2e7ab 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadget.h @@ -40,42 +40,57 @@ class QComboBox; QT_END_NAMESPACE namespace Core { - class IUAVGadgetConfiguration; -class CORE_EXPORT IUAVGadget : public IContext -{ +class CORE_EXPORT IUAVGadget : public IContext { Q_OBJECT public: IUAVGadget(QString classId, QObject *parent = 0) : - IContext(parent), - m_classId(classId) { } + IContext(parent), + m_classId(classId) {} virtual ~IUAVGadget() {} - QList context() const { return m_context; } - void setContext(QList context) { m_context = context; } + QList context() const + { + return m_context; + } + void setContext(QList context) + { + m_context = context; + } virtual QWidget *widget() = 0; - virtual QComboBox *toolBar() { return 0; } - virtual QString contextHelpId() const { return QString(); } - QString classId() const { return m_classId; } + virtual QComboBox *toolBar() + { + return 0; + } + virtual QString contextHelpId() const + { + return QString(); + } + QString classId() const + { + return m_classId; + } - virtual IUAVGadgetConfiguration *activeConfiguration() { return 0; } - virtual void loadConfiguration(IUAVGadgetConfiguration*) { } - virtual void saveState(QSettings* /*qSettings*/) { } - virtual void restoreState(QByteArray) { } - virtual void restoreState(QSettings* /*qSettings*/) { } + virtual IUAVGadgetConfiguration *activeConfiguration() + { + return 0; + } + virtual void loadConfiguration(IUAVGadgetConfiguration *) {} + virtual void saveState(QSettings * /*qSettings*/) {} + virtual void restoreState(QByteArray) {} + virtual void restoreState(QSettings * /*qSettings*/) {} public slots: - virtual void configurationChanged(IUAVGadgetConfiguration* ) { } - virtual void configurationAdded(IUAVGadgetConfiguration*) { } - virtual void configurationToBeDeleted(IUAVGadgetConfiguration*) { } - virtual void configurationNameChanged(QString, QString) { } + virtual void configurationChanged(IUAVGadgetConfiguration *) {} + virtual void configurationAdded(IUAVGadgetConfiguration *) {} + virtual void configurationToBeDeleted(IUAVGadgetConfiguration *) {} + virtual void configurationNameChanged(QString, QString) {} private slots: private: QString m_classId; QList m_context; }; - } // namespace Core #endif // IUAVGADGET_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.cpp index 5dc7dff9c..0a1e09293 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.cpp @@ -10,18 +10,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,5 +35,4 @@ IUAVGadgetConfiguration::IUAVGadgetConfiguration(QString classId, QObject *paren m_classId(classId), m_name(tr("default")), m_provisionalName(tr("default")) -{ -} +{} diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.h index 38eeecd09..278987b12 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetconfiguration.h @@ -9,19 +9,19 @@ * @{ * @brief The Core GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,24 +34,46 @@ #include namespace Core { - class UAVConfigInfo; -class CORE_EXPORT IUAVGadgetConfiguration : public QObject -{ -Q_OBJECT +class CORE_EXPORT IUAVGadgetConfiguration : public QObject { + Q_OBJECT public: explicit IUAVGadgetConfiguration(QString classId, QObject *parent = 0); - QString classId() { return m_classId; } - QString name() { return m_name; } - void setName(QString name) { m_name = name; } - QString provisionalName() { return m_provisionalName; } - void setProvisionalName(QString name) { m_provisionalName = name; } - bool locked() const { return m_locked; } - void setLocked(bool locked) { m_locked = locked; } + QString classId() + { + return m_classId; + } + QString name() + { + return m_name; + } + void setName(QString name) + { + m_name = name; + } + QString provisionalName() + { + return m_provisionalName; + } + void setProvisionalName(QString name) + { + m_provisionalName = name; + } + bool locked() const + { + return m_locked; + } + void setLocked(bool locked) + { + m_locked = locked; + } - virtual void saveConfig(QSettings* /*settings*/) const {}; - virtual void saveConfig(QSettings* settings, UAVConfigInfo* /*configInfo*/) const { saveConfig(settings); } + virtual void saveConfig(QSettings * /*settings*/) const {}; + virtual void saveConfig(QSettings *settings, UAVConfigInfo * /*configInfo*/) const + { + saveConfig(settings); + } virtual IUAVGadgetConfiguration *clone() = 0; @@ -64,9 +86,7 @@ private: QString m_classId; QString m_name; QString m_provisionalName; - }; - } // namespace Core #endif // IUAVGADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetfactory.h b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetfactory.h index 801343f7c..61f7d803e 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/iuavgadgetfactory.h @@ -40,41 +40,65 @@ class QStringList; QT_END_NAMESPACE namespace Core { - class IUAVGadget; class IUAVGadgetConfiguration; class IOptionsPage; -class CORE_EXPORT IUAVGadgetFactory : public QObject -{ +class CORE_EXPORT IUAVGadgetFactory : public QObject { Q_OBJECT public: IUAVGadgetFactory(QString classId, QString name, QObject *parent = 0) : - QObject(parent), - m_classId(classId), - m_name(name), - m_icon(QIcon()), - m_singleConfigurationGadget(false) {} + QObject(parent), + m_classId(classId), + m_name(name), + m_icon(QIcon()), + m_singleConfigurationGadget(false) {} virtual ~IUAVGadgetFactory() {} virtual IUAVGadget *createGadget(QWidget *parent) = 0; - virtual IUAVGadgetConfiguration *createConfiguration(QSettings* /*qSettings*/) { return 0; } - virtual IUAVGadgetConfiguration *createConfiguration(QSettings* qs, UAVConfigInfo */*configInfo*/) { return createConfiguration(qs); } - virtual IOptionsPage *createOptionsPage(IUAVGadgetConfiguration */*config*/) { return 0; } - QString classId() const { return m_classId; } - QString name() const { return m_name; } - QIcon icon() const { return m_icon; } - bool isSingleConfigurationGadget() { return m_singleConfigurationGadget; } + virtual IUAVGadgetConfiguration *createConfiguration(QSettings * /*qSettings*/) + { + return 0; + } + virtual IUAVGadgetConfiguration *createConfiguration(QSettings *qs, UAVConfigInfo * /*configInfo*/) + { + return createConfiguration(qs); + } + virtual IOptionsPage *createOptionsPage(IUAVGadgetConfiguration * /*config*/) + { + return 0; + } + QString classId() const + { + return m_classId; + } + QString name() const + { + return m_name; + } + QIcon icon() const + { + return m_icon; + } + bool isSingleConfigurationGadget() + { + return m_singleConfigurationGadget; + } protected: - void setIcon(QIcon icon) { m_icon = icon; } - void setSingleConfigurationGadgetTrue() { m_singleConfigurationGadget = true; } + void setIcon(QIcon icon) + { + m_icon = icon; + } + void setSingleConfigurationGadgetTrue() + { + m_singleConfigurationGadget = true; + } private: QString m_classId; // unique class id QString m_name; // display name, should also be unique QIcon m_icon; bool m_singleConfigurationGadget; // true if there is exactly one configuration for this gadget }; - } // namespace Core #endif // IUAVGADGETFACTORY_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iversioncontrol.h b/ground/openpilotgcs/src/plugins/coreplugin/iversioncontrol.h index 43379e30b..991ca58a1 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iversioncontrol.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/iversioncontrol.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,9 +35,7 @@ #include namespace Core { - -class CORE_EXPORT IVersionControl : public QObject -{ +class CORE_EXPORT IVersionControl : public QObject { Q_OBJECT public: enum Operation { AddOperation, DeleteOperation, OpenOperation }; @@ -45,7 +43,7 @@ public: IVersionControl(QObject *parent = 0) : QObject(parent) {} virtual ~IVersionControl() {} - virtual QString name() const = 0; + virtual QString name() const = 0; virtual bool isEnabled() const = 0; @@ -84,7 +82,7 @@ public: * * \note The EditorManager calls this for the editors. */ - virtual bool vcsOpen(const QString &fileName) = 0; + virtual bool vcsOpen(const QString &fileName) = 0; /*! * Called after a file has been added to a project If the version control @@ -94,7 +92,7 @@ public: * \note This function should be called from IProject subclasses after * files are added to the project. */ - virtual bool vcsAdd(const QString &filename) = 0; + virtual bool vcsAdd(const QString &filename) = 0; /*! * Called after a file has been removed from the project (if the user @@ -112,7 +110,6 @@ signals: // TODO: ADD A WAY TO DETECT WHETHER A FILE IS MANAGED, e.g // virtual bool sccManaged(const QString &filename) = 0; }; - } // namespace Core #endif // IVERSIONCONTROL_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/iview.h b/ground/openpilotgcs/src/plugins/coreplugin/iview.h index d36c297ba..28ffb3381 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/iview.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/iview.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,20 +34,17 @@ #include namespace Core { - -class CORE_EXPORT IView : public IContext -{ +class CORE_EXPORT IView : public IContext { Q_OBJECT public: - enum ViewPosition { First=0, Second=1, Third=2 }; + enum ViewPosition { First = 0, Second = 1, Third = 2 }; IView(QObject *parent = 0) : IContext(parent) {} virtual ~IView() {} - virtual const char *uniqueViewName() const = 0; + virtual const char *uniqueViewName() const = 0; virtual ViewPosition defaultPosition() const = 0; }; - } // namespace Core #endif // IVIEW_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp index 86176c9b2..d57393baf 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -104,7 +104,7 @@ MainWindow::MainWindow() : m_settings(new QSettings(XmlConfig::XmlSettingsFormat, QSettings::UserScope, QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS_config"), this)), m_globalSettings(new QSettings(XmlConfig::XmlSettingsFormat, QSettings::SystemScope, - QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS_config"), this)), + QLatin1String("OpenPilot"), QLatin1String("OpenPilotGCS_config"), this)), m_settingsDatabase(new SettingsDatabase(QFileInfo(m_settings->fileName()).path(), QLatin1String("OpenPilotGCS_config"), this)), @@ -149,10 +149,11 @@ MainWindow::MainWindow() : // Sometimes we get the standard windows 95 style as a fallback // e.g. if we are running on a KDE4 desktop QByteArray desktopEnvironment = qgetenv("DESKTOP_SESSION"); - if (desktopEnvironment == "kde") + if (desktopEnvironment == "kde") { baseName = QLatin1String("plastique"); - else + } else { baseName = QLatin1String("cleanlooks"); + } } #endif qApp->setStyle(new ManhattanStyle(baseName)); @@ -166,7 +167,7 @@ MainWindow::MainWindow() : registerDefaultActions(); m_modeStack = new MyTabWidget(this); - m_modeStack->setIconSize(QSize(24,24)); + m_modeStack->setIconSize(QSize(24, 24)); m_modeStack->setTabPosition(QTabWidget::South); m_modeStack->setMovable(false); m_modeStack->setMinimumWidth(512); @@ -178,18 +179,18 @@ MainWindow::MainWindow() : m_connectionManager = new ConnectionManager(this, m_modeStack); - m_messageManager = new MessageManager; + m_messageManager = new MessageManager; setCentralWidget(m_modeStack); - connect(QApplication::instance(), SIGNAL(focusChanged(QWidget*,QWidget*)), - this, SLOT(updateFocusWidget(QWidget*,QWidget*))); - connect(m_workspaceSettings, SIGNAL(tabBarSettingsApplied(QTabWidget::TabPosition,bool)), - this, SLOT(applyTabBarSettings(QTabWidget::TabPosition,bool))); - connect(m_modeManager, SIGNAL(newModeOrder(QVector)), m_workspaceSettings, SLOT(newModeOrder(QVector))); + connect(QApplication::instance(), SIGNAL(focusChanged(QWidget *, QWidget *)), + this, SLOT(updateFocusWidget(QWidget *, QWidget *))); + connect(m_workspaceSettings, SIGNAL(tabBarSettingsApplied(QTabWidget::TabPosition, bool)), + this, SLOT(applyTabBarSettings(QTabWidget::TabPosition, bool))); + connect(m_modeManager, SIGNAL(newModeOrder(QVector)), m_workspaceSettings, SLOT(newModeOrder(QVector))); statusBar()->setProperty("p_styled", true); setAcceptDrops(true); - foreach (QString engine, qxtLog->allLoggerEngines()) - qxtLog->removeLoggerEngine(engine); + foreach(QString engine, qxtLog->allLoggerEngines()) + qxtLog->removeLoggerEngine(engine); qxtLog->addLoggerEngine("std", new QxtBasicSTDLoggerEngine()); qxtLog->installAsMessageHandler(); qxtLog->enableAllLogLevels(); @@ -197,21 +198,19 @@ MainWindow::MainWindow() : MainWindow::~MainWindow() { - if (m_connectionManager) // Pip - { - m_connectionManager->disconnectDevice(); - m_connectionManager->suspendPolling(); - } + if (m_connectionManager) { // Pip + m_connectionManager->disconnectDevice(); + m_connectionManager->suspendPolling(); + } - hide(); + hide(); - qxtLog->removeAsMessageHandler(); - foreach (QString engine, qxtLog->allLoggerEngines()) - qxtLog->removeLoggerEngine(engine); + qxtLog->removeAsMessageHandler(); + foreach(QString engine, qxtLog->allLoggerEngines()) + qxtLog->removeLoggerEngine(engine); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); if (m_uavGadgetManagers.count() > 0) { - foreach (UAVGadgetManager *mode, m_uavGadgetManagers) - { + foreach(UAVGadgetManager * mode, m_uavGadgetManagers) { pm->removeObject(mode); delete mode; } @@ -221,11 +220,11 @@ MainWindow::~MainWindow() pm->removeObject(m_generalSettings); pm->removeObject(m_workspaceSettings); delete m_messageManager; - m_messageManager = 0; + m_messageManager = 0; delete m_shortcutSettings; - m_shortcutSettings = 0; + m_shortcutSettings = 0; delete m_generalSettings; - m_generalSettings = 0; + m_generalSettings = 0; delete m_workspaceSettings; m_workspaceSettings = 0; delete m_settings; @@ -235,10 +234,10 @@ MainWindow::~MainWindow() pm->removeObject(m_coreImpl); delete m_coreImpl; - m_coreImpl = 0; + m_coreImpl = 0; delete m_modeManager; - m_modeManager = 0; + m_modeManager = 0; delete m_mimeDatabase; m_mimeDatabase = 0; } @@ -247,7 +246,7 @@ bool MainWindow::init(QString *errorMessage) { Q_UNUSED(errorMessage) - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + ExtensionSystem::PluginManager * pm = ExtensionSystem::PluginManager::instance(); pm->addObject(m_coreImpl); m_modeManager->init(); m_connectionManager->init(); @@ -259,10 +258,8 @@ bool MainWindow::init(QString *errorMessage) return true; } -void MainWindow::modeChanged(Core::IMode */*mode*/) -{ - -} +void MainWindow::modeChanged(Core::IMode * /*mode*/) +{} void MainWindow::extensionsInitialized() { @@ -271,8 +268,8 @@ void MainWindow::extensionsInitialized() qs->beginGroup("General"); m_config_description = qs->value("Description", "none").toString(); - m_config_details = qs->value("Details", "none").toString(); - m_config_stylesheet = qs->value("StyleSheet", "none").toString(); + m_config_details = qs->value("Details", "none").toString(); + m_config_stylesheet = qs->value("StyleSheet", "none").toString(); qDebug() << "Configured style sheet:" << m_config_stylesheet; if (m_config_stylesheet == "wide") { @@ -281,7 +278,7 @@ void MainWindow::extensionsInitialized() } // Load common style sheet - QString style = loadStyleSheet(m_config_stylesheet + ".qss"); + QString style = loadStyleSheet(m_config_stylesheet + ".qss"); // Load and concatenate platform specific style sheet QString fileName = m_config_stylesheet; @@ -292,7 +289,7 @@ void MainWindow::extensionsInitialized() #else fileName += "_windows.qss"; #endif - style += loadStyleSheet(fileName); + style += loadStyleSheet(fileName); // We'll use qApp macro to get the QApplication pointer // and set the style sheet application wide. @@ -314,9 +311,11 @@ void MainWindow::extensionsInitialized() emit m_coreImpl->coreOpened(); } -QString MainWindow::loadStyleSheet(QString fileName) { +QString MainWindow::loadStyleSheet(QString fileName) +{ // Let's use QFile and point to a resource... QDir dir(QCoreApplication::applicationDirPath()); + #ifdef Q_OS_MAC dir.cdUp(); dir.cd("Resources"); @@ -336,8 +335,7 @@ QString MainWindow::loadStyleSheet(QString fileName) { // ...read file to a string. style = textStream.readAll(); file.close(); - } - else { + } else { qDebug() << "Failed to open style sheet file" << file.fileName(); } return style; @@ -353,7 +351,7 @@ void MainWindow::closeEvent(QCloseEvent *event) } const QList listeners = ExtensionSystem::PluginManager::instance()->getObjects(); - foreach (ICoreListener *listener, listeners) { + foreach(ICoreListener * listener, listeners) { if (!listener->coreAboutToClose()) { event->ignore(); return; @@ -372,14 +370,17 @@ void MainWindow::closeEvent(QCloseEvent *event) // Check for desktop file manager file drop events static bool isDesktopFileManagerDrop(const QMimeData *d, QStringList *files = 0) { - if (files) + if (files) { files->clear(); + } // Extract dropped files from Mime data. - if (!d->hasFormat(QLatin1String(uriListMimeFormatC))) + if (!d->hasFormat(QLatin1String(uriListMimeFormatC))) { return false; + } const QList urls = d->urls(); - if (urls.empty()) + if (urls.empty()) { return false; + } // Try to find local files bool hasFiles = false; const QList::const_iterator cend = urls.constEnd(); @@ -409,9 +410,10 @@ void MainWindow::dragEnterEvent(QDragEnterEvent *event) void MainWindow::dropEvent(QDropEvent *event) { QStringList files; + if (isDesktopFileManagerDrop(event->mimeData(), &files)) { event->accept(); - //openFiles(files); + // openFiles(files); } else { event->ignore(); } @@ -424,7 +426,7 @@ IContext *MainWindow::currentContextObject() const QStatusBar *MainWindow::statusBar() const { - return new QStatusBar();// m_modeStack->statusBar(); + return new QStatusBar(); // m_modeStack->statusBar(); } void MainWindow::registerDefaultContainers() @@ -494,6 +496,7 @@ void MainWindow::registerDefaultContainers() static Command *createSeparator(ActionManager *am, QObject *parent, const QString &name, const QList &context) { QAction *tmpaction = new QAction(parent); + tmpaction->setSeparator(true); Command *cmd = am->registerAction(tmpaction, name, context); return cmd; @@ -502,17 +505,18 @@ static Command *createSeparator(ActionManager *am, QObject *parent, const QStrin void MainWindow::registerDefaultActions() { ActionManagerPrivate *am = m_actionManager; - ActionContainer *mfile = am->actionContainer(Constants::M_FILE); - ActionContainer *medit = am->actionContainer(Constants::M_EDIT); - ActionContainer *mtools = am->actionContainer(Constants::M_TOOLS); + ActionContainer *mfile = am->actionContainer(Constants::M_FILE); + ActionContainer *medit = am->actionContainer(Constants::M_EDIT); + ActionContainer *mtools = am->actionContainer(Constants::M_TOOLS); ActionContainer *mwindow = am->actionContainer(Constants::M_WINDOW); - ActionContainer *mhelp = am->actionContainer(Constants::M_HELP); + ActionContainer *mhelp = am->actionContainer(Constants::M_HELP); // File menu separators Command *cmd = createSeparator(am, this, QLatin1String("QtCreator.File.Sep.Save"), m_globalContext); + mfile->addAction(cmd, Constants::G_FILE_SAVE); - cmd = createSeparator(am, this, QLatin1String("QtCreator.File.Sep.Close"), m_globalContext); + cmd = createSeparator(am, this, QLatin1String("QtCreator.File.Sep.Close"), m_globalContext); mfile->addAction(cmd, Constants::G_FILE_CLOSE); cmd = createSeparator(am, this, QLatin1String("QtCreator.File.Sep.Other"), m_globalContext); @@ -547,12 +551,12 @@ void MainWindow::registerDefaultActions() // New File Action /* - m_newAction = new QAction(QIcon(Constants::ICON_NEWFILE), tr("&New File or Project..."), this); - cmd = am->registerAction(m_newAction, Constants::NEW, m_globalContext); - cmd->setDefaultKeySequence(QKeySequence::New); - mfile->addAction(cmd, Constants::G_FILE_NEW); - connect(m_newAction, SIGNAL(triggered()), this, SLOT(newFile())); -*/ + m_newAction = new QAction(QIcon(Constants::ICON_NEWFILE), tr("&New File or Project..."), this); + cmd = am->registerAction(m_newAction, Constants::NEW, m_globalContext); + cmd->setDefaultKeySequence(QKeySequence::New); + mfile->addAction(cmd, Constants::G_FILE_NEW); + connect(m_newAction, SIGNAL(triggered()), this, SLOT(newFile())); + */ // Open Action /* @@ -561,7 +565,7 @@ void MainWindow::registerDefaultActions() cmd->setDefaultKeySequence(QKeySequence::Open); mfile->addAction(cmd, Constants::G_FILE_OPEN); connect(m_openAction, SIGNAL(triggered()), this, SLOT(openFile())); -*/ + */ // Open With Action /* @@ -569,14 +573,14 @@ void MainWindow::registerDefaultActions() cmd = am->registerAction(m_openWithAction, Constants::OPEN_WITH, m_globalContext); mfile->addAction(cmd, Constants::G_FILE_OPEN); connect(m_openWithAction, SIGNAL(triggered()), this, SLOT(openFileWith())); -*/ + */ - // File->Recent Files Menu + // File->Recent Files Menu /* ActionContainer *ac = am->createMenu(Constants::M_FILE_RECENTFILES); mfile->addMenu(ac, Constants::G_FILE_OPEN); ac->menu()->setTitle(tr("Recent Files")); -*/ + */ /* // Save Action QAction *tmpaction = new QAction(QIcon(Constants::ICON_SAVEFILE), tr("&Save"), this); @@ -589,13 +593,13 @@ void MainWindow::registerDefaultActions() // Save As Action tmpaction = new QAction(tr("Save &As..."), this); cmd = am->registerAction(tmpaction, Constants::SAVEAS, m_globalContext); -#ifdef Q_WS_MAC + #ifdef Q_WS_MAC cmd->setDefaultKeySequence(QKeySequence(tr("Ctrl+Shift+S"))); -#endif + #endif cmd->setAttribute(Command::CA_UpdateText); cmd->setDefaultText(tr("Save &As...")); mfile->addAction(cmd, Constants::G_FILE_SAVE); - */ + */ // SaveAll Action m_saveAllAction = new QAction(tr("Save &GCS Default Settings"), this); @@ -702,8 +706,8 @@ void MainWindow::registerDefaultActions() * UavGadgetManager Actions */ const QList uavGadgetManagerContext = - QList() << CoreImpl::instance()->uniqueIDManager()->uniqueIdentifier(Constants::C_UAVGADGETMANAGER); - //Window menu separators + QList() << CoreImpl::instance()->uniqueIDManager()->uniqueIdentifier(Constants::C_UAVGADGETMANAGER); + // Window menu separators QAction *tmpaction1 = new QAction(this); tmpaction1->setSeparator(true); cmd = am->registerAction(tmpaction1, QLatin1String("OpenPilot.Window.Sep.Split"), uavGadgetManagerContext); @@ -715,7 +719,7 @@ void MainWindow::registerDefaultActions() cmd->setDefaultKeySequence(QKeySequence(tr("Ctrl+Shift+F10"))); mwindow->addAction(cmd, Constants::G_WINDOW_HIDE_TOOLBAR); - //Window menu separators + // Window menu separators QAction *tmpaction2 = new QAction(this); tmpaction2->setSeparator(true); cmd = am->registerAction(tmpaction2, QLatin1String("OpenPilot.Window.Sep.Split2"), uavGadgetManagerContext); @@ -752,12 +756,12 @@ void MainWindow::registerDefaultActions() cmd->setDefaultKeySequence(QKeySequence(tr("%1+N").arg(prefix))); mwindow->addAction(cmd, Constants::G_WINDOW_SPLIT); - //Help Action + // Help Action tmpaction = new QAction(QIcon(Constants::ICON_HELP), tr("&Help..."), this); cmd = am->registerAction(tmpaction, Constants::G_HELP_HELP, m_globalContext); mhelp->addAction(cmd, Constants::G_HELP_HELP); tmpaction->setEnabled(true); - connect(tmpaction, SIGNAL(triggered()), this, SLOT(showHelp())); + connect(tmpaction, SIGNAL(triggered()), this, SLOT(showHelp())); // About sep #ifndef Q_WS_MAC // doesn't have the "About" actions in the Help menu @@ -777,9 +781,9 @@ void MainWindow::registerDefaultActions() #ifdef Q_WS_MAC #endif - connect(tmpaction, SIGNAL(triggered()), this, SLOT(aboutOpenPilotGCS())); + connect(tmpaction, SIGNAL(triggered()), this, SLOT(aboutOpenPilotGCS())); - //About Plugins Action + // About Plugins Action tmpaction = new QAction(QIcon(Constants::ICON_PLUGIN), tr("About &Plugins..."), this); cmd = am->registerAction(tmpaction, Constants::ABOUT_PLUGINS, m_globalContext); mhelp->addAction(cmd, Constants::G_HELP_ABOUT); @@ -787,9 +791,9 @@ void MainWindow::registerDefaultActions() #ifdef Q_WS_MAC cmd->action()->setMenuRole(QAction::ApplicationSpecificRole); #endif - connect(tmpaction, SIGNAL(triggered()), this, SLOT(aboutPlugins())); + connect(tmpaction, SIGNAL(triggered()), this, SLOT(aboutPlugins())); - //Credits Action + // Credits Action tmpaction = new QAction(QIcon(Constants::ICON_PLUGIN), tr("About &OpenPilot..."), this); cmd = am->registerAction(tmpaction, Constants::ABOUT_AUTHORS, m_globalContext); mhelp->addAction(cmd, Constants::G_HELP_ABOUT); @@ -797,29 +801,25 @@ void MainWindow::registerDefaultActions() #ifdef Q_WS_MAC cmd->action()->setMenuRole(QAction::ApplicationSpecificRole); #endif - connect(tmpaction, SIGNAL(triggered()), this, SLOT(aboutOpenPilotAuthors())); - - + connect(tmpaction, SIGNAL(triggered()), this, SLOT(aboutOpenPilotAuthors())); } void MainWindow::newFile() -{ -} +{} void MainWindow::openFile() -{ -} +{} /*static QList getNonEditorFileFactories() -{ + { QList tmp; return tmp; -} + } -static IFileFactory *findFileFactory(const QList &fileFactories, + static IFileFactory *findFileFactory(const QList &fileFactories, const MimeDatabase *db, const QFileInfo &fi) -{ + { if (const MimeType mt = db->findByFile(fi)) { const QString type = mt.type(); foreach (IFileFactory *factory, fileFactories) { @@ -828,11 +828,11 @@ static IFileFactory *findFileFactory(const QList &fileFactories, } } return 0; -} + } -// opens either an editor or loads a project -void MainWindow::openFiles(const QStringList &fileNames) -{ + // opens either an editor or loads a project + void MainWindow::openFiles(const QStringList &fileNames) + { QList nonEditorFileFactories = getNonEditorFileFactories(); foreach (const QString &fileName, fileNames) { @@ -844,16 +844,15 @@ void MainWindow::openFiles(const QStringList &fileNames) } } -}*/ + }*/ void MainWindow::setFocusToEditor() -{ - -} +{} bool MainWindow::showOptionsDialog(const QString &category, const QString &page, QWidget *parent) { emit m_coreImpl->optionsDialogRequested(); + if (!parent) { parent = this; } @@ -863,7 +862,9 @@ bool MainWindow::showOptionsDialog(const QString &category, const QString &page, void MainWindow::saveAll() { - if ( m_dontSaveSettings) return; + if (m_dontSaveSettings) { + return; + } emit m_coreImpl->saveSettingsRequested(); saveSettings(); // OpenPilot-specific. @@ -876,23 +877,23 @@ void MainWindow::exit() // since on close we are going to delete everything // so to prevent the deleting of that object we // just append it - QTimer::singleShot(0, this, SLOT(close())); + QTimer::singleShot(0, this, SLOT(close())); } void MainWindow::openFileWith() +{} + +void MainWindow::applyTabBarSettings(QTabWidget::TabPosition pos, bool movable) { - -} - -void MainWindow::applyTabBarSettings(QTabWidget::TabPosition pos, bool movable) { - if (m_modeStack->tabPosition() != pos) + if (m_modeStack->tabPosition() != pos) { m_modeStack->setTabPosition(pos); + } m_modeStack->setMovable(movable); } void MainWindow::showHelp() { - QDesktopServices::openUrl( QUrl(Constants::GCS_HELP, QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl(Constants::GCS_HELP, QUrl::StrictMode)); } ActionManager *MainWindow::actionManager() const @@ -912,20 +913,21 @@ MessageManager *MainWindow::messageManager() const QSettings *MainWindow::settings(QSettings::Scope scope) const { - if (scope == QSettings::UserScope) + if (scope == QSettings::UserScope) { return m_settings; - else + } else { return m_globalSettings; + } } VariableManager *MainWindow::variableManager() const { - return m_variableManager; + return m_variableManager; } ThreadManager *MainWindow::threadManager() const { - return m_threadManager; + return m_threadManager; } ConnectionManager *MainWindow::connectionManager() const @@ -933,7 +935,7 @@ ConnectionManager *MainWindow::connectionManager() const return m_connectionManager; } -QList MainWindow::uavGadgetManagers() const +QList MainWindow::uavGadgetManagers() const { return m_uavGadgetManagers; } @@ -954,7 +956,7 @@ MimeDatabase *MainWindow::mimeDatabase() const return m_mimeDatabase; } -GeneralSettings * MainWindow::generalSettings() const +GeneralSettings *MainWindow::generalSettings() const { return m_generalSettings; } @@ -966,43 +968,51 @@ IContext *MainWindow::contextObject(QWidget *widget) void MainWindow::addContextObject(IContext *context) { - if (!context) + if (!context) { return; + } QWidget *widget = context->widget(); - if (m_contextWidgets.contains(widget)) + if (m_contextWidgets.contains(widget)) { return; + } m_contextWidgets.insert(widget, context); } void MainWindow::removeContextObject(IContext *context) { - if (!context) + if (!context) { return; + } QWidget *widget = context->widget(); - if (!m_contextWidgets.contains(widget)) + if (!m_contextWidgets.contains(widget)) { return; + } m_contextWidgets.remove(widget); - if (m_activeContext == context) + if (m_activeContext == context) { updateContextObject(0); + } } void MainWindow::changeEvent(QEvent *e) { QMainWindow::changeEvent(e); + if (e->type() == QEvent::ActivationChange) { if (isActiveWindow()) { - if (debugMainWindow) + if (debugMainWindow) { qDebug() << "main window activated"; + } emit windowActivated(); } } else if (e->type() == QEvent::WindowStateChange) { #ifdef Q_WS_MAC bool minimized = isMinimized(); - if (debugMainWindow) + if (debugMainWindow) { qDebug() << "main window state changed to minimized=" << minimized; + } m_minimizeAction->setEnabled(!minimized); m_zoomAction->setEnabled(!minimized); #else @@ -1017,8 +1027,9 @@ void MainWindow::updateFocusWidget(QWidget *old, QWidget *now) Q_UNUSED(old) // Prevent changing the context object just because the menu is activated - if (qobject_cast(now)) + if (qobject_cast(now)) { return; + } IContext *newContext = 0; if (focusWidget()) { @@ -1038,16 +1049,18 @@ void MainWindow::updateFocusWidget(QWidget *old, QWidget *now) void MainWindow::updateContextObject(IContext *context) { - if (context == m_activeContext) + if (context == m_activeContext) { return; + } IContext *oldContext = m_activeContext; m_activeContext = context; if (!context || oldContext != m_activeContext) { emit m_coreImpl->contextAboutToChange(context); updateContext(); - if (debugMainWindow) + if (debugMainWindow) { qDebug() << "new context object =" << context << (context ? context->widget() : 0) << (context ? context->widget()->metaObject()->className() : 0); + } emit m_coreImpl->contextChanged(context); } } @@ -1059,8 +1072,8 @@ void MainWindow::resetContext() void MainWindow::shutdown() { - disconnect(QApplication::instance(), SIGNAL(focusChanged(QWidget*,QWidget*)), - this, SLOT(updateFocusWidget(QWidget*,QWidget*))); + disconnect(QApplication::instance(), SIGNAL(focusChanged(QWidget *, QWidget *)), + this, SLOT(updateFocusWidget(QWidget *, QWidget *))); m_activeContext = 0; // We have to remove all the existing gagdets at his point, not @@ -1079,21 +1092,23 @@ void MainWindow::showUavGadgetMenus(bool show, bool hasSplitter) m_gotoOtherSplitAction->setEnabled(show && hasSplitter); } -inline int takeLeastPriorityUavGadgetManager(const QList m_uavGadgetManagers) { +inline int takeLeastPriorityUavGadgetManager(const QList m_uavGadgetManagers) +{ int index = 0; - int prio = m_uavGadgetManagers.at(0)->priority(); + int prio = m_uavGadgetManagers.at(0)->priority(); + for (int i = 0; i < m_uavGadgetManagers.count(); i++) { int prio2 = m_uavGadgetManagers.at(i)->priority(); if (prio2 < prio) { - prio = prio2; + prio = prio2; index = i; } } return index; } -void MainWindow::createWorkspaces(QSettings* qs, bool diffOnly) { - +void MainWindow::createWorkspaces(QSettings *qs, bool diffOnly) +{ ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Core::UAVGadgetManager *uavGadgetManager; @@ -1101,12 +1116,14 @@ void MainWindow::createWorkspaces(QSettings* qs, bool diffOnly) { // If diffOnly is true, we only add/remove the number of workspaces // that has changed, // otherwise a complete reload of workspaces is done - int toRemoveFirst = m_uavGadgetManagers.count(); + int toRemoveFirst = m_uavGadgetManagers.count(); int newWorkspacesNo = m_workspaceSettings->numberOfWorkspaces(); - if (diffOnly && m_uavGadgetManagers.count() > newWorkspacesNo) + + if (diffOnly && m_uavGadgetManagers.count() > newWorkspacesNo) { toRemoveFirst = m_uavGadgetManagers.count() - newWorkspacesNo; - else + } else { toRemoveFirst = 0; + } int removed = 0; @@ -1132,11 +1149,11 @@ void MainWindow::createWorkspaces(QSettings* qs, bool diffOnly) { QElapsedTimer timer; timer.start(); - const QString name = m_workspaceSettings->name(i); + const QString name = m_workspaceSettings->name(i); const QString iconName = m_workspaceSettings->iconName(i); const QString modeName = m_workspaceSettings->modeName(i); uavGadgetManager = new Core::UAVGadgetManager(CoreImpl::instance(), name, QIcon(iconName), 90 - i + 1, modeName, - this); + this); connect(uavGadgetManager, SIGNAL(showUavGadgetMenus(bool, bool)), this, SLOT(showUavGadgetMenus(bool, bool))); @@ -1155,14 +1172,14 @@ void MainWindow::createWorkspaces(QSettings* qs, bool diffOnly) { qDebug() << "MainWindow::createWorkspaces - creating workspaces took" << totalTimer.elapsed() << "ms"; } -static const char *settingsGroup = "MainWindow"; -static const char *geometryKey = "Geometry"; +static const char *settingsGroup = "MainWindow"; +static const char *geometryKey = "Geometry"; static const char *colorKey = "Color"; static const char *maxKey = "Maximized"; -static const char *fullScreenKey = "FullScreen"; +static const char *fullScreenKey = "FullScreen"; static const char *modePriorities = "ModePriorities"; -void MainWindow::readSettings(QSettings* qs, bool workspaceDiffOnly) +void MainWindow::readSettings(QSettings *qs, bool workspaceDiffOnly) { if (!qs) { qs = m_settings; @@ -1201,17 +1218,16 @@ void MainWindow::readSettings(QSettings* qs, bool workspaceDiffOnly) qs->beginGroup(QLatin1String(modePriorities)); QStringList modeNames = qs->childKeys(); QMap map; - foreach (QString modeName, modeNames) { + foreach(QString modeName, modeNames) { map.insert(modeName, qs->value(modeName).toInt()); } m_modeManager->reorderModes(map); qs->endGroup(); - } -void MainWindow::saveSettings(QSettings* qs) +void MainWindow::saveSettings(QSettings *qs) { if (m_dontSaveSettings) { return; @@ -1228,8 +1244,8 @@ void MainWindow::saveSettings(QSettings* qs) qs->setValue(QLatin1String(colorKey), Utils::StyleHelper::baseColor()); if (windowState() & (Qt::WindowMaximized | Qt::WindowFullScreen)) { - qs->setValue(QLatin1String(maxKey), (bool) (windowState() & Qt::WindowMaximized)); - qs->setValue(QLatin1String(fullScreenKey), (bool) (windowState() & Qt::WindowFullScreen)); + qs->setValue(QLatin1String(maxKey), (bool)(windowState() & Qt::WindowMaximized)); + qs->setValue(QLatin1String(fullScreenKey), (bool)(windowState() & Qt::WindowFullScreen)); } else { qs->setValue(QLatin1String(maxKey), false); qs->setValue(QLatin1String(fullScreenKey), false); @@ -1240,49 +1256,48 @@ void MainWindow::saveSettings(QSettings* qs) // Write tab ordering qs->beginGroup(QLatin1String(modePriorities)); - QVector modes = m_modeManager->modes(); - foreach (IMode *mode, modes) { + QVector modes = m_modeManager->modes(); + foreach(IMode * mode, modes) { qs->setValue(QLatin1String(mode->uniqueModeName()), mode->priority()); } qs->endGroup(); - foreach (UAVGadgetManager *manager, m_uavGadgetManagers) { + foreach(UAVGadgetManager * manager, m_uavGadgetManagers) { manager->saveSettings(qs); } m_actionManager->saveSettings(qs); m_generalSettings->saveSettings(qs); qs->beginGroup("General"); - qs->setValue("Description",m_config_description); - qs->setValue("Details",m_config_details); - qs->setValue("StyleSheet",m_config_stylesheet); + qs->setValue("Description", m_config_description); + qs->setValue("Details", m_config_details); + qs->setValue("StyleSheet", m_config_stylesheet); qs->endGroup(); } -void MainWindow::readSettings(IConfigurablePlugin* plugin, QSettings* qs) +void MainWindow::readSettings(IConfigurablePlugin *plugin, QSettings *qs) { if (!qs) { qs = m_settings; } UAVConfigInfo configInfo; - QObject* qo = reinterpret_cast(plugin); + QObject *qo = reinterpret_cast(plugin); QString configName = qo->metaObject()->className(); qs->beginGroup("Plugins"); qs->beginGroup(configName); configInfo.read(qs); - configInfo.setNameOfConfigurable("Plugin-"+configName); + configInfo.setNameOfConfigurable("Plugin-" + configName); qs->beginGroup("data"); plugin->readConfig(qs, &configInfo); qs->endGroup(); qs->endGroup(); qs->endGroup(); - } -void MainWindow::saveSettings(IConfigurablePlugin* plugin, QSettings* qs) +void MainWindow::saveSettings(IConfigurablePlugin *plugin, QSettings *qs) { if (m_dontSaveSettings) { return; @@ -1303,7 +1318,6 @@ void MainWindow::saveSettings(IConfigurablePlugin* plugin, QSettings* qs) configInfo.save(qs); qs->endGroup(); qs->endGroup(); - } void MainWindow::deleteSettings() @@ -1315,21 +1329,25 @@ void MainWindow::deleteSettings() void MainWindow::addAdditionalContext(int context) { - if (context == 0) + if (context == 0) { return; + } - if (!m_additionalContexts.contains(context)) + if (!m_additionalContexts.contains(context)) { m_additionalContexts.prepend(context); + } } void MainWindow::removeAdditionalContext(int context) { - if (context == 0) + if (context == 0) { return; + } int index = m_additionalContexts.indexOf(context); - if (index != -1) + if (index != -1) { m_additionalContexts.removeAt(index); + } } bool MainWindow::hasContext(int context) const @@ -1341,16 +1359,18 @@ void MainWindow::updateContext() { QList contexts; - if (m_activeContext) + if (m_activeContext) { contexts += m_activeContext->context(); + } contexts += m_additionalContexts; QList uniquecontexts; for (int i = 0; i < contexts.size(); ++i) { const int c = contexts.at(i); - if (!uniquecontexts.contains(c)) + if (!uniquecontexts.contains(c)) { uniquecontexts << c; + } } m_actionManager->setContext(uniquecontexts); @@ -1360,6 +1380,7 @@ void MainWindow::aboutToShowRecentFiles() { ActionContainer *aci = m_actionManager->actionContainer(Constants::M_FILE_RECENTFILES); + if (aci) { aci->menu()->clear(); @@ -1371,12 +1392,13 @@ void MainWindow::aboutToShowRecentFiles() void MainWindow::openRecentFile() { - QAction *action = qobject_cast(sender()); - if (!action) + QAction *action = qobject_cast(sender()); + + if (!action) { return; - QString fileName = action->data().toString(); - if (!fileName.isEmpty()) { } + QString fileName = action->data().toString(); + if (!fileName.isEmpty()) {} } void MainWindow::aboutOpenPilotGCS() @@ -1419,22 +1441,24 @@ void MainWindow::destroyAuthorsDialog() void MainWindow::aboutPlugins() { PluginDialog dialog(this); + dialog.exec(); } void MainWindow::setFullScreen(bool on) { - if (bool(windowState() & Qt::WindowFullScreen) == on) + if (bool(windowState() & Qt::WindowFullScreen) == on) { return; + } if (on) { setWindowState(windowState() | Qt::WindowFullScreen); - //statusBar()->hide(); - //menuBar()->hide(); + // statusBar()->hide(); + // menuBar()->hide(); } else { setWindowState(windowState() & ~Qt::WindowFullScreen); - //menuBar()->show(); - //statusBar()->show(); + // menuBar()->show(); + // statusBar()->show(); } } @@ -1448,15 +1472,18 @@ bool MainWindow::showWarningWithOptions(const QString &title, const QString &settingsId, QWidget *parent) { - if (parent == 0) + if (parent == 0) { parent = this; + } QMessageBox msgBox(QMessageBox::Warning, title, text, QMessageBox::Ok, parent); - if (details.isEmpty()) + if (details.isEmpty()) { msgBox.setDetailedText(details); + } QAbstractButton *settingsButton = 0; - if (!settingsId.isEmpty() || !settingsCategory.isEmpty()) + if (!settingsId.isEmpty() || !settingsCategory.isEmpty()) { settingsButton = msgBox.addButton(tr("Settings..."), QMessageBox::AcceptRole); + } msgBox.exec(); if (settingsButton && msgBox.clickedButton() == settingsButton) { return showOptionsDialog(settingsCategory, settingsId); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.h b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.h index 641c8bf6c..a1aa1f508 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,7 +44,6 @@ class MyTabWidget; QT_END_NAMESPACE namespace Core { - class ActionManager; class BaseMode; class BaseView; @@ -67,7 +66,6 @@ class UAVGadgetInstanceManager; namespace Internal { - class ActionManagerPrivate; class CoreImpl; class FancyTabWidget; @@ -77,8 +75,7 @@ class WorkspaceSettings; class VersionDialog; class AuthorsDialog; -class CORE_EXPORT MainWindow : public EventFilteringMainWindow -{ +class CORE_EXPORT MainWindow : public EventFilteringMainWindow { Q_OBJECT public: @@ -93,17 +90,17 @@ public: void addContextObject(IContext *contex); void removeContextObject(IContext *contex); void resetContext(); - void readSettings(QSettings* qs = 0, bool workspaceDiffOnly = false); - void saveSettings(QSettings* qs = 0); - void readSettings(IConfigurablePlugin* plugin, QSettings* qs = 0); - void saveSettings(IConfigurablePlugin* plugin, QSettings* qs = 0); + void readSettings(QSettings *qs = 0, bool workspaceDiffOnly = false); + void saveSettings(QSettings *qs = 0); + void readSettings(IConfigurablePlugin *plugin, QSettings *qs = 0); + void saveSettings(IConfigurablePlugin *plugin, QSettings *qs = 0); void deleteSettings(); void openFiles(const QStringList &fileNames); Core::ActionManager *actionManager() const; Core::UniqueIDManager *uniqueIDManager() const; Core::MessageManager *messageManager() const; - QList uavGadgetManagers() const; + QList uavGadgetManagers() const; UAVGadgetInstanceManager *uavGadgetInstanceManager() const; Core::ConnectionManager *connectionManager() const; Core::VariableManager *variableManager() const; @@ -112,8 +109,11 @@ public: Core::MimeDatabase *mimeDatabase() const; Internal::GeneralSettings *generalSettings() const; QSettings *settings(QSettings::Scope scope) const; - inline SettingsDatabase *settingsDatabase() const { return m_settingsDatabase; } - IContext * currentContextObject() const; + inline SettingsDatabase *settingsDatabase() const + { + return m_settingsDatabase; + } + IContext *currentContextObject() const; QStatusBar *statusBar() const; void addAdditionalContext(int context); void removeAdditionalContext(int context); @@ -169,7 +169,7 @@ private: void updateContextObject(IContext *context); void registerDefaultContainers(); void registerDefaultActions(); - void createWorkspaces(QSettings* qs, bool diffOnly = false); + void createWorkspaces(QSettings *qs, bool diffOnly = false); QString loadStyleSheet(QString name); CoreImpl *m_coreImpl; @@ -185,7 +185,7 @@ private: VariableManager *m_variableManager; ThreadManager *m_threadManager; ModeManager *m_modeManager; - QList m_uavGadgetManagers; + QList m_uavGadgetManagers; UAVGadgetInstanceManager *m_uavGadgetInstanceManager; ConnectionManager *m_connectionManager; MimeDatabase *m_mimeDatabase; @@ -194,7 +194,7 @@ private: VersionDialog *m_versionDialog; AuthorsDialog *m_authorsDialog; - IContext * m_activeContext; + IContext *m_activeContext; QMap m_contextWidgets; @@ -226,9 +226,7 @@ private: QAction *m_minimizeAction; QAction *m_zoomAction; #endif - }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.cpp b/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.cpp index 2d15af167..a1c6a1c73 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.cpp @@ -10,19 +10,19 @@ * @{ * @brief The Core GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -65,10 +65,12 @@ bool styleEnabled(const QWidget *widget) { const QWidget *p = widget; + while (p) { - if (p->property("_q_custom_style_disabled").toBool()) + if (p->property("_q_custom_style_disabled").toBool()) { return false; - p = p->parentWidget(); + } + p = p->parentWidget(); } return true; } @@ -79,21 +81,21 @@ bool panelWidget(const QWidget *widget) const QWidget *p = widget; while (p) { - if (qobject_cast(p) && styleEnabled(p)) + if (qobject_cast(p) && styleEnabled(p)) { return true; - else if (qobject_cast(p) && styleEnabled(p)) + } else if (qobject_cast(p) && styleEnabled(p)) { return true; - else if (qobject_cast(p) && styleEnabled(p)) + } else if (qobject_cast(p) && styleEnabled(p)) { return true; - else if (p->property("panelwidget").toBool()) + } else if (p->property("panelwidget").toBool()) { return true; + } p = p->parentWidget(); } return false; } -class ManhattanStylePrivate -{ +class ManhattanStylePrivate { public: ManhattanStylePrivate(const QString &baseStyleName) { @@ -122,8 +124,7 @@ public: ManhattanStyle::ManhattanStyle(const QString &baseStyleName) : QWindowsStyle(), d(new ManhattanStylePrivate(baseStyleName)) -{ -} +{} ManhattanStyle::~ManhattanStyle() { @@ -142,57 +143,64 @@ void drawCornerImage(const QImage &img, QPainter *painter, QRect rect, int bottom = 0) { QSize size = img.size(); - if (top > 0) { //top - painter->drawImage(QRect(rect.left() + left, rect.top(), rect.width() -right - left, top), img, - QRect(left, 0, size.width() -right - left, top)); - if (left > 0) //top-left + + if (top > 0) { // top + painter->drawImage(QRect(rect.left() + left, rect.top(), rect.width() - right - left, top), img, + QRect(left, 0, size.width() - right - left, top)); + if (left > 0) { // top-left painter->drawImage(QRect(rect.left(), rect.top(), left, top), img, QRect(0, 0, left, top)); - if (right > 0) //top-right + } + if (right > 0) { // top-right painter->drawImage(QRect(rect.left() + rect.width() - right, rect.top(), right, top), img, QRect(size.width() - right, 0, right, top)); + } } - //left - if (left > 0) - painter->drawImage(QRect(rect.left(), rect.top()+top, left, rect.height() - top - bottom), img, + // left + if (left > 0) { + painter->drawImage(QRect(rect.left(), rect.top() + top, left, rect.height() - top - bottom), img, QRect(0, top, left, size.height() - bottom - top)); - //center - painter->drawImage(QRect(rect.left() + left, rect.top()+top, rect.width() -right - left, + } + // center + painter->drawImage(QRect(rect.left() + left, rect.top() + top, rect.width() - right - left, rect.height() - bottom - top), img, - QRect(left, top, size.width() -right -left, + QRect(left, top, size.width() - right - left, size.height() - bottom - top)); - if (right > 0) //right - painter->drawImage(QRect(rect.left() +rect.width() - right, rect.top()+top, right, rect.height() - top - bottom), img, + if (right > 0) { // right + painter->drawImage(QRect(rect.left() + rect.width() - right, rect.top() + top, right, rect.height() - top - bottom), img, QRect(size.width() - right, top, right, size.height() - bottom - top)); - if (bottom > 0) { //bottom - painter->drawImage(QRect(rect.left() +left, rect.top() + rect.height() - bottom, + } + if (bottom > 0) { // bottom + painter->drawImage(QRect(rect.left() + left, rect.top() + rect.height() - bottom, rect.width() - right - left, bottom), img, QRect(left, size.height() - bottom, size.width() - right - left, bottom)); - if (left > 0) //bottom-left - painter->drawImage(QRect(rect.left(), rect.top() + rect.height() - bottom, left, bottom), img, - QRect(0, size.height() - bottom, left, bottom)); - if (right > 0) //bottom-right - painter->drawImage(QRect(rect.left() + rect.width() - right, rect.top() + rect.height() - bottom, right, bottom), img, - QRect(size.width() - right, size.height() - bottom, right, bottom)); + if (left > 0) { // bottom-left + painter->drawImage(QRect(rect.left(), rect.top() + rect.height() - bottom, left, bottom), img, + QRect(0, size.height() - bottom, left, bottom)); + } + if (right > 0) { // bottom-right + painter->drawImage(QRect(rect.left() + rect.width() - right, rect.top() + rect.height() - bottom, right, bottom), img, + QRect(size.width() - right, size.height() - bottom, right, bottom)); + } } } QPixmap ManhattanStyle::generatedIconPixmap(QIcon::Mode iconMode, const QPixmap &pixmap, const QStyleOption *opt) const { QPixmap result; + result = d->style->generatedIconPixmap(iconMode, pixmap, opt); return result; } int ManhattanStyle::layoutSpacingImplementation(QSizePolicy::ControlType control1, - QSizePolicy::ControlType control2, - Qt::Orientation orientation, - const QStyleOption * option , - const QWidget * widget ) const + QSizePolicy::ControlType control2, + Qt::Orientation orientation, + const QStyleOption *option, + const QWidget *widget) const { return d->style->layoutSpacing(control1, control2, orientation, option, widget); - } QSize ManhattanStyle::sizeFromContents(ContentsType type, const QStyleOption *option, @@ -200,16 +208,18 @@ QSize ManhattanStyle::sizeFromContents(ContentsType type, const QStyleOption *op { QSize newSize = d->style->sizeFromContents(type, option, size, widget); - if (type == CT_Splitter && widget && widget->property("minisplitter").toBool()) + if (type == CT_Splitter && widget && widget->property("minisplitter").toBool()) { return QSize(1, 1); - else if (type == CT_ComboBox && panelWidget(widget)) + } else if (type == CT_ComboBox && panelWidget(widget)) { newSize += QSize(14, 0); + } return newSize; } QRect ManhattanStyle::subElementRect(SubElement element, const QStyleOption *option, const QWidget *widget) const { QRect rect; + rect = d->style->subElementRect(element, option, widget); return rect; } @@ -218,35 +228,30 @@ QRect ManhattanStyle::subControlRect(ComplexControl control, const QStyleOptionC SubControl subControl, const QWidget *widget) const { QRect rect; + #ifndef Q_WS_MACX // Not using OSX, size combo dropdown to fit contents - if(control == CC_ComboBox && subControl == SC_ComboBoxListBoxPopup) - { + if (control == CC_ComboBox && subControl == SC_ComboBoxListBoxPopup) { const QStyleOptionComboBox *cb = qstyleoption_cast(option); - const QComboBox* combo = qobject_cast(widget); + const QComboBox *combo = qobject_cast(widget); QRect comboRect = cb->rect; - int newWidth = combo->view()->sizeHintForColumn(0); - if(newWidth > comboRect.width()) - { + int newWidth = combo->view()->sizeHintForColumn(0); + if (newWidth > comboRect.width()) { // Set new rectangle, only width matters, list height is set by // combination of number of combo box items and setMaxVisibleItems rect.setRect(comboRect.x(), comboRect.y(), newWidth, comboRect.height()); rect = visualRect(cb->direction, cb->rect, rect); - } - else - { + } else { rect = d->style->subControlRect(control, option, subControl, widget); } - } - else - { + } else { rect = d->style->subControlRect(control, option, subControl, widget); } #else // Using OSX, use default style behaviour as this already sizes the // combo dropdown to fit rect = d->style->subControlRect(control, option, subControl, widget); -#endif +#endif // ifndef Q_WS_MACX return rect; } @@ -254,6 +259,7 @@ QStyle::SubControl ManhattanStyle::hitTestComplexControl(ComplexControl control, const QPoint &pos, const QWidget *widget) const { SubControl result = QStyle::SC_None; + result = d->style->hitTestComplexControl(control, option, pos, widget); return result; } @@ -261,34 +267,40 @@ QStyle::SubControl ManhattanStyle::hitTestComplexControl(ComplexControl control, int ManhattanStyle::pixelMetric(PixelMetric metric, const QStyleOption *option, const QWidget *widget) const { int retval = 0; + retval = d->style->pixelMetric(metric, option, widget); switch (metric) { case PM_SplitterWidth: - if (widget && widget->property("minisplitter").toBool()) + if (widget && widget->property("minisplitter").toBool()) { retval = 1; + } break; case PM_ToolBarIconSize: - if (panelWidget(widget)) + if (panelWidget(widget)) { retval = 16; + } break; case PM_MenuPanelWidth: case PM_MenuBarHMargin: case PM_MenuBarVMargin: case PM_ToolBarFrameWidth: - if (panelWidget(widget)) + if (panelWidget(widget)) { retval = 1; + } break; case PM_ButtonShiftVertical: case PM_ButtonShiftHorizontal: case PM_MenuBarPanelWidth: case PM_ToolBarItemMargin: case PM_ToolBarItemSpacing: - if (panelWidget(widget)) + if (panelWidget(widget)) { retval = 0; + } break; case PM_DefaultFrameWidth: - if (qobject_cast(widget) && panelWidget(widget)) + if (qobject_cast(widget) && panelWidget(widget)) { return 1; + } break; default: break; @@ -299,6 +311,7 @@ int ManhattanStyle::pixelMetric(PixelMetric metric, const QStyleOption *option, QPalette ManhattanStyle::standardPalette() const { QPalette result; + result = d->style->standardPalette(); return result; } @@ -317,6 +330,7 @@ QPalette panelPalette(const QPalette &oldPalette) { QColor color = Utils::StyleHelper::panelTextColor(); QPalette pal = oldPalette; + pal.setBrush(QPalette::All, QPalette::WindowText, color); pal.setBrush(QPalette::All, QPalette::ButtonText, color); pal.setBrush(QPalette::All, QPalette::Foreground, color); @@ -333,26 +347,25 @@ void ManhattanStyle::polish(QWidget *widget) // OxygenStyle forces a rounded widget mask on toolbars if (d->style->inherits("OxygenStyle")) { - if (qobject_cast(widget)) + if (qobject_cast(widget)) { widget->removeEventFilter(d->style); + } } if (panelWidget(widget)) { widget->setAttribute(Qt::WA_LayoutUsesWidgetRect, true); - if (qobject_cast(widget)) { + if (qobject_cast(widget)) { widget->setAttribute(Qt::WA_Hover); widget->setMaximumHeight(Utils::StyleHelper::navigationWidgetHeight() - 2); - } - else if (qobject_cast(widget)) { + } else if (qobject_cast(widget)) { widget->setAttribute(Qt::WA_Hover); widget->setMaximumHeight(Utils::StyleHelper::navigationWidgetHeight() - 2); - } - else if (qobject_cast(widget)) + } else if (qobject_cast(widget)) { widget->setPalette(panelPalette(widget->palette())); - else if (qobject_cast(widget) || widget->property("panelwidget_singlerow").toBool()) + } else if (qobject_cast(widget) || widget->property("panelwidget_singlerow").toBool()) { widget->setFixedHeight(Utils::StyleHelper::navigationWidgetHeight()); - else if (qobject_cast(widget)) + } else if (qobject_cast(widget)) { widget->setFixedHeight(Utils::StyleHelper::navigationWidgetHeight() + 2); - else if (qobject_cast(widget)) { + } else if (qobject_cast(widget)) { widget->setMaximumHeight(Utils::StyleHelper::navigationWidgetHeight() - 2); widget->setAttribute(Qt::WA_Hover); } @@ -364,12 +377,13 @@ void ManhattanStyle::unpolish(QWidget *widget) d->style->unpolish(widget); if (panelWidget(widget)) { widget->setAttribute(Qt::WA_LayoutUsesWidgetRect, false); - if (qobject_cast(widget)) + if (qobject_cast(widget)) { widget->setAttribute(Qt::WA_Hover, false); - else if (qobject_cast(widget)) + } else if (qobject_cast(widget)) { widget->setAttribute(Qt::WA_Hover, false); - else if (qobject_cast(widget)) + } else if (qobject_cast(widget)) { widget->setAttribute(Qt::WA_Hover, false); + } } } @@ -382,10 +396,12 @@ QIcon ManhattanStyle::standardIconImplementation(StandardPixmap standardIcon, co const QWidget *widget) const { QIcon icon; + switch (standardIcon) { case QStyle::SP_TitleBarCloseButton: case QStyle::SP_ToolBarHorizontalExtensionButton: return QIcon(standardPixmap(standardIcon, option, widget)); + default: icon = d->style->standardIcon(standardIcon, option, widget); } @@ -395,21 +411,24 @@ QIcon ManhattanStyle::standardIconImplementation(StandardPixmap standardIcon, co QPixmap ManhattanStyle::standardPixmap(StandardPixmap standardPixmap, const QStyleOption *opt, const QWidget *widget) const { - if (widget && !panelWidget(widget)) + if (widget && !panelWidget(widget)) { return d->style->standardPixmap(standardPixmap, opt, widget); + } QPixmap pixmap; switch (standardPixmap) { - case QStyle::SP_ToolBarHorizontalExtensionButton: { - static const QPixmap extButton(":/core/images/extension.png"); - pixmap = extButton; - } - break; - case QStyle::SP_TitleBarCloseButton: { - static const QPixmap closeButton(":/core/images/closebutton.png"); - pixmap = closeButton; - } - break; + case QStyle::SP_ToolBarHorizontalExtensionButton: + { + static const QPixmap extButton(":/core/images/extension.png"); + pixmap = extButton; + } + break; + case QStyle::SP_TitleBarCloseButton: + { + static const QPixmap closeButton(":/core/images/closebutton.png"); + pixmap = closeButton; + } + break; default: pixmap = d->style->standardPixmap(standardPixmap, opt, widget); } @@ -421,15 +440,18 @@ int ManhattanStyle::styleHint(StyleHint hint, const QStyleOption *option, const QStyleHintReturn *returnData) const { int ret = d->style->styleHint(hint, option, widget, returnData); + switch (hint) { // Make project explorer alternate rows all the way case QStyle::SH_ItemView_PaintAlternatingRowColorsForEmptyArea: - if (widget && widget->property("AlternateEmpty").toBool()) + if (widget && widget->property("AlternateEmpty").toBool()) { ret = true; + } break; case QStyle::SH_EtchDisabledText: - if (panelWidget(widget)) + if (panelWidget(widget)) { ret = false; + } break; default: break; @@ -440,16 +462,17 @@ int ManhattanStyle::styleHint(StyleHint hint, const QStyleOption *option, const void ManhattanStyle::drawPrimitive(PrimitiveElement element, const QStyleOption *option, QPainter *painter, const QWidget *widget) const { - if (!panelWidget(widget)) + if (!panelWidget(widget)) { return d->style->drawPrimitive(element, option, painter, widget); + } bool animating = (option->state & State_Animating); int state = option->state; - QRect rect = option->rect; + QRect rect = option->rect; QRect oldRect; QRect newRect; if (widget && (element == PE_PanelButtonTool) && !animating) { - QWidget *w = const_cast (widget); + QWidget *w = const_cast (widget); int oldState = w->property("_q_stylestate").toInt(); oldRect = w->property("_q_stylerect").toRect(); newRect = w->rect(); @@ -457,10 +480,9 @@ void ManhattanStyle::drawPrimitive(PrimitiveElement element, const QStyleOption w->setProperty("_q_stylerect", w->rect()); // Determine the animated transition - bool doTransition = ((state & State_On) != (oldState & State_On) || - (state & State_MouseOver) != (oldState & State_MouseOver)); - if (oldRect != newRect) - { + bool doTransition = ((state & State_On) != (oldState & State_On) || + (state & State_MouseOver) != (oldState & State_MouseOver)); + if (oldRect != newRect) { doTransition = false; d->animator.stopAnimation(widget); } @@ -468,9 +490,9 @@ void ManhattanStyle::drawPrimitive(PrimitiveElement element, const QStyleOption if (doTransition) { QImage startImage(option->rect.size(), QImage::Format_ARGB32_Premultiplied); QImage endImage(option->rect.size(), QImage::Format_ARGB32_Premultiplied); - Animation *anim = d->animator.widgetAnimation(widget); + Animation *anim = d->animator.widgetAnimation(widget); QStyleOption opt = *option; - opt.state = (QStyle::State)oldState; + opt.state = (QStyle::State)oldState; opt.state |= (State)State_Animating; startImage.fill(0); Transition *t = new Transition; @@ -490,239 +512,243 @@ void ManhattanStyle::drawPrimitive(PrimitiveElement element, const QStyleOption QPainter endPainter(&endImage); drawPrimitive(element, &endOpt, &endPainter, widget); t->setEndImage(endImage); - if (oldState & State_MouseOver) + if (oldState & State_MouseOver) { t->setDuration(150); - else + } else { t->setDuration(75); + } t->setStartTime(QTime::currentTime()); } } switch (element) { case PE_PanelLineEdit: - { - painter->save(); + { + painter->save(); - // Fill the line edit background - QRect filledRect = option->rect.adjusted(1, 1, -1, -1); - painter->setBrushOrigin(filledRect.topLeft()); - painter->fillRect(filledRect, option->palette.base()); + // Fill the line edit background + QRect filledRect = option->rect.adjusted(1, 1, -1, -1); + painter->setBrushOrigin(filledRect.topLeft()); + painter->fillRect(filledRect, option->palette.base()); - if (option->state & State_Enabled) - drawCornerImage(d->lineeditImage, painter, option->rect, 2, 2, 2, 2); - else - drawCornerImage(d->lineeditImage_disabled, painter, option->rect, 2, 2, 2, 2); - - if (option->state & State_HasFocus || option->state & State_MouseOver) { - QColor hover = Utils::StyleHelper::baseColor(); - if (state & State_HasFocus) - hover.setAlpha(100); - else - hover.setAlpha(50); - - painter->setPen(QPen(hover, 1)); - painter->drawRect(option->rect.adjusted(1, 1, -2 ,-2)); - } - painter->restore(); + if (option->state & State_Enabled) { + drawCornerImage(d->lineeditImage, painter, option->rect, 2, 2, 2, 2); + } else { + drawCornerImage(d->lineeditImage_disabled, painter, option->rect, 2, 2, 2, 2); } - break; + + if (option->state & State_HasFocus || option->state & State_MouseOver) { + QColor hover = Utils::StyleHelper::baseColor(); + if (state & State_HasFocus) { + hover.setAlpha(100); + } else { + hover.setAlpha(50); + } + + painter->setPen(QPen(hover, 1)); + painter->drawRect(option->rect.adjusted(1, 1, -2, -2)); + } + painter->restore(); + } + break; case PE_FrameStatusBarItem: break; - case PE_PanelButtonTool: { - Animation *anim = d->animator.widgetAnimation(widget); - if (!animating && anim) { - anim->paint(painter, option); - } else { - bool pressed = option->state & State_Sunken || option->state & State_On; - QColor shadow(0, 0, 0, 30); - painter->setPen(shadow); - if (pressed) { - QColor shade(0, 0, 0, 40); - painter->fillRect(rect, shade); - painter->drawLine(rect.topLeft() + QPoint(1, 0), rect.topRight() - QPoint(1, 0)); - painter->drawLine(rect.topLeft(), rect.bottomLeft()); - painter->drawLine(rect.topRight(), rect.bottomRight()); - // painter->drawLine(rect.bottomLeft() + QPoint(1, 0), rect.bottomRight() - QPoint(1, 0)); - QColor highlight(255, 255, 255, 30); - painter->setPen(highlight); - } - else if (option->state & State_Enabled && - option->state & State_MouseOver) { - QColor lighter(255, 255, 255, 37); - painter->fillRect(rect, lighter); - } - } + case PE_PanelButtonTool: + { + Animation *anim = d->animator.widgetAnimation(widget); + if (!animating && anim) { + anim->paint(painter, option); + } else { + bool pressed = option->state & State_Sunken || option->state & State_On; + QColor shadow(0, 0, 0, 30); + painter->setPen(shadow); + if (pressed) { + QColor shade(0, 0, 0, 40); + painter->fillRect(rect, shade); + painter->drawLine(rect.topLeft() + QPoint(1, 0), rect.topRight() - QPoint(1, 0)); + painter->drawLine(rect.topLeft(), rect.bottomLeft()); + painter->drawLine(rect.topRight(), rect.bottomRight()); + // painter->drawLine(rect.bottomLeft() + QPoint(1, 0), rect.bottomRight() - QPoint(1, 0)); + QColor highlight(255, 255, 255, 30); + painter->setPen(highlight); + } else if (option->state & State_Enabled && + option->state & State_MouseOver) { + QColor lighter(255, 255, 255, 37); + painter->fillRect(rect, lighter); + } } - break; + } + break; case PE_PanelStatusBar: - { - painter->save(); - QLinearGradient grad(option->rect.topLeft(), QPoint(rect.center().x(), rect.bottom())); - QColor startColor = Utils::StyleHelper::shadowColor().darker(164); - QColor endColor = Utils::StyleHelper::baseColor().darker(130); - grad.setColorAt(0, startColor); - grad.setColorAt(1, endColor); - painter->fillRect(option->rect, grad); - painter->setPen(QColor(255, 255, 255, 60)); - painter->drawLine(rect.topLeft() + QPoint(0,1), - rect.topRight()+ QPoint(0,1)); - painter->setPen(Utils::StyleHelper::borderColor().darker(110)); - painter->drawLine(rect.topLeft(), rect.topRight()); - painter->restore(); - } - break; + { + painter->save(); + QLinearGradient grad(option->rect.topLeft(), QPoint(rect.center().x(), rect.bottom())); + QColor startColor = Utils::StyleHelper::shadowColor().darker(164); + QColor endColor = Utils::StyleHelper::baseColor().darker(130); + grad.setColorAt(0, startColor); + grad.setColorAt(1, endColor); + painter->fillRect(option->rect, grad); + painter->setPen(QColor(255, 255, 255, 60)); + painter->drawLine(rect.topLeft() + QPoint(0, 1), + rect.topRight() + QPoint(0, 1)); + painter->setPen(Utils::StyleHelper::borderColor().darker(110)); + painter->drawLine(rect.topLeft(), rect.topRight()); + painter->restore(); + } + break; case PE_IndicatorToolBarSeparator: - { - QColor separatorColor = Utils::StyleHelper::borderColor(); - separatorColor.setAlpha(100); - painter->setPen(separatorColor); - const int margin = 6; - if (option->state & State_Horizontal) { - const int offset = rect.width()/2; - painter->drawLine(rect.bottomLeft().x() + offset, - rect.bottomLeft().y() - margin, - rect.topLeft().x() + offset, - rect.topLeft().y() + margin); - } else { //Draw vertical separator - const int offset = rect.height()/2; - painter->setPen(QPen(option->palette.background().color().darker(110))); - painter->drawLine(rect.topLeft().x() + margin , - rect.topLeft().y() + offset, - rect.topRight().x() - margin, - rect.topRight().y() + offset); - } + { + QColor separatorColor = Utils::StyleHelper::borderColor(); + separatorColor.setAlpha(100); + painter->setPen(separatorColor); + const int margin = 6; + if (option->state & State_Horizontal) { + const int offset = rect.width() / 2; + painter->drawLine(rect.bottomLeft().x() + offset, + rect.bottomLeft().y() - margin, + rect.topLeft().x() + offset, + rect.topLeft().y() + margin); + } else { // Draw vertical separator + const int offset = rect.height() / 2; + painter->setPen(QPen(option->palette.background().color().darker(110))); + painter->drawLine(rect.topLeft().x() + margin, + rect.topLeft().y() + offset, + rect.topRight().x() - margin, + rect.topRight().y() + offset); } - break; + } + break; case PE_IndicatorToolBarHandle: - { - bool horizontal = option->state & State_Horizontal; - painter->save(); - QPainterPath path; - int x = option->rect.x() + horizontal ? 2 : 6; - int y = option->rect.y() + horizontal ? 6 : 2; - static const int RectHeight = 2; - if (horizontal) { - while (y < option->rect.height() - RectHeight - 6) { - path.moveTo(x, y); - path.addRect(x, y, RectHeight, RectHeight); - y += 6; - } - } else { - while (x < option->rect.width() - RectHeight - 6) { - path.moveTo(x, y); - path.addRect(x, y, RectHeight, RectHeight); - x += 6; - } + { + bool horizontal = option->state & State_Horizontal; + painter->save(); + QPainterPath path; + int x = option->rect.x() + horizontal ? 2 : 6; + int y = option->rect.y() + horizontal ? 6 : 2; + static const int RectHeight = 2; + if (horizontal) { + while (y < option->rect.height() - RectHeight - 6) { + path.moveTo(x, y); + path.addRect(x, y, RectHeight, RectHeight); + y += 6; + } + } else { + while (x < option->rect.width() - RectHeight - 6) { + path.moveTo(x, y); + path.addRect(x, y, RectHeight, RectHeight); + x += 6; } - - painter->setPen(Qt::NoPen); - QColor dark = Utils::StyleHelper::borderColor(); - dark.setAlphaF(0.4); - - QColor light = Utils::StyleHelper::baseColor(); - light.setAlphaF(0.4); - - painter->fillPath(path, light); - painter->save(); - painter->translate(1, 1); - painter->fillPath(path, dark); - painter->restore(); - painter->translate(3, 3); - painter->fillPath(path, light); - painter->translate(1, 1); - painter->fillPath(path, dark); - painter->restore(); } - break; + + painter->setPen(Qt::NoPen); + QColor dark = Utils::StyleHelper::borderColor(); + dark.setAlphaF(0.4); + + QColor light = Utils::StyleHelper::baseColor(); + light.setAlphaF(0.4); + + painter->fillPath(path, light); + painter->save(); + painter->translate(1, 1); + painter->fillPath(path, dark); + painter->restore(); + painter->translate(3, 3); + painter->fillPath(path, light); + painter->translate(1, 1); + painter->fillPath(path, dark); + painter->restore(); + } + break; case PE_IndicatorArrowUp: case PE_IndicatorArrowDown: case PE_IndicatorArrowRight: case PE_IndicatorArrowLeft: - { - // From windowsstyle but modified to enable AA - if (option->rect.width() <= 1 || option->rect.height() <= 1) - break; - - QRect r = option->rect; - int size = qMin(r.height(), r.width()); - QPixmap pixmap; - QString pixmapName; - QTextStream(&pixmapName) << "$qt_ia" << "-" << metaObject()->className() << - "-" << uint(option->state) << "-" << element << "-" << size << - "-" << option->palette.cacheKey(); -// pixmapName.sprintf("%s-%s-%d-%d-%d-%lld", -// "$qt_ia", metaObject()->className(), -// uint(option->state), element, -// size, option->palette.cacheKey()); - if (!QPixmapCache::find(pixmapName, pixmap)) { - int border = size/5; - int sqsize = 2*(size/2); - QImage image(sqsize, sqsize, QImage::Format_ARGB32); - image.fill(Qt::transparent); - QPainter imagePainter(&image); - imagePainter.setRenderHint(QPainter::Antialiasing, true); - imagePainter.translate(0.5, 0.5); - QPolygon a; - switch (element) { - case PE_IndicatorArrowUp: - a.setPoints(3, border, sqsize/2, sqsize/2, border, sqsize - border, sqsize/2); - break; - case PE_IndicatorArrowDown: - a.setPoints(3, border, sqsize/2, sqsize/2, sqsize - border, sqsize - border, sqsize/2); - break; - case PE_IndicatorArrowRight: - a.setPoints(3, sqsize - border, sqsize/2, sqsize/2, border, sqsize/2, sqsize - border); - break; - case PE_IndicatorArrowLeft: - a.setPoints(3, border, sqsize/2, sqsize/2, border, sqsize/2, sqsize - border); - break; - default: - break; - } - - int bsx = 0; - int bsy = 0; - - if (option->state & State_Sunken) { - bsx = pixelMetric(PM_ButtonShiftHorizontal); - bsy = pixelMetric(PM_ButtonShiftVertical); - } - - QRect bounds = a.boundingRect(); - int sx = sqsize / 2 - bounds.center().x() - 1; - int sy = sqsize / 2 - bounds.center().y() - 1; - imagePainter.translate(sx + bsx, sy + bsy); - - if (!(option->state & State_Enabled)) { - QColor foreGround(150, 150, 150, 150); - imagePainter.setBrush(option->palette.mid().color()); - imagePainter.setPen(option->palette.mid().color()); - } else { - QColor shadow(0, 0, 0, 100); - imagePainter.translate(0, 1); - imagePainter.setPen(shadow); - imagePainter.setBrush(shadow); - QColor foreGround(255, 255, 255, 210); - imagePainter.drawPolygon(a); - imagePainter.translate(0, -1); - imagePainter.setPen(foreGround); - imagePainter.setBrush(foreGround); - } - imagePainter.drawPolygon(a); - imagePainter.end(); - pixmap = QPixmap::fromImage(image); - QPixmapCache::insert(pixmapName, pixmap); - } - int xOffset = r.x() + (r.width() - size)/2; - int yOffset = r.y() + (r.height() - size)/2; - painter->drawPixmap(xOffset, yOffset, pixmap); + { + // From windowsstyle but modified to enable AA + if (option->rect.width() <= 1 || option->rect.height() <= 1) { + break; } - break; + + QRect r = option->rect; + int size = qMin(r.height(), r.width()); + QPixmap pixmap; + QString pixmapName; + QTextStream(&pixmapName) << "$qt_ia" << "-" << metaObject()->className() << + "-" << uint(option->state) << "-" << element << "-" << size << + "-" << option->palette.cacheKey(); +// pixmapName.sprintf("%s-%s-%d-%d-%d-%lld", +// "$qt_ia", metaObject()->className(), +// uint(option->state), element, +// size, option->palette.cacheKey()); + if (!QPixmapCache::find(pixmapName, pixmap)) { + int border = size / 5; + int sqsize = 2 * (size / 2); + QImage image(sqsize, sqsize, QImage::Format_ARGB32); + image.fill(Qt::transparent); + QPainter imagePainter(&image); + imagePainter.setRenderHint(QPainter::Antialiasing, true); + imagePainter.translate(0.5, 0.5); + QPolygon a; + switch (element) { + case PE_IndicatorArrowUp: + a.setPoints(3, border, sqsize / 2, sqsize / 2, border, sqsize - border, sqsize / 2); + break; + case PE_IndicatorArrowDown: + a.setPoints(3, border, sqsize / 2, sqsize / 2, sqsize - border, sqsize - border, sqsize / 2); + break; + case PE_IndicatorArrowRight: + a.setPoints(3, sqsize - border, sqsize / 2, sqsize / 2, border, sqsize / 2, sqsize - border); + break; + case PE_IndicatorArrowLeft: + a.setPoints(3, border, sqsize / 2, sqsize / 2, border, sqsize / 2, sqsize - border); + break; + default: + break; + } + + int bsx = 0; + int bsy = 0; + + if (option->state & State_Sunken) { + bsx = pixelMetric(PM_ButtonShiftHorizontal); + bsy = pixelMetric(PM_ButtonShiftVertical); + } + + QRect bounds = a.boundingRect(); + int sx = sqsize / 2 - bounds.center().x() - 1; + int sy = sqsize / 2 - bounds.center().y() - 1; + imagePainter.translate(sx + bsx, sy + bsy); + + if (!(option->state & State_Enabled)) { + QColor foreGround(150, 150, 150, 150); + imagePainter.setBrush(option->palette.mid().color()); + imagePainter.setPen(option->palette.mid().color()); + } else { + QColor shadow(0, 0, 0, 100); + imagePainter.translate(0, 1); + imagePainter.setPen(shadow); + imagePainter.setBrush(shadow); + QColor foreGround(255, 255, 255, 210); + imagePainter.drawPolygon(a); + imagePainter.translate(0, -1); + imagePainter.setPen(foreGround); + imagePainter.setBrush(foreGround); + } + imagePainter.drawPolygon(a); + imagePainter.end(); + pixmap = QPixmap::fromImage(image); + QPixmapCache::insert(pixmapName, pixmap); + } + int xOffset = r.x() + (r.width() - size) / 2; + int yOffset = r.y() + (r.height() - size) / 2; + painter->drawPixmap(xOffset, yOffset, pixmap); + } + break; default: d->style->drawPrimitive(element, option, painter, widget); @@ -733,19 +759,20 @@ void ManhattanStyle::drawPrimitive(PrimitiveElement element, const QStyleOption void ManhattanStyle::drawControl(ControlElement element, const QStyleOption *option, QPainter *painter, const QWidget *widget) const { - if (!panelWidget(widget)) + if (!panelWidget(widget)) { return d->style->drawControl(element, option, painter, widget); + } switch (element) { case CE_MenuBarItem: painter->save(); - if (const QStyleOptionMenuItem *mbi = qstyleoption_cast(option)) { + if (const QStyleOptionMenuItem * mbi = qstyleoption_cast(option)) { QColor highlightOutline = Utils::StyleHelper::borderColor().lighter(120); bool act = mbi->state & State_Selected && mbi->state & State_Sunken; bool dis = !(mbi->state & State_Enabled); Utils::StyleHelper::menuGradient(painter, option->rect, option->rect); QStyleOptionMenuItem item = *mbi; - item.rect = mbi->rect; + item.rect = mbi->rect; QPalette pal = mbi->palette; pal.setBrush(QPalette::ButtonText, dis ? Qt::gray : Qt::black); item.palette = pal; @@ -770,10 +797,11 @@ void ManhattanStyle::drawControl(ControlElement element, const QStyleOption *opt painter->drawPoint(r.topLeft()); painter->drawPoint(r.topRight()); - QPalette pal = mbi->palette; + QPalette pal = mbi->palette; uint alignment = Qt::AlignCenter | Qt::TextShowMnemonic | Qt::TextDontClip | Qt::TextSingleLine; - if (!styleHint(SH_UnderlineShortcut, mbi, widget)) + if (!styleHint(SH_UnderlineShortcut, mbi, widget)) { alignment |= Qt::TextHideMnemonic; + } pal.setBrush(QPalette::Text, dis ? Qt::gray : QColor(0, 0, 0, 60)); drawItemText(painter, item.rect.translated(0, 1), alignment, pal, mbi->state & State_Enabled, mbi->text, QPalette::Text); pal.setBrush(QPalette::Text, dis ? Qt::gray : Qt::white); @@ -784,28 +812,30 @@ void ManhattanStyle::drawControl(ControlElement element, const QStyleOption *opt break; case CE_ComboBoxLabel: - if (const QStyleOptionComboBox *cb = qstyleoption_cast(option)) { + if (const QStyleOptionComboBox * cb = qstyleoption_cast(option)) { if (panelWidget(widget)) { - QRect editRect = subControlRect(CC_ComboBox, cb, SC_ComboBoxEditField, widget); + QRect editRect = subControlRect(CC_ComboBox, cb, SC_ComboBoxEditField, widget); QPalette customPal = cb->palette; if (!cb->currentIcon.isNull()) { QIcon::Mode mode = cb->state & State_Enabled ? QIcon::Normal - : QIcon::Disabled; - QPixmap pixmap = cb->currentIcon.pixmap(cb->iconSize, mode); + : QIcon::Disabled; + QPixmap pixmap = cb->currentIcon.pixmap(cb->iconSize, mode); QRect iconRect(editRect); iconRect.setWidth(cb->iconSize.width() + 4); iconRect = alignedRect(cb->direction, Qt::AlignLeft | Qt::AlignVCenter, iconRect.size(), editRect); - if (cb->editable) + if (cb->editable) { painter->fillRect(iconRect, customPal.brush(QPalette::Base)); + } drawItemPixmap(painter, iconRect, Qt::AlignCenter, pixmap); - if (cb->direction == Qt::RightToLeft) + if (cb->direction == Qt::RightToLeft) { editRect.translate(-4 - cb->iconSize.width(), 0); - else + } else { editRect.translate(cb->iconSize.width() + 4, 0); + } // Reserve some space for the down-arrow editRect.adjust(0, 0, -13, 0); @@ -827,104 +857,108 @@ void ManhattanStyle::drawControl(ControlElement element, const QStyleOption *opt } break; - case CE_SizeGrip: { - painter->save(); - QColor dark = Qt::white; - dark.setAlphaF(0.1); - int x, y, w, h; - option->rect.getRect(&x, &y, &w, &h); - int sw = qMin(h, w); - if (h > w) - painter->translate(0, h - w); - else - painter->translate(w - h, 0); - int sx = x; - int sy = y; - int s = 4; - painter->setPen(dark); - if (option->direction == Qt::RightToLeft) { - sx = x + sw; - for (int i = 0; i < 4; ++i) { - painter->drawLine(x, sy, sx, sw); - sx -= s; - sy += s; - } - } else { - for (int i = 0; i < 4; ++i) { - painter->drawLine(sx, sw, sw, sy); - sx += s; - sy += s; - } + case CE_SizeGrip: + { + painter->save(); + QColor dark = Qt::white; + dark.setAlphaF(0.1); + int x, y, w, h; + option->rect.getRect(&x, &y, &w, &h); + int sw = qMin(h, w); + if (h > w) { + painter->translate(0, h - w); + } else { + painter->translate(w - h, 0); + } + int sx = x; + int sy = y; + int s = 4; + painter->setPen(dark); + if (option->direction == Qt::RightToLeft) { + sx = x + sw; + for (int i = 0; i < 4; ++i) { + painter->drawLine(x, sy, sx, sw); + sx -= s; + sy += s; + } + } else { + for (int i = 0; i < 4; ++i) { + painter->drawLine(sx, sw, sw, sy); + sx += s; + sy += s; } - painter->restore(); } - break; + painter->restore(); + } + break; - case CE_MenuBarEmptyArea: { - Utils::StyleHelper::menuGradient(painter, option->rect, option->rect); - painter->save(); - painter->setPen(Utils::StyleHelper::borderColor()); - painter->drawLine(option->rect.bottomLeft(), option->rect.bottomRight()); - painter->restore(); - } - break; + case CE_MenuBarEmptyArea: + { + Utils::StyleHelper::menuGradient(painter, option->rect, option->rect); + painter->save(); + painter->setPen(Utils::StyleHelper::borderColor()); + painter->drawLine(option->rect.bottomLeft(), option->rect.bottomRight()); + painter->restore(); + } + break; case CE_ToolBar: - { - QString key; - key.sprintf("mh_toolbar %d %d %d", option->rect.width(), option->rect.height(), Utils::StyleHelper::baseColor().rgb());; + { + QString key; + key.sprintf("mh_toolbar %d %d %d", option->rect.width(), option->rect.height(), Utils::StyleHelper::baseColor().rgb());; - QPixmap pixmap; - QPainter *p = painter; - QRect rect = option->rect; - if (Utils::StyleHelper::usePixmapCache() && !QPixmapCache::find(key, pixmap)) { - pixmap = QPixmap(option->rect.size()); - p = new QPainter(&pixmap); - rect = QRect(0, 0, option->rect.width(), option->rect.height()); - } - - bool horizontal = option->state & State_Horizontal; - // Map offset for global window gradient - QPoint offset = widget->window()->mapToGlobal(option->rect.topLeft()) - - widget->mapToGlobal(option->rect.topLeft()); - QRect gradientSpan; - if (widget) { - gradientSpan = QRect(offset, widget->window()->size()); - } - if (horizontal) - Utils::StyleHelper::horizontalGradient(p, gradientSpan, rect); - else - Utils::StyleHelper::verticalGradient(p, gradientSpan, rect); - - painter->setPen(Utils::StyleHelper::borderColor()); - - if (horizontal) { - // Note: This is a hack to determine if the - // toolbar should draw the top or bottom outline - // (needed for the find toolbar for instance) - QColor lighter(255, 255, 255, 40); - if (widget && widget->property("topBorder").toBool()) { - p->drawLine(rect.topLeft(), rect.topRight()); - p->setPen(lighter); - p->drawLine(rect.topLeft() + QPoint(0, 1), rect.topRight() + QPoint(0, 1)); - } else { - p->drawLine(rect.bottomLeft(), rect.bottomRight()); - p->setPen(lighter); - p->drawLine(rect.topLeft(), rect.topRight()); - } - } else { - p->drawLine(rect.topLeft(), rect.bottomLeft()); - p->drawLine(rect.topRight(), rect.bottomRight()); - } - - if (Utils::StyleHelper::usePixmapCache() && !QPixmapCache::find(key, pixmap)) { - painter->drawPixmap(rect.topLeft(), pixmap); - p->end(); - delete p; - QPixmapCache::insert(key, pixmap); - } + QPixmap pixmap; + QPainter *p = painter; + QRect rect = option->rect; + if (Utils::StyleHelper::usePixmapCache() && !QPixmapCache::find(key, pixmap)) { + pixmap = QPixmap(option->rect.size()); + p = new QPainter(&pixmap); + rect = QRect(0, 0, option->rect.width(), option->rect.height()); } - break; + + bool horizontal = option->state & State_Horizontal; + // Map offset for global window gradient + QPoint offset = widget->window()->mapToGlobal(option->rect.topLeft()) - + widget->mapToGlobal(option->rect.topLeft()); + QRect gradientSpan; + if (widget) { + gradientSpan = QRect(offset, widget->window()->size()); + } + if (horizontal) { + Utils::StyleHelper::horizontalGradient(p, gradientSpan, rect); + } else { + Utils::StyleHelper::verticalGradient(p, gradientSpan, rect); + } + + painter->setPen(Utils::StyleHelper::borderColor()); + + if (horizontal) { + // Note: This is a hack to determine if the + // toolbar should draw the top or bottom outline + // (needed for the find toolbar for instance) + QColor lighter(255, 255, 255, 40); + if (widget && widget->property("topBorder").toBool()) { + p->drawLine(rect.topLeft(), rect.topRight()); + p->setPen(lighter); + p->drawLine(rect.topLeft() + QPoint(0, 1), rect.topRight() + QPoint(0, 1)); + } else { + p->drawLine(rect.bottomLeft(), rect.bottomRight()); + p->setPen(lighter); + p->drawLine(rect.topLeft(), rect.topRight()); + } + } else { + p->drawLine(rect.topLeft(), rect.bottomLeft()); + p->drawLine(rect.topRight(), rect.bottomRight()); + } + + if (Utils::StyleHelper::usePixmapCache() && !QPixmapCache::find(key, pixmap)) { + painter->drawPixmap(rect.topLeft(), pixmap); + p->end(); + delete p; + QPixmapCache::insert(key, pixmap); + } + } + break; default: d->style->drawControl(element, option, painter, widget); @@ -935,15 +969,16 @@ void ManhattanStyle::drawControl(ControlElement element, const QStyleOption *opt void ManhattanStyle::drawComplexControl(ComplexControl control, const QStyleOptionComplex *option, QPainter *painter, const QWidget *widget) const { - if (!panelWidget(widget)) - return d->style->drawComplexControl(control, option, painter, widget); + if (!panelWidget(widget)) { + return d->style->drawComplexControl(control, option, painter, widget); + } QRect rect = option->rect; switch (control) { case CC_ToolButton: - if (const QStyleOptionToolButton *toolbutton = qstyleoption_cast(option)) { + if (const QStyleOptionToolButton * toolbutton = qstyleoption_cast(option)) { QRect button, menuarea; - button = subControlRect(control, toolbutton, SC_ToolButton, widget); + button = subControlRect(control, toolbutton, SC_ToolButton, widget); menuarea = subControlRect(control, toolbutton, SC_ToolButtonMenu, widget); State bflags = toolbutton->state; @@ -955,16 +990,18 @@ void ManhattanStyle::drawComplexControl(ComplexControl control, const QStyleOpti State mflags = bflags; if (toolbutton->state & State_Sunken) { - if (toolbutton->activeSubControls & SC_ToolButton) + if (toolbutton->activeSubControls & SC_ToolButton) { bflags |= State_Sunken; - if (toolbutton->activeSubControls & SC_ToolButtonMenu) + } + if (toolbutton->activeSubControls & SC_ToolButtonMenu) { mflags |= State_Sunken; + } } QStyleOption tool(0); tool.palette = toolbutton->palette; if (toolbutton->subControls & SC_ToolButton) { - tool.rect = button; + tool.rect = button; tool.state = bflags; drawPrimitive(PE_PanelButtonTool, &tool, painter, widget); } @@ -973,9 +1010,10 @@ void ManhattanStyle::drawComplexControl(ComplexControl control, const QStyleOpti QStyleOptionFocusRect fr; fr.QStyleOption::operator=(*toolbutton); fr.rect.adjust(3, 3, -3, -3); - if (toolbutton->features & QStyleOptionToolButton::MenuButtonPopup) + if (toolbutton->features & QStyleOptionToolButton::MenuButtonPopup) { fr.rect.adjust(0, 0, -pixelMetric(QStyle::PM_MenuButtonIndicator, toolbutton, widget), 0); + } QPen oldPen = painter->pen(); QColor focusColor = Utils::StyleHelper::panelTextColor(); focusColor.setAlpha(120); @@ -990,12 +1028,12 @@ void ManhattanStyle::drawComplexControl(ComplexControl control, const QStyleOpti QStyleOptionToolButton label = *toolbutton; label.palette = panelPalette(option->palette); int fw = pixelMetric(PM_DefaultFrameWidth, option, widget); - label.rect = button.adjusted(fw, fw, -fw, -fw); + label.rect = button.adjusted(fw, fw, -fw, -fw); drawControl(CE_ToolButtonLabel, &label, painter, widget); if (toolbutton->subControls & SC_ToolButtonMenu) { tool.state = mflags; - tool.rect = menuarea.adjusted(1, 1, -1, -1); + tool.rect = menuarea.adjusted(1, 1, -1, -1); if (mflags & (State_Sunken | State_On | State_Raised)) { painter->setPen(Qt::gray); painter->drawLine(tool.rect.topLeft(), tool.rect.bottomLeft()); @@ -1017,15 +1055,15 @@ void ManhattanStyle::drawComplexControl(ComplexControl control, const QStyleOpti QRect ir = toolbutton->rect.adjusted(1, 1, -1, -1); QStyleOptionToolButton newBtn = *toolbutton; newBtn.palette = panelPalette(option->palette); - newBtn.rect = QRect(ir.right() - arrowSize - 1, - ir.height() - arrowSize - 2, arrowSize, arrowSize); + newBtn.rect = QRect(ir.right() - arrowSize - 1, + ir.height() - arrowSize - 2, arrowSize, arrowSize); drawPrimitive(PE_IndicatorArrowDown, &newBtn, painter, widget); } } break; case CC_ComboBox: - if (const QStyleOptionComboBox *cb = qstyleoption_cast(option)) { + if (const QStyleOptionComboBox * cb = qstyleoption_cast(option)) { painter->save(); bool isEmpty = cb->currentText.isEmpty() && cb->currentIcon.isNull(); bool reverse = option->direction == Qt::RightToLeft; @@ -1043,30 +1081,34 @@ void ManhattanStyle::drawComplexControl(ComplexControl control, const QStyleOpti grad.setColorAt(0.7, QColor(0, 0, 0, 70)); grad.setColorAt(1, QColor(0, 0, 0, 40)); painter->setPen(QPen(grad, 0)); - if (!reverse) - painter->drawLine(rect.topRight() - QPoint(1,0), rect.bottomRight() - QPoint(1,0)); - else + if (!reverse) { + painter->drawLine(rect.topRight() - QPoint(1, 0), rect.bottomRight() - QPoint(1, 0)); + } else { painter->drawLine(rect.topLeft(), rect.bottomLeft()); + } QStyleOption toolbutton = *option; - if (isEmpty) + if (isEmpty) { toolbutton.state &= ~(State_Enabled | State_Sunken); + } painter->save(); painter->setClipRect(toolbutton.rect.adjusted(0, 0, -2, 0)); drawPrimitive(PE_PanelButtonTool, &toolbutton, painter, widget); painter->restore(); // Draw arrow int menuButtonWidth = 12; - int left = !reverse ? rect.right() - menuButtonWidth : rect.left(); + int left = !reverse ? rect.right() - menuButtonWidth : rect.left(); int right = !reverse ? rect.right() : rect.left() + menuButtonWidth; QRect arrowRect((left + right) / 2 + (reverse ? 6 : -6), rect.center().y() - 3, 9, 9); - if (option->state & State_On) + if (option->state & State_On) { arrowRect.translate(d->style->pixelMetric(PM_ButtonShiftHorizontal, option, widget), d->style->pixelMetric(PM_ButtonShiftVertical, option, widget)); + } QStyleOption arrowOpt = *option; arrowOpt.rect = arrowRect; - if (isEmpty) + if (isEmpty) { arrowOpt.state &= ~(State_Enabled | State_Sunken); + } if (styleHint(SH_ComboBox_Popup, option, widget)) { arrowOpt.rect.translate(0, -3); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.h b/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.h index 241e41e84..bb90cf544 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/manhattanstyle.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,8 +40,7 @@ QT_END_NAMESPACE class ManhattanStylePrivate; -class CORE_EXPORT ManhattanStyle : public QWindowsStyle -{ +class CORE_EXPORT ManhattanStyle : public QWindowsStyle { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.cpp index d9e2d373f..a2996c8bf 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -47,6 +47,7 @@ MessageManager::MessageManager() MessageManager::~MessageManager() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + if (pm && m_messageOutputWindow) { pm->removeObject(m_messageOutputWindow); delete m_messageOutputWindow; @@ -63,22 +64,25 @@ void MessageManager::init() void MessageManager::showOutputPane() { - if (m_messageOutputWindow) + if (m_messageOutputWindow) { m_messageOutputWindow->popup(false); + } } void MessageManager::displayStatusBarMessage(const QString & /*text*/, int /*ms*/) { // TODO: Currently broken, but noone really notices, so... - //m_mainWindow->statusBar()->showMessage(text, ms); + // m_mainWindow->statusBar()->showMessage(text, ms); } void MessageManager::printToOutputPane(const QString &text, bool bringToForeground) { - if (!m_messageOutputWindow) + if (!m_messageOutputWindow) { return; - if (bringToForeground) + } + if (bringToForeground) { m_messageOutputWindow->popup(false); + } m_messageOutputWindow->append(text); } @@ -91,4 +95,3 @@ void MessageManager::printToOutputPane(const QString &text) { printToOutputPane(text, false); } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.h b/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.h index 60164a07e..1927b70fa 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/messagemanager.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,13 +33,11 @@ #include namespace Core { - namespace Internal { class MessageOutputWindow; } -class CORE_EXPORT MessageManager : public QObject -{ +class CORE_EXPORT MessageManager : public QObject { Q_OBJECT public: @@ -48,7 +46,10 @@ public: void init(); - static MessageManager *instance() { return m_instance; } + static MessageManager *instance() + { + return m_instance; + } void displayStatusBarMessage(const QString &text, int ms = 0); void showOutputPane(); @@ -63,7 +64,6 @@ private: static MessageManager *m_instance; }; - } // namespace Core #endif // MESSAGEMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.cpp b/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.cpp index a30fbd1c1..d4c4560ea 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -76,8 +76,7 @@ QString MessageOutputWindow::name() const } void MessageOutputWindow::visibilityChanged(bool /*b*/) -{ -} +{} void MessageOutputWindow::append(const QString &text) { @@ -100,14 +99,10 @@ bool MessageOutputWindow::canPrevious() } void MessageOutputWindow::goToNext() -{ - -} +{} void MessageOutputWindow::goToPrev() -{ - -} +{} bool MessageOutputWindow::canNavigate() { diff --git a/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.h b/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.h index f2fb5877d..fb76d3bb3 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/messageoutputwindow.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,9 +37,7 @@ QT_END_NAMESPACE namespace Core { namespace Internal { - -class MessageOutputWindow : public Core::IOutputPane -{ +class MessageOutputWindow : public Core::IOutputPane { Q_OBJECT public: @@ -47,7 +45,10 @@ public: ~MessageOutputWindow(); QWidget *outputWidget(QWidget *parent); - QList toolBarWidgets() const { return QList(); } + QList toolBarWidgets() const + { + return QList(); + } QString name() const; int priorityInStatusBar() const; @@ -68,7 +69,6 @@ public: private: QTextEdit *m_widget; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.cpp b/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.cpp index 609316b53..b037fc35b 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -49,51 +49,50 @@ enum { debugMimeDB = 0 }; // XML tags in mime files -static const char *mimeInfoTagC = "mime-info"; -static const char *mimeTypeTagC = "mime-type"; -static const char *mimeTypeAttributeC = "type"; -static const char *subClassTagC = "sub-class-of"; -static const char *commentTagC = "comment"; -static const char *globTagC = "glob"; -static const char *aliasTagC = "alias"; -static const char *patternAttributeC = "pattern"; -static const char *localeAttributeC = "xml:lang"; +static const char *mimeInfoTagC = "mime-info"; +static const char *mimeTypeTagC = "mime-type"; +static const char *mimeTypeAttributeC = "type"; +static const char *subClassTagC = "sub-class-of"; +static const char *commentTagC = "comment"; +static const char *globTagC = "glob"; +static const char *aliasTagC = "alias"; +static const char *patternAttributeC = "pattern"; +static const char *localeAttributeC = "xml:lang"; -static const char *magicTagC = "magic"; -static const char *priorityAttributeC = "priority"; -static const char *matchTagC = "match"; -static const char *matchValueAttributeC = "value"; -static const char *matchTypeAttributeC = "type"; -static const char *matchStringTypeValueC = "string"; -static const char *matchOffsetAttributeC = "offset"; +static const char *magicTagC = "magic"; +static const char *priorityAttributeC = "priority"; +static const char *matchTagC = "match"; +static const char *matchValueAttributeC = "value"; +static const char *matchTypeAttributeC = "type"; +static const char *matchStringTypeValueC = "string"; +static const char *matchOffsetAttributeC = "offset"; // Types -static const char *textTypeC = "text/plain"; -static const char *binaryTypeC = "application/octet-stream"; +static const char *textTypeC = "text/plain"; +static const char *binaryTypeC = "application/octet-stream"; // UTF16 byte order marks -static const char bigEndianByteOrderMarkC[] = "\xFE\xFF"; +static const char bigEndianByteOrderMarkC[] = "\xFE\xFF"; static const char littleEndianByteOrderMarkC[] = "\xFF\xFE"; // Fallback priorities, must be low. -enum { BinaryMatchPriority = 1, TextMatchPriority = 2}; +enum { BinaryMatchPriority = 1, TextMatchPriority = 2 }; /* Parse sth like ( being optional): - *\code -?xml version="1.0" encoding="UTF-8"?> - - - + *\code + ?xml version="1.0" encoding="UTF-8"?> + + + Qt QMake Profile - - - *\endcode -*/ + + + *\endcode + */ namespace Core { namespace Internal { - // FileMatchContext: Passed on to the mimetypes from the database // when looking for a file match. It exists to enable reading the file // contents "on demand" (as opposed to each mime type trying to open @@ -107,7 +106,10 @@ public: explicit FileMatchContext(const QFileInfo &fi); - inline QString fileName() const { return m_fileName; } + inline QString fileName() const + { + return m_fileName; + } // Return (cached) first MaxData bytes of file QByteArray data(); @@ -118,7 +120,8 @@ private: // Not read yet DataNotRead, // Available - DataRead }; + DataRead + }; const QFileInfo m_fileInfo; const QString m_fileName; State m_state; @@ -129,8 +132,7 @@ FileMatchContext::FileMatchContext(const QFileInfo &fi) : m_fileInfo(fi), m_fileName(fi.fileName()), m_state(fi.isFile() && fi.isReadable() && fi.size() > 0 ? DataNotRead : NoDataAvailable) -{ -} +{} QByteArray FileMatchContext::data() { @@ -139,7 +141,7 @@ QByteArray FileMatchContext::data() const QString fullName = m_fileInfo.absoluteFilePath(); QFile file(fullName); if (file.open(QIODevice::ReadOnly)) { - m_data = file.read(MaxData); + m_data = file.read(MaxData); m_state = DataRead; } else { qWarning("%s failed to open %s: %s\n", Q_FUNC_INFO, fullName.toUtf8().constData(), file.errorString().toUtf8().constData()); @@ -154,8 +156,14 @@ class BinaryMatcher : public IMagicMatcher { Q_DISABLE_COPY(BinaryMatcher) public: BinaryMatcher() {} - virtual bool matches(const QByteArray & /*data*/) const { return true; } - virtual int priority() const { return BinaryMatchPriority; } + virtual bool matches(const QByteArray & /*data*/) const + { + return true; + } + virtual int priority() const + { + return BinaryMatchPriority; + } }; // A heuristic text file matcher: If the data do not contain any character @@ -165,7 +173,10 @@ class HeuristicTextMagicMatcher : public IMagicMatcher { public: HeuristicTextMagicMatcher() {} virtual bool matches(const QByteArray &data) const; - virtual int priority() const { return TextMatchPriority; } + virtual int priority() const + { + return TextMatchPriority; + } static bool isTextFile(const QByteArray &data); }; @@ -173,12 +184,15 @@ public: bool HeuristicTextMagicMatcher::isTextFile(const QByteArray &data) { const int size = data.size(); + for (int i = 0; i < size; i++) { const char c = data.at(i); - if (c >= 0x01 && c < 0x09) // Sure-fire binary + if (c >= 0x01 && c < 0x09) { // Sure-fire binary return false; - if (c == 0) // Check for UTF16 + } + if (c == 0) { // Check for UTF16 return data.startsWith(bigEndianByteOrderMarkC) || data.startsWith(littleEndianByteOrderMarkC); + } } return true; } @@ -186,11 +200,12 @@ bool HeuristicTextMagicMatcher::isTextFile(const QByteArray &data) bool HeuristicTextMagicMatcher::matches(const QByteArray &data) const { const bool rc = isTextFile(data); - if (debugMimeDB) + + if (debugMimeDB) { qDebug() << Q_FUNC_INFO << " on " << data.size() << " returns " << rc; + } return rc; } - } // namespace Internal // MagicRule @@ -198,18 +213,20 @@ MagicRule::MagicRule(const QByteArray &pattern, int startPos, int endPos) : m_pattern(pattern), m_startPos(startPos), m_endPos(endPos) -{ -} +{} bool MagicRule::matches(const QByteArray &data) const { // Quick check const int dataSize = data.size(); - if ((m_startPos + m_pattern.size()) >= dataSize) + + if ((m_startPos + m_pattern.size()) >= dataSize) { return false; + } // Most common: some string at position 0: - if (m_startPos == 0 && m_startPos == m_endPos) + if (m_startPos == 0 && m_startPos == m_endPos) { return data.startsWith(m_pattern); + } // Range const int index = data.indexOf(m_pattern, m_startPos); return index != -1 && index < m_endPos; @@ -222,9 +239,8 @@ MagicRule *MagicRule::createStringRule(const QString &c, int startPos, int endPo // List matcher MagicRuleMatcher::MagicRuleMatcher() : - m_priority(65535) -{ -} + m_priority(65535) +{} void MagicRuleMatcher::add(const MagicRuleSharedPointer &rule) { @@ -234,9 +250,12 @@ void MagicRuleMatcher::add(const MagicRuleSharedPointer &rule) bool MagicRuleMatcher::matches(const QByteArray &data) const { const MagicRuleList::const_iterator cend = m_list.constEnd(); - for (MagicRuleList::const_iterator it = m_list.constBegin(); it != cend; ++it) - if ( (*it)->matches(data)) + + for (MagicRuleList::const_iterator it = m_list.constBegin(); it != cend; ++it) { + if ((*it)->matches(data)) { return true; + } + } return false; } @@ -253,7 +272,7 @@ void MagicRuleMatcher::setPriority(int p) // ---------- MimeTypeData class MimeTypeData : public QSharedData { public: - typedef QHash LocaleHash; + typedef QHash LocaleHash; void clear(); void debug(QTextStream &str, int indent = 0) const; @@ -287,21 +306,24 @@ void MimeTypeData::clear() void MimeTypeData::debug(QTextStream &str, int indent) const { const QString indentS = QString(indent, QLatin1Char(' ')); - const QString comma = QString(1, QLatin1Char(',')); + const QString comma = QString(1, QLatin1Char(',')); + str << indentS << "Type: " << type; - if (!aliases.empty()) + if (!aliases.empty()) { str << " Aliases: " << aliases.join(comma); + } str << ", magic: " << magicMatchers.size() << '\n'; str << indentS << "Comment: " << comment << '\n'; - if (!subClassesOf.empty()) + if (!subClassesOf.empty()) { str << indentS << "SubClassesOf: " << subClassesOf.join(comma) << '\n'; + } if (!globPatterns.empty()) { str << indentS << "Glob: "; - foreach (const QRegExp &r, globPatterns) - str << r.pattern() << ' '; + foreach(const QRegExp &r, globPatterns) + str << r.pattern() << ' '; str << '\n'; if (!suffixes.empty()) { - str << indentS << "Suffixes: " << suffixes.join(comma) + str << indentS << "Suffixes: " << suffixes.join(comma) << " preferred: " << preferredSuffix << '\n'; } } @@ -311,29 +333,26 @@ void MimeTypeData::debug(QTextStream &str, int indent) const // ---------------- MimeType MimeType::MimeType() : m_d(new MimeTypeData) -{ -} +{} MimeType::MimeType(const MimeType &rhs) : m_d(rhs.m_d) -{ -} +{} MimeType &MimeType::operator=(const MimeType &rhs) { - if (this != &rhs) + if (this != &rhs) { m_d = rhs.m_d; + } return *this; } MimeType::MimeType(const MimeTypeData &d) : m_d(new MimeTypeData(d)) -{ -} +{} MimeType::~MimeType() -{ -} +{} void MimeType::clear() { @@ -380,8 +399,10 @@ static inline QString systemLanguage() { QString name = QLocale::system().name(); const int underScorePos = name.indexOf(QLatin1Char('_')); - if (underScorePos != -1) + + if (underScorePos != -1) { name.truncate(underScorePos); + } return name; } @@ -389,14 +410,16 @@ QString MimeType::localeComment(const QString &localeArg) const { const QString locale = localeArg.isEmpty() ? systemLanguage() : localeArg; const MimeTypeData::LocaleHash::const_iterator it = m_d->localeComments.constFind(locale); - if (it == m_d->localeComments.constEnd()) + + if (it == m_d->localeComments.constEnd()) { return m_d->comment; + } return it.value(); } void MimeType::setLocaleComment(const QString &locale, const QString &comment) { - m_d->localeComments[locale] = comment; + m_d->localeComments[locale] = comment; } QStringList MimeType::aliases() const @@ -406,7 +429,7 @@ QStringList MimeType::aliases() const void MimeType::setAliases(const QStringList &a) { - m_d->aliases = a; + m_d->aliases = a; } QList MimeType::globPatterns() const @@ -450,8 +473,10 @@ bool MimeType::setPreferredSuffix(const QString &s) static QString formatFilterString(const QString &description, const QList &globs) { QString rc; - if (globs.empty()) // Binary files + + if (globs.empty()) { // Binary files return rc; + } { QTextStream str(&rc); str << description; @@ -459,8 +484,9 @@ static QString formatFilterString(const QString &description, const QListglobPatterns) { - if (pattern.exactMatch(c.fileName())) + foreach(QRegExp pattern, m_d->globPatterns) { + if (pattern.exactMatch(c.fileName())) { return GlobMatchPriority; + } } // Nope, try magic matchers on context data - if (m_d->magicMatchers.isEmpty()) + if (m_d->magicMatchers.isEmpty()) { return 0; + } const QByteArray data = c.data(); if (!data.isEmpty()) { - foreach (MimeTypeData::IMagicMatcherSharedPointer matcher, m_d->magicMatchers) { - if (matcher->matches(data)) + foreach(MimeTypeData::IMagicMatcherSharedPointer matcher, m_d->magicMatchers) { + if (matcher->matches(data)) { return matcher->priority(); + } } } return 0; @@ -530,14 +560,14 @@ QDebug operator<<(QDebug d, const MimeType &mt) QTextStream str(&s); mt.m_d->debug(str); } + d << s; return d; } namespace Internal { - -// MimeDatabase helpers: Generic parser for a sequence of . -// Calls abstract handler function process for MimeType it finds. +// MimeDatabase helpers: Generic parser for a sequence of . +// Calls abstract handler function process for MimeType it finds. class BaseMimeTypeParser { Q_DISABLE_COPY(BaseMimeTypeParser) public: @@ -553,23 +583,23 @@ private: void addGlobPattern(const QString &pattern, MimeTypeData *d) const; enum ParseStage { ParseBeginning, - ParseMimeInfo, - ParseMimeType, - ParseComment, - ParseGlobPattern, - ParseSubClass, - ParseAlias, - ParseMagic, - ParseMagicMatchRule, - ParseOtherMimeTypeSubTag, - ParseError }; + ParseMimeInfo, + ParseMimeType, + ParseComment, + ParseGlobPattern, + ParseSubClass, + ParseAlias, + ParseMagic, + ParseMagicMatchRule, + ParseOtherMimeTypeSubTag, + ParseError }; static ParseStage nextStage(ParseStage currentStage, const QStringRef &startElement); const QRegExp m_suffixPattern; }; -BaseMimeTypeParser:: BaseMimeTypeParser() : +BaseMimeTypeParser::BaseMimeTypeParser() : // RE to match a suffix glob pattern: "*.ext" (and not sth like "Makefile" or // "*.log[1-9]" m_suffixPattern(QLatin1String("^\\*\\.[\\w+]+$")) @@ -579,8 +609,9 @@ BaseMimeTypeParser:: BaseMimeTypeParser() : void BaseMimeTypeParser::addGlobPattern(const QString &pattern, MimeTypeData *d) const { - if (pattern.isEmpty()) + if (pattern.isEmpty()) { return; + } // Collect patterns as a QRegExp list and filter out the plain // suffix ones for our suffix list. Use first one as preferred const QRegExp wildCard(pattern, Qt::CaseSensitive, QRegExp::Wildcard); @@ -594,8 +625,9 @@ void BaseMimeTypeParser::addGlobPattern(const QString &pattern, MimeTypeData *d) if (m_suffixPattern.exactMatch(pattern)) { const QString suffix = pattern.right(pattern.size() - 2); d->suffixes.push_back(suffix); - if (d->preferredSuffix.isEmpty()) + if (d->preferredSuffix.isEmpty()) { d->preferredSuffix = suffix; + } } } @@ -603,13 +635,17 @@ BaseMimeTypeParser::ParseStage BaseMimeTypeParser::nextStage(ParseStage currentS { switch (currentStage) { case ParseBeginning: - if (startElement == QLatin1String(mimeInfoTagC)) + if (startElement == QLatin1String(mimeInfoTagC)) { return ParseMimeInfo; - if (startElement == QLatin1String(mimeTypeTagC)) + } + if (startElement == QLatin1String(mimeTypeTagC)) { return ParseMimeType; + } return ParseError; + case ParseMimeInfo: return startElement == QLatin1String(mimeTypeTagC) ? ParseMimeType : ParseError; + case ParseMimeType: case ParseComment: case ParseGlobPattern: @@ -617,22 +653,30 @@ BaseMimeTypeParser::ParseStage BaseMimeTypeParser::nextStage(ParseStage currentS case ParseAlias: case ParseOtherMimeTypeSubTag: case ParseMagicMatchRule: - if (startElement == QLatin1String(mimeTypeTagC)) // Sequence of + if (startElement == QLatin1String(mimeTypeTagC)) { // Sequence of return ParseMimeType; - if (startElement == QLatin1String(commentTagC )) + } + if (startElement == QLatin1String(commentTagC)) { return ParseComment; - if (startElement == QLatin1String(globTagC)) + } + if (startElement == QLatin1String(globTagC)) { return ParseGlobPattern; - if (startElement == QLatin1String(subClassTagC)) + } + if (startElement == QLatin1String(subClassTagC)) { return ParseSubClass; - if (startElement == QLatin1String(aliasTagC)) + } + if (startElement == QLatin1String(aliasTagC)) { return ParseAlias; - if (startElement == QLatin1String(magicTagC)) + } + if (startElement == QLatin1String(magicTagC)) { return ParseMagic; + } return ParseOtherMimeTypeSubTag; + case ParseMagic: - if (startElement == QLatin1String(matchTagC)) + if (startElement == QLatin1String(matchTagC)) { return ParseMagicMatchRule; + } break; case ParseError: break; @@ -644,6 +688,7 @@ BaseMimeTypeParser::ParseStage BaseMimeTypeParser::nextStage(ParseStage currentS static bool parseNumber(const QString &n, int *target, QString *errorMessage) { bool ok; + *target = n.toInt(&ok); if (!ok) { *errorMessage = QString::fromLatin1("Not a number '%1'.").arg(n); @@ -653,13 +698,14 @@ static bool parseNumber(const QString &n, int *target, QString *errorMessage) } // Evaluate a magic match rule like -// -// +// +// static bool addMagicMatchRule(const QXmlStreamAttributes &atts, MagicRuleMatcher *ruleMatcher, QString *errorMessage) { const QString type = atts.value(QLatin1String(matchTypeAttributeC)).toString(); + if (type != QLatin1String(matchStringTypeValueC)) { qWarning("%s: match type %s is not supported.", Q_FUNC_INFO, type.toUtf8().constData()); return true; @@ -671,14 +717,16 @@ static bool addMagicMatchRule(const QXmlStreamAttributes &atts, } // Parse for offset as "1" or "1:10" int startPos, endPos; - const QString offsetS = atts.value(QLatin1String(matchOffsetAttributeC)).toString(); - const int colonIndex = offsetS.indexOf(QLatin1Char(':')); + const QString offsetS = atts.value(QLatin1String(matchOffsetAttributeC)).toString(); + const int colonIndex = offsetS.indexOf(QLatin1Char(':')); const QString startPosS = colonIndex == -1 ? offsetS : offsetS.mid(0, colonIndex); const QString endPosS = colonIndex == -1 ? offsetS : offsetS.mid(colonIndex + 1); - if (!parseNumber(startPosS, &startPos, errorMessage) || !parseNumber(endPosS, &endPos, errorMessage)) + if (!parseNumber(startPosS, &startPos, errorMessage) || !parseNumber(endPosS, &endPos, errorMessage)) { return false; - if (debugMimeDB) + } + if (debugMimeDB) { qDebug() << Q_FUNC_INFO << value << startPos << endPos; + } ruleMatcher->add(QSharedPointer(MagicRule::createStringRule(value, startPos, endPos))); return true; } @@ -690,30 +738,34 @@ bool BaseMimeTypeParser::parse(QIODevice *dev, const QString &fileName, QString QXmlStreamReader reader(dev); ParseStage ps = ParseBeginning; + while (!reader.atEnd()) { switch (reader.readNext()) { case QXmlStreamReader::StartElement: ps = nextStage(ps, reader.name()); switch (ps) { - case ParseMimeType: { // start parsing a type - const QString type = reader.attributes().value(QLatin1String(mimeTypeAttributeC)).toString(); - if (type.isEmpty()) { - reader.raiseError(QString::fromLatin1("Missing 'type'-attribute")); - } else { - data.type = type; - } + case ParseMimeType: // start parsing a type + { const QString type = reader.attributes().value(QLatin1String(mimeTypeAttributeC)).toString(); + if (type.isEmpty()) { + reader.raiseError(QString::fromLatin1("Missing 'type'-attribute")); + } else { + data.type = type; + } } - break; + break; case ParseGlobPattern: addGlobPattern(reader.attributes().value(QLatin1String(patternAttributeC)).toString(), &data); break; - case ParseSubClass: { + case ParseSubClass: + { const QString inheritsFrom = reader.attributes().value(QLatin1String(mimeTypeAttributeC)).toString(); - if (!inheritsFrom.isEmpty()) + if (!inheritsFrom.isEmpty()) { data.subClassesOf.push_back(inheritsFrom); + } } - break; - case ParseComment: { + break; + case ParseComment: + { // comments have locale attributes. We want the default, English one QString locale = reader.attributes().value(QLatin1String(localeAttributeC)).toString(); const QString comment = QCoreApplication::translate("MimeType", reader.readElementText().toAscii()); @@ -723,28 +775,32 @@ bool BaseMimeTypeParser::parse(QIODevice *dev, const QString &fileName, QString data.localeComments.insert(locale, comment); } } - break; - case ParseAlias: { + break; + case ParseAlias: + { const QString alias = reader.attributes().value(QLatin1String(mimeTypeAttributeC)).toString(); - if (!alias.isEmpty()) + if (!alias.isEmpty()) { data.aliases.push_back(alias); + } } - break; - case ParseMagic: { + break; + case ParseMagic: + { int priority = 0; const QString priorityS = reader.attributes().value(QLatin1String(priorityAttributeC)).toString(); if (!priorityS.isEmpty()) { - if (!parseNumber(priorityS, &priority, errorMessage)) + if (!parseNumber(priorityS, &priority, errorMessage)) { return false; - + } } ruleMatcher = new MagicRuleMatcher; ruleMatcher->setPriority(priority); } - break; + break; case ParseMagicMatchRule: - if (!addMagicMatchRule(reader.attributes(), ruleMatcher, errorMessage)) + if (!addMagicMatchRule(reader.attributes(), ruleMatcher, errorMessage)) { return false; + } break; case ParseError: reader.raiseError(QString::fromLatin1("Unexpected element <%1>").arg(reader.name().toString())); @@ -756,8 +812,9 @@ bool BaseMimeTypeParser::parse(QIODevice *dev, const QString &fileName, QString // continue switch QXmlStreamReader::Token... case QXmlStreamReader::EndElement: // Finished element if (reader.name() == QLatin1String(mimeTypeTagC)) { - if (!process(MimeType(data), errorMessage)) + if (!process(MimeType(data), errorMessage)) { return false; + } data.clear(); } else { // Finished a match sequence @@ -779,15 +836,13 @@ bool BaseMimeTypeParser::parse(QIODevice *dev, const QString &fileName, QString } return true; } - } // namespace Internal // MimeMapEntry: Entry of a type map, consisting of type and level. enum { Dangling = 32767 }; -struct MimeMapEntry -{ +struct MimeMapEntry { explicit MimeMapEntry(const MimeType &t = MimeType(), int aLevel = Dangling); MimeType type; int level; // hierachy level @@ -796,8 +851,7 @@ struct MimeMapEntry MimeMapEntry::MimeMapEntry(const MimeType &t, int aLevel) : type(t), level(aLevel) -{ -} +{} /* MimeDatabasePrivate: Requirements for storage: * - Must be robust in case of incomplete hierachies, dangling entries @@ -822,8 +876,7 @@ MimeMapEntry::MimeMapEntry(const MimeType &t, int aLevel) : * to check the most specific types first). Starting a recursion from the * leaves is not suitable since it will hit parent nodes several times. */ -class MimeDatabasePrivate -{ +class MimeDatabasePrivate { Q_DISABLE_COPY(MimeDatabasePrivate) public: MimeDatabasePrivate(); @@ -864,31 +917,35 @@ private: MimeDatabasePrivate::MimeDatabasePrivate() : m_maxLevel(-1) -{ -} +{} namespace Internal { - // Parser that builds MimeDB hierarchy by adding to MimeDatabasePrivate - class MimeTypeParser : public BaseMimeTypeParser { - public: - explicit MimeTypeParser(MimeDatabasePrivate &db) : m_db(db) {} - private: - virtual bool process(const MimeType &t, QString *) { m_db.addMimeType(t); return true; } +// Parser that builds MimeDB hierarchy by adding to MimeDatabasePrivate +class MimeTypeParser : public BaseMimeTypeParser { +public: + explicit MimeTypeParser(MimeDatabasePrivate &db) : m_db(db) {} +private: + virtual bool process(const MimeType &t, QString *) + { + m_db.addMimeType(t); return true; + } - MimeDatabasePrivate &m_db; - }; + MimeDatabasePrivate &m_db; +}; } // namespace Internal bool MimeDatabasePrivate::addMimeTypes(QIODevice *device, const QString &fileName, QString *errorMessage) { Internal::MimeTypeParser parser(*this); + return parser.parse(device, fileName, errorMessage); } bool MimeDatabasePrivate::addMimeTypes(const QString &fileName, QString *errorMessage) { QFile file(fileName); - if (!file.open(QIODevice::ReadOnly|QIODevice::Text)) { + + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { *errorMessage = QString::fromLatin1("Cannot open %1: %2").arg(fileName, file.errorString()); return false; } @@ -902,8 +959,9 @@ bool MimeDatabasePrivate::addMimeTypes(QIODevice *device, QString *errorMessage) bool MimeDatabasePrivate::addMimeType(MimeType mt) { - if (!mt) + if (!mt) { return false; + } const QString type = mt.type(); // Hack: Add a magic text matcher to "text/plain" and the fallback matcher to @@ -911,8 +969,9 @@ bool MimeDatabasePrivate::addMimeType(MimeType mt) if (type == QLatin1String(textTypeC)) { mt.addMagicMatcher(QSharedPointer(new Internal::HeuristicTextMagicMatcher)); } else { - if (type == QLatin1String(binaryTypeC)) - mt.addMagicMatcher(QSharedPointer(new Internal::BinaryMatcher)); + if (type == QLatin1String(binaryTypeC)) { + mt.addMagicMatcher(QSharedPointer(new Internal::BinaryMatcher)); + } } // insert the type. m_typeMimeTypeMap.insert(type, MimeMapEntry(mt)); @@ -922,42 +981,48 @@ bool MimeDatabasePrivate::addMimeType(MimeType mt) const QStringList subClassesOf = mt.subClassesOf(); if (!subClassesOf.empty()) { const QStringList::const_iterator socend = subClassesOf.constEnd(); - for (QStringList::const_iterator soit = subClassesOf.constBegin(); soit != socend; ++soit) + for (QStringList::const_iterator soit = subClassesOf.constBegin(); soit != socend; ++soit) { m_parentChildrenMap.insert(resolveAlias(*soit), type); + } } // register aliasses const QStringList aliases = mt.aliases(); if (!aliases.empty()) { const QStringList::const_iterator cend = aliases.constEnd(); - for (QStringList::const_iterator it = aliases.constBegin(); it != cend; ++it) + for (QStringList::const_iterator it = aliases.constBegin(); it != cend; ++it) { m_aliasMap.insert(*it, type); + } } m_maxLevel = -1; // Mark as dirty return true; } -const QString &MimeDatabasePrivate::resolveAlias(const QString &name) const +const QString &MimeDatabasePrivate::resolveAlias(const QString &name) const { const AliasMap::const_iterator aliasIt = m_aliasMap.constFind(name); + return aliasIt == m_aliasMap.constEnd() ? name : aliasIt.value(); } void MimeDatabasePrivate::raiseLevelRecursion(MimeMapEntry &e, int level) { - if (e.level == Dangling || e.level < level) + if (e.level == Dangling || e.level < level) { e.level = level; - if (m_maxLevel < level) + } + if (m_maxLevel < level) { m_maxLevel = level; + } // At all events recurse over children since nodes might have been // added. const QStringList childTypes = m_parentChildrenMap.values(e.type.type()); - if (childTypes.empty()) + if (childTypes.empty()) { return; + } // look them up in the type->mime type map const int nextLevel = level + 1; const TypeMimeTypeMap::iterator tm_end = m_typeMimeTypeMap.end(); const QStringList::const_iterator cend = childTypes.constEnd(); - for (QStringList::const_iterator it = childTypes.constBegin(); it != cend; ++it) { + for (QStringList::const_iterator it = childTypes.constBegin(); it != cend; ++it) { const TypeMimeTypeMap::iterator tm_it = m_typeMimeTypeMap.find(resolveAlias(*it)); if (tm_it == tm_end) { qWarning("%s: Inconsistent mime hierarchy detected, child %s of %s cannot be found.", @@ -979,17 +1044,19 @@ void MimeDatabasePrivate::determineLevels() // sets of parents/children QSet parentSet, childrenSet; const ParentChildrenMap::const_iterator pcend = m_parentChildrenMap.constEnd(); - for (ParentChildrenMap::const_iterator it = m_parentChildrenMap.constBegin(); it != pcend; ++it) + for (ParentChildrenMap::const_iterator it = m_parentChildrenMap.constBegin(); it != pcend; ++it) { if (m_typeMimeTypeMap.contains(it.key())) { parentSet.insert(it.key()); childrenSet.insert(it.value()); } + } const QSet topLevels = parentSet.subtract(childrenSet); - if (debugMimeDB) + if (debugMimeDB) { qDebug() << Q_FUNC_INFO << "top levels" << topLevels; + } const TypeMimeTypeMap::iterator tm_end = m_typeMimeTypeMap.end(); const QSet::const_iterator tl_cend = topLevels.constEnd(); - for (QSet::const_iterator tl_it = topLevels.constBegin(); tl_it != tl_cend; ++tl_it) { + for (QSet::const_iterator tl_it = topLevels.constBegin(); tl_it != tl_cend; ++tl_it) { const TypeMimeTypeMap::iterator tm_it = m_typeMimeTypeMap.find(resolveAlias(*tl_it)); if (tm_it == tm_end) { qWarning("%s: Inconsistent mime hierarchy detected, top level element %s cannot be found.", @@ -1002,18 +1069,22 @@ void MimeDatabasePrivate::determineLevels() bool MimeDatabasePrivate::setPreferredSuffix(const QString &typeOrAlias, const QString &suffix) { - TypeMimeTypeMap::iterator tit = m_typeMimeTypeMap.find(resolveAlias(typeOrAlias)); - if (tit != m_typeMimeTypeMap.end()) + TypeMimeTypeMap::iterator tit = m_typeMimeTypeMap.find(resolveAlias(typeOrAlias)); + + if (tit != m_typeMimeTypeMap.end()) { return tit.value().type.setPreferredSuffix(suffix); + } return false; } // Returns a mime type or Null one if none found MimeType MimeDatabasePrivate::findByType(const QString &typeOrAlias) const { - const TypeMimeTypeMap::const_iterator tit = m_typeMimeTypeMap.constFind(resolveAlias(typeOrAlias)); - if (tit != m_typeMimeTypeMap.constEnd()) + const TypeMimeTypeMap::const_iterator tit = m_typeMimeTypeMap.constFind(resolveAlias(typeOrAlias)); + + if (tit != m_typeMimeTypeMap.constEnd()) { return tit.value().type; + } return MimeType(); } @@ -1021,8 +1092,10 @@ MimeType MimeDatabasePrivate::findByType(const QString &typeOrAlias) const MimeType MimeDatabasePrivate::findByFile(const QFileInfo &f) const { unsigned priority = 0; - if (debugMimeDB) + + if (debugMimeDB) { qDebug() << '>' << Q_FUNC_INFO << f.fileName(); + } const MimeType rc = findByFile(f, &priority); if (debugMimeDB) { if (rc) { @@ -1051,13 +1124,14 @@ MimeType MimeDatabasePrivate::findByFile(const QFileInfo &f, unsigned *priorityP MimeType rc; Internal::FileMatchContext context(f); const TypeMimeTypeMap::const_iterator cend = m_typeMimeTypeMap.constEnd(); - for (int level = m_maxLevel; level >= 0; level--) - for (TypeMimeTypeMap::const_iterator it = m_typeMimeTypeMap.constBegin(); it != cend; ++it) + for (int level = m_maxLevel; level >= 0; level--) { + for (TypeMimeTypeMap::const_iterator it = m_typeMimeTypeMap.constBegin(); it != cend; ++it) { if (it.value().level == level) { const unsigned priority = it.value().type.matchesFile(context); - if (debugMimeDB > 1) - qDebug() << "pass" << level << it.value().type.type() << " matches " << priority; - if (priority) + if (debugMimeDB > 1) { + qDebug() << "pass" << level << it.value().type.type() << " matches " << priority; + } + if (priority) { if (priority > maxPriority) { rc = it.value().type; maxPriority = priority; @@ -1067,7 +1141,10 @@ MimeType MimeDatabasePrivate::findByFile(const QFileInfo &f, unsigned *priorityP return rc; } } + } } + } + } return rc; } @@ -1076,8 +1153,10 @@ QStringList MimeDatabasePrivate::suffixes() const { QStringList rc; const TypeMimeTypeMap::const_iterator cend = m_typeMimeTypeMap.constEnd(); - for (TypeMimeTypeMap::const_iterator it = m_typeMimeTypeMap.constBegin(); it != cend; ++it) + + for (TypeMimeTypeMap::const_iterator it = m_typeMimeTypeMap.constBegin(); it != cend; ++it) { rc += it.value().type.suffixes(); + } return rc; } @@ -1085,8 +1164,10 @@ QStringList MimeDatabasePrivate::filterStrings() const { QStringList rc; const TypeMimeTypeMap::const_iterator cend = m_typeMimeTypeMap.constEnd(); - for (TypeMimeTypeMap::const_iterator it = m_typeMimeTypeMap.constBegin(); it != cend; ++it) + + for (TypeMimeTypeMap::const_iterator it = m_typeMimeTypeMap.constBegin(); it != cend; ++it) { rc += it.value().type.filterString(); + } return rc; } @@ -1104,8 +1185,7 @@ void MimeDatabasePrivate::debug(QTextStream &str) const // --------------- MimeDatabase MimeDatabase::MimeDatabase() : m_d(new MimeDatabasePrivate) -{ -} +{} MimeDatabase::~MimeDatabase() { @@ -1122,7 +1202,7 @@ MimeType MimeDatabase::findByFile(const QFileInfo &f) const return m_d->findByFile(f); } -bool MimeDatabase::addMimeType(const MimeType &mt) +bool MimeDatabase::addMimeType(const MimeType &mt) { return m_d->addMimeType(mt); } @@ -1149,15 +1229,17 @@ QStringList MimeDatabase::filterStrings() const QString MimeDatabase::preferredSuffixByType(const QString &type) const { - if (const MimeType mt = findByType(type)) + if (const MimeType mt = findByType(type)) { return mt.preferredSuffix(); + } return QString(); } QString MimeDatabase::preferredSuffixByFile(const QFileInfo &f) const { - if (const MimeType mt = findByFile(f)) + if (const MimeType mt = findByFile(f)) { return mt.preferredSuffix(); + } return QString(); } @@ -1173,8 +1255,8 @@ QDebug operator<<(QDebug d, const MimeDatabase &mt) QTextStream str(&s); mt.m_d->debug(str); } + d << s; return d; } - } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.h b/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.h index 9265a25d1..1726852f4 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/mimedatabase.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,18 +43,16 @@ class QFileInfo; QT_END_NAMESPACE namespace Core { - class MimeTypeData; class MimeDatabasePrivate; namespace Internal { - class BaseMimeTypeParser; - class FileMatchContext; +class BaseMimeTypeParser; +class FileMatchContext; } /* Magic (file contents) matcher interface. */ -class CORE_EXPORT IMagicMatcher -{ +class CORE_EXPORT IMagicMatcher { Q_DISABLE_COPY(IMagicMatcher) protected: IMagicMatcher() {} @@ -69,8 +67,7 @@ public: /* Utility class: A standard Magic match rule based on contents. Provides * static factory methods for creation (currently only for "string". This can * be extended to handle "little16"/"big16", etc.). */ -class CORE_EXPORT MagicRule -{ +class CORE_EXPORT MagicRule { Q_DISABLE_COPY(MagicRule) public: explicit MagicRule(const QByteArray &pattern, int startPos, int endPos); @@ -87,8 +84,7 @@ private: /* Utility class: A Magic matcher that checks a number of rules based on * operator "or". It is used for rules parsed from XML files. */ -class CORE_EXPORT MagicRuleMatcher : public IMagicMatcher -{ +class CORE_EXPORT MagicRuleMatcher : public IMagicMatcher { Q_DISABLE_COPY(MagicRuleMatcher) public: typedef QSharedPointer MagicRuleSharedPointer; @@ -115,15 +111,14 @@ private: * Extensions: * - List of suffixes and preferred suffix (derived from glob patterns). */ -class CORE_EXPORT MimeType -{ +class CORE_EXPORT MimeType { public: /* Return value of a glob match, which is higher than magic */ enum { GlobMatchPriority = 101 }; MimeType(); - MimeType(const MimeType&); - MimeType &operator=(const MimeType&); + MimeType(const MimeType &); + MimeType &operator=(const MimeType &); ~MimeType(); void clear(); @@ -156,7 +151,7 @@ public: // Extension over standard mime data QString preferredSuffix() const; - bool setPreferredSuffix(const QString&); + bool setPreferredSuffix(const QString &); // Check for type or one of the aliases bool matchesType(const QString &type) const; @@ -188,8 +183,7 @@ private: * * A good testcase is to run it over '/usr/share/mime/<*>/<*>.xml' on Linux. */ -class CORE_EXPORT MimeDatabase -{ +class CORE_EXPORT MimeDatabase { Q_DISABLE_COPY(MimeDatabase) public: MimeDatabase(); @@ -198,7 +192,7 @@ public: bool addMimeTypes(const QString &fileName, QString *errorMessage); bool addMimeTypes(QIODevice *device, QString *errorMessage); - bool addMimeType(const MimeType &mt); + bool addMimeType(const MimeType &mt); // Returns a mime type or Null one if none found MimeType findByType(const QString &type) const; @@ -220,7 +214,6 @@ public: private: MimeDatabasePrivate *m_d; }; - } // namespace Core #endif // MIMEDATABASE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.cpp b/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.cpp index d75105592..4142a9a21 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,12 +36,10 @@ namespace Core { namespace Internal { - -class MiniSplitterHandle : public QSplitterHandle -{ +class MiniSplitterHandle : public QSplitterHandle { public: MiniSplitterHandle(Qt::Orientation orientation, QSplitter *parent) - : QSplitterHandle(orientation, parent) + : QSplitterHandle(orientation, parent) { setMask(QRegion(contentsRect())); setAttribute(Qt::WA_MouseNoMask, true); @@ -50,7 +48,6 @@ protected: void resizeEvent(QResizeEvent *event); void paintEvent(QPaintEvent *event); }; - } // namespace Internal } // namespace Core @@ -66,10 +63,11 @@ void MiniSplitterHandle::resizeEvent(QResizeEvent *event) // This means that with Qt upgrades it's worthwhile to see if anything changed // in QSplitterHandle::resizeEvent, to see if there's anything important we miss. - if (orientation() == Qt::Horizontal) + if (orientation() == Qt::Horizontal) { setContentsMargins(6, 0, 6, 0); - else + } else { setContentsMargins(0, 6, 0, 6); + } setMask(QRegion(contentsRect())); QWidget::resizeEvent(event); @@ -78,6 +76,7 @@ void MiniSplitterHandle::resizeEvent(QResizeEvent *event) void MiniSplitterHandle::paintEvent(QPaintEvent *event) { QPainter painter(this); + painter.fillRect(event->rect(), Utils::StyleHelper::borderColor()); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.h b/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.h index 94803aacd..b446acbac 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/minisplitter.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,10 +38,8 @@ class QSplitterHandle; QT_END_NAMESPACE namespace Core { - /*! This is a simple helper-class to obtain mac-style 1-pixel wide splitters */ -class CORE_EXPORT MiniSplitter : public QSplitter -{ +class CORE_EXPORT MiniSplitter : public QSplitter { public: MiniSplitter(QWidget *parent = 0); MiniSplitter(Qt::Orientation orientation); @@ -49,7 +47,6 @@ public: protected: QSplitterHandle *createHandle(); }; - } // namespace Core #endif // MINISPLITTER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp index 6ecd3d5e6..2e67778f1 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -68,18 +68,18 @@ ModeManager::ModeManager(Internal::MainWindow *mainWindow, MyTabWidget *modeStac { m_instance = this; -// connect((m_modeStack), SIGNAL(currentAboutToShow(int)), SLOT(currentTabAboutToChange(int))); +// connect((m_modeStack), SIGNAL(currentAboutToShow(int)), SLOT(currentTabAboutToChange(int))); connect(m_modeStack, SIGNAL(currentChanged(int)), this, SLOT(currentTabChanged(int))); - connect(m_modeStack, SIGNAL(tabMoved(int,int)), this, SLOT(tabMoved(int,int))); + connect(m_modeStack, SIGNAL(tabMoved(int, int)), this, SLOT(tabMoved(int, int))); connect(m_signalMapper, SIGNAL(mapped(QString)), this, SLOT(activateMode(QString))); } void ModeManager::init() { - QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(objectAdded(QObject*)), - this, SLOT(objectAdded(QObject*))); - QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(aboutToRemoveObject(QObject*)), - this, SLOT(aboutToRemoveObject(QObject*))); + QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(objectAdded(QObject *)), + this, SLOT(objectAdded(QObject *))); + QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(aboutToRemoveObject(QObject *)), + this, SLOT(aboutToRemoveObject(QObject *))); } void ModeManager::addWidget(QWidget *widget) @@ -89,48 +89,53 @@ void ModeManager::addWidget(QWidget *widget) // We want the actionbar to stay on the bottom // so m_modeStack->cornerWidgetCount() -1 inserts it at the position immediately above // the actionbar -// m_modeStack->insertCornerWidget(m_modeStack->cornerWidgetCount() -1, widget); +// m_modeStack->insertCornerWidget(m_modeStack->cornerWidgetCount() -1, widget); } IMode *ModeManager::currentMode() const { - if (m_modes.count() > m_modeStack->currentIndex() ) + if (m_modes.count() > m_modeStack->currentIndex()) { return m_modes.at(m_modeStack->currentIndex()); - else + } else { m_modeStack->setCurrentIndex(0); // Fix illegal Index. + } return 0; } int ModeManager::indexOf(const QString &id) const { for (int i = 0; i < m_modes.count(); ++i) { - if (m_modes.at(i)->uniqueModeName() == id) + if (m_modes.at(i)->uniqueModeName() == id) { return i; + } } -// qDebug() << "Warning, no such mode:" << id; +// qDebug() << "Warning, no such mode:" << id; return -1; } IMode *ModeManager::mode(const QString &id) const { const int index = indexOf(id); - if (index >= 0) + + if (index >= 0) { return m_modes.at(index); + } return 0; } void ModeManager::activateMode(const QString &id) { const int index = indexOf(id); - if (index >= 0) + + if (index >= 0) { m_modeStack->setCurrentIndex(index); + } } void ModeManager::activateModeByWorkspaceName(const QString &id) { for (int i = 0; i < m_modes.count(); ++i) { - if (m_modes.at(i)->name() == id) - { + if (m_modes.at(i)->name() == id) { m_modeStack->setCurrentIndex(i); return; } @@ -140,22 +145,25 @@ void ModeManager::activateModeByWorkspaceName(const QString &id) void ModeManager::objectAdded(QObject *obj) { IMode *mode = Aggregation::query(obj); - if (!mode) + + if (!mode) { return; + } m_mainWindow->addContextObject(mode); // Count the number of modes with a higher priority int index = 0; - foreach (const IMode *m, m_modes) - if (m->priority() > mode->priority()) - ++index; + foreach(const IMode * m, m_modes) + if (m->priority() > mode->priority()) { + ++index; + } m_modes.insert(index, mode); m_modeStack->insertTab(index, mode->widget(), mode->icon(), mode->name()); // Register mode shortcut - ActionManager *am = m_mainWindow->actionManager(); + ActionManager *am = m_mainWindow->actionManager(); const QString shortcutId = QLatin1String("GCS.Mode.") + mode->uniqueModeName(); QShortcut *shortcut = new QShortcut(m_mainWindow); shortcut->setWhatsThis(tr("Switch to %1 mode").arg(mode->name())); @@ -170,36 +178,42 @@ void ModeManager::objectAdded(QObject *obj) connect(shortcut, SIGNAL(activated()), m_signalMapper, SLOT(map())); } -void ModeManager::setDefaultKeyshortcuts() { +void ModeManager::setDefaultKeyshortcuts() +{ for (int i = 0; i < m_modeShortcuts.size(); ++i) { Command *currentCmd = m_modeShortcuts.at(i); bool currentlyHasDefaultSequence = (currentCmd->keySequence() == currentCmd->defaultKeySequence()); #ifdef Q_WS_MAC - currentCmd->setDefaultKeySequence(QKeySequence(QString("Meta+%1").arg(i+1))); + currentCmd->setDefaultKeySequence(QKeySequence(QString("Meta+%1").arg(i + 1))); #else - currentCmd->setDefaultKeySequence(QKeySequence(QString("Ctrl+%1").arg(i+1))); + currentCmd->setDefaultKeySequence(QKeySequence(QString("Ctrl+%1").arg(i + 1))); #endif - if (currentlyHasDefaultSequence) + if (currentlyHasDefaultSequence) { currentCmd->setKeySequence(currentCmd->defaultKeySequence()); + } } } void ModeManager::updateModeToolTip() { Command *cmd = qobject_cast(sender()); + if (cmd) { int index = m_modeShortcuts.indexOf(cmd); - if (index != -1) + if (index != -1) { m_modeStack->setTabToolTip(index, cmd->stringWithAppendedShortcut(cmd->shortcut()->whatsThis())); + } } } void ModeManager::updateModeNameIcon(IMode *mode, const QIcon &icon, const QString &label) { int index = indexOf(mode->uniqueModeName()); - if (index < 0) + + if (index < 0) { return; + } m_modeStack->setTabIcon(index, icon); m_modeStack->setTabText(index, label); } @@ -207,8 +221,10 @@ void ModeManager::updateModeNameIcon(IMode *mode, const QIcon &icon, const QStri void ModeManager::aboutToRemoveObject(QObject *obj) { IMode *mode = Aggregation::query(obj); - if (!mode) + + if (!mode) { return; + } const int index = m_modes.indexOf(mode); m_modes.remove(index); @@ -228,25 +244,27 @@ void ModeManager::addAction(Command *command, int priority, QMenu *menu) // Count the number of commands with a higher priority int index = 0; - foreach (int p, m_actions.values()) - if (p > priority) - ++index; + foreach(int p, m_actions.values()) + if (p > priority) { + ++index; + } -// m_actionBar->insertAction(index, command->action(), menu); +// m_actionBar->insertAction(index, command->action(), menu); } void ModeManager::currentTabAboutToChange(int index) { if (index >= 0) { IMode *mode = m_modes.at(index); - if (mode) + if (mode) { emit currentModeAboutToChange(mode); + } } } void ModeManager::currentTabChanged(int index) { -// qDebug() << "Current tab changed " << index; +// qDebug() << "Current tab changed " << index; // Tab index changes to -1 when there is no tab left. if (index >= 0) { IMode *mode = m_modes.at(index); @@ -255,12 +273,12 @@ void ModeManager::currentTabChanged(int index) // they use the editor widget, which is already a context widget so the main window won't // go further up the parent tree to find the mode context. ICore *core = ICore::instance(); - foreach (const int context, m_addedContexts) - core->removeAdditionalContext(context); + foreach(const int context, m_addedContexts) + core->removeAdditionalContext(context); m_addedContexts = mode->context(); - foreach (const int context, m_addedContexts) - core->addAdditionalContext(context); + foreach(const int context, m_addedContexts) + core->addAdditionalContext(context); emit currentModeChanged(mode); core->updateContext(); } @@ -269,6 +287,7 @@ void ModeManager::currentTabChanged(int index) void ModeManager::tabMoved(int from, int to) { IMode *mode = m_modes.at(from); + m_modes.remove(from); m_modes.insert(to, mode); Command *cmd = m_modeShortcuts.at(from); @@ -278,30 +297,30 @@ void ModeManager::tabMoved(int from, int to) // Reprioritize, high priority means show to the left if (!m_isReprioritizing) { for (int i = 0; i < m_modes.count(); ++i) { - m_modes.at(i)->setPriority(100-i); + m_modes.at(i)->setPriority(100 - i); } emit newModeOrder(m_modes); - } + } } void ModeManager::reorderModes(QMap priorities) { - foreach (IMode *mode, m_modes) - mode->setPriority(priorities.value(QString(QLatin1String(mode->uniqueModeName())), mode->priority())); + foreach(IMode * mode, m_modes) + mode->setPriority(priorities.value(QString(QLatin1String(mode->uniqueModeName())), mode->priority())); m_isReprioritizing = true; IMode *current = currentMode(); // Bubble sort - bool swapped = false; + bool swapped = false; do { swapped = false; - for (int i = 0; i < m_modes.count()-1; ++i) { + for (int i = 0; i < m_modes.count() - 1; ++i) { IMode *mode1 = m_modes.at(i); - IMode *mode2 = m_modes.at(i+1); -// qDebug() << "Comparing " << i << " to " << i+1 << " p1 " << mode1->priority() << " p2 " << mode2->priority(); + IMode *mode2 = m_modes.at(i + 1); +// qDebug() << "Comparing " << i << " to " << i+1 << " p1 " << mode1->priority() << " p2 " << mode2->priority(); if (mode2->priority() > mode1->priority()) { - m_modeStack->moveTab(i, i+1); -// qDebug() << "Tab moved from " << i << " to " << i+1; + m_modeStack->moveTab(i, i + 1); +// qDebug() << "Tab moved from " << i << " to " << i+1; swapped = true; } } @@ -315,22 +334,25 @@ void ModeManager::reorderModes(QMap priorities) void ModeManager::setFocusToCurrentMode() { - IMode *mode = currentMode(); - QTC_ASSERT(mode, return); + IMode *mode = currentMode(); + + QTC_ASSERT(mode, return ); QWidget *widget = mode->widget(); if (widget) { QWidget *focusWidget = widget->focusWidget(); - if (focusWidget) + + if (focusWidget) { focusWidget->setFocus(); - else + } else { widget->setFocus(); + } } } void ModeManager::triggerAction(const QString &actionId) { - foreach(Command * command, m_actions.keys()){ - if(command->action()->objectName() == actionId) { + foreach(Command * command, m_actions.keys()) { + if (command->action()->objectName() == actionId) { command->action()->trigger(); break; } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h index a2ea95e8c..6568f5473 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,7 +44,6 @@ class MyTabWidget; QT_END_NAMESPACE namespace Core { - class Command; class IMode; @@ -54,29 +53,34 @@ class FancyActionBar; class MainWindow; } // namespace Internal -class CORE_EXPORT ModeManager : public QObject -{ +class CORE_EXPORT ModeManager : public QObject { Q_OBJECT public: ModeManager(Internal::MainWindow *mainWindow, MyTabWidget *modeStack); void init(); - static ModeManager *instance() { return m_instance; } + static ModeManager *instance() + { + return m_instance; + } - IMode* currentMode() const; - IMode* mode(const QString &id) const; + IMode *currentMode() const; + IMode *mode(const QString &id) const; void addAction(Command *command, int priority, QMenu *menu = 0); void addWidget(QWidget *widget); void updateModeNameIcon(IMode *mode, const QIcon &icon, const QString &label); - QVector modes() const { return m_modes; } + QVector modes() const + { + return m_modes; + } void reorderModes(QMap priorities); signals: void currentModeAboutToChange(Core::IMode *mode); void currentModeChanged(Core::IMode *mode); - void newModeOrder(QVector modes); + void newModeOrder(QVector modes); public slots: void activateMode(const QString &id); @@ -99,15 +103,14 @@ private: static ModeManager *m_instance; Internal::MainWindow *m_mainWindow; MyTabWidget *m_modeStack; - QMap m_actions; - QVector m_modes; - QVector m_modeShortcuts; + QMap m_actions; + QVector m_modes; + QVector m_modeShortcuts; QSignalMapper *m_signalMapper; QList m_addedContexts; QList m_tabOrder; bool m_isReprioritizing; }; - } // namespace Core #endif // MODEMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/outputpane.h b/ground/openpilotgcs/src/plugins/coreplugin/outputpane.h index 4e0b438c0..77e0d3684 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/outputpane.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/outputpane.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,7 +43,6 @@ class QPushButton; QT_END_NAMESPACE namespace Core { - class IMode; class IOutputPane; @@ -53,8 +52,7 @@ class MainWindow; } -class CORE_EXPORT OutputPanePlaceHolder : public QWidget -{ +class CORE_EXPORT OutputPanePlaceHolder : public QWidget { friend class Core::Internal::OutputPaneManager; // needs to set m_visible and thus access m_current Q_OBJECT public: @@ -62,20 +60,21 @@ public: ~OutputPanePlaceHolder(); void setCloseable(bool b); bool closeable(); - static OutputPanePlaceHolder *getCurrent() { return m_current; } + static OutputPanePlaceHolder *getCurrent() + { + return m_current; + } private slots: void currentModeChanged(Core::IMode *); private: Core::IMode *m_mode; bool m_closeable; - static OutputPanePlaceHolder* m_current; + static OutputPanePlaceHolder *m_current; }; namespace Internal { - -class OutputPaneManager : public QWidget -{ +class OutputPaneManager : public QWidget { Q_OBJECT public: @@ -127,7 +126,7 @@ private: QToolButton *m_nextToolButton; QWidget *m_toolBar; - QMap m_pageMap; + QMap m_pageMap; int m_lastIndex; QStackedWidget *m_outputWidgetPane; @@ -136,7 +135,6 @@ private: QMap m_buttons; QMap m_actions; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.cpp b/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.cpp index 49eaa081d..72e32b117 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -45,14 +45,15 @@ using namespace Core::Internal; PluginDialog::PluginDialog(QWidget *parent) : QDialog(parent), - m_view(new ExtensionSystem::PluginView(ExtensionSystem::PluginManager::instance(), this)) + m_view(new ExtensionSystem::PluginView(ExtensionSystem::PluginManager::instance(), this)) { QVBoxLayout *vl = new QVBoxLayout(this); + vl->addWidget(m_view); m_detailsButton = new QPushButton(tr("Details"), this); m_errorDetailsButton = new QPushButton(tr("Error Details"), this); - m_closeButton = new QPushButton(tr("Close"), this); + m_closeButton = new QPushButton(tr("Close"), this); m_detailsButton->setEnabled(false); m_errorDetailsButton->setEnabled(false); m_closeButton->setEnabled(true); @@ -70,10 +71,10 @@ PluginDialog::PluginDialog(QWidget *parent) setWindowTitle(tr("Installed Plugins")); setWindowIcon(QIcon(":/core/images/pluginicon.png")); - connect(m_view, SIGNAL(currentPluginChanged(ExtensionSystem::PluginSpec*)), + connect(m_view, SIGNAL(currentPluginChanged(ExtensionSystem::PluginSpec *)), this, SLOT(updateButtons())); - connect(m_view, SIGNAL(pluginActivated(ExtensionSystem::PluginSpec*)), - this, SLOT(openDetails(ExtensionSystem::PluginSpec*))); + connect(m_view, SIGNAL(pluginActivated(ExtensionSystem::PluginSpec *)), + this, SLOT(openDetails(ExtensionSystem::PluginSpec *))); connect(m_detailsButton, SIGNAL(clicked()), this, SLOT(openDetails())); connect(m_errorDetailsButton, SIGNAL(clicked()), this, SLOT(openErrorDetails())); connect(m_closeButton, SIGNAL(clicked()), this, SLOT(accept())); @@ -83,6 +84,7 @@ PluginDialog::PluginDialog(QWidget *parent) void PluginDialog::updateButtons() { ExtensionSystem::PluginSpec *selectedSpec = m_view->currentPlugin(); + if (selectedSpec) { m_detailsButton->setEnabled(true); m_errorDetailsButton->setEnabled(selectedSpec->hasError()); @@ -100,8 +102,9 @@ void PluginDialog::openDetails() void PluginDialog::openDetails(ExtensionSystem::PluginSpec *spec) { - if (!spec) + if (!spec) { return; + } QDialog dialog(this); dialog.setWindowTitle(tr("Plugin Details of %1").arg(spec->name())); QVBoxLayout *layout = new QVBoxLayout; @@ -120,8 +123,10 @@ void PluginDialog::openDetails(ExtensionSystem::PluginSpec *spec) void PluginDialog::openErrorDetails() { ExtensionSystem::PluginSpec *spec = m_view->currentPlugin(); - if (!spec) + + if (!spec) { return; + } QDialog dialog(this); dialog.setWindowTitle(tr("Plugin Errors of %1").arg(spec->name())); QVBoxLayout *layout = new QVBoxLayout; @@ -136,4 +141,3 @@ void PluginDialog::openErrorDetails() dialog.resize(500, 300); dialog.exec(); } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.h b/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.h index 486347cc5..b2d19e594 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/plugindialog.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,9 +42,7 @@ class PluginView; namespace Core { namespace Internal { - -class PluginDialog : public QDialog -{ +class PluginDialog : public QDialog { Q_OBJECT public: @@ -63,7 +61,6 @@ private: QPushButton *m_errorDetailsButton; QPushButton *m_closeButton; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/rightpane.cpp b/ground/openpilotgcs/src/plugins/coreplugin/rightpane.cpp index 29b7fc9af..9fe528ae0 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/rightpane.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/rightpane.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,13 +44,13 @@ using namespace Core::Internal; RightPanePlaceHolder *RightPanePlaceHolder::m_current = 0; -RightPanePlaceHolder* RightPanePlaceHolder::current() +RightPanePlaceHolder *RightPanePlaceHolder::current() { return m_current; } RightPanePlaceHolder::RightPanePlaceHolder(Core::IMode *mode, QWidget *parent) - :QWidget(parent), m_mode(mode) + : QWidget(parent), m_mode(mode) { setLayout(new QVBoxLayout); layout()->setMargin(0); @@ -73,14 +73,15 @@ void RightPanePlaceHolder::applyStoredSize(int width) if (splitter) { // A splitter we need to resize the splitter sizes QList sizes = splitter->sizes(); - int index = splitter->indexOf(this); - int diff = width - sizes.at(index); + int index = splitter->indexOf(this); + int diff = width - sizes.at(index); int adjust = sizes.count() > 1 ? (diff / (sizes.count() - 1)) : 0; for (int i = 0; i < sizes.count(); ++i) { - if (i != index) + if (i != index) { sizes[i] -= adjust; + } } - sizes[index]= width; + sizes[index] = width; splitter->setSizes(sizes); } else { QSize s = size(); @@ -152,6 +153,7 @@ RightPaneWidget::~RightPaneWidget() void RightPaneWidget::objectAdded(QObject *obj) { BaseRightPaneWidget *rpw = qobject_cast(obj); + if (rpw) { layout()->addWidget(rpw->widget()); setFocusProxy(rpw->widget()); @@ -161,6 +163,7 @@ void RightPaneWidget::objectAdded(QObject *obj) void RightPaneWidget::aboutToRemoveObject(QObject *obj) { BaseRightPaneWidget *rpw = qobject_cast(obj); + if (rpw) { delete rpw->widget(); } @@ -178,8 +181,9 @@ int RightPaneWidget::storedWidth() void RightPaneWidget::resizeEvent(QResizeEvent *re) { - if (m_width && re->size().width()) + if (m_width && re->size().width()) { m_width = re->size().width(); + } QWidget::resizeEvent(re); } @@ -194,15 +198,16 @@ void RightPaneWidget::readSettings(QSettings *settings) if (settings->contains("RightPane/Visible")) { setShown(settings->value("RightPane/Visible").toBool()); } else { - setShown(false); //TODO set to false + setShown(false); // TODO set to false } if (settings->contains("RightPane/Width")) { m_width = settings->value("RightPane/Width").toInt(); - if (!m_width) + if (!m_width) { m_width = 500; + } } else { - m_width = 500; //pixel + m_width = 500; // pixel } // Apply if (RightPanePlaceHolder::m_current) { @@ -212,8 +217,9 @@ void RightPaneWidget::readSettings(QSettings *settings) void RightPaneWidget::setShown(bool b) { - if (RightPanePlaceHolder::m_current) + if (RightPanePlaceHolder::m_current) { RightPanePlaceHolder::m_current->setVisible(b); + } m_shown = b; } @@ -232,9 +238,7 @@ BaseRightPaneWidget::BaseRightPaneWidget(QWidget *widget) } BaseRightPaneWidget::~BaseRightPaneWidget() -{ - -} +{} QWidget *BaseRightPaneWidget::widget() const { diff --git a/ground/openpilotgcs/src/plugins/coreplugin/rightpane.h b/ground/openpilotgcs/src/plugins/coreplugin/rightpane.h index 24259149e..e88f05dbe 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/rightpane.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/rightpane.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,7 +38,6 @@ class QSettings; QT_END_NAMESPACE namespace Core { - class IMode; class RightPaneWidget; @@ -49,8 +48,7 @@ class RightPaneWidget; // RightPaneWidget::setWidget(QWidget *w) Anyway if a second plugin wants to // show something there, redesign this API -class CORE_EXPORT RightPanePlaceHolder : public QWidget -{ +class CORE_EXPORT RightPanePlaceHolder : public QWidget { friend class Core::RightPaneWidget; Q_OBJECT @@ -65,12 +63,11 @@ private slots: private: void applyStoredSize(int width); Core::IMode *m_mode; - static RightPanePlaceHolder* m_current; + static RightPanePlaceHolder *m_current; }; -class CORE_EXPORT BaseRightPaneWidget : public QObject -{ +class CORE_EXPORT BaseRightPaneWidget : public QObject { Q_OBJECT public: @@ -83,8 +80,7 @@ private: }; -class CORE_EXPORT RightPaneWidget : public QWidget -{ +class CORE_EXPORT RightPaneWidget : public QWidget { Q_OBJECT public: @@ -113,7 +109,6 @@ private: int m_width; static RightPaneWidget *m_instance; }; - } // namespace Core #endif // RIGHTPANE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.cpp b/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.cpp index 3d61f20f0..365c27479 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -49,7 +49,7 @@ rewriting the whole file each time one of the settings change. The SettingsDatabase API mimics that of QSettings. -*/ + */ using namespace Core; using namespace Core::Internal; @@ -58,11 +58,9 @@ enum { debug_settings = 0 }; namespace Core { namespace Internal { - typedef QMap SettingsMap; -class SettingsDatabasePrivate -{ +class SettingsDatabasePrivate { public: QString effectiveGroup() const { @@ -72,8 +70,10 @@ public: QString effectiveKey(const QString &key) const { QString g = effectiveGroup(); - if (!g.isEmpty() && !key.isEmpty()) + + if (!g.isEmpty() && !key.isEmpty()) { g += QLatin1Char('/'); + } g += key; return g; } @@ -85,7 +85,6 @@ public: QSqlDatabase m_db; }; - } // namespace Internal } // namespace Core @@ -99,16 +98,19 @@ SettingsDatabase::SettingsDatabase(const QString &path, // TODO: Don't rely on a path, but determine automatically QDir pathDir(path); - if (!pathDir.exists()) + + if (!pathDir.exists()) { pathDir.mkpath(pathDir.absolutePath()); + } QString fileName = path; - if (!fileName.endsWith(slash)) + if (!fileName.endsWith(slash)) { fileName += slash; + } fileName += application; fileName += QLatin1String(".db"); - d->m_db = QSqlDatabase::addDatabase("QSQLITE", QLatin1String("settings")); + d->m_db = QSqlDatabase::addDatabase("QSQLITE", QLatin1String("settings")); d->m_db.setDatabaseName(fileName); if (!d->m_db.open()) { qWarning().nospace() << "Warning: Failed to open settings database at " << fileName << " (" @@ -119,9 +121,10 @@ SettingsDatabase::SettingsDatabase(const QString &path, query.prepare(QLatin1String("CREATE TABLE IF NOT EXISTS settings (" "key PRIMARY KEY ON CONFLICT REPLACE, " "value)")); - if (!query.exec()) + if (!query.exec()) { qWarning().nospace() << "Warning: Failed to prepare settings database! (" << query.lastError().driverText() << ")"; + } // Retrieve all available keys (values are retrieved lazily) if (query.exec(QLatin1String("SELECT key FROM settings"))) { @@ -147,8 +150,9 @@ void SettingsDatabase::setValue(const QString &key, const QVariant &value) // Add to cache d->m_settings.insert(effectiveKey, value); - if (!d->m_db.isOpen()) + if (!d->m_db.isOpen()) { return; + } // Instant apply (TODO: Delay writing out settings) QSqlQuery query(d->m_db); @@ -157,16 +161,18 @@ void SettingsDatabase::setValue(const QString &key, const QVariant &value) query.addBindValue(value); query.exec(); - if (debug_settings) + if (debug_settings) { qDebug() << "Stored:" << effectiveKey << "=" << value; + } } QVariant SettingsDatabase::value(const QString &key, const QVariant &defaultValue) const { - const QString effectiveKey = d->effectiveKey(key); + const QString effectiveKey = d->effectiveKey(key); QVariant value = defaultValue; SettingsMap::const_iterator i = d->m_settings.constFind(effectiveKey); + if (i != d->m_settings.constEnd() && i.value().isValid()) { value = i.value(); } else if (d->m_db.isOpen()) { @@ -178,8 +184,9 @@ QVariant SettingsDatabase::value(const QString &key, const QVariant &defaultValu if (query.next()) { value = query.value(0); - if (debug_settings) + if (debug_settings) { qDebug() << "Retrieved:" << effectiveKey << "=" << value; + } } // Cache the result @@ -199,18 +206,18 @@ void SettingsDatabase::remove(const QString &key) const QString effectiveKey = d->effectiveKey(key); // Remove keys from the cache - foreach (const QString &k, d->m_settings.keys()) { + foreach(const QString &k, d->m_settings.keys()) { // Either it's an exact match, or it matches up to a / if (k.startsWith(effectiveKey) && (k.length() == effectiveKey.length() - || k.at(effectiveKey.length()) == QLatin1Char('/'))) - { + || k.at(effectiveKey.length()) == QLatin1Char('/'))) { d->m_settings.remove(k); } } - if (!d->m_db.isOpen()) + if (!d->m_db.isOpen()) { return; + } // Delete keys from the database QSqlQuery query(d->m_db); @@ -240,6 +247,7 @@ QStringList SettingsDatabase::childKeys() const QStringList childs; const QString g = group(); + QMapIterator i(d->m_settings); while (i.hasNext()) { const QString &key = i.next().key(); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.h b/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.h index 1ed78dc59..0cbe96946 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/settingsdatabase.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,13 +37,11 @@ #include namespace Core { - namespace Internal { class SettingsDatabasePrivate; } -class CORE_EXPORT SettingsDatabase : public QObject -{ +class CORE_EXPORT SettingsDatabase : public QObject { public: SettingsDatabase(const QString &path, const QString &application, QObject *parent = 0); ~SettingsDatabase(); @@ -63,7 +61,6 @@ public: private: Internal::SettingsDatabasePrivate *d; }; - } // namespace Core #endif // SETTINGSDATABASE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/sidebar.cpp b/ground/openpilotgcs/src/plugins/coreplugin/sidebar.cpp index 15068f855..dcfc10ab5 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/sidebar.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/sidebar.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -48,17 +48,19 @@ SideBarItem::~SideBarItem() delete m_widget; } -SideBar::SideBar(QList itemList, - QList defaultVisible) +SideBar::SideBar(QList itemList, + QList defaultVisible) { setOrientation(Qt::Vertical); - foreach (SideBarItem *item, itemList) { + foreach(SideBarItem * item, itemList) { const QString title = item->widget()->windowTitle(); + m_itemMap.insert(title, item); } - foreach (SideBarItem *item, defaultVisible) { - if (!itemList.contains(item)) + foreach(SideBarItem * item, defaultVisible) { + if (!itemList.contains(item)) { continue; + } m_defaultVisible.append(item->widget()->windowTitle()); } @@ -77,7 +79,7 @@ QStringList SideBar::availableItems() const void SideBar::makeItemAvailable(SideBarItem *item) { - QMap::const_iterator it = m_itemMap.constBegin(); + QMap::const_iterator it = m_itemMap.constBegin(); while (it != m_itemMap.constEnd()) { if (it.value() == item) { m_availableItems.append(it.key()); @@ -100,6 +102,7 @@ SideBarItem *SideBar::item(const QString &title) SideBarWidget *SideBar::insertSideBarWidget(int position, const QString &title) { SideBarWidget *item = new SideBarWidget(this, title); + connect(item, SIGNAL(splitMe()), this, SLOT(splitSubWidget())); connect(item, SIGNAL(closeMe()), this, SLOT(closeSubWidget())); connect(item, SIGNAL(currentWidgetChanged()), this, SLOT(updateWidgets())); @@ -119,8 +122,9 @@ void SideBar::removeSideBarWidget(SideBarWidget *widget) void SideBar::splitSubWidget() { - SideBarWidget *original = qobject_cast(sender()); + SideBarWidget *original = qobject_cast(sender()); int pos = indexOf(original) + 1; + insertSideBarWidget(pos); updateWidgets(); } @@ -128,9 +132,10 @@ void SideBar::splitSubWidget() void SideBar::closeSubWidget() { if (m_widgets.count() != 1) { - SideBarWidget *widget = qobject_cast(sender()); - if (!widget) + SideBarWidget *widget = qobject_cast(sender()); + if (!widget) { return; + } removeSideBarWidget(widget); updateWidgets(); } @@ -138,44 +143,48 @@ void SideBar::closeSubWidget() void SideBar::updateWidgets() { - foreach (SideBarWidget *i, m_widgets) - i->updateAvailableItems(); + foreach(SideBarWidget * i, m_widgets) + i->updateAvailableItems(); } void SideBar::saveSettings(QSettings *settings) { QStringList views; - for (int i = 0; i < m_widgets.count(); ++i) + + for (int i = 0; i < m_widgets.count(); ++i) { views.append(m_widgets.at(i)->currentItemTitle()); + } settings->setValue("HelpSideBar/Views", views); - settings->setValue("HelpSideBar/Visible", true);//isVisible()); + settings->setValue("HelpSideBar/Visible", true); // isVisible()); settings->setValue("HelpSideBar/VerticalPosition", saveState()); settings->setValue("HelpSideBar/Width", width()); } void SideBar::readSettings(QSettings *settings) { - foreach (SideBarWidget *widget, m_widgets) - removeSideBarWidget(widget); + foreach(SideBarWidget * widget, m_widgets) + removeSideBarWidget(widget); if (settings->contains("HelpSideBar/Views")) { QStringList views = settings->value("HelpSideBar/Views").toStringList(); if (views.count()) { - foreach (const QString &title, views) - insertSideBarWidget(m_widgets.count(), title); + foreach(const QString &title, views) + insertSideBarWidget(m_widgets.count(), title); } else { insertSideBarWidget(0); } } else { - foreach (const QString &title, m_defaultVisible) - insertSideBarWidget(m_widgets.count(), title); + foreach(const QString &title, m_defaultVisible) + insertSideBarWidget(m_widgets.count(), title); } - if (settings->contains("HelpSideBar/Visible")) + if (settings->contains("HelpSideBar/Visible")) { setVisible(settings->value("HelpSideBar/Visible").toBool()); + } - if (settings->contains("HelpSideBar/VerticalPosition")) + if (settings->contains("HelpSideBar/VerticalPosition")) { restoreState(settings->value("HelpSideBar/VerticalPosition").toByteArray()); + } if (settings->contains("HelpSideBar/Width")) { QSize s = size(); @@ -186,7 +195,7 @@ void SideBar::readSettings(QSettings *settings) void SideBar::activateItem(SideBarItem *item) { - QMap::const_iterator it = m_itemMap.constBegin(); + QMap::const_iterator it = m_itemMap.constBegin(); QString title; while (it != m_itemMap.constEnd()) { if (it.value() == item) { @@ -196,8 +205,9 @@ void SideBar::activateItem(SideBarItem *item) ++it; } - if (title.isEmpty()) + if (title.isEmpty()) { return; + } for (int i = 0; i < m_widgets.count(); ++i) { if (m_widgets.at(i)->currentItemTitle() == title) { @@ -212,18 +222,17 @@ void SideBar::activateItem(SideBarItem *item) item->widget()->setFocus(); } -void SideBar::setShortcutMap(const QMap &shortcutMap) +void SideBar::setShortcutMap(const QMap &shortcutMap) { m_shortcutMap = shortcutMap; } -QMap SideBar::shortcutMap() const +QMap SideBar::shortcutMap() const { return m_shortcutMap; } - SideBarWidget::SideBarWidget(SideBar *sideBar, const QString &title) : m_currentItem(0) , m_sideBar(sideBar) @@ -231,7 +240,7 @@ SideBarWidget::SideBarWidget(SideBar *sideBar, const QString &title) m_comboBox = new ComboBox(this); m_comboBox->setMinimumContentsLength(15); - m_toolbar = new QToolBar(this); + m_toolbar = new QToolBar(this); m_toolbar->setContentsMargins(0, 0, 0, 0); m_toolbar->addWidget(m_comboBox); @@ -263,8 +272,9 @@ SideBarWidget::SideBarWidget(SideBar *sideBar, const QString &title) if (lst.count()) { m_comboBox->addItems(lst); m_comboBox->setCurrentIndex(0); - if (t.isEmpty()) + if (t.isEmpty()) { t = m_comboBox->currentText(); + } } setCurrentItem(t); @@ -273,8 +283,7 @@ SideBarWidget::SideBarWidget(SideBar *sideBar, const QString &title) } SideBarWidget::~SideBarWidget() -{ -} +{} QString SideBarWidget::currentItemTitle() const { @@ -285,38 +294,43 @@ void SideBarWidget::setCurrentItem(const QString &title) { if (!title.isEmpty()) { int idx = m_comboBox->findText(title); - if (idx < 0) + if (idx < 0) { idx = 0; + } bool blocked = m_comboBox->blockSignals(true); m_comboBox->setCurrentIndex(idx); m_comboBox->blockSignals(blocked); } SideBarItem *item = m_sideBar->item(title); - if (!item) + if (!item) { return; + } removeCurrentItem(); m_currentItem = item; layout()->addWidget(m_currentItem->widget()); // Add buttons and remember their actions for later removal - foreach (QToolButton *b, m_currentItem->createToolBarWidgets()) - m_addedToolBarActions.append(m_toolbar->insertWidget(m_splitAction, b)); + foreach(QToolButton * b, m_currentItem->createToolBarWidgets()) + m_addedToolBarActions.append(m_toolbar->insertWidget(m_splitAction, b)); } void SideBarWidget::updateAvailableItems() { - bool blocked = m_comboBox->blockSignals(true); - QString current = m_comboBox->currentText(); + bool blocked = m_comboBox->blockSignals(true); + QString current = m_comboBox->currentText(); + m_comboBox->clear(); QStringList itms = m_sideBar->availableItems(); - if (!current.isEmpty() && !itms.contains(current)) + if (!current.isEmpty() && !itms.contains(current)) { itms.append(current); + } qSort(itms); m_comboBox->addItems(itms); int idx = m_comboBox->findText(current); - if (idx < 0) + if (idx < 0) { idx = 0; + } m_comboBox->setCurrentIndex(idx); m_splitButton->setEnabled(itms.count() > 1); m_comboBox->blockSignals(blocked); @@ -324,8 +338,9 @@ void SideBarWidget::updateAvailableItems() void SideBarWidget::removeCurrentItem() { - if (!m_currentItem) + if (!m_currentItem) { return; + } QWidget *w = m_currentItem->widget(); layout()->removeWidget(w); @@ -347,19 +362,19 @@ void SideBarWidget::setCurrentIndex(int) Core::Command *SideBarWidget::command(const QString &title) const { - const QMap shortcutMap = m_sideBar->shortcutMap(); - QMap::const_iterator r = shortcutMap.find(title); - if (r != shortcutMap.end()) + const QMap shortcutMap = m_sideBar->shortcutMap(); + + QMap::const_iterator r = shortcutMap.find(title); + if (r != shortcutMap.end()) { return r.value(); + } return 0; } - ComboBox::ComboBox(SideBarWidget *sideBarWidget) : m_sideBarWidget(sideBarWidget) -{ -} +{} bool ComboBox::event(QEvent *e) { diff --git a/ground/openpilotgcs/src/plugins/coreplugin/sidebar.h b/ground/openpilotgcs/src/plugins/coreplugin/sidebar.h index 4527ab817..705ed3028 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/sidebar.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/sidebar.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,7 +44,6 @@ class QToolButton; QT_END_NAMESPACE namespace Core { - class Command; namespace Internal { @@ -59,8 +58,7 @@ class ComboBox; * * The SideBarItem takes ownership over the widget. */ -class CORE_EXPORT SideBarItem -{ +class CORE_EXPORT SideBarItem { public: SideBarItem(QWidget *widget) : m_widget(widget) @@ -68,7 +66,10 @@ public: virtual ~SideBarItem(); - QWidget *widget() { return m_widget; } + QWidget *widget() + { + return m_widget; + } /* Should always return a new set of tool buttons. * @@ -85,15 +86,14 @@ private: QWidget *m_widget; }; -class CORE_EXPORT SideBar : public MiniSplitter -{ +class CORE_EXPORT SideBar : public MiniSplitter { Q_OBJECT public: /* * The SideBar takes ownership of the SideBarItems. */ - SideBar(QList widgetList, - QList defaultVisible); + SideBar(QList widgetList, + QList defaultVisible); ~SideBar(); QStringList availableItems() const; @@ -105,8 +105,8 @@ public: void activateItem(SideBarItem *item); - void setShortcutMap(const QMap &shortcutMap); - QMap shortcutMap() const; + void setShortcutMap(const QMap &shortcutMap); + QMap shortcutMap() const; private slots: void splitSubWidget(); @@ -118,18 +118,16 @@ private: const QString &title = QString()); void removeSideBarWidget(Internal::SideBarWidget *widget); - QList m_widgets; + QList m_widgets; - QMap m_itemMap; + QMap m_itemMap; QStringList m_availableItems; QStringList m_defaultVisible; - QMap m_shortcutMap; + QMap m_shortcutMap; }; namespace Internal { - -class SideBarWidget : public QWidget -{ +class SideBarWidget : public QWidget { Q_OBJECT public: SideBarWidget(SideBar *sideBar, const QString &title); @@ -162,8 +160,7 @@ private: QToolButton *m_closeButton; }; -class ComboBox : public QComboBox -{ +class ComboBox : public QComboBox { Q_OBJECT public: @@ -175,7 +172,6 @@ protected: private: SideBarWidget *m_sideBarWidget; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.cpp b/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.cpp index 7d8c27afb..9a7b9c914 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,13 +30,15 @@ #include -Animation * StyleAnimator::widgetAnimation(const QWidget *widget) const +Animation *StyleAnimator::widgetAnimation(const QWidget *widget) const { - if (!widget) + if (!widget) { return 0; - foreach (Animation *a, animations) { - if (a->widget() == widget) + } + foreach(Animation * a, animations) { + if (a->widget() == widget) { return a; + } } return 0; } @@ -49,40 +51,42 @@ void Animation::paint(QPainter *painter, const QStyleOption *option) void Animation::drawBlendedImage(QPainter *painter, QRect rect, float alpha) { - if (m_secondaryImage.isNull() || m_primaryImage.isNull()) + if (m_secondaryImage.isNull() || m_primaryImage.isNull()) { return; + } - if (m_tempImage.isNull()) + if (m_tempImage.isNull()) { m_tempImage = m_secondaryImage; + } - const int a = qRound(alpha*256); - const int ia = 256 - a; - const int sw = m_primaryImage.width(); - const int sh = m_primaryImage.height(); + const int a = qRound(alpha * 256); + const int ia = 256 - a; + const int sw = m_primaryImage.width(); + const int sh = m_primaryImage.height(); const int bpl = m_primaryImage.bytesPerLine(); switch (m_primaryImage.depth()) { case 32: - { - uchar *mixed_data = m_tempImage.bits(); - const uchar *back_data = m_primaryImage.bits(); - const uchar *front_data = m_secondaryImage.bits(); - for (int sy = 0; sy < sh; sy++) { - quint32 *mixed = (quint32*)mixed_data; - const quint32* back = (const quint32*)back_data; - const quint32* front = (const quint32*)front_data; - for (int sx = 0; sx < sw; sx++) { - quint32 bp = back[sx]; - quint32 fp = front[sx]; - mixed[sx] = qRgba ((qRed(bp)*ia + qRed(fp)*a)>>8, - (qGreen(bp)*ia + qGreen(fp)*a)>>8, - (qBlue(bp)*ia + qBlue(fp)*a)>>8, - (qAlpha(bp)*ia + qAlpha(fp)*a)>>8); - } - mixed_data += bpl; - back_data += bpl; - front_data += bpl; + { + uchar *mixed_data = m_tempImage.bits(); + const uchar *back_data = m_primaryImage.bits(); + const uchar *front_data = m_secondaryImage.bits(); + for (int sy = 0; sy < sh; sy++) { + quint32 *mixed = (quint32 *)mixed_data; + const quint32 *back = (const quint32 *)back_data; + const quint32 *front = (const quint32 *)front_data; + for (int sx = 0; sx < sw; sx++) { + quint32 bp = back[sx]; + quint32 fp = front[sx]; + mixed[sx] = qRgba((qRed(bp) * ia + qRed(fp) * a) >> 8, + (qGreen(bp) * ia + qGreen(fp) * a) >> 8, + (qBlue(bp) * ia + qBlue(fp) * a) >> 8, + (qAlpha(bp) * ia + qAlpha(fp) * a) >> 8); } + mixed_data += bpl; + back_data += bpl; + front_data += bpl; } + } default: break; } @@ -92,20 +96,21 @@ void Animation::drawBlendedImage(QPainter *painter, QRect rect, float alpha) void Transition::paint(QPainter *painter, const QStyleOption *option) { float alpha = 1.0; + if (m_duration > 0) { QTime current = QTime::currentTime(); - if (m_startTime > current) + if (m_startTime > current) { m_startTime = current; + } int timeDiff = m_startTime.msecsTo(current); - alpha = timeDiff/(float)m_duration; + alpha = timeDiff / (float)m_duration; if (timeDiff > m_duration) { m_running = false; - alpha = 1.0; + alpha = 1.0; } - } - else { + } else { m_running = false; } drawBlendedImage(painter, option->rect, alpha); @@ -113,16 +118,16 @@ void Transition::paint(QPainter *painter, const QStyleOption *option) void StyleAnimator::timerEvent(QTimerEvent *) { - for (int i = animations.size() - 1 ; i >= 0 ; --i) { - if (animations[i]->widget()) + for (int i = animations.size() - 1; i >= 0; --i) { + if (animations[i]->widget()) { animations[i]->widget()->update(); + } if (!animations[i]->widget() || !animations[i]->widget()->isEnabled() || !animations[i]->widget()->isVisible() || animations[i]->widget()->window()->isMinimized() || - !animations[i]->running()) - { + !animations[i]->running()) { Animation *a = animations.takeAt(i); delete a; } @@ -134,7 +139,7 @@ void StyleAnimator::timerEvent(QTimerEvent *) void StyleAnimator::stopAnimation(const QWidget *w) { - for (int i = animations.size() - 1 ; i >= 0 ; --i) { + for (int i = animations.size() - 1; i >= 0; --i) { if (animations[i]->widget() == w) { Animation *a = animations.takeAt(i); delete a; diff --git a/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.h b/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.h index cc4201822..d5f1027d2 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/styleanimator.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,24 +36,41 @@ #include #include -/* +/* * This is a set of helper classes to allow for widget animations in * the style. Its mostly taken from Vista style so it should be fully documented * there. * */ - -class Animation -{ -public : - Animation() : m_running(true) { } - virtual ~Animation() { } - QWidget * widget() const { return m_widget; } - bool running() const { return m_running; } - const QTime &startTime() const { return m_startTime; } - void setRunning(bool val) { m_running = val; } - void setWidget(QWidget *widget) { m_widget = widget; } - void setStartTime(const QTime &startTime) { m_startTime = startTime; } + +class Animation { +public: + Animation() : m_running(true) {} + virtual ~Animation() {} + QWidget *widget() const + { + return m_widget; + } + bool running() const + { + return m_running; + } + const QTime &startTime() const + { + return m_startTime; + } + void setRunning(bool val) + { + m_running = val; + } + void setWidget(QWidget *widget) + { + m_widget = widget; + } + void setStartTime(const QTime &startTime) + { + m_startTime = startTime; + } virtual void paint(QPainter *painter, const QStyleOption *option); protected: @@ -67,21 +84,31 @@ protected: }; // Handles state transition animations -class Transition : public Animation -{ -public : +class Transition : public Animation { +public: Transition() : Animation() {} virtual ~Transition() {} - void setDuration(int duration) { m_duration = duration; } - void setStartImage(const QImage &image) { m_primaryImage = image; } - void setEndImage(const QImage &image) { m_secondaryImage = image; } + void setDuration(int duration) + { + m_duration = duration; + } + void setStartImage(const QImage &image) + { + m_primaryImage = image; + } + void setEndImage(const QImage &image) + { + m_secondaryImage = image; + } virtual void paint(QPainter *painter, const QStyleOption *option); - int duration() const { return m_duration; } - int m_duration; //set time in ms to complete a state transition + int duration() const + { + return m_duration; + } + int m_duration; // set time in ms to complete a state transition }; -class StyleAnimator : public QObject -{ +class StyleAnimator : public QObject { Q_OBJECT public: @@ -90,11 +117,11 @@ public: void timerEvent(QTimerEvent *); void startAnimation(Animation *); void stopAnimation(const QWidget *); - Animation* widgetAnimation(const QWidget *) const; - + Animation *widgetAnimation(const QWidget *) const; + private: QBasicTimer animationTimer; - QList animations; + QList animations; }; #endif // ANIMATION_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.cpp b/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.cpp index 0c96d200f..f51266071 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,13 +37,13 @@ using namespace Core::Internal; TabPositionIndicator::TabPositionIndicator() : QWidget(0, Qt::ToolTip) -{ -} +{} void TabPositionIndicator::paintEvent(QPaintEvent *event) { QPainter p(this); QPen pen = p.pen(); + pen.setWidth(2); pen.setColor(palette().color(QPalette::Active, QPalette::LinkVisited)); pen.setStyle(Qt::DotLine); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.h b/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.h index a4223adae..7de3823a2 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/tabpositionindicator.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,21 +33,21 @@ namespace Core { namespace Internal { - -class TabPositionIndicator : public QWidget -{ +class TabPositionIndicator : public QWidget { Q_OBJECT public: enum { TABPOSITIONINDICATOR_WIDTH = 2 }; TabPositionIndicator(); - int indicatorWidth() { return TABPOSITIONINDICATOR_WIDTH; } + int indicatorWidth() + { + return TABPOSITIONINDICATOR_WIDTH; + } private: void paintEvent(QPaintEvent *event); }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.cpp b/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.cpp index d7f6b052c..268c7f1d4 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.cpp @@ -7,8 +7,8 @@ TelemetryMonitorWidget::TelemetryMonitorWidget(QWidget *parent) : QGraphicsView(parent) { - setMinimumSize(180,100); - setMaximumSize(180,100); + setMinimumSize(180, 100); + setMaximumSize(180, 100); setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); @@ -18,7 +18,7 @@ TelemetryMonitorWidget::TelemetryMonitorWidget(QWidget *parent) : QGraphicsView( setAttribute(Qt::WA_TranslucentBackground); setWindowFlags(Qt::FramelessWindowHint); - QGraphicsScene *scene = new QGraphicsScene(0,0,180,100, this); + QGraphicsScene *scene = new QGraphicsScene(0, 0, 180, 100, this); QSvgRenderer *renderer = new QSvgRenderer(); if (renderer->load(QString(":/core/images/tx-rx.svg"))) { @@ -27,9 +27,9 @@ TelemetryMonitorWidget::TelemetryMonitorWidget(QWidget *parent) : QGraphicsView( graph->setElementId("txrxBackground"); QString name; - QGraphicsSvgItem* pt; + QGraphicsSvgItem *pt; - for (int i=0; ielementExists(name)) { pt = new QGraphicsSvgItem(); @@ -53,13 +53,13 @@ TelemetryMonitorWidget::TelemetryMonitorWidget(QWidget *parent) : QGraphicsView( txSpeed = new QGraphicsTextItem(); txSpeed->setDefaultTextColor(Qt::white); - txSpeed->setFont(QFont("Helvetica",22,2)); + txSpeed->setFont(QFont("Helvetica", 22, 2)); txSpeed->setParentItem(graph); scene->addItem(txSpeed); rxSpeed = new QGraphicsTextItem(); rxSpeed->setDefaultTextColor(Qt::white); - rxSpeed->setFont(QFont("Helvetica",22,2)); + rxSpeed->setFont(QFont("Helvetica", 22, 2)); rxSpeed->setParentItem(graph); scene->addItem(rxSpeed); @@ -68,8 +68,8 @@ TelemetryMonitorWidget::TelemetryMonitorWidget(QWidget *parent) : QGraphicsView( } connected = false; - txValue = 0.0; - rxValue = 0.0; + txValue = 0.0; + rxValue = 0.0; setMin(0.0); setMax(1200.0); @@ -79,34 +79,36 @@ TelemetryMonitorWidget::TelemetryMonitorWidget(QWidget *parent) : QGraphicsView( TelemetryMonitorWidget::~TelemetryMonitorWidget() { - while (!txNodes.isEmpty()) + while (!txNodes.isEmpty()) { delete txNodes.takeFirst(); + } - while (!rxNodes.isEmpty()) + while (!rxNodes.isEmpty()) { delete rxNodes.takeFirst(); + } } void TelemetryMonitorWidget::connect() { connected = true; - //flash the lights + // flash the lights updateTelemetry(maxValue, maxValue); } void TelemetryMonitorWidget::disconnect() { - //flash the lights + // flash the lights updateTelemetry(maxValue, maxValue); connected = false; - updateTelemetry(0.0,0.0); + updateTelemetry(0.0, 0.0); } /*! - \brief Called by the UAVObject which got updated + \brief Called by the UAVObject which got updated - Updates the numeric value and/or the icon if the dial wants this. - */ + Updates the numeric value and/or the icon if the dial wants this. + */ void TelemetryMonitorWidget::updateTelemetry(double txRate, double rxRate) { txValue = txRate; @@ -119,29 +121,30 @@ void TelemetryMonitorWidget::updateTelemetry(double txRate, double rxRate) // this enables smooth movement in moveIndex below void TelemetryMonitorWidget::showTelemetry() { - txIndex = (txValue-minValue)/(maxValue-minValue) * NODE_NUMELEM; - rxIndex = (rxValue-minValue)/(maxValue-minValue) * NODE_NUMELEM; + txIndex = (txValue - minValue) / (maxValue - minValue) * NODE_NUMELEM; + rxIndex = (rxValue - minValue) / (maxValue - minValue) * NODE_NUMELEM; - if (connected) + if (connected) { this->setToolTip(QString("Tx: %0 bytes/sec\nRx: %1 bytes/sec").arg(txValue).arg(rxValue)); - else + } else { this->setToolTip(QString("Disconnected")); + } int i; int nodeMargin = 8; int leftMargin = 60; - QGraphicsItem* node; + QGraphicsItem *node; - for (i=0; i < txNodes.count(); i++) { + for (i = 0; i < txNodes.count(); i++) { node = txNodes.at(i); - node->setPos((i*(node->boundingRect().width() + nodeMargin)) + leftMargin, (node->boundingRect().height()/2) - 2); + node->setPos((i * (node->boundingRect().width() + nodeMargin)) + leftMargin, (node->boundingRect().height() / 2) - 2); node->setVisible(connected && i < txIndex); node->update(); } - for (i=0; i < rxNodes.count(); i++) { + for (i = 0; i < rxNodes.count(); i++) { node = rxNodes.at(i); - node->setPos((i*(node->boundingRect().width() + nodeMargin)) + leftMargin, (node->boundingRect().height()*2) - 2); + node->setPos((i * (node->boundingRect().width() + nodeMargin)) + leftMargin, (node->boundingRect().height() * 2) - 2); node->setVisible(connected && i < rxIndex); node->update(); } @@ -165,11 +168,10 @@ void TelemetryMonitorWidget::showEvent(QShowEvent *event) fitInView(graph, Qt::KeepAspectRatio); } -void TelemetryMonitorWidget::resizeEvent(QResizeEvent* event) +void TelemetryMonitorWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - graph->setPos(0,-130); + graph->setPos(0, -130); fitInView(graph, Qt::KeepAspectRatio); } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.h b/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.h index 803362230..b157d734d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/telemetrymonitorwidget.h @@ -8,23 +8,34 @@ #include #include -class TelemetryMonitorWidget : public QGraphicsView -{ +class TelemetryMonitorWidget : public QGraphicsView { Q_OBJECT public: explicit TelemetryMonitorWidget(QWidget *parent = 0); ~TelemetryMonitorWidget(); - void setMin(double min) { minValue = min;} - double getMin() { return minValue; } - void setMax(double max) { maxValue = max;} - double getMax() { return maxValue; } + void setMin(double min) + { + minValue = min; + } + double getMin() + { + return minValue; + } + void setMax(double max) + { + maxValue = max; + } + double getMax() + { + return maxValue; + } - //number of tx/rx nodes in the graph + // number of tx/rx nodes in the graph static const int NODE_NUMELEM = 7; signals: - + public slots: void connect(); void disconnect(); @@ -37,19 +48,19 @@ protected: void resizeEvent(QResizeEvent *event); private: - QGraphicsSvgItem *graph; - QPointer txSpeed; - QPointer rxSpeed; - QList txNodes; - QList rxNodes; + QGraphicsSvgItem *graph; + QPointer txSpeed; + QPointer rxSpeed; + QList txNodes; + QList rxNodes; - bool connected; - double txIndex; - double txValue; - double rxIndex; - double rxValue; - double minValue; - double maxValue; + bool connected; + double txIndex; + double txValue; + double rxIndex; + double rxValue; + double minValue; + double maxValue; }; #endif // TELEMETRYMONITORWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.cpp index f4f747d90..629675f56 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,8 +35,8 @@ ThreadManager *ThreadManager::m_instance = 0; ThreadManager::ThreadManager(QObject *parent) : QObject(parent) { - m_instance = this; - realTimeThread= new QThread(this); + m_instance = this; + realTimeThread = new QThread(this); realTimeThread->start(QThread::TimeCriticalPriority); } @@ -49,5 +49,5 @@ ThreadManager::~ThreadManager() QThread *ThreadManager::getRealTimeThread() { - return realTimeThread; + return realTimeThread; } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.h index a2164f281..0cda72e8c 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/threadmanager.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,19 +35,20 @@ #include QT_BEGIN_NAMESPACE -QT_END_NAMESPACE + QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT ThreadManager : public QObject -{ +class CORE_EXPORT ThreadManager : public QObject { Q_OBJECT public: ThreadManager(QObject *parent); ~ThreadManager(); - static ThreadManager* instance() { return m_instance; } + static ThreadManager *instance() + { + return m_instance; + } QThread *getRealTimeThread(); @@ -56,7 +57,6 @@ private: QThread *realTimeThread; static ThreadManager *m_instance; }; - } // namespace Core #endif // THREADMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.cpp index 30b0877f8..be05426f3 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.cpp @@ -102,38 +102,40 @@ Returns true when the import should be done, false otherwise. -*/ + */ #include "uavconfiginfo.h" #include -#define VERSION_DEFAULT "0.0.0" +#define VERSION_DEFAULT "0.0.0" -#define TEXT_MINOR_LOSS_OF_CONFIGURATION tr( \ -" Some of the configured features might not be supported \ +#define TEXT_MINOR_LOSS_OF_CONFIGURATION \ + tr( \ + " Some of the configured features might not be supported \ by your version of the plugin. You might want to upgrade the plugin.") -#define TEXT_MISSING_CONFIGURATION tr( \ -" Some configuration is missing in the imported config and will be replaced \ +#define TEXT_MISSING_CONFIGURATION \ + tr( \ + " Some configuration is missing in the imported config and will be replaced \ by default settings.") -#define TEXT_MAJOR_LOSS_OF_CONFIGURATION tr( \ -" Major features can't be imported \ +#define TEXT_MAJOR_LOSS_OF_CONFIGURATION \ + tr( \ + " Major features can't be imported \ by your version of the plugin. You should upgrade the plugin to import these settings.") -#define TEXT_NOT_COMPATIBLE tr( \ -" The imported settings are not compatible with this plugin and won't be imported!") +#define TEXT_NOT_COMPATIBLE \ + tr( \ + " The imported settings are not compatible with this plugin and won't be imported!") using namespace Core; UAVConfigInfo::UAVConfigInfo(QObject *parent) : - QObject(parent), - m_version(VERSION_DEFAULT), - m_locked(false), - m_nameOfConfigurable("") -{ - -} + QObject(parent), + m_version(VERSION_DEFAULT), + m_locked(false), + m_nameOfConfigurable("") +{} UAVConfigInfo::UAVConfigInfo(QSettings *qs, QObject *parent) : QObject(parent), @@ -143,16 +145,14 @@ UAVConfigInfo::UAVConfigInfo(QSettings *qs, QObject *parent) : } UAVConfigInfo::UAVConfigInfo(UAVConfigVersion version, QString nameOfConfigurable, QObject *parent) : - QObject(parent), - m_version(version), - m_locked(false), - m_nameOfConfigurable(nameOfConfigurable) -{ - -} + QObject(parent), + m_version(version), + m_locked(false), + m_nameOfConfigurable(nameOfConfigurable) +{} UAVConfigInfo::UAVConfigInfo(IUAVGadgetConfiguration *config, QObject *parent) : - QObject(parent) + QObject(parent) { m_locked = config->locked(); m_nameOfConfigurable = config->classId() + "-" + config->name(); @@ -169,21 +169,21 @@ void UAVConfigInfo::save(QSettings *qs) void UAVConfigInfo::read(QSettings *qs) { qs->beginGroup("configInfo"); - m_version = UAVConfigVersion( qs->value("version", VERSION_DEFAULT ).toString()); - m_locked = qs->value("locked", false ).toBool(); + m_version = UAVConfigVersion(qs->value("version", VERSION_DEFAULT).toString()); + m_locked = qs->value("locked", false).toBool(); qs->endGroup(); } bool UAVConfigInfo::askToAbort(int compat, QString message) { QMessageBox msgBox; + msgBox.setInformativeText(tr("Do you want to continue the import?")); msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); int result; - switch(compat){ - + switch (compat) { case FullyCompatible: return false; @@ -215,30 +215,35 @@ bool UAVConfigInfo::askToAbort(int compat, QString message) default: msgBox.setText("INTERNAL ERROR: " + message + tr("Unknown compatibility level: " + compat)); } - if ( result == QMessageBox::Ok ) + if (result == QMessageBox::Ok) { return false; - else + } else { return true; - + } } void UAVConfigInfo::notify(QString message) { QMessageBox msgBox; + msgBox.setText(message); msgBox.exec(); } int UAVConfigInfo::checkCompatibilityWith(UAVConfigVersion programVersion) { - if ( m_version.majorNr != programVersion.majorNr ) + if (m_version.majorNr != programVersion.majorNr) { return NotCompatible; - if ( m_version.minorNr < programVersion.minorNr ) + } + if (m_version.minorNr < programVersion.minorNr) { return MissingConfiguration; - if ( m_version.minorNr > programVersion.minorNr ) + } + if (m_version.minorNr > programVersion.minorNr) { return MajorLossOfConfiguration; - if ( m_version.patchNr > programVersion.patchNr ) + } + if (m_version.patchNr > programVersion.patchNr) { return MinorLossOfConfiguration; + } return FullyCompatible; } @@ -246,31 +251,30 @@ int UAVConfigInfo::checkCompatibilityWith(UAVConfigVersion programVersion) bool UAVConfigInfo::standardVersionHandlingOK(UAVConfigVersion programVersion) { return !askToAbort( - checkCompatibilityWith(programVersion), - "("+m_nameOfConfigurable+")"); + checkCompatibilityWith(programVersion), + "(" + m_nameOfConfigurable + ")"); } UAVConfigVersion::UAVConfigVersion(int majorNum, int minorNum, int patchNum) - :majorNr(majorNum) - ,minorNr(minorNum) - ,patchNr(patchNum) -{ -} + : majorNr(majorNum) + , minorNr(minorNum) + , patchNr(patchNum) +{} UAVConfigVersion::UAVConfigVersion(QString versionString) { int begin; int end = 0; - begin = end; - end = versionString.indexOf(".", begin); - majorNr = versionString.mid(begin, end-begin).toInt(); + begin = end; + end = versionString.indexOf(".", begin); + majorNr = versionString.mid(begin, end - begin).toInt(); - begin = end+1; - end = versionString.indexOf(".", begin); - minorNr = versionString.mid(begin, end-begin).toInt(); + begin = end + 1; + end = versionString.indexOf(".", begin); + minorNr = versionString.mid(begin, end - begin).toInt(); - begin = end+1; + begin = end + 1; patchNr = versionString.mid(begin).toInt(); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.h b/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.h index 314d33459..bae3d4e8d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavconfiginfo.h @@ -33,12 +33,10 @@ #include "iuavgadgetconfiguration.h" #include "core_global.h" -namespace Core -{ - +namespace Core { class IUAVGadgetConfiguration; -class CORE_EXPORT UAVConfigVersion{ +class CORE_EXPORT UAVConfigVersion { public: UAVConfigVersion(QString versionString = "0.0.0"); UAVConfigVersion(int major, int minor, int patch); @@ -51,34 +49,57 @@ public: bool operator==(const UAVConfigVersion &other); }; -class CORE_EXPORT UAVConfigInfo : public QObject -{ +class CORE_EXPORT UAVConfigInfo : public QObject { Q_OBJECT public: explicit UAVConfigInfo(QObject *parent = 0); explicit UAVConfigInfo(QSettings *qs, QObject *parent = 0); - explicit UAVConfigInfo(IUAVGadgetConfiguration* config, QObject *parent = 0); + explicit UAVConfigInfo(IUAVGadgetConfiguration *config, QObject *parent = 0); UAVConfigInfo(UAVConfigVersion version, QString nameOfConfigurable, QObject *parent = 0); enum Compatibility { FullyCompatible, MinorLossOfConfiguration, MissingConfiguration, MajorLossOfConfiguration, NotCompatible }; - void setNameOfConfigurable(const QString nameOfConfigurable){m_nameOfConfigurable = nameOfConfigurable;} + void setNameOfConfigurable(const QString nameOfConfigurable) + { + m_nameOfConfigurable = nameOfConfigurable; + } void save(QSettings *qs); void read(QSettings *qs); - void setVersion(int major, int minor, int patch){m_version = UAVConfigVersion(major, minor, patch);} - void setVersion(const QString version){m_version = UAVConfigVersion(version);} - void setVersion(const UAVConfigVersion version){m_version = version;} - UAVConfigVersion version(){ return m_version;} - bool locked(){ return m_locked; } - void setLocked(bool locked){ m_locked = locked; } + void setVersion(int major, int minor, int patch) + { + m_version = UAVConfigVersion(major, minor, patch); + } + void setVersion(const QString version) + { + m_version = UAVConfigVersion(version); + } + void setVersion(const UAVConfigVersion version) + { + m_version = version; + } + UAVConfigVersion version() + { + return m_version; + } + bool locked() + { + return m_locked; + } + void setLocked(bool locked) + { + m_locked = locked; + } int checkCompatibilityWith(UAVConfigVersion programVersion); bool askToAbort(int compat, QString message); void notify(QString message); bool standardVersionHandlingOK(UAVConfigVersion programVersion); - bool standardVersionHandlingOK(QString programVersion){ return standardVersionHandlingOK(UAVConfigVersion(programVersion));} + bool standardVersionHandlingOK(QString programVersion) + { + return standardVersionHandlingOK(UAVConfigVersion(programVersion)); + } signals: @@ -88,9 +109,7 @@ private: UAVConfigVersion m_version; bool m_locked; QString m_nameOfConfigurable; - }; - } // namespace Core #endif // UAVCONFIGINFO_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.cpp index 56c02b00a..2fc7c580d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.cpp @@ -9,19 +9,19 @@ * @{ * @brief The Core GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,17 +33,17 @@ using namespace Core; -UAVGadgetDecorator::UAVGadgetDecorator(IUAVGadget *gadget, QList *configurations) : - IUAVGadget(gadget->classId(), gadget->parent()), - m_gadget(gadget), - m_toolbar(new QComboBox), - m_activeConfiguration(0), - m_configurations(configurations) +UAVGadgetDecorator::UAVGadgetDecorator(IUAVGadget *gadget, QList *configurations) : + IUAVGadget(gadget->classId(), gadget->parent()), + m_gadget(gadget), + m_toolbar(new QComboBox), + m_activeConfiguration(0), + m_configurations(configurations) { m_gadget->setParent(this); m_toolbar->setMinimumContentsLength(15); - foreach (IUAVGadgetConfiguration *config, *m_configurations) - m_toolbar->addItem(config->name()); + foreach(IUAVGadgetConfiguration * config, *m_configurations) + m_toolbar->addItem(config->name()); connect(m_toolbar, SIGNAL(activated(int)), this, SLOT(loadConfiguration(int))); updateToolbar(); } @@ -54,8 +54,10 @@ UAVGadgetDecorator::~UAVGadgetDecorator() delete m_toolbar; } -void UAVGadgetDecorator::loadConfiguration(int index) { +void UAVGadgetDecorator::loadConfiguration(int index) +{ IUAVGadgetConfiguration *config = m_configurations->at(index); + loadConfiguration(config); } @@ -65,19 +67,20 @@ void UAVGadgetDecorator::loadConfiguration(IUAVGadgetConfiguration *config) int index = m_toolbar->findText(config->name()); m_toolbar->setCurrentIndex(index); m_gadget->loadConfiguration(config); - } void UAVGadgetDecorator::configurationChanged(IUAVGadgetConfiguration *config) { - if (config == m_activeConfiguration) + if (config == m_activeConfiguration) { loadConfiguration(config); + } } void UAVGadgetDecorator::configurationAdded(IUAVGadgetConfiguration *config) { - if (config->classId() != classId()) + if (config->classId() != classId()) { return; + } m_configurations->append(config); m_toolbar->addItem(config->name()); updateToolbar(); @@ -85,8 +88,9 @@ void UAVGadgetDecorator::configurationAdded(IUAVGadgetConfiguration *config) void UAVGadgetDecorator::configurationToBeDeleted(IUAVGadgetConfiguration *config) { - if (config->classId() != classId()) + if (config->classId() != classId()) { return; + } int index = m_configurations->indexOf(config); if (index >= 0) { m_toolbar->removeItem(index); @@ -97,11 +101,13 @@ void UAVGadgetDecorator::configurationToBeDeleted(IUAVGadgetConfiguration *confi void UAVGadgetDecorator::configurationNameChanged(IUAVGadgetConfiguration *config, QString oldName, QString newName) { - if (config->classId() != classId()) + if (config->classId() != classId()) { return; + } for (int i = 0; i < m_toolbar->count(); ++i) { - if (m_toolbar->itemText(i) == oldName) + if (m_toolbar->itemText(i) == oldName) { m_toolbar->setItemText(i, newName); + } } } @@ -120,7 +126,8 @@ void UAVGadgetDecorator::saveState(QSettings *qSettings) void UAVGadgetDecorator::restoreState(QSettings *qSettings) { QString configName = qSettings->value("activeConfiguration").toString(); - foreach (IUAVGadgetConfiguration *config, *m_configurations) { + + foreach(IUAVGadgetConfiguration * config, *m_configurations) { if (config->name() == configName) { m_activeConfiguration = config; loadConfiguration(config); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.h b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.h index c7dd59509..0a233eefb 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.h @@ -10,18 +10,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,27 +30,34 @@ #include namespace Core { - class IUAVGadgetConfiguration; -class UAVGadgetDecorator : public IUAVGadget -{ -Q_OBJECT +class UAVGadgetDecorator : public IUAVGadget { + Q_OBJECT public: - explicit UAVGadgetDecorator(IUAVGadget *gadget, QList *configurations); + explicit UAVGadgetDecorator(IUAVGadget *gadget, QList *configurations); ~UAVGadgetDecorator(); - QWidget *widget() { return m_gadget->widget(); } - QComboBox *toolBar() { return m_toolbar; } - IUAVGadgetConfiguration *activeConfiguration() { return m_activeConfiguration; } + QWidget *widget() + { + return m_gadget->widget(); + } + QComboBox *toolBar() + { + return m_toolbar; + } + IUAVGadgetConfiguration *activeConfiguration() + { + return m_activeConfiguration; + } void loadConfiguration(IUAVGadgetConfiguration *config); - void saveState(QSettings* qSettings); - void restoreState(QSettings* qSettings); + void saveState(QSettings *qSettings); + void restoreState(QSettings *qSettings); public slots: - void configurationChanged(IUAVGadgetConfiguration* config); - void configurationAdded(IUAVGadgetConfiguration* config); - void configurationToBeDeleted(IUAVGadgetConfiguration* config); - void configurationNameChanged(IUAVGadgetConfiguration* config, QString oldName, QString newName); + void configurationChanged(IUAVGadgetConfiguration *config); + void configurationAdded(IUAVGadgetConfiguration *config); + void configurationToBeDeleted(IUAVGadgetConfiguration *config); + void configurationNameChanged(IUAVGadgetConfiguration *config, QString oldName, QString newName); private slots: void loadConfiguration(int index); private: @@ -58,10 +65,8 @@ private: IUAVGadget *m_gadget; QComboBox *m_toolbar; IUAVGadgetConfiguration *m_activeConfiguration; - QList *m_configurations; - + QList *m_configurations; }; - } // namespace Core #endif // UAVGADGETDECORATOR_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp index a20612c59..1aa28d890 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp @@ -9,19 +9,19 @@ * @{ * @brief The Core GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -50,12 +50,12 @@ UAVGadgetInstanceManager::UAVGadgetInstanceManager(QObject *parent) : QObject(parent) { m_pm = ExtensionSystem::PluginManager::instance(); - QList factories = m_pm->getObjects(); - foreach (IUAVGadgetFactory *f, factories) { + QList factories = m_pm->getObjects(); + foreach(IUAVGadgetFactory * f, factories) { if (!m_factories.contains(f)) { m_factories.append(f); QString classId = f->classId(); - QString name = f->name(); + QString name = f->name(); QIcon icon = f->icon(); m_classIdNameMap.insert(classId, name); m_classIdIconMap.insert(classId, icon); @@ -65,40 +65,37 @@ UAVGadgetInstanceManager::UAVGadgetInstanceManager(QObject *parent) : UAVGadgetInstanceManager::~UAVGadgetInstanceManager() { - foreach (IOptionsPage *page, m_optionsPages) { + foreach(IOptionsPage * page, m_optionsPages) { m_pm->removeObject(page); delete page; } - } void UAVGadgetInstanceManager::readSettings(QSettings *qs) { - while ( !m_configurations.isEmpty() ){ - emit configurationToBeDeleted(m_configurations.takeLast()); + while (!m_configurations.isEmpty()) { + emit configurationToBeDeleted(m_configurations.takeLast()); } qs->beginGroup("UAVGadgetConfigurations"); UAVConfigInfo configInfo(qs); configInfo.setNameOfConfigurable("UAVGadgetConfigurations"); - if ( configInfo.version() == UAVConfigVersion() ){ + if (configInfo.version() == UAVConfigVersion()) { // If version is not set, assume its a old version before readable config (1.0.0). // however compatibility to 1.0.0 is broken. configInfo.setVersion("1.0.0"); } - if ( configInfo.version() == UAVConfigVersion("1.1.0") ){ + if (configInfo.version() == UAVConfigVersion("1.1.0")) { configInfo.notify(tr("Migrating UAVGadgetConfigurations from version 1.1.0 to ") + m_versionUAVGadgetConfigurations.toString()); readConfigs_1_1_0(qs); // this is fully compatible with 1.2.0 - } - else if ( !configInfo.standardVersionHandlingOK(m_versionUAVGadgetConfigurations) ){ + } else if (!configInfo.standardVersionHandlingOK(m_versionUAVGadgetConfigurations)) { // We are in trouble now. User wants us to quit the import, but when he saves // the GCS, his old config will be lost. configInfo.notify( - tr("You might want to save your old config NOW since it might be replaced by broken one when you exit the GCS!") - ); - } - else{ + tr("You might want to save your old config NOW since it might be replaced by broken one when you exit the GCS!") + ); + } else { readConfigs_1_2_0(qs); } @@ -110,32 +107,31 @@ void UAVGadgetInstanceManager::readConfigs_1_2_0(QSettings *qs) { UAVConfigInfo configInfo; - foreach (QString classId, m_classIdNameMap.keys()) - { + foreach(QString classId, m_classIdNameMap.keys()) { IUAVGadgetFactory *f = factory(classId); + qs->beginGroup(classId); QStringList configs = QStringList(); configs = qs->childGroups(); - foreach (QString configName, configs) { - qDebug() << "Loading config: " << classId << "," << configName; + foreach(QString configName, configs) { + qDebug() << "Loading config: " << classId << "," << configName; qs->beginGroup(configName); configInfo.read(qs); - configInfo.setNameOfConfigurable(classId+"-"+configName); + configInfo.setNameOfConfigurable(classId + "-" + configName); qs->beginGroup("data"); IUAVGadgetConfiguration *config = f->createConfiguration(qs, &configInfo); - if (config){ + if (config) { config->setName(configName); config->setProvisionalName(configName); config->setLocked(configInfo.locked()); int idx = indexForConfig(m_configurations, classId, configName); - if ( idx >= 0 ){ + if (idx >= 0) { // We should replace the config, but it might be used, so just // throw it out of the list. The GCS should be reinitialised soon. m_configurations[idx] = config; - } - else{ + } else { m_configurations.append(config); } } @@ -161,31 +157,30 @@ void UAVGadgetInstanceManager::readConfigs_1_1_0(QSettings *qs) { UAVConfigInfo configInfo; - foreach (QString classId, m_classIdNameMap.keys()) - { + foreach(QString classId, m_classIdNameMap.keys()) { IUAVGadgetFactory *f = factory(classId); + qs->beginGroup(classId); QStringList configs = QStringList(); configs = qs->childGroups(); - foreach (QString configName, configs) { - qDebug() << "Loading config: " << classId << "," << configName; + foreach(QString configName, configs) { + qDebug() << "Loading config: " << classId << "," << configName; qs->beginGroup(configName); bool locked = qs->value("config.locked").toBool(); - configInfo.setNameOfConfigurable(classId+"-"+configName); + configInfo.setNameOfConfigurable(classId + "-" + configName); IUAVGadgetConfiguration *config = f->createConfiguration(qs, &configInfo); - if (config){ + if (config) { config->setName(configName); config->setProvisionalName(configName); config->setLocked(locked); int idx = indexForConfig(m_configurations, classId, configName); - if ( idx >= 0 ){ + if (idx >= 0) { // We should replace the config, but it might be used, so just // throw it out of the list. The GCS should be reinitialised soon. m_configurations[idx] = config; - } - else{ + } else { m_configurations.append(config); } } @@ -209,13 +204,13 @@ void UAVGadgetInstanceManager::readConfigs_1_1_0(QSettings *qs) void UAVGadgetInstanceManager::saveSettings(QSettings *qs) { UAVConfigInfo *configInfo; + qs->beginGroup("UAVGadgetConfigurations"); qs->remove(""); // Remove existing configurations configInfo = new UAVConfigInfo(m_versionUAVGadgetConfigurations, "UAVGadgetConfigurations"); configInfo->save(qs); delete configInfo; - foreach (IUAVGadgetConfiguration *config, m_configurations) - { + foreach(IUAVGadgetConfiguration * config, m_configurations) { configInfo = new UAVConfigInfo(config); qs->beginGroup(config->classId()); qs->beginGroup(config->name()); @@ -235,11 +230,11 @@ void UAVGadgetInstanceManager::createOptionsPages() // In case there are pages (import a configuration), remove them. // Maybe they should be deleted as well (memory-leak), // but this might lead to NULL-pointers? - while (!m_optionsPages.isEmpty()){ + while (!m_optionsPages.isEmpty()) { m_pm->removeObject(m_optionsPages.takeLast()); } - QMutableListIterator ite(m_configurations); + QMutableListIterator ite(m_configurations); while (ite.hasNext()) { IUAVGadgetConfiguration *config = ite.next(); IUAVGadgetFactory *f = factory(config->classId()); @@ -249,11 +244,10 @@ void UAVGadgetInstanceManager::createOptionsPages() page->setIcon(f->icon()); m_optionsPages.append(page); m_pm->addObject(page); - } - else { + } else { qWarning() - << "UAVGadgetInstanceManager::createOptionsPages - failed to create options page for configuration " - + config->classId() + ":" + config->name() + ", configuration will be removed."; + << "UAVGadgetInstanceManager::createOptionsPages - failed to create options page for configuration " + + config->classId() + ":" + config->name() + ", configuration will be removed."; // The m_optionsPages list and m_configurations list must be in synch otherwise nasty issues happen later // so if we fail to create an options page we must remove the associated configuration ite.remove(); @@ -265,8 +259,9 @@ void UAVGadgetInstanceManager::createOptionsPages() IUAVGadget *UAVGadgetInstanceManager::createGadget(QString classId, QWidget *parent, bool loadDefaultConfiguration) { IUAVGadgetFactory *f = factory(classId); + if (f) { - QList *configs = configurations(classId); + QList *configs = configurations(classId); IUAVGadget *g = f->createGadget(parent); UAVGadgetDecorator *gadget = new UAVGadgetDecorator(g, configs); if ((loadDefaultConfiguration && configs && configs->count()) > 0) { @@ -274,10 +269,10 @@ IUAVGadget *UAVGadgetInstanceManager::createGadget(QString classId, QWidget *par } m_gadgetInstances.append(gadget); - connect(this, SIGNAL(configurationAdded(IUAVGadgetConfiguration*)), gadget, SLOT(configurationAdded(IUAVGadgetConfiguration*))); - connect(this, SIGNAL(configurationChanged(IUAVGadgetConfiguration*)), gadget, SLOT(configurationChanged(IUAVGadgetConfiguration*))); - connect(this, SIGNAL(configurationNameChanged(IUAVGadgetConfiguration*, QString,QString)), gadget, SLOT(configurationNameChanged(IUAVGadgetConfiguration*, QString,QString))); - connect(this, SIGNAL(configurationToBeDeleted(IUAVGadgetConfiguration*)), gadget, SLOT(configurationToBeDeleted(IUAVGadgetConfiguration*))); + connect(this, SIGNAL(configurationAdded(IUAVGadgetConfiguration *)), gadget, SLOT(configurationAdded(IUAVGadgetConfiguration *))); + connect(this, SIGNAL(configurationChanged(IUAVGadgetConfiguration *)), gadget, SLOT(configurationChanged(IUAVGadgetConfiguration *))); + connect(this, SIGNAL(configurationNameChanged(IUAVGadgetConfiguration *, QString, QString)), gadget, SLOT(configurationNameChanged(IUAVGadgetConfiguration *, QString, QString))); + connect(this, SIGNAL(configurationToBeDeleted(IUAVGadgetConfiguration *)), gadget, SLOT(configurationToBeDeleted(IUAVGadgetConfiguration *))); return gadget; } return 0; @@ -293,15 +288,15 @@ void UAVGadgetInstanceManager::removeGadget(IUAVGadget *gadget) } /** - * Removes all the gadgets. This is called by the core plugin when - * shutting down: this ensures that all registered gadget factory destructors are - * indeed called when the GCS is shutting down. We can't destroy them at the end - * (coreplugin is deleted last), because the gadgets sometimes depend on other - * plugins, like uavobjects... - */ + * Removes all the gadgets. This is called by the core plugin when + * shutting down: this ensures that all registered gadget factory destructors are + * indeed called when the GCS is shutting down. We can't destroy them at the end + * (coreplugin is deleted last), because the gadgets sometimes depend on other + * plugins, like uavobjects... + */ void UAVGadgetInstanceManager::removeAllGadgets() { - foreach( IUAVGadget *gadget, m_gadgetInstances) { + foreach(IUAVGadget * gadget, m_gadgetInstances) { m_gadgetInstances.removeOne(gadget); delete gadget; } @@ -311,20 +306,21 @@ void UAVGadgetInstanceManager::removeAllGadgets() bool UAVGadgetInstanceManager::canDeleteConfiguration(IUAVGadgetConfiguration *config) { // to be able to delete a configuration, no instance must be using it - foreach (IUAVGadget *gadget, m_gadgetInstances) { + foreach(IUAVGadget * gadget, m_gadgetInstances) { if (gadget->activeConfiguration() == config) { - return false; + return false; } } // and it cannot be the only configuration - foreach (IUAVGadgetConfiguration *c, m_configurations) { - if (c != config && c->classId() == config->classId()) + foreach(IUAVGadgetConfiguration * c, m_configurations) { + if (c != config && c->classId() == config->classId()) { return true; + } } return false; } -void UAVGadgetInstanceManager::deleteConfiguration(IUAVGadgetConfiguration *config) +void UAVGadgetInstanceManager::deleteConfiguration(IUAVGadgetConfiguration *config) { m_provisionalDeletes.append(config); if (m_provisionalConfigs.contains(config)) { @@ -340,16 +336,17 @@ void UAVGadgetInstanceManager::deleteConfiguration(IUAVGadgetConfiguration *con configurationNameEdited("", false); } -void UAVGadgetInstanceManager::cloneConfiguration(IUAVGadgetConfiguration *configToClone) +void UAVGadgetInstanceManager::cloneConfiguration(IUAVGadgetConfiguration *configToClone) { QString name = suggestName(configToClone->classId(), configToClone->name()); IUAVGadgetConfiguration *config = configToClone->clone(); + config->setName(name); config->setProvisionalName(name); IUAVGadgetFactory *f = factory(config->classId()); IOptionsPage *p = f->createOptionsPage(config); - IOptionsPage *page = new UAVGadgetOptionsPageDecorator(p, config); + IOptionsPage *page = new UAVGadgetOptionsPageDecorator(p, config); page->setIcon(f->icon()); m_provisionalConfigs.append(config); m_provisionalOptionsPages.append(page); @@ -364,12 +361,13 @@ QString UAVGadgetInstanceManager::suggestName(QString classId, QString name) QString last = name.split(" ").last(); bool ok; int num = last.toInt(&ok); - int i = 1; + int i = 1; + if (ok) { QStringList n = name.split(" "); n.removeLast(); name = n.join(" "); - i = num+1; + i = num + 1; } do { suggestedName = name + " " + QString::number(i); @@ -391,7 +389,7 @@ void UAVGadgetInstanceManager::applyChanges(IUAVGadgetConfiguration *config) m_takenNames[config->classId()].removeAt(j); m_pm->removeObject(m_optionsPages.at(i)); m_configurations.removeAt(i); - m_optionsPages.removeAt(i);//TODO delete + m_optionsPages.removeAt(i); // TODO delete } return; } @@ -416,33 +414,39 @@ void UAVGadgetInstanceManager::applyChanges(IUAVGadgetConfiguration *config) void UAVGadgetInstanceManager::configurationNameEdited(QString text, bool hasText) { bool disable = false; - foreach (IUAVGadgetConfiguration *c, m_configurations) { - foreach (IUAVGadgetConfiguration *d, m_configurations) { - if (c != d && c->classId() == d->classId() && c->provisionalName() == d->provisionalName()) + + foreach(IUAVGadgetConfiguration * c, m_configurations) { + foreach(IUAVGadgetConfiguration * d, m_configurations) { + if (c != d && c->classId() == d->classId() && c->provisionalName() == d->provisionalName()) { disable = true; + } } - foreach (IUAVGadgetConfiguration *d, m_provisionalConfigs) { - if (c != d && c->classId() == d->classId() && c->provisionalName() == d->provisionalName()) + foreach(IUAVGadgetConfiguration * d, m_provisionalConfigs) { + if (c != d && c->classId() == d->classId() && c->provisionalName() == d->provisionalName()) { disable = true; + } } } - foreach (IUAVGadgetConfiguration *c, m_provisionalConfigs) { - foreach (IUAVGadgetConfiguration *d, m_provisionalConfigs) { - if (c != d && c->classId() == d->classId() && c->provisionalName() == d->provisionalName()) + foreach(IUAVGadgetConfiguration * c, m_provisionalConfigs) { + foreach(IUAVGadgetConfiguration * d, m_provisionalConfigs) { + if (c != d && c->classId() == d->classId() && c->provisionalName() == d->provisionalName()) { disable = true; + } } } - if (hasText && text == "") + if (hasText && text == "") { disable = true; + } m_settingsDialog->disableApplyOk(disable); - if (hasText) + if (hasText) { m_settingsDialog->updateText(text); + } } -void UAVGadgetInstanceManager::settingsDialogShown(Core::Internal::SettingsDialog* settingsDialog) +void UAVGadgetInstanceManager::settingsDialogShown(Core::Internal::SettingsDialog *settingsDialog) { - foreach (QString classId, classIds()) - m_takenNames.insert(classId, configurationNames(classId)); + foreach(QString classId, classIds()) + m_takenNames.insert(classId, configurationNames(classId)); m_settingsDialog = settingsDialog; } @@ -451,18 +455,20 @@ void UAVGadgetInstanceManager::settingsDialogRemoved() m_takenNames.clear(); m_provisionalConfigs.clear(); m_provisionalDeletes.clear(); - m_provisionalOptionsPages.clear(); //TODO delete - foreach (IUAVGadgetConfiguration *config, m_configurations) - config->setProvisionalName(config->name()); + m_provisionalOptionsPages.clear(); // TODO delete + foreach(IUAVGadgetConfiguration * config, m_configurations) + config->setProvisionalName(config->name()); m_settingsDialog = 0; } QStringList UAVGadgetInstanceManager::configurationNames(QString classId) const { QStringList names; - foreach (IUAVGadgetConfiguration *config, m_configurations) { - if (config->classId() == classId) + + foreach(IUAVGadgetConfiguration * config, m_configurations) { + if (config->classId() == classId) { names.append(config->name()); + } } return names; } @@ -479,30 +485,33 @@ QIcon UAVGadgetInstanceManager::gadgetIcon(QString classId) const IUAVGadgetFactory *UAVGadgetInstanceManager::factory(QString classId) const { - foreach (IUAVGadgetFactory *f, m_factories) { - if (f->classId() == classId) + foreach(IUAVGadgetFactory * f, m_factories) { + if (f->classId() == classId) { return f; + } } return 0; } -QList *UAVGadgetInstanceManager::configurations(QString classId) const +QList *UAVGadgetInstanceManager::configurations(QString classId) const { - QList *configs = new QList; - foreach (IUAVGadgetConfiguration *config, m_configurations) { - if (config->classId() == classId) + QList *configs = new QList; + foreach(IUAVGadgetConfiguration * config, m_configurations) { + if (config->classId() == classId) { configs->append(config); + } } return configs; } -int UAVGadgetInstanceManager::indexForConfig(QList configurations, - QString classId, QString configName) +int UAVGadgetInstanceManager::indexForConfig(QList configurations, + QString classId, QString configName) { - for ( int i=0; iclassId() == classId - && configurations.at(i)->name() == configName ) + for (int i = 0; i < configurations.length(); ++i) { + if (configurations.at(i)->classId() == classId + && configurations.at(i)->name() == configName) { return i; + } } return -1; } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.h b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.h index a67953fc1..23ee1075a 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.h @@ -9,19 +9,19 @@ * @{ * @brief The Core GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,12 +37,10 @@ #include "uavconfiginfo.h" namespace ExtensionSystem { - class PluginManager; +class PluginManager; } -namespace Core -{ - +namespace Core { namespace Internal { class SettingsDialog; } @@ -52,10 +50,8 @@ class IUAVGadgetConfiguration; class IOptionsPage; class IUAVGadgetFactory; -class CORE_EXPORT UAVGadgetInstanceManager : public QObject -{ - -Q_OBJECT +class CORE_EXPORT UAVGadgetInstanceManager : public QObject { + Q_OBJECT public: explicit UAVGadgetInstanceManager(QObject *parent = 0); ~UAVGadgetInstanceManager(); @@ -69,44 +65,46 @@ public: void cloneConfiguration(IUAVGadgetConfiguration *config); void applyChanges(IUAVGadgetConfiguration *config); void configurationNameEdited(QString text, bool hasText = true); - QStringList classIds() const { return m_classIdNameMap.keys(); } + QStringList classIds() const + { + return m_classIdNameMap.keys(); + } QStringList configurationNames(QString classId) const; QString gadgetName(QString classId) const; QIcon gadgetIcon(QString classId) const; signals: - void configurationChanged(IUAVGadgetConfiguration* config); - void configurationAdded(IUAVGadgetConfiguration* config); - void configurationToBeDeleted(IUAVGadgetConfiguration* config); - void configurationNameChanged(IUAVGadgetConfiguration* config, QString oldName, QString newName); + void configurationChanged(IUAVGadgetConfiguration *config); + void configurationAdded(IUAVGadgetConfiguration *config); + void configurationToBeDeleted(IUAVGadgetConfiguration *config); + void configurationNameChanged(IUAVGadgetConfiguration *config, QString oldName, QString newName); public slots: - void settingsDialogShown(Core::Internal::SettingsDialog* settingsDialog); + void settingsDialogShown(Core::Internal::SettingsDialog *settingsDialog); void settingsDialogRemoved(); private: IUAVGadgetFactory *factory(QString classId) const; void createOptionsPages(); - QList *configurations(QString classId) const; + QList *configurations(QString classId) const; QString suggestName(QString classId, QString name); - QList m_gadgetInstances; - QList m_factories; - QList m_configurations; - QList m_optionsPages; + QList m_gadgetInstances; + QList m_factories; + QList m_configurations; + QList m_optionsPages; QMap m_classIdNameMap; QMap m_classIdIconMap; QMap m_takenNames; - QList m_provisionalConfigs; - QList m_provisionalDeletes; - QList m_provisionalOptionsPages; + QList m_provisionalConfigs; + QList m_provisionalDeletes; + QList m_provisionalOptionsPages; Core::Internal::SettingsDialog *m_settingsDialog; ExtensionSystem::PluginManager *m_pm; - int indexForConfig(QList configurations, + int indexForConfig(QList configurations, QString classId, QString configName); void readConfigs_1_1_0(QSettings *qs); void readConfigs_1_2_0(QSettings *qs); }; - } // namespace Core #endif // UAVGADGETINSTANCEMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.cpp index 502f75eef..e9868b4a3 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.cpp @@ -43,21 +43,21 @@ using namespace Core; using namespace Core::Internal; SplitterOrView::SplitterOrView(Core::UAVGadgetManager *uavGadgetManager, Core::IUAVGadget *uavGadget) : - m_uavGadgetManager(uavGadgetManager), - m_splitter(0) + m_uavGadgetManager(uavGadgetManager), + m_splitter(0) { - m_view = new UAVGadgetView(m_uavGadgetManager, uavGadget, this); + m_view = new UAVGadgetView(m_uavGadgetManager, uavGadget, this); m_layout = new QStackedLayout(this); m_layout->addWidget(m_view); } SplitterOrView::~SplitterOrView() { - if(m_view) { + if (m_view) { delete m_view; m_view = 0; } - if(m_splitter) { + if (m_splitter) { delete m_splitter; m_splitter = 0; } @@ -65,8 +65,9 @@ SplitterOrView::~SplitterOrView() void SplitterOrView::mousePressEvent(QMouseEvent *e) { - if (e->button() != Qt::LeftButton) + if (e->button() != Qt::LeftButton) { return; + } if (gadget()) { setFocus(Qt::MouseFocusReason); m_uavGadgetManager->setCurrentGadget(this->gadget()); @@ -80,9 +81,11 @@ SplitterOrView *SplitterOrView::findFirstView() { if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) - if (SplitterOrView *result = splitterOrView->findFirstView()) + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { + if (SplitterOrView * result = splitterOrView->findFirstView()) { return result; + } + } } return 0; } @@ -94,13 +97,16 @@ SplitterOrView *SplitterOrView::findFirstView() */ SplitterOrView *SplitterOrView::findView(Core::IUAVGadget *uavGadget) { - if (!uavGadget || hasGadget(uavGadget)) + if (!uavGadget || hasGadget(uavGadget)) { return this; + } if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) - if (SplitterOrView *result = splitterOrView->findView(uavGadget)) + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { + if (SplitterOrView * result = splitterOrView->findView(uavGadget)) { return result; + } + } } } return 0; @@ -111,13 +117,16 @@ SplitterOrView *SplitterOrView::findView(Core::IUAVGadget *uavGadget) */ SplitterOrView *SplitterOrView::findView(UAVGadgetView *view) { - if (view == m_view) + if (view == m_view) { return this; + } if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) - if (SplitterOrView *result = splitterOrView->findView(view)) + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { + if (SplitterOrView * result = splitterOrView->findView(view)) { return result; + } + } } } return 0; @@ -131,11 +140,13 @@ SplitterOrView *SplitterOrView::findSplitter(Core::IUAVGadget *uavGadget) { if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) { - if (splitterOrView->hasGadget(uavGadget)) + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { + if (splitterOrView->hasGadget(uavGadget)) { return this; - if (SplitterOrView *result = splitterOrView->findSplitter(uavGadget)) + } + if (SplitterOrView * result = splitterOrView->findSplitter(uavGadget)) { return result; + } } } } @@ -150,11 +161,13 @@ SplitterOrView *SplitterOrView::findSplitter(SplitterOrView *child) { if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) { - if (splitterOrView == child) + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { + if (splitterOrView == child) { return this; - if (SplitterOrView *result = splitterOrView->findSplitter(child)) + } + if (SplitterOrView * result = splitterOrView->findSplitter(child)) { return result; + } } } } @@ -168,6 +181,7 @@ SplitterOrView *SplitterOrView::findSplitter(SplitterOrView *child) SplitterOrView *SplitterOrView::findNextView(SplitterOrView *view) { bool found = false; + return findNextView_helper(view, &found); } @@ -184,9 +198,10 @@ SplitterOrView *SplitterOrView::findNextView_helper(SplitterOrView *view, bool * if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) { - if (SplitterOrView *result = splitterOrView->findNextView_helper(view, found)) + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { + if (SplitterOrView * result = splitterOrView->findNextView_helper(view, found)) { return result; + } } } } @@ -195,16 +210,19 @@ SplitterOrView *SplitterOrView::findNextView_helper(SplitterOrView *view, bool * QSize SplitterOrView::minimumSizeHint() const { - if (m_splitter) + if (m_splitter) { return m_splitter->minimumSizeHint(); + } return QSize(64, 64); } QSplitter *SplitterOrView::takeSplitter() { QSplitter *oldSplitter = m_splitter; - if (m_splitter) + + if (m_splitter) { m_layout->removeWidget(m_splitter); + } m_splitter = 0; return oldSplitter; } @@ -212,21 +230,24 @@ QSplitter *SplitterOrView::takeSplitter() UAVGadgetView *SplitterOrView::takeView() { UAVGadgetView *oldView = m_view; - if (m_view) + + if (m_view) { m_layout->removeWidget(m_view); + } m_view = 0; return oldView; } -QList SplitterOrView::gadgets() +QList SplitterOrView::gadgets() { - QList g; - if (hasGadget()) + QList g; + if (hasGadget()) { g.append(gadget()); + } if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) { - QList result = splitterOrView->gadgets(); + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { + QList result = splitterOrView->gadgets(); g.append(result); } } @@ -240,7 +261,7 @@ void SplitterOrView::split(Qt::Orientation orientation) Q_ASSERT(!m_splitter); m_splitter = new MiniSplitter(this); m_splitter->setOrientation(orientation); - connect(m_splitter, SIGNAL(splitterMoved(int,int)), this, SLOT(onSplitterMoved(int,int))); + connect(m_splitter, SIGNAL(splitterMoved(int, int)), this, SLOT(onSplitterMoved(int, int))); m_layout->addWidget(m_splitter); Core::IUAVGadget *ourGadget = m_view->gadget(); @@ -263,7 +284,8 @@ void SplitterOrView::split(Qt::Orientation orientation) } } -void SplitterOrView::onSplitterMoved( int pos, int index ) { +void SplitterOrView::onSplitterMoved(int pos, int index) +{ Q_UNUSED(pos); Q_UNUSED(index); // Update when the splitter is actually moved. @@ -280,7 +302,7 @@ void SplitterOrView::unsplitAll(IUAVGadget *currentGadget) delete m_splitter; m_splitter = 0; - m_view = new UAVGadgetView(m_uavGadgetManager, currentGadget, this); + m_view = new UAVGadgetView(m_uavGadgetManager, currentGadget, this); m_layout->addWidget(m_view); } @@ -291,7 +313,7 @@ void SplitterOrView::unsplitAll_helper() } if (m_splitter) { for (int i = 0; i < m_splitter->count(); ++i) { - if (SplitterOrView *splitterOrView = qobject_cast(m_splitter->widget(i))) { + if (SplitterOrView * splitterOrView = qobject_cast(m_splitter->widget(i))) { splitterOrView->unsplitAll_helper(); } } @@ -300,10 +322,11 @@ void SplitterOrView::unsplitAll_helper() void SplitterOrView::unsplit() { - if (!m_splitter) + if (!m_splitter) { return; + } Q_ASSERT(m_splitter->count() == 1); - SplitterOrView *childSplitterOrView = qobject_cast(m_splitter->widget(0)); + SplitterOrView *childSplitterOrView = qobject_cast(m_splitter->widget(0)); QSplitter *oldSplitter = m_splitter; m_splitter = 0; @@ -316,7 +339,7 @@ void SplitterOrView::unsplit() UAVGadgetView *childView = childSplitterOrView->view(); Q_ASSERT(childView); if (m_view) { - if (IUAVGadget *e = childView->gadget()) { + if (IUAVGadget * e = childView->gadget()) { childView->removeGadget(); m_view->setGadget(e); } @@ -331,43 +354,45 @@ void SplitterOrView::unsplit() m_uavGadgetManager->setCurrentGadget(findFirstView()->gadget()); } -void SplitterOrView::saveState(QSettings* qSettings) const { +void SplitterOrView::saveState(QSettings *qSettings) const +{ if (m_splitter) { qSettings->setValue("type", "splitter"); qSettings->setValue("splitterOrientation", (qint32)m_splitter->orientation()); QList sizesQVariant; - foreach (int value, m_sizes) { + foreach(int value, m_sizes) { sizesQVariant.append(value); } qSettings->setValue("splitterSizes", sizesQVariant); qSettings->beginGroup("side0"); - static_cast(m_splitter->widget(0))->saveState(qSettings); + static_cast(m_splitter->widget(0))->saveState(qSettings); qSettings->endGroup(); qSettings->beginGroup("side1"); - static_cast(m_splitter->widget(1))->saveState(qSettings); + static_cast(m_splitter->widget(1))->saveState(qSettings); qSettings->endGroup(); } else if (gadget()) { m_view->saveState(qSettings); } } -void SplitterOrView::restoreState(QSettings* qSettings) +void SplitterOrView::restoreState(QSettings *qSettings) { QString mode = qSettings->value("type").toString(); + if (mode == "splitter") { qint32 orientation = qSettings->value("splitterOrientation").toInt(); QList sizesQVariant = qSettings->value("splitterSizes").toList(); m_sizes.clear(); - foreach (QVariant value, sizesQVariant) { + foreach(QVariant value, sizesQVariant) { m_sizes.append(value.toInt()); } split((Qt::Orientation)orientation); m_splitter->setSizes(m_sizes); qSettings->beginGroup("side0"); - static_cast(m_splitter->widget(0))->restoreState(qSettings); + static_cast(m_splitter->widget(0))->restoreState(qSettings); qSettings->endGroup(); qSettings->beginGroup("side1"); - static_cast(m_splitter->widget(1))->restoreState(qSettings); + static_cast(m_splitter->widget(1))->restoreState(qSettings); qSettings->endGroup(); } else if (mode == "uavGadget") { m_view->restoreState(qSettings); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.h b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.h index 02e4e253a..a3a782d33 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.h @@ -35,11 +35,8 @@ #include "uavgadgetview.h" namespace Core { - namespace Internal { - -class SplitterOrView : public QWidget -{ +class SplitterOrView : public QWidget { Q_OBJECT public: SplitterOrView(UAVGadgetManager *uavGadgetManager, Core::IUAVGadget *uavGadget = 0); @@ -48,20 +45,41 @@ public: void split(Qt::Orientation orientation); void unsplit(); - inline bool isView() const { return m_view != 0; } + inline bool isView() const + { + return m_view != 0; + } - inline bool isSplitter() const { return m_splitter != 0; } - inline Core::IUAVGadget *gadget() const { return m_view ? m_view->gadget() : 0; } - inline bool hasGadget(Core::IUAVGadget *uavGadget) const { return m_view && m_view->hasGadget(uavGadget); } - inline bool hasGadget() const { return m_view && (m_view->gadget() != 0); } - inline UAVGadgetView *view() const { return m_view; } - inline QSplitter *splitter() const { return m_splitter; } - QList gadgets(); + inline bool isSplitter() const + { + return m_splitter != 0; + } + inline Core::IUAVGadget *gadget() const + { + return m_view ? m_view->gadget() : 0; + } + inline bool hasGadget(Core::IUAVGadget *uavGadget) const + { + return m_view && m_view->hasGadget(uavGadget); + } + inline bool hasGadget() const + { + return m_view && (m_view->gadget() != 0); + } + inline UAVGadgetView *view() const + { + return m_view; + } + inline QSplitter *splitter() const + { + return m_splitter; + } + QList gadgets(); QSplitter *takeSplitter(); UAVGadgetView *takeView(); - void saveState(QSettings*) const; - void restoreState(QSettings*); + void saveState(QSettings *) const; + void restoreState(QSettings *); SplitterOrView *findView(Core::IUAVGadget *uavGadget); SplitterOrView *findView(UAVGadgetView *view); @@ -71,18 +89,21 @@ public: SplitterOrView *findNextView(SplitterOrView *view); - QSize sizeHint() const { return minimumSizeHint(); } + QSize sizeHint() const + { + return minimumSizeHint(); + } QSize minimumSizeHint() const; void unsplitAll(IUAVGadget *currentGadget); protected: -// void paintEvent(QPaintEvent *); +// void paintEvent(QPaintEvent *); void mousePressEvent(QMouseEvent *e); private slots: // Called when the user moves the splitter, and updates our m_sizes. - void onSplitterMoved( int pos, int index ); + void onSplitterMoved(int pos, int index); private: void unsplitAll_helper(); @@ -103,8 +124,6 @@ private: // The splitter sizes. We keep our own copy of these, since after loading they can't realiably be retrieved. QList m_sizes; }; - - } } #endif // SPLITTERORVIEW_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.cpp index 1033de615..6da5940e7 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.cpp @@ -63,20 +63,20 @@ #include #include -Q_DECLARE_METATYPE(Core::IUAVGadget*) +Q_DECLARE_METATYPE(Core::IUAVGadget *) using namespace Core; using namespace Core::Internal; using namespace Utils; -enum { debugUAVGadgetManager=0 }; +enum { debugUAVGadgetManager = 0 }; static inline ExtensionSystem::PluginManager *pluginManager() { return ExtensionSystem::PluginManager::instance(); } -//===================UAVGadgetManager===================== +// ===================UAVGadgetManager===================== UAVGadgetManager::UAVGadgetManager(ICore *core, QString name, QIcon icon, int priority, QString uniqueName, QWidget *parent) : m_showToolbars(true), @@ -88,24 +88,24 @@ UAVGadgetManager::UAVGadgetManager(ICore *core, QString name, QIcon icon, int pr m_priority(priority), m_widget(new QWidget(parent)) { - // checking that the mode name is unique gives harmless // warnings on the console output ModeManager *modeManager = ModeManager::instance(); + if (!modeManager->mode(uniqueName)) { m_uniqueName = uniqueName; } else { // this shouldn't happen m_uniqueName = uniqueName + QString::number(quint64(this)); } - m_uniqueNameBA = m_uniqueName.toLatin1(); + m_uniqueNameBA = m_uniqueName.toLatin1(); m_uniqueModeName = m_uniqueNameBA.data(); connect(m_core, SIGNAL(contextAboutToChange(Core::IContext *)), this, SLOT(handleContextChange(Core::IContext *))); - connect(modeManager, SIGNAL(currentModeChanged(Core::IMode*)), - this, SLOT(modeChanged(Core::IMode*))); + connect(modeManager, SIGNAL(currentModeChanged(Core::IMode *)), + this, SLOT(modeChanged(Core::IMode *))); // other setup m_splitterOrView = new SplitterOrView(this, 0); @@ -125,20 +125,21 @@ UAVGadgetManager::UAVGadgetManager(ICore *core, QString name, QIcon icon, int pr } UAVGadgetManager::~UAVGadgetManager() -{ -} +{} QList UAVGadgetManager::context() const { static QList contexts = QList() << - UniqueIDManager::instance()->uniqueIdentifier(Constants::C_UAVGADGETMANAGER); + UniqueIDManager::instance()->uniqueIdentifier(Constants::C_UAVGADGETMANAGER); + return contexts; } void UAVGadgetManager::modeChanged(Core::IMode *mode) { - if (mode != this) + if (mode != this) { return; + } m_currentGadget->widget()->setFocus(); showToolbars(toolbarsShown()); @@ -152,26 +153,31 @@ void UAVGadgetManager::init() void UAVGadgetManager::handleContextChange(Core::IContext *context) { -// if (debugUAVGadgetManager) -// qDebug() << Q_FUNC_INFO << context; - IUAVGadget *uavGadget = context ? qobject_cast(context) : 0; - if (uavGadget) +// if (debugUAVGadgetManager) +// qDebug() << Q_FUNC_INFO << context; + IUAVGadget *uavGadget = context ? qobject_cast(context) : 0; + + if (uavGadget) { setCurrentGadget(uavGadget); + } } void UAVGadgetManager::setCurrentGadget(IUAVGadget *uavGadget) { - if (m_currentGadget == uavGadget) + if (m_currentGadget == uavGadget) { return; + } SplitterOrView *oldView = currentSplitterOrView(); m_currentGadget = uavGadget; - SplitterOrView *view = currentSplitterOrView(); + SplitterOrView *view = currentSplitterOrView(); if (oldView != view) { - if (oldView) + if (oldView) { oldView->update(); - if (view) + } + if (view) { view->update(); + } } uavGadget->widget()->setFocus(); emit currentGadgetChanged(uavGadget); @@ -182,8 +188,9 @@ void UAVGadgetManager::setCurrentGadget(IUAVGadget *uavGadget) */ Core::Internal::SplitterOrView *UAVGadgetManager::currentSplitterOrView() const { - if (!m_splitterOrView) // this is only for startup + if (!m_splitterOrView) { // this is only for startup return 0; + } SplitterOrView *view = m_currentGadget ? m_splitterOrView->findView(m_currentGadget) : 0; @@ -197,33 +204,36 @@ IUAVGadget *UAVGadgetManager::currentGadget() const void UAVGadgetManager::emptyView(Core::Internal::UAVGadgetView *view) { - if (!view) + if (!view) { return; + } IUAVGadget *uavGadget = view->gadget(); -// emit uavGadgetAboutToClose(uavGadget); +// emit uavGadgetAboutToClose(uavGadget); removeGadget(uavGadget); view->removeGadget(); -// emit uavGadgetsClosed(uavGadgets); +// emit uavGadgetsClosed(uavGadgets); } void UAVGadgetManager::closeView(Core::Internal::UAVGadgetView *view) { - if (!view) + if (!view) { return; + } SplitterOrView *splitterOrView = m_splitterOrView->findView(view); Q_ASSERT(splitterOrView); Q_ASSERT(splitterOrView->view() == view); - if (splitterOrView == m_splitterOrView) + if (splitterOrView == m_splitterOrView) { return; + } IUAVGadget *gadget = view->gadget(); emptyView(view); UAVGadgetInstanceManager *im = ICore::instance()->uavGadgetInstanceManager(); im->removeGadget(gadget); - SplitterOrView *splitter = m_splitterOrView->findSplitter(splitterOrView); + SplitterOrView *splitter = m_splitterOrView->findSplitter(splitterOrView); Q_ASSERT(splitterOrView->hasGadget() == false); Q_ASSERT(splitter->isSplitter() == true); splitterOrView->hide(); @@ -233,36 +243,41 @@ void UAVGadgetManager::closeView(Core::Internal::UAVGadgetView *view) SplitterOrView *newCurrent = splitter->findFirstView(); Q_ASSERT(newCurrent); - if (newCurrent) + if (newCurrent) { setCurrentGadget(newCurrent->gadget()); + } } void UAVGadgetManager::addGadgetToContext(IUAVGadget *gadget) { - if (!gadget) + if (!gadget) { return; + } m_core->addContextObject(gadget); -// emit uavGadgetOpened(uavGadget); +// emit uavGadgetOpened(uavGadget); } void UAVGadgetManager::removeGadget(IUAVGadget *gadget) { - if (!gadget) + if (!gadget) { return; - m_core->removeContextObject(qobject_cast(gadget)); + } + m_core->removeContextObject(qobject_cast(gadget)); } void UAVGadgetManager::ensureUAVGadgetManagerVisible() { - if (!m_widget->isVisible()) + if (!m_widget->isVisible()) { m_core->modeManager()->activateMode(this->uniqueModeName()); + } } void UAVGadgetManager::showToolbars(bool show) { - if (m_core->modeManager()->currentMode() != this) + if (m_core->modeManager()->currentMode() != this) { return; + } m_showToolbars = show; SplitterOrView *next = m_splitterOrView->findFirstView(); @@ -276,25 +291,27 @@ void UAVGadgetManager::showToolbars(bool show) void UAVGadgetManager::updateUavGadgetMenus() { - if (m_core->modeManager()->currentMode() != this) + if (m_core->modeManager()->currentMode() != this) { return; - if (!m_splitterOrView) // this is only for startup + } + if (!m_splitterOrView) { // this is only for startup return; + } // Splitting is only possible when the toolbars are shown bool hasSplitter = m_splitterOrView->isSplitter(); emit showUavGadgetMenus(m_showToolbars, hasSplitter); } -void UAVGadgetManager::saveState(QSettings* qSettings) const +void UAVGadgetManager::saveState(QSettings *qSettings) const { - qSettings->setValue("version","UAVGadgetManagerV1"); - qSettings->setValue("showToolbars",m_showToolbars); + qSettings->setValue("version", "UAVGadgetManagerV1"); + qSettings->setValue("showToolbars", m_showToolbars); qSettings->beginGroup("splitter"); m_splitterOrView->saveState(qSettings); qSettings->endGroup(); } -bool UAVGadgetManager::restoreState(QSettings* qSettings) +bool UAVGadgetManager::restoreState(QSettings *qSettings) { removeAllSplits(); @@ -338,12 +355,13 @@ void UAVGadgetManager::saveSettings(QSettings *qs) void UAVGadgetManager::readSettings(QSettings *qs) { QString uavGadgetManagerRootKey = "UAVGadgetManager"; - if(!qs->childGroups().contains(uavGadgetManagerRootKey)) { + + if (!qs->childGroups().contains(uavGadgetManagerRootKey)) { return; } qs->beginGroup(uavGadgetManagerRootKey); - if(!qs->childGroups().contains(uniqueModeName())) { + if (!qs->childGroups().contains(uniqueModeName())) { qs->endGroup(); return; } @@ -359,16 +377,17 @@ void UAVGadgetManager::readSettings(QSettings *qs) void UAVGadgetManager::split(Qt::Orientation orientation) { - if (m_core->modeManager()->currentMode() != this) + if (m_core->modeManager()->currentMode() != this) { return; + } IUAVGadget *uavGadget = m_currentGadget; Q_ASSERT(uavGadget); - SplitterOrView *view = currentSplitterOrView(); + SplitterOrView *view = currentSplitterOrView(); Q_ASSERT(view); view->split(orientation); - SplitterOrView *sor = m_splitterOrView->findView(uavGadget); + SplitterOrView *sor = m_splitterOrView->findView(uavGadget); SplitterOrView *next = m_splitterOrView->findNextView(sor); setCurrentGadget(next->gadget()); updateUavGadgetMenus(); @@ -386,12 +405,14 @@ void UAVGadgetManager::splitSideBySide() void UAVGadgetManager::removeCurrentSplit() { - if (m_core->modeManager()->currentMode() != this) + if (m_core->modeManager()->currentMode() != this) { return; + } SplitterOrView *viewToClose = currentSplitterOrView(); - if (viewToClose == m_splitterOrView) + if (viewToClose == m_splitterOrView) { return; + } closeView(viewToClose->view()); updateUavGadgetMenus(); } @@ -399,17 +420,19 @@ void UAVGadgetManager::removeCurrentSplit() // Removes all gadgets and splits in the workspace, except the current/active gadget. void UAVGadgetManager::removeAllSplits() { - if (m_core->modeManager()->currentMode() != this) + if (m_core->modeManager()->currentMode() != this) { return; + } - if (!m_splitterOrView->isSplitter()) + if (!m_splitterOrView->isSplitter()) { return; + } // Use a QPointer, just in case we accidently delete the gadget we want to keep. QPointer currentGadget = m_currentGadget; Q_ASSERT(currentGadget); - QList gadgets = m_splitterOrView->gadgets(); + QList gadgets = m_splitterOrView->gadgets(); Q_ASSERT(gadgets.count(currentGadget) == 1); gadgets.removeOne(currentGadget); @@ -422,7 +445,7 @@ void UAVGadgetManager::removeAllSplits() // Remove all other gadgets from the instance manager. UAVGadgetInstanceManager *im = ICore::instance()->uavGadgetInstanceManager(); - foreach (IUAVGadget *g, gadgets) { + foreach(IUAVGadget * g, gadgets) { im->removeGadget(g); } updateUavGadgetMenus(); @@ -430,14 +453,16 @@ void UAVGadgetManager::removeAllSplits() void UAVGadgetManager::gotoOtherSplit() { - if (m_core->modeManager()->currentMode() != this) + if (m_core->modeManager()->currentMode() != this) { return; + } if (m_splitterOrView->isSplitter()) { SplitterOrView *currentView = currentSplitterOrView(); SplitterOrView *view = m_splitterOrView->findNextView(currentView); - if (!view) + if (!view) { view = m_splitterOrView->findFirstView(); + } if (view) { setCurrentGadget(view->gadget()); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.h index ff2c76267..547c85e58 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.h @@ -45,20 +45,16 @@ class QModelIndex; QT_END_NAMESPACE namespace Core { - class IContext; class ICore; class IUAVGadget; namespace Internal { - class UAVGadgetView; class SplitterOrView; - } // namespace Internal -class CORE_EXPORT UAVGadgetManager : public IMode -{ +class CORE_EXPORT UAVGadgetManager : public IMode { Q_OBJECT public: @@ -67,26 +63,47 @@ public: virtual ~UAVGadgetManager(); // IMode - QString name() const { return m_name; } - QIcon icon() const { return m_icon; } - int priority() const { return m_priority; } - void setPriority(int priority) { m_priority = priority; } - const char *uniqueModeName() const { return m_uniqueModeName; } + QString name() const + { + return m_name; + } + QIcon icon() const + { + return m_icon; + } + int priority() const + { + return m_priority; + } + void setPriority(int priority) + { + m_priority = priority; + } + const char *uniqueModeName() const + { + return m_uniqueModeName; + } QList context() const; void init(); - QWidget *widget() { return m_widget; } + QWidget *widget() + { + return m_widget; + } void ensureUAVGadgetManagerVisible(); IUAVGadget *currentGadget() const; - void saveState(QSettings*) const; - bool restoreState(QSettings* qSettings); + void saveState(QSettings *) const; + bool restoreState(QSettings *qSettings); - void saveSettings(QSettings* qs); - void readSettings(QSettings* qs); - bool toolbarsShown() { return m_showToolbars; } + void saveSettings(QSettings *qs); + void readSettings(QSettings *qs); + bool toolbarsShown() + { + return m_showToolbars; + } signals: void currentGadgetChanged(IUAVGadget *gadget); @@ -126,13 +143,12 @@ private: int m_priority; QString m_uniqueName; QByteArray m_uniqueNameBA; - const char* m_uniqueModeName; + const char *m_uniqueModeName; QWidget *m_widget; friend class Core::Internal::SplitterOrView; friend class Core::Internal::UAVGadgetView; }; - } // namespace Core #endif // UAVGADGETMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.cpp index ad56261a6..79f177943 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.cpp @@ -62,17 +62,16 @@ using namespace Core; using namespace Core::Internal; UAVGadgetView::UAVGadgetView(Core::UAVGadgetManager *uavGadgetManager, IUAVGadget *uavGadget, QWidget *parent) : - QWidget(parent), - m_uavGadgetManager(uavGadgetManager), - m_uavGadget(uavGadget), - m_toolBar(new QWidget(this)), - m_defaultToolBar(new QComboBox(this)), - m_uavGadgetList(new QComboBox(this)), - m_closeButton(new QToolButton(this)), - m_defaultIndex(0), - m_activeLabel(new QLabel) + QWidget(parent), + m_uavGadgetManager(uavGadgetManager), + m_uavGadget(uavGadget), + m_toolBar(new QWidget(this)), + m_defaultToolBar(new QComboBox(this)), + m_uavGadgetList(new QComboBox(this)), + m_closeButton(new QToolButton(this)), + m_defaultIndex(0), + m_activeLabel(new QLabel) { - tl = new QVBoxLayout(this); tl->setSpacing(0); tl->setMargin(0); @@ -82,24 +81,22 @@ UAVGadgetView::UAVGadgetView(Core::UAVGadgetManager *uavGadgetManager, IUAVGadge m_uavGadgetList->setMaxVisibleItems(40); m_uavGadgetList->setContextMenuPolicy(Qt::CustomContextMenu); UAVGadgetInstanceManager *im = ICore::instance()->uavGadgetInstanceManager(); - QStringList sl = im->classIds(); + QStringList sl = im->classIds(); int index = 0; bool startFromOne = false; - foreach(QString classId, sl) - { + foreach(QString classId, sl) { if (classId == QString("EmptyGadget")) { m_defaultIndex = 0; - startFromOne = true; + startFromOne = true; m_uavGadgetList->insertItem(0, im->gadgetName(classId), classId); m_uavGadgetList->setItemIcon(0, im->gadgetIcon(classId)); m_uavGadgetList->insertSeparator(1); } else { - int i = startFromOne ? 1 : 0; - for ( ; i < m_uavGadgetList->count(); i++) - { - if (QString::localeAwareCompare(m_uavGadgetList->itemText(i), im->gadgetName(classId)) > 0) + for (; i < m_uavGadgetList->count(); i++) { + if (QString::localeAwareCompare(m_uavGadgetList->itemText(i), im->gadgetName(classId)) > 0) { break; + } } m_uavGadgetList->insertItem(i, im->gadgetName(classId), classId); m_uavGadgetList->setItemIcon(i, im->gadgetIcon(classId)); @@ -141,7 +138,7 @@ UAVGadgetView::UAVGadgetView(Core::UAVGadgetManager *uavGadgetManager, IUAVGadge connect(m_uavGadgetList, SIGNAL(activated(int)), this, SLOT(listSelectionActivated(int))); connect(m_closeButton, SIGNAL(clicked()), this, SLOT(closeView()), Qt::QueuedConnection); - connect(m_uavGadgetManager, SIGNAL(currentGadgetChanged(IUAVGadget*)), this, SLOT(currentGadgetChanged(IUAVGadget*))); + connect(m_uavGadgetManager, SIGNAL(currentGadgetChanged(IUAVGadget *)), this, SLOT(currentGadgetChanged(IUAVGadget *))); } if (m_uavGadget) { setGadget(m_uavGadget); @@ -157,7 +154,7 @@ UAVGadgetView::~UAVGadgetView() bool UAVGadgetView::hasGadget(IUAVGadget *uavGadget) const { - return (m_uavGadget == uavGadget); + return m_uavGadget == uavGadget; } void UAVGadgetView::showToolbar(bool show) @@ -172,8 +169,9 @@ void UAVGadgetView::closeView() void UAVGadgetView::removeGadget() { - if (!m_uavGadget) + if (!m_uavGadget) { return; + } tl->removeWidget(m_uavGadget->widget()); m_uavGadget->setParent(0); @@ -214,15 +212,19 @@ void UAVGadgetView::setGadget(IUAVGadget *uavGadget) void UAVGadgetView::updateToolBar() { - if (!m_uavGadget) + if (!m_uavGadget) { return; + } QComboBox *toolBar = m_uavGadget->toolBar(); - if (!toolBar) + if (!toolBar) { toolBar = m_defaultToolBar; - if (m_activeToolBar == toolBar) + } + if (m_activeToolBar == toolBar) { return; - if (toolBar->count() == 0) + } + if (toolBar->count() == 0) { toolBar->hide(); + } m_toolBar->layout()->addWidget(toolBar); m_activeToolBar->setVisible(false); m_activeToolBar = toolBar; @@ -230,17 +232,19 @@ void UAVGadgetView::updateToolBar() void UAVGadgetView::listSelectionActivated(int index) { - if (index < 0 || index >= m_uavGadgetList->count()) + if (index < 0 || index >= m_uavGadgetList->count()) { index = m_defaultIndex; + } QString classId = m_uavGadgetList->itemData(index).toString(); - if (m_uavGadget && (m_uavGadget->classId() == classId)) + if (m_uavGadget && (m_uavGadget->classId() == classId)) { return; + } UAVGadgetInstanceManager *im = ICore::instance()->uavGadgetInstanceManager(); IUAVGadget *gadget = im->createGadget(classId, this); - IUAVGadget *gadgetToRemove = m_uavGadget; + IUAVGadget *gadgetToRemove = m_uavGadget; setGadget(gadget); m_uavGadgetManager->setCurrentGadget(gadget); im->removeGadget(gadgetToRemove); @@ -256,7 +260,7 @@ void UAVGadgetView::currentGadgetChanged(IUAVGadget *gadget) m_activeLabel->setVisible(m_uavGadget == gadget); } -void UAVGadgetView::saveState(QSettings* qSettings) +void UAVGadgetView::saveState(QSettings *qSettings) { qSettings->setValue("type", "uavGadget"); qSettings->setValue("classId", gadget()->classId()); @@ -265,10 +269,11 @@ void UAVGadgetView::saveState(QSettings* qSettings) qSettings->endGroup(); } -void UAVGadgetView::restoreState(QSettings* qSettings) +void UAVGadgetView::restoreState(QSettings *qSettings) { QString classId = qSettings->value("classId").toString(); int index = indexOfClassId(classId); + if (index < 0) { index = m_defaultIndex; } @@ -276,13 +281,12 @@ void UAVGadgetView::restoreState(QSettings* qSettings) IUAVGadget *newGadget; UAVGadgetInstanceManager *im = ICore::instance()->uavGadgetInstanceManager(); - if(qSettings->childGroups().contains("gadget")) { + if (qSettings->childGroups().contains("gadget")) { newGadget = im->createGadget(classId, this, false); qSettings->beginGroup("gadget"); newGadget->restoreState(qSettings); qSettings->endGroup(); - } - else { + } else { newGadget = im->createGadget(classId, this); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.h b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.h index dfd52adcf..37e7362c2 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.h @@ -52,15 +52,11 @@ class StyledBar; } namespace Core { - class IUAVGadget; class UAVGadgetManager; namespace Internal { - - -class UAVGadgetView : public QWidget -{ +class UAVGadgetView : public QWidget { Q_OBJECT public: @@ -101,7 +97,6 @@ private: int m_defaultIndex; QLabel *m_activeLabel; }; - } } #endif // UAVGADGETVIEW_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.cpp index caa2fa1f5..a2a956c99 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.cpp @@ -10,18 +10,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -58,8 +58,9 @@ QWidget *UAVGadgetOptionsPageDecorator::createPage(QWidget *parent) m_page->lockCheckBox->hide(); m_page->nameLineEdit->setDisabled(true); } - if (!m_instanceManager->canDeleteConfiguration(m_config)) + if (!m_instanceManager->canDeleteConfiguration(m_config)) { m_page->deleteButton->setDisabled(true); + } m_page->lockCheckBox->hide(); // m_page->nameLineEdit->setText(m_id); @@ -106,4 +107,3 @@ void UAVGadgetOptionsPageDecorator::textEdited(QString name) m_config->setProvisionalName(name); m_instanceManager->configurationNameEdited(name); } - diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.h b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.h index e71bfc5db..f82244a0d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetoptionspagedecorator.h @@ -10,18 +10,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,20 +35,30 @@ class Ui_TopOptionsPage; namespace Core { - class IUAVGadgetConfiguration; class UAVGadgetInstanceManager; -class CORE_EXPORT UAVGadgetOptionsPageDecorator : public Core::IOptionsPage -{ -Q_OBJECT +class CORE_EXPORT UAVGadgetOptionsPageDecorator : public Core::IOptionsPage { + Q_OBJECT public: explicit UAVGadgetOptionsPageDecorator(IOptionsPage *page, IUAVGadgetConfiguration *config, bool isSingleConfigurationGadget = false, QObject *parent = 0); - QString id() const { return m_id; } - QString trName() const { return m_id; } - QString category() const { return m_category; } - QString trCategory() const { return m_categoryTr; } + QString id() const + { + return m_id; + } + QString trName() const + { + return m_id; + } + QString category() const + { + return m_category; + } + QString trCategory() const + { + return m_categoryTr; + } QWidget *createPage(QWidget *parent); void apply(); @@ -74,7 +84,6 @@ private: Ui_TopOptionsPage *m_page; }; - } // namespace Core #endif // UAVGADGETOPTIONSPAGEDECORATOR_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.cpp index d609d6e09..f544bebcc 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -31,7 +31,7 @@ using namespace Core; -UniqueIDManager* UniqueIDManager::m_instance = 0; +UniqueIDManager *UniqueIDManager::m_instance = 0; UniqueIDManager::UniqueIDManager() { @@ -51,8 +51,9 @@ bool UniqueIDManager::hasUniqueIdentifier(const QString &id) const int UniqueIDManager::uniqueIdentifier(const QString &id) { - if (hasUniqueIdentifier(id)) + if (hasUniqueIdentifier(id)) { return m_uniqueIdentifiers.value(id); + } int uid = m_uniqueIdentifiers.count() + 1; m_uniqueIdentifiers.insert(id, uid); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.h index d86f2483f..2b0738a85 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uniqueidmanager.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,14 +35,15 @@ #include namespace Core { - -class CORE_EXPORT UniqueIDManager -{ +class CORE_EXPORT UniqueIDManager { public: UniqueIDManager(); ~UniqueIDManager(); - static UniqueIDManager *instance() { return m_instance; } + static UniqueIDManager *instance() + { + return m_instance; + } bool hasUniqueIdentifier(const QString &id) const; int uniqueIdentifier(const QString &id); @@ -52,7 +53,6 @@ private: QHash m_uniqueIdentifiers; static UniqueIDManager *m_instance; }; - } // namespace Core #endif // UNIQUEIDMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.cpp index 821c7850c..2d62e4150 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -52,7 +52,7 @@ void VariableManager::insert(const QString &variable, const QString &value) void VariableManager::insertFileInfo(const QString &tag, const QFileInfo &file) { insert(tag, file.filePath()); - insert(tag + QLatin1String(":absoluteFilePath"), file.absoluteFilePath()); + insert(tag + QLatin1String(":absoluteFilePath"), file.absoluteFilePath()); insert(tag + QLatin1String(":absolutePath"), file.absolutePath()); insert(tag + QLatin1String(":baseName"), file.baseName()); insert(tag + QLatin1String(":canonicalPath"), file.canonicalPath()); @@ -100,6 +100,7 @@ bool VariableManager::remove(const QString &variable) QString VariableManager::resolve(const QString &stringWithVariables) const { QString result = stringWithVariables; + QMapIterator i(m_map); while (i.hasNext()) { i.next(); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.h b/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.h index 81ce4192c..27d028985 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/variablemanager.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,16 +40,17 @@ class QFileInfo; QT_END_NAMESPACE namespace Core { - -class CORE_EXPORT VariableManager : public QObject -{ +class CORE_EXPORT VariableManager : public QObject { Q_OBJECT public: VariableManager(QObject *parent); ~VariableManager(); - static VariableManager* instance() { return m_instance; } + static VariableManager *instance() + { + return m_instance; + } void insert(const QString &variable, const QString &value); void insertFileInfo(const QString &tag, const QFileInfo &file); @@ -64,7 +65,6 @@ private: QMap m_map; static VariableManager *m_instance; }; - } // namespace Core #endif // VARIABLEMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.cpp b/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.cpp index 01cf29feb..27129b34c 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.cpp @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -60,28 +60,26 @@ VersionDialog::VersionDialog(QWidget *parent) layout->setSizeConstraint(QLayout::SetFixedSize); #ifdef UAVO_HASH - //: This gets conditionally inserted as argument %11 into the description string. - QByteArray uavoHashArray; - QString uavoHash = QString::fromLatin1(Core::Constants::UAVOSHA1_STR); - uavoHash.chop(2); - uavoHash.remove(0,2); - uavoHash=uavoHash.trimmed(); - bool ok; - foreach(QString str,uavoHash.split(",")) - { - uavoHashArray.append(str.toInt(&ok,16)); - } - QString gcsUavoHashStr; - foreach(char i, uavoHashArray) - { - gcsUavoHashStr.append(QString::number(i,16).right(2)); - } - QString uavoHashStr = gcsUavoHashStr; + // : This gets conditionally inserted as argument %11 into the description string. + QByteArray uavoHashArray; + QString uavoHash = QString::fromLatin1(Core::Constants::UAVOSHA1_STR); + uavoHash.chop(2); + uavoHash.remove(0, 2); + uavoHash = uavoHash.trimmed(); + bool ok; + foreach(QString str, uavoHash.split(",")) { + uavoHashArray.append(str.toInt(&ok, 16)); + } + QString gcsUavoHashStr; + foreach(char i, uavoHashArray) { + gcsUavoHashStr.append(QString::number(i, 16).right(2)); + } + QString uavoHashStr = gcsUavoHashStr; #else QString uavoHashStr = "N/A"; #endif - const QString description = tr( + const QString description = tr( "

OpenPilot Ground Control Station

" "GCS Revision: %1
" "UAVO Hash: %2
" @@ -100,17 +98,17 @@ VersionDialog::VersionDialog(QWidget *parent) "The program is provided AS IS with NO WARRANTY OF ANY KIND, " "INCLUDING THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A " "PARTICULAR PURPOSE." - ).arg( + ).arg( QString::fromLatin1(GCS_REVISION_STR).left(60), // %1 - uavoHashStr, // %2 - QLatin1String(GCS_ORIGIN_STR), // $3 - QLatin1String(__DATE__), // %4 - QLatin1String(__TIME__), // %5 - QLatin1String(QT_VERSION_STR), // %6 - QString::number(QSysInfo::WordSize), // %7 - QLatin1String(GCS_AUTHOR), // %8 - QLatin1String(GCS_YEAR_STR) // %9 - ); + uavoHashStr, // %2 + QLatin1String(GCS_ORIGIN_STR), // $3 + QLatin1String(__DATE__), // %4 + QLatin1String(__TIME__), // %5 + QLatin1String(QT_VERSION_STR), // %6 + QString::number(QSysInfo::WordSize), // %7 + QLatin1String(GCS_AUTHOR), // %8 + QLatin1String(GCS_YEAR_STR) // %9 + ); QLabel *copyRightLabel = new QLabel(description); copyRightLabel->setWordWrap(true); @@ -118,14 +116,14 @@ VersionDialog::VersionDialog(QWidget *parent) copyRightLabel->setTextInteractionFlags(Qt::TextBrowserInteraction); QDialogButtonBox *buttonBox = new QDialogButtonBox(QDialogButtonBox::Close); - QPushButton *closeButton = buttonBox->button(QDialogButtonBox::Close); + QPushButton *closeButton = buttonBox->button(QDialogButtonBox::Close); QTC_ASSERT(closeButton, /**/); buttonBox->addButton(closeButton, QDialogButtonBox::ButtonRole(QDialogButtonBox::RejectRole | QDialogButtonBox::AcceptRole)); - connect(buttonBox , SIGNAL(rejected()), this, SLOT(reject())); + connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject())); QLabel *logoLabel = new QLabel; logoLabel->setPixmap(QPixmap(QLatin1String(":/core/images/openpilot_logo_128.png"))); - layout->addWidget(logoLabel , 0, 0, 1, 1); + layout->addWidget(logoLabel, 0, 0, 1, 1); layout->addWidget(copyRightLabel, 0, 1, 4, 4); layout->addWidget(buttonBox, 4, 0, 1, 5); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.h b/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.h index fa5a8a89c..16ffe3d14 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/versiondialog.h @@ -11,18 +11,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -35,14 +35,11 @@ namespace Core { namespace Internal { - -class VersionDialog : public QDialog -{ +class VersionDialog : public QDialog { Q_OBJECT public: explicit VersionDialog(QWidget *parent); }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp index 1f3b12910..62279fd1a 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp @@ -10,18 +10,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,12 +41,10 @@ const int WorkspaceSettings::MAX_WORKSPACES = 10; WorkspaceSettings::WorkspaceSettings(QObject *parent) : IOptionsPage(parent) -{ -} +{} WorkspaceSettings::~WorkspaceSettings() -{ -} +{} // IOptionsPage @@ -95,14 +93,15 @@ QWidget *WorkspaceSettings::createPage(QWidget *parent) m_currentIndex = 0; selectWorkspace(m_currentIndex); - if (0 <= m_tabBarPlacementIndex && m_tabBarPlacementIndex < m_page->comboBoxTabBarPlacement->count()) + if (0 <= m_tabBarPlacementIndex && m_tabBarPlacementIndex < m_page->comboBoxTabBarPlacement->count()) { m_page->comboBoxTabBarPlacement->setCurrentIndex(m_tabBarPlacementIndex); + } m_page->checkBoxAllowTabMovement->setChecked(m_allowTabBarMovement); return w; } -void WorkspaceSettings::readSettings(QSettings* qs) +void WorkspaceSettings::readSettings(QSettings *qs) { m_names.clear(); m_iconNames.clear(); @@ -112,31 +111,31 @@ void WorkspaceSettings::readSettings(QSettings* qs) m_numberOfWorkspaces = qs->value(QLatin1String("NumberOfWorkspaces"), 2).toInt(); m_previousNumberOfWorkspaces = m_numberOfWorkspaces; for (int i = 1; i <= MAX_WORKSPACES; ++i) { - QString numberString = QString::number(i); - QString defaultName = "Workspace" + numberString; + QString numberString = QString::number(i); + QString defaultName = "Workspace" + numberString; QString defaultIconName = "Icon" + numberString; - QString name = qs->value(defaultName, defaultName).toString(); + QString name = qs->value(defaultName, defaultName).toString(); QString iconName = qs->value(defaultIconName, ":/core/images/openpilot_logo_64.png").toString(); m_names.append(name); m_iconNames.append(iconName); - m_modeNames.append(QString("Mode")+ QString::number(i)); + m_modeNames.append(QString("Mode") + QString::number(i)); } m_tabBarPlacementIndex = qs->value(QLatin1String("TabBarPlacementIndex"), 1).toInt(); // 1 == "Bottom" - m_allowTabBarMovement = qs->value(QLatin1String("AllowTabBarMovement"), false).toBool(); + m_allowTabBarMovement = qs->value(QLatin1String("AllowTabBarMovement"), false).toBool(); qs->endGroup(); QTabWidget::TabPosition pos = m_tabBarPlacementIndex == 0 ? QTabWidget::North : QTabWidget::South; emit tabBarSettingsApplied(pos, m_allowTabBarMovement); } -void WorkspaceSettings::saveSettings(QSettings* qs) +void WorkspaceSettings::saveSettings(QSettings *qs) { qs->beginGroup(QLatin1String("Workspace")); qs->setValue(QLatin1String("NumberOfWorkspaces"), m_numberOfWorkspaces); for (int i = 0; i < MAX_WORKSPACES; ++i) { - QString mode = QString("Mode")+ QString::number(i+1); + QString mode = QString("Mode") + QString::number(i + 1); int j = m_modeNames.indexOf(mode); - QString numberString = QString::number(i+1); - QString defaultName = "Workspace" + numberString; + QString numberString = QString::number(i + 1); + QString defaultName = "Workspace" + numberString; QString defaultIconName = "Icon" + numberString; qs->setValue(defaultName, m_names.at(j)); qs->setValue(defaultIconName, m_iconNames.at(j)); @@ -157,16 +156,16 @@ void WorkspaceSettings::apply() m_previousNumberOfWorkspaces = m_numberOfWorkspaces; } - ModeManager* modeManager = Core::ICore::instance()->modeManager(); + ModeManager *modeManager = Core::ICore::instance()->modeManager(); for (int i = 0; i < MAX_WORKSPACES; ++i) { IMode *baseMode = modeManager->mode(modeName(i)); - Core::UAVGadgetManager *mode = qobject_cast(baseMode); + Core::UAVGadgetManager *mode = qobject_cast(baseMode); if (mode) { modeManager->updateModeNameIcon(mode, QIcon(iconName(i)), name(i)); } } m_tabBarPlacementIndex = m_page->comboBoxTabBarPlacement->currentIndex(); - m_allowTabBarMovement = m_page->checkBoxAllowTabMovement->isChecked(); + m_allowTabBarMovement = m_page->checkBoxAllowTabMovement->isChecked(); QTabWidget::TabPosition pos = m_tabBarPlacementIndex == 0 ? QTabWidget::North : QTabWidget::South; emit tabBarSettingsApplied(pos, m_allowTabBarMovement); } @@ -185,6 +184,7 @@ void WorkspaceSettings::textEdited(QString name) void WorkspaceSettings::iconChanged() { QString iconName = m_page->iconPathChooser->path(); + m_page->workspaceComboBox->setItemIcon(m_currentIndex, QIcon(iconName)); } @@ -196,8 +196,8 @@ void WorkspaceSettings::numberOfWorkspacesChanged(int value) for (int i = count; i < value; ++i) { m_page->workspaceComboBox->addItem(QIcon(m_iconNames.at(i)), m_names.at(i)); } - } else if (value < count){ - for (int i = count-1; i >= value; --i) { + } else if (value < count) { + for (int i = count - 1; i >= value; --i) { m_page->workspaceComboBox->removeItem(i); } } @@ -213,19 +213,19 @@ void WorkspaceSettings::selectWorkspace(int index, bool store) m_page->workspaceComboBox->setItemText(m_currentIndex, m_names.at(m_currentIndex)); } - // display current workspace + // display current workspace QString iconName = m_iconNames.at(index); m_page->iconPathChooser->setPath(iconName); m_page->nameEdit->setText(m_names.at(index)); m_currentIndex = index; } -void WorkspaceSettings::newModeOrder(QVector modes) +void WorkspaceSettings::newModeOrder(QVector modes) { QList priorities; QStringList modeNames; for (int i = 0; i < modes.count(); ++i) { - Core::UAVGadgetManager *mode = qobject_cast(modes.at(i)); + Core::UAVGadgetManager *mode = qobject_cast(modes.at(i)); if (mode) { priorities.append(mode->priority()); modeNames.append(mode->uniqueModeName()); @@ -235,12 +235,12 @@ void WorkspaceSettings::newModeOrder(QVector modes) bool swapped = false; do { swapped = false; - for (int i = 0; i < m_names.count()-1; ++i) { - int j = i+1; + for (int i = 0; i < m_names.count() - 1; ++i) { + int j = i + 1; int p = modeNames.indexOf(m_modeNames.at(i)); int q = modeNames.indexOf(m_modeNames.at(j)); - bool nonShowingMode = (p == -1 && q >=0); - bool pqBothFound = (p >= 0 && q >= 0); + bool nonShowingMode = (p == -1 && q >= 0); + bool pqBothFound = (p >= 0 && q >= 0); if (nonShowingMode || (pqBothFound && (priorities.at(q) > priorities.at(p)))) { m_names.swap(i, j); m_iconNames.swap(i, j); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h index 6ff92cad9..37d2897ae 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h @@ -10,18 +10,18 @@ * @brief The Core GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,19 +36,16 @@ class QSettings; namespace Core { - - class ModeManager; - class IMode; +class ModeManager; +class IMode; namespace Internal { - namespace Ui { - class WorkspaceSettings; +class WorkspaceSettings; } -class WorkspaceSettings : public IOptionsPage -{ -Q_OBJECT +class WorkspaceSettings : public IOptionsPage { + Q_OBJECT public: WorkspaceSettings(QObject *parent = 0); ~WorkspaceSettings(); @@ -62,12 +59,24 @@ public: QWidget *createPage(QWidget *parent); void apply(); void finish(); - void readSettings(QSettings* qs); - void saveSettings(QSettings* qs); - int numberOfWorkspaces() const { return m_numberOfWorkspaces;} - QString iconName(int i) const { return m_iconNames.at(i);} - QString name(int i) const { return m_names.at(i);} - QString modeName(int i) const { return m_modeNames.at(i);} + void readSettings(QSettings *qs); + void saveSettings(QSettings *qs); + int numberOfWorkspaces() const + { + return m_numberOfWorkspaces; + } + QString iconName(int i) const + { + return m_iconNames.at(i); + } + QString name(int i) const + { + return m_names.at(i); + } + QString modeName(int i) const + { + return m_modeNames.at(i); + } signals: void tabBarSettingsApplied(QTabWidget::TabPosition pos, bool movable); @@ -77,7 +86,7 @@ public slots: void numberOfWorkspacesChanged(int value); void textEdited(QString string); void iconChanged(); - void newModeOrder(QVector modes); + void newModeOrder(QVector modes); private: Ui::WorkspaceSettings *m_page; @@ -91,7 +100,6 @@ private: bool m_allowTabBarMovement; static const int MAX_WORKSPACES; }; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debugengine.cpp b/ground/openpilotgcs/src/plugins/debuggadget/debugengine.cpp index 48ec18525..39a0a8f70 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debugengine.cpp +++ b/ground/openpilotgcs/src/plugins/debuggadget/debugengine.cpp @@ -1,13 +1,12 @@ #include "debugengine.h" debugengine::debugengine() -{ -} +{} void debugengine::writeToStdErr(const QString &level, const QList &msgs) { - emit dbgMsgError(level,msgs); + emit dbgMsgError(level, msgs); } void debugengine::writeToStdOut(const QString &level, const QList &msgs) { - emit dbgMsg(level,msgs); + emit dbgMsg(level, msgs); } diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debugengine.h b/ground/openpilotgcs/src/plugins/debuggadget/debugengine.h index 5452f7590..1871a6611 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debugengine.h +++ b/ground/openpilotgcs/src/plugins/debuggadget/debugengine.h @@ -2,17 +2,16 @@ #define DEBUGENGINE_H #include "qxtbasicstdloggerengine.h" #include -class debugengine:public QObject,public QxtBasicSTDLoggerEngine -{ +class debugengine : public QObject, public QxtBasicSTDLoggerEngine { Q_OBJECT public: debugengine(); protected: - void writeToStdErr ( const QString & level, const QList & msgs ); - void writeToStdOut ( const QString & level, const QList & msgs ); + void writeToStdErr(const QString & level, const QList & msgs); + void writeToStdOut(const QString & level, const QList & msgs); signals: - void dbgMsgError( const QString & level, const QList & msgs ); - void dbgMsg( const QString & level, const QList & msgs ); + void dbgMsgError(const QString & level, const QList & msgs); + void dbgMsg(const QString & level, const QList & msgs); }; #endif // DEBUGENGINE_H diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.cpp b/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.cpp index 05c147c03..ea5bcb0af 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.cpp +++ b/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -28,10 +28,9 @@ #include "debuggadgetwidget.h" DebugGadget::DebugGadget(QString classId, DebugGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} DebugGadget::~DebugGadget() { diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.h b/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.h index 68773df53..ca70511f5 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.h +++ b/ground/openpilotgcs/src/plugins/debuggadget/debuggadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,26 +33,34 @@ namespace Core { class IUAVGadget; } -//class QWidget; -//class QString; +// class QWidget; +// class QString; class DebugGadgetWidget; using namespace Core; -class DebugGadget : public Core::IUAVGadget -{ +class DebugGadget : public Core::IUAVGadget { Q_OBJECT public: DebugGadget(QString classId, DebugGadgetWidget *widget, QWidget *parent = 0); ~DebugGadget(); - QList context() const { return m_context; } - QWidget *widget() { return m_widget; } - QString contextHelpId() const { return QString(); } + QList context() const + { + return m_context; + } + QWidget *widget() + { + return m_widget; + } + QString contextHelpId() const + { + return QString(); + } private: QWidget *m_widget; - QList m_context; + QList m_context; }; diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.cpp b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.cpp index f7d925b39..fcacb7a28 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,18 +30,17 @@ #include DebugGadgetFactory::DebugGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("DebugGadget"), - tr("DebugGadget"), - parent) -{ -} + IUAVGadgetFactory(QString("DebugGadget"), + tr("DebugGadget"), + parent) +{} DebugGadgetFactory::~DebugGadgetFactory() +{} + +IUAVGadget *DebugGadgetFactory::createGadget(QWidget *parent) { + DebugGadgetWidget *gadgetWidget = new DebugGadgetWidget(parent); -} - -IUAVGadget* DebugGadgetFactory::createGadget(QWidget *parent) { - DebugGadgetWidget* gadgetWidget = new DebugGadgetWidget(parent); return new DebugGadget(QString("DebugGadget"), gadgetWidget, parent); } diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.h b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.h index 186fca5f6..aea8a33ed 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,8 +37,7 @@ class IUAVGadgetFactory; using namespace Core; -class DebugGadgetFactory : public IUAVGadgetFactory -{ +class DebugGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: DebugGadgetFactory(QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.cpp b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.cpp index 34043c806..100a1c4f9 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -43,11 +43,11 @@ DebugGadgetWidget::DebugGadgetWidget(QWidget *parent) : QLabel(parent) { m_config = new Ui_Form(); m_config->setupUi(this); - debugengine * de=new debugengine(); + debugengine *de = new debugengine(); QxtLogger::getInstance()->addLoggerEngine("debugplugin", de); - connect(de,SIGNAL(dbgMsg(QString,QList)),this,SLOT(dbgMsg(QString,QList))); - connect(de,SIGNAL(dbgMsgError(QString,QList)),this,SLOT(dbgMsgError(QString,QList))); - connect(m_config->pushButton,SIGNAL(clicked()),this,SLOT(saveLog())); + connect(de, SIGNAL(dbgMsg(QString, QList)), this, SLOT(dbgMsg(QString, QList))); + connect(de, SIGNAL(dbgMsgError(QString, QList)), this, SLOT(dbgMsgError(QString, QList))); + connect(m_config->pushButton, SIGNAL(clicked()), this, SLOT(saveLog())); } DebugGadgetWidget::~DebugGadgetWidget() @@ -59,7 +59,7 @@ void DebugGadgetWidget::dbgMsg(const QString &level, const QList &msgs { m_config->plainTextEdit->setTextColor(Qt::red); - m_config->plainTextEdit->append(QString("%2[%0]%1").arg(level).arg(msgs[0].toString()).arg(QTime::currentTime().toString())); + m_config->plainTextEdit->append(QString("%2[%0]%1").arg(level).arg(msgs[0].toString()).arg(QTime::currentTime().toString())); QScrollBar *sb = m_config->plainTextEdit->verticalScrollBar(); sb->setValue(sb->maximum()); @@ -70,7 +70,7 @@ void DebugGadgetWidget::dbgMsgError(const QString &level, const QList m_config->plainTextEdit->setTextColor(Qt::black); - m_config->plainTextEdit->append(QString("%2[%0]%1").arg(level).arg(msgs[0].toString()).arg(QTime::currentTime().toString())); + m_config->plainTextEdit->append(QString("%2[%0]%1").arg(level).arg(msgs[0].toString()).arg(QTime::currentTime().toString())); QScrollBar *sb = m_config->plainTextEdit->verticalScrollBar(); sb->setValue(sb->maximum()); @@ -78,13 +78,14 @@ void DebugGadgetWidget::dbgMsgError(const QString &level, const QList void DebugGadgetWidget::saveLog() { QString fileName = QFileDialog::getSaveFileName(0, tr("Save log File As"), ""); + if (fileName.isEmpty()) { return; } QFile file(fileName); if (file.open(QIODevice::WriteOnly) && - (file.write(m_config->plainTextEdit->toHtml().toAscii()) != -1)) { + (file.write(m_config->plainTextEdit->toHtml().toAscii()) != -1)) { file.close(); } else { QMessageBox::critical(0, diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.h b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.h index ed71f73c2..37826df95 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/debuggadget/debuggadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,8 +30,7 @@ #include #include "ui_debug.h" -class DebugGadgetWidget : public QLabel -{ +class DebugGadgetWidget : public QLabel { Q_OBJECT public: @@ -41,9 +40,9 @@ public: private: Ui_Form *m_config; private slots: - void saveLog(); - void dbgMsgError( const QString & level, const QList & msgs ); - void dbgMsg( const QString & level, const QList & msgs ); + void saveLog(); + void dbgMsgError(const QString & level, const QList & msgs); + void dbgMsg(const QString & level, const QList & msgs); }; #endif /* DEBUGGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.cpp b/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.cpp index f83395ef7..149fa57b3 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.cpp +++ b/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,32 +34,31 @@ DebugPlugin::DebugPlugin() { - // Do nothing + // Do nothing } DebugPlugin::~DebugPlugin() { - // Do nothing + // Do nothing } -bool DebugPlugin::initialize(const QStringList& args, QString *errMsg) +bool DebugPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new DebugGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new DebugGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void DebugPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void DebugPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(DebugPlugin) - diff --git a/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.h b/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.h index 9cdbefd02..83115c1c9 100644 --- a/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.h +++ b/ground/openpilotgcs/src/plugins/debuggadget/debugplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup DebugGadgetPlugin Debug Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class DebugGadgetFactory; -class DebugPlugin : public ExtensionSystem::IPlugin -{ +class DebugPlugin : public ExtensionSystem::IPlugin { public: - DebugPlugin(); - ~DebugPlugin(); + DebugPlugin(); + ~DebugPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - DebugGadgetFactory *mf; + DebugGadgetFactory *mf; }; #endif /* DEBUGPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadget.cpp b/ground/openpilotgcs/src/plugins/dial/dialgadget.cpp index ea944348e..46c46d507 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadget.cpp +++ b/ground/openpilotgcs/src/plugins/dial/dialgadget.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -31,10 +31,9 @@ #include "dialgadgetconfiguration.h" DialGadget::DialGadget(QString classId, DialGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} DialGadget::~DialGadget() { @@ -42,20 +41,21 @@ DialGadget::~DialGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void DialGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void DialGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - DialGadgetConfiguration *m = qobject_cast(config); + DialGadgetConfiguration *m = qobject_cast(config); + m_widget->setDialFile(m->dialFile(), m->dialBackground(), m->dialForeground(), m->dialNeedle1(), - m->dialNeedle2(),m->dialNeedle3(),m->getN1Move(), m->getN2Move(), + m->dialNeedle2(), m->dialNeedle3(), m->getN1Move(), m->getN2Move(), m->getN3Move()); - m_widget->enableOpenGL(m->useOpenGL()); - m_widget->enableSmoothUpdates(m->getBeSmooth()); + m_widget->enableOpenGL(m->useOpenGL()); + m_widget->enableSmoothUpdates(m->getBeSmooth()); m_widget->setN1Min(m->getN1Min()); m_widget->setN1Max(m->getN1Max()); @@ -67,8 +67,8 @@ void DialGadget::loadConfiguration(IUAVGadgetConfiguration* config) m_widget->setN3Max(m->getN3Max()); m_widget->setN3Factor(m->getN3Factor()); m_widget->setDialFont(m->getFont()); - m_widget->connectNeedles(m->getN1DataObject(),m->getN1ObjField(), - m->getN2DataObject(),m->getN2ObjField(), - m->getN3DataObject(),m->getN3ObjField() + m_widget->connectNeedles(m->getN1DataObject(), m->getN1ObjField(), + m->getN2DataObject(), m->getN2ObjField(), + m->getN3DataObject(), m->getN3ObjField() ); } diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadget.h b/ground/openpilotgcs/src/plugins/dial/dialgadget.h index 6bd91cc7b..2ed4d566b 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadget.h +++ b/ground/openpilotgcs/src/plugins/dial/dialgadget.h @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -39,15 +39,17 @@ class DialGadgetWidget; using namespace Core; -class DialGadget : public Core::IUAVGadget -{ +class DialGadget : public Core::IUAVGadget { Q_OBJECT public: DialGadget(QString classId, DialGadgetWidget *widget, QWidget *parent = 0); ~DialGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: DialGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.cpp index e8c3a8443..d85e64089 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,10 +33,10 @@ * Loads a saved configuration or defaults if non exist. * */ -DialGadgetConfiguration::DialGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +DialGadgetConfiguration::DialGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_defaultDial("Unknown"), - dialBackgroundID("background"), + dialBackgroundID("background"), dialForegroundID("foreground"), dialNeedleID1("needle"), dialNeedleID2("needle2"), @@ -52,42 +52,42 @@ DialGadgetConfiguration::DialGadgetConfiguration(QString classId, QSettings* qSe needle3Factor(1), needle1Move("Rotate"), needle2Move("Rotate"), - needle3Move("Rotate"), - useOpenGLFlag(false), - beSmooth(true) + needle3Move("Rotate"), + useOpenGLFlag(false), + beSmooth(true) { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { QString dialFile = qSettings->value("dialFile").toString(); - m_defaultDial=Utils::PathUtils().InsertDataPath(dialFile); - dialBackgroundID = qSettings->value("dialBackgroundID").toString(); - dialForegroundID = qSettings->value("dialForegroundID").toString(); - dialNeedleID1 = qSettings->value("dialNeedleID1").toString(); - dialNeedleID2 = qSettings->value("dialNeedleID2").toString(); - dialNeedleID3 = qSettings->value("dialNeedleID3").toString(); - needle1MinValue = qSettings->value("needle1MinValue").toDouble(); - needle1MaxValue = qSettings->value("needle1MaxValue").toDouble(); - needle2MinValue = qSettings->value("needle2MinValue").toDouble(); - needle2MaxValue = qSettings->value("needle2MaxValue").toDouble(); - needle3MinValue = qSettings->value("needle3MinValue").toDouble(); - needle3MaxValue = qSettings->value("needle3MaxValue").toDouble(); - needle1DataObject = qSettings->value("needle1DataObject").toString(); + m_defaultDial = Utils::PathUtils().InsertDataPath(dialFile); + dialBackgroundID = qSettings->value("dialBackgroundID").toString(); + dialForegroundID = qSettings->value("dialForegroundID").toString(); + dialNeedleID1 = qSettings->value("dialNeedleID1").toString(); + dialNeedleID2 = qSettings->value("dialNeedleID2").toString(); + dialNeedleID3 = qSettings->value("dialNeedleID3").toString(); + needle1MinValue = qSettings->value("needle1MinValue").toDouble(); + needle1MaxValue = qSettings->value("needle1MaxValue").toDouble(); + needle2MinValue = qSettings->value("needle2MinValue").toDouble(); + needle2MaxValue = qSettings->value("needle2MaxValue").toDouble(); + needle3MinValue = qSettings->value("needle3MinValue").toDouble(); + needle3MaxValue = qSettings->value("needle3MaxValue").toDouble(); + needle1DataObject = qSettings->value("needle1DataObject").toString(); needle1ObjectField = qSettings->value("needle1ObjectField").toString(); - needle2DataObject = qSettings->value("needle2DataObject").toString(); + needle2DataObject = qSettings->value("needle2DataObject").toString(); needle2ObjectField = qSettings->value("needle2ObjectField").toString(); - needle3DataObject = qSettings->value("needle3DataObject").toString(); + needle3DataObject = qSettings->value("needle3DataObject").toString(); needle3ObjectField = qSettings->value("needle3ObjectField").toString(); - needle1Factor = qSettings->value("needle1Factor").toDouble(); - needle2Factor = qSettings->value("needle2Factor").toDouble(); - needle3Factor = qSettings->value("needle3Factor").toDouble(); - needle1Move = qSettings->value("needle1Move").toString(); - needle2Move = qSettings->value("needle2Move").toString(); - needle3Move = qSettings->value("needle3Move").toString(); + needle1Factor = qSettings->value("needle1Factor").toDouble(); + needle2Factor = qSettings->value("needle2Factor").toDouble(); + needle3Factor = qSettings->value("needle3Factor").toDouble(); + needle1Move = qSettings->value("needle1Move").toString(); + needle2Move = qSettings->value("needle2Move").toString(); + needle3Move = qSettings->value("needle3Move").toString(); font = qSettings->value("font").toString(); - useOpenGLFlag = qSettings->value("useOpenGLFlag").toBool(); - beSmooth = qSettings->value("beSmooth").toBool(); - } + useOpenGLFlag = qSettings->value("useOpenGLFlag").toBool(); + beSmooth = qSettings->value("beSmooth").toBool(); + } } /** @@ -97,7 +97,8 @@ DialGadgetConfiguration::DialGadgetConfiguration(QString classId, QSettings* qSe IUAVGadgetConfiguration *DialGadgetConfiguration::clone() { DialGadgetConfiguration *m = new DialGadgetConfiguration(this->classId()); - m->m_defaultDial=m_defaultDial; + + m->m_defaultDial = m_defaultDial; m->setDialBackgroundID(dialBackgroundID); m->setDialForegroundID(dialForegroundID); m->setDialNeedleID1(dialNeedleID1); @@ -122,8 +123,8 @@ IUAVGadgetConfiguration *DialGadgetConfiguration::clone() m->setN2Move(needle2Move); m->setN3Move(needle3Move); m->setFont(font); - m->useOpenGLFlag = useOpenGLFlag; - m->beSmooth = beSmooth; + m->useOpenGLFlag = useOpenGLFlag; + m->beSmooth = beSmooth; return m; } @@ -132,11 +133,13 @@ IUAVGadgetConfiguration *DialGadgetConfiguration::clone() * Saves a configuration. * */ -void DialGadgetConfiguration::saveConfig(QSettings* settings) const { +void DialGadgetConfiguration::saveConfig(QSettings *settings) const +{ QString dialFile = Utils::PathUtils().RemoveDataPath(m_defaultDial); + settings->setValue("dialFile", dialFile); - settings->setValue("dialBackgroundID", dialBackgroundID); + settings->setValue("dialBackgroundID", dialBackgroundID); settings->setValue("dialForegroundID", dialForegroundID); settings->setValue("dialNeedleID1", dialNeedleID1); @@ -167,6 +170,6 @@ void DialGadgetConfiguration::saveConfig(QSettings* settings) const { settings->setValue("font", font); - settings->setValue("useOpenGLFlag", useOpenGLFlag); - settings->setValue("beSmooth", beSmooth); + settings->setValue("useOpenGLFlag", useOpenGLFlag); + settings->setValue("beSmooth", beSmooth); } diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.h index 79a3de02a..6929debf9 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetconfiguration.h @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,83 +35,244 @@ using namespace Core; /* Despite its name, this is actually a generic analog dial supporting up to two rotating "needle" indicators. - */ -class DialGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT + */ +class DialGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit DialGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit DialGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - //set dial configuration functions - void setDialFile(QString dialFile){m_defaultDial=dialFile;} - void setDialBackgroundID(QString elementID) { dialBackgroundID = elementID;} - void setDialForegroundID(QString elementID) { dialForegroundID = elementID;} - void setDialNeedleID1(QString elementID) { dialNeedleID1 = elementID;} - void setDialNeedleID2(QString elementID) { dialNeedleID2 = elementID;} - void setDialNeedleID3(QString elementID) { dialNeedleID3 = elementID;} - void setN1Min(double val) { needle1MinValue = val;} - void setN2Min(double val) { needle2MinValue = val;} - void setN3Min(double val) { needle3MinValue = val;} - void setN1Max(double val) { needle1MaxValue = val;} - void setN2Max(double val) { needle2MaxValue = val;} - void setN3Max(double val) { needle3MaxValue = val;} - void setN1Factor(double val) { needle1Factor = val;} - void setN2Factor(double val) { needle2Factor = val;} - void setN3Factor(double val) { needle3Factor = val;} - void setN1DataObject(QString text) {needle1DataObject = text; } - void setN2DataObject(QString text){ needle2DataObject = text; } - void setN3DataObject(QString text){ needle3DataObject = text; } - void setN1ObjField(QString text) { needle1ObjectField = text; } - void setN2ObjField(QString text) { needle2ObjectField = text; } - void setN3ObjField(QString text) { needle3ObjectField = text; } - void setN1Move( QString move) { needle1Move = move; } - void setN2Move( QString move) { needle2Move = move; } - void setN3Move( QString move) { needle3Move = move; } - void setFont(QString text) { font = text; } - void setUseOpenGL(bool flag) { useOpenGLFlag = flag; } - void setBeSmooth(bool flag) { beSmooth = flag;} + // set dial configuration functions + void setDialFile(QString dialFile) + { + m_defaultDial = dialFile; + } + void setDialBackgroundID(QString elementID) + { + dialBackgroundID = elementID; + } + void setDialForegroundID(QString elementID) + { + dialForegroundID = elementID; + } + void setDialNeedleID1(QString elementID) + { + dialNeedleID1 = elementID; + } + void setDialNeedleID2(QString elementID) + { + dialNeedleID2 = elementID; + } + void setDialNeedleID3(QString elementID) + { + dialNeedleID3 = elementID; + } + void setN1Min(double val) + { + needle1MinValue = val; + } + void setN2Min(double val) + { + needle2MinValue = val; + } + void setN3Min(double val) + { + needle3MinValue = val; + } + void setN1Max(double val) + { + needle1MaxValue = val; + } + void setN2Max(double val) + { + needle2MaxValue = val; + } + void setN3Max(double val) + { + needle3MaxValue = val; + } + void setN1Factor(double val) + { + needle1Factor = val; + } + void setN2Factor(double val) + { + needle2Factor = val; + } + void setN3Factor(double val) + { + needle3Factor = val; + } + void setN1DataObject(QString text) + { + needle1DataObject = text; + } + void setN2DataObject(QString text) + { + needle2DataObject = text; + } + void setN3DataObject(QString text) + { + needle3DataObject = text; + } + void setN1ObjField(QString text) + { + needle1ObjectField = text; + } + void setN2ObjField(QString text) + { + needle2ObjectField = text; + } + void setN3ObjField(QString text) + { + needle3ObjectField = text; + } + void setN1Move(QString move) + { + needle1Move = move; + } + void setN2Move(QString move) + { + needle2Move = move; + } + void setN3Move(QString move) + { + needle3Move = move; + } + void setFont(QString text) + { + font = text; + } + void setUseOpenGL(bool flag) + { + useOpenGLFlag = flag; + } + void setBeSmooth(bool flag) + { + beSmooth = flag; + } - //get dial configuration functions - QString dialFile() {return m_defaultDial;} - QString dialBackground() {return dialBackgroundID;} - QString dialForeground() {return dialForegroundID;} - QString dialNeedle1() {return dialNeedleID1;} - QString dialNeedle2() {return dialNeedleID2;} - QString dialNeedle3() {return dialNeedleID3;} - double getN1Min() { return needle1MinValue;} - double getN2Min() { return needle2MinValue;} - double getN3Min() { return needle3MinValue;} - double getN1Max() { return needle1MaxValue;} - double getN2Max() { return needle2MaxValue;} - double getN3Max() { return needle3MaxValue;} - double getN1Factor() { return needle1Factor;} - double getN2Factor() { return needle2Factor;} - double getN3Factor() { return needle3Factor;} - QString getN1DataObject() { return needle1DataObject; } - QString getN2DataObject() { return needle2DataObject; } - QString getN3DataObject() { return needle3DataObject; } - QString getN1ObjField() { return needle1ObjectField; } - QString getN2ObjField() { return needle2ObjectField; } - QString getN3ObjField() { return needle3ObjectField; } - QString getN1Move() { return needle1Move; } - QString getN2Move() { return needle2Move; } - QString getN3Move() { return needle3Move; } - QString getFont() { return font;} - bool useOpenGL() { return useOpenGLFlag; } - bool getBeSmooth() { return beSmooth; } + // get dial configuration functions + QString dialFile() + { + return m_defaultDial; + } + QString dialBackground() + { + return dialBackgroundID; + } + QString dialForeground() + { + return dialForegroundID; + } + QString dialNeedle1() + { + return dialNeedleID1; + } + QString dialNeedle2() + { + return dialNeedleID2; + } + QString dialNeedle3() + { + return dialNeedleID3; + } + double getN1Min() + { + return needle1MinValue; + } + double getN2Min() + { + return needle2MinValue; + } + double getN3Min() + { + return needle3MinValue; + } + double getN1Max() + { + return needle1MaxValue; + } + double getN2Max() + { + return needle2MaxValue; + } + double getN3Max() + { + return needle3MaxValue; + } + double getN1Factor() + { + return needle1Factor; + } + double getN2Factor() + { + return needle2Factor; + } + double getN3Factor() + { + return needle3Factor; + } + QString getN1DataObject() + { + return needle1DataObject; + } + QString getN2DataObject() + { + return needle2DataObject; + } + QString getN3DataObject() + { + return needle3DataObject; + } + QString getN1ObjField() + { + return needle1ObjectField; + } + QString getN2ObjField() + { + return needle2ObjectField; + } + QString getN3ObjField() + { + return needle3ObjectField; + } + QString getN1Move() + { + return needle1Move; + } + QString getN2Move() + { + return needle2Move; + } + QString getN3Move() + { + return needle3Move; + } + QString getFont() + { + return font; + } + bool useOpenGL() + { + return useOpenGLFlag; + } + bool getBeSmooth() + { + return beSmooth; + } - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: QString m_defaultDial; // The name of the dial's SVG source file QString dialBackgroundID; // SVG elementID of the background QString dialForegroundID; // ... of the foreground - QString dialNeedleID1; // ... and the first needle - QString dialNeedleID2; // ... and the second - QString dialNeedleID3; // ... and the third + QString dialNeedleID1; // ... and the first needle + QString dialNeedleID2; // ... and the second + QString dialNeedleID3; // ... and the third // Note: MinValue not used at the moment! double needle1MinValue; // Value corresponding to a 0 degree angle; @@ -139,8 +300,8 @@ private: QString needle2Move; QString needle3Move; - bool useOpenGLFlag; - bool beSmooth; + bool useOpenGLFlag; + bool beSmooth; }; #endif // DIALGADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.cpp index b4336cd58..3c07f11e5 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,29 +33,27 @@ #include DialGadgetFactory::DialGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("DialGadget"), - tr("Analog Dial"), - parent) -{ -} + IUAVGadgetFactory(QString("DialGadget"), + tr("Analog Dial"), + parent) +{} DialGadgetFactory::~DialGadgetFactory() -{ -} +{} -Core::IUAVGadget* DialGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *DialGadgetFactory::createGadget(QWidget *parent) { - DialGadgetWidget* gadgetWidget = new DialGadgetWidget(parent); + DialGadgetWidget *gadgetWidget = new DialGadgetWidget(parent); + return new DialGadget(QString("DialGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *DialGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *DialGadgetFactory::createConfiguration(QSettings *qSettings) { return new DialGadgetConfiguration(QString("DialGadget"), qSettings); } IOptionsPage *DialGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new DialGadgetOptionsPage(qobject_cast(config)); + return new DialGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.h b/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.h index 17aed6262..e4c51c4c8 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetfactory.h @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,15 +38,14 @@ class IUAVGadgetFactory; using namespace Core; -class DialGadgetFactory : public IUAVGadgetFactory -{ +class DialGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: DialGadgetFactory(QObject *parent = 0); ~DialGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.cpp index 1afd69305..292629f69 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -39,28 +39,26 @@ #include DialGadgetOptionsPage::DialGadgetOptionsPage(DialGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *DialGadgetOptionsPage::createPage(QWidget *parent) { - Q_UNUSED(parent); options_page = new Ui::DialGadgetOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget; - //main layout + // main layout options_page->setupUi(optionsPageWidget); // Fills the combo boxes for the UAVObjects ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - QList< QList > objList = objManager->getDataObjects(); - foreach (QList list, objList) { - foreach (UAVDataObject* obj, list) { + QList< QList > objList = objManager->getDataObjects(); + foreach(QList list, objList) { + foreach(UAVDataObject * obj, list) { options_page->uavObject1->addItem(obj->getName()); options_page->uavObject2->addItem(obj->getName()); options_page->uavObject3->addItem(obj->getName()); @@ -104,41 +102,41 @@ QWidget *DialGadgetOptionsPage::createPage(QWidget *parent) options_page->moveNeedle2->setCurrentIndex(options_page->moveNeedle2->findText(m_config->getN2Move())); options_page->moveNeedle3->setCurrentIndex(options_page->moveNeedle3->findText(m_config->getN3Move())); - options_page->useOpenGL->setChecked(m_config->useOpenGL()); - options_page->smoothUpdates->setChecked(m_config->getBeSmooth()); + options_page->useOpenGL->setChecked(m_config->useOpenGL()); + options_page->smoothUpdates->setChecked(m_config->getBeSmooth()); - //select saved UAV Object field values - if(options_page->uavObject1->findText(m_config->getN1DataObject())!=-1){ + // select saved UAV Object field values + if (options_page->uavObject1->findText(m_config->getN1DataObject()) != -1) { options_page->uavObject1->setCurrentIndex(options_page->uavObject1->findText(m_config->getN1DataObject())); // Now load the object field values - 1st check that the object saved in the config still exists - UAVDataObject* obj = dynamic_cast( objManager->getObject(m_config->getN1DataObject()) ); - if (obj != NULL ) { - on_uavObject1_currentIndexChanged(m_config->getN1DataObject()); - // And set the highlighed value from the settings: - options_page->objectField1->setCurrentIndex(options_page->objectField1->findText(m_config->getN1ObjField())); + UAVDataObject *obj = dynamic_cast(objManager->getObject(m_config->getN1DataObject())); + if (obj != NULL) { + on_uavObject1_currentIndexChanged(m_config->getN1DataObject()); + // And set the highlighed value from the settings: + options_page->objectField1->setCurrentIndex(options_page->objectField1->findText(m_config->getN1ObjField())); } } connect(options_page->uavObject1, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_uavObject1_currentIndexChanged(QString))); - if(options_page->uavObject2->findText(m_config->getN2DataObject())!=-1){ + if (options_page->uavObject2->findText(m_config->getN2DataObject()) != -1) { options_page->uavObject2->setCurrentIndex(options_page->uavObject2->findText(m_config->getN2DataObject())); // Now load the object field values: - UAVDataObject* obj = dynamic_cast( objManager->getObject(m_config->getN2DataObject())); - if (obj != NULL ) { + UAVDataObject *obj = dynamic_cast(objManager->getObject(m_config->getN2DataObject())); + if (obj != NULL) { on_uavObject2_currentIndexChanged(m_config->getN2DataObject()); options_page->objectField2->setCurrentIndex(options_page->objectField2->findText(m_config->getN2ObjField())); } } connect(options_page->uavObject2, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_uavObject2_currentIndexChanged(QString))); - if(options_page->uavObject3->findText(m_config->getN3DataObject())!=-1){ + if (options_page->uavObject3->findText(m_config->getN3DataObject()) != -1) { options_page->uavObject3->setCurrentIndex(options_page->uavObject3->findText(m_config->getN3DataObject())); // Now load the object field values: - UAVDataObject* obj = dynamic_cast( objManager->getObject(m_config->getN3DataObject())); - if (obj != NULL ) { + UAVDataObject *obj = dynamic_cast(objManager->getObject(m_config->getN3DataObject())); + if (obj != NULL) { on_uavObject3_currentIndexChanged(m_config->getN3DataObject()); - options_page->objectField3->setCurrentIndex(options_page->objectField3->findText(m_config->getN3ObjField())); + options_page->objectField3->setCurrentIndex(options_page->objectField3->findText(m_config->getN3ObjField())); } } connect(options_page->uavObject3, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_uavObject3_currentIndexChanged(QString))); @@ -157,7 +155,7 @@ QWidget *DialGadgetOptionsPage::createPage(QWidget *parent) void DialGadgetOptionsPage::apply() { m_config->setDialFile(options_page->svgSourceFile->path()); - m_config->setDialBackgroundID(options_page->backgroundID->text()); + m_config->setDialBackgroundID(options_page->backgroundID->text()); m_config->setDialForegroundID(options_page->foregroundID->text()); m_config->setDialNeedleID1(options_page->needle1ID->text()); m_config->setDialNeedleID2(options_page->needle2ID->text()); @@ -181,8 +179,8 @@ void DialGadgetOptionsPage::apply() m_config->setN2Move(options_page->moveNeedle2->currentText()); m_config->setN3Move(options_page->moveNeedle3->currentText()); m_config->setFont(font.toString()); - m_config->setUseOpenGL(options_page->useOpenGL->checkState()); - m_config->setBeSmooth(options_page->smoothUpdates->checkState()); + m_config->setUseOpenGL(options_page->useOpenGL->checkState()); + m_config->setBeSmooth(options_page->smoothUpdates->checkState()); } /** @@ -192,87 +190,86 @@ void DialGadgetOptionsPage::apply() void DialGadgetOptionsPage::on_fontPicker_clicked() { bool ok; - font = QFontDialog::getFont(&ok, QFont("Arial", 12), qobject_cast(this)); + + font = QFontDialog::getFont(&ok, QFont("Arial", 12), qobject_cast(this)); } /* - Fills in the field1 combo box when value is changed in the - object1 field -*/ -void DialGadgetOptionsPage::on_uavObject1_currentIndexChanged(QString val) { + Fills in the field1 combo box when value is changed in the + object1 field + */ +void DialGadgetOptionsPage::on_uavObject1_currentIndexChanged(QString val) +{ options_page->objectField1->clear(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(val) ); - QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { - if(field->getType() == UAVObjectField::STRING || field->getType() == UAVObjectField::ENUM ) + UAVDataObject *obj = dynamic_cast(objManager->getObject(val)); + QList fieldList = obj->getFields(); + foreach(UAVObjectField * field, fieldList) { + if (field->getType() == UAVObjectField::STRING || field->getType() == UAVObjectField::ENUM) { continue; - if(field->getElementNames().count() > 1) - { - foreach(QString elemName , field->getElementNames()) - { + } + if (field->getElementNames().count() > 1) { + foreach(QString elemName, field->getElementNames()) { options_page->objectField1->addItem(field->getName() + "-" + elemName); } - } - else + } else { options_page->objectField1->addItem(field->getName()); + } } } /* - Fills in the field2 combo box when value is changed in the - object2 field -*/ -void DialGadgetOptionsPage::on_uavObject2_currentIndexChanged(QString val) { + Fills in the field2 combo box when value is changed in the + object2 field + */ +void DialGadgetOptionsPage::on_uavObject2_currentIndexChanged(QString val) +{ options_page->objectField2->clear(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(val) ); - QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { - if(field->getType() == UAVObjectField::STRING || field->getType() == UAVObjectField::ENUM ) + UAVDataObject *obj = dynamic_cast(objManager->getObject(val)); + QList fieldList = obj->getFields(); + foreach(UAVObjectField * field, fieldList) { + if (field->getType() == UAVObjectField::STRING || field->getType() == UAVObjectField::ENUM) { continue; - if(field->getElementNames().count() > 1) - { - foreach(QString elemName , field->getElementNames()) - { + } + if (field->getElementNames().count() > 1) { + foreach(QString elemName, field->getElementNames()) { options_page->objectField2->addItem(field->getName() + "-" + elemName); } - } - else + } else { options_page->objectField2->addItem(field->getName()); + } } } /* - Fills in the field3 combo box when value is changed in the - object3 field -*/ -void DialGadgetOptionsPage::on_uavObject3_currentIndexChanged(QString val) { + Fills in the field3 combo box when value is changed in the + object3 field + */ +void DialGadgetOptionsPage::on_uavObject3_currentIndexChanged(QString val) +{ options_page->objectField3->clear(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(val) ); - QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { - if(field->getType() == UAVObjectField::STRING || field->getType() == UAVObjectField::ENUM ) + UAVDataObject *obj = dynamic_cast(objManager->getObject(val)); + QList fieldList = obj->getFields(); + foreach(UAVObjectField * field, fieldList) { + if (field->getType() == UAVObjectField::STRING || field->getType() == UAVObjectField::ENUM) { continue; - if(field->getElementNames().count() > 1) - { - foreach(QString elemName , field->getElementNames()) - { + } + if (field->getElementNames().count() > 1) { + foreach(QString elemName, field->getElementNames()) { options_page->objectField3->addItem(field->getName() + "-" + elemName); } - } - else + } else { options_page->objectField3->addItem(field->getName()); + } } } void DialGadgetOptionsPage::finish() -{ - -} +{} diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.h index 49d0d329d..1b87226f3 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetoptionspage.h @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -43,14 +43,13 @@ class IUAVGadgetConfiguration; class DialGadgetConfiguration; namespace Ui { - class DialGadgetOptionsPage; +class DialGadgetOptionsPage; } using namespace Core; -class DialGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class DialGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit DialGadgetOptionsPage(DialGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.cpp index cc9b05b5c..fd31187f3 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,16 +37,16 @@ DialGadgetWidget::DialGadgetWidget(QWidget *parent) : QGraphicsView(parent) // TODO: create a proper "needle" object instead of hardcoding all this // which is ugly (but easy). - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setScene(new QGraphicsScene(this)); setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); m_renderer = new QSvgRenderer(); - obj1 = NULL; - obj2 = NULL; - obj3 = NULL; + obj1 = NULL; + obj2 = NULL; + obj3 = NULL; m_text1 = NULL; m_text2 = NULL; m_text3 = NULL; // Should be initialized to NULL otherwise the setFont method @@ -56,8 +56,8 @@ DialGadgetWidget::DialGadgetWidget(QWidget *parent) : QGraphicsView(parent) needle2Target = 0; needle3Target = 0; -// beSmooth = true; - beSmooth = false; +// beSmooth = true; + beSmooth = false; // This timer mechanism makes needles rotate smoothly connect(&dialTimer, SIGNAL(timeout()), this, SLOT(rotateNeedles())); @@ -65,52 +65,54 @@ DialGadgetWidget::DialGadgetWidget(QWidget *parent) : QGraphicsView(parent) DialGadgetWidget::~DialGadgetWidget() { - // Do nothing + // Do nothing } /*! - \brief Enables/Disables OpenGL - */ + \brief Enables/Disables OpenGL + */ void DialGadgetWidget::enableOpenGL(bool flag) { - if (flag) - setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); - else - setViewport(new QWidget); + if (flag) { + setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); + } else { + setViewport(new QWidget); + } } /*! - \brief Connects the widget to the relevant UAVObjects - */ + \brief Connects the widget to the relevant UAVObjects + */ void DialGadgetWidget::connectNeedles(QString object1, QString nfield1, - QString object2, QString nfield2, - QString object3, QString nfield3) { - if (obj1 != NULL) - disconnect(obj1,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updateNeedle1(UAVObject*))); - if (obj2 != NULL) - disconnect(obj2,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updateNeedle2(UAVObject*))); - if (obj3 != NULL) - disconnect(obj3,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updateNeedle3(UAVObject*))); + QString object2, QString nfield2, + QString object3, QString nfield3) +{ + if (obj1 != NULL) { + disconnect(obj1, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateNeedle1(UAVObject *))); + } + if (obj2 != NULL) { + disconnect(obj2, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateNeedle2(UAVObject *))); + } + if (obj3 != NULL) { + disconnect(obj3, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateNeedle3(UAVObject *))); + } ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); // Check validity of arguments first, reject empty args and unknown fields. if (!(object1.isEmpty() || nfield1.isEmpty())) { - obj1 = dynamic_cast( objManager->getObject(object1) ); - if (obj1 != NULL ) { + obj1 = dynamic_cast(objManager->getObject(object1)); + if (obj1 != NULL) { // qDebug() << "Connected Object 1 (" << object1 << ")."; - connect(obj1, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateNeedle1(UAVObject*))); - if(nfield1.contains("-")) - { + connect(obj1, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateNeedle1(UAVObject *))); + if (nfield1.contains("-")) { QStringList fieldSubfield = nfield1.split("-", QString::SkipEmptyParts); - field1 = fieldSubfield.at(0); - subfield1 = fieldSubfield.at(1); + field1 = fieldSubfield.at(0); + subfield1 = fieldSubfield.at(1); haveSubField1 = true; - } - else - { - field1= nfield1; + } else { + field1 = nfield1; haveSubField1 = false; } } else { @@ -120,20 +122,17 @@ void DialGadgetWidget::connectNeedles(QString object1, QString nfield1, // And do the same for the second needle. if (!(object2.isEmpty() || nfield2.isEmpty())) { - obj2 = dynamic_cast( objManager->getObject(object2) ); - if (obj2 != NULL ) { + obj2 = dynamic_cast(objManager->getObject(object2)); + if (obj2 != NULL) { // qDebug() << "Connected Object 2 (" << object2 << ")."; - connect(obj2, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateNeedle2(UAVObject*))); - if(nfield2.contains("-")) - { + connect(obj2, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateNeedle2(UAVObject *))); + if (nfield2.contains("-")) { QStringList fieldSubfield = nfield2.split("-", QString::SkipEmptyParts); - field2 = fieldSubfield.at(0); - subfield2 = fieldSubfield.at(1); + field2 = fieldSubfield.at(0); + subfield2 = fieldSubfield.at(1); haveSubField2 = true; - } - else - { - field2= nfield2; + } else { + field2 = nfield2; haveSubField2 = false; } } else { @@ -143,20 +142,17 @@ void DialGadgetWidget::connectNeedles(QString object1, QString nfield1, // And do the same for the third needle. if (!(object3.isEmpty() || nfield3.isEmpty())) { - obj3 = dynamic_cast( objManager->getObject(object3) ); - if (obj3 != NULL ) { + obj3 = dynamic_cast(objManager->getObject(object3)); + if (obj3 != NULL) { // qDebug() << "Connected Object 3 (" << object3 << ")."; - connect(obj3, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateNeedle3(UAVObject*))); - if(nfield3.contains("-")) - { + connect(obj3, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateNeedle3(UAVObject *))); + if (nfield3.contains("-")) { QStringList fieldSubfield = nfield3.split("-", QString::SkipEmptyParts); - field3 = fieldSubfield.at(0); - subfield3 = fieldSubfield.at(1); + field3 = fieldSubfield.at(0); + subfield3 = fieldSubfield.at(1); haveSubField3 = true; - } - else - { - field3= nfield3; + } else { + field3 = nfield3; haveSubField3 = false; } } else { @@ -166,18 +162,21 @@ void DialGadgetWidget::connectNeedles(QString object1, QString nfield1, } /*! - \brief Called by the UAVObject which got updated - */ -void DialGadgetWidget::updateNeedle1(UAVObject *object1) { + \brief Called by the UAVObject which got updated + */ +void DialGadgetWidget::updateNeedle1(UAVObject *object1) +{ // Double check that the field exists: double value; - UAVObjectField* field = object1->getField(field1); + UAVObjectField *field = object1->getField(field1); + if (field) { - if(haveSubField1){ + if (haveSubField1) { int indexOfSubField = field->getElementNames().indexOf(QRegExp(subfield1, Qt::CaseSensitive, QRegExp::FixedString)); value = field->getDouble(indexOfSubField); - } else + } else { value = field->getDouble(); + } if (value != value) { qDebug() << "Dial widget: encountered NaN !!"; return; @@ -189,17 +188,20 @@ void DialGadgetWidget::updateNeedle1(UAVObject *object1) { } /*! - \brief Called by the UAVObject which got updated - */ -void DialGadgetWidget::updateNeedle2(UAVObject *object2) { + \brief Called by the UAVObject which got updated + */ +void DialGadgetWidget::updateNeedle2(UAVObject *object2) +{ double value; - UAVObjectField* field = object2->getField(field2); + UAVObjectField *field = object2->getField(field2); + if (field) { - if(haveSubField2){ + if (haveSubField2) { int indexOfSubField = field->getElementNames().indexOf(QRegExp(subfield2, Qt::CaseSensitive, QRegExp::FixedString)); value = field->getDouble(indexOfSubField); - } else + } else { value = field->getDouble(); + } if (value != value) { qDebug() << "Dial widget: encountered NaN !!"; return; @@ -211,17 +213,20 @@ void DialGadgetWidget::updateNeedle2(UAVObject *object2) { } /*! - \brief Called by the UAVObject which got updated - */ -void DialGadgetWidget::updateNeedle3(UAVObject *object3) { + \brief Called by the UAVObject which got updated + */ +void DialGadgetWidget::updateNeedle3(UAVObject *object3) +{ double value; - UAVObjectField* field = object3->getField(field3); + UAVObjectField *field = object3->getField(field3); + if (field) { - if(haveSubField3){ + if (haveSubField3) { int indexOfSubField = field->getElementNames().indexOf(QRegExp(subfield3, Qt::CaseSensitive, QRegExp::FixedString)); value = field->getDouble(indexOfSubField); - } else + } else { value = field->getDouble(); + } if (value != value) { qDebug() << "Dial widget: encountered NaN !!"; return; @@ -233,211 +238,207 @@ void DialGadgetWidget::updateNeedle3(UAVObject *object3) { } /* - Initializes the dial file, and does all the one-time calculations for - display later. This is the method which really initializes the dial. - */ + Initializes the dial file, and does all the one-time calculations for + display later. This is the method which really initializes the dial. + */ void DialGadgetWidget::setDialFile(QString dfn, QString bg, QString fg, QString n1, QString n2, - QString n3, QString n1Move, QString n2Move, QString n3Move) + QString n3, QString n1Move, QString n2Move, QString n3Move) { - fgenabled = false; - n2enabled = false; - n3enabled = false; - QGraphicsScene *l_scene = scene(); - setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); - if (QFile::exists(dfn) && m_renderer->load(dfn) && m_renderer->isValid()) - { - l_scene->clear(); // This also deletes all items contained in the scene. - m_background = new QGraphicsSvgItem(); - // All other items will be clipped to the shape of the background - m_background->setFlags(QGraphicsItem::ItemClipsChildrenToShape| - QGraphicsItem::ItemClipsToShape); - m_foreground = new QGraphicsSvgItem(); - m_needle1 = new QGraphicsSvgItem(); - m_needle2 = new QGraphicsSvgItem(); - m_needle3 = new QGraphicsSvgItem(); - m_needle1->setParentItem(m_background); - m_needle2->setParentItem(m_background); - m_needle3->setParentItem(m_background); - m_foreground->setParentItem(m_background); + fgenabled = false; + n2enabled = false; + n3enabled = false; + QGraphicsScene *l_scene = scene(); + setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); + if (QFile::exists(dfn) && m_renderer->load(dfn) && m_renderer->isValid()) { + l_scene->clear(); // This also deletes all items contained in the scene. + m_background = new QGraphicsSvgItem(); + // All other items will be clipped to the shape of the background + m_background->setFlags(QGraphicsItem::ItemClipsChildrenToShape | + QGraphicsItem::ItemClipsToShape); + m_foreground = new QGraphicsSvgItem(); + m_needle1 = new QGraphicsSvgItem(); + m_needle2 = new QGraphicsSvgItem(); + m_needle3 = new QGraphicsSvgItem(); + m_needle1->setParentItem(m_background); + m_needle2->setParentItem(m_background); + m_needle3->setParentItem(m_background); + m_foreground->setParentItem(m_background); - // We assume the dial contains at least the background - // and needle1 - m_background->setSharedRenderer(m_renderer); - m_background->setElementId(bg); - l_scene->addItem(m_background); + // We assume the dial contains at least the background + // and needle1 + m_background->setSharedRenderer(m_renderer); + m_background->setElementId(bg); + l_scene->addItem(m_background); - m_needle1->setSharedRenderer(m_renderer); - m_needle1->setElementId(n1); - // Note: no need to add the item explicitely because it - // is done automatically since it's a child item of the - // background. - //l_scene->addItem(m_needle1); + m_needle1->setSharedRenderer(m_renderer); + m_needle1->setElementId(n1); + // Note: no need to add the item explicitely because it + // is done automatically since it's a child item of the + // background. + // l_scene->addItem(m_needle1); - // The dial gadget allows Needle1 and Needle2 to be - // the same element, for combined movement. Needle3 - // is always independent. - if (n1 == n2) { - m_needle2 = m_needle1; - n2enabled = true; - } else { - if (m_renderer->elementExists(n2)) { - m_needle2->setSharedRenderer(m_renderer); - m_needle2->setElementId(n2); - //l_scene->addItem(m_needle2); - n2enabled = true; + // The dial gadget allows Needle1 and Needle2 to be + // the same element, for combined movement. Needle3 + // is always independent. + if (n1 == n2) { + m_needle2 = m_needle1; + n2enabled = true; + } else { + if (m_renderer->elementExists(n2)) { + m_needle2->setSharedRenderer(m_renderer); + m_needle2->setElementId(n2); + // l_scene->addItem(m_needle2); + n2enabled = true; + } } - } - if (m_renderer->elementExists(n3)) { - m_needle3->setSharedRenderer(m_renderer); - m_needle3->setElementId(n3); - //l_scene->addItem(m_needle3); - n3enabled = true; - } + if (m_renderer->elementExists(n3)) { + m_needle3->setSharedRenderer(m_renderer); + m_needle3->setElementId(n3); + // l_scene->addItem(m_needle3); + n3enabled = true; + } - if (m_renderer->elementExists(fg)) { - m_foreground->setSharedRenderer(m_renderer); - m_foreground->setElementId(fg); - // Center it on the scene + if (m_renderer->elementExists(fg)) { + m_foreground->setSharedRenderer(m_renderer); + m_foreground->setElementId(fg); + // Center it on the scene + QRectF rectB = m_background->boundingRect(); + QRectF rectF = m_foreground->boundingRect(); + m_foreground->setPos(rectB.width() / 2 - rectF.width() / 2, rectB.height() / 2 - rectF.height() / 2); + // l_scene->addItem(m_foreground); + fgenabled = true; + } + + rotateN1 = false; + horizN1 = false; + vertN1 = false; + rotateN2 = false; + horizN2 = false; + vertN2 = false; + rotateN3 = false; + horizN3 = false; + vertN3 = false; + + // Now setup the rotation/translation settings: + // this is UGLY UGLY UGLY, sorry... + if (n1Move.contains("Rotate")) { + rotateN1 = true; + } else if (n1Move.contains("Horizontal")) { + horizN1 = true; + } else if (n1Move.contains("Vertical")) { + vertN1 = true; + } + + if (n2Move.contains("Rotate")) { + rotateN2 = true; + } else if (n2Move.contains("Horizontal")) { + horizN2 = true; + } else if (n2Move.contains("Vertical")) { + vertN2 = true; + } + + if (n3Move.contains("Rotate")) { + rotateN3 = true; + } else if (n3Move.contains("Horizontal")) { + horizN3 = true; + } else if (n3Move.contains("Vertical")) { + vertN3 = true; + } + + l_scene->setSceneRect(m_background->boundingRect()); + + // Now Initialize the center for all transforms of the dial needles to the + // center of the background: + // - Move the center of the needle to the center of the background. QRectF rectB = m_background->boundingRect(); - QRectF rectF = m_foreground->boundingRect(); - m_foreground->setPos(rectB.width()/2-rectF.width()/2,rectB.height()/2-rectF.height()/2); - //l_scene->addItem(m_foreground); - fgenabled = true; - } + QRectF rectN = m_needle1->boundingRect(); + m_needle1->setPos(rectB.width() / 2 - rectN.width() / 2, rectB.height() / 2 - rectN.height() / 2); + // - Put the transform origin point of the needle at its center. + m_needle1->setTransformOriginPoint(rectN.width() / 2, rectN.height() / 2); - rotateN1 = false; - horizN1 = false; - vertN1 = false; - rotateN2 = false; - horizN2 = false; - vertN2 = false; - rotateN3 = false; - horizN3 = false; - vertN3 = false; + // Check whether the dial also wants display the numeric value: + if (m_renderer->elementExists(n1 + "-text")) { + QMatrix textMatrix = m_renderer->matrixForElement(n1 + "-text"); + qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement(n1 + "-text")).x(); + qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement(n1 + "-text")).y(); + QTransform matrix; + matrix.translate(startX, startY); + m_text1 = new QGraphicsTextItem("0.00"); + m_text1->setDefaultTextColor(QColor("White")); + m_text1->setTransform(matrix, false); + l_scene->addItem(m_text1); + } else { + m_text1 = NULL; + } - // Now setup the rotation/translation settings: - // this is UGLY UGLY UGLY, sorry... - if (n1Move.contains("Rotate")) { - rotateN1 = true; - } else if (n1Move.contains("Horizontal")) { - horizN1 = true; - } else if (n1Move.contains("Vertical")) { - vertN1 = true; - } - if (n2Move.contains("Rotate")) { - rotateN2 = true; - } else if (n2Move.contains("Horizontal")) { - horizN2 = true; - } else if (n2Move.contains("Vertical")) { - vertN2 = true; - } + if ((n1 != n2) && n2enabled) { + // Only do it for needle2 if it is not the same as n1 + rectN = m_needle2->boundingRect(); + m_needle2->setPos(rectB.width() / 2 - rectN.width() / 2, rectB.height() / 2 - rectN.height() / 2); + m_needle2->setTransformOriginPoint(rectN.width() / 2, rectN.height() / 2); + // Check whether the dial also wants display the numeric value: + if (m_renderer->elementExists(n2 + "-text")) { + QMatrix textMatrix = m_renderer->matrixForElement(n2 + "-text"); + qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement(n2 + "-text")).x(); + qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement(n2 + "-text")).y(); + QTransform matrix; + matrix.translate(startX, startY); + m_text2 = new QGraphicsTextItem("0.00"); + m_text2->setDefaultTextColor(QColor("White")); + m_text2->setTransform(matrix, false); + l_scene->addItem(m_text2); + } else { + m_text2 = NULL; + } + } + if (n3enabled) { + rectN = m_needle3->boundingRect(); + m_needle3->setPos(rectB.width() / 2 - rectN.width() / 2, rectB.height() / 2 - rectN.height() / 2); + m_needle3->setTransformOriginPoint(rectN.width() / 2, rectN.height() / 2); + // Check whether the dial also wants display the numeric value: + if (m_renderer->elementExists(n3 + "-text")) { + QMatrix textMatrix = m_renderer->matrixForElement(n3 + "-text"); + qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement(n3 + "-text")).x(); + qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement(n3 + "-text")).y(); + QTransform matrix; + matrix.translate(startX, startY); + m_text3 = new QGraphicsTextItem("0.00"); + m_text3->setDefaultTextColor(QColor("White")); + m_text3->setTransform(matrix, false); + l_scene->addItem(m_text3); + } else { + m_text3 = NULL; + } + } - if (n3Move.contains("Rotate")) { - rotateN3 = true; - } else if (n3Move.contains("Horizontal")) { - horizN3 = true; - } else if (n3Move.contains("Vertical")) { - vertN3 = true; - } - - l_scene->setSceneRect(m_background->boundingRect()); - - // Now Initialize the center for all transforms of the dial needles to the - // center of the background: - // - Move the center of the needle to the center of the background. - QRectF rectB = m_background->boundingRect(); - QRectF rectN = m_needle1->boundingRect(); - m_needle1->setPos(rectB.width()/2-rectN.width()/2,rectB.height()/2-rectN.height()/2); - // - Put the transform origin point of the needle at its center. - m_needle1->setTransformOriginPoint(rectN.width()/2,rectN.height()/2); - - // Check whether the dial also wants display the numeric value: - if (m_renderer->elementExists(n1+"-text")) { - QMatrix textMatrix = m_renderer->matrixForElement(n1+"-text"); - qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement(n1+"-text")).x(); - qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement(n1+"-text")).y(); - QTransform matrix; - matrix.translate(startX,startY); - m_text1 = new QGraphicsTextItem("0.00"); - m_text1->setDefaultTextColor(QColor("White")); - m_text1->setTransform(matrix,false); - l_scene->addItem(m_text1); + // Last: we just loaded the dial file which is by default positioned on a "zero" value + // of the needles, so we have to reset the needle values too upon dial file loading, otherwise + // we would end up with an offset whenever we change a dial file and the needle value + // is not zero at that time. + needle1Value = 0; + needle2Value = 0; + needle3Value = 0; + if (!dialTimer.isActive()) { + dialTimer.start(); + } + dialError = false; } else { - m_text1 = NULL; + qDebug() << "no file: display default background."; + m_renderer->load(QString(":/dial/images/empty.svg")); + l_scene->clear(); // This also deletes all items contained in the scene. + m_background = new QGraphicsSvgItem(); + m_background->setSharedRenderer(m_renderer); + l_scene->addItem(m_background); + m_text1 = NULL; + m_text2 = NULL; + m_text3 = NULL; + m_needle1 = NULL; + m_needle2 = NULL; + m_needle3 = NULL; + dialError = true; } - - - if ((n1 != n2) && n2enabled) { - // Only do it for needle2 if it is not the same as n1 - rectN = m_needle2->boundingRect(); - m_needle2->setPos(rectB.width()/2-rectN.width()/2,rectB.height()/2-rectN.height()/2); - m_needle2->setTransformOriginPoint(rectN.width()/2,rectN.height()/2); - // Check whether the dial also wants display the numeric value: - if (m_renderer->elementExists(n2+"-text")) { - QMatrix textMatrix = m_renderer->matrixForElement(n2+"-text"); - qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement(n2+"-text")).x(); - qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement(n2+"-text")).y(); - QTransform matrix; - matrix.translate(startX,startY); - m_text2 = new QGraphicsTextItem("0.00"); - m_text2->setDefaultTextColor(QColor("White")); - m_text2->setTransform(matrix,false); - l_scene->addItem(m_text2); - } else { - m_text2 = NULL; - } - - } - if (n3enabled) { - rectN = m_needle3->boundingRect(); - m_needle3->setPos(rectB.width()/2-rectN.width()/2,rectB.height()/2-rectN.height()/2); - m_needle3->setTransformOriginPoint(rectN.width()/2,rectN.height()/2); - // Check whether the dial also wants display the numeric value: - if (m_renderer->elementExists(n3+"-text")) { - QMatrix textMatrix = m_renderer->matrixForElement(n3+"-text"); - qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement(n3+"-text")).x(); - qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement(n3+"-text")).y(); - QTransform matrix; - matrix.translate(startX,startY); - m_text3 = new QGraphicsTextItem("0.00"); - m_text3->setDefaultTextColor(QColor("White")); - m_text3->setTransform(matrix,false); - l_scene->addItem(m_text3); - } else { - m_text3 = NULL; - } - - } - - // Last: we just loaded the dial file which is by default positioned on a "zero" value - // of the needles, so we have to reset the needle values too upon dial file loading, otherwise - // we would end up with an offset whenever we change a dial file and the needle value - // is not zero at that time. - needle1Value = 0; - needle2Value = 0; - needle3Value = 0; - if (!dialTimer.isActive()) - dialTimer.start(); - dialError = false; - } - else - { - qDebug()<<"no file: display default background."; - m_renderer->load(QString(":/dial/images/empty.svg")); - l_scene->clear(); // This also deletes all items contained in the scene. - m_background = new QGraphicsSvgItem(); - m_background->setSharedRenderer(m_renderer); - l_scene->addItem(m_background); - m_text1 = NULL; - m_text2 = NULL; - m_text3 = NULL; - m_needle1 = NULL; - m_needle2 = NULL; - m_needle3 = NULL; - dialError = true; - } } void DialGadgetWidget::paint() @@ -448,11 +449,11 @@ void DialGadgetWidget::paint() void DialGadgetWidget::paintEvent(QPaintEvent *event) { // Skip painting until the dial file is loaded - if (! m_renderer->isValid()) { - qDebug()<<"Dial file not loaded, not rendering"; + if (!m_renderer->isValid()) { + qDebug() << "Dial file not loaded, not rendering"; return; } - QGraphicsView::paintEvent(event); + QGraphicsView::paintEvent(event); } // This event enables the dial to be dynamically resized @@ -461,12 +462,13 @@ void DialGadgetWidget::paintEvent(QPaintEvent *event) void DialGadgetWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - fitInView(m_background, Qt::KeepAspectRatio ); + fitInView(m_background, Qt::KeepAspectRatio); } void DialGadgetWidget::setDialFont(QString fontProps) { - QFont font = QFont("Arial",12); + QFont font = QFont("Arial", 12); + font.fromString(fontProps); if (m_text1) { m_text1->setFont(font); @@ -476,60 +478,65 @@ void DialGadgetWidget::setDialFont(QString fontProps) // Converts the value into an angle: // this enables smooth rotation in rotateNeedles below -void DialGadgetWidget::setNeedle1(double value) { +void DialGadgetWidget::setNeedle1(double value) +{ if (rotateN1) { - needle1Target = 360*(value*n1Factor)/(n1MaxValue-n1MinValue); + needle1Target = 360 * (value * n1Factor) / (n1MaxValue - n1MinValue); } if (horizN1) { - needle1Target = (value*n1Factor)/(n1MaxValue-n1MinValue); + needle1Target = (value * n1Factor) / (n1MaxValue - n1MinValue); } if (vertN1) { - needle1Target = (value*n1Factor)/(n1MaxValue-n1MinValue); + needle1Target = (value * n1Factor) / (n1MaxValue - n1MinValue); } - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); + } if (m_text1) { QString s; - s.sprintf("%.2f",value*n1Factor); + s.sprintf("%.2f", value * n1Factor); m_text1->setPlainText(s); } } -void DialGadgetWidget::setNeedle2(double value) { +void DialGadgetWidget::setNeedle2(double value) +{ if (rotateN2) { - needle2Target = 360*(value*n2Factor)/(n2MaxValue-n2MinValue); + needle2Target = 360 * (value * n2Factor) / (n2MaxValue - n2MinValue); } if (horizN2) { - needle2Target = (value*n2Factor)/(n2MaxValue-n2MinValue); + needle2Target = (value * n2Factor) / (n2MaxValue - n2MinValue); } if (vertN2) { - needle2Target = (value*n2Factor)/(n2MaxValue-n2MinValue); + needle2Target = (value * n2Factor) / (n2MaxValue - n2MinValue); } - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); + } if (m_text2) { QString s; - s.sprintf("%.2f",value*n2Factor); + s.sprintf("%.2f", value * n2Factor); m_text2->setPlainText(s); } - } -void DialGadgetWidget::setNeedle3(double value) { +void DialGadgetWidget::setNeedle3(double value) +{ if (rotateN3) { - needle3Target = 360*(value*n3Factor)/(n3MaxValue-n3MinValue); + needle3Target = 360 * (value * n3Factor) / (n3MaxValue - n3MinValue); } if (horizN3) { - needle3Target = (value*n3Factor)/(n3MaxValue-n3MinValue); + needle3Target = (value * n3Factor) / (n3MaxValue - n3MinValue); } if (vertN3) { - needle3Target = (value*n3Factor)/(n3MaxValue-n3MinValue); + needle3Target = (value * n3Factor) / (n3MaxValue - n3MinValue); } - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); + } if (m_text3) { QString s; - s.sprintf("%.2f",value*n3Factor); + s.sprintf("%.2f", value * n3Factor); m_text3->setPlainText(s); } } @@ -551,30 +558,30 @@ void DialGadgetWidget::rotateNeedles() int dialRun = 3; if (n2enabled) { double needle2Diff; - if (abs((needle2Value-needle2Target)*10) > 5 && beSmooth) { - needle2Diff =(needle2Target - needle2Value)/5; + if (abs((needle2Value - needle2Target) * 10) > 5 && beSmooth) { + needle2Diff = (needle2Target - needle2Value) / 5; } else { needle2Diff = needle2Target - needle2Value; dialRun--; } if (rotateN2) { - m_needle2->setRotation(m_needle2->rotation()+needle2Diff); + m_needle2->setRotation(m_needle2->rotation() + needle2Diff); } else { - QPointF opd = QPointF(0,0); - if (horizN2) { - opd = QPointF(needle2Diff,0); - } - if (vertN2) { - opd = QPointF(0,needle2Diff); - } - m_needle2->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), true); - // Since we have moved the needle, we need to move - // the transform origin point the opposite way - // so that it keeps rotating from the same point. - // (this is only useful if needle1 and needle2 are the - // same object, for combined movement such as attitude indicator). - QPointF oop = m_needle2->transformOriginPoint(); - m_needle2->setTransformOriginPoint(oop.x()-opd.x(),oop.y()-opd.y()); + QPointF opd = QPointF(0, 0); + if (horizN2) { + opd = QPointF(needle2Diff, 0); + } + if (vertN2) { + opd = QPointF(0, needle2Diff); + } + m_needle2->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), true); + // Since we have moved the needle, we need to move + // the transform origin point the opposite way + // so that it keeps rotating from the same point. + // (this is only useful if needle1 and needle2 are the + // same object, for combined movement such as attitude indicator). + QPointF oop = m_needle2->transformOriginPoint(); + m_needle2->setTransformOriginPoint(oop.x() - opd.x(), oop.y() - opd.y()); } needle2Value += needle2Diff; } else { @@ -583,56 +590,58 @@ void DialGadgetWidget::rotateNeedles() // We assume that needle1 always exists! double needle1Diff; - if ((abs((needle1Value-needle1Target)*10) > 5) && beSmooth) { - needle1Diff = (needle1Target - needle1Value)/5; + if ((abs((needle1Value - needle1Target) * 10) > 5) && beSmooth) { + needle1Diff = (needle1Target - needle1Value) / 5; } else { needle1Diff = needle1Target - needle1Value; dialRun--; } if (rotateN1) { - m_needle1->setRotation(m_needle1->rotation()+needle1Diff); + m_needle1->setRotation(m_needle1->rotation() + needle1Diff); } else { - QPointF opd = QPointF(0,0); + QPointF opd = QPointF(0, 0); if (horizN1) { - opd = QPointF(needle1Diff,0); + opd = QPointF(needle1Diff, 0); } if (vertN1) { - opd = QPointF(0,needle1Diff); + opd = QPointF(0, needle1Diff); } - m_needle1->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), true); + m_needle1->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), true); QPointF oop = m_needle1->transformOriginPoint(); - m_needle1->setTransformOriginPoint((oop.x()-opd.x()),(oop.y()-opd.y())); + m_needle1->setTransformOriginPoint((oop.x() - opd.x()), (oop.y() - opd.y())); } needle1Value += needle1Diff; - if (n3enabled) { - double needle3Diff; - if ((abs((needle3Value-needle3Target)*10) > 5) && beSmooth) { - needle3Diff = (needle3Target - needle3Value)/5; - } else { - needle3Diff = needle3Target - needle3Value; - dialRun--; - } - if (rotateN3) { - m_needle3->setRotation(m_needle3->rotation()+needle3Diff); - } else { - QPointF opd = QPointF(0,0); + if (n3enabled) { + double needle3Diff; + if ((abs((needle3Value - needle3Target) * 10) > 5) && beSmooth) { + needle3Diff = (needle3Target - needle3Value) / 5; + } else { + needle3Diff = needle3Target - needle3Value; + dialRun--; + } + if (rotateN3) { + m_needle3->setRotation(m_needle3->rotation() + needle3Diff); + } else { + QPointF opd = QPointF(0, 0); if (horizN3) { - opd = QPointF(needle3Diff,0); + opd = QPointF(needle3Diff, 0); } - if (vertN3) { - opd = QPointF(0,needle3Diff); - } - m_needle3->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), true); - QPointF oop = m_needle3->transformOriginPoint(); - m_needle3->setTransformOriginPoint((oop.x()-opd.x()),(oop.y()-opd.y())); - } - needle3Value += needle3Diff; + if (vertN3) { + opd = QPointF(0, needle3Diff); + } + m_needle3->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), true); + QPointF oop = m_needle3->transformOriginPoint(); + m_needle3->setTransformOriginPoint((oop.x() - opd.x()), (oop.y() - opd.y())); + } + needle3Value += needle3Diff; } else { dialRun--; } // Now check: if dialRun is now zero, we should // just stop the timer since all needles have finished moving - if (!dialRun) dialTimer.stop(); + if (!dialRun) { + dialTimer.stop(); + } } diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.h b/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.h index 3fbc0415c..4bfddd747 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.h @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -40,114 +40,143 @@ #include #include -class DialGadgetWidget : public QGraphicsView -{ +class DialGadgetWidget : public QGraphicsView { Q_OBJECT public: DialGadgetWidget(QWidget *parent = 0); - ~DialGadgetWidget(); - void enableOpenGL(bool flag); - void enableSmoothUpdates(bool flag) { beSmooth = flag; } - void setDialFile(QString dfn, QString bg, QString fg, QString n1, QString n2, QString n3, - QString n1Move, QString n2Move, QString n3Move); - void paint(); + ~DialGadgetWidget(); + void enableOpenGL(bool flag); + void enableSmoothUpdates(bool flag) + { + beSmooth = flag; + } + void setDialFile(QString dfn, QString bg, QString fg, QString n1, QString n2, QString n3, + QString n1Move, QString n2Move, QString n3Move); + void paint(); // setNeedle1 and setNeedle2 use a timer to simulate // needle inertia - void setNeedle1(double value); - void setNeedle2(double value); - void setNeedle3(double value); - void setN1Min(double value) {n1MinValue = value;} - void setN1Max(double value) {n1MaxValue = value;} - void setN1Factor(double value) {n1Factor = value;} - void setN2Min(double value) {n2MinValue = value;} - void setN2Max(double value) {n2MaxValue = value;} - void setN2Factor(double value) {n2Factor = value;} - void setN3Min(double value) {n3MinValue = value;} - void setN3Max(double value) {n3MaxValue = value;} - void setN3Factor(double value) {n3Factor = value;} - // Sets up needle/UAVObject connections: - void connectNeedles(QString object1, QString field1, - QString object2, QString field2, - QString object3, QString field3); - void setDialFont(QString fontProps); + void setNeedle1(double value); + void setNeedle2(double value); + void setNeedle3(double value); + void setN1Min(double value) + { + n1MinValue = value; + } + void setN1Max(double value) + { + n1MaxValue = value; + } + void setN1Factor(double value) + { + n1Factor = value; + } + void setN2Min(double value) + { + n2MinValue = value; + } + void setN2Max(double value) + { + n2MaxValue = value; + } + void setN2Factor(double value) + { + n2Factor = value; + } + void setN3Min(double value) + { + n3MinValue = value; + } + void setN3Max(double value) + { + n3MaxValue = value; + } + void setN3Factor(double value) + { + n3Factor = value; + } + // Sets up needle/UAVObject connections: + void connectNeedles(QString object1, QString field1, + QString object2, QString field2, + QString object3, QString field3); + void setDialFont(QString fontProps); public slots: - void updateNeedle1(UAVObject *object1); // Called by the UAVObject - void updateNeedle2(UAVObject *object2); // Called by the UAVObject - void updateNeedle3(UAVObject *object3); // Called by the UAVObject + void updateNeedle1(UAVObject *object1); // Called by the UAVObject + void updateNeedle2(UAVObject *object2); // Called by the UAVObject + void updateNeedle3(UAVObject *object3); // Called by the UAVObject protected: - void paintEvent(QPaintEvent *event); - void resizeEvent(QResizeEvent *event); + void paintEvent(QPaintEvent *event); + void resizeEvent(QResizeEvent *event); private slots: - void rotateNeedles(); + void rotateNeedles(); private: - QSvgRenderer *m_renderer; - QGraphicsSvgItem *m_background; - QGraphicsSvgItem *m_foreground; - QGraphicsSvgItem *m_needle1; - QGraphicsSvgItem *m_needle2; - QGraphicsSvgItem *m_needle3; - QGraphicsTextItem *m_text1; - QGraphicsTextItem *m_text2; - QGraphicsTextItem *m_text3; + QSvgRenderer *m_renderer; + QGraphicsSvgItem *m_background; + QGraphicsSvgItem *m_foreground; + QGraphicsSvgItem *m_needle1; + QGraphicsSvgItem *m_needle2; + QGraphicsSvgItem *m_needle3; + QGraphicsTextItem *m_text1; + QGraphicsTextItem *m_text2; + QGraphicsTextItem *m_text3; - bool n3enabled; - bool n2enabled; // Simple flag to skip rendering if the - bool fgenabled; // layer does not exist. - bool dialError ; + bool n3enabled; + bool n2enabled; // Simple flag to skip rendering if the + bool fgenabled; // layer does not exist. + bool dialError; - // Settings concerning move of the dials - bool rotateN1; - bool rotateN2; - bool rotateN3; - bool horizN1; - bool horizN2; - bool horizN3; - bool vertN1; - bool vertN2; - bool vertN3; + // Settings concerning move of the dials + bool rotateN1; + bool rotateN2; + bool rotateN3; + bool horizN1; + bool horizN2; + bool horizN3; + bool vertN1; + bool vertN2; + bool vertN3; - double n1MinValue; - double n1MaxValue; - double n1Factor; - double n2MinValue; - double n2MaxValue; - double n2Factor; - double n3MinValue; - double n3MaxValue; - double n3Factor; + double n1MinValue; + double n1MaxValue; + double n1Factor; + double n2MinValue; + double n2MaxValue; + double n2Factor; + double n3MinValue; + double n3MaxValue; + double n3Factor; - // The Value and target variables - // are expressed in degrees - double needle1Target; - double needle1Value; - double needle2Target; - double needle2Value; - double needle3Target; - double needle3Value; + // The Value and target variables + // are expressed in degrees + double needle1Target; + double needle1Value; + double needle2Target; + double needle2Value; + double needle3Target; + double needle3Value; - // Name of the fields to read when an update is received: - UAVDataObject* obj1; - UAVDataObject* obj2; - UAVDataObject* obj3; - QString field1; - QString subfield1; - bool haveSubField1; - QString field2; - QString subfield2; - bool haveSubField2; - QString field3; - QString subfield3; - bool haveSubField3; + // Name of the fields to read when an update is received: + UAVDataObject *obj1; + UAVDataObject *obj2; + UAVDataObject *obj3; + QString field1; + QString subfield1; + bool haveSubField1; + QString field2; + QString subfield2; + bool haveSubField2; + QString field3; + QString subfield3; + bool haveSubField3; - // Rotation timer - QTimer dialTimer; + // Rotation timer + QTimer dialTimer; - bool beSmooth; + bool beSmooth; }; #endif /* DIALGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/dial/dialplugin.cpp b/ground/openpilotgcs/src/plugins/dial/dialplugin.cpp index c56b0f52d..4fe6d3533 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialplugin.cpp +++ b/ground/openpilotgcs/src/plugins/dial/dialplugin.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -36,32 +36,31 @@ DialPlugin::DialPlugin() { - // Do nothing + // Do nothing } DialPlugin::~DialPlugin() { - // Do nothing + // Do nothing } -bool DialPlugin::initialize(const QStringList& args, QString *errMsg) +bool DialPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new DialGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new DialGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void DialPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void DialPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(DialPlugin) - diff --git a/ground/openpilotgcs/src/plugins/dial/dialplugin.h b/ground/openpilotgcs/src/plugins/dial/dialplugin.h index 2014818b2..8d5231571 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialplugin.h +++ b/ground/openpilotgcs/src/plugins/dial/dialplugin.h @@ -8,7 +8,7 @@ * @{ * @addtogroup DialPlugin Dial Plugin * @{ - * @brief Plots flight information rotary style dials + * @brief Plots flight information rotary style dials *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,16 +33,15 @@ class DialGadgetFactory; -class DialPlugin : public ExtensionSystem::IPlugin -{ +class DialPlugin : public ExtensionSystem::IPlugin { public: - DialPlugin(); - ~DialPlugin(); + DialPlugin(); + ~DialPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - DialGadgetFactory *mf; + DialGadgetFactory *mf; }; #endif /* DIALPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/donothing/donothingplugin.cpp b/ground/openpilotgcs/src/plugins/donothing/donothingplugin.cpp index f748c6390..56172c451 100644 --- a/ground/openpilotgcs/src/plugins/donothing/donothingplugin.cpp +++ b/ground/openpilotgcs/src/plugins/donothing/donothingplugin.cpp @@ -7,52 +7,52 @@ * @{ * @addtogroup DoNothingPlugin Do Nothing Plugin * @{ - * @brief A minimal example plugin + * @brief A minimal example plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "donothingplugin.h" -#include -#include +#include "donothingplugin.h" +#include +#include -DoNothingPlugin::DoNothingPlugin() -{ - // Do nothing -} +DoNothingPlugin::DoNothingPlugin() +{ + // Do nothing +} -DoNothingPlugin::~DoNothingPlugin() -{ - // Do nothing -} +DoNothingPlugin::~DoNothingPlugin() +{ + // Do nothing +} -bool DoNothingPlugin::initialize(const QStringList& args, QString *errMsg) -{ - Q_UNUSED(args); - Q_UNUSED(errMsg); +bool DoNothingPlugin::initialize(const QStringList & args, QString *errMsg) +{ + Q_UNUSED(args); + Q_UNUSED(errMsg); - return true; -} + return true; +} -void DoNothingPlugin::extensionsInitialized() -{ - // Do nothing -} +void DoNothingPlugin::extensionsInitialized() +{ + // Do nothing +} -void DoNothingPlugin::shutdown() -{ - // Do nothing +void DoNothingPlugin::shutdown() +{ + // Do nothing } Q_EXPORT_PLUGIN(DoNothingPlugin) diff --git a/ground/openpilotgcs/src/plugins/donothing/donothingplugin.h b/ground/openpilotgcs/src/plugins/donothing/donothingplugin.h index 5505a6c1a..60ff00d89 100644 --- a/ground/openpilotgcs/src/plugins/donothing/donothingplugin.h +++ b/ground/openpilotgcs/src/plugins/donothing/donothingplugin.h @@ -7,37 +7,36 @@ * @{ * @addtogroup DoNothingPlugin Do Nothing Plugin * @{ - * @brief A minimal example plugin + * @brief A minimal example plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef DONOTHINGPLUGIN_H -#define DONOTHINGPLUGIN_H +#ifndef DONOTHINGPLUGIN_H +#define DONOTHINGPLUGIN_H -#include +#include -class DoNothingPlugin : public ExtensionSystem::IPlugin -{ -public: - DoNothingPlugin(); - ~DoNothingPlugin(); +class DoNothingPlugin : public ExtensionSystem::IPlugin { +public: + DoNothingPlugin(); + ~DoNothingPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); -}; + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); +}; -#endif // DONOTHINGPLUGIN_H \ No newline at end of file +#endif // DONOTHINGPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.cpp b/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.cpp index 8146d433b..5bbfdca2d 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.cpp +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -28,10 +28,9 @@ #include "emptygadgetwidget.h" EmptyGadget::EmptyGadget(QString classId, EmptyGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} EmptyGadget::~EmptyGadget() { diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.h b/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.h index a5b5dabb5..a9a5c0a26 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.h +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptygadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,26 +33,34 @@ namespace Core { class IUAVGadget; } -//class QWidget; -//class QString; +// class QWidget; +// class QString; class EmptyGadgetWidget; using namespace Core; -class EmptyGadget : public Core::IUAVGadget -{ +class EmptyGadget : public Core::IUAVGadget { Q_OBJECT public: EmptyGadget(QString classId, EmptyGadgetWidget *widget, QWidget *parent = 0); ~EmptyGadget(); - QList context() const { return m_context; } - QWidget *widget() { return m_widget; } - QString contextHelpId() const { return QString(); } + QList context() const + { + return m_context; + } + QWidget *widget() + { + return m_widget; + } + QString contextHelpId() const + { + return QString(); + } private: - QWidget *m_widget; - QList m_context; + QWidget *m_widget; + QList m_context; }; diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.cpp b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.cpp index 9f8db2bc9..8a5e6e1f7 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,18 +30,17 @@ #include EmptyGadgetFactory::EmptyGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("EmptyGadget"), - tr("Choose Gadget..."), - parent) -{ -} + IUAVGadgetFactory(QString("EmptyGadget"), + tr("Choose Gadget..."), + parent) +{} EmptyGadgetFactory::~EmptyGadgetFactory() +{} + +IUAVGadget *EmptyGadgetFactory::createGadget(QWidget *parent) { + EmptyGadgetWidget *gadgetWidget = new EmptyGadgetWidget(parent); -} - -IUAVGadget* EmptyGadgetFactory::createGadget(QWidget *parent) { - EmptyGadgetWidget* gadgetWidget = new EmptyGadgetWidget(parent); return new EmptyGadget(QString("EmptyGadget"), gadgetWidget, parent); } diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.h b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.h index 561d33fc6..84bd95138 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,8 +37,7 @@ class IUAVGadgetFactory; using namespace Core; -class EmptyGadgetFactory : public IUAVGadgetFactory -{ +class EmptyGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: EmptyGadgetFactory(QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.cpp b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.cpp index 48ff18dea..8df5f0601 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,7 +35,7 @@ EmptyGadgetWidget::EmptyGadgetWidget(QWidget *parent) : QLabel(parent) { - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); this-> setText(tr("Choose a gadget to display in this view.\n") + @@ -45,6 +45,5 @@ EmptyGadgetWidget::EmptyGadgetWidget(QWidget *parent) : QLabel(parent) EmptyGadgetWidget::~EmptyGadgetWidget() { - // Do nothing + // Do nothing } - diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.h b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.h index 7ee0bd326..737846899 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptygadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,13 +30,12 @@ #include -class EmptyGadgetWidget : public QLabel -{ +class EmptyGadgetWidget : public QLabel { Q_OBJECT public: EmptyGadgetWidget(QWidget *parent = 0); - ~EmptyGadgetWidget(); + ~EmptyGadgetWidget(); private: }; diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.cpp b/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.cpp index 63a00964f..23f6f8038 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.cpp +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,32 +34,31 @@ EmptyPlugin::EmptyPlugin() { - // Do nothing + // Do nothing } EmptyPlugin::~EmptyPlugin() { - // Do nothing + // Do nothing } -bool EmptyPlugin::initialize(const QStringList& args, QString *errMsg) +bool EmptyPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new EmptyGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new EmptyGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void EmptyPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void EmptyPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(EmptyPlugin) - diff --git a/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.h b/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.h index 71aea3285..4baa5e1d8 100644 --- a/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.h +++ b/ground/openpilotgcs/src/plugins/emptygadget/emptyplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class EmptyGadgetFactory; -class EmptyPlugin : public ExtensionSystem::IPlugin -{ +class EmptyPlugin : public ExtensionSystem::IPlugin { public: - EmptyPlugin(); - ~EmptyPlugin(); + EmptyPlugin(); + ~EmptyPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - EmptyGadgetFactory *mf; + EmptyGadgetFactory *mf; }; #endif /* EMPTYPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp index b8cea0d8c..ce2629bba 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp @@ -35,25 +35,24 @@ #define JOYSTICK_UPDATE_RATE 50 GCSControlGadget::GCSControlGadget(QString classId, GCSControlGadgetWidget *widget, QWidget *parent, QObject *plugin) : - IUAVGadget(classId, parent), - m_widget(widget) + IUAVGadget(classId, parent), + m_widget(widget) { - connect(getManualControlCommand(),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(manualControlCommandUpdated(UAVObject*))); - connect(widget,SIGNAL(sticksChanged(double,double,double,double)),this,SLOT(sticksChangedLocally(double,double,double,double))); - connect(this,SIGNAL(sticksChangedRemotely(double,double,double,double)),widget,SLOT(updateSticks(double,double,double,double))); + connect(getManualControlCommand(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(manualControlCommandUpdated(UAVObject *))); + connect(widget, SIGNAL(sticksChanged(double, double, double, double)), this, SLOT(sticksChangedLocally(double, double, double, double))); + connect(this, SIGNAL(sticksChangedRemotely(double, double, double, double)), widget, SLOT(updateSticks(double, double, double, double))); manualControlCommandUpdated(getManualControlCommand()); control_sock = new QUdpSocket(this); - connect(control_sock,SIGNAL(readyRead()),this,SLOT(readUDPCommand())); + connect(control_sock, SIGNAL(readyRead()), this, SLOT(readUDPCommand())); joystickTime.start(); - GCSControlPlugin *pl = dynamic_cast(plugin); - connect(pl->sdlGamepad,SIGNAL(gamepads(quint8)),this,SLOT(gamepads(quint8))); - connect(pl->sdlGamepad,SIGNAL(buttonState(ButtonNumber,bool)),this,SLOT(buttonState(ButtonNumber,bool))); - connect(pl->sdlGamepad,SIGNAL(axesValues(QListInt16)),this,SLOT(axesValues(QListInt16))); - + GCSControlPlugin *pl = dynamic_cast(plugin); + connect(pl->sdlGamepad, SIGNAL(gamepads(quint8)), this, SLOT(gamepads(quint8))); + connect(pl->sdlGamepad, SIGNAL(buttonState(ButtonNumber, bool)), this, SLOT(buttonState(ButtonNumber, bool))); + connect(pl->sdlGamepad, SIGNAL(axesValues(QListInt16)), this, SLOT(axesValues(QListInt16))); } GCSControlGadget::~GCSControlGadget() @@ -61,47 +60,48 @@ GCSControlGadget::~GCSControlGadget() delete m_widget; } -void GCSControlGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void GCSControlGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - GCSControlGadgetConfiguration *GCSControlConfig = qobject_cast< GCSControlGadgetConfiguration*>(config); + GCSControlGadgetConfiguration *GCSControlConfig = qobject_cast< GCSControlGadgetConfiguration *>(config); QList ql = GCSControlConfig->getChannelsMapping(); - rollChannel = ql.at(0); - pitchChannel = ql.at(1); - yawChannel = ql.at(2); + rollChannel = ql.at(0); + pitchChannel = ql.at(1); + yawChannel = ql.at(2); throttleChannel = ql.at(3); - // if(control_sock->isOpen()) - // control_sock->close(); - control_sock->bind(GCSControlConfig->getUDPControlHost(), GCSControlConfig->getUDPControlPort(),QUdpSocket::ShareAddress); + // if(control_sock->isOpen()) + // control_sock->close(); + control_sock->bind(GCSControlConfig->getUDPControlHost(), GCSControlConfig->getUDPControlPort(), QUdpSocket::ShareAddress); controlsMode = GCSControlConfig->getControlsMode(); int i; - for (i=0;i<8;i++) - { - buttonSettings[i].ActionID=GCSControlConfig->getbuttonSettings(i).ActionID; - buttonSettings[i].FunctionID=GCSControlConfig->getbuttonSettings(i).FunctionID; - buttonSettings[i].Amount=GCSControlConfig->getbuttonSettings(i).Amount; - buttonSettings[i].Amount=GCSControlConfig->getbuttonSettings(i).Amount; - channelReverse[i]=GCSControlConfig->getChannelsReverse().at(i); + for (i = 0; i < 8; i++) { + buttonSettings[i].ActionID = GCSControlConfig->getbuttonSettings(i).ActionID; + buttonSettings[i].FunctionID = GCSControlConfig->getbuttonSettings(i).FunctionID; + buttonSettings[i].Amount = GCSControlConfig->getbuttonSettings(i).Amount; + buttonSettings[i].Amount = GCSControlConfig->getbuttonSettings(i).Amount; + channelReverse[i] = GCSControlConfig->getChannelsReverse().at(i); } - } -ManualControlCommand* GCSControlGadget::getManualControlCommand() { +ManualControlCommand *GCSControlGadget::getManualControlCommand() +{ ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - return dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); + + return dynamic_cast(objManager->getObject(QString("ManualControlCommand"))); } -void GCSControlGadget::manualControlCommandUpdated(UAVObject * obj) { - double roll = obj->getField("Roll")->getDouble(); - double pitch = obj->getField("Pitch")->getDouble(); - double yaw = obj->getField("Yaw")->getDouble(); +void GCSControlGadget::manualControlCommandUpdated(UAVObject *obj) +{ + double roll = obj->getField("Roll")->getDouble(); + double pitch = obj->getField("Pitch")->getDouble(); + double yaw = obj->getField("Yaw")->getDouble(); double throttle = obj->getField("Throttle")->getDouble(); - // necessary against having the wrong joystick profile chosen, which shows weird values + // necessary against having the wrong joystick profile chosen, which shows weird values if (throttle > -1.0 && throttle <= 1.0) { // convert ManualControlCommand.Throttle range (0..1) to the widget's throttle stick range (-1..+1) throttle = -1.0 + (throttle * 2.0); @@ -118,31 +118,32 @@ void GCSControlGadget::manualControlCommandUpdated(UAVObject * obj) { switch (controlsMode) { case 1: // Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle - emit sticksChangedRemotely(yaw,-pitch,roll,throttle); + emit sticksChangedRemotely(yaw, -pitch, roll, throttle); break; case 2: // Mode 2: LeftX = Yaw, LeftY = Throttle, RightX = Roll, RightY = Pitch - emit sticksChangedRemotely(yaw,throttle,roll,-pitch); + emit sticksChangedRemotely(yaw, throttle, roll, -pitch); break; case 3: // Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle - emit sticksChangedRemotely(roll,-pitch,yaw,throttle); + emit sticksChangedRemotely(roll, -pitch, yaw, throttle); break; case 4: // Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch; - emit sticksChangedRemotely(roll,throttle,yaw,-pitch); + emit sticksChangedRemotely(roll, throttle, yaw, -pitch); break; } } /** - Update the manual commands - maps depending on mode - */ -void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double rightX, double rightY) { - ManualControlCommand * obj = getManualControlCommand(); - double oldRoll = obj->getField("Roll")->getDouble(); - double oldPitch = obj->getField("Pitch")->getDouble(); - double oldYaw = obj->getField("Yaw")->getDouble(); + Update the manual commands - maps depending on mode + */ +void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double rightX, double rightY) +{ + ManualControlCommand *obj = getManualControlCommand(); + double oldRoll = obj->getField("Roll")->getDouble(); + double oldPitch = obj->getField("Pitch")->getDouble(); + double oldYaw = obj->getField("Yaw")->getDouble(); double oldThrottle = obj->getField("Throttle")->getDouble(); double newRoll; @@ -154,67 +155,83 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r switch (controlsMode) { case 1: // Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle - newRoll = rightX; - newPitch = -leftY; - newYaw = leftX; + newRoll = rightX; + newPitch = -leftY; + newYaw = leftX; newThrottle = rightY; break; case 2: // Mode 2: LeftX = Yaw, LeftY = Throttle, RightX = Roll, RightY = Pitch - newRoll = rightX; - newPitch = -rightY; - newYaw = leftX; + newRoll = rightX; + newPitch = -rightY; + newYaw = leftX; newThrottle = leftY; break; case 3: // Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle - newRoll = leftX; - newPitch = -leftY; - newYaw = rightX; + newRoll = leftX; + newPitch = -leftY; + newYaw = rightX; newThrottle = rightY; break; case 4: // Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch; - newRoll = leftX; - newPitch = -rightY; - newYaw = rightX; + newRoll = leftX; + newPitch = -rightY; + newYaw = rightX; newThrottle = leftY; break; } - //check if buttons have control over this axis... if so don't update it - int buttonRollControl=0; - int buttonPitchControl=0; - int buttonYawControl=0; - int buttonThrottleControl=0; - for (int i=0;i<8;i++) - { - if ((buttonSettings[i].FunctionID==1)&&((buttonSettings[i].ActionID==1)||(buttonSettings[i].ActionID==2)))buttonRollControl=1; - if ((buttonSettings[i].FunctionID==2)&&((buttonSettings[i].ActionID==1)||(buttonSettings[i].ActionID==2)))buttonPitchControl=1; - if ((buttonSettings[i].FunctionID==3)&&((buttonSettings[i].ActionID==1)||(buttonSettings[i].ActionID==2)))buttonYawControl=1; - if ((buttonSettings[i].FunctionID==4)&&((buttonSettings[i].ActionID==1)||(buttonSettings[i].ActionID==2)))buttonThrottleControl=1; + // check if buttons have control over this axis... if so don't update it + int buttonRollControl = 0; + int buttonPitchControl = 0; + int buttonYawControl = 0; + int buttonThrottleControl = 0; + for (int i = 0; i < 8; i++) { + if ((buttonSettings[i].FunctionID == 1) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) { + buttonRollControl = 1; + } + if ((buttonSettings[i].FunctionID == 2) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) { + buttonPitchControl = 1; + } + if ((buttonSettings[i].FunctionID == 3) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) { + buttonYawControl = 1; + } + if ((buttonSettings[i].FunctionID == 4) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) { + buttonThrottleControl = 1; + } } - //if we are not in local gcs control mode, ignore the joystick input - if (((GCSControlGadgetWidget *)m_widget)->getGCSControl()==false || ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) + // if we are not in local gcs control mode, ignore the joystick input + if (((GCSControlGadgetWidget *)m_widget)->getGCSControl() == false || ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) { return; + } - //if (newThrottle != oldThrottle) { - // convert widget's throttle stick range (-1..+1) to ManualControlCommand.Throttle range (0..1) - newThrottle = (newThrottle + 1.0) / 2.0; - - // safety value to stop the motors from spinning at 0% throttle - if (newThrottle <= 0.01 ) { - newThrottle = -1; + // if (newThrottle != oldThrottle) { + // convert widget's throttle stick range (-1..+1) to ManualControlCommand.Throttle range (0..1) + newThrottle = (newThrottle + 1.0) / 2.0; + + // safety value to stop the motors from spinning at 0% throttle + if (newThrottle <= 0.01) { + newThrottle = -1; + } + // } + + + if ((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) { + if (buttonRollControl == 0) { + obj->getField("Roll")->setDouble(newRoll); + } + if (buttonPitchControl == 0) { + obj->getField("Pitch")->setDouble(newPitch); + } + if (buttonYawControl == 0) { + obj->getField("Yaw")->setDouble(newYaw); + } + if (buttonThrottleControl == 0) { + obj->getField("Throttle")->setDouble(newThrottle); } - //} - - - if((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) { - if (buttonRollControl==0)obj->getField("Roll")->setDouble(newRoll); - if (buttonPitchControl==0)obj->getField("Pitch")->setDouble(newPitch); - if (buttonYawControl==0)obj->getField("Yaw")->setDouble(newYaw); - if (buttonThrottleControl==0)obj->getField("Throttle")->setDouble(newThrottle); obj->updated(); } } @@ -223,179 +240,168 @@ void GCSControlGadget::gamepads(quint8 count) { Q_UNUSED(count); -// sdlGamepad.setGamepad(0); -// sdlGamepad.setTickRate(JOYSTICK_UPDATE_RATE); +// sdlGamepad.setGamepad(0); +// sdlGamepad.setTickRate(JOYSTICK_UPDATE_RATE); } void GCSControlGadget::readUDPCommand() { double pitch, yaw, roll, throttle; + while (control_sock->hasPendingDatagrams()) { QByteArray datagram; datagram.resize(control_sock->pendingDatagramSize()); control_sock->readDatagram(datagram.data(), datagram.size()); QDataStream readData(datagram); bool badPack = false; - int state = 0; - while(!readData.atEnd() && !badPack) - { + int state = 0; + while (!readData.atEnd() && !badPack) { double buffer; readData >> buffer; - switch(state) - { + switch (state) { case 0: - if(buffer == 42){ + if (buffer == 42) { state = 1; - }else{ - state = 0; + } else { + state = 0; badPack = true; } break; case 1: - pitch = buffer; - state = 2; + pitch = buffer; + state = 2; break; case 2: - yaw = buffer; - state = 3; + yaw = buffer; + state = 3; break; case 3: - roll = buffer; - state = 4; + roll = buffer; + state = 4; break; case 4: throttle = buffer; - state = 5; + state = 5; break; case 5: - if(buffer != 36 || !readData.atEnd()) - badPack=true; + if (buffer != 36 || !readData.atEnd()) { + badPack = true; + } break; } - } - if(!badPack && ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) - { - ManualControlCommand * obj = getManualControlCommand(); - bool update = false; + if (!badPack && ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) { + ManualControlCommand *obj = getManualControlCommand(); + bool update = false; - if(pitch != obj->getField("Pitch")->getDouble()){ - obj->getField("Pitch")->setDouble(constrain(pitch)); - update = true; - } - if(yaw != obj->getField("Yaw")->getDouble()){ - obj->getField("Yaw")->setDouble(constrain(yaw)); - update = true; - } - if(roll != obj->getField("Roll")->getDouble()){ - obj->getField("Roll")->setDouble(constrain(roll)); - update = true; - } - if(throttle != obj->getField("Throttle")->getDouble()){ + if (pitch != obj->getField("Pitch")->getDouble()) { + obj->getField("Pitch")->setDouble(constrain(pitch)); + update = true; + } + if (yaw != obj->getField("Yaw")->getDouble()) { + obj->getField("Yaw")->setDouble(constrain(yaw)); + update = true; + } + if (roll != obj->getField("Roll")->getDouble()) { + obj->getField("Roll")->setDouble(constrain(roll)); + update = true; + } + if (throttle != obj->getField("Throttle")->getDouble()) { obj->getField("Throttle")->setDouble(constrain(throttle)); update = true; - } - if(update) - obj->updated(); + } + if (update) { + obj->updated(); + } } } qDebug() << "Pitch: " << pitch << " Yaw: " << yaw << " Roll: " << roll << " Throttle: " << throttle; - - } double GCSControlGadget::constrain(double value) { - if(value < -1) + if (value < -1) { return -1; - if(value > 1) + } + if (value > 1) { return 1; + } return value; } void GCSControlGadget::buttonState(ButtonNumber number, bool pressed) { - if ((buttonSettings[number].ActionID>0)&&(buttonSettings[number].FunctionID>0)&&(pressed)) - {//this button is configured + if ((buttonSettings[number].ActionID > 0) && (buttonSettings[number].FunctionID > 0) && (pressed)) { // this button is configured ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("ManualControlCommand"))); bool currentCGSControl = ((GCSControlGadgetWidget *)m_widget)->getGCSControl(); bool currentUDPControl = ((GCSControlGadgetWidget *)m_widget)->getUDPControl(); - switch (buttonSettings[number].ActionID) - { - case 1://increase - if (currentCGSControl) - { - switch (buttonSettings[number].FunctionID) - { - case 1://Roll - obj->getField("Roll")->setValue(bound(obj->getField("Roll")->getValue().toDouble()+buttonSettings[number].Amount)); + switch (buttonSettings[number].ActionID) { + case 1: // increase + if (currentCGSControl) { + switch (buttonSettings[number].FunctionID) { + case 1: // Roll + obj->getField("Roll")->setValue(bound(obj->getField("Roll")->getValue().toDouble() + buttonSettings[number].Amount)); break; - case 2://Pitch - obj->getField("Pitch")->setValue(bound(obj->getField("Pitch")->getValue().toDouble()+buttonSettings[number].Amount)); - break; - case 3://Yaw - obj->getField("Yaw")->setValue(wrap(obj->getField("Yaw")->getValue().toDouble()+buttonSettings[number].Amount)); + case 2: // Pitch + obj->getField("Pitch")->setValue(bound(obj->getField("Pitch")->getValue().toDouble() + buttonSettings[number].Amount)); break; - case 4://Throttle - obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble()+buttonSettings[number].Amount)); + case 3: // Yaw + obj->getField("Yaw")->setValue(wrap(obj->getField("Yaw")->getValue().toDouble() + buttonSettings[number].Amount)); + break; + case 4: // Throttle + obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble() + buttonSettings[number].Amount)); break; } } break; - case 2://decrease - if (currentCGSControl) - { - switch (buttonSettings[number].FunctionID) - { - case 1://Roll - obj->getField("Roll")->setValue(bound(obj->getField("Roll")->getValue().toDouble()-buttonSettings[number].Amount)); + case 2: // decrease + if (currentCGSControl) { + switch (buttonSettings[number].FunctionID) { + case 1: // Roll + obj->getField("Roll")->setValue(bound(obj->getField("Roll")->getValue().toDouble() - buttonSettings[number].Amount)); break; - case 2://Pitch - obj->getField("Pitch")->setValue(bound(obj->getField("Pitch")->getValue().toDouble()-buttonSettings[number].Amount)); - break; - case 3://Yaw - obj->getField("Yaw")->setValue(wrap(obj->getField("Yaw")->getValue().toDouble()-buttonSettings[number].Amount)); + case 2: // Pitch + obj->getField("Pitch")->setValue(bound(obj->getField("Pitch")->getValue().toDouble() - buttonSettings[number].Amount)); break; - case 4://Throttle - obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble()-buttonSettings[number].Amount)); + case 3: // Yaw + obj->getField("Yaw")->setValue(wrap(obj->getField("Yaw")->getValue().toDouble() - buttonSettings[number].Amount)); + break; + case 4: // Throttle + obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble() - buttonSettings[number].Amount)); break; } } break; - case 3://toggle - switch (buttonSettings[number].FunctionID) - { - case 1://Armed - if (currentCGSControl) - { - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); + case 3: // toggle + switch (buttonSettings[number].FunctionID) { + case 1: // Armed + if (currentCGSControl) { + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("FlightStatus"))); - if(obj->getField("Armed")->getValue().toString().compare("Armed")==0) - { - obj->getField("Armed")->setValue("Disarmed"); - } - else - { - obj->getField("Armed")->setValue("Armed"); - } + if (obj->getField("Armed")->getValue().toString().compare("Armed") == 0) { + obj->getField("Armed")->setValue("Disarmed"); + } else { + obj->getField("Armed")->setValue("Armed"); } + } break; - case 2://GCS Control - //Toggle the GCS Control checkbox, its built in signalling will handle the update to OP + case 2: // GCS Control + // Toggle the GCS Control checkbox, its built in signalling will handle the update to OP ((GCSControlGadgetWidget *)m_widget)->setGCSControl(!currentCGSControl); - break; - case 3: //UDP Control - if(currentCGSControl) - ((GCSControlGadgetWidget *)m_widget)->setUDPControl(!currentUDPControl); + break; + case 3: // UDP Control + if (currentCGSControl) { + ((GCSControlGadgetWidget *)m_widget)->setUDPControl(!currentUDPControl); + } - break; + break; } break; @@ -403,16 +409,17 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed) obj->updated(); } - //buttonSettings[number].ActionID NIDT - //buttonSettings[number].FunctionID -RPYTAC - //buttonSettings[number].Amount + // buttonSettings[number].ActionID NIDT + // buttonSettings[number].FunctionID -RPYTAC + // buttonSettings[number].Amount } void GCSControlGadget::axesValues(QListInt16 values) { int chMax = values.length(); + if (rollChannel >= chMax || pitchChannel >= chMax || - yawChannel >= chMax || throttleChannel >= chMax ) { + yawChannel >= chMax || throttleChannel >= chMax) { qDebug() << "GCSControl: configuration is inconsistent with current joystick! Aborting update."; return; } @@ -421,15 +428,31 @@ void GCSControlGadget::axesValues(QListInt16 values) double pValue = (pitchChannel > -1) ? values[pitchChannel] : 0; double yValue = (yawChannel > -1) ? values[yawChannel] : 0; double tValue = (throttleChannel > -1) ? values[throttleChannel] : 0; - double max = 32767; + double max = 32767; - if (rollChannel > -1) if(channelReverse[rollChannel]==true)rValue = -rValue; - if (pitchChannel > -1) if(channelReverse[pitchChannel]==true)pValue = -pValue; - if (yawChannel > -1) if(channelReverse[yawChannel]==true)yValue = -yValue; - if (throttleChannel > -1) if(channelReverse[throttleChannel]==true)tValue = -tValue; + if (rollChannel > -1) { + if (channelReverse[rollChannel] == true) { + rValue = -rValue; + } + } + if (pitchChannel > -1) { + if (channelReverse[pitchChannel] == true) { + pValue = -pValue; + } + } + if (yawChannel > -1) { + if (channelReverse[yawChannel] == true) { + yValue = -yValue; + } + } + if (throttleChannel > -1) { + if (channelReverse[throttleChannel] == true) { + tValue = -tValue; + } + } - if(joystickTime.elapsed() > JOYSTICK_UPDATE_RATE) { + if (joystickTime.elapsed() > JOYSTICK_UPDATE_RATE) { joystickTime.restart(); // Remap RPYT to left X/Y and right X/Y depending on mode // Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle @@ -438,16 +461,16 @@ void GCSControlGadget::axesValues(QListInt16 values) // Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch; switch (controlsMode) { case 1: - sticksChangedLocally(yValue/max,-pValue/max,rValue/max,-tValue/max); + sticksChangedLocally(yValue / max, -pValue / max, rValue / max, -tValue / max); break; case 2: - sticksChangedLocally(yValue/max,-tValue/max,rValue/max,-pValue/max); + sticksChangedLocally(yValue / max, -tValue / max, rValue / max, -pValue / max); break; case 3: - sticksChangedLocally(rValue/max,-pValue/max,yValue/max,-tValue/max); + sticksChangedLocally(rValue / max, -pValue / max, yValue / max, -tValue / max); break; case 4: - sticksChangedLocally(rValue/max,-tValue/max,yValue/max,-pValue/max); + sticksChangedLocally(rValue / max, -tValue / max, yValue / max, -pValue / max); break; } } @@ -456,14 +479,22 @@ void GCSControlGadget::axesValues(QListInt16 values) double GCSControlGadget::bound(double input) { - if (input > 1.0)return 1.0; - if (input <-1.0)return -1.0; + if (input > 1.0) { + return 1.0; + } + if (input < -1.0) { + return -1.0; + } return input; } double GCSControlGadget::wrap(double input) { - while (input > 1.0)input -= 2.0; - while (input <-1.0)input += 2.0; + while (input > 1.0) { + input -= 2.0; + } + while (input < -1.0) { + input += 2.0; + } return input; } diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h index 14808275c..2a25bcef4 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h @@ -41,27 +41,35 @@ namespace Core { class IUAVGadget; } -//class QWidget; -//class QString; +// class QWidget; +// class QString; class GCSControlGadgetWidget; using namespace Core; -class GCSControlGadget : public Core::IUAVGadget -{ +class GCSControlGadget : public Core::IUAVGadget { Q_OBJECT public: - GCSControlGadget(QString classId, GCSControlGadgetWidget *widget, QWidget *parent = 0, QObject *plugin=0); + GCSControlGadget(QString classId, GCSControlGadgetWidget *widget, QWidget *parent = 0, QObject *plugin = 0); ~GCSControlGadget(); - QList context() const { return m_context; } - QWidget *widget() { return m_widget; } - QString contextHelpId() const { return QString(); } + QList context() const + { + return m_context; + } + QWidget *widget() + { + return m_widget; + } + QString contextHelpId() const + { + return QString(); + } - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); private: - ManualControlCommand* getManualControlCommand(); + ManualControlCommand *getManualControlCommand(); double constrain(double value); QTime joystickTime; QWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp index 517ee06c8..866b43867 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp @@ -31,7 +31,7 @@ * Loads a saved configuration or defaults if non exist. * */ -GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), rollChannel(-1), pitchChannel(-1), @@ -39,34 +39,32 @@ GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QS throttleChannel(-1) { int i; - for (i=0;i<8;i++) - { - buttonSettings[i].ActionID=0; - buttonSettings[i].FunctionID=0; - buttonSettings[i].Amount=0; + + for (i = 0; i < 8; i++) { + buttonSettings[i].ActionID = 0; + buttonSettings[i].FunctionID = 0; + buttonSettings[i].Amount = 0; channelReverse[i] = 0; } - //if a saved configuration exists load it - if(qSettings != 0) { - controlsMode = qSettings->value("controlsMode").toInt(); - rollChannel = qSettings->value("rollChannel").toInt(); - pitchChannel = qSettings->value("pitchChannel").toInt(); - yawChannel = qSettings->value("yawChannel").toInt(); + // if a saved configuration exists load it + if (qSettings != 0) { + controlsMode = qSettings->value("controlsMode").toInt(); + rollChannel = qSettings->value("rollChannel").toInt(); + pitchChannel = qSettings->value("pitchChannel").toInt(); + yawChannel = qSettings->value("yawChannel").toInt(); throttleChannel = qSettings->value("throttleChannel").toInt(); udp_port = qSettings->value("controlPortUDP").toUInt(); udp_host = QHostAddress(qSettings->value("controlHostUDP").toString()); int i; - for (i=0;i<8;i++) - { - buttonSettings[i].ActionID = qSettings->value(QString().sprintf("button%dAction",i)).toInt(); - buttonSettings[i].FunctionID = qSettings->value(QString().sprintf("button%dFunction",i)).toInt(); - buttonSettings[i].Amount = qSettings->value(QString().sprintf("button%dAmount",i)).toDouble(); - channelReverse[i] = qSettings->value(QString().sprintf("channel%dReverse",i)).toBool(); + for (i = 0; i < 8; i++) { + buttonSettings[i].ActionID = qSettings->value(QString().sprintf("button%dAction", i)).toInt(); + buttonSettings[i].FunctionID = qSettings->value(QString().sprintf("button%dFunction", i)).toInt(); + buttonSettings[i].Amount = qSettings->value(QString().sprintf("button%dAmount", i)).toDouble(); + channelReverse[i] = qSettings->value(QString().sprintf("channel%dReverse", i)).toBool(); } } - } void GCSControlGadgetConfiguration::setUDPControlSettings(int port, QString host) @@ -84,10 +82,11 @@ QHostAddress GCSControlGadgetConfiguration::getUDPControlHost() return udp_host; } -void GCSControlGadgetConfiguration::setRPYTchannels(int roll, int pitch, int yaw, int throttle) { - rollChannel = roll; - pitchChannel = pitch; - yawChannel = yaw; +void GCSControlGadgetConfiguration::setRPYTchannels(int roll, int pitch, int yaw, int throttle) +{ + rollChannel = roll; + pitchChannel = pitch; + yawChannel = yaw; throttleChannel = throttle; } @@ -101,7 +100,9 @@ QList GCSControlGadgetConfiguration::getChannelsReverse() { QList ql; int i; - for (i=0;i<8;i++)ql << channelReverse[i]; + for (i = 0; i < 8; i++) { + ql << channelReverse[i]; + } return ql; } @@ -114,21 +115,20 @@ IUAVGadgetConfiguration *GCSControlGadgetConfiguration::clone() { GCSControlGadgetConfiguration *m = new GCSControlGadgetConfiguration(this->classId()); - m->controlsMode = controlsMode; - m->rollChannel = rollChannel; - m->pitchChannel = pitchChannel; - m->yawChannel = yawChannel; + m->controlsMode = controlsMode; + m->rollChannel = rollChannel; + m->pitchChannel = pitchChannel; + m->yawChannel = yawChannel; m->throttleChannel = throttleChannel; m->udp_host = udp_host; m->udp_port = udp_port; int i; - for (i=0;i<8;i++) - { - m->buttonSettings[i].ActionID = buttonSettings[i].ActionID; + for (i = 0; i < 8; i++) { + m->buttonSettings[i].ActionID = buttonSettings[i].ActionID; m->buttonSettings[i].FunctionID = buttonSettings[i].FunctionID; - m->buttonSettings[i].Amount = buttonSettings[i].Amount; + m->buttonSettings[i].Amount = buttonSettings[i].Amount; m->channelReverse[i] = channelReverse[i]; } @@ -140,23 +140,22 @@ IUAVGadgetConfiguration *GCSControlGadgetConfiguration::clone() * Saves a configuration. * */ -void GCSControlGadgetConfiguration::saveConfig(QSettings* settings) const { +void GCSControlGadgetConfiguration::saveConfig(QSettings *settings) const +{ settings->setValue("controlsMode", controlsMode); settings->setValue("rollChannel", rollChannel); settings->setValue("pitchChannel", pitchChannel); settings->setValue("yawChannel", yawChannel); settings->setValue("throttleChannel", throttleChannel); - settings->setValue("controlPortUDP",QString::number(udp_port)); - settings->setValue("controlHostUDP",udp_host.toString()); + settings->setValue("controlPortUDP", QString::number(udp_port)); + settings->setValue("controlHostUDP", udp_host.toString()); int i; - for (i=0;i<8;i++) - { - settings->setValue(QString().sprintf("button%dAction",i), buttonSettings[i].ActionID); - settings->setValue(QString().sprintf("button%dFunction",i), buttonSettings[i].FunctionID); - settings->setValue(QString().sprintf("button%dAmount",i), buttonSettings[i].Amount); - settings->setValue(QString().sprintf("channel%dReverse",i), channelReverse[i]); + for (i = 0; i < 8; i++) { + settings->setValue(QString().sprintf("button%dAction", i), buttonSettings[i].ActionID); + settings->setValue(QString().sprintf("button%dFunction", i), buttonSettings[i].FunctionID); + settings->setValue(QString().sprintf("button%dAmount", i), buttonSettings[i].Amount); + settings->setValue(QString().sprintf("channel%dReverse", i), channelReverse[i]); } - } diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h index 7a963f7bc..66602ac35 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h @@ -31,60 +31,77 @@ #include #include -typedef struct{ - int ActionID; - int FunctionID; - double Amount; -}buttonSettingsStruct; +typedef struct { + int ActionID; + int FunctionID; + double Amount; +} buttonSettingsStruct; -typedef struct{ +typedef struct { int port; QHostAddress address; -}portSettingsStruct; +} portSettingsStruct; using namespace Core; - -class GCSControlGadgetConfiguration : public IUAVGadgetConfiguration -{ +class GCSControlGadgetConfiguration : public IUAVGadgetConfiguration { Q_OBJECT - public: - explicit GCSControlGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); +public: + explicit GCSControlGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void setControlsMode(int mode) { controlsMode = mode; } + void setControlsMode(int mode) + { + controlsMode = mode; + } void setRPYTchannels(int roll, int pitch, int yaw, int throttle); void setUDPControlSettings(int port, QString host); int getUDPControlPort(); QHostAddress getUDPControlHost(); - int getControlsMode() { return controlsMode; } + int getControlsMode() + { + return controlsMode; + } QList getChannelsMapping(); QList getChannelsReverse(); - buttonSettingsStruct getbuttonSettings(int i){return buttonSettings[i];} - void setbuttonSettingsAction(int i, int ActionID ){buttonSettings[i].ActionID=ActionID;return;} - void setbuttonSettingsFunction(int i, int FunctionID ){buttonSettings[i].FunctionID=FunctionID;return;} - void setbuttonSettingsAmount(int i, double Amount ){buttonSettings[i].Amount=Amount;return;} - void setChannelReverse(int i, bool Reverse ){channelReverse[i]=Reverse;return;} + buttonSettingsStruct getbuttonSettings(int i) + { + return buttonSettings[i]; + } + void setbuttonSettingsAction(int i, int ActionID) + { + buttonSettings[i].ActionID = ActionID; + } + void setbuttonSettingsFunction(int i, int FunctionID) + { + buttonSettings[i].FunctionID = FunctionID; + } + void setbuttonSettingsAmount(int i, double Amount) + { + buttonSettings[i].Amount = Amount; + } + void setChannelReverse(int i, bool Reverse) + { + channelReverse[i] = Reverse; + } - void saveConfig(QSettings* settings) const; - IUAVGadgetConfiguration *clone(); - - private: - int controlsMode; // Mode1 to Mode4 - // Joystick mappings for roll/pitch/yaw/throttle: - int rollChannel; - int pitchChannel; - int yawChannel; - int throttleChannel; - buttonSettingsStruct buttonSettings[8]; - bool channelReverse[8]; - int udp_port; - QHostAddress udp_host; - + void saveConfig(QSettings *settings) const; + IUAVGadgetConfiguration *clone(); +private: + int controlsMode; // Mode1 to Mode4 + // Joystick mappings for roll/pitch/yaw/throttle: + int rollChannel; + int pitchChannel; + int yawChannel; + int throttleChannel; + buttonSettingsStruct buttonSettings[8]; + bool channelReverse[8]; + int udp_port; + QHostAddress udp_host; }; #endif // GCSCONTROLGADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.cpp index f8135f4ed..bef6c191f 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.cpp @@ -30,30 +30,27 @@ #include GCSControlGadgetFactory::GCSControlGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("GCSControlGadget"), - tr("Controller"), - parent) -{ -} + IUAVGadgetFactory(QString("GCSControlGadget"), + tr("Controller"), + parent) +{} GCSControlGadgetFactory::~GCSControlGadgetFactory() +{} + +IUAVGadget *GCSControlGadgetFactory::createGadget(QWidget *parent) { + GCSControlGadgetWidget *gadgetWidget = new GCSControlGadgetWidget(parent); -} - -IUAVGadget* GCSControlGadgetFactory::createGadget(QWidget *parent) { - GCSControlGadgetWidget* gadgetWidget = new GCSControlGadgetWidget(parent); return new GCSControlGadget(QString("GCSControlGadget"), gadgetWidget, parent, this->parent()); } -IUAVGadgetConfiguration *GCSControlGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *GCSControlGadgetFactory::createConfiguration(QSettings *qSettings) { return new GCSControlGadgetConfiguration(QString("GCSControlGadget"), qSettings); } IOptionsPage *GCSControlGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new GCSControlGadgetOptionsPage(qobject_cast(config), this->parent()); + return new GCSControlGadgetOptionsPage(qobject_cast(config), this->parent()); } - - diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.h index a30e22cc8..0a13fd2f1 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetfactory.h @@ -39,17 +39,15 @@ class IUAVGadgetFactory; using namespace Core; -class GCSControlGadgetFactory : public IUAVGadgetFactory -{ +class GCSControlGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: GCSControlGadgetFactory(QObject *parent = 0); ~GCSControlGadgetFactory(); IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); - }; #endif // GCSControlGADGETFACTORY_H_ diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp index 3ca50313e..45316ce05 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp @@ -34,39 +34,32 @@ #include GCSControlGadgetOptionsPage::GCSControlGadgetOptionsPage(GCSControlGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) + IOptionsPage(parent), + m_config(config) { options_page = NULL; - sdlGamepad = dynamic_cast(parent)->sdlGamepad; - - - + sdlGamepad = dynamic_cast(parent)->sdlGamepad; } GCSControlGadgetOptionsPage::~GCSControlGadgetOptionsPage() -{ - -} +{} void GCSControlGadgetOptionsPage::buttonState(ButtonNumber number, bool pressed) { if (options_page) { - QList rbList; + QList rbList; rbList << options_page->buttonInput0 << - options_page->buttonInput1 << options_page->buttonInput2 << - options_page->buttonInput3 << options_page->buttonInput4 << - options_page->buttonInput5 << options_page->buttonInput6 << - options_page->buttonInput7; + options_page->buttonInput1 << options_page->buttonInput2 << + options_page->buttonInput3 << options_page->buttonInput4 << + options_page->buttonInput5 << options_page->buttonInput6 << + options_page->buttonInput7; - if (number<8) // We only support 8 buttons - { + if (number < 8) { // We only support 8 buttons rbList.at(number)->setChecked(pressed); } } - } void GCSControlGadgetOptionsPage::gamepads(quint8 count) @@ -74,37 +67,42 @@ void GCSControlGadgetOptionsPage::gamepads(quint8 count) Q_UNUSED(count); /*options_page->AvailableControllerList->clear(); - for (int i=0;iAvailableControllerList->addItem(QString().sprintf("%d",i));//SDL_JoystickName(i)); - }*/ - + }*/ } void GCSControlGadgetOptionsPage::axesValues(QListInt16 values) { if (options_page) { - QList pbList; + QList pbList; pbList << options_page->joyCh0 << - options_page->joyCh1 << options_page->joyCh2 << - options_page->joyCh3 << options_page->joyCh4 << - options_page->joyCh5 << options_page->joyCh6 << - options_page->joyCh7; - int i=0; - foreach (qint16 value, values) { - if (i>7) break; // We only support 7 channels - if (chRevList.at(i)->isChecked()==1)value = 65535 - value; - if (pbList.at(i)->minimum() > value) - pbList.at(i)->setMinimum(value); - if (pbList.at(i)->maximum() < value) - pbList.at(i)->setMaximum(value); + options_page->joyCh1 << options_page->joyCh2 << + options_page->joyCh3 << options_page->joyCh4 << + options_page->joyCh5 << options_page->joyCh6 << + options_page->joyCh7; + int i = 0; + foreach(qint16 value, values) { + if (i > 7) { + break; // We only support 7 channels + } + if (chRevList.at(i)->isChecked() == 1) { + value = 65535 - value; + } + if (pbList.at(i)->minimum() > value) { + pbList.at(i)->setMinimum(value); + } + if (pbList.at(i)->maximum() < value) { + pbList.at(i)->setMaximum(value); + } pbList.at(i++)->setValue(value); } } } -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *GCSControlGadgetOptionsPage::createPage(QWidget *parent) { Q_UNUSED(parent); @@ -115,106 +113,105 @@ QWidget *GCSControlGadgetOptionsPage::createPage(QWidget *parent) options_page->setupUi(optionsPageWidget); - - //QList chList; + // QList chList; chList.clear(); chList << options_page->channel0 << options_page->channel1 << - options_page->channel2 << options_page->channel3 << - options_page->channel4 << options_page->channel5 << - options_page->channel6 << options_page->channel7; + options_page->channel2 << options_page->channel3 << + options_page->channel4 << options_page->channel5 << + options_page->channel6 << options_page->channel7; QStringList chOptions; chOptions << "None" << "Roll" << "Pitch" << "Yaw" << "Throttle"; - foreach (QComboBox* qb, chList) { + foreach(QComboBox * qb, chList) { qb->addItems(chOptions); } - //QList chRevList; + // QList chRevList; chRevList.clear(); chRevList << options_page->revCheckBox_1 << options_page->revCheckBox_2 << - options_page->revCheckBox_3 << options_page->revCheckBox_4 << - options_page->revCheckBox_5 << options_page->revCheckBox_6 << - options_page->revCheckBox_7 << options_page->revCheckBox_8; + options_page->revCheckBox_3 << options_page->revCheckBox_4 << + options_page->revCheckBox_5 << options_page->revCheckBox_6 << + options_page->revCheckBox_7 << options_page->revCheckBox_8; - //QList buttonFunctionList; + // QList buttonFunctionList; buttonFunctionList.clear(); buttonFunctionList << options_page->buttonFunction0 << options_page->buttonFunction1 << - options_page->buttonFunction2 << options_page->buttonFunction3 << - options_page->buttonFunction4 << options_page->buttonFunction5 << - options_page->buttonFunction6 << options_page->buttonFunction7; + options_page->buttonFunction2 << options_page->buttonFunction3 << + options_page->buttonFunction4 << options_page->buttonFunction5 << + options_page->buttonFunction6 << options_page->buttonFunction7; QStringList buttonOptions; - buttonOptions <<"-" << "Roll" << "Pitch" << "Yaw" << "Throttle" << "Armed" << "GCS Control"; //added UDP control to action list - foreach (QComboBox* qb, buttonFunctionList) { + buttonOptions << "-" << "Roll" << "Pitch" << "Yaw" << "Throttle" << "Armed" << "GCS Control"; // added UDP control to action list + foreach(QComboBox * qb, buttonFunctionList) { qb->addItems(buttonOptions); } - //QList buttonActionList; + // QList buttonActionList; buttonActionList.clear(); buttonActionList << options_page->buttonAction0 << options_page->buttonAction1 << - options_page->buttonAction2 << options_page->buttonAction3 << - options_page->buttonAction4 << options_page->buttonAction5 << - options_page->buttonAction6 << options_page->buttonAction7; + options_page->buttonAction2 << options_page->buttonAction3 << + options_page->buttonAction4 << options_page->buttonAction5 << + options_page->buttonAction6 << options_page->buttonAction7; QStringList buttonActionOptions; buttonActionOptions << "Does nothing" << "Increases" << "Decreases" << "Toggles"; - foreach (QComboBox* qb, buttonActionList) { + foreach(QComboBox * qb, buttonActionList) { qb->addItems(buttonActionOptions); } - //QList buttonValueList; + // QList buttonValueList; buttonValueList.clear(); buttonValueList << options_page->buttonAmount0 << options_page->buttonAmount1 << - options_page->buttonAmount2 << options_page->buttonAmount3 << - options_page->buttonAmount4 << options_page->buttonAmount5 << - options_page->buttonAmount6 << options_page->buttonAmount7; - //QList buttonLabelList; + options_page->buttonAmount2 << options_page->buttonAmount3 << + options_page->buttonAmount4 << options_page->buttonAmount5 << + options_page->buttonAmount6 << options_page->buttonAmount7; + // QList buttonLabelList; buttonLabelList.clear(); buttonLabelList << options_page->buttonLabel0 << options_page->buttonLabel1 << - options_page->buttonLabel2 << options_page->buttonLabel3 << - options_page->buttonLabel4 << options_page->buttonLabel5 << - options_page->buttonLabel6 << options_page->buttonLabel7; + options_page->buttonLabel2 << options_page->buttonLabel3 << + options_page->buttonLabel4 << options_page->buttonLabel5 << + options_page->buttonLabel6 << options_page->buttonLabel7; - for (i=0;i<8;i++) - { + for (i = 0; i < 8; i++) { buttonActionList.at(i)->setCurrentIndex(m_config->getbuttonSettings(i).ActionID); buttonFunctionList.at(i)->setCurrentIndex(m_config->getbuttonSettings(i).FunctionID); buttonValueList.at(i)->setValue(m_config->getbuttonSettings(i).Amount); - connect(buttonFunctionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonFunction())); - //connect(buttonActionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonActions[i]())); + connect(buttonFunctionList.at(i), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonFunction())); + // connect(buttonActionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonActions[i]())); updateButtonAction(i); buttonFunctionList.at(i)->setCurrentIndex(m_config->getbuttonSettings(i).FunctionID); } - connect(buttonActionList.at(0),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_0())); - connect(buttonActionList.at(1),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_1())); - connect(buttonActionList.at(2),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_2())); - connect(buttonActionList.at(3),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_3())); - connect(buttonActionList.at(4),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_4())); - connect(buttonActionList.at(5),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_5())); - connect(buttonActionList.at(6),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_6())); - connect(buttonActionList.at(7),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonAction_7())); + connect(buttonActionList.at(0), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_0())); + connect(buttonActionList.at(1), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_1())); + connect(buttonActionList.at(2), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_2())); + connect(buttonActionList.at(3), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_3())); + connect(buttonActionList.at(4), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_4())); + connect(buttonActionList.at(5), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_5())); + connect(buttonActionList.at(6), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_6())); + connect(buttonActionList.at(7), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonAction_7())); - //updateButtonFunction(); + // updateButtonFunction(); options_page->udp_host->setText(m_config->getUDPControlHost().toString()); options_page->udp_port->setText(QString::number(m_config->getUDPControlPort())); // Controls mode are from 1 to 4. - if (m_config->getControlsMode()>0 && m_config->getControlsMode() < 5) - options_page->controlsMode->setCurrentIndex(m_config->getControlsMode()-1); - else + if (m_config->getControlsMode() > 0 && m_config->getControlsMode() < 5) { + options_page->controlsMode->setCurrentIndex(m_config->getControlsMode() - 1); + } else { qDebug() << "GCSControl: Invalid control modes setting! Did you edit by hand?"; + } QList ql = m_config->getChannelsMapping(); - for (int i=0; i<4; i++) { - if (ql.at(i) > -1) - chList.at(ql.at(i))->setCurrentIndex(i+1); + for (int i = 0; i < 4; i++) { + if (ql.at(i) > -1) { + chList.at(ql.at(i))->setCurrentIndex(i + 1); + } } QList qlChRev = m_config->getChannelsReverse(); - for (i=0; i<8; i++) - { + for (i = 0; i < 8; i++) { chRevList.at(i)->setChecked(qlChRev.at(i));; } - connect(sdlGamepad,SIGNAL(axesValues(QListInt16)),this,SLOT(axesValues(QListInt16))); - connect(sdlGamepad,SIGNAL(buttonState(ButtonNumber,bool)),this,SLOT(buttonState(ButtonNumber,bool))); - connect(sdlGamepad,SIGNAL(gamepads(quint8)),this,SLOT(gamepads(quint8))); + connect(sdlGamepad, SIGNAL(axesValues(QListInt16)), this, SLOT(axesValues(QListInt16))); + connect(sdlGamepad, SIGNAL(buttonState(ButtonNumber, bool)), this, SLOT(buttonState(ButtonNumber, bool))); + connect(sdlGamepad, SIGNAL(gamepads(quint8)), this, SLOT(gamepads(quint8))); return optionsPageWidget; } @@ -227,66 +224,63 @@ QWidget *GCSControlGadgetOptionsPage::createPage(QWidget *parent) */ void GCSControlGadgetOptionsPage::apply() { - m_config->setControlsMode(options_page->controlsMode->currentIndex()+1); - /*QList chList; - chList << options_page->channel0 << options_page->channel1 << - options_page->channel2 << options_page->channel3 << - options_page->channel4 << options_page->channel5 << - options_page->channel6 << options_page->channel7; - QList buttonFunctionList; - buttonFunctionList << options_page->buttonFunction0 << options_page->buttonFunction1 << - options_page->buttonFunction2 << options_page->buttonFunction3 << - options_page->buttonFunction4 << options_page->buttonFunction5 << - options_page->buttonFunction6 << options_page->buttonFunction7; - QList buttonActionList; - buttonActionList << options_page->buttonAction0 << options_page->buttonAction1 << - options_page->buttonAction2 << options_page->buttonAction3 << - options_page->buttonAction4 << options_page->buttonAction5 << - options_page->buttonAction6 << options_page->buttonAction7; - QList buttonValueList; - buttonValueList << options_page->buttonAmount0 << options_page->buttonAmount1 << - options_page->buttonAmount2 << options_page->buttonAmount3 << - options_page->buttonAmount4 << options_page->buttonAmount5 << - options_page->buttonAmount6 << options_page->buttonAmount7; -*/ + m_config->setControlsMode(options_page->controlsMode->currentIndex() + 1); + /*QList chList; + chList << options_page->channel0 << options_page->channel1 << + options_page->channel2 << options_page->channel3 << + options_page->channel4 << options_page->channel5 << + options_page->channel6 << options_page->channel7; + QList buttonFunctionList; + buttonFunctionList << options_page->buttonFunction0 << options_page->buttonFunction1 << + options_page->buttonFunction2 << options_page->buttonFunction3 << + options_page->buttonFunction4 << options_page->buttonFunction5 << + options_page->buttonFunction6 << options_page->buttonFunction7; + QList buttonActionList; + buttonActionList << options_page->buttonAction0 << options_page->buttonAction1 << + options_page->buttonAction2 << options_page->buttonAction3 << + options_page->buttonAction4 << options_page->buttonAction5 << + options_page->buttonAction6 << options_page->buttonAction7; + QList buttonValueList; + buttonValueList << options_page->buttonAmount0 << options_page->buttonAmount1 << + options_page->buttonAmount2 << options_page->buttonAmount3 << + options_page->buttonAmount4 << options_page->buttonAmount5 << + options_page->buttonAmount6 << options_page->buttonAmount7; + */ - int roll=-1 , pitch=-1, yaw=-1, throttle=-1; - for (int i=0; icurrentIndex()) { - case 1: - roll = i; - break; - case 2: - pitch =i; - break; - case 3: - yaw = i; - break; - case 4: - throttle = i; - break; - } - } - m_config->setRPYTchannels(roll,pitch,yaw,throttle); + int roll = -1, pitch = -1, yaw = -1, throttle = -1; + for (int i = 0; i < chList.length(); i++) { + switch (chList.at(i)->currentIndex()) { + case 1: + roll = i; + break; + case 2: + pitch = i; + break; + case 3: + yaw = i; + break; + case 4: + throttle = i; + break; + } + } + m_config->setRPYTchannels(roll, pitch, yaw, throttle); - m_config->setUDPControlSettings(options_page->udp_port->text().toInt(),options_page->udp_host->text()); - - - int j; - for (j=0;j<8;j++) - { - m_config->setbuttonSettingsAction(j,buttonActionList.at(j)->currentIndex()); - m_config->setbuttonSettingsFunction(j,buttonFunctionList.at(j)->currentIndex()); - m_config->setbuttonSettingsAmount(j,buttonValueList.at(j)->value()); - m_config->setChannelReverse(j,chRevList.at(j)->isChecked()); - } + m_config->setUDPControlSettings(options_page->udp_port->text().toInt(), options_page->udp_host->text()); + int j; + for (j = 0; j < 8; j++) { + m_config->setbuttonSettingsAction(j, buttonActionList.at(j)->currentIndex()); + m_config->setbuttonSettingsFunction(j, buttonFunctionList.at(j)->currentIndex()); + m_config->setbuttonSettingsAmount(j, buttonValueList.at(j)->value()); + m_config->setChannelReverse(j, chRevList.at(j)->isChecked()); + } } void GCSControlGadgetOptionsPage::finish() { - disconnect(sdlGamepad,0,this,0); + disconnect(sdlGamepad, 0, this, 0); delete options_page; options_page = NULL; } @@ -295,113 +289,98 @@ void GCSControlGadgetOptionsPage::finish() void GCSControlGadgetOptionsPage::updateButtonFunction() { int i; + /*QList buttonFunctionList; - buttonFunctionList << options_page->buttonFunction0 << options_page->buttonFunction1 << + buttonFunctionList << options_page->buttonFunction0 << options_page->buttonFunction1 << options_page->buttonFunction2 << options_page->buttonFunction3 << options_page->buttonFunction4 << options_page->buttonFunction5 << options_page->buttonFunction6 << options_page->buttonFunction7; - QList buttonActionList; - buttonActionList << options_page->buttonAction0 << options_page->buttonAction1 << + QList buttonActionList; + buttonActionList << options_page->buttonAction0 << options_page->buttonAction1 << options_page->buttonAction2 << options_page->buttonAction3 << options_page->buttonAction4 << options_page->buttonAction5 << options_page->buttonAction6 << options_page->buttonAction7; - QList buttonValueList; - buttonValueList << options_page->buttonAmount0 << options_page->buttonAmount1 << + QList buttonValueList; + buttonValueList << options_page->buttonAmount0 << options_page->buttonAmount1 << options_page->buttonAmount2 << options_page->buttonAmount3 << options_page->buttonAmount4 << options_page->buttonAmount5 << options_page->buttonAmount6 << options_page->buttonAmount7; - QList buttonLabelList; - buttonLabelList << options_page->buttonLabel0 << options_page->buttonLabel1 << + QList buttonLabelList; + buttonLabelList << options_page->buttonLabel0 << options_page->buttonLabel1 << options_page->buttonLabel2 << options_page->buttonLabel3 << options_page->buttonLabel4 << options_page->buttonLabel5 << options_page->buttonLabel6 << options_page->buttonLabel7; -*/ - for (i=0;i<8;i++) - { - if (buttonActionList.at(i)->currentText().compare("Does nothing")==0) - { + */ + for (i = 0; i < 8; i++) { + if (buttonActionList.at(i)->currentText().compare("Does nothing") == 0) { buttonFunctionList.at(i)->setVisible(0); buttonLabelList.at(i)->setVisible(0); buttonValueList.at(i)->setVisible(0); - } - else - if (buttonActionList.at(i)->currentText().compare("Toggles")==0) - { + } else if (buttonActionList.at(i)->currentText().compare("Toggles") == 0) { buttonFunctionList.at(i)->setVisible(1); buttonLabelList.at(i)->setVisible(0); buttonValueList.at(i)->setVisible(0); - } - else - { + } else { buttonFunctionList.at(i)->setVisible(1); buttonLabelList.at(i)->setVisible(1); buttonValueList.at(i)->setVisible(1); } } - - } void GCSControlGadgetOptionsPage::updateButtonAction(int controlID) { int i; QStringList buttonOptions; + /*QList buttonFunctionList; - buttonFunctionList << options_page->buttonFunction0 << options_page->buttonFunction1 << + buttonFunctionList << options_page->buttonFunction0 << options_page->buttonFunction1 << options_page->buttonFunction2 << options_page->buttonFunction3 << options_page->buttonFunction4 << options_page->buttonFunction5 << options_page->buttonFunction6 << options_page->buttonFunction7; - QList buttonActionList; - buttonActionList << options_page->buttonAction0 << options_page->buttonAction1 << + QList buttonActionList; + buttonActionList << options_page->buttonAction0 << options_page->buttonAction1 << options_page->buttonAction2 << options_page->buttonAction3 << options_page->buttonAction4 << options_page->buttonAction5 << options_page->buttonAction6 << options_page->buttonAction7; - QList buttonValueList; - buttonValueList << options_page->buttonAmount0 << options_page->buttonAmount1 << + QList buttonValueList; + buttonValueList << options_page->buttonAmount0 << options_page->buttonAmount1 << options_page->buttonAmount2 << options_page->buttonAmount3 << options_page->buttonAmount4 << options_page->buttonAmount5 << options_page->buttonAmount6 << options_page->buttonAmount7; - QList buttonLabelList; - buttonLabelList << options_page->buttonLabel0 << options_page->buttonLabel1 << + QList buttonLabelList; + buttonLabelList << options_page->buttonLabel0 << options_page->buttonLabel1 << options_page->buttonLabel2 << options_page->buttonLabel3 << options_page->buttonLabel4 << options_page->buttonLabel5 << options_page->buttonLabel6 << options_page->buttonLabel7; -*/ - //for (i=0;i<8;i++) - i=controlID; + */ + // for (i=0;i<8;i++) + i = controlID; { - if (buttonActionList.at(i)->currentText().compare("Does nothing")==0) - { + if (buttonActionList.at(i)->currentText().compare("Does nothing") == 0) { buttonFunctionList.at(i)->setVisible(0); buttonLabelList.at(i)->setVisible(0); buttonValueList.at(i)->setVisible(0); - } - else - if (buttonActionList.at(i)->currentText().compare("Toggles")==0) - { - disconnect(buttonFunctionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonFunction())); - buttonOptions <<"-" << "Armed" << "GCS Control" << "UDP Control"; + } else if (buttonActionList.at(i)->currentText().compare("Toggles") == 0) { + disconnect(buttonFunctionList.at(i), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonFunction())); + buttonOptions << "-" << "Armed" << "GCS Control" << "UDP Control"; buttonFunctionList.at(i)->clear(); - buttonFunctionList.at(i)->insertItems(-1,buttonOptions); + buttonFunctionList.at(i)->insertItems(-1, buttonOptions); buttonFunctionList.at(i)->setVisible(1); buttonLabelList.at(i)->setVisible(0); buttonValueList.at(i)->setVisible(0); - connect(buttonFunctionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonFunction())); - } - else - { - disconnect(buttonFunctionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonFunction())); - buttonOptions <<"-" << "Roll" << "Pitch" << "Yaw" << "Throttle" ; + connect(buttonFunctionList.at(i), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonFunction())); + } else { + disconnect(buttonFunctionList.at(i), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonFunction())); + buttonOptions << "-" << "Roll" << "Pitch" << "Yaw" << "Throttle"; buttonFunctionList.at(i)->clear(); buttonFunctionList.at(i)->addItems(buttonOptions); buttonFunctionList.at(i)->setVisible(1); buttonLabelList.at(i)->setVisible(1); buttonValueList.at(i)->setVisible(1); - connect(buttonFunctionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonFunction())); + connect(buttonFunctionList.at(i), SIGNAL(currentIndexChanged(int)), this, SLOT(updateButtonFunction())); } } - - } diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.h index 3d526cd0c..5eb5bf4dd 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.h @@ -46,14 +46,13 @@ class IUAVGadgetConfiguration; class GCSControlGadgetConfiguration; namespace Ui { - class GCSControlGadgetOptionsPage; +class GCSControlGadgetOptionsPage; } using namespace Core; -class GCSControlGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class GCSControlGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit GCSControlGadgetOptionsPage(GCSControlGadgetConfiguration *config, QObject *parent = 0); ~GCSControlGadgetOptionsPage(); @@ -67,12 +66,12 @@ private: GCSControlGadgetConfiguration *m_config; SDLGamepad *sdlGamepad; - QList chList; - QList chRevList; - QList buttonFunctionList; - QList buttonActionList; - QList buttonValueList; - QList buttonLabelList; + QList chList; + QList chRevList; + QList buttonFunctionList; + QList buttonActionList; + QList buttonValueList; + QList buttonLabelList; protected slots: // signals from joystick @@ -81,14 +80,38 @@ protected slots: void axesValues(QListInt16 values); void updateButtonFunction(); void updateButtonAction(int controlID); - void updateButtonAction_0(void){updateButtonAction(0);}; - void updateButtonAction_1(void){updateButtonAction(1);}; - void updateButtonAction_2(void){updateButtonAction(2);}; - void updateButtonAction_3(void){updateButtonAction(3);}; - void updateButtonAction_4(void){updateButtonAction(4);}; - void updateButtonAction_5(void){updateButtonAction(5);}; - void updateButtonAction_6(void){updateButtonAction(6);}; - void updateButtonAction_7(void){updateButtonAction(7);}; + void updateButtonAction_0(void) + { + updateButtonAction(0); + }; + void updateButtonAction_1(void) + { + updateButtonAction(1); + }; + void updateButtonAction_2(void) + { + updateButtonAction(2); + }; + void updateButtonAction_3(void) + { + updateButtonAction(3); + }; + void updateButtonAction_4(void) + { + updateButtonAction(4); + }; + void updateButtonAction_5(void) + { + updateButtonAction(5); + }; + void updateButtonAction_6(void) + { + updateButtonAction(6); + }; + void updateButtonAction_7(void) + { + updateButtonAction(7); + }; }; #endif // GCSCONTROLGADGETOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp index bbafda25e..d5cef0f0d 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp @@ -48,77 +48,80 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent) ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); - UAVObject::Metadata mdata = obj->getMetadata(); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("ManualControlCommand"))); + UAVObject::Metadata mdata = obj->getMetadata(); m_gcscontrol->checkBoxGcsControl->setChecked(UAVObject::GetFlightAccess(mdata) == UAVObject::ACCESS_READONLY); // Set up the drop down box for the flightmode - UAVDataObject* flightStatus = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); + UAVDataObject *flightStatus = dynamic_cast(objManager->getObject(QString("FlightStatus"))); m_gcscontrol->comboBoxFlightMode->addItems(flightStatus->getField("FlightMode")->getOptions()); // Set up slots and signals for joysticks - connect(m_gcscontrol->widgetLeftStick,SIGNAL(positionClicked(double,double)),this,SLOT(leftStickClicked(double,double))); - connect(m_gcscontrol->widgetRightStick,SIGNAL(positionClicked(double,double)),this,SLOT(rightStickClicked(double,double))); + connect(m_gcscontrol->widgetLeftStick, SIGNAL(positionClicked(double, double)), this, SLOT(leftStickClicked(double, double))); + connect(m_gcscontrol->widgetRightStick, SIGNAL(positionClicked(double, double)), this, SLOT(rightStickClicked(double, double))); // Connect misc controls connect(m_gcscontrol->checkBoxGcsControl, SIGNAL(stateChanged(int)), this, SLOT(toggleControl(int))); connect(m_gcscontrol->checkBoxArmed, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int))); connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(selectFlightMode(int))); - connect(m_gcscontrol->checkBoxUDPControl, SIGNAL(stateChanged(int)),this,SLOT(toggleUDPControl(int))); //UDP control checkbox + connect(m_gcscontrol->checkBoxUDPControl, SIGNAL(stateChanged(int)), this, SLOT(toggleUDPControl(int))); // UDP control checkbox // Connect object updated event from UAVObject to also update check boxes and dropdown - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(mccChanged(UAVObject*))); + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(mccChanged(UAVObject *))); - leftX = 0; - leftY = 0; + leftX = 0; + leftY = 0; rightX = 0; rightY = 0; // No point enabling OpenGL for the joysticks, and causes // issues on some computers: -// m_gcscontrol->widgetLeftStick->enableOpenGL(true); -// m_gcscontrol->widgetRightStick->enableOpenGL(true); +// m_gcscontrol->widgetLeftStick->enableOpenGL(true); +// m_gcscontrol->widgetRightStick->enableOpenGL(true); } GCSControlGadgetWidget::~GCSControlGadgetWidget() { - // Do nothing + // Do nothing } -void GCSControlGadgetWidget::updateSticks(double nleftX, double nleftY, double nrightX, double nrightY) { - leftX = nleftX; - leftY = nleftY; +void GCSControlGadgetWidget::updateSticks(double nleftX, double nleftY, double nrightX, double nrightY) +{ + leftX = nleftX; + leftY = nleftY; rightX = nrightX; rightY = nrightY; - m_gcscontrol->widgetLeftStick->changePosition(leftX,leftY); - m_gcscontrol->widgetRightStick->changePosition(rightX,rightY); + m_gcscontrol->widgetLeftStick->changePosition(leftX, leftY); + m_gcscontrol->widgetRightStick->changePosition(rightX, rightY); } -void GCSControlGadgetWidget::leftStickClicked(double X, double Y) { +void GCSControlGadgetWidget::leftStickClicked(double X, double Y) +{ leftX = X; leftY = Y; - emit sticksChanged(leftX,leftY,rightX,rightY); + emit sticksChanged(leftX, leftY, rightX, rightY); } -void GCSControlGadgetWidget::rightStickClicked(double X, double Y) { +void GCSControlGadgetWidget::rightStickClicked(double X, double Y) +{ rightX = X; rightY = Y; - emit sticksChanged(leftX,leftY,rightX,rightY); + emit sticksChanged(leftX, leftY, rightX, rightY); } /*! - \brief Called when the gcs control is toggled and enabled or disables flight write access to manual control command - */ + \brief Called when the gcs control is toggled and enabled or disables flight write access to manual control command + */ void GCSControlGadgetWidget::toggleControl(int state) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("ManualControlCommand"))); - UAVObject::Metadata mdata = obj->getMetadata(); - if (state) - { + UAVObject::Metadata mdata = obj->getMetadata(); + + if (state) { mccInitialData = mdata; UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); @@ -126,9 +129,7 @@ void GCSControlGadgetWidget::toggleControl(int state) UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); mdata.gcsTelemetryUpdatePeriod = 100; m_gcscontrol->checkBoxUDPControl->setEnabled(true); - } - else - { + } else { mdata = mccInitialData; toggleUDPControl(false); m_gcscontrol->checkBoxUDPControl->setEnabled(false); @@ -140,44 +141,46 @@ void GCSControlGadgetWidget::toggleArmed(int state) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); - if(state) + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("FlightStatus"))); + + if (state) { obj->getField("Armed")->setValue("Armed"); - else + } else { obj->getField("Armed")->setValue("Disarmed"); + } obj->updated(); } -void GCSControlGadgetWidget::mccChanged(UAVObject * obj) +void GCSControlGadgetWidget::mccChanged(UAVObject *obj) { Q_UNUSED(obj); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* flightStatus = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); + UAVDataObject *flightStatus = dynamic_cast(objManager->getObject(QString("FlightStatus"))); m_gcscontrol->comboBoxFlightMode->setCurrentIndex(m_gcscontrol->comboBoxFlightMode->findText(flightStatus->getField("FlightMode")->getValue().toString())); m_gcscontrol->checkBoxArmed->setChecked(flightStatus->getField("Armed")->getValue() == "Armed"); } void GCSControlGadgetWidget::toggleUDPControl(int state) { - if(state) - { + if (state) { setUDPControl(true); - }else{ + } else { setUDPControl(false); } } /*! - \brief Called when the flight mode drop down is changed and sets the ManualControlCommand->FlightMode accordingly - */ + \brief Called when the flight mode drop down is changed and sets the ManualControlCommand->FlightMode accordingly + */ void GCSControlGadgetWidget::selectFlightMode(int state) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); - UAVObjectField * field = obj->getField("FlightMode"); + UAVDataObject *obj = dynamic_cast(objManager->getObject(QString("FlightStatus"))); + UAVObjectField *field = obj->getField("FlightMode"); + field->setValue(field->getOptions()[state]); obj->updated(); } @@ -203,7 +206,6 @@ bool GCSControlGadgetWidget::getUDPControl(void) /** - * @} - * @} - */ - + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h index 05c3964cd..16f794d4c 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GCSControlGadgetPlugin GCSControl Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,8 +35,7 @@ class Ui_GCSControl; -class GCSControlGadgetWidget : public QLabel -{ +class GCSControlGadgetWidget : public QLabel { Q_OBJECT public: @@ -68,7 +67,7 @@ protected slots: private: Ui_GCSControl *m_gcscontrol; UAVObject::Metadata mccInitialData; - double leftX,leftY,rightX,rightY; + double leftX, leftY, rightX, rightY; }; #endif /* GCSControlGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.cpp index af866df17..3f8c8db41 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.cpp @@ -34,42 +34,42 @@ GCSControlPlugin::GCSControlPlugin() { - // Do nothing + // Do nothing } GCSControlPlugin::~GCSControlPlugin() { - // Do nothing + // Do nothing } -bool GCSControlPlugin::initialize(const QStringList& args, QString *errMsg) +bool GCSControlPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); + Q_UNUSED(args); + Q_UNUSED(errMsg); sdlGamepad = new SDLGamepad(); - if(sdlGamepad->init()) { - sdlGamepad->start(); - qRegisterMetaType("QListInt16"); - qRegisterMetaType("ButtonNumber"); - } - mf = new GCSControlGadgetFactory(this); - addAutoReleasedObject(mf); + if (sdlGamepad->init()) { + sdlGamepad->start(); + qRegisterMetaType("QListInt16"); + qRegisterMetaType("ButtonNumber"); + } + mf = new GCSControlGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void GCSControlPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void GCSControlPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(GCSControlPlugin) /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.h index e115e3546..4ab32503f 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolplugin.h @@ -33,19 +33,17 @@ class GCSControlGadgetFactory; -class GCSControlPlugin : public ExtensionSystem::IPlugin -{ +class GCSControlPlugin : public ExtensionSystem::IPlugin { public: GCSControlPlugin(); - ~GCSControlPlugin(); + ~GCSControlPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); - SDLGamepad *sdlGamepad; + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); + SDLGamepad *sdlGamepad; private: - GCSControlGadgetFactory *mf; - + GCSControlGadgetFactory *mf; }; #endif /* GCSControlPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcsonctrolgadgetwidget.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcsonctrolgadgetwidget.h index 7ee0bd326..737846899 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcsonctrolgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcsonctrolgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup EmptyGadgetPlugin Empty Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,13 +30,12 @@ #include -class EmptyGadgetWidget : public QLabel -{ +class EmptyGadgetWidget : public QLabel { Q_OBJECT public: EmptyGadgetWidget(QWidget *parent = 0); - ~EmptyGadgetWidget(); + ~EmptyGadgetWidget(); private: }; diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.cpp index 8a0fb1360..861e38c96 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.cpp @@ -33,8 +33,8 @@ #include /** - * @brief Constructor for JoystickControl widget. Sets up the image of a joystick - */ + * @brief Constructor for JoystickControl widget. Sets up the image of a joystick + */ JoystickControl::JoystickControl(QWidget *parent) : QGraphicsView(parent) { setMinimumSize(64, 64); @@ -61,7 +61,7 @@ JoystickControl::JoystickControl(QWidget *parent) : QGraphicsView(parent) m_joystickArea->setPos( (m_background->boundingRect().width() - m_joystickArea->boundingRect().width()) * 0.5, (m_background->boundingRect().height() - m_joystickArea->boundingRect().height()) * 0.5 - ); + ); m_joystickArea->setVisible(false); QGraphicsScene *l_scene = scene(); @@ -80,50 +80,61 @@ JoystickControl::~JoystickControl() } /** - * @brief Enables/Disables OpenGL - */ + * @brief Enables/Disables OpenGL + */ void JoystickControl::enableOpenGL(bool flag) { - if (flag) + if (flag) { setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); - else + } else { setViewport(new QWidget); + } } /** - * @brief Update the displayed position based on an MCC update - */ + * @brief Update the displayed position based on an MCC update + */ void JoystickControl::changePosition(double x, double y) { QRectF areaSize = m_joystickArea->boundingRect(); QPointF point( ((1.0 + x) * areaSize.width() - m_joystickEnd->boundingRect().width()) * 0.5, ((1.0 - y) * areaSize.height() - m_joystickEnd->boundingRect().height()) * 0.5 - ); + ); + m_joystickEnd->setPos(m_joystickArea->mapToScene(point)); } /** - * @brief Redirect mouse move events to control position - */ + * @brief Redirect mouse move events to control position + */ void JoystickControl::mouseMoveEvent(QMouseEvent *event) { - QPointF point = m_joystickArea->mapFromScene(mapToScene(event->pos())); + QPointF point = m_joystickArea->mapFromScene(mapToScene(event->pos())); QSizeF areaSize = m_joystickArea->boundingRect().size(); - double y = - (point.y() / areaSize.height() - 0.5) * 2.0; + double y = -(point.y() / areaSize.height() - 0.5) * 2.0; double x = (point.x() / areaSize.width() - 0.5) * 2.0; - if (y < -1.0) y = -1.0; - if (y > 1.0) y = 1.0; - if (x < -1.0) x = -1.0; - if (x > 1.0) x = 1.0; + + if (y < -1.0) { + y = -1.0; + } + if (y > 1.0) { + y = 1.0; + } + if (x < -1.0) { + x = -1.0; + } + if (x > 1.0) { + x = 1.0; + } emit positionClicked(x, y); } /** - * @brief Redirect mouse move clicks to control position - */ + * @brief Redirect mouse move clicks to control position + */ void JoystickControl::mousePressEvent(QMouseEvent *event) { if (event->button() == Qt::LeftButton) { @@ -153,6 +164,6 @@ void JoystickControl::resizeEvent(QResizeEvent *event) } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.h b/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.h index 9c6ac59d0..6bd3bb8d0 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.h @@ -35,11 +35,10 @@ #include "manualcontrolcommand.h" namespace Ui { - class JoystickControl; +class JoystickControl; } -class JoystickControl : public QGraphicsView -{ +class JoystickControl : public QGraphicsView { Q_OBJECT public: @@ -61,15 +60,15 @@ signals: void positionClicked(double x, double y); private: - QSvgRenderer *m_renderer; - QGraphicsSvgItem *m_background; - QGraphicsSvgItem *m_joystickArea; - QGraphicsSvgItem *m_joystickEnd; + QSvgRenderer *m_renderer; + QGraphicsSvgItem *m_background; + QGraphicsSvgItem *m_joystickArea; + QGraphicsSvgItem *m_joystickEnd; }; #endif // JOYSTICKCONTROL_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.cpp index 08d93c157..032fec211 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.cpp @@ -4,8 +4,8 @@ * @file buffer.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief see below - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and * objects. * @see The GNU Public License (GPL) Version 3 * @@ -28,7 +28,7 @@ */ /*! \file buffer.c \brief Multipurpose byte buffer structure and methods. */ -//***************************************************************************** +// ***************************************************************************** // // File Name : 'buffer.c' // Title : Multipurpose byte buffer structure and methods @@ -40,9 +40,9 @@ // Editor Tabs : 4 // // This code is distributed under the GNU Public License -// which can be found at http://www.gnu.org/licenses/gpl.txt +// which can be found at http://www.gnu.org/licenses/gpl.txt // -//***************************************************************************** +// ***************************************************************************** #include "buffer.h" @@ -50,90 +50,81 @@ // initialization -void bufferInit(cBuffer* buffer, unsigned char *start, unsigned short size) +void bufferInit(cBuffer *buffer, unsigned char *start, unsigned short size) { - // set start pointer of the buffer - buffer->dataptr = start; - buffer->size = size; - // initialize index and length - buffer->dataindex = 0; - buffer->datalength = 0; + // set start pointer of the buffer + buffer->dataptr = start; + buffer->size = size; + // initialize index and length + buffer->dataindex = 0; + buffer->datalength = 0; } // access routines -unsigned char bufferGetFromFront(cBuffer* buffer) +unsigned char bufferGetFromFront(cBuffer *buffer) { - unsigned char data = 0; + unsigned char data = 0; - // check to see if there's data in the buffer - if(buffer->datalength) - { - // get the first character from buffer - data = buffer->dataptr[buffer->dataindex]; - // move index down and decrement length - buffer->dataindex++; - if(buffer->dataindex >= buffer->size) - { - buffer->dataindex %= buffer->size; - } - buffer->datalength--; - } - // return - return data; + // check to see if there's data in the buffer + if (buffer->datalength) { + // get the first character from buffer + data = buffer->dataptr[buffer->dataindex]; + // move index down and decrement length + buffer->dataindex++; + if (buffer->dataindex >= buffer->size) { + buffer->dataindex %= buffer->size; + } + buffer->datalength--; + } + // return + return data; } -void bufferDumpFromFront(cBuffer* buffer, unsigned short numbytes) +void bufferDumpFromFront(cBuffer *buffer, unsigned short numbytes) { - // dump numbytes from the front of the buffer - // are we dumping less than the entire buffer? - if(numbytes < buffer->datalength) - { - // move index down by numbytes and decrement length by numbytes - buffer->dataindex += numbytes; - if(buffer->dataindex >= buffer->size) - { - buffer->dataindex %= buffer->size; - } - buffer->datalength -= numbytes; - } - else - { - // flush the whole buffer - buffer->datalength = 0; - } + // dump numbytes from the front of the buffer + // are we dumping less than the entire buffer? + if (numbytes < buffer->datalength) { + // move index down by numbytes and decrement length by numbytes + buffer->dataindex += numbytes; + if (buffer->dataindex >= buffer->size) { + buffer->dataindex %= buffer->size; + } + buffer->datalength -= numbytes; + } else { + // flush the whole buffer + buffer->datalength = 0; + } } -unsigned char bufferGetAtIndex(cBuffer* buffer, unsigned short index) +unsigned char bufferGetAtIndex(cBuffer *buffer, unsigned short index) { - // return character at index in buffer - return buffer->dataptr[(buffer->dataindex+index)%(buffer->size)]; + // return character at index in buffer + return buffer->dataptr[(buffer->dataindex + index) % (buffer->size)]; } -unsigned char bufferAddToEnd(cBuffer* buffer, unsigned char data) +unsigned char bufferAddToEnd(cBuffer *buffer, unsigned char data) { - // make sure the buffer has room - if(buffer->datalength < buffer->size) - { - // save data byte at end of buffer - buffer->dataptr[(buffer->dataindex + buffer->datalength) % buffer->size] = data; - // increment the length - buffer->datalength++; - // return success - return -1; - } - else return 0; + // make sure the buffer has room + if (buffer->datalength < buffer->size) { + // save data byte at end of buffer + buffer->dataptr[(buffer->dataindex + buffer->datalength) % buffer->size] = data; + // increment the length + buffer->datalength++; + // return success + return -1; + } else { return 0; } } -unsigned char bufferIsNotFull(cBuffer* buffer) +unsigned char bufferIsNotFull(cBuffer *buffer) { - // check to see if the buffer has room - // return true if there is room - return (buffer->datalength < buffer->size); + // check to see if the buffer has room + // return true if there is room + return buffer->datalength < buffer->size; } -void bufferFlush(cBuffer* buffer) +void bufferFlush(cBuffer *buffer) { - // flush contents of the buffer - buffer->datalength = 0; + // flush contents of the buffer + buffer->datalength = 0; } - diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.h b/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.h index 513b1b241..cb495f15d 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/buffer.h @@ -4,15 +4,15 @@ * @file buffer.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief see below - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and * objects. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ /*! \file buffer.h \brief Multipurpose byte buffer structure and methods. */ -//***************************************************************************** +// ***************************************************************************** // // File Name : 'buffer.h' // Title : Multipurpose byte buffer structure and methods @@ -40,48 +40,47 @@ /// maximum size.  This buffer is used in many places in the avrlib code. // // This code is distributed under the GNU Public License -// which can be found at http://www.gnu.org/licenses/gpl.txt +// which can be found at http://www.gnu.org/licenses/gpl.txt // -//***************************************************************************** -//@{ +// ***************************************************************************** +// @{ #ifndef BUFFER_HPP #define BUFFER_HPP // structure/typdefs -//! cBuffer structure -typedef struct struct_cBuffer -{ - unsigned char *dataptr; ///< the physical memory address where the buffer is stored - unsigned short size; ///< the allocated size of the buffer - unsigned short datalength; ///< the length of the data currently in the buffer - unsigned short dataindex; ///< the index into the buffer where the data starts +// ! cBuffer structure +typedef struct struct_cBuffer { + unsigned char *dataptr; ///< the physical memory address where the buffer is stored + unsigned short size; ///< the allocated size of the buffer + unsigned short datalength; ///< the length of the data currently in the buffer + unsigned short dataindex; ///< the index into the buffer where the data starts } cBuffer; // function prototypes -//! initialize a buffer to start at a given address and have given size -void bufferInit(cBuffer* buffer, unsigned char *start, unsigned short size); +// ! initialize a buffer to start at a given address and have given size +void bufferInit(cBuffer *buffer, unsigned char *start, unsigned short size); -//! get the first byte from the front of the buffer -unsigned char bufferGetFromFront(cBuffer* buffer); +// ! get the first byte from the front of the buffer +unsigned char bufferGetFromFront(cBuffer *buffer); -//! dump (discard) the first numbytes from the front of the buffer -void bufferDumpFromFront(cBuffer* buffer, unsigned short numbytes); +// ! dump (discard) the first numbytes from the front of the buffer +void bufferDumpFromFront(cBuffer *buffer, unsigned short numbytes); -//! get a byte at the specified index in the buffer (kind of like array access) +// ! get a byte at the specified index in the buffer (kind of like array access) // ** note: this does not remove the byte that was read from the buffer -unsigned char bufferGetAtIndex(cBuffer* buffer, unsigned short index); +unsigned char bufferGetAtIndex(cBuffer *buffer, unsigned short index); -//! add a byte to the end of the buffer -unsigned char bufferAddToEnd(cBuffer* buffer, unsigned char data); +// ! add a byte to the end of the buffer +unsigned char bufferAddToEnd(cBuffer *buffer, unsigned char data); -//! check if the buffer is full/not full (returns non-zero value if not full) -unsigned char bufferIsNotFull(cBuffer* buffer); +// ! check if the buffer is full/not full (returns non-zero value if not full) +unsigned char bufferIsNotFull(cBuffer *buffer); -//! flush (clear) the contents of the buffer -void bufferFlush(cBuffer* buffer); +// ! flush (clear) the contents of the buffer +void bufferFlush(cBuffer *buffer); -#endif -//@} +#endif // ifndef BUFFER_HPP +// @} diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp index e70f2ed72..2bf058844 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp @@ -35,7 +35,6 @@ */ GpsConstellationWidget::GpsConstellationWidget(QWidget *parent) : QGraphicsView(parent) { - // Create a layout, add a QGraphicsView and put the SVG inside. // The constellation widget looks like this: // |--------------------| @@ -64,7 +63,7 @@ GpsConstellationWidget::GpsConstellationWidget(QWidget *parent) : QGraphicsView( setScene(scene); // Now create 'maxSatellites' satellite icons which we will move around on the map: - for (int i=0; i < MAX_SATTELITES;i++) { + for (int i = 0; i < MAX_SATTELITES; i++) { satellites[i][0] = 0; satellites[i][1] = 0; satellites[i][2] = 0; @@ -75,7 +74,7 @@ GpsConstellationWidget::GpsConstellationWidget(QWidget *parent) : QGraphicsView( satIcons[i]->setElementId("sat-notSeen"); satIcons[i]->hide(); - satTexts[i] = new QGraphicsSimpleTextItem("##",satIcons[i]); + satTexts[i] = new QGraphicsSimpleTextItem("##", satIcons[i]); satTexts[i]->setBrush(QColor("Black")); satTexts[i]->setFont(QFont("Courier")); } @@ -86,8 +85,8 @@ GpsConstellationWidget::~GpsConstellationWidget() delete scene; scene = 0; - //delete renderer; - //renderer = 0; + // delete renderer; + // renderer = 0; } void GpsConstellationWidget::showEvent(QShowEvent *event) @@ -98,13 +97,12 @@ void GpsConstellationWidget::showEvent(QShowEvent *event) // the result is usually a ahrsbargraph that is way too small. fitInView(world, Qt::KeepAspectRatio); // Scale, can't use fitInView since that doesn't work until we're shown. - // qreal factor = height()/world->boundingRect().height(); -// world->setScale(factor); - // setSceneRect(world->boundingRect()); - + // qreal factor = height()/world->boundingRect().height(); +// world->setScale(factor); +// setSceneRect(world->boundingRect()); } -void GpsConstellationWidget::resizeEvent(QResizeEvent* event) +void GpsConstellationWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); fitInView(world, Qt::KeepAspectRatio); @@ -124,10 +122,10 @@ void GpsConstellationWidget::updateSat(int index, int prn, int elevation, int az satellites[index][3] = snr; if (prn && elevation >= 0) { - QPointF opd = polarToCoord(elevation,azimuth); + QPointF opd = polarToCoord(elevation, azimuth); opd += QPointF(-satIcons[index]->boundingRect().center().x(), -satIcons[index]->boundingRect().center().y()); - satIcons[index]->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false); + satIcons[index]->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), false); if (snr) { satIcons[index]->setElementId("satellite"); } else { @@ -135,30 +133,29 @@ void GpsConstellationWidget::updateSat(int index, int prn, int elevation, int az } satIcons[index]->show(); - QRectF iconRect = satIcons[index]->boundingRect(); + QRectF iconRect = satIcons[index]->boundingRect(); QString prnString = QString().number(prn); - if(prnString.length() == 1) { + if (prnString.length() == 1) { prnString = "0" + prnString; } satTexts[index]->setText(prnString); QRectF textRect = satTexts[index]->boundingRect(); QTransform matrix; - qreal scale = 0.70 * (iconRect.width() / textRect.width()); - matrix.translate(iconRect.width()/2, iconRect.height()/2); - matrix.scale(scale,scale); - matrix.translate(-textRect.width()/2,-textRect.height()/2); - satTexts[index]->setTransform(matrix,false); + qreal scale = 0.70 * (iconRect.width() / textRect.width()); + matrix.translate(iconRect.width() / 2, iconRect.height() / 2); + matrix.scale(scale, scale); + matrix.translate(-textRect.width() / 2, -textRect.height() / 2); + satTexts[index]->setTransform(matrix, false); } else { satIcons[index]->hide(); } - } /** - Converts the elevation/azimuth to X/Y coordinates on the map + Converts the elevation/azimuth to X/Y coordinates on the map - */ + */ QPointF GpsConstellationWidget::polarToCoord(int elevation, int azimuth) { double x; @@ -167,18 +164,17 @@ QPointF GpsConstellationWidget::polarToCoord(int elevation, int azimuth) double rad_azimuth; - rad_elevation = M_PI*elevation/180; - rad_azimuth = M_PI*azimuth/180; + rad_elevation = M_PI * elevation / 180; + rad_azimuth = M_PI * azimuth / 180; - x = cos(rad_elevation)*sin(rad_azimuth); - y = -cos(rad_elevation)*cos(rad_azimuth); + x = cos(rad_elevation) * sin(rad_azimuth); + y = -cos(rad_elevation) * cos(rad_azimuth); - x = world->boundingRect().width()/2 * x; - y = world->boundingRect().height()/2 * y; + x = world->boundingRect().width() / 2 * x; + y = world->boundingRect().height() / 2 * y; x = (world->boundingRect().width() / 2) + x; y = (world->boundingRect().height() / 2) + y; - return QPointF(x,y); - + return QPointF(x, y); } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.h index 6fc6e8543..b8b1424a5 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.h @@ -33,35 +33,32 @@ #include -class GpsConstellationWidget : public QGraphicsView -{ +class GpsConstellationWidget : public QGraphicsView { Q_OBJECT public: - explicit GpsConstellationWidget(QWidget *parent = 0); - ~GpsConstellationWidget(); + explicit GpsConstellationWidget(QWidget *parent = 0); + ~GpsConstellationWidget(); public slots: - void updateSat(int index, int prn, int elevation, int azimuth, int snr); + void updateSat(int index, int prn, int elevation, int azimuth, int snr); private slots: private: - static const int MAX_SATTELITES = 16; - int satellites[MAX_SATTELITES][4]; - QGraphicsScene *scene; - QSvgRenderer *renderer; - QGraphicsSvgItem* world; - QGraphicsSvgItem* satIcons[MAX_SATTELITES]; - QGraphicsSimpleTextItem* satTexts[MAX_SATTELITES]; + static const int MAX_SATTELITES = 16; + int satellites[MAX_SATTELITES][4]; + QGraphicsScene *scene; + QSvgRenderer *renderer; + QGraphicsSvgItem *world; + QGraphicsSvgItem *satIcons[MAX_SATTELITES]; + QGraphicsSimpleTextItem *satTexts[MAX_SATTELITES]; - QPointF polarToCoord(int elevation, int azimuth); + QPointF polarToCoord(int elevation, int azimuth); protected: void showEvent(QShowEvent *event); void resizeEvent(QResizeEvent *event); - - }; #endif /* GPSCONSTELLATIONWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.cpp index 7d8f6eb9e..75167ebf7 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,12 +30,12 @@ #include "gpsdisplaygadgetconfiguration.h" GpsDisplayGadget::GpsDisplayGadget(QString classId, GpsDisplayWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget), - connected(FALSE) + IUAVGadget(classId, parent), + m_widget(widget), + connected(FALSE) { - connect(m_widget->connectButton, SIGNAL(clicked(bool)), this,SLOT(onConnect())); - connect(m_widget->disconnectButton, SIGNAL(clicked(bool)), this,SLOT(onDisconnect())); + connect(m_widget->connectButton, SIGNAL(clicked(bool)), this, SLOT(onConnect())); + connect(m_widget->disconnectButton, SIGNAL(clicked(bool)), this, SLOT(onDisconnect())); } GpsDisplayGadget::~GpsDisplayGadget() @@ -44,49 +44,48 @@ GpsDisplayGadget::~GpsDisplayGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void GpsDisplayGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void GpsDisplayGadget::loadConfiguration(IUAVGadgetConfiguration *config) { // Delete the (old)port, this also closes it. - if(port) { + if (port) { delete port; } // Delete the (old)parser, this also disconnects all signals. - if(parser) { + if (parser) { delete parser; } - GpsDisplayGadgetConfiguration *gpsDisplayConfig = qobject_cast< GpsDisplayGadgetConfiguration*>(config); + GpsDisplayGadgetConfiguration *gpsDisplayConfig = qobject_cast< GpsDisplayGadgetConfiguration *>(config); if (gpsDisplayConfig->connectionMode() == "Serial") { PortSettings portsettings; - portsettings.BaudRate=gpsDisplayConfig->speed(); - portsettings.DataBits=gpsDisplayConfig->dataBits(); - portsettings.FlowControl=gpsDisplayConfig->flow(); - portsettings.Parity=gpsDisplayConfig->parity(); - portsettings.StopBits=gpsDisplayConfig->stopBits(); - portsettings.Timeout_Millisec=gpsDisplayConfig->timeOut(); + portsettings.BaudRate = gpsDisplayConfig->speed(); + portsettings.DataBits = gpsDisplayConfig->dataBits(); + portsettings.FlowControl = gpsDisplayConfig->flow(); + portsettings.Parity = gpsDisplayConfig->parity(); + portsettings.StopBits = gpsDisplayConfig->stopBits(); + portsettings.Timeout_Millisec = gpsDisplayConfig->timeOut(); // In case we find no port, buttons disabled m_widget->connectButton->setEnabled(false); m_widget->disconnectButton->setEnabled(false); QList ports = QextSerialEnumerator::getPorts(); - foreach( QextPortInfo nport, ports ) { - if(nport.friendName == gpsDisplayConfig->port()) - { + foreach(QextPortInfo nport, ports) { + if (nport.friendName == gpsDisplayConfig->port()) { qDebug() << "Using Serial parser"; parser = new NMEAParser(); #ifdef Q_OS_WIN - port=new QextSerialPort(nport.portName,portsettings,QextSerialPort::EventDriven); + port = new QextSerialPort(nport.portName, portsettings, QextSerialPort::EventDriven); #else - port=new QextSerialPort(nport.physName,portsettings,QextSerialPort::EventDriven); + port = new QextSerialPort(nport.physName, portsettings, QextSerialPort::EventDriven); #endif m_widget->connectButton->setEnabled(true); m_widget->disconnectButton->setEnabled(false); @@ -104,44 +103,45 @@ void GpsDisplayGadget::loadConfiguration(IUAVGadgetConfiguration* config) m_widget->connectButton->setHidden(true); m_widget->dataStreamGroupBox->setHidden(true); } else if (gpsDisplayConfig->connectionMode() == "Network") { - // Not implemented for now... + // Not implemented for now... m_widget->connectButton->setEnabled(false); m_widget->disconnectButton->setEnabled(false); m_widget->dataStreamGroupBox->setHidden(false); } - connect(parser, SIGNAL(sv(int)), m_widget,SLOT(setSVs(int))); - connect(parser, SIGNAL(position(double,double,double)), m_widget,SLOT(setPosition(double,double,double))); - connect(parser, SIGNAL(speedheading(double,double)), m_widget,SLOT(setSpeedHeading(double,double))); - connect(parser, SIGNAL(datetime(double,double)), m_widget,SLOT(setDateTime(double,double))); + connect(parser, SIGNAL(sv(int)), m_widget, SLOT(setSVs(int))); + connect(parser, SIGNAL(position(double, double, double)), m_widget, SLOT(setPosition(double, double, double))); + connect(parser, SIGNAL(speedheading(double, double)), m_widget, SLOT(setSpeedHeading(double, double))); + connect(parser, SIGNAL(datetime(double, double)), m_widget, SLOT(setDateTime(double, double))); connect(parser, SIGNAL(packet(QString)), m_widget, SLOT(dumpPacket(QString))); - connect(parser, SIGNAL(satellite(int,int,int,int,int)), m_widget->gpsSky, SLOT(updateSat(int,int,int,int,int))); - connect(parser, SIGNAL(satellite(int,int,int,int,int)), m_widget->gpsSnrWidget, SLOT(updateSat(int,int,int,int,int))); + connect(parser, SIGNAL(satellite(int, int, int, int, int)), m_widget->gpsSky, SLOT(updateSat(int, int, int, int, int))); + connect(parser, SIGNAL(satellite(int, int, int, int, int)), m_widget->gpsSnrWidget, SLOT(updateSat(int, int, int, int, int))); connect(parser, SIGNAL(fixtype(QString)), m_widget, SLOT(setFixType(QString))); - connect(parser, SIGNAL(dop(double,double,double)), m_widget, SLOT(setDOP(double,double,double))); + connect(parser, SIGNAL(dop(double, double, double)), m_widget, SLOT(setDOP(double, double, double))); } -void GpsDisplayGadget::onConnect() { +void GpsDisplayGadget::onConnect() +{ m_widget->textBrowser->append(QString("Connecting to GPS ...\n")); // TODO: Somehow mark that we're running, and disable connect button while so? if (port) { - qDebug() << "Opening: " << port->portName() << "."; - bool isOpen = port->open(QIODevice::ReadWrite); - qDebug() << "Open: " << isOpen; - if(isOpen) { + qDebug() << "Opening: " << port->portName() << "."; + bool isOpen = port->open(QIODevice::ReadWrite); + qDebug() << "Open: " << isOpen; + if (isOpen) { m_widget->connectButton->setEnabled(false); m_widget->disconnectButton->setEnabled(true); } } else { qDebug() << "Port undefined or invalid."; } - } -void GpsDisplayGadget::onDisconnect() { +void GpsDisplayGadget::onDisconnect() +{ if (port) { - qDebug() << "Closing: " << port->portName() << "."; + qDebug() << "Closing: " << port->portName() << "."; port->close(); m_widget->connectButton->setEnabled(true); m_widget->disconnectButton->setEnabled(false); @@ -150,23 +150,26 @@ void GpsDisplayGadget::onDisconnect() { } } -void GpsDisplayGadget::onDataAvailable() { +void GpsDisplayGadget::onDataAvailable() +{ int avail = port->bytesAvailable(); - if( avail > 0 ) { + + if (avail > 0) { QByteArray serialData; serialData.resize(avail); int bytesRead = port->read(serialData.data(), serialData.size()); - if( bytesRead > 0 ) { + if (bytesRead > 0) { processNewSerialData(serialData); } } } -void GpsDisplayGadget::processNewSerialData(QByteArray serialData) { - int dataLength = serialData.size(); - const char* data = serialData.constData(); +void GpsDisplayGadget::processNewSerialData(QByteArray serialData) +{ + int dataLength = serialData.size(); + const char *data = serialData.constData(); - for(int pos = 0; pos < dataLength; pos++) { + for (int pos = 0; pos < dataLength; pos++) { parser->processInputStream(data[pos]); } } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.h index 69d5607da..c0dd90b64 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -42,18 +42,20 @@ class GpsDisplayWidget; using namespace Core; -class GpsDisplayGadget : public Core::IUAVGadget -{ +class GpsDisplayGadget : public Core::IUAVGadget { Q_OBJECT public: GpsDisplayGadget(QString classId, GpsDisplayWidget *widget, QWidget *parent = 0); ~GpsDisplayGadget(); - QWidget *widget() { return m_widget; } + QWidget *widget() + { + return m_widget; + } - // void setMode(QString mode); // Either UAVTalk or serial port + // void setMode(QString mode); // Either UAVTalk or serial port - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); public slots: void onConnect(); void onDisconnect(); diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.cpp index 404aa36a3..d1baaba25 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,7 +32,7 @@ * Loads a saved configuration or defaults if non exist. * */ -GpsDisplayGadgetConfiguration::GpsDisplayGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +GpsDisplayGadgetConfiguration::GpsDisplayGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_connectionMode("Serial"), m_defaultPort("Unknown"), @@ -43,36 +43,35 @@ GpsDisplayGadgetConfiguration::GpsDisplayGadgetConfiguration(QString classId, QS m_defaultStopBits(STOP_1), m_defaultTimeOut(5000) { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { BaudRateType speed; DataBitsType databits; FlowType flow; ParityType parity; StopBitsType stopbits; - int ispeed = qSettings->value("defaultSpeed").toInt(); - int idatabits = qSettings->value("defaultDataBits").toInt(); - int iflow = qSettings->value("defaultFlow").toInt(); - int iparity = qSettings->value("defaultParity").toInt(); - int istopbits = qSettings->value("defaultStopBits").toInt(); - QString port = qSettings->value("defaultPort").toString(); + int ispeed = qSettings->value("defaultSpeed").toInt(); + int idatabits = qSettings->value("defaultDataBits").toInt(); + int iflow = qSettings->value("defaultFlow").toInt(); + int iparity = qSettings->value("defaultParity").toInt(); + int istopbits = qSettings->value("defaultStopBits").toInt(); + QString port = qSettings->value("defaultPort").toString(); QString conMode = qSettings->value("connectionMode").toString(); - databits = (DataBitsType) idatabits; - flow = (FlowType)iflow; - parity = (ParityType)iparity; + databits = (DataBitsType)idatabits; + flow = (FlowType)iflow; + parity = (ParityType)iparity; stopbits = (StopBitsType)istopbits; - speed = (BaudRateType)ispeed; - m_defaultPort = port; - m_defaultSpeed = speed; + speed = (BaudRateType)ispeed; + m_defaultPort = port; + m_defaultSpeed = speed; m_defaultDataBits = databits; - m_defaultFlow = flow; - m_defaultParity = parity; + m_defaultFlow = flow; + m_defaultParity = parity; m_defaultStopBits = stopbits; - m_connectionMode = conMode; + m_connectionMode = conMode; } - } /** @@ -83,13 +82,13 @@ IUAVGadgetConfiguration *GpsDisplayGadgetConfiguration::clone() { GpsDisplayGadgetConfiguration *m = new GpsDisplayGadgetConfiguration(this->classId()); - m->m_defaultSpeed = m_defaultSpeed; + m->m_defaultSpeed = m_defaultSpeed; m->m_defaultDataBits = m_defaultDataBits; - m->m_defaultFlow = m_defaultFlow; - m->m_defaultParity = m_defaultParity; + m->m_defaultFlow = m_defaultFlow; + m->m_defaultParity = m_defaultParity; m->m_defaultStopBits = m_defaultStopBits; - m->m_defaultPort = m_defaultPort; - m->m_connectionMode = m_connectionMode; + m->m_defaultPort = m_defaultPort; + m->m_connectionMode = m_connectionMode; return m; } @@ -97,13 +96,13 @@ IUAVGadgetConfiguration *GpsDisplayGadgetConfiguration::clone() * Saves a configuration. * */ -void GpsDisplayGadgetConfiguration::saveConfig(QSettings* settings) const { - settings->setValue("defaultSpeed", m_defaultSpeed); - settings->setValue("defaultDataBits", m_defaultDataBits); - settings->setValue("defaultFlow", m_defaultFlow); - settings->setValue("defaultParity", m_defaultParity); - settings->setValue("defaultStopBits", m_defaultStopBits); - settings->setValue("defaultPort", m_defaultPort); - settings->setValue("connectionMode", m_connectionMode); - +void GpsDisplayGadgetConfiguration::saveConfig(QSettings *settings) const +{ + settings->setValue("defaultSpeed", m_defaultSpeed); + settings->setValue("defaultDataBits", m_defaultDataBits); + settings->setValue("defaultFlow", m_defaultFlow); + settings->setValue("defaultParity", m_defaultParity); + settings->setValue("defaultStopBits", m_defaultStopBits); + settings->setValue("defaultPort", m_defaultPort); + settings->setValue("connectionMode", m_connectionMode); } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.h index 54fede7e8..3fdd94b64 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetconfiguration.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,46 +33,92 @@ using namespace Core; -class GpsDisplayGadgetConfiguration : public IUAVGadgetConfiguration -{ +class GpsDisplayGadgetConfiguration : public IUAVGadgetConfiguration { Q_OBJECT - public: - explicit GpsDisplayGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); +public: + explicit GpsDisplayGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void setConnectionMode(QString mode) { m_connectionMode = mode; } - QString connectionMode() { return m_connectionMode; } + void setConnectionMode(QString mode) + { + m_connectionMode = mode; + } + QString connectionMode() + { + return m_connectionMode; + } - //set port configuration functions - void setSpeed(BaudRateType speed) {m_defaultSpeed=speed;} - void setDataBits(DataBitsType databits) {m_defaultDataBits=databits;} - void setFlow(FlowType flow) {m_defaultFlow=flow;} - void setParity(ParityType parity) {m_defaultParity=parity;} - void setStopBits(StopBitsType stopbits) {m_defaultStopBits=stopbits;} - void setPort(QString port){m_defaultPort=port;} - void setTimeOut(long timeout){m_defaultTimeOut=timeout;} + // set port configuration functions + void setSpeed(BaudRateType speed) + { + m_defaultSpeed = speed; + } + void setDataBits(DataBitsType databits) + { + m_defaultDataBits = databits; + } + void setFlow(FlowType flow) + { + m_defaultFlow = flow; + } + void setParity(ParityType parity) + { + m_defaultParity = parity; + } + void setStopBits(StopBitsType stopbits) + { + m_defaultStopBits = stopbits; + } + void setPort(QString port) + { + m_defaultPort = port; + } + void setTimeOut(long timeout) + { + m_defaultTimeOut = timeout; + } - //get port configuration functions - QString port(){return m_defaultPort;} - BaudRateType speed() {return m_defaultSpeed;} - FlowType flow() {return m_defaultFlow;} - DataBitsType dataBits() {return m_defaultDataBits;} - StopBitsType stopBits() {return m_defaultStopBits;} - ParityType parity() {return m_defaultParity;} - long timeOut(){return m_defaultTimeOut;} + // get port configuration functions + QString port() + { + return m_defaultPort; + } + BaudRateType speed() + { + return m_defaultSpeed; + } + FlowType flow() + { + return m_defaultFlow; + } + DataBitsType dataBits() + { + return m_defaultDataBits; + } + StopBitsType stopBits() + { + return m_defaultStopBits; + } + ParityType parity() + { + return m_defaultParity; + } + long timeOut() + { + return m_defaultTimeOut; + } - void saveConfig(QSettings* settings) const; - IUAVGadgetConfiguration *clone(); - - private: - QString m_connectionMode; - QString m_defaultPort; - BaudRateType m_defaultSpeed; - DataBitsType m_defaultDataBits; - FlowType m_defaultFlow; - ParityType m_defaultParity; - StopBitsType m_defaultStopBits; - long m_defaultTimeOut; + void saveConfig(QSettings *settings) const; + IUAVGadgetConfiguration *clone(); +private: + QString m_connectionMode; + QString m_defaultPort; + BaudRateType m_defaultSpeed; + DataBitsType m_defaultDataBits; + FlowType m_defaultFlow; + ParityType m_defaultParity; + StopBitsType m_defaultStopBits; + long m_defaultTimeOut; }; #endif // GPSDISPLAYGADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.cpp index bedf86c19..2b97864d9 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,29 +32,27 @@ #include GpsDisplayGadgetFactory::GpsDisplayGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("GpsDisplayGadget"), - tr("GPS Display"), - parent) -{ -} + IUAVGadgetFactory(QString("GpsDisplayGadget"), + tr("GPS Display"), + parent) +{} GpsDisplayGadgetFactory::~GpsDisplayGadgetFactory() -{ -} +{} -Core::IUAVGadget* GpsDisplayGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *GpsDisplayGadgetFactory::createGadget(QWidget *parent) { - GpsDisplayWidget* gadgetWidget = new GpsDisplayWidget(parent); + GpsDisplayWidget *gadgetWidget = new GpsDisplayWidget(parent); + return new GpsDisplayGadget(QString("GpsDisplayGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *GpsDisplayGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *GpsDisplayGadgetFactory::createConfiguration(QSettings *qSettings) { return new GpsDisplayGadgetConfiguration(QString("GpsDisplayGadget"), qSettings); } IOptionsPage *GpsDisplayGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new GpsDisplayGadgetOptionsPage(qobject_cast(config)); + return new GpsDisplayGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.h index 54ce96d55..99bb20e50 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class GpsDisplayGadgetFactory : public IUAVGadgetFactory -{ +class GpsDisplayGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: GpsDisplayGadgetFactory(QObject *parent = 0); ~GpsDisplayGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.cpp index 4ec271c57..be8646636 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,135 +34,135 @@ #include GpsDisplayGadgetOptionsPage::GpsDisplayGadgetOptionsPage(GpsDisplayGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) + IOptionsPage(parent), + m_config(config) { -// Taken from the uploader gadget, since we also can use a serial port for this -// Gadget +// Taken from the uploader gadget, since we also can use a serial port for this +// Gadget - //the begining of some ugly code -//diferent OS's have diferent serial port capabilities + // the begining of some ugly code +// diferent OS's have diferent serial port capabilities #ifdef Q_OS_WIN -//load windows port capabilities -BaudRateTypeString - <<"BAUD110" - <<"BAUD300" - <<"BAUD600" - <<"BAUD1200" - <<"BAUD2400" - <<"BAUD4800" - <<"BAUD9600" - <<"BAUD14400" - <<"BAUD19200" - <<"BAUD38400" - <<"BAUD56000" - <<"BAUD57600" - <<"BAUD115200" - <<"BAUD128000" - <<"BAUD256000"; -DataBitsTypeString - <<"DATA_5" - <<"DATA_6" - <<"DATA_7" - <<"DATA_8"; -ParityTypeString - <<"PAR_NONE" - <<"PAR_ODD" - <<"PAR_EVEN" - <<"PAR_MARK" //WINDOWS ONLY - <<"PAR_SPACE"; -StopBitsTypeString - <<"STOP_1" - <<"STOP_1_5" //WINDOWS ONLY - <<"STOP_2"; -#else -//load POSIX port capabilities -BaudRateTypeString +// load windows port capabilities + BaudRateTypeString + << "BAUD110" + << "BAUD300" + << "BAUD600" + << "BAUD1200" + << "BAUD2400" + << "BAUD4800" + << "BAUD9600" + << "BAUD14400" + << "BAUD19200" + << "BAUD38400" + << "BAUD56000" + << "BAUD57600" + << "BAUD115200" + << "BAUD128000" + << "BAUD256000"; + DataBitsTypeString + << "DATA_5" + << "DATA_6" + << "DATA_7" + << "DATA_8"; + ParityTypeString + << "PAR_NONE" + << "PAR_ODD" + << "PAR_EVEN" + << "PAR_MARK" // WINDOWS ONLY + << "PAR_SPACE"; + StopBitsTypeString + << "STOP_1" + << "STOP_1_5" // WINDOWS ONLY + << "STOP_2"; +#else // ifdef Q_OS_WIN +// load POSIX port capabilities + BaudRateTypeString - <<"BAUD50" //POSIX ONLY - <<"BAUD75" //POSIX ONLY - <<"BAUD110" - <<"BAUD134" //POSIX ONLY - <<"BAUD150" //POSIX ONLY - <<"BAUD200" //POSIX ONLY - <<"BAUD300" - <<"BAUD600" - <<"BAUD1200" - <<"BAUD1800" //POSIX ONLY - <<"BAUD2400" - <<"BAUD4800" - <<"BAUD9600" - <<"BAUD19200" - <<"BAUD38400" - <<"BAUD57600" - <<"BAUD76800" //POSIX ONLY - <<"BAUD115200"; -DataBitsTypeString - <<"DATA_5" - <<"DATA_6" - <<"DATA_7" - <<"DATA_8"; -ParityTypeString - <<"PAR_NONE" - <<"PAR_ODD" - <<"PAR_EVEN" - <<"PAR_SPACE"; -StopBitsTypeString - <<"STOP_1" - <<"STOP_2"; -#endif -//load all OS's capabilities -BaudRateTypeStringALL - <<"BAUD50" //POSIX ONLY - <<"BAUD75" //POSIX ONLY - <<"BAUD110" - <<"BAUD134" //POSIX ONLY - <<"BAUD150" //POSIX ONLY - <<"BAUD200" //POSIX ONLY - <<"BAUD300" - <<"BAUD600" - <<"BAUD1200" - <<"BAUD1800" //POSIX ONLY - <<"BAUD2400" - <<"BAUD4800" - <<"BAUD9600" - <<"BAUD14400" - <<"BAUD19200" - <<"BAUD38400" - <<"BAUD56000" - <<"BAUD57600" - <<"BAUD76800" //POSIX ONLY - <<"BAUD115200" - <<"BAUD128000" - <<"BAUD256000"; -DataBitsTypeStringALL - <<"DATA_5" - <<"DATA_6" - <<"DATA_7" - <<"DATA_8"; -ParityTypeStringALL - <<"PAR_NONE" - <<"PAR_ODD" - <<"PAR_EVEN" - <<"PAR_MARK" //WINDOWS ONLY - <<"PAR_SPACE"; -StopBitsTypeStringALL - <<"STOP_1" - <<"STOP_1_5" //WINDOWS ONLY - <<"STOP_2"; + << "BAUD50" // POSIX ONLY + << "BAUD75" // POSIX ONLY + << "BAUD110" + << "BAUD134" // POSIX ONLY + << "BAUD150" // POSIX ONLY + << "BAUD200" // POSIX ONLY + << "BAUD300" + << "BAUD600" + << "BAUD1200" + << "BAUD1800" // POSIX ONLY + << "BAUD2400" + << "BAUD4800" + << "BAUD9600" + << "BAUD19200" + << "BAUD38400" + << "BAUD57600" + << "BAUD76800" // POSIX ONLY + << "BAUD115200"; + DataBitsTypeString + << "DATA_5" + << "DATA_6" + << "DATA_7" + << "DATA_8"; + ParityTypeString + << "PAR_NONE" + << "PAR_ODD" + << "PAR_EVEN" + << "PAR_SPACE"; + StopBitsTypeString + << "STOP_1" + << "STOP_2"; +#endif // ifdef Q_OS_WIN +// load all OS's capabilities + BaudRateTypeStringALL + << "BAUD50" // POSIX ONLY + << "BAUD75" // POSIX ONLY + << "BAUD110" + << "BAUD134" // POSIX ONLY + << "BAUD150" // POSIX ONLY + << "BAUD200" // POSIX ONLY + << "BAUD300" + << "BAUD600" + << "BAUD1200" + << "BAUD1800" // POSIX ONLY + << "BAUD2400" + << "BAUD4800" + << "BAUD9600" + << "BAUD14400" + << "BAUD19200" + << "BAUD38400" + << "BAUD56000" + << "BAUD57600" + << "BAUD76800" // POSIX ONLY + << "BAUD115200" + << "BAUD128000" + << "BAUD256000"; + DataBitsTypeStringALL + << "DATA_5" + << "DATA_6" + << "DATA_7" + << "DATA_8"; + ParityTypeStringALL + << "PAR_NONE" + << "PAR_ODD" + << "PAR_EVEN" + << "PAR_MARK" // WINDOWS ONLY + << "PAR_SPACE"; + StopBitsTypeStringALL + << "STOP_1" + << "STOP_1_5" // WINDOWS ONLY + << "STOP_2"; -FlowTypeString - <<"FLOW_OFF" - <<"FLOW_HARDWARE" - <<"FLOW_XONXOFF"; + FlowTypeString + << "FLOW_OFF" + << "FLOW_HARDWARE" + << "FLOW_XONXOFF"; } -bool sortPorts(QextPortInfo const& s1,QextPortInfo const& s2) +bool sortPorts(QextPortInfo const & s1, QextPortInfo const & s2) { -return s1.portName ports = QextSerialEnumerator::getPorts(); - qSort(ports.begin(), ports.end(),sortPorts); - foreach( QextPortInfo port, ports ) { + qSort(ports.begin(), ports.end(), sortPorts); + foreach(QextPortInfo port, ports) { qDebug() << "Adding port: " << port.friendName << " (" << port.portName << ")"; options_page->portComboBox->addItem(port.friendName, port.friendName); } int portIndex = options_page->portComboBox->findData(m_config->port()); - if(portIndex!=-1){ + if (portIndex != -1) { qDebug() << "createPage(): port is " << m_config->port(); options_page->portComboBox->setCurrentIndex(portIndex); } @@ -190,40 +190,40 @@ QWidget *GpsDisplayGadgetOptionsPage::createPage(QWidget *parent) options_page->portSpeedComboBox->addItems(BaudRateTypeString); int portSpeedIndex = options_page->portSpeedComboBox->findText(BaudRateTypeStringALL.at((int)m_config->speed())); - if(portSpeedIndex != -1){ - options_page->portSpeedComboBox->setCurrentIndex(portSpeedIndex); + if (portSpeedIndex != -1) { + options_page->portSpeedComboBox->setCurrentIndex(portSpeedIndex); } // FLOW CONTROL options_page->flowControlComboBox->addItems(FlowTypeString); int flowControlIndex = options_page->flowControlComboBox->findText(FlowTypeString.at((int)m_config->flow())); - if(flowControlIndex != -1){ - options_page->flowControlComboBox->setCurrentIndex(flowControlIndex); + if (flowControlIndex != -1) { + options_page->flowControlComboBox->setCurrentIndex(flowControlIndex); } // DATABITS options_page->dataBitsComboBox->addItems(DataBitsTypeString); int dataBitsIndex = options_page->dataBitsComboBox->findText(DataBitsTypeStringALL.at((int)m_config->dataBits())); - if(dataBitsIndex != -1){ - options_page->dataBitsComboBox->setCurrentIndex(dataBitsIndex); + if (dataBitsIndex != -1) { + options_page->dataBitsComboBox->setCurrentIndex(dataBitsIndex); } // STOPBITS options_page->stopBitsComboBox->addItems(StopBitsTypeString); int stopBitsIndex = options_page->stopBitsComboBox->findText(StopBitsTypeStringALL.at((int)m_config->stopBits())); - if(stopBitsIndex != -1){ - options_page->stopBitsComboBox->setCurrentIndex(stopBitsIndex); + if (stopBitsIndex != -1) { + options_page->stopBitsComboBox->setCurrentIndex(stopBitsIndex); } // PARITY options_page->parityComboBox->addItems(ParityTypeString); int parityIndex = options_page->parityComboBox->findText(ParityTypeStringALL.at((int)m_config->parity())); - if(parityIndex != -1){ - options_page->parityComboBox->setCurrentIndex(parityIndex); + if (parityIndex != -1) { + options_page->parityComboBox->setCurrentIndex(parityIndex); } // TIMEOUT @@ -233,8 +233,9 @@ QWidget *GpsDisplayGadgetOptionsPage::createPage(QWidget *parent) connectionModes << "Serial" << "Network" << "Telemetry"; options_page->connectionMode->addItems(connectionModes); int conMode = options_page->connectionMode->findText(m_config->connectionMode()); - if (conMode != -1) - options_page->connectionMode->setCurrentIndex(conMode); + if (conMode != -1) { + options_page->connectionMode->setCurrentIndex(conMode); + } return optionsPageWidget; @@ -249,6 +250,7 @@ QWidget *GpsDisplayGadgetOptionsPage::createPage(QWidget *parent) void GpsDisplayGadgetOptionsPage::apply() { int portIndex = options_page->portComboBox->currentIndex(); + m_config->setPort(options_page->portComboBox->itemData(portIndex).toString()); qDebug() << "apply(): port is " << m_config->port(); @@ -257,9 +259,8 @@ void GpsDisplayGadgetOptionsPage::apply() m_config->setDataBits((DataBitsType)DataBitsTypeStringALL.indexOf(options_page->dataBitsComboBox->currentText())); m_config->setStopBits((StopBitsType)StopBitsTypeStringALL.indexOf(options_page->stopBitsComboBox->currentText())); m_config->setParity((ParityType)ParityTypeStringALL.indexOf(options_page->parityComboBox->currentText())); - m_config->setTimeOut( options_page->timeoutSpinBox->value()); + m_config->setTimeOut(options_page->timeoutSpinBox->value()); m_config->setConnectionMode(options_page->connectionMode->currentText()); - } void GpsDisplayGadgetOptionsPage::finish() diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.h index b98af560a..a2fe2861f 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -41,14 +41,13 @@ class IUAVGadgetConfiguration; class GpsDisplayGadgetConfiguration; namespace Ui { - class GpsDisplayGadgetOptionsPage; +class GpsDisplayGadgetOptionsPage; } using namespace Core; -class GpsDisplayGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class GpsDisplayGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit GpsDisplayGadgetOptionsPage(GpsDisplayGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.cpp index 7f43e8fbb..0e2909bce 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,31 +35,31 @@ GpsDisplayPlugin::GpsDisplayPlugin() { - // Do nothing + // Do nothing } GpsDisplayPlugin::~GpsDisplayPlugin() { - // Do nothing + // Do nothing } -bool GpsDisplayPlugin::initialize(const QStringList& args, QString *errMsg) +bool GpsDisplayPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new GpsDisplayGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new GpsDisplayGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void GpsDisplayPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void GpsDisplayPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(GpsDisplayPlugin) diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.h index f1e9a4e90..f867c552d 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplayplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class GpsDisplayGadgetFactory; -class GpsDisplayPlugin : public ExtensionSystem::IPlugin -{ +class GpsDisplayPlugin : public ExtensionSystem::IPlugin { public: - GpsDisplayPlugin(); - ~GpsDisplayPlugin(); + GpsDisplayPlugin(); + ~GpsDisplayPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - GpsDisplayGadgetFactory *mf; + GpsDisplayGadgetFactory *mf; }; #endif /* GPSDISPLAYPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.cpp index bdc2684b0..67f30080a 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -41,52 +41,53 @@ GpsDisplayWidget::GpsDisplayWidget(QWidget *parent) : QWidget(parent) { setupUi(this); - //Not elegant, just load the image for now + // Not elegant, just load the image for now QGraphicsScene *fescene = new QGraphicsScene(this); - QPixmap earthpix( ":/gpsgadget/images/flatEarth.png" ); - fescene->addPixmap( earthpix ); + QPixmap earthpix(":/gpsgadget/images/flatEarth.png"); + fescene->addPixmap(earthpix); flatEarth->setScene(fescene); marker = new QGraphicsSvgItem(); QSvgRenderer *renderer = new QSvgRenderer(); renderer->load(QString(":/gpsgadget/images/marker.svg")); marker->setSharedRenderer(renderer); fescene->addItem(marker); - double scale = earthpix.width()/(marker->boundingRect().width()*20); + double scale = earthpix.width() / (marker->boundingRect().width() * 20); marker->setScale(scale); } GpsDisplayWidget::~GpsDisplayWidget() -{ -} +{} void GpsDisplayWidget::setSpeedHeading(double speed, double heading) { QString str; - speed_value->setText(str.sprintf("%.02f m/s",speed)); - bear_value->setText(str.sprintf("%.02f deg",heading)); + + speed_value->setText(str.sprintf("%.02f m/s", speed)); + bear_value->setText(str.sprintf("%.02f deg", heading)); } void GpsDisplayWidget::setDateTime(double date, double time) { - QString dstring1,dstring2; - dstring1.sprintf("%06.0f",date); - dstring1.insert(dstring1.length()-2,"."); - dstring1.insert(dstring1.length()-5,"."); - dstring2.sprintf("%06.0f",time); - dstring2.insert(dstring2.length()-2,":"); - dstring2.insert(dstring2.length()-5,":"); + QString dstring1, dstring2; + + dstring1.sprintf("%06.0f", date); + dstring1.insert(dstring1.length() - 2, "."); + dstring1.insert(dstring1.length() - 5, "."); + dstring2.sprintf("%06.0f", time); + dstring2.insert(dstring2.length() - 2, ":"); + dstring2.insert(dstring2.length() - 5, ":"); time_value->setText(dstring1 + " " + dstring2 + " GMT"); } void GpsDisplayWidget::setFixType(const QString &fixtype) { - if(fixtype =="NoGPS") { + if (fixtype == "NoGPS") { fix_value->setText("No GPS"); } else if (fixtype == "NoFix") { fix_value->setText("Fix not available"); } else if (fixtype == "Fix2D") { fix_value->setText("2D"); - } else if (fixtype =="Fix3D") { + } else if (fixtype == "Fix3D") { fix_value->setText("3D"); } else { fix_value->setText("Unknown"); @@ -96,7 +97,7 @@ void GpsDisplayWidget::setFixType(const QString &fixtype) void GpsDisplayWidget::dumpPacket(const QString &packet) { textBrowser->append(packet); - if(textBrowser->document()->lineCount() > 200) { + if (textBrowser->document()->lineCount() > 200) { QTextCursor tc = textBrowser->textCursor(); tc.movePosition(QTextCursor::Start); tc.movePosition(QTextCursor::Down, QTextCursor::KeepAnchor); @@ -108,6 +109,7 @@ void GpsDisplayWidget::dumpPacket(const QString &packet) void GpsDisplayWidget::setSVs(int sv) { QString temp; + temp.append(QString::number(sv)); status_value->setText(temp); status_value->adjustSize(); @@ -115,44 +117,45 @@ void GpsDisplayWidget::setSVs(int sv) void GpsDisplayWidget::setDOP(double hdop, double vdop, double pdop) { - QString str; + str.sprintf("%.2f / %.2f / %.2f", hdop, vdop, pdop); dop_value->setText(str); - } void GpsDisplayWidget::setPosition(double lat, double lon, double alt) { - //lat *= 1E-7; - //lon *= 1E-7; + // lat *= 1E-7; + // lon *= 1E-7; double deg = floor(fabs(lat)); - double min = (fabs(lat)-deg)*60; + double min = (fabs(lat) - deg) * 60; QString str1; - str1.sprintf("%.0f%c%.3f' ", deg,0x00b0, min); - if (lat>0) + + str1.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min); + if (lat > 0) { str1.append("N"); - else + } else { str1.append("S"); + } coord_value->setText(str1); deg = floor(fabs(lon)); - min = (fabs(lon)-deg)*60; + min = (fabs(lon) - deg) * 60; QString str2; - str2.sprintf("%.0f%c%.3f' ", deg,0x00b0, min); - if (lon>0) + str2.sprintf("%.0f%c%.3f' ", deg, 0x00b0, min); + if (lon > 0) { str2.append("E"); - else + } else { str2.append("W"); + } coord_value_2->setText(str2); QString str3; str3.sprintf("%.2f m", alt); coord_value_3->setText(str3); // Now place the marker: - double wscale = flatEarth->sceneRect().width()/360; - double hscale = flatEarth->sceneRect().height()/180; - QPointF opd = QPointF((lon+180)*wscale-marker->boundingRect().width()*marker->scale()/2, - (90-lat)*hscale-marker->boundingRect().height()*marker->scale()/2); - marker->setTransform(QTransform::fromTranslate( opd.x(), opd.y()) , false); - + double wscale = flatEarth->sceneRect().width() / 360; + double hscale = flatEarth->sceneRect().height() / 180; + QPointF opd = QPointF((lon + 180) * wscale - marker->boundingRect().width() * marker->scale() / 2, + (90 - lat) * hscale - marker->boundingRect().height() * marker->scale() / 2); + marker->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), false); } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.h index 50873791e..a7be00809 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaywidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GPSGadgetPlugin GPS Gadget Plugin * @{ - * @brief A gadget that displays GPS status and enables basic configuration + * @brief A gadget that displays GPS status and enables basic configuration *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,25 +38,24 @@ class Ui_GpsDisplayWidget; -class GpsDisplayWidget : public QWidget, public Ui_GpsDisplayWidget -{ +class GpsDisplayWidget : public QWidget, public Ui_GpsDisplayWidget { Q_OBJECT public: GpsDisplayWidget(QWidget *parent = 0); - ~GpsDisplayWidget(); + ~GpsDisplayWidget(); private slots: - void setSVs(int); - void setPosition(double, double, double); - void setDateTime(double, double); - void setSpeedHeading(double, double); - void dumpPacket(const QString &packet); - void setFixType(const QString &fixtype); - void setDOP(double hdop, double vdop, double pdop); + void setSVs(int); + void setPosition(double, double, double); + void setDateTime(double, double); + void setSpeedHeading(double, double); + void dumpPacket(const QString &packet); + void setFixType(const QString &fixtype); + void setDOP(double hdop, double vdop, double pdop); private: - GpsConstellationWidget * gpsConstellation; - QGraphicsSvgItem * marker; + GpsConstellationWidget *gpsConstellation; + QGraphicsSvgItem *marker; }; #endif /* GPSDISPLAYWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.cpp index 8f1717f45..53060e5eb 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.cpp @@ -33,11 +33,11 @@ GPSParser::GPSParser(QObject *parent) : QObject(parent) } GPSParser::~GPSParser() -{ +{} -} - -void GPSParser::processInputStream(char c) { +void GPSParser::processInputStream(char c) { - Q_UNUSED(c)} + { + Q_UNUSED(c) + } } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.h index a980f689a..ae0479ccc 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsparser.h @@ -32,27 +32,25 @@ #include #include -class GPSParser: public QObject -{ +class GPSParser : public QObject { Q_OBJECT -public: - ~GPSParser(); +public: ~GPSParser(); virtual void processInputStream(char c); protected: GPSParser(QObject *parent = 0); signals: - void sv(int); // Satellites in view - void position(double,double,double); // Lat, Lon, Alt - void datetime(double,double); // Date then time - void speedheading(double,double); - void packet(QString); // Raw NMEA Packet (or just info) - void satellite(int,int,int,int,int); // Index, PRN, Elevation, Azimuth, SNR - void fixmode(QString); // Mode of fix: "Auto", "Manual". - void fixtype(QString); // Type of fix: "NoGPS", "NoFix", "Fix2D", "Fix3D". - void dop(double, double, double); // HDOP, VDOP, PDOP - void fixSVs(QList); // SV's used for fix. + void sv(int); // Satellites in view + void position(double, double, double); // Lat, Lon, Alt + void datetime(double, double); // Date then time + void speedheading(double, double); + void packet(QString); // Raw NMEA Packet (or just info) + void satellite(int, int, int, int, int); // Index, PRN, Elevation, Azimuth, SNR + void fixmode(QString); // Mode of fix: "Auto", "Manual". + void fixtype(QString); // Type of fix: "NoGPS", "NoFix", "Fix2D", "Fix3D". + void dop(double, double, double); // HDOP, VDOP, PDOP + void fixSVs(QList); // SV's used for fix. }; #endif // GPSPARSER_H diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.cpp index 3edc51ee3..157a461d5 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.cpp @@ -1,13 +1,13 @@ #include "gpssnrwidget.h" GpsSnrWidget::GpsSnrWidget(QWidget *parent) : - QGraphicsView(parent) { - + QGraphicsView(parent) +{ scene = new QGraphicsScene(this); setScene(scene); // Now create 'maxSatellites' satellite icons which we will move around on the map: - for (int i=0; i < MAX_SATTELITES;i++) { + for (int i = 0; i < MAX_SATTELITES; i++) { satellites[i][0] = 0; satellites[i][1] = 0; satellites[i][2] = 0; @@ -18,40 +18,42 @@ GpsSnrWidget::GpsSnrWidget(QWidget *parent) : scene->addItem(boxes[i]); boxes[i]->hide(); - satTexts[i] = new QGraphicsSimpleTextItem("##",boxes[i]); + satTexts[i] = new QGraphicsSimpleTextItem("##", boxes[i]); satTexts[i]->setBrush(QColor("Black")); satTexts[i]->setFont(QFont("Courier")); - satSNRs[i] = new QGraphicsSimpleTextItem("##",boxes[i]); + satSNRs[i] = new QGraphicsSimpleTextItem("##", boxes[i]); satSNRs[i]->setBrush(QColor("Black")); satSNRs[i]->setFont(QFont("Courier")); - } - } -GpsSnrWidget::~GpsSnrWidget() { +GpsSnrWidget::~GpsSnrWidget() +{ delete scene; scene = 0; } -void GpsSnrWidget::showEvent(QShowEvent *event) { +void GpsSnrWidget::showEvent(QShowEvent *event) +{ Q_UNUSED(event) - scene->setSceneRect(0,0, this->viewport()->width(), this->viewport()->height()); - for(int index = 0 ;index < MAX_SATTELITES ; index++) { + scene->setSceneRect(0, 0, this->viewport()->width(), this->viewport()->height()); + for (int index = 0; index < MAX_SATTELITES; index++) { drawSat(index); } } -void GpsSnrWidget::resizeEvent(QResizeEvent* event) { +void GpsSnrWidget::resizeEvent(QResizeEvent *event) +{ Q_UNUSED(event); - scene->setSceneRect(0,0, this->viewport()->width(), this->viewport()->height()); - for(int index = 0 ;index < MAX_SATTELITES ; index++) { + scene->setSceneRect(0, 0, this->viewport()->width(), this->viewport()->height()); + for (int index = 0; index < MAX_SATTELITES; index++) { drawSat(index); } } -void GpsSnrWidget::updateSat(int index, int prn, int elevation, int azimuth, int snr) { +void GpsSnrWidget::updateSat(int index, int prn, int elevation, int azimuth, int snr) +{ if (index >= MAX_SATTELITES) { // A bit of error checking never hurts. return; @@ -66,7 +68,8 @@ void GpsSnrWidget::updateSat(int index, int prn, int elevation, int azimuth, int drawSat(index); } -void GpsSnrWidget::drawSat(int index) { +void GpsSnrWidget::drawSat(int index) +{ if (index >= MAX_SATTELITES) { // A bit of error checking never hurts. return; @@ -85,10 +88,10 @@ void GpsSnrWidget::drawSat(int index) { // Casting to int rounds down, which is what I want. // Minus 2 to allow a pixel of white left and right. - int availableWidth = (int)((scene->width()-2) / MAX_SATTELITES); + int availableWidth = (int)((scene->width() - 2) / MAX_SATTELITES); // 2 pixels, one on each side. - qreal width = availableWidth - 2; + qreal width = availableWidth - 2; // SNR = 1-99 (0 is special).. qreal height = int((scene->height() / 99) * snr + 0.5); // 1 for showing a pixel of white to the left. @@ -96,38 +99,37 @@ void GpsSnrWidget::drawSat(int index) { // Rember, 0 is at the top. qreal y = scene->height() - height; // Compensate for the extra pixel for the border. - boxes[index]->setRect(0,0,width-1,height-1); - boxes[index]->setPos(x,y); + boxes[index]->setRect(0, 0, width - 1, height - 1); + boxes[index]->setPos(x, y); - QRectF boxRect = boxes[index]->boundingRect(); + QRectF boxRect = boxes[index]->boundingRect(); QString prnString = QString().number(prn); - if(prnString.length() == 1) { + if (prnString.length() == 1) { prnString = "0" + prnString; } satTexts[index]->setText(prnString); QRectF textRect = satTexts[index]->boundingRect(); QTransform matrix; - qreal scale = 0.85 * (boxRect.width() / textRect.width()); - matrix.translate( boxRect.width()/2, boxRect.height()); - matrix.scale(scale,scale); - matrix.translate(-textRect.width()/2,-textRect.height()); - satTexts[index]->setTransform(matrix,false); + qreal scale = 0.85 * (boxRect.width() / textRect.width()); + matrix.translate(boxRect.width() / 2, boxRect.height()); + matrix.scale(scale, scale); + matrix.translate(-textRect.width() / 2, -textRect.height()); + satTexts[index]->setTransform(matrix, false); QString snrString = QString().number(snr); - if (snrString.length() ==1) { // Will probably never happen! + if (snrString.length() == 1) { // Will probably never happen! snrString = "0" + snrString; } satSNRs[index]->setText(snrString); textRect = satSNRs[index]->boundingRect(); matrix.reset(); - scale = 0.85 * (boxRect.width() / textRect.width()); - matrix.translate( boxRect.width()/2,0); - matrix.scale(scale,scale); - matrix.translate(-textRect.width()/2,-textRect.height()); - satSNRs[index]->setTransform(matrix,false); - + scale = 0.85 * (boxRect.width() / textRect.width()); + matrix.translate(boxRect.width() / 2, 0); + matrix.scale(scale, scale); + matrix.translate(-textRect.width() / 2, -textRect.height()); + satSNRs[index]->setTransform(matrix, false); } else { boxes[index]->hide(); } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.h b/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.h index 7b7f9517c..6b8d2e7b8 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpssnrwidget.h @@ -4,8 +4,7 @@ #include #include -class GpsSnrWidget : public QGraphicsView -{ +class GpsSnrWidget : public QGraphicsView { Q_OBJECT public: explicit GpsSnrWidget(QWidget *parent = 0); @@ -21,15 +20,14 @@ private: int satellites[MAX_SATTELITES][4]; QGraphicsScene *scene; QGraphicsRectItem *boxes[MAX_SATTELITES]; - QGraphicsSimpleTextItem* satTexts[MAX_SATTELITES]; - QGraphicsSimpleTextItem* satSNRs[MAX_SATTELITES]; + QGraphicsSimpleTextItem *satTexts[MAX_SATTELITES]; + QGraphicsSimpleTextItem *satSNRs[MAX_SATTELITES]; void drawSat(int index); protected: void showEvent(QShowEvent *event); void resizeEvent(QResizeEvent *event); - }; #endif // GPSSNRWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp index 735316496..af0193e7c 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp @@ -36,59 +36,56 @@ #include // Message Codes -#define NMEA_NODATA 0 // No data. Packet not available, bad, or not decoded -#define NMEA_GPGGA 1 // Global Positioning System Fix Data -#define NMEA_GPVTG 2 // Course over ground and ground speed -#define NMEA_GPGLL 3 // Geographic position - latitude/longitude -#define NMEA_GPGSV 4 // GPS satellites in view -#define NMEA_GPGSA 5 // GPS DOP and active satellites -#define NMEA_GPRMC 6 // Recommended minimum specific GPS data -#define NMEA_GPZDA 7 // Time and Date -#define NMEA_UNKNOWN 0xFF// Packet received but not known +#define NMEA_NODATA 0 // No data. Packet not available, bad, or not decoded +#define NMEA_GPGGA 1 // Global Positioning System Fix Data +#define NMEA_GPVTG 2 // Course over ground and ground speed +#define NMEA_GPGLL 3 // Geographic position - latitude/longitude +#define NMEA_GPGSV 4 // GPS satellites in view +#define NMEA_GPGSA 5 // GPS DOP and active satellites +#define NMEA_GPRMC 6 // Recommended minimum specific GPS data +#define NMEA_GPZDA 7 // Time and Date +#define NMEA_UNKNOWN 0xFF // Packet received but not known #define GPS_TIMEOUT_MS 500 // Debugging -//#define GPSDEBUG -//#define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages +// #define GPSDEBUG +// #define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages #ifdef GPSDEBUG - #define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages - #define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages - #define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages - #define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages - #define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages - #define NMEA_DEBUG_ZDA ///< define to enable debug of ZDA messages + #define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages + #define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages + #define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages + #define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages + #define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages + #define NMEA_DEBUG_ZDA ///< define to enable debug of ZDA messages #endif /** * Initialize the parser */ -NMEAParser::NMEAParser(QObject *parent):GPSParser(parent) +NMEAParser::NMEAParser(QObject *parent) : GPSParser(parent) { bufferInit(&gpsRxBuffer, (unsigned char *)gpsRxData, 512); - gpsRxOverflow=0; + gpsRxOverflow = 0; } NMEAParser::~NMEAParser() -{ - -} +{} /** * Called each time there are data in the input buffer */ void NMEAParser::processInputStream(char c) { - if( !bufferAddToEnd(&gpsRxBuffer, c) ) - { - // no space in buffer - // count overflow - gpsRxOverflow++; - return; - } - nmeaProcess(&gpsRxBuffer); + if (!bufferAddToEnd(&gpsRxBuffer, c)) { + // no space in buffer + // count overflow + gpsRxOverflow++; + return; + } + nmeaProcess(&gpsRxBuffer); } @@ -98,48 +95,39 @@ void NMEAParser::processInputStream(char c) * \return 0 checksum not valid * \return 1 checksum valid */ -char NMEAParser::nmeaChecksum(char* gps_buffer) +char NMEAParser::nmeaChecksum(char *gps_buffer) { - char checksum=0; - char checksum_received=0; + char checksum = 0; + char checksum_received = 0; - for(int x=0; xdatalength) - { - // look for a start of NMEA packet - if(bufferGetAtIndex(rxBuffer,0) == '$') - { - // found start - startFlag = TRUE; - // when start is found, we leave it intact in the receive buffer - // in case the full NMEA string is not completely received. The - // start will be detected in the next nmeaProcess iteration. + // process the receive buffer + // go through buffer looking for packets + while (rxBuffer->datalength) { + // look for a start of NMEA packet + if (bufferGetAtIndex(rxBuffer, 0) == '$') { + // found start + startFlag = TRUE; + // when start is found, we leave it intact in the receive buffer + // in case the full NMEA string is not completely received. The + // start will be detected in the next nmeaProcess iteration. - // done looking for start - break; - } - else + // done looking for start + break; + } else { + bufferGetFromFront(rxBuffer); + } + } + + // if we detected a start, look for end of packet + if (startFlag) { + for (i = 1; i < (rxBuffer->datalength) - 1; i++) { + // check for end of NMEA packet + if ((bufferGetAtIndex(rxBuffer, i) == '\r') && (bufferGetAtIndex(rxBuffer, i + 1) == '\n')) { + // have a packet end + // dump initial '$' + bufferGetFromFront(rxBuffer); + // copy packet to NmeaPacket + for (j = 0; j < (i - 1); j++) { + // although NMEA strings should be 80 characters or less, + // receive buffer errors can generate erroneous packets. + // Protect against packet buffer overflow + if (j < (NMEA_BUFFERSIZE - 1)) { + NmeaPacket[j] = bufferGetFromFront(rxBuffer); + } else { bufferGetFromFront(rxBuffer); - } - - // if we detected a start, look for end of packet - if(startFlag) - { - for(i=1; i<(rxBuffer->datalength)-1; i++) - { - // check for end of NMEA packet - if((bufferGetAtIndex(rxBuffer,i) == '\r') && (bufferGetAtIndex(rxBuffer,i+1) == '\n')) - { - // have a packet end - // dump initial '$' - bufferGetFromFront(rxBuffer); - // copy packet to NmeaPacket - for(j=0; j<(i-1); j++) - { - // although NMEA strings should be 80 characters or less, - // receive buffer errors can generate erroneous packets. - // Protect against packet buffer overflow - if(j<(NMEA_BUFFERSIZE-1)) - NmeaPacket[j] = bufferGetFromFront(rxBuffer); - else - bufferGetFromFront(rxBuffer); - } - // null terminate it - if (j<(NMEA_BUFFERSIZE-1)) { - NmeaPacket[j] = 0; - } else { - NmeaPacket[NMEA_BUFFERSIZE-1] = 0; - } - // dump from rxBuffer - bufferGetFromFront(rxBuffer); - bufferGetFromFront(rxBuffer); - //DEBUG + } + } + // null terminate it + if (j < (NMEA_BUFFERSIZE - 1)) { + NmeaPacket[j] = 0; + } else { + NmeaPacket[NMEA_BUFFERSIZE - 1] = 0; + } + // dump from rxBuffer + bufferGetFromFront(rxBuffer); + bufferGetFromFront(rxBuffer); + // DEBUG #ifdef NMEA_DEBUG_PKT - qDebug() << NmeaPacket; + qDebug() << NmeaPacket; #endif - emit packet(QString(NmeaPacket)); - // found a packet - // done with this processing session - foundpacket = NMEA_UNKNOWN; - break; - } - } + emit packet(QString(NmeaPacket)); + // found a packet + // done with this processing session + foundpacket = NMEA_UNKNOWN; + break; + } } + } - if(foundpacket) - { - // check message type and process appropriately - if(!strncmp(NmeaPacket, "GPGGA", 5)) - { - // process packet of this type - nmeaProcessGPGGA(NmeaPacket); - // report packet type - foundpacket = NMEA_GPGGA; - } - else if(!strncmp(NmeaPacket, "GPVTG", 5)) - { - // process packet of this type - nmeaProcessGPVTG(NmeaPacket); - // report packet type - foundpacket = NMEA_GPVTG; - } - else if(!strncmp(NmeaPacket, "GPGSA", 5)) - { - // process packet of this type - nmeaProcessGPGSA(NmeaPacket); - // report packet type - foundpacket = NMEA_GPGSA; - } - else if(!strncmp(NmeaPacket, "GPRMC", 5)) - { - // process packet of this type - nmeaProcessGPRMC(NmeaPacket); - // report packet type - foundpacket = NMEA_GPRMC; - } - else if(!strncmp(NmeaPacket, "GPGSV", 5)) - { - // Process packet of this type - nmeaProcessGPGSV(NmeaPacket); - // rerpot packet type - foundpacket = NMEA_GPGSV; - } - else if(!strncmp(NmeaPacket, "GPZDA", 5)) - { - // Process packet of this type - nmeaProcessGPZDA(NmeaPacket); - // rerpot packet type - foundpacket = NMEA_GPZDA; - } + if (foundpacket) { + // check message type and process appropriately + if (!strncmp(NmeaPacket, "GPGGA", 5)) { + // process packet of this type + nmeaProcessGPGGA(NmeaPacket); + // report packet type + foundpacket = NMEA_GPGGA; + } else if (!strncmp(NmeaPacket, "GPVTG", 5)) { + // process packet of this type + nmeaProcessGPVTG(NmeaPacket); + // report packet type + foundpacket = NMEA_GPVTG; + } else if (!strncmp(NmeaPacket, "GPGSA", 5)) { + // process packet of this type + nmeaProcessGPGSA(NmeaPacket); + // report packet type + foundpacket = NMEA_GPGSA; + } else if (!strncmp(NmeaPacket, "GPRMC", 5)) { + // process packet of this type + nmeaProcessGPRMC(NmeaPacket); + // report packet type + foundpacket = NMEA_GPRMC; + } else if (!strncmp(NmeaPacket, "GPGSV", 5)) { + // Process packet of this type + nmeaProcessGPGSV(NmeaPacket); + // rerpot packet type + foundpacket = NMEA_GPGSV; + } else if (!strncmp(NmeaPacket, "GPZDA", 5)) { + // Process packet of this type + nmeaProcessGPZDA(NmeaPacket); + // rerpot packet type + foundpacket = NMEA_GPZDA; } - else if(rxBuffer->datalength >= rxBuffer->size) - { - // if we found no packet, and the buffer is full - // we're logjammed, flush entire buffer - bufferFlush(rxBuffer); - } - return foundpacket; + } else if (rxBuffer->datalength >= rxBuffer->size) { + // if we found no packet, and the buffer is full + // we're logjammed, flush entire buffer + bufferFlush(rxBuffer); + } + return foundpacket; } /** - * Processes NMEA GSV sentences (satellites in view) - * \param[in] Buffer for parsed nmea GSV sentence - */ + * Processes NMEA GSV sentences (satellites in view) + * \param[in] Buffer for parsed nmea GSV sentence + */ void NMEAParser::nmeaProcessGPGSV(char *packet) { - // start parsing just after "GPGSV," // attempt to reject empty packets right away - if(packet[6]==',' && packet[7]==',') - return; + if (packet[6] == ',' && packet[7] == ',') { + return; + } - if(!nmeaChecksum(packet)) - { - // checksum not valid - return; + if (!nmeaChecksum(packet)) { + // checksum not valid + return; } nmeaTerminateAtChecksum(packet); - QString nmeaString( packet ); + QString nmeaString(packet); QStringList tokenslist = nmeaString.split(","); @@ -302,21 +270,21 @@ void NMEAParser::nmeaProcessGPGSV(char *packet) const int sentence_total = tokenslist.at(1).toInt(); // Number of sentences for full data const int sentence_index = tokenslist.at(2).toInt(); // sentence x of y - int sats = (tokenslist.size() - 4) /4; - for(int sat = 0; sat < sats; sat++) { - int base = 4+sat*4; - const int id = tokenslist.at(base+0).toInt(); // Satellite PRN number - const int elv = tokenslist.at(base+1).toInt(); // Elevation, degrees - const int azimuth = tokenslist.at(base+2).toInt(); // Azimuth, degrees - const int sig = tokenslist.at(base+3).toInt(); // SNR - higher is better - const int index = (sentence_index-1) * 4 + sat; + int sats = (tokenslist.size() - 4) / 4; + for (int sat = 0; sat < sats; sat++) { + int base = 4 + sat * 4; + const int id = tokenslist.at(base + 0).toInt(); // Satellite PRN number + const int elv = tokenslist.at(base + 1).toInt(); // Elevation, degrees + const int azimuth = tokenslist.at(base + 2).toInt(); // Azimuth, degrees + const int sig = tokenslist.at(base + 3).toInt(); // SNR - higher is better + const int index = (sentence_index - 1) * 4 + sat; emit satellite(index, id, elv, azimuth, sig); } - if(sentence_index == sentence_total) { + if (sentence_index == sentence_total) { // Last sentence - int total_sats = (sentence_index-1) * 4 + sats; - for(int emptySatIndex = total_sats; emptySatIndex < 16; emptySatIndex++) { + int total_sats = (sentence_index - 1) * 4 + sats; + for (int emptySatIndex = total_sats; emptySatIndex < 16; emptySatIndex++) { // Wipe the rest. emit satellite(emptySatIndex, 0, 0, 0, 0); } @@ -327,76 +295,79 @@ void NMEAParser::nmeaProcessGPGSV(char *packet) * Prosesses NMEA GPGGA sentences * \param[in] Buffer for parsed nmea GPGGA sentence */ -void NMEAParser::nmeaProcessGPGGA(char* packet) +void NMEAParser::nmeaProcessGPGGA(char *packet) { - // start parsing just after "GPGGA," - // attempt to reject empty packets right away - if(packet[6]==',' && packet[7]==',') - return; + // start parsing just after "GPGGA," + // attempt to reject empty packets right away + if (packet[6] == ',' && packet[7] == ',') { + return; + } - if(!nmeaChecksum(packet)) - { - // checksum not valid - return; - } - nmeaTerminateAtChecksum(packet); + if (!nmeaChecksum(packet)) { + // checksum not valid + return; + } + nmeaTerminateAtChecksum(packet); - QString nmeaString( packet ); - QStringList tokenslist = nmeaString.split(","); - GpsData.GPStime = tokenslist.at(1).toDouble(); - GpsData.Latitude = tokenslist.at(2).toDouble(); - int deg = (int)GpsData.Latitude/100; - double min = ((GpsData.Latitude)-(deg*100))/60.0; - GpsData.Latitude=deg+min; - // next field: N/S indicator - // correct latitute for N/S - if(tokenslist.at(3).contains("S")) GpsData.Latitude = -GpsData.Latitude; + QString nmeaString(packet); + QStringList tokenslist = nmeaString.split(","); + GpsData.GPStime = tokenslist.at(1).toDouble(); + GpsData.Latitude = tokenslist.at(2).toDouble(); + int deg = (int)GpsData.Latitude / 100; + double min = ((GpsData.Latitude) - (deg * 100)) / 60.0; + GpsData.Latitude = deg + min; + // next field: N/S indicator + // correct latitute for N/S + if (tokenslist.at(3).contains("S")) { + GpsData.Latitude = -GpsData.Latitude; + } - GpsData.Longitude = tokenslist.at(4).toDouble(); - deg = (int)GpsData.Longitude/100; - min = ((GpsData.Longitude)-(deg*100))/60.0; - GpsData.Longitude=deg+min; - // next field: E/W indicator - // correct latitute for E/W - if(tokenslist.at(5).contains("W")) GpsData.Longitude = -GpsData.Longitude; + GpsData.Longitude = tokenslist.at(4).toDouble(); + deg = (int)GpsData.Longitude / 100; + min = ((GpsData.Longitude) - (deg * 100)) / 60.0; + GpsData.Longitude = deg + min; + // next field: E/W indicator + // correct latitute for E/W + if (tokenslist.at(5).contains("W")) { + GpsData.Longitude = -GpsData.Longitude; + } - GpsData.SV = tokenslist.at(7).toInt(); - - GpsData.Altitude = tokenslist.at(9).toDouble(); - GpsData.GeoidSeparation = tokenslist.at(11).toDouble(); - emit position(GpsData.Latitude,GpsData.Longitude,GpsData.Altitude); - emit sv(GpsData.SV); - emit datetime(GpsData.GPSdate,GpsData.GPStime); + GpsData.SV = tokenslist.at(7).toInt(); + GpsData.Altitude = tokenslist.at(9).toDouble(); + GpsData.GeoidSeparation = tokenslist.at(11).toDouble(); + emit position(GpsData.Latitude, GpsData.Longitude, GpsData.Altitude); + emit sv(GpsData.SV); + emit datetime(GpsData.GPSdate, GpsData.GPStime); } /** * Prosesses NMEA GPRMC sentences * \param[in] Buffer for parsed nmea GPRMC sentence */ -void NMEAParser::nmeaProcessGPRMC(char* packet) +void NMEAParser::nmeaProcessGPRMC(char *packet) { - // start parsing just after "GPRMC," - // attempt to reject empty packets right away - if(packet[6]==',' && packet[7]==',') - return; + // start parsing just after "GPRMC," + // attempt to reject empty packets right away + if (packet[6] == ',' && packet[7] == ',') { + return; + } - if(!nmeaChecksum(packet)) - { - // checksum not valid - return; - } - nmeaTerminateAtChecksum(packet); + if (!nmeaChecksum(packet)) { + // checksum not valid + return; + } + nmeaTerminateAtChecksum(packet); - QString nmeaString( packet ); - QStringList tokenslist = nmeaString.split(","); - GpsData.GPStime = tokenslist.at(1).toDouble(); - GpsData.Groundspeed = tokenslist.at(7).toDouble(); - GpsData.Groundspeed = GpsData.Groundspeed*0.51444; - GpsData.Heading = tokenslist.at(8).toDouble(); - GpsData.GPSdate = tokenslist.at(9).toDouble(); - emit datetime(GpsData.GPSdate,GpsData.GPStime); - emit speedheading(GpsData.Groundspeed,GpsData.Heading); + QString nmeaString(packet); + QStringList tokenslist = nmeaString.split(","); + GpsData.GPStime = tokenslist.at(1).toDouble(); + GpsData.Groundspeed = tokenslist.at(7).toDouble(); + GpsData.Groundspeed = GpsData.Groundspeed * 0.51444; + GpsData.Heading = tokenslist.at(8).toDouble(); + GpsData.GPSdate = tokenslist.at(9).toDouble(); + emit datetime(GpsData.GPSdate, GpsData.GPStime); + emit speedheading(GpsData.Groundspeed, GpsData.Heading); } @@ -404,111 +375,113 @@ void NMEAParser::nmeaProcessGPRMC(char* packet) * Prosesses NMEA GPVTG sentences * \param[in] Buffer for parsed nmea GPVTG sentence */ -void NMEAParser::nmeaProcessGPVTG(char* packet) +void NMEAParser::nmeaProcessGPVTG(char *packet) { - // start parsing just after "GPVTG," - // attempt to reject empty packets right away - if(packet[6]==',' && packet[7]==',') - return; + // start parsing just after "GPVTG," + // attempt to reject empty packets right away + if (packet[6] == ',' && packet[7] == ',') { + return; + } - if(!nmeaChecksum(packet)) - { - // checksum not valid - return; - } - nmeaTerminateAtChecksum(packet); + if (!nmeaChecksum(packet)) { + // checksum not valid + return; + } + nmeaTerminateAtChecksum(packet); - QString nmeaString( packet ); - QStringList tokenslist = nmeaString.split(","); + QString nmeaString(packet); + QStringList tokenslist = nmeaString.split(","); - GpsData.Heading = tokenslist.at(1).toDouble(); - GpsData.Groundspeed = tokenslist.at(7).toDouble(); - GpsData.Groundspeed = GpsData.Groundspeed/3.6; - emit speedheading(GpsData.Groundspeed,GpsData.Heading); + GpsData.Heading = tokenslist.at(1).toDouble(); + GpsData.Groundspeed = tokenslist.at(7).toDouble(); + GpsData.Groundspeed = GpsData.Groundspeed / 3.6; + emit speedheading(GpsData.Groundspeed, GpsData.Heading); } /** * Prosesses NMEA GPGSA sentences * \param[in] Buffer for parsed nmea GPGSA sentence */ -void NMEAParser::nmeaProcessGPGSA(char* packet) +void NMEAParser::nmeaProcessGPGSA(char *packet) { - // start parsing just after "GPGSA," - // attempt to reject empty packets right away - if(packet[6]==',' && packet[7]==',') - return; + // start parsing just after "GPGSA," + // attempt to reject empty packets right away + if (packet[6] == ',' && packet[7] == ',') { + return; + } - if(!nmeaChecksum(packet)) { - // checksum not valid - return; + if (!nmeaChecksum(packet)) { + // checksum not valid + return; + } + nmeaTerminateAtChecksum(packet); + + QString nmeaString(packet); + QStringList tokenslist = nmeaString.split(","); + + // M=Manual, forced to operate in 2D or 3D + // A=Automatic, 3D/2D + QString fixmodeValue = tokenslist.at(1); + if (fixmodeValue == "A") { + emit fixmode(QString("Auto")); + } else if (fixmodeValue == "B") { + emit fixmode(QString("Manual")); + } + + // Mode: 1=Fix not available, 2=2D, 3=3D + int fixtypeValue = tokenslist.at(2).toInt(); + if (fixtypeValue == 1) { + emit fixtype(QString("NoFix")); + } else if (fixtypeValue == 2) { + emit fixtype(QString("Fix2D")); + } else if (fixtypeValue == 3) { + emit fixtype(QString("Fix3D")); + } + + // 3-14 = IDs of SVs used in position fix (null for unused fields) + QList svList; + for (int pos = 0; pos < 12; pos++) { + QString sv = tokenslist.at(3 + pos); + if (!sv.isEmpty()) { + svList.append(sv.toInt()); } - nmeaTerminateAtChecksum(packet); + } + emit fixSVs(svList); - QString nmeaString( packet ); - QStringList tokenslist = nmeaString.split(","); - - // M=Manual, forced to operate in 2D or 3D - // A=Automatic, 3D/2D - QString fixmodeValue = tokenslist.at(1); - if (fixmodeValue == "A") { - emit fixmode(QString("Auto")); - } else if (fixmodeValue == "B") { - emit fixmode(QString("Manual")); - } - - // Mode: 1=Fix not available, 2=2D, 3=3D - int fixtypeValue = tokenslist.at(2).toInt(); - if (fixtypeValue == 1) { - emit fixtype(QString("NoFix")); - } else if (fixtypeValue == 2) { - emit fixtype(QString("Fix2D")); - } else if (fixtypeValue == 3) { - emit fixtype(QString("Fix3D")); - } - - // 3-14 = IDs of SVs used in position fix (null for unused fields) - QList svList; - for(int pos = 0; pos < 12;pos ++) { - QString sv = tokenslist.at(3+pos); - if(!sv.isEmpty()) { - svList.append(sv.toInt()); - } - } - emit fixSVs(svList); - - // 15 = PDOP - // 16 = HDOP - // 17 = VDOP - GpsData.PDOP = tokenslist.at(15).toDouble(); - GpsData.HDOP = tokenslist.at(16).toDouble(); - GpsData.VDOP = tokenslist.at(17).toDouble(); - emit dop(GpsData.HDOP, GpsData.VDOP, GpsData.PDOP); + // 15 = PDOP + // 16 = HDOP + // 17 = VDOP + GpsData.PDOP = tokenslist.at(15).toDouble(); + GpsData.HDOP = tokenslist.at(16).toDouble(); + GpsData.VDOP = tokenslist.at(17).toDouble(); + emit dop(GpsData.HDOP, GpsData.VDOP, GpsData.PDOP); } /** * Prosesses NMEA GPZDA sentences * \param[in] Buffer for parsed nmea GPZDA sentence */ -void NMEAParser::nmeaProcessGPZDA(char* packet) +void NMEAParser::nmeaProcessGPZDA(char *packet) { - // start parsing just after "GPZDA," - // attempt to reject empty packets right away - if(packet[6]==',' && packet[7]==',') - return; + // start parsing just after "GPZDA," + // attempt to reject empty packets right away + if (packet[6] == ',' && packet[7] == ',') { + return; + } - if(!nmeaChecksum(packet)) { - // checksum not valid - return; - } - nmeaTerminateAtChecksum(packet); + if (!nmeaChecksum(packet)) { + // checksum not valid + return; + } + nmeaTerminateAtChecksum(packet); - QString nmeaString( packet ); - QStringList tokenslist = nmeaString.split(","); + QString nmeaString(packet); + QStringList tokenslist = nmeaString.split(","); - GpsData.GPStime = tokenslist.at(1).toDouble(); - int day = tokenslist.at(2).toInt(); - int month = tokenslist.at(3).toInt(); - int year = tokenslist.at(4).toInt(); - GpsData.GPSdate = day*10000+month*100+(year-2000); - emit datetime(GpsData.GPSdate,GpsData.GPStime); + GpsData.GPStime = tokenslist.at(1).toDouble(); + int day = tokenslist.at(2).toInt(); + int month = tokenslist.at(3).toInt(); + int year = tokenslist.at(4).toInt(); + GpsData.GPSdate = day * 10000 + month * 100 + (year - 2000); + emit datetime(GpsData.GPSdate, GpsData.GPStime); } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.h b/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.h index ad4f29ab6..158704333 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.h @@ -35,52 +35,48 @@ #include "gpsparser.h" // constants/macros/typdefs -#define NMEA_BUFFERSIZE 128 +#define NMEA_BUFFERSIZE 128 -typedef struct struct_GpsData -{ - double Latitude; - double Longitude; - double Altitude; - double Groundspeed; - double Heading; - int SV; - int Status; - double PDOP; - double HDOP; - double VDOP; - double GeoidSeparation; - double GPStime; - double GPSdate; +typedef struct struct_GpsData { + double Latitude; + double Longitude; + double Altitude; + double Groundspeed; + double Heading; + int SV; + int Status; + double PDOP; + double HDOP; + double VDOP; + double GeoidSeparation; + double GPStime; + double GPSdate; +} GpsData_t; -}GpsData_t; - -class NMEAParser: public GPSParser -{ +class NMEAParser : public GPSParser { Q_OBJECT public: - NMEAParser(QObject *parent = 0); - ~NMEAParser(); - void processInputStream(char c); - char* nmeaGetPacketBuffer(void); - char nmeaChecksum(char* gps_buffer); - void nmeaTerminateAtChecksum(char* gps_buffer); - uint8_t nmeaProcess(cBuffer* rxBuffer); - void nmeaProcessGPGGA(char* packet); - void nmeaProcessGPRMC(char* packet); - void nmeaProcessGPVTG(char* packet); - void nmeaProcessGPGSA(char* packet); - void nmeaProcessGPGSV(char* packet); - void nmeaProcessGPZDA(char* packet); - GpsData_t GpsData; - cBuffer gpsRxBuffer; - char gpsRxData[512]; - char NmeaPacket[NMEA_BUFFERSIZE]; - uint32_t numUpdates; - uint32_t numErrors; - int32_t gpsRxOverflow; - + NMEAParser(QObject *parent = 0); + ~NMEAParser(); + void processInputStream(char c); + char *nmeaGetPacketBuffer(void); + char nmeaChecksum(char *gps_buffer); + void nmeaTerminateAtChecksum(char *gps_buffer); + uint8_t nmeaProcess(cBuffer *rxBuffer); + void nmeaProcessGPGGA(char *packet); + void nmeaProcessGPRMC(char *packet); + void nmeaProcessGPVTG(char *packet); + void nmeaProcessGPGSA(char *packet); + void nmeaProcessGPGSV(char *packet); + void nmeaProcessGPZDA(char *packet); + GpsData_t GpsData; + cBuffer gpsRxBuffer; + char gpsRxData[512]; + char NmeaPacket[NMEA_BUFFERSIZE]; + uint32_t numUpdates; + uint32_t numErrors; + int32_t gpsRxOverflow; }; #endif // NMEAPARSER_H diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp index b647977c0..70fc4bc1a 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp @@ -39,47 +39,47 @@ TelemetryParser::TelemetryParser(QObject *parent) : GPSParser(parent) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject *gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + UAVDataObject *gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + if (gpsObj != NULL) { - connect(gpsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateGPS(UAVObject*))); + connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGPS(UAVObject *))); } else { qDebug() << "Error: Object is unknown (GPSPosition)."; } - gpsObj = dynamic_cast(objManager->getObject("GPSTime")); + gpsObj = dynamic_cast(objManager->getObject("GPSTime")); if (gpsObj != NULL) { - connect(gpsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateTime(UAVObject*))); + connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateTime(UAVObject *))); } else { qDebug() << "Error: Object is unknown (GPSTime)."; } - gpsObj = dynamic_cast(objManager->getObject("GPSSatellites")); + gpsObj = dynamic_cast(objManager->getObject("GPSSatellites")); if (gpsObj != NULL) { - connect(gpsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateSats(UAVObject*))); + connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateSats(UAVObject *))); } - } TelemetryParser::~TelemetryParser() +{} + + +void TelemetryParser::updateGPS(UAVObject *object1) { - -} - - -void TelemetryParser::updateGPS( UAVObject* object1) { - UAVObjectField* field = object1->getField(QString("Satellites")); + UAVObjectField *field = object1->getField(QString("Satellites")); emit sv(field->getValue().toInt()); double lat = object1->getField(QString("Latitude"))->getDouble(); double lon = object1->getField(QString("Longitude"))->getDouble(); double alt = object1->getField(QString("Altitude"))->getDouble(); + lat *= 1E-7; lon *= 1E-7; - emit position(lat,lon,alt); + emit position(lat, lon, alt); - double hdg = object1->getField(QString("Heading"))->getDouble(); - double spd = object1->getField(QString("Groundspeed"))->getDouble(); - emit speedheading(spd,hdg); + double hdg = object1->getField(QString("Heading"))->getDouble(); + double spd = object1->getField(QString("Groundspeed"))->getDouble(); + emit speedheading(spd, hdg); QString fix = object1->getField(QString("Status"))->getValue().toString(); emit fixtype(fix); @@ -87,39 +87,38 @@ void TelemetryParser::updateGPS( UAVObject* object1) { double hdop = object1->getField(QString("HDOP"))->getDouble(); double vdop = object1->getField(QString("VDOP"))->getDouble(); double pdop = object1->getField(QString("PDOP"))->getDouble(); - emit dop(hdop,vdop,pdop); - - + emit dop(hdop, vdop, pdop); } -void TelemetryParser::updateTime( UAVObject* object1) { - double hour = object1->getField(QString("Hour"))->getDouble(); +void TelemetryParser::updateTime(UAVObject *object1) +{ + double hour = object1->getField(QString("Hour"))->getDouble(); double minute = object1->getField(QString("Minute"))->getDouble(); double second = object1->getField(QString("Second"))->getDouble(); - double time = second + minute*100 + hour*10000; - double year = object1->getField(QString("Year"))->getDouble(); - double month = object1->getField(QString("Month"))->getDouble(); - double day = object1->getField(QString("Day"))->getDouble(); - double date = day + month * 100 + year * 10000; - emit datetime(date,time); + double time = second + minute * 100 + hour * 10000; + double year = object1->getField(QString("Year"))->getDouble(); + double month = object1->getField(QString("Month"))->getDouble(); + double day = object1->getField(QString("Day"))->getDouble(); + double date = day + month * 100 + year * 10000; + emit datetime(date, time); } /** - Updates the satellite constellation. + Updates the satellite constellation. - Not really optimized for now, we should only send updates for the stas - which have changed instead of updating everything... That said, Qt is supposed - to be able to optimize redraws anyway. - */ -void TelemetryParser::updateSats( UAVObject* object1) { - UAVObjectField* prn = object1->getField(QString("PRN")); - UAVObjectField* elevation = object1->getField(QString("Elevation")); - UAVObjectField* azimuth = object1->getField(QString("Azimuth")); - UAVObjectField* snr = object1->getField(QString("SNR")); + Not really optimized for now, we should only send updates for the stas + which have changed instead of updating everything... That said, Qt is supposed + to be able to optimize redraws anyway. + */ +void TelemetryParser::updateSats(UAVObject *object1) +{ + UAVObjectField *prn = object1->getField(QString("PRN")); + UAVObjectField *elevation = object1->getField(QString("Elevation")); + UAVObjectField *azimuth = object1->getField(QString("Azimuth")); + UAVObjectField *snr = object1->getField(QString("SNR")); - for (unsigned int i=0;i< prn->getNumElements();i++) { - emit satellite(i,prn->getValue(i).toInt(),elevation->getValue(i).toInt(), + for (unsigned int i = 0; i < prn->getNumElements(); i++) { + emit satellite(i, prn->getValue(i).toInt(), elevation->getValue(i).toInt(), azimuth->getValue(i).toInt(), snr->getValue(i).toInt()); } - } diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.h b/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.h index 67ae6ea17..4b9f26517 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.h +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.h @@ -36,20 +36,17 @@ #include "gpsparser.h" -class TelemetryParser: public GPSParser -{ - +class TelemetryParser : public GPSParser { Q_OBJECT - + public: - TelemetryParser(QObject *parent = 0); - ~TelemetryParser(); + TelemetryParser(QObject *parent = 0); + ~TelemetryParser(); public slots: - void updateGPS(UAVObject* object1); - void updateTime(UAVObject* object1); - void updateSats(UAVObject* object1); - + void updateGPS(UAVObject *object1); + void updateTime(UAVObject *object1); + void updateSats(UAVObject *object1); }; #endif // TELEMETRYPARSER_H diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/aerosimrcdatastruct.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/aerosimrcdatastruct.h index 697695bc2..402192979 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/aerosimrcdatastruct.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/aerosimrcdatastruct.h @@ -31,175 +31,171 @@ #include const quint8 AEROSIMRC_MAX_CHANNELS = 39; -const quint16 DBG_BUFFER_MAX_SIZE = 4096; +const quint16 DBG_BUFFER_MAX_SIZE = 4096; #define MAX_DLL_USER_MENU_ITEMS 16 #define OBSOLETE_MIT_COMMAND (1 << 0) #define OBSOLETE_MIT_CHECKBOX (1 << 1) #define OBSOLETE_MIT_SEPARATOR (1 << 7) -#define PACK_STRUCT __attribute__((packed)) +#define PACK_STRUCT __attribute__((packed)) -struct simToPlugin -{ - quint16 structSize; - float simTimeStep; - float chSimTX[AEROSIMRC_MAX_CHANNELS]; - float chSimRX[AEROSIMRC_MAX_CHANNELS]; - uchar *OSDVideoBuf; - quint32 simMenuStatus; - float initPosX; - float initPosY; - float initPosZ; - float initHeading; - float initPitch; - float initRoll; - float wpHomeX; - float wpHomeY; - float wpHomeLat; - float wpHomeLong; +struct simToPlugin { + quint16 structSize; + float simTimeStep; + float chSimTX[AEROSIMRC_MAX_CHANNELS]; + float chSimRX[AEROSIMRC_MAX_CHANNELS]; + uchar *OSDVideoBuf; + quint32 simMenuStatus; + float initPosX; + float initPosY; + float initPosZ; + float initHeading; + float initPitch; + float initRoll; + float wpHomeX; + float wpHomeY; + float wpHomeLat; + float wpHomeLong; const char *wpHomeDesc; // (m, deg, string) - float wpAX; - float wpAY; - float wpALat; - float wpALong; - const char *wpADesc; // (m, deg, string) - float wpBX; - float wpBY; - float wpBLat; - float wpBLong; - const char *wpBDesc; // (m, deg, string) - float wpCX; - float wpCY; - float wpCLat; - float wpCLong; - const char *wpCDesc; // (m, deg, string) - float wpDX; - float wpDY; - float wpDLat; - float wpDLong; - const char *wpDDesc; // (m, deg, string) - float posX; - float posY; - float posZ; - float velX; - float velY; - float velZ; - float angVelX; - float angVelY; - float angVelZ; - float accelX; - float accelY; - float accelZ; - qreal latitude; - qreal longitude; - float AGL; - float heading; - float pitch; - float roll; - float windVelX; - float windVelY; - float windVelZ; - float eng1RPM; - float eng2RPM; - float eng3RPM; - float eng4RPM; - float voltage; // V - float current; // A - float consumedCharge; // Ah - float capacity; // Ah - float fuelConsumed; // l - float fuelTankCapacity; // l + float wpAX; + float wpAY; + float wpALat; + float wpALong; + const char *wpADesc; // (m, deg, string) + float wpBX; + float wpBY; + float wpBLat; + float wpBLong; + const char *wpBDesc; // (m, deg, string) + float wpCX; + float wpCY; + float wpCLat; + float wpCLong; + const char *wpCDesc; // (m, deg, string) + float wpDX; + float wpDY; + float wpDLat; + float wpDLong; + const char *wpDDesc; // (m, deg, string) + float posX; + float posY; + float posZ; + float velX; + float velY; + float velZ; + float angVelX; + float angVelY; + float angVelZ; + float accelX; + float accelY; + float accelZ; + qreal latitude; + qreal longitude; + float AGL; + float heading; + float pitch; + float roll; + float windVelX; + float windVelY; + float windVelZ; + float eng1RPM; + float eng2RPM; + float eng3RPM; + float eng4RPM; + float voltage; // V + float current; // A + float consumedCharge; // Ah + float capacity; // Ah + float fuelConsumed; // l + float fuelTankCapacity; // l // ver 3.83 - qint16 screenW; - qint16 screenH; + qint16 screenW; + qint16 screenH; // Model Orientation Matrix (X=Right, Y=Front, Z=Up) - float axisXx; - float axisXy; - float axisXz; - float axisYx; - float axisYy; - float axisYz; - float axisZx; - float axisZy; - float axisZz; + float axisXx; + float axisXy; + float axisXz; + float axisYx; + float axisYy; + float axisYz; + float axisZx; + float axisZy; + float axisZz; // Model data in body frame coordinates (X=Right, Y=Front, Z=Up) - float velXm; // m/s Model velocity in body coordinates - float velYm; - float velZm; - float angVelXm; // rad/s Model angular velocity in body coordinates - float angVelYm; - float angVelZm; - float accelXm; // m/s/s Model acceleration in body coordinates - float accelYm; - float accelZm; + float velXm; // m/s Model velocity in body coordinates + float velYm; + float velZm; + float angVelXm; // rad/s Model angular velocity in body coordinates + float angVelYm; + float angVelZm; + float accelXm; // m/s/s Model acceleration in body coordinates + float accelYm; + float accelZm; // ver 3.90 - quint32 OSDVideoBufSize; -} PACK_STRUCT ; // normal - 592, packed - 582 OK (3.81) - // normal - ???, packed - 658 OK (3.83) - // normal - ???, packed - 662 OK (3.90) + quint32 OSDVideoBufSize; +} PACK_STRUCT; // normal - 592, packed - 582 OK (3.81) + // normal - ???, packed - 658 OK (3.83) + // normal - ???, packed - 662 OK (3.90) -struct pluginToSim -{ - quint16 structSize; +struct pluginToSim { + quint16 structSize; const char *dbgInfoText; - uchar chOverTX[AEROSIMRC_MAX_CHANNELS]; - float chNewTX[AEROSIMRC_MAX_CHANNELS]; - uchar chOverRX[AEROSIMRC_MAX_CHANNELS]; - float chNewRX[AEROSIMRC_MAX_CHANNELS]; - float newPosX; // m - float newPosY; - float newPosZ; - float newVelX; // m/s - float newVelY; - float newVelZ; - float newAngVelX; // rad/s - float newAngVelY; - float newAngVelZ; - float newHeading; // rad - float newPitch; - float newRoll; + uchar chOverTX[AEROSIMRC_MAX_CHANNELS]; + float chNewTX[AEROSIMRC_MAX_CHANNELS]; + uchar chOverRX[AEROSIMRC_MAX_CHANNELS]; + float chNewRX[AEROSIMRC_MAX_CHANNELS]; + float newPosX; // m + float newPosY; + float newPosZ; + float newVelX; // m/s + float newVelY; + float newVelZ; + float newAngVelX; // rad/s + float newAngVelY; + float newAngVelZ; + float newHeading; // rad + float newPitch; + float newRoll; quint32 modelOverrideFlags; quint32 newMenuStatus; - quint8 isOSDShow; - quint8 isOSDChanged; + quint8 isOSDShow; + quint8 isOSDChanged; quint16 OSDWindow_DX; quint16 OSDWindow_DY; - float OSDScale; - float newWindVelX; - float newWindVelY; - float newWindVelZ; - float newEng1RPM; - float newEng2RPM; - float newEng3RPM; - float newEng4RPM; - float newVoltage; - float newCurrent; - float newConsumedCharge; - float newFuelConsumed; - quint8 modelCrashInhibit; + float OSDScale; + float newWindVelX; + float newWindVelY; + float newWindVelZ; + float newEng1RPM; + float newEng2RPM; + float newEng3RPM; + float newEng4RPM; + float newVoltage; + float newCurrent; + float newConsumedCharge; + float newFuelConsumed; + quint8 modelCrashInhibit; // ver 3.83 - qint16 newScreenW; // Simulator window position and size on screen - qint16 newScreenH; - qint16 newScreenX; - qint16 newScreenY; -} PACK_STRUCT ; // normal 516, packed 507 OK (3.81) - // normal ???, packed 515 OK (3.83 & 3.90) + qint16 newScreenW; // Simulator window position and size on screen + qint16 newScreenH; + qint16 newScreenX; + qint16 newScreenY; +} PACK_STRUCT; // normal 516, packed 507 OK (3.81) + // normal ???, packed 515 OK (3.83 & 3.90) -struct TPluginMenuItem -{ +struct TPluginMenuItem { quint32 OBSOLETE_eType; - char *OBSOLETE_strName; -} PACK_STRUCT ; + char *OBSOLETE_strName; +} PACK_STRUCT; -struct pluginInit -{ - quint32 nStructSize; - char *OBSOLETE_strMenuTitle; +struct pluginInit { + quint32 nStructSize; + char *OBSOLETE_strMenuTitle; TPluginMenuItem OBSOLETE_atMenuItem[MAX_DLL_USER_MENU_ITEMS]; const char *strPluginFolder; const char *strOutputFolder; -} PACK_STRUCT ; // normal - 144, packed - 144 OK (3.81 & 3.83 & 3.90) +} PACK_STRUCT; // normal - 144, packed - 144 OK (3.81 & 3.83 & 3.90) #undef PACK_STRUCT diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/enums.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/enums.h index 31abb1e0a..bfcd874ad 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/enums.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/enums.h @@ -30,21 +30,20 @@ // Custom Menu Item masks enum MenuMasks { - MenuEnable = (1 << 0), - MenuTx = (1 << 1), - MenuRx = (1 << 2), - MenuScreen = (1 << 3), - MenuNextWpt = (1 << 4), - MenuCmdReset = (1 << 5), - MenuLedBlue = (1 << 6), - MenuLedGreen = (1 << 7), - MenuFMode1 = (1 << 8), - MenuFMode2 = (1 << 9), - MenuFMode3 = (1 << 10) + MenuEnable = (1 << 0), + MenuTx = (1 << 1), + MenuRx = (1 << 2), + MenuScreen = (1 << 3), + MenuNextWpt = (1 << 4), + MenuCmdReset = (1 << 5), + MenuLedBlue = (1 << 6), + MenuLedGreen = (1 << 7), + MenuFMode1 = (1 << 8), + MenuFMode2 = (1 << 9), + MenuFMode3 = (1 << 10) }; -enum EOverrideFlags -{ +enum EOverrideFlags { OVR_POS = (1 << 0), OVR_VEL = (1 << 1), OVR_ANG_VEL = (1 << 2), @@ -53,7 +52,7 @@ enum EOverrideFlags OVR_ENGINE_RPM = (1 << 5), // Override RPM of all Engines or Motors OVR_BAT_VOLT = (1 << 6), // Override motor Battery Voltage OVR_BAT_AMP = (1 << 7), // Override motor Battery current - OVR_BAT_AH_CONSUMED = (1 << 8), // Override motor Battery AmpsHour consumed + OVR_BAT_AH_CONSUMED = (1 << 8), // Override motor Battery AmpsHour consumed OVR_FUEL_CONSUMED = (1 << 9) // Override Fuel consumed (gas & jet engines) }; diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.cpp index 18ba2c15d..8aac55d6b 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.cpp @@ -45,12 +45,12 @@ UdpReceiver *rcvr; const float RAD2DEG = (float)(180.0 / M_PI); const float DEG2RAD = (float)(M_PI / 180.0); -//extern "C" int __stdcall DllMain(HINSTANCE hinstDLL, DWORD fdwReason, LPVOID lpvReserved) -extern "C" int __stdcall DllMain(void*, quint32 fdwReason, void*) +// extern "C" int __stdcall DllMain(HINSTANCE hinstDLL, DWORD fdwReason, LPVOID lpvReserved) +extern "C" int __stdcall DllMain(void *, quint32 fdwReason, void *) { switch (fdwReason) { case 0: -// qDebug() << hinstDLL << "DLL_PROCESS_DETACH " << lpvReserved; +// qDebug() << hinstDLL << "DLL_PROCESS_DETACH " << lpvReserved; // free resources here rcvr->stop(); rcvr->wait(500); @@ -59,13 +59,13 @@ extern "C" int __stdcall DllMain(void*, quint32 fdwReason, void*) qDebug("------"); break; case 1: -// qDebug() << hinstDLL << " DLL_PROCESS_ATTACH " << lpvReserved; +// qDebug() << hinstDLL << " DLL_PROCESS_ATTACH " << lpvReserved; break; case 2: -// qDebug() << hinstDLL << "DLL_THREAD_ATTACH " << lpvReserved; +// qDebug() << hinstDLL << "DLL_THREAD_ATTACH " << lpvReserved; break; case 3: -// qDebug() << hinstDLL << "DLL_THREAD_DETACH " << lpvReserved; +// qDebug() << hinstDLL << "DLL_THREAD_DETACH " << lpvReserved; break; } return true; @@ -115,28 +115,28 @@ SIM_DLL_EXPORT void AeroSIMRC_Plugin_Init(pluginInit *p) qDebug() << "AeroSIMRC_Plugin_Init done"; } -//----------------------------------------------------------------------------- +// ----------------------------------------------------------------------------- -void Run_Command_Reset(/*const simToPlugin *stp, - pluginToSim *pts*/) +void Run_Command_Reset( /*const simToPlugin *stp, + pluginToSim *pts*/) { // Print some debug info, although it will only be seen during one frame debugInfo.append("\nRESET"); } void Run_Command_WindowSizeAndPos(const simToPlugin *stp, - pluginToSim *pts) + pluginToSim *pts) { static quint8 snSequence = 0; quint8 idx = snSequence * 4; - if (snSequence >= videoModes.at(0)) { // set fullscreen + if (snSequence >= videoModes.at(0)) { // set fullscreen pts->newScreenX = 0; pts->newScreenY = 0; pts->newScreenW = stp->screenW; pts->newScreenH = stp->screenH; snSequence = 0; - } else { // set video mode from config + } else { // set video mode from config pts->newScreenX = videoModes.at(idx + 1); pts->newScreenY = videoModes.at(idx + 2); pts->newScreenW = videoModes.at(idx + 3); @@ -146,11 +146,11 @@ void Run_Command_WindowSizeAndPos(const simToPlugin *stp, } void Run_Command_MoveToNextWaypoint(const simToPlugin *stp, - pluginToSim *pts) + pluginToSim *pts) { static quint8 snSequence = 0; - switch(snSequence) { + switch (snSequence) { case 0: pts->newPosX = stp->wpAX; pts->newPosY = stp->wpAY; @@ -179,16 +179,17 @@ void Run_Command_MoveToNextWaypoint(const simToPlugin *stp, default: qFatal("Run_Command_MoveToNextWaypoint switch error"); } - pts->modelOverrideFlags = 0; + pts->modelOverrideFlags = 0; pts->modelOverrideFlags |= OVR_POS; snSequence++; - if(snSequence > 4) + if (snSequence > 4) { snSequence = 0; + } } void Run_BlinkLEDs(const simToPlugin *stp, - pluginToSim *pts) + pluginToSim *pts) { if ((stp->simMenuStatus & MenuEnable) != 0) { pts->newMenuStatus |= MenuLedGreen; @@ -198,14 +199,15 @@ void Run_BlinkLEDs(const simToPlugin *stp, rcvr->getFlighStatus(armed, mode); debugInfo.append(QString("armed: %1, mode: %2\n").arg(armed).arg(mode)); - if (armed == 0) // disarm + if (armed == 0) { // disarm timeout = 1000; - else if (armed == 1) // arming + } else if (armed == 1) { // arming timeout = 40; - else if (armed == 2) // armed + } else if (armed == 2) { // armed timeout = 100; - else // unknown + } else { // unknown timeout = 2000; + } if (ledTimer.elapsed() > timeout) { ledTimer.restart(); pts->newMenuStatus ^= MenuLedBlue; @@ -235,7 +237,7 @@ void Run_BlinkLEDs(const simToPlugin *stp, pts->newMenuStatus &= ~MenuFMode3; pts->newMenuStatus &= ~MenuFMode2; pts->newMenuStatus |= MenuFMode1; - } else /*(mode == 0)*/ { + } else { /*(mode == 0)*/ pts->newMenuStatus &= ~MenuFMode3; pts->newMenuStatus &= ~MenuFMode2; pts->newMenuStatus &= ~MenuFMode1; @@ -246,112 +248,112 @@ void Run_BlinkLEDs(const simToPlugin *stp, } void InfoText(const simToPlugin *stp, - pluginToSim *pts) + pluginToSim *pts) { debugInfo.append( - QString( - "Plugin Folder = %1\n" - "Output Folder = %2\n" - "nStructSize = %3 " - "fIntegrationTimeStep = %4\n" - "\n" - "Aileron TX = %5 RX = %6 RCMD TX = %7 RX = %8\n" - "Elevator TX = %9 RX = %10 RCMD TX = %11 RX = %12\n" - "Throttle TX = %13 RX = %14 RCMD TX = %15 RX = %16\n" - "Rudder TX = %17 RX = %18 RCMD TX = %19 RX = %20\n" - "Channel5 TX = %21 RX = %22 RCMD TX = %23 RX = %24\n" - "Channel6 TX = %25 RX = %26 RCMD TX = %27 RX = %28\n" - "Channel7 TX = %29 RX = %30 RCMD TX = %31 RX = %32\n" - "PluginCh1 TX = %33 RX = %34 RCMD TX = %35 RX = %36\n" - "PluginCh2 TX = %37 RX = %38 RCMD TX = %39 RX = %40\n" - "FPVCamPan TX = %41 RX = %42 RCMD TX = %43 RX = %44\n" - "FPVCamTil TX = %45 RX = %46 RCMD TX = %47 RX = %48\n" - "\n" - "MenuItems = %49\n" - // Model data - "\n" - "fPosX,Y,Z = (%50, %51, %52)\n" - "fVelX,Y,Z = (%53, %54, %55)\n" - "fAngVelX,Y,Z = (%56, %57, %58)\n" - "fAccelX,Y,Z = (%59, %60, %61)\n" - "\n" - "Lat, Long = %62, %63\n" - "fHeightAboveTerrain = %64\n" - "\n" - "fHeading = %65 fPitch = %66 fRoll = %67\n" - ) - .arg(pluginFolder) - .arg(outputFolder) - .arg(stp->structSize) - .arg(1.0 / stp->simTimeStep, 4, 'f', 1) - .arg(stp->chSimTX[Ch1Aileron], 5, 'f', 2) - .arg(stp->chSimRX[Ch1Aileron], 5, 'f', 2) - .arg(pts->chNewTX[Ch1Aileron], 5, 'f', 2) - .arg(pts->chNewRX[Ch1Aileron], 5, 'f', 2) - .arg(stp->chSimTX[Ch2Elevator], 5, 'f', 2) - .arg(stp->chSimRX[Ch2Elevator], 5, 'f', 2) - .arg(pts->chNewTX[Ch2Elevator], 5, 'f', 2) - .arg(pts->chNewRX[Ch2Elevator], 5, 'f', 2) - .arg(stp->chSimTX[Ch3Throttle], 5, 'f', 2) - .arg(stp->chSimRX[Ch3Throttle], 5, 'f', 2) - .arg(pts->chNewTX[Ch3Throttle], 5, 'f', 2) - .arg(pts->chNewRX[Ch3Throttle], 5, 'f', 2) - .arg(stp->chSimTX[Ch4Rudder], 5, 'f', 2) - .arg(stp->chSimRX[Ch4Rudder], 5, 'f', 2) - .arg(pts->chNewTX[Ch4Rudder], 5, 'f', 2) - .arg(pts->chNewRX[Ch4Rudder], 5, 'f', 2) - .arg(stp->chSimTX[Ch5], 5, 'f', 2) - .arg(stp->chSimRX[Ch5], 5, 'f', 2) - .arg(pts->chNewTX[Ch5], 5, 'f', 2) - .arg(pts->chNewRX[Ch5], 5, 'f', 2) - .arg(stp->chSimTX[Ch6], 5, 'f', 2) - .arg(stp->chSimRX[Ch6], 5, 'f', 2) - .arg(pts->chNewTX[Ch6], 5, 'f', 2) - .arg(pts->chNewRX[Ch6], 5, 'f', 2) - .arg(stp->chSimTX[Ch7], 5, 'f', 2) - .arg(stp->chSimRX[Ch7], 5, 'f', 2) - .arg(pts->chNewTX[Ch7], 5, 'f', 2) - .arg(pts->chNewRX[Ch7], 5, 'f', 2) - .arg(stp->chSimTX[Ch23Plugin1], 5, 'f', 2) - .arg(stp->chSimRX[Ch23Plugin1], 5, 'f', 2) - .arg(pts->chNewTX[Ch23Plugin1], 5, 'f', 2) - .arg(pts->chNewRX[Ch23Plugin1], 5, 'f', 2) - .arg(stp->chSimTX[Ch24Plugin2], 5, 'f', 2) - .arg(stp->chSimRX[Ch24Plugin2], 5, 'f', 2) - .arg(pts->chNewTX[Ch24Plugin2], 5, 'f', 2) - .arg(pts->chNewRX[Ch24Plugin2], 5, 'f', 2) - .arg(stp->chSimTX[Ch12FPVCamPan], 5, 'f', 2) - .arg(stp->chSimRX[Ch12FPVCamPan], 5, 'f', 2) - .arg(pts->chNewTX[Ch12FPVCamPan], 5, 'f', 2) - .arg(pts->chNewRX[Ch12FPVCamPan], 5, 'f', 2) - .arg(stp->chSimTX[Ch13FPVCamTilt], 5, 'f', 2) - .arg(stp->chSimRX[Ch13FPVCamTilt], 5, 'f', 2) - .arg(pts->chNewTX[Ch13FPVCamTilt], 5, 'f', 2) - .arg(pts->chNewRX[Ch13FPVCamTilt], 5, 'f', 2) - .arg(stp->simMenuStatus) - .arg(stp->posX, 5, 'f', 2) - .arg(stp->posY, 5, 'f', 2) - .arg(stp->posZ, 5, 'f', 2) - .arg(stp->velX, 5, 'f', 2) - .arg(stp->velY, 5, 'f', 2) - .arg(stp->velZ, 5, 'f', 2) - .arg(stp->angVelXm, 5, 'f', 2) - .arg(stp->angVelYm, 5, 'f', 2) - .arg(stp->angVelZm, 5, 'f', 2) - .arg(stp->accelXm, 5, 'f', 2) - .arg(stp->accelYm, 5, 'f', 2) - .arg(stp->accelZm, 5, 'f', 2) - .arg(stp->latitude, 5, 'f', 2) - .arg(stp->longitude, 5, 'f', 2) - .arg(stp->AGL, 5, 'f', 2) - .arg(stp->heading*RAD2DEG, 5, 'f', 2) - .arg(stp->pitch*RAD2DEG, 5, 'f', 2) - .arg(stp->roll*RAD2DEG, 5, 'f', 2) - ); + QString( + "Plugin Folder = %1\n" + "Output Folder = %2\n" + "nStructSize = %3 " + "fIntegrationTimeStep = %4\n" + "\n" + "Aileron TX = %5 RX = %6 RCMD TX = %7 RX = %8\n" + "Elevator TX = %9 RX = %10 RCMD TX = %11 RX = %12\n" + "Throttle TX = %13 RX = %14 RCMD TX = %15 RX = %16\n" + "Rudder TX = %17 RX = %18 RCMD TX = %19 RX = %20\n" + "Channel5 TX = %21 RX = %22 RCMD TX = %23 RX = %24\n" + "Channel6 TX = %25 RX = %26 RCMD TX = %27 RX = %28\n" + "Channel7 TX = %29 RX = %30 RCMD TX = %31 RX = %32\n" + "PluginCh1 TX = %33 RX = %34 RCMD TX = %35 RX = %36\n" + "PluginCh2 TX = %37 RX = %38 RCMD TX = %39 RX = %40\n" + "FPVCamPan TX = %41 RX = %42 RCMD TX = %43 RX = %44\n" + "FPVCamTil TX = %45 RX = %46 RCMD TX = %47 RX = %48\n" + "\n" + "MenuItems = %49\n" + // Model data + "\n" + "fPosX,Y,Z = (%50, %51, %52)\n" + "fVelX,Y,Z = (%53, %54, %55)\n" + "fAngVelX,Y,Z = (%56, %57, %58)\n" + "fAccelX,Y,Z = (%59, %60, %61)\n" + "\n" + "Lat, Long = %62, %63\n" + "fHeightAboveTerrain = %64\n" + "\n" + "fHeading = %65 fPitch = %66 fRoll = %67\n" + ) + .arg(pluginFolder) + .arg(outputFolder) + .arg(stp->structSize) + .arg(1.0 / stp->simTimeStep, 4, 'f', 1) + .arg(stp->chSimTX[Ch1Aileron], 5, 'f', 2) + .arg(stp->chSimRX[Ch1Aileron], 5, 'f', 2) + .arg(pts->chNewTX[Ch1Aileron], 5, 'f', 2) + .arg(pts->chNewRX[Ch1Aileron], 5, 'f', 2) + .arg(stp->chSimTX[Ch2Elevator], 5, 'f', 2) + .arg(stp->chSimRX[Ch2Elevator], 5, 'f', 2) + .arg(pts->chNewTX[Ch2Elevator], 5, 'f', 2) + .arg(pts->chNewRX[Ch2Elevator], 5, 'f', 2) + .arg(stp->chSimTX[Ch3Throttle], 5, 'f', 2) + .arg(stp->chSimRX[Ch3Throttle], 5, 'f', 2) + .arg(pts->chNewTX[Ch3Throttle], 5, 'f', 2) + .arg(pts->chNewRX[Ch3Throttle], 5, 'f', 2) + .arg(stp->chSimTX[Ch4Rudder], 5, 'f', 2) + .arg(stp->chSimRX[Ch4Rudder], 5, 'f', 2) + .arg(pts->chNewTX[Ch4Rudder], 5, 'f', 2) + .arg(pts->chNewRX[Ch4Rudder], 5, 'f', 2) + .arg(stp->chSimTX[Ch5], 5, 'f', 2) + .arg(stp->chSimRX[Ch5], 5, 'f', 2) + .arg(pts->chNewTX[Ch5], 5, 'f', 2) + .arg(pts->chNewRX[Ch5], 5, 'f', 2) + .arg(stp->chSimTX[Ch6], 5, 'f', 2) + .arg(stp->chSimRX[Ch6], 5, 'f', 2) + .arg(pts->chNewTX[Ch6], 5, 'f', 2) + .arg(pts->chNewRX[Ch6], 5, 'f', 2) + .arg(stp->chSimTX[Ch7], 5, 'f', 2) + .arg(stp->chSimRX[Ch7], 5, 'f', 2) + .arg(pts->chNewTX[Ch7], 5, 'f', 2) + .arg(pts->chNewRX[Ch7], 5, 'f', 2) + .arg(stp->chSimTX[Ch23Plugin1], 5, 'f', 2) + .arg(stp->chSimRX[Ch23Plugin1], 5, 'f', 2) + .arg(pts->chNewTX[Ch23Plugin1], 5, 'f', 2) + .arg(pts->chNewRX[Ch23Plugin1], 5, 'f', 2) + .arg(stp->chSimTX[Ch24Plugin2], 5, 'f', 2) + .arg(stp->chSimRX[Ch24Plugin2], 5, 'f', 2) + .arg(pts->chNewTX[Ch24Plugin2], 5, 'f', 2) + .arg(pts->chNewRX[Ch24Plugin2], 5, 'f', 2) + .arg(stp->chSimTX[Ch12FPVCamPan], 5, 'f', 2) + .arg(stp->chSimRX[Ch12FPVCamPan], 5, 'f', 2) + .arg(pts->chNewTX[Ch12FPVCamPan], 5, 'f', 2) + .arg(pts->chNewRX[Ch12FPVCamPan], 5, 'f', 2) + .arg(stp->chSimTX[Ch13FPVCamTilt], 5, 'f', 2) + .arg(stp->chSimRX[Ch13FPVCamTilt], 5, 'f', 2) + .arg(pts->chNewTX[Ch13FPVCamTilt], 5, 'f', 2) + .arg(pts->chNewRX[Ch13FPVCamTilt], 5, 'f', 2) + .arg(stp->simMenuStatus) + .arg(stp->posX, 5, 'f', 2) + .arg(stp->posY, 5, 'f', 2) + .arg(stp->posZ, 5, 'f', 2) + .arg(stp->velX, 5, 'f', 2) + .arg(stp->velY, 5, 'f', 2) + .arg(stp->velZ, 5, 'f', 2) + .arg(stp->angVelXm, 5, 'f', 2) + .arg(stp->angVelYm, 5, 'f', 2) + .arg(stp->angVelZm, 5, 'f', 2) + .arg(stp->accelXm, 5, 'f', 2) + .arg(stp->accelYm, 5, 'f', 2) + .arg(stp->accelZm, 5, 'f', 2) + .arg(stp->latitude, 5, 'f', 2) + .arg(stp->longitude, 5, 'f', 2) + .arg(stp->AGL, 5, 'f', 2) + .arg(stp->heading * RAD2DEG, 5, 'f', 2) + .arg(stp->pitch * RAD2DEG, 5, 'f', 2) + .arg(stp->roll * RAD2DEG, 5, 'f', 2) + ); } SIM_DLL_EXPORT void AeroSIMRC_Plugin_Run(const simToPlugin *stp, - pluginToSim *pts) + pluginToSim *pts) { debugInfo = "---\n"; // By default do not change the Menu Items of type CheckBox @@ -365,7 +367,7 @@ SIM_DLL_EXPORT void AeroSIMRC_Plugin_Run(const simToPlugin *stp, bool isNextWp = (stp->simMenuStatus & MenuNextWpt) != 0; // Run commands if (isReset) { - Run_Command_Reset(/*stp, pts*/); + Run_Command_Reset( /*stp, pts*/); } else if (isScreen) { Run_Command_WindowSizeAndPos(stp, pts); } else if (isNextWp) { @@ -373,10 +375,12 @@ SIM_DLL_EXPORT void AeroSIMRC_Plugin_Run(const simToPlugin *stp, } else { Run_BlinkLEDs(stp, pts); if (isEnable) { - if (isTxON) + if (isTxON) { sndr->sendDatagram(stp); - if (isRxON) + } + if (isRxON) { rcvr->setChannels(pts); + } } // network lag diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.h index a590c8a24..be94f15ff 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.h @@ -38,16 +38,13 @@ SIM_DLL_EXPORT void AeroSIMRC_Plugin_ReportStructSizes( quint32 *sizeSimToPlugin, quint32 *sizePluginToSim, - quint32 *sizePluginInit -); + quint32 *sizePluginInit); SIM_DLL_EXPORT void AeroSIMRC_Plugin_Init( - pluginInit *p -); + pluginInit *p); SIM_DLL_EXPORT void AeroSIMRC_Plugin_Run( const simToPlugin *stp, - pluginToSim *pts -); + pluginToSim *pts); #endif // PLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/qdebughandler.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/qdebughandler.cpp index bb9617511..4e859c60c 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/qdebughandler.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/qdebughandler.cpp @@ -59,6 +59,7 @@ void myQDebugHandler(QtMsgType type, const char *msg) ts << time.currentTime().toString("hh:mm:ss.zzz") << " " << txt << endl; - if (type == QtFatalMsg) + if (type == QtFatalMsg) { abort(); + } } diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.cpp index 6c19f91dc..f2cac7caf 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.cpp @@ -30,20 +30,22 @@ Settings::Settings(QString settingsPath, QObject *parent) : QObject(parent) { - settings = new QSettings(settingsPath + "/cc_plugin.ini", QSettings::IniFormat); - // default settings - sendToHost = "127.0.0.1"; - sendToPort = 40100; - listenOnHost = "127.0.0.1"; - listenOnPort = 40200; - channels.reserve(60); - for (quint8 i = 0; i < 10; ++i) - inputMap << 255; - for (quint8 i = 0; i < 8; ++i) - outputMap << 255; - sendToRX = true; - takeFromTX = true; - videoModes << 1 << 50 << 50 << 800 << 600; + settings = new QSettings(settingsPath + "/cc_plugin.ini", QSettings::IniFormat); + // default settings + sendToHost = "127.0.0.1"; + sendToPort = 40100; + listenOnHost = "127.0.0.1"; + listenOnPort = 40200; + channels.reserve(60); + for (quint8 i = 0; i < 10; ++i) { + inputMap << 255; + } + for (quint8 i = 0; i < 8; ++i) { + outputMap << 255; + } + sendToRX = true; + takeFromTX = true; + videoModes << 1 << 50 << 50 << 800 << 600; } void Settings::read() @@ -51,20 +53,20 @@ void Settings::read() // network listenOnHost = settings->value("listen_on_host", listenOnHost).toString(); listenOnPort = settings->value("listen_on_port", listenOnPort).toInt(); - sendToHost = settings->value("send_to_host", sendToHost).toString(); - sendToPort = settings->value("send_to_port", sendToPort).toInt(); + sendToHost = settings->value("send_to_host", sendToHost).toString(); + sendToPort = settings->value("send_to_port", sendToPort).toInt(); QString allChannels = settings->value("all_channels").toString(); QString chan; int i = 0; - foreach (chan, allChannels.split(" ")) - channels.insert(chan, i++); + foreach(chan, allChannels.split(" ")) + channels.insert(chan, i++); // inputs QString num = ""; QString map = ""; for (quint8 i = 0; i < 10; ++i) { - num = QString::number(i+1); + num = QString::number(i + 1); map = settings->value("Input/cc_channel_" + num).toString(); inputMap[i] = channels.value(map, inputMap.at(i)); } @@ -74,7 +76,7 @@ void Settings::read() // outputs for (quint8 i = 0; i < 8; ++i) { - num = QString::number(i+1); + num = QString::number(i + 1); map = settings->value("Output/sim_channel_" + num).toString(); outputMap[i] = channels.value(map, outputMap.at(i)); } @@ -88,11 +90,11 @@ void Settings::read() videoModes.clear(); videoModes << resolutionNum; for (quint8 i = 0; i < resolutionNum; ++i) { - num = QString::number(i+1); + num = QString::number(i + 1); QString modes = settings->value("Video/resolution_" + num, "0, 0, 640, 480").toString(); QString mode; - foreach (mode, modes.split(" ")) - videoModes << mode.toInt(); + foreach(mode, modes.split(" ")) + videoModes << mode.toInt(); } } } diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.h index 5bc39135d..b10359a78 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.h @@ -35,20 +35,46 @@ #include #include -class Settings : public QObject -{ +class Settings : public QObject { public: explicit Settings(QString settingsPath, QObject *parent = 0); void read(); - QString remoteHost() { return sendToHost; } - quint16 remotePort() { return sendToPort; } - QString localHost() { return listenOnHost; } - quint16 localPort() { return listenOnPort; } - QList getInputMap() { return inputMap; } - QList getOutputMap() { return outputMap; } - bool isToRX() { return sendToRX; } - bool isFromTX() { return takeFromTX; } - QList getVideoModes() { return videoModes; } + QString remoteHost() + { + return sendToHost; + } + quint16 remotePort() + { + return sendToPort; + } + QString localHost() + { + return listenOnHost; + } + quint16 localPort() + { + return listenOnPort; + } + QList getInputMap() + { + return inputMap; + } + QList getOutputMap() + { + return outputMap; + } + bool isToRX() + { + return sendToRX; + } + bool isFromTX() + { + return takeFromTX; + } + QList getVideoModes() + { + return videoModes; + } private: QHash channels; diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp index 44151b661..c15d34cea 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp @@ -35,32 +35,35 @@ UdpSender::UdpSender(const QList map, { qDebug() << this << "UdpSender::UdpSender thread:" << thread(); outSocket = NULL; - for (int i = 0; i < 8; ++i) + for (int i = 0; i < 8; ++i) { channels << 0.0; - channelsMap = map; - takeFromTX = isTX; + } + channelsMap = map; + takeFromTX = isTX; packetsSended = 0; } UdpSender::~UdpSender() { - qDebug() << this << "UdpSender::~UdpSender"; - if (outSocket) + qDebug() << this << "UdpSender::~UdpSender"; + if (outSocket) { delete outSocket; + } } // public void UdpSender::init(const QString &remoteHost, quint16 remotePort) { qDebug() << this << "UdpSender::init"; - outHost = remoteHost; - outPort = remotePort; + outHost = remoteHost; + outPort = remotePort; outSocket = new QUdpSocket(); } void UdpSender::sendDatagram(const simToPlugin *stp) { QByteArray data; + data.resize(188); QDataStream out(&data, QIODevice::WriteOnly); out.setFloatingPointPrecision(QDataStream::SinglePrecision); @@ -70,24 +73,24 @@ void UdpSender::sendDatagram(const simToPlugin *stp) // simulation step out << stp->simTimeStep; // home location - out << stp->initPosX << stp->initPosY << stp->initPosZ; - out << stp->wpHomeX << stp->wpHomeY << stp->wpHomeLat << stp->wpHomeLong; + out << stp->initPosX << stp->initPosY << stp->initPosZ; + out << stp->wpHomeX << stp->wpHomeY << stp->wpHomeLat << stp->wpHomeLong; // position - out << stp->posX << stp->posY << stp->posZ; + out << stp->posX << stp->posY << stp->posZ; // velocity (world) - out << stp->velX << stp->velY << stp->velZ; + out << stp->velX << stp->velY << stp->velZ; // angular velocity (model) - out << stp->angVelXm << stp->angVelYm << stp->angVelZm; + out << stp->angVelXm << stp->angVelYm << stp->angVelZm; // acceleration (model) - out << stp->accelXm << stp->accelYm << stp->accelZm; + out << stp->accelXm << stp->accelYm << stp->accelZm; // coordinates - out << stp->latitude << stp->longitude; + out << stp->latitude << stp->longitude; // sonar out << stp->AGL; // attitude - out << stp->heading << stp->pitch << stp->roll; + out << stp->heading << stp->pitch << stp->roll; // electric - out << stp->voltage << stp->current << stp->consumedCharge; + out << stp->voltage << stp->current << stp->consumedCharge; // matrix out << stp->axisXx << stp->axisXy << stp->axisXz; out << stp->axisYx << stp->axisYy << stp->axisYz; @@ -95,12 +98,13 @@ void UdpSender::sendDatagram(const simToPlugin *stp) // channels for (int i = 0; i < 8; ++i) { quint8 mapTo = channelsMap.at(i); - if (mapTo == 255) // unused channel + if (mapTo == 255) { // unused channel out << 0.0; - else if (takeFromTX) // use values from simulators transmitter + } else if (takeFromTX) { // use values from simulators transmitter out << stp->chSimTX[mapTo]; - else // direct use values from ESC/motors/ailerons/etc + } else { // direct use values from ESC/motors/ailerons/etc out << stp->chSimRX[mapTo]; + } } // packet counter @@ -110,7 +114,7 @@ void UdpSender::sendDatagram(const simToPlugin *stp) ++packetsSended; } -//----------------------------------------------------------------------------- +// ----------------------------------------------------------------------------- UdpReceiver::UdpReceiver(const QList map, bool isRX, @@ -119,12 +123,13 @@ UdpReceiver::UdpReceiver(const QList map, { qDebug() << this << "UdpReceiver::UdpReceiver thread:" << thread(); - stopped = false; + stopped = false; inSocket = NULL; - for (int i = 0; i < 10; ++i) + for (int i = 0; i < 10; ++i) { channels << -1.0; - channelsMap = map; - sendToRX = isRX; + } + channelsMap = map; + sendToRX = isRX; armed = 0; mode = 0; packetsRecived = 1; @@ -132,9 +137,10 @@ UdpReceiver::UdpReceiver(const QList map, UdpReceiver::~UdpReceiver() { - qDebug() << this << "UdpReceiver::~UdpReceiver"; - if (inSocket) + qDebug() << this << "UdpReceiver::~UdpReceiver"; + if (inSocket) { delete inSocket; + } } // public @@ -151,8 +157,9 @@ void UdpReceiver::init(const QString &localHost, quint16 localPort) void UdpReceiver::run() { qDebug() << this << "UdpReceiver::run start"; - while (!stopped) + while (!stopped) { onReadyRead(); + } qDebug() << this << "UdpReceiver::run ended"; } @@ -172,11 +179,11 @@ void UdpReceiver::setChannels(pluginToSim *pts) float channelValue = qBound(-1.0f, channels.at(i), 1.0f); if (sendToRX) { // direct connect to ESC/motors/ailerons/etc - pts->chNewRX[mapTo] = channelValue; + pts->chNewRX[mapTo] = channelValue; pts->chOverRX[mapTo] = true; } else { // replace simulators transmitter - pts->chNewTX[mapTo] = channelValue; + pts->chNewTX[mapTo] = channelValue; pts->chOverTX[mapTo] = true; } } @@ -194,8 +201,9 @@ void UdpReceiver::getFlighStatus(quint8 &arm, quint8 &mod) // private void UdpReceiver::onReadyRead() { - if (!inSocket->waitForReadyRead(8)) // 1/60fps ~= 16.7ms, 1/120fps ~= 8.3ms + if (!inSocket->waitForReadyRead(8)) { // 1/60fps ~= 16.7ms, 1/120fps ~= 8.3ms return; + } // TODO: add failsafe // if a command not recieved then slowly return all channel to neutral // @@ -212,15 +220,18 @@ void UdpReceiver::onReadyRead() void UdpReceiver::processDatagram(QByteArray &datagram) { QDataStream stream(datagram); + stream.setFloatingPointPrecision(QDataStream::SinglePrecision); // check magic header quint32 magic; stream >> magic; - if (magic != 0x52434D44) // "RCMD" + if (magic != 0x52434D44) { // "RCMD" return; + } // read channels - for (int i = 0; i < 10; ++i) + for (int i = 0; i < 10; ++i) { stream >> channels[i]; + } // read flight mode stream >> armed >> mode; // read counter diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.h index ead07f046..d3365b955 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.h @@ -36,15 +36,17 @@ #include #include "aerosimrcdatastruct.h" -class UdpSender : public QObject -{ -// Q_OBJECT +class UdpSender : public QObject { +// Q_OBJECT public: explicit UdpSender(const QList map, bool isTX, QObject *parent = 0); ~UdpSender(); void init(const QString &remoteHost, quint16 remotePort); void sendDatagram(const simToPlugin *stp); - quint32 pcks() { return packetsSended; } + quint32 pcks() + { + return packetsSended; + } private: QUdpSocket *outSocket; @@ -57,9 +59,8 @@ private: }; -class UdpReceiver : public QThread -{ -// Q_OBJECT +class UdpReceiver : public QThread { +// Q_OBJECT public: explicit UdpReceiver(const QList map, bool isRX, QObject *parent = 0); ~UdpReceiver(); @@ -69,9 +70,18 @@ public: // function getChannels for other threads void setChannels(pluginToSim *pts); void getFlighStatus(quint8 &arm, quint8 &mod); - quint8 getArmed() { return armed; } - quint8 getMode() { return mode; } - quint32 pcks() { return packetsRecived; } + quint8 getArmed() + { + return armed; + } + quint8 getMode() + { + return mode; + } + quint32 pcks() + { + return packetsRecived; + } private: volatile bool stopped; diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestmain.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestmain.cpp index 744e4bf25..252906baa 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestmain.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestmain.cpp @@ -32,6 +32,7 @@ int main(int argc, char *argv[]) { QApplication a(argc, argv); Widget w; + w.show(); return a.exec(); diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.cpp index 9ab45e45a..47920a9f3 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.cpp @@ -34,8 +34,8 @@ Widget::Widget(QWidget *parent) : { ui->setupUi(this); - inSocket = NULL; - outSocket = NULL; + inSocket = NULL; + outSocket = NULL; screenTimeout.start(); packetCounter = 0; @@ -45,10 +45,10 @@ Widget::Widget(QWidget *parent) : Widget::~Widget() { - if(outSocket) { + if (outSocket) { delete outSocket; } - if(inSocket) { + if (inSocket) { delete inSocket; } delete ui; @@ -80,7 +80,7 @@ void Widget::on_btReciveStart_clicked() void Widget::on_btReciveStop_clicked() { - if(inSocket) { + if (inSocket) { delete inSocket; inSocket = NULL; ui->listWidget->addItem("unbind ok"); @@ -98,8 +98,8 @@ void Widget::on_btTransmitStart_clicked() on_btTransmitStop_clicked(); outSocket = new QUdpSocket(); - outHost = ui->simHost->text(); - outPort = ui->simPort->text().toInt(); + outHost = ui->simHost->text(); + outPort = ui->simPort->text().toInt(); ui->listWidget->addItem("transmit started"); ui->btTransmitStop->setEnabled(1); @@ -110,7 +110,7 @@ void Widget::on_btTransmitStart_clicked() void Widget::on_btTransmitStop_clicked() { - if(outSocket) { + if (outSocket) { delete outSocket; outSocket = NULL; ui->listWidget->addItem("transmit stopped"); @@ -135,8 +135,9 @@ void Widget::readDatagram() Q_UNUSED(datagramSize); processDatagram(datagram); - if (ui->autoAnswer->isChecked()) + if (ui->autoAnswer->isChecked()) { sendDatagram(); + } } } @@ -146,24 +147,25 @@ void Widget::processDatagram(const QByteArray &data) { QByteArray buf = data; QDataStream stream(&buf, QIODevice::ReadOnly); + stream.setFloatingPointPrecision(QDataStream::SinglePrecision); // check magic header quint32 magic; stream >> magic; - if (magic == 0x4153494D) { // "AERO" - float timeStep, - homeX, homeY, homeZ, - WpHX, WpHY, WpLat, WpLon, - posX, posY, posZ, - velX, velY, velZ, - angX, angY, angZ, - accX, accY, accZ, - lat, lon, alt, - head, pitch, roll, - volt, curr, - rx, ry, rz, fx, fy, fz, ux, uy, uz, - chAil, chEle, chThr, chRud, chPlg1, chPlg2, chFpv1, chFpv2; + if (magic == 0x4153494D) { // "AERO" + float timeStep, + homeX, homeY, homeZ, + WpHX, WpHY, WpLat, WpLon, + posX, posY, posZ, + velX, velY, velZ, + angX, angY, angZ, + accX, accY, accZ, + lat, lon, alt, + head, pitch, roll, + volt, curr, + rx, ry, rz, fx, fy, fz, ux, uy, uz, + chAil, chEle, chThr, chRud, chPlg1, chPlg2, chFpv1, chFpv2; stream >> timeStep; stream >> homeX >> homeY >> homeZ; @@ -179,59 +181,61 @@ void Widget::processDatagram(const QByteArray &data) stream >> chAil >> chEle >> chThr >> chRud >> chPlg1 >> chPlg2 >> chFpv1 >> chFpv2; stream >> packetCounter; - if(ui->tabWidget->currentIndex() != 0) + if (ui->tabWidget->currentIndex() != 0) { return; + } - if (screenTimeout.elapsed() < 200) + if (screenTimeout.elapsed() < 200) { return; + } ui->listWidget->clear(); /* - ui->listWidget->addItem("time step (s)"); - ui->listWidget->addItem(QString("%1") + ui->listWidget->addItem("time step (s)"); + ui->listWidget->addItem(QString("%1") .arg(timeStep); - ui->listWidget->addItem("home location (m)"); - ui->listWidget->addItem(QString("%1, %2, %3") + ui->listWidget->addItem("home location (m)"); + ui->listWidget->addItem(QString("%1, %2, %3") .arg(homeX, 7, 'f', 4) .arg(homeY, 7, 'f', 4) .arg(homeZ, 7, 'f', 4)); - ui->listWidget->addItem("home waypoint"); - ui->listWidget->addItem(QString("%1, %2, %3, %4") + ui->listWidget->addItem("home waypoint"); + ui->listWidget->addItem(QString("%1, %2, %3, %4") .arg(WpHX, 7, 'f', 4) .arg(WpHY, 7, 'f', 4) .arg(WpLat, 7, 'f', 4) .arg(WpLon, 7, 'f', 4)); - ui->listWidget->addItem("model position (m)"); - ui->listWidget->addItem(QString("%1, %2, %3") + ui->listWidget->addItem("model position (m)"); + ui->listWidget->addItem(QString("%1, %2, %3") .arg(posX, 7, 'f', 4) .arg(posY, 7, 'f', 4) .arg(posZ, 7, 'f', 4)); - ui->listWidget->addItem("model velocity (m/s)"); - ui->listWidget->addItem(QString("%1, %2, %3") + ui->listWidget->addItem("model velocity (m/s)"); + ui->listWidget->addItem(QString("%1, %2, %3") .arg(velX, 7, 'f', 4) .arg(velY, 7, 'f', 4) .arg(velZ, 7, 'f', 4)); - ui->listWidget->addItem("model angular velocity (rad/s)"); - ui->listWidget->addItem(QString("%1, %2, %3") + ui->listWidget->addItem("model angular velocity (rad/s)"); + ui->listWidget->addItem(QString("%1, %2, %3") .arg(angX, 7, 'f', 4) .arg(angY, 7, 'f', 4) .arg(angZ, 7, 'f', 4)); - ui->listWidget->addItem("model acceleration (m/s/s)"); - ui->listWidget->addItem(QString("%1, %2, %3") + ui->listWidget->addItem("model acceleration (m/s/s)"); + ui->listWidget->addItem(QString("%1, %2, %3") .arg(accX, 7, 'f', 4) .arg(accY, 7, 'f', 4) .arg(accZ, 7, 'f', 4)); - ui->listWidget->addItem("model coordinates (deg, deg, m)"); - ui->listWidget->addItem(QString("%1, %2, %3") + ui->listWidget->addItem("model coordinates (deg, deg, m)"); + ui->listWidget->addItem(QString("%1, %2, %3") .arg(lat, 7, 'f', 4) .arg(lon, 7, 'f', 4) .arg(alt, 7, 'f', 4)); - ui->listWidget->addItem("model electrics"); - ui->listWidget->addItem(QString("%1V, %2A") + ui->listWidget->addItem("model electrics"); + ui->listWidget->addItem(QString("%1V, %2A") .arg(volt, 7, 'f', 4) .arg(curr, 7, 'f', 4)); - ui->listWidget->addItem("channels"); - ui->listWidget->addItem(QString("%1 %2 %3 %4 %5 %6 %7 %8") + ui->listWidget->addItem("channels"); + ui->listWidget->addItem(QString("%1 %2 %3 %4 %5 %6 %7 %8") .arg(chAil, 6, 'f', 3) .arg(chEle, 6, 'f', 3) .arg(chThr, 6, 'f', 3) @@ -240,18 +244,18 @@ void Widget::processDatagram(const QByteArray &data) .arg(chPlg2, 6, 'f', 3) .arg(chFpv1, 6, 'f', 3) .arg(chFpv2, 6, 'f', 3)); - ui->listWidget->addItem("datagram size (bytes), packet counter"); - ui->listWidget->addItem(QString("%1 %2") + ui->listWidget->addItem("datagram size (bytes), packet counter"); + ui->listWidget->addItem(QString("%1 %2") .arg(data.size()) .arg(packetCounter)); -*/ + */ // matrix calculation start // model matrix - QMatrix4x4 m = QMatrix4x4( fy, fx, -fz, 0.0, - ry, rx, -rz, 0.0, - -uy, -ux, uz, 0.0, - 0.0, 0.0, 0.0, 1.0); + QMatrix4x4 m = QMatrix4x4(fy, fx, -fz, 0.0, + ry, rx, -rz, 0.0, + -uy, -ux, uz, 0.0, + 0.0, 0.0, 0.0, 1.0); m.optimize(); // world matrix @@ -301,9 +305,9 @@ void Widget::processDatagram(const QByteArray &data) .arg(q.scalar(), 7, 'f', 4)); ui->listWidget->addItem("model attitude (deg)"); ui->listWidget->addItem(QString("%1, %2, %3") - .arg(roll*RAD2DEG, 7, 'f', 4) - .arg(pitch*RAD2DEG, 7, 'f', 4) - .arg(head*RAD2DEG, 7, 'f', 4)); + .arg(roll * RAD2DEG, 7, 'f', 4) + .arg(pitch * RAD2DEG, 7, 'f', 4) + .arg(head * RAD2DEG, 7, 'f', 4)); ui->listWidget->addItem("CC attitude calculated (deg)"); ui->listWidget->addItem(QString("%1, %2, %3") .arg(rpy.x(), 7, 'f', 4) @@ -311,16 +315,16 @@ void Widget::processDatagram(const QByteArray &data) .arg(rpy.z(), 7, 'f', 4)); screenTimeout.restart(); - } else if (magic == 0x52434D44) { // "RCMD" qreal ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8, ch9, ch10; stream >> ch1 >> ch2 >> ch3 >> ch4 >> ch5 >> ch6 >> ch7 >> ch8 >> ch9 >> ch10; quint8 armed, mode; stream >> armed >> mode; - if(ui->tabWidget->currentIndex() == 0) { - if (screenTimeout.elapsed() < 200) + if (ui->tabWidget->currentIndex() == 0) { + if (screenTimeout.elapsed() < 200) { return; + } ui->listWidget->clear(); ui->listWidget->addItem("channels"); ui->listWidget->addItem("CH1: " + QString::number(ch1)); @@ -344,10 +348,11 @@ void Widget::processDatagram(const QByteArray &data) void Widget::sendDatagram() { - if(!outSocket) + if (!outSocket) { return; + } - float ch[10]; // = {0,0,0,0,0,0,0,0,0,0}; + float ch[10]; // = {0,0,0,0,0,0,0,0,0,0}; quint8 armed; quint8 fmode; const float coeff = 1.0 / 512.0; @@ -430,30 +435,31 @@ void Widget::asQuat2RPY(const QQuaternion &q, QVector3D &rpy) qreal pitch; qreal yaw; - const qreal d2 = 2.0; + const qreal d2 = 2.0; const qreal qss = q.scalar() * q.scalar(); const qreal qxx = q.x() * q.x(); const qreal qyy = q.y() * q.y(); const qreal qzz = q.z() * q.z(); qreal test = -d2 * (q.x() * q.z() - q.scalar() * q.y()); + if (qFabs(test) > 0.998) { // ~86.3°, gimbal lock qreal R10 = d2 * (q.x() * q.y() - q.scalar() * q.z()); qreal R11 = qss - qxx + qyy - qzz; - roll = 0.0; + roll = 0.0; pitch = copysign(M_PI_2, test); - yaw = qAtan2(-R10, R11); + yaw = qAtan2(-R10, R11); } else { qreal R12 = d2 * (q.y() * q.z() + q.scalar() * q.x()); qreal R22 = qss - qxx - qyy + qzz; qreal R01 = d2 * (q.x() * q.y() + q.scalar() * q.z()); qreal R00 = qss + qxx - qyy - qzz; - roll = qAtan2(R12, R22); - pitch = qAsin(test); - yaw = qAtan2(R01, R00); + roll = qAtan2(R12, R22); + pitch = qAsin(test); + yaw = qAtan2(R01, R00); } rpy.setX(RAD2DEG * roll); rpy.setY(RAD2DEG * pitch); @@ -477,15 +483,15 @@ void Widget::asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy) yaw = qAtan2(m(0, 1), m(0, 0)); } - rpy.setX(roll * RAD2DEG); + rpy.setX(roll * RAD2DEG); rpy.setY(pitch * RAD2DEG); - rpy.setZ(yaw * RAD2DEG); + rpy.setZ(yaw * RAD2DEG); } /* // not used -void Widget::ccRPY2Quat(const QVector3D &rpy, QQuaternion &q) -{ + void Widget::ccRPY2Quat(const QVector3D &rpy, QQuaternion &q) + { float phi, theta, psi; float cphi, sphi, ctheta, stheta, cpsi, spsi; @@ -510,10 +516,10 @@ void Widget::ccRPY2Quat(const QVector3D &rpy, QQuaternion &q) q.setY(-q.y()); q.setZ(-q.z()); } -} + } -void Widget::ccQuat2Matrix(const QQuaternion &q, QMatrix4x4 &m) -{ + void Widget::ccQuat2Matrix(const QQuaternion &q, QMatrix4x4 &m) + { float q0s = q.scalar() * q.scalar(); float q1s = q.x() * q.x(); float q2s = q.y() * q.y(); @@ -533,5 +539,5 @@ void Widget::ccQuat2Matrix(const QQuaternion &q, QMatrix4x4 &m) m10, m11, m12, 0, m20, m21, m22, 0, 0, 0, 0, 1); -} -*/ + } + */ diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.h index 500f35482..115985552 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.h @@ -38,14 +38,13 @@ #include namespace Ui { - class Widget; +class Widget; } -const float RAD2DEG = (float)(180.0/M_PI); -const float DEG2RAD = (float)(M_PI/180.0); +const float RAD2DEG = (float)(180.0 / M_PI); +const float DEG2RAD = (float)(M_PI / 180.0); -class Widget : public QWidget -{ +class Widget : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp index 09f96baa5..d829bd937 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp @@ -37,12 +37,12 @@ AeroSimRCSimulator::AeroSimRCSimulator(const SimulatorSettings ¶ms) } AeroSimRCSimulator::~AeroSimRCSimulator() -{ -} +{} bool AeroSimRCSimulator::setupProcess() { QMutexLocker locker(&lock); + return true; } @@ -60,6 +60,7 @@ void AeroSimRCSimulator::transmitUpdate() { // read actuator output ActuatorCommand::DataFields actCmdData; + actCmdData = actCommand->getData(); float channels[10]; for (int i = 0; i < 10; ++i) { @@ -73,34 +74,34 @@ void AeroSimRCSimulator::transmitUpdate() } ActuatorDesired::DataFields actData; - FlightStatus::DataFields flightStatusData = flightStatus->getData(); + FlightStatus::DataFields flightStatusData = flightStatus->getData(); ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData(); - float roll = -1; - float pitch = -1; - float yaw = -1; + float roll = -1; + float pitch = -1; + float yaw = -1; float throttle = -1; if (flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) { // Read joystick input if (flightStatusData.Armed == FlightStatus::ARMED_ARMED) { // Note: Pitch sign is reversed in FG ? - roll = manCtrlData.Roll; - pitch = -manCtrlData.Pitch; - yaw = manCtrlData.Yaw; + roll = manCtrlData.Roll; + pitch = -manCtrlData.Pitch; + yaw = manCtrlData.Yaw; throttle = manCtrlData.Throttle; } } else { // Read ActuatorDesired from autopilot - actData = actDesired->getData(); + actData = actDesired->getData(); - roll = actData.Roll; - pitch = -actData.Pitch; - yaw = actData.Yaw; - throttle = (actData.Throttle*2.0)-1.0; + roll = actData.Roll; + pitch = -actData.Pitch; + yaw = actData.Yaw; + throttle = (actData.Throttle * 2.0) - 1.0; } channels[0] = roll; channels[1] = pitch; - if(throttle < -1) { + if (throttle < -1) { throttle = -1; } channels[2] = throttle; @@ -110,19 +111,19 @@ void AeroSimRCSimulator::transmitUpdate() quint8 armed; quint8 mode; armed = flightStatusData.Armed; - mode = flightStatusData.FlightMode; + mode = flightStatusData.FlightMode; QByteArray data; // 50 - current size of values, 4(quint32) + 10*4(float) + 2(quint8) + 4(quint32) data.resize(50); QDataStream stream(&data, QIODevice::WriteOnly); stream.setFloatingPointPrecision(QDataStream::SinglePrecision); - stream << quint32(0x52434D44); // magic header, "RCMD" + stream << quint32(0x52434D44); // magic header, "RCMD" for (int i = 0; i < 10; ++i) { - stream << channels[i]; // channels + stream << channels[i]; // channels } - stream << armed << mode; // flight status - stream << udpCounterASrecv; // packet counter + stream << armed << mode; // flight status + stream << udpCounterASrecv; // packet counter if (outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { qDebug() << "write failed: " << outSocket->errorString(); @@ -154,25 +155,25 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data) // check magic header quint32 magic; stream >> magic; - if (magic != 0x4153494D) { // "AERO" + if (magic != 0x4153494D) { // "AERO" qDebug() << "wrong magic: " << magic << ", correct: " << quint32(0x4153494D); return; } #define AEROSIM_RCCHANNEL_NUMELEM 8 - float delT, - homeX, homeY, homeZ, - WpHX, WpHY, WpLat, WpLon, - posX, posY, posZ, // world - velX, velY, velZ, // world - angX, angY, angZ, // model - accX, accY, accZ; // model - qreal lat, lon; - float agl, // world - yaw, pitch, roll, // model - volt, curr, cons, - rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix - ch[AEROSIM_RCCHANNEL_NUMELEM]; + float delT, + homeX, homeY, homeZ, + WpHX, WpHY, WpLat, WpLon, + posX, posY, posZ, // world + velX, velY, velZ, // world + angX, angY, angZ, // model + accX, accY, accZ; // model + qreal lat, lon; + float agl, // world + yaw, pitch, roll, // model + volt, curr, cons, + rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix + ch[AEROSIM_RCCHANNEL_NUMELEM]; stream >> delT; stream >> homeX >> homeY >> homeZ; @@ -195,65 +196,65 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data) out.delT = delT; /*************************************************************************************/ - for (int i=0; i< AEROSIM_RCCHANNEL_NUMELEM; i++) { - out.rc_channel[i]=ch[i]; //Elements in rc_channel are between -1 and 1 + for (int i = 0; i < AEROSIM_RCCHANNEL_NUMELEM; i++) { + out.rc_channel[i] = ch[i]; // Elements in rc_channel are between -1 and 1 } /**********************************************************************************************/ QMatrix4x4 mat; - mat = QMatrix4x4( fy, fx, -fz, 0.0, // model matrix - ry, rx, -rz, 0.0, // (X,Y,Z) -> (+Y,+X,-Z) - -uy, -ux, uz, 0.0, - 0.0, 0.0, 0.0, 1.0); + mat = QMatrix4x4(fy, fx, -fz, 0.0, // model matrix + ry, rx, -rz, 0.0, // (X,Y,Z) -> (+Y,+X,-Z) + -uy, -ux, uz, 0.0, + 0.0, 0.0, 0.0, 1.0); mat.optimize(); - QQuaternion quat; // model quat + QQuaternion quat; // model quat asMatrix2Quat(mat, quat); /*************************************************************************************/ // rotate gravity - QVector3D acc = QVector3D(accY, accX, -accZ); // accel (X,Y,Z) -> (+Y,+X,-Z) + QVector3D acc = QVector3D(accY, accX, -accZ); // accel (X,Y,Z) -> (+Y,+X,-Z) QVector3D gee = QVector3D(0.0, 0.0, -GEE); QQuaternion qWorld = quat.conjugate(); - gee = qWorld.rotatedVector(gee); + gee = qWorld.rotatedVector(gee); acc += gee; - out.rollRate = angY * RAD2DEG; // gyros (X,Y,Z) -> (+Y,+X,-Z) + out.rollRate = angY * RAD2DEG; // gyros (X,Y,Z) -> (+Y,+X,-Z) out.pitchRate = angX * RAD2DEG; - out.yawRate = angZ * -RAD2DEG; + out.yawRate = angZ * -RAD2DEG; out.accX = acc.x(); out.accY = acc.y(); out.accZ = acc.z(); /*************************************************************************************/ - QVector3D rpy; // model roll, pitch, yaw + QVector3D rpy; // model roll, pitch, yaw asMatrix2RPY(mat, rpy); - out.roll = rpy.x(); - out.pitch = rpy.y(); + out.roll = rpy.x(); + out.pitch = rpy.y(); out.heading = rpy.z(); /**********************************************************************************************/ - out.altitude = posZ; + out.altitude = posZ; out.agl = posZ; - out.heading = yaw * RAD2DEG; - out.latitude = lat * 10e6; - out.longitude = lon * 10e6; + out.heading = yaw * RAD2DEG; + out.latitude = lat * 10e6; + out.longitude = lon * 10e6; out.groundspeed = qSqrt(velX * velX + velY * velY); /**********************************************************************************************/ - out.dstN = posY; - out.dstE = posX; - out.dstD = -posZ; + out.dstN = posY; + out.dstE = posX; + out.dstD = -posZ; - out.velNorth = velY; - out.velEast = velX; - out.velDown = -velZ; + out.velNorth = velY; + out.velEast = velX; + out.velDown = -velZ; - out.voltage = volt; - out.current = curr; + out.voltage = volt; + out.current = curr; out.consumption = cons * 1000.0; updateUAVOs(out); @@ -306,7 +307,7 @@ void AeroSimRCSimulator::asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy) yaw = qAtan2(m(0, 1), m(0, 0)); } - rpy.setX(roll * RAD2DEG); + rpy.setX(roll * RAD2DEG); rpy.setY(pitch * RAD2DEG); - rpy.setZ(yaw * RAD2DEG); + rpy.setZ(yaw * RAD2DEG); } diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.h index b265b9463..1d9441991 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.h @@ -34,8 +34,7 @@ #include #include "simulator.h" -class AeroSimRCSimulator: public Simulator -{ +class AeroSimRCSimulator : public Simulator { Q_OBJECT public: @@ -43,13 +42,13 @@ public: ~AeroSimRCSimulator(); bool setupProcess(); - void setupUdpPorts(const QString& host, int inPort, int outPort); + void setupUdpPorts(const QString & host, int inPort, int outPort); private slots: void transmitUpdate(); private: - quint32 udpCounterASrecv; //keeps track of udp packets received by ASim + quint32 udpCounterASrecv; // keeps track of udp packets received by ASim void processUpdate(const QByteArray &data); @@ -57,14 +56,13 @@ private: void asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy); }; -class AeroSimRCSimulatorCreator : public SimulatorCreator -{ +class AeroSimRCSimulatorCreator : public SimulatorCreator { public: AeroSimRCSimulatorCreator(const QString &classId, const QString &description) - : SimulatorCreator (classId, description) + : SimulatorCreator(classId, description) {} - Simulator* createSimulator(const SimulatorSettings ¶ms) + Simulator *createSimulator(const SimulatorSettings ¶ms) { return new AeroSimRCSimulator(params); } diff --git a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp index 77fd8ecba..790265ea2 100644 --- a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp @@ -31,25 +31,25 @@ #include "coreplugin/threadmanager.h" #ifndef M_PI -#define M_PI 3.14159265358979323846 +#define M_PI 3.14159265358979323846 #endif -//FGSimulator::FGSimulator(QString hostAddr, int outPort, int inPort, bool manual, QString binPath, QString dataPath) : -// Simulator(hostAddr, outPort, inPort, manual, binPath, dataPath), -// fgProcess(NULL) -//{ -// // Note: Only tested on windows 7 -//#if defined(Q_WS_WIN) -// cmdShell = QString("c:/windows/system32/cmd.exe"); -//#else -// cmdShell = QString("bash"); -//#endif -//} +// FGSimulator::FGSimulator(QString hostAddr, int outPort, int inPort, bool manual, QString binPath, QString dataPath) : +// Simulator(hostAddr, outPort, inPort, manual, binPath, dataPath), +// fgProcess(NULL) +// { +//// Note: Only tested on windows 7 +// #if defined(Q_WS_WIN) +// cmdShell = QString("c:/windows/system32/cmd.exe"); +// #else +// cmdShell = QString("bash"); +// #endif +// } -FGSimulator::FGSimulator(const SimulatorSettings& params) : +FGSimulator::FGSimulator(const SimulatorSettings & params) : Simulator(params) { - udpCounterFGrecv = 0; + udpCounterFGrecv = 0; udpCounterGCSsend = 0; } @@ -58,14 +58,15 @@ FGSimulator::~FGSimulator() disconnect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead())); } -void FGSimulator::setupUdpPorts(const QString& host, int inPort, int outPort) +void FGSimulator::setupUdpPorts(const QString & host, int inPort, int outPort) { Q_UNUSED(outPort); - if(inSocket->bind(QHostAddress(host), inPort)) + if (inSocket->bind(QHostAddress(host), inPort)) { emit processOutput("Successfully bound to address " + host + " on port " + QString::number(inPort) + "\n"); - else + } else { emit processOutput("Cannot bind to address " + host + " on port " + QString::number(inPort) + "\n"); + } } bool FGSimulator::setupProcess() @@ -75,17 +76,18 @@ bool FGSimulator::setupProcess() // Copy FlightGear generic protocol configuration file to the FG protocol directory // NOTE: Not working on Windows 7, if FG is installed in the "Program Files", // likelly due to permissions. The file should be manually copied to data/Protocol/opfgprotocol.xml - // QFile xmlFile(":/flightgear/genericprotocol/opfgprotocol.xml"); - // xmlFile.open(QIODevice::ReadOnly | QIODevice::Text); - // QString xml = xmlFile.readAll(); - // xmlFile.close(); - // QFile xmlFileOut(pathData + "/Protocol/opfgprotocol.xml"); - // xmlFileOut.open(QIODevice::WriteOnly | QIODevice::Text); - // xmlFileOut.write(xml.toAscii()); - // xmlFileOut.close(); + // QFile xmlFile(":/flightgear/genericprotocol/opfgprotocol.xml"); + // xmlFile.open(QIODevice::ReadOnly | QIODevice::Text); + // QString xml = xmlFile.readAll(); + // xmlFile.close(); + // QFile xmlFileOut(pathData + "/Protocol/opfgprotocol.xml"); + // xmlFileOut.open(QIODevice::WriteOnly | QIODevice::Text); + // xmlFileOut.write(xml.toAscii()); + // xmlFileOut.close(); Qt::HANDLE mainThread = QThread::currentThreadId(); - qDebug() << "setupProcess Thread: "<< mainThread; + + qDebug() << "setupProcess Thread: " << mainThread; simProcess = new QProcess(); simProcess->setReadChannelMode(QProcess::MergedChannels); @@ -99,8 +101,7 @@ bool FGSimulator::setupProcess() // Start shell (Note: Could not start FG directly on Windows, only through terminal!) simProcess->start(cmdShell); - if (simProcess->waitForStarted() == false) - { + if (simProcess->waitForStarted() == false) { emit processOutput("Error:" + simProcess->errorString()); return false; } @@ -117,19 +118,15 @@ bool FGSimulator::setupProcess() "--vc=100 " + "--log-level=alert " + "--generic=socket,out,20," + settings.hostAddress + "," + QString::number(settings.inPort) + ",udp,opfgprotocol"); - if(settings.manualControlEnabled) // <--[BCH] What does this do? Why does it depend on ManualControl? - { + if (settings.manualControlEnabled) { // <--[BCH] What does this do? Why does it depend on ManualControl? args.append(" --generic=socket,in,400," + settings.remoteAddress + "," + QString::number(settings.outPort) + ",udp,opfgprotocol"); } // Start FlightGear - only if checkbox is selected in HITL options page - if (settings.startSim) - { + if (settings.startSim) { QString cmd("\"" + settings.binPath + "\" " + args + "\n"); simProcess->write(cmd.toAscii()); - } - else - { + } else { emit processOutput("Start Flightgear from the command line with the following arguments: \n\n" + args + "\n\n" + "You can optionally run Flightgear from a networked computer.\n" + "Make sure the computer running Flightgear can can ping your local interface adapter. ie." + settings.hostAddress + "\n" @@ -145,8 +142,8 @@ void FGSimulator::processReadyRead() { QByteArray bytes = simProcess->readAllStandardOutput(); QString str(bytes); - if ( !str.contains("Error reading data") ) // ignore error - { + + if (!str.contains("Error reading data")) { // ignore error emit processOutput(str); } } @@ -154,140 +151,133 @@ void FGSimulator::processReadyRead() void FGSimulator::transmitUpdate() { ActuatorDesired::DataFields actData; - FlightStatus::DataFields flightStatusData = flightStatus->getData(); + FlightStatus::DataFields flightStatusData = flightStatus->getData(); ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData(); float ailerons = -1; float elevator = -1; - float rudder = -1; + float rudder = -1; float throttle = -1; - if(flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) - { + if (flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) { // Read joystick input - if(flightStatusData.Armed == FlightStatus::ARMED_ARMED) - { + if (flightStatusData.Armed == FlightStatus::ARMED_ARMED) { // Note: Pitch sign is reversed in FG ? ailerons = manCtrlData.Roll; elevator = -manCtrlData.Pitch; - rudder = manCtrlData.Yaw; + rudder = manCtrlData.Yaw; throttle = manCtrlData.Throttle; } - } - else - { + } else { // Read ActuatorDesired from autopilot - actData = actDesired->getData(); + actData = actDesired->getData(); ailerons = actData.Roll; elevator = -actData.Pitch; - rudder = actData.Yaw; + rudder = actData.Yaw; throttle = actData.Throttle; } int allowableDifference = 10; - //qDebug() << "UDP sent:" << udpCounterGCSsend << " - UDP Received:" << udpCounterFGrecv; + // qDebug() << "UDP sent:" << udpCounterGCSsend << " - UDP Received:" << udpCounterFGrecv; - if(udpCounterFGrecv == udpCounterGCSsend) + if (udpCounterFGrecv == udpCounterGCSsend) { udpCounterGCSsend = 0; - - if((udpCounterGCSsend < allowableDifference) || (udpCounterFGrecv==0) ) //FG udp queue is not delayed - { + } + + if ((udpCounterGCSsend < allowableDifference) || (udpCounterFGrecv == 0)) { // FG udp queue is not delayed udpCounterGCSsend++; // Send update to FlightGear QString cmd; cmd = QString("%1,%2,%3,%4,%5\n") - .arg(ailerons) //ailerons - .arg(elevator) //elevator - .arg(rudder) //rudder - .arg(throttle) //throttle - .arg(udpCounterGCSsend); //UDP packet counter delay + .arg(ailerons) // ailerons + .arg(elevator) // elevator + .arg(rudder) // rudder + .arg(throttle) // throttle + .arg(udpCounterGCSsend); // UDP packet counter delay QByteArray data = cmd.toAscii(); - if(outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) - { + if (outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { emit processOutput("Error sending UDP packet to FG: " + outSocket->errorString() + "\n"); } - } - else - { + } else { // don't send new packet. Flightgear cannot process UDP fast enough. // V1.9.1 reads udp packets at set frequency and will get delayed if packets are sent too fast // V2.0 does not currently work with --generic-protocol } - - if(settings.manualControlEnabled) - { - actData.Roll = ailerons; - actData.Pitch = -elevator; - actData.Yaw = rudder; + + if (settings.manualControlEnabled) { + actData.Roll = ailerons; + actData.Pitch = -elevator; + actData.Yaw = rudder; actData.Throttle = throttle; - //actData.NumLongUpdates = (float)udpCounterFGrecv; - //actData.UpdateTime = (float)udpCounterGCSsend; + // actData.NumLongUpdates = (float)udpCounterFGrecv; + // actData.UpdateTime = (float)udpCounterGCSsend; actDesired->setData(actData); } } -void FGSimulator::processUpdate(const QByteArray& inp) +void FGSimulator::processUpdate(const QByteArray & inp) { - //TODO: this does not use the FLIGHT_PARAM structure, it should! + // TODO: this does not use the FLIGHT_PARAM structure, it should! // Split QString data(inp); QStringList fields = data.split(","); // Get xRate (deg/s) - // float xRate = fields[0].toFloat() * 180.0/M_PI; + // float xRate = fields[0].toFloat() * 180.0/M_PI; // Get yRate (deg/s) - // float yRate = fields[1].toFloat() * 180.0/M_PI; + // float yRate = fields[1].toFloat() * 180.0/M_PI; // Get zRate (deg/s) - // float zRate = fields[2].toFloat() * 180.0/M_PI; + // float zRate = fields[2].toFloat() * 180.0/M_PI; // Get xAccel (m/s^2) - float xAccel = fields[3].toFloat() * FT2M; + float xAccel = fields[3].toFloat() * FT2M; // Get yAccel (m/s^2) - float yAccel = fields[4].toFloat() * FT2M; + float yAccel = fields[4].toFloat() * FT2M; // Get xAccel (m/s^2) - float zAccel = fields[5].toFloat() * FT2M; + float zAccel = fields[5].toFloat() * FT2M; // Get pitch (deg) - float pitch = fields[6].toFloat(); + float pitch = fields[6].toFloat(); // Get pitchRate (deg/s) float pitchRate = fields[7].toFloat(); // Get roll (deg) - float roll = fields[8].toFloat(); + float roll = fields[8].toFloat(); // Get rollRate (deg/s) float rollRate = fields[9].toFloat(); // Get yaw (deg) - float yaw = fields[10].toFloat(); + float yaw = fields[10].toFloat(); // Get yawRate (deg/s) - float yawRate = fields[11].toFloat(); + float yawRate = fields[11].toFloat(); // Get latitude (deg) - float latitude = fields[12].toFloat(); + float latitude = fields[12].toFloat(); // Get longitude (deg) - float longitude = fields[13].toFloat(); + float longitude = fields[13].toFloat(); // Get heading (deg) - float heading = fields[14].toFloat(); + float heading = fields[14].toFloat(); // Get altitude (m) float altitude_msl = fields[15].toFloat() * FT2M; // Get altitudeAGL (m) float altitude_agl = fields[16].toFloat() * FT2M; // Get groundspeed (m/s) - float groundspeed = fields[17].toFloat() * KT2MPS; + float groundspeed = fields[17].toFloat() * KT2MPS; // Get airspeed (m/s) - float airspeed = fields[18].toFloat() * KT2MPS; + float airspeed = fields[18].toFloat() * KT2MPS; // Get temperature (degC) - float temperature = fields[19].toFloat(); + float temperature = fields[19].toFloat(); // Get pressure (kpa) - float pressure = fields[20].toFloat() * INHG2KPA; + float pressure = fields[20].toFloat() * INHG2KPA; // Get VelocityActual Down (cm/s) - float velocityActualDown = - fields[21].toFloat() * FPS2CMPS; + float velocityActualDown = -fields[21].toFloat() * FPS2CMPS; // Get VelocityActual East (cm/s) - float velocityActualEast = fields[22].toFloat() * FPS2CMPS; + float velocityActualEast = fields[22].toFloat() * FPS2CMPS; // Get VelocityActual Down (cm/s) float velocityActualNorth = fields[23].toFloat() * FPS2CMPS; // Get UDP packets received by FG int n = fields[24].toInt(); + udpCounterFGrecv = n; /////// @@ -299,18 +289,18 @@ void FGSimulator::processUpdate(const QByteArray& inp) float NED[3]; // convert from cm back to meters - double LLA[3] = {latitude, longitude, altitude_msl}; + double LLA[3] = { latitude, longitude, altitude_msl }; double ECEF[3]; double RNE[9]; - Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE); - Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF); - Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float (*)[3]) RNE, NED); + Utils::CoordinateConversions().RneFromLLA(LLA, (double(*)[3])RNE); + Utils::CoordinateConversions().LLA2ECEF(LLA, ECEF); + Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float(*)[3])RNE, NED); // Update GPS Position objects - out.latitude = latitude * 1e7; - out.longitude = longitude * 1e7; - out.altitude = altitude_msl; + out.latitude = latitude * 1e7; + out.longitude = longitude * 1e7; + out.altitude = altitude_msl; out.agl = altitude_agl; out.groundspeed = groundspeed; @@ -319,32 +309,31 @@ void FGSimulator::processUpdate(const QByteArray& inp) // Update BaroAltitude object out.temperature = temperature; - out.pressure = pressure; + out.pressure = pressure; // Update attActual object - out.roll = roll; //roll; - out.pitch = pitch; // pitch - out.heading = yaw; // yaw + out.roll = roll; // roll; + out.pitch = pitch; // pitch + out.heading = yaw; // yaw - out.dstN= NED[0]; - out.dstE= NED[1]; - out.dstD= NED[2]; + out.dstN = NED[0]; + out.dstE = NED[1]; + out.dstD = NED[2]; // Update VelocityActual.{North,East,Down} - out.velNorth = velocityActualNorth; - out.velEast = velocityActualEast; - out.velDown = velocityActualDown; + out.velNorth = velocityActualNorth; + out.velEast = velocityActualEast; + out.velDown = velocityActualDown; - //Update gyroscope sensor data - out.rollRate = rollRate; + // Update gyroscope sensor data + out.rollRate = rollRate; out.pitchRate = pitchRate; - out.yawRate = yawRate; + out.yawRate = yawRate; - //Update accelerometer sensor data - out.accX = xAccel; - out.accY = yAccel; - out.accZ = -zAccel; + // Update accelerometer sensor data + out.accX = xAccel; + out.accY = yAccel; + out.accZ = -zAccel; updateUAVOs(out); } - diff --git a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.h b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.h index 9405389e6..be5e48539 100644 --- a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.h @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -31,40 +31,37 @@ #include #include "simulator.h" -class FGSimulator: public Simulator -{ +class FGSimulator : public Simulator { Q_OBJECT public: - FGSimulator(const SimulatorSettings& params); - ~FGSimulator(); + FGSimulator(const SimulatorSettings & params); + ~FGSimulator(); - bool setupProcess(); - void setupUdpPorts(const QString& host, int inPort, int outPort); + bool setupProcess(); + void setupUdpPorts(const QString & host, int inPort, int outPort); private slots: void transmitUpdate(); - void processReadyRead(); + void processReadyRead(); private: - int udpCounterGCSsend; //keeps track of udp packets sent to FG - int udpCounterFGrecv; //keeps track of udp packets received by FG + int udpCounterGCSsend; // keeps track of udp packets sent to FG + int udpCounterFGrecv; // keeps track of udp packets received by FG - void processUpdate(const QByteArray& data); + void processUpdate(const QByteArray & data); }; -class FGSimulatorCreator : public SimulatorCreator -{ +class FGSimulatorCreator : public SimulatorCreator { public: - FGSimulatorCreator(const QString& classId, const QString& description) - : SimulatorCreator (classId,description) - {} - - Simulator* createSimulator(const SimulatorSettings& params) - { - return new FGSimulator(params); - } + FGSimulatorCreator(const QString & classId, const QString & description) + : SimulatorCreator(classId, description) + {} + Simulator *createSimulator(const SimulatorSettings & params) + { + return new FGSimulator(params); + } }; #endif // FGSIMULATOR_H diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp index 3055ec1df..244c27b6c 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp @@ -7,125 +7,124 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "hitlconfiguration.h" -HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +HITLConfiguration::HITLConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent) { - - //Default settings values - settings.simulatorId = ""; - settings.binPath = ""; - settings.dataPath = ""; - settings.manualControlEnabled = true; - settings.startSim = false; - settings.addNoise = false; - settings.hostAddress = "127.0.0.1"; - settings.remoteAddress = "127.0.0.1"; - settings.outPort = 0; + // Default settings values + settings.simulatorId = ""; + settings.binPath = ""; + settings.dataPath = ""; + settings.manualControlEnabled = true; + settings.startSim = false; + settings.addNoise = false; + settings.hostAddress = "127.0.0.1"; + settings.remoteAddress = "127.0.0.1"; + settings.outPort = 0; settings.inPort = 0; - settings.latitude = ""; - settings.longitude = ""; + settings.latitude = ""; + settings.longitude = ""; - settings.attRawEnabled = false; - settings.attRawRate = 20; + settings.attRawEnabled = false; + settings.attRawRate = 20; - settings.attActualEnabled = true; - settings.attActHW = false; - settings.attActSim = true; - settings.attActCalc = false; + settings.attActualEnabled = true; + settings.attActHW = false; + settings.attActSim = true; + settings.attActCalc = false; - settings.gpsPositionEnabled = false; - settings.gpsPosRate = 100; + settings.gpsPositionEnabled = false; + settings.gpsPosRate = 100; - settings.groundTruthEnabled = false; - settings.groundTruthRate = 100; + settings.groundTruthEnabled = false; + settings.groundTruthRate = 100; - settings.inputCommand = false; - settings.gcsReceiverEnabled = false; - settings.manualControlEnabled= false; - settings.minOutputPeriod = 100; + settings.inputCommand = false; + settings.gcsReceiverEnabled = false; + settings.manualControlEnabled = false; + settings.minOutputPeriod = 100; - settings.airspeedActualEnabled= false; - settings.airspeedActualRate = 100; + settings.airspeedActualEnabled = false; + settings.airspeedActualRate = 100; // if a saved configuration exists load it, and overwrite defaults if (qSettings != 0) { + settings.simulatorId = qSettings->value("simulatorId").toString(); + settings.binPath = qSettings->value("binPath").toString(); + settings.dataPath = qSettings->value("dataPath").toString(); - settings.simulatorId = qSettings->value("simulatorId").toString(); - settings.binPath = qSettings->value("binPath").toString(); - settings.dataPath = qSettings->value("dataPath").toString(); + settings.hostAddress = qSettings->value("hostAddress").toString(); + settings.remoteAddress = qSettings->value("remoteAddress").toString(); + settings.outPort = qSettings->value("outPort").toInt(); + settings.inPort = qSettings->value("inPort").toInt(); - settings.hostAddress = qSettings->value("hostAddress").toString(); - settings.remoteAddress = qSettings->value("remoteAddress").toString(); - settings.outPort = qSettings->value("outPort").toInt(); - settings.inPort = qSettings->value("inPort").toInt(); + settings.latitude = qSettings->value("latitude").toString(); + settings.longitude = qSettings->value("longitude").toString(); + settings.startSim = qSettings->value("startSim").toBool(); + settings.addNoise = qSettings->value("noiseCheckBox").toBool(); - settings.latitude = qSettings->value("latitude").toString(); - settings.longitude = qSettings->value("longitude").toString(); - settings.startSim = qSettings->value("startSim").toBool(); - settings.addNoise = qSettings->value("noiseCheckBox").toBool(); + settings.gcsReceiverEnabled = qSettings->value("gcsReceiverEnabled").toBool(); + settings.manualControlEnabled = qSettings->value("manualControlEnabled").toBool(); - settings.gcsReceiverEnabled = qSettings->value("gcsReceiverEnabled").toBool(); - settings.manualControlEnabled= qSettings->value("manualControlEnabled").toBool(); + settings.attRawEnabled = qSettings->value("attRawEnabled").toBool(); + settings.attRawRate = qSettings->value("attRawRate").toInt(); - settings.attRawEnabled = qSettings->value("attRawEnabled").toBool(); - settings.attRawRate = qSettings->value("attRawRate").toInt(); + settings.attActualEnabled = qSettings->value("attActualEnabled").toBool(); + settings.attActHW = qSettings->value("attActHW").toBool(); + settings.attActSim = qSettings->value("attActSim").toBool(); + settings.attActCalc = qSettings->value("attActCalc").toBool(); - settings.attActualEnabled = qSettings->value("attActualEnabled").toBool(); - settings.attActHW = qSettings->value("attActHW").toBool(); - settings.attActSim = qSettings->value("attActSim").toBool(); - settings.attActCalc = qSettings->value("attActCalc").toBool(); + settings.baroAltitudeEnabled = qSettings->value("baroAltitudeEnabled").toBool(); + settings.baroAltRate = qSettings->value("baroAltRate").toInt(); - settings.baroAltitudeEnabled = qSettings->value("baroAltitudeEnabled").toBool(); - settings.baroAltRate = qSettings->value("baroAltRate").toInt(); + settings.gpsPositionEnabled = qSettings->value("gpsPositionEnabled").toBool(); + settings.gpsPosRate = qSettings->value("gpsPosRate").toInt(); - settings.gpsPositionEnabled = qSettings->value("gpsPositionEnabled").toBool(); - settings.gpsPosRate = qSettings->value("gpsPosRate").toInt(); + settings.groundTruthEnabled = qSettings->value("groundTruthEnabled").toBool(); + settings.groundTruthRate = qSettings->value("groundTruthRate").toInt(); - settings.groundTruthEnabled = qSettings->value("groundTruthEnabled").toBool(); - settings.groundTruthRate = qSettings->value("groundTruthRate").toInt(); + settings.inputCommand = qSettings->value("inputCommand").toBool(); + settings.minOutputPeriod = qSettings->value("minOutputPeriod").toInt(); - settings.inputCommand = qSettings->value("inputCommand").toBool(); - settings.minOutputPeriod = qSettings->value("minOutputPeriod").toInt(); - - settings.airspeedActualEnabled=qSettings->value("airspeedActualEnabled").toBool(); - settings.airspeedActualRate = qSettings->value("airspeedActualRate").toInt(); + settings.airspeedActualEnabled = qSettings->value("airspeedActualEnabled").toBool(); + settings.airspeedActualRate = qSettings->value("airspeedActualRate").toInt(); } } IUAVGadgetConfiguration *HITLConfiguration::clone() { - HITLConfiguration *m = new HITLConfiguration(this->classId()); + HITLConfiguration *m = new HITLConfiguration(this->classId()); - m->settings = settings; + m->settings = settings; return m; } - /** - * Saves a configuration. - * - */ -void HITLConfiguration::saveConfig(QSettings* qSettings) const { +/** + * Saves a configuration. + * + */ +void HITLConfiguration::saveConfig(QSettings *qSettings) const +{ qSettings->setValue("simulatorId", settings.simulatorId); qSettings->setValue("binPath", settings.binPath); qSettings->setValue("dataPath", settings.dataPath); @@ -161,4 +160,3 @@ void HITLConfiguration::saveConfig(QSettings* qSettings) const { qSettings->setValue("airspeedActualEnabled", settings.airspeedActualEnabled); qSettings->setValue("airspeedActualRate", settings.airspeedActualRate); } - diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.h b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.h index 70d2c90f2..1b434c9ff 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.h @@ -7,21 +7,21 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,27 +36,29 @@ using namespace Core; -class HITLConfiguration : public IUAVGadgetConfiguration -{ - - Q_OBJECT - - Q_PROPERTY(SimulatorSettings settings READ Settings WRITE setSimulatorSettings) +class HITLConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT Q_PROPERTY(SimulatorSettings settings READ Settings WRITE setSimulatorSettings) public: - explicit HITLConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit HITLConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); - SimulatorSettings Settings() const { return settings; } + SimulatorSettings Settings() const + { + return settings; + } public slots: - void setSimulatorSettings (const SimulatorSettings& params ) { settings = params; } + void setSimulatorSettings(const SimulatorSettings & params) + { + settings = params; + } private: - SimulatorSettings settings; + SimulatorSettings settings; }; #endif // HITLCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlfactory.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlfactory.cpp index 248b1f21c..4d7e7d833 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlfactory.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,30 +32,25 @@ #include HITLFactory::HITLFactory(QObject *parent) - : IUAVGadgetFactory(QString("HITL"), tr("HITL Simulation"), parent) -{ - -} + : IUAVGadgetFactory(QString("HITL"), tr("HITL Simulation"), parent) +{} HITLFactory::~HITLFactory() +{} + +Core::IUAVGadget *HITLFactory::createGadget(QWidget *parent) { + HITLWidget *gadgetWidget = new HITLWidget(parent); + + return new HITLGadget(QString("HITL"), gadgetWidget, parent); } -Core::IUAVGadget* HITLFactory::createGadget(QWidget *parent) -{ - - - HITLWidget* gadgetWidget = new HITLWidget(parent); - return new HITLGadget(QString("HITL"), gadgetWidget, parent); -} - -IUAVGadgetConfiguration *HITLFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *HITLFactory::createConfiguration(QSettings *qSettings) { return new HITLConfiguration(QString("HITL"), qSettings); } IOptionsPage *HITLFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new HITLOptionsPage(qobject_cast(config)); + return new HITLOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlfactory.h b/ground/openpilotgcs/src/plugins/hitl/hitlfactory.h index b761a1014..16da8ae80 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlfactory.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class HITLFactory : public Core::IUAVGadgetFactory -{ +class HITLFactory : public Core::IUAVGadgetFactory { Q_OBJECT public: HITLFactory(QObject *parent = 0); ~HITLFactory(); - IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadget *createGadget(QWidget *parent); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlgadget.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlgadget.cpp index a05b5ce71..76da7afb4 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlgadget.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlgadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,10 +30,10 @@ #include "simulator.h" HITLGadget::HITLGadget(QString classId, HITLWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) + IUAVGadget(classId, parent), + m_widget(widget) { - connect(this,SIGNAL(changeConfiguration(void)),m_widget,SLOT(stopButtonClicked(void))); + connect(this, SIGNAL(changeConfiguration(void)), m_widget, SLOT(stopButtonClicked(void))); } HITLGadget::~HITLGadget() @@ -41,11 +41,11 @@ HITLGadget::~HITLGadget() delete m_widget; } -void HITLGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void HITLGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - HITLConfiguration *m = qobject_cast(config); + HITLConfiguration *m = qobject_cast(config); // IL2 <-- Is this still necessary? [KDS] - emit changeConfiguration(); - m_widget->setSettingParameters(m->Settings()); -} + emit changeConfiguration(); + m_widget->setSettingParameters(m->Settings()); +} diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlgadget.h b/ground/openpilotgcs/src/plugins/hitl/hitlgadget.h index 78add1e1d..dc7221862 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlgadget.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlgadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,22 +38,24 @@ class Simulator; using namespace Core; -class HITLGadget : public Core::IUAVGadget -{ +class HITLGadget : public Core::IUAVGadget { Q_OBJECT public: - HITLGadget(QString classId, HITLWidget *widget, QWidget *parent = 0); - ~HITLGadget(); + HITLGadget(QString classId, HITLWidget *widget, QWidget *parent = 0); + ~HITLGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); signals: - void changeConfiguration(); + void changeConfiguration(); private: - HITLWidget* m_widget; - Simulator* simulator; + HITLWidget *m_widget; + Simulator *simulator; }; diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp index f24a88268..637707c44 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp @@ -35,44 +35,44 @@ HitlNoiseGeneration::HitlNoiseGeneration() HitlNoiseGeneration::~HitlNoiseGeneration() +{} + +Noise HitlNoiseGeneration::getNoise() { -} - -Noise HitlNoiseGeneration::getNoise(){ return noise; } -Noise HitlNoiseGeneration::generateNoise(){ +Noise HitlNoiseGeneration::generateNoise() +{ + noise.accelData.x = 0; + noise.accelData.y = 0; + noise.accelData.z = 0; - noise.accelData.x=0; - noise.accelData.y=0; - noise.accelData.z=0; + noise.gpsPosData.Latitude = 0; + noise.gpsPosData.Longitude = 0; + noise.gpsPosData.Groundspeed = 0; + noise.gpsPosData.Heading = 0; + noise.gpsPosData.Altitude = 0; - noise.gpsPosData.Latitude=0; - noise.gpsPosData.Longitude=0; - noise.gpsPosData.Groundspeed=0; - noise.gpsPosData.Heading=0; - noise.gpsPosData.Altitude=0; + noise.gpsVelData.North = 0; + noise.gpsVelData.East = 0; + noise.gpsVelData.Down = 0; - noise.gpsVelData.North=0; - noise.gpsVelData.East=0; - noise.gpsVelData.Down=0; + noise.baroAltData.Altitude = 0; - noise.baroAltData.Altitude=0; + noise.attActualData.Roll = 0; + noise.attActualData.Pitch = 0; + noise.attActualData.Yaw = 0; - noise.attActualData.Roll=0; - noise.attActualData.Pitch=0; - noise.attActualData.Yaw=0; + noise.gyroData.x = 0; + noise.gyroData.y = 0; + noise.gyroData.z = 0; - noise.gyroData.x=0; - noise.gyroData.y=0; - noise.gyroData.z=0; + noise.accelData.x = 0; + noise.accelData.y = 0; + noise.accelData.z = 0; - noise.accelData.x=0; - noise.accelData.y=0; - noise.accelData.z=0; - - noise.airspeedActual.CalibratedAirspeed=0; + noise.airspeedActual.CalibratedAirspeed = 0; return noise; } diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h index cdf34b318..ead9ba9dd 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h @@ -28,30 +28,29 @@ #ifndef HITLNOISEGENERATION_H #define HITLNOISEGENERATION_H -//#include -//#include +// #include +// #include #include "xplanesimulator.h" #include "hitlnoisegeneration.h" #include "extensionsystem/pluginmanager.h" #include #include -struct Noise{ +struct Noise { Accels::DataFields accelData; AttitudeActual::DataFields attActualData; - BaroAltitude::DataFields baroAltData; + BaroAltitude::DataFields baroAltData; AirspeedActual::DataFields airspeedActual; - GPSPosition::DataFields gpsPosData; - GPSVelocity::DataFields gpsVelData; + GPSPosition::DataFields gpsPosData; + GPSVelocity::DataFields gpsVelData; Gyros::DataFields gyroData; - HomeLocation::DataFields homeData; + HomeLocation::DataFields homeData; PositionActual::DataFields positionActualData; VelocityActual::DataFields velocityActualData; }; -class HitlNoiseGeneration -{ -// Q_OBJECT +class HitlNoiseGeneration { +// Q_OBJECT public: HitlNoiseGeneration(); ~HitlNoiseGeneration(); diff --git a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp index f26fa09c7..c85b2fb11 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp @@ -7,21 +7,21 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,12 +36,10 @@ #include - HITLOptionsPage::HITLOptionsPage(HITLConfiguration *conf, QObject *parent) : IOptionsPage(parent), - config(conf) -{ -} + config(conf) +{} QWidget *HITLOptionsPage::createPage(QWidget *parent) { @@ -49,31 +47,31 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent) // Create page m_optionsPage = new Ui::HITLOptionsPage(); - QWidget* optionsPageWidget = new QWidget; + QWidget *optionsPageWidget = new QWidget; m_optionsPage->setupUi(optionsPageWidget); - int index = 0; - foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) - { - m_optionsPage->chooseFlightSimulator->insertItem(index++, creator->Description(),creator->ClassId()); - } + int index = 0; + foreach(SimulatorCreator * creator, HITLPlugin::typeSimulators) { + m_optionsPage->chooseFlightSimulator->insertItem(index++, creator->Description(), creator->ClassId()); + } - //QString classId = widget->listSimulators->itemData(0).toString(); - //SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(classId); + // QString classId = widget->listSimulators->itemData(0).toString(); + // SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(classId); - //QWidget* embedPage = creator->createOptionsPage(); - //m_optionsPage->verticalLayout->addWidget(embedPage); + // QWidget* embedPage = creator->createOptionsPage(); + // m_optionsPage->verticalLayout->addWidget(embedPage); - m_optionsPage->executablePath->setExpectedKind(Utils::PathChooser::File); - m_optionsPage->executablePath->setPromptDialogTitle(tr("Choose flight simulator executable")); - m_optionsPage->dataPath->setExpectedKind(Utils::PathChooser::Directory); - m_optionsPage->dataPath->setPromptDialogTitle(tr("Choose flight simulator data directory")); + m_optionsPage->executablePath->setExpectedKind(Utils::PathChooser::File); + m_optionsPage->executablePath->setPromptDialogTitle(tr("Choose flight simulator executable")); + m_optionsPage->dataPath->setExpectedKind(Utils::PathChooser::Directory); + m_optionsPage->dataPath->setPromptDialogTitle(tr("Choose flight simulator data directory")); // Restore the contents from the settings: - foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) - { + foreach(SimulatorCreator * creator, HITLPlugin::typeSimulators) { QString id = config->Settings().simulatorId; - if(creator->ClassId() == id) + + if (creator->ClassId() == id) { m_optionsPage->chooseFlightSimulator->setCurrentIndex(HITLPlugin::typeSimulators.indexOf(creator)); + } } m_optionsPage->executablePath->setPath(config->Settings().binPath); @@ -98,11 +96,10 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent) m_optionsPage->attRawCheckbox->setChecked(config->Settings().attRawEnabled); - m_optionsPage->attRawRateSpinbox->setValue(config->Settings().attRawRate); m_optionsPage->gpsPosRateSpinbox->setValue(config->Settings().gpsPosRate); m_optionsPage->groundTruthRateSpinbox->setValue(config->Settings().groundTruthRate); -// m_optionsPage->attActualRate->setValue(config->Settings().attActualRate); +// m_optionsPage->attActualRate->setValue(config->Settings().attActualRate); m_optionsPage->baroAltitudeCheckbox->setChecked(config->Settings().baroAltitudeEnabled); m_optionsPage->baroAltRateSpinbox->setValue(config->Settings().baroAltRate); @@ -124,48 +121,48 @@ void HITLOptionsPage::apply() SimulatorSettings settings; int i = m_optionsPage->chooseFlightSimulator->currentIndex(); - settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); - settings.binPath = m_optionsPage->executablePath->path(); - settings.dataPath = m_optionsPage->dataPath->path(); - settings.startSim = m_optionsPage->startSim->isChecked(); - settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); - settings.hostAddress = m_optionsPage->hostAddress->text(); - settings.remoteAddress = m_optionsPage->remoteAddress->text(); + settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); + settings.binPath = m_optionsPage->executablePath->path(); + settings.dataPath = m_optionsPage->dataPath->path(); + settings.startSim = m_optionsPage->startSim->isChecked(); + settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); + settings.hostAddress = m_optionsPage->hostAddress->text(); + settings.remoteAddress = m_optionsPage->remoteAddress->text(); settings.inPort = m_optionsPage->inputPort->text().toInt(); - settings.outPort = m_optionsPage->outputPort->text().toInt(); - settings.longitude = m_optionsPage->longitude->text(); - settings.latitude = m_optionsPage->latitude->text(); + settings.outPort = m_optionsPage->outputPort->text().toInt(); + settings.longitude = m_optionsPage->longitude->text(); + settings.latitude = m_optionsPage->latitude->text(); - settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); + settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); - settings.attRawEnabled = m_optionsPage->attRawCheckbox->isChecked(); - settings.attRawRate = m_optionsPage->attRawRateSpinbox->value(); + settings.attRawEnabled = m_optionsPage->attRawCheckbox->isChecked(); + settings.attRawRate = m_optionsPage->attRawRateSpinbox->value(); - settings.attActualEnabled = m_optionsPage->attActualCheckbox->isChecked(); + settings.attActualEnabled = m_optionsPage->attActualCheckbox->isChecked(); - settings.gpsPositionEnabled = m_optionsPage->gpsPositionCheckbox->isChecked(); - settings.gpsPosRate = m_optionsPage->gpsPosRateSpinbox->value(); + settings.gpsPositionEnabled = m_optionsPage->gpsPositionCheckbox->isChecked(); + settings.gpsPosRate = m_optionsPage->gpsPosRateSpinbox->value(); - settings.groundTruthEnabled = m_optionsPage->groundTruthCheckbox->isChecked(); - settings.groundTruthRate = m_optionsPage->groundTruthRateSpinbox->value(); + settings.groundTruthEnabled = m_optionsPage->groundTruthCheckbox->isChecked(); + settings.groundTruthRate = m_optionsPage->groundTruthRateSpinbox->value(); - settings.baroAltitudeEnabled = m_optionsPage->baroAltitudeCheckbox->isChecked(); - settings.baroAltRate = m_optionsPage->baroAltRateSpinbox->value(); + settings.baroAltitudeEnabled = m_optionsPage->baroAltitudeCheckbox->isChecked(); + settings.baroAltRate = m_optionsPage->baroAltRateSpinbox->value(); - settings.minOutputPeriod = m_optionsPage->minOutputPeriodSpinbox->value(); + settings.minOutputPeriod = m_optionsPage->minOutputPeriodSpinbox->value(); - settings.manualControlEnabled = m_optionsPage->manualControlRadioButton->isChecked(); - settings.gcsReceiverEnabled = m_optionsPage->gcsReceiverRadioButton->isChecked(); + settings.manualControlEnabled = m_optionsPage->manualControlRadioButton->isChecked(); + settings.gcsReceiverEnabled = m_optionsPage->gcsReceiverRadioButton->isChecked(); - settings.attActHW = m_optionsPage->attActHW->isChecked(); - settings.attActSim = m_optionsPage->attActSim->isChecked(); - settings.attActCalc = m_optionsPage->attActCalc->isChecked(); + settings.attActHW = m_optionsPage->attActHW->isChecked(); + settings.attActSim = m_optionsPage->attActSim->isChecked(); + settings.attActCalc = m_optionsPage->attActCalc->isChecked(); - settings.airspeedActualEnabled=m_optionsPage->airspeedActualCheckbox->isChecked(); - settings.airspeedActualRate=m_optionsPage->airspeedRateSpinbox->value(); + settings.airspeedActualEnabled = m_optionsPage->airspeedActualCheckbox->isChecked(); + settings.airspeedActualRate = m_optionsPage->airspeedRateSpinbox->value(); - //Write settings to file + // Write settings to file config->setSimulatorSettings(settings); } diff --git a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.h b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.h index a6749cc12..8a34d419b 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.h @@ -7,21 +7,21 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -39,27 +39,28 @@ class HITLConfiguration; using namespace Core; namespace Ui { - class HITLOptionsPage; +class HITLOptionsPage; } -class HITLOptionsPage : public IOptionsPage -{ -Q_OBJECT +class HITLOptionsPage : public IOptionsPage { + Q_OBJECT public: - explicit HITLOptionsPage(HITLConfiguration *conf, QObject *parent = 0); + explicit HITLOptionsPage(HITLConfiguration *conf, QObject *parent = 0); - QWidget *createPage(QWidget *parent); + QWidget *createPage(QWidget *parent); void apply(); void finish(); - bool isDecorated() const { return true;} + bool isDecorated() const + { + return true; + } signals: private slots: private: - HITLConfiguration* config; - Ui::HITLOptionsPage* m_optionsPage; - + HITLConfiguration *config; + Ui::HITLOptionsPage *m_optionsPage; }; #endif // HITLOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp index 351c62610..a1bd18e8a 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,42 +34,41 @@ #include "il2simulator.h" #include "xplanesimulator.h" -QList HITLPlugin::typeSimulators; +QList HITLPlugin::typeSimulators; HITLPlugin::HITLPlugin() { - // Do nothing + // Do nothing } HITLPlugin::~HITLPlugin() { - // Do nothing + // Do nothing } -bool HITLPlugin::initialize(const QStringList& args, QString *errMsg) +bool HITLPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new HITLFactory(this); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new HITLFactory(this); - addAutoReleasedObject(mf); + addAutoReleasedObject(mf); - addSimulator(new AeroSimRCSimulatorCreator("ASimRC", "AeroSimRC")); - addSimulator(new FGSimulatorCreator("FG","FlightGear")); - addSimulator(new IL2SimulatorCreator("IL2","IL2")); - addSimulator(new XplaneSimulatorCreator("X-Plane","X-Plane")); + addSimulator(new AeroSimRCSimulatorCreator("ASimRC", "AeroSimRC")); + addSimulator(new FGSimulatorCreator("FG", "FlightGear")); + addSimulator(new IL2SimulatorCreator("IL2", "IL2")); + addSimulator(new XplaneSimulatorCreator("X-Plane", "X-Plane")); - return true; + return true; } void HITLPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void HITLPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(HITLPlugin) - diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlplugin.h b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.h index 5639c3be4..cb7254edb 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlplugin.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,37 +35,34 @@ class HITLFactory; -class HITLPlugin : public ExtensionSystem::IPlugin -{ +class HITLPlugin : public ExtensionSystem::IPlugin { public: HITLPlugin(); - ~HITLPlugin(); + ~HITLPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); - static void addSimulator(SimulatorCreator* creator) - { - HITLPlugin::typeSimulators.append(creator); - } + static void addSimulator(SimulatorCreator *creator) + { + HITLPlugin::typeSimulators.append(creator); + } - static SimulatorCreator* getSimulatorCreator(const QString classId) - { - foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) - { - if(classId == creator->ClassId()) - return creator; - } - return 0; - } + static SimulatorCreator *getSimulatorCreator(const QString classId) + { + foreach(SimulatorCreator * creator, HITLPlugin::typeSimulators) { + if (classId == creator->ClassId()) { + return creator; + } + } + return 0; + } - static QList typeSimulators; + static QList typeSimulators; private: - HITLFactory *mf; - - + HITLFactory *mf; }; #endif /* HITLPLUGIN_H */ diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlwidget.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlwidget.cpp index f29035f39..42e449b7c 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlwidget.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlwidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -43,139 +43,129 @@ QStringList Simulator::instances; HITLWidget::HITLWidget(QWidget *parent) - : QWidget(parent), - simulator(0) + : QWidget(parent), + simulator(0) { - widget = new Ui_HITLWidget(); - widget->setupUi(this); - widget->startButton->setEnabled(true); - widget->stopButton->setEnabled(false); + widget = new Ui_HITLWidget(); + widget->setupUi(this); + widget->startButton->setEnabled(true); + widget->stopButton->setEnabled(false); - greenColor = "rgb(35, 221, 35)"; //Change the green color in order to make it a bit more vibrant - strStyleEnable = QString("QFrame{background-color: %1; color: white}").arg(greenColor); + greenColor = "rgb(35, 221, 35)"; // Change the green color in order to make it a bit more vibrant + strStyleEnable = QString("QFrame{background-color: %1; color: white}").arg(greenColor); strStyleDisable = "QFrame{background-color: red; color: white}"; strAutopilotDisconnected = " Autopilot OFF "; strSimulatorDisconnected = " Simulator OFF "; - strAutopilotConnected = " Autopilot ON "; + strAutopilotConnected = " Autopilot ON "; - widget->apLabel->setText(strAutopilotDisconnected); - widget->simLabel->setText(strSimulatorDisconnected); + widget->apLabel->setText(strAutopilotDisconnected); + widget->simLabel->setText(strSimulatorDisconnected); - connect(widget->startButton, SIGNAL(clicked()), this, SLOT(startButtonClicked())); - connect(widget->stopButton, SIGNAL(clicked()), this, SLOT(stopButtonClicked())); - connect(widget->buttonClearLog, SIGNAL(clicked()), this, SLOT(buttonClearLogClicked())); + connect(widget->startButton, SIGNAL(clicked()), this, SLOT(startButtonClicked())); + connect(widget->stopButton, SIGNAL(clicked()), this, SLOT(stopButtonClicked())); + connect(widget->buttonClearLog, SIGNAL(clicked()), this, SLOT(buttonClearLogClicked())); } HITLWidget::~HITLWidget() { - delete widget; + delete widget; } void HITLWidget::startButtonClicked() { - QThread* mainThread = QThread::currentThread(); - qDebug() << "Main Thread: "<< mainThread; + QThread *mainThread = QThread::currentThread(); - //Allow only one instance per simulator - if(Simulator::Instances().indexOf(settings.simulatorId) != -1) - { - widget->textBrowser->append(settings.simulatorId + " alreary started!"); - return; - } + qDebug() << "Main Thread: " << mainThread; - if(!HITLPlugin::typeSimulators.size()) - { - qxtLog->info("There is no registered simulators, add through HITLPlugin::addSimulator"); - return; - } + // Allow only one instance per simulator + if (Simulator::Instances().indexOf(settings.simulatorId) != -1) { + widget->textBrowser->append(settings.simulatorId + " alreary started!"); + return; + } - // Stop running process if one is active - if(simulator) - { - QMetaObject::invokeMethod(simulator, "onDeleteSimulator",Qt::QueuedConnection); - simulator = NULL; - } + if (!HITLPlugin::typeSimulators.size()) { + qxtLog->info("There is no registered simulators, add through HITLPlugin::addSimulator"); + return; + } - if(settings.hostAddress == "" || settings.inPort == 0) - { - widget->textBrowser->append("Before start, set UDP parameters in options page!"); - return; - } + // Stop running process if one is active + if (simulator) { + QMetaObject::invokeMethod(simulator, "onDeleteSimulator", Qt::QueuedConnection); + simulator = NULL; + } - SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(settings.simulatorId); + if (settings.hostAddress == "" || settings.inPort == 0) { + widget->textBrowser->append("Before start, set UDP parameters in options page!"); + return; + } + + SimulatorCreator *creator = HITLPlugin::getSimulatorCreator(settings.simulatorId); simulator = creator->createSimulator(settings); // move to thread <--[BCH] - simulator->setName(creator->Description()); - simulator->setSimulatorId(creator->ClassId()); + simulator->setName(creator->Description()); + simulator->setSimulatorId(creator->ClassId()); connect(simulator, SIGNAL(processOutput(QString)), this, SLOT(onProcessOutput(QString))); - // Setup process - widget->textBrowser->append(QString("[%1] Starting %2... ").arg(QTime::currentTime().toString("hh:mm:ss")).arg(creator->Description())); - qxtLog->info("HITL: Starting " + creator->Description()); + // Setup process + widget->textBrowser->append(QString("[%1] Starting %2... ").arg(QTime::currentTime().toString("hh:mm:ss")).arg(creator->Description())); + qxtLog->info("HITL: Starting " + creator->Description()); - // Start bridge - bool ret = QMetaObject::invokeMethod(simulator, "setupProcess",Qt::QueuedConnection); - if(ret) - { - Simulator::setInstance(settings.simulatorId); + // Start bridge + bool ret = QMetaObject::invokeMethod(simulator, "setupProcess", Qt::QueuedConnection); + if (ret) { + Simulator::setInstance(settings.simulatorId); - connect(this,SIGNAL(deleteSimulator()),simulator, SLOT(onDeleteSimulator()),Qt::QueuedConnection); + connect(this, SIGNAL(deleteSimulator()), simulator, SLOT(onDeleteSimulator()), Qt::QueuedConnection); - widget->startButton->setEnabled(false); - widget->stopButton->setEnabled(true); - qxtLog->info("HITL: Starting bridge, initializing flight simulator and Autopilot connections"); + widget->startButton->setEnabled(false); + widget->stopButton->setEnabled(true); + qxtLog->info("HITL: Starting bridge, initializing flight simulator and Autopilot connections"); - connect(simulator, SIGNAL(autopilotConnected()), this, SLOT(onAutopilotConnect()),Qt::QueuedConnection); - connect(simulator, SIGNAL(autopilotDisconnected()), this, SLOT(onAutopilotDisconnect()),Qt::QueuedConnection); - connect(simulator, SIGNAL(simulatorConnected()), this, SLOT(onSimulatorConnect()),Qt::QueuedConnection); - connect(simulator, SIGNAL(simulatorDisconnected()), this, SLOT(onSimulatorDisconnect()),Qt::QueuedConnection); + connect(simulator, SIGNAL(autopilotConnected()), this, SLOT(onAutopilotConnect()), Qt::QueuedConnection); + connect(simulator, SIGNAL(autopilotDisconnected()), this, SLOT(onAutopilotDisconnect()), Qt::QueuedConnection); + connect(simulator, SIGNAL(simulatorConnected()), this, SLOT(onSimulatorConnect()), Qt::QueuedConnection); + connect(simulator, SIGNAL(simulatorDisconnected()), this, SLOT(onSimulatorDisconnect()), Qt::QueuedConnection); - // Initialize connection status - if ( simulator->isAutopilotConnected() ) - { - onAutopilotConnect(); - } - else - { - onAutopilotDisconnect(); - } + // Initialize connection status + if (simulator->isAutopilotConnected()) { + onAutopilotConnect(); + } else { + onAutopilotDisconnect(); + } - if ( simulator->isSimulatorConnected() ) - { - onSimulatorConnect(); - } - else - { - onSimulatorDisconnect(); - } - } + if (simulator->isSimulatorConnected()) { + onSimulatorConnect(); + } else { + onSimulatorDisconnect(); + } + } } void HITLWidget::stopButtonClicked() { - if(simulator) - widget->textBrowser->append(QString("[%1] Terminate %2 ").arg(QTime::currentTime().toString("hh:mm:ss")).arg(simulator->Name())); + if (simulator) { + widget->textBrowser->append(QString("[%1] Terminate %2 ").arg(QTime::currentTime().toString("hh:mm:ss")).arg(simulator->Name())); + } - widget->startButton->setEnabled(true); - widget->stopButton->setEnabled(false); + widget->startButton->setEnabled(true); + widget->stopButton->setEnabled(false); widget->apLabel->setStyleSheet(QString::fromUtf8("QFrame{background-color: transparent; color: white}")); widget->simLabel->setStyleSheet(QString::fromUtf8("QFrame{background-color: transparent; color: white}")); widget->apLabel->setText(strAutopilotDisconnected); - widget->simLabel->setText(strSimulatorDisconnected); - if(simulator) - { - QMetaObject::invokeMethod(simulator, "onDeleteSimulator",Qt::QueuedConnection); - simulator = NULL; - } + widget->simLabel->setText(strSimulatorDisconnected); + if (simulator) { + QMetaObject::invokeMethod(simulator, "onDeleteSimulator", Qt::QueuedConnection); + simulator = NULL; + } } void HITLWidget::buttonClearLogClicked() { - widget->textBrowser->clear(); + widget->textBrowser->clear(); } void HITLWidget::onProcessOutput(QString text) @@ -187,26 +177,26 @@ void HITLWidget::onAutopilotConnect() { widget->apLabel->setStyleSheet(strStyleEnable); widget->apLabel->setText(strAutopilotConnected); - qxtLog->info("HITL: Autopilot connected, initializing for HITL simulation"); + qxtLog->info("HITL: Autopilot connected, initializing for HITL simulation"); } void HITLWidget::onAutopilotDisconnect() { widget->apLabel->setStyleSheet(strStyleDisable); - widget->apLabel->setText(strAutopilotDisconnected); - qxtLog->info(strAutopilotDisconnected); + widget->apLabel->setText(strAutopilotDisconnected); + qxtLog->info(strAutopilotDisconnected); } void HITLWidget::onSimulatorConnect() { widget->simLabel->setStyleSheet(strStyleEnable); - widget->simLabel->setText(" " + simulator->Name() +" connected "); - qxtLog->info(QString("HITL: %1 connected").arg(simulator->Name())); + widget->simLabel->setText(" " + simulator->Name() + " connected "); + qxtLog->info(QString("HITL: %1 connected").arg(simulator->Name())); } void HITLWidget::onSimulatorDisconnect() { widget->simLabel->setStyleSheet(strStyleDisable); - widget->simLabel->setText(" " + simulator->Name() +" disconnected "); - qxtLog->info(QString("HITL: %1 disconnected").arg(simulator->Name())); + widget->simLabel->setText(" " + simulator->Name() + " disconnected "); + qxtLog->info(QString("HITL: %1 disconnected").arg(simulator->Name())); } diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlwidget.h b/ground/openpilotgcs/src/plugins/hitl/hitlwidget.h index 0a1f1c881..5a746c430 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlwidget.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup HITLPlugin HITL Plugin * @{ - * @brief The Hardware In The Loop plugin + * @brief The Hardware In The Loop plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,37 +34,39 @@ class Ui_HITLWidget; -class HITLWidget : public QWidget -{ +class HITLWidget : public QWidget { Q_OBJECT public: HITLWidget(QWidget *parent = 0); ~HITLWidget(); - void setSettingParameters(const SimulatorSettings& params) {settings = params;} + void setSettingParameters(const SimulatorSettings & params) + { + settings = params; + } signals: - void deleteSimulator(); + void deleteSimulator(); private slots: void startButtonClicked(); void stopButtonClicked(); - void buttonClearLogClicked(); + void buttonClearLogClicked(); void onProcessOutput(QString text); void onAutopilotConnect(); void onAutopilotDisconnect(); - void onSimulatorConnect(); - void onSimulatorDisconnect(); + void onSimulatorConnect(); + void onSimulatorDisconnect(); private: - Ui_HITLWidget* widget; - Simulator* simulator; - SimulatorSettings settings; + Ui_HITLWidget *widget; + Simulator *simulator; + SimulatorSettings settings; - QString greenColor; - QString strAutopilotDisconnected; - QString strSimulatorDisconnected; - QString strAutopilotConnected; + QString greenColor; + QString strAutopilotDisconnected; + QString strSimulatorDisconnected; + QString strAutopilotConnected; QString strStyleEnable; QString strStyleDisable; }; diff --git a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp index f1defa953..c4936d33d 100644 --- a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp @@ -68,34 +68,34 @@ #include #include -const float IL2Simulator::FT2M = 12*.254; -const float IL2Simulator::KT2MPS = 0.514444444; -const float IL2Simulator::MPS2KMH = 3.6; -const float IL2Simulator::KMH2MPS = (1.0/3.6); +const float IL2Simulator::FT2M = 12 * .254; +const float IL2Simulator::KT2MPS = 0.514444444; +const float IL2Simulator::MPS2KMH = 3.6; +const float IL2Simulator::KMH2MPS = (1.0 / 3.6); const float IL2Simulator::INHG2KPA = 3.386; -const float IL2Simulator::RAD2DEG = (180.0/M_PI); -const float IL2Simulator::DEG2RAD = (M_PI/180.0); -const float IL2Simulator::NM2DEG = 60.*1852.; // 60 miles per degree times 1852 meters per mile -const float IL2Simulator::DEG2NM = (1.0/(60.*1852.)); +const float IL2Simulator::RAD2DEG = (180.0 / M_PI); +const float IL2Simulator::DEG2RAD = (M_PI / 180.0); +const float IL2Simulator::NM2DEG = 60. * 1852.; // 60 miles per degree times 1852 meters per mile +const float IL2Simulator::DEG2NM = (1.0 / (60. * 1852.)); -IL2Simulator::IL2Simulator(const SimulatorSettings& params) : +IL2Simulator::IL2Simulator(const SimulatorSettings & params) : Simulator(params) { - airParameters=getAirParameters(); + airParameters = getAirParameters(); } IL2Simulator::~IL2Simulator() -{ -} +{} -void IL2Simulator::setupUdpPorts(const QString& host, int inPort, int outPort) +void IL2Simulator::setupUdpPorts(const QString & host, int inPort, int outPort) { Q_UNUSED(outPort); - inSocket->connectToHost(host,inPort); // IL2 - if(!inSocket->waitForConnected()) + inSocket->connectToHost(host, inPort); // IL2 + if (!inSocket->waitForConnected()) { qxtLog->error(Name() + " cann't connect to UDP Port: " + QString::number(inPort)); + } } void IL2Simulator::transmitUpdate() @@ -104,59 +104,60 @@ void IL2Simulator::transmitUpdate() ActuatorDesired::DataFields actData = actDesired->getData(); float ailerons = actData.Roll; float elevator = actData.Pitch; - float rudder = actData.Yaw; - float throttle = actData.Throttle*2-1.0; + float rudder = actData.Yaw; + float throttle = actData.Throttle * 2 - 1.0; // Send update to Il2 QString cmd; - cmd=QString("R/30/32/40/42/46/48/81\\%1/85\\%2/87\\%3/89\\%4/") - .arg(throttle) - .arg(ailerons) - .arg(elevator) - .arg(rudder); + + cmd = QString("R/30/32/40/42/46/48/81\\%1/85\\%2/87\\%3/89\\%4/") + .arg(throttle) + .arg(ailerons) + .arg(elevator) + .arg(rudder); QByteArray data = cmd.toAscii(); - //outSocket->write(data); - inSocket->write(data); // for IL2 must send to the same port as input!!!!!!!!!!!!! + // outSocket->write(data); + inSocket->write(data); // for IL2 must send to the same port as input!!!!!!!!!!!!! } /** * process data string from flight simulator */ -void IL2Simulator::processUpdate(const QByteArray& inp) +void IL2Simulator::processUpdate(const QByteArray & inp) { // save old flight data to calculate delta's later - old=current; + old = current; QString data(inp); // Split QStringList fields = data.split("/"); // split up response string int t; - for (t=0; t=2) { + if (values.length() >= 2) { int id = values[0].toInt(); float value = values[1].toFloat(); switch (id) { case 30: - current.cas=value * KMH2MPS; + current.cas = value * KMH2MPS; break; case 32: - current.dZ=value; + current.dZ = value; break; case 40: - current.Z=value; + current.Z = value; break; case 42: - current.azimuth=value; + current.azimuth = value; break; case 46: - current.roll=-value; + current.roll = -value; break; case 48: - current.pitch=value; + current.pitch = value; break; } } @@ -164,50 +165,64 @@ void IL2Simulator::processUpdate(const QByteArray& inp) // measure time current.dT = ((float)time->restart()) / 1000.0; - if (current.dT<0.001) current.dT=0.001; - current.T = old.T+current.dT; - current.i = old.i+1; - if (current.i==1) { - old.dRoll=0; - old.dPitch=0; - old.dAzimuth=0; - old.ddX=0; - old.ddX=0; - old.ddX=0; + if (current.dT < 0.001) { + current.dT = 0.001; + } + current.T = old.T + current.dT; + current.i = old.i + 1; + if (current.i == 1) { + old.dRoll = 0; + old.dPitch = 0; + old.dAzimuth = 0; + old.ddX = 0; + old.ddX = 0; + old.ddX = 0; } // calculate TAS from alt and CAS - float gravity =9.805; + float gravity = 9.805; current.tas = cas2tas(current.cas, current.Z, airParameters, gravity); // assume the plane actually flies straight and no wind // groundspeed is horizontal vector of TAS - current.groundspeed = current.tas*cos(current.pitch*DEG2RAD); + current.groundspeed = current.tas * cos(current.pitch * DEG2RAD); // x and y vector components - current.dX = current.groundspeed*sin(current.azimuth*DEG2RAD); - current.dY = current.groundspeed*cos(current.azimuth*DEG2RAD); + current.dX = current.groundspeed * sin(current.azimuth * DEG2RAD); + current.dY = current.groundspeed * cos(current.azimuth * DEG2RAD); // simple IMS - integration over time the easy way... - current.X = old.X + (current.dX*current.dT); - current.Y = old.Y + (current.dY*current.dT); + current.X = old.X + (current.dX * current.dT); + current.Y = old.Y + (current.dY * current.dT); // accelerations (filtered) - if (isnan(old.ddX) || isinf(old.ddX)) old.ddX=0; - if (isnan(old.ddY) || isinf(old.ddY)) old.ddY=0; - if (isnan(old.ddZ) || isinf(old.ddZ)) old.ddZ=0; + if (isnan(old.ddX) || isinf(old.ddX)) { + old.ddX = 0; + } + if (isnan(old.ddY) || isinf(old.ddY)) { + old.ddY = 0; + } + if (isnan(old.ddZ) || isinf(old.ddZ)) { + old.ddZ = 0; + } #define SPEED_FILTER 10 - current.ddX = ((current.dX-old.dX)/current.dT + SPEED_FILTER * (old.ddX)) / (SPEED_FILTER+1); - current.ddY = ((current.dY-old.dY)/current.dT + SPEED_FILTER * (old.ddY)) / (SPEED_FILTER+1); - current.ddZ = ((current.dZ-old.dZ)/current.dT + SPEED_FILTER * (old.ddZ)) / (SPEED_FILTER+1); + current.ddX = ((current.dX - old.dX) / current.dT + SPEED_FILTER * (old.ddX)) / (SPEED_FILTER + 1); + current.ddY = ((current.dY - old.dY) / current.dT + SPEED_FILTER * (old.ddY)) / (SPEED_FILTER + 1); + current.ddZ = ((current.dZ - old.dZ) / current.dT + SPEED_FILTER * (old.ddZ)) / (SPEED_FILTER + 1); -#define TURN_FILTER 10 +#define TURN_FILTER 10 // turn speeds (filtered) - if (isnan(old.dAzimuth) || isinf(old.dAzimuth)) old.dAzimuth=0; - if (isnan(old.dPitch) || isinf(old.dPitch)) old.dPitch=0; - if (isnan(old.dRoll) || isinf(old.dRoll)) old.dRoll=0; - current.dAzimuth = (angleDifference(current.azimuth,old.azimuth)/current.dT + TURN_FILTER * (old.dAzimuth)) / (TURN_FILTER+1); - current.dPitch = (angleDifference(current.pitch,old.pitch)/current.dT + TURN_FILTER * (old.dPitch)) / (TURN_FILTER+1); - current.dRoll = (angleDifference(current.roll,old.roll)/current.dT + TURN_FILTER * (old.dRoll)) / (TURN_FILTER+1); + if (isnan(old.dAzimuth) || isinf(old.dAzimuth)) { + old.dAzimuth = 0; + } + if (isnan(old.dPitch) || isinf(old.dPitch)) { + old.dPitch = 0; + } + if (isnan(old.dRoll) || isinf(old.dRoll)) { + old.dRoll = 0; + } + current.dAzimuth = (angleDifference(current.azimuth, old.azimuth) / current.dT + TURN_FILTER * (old.dAzimuth)) / (TURN_FILTER + 1); + current.dPitch = (angleDifference(current.pitch, old.pitch) / current.dT + TURN_FILTER * (old.dPitch)) / (TURN_FILTER + 1); + current.dRoll = (angleDifference(current.roll, old.roll) / current.dT + TURN_FILTER * (old.dRoll)) / (TURN_FILTER + 1); /////// // Output formatting @@ -219,68 +234,68 @@ void IL2Simulator::processUpdate(const QByteArray& inp) float Rbe[3][3]; float rpy[3]; float quat[4]; - rpy[0]=current.roll; - rpy[1]=current.pitch; - rpy[2]=current.azimuth; - Utils::CoordinateConversions().RPY2Quaternion(rpy,quat); - Utils::CoordinateConversions().Quaternion2R(quat,Rbe); + rpy[0] = current.roll; + rpy[1] = current.pitch; + rpy[2] = current.azimuth; + Utils::CoordinateConversions().RPY2Quaternion(rpy, quat); + Utils::CoordinateConversions().Quaternion2R(quat, Rbe); // Update GPS Position objects double HomeLLA[3]; double LLA[3]; double NED[3]; - HomeLLA[0]=settings.latitude.toFloat(); - HomeLLA[1]=settings.longitude.toFloat(); - HomeLLA[2]=0; - NED[0] = current.Y; - NED[1] = current.X; - NED[2] = -current.Z; - Utils::CoordinateConversions().NED2LLA_HomeLLA(HomeLLA,NED,LLA); - out.latitude = LLA[0] * 1e7; - out.longitude = LLA[1] * 1e7; - out.groundspeed = current.groundspeed; + HomeLLA[0] = settings.latitude.toFloat(); + HomeLLA[1] = settings.longitude.toFloat(); + HomeLLA[2] = 0; + NED[0] = current.Y; + NED[1] = current.X; + NED[2] = -current.Z; + Utils::CoordinateConversions().NED2LLA_HomeLLA(HomeLLA, NED, LLA); + out.latitude = LLA[0] * 1e7; + out.longitude = LLA[1] * 1e7; + out.groundspeed = current.groundspeed; out.calibratedAirspeed = current.cas; - out.trueAirspeed=cas2tas(current.cas, current.Z, airParameters, gravity); + out.trueAirspeed = cas2tas(current.cas, current.Z, airParameters, gravity); - out.dstN=current.Y; - out.dstE=current.X; - out.dstD=-current.Z; + out.dstN = current.Y; + out.dstE = current.X; + out.dstD = -current.Z; // Update BaroAltitude object - out.altitude = current.Z; - out.agl = current.Z; - out.temperature = airParameters.groundTemp + (current.Z * airParameters.tempLapseRate) - 273.0; - out.pressure = airPressureFromAltitude(current.Z, airParameters, gravity) ; // kpa + out.altitude = current.Z; + out.agl = current.Z; + out.temperature = airParameters.groundTemp + (current.Z * airParameters.tempLapseRate) - 273.0; + out.pressure = airPressureFromAltitude(current.Z, airParameters, gravity); // kpa // Update attActual object - out.roll = current.roll; //roll; - out.pitch = current.pitch; // pitch + out.roll = current.roll; // roll; + out.pitch = current.pitch; // pitch out.heading = current.azimuth; // yaw // Update VelocityActual.{North,East,Down} out.velNorth = current.dY; - out.velEast = current.dX; - out.velDown = -current.dZ; + out.velEast = current.dX; + out.velDown = -current.dZ; // rotate turn rates and accelerations into body frame // (note: rotation deltas are NOT in NED frame but in RPY - manual conversion!) - out.rollRate = current.dRoll; + out.rollRate = current.dRoll; out.pitchRate = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth; - out.yawRate = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch; + out.yawRate = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch; - //Update accelerometer sensor data - out.accX = current.ddX*Rbe[0][0] - +current.ddY*Rbe[0][1] - +(current.ddZ+GEE)*Rbe[0][2]; - out.accY = current.ddX*Rbe[1][0] - +current.ddY*Rbe[1][1] - +(current.ddZ+GEE)*Rbe[1][2]; - out.accZ = - (current.ddX*Rbe[2][0] - +current.ddY*Rbe[2][1] - +(current.ddZ+GEE)*Rbe[2][2]); + // Update accelerometer sensor data + out.accX = current.ddX * Rbe[0][0] + + current.ddY * Rbe[0][1] + + (current.ddZ + GEE) * Rbe[0][2]; + out.accY = current.ddX * Rbe[1][0] + + current.ddY * Rbe[1][1] + + (current.ddZ + GEE) * Rbe[1][2]; + out.accZ = -(current.ddX * Rbe[2][0] + + current.ddY * Rbe[2][1] + + (current.ddZ + GEE) * Rbe[2][2]); updateUAVOs(out); } @@ -290,8 +305,13 @@ void IL2Simulator::processUpdate(const QByteArray& inp) */ float IL2Simulator::angleDifference(float a, float b) { - float d=a-b; - if (d>180) d-=360; - if (d<-180)d+=360; + float d = a - b; + + if (d > 180) { + d -= 360; + } + if (d < -180) { + d += 360; + } return d; } diff --git a/ground/openpilotgcs/src/plugins/hitl/il2simulator.h b/ground/openpilotgcs/src/plugins/hitl/il2simulator.h index 62823f9fc..e50702150 100644 --- a/ground/openpilotgcs/src/plugins/hitl/il2simulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/il2simulator.h @@ -31,17 +31,16 @@ #include #include -class IL2Simulator: public Simulator -{ - Q_OBJECT +class IL2Simulator : public Simulator { + Q_OBJECT public: - IL2Simulator(const SimulatorSettings& params); - ~IL2Simulator(); + IL2Simulator(const SimulatorSettings & params); + ~IL2Simulator(); - void setupUdpPorts(const QString& host, int inPort, int outPort); + void setupUdpPorts(const QString & host, int inPort, int outPort); private slots: - void transmitUpdate(); + void transmitUpdate(); private: static const float FT2M; @@ -54,23 +53,22 @@ private: static const float NM2DEG; static const float DEG2NM; - void processUpdate(const QByteArray& data); - float angleDifference(float a,float b); + void processUpdate(const QByteArray & data); + float angleDifference(float a, float b); AirParameters airParameters; }; -class IL2SimulatorCreator : public SimulatorCreator -{ +class IL2SimulatorCreator : public SimulatorCreator { public: - IL2SimulatorCreator(const QString& classId, const QString& description) - : SimulatorCreator (classId,description) - {} + IL2SimulatorCreator(const QString & classId, const QString & description) + : SimulatorCreator(classId, description) + {} - Simulator* createSimulator(const SimulatorSettings& params) - { - return new IL2Simulator(params); - } + Simulator *createSimulator(const SimulatorSettings & params) + { + return new IL2Simulator(params); + } }; #endif // IL2SIMULATOR_H diff --git a/ground/openpilotgcs/src/plugins/hitl/isimulator.h b/ground/openpilotgcs/src/plugins/hitl/isimulator.h index 183a030d7..842e69a8f 100644 --- a/ground/openpilotgcs/src/plugins/hitl/isimulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/isimulator.h @@ -15,59 +15,58 @@ #include "positionactual.h" #include "gcstelemetrystats.h" -class Simulator: public QObject -{ - Q_OBJECT +class Simulator : public QObject { + Q_OBJECT public: - //static ISimulator* Instance(); -//protected: - Simulator(); - ~ISimulator(); + // static ISimulator* Instance(); +// protected: + Simulator(); + ~ISimulator(); - bool isAutopilotConnected(); - bool isFGConnected(); + bool isAutopilotConnected(); + bool isFGConnected(); signals: - void myStart(); - void autopilotConnected(); - void autopilotDisconnected(); - void fgConnected(); - void fgDisconnected(); + void myStart(); + void autopilotConnected(); + void autopilotDisconnected(); + void fgConnected(); + void fgDisconnected(); private slots: - void onStart(); - void transmitUpdate(); - void receiveUpdate(); - void onAutopilotConnect(); - void onAutopilotDisconnect(); - void onFGConnectionTimeout(); - void telStatsUpdated(UAVObject* obj); + void onStart(); + void transmitUpdate(); + void receiveUpdate(); + void onAutopilotConnect(); + void onAutopilotDisconnect(); + void onFGConnectionTimeout(); + void telStatsUpdated(UAVObject *obj); private: - //static ISimulator* _instance; + // static ISimulator* _instance; - QUdpSocket* inSocket; - QUdpSocket* outSocket; - ActuatorDesired* actDesired; - AltitudeActual* altActual; - VelocityActual* velActual; - AttitudeActual* attActual; - PositionActual* posActual; - GCSTelemetryStats* telStats; - QHostAddress fgHost; - int inPort; - int outPort; - int updatePeriod; - QTimer* txTimer; - QTimer* fgTimer; - bool autopilotConnectionStatus; - bool fgConnectionStatus; - int fgTimeout; + QUdpSocket *inSocket; + QUdpSocket *outSocket; + ActuatorDesired *actDesired; + AltitudeActual *altActual; + VelocityActual *velActual; + AttitudeActual *attActual; + PositionActual *posActual; + GCSTelemetryStats *telStats; + QHostAddress fgHost; + int inPort; + int outPort; + int updatePeriod; + QTimer *txTimer; + QTimer *fgTimer; + bool autopilotConnectionStatus; + bool fgConnectionStatus; + int fgTimeout; - void processUpdate(QString& data); - void setupOutputObject(UAVObject* obj, int updatePeriod); - void setupInputObject(UAVObject* obj, int updatePeriod); - void setupObjects(); + void processUpdate(QString & data); + void setupOutputObject(UAVObject *obj, int updatePeriod); + void setupInputObject(UAVObject *obj, int updatePeriod); + void setupObjects(); }; diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp index 01dbfa136..f3d594d36 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp @@ -35,234 +35,228 @@ volatile bool Simulator::isStarted = false; -const float Simulator::GEE = 9.81; -const float Simulator::FT2M = 0.3048; -const float Simulator::KT2MPS = 0.514444444; +const float Simulator::GEE = 9.81; +const float Simulator::FT2M = 0.3048; +const float Simulator::KT2MPS = 0.514444444; const float Simulator::INHG2KPA = 3.386; const float Simulator::FPS2CMPS = 30.48; -const float Simulator::DEG2RAD = (M_PI/180.0); -const float Simulator::RAD2DEG = (180.0/M_PI); +const float Simulator::DEG2RAD = (M_PI / 180.0); +const float Simulator::RAD2DEG = (180.0 / M_PI); -Simulator::Simulator(const SimulatorSettings& params) : - simProcess(NULL), - time(NULL), - inSocket(NULL), - outSocket(NULL), - settings(params), - updatePeriod(50), - simTimeout(8000), - autopilotConnectionStatus(false), - simConnectionStatus(false), - txTimer(NULL), - simTimer(NULL), - name("") +Simulator::Simulator(const SimulatorSettings & params) : + simProcess(NULL), + time(NULL), + inSocket(NULL), + outSocket(NULL), + settings(params), + updatePeriod(50), + simTimeout(8000), + autopilotConnectionStatus(false), + simConnectionStatus(false), + txTimer(NULL), + simTimer(NULL), + name("") { - // move to thread - moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread()); - connect(this, SIGNAL(myStart()), this, SLOT(onStart()),Qt::QueuedConnection); - emit myStart(); + // move to thread + moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread()); + connect(this, SIGNAL(myStart()), this, SLOT(onStart()), Qt::QueuedConnection); + emit myStart(); - QTime currentTime=QTime::currentTime(); - gpsPosTime = currentTime; - groundTruthTime = currentTime; - gcsRcvrTime = currentTime; - attRawTime = currentTime; - baroAltTime = currentTime; + QTime currentTime = QTime::currentTime(); + gpsPosTime = currentTime; + groundTruthTime = currentTime; + gcsRcvrTime = currentTime; + attRawTime = currentTime; + baroAltTime = currentTime; battTime = currentTime; - airspeedActualTime=currentTime; + airspeedActualTime = currentTime; - //Define standard atmospheric constants - airParameters.univGasConstant=8.31447; //[J/(mol·K)] - airParameters.dryAirConstant=287.058; //[J/(kg*K)] - airParameters.groundDensity=1.225; //[kg/m^3] - airParameters.groundTemp=15+273.15; //[K] - airParameters.tempLapseRate=0.0065; //[deg/m] - airParameters.M=0.0289644; //[kg/mol] - airParameters.relativeHumidity=20; //[%] - airParameters.seaLevelPress=101.325; //[kPa] + // Define standard atmospheric constants + airParameters.univGasConstant = 8.31447; // [J/(mol·K)] + airParameters.dryAirConstant = 287.058; // [J/(kg*K)] + airParameters.groundDensity = 1.225; // [kg/m^3] + airParameters.groundTemp = 15 + 273.15; // [K] + airParameters.tempLapseRate = 0.0065; // [deg/m] + airParameters.M = 0.0289644; // [kg/mol] + airParameters.relativeHumidity = 20; // [%] + airParameters.seaLevelPress = 101.325; // [kPa] } Simulator::~Simulator() { - if(inSocket) - { - delete inSocket; - inSocket = NULL; - } + if (inSocket) { + delete inSocket; + inSocket = NULL; + } - if(outSocket) - { - delete outSocket; - outSocket = NULL; - } + if (outSocket) { + delete outSocket; + outSocket = NULL; + } - if(txTimer) - { - delete txTimer; - txTimer = NULL; - } + if (txTimer) { + delete txTimer; + txTimer = NULL; + } - if(simTimer) - { - delete simTimer; - simTimer = NULL; - } - // NOTE: Does not currently work, may need to send control+c to through the terminal - if (simProcess != NULL) - { - //connect(simProcess,SIGNAL(finished(int, QProcess::ExitStatus)),this,SLOT(onFinished(int, QProcess::ExitStatus))); + if (simTimer) { + delete simTimer; + simTimer = NULL; + } + // NOTE: Does not currently work, may need to send control+c to through the terminal + if (simProcess != NULL) { + // connect(simProcess,SIGNAL(finished(int, QProcess::ExitStatus)),this,SLOT(onFinished(int, QProcess::ExitStatus))); - simProcess->disconnect(); - if(simProcess->state() == QProcess::Running) - simProcess->kill(); - //if(simProcess->waitForFinished()) - //emit deleteSimProcess(); - delete simProcess; - simProcess = NULL; - } + simProcess->disconnect(); + if (simProcess->state() == QProcess::Running) { + simProcess->kill(); + } + // if(simProcess->waitForFinished()) + // emit deleteSimProcess(); + delete simProcess; + simProcess = NULL; + } } void Simulator::onDeleteSimulator(void) { - // [1] - Simulator::setStarted(false); - // [2] - Simulator::Instances().removeOne(simulatorId); + // [1] + Simulator::setStarted(false); + // [2] + Simulator::Instances().removeOne(simulatorId); - disconnect(this); - delete this; + disconnect(this); + delete this; } void Simulator::onStart() { QMutexLocker locker(&lock); - QThread* mainThread = QThread::currentThread(); + QThread *mainThread = QThread::currentThread(); - qDebug() << "Simulator Thread: "<< mainThread; + qDebug() << "Simulator Thread: " << mainThread; // Get required UAVObjects - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager* objManager = pm->getObject(); - actDesired = ActuatorDesired::GetInstance(objManager); - actCommand = ActuatorCommand::GetInstance(objManager); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + actDesired = ActuatorDesired::GetInstance(objManager); + actCommand = ActuatorCommand::GetInstance(objManager); manCtrlCommand = ManualControlCommand::GetInstance(objManager); - gcsReceiver = GCSReceiver::GetInstance(objManager); - flightStatus = FlightStatus::GetInstance(objManager); - posHome = HomeLocation::GetInstance(objManager); - velActual = VelocityActual::GetInstance(objManager); - posActual = PositionActual::GetInstance(objManager); - baroAlt = BaroAltitude::GetInstance(objManager); - flightBatt = FlightBatteryState::GetInstance(objManager); + gcsReceiver = GCSReceiver::GetInstance(objManager); + flightStatus = FlightStatus::GetInstance(objManager); + posHome = HomeLocation::GetInstance(objManager); + velActual = VelocityActual::GetInstance(objManager); + posActual = PositionActual::GetInstance(objManager); + baroAlt = BaroAltitude::GetInstance(objManager); + flightBatt = FlightBatteryState::GetInstance(objManager); airspeedActual = AirspeedActual::GetInstance(objManager); - attActual = AttitudeActual::GetInstance(objManager); - attSettings = AttitudeSettings::GetInstance(objManager); - accels = Accels::GetInstance(objManager); - gyros = Gyros::GetInstance(objManager); - gpsPos = GPSPosition::GetInstance(objManager); - gpsVel = GPSVelocity::GetInstance(objManager); - telStats = GCSTelemetryStats::GetInstance(objManager); + attActual = AttitudeActual::GetInstance(objManager); + attSettings = AttitudeSettings::GetInstance(objManager); + accels = Accels::GetInstance(objManager); + gyros = Gyros::GetInstance(objManager); + gpsPos = GPSPosition::GetInstance(objManager); + gpsVel = GPSVelocity::GetInstance(objManager); + telStats = GCSTelemetryStats::GetInstance(objManager); groundTruth = GroundTruth::GetInstance(objManager); // Listen to autopilot connection events - TelemetryManager* telMngr = pm->getObject(); + TelemetryManager *telMngr = pm->getObject(); connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); - //connect(telStats, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(telStatsUpdated(UAVObject*))); + // connect(telStats, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(telStatsUpdated(UAVObject*))); // If already connect setup autopilot GCSTelemetryStats::DataFields stats = telStats->getData(); - if ( stats.Status == GCSTelemetryStats::STATUS_CONNECTED ) + if (stats.Status == GCSTelemetryStats::STATUS_CONNECTED) { onAutopilotConnect(); + } - inSocket = new QUdpSocket(); + inSocket = new QUdpSocket(); outSocket = new QUdpSocket(); - setupUdpPorts(settings.hostAddress,settings.inPort,settings.outPort); + setupUdpPorts(settings.hostAddress, settings.inPort, settings.outPort); - emit processOutput("\nLocal interface: " + settings.hostAddress + "\n" + \ - "Remote interface: " + settings.remoteAddress + "\n" + \ - "inputPort: " + QString::number(settings.inPort) + "\n" + \ - "outputPort: " + QString::number(settings.outPort) + "\n"); + emit processOutput("\nLocal interface: " + settings.hostAddress + "\n" + \ + "Remote interface: " + settings.remoteAddress + "\n" + \ + "inputPort: " + QString::number(settings.inPort) + "\n" + \ + "outputPort: " + QString::number(settings.outPort) + "\n"); - qxtLog->info("\nLocal interface: " + settings.hostAddress + "\n" + \ - "Remote interface: " + settings.remoteAddress + "\n" + \ - "inputPort: " + QString::number(settings.inPort) + "\n" + \ - "outputPort: " + QString::number(settings.outPort) + "\n"); + qxtLog->info("\nLocal interface: " + settings.hostAddress + "\n" + \ + "Remote interface: " + settings.remoteAddress + "\n" + \ + "inputPort: " + QString::number(settings.inPort) + "\n" + \ + "outputPort: " + QString::number(settings.outPort) + "\n"); -// if(!inSocket->waitForConnected(5000)) -// emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.inPort)); -// outSocket->connectToHost(settings.hostAddress,settings.outPort); // FG -// if(!outSocket->waitForConnected(5000)) -// emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.outPort)); +// if(!inSocket->waitForConnected(5000)) +// emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.inPort)); +// outSocket->connectToHost(settings.hostAddress,settings.outPort); // FG +// if(!outSocket->waitForConnected(5000)) +// emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.outPort)); - connect(inSocket, SIGNAL(readyRead()), this, SLOT(receiveUpdate()),Qt::DirectConnection); + connect(inSocket, SIGNAL(readyRead()), this, SLOT(receiveUpdate()), Qt::DirectConnection); - // Setup transmit timer - txTimer = new QTimer(); - connect(txTimer, SIGNAL(timeout()), this, SLOT(transmitUpdate()),Qt::DirectConnection); - txTimer->setInterval(updatePeriod); - txTimer->start(); - // Setup simulator connection timer - simTimer = new QTimer(); - connect(simTimer, SIGNAL(timeout()), this, SLOT(onSimulatorConnectionTimeout()),Qt::DirectConnection); - simTimer->setInterval(simTimeout); - simTimer->start(); - - // setup time - time = new QTime(); - time->start(); - current.T=0; - current.i=0; + // Setup transmit timer + txTimer = new QTimer(); + connect(txTimer, SIGNAL(timeout()), this, SLOT(transmitUpdate()), Qt::DirectConnection); + txTimer->setInterval(updatePeriod); + txTimer->start(); + // Setup simulator connection timer + simTimer = new QTimer(); + connect(simTimer, SIGNAL(timeout()), this, SLOT(onSimulatorConnectionTimeout()), Qt::DirectConnection); + simTimer->setInterval(simTimeout); + simTimer->start(); + // setup time + time = new QTime(); + time->start(); + current.T = 0; + current.i = 0; } void Simulator::receiveUpdate() { - // Update connection timer and status - simTimer->setInterval(simTimeout); - simTimer->stop(); - simTimer->start(); - if ( !simConnectionStatus ) - { - simConnectionStatus = true; - emit simulatorConnected(); - } + // Update connection timer and status + simTimer->setInterval(simTimeout); + simTimer->stop(); + simTimer->start(); + if (!simConnectionStatus) { + simConnectionStatus = true; + emit simulatorConnected(); + } - // Process data - while(inSocket->hasPendingDatagrams()) { - // Receive datagram - QByteArray datagram; - datagram.resize(inSocket->pendingDatagramSize()); - QHostAddress sender; - quint16 senderPort; - inSocket->readDatagram(datagram.data(), datagram.size(), - &sender, &senderPort); - //QString datastr(datagram); - // Process incomming data - processUpdate(datagram); - } + // Process data + while (inSocket->hasPendingDatagrams()) { + // Receive datagram + QByteArray datagram; + datagram.resize(inSocket->pendingDatagramSize()); + QHostAddress sender; + quint16 senderPort; + inSocket->readDatagram(datagram.data(), datagram.size(), + &sender, &senderPort); + // QString datastr(datagram); + // Process incomming data + processUpdate(datagram); + } } void Simulator::setupObjects() { - if (settings.gcsReceiverEnabled) { - setupInputObject(actCommand, settings.minOutputPeriod); //Input to the simulator + setupInputObject(actCommand, settings.minOutputPeriod); // Input to the simulator setupOutputObject(gcsReceiver, settings.minOutputPeriod); } else if (settings.manualControlEnabled) { - setupInputObject(actDesired, settings.minOutputPeriod); //Input to the simulator + setupInputObject(actDesired, settings.minOutputPeriod); // Input to the simulator } - setupOutputObject(posHome, 10000); //Hardcoded? Bleh. + setupOutputObject(posHome, 10000); // Hardcoded? Bleh. - if (settings.gpsPositionEnabled){ + if (settings.gpsPositionEnabled) { setupOutputObject(gpsPos, settings.gpsPosRate); setupOutputObject(gpsVel, settings.gpsPosRate); } - if (settings.groundTruthEnabled){ + if (settings.groundTruthEnabled) { setupOutputObject(posActual, settings.groundTruthRate); setupOutputObject(velActual, settings.groundTruthRate); } @@ -272,31 +266,31 @@ void Simulator::setupObjects() setupOutputObject(gyros, settings.attRawRate); } - if (settings.attActualEnabled && settings.attActHW) { + if (settings.attActualEnabled && settings.attActHW) { setupOutputObject(accels, settings.attRawRate); setupOutputObject(gyros, settings.attRawRate); } - if (settings.attActualEnabled && !settings.attActHW) - setupOutputObject(attActual, 20); //Hardcoded? Bleh. - else - setupWatchedObject(attActual, 100); //Hardcoded? Bleh. - - if(settings.airspeedActualEnabled) + if (settings.attActualEnabled && !settings.attActHW) { + setupOutputObject(attActual, 20); // Hardcoded? Bleh. + } else { + setupWatchedObject(attActual, 100); // Hardcoded? Bleh. + } + if (settings.airspeedActualEnabled) { setupOutputObject(airspeedActual, settings.airspeedActualRate); + } - if(settings.baroAltitudeEnabled) - { + if (settings.baroAltitudeEnabled) { setupOutputObject(baroAlt, settings.baroAltRate); setupOutputObject(flightBatt, settings.baroAltRate); } - } -void Simulator::setupInputObject(UAVObject* obj, quint32 updatePeriod) +void Simulator::setupInputObject(UAVObject *obj, quint32 updatePeriod) { UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READONLY); @@ -317,6 +311,7 @@ void Simulator::setupInputObject(UAVObject* obj, quint32 updatePeriod) void Simulator::setupWatchedObject(UAVObject *obj, quint32 updatePeriod) { UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READONLY); @@ -333,99 +328,95 @@ void Simulator::setupWatchedObject(UAVObject *obj, quint32 updatePeriod) } -void Simulator::setupOutputObject(UAVObject* obj, quint32 updatePeriod) +void Simulator::setupOutputObject(UAVObject *obj, quint32 updatePeriod) { - UAVObject::Metadata mdata; - mdata = obj->getDefaultMetadata(); + UAVObject::Metadata mdata; + + mdata = obj->getDefaultMetadata(); UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READWRITE); UAVObject::SetGcsTelemetryAcked(mdata, false); - UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); + UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.gcsTelemetryUpdatePeriod = updatePeriod; UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY); - UAVObject::SetFlightTelemetryUpdateMode(mdata,UAVObject::UPDATEMODE_MANUAL); + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL); obj->setMetadata(mdata); } void Simulator::onAutopilotConnect() { - autopilotConnectionStatus = true; - setupObjects(); - emit autopilotConnected(); + autopilotConnectionStatus = true; + setupObjects(); + emit autopilotConnected(); } void Simulator::onAutopilotDisconnect() { - autopilotConnectionStatus = false; - emit autopilotDisconnected(); + autopilotConnectionStatus = false; + emit autopilotDisconnected(); } void Simulator::onSimulatorConnectionTimeout() { - if ( simConnectionStatus ) - { - simConnectionStatus = false; - emit simulatorDisconnected(); - } + if (simConnectionStatus) { + simConnectionStatus = false; + emit simulatorDisconnected(); + } } -void Simulator::telStatsUpdated(UAVObject* obj) +void Simulator::telStatsUpdated(UAVObject *obj) { Q_UNUSED(obj); GCSTelemetryStats::DataFields stats = telStats->getData(); - if ( !autopilotConnectionStatus && stats.Status == GCSTelemetryStats::STATUS_CONNECTED ) - { - onAutopilotConnect(); - } - else if ( autopilotConnectionStatus && stats.Status != GCSTelemetryStats::STATUS_CONNECTED ) - { - onAutopilotDisconnect(); - } + if (!autopilotConnectionStatus && stats.Status == GCSTelemetryStats::STATUS_CONNECTED) { + onAutopilotConnect(); + } else if (autopilotConnectionStatus && stats.Status != GCSTelemetryStats::STATUS_CONNECTED) { + onAutopilotDisconnect(); + } } -void Simulator::resetInitialHomePosition(){ - once=false; +void Simulator::resetInitialHomePosition() +{ + once = false; } -void Simulator::updateUAVOs(Output2Hardware out){ - +void Simulator::updateUAVOs(Output2Hardware out) +{ QTime currentTime = QTime::currentTime(); Noise noise; HitlNoiseGeneration noiseSource; - if(settings.addNoise){ + if (settings.addNoise) { noise = noiseSource.generateNoise(); - } - else{ + } else { memset(&noise, 0, sizeof(Noise)); } /*******************************/ HomeLocation::DataFields homeData = posHome->getData(); - if(!once) - { + if (!once) { // Upon startup, we reset the HomeLocation object to // the plane's location: memset(&homeData, 0, sizeof(HomeLocation::DataFields)); // Update homelocation - homeData.Latitude = out.latitude; //Already in *10^7 integer format - homeData.Longitude = out.longitude; //Already in *10^7 integer format - homeData.Altitude = out.agl; + homeData.Latitude = out.latitude; // Already in *10^7 integer format + homeData.Longitude = out.longitude; // Already in *10^7 integer format + homeData.Altitude = out.agl; double LLA[3]; - LLA[0]=out.latitude; - LLA[1]=out.longitude; - LLA[2]=out.altitude; + LLA[0] = out.latitude; + LLA[1] = out.longitude; + LLA[2] = out.altitude; - homeData.Be[0]=0; - homeData.Be[1]=0; - homeData.Be[2]=0; + homeData.Be[0] = 0; + homeData.Be[1] = 0; + homeData.Be[2] = 0; posHome->setData(homeData); posHome->updated(); @@ -434,40 +425,40 @@ void Simulator::updateUAVOs(Output2Hardware out){ initE = out.dstE; initD = out.dstD; - once=true; + once = true; } /*******************************/ - //Copy everything to the ground truth object. GroundTruth is Noise-free. + // Copy everything to the ground truth object. GroundTruth is Noise-free. GroundTruth::DataFields groundTruthData; groundTruthData = groundTruth->getData(); - groundTruthData.AccelerationXYZ[0]=out.accX; - groundTruthData.AccelerationXYZ[1]=out.accY; - groundTruthData.AccelerationXYZ[2]=out.accZ; + groundTruthData.AccelerationXYZ[0] = out.accX; + groundTruthData.AccelerationXYZ[1] = out.accY; + groundTruthData.AccelerationXYZ[2] = out.accZ; - groundTruthData.AngularRates[0]=out.rollRate; - groundTruthData.AngularRates[1]=out.pitchRate; - groundTruthData.AngularRates[2]=out.yawRate; + groundTruthData.AngularRates[0] = out.rollRate; + groundTruthData.AngularRates[1] = out.pitchRate; + groundTruthData.AngularRates[2] = out.yawRate; - groundTruthData.CalibratedAirspeed=out.calibratedAirspeed; - groundTruthData.TrueAirspeed=out.trueAirspeed; - groundTruthData.AngleOfAttack=out.angleOfAttack; - groundTruthData.AngleOfSlip=out.angleOfSlip; + groundTruthData.CalibratedAirspeed = out.calibratedAirspeed; + groundTruthData.TrueAirspeed = out.trueAirspeed; + groundTruthData.AngleOfAttack = out.angleOfAttack; + groundTruthData.AngleOfSlip = out.angleOfSlip; - groundTruthData.PositionNED[0]=out.dstN-initN; - groundTruthData.PositionNED[1]=out.dstE-initD; - groundTruthData.PositionNED[2]=out.dstD-initD; + groundTruthData.PositionNED[0] = out.dstN - initN; + groundTruthData.PositionNED[1] = out.dstE - initD; + groundTruthData.PositionNED[2] = out.dstD - initD; - groundTruthData.VelocityNED[0]=out.velNorth; - groundTruthData.VelocityNED[1]=out.velEast; - groundTruthData.VelocityNED[2]=out.velDown; + groundTruthData.VelocityNED[0] = out.velNorth; + groundTruthData.VelocityNED[1] = out.velEast; + groundTruthData.VelocityNED[2] = out.velDown; - groundTruthData.RPY[0]=out.roll; - groundTruthData.RPY[0]=out.pitch; - groundTruthData.RPY[0]=out.heading; + groundTruthData.RPY[0] = out.roll; + groundTruthData.RPY[0] = out.pitch; + groundTruthData.RPY[0] = out.heading; - //Set UAVO + // Set UAVO groundTruth->setData(groundTruthData); /*******************************/ @@ -480,33 +471,33 @@ void Simulator::updateUAVOs(Output2Hardware out){ /*****************************************/ } else if (settings.attActSim) { // take all data from simulator - attActualData.Roll = out.roll + noise.attActualData.Roll; //roll; - attActualData.Pitch = out.pitch + noise.attActualData.Pitch; // pitch - attActualData.Yaw = out.heading + noise.attActualData.Yaw; // Yaw + attActualData.Roll = out.roll + noise.attActualData.Roll; // roll; + attActualData.Pitch = out.pitch + noise.attActualData.Pitch; // pitch + attActualData.Yaw = out.heading + noise.attActualData.Yaw; // Yaw float rpy[3]; float quat[4]; rpy[0] = attActualData.Roll; rpy[1] = attActualData.Pitch; rpy[2] = attActualData.Yaw; - Utils::CoordinateConversions().RPY2Quaternion(rpy,quat); + Utils::CoordinateConversions().RPY2Quaternion(rpy, quat); attActualData.q1 = quat[0]; attActualData.q2 = quat[1]; attActualData.q3 = quat[2]; attActualData.q4 = quat[3]; - //Set UAVO + // Set UAVO attActual->setData(attActualData); /*****************************************/ } else if (settings.attActCalc) { // calculate RPY with code from Attitude module - static float q[4] = {1, 0, 0, 0}; + static float q[4] = { 1, 0, 0, 0 }; static float gyro_correct_int2 = 0; float dT = out.delT; AttitudeSettings::DataFields attSettData = attSettings->getData(); - float accelKp = attSettData.AccelKp * 0.1666666666666667; - float accelKi = attSettData.AccelKp * 0.1666666666666667; + float accelKp = attSettData.AccelKp * 0.1666666666666667; + float accelKi = attSettData.AccelKp * 0.1666666666666667; float yawBiasRate = attSettData.YawBiasRate; // calibrate sensors on arming @@ -515,8 +506,8 @@ void Simulator::updateUAVOs(Output2Hardware out){ accelKi = 0.9; } - float gyro[3] = {out.rollRate, out.pitchRate, out.yawRate}; - float attRawAcc[3] = {out.accX, out.accY, out.accZ}; + float gyro[3] = { out.rollRate, out.pitchRate, out.yawRate }; + float attRawAcc[3] = { out.accX, out.accY, out.accZ }; // code from Attitude module begin /////////////////////////////// float *accels = attRawAcc; @@ -526,13 +517,13 @@ void Simulator::updateUAVOs(Output2Hardware out){ // Rotate gravity to body frame and cross with accels grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); - grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); + grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); // CrossProduct { - accel_err[0] = accels[1]*grot[2] - grot[1]*accels[2]; - accel_err[1] = grot[0]*accels[2] - accels[0]*grot[2]; - accel_err[2] = accels[0]*grot[1] - grot[0]*accels[1]; + accel_err[0] = accels[1] * grot[2] - grot[1] * accels[2]; + accel_err[1] = grot[0] * accels[2] - accels[0] * grot[2]; + accel_err[2] = accels[0] * grot[1] - grot[0] * accels[1]; } // Account for accel magnitude @@ -558,12 +549,12 @@ void Simulator::updateUAVOs(Output2Hardware out){ qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2; // Take a time step - q[0] += qdot[0]; - q[1] += qdot[1]; - q[2] += qdot[2]; - q[3] += qdot[3]; + q[0] += qdot[0]; + q[1] += qdot[1]; + q[2] += qdot[2]; + q[3] += qdot[3]; - if(q[0] < 0) { + if (q[0] < 0) { q[0] = -q[0]; q[1] = -q[1]; q[2] = -q[2]; @@ -579,7 +570,7 @@ void Simulator::updateUAVOs(Output2Hardware out){ // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN - if((fabs(qmag) < 1e-3) || (qmag != qmag)) { + if ((fabs(qmag) < 1e-3) || (qmag != qmag)) { q[0] = 1; q[1] = 0; q[2] = 0; @@ -590,19 +581,19 @@ void Simulator::updateUAVOs(Output2Hardware out){ // Quaternion2RPY { float q0s, q1s, q2s, q3s; - q0s = q[0] * q[0]; - q1s = q[1] * q[1]; - q2s = q[2] * q[2]; - q3s = q[3] * q[3]; + q0s = q[0] * q[0]; + q1s = q[1] * q[1]; + q2s = q[2] * q[2]; + q3s = q[3] * q[3]; float R13, R11, R12, R23, R33; - R13 = 2 * (q[1] * q[3] - q[0] * q[2]); - R11 = q0s + q1s - q2s - q3s; - R12 = 2 * (q[1] * q[2] + q[0] * q[3]); - R23 = 2 * (q[2] * q[3] + q[0] * q[1]); - R33 = q0s - q1s - q2s + q3s; + R13 = 2 * (q[1] * q[3] - q[0] * q[2]); + R11 = q0s + q1s - q2s - q3s; + R12 = 2 * (q[1] * q[2] + q[0] * q[3]); + R23 = 2 * (q[2] * q[3] + q[0] * q[1]); + R33 = q0s - q1s - q2s + q3s; - rpy2[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 + rpy2[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 rpy2[2] = RAD2DEG * atan2f(R12, R11); rpy2[0] = RAD2DEG * atan2f(R23, R33); } @@ -610,12 +601,12 @@ void Simulator::updateUAVOs(Output2Hardware out){ attActualData.Roll = rpy2[0]; attActualData.Pitch = rpy2[1]; attActualData.Yaw = rpy2[2]; - attActualData.q1 = q[0]; - attActualData.q2 = q[1]; - attActualData.q3 = q[2]; - attActualData.q4 = q[3]; + attActualData.q1 = q[0]; + attActualData.q2 = q[1]; + attActualData.q3 = q[2]; + attActualData.q4 = q[3]; - //Set UAVO + // Set UAVO attActual->setData(attActualData); /*****************************************/ } @@ -626,14 +617,13 @@ void Simulator::updateUAVOs(Output2Hardware out){ GCSReceiver::DataFields gcsRcvrData; memset(&gcsRcvrData, 0, sizeof(GCSReceiver::DataFields)); - for (quint16 i = 0; i < GCSReceiver::CHANNEL_NUMELEM; i++){ - gcsRcvrData.Channel[i] = 1500 + (out.rc_channel[i]*500); //Elements in rc_channel are between -1 and 1 + for (quint16 i = 0; i < GCSReceiver::CHANNEL_NUMELEM; i++) { + gcsRcvrData.Channel[i] = 1500 + (out.rc_channel[i] * 500); // Elements in rc_channel are between -1 and 1 } gcsReceiver->setData(gcsRcvrData); - gcsRcvrTime=gcsRcvrTime.addMSecs(settings.minOutputPeriod); - + gcsRcvrTime = gcsRcvrTime.addMSecs(settings.minOutputPeriod); } } @@ -641,19 +631,19 @@ void Simulator::updateUAVOs(Output2Hardware out){ /*******************************/ if (settings.gpsPositionEnabled) { if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) { - qDebug()<< " GPS time:" << gpsPosTime << ", currentTime: " << currentTime << ", difference: " << gpsPosTime.msecsTo(currentTime); + qDebug() << " GPS time:" << gpsPosTime << ", currentTime: " << currentTime << ", difference: " << gpsPosTime.msecsTo(currentTime); // Update GPS Position objects GPSPosition::DataFields gpsPosData; memset(&gpsPosData, 0, sizeof(GPSPosition::DataFields)); - gpsPosData.Altitude = out.altitude + noise.gpsPosData.Altitude; - gpsPosData.Heading = out.heading + noise.gpsPosData.Heading; - gpsPosData.Groundspeed = out.groundspeed + noise.gpsPosData.Groundspeed; - gpsPosData.Latitude = out.latitude + noise.gpsPosData.Latitude; //Already in *10^7 integer format - gpsPosData.Longitude = out.longitude + noise.gpsPosData.Longitude; //Already in *10^7 integer format + gpsPosData.Altitude = out.altitude + noise.gpsPosData.Altitude; + gpsPosData.Heading = out.heading + noise.gpsPosData.Heading; + gpsPosData.Groundspeed = out.groundspeed + noise.gpsPosData.Groundspeed; + gpsPosData.Latitude = out.latitude + noise.gpsPosData.Latitude; // Already in *10^7 integer format + gpsPosData.Longitude = out.longitude + noise.gpsPosData.Longitude; // Already in *10^7 integer format gpsPosData.GeoidSeparation = 0.0; gpsPosData.PDOP = 3.0; - gpsPosData.VDOP = gpsPosData.PDOP*1.5; - gpsPosData.Satellites = 10; + gpsPosData.VDOP = gpsPosData.PDOP * 1.5; + gpsPosData.Satellites = 10; gpsPosData.Status = GPSPosition::STATUS_FIX3D; gpsPos->setData(gpsPosData); @@ -662,12 +652,12 @@ void Simulator::updateUAVOs(Output2Hardware out){ GPSVelocity::DataFields gpsVelData; memset(&gpsVelData, 0, sizeof(GPSVelocity::DataFields)); gpsVelData.North = out.velNorth + noise.gpsVelData.North; - gpsVelData.East = out.velEast + noise.gpsVelData.East; - gpsVelData.Down = out.velDown + noise.gpsVelData.Down; + gpsVelData.East = out.velEast + noise.gpsVelData.East; + gpsVelData.Down = out.velDown + noise.gpsVelData.Down; gpsVel->setData(gpsVelData); - gpsPosTime=gpsPosTime.addMSecs(settings.gpsPosRate); + gpsPosTime = gpsPosTime.addMSecs(settings.gpsPosRate); } } @@ -678,87 +668,87 @@ void Simulator::updateUAVOs(Output2Hardware out){ VelocityActual::DataFields velocityActualData; memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields)); velocityActualData.North = out.velNorth + noise.velocityActualData.North; - velocityActualData.East = out.velEast + noise.velocityActualData.East; - velocityActualData.Down = out.velDown + noise.velocityActualData.Down; + velocityActualData.East = out.velEast + noise.velocityActualData.East; + velocityActualData.Down = out.velDown + noise.velocityActualData.Down; velActual->setData(velocityActualData); // Update PositionActual.{Nort,East,Down} PositionActual::DataFields positionActualData; memset(&positionActualData, 0, sizeof(PositionActual::DataFields)); - positionActualData.North = (out.dstN-initN) + noise.positionActualData.North; - positionActualData.East = (out.dstE-initE) + noise.positionActualData.East; - positionActualData.Down = (out.dstD/*-initD*/) + noise.positionActualData.Down; + positionActualData.North = (out.dstN - initN) + noise.positionActualData.North; + positionActualData.East = (out.dstE - initE) + noise.positionActualData.East; + positionActualData.Down = (out.dstD /*-initD*/) + noise.positionActualData.Down; posActual->setData(positionActualData); - groundTruthTime=groundTruthTime.addMSecs(settings.groundTruthRate); + groundTruthTime = groundTruthTime.addMSecs(settings.groundTruthRate); } } -// /*******************************/ -// if (settings.sonarAltitude) { -// static QTime sonarAltTime = currentTime; -// if (sonarAltTime.msecsTo(currentTime) >= settings.sonarAltRate) { -// SonarAltitude::DataFields sonarAltData; -// sonarAltData = sonarAlt->getData(); +///*******************************/ +// if (settings.sonarAltitude) { +// static QTime sonarAltTime = currentTime; +// if (sonarAltTime.msecsTo(currentTime) >= settings.sonarAltRate) { +// SonarAltitude::DataFields sonarAltData; +// sonarAltData = sonarAlt->getData(); -// float sAlt = settings.sonarMaxAlt; -// // 0.35 rad ~= 20 degree -// if ((agl < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) { -// float x = agl * qTan(roll); -// float y = agl * qTan(pitch); -// float h = qSqrt(x*x + y*y + agl*agl); -// sAlt = qMin(h, sAlt); -// } +// float sAlt = settings.sonarMaxAlt; +//// 0.35 rad ~= 20 degree +// if ((agl < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) { +// float x = agl * qTan(roll); +// float y = agl * qTan(pitch); +// float h = qSqrt(x*x + y*y + agl*agl); +// sAlt = qMin(h, sAlt); +// } -// sonarAltData.Altitude = sAlt; -// sonarAlt->setData(sonarAltData); -// sonarAltTime = currentTime; -// } -// } +// sonarAltData.Altitude = sAlt; +// sonarAlt->setData(sonarAltData); +// sonarAltTime = currentTime; +// } +// } /*******************************/ // Update BaroAltitude object - if (settings.baroAltitudeEnabled){ + if (settings.baroAltitudeEnabled) { if (baroAltTime.msecsTo(currentTime) >= settings.baroAltRate) { - BaroAltitude::DataFields baroAltData; - memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields)); - baroAltData.Altitude = out.altitude + noise.baroAltData.Altitude; - baroAltData.Temperature = out.temperature + noise.baroAltData.Temperature; - baroAltData.Pressure = out.pressure + noise.baroAltData.Pressure; - baroAlt->setData(baroAltData); + BaroAltitude::DataFields baroAltData; + memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields)); + baroAltData.Altitude = out.altitude + noise.baroAltData.Altitude; + baroAltData.Temperature = out.temperature + noise.baroAltData.Temperature; + baroAltData.Pressure = out.pressure + noise.baroAltData.Pressure; + baroAlt->setData(baroAltData); - baroAltTime=baroAltTime.addMSecs(settings.baroAltRate); + baroAltTime = baroAltTime.addMSecs(settings.baroAltRate); } } /*******************************/ // Update FlightBatteryState object - if (settings.baroAltitudeEnabled){ + if (settings.baroAltitudeEnabled) { if (battTime.msecsTo(currentTime) >= settings.baroAltRate) { - FlightBatteryState::DataFields batteryData; - memset(&batteryData, 0, sizeof(FlightBatteryState::DataFields)); - batteryData.Voltage = out.voltage; - batteryData.Current = out.current; - batteryData.ConsumedEnergy = out.consumption; - flightBatt->setData(batteryData); + FlightBatteryState::DataFields batteryData; + memset(&batteryData, 0, sizeof(FlightBatteryState::DataFields)); + batteryData.Voltage = out.voltage; + batteryData.Current = out.current; + batteryData.ConsumedEnergy = out.consumption; + flightBatt->setData(batteryData); - battTime=battTime.addMSecs(settings.baroAltRate); + battTime = battTime.addMSecs(settings.baroAltRate); } } /*******************************/ // Update AirspeedActual object - if (settings.airspeedActualEnabled){ + if (settings.airspeedActualEnabled) { if (airspeedActualTime.msecsTo(currentTime) >= settings.airspeedActualRate) { - AirspeedActual::DataFields airspeedActualData; - memset(&airspeedActualData, 0, sizeof(AirspeedActual::DataFields)); - airspeedActualData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedActual.CalibratedAirspeed; - airspeedActualData.TrueAirspeed = out.trueAirspeed + noise.airspeedActual.TrueAirspeed; - //airspeedActualData.alpha=out.angleOfAttack; // to be implemented - //airspeedActualData.beta=out.angleOfSlip; - airspeedActual->setData(airspeedActualData); + AirspeedActual::DataFields airspeedActualData; + memset(&airspeedActualData, 0, sizeof(AirspeedActual::DataFields)); + airspeedActualData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedActual.CalibratedAirspeed; + airspeedActualData.TrueAirspeed = out.trueAirspeed + noise.airspeedActual.TrueAirspeed; + // airspeedActualData.alpha=out.angleOfAttack; // to be implemented + // airspeedActualData.beta=out.angleOfSlip; + airspeedActual->setData(airspeedActualData); - airspeedActualTime=airspeedActualTime.addMSecs(settings.airspeedActualRate); + airspeedActualTime = airspeedActualTime.addMSecs(settings.airspeedActualRate); } } @@ -766,7 +756,7 @@ void Simulator::updateUAVOs(Output2Hardware out){ // Update raw attitude sensors if (settings.attRawEnabled) { if (attRawTime.msecsTo(currentTime) >= settings.attRawRate) { - //Update gyroscope sensor data + // Update gyroscope sensor data Gyros::DataFields gyroData; memset(&gyroData, 0, sizeof(Gyros::DataFields)); gyroData.x = out.rollRate + noise.gyroData.x; @@ -774,7 +764,7 @@ void Simulator::updateUAVOs(Output2Hardware out){ gyroData.z = out.yawRate + noise.gyroData.z; gyros->setData(gyroData); - //Update accelerometer sensor data + // Update accelerometer sensor data Accels::DataFields accelData; memset(&accelData, 0, sizeof(Accels::DataFields)); accelData.x = out.accX + noise.accelData.x; @@ -782,7 +772,7 @@ void Simulator::updateUAVOs(Output2Hardware out){ accelData.z = out.accZ + noise.accelData.z; accels->setData(accelData); - attRawTime=attRawTime.addMSecs(settings.attRawRate); + attRawTime = attRawTime.addMSecs(settings.attRawRate); } } } @@ -790,9 +780,10 @@ void Simulator::updateUAVOs(Output2Hardware out){ /** * calculate air density from altitude. http://en.wikipedia.org/wiki/Density_of_air */ -float Simulator::airDensityFromAltitude(float alt, AirParameters air, float gravity) { - float p= airPressureFromAltitude(alt, air, gravity); - float rho=p*air.M/(air.univGasConstant*(air.groundTemp-air.tempLapseRate*alt)); +float Simulator::airDensityFromAltitude(float alt, AirParameters air, float gravity) +{ + float p = airPressureFromAltitude(alt, air, gravity); + float rho = p * air.M / (air.univGasConstant * (air.groundTemp - air.tempLapseRate * alt)); return rho; } @@ -804,8 +795,9 @@ float Simulator::airDensityFromAltitude(float alt, AirParameters air, float grav * @param gravity * @return */ -float Simulator::airPressureFromAltitude(float alt, AirParameters air, float gravity) { - return air.seaLevelPress* pow(1 - air.tempLapseRate * alt /air.groundTemp, gravity * air.M/(air.univGasConstant*air.tempLapseRate)); +float Simulator::airPressureFromAltitude(float alt, AirParameters air, float gravity) +{ + return air.seaLevelPress * pow(1 - air.tempLapseRate * alt / air.groundTemp, gravity * air.M / (air.univGasConstant * air.tempLapseRate)); } /** @@ -816,10 +808,11 @@ float Simulator::airPressureFromAltitude(float alt, AirParameters air, float gra * @param gravity * @return True airspeed */ -float Simulator::cas2tas(float CAS, float alt, AirParameters air, float gravity) { - float rho=airDensityFromAltitude(alt, air, gravity); +float Simulator::cas2tas(float CAS, float alt, AirParameters air, float gravity) +{ + float rho = airDensityFromAltitude(alt, air, gravity); - return (CAS * sqrt(air.groundDensity/rho)); + return CAS * sqrt(air.groundDensity / rho); } /** @@ -830,17 +823,19 @@ float Simulator::cas2tas(float CAS, float alt, AirParameters air, float gravity) * @param gravity * @return Calibrated airspeed */ -float Simulator::tas2cas(float TAS, float alt, AirParameters air, float gravity) { - float rho=airDensityFromAltitude(alt, air, gravity); +float Simulator::tas2cas(float TAS, float alt, AirParameters air, float gravity) +{ + float rho = airDensityFromAltitude(alt, air, gravity); - return (TAS / sqrt(air.groundDensity/rho)); + return TAS / sqrt(air.groundDensity / rho); } /** * @brief Simulator::getAirParameters get air parameters * @return airParameters */ -AirParameters Simulator::getAirParameters(){ +AirParameters Simulator::getAirParameters() +{ return airParameters; } @@ -848,6 +843,7 @@ AirParameters Simulator::getAirParameters(){ * @brief Simulator::setAirParameters set air parameters * @param airParameters */ -void Simulator::setAirParameters(AirParameters airParameters){ - this->airParameters=airParameters; +void Simulator::setAirParameters(AirParameters airParameters) +{ + this->airParameters = airParameters; } diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.h b/ground/openpilotgcs/src/plugins/hitl/simulator.h index c4795142c..e9136a031 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.h @@ -65,15 +65,14 @@ * just imagine this was a class without methods and all public properties */ typedef struct _FLIGHT_PARAM { - // time float T; float dT; unsigned int i; // speeds - float cas; //Calibrated airspeed - float tas; //True airspeed + float cas; // Calibrated airspeed + float tas; // True airspeed float groundspeed; // position (absolute) @@ -91,104 +90,100 @@ typedef struct _FLIGHT_PARAM { float ddY; float ddZ; - //angle + // angle float azimuth; float pitch; float roll; - //rotation speed + // rotation speed float dAzimuth; float dPitch; float dRoll; - } FLIGHT_PARAM; -struct AirParameters -{ +struct AirParameters { float groundDensity; float groundTemp; float seaLevelPress; float tempLapseRate; float univGasConstant; float dryAirConstant; - float relativeHumidity; //[%] - float M; //Molar mass + float relativeHumidity; // [%] + float M; // Molar mass }; -typedef struct _CONNECTION -{ +typedef struct _CONNECTION { QString simulatorId; QString binPath; QString dataPath; QString hostAddress; QString remoteAddress; - int outPort; - int inPort; - bool startSim; - bool addNoise; + int outPort; + int inPort; + bool startSim; + bool addNoise; QString latitude; QString longitude; -// bool homeLocation; +// bool homeLocation; - bool attRawEnabled; - quint8 attRawRate; + bool attRawEnabled; + quint8 attRawRate; - bool attActualEnabled; - bool attActHW; - bool attActSim; - bool attActCalc; + bool attActualEnabled; + bool attActHW; + bool attActSim; + bool attActCalc; - bool baroAltitudeEnabled; + bool baroAltitudeEnabled; quint16 baroAltRate; - bool groundTruthEnabled; + bool groundTruthEnabled; quint16 groundTruthRate; - bool gpsPositionEnabled; + bool gpsPositionEnabled; quint16 gpsPosRate; - bool inputCommand; - bool gcsReceiverEnabled; - bool manualControlEnabled; + bool inputCommand; + bool gcsReceiverEnabled; + bool manualControlEnabled; quint16 minOutputPeriod; - bool airspeedActualEnabled; + bool airspeedActualEnabled; quint16 airspeedActualRate; - } SimulatorSettings; -struct Output2Hardware{ +struct Output2Hardware { float latitude; float longitude; float altitude; - float agl; //[m] + float agl; // [m] float heading; - float groundspeed; //[m/s] - float calibratedAirspeed; //[m/s] - float trueAirspeed; //[m/s] + float groundspeed; // [m/s] + float calibratedAirspeed; // [m/s] + float trueAirspeed; // [m/s] float angleOfAttack; float angleOfSlip; float roll; float pitch; float pressure; float temperature; - float velNorth; //[m/s] - float velEast; //[m/s] - float velDown; //[m/s] - float dstN; //[m] - float dstE; //[m] - float dstD; //[m] - float accX; //[m/s^2] - float accY; //[m/s^2] - float accZ; //[m/s^2] - float rollRate; //[deg/s] - float pitchRate; //[deg/s] - float yawRate; //[deg/s] - float delT; //[s] + float velNorth; // [m/s] + float velEast; // [m/s] + float velDown; // [m/s] + float dstN; // [m] + float dstE; // [m] + float dstD; // [m] + float accX; // [m/s^2] + float accY; // [m/s^2] + float accZ; // [m/s^2] + float rollRate; // [deg/s] + float pitchRate; // [deg/s] + float yawRate; // [deg/s] + float delT; // [s] - float rc_channel[GCSReceiver::CHANNEL_NUMELEM]; //Elements in rc_channel are between -1 and 1 + float rc_channel[GCSReceiver::CHANNEL_NUMELEM]; // Elements in rc_channel are between -1 and 1 float rollDesired; @@ -201,33 +196,50 @@ struct Output2Hardware{ float consumption; }; -//struct Output2Simulator{ -// float roll; -// float pitch; -// float yaw; -// float throttle; +// struct Output2Simulator{ +// float roll; +// float pitch; +// float yaw; +// float throttle; -// float ailerons; -// float rudder; -// float elevator; -// float motor; -//}; +// float ailerons; +// float rudder; +// float elevator; +// float motor; +// }; -class Simulator : public QObject -{ +class Simulator : public QObject { Q_OBJECT public: - Simulator(const SimulatorSettings& params); + Simulator(const SimulatorSettings & params); virtual ~Simulator(); - bool isAutopilotConnected() const { return autopilotConnectionStatus; } - bool isSimulatorConnected() const { return simConnectionStatus; } - QString Name() const { return name; } - void setName(QString str) { name = str; } + bool isAutopilotConnected() const + { + return autopilotConnectionStatus; + } + bool isSimulatorConnected() const + { + return simConnectionStatus; + } + QString Name() const + { + return name; + } + void setName(QString str) + { + name = str; + } - QString SimulatorId() const { return simulatorId; } - void setSimulatorId(QString str) { simulatorId = str; } + QString SimulatorId() const + { + return simulatorId; + } + void setSimulatorId(QString str) + { + simulatorId = str; + } float airDensityFromAltitude(float alt, AirParameters air, float gravity); float airPressureFromAltitude(float alt, AirParameters air, float gravity); @@ -235,13 +247,28 @@ public: float tas2cas(float TAS, float alt, AirParameters air, float gravity); - static bool IsStarted() { return isStarted; } - static void setStarted(bool val) { isStarted = val; } - static QStringList& Instances() { return Simulator::instances; } - static void setInstance(const QString& str) { Simulator::instances.append(str); } + static bool IsStarted() + { + return isStarted; + } + static void setStarted(bool val) + { + isStarted = val; + } + static QStringList & Instances() + { + return Simulator::instances; + } + static void setInstance(const QString & str) + { + Simulator::instances.append(str); + } virtual void stopProcess() {} - virtual void setupUdpPorts(const QString& host, int inPort, int outPort) { Q_UNUSED(host) Q_UNUSED(inPort) Q_UNUSED(outPort)} + virtual void setupUdpPorts(const QString & host, int inPort, int outPort) + { + Q_UNUSED(host) Q_UNUSED(inPort) Q_UNUSED(outPort) + } void resetInitialHomePosition(); void updateUAVOs(Output2Hardware out); @@ -258,19 +285,22 @@ signals: void deleteSimProcess(); void myStart(); public slots: - Q_INVOKABLE virtual bool setupProcess() { return true;} + Q_INVOKABLE virtual bool setupProcess() + { + return true; + } private slots: void onStart(); - //void transmitUpdate(); + // void transmitUpdate(); void receiveUpdate(); void onAutopilotConnect(); void onAutopilotDisconnect(); void onSimulatorConnectionTimeout(); - void telStatsUpdated(UAVObject* obj); + void telStatsUpdated(UAVObject *obj); Q_INVOKABLE void onDeleteSimulator(void); virtual void transmitUpdate() = 0; - virtual void processUpdate(const QByteArray& data) = 0; + virtual void processUpdate(const QByteArray & data) = 0; protected: static const float GEE; @@ -281,30 +311,30 @@ protected: static const float DEG2RAD; static const float RAD2DEG; - QProcess* simProcess; - QTime* time; - QUdpSocket* inSocket;//(new QUdpSocket()); - QUdpSocket* outSocket; + QProcess *simProcess; + QTime *time; + QUdpSocket *inSocket; // (new QUdpSocket()); + QUdpSocket *outSocket; - ActuatorCommand* actCommand; - ActuatorDesired* actDesired; - ManualControlCommand* manCtrlCommand; - FlightStatus* flightStatus; - FlightBatteryState* flightBatt; - BaroAltitude* baroAlt; - AirspeedActual* airspeedActual; - AttitudeActual* attActual; - AttitudeSettings* attSettings; - VelocityActual* velActual; - GPSPosition* gpsPos; - GPSVelocity* gpsVel; - PositionActual* posActual; - HomeLocation* posHome; - Accels* accels; - Gyros* gyros; - GCSTelemetryStats* telStats; - GCSReceiver* gcsReceiver; - GroundTruth* groundTruth; + ActuatorCommand *actCommand; + ActuatorDesired *actDesired; + ManualControlCommand *manCtrlCommand; + FlightStatus *flightStatus; + FlightBatteryState *flightBatt; + BaroAltitude *baroAlt; + AirspeedActual *airspeedActual; + AttitudeActual *attActual; + AttitudeSettings *attSettings; + VelocityActual *velActual; + GPSPosition *gpsPos; + GPSVelocity *gpsVel; + PositionActual *posActual; + HomeLocation *posHome; + Accels *accels; + Gyros *gyros; + GCSTelemetryStats *telStats; + GCSReceiver *gcsReceiver; + GroundTruth *groundTruth; SimulatorSettings settings; @@ -322,8 +352,8 @@ private: int simTimeout; volatile bool autopilotConnectionStatus; volatile bool simConnectionStatus; - QTimer* txTimer; - QTimer* simTimer; + QTimer *txTimer; + QTimer *simTimer; QTime attRawTime; QTime gpsPosTime; @@ -337,9 +367,9 @@ private: QString simulatorId; volatile static bool isStarted; static QStringList instances; - //QList > requiredUAVObjects; - void setupOutputObject(UAVObject* obj, quint32 updatePeriod); - void setupInputObject(UAVObject* obj, quint32 updatePeriod); + // QList > requiredUAVObjects; + void setupOutputObject(UAVObject *obj, quint32 updatePeriod); + void setupInputObject(UAVObject *obj, quint32 updatePeriod); void setupWatchedObject(UAVObject *obj, quint32 updatePeriod); void setupObjects(); @@ -347,9 +377,7 @@ private: }; - -class SimulatorCreator -{ +class SimulatorCreator { public: SimulatorCreator(QString id, QString descr) : classId(id), @@ -357,10 +385,16 @@ public: {} virtual ~SimulatorCreator() {} - QString ClassId() const {return classId;} - QString Description() const {return description;} + QString ClassId() const + { + return classId; + } + QString Description() const + { + return description; + } - virtual Simulator* createSimulator(const SimulatorSettings& params) = 0; + virtual Simulator *createSimulator(const SimulatorSettings & params) = 0; private: QString classId; diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp index e79f8f45f..f26dc3229 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp @@ -63,9 +63,9 @@ #include #include -void TraceBuf(const char* buf,int len); +void TraceBuf(const char *buf, int len); -XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) : +XplaneSimulator::XplaneSimulator(const SimulatorSettings & params) : Simulator(params) { resetInitialHomePosition(); @@ -73,25 +73,23 @@ XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) : XplaneSimulator::~XplaneSimulator() -{ -} +{} -void XplaneSimulator::setupUdpPorts(const QString& host, int inPort, int outPort) +void XplaneSimulator::setupUdpPorts(const QString & host, int inPort, int outPort) { Q_UNUSED(outPort); inSocket->bind(QHostAddress(host), inPort); - //outSocket->bind(QHostAddress(host), outPort); + // outSocket->bind(QHostAddress(host), outPort); resetInitialHomePosition(); - } bool XplaneSimulator::setupProcess() { emit processOutput(QString("Please start X-Plane manually, and make sure it is setup to output its ") + "data to host " + settings.hostAddress + " UDP port " + QString::number(settings.inPort)); - return true; + return true; } /** @@ -100,198 +98,190 @@ bool XplaneSimulator::setupProcess() void XplaneSimulator::transmitUpdate() { if (settings.manualControlEnabled) { - //Read ActuatorDesired from autopilot + // Read ActuatorDesired from autopilot ActuatorDesired::DataFields actData = actDesired->getData(); float ailerons = actData.Roll; float elevator = actData.Pitch; - float rudder = actData.Yaw; - float throttle = actData.Throttle > 0? actData.Throttle : 0; - float none = -999; - //quint32 none = *((quint32*)&tmp); // get float as 4 bytes - + float rudder = actData.Yaw; + float throttle = actData.Throttle > 0 ? actData.Throttle : 0; + float none = -999; + // quint32 none = *((quint32*)&tmp); // get float as 4 bytes + quint32 code; QByteArray buf; - QDataStream stream(&buf,QIODevice::ReadWrite); - + QDataStream stream(&buf, QIODevice::ReadWrite); + // !!! LAN byte order - Big Endian #if Q_BYTE_ORDER == Q_LITTLE_ENDIAN stream.setByteOrder(QDataStream::LittleEndian); #endif - + // 11th data settings (flight con: ail/elv/rud) buf.clear(); code = 11; - //quint8 header[] = "DATA"; + // quint8 header[] = "DATA"; /* - stream << *((quint32*)header) << + stream << *((quint32*)header) << (quint8)0x30 << code << - *((quint32*)&elevator) << - *((quint32*)&ailerons) << - *((quint32*)&rudder) << + *((quint32*)&elevator) << + *((quint32*)&ailerons) << + *((quint32*)&rudder) << none << - *((quint32*)&ailerons) << + *((quint32*)&ailerons) << none << none << none; - */ + */ buf.append("DATA0"); - buf.append(reinterpret_cast(&code), sizeof(code)); - buf.append(reinterpret_cast(&elevator), sizeof(elevator)); - buf.append(reinterpret_cast(&ailerons), sizeof(ailerons)); - buf.append(reinterpret_cast(&rudder), sizeof(rudder)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&rudder), sizeof(rudder)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); -// TraceBuf(buf.data(),41); - - if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) - { + buf.append(reinterpret_cast(&code), sizeof(code)); + buf.append(reinterpret_cast(&elevator), sizeof(elevator)); + buf.append(reinterpret_cast(&ailerons), sizeof(ailerons)); + buf.append(reinterpret_cast(&rudder), sizeof(rudder)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&rudder), sizeof(rudder)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); +// TraceBuf(buf.data(),41); + + if (outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); } - //outSocket->write(buf); - + // outSocket->write(buf); + // 25th data settings (throttle command) buf.clear(); code = 25; - //stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none - // << none << none << none << none << none; + // stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none + // << none << none << none << none << none; buf.append("DATA0"); - buf.append(reinterpret_cast(&code), sizeof(code)); - buf.append(reinterpret_cast(&throttle), sizeof(throttle)); - buf.append(reinterpret_cast(&throttle), sizeof(throttle)); - buf.append(reinterpret_cast(&throttle), sizeof(throttle)); - buf.append(reinterpret_cast(&throttle), sizeof(throttle)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - - if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) - { + buf.append(reinterpret_cast(&code), sizeof(code)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + + if (outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); } - - //outSocket->write(buf); - - - - /** !!! this settings was given from ardupilot X-Plane.pl, I comment them - but if it needed comment should be removed !!! - - // 8th data settings (joystick 1 ail/elv/rud) - stream << "DATA0" << quint32(11) << elevator << ailerons << rudder - << float(-999) << float(-999) << float(-999) << float(-999) << float(-999); - outSocket->write(buf); - */ - } + // outSocket->write(buf); + + + /** !!! this settings was given from ardupilot X-Plane.pl, I comment them + but if it needed comment should be removed !!! + + // 8th data settings (joystick 1 ail/elv/rud) + stream << "DATA0" << quint32(11) << elevator << ailerons << rudder + << float(-999) << float(-999) << float(-999) << float(-999) << float(-999); + outSocket->write(buf); + */ + } } /** * process data string from X-Plane simulator */ -void XplaneSimulator::processUpdate(const QByteArray& dataBuf) +void XplaneSimulator::processUpdate(const QByteArray & dataBuf) { - float altitude_msl = 0; - float altitude_agl = 0; - float latitude = 0; - float longitude = 0; - float airspeed_keas = 0; + float altitude_msl = 0; + float altitude_agl = 0; + float latitude = 0; + float longitude = 0; + float airspeed_keas = 0; float groundspeed_ktgs = 0; - float pitch = 0; - float roll = 0; - float heading = 0; - float pressure = 0; - float temperature = 0; - float velX = 0; - float velY = 0; - float velZ = 0; - float dstX = 0; - float dstY = 0; - float dstZ = 0; - float accX = 0; - float accY = 0; - float accZ = 0; - float rollRate_rad=0; - float pitchRate_rad=0; - float yawRate_rad=0; + float pitch = 0; + float roll = 0; + float heading = 0; + float pressure = 0; + float temperature = 0; + float velX = 0; + float velY = 0; + float velZ = 0; + float dstX = 0; + float dstY = 0; + float dstZ = 0; + float accX = 0; + float accY = 0; + float accZ = 0; + float rollRate_rad = 0; + float pitchRate_rad = 0; + float yawRate_rad = 0; - // QString str; - QByteArray& buf = const_cast(dataBuf); + // QString str; + QByteArray & buf = const_cast(dataBuf); QString data(buf); - if(data.left(4) == "DATA") // check type of packet - { - buf.remove(0,5); - if(dataBuf.size() % 36) - { - qxtLog->info("incorrect length of UDP packet: ",buf); + if (data.left(4) == "DATA") { // check type of packet + buf.remove(0, 5); + if (dataBuf.size() % 36) { + qxtLog->info("incorrect length of UDP packet: ", buf); return; // incorrect length of struct } // check correctness of data length, length must be multiple of (id_size+8*float_size)=4+8*4=36 int channelCounter = dataBuf.size() / 36; - do - { - switch(buf[0]) // switch by id - { + do { + switch (buf[0]) { // switch by id case XplaneSimulator::LatitudeLongitudeAltitude: - latitude = *((float*)(buf.data()+4*1)); - longitude = *((float*)(buf.data()+4*2)); - altitude_msl = *((float*)(buf.data()+4*3))* FT2M; - altitude_agl = *((float*)(buf.data()+4*4))* FT2M; + latitude = *((float *)(buf.data() + 4 * 1)); + longitude = *((float *)(buf.data() + 4 * 2)); + altitude_msl = *((float *)(buf.data() + 4 * 3)) * FT2M; + altitude_agl = *((float *)(buf.data() + 4 * 4)) * FT2M; break; case XplaneSimulator::Speed: - airspeed_keas = *((float*)(buf.data()+4*2)); - groundspeed_ktgs = *((float*)(buf.data()+4*4)); + airspeed_keas = *((float *)(buf.data() + 4 * 2)); + groundspeed_ktgs = *((float *)(buf.data() + 4 * 4)); break; case XplaneSimulator::PitchRollHeading: - pitch = *((float*)(buf.data()+4*1)); - roll = *((float*)(buf.data()+4*2)); - heading = *((float*)(buf.data()+4*3)); + pitch = *((float *)(buf.data() + 4 * 1)); + roll = *((float *)(buf.data() + 4 * 2)); + heading = *((float *)(buf.data() + 4 * 3)); break; - /* - case XplaneSimulator::SystemPressures: - pressure = *((float*)(buf.data()+4*1)); - break; - */ + /* + case XplaneSimulator::SystemPressures: + pressure = *((float*)(buf.data()+4*1)); + break; + */ case XplaneSimulator::AtmosphereWeather: - pressure = *((float*)(buf.data()+4*1)) * INHG2KPA; - temperature = *((float*)(buf.data()+4*2)); + pressure = *((float *)(buf.data() + 4 * 1)) * INHG2KPA; + temperature = *((float *)(buf.data() + 4 * 2)); break; case XplaneSimulator::LocVelDistTraveled: - dstX = *((float*)(buf.data()+4*1)); - dstY = - *((float*)(buf.data()+4*3)); - dstZ = *((float*)(buf.data()+4*2)); - velX = *((float*)(buf.data()+4*4)); - velY = - *((float*)(buf.data()+4*6)); - velZ = *((float*)(buf.data()+4*5)); + dstX = *((float *)(buf.data() + 4 * 1)); + dstY = -*((float *)(buf.data() + 4 * 3)); + dstZ = *((float *)(buf.data() + 4 * 2)); + velX = *((float *)(buf.data() + 4 * 4)); + velY = -*((float *)(buf.data() + 4 * 6)); + velZ = *((float *)(buf.data() + 4 * 5)); break; - case XplaneSimulator::AngularVelocities: //In [rad/s] - pitchRate_rad = *((float*)(buf.data()+4*1)); - rollRate_rad = *((float*)(buf.data()+4*2)); - yawRate_rad = *((float*)(buf.data()+4*3)); - break; + case XplaneSimulator::AngularVelocities: // In [rad/s] + pitchRate_rad = *((float *)(buf.data() + 4 * 1)); + rollRate_rad = *((float *)(buf.data() + 4 * 2)); + yawRate_rad = *((float *)(buf.data() + 4 * 3)); + break; case XplaneSimulator::Gload: - accX = *((float*)(buf.data()+4*6)) * GEE; - accY = *((float*)(buf.data()+4*7)) * GEE; - accZ = *((float*)(buf.data()+4*5)) * GEE; + accX = *((float *)(buf.data() + 4 * 6)) * GEE; + accY = *((float *)(buf.data() + 4 * 7)) * GEE; + accZ = *((float *)(buf.data() + 4 * 5)) * GEE; break; default: break; } channelCounter--; - buf.remove(0,36); + buf.remove(0, 36); } while (channelCounter); @@ -302,73 +292,70 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf) memset(&out, 0, sizeof(Output2Hardware)); // Update GPS Position objects - out.latitude = latitude * 1e7; - out.longitude = longitude * 1e7; - out.altitude = altitude_msl; + out.latitude = latitude * 1e7; + out.longitude = longitude * 1e7; + out.altitude = altitude_msl; out.agl = altitude_agl; - out.groundspeed = groundspeed_ktgs*1.15*1.6089/3.6; //Convert from [kts] to [m/s] + out.groundspeed = groundspeed_ktgs * 1.15 * 1.6089 / 3.6; // Convert from [kts] to [m/s] - out.calibratedAirspeed = airspeed_keas*1.15*1.6089/3.6; //Convert from [kts] to [m/s] + out.calibratedAirspeed = airspeed_keas * 1.15 * 1.6089 / 3.6; // Convert from [kts] to [m/s] // Update BaroAltitude object out.temperature = temperature; - out.pressure = pressure; + out.pressure = pressure; // Update attActual object - out.roll = roll; //roll; - out.pitch = pitch; // pitch - out.heading = heading; // yaw + out.roll = roll; // roll; + out.pitch = pitch; // pitch + out.heading = heading; // yaw - out.dstN=dstY; - out.dstE=dstX; - out.dstD=-dstZ; + out.dstN = dstY; + out.dstE = dstX; + out.dstD = -dstZ; // Update VelocityActual.{North,East,Down} - out.velNorth = velY; - out.velEast = velX; - out.velDown = -velZ; + out.velNorth = velY; + out.velEast = velX; + out.velDown = -velZ; - //Update gyroscope sensor data - out.rollRate = rollRate_rad; + // Update gyroscope sensor data + out.rollRate = rollRate_rad; out.pitchRate = pitchRate_rad; - out.yawRate = yawRate_rad; + out.yawRate = yawRate_rad; - //Update accelerometer sensor data - out.accX = accX; - out.accY = accY; - out.accZ = -accZ; + // Update accelerometer sensor data + out.accX = accX; + out.accY = accY; + out.accZ = -accZ; updateUAVOs(out); } // issue manual update - //attActual->updated(); - //altActual->updated(); - //posActual->updated(); - + // attActual->updated(); + // altActual->updated(); + // posActual->updated(); } -void TraceBuf(const char* buf,int len) +void TraceBuf(const char *buf, int len) { - QString str; - bool reminder=true; - for(int i=0; i < len; i++) - { - if(!(i%16)) - { - if(i>0) - { - qDebug() << str; - str.clear(); - reminder=false; - } - reminder=true; - } - str+=QString(" 0x%1").arg((quint8)buf[i],2,16,QLatin1Char('0')); - } + QString str; + bool reminder = true; - if(reminder){ + for (int i = 0; i < len; i++) { + if (!(i % 16)) { + if (i > 0) { + qDebug() << str; + str.clear(); + reminder = false; + } + reminder = true; + } + str += QString(" 0x%1").arg((quint8)buf[i], 2, 16, QLatin1Char('0')); + } + + if (reminder) { qDebug() << str; } } diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h index 0aaa42bd2..b1c33f36f 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h @@ -31,61 +31,58 @@ #include #include -class XplaneSimulator: public Simulator -{ - Q_OBJECT +class XplaneSimulator : public Simulator { + Q_OBJECT public: - XplaneSimulator(const SimulatorSettings& params); - ~XplaneSimulator(); - bool setupProcess(); + XplaneSimulator(const SimulatorSettings & params); + ~XplaneSimulator(); + bool setupProcess(); - void setupUdpPorts(const QString& host, int inPort, int outPort); + void setupUdpPorts(const QString & host, int inPort, int outPort); private slots: - void transmitUpdate(); + void transmitUpdate(); private: - enum XplaneOutputData //***WARNING***: Elements in this enum are in a precise order, do - { // not change. Cf. http://www.nuclearprojects.com/xplane/info.shtml - FramRate, - Times, - SimStats, - Speed, - Gload, - AtmosphereWeather, - AtmosphereAircraft, - SystemPressures, - Joystick1, - Joystick2, - ArtStab, - FlightCon, - WingSweep, - Trim, - Brakes, - AngularMoments, - AngularAccelerations, + enum XplaneOutputData // ***WARNING***: Elements in this enum are in a precise order, do + { // not change. Cf. http://www.nuclearprojects.com/xplane/info.shtml + FramRate, + Times, + SimStats, + Speed, + Gload, + AtmosphereWeather, + AtmosphereAircraft, + SystemPressures, + Joystick1, + Joystick2, + ArtStab, + FlightCon, + WingSweep, + Trim, + Brakes, + AngularMoments, + AngularAccelerations, AngularVelocities, PitchRollHeading, - AoA, + AoA, LatitudeLongitudeAltitude, - LocVelDistTraveled - }; - - void processUpdate(const QByteArray& data); + LocVelDistTraveled + }; + void processUpdate(const QByteArray & data); }; -class XplaneSimulatorCreator : public SimulatorCreator -{ +class XplaneSimulatorCreator : public SimulatorCreator { public: - XplaneSimulatorCreator(const QString& classId, const QString& description) - : SimulatorCreator (classId,description) - {} + XplaneSimulatorCreator(const QString & classId, const QString & description) + : SimulatorCreator(classId, description) + {} - Simulator* createSimulator(const SimulatorSettings& params) - { - return new XplaneSimulator(params); - } + Simulator *createSimulator(const SimulatorSettings & params) + { + return new XplaneSimulator(params); + } }; #endif // XPLANESIMULATOR_H diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportdialog.cpp b/ground/openpilotgcs/src/plugins/importexport/importexportdialog.cpp index df38653f8..952ea6e35 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportdialog.cpp +++ b/ground/openpilotgcs/src/plugins/importexport/importexportdialog.cpp @@ -8,7 +8,7 @@ ImportExportDialog::ImportExportDialog(QWidget *parent) : ui->setupUi(this); setWindowTitle(tr("Import Export Settings")); - connect( ui->widget, SIGNAL(done()), this, SLOT(close())); + connect(ui->widget, SIGNAL(done()), this, SLOT(close())); } ImportExportDialog::~ImportExportDialog() @@ -19,6 +19,7 @@ ImportExportDialog::~ImportExportDialog() void ImportExportDialog::changeEvent(QEvent *e) { QDialog::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: ui->retranslateUi(this); diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportdialog.h b/ground/openpilotgcs/src/plugins/importexport/importexportdialog.h index 008d588d1..b897673ca 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportdialog.h +++ b/ground/openpilotgcs/src/plugins/importexport/importexportdialog.h @@ -29,11 +29,10 @@ #include namespace Ui { - class ImportExportDialog; +class ImportExportDialog; } -class ImportExportDialog : public QDialog -{ +class ImportExportDialog : public QDialog { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportgadget.cpp b/ground/openpilotgcs/src/plugins/importexport/importexportgadget.cpp index 41a9b1616..e3080d8b8 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportgadget.cpp +++ b/ground/openpilotgcs/src/plugins/importexport/importexportgadget.cpp @@ -32,10 +32,9 @@ #include "importexportgadgetconfiguration.h" ImportExportGadget::ImportExportGadget(QString classId, ImportExportGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} ImportExportGadget::~ImportExportGadget() { @@ -43,14 +42,14 @@ ImportExportGadget::~ImportExportGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void ImportExportGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void ImportExportGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - m_widget->loadConfiguration(qobject_cast(config)); + m_widget->loadConfiguration(qobject_cast(config)); } /** * @} diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportgadget.h b/ground/openpilotgcs/src/plugins/importexport/importexportgadget.h index 13d7a7db6..f01084994 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportgadget.h +++ b/ground/openpilotgcs/src/plugins/importexport/importexportgadget.h @@ -37,17 +37,17 @@ class ImportExportGadgetWidget; using namespace Core; -class IMPORTEXPORT_EXPORT ImportExportGadget : public Core::IUAVGadget -{ +class IMPORTEXPORT_EXPORT ImportExportGadget : public Core::IUAVGadget { Q_OBJECT public: ImportExportGadget(QString classId, ImportExportGadgetWidget *widget, QWidget *parent = 0); ~ImportExportGadget(); - QWidget *widget() { + QWidget *widget() + { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); private: ImportExportGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.cpp index 35d4696a0..f7a2b3179 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.cpp @@ -43,13 +43,13 @@ #include ImportExportGadgetWidget::ImportExportGadgetWidget(QWidget *parent) : - QWidget(parent), - ui(new Ui::ImportExportGadgetWidget) + QWidget(parent), + ui(new Ui::ImportExportGadgetWidget) { setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); ui->setupUi(this); - filename = ""; + filename = ""; } ImportExportGadgetWidget::~ImportExportGadgetWidget() @@ -60,6 +60,7 @@ ImportExportGadgetWidget::~ImportExportGadgetWidget() void ImportExportGadgetWidget::changeEvent(QEvent *e) { QWidget::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: ui->retranslateUi(this); @@ -71,26 +72,28 @@ void ImportExportGadgetWidget::changeEvent(QEvent *e) void ImportExportGadgetWidget::on_exportButton_clicked() { - QString file = filename; - QString filter = tr("GCS Settings file (*.xml)"); - file = QFileDialog::getSaveFileName(this, tr("Save GCS Settings too file .."), QFileInfo(file).absoluteFilePath(), filter).trimmed(); - if (file.isEmpty()) { - return; - } + QString file = filename; + QString filter = tr("GCS Settings file (*.xml)"); - // Add a "XML" extension to the file in case it does not exist: - if (!file.toLower().endsWith(".xml")) + file = QFileDialog::getSaveFileName(this, tr("Save GCS Settings too file .."), QFileInfo(file).absoluteFilePath(), filter).trimmed(); + if (file.isEmpty()) { + return; + } + + // Add a "XML" extension to the file in case it does not exist: + if (!file.toLower().endsWith(".xml")) { file.append(".xml"); + } - filename = file; + filename = file; qDebug() << "Export pressed! Write to file " << QFileInfo(file).absoluteFilePath(); QMessageBox msgBox; QDir dir = QFileInfo(file).absoluteDir(); - if (! dir.exists()) { + if (!dir.exists()) { msgBox.setText(tr("Can't write file ") + QFileInfo(file).absoluteFilePath() - + " since directory "+ dir.absolutePath() + " doesn't exist!"); + + " since directory " + dir.absolutePath() + " doesn't exist!"); msgBox.exec(); return; } @@ -99,28 +102,27 @@ void ImportExportGadgetWidget::on_exportButton_clicked() msgBox.setText(tr("The settings have been exported to ") + QFileInfo(file).absoluteFilePath()); msgBox.exec(); emit done(); - } -QList ImportExportGadgetWidget::getConfigurables() +QList ImportExportGadgetWidget::getConfigurables() { - QList configurables; + QList configurables; QList specs = ExtensionSystem::PluginManager::instance()->plugins(); - foreach ( ExtensionSystem::PluginSpec* spec, specs ){ - if ( Core::IConfigurablePlugin* plugin = dynamic_cast(spec->plugin()) ){ - qDebug()<< "Configurable: " << plugin->metaObject()->className(); + foreach(ExtensionSystem::PluginSpec * spec, specs) { + if (Core::IConfigurablePlugin * plugin = dynamic_cast(spec->plugin())) { + qDebug() << "Configurable: " << plugin->metaObject()->className(); configurables.append(plugin); } } return configurables; } -void ImportExportGadgetWidget::exportConfiguration(const QString& fileName) +void ImportExportGadgetWidget::exportConfiguration(const QString & fileName) { - bool doGeneral = ui->checkBoxGeneral->isChecked(); + bool doGeneral = ui->checkBoxGeneral->isChecked(); bool doAllGadgets = ui->checkBoxAllGadgets->isChecked(); - bool doPlugins = ui->checkBoxPlugins->isChecked(); + bool doPlugins = ui->checkBoxPlugins->isChecked(); QSettings::Format format = XmlConfig::XmlSettingsFormat; QSettings qs(fileName, format); @@ -131,9 +133,9 @@ void ImportExportGadgetWidget::exportConfiguration(const QString& fileName) if (doAllGadgets) { Core::ICore::instance()->uavGadgetInstanceManager()->saveSettings(&qs); } - if ( doPlugins ){ - foreach ( Core::IConfigurablePlugin *plugin, getConfigurables()){ - Core::ICore::instance()->saveSettings(plugin,&qs); + if (doPlugins) { + foreach(Core::IConfigurablePlugin * plugin, getConfigurables()) { + Core::ICore::instance()->saveSettings(plugin, &qs); } } @@ -141,26 +143,27 @@ void ImportExportGadgetWidget::exportConfiguration(const QString& fileName) } -void ImportExportGadgetWidget::writeError(const QString& msg) const +void ImportExportGadgetWidget::writeError(const QString & msg) const { qWarning() << "ERROR: " << msg; } void ImportExportGadgetWidget::on_importButton_clicked() { - QString file = filename; - QString filter = tr("GCS Settings file (*.xml)"); - file = QFileDialog::getOpenFileName(this, tr("Load GCS Settings from file .."), QFileInfo(file).absoluteFilePath(), filter).trimmed(); - if (file.isEmpty()) { - return; - } + QString file = filename; + QString filter = tr("GCS Settings file (*.xml)"); - filename = file; + file = QFileDialog::getOpenFileName(this, tr("Load GCS Settings from file .."), QFileInfo(file).absoluteFilePath(), filter).trimmed(); + if (file.isEmpty()) { + return; + } + + filename = file; qDebug() << "Import pressed! Read from file " << QFileInfo(file).absoluteFilePath(); QMessageBox msgBox; - if (! QFileInfo(file).isReadable()) { + if (!QFileInfo(file).isReadable()) { msgBox.setText(tr("Can't read file ") + QFileInfo(file).absoluteFilePath()); msgBox.exec(); return; @@ -174,23 +177,23 @@ void ImportExportGadgetWidget::on_importButton_clicked() emit done(); } -void ImportExportGadgetWidget::importConfiguration(const QString& fileName) +void ImportExportGadgetWidget::importConfiguration(const QString & fileName) { - bool doGeneral = ui->checkBoxGeneral->isChecked(); + bool doGeneral = ui->checkBoxGeneral->isChecked(); bool doAllGadgets = ui->checkBoxAllGadgets->isChecked(); - bool doPlugins = ui->checkBoxPlugins->isChecked(); + bool doPlugins = ui->checkBoxPlugins->isChecked(); QSettings qs(fileName, XmlConfig::XmlSettingsFormat); - if ( doAllGadgets ) { + if (doAllGadgets) { Core::ICore::instance()->uavGadgetInstanceManager()->readSettings(&qs); } - if ( doGeneral ) { + if (doGeneral) { Core::ICore::instance()->readMainSettings(&qs); } - if ( doPlugins ){ - foreach ( Core::IConfigurablePlugin *plugin, getConfigurables()){ - Core::ICore::instance()->readSettings(plugin,&qs); + if (doPlugins) { + foreach(Core::IConfigurablePlugin * plugin, getConfigurables()) { + Core::ICore::instance()->readSettings(plugin, &qs); } } @@ -206,15 +209,15 @@ void ImportExportGadgetWidget::on_helpButton_clicked() void ImportExportGadgetWidget::on_resetButton_clicked() { QMessageBox msgBox; + msgBox.setText(tr("All your settings will be deleted!")); msgBox.setInformativeText(tr("You must restart the GCS in order to activate the changes.")); msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); msgBox.setDefaultButton(QMessageBox::Ok); - if ( msgBox.exec() == QMessageBox::Ok ){ + if (msgBox.exec() == QMessageBox::Ok) { qDebug() << "Reset requested!"; Core::ICore::instance()->deleteSettings(); - } - else{ + } else { qDebug() << "Reset canceled!"; return; } @@ -225,4 +228,3 @@ void ImportExportGadgetWidget::on_resetButton_clicked() * @} * @} */ - diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.h b/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.h index bb6d46aa3..4c8b6d54f 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/importexport/importexportgadgetwidget.h @@ -16,13 +16,11 @@ #include "importexport_global.h" #include -namespace Ui -{ +namespace Ui { class ImportExportGadgetWidget; } -class IMPORTEXPORT_EXPORT ImportExportGadgetWidget : public QWidget -{ +class IMPORTEXPORT_EXPORT ImportExportGadgetWidget : public QWidget { Q_OBJECT public: ImportExportGadgetWidget(QWidget *parent = 0); @@ -36,15 +34,15 @@ protected: private: Ui::ImportExportGadgetWidget *ui; - void writeError(const QString&) const; - void exportConfiguration(const QString& fileName); - void importConfiguration(const QString& fileName); - QList getConfigurables(); + void writeError(const QString &) const; + void exportConfiguration(const QString & fileName); + void importConfiguration(const QString & fileName); + QList getConfigurables(); - QString filename; + QString filename; private slots: - void on_resetButton_clicked(); + void on_resetButton_clicked(); void on_helpButton_clicked(); void on_importButton_clicked(); void on_exportButton_clicked(); diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportplugin.cpp b/ground/openpilotgcs/src/plugins/importexport/importexportplugin.cpp index d7fa31a54..7877844de 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportplugin.cpp +++ b/ground/openpilotgcs/src/plugins/importexport/importexportplugin.cpp @@ -50,25 +50,25 @@ ImportExportPlugin::~ImportExportPlugin() // Do nothing } -bool ImportExportPlugin::initialize(const QStringList& args, QString *errMsg) +bool ImportExportPlugin::initialize(const QStringList & args, QString *errMsg) { Q_UNUSED(args); Q_UNUSED(errMsg); // Add Menu entry - Core::ActionManager* am = Core::ICore::instance()->actionManager(); - Core::ActionContainer* ac = am->actionContainer(Core::Constants::M_FILE); + Core::ActionManager *am = Core::ICore::instance()->actionManager(); + Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_FILE); - Core::Command* cmd = am->registerAction(new QAction(this), + Core::Command *cmd = am->registerAction(new QAction(this), "ImportExportPlugin.ImportExport", QList() << Core::Constants::C_GLOBAL_ID); cmd->setDefaultKeySequence(QKeySequence("Ctrl+S")); cmd->action()->setText(tr("GCS Settings Import/Export...")); -// ac->menu()->addSeparator(); -// ac->appendGroup("ImportExport"); -// ac->addAction(cmd, "ImportExport"); +// ac->menu()->addSeparator(); +// ac->appendGroup("ImportExport"); +// ac->addAction(cmd, "ImportExport"); ac->addAction(cmd, Core::Constants::G_FILE_SAVE); diff --git a/ground/openpilotgcs/src/plugins/importexport/importexportplugin.h b/ground/openpilotgcs/src/plugins/importexport/importexportplugin.h index 7070ce56c..92d8e0c6b 100644 --- a/ground/openpilotgcs/src/plugins/importexport/importexportplugin.h +++ b/ground/openpilotgcs/src/plugins/importexport/importexportplugin.h @@ -30,8 +30,7 @@ #include #include "importexport_global.h" -class IMPORTEXPORT_EXPORT ImportExportPlugin : public ExtensionSystem::IPlugin -{ +class IMPORTEXPORT_EXPORT ImportExportPlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -39,13 +38,12 @@ public: ~ImportExportPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); private: private slots: void importExport(); - }; #endif /* IMPORTEXPORTPLUGIN_H_ */ /** diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnection_internal.h b/ground/openpilotgcs/src/plugins/ipconnection/ipconnection_internal.h index c0eddc508..b7c2f3992 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnection_internal.h +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnection_internal.h @@ -3,16 +3,15 @@ #include "ipconnectionplugin.h" -//Simple class for creating & destroying a socket in the real-time thread -//Needed because sockets need to be created in the same thread that they're used -class IPConnection : public QObject -{ +// Simple class for creating & destroying a socket in the real-time thread +// Needed because sockets need to be created in the same thread that they're used +class IPConnection : public QObject { Q_OBJECT public: IPConnection(IPconnectionConnection *connection); - //virtual ~IPConnection(); + // virtual ~IPConnection(); public slots: diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.cpp b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.cpp index 537965577..14c9eccd0 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.cpp @@ -28,7 +28,7 @@ #include "ipconnectionconfiguration.h" #include -IPconnectionConfiguration::IPconnectionConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +IPconnectionConfiguration::IPconnectionConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_HostName("127.0.0.1"), m_Port(1000), @@ -40,15 +40,15 @@ IPconnectionConfiguration::IPconnectionConfiguration(QString classId, QSettings* } IPconnectionConfiguration::~IPconnectionConfiguration() -{ -} +{} IUAVGadgetConfiguration *IPconnectionConfiguration::clone() { IPconnectionConfiguration *m = new IPconnectionConfiguration(this->classId()); - m->m_Port = m_Port; + + m->m_Port = m_Port; m->m_HostName = m_HostName; - m->m_UseTCP = m_UseTCP; + m->m_UseTCP = m_UseTCP; return m; } @@ -56,23 +56,24 @@ IUAVGadgetConfiguration *IPconnectionConfiguration::clone() * Saves a configuration. * */ -void IPconnectionConfiguration::saveConfig(QSettings* qSettings) const { - qSettings->setValue("port", m_Port); - qSettings->setValue("hostName", m_HostName); - qSettings->setValue("useTCP", m_UseTCP); +void IPconnectionConfiguration::saveConfig(QSettings *qSettings) const +{ + qSettings->setValue("port", m_Port); + qSettings->setValue("hostName", m_HostName); + qSettings->setValue("useTCP", m_UseTCP); } void IPconnectionConfiguration::savesettings() const { settings->beginGroup(QLatin1String("IPconnection")); - settings->beginWriteArray("Current"); - settings->setArrayIndex(0); - settings->setValue(QLatin1String("HostName"), m_HostName); - settings->setValue(QLatin1String("Port"), m_Port); - settings->setValue(QLatin1String("UseTCP"), m_UseTCP); - settings->endArray(); - settings->endGroup(); + settings->beginWriteArray("Current"); + settings->setArrayIndex(0); + settings->setValue(QLatin1String("HostName"), m_HostName); + settings->setValue(QLatin1String("Port"), m_Port); + settings->setValue(QLatin1String("UseTCP"), m_UseTCP); + settings->endArray(); + settings->endGroup(); } @@ -80,14 +81,11 @@ void IPconnectionConfiguration::restoresettings() { settings->beginGroup(QLatin1String("IPconnection")); - settings->beginReadArray("Current"); - settings->setArrayIndex(0); - m_HostName = (settings->value(QLatin1String("HostName"), tr("")).toString()); - m_Port = (settings->value(QLatin1String("Port"), tr("")).toInt()); - m_UseTCP = (settings->value(QLatin1String("UseTCP"), tr("")).toInt()); - settings->endArray(); - settings->endGroup(); - - + settings->beginReadArray("Current"); + settings->setArrayIndex(0); + m_HostName = (settings->value(QLatin1String("HostName"), tr("")).toString()); + m_Port = (settings->value(QLatin1String("Port"), tr("")).toInt()); + m_UseTCP = (settings->value(QLatin1String("UseTCP"), tr("")).toInt()); + settings->endArray(); + settings->endGroup(); } - diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.h b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.h index 4286b91ca..63b70c5b4 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.h +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.h @@ -34,41 +34,55 @@ using namespace Core; -class IPconnectionConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT -Q_PROPERTY(QString HostName READ HostName WRITE setHostName) -Q_PROPERTY(int Port READ Port WRITE setPort) -Q_PROPERTY(int UseTCP READ UseTCP WRITE setUseTCP) +class IPconnectionConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT Q_PROPERTY(QString HostName READ HostName WRITE setHostName) + Q_PROPERTY(int Port READ Port WRITE setPort) + Q_PROPERTY(int UseTCP READ UseTCP WRITE setUseTCP) public: - explicit IPconnectionConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit IPconnectionConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); virtual ~IPconnectionConfiguration(); - void saveConfig(QSettings* settings) const; - //void savesettings(QSettings* settings) const; - //void restoresettings(QSettings* settings); + void saveConfig(QSettings *settings) const; + // void savesettings(QSettings* settings) const; + // void restoresettings(QSettings* settings); void savesettings() const; void restoresettings(); - IUAVGadgetConfiguration *clone(); + IUAVGadgetConfiguration *clone(); - QString HostName() const { return m_HostName; } - int Port() const { return m_Port; } - int UseTCP() const { return m_UseTCP; } + QString HostName() const + { + return m_HostName; + } + int Port() const + { + return m_Port; + } + int UseTCP() const + { + return m_UseTCP; + } public slots: - void setHostName(QString HostName) { m_HostName = HostName; } - void setPort(int Port) { m_Port = Port; } - void setUseTCP(int UseTCP) { m_UseTCP = UseTCP; } + void setHostName(QString HostName) + { + m_HostName = HostName; + } + void setPort(int Port) + { + m_Port = Port; + } + void setUseTCP(int UseTCP) + { + m_UseTCP = UseTCP; + } private: QString m_HostName; int m_Port; int m_UseTCP; - QSettings* settings; - - + QSettings *settings; }; #endif // IPconnectionCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.cpp b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.cpp index a3dc4b5bc..af274a67c 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.cpp @@ -41,23 +41,19 @@ IPconnectionOptionsPage::IPconnectionOptionsPage(IPconnectionConfiguration *config, QObject *parent) : IOptionsPage(parent), m_config(config) -{ - -} +{} IPconnectionOptionsPage::~IPconnectionOptionsPage() -{ -} +{} QWidget *IPconnectionOptionsPage::createPage(QWidget *parent) { - m_page = new Ui::IPconnectionOptionsPage(); QWidget *w = new QWidget(parent); m_page->setupUi(w); m_page->Port->setValue(m_config->Port()); m_page->HostName->setText(m_config->HostName()); - m_page->UseTCP->setChecked(m_config->UseTCP()?true:false); - m_page->UseUDP->setChecked(m_config->UseTCP()?false:true); + m_page->UseTCP->setChecked(m_config->UseTCP() ? true : false); + m_page->UseUDP->setChecked(m_config->UseTCP() ? false : true); return w; } @@ -66,17 +62,13 @@ void IPconnectionOptionsPage::apply() { m_config->setPort(m_page->Port->value()); m_config->setHostName(m_page->HostName->text()); - m_config->setUseTCP(m_page->UseTCP->isChecked()?1:0); + m_config->setUseTCP(m_page->UseTCP->isChecked() ? 1 : 0); m_config->savesettings(); emit availableDevChanged(); - - - } void IPconnectionOptionsPage::finish() { delete m_page; } - diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.h b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.h index e750159db..d0110339c 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.h +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionoptionspage.h @@ -33,26 +33,37 @@ class IPconnectionConfiguration; namespace Core { - class IUAVGadgetConfiguration; +class IUAVGadgetConfiguration; } namespace Ui { - class IPconnectionOptionsPage; +class IPconnectionOptionsPage; } using namespace Core; -class IPconnectionOptionsPage : public IOptionsPage -{ -Q_OBJECT +class IPconnectionOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit IPconnectionOptionsPage(IPconnectionConfiguration *config, QObject *parent = 0); virtual ~IPconnectionOptionsPage(); - QString id() const { return QLatin1String("settings"); } - QString trName() const { return tr("settings"); } - QString category() const { return "IP Network Telemetry"; }; - QString trCategory() const { return "IP Network Telemetry"; }; + QString id() const + { + return QLatin1String("settings"); + } + QString trName() const + { + return tr("settings"); + } + QString category() const + { + return "IP Network Telemetry"; + }; + QString trCategory() const + { + return "IP Network Telemetry"; + }; QWidget *createPage(QWidget *parent); void apply(); @@ -65,7 +76,6 @@ public slots: private: IPconnectionConfiguration *m_config; Ui::IPconnectionOptionsPage *m_page; - }; #endif // IPconnectionOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp index c4d95ec4b..7ee944071 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp @@ -25,7 +25,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -//The core of this plugin has been directly copied from the serial plugin and converted to work over a TCP link instead of a direct serial link +// The core of this plugin has been directly copied from the serial plugin and converted to work over a TCP link instead of a direct serial link #include "ipconnectionplugin.h" @@ -46,11 +46,11 @@ #include -//Communication between IPconnectionConnection::OpenDevice() and IPConnection::onOpenDevice() +// Communication between IPconnectionConnection::OpenDevice() and IPConnection::onOpenDevice() QString errorMsg; QWaitCondition openDeviceWait; QWaitCondition closeDeviceWait; -//QReadWriteLock dummyLock; +// QReadWriteLock dummyLock; QMutex ipConMutex; QAbstractSocket *ret; @@ -58,16 +58,16 @@ IPConnection::IPConnection(IPconnectionConnection *connection) : QObject() { moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread()); - QObject::connect(connection, SIGNAL(CreateSocket(QString,int,bool)), - this, SLOT(onOpenDevice(QString,int,bool))); - QObject::connect(connection, SIGNAL(CloseSocket(QAbstractSocket*)), - this, SLOT(onCloseDevice(QAbstractSocket*))); + QObject::connect(connection, SIGNAL(CreateSocket(QString, int, bool)), + this, SLOT(onOpenDevice(QString, int, bool))); + QObject::connect(connection, SIGNAL(CloseSocket(QAbstractSocket *)), + this, SLOT(onCloseDevice(QAbstractSocket *))); } /*IPConnection::~IPConnection() -{ + { -}*/ + }*/ void IPConnection::onOpenDevice(QString HostName, int Port, bool UseTCP) { @@ -81,27 +81,25 @@ void IPConnection::onOpenDevice(QString HostName, int Port, bool UseTCP) ipSocket = new QUdpSocket(this); } - //do sanity check on hostname and port... - if((HostName.length()==0)||(Port<1)){ + // do sanity check on hostname and port... + if ((HostName.length() == 0) || (Port < 1)) { errorMsg = "Please configure Host and Port options before opening the connection"; - - } - else { - //try to connect... + } else { + // try to connect... ipSocket->connectToHost(HostName, Port); - //in blocking mode so we wait for the connection to succeed + // in blocking mode so we wait for the connection to succeed if (ipSocket->waitForConnected(Timeout)) { ret = ipSocket; openDeviceWait.wakeAll(); ipConMutex.unlock(); return; } - //tell user something went wrong - errorMsg = ipSocket->errorString (); + // tell user something went wrong + errorMsg = ipSocket->errorString(); } /* BUGBUG TODO - returning null here leads to segfault because some caller still calls disconnect without checking our return value properly - * someone needs to debug this, I got lost in the calling chain.*/ + * someone needs to debug this, I got lost in the calling chain.*/ ret = NULL; openDeviceWait.wakeAll(); ipConMutex.unlock(); @@ -110,28 +108,29 @@ void IPConnection::onOpenDevice(QString HostName, int Port, bool UseTCP) void IPConnection::onCloseDevice(QAbstractSocket *ipSocket) { ipConMutex.lock(); - ipSocket->close (); - delete(ipSocket); + ipSocket->close(); + delete (ipSocket); closeDeviceWait.wakeAll(); ipConMutex.unlock(); } -IPConnection * connection = 0; +IPConnection *connection = 0; IPconnectionConnection::IPconnectionConnection() { ipSocket = NULL; - //create all our objects + // create all our objects m_config = new IPconnectionConfiguration("IP Network Telemetry", NULL, this); m_config->restoresettings(); - m_optionspage = new IPconnectionOptionsPage(m_config,this); + m_optionspage = new IPconnectionOptionsPage(m_config, this); - if(!connection) + if (!connection) { connection = new IPConnection(this); + } - //just signal whenever we have a device event... + // just signal whenever we have a device event... QMainWindow *mw = Core::ICore::instance()->mainWindow(); QObject::connect(mw, SIGNAL(deviceChange()), this, SLOT(onEnumerationChanged())); @@ -140,35 +139,34 @@ IPconnectionConnection::IPconnectionConnection() } IPconnectionConnection::~IPconnectionConnection() -{//clean up out resources... - if (ipSocket){ - ipSocket->close (); - delete(ipSocket); +{ // clean up out resources... + if (ipSocket) { + ipSocket->close(); + delete (ipSocket); } - if(connection) - { + if (connection) { delete connection; connection = NULL; } } void IPconnectionConnection::onEnumerationChanged() -{//no change from serial plugin +{ // no change from serial plugin emit availableDevChanged(this); } - QList IPconnectionConnection::availableDevices() { QList list; device d; - if (m_config->HostName().length()>1) - d.displayName=(const QString )m_config->HostName(); - else - d.displayName="Unconfigured"; - d.name=(const QString )m_config->HostName(); - //we only have one "device" as defined by the configuration m_config + if (m_config->HostName().length() > 1) { + d.displayName = (const QString)m_config->HostName(); + } else { + d.displayName = "Unconfigured"; + } + d.name = (const QString)m_config->HostName(); + // we only have one "device" as defined by the configuration m_config list.append(d); return list; @@ -181,13 +179,13 @@ QIODevice *IPconnectionConnection::openDevice(const QString &) bool UseTCP; QMessageBox msgBox; - //get the configuration info + // get the configuration info HostName = m_config->HostName(); - Port = m_config->Port(); - UseTCP = m_config->UseTCP(); + Port = m_config->Port(); + UseTCP = m_config->UseTCP(); - if (ipSocket){ - //Andrew: close any existing socket... this should never occur + if (ipSocket) { + // Andrew: close any existing socket... this should never occur ipConMutex.lock(); emit CloseSocket(ipSocket); closeDeviceWait.wait(&ipConMutex); @@ -200,9 +198,8 @@ QIODevice *IPconnectionConnection::openDevice(const QString &) openDeviceWait.wait(&ipConMutex); ipConMutex.unlock(); ipSocket = ret; - if(ipSocket == NULL) - { - msgBox.setText((const QString )errorMsg); + if (ipSocket == NULL) { + msgBox.setText((const QString)errorMsg); msgBox.exec(); } return ipSocket; @@ -210,7 +207,7 @@ QIODevice *IPconnectionConnection::openDevice(const QString &) void IPconnectionConnection::closeDevice(const QString &) { - if (ipSocket){ + if (ipSocket) { ipConMutex.lock(); emit CloseSocket(ipSocket); closeDeviceWait.wait(&ipConMutex); @@ -221,12 +218,12 @@ void IPconnectionConnection::closeDevice(const QString &) QString IPconnectionConnection::connectionName() -{//updated from serial plugin +{ // updated from serial plugin return QString("Network telemetry port"); } QString IPconnectionConnection::shortName() -{//updated from serial plugin +{ // updated from serial plugin if (m_config->UseTCP()) { return QString("TCP"); } else { @@ -236,11 +233,11 @@ QString IPconnectionConnection::shortName() IPconnectionPlugin::IPconnectionPlugin() -{//no change from serial plugin +{ // no change from serial plugin } IPconnectionPlugin::~IPconnectionPlugin() -{//manually remove the options page object +{ // manually remove the options page object removeObject(m_connection->Optionspage()); } @@ -254,9 +251,9 @@ bool IPconnectionPlugin::initialize(const QStringList &arguments, QString *error Q_UNUSED(arguments); Q_UNUSED(errorString); m_connection = new IPconnectionConnection(); - //must manage this registration of child object ourselves - //if we use an autorelease here it causes the GCS to crash - //as it is deleting objects as the app closes... + // must manage this registration of child object ourselves + // if we use an autorelease here it causes the GCS to crash + // as it is deleting objects as the app closes... addObject(m_connection->Optionspage()); return true; diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.h b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.h index 75500555d..7a1bea5bb 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.h +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.h @@ -33,7 +33,7 @@ #include "ipconnectionconfiguration.h" #include "coreplugin/iconnection.h" #include -//#include +// #include class QAbstractSocket; @@ -42,13 +42,12 @@ class QUdpSocket; class IConnection; /** -* Define a connection via the IConnection interface -* Plugin will add a instance of this class to the pool, -* so the connection manager can use it. -*/ + * Define a connection via the IConnection interface + * Plugin will add a instance of this class to the pool, + * so the connection manager can use it. + */ class IPconnection_EXPORT IPconnectionConnection - : public Core::IConnection -{ + : public Core::IConnection { Q_OBJECT public: IPconnectionConnection(); @@ -61,30 +60,33 @@ public: virtual QString connectionName(); virtual QString shortName(); - IPconnectionConfiguration * Config() const { return m_config; } - IPconnectionOptionsPage * Optionspage() const { return m_optionspage; } - + IPconnectionConfiguration *Config() const + { + return m_config; + } + IPconnectionOptionsPage *Optionspage() const + { + return m_optionspage; + } protected slots: void onEnumerationChanged(); -signals: //For the benefit of IPConnection +signals: // For the benefit of IPConnection void CreateSocket(QString HostName, int Port, bool UseTCP); void CloseSocket(QAbstractSocket *socket); private: - QAbstractSocket *ipSocket; - IPconnectionConfiguration *m_config; - IPconnectionOptionsPage *m_optionspage; - //QSettings* settings; - + QAbstractSocket *ipSocket; + IPconnectionConfiguration *m_config; + IPconnectionOptionsPage *m_optionspage; + // QSettings* settings; }; class IPconnection_EXPORT IPconnectionPlugin - : public ExtensionSystem::IPlugin -{ + : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -96,7 +98,6 @@ public: private: IPconnectionConnection *m_connection; - }; diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.cpp b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.cpp index be5a3356f..dc2e21c47 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.cpp +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,10 +30,9 @@ #include "lineardialgadgetconfiguration.h" LineardialGadget::LineardialGadget(QString classId, LineardialGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} LineardialGadget::~LineardialGadget() { @@ -41,22 +40,23 @@ LineardialGadget::~LineardialGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void LineardialGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void LineardialGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - LineardialGadgetConfiguration *m = qobject_cast(config); + LineardialGadgetConfiguration *m = qobject_cast(config); + m_widget->setFactor(m->getFactor()); m_widget->setDecimalPlaces(m->getDecimalPlaces()); - m_widget->setRange(m->getMin(),m->getMax()); + m_widget->setRange(m->getMin(), m->getMax()); m_widget->setGreenRange(m->getGreenMin(), m->getGreenMax()); m_widget->setYellowRange(m->getYellowMin(), m->getYellowMax()); m_widget->setRedRange(m->getRedMin(), m->getRedMax()); m_widget->setDialFile(m->getDialFile()); // Triggers widget repaint m_widget->setDialFont(m->getFont()); m_widget->connectInput(m->getSourceDataObject(), m->getSourceObjectField()); - m_widget->enableOpenGL(m->useOpenGL()); + m_widget->enableOpenGL(m->useOpenGL()); } diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.h b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.h index 16d280576..4f1311f42 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.h +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,15 +38,17 @@ class LineardialGadgetWidget; using namespace Core; -class LineardialGadget : public Core::IUAVGadget -{ +class LineardialGadget : public Core::IUAVGadget { Q_OBJECT public: LineardialGadget(QString classId, LineardialGadgetWidget *widget, QWidget *parent = 0); ~LineardialGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: LineardialGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.cpp index e622b29f4..1bfc5a434 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.cpp @@ -32,7 +32,7 @@ * Loads a saved configuration or defaults if non exist. * */ -LineardialGadgetConfiguration::LineardialGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +LineardialGadgetConfiguration::LineardialGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), dialFile("Unknown"), sourceDataObject("Unknown"), @@ -46,28 +46,28 @@ LineardialGadgetConfiguration::LineardialGadgetConfiguration(QString classId, QS greenMin(66), greenMax(100), factor(1.00), - decimalPlaces(0), - useOpenGLFlag(false) + decimalPlaces(0), + useOpenGLFlag(false) { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { QString dFile = qSettings->value("dFile").toString(); - dialFile = Utils::PathUtils().InsertDataPath(dFile); - sourceDataObject = qSettings->value("sourceDataObject").toString(); + dialFile = Utils::PathUtils().InsertDataPath(dFile); + sourceDataObject = qSettings->value("sourceDataObject").toString(); sourceObjectField = qSettings->value("sourceObjectField").toString(); - minValue = qSettings->value("minValue").toDouble(); - maxValue = qSettings->value("maxValue").toDouble(); - redMin = qSettings->value("redMin").toDouble(); - redMax = qSettings->value("redMax").toDouble(); - yellowMin = qSettings->value("yellowMin").toDouble(); - yellowMax = qSettings->value("yellowMax").toDouble(); - greenMin = qSettings->value("greenMin").toDouble(); - greenMax = qSettings->value("greenMax").toDouble(); + minValue = qSettings->value("minValue").toDouble(); + maxValue = qSettings->value("maxValue").toDouble(); + redMin = qSettings->value("redMin").toDouble(); + redMax = qSettings->value("redMax").toDouble(); + yellowMin = qSettings->value("yellowMin").toDouble(); + yellowMax = qSettings->value("yellowMax").toDouble(); + greenMin = qSettings->value("greenMin").toDouble(); + greenMax = qSettings->value("greenMax").toDouble(); font = qSettings->value("font").toString(); - decimalPlaces = qSettings->value("decimalPlaces").toInt(); - factor = qSettings->value("factor").toDouble(); - useOpenGLFlag = qSettings->value("useOpenGLFlag").toBool(); - } + decimalPlaces = qSettings->value("decimalPlaces").toInt(); + factor = qSettings->value("factor").toDouble(); + useOpenGLFlag = qSettings->value("useOpenGLFlag").toBool(); + } } /** @@ -77,21 +77,22 @@ LineardialGadgetConfiguration::LineardialGadgetConfiguration(QString classId, QS IUAVGadgetConfiguration *LineardialGadgetConfiguration::clone() { LineardialGadgetConfiguration *m = new LineardialGadgetConfiguration(this->classId()); - m->dialFile=dialFile; - m->sourceDataObject = sourceDataObject; + + m->dialFile = dialFile; + m->sourceDataObject = sourceDataObject; m->sourceObjectField = sourceObjectField; - m->minValue = minValue; - m->maxValue = maxValue; - m->redMin = redMin; - m->redMax = redMax; - m->yellowMin = yellowMin; - m->yellowMax = yellowMax; - m->greenMin = greenMin; - m->greenMax = greenMax; + m->minValue = minValue; + m->maxValue = maxValue; + m->redMin = redMin; + m->redMax = redMax; + m->yellowMin = yellowMin; + m->yellowMax = yellowMax; + m->greenMin = greenMin; + m->greenMax = greenMax; m->font = font; - m->decimalPlaces = decimalPlaces; - m->factor = factor; - m->useOpenGLFlag = useOpenGLFlag; + m->decimalPlaces = decimalPlaces; + m->factor = factor; + m->useOpenGLFlag = useOpenGLFlag; return m; } @@ -100,21 +101,23 @@ IUAVGadgetConfiguration *LineardialGadgetConfiguration::clone() * Saves a configuration. * */ -void LineardialGadgetConfiguration::saveConfig(QSettings* qSettings) const { - QString dFile = Utils::PathUtils().RemoveDataPath(dialFile); - qSettings->setValue("dFile", dFile); - qSettings->setValue("sourceDataObject", sourceDataObject); - qSettings->setValue("sourceObjectField", sourceObjectField); - qSettings->setValue("minValue", minValue); - qSettings->setValue("maxValue", maxValue); - qSettings->setValue("redMin", redMin); - qSettings->setValue("redMax", redMax); - qSettings->setValue("yellowMin", yellowMin); - qSettings->setValue("yellowMax", yellowMax); - qSettings->setValue("greenMin", greenMin); - qSettings->setValue("greenMax", greenMax); - qSettings->setValue("font", font); - qSettings->setValue("decimalPlaces", decimalPlaces); - qSettings->setValue("factor", factor); - qSettings->setValue("useOpenGLFlag", useOpenGLFlag); +void LineardialGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ + QString dFile = Utils::PathUtils().RemoveDataPath(dialFile); + + qSettings->setValue("dFile", dFile); + qSettings->setValue("sourceDataObject", sourceDataObject); + qSettings->setValue("sourceObjectField", sourceObjectField); + qSettings->setValue("minValue", minValue); + qSettings->setValue("maxValue", maxValue); + qSettings->setValue("redMin", redMin); + qSettings->setValue("redMax", redMax); + qSettings->setValue("yellowMin", yellowMin); + qSettings->setValue("yellowMax", yellowMax); + qSettings->setValue("greenMin", greenMin); + qSettings->setValue("greenMax", greenMax); + qSettings->setValue("font", font); + qSettings->setValue("decimalPlaces", decimalPlaces); + qSettings->setValue("factor", factor); + qSettings->setValue("useOpenGLFlag", useOpenGLFlag); } diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.h index d8991c0c1..3a85146a1 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetconfiguration.h @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,48 +34,125 @@ using namespace Core; /* This is a generic bargraph dial supporting one indicator. - */ -class LineardialGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT + */ +class LineardialGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit LineardialGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit LineardialGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - //set dial configuration functions - void setDialFile(QString filename){dialFile=filename;} - void setRange(double min, double max) { minValue = min; maxValue = max;} - void setGreenRange(double min, double max) { greenMin = min; greenMax = max;} - void setYellowRange(double min, double max) { yellowMin = min; yellowMax = max;} - void setRedRange(double min, double max) { redMin = min; redMax = max;} + // set dial configuration functions + void setDialFile(QString filename) + { + dialFile = filename; + } + void setRange(double min, double max) + { + minValue = min; maxValue = max; + } + void setGreenRange(double min, double max) + { + greenMin = min; greenMax = max; + } + void setYellowRange(double min, double max) + { + yellowMin = min; yellowMax = max; + } + void setRedRange(double min, double max) + { + redMin = min; redMax = max; + } - void setFont(QString text) { font = text; } + void setFont(QString text) + { + font = text; + } - void setFactor(double val) { factor = val; } - void setDecimalPlaces (int val) { decimalPlaces = val; } + void setFactor(double val) + { + factor = val; + } + void setDecimalPlaces(int val) + { + decimalPlaces = val; + } - void setSourceDataObject(QString text) {sourceDataObject = text; } - void setSourceObjField(QString text) { sourceObjectField = text; } + void setSourceDataObject(QString text) + { + sourceDataObject = text; + } + void setSourceObjField(QString text) + { + sourceObjectField = text; + } - void setUseOpenGL(bool flag) { useOpenGLFlag = flag; } + void setUseOpenGL(bool flag) + { + useOpenGLFlag = flag; + } - //get dial configuration functions - QString getDialFile() {return dialFile;} - double getMin() { return minValue;} - double getMax() { return maxValue;} - double getGreenMin(){ return greenMin;} - double getGreenMax(){ return greenMax;} - double getYellowMin(){ return yellowMin;} - double getYellowMax(){ return yellowMax;} - double getRedMin(){ return redMin;} - double getRedMax(){ return redMax;} - QString getSourceDataObject() { return sourceDataObject;} - QString getSourceObjectField() { return sourceObjectField;} - QString getFont() { return font;} - int getDecimalPlaces() { return decimalPlaces; } - double getFactor() { return factor; } - bool useOpenGL() { return useOpenGLFlag; } + // get dial configuration functions + QString getDialFile() + { + return dialFile; + } + double getMin() + { + return minValue; + } + double getMax() + { + return maxValue; + } + double getGreenMin() + { + return greenMin; + } + double getGreenMax() + { + return greenMax; + } + double getYellowMin() + { + return yellowMin; + } + double getYellowMax() + { + return yellowMax; + } + double getRedMin() + { + return redMin; + } + double getRedMax() + { + return redMax; + } + QString getSourceDataObject() + { + return sourceDataObject; + } + QString getSourceObjectField() + { + return sourceObjectField; + } + QString getFont() + { + return font; + } + int getDecimalPlaces() + { + return decimalPlaces; + } + double getFactor() + { + return factor; + } + bool useOpenGL() + { + return useOpenGLFlag; + } - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: @@ -101,7 +178,7 @@ private: double greenMax; double factor; - bool useOpenGLFlag; + bool useOpenGLFlag; int decimalPlaces; }; diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.cpp index 0fc4e2616..4d2e27827 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,29 +32,27 @@ #include LineardialGadgetFactory::LineardialGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("LineardialGadget"), - tr("Bargraph Dial"), - parent) -{ -} + IUAVGadgetFactory(QString("LineardialGadget"), + tr("Bargraph Dial"), + parent) +{} LineardialGadgetFactory::~LineardialGadgetFactory() -{ -} +{} -Core::IUAVGadget* LineardialGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *LineardialGadgetFactory::createGadget(QWidget *parent) { - LineardialGadgetWidget* gadgetWidget = new LineardialGadgetWidget(parent); + LineardialGadgetWidget *gadgetWidget = new LineardialGadgetWidget(parent); + return new LineardialGadget(QString("LineardialGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *LineardialGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *LineardialGadgetFactory::createConfiguration(QSettings *qSettings) { return new LineardialGadgetConfiguration(QString("LineardialGadget"), qSettings); } IOptionsPage *LineardialGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new LineardialGadgetOptionsPage(qobject_cast(config)); + return new LineardialGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.h b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.h index e8040d68d..7eeecb307 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class LineardialGadgetFactory : public IUAVGadgetFactory -{ +class LineardialGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: LineardialGadgetFactory(QObject *parent = 0); ~LineardialGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.cpp index ccd34af51..ae257508a 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,21 +38,20 @@ #include LineardialGadgetOptionsPage::LineardialGadgetOptionsPage(LineardialGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) + IOptionsPage(parent), + m_config(config) { font = QFont("Arial", 12); // Default in case nothing exists yet. } -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *LineardialGadgetOptionsPage::createPage(QWidget *parent) { - Q_UNUSED(parent); options_page = new Ui::LineardialGadgetOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget; - //main layout + // main layout options_page->setupUi(optionsPageWidget); @@ -75,27 +74,27 @@ QWidget *LineardialGadgetOptionsPage::createPage(QWidget *parent) options_page->redMax->setValue(m_config->getRedMax()); options_page->factor->setValue(m_config->getFactor()); options_page->decPlaces->setValue(m_config->getDecimalPlaces()); - font.fromString(m_config->getFont()); - options_page->useOpenGL->setChecked(m_config->useOpenGL()); + font.fromString(m_config->getFont()); + options_page->useOpenGL->setChecked(m_config->useOpenGL()); // Fills the combo boxes for the UAVObjects ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - QList< QList > objList = objManager->getDataObjects(); - foreach (QList list, objList) { - foreach (UAVDataObject* obj, list) { + QList< QList > objList = objManager->getDataObjects(); + foreach(QList list, objList) { + foreach(UAVDataObject * obj, list) { options_page->objectName->addItem(obj->getName()); } } - //select saved UAV Object field values - if(options_page->objectName->findText(m_config->getSourceDataObject())!=-1){ + // select saved UAV Object field values + if (options_page->objectName->findText(m_config->getSourceDataObject()) != -1) { options_page->objectName->setCurrentIndex(options_page->objectName->findText(m_config->getSourceDataObject())); // Now load the object field values: - UAVDataObject* obj = dynamic_cast( objManager->getObject(m_config->getSourceDataObject()) ); - if (obj != NULL ) { - on_objectName_currentIndexChanged(m_config->getSourceDataObject()); - // And set the highlighed value from the settings: - options_page->objectField->setCurrentIndex(options_page->objectField->findText(m_config->getSourceObjectField())); + UAVDataObject *obj = dynamic_cast(objManager->getObject(m_config->getSourceDataObject())); + if (obj != NULL) { + on_objectName_currentIndexChanged(m_config->getSourceDataObject()); + // And set the highlighed value from the settings: + options_page->objectField->setCurrentIndex(options_page->objectField->findText(m_config->getSourceObjectField())); } } @@ -122,7 +121,6 @@ void LineardialGadgetOptionsPage::on_rangeMin_valueChanged(double val) options_page->greenMax->setMinimum(val); options_page->yellowMax->setMinimum(val); options_page->redMax->setMinimum(val); - } /** @@ -155,16 +153,16 @@ void LineardialGadgetOptionsPage::apply() m_config->setDialFile(options_page->svgSourceFile->path()); double rangeMin = options_page->minValue->value(); double rangeMax = options_page->maxValue->value(); - m_config->setRange(rangeMin,rangeMax); - m_config->setGreenRange(options_page->greenMin->value(),options_page->greenMax->value()); - m_config->setYellowRange(options_page->yellowMin->value(),options_page->yellowMax->value()); - m_config->setRedRange(options_page->redMin->value(),options_page->redMax->value()); + m_config->setRange(rangeMin, rangeMax); + m_config->setGreenRange(options_page->greenMin->value(), options_page->greenMax->value()); + m_config->setYellowRange(options_page->yellowMin->value(), options_page->yellowMax->value()); + m_config->setRedRange(options_page->redMin->value(), options_page->redMax->value()); m_config->setSourceDataObject(options_page->objectName->currentText()); m_config->setSourceObjField(options_page->objectField->currentText()); m_config->setFont(font.toString()); m_config->setDecimalPlaces(options_page->decPlaces->value()); m_config->setFactor(options_page->factor->value()); - m_config->setUseOpenGL(options_page->useOpenGL->checkState()); + m_config->setUseOpenGL(options_page->useOpenGL->checkState()); } /** @@ -174,34 +172,32 @@ void LineardialGadgetOptionsPage::apply() void LineardialGadgetOptionsPage::on_fontPicker_clicked() { bool ok; - font = QFontDialog::getFont(&ok, font , qobject_cast(this)); + + font = QFontDialog::getFont(&ok, font, qobject_cast(this)); } /* - Fills in the field1 combo box when value is changed in the - object1 field -*/ -void LineardialGadgetOptionsPage::on_objectName_currentIndexChanged(QString val) { + Fills in the field1 combo box when value is changed in the + object1 field + */ +void LineardialGadgetOptionsPage::on_objectName_currentIndexChanged(QString val) +{ options_page->objectField->clear(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(val) ); - QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { - if(field->getElementNames().count() > 1) - { - foreach(QString elemName , field->getElementNames()) - { + UAVDataObject *obj = dynamic_cast(objManager->getObject(val)); + QList fieldList = obj->getFields(); + foreach(UAVObjectField * field, fieldList) { + if (field->getElementNames().count() > 1) { + foreach(QString elemName, field->getElementNames()) { options_page->objectField->addItem(field->getName() + "-" + elemName); } - } - else + } else { options_page->objectField->addItem(field->getName()); + } } } void LineardialGadgetOptionsPage::finish() -{ - -} +{} diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.h index a9939de2d..de79c2255 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetoptionspage.h @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -41,14 +41,13 @@ class IUAVGadgetConfiguration; class LineardialGadgetConfiguration; namespace Ui { - class LineardialGadgetOptionsPage; +class LineardialGadgetOptionsPage; } using namespace Core; -class LineardialGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class LineardialGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit LineardialGadgetOptionsPage(LineardialGadgetConfiguration *config, QObject *parent = 0); @@ -66,7 +65,6 @@ private slots: void on_objectName_currentIndexChanged(QString val); void on_rangeMin_valueChanged(double val); void on_rangeMax_valueChanged(double val); - }; #endif // LINEARDIALGADGETOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.cpp index a108877a8..d4ee357bb 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.cpp @@ -33,52 +33,53 @@ LineardialGadgetWidget::LineardialGadgetWidget(QWidget *parent) : QGraphicsView(parent) { - setMinimumSize(32,32); + setMinimumSize(32, 32); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setScene(new QGraphicsScene(this)); setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); - m_renderer = new QSvgRenderer(); + m_renderer = new QSvgRenderer(); verticalDial = false; paint(); obj1 = NULL; - fieldName = NULL; - fieldValue = NULL; + fieldName = NULL; + fieldValue = NULL; indexTarget = 0; - indexValue = 0; + indexValue = 0; places = 0; factor = 1; // This timer mechanism makes the index rotate smoothly connect(&dialTimer, SIGNAL(timeout()), this, SLOT(moveIndex())); dialTimer.start(30); - } LineardialGadgetWidget::~LineardialGadgetWidget() { - // Do nothing + // Do nothing } /*! - \brief Enables/Disables OpenGL - */ + \brief Enables/Disables OpenGL + */ void LineardialGadgetWidget::enableOpenGL(bool flag) { - if (flag) - setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); - else - setViewport(new QWidget); + if (flag) { + setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); + } else { + setViewport(new QWidget); + } } /*! - \brief Connects the widget to the relevant UAVObjects - */ -void LineardialGadgetWidget::connectInput(QString object1, QString nfield1) { - - if (obj1 != NULL) - disconnect(obj1,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updateIndex(UAVObject*))); + \brief Connects the widget to the relevant UAVObjects + */ +void LineardialGadgetWidget::connectInput(QString object1, QString nfield1) +{ + if (obj1 != NULL) { + disconnect(obj1, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateIndex(UAVObject *))); + } ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); @@ -86,25 +87,22 @@ void LineardialGadgetWidget::connectInput(QString object1, QString nfield1) { // Check validity of arguments first, reject empty args and unknown fields. if (!(object1.isEmpty() || nfield1.isEmpty())) { - obj1 = dynamic_cast( objManager->getObject(object1) ); - if (obj1 != NULL ) { - connect(obj1, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateIndex(UAVObject*))); - if(nfield1.contains("-")) - { + obj1 = dynamic_cast(objManager->getObject(object1)); + if (obj1 != NULL) { + connect(obj1, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateIndex(UAVObject *))); + if (nfield1.contains("-")) { QStringList fieldSubfield = nfield1.split("-", QString::SkipEmptyParts); - field1 = fieldSubfield.at(0); - subfield1 = fieldSubfield.at(1); + field1 = fieldSubfield.at(0); + subfield1 = fieldSubfield.at(1); haveSubField1 = true; - } - else - { - field1= nfield1; + } else { + field1 = nfield1; haveSubField1 = false; } - if (fieldName) + if (fieldName) { fieldName->setPlainText(nfield1); + } updateIndex(obj1); - } else { qDebug() << "Error: Object is unknown (" << object1 << ") this should not happen."; } @@ -112,24 +110,27 @@ void LineardialGadgetWidget::connectInput(QString object1, QString nfield1) { } /*! - \brief Called by the UAVObject which got updated + \brief Called by the UAVObject which got updated - Updates the numeric value and/or the icon if the dial wants this. - */ -void LineardialGadgetWidget::updateIndex(UAVObject *object1) { + Updates the numeric value and/or the icon if the dial wants this. + */ +void LineardialGadgetWidget::updateIndex(UAVObject *object1) +{ // Double check that the field exists: - UAVObjectField* field = object1->getField(field1); + UAVObjectField *field = object1->getField(field1); + if (field) { QString s; if (field->isNumeric()) { double v; - if(haveSubField1){ + if (haveSubField1) { int indexOfSubField = field->getElementNames().indexOf(QRegExp(subfield1, Qt::CaseSensitive, QRegExp::FixedString)); - v = field->getDouble(indexOfSubField)*factor; - }else - v = field->getDouble()*factor; + v = field->getDouble(indexOfSubField) * factor; + } else { + v = field->getDouble() * factor; + } setIndex(v); - s.sprintf("%.*f",places,v); + s.sprintf("%.*f", places, v); } if (field->isText()) { s = field->getValue().toString(); @@ -144,191 +145,192 @@ void LineardialGadgetWidget::updateIndex(UAVObject *object1) { } } - if (fieldValue) + if (fieldValue) { fieldValue->setPlainText(s); + } - if (index && !dialTimer.isActive()) + if (index && !dialTimer.isActive()) { dialTimer.start(); + } } else { qDebug() << "Wrong field, maybe an issue with object disconnection ?"; } } /*! - \brief Setup dial using its master SVG template. + \brief Setup dial using its master SVG template. - Should only be called after the min/max ranges have been set. + Should only be called after the min/max ranges have been set. - */ + */ void LineardialGadgetWidget::setDialFile(QString dfn) { QGraphicsScene *l_scene = scene(); + setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); - if (QFile::exists(dfn) && m_renderer->load(dfn) && m_renderer->isValid() ) - { - l_scene->clear(); // Beware: clear also deletes all objects - // which are currently in the scene - background = new QGraphicsSvgItem(); - background->setSharedRenderer(m_renderer); - background->setElementId("background"); - background->setFlags(QGraphicsItem::ItemClipsChildrenToShape| - QGraphicsItem::ItemClipsToShape); - l_scene->addItem(background); + if (QFile::exists(dfn) && m_renderer->load(dfn) && m_renderer->isValid()) { + l_scene->clear(); // Beware: clear also deletes all objects + // which are currently in the scene + background = new QGraphicsSvgItem(); + background->setSharedRenderer(m_renderer); + background->setElementId("background"); + background->setFlags(QGraphicsItem::ItemClipsChildrenToShape | + QGraphicsItem::ItemClipsToShape); + l_scene->addItem(background); - // The red/yellow/green zones are optional, we just - // test on the presence of "red" - if (m_renderer->elementExists("red")) { - // Order is important: red, then yellow then green - // overlayed on top of each other - red = new QGraphicsSvgItem(); - red->setSharedRenderer(m_renderer); - red->setElementId("red"); - red->setParentItem(background); - yellow = new QGraphicsSvgItem(); - yellow->setSharedRenderer(m_renderer); - yellow->setElementId("yellow"); - yellow->setParentItem(background); - green = new QGraphicsSvgItem(); - green->setSharedRenderer(m_renderer); - green->setElementId("green"); - green->setParentItem(background); - // In order to properly render the Green/Yellow/Red graphs, we need to find out - // the starting location of the bargraph rendering area: - QMatrix textMatrix = m_renderer->matrixForElement("bargraph"); - qreal bgX = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).x(); - qreal bgY = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).y(); - bargraphSize = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).width(); - // Detect if the bargraph is vertical or horizontal. - qreal bargraphHeight = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).height(); - if (bargraphHeight > bargraphSize) { - verticalDial = true; - bargraphSize = bargraphHeight; - } else { - verticalDial = false; - } - // Now adjust the red/yellow/green zones: - double range = maxValue-minValue; + // The red/yellow/green zones are optional, we just + // test on the presence of "red" + if (m_renderer->elementExists("red")) { + // Order is important: red, then yellow then green + // overlayed on top of each other + red = new QGraphicsSvgItem(); + red->setSharedRenderer(m_renderer); + red->setElementId("red"); + red->setParentItem(background); + yellow = new QGraphicsSvgItem(); + yellow->setSharedRenderer(m_renderer); + yellow->setElementId("yellow"); + yellow->setParentItem(background); + green = new QGraphicsSvgItem(); + green->setSharedRenderer(m_renderer); + green->setElementId("green"); + green->setParentItem(background); + // In order to properly render the Green/Yellow/Red graphs, we need to find out + // the starting location of the bargraph rendering area: + QMatrix textMatrix = m_renderer->matrixForElement("bargraph"); + qreal bgX = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).x(); + qreal bgY = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).y(); + bargraphSize = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).width(); + // Detect if the bargraph is vertical or horizontal. + qreal bargraphHeight = textMatrix.mapRect(m_renderer->boundsOnElement("bargraph")).height(); + if (bargraphHeight > bargraphSize) { + verticalDial = true; + bargraphSize = bargraphHeight; + } else { + verticalDial = false; + } + // Now adjust the red/yellow/green zones: + double range = maxValue - minValue; - green->resetTransform(); - double greenScale = (greenMax-greenMin)/range; - double greenStart = verticalDial ? (maxValue-greenMax)/range*green->boundingRect().height() : - (greenMin-minValue)/range*green->boundingRect().width(); - QTransform matrix; - matrix.reset(); - if (verticalDial) { - matrix.scale(1,greenScale); - matrix.translate(bgX,(greenStart+bgY)/greenScale); - } else { - matrix.scale(greenScale,1); - matrix.translate((greenStart+bgX)/greenScale,bgY); - } - green->setTransform(matrix,false); + green->resetTransform(); + double greenScale = (greenMax - greenMin) / range; + double greenStart = verticalDial ? (maxValue - greenMax) / range * green->boundingRect().height() : + (greenMin - minValue) / range * green->boundingRect().width(); + QTransform matrix; + matrix.reset(); + if (verticalDial) { + matrix.scale(1, greenScale); + matrix.translate(bgX, (greenStart + bgY) / greenScale); + } else { + matrix.scale(greenScale, 1); + matrix.translate((greenStart + bgX) / greenScale, bgY); + } + green->setTransform(matrix, false); - yellow->resetTransform(); - double yellowScale = (yellowMax-yellowMin)/range; - double yellowStart = verticalDial ? (maxValue-yellowMax)/range*yellow->boundingRect().height() : - (yellowMin-minValue)/range*yellow->boundingRect().width(); - matrix.reset(); - if (verticalDial) { - matrix.scale(1,yellowScale); - matrix.translate(bgX,(yellowStart+bgY)/yellowScale); - } else { - matrix.scale(yellowScale,1); - matrix.translate((yellowStart+bgX)/yellowScale,bgY); - } - yellow->setTransform(matrix,false); + yellow->resetTransform(); + double yellowScale = (yellowMax - yellowMin) / range; + double yellowStart = verticalDial ? (maxValue - yellowMax) / range * yellow->boundingRect().height() : + (yellowMin - minValue) / range * yellow->boundingRect().width(); + matrix.reset(); + if (verticalDial) { + matrix.scale(1, yellowScale); + matrix.translate(bgX, (yellowStart + bgY) / yellowScale); + } else { + matrix.scale(yellowScale, 1); + matrix.translate((yellowStart + bgX) / yellowScale, bgY); + } + yellow->setTransform(matrix, false); - red->resetTransform(); - double redScale = (redMax-redMin)/range; - double redStart = verticalDial ? (maxValue-redMax)/range*red->boundingRect().height() : - (redMin-minValue)/range*red->boundingRect().width(); - matrix.reset(); - if (verticalDial) { - matrix.scale(1,redScale); - matrix.translate(bgX,(redStart+bgY)/redScale); - } else { - matrix.scale(redScale,1); - matrix.translate((redStart+bgX)/redScale,bgY); - } - red->setTransform(matrix,false); - - } else { - red = NULL; + red->resetTransform(); + double redScale = (redMax - redMin) / range; + double redStart = verticalDial ? (maxValue - redMax) / range * red->boundingRect().height() : + (redMin - minValue) / range * red->boundingRect().width(); + matrix.reset(); + if (verticalDial) { + matrix.scale(1, redScale); + matrix.translate(bgX, (redStart + bgY) / redScale); + } else { + matrix.scale(redScale, 1); + matrix.translate((redStart + bgX) / redScale, bgY); + } + red->setTransform(matrix, false); + } else { + red = NULL; yellow = NULL; - green = NULL; - } + green = NULL; + } - // Check whether the dial wants to display a moving index: - if (m_renderer->elementExists("needle")) { - QMatrix textMatrix = m_renderer->matrixForElement("needle"); - QRectF nRect = textMatrix.mapRect(m_renderer->boundsOnElement("needle")); - startX = nRect.x(); - startY = nRect.y(); - QTransform matrix; - matrix.translate(startX,startY); - index = new QGraphicsSvgItem(); - index->setSharedRenderer(m_renderer); - index->setElementId("needle"); - index->setTransform(matrix,false); - index->setParentItem(background); - } else { - index = NULL; - } + // Check whether the dial wants to display a moving index: + if (m_renderer->elementExists("needle")) { + QMatrix textMatrix = m_renderer->matrixForElement("needle"); + QRectF nRect = textMatrix.mapRect(m_renderer->boundsOnElement("needle")); + startX = nRect.x(); + startY = nRect.y(); + QTransform matrix; + matrix.translate(startX, startY); + index = new QGraphicsSvgItem(); + index->setSharedRenderer(m_renderer); + index->setElementId("needle"); + index->setTransform(matrix, false); + index->setParentItem(background); + } else { + index = NULL; + } - // Check whether the dial wants display its field name: - if (m_renderer->elementExists("field")) { - QMatrix textMatrix = m_renderer->matrixForElement("field"); - QRectF rect = textMatrix.mapRect(m_renderer->boundsOnElement("field")); - qreal startX = rect.x(); - qreal startY = rect.y(); - qreal elHeight = rect.height(); - QTransform matrix; - matrix.translate(startX,startY-elHeight/2); - fieldName = new QGraphicsTextItem("field"); - fieldName->setFont(QFont("Arial",(int)elHeight)); - fieldName->setDefaultTextColor(QColor("White")); - fieldName->setTransform(matrix,false); - fieldName->setParentItem(background); - } else { - fieldName = NULL; - } + // Check whether the dial wants display its field name: + if (m_renderer->elementExists("field")) { + QMatrix textMatrix = m_renderer->matrixForElement("field"); + QRectF rect = textMatrix.mapRect(m_renderer->boundsOnElement("field")); + qreal startX = rect.x(); + qreal startY = rect.y(); + qreal elHeight = rect.height(); + QTransform matrix; + matrix.translate(startX, startY - elHeight / 2); + fieldName = new QGraphicsTextItem("field"); + fieldName->setFont(QFont("Arial", (int)elHeight)); + fieldName->setDefaultTextColor(QColor("White")); + fieldName->setTransform(matrix, false); + fieldName->setParentItem(background); + } else { + fieldName = NULL; + } - // Check whether the dial wants display the numeric value: - if (m_renderer->elementExists("value")) { - QMatrix textMatrix = m_renderer->matrixForElement("value"); - QRectF nRect = textMatrix.mapRect(m_renderer->boundsOnElement("value")); - qreal startX = nRect.x(); - qreal startY = nRect.y(); - qreal elHeight = nRect.height(); - QTransform matrix; - matrix.translate(startX,startY-elHeight/2); - fieldValue = new QGraphicsTextItem("0.00"); - fieldValue->setFont(QFont("Arial",(int)elHeight)); - fieldValue->setDefaultTextColor(QColor("White")); - fieldValue->setTransform(matrix,false); - fieldValue->setParentItem(background); - } else { - fieldValue = NULL; - } + // Check whether the dial wants display the numeric value: + if (m_renderer->elementExists("value")) { + QMatrix textMatrix = m_renderer->matrixForElement("value"); + QRectF nRect = textMatrix.mapRect(m_renderer->boundsOnElement("value")); + qreal startX = nRect.x(); + qreal startY = nRect.y(); + qreal elHeight = nRect.height(); + QTransform matrix; + matrix.translate(startX, startY - elHeight / 2); + fieldValue = new QGraphicsTextItem("0.00"); + fieldValue->setFont(QFont("Arial", (int)elHeight)); + fieldValue->setDefaultTextColor(QColor("White")); + fieldValue->setTransform(matrix, false); + fieldValue->setParentItem(background); + } else { + fieldValue = NULL; + } - // Check whether the dial wants to display the value as a - // symbol (only works for text values): - if (m_renderer->elementExists("symbol")) { - QMatrix textMatrix = m_renderer->matrixForElement("symbol"); - qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement("symbol")).x(); - qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement("symbol")).y(); - QTransform matrix; - matrix.translate(startX,startY); - fieldSymbol = new QGraphicsSvgItem(); - fieldSymbol->setElementId("symbol"); - fieldSymbol->setSharedRenderer(m_renderer); - fieldSymbol->setTransform(matrix,false); - fieldSymbol->setParentItem(background); - } else { - fieldSymbol = NULL; - } + // Check whether the dial wants to display the value as a + // symbol (only works for text values): + if (m_renderer->elementExists("symbol")) { + QMatrix textMatrix = m_renderer->matrixForElement("symbol"); + qreal startX = textMatrix.mapRect(m_renderer->boundsOnElement("symbol")).x(); + qreal startY = textMatrix.mapRect(m_renderer->boundsOnElement("symbol")).y(); + QTransform matrix; + matrix.translate(startX, startY); + fieldSymbol = new QGraphicsSvgItem(); + fieldSymbol->setElementId("symbol"); + fieldSymbol->setSharedRenderer(m_renderer); + fieldSymbol->setTransform(matrix, false); + fieldSymbol->setParentItem(background); + } else { + fieldSymbol = NULL; + } - if (m_renderer->elementExists("foreground")) { + if (m_renderer->elementExists("foreground")) { foreground = new QGraphicsSvgItem(); foreground->setSharedRenderer(m_renderer); foreground->setElementId("foreground"); @@ -338,26 +340,25 @@ void LineardialGadgetWidget::setDialFile(QString dfn) fgenabled = false; } - l_scene->setSceneRect(background->boundingRect()); + l_scene->setSceneRect(background->boundingRect()); - // Reset the current index value: - indexValue = 0; - if (!dialTimer.isActive() && index) - dialTimer.start(); - } - else - { - qDebug() << "no file "; - m_renderer->load(QString(":/lineardial/images/empty.svg")); - l_scene->clear(); // This also deletes all items contained in the scene. - background = new QGraphicsSvgItem(); - background->setSharedRenderer(m_renderer); - l_scene->addItem(background); - fieldName = NULL; - fieldValue = NULL; - fieldSymbol = NULL; - index = NULL; - } + // Reset the current index value: + indexValue = 0; + if (!dialTimer.isActive() && index) { + dialTimer.start(); + } + } else { + qDebug() << "no file "; + m_renderer->load(QString(":/lineardial/images/empty.svg")); + l_scene->clear(); // This also deletes all items contained in the scene. + background = new QGraphicsSvgItem(); + background->setSharedRenderer(m_renderer); + l_scene->addItem(background); + fieldName = NULL; + fieldValue = NULL; + fieldSymbol = NULL; + index = NULL; + } } @@ -365,7 +366,8 @@ void LineardialGadgetWidget::setDialFont(QString fontProps) { // Note: a bit of juggling to preserve the automatic // font size which was calculated upon dial initialization. - QFont font = QFont("Arial",12); + QFont font = QFont("Arial", 12); + font.fromString(fontProps); if (fieldName) { int fieldSize = fieldName->font().pointSize(); @@ -373,10 +375,10 @@ void LineardialGadgetWidget::setDialFont(QString fontProps) fieldName->setFont(font); } if (fieldValue) { - int fieldSize = fieldValue->font().pointSize(); - font.setPointSize(fieldSize); - fieldValue->setFont(font); - } + int fieldSize = fieldValue->font().pointSize(); + font.setPointSize(fieldSize); + fieldValue->setFont(font); + } } @@ -388,11 +390,11 @@ void LineardialGadgetWidget::paint() void LineardialGadgetWidget::paintEvent(QPaintEvent *event) { // Skip painting until the dial file is loaded - if (! m_renderer->isValid()) { - qDebug()<<"Dial file not loaded, not rendering"; + if (!m_renderer->isValid()) { + qDebug() << "Dial file not loaded, not rendering"; return; } - QGraphicsView::paintEvent(event); + QGraphicsView::paintEvent(event); } // This event enables the dial to be dynamically resized @@ -401,16 +403,17 @@ void LineardialGadgetWidget::paintEvent(QPaintEvent *event) void LineardialGadgetWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - fitInView(background, Qt::KeepAspectRatio ); + fitInView(background, Qt::KeepAspectRatio); } // Converts the value into an percentage: // this enables smooth movement in moveIndex below -void LineardialGadgetWidget::setIndex(double value) { +void LineardialGadgetWidget::setIndex(double value) +{ if (verticalDial) { - indexTarget = 100*(maxValue-value)/(maxValue-minValue); + indexTarget = 100 * (maxValue - value) / (maxValue - minValue); } else { - indexTarget = 100*(value-minValue)/(maxValue-minValue); + indexTarget = 100 * (value - minValue) / (maxValue - minValue); } } @@ -423,21 +426,21 @@ void LineardialGadgetWidget::moveIndex() dialTimer.stop(); return; } - if ((abs((indexValue-indexTarget)*10) > 3)) { - indexValue += (indexTarget - indexValue)/5; + if ((abs((indexValue - indexTarget) * 10) > 3)) { + indexValue += (indexTarget - indexValue) / 5; } else { indexValue = indexTarget; dialTimer.stop(); } QTransform matrix; index->resetTransform(); - qreal trans = indexValue*bargraphSize/100; + qreal trans = indexValue * bargraphSize / 100; if (verticalDial) { - matrix.translate(startX,trans+startY); + matrix.translate(startX, trans + startY); } else { - matrix.translate(trans+startX,startY); + matrix.translate(trans + startX, startY); } - index->setTransform(matrix,false); - + index->setTransform(matrix, false); + update(); } diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.h b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.h index 9e8030e0d..12f476208 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -39,86 +39,102 @@ #include #include -class LineardialGadgetWidget : public QGraphicsView -{ +class LineardialGadgetWidget : public QGraphicsView { Q_OBJECT public: LineardialGadgetWidget(QWidget *parent = 0); - ~LineardialGadgetWidget(); - void enableOpenGL(bool flag); - void setDialFile(QString dfn); - void paint(); - void setRange(double min, double max) { minValue=min; maxValue=max;} - void setGreenRange(double min, double max) {greenMin=min; greenMax=max;} - void setYellowRange(double min, double max) {yellowMin=min; yellowMax=max;} - void setRedRange(double min, double max) {redMin=min; redMax=max;} - void connectInput(QString obj, QString field); - void setIndex(double val); - void setDialFont(QString fontProps); - void setFactor (double val) { factor = val;} - void setDecimalPlaces(int val) { places = val;} + ~LineardialGadgetWidget(); + void enableOpenGL(bool flag); + void setDialFile(QString dfn); + void paint(); + void setRange(double min, double max) + { + minValue = min; maxValue = max; + } + void setGreenRange(double min, double max) + { + greenMin = min; greenMax = max; + } + void setYellowRange(double min, double max) + { + yellowMin = min; yellowMax = max; + } + void setRedRange(double min, double max) + { + redMin = min; redMax = max; + } + void connectInput(QString obj, QString field); + void setIndex(double val); + void setDialFont(QString fontProps); + void setFactor(double val) + { + factor = val; + } + void setDecimalPlaces(int val) + { + places = val; + } public slots: void updateIndex(UAVObject *object1); protected: - void paintEvent(QPaintEvent *event); - void resizeEvent(QResizeEvent *event); + void paintEvent(QPaintEvent *event); + void resizeEvent(QResizeEvent *event); private: private slots: - void moveIndex(); + void moveIndex(); private: - QSvgRenderer *m_renderer; - QGraphicsSvgItem *background; - QGraphicsSvgItem *foreground; - QGraphicsSvgItem *index; - QGraphicsSvgItem *green; - QGraphicsSvgItem *yellow; - QGraphicsSvgItem *red; - QGraphicsSvgItem *fieldSymbol; + QSvgRenderer *m_renderer; + QGraphicsSvgItem *background; + QGraphicsSvgItem *foreground; + QGraphicsSvgItem *index; + QGraphicsSvgItem *green; + QGraphicsSvgItem *yellow; + QGraphicsSvgItem *red; + QGraphicsSvgItem *fieldSymbol; - QGraphicsTextItem *fieldName; - QGraphicsTextItem *fieldValue; + QGraphicsTextItem *fieldName; + QGraphicsTextItem *fieldValue; - // Simple flag to skip rendering if the - bool fgenabled; // layer does not exist. - bool verticalDial; // True if the dials scales vertically. + // Simple flag to skip rendering if the + bool fgenabled; // layer does not exist. + bool verticalDial; // True if the dials scales vertically. - qreal startX; // Where we should draw the bargraph - qreal startY; // green/yellow/red zones. - qreal bargraphSize; - qreal indexHeight; - qreal indexWidth; + qreal startX; // Where we should draw the bargraph + qreal startY; // green/yellow/red zones. + qreal bargraphSize; + qreal indexHeight; + qreal indexWidth; - double minValue; - double maxValue; - double greenMin; - double greenMax; - double yellowMin; - double yellowMax; - double redMin; - double redMax; - double factor; - int places; + double minValue; + double maxValue; + double greenMin; + double greenMax; + double yellowMin; + double yellowMax; + double redMin; + double redMax; + double factor; + int places; - // The Value and target variables - // are expressed in degrees - double indexTarget; - double indexValue; + // The Value and target variables + // are expressed in degrees + double indexTarget; + double indexValue; - // Rotation timer - QTimer dialTimer; - - // Name of the fields to read when an update is received: - UAVDataObject* obj1; - QString field1; - QString subfield1; - bool haveSubField1; + // Rotation timer + QTimer dialTimer; + // Name of the fields to read when an update is received: + UAVDataObject *obj1; + QString field1; + QString subfield1; + bool haveSubField1; }; #endif /* LINEARDIALGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.cpp b/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.cpp index 8441d9b99..451be06fc 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.cpp +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,32 +35,31 @@ LineardialPlugin::LineardialPlugin() { - // Do nothing + // Do nothing } LineardialPlugin::~LineardialPlugin() { - // Do nothing + // Do nothing } -bool LineardialPlugin::initialize(const QStringList& args, QString *errMsg) +bool LineardialPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new LineardialGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new LineardialGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void LineardialPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void LineardialPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(LineardialPlugin) - diff --git a/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.h b/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.h index 6ababda8e..820444c48 100644 --- a/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.h +++ b/ground/openpilotgcs/src/plugins/lineardial/lineardialplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup LinearDialPlugin Linear Dial Plugin * @{ - * @brief Impliments a gadget that displays linear gauges + * @brief Impliments a gadget that displays linear gauges *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class LineardialGadgetFactory; -class LineardialPlugin : public ExtensionSystem::IPlugin -{ +class LineardialPlugin : public ExtensionSystem::IPlugin { public: - LineardialPlugin(); - ~LineardialPlugin(); + LineardialPlugin(); + ~LineardialPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - LineardialGadgetFactory *mf; + LineardialGadgetFactory *mf; }; #endif /* LINEARDIALPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/logging/logfile.cpp b/ground/openpilotgcs/src/plugins/logging/logfile.cpp index 7a74d6367..ce0dd4146 100644 --- a/ground/openpilotgcs/src/plugins/logging/logfile.cpp +++ b/ground/openpilotgcs/src/plugins/logging/logfile.cpp @@ -13,8 +13,8 @@ LogFile::LogFile(QObject *parent) : * we want to save the logfile, we open in WriteOnly. In case we * want to read the logfile, we open in ReadOnly. */ -bool LogFile::open(OpenMode mode) { - +bool LogFile::open(OpenMode mode) +{ // start a timer for playback myTime.restart(); if (file.isOpen()) { @@ -25,8 +25,7 @@ bool LogFile::open(OpenMode mode) { return true; } - if(file.open(mode) == FALSE) - { + if (file.open(mode) == FALSE) { qDebug() << "Unable to open " << file.fileName() << " for logging"; return false; } @@ -46,33 +45,39 @@ void LogFile::close() { emit aboutToClose(); - if (timer.isActive()) + if (timer.isActive()) { timer.stop(); + } file.close(); QIODevice::close(); } -qint64 LogFile::writeData(const char * data, qint64 dataSize) { - if (!file.isWritable()) +qint64 LogFile::writeData(const char *data, qint64 dataSize) +{ + if (!file.isWritable()) { return dataSize; + } quint32 timeStamp = myTime.elapsed(); - file.write((char *) &timeStamp,sizeof(timeStamp)); - file.write((char *) &dataSize, sizeof(dataSize)); + file.write((char *)&timeStamp, sizeof(timeStamp)); + file.write((char *)&dataSize, sizeof(dataSize)); qint64 written = file.write(data, dataSize); - if(written != -1) + if (written != -1) { emit bytesWritten(written); + } return dataSize; } -qint64 LogFile::readData(char * data, qint64 maxSize) { +qint64 LogFile::readData(char *data, qint64 maxSize) +{ QMutexLocker locker(&mutex); - qint64 toRead = qMin(maxSize,(qint64)dataBuffer.size()); - memcpy(data,dataBuffer.data(),toRead); - dataBuffer.remove(0,toRead); + qint64 toRead = qMin(maxSize, (qint64)dataBuffer.size()); + + memcpy(data, dataBuffer.data(), toRead); + dataBuffer.remove(0, toRead); return toRead; } @@ -85,28 +90,26 @@ void LogFile::timerFired() { qint64 dataSize; - if(file.bytesAvailable() > 4) - { - - int time; - time = myTime.elapsed(); + if (file.bytesAvailable() > 4) { + int time; + time = myTime.elapsed(); // TODO: going back in time will be a problem - while ((lastPlayed + ((time - timeOffset)* playbackSpeed) > lastTimeStamp)) { - lastPlayed += ((time - timeOffset)* playbackSpeed); - if(file.bytesAvailable() < 4) { + while ((lastPlayed + ((time - timeOffset) * playbackSpeed) > lastTimeStamp)) { + lastPlayed += ((time - timeOffset) * playbackSpeed); + if (file.bytesAvailable() < 4) { stopReplay(); return; } - file.read((char *) &dataSize, sizeof(dataSize)); + file.read((char *)&dataSize, sizeof(dataSize)); - if (dataSize<1 || dataSize>(1024*1024)) { - qDebug() << "Error: Logfile corrupted! Unlikely packet size: " << dataSize << "\n"; - stopReplay(); - return; - } - if(file.bytesAvailable() < dataSize) { + if (dataSize < 1 || dataSize > (1024 * 1024)) { + qDebug() << "Error: Logfile corrupted! Unlikely packet size: " << dataSize << "\n"; + stopReplay(); + return; + } + if (file.bytesAvailable() < dataSize) { stopReplay(); return; } @@ -116,45 +119,45 @@ void LogFile::timerFired() mutex.unlock(); emit readyRead(); - if(file.bytesAvailable() < 4) { + if (file.bytesAvailable() < 4) { stopReplay(); return; } - int save=lastTimeStamp; - file.read((char *) &lastTimeStamp,sizeof(lastTimeStamp)); - // some validity checks - if (lastTimeStamp (60*60*1000)) { // gap of more than 60 minutes) - qDebug() << "Error: Logfile corrupted! Unlikely timestamp " << lastTimeStamp << " after "<< save << "\n"; - stopReplay(); - return; + int save = lastTimeStamp; + file.read((char *)&lastTimeStamp, sizeof(lastTimeStamp)); + // some validity checks + if (lastTimeStamp < save // logfile goies back in time + || (lastTimeStamp - save) > (60 * 60 * 1000)) { // gap of more than 60 minutes) + qDebug() << "Error: Logfile corrupted! Unlikely timestamp " << lastTimeStamp << " after " << save << "\n"; + stopReplay(); + return; } timeOffset = time; time = myTime.elapsed(); - } } else { stopReplay(); } - } -bool LogFile::startReplay() { +bool LogFile::startReplay() +{ dataBuffer.clear(); myTime.restart(); - timeOffset = 0; - lastPlayed = 0; + timeOffset = 0; + lastPlayed = 0; playbackSpeed = 1; - file.read((char *) &lastTimeStamp,sizeof(lastTimeStamp)); + file.read((char *)&lastTimeStamp, sizeof(lastTimeStamp)); timer.setInterval(10); timer.start(); emit replayStarted(); return true; } -bool LogFile::stopReplay() { +bool LogFile::stopReplay() +{ close(); emit replayFinished(); return true; @@ -170,4 +173,3 @@ void LogFile::resumeReplay() timeOffset = myTime.elapsed(); timer.start(); } - diff --git a/ground/openpilotgcs/src/plugins/logging/logfile.h b/ground/openpilotgcs/src/plugins/logging/logfile.h index a07e1832a..9bd588fd5 100644 --- a/ground/openpilotgcs/src/plugins/logging/logfile.h +++ b/ground/openpilotgcs/src/plugins/logging/logfile.h @@ -10,24 +10,32 @@ #include "uavobjectmanager.h" #include -class LogFile : public QIODevice -{ +class LogFile : public QIODevice { Q_OBJECT public: explicit LogFile(QObject *parent = 0); qint64 bytesAvailable() const; - qint64 bytesToWrite() { return file.bytesToWrite(); }; + qint64 bytesToWrite() + { + return file.bytesToWrite(); + }; bool open(OpenMode mode); - void setFileName(QString name) { file.setFileName(name); }; + void setFileName(QString name) + { + file.setFileName(name); + }; void close(); - qint64 writeData(const char * data, qint64 dataSize); - qint64 readData(char * data, qint64 maxlen); + qint64 writeData(const char *data, qint64 dataSize); + qint64 readData(char *data, qint64 maxlen); bool startReplay(); bool stopReplay(); public slots: - void setReplaySpeed(double val) { playbackSpeed = val; qDebug() << playbackSpeed; }; + void setReplaySpeed(double val) + { + playbackSpeed = val; qDebug() << playbackSpeed; + }; void pauseReplay(); void resumeReplay(); diff --git a/ground/openpilotgcs/src/plugins/logging/logginggadget.cpp b/ground/openpilotgcs/src/plugins/logging/logginggadget.cpp index c067fc52d..a4fbc8d7d 100644 --- a/ground/openpilotgcs/src/plugins/logging/logginggadget.cpp +++ b/ground/openpilotgcs/src/plugins/logging/logginggadget.cpp @@ -32,16 +32,14 @@ #include "uavobject.h" LoggingGadget::LoggingGadget(QString classId, LoggingGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} LoggingGadget::~LoggingGadget() { delete m_widget; } -void LoggingGadget::loadConfiguration(IUAVGadgetConfiguration* config) -{ -} +void LoggingGadget::loadConfiguration(IUAVGadgetConfiguration *config) +{} diff --git a/ground/openpilotgcs/src/plugins/logging/logginggadget.h b/ground/openpilotgcs/src/plugins/logging/logginggadget.h index 2805c7afa..a5dcc8ca9 100644 --- a/ground/openpilotgcs/src/plugins/logging/logginggadget.h +++ b/ground/openpilotgcs/src/plugins/logging/logginggadget.h @@ -33,27 +33,35 @@ namespace Core { class IUAVGadget; } -//class QWidget; -//class QString; +// class QWidget; +// class QString; class LoggingGadgetWidget; using namespace Core; -class LoggingGadget : public Core::IUAVGadget -{ +class LoggingGadget : public Core::IUAVGadget { Q_OBJECT public: LoggingGadget(QString classId, LoggingGadgetWidget *widget, QWidget *parent = 0); ~LoggingGadget(); - QList context() const { return m_context; } - QWidget *widget() { return m_widget; } - QString contextHelpId() const { return QString(); } + QList context() const + { + return m_context; + } + QWidget *widget() + { + return m_widget; + } + QString contextHelpId() const + { + return QString(); + } - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); private: - QWidget *m_widget; - QList m_context; + QWidget *m_widget; + QList m_context; }; diff --git a/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.cpp b/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.cpp index 5874a8cb1..37501187f 100644 --- a/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.cpp @@ -30,20 +30,20 @@ #include LoggingGadgetFactory::LoggingGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("LoggingGadget"), - tr("Logging"), - parent) + IUAVGadgetFactory(QString("LoggingGadget"), + tr("Logging"), + parent) { - loggingPlugin = (LoggingPlugin *) parent; + loggingPlugin = (LoggingPlugin *)parent; } LoggingGadgetFactory::~LoggingGadgetFactory() +{} + +IUAVGadget *LoggingGadgetFactory::createGadget(QWidget *parent) { + LoggingGadgetWidget *gadgetWidget = new LoggingGadgetWidget(parent); -} - -IUAVGadget* LoggingGadgetFactory::createGadget(QWidget *parent) { - LoggingGadgetWidget* gadgetWidget = new LoggingGadgetWidget(parent); gadgetWidget->setPlugin(loggingPlugin); return new LoggingGadget(QString("LoggingGadget"), gadgetWidget, parent); } diff --git a/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.h b/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.h index 05c020f17..15129025d 100644 --- a/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/logging/logginggadgetfactory.h @@ -38,18 +38,20 @@ class IUAVGadgetFactory; using namespace Core; class LoggingPlugin; -class LoggingGadgetFactory : public IUAVGadgetFactory -{ +class LoggingGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: LoggingGadgetFactory(QObject *parent = 0); ~LoggingGadgetFactory(); - void setPlugin(LoggingPlugin * p) { loggingPlugin = p; }; + void setPlugin(LoggingPlugin *p) + { + loggingPlugin = p; + }; IUAVGadget *createGadget(QWidget *parent); private: - LoggingPlugin * loggingPlugin; + LoggingPlugin *loggingPlugin; }; #endif // LoggingGADGETFACTORY_H_ diff --git a/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.cpp b/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.cpp index f40213148..d5788322f 100644 --- a/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.cpp @@ -42,23 +42,22 @@ LoggingGadgetWidget::LoggingGadgetWidget(QWidget *parent) : QLabel(parent) ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); scpPlugin = pm->getObject(); - } LoggingGadgetWidget::~LoggingGadgetWidget() { - // Do nothing + // Do nothing } -void LoggingGadgetWidget::setPlugin(LoggingPlugin * p) +void LoggingGadgetWidget::setPlugin(LoggingPlugin *p) { loggingPlugin = p; - connect(p,SIGNAL(stateChanged(QString)),this,SLOT(stateChanged(QString))); - connect(m_logging->playButton,SIGNAL(clicked()),p->getLogfile(),SLOT(resumeReplay())); + connect(p, SIGNAL(stateChanged(QString)), this, SLOT(stateChanged(QString))); + connect(m_logging->playButton, SIGNAL(clicked()), p->getLogfile(), SLOT(resumeReplay())); connect(m_logging->playButton, SIGNAL(clicked()), scpPlugin, SLOT(startPlotting())); - connect(m_logging->pauseButton,SIGNAL(clicked()),p->getLogfile(),SLOT(pauseReplay())); + connect(m_logging->pauseButton, SIGNAL(clicked()), p->getLogfile(), SLOT(pauseReplay())); connect(m_logging->pauseButton, SIGNAL(clicked()), scpPlugin, SLOT(stopPlotting())); - connect(m_logging->playbackSpeed,SIGNAL(valueChanged(double)),p->getLogfile(),SLOT(setReplaySpeed(double))); + connect(m_logging->playbackSpeed, SIGNAL(valueChanged(double)), p->getLogfile(), SLOT(setReplaySpeed(double))); void pauseReplay(); void resumeReplay(); } @@ -70,6 +69,6 @@ void LoggingGadgetWidget::stateChanged(QString status) } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.h b/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.h index bee7028f3..a2a166b3c 100644 --- a/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/logging/logginggadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GCSControlGadgetPlugin GCSControl Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,14 +37,13 @@ class Ui_Logging; class LoggingPlugin; -class LoggingGadgetWidget : public QLabel -{ +class LoggingGadgetWidget : public QLabel { Q_OBJECT public: LoggingGadgetWidget(QWidget *parent = 0); ~LoggingGadgetWidget(); - void setPlugin(LoggingPlugin * p); + void setPlugin(LoggingPlugin *p); protected slots: void stateChanged(QString status); @@ -55,10 +54,8 @@ signals: private: Ui_Logging *m_logging; - LoggingPlugin * loggingPlugin; - ScopeGadgetFactory * scpPlugin; - - + LoggingPlugin *loggingPlugin; + ScopeGadgetFactory *scpPlugin; }; #endif /* LoggingGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp b/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp index 6333c4e98..cbf2dfe9d 100644 --- a/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp +++ b/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp @@ -45,35 +45,32 @@ LoggingConnection::LoggingConnection() -{ - -} +{} LoggingConnection::~LoggingConnection() -{ -} +{} void LoggingConnection::onEnumerationChanged() { - emit availableDevChanged(this); + emit availableDevChanged(this); } QList LoggingConnection::availableDevices() { QList list; device d; - d.displayName="Logfile replay..."; - d.name="Logfile replay..."; - list <getObject(); uavTalk = new UAVTalk(&logFile, objManager); - connect(parent,SIGNAL(stopLoggingSignal()),this,SLOT(stopLogging())); + connect(parent, SIGNAL(stopLoggingSignal()), this, SLOT(stopLogging())); return true; }; /** - * Logs an object update to the file. Data format is the - * timestamp as a 32 bit uint counting ms from start of - * file writing (flight time will be embedded in stream), - * then object packet size, then the packed UAVObject. - */ -void LoggingThread::objectUpdated(UAVObject * obj) + * Logs an object update to the file. Data format is the + * timestamp as a 32 bit uint counting ms from start of + * file writing (flight time will be embedded in stream), + * then object packet size, then the packed UAVObject. + */ +void LoggingThread::objectUpdated(UAVObject *obj) { QWriteLocker locker(&lock); - if(!uavTalk->sendObject(obj,false,false) ) + + if (!uavTalk->sendObject(obj, false, false)) { qDebug() << "Error logging " << obj->getName(); + } }; /** - * Connect signals from all the objects updates to the write routine then - * run event loop - */ + * Connect signals from all the objects updates to the write routine then + * run event loop + */ void LoggingThread::run() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - QList< QList > list; + QList< QList > list; list = objManager->getObjects(); - QList< QList >::const_iterator i; - QList::const_iterator j; + QList< QList >::const_iterator i; + QList::const_iterator j; int objects = 0; - for (i = list.constBegin(); i != list.constEnd(); ++i) - { - for (j = (*i).constBegin(); j != (*i).constEnd(); ++j) - { - connect(*j, SIGNAL(objectUpdated(UAVObject*)), (LoggingThread*) this, SLOT(objectUpdated(UAVObject*))); + for (i = list.constBegin(); i != list.constEnd(); ++i) { + for (j = (*i).constBegin(); j != (*i).constEnd(); ++j) { + connect(*j, SIGNAL(objectUpdated(UAVObject *)), (LoggingThread *)this, SLOT(objectUpdated(UAVObject *))); objects++; - //qDebug() << "Detected " << j[0]; + // qDebug() << "Detected " << j[0]; } } - GCSTelemetryStats* gcsStatsObj = GCSTelemetryStats::GetInstance(objManager); + GCSTelemetryStats *gcsStatsObj = GCSTelemetryStats::GetInstance(objManager); GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); - if ( gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED ) - { + if (gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED) { qDebug() << "Logging: connected already, ask for all settings"; retrieveSettings(); } else { @@ -193,8 +185,8 @@ void LoggingThread::run() /** - * Pass this command to the correct thread then close the file - */ + * Pass this command to the correct thread then close the file + */ void LoggingThread::stopLogging() { QWriteLocker locker(&lock); @@ -203,16 +195,14 @@ void LoggingThread::stopLogging() ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - QList< QList > list; + QList< QList > list; list = objManager->getObjects(); - QList< QList >::const_iterator i; - QList::const_iterator j; + QList< QList >::const_iterator i; + QList::const_iterator j; - for (i = list.constBegin(); i != list.constEnd(); ++i) - { - for (j = (*i).constBegin(); j != (*i).constEnd(); ++j) - { - disconnect(*j, SIGNAL(objectUpdated(UAVObject*)), (LoggingThread*) this, SLOT(objectUpdated(UAVObject*))); + for (i = list.constBegin(); i != list.constEnd(); ++i) { + for (j = (*i).constBegin(); j != (*i).constEnd(); ++j) { + disconnect(*j, SIGNAL(objectUpdated(UAVObject *)), (LoggingThread *)this, SLOT(objectUpdated(UAVObject *))); } } @@ -230,20 +220,18 @@ void LoggingThread::retrieveSettings() queue.clear(); // Get all objects, add metaobjects, settings and data objects with OnChange update mode to the queue // Get UAVObjectManager instance - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objMngr = pm->getObject(); - QList< QList > objs = objMngr->getDataObjects(); - for (int n = 0; n < objs.length(); ++n) - { - UAVDataObject* obj = objs[n][0]; - if ( obj->isSettings() ) - { - queue.enqueue(obj); - } + QList< QList > objs = objMngr->getDataObjects(); + for (int n = 0; n < objs.length(); ++n) { + UAVDataObject *obj = objs[n][0]; + if (obj->isSettings()) { + queue.enqueue(obj); } + } // Start retrieving qDebug() << tr("Logging: retrieve settings objects from the autopilot (%1 objects)") - .arg( queue.length()); + .arg(queue.length()); retrieveNextObject(); } @@ -254,15 +242,14 @@ void LoggingThread::retrieveSettings() void LoggingThread::retrieveNextObject() { // If queue is empty return - if ( queue.isEmpty() ) - { + if (queue.isEmpty()) { qDebug() << "Logging: Object retrieval completed"; return; } // Get next object from the queue - UAVObject* obj = queue.dequeue(); + UAVObject *obj = queue.dequeue(); // Connect to object - connect(obj, SIGNAL(transactionCompleted(UAVObject*,bool)), this, SLOT(transactionCompleted(UAVObject*,bool))); + connect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool))); // Request update obj->requestUpdate(); } @@ -270,30 +257,26 @@ void LoggingThread::retrieveNextObject() /** * Called by the retrieved object when a transaction is completed. */ -void LoggingThread::transactionCompleted(UAVObject* obj, bool success) +void LoggingThread::transactionCompleted(UAVObject *obj, bool success) { Q_UNUSED(success); // Disconnect from sending object obj->disconnect(this); // Process next object if telemetry is still available // Get stats objects - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - GCSTelemetryStats* gcsStatsObj = GCSTelemetryStats::GetInstance(objManager); + GCSTelemetryStats *gcsStatsObj = GCSTelemetryStats::GetInstance(objManager); GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); - if ( gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED ) - { + if (gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED) { retrieveNextObject(); - } - else - { + } else { qDebug() << "Logging: Object retrieval has been cancelled"; queue.clear(); } } - /**************************************************************** Logging plugin ********************************/ @@ -306,17 +289,18 @@ LoggingPlugin::LoggingPlugin() : state(IDLE) LoggingPlugin::~LoggingPlugin() { - if (loggingThread) + if (loggingThread) { delete loggingThread; + } // Don't delete it, the plugin manager will do it: - //delete logConnection; + // delete logConnection; } /** - * Add Logging plugin to File menu - */ -bool LoggingPlugin::initialize(const QStringList& args, QString *errMsg) + * Add Logging plugin to File menu + */ +bool LoggingPlugin::initialize(const QStringList & args, QString *errMsg) { Q_UNUSED(args); Q_UNUSED(errMsg); @@ -325,14 +309,14 @@ bool LoggingPlugin::initialize(const QStringList& args, QString *errMsg) // Add Menu entry - Core::ActionManager* am = Core::ICore::instance()->actionManager(); - Core::ActionContainer* ac = am->actionContainer(Core::Constants::M_TOOLS); + Core::ActionManager *am = Core::ICore::instance()->actionManager(); + Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_TOOLS); // Command to start logging cmd = am->registerAction(new QAction(this), - "LoggingPlugin.Logging", - QList() << - Core::Constants::C_GLOBAL_ID); + "LoggingPlugin.Logging", + QList() << + Core::Constants::C_GLOBAL_ID); cmd->setDefaultKeySequence(QKeySequence("Ctrl+L")); cmd->action()->setText("Start logging..."); @@ -347,33 +331,29 @@ bool LoggingPlugin::initialize(const QStringList& args, QString *errMsg) addAutoReleasedObject(mf); // Map signal from end of replay to replay stopped - connect(getLogfile(),SIGNAL(replayFinished()), this, SLOT(replayStopped())); - connect(getLogfile(),SIGNAL(replayStarted()), this, SLOT(replayStarted())); + connect(getLogfile(), SIGNAL(replayFinished()), this, SLOT(replayStopped())); + connect(getLogfile(), SIGNAL(replayStarted()), this, SLOT(replayStarted())); return true; } /** - * The action that is triggered by the menu item which opens the - * file and begins logging if successful - */ + * The action that is triggered by the menu item which opens the + * file and begins logging if successful + */ void LoggingPlugin::toggleLogging() { - if(state == IDLE) - { - + if (state == IDLE) { QString fileName = QFileDialog::getSaveFileName(NULL, tr("Start Log"), - tr("OP-%0.opl").arg(QDateTime::currentDateTime().toString("yyyy-MM-dd_hh-mm-ss")), - tr("OpenPilot Log (*.opl)")); - if (fileName.isEmpty()) + tr("OP-%0.opl").arg(QDateTime::currentDateTime().toString("yyyy-MM-dd_hh-mm-ss")), + tr("OpenPilot Log (*.opl)")); + if (fileName.isEmpty()) { return; + } startLogging(fileName); cmd->action()->setText(tr("Stop logging")); - - } - else if(state == LOGGING) - { + } else if (state == LOGGING) { stopLogging(); cmd->action()->setText(tr("Start logging...")); } @@ -381,18 +361,18 @@ void LoggingPlugin::toggleLogging() /** - * Starts the logging thread to a certain file - */ + * Starts the logging thread to a certain file + */ void LoggingPlugin::startLogging(QString file) { qDebug() << "Logging to " << file; // We have to delete the previous logging thread if is was still there! - if (loggingThread) + if (loggingThread) { delete loggingThread; + } loggingThread = new LoggingThread(); - if(loggingThread->openFile(file,this)) - { - connect(loggingThread,SIGNAL(finished()),this,SLOT(loggingStopped())); + if (loggingThread->openFile(file, this)) { + connect(loggingThread, SIGNAL(finished()), this, SLOT(loggingStopped())); state = LOGGING; loggingThread->start(); emit stateChanged("LOGGING"); @@ -404,24 +384,25 @@ void LoggingPlugin::startLogging(QString file) } /** - * Send the stop logging signal to the LoggingThread - */ + * Send the stop logging signal to the LoggingThread + */ void LoggingPlugin::stopLogging() { emit stopLoggingSignal(); - disconnect( this,SIGNAL(stopLoggingSignal()),0,0); + disconnect(this, SIGNAL(stopLoggingSignal()), 0, 0); } /** - * Receive the logging stopped signal from the LoggingThread - * and change status to not logging - */ + * Receive the logging stopped signal from the LoggingThread + * and change status to not logging + */ void LoggingPlugin::loggingStopped() { - if(state == LOGGING) + if (state == LOGGING) { state = IDLE; + } emit stateChanged("IDLE"); @@ -430,8 +411,8 @@ void LoggingPlugin::loggingStopped() } /** - * Received the replay stopped signal from the LogFile - */ + * Received the replay stopped signal from the LogFile + */ void LoggingPlugin::replayStopped() { state = IDLE; @@ -439,8 +420,8 @@ void LoggingPlugin::replayStopped() } /** - * Received the replay started signal from the LogFile - */ + * Received the replay started signal from the LogFile + */ void LoggingPlugin::replayStarted() { state = REPLAY; @@ -448,8 +429,6 @@ void LoggingPlugin::replayStarted() } - - void LoggingPlugin::extensionsInitialized() { addAutoReleasedObject(logConnection); diff --git a/ground/openpilotgcs/src/plugins/logging/loggingplugin.h b/ground/openpilotgcs/src/plugins/logging/loggingplugin.h index 1ecf35b9b..64abda4fc 100644 --- a/ground/openpilotgcs/src/plugins/logging/loggingplugin.h +++ b/ground/openpilotgcs/src/plugins/logging/loggingplugin.h @@ -45,13 +45,12 @@ class LoggingPlugin; class LoggingGadgetFactory; /** -* Define a connection via the IConnection interface -* Plugin will add a instance of this class to the pool, -* so the connection manager can use it. -*/ + * Define a connection via the IConnection interface + * Plugin will add a instance of this class to the pool, + * so the connection manager can use it. + */ class LoggingConnection - : public Core::IConnection -{ + : public Core::IConnection { Q_OBJECT public: LoggingConnection(); @@ -64,8 +63,14 @@ public: virtual QString connectionName(); virtual QString shortName(); - bool deviceOpened() {return m_deviceOpened;} - LogFile* getLogfile() { return &logFile;} + bool deviceOpened() + { + return m_deviceOpened; + } + LogFile *getLogfile() + { + return &logFile; + } private: @@ -81,16 +86,14 @@ protected: }; - -class LoggingThread : public QThread -{ -Q_OBJECT +class LoggingThread : public QThread { + Q_OBJECT public: - bool openFile(QString file, LoggingPlugin * parent); + bool openFile(QString file, LoggingPlugin *parent); private slots: - void objectUpdated(UAVObject * obj); - void transactionCompleted(UAVObject* obj, bool success); + void objectUpdated(UAVObject *obj); + void transactionCompleted(UAVObject *obj, bool success); public slots: void stopLogging(); @@ -99,18 +102,16 @@ protected: void run(); QReadWriteLock lock; LogFile logFile; - UAVTalk * uavTalk; + UAVTalk *uavTalk; private: - QQueue queue; + QQueue queue; void retrieveSettings(); void retrieveNextObject(); - }; -class LoggingPlugin : public ExtensionSystem::IPlugin -{ +class LoggingPlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -118,11 +119,17 @@ public: ~LoggingPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); - LoggingConnection* getLogConnection() { return logConnection; }; - LogFile* getLogfile() { return logConnection->getLogfile();} + LoggingConnection *getLogConnection() + { + return logConnection; + }; + LogFile *getLogfile() + { + return logConnection->getLogfile(); + } void setLogMenuTitle(QString str); @@ -133,11 +140,11 @@ signals: protected: - enum {IDLE, LOGGING, REPLAY} state; - LoggingThread * loggingThread; + enum { IDLE, LOGGING, REPLAY } state; + LoggingThread *loggingThread; // These are used for replay, logging in its own thread - LoggingConnection* logConnection; + LoggingConnection *logConnection; private slots: void toggleLogging(); @@ -149,8 +156,7 @@ private slots: private: LoggingGadgetFactory *mf; - Core::Command* cmd; - + Core::Command *cmd; }; #endif /* LoggingPLUGIN_H_ */ /** diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.cpp b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.cpp index f1c330fec..f4c6f0f84 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.cpp +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.cpp @@ -33,17 +33,16 @@ #include MagicWaypointGadget::MagicWaypointGadget(QString classId, MagicWaypointGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} MagicWaypointGadget::~MagicWaypointGadget() { delete m_widget; } -void MagicWaypointGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void MagicWaypointGadget::loadConfiguration(IUAVGadgetConfiguration *config) { Q_UNUSED(config); } diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.h b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.h index 16f432d3b..120b0c2ac 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.h +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadget.h @@ -33,27 +33,35 @@ namespace Core { class IUAVGadget; } -//class QWidget; -//class QString; +// class QWidget; +// class QString; class MagicWaypointGadgetWidget; using namespace Core; -class MagicWaypointGadget : public Core::IUAVGadget -{ +class MagicWaypointGadget : public Core::IUAVGadget { Q_OBJECT public: MagicWaypointGadget(QString classId, MagicWaypointGadgetWidget *widget, QWidget *parent = 0); ~MagicWaypointGadget(); - QList context() const { return m_context; } - QWidget *widget() { return m_widget; } - QString contextHelpId() const { return QString(); } + QList context() const + { + return m_context; + } + QWidget *widget() + { + return m_widget; + } + QString contextHelpId() const + { + return QString(); + } - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); private: - QWidget *m_widget; - QList m_context; + QWidget *m_widget; + QList m_context; }; diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.cpp index fc0e52362..b69ca17f0 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.cpp @@ -31,18 +31,17 @@ #include MagicWaypointGadgetFactory::MagicWaypointGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("MagicWaypointGadget"), - tr("Magic Waypoint"), - parent) -{ -} + IUAVGadgetFactory(QString("MagicWaypointGadget"), + tr("Magic Waypoint"), + parent) +{} MagicWaypointGadgetFactory::~MagicWaypointGadgetFactory() +{} + +IUAVGadget *MagicWaypointGadgetFactory::createGadget(QWidget *parent) { + MagicWaypointGadgetWidget *gadgetWidget = new MagicWaypointGadgetWidget(parent); -} - -IUAVGadget* MagicWaypointGadgetFactory::createGadget(QWidget *parent) { - MagicWaypointGadgetWidget* gadgetWidget = new MagicWaypointGadgetWidget(parent); return new MagicWaypointGadget(QString("MagicWaypointGadget"), gadgetWidget, parent); } diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.h b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.h index 92d65edd1..bc542b887 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetfactory.h @@ -37,8 +37,7 @@ class IUAVGadgetFactory; using namespace Core; -class MagicWaypointGadgetFactory : public IUAVGadgetFactory -{ +class MagicWaypointGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: MagicWaypointGadgetFactory(QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp index cc2e99418..bd7d9e585 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp @@ -47,13 +47,13 @@ MagicWaypointGadgetWidget::MagicWaypointGadgetWidget(QWidget *parent) : QLabel(p m_magicwaypoint->setupUi(this); // Connect object updated event from UAVObject to also update check boxes - connect(getPathDesired(), SIGNAL(objectUpdated(UAVObject*)), this, SLOT(positionObjectChanged(UAVObject*))); - connect(getPositionActual(), SIGNAL(objectUpdated(UAVObject*)), this, SLOT(positionObjectChanged(UAVObject*))); + connect(getPathDesired(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(positionObjectChanged(UAVObject *))); + connect(getPositionActual(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(positionObjectChanged(UAVObject *))); // Connect updates from the position widget to this widget - connect(m_magicwaypoint->widgetPosition, SIGNAL(positionClicked(double,double)), this, SLOT(positionSelected(double,double))); - connect(this, SIGNAL(positionActualObjectChanged(double,double)), m_magicwaypoint->widgetPosition, SLOT(updateActualIndicator(double,double))); - connect(this, SIGNAL(positionDesiredObjectChanged(double,double)), m_magicwaypoint->widgetPosition, SLOT(updateDesiredIndicator(double,double))); + connect(m_magicwaypoint->widgetPosition, SIGNAL(positionClicked(double, double)), this, SLOT(positionSelected(double, double))); + connect(this, SIGNAL(positionActualObjectChanged(double, double)), m_magicwaypoint->widgetPosition, SLOT(updateActualIndicator(double, double))); + connect(this, SIGNAL(positionDesiredObjectChanged(double, double)), m_magicwaypoint->widgetPosition, SLOT(updateDesiredIndicator(double, double))); // Catch changes in scale for visualization connect(m_magicwaypoint->horizontalSliderScale, SIGNAL(valueChanged(int)), this, SLOT(scaleChanged(int))); @@ -64,45 +64,48 @@ MagicWaypointGadgetWidget::MagicWaypointGadgetWidget(QWidget *parent) : QLabel(p MagicWaypointGadgetWidget::~MagicWaypointGadgetWidget() { - // Do nothing + // Do nothing } /*! - \brief Returns the @ref PositionDesired UAVObject - */ -PathDesired* MagicWaypointGadgetWidget::getPathDesired() + \brief Returns the @ref PositionDesired UAVObject + */ +PathDesired *MagicWaypointGadgetWidget::getPathDesired() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - PathDesired* obj = PathDesired::GetInstance(objManager); + PathDesired *obj = PathDesired::GetInstance(objManager); + Q_ASSERT(obj != NULL); // Save crashes later return obj; } /*! - \brief Returns the @ref PositionActual UAVObject - */ -PositionActual* MagicWaypointGadgetWidget::getPositionActual() + \brief Returns the @ref PositionActual UAVObject + */ +PositionActual *MagicWaypointGadgetWidget::getPositionActual() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); PositionActual *obj = PositionActual::GetInstance(objManager); + Q_ASSERT(obj != NULL); return obj; } /** - * Detect changes in scale and update visualization (does not change position) - */ -void MagicWaypointGadgetWidget::scaleChanged(int scale) { + * Detect changes in scale and update visualization (does not change position) + */ +void MagicWaypointGadgetWidget::scaleChanged(int scale) +{ Q_UNUSED(scale); pathDesiredChanged(getPathDesired()); positionActualChanged(getPositionActual()); } /** - * Emit a position changed signal when @ref PositionActual object is changed - */ + * Emit a position changed signal when @ref PositionActual object is changed + */ void MagicWaypointGadgetWidget::positionActualChanged(UAVObject *) { PositionActual::DataFields positionActual = getPositionActual()->getData(); @@ -113,8 +116,8 @@ void MagicWaypointGadgetWidget::positionActualChanged(UAVObject *) } /** - * Emit a position changed signal when @ref PathDesired is changed - */ + * Emit a position changed signal when @ref PathDesired is changed + */ void MagicWaypointGadgetWidget::pathDesiredChanged(UAVObject *) { PathDesired::DataFields pathDesired = getPathDesired()->getData(); @@ -125,19 +128,21 @@ void MagicWaypointGadgetWidget::pathDesiredChanged(UAVObject *) } /** - * Slot called by visualization when a new @ref PathDesired is requested - */ -void MagicWaypointGadgetWidget::positionSelected(double north, double east) { + * Slot called by visualization when a new @ref PathDesired is requested + */ +void MagicWaypointGadgetWidget::positionSelected(double north, double east) +{ double scale = m_magicwaypoint->horizontalSliderScale->value(); PathDesired::DataFields pathDesired = getPathDesired()->getData(); + pathDesired.End[PathDesired::END_NORTH] = north * scale; - pathDesired.End[PathDesired::END_EAST] = east * scale; + pathDesired.End[PathDesired::END_EAST] = east * scale; pathDesired.Mode = PathDesired::MODE_FLYENDPOINT; getPathDesired()->setData(pathDesired); } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h index 59d55197b..49e8f1251 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GCSControlGadgetPlugin GCSControl Gadget Plugin * @{ - * @brief A place holder gadget plugin + * @brief A place holder gadget plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,8 +34,7 @@ class Ui_MagicWaypoint; -class MagicWaypointGadgetWidget : public QLabel -{ +class MagicWaypointGadgetWidget : public QLabel { Q_OBJECT public: @@ -53,9 +52,9 @@ protected slots: void positionSelected(double north, double east); private: - PathDesired * getPathDesired(); - PositionActual * getPositionActual(); - Ui_MagicWaypoint * m_magicwaypoint; + PathDesired *getPathDesired(); + PositionActual *getPositionActual(); + Ui_MagicWaypoint *m_magicwaypoint; }; #endif /* MagicWaypointGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.cpp b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.cpp index 2dd8025e0..06ffe24dd 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.cpp +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.cpp @@ -34,36 +34,36 @@ MagicWaypointPlugin::MagicWaypointPlugin() { - // Do nothing + // Do nothing } MagicWaypointPlugin::~MagicWaypointPlugin() { - // Do nothing + // Do nothing } -bool MagicWaypointPlugin::initialize(const QStringList& args, QString *errMsg) +bool MagicWaypointPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new MagicWaypointGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new MagicWaypointGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void MagicWaypointPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void MagicWaypointPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(MagicWaypointPlugin) /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.h b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.h index 030786707..636c0c3f3 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.h +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointplugin.h @@ -32,16 +32,15 @@ class MagicWaypointGadgetFactory; -class MagicWaypointPlugin : public ExtensionSystem::IPlugin -{ +class MagicWaypointPlugin : public ExtensionSystem::IPlugin { public: MagicWaypointPlugin(); - ~MagicWaypointPlugin(); + ~MagicWaypointPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - MagicWaypointGadgetFactory *mf; + MagicWaypointGadgetFactory *mf; }; #endif /* GCSControlPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.cpp b/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.cpp index 26172ce4a..65fd77069 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.cpp +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.cpp @@ -38,12 +38,12 @@ #include /** - * @brief Constructor for JoystickControl widget. Sets up the image of a joystick - */ + * @brief Constructor for JoystickControl widget. Sets up the image of a joystick + */ PositionField::PositionField(QWidget *parent) : QGraphicsView(parent) { - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setScene(new QGraphicsScene(this)); setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); @@ -59,12 +59,12 @@ PositionField::PositionField(QWidget *parent) : m_positiondesired = new QGraphicsSvgItem(); m_positiondesired->setSharedRenderer(m_renderer); m_positiondesired->setElementId(QString("desiredPosition")); - m_positiondesired->setPos(0,0); + m_positiondesired->setPos(0, 0); m_positionactual = new QGraphicsSvgItem(); m_positionactual->setSharedRenderer(m_renderer); m_positionactual->setElementId(QString("actualPosition")); - m_positionactual->setPos(0,0); + m_positionactual->setPos(0, 0); QGraphicsScene *l_scene = scene(); l_scene->clear(); // This also deletes all items contained in the scene. @@ -75,20 +75,18 @@ PositionField::PositionField(QWidget *parent) : } PositionField::~PositionField() -{ - -} +{} /** - * @brief Update aircraft position on image (values go from -1 to 1) - */ + * @brief Update aircraft position on image (values go from -1 to 1) + */ void PositionField::updateDesiredIndicator(double north, double east) { QRectF sceneSize = scene()->sceneRect(); m_positiondesired->setPos( - (east+1)/2*sceneSize.width() - m_positiondesired->boundingRect().width() / 2, - (-north+1)/2*sceneSize.height() - m_positiondesired->boundingRect().height() / 2); + (east + 1) / 2 * sceneSize.width() - m_positiondesired->boundingRect().width() / 2, + (-north + 1) / 2 * sceneSize.height() - m_positiondesired->boundingRect().height() / 2); } void PositionField::updateActualIndicator(double north, double east) @@ -96,29 +94,29 @@ void PositionField::updateActualIndicator(double north, double east) QRectF sceneSize = scene()->sceneRect(); m_positionactual->setPos( - (east+1)/2*sceneSize.width() - m_positionactual->boundingRect().width() / 2, - (-north+1)/2*sceneSize.height() - m_positionactual->boundingRect().height() / 2); + (east + 1) / 2 * sceneSize.width() - m_positionactual->boundingRect().width() / 2, + (-north + 1) / 2 * sceneSize.height() - m_positionactual->boundingRect().height() / 2); } /** - * @brief Redirect mouse move events to control position - */ + * @brief Redirect mouse move events to control position + */ void PositionField::mouseMoveEvent(QMouseEvent *event) { - QPointF point = mapToScene(event->pos()); + QPointF point = mapToScene(event->pos()); QRectF sceneSize = scene()->sceneRect(); - double north = - (point.y() / sceneSize.height() - .5) * 2; + double north = -(point.y() / sceneSize.height() - .5) * 2; double east = (point.x() / sceneSize.width() - .5) * 2; emit positionClicked(north, east); } /** - * @brief Redirect mouse move clicks to control position - */ + * @brief Redirect mouse move clicks to control position + */ void PositionField::mousePressEvent(QMouseEvent *event) { - if( event->button() == Qt::LeftButton ) { + if (event->button() == Qt::LeftButton) { mouseMoveEvent(event); } } @@ -131,8 +129,8 @@ void PositionField::paint() void PositionField::paintEvent(QPaintEvent *event) { // Skip painting until the dial file is loaded - if (! m_renderer->isValid()) { - qDebug()<<"Image file not loaded, not rendering"; + if (!m_renderer->isValid()) { + qDebug() << "Image file not loaded, not rendering"; } QGraphicsView::paintEvent(event); @@ -141,11 +139,11 @@ void PositionField::paintEvent(QPaintEvent *event) void PositionField::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - fitInView(m_background, Qt::IgnoreAspectRatio ); + fitInView(m_background, Qt::IgnoreAspectRatio); } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.h b/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.h index 3a8b17cbf..0c5ce7834 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.h +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/positionfield.h @@ -35,11 +35,10 @@ #include namespace Ui { - class PositionField; +class PositionField; } -class PositionField : public QGraphicsView -{ +class PositionField : public QGraphicsView { Q_OBJECT public: @@ -70,6 +69,6 @@ private: #endif // POSITIONFIELD_H /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.cpp index 899345b13..b84a6b5af 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -29,22 +29,21 @@ #include "modelviewgadgetconfiguration.h" ModelViewGadget::ModelViewGadget(QString classId, ModelViewGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} ModelViewGadget::~ModelViewGadget() { delete m_widget; } -void ModelViewGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void ModelViewGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - ModelViewGadgetConfiguration *m = qobject_cast(config); + ModelViewGadgetConfiguration *m = qobject_cast(config); + m_widget->setAcFilename(m->acFilename()); m_widget->setBgFilename(m->bgFilename()); m_widget->setVboEnable(m->vboEnabled()); m_widget->reloadScene(); } - diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.h b/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.h index 2c6734cfd..7ca4282aa 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.h +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,15 +38,17 @@ class ModelViewGadgetWidget; using namespace Core; -class ModelViewGadget : public Core::IUAVGadget -{ +class ModelViewGadget : public Core::IUAVGadget { Q_OBJECT public: ModelViewGadget(QString classId, ModelViewGadgetWidget *widget, QWidget *parent = 0); ~ModelViewGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: ModelViewGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.cpp index 50416d4b4..dd886d17f 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.cpp @@ -7,38 +7,38 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "modelviewgadgetconfiguration.h" #include "utils/pathutils.h" -ModelViewGadgetConfiguration::ModelViewGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +ModelViewGadgetConfiguration::ModelViewGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_acFilename("../share/openpilotgcs/models/planes/Easystar/EasyStar.3ds"), m_bgFilename(""), m_enableVbo(false) { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { QString modelFile = qSettings->value("acFilename").toString(); - QString bgFile = qSettings->value("bgFilename").toString(); - m_enableVbo = qSettings->value("enableVbo").toBool(); + QString bgFile = qSettings->value("bgFilename").toString(); + m_enableVbo = qSettings->value("enableVbo").toBool(); m_acFilename = Utils::PathUtils().InsertDataPath(modelFile); m_bgFilename = Utils::PathUtils().InsertDataPath(bgFile); } @@ -47,9 +47,10 @@ ModelViewGadgetConfiguration::ModelViewGadgetConfiguration(QString classId, QSet IUAVGadgetConfiguration *ModelViewGadgetConfiguration::clone() { ModelViewGadgetConfiguration *mv = new ModelViewGadgetConfiguration(this->classId()); + mv->m_acFilename = m_acFilename; mv->m_bgFilename = m_bgFilename; - mv->m_enableVbo = m_enableVbo; + mv->m_enableVbo = m_enableVbo; return mv; } @@ -57,8 +58,9 @@ IUAVGadgetConfiguration *ModelViewGadgetConfiguration::clone() * Saves a configuration. * */ -void ModelViewGadgetConfiguration::saveConfig(QSettings* qSettings) const { - qSettings->setValue("acFilename", Utils::PathUtils().RemoveDataPath(m_acFilename)); - qSettings->setValue("bgFilename", Utils::PathUtils().RemoveDataPath(m_bgFilename)); - qSettings->setValue("enableVbo", m_enableVbo); +void ModelViewGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ + qSettings->setValue("acFilename", Utils::PathUtils().RemoveDataPath(m_acFilename)); + qSettings->setValue("bgFilename", Utils::PathUtils().RemoveDataPath(m_bgFilename)); + qSettings->setValue("enableVbo", m_enableVbo); } diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.h index a43684487..ce3b0a939 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetconfiguration.h @@ -7,21 +7,21 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,20 +32,37 @@ using namespace Core; -class ModelViewGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT +class ModelViewGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit ModelViewGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit ModelViewGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); - QString acFilename() {return m_acFilename;} - void setAcFilename(QString acFile) { m_acFilename = acFile; } - QString bgFilename() { return m_bgFilename; } - void setBgFilename(QString bgFile) { m_bgFilename = bgFile; } - bool vboEnabled() { return m_enableVbo; } - void setVboEnabled(bool vboEnable) { m_enableVbo = vboEnable; } + QString acFilename() + { + return m_acFilename; + } + void setAcFilename(QString acFile) + { + m_acFilename = acFile; + } + QString bgFilename() + { + return m_bgFilename; + } + void setBgFilename(QString bgFile) + { + m_bgFilename = bgFile; + } + bool vboEnabled() + { + return m_enableVbo; + } + void setVboEnabled(bool vboEnable) + { + m_enableVbo = vboEnable; + } signals: public slots: @@ -53,7 +70,7 @@ public slots: private: QString m_acFilename; QString m_bgFilename; - bool m_enableVbo; // Vertex buffer objects, a few GPUs crash if enabled + bool m_enableVbo; // Vertex buffer objects, a few GPUs crash if enabled }; #endif // MODELVIEWGADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.cpp index 4c1f4ef52..bad149297 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,27 +33,25 @@ #include ModelViewGadgetFactory::ModelViewGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("ModelViewGadget"), tr("ModelView"), parent) -{ -} + IUAVGadgetFactory(QString("ModelViewGadget"), tr("ModelView"), parent) +{} ModelViewGadgetFactory::~ModelViewGadgetFactory() +{} + +Core::IUAVGadget *ModelViewGadgetFactory::createGadget(QWidget *parent) { + ModelViewGadgetWidget *gadgetWidget = new ModelViewGadgetWidget(parent); + + return new ModelViewGadget(QString("ModelViewGadget"), gadgetWidget, parent); } -Core::IUAVGadget* ModelViewGadgetFactory::createGadget(QWidget *parent) -{ - ModelViewGadgetWidget* gadgetWidget = new ModelViewGadgetWidget(parent); - return new ModelViewGadget(QString("ModelViewGadget"), gadgetWidget, parent); -} - -IUAVGadgetConfiguration *ModelViewGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *ModelViewGadgetFactory::createConfiguration(QSettings *qSettings) { return new ModelViewGadgetConfiguration(QString("ModelViewGadget"), qSettings); } IOptionsPage *ModelViewGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new ModelViewGadgetOptionsPage(qobject_cast(config)); + return new ModelViewGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.h b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.h index ff3ec52c2..32267d845 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class ModelViewGadgetFactory : public Core::IUAVGadgetFactory -{ +class ModelViewGadgetFactory : public Core::IUAVGadgetFactory { Q_OBJECT public: ModelViewGadgetFactory(QObject *parent = 0); ~ModelViewGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.cpp index 005e73242..2822555a4 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.cpp @@ -7,21 +7,21 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,8 +34,7 @@ ModelViewGadgetOptionsPage::ModelViewGadgetOptionsPage(ModelViewGadgetConfiguration *config, QObject *parent) : IOptionsPage(parent), m_config(config) -{ -} +{} QWidget *ModelViewGadgetOptionsPage::createPage(QWidget *parent) { @@ -70,4 +69,3 @@ void ModelViewGadgetOptionsPage::finish() { delete m_page; } - diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.h index 04e604f1a..e5875583f 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetoptionspage.h @@ -7,21 +7,21 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,23 +38,34 @@ class ModelViewGadgetConfiguration; class QFileDialog; namespace Core { - class IUAVGadgetConfiguration; +class IUAVGadgetConfiguration; } namespace Ui { - class ModelViewOptionsPage; +class ModelViewOptionsPage; } using namespace Core; -class ModelViewGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class ModelViewGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit ModelViewGadgetOptionsPage(ModelViewGadgetConfiguration *config, QObject *parent = 0); - QString id() const { return ""; } - QString trName() const { return ""; } - QString category() const { return ""; } - QString trCategory() const { return ""; } + QString id() const + { + return ""; + } + QString trName() const + { + return ""; + } + QString category() const + { + return ""; + } + QString trCategory() const + { + return ""; + } QWidget *createPage(QWidget *parent); void apply(); diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp index 562be089c..11047c7b5 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,67 +35,61 @@ #include -ModelViewGadgetWidget::ModelViewGadgetWidget(QWidget *parent) -: QGLWidget(new GLC_Context(QGLFormat(QGL::SampleBuffers)),parent) -, m_Light() -, m_World() -, m_GlView(this) -, m_MoverController() -, m_ModelBoundingBox() -, m_MotionTimer() -, acFilename() -, bgFilename() -, vboEnable(false) +ModelViewGadgetWidget::ModelViewGadgetWidget(QWidget *parent) + : QGLWidget(new GLC_Context(QGLFormat(QGL::SampleBuffers)), parent) + , m_Light() + , m_World() + , m_GlView(this) + , m_MoverController() + , m_ModelBoundingBox() + , m_MotionTimer() + , acFilename() + , bgFilename() + , vboEnable(false) { - setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); CreateScene(); QColor repColor; repColor.setRgbF(1.0, 0.11372, 0.11372, 0.0); - m_MoverController= GLC_Factory::instance()->createDefaultMoverController(repColor, &m_GlView); + m_MoverController = GLC_Factory::instance()->createDefaultMoverController(repColor, &m_GlView); // Get required UAVObjects - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager* objManager = pm->getObject(); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); attActual = AttitudeActual::GetInstance(objManager); connect(&m_MotionTimer, SIGNAL(timeout()), this, SLOT(updateAttitude())); } ModelViewGadgetWidget::~ModelViewGadgetWidget() -{ - -} +{} void ModelViewGadgetWidget::setAcFilename(QString acf) { - if(QFile::exists(acf)) + if (QFile::exists(acf)) { acFilename = acf; - else - { - acFilename= acf= ":/modelview/models/warning_sign.obj"; + } else { + acFilename = acf = ":/modelview/models/warning_sign.obj"; m_GlView.cameraHandle()->setFrontView(); // set to front camera to see/read the warning sign } } void ModelViewGadgetWidget::setBgFilename(QString bgf) { - if (QFile::exists(bgFilename)) + if (QFile::exists(bgFilename)) { bgFilename = bgf; - else - { - qDebug() << "file " << bgf << " doesn't exists"; - bgFilename= ":/modelview/models/black.jpg"; // will put a black background if there's no background + } else { + qDebug() << "file " << bgf << " doesn't exists"; + bgFilename = ":/modelview/models/black.jpg"; // will put a black background if there's no background } - } void ModelViewGadgetWidget::setVboEnable(bool eVbo) { - vboEnable = eVbo; - m_World.collection()->setVboUsage(vboEnable); + vboEnable = eVbo; + m_World.collection()->setVboUsage(vboEnable); } //// Public funcitons //// @@ -127,32 +121,30 @@ void ModelViewGadgetWidget::initializeGL() void ModelViewGadgetWidget::paintGL() { - try - { - // Clear screen - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + try { + // Clear screen + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - // OpenGL error handler - { - GLenum error= glGetError(); - if (error != GL_NO_ERROR) - { - GLC_OpenGlException OpenGlException("ModelViewGadgetWidget::paintGL() ", error); - throw(OpenGlException); - } - } + // OpenGL error handler + { + GLenum error = glGetError(); + if (error != GL_NO_ERROR) { + GLC_OpenGlException OpenGlException("ModelViewGadgetWidget::paintGL() ", error); + throw(OpenGlException); + } + } - // Load identity matrix - glLoadIdentity(); + // Load identity matrix + glLoadIdentity(); - // Enable antialiasing - glEnable(GL_MULTISAMPLE); + // Enable antialiasing + glEnable(GL_MULTISAMPLE); // Calculate camera depth of view m_GlView.setDistMinAndMax(m_World.boundingBox()); // define view matrix - m_Light.glExecute(); + m_Light.glExecute(); m_GlView.glExecuteCam(); // Display the collection of GLC_Object @@ -161,169 +153,155 @@ void ModelViewGadgetWidget::paintGL() // Display UI Info (orbit circle) m_MoverController.drawActiveMoverRep(); + } catch(GLC_Exception &e) { + qDebug() << e.what(); } - catch (GLC_Exception &e) - { - qDebug() << e.what(); - } - - } void ModelViewGadgetWidget::resizeGL(int width, int height) { - m_GlView.setWinGLSize(width, height); // Compute window aspect ratio - // OpenGL error handler - { - GLenum error= glGetError(); - if (error != GL_NO_ERROR) - { - GLC_OpenGlException OpenGlException("ModelViewGadgetWidget::resizeGL() ", error); - throw(OpenGlException); - } - } - + m_GlView.setWinGLSize(width, height); // Compute window aspect ratio + // OpenGL error handler + { + GLenum error = glGetError(); + if (error != GL_NO_ERROR) { + GLC_OpenGlException OpenGlException("ModelViewGadgetWidget::resizeGL() ", error); + throw(OpenGlException); + } + } } // Create GLC_Object to display void ModelViewGadgetWidget::CreateScene() { // put a black background if the 3D model is invalid or if the background image is also invalid - if (acFilename == ":/modelview/models/warning_sign.obj" or !QFile::exists(bgFilename)) - bgFilename= ":/modelview/models/black.jpg"; - - - try - { - m_GlView.loadBackGroundImage(bgFilename); + if (acFilename == ":/modelview/models/warning_sign.obj" or !QFile::exists(bgFilename)) { + bgFilename = ":/modelview/models/black.jpg"; } - catch(GLC_Exception e) - { + + + try { + m_GlView.loadBackGroundImage(bgFilename); + } catch(GLC_Exception e) { qDebug("ModelView: background image file loading failed."); } - try - { - if(QFile::exists(acFilename)) - { + try { + if (QFile::exists(acFilename)) { QFile aircraft(acFilename); - m_World= GLC_Factory::instance()->createWorldFromFile(aircraft); - m_ModelBoundingBox= m_World.boundingBox(); + m_World = GLC_Factory::instance()->createWorldFromFile(aircraft); + m_ModelBoundingBox = m_World.boundingBox(); m_GlView.reframe(m_ModelBoundingBox); // center 3D model in the scene } else { - qDebug("ModelView: aircraft file not found."); + qDebug("ModelView: aircraft file not found."); } - } - catch(GLC_Exception e) - { + } catch(GLC_Exception e) { qDebug("ModelView: aircraft file loading failed."); } } -void ModelViewGadgetWidget::wheelEvent(QWheelEvent * e) +void ModelViewGadgetWidget::wheelEvent(QWheelEvent *e) { - double delta = m_GlView.cameraHandle()->distEyeTarget() - (e->delta()/4) ; - m_GlView.cameraHandle()->setDistEyeTarget(delta); - m_GlView.setDistMinAndMax(m_World.boundingBox()); + double delta = m_GlView.cameraHandle()->distEyeTarget() - (e->delta() / 4); + + m_GlView.cameraHandle()->setDistEyeTarget(delta); + m_GlView.setDistMinAndMax(m_World.boundingBox()); } void ModelViewGadgetWidget::mousePressEvent(QMouseEvent *e) { - GLC_UserInput userInput(e->x(), e->y()); - if (m_MoverController.hasActiveMover()) return; + GLC_UserInput userInput(e->x(), e->y()); - switch (e->button()) - { - case (Qt::LeftButton): - m_MotionTimer.stop(); - m_MoverController.setActiveMover(GLC_MoverController::TurnTable, userInput); - updateGL(); - break; - case (Qt::RightButton): - printf("VBO enabled: %s, VBO supported: %s, VBO used: %s\n", - vboEnable ? "yes" : "no", - GLC_State::vboSupported() ? "yes" : "no", - GLC_State::vboUsed() ? "yes" : "no"); - printf("Renderer - %s \n", (char*)glGetString(GL_RENDERER)); - printf("Extensions - %s\n", (char*)glGetString(GL_EXTENSIONS)); - break; - default: - break; - } -} + if (m_MoverController.hasActiveMover()) { + return; + } -void ModelViewGadgetWidget::mouseMoveEvent(QMouseEvent * e) -{ - GLC_UserInput userInput(e->x(), e->y()); - if (not m_MoverController.hasActiveMover()) return; - m_MoverController.move(userInput); - m_GlView.setDistMinAndMax(m_World.boundingBox()); + switch (e->button()) { + case (Qt::LeftButton): + m_MotionTimer.stop(); + m_MoverController.setActiveMover(GLC_MoverController::TurnTable, userInput); updateGL(); + break; + case (Qt::RightButton): + printf("VBO enabled: %s, VBO supported: %s, VBO used: %s\n", + vboEnable ? "yes" : "no", + GLC_State::vboSupported() ? "yes" : "no", + GLC_State::vboUsed() ? "yes" : "no"); + printf("Renderer - %s \n", (char *)glGetString(GL_RENDERER)); + printf("Extensions - %s\n", (char *)glGetString(GL_EXTENSIONS)); + break; + default: + break; + } } -void ModelViewGadgetWidget::mouseReleaseEvent(QMouseEvent*) +void ModelViewGadgetWidget::mouseMoveEvent(QMouseEvent *e) { - if (not m_MoverController.hasActiveMover()) return; - m_MoverController.setNoMover(); - m_MotionTimer.start(); - updateGL(); + GLC_UserInput userInput(e->x(), e->y()); + + if (not m_MoverController.hasActiveMover()) { + return; + } + m_MoverController.move(userInput); + m_GlView.setDistMinAndMax(m_World.boundingBox()); + updateGL(); } -void ModelViewGadgetWidget::keyPressEvent(QKeyEvent * e) // switch between camera +void ModelViewGadgetWidget::mouseReleaseEvent(QMouseEvent *) { - if (e->key() == Qt::Key_1) - { + if (not m_MoverController.hasActiveMover()) { + return; + } + m_MoverController.setNoMover(); + m_MotionTimer.start(); + updateGL(); +} + +void ModelViewGadgetWidget::keyPressEvent(QKeyEvent *e) // switch between camera +{ + if (e->key() == Qt::Key_1) { m_GlView.cameraHandle()->setIsoView(); updateGL(); } - if (e->key() == Qt::Key_2) - { + if (e->key() == Qt::Key_2) { m_GlView.cameraHandle()->setFrontView(); updateGL(); } - if (e->key() == Qt::Key_3) - { + if (e->key() == Qt::Key_3) { m_GlView.cameraHandle()->setIsoView(); - m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS,glc::toRadian(90)); + m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS, glc::toRadian(90)); updateGL(); } - if (e->key() == Qt::Key_4) - { + if (e->key() == Qt::Key_4) { m_GlView.cameraHandle()->setLeftView(); updateGL(); } - if (e->key() == Qt::Key_5) - { + if (e->key() == Qt::Key_5) { m_GlView.cameraHandle()->setTopView(); - m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS,glc::toRadian(180)); + m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS, glc::toRadian(180)); updateGL(); } - if (e->key() == Qt::Key_6) - { + if (e->key() == Qt::Key_6) { m_GlView.cameraHandle()->setRightView();; updateGL(); } - if (e->key() == Qt::Key_7) - { + if (e->key() == Qt::Key_7) { m_GlView.cameraHandle()->setIsoView(); - m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS,glc::toRadian(-90)); + m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS, glc::toRadian(-90)); updateGL(); } - if (e->key() == Qt::Key_8) - { + if (e->key() == Qt::Key_8) { m_GlView.cameraHandle()->setRearView();; updateGL(); } - if (e->key() == Qt::Key_9) - { + if (e->key() == Qt::Key_9) { m_GlView.cameraHandle()->setIsoView(); - m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS,glc::toRadian(180)); + m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS, glc::toRadian(180)); updateGL(); } - if (e->key() == Qt::Key_0) - { + if (e->key() == Qt::Key_0) { m_GlView.cameraHandle()->setBottomView(); - m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS,glc::toRadian(180)); + m_GlView.cameraHandle()->rotateAroundTarget(glc::Z_AXIS, glc::toRadian(180)); updateGL(); } } @@ -334,32 +312,34 @@ void ModelViewGadgetWidget::keyPressEvent(QKeyEvent * e) // switch between camer void ModelViewGadgetWidget::updateAttitude() { AttitudeActual::DataFields data = attActual->getData(); // get attitude data - GLC_StructOccurence* rootObject= m_World.rootOccurence(); // get the full 3D model - double x= data.q3; - double y= data.q2; - double z= data.q4; - double w= data.q1; - if (w == 0.0) + GLC_StructOccurence *rootObject = m_World.rootOccurence(); // get the full 3D model + double x = data.q3; + double y = data.q2; + double z = data.q4; + double w = data.q1; + + if (w == 0.0) { w = 1.0; + } // create and gives the product of 2 4x4 matrices to get the rotation of the 3D model's matrix QMatrix4x4 m1; - m1.setRow(0, QVector4D(w,z,-y,x)); - m1.setRow(1, QVector4D(-z,w,x,y)); - m1.setRow(2, QVector4D(y,-x,w,z)); - m1.setRow(3, QVector4D(-x,-y,-z,w)); + m1.setRow(0, QVector4D(w, z, -y, x)); + m1.setRow(1, QVector4D(-z, w, x, y)); + m1.setRow(2, QVector4D(y, -x, w, z)); + m1.setRow(3, QVector4D(-x, -y, -z, w)); QMatrix4x4 m2; - m2.setRow(0, QVector4D(w,z,-y,-x)); - m2.setRow(1, QVector4D(-z,w,x,-y)); - m2.setRow(2, QVector4D(y,-x,w,-z)); - m2.setRow(3, QVector4D(x,y,z,w)); - QMatrix4x4 m0= m1 * m2; + m2.setRow(0, QVector4D(w, z, -y, -x)); + m2.setRow(1, QVector4D(-z, w, x, -y)); + m2.setRow(2, QVector4D(y, -x, w, -z)); + m2.setRow(3, QVector4D(x, y, z, w)); + QMatrix4x4 m0 = m1 * m2; // convert QMatrix4x4 to GLC_Matrix4x4 GLC_Matrix4x4 rootObjectRotation; { - double* newMatrixData = rootObjectRotation.setData(); - double* oldMatrixData = (double*) m0.data(); - for (int i=0; i<16; i++){ - newMatrixData[i]= oldMatrixData[i]; + double *newMatrixData = rootObjectRotation.setData(); + double *oldMatrixData = (double *)m0.data(); + for (int i = 0; i < 16; i++) { + newMatrixData[i] = oldMatrixData[i]; } } // sets and updates the 3D model's matrix diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h index 7ab6f19b2..353aeba7a 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -42,34 +42,32 @@ #include "attitudeactual.h" - -class ModelViewGadgetWidget : public QGLWidget -{ +class ModelViewGadgetWidget : public QGLWidget { Q_OBJECT public: ModelViewGadgetWidget(QWidget *parent = 0); - ~ModelViewGadgetWidget(); - void setAcFilename(QString acf); + ~ModelViewGadgetWidget(); + void setAcFilename(QString acf); - void setBgFilename(QString bgf); - void setVboEnable(bool eVbo); - void reloadScene(); - void updateAttitude(int value); + void setBgFilename(QString bgf); + void setVboEnable(bool eVbo); + void reloadScene(); + void updateAttitude(int value); private: - void initializeGL(); - void paintGL(); - void resizeGL(int width, int height); - // Create GLC_Object to display - void CreateScene(); + void initializeGL(); + void paintGL(); + void resizeGL(int width, int height); + // Create GLC_Object to display + void CreateScene(); - //Mouse events - void mousePressEvent(QMouseEvent * e); - void mouseMoveEvent(QMouseEvent * e); - void mouseReleaseEvent(QMouseEvent * e); - void wheelEvent(QWheelEvent * e); - void keyPressEvent(QKeyEvent * e); + // Mouse events + void mousePressEvent(QMouseEvent *e); + void mouseMoveEvent(QMouseEvent *e); + void mouseReleaseEvent(QMouseEvent *e); + void wheelEvent(QWheelEvent *e); + void keyPressEvent(QKeyEvent *e); ////////////////////////////////////////////////////////////////////// // Private slots Functions @@ -78,20 +76,20 @@ private slots: void updateAttitude(); private: - GLC_Factory* m_pFactory; + GLC_Factory *m_pFactory; GLC_Light m_Light; GLC_World m_World; GLC_Viewport m_GlView; GLC_MoverController m_MoverController; GLC_BoundingBox m_ModelBoundingBox; - //! The timer used for motion + // ! The timer used for motion QTimer m_MotionTimer; QString acFilename; QString bgFilename; bool vboEnable; - AttitudeActual* attActual; + AttitudeActual *attActual; }; #endif /* MODELVIEWGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.cpp index de4831d51..12540de27 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -34,32 +34,31 @@ ModelViewPlugin::ModelViewPlugin() { - // Do nothing + // Do nothing } ModelViewPlugin::~ModelViewPlugin() { - // Do nothing + // Do nothing } -bool ModelViewPlugin::initialize(const QStringList& args, QString *errMsg) +bool ModelViewPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mvf = new ModelViewGadgetFactory(this); - addAutoReleasedObject(mvf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mvf = new ModelViewGadgetFactory(this); + addAutoReleasedObject(mvf); - return true; + return true; } void ModelViewPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void ModelViewPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(ModelViewPlugin) - diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.h b/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.h index f0c9bca8b..74cbc6447 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.h +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup ModelViewPlugin ModelView Plugin * @{ - * @brief A gadget that displays a 3D representation of the UAV + * @brief A gadget that displays a 3D representation of the UAV *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class ModelViewGadgetFactory; -class ModelViewPlugin : public ExtensionSystem::IPlugin -{ +class ModelViewPlugin : public ExtensionSystem::IPlugin { public: - ModelViewPlugin(); - ~ModelViewPlugin(); + ModelViewPlugin(); + ~ModelViewPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - ModelViewGadgetFactory *mvf; + ModelViewGadgetFactory *mvf; }; #endif /* MODELVIEWPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/notify/notificationitem.cpp b/ground/openpilotgcs/src/plugins/notify/notificationitem.cpp index a54e7ff89..b3d627838 100644 --- a/ground/openpilotgcs/src/plugins/notify/notificationitem.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notificationitem.cpp @@ -25,7 +25,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -//Qt headers +// Qt headers #include #include @@ -40,9 +40,6 @@ #include "notifylogging.h" - - - QStringList NotificationItem::sayOrderValues; QStringList NotificationItem::retryValues; @@ -69,47 +66,44 @@ NotificationItem::NotificationItem(QObject *parent) , _expireTimeout(eDefaultTimeout) , _mute(false) { - NotificationItem::sayOrderValues.clear(); - NotificationItem::sayOrderValues.insert(never,QString(tr("Never"))); - NotificationItem::sayOrderValues.insert(beforeFirst,QString(tr("Before first"))); - NotificationItem::sayOrderValues.insert(beforeSecond,QString(tr("Before second"))); - NotificationItem::sayOrderValues.insert(afterSecond,QString(tr("After second"))); + NotificationItem::sayOrderValues.insert(never, QString(tr("Never"))); + NotificationItem::sayOrderValues.insert(beforeFirst, QString(tr("Before first"))); + NotificationItem::sayOrderValues.insert(beforeSecond, QString(tr("Before second"))); + NotificationItem::sayOrderValues.insert(afterSecond, QString(tr("After second"))); NotificationItem::retryValues.clear(); - NotificationItem::retryValues.insert(repeatOnce,QString(tr("Repeat Once"))); - NotificationItem::retryValues.insert(repeatOncePerUpdate,QString(tr("Repeat Once per update"))); - NotificationItem::retryValues.insert(repeatInstantly,QString(tr("Repeat Instantly"))); - NotificationItem::retryValues.insert(repeat10seconds,QString(tr("Repeat 10 seconds"))); - NotificationItem::retryValues.insert(repeat30seconds,QString(tr("Repeat 30 seconds"))); - NotificationItem::retryValues.insert(repeat1minute,QString(tr("Repeat 1 minute"))); - + NotificationItem::retryValues.insert(repeatOnce, QString(tr("Repeat Once"))); + NotificationItem::retryValues.insert(repeatOncePerUpdate, QString(tr("Repeat Once per update"))); + NotificationItem::retryValues.insert(repeatInstantly, QString(tr("Repeat Instantly"))); + NotificationItem::retryValues.insert(repeat10seconds, QString(tr("Repeat 10 seconds"))); + NotificationItem::retryValues.insert(repeat30seconds, QString(tr("Repeat 30 seconds"))); + NotificationItem::retryValues.insert(repeat1minute, QString(tr("Repeat 1 minute"))); } -void NotificationItem::copyTo(NotificationItem* that) const +void NotificationItem::copyTo(NotificationItem *that) const { - that->isNowPlaying = isNowPlaying; - that->_isPlayed = _isPlayed; + that->isNowPlaying = isNowPlaying; + that->_isPlayed = _isPlayed; that->_soundCollectionPath = _soundCollectionPath; - that->_currentLanguage = _currentLanguage; + that->_currentLanguage = _currentLanguage; that->_soundCollectionPath = _soundCollectionPath; - that->_dataObject = _dataObject; - that->_objectField = _objectField; - that->_condition = _condition; - that->_sound1 = _sound1; - that->_sound2 = _sound2; - that->_sound3 = _sound3; - that->_sayOrder = _sayOrder; - that->_singleValue = _singleValue; - that->_valueRange2 = _valueRange2; - that->_repeatValue = _repeatValue; + that->_dataObject = _dataObject; + that->_objectField = _objectField; + that->_condition = _condition; + that->_sound1 = _sound1; + that->_sound2 = _sound2; + that->_sound3 = _sound3; + that->_sayOrder = _sayOrder; + that->_singleValue = _singleValue; + that->_valueRange2 = _valueRange2; + that->_repeatValue = _repeatValue; that->_expireTimeout = _expireTimeout; that->_mute = _mute; - } -void NotificationItem::saveState(QSettings* settings) const +void NotificationItem::saveState(QSettings *settings) const { settings->setValue("SoundCollectionPath", Utils::PathUtils().RemoveDataPath(getSoundCollectionPath())); settings->setValue(QLatin1String("CurrentLanguage"), getCurrentLanguage()); @@ -127,9 +121,9 @@ void NotificationItem::saveState(QSettings* settings) const settings->setValue(QLatin1String("Mute"), mute()); } -void NotificationItem::restoreState(QSettings* settings) +void NotificationItem::restoreState(QSettings *settings) { - //settings = Core::ICore::instance()->settings(); + // settings = Core::ICore::instance()->settings(); setSoundCollectionPath(Utils::PathUtils().InsertDataPath(settings->value(QLatin1String("SoundCollectionPath"), tr("")).toString())); setCurrentLanguage(settings->value(QLatin1String("CurrentLanguage"), tr("")).toString()); setDataObject(settings->value(QLatin1String("DataObject"), tr("")).toString()); @@ -147,14 +141,14 @@ void NotificationItem::restoreState(QSettings* settings) setMute(settings->value(QLatin1String("Mute"), tr("")).toInt()); } -void NotificationItem::serialize(QDataStream& stream) +void NotificationItem::serialize(QDataStream & stream) { stream << this->_soundCollectionPath; stream << this->_currentLanguage; stream << this->_dataObject; stream << this->_objectField; stream << this->_condition; - qNotifyDebug()<<"getOptionsPageValues seriaize"<<_condition; + qNotifyDebug() << "getOptionsPageValues seriaize" << _condition; stream << this->_sound1; stream << this->_sound2; stream << this->_sound3; @@ -166,7 +160,7 @@ void NotificationItem::serialize(QDataStream& stream) stream << this->_mute; } -void NotificationItem::deserialize(QDataStream& stream) +void NotificationItem::deserialize(QDataStream & stream) { stream >> this->_soundCollectionPath; stream >> this->_currentLanguage; @@ -190,16 +184,18 @@ void NotificationItem::startTimer(int msec) _timer = new QTimer(this); _timer->setInterval(msec); } - if (!_timer->isActive()) + if (!_timer->isActive()) { _timer->start(); + } } void NotificationItem::restartTimer() { if (!_timer) { - if (!_timer->isActive()) + if (!_timer->isActive()) { _timer->start(); + } } } @@ -207,8 +203,9 @@ void NotificationItem::restartTimer() void NotificationItem::stopTimer() { if (_timer) { - if (_timer->isActive()) + if (_timer->isActive()) { _timer->stop(); + } } } @@ -232,8 +229,9 @@ void NotificationItem::startExpireTimer() void NotificationItem::stopExpireTimer() { if (_expireTimer) { - if (_expireTimer) + if (_expireTimer) { _expireTimer->stop(); + } } } @@ -248,7 +246,7 @@ void NotificationItem::disposeExpireTimer() int getValuePosition(QString sayOrder) { - return NotificationItem::sayOrderValues.indexOf(sayOrder)-1; + return NotificationItem::sayOrderValues.indexOf(sayOrder) - 1; } QString NotificationItem::checkSoundExists(QString fileName) @@ -257,67 +255,71 @@ QString NotificationItem::checkSoundExists(QString fileName) QString filePath = QDir::toNativeSeparators(getSoundCollectionPath() + "/" + getCurrentLanguage() + "/" + name); - if(QFile::exists(filePath)) + + if (QFile::exists(filePath)) { return filePath; - else { + } else { filePath = QDir::toNativeSeparators(getSoundCollectionPath() + "/default/" + name); - if(!QFile::exists(filePath)) + if (!QFile::exists(filePath)) { filePath.clear(); + } } return filePath; } QStringList valueToSoundList(QString value) { - qNotifyDebug()<<"notificationItem valueToSoundList input param"< number < 1 digitWavs.append(numberParts.at(1)); } else { @@ -327,20 +329,22 @@ QStringList valueToSoundList(QString value) digitWavs.append(numberParts.at(1).right(1)); } } - qNotifyDebug()<<"notificationItem valueToSoundList return value"<getType()) { - if(!field->getOptions().contains(value.toString())) + if (!field->getOptions().contains(value.toString())) { return QString(); + } str = value.toString(); } else { str = QString("%L1").arg(value.toDouble()); @@ -351,62 +355,68 @@ QString stringFromValue(QVariant value, UAVObjectField* field) QString NotificationItem::toString() { QString str; - UAVObjectField* field = getUAVObjectField(); + UAVObjectField *field = getUAVObjectField(); QString value = stringFromValue(singleValue(), field); - int pos = getSayOrder()-1; + int pos = getSayOrder() - 1; QStringList lst; + lst.append(getSoundCaption(getSound1())); lst.append(getSoundCaption(getSound2())); lst.append(getSoundCaption(getSound3())); QStringList valueSounds = valueToSoundList(value); bool missed = false; foreach(QString sound, valueSounds) { - if(checkSoundExists(sound).isEmpty()) { - missed = true; - break; + if (checkSoundExists(sound).isEmpty()) { + missed = true; + break; } } // if not "Never" case - if(-1 != pos) { - if(missed) + if (-1 != pos) { + if (missed) { lst.insert(pos, "[missed]" + value); - else + } else { lst.insert(pos, value); + } } str = lst.join(" "); return str; } -QStringList& NotificationItem::toSoundList() +QStringList & NotificationItem::toSoundList() { // tips: // check of *.wav files exist needed for playing phonon queues; // if phonon player don't find next file in queue, it buzz - UAVObjectField* field = getUAVObjectField(); + UAVObjectField *field = getUAVObjectField(); QString value = stringFromValue(singleValue(), field); // generate queue of sound files to play _messageSequence.clear(); - int pos = getSayOrder()-1; + int pos = getSayOrder() - 1; QStringList lst; - if(!getSound1().isEmpty()) + if (!getSound1().isEmpty()) { lst.append(getSound1()); - if(!getSound2().isEmpty()) + } + if (!getSound2().isEmpty()) { lst.append(getSound2()); - if(!getSound3().isEmpty()) + } + if (!getSound3().isEmpty()) { lst.append(getSound3()); + } // if not "Never" case - if(-1 != pos) { + if (-1 != pos) { QStringList valueSounds = valueToSoundList(value); foreach(QString sound, valueSounds) - lst.insert(pos++, sound); + lst.insert(pos++, sound); } foreach(QString sound, lst) { QString path = checkSoundExists(sound); + if (!path.isEmpty()) { _messageSequence.append(path); } else { @@ -419,17 +429,21 @@ QStringList& NotificationItem::toSoundList() QString NotificationItem::getSoundCaption(QString fileName) { - if(fileName.isEmpty()) return QString(); - if(checkSoundExists(fileName).isEmpty()) { + if (fileName.isEmpty()) { + return QString(); + } + if (checkSoundExists(fileName).isEmpty()) { return QString("[missed]") + fileName; } return fileName; } -UAVObjectField* NotificationItem::getUAVObjectField() { +UAVObjectField *NotificationItem::getUAVObjectField() +{ return getUAVObject()->getField(getObjectField()); } -UAVDataObject* NotificationItem::getUAVObject() { - return dynamic_cast((ExtensionSystem::PluginManager::instance()->getObject())->getObject(getDataObject())); +UAVDataObject *NotificationItem::getUAVObject() +{ + return dynamic_cast((ExtensionSystem::PluginManager::instance()->getObject())->getObject(getDataObject())); } diff --git a/ground/openpilotgcs/src/plugins/notify/notificationitem.h b/ground/openpilotgcs/src/plugins/notify/notificationitem.h index 1475fd64f..c90b9c894 100644 --- a/ground/openpilotgcs/src/plugins/notify/notificationitem.h +++ b/ground/openpilotgcs/src/plugins/notify/notificationitem.h @@ -37,112 +37,195 @@ using namespace Core; #define DECLARE_SOUND(number) \ - QString getSound##number() const { return _sound##number; } \ - void setSound##number(QString text) { _sound##number = text; } \ + QString getSound##number() const { return _sound##number; } \ + void setSound##number(QString text) { _sound##number = text; } \ class UAVDataObject; class UAVObjectField; -class NotificationItem : public QObject -{ +class NotificationItem : public QObject { Q_OBJECT public: enum { eDefaultTimeout = 15 }; // in sec - enum {never,beforeFirst,beforeSecond,afterSecond}; - enum {repeatOncePerUpdate,repeatOnce,repeatInstantly,repeat10seconds, - repeat30seconds,repeat1minute}; + enum { never, beforeFirst, beforeSecond, afterSecond }; + enum { repeatOncePerUpdate, repeatOnce, repeatInstantly, repeat10seconds, + repeat30seconds, repeat1minute }; explicit NotificationItem(QObject *parent = 0); - void copyTo(NotificationItem*) const; + void copyTo(NotificationItem *) const; DECLARE_SOUND(1) DECLARE_SOUND(2) DECLARE_SOUND(3) - bool getCurrentUpdatePlayed() const {return _currentUpdatePlayed;} - void setCurrentUpdatePlayed(bool value){_currentUpdatePlayed=value;} + bool getCurrentUpdatePlayed() const + { + return _currentUpdatePlayed; + } + void setCurrentUpdatePlayed(bool value) + { + _currentUpdatePlayed = value; + } - int getCondition() const { return _condition; } - void setCondition(int value) { _condition = value; } + int getCondition() const + { + return _condition; + } + void setCondition(int value) + { + _condition = value; + } - int getSayOrder() const { return _sayOrder; } - void setSayOrder(int text) { _sayOrder = text; } + int getSayOrder() const + { + return _sayOrder; + } + void setSayOrder(int text) + { + _sayOrder = text; + } - QVariant singleValue() const { return _singleValue; } - void setSingleValue(QVariant value) { _singleValue = value; } + QVariant singleValue() const + { + return _singleValue; + } + void setSingleValue(QVariant value) + { + _singleValue = value; + } - double valueRange2() const { return _valueRange2; } - void setValueRange2(double value) { _valueRange2 = value; } + double valueRange2() const + { + return _valueRange2; + } + void setValueRange2(double value) + { + _valueRange2 = value; + } - QString getDataObject() const { return _dataObject; } - void setDataObject(QString text) { _dataObject = text; } + QString getDataObject() const + { + return _dataObject; + } + void setDataObject(QString text) + { + _dataObject = text; + } - QString getObjectField() const { return _objectField; } - void setObjectField(QString text) { _objectField = text; } + QString getObjectField() const + { + return _objectField; + } + void setObjectField(QString text) + { + _objectField = text; + } - QString getSoundCollectionPath() const { return _soundCollectionPath; } - void setSoundCollectionPath(QString path) { _soundCollectionPath = path; } + QString getSoundCollectionPath() const + { + return _soundCollectionPath; + } + void setSoundCollectionPath(QString path) + { + _soundCollectionPath = path; + } - QString getCurrentLanguage() const { return _currentLanguage; } - void setCurrentLanguage(QString text) { _currentLanguage = text; } + QString getCurrentLanguage() const + { + return _currentLanguage; + } + void setCurrentLanguage(QString text) + { + _currentLanguage = text; + } - QStringList getMessageSequence() const { return _messageSequence; } - void setMessageSequence(QStringList sequence) { _messageSequence = sequence; } + QStringList getMessageSequence() const + { + return _messageSequence; + } + void setMessageSequence(QStringList sequence) + { + _messageSequence = sequence; + } - int retryValue() const { return _repeatValue; } - void setRetryValue(int value) { _repeatValue = value; } + int retryValue() const + { + return _repeatValue; + } + void setRetryValue(int value) + { + _repeatValue = value; + } - int lifetime() const { return _expireTimeout; } - void setLifetime(int value) { _expireTimeout = value; } + int lifetime() const + { + return _expireTimeout; + } + void setLifetime(int value) + { + _expireTimeout = value; + } - bool mute() const { return _mute; } - void setMute(bool value) { _mute = value; } + bool mute() const + { + return _mute; + } + void setMute(bool value) + { + _mute = value; + } - void saveState(QSettings* settings) const; - void restoreState(QSettings* settings); + void saveState(QSettings *settings) const; + void restoreState(QSettings *settings); - UAVDataObject* getUAVObject(void); - UAVObjectField* getUAVObjectField(void); + UAVDataObject *getUAVObject(void); + UAVObjectField *getUAVObjectField(void); - void serialize(QDataStream& stream); - void deserialize(QDataStream& stream); + void serialize(QDataStream & stream); + void deserialize(QDataStream & stream); /** - * Convert notification item fields in single string, - * to show in table for example - * - * @return string which describe notification - */ + * Convert notification item fields in single string, + * to show in table for example + * + * @return string which describe notification + */ QString toString(); /** - * Generate list of sound files needed to play notification - * - * @return success - reference to non-empty _messageSequence; - * error - if one of sounds doesn't exist returns - * reference to empty _messageSequence; - */ - QStringList& toSoundList(); + * Generate list of sound files needed to play notification + * + * @return success - reference to non-empty _messageSequence; + * error - if one of sounds doesn't exist returns + * reference to empty _messageSequence; + */ + QStringList & toSoundList(); /** - * Returns sound caption name, needed to create string representation of notification. - * - * @return success - string == , if sound file exists - * error - string == [missind], if sound file doesn't exist - */ + * Returns sound caption name, needed to create string representation of notification. + * + * @return success - string == , if sound file exists + * error - string == [missind], if sound file doesn't exist + */ QString getSoundCaption(QString fileName); - QTimer* getTimer() const { return _timer; } + QTimer *getTimer() const + { + return _timer; + } void startTimer(int value); void restartTimer(); void stopTimer(); void disposeTimer(); - QTimer* getExpireTimer() const { return _expireTimer; } + QTimer *getExpireTimer() const + { + return _expireTimer; + } void startExpireTimer(); void stopExpireTimer(); @@ -161,57 +244,57 @@ private: bool _currentUpdatePlayed; - QTimer* _timer; + QTimer *_timer; - //! time from putting notification in queue till moment when notification became out-of-date - //! NOTE: each notification has it lifetime, this time setups individually for each notification - //! according to its priority - QTimer* _expireTimer; + // ! time from putting notification in queue till moment when notification became out-of-date + // ! NOTE: each notification has it lifetime, this time setups individually for each notification + // ! according to its priority + QTimer *_expireTimer; - //! list of wav files from which notification consists + // ! list of wav files from which notification consists QStringList _messageSequence; - //! path to folder with sound files + // ! path to folder with sound files QString _soundCollectionPath; - //! language in what notifications will be spelled + // ! language in what notifications will be spelled QString _currentLanguage; - //! one UAV object per one notification + // ! one UAV object per one notification QString _dataObject; - //! one field value change can be assigned to one notification + // ! one field value change can be assigned to one notification QString _objectField; - //! fire condition for UAV field value (lower, greater, in range) + // ! fire condition for UAV field value (lower, greater, in range) int _condition; - //! possible sounds(at least one required to play notification) + // ! possible sounds(at least one required to play notification) QString _sound1; QString _sound2; QString _sound3; - //! order in what sounds 1-3 will be played + // ! order in what sounds 1-3 will be played int _sayOrder; - //! one-side range, value(numeric or ENUM type) maybe lower, greater or in range + // ! one-side range, value(numeric or ENUM type) maybe lower, greater or in range QVariant _singleValue; - //! both-side range, value should be inside the range - //double _valueRange1; + // ! both-side range, value should be inside the range + // double _valueRange1; double _valueRange2; - //! how often or what periodicaly notification should be played + // ! how often or what periodicaly notification should be played int _repeatValue; - //! time after event occured till notification became invalid - //! and will be removed from list + // ! time after event occured till notification became invalid + // ! and will be removed from list int _expireTimeout; - //! enables/disables playing of current notification + // ! enables/disables playing of current notification bool _mute; }; -Q_DECLARE_METATYPE(NotificationItem*) +Q_DECLARE_METATYPE(NotificationItem *) #endif // NotificationItem_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.cpp b/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.cpp index 1db47049b..c3da268cb 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.cpp @@ -31,28 +31,27 @@ #include "notifylogging.h" #include "notificationitem.h" -NotifyItemDelegate::NotifyItemDelegate(QObject* parent) +NotifyItemDelegate::NotifyItemDelegate(QObject *parent) : QItemDelegate(parent) , _parent(parent) -{ -} +{} -QWidget *NotifyItemDelegate::createEditor(QWidget* parent, const QStyleOptionViewItem& /*none*/, - const QModelIndex& index) const +QWidget *NotifyItemDelegate::createEditor(QWidget *parent, const QStyleOptionViewItem & /*none*/, + const QModelIndex & index) const { if (eRepeatValue == index.column()) { - QComboBox* editor = new QComboBox(parent); + QComboBox *editor = new QComboBox(parent); editor->clear(); editor->addItems(NotificationItem::retryValues); return editor; } else { if (eExpireTimer == index.column()) { - QSpinBox* editor = new QSpinBox(parent); + QSpinBox *editor = new QSpinBox(parent); connect(editor, SIGNAL(editingFinished()), this, SLOT(commitAndCloseEditor())); return editor; } else { if (eTurnOn == index.column()) { - QCheckBox* editor = new QCheckBox(parent); + QCheckBox *editor = new QCheckBox(parent); connect(editor, SIGNAL(editingFinished()), this, SLOT(commitAndCloseEditor())); return editor; } @@ -64,26 +63,24 @@ QWidget *NotifyItemDelegate::createEditor(QWidget* parent, const QStyleOptionVie void NotifyItemDelegate::commitAndCloseEditor() { - QLineEdit* editor = qobject_cast(sender()); + QLineEdit *editor = qobject_cast(sender()); + if (editor) { emit commitData(editor); emit closeEditor(editor); } else { - QComboBox* editor = qobject_cast(sender()); - if (editor) - { + QComboBox *editor = qobject_cast(sender()); + if (editor) { emit commitData(editor); emit closeEditor(editor); } else { - QSpinBox* editor = qobject_cast(sender()); - if (editor) - { + QSpinBox *editor = qobject_cast(sender()); + if (editor) { emit commitData(editor); emit closeEditor(editor); } else { - QCheckBox* editor = qobject_cast(sender()); - if (editor) - { + QCheckBox *editor = qobject_cast(sender()); + if (editor) { emit commitData(editor); emit closeEditor(editor); } @@ -94,21 +91,23 @@ void NotifyItemDelegate::commitAndCloseEditor() void NotifyItemDelegate::setEditorData(QWidget *editor, const QModelIndex &index) const { - QLineEdit* edit = qobject_cast(editor); + QLineEdit *edit = qobject_cast(editor); + if (edit) { edit->setText(index.model()->data(index, Qt::EditRole).toString()); } else { - QComboBox* repeatEditor = qobject_cast(editor); - if (repeatEditor) + QComboBox *repeatEditor = qobject_cast(editor); + if (repeatEditor) { repeatEditor->setCurrentIndex(repeatEditor->findText(index.model()->data(index, Qt::EditRole).toString())); - else { - QSpinBox* expireEditor = qobject_cast(editor); - if (expireEditor) + } else { + QSpinBox *expireEditor = qobject_cast(editor); + if (expireEditor) { expireEditor->setValue(index.model()->data(index, Qt::EditRole).toInt()); - else { - QCheckBox* enablePlayEditor = qobject_cast(editor); - if (enablePlayEditor) + } else { + QCheckBox *enablePlayEditor = qobject_cast(editor); + if (enablePlayEditor) { enablePlayEditor->setChecked(index.model()->data(index, Qt::EditRole).toBool()); + } } } } @@ -117,18 +116,19 @@ void NotifyItemDelegate::setEditorData(QWidget *editor, const QModelIndex &index void NotifyItemDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, const QModelIndex &index) const { QLineEdit *edit = qobject_cast(editor); + if (edit) { model->setData(index, edit->text()); } else { - QComboBox * repeatEditor = qobject_cast(editor); + QComboBox *repeatEditor = qobject_cast(editor); if (repeatEditor) { model->setData(index, repeatEditor->currentText()); } else { - QSpinBox * expireEditor = qobject_cast(editor); + QSpinBox *expireEditor = qobject_cast(editor); if (expireEditor) { model->setData(index, expireEditor->value(), Qt::EditRole); } else { - QCheckBox* enablePlayEditor = qobject_cast(editor); + QCheckBox *enablePlayEditor = qobject_cast(editor); if (enablePlayEditor) { model->setData(index, enablePlayEditor->isChecked(), Qt::EditRole); } @@ -137,20 +137,22 @@ void NotifyItemDelegate::setModelData(QWidget *editor, QAbstractItemModel *model } } -void NotifyItemDelegate::selectRow(const QString& text) +void NotifyItemDelegate::selectRow(const QString & text) { - QComboBox* combo = qobject_cast(sender()); - QTableWidget* table = new QTableWidget; - table = (QTableWidget*)(combo->parent()); + QComboBox *combo = qobject_cast(sender()); + QTableWidget *table = new QTableWidget; + + table = (QTableWidget *)(combo->parent()); qNotifyDebug() << table->columnCount(); qNotifyDebug() << table->rowCount(); qNotifyDebug() << table->currentRow(); } -QSize NotifyItemDelegate::sizeHint(const QStyleOptionViewItem& option, const QModelIndex& index) const +QSize NotifyItemDelegate::sizeHint(const QStyleOptionViewItem & option, const QModelIndex & index) const { QSize s = QItemDelegate::sizeHint(option, index); + s.setHeight(10); return s; } diff --git a/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.h b/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.h index cdebd4928..904cc2d29 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.h +++ b/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.h @@ -32,25 +32,24 @@ #include -class NotifyItemDelegate : public QItemDelegate -{ +class NotifyItemDelegate : public QItemDelegate { Q_OBJECT public: NotifyItemDelegate(QObject *parent = 0); QWidget *createEditor(QWidget *parent, const QStyleOptionViewItem &, - const QModelIndex &index) const; + const QModelIndex &index) const; void setEditorData(QWidget *editor, const QModelIndex &index) const; void setModelData(QWidget *editor, QAbstractItemModel *model, - const QModelIndex &index) const; - QSize sizeHint ( const QStyleOptionViewItem & option, const QModelIndex & index ) const; + const QModelIndex &index) const; + QSize sizeHint(const QStyleOptionViewItem & option, const QModelIndex & index) const; private slots: void selectRow(const QString & text); void commitAndCloseEditor(); private: - QObject* _parent; + QObject *_parent; }; #endif // NOTIFYITEMDELEGATE_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifylogging.h b/ground/openpilotgcs/src/plugins/notify/notifylogging.h index 43e54f3b7..9fb58906f 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifylogging.h +++ b/ground/openpilotgcs/src/plugins/notify/notifylogging.h @@ -41,6 +41,8 @@ QDebug qNotifyDebug(); QNoDebug qNotifyDebug(); #endif -#define qNotifyDebug_if(test) if(test) qNotifyDebug() +#define qNotifyDebug_if(test) \ + if (test) \ + qNotifyDebug() #endif // NOTIFYLOGGING_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp b/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp index 19f38b626..72920e549 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp @@ -3,25 +3,25 @@ * * @file notifyplugin.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 * @defgroup notifyplugin * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,7 +42,7 @@ static const QString VERSION = "1.0.0"; -//#define DEBUG_NOTIFIES +// #define DEBUG_NOTIFIES SoundNotifyPlugin::SoundNotifyPlugin() @@ -53,33 +53,36 @@ SoundNotifyPlugin::SoundNotifyPlugin() SoundNotifyPlugin::~SoundNotifyPlugin() { Core::ICore::instance()->saveSettings(this); - if (phonon.mo != NULL) + + if (phonon.mo != NULL) { delete phonon.mo; + } } -bool SoundNotifyPlugin::initialize(const QStringList& args, QString *errMsg) -{ - Q_UNUSED(args); - Q_UNUSED(errMsg); +bool SoundNotifyPlugin::initialize(const QStringList & args, QString *errMsg) +{ + Q_UNUSED(args); + Q_UNUSED(errMsg); - mop = new NotifyPluginOptionsPage(this); - addAutoReleasedObject(mop); + mop = new NotifyPluginOptionsPage(this); + addAutoReleasedObject(mop); - return true; -} + return true; +} void SoundNotifyPlugin::extensionsInitialized() -{ +{ Core::ICore::instance()->readSettings(this); - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); - connect(pm, SIGNAL(objectAdded(QObject*)), this, SLOT(onTelemetryManagerAdded(QObject*))); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + + connect(pm, SIGNAL(objectAdded(QObject *)), this, SLOT(onTelemetryManagerAdded(QObject *))); _toRemoveNotifications.clear(); connectNotifications(); -} - -void SoundNotifyPlugin::saveConfig( QSettings* settings, UAVConfigInfo *configInfo){ +} +void SoundNotifyPlugin::saveConfig(QSettings *settings, UAVConfigInfo *configInfo) +{ configInfo->setVersion(VERSION); settings->beginWriteArray("Current"); @@ -98,13 +101,12 @@ void SoundNotifyPlugin::saveConfig( QSettings* settings, UAVConfigInfo *configIn } settings->endArray(); settings->setValue(QLatin1String("EnableSound"), enableSound); - } -void SoundNotifyPlugin::readConfig( QSettings* settings, UAVConfigInfo * /* configInfo */) +void SoundNotifyPlugin::readConfig(QSettings *settings, UAVConfigInfo * /* configInfo */) { // Just for migration to the new format. - //Q_ASSERT(configInfo->version() == UAVConfigVersion()); + // Q_ASSERT(configInfo->version() == UAVConfigVersion()); settings->beginReadArray("Current"); settings->setArrayIndex(0); @@ -115,24 +117,25 @@ void SoundNotifyPlugin::readConfig( QSettings* settings, UAVConfigInfo * /* conf int size = settings->beginReadArray("listNotifies"); for (int i = 0; i < size; ++i) { settings->setArrayIndex(i); - NotificationItem* notification = new NotificationItem; + NotificationItem *notification = new NotificationItem; notification->restoreState(settings); _notificationList.append(notification); } settings->endArray(); - setEnableSound(settings->value(QLatin1String("EnableSound"),0).toBool()); + setEnableSound(settings->value(QLatin1String("EnableSound"), 0).toBool()); } -void SoundNotifyPlugin::onTelemetryManagerAdded(QObject* obj) +void SoundNotifyPlugin::onTelemetryManagerAdded(QObject *obj) { telMngr = qobject_cast(obj); - if (telMngr) + if (telMngr) { connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); + } } void SoundNotifyPlugin::shutdown() -{ - // Do nothing +{ + // Do nothing } void SoundNotifyPlugin::onAutopilotDisconnect() @@ -146,8 +149,8 @@ void SoundNotifyPlugin::onAutopilotDisconnect() */ void SoundNotifyPlugin::resetNotification(void) { - //first, reject empty args and unknown fields. - foreach(NotificationItem* ntf, _notificationList) { + // first, reject empty args and unknown fields. + foreach(NotificationItem * ntf, _notificationList) { ntf->disposeTimer(); disconnect(ntf->getTimer(), SIGNAL(timeout()), this, SLOT(on_timerRepeated_Notification())); ntf->disposeExpireTimer(); @@ -159,12 +162,12 @@ void SoundNotifyPlugin::resetNotification(void) update list of notifies; will be perform on OK or APPLY press of option page */ -void SoundNotifyPlugin::updateNotificationList(QList list) +void SoundNotifyPlugin::updateNotificationList(QList list) { _toRemoveNotifications.clear(); resetNotification(); _notificationList.clear(); - _notificationList=list; + _notificationList = list; connectNotifications(); Core::ICore::instance()->saveSettings(this); @@ -172,16 +175,19 @@ void SoundNotifyPlugin::updateNotificationList(QList list) void SoundNotifyPlugin::connectNotifications() { - foreach(UAVDataObject* obj,lstNotifiedUAVObjects) { - if (obj != NULL) - disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(on_arrived_Notification(UAVObject*))); + foreach(UAVDataObject * obj, lstNotifiedUAVObjects) { + if (obj != NULL) { + disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(on_arrived_Notification(UAVObject *))); + } } if (phonon.mo != NULL) { delete phonon.mo; phonon.mo = NULL; } - if (!enableSound) return; + if (!enableSound) { + return; + } ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); @@ -191,23 +197,27 @@ void SoundNotifyPlugin::connectNotifications() _notificationList.append(_toRemoveNotifications); _toRemoveNotifications.clear(); - //first, reject empty args and unknown fields. - foreach(NotificationItem* notify, _notificationList) { - notify->_isPlayed = false; - notify->isNowPlaying=false; + // first, reject empty args and unknown fields. + foreach(NotificationItem * notify, _notificationList) { + notify->_isPlayed = false; + notify->isNowPlaying = false; - if(notify->mute()) continue; + if (notify->mute()) { + continue; + } // check is all sounds presented for notification, // if not - we must not subscribe to it at all - if(notify->toSoundList().isEmpty()) continue; + if (notify->toSoundList().isEmpty()) { + continue; + } - UAVDataObject* obj = dynamic_cast( objManager->getObject(notify->getDataObject()) ); - if (obj != NULL ) { + UAVDataObject *obj = dynamic_cast(objManager->getObject(notify->getDataObject())); + if (obj != NULL) { if (!lstNotifiedUAVObjects.contains(obj)) { lstNotifiedUAVObjects.append(obj); - connect(obj, SIGNAL(objectUpdated(UAVObject*)), - this, SLOT(on_arrived_Notification(UAVObject*)), + connect(obj, SIGNAL(objectUpdated(UAVObject *)), + this, SLOT(on_arrived_Notification(UAVObject *)), Qt::QueuedConnection); } } else { @@ -215,64 +225,71 @@ void SoundNotifyPlugin::connectNotifications() } } - if (_notificationList.isEmpty()) return; + if (_notificationList.isEmpty()) { + return; + } // set notification message to current event phonon.mo = Phonon::createPlayer(Phonon::NotificationCategory); phonon.mo->clearQueue(); phonon.firstPlay = true; QList audioOutputDevices = - Phonon::BackendCapabilities::availableAudioOutputDevices(); + Phonon::BackendCapabilities::availableAudioOutputDevices(); foreach(Phonon::AudioOutputDevice dev, audioOutputDevices) { qNotifyDebug() << "Notify: Audio Output device: " << dev.name() << " - " << dev.description(); } - connect(phonon.mo, SIGNAL(stateChanged(Phonon::State,Phonon::State)), - this, SLOT(stateChanged(Phonon::State,Phonon::State))); + connect(phonon.mo, SIGNAL(stateChanged(Phonon::State, Phonon::State)), + this, SLOT(stateChanged(Phonon::State, Phonon::State))); } void SoundNotifyPlugin::on_arrived_Notification(UAVObject *object) { - foreach(NotificationItem* ntf, _notificationList) { - if (object->getName() != ntf->getDataObject()) + foreach(NotificationItem * ntf, _notificationList) { + if (object->getName() != ntf->getDataObject()) { continue; + } // skip duplicate notifications - if (_nowPlayingNotification == ntf) + if (_nowPlayingNotification == ntf) { continue; + } // skip periodical notifications // this condition accepts: // 1. Periodical notifications played firstly; - // NOTE: At first time it will be played, then it played only by timer, - // when conditions became false firstStart flag has been cleared and - // notification can be accepted again; + // NOTE: At first time it will be played, then it played only by timer, + // when conditions became false firstStart flag has been cleared and + // notification can be accepted again; // 2. Once time notifications, they removed immediately after first playing; // 3. Instant notifications(played one by one without interval); if (ntf->retryValue() != NotificationItem::repeatInstantly && ntf->retryValue() != NotificationItem::repeatOncePerUpdate && - ntf->retryValue() != NotificationItem::repeatOnce && ntf->_isPlayed) - continue; + ntf->retryValue() != NotificationItem::repeatOnce && ntf->_isPlayed) { + continue; + } qNotifyDebug() << QString("new notification: | %1 | %2 | val1: %3 | val2: %4") - .arg(ntf->getDataObject()) - .arg(ntf->getObjectField()) - .arg(ntf->singleValue().toString()) - .arg(ntf->valueRange2()); + .arg(ntf->getDataObject()) + .arg(ntf->getObjectField()) + .arg(ntf->singleValue().toString()) + .arg(ntf->valueRange2()); checkNotificationRule(ntf, object); } - connect(object, SIGNAL(objectUpdated(UAVObject*)), - this, SLOT(on_arrived_Notification(UAVObject*)), Qt::UniqueConnection); + connect(object, SIGNAL(objectUpdated(UAVObject *)), + this, SLOT(on_arrived_Notification(UAVObject *)), Qt::UniqueConnection); } void SoundNotifyPlugin::on_timerRepeated_Notification() { - NotificationItem* notification = static_cast(sender()->parent()); - if (!notification) + NotificationItem *notification = static_cast(sender()->parent()); + + if (!notification) { return; + } // skip duplicate notifications // WARNING: generally we shoudn't ever trap here - // this means, that timer fires to early and notification overlap itself + // this means, that timer fires to early and notification overlap itself if (_nowPlayingNotification == notification) { qNotifyDebug() << "WARN: on_timerRepeated - notification was skipped!"; notification->restartTimer(); @@ -280,29 +297,32 @@ void SoundNotifyPlugin::on_timerRepeated_Notification() } qNotifyDebug() << QString("repeatTimer: %1% | %2 | %3").arg(notification->getDataObject()) - .arg(notification->getObjectField()) - .arg(notification->toString()); + .arg(notification->getObjectField()) + .arg(notification->toString()); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVObject* object = objManager->getObject(notification->getDataObject()); - if (object) - checkNotificationRule(notification,object); + UAVObject *object = objManager->getObject(notification->getDataObject()); + if (object) { + checkNotificationRule(notification, object); + } } void SoundNotifyPlugin::on_expiredTimer_Notification() { // fire expire timer - NotificationItem* notification = static_cast(sender()->parent()); - if(!notification) + NotificationItem *notification = static_cast(sender()->parent()); + + if (!notification) { return; + } notification->stopExpireTimer(); if (!_pendingNotifications.isEmpty()) { qNotifyDebug() << QString("expireTimer: %1% | %2 | %3").arg(notification->getDataObject()) - .arg(notification->getObjectField()) - .arg(notification->toString()); + .arg(notification->getObjectField()) + .arg(notification->toString()); _pendingNotifications.removeOne(notification); } @@ -312,19 +332,19 @@ void SoundNotifyPlugin::stateChanged(Phonon::State newstate, Phonon::State oldst { Q_UNUSED(oldstate) - //qNotifyDebug() << "File length (ms): " << phonon.mo->totalTime(); + // qNotifyDebug() << "File length (ms): " << phonon.mo->totalTime(); #ifndef Q_OS_WIN // This is a hack to force Linux to wait until the end of the // wav file before moving to the next in the queue. // I wish I did not have to go through a #define, but I did not // manage to make this work on both platforms any other way! - if (phonon.mo->totalTime()>0) + if (phonon.mo->totalTime() > 0) { phonon.mo->setTransitionTime(phonon.mo->totalTime()); + } #endif - if ((newstate == Phonon::PausedState) || - (newstate == Phonon::StoppedState)) - { + if ((newstate == Phonon::PausedState) || + (newstate == Phonon::StoppedState)) { qNotifyDebug() << "New State: " << QVariant(newstate).toString(); // assignment to NULL needed to detect that palying is finished @@ -332,16 +352,15 @@ void SoundNotifyPlugin::stateChanged(Phonon::State newstate, Phonon::State oldst // that notification has not overlap with itself _nowPlayingNotification = NULL; - if (!_pendingNotifications.isEmpty()) - { - NotificationItem* notification = _pendingNotifications.takeFirst(); + if (!_pendingNotifications.isEmpty()) { + NotificationItem *notification = _pendingNotifications.takeFirst(); qNotifyDebug_if(notification) << "play audioFree - " << notification->toString(); playNotification(notification); - qNotifyDebug()<<"end playNotification"; + qNotifyDebug() << "end playNotification"; } } else { - if (newstate == Phonon::ErrorState) { - if (phonon.mo->errorType()==0) { + if (newstate == Phonon::ErrorState) { + if (phonon.mo->errorType() == 0) { qDebug() << "Phonon::ErrorState: ErrorType = " << phonon.mo->errorType(); phonon.mo->clearQueue(); } @@ -351,10 +370,9 @@ void SoundNotifyPlugin::stateChanged(Phonon::State newstate, Phonon::State oldst bool checkRange(QString fieldValue, QString enumValue, QStringList /* values */, int direction) { - bool ret = false; - switch(direction) - { + + switch (direction) { case NotifyPluginOptionsPage::equal: ret = !QString::compare(enumValue, fieldValue, Qt::CaseInsensitive) ? true : false; break; @@ -362,16 +380,17 @@ bool checkRange(QString fieldValue, QString enumValue, QStringList /* values */, default: ret = true; break; - }; + } + ; return ret; } bool checkRange(double fieldValue, double min, double max, int direction) { bool ret = false; - //Q_ASSERT(min < max); - switch(direction) - { + + // Q_ASSERT(min < max); + switch (direction) { case NotifyPluginOptionsPage::equal: ret = (fieldValue == min); break; @@ -387,44 +406,48 @@ bool checkRange(double fieldValue, double min, double max, int direction) default: ret = (fieldValue > min) && (fieldValue < max); break; - }; + } + ; return ret; } -void SoundNotifyPlugin::checkNotificationRule(NotificationItem* notification, UAVObject* object) +void SoundNotifyPlugin::checkNotificationRule(NotificationItem *notification, UAVObject *object) { - if(notification->getDataObject()!=object->getName() || object->getField(notification->getObjectField())==NULL) + if (notification->getDataObject() != object->getName() || object->getField(notification->getObjectField()) == NULL) { return; - bool condition=false; + } + bool condition = false; - if (notification->mute()) + if (notification->mute()) { return; + } - int direction = notification->getCondition(); - QString fieldName = notification->getObjectField(); - UAVObjectField* field = object->getField(fieldName); + int direction = notification->getCondition(); + QString fieldName = notification->getObjectField(); + UAVObjectField *field = object->getField(fieldName); - if (field->getName().isEmpty()) + if (field->getName().isEmpty()) { return; + } QVariant value = field->getValue(); - if(UAVObjectField::ENUM == field->getType()) { - qNotifyDebug()<<"Check range ENUM"<singleValue().toString()<<"|"<getOptions()<<"|"<< - direction<singleValue().toString(), - field->getOptions(), - direction);; + if (UAVObjectField::ENUM == field->getType()) { + qNotifyDebug() << "Check range ENUM" << value.toString() << "|" << notification->singleValue().toString() << "|" << field->getOptions() << "|" << + direction << checkRange(value.toString(), + notification->singleValue().toString(), + field->getOptions(), + direction);; condition = checkRange(value.toString(), notification->singleValue().toString(), field->getOptions(), direction); } else { - qNotifyDebug()<<"Check range VAL"<singleValue().toString()<<"|"<getOptions()<<"|"<< - direction<singleValue().toDouble(), - notification->valueRange2(), - direction); + qNotifyDebug() << "Check range VAL" << value.toString() << "|" << notification->singleValue().toString() << "|" << field->getOptions() << "|" << + direction << checkRange(value.toDouble(), + notification->singleValue().toDouble(), + notification->valueRange2(), + direction); condition = checkRange(value.toDouble(), notification->singleValue().toDouble(), notification->valueRange2(), @@ -439,18 +462,19 @@ void SoundNotifyPlugin::checkNotificationRule(NotificationItem* notification, UA notification->setCurrentUpdatePlayed(false); return; } - if(notification->retryValue() == NotificationItem::repeatOncePerUpdate && notification->getCurrentUpdatePlayed()) + if (notification->retryValue() == NotificationItem::repeatOncePerUpdate && notification->getCurrentUpdatePlayed()) { return; + } if (!playNotification(notification)) { if (!_pendingNotifications.contains(notification) - && (_nowPlayingNotification != notification)) { + && (_nowPlayingNotification != notification)) { notification->stopTimer(); qNotifyDebug() << "add to pending list - " << notification->toString(); // if audio is busy, start expiration timer - //ms = (notification->getExpiredTimeout()[in sec])*1000 - //QxtTimer::singleShot(notification->getExpireTimeout()*1000, this, SLOT(expirationTimerHandler(NotificationItem*)), qVariantFromValue(notification)); + // ms = (notification->getExpiredTimeout()[in sec])*1000 + // QxtTimer::singleShot(notification->getExpireTimeout()*1000, this, SLOT(expirationTimerHandler(NotificationItem*)), qVariantFromValue(notification)); _pendingNotifications.append(notification); notification->startExpireTimer(); connect(notification->getExpireTimer(), SIGNAL(timeout()), @@ -459,34 +483,34 @@ void SoundNotifyPlugin::checkNotificationRule(NotificationItem* notification, UA } } -bool SoundNotifyPlugin::playNotification(NotificationItem* notification) +bool SoundNotifyPlugin::playNotification(NotificationItem *notification) { - if(!notification) + if (!notification) { return false; + } // Check: race condition, if phonon.mo got deleted don't go further - if (phonon.mo == NULL) + if (phonon.mo == NULL) { return false; + } - //qNotifyDebug() << "Phonon State: " << phonon.mo->state(); + // qNotifyDebug() << "Phonon State: " << phonon.mo->state(); - if ((phonon.mo->state()==Phonon::PausedState) - || (phonon.mo->state()==Phonon::StoppedState) - || phonon.firstPlay) - { + if ((phonon.mo->state() == Phonon::PausedState) + || (phonon.mo->state() == Phonon::StoppedState) + || phonon.firstPlay) { _nowPlayingNotification = notification; notification->stopExpireTimer(); if (notification->retryValue() == NotificationItem::repeatOnce) { _toRemoveNotifications.append(_notificationList.takeAt(_notificationList.indexOf(notification))); - } - else if(notification->retryValue() == NotificationItem::repeatOncePerUpdate) + } else if (notification->retryValue() == NotificationItem::repeatOncePerUpdate) { notification->setCurrentUpdatePlayed(true); - else { + } else { if (notification->retryValue() != NotificationItem::repeatInstantly) { QRegExp rxlen("(\\d+)"); QString value; - int timer_value=0; + int timer_value = 0; int pos = rxlen.indexIn(NotificationItem::retryValues.at(notification->retryValue())); if (pos > -1) { value = rxlen.cap(1); // "189" @@ -509,18 +533,18 @@ bool SoundNotifyPlugin::playNotification(NotificationItem* notification) } phonon.mo->clear(); qNotifyDebug() << "play: " << notification->toString(); - foreach (QString item, notification->toSoundList()) { + foreach(QString item, notification->toSoundList()) { Phonon::MediaSource *ms = new Phonon::MediaSource(item); + ms->setAutoDelete(true); phonon.mo->enqueue(*ms); } - qNotifyDebug()<<"begin play"; + qNotifyDebug() << "begin play"; phonon.mo->play(); - qNotifyDebug()<<"end play"; + qNotifyDebug() << "end play"; phonon.firstPlay = false; // On Linux, you sometimes have to nudge Phonon to play 1 time before // the state is not "Loading" anymore. return true; - } return false; diff --git a/ground/openpilotgcs/src/plugins/notify/notifyplugin.h b/ground/openpilotgcs/src/plugins/notify/notifyplugin.h index c9027bf82..d54126f29 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyplugin.h +++ b/ground/openpilotgcs/src/plugins/notify/notifyplugin.h @@ -3,31 +3,31 @@ * * @file notifyplugin.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 * @defgroup notifyplugin * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef SOUNDNOTIFYPLUGIN_H #define SOUNDNOTIFYPLUGIN_H -#include +#include #include #include "uavtalk/telemetrymanager.h" #include "uavobjectmanager.h" @@ -43,44 +43,55 @@ class NotifyPluginOptionsPage; typedef struct { - Phonon::MediaObject* mo; - NotificationItem* notify; - bool firstPlay; + Phonon::MediaObject *mo; + NotificationItem *notify; + bool firstPlay; } PhononObject, *pPhononObject; -class SoundNotifyPlugin : public Core::IConfigurablePlugin -{ +class SoundNotifyPlugin : public Core::IConfigurablePlugin { Q_OBJECT public: SoundNotifyPlugin(); ~SoundNotifyPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void readConfig( QSettings* qSettings, Core::UAVConfigInfo *configInfo); - void saveConfig( QSettings* qSettings, Core::UAVConfigInfo *configInfo); + bool initialize(const QStringList & arguments, QString *errorString); + void readConfig(QSettings *qSettings, Core::UAVConfigInfo *configInfo); + void saveConfig(QSettings *qSettings, Core::UAVConfigInfo *configInfo); void shutdown(); - QList getListNotifications() { return _notificationList; } - NotificationItem* getCurrentNotification(){ return ¤tNotification;} + QList getListNotifications() + { + return _notificationList; + } + NotificationItem *getCurrentNotification() + { + return ¤tNotification; + } - bool getEnableSound() const { return enableSound; } - void setEnableSound(bool value) {enableSound = value; } + bool getEnableSound() const + { + return enableSound; + } + void setEnableSound(bool value) + { + enableSound = value; + } private: Q_DISABLE_COPY(SoundNotifyPlugin) - bool playNotification(NotificationItem* notification); - void checkNotificationRule(NotificationItem* notification, UAVObject* object); + bool playNotification(NotificationItem *notification); + void checkNotificationRule(NotificationItem *notification, UAVObject *object); private slots: - void onTelemetryManagerAdded(QObject* obj); + void onTelemetryManagerAdded(QObject *obj); void onAutopilotDisconnect(); void connectNotifications(); - void updateNotificationList(QList list); + void updateNotificationList(QList list); void resetNotification(void); void on_arrived_Notification(UAVObject *object); void on_timerRepeated_Notification(void); @@ -90,17 +101,17 @@ private slots: private: bool enableSound; - QList lstNotifiedUAVObjects; - QList _notificationList; - QList _pendingNotifications; - QList _toRemoveNotifications; + QList lstNotifiedUAVObjects; + QList _notificationList; + QList _pendingNotifications; + QList _toRemoveNotifications; NotificationItem currentNotification; - NotificationItem* _nowPlayingNotification; + NotificationItem *_nowPlayingNotification; PhononObject phonon; - NotifyPluginOptionsPage* mop; - TelemetryManager* telMngr; -}; + NotifyPluginOptionsPage *mop; + TelemetryManager *telMngr; +}; #endif // SOUNDNOTIFYPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.cpp b/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.cpp index 81b365741..454fafe94 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.cpp @@ -25,27 +25,25 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "notifypluginfactory.h" -//#include "notifypluginwidget.h" +// #include "notifypluginwidget.h" #include "notifyplugin.h" #include "notifypluginconfiguration.h" #include "notifypluginoptionspage.h" #include NotifyPluginFactory::NotifyPluginFactory(QObject *parent) : - IUAVGadgetFactory(QString("Notify Plugin"), - tr("Notify Plugin"), - parent) -{ -} + IUAVGadgetFactory(QString("Notify Plugin"), + tr("Notify Plugin"), + parent) +{} NotifyPluginFactory::~NotifyPluginFactory() -{ -} +{} -Core::IUAVGadget* NotifyPluginFactory::createGadget(QWidget *parent) +Core::IUAVGadget *NotifyPluginFactory::createGadget(QWidget *parent) { - // NotifyPluginWidget* gadgetWidget = new NotifyPluginWidget(parent); - return (Core::IUAVGadget*)0;//new NotifyPlugin(QString("NotifyPlugin"), gadgetWidget, parent); + // NotifyPluginWidget* gadgetWidget = new NotifyPluginWidget(parent); + return (Core::IUAVGadget *)0; // new NotifyPlugin(QString("NotifyPlugin"), gadgetWidget, parent); } IUAVGadgetConfiguration *NotifyPluginFactory::createConfiguration(const QByteArray &state) @@ -55,7 +53,5 @@ IUAVGadgetConfiguration *NotifyPluginFactory::createConfiguration(const QByteArr IOptionsPage *NotifyPluginFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new NotifyPluginOptionsPage(qobject_cast(config)); + return new NotifyPluginOptionsPage(qobject_cast(config)); } - - diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.h b/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.h index 8784d3943..a14d930e8 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.h +++ b/ground/openpilotgcs/src/plugins/notify/notifypluginfactory.h @@ -38,8 +38,7 @@ class IUAVGadgetFactory; using namespace Core; -class NotifyPluginFactory : public IUAVGadgetFactory -{ +class NotifyPluginFactory : public IUAVGadgetFactory { Q_OBJECT public: NotifyPluginFactory(QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/notify/notifyplugingadget.h b/ground/openpilotgcs/src/plugins/notify/notifyplugingadget.h index f6aa2f912..ec780ad54 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyplugingadget.h +++ b/ground/openpilotgcs/src/plugins/notify/notifyplugingadget.h @@ -29,27 +29,26 @@ #define NOTIFYPLUGINGADGET_H #include -//#include "NotifyPlugingadgetwidget.h" +// #include "NotifyPlugingadgetwidget.h" class IUAVGadget; class QWidget; class QString; -//class NotifyPluginGadgetWidget; +// class NotifyPluginGadgetWidget; using namespace Core; -class NotifyPluginGadget : public Core::IUAVGadget -{ +class NotifyPluginGadget : public Core::IUAVGadget { Q_OBJECT public: NotifyPluginGadget(QString classId, NotifyPluginGadgetWidget *widget, QWidget *parent = 0); ~NotifyPluginGadget(); - // QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + // QWidget *widget() { return m_widget; } + void loadConfiguration(IUAVGadgetConfiguration *config); private: -// NotifyPluginGadgetWidget *m_widget; +// NotifyPluginGadgetWidget *m_widget; }; diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp index 38df0eb52..7ef248ad2 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp @@ -52,7 +52,7 @@ QStringList NotifyPluginOptionsPage::conditionValues; NotifyPluginOptionsPage::NotifyPluginOptionsPage(QObject *parent) : IOptionsPage(parent) , _objManager(*ExtensionSystem::PluginManager::instance()->getObject()) - , _owner(qobject_cast(parent)) + , _owner(qobject_cast(parent)) , _dynamicFieldCondition(NULL) , _dynamicFieldWidget(NULL) , _dynamicFieldType(-1) @@ -60,11 +60,10 @@ NotifyPluginOptionsPage::NotifyPluginOptionsPage(QObject *parent) , _form(NULL) , _selectedNotification(NULL) { - NotifyPluginOptionsPage::conditionValues.insert(equal,tr("Equal to")); - NotifyPluginOptionsPage::conditionValues.insert(bigger,tr("Large than")); - NotifyPluginOptionsPage::conditionValues.insert(smaller,tr("Lower than")); - NotifyPluginOptionsPage::conditionValues.insert(inrange,tr("In range")); - + NotifyPluginOptionsPage::conditionValues.insert(equal, tr("Equal to")); + NotifyPluginOptionsPage::conditionValues.insert(bigger, tr("Large than")); + NotifyPluginOptionsPage::conditionValues.insert(smaller, tr("Lower than")); + NotifyPluginOptionsPage::conditionValues.insert(inrange, tr("In range")); } NotifyPluginOptionsPage::~NotifyPluginOptionsPage() @@ -73,27 +72,27 @@ NotifyPluginOptionsPage::~NotifyPluginOptionsPage() QWidget *NotifyPluginOptionsPage::createPage(QWidget * /* parent */) { _optionsPage.reset(new Ui::NotifyPluginOptionsPage()); - //main widget - QWidget* optionsPageWidget = new QWidget; - _dynamicFieldWidget = NULL; + // main widget + QWidget *optionsPageWidget = new QWidget; + _dynamicFieldWidget = NULL; _dynamicFieldCondition = NULL; resetFieldType(); - //save ref to form, needed for binding dynamic fields in future + // save ref to form, needed for binding dynamic fields in future _form = optionsPageWidget; - //main layout + // main layout _optionsPage->setupUi(optionsPageWidget); _optionsPage->SoundDirectoryPathChooser->setExpectedKind(Utils::PathChooser::Directory); _optionsPage->SoundDirectoryPathChooser->setPromptDialogTitle(tr("Choose sound collection directory")); - connect(_optionsPage->SoundDirectoryPathChooser, SIGNAL(changed(const QString&)), - this, SLOT(on_clicked_buttonSoundFolder(const QString&))); - connect(_optionsPage->SoundCollectionList, SIGNAL(currentIndexChanged (int)), + connect(_optionsPage->SoundDirectoryPathChooser, SIGNAL(changed(const QString &)), + this, SLOT(on_clicked_buttonSoundFolder(const QString &))); + connect(_optionsPage->SoundCollectionList, SIGNAL(currentIndexChanged(int)), this, SLOT(on_changedIndex_soundLanguage(int))); - connect(this, SIGNAL(updateNotifications(QList)), - _owner, SLOT(updateNotificationList(QList))); - //connect(this, SIGNAL(resetNotification()),owner, SLOT(resetNotification())); + connect(this, SIGNAL(updateNotifications(QList)), + _owner, SLOT(updateNotificationList(QList))); + // connect(this, SIGNAL(resetNotification()),owner, SLOT(resetNotification())); _privListNotifications = _owner->getListNotifications(); @@ -110,7 +109,7 @@ QWidget *NotifyPluginOptionsPage::createPage(QWidget * /* parent */) int curr_row = _privListNotifications.indexOf(_selectedNotification); _notifyRulesSelection->setCurrentIndex(_notifyRulesModel->index(curr_row, 0, QModelIndex()), - QItemSelectionModel::ClearAndSelect | QItemSelectionModel::Rows); + QItemSelectionModel::ClearAndSelect | QItemSelectionModel::Rows); return optionsPageWidget; } @@ -127,8 +126,8 @@ void NotifyPluginOptionsPage::finish() disconnect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_changedIndex_UAVField(QString))); - disconnect(_testSound.data(),SIGNAL(stateChanged(Phonon::State,Phonon::State)), - this,SLOT(on_changed_playButtonText(Phonon::State,Phonon::State))); + disconnect(_testSound.data(), SIGNAL(stateChanged(Phonon::State, Phonon::State)), + this, SLOT(on_changed_playButtonText(Phonon::State, Phonon::State))); if (_testSound) { _testSound->stop(); _testSound->clear(); @@ -157,8 +156,8 @@ void NotifyPluginOptionsPage::initButtons() void NotifyPluginOptionsPage::initPhononPlayer() { _testSound.reset(Phonon::createPlayer(Phonon::NotificationCategory)); - connect(_testSound.data(),SIGNAL(stateChanged(Phonon::State,Phonon::State)), - this,SLOT(on_changed_playButtonText(Phonon::State,Phonon::State))); + connect(_testSound.data(), SIGNAL(stateChanged(Phonon::State, Phonon::State)), + this, SLOT(on_changed_playButtonText(Phonon::State, Phonon::State))); connect(_testSound.data(), SIGNAL(finished(void)), this, SLOT(on_FinishedPlaying(void))); } @@ -169,8 +168,8 @@ void NotifyPluginOptionsPage::initRulesTable() _notifyRulesModel.reset(new NotifyTableModel(_privListNotifications)); _notifyRulesSelection = new QItemSelectionModel(_notifyRulesModel.data()); - connect(_notifyRulesSelection, SIGNAL(selectionChanged ( const QItemSelection &, const QItemSelection & )), - this, SLOT(on_changedSelection_notifyTable( const QItemSelection & , const QItemSelection & ))); + connect(_notifyRulesSelection, SIGNAL(selectionChanged(const QItemSelection &, const QItemSelection &)), + this, SLOT(on_changedSelection_notifyTable(const QItemSelection &, const QItemSelection &))); connect(this, SIGNAL(entryUpdated(int)), _notifyRulesModel.data(), SLOT(entryUpdated(int))); @@ -179,26 +178,26 @@ void NotifyPluginOptionsPage::initRulesTable() _optionsPage->notifyRulesView->setItemDelegate(new NotifyItemDelegate(this)); _optionsPage->notifyRulesView->resizeRowsToContents(); - _optionsPage->notifyRulesView->setColumnWidth(eMessageName,200); - _optionsPage->notifyRulesView->setColumnWidth(eRepeatValue,120); - _optionsPage->notifyRulesView->setColumnWidth(eExpireTimer,100); - _optionsPage->notifyRulesView->setColumnWidth(eTurnOn,60); + _optionsPage->notifyRulesView->setColumnWidth(eMessageName, 200); + _optionsPage->notifyRulesView->setColumnWidth(eRepeatValue, 120); + _optionsPage->notifyRulesView->setColumnWidth(eExpireTimer, 100); + _optionsPage->notifyRulesView->setColumnWidth(eTurnOn, 60); _optionsPage->notifyRulesView->setDragEnabled(true); _optionsPage->notifyRulesView->setAcceptDrops(true); _optionsPage->notifyRulesView->setDropIndicatorShown(true); _optionsPage->notifyRulesView->setDragDropMode(QAbstractItemView::InternalMove); } -UAVObjectField* NotifyPluginOptionsPage::getObjectFieldFromSelected() +UAVObjectField *NotifyPluginOptionsPage::getObjectFieldFromSelected() { return (_currUAVObject) ? _currUAVObject->getField(_selectedNotification->getObjectField()) : NULL; } -void NotifyPluginOptionsPage::setSelectedNotification(NotificationItem* ntf) +void NotifyPluginOptionsPage::setSelectedNotification(NotificationItem *ntf) { _selectedNotification = ntf; - _currUAVObject = dynamic_cast(_objManager.getObject(_selectedNotification->getDataObject())); - if(!_currUAVObject) { + _currUAVObject = dynamic_cast(_objManager.getObject(_selectedNotification->getDataObject())); + if (!_currUAVObject) { qNotifyDebug() << "no such UAVObject: " << _selectedNotification->getDataObject(); } } @@ -209,12 +208,13 @@ void NotifyPluginOptionsPage::addDynamicFieldLayout() // thus it doesn't use in this field directly QSizePolicy labelSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + labelSizePolicy.setHorizontalStretch(0); labelSizePolicy.setVerticalStretch(0); -// labelSizePolicy.setHeightForWidth(UAVObject->sizePolicy().hasHeightForWidth()); +// labelSizePolicy.setHeightForWidth(UAVObject->sizePolicy().hasHeightForWidth()); - QLabel* labelSayOrder = new QLabel("Say order ", _form); + QLabel *labelSayOrder = new QLabel("Say order ", _form); labelSayOrder->setSizePolicy(labelSizePolicy); _optionsPage->dynamicValueLayout->addWidget(labelSayOrder); @@ -222,24 +222,24 @@ void NotifyPluginOptionsPage::addDynamicFieldLayout() _optionsPage->dynamicValueLayout->addWidget(_sayOrder); _sayOrder->addItems(NotificationItem::sayOrderValues); - QLabel* labelValueIs = new QLabel("Value is ", _form); + QLabel *labelValueIs = new QLabel("Value is ", _form); labelValueIs->setSizePolicy(labelSizePolicy); _optionsPage->dynamicValueLayout->addWidget(labelValueIs); _dynamicFieldCondition = new QComboBox(_form); _optionsPage->dynamicValueLayout->addWidget(_dynamicFieldCondition); - UAVObjectField* field = getObjectFieldFromSelected(); + UAVObjectField *field = getObjectFieldFromSelected(); addDynamicField(field); } -void NotifyPluginOptionsPage::addDynamicField(UAVObjectField* objField) +void NotifyPluginOptionsPage::addDynamicField(UAVObjectField *objField) { - if(!objField) { + if (!objField) { qNotifyDebug() << "addDynamicField | input objField == NULL"; return; } if (objField->getType() == _dynamicFieldType) { - if (QComboBox* fieldValue = dynamic_cast(_dynamicFieldWidget)) { + if (QComboBox * fieldValue = dynamic_cast(_dynamicFieldWidget)) { fieldValue->clear(); QStringList enumValues(objField->getOptions()); fieldValue->addItems(enumValues); @@ -256,20 +256,21 @@ void NotifyPluginOptionsPage::addDynamicField(UAVObjectField* objField) _dynamicFieldCondition->removeItem(smaller); _dynamicFieldCondition->removeItem(bigger); } - int cond=_selectedNotification->getCondition(); - if(cond<0) + int cond = _selectedNotification->getCondition(); + if (cond < 0) { return; + } _dynamicFieldCondition->setCurrentIndex(_dynamicFieldCondition->findText(NotifyPluginOptionsPage::conditionValues.at(cond))); - connect(_dynamicFieldCondition, SIGNAL(currentIndexChanged(QString)), - this, SLOT(on_changedIndex_rangeValue(QString))); + connect(_dynamicFieldCondition, SIGNAL(currentIndexChanged(QString)), + this, SLOT(on_changedIndex_rangeValue(QString))); addDynamicFieldWidget(objField); } -void NotifyPluginOptionsPage::addDynamicFieldWidget(UAVObjectField* objField) +void NotifyPluginOptionsPage::addDynamicFieldWidget(UAVObjectField *objField) { - if(!objField) { + if (!objField) { qNotifyDebug() << "objField == NULL!"; return; } @@ -286,49 +287,50 @@ void NotifyPluginOptionsPage::addDynamicFieldWidget(UAVObjectField* objField) sizePolicy.setVerticalStretch(0); _dynamicFieldType = objField->getType(); - switch(_dynamicFieldType) - { + switch (_dynamicFieldType) { case UAVObjectField::ENUM: - { - _dynamicFieldWidget = new QComboBox(_form); - QStringList enumValues(objField->getOptions()); - (dynamic_cast(_dynamicFieldWidget))->addItems(enumValues); - } - break; + { + _dynamicFieldWidget = new QComboBox(_form); + QStringList enumValues(objField->getOptions()); + (dynamic_cast(_dynamicFieldWidget))->addItems(enumValues); + } + break; default: Q_ASSERT(_dynamicFieldCondition); if (NotifyPluginOptionsPage::conditionValues.indexOf(_dynamicFieldCondition->currentText()) == NotifyPluginOptionsPage::inrange) { _dynamicFieldWidget = new QLineEdit(_form); - (static_cast(_dynamicFieldWidget))->setInputMask("#999.99 : #999.99;"); - (static_cast(_dynamicFieldWidget))->setText("0000000000"); - (static_cast(_dynamicFieldWidget))->setCursorPosition(0); + (static_cast(_dynamicFieldWidget))->setInputMask("#999.99 : #999.99;"); + (static_cast(_dynamicFieldWidget))->setText("0000000000"); + (static_cast(_dynamicFieldWidget))->setCursorPosition(0); } else { _dynamicFieldWidget = new QDoubleSpinBox(_form); - (dynamic_cast(_dynamicFieldWidget))->setRange(-999.99, 999.99); + (dynamic_cast(_dynamicFieldWidget))->setRange(-999.99, 999.99); } break; - }; + } + ; enum { eDynamicFieldWidth = 100 }; _dynamicFieldWidget->setSizePolicy(sizePolicy); _dynamicFieldWidget->setFixedWidth(eDynamicFieldWidth); _optionsPage->dynamicValueLayout->addWidget(_dynamicFieldWidget); } -void NotifyPluginOptionsPage::setDynamicFieldValue(NotificationItem* notification) +void NotifyPluginOptionsPage::setDynamicFieldValue(NotificationItem *notification) { - if (QDoubleSpinBox* seValue = dynamic_cast(_dynamicFieldWidget)) + if (QDoubleSpinBox * seValue = dynamic_cast(_dynamicFieldWidget)) { seValue->setValue(notification->singleValue().toDouble()); - else { - if (QComboBox* cbValue = dynamic_cast(_dynamicFieldWidget)) { + } else { + if (QComboBox * cbValue = dynamic_cast(_dynamicFieldWidget)) { int idx = cbValue->findText(notification->singleValue().toString()); - if(-1 != idx) + if (-1 != idx) { cbValue->setCurrentIndex(idx); + } } else { - if (QLineEdit* rangeValue = dynamic_cast(_dynamicFieldWidget)) { + if (QLineEdit * rangeValue = dynamic_cast(_dynamicFieldWidget)) { QString str = QString("%1%2").arg(notification->singleValue().toDouble(), 5, 'f', 2, '0') - .arg(notification->valueRange2(), 5, 'f', 2, '0'); + .arg(notification->valueRange2(), 5, 'f', 2, '0'); rangeValue->setText(str); } else { qNotifyDebug() << "NotifyPluginOptionsPage::setDynamicValueField | unknown _fieldValue: " << _dynamicFieldWidget; @@ -342,7 +344,7 @@ void NotifyPluginOptionsPage::resetFieldType() _dynamicFieldType = -1; } -void NotifyPluginOptionsPage::getOptionsPageValues(NotificationItem* notification) +void NotifyPluginOptionsPage::getOptionsPageValues(NotificationItem *notification) { Q_ASSERT(notification); notification->setSoundCollectionPath(_optionsPage->SoundDirectoryPathChooser->path()); @@ -354,13 +356,13 @@ void NotifyPluginOptionsPage::getOptionsPageValues(NotificationItem* notificatio notification->setSound3(_optionsPage->Sound3->currentText()); notification->setSayOrder(_sayOrder->currentIndex()); notification->setCondition(NotifyPluginOptionsPage::conditionValues.indexOf(_dynamicFieldCondition->currentText())); - if (QDoubleSpinBox* spinValue = dynamic_cast(_dynamicFieldWidget)) + if (QDoubleSpinBox * spinValue = dynamic_cast(_dynamicFieldWidget)) { notification->setSingleValue(spinValue->value()); - else { - if (QComboBox* comboBoxValue = dynamic_cast(_dynamicFieldWidget)) + } else { + if (QComboBox * comboBoxValue = dynamic_cast(_dynamicFieldWidget)) { notification->setSingleValue(comboBoxValue->currentText()); - else { - if (QLineEdit* rangeValue = dynamic_cast(_dynamicFieldWidget)) { + } else { + if (QLineEdit * rangeValue = dynamic_cast(_dynamicFieldWidget)) { QString str = rangeValue->text(); QStringList range = str.split(':'); notification->setSingleValue(range.at(0).toDouble()); @@ -370,13 +372,13 @@ void NotifyPluginOptionsPage::getOptionsPageValues(NotificationItem* notificatio } } -void NotifyPluginOptionsPage::updateConfigView(NotificationItem* notification) +void NotifyPluginOptionsPage::updateConfigView(NotificationItem *notification) { Q_ASSERT(notification); disconnect(_optionsPage->UAVObject, SIGNAL(currentIndexChanged(QString)), - this, SLOT(on_changedIndex_UAVObject(QString))); + this, SLOT(on_changedIndex_UAVObject(QString))); disconnect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), - this, SLOT(on_changedIndex_UAVField(QString))); + this, SLOT(on_changedIndex_UAVField(QString))); QString path = notification->getSoundCollectionPath(); if (path.isEmpty()) { @@ -392,9 +394,9 @@ void NotifyPluginOptionsPage::updateConfigView(NotificationItem* notification) } // Fills the combo boxes for the UAVObjects - QList< QList > objList = _objManager.getDataObjects(); - foreach (QList list, objList) { - foreach (UAVDataObject* obj, list) { + QList< QList > objList = _objManager.getDataObjects(); + foreach(QList list, objList) { + foreach(UAVDataObject * obj, list) { _optionsPage->UAVObject->addItem(obj->getName()); } } @@ -404,10 +406,10 @@ void NotifyPluginOptionsPage::updateConfigView(NotificationItem* notification) } _optionsPage->UAVObjectField->clear(); - if(_currUAVObject) { - QList fieldList = _currUAVObject->getFields(); - foreach (UAVObjectField* field, fieldList) - _optionsPage->UAVObjectField->addItem(field->getName()); + if (_currUAVObject) { + QList fieldList = _currUAVObject->getFields(); + foreach(UAVObjectField * field, fieldList) + _optionsPage->UAVObjectField->addItem(field->getName()); } if (-1 != _optionsPage->UAVObjectField->findText(notification->getObjectField())) { @@ -437,9 +439,10 @@ void NotifyPluginOptionsPage::updateConfigView(NotificationItem* notification) _optionsPage->SoundCollectionList->setCurrentIndex(_optionsPage->SoundCollectionList->findText("default")); _optionsPage->Sound3->setCurrentIndex(_optionsPage->Sound3->findText(notification->getSound3())); } - int cond=notification->getCondition(); - if(cond<0) + int cond = notification->getCondition(); + if (cond < 0) { return; + } _dynamicFieldCondition->setCurrentIndex(_dynamicFieldCondition->findText(NotifyPluginOptionsPage::conditionValues.at(cond))); _sayOrder->setCurrentIndex(notification->getSayOrder()); @@ -450,13 +453,12 @@ void NotifyPluginOptionsPage::updateConfigView(NotificationItem* notification) this, SLOT(on_changedIndex_UAVObject(QString))); connect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_changedIndex_UAVField(QString))); - } void NotifyPluginOptionsPage::on_changedIndex_rangeValue(QString /* rangeStr */) { Q_ASSERT(_dynamicFieldWidget); - UAVObjectField* field = getObjectFieldFromSelected(); + UAVObjectField *field = getObjectFieldFromSelected(); Q_ASSERT(!!field); addDynamicFieldWidget(field); setDynamicFieldValue(_selectedNotification); @@ -467,22 +469,21 @@ void NotifyPluginOptionsPage::on_changedIndex_UAVField(QString field) resetFieldType(); Q_ASSERT(_currUAVObject); addDynamicField(_currUAVObject->getField(field)); - } void NotifyPluginOptionsPage::on_changedIndex_UAVObject(QString val) { resetFieldType(); - _currUAVObject = dynamic_cast( _objManager.getObject(val) ); - if(!_currUAVObject) { + _currUAVObject = dynamic_cast(_objManager.getObject(val)); + if (!_currUAVObject) { qNotifyDebug() << "on_UAVObject_indexChanged | no such UAVOBject"; return; } - QList fieldList = _currUAVObject->getFields(); + QList fieldList = _currUAVObject->getFields(); disconnect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_changedIndex_UAVField(QString))); _optionsPage->UAVObjectField->clear(); - foreach (UAVObjectField* field, fieldList) { - _optionsPage->UAVObjectField->addItem(field->getName()); + foreach(UAVObjectField * field, fieldList) { + _optionsPage->UAVObjectField->addItem(field->getName()); } connect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_changedIndex_UAVField(QString))); _selectedNotification->setObjectField(fieldList.at(0)->getName()); @@ -494,7 +495,7 @@ void NotifyPluginOptionsPage::on_changedIndex_soundLanguage(int index) { _optionsPage->SoundCollectionList->setCurrentIndex(index); QString collectionPath = _optionsPage->SoundDirectoryPathChooser->path() - + QDir::toNativeSeparators("/" + _optionsPage->SoundCollectionList->currentText()); + + QDir::toNativeSeparators("/" + _optionsPage->SoundCollectionList->currentText()); QDir dirPath(collectionPath); QStringList filters; @@ -514,29 +515,30 @@ void NotifyPluginOptionsPage::on_changedIndex_soundLanguage(int index) } -void NotifyPluginOptionsPage::on_changed_playButtonText(Phonon::State newstate, Phonon::State /* oldstate */) +void NotifyPluginOptionsPage::on_changed_playButtonText(Phonon::State newstate, Phonon::State /* oldstate */) { - //Q_ASSERT(Phonon::ErrorState != newstate); + // Q_ASSERT(Phonon::ErrorState != newstate); - if (newstate == Phonon::PausedState || newstate == Phonon::StoppedState) { + if (newstate == Phonon::PausedState || newstate == Phonon::StoppedState) { _optionsPage->buttonPlayNotification->setText("Play"); _optionsPage->buttonPlayNotification->setIcon(QPixmap(":/notify/images/play.png")); } else { - if (newstate == Phonon::PlayingState) { + if (newstate == Phonon::PlayingState) { _optionsPage->buttonPlayNotification->setText("Stop"); _optionsPage->buttonPlayNotification->setIcon(QPixmap(":/notify/images/stop.png")); } } } -void NotifyPluginOptionsPage::on_changedSelection_notifyTable(const QItemSelection & selected, const QItemSelection & /* deselected */ ) +void NotifyPluginOptionsPage::on_changedSelection_notifyTable(const QItemSelection & selected, const QItemSelection & /* deselected */) { bool select = false; + _testSound->stop(); if (selected.indexes().size()) { select = true; setSelectedNotification(_privListNotifications.at(selected.indexes().at(0).row())); - UAVObjectField* field = getObjectFieldFromSelected(); + UAVObjectField *field = getObjectFieldFromSelected(); addDynamicField(field); updateConfigView(_selectedNotification); } @@ -546,17 +548,19 @@ void NotifyPluginOptionsPage::on_changedSelection_notifyTable(const QItemSelecti _optionsPage->buttonPlayNotification->setEnabled(select); } -void NotifyPluginOptionsPage::on_clicked_buttonSoundFolder(const QString& path) +void NotifyPluginOptionsPage::on_clicked_buttonSoundFolder(const QString & path) { QDir dirPath(path); QStringList listDirCollections = dirPath.entryList(QDir::AllDirs | QDir::NoDotAndDotDot); + _optionsPage->SoundCollectionList->clear(); _optionsPage->SoundCollectionList->addItems(listDirCollections); } void NotifyPluginOptionsPage::on_clicked_buttonTestSoundNotification() { - NotificationItem* notification = NULL; + NotificationItem *notification = NULL; + qNotifyDebug() << "on_buttonTestSoundNotification_clicked"; Q_ASSERT(-1 != _notifyRulesSelection->currentIndex().row()); _testSound->clearQueue(); @@ -575,11 +579,11 @@ void NotifyPluginOptionsPage::on_clicked_buttonTestSoundNotification() void NotifyPluginOptionsPage::on_clicked_buttonAddNotification() { - NotificationItem* notification = new NotificationItem; + NotificationItem *notification = new NotificationItem; if (_optionsPage->SoundDirectoryPathChooser->path().isEmpty()) { - QPalette textPalette=_optionsPage->SoundDirectoryPathChooser->palette(); - textPalette.setColor(QPalette::Normal,QPalette::Text, Qt::red); + QPalette textPalette = _optionsPage->SoundDirectoryPathChooser->palette(); + textPalette.setColor(QPalette::Normal, QPalette::Text, Qt::red); _optionsPage->SoundDirectoryPathChooser->setPalette(textPalette); _optionsPage->SoundDirectoryPathChooser->setPath("please select sound collection folder"); delete notification; @@ -587,8 +591,8 @@ void NotifyPluginOptionsPage::on_clicked_buttonAddNotification() } getOptionsPageValues(notification); - if ( ((!_optionsPage->Sound2->currentText().isEmpty()) && (_sayOrder->currentText()=="After second")) - || ((!_optionsPage->Sound3->currentText().isEmpty()) && (_sayOrder->currentText()=="After third")) ) { + if (((!_optionsPage->Sound2->currentText().isEmpty()) && (_sayOrder->currentText() == "After second")) + || ((!_optionsPage->Sound3->currentText().isEmpty()) && (_sayOrder->currentText() == "After third"))) { delete notification; return; } else { @@ -596,8 +600,8 @@ void NotifyPluginOptionsPage::on_clicked_buttonAddNotification() } _notifyRulesModel->entryAdded(notification); - _notifyRulesSelection->setCurrentIndex(_notifyRulesModel->index(_privListNotifications.size()-1,0,QModelIndex()), - QItemSelectionModel::ClearAndSelect | QItemSelectionModel::Rows); + _notifyRulesSelection->setCurrentIndex(_notifyRulesModel->index(_privListNotifications.size() - 1, 0, QModelIndex()), + QItemSelectionModel::ClearAndSelect | QItemSelectionModel::Rows); } void NotifyPluginOptionsPage::on_clicked_buttonDeleteNotification() @@ -605,8 +609,7 @@ void NotifyPluginOptionsPage::on_clicked_buttonDeleteNotification() _notifyRulesModel->removeRow(_notifyRulesSelection->currentIndex().row()); if (!_notifyRulesModel->rowCount() && (_notifyRulesSelection->currentIndex().row() > 0 - && _notifyRulesSelection->currentIndex().row() < _notifyRulesModel->rowCount()) ) - { + && _notifyRulesSelection->currentIndex().row() < _notifyRulesModel->rowCount())) { _optionsPage->buttonDelete->setEnabled(false); _optionsPage->buttonModify->setEnabled(false); _optionsPage->buttonPlayNotification->setEnabled(false); @@ -615,26 +618,27 @@ void NotifyPluginOptionsPage::on_clicked_buttonDeleteNotification() void NotifyPluginOptionsPage::on_clicked_buttonModifyNotification() { - NotificationItem* notification = new NotificationItem; + NotificationItem *notification = new NotificationItem; + getOptionsPageValues(notification); notification->setRetryValue(_privListNotifications.at(_notifyRulesSelection->currentIndex().row())->retryValue()); notification->setLifetime(_privListNotifications.at(_notifyRulesSelection->currentIndex().row())->lifetime()); notification->setMute(_privListNotifications.at(_notifyRulesSelection->currentIndex().row())->mute()); - _privListNotifications.replace(_notifyRulesSelection->currentIndex().row(),notification); + _privListNotifications.replace(_notifyRulesSelection->currentIndex().row(), notification); entryUpdated(_notifyRulesSelection->currentIndex().row()); } -void NotifyPluginOptionsPage::on_FinishedPlaying() +void NotifyPluginOptionsPage::on_FinishedPlaying() { _testSound->clear(); } void NotifyPluginOptionsPage::on_toggled_checkEnableSound(bool state) { - bool state1 = 1^state; + bool state1 = 1 ^ state; - QList listOutputs = _testSound->outputPaths(); - Phonon::AudioOutput * audioOutput = (Phonon::AudioOutput*)listOutputs.last().sink(); + QList listOutputs = _testSound->outputPaths(); + Phonon::AudioOutput *audioOutput = (Phonon::AudioOutput *)listOutputs.last().sink(); audioOutput->setMuted(state1); } diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.h b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.h index fb3c2885d..adeadde9a 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.h +++ b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.h @@ -50,23 +50,34 @@ class NotificationItem; class SoundNotifyPlugin; namespace Ui { - class NotifyPluginOptionsPage; +class NotifyPluginOptionsPage; }; using namespace Core; -class NotifyPluginOptionsPage : public IOptionsPage -{ +class NotifyPluginOptionsPage : public IOptionsPage { Q_OBJECT public: - enum {equal,bigger,smaller,inrange}; + enum { equal, bigger, smaller, inrange }; explicit NotifyPluginOptionsPage(QObject *parent = 0); ~NotifyPluginOptionsPage(); - QString id() const { return QLatin1String("settings"); } - QString trName() const { return tr("settings"); } - QString category() const { return QLatin1String("Notify Plugin");} - QString trCategory() const { return tr("Notify Plugin");} + QString id() const + { + return QLatin1String("settings"); + } + QString trName() const + { + return tr("settings"); + } + QString category() const + { + return QLatin1String("Notify Plugin"); + } + QString trCategory() const + { + return tr("Notify Plugin"); + } QWidget *createPage(QWidget *parent); void apply(); @@ -75,7 +86,7 @@ public: static QStringList conditionValues; signals: - void updateNotifications(QList list); + void updateNotifications(QList list); void entryUpdated(int index); private slots: @@ -88,10 +99,10 @@ private slots: * We can use continuous selection, to select simultaneously * multiple rows to move them(using drag & drop) inside table ranges. */ - void on_changedSelection_notifyTable( const QItemSelection & selected, const QItemSelection & deselected ); + void on_changedSelection_notifyTable(const QItemSelection & selected, const QItemSelection & deselected); void on_changedIndex_soundLanguage(int index); - void on_clicked_buttonSoundFolder(const QString& path); + void on_clicked_buttonSoundFolder(const QString & path); void on_changedIndex_UAVObject(QString val); void on_changedIndex_UAVField(QString val); void on_changed_playButtonText(Phonon::State newstate, Phonon::State oldstate); @@ -115,30 +126,30 @@ private: void initPhononPlayer(); void initRulesTable(); - void setSelectedNotification(NotificationItem* ntf); + void setSelectedNotification(NotificationItem *ntf); void resetValueRange(); void resetFieldType(); - void updateConfigView(NotificationItem* notification); - void getOptionsPageValues(NotificationItem* notification); - UAVObjectField* getObjectFieldFromPage(); - UAVObjectField* getObjectFieldFromSelected(); + void updateConfigView(NotificationItem *notification); + void getOptionsPageValues(NotificationItem *notification); + UAVObjectField *getObjectFieldFromPage(); + UAVObjectField *getObjectFieldFromSelected(); void addDynamicFieldLayout(); - void addDynamicField(UAVObjectField* objField); - void addDynamicFieldWidget(UAVObjectField* objField); - void setDynamicFieldValue(NotificationItem* notification); + void addDynamicField(UAVObjectField *objField); + void addDynamicFieldWidget(UAVObjectField *objField); + void setDynamicFieldValue(NotificationItem *notification); private: - UAVObjectManager& _objManager; - SoundNotifyPlugin* _owner; + UAVObjectManager & _objManager; + SoundNotifyPlugin *_owner; - //! Media object uses to test sound playing + // ! Media object uses to test sound playing QScopedPointer _testSound; QScopedPointer _notifyRulesModel; - QItemSelectionModel* _notifyRulesSelection; + QItemSelectionModel *_notifyRulesSelection; /** * Local copy of notification list, which owned by notify plugin. @@ -148,40 +159,39 @@ private: * we don't have additional cost for that, copy will created * only after modification of private notify list. */ - QList _privListNotifications; + QList _privListNotifications; QScopedPointer _optionsPage; - //! Widget to convinient selection of condition for field value (equal, lower, greater) - QComboBox* _dynamicFieldCondition; + // ! Widget to convinient selection of condition for field value (equal, lower, greater) + QComboBox *_dynamicFieldCondition; - //! Represents edit widget for dynamic UAVObjectfield, - //! can be spinbox - for numerics, combobox - enums, or - //! lineedit - for numerics with range constraints - QWidget* _dynamicFieldWidget; + // ! Represents edit widget for dynamic UAVObjectfield, + // ! can be spinbox - for numerics, combobox - enums, or + // ! lineedit - for numerics with range constraints + QWidget *_dynamicFieldWidget; - //! Type of UAVObjectField - numeric or ENUM, - //! this variable needs to correctly set appropriate dynamic UI element (_dynamicFieldWidget) - //! NOTE: ocassionaly it should be invalidated (= -1) to reset _dynamicFieldWidget + // ! Type of UAVObjectField - numeric or ENUM, + // ! this variable needs to correctly set appropriate dynamic UI element (_dynamicFieldWidget) + // ! NOTE: ocassionaly it should be invalidated (= -1) to reset _dynamicFieldWidget int _dynamicFieldType; - //! Widget to convinient selection of position of - //! between sounds[1..3] - QComboBox* _sayOrder; + // ! Widget to convinient selection of position of + // ! between sounds[1..3] + QComboBox *_sayOrder; - //! Actualy reference to optionsPageWidget, - //! we MUST hold it beyond the scope of createPage func - //! to have possibility change dynamic parts of options page layout in future - QWidget* _form; + // ! Actualy reference to optionsPageWidget, + // ! we MUST hold it beyond the scope of createPage func + // ! to have possibility change dynamic parts of options page layout in future + QWidget *_form; - //! Currently selected notification, all controls filled accroding to it. - //! On options page startup, always points to first row. - NotificationItem* _selectedNotification; - - //! Retrieved from UAVObjectManager by name from _selectedNotification, - //! if UAVObjectManager doesn't have such object, this field will be NULL - UAVDataObject* _currUAVObject; + // ! Currently selected notification, all controls filled accroding to it. + // ! On options page startup, always points to first row. + NotificationItem *_selectedNotification; + // ! Retrieved from UAVObjectManager by name from _selectedNotification, + // ! if UAVObjectManager doesn't have such object, this field will be NULL + UAVDataObject *_currUAVObject; }; #endif // NOTIFYPLUGINOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifytablemodel.cpp b/ground/openpilotgcs/src/plugins/notify/notifytablemodel.cpp index 0242214bd..8871f2352 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifytablemodel.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifytablemodel.cpp @@ -30,19 +30,19 @@ #include #include -const char* mime_type_notify_table = "openpilot/notify_plugin_table"; +const char *mime_type_notify_table = "openpilot/notify_plugin_table"; -NotifyTableModel::NotifyTableModel(QList& parentList, QObject* parent) - : QAbstractTableModel(parent) - , _list(parentList) +NotifyTableModel::NotifyTableModel(QList & parentList, QObject *parent) + : QAbstractTableModel(parent) + , _list(parentList) { - _headerStrings << "Name" << "Repeats" << "Lifetime,sec" << "Mute"; - connect(this, SIGNAL(dragRows(int, int)), this, SLOT(dropRows(int, int))); + _headerStrings << "Name" << "Repeats" << "Lifetime,sec" << "Mute"; + connect(this, SIGNAL(dragRows(int, int)), this, SLOT(dropRows(int, int))); } bool NotifyTableModel::setData(const QModelIndex &index, - const QVariant &value, int role) + const QVariant &value, int role) { if (index.isValid() && role == Qt::DisplayRole) { if (eMessageName == index.column()) { @@ -51,14 +51,15 @@ bool NotifyTableModel::setData(const QModelIndex &index, } } if (index.isValid() && role == Qt::EditRole) { - if (eRepeatValue == index.column()) - _list.at(index.row())->setRetryValue(NotificationItem::retryValues.indexOf(value.toString())); - else { - if (eExpireTimer == index.column()) + if (eRepeatValue == index.column()) { + _list.at(index.row())->setRetryValue(NotificationItem::retryValues.indexOf(value.toString())); + } else { + if (eExpireTimer == index.column()) { _list.at(index.row())->setLifetime(value.toInt()); - else { - if (eTurnOn == index.column()) + } else { + if (eTurnOn == index.column()) { _list.at(index.row())->setMute(value.toBool()); + } } } emit dataChanged(index, index); @@ -74,18 +75,17 @@ QVariant NotifyTableModel::data(const QModelIndex &index, int role) const return QVariant(); } - if (index.row() >= _list.size()) + if (index.row() >= _list.size()) { return QVariant(); + } - if (role == Qt::DisplayRole || role == Qt::EditRole) - { - switch(index.column()) - { + if (role == Qt::DisplayRole || role == Qt::EditRole) { + switch (index.column()) { case eMessageName: return _list.at(index.row())->toString(); case eRepeatValue: - return (NotificationItem::retryValues.at(_list.at(index.row())->retryValue())); + return NotificationItem::retryValues.at(_list.at(index.row())->retryValue()); case eExpireTimer: return _list.at(index.row())->lifetime(); @@ -96,11 +96,9 @@ QVariant NotifyTableModel::data(const QModelIndex &index, int role) const default: return QVariant(); } - } - else - { - if (Qt::SizeHintRole == role){ - return QVariant(10); + } else { + if (Qt::SizeHintRole == role) { + return QVariant(10); } } return QVariant(); @@ -108,24 +106,26 @@ QVariant NotifyTableModel::data(const QModelIndex &index, int role) const QVariant NotifyTableModel::headerData(int section, Qt::Orientation orientation, int role) const { - if (role != Qt::DisplayRole) + if (role != Qt::DisplayRole) { return QVariant(); + } - if (orientation == Qt::Horizontal) + if (orientation == Qt::Horizontal) { return _headerStrings.at(section); - else - if (orientation == Qt::Vertical) - return QString("%1").arg(section); + } else if (orientation == Qt::Vertical) { + return QString("%1").arg(section); + } return QVariant(); } -bool NotifyTableModel::insertRows(int position, int rows, const QModelIndex& index) +bool NotifyTableModel::insertRows(int position, int rows, const QModelIndex & index) { Q_UNUSED(index); - if (-1 == position || -1 == rows) + if (-1 == position || -1 == rows) { return false; + } beginInsertRows(QModelIndex(), position, position + rows - 1); @@ -137,12 +137,13 @@ bool NotifyTableModel::insertRows(int position, int rows, const QModelIndex& ind return true; } -bool NotifyTableModel::removeRows(int position, int rows, const QModelIndex& index) +bool NotifyTableModel::removeRows(int position, int rows, const QModelIndex & index) { Q_UNUSED(index); - if ((-1 == position) || (-1 == rows) ) + if ((-1 == position) || (-1 == rows)) { return false; + } beginRemoveRows(QModelIndex(), position, position + rows - 1); @@ -160,10 +161,10 @@ void NotifyTableModel::entryUpdated(int offset) emit dataChanged(idx, idx); } -void NotifyTableModel::entryAdded(NotificationItem* item) +void NotifyTableModel::entryAdded(NotificationItem *item) { insertRows(rowCount(), 1, QModelIndex()); - NotificationItem* tmp = _list.at(rowCount() - 1); + NotificationItem *tmp = _list.at(rowCount() - 1); _list.replace(rowCount() - 1, item); delete tmp; entryUpdated(rowCount() - 1); @@ -177,77 +178,87 @@ Qt::DropActions NotifyTableModel::supportedDropActions() const QStringList NotifyTableModel::mimeTypes() const { QStringList types; + types << mime_type_notify_table; return types; } -bool NotifyTableModel::dropMimeData( const QMimeData * data, Qt::DropAction action, int row, - int column, const QModelIndex& parent) +bool NotifyTableModel::dropMimeData(const QMimeData *data, Qt::DropAction action, int row, + int column, const QModelIndex & parent) { - if (action == Qt::IgnoreAction) + if (action == Qt::IgnoreAction) { return true; + } - if (!data->hasFormat(mime_type_notify_table)) + if (!data->hasFormat(mime_type_notify_table)) { return false; + } int beginRow = -1; - if (row != -1) + if (row != -1) { beginRow = row; - else { - if (parent.isValid()) + } else { + if (parent.isValid()) { beginRow = parent.row(); - else + } else { beginRow = rowCount(QModelIndex()); + } } - if (-1 == beginRow) + if (-1 == beginRow) { return false; + } QByteArray encodedData = data->data(mime_type_notify_table); QDataStream stream(&encodedData, QIODevice::ReadOnly); int rows = beginRow; // read next item from input MIME and drop into the table line by line - while(!stream.atEnd()) { + while (!stream.atEnd()) { quintptr ptr; stream >> ptr; - NotificationItem* item = reinterpret_cast(ptr); + NotificationItem *item = reinterpret_cast(ptr); int dragged = _list.indexOf(item); // we can drag item from top rows to bottom (DOWN_DIRECTION), // or from bottom rows to top rows (UP_DIRECTION) enum { UP_DIRECTION, DOWN_DIRECTION }; int direction = (dragged < rows) ? DOWN_DIRECTION : (dragged += 1, UP_DIRECTION); // check drop bounds - if (dragged < 0 || ((dragged + 1) >= _list.size() && direction == DOWN_DIRECTION) || dragged == rows) { + if (dragged < 0 || ((dragged + 1) >= _list.size() && direction == DOWN_DIRECTION) || dragged == rows) { qNotifyDebug() << "no such item"; continue; } // addiional check in case dropping of multiple rows - if(rows + direction > _list.size()) continue; + if (rows + direction > _list.size()) { + continue; + } bool success = insertRows(rows + direction, 1, QModelIndex()); Q_ASSERT(success); _list.replace(rows + direction, item); success = removeRows(dragged, 1, QModelIndex()); Q_ASSERT(success); - if (direction == UP_DIRECTION) + if (direction == UP_DIRECTION) { ++rows; - }; + } + } + ; - QModelIndex idxTopLeft = index(beginRow, 0, QModelIndex()); + QModelIndex idxTopLeft = index(beginRow, 0, QModelIndex()); QModelIndex idxBotRight = index(beginRow, columnCount(QModelIndex()), QModelIndex()); emit dataChanged(idxTopLeft, idxBotRight); return true; } -QMimeData* NotifyTableModel::mimeData(const QModelIndexList& indexes) const +QMimeData *NotifyTableModel::mimeData(const QModelIndexList & indexes) const { - QMimeData* mimeData = new QMimeData(); + QMimeData *mimeData = new QMimeData(); QByteArray encodedData; QDataStream stream(&encodedData, QIODevice::WriteOnly); int rows = 0; - foreach (const QModelIndex& index, indexes) { + + foreach(const QModelIndex &index, indexes) { if (!index.column()) { quintptr item = reinterpret_cast(_list.at(index.row())); stream << item; diff --git a/ground/openpilotgcs/src/plugins/notify/notifytablemodel.h b/ground/openpilotgcs/src/plugins/notify/notifytablemodel.h index 0b4c75b93..01974baab 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifytablemodel.h +++ b/ground/openpilotgcs/src/plugins/notify/notifytablemodel.h @@ -35,37 +35,37 @@ enum ColumnNames { eMessageName, eRepeatValue, eExpireTimer, eTurnOn }; -class NotifyTableModel : public QAbstractTableModel -{ +class NotifyTableModel : public QAbstractTableModel { Q_OBJECT - enum {eColumnCount = 4 }; + enum { eColumnCount = 4 }; public: - NotifyTableModel(QList& parentList, QObject* parent = 0); - int rowCount(const QModelIndex& parent = QModelIndex()) const + NotifyTableModel(QList & parentList, QObject *parent = 0); + int rowCount(const QModelIndex & parent = QModelIndex()) const { return _list.count(); } - int columnCount(const QModelIndex &/*parent*/) const + int columnCount(const QModelIndex & /*parent*/) const { return eColumnCount; } Qt::ItemFlags flags(const QModelIndex &index) const { - if (!index.isValid()) + if (!index.isValid()) { return Qt::ItemIsEnabled | Qt::ItemIsDropEnabled; + } return QAbstractItemModel::flags(index) | Qt::ItemIsEditable | Qt::ItemIsDragEnabled | Qt::ItemIsDropEnabled; } QStringList mimeTypes() const; Qt::DropActions supportedDropActions() const; - bool dropMimeData( const QMimeData * data, Qt::DropAction action, int row, - int column, const QModelIndex& parent); - QMimeData* mimeData(const QModelIndexList &indexes) const; + bool dropMimeData(const QMimeData *data, Qt::DropAction action, int row, + int column, const QModelIndex & parent); + QMimeData *mimeData(const QModelIndexList &indexes) const; bool setData(const QModelIndex &index, const QVariant &value, int role); @@ -73,7 +73,7 @@ public: QVariant headerData(int section, Qt::Orientation orientation, int role) const; bool insertRows(int position, int rows, const QModelIndex &index); bool removeRows(int position, int rows, const QModelIndex &index); - void entryAdded(NotificationItem* item); + void entryAdded(NotificationItem *item); signals: void dragRows(int position, int count); @@ -83,7 +83,7 @@ private slots: void dropRows(int position, int count) const; private: - QList& _list; + QList & _list; QStringList _headerStrings; }; diff --git a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp index 4ca229866..633199aa5 100644 --- a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp @@ -28,392 +28,452 @@ #include "flightdatamodel.h" #include #include -flightDataModel::flightDataModel(QObject *parent):QAbstractTableModel(parent) -{ +flightDataModel::flightDataModel(QObject *parent) : QAbstractTableModel(parent) +{} -} - -int flightDataModel::rowCount(const QModelIndex &/*parent*/) const +int flightDataModel::rowCount(const QModelIndex & /*parent*/) const { return dataStorage.length(); } int flightDataModel::columnCount(const QModelIndex &parent) const { - if (parent.isValid()) + if (parent.isValid()) { return 0; + } return 23; } QVariant flightDataModel::data(const QModelIndex &index, int role) const { - if (role == Qt::DisplayRole||role==Qt::EditRole) - { - int rowNumber=index.row(); - int columnNumber=index.column(); - if(rowNumber>dataStorage.length()-1 || rowNumber<0) + if (role == Qt::DisplayRole || role == Qt::EditRole) { + int rowNumber = index.row(); + int columnNumber = index.column(); + if (rowNumber > dataStorage.length() - 1 || rowNumber < 0) { return QVariant::Invalid; - pathPlanData * myRow=dataStorage.at(rowNumber); - QVariant ret=getColumnByIndex(myRow,columnNumber); + } + pathPlanData *myRow = dataStorage.at(rowNumber); + QVariant ret = getColumnByIndex(myRow, columnNumber); return ret; } /* - else if (role == Qt::BackgroundRole) { - // WaypointActive::DataFields waypointActive = waypointActiveObj->getData(); + else if (role == Qt::BackgroundRole) { + // WaypointActive::DataFields waypointActive = waypointActiveObj->getData(); if(index.row() == waypointActive.Index) { return QBrush(Qt::lightGray); } else return QVariant::Invalid; - }*/ + }*/ else { return QVariant::Invalid; } } -bool flightDataModel::setColumnByIndex(pathPlanData *row,const int index,const QVariant value) +bool flightDataModel::setColumnByIndex(pathPlanData *row, const int index, const QVariant value) { - switch(index) - { + switch (index) { case WPDESCRITPTION: - row->wpDescritption=value.toString(); + row->wpDescritption = value.toString(); return true; + break; case LATPOSITION: - row->latPosition=value.toDouble(); + row->latPosition = value.toDouble(); return true; + break; case LNGPOSITION: - row->lngPosition=value.toDouble(); + row->lngPosition = value.toDouble(); return true; + break; case DISRELATIVE: - row->disRelative=value.toDouble(); + row->disRelative = value.toDouble(); return true; + break; case BEARELATIVE: - row->beaRelative=value.toDouble(); + row->beaRelative = value.toDouble(); return true; + break; case ALTITUDERELATIVE: - row->altitudeRelative=value.toFloat(); + row->altitudeRelative = value.toFloat(); return true; + break; case ISRELATIVE: - row->isRelative=value.toBool(); + row->isRelative = value.toBool(); return true; + break; case ALTITUDE: - row->altitude=value.toDouble(); + row->altitude = value.toDouble(); return true; + break; case VELOCITY: - row->velocity=value.toFloat(); + row->velocity = value.toFloat(); return true; + break; case MODE: - row->mode=value.toInt(); + row->mode = value.toInt(); return true; + break; case MODE_PARAMS0: - row->mode_params[0]=value.toFloat(); + row->mode_params[0] = value.toFloat(); return true; + break; case MODE_PARAMS1: - row->mode_params[1]=value.toFloat(); + row->mode_params[1] = value.toFloat(); return true; + break; case MODE_PARAMS2: - row->mode_params[2]=value.toFloat(); + row->mode_params[2] = value.toFloat(); return true; + break; case MODE_PARAMS3: - row->mode_params[3]=value.toFloat(); + row->mode_params[3] = value.toFloat(); return true; + break; case CONDITION: - row->condition=value.toInt(); + row->condition = value.toInt(); return true; + break; case CONDITION_PARAMS0: - row->condition_params[0]=value.toFloat(); + row->condition_params[0] = value.toFloat(); return true; + break; case CONDITION_PARAMS1: - row->condition_params[1]=value.toFloat(); + row->condition_params[1] = value.toFloat(); return true; + break; case CONDITION_PARAMS2: - row->condition_params[2]=value.toFloat(); + row->condition_params[2] = value.toFloat(); return true; + break; case CONDITION_PARAMS3: - row->condition_params[3]=value.toFloat(); + row->condition_params[3] = value.toFloat(); return true; + break; case COMMAND: - row->command=value.toInt(); + row->command = value.toInt(); break; case JUMPDESTINATION: - row->jumpdestination=value.toInt(); + row->jumpdestination = value.toInt(); return true; + break; case ERRORDESTINATION: - row->errordestination=value.toInt(); + row->errordestination = value.toInt(); return true; + break; case LOCKED: - row->locked=value.toBool(); + row->locked = value.toBool(); return true; + break; default: return false; } return false; } -QVariant flightDataModel::getColumnByIndex(const pathPlanData *row,const int index) const +QVariant flightDataModel::getColumnByIndex(const pathPlanData *row, const int index) const { - switch(index) - { + switch (index) { case WPDESCRITPTION: return row->wpDescritption; + break; case LATPOSITION: return row->latPosition; + break; case LNGPOSITION: return row->lngPosition; + break; case DISRELATIVE: return row->disRelative; + break; case BEARELATIVE: return row->beaRelative; + break; case ALTITUDERELATIVE: return row->altitudeRelative; + break; case ISRELATIVE: return row->isRelative; + break; case ALTITUDE: return row->altitude; + break; case VELOCITY: return row->velocity; + break; case MODE: return row->mode; + break; case MODE_PARAMS0: return row->mode_params[0]; + break; case MODE_PARAMS1: return row->mode_params[1]; + break; case MODE_PARAMS2: return row->mode_params[2]; + break; case MODE_PARAMS3: return row->mode_params[3]; + break; case CONDITION: return row->condition; + break; case CONDITION_PARAMS0: return row->condition_params[0]; + break; case CONDITION_PARAMS1: return row->condition_params[1]; + break; case CONDITION_PARAMS2: return row->condition_params[2]; + break; case CONDITION_PARAMS3: return row->condition_params[3]; + break; case COMMAND: return row->command; + break; case JUMPDESTINATION: return row->jumpdestination; + break; case ERRORDESTINATION: return row->errordestination; + break; case LOCKED: return row->locked; } } QVariant flightDataModel::headerData(int section, Qt::Orientation orientation, int role) const - { - if (role == Qt::DisplayRole) - { - if(orientation==Qt::Vertical) - { - return QString::number(section+1); - } - else if (orientation == Qt::Horizontal) { - switch (section) - { - case WPDESCRITPTION: - return QString("Description"); - break; - case LATPOSITION: - return QString("Latitude"); - break; - case LNGPOSITION: - return QString("Longitude"); - break; - case DISRELATIVE: - return QString("Distance to home"); - break; - case BEARELATIVE: - return QString("Bearing from home"); - break; - case ALTITUDERELATIVE: - return QString("Altitude above home"); - break; - case ISRELATIVE: - return QString("Relative to home"); - break; - case ALTITUDE: - return QString("Altitude"); - break; - case VELOCITY: - return QString("Velocity"); - break; - case MODE: - return QString("Mode"); - break; - case MODE_PARAMS0: - return QString("Mode parameter 0"); - break; - case MODE_PARAMS1: - return QString("Mode parameter 1"); - break; - case MODE_PARAMS2: - return QString("Mode parameter 2"); - break; - case MODE_PARAMS3: - return QString("Mode parameter 3"); - break; - case CONDITION: - return QString("Condition"); - break; - case CONDITION_PARAMS0: - return QString("Condition parameter 0"); - break; - case CONDITION_PARAMS1: - return QString("Condition parameter 1"); - break; - case CONDITION_PARAMS2: - return QString("Condition parameter 2"); - break; - case CONDITION_PARAMS3: - return QString("Condition parameter 3"); - break; - case COMMAND: - return QString("Command"); - break; - case JUMPDESTINATION: - return QString("Jump Destination"); - break; - case ERRORDESTINATION: - return QString("Error Destination"); - break; - case LOCKED: - return QString("Locked"); - break; - default: - return QString(); - break; - } - } - } - else - return QAbstractTableModel::headerData(section, orientation, role); +{ + if (role == Qt::DisplayRole) { + if (orientation == Qt::Vertical) { + return QString::number(section + 1); + } else if (orientation == Qt::Horizontal) { + switch (section) { + case WPDESCRITPTION: + return QString("Description"); + + break; + case LATPOSITION: + return QString("Latitude"); + + break; + case LNGPOSITION: + return QString("Longitude"); + + break; + case DISRELATIVE: + return QString("Distance to home"); + + break; + case BEARELATIVE: + return QString("Bearing from home"); + + break; + case ALTITUDERELATIVE: + return QString("Altitude above home"); + + break; + case ISRELATIVE: + return QString("Relative to home"); + + break; + case ALTITUDE: + return QString("Altitude"); + + break; + case VELOCITY: + return QString("Velocity"); + + break; + case MODE: + return QString("Mode"); + + break; + case MODE_PARAMS0: + return QString("Mode parameter 0"); + + break; + case MODE_PARAMS1: + return QString("Mode parameter 1"); + + break; + case MODE_PARAMS2: + return QString("Mode parameter 2"); + + break; + case MODE_PARAMS3: + return QString("Mode parameter 3"); + + break; + case CONDITION: + return QString("Condition"); + + break; + case CONDITION_PARAMS0: + return QString("Condition parameter 0"); + + break; + case CONDITION_PARAMS1: + return QString("Condition parameter 1"); + + break; + case CONDITION_PARAMS2: + return QString("Condition parameter 2"); + + break; + case CONDITION_PARAMS3: + return QString("Condition parameter 3"); + + break; + case COMMAND: + return QString("Command"); + + break; + case JUMPDESTINATION: + return QString("Jump Destination"); + + break; + case ERRORDESTINATION: + return QString("Error Destination"); + + break; + case LOCKED: + return QString("Locked"); + + break; + default: + return QString(); + + break; + } + } + } else { + return QAbstractTableModel::headerData(section, orientation, role); + } } bool flightDataModel::setData(const QModelIndex &index, const QVariant &value, int role) { - if (role == Qt::EditRole) - { - int columnIndex=index.column(); - int rowIndex=index.row(); - if(rowIndex>dataStorage.length()-1) + if (role == Qt::EditRole) { + int columnIndex = index.column(); + int rowIndex = index.row(); + if (rowIndex > dataStorage.length() - 1) { return false; - pathPlanData * myRow=dataStorage.at(rowIndex); - setColumnByIndex(myRow,columnIndex,value); - emit dataChanged(index,index); + } + pathPlanData *myRow = dataStorage.at(rowIndex); + setColumnByIndex(myRow, columnIndex, value); + emit dataChanged(index, index); } return true; } Qt::ItemFlags flightDataModel::flags(const QModelIndex & /*index*/) const - { - return Qt::ItemIsSelectable | Qt::ItemIsEditable | Qt::ItemIsEnabled ; +{ + return Qt::ItemIsSelectable | Qt::ItemIsEditable | Qt::ItemIsEnabled; } -bool flightDataModel::insertRows(int row, int count, const QModelIndex &/*parent*/) +bool flightDataModel::insertRows(int row, int count, const QModelIndex & /*parent*/) { - pathPlanData * data; - beginInsertRows(QModelIndex(),row,row+count-1); - for(int x=0; xlatPosition=0; - data->lngPosition=0; - data->disRelative=0; - data->beaRelative=0; - data->altitudeRelative=0; - data->isRelative=true; - data->altitude=0; - data->velocity=0; - data->mode=1; - data->mode_params[0]=0; - data->mode_params[1]=0; - data->mode_params[2]=0; - data->mode_params[3]=0; - data->condition=3; - data->condition_params[0]=0; - data->condition_params[1]=0; - data->condition_params[2]=0; - data->condition_params[3]=0; - data->command=0; - data->jumpdestination=0; - data->errordestination=0; - data->locked=false; - if(rowCount()>0) - { - data->altitude=this->data(this->index(rowCount()-1,ALTITUDE)).toDouble(); - data->altitudeRelative=this->data(this->index(rowCount()-1,ALTITUDERELATIVE)).toDouble(); - data->isRelative=this->data(this->index(rowCount()-1,ISRELATIVE)).toBool(); - data->velocity=this->data(this->index(rowCount()-1,VELOCITY)).toFloat(); - data->mode=this->data(this->index(rowCount()-1,MODE)).toInt(); - data->mode_params[0]=this->data(this->index(rowCount()-1,MODE_PARAMS0)).toFloat(); - data->mode_params[1]=this->data(this->index(rowCount()-1,MODE_PARAMS1)).toFloat(); - data->mode_params[2]=this->data(this->index(rowCount()-1,MODE_PARAMS2)).toFloat(); - data->mode_params[3]=this->data(this->index(rowCount()-1,MODE_PARAMS3)).toFloat(); - data->condition=this->data(this->index(rowCount()-1,CONDITION)).toInt(); - data->condition_params[0]=this->data(this->index(rowCount()-1,CONDITION_PARAMS0)).toFloat(); - data->condition_params[1]=this->data(this->index(rowCount()-1,CONDITION_PARAMS1)).toFloat(); - data->condition_params[2]=this->data(this->index(rowCount()-1,CONDITION_PARAMS2)).toFloat(); - data->condition_params[3]=this->data(this->index(rowCount()-1,CONDITION_PARAMS3)).toFloat(); - data->command=this->data(this->index(rowCount()-1,COMMAND)).toInt(); - data->errordestination=this->data(this->index(rowCount()-1,ERRORDESTINATION)).toInt(); + pathPlanData *data; + + beginInsertRows(QModelIndex(), row, row + count - 1); + for (int x = 0; x < count; ++x) { + data = new pathPlanData; + data->latPosition = 0; + data->lngPosition = 0; + data->disRelative = 0; + data->beaRelative = 0; + data->altitudeRelative = 0; + data->isRelative = true; + data->altitude = 0; + data->velocity = 0; + data->mode = 1; + data->mode_params[0] = 0; + data->mode_params[1] = 0; + data->mode_params[2] = 0; + data->mode_params[3] = 0; + data->condition = 3; + data->condition_params[0] = 0; + data->condition_params[1] = 0; + data->condition_params[2] = 0; + data->condition_params[3] = 0; + data->command = 0; + data->jumpdestination = 0; + data->errordestination = 0; + data->locked = false; + if (rowCount() > 0) { + data->altitude = this->data(this->index(rowCount() - 1, ALTITUDE)).toDouble(); + data->altitudeRelative = this->data(this->index(rowCount() - 1, ALTITUDERELATIVE)).toDouble(); + data->isRelative = this->data(this->index(rowCount() - 1, ISRELATIVE)).toBool(); + data->velocity = this->data(this->index(rowCount() - 1, VELOCITY)).toFloat(); + data->mode = this->data(this->index(rowCount() - 1, MODE)).toInt(); + data->mode_params[0] = this->data(this->index(rowCount() - 1, MODE_PARAMS0)).toFloat(); + data->mode_params[1] = this->data(this->index(rowCount() - 1, MODE_PARAMS1)).toFloat(); + data->mode_params[2] = this->data(this->index(rowCount() - 1, MODE_PARAMS2)).toFloat(); + data->mode_params[3] = this->data(this->index(rowCount() - 1, MODE_PARAMS3)).toFloat(); + data->condition = this->data(this->index(rowCount() - 1, CONDITION)).toInt(); + data->condition_params[0] = this->data(this->index(rowCount() - 1, CONDITION_PARAMS0)).toFloat(); + data->condition_params[1] = this->data(this->index(rowCount() - 1, CONDITION_PARAMS1)).toFloat(); + data->condition_params[2] = this->data(this->index(rowCount() - 1, CONDITION_PARAMS2)).toFloat(); + data->condition_params[3] = this->data(this->index(rowCount() - 1, CONDITION_PARAMS3)).toFloat(); + data->command = this->data(this->index(rowCount() - 1, COMMAND)).toInt(); + data->errordestination = this->data(this->index(rowCount() - 1, ERRORDESTINATION)).toInt(); } - dataStorage.insert(row,data); + dataStorage.insert(row, data); } endInsertRows(); } -bool flightDataModel::removeRows(int row, int count, const QModelIndex &/*parent*/) +bool flightDataModel::removeRows(int row, int count, const QModelIndex & /*parent*/) { - if(row<0) + if (row < 0) { return false; - beginRemoveRows(QModelIndex(),row,row+count-1); - for(int x=0; xwpDescritption); - field.setAttribute("name","description"); + QDomElement field = doc.createElement("field"); + field.setAttribute("value", obj->wpDescritption); + field.setAttribute("name", "description"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->latPosition); - field.setAttribute("name","latitude"); + field = doc.createElement("field"); + field.setAttribute("value", obj->latPosition); + field.setAttribute("name", "latitude"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->lngPosition); - field.setAttribute("name","longitude"); + field = doc.createElement("field"); + field.setAttribute("value", obj->lngPosition); + field.setAttribute("name", "longitude"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->disRelative); - field.setAttribute("name","distance_to_home"); + field = doc.createElement("field"); + field.setAttribute("value", obj->disRelative); + field.setAttribute("name", "distance_to_home"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->beaRelative); - field.setAttribute("name","bearing_from_home"); + field = doc.createElement("field"); + field.setAttribute("value", obj->beaRelative); + field.setAttribute("name", "bearing_from_home"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->altitudeRelative); - field.setAttribute("name","altitude_above_home"); + field = doc.createElement("field"); + field.setAttribute("value", obj->altitudeRelative); + field.setAttribute("name", "altitude_above_home"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->isRelative); - field.setAttribute("name","is_relative_to_home"); + field = doc.createElement("field"); + field.setAttribute("value", obj->isRelative); + field.setAttribute("name", "is_relative_to_home"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->altitude); - field.setAttribute("name","altitude"); + field = doc.createElement("field"); + field.setAttribute("value", obj->altitude); + field.setAttribute("name", "altitude"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->velocity); - field.setAttribute("name","velocity"); + field = doc.createElement("field"); + field.setAttribute("value", obj->velocity); + field.setAttribute("name", "velocity"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->mode); - field.setAttribute("name","mode"); + field = doc.createElement("field"); + field.setAttribute("value", obj->mode); + field.setAttribute("name", "mode"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->mode_params[0]); - field.setAttribute("name","mode_param0"); + field = doc.createElement("field"); + field.setAttribute("value", obj->mode_params[0]); + field.setAttribute("name", "mode_param0"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->mode_params[1]); - field.setAttribute("name","mode_param1"); + field = doc.createElement("field"); + field.setAttribute("value", obj->mode_params[1]); + field.setAttribute("name", "mode_param1"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->mode_params[2]); - field.setAttribute("name","mode_param2"); + field = doc.createElement("field"); + field.setAttribute("value", obj->mode_params[2]); + field.setAttribute("name", "mode_param2"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->mode_params[3]); - field.setAttribute("name","mode_param3"); + field = doc.createElement("field"); + field.setAttribute("value", obj->mode_params[3]); + field.setAttribute("name", "mode_param3"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->condition); - field.setAttribute("name","condition"); + field = doc.createElement("field"); + field.setAttribute("value", obj->condition); + field.setAttribute("name", "condition"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->condition_params[0]); - field.setAttribute("name","condition_param0"); + field = doc.createElement("field"); + field.setAttribute("value", obj->condition_params[0]); + field.setAttribute("name", "condition_param0"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->condition_params[1]); - field.setAttribute("name","condition_param1"); + field = doc.createElement("field"); + field.setAttribute("value", obj->condition_params[1]); + field.setAttribute("name", "condition_param1"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->condition_params[2]); - field.setAttribute("name","condition_param2"); + field = doc.createElement("field"); + field.setAttribute("value", obj->condition_params[2]); + field.setAttribute("name", "condition_param2"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->condition_params[3]); - field.setAttribute("name","condition_param3"); + field = doc.createElement("field"); + field.setAttribute("value", obj->condition_params[3]); + field.setAttribute("name", "condition_param3"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->command); - field.setAttribute("name","command"); + field = doc.createElement("field"); + field.setAttribute("value", obj->command); + field.setAttribute("name", "command"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->jumpdestination); - field.setAttribute("name","jumpdestination"); + field = doc.createElement("field"); + field.setAttribute("value", obj->jumpdestination); + field.setAttribute("name", "jumpdestination"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->errordestination); - field.setAttribute("name","errordestination"); + field = doc.createElement("field"); + field.setAttribute("value", obj->errordestination); + field.setAttribute("name", "errordestination"); waypoint.appendChild(field); - field=doc.createElement("field"); - field.setAttribute("value",obj->locked); - field.setAttribute("name","is_locked"); + field = doc.createElement("field"); + field.setAttribute("value", obj->locked); + field.setAttribute("name", "is_locked"); waypoint.appendChild(field); - } file.write(doc.toString().toAscii()); file.close(); @@ -562,14 +619,14 @@ bool flightDataModel::writeToFile(QString fileName) } void flightDataModel::readFromFile(QString fileName) { - //TODO warning message - removeRows(0,rowCount()); + // TODO warning message + removeRows(0, rowCount()); QFile file(fileName); file.open(QIODevice::ReadOnly); QDomDocument doc("PathPlan"); - QByteArray array=file.readAll(); + QByteArray array = file.readAll(); QString error; - if (!doc.setContent(array,&error)) { + if (!doc.setContent(array, &error)) { QMessageBox msgBox; msgBox.setText(tr("File Parsing Failed.")); msgBox.setInformativeText(QString(tr("This file is not a correct XML file:%0")).arg(error)); @@ -590,73 +647,72 @@ void flightDataModel::readFromFile(QString fileName) return; } - pathPlanData * data=NULL; + pathPlanData *data = NULL; QDomNode node = root.firstChild(); while (!node.isNull()) { QDomElement e = node.toElement(); if (e.tagName() == "waypoint") { - QDomNode fieldNode=e.firstChild(); - data=new pathPlanData; + QDomNode fieldNode = e.firstChild(); + data = new pathPlanData; while (!fieldNode.isNull()) { QDomElement field = fieldNode.toElement(); if (field.tagName() == "field") { - if(field.attribute("name")=="altitude") - data->altitude=field.attribute("value").toDouble(); - else if(field.attribute("name")=="description") - data->wpDescritption=field.attribute("value"); - else if(field.attribute("name")=="latitude") - data->latPosition=field.attribute("value").toDouble(); - else if(field.attribute("name")=="longitude") - data->lngPosition=field.attribute("value").toDouble(); - else if(field.attribute("name")=="distance_to_home") - data->disRelative=field.attribute("value").toDouble(); - else if(field.attribute("name")=="bearing_from_home") - data->beaRelative=field.attribute("value").toDouble(); - else if(field.attribute("name")=="altitude_above_home") - data->altitudeRelative=field.attribute("value").toFloat(); - else if(field.attribute("name")=="is_relative_to_home") - data->isRelative=field.attribute("value").toInt(); - else if(field.attribute("name")=="altitude") - data->altitude=field.attribute("value").toDouble(); - else if(field.attribute("name")=="velocity") - data->velocity=field.attribute("value").toFloat(); - else if(field.attribute("name")=="mode") - data->mode=field.attribute("value").toInt(); - else if(field.attribute("name")=="mode_param0") - data->mode_params[0]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="mode_param1") - data->mode_params[1]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="mode_param2") - data->mode_params[2]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="mode_param3") - data->mode_params[3]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="condition") - data->condition=field.attribute("value").toDouble(); - else if(field.attribute("name")=="condition_param0") - data->condition_params[0]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="condition_param1") - data->condition_params[1]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="condition_param2") - data->condition_params[2]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="condition_param3") - data->condition_params[3]=field.attribute("value").toFloat(); - else if(field.attribute("name")=="command") - data->command=field.attribute("value").toInt(); - else if(field.attribute("name")=="jumpdestination") - data->jumpdestination=field.attribute("value").toInt(); - else if(field.attribute("name")=="errordestination") - data->errordestination=field.attribute("value").toInt(); - else if(field.attribute("name")=="is_locked") - data->locked=field.attribute("value").toInt(); - + if (field.attribute("name") == "altitude") { + data->altitude = field.attribute("value").toDouble(); + } else if (field.attribute("name") == "description") { + data->wpDescritption = field.attribute("value"); + } else if (field.attribute("name") == "latitude") { + data->latPosition = field.attribute("value").toDouble(); + } else if (field.attribute("name") == "longitude") { + data->lngPosition = field.attribute("value").toDouble(); + } else if (field.attribute("name") == "distance_to_home") { + data->disRelative = field.attribute("value").toDouble(); + } else if (field.attribute("name") == "bearing_from_home") { + data->beaRelative = field.attribute("value").toDouble(); + } else if (field.attribute("name") == "altitude_above_home") { + data->altitudeRelative = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "is_relative_to_home") { + data->isRelative = field.attribute("value").toInt(); + } else if (field.attribute("name") == "altitude") { + data->altitude = field.attribute("value").toDouble(); + } else if (field.attribute("name") == "velocity") { + data->velocity = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "mode") { + data->mode = field.attribute("value").toInt(); + } else if (field.attribute("name") == "mode_param0") { + data->mode_params[0] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "mode_param1") { + data->mode_params[1] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "mode_param2") { + data->mode_params[2] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "mode_param3") { + data->mode_params[3] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "condition") { + data->condition = field.attribute("value").toDouble(); + } else if (field.attribute("name") == "condition_param0") { + data->condition_params[0] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "condition_param1") { + data->condition_params[1] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "condition_param2") { + data->condition_params[2] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "condition_param3") { + data->condition_params[3] = field.attribute("value").toFloat(); + } else if (field.attribute("name") == "command") { + data->command = field.attribute("value").toInt(); + } else if (field.attribute("name") == "jumpdestination") { + data->jumpdestination = field.attribute("value").toInt(); + } else if (field.attribute("name") == "errordestination") { + data->errordestination = field.attribute("value").toInt(); + } else if (field.attribute("name") == "is_locked") { + data->locked = field.attribute("value").toInt(); + } } - fieldNode=fieldNode.nextSibling(); + fieldNode = fieldNode.nextSibling(); } - beginInsertRows(QModelIndex(),dataStorage.length(),dataStorage.length()); - dataStorage.append(data); - endInsertRows(); + beginInsertRows(QModelIndex(), dataStorage.length(), dataStorage.length()); + dataStorage.append(data); + endInsertRows(); } - node=node.nextSibling(); + node = node.nextSibling(); } } - diff --git a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.h b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.h index 3043c607c..3dcdf337c 100644 --- a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.h +++ b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.h @@ -29,48 +29,45 @@ #include #include "opmapcontrol/opmapcontrol.h" -struct pathPlanData -{ +struct pathPlanData { QString wpDescritption; - double latPosition; - double lngPosition; - double disRelative; - double beaRelative; - double altitudeRelative; - bool isRelative; - double altitude; - float velocity; - int mode; - float mode_params[4]; - int condition; - float condition_params[4]; - int command; - int jumpdestination; - int errordestination; - bool locked; + double latPosition; + double lngPosition; + double disRelative; + double beaRelative; + double altitudeRelative; + bool isRelative; + double altitude; + float velocity; + int mode; + float mode_params[4]; + int condition; + float condition_params[4]; + int command; + int jumpdestination; + int errordestination; + bool locked; }; -class flightDataModel:public QAbstractTableModel -{ +class flightDataModel : public QAbstractTableModel { Q_OBJECT public: - enum pathPlanDataEnum - { - WPDESCRITPTION,LATPOSITION,LNGPOSITION,DISRELATIVE,BEARELATIVE,ALTITUDERELATIVE,ISRELATIVE,ALTITUDE, - VELOCITY,MODE,MODE_PARAMS0,MODE_PARAMS1,MODE_PARAMS2,MODE_PARAMS3, - CONDITION,CONDITION_PARAMS0,CONDITION_PARAMS1,CONDITION_PARAMS2,CONDITION_PARAMS3, - COMMAND,JUMPDESTINATION,ERRORDESTINATION,LOCKED + enum pathPlanDataEnum { + WPDESCRITPTION, LATPOSITION, LNGPOSITION, DISRELATIVE, BEARELATIVE, ALTITUDERELATIVE, ISRELATIVE, ALTITUDE, + VELOCITY, MODE, MODE_PARAMS0, MODE_PARAMS1, MODE_PARAMS2, MODE_PARAMS3, + CONDITION, CONDITION_PARAMS0, CONDITION_PARAMS1, CONDITION_PARAMS2, CONDITION_PARAMS3, + COMMAND, JUMPDESTINATION, ERRORDESTINATION, LOCKED }; flightDataModel(QObject *parent); - int rowCount(const QModelIndex &parent = QModelIndex()) const ; + int rowCount(const QModelIndex &parent = QModelIndex()) const; int columnCount(const QModelIndex &parent = QModelIndex()) const; QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const; QVariant headerData(int section, Qt::Orientation orientation, int role) const; bool setData(const QModelIndex & index, const QVariant & value, int role = Qt::EditRole); - Qt::ItemFlags flags(const QModelIndex & index) const ; - bool insertRows ( int row, int count, const QModelIndex & parent = QModelIndex() ); - bool removeRows ( int row, int count, const QModelIndex & parent = QModelIndex() ); + Qt::ItemFlags flags(const QModelIndex & index) const; + bool insertRows(int row, int count, const QModelIndex & parent = QModelIndex()); + bool removeRows(int row, int count, const QModelIndex & parent = QModelIndex()); bool writeToFile(QString filename); void readFromFile(QString fileName); private: diff --git a/ground/openpilotgcs/src/plugins/opmap/homeeditor.cpp b/ground/openpilotgcs/src/plugins/opmap/homeeditor.cpp index 47b5466f0..299dc3b12 100644 --- a/ground/openpilotgcs/src/plugins/opmap/homeeditor.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/homeeditor.cpp @@ -32,13 +32,12 @@ homeEditor::homeEditor(HomeItem *home, QWidget *parent) : ui(new Ui::homeEditor), myhome(home) { - if(!home) - { + if (!home) { deleteLater(); return; } ui->setupUi(this); - this->setAttribute(Qt::WA_DeleteOnClose,true); + this->setAttribute(Qt::WA_DeleteOnClose, true); ui->altitude->setValue(home->Altitude()); ui->latitude->setValue(home->Coord().Lat()); ui->longitude->setValue(home->Coord().Lng()); @@ -52,7 +51,7 @@ homeEditor::~homeEditor() void homeEditor::on_buttonBox_accepted() { - myhome->SetCoord(internals::PointLatLng(ui->latitude->value(),ui->longitude->value())); + myhome->SetCoord(internals::PointLatLng(ui->latitude->value(), ui->longitude->value())); myhome->SetAltitude(ui->altitude->value()); } diff --git a/ground/openpilotgcs/src/plugins/opmap/homeeditor.h b/ground/openpilotgcs/src/plugins/opmap/homeeditor.h index e0404e6b9..89905357d 100644 --- a/ground/openpilotgcs/src/plugins/opmap/homeeditor.h +++ b/ground/openpilotgcs/src/plugins/opmap/homeeditor.h @@ -36,14 +36,13 @@ namespace Ui { class homeEditor; } -class homeEditor : public QDialog -{ +class homeEditor : public QDialog { Q_OBJECT - + public: - explicit homeEditor(HomeItem * home,QWidget *parent = 0); + explicit homeEditor(HomeItem *home, QWidget *parent = 0); ~homeEditor(); - + private slots: void on_buttonBox_accepted(); @@ -51,7 +50,7 @@ private slots: private: Ui::homeEditor *ui; - HomeItem * myhome; + HomeItem *myhome; }; #endif // HOMEEDITOR_H diff --git a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp index a802d69f8..2dac1b0ac 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp @@ -27,45 +27,48 @@ #include "modelmapproxy.h" -modelMapProxy::modelMapProxy(QObject *parent,OPMapWidget *map,flightDataModel * model,QItemSelectionModel * selectionModel):QObject(parent),myMap(map),model(model),selection(selectionModel) +modelMapProxy::modelMapProxy(QObject *parent, OPMapWidget *map, flightDataModel *model, QItemSelectionModel *selectionModel) : QObject(parent), myMap(map), model(model), selection(selectionModel) { - connect(model,SIGNAL(rowsInserted(const QModelIndex&,int,int)),this,SLOT(rowsInserted(const QModelIndex&,int,int))); - connect(model,SIGNAL(rowsRemoved(const QModelIndex&,int,int)),this,SLOT(rowsRemoved(const QModelIndex&,int,int))); - connect(selection,SIGNAL(currentRowChanged(QModelIndex,QModelIndex)),this,SLOT(currentRowChanged(QModelIndex,QModelIndex))); - connect(model,SIGNAL(dataChanged(QModelIndex,QModelIndex)),this,SLOT(dataChanged(QModelIndex,QModelIndex))); - connect(myMap,SIGNAL(selectedWPChanged(QList)),this,SLOT(selectedWPChanged(QList))); - connect(myMap,SIGNAL(WPValuesChanged(WayPointItem*)),this,SLOT(WPValuesChanged(WayPointItem*))); + connect(model, SIGNAL(rowsInserted(const QModelIndex &, int, int)), this, SLOT(rowsInserted(const QModelIndex &, int, int))); + connect(model, SIGNAL(rowsRemoved(const QModelIndex &, int, int)), this, SLOT(rowsRemoved(const QModelIndex &, int, int))); + connect(selection, SIGNAL(currentRowChanged(QModelIndex, QModelIndex)), this, SLOT(currentRowChanged(QModelIndex, QModelIndex))); + connect(model, SIGNAL(dataChanged(QModelIndex, QModelIndex)), this, SLOT(dataChanged(QModelIndex, QModelIndex))); + connect(myMap, SIGNAL(selectedWPChanged(QList)), this, SLOT(selectedWPChanged(QList))); + connect(myMap, SIGNAL(WPValuesChanged(WayPointItem *)), this, SLOT(WPValuesChanged(WayPointItem *))); } -void modelMapProxy::WPValuesChanged(WayPointItem * wp) +void modelMapProxy::WPValuesChanged(WayPointItem *wp) { QModelIndex index; - index=model->index(wp->Number(),flightDataModel::LATPOSITION); - if(!index.isValid()) + + index = model->index(wp->Number(), flightDataModel::LATPOSITION); + if (!index.isValid()) { return; - model->setData(index,wp->Coord().Lat(),Qt::EditRole); - index=model->index(wp->Number(),flightDataModel::LNGPOSITION); - model->setData(index,wp->Coord().Lng(),Qt::EditRole); + } + model->setData(index, wp->Coord().Lat(), Qt::EditRole); + index = model->index(wp->Number(), flightDataModel::LNGPOSITION); + model->setData(index, wp->Coord().Lng(), Qt::EditRole); - index=model->index(wp->Number(),flightDataModel::ALTITUDE); - model->setData(index,wp->Altitude(),Qt::EditRole); + index = model->index(wp->Number(), flightDataModel::ALTITUDE); + model->setData(index, wp->Altitude(), Qt::EditRole); - index=model->index(wp->Number(),flightDataModel::DISRELATIVE); - model->setData(index,wp->getRelativeCoord().distance,Qt::EditRole); - index=model->index(wp->Number(),flightDataModel::BEARELATIVE); - model->setData(index,wp->getRelativeCoord().bearingToDegrees(),Qt::EditRole); - index=model->index(wp->Number(),flightDataModel::ALTITUDERELATIVE); - model->setData(index,wp->getRelativeCoord().altitudeRelative,Qt::EditRole); + index = model->index(wp->Number(), flightDataModel::DISRELATIVE); + model->setData(index, wp->getRelativeCoord().distance, Qt::EditRole); + index = model->index(wp->Number(), flightDataModel::BEARELATIVE); + model->setData(index, wp->getRelativeCoord().bearingToDegrees(), Qt::EditRole); + index = model->index(wp->Number(), flightDataModel::ALTITUDERELATIVE); + model->setData(index, wp->getRelativeCoord().altitudeRelative, Qt::EditRole); } void modelMapProxy::currentRowChanged(QModelIndex current, QModelIndex previous) { Q_UNUSED(previous); - QList list; - WayPointItem * wp=findWayPointNumber(current.row()); - if(!wp) + QList list; + WayPointItem *wp = findWayPointNumber(current.row()); + if (!wp) { return; + } list.append(wp); myMap->setSelectedWP(list); } @@ -73,132 +76,132 @@ void modelMapProxy::currentRowChanged(QModelIndex current, QModelIndex previous) void modelMapProxy::selectedWPChanged(QList list) { selection->clearSelection(); - foreach(WayPointItem * wp,list) - { - QModelIndex index=model->index(wp->Number(),0); - selection->setCurrentIndex(index,QItemSelectionModel::Select | QItemSelectionModel::Rows); + foreach(WayPointItem * wp, list) { + QModelIndex index = model->index(wp->Number(), 0); + + selection->setCurrentIndex(index, QItemSelectionModel::Select | QItemSelectionModel::Rows); } } modelMapProxy::overlayType modelMapProxy::overlayTranslate(int type) { - switch(type) - { + switch (type) { case MapDataDelegate::MODE_FLYENDPOINT: case MapDataDelegate::MODE_FLYVECTOR: case MapDataDelegate::MODE_DRIVEENDPOINT: case MapDataDelegate::MODE_DRIVEVECTOR: - return OVERLAY_LINE; + return OVERLAY_LINE; + break; case MapDataDelegate::MODE_FLYCIRCLERIGHT: case MapDataDelegate::MODE_DRIVECIRCLERIGHT: return OVERLAY_CIRCLE_RIGHT; - break; + + break; case MapDataDelegate::MODE_FLYCIRCLELEFT: case MapDataDelegate::MODE_DRIVECIRCLELEFT: return OVERLAY_CIRCLE_LEFT; + break; default: break; } } -void modelMapProxy::createOverlay(WayPointItem *from, WayPointItem *to, modelMapProxy::overlayType type,QColor color) +void modelMapProxy::createOverlay(WayPointItem *from, WayPointItem *to, modelMapProxy::overlayType type, QColor color) { - if(from==NULL || to==NULL || from==to) + if (from == NULL || to == NULL || from == to) { return; - switch(type) - { + } + switch (type) { case OVERLAY_LINE: - myMap->WPLineCreate(from,to,color); + myMap->WPLineCreate(from, to, color); break; case OVERLAY_CIRCLE_RIGHT: - myMap->WPCircleCreate(to,from,true,color); + myMap->WPCircleCreate(to, from, true, color); break; case OVERLAY_CIRCLE_LEFT: - myMap->WPCircleCreate(to,from,false,color); + myMap->WPCircleCreate(to, from, false, color); break; default: break; - } } -void modelMapProxy::createOverlay(WayPointItem *from, HomeItem *to, modelMapProxy::overlayType type,QColor color) +void modelMapProxy::createOverlay(WayPointItem *from, HomeItem *to, modelMapProxy::overlayType type, QColor color) { - if(from==NULL || to==NULL) + if (from == NULL || to == NULL) { return; - switch(type) - { + } + switch (type) { case OVERLAY_LINE: - myMap->WPLineCreate(to,from,color); + myMap->WPLineCreate(to, from, color); break; case OVERLAY_CIRCLE_RIGHT: - myMap->WPCircleCreate(to,from,true,color); + myMap->WPCircleCreate(to, from, true, color); break; case OVERLAY_CIRCLE_LEFT: - myMap->WPCircleCreate(to,from,false,color); + myMap->WPCircleCreate(to, from, false, color); break; default: break; - } } void modelMapProxy::refreshOverlays() { myMap->deleteAllOverlays(); - if(model->rowCount()<1) + if (model->rowCount() < 1) { return; - WayPointItem * wp_current=NULL; - WayPointItem * wp_next=NULL; + } + WayPointItem *wp_current = NULL; + WayPointItem *wp_next = NULL; int wp_jump; int wp_error; overlayType wp_next_overlay; overlayType wp_jump_overlay; overlayType wp_error_overlay; - wp_current=findWayPointNumber(0); - overlayType wp_current_overlay=overlayTranslate(model->data(model->index(0,flightDataModel::MODE)).toInt()); - createOverlay(wp_current,myMap->Home,wp_current_overlay,Qt::green); - for(int x=0;xrowCount();++x) - { - wp_current=findWayPointNumber(x); - wp_jump=model->data(model->index(x,flightDataModel::JUMPDESTINATION)).toInt()-1; - wp_error=model->data(model->index(x,flightDataModel::ERRORDESTINATION)).toInt()-1; - wp_next_overlay=overlayTranslate(model->data(model->index(x+1,flightDataModel::MODE)).toInt()); - wp_jump_overlay=overlayTranslate(model->data(model->index(wp_jump,flightDataModel::MODE)).toInt()); - wp_error_overlay=overlayTranslate(model->data(model->index(wp_error,flightDataModel::MODE)).toInt()); - createOverlay(wp_current,findWayPointNumber(wp_error),wp_error_overlay,Qt::red); - switch(model->data(model->index(x,flightDataModel::COMMAND)).toInt()) - { + wp_current = findWayPointNumber(0); + overlayType wp_current_overlay = overlayTranslate(model->data(model->index(0, flightDataModel::MODE)).toInt()); + createOverlay(wp_current, myMap->Home, wp_current_overlay, Qt::green); + for (int x = 0; x < model->rowCount(); ++x) { + wp_current = findWayPointNumber(x); + wp_jump = model->data(model->index(x, flightDataModel::JUMPDESTINATION)).toInt() - 1; + wp_error = model->data(model->index(x, flightDataModel::ERRORDESTINATION)).toInt() - 1; + wp_next_overlay = overlayTranslate(model->data(model->index(x + 1, flightDataModel::MODE)).toInt()); + wp_jump_overlay = overlayTranslate(model->data(model->index(wp_jump, flightDataModel::MODE)).toInt()); + wp_error_overlay = overlayTranslate(model->data(model->index(wp_error, flightDataModel::MODE)).toInt()); + createOverlay(wp_current, findWayPointNumber(wp_error), wp_error_overlay, Qt::red); + switch (model->data(model->index(x, flightDataModel::COMMAND)).toInt()) { case MapDataDelegate::COMMAND_ONCONDITIONNEXTWAYPOINT: - wp_next=findWayPointNumber(x+1); - createOverlay(wp_current,wp_next,wp_next_overlay,Qt::green); + wp_next = findWayPointNumber(x + 1); + createOverlay(wp_current, wp_next, wp_next_overlay, Qt::green); break; case MapDataDelegate::COMMAND_ONCONDITIONJUMPWAYPOINT: - wp_next=findWayPointNumber(wp_jump); - createOverlay(wp_current,wp_next,wp_jump_overlay,Qt::green); + wp_next = findWayPointNumber(wp_jump); + createOverlay(wp_current, wp_next, wp_jump_overlay, Qt::green); break; case MapDataDelegate::COMMAND_ONNOTCONDITIONJUMPWAYPOINT: - wp_next=findWayPointNumber(wp_jump); - createOverlay(wp_current,wp_next,wp_jump_overlay,Qt::yellow); + wp_next = findWayPointNumber(wp_jump); + createOverlay(wp_current, wp_next, wp_jump_overlay, Qt::yellow); break; case MapDataDelegate::COMMAND_ONNOTCONDITIONNEXTWAYPOINT: - wp_next=findWayPointNumber(x+1); - createOverlay(wp_current,wp_next,wp_next_overlay,Qt::yellow); + wp_next = findWayPointNumber(x + 1); + createOverlay(wp_current, wp_next, wp_next_overlay, Qt::yellow); break; case MapDataDelegate::COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: - wp_next=findWayPointNumber(wp_jump); - createOverlay(wp_current,wp_next,wp_jump_overlay,Qt::green); - wp_next=findWayPointNumber(x+1); - createOverlay(wp_current,wp_next,wp_next_overlay,Qt::green); + wp_next = findWayPointNumber(wp_jump); + createOverlay(wp_current, wp_next, wp_jump_overlay, Qt::green); + wp_next = findWayPointNumber(x + 1); + createOverlay(wp_current, wp_next, wp_next_overlay, Qt::green); break; } } } -WayPointItem * modelMapProxy::findWayPointNumber(int number) +WayPointItem *modelMapProxy::findWayPointNumber(int number) { - if(number<0) + if (number < 0) { return NULL; + } return myMap->WPFind(number); } @@ -206,8 +209,7 @@ void modelMapProxy::rowsRemoved(const QModelIndex &parent, int first, int last) { Q_UNUSED(parent); - for(int x=last;x>first-1;x--) - { + for (int x = last; x > first - 1; x--) { myMap->WPDelete(x); } refreshOverlays(); @@ -217,76 +219,77 @@ void modelMapProxy::dataChanged(const QModelIndex &topLeft, const QModelIndex &b { Q_UNUSED(bottomRight); - WayPointItem * item=findWayPointNumber(topLeft.row()); - if(!item) + WayPointItem *item = findWayPointNumber(topLeft.row()); + if (!item) { return; + } internals::PointLatLng latlng; - int x=topLeft.row(); + int x = topLeft.row(); distBearingAltitude distBearing; double altitude; bool relative; QModelIndex index; QString desc; - switch(topLeft.column()) - { + switch (topLeft.column()) { case flightDataModel::COMMAND: case flightDataModel::CONDITION: case flightDataModel::JUMPDESTINATION: case flightDataModel::ERRORDESTINATION: case flightDataModel::MODE: refreshOverlays(); - break; + break; case flightDataModel::WPDESCRITPTION: - index=model->index(x,flightDataModel::WPDESCRITPTION); - desc=index.data(Qt::DisplayRole).toString(); + index = model->index(x, flightDataModel::WPDESCRITPTION); + desc = index.data(Qt::DisplayRole).toString(); item->SetDescription(desc); break; case flightDataModel::LATPOSITION: - latlng=item->Coord(); - index=model->index(x,flightDataModel::LATPOSITION); + latlng = item->Coord(); + index = model->index(x, flightDataModel::LATPOSITION); latlng.SetLat(index.data(Qt::DisplayRole).toDouble()); item->SetCoord(latlng); break; case flightDataModel::LNGPOSITION: - latlng=item->Coord(); - index=model->index(x,flightDataModel::LNGPOSITION); + latlng = item->Coord(); + index = model->index(x, flightDataModel::LNGPOSITION); latlng.SetLng(index.data(Qt::DisplayRole).toDouble()); item->SetCoord(latlng); break; case flightDataModel::BEARELATIVE: - distBearing=item->getRelativeCoord(); - index=model->index(x,flightDataModel::BEARELATIVE); + distBearing = item->getRelativeCoord(); + index = model->index(x, flightDataModel::BEARELATIVE); distBearing.setBearingFromDegrees(index.data(Qt::DisplayRole).toDouble()); item->setRelativeCoord(distBearing); break; case flightDataModel::DISRELATIVE: - distBearing=item->getRelativeCoord(); - index=model->index(x,flightDataModel::DISRELATIVE); - distBearing.distance=index.data(Qt::DisplayRole).toDouble(); + distBearing = item->getRelativeCoord(); + index = model->index(x, flightDataModel::DISRELATIVE); + distBearing.distance = index.data(Qt::DisplayRole).toDouble(); item->setRelativeCoord(distBearing); break; case flightDataModel::ALTITUDERELATIVE: - distBearing=item->getRelativeCoord(); - index=model->index(x,flightDataModel::ALTITUDERELATIVE); - distBearing.altitudeRelative=index.data(Qt::DisplayRole).toFloat(); + distBearing = item->getRelativeCoord(); + index = model->index(x, flightDataModel::ALTITUDERELATIVE); + distBearing.altitudeRelative = index.data(Qt::DisplayRole).toFloat(); item->setRelativeCoord(distBearing); break; case flightDataModel::ISRELATIVE: - index=model->index(x,flightDataModel::ISRELATIVE); - relative=index.data(Qt::DisplayRole).toBool(); - if(relative) + index = model->index(x, flightDataModel::ISRELATIVE); + relative = index.data(Qt::DisplayRole).toBool(); + if (relative) { item->setWPType(mapcontrol::WayPointItem::relative); - else + } else { item->setWPType(mapcontrol::WayPointItem::absolute); + } break; case flightDataModel::ALTITUDE: - index=model->index(x,flightDataModel::ALTITUDE); - altitude=index.data(Qt::DisplayRole).toDouble(); + index = model->index(x, flightDataModel::ALTITUDE); + altitude = index.data(Qt::DisplayRole).toDouble(); item->SetAltitude(altitude); break; case flightDataModel::LOCKED: - index=model->index(x,flightDataModel::LOCKED); - item->setFlag(QGraphicsItem::ItemIsMovable,!index.data(Qt::DisplayRole).toBool()); + index = model->index(x, flightDataModel::LOCKED); + item->setFlag(QGraphicsItem::ItemIsMovable, !index.data(Qt::DisplayRole).toBool()); break; } } @@ -294,55 +297,55 @@ void modelMapProxy::dataChanged(const QModelIndex &topLeft, const QModelIndex &b void modelMapProxy::rowsInserted(const QModelIndex &parent, int first, int last) { Q_UNUSED(parent); - for(int x=first;xindex(x,flightDataModel::WPDESCRITPTION); - QString desc=index.data(Qt::DisplayRole).toString(); - index=model->index(x,flightDataModel::LATPOSITION); + index = model->index(x, flightDataModel::WPDESCRITPTION); + QString desc = index.data(Qt::DisplayRole).toString(); + index = model->index(x, flightDataModel::LATPOSITION); latlng.SetLat(index.data(Qt::DisplayRole).toDouble()); - index=model->index(x,flightDataModel::LNGPOSITION); + index = model->index(x, flightDataModel::LNGPOSITION); latlng.SetLng(index.data(Qt::DisplayRole).toDouble()); - index=model->index(x,flightDataModel::DISRELATIVE); - distBearing.distance=index.data(Qt::DisplayRole).toDouble(); - index=model->index(x,flightDataModel::BEARELATIVE); + index = model->index(x, flightDataModel::DISRELATIVE); + distBearing.distance = index.data(Qt::DisplayRole).toDouble(); + index = model->index(x, flightDataModel::BEARELATIVE); distBearing.setBearingFromDegrees(index.data(Qt::DisplayRole).toDouble()); - index=model->index(x,flightDataModel::ALTITUDERELATIVE); - distBearing.altitudeRelative=index.data(Qt::DisplayRole).toFloat(); - index=model->index(x,flightDataModel::ISRELATIVE); - relative=index.data(Qt::DisplayRole).toBool(); - index=model->index(x,flightDataModel::ALTITUDE); - altitude=index.data(Qt::DisplayRole).toDouble(); - if(relative) - item=myMap->WPInsert(distBearing,desc,x); - else - item=myMap->WPInsert(latlng,altitude,desc,x); + index = model->index(x, flightDataModel::ALTITUDERELATIVE); + distBearing.altitudeRelative = index.data(Qt::DisplayRole).toFloat(); + index = model->index(x, flightDataModel::ISRELATIVE); + relative = index.data(Qt::DisplayRole).toBool(); + index = model->index(x, flightDataModel::ALTITUDE); + altitude = index.data(Qt::DisplayRole).toDouble(); + if (relative) { + item = myMap->WPInsert(distBearing, desc, x); + } else { + item = myMap->WPInsert(latlng, altitude, desc, x); + } } refreshOverlays(); } void modelMapProxy::deleteWayPoint(int number) { - model->removeRow(number,QModelIndex()); + model->removeRow(number, QModelIndex()); } void modelMapProxy::createWayPoint(internals::PointLatLng coord) { - model->insertRow(model->rowCount(),QModelIndex()); - QModelIndex index=model->index(model->rowCount()-1,flightDataModel::LATPOSITION,QModelIndex()); - model->setData(index,coord.Lat(),Qt::EditRole); - index=model->index(model->rowCount()-1,flightDataModel::LNGPOSITION,QModelIndex()); - model->setData(index,coord.Lng(),Qt::EditRole); - index=model->index(model->rowCount()-1,flightDataModel::JUMPDESTINATION,QModelIndex()); - model->setData(index,1,Qt::EditRole); - index=model->index(model->rowCount()-1,flightDataModel::ERRORDESTINATION,QModelIndex()); - model->setData(index,1,Qt::EditRole); + model->insertRow(model->rowCount(), QModelIndex()); + QModelIndex index = model->index(model->rowCount() - 1, flightDataModel::LATPOSITION, QModelIndex()); + model->setData(index, coord.Lat(), Qt::EditRole); + index = model->index(model->rowCount() - 1, flightDataModel::LNGPOSITION, QModelIndex()); + model->setData(index, coord.Lng(), Qt::EditRole); + index = model->index(model->rowCount() - 1, flightDataModel::JUMPDESTINATION, QModelIndex()); + model->setData(index, 1, Qt::EditRole); + index = model->index(model->rowCount() - 1, flightDataModel::ERRORDESTINATION, QModelIndex()); + model->setData(index, 1, Qt::EditRole); } void modelMapProxy::deleteAll() { - model->removeRows(0,model->rowCount(),QModelIndex()); + model->removeRows(0, model->rowCount(), QModelIndex()); } diff --git a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.h b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.h index 29c61c9b1..04269c9ba 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.h @@ -38,31 +38,30 @@ using namespace mapcontrol; -class modelMapProxy:public QObject -{ - typedef enum {OVERLAY_LINE,OVERLAY_CIRCLE_RIGHT,OVERLAY_CIRCLE_LEFT} overlayType; +class modelMapProxy : public QObject { + typedef enum { OVERLAY_LINE, OVERLAY_CIRCLE_RIGHT, OVERLAY_CIRCLE_LEFT } overlayType; Q_OBJECT public: - explicit modelMapProxy(QObject *parent,OPMapWidget * map,flightDataModel * model,QItemSelectionModel * selectionModel); + explicit modelMapProxy(QObject *parent, OPMapWidget *map, flightDataModel *model, QItemSelectionModel *selectionModel); WayPointItem *findWayPointNumber(int number); void createWayPoint(internals::PointLatLng coord); void deleteWayPoint(int number); void deleteAll(); private slots: - void dataChanged ( const QModelIndex & topLeft, const QModelIndex & bottomRight ); - void rowsInserted ( const QModelIndex & parent, int first, int last ); - void rowsRemoved ( const QModelIndex & parent, int first, int last ); + void dataChanged(const QModelIndex & topLeft, const QModelIndex & bottomRight); + void rowsInserted(const QModelIndex & parent, int first, int last); + void rowsRemoved(const QModelIndex & parent, int first, int last); void WPValuesChanged(WayPointItem *wp); - void currentRowChanged(QModelIndex,QModelIndex); - void selectedWPChanged(QList); + void currentRowChanged(QModelIndex, QModelIndex); + void selectedWPChanged(QList); private: overlayType overlayTranslate(int type); - void createOverlay(WayPointItem * from,WayPointItem * to,overlayType type,QColor color); + void createOverlay(WayPointItem *from, WayPointItem *to, overlayType type, QColor color); void createOverlay(WayPointItem *from, HomeItem *to, modelMapProxy::overlayType type, QColor color); - OPMapWidget * myMap; - flightDataModel * model; + OPMapWidget *myMap; + flightDataModel *model; void refreshOverlays(); - QItemSelectionModel * selection; + QItemSelectionModel *selection; }; #endif // MODELMAPPROXY_H diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index 192f5b932..bc8c580ed 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -27,93 +27,92 @@ #include "modeluavoproxy.h" #include "extensionsystem/pluginmanager.h" #include -modelUavoProxy::modelUavoProxy(QObject *parent,flightDataModel * model):QObject(parent),myModel(model) +modelUavoProxy::modelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm != NULL); - objManager = pm->getObject(); + objManager = pm->getObject(); Q_ASSERT(objManager != NULL); - waypointObj = Waypoint::GetInstance(objManager); + waypointObj = Waypoint::GetInstance(objManager); Q_ASSERT(waypointObj != NULL); - pathactionObj=PathAction::GetInstance(objManager); + pathactionObj = PathAction::GetInstance(objManager); Q_ASSERT(pathactionObj != NULL); } void modelUavoProxy::modelToObjects() { - PathAction * act=NULL; - Waypoint * wp=NULL; + PathAction *act = NULL; + Waypoint *wp = NULL; QModelIndex index; double distance; double bearing; double altitude; - int lastaction=-1; - for(int x=0;xrowCount();++x) - { - int instances=objManager->getNumInstances(waypointObj->getObjID()); - if(x>instances-1) - { - wp=new Waypoint; - wp->initialize(x,wp->getMetaObject()); + int lastaction = -1; + + for (int x = 0; x < myModel->rowCount(); ++x) { + int instances = objManager->getNumInstances(waypointObj->getObjID()); + if (x > instances - 1) { + wp = new Waypoint; + wp->initialize(x, wp->getMetaObject()); objManager->registerObject(wp); + } else { + wp = Waypoint::GetInstance(objManager, x); } - else - { - wp=Waypoint::GetInstance(objManager,x); - } - act=new PathAction; + act = new PathAction; Q_ASSERT(act); Q_ASSERT(wp); Waypoint::DataFields waypoint = wp->getData(); PathAction::DataFields action = act->getData(); ///Waypoint object data - index=myModel->index(x,flightDataModel::DISRELATIVE); - distance=myModel->data(index).toDouble(); - index=myModel->index(x,flightDataModel::BEARELATIVE); - bearing=myModel->data(index).toDouble(); - index=myModel->index(x,flightDataModel::ALTITUDERELATIVE); - altitude=myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::VELOCITY); - waypoint.Velocity=myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::DISRELATIVE); + distance = myModel->data(index).toDouble(); + index = myModel->index(x, flightDataModel::BEARELATIVE); + bearing = myModel->data(index).toDouble(); + index = myModel->index(x, flightDataModel::ALTITUDERELATIVE); + altitude = myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::VELOCITY); + waypoint.Velocity = myModel->data(index).toFloat(); - waypoint.Position[Waypoint::POSITION_NORTH]=distance*cos(bearing/180*M_PI); - waypoint.Position[Waypoint::POSITION_EAST]=distance*sin(bearing/180*M_PI); - waypoint.Position[Waypoint::POSITION_DOWN]=(-1.0f)*altitude; + waypoint.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / 180 * M_PI); + waypoint.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / 180 * M_PI); + waypoint.Position[Waypoint::POSITION_DOWN] = (-1.0f) * altitude; ///PathAction object data - index=myModel->index(x,flightDataModel::MODE); - action.Mode=myModel->data(index).toInt(); - index=myModel->index(x,flightDataModel::MODE_PARAMS0); - action.ModeParameters[0]=myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::MODE_PARAMS1); - action.ModeParameters[1]=myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::MODE_PARAMS2); - action.ModeParameters[2]=myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::MODE_PARAMS3); - action.ModeParameters[3]=myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::MODE); + action.Mode = myModel->data(index).toInt(); + index = myModel->index(x, flightDataModel::MODE_PARAMS0); + action.ModeParameters[0] = myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::MODE_PARAMS1); + action.ModeParameters[1] = myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::MODE_PARAMS2); + action.ModeParameters[2] = myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::MODE_PARAMS3); + action.ModeParameters[3] = myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::CONDITION); - action.EndCondition=myModel->data(index).toInt(); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS0); - action.ConditionParameters[0]=myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS1); - action.ConditionParameters[1]=myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS2); - action.ConditionParameters[2]=myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS3); - action.ConditionParameters[3]=myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::CONDITION); + action.EndCondition = myModel->data(index).toInt(); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS0); + action.ConditionParameters[0] = myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS1); + action.ConditionParameters[1] = myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS2); + action.ConditionParameters[2] = myModel->data(index).toFloat(); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS3); + action.ConditionParameters[3] = myModel->data(index).toFloat(); - index=myModel->index(x,flightDataModel::COMMAND); - action.Command=myModel->data(index).toInt(); - index=myModel->index(x,flightDataModel::JUMPDESTINATION); - action.JumpDestination=myModel->data(index).toInt()-1; - index=myModel->index(x,flightDataModel::ERRORDESTINATION); - action.ErrorDestination=myModel->data(index).toInt()-1; + index = myModel->index(x, flightDataModel::COMMAND); + action.Command = myModel->data(index).toInt(); + index = myModel->index(x, flightDataModel::JUMPDESTINATION); + action.JumpDestination = myModel->data(index).toInt() - 1; + index = myModel->index(x, flightDataModel::ERRORDESTINATION); + action.ErrorDestination = myModel->data(index).toInt() - 1; - int actionNumber=addAction(act,action,lastaction); - if(actionNumber>lastaction) - lastaction=actionNumber; - waypoint.Action=actionNumber; + int actionNumber = addAction(act, action, lastaction); + if (actionNumber > lastaction) { + lastaction = actionNumber; + } + waypoint.Action = actionNumber; wp->setData(waypoint); wp->updated(); } @@ -121,129 +120,128 @@ void modelUavoProxy::modelToObjects() void modelUavoProxy::objectsToModel() { - Waypoint * wp; + Waypoint *wp; Waypoint::DataFields wpfields; - PathAction * action; + PathAction *action; QModelIndex index; double distance; double bearing; PathAction::DataFields actionfields; - myModel->removeRows(0,myModel->rowCount()); - for(int x=0;xgetNumInstances(waypointObj->getObjID());++x) - { - wp=Waypoint::GetInstance(objManager,x); + myModel->removeRows(0, myModel->rowCount()); + for (int x = 0; x < objManager->getNumInstances(waypointObj->getObjID()); ++x) { + wp = Waypoint::GetInstance(objManager, x); Q_ASSERT(wp); - if(!wp) + if (!wp) { continue; - wpfields=wp->getData(); + } + wpfields = wp->getData(); myModel->insertRow(x); - index=myModel->index(x,flightDataModel::VELOCITY); - myModel->setData(index,wpfields.Velocity); - distance=sqrt(wpfields.Position[Waypoint::POSITION_NORTH]*wpfields.Position[Waypoint::POSITION_NORTH]+ - wpfields.Position[Waypoint::POSITION_EAST]*wpfields.Position[Waypoint::POSITION_EAST]); - bearing=atan2(wpfields.Position[Waypoint::POSITION_EAST],wpfields.Position[Waypoint::POSITION_NORTH])*180/M_PI; + index = myModel->index(x, flightDataModel::VELOCITY); + myModel->setData(index, wpfields.Velocity); + distance = sqrt(wpfields.Position[Waypoint::POSITION_NORTH] * wpfields.Position[Waypoint::POSITION_NORTH] + + wpfields.Position[Waypoint::POSITION_EAST] * wpfields.Position[Waypoint::POSITION_EAST]); + bearing = atan2(wpfields.Position[Waypoint::POSITION_EAST], wpfields.Position[Waypoint::POSITION_NORTH]) * 180 / M_PI; - if(bearing!=bearing) - bearing=0; - index=myModel->index(x,flightDataModel::DISRELATIVE); - myModel->setData(index,distance); - index=myModel->index(x,flightDataModel::BEARELATIVE); - myModel->setData(index,bearing); - index=myModel->index(x,flightDataModel::ALTITUDERELATIVE); - myModel->setData(index,(-1.0f)*wpfields.Position[Waypoint::POSITION_DOWN]); + if (bearing != bearing) { + bearing = 0; + } + index = myModel->index(x, flightDataModel::DISRELATIVE); + myModel->setData(index, distance); + index = myModel->index(x, flightDataModel::BEARELATIVE); + myModel->setData(index, bearing); + index = myModel->index(x, flightDataModel::ALTITUDERELATIVE); + myModel->setData(index, (-1.0f) * wpfields.Position[Waypoint::POSITION_DOWN]); - action=PathAction::GetInstance(objManager,wpfields.Action); + action = PathAction::GetInstance(objManager, wpfields.Action); Q_ASSERT(action); - if(!action) + if (!action) { continue; - actionfields=action->getData(); + } + actionfields = action->getData(); - index=myModel->index(x,flightDataModel::ISRELATIVE); - myModel->setData(index,true); + index = myModel->index(x, flightDataModel::ISRELATIVE); + myModel->setData(index, true); - index=myModel->index(x,flightDataModel::COMMAND); - myModel->setData(index,actionfields.Command); + index = myModel->index(x, flightDataModel::COMMAND); + myModel->setData(index, actionfields.Command); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS0); - myModel->setData(index,actionfields.ConditionParameters[0]); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS1); - myModel->setData(index,actionfields.ConditionParameters[1]); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS2); - myModel->setData(index,actionfields.ConditionParameters[2]); - index=myModel->index(x,flightDataModel::CONDITION_PARAMS3); - myModel->setData(index,actionfields.ConditionParameters[3]); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS0); + myModel->setData(index, actionfields.ConditionParameters[0]); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS1); + myModel->setData(index, actionfields.ConditionParameters[1]); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS2); + myModel->setData(index, actionfields.ConditionParameters[2]); + index = myModel->index(x, flightDataModel::CONDITION_PARAMS3); + myModel->setData(index, actionfields.ConditionParameters[3]); - index=myModel->index(x,flightDataModel::CONDITION); - myModel->setData(index,actionfields.EndCondition); + index = myModel->index(x, flightDataModel::CONDITION); + myModel->setData(index, actionfields.EndCondition); - index=myModel->index(x,flightDataModel::ERRORDESTINATION); - myModel->setData(index,actionfields.ErrorDestination+1); + index = myModel->index(x, flightDataModel::ERRORDESTINATION); + myModel->setData(index, actionfields.ErrorDestination + 1); - index=myModel->index(x,flightDataModel::JUMPDESTINATION); - myModel->setData(index,actionfields.JumpDestination+1); + index = myModel->index(x, flightDataModel::JUMPDESTINATION); + myModel->setData(index, actionfields.JumpDestination + 1); - index=myModel->index(x,flightDataModel::MODE); - myModel->setData(index,actionfields.Mode); + index = myModel->index(x, flightDataModel::MODE); + myModel->setData(index, actionfields.Mode); - index=myModel->index(x,flightDataModel::MODE_PARAMS0); - myModel->setData(index,actionfields.ModeParameters[0]); - index=myModel->index(x,flightDataModel::MODE_PARAMS1); - myModel->setData(index,actionfields.ModeParameters[1]); - index=myModel->index(x,flightDataModel::MODE_PARAMS2); - myModel->setData(index,actionfields.ModeParameters[2]); - index=myModel->index(x,flightDataModel::MODE_PARAMS3); - myModel->setData(index,actionfields.ModeParameters[3]); + index = myModel->index(x, flightDataModel::MODE_PARAMS0); + myModel->setData(index, actionfields.ModeParameters[0]); + index = myModel->index(x, flightDataModel::MODE_PARAMS1); + myModel->setData(index, actionfields.ModeParameters[1]); + index = myModel->index(x, flightDataModel::MODE_PARAMS2); + myModel->setData(index, actionfields.ModeParameters[2]); + index = myModel->index(x, flightDataModel::MODE_PARAMS3); + myModel->setData(index, actionfields.ModeParameters[3]); } } -int modelUavoProxy::addAction(PathAction * actionObj,PathAction::DataFields actionFields,int lastaction) +int modelUavoProxy::addAction(PathAction *actionObj, PathAction::DataFields actionFields, int lastaction) { - //check if a similar action already exhists - int instances=objManager->getNumInstances(pathactionObj->getObjID()); - for(int x=0;xgetNumInstances(pathactionObj->getObjID()); + + for (int x = 0; x < lastaction + 1; ++x) { + PathAction *action = PathAction::GetInstance(objManager, x); Q_ASSERT(action); - if(!action) + if (!action) { continue; - PathAction::DataFields fields=action->getData(); - if(fields.Command==actionFields.Command - && fields.ConditionParameters[0]==actionFields.ConditionParameters[0] - && fields.ConditionParameters[1]==actionFields.ConditionParameters[1] - && fields.ConditionParameters[2]==actionFields.ConditionParameters[2] - &&fields.EndCondition==actionFields.EndCondition - &&fields.ErrorDestination==actionFields.ErrorDestination - &&fields.JumpDestination==actionFields.JumpDestination - &&fields.Mode==actionFields.Mode - &&fields.ModeParameters[0]==actionFields.ModeParameters[0] - &&fields.ModeParameters[1]==actionFields.ModeParameters[1] - &&fields.ModeParameters[2]==actionFields.ModeParameters[2]) - { - qDebug()<<"ModelUAVProxy:"<<"found similar action instance:"<getData(); + if (fields.Command == actionFields.Command + && fields.ConditionParameters[0] == actionFields.ConditionParameters[0] + && fields.ConditionParameters[1] == actionFields.ConditionParameters[1] + && fields.ConditionParameters[2] == actionFields.ConditionParameters[2] + && fields.EndCondition == actionFields.EndCondition + && fields.ErrorDestination == actionFields.ErrorDestination + && fields.JumpDestination == actionFields.JumpDestination + && fields.Mode == actionFields.Mode + && fields.ModeParameters[0] == actionFields.ModeParameters[0] + && fields.ModeParameters[1] == actionFields.ModeParameters[1] + && fields.ModeParameters[2] == actionFields.ModeParameters[2]) { + qDebug() << "ModelUAVProxy:" << "found similar action instance:" << x; actionObj->deleteLater(); return x; } } - //if we get here it means no similar action was found, we have to create it - if(instancesinitialize(instances,actionObj->getMetaObject()); + // if we get here it means no similar action was found, we have to create it + if (instances < lastaction + 2) { + actionObj->initialize(instances, actionObj->getMetaObject()); objManager->registerObject(actionObj); actionObj->setData(actionFields); actionObj->updated(); - qDebug()<<"ModelUAVProxy:"<<"created new action instance:"<setData(actionFields); action->updated(); actionObj->deleteLater(); - qDebug()<<"ModelUAVProxy:"<<"reused action instance:"<setupUi(this); - connect(ui->checkBoxLocked,SIGNAL(toggled(bool)),this,SLOT(enableEditWidgets(bool))); - connect(ui->cbMode,SIGNAL(currentIndexChanged(int)),this,SLOT(setupModeWidgets())); - connect(ui->cbCondition,SIGNAL(currentIndexChanged(int)),this,SLOT(setupConditionWidgets())); - connect(ui->pushButtonCancel,SIGNAL(clicked()),this,SLOT(pushButtonCancel_clicked())); - MapDataDelegate::loadComboBox(ui->cbMode,flightDataModel::MODE); - MapDataDelegate::loadComboBox(ui->cbCondition,flightDataModel::CONDITION); - MapDataDelegate::loadComboBox(ui->cbCommand,flightDataModel::COMMAND); + connect(ui->checkBoxLocked, SIGNAL(toggled(bool)), this, SLOT(enableEditWidgets(bool))); + connect(ui->cbMode, SIGNAL(currentIndexChanged(int)), this, SLOT(setupModeWidgets())); + connect(ui->cbCondition, SIGNAL(currentIndexChanged(int)), this, SLOT(setupConditionWidgets())); + connect(ui->pushButtonCancel, SIGNAL(clicked()), this, SLOT(pushButtonCancel_clicked())); + MapDataDelegate::loadComboBox(ui->cbMode, flightDataModel::MODE); + MapDataDelegate::loadComboBox(ui->cbCondition, flightDataModel::CONDITION); + MapDataDelegate::loadComboBox(ui->cbCommand, flightDataModel::COMMAND); mapper = new QDataWidgetMapper(this); mapper->setItemDelegate(new MapDataDelegate(this)); - connect(mapper,SIGNAL(currentIndexChanged(int)),this,SLOT(currentIndexChanged(int))); + connect(mapper, SIGNAL(currentIndexChanged(int)), this, SLOT(currentIndexChanged(int))); mapper->setModel(model); mapper->setSubmitPolicy(QDataWidgetMapper::AutoSubmit); - mapper->addMapping(ui->checkBoxLocked,flightDataModel::LOCKED); - mapper->addMapping(ui->doubleSpinBoxLatitude,flightDataModel::LATPOSITION); - mapper->addMapping(ui->doubleSpinBoxLongitude,flightDataModel::LNGPOSITION); - mapper->addMapping(ui->doubleSpinBoxAltitude,flightDataModel::ALTITUDE); - mapper->addMapping(ui->lineEditDescription,flightDataModel::WPDESCRITPTION); - mapper->addMapping(ui->checkBoxRelative,flightDataModel::ISRELATIVE); - mapper->addMapping(ui->doubleSpinBoxBearing,flightDataModel::BEARELATIVE); - mapper->addMapping(ui->doubleSpinBoxVelocity,flightDataModel::VELOCITY); - mapper->addMapping(ui->doubleSpinBoxDistance,flightDataModel::DISRELATIVE); - mapper->addMapping(ui->doubleSpinBoxRelativeAltitude,flightDataModel::ALTITUDERELATIVE); - mapper->addMapping(ui->cbMode,flightDataModel::MODE); - mapper->addMapping(ui->dsb_modeParam1,flightDataModel::MODE_PARAMS0); - mapper->addMapping(ui->dsb_modeParam2,flightDataModel::MODE_PARAMS1); - mapper->addMapping(ui->dsb_modeParam3,flightDataModel::MODE_PARAMS2); - mapper->addMapping(ui->dsb_modeParam4,flightDataModel::MODE_PARAMS3); + mapper->addMapping(ui->checkBoxLocked, flightDataModel::LOCKED); + mapper->addMapping(ui->doubleSpinBoxLatitude, flightDataModel::LATPOSITION); + mapper->addMapping(ui->doubleSpinBoxLongitude, flightDataModel::LNGPOSITION); + mapper->addMapping(ui->doubleSpinBoxAltitude, flightDataModel::ALTITUDE); + mapper->addMapping(ui->lineEditDescription, flightDataModel::WPDESCRITPTION); + mapper->addMapping(ui->checkBoxRelative, flightDataModel::ISRELATIVE); + mapper->addMapping(ui->doubleSpinBoxBearing, flightDataModel::BEARELATIVE); + mapper->addMapping(ui->doubleSpinBoxVelocity, flightDataModel::VELOCITY); + mapper->addMapping(ui->doubleSpinBoxDistance, flightDataModel::DISRELATIVE); + mapper->addMapping(ui->doubleSpinBoxRelativeAltitude, flightDataModel::ALTITUDERELATIVE); + mapper->addMapping(ui->cbMode, flightDataModel::MODE); + mapper->addMapping(ui->dsb_modeParam1, flightDataModel::MODE_PARAMS0); + mapper->addMapping(ui->dsb_modeParam2, flightDataModel::MODE_PARAMS1); + mapper->addMapping(ui->dsb_modeParam3, flightDataModel::MODE_PARAMS2); + mapper->addMapping(ui->dsb_modeParam4, flightDataModel::MODE_PARAMS3); - mapper->addMapping(ui->cbCondition,flightDataModel::CONDITION); - mapper->addMapping(ui->dsb_condParam1,flightDataModel::CONDITION_PARAMS0); - mapper->addMapping(ui->dsb_condParam2,flightDataModel::CONDITION_PARAMS1); - mapper->addMapping(ui->dsb_condParam3,flightDataModel::CONDITION_PARAMS2); - mapper->addMapping(ui->dsb_condParam4,flightDataModel::CONDITION_PARAMS0); + mapper->addMapping(ui->cbCondition, flightDataModel::CONDITION); + mapper->addMapping(ui->dsb_condParam1, flightDataModel::CONDITION_PARAMS0); + mapper->addMapping(ui->dsb_condParam2, flightDataModel::CONDITION_PARAMS1); + mapper->addMapping(ui->dsb_condParam3, flightDataModel::CONDITION_PARAMS2); + mapper->addMapping(ui->dsb_condParam4, flightDataModel::CONDITION_PARAMS0); - mapper->addMapping(ui->cbCommand,flightDataModel::COMMAND); - mapper->addMapping(ui->sbJump,flightDataModel::JUMPDESTINATION); - mapper->addMapping(ui->sbError,flightDataModel::ERRORDESTINATION); - connect(itemSelection,SIGNAL(currentRowChanged(QModelIndex,QModelIndex)),this,SLOT(currentRowChanged(QModelIndex,QModelIndex))); + mapper->addMapping(ui->cbCommand, flightDataModel::COMMAND); + mapper->addMapping(ui->sbJump, flightDataModel::JUMPDESTINATION); + mapper->addMapping(ui->sbError, flightDataModel::ERRORDESTINATION); + connect(itemSelection, SIGNAL(currentRowChanged(QModelIndex, QModelIndex)), this, SLOT(currentRowChanged(QModelIndex, QModelIndex))); } void opmap_edit_waypoint_dialog::currentIndexChanged(int index) { - ui->lbNumber->setText(QString::number(index+1)); - QModelIndex idx=mapper->model()->index(index,0); - if(index==itemSelection->currentIndex().row()) + ui->lbNumber->setText(QString::number(index + 1)); + QModelIndex idx = mapper->model()->index(index, 0); + if (index == itemSelection->currentIndex().row()) { return; + } itemSelection->clear(); - itemSelection->setCurrentIndex(idx,QItemSelectionModel::Select | QItemSelectionModel::Rows); + itemSelection->setCurrentIndex(idx, QItemSelectionModel::Select | QItemSelectionModel::Rows); } opmap_edit_waypoint_dialog::~opmap_edit_waypoint_dialog() @@ -100,9 +101,9 @@ void opmap_edit_waypoint_dialog::on_pushButtonOK_clicked() void opmap_edit_waypoint_dialog::setupModeWidgets() { - MapDataDelegate::ModeOptions mode=(MapDataDelegate::ModeOptions)ui->cbMode->itemData(ui->cbMode->currentIndex()).toInt(); - switch(mode) - { + MapDataDelegate::ModeOptions mode = (MapDataDelegate::ModeOptions)ui->cbMode->itemData(ui->cbMode->currentIndex()).toInt(); + + switch (mode) { case MapDataDelegate::MODE_FLYENDPOINT: case MapDataDelegate::MODE_FLYVECTOR: case MapDataDelegate::MODE_FLYCIRCLERIGHT: @@ -146,14 +147,14 @@ void opmap_edit_waypoint_dialog::setupModeWidgets() ui->dsb_modeParam2->setVisible(true); ui->dsb_modeParam3->setVisible(false); ui->dsb_modeParam4->setVisible(false); - break; + break; } } void opmap_edit_waypoint_dialog::setupConditionWidgets() { - MapDataDelegate::EndConditionOptions mode=(MapDataDelegate::EndConditionOptions)ui->cbCondition->itemData(ui->cbCondition->currentIndex()).toInt(); - switch(mode) - { + MapDataDelegate::EndConditionOptions mode = (MapDataDelegate::EndConditionOptions)ui->cbCondition->itemData(ui->cbCondition->currentIndex()).toInt(); + + switch (mode) { case MapDataDelegate::ENDCONDITION_NONE: case MapDataDelegate::ENDCONDITION_IMMEDIATE: case MapDataDelegate::ENDCONDITION_PYTHONSCRIPT: @@ -187,7 +188,7 @@ void opmap_edit_waypoint_dialog::setupConditionWidgets() ui->dsb_condParam3->setVisible(false); ui->dsb_condParam4->setVisible(false); ui->condParam1->setText("Distance(m)"); - ui->condParam2->setText("Flag(0=2D,1=3D)");//FIXME + ui->condParam2->setText("Flag(0=2D,1=3D)"); // FIXME break; case MapDataDelegate::ENDCONDITION_LEGREMAINING: ui->condParam1->setVisible(true); @@ -258,13 +259,18 @@ void opmap_edit_waypoint_dialog::pushButtonCancel_clicked() } void opmap_edit_waypoint_dialog::editWaypoint(mapcontrol::WayPointItem *waypoint_item) { - if (!waypoint_item) return; - if(!isVisible()) + if (!waypoint_item) { + return; + } + if (!isVisible()) { show(); - if(isMinimized()) + } + if (isMinimized()) { showNormal(); - if(!isActiveWindow()) + } + if (!isActiveWindow()) { activateWindow(); + } raise(); setFocus(Qt::OtherFocusReason); mapper->setCurrentIndex(waypoint_item->Number()); @@ -282,24 +288,29 @@ void opmap_edit_waypoint_dialog::on_pushButton_2_clicked() void opmap_edit_waypoint_dialog::enableEditWidgets(bool value) { - QWidget * w; - foreach(QWidget * obj,this->findChildren()) - { - w=qobject_cast(obj); - if(w) + QWidget *w; + + foreach(QWidget * obj, this->findChildren()) { + w = qobject_cast(obj); + if (w) { w->setEnabled(!value); - w=qobject_cast(obj); - if(w) + } + w = qobject_cast(obj); + if (w) { w->setEnabled(!value); - w=qobject_cast(obj); - if(w) + } + w = qobject_cast(obj); + if (w) { w->setEnabled(!value); - w=qobject_cast(obj); - if(w && w!=ui->checkBoxLocked) + } + w = qobject_cast(obj); + if (w && w != ui->checkBoxLocked) { w->setEnabled(!value); - w=qobject_cast(obj); - if(w) + } + w = qobject_cast(obj); + if (w) { w->setEnabled(!value); + } } } diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.h b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.h index 435871dbc..46dadab97 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,29 +33,28 @@ #include "opmapcontrol/opmapcontrol.h" #include "flightdatamodel.h" namespace Ui { - class opmap_edit_waypoint_dialog; +class opmap_edit_waypoint_dialog; } using namespace mapcontrol; -class opmap_edit_waypoint_dialog : public QWidget -{ +class opmap_edit_waypoint_dialog : public QWidget { Q_OBJECT public: - opmap_edit_waypoint_dialog(QWidget *parent,QAbstractItemModel * model,QItemSelectionModel * selection); + opmap_edit_waypoint_dialog(QWidget *parent, QAbstractItemModel *model, QItemSelectionModel *selection); ~opmap_edit_waypoint_dialog(); /** - * @brief public functions - * - * @param - */ + * @brief public functions + * + * @param + */ void editWaypoint(mapcontrol::WayPointItem *waypoint_item); private: Ui::opmap_edit_waypoint_dialog *ui; QDataWidgetMapper *mapper; - QAbstractItemModel * model; - QItemSelectionModel * itemSelection; + QAbstractItemModel *model; + QItemSelectionModel *itemSelection; private slots: private slots: @@ -67,7 +66,7 @@ private slots: void on_pushButton_clicked(); void on_pushButton_2_clicked(); void enableEditWidgets(bool); - void currentRowChanged(QModelIndex,QModelIndex); + void currentRowChanged(QModelIndex, QModelIndex); }; #endif // OPMAP_EDIT_WAYPOINT_DIALOG_H diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.cpp index bece61bad..32285454a 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.h b/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.h index afcd3ab23..9992ab7ef 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_statusbar_widget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -31,11 +31,10 @@ #include namespace Ui { - class opmap_statusbar_widget; +class opmap_statusbar_widget; } -class opmap_statusbar_widget : public QWidget -{ +class opmap_statusbar_widget : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.cpp index e3b19afec..83ba9c099 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.h b/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.h index bcf5055ed..077651666 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_zoom_slider_widget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -31,11 +31,10 @@ #include namespace Ui { - class opmap_zoom_slider_widget; +class opmap_zoom_slider_widget; } -class opmap_zoom_slider_widget : public QWidget -{ +class opmap_zoom_slider_widget : public QWidget { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp index a0a92cdc8..6facd9ef9 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -28,21 +28,20 @@ #include "opmapgadgetwidget.h" OPMapGadget::OPMapGadget(QString classId, OPMapGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget),m_config(NULL) + IUAVGadget(classId, parent), + m_widget(widget), m_config(NULL) { - connect(m_widget,SIGNAL(defaultLocationAndZoomChanged(double,double,double)),this,SLOT(saveDefaultLocation(double,double,double))); - connect(m_widget,SIGNAL(overlayOpacityChanged(qreal)),this,SLOT(saveOpacity(qreal))); + connect(m_widget, SIGNAL(defaultLocationAndZoomChanged(double, double, double)), this, SLOT(saveDefaultLocation(double, double, double))); + connect(m_widget, SIGNAL(overlayOpacityChanged(qreal)), this, SLOT(saveOpacity(qreal))); } OPMapGadget::~OPMapGadget() { delete m_widget; } -void OPMapGadget::saveDefaultLocation(double lng,double lat,double zoom) +void OPMapGadget::saveDefaultLocation(double lng, double lat, double zoom) { - if(m_config) - { + if (m_config) { m_config->setLatitude(lat); m_config->setLongitude(lng); m_config->setZoom(zoom); @@ -52,14 +51,13 @@ void OPMapGadget::saveDefaultLocation(double lng,double lat,double zoom) void OPMapGadget::saveOpacity(qreal value) { - if(m_config) - { + if (m_config) { m_config->setOpacity(value); } } void OPMapGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - m_config = qobject_cast(config); + m_config = qobject_cast(config); m_widget->setMapProvider(m_config->mapProvider()); m_widget->setUseOpenGL(m_config->useOpenGL()); m_widget->setShowTileGridLines(m_config->showTileGridLines()); @@ -72,4 +70,3 @@ void OPMapGadget::loadConfiguration(IUAVGadgetConfiguration *config) m_widget->setHomePosition(QPointF(m_config->longitude(), m_config->latitude())); m_widget->setOverlayOpacity(m_config->opacity()); } - diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadget.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadget.h index 250994e9b..bbc4f4ae9 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -39,15 +39,17 @@ class OPMapGadgetWidget; using namespace Core; -class OPMapGadget : public Core::IUAVGadget -{ +class OPMapGadget : public Core::IUAVGadget { Q_OBJECT public: OPMapGadget(QString classId, OPMapGadgetWidget *widget, QWidget *parent = 0); ~OPMapGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* m_config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *m_config); private: OPMapGadgetWidget *m_widget; OPMapGadgetConfiguration *m_config; diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp index 325cb1721..922d41e16 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -29,7 +29,7 @@ #include "utils/pathutils.h" #include -OPMapGadgetConfiguration::OPMapGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +OPMapGadgetConfiguration::OPMapGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_mapProvider("GoogleHybrid"), m_defaultZoom(2), @@ -40,98 +40,105 @@ OPMapGadgetConfiguration::OPMapGadgetConfiguration(QString classId, QSettings* m_accessMode("ServerAndCache"), m_useMemoryCache(true), m_cacheLocation(Utils::PathUtils().GetStoragePath() + "mapscache" + QDir::separator()), - m_uavSymbol(QString::fromUtf8(":/uavs/images/mapquad.png")), - m_maxUpdateRate(2000), // ms + m_uavSymbol(QString::fromUtf8(":/uavs/images/mapquad.png")), + m_maxUpdateRate(2000), // ms m_settings(qSettings), m_opacity(1) { - - //if a saved configuration exists load it - if (qSettings != 0) { - - QString mapProvider = qSettings->value("mapProvider").toString(); + // if a saved configuration exists load it + if (qSettings != 0) { + QString mapProvider = qSettings->value("mapProvider").toString(); int zoom = qSettings->value("defaultZoom").toInt(); - double latitude= qSettings->value("defaultLatitude").toDouble(); - double longitude= qSettings->value("defaultLongitude").toDouble(); - bool useOpenGL= qSettings->value("useOpenGL").toBool(); - bool showTileGridLines= qSettings->value("showTileGridLines").toBool(); - QString accessMode= qSettings->value("accessMode").toString(); - bool useMemoryCache= qSettings->value("useMemoryCache").toBool(); - QString cacheLocation= qSettings->value("cacheLocation").toString(); - QString uavSymbol=qSettings->value("uavSymbol").toString(); - int max_update_rate = qSettings->value("maxUpdateRate").toInt(); + double latitude = qSettings->value("defaultLatitude").toDouble(); + double longitude = qSettings->value("defaultLongitude").toDouble(); + bool useOpenGL = qSettings->value("useOpenGL").toBool(); + bool showTileGridLines = qSettings->value("showTileGridLines").toBool(); + QString accessMode = qSettings->value("accessMode").toString(); + bool useMemoryCache = qSettings->value("useMemoryCache").toBool(); + QString cacheLocation = qSettings->value("cacheLocation").toString(); + QString uavSymbol = qSettings->value("uavSymbol").toString(); + int max_update_rate = qSettings->value("maxUpdateRate").toInt(); - m_opacity=qSettings->value("overlayOpacity",1).toReal(); + m_opacity = qSettings->value("overlayOpacity", 1).toReal(); - if (!mapProvider.isEmpty()) m_mapProvider = mapProvider; - m_defaultZoom = zoom; - m_defaultLatitude = latitude; - m_defaultLongitude = longitude; + if (!mapProvider.isEmpty()) { + m_mapProvider = mapProvider; + } + m_defaultZoom = zoom; + m_defaultLatitude = latitude; + m_defaultLongitude = longitude; m_useOpenGL = useOpenGL; m_showTileGridLines = showTileGridLines; - m_uavSymbol = uavSymbol; + m_uavSymbol = uavSymbol; - m_maxUpdateRate = max_update_rate; - if (m_maxUpdateRate < 100 || m_maxUpdateRate > 5000) - m_maxUpdateRate = 2000; + m_maxUpdateRate = max_update_rate; + if (m_maxUpdateRate < 100 || m_maxUpdateRate > 5000) { + m_maxUpdateRate = 2000; + } - if (!accessMode.isEmpty()) - m_accessMode = accessMode; + if (!accessMode.isEmpty()) { + m_accessMode = accessMode; + } m_useMemoryCache = useMemoryCache; - if (!cacheLocation.isEmpty()) - m_cacheLocation = Utils::PathUtils().InsertStoragePath(cacheLocation); + if (!cacheLocation.isEmpty()) { + m_cacheLocation = Utils::PathUtils().InsertStoragePath(cacheLocation); + } } } -IUAVGadgetConfiguration * OPMapGadgetConfiguration::clone() +IUAVGadgetConfiguration *OPMapGadgetConfiguration::clone() { OPMapGadgetConfiguration *m = new OPMapGadgetConfiguration(this->classId()); - m->m_mapProvider = m_mapProvider; - m->m_defaultZoom = m_defaultZoom; - m->m_defaultLatitude = m_defaultLatitude; - m->m_defaultLongitude = m_defaultLongitude; + m->m_mapProvider = m_mapProvider; + m->m_defaultZoom = m_defaultZoom; + m->m_defaultLatitude = m_defaultLatitude; + m->m_defaultLongitude = m_defaultLongitude; m->m_useOpenGL = m_useOpenGL; m->m_showTileGridLines = m_showTileGridLines; m->m_accessMode = m_accessMode; - m->m_useMemoryCache = m_useMemoryCache; - m->m_cacheLocation = m_cacheLocation; - m->m_uavSymbol = m_uavSymbol; - m->m_maxUpdateRate = m_maxUpdateRate; - m->m_opacity=m_opacity; + m->m_useMemoryCache = m_useMemoryCache; + m->m_cacheLocation = m_cacheLocation; + m->m_uavSymbol = m_uavSymbol; + m->m_maxUpdateRate = m_maxUpdateRate; + m->m_opacity = m_opacity; return m; } -void OPMapGadgetConfiguration::saveConfig() const { - if(!m_settings) +void OPMapGadgetConfiguration::saveConfig() const +{ + if (!m_settings) { return; - m_settings->setValue("mapProvider", m_mapProvider); - m_settings->setValue("defaultZoom", m_defaultZoom); - m_settings->setValue("defaultLatitude", m_defaultLatitude); - m_settings->setValue("defaultLongitude", m_defaultLongitude); - m_settings->setValue("useOpenGL", m_useOpenGL); - m_settings->setValue("showTileGridLines", m_showTileGridLines); - m_settings->setValue("accessMode", m_accessMode); - m_settings->setValue("useMemoryCache", m_useMemoryCache); - m_settings->setValue("uavSymbol", m_uavSymbol); - m_settings->setValue("cacheLocation", Utils::PathUtils().RemoveStoragePath(m_cacheLocation)); - m_settings->setValue("maxUpdateRate", m_maxUpdateRate); - m_settings->setValue("overlayOpacity",m_opacity); + } + m_settings->setValue("mapProvider", m_mapProvider); + m_settings->setValue("defaultZoom", m_defaultZoom); + m_settings->setValue("defaultLatitude", m_defaultLatitude); + m_settings->setValue("defaultLongitude", m_defaultLongitude); + m_settings->setValue("useOpenGL", m_useOpenGL); + m_settings->setValue("showTileGridLines", m_showTileGridLines); + m_settings->setValue("accessMode", m_accessMode); + m_settings->setValue("useMemoryCache", m_useMemoryCache); + m_settings->setValue("uavSymbol", m_uavSymbol); + m_settings->setValue("cacheLocation", Utils::PathUtils().RemoveStoragePath(m_cacheLocation)); + m_settings->setValue("maxUpdateRate", m_maxUpdateRate); + m_settings->setValue("overlayOpacity", m_opacity); } -void OPMapGadgetConfiguration::saveConfig(QSettings* qSettings) const { - qSettings->setValue("mapProvider", m_mapProvider); - qSettings->setValue("defaultZoom", m_defaultZoom); - qSettings->setValue("defaultLatitude", m_defaultLatitude); - qSettings->setValue("defaultLongitude", m_defaultLongitude); - qSettings->setValue("useOpenGL", m_useOpenGL); - qSettings->setValue("showTileGridLines", m_showTileGridLines); - qSettings->setValue("accessMode", m_accessMode); - qSettings->setValue("useMemoryCache", m_useMemoryCache); - qSettings->setValue("uavSymbol", m_uavSymbol); - qSettings->setValue("cacheLocation", Utils::PathUtils().RemoveStoragePath(m_cacheLocation)); - qSettings->setValue("maxUpdateRate", m_maxUpdateRate); - qSettings->setValue("overlayOpacity",m_opacity); +void OPMapGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ + qSettings->setValue("mapProvider", m_mapProvider); + qSettings->setValue("defaultZoom", m_defaultZoom); + qSettings->setValue("defaultLatitude", m_defaultLatitude); + qSettings->setValue("defaultLongitude", m_defaultLongitude); + qSettings->setValue("useOpenGL", m_useOpenGL); + qSettings->setValue("showTileGridLines", m_showTileGridLines); + qSettings->setValue("accessMode", m_accessMode); + qSettings->setValue("useMemoryCache", m_useMemoryCache); + qSettings->setValue("uavSymbol", m_uavSymbol); + qSettings->setValue("cacheLocation", Utils::PathUtils().RemoveStoragePath(m_cacheLocation)); + qSettings->setValue("maxUpdateRate", m_maxUpdateRate); + qSettings->setValue("overlayOpacity", m_opacity); } -void OPMapGadgetConfiguration::setCacheLocation(QString cacheLocation){ +void OPMapGadgetConfiguration::setCacheLocation(QString cacheLocation) +{ m_cacheLocation = cacheLocation; } diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.h index 22b3e749e..19462b894 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,55 +33,121 @@ using namespace Core; -class OPMapGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT - -Q_PROPERTY(QString mapProvider READ mapProvider WRITE setMapProvider) -Q_PROPERTY(int zoommo READ zoom WRITE setZoom) -Q_PROPERTY(double latitude READ latitude WRITE setLatitude) -Q_PROPERTY(double longitude READ longitude WRITE setLongitude) -Q_PROPERTY(bool useOpenGL READ useOpenGL WRITE setUseOpenGL) -Q_PROPERTY(bool showTileGridLines READ showTileGridLines WRITE setShowTileGridLines) -Q_PROPERTY(QString accessMode READ accessMode WRITE setAccessMode) -Q_PROPERTY(bool useMemoryCache READ useMemoryCache WRITE setUseMemoryCache) -Q_PROPERTY(QString cacheLocation READ cacheLocation WRITE setCacheLocation) -Q_PROPERTY(QString uavSymbol READ uavSymbol WRITE setUavSymbol) -Q_PROPERTY(int maxUpdateRate READ maxUpdateRate WRITE setMaxUpdateRate) -Q_PROPERTY(qreal overlayOpacity READ opacity WRITE setOpacity) +class OPMapGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT Q_PROPERTY(QString mapProvider READ mapProvider WRITE setMapProvider) + Q_PROPERTY(int zoommo READ zoom WRITE setZoom) + Q_PROPERTY(double latitude READ latitude WRITE setLatitude) + Q_PROPERTY(double longitude READ longitude WRITE setLongitude) + Q_PROPERTY(bool useOpenGL READ useOpenGL WRITE setUseOpenGL) + Q_PROPERTY(bool showTileGridLines READ showTileGridLines WRITE setShowTileGridLines) + Q_PROPERTY(QString accessMode READ accessMode WRITE setAccessMode) + Q_PROPERTY(bool useMemoryCache READ useMemoryCache WRITE setUseMemoryCache) + Q_PROPERTY(QString cacheLocation READ cacheLocation WRITE setCacheLocation) + Q_PROPERTY(QString uavSymbol READ uavSymbol WRITE setUavSymbol) + Q_PROPERTY(int maxUpdateRate READ maxUpdateRate WRITE setMaxUpdateRate) + Q_PROPERTY(qreal overlayOpacity READ opacity WRITE setOpacity) public: - explicit OPMapGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit OPMapGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); - QString mapProvider() const { return m_mapProvider; } - int zoom() const { return m_defaultZoom; } - double latitude() const { return m_defaultLatitude; } - double longitude() const { return m_defaultLongitude; } - bool useOpenGL() const { return m_useOpenGL; } - bool showTileGridLines() const { return m_showTileGridLines; } - QString accessMode() const { return m_accessMode; } - bool useMemoryCache() const { return m_useMemoryCache; } - QString cacheLocation() const { return m_cacheLocation; } - QString uavSymbol() const { return m_uavSymbol; } - int maxUpdateRate() const { return m_maxUpdateRate; } - qreal opacity() const { return m_opacity; } + QString mapProvider() const + { + return m_mapProvider; + } + int zoom() const + { + return m_defaultZoom; + } + double latitude() const + { + return m_defaultLatitude; + } + double longitude() const + { + return m_defaultLongitude; + } + bool useOpenGL() const + { + return m_useOpenGL; + } + bool showTileGridLines() const + { + return m_showTileGridLines; + } + QString accessMode() const + { + return m_accessMode; + } + bool useMemoryCache() const + { + return m_useMemoryCache; + } + QString cacheLocation() const + { + return m_cacheLocation; + } + QString uavSymbol() const + { + return m_uavSymbol; + } + int maxUpdateRate() const + { + return m_maxUpdateRate; + } + qreal opacity() const + { + return m_opacity; + } void saveConfig() const; public slots: - void setMapProvider(QString provider) { m_mapProvider = provider; } - void setZoom(int zoom) { m_defaultZoom = zoom; } - void setLatitude(double latitude) { m_defaultLatitude = latitude; } - void setOpacity(qreal value) { m_opacity = value; } - void setLongitude(double longitude) { m_defaultLongitude = longitude; } - void setUseOpenGL(bool useOpenGL) { m_useOpenGL = useOpenGL; } - void setShowTileGridLines(bool showTileGridLines) { m_showTileGridLines = showTileGridLines; } - void setAccessMode(QString accessMode) { m_accessMode = accessMode; } - void setUseMemoryCache(bool useMemoryCache) { m_useMemoryCache = useMemoryCache; } + void setMapProvider(QString provider) + { + m_mapProvider = provider; + } + void setZoom(int zoom) + { + m_defaultZoom = zoom; + } + void setLatitude(double latitude) + { + m_defaultLatitude = latitude; + } + void setOpacity(qreal value) + { + m_opacity = value; + } + void setLongitude(double longitude) + { + m_defaultLongitude = longitude; + } + void setUseOpenGL(bool useOpenGL) + { + m_useOpenGL = useOpenGL; + } + void setShowTileGridLines(bool showTileGridLines) + { + m_showTileGridLines = showTileGridLines; + } + void setAccessMode(QString accessMode) + { + m_accessMode = accessMode; + } + void setUseMemoryCache(bool useMemoryCache) + { + m_useMemoryCache = useMemoryCache; + } void setCacheLocation(QString cacheLocation); - void setUavSymbol(QString symbol){m_uavSymbol=symbol;} - void setMaxUpdateRate(int update_rate){m_maxUpdateRate = update_rate;} + void setUavSymbol(QString symbol) + { + m_uavSymbol = symbol; + } + void setMaxUpdateRate(int update_rate) + { + m_maxUpdateRate = update_rate; + } private: QString m_mapProvider; @@ -94,8 +160,8 @@ private: bool m_useMemoryCache; QString m_cacheLocation; QString m_uavSymbol; - int m_maxUpdateRate; - QSettings * m_settings; + int m_maxUpdateRate; + QSettings *m_settings; qreal m_opacity; }; diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.cpp index fb4b73efa..111cef2cd 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,27 +32,25 @@ #include OPMapGadgetFactory::OPMapGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("OPMapGadget"), tr("OPMap"), parent) -{ -} + IUAVGadgetFactory(QString("OPMapGadget"), tr("OPMap"), parent) +{} OPMapGadgetFactory::~OPMapGadgetFactory() -{ -} +{} -Core::IUAVGadget * OPMapGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *OPMapGadgetFactory::createGadget(QWidget *parent) { OPMapGadgetWidget *gadgetWidget = new OPMapGadgetWidget(parent); + return new OPMapGadget(QString("OPMapGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *OPMapGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *OPMapGadgetFactory::createConfiguration(QSettings *qSettings) { return new OPMapGadgetConfiguration(QString("OPMapGadget"), qSettings); } -IOptionsPage * OPMapGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) +IOptionsPage *OPMapGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new OPMapGadgetOptionsPage(qobject_cast(config)); + return new OPMapGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.h index 138efcb90..908bd1f4d 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -31,14 +31,13 @@ #include namespace Core { - class IUAVGadget; - class IUAVGadgetFactory; +class IUAVGadget; +class IUAVGadgetFactory; } using namespace Core; -class OPMapGadgetFactory : public Core::IUAVGadgetFactory -{ +class OPMapGadgetFactory : public Core::IUAVGadgetFactory { Q_OBJECT public: @@ -46,8 +45,8 @@ public: ~OPMapGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; -#endif +#endif // ifndef OPAMP_GADGETFACTORY_H_ diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.cpp index acf343a46..91a71e6ea 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -44,14 +44,13 @@ OPMapGadgetOptionsPage::OPMapGadgetOptionsPage(OPMapGadgetConfiguration *config, QObject *parent) : IOptionsPage(parent), m_config(config) -{ -} +{} QWidget *OPMapGadgetOptionsPage::createPage(QWidget *parent) { - int index; + int index; - m_page = new Ui::OPMapGadgetOptionsPage(); + m_page = new Ui::OPMapGadgetOptionsPage(); QWidget *w = new QWidget(parent); m_page->setupUi(w); @@ -63,22 +62,22 @@ QWidget *OPMapGadgetOptionsPage::createPage(QWidget *parent) m_page->accessModeComboBox->clear(); m_page->accessModeComboBox->addItems(mapcontrol::Helper::AccessModeTypes()); - index = m_page->providerComboBox->findText(m_config->mapProvider()); - index = (index >= 0) ? index : 0; - m_page->providerComboBox->setCurrentIndex(index); + index = m_page->providerComboBox->findText(m_config->mapProvider()); + index = (index >= 0) ? index : 0; + m_page->providerComboBox->setCurrentIndex(index); - // populate the map max update rate combobox - m_page->maxUpdateRateComboBox->clear(); - m_page->maxUpdateRateComboBox->addItem("100ms", 100); - m_page->maxUpdateRateComboBox->addItem("200ms", 200); - m_page->maxUpdateRateComboBox->addItem("500ms", 500); - m_page->maxUpdateRateComboBox->addItem("1 sec", 1000); - m_page->maxUpdateRateComboBox->addItem("2 sec", 2000); - m_page->maxUpdateRateComboBox->addItem("5 sec", 5000); + // populate the map max update rate combobox + m_page->maxUpdateRateComboBox->clear(); + m_page->maxUpdateRateComboBox->addItem("100ms", 100); + m_page->maxUpdateRateComboBox->addItem("200ms", 200); + m_page->maxUpdateRateComboBox->addItem("500ms", 500); + m_page->maxUpdateRateComboBox->addItem("1 sec", 1000); + m_page->maxUpdateRateComboBox->addItem("2 sec", 2000); + m_page->maxUpdateRateComboBox->addItem("5 sec", 5000); - index = m_page->maxUpdateRateComboBox->findData(m_config->maxUpdateRate()); - index = (index >= 0) ? index : 4; - m_page->maxUpdateRateComboBox->setCurrentIndex(index); + index = m_page->maxUpdateRateComboBox->findData(m_config->maxUpdateRate()); + index = (index >= 0) ? index : 4; + m_page->maxUpdateRateComboBox->setCurrentIndex(index); m_page->zoomSpinBox->setValue(m_config->zoom()); m_page->latitudeSpinBox->setValue(m_config->latitude()); @@ -98,16 +97,14 @@ QWidget *OPMapGadgetOptionsPage::createPage(QWidget *parent) m_page->lineEditCacheLocation->setPath(m_config->cacheLocation()); QDir dir(":/uavs/images/"); - QStringList list=dir.entryList(); - foreach(QString i,list) - { - QIcon icon(QPixmap(":/uavs/images/"+i)); - m_page->uavSymbolComboBox->addItem(icon,QString(),i); + QStringList list = dir.entryList(); + foreach(QString i, list) { + QIcon icon(QPixmap(":/uavs/images/" + i)); + + m_page->uavSymbolComboBox->addItem(icon, QString(), i); } - for(int x=0;xuavSymbolComboBox->count();++x) - { - if(m_page->uavSymbolComboBox->itemData(x).toString()==m_config->uavSymbol()) - { + for (int x = 0; x < m_page->uavSymbolComboBox->count(); ++x) { + if (m_page->uavSymbolComboBox->itemData(x).toString() == m_config->uavSymbol()) { m_page->uavSymbolComboBox->setCurrentIndex(x); } } @@ -120,12 +117,12 @@ QWidget *OPMapGadgetOptionsPage::createPage(QWidget *parent) void OPMapGadgetOptionsPage::on_pushButtonCacheDefaults_clicked() { int index = m_page->accessModeComboBox->findText("ServerAndCache"); + index = (index >= 0) ? index : 0; m_page->accessModeComboBox->setCurrentIndex(index); m_page->checkBoxUseMemoryCache->setChecked(true); m_page->lineEditCacheLocation->setPath(Utils::PathUtils().GetStoragePath() + "mapscache" + QDir::separator()); - } void OPMapGadgetOptionsPage::apply() @@ -140,7 +137,7 @@ void OPMapGadgetOptionsPage::apply() m_config->setUseMemoryCache(m_page->checkBoxUseMemoryCache->isChecked()); m_config->setCacheLocation(m_page->lineEditCacheLocation->path()); m_config->setUavSymbol(m_page->uavSymbolComboBox->itemData(m_page->uavSymbolComboBox->currentIndex()).toString()); - m_config->setMaxUpdateRate(m_page->maxUpdateRateComboBox->itemData(m_page->maxUpdateRateComboBox->currentIndex()).toInt()); + m_config->setMaxUpdateRate(m_page->maxUpdateRateComboBox->itemData(m_page->maxUpdateRateComboBox->currentIndex()).toInt()); } void OPMapGadgetOptionsPage::finish() diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.h index 1f421cd03..2ad5c551b 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetoptionspage.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -33,18 +33,17 @@ class OPMapGadgetConfiguration; namespace Core { - class IUAVGadgetConfiguration; +class IUAVGadgetConfiguration; } namespace Ui { - class OPMapGadgetOptionsPage; +class OPMapGadgetOptionsPage; } using namespace Core; -class OPMapGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class OPMapGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit OPMapGadgetOptionsPage(OPMapGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 223343ec4..c7d193f24 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -61,26 +61,26 @@ // ************************************************************************************* -#define deg_to_rad ((double)M_PI / 180.0) -#define rad_to_deg (180.0 / (double)M_PI) +#define deg_to_rad ((double)M_PI / 180.0) +#define rad_to_deg (180.0 / (double)M_PI) -#define earth_mean_radius 6371 // kilometers +#define earth_mean_radius 6371 // kilometers -#define max_digital_zoom 3 // maximum allowed digital zoom level +#define max_digital_zoom 3 // maximum allowed digital zoom level -const int safe_area_radius_list[] = {5, 10, 20, 50, 100, 200, 500, 1000, 2000, 5000}; // meters +const int safe_area_radius_list[] = { 5, 10, 20, 50, 100, 200, 500, 1000, 2000, 5000 }; // meters -const int uav_trail_time_list[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; // seconds +const int uav_trail_time_list[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 }; // seconds -const int uav_trail_distance_list[] = {1, 2, 5, 10, 20, 50, 100, 200, 500}; // meters +const int uav_trail_distance_list[] = { 1, 2, 5, 10, 20, 50, 100, 200, 500 }; // meters -const int max_update_rate_list[] = {100, 200, 500, 1000, 2000, 5000}; // milliseconds +const int max_update_rate_list[] = { 100, 200, 500, 1000, 2000, 5000 }; // milliseconds // ************************************************************************************* // ************************************************************************************* -// NOTE: go back to SVN REV 2137 and earlier to get back to experimental waypoint support. +// NOTE: go back to SVN REV 2137 and earlier to get back to experimental waypoint support. // ************************************************************************************* @@ -90,54 +90,53 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) // ************** m_widget = NULL; - m_map = NULL; + m_map = NULL; findPlaceCompleter = NULL; - m_mouse_waypoint = NULL; + m_mouse_waypoint = NULL; - pm = NULL; - obm = NULL; - obum = NULL; + pm = NULL; + obm = NULL; + obum = NULL; - m_prev_tile_number = 0; + m_prev_tile_number = 0; - m_min_zoom = m_max_zoom = 0; + m_min_zoom = m_max_zoom = 0; m_map_mode = Normal_MapMode; - m_maxUpdateRate = max_update_rate_list[4]; // 2 seconds //SHOULDN'T THIS BE LOADED FROM THE USER PREFERENCES? + m_maxUpdateRate = max_update_rate_list[4]; // 2 seconds //SHOULDN'T THIS BE LOADED FROM THE USER PREFERENCES? - m_telemetry_connected = false; + m_telemetry_connected = false; - m_context_menu_lat_lon = m_mouse_lat_lon = internals::PointLatLng(0, 0); + m_context_menu_lat_lon = m_mouse_lat_lon = internals::PointLatLng(0, 0); setMouseTracking(true); - pm = ExtensionSystem::PluginManager::instance(); - if (pm) - { - obm = pm->getObject(); - obum = pm->getObject(); - } + pm = ExtensionSystem::PluginManager::instance(); + if (pm) { + obm = pm->getObject(); + obum = pm->getObject(); + } - // ************** + // ************** // get current location - double latitude = 0; + double latitude = 0; double longitude = 0; - double altitude = 0; + double altitude = 0; - // current position - getUAVPosition(latitude, longitude, altitude); + // current position + getUAVPosition(latitude, longitude, altitude); internals::PointLatLng pos_lat_lon = internals::PointLatLng(latitude, longitude); // ************** // default home position - m_home_position.coord = pos_lat_lon; - m_home_position.altitude = altitude; - m_home_position.locked = false; + m_home_position.coord = pos_lat_lon; + m_home_position.altitude = altitude; + m_home_position.locked = false; // ************** // create the widget that holds the user controls and the map @@ -148,39 +147,39 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) // ************** // create the central map widget - m_map = new mapcontrol::OPMapWidget(); // create the map object + m_map = new mapcontrol::OPMapWidget(); // create the map object - m_map->setFrameStyle(QFrame::NoFrame); // no border frame + m_map->setFrameStyle(QFrame::NoFrame); // no border frame m_map->setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); // tile background - m_map->configuration->DragButton = Qt::LeftButton; // use the left mouse button for map dragging + m_map->configuration->DragButton = Qt::LeftButton; // use the left mouse button for map dragging - m_widget->horizontalSliderZoom->setMinimum(m_map->MinZoom()); // - m_widget->horizontalSliderZoom->setMaximum(m_map->MaxZoom() + max_digital_zoom); // + m_widget->horizontalSliderZoom->setMinimum(m_map->MinZoom()); // + m_widget->horizontalSliderZoom->setMaximum(m_map->MaxZoom() + max_digital_zoom); // - m_min_zoom = m_widget->horizontalSliderZoom->minimum(); // minimum zoom we can accept - m_max_zoom = m_widget->horizontalSliderZoom->maximum(); // maximum zoom we can accept + m_min_zoom = m_widget->horizontalSliderZoom->minimum(); // minimum zoom we can accept + m_max_zoom = m_widget->horizontalSliderZoom->maximum(); // maximum zoom we can accept - m_map->SetMouseWheelZoomType(internals::MouseWheelZoomType::MousePositionWithoutCenter); // set how the mouse wheel zoom functions - m_map->SetFollowMouse(true); // we want a contiuous mouse position reading + m_map->SetMouseWheelZoomType(internals::MouseWheelZoomType::MousePositionWithoutCenter); // set how the mouse wheel zoom functions + m_map->SetFollowMouse(true); // we want a contiuous mouse position reading - m_map->SetShowHome(true); // display the HOME position on the map - m_map->SetShowUAV(true); // display the UAV position on the map + m_map->SetShowHome(true); // display the HOME position on the map + m_map->SetShowUAV(true); // display the UAV position on the map - m_map->Home->SetSafeArea(safe_area_radius_list[0]); // set radius (meters) //SHOULDN'T THE DEFAULT BE USER DEFINED? - m_map->Home->SetShowSafeArea(true); // show the safe area //SHOULDN'T THE DEFAULT BE USER DEFINED? + m_map->Home->SetSafeArea(safe_area_radius_list[0]); // set radius (meters) //SHOULDN'T THE DEFAULT BE USER DEFINED? + m_map->Home->SetShowSafeArea(true); // show the safe area //SHOULDN'T THE DEFAULT BE USER DEFINED? m_map->Home->SetToggleRefresh(true); - if(m_map->Home) - connect(m_map->Home,SIGNAL(homedoubleclick(HomeItem*)),this,SLOT(onHomeDoubleClick(HomeItem*))); - m_map->UAV->SetTrailTime(uav_trail_time_list[0]); // seconds - m_map->UAV->SetTrailDistance(uav_trail_distance_list[1]); // meters + if (m_map->Home) { + connect(m_map->Home, SIGNAL(homedoubleclick(HomeItem *)), this, SLOT(onHomeDoubleClick(HomeItem *))); + } + m_map->UAV->SetTrailTime(uav_trail_time_list[0]); // seconds + m_map->UAV->SetTrailDistance(uav_trail_distance_list[1]); // meters m_map->UAV->SetTrailType(UAVTrailType::ByTimeElapsed); - if(m_map->GPS) - { - m_map->GPS->SetTrailTime(uav_trail_time_list[0]); // seconds - m_map->GPS->SetTrailDistance(uav_trail_distance_list[1]); // meters + if (m_map->GPS) { + m_map->GPS->SetTrailTime(uav_trail_time_list[0]); // seconds + m_map->GPS->SetTrailDistance(uav_trail_distance_list[1]); // meters m_map->GPS->SetTrailType(UAVTrailType::ByTimeElapsed); } @@ -194,9 +193,9 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) layout->addWidget(m_map); m_widget->mapWidget->setLayout(layout); - m_widget->toolButtonMagicWaypointMapMode->setChecked(false); - m_widget->toolButtonNormalMapMode->setChecked(true); - hideMagicWaypointControls(); + m_widget->toolButtonMagicWaypointMapMode->setChecked(false); + m_widget->toolButtonNormalMapMode->setChecked(true); + hideMagicWaypointControls(); m_widget->labelUAVPos->setText("---"); m_widget->labelMapPos->setText("---"); @@ -205,30 +204,31 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) m_widget->progressBarMap->setMaximum(1); - connect(m_map, SIGNAL(zoomChanged(double, double, double)), this, SLOT(zoomChanged(double, double, double))); // map zoom change signals - connect(m_map, SIGNAL(OnCurrentPositionChanged(internals::PointLatLng)), this, SLOT(OnCurrentPositionChanged(internals::PointLatLng))); // map poisition change signals - connect(m_map, SIGNAL(OnTileLoadComplete()), this, SLOT(OnTileLoadComplete())); // tile loading stop signals - connect(m_map, SIGNAL(OnTileLoadStart()), this, SLOT(OnTileLoadStart())); // tile loading start signals - connect(m_map, SIGNAL(OnTilesStillToLoad(int)), this, SLOT(OnTilesStillToLoad(int))); // tile loading signals - connect(m_map,SIGNAL(OnWayPointDoubleClicked(WayPointItem*)),this,SLOT(wpDoubleClickEvent(WayPointItem*))); - m_map->SetCurrentPosition(m_home_position.coord); // set the map position - m_map->Home->SetCoord(m_home_position.coord); // set the HOME position - m_map->UAV->SetUAVPos(m_home_position.coord, 0.0); // set the UAV position + connect(m_map, SIGNAL(zoomChanged(double, double, double)), this, SLOT(zoomChanged(double, double, double))); // map zoom change signals + connect(m_map, SIGNAL(OnCurrentPositionChanged(internals::PointLatLng)), this, SLOT(OnCurrentPositionChanged(internals::PointLatLng))); // map poisition change signals + connect(m_map, SIGNAL(OnTileLoadComplete()), this, SLOT(OnTileLoadComplete())); // tile loading stop signals + connect(m_map, SIGNAL(OnTileLoadStart()), this, SLOT(OnTileLoadStart())); // tile loading start signals + connect(m_map, SIGNAL(OnTilesStillToLoad(int)), this, SLOT(OnTilesStillToLoad(int))); // tile loading signals + connect(m_map, SIGNAL(OnWayPointDoubleClicked(WayPointItem *)), this, SLOT(wpDoubleClickEvent(WayPointItem *))); + m_map->SetCurrentPosition(m_home_position.coord); // set the map position + m_map->Home->SetCoord(m_home_position.coord); // set the HOME position + m_map->UAV->SetUAVPos(m_home_position.coord, 0.0); // set the UAV position m_map->UAV->update(); - if(m_map->GPS) - m_map->GPS->SetUAVPos(m_home_position.coord, 0.0); // set the GPS position + if (m_map->GPS) { + m_map->GPS->SetUAVPos(m_home_position.coord, 0.0); // set the GPS position + } #ifdef USE_PATHPLANNER - model=new flightDataModel(this); - table=new pathPlanner(); - selectionModel=new QItemSelectionModel(model); - mapProxy=new modelMapProxy(this,m_map,model,selectionModel); - table->setModel(model,selectionModel); - waypoint_edit_dialog=new opmap_edit_waypoint_dialog(this,model,selectionModel); - UAVProxy=new modelUavoProxy(this,model); - connect(table,SIGNAL(sendPathPlanToUAV()),UAVProxy,SLOT(modelToObjects())); - connect(table,SIGNAL(receivePathPlanFromUAV()),UAVProxy,SLOT(objectsToModel())); + model = new flightDataModel(this); + table = new pathPlanner(); + selectionModel = new QItemSelectionModel(model); + mapProxy = new modelMapProxy(this, m_map, model, selectionModel); + table->setModel(model, selectionModel); + waypoint_edit_dialog = new opmap_edit_waypoint_dialog(this, model, selectionModel); + UAVProxy = new modelUavoProxy(this, model); + connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(modelToObjects())); + connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(objectsToModel())); #endif - magicWayPoint=m_map->magicWPCreate(); + magicWayPoint = m_map->magicWPCreate(); magicWayPoint->setVisible(false); m_map->setOverlayOpacity(0.5); @@ -241,22 +241,18 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) // ************** // connect to the UAVObject updates we require to become a bit aware of our environment: - if (pm) - { + if (pm) { // Register for Home Location state changes - if (obm) - { - UAVDataObject *obj = dynamic_cast(obm->getObject(QString("HomeLocation"))); - if (obj) - { - connect(obj, SIGNAL(objectUpdated(UAVObject *)), this , SLOT(homePositionUpdated(UAVObject *))); + if (obm) { + UAVDataObject *obj = dynamic_cast(obm->getObject(QString("HomeLocation"))); + if (obj) { + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(homePositionUpdated(UAVObject *))); } } // Listen to telemetry connection events TelemetryManager *telMngr = pm->getObject(); - if (telMngr) - { + if (telMngr) { connect(telMngr, SIGNAL(connected()), this, SLOT(onTelemetryConnect())); connect(telMngr, SIGNAL(disconnected()), this, SLOT(onTelemetryDisconnect())); } @@ -266,13 +262,13 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) // create the desired timers m_updateTimer = new QTimer(); - m_updateTimer->setInterval(m_maxUpdateRate); + m_updateTimer->setInterval(m_maxUpdateRate); connect(m_updateTimer, SIGNAL(timeout()), this, SLOT(updatePosition())); m_updateTimer->start(); m_statusUpdateTimer = new QTimer(); - m_statusUpdateTimer->setInterval(200); - connect(m_statusUpdateTimer, SIGNAL(timeout()), this, SLOT(updateMousePos())); + m_statusUpdateTimer->setInterval(200); + connect(m_statusUpdateTimer, SIGNAL(timeout()), this, SLOT(updateMousePos())); m_statusUpdateTimer->start(); // ************** @@ -282,30 +278,34 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) // destructor OPMapGadgetWidget::~OPMapGadgetWidget() { - if (m_map) - { - disconnect(m_map, 0, 0, 0); - m_map->SetShowHome(false); // doing this appears to stop the map lib crashing on exit - m_map->SetShowUAV(false); // " " - } + if (m_map) { + disconnect(m_map, 0, 0, 0); + m_map->SetShowHome(false); // doing this appears to stop the map lib crashing on exit + m_map->SetShowUAV(false); // " " + } - if (m_map) - { - delete m_map; - m_map = NULL; - } - if(!model.isNull()) + if (m_map) { + delete m_map; + m_map = NULL; + } + if (!model.isNull()) { delete model; - if(!table.isNull()) + } + if (!table.isNull()) { delete table; - if(!selectionModel.isNull()) + } + if (!selectionModel.isNull()) { delete selectionModel; - if(!mapProxy.isNull()) + } + if (!mapProxy.isNull()) { delete mapProxy; - if(!waypoint_edit_dialog.isNull()) + } + if (!waypoint_edit_dialog.isNull()) { delete waypoint_edit_dialog; - if(!UAVProxy.isNull()) + } + if (!UAVProxy.isNull()) { delete UAVProxy; + } } // ************************************************************************************* @@ -318,39 +318,35 @@ void OPMapGadgetWidget::resizeEvent(QResizeEvent *event) void OPMapGadgetWidget::mouseMoveEvent(QMouseEvent *event) { - if (m_widget && m_map) - { - } + if (m_widget && m_map) {} - if (event->buttons() & Qt::LeftButton) - { - } + if (event->buttons() & Qt::LeftButton) {} QWidget::mouseMoveEvent(event); } -void OPMapGadgetWidget::wpDoubleClickEvent(WayPointItem *wp) +void OPMapGadgetWidget::wpDoubleClickEvent(WayPointItem *wp) { - m_mouse_waypoint = wp; - onEditWayPointAct_triggered(); + m_mouse_waypoint = wp; + onEditWayPointAct_triggered(); } void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) -{ // the user has right clicked on the map - create the pop-up context menu and display it - +{ // the user has right clicked on the map - create the pop-up context menu and display it QString s; - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - if (event->reason() != QContextMenuEvent::Mouse) - return; // not a mouse click event - + if (event->reason() != QContextMenuEvent::Mouse) { + return; // not a mouse click event + } // current mouse position QPoint p = m_map->mapFromGlobal(event->globalPos()); m_context_menu_lat_lon = m_map->GetFromLocalToLatLng(p); - if (!m_map->contentsRect().contains(p)) - return; // the mouse click was not on the map - + if (!m_map->contentsRect().contains(p)) { + return; // the mouse click was not on the map + } // show the mouse position s = QString::number(m_context_menu_lat_lon.Lat(), 'f', 7) + " " + QString::number(m_context_menu_lat_lon.Lng(), 'f', 7); m_widget->labelMousePos->setText(s); @@ -361,8 +357,9 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) // find out if the waypoint is locked (or not) bool waypoint_locked = false; - if (m_mouse_waypoint) + if (m_mouse_waypoint) { waypoint_locked = (m_mouse_waypoint->flags() & QGraphicsItem::ItemIsMovable) == 0; + } // **************** // Dynamically create the popup menu @@ -375,28 +372,31 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) contextMenu.addSeparator(); QMenu maxUpdateRateSubMenu(tr("&Max Update Rate ") + "(" + QString::number(m_maxUpdateRate) + " ms)", this); - for (int i = 0; i < maxUpdateRateAct.count(); i++) + for (int i = 0; i < maxUpdateRateAct.count(); i++) { maxUpdateRateSubMenu.addAction(maxUpdateRateAct.at(i)); + } contextMenu.addMenu(&maxUpdateRateSubMenu); contextMenu.addSeparator(); - switch (m_map_mode) - { - case Normal_MapMode: s = tr(" (Normal)"); break; - case MagicWaypoint_MapMode: s = tr(" (Magic Waypoint)"); break; - default: s = tr(" (Unknown)"); break; + switch (m_map_mode) { + case Normal_MapMode: s = tr(" (Normal)"); break; + case MagicWaypoint_MapMode: s = tr(" (Magic Waypoint)"); break; + default: s = tr(" (Unknown)"); break; } - for (int i = 0; i < mapModeAct.count(); i++) - { // set the menu to checked (or not) + for (int i = 0; i < mapModeAct.count(); i++) { // set the menu to checked (or not) QAction *act = mapModeAct.at(i); - if (!act) continue; - if (act->data().toInt() == (int)m_map_mode) + if (!act) { + continue; + } + if (act->data().toInt() == (int)m_map_mode) { act->setChecked(true); + } } QMenu mapModeSubMenu(tr("Map mode") + s, this); - for (int i = 0; i < mapModeAct.count(); i++) + for (int i = 0; i < mapModeAct.count(); i++) { mapModeSubMenu.addAction(mapModeAct.at(i)); + } contextMenu.addMenu(&mapModeSubMenu); contextMenu.addSeparator(); @@ -412,10 +412,11 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) contextMenu.addSeparator(); QMenu safeArea("Safety Area definitions"); - // menu.addAction(showSafeAreaAct); + // menu.addAction(showSafeAreaAct); QMenu safeAreaSubMenu(tr("Safe Area Radius") + " (" + QString::number(m_map->Home->SafeArea()) + "m)", this); - for (int i = 0; i < safeAreaAct.count(); i++) + for (int i = 0; i < safeAreaAct.count(); i++) { safeAreaSubMenu.addAction(safeAreaAct.at(i)); + } safeArea.addMenu(&safeAreaSubMenu); safeArea.addAction(showSafeAreaAct); contextMenu.addMenu(&safeArea); @@ -434,8 +435,9 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) contextMenu.addAction(zoomOutAct); QMenu zoomSubMenu(tr("&Zoom ") + "(" + QString::number(m_map->ZoomTotal()) + ")", this); - for (int i = 0; i < zoomAct.count(); i++) + for (int i = 0; i < zoomAct.count(); i++) { zoomSubMenu.addAction(zoomAct.at(i)); + } contextMenu.addMenu(&zoomSubMenu); contextMenu.addSeparator(); @@ -454,18 +456,21 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) uav_menu.addSeparator()->setText(tr("UAV Trail")); contextMenu.addMenu(&uav_menu); QMenu uavTrailTypeSubMenu(tr("UAV trail type") + " (" + mapcontrol::Helper::StrFromUAVTrailType(m_map->UAV->GetTrailType()) + ")", this); - for (int i = 0; i < uavTrailTypeAct.count(); i++) + for (int i = 0; i < uavTrailTypeAct.count(); i++) { uavTrailTypeSubMenu.addAction(uavTrailTypeAct.at(i)); + } uav_menu.addMenu(&uavTrailTypeSubMenu); QMenu uavTrailTimeSubMenu(tr("UAV trail time") + " (" + QString::number(m_map->UAV->TrailTime()) + " sec)", this); - for (int i = 0; i < uavTrailTimeAct.count(); i++) + for (int i = 0; i < uavTrailTimeAct.count(); i++) { uavTrailTimeSubMenu.addAction(uavTrailTimeAct.at(i)); + } uav_menu.addMenu(&uavTrailTimeSubMenu); QMenu uavTrailDistanceSubMenu(tr("UAV trail distance") + " (" + QString::number(m_map->UAV->TrailDistance()) + " meters)", this); - for (int i = 0; i < uavTrailDistanceAct.count(); i++) + for (int i = 0; i < uavTrailDistanceAct.count(); i++) { uavTrailDistanceSubMenu.addAction(uavTrailDistanceAct.at(i)); + } uav_menu.addMenu(&uavTrailDistanceSubMenu); uav_menu.addAction(showTrailAct); @@ -485,49 +490,49 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) // ********* #ifdef USE_PATHPLANNER - switch (m_map_mode) - { - case Normal_MapMode: + switch (m_map_mode) { + case Normal_MapMode: // only show the waypoint stuff if not in 'magic waypoint' mode - contextMenu.addSeparator()->setText(tr("Waypoints")); + contextMenu.addSeparator()->setText(tr("Waypoints")); - contextMenu.addAction(wayPointEditorAct); - contextMenu.addAction(addWayPointActFromContextMenu); + contextMenu.addAction(wayPointEditorAct); + contextMenu.addAction(addWayPointActFromContextMenu); - if (m_mouse_waypoint) - { // we have a waypoint under the mouse - contextMenu.addAction(editWayPointAct); + if (m_mouse_waypoint) { // we have a waypoint under the mouse + contextMenu.addAction(editWayPointAct); - lockWayPointAct->setChecked(waypoint_locked); - contextMenu.addAction(lockWayPointAct); + lockWayPointAct->setChecked(waypoint_locked); + contextMenu.addAction(lockWayPointAct); - if (!waypoint_locked) - contextMenu.addAction(deleteWayPointAct); + if (!waypoint_locked) { + contextMenu.addAction(deleteWayPointAct); } + } - if (m_map->WPPresent()) - contextMenu.addAction(clearWayPointsAct); // we have waypoints + if (m_map->WPPresent()) { + contextMenu.addAction(clearWayPointsAct); // we have waypoints + } + break; - break; - - case MagicWaypoint_MapMode: - contextMenu.addSeparator()->setText(tr("Waypoints")); - contextMenu.addAction(homeMagicWaypointAct); - break; + case MagicWaypoint_MapMode: + contextMenu.addSeparator()->setText(tr("Waypoints")); + contextMenu.addAction(homeMagicWaypointAct); + break; } -#endif - // ********* +#endif // ifdef USE_PATHPLANNER + // ********* - QMenu overlaySubMenu(tr("&Overlay Opacity "),this); - for (int i = 0; i < overlayOpacityAct.count(); i++) + QMenu overlaySubMenu(tr("&Overlay Opacity "), this); + for (int i = 0; i < overlayOpacityAct.count(); i++) { overlaySubMenu.addAction(overlayOpacityAct.at(i)); + } contextMenu.addMenu(&overlaySubMenu); contextMenu.addSeparator(); contextMenu.addAction(closeAct2); - contextMenu.exec(event->globalPos()); // popup the menu + contextMenu.exec(event->globalPos()); // popup the menu // **************** } @@ -542,166 +547,164 @@ void OPMapGadgetWidget::closeEvent(QCloseEvent *event) // timer signals /** - Updates the UAV position on the map. It is called at a user-defined frequency, - as set inside the map widget. -*/ + Updates the UAV position on the map. It is called at a user-defined frequency, + as set inside the map widget. + */ void OPMapGadgetWidget::updatePosition() { double uav_latitude, uav_longitude, uav_altitude, uav_yaw; double gps_latitude, gps_longitude, gps_altitude, gps_heading; - internals::PointLatLng uav_pos; - internals::PointLatLng gps_pos; + internals::PointLatLng uav_pos; + internals::PointLatLng gps_pos; - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } QMutexLocker locker(&m_map_mutex); - // ************* - // get the current UAV details + // ************* + // get the current UAV details // get current UAV position - if (!getUAVPosition(uav_latitude, uav_longitude, uav_altitude)) + if (!getUAVPosition(uav_latitude, uav_longitude, uav_altitude)) { return; + } // get current UAV heading - uav_yaw = getUAV_Yaw(); + uav_yaw = getUAV_Yaw(); - uav_pos = internals::PointLatLng(uav_latitude, uav_longitude); + uav_pos = internals::PointLatLng(uav_latitude, uav_longitude); - // ************* + // ************* // get the current GPS position and heading GPSPosition *gpsPositionObj = GPSPosition::GetInstance(obm); Q_ASSERT(gpsPositionObj); GPSPosition::DataFields gpsPositionData = gpsPositionObj->getData(); - gps_heading = gpsPositionData.Heading; - gps_latitude = gpsPositionData.Latitude; + gps_heading = gpsPositionData.Heading; + gps_latitude = gpsPositionData.Latitude; gps_longitude = gpsPositionData.Longitude; - gps_altitude = gpsPositionData.Altitude; + gps_altitude = gpsPositionData.Altitude; - gps_pos = internals::PointLatLng(gps_latitude*1e-7, gps_longitude*1e-7); + gps_pos = internals::PointLatLng(gps_latitude * 1e-7, gps_longitude * 1e-7); - //********************** + // ********************** // get the current position and heading estimates AttitudeActual *attitudeActualObj = AttitudeActual::GetInstance(obm); PositionActual *positionActualObj = PositionActual::GetInstance(obm); VelocityActual *velocityActualObj = VelocityActual::GetInstance(obm); - AirspeedActual *airspeedActualObj = AirspeedActual::GetInstance(obm); + AirspeedActual *airspeedActualObj = AirspeedActual::GetInstance(obm); Gyros *gyrosObj = Gyros::GetInstance(obm); Q_ASSERT(attitudeActualObj); Q_ASSERT(positionActualObj); Q_ASSERT(velocityActualObj); - Q_ASSERT(airspeedActualObj); + Q_ASSERT(airspeedActualObj); Q_ASSERT(gyrosObj); AttitudeActual::DataFields attitudeActualData = attitudeActualObj->getData(); PositionActual::DataFields positionActualData = positionActualObj->getData(); VelocityActual::DataFields velocityActualData = velocityActualObj->getData(); - AirspeedActual::DataFields airspeedActualData = airspeedActualObj->getData(); + AirspeedActual::DataFields airspeedActualData = airspeedActualObj->getData(); Gyros::DataFields gyrosData = gyrosObj->getData(); - double NED[3]={positionActualData.North, positionActualData.East, positionActualData.Down}; - double vNED[3]={velocityActualData.North, velocityActualData.East, velocityActualData.Down}; + double NED[3] = { positionActualData.North, positionActualData.East, positionActualData.Down }; + double vNED[3] = { velocityActualData.North, velocityActualData.East, velocityActualData.Down }; - //Set the position and heading estimates in the painter module + // Set the position and heading estimates in the painter module m_map->UAV->SetNED(NED); - m_map->UAV->SetCAS(airspeedActualData.CalibratedAirspeed); + m_map->UAV->SetCAS(airspeedActualData.CalibratedAirspeed); m_map->UAV->SetGroundspeed(vNED, m_maxUpdateRate); - //Convert angular velocities into a rotationg rate around the world-frame yaw axis. This is found by simply taking the dot product of the angular Euler-rate matrix with the angular rates. - float psiRate_dps=0*gyrosData.z + sin(attitudeActualData.Roll*deg_to_rad)/cos(attitudeActualData.Pitch*deg_to_rad)*gyrosData.y + cos(attitudeActualData.Roll*deg_to_rad)/cos(attitudeActualData.Pitch*deg_to_rad)*gyrosData.z; + // Convert angular velocities into a rotationg rate around the world-frame yaw axis. This is found by simply taking the dot product of the angular Euler-rate matrix with the angular rates. + float psiRate_dps = 0 * gyrosData.z + sin(attitudeActualData.Roll * deg_to_rad) / cos(attitudeActualData.Pitch * deg_to_rad) * gyrosData.y + cos(attitudeActualData.Roll * deg_to_rad) / cos(attitudeActualData.Pitch * deg_to_rad) * gyrosData.z; - //Set the angular rate in the painter module - m_map->UAV->SetYawRate(psiRate_dps); //Not correct, but I'm being lazy right now. + // Set the angular rate in the painter module + m_map->UAV->SetYawRate(psiRate_dps); // Not correct, but I'm being lazy right now. // ************* - // display the UAV position + // display the UAV position QString str = - "lat: " + QString::number(uav_pos.Lat(), 'f', 7) + - " lon: " + QString::number(uav_pos.Lng(), 'f', 7) + - " " + QString::number(uav_yaw, 'f', 1) + "deg" + - " " + QString::number(uav_altitude, 'f', 1) + "m"; -// " " + QString::number(uav_ground_speed_meters_per_second, 'f', 1) + "m/s"; + "lat: " + QString::number(uav_pos.Lat(), 'f', 7) + + " lon: " + QString::number(uav_pos.Lng(), 'f', 7) + + " " + QString::number(uav_yaw, 'f', 1) + "deg" + + " " + QString::number(uav_altitude, 'f', 1) + "m"; +// " " + QString::number(uav_ground_speed_meters_per_second, 'f', 1) + "m/s"; m_widget->labelUAVPos->setText(str); - // ************* - // set the UAV icon position on the map + // ************* + // set the UAV icon position on the map - m_map->UAV->SetUAVPos(uav_pos, uav_altitude); // set the maps UAV position -// qDebug()<<"UAVPOSITION"<UAV->SetUAVHeading(uav_yaw); // set the maps UAV heading + m_map->UAV->SetUAVPos(uav_pos, uav_altitude); // set the maps UAV position +// qDebug()<<"UAVPOSITION"<UAV->SetUAVHeading(uav_yaw); // set the maps UAV heading - // ************* - // set the GPS icon position on the map - if(m_map->GPS) - { + // ************* + // set the GPS icon position on the map + if (m_map->GPS) { m_map->GPS->SetUAVPos(gps_pos, gps_altitude); // set the maps GPS position - m_map->GPS->SetUAVHeading(gps_heading); // set the maps GPS heading + m_map->GPS->SetUAVHeading(gps_heading); // set the maps GPS heading m_map->GPS->update(); } m_map->UAV->updateTextOverlay(); m_map->UAV->update(); - // ************* + // ************* } /** - Update plugin behaviour based on mouse position; Called every few ms by a - timer. + Update plugin behaviour based on mouse position; Called every few ms by a + timer. */ void OPMapGadgetWidget::updateMousePos() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } QMutexLocker locker(&m_map_mutex); QPoint p = m_map->mapFromGlobal(QCursor::pos()); - internals::PointLatLng lat_lon = m_map->GetFromLocalToLatLng(p); // fetch the current lat/lon mouse position - lastLatLngMouse=lat_lon; - if (!m_map->contentsRect().contains(p)) - return; // the mouse is not on the map - -// internals::PointLatLng lat_lon = m_map->currentMousePosition(); // fetch the current lat/lon mouse position + internals::PointLatLng lat_lon = m_map->GetFromLocalToLatLng(p); // fetch the current lat/lon mouse position + lastLatLngMouse = lat_lon; + if (!m_map->contentsRect().contains(p)) { + return; // the mouse is not on the map + } +// internals::PointLatLng lat_lon = m_map->currentMousePosition(); // fetch the current lat/lon mouse position QGraphicsItem *item = m_map->itemAt(p); // find out if we are over the home position - mapcontrol::HomeItem *home = qgraphicsitem_cast(item); + mapcontrol::HomeItem *home = qgraphicsitem_cast(item); // find out if we have a waypoint under the mouse cursor mapcontrol::WayPointItem *wp = qgraphicsitem_cast(item); - if (m_mouse_lat_lon == lat_lon) - return; // the mouse has not moved - - m_mouse_lat_lon = lat_lon; // yes it has! + if (m_mouse_lat_lon == lat_lon) { + return; // the mouse has not moved + } + m_mouse_lat_lon = lat_lon; // yes it has! internals::PointLatLng home_lat_lon = m_map->Home->Coord(); - QString s = QString::number(m_mouse_lat_lon.Lat(), 'f', 7) + " " + QString::number(m_mouse_lat_lon.Lng(), 'f', 7); - if (wp) - { + QString s = QString::number(m_mouse_lat_lon.Lat(), 'f', 7) + " " + QString::number(m_mouse_lat_lon.Lng(), 'f', 7); + if (wp) { s += " wp[" + QString::number(wp->numberAdjusted()) + "]"; double dist = distance(home_lat_lon, wp->Coord()); double bear = bearing(home_lat_lon, wp->Coord()); s += " " + QString::number(dist * 1000, 'f', 1) + "m"; s += " " + QString::number(bear, 'f', 1) + "deg"; - } - else - if (home) - { + } else if (home) { s += " home"; - double dist = distance(home_lat_lon, m_mouse_lat_lon); - double bear = bearing(home_lat_lon, m_mouse_lat_lon); + double dist = distance(home_lat_lon, m_mouse_lat_lon); + double bear = bearing(home_lat_lon, m_mouse_lat_lon); s += " " + QString::number(dist * 1000, 'f', 1) + "m"; s += " " + QString::number(bear, 'f', 1) + "deg"; } @@ -713,75 +716,84 @@ void OPMapGadgetWidget::updateMousePos() /** - Update the Plugin UI to reflect a change in zoom level - */ + Update the Plugin UI to reflect a change in zoom level + */ void OPMapGadgetWidget::zoomChanged(double zoomt, double zoom, double zoomd) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - QString s = "tot:" + QString::number(zoomt, 'f', 1) + " rea:" + QString::number(zoom, 'f', 1) + " dig:" + QString::number(zoomd, 'f', 1); + QString s = "tot:" + QString::number(zoomt, 'f', 1) + " rea:" + QString::number(zoom, 'f', 1) + " dig:" + QString::number(zoomd, 'f', 1); m_widget->labelMapZoom->setText(s); int i_zoom = (int)(zoomt + 0.5); - if (i_zoom < m_min_zoom) i_zoom = m_min_zoom; - else - if (i_zoom > m_max_zoom) i_zoom = m_max_zoom; + if (i_zoom < m_min_zoom) { + i_zoom = m_min_zoom; + } else if (i_zoom > m_max_zoom) { + i_zoom = m_max_zoom; + } - if (m_widget->horizontalSliderZoom->value() != i_zoom) - m_widget->horizontalSliderZoom->setValue(i_zoom); // set the GUI zoom slider position - - int index0_zoom = i_zoom - m_min_zoom; // zoom level starting at index level '0' - if (index0_zoom < zoomAct.count()) - zoomAct.at(index0_zoom)->setChecked(true); // set the right-click context menu zoom level + if (m_widget->horizontalSliderZoom->value() != i_zoom) { + m_widget->horizontalSliderZoom->setValue(i_zoom); // set the GUI zoom slider position + } + int index0_zoom = i_zoom - m_min_zoom; // zoom level starting at index level '0' + if (index0_zoom < zoomAct.count()) { + zoomAct.at(index0_zoom)->setChecked(true); // set the right-click context menu zoom level + } } void OPMapGadgetWidget::OnCurrentPositionChanged(internals::PointLatLng point) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } QString coord_str = QString::number(point.Lat(), 'f', 7) + " " + QString::number(point.Lng(), 'f', 7) + " "; m_widget->labelMapPos->setText(coord_str); } /** - Update the progress bar while there are still tiles to load - */ + Update the progress bar while there are still tiles to load + */ void OPMapGadgetWidget::OnTilesStillToLoad(int number) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - if (m_widget->progressBarMap->maximum() < number) - m_widget->progressBarMap->setMaximum(number); + if (m_widget->progressBarMap->maximum() < number) { + m_widget->progressBarMap->setMaximum(number); + } - m_widget->progressBarMap->setValue(m_widget->progressBarMap->maximum() - number); // update the progress bar + m_widget->progressBarMap->setValue(m_widget->progressBarMap->maximum() - number); // update the progress bar - m_prev_tile_number = number; + m_prev_tile_number = number; } /** - Show the progress bar as soon as the map lib starts downloading - */ + Show the progress bar as soon as the map lib starts downloading + */ void OPMapGadgetWidget::OnTileLoadStart() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_widget->progressBarMap->setVisible(true); } /** - Hide the progress bar once the map lib has finished downloading + Hide the progress bar once the map lib has finished downloading - TODO: somehow this gets called before tile load is actually complete? - */ + TODO: somehow this gets called before tile load is actually complete? + */ void OPMapGadgetWidget::OnTileLoadComplete() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_widget->progressBarMap->setVisible(false); } @@ -789,25 +801,29 @@ void OPMapGadgetWidget::OnTileLoadComplete() void OPMapGadgetWidget::on_toolButtonZoomP_clicked() { QMutexLocker locker(&m_map_mutex); + zoomIn(); } void OPMapGadgetWidget::on_toolButtonZoomM_clicked() { QMutexLocker locker(&m_map_mutex); + zoomOut(); } void OPMapGadgetWidget::on_toolButtonMapHome_clicked() { QMutexLocker locker(&m_map_mutex); + goHome(); } void OPMapGadgetWidget::on_toolButtonMapUAV_clicked() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } QMutexLocker locker(&m_map_mutex); @@ -816,16 +832,18 @@ void OPMapGadgetWidget::on_toolButtonMapUAV_clicked() void OPMapGadgetWidget::on_toolButtonMapUAVheading_clicked() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } followUAVheadingAct->toggle(); } void OPMapGadgetWidget::on_horizontalSliderZoom_sliderMoved(int position) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } QMutexLocker locker(&m_map_mutex); @@ -858,103 +876,119 @@ void OPMapGadgetWidget::on_toolButtonMoveToWP_clicked() void OPMapGadgetWidget::onTelemetryConnect() { - m_telemetry_connected = true; + m_telemetry_connected = true; - if (!obum) return; + if (!obum) { + return; + } - bool set; - double LLA[3]; + bool set; + double LLA[3]; - // *********************** - // fetch the home location + // *********************** + // fetch the home location - if (obum->getHomeLocation(set, LLA) < 0) - return; // error + if (obum->getHomeLocation(set, LLA) < 0) { + return; // error + } + setHome(internals::PointLatLng(LLA[0], LLA[1]), LLA[2]); - setHome(internals::PointLatLng(LLA[0], LLA[1]),LLA[2]); - - if (m_map) - { - if(m_map->UAV->GetMapFollowType()!=UAVMapFollowType::None) - m_map->SetCurrentPosition(m_home_position.coord); // set the map position + if (m_map) { + if (m_map->UAV->GetMapFollowType() != UAVMapFollowType::None) { + m_map->SetCurrentPosition(m_home_position.coord); // set the map position + } } // *********************** } void OPMapGadgetWidget::onTelemetryDisconnect() { - m_telemetry_connected = false; + m_telemetry_connected = false; } // Updates the Home position icon whenever the HomePosition object is updated void OPMapGadgetWidget::homePositionUpdated(UAVObject *hp) { Q_UNUSED(hp); - if (!obum) return; + if (!obum) { + return; + } bool set; double LLA[3]; - if (obum->getHomeLocation(set, LLA) < 0) - return; // error - setHome(internals::PointLatLng(LLA[0], LLA[1]),LLA[2]); - + if (obum->getHomeLocation(set, LLA) < 0) { + return; // error + } + setHome(internals::PointLatLng(LLA[0], LLA[1]), LLA[2]); } // ************************************************************************************* // public functions /** - Sets the home position on the map widget - */ + Sets the home position on the map widget + */ void OPMapGadgetWidget::setHome(QPointF pos) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - double latitude = pos.x(); + double latitude = pos.x(); double longitude = pos.y(); - if (latitude > 90) latitude = 90; - else - if (latitude < -90) latitude = -90; + if (latitude > 90) { + latitude = 90; + } else if (latitude < -90) { + latitude = -90; + } - if (longitude != longitude) longitude = 0; // nan detection - else - if (longitude > 180) longitude = 180; - else - if (longitude < -180) longitude = -180; + if (longitude != longitude) { + longitude = 0; // nan detection + } else if (longitude > 180) { + longitude = 180; + } else if (longitude < -180) { + longitude = -180; + } - setHome(internals::PointLatLng(latitude, longitude),0); + setHome(internals::PointLatLng(latitude, longitude), 0); } /** - Sets the home position on the map widget - */ + Sets the home position on the map widget + */ void OPMapGadgetWidget::setHome(internals::PointLatLng pos_lat_lon, double altitude) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - if (pos_lat_lon.Lat() != pos_lat_lon.Lat() || pos_lat_lon.Lng() != pos_lat_lon.Lng()) - return;; // nan prevention + if (pos_lat_lon.Lat() != pos_lat_lon.Lat() || pos_lat_lon.Lng() != pos_lat_lon.Lng()) { + return; + } + ; // nan prevention - double latitude = pos_lat_lon.Lat(); + double latitude = pos_lat_lon.Lat(); double longitude = pos_lat_lon.Lng(); - if (latitude != latitude) latitude = 0; // nan detection - else - if (latitude > 90) latitude = 90; - else - if (latitude < -90) latitude = -90; + if (latitude != latitude) { + latitude = 0; // nan detection + } else if (latitude > 90) { + latitude = 90; + } else if (latitude < -90) { + latitude = -90; + } - if (longitude != longitude) longitude = 0; // nan detection - else - if (longitude > 180) longitude = 180; - else - if (longitude < -180) longitude = -180; + if (longitude != longitude) { + longitude = 0; // nan detection + } else if (longitude > 180) { + longitude = 180; + } else if (longitude < -180) { + longitude = -180; + } // ********* - m_home_position.coord = internals::PointLatLng(latitude, longitude); + m_home_position.coord = internals::PointLatLng(latitude, longitude); m_home_position.altitude = altitude; m_map->Home->SetCoord(m_home_position.coord); @@ -967,78 +1001,92 @@ void OPMapGadgetWidget::setHome(internals::PointLatLng pos_lat_lon, double altit /** - Centers the map over the home position - */ + Centers the map over the home position + */ void OPMapGadgetWidget::goHome() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } followUAVpositionAct->setChecked(false); - internals::PointLatLng home_pos = m_home_position.coord; // get the home location - m_map->SetCurrentPosition(home_pos); // center the map onto the home location + internals::PointLatLng home_pos = m_home_position.coord; // get the home location + m_map->SetCurrentPosition(home_pos); // center the map onto the home location } void OPMapGadgetWidget::zoomIn() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } int zoom = m_map->ZoomTotal() + 1; - if (zoom < m_min_zoom) zoom = m_min_zoom; - else - if (zoom > m_max_zoom) zoom = m_max_zoom; + if (zoom < m_min_zoom) { + zoom = m_min_zoom; + } else if (zoom > m_max_zoom) { + zoom = m_max_zoom; + } m_map->SetZoom(zoom); } void OPMapGadgetWidget::zoomOut() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } int zoom = m_map->ZoomTotal() - 1; - if (zoom < m_min_zoom) zoom = m_min_zoom; - else - if (zoom > m_max_zoom) zoom = m_max_zoom; + if (zoom < m_min_zoom) { + zoom = m_min_zoom; + } else if (zoom > m_max_zoom) { + zoom = m_max_zoom; + } m_map->SetZoom(zoom); } void OPMapGadgetWidget::setMaxUpdateRate(int update_rate) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - int list_size = sizeof(max_update_rate_list) / sizeof(max_update_rate_list[0]); - int min_rate = max_update_rate_list[0]; - int max_rate = max_update_rate_list[list_size - 1]; + int list_size = sizeof(max_update_rate_list) / sizeof(max_update_rate_list[0]); + int min_rate = max_update_rate_list[0]; + int max_rate = max_update_rate_list[list_size - 1]; - if (update_rate < min_rate) update_rate = min_rate; - else - if (update_rate > max_rate) update_rate = max_rate; + if (update_rate < min_rate) { + update_rate = min_rate; + } else if (update_rate > max_rate) { + update_rate = max_rate; + } - m_maxUpdateRate = update_rate; + m_maxUpdateRate = update_rate; - if (m_updateTimer) - m_updateTimer->setInterval(m_maxUpdateRate); + if (m_updateTimer) { + m_updateTimer->setInterval(m_maxUpdateRate); + } -// if (m_statusUpdateTimer) -// m_statusUpdateTimer->setInterval(m_maxUpdateRate); +// if (m_statusUpdateTimer) +// m_statusUpdateTimer->setInterval(m_maxUpdateRate); } void OPMapGadgetWidget::setZoom(int zoom) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - if (zoom < m_min_zoom) zoom = m_min_zoom; - else - if (zoom > m_max_zoom) zoom = m_max_zoom; + if (zoom < m_min_zoom) { + zoom = m_min_zoom; + } else if (zoom > m_max_zoom) { + zoom = m_max_zoom; + } internals::MouseWheelZoomType::Types zoom_type = m_map->GetMouseWheelZoomType(); m_map->SetMouseWheelZoomType(internals::MouseWheelZoomType::ViewCenter); @@ -1049,167 +1097,188 @@ void OPMapGadgetWidget::setZoom(int zoom) } void OPMapGadgetWidget::setOverlayOpacity(qreal value) { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } m_map->setOverlayOpacity(value); - overlayOpacityAct.at(value*10)->setChecked(true); + overlayOpacityAct.at(value * 10)->setChecked(true); } void OPMapGadgetWidget::setHomePosition(QPointF pos) { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - double latitude = pos.y(); + double latitude = pos.y(); double longitude = pos.x(); - if (latitude != latitude || longitude != longitude) + if (latitude != latitude || longitude != longitude) { return; // nan prevention + } + if (latitude > 90) { + latitude = 90; + } else if (latitude < -90) { + latitude = -90; + } - if (latitude > 90) latitude = 90; - else - if (latitude < -90) latitude = -90; - - if (longitude > 180) longitude = 180; - else - if (longitude < -180) longitude = -180; + if (longitude > 180) { + longitude = 180; + } else if (longitude < -180) { + longitude = -180; + } m_map->Home->SetCoord(internals::PointLatLng(latitude, longitude)); } void OPMapGadgetWidget::setPosition(QPointF pos) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - double latitude = pos.y(); + double latitude = pos.y(); double longitude = pos.x(); - if (latitude != latitude || longitude != longitude) + if (latitude != latitude || longitude != longitude) { return; // nan prevention + } + if (latitude > 90) { + latitude = 90; + } else if (latitude < -90) { + latitude = -90; + } - if (latitude > 90) latitude = 90; - else - if (latitude < -90) latitude = -90; - - if (longitude > 180) longitude = 180; - else - if (longitude < -180) longitude = -180; + if (longitude > 180) { + longitude = 180; + } else if (longitude < -180) { + longitude = -180; + } m_map->SetCurrentPosition(internals::PointLatLng(latitude, longitude)); } void OPMapGadgetWidget::setMapProvider(QString provider) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->SetMapType(mapcontrol::Helper::MapTypeFromString(provider)); } void OPMapGadgetWidget::setAccessMode(QString accessMode) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->configuration->SetAccessMode(mapcontrol::Helper::AccessModeFromString(accessMode)); } void OPMapGadgetWidget::setUseOpenGL(bool useOpenGL) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->SetUseOpenGL(useOpenGL); } void OPMapGadgetWidget::setShowTileGridLines(bool showTileGridLines) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->SetShowTileGridLines(showTileGridLines); } void OPMapGadgetWidget::setUseMemoryCache(bool useMemoryCache) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->configuration->SetUseMemoryCache(useMemoryCache); } void OPMapGadgetWidget::setCacheLocation(QString cacheLocation) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - cacheLocation = cacheLocation.simplified(); // remove any surrounding spaces + cacheLocation = cacheLocation.simplified(); // remove any surrounding spaces - if (cacheLocation.isEmpty()) return; + if (cacheLocation.isEmpty()) { + return; + } - if (!cacheLocation.endsWith(QDir::separator())) cacheLocation += QDir::separator(); + if (!cacheLocation.endsWith(QDir::separator())) { + cacheLocation += QDir::separator(); + } QDir dir; - if (!dir.exists(cacheLocation)) - if (!dir.mkpath(cacheLocation)) + if (!dir.exists(cacheLocation)) { + if (!dir.mkpath(cacheLocation)) { return; + } + } m_map->configuration->SetCacheLocation(cacheLocation); } void OPMapGadgetWidget::setMapMode(opMapModeType mode) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - if (mode != Normal_MapMode && mode != MagicWaypoint_MapMode) - mode = Normal_MapMode; // fix error - - if (m_map_mode == mode) - { // no change in map mode - switch (mode) - { // make sure the UI buttons are set correctly - case Normal_MapMode: - m_widget->toolButtonMagicWaypointMapMode->setChecked(false); - m_widget->toolButtonNormalMapMode->setChecked(true); - break; - case MagicWaypoint_MapMode: - m_widget->toolButtonNormalMapMode->setChecked(false); - m_widget->toolButtonMagicWaypointMapMode->setChecked(true); - break; + if (mode != Normal_MapMode && mode != MagicWaypoint_MapMode) { + mode = Normal_MapMode; // fix error + } + if (m_map_mode == mode) { // no change in map mode + switch (mode) { // make sure the UI buttons are set correctly + case Normal_MapMode: + m_widget->toolButtonMagicWaypointMapMode->setChecked(false); + m_widget->toolButtonNormalMapMode->setChecked(true); + break; + case MagicWaypoint_MapMode: + m_widget->toolButtonNormalMapMode->setChecked(false); + m_widget->toolButtonMagicWaypointMapMode->setChecked(true); + break; } return; } - switch (mode) - { - case Normal_MapMode: - m_map_mode = Normal_MapMode; + switch (mode) { + case Normal_MapMode: + m_map_mode = Normal_MapMode; - m_widget->toolButtonMagicWaypointMapMode->setChecked(false); - m_widget->toolButtonNormalMapMode->setChecked(true); + m_widget->toolButtonMagicWaypointMapMode->setChecked(false); + m_widget->toolButtonNormalMapMode->setChecked(true); - hideMagicWaypointControls(); + hideMagicWaypointControls(); - magicWayPoint->setVisible(false); - m_map->WPSetVisibleAll(true); + magicWayPoint->setVisible(false); + m_map->WPSetVisibleAll(true); - break; + break; - case MagicWaypoint_MapMode: - m_map_mode = MagicWaypoint_MapMode; + case MagicWaypoint_MapMode: + m_map_mode = MagicWaypoint_MapMode; - m_widget->toolButtonNormalMapMode->setChecked(false); - m_widget->toolButtonMagicWaypointMapMode->setChecked(true); + m_widget->toolButtonNormalMapMode->setChecked(false); + m_widget->toolButtonMagicWaypointMapMode->setChecked(true); - showMagicWaypointControls(); + showMagicWaypointControls(); - // delete the normal waypoints from the map + // delete the normal waypoints from the map - m_map->WPSetVisibleAll(false); - magicWayPoint->setVisible(true); + m_map->WPSetVisibleAll(false); + magicWayPoint->setVisible(true); - break; + break; } } @@ -1218,10 +1287,11 @@ void OPMapGadgetWidget::setMapMode(opMapModeType mode) void OPMapGadgetWidget::createActions() { - int list_size; + int list_size; - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } // *********************** // create menu actions @@ -1306,7 +1376,7 @@ void OPMapGadgetWidget::createActions() setHomeAct = new QAction(tr("Set the home location"), this); setHomeAct->setStatusTip(tr("Set the home location to where you clicked")); #if !defined(allow_manual_home_location_move) - setHomeAct->setEnabled(false); + setHomeAct->setEnabled(false); #endif connect(setHomeAct, SIGNAL(triggered()), this, SLOT(onSetHomeAct_triggered())); @@ -1335,8 +1405,8 @@ void OPMapGadgetWidget::createActions() connect(followUAVheadingAct, SIGNAL(toggled(bool)), this, SLOT(onFollowUAVheadingAct_toggled(bool))); /* - TODO: Waypoint support is disabled for v1.0 - */ + TODO: Waypoint support is disabled for v1.0 + */ #ifdef USE_PATHPLANNER wayPointEditorAct = new QAction(tr("&Waypoint editor"), this); @@ -1375,15 +1445,14 @@ void OPMapGadgetWidget::createActions() clearWayPointsAct->setShortcut(tr("Ctrl+C")); clearWayPointsAct->setStatusTip(tr("Clear waypoints")); connect(clearWayPointsAct, SIGNAL(triggered()), this, SLOT(onClearWayPointsAct_triggered())); -#endif +#endif // ifdef USE_PATHPLANNER overlayOpacityActGroup = new QActionGroup(this); connect(overlayOpacityActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onOverlayOpacityActGroup_triggered(QAction *))); overlayOpacityAct.clear(); - for (int i = 0; i <= 10; i++) - { - QAction *overlayAct = new QAction(QString::number(i*10), overlayOpacityActGroup); + for (int i = 0; i <= 10; i++) { + QAction *overlayAct = new QAction(QString::number(i * 10), overlayOpacityActGroup); overlayAct->setCheckable(true); - overlayAct->setData(i*10); + overlayAct->setData(i * 10); overlayOpacityAct.append(overlayAct); } @@ -1413,30 +1482,28 @@ void OPMapGadgetWidget::createActions() zoomActGroup = new QActionGroup(this); connect(zoomActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onZoomActGroup_triggered(QAction *))); zoomAct.clear(); - for (int i = m_min_zoom; i <= m_max_zoom; i++) - { + for (int i = m_min_zoom; i <= m_max_zoom; i++) { QAction *zoom_act = new QAction(QString::number(i), zoomActGroup); zoom_act->setCheckable(true); zoom_act->setData(i); zoomAct.append(zoom_act); } - maxUpdateRateActGroup = new QActionGroup(this); - connect(maxUpdateRateActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onMaxUpdateRateActGroup_triggered(QAction *))); - maxUpdateRateAct.clear(); - list_size = sizeof(max_update_rate_list) / sizeof(max_update_rate_list[0]); - for (int i = 0; i < list_size; i++) - { - QAction *maxUpdateRate_act; - int j = max_update_rate_list[i]; - maxUpdateRate_act = new QAction(QString::number(j), maxUpdateRateActGroup); - maxUpdateRate_act->setCheckable(true); - maxUpdateRate_act->setData(j); - maxUpdateRate_act->setChecked(j == m_maxUpdateRate); - maxUpdateRateAct.append(maxUpdateRate_act); - } + maxUpdateRateActGroup = new QActionGroup(this); + connect(maxUpdateRateActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onMaxUpdateRateActGroup_triggered(QAction *))); + maxUpdateRateAct.clear(); + list_size = sizeof(max_update_rate_list) / sizeof(max_update_rate_list[0]); + for (int i = 0; i < list_size; i++) { + QAction *maxUpdateRate_act; + int j = max_update_rate_list[i]; + maxUpdateRate_act = new QAction(QString::number(j), maxUpdateRateActGroup); + maxUpdateRate_act->setCheckable(true); + maxUpdateRate_act->setData(j); + maxUpdateRate_act->setChecked(j == m_maxUpdateRate); + maxUpdateRateAct.append(maxUpdateRate_act); + } - // ***** + // ***** // safe area showSafeAreaAct = new QAction(tr("Show Safe Area"), this); @@ -1448,9 +1515,8 @@ void OPMapGadgetWidget::createActions() safeAreaActGroup = new QActionGroup(this); connect(safeAreaActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onSafeAreaActGroup_triggered(QAction *))); safeAreaAct.clear(); - list_size = sizeof(safe_area_radius_list) / sizeof(safe_area_radius_list[0]); - for (int i = 0; i < list_size; i++) - { + list_size = sizeof(safe_area_radius_list) / sizeof(safe_area_radius_list[0]); + for (int i = 0; i < list_size; i++) { int safeArea = safe_area_radius_list[i]; QAction *safeArea_act = new QAction(QString::number(safeArea) + "m", safeAreaActGroup); safeArea_act->setCheckable(true); @@ -1466,8 +1532,7 @@ void OPMapGadgetWidget::createActions() connect(uavTrailTypeActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onUAVTrailTypeActGroup_triggered(QAction *))); uavTrailTypeAct.clear(); QStringList uav_trail_type_list = mapcontrol::Helper::UAVTrailTypes(); - for (int i = 0; i < uav_trail_type_list.count(); i++) - { + for (int i = 0; i < uav_trail_type_list.count(); i++) { mapcontrol::UAVTrailType::Types uav_trail_type = mapcontrol::Helper::UAVTrailTypeFromString(uav_trail_type_list[i]); QAction *uavTrailType_act = new QAction(mapcontrol::Helper::StrFromUAVTrailType(uav_trail_type), uavTrailTypeActGroup); uavTrailType_act->setCheckable(true); @@ -1495,9 +1560,8 @@ void OPMapGadgetWidget::createActions() uavTrailTimeActGroup = new QActionGroup(this); connect(uavTrailTimeActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onUAVTrailTimeActGroup_triggered(QAction *))); uavTrailTimeAct.clear(); - list_size = sizeof(uav_trail_time_list) / sizeof(uav_trail_time_list[0]); - for (int i = 0; i < list_size; i++) - { + list_size = sizeof(uav_trail_time_list) / sizeof(uav_trail_time_list[0]); + for (int i = 0; i < list_size; i++) { int uav_trail_time = uav_trail_time_list[i]; QAction *uavTrailTime_act = new QAction(QString::number(uav_trail_time) + " sec", uavTrailTimeActGroup); uavTrailTime_act->setCheckable(true); @@ -1509,9 +1573,8 @@ void OPMapGadgetWidget::createActions() uavTrailDistanceActGroup = new QActionGroup(this); connect(uavTrailDistanceActGroup, SIGNAL(triggered(QAction *)), this, SLOT(onUAVTrailDistanceActGroup_triggered(QAction *))); uavTrailDistanceAct.clear(); - list_size = sizeof(uav_trail_distance_list) / sizeof(uav_trail_distance_list[0]); - for (int i = 0; i < list_size; i++) - { + list_size = sizeof(uav_trail_distance_list) / sizeof(uav_trail_distance_list[0]); + for (int i = 0; i < list_size; i++) { int uav_trail_distance = uav_trail_distance_list[i]; QAction *uavTrailDistance_act = new QAction(QString::number(uav_trail_distance) + " meters", uavTrailDistanceActGroup); uavTrailDistance_act->setCheckable(true); @@ -1523,8 +1586,9 @@ void OPMapGadgetWidget::createActions() void OPMapGadgetWidget::onReloadAct_triggered() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->ReloadMap(); } @@ -1537,88 +1601,102 @@ void OPMapGadgetWidget::onRipAct_triggered() void OPMapGadgetWidget::onCopyMouseLatLonToClipAct_triggered() { QClipboard *clipboard = QApplication::clipboard(); - clipboard->setText(QString::number(m_context_menu_lat_lon.Lat(), 'f', 7) + ", " + QString::number(m_context_menu_lat_lon.Lng(), 'f', 7), QClipboard::Clipboard); + + clipboard->setText(QString::number(m_context_menu_lat_lon.Lat(), 'f', 7) + ", " + QString::number(m_context_menu_lat_lon.Lng(), 'f', 7), QClipboard::Clipboard); } void OPMapGadgetWidget::onCopyMouseLatToClipAct_triggered() { QClipboard *clipboard = QApplication::clipboard(); - clipboard->setText(QString::number(m_context_menu_lat_lon.Lat(), 'f', 7), QClipboard::Clipboard); + + clipboard->setText(QString::number(m_context_menu_lat_lon.Lat(), 'f', 7), QClipboard::Clipboard); } void OPMapGadgetWidget::onCopyMouseLonToClipAct_triggered() { QClipboard *clipboard = QApplication::clipboard(); - clipboard->setText(QString::number(m_context_menu_lat_lon.Lng(), 'f', 7), QClipboard::Clipboard); + + clipboard->setText(QString::number(m_context_menu_lat_lon.Lng(), 'f', 7), QClipboard::Clipboard); } void OPMapGadgetWidget::onShowCompassAct_toggled(bool show) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->SetShowCompass(show); } void OPMapGadgetWidget::onShowDiagnostics_toggled(bool show) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->SetShowDiagnostics(show); } void OPMapGadgetWidget::onShowUAVInfo_toggled(bool show) { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } m_map->UAV->SetShowUAVInfo(show); } void OPMapGadgetWidget::onShowHomeAct_toggled(bool show) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->Home->setVisible(show); } void OPMapGadgetWidget::onShowUAVAct_toggled(bool show) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->UAV->setVisible(show); - if(m_map->GPS) + if (m_map->GPS) { m_map->GPS->setVisible(show); + } } void OPMapGadgetWidget::onShowTrailAct_toggled(bool show) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->UAV->SetShowTrail(show); - if(m_map->GPS) + if (m_map->GPS) { m_map->GPS->SetShowTrail(show); + } } void OPMapGadgetWidget::onShowTrailLineAct_toggled(bool show) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->UAV->SetShowTrailLine(show); - if(m_map->GPS) + if (m_map->GPS) { m_map->GPS->SetShowTrailLine(show); + } } void OPMapGadgetWidget::onMapModeActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) + if (!m_widget || !m_map || !action) { return; + } opMapModeType mode = (opMapModeType)action->data().toInt(); @@ -1637,101 +1715,113 @@ void OPMapGadgetWidget::onGoZoomOutAct_triggered() void OPMapGadgetWidget::onZoomActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) - return; + if (!m_widget || !m_map || !action) { + return; + } setZoom(action->data().toInt()); } void OPMapGadgetWidget::onMaxUpdateRateActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) - return; + if (!m_widget || !m_map || !action) { + return; + } setMaxUpdateRate(action->data().toInt()); } void OPMapGadgetWidget::onChangeDefaultLocalAndZoom() { - emit defaultLocationAndZoomChanged(m_map->CurrentPosition().Lng(),m_map->CurrentPosition().Lat(),m_map->ZoomTotal()); + emit defaultLocationAndZoomChanged(m_map->CurrentPosition().Lng(), m_map->CurrentPosition().Lat(), m_map->ZoomTotal()); } void OPMapGadgetWidget::onGoMouseClickAct_triggered() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - m_map->SetCurrentPosition(m_map->currentMousePosition()); // center the map onto the mouse position + m_map->SetCurrentPosition(m_map->currentMousePosition()); // center the map onto the mouse position } void OPMapGadgetWidget::onSetHomeAct_triggered() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - float altitude=0; + float altitude = 0; bool ok; - //Get desired HomeLocation altitude from dialog box. - //TODO: Populate box with altitude already in HomeLocation UAVO + // Get desired HomeLocation altitude from dialog box. + // TODO: Populate box with altitude already in HomeLocation UAVO altitude = QInputDialog::getDouble(this, tr("Set home altitude"), - tr("In [m], referenced to WGS84:"), altitude, -100, 100000, 2, &ok); + tr("In [m], referenced to WGS84:"), altitude, -100, 100000, 2, &ok); setHome(m_context_menu_lat_lon, altitude); - setHomeLocationObject(); // update the HomeLocation UAVObject + setHomeLocationObject(); // update the HomeLocation UAVObject } void OPMapGadgetWidget::onGoHomeAct_triggered() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } goHome(); } void OPMapGadgetWidget::onGoUAVAct_triggered() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } double latitude; double longitude; double altitude; - if (getUAVPosition(latitude, longitude, altitude)) // get current UAV position - { - internals::PointLatLng uav_pos = internals::PointLatLng(latitude, longitude); // current UAV position - internals::PointLatLng map_pos = m_map->CurrentPosition(); // current MAP position - if (map_pos != uav_pos) m_map->SetCurrentPosition(uav_pos); // center the map onto the UAV + if (getUAVPosition(latitude, longitude, altitude)) { // get current UAV position + internals::PointLatLng uav_pos = internals::PointLatLng(latitude, longitude); // current UAV position + internals::PointLatLng map_pos = m_map->CurrentPosition(); // current MAP position + if (map_pos != uav_pos) { + m_map->SetCurrentPosition(uav_pos); // center the map onto the UAV + } } } void OPMapGadgetWidget::onFollowUAVpositionAct_toggled(bool checked) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - if (m_widget->toolButtonMapUAV->isChecked() != checked) + if (m_widget->toolButtonMapUAV->isChecked() != checked) { m_widget->toolButtonMapUAV->setChecked(checked); + } setMapFollowingMode(); } void OPMapGadgetWidget::onFollowUAVheadingAct_toggled(bool checked) { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } - if (m_widget->toolButtonMapUAVheading->isChecked() != checked) + if (m_widget->toolButtonMapUAVheading->isChecked() != checked) { m_widget->toolButtonMapUAVheading->setChecked(checked); + } setMapFollowingMode(); } void OPMapGadgetWidget::onUAVTrailTypeActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) - return; + if (!m_widget || !m_map || !action) { + return; + } int trail_type_idx = action->data().toInt(); @@ -1743,18 +1833,21 @@ void OPMapGadgetWidget::onUAVTrailTypeActGroup_triggered(QAction *action) void OPMapGadgetWidget::onClearUAVtrailAct_triggered() { - if (!m_widget || !m_map) - return; + if (!m_widget || !m_map) { + return; + } m_map->UAV->DeleteTrail(); - if(m_map->GPS) + if (m_map->GPS) { m_map->GPS->DeleteTrail(); + } } void OPMapGadgetWidget::onUAVTrailTimeActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) - return; + if (!m_widget || !m_map || !action) { + return; + } int trail_time = (double)action->data().toInt(); @@ -1763,8 +1856,9 @@ void OPMapGadgetWidget::onUAVTrailTimeActGroup_triggered(QAction *action) void OPMapGadgetWidget::onUAVTrailDistanceActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) - return; + if (!m_widget || !m_map || !action) { + return; + } int trail_distance = action->data().toInt(); @@ -1786,33 +1880,38 @@ void OPMapGadgetWidget::onAddWayPointAct_triggeredFromThis() void OPMapGadgetWidget::onAddWayPointAct_triggered(internals::PointLatLng coord) { - if (!m_widget || !m_map) - return; - - if (m_map_mode != Normal_MapMode) + if (!m_widget || !m_map) { return; + } + + if (m_map_mode != Normal_MapMode) { + return; + } mapProxy->createWayPoint(coord); } /** - * Called when the user asks to edit a waypoint from the map - * - * TODO: should open an interface to edit waypoint properties, or - * propagate the signal to a specific WP plugin (tbd). - **/ + * Called when the user asks to edit a waypoint from the map + * + * TODO: should open an interface to edit waypoint properties, or + * propagate the signal to a specific WP plugin (tbd). + **/ void OPMapGadgetWidget::onEditWayPointAct_triggered() { - if (!m_widget || !m_map) - return; - - if (m_map_mode != Normal_MapMode) + if (!m_widget || !m_map) { return; + } - if (!m_mouse_waypoint) + if (m_map_mode != Normal_MapMode) { return; + } + + if (!m_mouse_waypoint) { + return; + } waypoint_edit_dialog->editWaypoint(m_mouse_waypoint); m_mouse_waypoint = NULL; @@ -1820,24 +1919,27 @@ void OPMapGadgetWidget::onEditWayPointAct_triggered() /** - * TODO: unused for v1.0 - */ + * TODO: unused for v1.0 + */ void OPMapGadgetWidget::onLockWayPointAct_triggered() { - if (!m_widget || !m_map || !m_mouse_waypoint) + if (!m_widget || !m_map || !m_mouse_waypoint) { return; + } - if (m_map_mode != Normal_MapMode) + if (m_map_mode != Normal_MapMode) { return; + } bool locked = (m_mouse_waypoint->flags() & QGraphicsItem::ItemIsMovable) == 0; m_mouse_waypoint->setFlag(QGraphicsItem::ItemIsMovable, locked); - if (!locked) + if (!locked) { m_mouse_waypoint->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker2.png")); - else + } else { m_mouse_waypoint->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker1.png")); + } m_mouse_waypoint->update(); m_mouse_waypoint = NULL; @@ -1845,42 +1947,45 @@ void OPMapGadgetWidget::onLockWayPointAct_triggered() void OPMapGadgetWidget::onDeleteWayPointAct_triggered() { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - if (m_map_mode != Normal_MapMode) + if (m_map_mode != Normal_MapMode) { return; + } - if (!m_mouse_waypoint) + if (!m_mouse_waypoint) { return; + } mapProxy->deleteWayPoint(m_mouse_waypoint->Number()); } void OPMapGadgetWidget::onClearWayPointsAct_triggered() { - - //First, ask to ensure this is what the user wants to do + // First, ask to ensure this is what the user wants to do QMessageBox msgBox; + msgBox.setText(tr("Are you sure you want to clear waypoints?")); msgBox.setInformativeText(tr("All associated data will be lost.")); msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No); int ret = msgBox.exec(); - if (ret == QMessageBox::No) - { + if (ret == QMessageBox::No) { return; } - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - if (m_map_mode != Normal_MapMode) + if (m_map_mode != Normal_MapMode) { return; + } mapProxy->deleteAll(); - - } +} void OPMapGadgetWidget::onHomeMagicWaypointAct_triggered() @@ -1891,22 +1996,24 @@ void OPMapGadgetWidget::onHomeMagicWaypointAct_triggered() void OPMapGadgetWidget::onShowSafeAreaAct_toggled(bool show) { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - m_map->Home->SetShowSafeArea(show); // show the safe area + m_map->Home->SetShowSafeArea(show); // show the safe area m_map->Home->SetToggleRefresh(true); m_map->Home->RefreshPos(); } void OPMapGadgetWidget::onSafeAreaActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) + if (!m_widget || !m_map || !action) { return; + } int radius = action->data().toInt(); - m_map->Home->SetSafeArea(radius); // set the radius (meters) + m_map->Home->SetSafeArea(radius); // set the radius (meters) m_map->Home->RefreshPos(); // move the magic waypoint if need be to keep it within the safe area around the home position @@ -1914,15 +2021,17 @@ void OPMapGadgetWidget::onSafeAreaActGroup_triggered(QAction *action) } /** -* move the magic waypoint to the home position -**/ + * move the magic waypoint to the home position + **/ void OPMapGadgetWidget::homeMagicWaypoint() { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - if (m_map_mode != MagicWaypoint_MapMode) + if (m_map_mode != MagicWaypoint_MapMode) { return; + } magicWayPoint->SetCoord(m_home_position.coord); } @@ -1932,11 +2041,13 @@ void OPMapGadgetWidget::homeMagicWaypoint() void OPMapGadgetWidget::moveToMagicWaypointPosition() { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - if (m_map_mode != MagicWaypoint_MapMode) + if (m_map_mode != MagicWaypoint_MapMode) { return; + } } // ************************************************************************************* @@ -1955,9 +2066,9 @@ void OPMapGadgetWidget::showMagicWaypointControls() m_widget->toolButtonHomeWaypoint->setVisible(true); #if defined(allow_manual_home_location_move) - m_widget->toolButtonMoveToWP->setVisible(true); + m_widget->toolButtonMoveToWP->setVisible(true); #else - m_widget->toolButtonMoveToWP->setVisible(false); + m_widget->toolButtonMoveToWP->setVisible(false); #endif } @@ -1966,7 +2077,6 @@ void OPMapGadgetWidget::showMagicWaypointControls() void OPMapGadgetWidget::keepMagicWaypointWithInSafeArea() { - // calcute the bearing and distance from the home position to the magic waypoint double dist = distance(m_home_position.coord, magicWayPoint->Coord()); double bear = bearing(m_home_position.coord, magicWayPoint->Coord()); @@ -1974,14 +2084,16 @@ void OPMapGadgetWidget::keepMagicWaypointWithInSafeArea() // get the maximum safe distance - in kilometers double boundry_dist = (double)m_map->Home->SafeArea() / 1000; - if (dist > boundry_dist) dist = boundry_dist; + if (dist > boundry_dist) { + dist = boundry_dist; + } // move the magic waypoint; - if (m_map_mode == MagicWaypoint_MapMode) - { // move the on-screen waypoint - if (magicWayPoint) + if (m_map_mode == MagicWaypoint_MapMode) { // move the on-screen waypoint + if (magicWayPoint) { magicWayPoint->SetCoord(destPoint(m_home_position.coord, bear, dist)); + } } } @@ -1996,7 +2108,7 @@ double OPMapGadgetWidget::distance(internals::PointLatLng from, internals::Point double lat2 = to.Lat() * deg_to_rad; double lon2 = to.Lng() * deg_to_rad; - return (acos(sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(lon2 - lon1)) * earth_mean_radius); + return acos(sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(lon2 - lon1)) * earth_mean_radius; // *********************** } @@ -2012,16 +2124,20 @@ double OPMapGadgetWidget::bearing(internals::PointLatLng from, internals::PointL double lat2 = to.Lat() * deg_to_rad; double lon2 = to.Lng() * deg_to_rad; -// double delta_lat = lat2 - lat1; +// double delta_lat = lat2 - lat1; double delta_lon = lon2 - lon1; - double y = sin(delta_lon) * cos(lat2); - double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(delta_lon); + double y = sin(delta_lon) * cos(lat2); + double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(delta_lon); double bear = atan2(y, x) * rad_to_deg; bear += 360; - while (bear < 0) bear += 360; - while (bear >= 360) bear -= 360; + while (bear < 0) { + bear += 360; + } + while (bear >= 360) { + bear -= 360; + } return bear; } @@ -2036,7 +2152,7 @@ internals::PointLatLng OPMapGadgetWidget::destPoint(internals::PointLatLng sourc bear *= deg_to_rad; - double ad = dist / earth_mean_radius; + double ad = dist / earth_mean_radius; double lat2 = asin(sin(lat1) * cos(ad) + cos(lat1) * sin(ad) * cos(bear)); double lon2 = lon1 + atan2(sin(bear) * sin(ad) * cos(lat1), cos(ad) - sin(lat1) * sin(lat2)); @@ -2072,52 +2188,68 @@ bool OPMapGadgetWidget::getUAVPosition(double &latitude, double &longitude, doub Utils::CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA); - latitude = LLA[0]; + latitude = LLA[0]; longitude = LLA[1]; - altitude = LLA[2]; + altitude = LLA[2]; - if (latitude != latitude) latitude = 0; // nan detection - else if (latitude > 90) latitude = 90; - else if (latitude < -90) latitude = -90; + if (latitude != latitude) { + latitude = 0; // nan detection + } else if (latitude > 90) { + latitude = 90; + } else if (latitude < -90) { + latitude = -90; + } - if (longitude != longitude) longitude = 0; // nan detection - else if (longitude > 180) longitude = 180; - else if (longitude < -180) longitude = -180; - - if (altitude != altitude) altitude = 0; // nan detection + if (longitude != longitude) { + longitude = 0; // nan detection + } else if (longitude > 180) { + longitude = 180; + } else if (longitude < -180) { + longitude = -180; + } + if (altitude != altitude) { + altitude = 0; // nan detection + } return true; } double OPMapGadgetWidget::getUAV_Yaw() { - if (!obm) - return 0; + if (!obm) { + return 0; + } - UAVObject *obj = dynamic_cast(obm->getObject(QString("AttitudeActual"))); - double yaw = obj->getField(QString("Yaw"))->getDouble(); + UAVObject *obj = dynamic_cast(obm->getObject(QString("AttitudeActual"))); + double yaw = obj->getField(QString("Yaw"))->getDouble(); - if (yaw != yaw) yaw = 0; // nan detection + if (yaw != yaw) { + yaw = 0; // nan detection + } + while (yaw < 0) { + yaw += 360; + } + while (yaw >= 360) { + yaw -= 360; + } - while (yaw < 0) yaw += 360; - while (yaw >= 360) yaw -= 360; - - return yaw; + return yaw; } bool OPMapGadgetWidget::getGPSPosition(double &latitude, double &longitude, double &altitude) { - double LLA[3]; + double LLA[3]; - if (!obum) - return false; + if (!obum) { + return false; + } - if (obum->getGPSPosition(LLA) < 0) - return false; // error - - latitude = LLA[0]; - longitude = LLA[1]; - altitude = LLA[2]; + if (obum->getGPSPosition(LLA) < 0) { + return false; // error + } + latitude = LLA[0]; + longitude = LLA[1]; + altitude = LLA[2]; return true; } @@ -2126,25 +2258,20 @@ bool OPMapGadgetWidget::getGPSPosition(double &latitude, double &longitude, doub void OPMapGadgetWidget::setMapFollowingMode() { - if (!m_widget || !m_map) + if (!m_widget || !m_map) { return; + } - if (!followUAVpositionAct->isChecked()) - { + if (!followUAVpositionAct->isChecked()) { m_map->UAV->SetMapFollowType(UAVMapFollowType::None); - m_map->SetRotate(0); // reset map rotation to 0deg - } - else - if (!followUAVheadingAct->isChecked()) - { + m_map->SetRotate(0); // reset map rotation to 0deg + } else if (!followUAVheadingAct->isChecked()) { m_map->UAV->SetMapFollowType(UAVMapFollowType::CenterMap); - m_map->SetRotate(0); // reset map rotation to 0deg - } - else - { - m_map->UAV->SetMapFollowType(UAVMapFollowType::CenterMap); // the map library won't let you reset the uav rotation if it's already in rotate mode + m_map->SetRotate(0); // reset map rotation to 0deg + } else { + m_map->UAV->SetMapFollowType(UAVMapFollowType::CenterMap); // the map library won't let you reset the uav rotation if it's already in rotate mode - m_map->UAV->SetUAVHeading(0); // reset the UAV heading to 0deg + m_map->UAV->SetUAVHeading(0); // reset the UAV heading to 0deg m_map->UAV->SetMapFollowType(UAVMapFollowType::CenterAndRotateMap); } } @@ -2154,11 +2281,12 @@ void OPMapGadgetWidget::setMapFollowingMode() bool OPMapGadgetWidget::setHomeLocationObject() { - if (!obum) - return false; + if (!obum) { + return false; + } - double LLA[3] = {m_home_position.coord.Lat(), m_home_position.coord.Lng(), m_home_position.altitude}; - return (obum->setHomeLocation(LLA, true) >= 0); + double LLA[3] = { m_home_position.coord.Lat(), m_home_position.coord.Lng(), m_home_position.altitude }; + return obum->setHomeLocation(LLA, true) >= 0; } // ************************************************************************************* @@ -2172,32 +2300,31 @@ void OPMapGadgetWidget::on_tbFind_clicked() { QPalette pal = m_widget->leFind->palette(); - int result=m_map->SetCurrentPositionByKeywords(m_widget->leFind->text()); - if(result==core::GeoCoderStatusCode::G_GEO_SUCCESS) - { - pal.setColor( m_widget->leFind->backgroundRole(), Qt::green); + int result = m_map->SetCurrentPositionByKeywords(m_widget->leFind->text()); + + if (result == core::GeoCoderStatusCode::G_GEO_SUCCESS) { + pal.setColor(m_widget->leFind->backgroundRole(), Qt::green); m_widget->leFind->setPalette(pal); m_map->SetZoom(12); - } - else - { - pal.setColor( m_widget->leFind->backgroundRole(), Qt::red); + } else { + pal.setColor(m_widget->leFind->backgroundRole(), Qt::red); m_widget->leFind->setPalette(pal); } } void OPMapGadgetWidget::onHomeDoubleClick(HomeItem *) { - new homeEditor(m_map->Home,this); + new homeEditor(m_map->Home, this); } void OPMapGadgetWidget::onOverlayOpacityActGroup_triggered(QAction *action) { - if (!m_widget || !m_map || !action) + if (!m_widget || !m_map || !action) { return; + } - m_map->setOverlayOpacity(action->data().toReal()/100); - emit overlayOpacityChanged(action->data().toReal()/100); + m_map->setOverlayOpacity(action->data().toReal() / 100); + emit overlayOpacityChanged(action->data().toReal() / 100); } void OPMapGadgetWidget::on_leFind_returnPressed() diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h index 78a2313fb..7de84fce2 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -64,17 +64,15 @@ // ****************************************************** -namespace Ui -{ - class OPMap_Widget; +namespace Ui { +class OPMap_Widget; } using namespace mapcontrol; // ****************************************************** -typedef struct t_home -{ +typedef struct t_home { internals::PointLatLng coord; double altitude; bool locked; @@ -83,23 +81,22 @@ typedef struct t_home // ****************************************************** enum opMapModeType { Normal_MapMode = 0, - MagicWaypoint_MapMode = 1}; + MagicWaypoint_MapMode = 1 }; // ****************************************************** -class OPMapGadgetWidget : public QWidget -{ +class OPMapGadgetWidget : public QWidget { Q_OBJECT public: OPMapGadgetWidget(QWidget *parent = 0); - ~OPMapGadgetWidget(); + ~OPMapGadgetWidget(); /** - * @brief public functions - * - * @param - */ + * @brief public functions + * + * @param + */ void setHome(QPointF pos); void setHome(internals::PointLatLng pos_lat_lon, double altitude); void goHome(); @@ -112,13 +109,13 @@ public: void setUseMemoryCache(bool useMemoryCache); void setCacheLocation(QString cacheLocation); void setMapMode(opMapModeType mode); - void SetUavPic(QString UAVPic); + void SetUavPic(QString UAVPic); void setMaxUpdateRate(int update_rate); void setHomePosition(QPointF pos); void setOverlayOpacity(qreal value); bool getGPSPosition(double &latitude, double &longitude, double &altitude); signals: - void defaultLocationAndZoomChanged(double lng,double lat,double zoom); + void defaultLocationAndZoomChanged(double lng, double lat, double zoom); void overlayOpacityChanged(qreal); public slots: @@ -141,10 +138,10 @@ private slots: void zoomOut(); /** - * @brief signals received from the various map plug-in widget user controls - * - * Some are currently disabled for the v1.0 plugin version. - */ + * @brief signals received from the various map plug-in widget user controls + * + * Some are currently disabled for the v1.0 plugin version. + */ void on_toolButtonZoomM_clicked(); void on_toolButtonZoomP_clicked(); void on_toolButtonMapHome_clicked(); @@ -157,17 +154,17 @@ private slots: void on_toolButtonMoveToWP_clicked(); /** - * @brief signals received from the map object - */ - void zoomChanged(double zoomt,double zoom, double zoomd); + * @brief signals received from the map object + */ + void zoomChanged(double zoomt, double zoom, double zoomd); void OnCurrentPositionChanged(internals::PointLatLng point); void OnTileLoadComplete(); void OnTileLoadStart(); void OnTilesStillToLoad(int number); /** - * @brief mouse right click context menu signals - */ + * @brief mouse right click context menu signals + */ void onReloadAct_triggered(); void onRipAct_triggered(); void onCopyMouseLatLonToClipAct_triggered(); @@ -207,43 +204,43 @@ private slots: void onClearUAVtrailAct_triggered(); void onUAVTrailTimeActGroup_triggered(QAction *action); void onUAVTrailDistanceActGroup_triggered(QAction *action); - void onMaxUpdateRateActGroup_triggered(QAction *action); + void onMaxUpdateRateActGroup_triggered(QAction *action); void onChangeDefaultLocalAndZoom(); void on_tbFind_clicked(); - void onHomeDoubleClick(HomeItem*); + void onHomeDoubleClick(HomeItem *); void onOverlayOpacityActGroup_triggered(QAction *action); void on_leFind_returnPressed(); private: - int m_min_zoom; - int m_max_zoom; - double m_heading; // uav heading - internals::PointLatLng m_mouse_lat_lon; - internals::PointLatLng m_context_menu_lat_lon; - int m_prev_tile_number; + int m_min_zoom; + int m_max_zoom; + double m_heading; // uav heading + internals::PointLatLng m_mouse_lat_lon; + internals::PointLatLng m_context_menu_lat_lon; + int m_prev_tile_number; opMapModeType m_map_mode; - int m_maxUpdateRate; - t_home m_home_position; - QStringList findPlaceWordList; + int m_maxUpdateRate; + t_home m_home_position; + QStringList findPlaceWordList; QCompleter *findPlaceCompleter; QTimer *m_updateTimer; QTimer *m_statusUpdateTimer; Ui::OPMap_Widget *m_widget; mapcontrol::OPMapWidget *m_map; - ExtensionSystem::PluginManager *pm; - UAVObjectManager *obm; - UAVObjectUtilManager *obum; + ExtensionSystem::PluginManager *pm; + UAVObjectManager *obm; + UAVObjectUtilManager *obum; QPointer waypoint_edit_dialog; QStandardItemModel wayPoint_treeView_model; mapcontrol::WayPointItem *m_mouse_waypoint; QPointer UAVProxy; QMutex m_map_mutex; - bool m_telemetry_connected; + bool m_telemetry_connected; QAction *closeAct1; QAction *closeAct2; QAction *reloadAct; QAction *ripAct; - QAction *copyMouseLatLonToClipAct; + QAction *copyMouseLatLonToClipAct; QAction *copyMouseLatToClipAct; QAction *copyMouseLonToClipAct; QAction *showCompassAct; @@ -293,11 +290,11 @@ private: QList zoomAct; QList overlayOpacityAct; - QActionGroup *maxUpdateRateActGroup; - QList maxUpdateRateAct; + QActionGroup *maxUpdateRateActGroup; + QList maxUpdateRateAct; void createActions(); - void homeMagicWaypoint(); + void homeMagicWaypoint(); void moveToMagicWaypointPosition(); void hideMagicWaypointControls(); void showMagicWaypointControls(); @@ -307,15 +304,15 @@ private: double bearing(internals::PointLatLng from, internals::PointLatLng to); internals::PointLatLng destPoint(internals::PointLatLng source, double bear, double dist); - bool getUAVPosition(double &latitude, double &longitude, double &altitude); + bool getUAVPosition(double &latitude, double &longitude, double &altitude); double getUAV_Yaw(); void setMapFollowingMode(); - bool setHomeLocationObject(); + bool setHomeLocationObject(); QMenu contextMenu; internals::PointLatLng lastLatLngMouse; - WayPointItem * magicWayPoint; + WayPointItem *magicWayPoint; QPointer model; QPointer table; diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapplugin.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapplugin.cpp index df155f0ac..38fe1b3a9 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapplugin.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,32 +32,32 @@ OPMapPlugin::OPMapPlugin() { - // Do nothing + // Do nothing } OPMapPlugin::~OPMapPlugin() { - // Do nothing + // Do nothing } -bool OPMapPlugin::initialize(const QStringList& args, QString *errMsg) +bool OPMapPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); + Q_UNUSED(args); + Q_UNUSED(errMsg); - mf = new OPMapGadgetFactory(this); - addAutoReleasedObject(mf); + mf = new OPMapGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void OPMapPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void OPMapPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(OPMapPlugin) diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapplugin.h b/ground/openpilotgcs/src/plugins/opmap/opmapplugin.h index 07c41b533..263e4bcd6 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapplugin.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ - * @brief The OpenPilot Map plugin + * @brief The OpenPilot Map plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class OPMapGadgetFactory; -class OPMapPlugin : public ExtensionSystem::IPlugin -{ +class OPMapPlugin : public ExtensionSystem::IPlugin { public: OPMapPlugin(); ~OPMapPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - OPMapGadgetFactory *mf; + OPMapGadgetFactory *mf; }; #endif /* OPMAP_PLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp b/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp index a7c9425a9..80bba9ce6 100644 --- a/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp @@ -39,35 +39,38 @@ PathCompiler::PathCompiler(QObject *parent) : HomeLocation *homeLocation = NULL; /* To catch new waypoint UAVOs */ - connect(getObjectManager(), SIGNAL(newInstance(UAVObject*)), this, SLOT(doNewInstance(UAVObject*))); + connect(getObjectManager(), SIGNAL(newInstance(UAVObject *)), this, SLOT(doNewInstance(UAVObject *))); /* Connect the object updates */ int numWaypoints = getObjectManager()->getNumInstances(Waypoint::OBJID); for (int i = 0; i < numWaypoints; i++) { Waypoint *waypoint = Waypoint::GetInstance(getObjectManager(), i); Q_ASSERT(waypoint); - if(waypoint) - connect(waypoint, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doUpdateFromUAV(UAVObject*))); + if (waypoint) { + connect(waypoint, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doUpdateFromUAV(UAVObject *))); + } } homeLocation = HomeLocation::GetInstance(getObjectManager()); Q_ASSERT(homeLocation); - if(homeLocation) - connect(homeLocation, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doUpdateFromUAV(UAVObject*))); + if (homeLocation) { + connect(homeLocation, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doUpdateFromUAV(UAVObject *))); + } } /** - * Helper method to get the uavobjectamanger - */ -UAVObjectManager * PathCompiler::getObjectManager() + * Helper method to get the uavobjectamanger + */ +UAVObjectManager *PathCompiler::getObjectManager() { ExtensionSystem::PluginManager *pm = NULL; UAVObjectManager *objMngr = NULL; pm = ExtensionSystem::PluginManager::instance(); Q_ASSERT(pm); - if(pm) + if (pm) { objMngr = pm->getObject(); + } Q_ASSERT(objMngr); return objMngr; @@ -96,31 +99,33 @@ int PathCompiler::loadPath(QString filename) } /** - * Called whenever a new object instance is created so we can check - * if it's a waypoint and if so connect to it - * @param [in] obj The point to the object being created - */ -void PathCompiler::doNewInstance(UAVObject* obj) + * Called whenever a new object instance is created so we can check + * if it's a waypoint and if so connect to it + * @param [in] obj The point to the object being created + */ +void PathCompiler::doNewInstance(UAVObject *obj) { Q_ASSERT(obj); - if (!obj) + if (!obj) { return; + } - if (obj->getObjID() == Waypoint::OBJID) - connect(obj, SIGNAL(objectUpdated(UAVObject*)),this,SLOT(doUpdateFromUAV(UAVObject*))); + if (obj->getObjID() == Waypoint::OBJID) { + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doUpdateFromUAV(UAVObject *))); + } } /** - * add a waypoint - * @param waypoint the new waypoint to add - * @param position which position to insert it to, defaults to end - */ + * add a waypoint + * @param waypoint the new waypoint to add + * @param position which position to insert it to, defaults to end + */ void PathCompiler::doAddWaypoint(waypoint newWaypointInternal, int /*position*/) { /* TODO: If a waypoint is inserted not at the end shift them all by one and */ /* add the data there */ - UAVObjectManager *objManager = getObjectManager(); + UAVObjectManager *objManager = getObjectManager(); // Format the data from the map into a UAVO Waypoint::DataFields newWaypoint = InternalToUavo(newWaypointInternal); @@ -129,14 +134,16 @@ void PathCompiler::doAddWaypoint(waypoint newWaypointInternal, int /*position*/) // should add the waypoint immediately after that one int numWaypoints = objManager->getNumInstances(Waypoint::OBJID); int i; + for (i = 0; i < numWaypoints; i++) { Waypoint *waypoint = Waypoint::GetInstance(objManager, i); Q_ASSERT(waypoint); - if(waypoint == NULL) + if (waypoint == NULL) { return; + } Waypoint::DataFields waypointData = waypoint->getData(); - if(waypointData.Action == Waypoint::ACTION_STOP) { + if (waypointData.Action == Waypoint::ACTION_STOP) { waypointData.Action = Waypoint::ACTION_PATHTONEXT; waypoint->setData(waypointData); waypoint->updated(); @@ -152,7 +159,7 @@ void PathCompiler::doAddWaypoint(waypoint newWaypointInternal, int /*position*/) if (waypoint) { // Register a new waypoint instance quint32 newInstId = objManager->getNumInstances(waypoint->getObjID()); - waypoint->initialize(newInstId,waypoint->getMetaObject()); + waypoint->initialize(newInstId, waypoint->getMetaObject()); objManager->registerObject(waypoint); // Set the data in the new object @@ -170,14 +177,16 @@ void PathCompiler::doAddWaypoint(waypoint newWaypointInternal, int /*position*/) } /** - * Update waypoint - */ + * Update waypoint + */ void PathCompiler::doUpdateWaypoints(PathCompiler::waypoint changedWaypoint, int position) { int numWaypoints = getObjectManager()->getNumInstances(Waypoint::OBJID); + Q_ASSERT(position < numWaypoints); - if (position >= numWaypoints) + if (position >= numWaypoints) { return; + } Waypoint *waypointInst = Waypoint::GetInstance(getObjectManager(), position); Q_ASSERT(waypointInst); @@ -185,7 +194,7 @@ void PathCompiler::doUpdateWaypoints(PathCompiler::waypoint changedWaypoint, int // Mirror over the updated position. We don't just use the changedWaypoint // because things like action might need to be preserved Waypoint::DataFields changedWaypointUAVO = InternalToUavo(changedWaypoint); - Waypoint::DataFields oldWaypointUAVO = waypointInst->getData(); + Waypoint::DataFields oldWaypointUAVO = waypointInst->getData(); oldWaypointUAVO.Position[0] = changedWaypointUAVO.Position[0]; oldWaypointUAVO.Position[1] = changedWaypointUAVO.Position[1]; // Don't take the altitude from the map for now @@ -195,9 +204,9 @@ void PathCompiler::doUpdateWaypoints(PathCompiler::waypoint changedWaypoint, int } /** - * Delete a waypoint - * @param index which waypoint to delete - */ + * Delete a waypoint + * @param index which waypoint to delete + */ void PathCompiler::doDelWaypoint(int index) { // This method is awkward because there is no support @@ -207,20 +216,23 @@ void PathCompiler::doDelWaypoint(int index) UAVObjectManager *objManager = getObjectManager(); Waypoint *waypoint = Waypoint::GetInstance(objManager); + Q_ASSERT(waypoint); - if (!waypoint) + if (!waypoint) { return; + } int numWaypoints = objManager->getNumInstances(waypoint->getObjID()); for (int i = index; i < numWaypoints - 1; i++) { Waypoint *waypointDest = Waypoint::GetInstance(objManager, i); Q_ASSERT(waypointDest); - Waypoint *waypointSrc = Waypoint::GetInstance(objManager, i + 1); + Waypoint *waypointSrc = Waypoint::GetInstance(objManager, i + 1); Q_ASSERT(waypointSrc); - if (!waypointDest || !waypointSrc) + if (!waypointDest || !waypointSrc) { return; + } // Copy the data down an index Waypoint::DataFields waypoint = waypointSrc->getData(); @@ -243,14 +255,16 @@ void PathCompiler::doDelWaypoint(int index) } /** - * Delete all the waypoints - */ + * Delete all the waypoints + */ void PathCompiler::doDelAllWaypoints() { Waypoint *waypointObj = Waypoint::GetInstance(getObjectManager(), 0); + Q_ASSERT(waypointObj); - if (waypointObj == NULL) + if (waypointObj == NULL) { return; + } int numWaypoints = getObjectManager()->getNumInstances(waypointObj->getObjID()); for (int i = 0; i < numWaypoints; i++) { @@ -267,30 +281,34 @@ void PathCompiler::doDelAllWaypoints() } /** - * When the UAV waypoints change trigger the pathcompiler to - * get the latest version and then update the visualization - */ + * When the UAV waypoints change trigger the pathcompiler to + * get the latest version and then update the visualization + */ void PathCompiler::doUpdateFromUAV(UAVObject *obj) { UAVObjectManager *objManager = getObjectManager(); - if (!objManager) + + if (!objManager) { return; + } Waypoint *waypointObj = Waypoint::GetInstance(getObjectManager()); Q_ASSERT(waypointObj); - if (waypointObj == NULL) + if (waypointObj == NULL) { return; + } /* Get all the waypoints from the UAVO and create a representation for the visualization */ QList waypoints; waypoints.clear(); int numWaypoints = objManager->getNumInstances(waypointObj->getObjID()); - bool stopped = false; + bool stopped = false; for (int i = 0; i < numWaypoints && !stopped; i++) { Waypoint *waypoint = Waypoint::GetInstance(objManager, i); Q_ASSERT(waypoint); - if(waypoint == NULL) + if (waypoint == NULL) { return; + } Waypoint::DataFields waypointData = waypoint->getData(); waypoints.append(UavoToInternal(waypointData)); @@ -308,10 +326,10 @@ void PathCompiler::doUpdateFromUAV(UAVObject *obj) } /** - * Convert a UAVO waypoint to the local structure - * @param uavo The UAVO data representation - * @return The waypoint structure for visualization - */ + * Convert a UAVO waypoint to the local structure + * @param uavo The UAVO data representation + * @return The waypoint structure for visualization + */ struct PathCompiler::waypoint PathCompiler::UavoToInternal(Waypoint::DataFields uavo) { double homeLLA[3]; @@ -320,30 +338,32 @@ struct PathCompiler::waypoint PathCompiler::UavoToInternal(Waypoint::DataFields waypoint internalWaypoint; HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); + Q_ASSERT(homeLocation); - if (homeLocation == NULL) + if (homeLocation == NULL) { return internalWaypoint; + } HomeLocation::DataFields homeLocationData = homeLocation->getData(); homeLLA[0] = homeLocationData.Latitude / 10e6; homeLLA[1] = homeLocationData.Longitude / 10e6; homeLLA[2] = homeLocationData.Altitude; - NED[0] = uavo.Position[Waypoint::POSITION_NORTH]; - NED[1] = uavo.Position[Waypoint::POSITION_EAST]; - NED[2] = uavo.Position[Waypoint::POSITION_DOWN]; + NED[0] = uavo.Position[Waypoint::POSITION_NORTH]; + NED[1] = uavo.Position[Waypoint::POSITION_EAST]; + NED[2] = uavo.Position[Waypoint::POSITION_DOWN]; Utils::CoordinateConversions().GetLLA(homeLLA, NED, LLA); - internalWaypoint.latitude = LLA[0]; + internalWaypoint.latitude = LLA[0]; internalWaypoint.longitude = LLA[1]; - internalWaypoint.altitude = LLA[2]; + internalWaypoint.altitude = LLA[2]; return internalWaypoint; } /** - * Convert a UAVO waypoint to the local structure - * @param internal The internal structure type - * @returns The waypoint UAVO data structure - */ + * Convert a UAVO waypoint to the local structure + * @param internal The internal structure type + * @returns The waypoint UAVO data structure + */ Waypoint::DataFields PathCompiler::InternalToUavo(waypoint internal) { Waypoint::DataFields uavo; @@ -353,31 +373,32 @@ Waypoint::DataFields PathCompiler::InternalToUavo(waypoint internal) double NED[3]; HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); + Q_ASSERT(homeLocation); - if (homeLocation == NULL) + if (homeLocation == NULL) { return uavo; + } HomeLocation::DataFields homeLocationData = homeLocation->getData(); homeLLA[0] = homeLocationData.Latitude / 10e6; homeLLA[1] = homeLocationData.Longitude / 10e6; homeLLA[2] = homeLocationData.Altitude; // TODO: Give the point a concept of altitude - LLA[0] = internal.latitude; - LLA[1] = internal.longitude; - LLA[2] = internal.altitude; + LLA[0] = internal.latitude; + LLA[1] = internal.longitude; + LLA[2] = internal.altitude; Utils::CoordinateConversions().GetNED(homeLLA, LLA, NED); uavo.Position[Waypoint::POSITION_NORTH] = NED[0]; - uavo.Position[Waypoint::POSITION_EAST] = NED[1]; - uavo.Position[Waypoint::POSITION_DOWN] = NED[2]; + uavo.Position[Waypoint::POSITION_EAST] = NED[1]; + uavo.Position[Waypoint::POSITION_DOWN] = NED[2]; uavo.Action = Waypoint::ACTION_PATHTONEXT; uavo.Velocity[Waypoint::VELOCITY_NORTH] = 5; - uavo.Velocity[Waypoint::VELOCITY_EAST] = 0; - uavo.Velocity[Waypoint::VELOCITY_DOWN] = 0; + uavo.Velocity[Waypoint::VELOCITY_EAST] = 0; + uavo.Velocity[Waypoint::VELOCITY_DOWN] = 0; return uavo; } - diff --git a/ground/openpilotgcs/src/plugins/opmap/pathcompiler.h b/ground/openpilotgcs/src/plugins/opmap/pathcompiler.h index ed721794c..655df9297 100644 --- a/ground/openpilotgcs/src/plugins/opmap/pathcompiler.h +++ b/ground/openpilotgcs/src/plugins/opmap/pathcompiler.h @@ -36,41 +36,41 @@ // factory? static variables? /** - * This class is a two way adapter between a visualization of a path and the - * UAVObject representation on the flight controller. It also can support multiple - * ways of converting a path from what the user clicked to the underlying representation - * to achieve the desired end flight trajectory - * - * So the chain of data for the map lib is: - * FC <-> PathCompiler <-> OPMapGadget <-> OPMapLib - * - * The goal is that PathCompiler be as state free as is possible. Eventually for more - * complicated path compilation this will probably not be achievable. That means it - * should not cache a copy of waypoints locally if that can be avoided (i.e. it should - * refer directly to what is stored on the FC). - * - * For the visualization to have the ability to manipulate the path though it needs to - * be able to map unambiguously from the graphical items to the internal waypoints. It - * must cache a lookup from the graphical item to the index from this tool. - */ -class PathCompiler : public QObject -{ + * This class is a two way adapter between a visualization of a path and the + * UAVObject representation on the flight controller. It also can support multiple + * ways of converting a path from what the user clicked to the underlying representation + * to achieve the desired end flight trajectory + * + * So the chain of data for the map lib is: + * FC <-> PathCompiler <-> OPMapGadget <-> OPMapLib + * + * The goal is that PathCompiler be as state free as is possible. Eventually for more + * complicated path compilation this will probably not be achievable. That means it + * should not cache a copy of waypoints locally if that can be avoided (i.e. it should + * refer directly to what is stored on the FC). + * + * For the visualization to have the ability to manipulate the path though it needs to + * be able to map unambiguously from the graphical items to the internal waypoints. It + * must cache a lookup from the graphical item to the index from this tool. + */ +class PathCompiler : public QObject { Q_OBJECT public: explicit PathCompiler(QObject *parent = 0); - //! This method opens a dialog (if filename is null) and saves the path + // ! This method opens a dialog (if filename is null) and saves the path int savePath(QString filename = NULL); - //! This method opens a dialog (if filename is null) and loads the path + // ! This method opens a dialog (if filename is null) and loads the path int loadPath(QString filename = NULL); - //! Waypoint representation that is exchanged between visualization + // ! Waypoint representation that is exchanged between visualization class waypoint { - public: +public: waypoint() {} - const bool operator==(const waypoint other) { + const bool operator==(const waypoint other) + { return (other.latitude == latitude) && (other.longitude == longitude); } @@ -80,68 +80,68 @@ public: }; private: - //! Helper method to get uavobject manager - UAVObjectManager * getObjectManager(); + // ! Helper method to get uavobject manager + UAVObjectManager *getObjectManager(); - //! Convert a UAVO waypoint to the local structure + // ! Convert a UAVO waypoint to the local structure struct PathCompiler::waypoint UavoToInternal(Waypoint::DataFields); - //! Convert a UAVO waypoint to the local structure + // ! Convert a UAVO waypoint to the local structure Waypoint::DataFields InternalToUavo(waypoint); QList previousWaypoints; signals: /** - * Indicates something changed the waypoints and the map should - * update the display - */ + * Indicates something changed the waypoints and the map should + * update the display + */ void visualizationChanged(QList); public slots: /** - * These are slots that the visualization can call to manipulate the path. - * It is an important design detail that the visualiation _not_ attempt to maintain - * the list of waypoints itself. This starts the slippery of moving the path logic - * into the view. - */ + * These are slots that the visualization can call to manipulate the path. + * It is an important design detail that the visualiation _not_ attempt to maintain + * the list of waypoints itself. This starts the slippery of moving the path logic + * into the view. + */ /** - * Called when new instances are registered - */ - void doNewInstance(UAVObject*); + * Called when new instances are registered + */ + void doNewInstance(UAVObject *); /** - * add a waypoint - * @param waypoint the new waypoint to add - * @param position which position to insert it to, defaults to end - */ + * add a waypoint + * @param waypoint the new waypoint to add + * @param position which position to insert it to, defaults to end + */ void doAddWaypoint(waypoint, int position = -1); /** - * Update waypoint - */ + * Update waypoint + */ void doUpdateWaypoints(PathCompiler::waypoint, int position); /** - * Delete a waypoint - * @param index which waypoint to delete - */ + * Delete a waypoint + * @param index which waypoint to delete + */ void doDelWaypoint(int index); /** - * Delete all the waypoints - */ + * Delete all the waypoints + */ void doDelAllWaypoints(); public slots: /** - * These are slots that the UAV can call to update the path. - */ + * These are slots that the UAV can call to update the path. + */ /** - * When the UAV waypoints change trigger the pathcompiler to - * get the latest version and then update the visualization - */ + * When the UAV waypoints change trigger the pathcompiler to + * get the latest version and then update the visualization + */ void doUpdateFromUAV(UAVObject *); }; diff --git a/ground/openpilotgcs/src/plugins/opmap/pathplanner.cpp b/ground/openpilotgcs/src/plugins/opmap/pathplanner.cpp index aed5eed6e..dd02a6258 100644 --- a/ground/openpilotgcs/src/plugins/opmap/pathplanner.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/pathplanner.cpp @@ -33,7 +33,7 @@ pathPlanner::pathPlanner(QWidget *parent) : QWidget(parent), - ui(new Ui::pathPlannerUI),wid(NULL),myModel(NULL) + ui(new Ui::pathPlannerUI), wid(NULL), myModel(NULL) { ui->setupUi(this); } @@ -41,31 +41,31 @@ pathPlanner::pathPlanner(QWidget *parent) : pathPlanner::~pathPlanner() { delete ui; - if(wid) + if (wid) { delete wid; + } } -void pathPlanner::setModel(flightDataModel *model,QItemSelectionModel *selection) +void pathPlanner::setModel(flightDataModel *model, QItemSelectionModel *selection) { - myModel=model; + myModel = model; ui->tableView->setModel(model); ui->tableView->setSelectionModel(selection); ui->tableView->setSelectionBehavior(QAbstractItemView::SelectRows); ui->tableView->setItemDelegate(new MapDataDelegate(this)); - connect(model,SIGNAL(rowsInserted(const QModelIndex&,int,int)),this,SLOT(rowsInserted(const QModelIndex&,int,int))); - wid=new opmap_edit_waypoint_dialog(NULL,model,selection); + connect(model, SIGNAL(rowsInserted(const QModelIndex &, int, int)), this, SLOT(rowsInserted(const QModelIndex &, int, int))); + wid = new opmap_edit_waypoint_dialog(NULL, model, selection); ui->tableView->resizeColumnsToContents(); } -void pathPlanner::rowsInserted ( const QModelIndex & parent, int start, int end ) +void pathPlanner::rowsInserted(const QModelIndex & parent, int start, int end) { Q_UNUSED(parent); - for(int x=start;xtableView->model()->index(x,flightDataModel::MODE); + for (int x = start; x < end + 1; x++) { + QModelIndex index = ui->tableView->model()->index(x, flightDataModel::MODE); ui->tableView->openPersistentEditor(index); - index=ui->tableView->model()->index(x,flightDataModel::CONDITION); + index = ui->tableView->model()->index(x, flightDataModel::CONDITION); ui->tableView->openPersistentEditor(index); - index=ui->tableView->model()->index(x,flightDataModel::COMMAND); + index = ui->tableView->model()->index(x, flightDataModel::COMMAND); ui->tableView->openPersistentEditor(index); ui->tableView->size().setHeight(10); } @@ -88,24 +88,27 @@ void pathPlanner::on_tbInsert_clicked() void pathPlanner::on_tbReadFromFile_clicked() { - if(!myModel) + if (!myModel) { return; + } QString fileName = QFileDialog::getOpenFileName(this, tr("Open File")); myModel->readFromFile(fileName); } void pathPlanner::on_tbSaveToFile_clicked() { - if(!myModel) + if (!myModel) { return; + } QString fileName = QFileDialog::getSaveFileName(this, tr("Save File")); myModel->writeToFile(fileName); } void pathPlanner::on_tbDetails_clicked() { - if(wid) - wid->show(); + if (wid) { + wid->show(); + } } void pathPlanner::on_tbSendToUAV_clicked() diff --git a/ground/openpilotgcs/src/plugins/opmap/pathplanner.h b/ground/openpilotgcs/src/plugins/opmap/pathplanner.h index 274e0ae67..10ec86785 100644 --- a/ground/openpilotgcs/src/plugins/opmap/pathplanner.h +++ b/ground/openpilotgcs/src/plugins/opmap/pathplanner.h @@ -35,38 +35,37 @@ namespace Ui { class pathPlannerUI; } -class pathPlanner : public QWidget -{ +class pathPlanner : public QWidget { Q_OBJECT - + public: explicit pathPlanner(QWidget *parent = 0); ~pathPlanner(); - - void setModel(flightDataModel *model,QItemSelectionModel *selection); + + void setModel(flightDataModel *model, QItemSelectionModel *selection); private slots: - void rowsInserted ( const QModelIndex & parent, int start, int end ); + void rowsInserted(const QModelIndex & parent, int start, int end); - void on_tbAdd_clicked(); + void on_tbAdd_clicked(); - void on_tbDelete_clicked(); + void on_tbDelete_clicked(); - void on_tbInsert_clicked(); + void on_tbInsert_clicked(); - void on_tbReadFromFile_clicked(); + void on_tbReadFromFile_clicked(); - void on_tbSaveToFile_clicked(); + void on_tbSaveToFile_clicked(); - void on_tbDetails_clicked(); + void on_tbDetails_clicked(); - void on_tbSendToUAV_clicked(); + void on_tbSendToUAV_clicked(); - void on_tbFetchFromUAV_clicked(); + void on_tbFetchFromUAV_clicked(); private: Ui::pathPlannerUI *ui; - opmap_edit_waypoint_dialog * wid; - flightDataModel * myModel; + opmap_edit_waypoint_dialog *wid; + flightDataModel *myModel; signals: void sendPathPlanToUAV(); void receivePathPlanFromUAV(); diff --git a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp index fd44550a6..a6cd1b642 100644 --- a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp @@ -29,31 +29,35 @@ #include #include QWidget *MapDataDelegate::createEditor(QWidget *parent, - const QStyleOptionViewItem & option, - const QModelIndex & index) const + const QStyleOptionViewItem & option, + const QModelIndex & index) const { - int column=index.column(); - QComboBox * box; - switch(column) - { + int column = index.column(); + QComboBox *box; + + switch (column) { case flightDataModel::MODE: - box=new QComboBox(parent); - MapDataDelegate::loadComboBox(box,flightDataModel::MODE); + box = new QComboBox(parent); + MapDataDelegate::loadComboBox(box, flightDataModel::MODE); return box; + break; case flightDataModel::CONDITION: - box=new QComboBox(parent); - MapDataDelegate::loadComboBox(box,flightDataModel::CONDITION); + box = new QComboBox(parent); + MapDataDelegate::loadComboBox(box, flightDataModel::CONDITION); return box; + break; case flightDataModel::COMMAND: - box=new QComboBox(parent); - MapDataDelegate::loadComboBox(box,flightDataModel::COMMAND); + box = new QComboBox(parent); + MapDataDelegate::loadComboBox(box, flightDataModel::COMMAND); return box; + break; default: - return QItemDelegate::createEditor(parent,option,index); + return QItemDelegate::createEditor(parent, option, index); + break; } @@ -62,84 +66,84 @@ QWidget *MapDataDelegate::createEditor(QWidget *parent, } void MapDataDelegate::setEditorData(QWidget *editor, - const QModelIndex &index) const + const QModelIndex &index) const { - if(!index.isValid()) + if (!index.isValid()) { return; - QString className=editor->metaObject()->className(); + } + QString className = editor->metaObject()->className(); if (className.contains("QComboBox")) { int value = index.model()->data(index, Qt::EditRole).toInt(); - QComboBox *comboBox = static_cast(editor); - int x=comboBox->findData(value); - qDebug()<<"VALUE="<(editor); + int x = comboBox->findData(value); + qDebug() << "VALUE=" << x; comboBox->setCurrentIndex(x); - } - else + } else { QItemDelegate::setEditorData(editor, index); + } } void MapDataDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, - const QModelIndex &index) const + const QModelIndex &index) const { - QString className=editor->metaObject()->className(); + QString className = editor->metaObject()->className(); + if (className.contains("QComboBox")) { - QComboBox *comboBox = static_cast(editor); + QComboBox *comboBox = static_cast(editor); int value = comboBox->itemData(comboBox->currentIndex()).toInt(); model->setData(index, value, Qt::EditRole); + } else { + QItemDelegate::setModelData(editor, model, index); } - else - QItemDelegate::setModelData(editor,model,index); } void MapDataDelegate::updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &/* index */) const + const QStyleOptionViewItem &option, const QModelIndex & /* index */) const { editor->setGeometry(option.rect); } void MapDataDelegate::loadComboBox(QComboBox *combo, flightDataModel::pathPlanDataEnum type) { - switch(type) - { + switch (type) { case flightDataModel::MODE: - combo->addItem("Fly Direct",MODE_FLYENDPOINT); - combo->addItem("Fly Vector",MODE_FLYVECTOR); - combo->addItem("Fly Circle Right",MODE_FLYCIRCLERIGHT); - combo->addItem("Fly Circle Left",MODE_FLYCIRCLELEFT); + combo->addItem("Fly Direct", MODE_FLYENDPOINT); + combo->addItem("Fly Vector", MODE_FLYVECTOR); + combo->addItem("Fly Circle Right", MODE_FLYCIRCLERIGHT); + combo->addItem("Fly Circle Left", MODE_FLYCIRCLELEFT); - combo->addItem("Drive Direct",MODE_DRIVEENDPOINT); - combo->addItem("Drive Vector",MODE_DRIVEVECTOR); - combo->addItem("Drive Circle Right",MODE_DRIVECIRCLELEFT); - combo->addItem("Drive Circle Left",MODE_DRIVECIRCLERIGHT); + combo->addItem("Drive Direct", MODE_DRIVEENDPOINT); + combo->addItem("Drive Vector", MODE_DRIVEVECTOR); + combo->addItem("Drive Circle Right", MODE_DRIVECIRCLELEFT); + combo->addItem("Drive Circle Left", MODE_DRIVECIRCLERIGHT); - combo->addItem("Fixed Attitude",MODE_FIXEDATTITUDE); - combo->addItem("Set Accessory",MODE_SETACCESSORY); - combo->addItem("Disarm Alarm",MODE_DISARMALARM); + combo->addItem("Fixed Attitude", MODE_FIXEDATTITUDE); + combo->addItem("Set Accessory", MODE_SETACCESSORY); + combo->addItem("Disarm Alarm", MODE_DISARMALARM); break; case flightDataModel::CONDITION: - combo->addItem("None",ENDCONDITION_NONE); - combo->addItem("Timeout",ENDCONDITION_TIMEOUT); - combo->addItem("Distance to tgt",ENDCONDITION_DISTANCETOTARGET); - combo->addItem("Leg remaining",ENDCONDITION_LEGREMAINING); - combo->addItem("Below Error",ENDCONDITION_BELOWERROR); - combo->addItem("Above Altitude",ENDCONDITION_ABOVEALTITUDE); - combo->addItem("Above Speed",ENDCONDITION_ABOVESPEED); - combo->addItem("Pointing towards next",ENDCONDITION_POINTINGTOWARDSNEXT); - combo->addItem("Python script",ENDCONDITION_PYTHONSCRIPT); - combo->addItem("Immediate",ENDCONDITION_IMMEDIATE); + combo->addItem("None", ENDCONDITION_NONE); + combo->addItem("Timeout", ENDCONDITION_TIMEOUT); + combo->addItem("Distance to tgt", ENDCONDITION_DISTANCETOTARGET); + combo->addItem("Leg remaining", ENDCONDITION_LEGREMAINING); + combo->addItem("Below Error", ENDCONDITION_BELOWERROR); + combo->addItem("Above Altitude", ENDCONDITION_ABOVEALTITUDE); + combo->addItem("Above Speed", ENDCONDITION_ABOVESPEED); + combo->addItem("Pointing towards next", ENDCONDITION_POINTINGTOWARDSNEXT); + combo->addItem("Python script", ENDCONDITION_PYTHONSCRIPT); + combo->addItem("Immediate", ENDCONDITION_IMMEDIATE); break; case flightDataModel::COMMAND: - combo->addItem("On conditon next wp",COMMAND_ONCONDITIONNEXTWAYPOINT); - combo->addItem("On NOT conditon next wp",COMMAND_ONNOTCONDITIONNEXTWAYPOINT); - combo->addItem("On conditon jump wp",COMMAND_ONCONDITIONJUMPWAYPOINT); - combo->addItem("On NOT conditon jump wp",COMMAND_ONNOTCONDITIONJUMPWAYPOINT); - combo->addItem("On conditon jump wp else next wp",COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT); + combo->addItem("On conditon next wp", COMMAND_ONCONDITIONNEXTWAYPOINT); + combo->addItem("On NOT conditon next wp", COMMAND_ONNOTCONDITIONNEXTWAYPOINT); + combo->addItem("On conditon jump wp", COMMAND_ONCONDITIONJUMPWAYPOINT); + combo->addItem("On NOT conditon jump wp", COMMAND_ONNOTCONDITIONJUMPWAYPOINT); + combo->addItem("On conditon jump wp else next wp", COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT); break; default: break; } } -MapDataDelegate::MapDataDelegate(QObject *parent):QItemDelegate(parent) -{ -} +MapDataDelegate::MapDataDelegate(QObject *parent) : QItemDelegate(parent) +{} diff --git a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h index 53aac4887..2b824c979 100644 --- a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h +++ b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h @@ -32,34 +32,33 @@ #include "flightdatamodel.h" -class MapDataDelegate : public QItemDelegate - { - Q_OBJECT +class MapDataDelegate : public QItemDelegate { + Q_OBJECT - public: - typedef enum { MODE_FLYENDPOINT=0, MODE_FLYVECTOR=1, MODE_FLYCIRCLERIGHT=2, MODE_FLYCIRCLELEFT=3, - MODE_DRIVEENDPOINT=4, MODE_DRIVEVECTOR=5, MODE_DRIVECIRCLELEFT=6, MODE_DRIVECIRCLERIGHT=7, - MODE_FIXEDATTITUDE=8, MODE_SETACCESSORY=9, MODE_DISARMALARM=10 } ModeOptions; - typedef enum { ENDCONDITION_NONE=0, ENDCONDITION_TIMEOUT=1, ENDCONDITION_DISTANCETOTARGET=2, - ENDCONDITION_LEGREMAINING=3, ENDCONDITION_BELOWERROR=4, ENDCONDITION_ABOVEALTITUDE=5, - ENDCONDITION_ABOVESPEED=6, ENDCONDITION_POINTINGTOWARDSNEXT=7, ENDCONDITION_PYTHONSCRIPT=8, - ENDCONDITION_IMMEDIATE=9 } EndConditionOptions; - typedef enum { COMMAND_ONCONDITIONNEXTWAYPOINT=0, COMMAND_ONNOTCONDITIONNEXTWAYPOINT=1, - COMMAND_ONCONDITIONJUMPWAYPOINT=2, COMMAND_ONNOTCONDITIONJUMPWAYPOINT=3, - COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT=4 } CommandOptions; +public: + typedef enum { MODE_FLYENDPOINT = 0, MODE_FLYVECTOR = 1, MODE_FLYCIRCLERIGHT = 2, MODE_FLYCIRCLELEFT = 3, + MODE_DRIVEENDPOINT = 4, MODE_DRIVEVECTOR = 5, MODE_DRIVECIRCLELEFT = 6, MODE_DRIVECIRCLERIGHT = 7, + MODE_FIXEDATTITUDE = 8, MODE_SETACCESSORY = 9, MODE_DISARMALARM = 10 } ModeOptions; + typedef enum { ENDCONDITION_NONE = 0, ENDCONDITION_TIMEOUT = 1, ENDCONDITION_DISTANCETOTARGET = 2, + ENDCONDITION_LEGREMAINING = 3, ENDCONDITION_BELOWERROR = 4, ENDCONDITION_ABOVEALTITUDE = 5, + ENDCONDITION_ABOVESPEED = 6, ENDCONDITION_POINTINGTOWARDSNEXT = 7, ENDCONDITION_PYTHONSCRIPT = 8, + ENDCONDITION_IMMEDIATE = 9 } EndConditionOptions; + typedef enum { COMMAND_ONCONDITIONNEXTWAYPOINT = 0, COMMAND_ONNOTCONDITIONNEXTWAYPOINT = 1, + COMMAND_ONCONDITIONJUMPWAYPOINT = 2, COMMAND_ONNOTCONDITIONJUMPWAYPOINT = 3, + COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT = 4 } CommandOptions; - MapDataDelegate(QObject *parent = 0); + MapDataDelegate(QObject *parent = 0); - QWidget *createEditor(QWidget *parent, const QStyleOptionViewItem &option, - const QModelIndex &index) const; + QWidget *createEditor(QWidget *parent, const QStyleOptionViewItem &option, + const QModelIndex &index) const; - void setEditorData(QWidget *editor, const QModelIndex &index) const; - void setModelData(QWidget *editor, QAbstractItemModel *model, - const QModelIndex &index) const; + void setEditorData(QWidget *editor, const QModelIndex &index) const; + void setModelData(QWidget *editor, QAbstractItemModel *model, + const QModelIndex &index) const; - void updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &index) const; - static void loadComboBox(QComboBox * combo,flightDataModel::pathPlanDataEnum type); - }; + void updateEditorGeometry(QWidget *editor, + const QStyleOptionViewItem &option, const QModelIndex &index) const; + static void loadComboBox(QComboBox *combo, flightDataModel::pathPlanDataEnum type); +}; #endif // WIDGETDELEGATES_H diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.cpp index fbfb0d613..ccff62dd8 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.cpp @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgearthviewgadget.cpp -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgearthviewgadget.cpp + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -28,10 +28,9 @@ #include "osgearthviewgadgetconfiguration.h" OsgEarthviewGadget::OsgEarthviewGadget(QString classId, OsgEarthviewWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} OsgEarthviewGadget::~OsgEarthviewGadget() { @@ -39,12 +38,12 @@ OsgEarthviewGadget::~OsgEarthviewGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void OsgEarthviewGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void OsgEarthviewGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - OsgEarthviewGadgetConfiguration *m = qobject_cast(config); + OsgEarthviewGadgetConfiguration *m = qobject_cast(config); } diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.h b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.h index d5b8234c6..a02925c86 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.h +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadget.h @@ -1,13 +1,13 @@ /******************************************************************************** -* -* @file osgearthviewgadget.h -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * + * @file osgearthviewgadget.h + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -37,15 +37,17 @@ class OsgEarthviewWidget; using namespace Core; -class OsgEarthviewGadget : public Core::IUAVGadget -{ +class OsgEarthviewGadget : public Core::IUAVGadget { Q_OBJECT public: OsgEarthviewGadget(QString classId, OsgEarthviewWidget *widget, QWidget *parent = 0); ~OsgEarthviewGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: OsgEarthviewWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.cpp index 50126677e..5a3955ee2 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.cpp @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgearthviewgadgetconfiguration.cpp -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgearthviewgadgetconfiguration.cpp + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -30,13 +30,12 @@ * Loads a saved configuration or defaults if non exist. * */ -OsgEarthviewGadgetConfiguration::OsgEarthviewGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +OsgEarthviewGadgetConfiguration::OsgEarthviewGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent) { Q_UNUSED(qSettings); - //if a saved configuration exists load it - if(qSettings != 0) { - } + // if a saved configuration exists load it + if (qSettings != 0) {} } /** @@ -46,6 +45,7 @@ OsgEarthviewGadgetConfiguration::OsgEarthviewGadgetConfiguration(QString classId IUAVGadgetConfiguration *OsgEarthviewGadgetConfiguration::clone() { OsgEarthviewGadgetConfiguration *m = new OsgEarthviewGadgetConfiguration(this->classId()); + return m; } @@ -53,5 +53,4 @@ IUAVGadgetConfiguration *OsgEarthviewGadgetConfiguration::clone() * Saves a configuration. * */ -void OsgEarthviewGadgetConfiguration::saveConfig(QSettings* qSettings) const { -} +void OsgEarthviewGadgetConfiguration::saveConfig(QSettings *qSettings) const {} diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.h index 9ac03c3fa..02d2560d6 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetconfiguration.h @@ -1,13 +1,13 @@ /******************************************************************************** -* -* @file osgearthviewgadgetconfiguration.h -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * + * @file osgearthviewgadgetconfiguration.h + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -31,13 +31,12 @@ using namespace Core; -class OsgEarthviewGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT +class OsgEarthviewGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit OsgEarthviewGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit OsgEarthviewGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.cpp index 245455ca0..d01a2d5ab 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.cpp @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgearthviewgadgetfactory.cpp -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgearthviewgadgetfactory.cpp + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -30,29 +30,27 @@ #include OsgEarthviewGadgetFactory::OsgEarthviewGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("OsgEarthviewGadget"), - tr("Osg Earth View"), - parent) -{ -} + IUAVGadgetFactory(QString("OsgEarthviewGadget"), + tr("Osg Earth View"), + parent) +{} OsgEarthviewGadgetFactory::~OsgEarthviewGadgetFactory() -{ -} +{} -Core::IUAVGadget* OsgEarthviewGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *OsgEarthviewGadgetFactory::createGadget(QWidget *parent) { - OsgEarthviewWidget* gadgetWidget = new OsgEarthviewWidget(parent); + OsgEarthviewWidget *gadgetWidget = new OsgEarthviewWidget(parent); + return new OsgEarthviewGadget(QString("OsgEarthviewGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *OsgEarthviewGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *OsgEarthviewGadgetFactory::createConfiguration(QSettings *qSettings) { return new OsgEarthviewGadgetConfiguration(QString("OsgEarthviewGadget"), qSettings); } IOptionsPage *OsgEarthviewGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new OsgEarthviewGadgetOptionsPage(qobject_cast(config)); + return new OsgEarthviewGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.h b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.h index 5b6381c36..9557c8570 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OsgEarthview Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class OsgEarthviewGadgetFactory : public IUAVGadgetFactory -{ +class OsgEarthviewGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: OsgEarthviewGadgetFactory(QObject *parent = 0); ~OsgEarthviewGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.cpp index 181005f47..21d424e03 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.cpp @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgearthviewgadgetoptions.cpp -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgearthviewgadgetoptions.cpp + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -36,19 +36,17 @@ #include OsgEarthviewGadgetOptionsPage::OsgEarthviewGadgetOptionsPage(OsgEarthviewGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *OsgEarthviewGadgetOptionsPage::createPage(QWidget *parent) { - options_page = new Ui::OsgEarthviewGadgetOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget; - //main layout + // main layout options_page->setupUi(optionsPageWidget); return optionsPageWidget; @@ -61,11 +59,8 @@ QWidget *OsgEarthviewGadgetOptionsPage::createPage(QWidget *parent) * */ void OsgEarthviewGadgetOptionsPage::apply() -{ -} - +{} void OsgEarthviewGadgetOptionsPage::finish() -{ -} +{} diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.h index 856f5607d..7a77b391d 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewgadgetoptionspage.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -40,14 +40,13 @@ class IUAVGadgetConfiguration; class OsgEarthviewGadgetConfiguration; namespace Ui { - class OsgEarthviewGadgetOptionsPage; +class OsgEarthviewGadgetOptionsPage; } using namespace Core; -class OsgEarthviewGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class OsgEarthviewGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit OsgEarthviewGadgetOptionsPage(OsgEarthviewGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.cpp index 4f7fd7408..ea357544f 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.cpp @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgearthviewplugin.cpp -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgearthviewplugin.cpp + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -35,34 +35,33 @@ OsgEarthviewPlugin::OsgEarthviewPlugin() { - // Do nothing + // Do nothing } OsgEarthviewPlugin::~OsgEarthviewPlugin() { - // Do nothing + // Do nothing } -bool OsgEarthviewPlugin::initialize(const QStringList& args, QString *errMsg) +bool OsgEarthviewPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new OsgEarthviewGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new OsgEarthviewGadgetFactory(this); + addAutoReleasedObject(mf); - osgQt::initQtWindowingSystem(); + osgQt::initQtWindowingSystem(); - return true; + return true; } void OsgEarthviewPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void OsgEarthviewPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(OsgEarthviewPlugin) - diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.h b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.h index ced4dc089..5dafd81ef 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.h +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class OsgEarthviewGadgetFactory; -class OsgEarthviewPlugin : public ExtensionSystem::IPlugin -{ +class OsgEarthviewPlugin : public ExtensionSystem::IPlugin { public: - OsgEarthviewPlugin(); - ~OsgEarthviewPlugin(); + OsgEarthviewPlugin(); + ~OsgEarthviewPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - OsgEarthviewGadgetFactory *mf; + OsgEarthviewGadgetFactory *mf; }; #endif /* PFDPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp index 4d0b2cb80..c96ab3f77 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgearthviewwidget.cpp -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin Widget -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgearthviewwidget.cpp + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin Widget + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -101,26 +101,21 @@ using namespace Utils; OsgEarthviewWidget::OsgEarthviewWidget(QWidget *parent) : QWidget(parent) { - m_widget = new Ui_OsgEarthview(); m_widget->setupUi(this); /*viewWidget = new OsgViewerWidget(this); - viewWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + viewWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - setLayout(new QVBoxLayout()); - layout()->addWidget(viewWidget);*/ + setLayout(new QVBoxLayout()); + layout()->addWidget(viewWidget);*/ } OsgEarthviewWidget::~OsgEarthviewWidget() -{ -} +{} -void OsgEarthviewWidget::paintEvent( QPaintEvent* event ) -{ -} +void OsgEarthviewWidget::paintEvent(QPaintEvent *event) +{} void OsgEarthviewWidget::resizeEvent(QResizeEvent *event) -{ -} - +{} diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.h b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.h index be7545e29..d0eac2652 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.h +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.h @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgearthviewwidget.h -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgearthviewwidget.h + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -83,19 +83,18 @@ using namespace osgEarth::Annotation; class Ui_OsgEarthview; -class OsgEarthviewWidget : public QWidget -{ +class OsgEarthviewWidget : public QWidget { Q_OBJECT public: OsgEarthviewWidget(QWidget *parent = 0); - ~OsgEarthviewWidget(); + ~OsgEarthviewWidget(); public slots: protected: /* Protected methods */ - void paintEvent(QPaintEvent *event); - void resizeEvent(QResizeEvent *event); + void paintEvent(QPaintEvent *event); + void resizeEvent(QResizeEvent *event); OsgViewerWidget *viewWidget; Ui_OsgEarthview *m_widget; diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp index 89619f99b..2b2d11b73 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgviewerwidget.cpp -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin Widget -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgviewerwidget.cpp + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin Widget + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -108,19 +108,18 @@ OsgViewerWidget::OsgViewerWidget(QWidget *parent) : QWidget(parent) setThreadingModel(osgViewer::ViewerBase::CullThreadPerCameraDrawThreadPerContext); setAttribute(Qt::WA_PaintOnScreen, true); - osg::Group* root = new osg::Group; - osg::Node* earth = osgDB::readNodeFile("/Users/Cotton/Programming/osg/osgearth/tests/boston.earth"); - mapNode = osgEarth::MapNode::findMapNode( earth ); - if (!mapNode) - { - qDebug() <<"Uhoh"; + osg::Group *root = new osg::Group; + osg::Node *earth = osgDB::readNodeFile("/Users/Cotton/Programming/osg/osgearth/tests/boston.earth"); + mapNode = osgEarth::MapNode::findMapNode(earth); + if (!mapNode) { + qDebug() << "Uhoh"; } root->addChild(earth); - osg::Node* airplane = createAirplane(); + osg::Node *airplane = createAirplane(); uavPos = new osgEarth::Util::ObjectLocatorNode(mapNode->getMap()); - uavPos->getLocator()->setPosition( osg::Vec3d(-71.100549, 42.349273, 150) ); + uavPos->getLocator()->setPosition(osg::Vec3d(-71.100549, 42.349273, 150)); uavPos->addChild(airplane); root->addChild(uavPos); @@ -128,7 +127,7 @@ OsgViewerWidget::OsgViewerWidget(QWidget *parent) : QWidget(parent) osgUtil::Optimizer optimizer; optimizer.optimize(root); - QWidget* viewWidget = createViewWidget( createCamera(0,0,200,200,"Earth",false), root); + QWidget *viewWidget = createViewWidget(createCamera(0, 0, 200, 200, "Earth", false), root); viewWidget->show(); setLayout(new QVBoxLayout(this)); @@ -136,84 +135,86 @@ OsgViewerWidget::OsgViewerWidget(QWidget *parent) : QWidget(parent) layout()->addWidget(viewWidget); - connect( &_timer, SIGNAL(timeout()), this, SLOT(update()) ); - _timer.start( 10 ); + connect(&_timer, SIGNAL(timeout()), this, SLOT(update())); + _timer.start(10); } OsgViewerWidget::~OsgViewerWidget() +{} + +QWidget *OsgViewerWidget::createViewWidget(osg::Camera *camera, osg::Node *scene) { -} + osgViewer::View *view = new osgViewer::View; -QWidget* OsgViewerWidget::createViewWidget( osg::Camera* camera, osg::Node* scene ) -{ - osgViewer::View* view = new osgViewer::View; - view->setCamera( camera ); + view->setCamera(camera); - addView( view ); + addView(view); - view->setSceneData( scene ); - view->addEventHandler( new osgViewer::StatsHandler ); - view->getDatabasePager()->setDoPreCompile( true ); + view->setSceneData(scene); + view->addEventHandler(new osgViewer::StatsHandler); + view->getDatabasePager()->setDoPreCompile(true); manip = new EarthManipulator(); - view->setCameraManipulator( manip ); + view->setCameraManipulator(manip); -// osgGA::NodeTrackerManipulator *camTracker = new osgGA::NodeTrackerManipulator(); -// camTracker->setTrackNode(uavPos); -// camTracker->setMinimumDistance(0.0001f); -// camTracker->setDistance(0.001f); -// camTracker->setTrackerMode(osgGA::NodeTrackerManipulator::NODE_CENTER); -// view->setCameraManipulator(camTracker); +// osgGA::NodeTrackerManipulator *camTracker = new osgGA::NodeTrackerManipulator(); +// camTracker->setTrackNode(uavPos); +// camTracker->setMinimumDistance(0.0001f); +// camTracker->setDistance(0.001f); +// camTracker->setTrackerMode(osgGA::NodeTrackerManipulator::NODE_CENTER); +// view->setCameraManipulator(camTracker); - Grid* grid = new Grid(); - grid->setControl(0,0,new LabelControl("OpenPilot")); + Grid *grid = new Grid(); + grid->setControl(0, 0, new LabelControl("OpenPilot")); ControlCanvas::get(view, true)->addControl(grid); // zoom to a good startup position - manip->setViewpoint( Viewpoint(-71.100549, 42.349273, 0, 24.261, -21.6, 350.0), 5.0 ); - //manip->setViewpoint( Viewpoint(-71.100549, 42.349273, 0, 24.261, -81.6, 650.0), 5.0 ); - //manip->setHomeViewpoint(Viewpoint("Boston", osg::Vec3d(-71.0763, 42.34425, 0), 24.261, -21.6, 3450.0)); + manip->setViewpoint(Viewpoint(-71.100549, 42.349273, 0, 24.261, -21.6, 350.0), 5.0); + // manip->setViewpoint( Viewpoint(-71.100549, 42.349273, 0, 24.261, -81.6, 650.0), 5.0 ); + // manip->setHomeViewpoint(Viewpoint("Boston", osg::Vec3d(-71.0763, 42.34425, 0), 24.261, -21.6, 3450.0)); manip->setTetherNode(uavPos); - osgQt::GraphicsWindowQt* gw = dynamic_cast( camera->getGraphicsContext() ); + osgQt::GraphicsWindowQt *gw = dynamic_cast(camera->getGraphicsContext()); return gw ? gw->getGLWidget() : NULL; } -osg::Camera* OsgViewerWidget::createCamera( int x, int y, int w, int h, const std::string& name="", bool windowDecoration=false ) +osg::Camera *OsgViewerWidget::createCamera(int x, int y, int w, int h, const std::string & name = "", bool windowDecoration = false) { - osg::DisplaySettings* ds = osg::DisplaySettings::instance().get(); + osg::DisplaySettings *ds = osg::DisplaySettings::instance().get(); osg::ref_ptr traits = new osg::GraphicsContext::Traits; + traits->windowName = name; traits->windowDecoration = windowDecoration; - traits->x = x; - traits->y = y; - traits->width = w; - traits->height = h; + traits->x = x; + traits->y = y; + traits->width = w; + traits->height = h; traits->doubleBuffer = true; - traits->alpha = ds->getMinimumNumAlphaBits(); + traits->alpha = ds->getMinimumNumAlphaBits(); traits->stencil = ds->getMinimumNumStencilBits(); traits->sampleBuffers = ds->getMultiSamples(); traits->samples = ds->getNumMultiSamples(); osg::ref_ptr camera = new osg::Camera; - camera->setGraphicsContext( new osgQt::GraphicsWindowQt(traits.get()) ); + camera->setGraphicsContext(new osgQt::GraphicsWindowQt(traits.get())); - camera->setClearColor( osg::Vec4(0.2, 0.2, 0.6, 1.0) ); - camera->setViewport( new osg::Viewport(0, 0, traits->width, traits->height) ); + camera->setClearColor(osg::Vec4(0.2, 0.2, 0.6, 1.0)); + camera->setViewport(new osg::Viewport(0, 0, traits->width, traits->height)); camera->setProjectionMatrixAsPerspective( - 30.0f, static_cast(traits->width)/static_cast(traits->height), 1.0f, 10000.0f ); + 30.0f, static_cast(traits->width) / static_cast(traits->height), 1.0f, 10000.0f); return camera.release(); } -osg::Node* OsgViewerWidget::createAirplane() +osg::Node *OsgViewerWidget::createAirplane() { - osg::Group* model = new osg::Group; + osg::Group *model = new osg::Group; osg::Node *uav; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm); - UAVObjectManager * objMngr = pm->getObject(); + UAVObjectManager *objMngr = pm->getObject(); Q_ASSERT(objMngr); SystemSettings *systemSettingsObj = SystemSettings::GetInstance(objMngr); @@ -221,56 +222,57 @@ osg::Node* OsgViewerWidget::createAirplane() qDebug() << "Frame type:" << systemSettingsObj->getField("AirframeType")->getValue().toString(); // Get model that matches airframe type - switch(systemSettings.AirframeType) { + switch (systemSettings.AirframeType) { case SystemSettings::AIRFRAMETYPE_FIXEDWING: case SystemSettings::AIRFRAMETYPE_FIXEDWINGELEVON: - case SystemSettings::AIRFRAMETYPE_FIXEDWINGVTAIL: + case SystemSettings::AIRFRAMETYPE_FIXEDWINGVTAIL: uav = osgDB::readNodeFile("/Users/Cotton/Programming/OpenPilot/artwork/3D Model/planes/Easystar/easystar.3ds"); break; default: uav = osgDB::readNodeFile("/Users/Cotton/Programming/OpenPilot/artwork/3D Model/multi/joes_cnc/J14-QT_+.3DS"); } - if(uav) { + if (uav) { uavAttitudeAndScale = new osg::MatrixTransform(); - uavAttitudeAndScale->setMatrix(osg::Matrixd::scale(0.2e0,0.2e0,0.2e0)); + uavAttitudeAndScale->setMatrix(osg::Matrixd::scale(0.2e0, 0.2e0, 0.2e0)); // Apply a rotation so model is NED before any other rotations osg::MatrixTransform *rotateModelNED = new osg::MatrixTransform(); - rotateModelNED->setMatrix(osg::Matrixd::scale(0.05e0,0.05e0,0.05e0) * osg::Matrixd::rotate(M_PI, osg::Vec3d(0,0,1))); - rotateModelNED->addChild( uav ); + rotateModelNED->setMatrix(osg::Matrixd::scale(0.05e0, 0.05e0, 0.05e0) * osg::Matrixd::rotate(M_PI, osg::Vec3d(0, 0, 1))); + rotateModelNED->addChild(uav); - uavAttitudeAndScale->addChild( rotateModelNED ); + uavAttitudeAndScale->addChild(rotateModelNED); model->addChild(uavAttitudeAndScale); - } else + } else { qDebug() << "Bad model file"; + } return model; } -void OsgViewerWidget::paintEvent( QPaintEvent* event ) +void OsgViewerWidget::paintEvent(QPaintEvent *event) { Q_UNUSED(event); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * objMngr = pm->getObject(); + UAVObjectManager *objMngr = pm->getObject(); - PositionActual *positionActualObj = PositionActual::GetInstance(objMngr); + PositionActual *positionActualObj = PositionActual::GetInstance(objMngr); PositionActual::DataFields positionActual = positionActualObj->getData(); - double NED[3] = {positionActual.North, positionActual.East, positionActual.Down}; + double NED[3] = { positionActual.North, positionActual.East, positionActual.Down }; bool positionActualUpdate = true; - if(positionActualUpdate) { + if (positionActualUpdate) { HomeLocation *homeLocationObj = HomeLocation::GetInstance(objMngr); HomeLocation::DataFields homeLocation = homeLocationObj->getData(); - double homeLLA[3] = {homeLocation.Latitude / 10.0e6, homeLocation.Longitude / 10.0e6, homeLocation.Altitude}; + double homeLLA[3] = { homeLocation.Latitude / 10.0e6, homeLocation.Longitude / 10.0e6, homeLocation.Altitude }; double LLA[3]; - CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA); - uavPos->getLocator()->setPosition( osg::Vec3d(LLA[1], LLA[0], LLA[2]) ); // Note this takes longtitude first + CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA); + uavPos->getLocator()->setPosition(osg::Vec3d(LLA[1], LLA[0], LLA[2])); // Note this takes longtitude first } else { GPSPosition *gpsPosObj = GPSPosition::GetInstance(objMngr); GPSPosition::DataFields gpsPos = gpsPosObj->getData(); - uavPos->getLocator()->setPosition( osg::Vec3d(gpsPos.Longitude / 10.0e6, gpsPos.Latitude / 10.0e6, gpsPos.Altitude)); + uavPos->getLocator()->setPosition(osg::Vec3d(gpsPos.Longitude / 10.0e6, gpsPos.Latitude / 10.0e6, gpsPos.Altitude)); } // Set the attitude (reverse the attitude) @@ -281,8 +283,8 @@ void OsgViewerWidget::paintEvent( QPaintEvent* event ) // Have to rotate the axes from OP NED frame to OSG frame (X east, Y north, Z down) double angle; osg::Vec3d axis; - quat.getRotate(angle,axis); - quat.makeRotate(angle, osg::Vec3d(axis[1],axis[0],-axis[2])); + quat.getRotate(angle, axis); + quat.makeRotate(angle, osg::Vec3d(axis[1], axis[0], -axis[2])); osg::Matrixd rot = osg::Matrixd::rotate(quat); uavAttitudeAndScale->setMatrix(rot); diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.h b/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.h index cc7e8894c..03002c32f 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.h +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.h @@ -1,12 +1,12 @@ /******************************************************************************** -* @file osgviewerwidget.h -* @author The OpenPilot Team Copyright (C) 2012. -* @addtogroup GCSPlugins GCS Plugins -* @{ -* @addtogroup OsgEarthview Plugin -* @{ -* @brief Osg Earth view of UAV -*****************************************************************************/ + * @file osgviewerwidget.h + * @author The OpenPilot Team Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup OsgEarthview Plugin + * @{ + * @brief Osg Earth view of UAV + *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -82,8 +82,7 @@ using namespace osgEarth::Annotation; #include -class OsgViewerWidget : public QWidget, public osgViewer::CompositeViewer -{ +class OsgViewerWidget : public QWidget, public osgViewer::CompositeViewer { Q_OBJECT public: explicit OsgViewerWidget(QWidget *parent = 0); @@ -96,20 +95,20 @@ protected: void paintEvent(QPaintEvent *event); /* Create a osgQt::GraphicsWindowQt to add to the widget */ - QWidget* createViewWidget( osg::Camera* camera, osg::Node* scene ); + QWidget *createViewWidget(osg::Camera *camera, osg::Node *scene); /* Create an osg::Camera which sets up the OSG view */ - osg::Camera* createCamera( int x, int y, int w, int h, const std::string& name, bool windowDecoration ); + osg::Camera *createCamera(int x, int y, int w, int h, const std::string & name, bool windowDecoration); /* Get the model to render */ - osg::Node* createAirplane(); + osg::Node *createAirplane(); private: /* Private variables */ QTimer _timer; - EarthManipulator* manip; - osgEarth::Util::ObjectLocatorNode* uavPos; - osg::MatrixTransform* uavAttitudeAndScale; - osgEarth::MapNode* mapNode; + EarthManipulator *manip; + osgEarth::Util::ObjectLocatorNode *uavPos; + osg::MatrixTransform *uavAttitudeAndScale; + osgEarth::MapNode *mapNode; }; diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.cpp index c0ac4fa01..028dc82c3 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.cpp @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -29,16 +29,15 @@ #include "fieldtreeitem.h" BrowserItemDelegate::BrowserItemDelegate(QObject *parent) : - QStyledItemDelegate(parent) -{ -} + QStyledItemDelegate(parent) +{} QWidget *BrowserItemDelegate::createEditor(QWidget *parent, - const QStyleOptionViewItem & option , - const QModelIndex & index ) const + const QStyleOptionViewItem & option, + const QModelIndex & index) const { Q_UNUSED(option) - FieldTreeItem *item = static_cast(index.internalPointer()); + FieldTreeItem * item = static_cast(index.internalPointer()); QWidget *editor = item->createEditor(parent); Q_ASSERT(editor); return editor; @@ -48,21 +47,23 @@ QWidget *BrowserItemDelegate::createEditor(QWidget *parent, void BrowserItemDelegate::setEditorData(QWidget *editor, const QModelIndex &index) const { - FieldTreeItem *item = static_cast(index.internalPointer()); + FieldTreeItem *item = static_cast(index.internalPointer()); QVariant value = index.model()->data(index, Qt::EditRole); + item->setEditorValue(editor, value); } void BrowserItemDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, const QModelIndex &index) const { - FieldTreeItem *item = static_cast(index.internalPointer()); + FieldTreeItem *item = static_cast(index.internalPointer()); QVariant value = item->getEditorValue(editor); + model->setData(index, value, Qt::EditRole); } void BrowserItemDelegate::updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &/* index */) const + const QStyleOptionViewItem &option, const QModelIndex & /* index */) const { editor->setGeometry(option.rect); } diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.h b/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.h index 9c6a0e983..17952b5db 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,9 +30,8 @@ #include -class BrowserItemDelegate : public QStyledItemDelegate -{ -Q_OBJECT +class BrowserItemDelegate : public QStyledItemDelegate { + Q_OBJECT public: explicit BrowserItemDelegate(QObject *parent = 0); @@ -44,15 +43,14 @@ public: const QModelIndex &index) const; void updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &index) const; + const QStyleOptionViewItem &option, const QModelIndex &index) const; QSize sizeHint(const QStyleOptionViewItem & option, - const QModelIndex &index) const; + const QModelIndex &index) const; signals: public slots: - }; #endif // BROWSERITEMDELEGATE_H diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.cpp index fa3c9e203..2c1407d06 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.cpp @@ -10,20 +10,19 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "fieldtreeitem.h" - diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.h b/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.h index 7d33c431f..27c4566f7 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.h @@ -37,88 +37,102 @@ #include #include -#define QINT8MIN std::numeric_limits::min() -#define QINT8MAX std::numeric_limits::max() -#define QUINTMIN std::numeric_limits::min() -#define QUINT8MAX std::numeric_limits::max() -#define QINT16MIN std::numeric_limits::min() -#define QINT16MAX std::numeric_limits::max() +#define QINT8MIN std::numeric_limits::min() +#define QINT8MAX std::numeric_limits::max() +#define QUINTMIN std::numeric_limits::min() +#define QUINT8MAX std::numeric_limits::max() +#define QINT16MIN std::numeric_limits::min() +#define QINT16MAX std::numeric_limits::max() #define QUINT16MAX std::numeric_limits::max() -#define QINT32MIN std::numeric_limits::min() -#define QINT32MAX std::numeric_limits::max() +#define QINT32MIN std::numeric_limits::min() +#define QINT32MAX std::numeric_limits::max() #define QUINT32MAX std::numeric_limits::max() -//#define USE_SCIENTIFIC_NOTATION +// #define USE_SCIENTIFIC_NOTATION -class FieldTreeItem : public TreeItem -{ -Q_OBJECT +class FieldTreeItem : public TreeItem { + Q_OBJECT public: FieldTreeItem(int index, const QList &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_index(index) { } + TreeItem(data, parent), m_index(index) {} FieldTreeItem(int index, const QVariant &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_index(index) { } - bool isEditable() { return true; } - virtual QWidget *createEditor(QWidget *parent) = 0; + TreeItem(data, parent), m_index(index) {} + bool isEditable() + { + return true; + } + virtual QWidget *createEditor(QWidget *parent) = 0; virtual QVariant getEditorValue(QWidget *editor) = 0; virtual void setEditorValue(QWidget *editor, QVariant value) = 0; - virtual void apply() { } + virtual void apply() {} protected: int m_index; }; -class EnumFieldTreeItem : public FieldTreeItem -{ -Q_OBJECT +class EnumFieldTreeItem : public FieldTreeItem { + Q_OBJECT public: EnumFieldTreeItem(UAVObjectField *field, int index, const QList &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) { } + FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) {} EnumFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) { } - void setData(QVariant value, int column) { + FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) {} + void setData(QVariant value, int column) + { QStringList options = m_field->getOptions(); - QVariant tmpValue = m_field->getValue(m_index); - int tmpValIndex = options.indexOf(tmpValue.toString()); + QVariant tmpValue = m_field->getValue(m_index); + int tmpValIndex = options.indexOf(tmpValue.toString()); TreeItem::setData(value, column); + setChanged(tmpValIndex != value); } - QString enumOptions(int index) { - if((index < 0) || (index >= m_enumOptions.length())) { + QString enumOptions(int index) + { + if ((index < 0) || (index >= m_enumOptions.length())) { return QString("Invalid Value (") + QString().setNum(index) + QString(")"); } return m_enumOptions.at(index); } - void apply() { + void apply() + { int value = data(dataColumn).toInt(); QStringList options = m_field->getOptions(); + m_field->setValue(options[value], m_index); setChanged(false); } - void update() { + void update() + { QStringList options = m_field->getOptions(); QVariant value = m_field->getValue(m_index); - int valIndex = options.indexOf(value.toString()); + int valIndex = options.indexOf(value.toString()); + if (data() != valIndex || changed()) { TreeItem::setData(valIndex); setHighlight(true); } } - QWidget *createEditor(QWidget *parent) { + QWidget *createEditor(QWidget *parent) + { QComboBox *editor = new QComboBox(parent); - foreach (QString option, m_enumOptions) - editor->addItem(option); + + foreach(QString option, m_enumOptions) + editor->addItem(option); return editor; } - QVariant getEditorValue(QWidget *editor) { - QComboBox *comboBox = static_cast(editor); + QVariant getEditorValue(QWidget *editor) + { + QComboBox *comboBox = static_cast(editor); + return comboBox->currentIndex(); } - void setEditorValue(QWidget *editor, QVariant value) { - QComboBox *comboBox = static_cast(editor); + void setEditorValue(QWidget *editor, QVariant value) + { + QComboBox *comboBox = static_cast(editor); + comboBox->setCurrentIndex(value.toInt()); } private: @@ -126,20 +140,22 @@ private: UAVObjectField *m_field; }; -class IntFieldTreeItem : public FieldTreeItem -{ -Q_OBJECT +class IntFieldTreeItem : public FieldTreeItem { + Q_OBJECT public: IntFieldTreeItem(UAVObjectField *field, int index, const QList &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { + FieldTreeItem(index, data, parent), m_field(field) + { setMinMaxValues(); } IntFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { + FieldTreeItem(index, data, parent), m_field(field) + { setMinMaxValues(); } - void setMinMaxValues() { + void setMinMaxValues() + { switch (m_field->getType()) { case UAVObjectField::INT8: m_minValue = QINT8MIN; @@ -171,34 +187,45 @@ public: } } - QWidget *createEditor(QWidget *parent) { + QWidget *createEditor(QWidget *parent) + { QSpinBox *editor = new QSpinBox(parent); + editor->setMinimum(m_minValue); editor->setMaximum(m_maxValue); return editor; } - QVariant getEditorValue(QWidget *editor) { - QSpinBox *spinBox = static_cast(editor); + QVariant getEditorValue(QWidget *editor) + { + QSpinBox *spinBox = static_cast(editor); + spinBox->interpretText(); return spinBox->value(); } - void setEditorValue(QWidget *editor, QVariant value) { - QSpinBox *spinBox = static_cast(editor); + void setEditorValue(QWidget *editor, QVariant value) + { + QSpinBox *spinBox = static_cast(editor); + spinBox->setValue(value.toInt()); } - void setData(QVariant value, int column) { - QVariant old=m_field->getValue(m_index); - TreeItem::setData(value, column); + void setData(QVariant value, int column) + { + QVariant old = m_field->getValue(m_index); + TreeItem::setData(value, column); + setChanged(old != value); } - void apply() { + void apply() + { m_field->setValue(data(dataColumn).toInt(), m_index); setChanged(false); } - void update() { + void update() + { int value = m_field->getValue(m_index).toInt(); + if (data() != value || changed()) { TreeItem::setData(value); setHighlight(true); @@ -211,112 +238,137 @@ private: int m_maxValue; }; -class FloatFieldTreeItem : public FieldTreeItem -{ -Q_OBJECT +class FloatFieldTreeItem : public FieldTreeItem { + Q_OBJECT public: FloatFieldTreeItem(UAVObjectField *field, int index, const QList &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { } + FieldTreeItem(index, data, parent), m_field(field) {} FloatFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { } - void setData(QVariant value, int column) { - QVariant old=m_field->getValue(m_index); + FieldTreeItem(index, data, parent), m_field(field) {} + void setData(QVariant value, int column) + { + QVariant old = m_field->getValue(m_index); TreeItem::setData(value, column); + setChanged(old != value); } - void apply() { + void apply() + { m_field->setValue(data(dataColumn).toDouble(), m_index); setChanged(false); } - void update() { + void update() + { double value = m_field->getValue(m_index).toDouble(); + if (data() != value || changed()) { TreeItem::setData(value); setHighlight(true); } } - QWidget *createEditor(QWidget *parent) { - #ifdef USE_SCIENTIFIC_NOTATION - QScienceSpinBox *editor = new QScienceSpinBox(parent); - editor->setDecimals(6); - #else - QDoubleSpinBox *editor = new QDoubleSpinBox(parent); - editor->setDecimals(8); - #endif + QWidget *createEditor(QWidget *parent) + { + #ifdef USE_SCIENTIFIC_NOTATION + QScienceSpinBox *editor = new QScienceSpinBox(parent); + editor->setDecimals(6); + #else + QDoubleSpinBox *editor = new QDoubleSpinBox(parent); + editor->setDecimals(8); + #endif editor->setMinimum(-std::numeric_limits::max()); editor->setMaximum(std::numeric_limits::max()); return editor; } - QVariant getEditorValue(QWidget *editor) { - #ifdef USE_SCIENTIFIC_NOTATION - QScienceSpinBox *spinBox = static_cast(editor); - #else - QDoubleSpinBox *spinBox = static_cast(editor); - #endif - spinBox->interpretText(); + QVariant getEditorValue(QWidget *editor) + { + #ifdef USE_SCIENTIFIC_NOTATION + QScienceSpinBox *spinBox = static_cast(editor); + #else + QDoubleSpinBox *spinBox = static_cast(editor); + #endif + spinBox->interpretText(); return spinBox->value(); } - void setEditorValue(QWidget *editor, QVariant value) { - #ifdef USE_SCIENTIFIC_NOTATION - QScienceSpinBox *spinBox = static_cast(editor); - #else - QDoubleSpinBox *spinBox = static_cast(editor); - #endif - spinBox->setValue(value.toDouble()); + void setEditorValue(QWidget *editor, QVariant value) + { + #ifdef USE_SCIENTIFIC_NOTATION + QScienceSpinBox *spinBox = static_cast(editor); + #else + QDoubleSpinBox *spinBox = static_cast(editor); + #endif + spinBox->setValue(value.toDouble()); } private: UAVObjectField *m_field; }; -class ActionFieldTreeItem : public FieldTreeItem -{ -Q_OBJECT +class ActionFieldTreeItem : public FieldTreeItem { + Q_OBJECT public: ActionFieldTreeItem(UAVObjectField *field, int index, const QList &data, QStringList *actions, - TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { m_enumOptions=actions; } + TreeItem *parent = 0) : + FieldTreeItem(index, data, parent), m_field(field) + { + m_enumOptions = actions; + } ActionFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, QStringList *actions, - TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { m_enumOptions=actions; } - void setData(QVariant value, int column) { + TreeItem *parent = 0) : + FieldTreeItem(index, data, parent), m_field(field) + { + m_enumOptions = actions; + } + void setData(QVariant value, int column) + { int tmpValIndex = m_field->getValue(m_index).toInt(); TreeItem::setData(value, column); + setChanged(tmpValIndex != value); } - QString enumOptions(int index) { - if((index < 0) || (index >= m_enumOptions->length())) { + QString enumOptions(int index) + { + if ((index < 0) || (index >= m_enumOptions->length())) { return QString("Invalid Value (") + QString().setNum(index) + QString(")"); } return m_enumOptions->at(index); } - void apply() { + void apply() + { int value = data(dataColumn).toInt(); + m_field->setValue(value, m_index); setChanged(false); } - void update() { + void update() + { int valIndex = m_field->getValue(m_index).toInt(); + if (data() != valIndex || changed()) { TreeItem::setData(valIndex); setHighlight(true); } } - QWidget *createEditor(QWidget *parent) { + QWidget *createEditor(QWidget *parent) + { QComboBox *editor = new QComboBox(parent); - foreach (QString option, *m_enumOptions) - editor->addItem(option); + + foreach(QString option, *m_enumOptions) + editor->addItem(option); return editor; } - QVariant getEditorValue(QWidget *editor) { - QComboBox *comboBox = static_cast(editor); + QVariant getEditorValue(QWidget *editor) + { + QComboBox *comboBox = static_cast(editor); + return comboBox->currentIndex(); } - void setEditorValue(QWidget *editor, QVariant value) { - QComboBox *comboBox = static_cast(editor); + void setEditorValue(QWidget *editor, QVariant value) + { + QComboBox *comboBox = static_cast(editor); + comboBox->setCurrentIndex(value.toInt()); } private: diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.cpp index 078c58525..0d78b536c 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.cpp @@ -32,17 +32,16 @@ #include PathActionEditorGadget::PathActionEditorGadget(QString classId, PathActionEditorGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} PathActionEditorGadget::~PathActionEditorGadget() { delete m_widget; } -void PathActionEditorGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void PathActionEditorGadget::loadConfiguration(IUAVGadgetConfiguration *config) { Q_UNUSED(config); } diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.h b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.h index 936019940..caeb66f30 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.h @@ -32,27 +32,35 @@ namespace Core { class IUAVGadget; } -//class QWidget; -//class QString; +// class QWidget; +// class QString; class PathActionEditorGadgetWidget; using namespace Core; -class PathActionEditorGadget : public Core::IUAVGadget -{ +class PathActionEditorGadget : public Core::IUAVGadget { Q_OBJECT public: PathActionEditorGadget(QString classId, PathActionEditorGadgetWidget *widget, QWidget *parent = 0); ~PathActionEditorGadget(); - QList context() const { return m_context; } - QWidget *widget() { return m_widget; } - QString contextHelpId() const { return QString(); } + QList context() const + { + return m_context; + } + QWidget *widget() + { + return m_widget; + } + QString contextHelpId() const + { + return QString(); + } - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); private: - QWidget *m_widget; - QList m_context; + QWidget *m_widget; + QList m_context; }; diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.cpp index c84250e21..b6f48a522 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.cpp @@ -30,18 +30,17 @@ #include PathActionEditorGadgetFactory::PathActionEditorGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("PathActionEditorGadget"), - tr("PathAction Editor"), - parent) -{ -} + IUAVGadgetFactory(QString("PathActionEditorGadget"), + tr("PathAction Editor"), + parent) +{} PathActionEditorGadgetFactory::~PathActionEditorGadgetFactory() +{} + +IUAVGadget *PathActionEditorGadgetFactory::createGadget(QWidget *parent) { + PathActionEditorGadgetWidget *gadgetWidget = new PathActionEditorGadgetWidget(parent); -} - -IUAVGadget* PathActionEditorGadgetFactory::createGadget(QWidget *parent) { - PathActionEditorGadgetWidget* gadgetWidget = new PathActionEditorGadgetWidget(parent); return new PathActionEditorGadget(QString("PathActionEditorGadget"), gadgetWidget, parent); } diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.h b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.h index e78021528..a2295dda7 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.h @@ -36,8 +36,7 @@ class IUAVGadgetFactory; using namespace Core; -class PathActionEditorGadgetFactory : public IUAVGadgetFactory -{ +class PathActionEditorGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: PathActionEditorGadgetFactory(QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.cpp index a1b568988..00f306587 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.cpp @@ -58,7 +58,7 @@ PathActionEditorGadgetWidget::PathActionEditorGadgetWidget(QWidget *parent) : QL Q_ASSERT(objManager != NULL); pathactionObj = PathAction::GetInstance(objManager); Q_ASSERT(pathactionObj != NULL); - waypointObj = Waypoint::GetInstance(objManager); + waypointObj = Waypoint::GetInstance(objManager); Q_ASSERT(waypointObj != NULL); // Connect the signals @@ -70,24 +70,24 @@ PathActionEditorGadgetWidget::PathActionEditorGadgetWidget(QWidget *parent) : QL PathActionEditorGadgetWidget::~PathActionEditorGadgetWidget() { - // Do nothing + // Do nothing } void PathActionEditorGadgetWidget::pathactionChanged(UAVObject *) -{ -} +{} void PathActionEditorGadgetWidget::addPathActionInstance() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm != NULL); UAVObjectManager *objManager = pm->getObject(); Q_ASSERT(objManager != NULL); qDebug() << "Instances before: " << objManager->getNumInstances(pathactionObj->getObjID()); - PathAction *obj = new PathAction(); + PathAction *obj = new PathAction(); quint32 newInstId = objManager->getNumInstances(pathactionObj->getObjID()); - obj->initialize(newInstId,obj->getMetaObject()); + obj->initialize(newInstId, obj->getMetaObject()); objManager->registerObject(obj); qDebug() << "Instances after: " << objManager->getNumInstances(pathactionObj->getObjID()); } @@ -95,19 +95,20 @@ void PathActionEditorGadgetWidget::addPathActionInstance() void PathActionEditorGadgetWidget::addWaypointInstance() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm != NULL); UAVObjectManager *objManager = pm->getObject(); Q_ASSERT(objManager != NULL); qDebug() << "Instances before: " << objManager->getNumInstances(waypointObj->getObjID()); - Waypoint *obj = new Waypoint(); + Waypoint *obj = new Waypoint(); quint32 newInstId = objManager->getNumInstances(waypointObj->getObjID()); - obj->initialize(newInstId,obj->getMetaObject()); + obj->initialize(newInstId, obj->getMetaObject()); objManager->registerObject(obj); qDebug() << "Instances after: " << objManager->getNumInstances(waypointObj->getObjID()); } /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.h b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.h index bfbcb6193..f01411861 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.h @@ -36,8 +36,7 @@ class Ui_PathActionEditor; -class PathActionEditorGadgetWidget : public QLabel -{ +class PathActionEditorGadgetWidget : public QLabel { Q_OBJECT public: @@ -52,7 +51,7 @@ protected slots: void addWaypointInstance(); private: - Ui_PathActionEditor * m_pathactioneditor; + Ui_PathActionEditor *m_pathactioneditor; PathActionEditorTreeModel *m_model; PathAction *pathactionObj; Waypoint *waypointObj; diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.cpp index 090397bcc..99c1a611e 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.cpp @@ -33,36 +33,36 @@ PathActionEditorPlugin::PathActionEditorPlugin() { - // Do nothing + // Do nothing } PathActionEditorPlugin::~PathActionEditorPlugin() { - // Do nothing + // Do nothing } -bool PathActionEditorPlugin::initialize(const QStringList& args, QString *errMsg) +bool PathActionEditorPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new PathActionEditorGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new PathActionEditorGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void PathActionEditorPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void PathActionEditorPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(PathActionEditorPlugin) /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.h b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.h index d0102ada7..2a00feda4 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.h @@ -31,16 +31,15 @@ class PathActionEditorGadgetFactory; -class PathActionEditorPlugin : public ExtensionSystem::IPlugin -{ +class PathActionEditorPlugin : public ExtensionSystem::IPlugin { public: PathActionEditorPlugin(); - ~PathActionEditorPlugin(); + ~PathActionEditorPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - PathActionEditorGadgetFactory *mf; + PathActionEditorGadgetFactory *mf; }; #endif /* PathActionEditorPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.cpp index 86b0c8400..629140e1a 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.cpp @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,7 @@ #include "uavobjectfield.h" #include "extensionsystem/pluginmanager.h" #include -//#include +// #include #include #include #include @@ -43,17 +43,18 @@ #include "pathaction.h" PathActionEditorTreeModel::PathActionEditorTreeModel(QObject *parent) : - QAbstractItemModel(parent), - m_recentlyUpdatedColor(QColor(255, 230, 230)), - m_manuallyChangedColor(QColor(230, 230, 255)) + QAbstractItemModel(parent), + m_recentlyUpdatedColor(QColor(255, 230, 230)), + m_manuallyChangedColor(QColor(230, 230, 255)) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + m_objManager = pm->getObject(); - connect(m_objManager, SIGNAL(newInstance(UAVObject*)), this, SLOT(newInstance(UAVObject*))); - connect(m_objManager->getObject("WaypointActive"),SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objUpdated(UAVObject*))); - connect(m_objManager->getObject("PathAction"),SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objUpdated(UAVObject*))); - connect(m_objManager->getObject("Waypoint"),SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objUpdated(UAVObject*))); + connect(m_objManager, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *))); + connect(m_objManager->getObject("WaypointActive"), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(objUpdated(UAVObject *))); + connect(m_objManager->getObject("PathAction"), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(objUpdated(UAVObject *))); + connect(m_objManager->getObject("Waypoint"), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(objUpdated(UAVObject *))); setupModelData(); } @@ -65,7 +66,6 @@ PathActionEditorTreeModel::~PathActionEditorTreeModel() void PathActionEditorTreeModel::setupModelData() { - m_actions = new QStringList(); updateActions(); @@ -76,52 +76,54 @@ void PathActionEditorTreeModel::setupModelData() m_pathactionsTree = new TopTreeItem(tr("PathActions"), m_rootItem); m_rootItem->appendChild(m_pathactionsTree); - m_waypointsTree = new TopTreeItem(tr("Waypoints"), m_rootItem); + m_waypointsTree = new TopTreeItem(tr("Waypoints"), m_rootItem); m_rootItem->appendChild(m_waypointsTree); - connect(m_rootItem, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); - connect(m_pathactionsTree, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); - connect(m_waypointsTree, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(m_rootItem, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); + connect(m_pathactionsTree, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); + connect(m_waypointsTree, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); { - QList list = m_objManager->getObjectInstances("PathAction"); - foreach (UAVObject* obj, list) { - addInstance(obj,m_pathactionsTree); + QList list = m_objManager->getObjectInstances("PathAction"); + foreach(UAVObject * obj, list) { + addInstance(obj, m_pathactionsTree); } } { - QList list = m_objManager->getObjectInstances("Waypoint"); - foreach (UAVObject* obj, list) { - addInstance(obj,m_waypointsTree); + QList list = m_objManager->getObjectInstances("Waypoint"); + foreach(UAVObject * obj, list) { + addInstance(obj, m_waypointsTree); } } } -void PathActionEditorTreeModel::updateActions() { - m_actions->clear(); - QList list = m_objManager->getObjectInstances("PathAction"); - foreach (UAVObject* obj, list) { - QString title; - title.append((QVariant(obj->getInstID()).toString())); - title.append(" "); - title.append((obj->getField("Mode")->getValue().toString())); - title.append(" "); - title.append((obj->getField("Command")->getValue().toString())); - title.append(":"); - title.append((obj->getField("EndCondition")->getValue().toString())); - title.append(" "); - m_actions->append(title); - } +void PathActionEditorTreeModel::updateActions() +{ + m_actions->clear(); + QList list = m_objManager->getObjectInstances("PathAction"); + foreach(UAVObject * obj, list) { + QString title; + + title.append((QVariant(obj->getInstID()).toString())); + title.append(" "); + title.append((obj->getField("Mode")->getValue().toString())); + title.append(" "); + title.append((obj->getField("Command")->getValue().toString())); + title.append(":"); + title.append((obj->getField("EndCondition")->getValue().toString())); + title.append(" "); + m_actions->append(title); + } } void PathActionEditorTreeModel::addInstance(UAVObject *obj, TreeItem *parent) { - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(highlightUpdatedObject(UAVObject*))); + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(highlightUpdatedObject(UAVObject *))); TreeItem *item; QString name = QString::number(obj->getInstID()); item = new InstanceTreeItem(obj, name); - connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(item, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); parent->appendChild(item); - foreach (UAVObjectField *field, obj->getFields()) { + foreach(UAVObjectField * field, obj->getFields()) { if (field->getNumElements() > 1) { addArrayField(field, item); } else { @@ -134,7 +136,8 @@ void PathActionEditorTreeModel::addInstance(UAVObject *obj, TreeItem *parent) void PathActionEditorTreeModel::addArrayField(UAVObjectField *field, TreeItem *parent) { TreeItem *item = new ArrayFieldTreeItem(field->getName()); - connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + + connect(item, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); for (uint i = 0; i < field->getNumElements(); ++i) { addSingleField(i, field, item); } @@ -144,83 +147,90 @@ void PathActionEditorTreeModel::addArrayField(UAVObjectField *field, TreeItem *p void PathActionEditorTreeModel::addSingleField(int index, UAVObjectField *field, TreeItem *parent) { QList data; - if (field->getNumElements() == 1) + if (field->getNumElements() == 1) { data.append(field->getName()); - else - data.append( QString("[%1]").arg((field->getElementNames())[index]) ); + } else { + data.append(QString("[%1]").arg((field->getElementNames())[index])); + } FieldTreeItem *item; UAVObjectField::FieldType type = field->getType(); // hack: list available actions in an enum - if (field->getName().compare("Action")==0 && type==UAVObjectField::UINT8) { - data.append( field->getValue(index).toInt()); - data.append( field->getUnits()); + if (field->getName().compare("Action") == 0 && type == UAVObjectField::UINT8) { + data.append(field->getValue(index).toInt()); + data.append(field->getUnits()); item = new ActionFieldTreeItem(field, index, data, m_actions); } else { - switch (type) { - case UAVObjectField::ENUM: { - QStringList options = field->getOptions(); - QVariant value = field->getValue(); - data.append( options.indexOf(value.toString()) ); - data.append(field->getUnits()); - item = new EnumFieldTreeItem(field, index, data); - break; + switch (type) { + case UAVObjectField::ENUM: + { + QStringList options = field->getOptions(); + QVariant value = field->getValue(); + data.append(options.indexOf(value.toString())); + data.append(field->getUnits()); + item = new EnumFieldTreeItem(field, index, data); + break; + } + case UAVObjectField::INT8: + case UAVObjectField::INT16: + case UAVObjectField::INT32: + case UAVObjectField::UINT8: + case UAVObjectField::UINT16: + case UAVObjectField::UINT32: + data.append(field->getValue(index)); + data.append(field->getUnits()); + item = new IntFieldTreeItem(field, index, data); + break; + case UAVObjectField::FLOAT32: + data.append(field->getValue(index)); + data.append(field->getUnits()); + item = new FloatFieldTreeItem(field, index, data); + break; + default: + Q_ASSERT(false); + } } - case UAVObjectField::INT8: - case UAVObjectField::INT16: - case UAVObjectField::INT32: - case UAVObjectField::UINT8: - case UAVObjectField::UINT16: - case UAVObjectField::UINT32: - data.append(field->getValue(index)); - data.append(field->getUnits()); - item = new IntFieldTreeItem(field, index, data); - break; - case UAVObjectField::FLOAT32: - data.append(field->getValue(index)); - data.append(field->getUnits()); - item = new FloatFieldTreeItem(field, index, data); - break; - default: - Q_ASSERT(false); - } - } - connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(item, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); parent->appendChild(item); } QModelIndex PathActionEditorTreeModel::index(int row, int column, const QModelIndex &parent) - const +const { - if (!hasIndex(row, column, parent)) + if (!hasIndex(row, column, parent)) { return QModelIndex(); + } TreeItem *parentItem; - if (!parent.isValid()) + if (!parent.isValid()) { parentItem = m_rootItem; - else - parentItem = static_cast(parent.internalPointer()); + } else { + parentItem = static_cast(parent.internalPointer()); + } TreeItem *childItem = parentItem->child(row); - if (childItem) + if (childItem) { return createIndex(row, column, childItem); - else + } else { return QModelIndex(); + } } QModelIndex PathActionEditorTreeModel::index(TreeItem *item) { - if (item->parent() == 0) + if (item->parent() == 0) { return QModelIndex(); + } QModelIndex root = index(item->parent()); for (int i = 0; i < rowCount(root); ++i) { QModelIndex childIndex = index(i, 0, root); - TreeItem *child = static_cast(childIndex.internalPointer()); - if (child == item) + TreeItem *child = static_cast(childIndex.internalPointer()); + if (child == item) { return childIndex; + } } Q_ASSERT(false); return QModelIndex(); @@ -228,14 +238,16 @@ QModelIndex PathActionEditorTreeModel::index(TreeItem *item) QModelIndex PathActionEditorTreeModel::parent(const QModelIndex &index) const { - if (!index.isValid()) + if (!index.isValid()) { return QModelIndex(); + } - TreeItem *childItem = static_cast(index.internalPointer()); + TreeItem *childItem = static_cast(index.internalPointer()); TreeItem *parentItem = childItem->parent(); - if (parentItem == m_rootItem) + if (parentItem == m_rootItem) { return QModelIndex(); + } return createIndex(parentItem->row(), 0, parentItem); } @@ -243,72 +255,81 @@ QModelIndex PathActionEditorTreeModel::parent(const QModelIndex &index) const int PathActionEditorTreeModel::rowCount(const QModelIndex &parent) const { TreeItem *parentItem; - if (parent.column() > 0) - return 0; - if (!parent.isValid()) + if (parent.column() > 0) { + return 0; + } + + if (!parent.isValid()) { parentItem = m_rootItem; - else - parentItem = static_cast(parent.internalPointer()); + } else { + parentItem = static_cast(parent.internalPointer()); + } return parentItem->childCount(); } int PathActionEditorTreeModel::columnCount(const QModelIndex &parent) const { - if (parent.isValid()) - return static_cast(parent.internalPointer())->columnCount(); - else + if (parent.isValid()) { + return static_cast(parent.internalPointer())->columnCount(); + } else { return m_rootItem->columnCount(); + } } QVariant PathActionEditorTreeModel::data(const QModelIndex &index, int role) const { - if (!index.isValid()) + if (!index.isValid()) { return QVariant(); + } if (index.column() == TreeItem::dataColumn && role == Qt::EditRole) { - TreeItem *item = static_cast(index.internalPointer()); + TreeItem *item = static_cast(index.internalPointer()); return item->data(index.column()); } -// if (role == Qt::DecorationRole) -// return QIcon(":/core/images/openpilot_logo_128.png"); +// if (role == Qt::DecorationRole) +// return QIcon(":/core/images/openpilot_logo_128.png"); if (role == Qt::ToolTipRole) { - TreeItem *item = static_cast(index.internalPointer()); + TreeItem *item = static_cast(index.internalPointer()); return item->description(); } - TreeItem *item = static_cast(index.internalPointer()); + TreeItem *item = static_cast(index.internalPointer()); if (index.column() == 0 && role == Qt::BackgroundRole) { - ObjectTreeItem *objItem = dynamic_cast(item); - if (objItem && objItem->highlighted()) + ObjectTreeItem *objItem = dynamic_cast(item); + if (objItem && objItem->highlighted()) { return QVariant(m_recentlyUpdatedColor); + } } if (index.column() == TreeItem::dataColumn && role == Qt::BackgroundRole) { - FieldTreeItem *fieldItem = dynamic_cast(item); - if (fieldItem && fieldItem->highlighted()) + FieldTreeItem *fieldItem = dynamic_cast(item); + if (fieldItem && fieldItem->highlighted()) { return QVariant(m_recentlyUpdatedColor); - if (fieldItem && fieldItem->changed()) + } + if (fieldItem && fieldItem->changed()) { return QVariant(m_manuallyChangedColor); + } } - if (role != Qt::DisplayRole) + if (role != Qt::DisplayRole) { return QVariant(); + } if (index.column() == TreeItem::dataColumn) { - EnumFieldTreeItem *fieldItem = dynamic_cast(item); + EnumFieldTreeItem *fieldItem = dynamic_cast(item); if (fieldItem) { int enumIndex = fieldItem->data(index.column()).toInt(); return fieldItem->enumOptions(enumIndex); } else { - ActionFieldTreeItem *afieldItem = dynamic_cast(item); + ActionFieldTreeItem *afieldItem = dynamic_cast(item); if (afieldItem) { int enumIndex = afieldItem->data(index.column()).toInt(); return afieldItem->enumOptions(enumIndex); } - } + } } return item->data(index.column()); @@ -317,7 +338,7 @@ QVariant PathActionEditorTreeModel::data(const QModelIndex &index, int role) con bool PathActionEditorTreeModel::setData(const QModelIndex &index, const QVariant & value, int role) { Q_UNUSED(role) - TreeItem *item = static_cast(index.internalPointer()); + TreeItem * item = static_cast(index.internalPointer()); item->setData(value, index.column()); return true; } @@ -325,23 +346,26 @@ bool PathActionEditorTreeModel::setData(const QModelIndex &index, const QVariant Qt::ItemFlags PathActionEditorTreeModel::flags(const QModelIndex &index) const { - if (!index.isValid()) + if (!index.isValid()) { return 0; + } if (index.column() == TreeItem::dataColumn) { - TreeItem *item = static_cast(index.internalPointer()); - if (item->isEditable()) + TreeItem *item = static_cast(index.internalPointer()); + if (item->isEditable()) { return Qt::ItemIsEnabled | Qt::ItemIsSelectable | Qt::ItemIsEditable; + } } return Qt::ItemIsEnabled | Qt::ItemIsSelectable; } QVariant PathActionEditorTreeModel::headerData(int section, Qt::Orientation orientation, - int role) const + int role) const { - if (orientation == Qt::Horizontal && role == Qt::DisplayRole) + if (orientation == Qt::Horizontal && role == Qt::DisplayRole) { return m_rootItem->data(section); + } return QVariant(); } @@ -349,21 +373,23 @@ QVariant PathActionEditorTreeModel::headerData(int section, Qt::Orientation orie void PathActionEditorTreeModel::updateHighlight(TreeItem *item) { QModelIndex itemIndex = index(item); + Q_ASSERT(itemIndex != QModelIndex()); emit dataChanged(itemIndex, itemIndex.sibling(itemIndex.row(), TreeItem::dataColumn)); // update uavobject if any - go up the tree until there is ObjectTreeItem *objItem = 0; - TreeItem *searchItem = item; + TreeItem *searchItem = item; while (searchItem) { - objItem = dynamic_cast(searchItem); - if (objItem) + objItem = dynamic_cast(searchItem); + if (objItem) { break; + } searchItem = searchItem->parent(); } if (objItem) { - item->apply(); - objItem->apply(); + item->apply(); + objItem->apply(); UAVObject *obj = objItem->object(); Q_ASSERT(obj); obj->updated(); @@ -373,23 +399,22 @@ void PathActionEditorTreeModel::updateHighlight(TreeItem *item) void PathActionEditorTreeModel::highlightUpdatedObject(UAVObject *obj) { Q_ASSERT(obj); - if (obj->getName().compare("Waypoint")==0) { + if (obj->getName().compare("Waypoint") == 0) { m_waypointsTree->update(); - //emit dataChanged(index(m_waypointsTree), index(m_waypointsTree)); - } else if (obj->getName().compare("PathAction")==0) { + // emit dataChanged(index(m_waypointsTree), index(m_waypointsTree)); + } else if (obj->getName().compare("PathAction") == 0) { m_pathactionsTree->update(); - //emit dataChanged(index(m_pathactionsTree), index(m_pathactionsTree)); + // emit dataChanged(index(m_pathactionsTree), index(m_pathactionsTree)); } } void PathActionEditorTreeModel::newInstance(UAVObject *obj) { - - if (obj->getName().compare("Waypoint")==0) { - addInstance(obj,m_waypointsTree); + if (obj->getName().compare("Waypoint") == 0) { + addInstance(obj, m_waypointsTree); m_waypointsTree->update(); - } else if (obj->getName().compare("PathAction")==0) { - addInstance(obj,m_pathactionsTree); + } else if (obj->getName().compare("PathAction") == 0) { + addInstance(obj, m_pathactionsTree); m_pathactionsTree->update(); } updateActions(); @@ -400,8 +425,10 @@ void PathActionEditorTreeModel::objUpdated(UAVObject *obj) { quint16 index = m_objManager->getObject("WaypointActive")->getField("Index")->getValue().toInt(); quint16 action; - foreach (TreeItem *child,m_waypointsTree->treeChildren()) { - ObjectTreeItem *objItem = dynamic_cast(child); + + foreach(TreeItem * child, m_waypointsTree->treeChildren()) { + ObjectTreeItem *objItem = dynamic_cast(child); + if (index == objItem->object()->getInstID()) { child->setActive(true); action = objItem->object()->getField("Action")->getValue().toInt(); @@ -409,8 +436,9 @@ void PathActionEditorTreeModel::objUpdated(UAVObject *obj) child->setActive(false); } } - foreach (TreeItem *child,m_pathactionsTree->treeChildren()) { - ObjectTreeItem *objItem = dynamic_cast(child); + foreach(TreeItem * child, m_pathactionsTree->treeChildren()) { + ObjectTreeItem *objItem = dynamic_cast(child); + if (action == objItem->object()->getInstID()) { child->setActive(true); } else { @@ -420,4 +448,3 @@ void PathActionEditorTreeModel::objUpdated(UAVObject *obj) updateActions(); emit layoutChanged(); } - diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.h b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.h index f34c30a41..30300bacb 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,9 +44,8 @@ class UAVObjectManager; class QSignalMapper; class QTimer; -class PathActionEditorTreeModel : public QAbstractItemModel -{ -Q_OBJECT +class PathActionEditorTreeModel : public QAbstractItemModel { + Q_OBJECT public: explicit PathActionEditorTreeModel(QObject *parent = 0); ~PathActionEditorTreeModel(); @@ -62,8 +61,14 @@ public: int rowCount(const QModelIndex &parent = QModelIndex()) const; int columnCount(const QModelIndex &parent = QModelIndex()) const; - void setRecentlyUpdatedColor(QColor color) { m_recentlyUpdatedColor = color; } - void setManuallyChangedColor(QColor color) { m_manuallyChangedColor = color; } + void setRecentlyUpdatedColor(QColor color) + { + m_recentlyUpdatedColor = color; + } + void setManuallyChangedColor(QColor color) + { + m_manuallyChangedColor = color; + } signals: @@ -73,7 +78,7 @@ public slots: private slots: void highlightUpdatedObject(UAVObject *obj); - void updateHighlight(TreeItem*); + void updateHighlight(TreeItem *); private: QModelIndex index(TreeItem *item); @@ -81,7 +86,7 @@ private: void addSingleField(int index, UAVObjectField *field, TreeItem *parent); void addInstance(UAVObject *obj, TreeItem *parent); - //QString updateMode(quint8 updateMode); + // QString updateMode(quint8 updateMode); void setupModelData(); void updateActions(); diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.cpp b/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.cpp index 125545cb5..2fe0b7717 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.cpp +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.cpp @@ -10,37 +10,36 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "treeitem.h" TreeItem::TreeItem(const QList &data, TreeItem *parent) : - QObject(0), - m_data(data), - m_parent(parent), - m_highlight(false), - m_changed(false) -{ -} + QObject(0), + m_data(data), + m_parent(parent), + m_highlight(false), + m_changed(false) +{} TreeItem::TreeItem(const QVariant &data, TreeItem *parent) : - QObject(0), - m_parent(parent), - m_highlight(false), - m_changed(false) + QObject(0), + m_parent(parent), + m_highlight(false), + m_changed(false) { m_data << data << "" << ""; } @@ -73,8 +72,9 @@ int TreeItem::childCount() const } int TreeItem::row() const { - if (m_parent) - return m_parent->m_children.indexOf(const_cast(this)); + if (m_parent) { + return m_parent->m_children.indexOf(const_cast(this)); + } return 0; } @@ -94,29 +94,34 @@ void TreeItem::setData(QVariant value, int column) m_data.replace(column, value); } -void TreeItem::update() { - foreach(TreeItem *child, treeChildren()) - child->update(); +void TreeItem::update() +{ + foreach(TreeItem * child, treeChildren()) + child->update(); } -void TreeItem::apply() { - foreach(TreeItem *child, treeChildren()) - child->apply(); +void TreeItem::apply() +{ + foreach(TreeItem * child, treeChildren()) + child->apply(); } -void TreeItem::setHighlight(bool highlight) { - //m_highlight = highlight; +void TreeItem::setHighlight(bool highlight) +{ + // m_highlight = highlight; m_changed = false; } -void TreeItem::setActive(bool highlight) { +void TreeItem::setActive(bool highlight) +{ m_highlight = highlight; - foreach(TreeItem *child, treeChildren()) - child->setActive(highlight); + foreach(TreeItem * child, treeChildren()) + child->setActive(highlight); } -void TreeItem::removeHighlight() { +void TreeItem::removeHighlight() +{ m_highlight = false; update(); } diff --git a/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.h b/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.h index 7a39023c5..376b24163 100644 --- a/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.h +++ b/ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,9 +36,8 @@ #include -class TreeItem : public QObject -{ -Q_OBJECT +class TreeItem : public QObject { + Q_OBJECT public: TreeItem(const QList &data, TreeItem *parent = 0); TreeItem(const QVariant &data, TreeItem *parent = 0); @@ -48,14 +47,22 @@ public: void insert(int index, TreeItem *child); TreeItem *child(int row); - inline QList treeChildren() const { return m_children; } + inline QList treeChildren() const + { + return m_children; + } int childCount() const; int columnCount() const; QVariant data(int column = 1) const; - QString description() { return m_description; } - void setDescription(QString d) { // Split around 40 characters - int idx = d.indexOf(" ",40); - d.insert(idx,QString("
")); + QString description() + { + return m_description; + } + void setDescription(QString d) // Split around 40 characters + { + int idx = d.indexOf(" ", 40); + + d.insert(idx, QString("
")); d.remove("@Ref", Qt::CaseInsensitive); m_description = d; } @@ -63,27 +70,47 @@ public: // other columns are initialized in constructor virtual void setData(QVariant value, int column = 1); int row() const; - TreeItem *parent() { return m_parent; } - void setParentTree(TreeItem *parent) { m_parent = parent; } - inline virtual bool isEditable() { return false; } + TreeItem *parent() + { + return m_parent; + } + void setParentTree(TreeItem *parent) + { + m_parent = parent; + } + inline virtual bool isEditable() + { + return false; + } virtual void update(); virtual void apply(); - inline bool highlighted() { return m_highlight; } + inline bool highlighted() + { + return m_highlight; + } void setHighlight(bool highlight); void setActive(bool highlight); - inline bool changed() { return m_changed; } - inline void setChanged(bool changed) { m_changed = changed; if(changed) emit updateHighlight(this); } + inline bool changed() + { + return m_changed; + } + inline void setChanged(bool changed) + { + m_changed = changed; if (changed) { + emit updateHighlight(this); + } + } signals: - void updateHighlight(TreeItem*); + void updateHighlight(TreeItem *); private slots: void removeHighlight(); private: - QList m_children; + QList m_children; // m_data contains: [0] property name, [1] value, [2] unit QList m_data; QString m_description; @@ -95,20 +122,30 @@ public: private: }; -class TopTreeItem : public TreeItem -{ -Q_OBJECT +class TopTreeItem : public TreeItem { + Q_OBJECT public: - TopTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) { } - TopTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { } + TopTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) {} + TopTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) {} - QList objIds() { return m_objIds; } - void addObjId(quint32 objId) { m_objIds.append(objId); } - void insertObjId(int index, quint32 objId) { m_objIds.insert(index, objId); } - int nameIndex(QString name) { + QList objIds() + { + return m_objIds; + } + void addObjId(quint32 objId) + { + m_objIds.append(objId); + } + void insertObjId(int index, quint32 objId) + { + m_objIds.insert(index, objId); + } + int nameIndex(QString name) + { for (int i = 0; i < childCount(); ++i) { - if (name < child(i)->data(0).toString()) + if (name < child(i)->data(0).toString()) { return i; + } } return childCount(); } @@ -117,72 +154,97 @@ private: QList m_objIds; }; -class ObjectTreeItem : public TreeItem -{ -Q_OBJECT +class ObjectTreeItem : public TreeItem { + Q_OBJECT public: ObjectTreeItem(const QList &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_obj(0) { } + TreeItem(data, parent), m_obj(0) {} ObjectTreeItem(const QVariant &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_obj(0) { } - void setObject(UAVObject *obj) { m_obj = obj; setDescription(obj->getDescription()); } - inline UAVObject *object() { return m_obj; } + TreeItem(data, parent), m_obj(0) {} + void setObject(UAVObject *obj) + { + m_obj = obj; setDescription(obj->getDescription()); + } + inline UAVObject *object() + { + return m_obj; + } private: UAVObject *m_obj; }; -class MetaObjectTreeItem : public ObjectTreeItem -{ -Q_OBJECT +class MetaObjectTreeItem : public ObjectTreeItem { + Q_OBJECT public: MetaObjectTreeItem(UAVObject *obj, const QList &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { setObject(obj); } + ObjectTreeItem(data, parent) + { + setObject(obj); + } MetaObjectTreeItem(UAVObject *obj, const QVariant &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { setObject(obj); } + ObjectTreeItem(data, parent) + { + setObject(obj); + } }; -class DataObjectTreeItem : public ObjectTreeItem -{ -Q_OBJECT +class DataObjectTreeItem : public ObjectTreeItem { + Q_OBJECT public: DataObjectTreeItem(const QList &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { } + ObjectTreeItem(data, parent) {} DataObjectTreeItem(const QVariant &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { } - virtual void apply() { - foreach(TreeItem *child, treeChildren()) { - MetaObjectTreeItem *metaChild = dynamic_cast(child); - if (!metaChild) + ObjectTreeItem(data, parent) {} + virtual void apply() + { + foreach(TreeItem * child, treeChildren()) { + MetaObjectTreeItem *metaChild = dynamic_cast(child); + + if (!metaChild) { child->apply(); + } } } - virtual void update() { - foreach(TreeItem *child, treeChildren()) { - MetaObjectTreeItem *metaChild = dynamic_cast(child); - if (!metaChild) + virtual void update() + { + foreach(TreeItem * child, treeChildren()) { + MetaObjectTreeItem *metaChild = dynamic_cast(child); + + if (!metaChild) { child->update(); + } } } }; -class InstanceTreeItem : public DataObjectTreeItem -{ -Q_OBJECT +class InstanceTreeItem : public DataObjectTreeItem { + Q_OBJECT public: InstanceTreeItem(UAVObject *obj, const QList &data, TreeItem *parent = 0) : - DataObjectTreeItem(data, parent) { setObject(obj); } + DataObjectTreeItem(data, parent) + { + setObject(obj); + } InstanceTreeItem(UAVObject *obj, const QVariant &data, TreeItem *parent = 0) : - DataObjectTreeItem(data, parent) { setObject(obj); } - virtual void apply() { TreeItem::apply(); } - virtual void update() { TreeItem::update(); } + DataObjectTreeItem(data, parent) + { + setObject(obj); + } + virtual void apply() + { + TreeItem::apply(); + } + virtual void update() + { + TreeItem::update(); + } }; -class ArrayFieldTreeItem : public TreeItem -{ -Q_OBJECT +class ArrayFieldTreeItem : public TreeItem { + Q_OBJECT public: - ArrayFieldTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) { } - ArrayFieldTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { } + ArrayFieldTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) {} + ArrayFieldTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) {} }; #endif // TREEITEM_H diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadget.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdgadget.cpp index aee36b74e..6d91e3c88 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadget.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -30,10 +30,9 @@ #include "pfdgadgetconfiguration.h" PFDGadget::PFDGadget(QString classId, PFDGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} PFDGadget::~PFDGadget() { @@ -41,14 +40,15 @@ PFDGadget::~PFDGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void PFDGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void PFDGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - PFDGadgetConfiguration *m = qobject_cast(config); + PFDGadgetConfiguration *m = qobject_cast(config); + m_widget->setHqFonts(m->getHqFonts()); m_widget->setDialFile(m->dialFile()); m_widget->enableOpenGL(m->useOpenGL()); diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadget.h b/ground/openpilotgcs/src/plugins/pfd/pfdgadget.h index 46864ee4a..c161aa97f 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadget.h +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,15 +38,17 @@ class PFDGadgetWidget; using namespace Core; -class PFDGadget : public Core::IUAVGadget -{ +class PFDGadget : public Core::IUAVGadget { Q_OBJECT public: PFDGadget(QString classId, PFDGadgetWidget *widget, QWidget *parent = 0); ~PFDGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: PFDGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.cpp index 0dae38c58..f05235409 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,18 +32,18 @@ * Loads a saved configuration or defaults if non exist. * */ -PFDGadgetConfiguration::PFDGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +PFDGadgetConfiguration::PFDGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_defaultDial("Unknown"), beSmooth(true) { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { QString dialFile = qSettings->value("dialFile").toString(); useOpenGLFlag = qSettings->value("useOpenGLFlag").toBool(); hqFonts = qSettings->value("hqFonts").toBool(); beSmooth = qSettings->value("beSmooth").toBool(); - m_defaultDial=Utils::PathUtils().InsertDataPath(dialFile); + m_defaultDial = Utils::PathUtils().InsertDataPath(dialFile); } } @@ -54,9 +54,10 @@ PFDGadgetConfiguration::PFDGadgetConfiguration(QString classId, QSettings* qSett IUAVGadgetConfiguration *PFDGadgetConfiguration::clone() { PFDGadgetConfiguration *m = new PFDGadgetConfiguration(this->classId()); - m->m_defaultDial=m_defaultDial; + + m->m_defaultDial = m_defaultDial; m->useOpenGLFlag = useOpenGLFlag; - m->hqFonts = hqFonts; + m->hqFonts = hqFonts; m->beSmooth = beSmooth; return m; } @@ -65,8 +66,10 @@ IUAVGadgetConfiguration *PFDGadgetConfiguration::clone() * Saves a configuration. * */ -void PFDGadgetConfiguration::saveConfig(QSettings* qSettings) const { +void PFDGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ QString dialFile = Utils::PathUtils().RemoveDataPath(m_defaultDial); + qSettings->setValue("dialFile", dialFile); qSettings->setValue("useOpenGLFlag", useOpenGLFlag); qSettings->setValue("hqFonts", hqFonts); diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.h index 3014a0722..9f3152c63 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetconfiguration.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,25 +32,48 @@ using namespace Core; -class PFDGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT +class PFDGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit PFDGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit PFDGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - //set dial configuration functions - void setDialFile(QString dialFile){m_defaultDial=dialFile;} - void setUseOpenGL(bool flag) { useOpenGLFlag = flag; } - void setHqFonts(bool flag) { hqFonts = flag; } - void setBeSmooth(bool flag) { beSmooth = flag;} + // set dial configuration functions + void setDialFile(QString dialFile) + { + m_defaultDial = dialFile; + } + void setUseOpenGL(bool flag) + { + useOpenGLFlag = flag; + } + void setHqFonts(bool flag) + { + hqFonts = flag; + } + void setBeSmooth(bool flag) + { + beSmooth = flag; + } - //get dial configuration functions - QString dialFile() {return m_defaultDial;} - bool useOpenGL() { return useOpenGLFlag; } - bool getHqFonts() { return hqFonts; } - bool getBeSmooth() { return beSmooth; } + // get dial configuration functions + QString dialFile() + { + return m_defaultDial; + } + bool useOpenGL() + { + return useOpenGLFlag; + } + bool getHqFonts() + { + return hqFonts; + } + bool getBeSmooth() + { + return beSmooth; + } - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.cpp index 5bc51c775..1e3725189 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,29 +32,27 @@ #include PFDGadgetFactory::PFDGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("PFDGadget"), - tr("Primary Flight Display"), - parent) -{ -} + IUAVGadgetFactory(QString("PFDGadget"), + tr("Primary Flight Display"), + parent) +{} PFDGadgetFactory::~PFDGadgetFactory() -{ -} +{} -Core::IUAVGadget* PFDGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *PFDGadgetFactory::createGadget(QWidget *parent) { - PFDGadgetWidget* gadgetWidget = new PFDGadgetWidget(parent); + PFDGadgetWidget *gadgetWidget = new PFDGadgetWidget(parent); + return new PFDGadget(QString("PFDGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *PFDGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *PFDGadgetFactory::createConfiguration(QSettings *qSettings) { return new PFDGadgetConfiguration(QString("PFDGadget"), qSettings); } IOptionsPage *PFDGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new PFDGadgetOptionsPage(qobject_cast(config)); + return new PFDGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.h b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.h index 8bc97e9a9..be7867494 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class PFDGadgetFactory : public IUAVGadgetFactory -{ +class PFDGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: PFDGadgetFactory(QObject *parent = 0); ~PFDGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.cpp index e8dd3378e..3af712dd4 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,24 +38,22 @@ #include PFDGadgetOptionsPage::PFDGadgetOptionsPage(PFDGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *PFDGadgetOptionsPage::createPage(QWidget *parent) { Q_UNUSED(parent); options_page = new Ui::PFDGadgetOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget; - //main layout + // main layout options_page->setupUi(optionsPageWidget); - // Restore the contents from the settings: options_page->svgSourceFile->setExpectedKind(Utils::PathChooser::File); options_page->svgSourceFile->setPromptDialogFilter(tr("SVG image (*.svg)")); @@ -83,7 +81,5 @@ void PFDGadgetOptionsPage::apply() } - void PFDGadgetOptionsPage::finish() -{ -} +{} diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.h index bf30f19c1..83ea51ded 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -40,14 +40,13 @@ class IUAVGadgetConfiguration; class PFDGadgetConfiguration; namespace Ui { - class PFDGadgetOptionsPage; +class PFDGadgetOptionsPage; } using namespace Core; -class PFDGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class PFDGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit PFDGadgetOptionsPage(PFDGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp index edd07ae44..bbd034193 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -36,36 +36,35 @@ PFDGadgetWidget::PFDGadgetWidget(QWidget *parent) : QGraphicsView(parent) { - - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setScene(new QGraphicsScene(this)); -// setRenderHint(QPainter::SmoothPixmapTransform); +// setRenderHint(QPainter::SmoothPixmapTransform); setViewportUpdateMode(QGraphicsView::FullViewportUpdate); - //setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); - //setRenderHints(QPainter::TextAntialiasing); - //setRenderHints(QPainter::HighQualityAntialiasing); + // setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); + // setRenderHints(QPainter::TextAntialiasing); + // setRenderHints(QPainter::HighQualityAntialiasing); - m_renderer = new QSvgRenderer(); + m_renderer = new QSvgRenderer(); - attitudeObj = NULL; - headingObj = NULL; - gcsBatteryObj = NULL; + attitudeObj = NULL; + headingObj = NULL; + gcsBatteryObj = NULL; gpsObj = NULL; - compassBandWidth = 0; + compassBandWidth = 0; pfdError = true; hqFonts = false; - rollTarget = 0; + rollTarget = 0; rollValue = 0; - pitchTarget = 0; - pitchValue = 0; - headingTarget = 0; - headingValue = 0; + pitchTarget = 0; + pitchValue = 0; + headingTarget = 0; + headingValue = 0; groundspeedTarget = 0; - groundspeedValue = 0; - altitudeTarget = 0; - altitudeValue = 0; + groundspeedValue = 0; + altitudeTarget = 0; + altitudeValue = 0; // This timer mechanism makes needles rotate smoothly connect(&dialTimer, SIGNAL(timeout()), this, SLOT(moveNeedles())); @@ -73,8 +72,6 @@ PFDGadgetWidget::PFDGadgetWidget(QWidget *parent) : QGraphicsView(parent) connect(&skyDialTimer, SIGNAL(timeout()), this, SLOT(moveSky())); skyDialTimer.start(30); - - } PFDGadgetWidget::~PFDGadgetWidget() @@ -85,16 +82,19 @@ PFDGadgetWidget::~PFDGadgetWidget() void PFDGadgetWidget::setToolTipPrivate() { - static qint32 updateRate=0; - UAVObject::Metadata mdata=attitudeObj->getMetadata(); - if(mdata.flightTelemetryUpdatePeriod!=updateRate) - this->setToolTip("Current refresh rate:"+QString::number(mdata.flightTelemetryUpdatePeriod)+" miliseconds"+"\nIf you want to change it please edit the AttitudeActual metadata on the object browser."); + static qint32 updateRate = 0; + UAVObject::Metadata mdata = attitudeObj->getMetadata(); + + if (mdata.flightTelemetryUpdatePeriod != updateRate) { + this->setToolTip("Current refresh rate:" + QString::number(mdata.flightTelemetryUpdatePeriod) + " miliseconds" + "\nIf you want to change it please edit the AttitudeActual metadata on the object browser."); + } } /*! - \brief Enables/Disables OpenGL - */ -void PFDGadgetWidget::enableOpenGL(bool flag) { + \brief Enables/Disables OpenGL + */ +void PFDGadgetWidget::enableOpenGL(bool flag) +{ if (flag) { setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); } else { @@ -103,102 +103,110 @@ void PFDGadgetWidget::enableOpenGL(bool flag) { } /*! - \brief Connects the widget to the relevant UAVObjects + \brief Connects the widget to the relevant UAVObjects - Should only be called after the PFD artwork is loaded. - We want: AttitudeActual, FlightBattery, Location. + Should only be called after the PFD artwork is loaded. + We want: AttitudeActual, FlightBattery, Location. - */ -void PFDGadgetWidget::connectNeedles() { - if (attitudeObj != NULL) - disconnect(attitudeObj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updateAttitude(UAVObject*))); + */ +void PFDGadgetWidget::connectNeedles() +{ + if (attitudeObj != NULL) { + disconnect(attitudeObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateAttitude(UAVObject *))); + } - if (headingObj != NULL) - disconnect(headingObj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updateHeading(UAVObject*))); + if (headingObj != NULL) { + disconnect(headingObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateHeading(UAVObject *))); + } - if (gcsBatteryObj != NULL) - disconnect(gcsBatteryObj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updateBattery(UAVObject*))); + if (gcsBatteryObj != NULL) { + disconnect(gcsBatteryObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateBattery(UAVObject *))); + } - if (gpsObj != NULL) - disconnect(gpsObj,SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateGPS(UAVObject*))); + if (gpsObj != NULL) { + disconnect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGPS(UAVObject *))); + } // Safeguard: if artwork did not load properly, don't go further - if (pfdError) - return; + if (pfdError) { + return; + } ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - airspeedObj = dynamic_cast(objManager->getObject("AirspeedActual")); - if (airspeedObj != NULL ) { - connect(airspeedObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateAirspeed(UAVObject*))); + airspeedObj = dynamic_cast(objManager->getObject("AirspeedActual")); + if (airspeedObj != NULL) { + connect(airspeedObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateAirspeed(UAVObject *))); } else { - qDebug() << "Error: Object is unknown (AirspeedActual)."; + qDebug() << "Error: Object is unknown (AirspeedActual)."; } - groundspeedObj = dynamic_cast(objManager->getObject("VelocityActual")); - if (groundspeedObj != NULL ) { - connect(groundspeedObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateGroundspeed(UAVObject*))); + groundspeedObj = dynamic_cast(objManager->getObject("VelocityActual")); + if (groundspeedObj != NULL) { + connect(groundspeedObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGroundspeed(UAVObject *))); } else { - qDebug() << "Error: Object is unknown (VelocityActual)."; + qDebug() << "Error: Object is unknown (VelocityActual)."; } - altitudeObj = dynamic_cast(objManager->getObject("PositionActual")); - if (altitudeObj != NULL ) { - connect(altitudeObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateAltitude(UAVObject*))); + altitudeObj = dynamic_cast(objManager->getObject("PositionActual")); + if (altitudeObj != NULL) { + connect(altitudeObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateAltitude(UAVObject *))); } else { - qDebug() << "Error: Object is unknown (PositionActual)."; - } - - attitudeObj = dynamic_cast(objManager->getObject("AttitudeActual")); - if (attitudeObj != NULL ) { - connect(attitudeObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateAttitude(UAVObject*))); - } else { - qDebug() << "Error: Object is unknown (AttitudeActual)."; - } - - headingObj = dynamic_cast(objManager->getObject("PositionActual")); - if (headingObj != NULL ) { - connect(headingObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateHeading(UAVObject*))); - } else { qDebug() << "Error: Object is unknown (PositionActual)."; - } - - if (gcsGPSStats) { - gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); - if (gpsObj != NULL) { - connect(gpsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateGPS(UAVObject*))); - } else { - qDebug() << "Error: Object is unknown (GPSPosition)."; - } - } - - if (gcsTelemetryArrow || gcsTelemetryStats ) { - // Only register if the PFD wants link stats/status - gcsTelemetryObj = dynamic_cast(objManager->getObject("GCSTelemetryStats")); - if (gcsTelemetryObj != NULL ) { - connect(gcsTelemetryObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateLinkStatus(UAVObject*))); - } else { - qDebug() << "Error: Object is unknown (GCSTelemetryStats)."; - } } - if (gcsBatteryStats) { // Only register if the PFD wants battery display - gcsBatteryObj = dynamic_cast(objManager->getObject("FlightBatteryState")); - if (gcsBatteryObj != NULL ) { - connect(gcsBatteryObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateBattery(UAVObject*))); - } else { + attitudeObj = dynamic_cast(objManager->getObject("AttitudeActual")); + if (attitudeObj != NULL) { + connect(attitudeObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateAttitude(UAVObject *))); + } else { + qDebug() << "Error: Object is unknown (AttitudeActual)."; + } + + headingObj = dynamic_cast(objManager->getObject("PositionActual")); + if (headingObj != NULL) { + connect(headingObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateHeading(UAVObject *))); + } else { + qDebug() << "Error: Object is unknown (PositionActual)."; + } + + if (gcsGPSStats) { + gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + if (gpsObj != NULL) { + connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGPS(UAVObject *))); + } else { + qDebug() << "Error: Object is unknown (GPSPosition)."; + } + } + + if (gcsTelemetryArrow || gcsTelemetryStats) { + // Only register if the PFD wants link stats/status + gcsTelemetryObj = dynamic_cast(objManager->getObject("GCSTelemetryStats")); + if (gcsTelemetryObj != NULL) { + connect(gcsTelemetryObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateLinkStatus(UAVObject *))); + } else { + qDebug() << "Error: Object is unknown (GCSTelemetryStats)."; + } + } + + if (gcsBatteryStats) { // Only register if the PFD wants battery display + gcsBatteryObj = dynamic_cast(objManager->getObject("FlightBatteryState")); + if (gcsBatteryObj != NULL) { + connect(gcsBatteryObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateBattery(UAVObject *))); + } else { qDebug() << "Error: Object is unknown (FlightBatteryState)."; - } - } + } + } } /*! - \brief Updates the GPS stats - */ -void PFDGadgetWidget::updateGPS(UAVObject *object1) { - UAVObjectField* field = object1->getField(QString("Satellites")); - UAVObjectField* field1 = object1->getField(QString("PDOP")); + \brief Updates the GPS stats + */ +void PFDGadgetWidget::updateGPS(UAVObject *object1) +{ + UAVObjectField *field = object1->getField(QString("Satellites")); + UAVObjectField *field1 = object1->getField(QString("PDOP")); + if (field && field1) { QString s = QString("GPS: ") + field->getValue().toString() + "\nPDP: " + field1->getValue().toString(); @@ -210,59 +218,64 @@ void PFDGadgetWidget::updateGPS(UAVObject *object1) { } /*! - \brief Updates the link stats + \brief Updates the link stats */ -void PFDGadgetWidget::updateLinkStatus(UAVObject *object1) { +void PFDGadgetWidget::updateLinkStatus(UAVObject *object1) +{ // TODO: find a way to avoid updating the graphics if the value - // has not changed since the last update + // has not changed since the last update // Double check that the field exists: - QString st = QString("Status"); + QString st = QString("Status"); QString tdr = QString("TxDataRate"); QString rdr = QString("RxDataRate"); - UAVObjectField* field = object1->getField(st); - UAVObjectField* field2 = object1->getField(tdr); - UAVObjectField* field3 = object1->getField(rdr); + UAVObjectField *field = object1->getField(st); + UAVObjectField *field2 = object1->getField(tdr); + UAVObjectField *field3 = object1->getField(rdr); + if (field && field2 && field3) { - QString s = field->getValue().toString(); + QString s = field->getValue().toString(); if (m_renderer->elementExists("gcstelemetry-" + s) && gcsTelemetryArrow) { - gcsTelemetryArrow->setElementId("gcstelemetry-" + s); - } else { // Safeguard - gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); - } + gcsTelemetryArrow->setElementId("gcstelemetry-" + s); + } else { // Safeguard + gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); + } double v1 = field2->getDouble(); double v2 = field3->getDouble(); - s.sprintf("%.0f/%.0f",v1,v2); - if (gcsTelemetryStats) gcsTelemetryStats->setPlainText(s); + s.sprintf("%.0f/%.0f", v1, v2); + if (gcsTelemetryStats) { + gcsTelemetryStats->setPlainText(s); + } } else { qDebug() << "UpdateLinkStatus: Wrong field, maybe an issue with object disconnection ?"; } } /*! - \brief Reads the updated attitude and computes the new display position + \brief Reads the updated attitude and computes the new display position - Resolution is 1 degree roll & 1/7.5 degree pitch. - */ -void PFDGadgetWidget::updateAttitude(UAVObject *object1) { + Resolution is 1 degree roll & 1/7.5 degree pitch. + */ +void PFDGadgetWidget::updateAttitude(UAVObject *object1) +{ setToolTipPrivate(); - UAVObjectField * rollField = object1->getField(QString("Roll")); - UAVObjectField * yawField = object1->getField(QString("Yaw")); - UAVObjectField * pitchField = object1->getField(QString("Pitch")); - if(rollField && yawField && pitchField) { + UAVObjectField *rollField = object1->getField(QString("Roll")); + UAVObjectField *yawField = object1->getField(QString("Yaw")); + UAVObjectField *pitchField = object1->getField(QString("Pitch")); + if (rollField && yawField && pitchField) { // These factors assume some things about the PFD SVG, namely: // - Roll, Pitch and Heading value in degrees // - Pitch lines are 300px high for a +20/-20 range, which means - // 7.5 pixels per pitch degree. + // 7.5 pixels per pitch degree. // TODO: loosen this constraint and only require a +/- 20 deg range, - // and compute the height from the SVG element. + // and compute the height from the SVG element. // Also: keep the integer value only, to avoid unnecessary redraws - rollTarget = -floor(rollField->getDouble()*10)/10; + rollTarget = -floor(rollField->getDouble() * 10) / 10; if ((rollTarget - rollValue) > 180) { rollValue += 360; } else if (((rollTarget - rollValue) < -180)) { rollValue -= 360; } - pitchTarget = floor(pitchField->getDouble()*7.5); + pitchTarget = floor(pitchField->getDouble() * 7.5); // These factors assume some things about the PFD SVG, namely: // - Heading value in degrees @@ -271,51 +284,55 @@ void PFDGadgetWidget::updateAttitude(UAVObject *object1) { // Corvus Corax: "If you want a smooth transition between two angles, It is usually solved that by substracting // one from another, and if the result is >180 or <-180 I substract (respectively add) 360 degrees // to it. That way you always get the "shorter difference" to turn in." - double fac = compassBandWidth/540; - headingTarget = yawField->getDouble()*(-fac); - if (headingTarget != headingTarget) + double fac = compassBandWidth / 540; + headingTarget = yawField->getDouble() * (-fac); + if (headingTarget != headingTarget) { headingTarget = headingValue; // NaN checking. - if ((headingValue - headingTarget)/fac > 180) { - headingTarget += 360*fac; - } else if (((headingValue - headingTarget)/fac < -180)) { - headingTarget -= 360*fac; } - headingTarget = floor(headingTarget*10)/10; // Avoid stupid redraws + if ((headingValue - headingTarget) / fac > 180) { + headingTarget += 360 * fac; + } else if (((headingValue - headingTarget) / fac < -180)) { + headingTarget -= 360 * fac; + } + headingTarget = floor(headingTarget * 10) / 10; // Avoid stupid redraws - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); // Rearm the dial Timer which might be stopped. - + } } else { qDebug() << "Unable to get one of the fields for attitude update"; } } /*! - \brief Updates the compass reading and speed dial. + \brief Updates the compass reading and speed dial. - This also updates speed & altitude according to PositionActual data. + This also updates speed & altitude according to PositionActual data. Note: the speed dial shows the ground speed at the moment, because there is no airspeed by default. Should become configurable in a future gadget release (TODO) - */ -void PFDGadgetWidget::updateHeading(UAVObject *object) { + */ +void PFDGadgetWidget::updateHeading(UAVObject *object) +{ Q_UNUSED(object); } /*! - \brief Called by updates to @PositionActual to compute groundspeed from velocity - */ -void PFDGadgetWidget::updateGroundspeed(UAVObject *object) { - UAVObjectField* northField = object->getField("North"); - UAVObjectField* eastField = object->getField("East"); + \brief Called by updates to @PositionActual to compute groundspeed from velocity + */ +void PFDGadgetWidget::updateGroundspeed(UAVObject *object) +{ + UAVObjectField *northField = object->getField("North"); + UAVObjectField *eastField = object->getField("East"); + if (northField && eastField) { - double val = floor(sqrt(pow(northField->getDouble(),2) + pow(eastField->getDouble(),2))*10)/10; - groundspeedTarget = 3.6*val*speedScaleHeight/30; + double val = floor(sqrt(pow(northField->getDouble(), 2) + pow(eastField->getDouble(), 2)) * 10) / 10; + groundspeedTarget = 3.6 * val * speedScaleHeight / 30; - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); // Rearm the dial Timer which might be stopped. - + } } else { qDebug() << "UpdateHeading: Wrong field, maybe an issue with object disconnection ?"; } @@ -323,32 +340,36 @@ void PFDGadgetWidget::updateGroundspeed(UAVObject *object) { /*! - \brief Called by updates to @AirspeedActual - */ -void PFDGadgetWidget::updateAirspeed(UAVObject *object) { - UAVObjectField* airspeedField = object->getField("CalibratedAirspeed"); + \brief Called by updates to @AirspeedActual + */ +void PFDGadgetWidget::updateAirspeed(UAVObject *object) +{ + UAVObjectField *airspeedField = object->getField("CalibratedAirspeed"); + if (airspeedField) { airspeedTarget = airspeedField->getDouble(); - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); // Rearm the dial Timer which might be stopped. - + } } else { qDebug() << "UpdateHeading: Wrong field, maybe an issue with object disconnection ?"; } } /*! - \brief Called by the @ref PositionActual updates to show altitude - */ -void PFDGadgetWidget::updateAltitude(UAVObject *object) { - UAVObjectField* downField = object->getField("Down"); + \brief Called by the @ref PositionActual updates to show altitude + */ +void PFDGadgetWidget::updateAltitude(UAVObject *object) +{ + UAVObjectField *downField = object->getField("Down"); + if (downField) { altitudeTarget = -downField->getDouble(); - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); // Rearm the dial Timer which might be stopped. - + } } else { qDebug() << "Unable to get field for altitude update. Obj: " << object->getName(); } @@ -356,22 +377,24 @@ void PFDGadgetWidget::updateAltitude(UAVObject *object) { /*! - \brief Called by the UAVObject which got updated - */ -void PFDGadgetWidget::updateBattery(UAVObject *object1) { + \brief Called by the UAVObject which got updated + */ +void PFDGadgetWidget::updateBattery(UAVObject *object1) +{ // Double check that the field exists: QString voltage = QString("Voltage"); QString current = QString("Current"); - QString energy = QString("ConsumedEnergy"); - UAVObjectField* field = object1->getField(voltage); - UAVObjectField* field2 = object1->getField(current); - UAVObjectField* field3 = object1->getField(energy); + QString energy = QString("ConsumedEnergy"); + UAVObjectField *field = object1->getField(voltage); + UAVObjectField *field2 = object1->getField(current); + UAVObjectField *field3 = object1->getField(energy); + if (field && field2 && field3) { - QString s = QString(); - double v0 = field->getDouble(); + QString s = QString(); + double v0 = field->getDouble(); double v1 = field2->getDouble(); double v2 = field3->getDouble(); - s.sprintf("%.2fV\n%.2fA\n%.0fmAh",v0,v1,v2); + s.sprintf("%.2fV\n%.2fA\n%.0fmAh", v0, v1, v2); if (s != batString) { gcsBatteryStats->setPlainText(s); batString = s; @@ -379,21 +402,20 @@ void PFDGadgetWidget::updateBattery(UAVObject *object1) { } else { qDebug() << "UpdateBattery: Wrong field, maybe an issue with object disconnection ?"; } - } /*! - \brief Sets up the PFD from the SVG master file. + \brief Sets up the PFD from the SVG master file. - Initializes the display, and does all the one-time calculations. - */ + Initializes the display, and does all the one-time calculations. + */ void PFDGadgetWidget::setDialFile(QString dfn) { - QGraphicsScene *l_scene = scene(); - setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); - if (QFile::exists(dfn) && m_renderer->load(dfn) && m_renderer->isValid()) - { + QGraphicsScene *l_scene = scene(); + + setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); + if (QFile::exists(dfn) && m_renderer->load(dfn) && m_renderer->isValid()) { /* The PFD element IDs are fixed, not like with the analog dial. - Background: background - Foreground: foreground (contains all fixed elements, including plane) @@ -416,351 +438,369 @@ void PFDGadgetWidget::setDialFile(QString dfn) - GPS status text: gps-txt - Battery stats: battery-txt */ - l_scene->clear(); // Deletes all items contained in the scene as well. - m_background = new CachedSvgItem(); - // All other items will be clipped to the shape of the background - m_background->setFlags(QGraphicsItem::ItemClipsChildrenToShape| - QGraphicsItem::ItemClipsToShape); - m_background->setSharedRenderer(m_renderer); - m_background->setElementId("background"); - l_scene->addItem(m_background); + l_scene->clear(); // Deletes all items contained in the scene as well. + m_background = new CachedSvgItem(); + // All other items will be clipped to the shape of the background + m_background->setFlags(QGraphicsItem::ItemClipsChildrenToShape | + QGraphicsItem::ItemClipsToShape); + m_background->setSharedRenderer(m_renderer); + m_background->setElementId("background"); + l_scene->addItem(m_background); - m_world = new CachedSvgItem(); - m_world->setParentItem(m_background); - m_world->setSharedRenderer(m_renderer); - m_world->setElementId("world"); - l_scene->addItem(m_world); + m_world = new CachedSvgItem(); + m_world->setParentItem(m_background); + m_world->setSharedRenderer(m_renderer); + m_world->setElementId("world"); + l_scene->addItem(m_world); - // red Roll scale: rollscale - m_rollscale = new CachedSvgItem(); - m_rollscale->setSharedRenderer(m_renderer); - m_rollscale->setElementId("rollscale"); - l_scene->addItem(m_rollscale); + // red Roll scale: rollscale + m_rollscale = new CachedSvgItem(); + m_rollscale->setSharedRenderer(m_renderer); + m_rollscale->setElementId("rollscale"); + l_scene->addItem(m_rollscale); - // Home point: - m_homewaypoint = new CachedSvgItem(); - // Next point: - m_nextwaypoint = new CachedSvgItem(); - // Home point bearing: - m_homepointbearing = new CachedSvgItem(); - // Next point bearing: - m_nextpointbearing = new CachedSvgItem(); + // Home point: + m_homewaypoint = new CachedSvgItem(); + // Next point: + m_nextwaypoint = new CachedSvgItem(); + // Home point bearing: + m_homepointbearing = new CachedSvgItem(); + // Next point bearing: + m_nextpointbearing = new CachedSvgItem(); - QGraphicsSvgItem *m_foreground = new CachedSvgItem(); - m_foreground->setParentItem(m_background); - m_foreground->setSharedRenderer(m_renderer); - m_foreground->setElementId("foreground"); - l_scene->addItem(m_foreground); + QGraphicsSvgItem *m_foreground = new CachedSvgItem(); + m_foreground->setParentItem(m_background); + m_foreground->setSharedRenderer(m_renderer); + m_foreground->setElementId("foreground"); + l_scene->addItem(m_foreground); - //////////////////// - // Compass - //////////////////// - // Get the default location of the Compass: - QMatrix compassMatrix = m_renderer->matrixForElement("compass"); - qreal startX = compassMatrix.mapRect(m_renderer->boundsOnElement("compass")).x(); - qreal startY = compassMatrix.mapRect(m_renderer->boundsOnElement("compass")).y(); - // Then once we have the initial location, we can put it - // into a QGraphicsSvgItem which we will display at the same - // place: we do this so that the heading scale can be clipped to - // the compass dial region. - m_compass = new CachedSvgItem(); - m_compass->setSharedRenderer(m_renderer); - m_compass->setElementId("compass"); - m_compass->setFlags(QGraphicsItem::ItemClipsChildrenToShape| + //////////////////// + // Compass + //////////////////// + // Get the default location of the Compass: + QMatrix compassMatrix = m_renderer->matrixForElement("compass"); + qreal startX = compassMatrix.mapRect(m_renderer->boundsOnElement("compass")).x(); + qreal startY = compassMatrix.mapRect(m_renderer->boundsOnElement("compass")).y(); + // Then once we have the initial location, we can put it + // into a QGraphicsSvgItem which we will display at the same + // place: we do this so that the heading scale can be clipped to + // the compass dial region. + m_compass = new CachedSvgItem(); + m_compass->setSharedRenderer(m_renderer); + m_compass->setElementId("compass"); + m_compass->setFlags(QGraphicsItem::ItemClipsChildrenToShape | + QGraphicsItem::ItemClipsToShape); + l_scene->addItem(m_compass); + QTransform matrix; + matrix.translate(startX, startY); + m_compass->setTransform(matrix, false); + + // Now place the compass scale inside: + m_compassband = new CachedSvgItem(); + m_compassband->setSharedRenderer(m_renderer); + m_compassband->setElementId("compass-band"); + m_compassband->setParentItem(m_compass); + l_scene->addItem(m_compassband); + matrix.reset(); + // Note: the compass band has to be a path, which means all text elements have to be + // converted, ortherwise boundsOnElement does not compute the height correctly + // if the highest element is a text element. This is a Qt Bug as far as I can tell. + + // compass-scale is the while bottom line inside the band: using the band's width + // includes half the width of the letters, which causes errors: + compassBandWidth = m_renderer->boundsOnElement("compass-scale").width(); + + //////////////////// + // Speed + //////////////////// + // Speedometer on the left hand: + // + compassMatrix = m_renderer->matrixForElement("speed-bg"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).y(); + QGraphicsSvgItem *verticalbg = new CachedSvgItem(); + verticalbg->setSharedRenderer(m_renderer); + verticalbg->setElementId("speed-bg"); + verticalbg->setFlags(QGraphicsItem::ItemClipsChildrenToShape | QGraphicsItem::ItemClipsToShape); - l_scene->addItem(m_compass); - QTransform matrix; - matrix.translate(startX,startY); - m_compass->setTransform(matrix,false); + l_scene->addItem(verticalbg); + matrix.reset(); + matrix.translate(startX, startY); + verticalbg->setTransform(matrix, false); - // Now place the compass scale inside: - m_compassband = new CachedSvgItem(); - m_compassband->setSharedRenderer(m_renderer); - m_compassband->setElementId("compass-band"); - m_compassband->setParentItem(m_compass); - l_scene->addItem(m_compassband); - matrix.reset(); - // Note: the compass band has to be a path, which means all text elements have to be - // converted, ortherwise boundsOnElement does not compute the height correctly - // if the highest element is a text element. This is a Qt Bug as far as I can tell. + // Note: speed-scale should contain exactly 6 major ticks + // for 30km/h + m_speedscale = new QGraphicsItemGroup(); + m_speedscale->setParentItem(verticalbg); - // compass-scale is the while bottom line inside the band: using the band's width - // includes half the width of the letters, which causes errors: - compassBandWidth = m_renderer->boundsOnElement("compass-scale").width(); + QGraphicsSvgItem *speedscalelines = new CachedSvgItem(); + speedscalelines->setSharedRenderer(m_renderer); + speedscalelines->setElementId("speed-scale"); + speedScaleHeight = m_renderer->matrixForElement("speed-scale").mapRect( + m_renderer->boundsOnElement("speed-scale")).height(); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).width(); + startX -= m_renderer->matrixForElement("speed-scale").mapRect( + m_renderer->boundsOnElement("speed-scale")).width(); + matrix.reset(); + matrix.translate(startX, 0); + speedscalelines->setTransform(matrix, false); + // Quick way to reposition the item before putting it in the group: + speedscalelines->setParentItem(verticalbg); + m_speedscale->addToGroup(speedscalelines); // (reparents the item) - //////////////////// - // Speed - //////////////////// - // Speedometer on the left hand: - // - compassMatrix = m_renderer->matrixForElement("speed-bg"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).y(); - QGraphicsSvgItem *verticalbg = new CachedSvgItem(); - verticalbg->setSharedRenderer(m_renderer); - verticalbg->setElementId("speed-bg"); - verticalbg->setFlags(QGraphicsItem::ItemClipsChildrenToShape| + // Add the scale text elements: + QGraphicsTextItem *speed0 = new QGraphicsTextItem("0"); + speed0->setDefaultTextColor(QColor("White")); + speed0->setFont(QFont("Arial", (int)speedScaleHeight / 30)); + if (hqFonts) { + speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + matrix.reset(); + matrix.translate(compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).width() / 10, -speedScaleHeight / 30); + speed0->setTransform(matrix, false); + speed0->setParentItem(verticalbg); + m_speedscale->addToGroup(speed0); + for (int i = 0; i < 6; i++) { + speed0 = new QGraphicsTextItem(""); + speed0->setDefaultTextColor(QColor("White")); + speed0->setFont(QFont("Arial", (int)speedScaleHeight / 30)); + speed0->setPlainText(QString().setNum(i * 5 + 1)); + if (hqFonts) { + speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + matrix.translate(0, speedScaleHeight / 6); + speed0->setTransform(matrix, false); + speed0->setParentItem(verticalbg); + m_speedscale->addToGroup(speed0); + } + // Now vertically center the speed scale on the speed background + QRectF rectB = verticalbg->boundingRect(); + QRectF rectN = speedscalelines->boundingRect(); + m_speedscale->setPos(0, rectB.height() / 2 - rectN.height() / 2 - rectN.height() / 6); + + // Isolate the speed window and put it above the speed scale + compassMatrix = m_renderer->matrixForElement("speed-window"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).y(); + qreal speedWindowHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).height(); + QGraphicsSvgItem *speedwindow = new CachedSvgItem(); + speedwindow->setSharedRenderer(m_renderer); + speedwindow->setElementId("speed-window"); + speedwindow->setFlags(QGraphicsItem::ItemClipsChildrenToShape | + QGraphicsItem::ItemClipsToShape); + l_scene->addItem(speedwindow); + matrix.reset(); + matrix.translate(startX, startY); + speedwindow->setTransform(matrix, false); + + // Last create a Text Item at the location of the speed window + // and save it for future updates: + m_speedtext = new QGraphicsTextItem("0000"); + m_speedtext->setDefaultTextColor(QColor("White")); + m_speedtext->setFont(QFont("Arial", (int)speedWindowHeight / 2)); + if (hqFonts) { + m_speedtext->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + m_speedtext->setParentItem(speedwindow); + + //////////////////// + // Altitude + //////////////////// + // Right hand, very similar to speed + compassMatrix = m_renderer->matrixForElement("altitude-bg"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-bg")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-bg")).y(); + verticalbg = new CachedSvgItem(); + verticalbg->setSharedRenderer(m_renderer); + verticalbg->setElementId("altitude-bg"); + verticalbg->setFlags(QGraphicsItem::ItemClipsChildrenToShape | QGraphicsItem::ItemClipsToShape); - l_scene->addItem(verticalbg); - matrix.reset(); - matrix.translate(startX,startY); - verticalbg->setTransform(matrix,false); + l_scene->addItem(verticalbg); + matrix.reset(); + matrix.translate(startX, startY); + verticalbg->setTransform(matrix, false); - // Note: speed-scale should contain exactly 6 major ticks - // for 30km/h - m_speedscale = new QGraphicsItemGroup(); - m_speedscale->setParentItem(verticalbg); + // Note: altitude-scale should contain exactly 6 major ticks + // for 30 meters + m_altitudescale = new QGraphicsItemGroup(); + m_altitudescale->setParentItem(verticalbg); - QGraphicsSvgItem *speedscalelines = new CachedSvgItem(); - speedscalelines->setSharedRenderer(m_renderer); - speedscalelines->setElementId("speed-scale"); - speedScaleHeight = m_renderer->matrixForElement("speed-scale").mapRect( - m_renderer->boundsOnElement("speed-scale")).height(); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).width(); - startX -= m_renderer->matrixForElement("speed-scale").mapRect( - m_renderer->boundsOnElement("speed-scale")).width(); - matrix.reset(); - matrix.translate(startX,0); - speedscalelines->setTransform(matrix,false); - // Quick way to reposition the item before putting it in the group: - speedscalelines->setParentItem(verticalbg); - m_speedscale->addToGroup(speedscalelines); // (reparents the item) + QGraphicsSvgItem *altitudescalelines = new CachedSvgItem(); + altitudescalelines->setSharedRenderer(m_renderer); + altitudescalelines->setElementId("altitude-scale"); + altitudeScaleHeight = m_renderer->matrixForElement("altitude-scale").mapRect( + m_renderer->boundsOnElement("altitude-scale")).height(); + // Quick way to reposition the item before putting it in the group: + altitudescalelines->setParentItem(verticalbg); + m_altitudescale->addToGroup(altitudescalelines); // (reparents the item) - // Add the scale text elements: - QGraphicsTextItem *speed0 = new QGraphicsTextItem("0"); - speed0->setDefaultTextColor(QColor("White")); - speed0->setFont(QFont("Arial",(int) speedScaleHeight/30)); - if (hqFonts) speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - matrix.reset(); - matrix.translate(compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).width()/10,-speedScaleHeight/30); - speed0->setTransform(matrix,false); - speed0->setParentItem(verticalbg); - m_speedscale->addToGroup(speed0); - for (int i=0; i<6;i++) { - speed0 = new QGraphicsTextItem(""); - speed0->setDefaultTextColor(QColor("White")); - speed0->setFont(QFont("Arial",(int) speedScaleHeight/30)); - speed0->setPlainText(QString().setNum(i*5+1)); - if (hqFonts) speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - matrix.translate(0,speedScaleHeight/6); - speed0->setTransform(matrix,false); - speed0->setParentItem(verticalbg); - m_speedscale->addToGroup(speed0); - } - // Now vertically center the speed scale on the speed background - QRectF rectB = verticalbg->boundingRect(); - QRectF rectN = speedscalelines->boundingRect(); - m_speedscale->setPos(0,rectB.height()/2-rectN.height()/2-rectN.height()/6); + // Add the scale text elements: + speed0 = new QGraphicsTextItem("XXXX"); + speed0->setDefaultTextColor(QColor("White")); + speed0->setFont(QFont("Arial", (int)altitudeScaleHeight / 30)); + if (hqFonts) { + speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + matrix.reset(); + matrix.translate(m_renderer->matrixForElement("altitude-scale").mapRect(m_renderer->boundsOnElement("altitude-scale")).width() + + m_renderer->matrixForElement("altitude-bg").mapRect(m_renderer->boundsOnElement("altitude-bg")).width() / 10, -altitudeScaleHeight / 30); + speed0->setTransform(matrix, false); + speed0->setParentItem(verticalbg); + m_altitudescale->addToGroup(speed0); + for (int i = 0; i < 6; i++) { + speed0 = new QGraphicsTextItem("XXXX"); + speed0->setDefaultTextColor(QColor("White")); + speed0->setFont(QFont("Arial", (int)altitudeScaleHeight / 30)); + speed0->setPlainText(QString().setNum(i * 5 + 1)); + if (hqFonts) { + speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + matrix.translate(0, altitudeScaleHeight / 6); + speed0->setTransform(matrix, false); + speed0->setParentItem(verticalbg); + m_altitudescale->addToGroup(speed0); + } + // Now vertically center the speed scale on the speed background + rectB = verticalbg->boundingRect(); + rectN = altitudescalelines->boundingRect(); + m_altitudescale->setPos(0, rectB.height() / 2 - rectN.height() / 2 - rectN.height() / 6); - // Isolate the speed window and put it above the speed scale - compassMatrix = m_renderer->matrixForElement("speed-window"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).y(); - qreal speedWindowHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).height(); - QGraphicsSvgItem *speedwindow = new CachedSvgItem(); - speedwindow->setSharedRenderer(m_renderer); - speedwindow->setElementId("speed-window"); - speedwindow->setFlags(QGraphicsItem::ItemClipsChildrenToShape| - QGraphicsItem::ItemClipsToShape); - l_scene->addItem(speedwindow); - matrix.reset(); - matrix.translate(startX,startY); - speedwindow->setTransform(matrix,false); + // Isolate the Altitude window and put it above the altitude scale + compassMatrix = m_renderer->matrixForElement("altitude-window"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).y(); + qreal altitudeWindowHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).height(); + QGraphicsSvgItem *altitudewindow = new CachedSvgItem(); + altitudewindow->setSharedRenderer(m_renderer); + altitudewindow->setElementId("altitude-window"); + altitudewindow->setFlags(QGraphicsItem::ItemClipsChildrenToShape | + QGraphicsItem::ItemClipsToShape); + l_scene->addItem(altitudewindow); + matrix.reset(); + matrix.translate(startX, startY); + altitudewindow->setTransform(matrix, false); - // Last create a Text Item at the location of the speed window - // and save it for future updates: - m_speedtext = new QGraphicsTextItem("0000"); - m_speedtext->setDefaultTextColor(QColor("White")); - m_speedtext->setFont(QFont("Arial",(int) speedWindowHeight/2)); - if (hqFonts) m_speedtext->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - m_speedtext->setParentItem(speedwindow); + // Last create a Text Item at the location of the speed window + // and save it for future updates: + m_altitudetext = new QGraphicsTextItem("0000"); + m_altitudetext->setDefaultTextColor(QColor("White")); + m_altitudetext->setFont(QFont("Arial", (int)altitudeWindowHeight / 2)); + if (hqFonts) { + m_altitudetext->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + m_altitudetext->setParentItem(altitudewindow); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).width() / 10; + matrix.reset(); + matrix.translate(startX, 0); + m_altitudetext->setTransform(matrix, false); - //////////////////// - // Altitude - //////////////////// - // Right hand, very similar to speed - compassMatrix = m_renderer->matrixForElement("altitude-bg"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-bg")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-bg")).y(); - verticalbg = new CachedSvgItem(); - verticalbg->setSharedRenderer(m_renderer); - verticalbg->setElementId("altitude-bg"); - verticalbg->setFlags(QGraphicsItem::ItemClipsChildrenToShape| - QGraphicsItem::ItemClipsToShape); - l_scene->addItem(verticalbg); - matrix.reset(); - matrix.translate(startX,startY); - verticalbg->setTransform(matrix,false); + //////////////// + // GCS Telemetry Indicator + //////////////// + if (m_renderer->elementExists("gcstelemetry-Disconnected")) { + compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); + gcsTelemetryArrow = new CachedSvgItem(); + gcsTelemetryArrow->setSharedRenderer(m_renderer); + gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); + l_scene->addItem(gcsTelemetryArrow); + matrix.reset(); + matrix.translate(startX, startY); + gcsTelemetryArrow->setTransform(matrix, false); + } else { + gcsTelemetryArrow = NULL; + } - // Note: altitude-scale should contain exactly 6 major ticks - // for 30 meters - m_altitudescale = new QGraphicsItemGroup(); - m_altitudescale->setParentItem(verticalbg); - - QGraphicsSvgItem *altitudescalelines = new CachedSvgItem(); - altitudescalelines->setSharedRenderer(m_renderer); - altitudescalelines->setElementId("altitude-scale"); - altitudeScaleHeight = m_renderer->matrixForElement("altitude-scale").mapRect( - m_renderer->boundsOnElement("altitude-scale")).height(); - // Quick way to reposition the item before putting it in the group: - altitudescalelines->setParentItem(verticalbg); - m_altitudescale->addToGroup(altitudescalelines); // (reparents the item) - - // Add the scale text elements: - speed0 = new QGraphicsTextItem("XXXX"); - speed0->setDefaultTextColor(QColor("White")); - speed0->setFont(QFont("Arial",(int) altitudeScaleHeight/30)); - if (hqFonts) speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - matrix.reset(); - matrix.translate(m_renderer->matrixForElement("altitude-scale").mapRect(m_renderer->boundsOnElement("altitude-scale")).width() - + m_renderer->matrixForElement("altitude-bg").mapRect(m_renderer->boundsOnElement("altitude-bg")).width()/10,-altitudeScaleHeight/30); - speed0->setTransform(matrix,false); - speed0->setParentItem(verticalbg); - m_altitudescale->addToGroup(speed0); - for (int i=0; i<6;i++) { - speed0 = new QGraphicsTextItem("XXXX"); - speed0->setDefaultTextColor(QColor("White")); - speed0->setFont(QFont("Arial",(int) altitudeScaleHeight/30)); - speed0->setPlainText(QString().setNum(i*5+1)); - if (hqFonts) speed0->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - matrix.translate(0,altitudeScaleHeight/6); - speed0->setTransform(matrix,false); - speed0->setParentItem(verticalbg); - m_altitudescale->addToGroup(speed0); - } - // Now vertically center the speed scale on the speed background - rectB = verticalbg->boundingRect(); - rectN = altitudescalelines->boundingRect(); - m_altitudescale->setPos(0,rectB.height()/2-rectN.height()/2-rectN.height()/6); - - // Isolate the Altitude window and put it above the altitude scale - compassMatrix = m_renderer->matrixForElement("altitude-window"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).y(); - qreal altitudeWindowHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).height(); - QGraphicsSvgItem *altitudewindow = new CachedSvgItem(); - altitudewindow->setSharedRenderer(m_renderer); - altitudewindow->setElementId("altitude-window"); - altitudewindow->setFlags(QGraphicsItem::ItemClipsChildrenToShape| - QGraphicsItem::ItemClipsToShape); - l_scene->addItem(altitudewindow); - matrix.reset(); - matrix.translate(startX,startY); - altitudewindow->setTransform(matrix,false); - - // Last create a Text Item at the location of the speed window - // and save it for future updates: - m_altitudetext = new QGraphicsTextItem("0000"); - m_altitudetext->setDefaultTextColor(QColor("White")); - m_altitudetext->setFont(QFont("Arial",(int) altitudeWindowHeight/2)); - if (hqFonts) m_altitudetext->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - m_altitudetext->setParentItem(altitudewindow); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).width()/10; - matrix.reset(); - matrix.translate(startX,0); - m_altitudetext->setTransform(matrix,false); - - //////////////// - // GCS Telemetry Indicator - //////////////// - if (m_renderer->elementExists("gcstelemetry-Disconnected")) { - compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); - gcsTelemetryArrow = new CachedSvgItem(); - gcsTelemetryArrow->setSharedRenderer(m_renderer); - gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); - l_scene->addItem(gcsTelemetryArrow); - matrix.reset(); - matrix.translate(startX,startY); - gcsTelemetryArrow->setTransform(matrix,false); - } else { - gcsTelemetryArrow = NULL; - } - - if (m_renderer->elementExists("linkrate")) { - compassMatrix = m_renderer->matrixForElement("linkrate"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("linkrate")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("linkrate")).y(); - qreal linkRateHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("linkrate")).height(); - gcsTelemetryStats = new QGraphicsTextItem(); - gcsTelemetryStats->setDefaultTextColor(QColor("White")); - gcsTelemetryStats->setFont(QFont("Arial",(int) linkRateHeight)); - if (hqFonts) gcsTelemetryStats->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - l_scene->addItem(gcsTelemetryStats); - matrix.reset(); - matrix.translate(startX,startY-linkRateHeight/2); - gcsTelemetryStats->setTransform(matrix,false); - } else { - gcsTelemetryStats = NULL; - } + if (m_renderer->elementExists("linkrate")) { + compassMatrix = m_renderer->matrixForElement("linkrate"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("linkrate")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("linkrate")).y(); + qreal linkRateHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("linkrate")).height(); + gcsTelemetryStats = new QGraphicsTextItem(); + gcsTelemetryStats->setDefaultTextColor(QColor("White")); + gcsTelemetryStats->setFont(QFont("Arial", (int)linkRateHeight)); + if (hqFonts) { + gcsTelemetryStats->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + l_scene->addItem(gcsTelemetryStats); + matrix.reset(); + matrix.translate(startX, startY - linkRateHeight / 2); + gcsTelemetryStats->setTransform(matrix, false); + } else { + gcsTelemetryStats = NULL; + } - //////////////// - // GCS Battery Indicator - //////////////// - /* (to be used the day I add a green/yellow/red indicator) - compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); - gcsTelemetryArrow = new CachedSvgItem(); - gcsTelemetryArrow->setSharedRenderer(m_renderer); - gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); - l_scene->addItem(gcsTelemetryArrow); - matrix.reset(); - matrix.translate(startX,startY); - gcsTelemetryArrow->setTransform(matrix,false); + //////////////// + // GCS Battery Indicator + //////////////// + /* (to be used the day I add a green/yellow/red indicator) + compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); + gcsTelemetryArrow = new CachedSvgItem(); + gcsTelemetryArrow->setSharedRenderer(m_renderer); + gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); + l_scene->addItem(gcsTelemetryArrow); + matrix.reset(); + matrix.translate(startX,startY); + gcsTelemetryArrow->setTransform(matrix,false); */ - if (m_renderer->elementExists("battery-txt")) { - compassMatrix = m_renderer->matrixForElement("battery-txt"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("battery-txt")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("battery-txt")).y(); - qreal batStatHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("battery-txt")).height(); - gcsBatteryStats = new QGraphicsTextItem("Battery"); - gcsBatteryStats->setDefaultTextColor(QColor("White")); - gcsBatteryStats->setFont(QFont("Arial",(int) batStatHeight)); - if (hqFonts) gcsBatteryStats->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - l_scene->addItem(gcsBatteryStats); - matrix.reset(); - matrix.translate(startX,startY-batStatHeight/2); - gcsBatteryStats->setTransform(matrix,false); - } else { - gcsBatteryStats = NULL; - } + if (m_renderer->elementExists("battery-txt")) { + compassMatrix = m_renderer->matrixForElement("battery-txt"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("battery-txt")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("battery-txt")).y(); + qreal batStatHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("battery-txt")).height(); + gcsBatteryStats = new QGraphicsTextItem("Battery"); + gcsBatteryStats->setDefaultTextColor(QColor("White")); + gcsBatteryStats->setFont(QFont("Arial", (int)batStatHeight)); + if (hqFonts) { + gcsBatteryStats->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + l_scene->addItem(gcsBatteryStats); + matrix.reset(); + matrix.translate(startX, startY - batStatHeight / 2); + gcsBatteryStats->setTransform(matrix, false); + } else { + gcsBatteryStats = NULL; + } - //////////////// - // GCS GPS Indicator - //////////////// - /* (to be used the day I add a green/yellow/red indicator) - compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); - gcsTelemetryArrow = new CachedSvgItem(); - gcsTelemetryArrow->setSharedRenderer(m_renderer); - gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); - l_scene->addItem(gcsTelemetryArrow); - matrix.reset(); - matrix.translate(startX,startY); - gcsTelemetryArrow->setTransform(matrix,false); + //////////////// + // GCS GPS Indicator + //////////////// + /* (to be used the day I add a green/yellow/red indicator) + compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); + gcsTelemetryArrow = new CachedSvgItem(); + gcsTelemetryArrow->setSharedRenderer(m_renderer); + gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); + l_scene->addItem(gcsTelemetryArrow); + matrix.reset(); + matrix.translate(startX,startY); + gcsTelemetryArrow->setTransform(matrix,false); */ - if (m_renderer->elementExists("gps-txt")) { - compassMatrix = m_renderer->matrixForElement("gps-txt"); - startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gps-txt")).x(); - startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gps-txt")).y(); - qreal gpsStatHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("gps-txt")).height(); - gcsGPSStats = new QGraphicsTextItem("GPS"); - gcsGPSStats->setDefaultTextColor(QColor("White")); - gcsGPSStats->setFont(QFont("Arial",(int) gpsStatHeight)); - if (hqFonts) gcsGPSStats->setCacheMode(QGraphicsItem::DeviceCoordinateCache); - l_scene->addItem(gcsGPSStats); - matrix.reset(); - matrix.translate(startX,startY-gpsStatHeight/2); - gcsGPSStats->setTransform(matrix,false); - } else { - gcsGPSStats = NULL; - } + if (m_renderer->elementExists("gps-txt")) { + compassMatrix = m_renderer->matrixForElement("gps-txt"); + startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gps-txt")).x(); + startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gps-txt")).y(); + qreal gpsStatHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("gps-txt")).height(); + gcsGPSStats = new QGraphicsTextItem("GPS"); + gcsGPSStats->setDefaultTextColor(QColor("White")); + gcsGPSStats->setFont(QFont("Arial", (int)gpsStatHeight)); + if (hqFonts) { + gcsGPSStats->setCacheMode(QGraphicsItem::DeviceCoordinateCache); + } + l_scene->addItem(gcsGPSStats); + matrix.reset(); + matrix.translate(startX, startY - gpsStatHeight / 2); + gcsGPSStats->setTransform(matrix, false); + } else { + gcsGPSStats = NULL; + } l_scene->setSceneRect(m_background->boundingRect()); @@ -774,57 +814,55 @@ void PFDGadgetWidget::setDialFile(QString dfn) // 1) Move the center of the needle to the center of the background. rectB = m_background->boundingRect(); rectN = m_world->boundingRect(); - m_world->setPos(rectB.width()/2-rectN.width()/2,rectB.height()/2-rectN.height()/2); + m_world->setPos(rectB.width() / 2 - rectN.width() / 2, rectB.height() / 2 - rectN.height() / 2); // 2) Put the transform origin point of the needle at its center. - m_world->setTransformOriginPoint(rectN.width()/2,rectN.height()/2); + m_world->setTransformOriginPoint(rectN.width() / 2, rectN.height() / 2); rectN = m_rollscale->boundingRect(); - m_rollscale->setPos(rectB.width()/2-rectN.width()/2,rectB.height()/2-rectN.height()/2); - m_rollscale->setTransformOriginPoint(rectN.width()/2,rectN.height()/2); + m_rollscale->setPos(rectB.width() / 2 - rectN.width() / 2, rectB.height() / 2 - rectN.height() / 2); + m_rollscale->setTransformOriginPoint(rectN.width() / 2, rectN.height() / 2); // Also to the same init for the compass: rectB = m_compass->boundingRect(); rectN = m_compassband->boundingRect(); - m_compassband->setPos(rectB.width()/2-rectN.width()/2,rectB.height()/2-rectN.height()/2); - m_compassband->setTransformOriginPoint(rectN.width()/2,rectN.height()/2); + m_compassband->setPos(rectB.width() / 2 - rectN.width() / 2, rectB.height() / 2 - rectN.height() / 2); + m_compassband->setTransformOriginPoint(rectN.width() / 2, rectN.height() / 2); // Last: we just loaded the dial file which is by default valid for a "zero" value // of the needles, so we have to reset the needles too upon dial file loading, otherwise // we would end up with an offset when we change a dial file and the needle value // is not zero at that time. - rollValue = 0; - pitchValue = 0; - headingValue = 0; + rollValue = 0; + pitchValue = 0; + headingValue = 0; groundspeedValue = 0; - altitudeValue = 0; + altitudeValue = 0; pfdError = false; - if (!dialTimer.isActive()) + if (!dialTimer.isActive()) { dialTimer.start(); // Rearm the dial Timer which might be stopped. - } - else - { qDebug()<<"Error on PFD artwork file."; - m_renderer->load(QString(":/pfd/images/pfd-default.svg")); - l_scene->clear(); // This also deletes all items contained in the scene. - m_background = new CachedSvgItem(); - m_background->setSharedRenderer(m_renderer); - l_scene->addItem(m_background); - pfdError = true; - } + } + } else { qDebug() << "Error on PFD artwork file."; + m_renderer->load(QString(":/pfd/images/pfd-default.svg")); + l_scene->clear(); // This also deletes all items contained in the scene. + m_background = new CachedSvgItem(); + m_background->setSharedRenderer(m_renderer); + l_scene->addItem(m_background); + pfdError = true; } } void PFDGadgetWidget::paint() { - // update(); + // update(); } void PFDGadgetWidget::paintEvent(QPaintEvent *event) { // Skip painting until the dial file is loaded - if (! m_renderer->isValid()) { + if (!m_renderer->isValid()) { qDebug() << "Dial file not loaded, not rendering"; return; } - QGraphicsView::paintEvent(event); + QGraphicsView::paintEvent(event); } // This event enables the dial to be dynamically resized @@ -833,24 +871,24 @@ void PFDGadgetWidget::paintEvent(QPaintEvent *event) void PFDGadgetWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - fitInView(m_background, Qt::KeepAspectRatio ); + fitInView(m_background, Qt::KeepAspectRatio); } /*! - \brief Update method for the vertical scales - */ -void PFDGadgetWidget::moveVerticalScales() { + \brief Update method for the vertical scales + */ +void PFDGadgetWidget::moveVerticalScales() {} -} - -void PFDGadgetWidget::moveSky() { +void PFDGadgetWidget::moveSky() +{ int dialCount = 2; // Gets decreased by one for each element - // which has finished moving -// qDebug() << "MoveSky"; + + // which has finished moving +// qDebug() << "MoveSky"; /// TODO: optimize!!! if (pfdError) { - //skyDialTimer.stop(); + // skyDialTimer.stop(); return; } @@ -859,21 +897,22 @@ void PFDGadgetWidget::moveSky() { // the PFD and the whole GCS: for this reason, we check this here. // The strange check below works, it is a workaround because "isnan(double)" // is not supported on every compiler. - if (rollTarget != rollTarget || pitchTarget != pitchTarget) + if (rollTarget != rollTarget || pitchTarget != pitchTarget) { return; + } ////// // Roll ////// if (rollValue != rollTarget) { double rollDiff; - if ((abs((rollValue-rollTarget)*10) > 5) && beSmooth ) { - rollDiff =(rollTarget - rollValue)/2; + if ((abs((rollValue - rollTarget) * 10) > 5) && beSmooth) { + rollDiff = (rollTarget - rollValue) / 2; } else { rollDiff = rollTarget - rollValue; dialCount--; } - m_world->setRotation(m_world->rotation()+rollDiff); - m_rollscale->setRotation(m_rollscale->rotation()+rollDiff); + m_world->setRotation(m_world->rotation() + rollDiff); + m_rollscale->setRotation(m_rollscale->rotation() + rollDiff); rollValue += rollDiff; } else { dialCount--; @@ -884,27 +923,27 @@ void PFDGadgetWidget::moveSky() { ////// if (pitchValue != pitchTarget) { double pitchDiff; - if ((abs((pitchValue-pitchTarget)*10) > 5) && beSmooth ) { - // if (0) { - pitchDiff = (pitchTarget - pitchValue)/2; + if ((abs((pitchValue - pitchTarget) * 10) > 5) && beSmooth) { + // if (0) { + pitchDiff = (pitchTarget - pitchValue) / 2; } else { pitchDiff = pitchTarget - pitchValue; dialCount--; } - QPointF opd = QPointF(0,pitchDiff); - m_world->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), true); + QPointF opd = QPointF(0, pitchDiff); + m_world->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), true); QPointF oop = m_world->transformOriginPoint(); - m_world->setTransformOriginPoint((oop.x()-opd.x()),(oop.y()-opd.y())); + m_world->setTransformOriginPoint((oop.x() - opd.x()), (oop.y() - opd.y())); pitchValue += pitchDiff; } else { dialCount--; } - if (dialCount) + if (dialCount) { scene()->update(sceneRect()); - // if (!dialCount) - // skyDialTimer.stop(); - + } + // if (!dialCount) + // skyDialTimer.stop(); } @@ -915,14 +954,15 @@ void PFDGadgetWidget::moveSky() { void PFDGadgetWidget::moveNeedles() { int dialCount = 3; // Gets decreased by one for each element - // which has finished moving -// qDebug() << "MoveNeedles"; + // which has finished moving + +// qDebug() << "MoveNeedles"; /// TODO: optimize!!! if (pfdError) { - dialTimer.stop(); - return; + dialTimer.stop(); + return; } ////// @@ -934,28 +974,28 @@ void PFDGadgetWidget::moveNeedles() ////// if (headingValue != headingTarget) { double headingDiff; - if ((abs((headingValue-headingTarget)*10) > 5) && beSmooth ) { - headingDiff = (headingTarget - headingValue)/5; + if ((abs((headingValue - headingTarget) * 10) > 5) && beSmooth) { + headingDiff = (headingTarget - headingValue) / 5; } else { - headingDiff = headingTarget-headingValue; + headingDiff = headingTarget - headingValue; dialCount--; } - double threshold = 180*compassBandWidth/540; + double threshold = 180 * compassBandWidth / 540; // Note: rendering can jump oh so very slightly when crossing the 180 degree // boundary, should not impact actual useability of the display. - if ((headingValue+headingDiff)>=threshold) { + if ((headingValue + headingDiff) >= threshold) { // We went over 180°: activate a -360 degree offset - headingDiff -= 2*threshold; - headingTarget -= 2*threshold; - } else if ((headingValue+headingDiff)<-threshold) { + headingDiff -= 2 * threshold; + headingTarget -= 2 * threshold; + } else if ((headingValue + headingDiff) < -threshold) { // We went under -180°: remove the -360 degree offset - headingDiff += 2*threshold; - headingTarget += 2*threshold; + headingDiff += 2 * threshold; + headingTarget += 2 * threshold; } - QPointF opd = QPointF(headingDiff,0); - m_compassband->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), true); + QPointF opd = QPointF(headingDiff, 0); + m_compassband->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), true); headingValue += headingDiff; - } else { + } else { dialCount--; } @@ -963,37 +1003,38 @@ void PFDGadgetWidget::moveNeedles() // Airspeed ////// if (airspeedValue != airspeedTarget) { - if ((abs(airspeedValue-airspeedTarget) > speedScaleHeight/100) && beSmooth ) { - airspeedValue += (airspeedTarget-airspeedValue)/2; + if ((abs(airspeedValue - airspeedTarget) > speedScaleHeight / 100) && beSmooth) { + airspeedValue += (airspeedTarget - airspeedValue) / 2; } else { airspeedValue = airspeedTarget; dialCount--; } - float airspeed_kph=airspeedValue*3.6; - float airspeed_kph_scale = airspeed_kph*speedScaleHeight/30; + float airspeed_kph = airspeedValue * 3.6; + float airspeed_kph_scale = airspeed_kph * speedScaleHeight / 30; - qreal x = m_speedscale->transform().dx(); - //opd = QPointF(x,fmod(airspeed_kph,speedScaleHeight/6)); + qreal x = m_speedscale->transform().dx(); + // opd = QPointF(x,fmod(airspeed_kph,speedScaleHeight/6)); // fmod does rounding errors!! the formula below works better: - QPointF opd = QPointF(x, airspeed_kph_scale-floor(airspeed_kph_scale/speedScaleHeight*6)*speedScaleHeight/6); - m_speedscale->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false); + QPointF opd = QPointF(x, airspeed_kph_scale - floor(airspeed_kph_scale / speedScaleHeight * 6) * speedScaleHeight / 6); + m_speedscale->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), false); double speedText = airspeed_kph; - QString s = QString().sprintf("%.0f",speedText); + QString s = QString().sprintf("%.0f", speedText); m_speedtext->setPlainText(s); // Now update the text elements inside the scale: // (Qt documentation states that the list has the same order // as the item add order in the QGraphicsItemGroup) QList textList = m_speedscale->childItems(); - qreal val = 5*floor(airspeed_kph_scale/speedScaleHeight*6)+20; - foreach( QGraphicsItem * item, textList) { - if (QGraphicsTextItem *text = qgraphicsitem_cast(item)) { - QString s = (val<0) ? QString() : QString().sprintf("%.0f",val); - if (text->toPlainText() == s) + qreal val = 5 * floor(airspeed_kph_scale / speedScaleHeight * 6) + 20; + foreach(QGraphicsItem * item, textList) { + if (QGraphicsTextItem * text = qgraphicsitem_cast(item)) { + QString s = (val < 0) ? QString() : QString().sprintf("%.0f", val); + if (text->toPlainText() == s) { break; // This should happen at element one if is has not changed, indicating - // that it's not necessary to do the rest of the list + } + // that it's not necessary to do the rest of the list text->setPlainText(s); val -= 5; } @@ -1007,68 +1048,70 @@ void PFDGadgetWidget::moveNeedles() ////// if (groundspeedValue != groundspeedTarget) { groundspeedValue = groundspeedTarget; - qreal x = m_speedscale->transform().dx(); - //opd = QPointF(x,fmod(groundspeedValue,speedScaleHeight/6)); - // fmod does rounding errors!! the formula below works better: - QPointF opd = QPointF(x,groundspeedValue-floor(groundspeedValue/speedScaleHeight*6)*speedScaleHeight/6); - m_speedscale->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false); + qreal x = m_speedscale->transform().dx(); + // opd = QPointF(x,fmod(groundspeedValue,speedScaleHeight/6)); + // fmod does rounding errors!! the formula below works better: + QPointF opd = QPointF(x, groundspeedValue - floor(groundspeedValue / speedScaleHeight * 6) * speedScaleHeight / 6); + m_speedscale->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), false); - double speedText = groundspeedValue/speedScaleHeight*30; - QString s = QString().sprintf("%.0f",speedText); - m_speedtext->setPlainText(s); + double speedText = groundspeedValue / speedScaleHeight * 30; + QString s = QString().sprintf("%.0f", speedText); + m_speedtext->setPlainText(s); - // Now update the text elements inside the scale: - // (Qt documentation states that the list has the same order - // as the item add order in the QGraphicsItemGroup) - QList textList = m_speedscale->childItems(); - qreal val = 5*floor(groundspeedValue/speedScaleHeight*6)+20; - foreach( QGraphicsItem * item, textList) { - if (QGraphicsTextItem *text = qgraphicsitem_cast(item)) { - QString s = (val<0) ? QString() : QString().sprintf("%.0f",val); - if (text->toPlainText() == s) - break; // This should happen at element one if is has not changed, indicating - // that it's not necessary to do the rest of the list - text->setPlainText(s); - val -= 5; - } - } + // Now update the text elements inside the scale: + // (Qt documentation states that the list has the same order + // as the item add order in the QGraphicsItemGroup) + QList textList = m_speedscale->childItems(); + qreal val = 5 * floor(groundspeedValue / speedScaleHeight * 6) + 20; + foreach(QGraphicsItem * item, textList) { + if (QGraphicsTextItem * text = qgraphicsitem_cast(item)) { + QString s = (val < 0) ? QString() : QString().sprintf("%.0f", val); + if (text->toPlainText() == s) { + break; // This should happen at element one if is has not changed, indicating + } + // that it's not necessary to do the rest of the list + text->setPlainText(s); + val -= 5; + } + } } ////// // Altitude ////// if (altitudeValue != altitudeTarget) { - if ((abs(altitudeValue-altitudeTarget) > altitudeScaleHeight/100) && beSmooth ) { - altitudeValue += (altitudeTarget-altitudeValue)/2; + if ((abs(altitudeValue - altitudeTarget) > altitudeScaleHeight / 100) && beSmooth) { + altitudeValue += (altitudeTarget - altitudeValue) / 2; } else { altitudeValue = altitudeTarget; dialCount--; } // The altitude scale represents 30 meters - float altitudeValue_scale = floor(altitudeValue*10)/10*altitudeScaleHeight/30; + float altitudeValue_scale = floor(altitudeValue * 10) / 10 * altitudeScaleHeight / 30; - qreal x = m_altitudescale->transform().dx(); - //opd = QPointF(x,fmod(altitudeValue,altitudeScaleHeight/6)); + qreal x = m_altitudescale->transform().dx(); + // opd = QPointF(x,fmod(altitudeValue,altitudeScaleHeight/6)); // fmod does rounding errors!! the formula below works better: - QPointF opd = QPointF(x,altitudeValue_scale-floor(altitudeValue_scale/altitudeScaleHeight*6)*altitudeScaleHeight/6); - m_altitudescale->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false); + QPointF opd = QPointF(x, altitudeValue_scale - floor(altitudeValue_scale / altitudeScaleHeight * 6) * altitudeScaleHeight / 6); + m_altitudescale->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), false); double altitudeText = altitudeValue; - QString s = QString().sprintf("%.0f",altitudeText); + QString s = QString().sprintf("%.0f", altitudeText); m_altitudetext->setPlainText(s); // Now update the text elements inside the scale: // (Qt documentation states that the list has the same order // as the item add order in the QGraphicsItemGroup) QList textList = m_altitudescale->childItems(); - qreal val = 5*floor(altitudeValue_scale/altitudeScaleHeight*6)+20; - foreach( QGraphicsItem * item, textList) { - if (QGraphicsTextItem *text = qgraphicsitem_cast(item)) { - QString s = (val<0) ? QString() : QString().sprintf("%.0f",val); - if (text->toPlainText() == s) + qreal val = 5 * floor(altitudeValue_scale / altitudeScaleHeight * 6) + 20; + foreach(QGraphicsItem * item, textList) { + if (QGraphicsTextItem * text = qgraphicsitem_cast(item)) { + QString s = (val < 0) ? QString() : QString().sprintf("%.0f", val); + if (text->toPlainText() == s) { break; // This should happen at element one if is has not changed, indicating - // that it's not necessary to do the rest of the list + } + // that it's not necessary to do the rest of the list text->setPlainText(s); val -= 5; } @@ -1077,13 +1120,14 @@ void PFDGadgetWidget::moveNeedles() dialCount--; } - if (!dialCount) - dialTimer.stop(); - else - scene()->update(sceneRect()); + if (!dialCount) { + dialTimer.stop(); + } else { + scene()->update(sceneRect()); + } } /** - @} - @} - */ + @} + @} + */ diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.h b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.h index d3ef40ec8..d80b73432 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -39,118 +39,122 @@ #include #include -class PFDGadgetWidget : public QGraphicsView -{ +class PFDGadgetWidget : public QGraphicsView { Q_OBJECT public: PFDGadgetWidget(QWidget *parent = 0); - ~PFDGadgetWidget(); - void setDialFile(QString dfn); - void paint(); - // Sets up needle/UAVObject connections: - void connectNeedles(); - void enableOpenGL(bool flag); - void setHqFonts(bool flag) { hqFonts = flag; } - void enableSmoothUpdates(bool flag) { beSmooth = flag; } + ~PFDGadgetWidget(); + void setDialFile(QString dfn); + void paint(); + // Sets up needle/UAVObject connections: + void connectNeedles(); + void enableOpenGL(bool flag); + void setHqFonts(bool flag) + { + hqFonts = flag; + } + void enableSmoothUpdates(bool flag) + { + beSmooth = flag; + } public slots: - void updateAttitude(UAVObject *object1); - void updateHeading(UAVObject *object1); - void updateGPS(UAVObject *object1); - void updateGroundspeed(UAVObject *object1); - void updateAirspeed(UAVObject *object1); - void updateAltitude(UAVObject *object1); - void updateBattery(UAVObject *object1); - void updateLinkStatus(UAVObject *object1); + void updateAttitude(UAVObject *object1); + void updateHeading(UAVObject *object1); + void updateGPS(UAVObject *object1); + void updateGroundspeed(UAVObject *object1); + void updateAirspeed(UAVObject *object1); + void updateAltitude(UAVObject *object1); + void updateBattery(UAVObject *object1); + void updateLinkStatus(UAVObject *object1); protected: - void paintEvent(QPaintEvent *event); - void resizeEvent(QResizeEvent *event); + void paintEvent(QPaintEvent *event); + void resizeEvent(QResizeEvent *event); private slots: - void moveNeedles(); - void moveVerticalScales(); - void moveSky(); - void setToolTipPrivate(); + void moveNeedles(); + void moveVerticalScales(); + void moveSky(); + void setToolTipPrivate(); private: - QSvgRenderer *m_renderer; + QSvgRenderer *m_renderer; - // Background: background - QGraphicsSvgItem *m_background; - // earth/sky : world - QGraphicsSvgItem *m_world; - // Roll scale: rollscale - QGraphicsSvgItem *m_rollscale; - // Compass dial: - QGraphicsSvgItem *m_compass; - // Compass band: - QGraphicsSvgItem *m_compassband; - // Home point: - QGraphicsSvgItem *m_homewaypoint; - // Next point: - QGraphicsSvgItem *m_nextwaypoint; - // Home point bearing: - QGraphicsSvgItem *m_homepointbearing; - // Next point bearing: - QGraphicsSvgItem *m_nextpointbearing; - // Speed scale: - QGraphicsItemGroup *m_speedscale; - // Speed indicator text: - QGraphicsTextItem *m_speedtext; - // Vertical altitude scale: - QGraphicsItemGroup *m_altitudescale; - // Altitude indicator text: - QGraphicsTextItem *m_altitudetext; - // GCS link status Arrow - QGraphicsSvgItem *gcsTelemetryArrow; - QGraphicsTextItem *gcsTelemetryStats; - QGraphicsTextItem *gcsBatteryStats; - QGraphicsTextItem *gcsGPSStats; + // Background: background + QGraphicsSvgItem *m_background; + // earth/sky : world + QGraphicsSvgItem *m_world; + // Roll scale: rollscale + QGraphicsSvgItem *m_rollscale; + // Compass dial: + QGraphicsSvgItem *m_compass; + // Compass band: + QGraphicsSvgItem *m_compassband; + // Home point: + QGraphicsSvgItem *m_homewaypoint; + // Next point: + QGraphicsSvgItem *m_nextwaypoint; + // Home point bearing: + QGraphicsSvgItem *m_homepointbearing; + // Next point bearing: + QGraphicsSvgItem *m_nextpointbearing; + // Speed scale: + QGraphicsItemGroup *m_speedscale; + // Speed indicator text: + QGraphicsTextItem *m_speedtext; + // Vertical altitude scale: + QGraphicsItemGroup *m_altitudescale; + // Altitude indicator text: + QGraphicsTextItem *m_altitudetext; + // GCS link status Arrow + QGraphicsSvgItem *gcsTelemetryArrow; + QGraphicsTextItem *gcsTelemetryStats; + QGraphicsTextItem *gcsBatteryStats; + QGraphicsTextItem *gcsGPSStats; - // The Value and target variables - // are expressed in degrees - double rollTarget; - double rollValue; - double pitchTarget; - double pitchValue; - double headingTarget; - double headingValue; - double groundspeedTarget; - double groundspeedValue; - double airspeedTarget; - double airspeedValue; - double altitudeTarget; - double altitudeValue; + // The Value and target variables + // are expressed in degrees + double rollTarget; + double rollValue; + double pitchTarget; + double pitchValue; + double headingTarget; + double headingValue; + double groundspeedTarget; + double groundspeedValue; + double airspeedTarget; + double airspeedValue; + double altitudeTarget; + double altitudeValue; - qreal compassBandWidth; - qreal speedScaleHeight; - qreal altitudeScaleHeight; + qreal compassBandWidth; + qreal speedScaleHeight; + qreal altitudeScaleHeight; - // Name of the fields to read when an update is received: - UAVDataObject* airspeedObj; - UAVDataObject* altitudeObj; - UAVDataObject* attitudeObj; - UAVDataObject* groundspeedObj; - UAVDataObject* headingObj; - UAVDataObject* gpsObj; - UAVDataObject* gcsTelemetryObj; - UAVDataObject* gcsBatteryObj; + // Name of the fields to read when an update is received: + UAVDataObject *airspeedObj; + UAVDataObject *altitudeObj; + UAVDataObject *attitudeObj; + UAVDataObject *groundspeedObj; + UAVDataObject *headingObj; + UAVDataObject *gpsObj; + UAVDataObject *gcsTelemetryObj; + UAVDataObject *gcsBatteryObj; - // Rotation timer - QTimer dialTimer; - QTimer skyDialTimer; + // Rotation timer + QTimer dialTimer; + QTimer skyDialTimer; - QString satString; - QString batString; - - // Flag to check for pfd Error - bool pfdError; - // Flag to enable better rendering of fonts in OpenGL - bool hqFonts; - bool beSmooth; + QString satString; + QString batString; + // Flag to check for pfd Error + bool pfdError; + // Flag to enable better rendering of fonts in OpenGL + bool hqFonts; + bool beSmooth; }; #endif /* PFDGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdplugin.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdplugin.cpp index 662c109bf..fce87ea88 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdplugin.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,32 +35,31 @@ PFDPlugin::PFDPlugin() { - // Do nothing + // Do nothing } PFDPlugin::~PFDPlugin() { - // Do nothing + // Do nothing } -bool PFDPlugin::initialize(const QStringList& args, QString *errMsg) +bool PFDPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new PFDGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new PFDGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void PFDPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void PFDPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(PFDPlugin) - diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdplugin.h b/ground/openpilotgcs/src/plugins/pfd/pfdplugin.h index 1a05f6d25..067384793 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdplugin.h +++ b/ground/openpilotgcs/src/plugins/pfd/pfdplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin Primary Flight Display Plugin * @{ - * @brief The Primary Flight Display Gadget + * @brief The Primary Flight Display Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class PFDGadgetFactory; -class PFDPlugin : public ExtensionSystem::IPlugin -{ +class PFDPlugin : public ExtensionSystem::IPlugin { public: - PFDPlugin(); - ~PFDPlugin(); + PFDPlugin(); + ~PFDPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - PFDGadgetFactory *mf; + PFDGadgetFactory *mf; }; #endif /* PFDPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/pfdqml/osgearth.cpp b/ground/openpilotgcs/src/plugins/pfdqml/osgearth.cpp index a14fca75e..6cbe03aed 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/osgearth.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/osgearth.cpp @@ -41,7 +41,7 @@ #include "utils/pathutils.h" -OsgEarthItem::OsgEarthItem(QDeclarativeItem *parent): +OsgEarthItem::OsgEarthItem(QDeclarativeItem *parent) : QDeclarativeItem(parent), m_renderer(0), m_rendererThread(0), @@ -63,8 +63,8 @@ OsgEarthItem::~OsgEarthItem() { if (m_renderer) { m_rendererThread->exit(); - //wait up to 10 seconds for renderer thread to exit - m_rendererThread->wait(10*1000); + // wait up to 10 seconds for renderer thread to exit + m_rendererThread->wait(10 * 1000); delete m_renderer; delete m_rendererThread; @@ -75,9 +75,9 @@ QString OsgEarthItem::resolvedSceneFile() const { QString sceneFile = m_sceneFile; - //try to resolve the relative scene file name: + // try to resolve the relative scene file name: if (!QFileInfo(sceneFile).exists()) { - QDeclarativeView *view = qobject_cast(scene()->views().first()); + QDeclarativeView *view = qobject_cast(scene()->views().first()); if (view) { QUrl baseUrl = view->engine()->baseUrl(); @@ -93,16 +93,16 @@ void OsgEarthItem::geometryChanged(const QRectF &newGeometry, const QRectF &oldG Q_UNUSED(oldGeometry); Q_UNUSED(newGeometry); - //Dynamic gyometry changes are not supported yet, - //terrain is rendered to fixed geompetry and scalled for now + // Dynamic gyometry changes are not supported yet, + // terrain is rendered to fixed geompetry and scalled for now /* - qDebug() << Q_FUNC_INFO << newGeometry; + qDebug() << Q_FUNC_INFO << newGeometry; - int w = qRound(newGeometry.width()); - int h = qRound(newGeometry.height()); + int w = qRound(newGeometry.width()); + int h = qRound(newGeometry.height()); - if (m_currentSize != QSize(w,h) && m_gw.get()) { + if (m_currentSize != QSize(w,h) && m_gw.get()) { m_currentSize = QSize(w,h); m_gw->getEventQueue()->windowResize(0,0,w,h); @@ -111,15 +111,15 @@ void OsgEarthItem::geometryChanged(const QRectF &newGeometry, const QRectF &oldG osg::Camera *camera = m_viewer->getCamera(); camera->setViewport(new osg::Viewport(0,0,w,h)); camera->setProjectionMatrixAsPerspective(m_fieldOfView, qreal(w)/h, 1.0f, 10000.0f); - } - */ + } + */ } void OsgEarthItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *style, QWidget *widget) { Q_UNUSED(painter); Q_UNUSED(style); - QGLWidget *glWidget = qobject_cast(widget); + QGLWidget *glWidget = qobject_cast(widget); if (!m_renderer) { m_renderer = new OsgEarthItemRenderer(this, glWidget); @@ -136,8 +136,9 @@ void OsgEarthItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *styl QGLFramebufferObject *fbo = m_renderer->lastFrame(); - if (glWidget && fbo) + if (glWidget && fbo) { glWidget->drawTexture(boundingRect(), fbo->texture()); + } } void OsgEarthItem::updateView() @@ -182,7 +183,7 @@ void OsgEarthItem::setYaw(qreal arg) void OsgEarthItem::setLatitude(double arg) { - //not sure qFuzzyCompare is accurate enough for geo coordinates + // not sure qFuzzyCompare is accurate enough for geo coordinates if (m_latitude != arg) { m_latitude = arg; emit latitudeChanged(arg); @@ -199,26 +200,26 @@ void OsgEarthItem::setLongitude(double arg) void OsgEarthItem::setAltitude(double arg) { - if (!qFuzzyCompare(m_altitude,arg)) { + if (!qFuzzyCompare(m_altitude, arg)) { m_altitude = arg; emit altitudeChanged(arg); } } -//! Camera vertical field of view in degrees +// ! Camera vertical field of view in degrees void OsgEarthItem::setFieldOfView(qreal arg) { - if (!qFuzzyCompare(m_fieldOfView,arg)) { + if (!qFuzzyCompare(m_fieldOfView, arg)) { m_fieldOfView = arg; emit fieldOfViewChanged(arg); - //it should be a queued call to OsgEarthItemRenderer instead + // it should be a queued call to OsgEarthItemRenderer instead /*if (m_viewer.get()) { m_viewer->getCamera()->setProjectionMatrixAsPerspective( m_fieldOfView, qreal(m_currentSize.width())/m_currentSize.height(), 1.0f, 10000.0f); - }*/ + }*/ updateFrame(); } @@ -239,23 +240,23 @@ OsgEarthItemRenderer::OsgEarthItemRenderer(OsgEarthItem *item, QGLWidget *glWidg m_currentSize(640, 480), m_cameraDirty(false) { - //make a shared gl widget to avoid - //osg rendering to mess with qpainter state - //this runs in the main thread + // make a shared gl widget to avoid + // osg rendering to mess with qpainter state + // this runs in the main thread m_glWidget = new QGLWidget(0, glWidget); m_glWidget.data()->setAttribute(Qt::WA_PaintOutsidePaintEvent); - for (int i=0; imakeCurrent(); - for (int i=0; iresolvedSceneFile(); m_model = osgDB::readNodeFile(sceneFile.toStdString()); - //setup caching + // setup caching osgEarth::MapNode *mapNode = osgEarth::MapNode::findMapNode(m_model.get()); if (!mapNode) { qWarning() << Q_FUNC_INFO << sceneFile << " doesn't look like an osgEarth file"; } - m_gw = new osgViewer::GraphicsWindowEmbedded(0,0,w,h); + m_gw = new osgViewer::GraphicsWindowEmbedded(0, 0, w, h); m_viewer = new osgViewer::Viewer(); m_viewer->setThreadingModel(osgViewer::Viewer::SingleThreaded); @@ -293,26 +294,27 @@ void OsgEarthItemRenderer::initScene() m_viewer->getDatabasePager()->setDoPreCompile(true); osg::Camera *camera = m_viewer->getCamera(); - camera->setViewport(new osg::Viewport(0,0,w,h)); + camera->setViewport(new osg::Viewport(0, 0, w, h)); camera->setGraphicsContext(m_gw); - camera->setClearMask(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); + camera->setClearMask(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // configure the near/far so we don't clip things that are up close camera->setNearFarRatio(0.00002); - camera->setProjectionMatrixAsPerspective(m_item->fieldOfView(), qreal(w)/h, 1.0f, 10000.0f); + camera->setProjectionMatrixAsPerspective(m_item->fieldOfView(), qreal(w) / h, 1.0f, 10000.0f); updateFrame(); } void OsgEarthItemRenderer::updateFrame() { - if (!m_cameraDirty || !m_viewer.get() || m_glWidget.isNull()) + if (!m_cameraDirty || !m_viewer.get() || m_glWidget.isNull()) { return; + } m_glWidget.data()->makeCurrent(); - //To find a camera view matrix, find placer matrixes for two points - //onr at requested coords and another latitude shifted by 0.01 deg + // To find a camera view matrix, find placer matrixes for two points + // onr at requested coords and another latitude shifted by 0.01 deg osgEarth::Util::ObjectPlacer placer(m_viewer->getSceneData()); m_cameraDirty = false; @@ -320,40 +322,40 @@ void OsgEarthItemRenderer::updateFrame() osg::Matrixd positionMatrix; placer.createPlacerMatrix(m_item->latitude(), m_item->longitude(), m_item->altitude(), positionMatrix); osg::Matrixd positionMatrix2; - placer.createPlacerMatrix(m_item->latitude()+0.01, m_item->longitude(), m_item->altitude(), positionMatrix2); + placer.createPlacerMatrix(m_item->latitude() + 0.01, m_item->longitude(), m_item->altitude(), positionMatrix2); osg::Vec3d eye(0.0f, 0.0f, 0.0f); osg::Vec3d viewVector(0.0f, 0.0f, 0.0f); osg::Vec3d upVector(0.0f, 0.0f, 1.0f); eye = positionMatrix.preMult(eye); - upVector = positionMatrix.preMult(upVector); + upVector = positionMatrix.preMult(upVector); upVector.normalize(); - viewVector = positionMatrix2.preMult(viewVector) - eye; + viewVector = positionMatrix2.preMult(viewVector) - eye; viewVector.normalize(); viewVector *= 10.0; - //TODO: clarify the correct rotation order, - //currently assuming yaw, pitch, roll + // TODO: clarify the correct rotation order, + // currently assuming yaw, pitch, roll osg::Quat q; - q.makeRotate(-m_item->yaw()*M_PI/180.0, upVector); - upVector = q * upVector; + q.makeRotate(-m_item->yaw() * M_PI / 180.0, upVector); + upVector = q * upVector; viewVector = q * viewVector; osg::Vec3d side = viewVector ^ upVector; - q.makeRotate(m_item->pitch()*M_PI/180.0, side); - upVector = q * upVector; + q.makeRotate(m_item->pitch() * M_PI / 180.0, side); + upVector = q * upVector; viewVector = q * viewVector; - q.makeRotate(m_item->roll()*M_PI/180.0, viewVector); - upVector = q * upVector; + q.makeRotate(m_item->roll() * M_PI / 180.0, viewVector); + upVector = q * upVector; viewVector = q * viewVector; osg::Vec3d center = eye + viewVector; -// qDebug() << "e " << eye.x() << eye.y() << eye.z(); -// qDebug() << "c " << center.x() << center.y() << center.z(); -// qDebug() << "up" << upVector.x() << upVector.y() << upVector.z(); +// qDebug() << "e " << eye.x() << eye.y() << eye.z(); +// qDebug() << "c " << center.x() << center.y() << center.z(); +// qDebug() << "up" << upVector.x() << upVector.y() << upVector.z(); m_viewer->getCamera()->setViewMatrixAsLookAt(osg::Vec3d(eye.x(), eye.y(), eye.z()), osg::Vec3d(center.x(), center.y(), center.z()), diff --git a/ground/openpilotgcs/src/plugins/pfdqml/osgearth.h b/ground/openpilotgcs/src/plugins/pfdqml/osgearth.h index 03f0543a1..332c27156 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/osgearth.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/osgearth.h @@ -26,10 +26,8 @@ class QGLFramebufferObject; class QGLWidget; class OsgEarthItemRenderer; -class OsgEarthItem : public QDeclarativeItem -{ - Q_OBJECT - Q_DISABLE_COPY(OsgEarthItem) +class OsgEarthItem : public QDeclarativeItem { + Q_OBJECT Q_DISABLE_COPY(OsgEarthItem) Q_PROPERTY(QString sceneFile READ sceneFile WRITE setSceneFile NOTIFY sceneFileChanged) Q_PROPERTY(qreal fieldOfView READ fieldOfView WRITE setFieldOfView NOTIFY fieldOfViewChanged) @@ -46,17 +44,41 @@ public: OsgEarthItem(QDeclarativeItem *parent = 0); ~OsgEarthItem(); - QString sceneFile() const { return m_sceneFile; } + QString sceneFile() const + { + return m_sceneFile; + } QString resolvedSceneFile() const; - qreal fieldOfView() const { return m_fieldOfView; } + qreal fieldOfView() const + { + return m_fieldOfView; + } - qreal roll() const { return m_roll; } - qreal pitch() const { return m_pitch; } - qreal yaw() const { return m_yaw; } + qreal roll() const + { + return m_roll; + } + qreal pitch() const + { + return m_pitch; + } + qreal yaw() const + { + return m_yaw; + } - double latitude() const { return m_latitude; } - double longitude() const { return m_longitude; } - double altitude() const { return m_altitude; } + double latitude() const + { + return m_latitude; + } + double longitude() const + { + return m_longitude; + } + double altitude() const + { + return m_altitude; + } protected: void geometryChanged(const QRectF &newGeometry, const QRectF &oldGeometry); @@ -106,18 +128,19 @@ private: qreal m_fieldOfView; QString m_sceneFile; - }; -class OsgEarthItemRenderer : public QObject -{ +class OsgEarthItemRenderer : public QObject { Q_OBJECT public: OsgEarthItemRenderer(OsgEarthItem *item, QGLWidget *glWidget); ~OsgEarthItemRenderer(); QGLFramebufferObject *lastFrame(); - void markDirty() { m_cameraDirty = true; } + void markDirty() + { + m_cameraDirty = true; + } public slots: void initScene(); @@ -135,7 +158,7 @@ private: osg::ref_ptr m_model; QWeakPointer m_glWidget; - QGLFramebufferObject* m_fbo[FboCount]; + QGLFramebufferObject *m_fbo[FboCount]; int m_lastFboNumber; QSize m_currentSize; @@ -146,4 +169,3 @@ private: QML_DECLARE_TYPE(OsgEarthItem) #endif // OSGEARTH_H - diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.cpp index 770cc9744..265a96497 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.cpp @@ -19,10 +19,9 @@ #include "pfdqmlgadgetconfiguration.h" PfdQmlGadget::PfdQmlGadget(QString classId, PfdQmlGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} PfdQmlGadget::~PfdQmlGadget() { @@ -30,14 +29,15 @@ PfdQmlGadget::~PfdQmlGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void PfdQmlGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void PfdQmlGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - PfdQmlGadgetConfiguration *m = qobject_cast(config); + PfdQmlGadgetConfiguration *m = qobject_cast(config); + m_widget->setOpenGLEnabled(m->openGLEnabled()); m_widget->setQmlFile(m->qmlFile()); m_widget->setEarthFile(m->earthFile()); @@ -47,8 +47,8 @@ void PfdQmlGadget::loadConfiguration(IUAVGadgetConfiguration* config) m_widget->setLongitude(m->longitude()); m_widget->setAltitude(m->altitude()); - //setting OSGEARTH_CACHE_ONLY seems to work the most reliably - //between osgEarth versions I tried + // setting OSGEARTH_CACHE_ONLY seems to work the most reliably + // between osgEarth versions I tried if (m->cacheOnly()) { qputenv("OSGEARTH_CACHE_ONLY", "true"); } else { diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.h b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.h index be6625ab0..b6525007f 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.h @@ -27,15 +27,17 @@ class PfdQmlGadgetWidget; using namespace Core; -class PfdQmlGadget : public Core::IUAVGadget -{ +class PfdQmlGadget : public Core::IUAVGadget { Q_OBJECT public: PfdQmlGadget(QString classId, PfdQmlGadgetWidget *widget, QWidget *parent = 0); ~PfdQmlGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: PfdQmlGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.cpp index ba85992ad..e00e52a1d 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.cpp @@ -33,21 +33,21 @@ PfdQmlGadgetConfiguration::PfdQmlGadgetConfiguration(QString classId, QSettings m_altitude(0), m_cacheOnly(false) { - //if a saved configuration exists load it - if(qSettings != 0) { - m_qmlFile = qSettings->value("qmlFile").toString(); - m_qmlFile=Utils::PathUtils().InsertDataPath(m_qmlFile); + // if a saved configuration exists load it + if (qSettings != 0) { + m_qmlFile = qSettings->value("qmlFile").toString(); + m_qmlFile = Utils::PathUtils().InsertDataPath(m_qmlFile); - m_earthFile = qSettings->value("earthFile").toString(); - m_earthFile=Utils::PathUtils().InsertDataPath(m_earthFile); + m_earthFile = qSettings->value("earthFile").toString(); + m_earthFile = Utils::PathUtils().InsertDataPath(m_earthFile); - m_openGLEnabled = qSettings->value("openGLEnabled", true).toBool(); - m_terrainEnabled = qSettings->value("terrainEnabled").toBool(); + m_openGLEnabled = qSettings->value("openGLEnabled", true).toBool(); + m_terrainEnabled = qSettings->value("terrainEnabled").toBool(); m_actualPositionUsed = qSettings->value("actualPositionUsed").toBool(); - m_latitude = qSettings->value("latitude").toDouble(); - m_longitude = qSettings->value("longitude").toDouble(); - m_altitude = qSettings->value("altitude").toDouble(); - m_cacheOnly = qSettings->value("cacheOnly").toBool(); + m_latitude = qSettings->value("latitude").toDouble(); + m_longitude = qSettings->value("longitude").toDouble(); + m_altitude = qSettings->value("altitude").toDouble(); + m_cacheOnly = qSettings->value("cacheOnly").toBool(); } } @@ -58,15 +58,16 @@ PfdQmlGadgetConfiguration::PfdQmlGadgetConfiguration(QString classId, QSettings IUAVGadgetConfiguration *PfdQmlGadgetConfiguration::clone() { PfdQmlGadgetConfiguration *m = new PfdQmlGadgetConfiguration(this->classId()); - m->m_qmlFile = m_qmlFile; - m->m_openGLEnabled = m_openGLEnabled; - m->m_earthFile = m_earthFile; - m->m_terrainEnabled = m_terrainEnabled; + + m->m_qmlFile = m_qmlFile; + m->m_openGLEnabled = m_openGLEnabled; + m->m_earthFile = m_earthFile; + m->m_terrainEnabled = m_terrainEnabled; m->m_actualPositionUsed = m_actualPositionUsed; - m->m_latitude = m_latitude; - m->m_longitude = m_longitude; - m->m_altitude = m_altitude; - m->m_cacheOnly = m_cacheOnly; + m->m_latitude = m_latitude; + m->m_longitude = m_longitude; + m->m_altitude = m_altitude; + m->m_cacheOnly = m_cacheOnly; return m; } @@ -75,8 +76,10 @@ IUAVGadgetConfiguration *PfdQmlGadgetConfiguration::clone() * Saves a configuration. * */ -void PfdQmlGadgetConfiguration::saveConfig(QSettings* qSettings) const { - QString qmlFile = Utils::PathUtils().RemoveDataPath(m_qmlFile); +void PfdQmlGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ + QString qmlFile = Utils::PathUtils().RemoveDataPath(m_qmlFile); + qSettings->setValue("qmlFile", qmlFile); QString earthFile = Utils::PathUtils().RemoveDataPath(m_earthFile); qSettings->setValue("earthFile", earthFile); diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.h index 3f12f25b1..f09e4a0ad 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetconfiguration.h @@ -21,33 +21,86 @@ using namespace Core; -class PfdQmlGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT +class PfdQmlGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit PfdQmlGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit PfdQmlGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void setQmlFile(const QString &fileName) { m_qmlFile=fileName; } - void setEarthFile(const QString &fileName) { m_earthFile=fileName; } - void setOpenGLEnabled(bool flag) { m_openGLEnabled = flag; } - void setTerrainEnabled(bool flag) { m_terrainEnabled = flag; } - void setActualPositionUsed(bool flag) { m_actualPositionUsed = flag; } - void setLatitude(double value) { m_latitude = value; } - void setLongitude(double value) { m_longitude = value; } - void setAltitude(double value) { m_altitude = value; } - void setCacheOnly(bool flag) { m_cacheOnly = flag; } + void setQmlFile(const QString &fileName) + { + m_qmlFile = fileName; + } + void setEarthFile(const QString &fileName) + { + m_earthFile = fileName; + } + void setOpenGLEnabled(bool flag) + { + m_openGLEnabled = flag; + } + void setTerrainEnabled(bool flag) + { + m_terrainEnabled = flag; + } + void setActualPositionUsed(bool flag) + { + m_actualPositionUsed = flag; + } + void setLatitude(double value) + { + m_latitude = value; + } + void setLongitude(double value) + { + m_longitude = value; + } + void setAltitude(double value) + { + m_altitude = value; + } + void setCacheOnly(bool flag) + { + m_cacheOnly = flag; + } - QString qmlFile() const { return m_qmlFile; } - QString earthFile() const { return m_earthFile; } - bool openGLEnabled() const { return m_openGLEnabled; } - bool terrainEnabled() const { return m_terrainEnabled; } - bool actualPositionUsed() const { return m_actualPositionUsed; } - double latitude() const { return m_latitude; } - double longitude() const { return m_longitude; } - double altitude() const { return m_altitude; } - bool cacheOnly() const { return m_cacheOnly; } + QString qmlFile() const + { + return m_qmlFile; + } + QString earthFile() const + { + return m_earthFile; + } + bool openGLEnabled() const + { + return m_openGLEnabled; + } + bool terrainEnabled() const + { + return m_terrainEnabled; + } + bool actualPositionUsed() const + { + return m_actualPositionUsed; + } + double latitude() const + { + return m_latitude; + } + double longitude() const + { + return m_longitude; + } + double altitude() const + { + return m_altitude; + } + bool cacheOnly() const + { + return m_cacheOnly; + } - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.cpp index c49fa904d..0fa0503aa 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.cpp @@ -21,19 +21,18 @@ #include PfdQmlGadgetFactory::PfdQmlGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("PfdQmlGadget"), - tr("PFD (qml)"), - parent) -{ -} + IUAVGadgetFactory(QString("PfdQmlGadget"), + tr("PFD (qml)"), + parent) +{} PfdQmlGadgetFactory::~PfdQmlGadgetFactory() -{ -} +{} -Core::IUAVGadget* PfdQmlGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *PfdQmlGadgetFactory::createGadget(QWidget *parent) { - PfdQmlGadgetWidget* gadgetWidget = new PfdQmlGadgetWidget(parent); + PfdQmlGadgetWidget *gadgetWidget = new PfdQmlGadgetWidget(parent); + return new PfdQmlGadget(QString("PfdQmlGadget"), gadgetWidget, parent); } @@ -44,6 +43,5 @@ IUAVGadgetConfiguration *PfdQmlGadgetFactory::createConfiguration(QSettings *qSe IOptionsPage *PfdQmlGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new PfdQmlGadgetOptionsPage(qobject_cast(config)); + return new PfdQmlGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.h b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.h index 348afc249..23e4610ed 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetfactory.h @@ -26,8 +26,7 @@ class IUAVGadgetFactory; using namespace Core; -class PfdQmlGadgetFactory : public IUAVGadgetFactory -{ +class PfdQmlGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: PfdQmlGadgetFactory(QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.cpp index 2221ec691..0b20b8480 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.cpp @@ -27,18 +27,17 @@ #include PfdQmlGadgetOptionsPage::PfdQmlGadgetOptionsPage(PfdQmlGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *PfdQmlGadgetOptionsPage::createPage(QWidget *parent) { options_page = new Ui::PfdQmlGadgetOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget(parent); - //main layout + // main layout options_page->setupUi(optionsPageWidget); // Restore the contents from the settings: @@ -97,5 +96,4 @@ void PfdQmlGadgetOptionsPage::apply() } void PfdQmlGadgetOptionsPage::finish() -{ -} +{} diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.h index 44711bdb8..a324c0709 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetoptionspage.h @@ -29,14 +29,13 @@ class IUAVGadgetConfiguration; class PfdQmlGadgetConfiguration; namespace Ui { - class PfdQmlGadgetOptionsPage; +class PfdQmlGadgetOptionsPage; } using namespace Core; -class PfdQmlGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class PfdQmlGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit PfdQmlGadgetOptionsPage(PfdQmlGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp index 4cde9f2e8..60fe0539c 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp @@ -41,36 +41,38 @@ PfdQmlGadgetWidget::PfdQmlGadgetWidget(QWidget *parent) : m_longitude(10.158932), m_altitude(2000) { - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setResizeMode(SizeRootObjectToView); - //setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); + // setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); QStringList objectsToExport; objectsToExport << "VelocityActual" << - "PositionActual" << - "AttitudeActual" << - "Accels" << - "VelocityDesired" << - "PositionDesired" << - "AttitudeHoldDesired" << - "GPSPosition" << - "GCSTelemetryStats" << - "FlightBatteryState"; + "PositionActual" << + "AttitudeActual" << + "Accels" << + "VelocityDesired" << + "PositionDesired" << + "AttitudeHoldDesired" << + "GPSPosition" << + "GCSTelemetryStats" << + "FlightBatteryState"; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - foreach (const QString &objectName, objectsToExport) { - UAVObject* object = objManager->getObject(objectName); - if (object) + foreach(const QString &objectName, objectsToExport) { + UAVObject *object = objManager->getObject(objectName); + + if (object) { engine()->rootContext()->setContextProperty(objectName, object); - else + } else { qWarning() << "Failed to load object" << objectName; + } } - //to expose settings values + // to expose settings values engine()->rootContext()->setContextProperty("qmlWidget", this); #ifdef USE_OSG qmlRegisterType("org.OpenPilot", 1, 0, "OsgEarth"); @@ -78,8 +80,7 @@ PfdQmlGadgetWidget::PfdQmlGadgetWidget(QWidget *parent) : } PfdQmlGadgetWidget::~PfdQmlGadgetWidget() -{ -} +{} void PfdQmlGadgetWidget::setQmlFile(QString fn) { @@ -89,7 +90,7 @@ void PfdQmlGadgetWidget::setQmlFile(QString fn) SvgImageProvider *svgProvider = new SvgImageProvider(fn); engine()->addImageProvider("svg", svgProvider); - //it's necessary to allow qml side to query svg element position + // it's necessary to allow qml side to query svg element position engine()->rootContext()->setContextProperty("svgRenderer", svgProvider); engine()->setBaseUrl(QUrl::fromLocalFile(fn)); @@ -112,10 +113,12 @@ void PfdQmlGadgetWidget::setEarthFile(QString arg) void PfdQmlGadgetWidget::setTerrainEnabled(bool arg) { bool wasEnabled = terrainEnabled(); + m_terrainEnabled = arg; - if (wasEnabled != terrainEnabled()) + if (wasEnabled != terrainEnabled()) { emit terrainEnabledChanged(terrainEnabled()); + } } void PfdQmlGadgetWidget::setOpenGLEnabled(bool arg) @@ -124,18 +127,19 @@ void PfdQmlGadgetWidget::setOpenGLEnabled(bool arg) m_openGLEnabled = arg; qDebug() << Q_FUNC_INFO << "Set OPENGL" << m_openGLEnabled; - if (m_openGLEnabled) + if (m_openGLEnabled) { setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); - else + } else { setViewport(new QWidget); + } - //update terrainEnabled status with opengl status chaged + // update terrainEnabled status with opengl status chaged setTerrainEnabled(m_terrainEnabled); } } -//Switch between PositionActual UAVObject position -//and pre-defined latitude/longitude/altitude properties +// Switch between PositionActual UAVObject position +// and pre-defined latitude/longitude/altitude properties void PfdQmlGadgetWidget::setActualPositionUsed(bool arg) { if (m_actualPositionUsed != arg) { @@ -146,7 +150,7 @@ void PfdQmlGadgetWidget::setActualPositionUsed(bool arg) void PfdQmlGadgetWidget::setLatitude(double arg) { - //not sure qFuzzyCompare is accurate enough for geo coordinates + // not sure qFuzzyCompare is accurate enough for geo coordinates if (m_latitude != arg) { m_latitude = arg; emit latitudeChanged(arg); @@ -163,7 +167,7 @@ void PfdQmlGadgetWidget::setLongitude(double arg) void PfdQmlGadgetWidget::setAltitude(double arg) { - if (!qFuzzyCompare(m_altitude,arg)) { + if (!qFuzzyCompare(m_altitude, arg)) { m_altitude = arg; emit altitudeChanged(arg); } diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.h b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.h index 56cc17dd4..77ab49f9d 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.h @@ -20,31 +20,47 @@ #include "pfdqmlgadgetconfiguration.h" #include -class PfdQmlGadgetWidget : public QDeclarativeView -{ - Q_OBJECT - Q_PROPERTY(QString earthFile READ earthFile WRITE setEarthFile NOTIFY earthFileChanged) +class PfdQmlGadgetWidget : public QDeclarativeView { + Q_OBJECT Q_PROPERTY(QString earthFile READ earthFile WRITE setEarthFile NOTIFY earthFileChanged) Q_PROPERTY(bool terrainEnabled READ terrainEnabled WRITE setTerrainEnabled NOTIFY terrainEnabledChanged) Q_PROPERTY(bool actualPositionUsed READ actualPositionUsed WRITE setActualPositionUsed NOTIFY actualPositionUsedChanged) - //pre-defined fallback position + // pre-defined fallback position Q_PROPERTY(double latitude READ latitude WRITE setLatitude NOTIFY latitudeChanged) Q_PROPERTY(double longitude READ longitude WRITE setLongitude NOTIFY longitudeChanged) Q_PROPERTY(double altitude READ altitude WRITE setAltitude NOTIFY altitudeChanged) public: PfdQmlGadgetWidget(QWidget *parent = 0); - ~PfdQmlGadgetWidget(); + ~PfdQmlGadgetWidget(); void setQmlFile(QString fn); - QString earthFile() const { return m_earthFile; } - bool terrainEnabled() const { return m_terrainEnabled && m_openGLEnabled; } + QString earthFile() const + { + return m_earthFile; + } + bool terrainEnabled() const + { + return m_terrainEnabled && m_openGLEnabled; + } - bool actualPositionUsed() const { return m_actualPositionUsed; } - double latitude() const { return m_latitude; } - double longitude() const { return m_longitude; } - double altitude() const { return m_altitude; } + bool actualPositionUsed() const + { + return m_actualPositionUsed; + } + double latitude() const + { + return m_latitude; + } + double longitude() const + { + return m_longitude; + } + double altitude() const + { + return m_altitude; + } public slots: void setEarthFile(QString arg); diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.cpp index e71763638..085ca06f6 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.cpp @@ -24,32 +24,31 @@ PfdQmlPlugin::PfdQmlPlugin() { - // Do nothing + // Do nothing } PfdQmlPlugin::~PfdQmlPlugin() { - // Do nothing + // Do nothing } -bool PfdQmlPlugin::initialize(const QStringList& args, QString *errMsg) +bool PfdQmlPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new PfdQmlGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new PfdQmlGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void PfdQmlPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void PfdQmlPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(PfdQmlPlugin) - diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.h b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.h index 8686cf9c9..344d22128 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlplugin.h @@ -21,16 +21,15 @@ class PfdQmlGadgetFactory; -class PfdQmlPlugin : public ExtensionSystem::IPlugin -{ +class PfdQmlPlugin : public ExtensionSystem::IPlugin { public: - PfdQmlPlugin(); - ~PfdQmlPlugin(); + PfdQmlPlugin(); + ~PfdQmlPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList &arguments, QString *errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList &arguments, QString *errorString); + void shutdown(); private: - PfdQmlGadgetFactory *mf; + PfdQmlGadgetFactory *mf; }; #endif /* PFDQMLPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.cpp b/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.cpp index e0124789e..da2080638 100644 --- a/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.cpp +++ b/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.cpp @@ -44,44 +44,43 @@ #include - /** - * Sets the file to use for logging and takes the parent plugin - * to connect to stop logging signal - * @param[in] file File name to write to - * @param[in] parent plugin - */ -bool PowerlogThread::openFile(QString file, PowerlogPlugin * parent) + * Sets the file to use for logging and takes the parent plugin + * to connect to stop logging signal + * @param[in] file File name to write to + * @param[in] parent plugin + */ +bool PowerlogThread::openFile(QString file, PowerlogPlugin *parent) { logFile.setFileName(file); logFile.open(QIODevice::WriteOnly); fileStream.setDevice(&logFile); - connect(parent,SIGNAL(stopLoggingSignal()),this,SLOT(stopLogging())); + connect(parent, SIGNAL(stopLoggingSignal()), this, SLOT(stopLogging())); return true; }; /** - * Get all logs from Powerlog - */ + * Get all logs from Powerlog + */ void PowerlogThread::run() { - - // TODO: pop up a dialog here! qDebug() << "Connect a Junsi PowerLog 6S and watch the logging output"; pjrc_rawhid hidHandle; - int numDevices = hidHandle.open(1, 0x0483,0x5750,0,0); //0xff9c,0x0001); - if( numDevices == 0 ) - numDevices = hidHandle.open(1,0x0483,0,0,0); + int numDevices = hidHandle.open(1, 0x0483, 0x5750, 0, 0); // 0xff9c,0x0001); + if (numDevices == 0) { + numDevices = hidHandle.open(1, 0x0483, 0, 0, 0); + } qDebug() << numDevices << " device(s) opened"; - if (numDevices == 0) + if (numDevices == 0) { return; + } - //hidHandle.mytest(0); + // hidHandle.mytest(0); char buf[BUF_LEN]; buf[0] = 2; @@ -89,119 +88,118 @@ void PowerlogThread::run() fileStream << "Interval,Current,Volt,Cap,Cell1,Cell2,Cell3,Cell4,Cell5,Cell6,RPM,Temp0,Temp1,Temp2,Temp3,Period,Pulse\n"; - while (int received = hidHandle.receive(0, buf, BUF_LEN, 3500) ) { + while (int received = hidHandle.receive(0, buf, BUF_LEN, 3500)) { ShowInf(buf); fileStream.flush(); // Just to be sure... } stopLogging(); - } /** - * Pass this command to the correct thread then close the file - */ + * Pass this command to the correct thread then close the file + */ void PowerlogThread::stopLogging() { QWriteLocker locker(&lock); + fileStream.flush(); logFile.close(); quit(); } /** - * Formats the content of the buffer we just read and write - * to the logfile - */ + * Formats the content of the buffer we just read and write + * to the logfile + */ void PowerlogThread::ShowInf(char *pBuf) { POWERLOG_HID_PACK Inf; int i; int Count; - Count=0; - Inf.Len = pBuf[Count]; - Count += sizeof(Inf.Len); + Count = 0; + Inf.Len = pBuf[Count]; + Count += sizeof(Inf.Len); - Inf.Type = pBuf[Count]; - Count += sizeof(Inf.Type); + Inf.Type = pBuf[Count]; + Count += sizeof(Inf.Type); Inf.Interval = *((DWORD *)&pBuf[Count]); fileStream << QString::number(Inf.Interval) << ","; - Count += sizeof(Inf.Interval); + Count += sizeof(Inf.Interval); Inf.LogState = pBuf[Count]; - Count += sizeof(Inf.LogState); + Count += sizeof(Inf.LogState); - if(((Inf.Type == TYPE_DATA_ONLINE)||(Inf.Type == TYPE_DATA_OFFLINE)) && (Inf.Len == 0x29))//0x27 - { + if (((Inf.Type == TYPE_DATA_ONLINE) || (Inf.Type == TYPE_DATA_OFFLINE)) && (Inf.Len == 0x29)) { // 0x27 Inf.Current = *((SHORT *)&pBuf[Count]); - Count += sizeof(Inf.Current); - GetShowValue(QString("Current"),Inf.Current,5,2); + Count += sizeof(Inf.Current); + GetShowValue(QString("Current"), Inf.Current, 5, 2); Inf.Volt = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.Volt); - GetShowValue(QString("Voltage"),Inf.Volt,5,2); + Count += sizeof(Inf.Volt); + GetShowValue(QString("Voltage"), Inf.Volt, 5, 2); - Inf.Cap = *((DWORD *)&pBuf[Count]); - Count += sizeof(Inf.Cap); - GetShowValue(QString("Cap"),Inf.Cap,6,0); + Inf.Cap = *((DWORD *)&pBuf[Count]); + Count += sizeof(Inf.Cap); + GetShowValue(QString("Cap"), Inf.Cap, 6, 0); - for(i=0;i<6;i++) - { - Inf.Cell[i] = *((SHORT *)&pBuf[Count]); - Count += sizeof(Inf.Cell[i]); + for (i = 0; i < 6; i++) { + Inf.Cell[i] = *((SHORT *)&pBuf[Count]); + Count += sizeof(Inf.Cell[i]); } - GetShowValue(QString("Cell 1"),Inf.Cell[0],5,3); - GetShowValue(QString("Cell 2"),Inf.Cell[1],5,3); - GetShowValue(QString("Cell 3"),Inf.Cell[2],5,3); - GetShowValue(QString("Cell 4"),Inf.Cell[3],5,3); - GetShowValue(QString("Cell 5"),Inf.Cell[4],5,3); - GetShowValue(QString("Cell 6"),Inf.Cell[5],5,3); + GetShowValue(QString("Cell 1"), Inf.Cell[0], 5, 3); + GetShowValue(QString("Cell 2"), Inf.Cell[1], 5, 3); + GetShowValue(QString("Cell 3"), Inf.Cell[2], 5, 3); + GetShowValue(QString("Cell 4"), Inf.Cell[3], 5, 3); + GetShowValue(QString("Cell 5"), Inf.Cell[4], 5, 3); + GetShowValue(QString("Cell 6"), Inf.Cell[5], 5, 3); Inf.RPM = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.RPM); - GetShowValue(QString("RPM"),Inf.RPM,6,0); - - for(i=0;i<4;i++) - { - Inf.Temp[i] = *((SHORT *)&pBuf[Count]); - Count += sizeof(Inf.Temp[i]); + Count += sizeof(Inf.RPM); + GetShowValue(QString("RPM"), Inf.RPM, 6, 0); + for (i = 0; i < 4; i++) { + Inf.Temp[i] = *((SHORT *)&pBuf[Count]); + Count += sizeof(Inf.Temp[i]); } - GetShowValue(QString("Int Temp0"),Inf.Temp[0],4,1); + GetShowValue(QString("Int Temp0"), Inf.Temp[0], 4, 1); - if (Inf.Temp[1]==0x7fff) - fileStream << "0.0,"; - else - GetShowValue(QString("Ext temp1"),Inf.Temp[1],4,1); - - if (Inf.Temp[2]==0x7fff) + if (Inf.Temp[1] == 0x7fff) { fileStream << "0.0,"; - else - GetShowValue(QString("Ext temp2"),Inf.Temp[2],4,1); + } else { + GetShowValue(QString("Ext temp1"), Inf.Temp[1], 4, 1); + } - if (Inf.Temp[3]==0x7fff) + if (Inf.Temp[2] == 0x7fff) { fileStream << "0.0,"; - else - GetShowValue(QString("Ext temp3"),Inf.Temp[3],4,1); + } else { + GetShowValue(QString("Ext temp2"), Inf.Temp[2], 4, 1); + } + + if (Inf.Temp[3] == 0x7fff) { + fileStream << "0.0,"; + } else { + GetShowValue(QString("Ext temp3"), Inf.Temp[3], 4, 1); + } Inf.Period = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.Period); - GetShowValue(QString("Period:"),Inf.Period,6,0); + Count += sizeof(Inf.Period); + GetShowValue(QString("Period:"), Inf.Period, 6, 0); Inf.Pulse = *((USHORT *)&pBuf[Count]); - Count += sizeof(Inf.Pulse); - GetShowValue(QString("Pulse:"),Inf.Pulse,6,0); + Count += sizeof(Inf.Pulse); + GetShowValue(QString("Pulse:"), Inf.Pulse, 6, 0); fileStream << "\n"; } } /** - * Formats a numeric value - */ + * Formats a numeric value + */ void PowerlogThread::GetShowValue(QString label, DWORD Value, WORD Len, WORD Dot) { QString out; @@ -211,23 +209,21 @@ void PowerlogThread::GetShowValue(QString label, DWORD Value, WORD Len, WORD Dot Value = -Value; } - if(Dot==1) - fileStream << Value/10 << "." << Value%10; // printf("%ld.%01lu",Value/10,Value%10); - else if(Dot==2) - fileStream << Value/100 << "." << Value%100; // printf("%ld.%02lu",Value/100,Value%100); - else if(Dot==3) - fileStream << Value/1000 << "." << Value%1000; // printf("%ld.%03lu",Value/1000,Value%1000); - else if(Dot==4) - fileStream << Value/10000 << "." << Value%10000; // printf("%ld.%04lu",Value/10000,Value%10000); - else - fileStream << Value; // printf("%ld",Value); - + if (Dot == 1) { + fileStream << Value / 10 << "." << Value % 10; // printf("%ld.%01lu",Value/10,Value%10); + } else if (Dot == 2) { + fileStream << Value / 100 << "." << Value % 100; // printf("%ld.%02lu",Value/100,Value%100); + } else if (Dot == 3) { + fileStream << Value / 1000 << "." << Value % 1000; // printf("%ld.%03lu",Value/1000,Value%1000); + } else if (Dot == 4) { + fileStream << Value / 10000 << "." << Value % 10000; // printf("%ld.%04lu",Value/10000,Value%10000); + } else { + fileStream << Value; // printf("%ld",Value); + } fileStream << out << ","; - } - /**************************************************************** Logging plugin ********************************/ @@ -237,33 +233,29 @@ PowerlogPlugin::PowerlogPlugin() : devSerialNumber(""), logging(false), loggingThread(NULL) -{ - -} +{} PowerlogPlugin::~PowerlogPlugin() -{ - -} +{} /** - * Add Powerlog plugin entry to File menu - */ -bool PowerlogPlugin::initialize(const QStringList& args, QString *errMsg) + * Add Powerlog plugin entry to File menu + */ +bool PowerlogPlugin::initialize(const QStringList & args, QString *errMsg) { Q_UNUSED(args); Q_UNUSED(errMsg); // Add Menu entry - Core::ActionManager* am = Core::ICore::instance()->actionManager(); - Core::ActionContainer* ac = am->actionContainer(Core::Constants::M_TOOLS); + Core::ActionManager *am = Core::ICore::instance()->actionManager(); + Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_TOOLS); // Command to start logging cmd = am->registerAction(new QAction(this), - "PowerlogPlugin.Transfer", - QList() << - Core::Constants::C_GLOBAL_ID); + "PowerlogPlugin.Transfer", + QList() << + Core::Constants::C_GLOBAL_ID); cmd->action()->setText("Receive from PowerLog6S..."); ac->menu()->addSeparator(); @@ -275,16 +267,16 @@ bool PowerlogPlugin::initialize(const QStringList& args, QString *errMsg) // At this stage we know that other plugins we depend upon are // initialized, in prticular the USB Monitor is now running: USBMonitor *mon = USBMonitor::instance(); - connect(mon,SIGNAL(deviceDiscovered(USBPortInfo)), this, SLOT(devConnected(USBPortInfo))); - connect(mon,SIGNAL(deviceRemoved(USBPortInfo)), this, SLOT(devRemoved(USBPortInfo))); + connect(mon, SIGNAL(deviceDiscovered(USBPortInfo)), this, SLOT(devConnected(USBPortInfo))); + connect(mon, SIGNAL(deviceRemoved(USBPortInfo)), this, SLOT(devRemoved(USBPortInfo))); return true; } /** - * The action that is triggered by the menu item which opens the - * file and begins log reception if successful - */ + * The action that is triggered by the menu item which opens the + * file and begins log reception if successful + */ void PowerlogPlugin::receiveLog() { if (logging) { @@ -293,29 +285,30 @@ void PowerlogPlugin::receiveLog() cmd->action()->setText("Receive from PowerLog6S..."); } else { QString fileName = QFileDialog::getSaveFileName(NULL, tr("Log filename"), - tr("PowerLog-%0.csv").arg(QDateTime::currentDateTime().toString("yyyy-MM-dd_hh-mm-ss")), - tr("Comma Separated Values (*.csv)")); - if (fileName.isEmpty()) + tr("PowerLog-%0.csv").arg(QDateTime::currentDateTime().toString("yyyy-MM-dd_hh-mm-ss")), + tr("Comma Separated Values (*.csv)")); + if (fileName.isEmpty()) { return; + } loggingThread = new PowerlogThread(); - if (loggingThread->openFile(fileName,this)) { + if (loggingThread->openFile(fileName, this)) { loggingThread->start(); cmd->action()->setText("Stop PowerLog6S reception"); logging = true; } } - } /** - Device connected, check whether it is a powerlog & act accordingly - */ + Device connected, check whether it is a powerlog & act accordingly + */ void PowerlogPlugin::devConnected(USBPortInfo port) { - if (devSerialNumber.length() > 0) + if (devSerialNumber.length() > 0) { return; - if ((port.vendorID == 0x0483) && (port.productID==0x5750)) { + } + if ((port.vendorID == 0x0483) && (port.productID == 0x5750)) { devSerialNumber = port.serialNumber; cmd->action()->setEnabled(true); } @@ -323,18 +316,19 @@ void PowerlogPlugin::devConnected(USBPortInfo port) /** - Device Removed, check whether it is a powerlog & act accordingly. - As when the device is removed, we don't get the info on the device, - we have to list all available remaining devices and check if the serial - number of our device is missing... - */ + Device Removed, check whether it is a powerlog & act accordingly. + As when the device is removed, we don't get the info on the device, + we have to list all available remaining devices and check if the serial + number of our device is missing... + */ void PowerlogPlugin::devRemoved(USBPortInfo port) { bool foundDevice; + QList ports = USBMonitor::instance()->availableDevices(); foreach(USBPortInfo port, ports) { - if ((port.vendorID == 0x0483) && (port.productID==0x5750) && - (devSerialNumber == port.serialNumber)) { + if ((port.vendorID == 0x0483) && (port.productID == 0x5750) && + (devSerialNumber == port.serialNumber)) { foundDevice = true; break; } @@ -343,8 +337,9 @@ void PowerlogPlugin::devRemoved(USBPortInfo port) devSerialNumber = QString(""); cmd->action()->setEnabled(false); // Also stop logging in case we were logging: - if (loggingThread) + if (loggingThread) { loggingThread->stopLogging(); + } } } @@ -354,7 +349,7 @@ void PowerlogPlugin::extensionsInitialized() cmd->action()->setEnabled(false); QList ports = USBMonitor::instance()->availableDevices(); foreach(USBPortInfo port, ports) { - if ((port.vendorID == 0x0483) && (port.productID==0x5750)) { + if ((port.vendorID == 0x0483) && (port.productID == 0x5750)) { devSerialNumber = port.serialNumber; cmd->action()->setEnabled(true); break; diff --git a/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.h b/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.h index bcb971447..517f950de 100644 --- a/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.h +++ b/ground/openpilotgcs/src/plugins/powerlog/powerlogplugin.h @@ -41,50 +41,44 @@ using namespace std; -typedef unsigned long ULONG; // 4 Bytes +typedef unsigned long ULONG; // 4 Bytes typedef short SHORT; -typedef unsigned short USHORT; // 2 Bytes -typedef unsigned char BYTE; // 1 Byte -typedef unsigned short WORD; // 2 Bytes -typedef unsigned long DWORD; // 4 Bytes - +typedef unsigned short USHORT; // 2 Bytes +typedef unsigned char BYTE; // 1 Byte +typedef unsigned short WORD; // 2 Bytes +typedef unsigned long DWORD; // 4 Bytes #define BUF_LEN 64 -struct POWERLOG_HID_PACK -{ - BYTE Len; - BYTE Type; - DWORD Interval; - BYTE LogState; - SHORT Current; - USHORT Volt; - DWORD Cap; - SHORT Cell[6]; - USHORT RPM; - SHORT Temp[4]; - USHORT Period; - USHORT Pulse; +struct POWERLOG_HID_PACK { + BYTE Len; + BYTE Type; + DWORD Interval; + BYTE LogState; + SHORT Current; + USHORT Volt; + DWORD Cap; + SHORT Cell[6]; + USHORT RPM; + SHORT Temp[4]; + USHORT Period; + USHORT Pulse; }; -enum -{ - TYPE_DATA_ONLINE = 0x10, - TYPE_DATA_OFFLINE = 0x11, - TYPE_ORDER = 0x20, +enum { + TYPE_DATA_ONLINE = 0x10, + TYPE_DATA_OFFLINE = 0x11, + TYPE_ORDER = 0x20, }; - - class PowerlogPlugin; -class PowerlogThread : public QThread -{ +class PowerlogThread : public QThread { Q_OBJECT public: - bool openFile(QString file, PowerlogPlugin * parent); + bool openFile(QString file, PowerlogPlugin *parent); private slots: @@ -99,12 +93,11 @@ protected: private: void ShowInf(char *pBuf); - void GetShowValue(QString label,DWORD Value,WORD Len,WORD Dot); + void GetShowValue(QString label, DWORD Value, WORD Len, WORD Dot); }; -class PowerlogPlugin : public ExtensionSystem::IPlugin -{ +class PowerlogPlugin : public ExtensionSystem::IPlugin { Q_OBJECT @@ -113,7 +106,7 @@ public: ~PowerlogPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); void setPowerlogMenuTitle(QString str); @@ -130,11 +123,10 @@ private slots: void devRemoved(USBPortInfo); private: - Core::Command* cmd; + Core::Command *cmd; QString devSerialNumber; - PowerlogThread* loggingThread; + PowerlogThread *loggingThread; bool logging; - }; #endif /* POWERLOGPLUGIN_H_ */ /** diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.cpp index 7a65746c7..d5aa1749c 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -31,10 +31,9 @@ #include "qmlviewgadgetconfiguration.h" QmlViewGadget::QmlViewGadget(QString classId, QmlViewGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} QmlViewGadget::~QmlViewGadget() { @@ -42,14 +41,15 @@ QmlViewGadget::~QmlViewGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void QmlViewGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void QmlViewGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - QmlViewGadgetConfiguration *m = qobject_cast(config); + QmlViewGadgetConfiguration *m = qobject_cast(config); + m_widget->setQmlFile(m->dialFile()); m_widget->enableOpenGL(m->useOpenGL()); } diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.h b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.h index 036ce47d0..152c58042 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.h +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,15 +38,17 @@ class QmlViewGadgetWidget; using namespace Core; -class QmlViewGadget : public Core::IUAVGadget -{ +class QmlViewGadget : public Core::IUAVGadget { Q_OBJECT public: QmlViewGadget(QString classId, QmlViewGadgetWidget *widget, QWidget *parent = 0); ~QmlViewGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: QmlViewGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.cpp index 40d096ebb..95183e09b 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,15 +32,15 @@ * Loads a saved configuration or defaults if non exist. * */ -QmlViewGadgetConfiguration::QmlViewGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +QmlViewGadgetConfiguration::QmlViewGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_defaultDial("Unknown") { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { QString dialFile = qSettings->value("dialFile").toString(); useOpenGLFlag = qSettings->value("useOpenGLFlag").toBool(); - m_defaultDial=Utils::PathUtils().InsertDataPath(dialFile); + m_defaultDial = Utils::PathUtils().InsertDataPath(dialFile); } } @@ -51,7 +51,8 @@ QmlViewGadgetConfiguration::QmlViewGadgetConfiguration(QString classId, QSetting IUAVGadgetConfiguration *QmlViewGadgetConfiguration::clone() { QmlViewGadgetConfiguration *m = new QmlViewGadgetConfiguration(this->classId()); - m->m_defaultDial=m_defaultDial; + + m->m_defaultDial = m_defaultDial; m->useOpenGLFlag = useOpenGLFlag; return m; } @@ -60,8 +61,10 @@ IUAVGadgetConfiguration *QmlViewGadgetConfiguration::clone() * Saves a configuration. * */ -void QmlViewGadgetConfiguration::saveConfig(QSettings* qSettings) const { +void QmlViewGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ QString dialFile = Utils::PathUtils().RemoveDataPath(m_defaultDial); + qSettings->setValue("dialFile", dialFile); qSettings->setValue("useOpenGLFlag", useOpenGLFlag); } diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.h index 5e8f4687a..416d5a8db 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetconfiguration.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,21 +32,32 @@ using namespace Core; -class QmlViewGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT +class QmlViewGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit QmlViewGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit QmlViewGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - //set dial configuration functions - void setDialFile(QString dialFile){m_defaultDial=dialFile;} - void setUseOpenGL(bool flag) { useOpenGLFlag = flag; } + // set dial configuration functions + void setDialFile(QString dialFile) + { + m_defaultDial = dialFile; + } + void setUseOpenGL(bool flag) + { + useOpenGLFlag = flag; + } - //get dial configuration functions - QString dialFile() {return m_defaultDial;} - bool useOpenGL() { return useOpenGLFlag; } + // get dial configuration functions + QString dialFile() + { + return m_defaultDial; + } + bool useOpenGL() + { + return useOpenGLFlag; + } - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.cpp index 6d8f0b7c5..20b7693bb 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,29 +32,27 @@ #include QmlViewGadgetFactory::QmlViewGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("QmlViewGadget"), - tr("QML Viewer, QML"), - parent) -{ -} + IUAVGadgetFactory(QString("QmlViewGadget"), + tr("QML Viewer, QML"), + parent) +{} QmlViewGadgetFactory::~QmlViewGadgetFactory() -{ -} +{} -Core::IUAVGadget* QmlViewGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *QmlViewGadgetFactory::createGadget(QWidget *parent) { - QmlViewGadgetWidget* gadgetWidget = new QmlViewGadgetWidget(parent); + QmlViewGadgetWidget *gadgetWidget = new QmlViewGadgetWidget(parent); + return new QmlViewGadget(QString("QmlViewGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *QmlViewGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *QmlViewGadgetFactory::createConfiguration(QSettings *qSettings) { return new QmlViewGadgetConfiguration(QString("QmlViewGadget"), qSettings); } IOptionsPage *QmlViewGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new QmlViewGadgetOptionsPage(qobject_cast(config)); + return new QmlViewGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.h b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.h index babbf2ae6..759778875 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetfactory.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class QmlViewGadgetFactory : public IUAVGadgetFactory -{ +class QmlViewGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: QmlViewGadgetFactory(QObject *parent = 0); ~QmlViewGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.cpp index 9e0c20a6e..15921007b 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -38,24 +38,22 @@ #include QmlViewGadgetOptionsPage::QmlViewGadgetOptionsPage(QmlViewGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *QmlViewGadgetOptionsPage::createPage(QWidget *parent) { Q_UNUSED(parent); options_page = new Ui::QmlViewGadgetOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget; - //main layout + // main layout options_page->setupUi(optionsPageWidget); - // Restore the contents from the settings: options_page->svgSourceFile->setExpectedKind(Utils::PathChooser::File); options_page->svgSourceFile->setPromptDialogFilter(tr("QML file (*.qml)")); @@ -79,5 +77,4 @@ void QmlViewGadgetOptionsPage::apply() } void QmlViewGadgetOptionsPage::finish() -{ -} +{} diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.h index 9fd2f335a..6dfd6c48d 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -40,14 +40,13 @@ class IUAVGadgetConfiguration; class QmlViewGadgetConfiguration; namespace Ui { - class QmlViewGadgetOptionsPage; +class QmlViewGadgetOptionsPage; } using namespace Core; -class QmlViewGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class QmlViewGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit QmlViewGadgetOptionsPage(QmlViewGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp index fda9042ef..4bc172fee 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp @@ -8,7 +8,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -44,35 +44,36 @@ QmlViewGadgetWidget::QmlViewGadgetWidget(QWidget *parent) : QDeclarativeView(parent) { - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setResizeMode(SizeRootObjectToView); QStringList objectsToExport; objectsToExport << "VelocityActual" << - "PositionActual" << - "AttitudeActual" << - "GPSPosition" << - "GCSTelemetryStats" << - "FlightBatteryState"; + "PositionActual" << + "AttitudeActual" << + "GPSPosition" << + "GCSTelemetryStats" << + "FlightBatteryState"; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - foreach (const QString &objectName, objectsToExport) { - UAVObject* object = objManager->getObject(objectName); - if (object) + foreach(const QString &objectName, objectsToExport) { + UAVObject *object = objManager->getObject(objectName); + + if (object) { engine()->rootContext()->setContextProperty(objectName, object); - else + } else { qWarning() << "Failed to load object" << objectName; + } } engine()->rootContext()->setContextProperty("qmlWidget", this); } QmlViewGadgetWidget::~QmlViewGadgetWidget() -{ -} +{} void QmlViewGadgetWidget::setQmlFile(QString fn) { @@ -82,7 +83,7 @@ void QmlViewGadgetWidget::setQmlFile(QString fn) SvgImageProvider *svgProvider = new SvgImageProvider(fn); engine()->addImageProvider("svg", svgProvider); - //it's necessary to allow qml side to query svg element position + // it's necessary to allow qml side to query svg element position engine()->rootContext()->setContextProperty("svgRenderer", svgProvider); engine()->setBaseUrl(QUrl::fromLocalFile(fn)); @@ -95,9 +96,10 @@ void QmlViewGadgetWidget::setQmlFile(QString fn) } /*! - \brief Enables/Disables OpenGL - */ -void QmlViewGadgetWidget::enableOpenGL(bool flag) { + \brief Enables/Disables OpenGL + */ +void QmlViewGadgetWidget::enableOpenGL(bool flag) +{ if (flag) { setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); } else { diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.h b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.h index 642349911..783cc963e 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -40,21 +40,23 @@ class UAVObject; -class QmlViewGadgetWidget : public QDeclarativeView -{ +class QmlViewGadgetWidget : public QDeclarativeView { Q_OBJECT public: QmlViewGadgetWidget(QWidget *parent = 0); - ~QmlViewGadgetWidget(); - void setQmlFile(QString fn); + ~QmlViewGadgetWidget(); + void setQmlFile(QString fn); - void enableOpenGL(bool flag); - void enableSmoothUpdates(bool flag) { beSmooth = flag; } + void enableOpenGL(bool flag); + void enableSmoothUpdates(bool flag) + { + beSmooth = flag; + } private: - // Flag to enable better rendering of fonts in OpenGL - bool beSmooth; - QString m_fn; + // Flag to enable better rendering of fonts in OpenGL + bool beSmooth; + QString m_fn; }; #endif /* QmlViewGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.cpp index e58a3559f..2cda37fe8 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -35,32 +35,31 @@ QmlViewPlugin::QmlViewPlugin() { - // Do nothing + // Do nothing } QmlViewPlugin::~QmlViewPlugin() { - // Do nothing + // Do nothing } -bool QmlViewPlugin::initialize(const QStringList& args, QString *errMsg) +bool QmlViewPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new QmlViewGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new QmlViewGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void QmlViewPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void QmlViewPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(QmlViewPlugin) - diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.h b/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.h index 33ad1d9e5..de21c5821 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.h +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewplugin.h @@ -7,7 +7,7 @@ * @{ * @addtogroup OPMapPlugin QML Viewer Plugin * @{ - * @brief The QML Viewer Gadget + * @brief The QML Viewer Gadget *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -32,16 +32,15 @@ class QmlViewGadgetFactory; -class QmlViewPlugin : public ExtensionSystem::IPlugin -{ +class QmlViewPlugin : public ExtensionSystem::IPlugin { public: - QmlViewPlugin(); - ~QmlViewPlugin(); + QmlViewPlugin(); + ~QmlViewPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - QmlViewGadgetFactory *mf; + QmlViewGadgetFactory *mf; }; #endif /* QMLVIEWPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h index f981f6a94..486f88b92 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h @@ -36,14 +36,14 @@ #include #include "rawhid_global.h" -#if defined( Q_OS_MAC) +#if defined(Q_OS_MAC) #include #include #include #elif defined(Q_OS_UNIX) -//#elif defined(Q_OS_LINUX) +// #elif defined(Q_OS_LINUX) #include #include #include @@ -56,16 +56,15 @@ // ************ -#if defined( Q_OS_MAC) +#if defined(Q_OS_MAC) // todo: #elif defined(Q_OS_UNIX) -//#elif defined(Q_OS_LINUX) +// #elif defined(Q_OS_LINUX) typedef struct hid_struct hid_t; -struct hid_struct -{ +struct hid_struct { usb_dev_handle *usb; int open; int iface; @@ -79,21 +78,17 @@ struct hid_struct typedef struct hid_struct hid_t; -struct hid_struct -{ +struct hid_struct { HANDLE handle; - int open; + int open; struct hid_struct *prev; struct hid_struct *next; }; -#endif +#endif // if defined(Q_OS_MAC) - - -class RAWHID_EXPORT pjrc_rawhid: public QObject -{ +class RAWHID_EXPORT pjrc_rawhid : public QObject { Q_OBJECT public: @@ -105,51 +100,51 @@ public: int send(int num, void *buf, int len, int timeout); QString getserial(int num); signals: - void deviceUnplugged(int); + void deviceUnplugged(int); private: -#if defined( Q_OS_MAC) +#if defined(Q_OS_MAC) - // Static callbacks called by the HID system with handles to the PJRC object - static void attach_callback(void *, IOReturn, void *, IOHIDDeviceRef); - static void dettach_callback(void *, IOReturn, void *hid_mgr, IOHIDDeviceRef dev); - static void input_callback(void *, IOReturn, void *, IOHIDReportType, uint32_t, uint8_t *, CFIndex); - static void timeout_callback(CFRunLoopTimerRef, void *); + // Static callbacks called by the HID system with handles to the PJRC object + static void attach_callback(void *, IOReturn, void *, IOHIDDeviceRef); + static void dettach_callback(void *, IOReturn, void *hid_mgr, IOHIDDeviceRef dev); + static void input_callback(void *, IOReturn, void *, IOHIDReportType, uint32_t, uint8_t *, CFIndex); + static void timeout_callback(CFRunLoopTimerRef, void *); - // Non static methods to call into - void attach(IOHIDDeviceRef dev); - void dettach(IOHIDDeviceRef dev); - void input(uint8_t *, CFIndex); + // Non static methods to call into + void attach(IOHIDDeviceRef dev); + void dettach(IOHIDDeviceRef dev); + void input(uint8_t *, CFIndex); - // Platform specific handles for the USB device - IOHIDManagerRef hid_manager; - IOHIDDeviceRef dev; - CFRunLoopRef the_correct_runloop; - CFRunLoopRef received_runloop; + // Platform specific handles for the USB device + IOHIDManagerRef hid_manager; + IOHIDDeviceRef dev; + CFRunLoopRef the_correct_runloop; + CFRunLoopRef received_runloop; - static const int BUFFER_SIZE = 64; - uint8_t buffer[BUFFER_SIZE]; - int attach_count; - int buffer_count; - bool device_open; - bool unplugged; + static const int BUFFER_SIZE = 64; + uint8_t buffer[BUFFER_SIZE]; + int attach_count; + int buffer_count; + bool device_open; + bool unplugged; - QMutex *m_writeMutex; - QMutex *m_readMutex; + QMutex *m_writeMutex; + QMutex *m_readMutex; #elif defined(Q_OS_UNIX) - hid_t *first_hid; + hid_t * first_hid; hid_t *last_hid; void add_hid(hid_t *h); - hid_t * get_hid(int num); + hid_t *get_hid(int num); void free_all_hid(void); void hid_close(hid_t *hid); - int hid_parse_item(uint32_t *val, uint8_t **data, const uint8_t *end); + int hid_parse_item(uint32_t *val, uint8_t * *data, const uint8_t *end); #elif defined(Q_OS_WIN32) - hid_t *first_hid; + hid_t * first_hid; hid_t *last_hid; HANDLE rx_event; HANDLE tx_event; @@ -157,12 +152,12 @@ private: CRITICAL_SECTION tx_mutex; void add_hid(hid_t *h); - hid_t * get_hid(int num); + hid_t *get_hid(int num); void free_all_hid(void); void hid_close(hid_t *hid); void print_win32_err(DWORD err); -#endif +#endif // if defined(Q_OS_MAC) }; -#endif +#endif // ifndef PJRC_RAWHID_H diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp index 80d5deda5..1ca909ccd 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp @@ -54,7 +54,7 @@ pjrc_rawhid::pjrc_rawhid() : device_open(false), hid_manager(NULL), buffer_count(0), unplugged(false) { m_writeMutex = new QMutex(); - m_readMutex = new QMutex(); + m_readMutex = new QMutex(); } pjrc_rawhid::~pjrc_rawhid() @@ -75,14 +75,14 @@ pjrc_rawhid::~pjrc_rawhid() } /** - * @brief open - open 1 or more devices - * @param[in] max maximum number of devices to open - * @param[in] vid Vendor ID, or -1 if any - * @param[in] pid Product ID, or -1 if any - * @param[in] usage_page top level usage page, or -1 if any - * @param[in] usage top level usage number, or -1 if any - * @returns actual number of devices opened - */ + * @brief open - open 1 or more devices + * @param[in] max maximum number of devices to open + * @param[in] vid Vendor ID, or -1 if any + * @param[in] pid Product ID, or -1 if any + * @param[in] usage_page top level usage page, or -1 if any + * @param[in] usage top level usage number, or -1 if any + * @returns actual number of devices opened + */ int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) { CFMutableDictionaryRef dict; @@ -95,9 +95,11 @@ int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) attach_count = 0; // Start the HID Manager - hid_manager = IOHIDManagerCreate(kCFAllocatorDefault, kIOHIDOptionsTypeNone); + hid_manager = IOHIDManagerCreate(kCFAllocatorDefault, kIOHIDOptionsTypeNone); if (hid_manager == NULL || CFGetTypeID(hid_manager) != IOHIDManagerGetTypeID()) { - if (hid_manager) CFRelease(hid_manager); + if (hid_manager) { + CFRelease(hid_manager); + } return 0; } @@ -105,7 +107,9 @@ int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) // Tell the HID Manager what type of devices we want dict = CFDictionaryCreateMutable(kCFAllocatorDefault, 0, &kCFTypeDictionaryKeyCallBacks, &kCFTypeDictionaryValueCallBacks); - if (!dict) return 0; + if (!dict) { + return 0; + } if (vid > 0) { num = CFNumberCreate(kCFAllocatorDefault, kCFNumberIntType, &vid); CFDictionarySetValue(dict, CFSTR(kIOHIDVendorIDKey), num); @@ -149,7 +153,9 @@ int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) } // let it do the callback for all devices - while (CFRunLoopRunInMode(kCFRunLoopDefaultMode, 0, true) == kCFRunLoopRunHandledSource) ; + while (CFRunLoopRunInMode(kCFRunLoopDefaultMode, 0, true) == kCFRunLoopRunHandledSource) { + ; + } // count up how many were added by the callback return attach_count; @@ -166,18 +172,20 @@ int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) int pjrc_rawhid::receive(int, void *buf, int len, int timeout) { QMutexLocker locker(m_readMutex); + Q_UNUSED(locker); - if (!device_open) + if (!device_open) { return -1; + } // Pass information to the callback to stop this run loop and signal if a timeout occurred struct timeout_info info; - info.loopRef = CFRunLoopGetCurrent();; + info.loopRef = CFRunLoopGetCurrent();; info.timed_out = false; CFRunLoopTimerContext context; memset(&context, 0, sizeof(context)); - context.info = &info; + context.info = &info; // Set up the timer for the timeout CFRunLoopTimerRef timer; @@ -187,9 +195,11 @@ int pjrc_rawhid::receive(int, void *buf, int len, int timeout) received_runloop = CFRunLoopGetCurrent(); // Run the CFRunLoop until either a timeout or data is available - while(1) { + while (1) { if (buffer_count != 0) { - if (len > buffer_count) len = buffer_count; + if (len > buffer_count) { + len = buffer_count; + } memcpy(buf, buffer, len); buffer_count = 0; break; @@ -212,14 +222,14 @@ int pjrc_rawhid::receive(int, void *buf, int len, int timeout) * @brief Helper class that will workaround the fact * that the HID send is broken on OSX */ -class Sender : public QThread -{ +class Sender : public QThread { public: - Sender(IOHIDDeviceRef d, uint8_t * b, int l) : - dev(d), buf(b), len(l), result(-1) { } + Sender(IOHIDDeviceRef d, uint8_t *b, int l) : + dev(d), buf(b), len(l), result(-1) {} - void run() { - ret = IOHIDDeviceSetReport(dev, kIOHIDReportTypeOutput, 2, buf, len); + void run() + { + ret = IOHIDDeviceSetReport(dev, kIOHIDReportTypeOutput, 2, buf, len); result = (ret == kIOReturnSuccess) ? len : -1; } @@ -227,7 +237,7 @@ public: IOReturn ret; private: IOHIDDeviceRef dev; - uint8_t * buf; + uint8_t *buf; int len; }; @@ -244,14 +254,15 @@ int pjrc_rawhid::send(int, void *buf, int len, int timeout) // This lock ensures that when closing we don't do it until the // write has terminated (and then the device_open flag is set to false) QMutexLocker locker(m_writeMutex); + Q_UNUSED(locker); - if(!device_open || unplugged) { + if (!device_open || unplugged) { return -1; } - uint8_t *report_buf = (uint8_t *) malloc(len); - memcpy(&report_buf[0], buf,len); + uint8_t *report_buf = (uint8_t *)malloc(len); + memcpy(&report_buf[0], buf, len); QEventLoop el; Sender sender(dev, report_buf, len); @@ -263,29 +274,31 @@ int pjrc_rawhid::send(int, void *buf, int len, int timeout) return sender.result; } -//! Get the serial number for a HID device -QString pjrc_rawhid::getserial(int num) { +// ! Get the serial number for a HID device +QString pjrc_rawhid::getserial(int num) +{ QMutexLocker locker(m_readMutex); + Q_UNUSED(locker); - if (!device_open || unplugged) + if (!device_open || unplugged) { return ""; + } CFTypeRef serialnum = IOHIDDeviceGetProperty(dev, CFSTR(kIOHIDSerialNumberKey)); - if(serialnum && CFGetTypeID(serialnum) == CFStringGetTypeID()) - { - //Note: I'm not sure it will always succeed if encoded as MacRoman but that - //is a superset of UTF8 so I think this is fine - CFStringRef str = static_cast(serialnum); + if (serialnum && CFGetTypeID(serialnum) == CFStringGetTypeID()) { + // Note: I'm not sure it will always succeed if encoded as MacRoman but that + // is a superset of UTF8 so I think this is fine + CFStringRef str = static_cast(serialnum); int length = CFStringGetLength(str); if (length == 0) { return ""; } - char* ptr = (char*)malloc(length+1); - Boolean ret = CFStringGetCString(str, ptr, length+1, kCFStringEncodingMacRoman); + char *ptr = (char *)malloc(length + 1); + Boolean ret = CFStringGetCString(str, ptr, length + 1, kCFStringEncodingMacRoman); QString strResult; if (ret == true) { - strResult = ptr; + strResult = ptr; } free(ptr); return strResult; @@ -294,7 +307,7 @@ QString pjrc_rawhid::getserial(int num) { return QString("Error"); } -//! Close the HID device +// ! Close the HID device void pjrc_rawhid::close(int) { // Make sure any pending locks are done @@ -326,47 +339,56 @@ void pjrc_rawhid::close(int) */ void pjrc_rawhid::input(uint8_t *data, CFIndex len) { - if (!device_open) + if (!device_open) { return; + } - if (len > BUFFER_SIZE) len = BUFFER_SIZE; + if (len > BUFFER_SIZE) { + len = BUFFER_SIZE; + } // Note: packet preprocessing done in OS independent code memcpy(buffer, &data[0], len); buffer_count = len; - if (received_runloop) + if (received_runloop) { CFRunLoopStop(received_runloop); + } } -//! Callback for the HID driver on an input report +// ! Callback for the HID driver on an input report void pjrc_rawhid::input_callback(void *c, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len) { - if (ret != kIOReturnSuccess || len < 1) return; + if (ret != kIOReturnSuccess || len < 1) { + return; + } - pjrc_rawhid *context = (pjrc_rawhid *) c; + pjrc_rawhid *context = (pjrc_rawhid *)c; context->input(data, len); } -//! Timeout used for the +// ! Timeout used for the void pjrc_rawhid::timeout_callback(CFRunLoopTimerRef, void *i) { - struct timeout_info *info = (struct timeout_info *) i; + struct timeout_info *info = (struct timeout_info *)i; + info->timed_out = true; CFRunLoopStop(info->loopRef); } -//! Called on a dettach event +// ! Called on a dettach event void pjrc_rawhid::dettach(IOHIDDeviceRef d) { unplugged = true; - if (d == dev) + if (d == dev) { emit deviceUnplugged(0); + } } -//! Called from the USB system and forwarded to the instance (context) +// ! Called from the USB system and forwarded to the instance (context) void pjrc_rawhid::dettach_callback(void *context, IOReturn, void *, IOHIDDeviceRef dev) { - pjrc_rawhid *p = (pjrc_rawhid*) context; + pjrc_rawhid *p = (pjrc_rawhid *)context; + p->dettach(dev); } @@ -379,7 +401,9 @@ void pjrc_rawhid::attach(IOHIDDeviceRef d) // Store the device handle dev = d; - if (IOHIDDeviceOpen(dev, kIOHIDOptionsTypeNone) != kIOReturnSuccess) return; + if (IOHIDDeviceOpen(dev, kIOHIDOptionsTypeNone) != kIOReturnSuccess) { + return; + } // Disconnect the attach callback since we don't want to automatically reconnect IOHIDManagerRegisterDeviceMatchingCallback(hid_manager, NULL, NULL); IOHIDDeviceScheduleWithRunLoop(dev, the_correct_runloop, kCFRunLoopDefaultMode); @@ -387,13 +411,13 @@ void pjrc_rawhid::attach(IOHIDDeviceRef d) attach_count++; device_open = true; - unplugged = false; + unplugged = false; } -//! Called from the USB system and forwarded to the instance (context) +// ! Called from the USB system and forwarded to the instance (context) void pjrc_rawhid::attach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) { - pjrc_rawhid *p = (pjrc_rawhid*) context; + pjrc_rawhid *p = (pjrc_rawhid *)context; + p->attach(dev); } - diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_unix.cpp b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_unix.cpp index bd44a9144..f8f123c3a 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_unix.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_unix.cpp @@ -43,204 +43,232 @@ pjrc_rawhid::pjrc_rawhid() { first_hid = NULL; - last_hid = NULL; + last_hid = NULL; } pjrc_rawhid::~pjrc_rawhid() -{ -} +{} -// open - open 1 or more devices +// open - open 1 or more devices // -// Inputs: -// max = maximum number of devices to open -// vid = Vendor ID, or -1 if any -// pid = Product ID, or -1 if any -// usage_page = top level usage page, or -1 if any -// usage = top level usage number, or -1 if any -// Output: -// actual number of devices opened +// Inputs: +// max = maximum number of devices to open +// vid = Vendor ID, or -1 if any +// pid = Product ID, or -1 if any +// usage_page = top level usage page, or -1 if any +// usage = top level usage number, or -1 if any +// Output: +// actual number of devices opened // int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) { - struct usb_bus *bus; - struct usb_device *dev; - struct usb_interface *iface; - struct usb_interface_descriptor *desc; - struct usb_endpoint_descriptor *ep; - usb_dev_handle *u; - uint8_t buf[1024], *p; - int i, n, len, tag, ep_in, ep_out, count=0, claimed; - uint32_t val=0, parsed_usage, parsed_usage_page; - hid_t *hid; + struct usb_bus *bus; + struct usb_device *dev; + struct usb_interface *iface; + struct usb_interface_descriptor *desc; + struct usb_endpoint_descriptor *ep; + usb_dev_handle *u; + uint8_t buf[1024], *p; + int i, n, len, tag, ep_in, ep_out, count = 0, claimed; + uint32_t val = 0, parsed_usage, parsed_usage_page; + hid_t *hid; - if (first_hid) free_all_hid(); - //printf("pjrc_rawhid_open, max=%d\n", max); + if (first_hid) { + free_all_hid(); + } + // printf("pjrc_rawhid_open, max=%d\n", max); - if (max < 1) return 0; + if (max < 1) { + return 0; + } - usb_init(); - usb_find_busses(); - usb_find_devices(); + usb_init(); + usb_find_busses(); + usb_find_devices(); - for (bus = usb_get_busses(); bus; bus = bus->next) - { - for (dev = bus->devices; dev; dev = dev->next) - { - if (vid > 0 && dev->descriptor.idVendor != vid) continue; - if (pid > 0 && dev->descriptor.idProduct != pid) continue; - if (!dev->config) continue; - if (dev->config->bNumInterfaces < 1) continue; - printf("device: vid=%04X, pic=%04X, with %d iface", + for (bus = usb_get_busses(); bus; bus = bus->next) { + for (dev = bus->devices; dev; dev = dev->next) { + if (vid > 0 && dev->descriptor.idVendor != vid) { + continue; + } + if (pid > 0 && dev->descriptor.idProduct != pid) { + continue; + } + if (!dev->config) { + continue; + } + if (dev->config->bNumInterfaces < 1) { + continue; + } + printf("device: vid=%04X, pic=%04X, with %d iface", dev->descriptor.idVendor, dev->descriptor.idProduct, dev->config->bNumInterfaces); - iface = dev->config->interface; - u = NULL; - claimed = 0; - for (i=0; iconfig->bNumInterfaces && iface; i++, iface++) - { - desc = iface->altsetting; - if (!desc) continue; + iface = dev->config->interface; + u = NULL; + claimed = 0; + for (i = 0; i < dev->config->bNumInterfaces && iface; i++, iface++) { + desc = iface->altsetting; + if (!desc) { + continue; + } - printf(" type %d, %d, %d", desc->bInterfaceClass, desc->bInterfaceSubClass, desc->bInterfaceProtocol); + printf(" type %d, %d, %d", desc->bInterfaceClass, desc->bInterfaceSubClass, desc->bInterfaceProtocol); - if (desc->bInterfaceClass != 3) continue; - if (desc->bInterfaceSubClass != 0) continue; - if (desc->bInterfaceProtocol != 0) continue; + if (desc->bInterfaceClass != 3) { + continue; + } + if (desc->bInterfaceSubClass != 0) { + continue; + } + if (desc->bInterfaceProtocol != 0) { + continue; + } - ep = desc->endpoint; - ep_in = ep_out = 0; - for (n = 0; n < desc->bNumEndpoints; n++, ep++) - { - if (ep->bEndpointAddress & 0x80) - { - if (!ep_in) ep_in = ep->bEndpointAddress & 0x7F; - qDebug() << " IN endpoint " << ep_in; - } - else - { - if (!ep_out) ep_out = ep->bEndpointAddress; - qDebug() << " OUT endpoint " << ep_out; - } - } - if (!ep_in) continue; + ep = desc->endpoint; + ep_in = ep_out = 0; + for (n = 0; n < desc->bNumEndpoints; n++, ep++) { + if (ep->bEndpointAddress & 0x80) { + if (!ep_in) { + ep_in = ep->bEndpointAddress & 0x7F; + } + qDebug() << " IN endpoint " << ep_in; + } else { + if (!ep_out) { + ep_out = ep->bEndpointAddress; + } + qDebug() << " OUT endpoint " << ep_out; + } + } + if (!ep_in) { + continue; + } - if (!u) - { - u = usb_open(dev); - if (!u) - { - qDebug() << " unable to open device"; - break; - } - } - qDebug() << " hid interface (generic)"; - if (usb_get_driver_np(u, i, (char *)buf, sizeof(buf)) >= 0) - { - printf(" in use by driver \"%s\"", buf); - if (usb_detach_kernel_driver_np(u, i) < 0) - { - printf(" unable to detach from kernel"); - continue; - } - } + if (!u) { + u = usb_open(dev); + if (!u) { + qDebug() << " unable to open device"; + break; + } + } + qDebug() << " hid interface (generic)"; + if (usb_get_driver_np(u, i, (char *)buf, sizeof(buf)) >= 0) { + printf(" in use by driver \"%s\"", buf); + if (usb_detach_kernel_driver_np(u, i) < 0) { + printf(" unable to detach from kernel"); + continue; + } + } - if (usb_claim_interface(u, i) < 0) - { - printf(" unable claim interface %d", i); - continue; - } + if (usb_claim_interface(u, i) < 0) { + printf(" unable claim interface %d", i); + continue; + } - len = usb_control_msg(u, 0x81, 6, 0x2200, i, (char *)buf, sizeof(buf), 250); - printf(" descriptor, len=%d", len); - if (len < 2) - { - usb_release_interface(u, i); - continue; - } + len = usb_control_msg(u, 0x81, 6, 0x2200, i, (char *)buf, sizeof(buf), 250); + printf(" descriptor, len=%d", len); + if (len < 2) { + usb_release_interface(u, i); + continue; + } - p = buf; - parsed_usage_page = parsed_usage = 0; - while ((tag = hid_parse_item(&val, &p, buf + len)) >= 0) - { - printf(" tag: %X, val %X", tag, val); - if (tag == 4) parsed_usage_page = val; - if (tag == 8) parsed_usage = val; - if (parsed_usage_page && parsed_usage) break; - } - if ((!parsed_usage_page) || (!parsed_usage) || + p = buf; + parsed_usage_page = parsed_usage = 0; + while ((tag = hid_parse_item(&val, &p, buf + len)) >= 0) { + printf(" tag: %X, val %X", tag, val); + if (tag == 4) { + parsed_usage_page = val; + } + if (tag == 8) { + parsed_usage = val; + } + if (parsed_usage_page && parsed_usage) { + break; + } + } + if ((!parsed_usage_page) || (!parsed_usage) || (usage_page > 0 && parsed_usage_page != (uint32_t)usage_page) || - (usage > 0 && parsed_usage != (uint32_t)usage)) - { - usb_release_interface(u, i); - continue; - } + (usage > 0 && parsed_usage != (uint32_t)usage)) { + usb_release_interface(u, i); + continue; + } - hid = (struct hid_struct *)malloc(sizeof(struct hid_struct)); - if (!hid) - { - usb_release_interface(u, i); - continue; - } + hid = (struct hid_struct *)malloc(sizeof(struct hid_struct)); + if (!hid) { + usb_release_interface(u, i); + continue; + } - hid->usb = u; - hid->iface = i; - hid->ep_in = ep_in; - hid->ep_out = ep_out; - hid->open = 1; - add_hid(hid); + hid->usb = u; + hid->iface = i; + hid->ep_in = ep_in; + hid->ep_out = ep_out; + hid->open = 1; + add_hid(hid); - claimed++; - count++; - if (count >= max) return count; - } + claimed++; + count++; + if (count >= max) { + return count; + } + } - if (u && !claimed) usb_close(u); - } - } + if (u && !claimed) { + usb_close(u); + } + } + } - return count; + return count; } -// recveive - receive a packet -// Inputs: -// num = device to receive from (zero based) -// buf = buffer to receive packet -// len = buffer's size -// timeout = time to wait, in milliseconds -// Output: -// number of bytes received, or -1 on error +// recveive - receive a packet +// Inputs: +// num = device to receive from (zero based) +// buf = buffer to receive packet +// len = buffer's size +// timeout = time to wait, in milliseconds +// Output: +// number of bytes received, or -1 on error // int pjrc_rawhid::receive(int num, void *buf, int len, int timeout) { - if (!buf) return -1; + if (!buf) { + return -1; + } - hid_t *hid = get_hid(num); - if (!hid || !hid->open) return -1; - - int r = usb_interrupt_read(hid->usb, hid->ep_in, (char *)buf, len, timeout); - if (r >= 0) return r; - if (r == -110) return 0; // timeout + hid_t *hid = get_hid(num); + if (!hid || !hid->open) { + return -1; + } + int r = usb_interrupt_read(hid->usb, hid->ep_in, (char *)buf, len, timeout); + if (r >= 0) { + return r; + } + if (r == -110) { + return 0; // timeout + } return -1; } -// send - send a packet -// Inputs: -// num = device to transmit to (zero based) -// buf = buffer containing packet to send -// len = number of bytes to transmit -// timeout = time to wait, in milliseconds -// Output: -// number of bytes sent, or -1 on error +// send - send a packet +// Inputs: +// num = device to transmit to (zero based) +// buf = buffer containing packet to send +// len = number of bytes to transmit +// timeout = time to wait, in milliseconds +// Output: +// number of bytes sent, or -1 on error // int pjrc_rawhid::send(int num, void *buf, int len, int timeout) { hid_t *hid; hid = get_hid(num); - if (!hid || !hid->open) return -1; + if (!hid || !hid->open) { + return -1; + } if (hid->ep_out) { return usb_interrupt_write(hid->usb, hid->ep_out, (char *)buf, len, timeout); } else { @@ -248,34 +276,38 @@ int pjrc_rawhid::send(int num, void *buf, int len, int timeout) } } -// getserial - get the serialnumber of the device +// getserial - get the serialnumber of the device // -// Inputs: -// num = device to close (zero based) -// buf = buffer to read the serialnumber into -// Output -// number of bytes in found, or -1 on error +// Inputs: +// num = device to close (zero based) +// buf = buffer to read the serialnumber into +// Output +// number of bytes in found, or -1 on error // -QString pjrc_rawhid::getserial(int num) { +QString pjrc_rawhid::getserial(int num) +{ hid_t *hid; char buf[128]; + hid = get_hid(num); - if (!hid || !hid->open) return QString(""); + if (!hid || !hid->open) { + return QString(""); + } int retlen = usb_get_string_simple(hid->usb, 3, buf, 128); - return QString().fromAscii(buf,-1); + return QString().fromAscii(buf, -1); } -// close - close a device +// close - close a device // -// Inputs: -// num = device to close (zero based) -// Output -// (nothing) +// Inputs: +// num = device to close (zero based) +// Output +// (nothing) // void pjrc_rawhid::close(int num) { - hid_close(get_hid(num)); + hid_close(get_hid(num)); } // Chuck Robey wrote a real HID report parser @@ -284,25 +316,31 @@ void pjrc_rawhid::close(int num) // this tiny thing only needs to extract the top-level usage page // and usage, and even then is may not be truly correct, but it does // work with the Teensy Raw HID example. -int pjrc_rawhid::hid_parse_item(uint32_t *val, uint8_t **data, const uint8_t *end) +int pjrc_rawhid::hid_parse_item(uint32_t *val, uint8_t * *data, const uint8_t *end) { const uint8_t *p = *data; uint8_t tag; - int table[4] = {0, 1, 2, 4}; + int table[4] = { 0, 1, 2, 4 }; int len; - if (p >= end) return -1; + if (p >= end) { + return -1; + } if (p[0] == 0xFE) { // long item, HID 1.11, 6.2.2.3, page 27 - if (p + 5 >= end || p + p[1] >= end) return -1; - tag = p[2]; + if (p + 5 >= end || p + p[1] >= end) { + return -1; + } + tag = p[2]; *val = 0; - len = p[1] + 5; + len = p[1] + 5; } else { // short item, HID 1.11, 6.2.2.2, page 26 tag = p[0] & 0xFC; len = table[p[0] & 0x03]; - if (p + len + 1 >= end) return -1; + if (p + len + 1 >= end) { + return -1; + } switch (p[0] & 0x03) { case 3: *val = p[1] | (p[2] << 8) | (p[3] << 16) | (p[4] << 24); break; case 2: *val = p[1] | (p[2] << 8); break; @@ -316,36 +354,40 @@ int pjrc_rawhid::hid_parse_item(uint32_t *val, uint8_t **data, const uint8_t *en void pjrc_rawhid::add_hid(hid_t *h) { - if (!h) return; + if (!h) { + return; + } - if (!first_hid || !last_hid) - { + if (!first_hid || !last_hid) { first_hid = last_hid = h; - h->next = h->prev = NULL; + h->next = h->prev = NULL; return; } last_hid->next = h; - h->prev = last_hid; - h->next = NULL; + h->prev = last_hid; + h->next = NULL; last_hid = h; } -hid_t * pjrc_rawhid::get_hid(int num) +hid_t *pjrc_rawhid::get_hid(int num) { - hid_t *p = NULL; - for (p = first_hid; p && num > 0; p = p->next, num--) ; + hid_t *p = NULL; + + for (p = first_hid; p && num > 0; p = p->next, num--) { + ; + } return p; } void pjrc_rawhid::free_all_hid(void) { - for (hid_t *p = first_hid; p; p = p->next) + for (hid_t *p = first_hid; p; p = p->next) { hid_close(p); + } - hid_t *p = first_hid; - while (p) - { - hid_t *q = p; + hid_t *p = first_hid; + while (p) { + hid_t *q = p; p = p->next; free(q); } @@ -355,20 +397,25 @@ void pjrc_rawhid::free_all_hid(void) void pjrc_rawhid::hid_close(hid_t *hid) { - if (!hid) return; - if (!hid->open) return; - - usb_release_interface(hid->usb, hid->iface); - - int others = 0; - for (hid_t *p = first_hid; p; p = p->next) - { - if (p->open && p->usb == hid->usb) - others++; + if (!hid) { + return; + } + if (!hid->open) { + return; } - if (!others) - usb_close(hid->usb); - hid->usb = NULL; + usb_release_interface(hid->usb, hid->iface); + + int others = 0; + for (hid_t *p = first_hid; p; p = p->next) { + if (p->open && p->usb == hid->usb) { + others++; + } + } + if (!others) { + usb_close(hid->usb); + } + + hid->usb = NULL; hid->open = 0; } diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_win.cpp b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_win.cpp index 4a5421de6..37d4a9075 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_win.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_win.cpp @@ -43,436 +43,441 @@ pjrc_rawhid::pjrc_rawhid() { - first_hid = NULL; - last_hid = NULL; - rx_event = NULL; - tx_event = NULL; + last_hid = NULL; + rx_event = NULL; + tx_event = NULL; } pjrc_rawhid::~pjrc_rawhid() -{ +{} -} - -// open - open 1 or more devices +// open - open 1 or more devices // -// Inputs: -// max = maximum number of devices to open -// vid = Vendor ID, or -1 if any -// pid = Product ID, or -1 if any -// usage_page = top level usage page, or -1 if any -// usage = top level usage number, or -1 if any -// Output: -// actual number of devices opened +// Inputs: +// max = maximum number of devices to open +// vid = Vendor ID, or -1 if any +// pid = Product ID, or -1 if any +// usage_page = top level usage page, or -1 if any +// usage = top level usage number, or -1 if any +// Output: +// actual number of devices opened // int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) { - GUID guid; - HDEVINFO info; - DWORD index=0, reqd_size; - SP_DEVICE_INTERFACE_DATA iface; - SP_DEVICE_INTERFACE_DETAIL_DATA *details; - HIDD_ATTRIBUTES attrib; - PHIDP_PREPARSED_DATA hid_data; - HIDP_CAPS capabilities; - HANDLE h; - BOOL ret; - hid_t *hid; + GUID guid; + HDEVINFO info; + DWORD index = 0, reqd_size; + SP_DEVICE_INTERFACE_DATA iface; + SP_DEVICE_INTERFACE_DETAIL_DATA *details; + HIDD_ATTRIBUTES attrib; + PHIDP_PREPARSED_DATA hid_data; + HIDP_CAPS capabilities; + HANDLE h; + BOOL ret; + hid_t *hid; - int count=0; + int count = 0; - if (first_hid) free_all_hid(); - - if (max < 1) return 0; - - if (!rx_event) - { - rx_event = CreateEvent(NULL, TRUE, TRUE, NULL); - tx_event = CreateEvent(NULL, TRUE, TRUE, NULL); - InitializeCriticalSection(&rx_mutex); - InitializeCriticalSection(&tx_mutex); + if (first_hid) { + free_all_hid(); } - HidD_GetHidGuid(&guid); + if (max < 1) { + return 0; + } - info = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE); - if (info == INVALID_HANDLE_VALUE) return 0; + if (!rx_event) { + rx_event = CreateEvent(NULL, TRUE, TRUE, NULL); + tx_event = CreateEvent(NULL, TRUE, TRUE, NULL); + InitializeCriticalSection(&rx_mutex); + InitializeCriticalSection(&tx_mutex); + } - for (index=0; 1 ;index++) - { - iface.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); - ret = SetupDiEnumDeviceInterfaces(info, NULL, &guid, index, &iface); - if (!ret) return count; + HidD_GetHidGuid(&guid); - SetupDiGetInterfaceDeviceDetail(info, &iface, NULL, 0, &reqd_size, NULL); - details = (SP_DEVICE_INTERFACE_DETAIL_DATA *)malloc(reqd_size); - if (details == NULL) continue; + info = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE); + if (info == INVALID_HANDLE_VALUE) { + return 0; + } - memset(details, 0, reqd_size); - details->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA); - ret = SetupDiGetDeviceInterfaceDetail(info, &iface, details, reqd_size, NULL, NULL); - if (!ret) - { - free(details); - continue; - } - - h = CreateFile(details->DevicePath, GENERIC_READ|GENERIC_WRITE, FILE_SHARE_READ|FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL); - if (h == INVALID_HANDLE_VALUE) - { - DWORD err = GetLastError(); - - // I get ERROR_ACCESS_DENIED with most/all my input devices (mice/trackballs/tablet). - // Let's not log it :) - if (err == ERROR_ACCESS_DENIED) - { - free(details); - continue; - } - - // qDebug wipes the GetLastError() it seems, so do that after print_win32_err(). - print_win32_err(err); - qDebug() << "Problem opening handle, path: " << QString().fromWCharArray(details->DevicePath); - - free(details); - continue; - } - - free(details); - - attrib.Size = sizeof(HIDD_ATTRIBUTES); - ret = HidD_GetAttributes(h, &attrib); - //printf("vid: %4x\n", attrib.VendorID); - if (!ret || (vid > 0 && attrib.VendorID != vid) || - (pid > 0 && attrib.ProductID != pid) || - !HidD_GetPreparsedData(h, &hid_data)) - { - CloseHandle(h); - continue; - } - - if (!HidP_GetCaps(hid_data, &capabilities) || - (usage_page > 0 && capabilities.UsagePage != usage_page) || - (usage > 0 && capabilities.Usage != usage)) - { - HidD_FreePreparsedData(hid_data); - CloseHandle(h); - continue; - } - - HidD_FreePreparsedData(hid_data); - - hid = (struct hid_struct *)malloc(sizeof(struct hid_struct)); - if (!hid) - { - CloseHandle(h); - continue; - } - -// COMMTIMEOUTS CommTimeouts; -// CommTimeouts.ReadIntervalTimeout = 100; // 100ms -// CommTimeouts.ReadTotalTimeoutConstant = 5; // ms -// CommTimeouts.ReadTotalTimeoutMultiplier = 1; // -// CommTimeouts.WriteTotalTimeoutConstant = 5; // ms -// CommTimeouts.WriteTotalTimeoutMultiplier = 1; // -// if (!SetCommTimeouts(h, &CommTimeouts)) -// { -//// DWORD err = GetLastError(); -// -// } - - qDebug("Open: Handle address: %li for num: %i", (long int) h, count); - - hid->handle = h; - add_hid(hid); - - count++; - if (count >= max) return count; + for (index = 0; 1; index++) { + iface.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); + ret = SetupDiEnumDeviceInterfaces(info, NULL, &guid, index, &iface); + if (!ret) { + return count; } - return count; + SetupDiGetInterfaceDeviceDetail(info, &iface, NULL, 0, &reqd_size, NULL); + details = (SP_DEVICE_INTERFACE_DETAIL_DATA *)malloc(reqd_size); + if (details == NULL) { + continue; + } + + memset(details, 0, reqd_size); + details->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA); + ret = SetupDiGetDeviceInterfaceDetail(info, &iface, details, reqd_size, NULL, NULL); + if (!ret) { + free(details); + continue; + } + + h = CreateFile(details->DevicePath, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL); + if (h == INVALID_HANDLE_VALUE) { + DWORD err = GetLastError(); + + // I get ERROR_ACCESS_DENIED with most/all my input devices (mice/trackballs/tablet). + // Let's not log it :) + if (err == ERROR_ACCESS_DENIED) { + free(details); + continue; + } + + // qDebug wipes the GetLastError() it seems, so do that after print_win32_err(). + print_win32_err(err); + qDebug() << "Problem opening handle, path: " << QString().fromWCharArray(details->DevicePath); + + free(details); + continue; + } + + free(details); + + attrib.Size = sizeof(HIDD_ATTRIBUTES); + ret = HidD_GetAttributes(h, &attrib); + // printf("vid: %4x\n", attrib.VendorID); + if (!ret || (vid > 0 && attrib.VendorID != vid) || + (pid > 0 && attrib.ProductID != pid) || + !HidD_GetPreparsedData(h, &hid_data)) { + CloseHandle(h); + continue; + } + + if (!HidP_GetCaps(hid_data, &capabilities) || + (usage_page > 0 && capabilities.UsagePage != usage_page) || + (usage > 0 && capabilities.Usage != usage)) { + HidD_FreePreparsedData(hid_data); + CloseHandle(h); + continue; + } + + HidD_FreePreparsedData(hid_data); + + hid = (struct hid_struct *)malloc(sizeof(struct hid_struct)); + if (!hid) { + CloseHandle(h); + continue; + } + +// COMMTIMEOUTS CommTimeouts; +// CommTimeouts.ReadIntervalTimeout = 100; // 100ms +// CommTimeouts.ReadTotalTimeoutConstant = 5; // ms +// CommTimeouts.ReadTotalTimeoutMultiplier = 1; // +// CommTimeouts.WriteTotalTimeoutConstant = 5; // ms +// CommTimeouts.WriteTotalTimeoutMultiplier = 1; // +// if (!SetCommTimeouts(h, &CommTimeouts)) +// { +//// DWORD err = GetLastError(); +// +// } + + qDebug("Open: Handle address: %li for num: %i", (long int)h, count); + + hid->handle = h; + add_hid(hid); + + count++; + if (count >= max) { + return count; + } + } + + return count; } -// recveive - receive a packet -// Inputs: -// num = device to receive from (zero based) -// buf = buffer to receive packet -// len = buffer's size -// timeout = time to wait, in milliseconds -// Output: -// number of bytes received, or -1 on error +// recveive - receive a packet +// Inputs: +// num = device to receive from (zero based) +// buf = buffer to receive packet +// len = buffer's size +// timeout = time to wait, in milliseconds +// Output: +// number of bytes received, or -1 on error // int pjrc_rawhid::receive(int num, void *buf, int len, int timeout) { - OVERLAPPED ov; - DWORD n; + OVERLAPPED ov; + DWORD n; - hid_t *hid = get_hid(num); - if (!hid) - return -1; - if (!hid->handle) - return -1; + hid_t *hid = get_hid(num); - EnterCriticalSection(&rx_mutex); + if (!hid) { + return -1; + } + if (!hid->handle) { + return -1; + } - ResetEvent(&rx_event); + EnterCriticalSection(&rx_mutex); - memset(&ov, 0, sizeof(ov)); - ov.hEvent = rx_event; + ResetEvent(&rx_event); - if (!ReadFile(hid->handle, buf, len, NULL, &ov)) - { - DWORD err = GetLastError(); + memset(&ov, 0, sizeof(ov)); + ov.hEvent = rx_event; - if (err == ERROR_DEVICE_NOT_CONNECTED) - { // the device has been unplugged - print_win32_err(err); - hid_close(hid); - LeaveCriticalSection(&rx_mutex); - emit deviceUnplugged(num); - return -1; - } + if (!ReadFile(hid->handle, buf, len, NULL, &ov)) { + DWORD err = GetLastError(); - if (err != ERROR_IO_PENDING) - { - print_win32_err(err); - LeaveCriticalSection(&rx_mutex); - return -1; - } - - DWORD r = WaitForSingleObject(rx_event, timeout); - if (r == WAIT_TIMEOUT) - { - CancelIo(hid->handle); - LeaveCriticalSection(&rx_mutex); - return 0; - } - if (r != WAIT_OBJECT_0) - { - DWORD err = GetLastError(); - print_win32_err(err); - LeaveCriticalSection(&rx_mutex); - return -1; - } + if (err == ERROR_DEVICE_NOT_CONNECTED) { // the device has been unplugged + print_win32_err(err); + hid_close(hid); + LeaveCriticalSection(&rx_mutex); + emit deviceUnplugged(num); + return -1; } - if (!GetOverlappedResult(hid->handle, &ov, &n, FALSE)) - { - DWORD err = GetLastError(); - print_win32_err(err); + if (err != ERROR_IO_PENDING) { + print_win32_err(err); + LeaveCriticalSection(&rx_mutex); + return -1; + } - if (err == ERROR_DEVICE_NOT_CONNECTED) - { // the device has been unplugged - hid_close(hid); - LeaveCriticalSection(&rx_mutex); - emit deviceUnplugged(num); - return -1; - } + DWORD r = WaitForSingleObject(rx_event, timeout); + if (r == WAIT_TIMEOUT) { + CancelIo(hid->handle); + LeaveCriticalSection(&rx_mutex); + return 0; + } + if (r != WAIT_OBJECT_0) { + DWORD err = GetLastError(); + print_win32_err(err); + LeaveCriticalSection(&rx_mutex); + return -1; + } + } - LeaveCriticalSection(&rx_mutex); - return -1; + if (!GetOverlappedResult(hid->handle, &ov, &n, FALSE)) { + DWORD err = GetLastError(); + print_win32_err(err); + + if (err == ERROR_DEVICE_NOT_CONNECTED) { // the device has been unplugged + hid_close(hid); + LeaveCriticalSection(&rx_mutex); + emit deviceUnplugged(num); + return -1; } LeaveCriticalSection(&rx_mutex); + return -1; + } - if (n <= 0) return -1; + LeaveCriticalSection(&rx_mutex); -// qDebug("Received %i bytes, first %x, second %x", len, *((char *) buf),*((char *)buf + 1)); + if (n <= 0) { + return -1; + } - if ((int)n > len) n = len; - return n; +// qDebug("Received %i bytes, first %x, second %x", len, *((char *) buf),*((char *)buf + 1)); + + if ((int)n > len) { + n = len; + } + return n; } -// send - send a packet -// Inputs: -// num = device to transmit to (zero based) -// buf = buffer containing packet to send -// len = number of bytes to transmit -// timeout = time to wait, in milliseconds -// Output: -// number of bytes sent, or -1 on error +// send - send a packet +// Inputs: +// num = device to transmit to (zero based) +// buf = buffer containing packet to send +// len = number of bytes to transmit +// timeout = time to wait, in milliseconds +// Output: +// number of bytes sent, or -1 on error // int pjrc_rawhid::send(int num, void *buf, int len, int timeout) { - OVERLAPPED ov; - DWORD n, r; + OVERLAPPED ov; + DWORD n, r; - hid_t *hid = get_hid(num); - if (!hid) - return -1; - if (!hid->handle) - return -1; + hid_t *hid = get_hid(num); -// qDebug("Send: Handle address: %li for num: %i", (long int) hid->handle, num); + if (!hid) { + return -1; + } + if (!hid->handle) { + return -1; + } - EnterCriticalSection(&tx_mutex); +// qDebug("Send: Handle address: %li for num: %i", (long int) hid->handle, num); - ResetEvent(&tx_event); + EnterCriticalSection(&tx_mutex); - memset(&ov, 0, sizeof(ov)); - ov.hEvent = tx_event; + ResetEvent(&tx_event); -// qDebug("Trying to write %u bytes. First %x second %x",len, *((char *) buf), *((char *)buf + 1)); + memset(&ov, 0, sizeof(ov)); + ov.hEvent = tx_event; - if (!WriteFile(hid->handle, buf, len, NULL, &ov)) - { - DWORD err = GetLastError(); +// qDebug("Trying to write %u bytes. First %x second %x",len, *((char *) buf), *((char *)buf + 1)); - if (err == ERROR_DEVICE_NOT_CONNECTED) - { // the device has been unplugged - hid_close(hid); - LeaveCriticalSection(&tx_mutex); - emit deviceUnplugged(num); - return -1; - } + if (!WriteFile(hid->handle, buf, len, NULL, &ov)) { + DWORD err = GetLastError(); - if (err == ERROR_SUCCESS || err == ERROR_IO_PENDING) - { -// qDebug("Waiting for write to finish"); - r = WaitForSingleObject(tx_event, timeout); - if (r == WAIT_TIMEOUT) - { - CancelIo(hid->handle); - LeaveCriticalSection(&tx_mutex); - return 0; - } - if (r != WAIT_OBJECT_0) - { - DWORD err = GetLastError(); - print_win32_err(err); - LeaveCriticalSection(&tx_mutex); - return -1; - } - } - else - { -// qDebug("Error writing to file"); - print_win32_err(err); - LeaveCriticalSection(&tx_mutex); - return -1; - } + if (err == ERROR_DEVICE_NOT_CONNECTED) { // the device has been unplugged + hid_close(hid); + LeaveCriticalSection(&tx_mutex); + emit deviceUnplugged(num); + return -1; } - if (!GetOverlappedResult(hid->handle, &ov, &n, FALSE)) - { + if (err == ERROR_SUCCESS || err == ERROR_IO_PENDING) { +// qDebug("Waiting for write to finish"); + r = WaitForSingleObject(tx_event, timeout); + if (r == WAIT_TIMEOUT) { + CancelIo(hid->handle); + LeaveCriticalSection(&tx_mutex); + return 0; + } + if (r != WAIT_OBJECT_0) { DWORD err = GetLastError(); - - qDebug("Problem getting overlapped result"); print_win32_err(err); - - if (err == ERROR_DEVICE_NOT_CONNECTED) - { // the device has been unplugged - hid_close(hid); - LeaveCriticalSection(&tx_mutex); - emit deviceUnplugged(num); - return -1; - } + LeaveCriticalSection(&tx_mutex); + return -1; + } + } else { +// qDebug("Error writing to file"); + print_win32_err(err); + LeaveCriticalSection(&tx_mutex); + return -1; } + } - LeaveCriticalSection(&tx_mutex); + if (!GetOverlappedResult(hid->handle, &ov, &n, FALSE)) { + DWORD err = GetLastError(); - if (n <= 0) return -1; - return n; + qDebug("Problem getting overlapped result"); + print_win32_err(err); + + if (err == ERROR_DEVICE_NOT_CONNECTED) { // the device has been unplugged + hid_close(hid); + LeaveCriticalSection(&tx_mutex); + emit deviceUnplugged(num); + return -1; + } + } + + LeaveCriticalSection(&tx_mutex); + + if (n <= 0) { + return -1; + } + return n; } QString pjrc_rawhid::getserial(int num) { - hid_t *hid = get_hid(num); - if (!hid) - return ""; - if (!hid->handle) - return ""; + hid_t *hid = get_hid(num); - // Should we do some "critical section" stuff here?? - char temp[126]; - if (!HidD_GetSerialNumberString(hid->handle, temp, sizeof(temp))) - { - DWORD err = GetLastError(); - print_win32_err(err); + if (!hid) { + return ""; + } + if (!hid->handle) { + return ""; + } - if (err == ERROR_DEVICE_NOT_CONNECTED) - { // the device has been unplugged - hid_close(hid); - emit deviceUnplugged(num); - return ""; - } + // Should we do some "critical section" stuff here?? + char temp[126]; + if (!HidD_GetSerialNumberString(hid->handle, temp, sizeof(temp))) { + DWORD err = GetLastError(); + print_win32_err(err); - return QString("Error"); + if (err == ERROR_DEVICE_NOT_CONNECTED) { // the device has been unplugged + hid_close(hid); + emit deviceUnplugged(num); + return ""; } - return QString().fromUtf16((ushort*)temp,-1); + return QString("Error"); + } + + return QString().fromUtf16((ushort *)temp, -1); } -// close - close a device +// close - close a device // -// Inputs: -// num = device to close (zero based) -// Output -// (nothing) +// Inputs: +// num = device to close (zero based) +// Output +// (nothing) // void pjrc_rawhid::close(int num) { - hid_close(get_hid(num)); + hid_close(get_hid(num)); } void pjrc_rawhid::add_hid(hid_t *h) { - if (!h) return; + if (!h) { + return; + } - if (!first_hid || !last_hid) - { - first_hid = last_hid = h; - h->next = h->prev = NULL; - return; - } + if (!first_hid || !last_hid) { + first_hid = last_hid = h; + h->next = h->prev = NULL; + return; + } - last_hid->next = h; - h->prev = last_hid; - h->next = NULL; - last_hid = h; + last_hid->next = h; + h->prev = last_hid; + h->next = NULL; + last_hid = h; } -hid_t * pjrc_rawhid::get_hid(int num) +hid_t *pjrc_rawhid::get_hid(int num) { - hid_t *p; - for (p = first_hid; p && num > 0; p = p->next, num--) ; - return p; + hid_t *p; + + for (p = first_hid; p && num > 0; p = p->next, num--) { + ; + } + return p; } void pjrc_rawhid::free_all_hid(void) { - for (hid_t *p = first_hid; p; p = p->next) - hid_close(p); + for (hid_t *p = first_hid; p; p = p->next) { + hid_close(p); + } - hid_t *p = first_hid; - while (p) - { - hid_t *q = p; - p = p->next; - free(q); - } + hid_t *p = first_hid; + while (p) { + hid_t *q = p; + p = p->next; + free(q); + } - first_hid = last_hid = NULL; + first_hid = last_hid = NULL; } void pjrc_rawhid::hid_close(hid_t *hid) { - if (!hid) return; - if (!hid->handle) return; + if (!hid) { + return; + } + if (!hid->handle) { + return; + } - CloseHandle(hid->handle); - hid->handle = NULL; + CloseHandle(hid->handle); + hid->handle = NULL; } void pjrc_rawhid::print_win32_err(DWORD err) { - char buf[256]; - char temp[256]; + char buf[256]; + char temp[256]; - //FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, err, 0, (WCHAR*)buf, sizeof(buf), NULL); - FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, err, MAKELANGID(LANG_ENGLISH, SUBLANG_DEFAULT), (WCHAR*)buf, sizeof(buf), NULL); - WideCharToMultiByte( CP_ACP, 0, (WCHAR*)buf, sizeof(buf), temp, sizeof(temp), NULL, NULL ); - printf("err %ld: %s\n", err, temp); + // FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, err, 0, (WCHAR*)buf, sizeof(buf), NULL); + FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, err, MAKELANGID(LANG_ENGLISH, SUBLANG_DEFAULT), (WCHAR *)buf, sizeof(buf), NULL); + WideCharToMultiByte(CP_ACP, 0, (WCHAR *)buf, sizeof(buf), temp, sizeof(temp), NULL, NULL); + printf("err %ld: %s\n", err, temp); } - diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp b/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp index 3a1aa896d..3e3d8a4ad 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp @@ -37,22 +37,20 @@ class IConnection; -//timeout value used when we want to return directly without waiting -static const int READ_TIMEOUT = 200; -static const int READ_SIZE = 64; +// timeout value used when we want to return directly without waiting +static const int READ_TIMEOUT = 200; +static const int READ_SIZE = 64; static const int WRITE_TIMEOUT = 1000; -static const int WRITE_SIZE = 64; - +static const int WRITE_SIZE = 64; // ********************************************************************************* /** -* Thread to desynchronize reading from the device -*/ -class RawHIDReadThread : public QThread -{ + * Thread to desynchronize reading from the device + */ +class RawHIDReadThread : public QThread { public: RawHIDReadThread(RawHID *hid); virtual ~RawHIDReadThread(); @@ -64,7 +62,8 @@ public: qint64 getBytesAvailable(); public slots: - void terminate() { + void terminate() + { m_running = false; } @@ -72,7 +71,7 @@ protected: void run(); /** QByteArray might not be the most efficient way to implement - a circular buffer but it's good enough and very simple */ + a circular buffer but it's good enough and very simple */ QByteArray m_readBuffer; /** A mutex to protect read buffer */ @@ -90,10 +89,9 @@ protected: // ********************************************************************************* /** -* This class is nearly the same than RawHIDReadThread but for writing -*/ -class RawHIDWriteThread : public QThread -{ + * This class is nearly the same than RawHIDReadThread but for writing + */ +class RawHIDWriteThread : public QThread { public: RawHIDWriteThread(RawHID *hid); virtual ~RawHIDWriteThread(); @@ -105,7 +103,8 @@ public: qint64 getBytesToWrite(); public slots: - void terminate() { + void terminate() + { m_running = false; } @@ -113,7 +112,7 @@ protected: void run(); /** QByteArray might not be the most efficient way to implement - a circular buffer but it's good enough and very simple */ + a circular buffer but it's good enough and very simple */ QByteArray m_writeBuffer; /** A mutex to protect read buffer */ @@ -144,43 +143,38 @@ RawHIDReadThread::RawHIDReadThread(RawHID *hid) RawHIDReadThread::~RawHIDReadThread() { m_running = false; - //wait for the thread to terminate - if(wait(10000) == false) + // wait for the thread to terminate + if (wait(10000) == false) { qDebug() << "Cannot terminate RawHIDReadThread"; + } } void RawHIDReadThread::run() { m_running = m_hid->openDevice(); - while(m_running) - { - //here we use a temporary buffer so we don't need to lock - //the mutex while we are reading from the device + while (m_running) { + // here we use a temporary buffer so we don't need to lock + // the mutex while we are reading from the device // Want to read in regular chunks that match the packet size the device // is using. In this case it is 64 bytes (the interrupt packet limit) // although it would be nice if the device had a different report to // configure this - char buffer[READ_SIZE] = {0}; + char buffer[READ_SIZE] = { 0 }; int ret = hiddev->receive(hidno, buffer, READ_SIZE, READ_TIMEOUT); - if(ret > 0) //read some data - { + if (ret > 0) { // read some data QMutexLocker lock(&m_readBufMtx); // Note: Preprocess the USB packets in this OS independent code // First byte is report ID, second byte is the number of valid bytes m_readBuffer.append(&buffer[2], buffer[1]); emit m_hid->readyRead(); - } - else if(ret == 0) //nothing read - { - } - else // < 0 => error - { - //TODO! make proper error handling, this only quick hack for unplug freeze - m_running=false; + } else if (ret == 0) { // nothing read + } else { // < 0 => error + // TODO! make proper error handling, this only quick hack for unplug freeze + m_running = false; } } m_hid->closeDevice(); @@ -201,6 +195,7 @@ int RawHIDReadThread::getReadData(char *data, int size) qint64 RawHIDReadThread::getBytesAvailable() { QMutexLocker lock(&m_readBufMtx); + return m_readBuffer.size(); } @@ -209,66 +204,60 @@ RawHIDWriteThread::RawHIDWriteThread(RawHID *hid) hiddev(&hid->dev), hidno(hid->m_deviceNo), m_running(true) -{ -} +{} // ********************************************************************************* RawHIDWriteThread::~RawHIDWriteThread() { m_running = false; - //wait for the thread to terminate - if(wait(10000) == false) + // wait for the thread to terminate + if (wait(10000) == false) { qDebug() << "Cannot terminate RawHIDReadThread"; + } } void RawHIDWriteThread::run() { - while(m_running) - { - char buffer[WRITE_SIZE] = {0}; + while (m_running) { + char buffer[WRITE_SIZE] = { 0 }; m_writeBufMtx.lock(); - int size = qMin(WRITE_SIZE-2, m_writeBuffer.size()); - while(size <= 0) - { - //wait on new data to write condition, the timeout - //enable the thread to shutdown properly + int size = qMin(WRITE_SIZE - 2, m_writeBuffer.size()); + while (size <= 0) { + // wait on new data to write condition, the timeout + // enable the thread to shutdown properly m_newDataToWrite.wait(&m_writeBufMtx, 200); - if(!m_running) + if (!m_running) { return; + } size = m_writeBuffer.size(); } - //NOTE: data size is limited to 2 bytes less than the - //usb packet size (64 bytes for interrupt) to make room - //for the reportID and valid data length - size = qMin(WRITE_SIZE-2, m_writeBuffer.size()); + // NOTE: data size is limited to 2 bytes less than the + // usb packet size (64 bytes for interrupt) to make room + // for the reportID and valid data length + size = qMin(WRITE_SIZE - 2, m_writeBuffer.size()); memcpy(&buffer[2], m_writeBuffer.constData(), size); - buffer[1] = size; //valid data length - buffer[0] = 2; //reportID + buffer[1] = size; // valid data length + buffer[0] = 2; // reportID m_writeBufMtx.unlock(); // must hold lock through the send to know how much was sent int ret = hiddev->send(hidno, buffer, WRITE_SIZE, WRITE_TIMEOUT); - if(ret > 0) - { - //only remove the size actually written to the device + if (ret > 0) { + // only remove the size actually written to the device QMutexLocker lock(&m_writeBufMtx); m_writeBuffer.remove(0, size); emit m_hid->bytesWritten(ret - 2); - } - else if(ret < 0) // < 0 => error - { - //TODO! make proper error handling, this only quick hack for unplug freeze - m_running=false; + } else if (ret < 0) { // < 0 => error + // TODO! make proper error handling, this only quick hack for unplug freeze + m_running = false; qDebug() << "Error writing to device"; - } - else - { + } else { qDebug() << "No data written to device ??"; } } @@ -279,7 +268,7 @@ int RawHIDWriteThread::pushDataToWrite(const char *data, int size) QMutexLocker lock(&m_writeBufMtx); m_writeBuffer.append(data, size); - m_newDataToWrite.wakeOne(); //signal that new data arrived + m_newDataToWrite.wakeOne(); // signal that new data arrived return size; } @@ -293,14 +282,13 @@ qint64 RawHIDWriteThread::getBytesToWrite() // ********************************************************************************* RawHID::RawHID(const QString &deviceName) - :QIODevice(), + : QIODevice(), serialNumber(deviceName), m_deviceNo(-1), m_readThread(NULL), - m_writeThread(NULL), - m_mutex(NULL) + m_writeThread(NULL), + m_mutex(NULL) { - m_mutex = new QMutex(QMutex::Recursive); m_startedMutex = new QMutex(); @@ -323,22 +311,24 @@ RawHID::RawHID(const QString &deviceName) * system code is registered in that thread instead of the calling * thread (usually UI) */ -bool RawHID::openDevice() { +bool RawHID::openDevice() +{ int opened = dev.open(USB_MAX_DEVICES, USBMonitor::idVendor_OpenPilot, -1, USB_USAGE_PAGE, USB_USAGE); - for (int i =0; i< opened; i++) { - if (serialNumber == dev.getserial(i)) + + for (int i = 0; i < opened; i++) { + if (serialNumber == dev.getserial(i)) { m_deviceNo = i; - else + } else { dev.close(i); + } } // Now things are opened or not (from read thread) allow the constructor to complete m_startedMutex->unlock(); - //didn't find the device we are trying to open (shouldnt happen) + // didn't find the device we are trying to open (shouldnt happen) device_open = opened >= 0; - if (opened < 0) - { + if (opened < 0) { return false; } @@ -352,39 +342,47 @@ bool RawHID::openDevice() { * It is uses as a callback from the read thread so that the USB * system code is unregistered from that thread\ */ -bool RawHID::closeDevice() { +bool RawHID::closeDevice() +{ dev.close(m_deviceNo); } RawHID::~RawHID() { // If the read thread exists then the device is open - if (m_readThread) + if (m_readThread) { close(); + } } void RawHID::onDeviceUnplugged(int num) { - if (num != m_deviceNo) - return; + if (num != m_deviceNo) { + return; + } - // the USB device has been unplugged - close(); + // the USB device has been unplugged + close(); } bool RawHID::open(OpenMode mode) { - QMutexLocker locker(m_mutex); + QMutexLocker locker(m_mutex); - if (m_deviceNo < 0) + if (m_deviceNo < 0) { return false; + } QIODevice::open(mode); Q_ASSERT(m_readThread); Q_ASSERT(m_writeThread); - if (m_readThread) m_readThread->start(); - if (m_writeThread) m_writeThread->start(); + if (m_readThread) { + m_readThread->start(); + } + if (m_writeThread) { + m_writeThread->start(); + } return true; } @@ -393,8 +391,7 @@ void RawHID::close() { qDebug() << "RawHID::close()"; emit aboutToClose(); - if (m_writeThread) - { + if (m_writeThread) { qDebug() << "About to terminate write thread"; m_writeThread->terminate(); delete m_writeThread; @@ -403,8 +400,7 @@ void RawHID::close() } - if (m_readThread) - { + if (m_readThread) { qDebug() << "About to terminate read thread"; m_readThread->terminate(); delete m_readThread; // calls wait @@ -424,42 +420,46 @@ bool RawHID::isSequential() const qint64 RawHID::bytesAvailable() const { - QMutexLocker locker(m_mutex); + QMutexLocker locker(m_mutex); - if (!m_readThread) - return -1; + if (!m_readThread) { + return -1; + } - return m_readThread->getBytesAvailable() + QIODevice::bytesAvailable(); + return m_readThread->getBytesAvailable() + QIODevice::bytesAvailable(); } qint64 RawHID::bytesToWrite() const { - QMutexLocker locker(m_mutex); + QMutexLocker locker(m_mutex); - if (!m_writeThread) - return -1; + if (!m_writeThread) { + return -1; + } return m_writeThread->getBytesToWrite() + QIODevice::bytesToWrite(); } qint64 RawHID::readData(char *data, qint64 maxSize) { - QMutexLocker locker(m_mutex); + QMutexLocker locker(m_mutex); - if (!m_readThread || !data) - return -1; + if (!m_readThread || !data) { + return -1; + } - return m_readThread->getReadData(data, maxSize); + return m_readThread->getReadData(data, maxSize); } qint64 RawHID::writeData(const char *data, qint64 maxSize) { - QMutexLocker locker(m_mutex); + QMutexLocker locker(m_mutex); - if (!m_writeThread || !data) - return -1; + if (!m_writeThread || !data) { + return -1; + } - return m_writeThread->pushDataToWrite(data, maxSize); + return m_writeThread->pushDataToWrite(data, maxSize); } // ********************************************************************************* diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhid.h b/ground/openpilotgcs/src/plugins/rawhid/rawhid.h index f3487bf20..72275b4b4 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhid.h +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhid.h @@ -38,17 +38,16 @@ #include "pjrc_rawhid.h" #include "usbmonitor.h" -//helper classes +// helper classes class RawHIDReadThread; class RawHIDWriteThread; /** -* The actual IO device that will be used to communicate -* with the board. -*/ -class RAWHID_EXPORT RawHID : public QIODevice -{ - Q_OBJECT + * The actual IO device that will be used to communicate + * with the board. + */ +class RAWHID_EXPORT RawHID : public QIODevice { + Q_OBJECT friend class RawHIDReadThread; friend class RawHIDWriteThread; @@ -63,10 +62,10 @@ public: virtual bool isSequential() const; signals: - void closed(); + void closed(); public slots: - void onDeviceUnplugged(int num); + void onDeviceUnplugged(int num); protected: virtual qint64 readData(char *data, qint64 maxSize); @@ -74,10 +73,10 @@ protected: virtual qint64 bytesAvailable() const; virtual qint64 bytesToWrite() const; - //! Callback from the read thread to open the device + // ! Callback from the read thread to open the device bool openDevice(); - //! Callback from teh read thread to close the device + // ! Callback from teh read thread to close the device bool closeDevice(); QString serialNumber; @@ -89,7 +88,7 @@ protected: RawHIDReadThread *m_readThread; RawHIDWriteThread *m_writeThread; - QMutex *m_mutex; + QMutex *m_mutex; QMutex *m_startedMutex; }; diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhid_const.h b/ground/openpilotgcs/src/plugins/rawhid/rawhid_const.h index db85c4add..9e58720cf 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhid_const.h +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhid_const.h @@ -28,13 +28,13 @@ #ifndef RAWHID_CONST_H #define RAWHID_CONST_H -static const int USB_MAX_DEVICES = 10; +static const int USB_MAX_DEVICES = 10; -static const int USB_VID = 0x20A0; -static const int USB_PID = 0x4117; +static const int USB_VID = 0x20A0; +static const int USB_PID = 0x4117; -static const int USB_USAGE_PAGE = 0xFF9C; -static const int USB_USAGE = 0x0001; +static const int USB_USAGE_PAGE = 0xFF9C; +static const int USB_USAGE = 0x0001; static const int USB_DEV_SERIAL_LEN = 24; diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhid_global.h b/ground/openpilotgcs/src/plugins/rawhid/rawhid_global.h index 31cb98f1e..84ab9c0e0 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhid_global.h +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhid_global.h @@ -10,18 +10,18 @@ * @brief Impliments a HID USB connection to the flight hardware as a QIODevice *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp b/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp index 24276b981..4dd682758 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp @@ -10,18 +10,18 @@ * @brief Impliments a HID USB connection to the flight hardware as a QIODevice *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,55 +41,57 @@ RawHIDConnection::RawHIDConnection() { - //added by andrew - RawHidHandle = NULL; + // added by andrew + RawHidHandle = NULL; enablePolling = true; - m_usbMonitor = USBMonitor::instance(); + m_usbMonitor = USBMonitor::instance(); connect(m_usbMonitor, SIGNAL(deviceDiscovered(USBPortInfo)), this, SLOT(onDeviceConnected())); connect(m_usbMonitor, SIGNAL(deviceRemoved(USBPortInfo)), this, SLOT(onDeviceDisconnected())); - } RawHIDConnection::~RawHIDConnection() { - if (RawHidHandle) - if (RawHidHandle->isOpen()) - RawHidHandle->close(); + if (RawHidHandle) { + if (RawHidHandle->isOpen()) { + RawHidHandle->close(); + } + } } /** - The USB monitor tells us a new device appeared - */ + The USB monitor tells us a new device appeared + */ void RawHIDConnection::onDeviceConnected() { emit availableDevChanged(this); } /** - The USB monitor tells us a device disappeard - */ + The USB monitor tells us a device disappeard + */ void RawHIDConnection::onDeviceDisconnected() { qDebug() << "onDeviceDisconnected()"; - if (enablePolling) + if (enablePolling) { emit availableDevChanged(this); + } } /** - Returns the list of all currently available devices - */ + Returns the list of all currently available devices + */ QList < Core::IConnection::device> RawHIDConnection::availableDevices() { QList < Core::IConnection::device> devices; - QList portsList = m_usbMonitor->availableDevices(USBMonitor::idVendor_OpenPilot, -1, -1,USBMonitor::Running); + QList portsList = m_usbMonitor->availableDevices(USBMonitor::idVendor_OpenPilot, -1, -1, USBMonitor::Running); // We currently list devices by their serial number device dev; foreach(USBPortInfo prt, portsList) { - dev.name=prt.serialNumber; - dev.displayName=prt.product; + dev.name = prt.serialNumber; + dev.displayName = prt.product; devices.append(dev); } return devices; @@ -97,12 +99,13 @@ QList < Core::IConnection::device> RawHIDConnection::availableDevices() QIODevice *RawHIDConnection::openDevice(const QString &deviceName) { - //added by andrew - if (RawHidHandle) + // added by andrew + if (RawHidHandle) { closeDevice(deviceName); - //end added by andrew + } + // end added by andrew - //return new RawHID(deviceName); + // return new RawHID(deviceName); RawHidHandle = new RawHID(deviceName); return RawHidHandle; } @@ -111,12 +114,11 @@ QIODevice *RawHIDConnection::openDevice(const QString &deviceName) void RawHIDConnection::closeDevice(const QString &deviceName) { Q_UNUSED(deviceName); - if (RawHidHandle) - { + if (RawHidHandle) { qDebug() << "Closing the device here"; RawHidHandle->close(); - delete RawHidHandle; - RawHidHandle = NULL; + delete RawHidHandle; + RawHidHandle = NULL; } } @@ -131,7 +133,7 @@ QString RawHIDConnection::shortName() } /** - Tells the Raw HID plugin to stop polling for USB devices + Tells the Raw HID plugin to stop polling for USB devices */ void RawHIDConnection::suspendPolling() { @@ -139,7 +141,7 @@ void RawHIDConnection::suspendPolling() } /** - Tells the Raw HID plugin to resume polling for USB devices + Tells the Raw HID plugin to resume polling for USB devices */ void RawHIDConnection::resumePolling() { @@ -150,26 +152,25 @@ void RawHIDConnection::resumePolling() RawHIDPlugin::RawHIDPlugin() { - hidConnection = NULL; // Pip + hidConnection = NULL; // Pip } RawHIDPlugin::~RawHIDPlugin() { m_usbMonitor->quit(); m_usbMonitor->wait(500); - } void RawHIDPlugin::extensionsInitialized() { - hidConnection = new RawHIDConnection(); - addAutoReleasedObject(hidConnection); + hidConnection = new RawHIDConnection(); + addAutoReleasedObject(hidConnection); - //temp for test - //addAutoReleasedObject(new RawHIDTestThread); + // temp for test + // addAutoReleasedObject(new RawHIDTestThread); } -bool RawHIDPlugin::initialize(const QStringList & arguments, QString * errorString) +bool RawHIDPlugin::initialize(const QStringList & arguments, QString *errorString) { Q_UNUSED(arguments); Q_UNUSED(errorString); diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.h b/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.h index 324471a14..3b241c6e5 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.h +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.h @@ -10,18 +10,18 @@ * @brief Impliments a HID USB connection to the flight hardware as a QIODevice *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -42,13 +42,12 @@ class RawHIDConnection; /** -* Define a connection via the IConnection interface -* Plugin will add a instance of this class to the pool, -* so the connection manager can use it. -*/ + * Define a connection via the IConnection interface + * Plugin will add a instance of this class to the pool, + * so the connection manager can use it. + */ class RAWHID_EXPORT RawHIDConnection - : public Core::IConnection -{ + : public Core::IConnection { Q_OBJECT public: RawHIDConnection(); @@ -63,7 +62,10 @@ public: virtual void suspendPolling(); virtual void resumePolling(); - bool deviceOpened() { return (RawHidHandle != NULL); } // Pip + bool deviceOpened() + { + return RawHidHandle != NULL; + } // Pip protected slots: void onDeviceConnected(); @@ -75,13 +77,12 @@ private: protected: QMutex m_enumMutex; - USBMonitor* m_usbMonitor; + USBMonitor *m_usbMonitor; bool m_deviceOpened; }; class RAWHID_EXPORT RawHIDPlugin - : public ExtensionSystem::IPlugin -{ + : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -92,8 +93,7 @@ public: virtual void extensionsInitialized(); private: RawHIDConnection *hidConnection; - USBMonitor* m_usbMonitor; - + USBMonitor *m_usbMonitor; }; #endif // RAWHIDPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor.h b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor.h index fd114042f..9dde7e1ec 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor.h +++ b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor.h @@ -34,7 +34,7 @@ #include // Depending on the OS, we'll need different things: -#if defined( Q_OS_MAC) +#if defined(Q_OS_MAC) #include #include #elif defined(Q_OS_UNIX) @@ -42,23 +42,22 @@ #include #include -#elif defined (Q_OS_WIN32) +#elif defined(Q_OS_WIN32) #ifndef _WIN32_WINNT - #define _WIN32_WINNT 0x0500 + #define _WIN32_WINNT 0x0500 #endif #ifndef _WIN32_WINDOWS #define _WIN32_WINDOWS 0x0500 #endif #ifndef WINVER - #define WINVER 0x0500 + #define WINVER 0x0500 #endif #include #include #include #include #include -#endif - +#endif // if defined(Q_OS_MAC) #ifdef Q_OS_WIN @@ -66,60 +65,59 @@ #include class USBMonitor; -class USBRegistrationWidget : public QWidget -{ +class USBRegistrationWidget : public QWidget { Q_OBJECT public: - USBRegistrationWidget( USBMonitor* qese ) { + USBRegistrationWidget(USBMonitor *qese) + { this->qese = qese; } - ~USBRegistrationWidget( ) {} + ~USBRegistrationWidget() {} protected: - USBMonitor* qese; - bool winEvent( MSG* message, long* result ); + USBMonitor *qese; + bool winEvent(MSG *message, long *result); }; #endif #endif struct USBPortInfo { - //QString friendName; ///< Friendly name. - //QString physName; - //QString enumName; ///< It seems its the only one with meaning + // QString friendName; ///< Friendly name. + // QString physName; + // QString enumName; ///< It seems its the only one with meaning QString serialNumber; // As a string as it can be anything, really... QString manufacturer; QString product; #if defined(Q_OS_WIN32) - QString devicePath; //only has meaning on windows + QString devicePath; // only has meaning on windows #elif defined(Q_OS_MAC) IOHIDDeviceRef dev_handle; #endif int UsagePage; int Usage; - int vendorID; ///< Vendor ID. - int productID; ///< Product ID + int vendorID; ///< Vendor ID. + int productID; ///< Product ID int bcdDevice; }; /** -* A monitoring thread which will wait for device events. -*/ + * A monitoring thread which will wait for device events. + */ -class RAWHID_EXPORT USBMonitor : public QThread -{ +class RAWHID_EXPORT USBMonitor : public QThread { Q_OBJECT public: enum RunState { Bootloader = 0x01, - Running = 0x02 + Running = 0x02 }; enum USBConstants { - idVendor_OpenPilot = 0x20a0, - idProduct_OpenPilot = 0x415a, + idVendor_OpenPilot = 0x20a0, + idProduct_OpenPilot = 0x415a, idProduct_CopterControl = 0x415b, - idProduct_PipXtreme = 0x415c + idProduct_PipXtreme = 0x415c }; static USBMonitor *instance(); @@ -128,48 +126,48 @@ public: ~USBMonitor(); QList availableDevices(); QList availableDevices(int vid, int pid, int boardModel, int runState); - #if defined (Q_OS_WIN32) - LRESULT onDeviceChangeWin( WPARAM wParam, LPARAM lParam ); + #if defined(Q_OS_WIN32) + LRESULT onDeviceChangeWin(WPARAM wParam, LPARAM lParam); #endif signals: /*! - A new device has been connected to the system. + A new device has been connected to the system. - setUpNotifications() must be called first to enable event-driven device notifications. - Currently only implemented on Windows and OS X. - \param info The device that has been discovered. - */ - void deviceDiscovered( const USBPortInfo & info ); + setUpNotifications() must be called first to enable event-driven device notifications. + Currently only implemented on Windows and OS X. + \param info The device that has been discovered. + */ + void deviceDiscovered(const USBPortInfo & info); /*! - A device has been disconnected from the system. + A device has been disconnected from the system. - setUpNotifications() must be called first to enable event-driven device notifications. - Currently only implemented on Windows and OS X. - \param info The device that was disconnected. - */ - void deviceRemoved( const USBPortInfo & info ); + setUpNotifications() must be called first to enable event-driven device notifications. + Currently only implemented on Windows and OS X. + \param info The device that was disconnected. + */ + void deviceRemoved(const USBPortInfo & info); private slots: /** - Callback available for whenever the system that is put in place gets - an event + Callback available for whenever the system that is put in place gets + an event */ void deviceEventReceived(); private: - //! Mutex for modifying the list of available devices - QMutex * listMutex; + // ! Mutex for modifying the list of available devices + QMutex *listMutex; - //! List of known devices maintained by callbacks + // ! List of known devices maintained by callbacks QList knowndevices; Q_DISABLE_COPY(USBMonitor) - static USBMonitor *m_instance; + static USBMonitor * m_instance; // Depending on the OS, we'll need different things: -#if defined( Q_OS_MAC) +#if defined(Q_OS_MAC) static void attach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev); static void detach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev); void addDevice(USBPortInfo info); @@ -180,10 +178,10 @@ private: struct udev_monitor *monitor; QSocketNotifier *monitorNotifier; USBPortInfo makePortInfo(struct udev_device *dev); -#elif defined (Q_OS_WIN32) +#elif defined(Q_OS_WIN32) GUID guid_hid; void setUpNotifications(); - /*! + /*! * Get specific property from registry. * \param devInfo pointer to the device information set that contains the interface * and its underlying device. Returned by SetupDiGetClassDevs() function. @@ -193,13 +191,12 @@ private: * \return property string. */ static QString getDeviceProperty(HDEVINFO devInfo, PSP_DEVINFO_DATA devData, DWORD property); - static int infoFromHandle(const GUID & guid,USBPortInfo & info,HDEVINFO & devInfo,DWORD & index); - static void enumerateDevicesWin( const GUID & guidDev, QList* infoList ); + static int infoFromHandle(const GUID & guid, USBPortInfo & info, HDEVINFO & devInfo, DWORD & index); + static void enumerateDevicesWin(const GUID & guidDev, QList *infoList); bool matchAndDispatchChangedDevice(const QString & deviceID, const GUID & guid, WPARAM wParam); #ifdef QT_GUI_LIB - USBRegistrationWidget* notificationWidget; + USBRegistrationWidget *notificationWidget; #endif -#endif - +#endif // if defined(Q_OS_MAC) }; #endif // USBMONITOR_H diff --git a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_linux.cpp b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_linux.cpp index 6b44cc819..7fc9d77c8 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_linux.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_linux.cpp @@ -26,9 +26,9 @@ */ /* - Need help wihth udev ? + Need help wihth udev ? There is a great tuturial at: http://www.signal11.us/oss/udev/ - */ + */ #include "usbmonitor.h" #include @@ -43,65 +43,62 @@ void printPortInfo(struct udev_device *dev) printf(" Devtype: %s", udev_device_get_devtype(dev)); printf(" Action: %s", udev_device_get_action(dev)); /* From here, we can call get_sysattr_value() for each file - in the device's /sys entry. The strings passed into these - functions (idProduct, idVendor, serial, etc.) correspond - directly to the files in the directory which represents - the USB device. Note that USB strings are Unicode, UCS2 - encoded, but the strings returned from - udev_device_get_sysattr_value() are UTF-8 encoded. */ + in the device's /sys entry. The strings passed into these + functions (idProduct, idVendor, serial, etc.) correspond + directly to the files in the directory which represents + the USB device. Note that USB strings are Unicode, UCS2 + encoded, but the strings returned from + udev_device_get_sysattr_value() are UTF-8 encoded. */ printf(" VID/PID/bcdDevice : %s %s %s", - udev_device_get_sysattr_value(dev,"idVendor"), + udev_device_get_sysattr_value(dev, "idVendor"), udev_device_get_sysattr_value(dev, "idProduct"), - udev_device_get_sysattr_value(dev,"bcdDevice")); + udev_device_get_sysattr_value(dev, "bcdDevice")); printf(" %s - %s", - udev_device_get_sysattr_value(dev,"manufacturer"), - udev_device_get_sysattr_value(dev,"product")); + udev_device_get_sysattr_value(dev, "manufacturer"), + udev_device_get_sysattr_value(dev, "product")); printf(" serial: %s", - udev_device_get_sysattr_value(dev, "serial")); - + udev_device_get_sysattr_value(dev, "serial")); } -void USBMonitor::deviceEventReceived() { - +void USBMonitor::deviceEventReceived() +{ qDebug() << "Device event"; struct udev_device *dev; dev = udev_monitor_receive_device(this->monitor); if (dev) { printf("------- Got Device Event"); - QString action = QString(udev_device_get_action(dev)); + QString action = QString(udev_device_get_action(dev)); QString devtype = QString(udev_device_get_devtype(dev)); if (action == "add" && devtype == "usb_device") { printPortInfo(dev); emit deviceDiscovered(makePortInfo(dev)); - } else if (action == "remove" && devtype == "usb_device"){ + } else if (action == "remove" && devtype == "usb_device") { printPortInfo(dev); emit deviceRemoved(makePortInfo(dev)); } udev_device_unref(dev); - } - else { + } else { printf("No Device from receive_device(). An error occured."); } - } -USBMonitor* USBMonitor::instance() +USBMonitor *USBMonitor::instance() { return m_instance; } -USBMonitor* USBMonitor::m_instance = 0; +USBMonitor *USBMonitor::m_instance = 0; /** - Initialize the udev monitor here - */ -USBMonitor::USBMonitor(QObject *parent): QThread(parent) { - - m_instance = this; + Initialize the udev monitor here + */ +USBMonitor::USBMonitor(QObject *parent) : QThread(parent) +{ + m_instance = this; this->context = udev_new(); @@ -124,8 +121,8 @@ USBMonitor::~USBMonitor() } /** -Returns a list of all currently available devices -*/ + Returns a list of all currently available devices + */ QList USBMonitor::availableDevices() { QList devicesList; @@ -134,56 +131,55 @@ QList USBMonitor::availableDevices() struct udev_device *dev; enumerate = udev_enumerate_new(this->context); - udev_enumerate_add_match_subsystem(enumerate,"usb"); -// udev_enumerate_add_match_sysattr(enumerate, "idVendor", "20a0"); + udev_enumerate_add_match_subsystem(enumerate, "usb"); +// udev_enumerate_add_match_sysattr(enumerate, "idVendor", "20a0"); udev_enumerate_scan_devices(enumerate); devices = udev_enumerate_get_list_entry(enumerate); // Will use the 'native' udev functions to loop: - udev_list_entry_foreach(dev_list_entry,devices) { + udev_list_entry_foreach(dev_list_entry, devices) { const char *path; /* Get the filename of the /sys entry for the device - and create a udev_device object (dev) representing it */ + and create a udev_device object (dev) representing it */ path = udev_list_entry_get_name(dev_list_entry); - dev = udev_device_new_from_syspath(this->context, path); - if (QString(udev_device_get_devtype(dev)) == "usb_device") + dev = udev_device_new_from_syspath(this->context, path); + if (QString(udev_device_get_devtype(dev)) == "usb_device") { devicesList.append(makePortInfo(dev)); + } udev_device_unref(dev); } /* free the enumerator object */ udev_enumerate_unref(enumerate); return devicesList; - } /** - Be a bit more picky and ask only for a specific type of device: + Be a bit more picky and ask only for a specific type of device: On OpenPilot, the bcdDeviceLSB indicates the run state: bootloader or running. bcdDeviceMSB indicates the board model. - */ + */ QList USBMonitor::availableDevices(int vid, int pid, int bcdDeviceMSB, int bcdDeviceLSB) { QList allPorts = availableDevices(); QList thePortsWeWant; - foreach (USBPortInfo port, allPorts) { - if((port.vendorID==vid || vid==-1) && (port.productID==pid || pid==-1) && ((port.bcdDevice>>8)==bcdDeviceMSB || bcdDeviceMSB==-1) && - ( (port.bcdDevice&0x00ff) ==bcdDeviceLSB || bcdDeviceLSB==-1)) + foreach(USBPortInfo port, allPorts) { + if ((port.vendorID == vid || vid == -1) && (port.productID == pid || pid == -1) && ((port.bcdDevice >> 8) == bcdDeviceMSB || bcdDeviceMSB == -1) && + ((port.bcdDevice & 0x00ff) == bcdDeviceLSB || bcdDeviceLSB == -1)) { thePortsWeWant.append(port); + } } return thePortsWeWant; } - - USBPortInfo USBMonitor::makePortInfo(struct udev_device *dev) { USBPortInfo prtInfo; ////////// - // Debug info + // Debug info ////////// #ifdef DEBUG printPortInfo(dev); @@ -191,19 +187,15 @@ USBPortInfo USBMonitor::makePortInfo(struct udev_device *dev) bool ok; - prtInfo.vendorID = QString(udev_device_get_sysattr_value(dev, "idVendor")).toInt(&ok, 16); - prtInfo.productID = QString(udev_device_get_sysattr_value(dev, "idProduct")).toInt(&ok, 16); + prtInfo.vendorID = QString(udev_device_get_sysattr_value(dev, "idVendor")).toInt(&ok, 16); + prtInfo.productID = QString(udev_device_get_sysattr_value(dev, "idProduct")).toInt(&ok, 16); prtInfo.serialNumber = QString(udev_device_get_sysattr_value(dev, "serial")); - prtInfo.manufacturer = QString(udev_device_get_sysattr_value(dev,"manufacturer")); - prtInfo.product = QString(udev_device_get_sysattr_value(dev,"product")); -// prtInfo.UsagePage = QString(udev_device_get_sysattr_value(dev,"")); -// prtInfo.Usage = QString(udev_device_get_sysattr_value(dev,"")); - prtInfo.bcdDevice = QString(udev_device_get_sysattr_value(dev,"bcdDevice")).toInt(&ok, 16); - - - + prtInfo.manufacturer = QString(udev_device_get_sysattr_value(dev, "manufacturer")); + prtInfo.product = QString(udev_device_get_sysattr_value(dev, "product")); +// prtInfo.UsagePage = QString(udev_device_get_sysattr_value(dev,"")); +// prtInfo.Usage = QString(udev_device_get_sysattr_value(dev,"")); + prtInfo.bcdDevice = QString(udev_device_get_sysattr_value(dev, "bcdDevice")).toInt(&ok, 16); return prtInfo; - } diff --git a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp index 2f04b4b37..19aac0a47 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp @@ -36,25 +36,28 @@ #define printf qDebug -//! Local helper functions -static bool HID_GetIntProperty(IOHIDDeviceRef dev, CFStringRef property, int * value); +// ! Local helper functions +static bool HID_GetIntProperty(IOHIDDeviceRef dev, CFStringRef property, int *value); static bool HID_GetStrProperty(IOHIDDeviceRef dev, CFStringRef property, QString & value); /** - Initialize the USB monitor here - */ -USBMonitor::USBMonitor(QObject *parent): QThread(parent) { - hid_manager=NULL; + Initialize the USB monitor here + */ +USBMonitor::USBMonitor(QObject *parent) : QThread(parent) +{ + hid_manager = NULL; IOReturn ret; - m_instance = this; + m_instance = this; - listMutex = new QMutex(); + listMutex = new QMutex(); knowndevices.clear(); hid_manager = IOHIDManagerCreate(kCFAllocatorDefault, kIOHIDOptionsTypeNone); if (hid_manager == NULL || CFGetTypeID(hid_manager) != IOHIDManagerGetTypeID()) { - if (hid_manager) CFRelease(hid_manager); + if (hid_manager) { + CFRelease(hid_manager); + } Q_ASSERT(0); } @@ -78,41 +81,40 @@ USBMonitor::USBMonitor(QObject *parent): QThread(parent) { USBMonitor::~USBMonitor() { - //if(hid_manager != NULL) - // IOHIDManagerUnscheduleFromRunLoop(hid_manager, CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); + // if(hid_manager != NULL) + // IOHIDManagerUnscheduleFromRunLoop(hid_manager, CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); quit(); } -void USBMonitor::deviceEventReceived() { - +void USBMonitor::deviceEventReceived() +{ qDebug() << "Device event"; } -USBMonitor* USBMonitor::instance() +USBMonitor *USBMonitor::instance() { return m_instance; } -USBMonitor* USBMonitor::m_instance = 0; +USBMonitor *USBMonitor::m_instance = 0; -void USBMonitor::removeDevice(IOHIDDeviceRef dev) { - for( int i = 0; i < knowndevices.length(); i++) { +void USBMonitor::removeDevice(IOHIDDeviceRef dev) +{ + for (int i = 0; i < knowndevices.length(); i++) { USBPortInfo port = knowndevices.at(i); - if(port.dev_handle == dev) { + if (port.dev_handle == dev) { QMutexLocker locker(listMutex); knowndevices.removeAt(i); emit deviceRemoved(port); return; } } - - } /** - * @brief Static callback for the USB driver to indicate device removed - */ + * @brief Static callback for the USB driver to indicate device removed + */ void USBMonitor::detach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) { Q_UNUSED(context); @@ -123,8 +125,10 @@ void USBMonitor::detach_callback(void *context, IOReturn r, void *hid_mgr, IOHID instance()->removeDevice(dev); } -void USBMonitor::addDevice(USBPortInfo info) { +void USBMonitor::addDevice(USBPortInfo info) +{ QMutexLocker locker(listMutex); + knowndevices.append(info); emit deviceDiscovered(info); } @@ -144,69 +148,72 @@ void USBMonitor::attach_callback(void *context, IOReturn r, void *hid_mgr, IOHID qDebug() << "USBMonitor: Device attached event"; // Populate the device info structure - got_properties &= HID_GetIntProperty(dev, CFSTR( kIOHIDVendorIDKey ), &deviceInfo.vendorID); - got_properties &= HID_GetIntProperty(dev, CFSTR( kIOHIDProductIDKey ), &deviceInfo.productID); - got_properties &= HID_GetIntProperty(dev, CFSTR( kIOHIDVersionNumberKey ), &deviceInfo.bcdDevice); - got_properties &= HID_GetStrProperty(dev, CFSTR( kIOHIDSerialNumberKey ), deviceInfo.serialNumber); - got_properties &= HID_GetStrProperty(dev, CFSTR( kIOHIDProductKey ), deviceInfo.product); - got_properties &= HID_GetStrProperty(dev, CFSTR( kIOHIDManufacturerKey ), deviceInfo.manufacturer); + got_properties &= HID_GetIntProperty(dev, CFSTR(kIOHIDVendorIDKey), &deviceInfo.vendorID); + got_properties &= HID_GetIntProperty(dev, CFSTR(kIOHIDProductIDKey), &deviceInfo.productID); + got_properties &= HID_GetIntProperty(dev, CFSTR(kIOHIDVersionNumberKey), &deviceInfo.bcdDevice); + got_properties &= HID_GetStrProperty(dev, CFSTR(kIOHIDSerialNumberKey), deviceInfo.serialNumber); + got_properties &= HID_GetStrProperty(dev, CFSTR(kIOHIDProductKey), deviceInfo.product); + got_properties &= HID_GetStrProperty(dev, CFSTR(kIOHIDManufacturerKey), deviceInfo.manufacturer); // TOOD: Eventually want to take array of usages if devices start needing that - got_properties &= HID_GetIntProperty(dev, CFSTR( kIOHIDPrimaryUsageKey ), &deviceInfo.Usage); - got_properties &= HID_GetIntProperty(dev, CFSTR( kIOHIDPrimaryUsagePageKey ), &deviceInfo.UsagePage); + got_properties &= HID_GetIntProperty(dev, CFSTR(kIOHIDPrimaryUsageKey), &deviceInfo.Usage); + got_properties &= HID_GetIntProperty(dev, CFSTR(kIOHIDPrimaryUsagePageKey), &deviceInfo.UsagePage); // Currently only enumerating objects that have the complete list of properties - if(got_properties) { + if (got_properties) { qDebug() << "USBMonitor: Adding device"; instance()->addDevice(deviceInfo); } } /** -Returns a list of all currently available devices -*/ + Returns a list of all currently available devices + */ QList USBMonitor::availableDevices() { - //QMutexLocker locker(listMutex); + // QMutexLocker locker(listMutex); return knowndevices; } /** - * @brief Be a bit more picky and ask only for a specific type of device: - * @param[in] vid VID to screen or -1 to ignore - * @param[in] pid PID to screen or -1 to ignore - * @param[in] bcdDeviceMSB MSB of bcdDevice to screen or -1 to ignore - * @param[in] bcdDeviceLSB LSB of bcdDevice to screen or -1 to ignore - * @return List of USBPortInfo that meet this criterion - * @note - * On OpenPilot, the bcdDeviceLSB indicates the run state: bootloader or running. - * bcdDeviceMSB indicates the board model. - */ + * @brief Be a bit more picky and ask only for a specific type of device: + * @param[in] vid VID to screen or -1 to ignore + * @param[in] pid PID to screen or -1 to ignore + * @param[in] bcdDeviceMSB MSB of bcdDevice to screen or -1 to ignore + * @param[in] bcdDeviceLSB LSB of bcdDevice to screen or -1 to ignore + * @return List of USBPortInfo that meet this criterion + * @note + * On OpenPilot, the bcdDeviceLSB indicates the run state: bootloader or running. + * bcdDeviceMSB indicates the board model. + */ QList USBMonitor::availableDevices(int vid, int pid, int bcdDeviceMSB, int bcdDeviceLSB) { QList allPorts = availableDevices(); QList thePortsWeWant; - foreach (USBPortInfo port, allPorts) { - if((port.vendorID==vid || vid==-1) && (port.productID==pid || pid==-1) && ((port.bcdDevice>>8)==bcdDeviceMSB || bcdDeviceMSB==-1) && - ( (port.bcdDevice&0x00ff) ==bcdDeviceLSB || bcdDeviceLSB==-1)) + foreach(USBPortInfo port, allPorts) { + if ((port.vendorID == vid || vid == -1) && (port.productID == pid || pid == -1) && ((port.bcdDevice >> 8) == bcdDeviceMSB || bcdDeviceMSB == -1) && + ((port.bcdDevice & 0x00ff) == bcdDeviceLSB || bcdDeviceLSB == -1)) { thePortsWeWant.append(port); + } } return thePortsWeWant; } /** - * @brief Helper function get get a HID integer property - * @param[in] dev Device reference - * @param[in] property The property to get (constants defined in IOKIT) - * @param[out] value Pointer to integer to set - * @return True if successful, false otherwise - */ -static bool HID_GetIntProperty(IOHIDDeviceRef dev, CFStringRef property, int * value) { + * @brief Helper function get get a HID integer property + * @param[in] dev Device reference + * @param[in] property The property to get (constants defined in IOKIT) + * @param[out] value Pointer to integer to set + * @return True if successful, false otherwise + */ +static bool HID_GetIntProperty(IOHIDDeviceRef dev, CFStringRef property, int *value) +{ CFTypeRef prop = IOHIDDeviceGetProperty(dev, property); + if (prop) { if (CFNumberGetTypeID() == CFGetTypeID(prop)) { // if a number - CFNumberGetValue((CFNumberRef) prop, kCFNumberSInt32Type, value); + CFNumberGetValue((CFNumberRef)prop, kCFNumberSInt32Type, value); return true; } } @@ -214,18 +221,20 @@ static bool HID_GetIntProperty(IOHIDDeviceRef dev, CFStringRef property, int * v } /** - * @brief Helper function get get a HID string property - * @param[in] dev Device reference - * @param[in] property The property to get (constants defined in IOKIT) - * @param[out] value The QString to set - * @return True if successful, false otherwise - */ -static bool HID_GetStrProperty(IOHIDDeviceRef dev, CFStringRef property, QString & value) { + * @brief Helper function get get a HID string property + * @param[in] dev Device reference + * @param[in] property The property to get (constants defined in IOKIT) + * @param[out] value The QString to set + * @return True if successful, false otherwise + */ +static bool HID_GetStrProperty(IOHIDDeviceRef dev, CFStringRef property, QString & value) +{ CFTypeRef prop = IOHIDDeviceGetProperty(dev, property); + if (prop) { if (CFStringGetTypeID() == CFGetTypeID(prop)) { // if a string char buffer[2550]; - bool success = CFStringGetCString ( (CFStringRef) prop, buffer, sizeof(buffer), kCFStringEncodingMacRoman ); + bool success = CFStringGetCString((CFStringRef)prop, buffer, sizeof(buffer), kCFStringEncodingMacRoman); value = QString(buffer); return success; } diff --git a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_win.cpp b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_win.cpp index aa653179a..04f5c8154 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_win.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_win.cpp @@ -34,105 +34,108 @@ #include #define printf qDebug -void USBMonitor::deviceEventReceived() { - +void USBMonitor::deviceEventReceived() +{ qDebug() << "Device event"; // Dispatch and emit the signals here... - } -USBMonitor* USBMonitor::instance() +USBMonitor *USBMonitor::instance() { return m_instance; } -USBMonitor* USBMonitor::m_instance = 0; +USBMonitor *USBMonitor::m_instance = 0; - -USBMonitor::USBMonitor(QObject *parent): QThread(parent) { +USBMonitor::USBMonitor(QObject *parent) : QThread(parent) +{ HidD_GetHidGuid(&guid_hid); - if( !QMetaType::isRegistered( QMetaType::type("USBPortInfo") ) ) + if (!QMetaType::isRegistered(QMetaType::type("USBPortInfo"))) { qRegisterMetaType("USBPortInfo"); + } #if (defined QT_GUI_LIB) notificationWidget = 0; #endif // Q_OS_WIN setUpNotifications(); - m_instance=this; + m_instance = this; } USBMonitor::~USBMonitor() { #if (defined QT_GUI_LIB) - if( notificationWidget ) + if (notificationWidget) { delete notificationWidget; + } #endif quit(); } /** - Be a bit more picky and ask only for a specific type of device: + Be a bit more picky and ask only for a specific type of device: On OpenPilot, the bcdDeviceLSB indicates the run state: bootloader or running. bcdDeviceMSB indicates the board model. - */ + */ QList USBMonitor::availableDevices(int vid, int pid, int bcdDeviceMSB, int bcdDeviceLSB) { QList allPorts = availableDevices(); - qDebug()<<"USBMonitor::availableDevices list off all ports:"; - qDebug()<<"USBMonitor::availableDevices total ports:"< thePortsWeWant; - qDebug()<<"USBMonitor::availableDevices bcdLSB="< QString conversions and vice versa */ #ifdef UNICODE -#define QStringToTCHAR(x) (wchar_t*) x.utf16() -#define PQStringToTCHAR(x) (wchar_t*) x->utf16() -#define TCHARToQString(x) QString::fromUtf16((ushort*)(x)) -#define TCHARToQStringN(x,y) QString::fromUtf16((ushort*)(x),(y)) +#define QStringToTCHAR(x) (wchar_t *)x.utf16() +#define PQStringToTCHAR(x) (wchar_t *)x->utf16() +#define TCHARToQString(x) QString::fromUtf16((ushort *)(x)) +#define TCHARToQStringN(x, y) QString::fromUtf16((ushort *)(x), (y)) #else #define QStringToTCHAR(x) x.local8Bit().constData() #define PQStringToTCHAR(x) x->local8Bit().constData() #define TCHARToQString(x) QString::fromLocal8Bit((x)) -#define TCHARToQStringN(x,y) QString::fromLocal8Bit((x),(y)) +#define TCHARToQStringN(x, y) QString::fromLocal8Bit((x), (y)) #endif /*UNICODE*/ -void USBMonitor::setUpNotifications( ) +void USBMonitor::setUpNotifications() { #ifdef QT_GUI_LIB - if(notificationWidget) + if (notificationWidget) { return; + } notificationWidget = new USBRegistrationWidget(this); DEV_BROADCAST_DEVICEINTERFACE dbh; @@ -140,36 +143,35 @@ void USBMonitor::setUpNotifications( ) dbh.dbcc_size = sizeof(dbh); dbh.dbcc_devicetype = DBT_DEVTYP_DEVICEINTERFACE; CopyMemory(&dbh.dbcc_classguid, &guid_hid, sizeof(GUID)); - if( RegisterDeviceNotification( notificationWidget->winId( ), &dbh, DEVICE_NOTIFY_WINDOW_HANDLE ) == NULL) + if (RegisterDeviceNotification(notificationWidget->winId(), &dbh, DEVICE_NOTIFY_WINDOW_HANDLE) == NULL) { qWarning() << "RegisterDeviceNotification failed:" << GetLastError(); + } // setting up notifications doesn't tell us about devices already connected // so get those manually - foreach( USBPortInfo port,availableDevices() ) - emit deviceDiscovered( port ); + foreach(USBPortInfo port, availableDevices()) + emit deviceDiscovered(port); #else qWarning("GUI not enabled - can't register for device notifications."); #endif // QT_GUI_LIB } -LRESULT USBMonitor::onDeviceChangeWin( WPARAM wParam, LPARAM lParam ) +LRESULT USBMonitor::onDeviceChangeWin(WPARAM wParam, LPARAM lParam) { - if ( DBT_DEVICEARRIVAL == wParam || DBT_DEVICEREMOVECOMPLETE == wParam ) - { + if (DBT_DEVICEARRIVAL == wParam || DBT_DEVICEREMOVECOMPLETE == wParam) { PDEV_BROADCAST_HDR pHdr = (PDEV_BROADCAST_HDR)lParam; - if( pHdr->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE ) - { + if (pHdr->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE) { PDEV_BROADCAST_DEVICEINTERFACE pDevInf = (PDEV_BROADCAST_DEVICEINTERFACE)pHdr; // delimiters are different across APIs...change to backslash. ugh. QString deviceID = TCHARToQString(pDevInf->dbcc_name).toUpper().replace("#", "\\"); - matchAndDispatchChangedDevice(deviceID,guid_hid, wParam); + matchAndDispatchChangedDevice(deviceID, guid_hid, wParam); } } return 0; } #ifdef QT_GUI_LIB -bool USBRegistrationWidget::winEvent( MSG* message, long* result ) +bool USBRegistrationWidget::winEvent(MSG *message, long *result) { - if ( message->message == WM_DEVICECHANGE ) { - qese->onDeviceChangeWin( message->wParam, message->lParam ); + if (message->message == WM_DEVICECHANGE) { + qese->onDeviceChangeWin(message->wParam, message->lParam); *result = 1; return true; } @@ -178,76 +180,62 @@ bool USBRegistrationWidget::winEvent( MSG* message, long* result ) #endif bool USBMonitor::matchAndDispatchChangedDevice(const QString & deviceID, const GUID & guid, WPARAM wParam) { - qDebug()<<"USB_MONITOR matchAndDispatchChangedDevice deviceID="< USBMonitor::availableDevices() { QList ports; enumerateDevicesWin(guid_hid, &ports); - //qDebug()<<"USBMonitorWin availabledevices="<append(info); - else if (r==0) + } else if (r == 0) { break; + } } SetupDiDestroyDeviceInfoList(devInfo); } } -int USBMonitor::infoFromHandle(const GUID & guid,USBPortInfo & info,HDEVINFO & devInfo,DWORD & index) +int USBMonitor::infoFromHandle(const GUID & guid, USBPortInfo & info, HDEVINFO & devInfo, DWORD & index) { - //qDebug()<<"index0="<DevicePath); + + free(details); + return 2; } - //qDebug()<<"index3="<DevicePath, GENERIC_READ|GENERIC_WRITE, FILE_SHARE_READ|FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL); - - if (h == INVALID_HANDLE_VALUE) - { - DWORD err = GetLastError(); - - // I get ERROR_ACCESS_DENIED with most/all my input devices (mice/trackballs/tablet). - // Let's not log it :) - if (err == ERROR_ACCESS_DENIED) - { - free(details); - return 2; - } - - qDebug() << "Problem opening handle, path: " << QString().fromWCharArray(details->DevicePath); - - free(details); - return 2; - } - //qDebug()<<"index4="<DevicePath).toUpper().replace("#", "\\"); - attrib.Size = sizeof(HIDD_ATTRIBUTES); + // qDebug()<<"DETAILS???"<DevicePath).toUpper().replace("#", "\\"); + attrib.Size = sizeof(HIDD_ATTRIBUTES); ret = HidD_GetAttributes(h, &attrib); - info.vendorID=attrib.VendorID; - info.productID=attrib.ProductID; - info.bcdDevice=attrib.VersionNumber; + info.vendorID = attrib.VendorID; + info.productID = attrib.ProductID; + info.bcdDevice = attrib.VersionNumber; - if (!ret || !HidD_GetPreparsedData(h, &hid_data)) - { - CloseHandle(h); - return 2; + if (!ret || !HidD_GetPreparsedData(h, &hid_data)) { + CloseHandle(h); + return 2; } - //qDebug()<<"index5="<> 8) == m_boardModel || m_boardModel == -1) && + ((port.bcdDevice & 0x00ff) == m_runState || m_runState == -1)) { + qDebug() << "USBSignalFilter emit device discovered"; emit deviceDiscovered(); } } -USBSignalFilter::USBSignalFilter(int vid, int pid, int boardModel, int runState): +USBSignalFilter::USBSignalFilter(int vid, int pid, int boardModel, int runState) : m_vid(vid), m_pid(pid), m_boardModel(boardModel), m_runState(runState) { - connect(USBMonitor::instance(),SIGNAL(deviceDiscovered(USBPortInfo)),this,SLOT(m_deviceDiscovered(USBPortInfo))); + connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), this, SLOT(m_deviceDiscovered(USBPortInfo))); } - - diff --git a/ground/openpilotgcs/src/plugins/rawhid/usbsignalfilter.h b/ground/openpilotgcs/src/plugins/rawhid/usbsignalfilter.h index 6a9310d46..a3eafa131 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/usbsignalfilter.h +++ b/ground/openpilotgcs/src/plugins/rawhid/usbsignalfilter.h @@ -3,8 +3,7 @@ #include #include "usbmonitor.h" -class RAWHID_EXPORT USBSignalFilter : public QObject -{ +class RAWHID_EXPORT USBSignalFilter : public QObject { Q_OBJECT private: int m_vid; diff --git a/ground/openpilotgcs/src/plugins/scope/plotdata.cpp b/ground/openpilotgcs/src/plugins/scope/plotdata.cpp index 8d4a8ff69..673b322d8 100644 --- a/ground/openpilotgcs/src/plugins/scope/plotdata.cpp +++ b/ground/openpilotgcs/src/plugins/scope/plotdata.cpp @@ -31,49 +31,47 @@ #include PlotData::PlotData(QString p_uavObject, QString p_uavField) -{ +{ uavObject = p_uavObject; - if(p_uavField.contains("-")) - { + if (p_uavField.contains("-")) { QStringList fieldSubfield = p_uavField.split("-", QString::SkipEmptyParts); - uavField = fieldSubfield.at(0); - uavSubField = fieldSubfield.at(1); + uavField = fieldSubfield.at(0); + uavSubField = fieldSubfield.at(1); haveSubField = true; - } - else - { - uavField = p_uavField; + } else { + uavField = p_uavField; haveSubField = false; } - xData = new QVector(); - yData = new QVector(); - yDataHistory = new QVector(); + xData = new QVector(); + yData = new QVector(); + yDataHistory = new QVector(); - curve = 0; - scalePower = 0; + curve = 0; + scalePower = 0; meanSamples = 1; - meanSum = 0.0f; -// mathFunction=0; - correctionSum = 0.0f; + meanSum = 0.0f; +// mathFunction=0; + correctionSum = 0.0f; correctionCount = 0; - yMinimum = 0; - yMaximum = 0; + yMinimum = 0; + yMaximum = 0; - m_xWindowSize = 0; + m_xWindowSize = 0; } -double PlotData::valueAsDouble(UAVObject* obj, UAVObjectField* field) +double PlotData::valueAsDouble(UAVObject *obj, UAVObjectField *field) { Q_UNUSED(obj); QVariant value; - if(haveSubField){ + if (haveSubField) { int indexOfSubField = field->getElementNames().indexOf(QRegExp(uavSubField, Qt::CaseSensitive, QRegExp::FixedString)); value = field->getValue(indexOfSubField); - }else + } else { value = field->getValue(); + } // qDebug() << "Data (" << value.typeName() << ") " << value.toString(); @@ -88,63 +86,60 @@ PlotData::~PlotData() } -bool SequentialPlotData::append(UAVObject* obj) +bool SequentialPlotData::append(UAVObject *obj) { if (uavObject == obj->getName()) { - - //Get the field of interest - UAVObjectField* field = obj->getField(uavField); + // Get the field of interest + UAVObjectField *field = obj->getField(uavField); if (field) { - double currentValue = valueAsDouble(obj, field) * pow(10, scalePower); - //Perform scope math, if necessary - if (mathFunction == "Boxcar average" || mathFunction == "Standard deviation"){ - //Put the new value at the front - yDataHistory->append( currentValue ); + // Perform scope math, if necessary + if (mathFunction == "Boxcar average" || mathFunction == "Standard deviation") { + // Put the new value at the front + yDataHistory->append(currentValue); // calculate average value meanSum += currentValue; - if(yDataHistory->size() > meanSamples) { + if (yDataHistory->size() > meanSamples) { meanSum -= yDataHistory->first(); yDataHistory->pop_front(); } // make sure to correct the sum every meanSamples steps to prevent it // from running away due to floating point rounding errors - correctionSum+=currentValue; + correctionSum += currentValue; if (++correctionCount >= meanSamples) { meanSum = correctionSum; correctionSum = 0.0f; correctionCount = 0; } - double boxcarAvg=meanSum/yDataHistory->size(); + double boxcarAvg = meanSum / yDataHistory->size(); - if ( mathFunction == "Standard deviation" ){ - //Calculate square of sample standard deviation, with Bessel's correction - double stdSum=0; - for (int i=0; i < yDataHistory->size(); i++){ - stdSum+= pow(yDataHistory->at(i)- boxcarAvg,2)/(meanSamples-1); + if (mathFunction == "Standard deviation") { + // Calculate square of sample standard deviation, with Bessel's correction + double stdSum = 0; + for (int i = 0; i < yDataHistory->size(); i++) { + stdSum += pow(yDataHistory->at(i) - boxcarAvg, 2) / (meanSamples - 1); } yData->append(sqrt(stdSum)); - } - else { + } else { yData->append(boxcarAvg); } - } - else{ - yData->append( currentValue ); + } else { + yData->append(currentValue); } - if (yData->size() > m_xWindowSize) { //If new data overflows the window, remove old data... + if (yData->size() > m_xWindowSize) { // If new data overflows the window, remove old data... yData->pop_front(); - } else //...otherwise, add a new y point at position xData + } else { // ...otherwise, add a new y point at position xData xData->insert(xData->size(), xData->size()); + } - //notify the gui of changes in the data - //dataChanged(); + // notify the gui of changes in the data + // dataChanged(); return true; } } @@ -152,65 +147,63 @@ bool SequentialPlotData::append(UAVObject* obj) return false; } -bool ChronoPlotData::append(UAVObject* obj) +bool ChronoPlotData::append(UAVObject *obj) { if (uavObject == obj->getName()) { - //Get the field of interest - UAVObjectField* field = obj->getField(uavField); - //qDebug() << "uavObject: " << uavObject << ", uavField: " << uavField; + // Get the field of interest + UAVObjectField *field = obj->getField(uavField); + // qDebug() << "uavObject: " << uavObject << ", uavField: " << uavField; if (field) { - QDateTime NOW = QDateTime::currentDateTime(); //THINK ABOUT REIMPLEMENTING THIS TO SHOW UAVO TIME, NOT SYSTEM TIME + QDateTime NOW = QDateTime::currentDateTime(); // THINK ABOUT REIMPLEMENTING THIS TO SHOW UAVO TIME, NOT SYSTEM TIME double currentValue = valueAsDouble(obj, field) * pow(10, scalePower); - //Perform scope math, if necessary - if (mathFunction == "Boxcar average" || mathFunction == "Standard deviation"){ - //Put the new value at the back - yDataHistory->append( currentValue ); + // Perform scope math, if necessary + if (mathFunction == "Boxcar average" || mathFunction == "Standard deviation") { + // Put the new value at the back + yDataHistory->append(currentValue); // calculate average value meanSum += currentValue; - if(yDataHistory->size() > meanSamples) { + if (yDataHistory->size() > meanSamples) { meanSum -= yDataHistory->first(); yDataHistory->pop_front(); } // make sure to correct the sum every meanSamples steps to prevent it // from running away due to floating point rounding errors - correctionSum+=currentValue; + correctionSum += currentValue; if (++correctionCount >= meanSamples) { meanSum = correctionSum; correctionSum = 0.0f; correctionCount = 0; } - double boxcarAvg=meanSum/yDataHistory->size(); -//qDebug()<size(); i++){ - stdSum+= pow(yDataHistory->at(i)- boxcarAvg,2)/(meanSamples-1); + double boxcarAvg = meanSum / yDataHistory->size(); +// qDebug()<size(); i++) { + stdSum += pow(yDataHistory->at(i) - boxcarAvg, 2) / (meanSamples - 1); } yData->append(sqrt(stdSum)); - } - else { + } else { yData->append(boxcarAvg); } - } - else{ - yData->append( currentValue ); + } else { + yData->append(currentValue); } double valueX = NOW.toTime_t() + NOW.time().msec() / 1000.0; xData->append(valueX); - //qDebug() << "Data " << uavObject << "." << field->getName() << " X,Y:" << valueX << "," << valueY; + // qDebug() << "Data " << uavObject << "." << field->getName() << " X,Y:" << valueX << "," << valueY; - //Remove stale data + // Remove stale data removeStaleData(); - //notify the gui of chages in the data - //dataChanged(); + // notify the gui of chages in the data + // dataChanged(); return true; } } @@ -224,8 +217,9 @@ void ChronoPlotData::removeStaleData() double oldestValue; while (1) { - if (xData->size() == 0) + if (xData->size() == 0) { break; + } newestValue = xData->last(); oldestValue = xData->first(); @@ -233,21 +227,22 @@ void ChronoPlotData::removeStaleData() if (newestValue - oldestValue > m_xWindowSize) { yData->pop_front(); xData->pop_front(); - } else + } else { break; + } } - //qDebug() << "removeStaleData "; + // qDebug() << "removeStaleData "; } void ChronoPlotData::removeStaleDataTimeout() { removeStaleData(); - //dataChanged(); - //qDebug() << "removeStaleDataTimeout"; + // dataChanged(); + // qDebug() << "removeStaleDataTimeout"; } -bool UAVObjectPlotData::append(UAVObject* obj) +bool UAVObjectPlotData::append(UAVObject *obj) { Q_UNUSED(obj); return false; diff --git a/ground/openpilotgcs/src/plugins/scope/plotdata.h b/ground/openpilotgcs/src/plugins/scope/plotdata.h index 358f97aa2..5b17ca917 100644 --- a/ground/openpilotgcs/src/plugins/scope/plotdata.h +++ b/ground/openpilotgcs/src/plugins/scope/plotdata.h @@ -41,8 +41,8 @@ #include /*! -\brief Defines the different type of plots. - */ + \brief Defines the different type of plots. + */ enum PlotType { SequentialPlot, ChronoPlot, @@ -52,10 +52,9 @@ enum PlotType { }; /*! - \brief Base class that keeps the data for each curve in the plot. - */ -class PlotData : public QObject -{ + \brief Base class that keeps the data for each curve in the plot. + */ +class PlotData : public QObject { Q_OBJECT public: @@ -66,7 +65,7 @@ public: QString uavField; QString uavSubField; bool haveSubField; - int scalePower; //This is the power to which each value must be raised + int scalePower; // This is the power to which each value must be raised int meanSamples; double meanSum; QString mathFunction; @@ -75,71 +74,71 @@ public: double yMinimum; double yMaximum; double m_xWindowSize; - QwtPlotCurve* curve; - QVector* xData; - QVector* yData; - QVector* yDataHistory; + QwtPlotCurve *curve; + QVector *xData; + QVector *yData; + QVector *yDataHistory; - virtual bool append(UAVObject* obj) = 0; - virtual PlotType plotType() = 0; + virtual bool append(UAVObject *obj) = 0; + virtual PlotType plotType() = 0; virtual void removeStaleData() = 0; void updatePlotCurveData(); protected: - double valueAsDouble(UAVObject* obj, UAVObjectField* field); + double valueAsDouble(UAVObject *obj, UAVObjectField *field); signals: void dataChanged(); }; /*! - \brief The sequential plot have a fixed size buffer of data. All the curves in one plot - have the same size buffer. - */ -class SequentialPlotData : public PlotData -{ + \brief The sequential plot have a fixed size buffer of data. All the curves in one plot + have the same size buffer. + */ +class SequentialPlotData : public PlotData { Q_OBJECT public: SequentialPlotData(QString uavObject, QString uavField) - : PlotData(uavObject, uavField) {} + : PlotData(uavObject, uavField) {} ~SequentialPlotData() {} /*! - \brief Append new data to the plot - */ - bool append(UAVObject* obj); + \brief Append new data to the plot + */ + bool append(UAVObject *obj); /*! - \brief The type of plot - */ - virtual PlotType plotType() { + \brief The type of plot + */ + virtual PlotType plotType() + { return SequentialPlot; } /*! - \brief Removes the old data from the buffer - */ - virtual void removeStaleData(){} + \brief Removes the old data from the buffer + */ + virtual void removeStaleData() {} }; /*! - \brief The chrono plot have a variable sized buffer of data, where the data is for a specified time period. - */ -class ChronoPlotData : public PlotData -{ + \brief The chrono plot have a variable sized buffer of data, where the data is for a specified time period. + */ +class ChronoPlotData : public PlotData { Q_OBJECT public: ChronoPlotData(QString uavObject, QString uavField) - : PlotData(uavObject, uavField) { + : PlotData(uavObject, uavField) + { scalePower = 1; } - ~ChronoPlotData() { - } + ~ChronoPlotData() {} - bool append(UAVObject* obj); + bool append(UAVObject *obj); - virtual PlotType plotType() { + virtual PlotType plotType() + { return ChronoPlot; } @@ -152,24 +151,24 @@ private slots: }; /*! - \brief UAVObject plot use a fixed size buffer of data, where the horizontal axis values come from - a UAVObject field. - */ -class UAVObjectPlotData : public PlotData -{ + \brief UAVObject plot use a fixed size buffer of data, where the horizontal axis values come from + a UAVObject field. + */ +class UAVObjectPlotData : public PlotData { Q_OBJECT public: UAVObjectPlotData(QString uavObject, QString uavField) - : PlotData(uavObject, uavField) {} + : PlotData(uavObject, uavField) {} ~UAVObjectPlotData() {} - bool append(UAVObject* obj); + bool append(UAVObject *obj); - virtual PlotType plotType() { + virtual PlotType plotType() + { return UAVObjectPlot; - } + } - virtual void removeStaleData(){} + virtual void removeStaleData() {} }; #endif // PLOTDATA_H diff --git a/ground/openpilotgcs/src/plugins/scope/scope_global.h b/ground/openpilotgcs/src/plugins/scope/scope_global.h index 6da462245..648369745 100644 --- a/ground/openpilotgcs/src/plugins/scope/scope_global.h +++ b/ground/openpilotgcs/src/plugins/scope/scope_global.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -38,4 +38,3 @@ #endif #endif // UAVOBJECTS_GLOBAL_H - diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadget.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadget.cpp index ec7c01113..a403233bd 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadget.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadget.cpp @@ -59,7 +59,8 @@ void ScopeGadget::loadConfiguration(IUAVGadgetConfiguration *config) int mean = plotCurveConfig->yMeanSamples; QString mathFunction = plotCurveConfig->mathFunction; QRgb color = plotCurveConfig->color; - bool antialiased = plotCurveConfig->drawAntialiased; + bool antialiased = plotCurveConfig->drawAntialiased; + widget->addCurvePlot( uavObject, uavField, @@ -71,7 +72,7 @@ void ScopeGadget::loadConfiguration(IUAVGadgetConfiguration *config) Qt::SolidLine, Qt::SquareCap, Qt::BevelJoin), - antialiased + antialiased ); } diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadget.h b/ground/openpilotgcs/src/plugins/scope/scopegadget.h index 9d7c5ef3e..8b06739b6 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadget.h +++ b/ground/openpilotgcs/src/plugins/scope/scopegadget.h @@ -33,29 +33,31 @@ #include "scopegadgetwidget.h" class IUAVGadget; -//class QList; +// class QList; class QWidget; class QString; class ScopeGadgetWidget; using namespace Core; -class ScopeGadget : public Core::IUAVGadget -{ +class ScopeGadget : public Core::IUAVGadget { Q_OBJECT public: ScopeGadget(QString classId, ScopeGadgetWidget *widget, QWidget *parent = 0); ~ScopeGadget(); - void loadConfiguration(IUAVGadgetConfiguration* config); + void loadConfiguration(IUAVGadgetConfiguration *config); - QList context() const { + QList context() const + { return m_context; } - QWidget *widget() { + QWidget *widget() + { return m_widget; } - QString contextHelpId() const { + QString contextHelpId() const + { return QString(); } diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.cpp index cc1095cfc..0dd99f35d 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.cpp @@ -55,16 +55,16 @@ ScopeGadgetConfiguration::ScopeGadgetConfiguration(QString classId, QSettings *q qSettings->beginGroup(QString("plotCurve") + QString().number(plotDatasLoadIndex)); PlotCurveConfiguration *plotCurveConf = new PlotCurveConfiguration(); - plotCurveConf->uavObject = qSettings->value("uavObject").toString(); - plotCurveConf->uavField = qSettings->value("uavField").toString(); - plotCurveConf->color = qSettings->value("color").value(); - plotCurveConf->yScalePower = qSettings->value("yScalePower").toInt(); - plotCurveConf->mathFunction = qSettings->value("mathFunction").toString(); - plotCurveConf->yMeanSamples = qSettings->value("yMeanSamples", 1).toInt(); - plotCurveConf->yMeanSamples = qSettings->value("yMeanSamples", 1).toInt(); + plotCurveConf->uavObject = qSettings->value("uavObject").toString(); + plotCurveConf->uavField = qSettings->value("uavField").toString(); + plotCurveConf->color = qSettings->value("color").value(); + plotCurveConf->yScalePower = qSettings->value("yScalePower").toInt(); + plotCurveConf->mathFunction = qSettings->value("mathFunction").toString(); + plotCurveConf->yMeanSamples = qSettings->value("yMeanSamples", 1).toInt(); + plotCurveConf->yMeanSamples = qSettings->value("yMeanSamples", 1).toInt(); plotCurveConf->drawAntialiased = qSettings->value("drawAntialiased", true).toBool(); - plotCurveConf->yMinimum = qSettings->value("yMinimum").toDouble(); - plotCurveConf->yMaximum = qSettings->value("yMaximum").toDouble(); + plotCurveConf->yMinimum = qSettings->value("yMinimum").toDouble(); + plotCurveConf->yMaximum = qSettings->value("yMaximum").toDouble(); m_plotCurveConfigs.append(plotCurveConf); qSettings->endGroup(); @@ -72,7 +72,7 @@ ScopeGadgetConfiguration::ScopeGadgetConfiguration(QString classId, QSettings *q m_loggingEnabled = qSettings->value("LoggingEnabled").toBool(); m_loggingNewFileOnConnect = qSettings->value("LoggingNewFileOnConnect").toBool(); - m_loggingPath = qSettings->value("LoggingPath").toString(); + m_loggingPath = qSettings->value("LoggingPath").toString(); } } @@ -109,15 +109,15 @@ IUAVGadgetConfiguration *ScopeGadgetConfiguration::clone() PlotCurveConfiguration *currentPlotCurveConf = m_plotCurveConfigs.at(plotDatasLoadIndex); PlotCurveConfiguration *newPlotCurveConf = new PlotCurveConfiguration(); - newPlotCurveConf->uavObject = currentPlotCurveConf->uavObject; - newPlotCurveConf->uavField = currentPlotCurveConf->uavField; - newPlotCurveConf->color = currentPlotCurveConf->color; - newPlotCurveConf->yScalePower = currentPlotCurveConf->yScalePower; - newPlotCurveConf->yMeanSamples = currentPlotCurveConf->yMeanSamples; - newPlotCurveConf->mathFunction = currentPlotCurveConf->mathFunction; + newPlotCurveConf->uavObject = currentPlotCurveConf->uavObject; + newPlotCurveConf->uavField = currentPlotCurveConf->uavField; + newPlotCurveConf->color = currentPlotCurveConf->color; + newPlotCurveConf->yScalePower = currentPlotCurveConf->yScalePower; + newPlotCurveConf->yMeanSamples = currentPlotCurveConf->yMeanSamples; + newPlotCurveConf->mathFunction = currentPlotCurveConf->mathFunction; newPlotCurveConf->drawAntialiased = currentPlotCurveConf->drawAntialiased; - newPlotCurveConf->yMinimum = currentPlotCurveConf->yMinimum; - newPlotCurveConf->yMaximum = currentPlotCurveConf->yMaximum; + newPlotCurveConf->yMinimum = currentPlotCurveConf->yMinimum; + newPlotCurveConf->yMaximum = currentPlotCurveConf->yMaximum; m->addPlotCurveConfig(newPlotCurveConf); } diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.cpp index adbdcdd0d..df75fe0a5 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.cpp @@ -32,15 +32,13 @@ #include ScopeGadgetFactory::ScopeGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("ScopeGadget"), - tr("Scope"), - parent) -{ -} + IUAVGadgetFactory(QString("ScopeGadget"), + tr("Scope"), + parent) +{} ScopeGadgetFactory::~ScopeGadgetFactory() -{ -} +{} void ScopeGadgetFactory::stopPlotting() { @@ -53,20 +51,21 @@ void ScopeGadgetFactory::startPlotting() } -Core::IUAVGadget* ScopeGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *ScopeGadgetFactory::createGadget(QWidget *parent) { - ScopeGadgetWidget* gadgetWidget = new ScopeGadgetWidget(parent); - connect(this,SIGNAL(onStartPlotting()), gadgetWidget, SLOT(startPlotting())); - connect(this,SIGNAL(onStopPlotting()), gadgetWidget, SLOT(stopPlotting())); + ScopeGadgetWidget *gadgetWidget = new ScopeGadgetWidget(parent); + + connect(this, SIGNAL(onStartPlotting()), gadgetWidget, SLOT(startPlotting())); + connect(this, SIGNAL(onStopPlotting()), gadgetWidget, SLOT(stopPlotting())); return new ScopeGadget(QString("ScopeGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *ScopeGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *ScopeGadgetFactory::createConfiguration(QSettings *qSettings) { return new ScopeGadgetConfiguration(QString("ScopeGadget"), qSettings); } IOptionsPage *ScopeGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new ScopeGadgetOptionsPage(qobject_cast(config)); + return new ScopeGadgetOptionsPage(qobject_cast(config)); } diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.h b/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.h index 16fd4f431..11a77f2ca 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetfactory.h @@ -31,23 +31,21 @@ #include "scope_global.h" #include -namespace Core -{ +namespace Core { class IUAVGadget; class IUAVGadgetFactory; } using namespace Core; -class SCOPE_EXPORT ScopeGadgetFactory : public IUAVGadgetFactory -{ +class SCOPE_EXPORT ScopeGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: ScopeGadgetFactory(QObject *parent = 0); ~ScopeGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); public slots: @@ -57,7 +55,6 @@ public slots: signals: void onStopPlotting(); void onStartPlotting(); - }; #endif // SCOPEGADGETFACTORY_H_ diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.cpp index e0c0a0143..8ba4440b6 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.cpp @@ -103,13 +103,13 @@ QWidget *ScopeGadgetOptionsPage::createPage(QWidget *parent) // add the configured curves foreach(PlotCurveConfiguration * plotData, m_config->plotCurveConfigs()) { - QString uavObject = plotData->uavObject; - QString uavField = plotData->uavField; + QString uavObject = plotData->uavObject; + QString uavField = plotData->uavField; int scale = plotData->yScalePower; int mean = plotData->yMeanSamples; QString mathFunction = plotData->mathFunction; - QVariant varColor = plotData->color; - bool antialiased = plotData->drawAntialiased; + QVariant varColor = plotData->color; + bool antialiased = plotData->drawAntialiased; addPlotCurveConfig(uavObject, uavField, scale, mean, mathFunction, varColor, antialiased); } @@ -335,7 +335,7 @@ void ScopeGadgetOptionsPage::on_btnAddCurve_clicked() QVariant varColor = (int)QColor(options_page->btnColor->text()).rgb(); - bool antialiased = options_page->drawAntialiasedCheckBox->isChecked(); + bool antialiased = options_page->drawAntialiasedCheckBox->isChecked(); // Find an existing plot curve config based on the uavobject and uav field. If it // exists, update it, else add a new one. if (options_page->lstCurves->count() && @@ -363,7 +363,7 @@ void ScopeGadgetOptionsPage::addPlotCurveConfig(QString uavObject, QString uavFi void ScopeGadgetOptionsPage::setCurvePlotProperties(QListWidgetItem *listWidgetItem, QString uavObject, QString uavField, int scale, int mean, QString mathFunction, QVariant varColor, bool antialias) { - bool parseOK = false; + bool parseOK = false; // Set the properties of the newly added list item QRgb rgbColor = (QRgb)varColor.toInt(&parseOK); diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.h b/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.h index da081c5c5..a7e57b65d 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.h @@ -42,25 +42,24 @@ #include /*! - \brief This class is used to render the time values on the horizontal axis for the - ChronoPlot. - */ -class TimeScaleDraw : public QwtScaleDraw -{ + \brief This class is used to render the time values on the horizontal axis for the + ChronoPlot. + */ +class TimeScaleDraw : public QwtScaleDraw { public: - TimeScaleDraw() { - } - virtual QwtText label(double v) const { - uint seconds = (uint)(v); + TimeScaleDraw() {} + virtual QwtText label(double v) const + { + uint seconds = (uint)(v); QDateTime upTime = QDateTime::fromTime_t(seconds); - QTime timePart = upTime.time().addMSecs((v - seconds )* 1000); + QTime timePart = upTime.time().addMSecs((v - seconds) * 1000); + upTime.setTime(timePart); return upTime.toLocalTime().toString("hh:mm:ss"); } }; -class ScopeGadgetWidget : public QwtPlot -{ +class ScopeGadgetWidget : public QwtPlot { Q_OBJECT public: @@ -70,34 +69,58 @@ public: void setupSequentialPlot(); void setupChronoPlot(); void setupUAVObjectPlot(); - PlotType plotType(){return m_plotType;} + PlotType plotType() + { + return m_plotType; + } - void setXWindowSize(double xWindowSize){m_xWindowSize = xWindowSize;} - double xWindowSize(){return m_xWindowSize;} - void setRefreshInterval(double refreshInterval){m_refreshInterval = refreshInterval;} - int refreshInterval(){return m_refreshInterval;} + void setXWindowSize(double xWindowSize) + { + m_xWindowSize = xWindowSize; + } + double xWindowSize() + { + return m_xWindowSize; + } + void setRefreshInterval(double refreshInterval) + { + m_refreshInterval = refreshInterval; + } + int refreshInterval() + { + return m_refreshInterval; + } void addCurvePlot(QString uavObject, QString uavFieldSubField, int scaleOrderFactor = 0, int meanSamples = 1, - QString mathFunction= "None", QPen pen = QPen(Qt::black), bool antialiased = true); + QString mathFunction = "None", QPen pen = QPen(Qt::black), bool antialiased = true); void clearCurvePlots(); int csvLoggingStart(); int csvLoggingStop(); void csvLoggingSetName(QString); - void setLoggingEnabled(bool value){m_csvLoggingEnabled=value;} - void setLoggingNewFileOnConnect(bool value){m_csvLoggingNewFileOnConnect=value;} - void setLoggingPath(QString value){m_csvLoggingPath=value;} + void setLoggingEnabled(bool value) + { + m_csvLoggingEnabled = value; + } + void setLoggingNewFileOnConnect(bool value) + { + m_csvLoggingNewFileOnConnect = value; + } + void setLoggingPath(QString value) + { + m_csvLoggingPath = value; + } protected: - void mousePressEvent(QMouseEvent *e); - void mouseReleaseEvent(QMouseEvent *e); - void mouseDoubleClickEvent(QMouseEvent *e); - void mouseMoveEvent(QMouseEvent *e); - void wheelEvent(QWheelEvent *e); + void mousePressEvent(QMouseEvent *e); + void mouseReleaseEvent(QMouseEvent *e); + void mouseDoubleClickEvent(QMouseEvent *e); + void mouseMoveEvent(QMouseEvent *e); + void wheelEvent(QWheelEvent *e); void showEvent(QShowEvent *e); private slots: - void uavObjectReceived(UAVObject*); + void uavObjectReceived(UAVObject *); void replotNewData(); void showCurve(QwtPlotItem *item, bool on); void startPlotting(); @@ -115,7 +138,7 @@ private: double m_xWindowSize; int m_refreshInterval; QList m_connectedUAVObjects; - QMap m_curvesData; + QMap m_curvesData; QTimer *replotTimer; @@ -136,14 +159,14 @@ private: QString m_csvLoggingBuffer; QFile m_csvLoggingFile; - QMutex mutex; + QMutex mutex; int csvLoggingInsertHeader(); int csvLoggingAddData(); int csvLoggingInsertData(); - void deleteLegend(); - void addLegend(); + void deleteLegend(); + void addLegend(); }; diff --git a/ground/openpilotgcs/src/plugins/scope/scopeplugin.cpp b/ground/openpilotgcs/src/plugins/scope/scopeplugin.cpp index 889c29f3b..9ff600b20 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopeplugin.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopeplugin.cpp @@ -44,7 +44,7 @@ ScopePlugin::~ScopePlugin() } -bool ScopePlugin::initialize(const QStringList& args, QString *errMsg) +bool ScopePlugin::initialize(const QStringList & args, QString *errMsg) { Q_UNUSED(args); Q_UNUSED(errMsg); @@ -64,4 +64,3 @@ void ScopePlugin::shutdown() // Do nothing } Q_EXPORT_PLUGIN(ScopePlugin) - diff --git a/ground/openpilotgcs/src/plugins/scope/scopeplugin.h b/ground/openpilotgcs/src/plugins/scope/scopeplugin.h index 1abadcfee..d51527c30 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopeplugin.h +++ b/ground/openpilotgcs/src/plugins/scope/scopeplugin.h @@ -32,19 +32,16 @@ class ScopeGadgetFactory; -class ScopePlugin : public ExtensionSystem::IPlugin -{ +class ScopePlugin : public ExtensionSystem::IPlugin { public: ScopePlugin(); ~ScopePlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); private: ScopeGadgetFactory *mf; - - }; #endif /* SCOPEPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serial_global.h b/ground/openpilotgcs/src/plugins/serialconnection/serial_global.h index 832676a99..e284778a1 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serial_global.h +++ b/ground/openpilotgcs/src/plugins/serialconnection/serial_global.h @@ -3,7 +3,7 @@ * * @file serial_global.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief + * @brief * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup SerialPlugin Serial Connection Plugin @@ -11,18 +11,18 @@ * @brief Impliments serial connection to the flight hardware for Telemetry *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp b/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp index 1f7a476f0..b56dbb001 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp @@ -10,18 +10,18 @@ * @brief Impliments serial connection to the flight hardware for Telemetry *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -36,38 +36,34 @@ #include - SerialEnumerationThread::SerialEnumerationThread(SerialConnection *serial) : m_serial(serial), m_running(true) -{ -} +{} SerialEnumerationThread::~SerialEnumerationThread() { m_running = false; - //wait for the thread to terminate - if(wait(2100) == false) + // wait for the thread to terminate + if (wait(2100) == false) { qDebug() << "Cannot terminate SerialEnumerationThread"; + } } void SerialEnumerationThread::run() { QList devices = m_serial->availableDevices(); - while(m_running) - { - if(!m_serial->deviceOpened()) - { + while (m_running) { + if (!m_serial->deviceOpened()) { QList newDev = m_serial->availableDevices(); - if(devices != newDev) - { + if (devices != newDev) { devices = newDev; emit enumerationChanged(); } } - msleep(2000); //update available devices every two seconds (doesn't need more) + msleep(2000); // update available devices every two seconds (doesn't need more) } } @@ -75,45 +71,45 @@ void SerialEnumerationThread::run() SerialConnection::SerialConnection() : enablePolling(true), m_enumerateThread(this) { - serialHandle = NULL; - m_config = new SerialPluginConfiguration("Serial Telemetry", NULL, this); + serialHandle = NULL; + m_config = new SerialPluginConfiguration("Serial Telemetry", NULL, this); m_config->restoresettings(); - m_optionspage = new SerialPluginOptionsPage(m_config,this); + m_optionspage = new SerialPluginOptionsPage(m_config, this); // Experimental: enable polling on all OS'es since there // were reports that autodetect does not work on XP amongst // others. - //#ifdef Q_OS_WIN -// //I'm cheating a little bit here: -// //Knowing if the device enumeration really changed is a bit complicated -// //so I just signal it whenever we have a device event... -// QMainWindow *mw = Core::ICore::instance()->mainWindow(); -// QObject::connect(mw, SIGNAL(deviceChange()), -// this, SLOT(onEnumerationChanged())); -//#else + // #ifdef Q_OS_WIN +////I'm cheating a little bit here: +////Knowing if the device enumeration really changed is a bit complicated +////so I just signal it whenever we have a device event... +// QMainWindow *mw = Core::ICore::instance()->mainWindow(); +// QObject::connect(mw, SIGNAL(deviceChange()), +// this, SLOT(onEnumerationChanged())); +// #else // Other OSes do not send such signals: QObject::connect(&m_enumerateThread, SIGNAL(enumerationChanged()), this, SLOT(onEnumerationChanged())); m_enumerateThread.start(); -//#endif +// #endif } SerialConnection::~SerialConnection() -{ -} +{} void SerialConnection::onEnumerationChanged() { - if (enablePolling) + if (enablePolling) { emit availableDevChanged(this); + } } -bool sortPorts(const QextPortInfo &s1,const QextPortInfo &s2) +bool sortPorts(const QextPortInfo &s1, const QextPortInfo &s2) { - return s1.portName SerialConnection::availableDevices() @@ -123,13 +119,14 @@ QList SerialConnection::availableDevices() if (enablePolling) { QList ports = QextSerialEnumerator::getPorts(); - //sort the list by port number (nice idea from PT_Dreamer :)) - qSort(ports.begin(), ports.end(),sortPorts); - foreach( QextPortInfo port, ports ) { - device d; - d.displayName=port.friendName; - d.name=port.physName; - list.append(d); + // sort the list by port number (nice idea from PT_Dreamer :)) + qSort(ports.begin(), ports.end(), sortPorts); + foreach(QextPortInfo port, ports) { + device d; + + d.displayName = port.friendName; + d.name = port.physName; + list.append(d); } } @@ -138,28 +135,27 @@ QList SerialConnection::availableDevices() QIODevice *SerialConnection::openDevice(const QString &deviceName) { - if (serialHandle){ + if (serialHandle) { closeDevice(deviceName); } QList ports = QextSerialEnumerator::getPorts(); - foreach( QextPortInfo port, ports ) { - if(port.physName == deviceName) - { - //we need to handle port settings here... + foreach(QextPortInfo port, ports) { + if (port.physName == deviceName) { + // we need to handle port settings here... PortSettings set; - set.BaudRate = stringToBaud(m_config->speed()); - qDebug()<<"Serial telemetry running at "<speed(); - set.DataBits = DATA_8; - set.Parity = PAR_NONE; - set.StopBits = STOP_1; + set.BaudRate = stringToBaud(m_config->speed()); + qDebug() << "Serial telemetry running at " << m_config->speed(); + set.DataBits = DATA_8; + set.Parity = PAR_NONE; + set.StopBits = STOP_1; set.FlowControl = FLOW_OFF; set.Timeout_Millisec = 500; #ifdef Q_OS_WIN - serialHandle = new QextSerialPort(port.portName, set); + serialHandle = new QextSerialPort(port.portName, set); #else - serialHandle = new QextSerialPort(port.physName, set); + serialHandle = new QextSerialPort(port.physName, set); #endif - m_deviceOpened = true; + m_deviceOpened = true; return serialHandle; } } @@ -169,10 +165,10 @@ QIODevice *SerialConnection::openDevice(const QString &deviceName) void SerialConnection::closeDevice(const QString &deviceName) { Q_UNUSED(deviceName); - //we have to delete the serial connection we created - if (serialHandle){ + // we have to delete the serial connection we created + if (serialHandle) { serialHandle->deleteLater(); - serialHandle = NULL; + serialHandle = NULL; m_deviceOpened = false; } } @@ -189,7 +185,7 @@ QString SerialConnection::shortName() } /** - Tells the Serial plugin to stop polling for serial devices + Tells the Serial plugin to stop polling for serial devices */ void SerialConnection::suspendPolling() { @@ -197,7 +193,7 @@ void SerialConnection::suspendPolling() } /** - Tells the Serial plugin to resume polling for serial devices + Tells the Serial plugin to resume polling for serial devices */ void SerialConnection::resumePolling() { @@ -205,48 +201,49 @@ void SerialConnection::resumePolling() } BaudRateType SerialConnection::stringToBaud(QString str) -{ - if(str=="1200") +{ + if (str == "1200") { return BAUD1200; - if(str=="1800") + } + if (str == "1800") { return BAUD1800; - else if(str=="2400") + } else if (str == "2400") { return BAUD2400; - else if(str== "4800") + } else if (str == "4800") { return BAUD4800; - else if(str== "9600") + } else if (str == "9600") { return BAUD9600; - else if(str== "14400") + } else if (str == "14400") { return BAUD14400; - else if(str== "19200") + } else if (str == "19200") { return BAUD19200; - else if(str== "38400") + } else if (str == "38400") { return BAUD38400; - else if(str== "56000") + } else if (str == "56000") { return BAUD56000; - else if(str== "57600") + } else if (str == "57600") { return BAUD57600; - else if(str== "76800") + } else if (str == "76800") { return BAUD76800; - else if(str== "115200") + } else if (str == "115200") { return BAUD115200; - else if(str== "128000") + } else if (str == "128000") { return BAUD128000; - else if(str== "230400") + } else if (str == "230400") { return BAUD230400; - else if(str== "256000") + } else if (str == "256000") { return BAUD256000; - else if(str== "460800") + } else if (str == "460800") { return BAUD460800; - else if(str== "921600") + } else if (str == "921600") { return BAUD921600; - else + } else { return BAUD57600; + } } SerialPlugin::SerialPlugin() -{ -} +{} SerialPlugin::~SerialPlugin() { @@ -263,9 +260,9 @@ bool SerialPlugin::initialize(const QStringList &arguments, QString *errorString Q_UNUSED(arguments); Q_UNUSED(errorString); m_connection = new SerialConnection(); - //must manage this registration of child object ourselves - //if we use an autorelease here it causes the GCS to crash - //as it is deleting objects as the app closes... + // must manage this registration of child object ourselves + // if we use an autorelease here it causes the GCS to crash + // as it is deleting objects as the app closes... addObject(m_connection->Optionspage()); return true; } diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.h b/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.h index 388117ead..41fccdf2a 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.h +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.h @@ -10,25 +10,25 @@ * @brief Impliments serial connection to the flight hardware for Telemetry *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef SERIALPLUGIN_H #define SERIALPLUGIN_H -//#include "serial_global.h" +// #include "serial_global.h" #include #include #include "coreplugin/iconnection.h" @@ -42,13 +42,12 @@ class QextSerialEnumerator; class SerialConnection; /** -* Helper thread to check on new serial port connection/disconnection -* Some operating systems do not send device insertion events so -* for those we have to poll -*/ -//class SERIAL_EXPORT SerialEnumerationThread : public QThread -class SerialEnumerationThread : public QThread -{ + * Helper thread to check on new serial port connection/disconnection + * Some operating systems do not send device insertion events so + * for those we have to poll + */ +// class SERIAL_EXPORT SerialEnumerationThread : public QThread +class SerialEnumerationThread : public QThread { Q_OBJECT public: SerialEnumerationThread(SerialConnection *serial); @@ -66,14 +65,13 @@ protected: /** -* Define a connection via the IConnection interface -* Plugin will add a instance of this class to the pool, -* so the connection manager can use it. -*/ -//class SERIAL_EXPORT SerialConnection + * Define a connection via the IConnection interface + * Plugin will add a instance of this class to the pool, + * so the connection manager can use it. + */ +// class SERIAL_EXPORT SerialConnection class SerialConnection - : public Core::IConnection -{ + : public Core::IConnection { Q_OBJECT public: SerialConnection(); @@ -88,13 +86,22 @@ public: virtual void suspendPolling(); virtual void resumePolling(); - bool deviceOpened() {return m_deviceOpened;} - SerialPluginConfiguration * Config() const { return m_config; } - SerialPluginOptionsPage * Optionspage() const { return m_optionspage; } + bool deviceOpened() + { + return m_deviceOpened; + } + SerialPluginConfiguration *Config() const + { + return m_config; + } + SerialPluginOptionsPage *Optionspage() const + { + return m_optionspage; + } private: - QextSerialPort* serialHandle; + QextSerialPort *serialHandle; bool enablePolling; SerialPluginConfiguration *m_config; SerialPluginOptionsPage *m_optionspage; @@ -109,11 +116,9 @@ protected: }; - -//class SERIAL_EXPORT SerialPlugin +// class SERIAL_EXPORT SerialPlugin class SerialPlugin - : public ExtensionSystem::IPlugin -{ + : public ExtensionSystem::IPlugin { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.cpp b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.cpp index 8d8c9bbe6..54c896781 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.cpp @@ -46,8 +46,7 @@ SerialPluginConfiguration::SerialPluginConfiguration(QString classId, QSettings } SerialPluginConfiguration::~SerialPluginConfiguration() -{ -} +{} /** * Clones a configuration. @@ -56,6 +55,7 @@ SerialPluginConfiguration::~SerialPluginConfiguration() IUAVGadgetConfiguration *SerialPluginConfiguration::clone() { SerialPluginConfiguration *m = new SerialPluginConfiguration(this->classId()); + m->m_speed = m_speed; return m; } @@ -64,8 +64,9 @@ IUAVGadgetConfiguration *SerialPluginConfiguration::clone() * Saves a configuration. * */ -void SerialPluginConfiguration::saveConfig(QSettings* settings) const { - settings->setValue("speed", m_speed); +void SerialPluginConfiguration::saveConfig(QSettings *settings) const +{ + settings->setValue("speed", m_speed); } void SerialPluginConfiguration::restoresettings() @@ -75,8 +76,7 @@ void SerialPluginConfiguration::restoresettings() qDebug() << "SerialPluginConfiguration::restoresettings - speed" << str; if (str.isEmpty()) { m_speed = "57600"; - } - else { + } else { m_speed = str; } settings->endGroup(); diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.h b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.h index 41a1e2c5a..8335105ea 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.h +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.h @@ -35,15 +35,17 @@ using namespace Core; /* Despite its name, this is actually a generic analog Serial supporting up to two rotating "needle" indicators. - */ -class SerialPluginConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT + */ +class SerialPluginConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: explicit SerialPluginConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); virtual ~SerialPluginConfiguration(); - QString speed() { return m_speed; } + QString speed() + { + return m_speed; + } void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); void savesettings() const; @@ -54,8 +56,10 @@ private: QSettings *settings; public slots: - void setSpeed(QString speed) { m_speed = speed; } - + void setSpeed(QString speed) + { + m_speed = speed; + } }; #endif // SERIALPLUGINCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.cpp b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.cpp index bd46dbb47..b83f98b9f 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.cpp @@ -10,7 +10,7 @@ * @{ * @brief Impliments serial connection to the flight hardware for Telemetry *****************************************************************************/ - /* +/* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3 of the License, or @@ -32,50 +32,48 @@ #include "extensionsystem/pluginmanager.h" SerialPluginOptionsPage::SerialPluginOptionsPage(SerialPluginConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *SerialPluginOptionsPage::createPage(QWidget *parent) { - Q_UNUSED(parent); options_page = new Ui::SerialPluginOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget; - //main layout + // main layout options_page->setupUi(optionsPageWidget); QStringList allowedSpeeds; - allowedSpeeds<<"1200" -#ifdef Q_OS_UNIX - <<"1800" //POSIX ONLY -#endif - <<"2400" - <<"4800" - <<"9600" -#ifdef Q_OS_WIN - <<"14400" //WINDOWS ONLY -#endif - <<"19200" - <<"38400" -#ifdef Q_OS_WIN - <<"56000" //WINDOWS ONLY -#endif - <<"57600" -#ifdef Q_OS_UNIX - <<"76800" //POSIX ONLY -#endif - <<"115200" -#ifdef Q_OS_WIN - <<"128000" //WINDOWS ONLY - <<"230400" //WINDOWS ONLY - <<"256000" //WINDOWS ONLY - <<"460800" //WINDOWS ONLY - <<"921600" //WINDOWS ONLY -#endif - ; + allowedSpeeds << "1200" + #ifdef Q_OS_UNIX + << "1800" // POSIX ONLY + #endif + << "2400" + << "4800" + << "9600" + #ifdef Q_OS_WIN + << "14400" // WINDOWS ONLY + #endif + << "19200" + << "38400" + #ifdef Q_OS_WIN + << "56000" // WINDOWS ONLY + #endif + << "57600" + #ifdef Q_OS_UNIX + << "76800" // POSIX ONLY + #endif + << "115200" + #ifdef Q_OS_WIN + << "128000" // WINDOWS ONLY + << "230400" // WINDOWS ONLY + << "256000" // WINDOWS ONLY + << "460800" // WINDOWS ONLY + << "921600" // WINDOWS ONLY + #endif + ; options_page->cb_speed->addItems(allowedSpeeds); @@ -96,7 +94,6 @@ void SerialPluginOptionsPage::apply() } - void SerialPluginOptionsPage::finish() { delete options_page; diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.h b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.h index 6ce23b743..1a0475f48 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.h +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginoptionspage.h @@ -10,7 +10,7 @@ * @{ * @brief Impliments serial connection to the flight hardware for Telemetry *****************************************************************************/ - /* +/* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3 of the License, or @@ -42,21 +42,32 @@ class IUAVpluginConfiguration; class SerialPluginConfiguration; namespace Ui { - class SerialPluginOptionsPage; +class SerialPluginOptionsPage; } using namespace Core; -class SerialPluginOptionsPage : public IOptionsPage -{ -Q_OBJECT +class SerialPluginOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit SerialPluginOptionsPage(SerialPluginConfiguration *config, QObject *parent = 0); - QString id() const { return QLatin1String("settings"); } - QString trName() const { return tr("settings"); } - QString category() const { return "Serial Telemetry"; } - QString trCategory() const { return "Serial Telemetry"; } + QString id() const + { + return QLatin1String("settings"); + } + QString trName() const + { + return tr("settings"); + } + QString category() const + { + return "Serial Telemetry"; + } + QString trCategory() const + { + return "Serial Telemetry"; + } QWidget *createPage(QWidget *parent); void apply(); void finish(); @@ -64,8 +75,6 @@ public: private: Ui::SerialPluginOptionsPage *options_page; SerialPluginConfiguration *m_config; - - }; #endif // SERIALpluginOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp index bfba40313..1b3f64804 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp @@ -37,31 +37,29 @@ BiasCalibrationUtil::BiasCalibrationUtil(long measurementCount, long measurementRate) : QObject(), m_isMeasuring(false), m_accelMeasurementCount(measurementCount), m_gyroMeasurementCount(measurementCount), m_accelMeasurementRate(measurementRate), m_gyroMeasurementRate(measurementRate) -{ -} +{} BiasCalibrationUtil::BiasCalibrationUtil(long accelMeasurementCount, long accelMeasurementRate, - long gyroMeasurementCount, long gyroMeasurementRate) : QObject(), + long gyroMeasurementCount, long gyroMeasurementRate) : QObject(), m_isMeasuring(false), m_accelMeasurementCount(accelMeasurementCount), m_gyroMeasurementCount(gyroMeasurementCount), m_accelMeasurementRate(accelMeasurementRate), m_gyroMeasurementRate(gyroMeasurementRate) -{ -} +{} void BiasCalibrationUtil::start() { - if(!m_isMeasuring) { + if (!m_isMeasuring) { startMeasurement(); // Set up timeout timer connect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout())); - //Set timeout to max time of measurement + 10 secs + // Set timeout to max time of measurement + 10 secs m_timeoutTimer.start(std::max(m_accelMeasurementCount * m_accelMeasurementRate, m_gyroMeasurementCount * m_gyroMeasurementRate) + 10000); } } void BiasCalibrationUtil::abort() { - if(m_isMeasuring) { + if (m_isMeasuring) { stopMeasurement(); } } @@ -70,12 +68,12 @@ void BiasCalibrationUtil::gyroMeasurementsUpdated(UAVObject *obj) { Q_UNUSED(obj); - if(m_receivedGyroUpdates < m_gyroMeasurementCount) { + if (m_receivedGyroUpdates < m_gyroMeasurementCount) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * uavObjectManager = pm->getObject(); + UAVObjectManager *uavObjectManager = pm->getObject(); Q_ASSERT(uavObjectManager); - Gyros * gyros = Gyros::GetInstance(uavObjectManager); + Gyros *gyros = Gyros::GetInstance(uavObjectManager); Gyros::DataFields gyrosData = gyros->getData(); m_gyroX += gyrosData.x; @@ -84,9 +82,8 @@ void BiasCalibrationUtil::gyroMeasurementsUpdated(UAVObject *obj) m_receivedGyroUpdates++; emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount); - } - else if (m_receivedAccelUpdates >= m_accelMeasurementCount && - m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { + } else if (m_receivedAccelUpdates >= m_accelMeasurementCount && + m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { stopMeasurement(); } } @@ -95,12 +92,12 @@ void BiasCalibrationUtil::accelMeasurementsUpdated(UAVObject *obj) { Q_UNUSED(obj); - if(m_receivedAccelUpdates < m_accelMeasurementCount) { + if (m_receivedAccelUpdates < m_accelMeasurementCount) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * uavObjectManager = pm->getObject(); + UAVObjectManager *uavObjectManager = pm->getObject(); Q_ASSERT(uavObjectManager); - Accels * accels = Accels::GetInstance(uavObjectManager); + Accels *accels = Accels::GetInstance(uavObjectManager); Accels::DataFields accelsData = accels->getData(); m_accelerometerX += accelsData.x; @@ -109,9 +106,8 @@ void BiasCalibrationUtil::accelMeasurementsUpdated(UAVObject *obj) m_receivedAccelUpdates++; emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount); - } - else if (m_receivedAccelUpdates >= m_accelMeasurementCount && - m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { + } else if (m_receivedAccelUpdates >= m_accelMeasurementCount && + m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { stopMeasurement(); } } @@ -138,7 +134,7 @@ void BiasCalibrationUtil::startMeasurement() m_gyroZ = 0; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * uavObjectManager = pm->getObject(); + UAVObjectManager *uavObjectManager = pm->getObject(); Q_ASSERT(uavObjectManager); // Disable gyro bias correction to see raw data @@ -148,7 +144,7 @@ void BiasCalibrationUtil::startMeasurement() // Set up to receive updates for accels UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager); - connect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelMeasurementsUpdated(UAVObject*))); + connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(accelMeasurementsUpdated(UAVObject *))); // Set update period for accels m_previousAccelMetaData = uavObject->getMetadata(); @@ -159,7 +155,7 @@ void BiasCalibrationUtil::startMeasurement() // Set up to receive updates from gyros uavObject = Gyros::GetInstance(uavObjectManager); - connect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(gyroMeasurementsUpdated(UAVObject*))); + connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroMeasurementsUpdated(UAVObject *))); // Set update period for gyros m_previousGyroMetaData = uavObject->getMetadata(); @@ -175,22 +171,22 @@ void BiasCalibrationUtil::stopMeasurement() m_isMeasuring = false; - //Stop timeout timer + // Stop timeout timer m_timeoutTimer.stop(); disconnect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout())); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * uavObjectManager = pm->getObject(); + UAVObjectManager *uavObjectManager = pm->getObject(); Q_ASSERT(uavObjectManager); // Stop listening for updates from accels UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager); - disconnect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelMeasurementsUpdated(UAVObject*))); + disconnect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(accelMeasurementsUpdated(UAVObject *))); uavObject->setMetadata(m_previousAccelMetaData); // Stop listening for updates from gyros uavObject = Gyros::GetInstance(uavObjectManager); - disconnect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(gyroMeasurementsUpdated(UAVObject*))); + disconnect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroMeasurementsUpdated(UAVObject *))); uavObject->setMetadata(m_previousGyroMetaData); // Enable gyro bias correction again diff --git a/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.h b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.h index 980305eeb..c5e0af4ed 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.h @@ -34,13 +34,12 @@ #include "uavobject.h" #include "vehicleconfigurationsource.h" -class BiasCalibrationUtil : public QObject -{ +class BiasCalibrationUtil : public QObject { Q_OBJECT public: explicit BiasCalibrationUtil(long measurementCount, long measurementRate); explicit BiasCalibrationUtil(long accelMeasurementCount, long accelMeasurementRate, - long gyroMeasurementCount, long gyroMeasurementRate); + long gyroMeasurementCount, long gyroMeasurementRate); signals: void progress(long current, long total); @@ -52,8 +51,8 @@ public slots: void abort(); private slots: - void gyroMeasurementsUpdated(UAVObject * obj); - void accelMeasurementsUpdated(UAVObject * obj); + void gyroMeasurementsUpdated(UAVObject *obj); + void accelMeasurementsUpdated(UAVObject *obj); void timeout(); private: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp index 9f1064c39..4a19eb4cf 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp @@ -31,7 +31,7 @@ #include "connectiondiagram.h" #include "ui_connectiondiagram.h" -ConnectionDiagram::ConnectionDiagram(QWidget *parent, VehicleConfigurationSource* configSource) : +ConnectionDiagram::ConnectionDiagram(QWidget *parent, VehicleConfigurationSource *configSource) : QDialog(parent), ui(new Ui::ConnectionDiagram), m_configSource(configSource), m_background(0) { ui->setupUi(this); @@ -47,12 +47,14 @@ ConnectionDiagram::~ConnectionDiagram() void ConnectionDiagram::resizeEvent(QResizeEvent *event) { QWidget::resizeEvent(event); + ui->connectionDiagram->fitInView(m_background, Qt::KeepAspectRatio); } -void ConnectionDiagram::showEvent(QShowEvent * event) +void ConnectionDiagram::showEvent(QShowEvent *event) { QWidget::showEvent(event); + ui->connectionDiagram->fitInView(m_background, Qt::KeepAspectRatio); } @@ -60,90 +62,85 @@ void ConnectionDiagram::setupGraphicsScene() { m_renderer = new QSvgRenderer(); if (QFile::exists(QString(":/setupwizard/resources/connection-diagrams.svg")) && - m_renderer->load(QString(":/setupwizard/resources/connection-diagrams.svg")) && - m_renderer->isValid()) - { + m_renderer->load(QString(":/setupwizard/resources/connection-diagrams.svg")) && + m_renderer->isValid()) { m_scene = new QGraphicsScene(this); ui->connectionDiagram->setScene(m_scene); - //ui->connectionDiagram->setViewportUpdateMode(QGraphicsView::FullViewportUpdate); + // ui->connectionDiagram->setViewportUpdateMode(QGraphicsView::FullViewportUpdate); m_background = new QGraphicsSvgItem(); m_background->setSharedRenderer(m_renderer); m_background->setElementId("background"); m_background->setOpacity(0); - //m_background->setFlags(QGraphicsItem::ItemClipsToShape); + // m_background->setFlags(QGraphicsItem::ItemClipsToShape); m_background->setZValue(-1); m_scene->addItem(m_background); QList elementsToShow; - switch(m_configSource->getControllerType()) - { - case VehicleConfigurationSource::CONTROLLER_CC: - case VehicleConfigurationSource::CONTROLLER_CC3D: - elementsToShow << "controller-cc"; - break; - case VehicleConfigurationSource::CONTROLLER_REVO: - elementsToShow << "controller-revo"; - break; - case VehicleConfigurationSource::CONTROLLER_OPLINK: - default: - elementsToShow << "controller-cc"; - break; + switch (m_configSource->getControllerType()) { + case VehicleConfigurationSource::CONTROLLER_CC: + case VehicleConfigurationSource::CONTROLLER_CC3D: + elementsToShow << "controller-cc"; + break; + case VehicleConfigurationSource::CONTROLLER_REVO: + elementsToShow << "controller-revo"; + break; + case VehicleConfigurationSource::CONTROLLER_OPLINK: + default: + elementsToShow << "controller-cc"; + break; } - switch (m_configSource->getVehicleType()) - { - case VehicleConfigurationSource::VEHICLE_MULTI: - switch (m_configSource->getVehicleSubType()) - { - case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: - elementsToShow << "tri"; - break; - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: - elementsToShow << "quad-x"; - break; - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: - elementsToShow << "quad-p"; - break; - case VehicleConfigurationSource::MULTI_ROTOR_HEXA: - elementsToShow << "hexa"; - break; - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: - elementsToShow << "hexa-y"; - break; - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: - elementsToShow << "hexa-h"; - break; - default: - break; - } + switch (m_configSource->getVehicleType()) { + case VehicleConfigurationSource::VEHICLE_MULTI: + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: + elementsToShow << "tri"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: + elementsToShow << "quad-x"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: + elementsToShow << "quad-p"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA: + elementsToShow << "hexa"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: + elementsToShow << "hexa-y"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: + elementsToShow << "hexa-h"; break; - case VehicleConfigurationSource::VEHICLE_FIXEDWING: - case VehicleConfigurationSource::VEHICLE_HELI: - case VehicleConfigurationSource::VEHICLE_SURFACE: default: break; + } + break; + case VehicleConfigurationSource::VEHICLE_FIXEDWING: + case VehicleConfigurationSource::VEHICLE_HELI: + case VehicleConfigurationSource::VEHICLE_SURFACE: + default: + break; } - switch (m_configSource->getInputType()) - { - case VehicleConfigurationSource::INPUT_PWM: - elementsToShow << "pwm" ; - break; - case VehicleConfigurationSource::INPUT_PPM: - elementsToShow << "ppm"; - break; - case VehicleConfigurationSource::INPUT_SBUS: - elementsToShow << "sbus"; - break; - case VehicleConfigurationSource::INPUT_DSMX10: - case VehicleConfigurationSource::INPUT_DSMX11: - case VehicleConfigurationSource::INPUT_DSM2: - elementsToShow << "satellite"; - break; - default: - break; + switch (m_configSource->getInputType()) { + case VehicleConfigurationSource::INPUT_PWM: + elementsToShow << "pwm"; + break; + case VehicleConfigurationSource::INPUT_PPM: + elementsToShow << "ppm"; + break; + case VehicleConfigurationSource::INPUT_SBUS: + elementsToShow << "sbus"; + break; + case VehicleConfigurationSource::INPUT_DSMX10: + case VehicleConfigurationSource::INPUT_DSMX11: + case VehicleConfigurationSource::INPUT_DSM2: + elementsToShow << "satellite"; + break; + default: + break; } setupGraphicsSceneItems(elementsToShow); @@ -161,24 +158,23 @@ void ConnectionDiagram::setupGraphicsSceneItems(QList elementsToShow) QRectF backgBounds = m_renderer->boundsOnElement("background"); foreach(QString elementId, elementsToShow) { - if(m_renderer->elementExists(elementId)) { - QGraphicsSvgItem* element = new QGraphicsSvgItem(); + if (m_renderer->elementExists(elementId)) { + QGraphicsSvgItem *element = new QGraphicsSvgItem(); element->setSharedRenderer(m_renderer); element->setElementId(elementId); element->setZValue(z++); element->setOpacity(1.0); QMatrix matrix = m_renderer->matrixForElement(elementId); - QRectF orig = matrix.mapRect(m_renderer->boundsOnElement(elementId)); - element->setPos(orig.x(),orig.y()); + QRectF orig = matrix.mapRect(m_renderer->boundsOnElement(elementId)); + element->setPos(orig.x(), orig.y()); - //QRectF orig = m_renderer->boundsOnElement(elementId); - //element->setPos(orig.x() - backgBounds.x(), orig.y() - backgBounds.y()); + // QRectF orig = m_renderer->boundsOnElement(elementId); + // element->setPos(orig.x() - backgBounds.x(), orig.y() - backgBounds.y()); m_scene->addItem(element); qDebug() << "Adding " << elementId << " to scene at " << element->pos(); - } - else { + } else { qDebug() << "Element with id: " << elementId << " not found."; } } @@ -187,11 +183,12 @@ void ConnectionDiagram::setupGraphicsSceneItems(QList elementsToShow) void ConnectionDiagram::on_saveButton_clicked() { QImage image(2200, 1100, QImage::Format_ARGB32); + image.fill(0); QPainter painter(&image); m_scene->render(&painter); QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "", tr("Images (*.png *.xpm *.jpg)")); - if(!fileName.isEmpty()) { + if (!fileName.isEmpty()) { image.save(fileName); } } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.h b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.h index e03ed6b0e..8d7a0574d 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.h @@ -40,20 +40,19 @@ namespace Ui { class ConnectionDiagram; } -class ConnectionDiagram : public QDialog -{ +class ConnectionDiagram : public QDialog { Q_OBJECT - + public: explicit ConnectionDiagram(QWidget *parent, VehicleConfigurationSource *configSource); ~ConnectionDiagram(); - + private: Ui::ConnectionDiagram *ui; VehicleConfigurationSource *m_configSource; QSvgRenderer *m_renderer; - QGraphicsSvgItem* m_background; + QGraphicsSvgItem *m_background; QGraphicsScene *m_scene; void setupGraphicsScene(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp b/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp index 1e7e48959..e75b95b2b 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp @@ -35,6 +35,7 @@ OutputCalibrationUtil::OutputCalibrationUtil(QObject *parent) : QObject(parent), m_outputChannel(-1), m_safeValue(1000) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + m_uavObjectManager = pm->getObject(); Q_ASSERT(m_uavObjectManager); } @@ -46,31 +47,30 @@ OutputCalibrationUtil::~OutputCalibrationUtil() void OutputCalibrationUtil::startChannelOutput(quint16 channel, quint16 safeValue) { - if(m_outputChannel < 0 && channel < ActuatorCommand::CHANNEL_NUMELEM) - { - //Start output... + if (m_outputChannel < 0 && channel < ActuatorCommand::CHANNEL_NUMELEM) { + // Start output... m_outputChannel = channel; - m_safeValue = safeValue; + m_safeValue = safeValue; qDebug() << "Starting output for channel " << m_outputChannel << "..."; ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager); Q_ASSERT(actuatorCommand); - UAVObject::Metadata metaData = actuatorCommand->getMetadata(); + UAVObject::Metadata metaData = actuatorCommand->getMetadata(); m_savedActuatorCommandMetadata = metaData; - //Store current data for later restore - m_savedActuatorCommandData = actuatorCommand->getData(); + // Store current data for later restore + m_savedActuatorCommandData = actuatorCommand->getData(); - //Enable actuator control from GCS... - //Store current metadata for later restore + // Enable actuator control from GCS... + // Store current metadata for later restore UAVObject::SetFlightAccess(metaData, UAVObject::ACCESS_READONLY); UAVObject::SetFlightTelemetryUpdateMode(metaData, UAVObject::UPDATEMODE_ONCHANGE); UAVObject::SetGcsTelemetryAcked(metaData, false); UAVObject::SetGcsTelemetryUpdateMode(metaData, UAVObject::UPDATEMODE_ONCHANGE); metaData.gcsTelemetryUpdatePeriod = 100; - //Apply changes + // Apply changes actuatorCommand->setMetadata(metaData); actuatorCommand->updated(); @@ -80,17 +80,16 @@ void OutputCalibrationUtil::startChannelOutput(quint16 channel, quint16 safeValu void OutputCalibrationUtil::stopChannelOutput() { - if(m_outputChannel >= 0) - { + if (m_outputChannel >= 0) { qDebug() << "Stopping output for channel " << m_outputChannel << "..."; - //Stop output... + // Stop output... setChannelOutputValue(m_safeValue); qDebug() << "Settings output for channel " << m_outputChannel << " to " << m_safeValue; // Restore metadata to what it was before ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager); Q_ASSERT(actuatorCommand); - //actuatorCommand->setData(m_savedActuatorCommandData); + // actuatorCommand->setData(m_savedActuatorCommandData); actuatorCommand->setMetadata(m_savedActuatorCommandMetadata); actuatorCommand->updated(); @@ -102,9 +101,8 @@ void OutputCalibrationUtil::stopChannelOutput() void OutputCalibrationUtil::setChannelOutputValue(quint16 value) { - if(m_outputChannel >= 0) - { - //Set output value + if (m_outputChannel >= 0) { + // Set output value qDebug() << "Setting output value for channel " << m_outputChannel << " to " << value << "."; ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager); Q_ASSERT(actuatorCommand); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.h b/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.h index dc9fc7fc8..4867e5660 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.h @@ -36,15 +36,14 @@ #include "actuatorcommand.h" -class OutputCalibrationUtil : public QObject -{ +class OutputCalibrationUtil : public QObject { Q_OBJECT public: explicit OutputCalibrationUtil(QObject *parent = 0); ~OutputCalibrationUtil(); - + signals: - + public slots: void startChannelOutput(quint16 channel, quint16 safeValue); void stopChannelOutput(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.cpp index 2e5ed82da..d7ea0e743 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.cpp @@ -27,9 +27,9 @@ #include "abstractwizardpage.h" -AbstractWizardPage::AbstractWizardPage(SetupWizard* wizard, QWidget *parent) : +AbstractWizardPage::AbstractWizardPage(SetupWizard *wizard, QWidget *parent) : QWizardPage(parent) { m_wizard = wizard; - //setFixedSize(600, 400); + // setFixedSize(600, 400); } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.h index 072d619e7..904f1771b 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.h @@ -31,8 +31,7 @@ #include #include "setupwizard.h" -class AbstractWizardPage : public QWizardPage -{ +class AbstractWizardPage : public QWizardPage { Q_OBJECT protected: explicit AbstractWizardPage(SetupWizard *wizard, QWidget *parent = 0); @@ -41,8 +40,10 @@ private: SetupWizard *m_wizard; public: - SetupWizard* getWizard() { return m_wizard; } - + SetupWizard *getWizard() + { + return m_wizard; + } }; #endif // ABSTRACTWIZARDPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp index 6542940f1..6506fde34 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.cpp @@ -13,11 +13,11 @@ AutoUpdatePage::AutoUpdatePage(SetupWizard *wizard, QWidget *parent) : ui->setupUi(this); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Q_ASSERT(pm); - UploaderGadgetFactory *uploader = pm->getObject(); + UploaderGadgetFactory *uploader = pm->getObject(); Q_ASSERT(uploader); connect(ui->startUpdate, SIGNAL(clicked()), this, SLOT(disableButtons())); connect(ui->startUpdate, SIGNAL(clicked()), uploader, SIGNAL(autoUpdate())); - connect(uploader, SIGNAL(autoUpdateSignal(uploader::AutoUpdateStep,QVariant)), this, SLOT(updateStatus(uploader::AutoUpdateStep, QVariant))); + connect(uploader, SIGNAL(autoUpdateSignal(uploader::AutoUpdateStep, QVariant)), this, SLOT(updateStatus(uploader::AutoUpdateStep, QVariant))); } AutoUpdatePage::~AutoUpdatePage() @@ -37,8 +37,7 @@ void AutoUpdatePage::enableButtons(bool enable = false) void AutoUpdatePage::updateStatus(uploader::AutoUpdateStep status, QVariant value) { - switch(status) - { + switch (status) { case uploader::WAITING_DISCONNECT: getWizard()->setWindowFlags(getWizard()->windowFlags() & ~Qt::WindowStaysOnTopHint); disableButtons(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.h index f76873a9f..a8c4a402d 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.h @@ -39,8 +39,7 @@ namespace Ui { class AutoUpdatePage; } -class AutoUpdatePage : public AbstractWizardPage -{ +class AutoUpdatePage : public AbstractWizardPage { Q_OBJECT public: @@ -48,13 +47,15 @@ public: ~AutoUpdatePage(); private slots: - void updateStatus(uploader::AutoUpdateStep ,QVariant); - void disableButtons(){ enableButtons(false); } + void updateStatus(uploader::AutoUpdateStep, QVariant); + void disableButtons() + { + enableButtons(false); + } void enableButtons(bool enable); private: Ui::AutoUpdatePage *ui; - }; #endif // AUTOUPDATEPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.cpp index 3fb5a3ca6..e5b2ca62f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.cpp @@ -33,7 +33,7 @@ BiasCalibrationPage::BiasCalibrationPage(SetupWizard *wizard, QWidget *parent) : AbstractWizardPage(wizard, parent), - ui(new Ui::BiasCalibrationPage), m_calibrationUtil(0) + ui(new Ui::BiasCalibrationPage), m_calibrationUtil(0) { ui->setupUi(this); connect(ui->levelButton, SIGNAL(clicked()), this, SLOT(performCalibration())); @@ -41,7 +41,7 @@ BiasCalibrationPage::BiasCalibrationPage(SetupWizard *wizard, QWidget *parent) : BiasCalibrationPage::~BiasCalibrationPage() { - if(m_calibrationUtil) { + if (m_calibrationUtil) { delete m_calibrationUtil; } delete ui; @@ -69,7 +69,7 @@ void BiasCalibrationPage::enableButtons(bool enable) void BiasCalibrationPage::performCalibration() { - if(!getWizard()->getConnectionManager()->isConnected()) { + if (!getWizard()->getConnectionManager()->isConnected()) { QMessageBox msgBox; msgBox.setText(tr("An OpenPilot controller must be connected to your computer to perform bias " "calculations.\nPlease connect your OpenPilot controller to your computer and try again.")); @@ -83,12 +83,11 @@ void BiasCalibrationPage::performCalibration() ui->progressLabel->setText(QString(tr("Retrieving data..."))); - if(!m_calibrationUtil) - { + if (!m_calibrationUtil) { m_calibrationUtil = new BiasCalibrationUtil(BIAS_CYCLES, BIAS_RATE); } - connect(m_calibrationUtil, SIGNAL(progress(long,long)), this, SLOT(calibrationProgress(long,long))); + connect(m_calibrationUtil, SIGNAL(progress(long, long)), this, SLOT(calibrationProgress(long, long))); connect(m_calibrationUtil, SIGNAL(done(accelGyroBias)), this, SLOT(calibrationDone(accelGyroBias))); connect(m_calibrationUtil, SIGNAL(timeout(QString)), this, SLOT(calibrationTimeout(QString))); @@ -97,10 +96,10 @@ void BiasCalibrationPage::performCalibration() void BiasCalibrationPage::calibrationProgress(long current, long total) { - if(ui->levellinProgressBar->maximum() != (int)total) { + if (ui->levellinProgressBar->maximum() != (int)total) { ui->levellinProgressBar->setMaximum((int)total); } - if(ui->levellinProgressBar->value() != (int)current) { + if (ui->levellinProgressBar->value() != (int)current) { ui->levellinProgressBar->setValue((int)current); } } @@ -125,9 +124,8 @@ void BiasCalibrationPage::calibrationTimeout(QString message) void BiasCalibrationPage::stopCalibration() { - if(m_calibrationUtil) - { - disconnect(m_calibrationUtil, SIGNAL(progress(long,long)), this, SLOT(calibrationProgress(long,long))); + if (m_calibrationUtil) { + disconnect(m_calibrationUtil, SIGNAL(progress(long, long)), this, SLOT(calibrationProgress(long, long))); disconnect(m_calibrationUtil, SIGNAL(done(accelGyroBias)), this, SLOT(calibrationDone(accelGyroBias))); disconnect(m_calibrationUtil, SIGNAL(timeout(QString)), this, SLOT(calibrationTimeout(QString))); ui->progressLabel->setText(QString(tr("Done!"))); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.h index f34c48abe..002edf51f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.h @@ -35,8 +35,7 @@ namespace Ui { class BiasCalibrationPage; } -class BiasCalibrationPage : public AbstractWizardPage -{ +class BiasCalibrationPage : public AbstractWizardPage { Q_OBJECT public: @@ -53,7 +52,7 @@ private slots: private: static const int BIAS_CYCLES = 200; - static const int BIAS_RATE = 50; + static const int BIAS_RATE = 50; Ui::BiasCalibrationPage *ui; BiasCalibrationUtil *m_calibrationUtil; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp index 595928931..5c67612dc 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp @@ -62,11 +62,10 @@ ControllerPage::~ControllerPage() void ControllerPage::initializePage() { - if(anyControllerConnected()) { + if (anyControllerConnected()) { SetupWizard::CONTROLLER_TYPE type = getControllerType(); setControllerType(type); - } - else { + } else { setControllerType(SetupWizard::CONTROLLER_UNKNOWN); } emit completeChanged(); @@ -75,7 +74,7 @@ void ControllerPage::initializePage() bool ControllerPage::isComplete() const { return m_telemtryManager->isConnected() && ui->boardTypeCombo->currentIndex() > 0 && - m_connectionManager->getCurrentDevice().getConName().startsWith("USB:", Qt::CaseInsensitive); + m_connectionManager->getCurrentDevice().getConName().startsWith("USB:", Qt::CaseInsensitive); } bool ControllerPage::validatePage() @@ -92,18 +91,22 @@ bool ControllerPage::anyControllerConnected() SetupWizard::CONTROLLER_TYPE ControllerPage::getControllerType() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectUtilManager* utilMngr = pm->getObject(); + UAVObjectUtilManager *utilMngr = pm->getObject(); int id = utilMngr->getBoardModel(); switch (id) { case 0x0301: return SetupWizard::CONTROLLER_OPLINK; + case 0x0401: return SetupWizard::CONTROLLER_CC; + case 0x0402: return SetupWizard::CONTROLLER_CC3D; + case 0x0903: return SetupWizard::CONTROLLER_REVO; + default: return SetupWizard::CONTROLLER_UNKNOWN; } @@ -118,6 +121,7 @@ void ControllerPage::setupDeviceList() void ControllerPage::setupBoardTypes() { QVariant v(0); + ui->boardTypeCombo->addItem(tr(""), SetupWizard::CONTROLLER_UNKNOWN); ui->boardTypeCombo->addItem(tr("OpenPilot CopterControl"), SetupWizard::CONTROLLER_CC); ui->boardTypeCombo->addItem(tr("OpenPilot CopterControl 3D"), SetupWizard::CONTROLLER_CC3D); @@ -127,8 +131,8 @@ void ControllerPage::setupBoardTypes() void ControllerPage::setControllerType(SetupWizard::CONTROLLER_TYPE type) { - for(int i = 0; i < ui->boardTypeCombo->count(); ++i) { - if(ui->boardTypeCombo->itemData(i) == type) { + for (int i = 0; i < ui->boardTypeCombo->count(); ++i) { + if (ui->boardTypeCombo->itemData(i) == type) { ui->boardTypeCombo->setCurrentIndex(i); break; } @@ -139,7 +143,7 @@ void ControllerPage::devicesChanged(QLinkedList devices) { // Get the selected item before the update if any QString currSelectedDeviceName = ui->deviceCombo->currentIndex() != -1 ? - ui->deviceCombo->itemData(ui->deviceCombo->currentIndex(), Qt::ToolTipRole).toString() : ""; + ui->deviceCombo->itemData(ui->deviceCombo->currentIndex(), Qt::ToolTipRole).toString() : ""; // Clear the box ui->deviceCombo->clear(); @@ -148,36 +152,35 @@ void ControllerPage::devicesChanged(QLinkedList devices) int i = 0; // Loop and fill the combo with items from connectionmanager - foreach (Core::DevListItem deviceItem, devices) - { + foreach(Core::DevListItem deviceItem, devices) { ui->deviceCombo->addItem(deviceItem.getConName()); QString deviceName = (const QString)deviceItem.getConName(); ui->deviceCombo->setItemData(ui->deviceCombo->count() - 1, deviceName, Qt::ToolTipRole); - if(!deviceName.startsWith("USB:", Qt::CaseInsensitive)) { + if (!deviceName.startsWith("USB:", Qt::CaseInsensitive)) { ui->deviceCombo->setItemData(ui->deviceCombo->count() - 1, QVariant(0), Qt::UserRole - 1); } - if(currSelectedDeviceName != "" && currSelectedDeviceName == deviceName) { + if (currSelectedDeviceName != "" && currSelectedDeviceName == deviceName) { indexOfSelectedItem = i; } i++; } // Re select the item that was selected before if any - if(indexOfSelectedItem != -1) { + if (indexOfSelectedItem != -1) { ui->deviceCombo->setCurrentIndex(indexOfSelectedItem); } - //connectionStatusChanged(); + // connectionStatusChanged(); } void ControllerPage::connectionStatusChanged() { - if(m_connectionManager->isConnected()) { + if (m_connectionManager->isConnected()) { ui->deviceCombo->setEnabled(false); ui->connectButton->setText(tr("Disconnect")); ui->boardTypeCombo->setEnabled(false); QString connectedDeviceName = m_connectionManager->getCurrentDevice().getConName(); - for(int i = 0; i < ui->deviceCombo->count(); ++i) { - if(connectedDeviceName == ui->deviceCombo->itemData(i, Qt::ToolTipRole).toString()) { + for (int i = 0; i < ui->deviceCombo->count(); ++i) { + if (connectedDeviceName == ui->deviceCombo->itemData(i, Qt::ToolTipRole).toString()) { ui->deviceCombo->setCurrentIndex(i); break; } @@ -186,8 +189,7 @@ void ControllerPage::connectionStatusChanged() SetupWizard::CONTROLLER_TYPE type = getControllerType(); setControllerType(type); qDebug() << "Connection status changed: Connected, controller type: " << getControllerType(); - } - else { + } else { ui->deviceCombo->setEnabled(true); ui->connectButton->setText(tr("Connect")); ui->boardTypeCombo->setEnabled(false); @@ -200,10 +202,9 @@ void ControllerPage::connectionStatusChanged() void ControllerPage::connectDisconnect() { - if(m_connectionManager->isConnected()) { + if (m_connectionManager->isConnected()) { m_connectionManager->disconnectDevice(); - } - else { + } else { m_connectionManager->connectDevice(m_connectionManager->findDevice(ui->deviceCombo->itemData(ui->deviceCombo->currentIndex(), Qt::ToolTipRole).toString())); } emit completeChanged(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h index 9370f4cd4..5bae29d6b 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h @@ -38,17 +38,16 @@ namespace Ui { class ControllerPage; } -class ControllerPage : public AbstractWizardPage -{ +class ControllerPage : public AbstractWizardPage { Q_OBJECT - + public: explicit ControllerPage(SetupWizard *wizard, QWidget *parent = 0); ~ControllerPage(); void initializePage(); bool isComplete() const; bool validatePage(); - + private: Ui::ControllerPage *ui; bool anyControllerConnected(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.cpp index 34228c401..48b08fb75 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.cpp @@ -48,14 +48,13 @@ EndPage::~EndPage() void EndPage::openInputWizard() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - ConfigGadgetFactory* configGadgetFactory = pm->getObject(); + ConfigGadgetFactory *configGadgetFactory = pm->getObject(); - if(configGadgetFactory) { - //Core::ModeManager::instance()->activateModeByWorkspaceName("Configuration"); + if (configGadgetFactory) { + // Core::ModeManager::instance()->activateModeByWorkspaceName("Configuration"); getWizard()->close(); configGadgetFactory->startInputWizard(); - } - else { + } else { QMessageBox msgBox; msgBox.setText(tr("Unable to open Input Wizard since the Config Plugin is not\nloaded in the current workspace.")); msgBox.setStandardButtons(QMessageBox::Ok); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.h index dfbad7942..92bfc194f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.h @@ -34,14 +34,13 @@ namespace Ui { class EndPage; } -class EndPage : public AbstractWizardPage -{ +class EndPage : public AbstractWizardPage { Q_OBJECT - + public: explicit EndPage(SetupWizard *wizard, QWidget *parent = 0); ~EndPage(); - + private slots: void openInputWizard(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h index 697721a9e..6a99ef2fe 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h @@ -34,14 +34,13 @@ namespace Ui { class FixedWingPage; } -class FixedWingPage : public AbstractWizardPage -{ +class FixedWingPage : public AbstractWizardPage { Q_OBJECT - + public: explicit FixedWingPage(SetupWizard *wizard, QWidget *parent = 0); ~FixedWingPage(); - + private: Ui::FixedWingPage *ui; }; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.h index 2b2b5b894..94727da41 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.h @@ -34,14 +34,13 @@ namespace Ui { class HeliPage; } -class HeliPage : public AbstractWizardPage -{ +class HeliPage : public AbstractWizardPage { Q_OBJECT - + public: explicit HeliPage(SetupWizard *wizard, QWidget *parent = 0); ~HeliPage(); - + private: Ui::HeliPage *ui; }; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp index bfa83ba7e..5515a31dd 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp @@ -33,7 +33,7 @@ #include "hwsettings.h" InputPage::InputPage(SetupWizard *wizard, QWidget *parent) : - AbstractWizardPage(wizard, parent), + AbstractWizardPage(wizard, parent), ui(new Ui::InputPage) { @@ -47,19 +47,15 @@ InputPage::~InputPage() bool InputPage::validatePage() { - if(ui->pwmButton->isChecked()) { + if (ui->pwmButton->isChecked()) { getWizard()->setInputType(SetupWizard::INPUT_PWM); - } - else if(ui->ppmButton->isChecked()) { + } else if (ui->ppmButton->isChecked()) { getWizard()->setInputType(SetupWizard::INPUT_PPM); - } - else if(ui->sbusButton->isChecked()) { + } else if (ui->sbusButton->isChecked()) { getWizard()->setInputType(SetupWizard::INPUT_SBUS); - } - else if(ui->spectrumButton->isChecked()) { + } else if (ui->spectrumButton->isChecked()) { getWizard()->setInputType(SetupWizard::INPUT_DSM2); - } - else { + } else { getWizard()->setInputType(SetupWizard::INPUT_PWM); } getWizard()->setRestartNeeded(getWizard()->isRestartNeeded() || restartNeeded(getWizard()->getInputType())); @@ -71,45 +67,52 @@ bool InputPage::restartNeeded(VehicleConfigurationSource::INPUT_TYPE selectedTyp { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *uavoManager = pm->getObject(); + Q_ASSERT(uavoManager); - HwSettings* hwSettings = HwSettings::GetInstance(uavoManager); + HwSettings *hwSettings = HwSettings::GetInstance(uavoManager); HwSettings::DataFields data = hwSettings->getData(); - switch(getWizard()->getControllerType()) { - case SetupWizard::CONTROLLER_CC: - case SetupWizard::CONTROLLER_CC3D: - { - switch(selectedType) - { - case VehicleConfigurationSource::INPUT_PWM: - return data.CC_RcvrPort != HwSettings::CC_RCVRPORT_PWM; - case VehicleConfigurationSource::INPUT_PPM: - return data.CC_RcvrPort != HwSettings::CC_RCVRPORT_PPM; - case VehicleConfigurationSource::INPUT_SBUS: - return data.CC_MainPort != HwSettings::CC_MAINPORT_SBUS; - case VehicleConfigurationSource::INPUT_DSM2: - // TODO: Handle all of the DSM types ?? Which is most common? - return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM2; - default: return true; - } - break; - } - case SetupWizard::CONTROLLER_REVO: - { - switch(selectedType) - { - case VehicleConfigurationSource::INPUT_PWM: - return data.RM_RcvrPort != HwSettings::CC_RCVRPORT_PWM; - case VehicleConfigurationSource::INPUT_PPM: - return data.RM_RcvrPort != HwSettings::CC_RCVRPORT_PPM; - case VehicleConfigurationSource::INPUT_SBUS: - return data.RM_MainPort != HwSettings::CC_MAINPORT_SBUS; - case VehicleConfigurationSource::INPUT_DSM2: - // TODO: Handle all of the DSM types ?? Which is most common? - return data.RM_MainPort != HwSettings::CC_MAINPORT_DSM2; - default: return true; - } - break; - } + switch (getWizard()->getControllerType()) { + case SetupWizard::CONTROLLER_CC: + case SetupWizard::CONTROLLER_CC3D: + { + switch (selectedType) { + case VehicleConfigurationSource::INPUT_PWM: + return data.CC_RcvrPort != HwSettings::CC_RCVRPORT_PWM; + + case VehicleConfigurationSource::INPUT_PPM: + return data.CC_RcvrPort != HwSettings::CC_RCVRPORT_PPM; + + case VehicleConfigurationSource::INPUT_SBUS: + return data.CC_MainPort != HwSettings::CC_MAINPORT_SBUS; + + case VehicleConfigurationSource::INPUT_DSM2: + // TODO: Handle all of the DSM types ?? Which is most common? + return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM2; + default: return true; + } + break; + } + case SetupWizard::CONTROLLER_REVO: + { + switch (selectedType) { + case VehicleConfigurationSource::INPUT_PWM: + return data.RM_RcvrPort != HwSettings::CC_RCVRPORT_PWM; + + case VehicleConfigurationSource::INPUT_PPM: + return data.RM_RcvrPort != HwSettings::CC_RCVRPORT_PPM; + + case VehicleConfigurationSource::INPUT_SBUS: + return data.RM_MainPort != HwSettings::CC_MAINPORT_SBUS; + + case VehicleConfigurationSource::INPUT_DSM2: + // TODO: Handle all of the DSM types ?? Which is most common? + return data.RM_MainPort != HwSettings::CC_MAINPORT_DSM2; + + default: return true; + } + break; + } + default: return true; } } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.h index dde50b6e6..7b0221527 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.h @@ -34,8 +34,7 @@ namespace Ui { class InputPage; } -class InputPage : public AbstractWizardPage -{ +class InputPage : public AbstractWizardPage { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp index 79ea7131c..1c47894cc 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp @@ -33,7 +33,7 @@ MultiPage::MultiPage(SetupWizard *wizard, QWidget *parent) : AbstractWizardPage(wizard, parent), ui(new Ui::MultiPage) { - ui->setupUi(this); + ui->setupUi(this); QSvgRenderer *renderer = new QSvgRenderer(); renderer->load(QString(":/configgadget/images/multirotor-shapes.svg")); @@ -65,7 +65,8 @@ void MultiPage::initializePage() bool MultiPage::validatePage() { - SetupWizard::VEHICLE_SUB_TYPE type = (SetupWizard::VEHICLE_SUB_TYPE) ui->typeCombo->itemData(ui->typeCombo->currentIndex()).toInt(); + SetupWizard::VEHICLE_SUB_TYPE type = (SetupWizard::VEHICLE_SUB_TYPE)ui->typeCombo->itemData(ui->typeCombo->currentIndex()).toInt(); + getWizard()->setVehicleSubType(type); return true; } @@ -73,7 +74,7 @@ bool MultiPage::validatePage() void MultiPage::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - if(m_multiPic) { + if (m_multiPic) { ui->typeGraphicsView->setSceneRect(m_multiPic->boundingRect()); ui->typeGraphicsView->fitInView(m_multiPic, Qt::KeepAspectRatio); } @@ -108,71 +109,71 @@ void MultiPage::setupMultiTypesCombo() // Fredrik Arvidsson(m_thread) 2012-08-26 Disable Octos until further notice /* - ui->typeCombo->addItem(tr("Octocopter"), SetupWizard::MULTI_ROTOR_OCTO); - m_descriptions << tr("Octocopter"); + ui->typeCombo->addItem(tr("Octocopter"), SetupWizard::MULTI_ROTOR_OCTO); + m_descriptions << tr("Octocopter"); - ui->typeCombo->addItem(tr("Octocopter Coax X"), SetupWizard::MULTI_ROTOR_OCTO_COAX_X); - m_descriptions << tr("Octocopter Coax X"); + ui->typeCombo->addItem(tr("Octocopter Coax X"), SetupWizard::MULTI_ROTOR_OCTO_COAX_X); + m_descriptions << tr("Octocopter Coax X"); - ui->typeCombo->addItem(tr("Octocopter Coax +"), SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS); - m_descriptions << tr("Octocopter Coax +"); + ui->typeCombo->addItem(tr("Octocopter Coax +"), SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS); + m_descriptions << tr("Octocopter Coax +"); - ui->typeCombo->addItem(tr("Octocopter V"), SetupWizard::MULTI_ROTOR_OCTO_V); - m_descriptions << tr("Octocopter V"); - */ + ui->typeCombo->addItem(tr("Octocopter V"), SetupWizard::MULTI_ROTOR_OCTO_V); + m_descriptions << tr("Octocopter V"); + */ } void MultiPage::updateAvailableTypes() { /* - QVariant enable = (getWizard()->getInputType() == SetupWizard::INPUT_PWM) ? QVariant(0) : QVariant(1 | 32); - ui->typeCombo->model()->setData(ui->typeCombo->model()->index(6, 0), enable, Qt::UserRole - 1); - ui->typeCombo->model()->setData(ui->typeCombo->model()->index(7, 0), enable, Qt::UserRole - 1); - ui->typeCombo->model()->setData(ui->typeCombo->model()->index(8, 0), enable, Qt::UserRole - 1); - ui->typeCombo->model()->setData(ui->typeCombo->model()->index(9, 0), enable, Qt::UserRole - 1); - */ + QVariant enable = (getWizard()->getInputType() == SetupWizard::INPUT_PWM) ? QVariant(0) : QVariant(1 | 32); + ui->typeCombo->model()->setData(ui->typeCombo->model()->index(6, 0), enable, Qt::UserRole - 1); + ui->typeCombo->model()->setData(ui->typeCombo->model()->index(7, 0), enable, Qt::UserRole - 1); + ui->typeCombo->model()->setData(ui->typeCombo->model()->index(8, 0), enable, Qt::UserRole - 1); + ui->typeCombo->model()->setData(ui->typeCombo->model()->index(9, 0), enable, Qt::UserRole - 1); + */ } void MultiPage::updateImageAndDescription() { - SetupWizard::VEHICLE_SUB_TYPE type = (SetupWizard::VEHICLE_SUB_TYPE) ui->typeCombo->itemData(ui->typeCombo->currentIndex()).toInt(); - QString elementId = ""; + SetupWizard::VEHICLE_SUB_TYPE type = (SetupWizard::VEHICLE_SUB_TYPE)ui->typeCombo->itemData(ui->typeCombo->currentIndex()).toInt(); + QString elementId = ""; QString description = m_descriptions.at(ui->typeCombo->currentIndex()); - switch(type) - { - case SetupWizard::MULTI_ROTOR_TRI_Y: - elementId = "tri"; - break; - case SetupWizard::MULTI_ROTOR_QUAD_X: - elementId = "quad-x"; - break; - case SetupWizard::MULTI_ROTOR_QUAD_PLUS: - elementId = "quad-plus"; - break; - case SetupWizard::MULTI_ROTOR_HEXA: - elementId = "quad-hexa"; - break; - case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: - elementId = "hexa-coax"; - break; - case SetupWizard::MULTI_ROTOR_HEXA_H: - elementId = "quad-hexa-H"; - break; - case SetupWizard::MULTI_ROTOR_OCTO: - elementId = "quad-octo"; - break; - case SetupWizard::MULTI_ROTOR_OCTO_COAX_X: - elementId = "octo-coax-X"; - break; - case SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS: - elementId = "octo-coax-P"; - break; - case SetupWizard::MULTI_ROTOR_OCTO_V: - elementId = "quad-octo-v"; - break; - default: - elementId = ""; - break; + + switch (type) { + case SetupWizard::MULTI_ROTOR_TRI_Y: + elementId = "tri"; + break; + case SetupWizard::MULTI_ROTOR_QUAD_X: + elementId = "quad-x"; + break; + case SetupWizard::MULTI_ROTOR_QUAD_PLUS: + elementId = "quad-plus"; + break; + case SetupWizard::MULTI_ROTOR_HEXA: + elementId = "quad-hexa"; + break; + case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: + elementId = "hexa-coax"; + break; + case SetupWizard::MULTI_ROTOR_HEXA_H: + elementId = "quad-hexa-H"; + break; + case SetupWizard::MULTI_ROTOR_OCTO: + elementId = "quad-octo"; + break; + case SetupWizard::MULTI_ROTOR_OCTO_COAX_X: + elementId = "octo-coax-X"; + break; + case SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS: + elementId = "octo-coax-P"; + break; + case SetupWizard::MULTI_ROTOR_OCTO_V: + elementId = "quad-octo-v"; + break; + default: + elementId = ""; + break; } m_multiPic->setElementId(elementId); ui->typeGraphicsView->setSceneRect(m_multiPic->boundingRect()); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.h index 5614ea6cb..ced899ebe 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.h @@ -38,10 +38,9 @@ namespace Ui { class MultiPage; } -class MultiPage : public AbstractWizardPage -{ +class MultiPage : public AbstractWizardPage { Q_OBJECT - + public: explicit MultiPage(SetupWizard *wizard, QWidget *parent = 0); ~MultiPage(); @@ -51,7 +50,7 @@ public: protected: void resizeEvent(QResizeEvent *event); - + private: Ui::MultiPage *ui; void setupMultiTypesCombo(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.h index 4f323d9fc..93d4f1231 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.h @@ -34,14 +34,13 @@ namespace Ui { class NotYetImplementedPage; } -class NotYetImplementedPage : public AbstractWizardPage -{ +class NotYetImplementedPage : public AbstractWizardPage { Q_OBJECT - + public: explicit NotYetImplementedPage(SetupWizard *wizard, QWidget *parent = 0); ~NotYetImplementedPage(); - + private: Ui::NotYetImplementedPage *ui; }; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index 70a2a9fc5..c1eebfa1d 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -38,9 +38,8 @@ OutputCalibrationPage::OutputCalibrationPage(SetupWizard *wizard, QWidget *paren m_vehicleRenderer = new QSvgRenderer(); if (QFile::exists(QString(":/setupwizard/resources/multirotor-shapes.svg")) && - m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) && - m_vehicleRenderer->isValid()) - { + m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->isValid()) { m_vehicleScene = new QGraphicsScene(this); ui->vehicleView->setScene(m_vehicleScene); } @@ -48,7 +47,7 @@ OutputCalibrationPage::OutputCalibrationPage(SetupWizard *wizard, QWidget *paren OutputCalibrationPage::~OutputCalibrationPage() { - if(m_calibrationUtil) { + if (m_calibrationUtil) { delete m_calibrationUtil; m_calibrationUtil = 0; } @@ -64,56 +63,55 @@ void OutputCalibrationPage::setupVehicle() m_channelIndex.clear(); m_currentWizardIndex = 0; m_vehicleScene->clear(); - switch(getWizard()->getVehicleSubType()) - { - case SetupWizard::MULTI_ROTOR_TRI_Y: - m_wizardIndexes << 0 << 1 << 1 << 1 << 2 << 3 << 4; - m_vehicleElementIds << "tri" << "tri-frame" << "tri-m1" << "tri-m2" << "tri-m3" << "tri-s1"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 4 << 4; - m_channelIndex << 0 << 0 << 1 << 2 << 3 << 3 << 3; - m_actuatorSettings[3].channelMin = 1500; - m_actuatorSettings[3].channelNeutral = 1500; - m_actuatorSettings[3].channelMax = 1500; - getWizard()->setActuatorSettings(m_actuatorSettings); - break; - case SetupWizard::MULTI_ROTOR_QUAD_X: - m_wizardIndexes << 0 << 1 << 1 << 1 << 1; - m_vehicleElementIds << "quad-x" << "quad-x-frame" << "quad-x-m1" << "quad-x-m2" << "quad-x-m3" << "quad-x-m4"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4; - m_channelIndex << 0 << 0 << 1 << 2 << 3; - break; - case SetupWizard::MULTI_ROTOR_QUAD_PLUS: - m_wizardIndexes << 0 << 1 << 1 << 1 << 1; - m_vehicleElementIds << "quad-p" << "quad-p-frame" << "quad-p-m1" << "quad-p-m2" << "quad-p-m3" << "quad-p-m4"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4; - m_channelIndex << 0 << 0 << 1 << 2 << 3; - break; - case SetupWizard::MULTI_ROTOR_HEXA: - m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; - m_vehicleElementIds << "hexa" << "hexa-frame" << "hexa-m1" << "hexa-m2" << "hexa-m3" << "hexa-m4" << "hexa-m5" << "hexa-m6"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; - m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; - break; - case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: - m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; - m_vehicleElementIds << "hexa-y6" << "hexa-y6-frame" << "hexa-y6-m2" << "hexa-y6-m1" << "hexa-y6-m4" << "hexa-y6-m3" << "hexa-y6-m6" << "hexa-y6-m5"; - m_vehicleHighlightElementIndexes << 0 << 2 << 1 << 4 << 3 << 6 << 5; - m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; - break; - case SetupWizard::MULTI_ROTOR_HEXA_H: - m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; - m_vehicleElementIds << "hexa-h" << "hexa-h-frame" << "hexa-h-m1" << "hexa-h-m2" << "hexa-h-m3" << "hexa-h-m4" << "hexa-h-m5" << "hexa-h-m6"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; - m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; - break; - default: - break; + switch (getWizard()->getVehicleSubType()) { + case SetupWizard::MULTI_ROTOR_TRI_Y: + m_wizardIndexes << 0 << 1 << 1 << 1 << 2 << 3 << 4; + m_vehicleElementIds << "tri" << "tri-frame" << "tri-m1" << "tri-m2" << "tri-m3" << "tri-s1"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 4 << 4; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 3 << 3; + m_actuatorSettings[3].channelMin = 1500; + m_actuatorSettings[3].channelNeutral = 1500; + m_actuatorSettings[3].channelMax = 1500; + getWizard()->setActuatorSettings(m_actuatorSettings); + break; + case SetupWizard::MULTI_ROTOR_QUAD_X: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "quad-x" << "quad-x-frame" << "quad-x-m1" << "quad-x-m2" << "quad-x-m3" << "quad-x-m4"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4; + m_channelIndex << 0 << 0 << 1 << 2 << 3; + break; + case SetupWizard::MULTI_ROTOR_QUAD_PLUS: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "quad-p" << "quad-p-frame" << "quad-p-m1" << "quad-p-m2" << "quad-p-m3" << "quad-p-m4"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4; + m_channelIndex << 0 << 0 << 1 << 2 << 3; + break; + case SetupWizard::MULTI_ROTOR_HEXA: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "hexa" << "hexa-frame" << "hexa-m1" << "hexa-m2" << "hexa-m3" << "hexa-m4" << "hexa-m5" << "hexa-m6"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + break; + case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "hexa-y6" << "hexa-y6-frame" << "hexa-y6-m2" << "hexa-y6-m1" << "hexa-y6-m4" << "hexa-y6-m3" << "hexa-y6-m6" << "hexa-y6-m5"; + m_vehicleHighlightElementIndexes << 0 << 2 << 1 << 4 << 3 << 6 << 5; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + break; + case SetupWizard::MULTI_ROTOR_HEXA_H: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "hexa-h" << "hexa-h-frame" << "hexa-h-m1" << "hexa-h-m2" << "hexa-h-m3" << "hexa-h-m4" << "hexa-h-m5" << "hexa-h-m6"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + break; + default: + break; } VehicleConfigurationHelper helper(getWizard()); helper.setupVehicle(false); - if(m_calibrationUtil) { + if (m_calibrationUtil) { delete m_calibrationUtil; m_calibrationUtil = 0; } @@ -134,8 +132,7 @@ void OutputCalibrationPage::setupVehicleItems() QRectF parentBounds = m_vehicleRenderer->boundsOnElement(m_vehicleElementIds[0]); - for(int i = 1; i < m_vehicleElementIds.size(); i++) - { + for (int i = 1; i < m_vehicleElementIds.size(); i++) { QGraphicsSvgItem *item = new QGraphicsSvgItem(); item->setSharedRenderer(m_vehicleRenderer); item->setElementId(m_vehicleElementIds[i]); @@ -160,9 +157,10 @@ void OutputCalibrationPage::setupVehicleHighlightedPart() { qreal dimOpaque = m_currentWizardIndex == 0 ? 1.0 : 0.3; qreal highlightOpaque = 1.0; - int highlightedIndex = m_vehicleHighlightElementIndexes[m_currentWizardIndex]; - for(int i = 0; i < m_vehicleItems.size(); i++) { - QGraphicsSvgItem* item = m_vehicleItems[i]; + int highlightedIndex = m_vehicleHighlightElementIndexes[m_currentWizardIndex]; + + for (int i = 0; i < m_vehicleItems.size(); i++) { + QGraphicsSvgItem *item = m_vehicleItems[i]; item->setOpacity((highlightedIndex == i) ? highlightOpaque : dimOpaque); } } @@ -180,18 +178,15 @@ void OutputCalibrationPage::setWizardPage() int currentChannel = getCurrentChannel(); qDebug() << "Current channel: " << currentChannel; - if(currentChannel >= 0) { - if(currentPageIndex == 1) { + if (currentChannel >= 0) { + if (currentPageIndex == 1) { ui->motorNeutralSlider->setValue(m_actuatorSettings[currentChannel].channelNeutral); - } - else if(currentPageIndex == 2) { + } else if (currentPageIndex == 2) { ui->servoCenterSlider->setValue(m_actuatorSettings[currentChannel].channelNeutral); - } - else if(currentPageIndex == 3) { + } else if (currentPageIndex == 3) { ui->servoMinAngleSlider->setMaximum(m_actuatorSettings[currentChannel].channelNeutral); ui->servoMinAngleSlider->setValue(m_actuatorSettings[currentChannel].channelMin); - } - else if(currentPageIndex == 4) { + } else if (currentPageIndex == 4) { ui->servoMaxAngleSlider->setMinimum(m_actuatorSettings[currentChannel].channelNeutral); ui->servoMaxAngleSlider->setValue(m_actuatorSettings[currentChannel].channelMax); } @@ -201,7 +196,7 @@ void OutputCalibrationPage::setWizardPage() void OutputCalibrationPage::initializePage() { - if(m_vehicleScene) { + if (m_vehicleScene) { setupVehicle(); startWizard(); } @@ -209,7 +204,7 @@ void OutputCalibrationPage::initializePage() bool OutputCalibrationPage::validatePage() { - if(isFinished()) { + if (isFinished()) { getWizard()->setActuatorSettings(m_actuatorSettings); return true; } else { @@ -222,7 +217,7 @@ bool OutputCalibrationPage::validatePage() void OutputCalibrationPage::showEvent(QShowEvent *event) { Q_UNUSED(event); - if(m_vehicleBoundsItem) { + if (m_vehicleBoundsItem) { ui->vehicleView->setSceneRect(m_vehicleBoundsItem->boundingRect()); ui->vehicleView->fitInView(m_vehicleBoundsItem, Qt::KeepAspectRatio); } @@ -231,7 +226,7 @@ void OutputCalibrationPage::showEvent(QShowEvent *event) void OutputCalibrationPage::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - if(m_vehicleBoundsItem) { + if (m_vehicleBoundsItem) { ui->vehicleView->setSceneRect(m_vehicleBoundsItem->boundingRect()); ui->vehicleView->fitInView(m_vehicleBoundsItem, Qt::KeepAspectRatio); } @@ -239,20 +234,17 @@ void OutputCalibrationPage::resizeEvent(QResizeEvent *event) void OutputCalibrationPage::customBackClicked() { - if(m_currentWizardIndex > 0) - { + if (m_currentWizardIndex > 0) { m_currentWizardIndex--; setWizardPage(); - } - else - { + } else { getWizard()->back(); } } quint16 OutputCalibrationPage::getCurrentChannel() { - return m_channelIndex[m_currentWizardIndex]; + return m_channelIndex[m_currentWizardIndex]; } void OutputCalibrationPage::enableButtons(bool enable) @@ -271,19 +263,18 @@ void OutputCalibrationPage::on_motorNeutralButton_toggled(bool checked) onStartButtonToggle(ui->motorNeutralButton, channel, m_actuatorSettings[channel].channelNeutral, 1000, ui->motorNeutralSlider); } -void OutputCalibrationPage::onStartButtonToggle(QAbstractButton *button, quint16 channel, quint16 value, quint16 safeValue, QSlider *slider) { - if(button->isChecked()) { - if(checkAlarms()) { +void OutputCalibrationPage::onStartButtonToggle(QAbstractButton *button, quint16 channel, quint16 value, quint16 safeValue, QSlider *slider) +{ + if (button->isChecked()) { + if (checkAlarms()) { enableButtons(false); m_calibrationUtil->startChannelOutput(channel, safeValue); slider->setValue(value); m_calibrationUtil->setChannelOutputValue(value); - } - else { + } else { button->setChecked(false); } - } - else { + } else { m_calibrationUtil->stopChannelOutput(); enableButtons(true); } @@ -294,12 +285,13 @@ bool OutputCalibrationPage::checkAlarms() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *uavObjectManager = pm->getObject(); + Q_ASSERT(uavObjectManager); - SystemAlarms * systemAlarms = SystemAlarms::GetInstance(uavObjectManager); + SystemAlarms *systemAlarms = SystemAlarms::GetInstance(uavObjectManager); Q_ASSERT(systemAlarms); SystemAlarms::DataFields data = systemAlarms->getData(); - if(data.Alarm[SystemAlarms::ALARM_ACTUATOR] != SystemAlarms::ALARM_OK) { + if (data.Alarm[SystemAlarms::ALARM_ACTUATOR] != SystemAlarms::ALARM_OK) { QMessageBox mbox(this); mbox.setText(QString(tr("The actuator module is in an error state.\n\n" "Please make sure the correct firmware version is used then " @@ -323,6 +315,7 @@ bool OutputCalibrationPage::checkAlarms() void OutputCalibrationPage::debugLogChannelValues() { quint16 channel = getCurrentChannel(); + qDebug() << "ChannelMin : " << m_actuatorSettings[channel].channelMin; qDebug() << "ChannelNeutral: " << m_actuatorSettings[channel].channelNeutral; qDebug() << "ChannelMax : " << m_actuatorSettings[channel].channelMax; @@ -331,7 +324,7 @@ void OutputCalibrationPage::debugLogChannelValues() void OutputCalibrationPage::on_motorNeutralSlider_valueChanged(int value) { Q_UNUSED(value); - if(ui->motorNeutralButton->isChecked()) { + if (ui->motorNeutralButton->isChecked()) { quint16 value = ui->motorNeutralSlider->value(); m_calibrationUtil->setChannelOutputValue(value); m_actuatorSettings[getCurrentChannel()].channelNeutral = value; @@ -342,7 +335,7 @@ void OutputCalibrationPage::on_motorNeutralSlider_valueChanged(int value) void OutputCalibrationPage::on_servoCenterButton_toggled(bool checked) { ui->servoCenterButton->setText(checked ? tr("Stop") : tr("Start")); - quint16 channel = getCurrentChannel(); + quint16 channel = getCurrentChannel(); quint16 safeValue = m_actuatorSettings[channel].channelNeutral; onStartButtonToggle(ui->servoCenterButton, channel, safeValue, safeValue, ui->servoCenterSlider); } @@ -350,17 +343,17 @@ void OutputCalibrationPage::on_servoCenterButton_toggled(bool checked) void OutputCalibrationPage::on_servoCenterSlider_valueChanged(int position) { Q_UNUSED(position); - if(ui->servoCenterButton->isChecked()) { - quint16 value = ui->servoCenterSlider->value(); + if (ui->servoCenterButton->isChecked()) { + quint16 value = ui->servoCenterSlider->value(); m_calibrationUtil->setChannelOutputValue(value); quint16 channel = getCurrentChannel(); m_actuatorSettings[channel].channelNeutral = value; - //Adjust min and max - if(value < m_actuatorSettings[channel].channelMin) { + // Adjust min and max + if (value < m_actuatorSettings[channel].channelMin) { m_actuatorSettings[channel].channelMin = value; } - if(value > m_actuatorSettings[channel].channelMax) { + if (value > m_actuatorSettings[channel].channelMax) { m_actuatorSettings[channel].channelMax = value; } debugLogChannelValues(); @@ -370,7 +363,7 @@ void OutputCalibrationPage::on_servoCenterSlider_valueChanged(int position) void OutputCalibrationPage::on_servoMinAngleButton_toggled(bool checked) { ui->servoMinAngleButton->setText(checked ? tr("Stop") : tr("Start")); - quint16 channel = getCurrentChannel(); + quint16 channel = getCurrentChannel(); quint16 safeValue = m_actuatorSettings[channel].channelNeutral; onStartButtonToggle(ui->servoMinAngleButton, channel, m_actuatorSettings[channel].channelMin, safeValue, ui->servoMinAngleSlider); } @@ -378,7 +371,7 @@ void OutputCalibrationPage::on_servoMinAngleButton_toggled(bool checked) void OutputCalibrationPage::on_servoMinAngleSlider_valueChanged(int position) { Q_UNUSED(position); - if(ui->servoMinAngleButton->isChecked()) { + if (ui->servoMinAngleButton->isChecked()) { quint16 value = ui->servoMinAngleSlider->value(); m_calibrationUtil->setChannelOutputValue(value); m_actuatorSettings[getCurrentChannel()].channelMin = value; @@ -389,7 +382,7 @@ void OutputCalibrationPage::on_servoMinAngleSlider_valueChanged(int position) void OutputCalibrationPage::on_servoMaxAngleButton_toggled(bool checked) { ui->servoMaxAngleButton->setText(checked ? tr("Stop") : tr("Start")); - quint16 channel = getCurrentChannel(); + quint16 channel = getCurrentChannel(); quint16 safeValue = m_actuatorSettings[channel].channelNeutral; onStartButtonToggle(ui->servoMaxAngleButton, channel, m_actuatorSettings[channel].channelMax, safeValue, ui->servoMaxAngleSlider); } @@ -397,7 +390,7 @@ void OutputCalibrationPage::on_servoMaxAngleButton_toggled(bool checked) void OutputCalibrationPage::on_servoMaxAngleSlider_valueChanged(int position) { Q_UNUSED(position); - if(ui->servoMaxAngleButton->isChecked()) { + if (ui->servoMaxAngleButton->isChecked()) { quint16 value = ui->servoMaxAngleSlider->value(); m_calibrationUtil->setChannelOutputValue(value); m_actuatorSettings[getCurrentChannel()].channelMax = value; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h index 1a3302e53..50791517a 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h @@ -38,22 +38,24 @@ namespace Ui { class OutputCalibrationPage; } -class OutputCalibrationPage : public AbstractWizardPage -{ +class OutputCalibrationPage : public AbstractWizardPage { Q_OBJECT - + public: explicit OutputCalibrationPage(SetupWizard *wizard, QWidget *parent = 0); ~OutputCalibrationPage(); void initializePage(); bool validatePage(); - bool isFinished() { return m_currentWizardIndex >= m_wizardIndexes.size() - 1; } + bool isFinished() + { + return m_currentWizardIndex >= m_wizardIndexes.size() - 1; + } protected: void showEvent(QShowEvent *event); void resizeEvent(QResizeEvent *event); - + public slots: void customBackClicked(); @@ -90,7 +92,7 @@ private: qint16 m_currentWizardIndex; QList m_vehicleElementIds; - QList m_vehicleItems; + QList m_vehicleItems; QList m_vehicleHighlightElementIndexes; QList m_channelIndex; QList m_wizardIndexes; @@ -98,7 +100,6 @@ private: QList m_actuatorSettings; OutputCalibrationUtil *m_calibrationUtil; - }; #endif // OUTPUTCALIBRATIONPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp index d9903af57..b57237f3f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp @@ -30,7 +30,7 @@ #include "setupwizard.h" OutputPage::OutputPage(SetupWizard *wizard, QWidget *parent) : - AbstractWizardPage(wizard, parent), + AbstractWizardPage(wizard, parent), ui(new Ui::OutputPage) { @@ -44,10 +44,9 @@ OutputPage::~OutputPage() bool OutputPage::validatePage() { - if(ui->rapidESCButton->isChecked()) { + if (ui->rapidESCButton->isChecked()) { getWizard()->setESCType(SetupWizard::ESC_RAPID); - } - else { + } else { getWizard()->setESCType(SetupWizard::ESC_LEGACY); } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.h index 3a7130238..40ba78ac3 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.h @@ -34,15 +34,14 @@ namespace Ui { class OutputPage; } -class OutputPage : public AbstractWizardPage -{ +class OutputPage : public AbstractWizardPage { Q_OBJECT - + public: explicit OutputPage(SetupWizard *wizard, QWidget *parent = 0); ~OutputPage(); bool validatePage(); - + private: Ui::OutputPage *ui; }; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.cpp index 43153380b..4ca0c9c74 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.cpp @@ -46,7 +46,7 @@ RebootPage::~RebootPage() void RebootPage::initializePage() { - if(!m_timer.isActive()) { + if (!m_timer.isActive()) { connect(&m_timer, SIGNAL(timeout()), this, SLOT(toggleLabel())); m_timer.setInterval(500); m_timer.setSingleShot(false); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.h index c3d32fca2..786a77253 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.h @@ -34,17 +34,16 @@ namespace Ui { class RebootPage; } -class RebootPage : public AbstractWizardPage -{ +class RebootPage : public AbstractWizardPage { Q_OBJECT - + public: explicit RebootPage(SetupWizard *wizard, QWidget *parent = 0); ~RebootPage(); void initializePage(); bool validatePage(); - + private: Ui::RebootPage *ui; QTimer m_timer; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/revocalibrationpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/revocalibrationpage.h index 9939d59ea..c9134ea8f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/revocalibrationpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/revocalibrationpage.h @@ -34,14 +34,13 @@ namespace Ui { class RevoCalibrationPage; } -class RevoCalibrationPage : public AbstractWizardPage -{ +class RevoCalibrationPage : public AbstractWizardPage { Q_OBJECT - + public: explicit RevoCalibrationPage(SetupWizard *wizard, QWidget *parent = 0); ~RevoCalibrationPage(); - + private: Ui::RevoCalibrationPage *ui; }; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.cpp index 3069d20e6..22084b277 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.cpp @@ -32,7 +32,7 @@ #include "vehicleconfigurationhelper.h" SavePage::SavePage(SetupWizard *wizard, QWidget *parent) : - AbstractWizardPage(wizard, parent), + AbstractWizardPage(wizard, parent), ui(new Ui::SavePage), m_successfulWrite(false) { ui->setupUi(this); @@ -45,7 +45,7 @@ SavePage::~SavePage() } bool SavePage::validatePage() -{ +{ return true; } @@ -56,7 +56,7 @@ bool SavePage::isComplete() const void SavePage::writeToController() { - if(!getWizard()->getConnectionManager()->isConnected()) { + if (!getWizard()->getConnectionManager()->isConnected()) { QMessageBox msgBox; msgBox.setText(tr("An OpenPilot controller must be connected to your computer to save the " "configuration.\nPlease connect your OpenPilot controller to your computer and try again.")); @@ -68,11 +68,11 @@ void SavePage::writeToController() enableButtons(false); VehicleConfigurationHelper helper(getWizard()); - connect(&helper, SIGNAL(saveProgress(int, int, QString)),this, SLOT(saveProgress(int, int, QString))); + connect(&helper, SIGNAL(saveProgress(int, int, QString)), this, SLOT(saveProgress(int, int, QString))); m_successfulWrite = helper.setupVehicle(); - disconnect(&helper, SIGNAL(saveProgress(int, int, QString)),this, SLOT(saveProgress(int, int, QString))); + disconnect(&helper, SIGNAL(saveProgress(int, int, QString)), this, SLOT(saveProgress(int, int, QString))); ui->saveProgressLabel->setText(QString("%2").arg(m_successfulWrite ? "green" : "red", ui->saveProgressLabel->text())); enableButtons(true); @@ -92,13 +92,13 @@ void SavePage::enableButtons(bool enable) void SavePage::saveProgress(int total, int current, QString description) { qDebug() << "Progress " << current << "(" << total << ")"; - if(ui->saveProgressBar->maximum() != total) { + if (ui->saveProgressBar->maximum() != total) { ui->saveProgressBar->setMaximum(total); } - if(ui->saveProgressBar->value() != current) { + if (ui->saveProgressBar->value() != current) { ui->saveProgressBar->setValue(current); } - if(ui->saveProgressLabel->text() != description) { + if (ui->saveProgressLabel->text() != description) { ui->saveProgressLabel->setText(description); } } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.h index d8382df93..9310d837c 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.h @@ -34,8 +34,7 @@ namespace Ui { class SavePage; } -class SavePage : public AbstractWizardPage -{ +class SavePage : public AbstractWizardPage { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.cpp index bc0ee36f1..568da659c 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.cpp @@ -32,7 +32,7 @@ StartPage::StartPage(SetupWizard *wizard, QWidget *parent) : ui(new Ui::StartPage) { ui->setupUi(this); - setFont(QFont("Ubuntu",2)); + setFont(QFont("Ubuntu", 2)); } StartPage::~StartPage() diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.h index 3e9e120cc..da44d920e 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.h @@ -33,14 +33,13 @@ namespace Ui { class StartPage; } -class StartPage : public AbstractWizardPage -{ +class StartPage : public AbstractWizardPage { Q_OBJECT - + public: explicit StartPage(SetupWizard *wizard, QWidget *parent = 0); ~StartPage(); - + private: Ui::StartPage *ui; }; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.cpp index c4846e782..884c94eb9 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.cpp @@ -31,7 +31,7 @@ #include "connectiondiagram.h" SummaryPage::SummaryPage(SetupWizard *wizard, QWidget *parent) : - AbstractWizardPage(wizard, parent), + AbstractWizardPage(wizard, parent), ui(new Ui::SummaryPage) { ui->setupUi(this); @@ -56,5 +56,6 @@ void SummaryPage::initializePage() void SummaryPage::showDiagram() { ConnectionDiagram diagram(this, getWizard()); + diagram.exec(); } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.h index 616f82392..daf5402b1 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.h @@ -34,8 +34,7 @@ namespace Ui { class SummaryPage; } -class SummaryPage : public AbstractWizardPage -{ +class SummaryPage : public AbstractWizardPage { Q_OBJECT public: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.h index 593aa1fca..0bef8dc5c 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.h @@ -34,14 +34,13 @@ namespace Ui { class SurfacePage; } -class SurfacePage : public AbstractWizardPage -{ +class SurfacePage : public AbstractWizardPage { Q_OBJECT - + public: explicit SurfacePage(SetupWizard *wizard, QWidget *parent = 0); ~SurfacePage(); - + private: Ui::SurfacePage *ui; }; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp index a616a1194..b7de92bc9 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp @@ -42,19 +42,15 @@ VehiclePage::~VehiclePage() bool VehiclePage::validatePage() { - if(ui->multirotorButton->isChecked()) { + if (ui->multirotorButton->isChecked()) { getWizard()->setVehicleType(SetupWizard::VEHICLE_MULTI); - } - else if(ui->fixedwingButton->isChecked()) { + } else if (ui->fixedwingButton->isChecked()) { getWizard()->setVehicleType(SetupWizard::VEHICLE_FIXEDWING); - } - else if(ui->heliButton->isChecked()) { + } else if (ui->heliButton->isChecked()) { getWizard()->setVehicleType(SetupWizard::VEHICLE_HELI); - } - else if(ui->surfaceButton->isChecked()) { + } else if (ui->surfaceButton->isChecked()) { getWizard()->setVehicleType(SetupWizard::VEHICLE_SURFACE); - } - else { + } else { getWizard()->setVehicleType(SetupWizard::VEHICLE_UNKNOWN); } return true; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h index 5e72a3d21..737f1fc25 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h @@ -34,10 +34,9 @@ namespace Ui { class VehiclePage; } -class VehiclePage : public AbstractWizardPage -{ +class VehiclePage : public AbstractWizardPage { Q_OBJECT - + public: explicit VehiclePage(SetupWizard *wizard, QWidget *parent = 0); ~VehiclePage(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index bd237f875..86b009314 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -56,8 +56,7 @@ SetupWizard::SetupWizard(QWidget *parent) : QWizard(parent), VehicleConfiguratio { setWindowTitle(tr("OpenPilot Setup Wizard")); setOption(QWizard::IndependentPages, false); - for(quint16 i = 0; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) - { + for (quint16 i = 0; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) { m_actuatorSettings << actuatorChannelSettings(); } setWizardStyle(QWizard::ModernStyle); @@ -69,210 +68,218 @@ SetupWizard::SetupWizard(QWidget *parent) : QWizard(parent), VehicleConfiguratio int SetupWizard::nextId() const { switch (currentId()) { - case PAGE_START: - if(canAutoUpdate()) { - return PAGE_UPDATE; - } else { - return PAGE_CONTROLLER; - } - case PAGE_UPDATE: + case PAGE_START: + if (canAutoUpdate()) { + return PAGE_UPDATE; + } else { return PAGE_CONTROLLER; - case PAGE_CONTROLLER: { - switch(getControllerType()) - { - case CONTROLLER_CC: - case CONTROLLER_CC3D: - case CONTROLLER_REVO: - return PAGE_INPUT; - case CONTROLLER_OPLINK: - default: - return PAGE_NOTYETIMPLEMENTED; - } } - case PAGE_VEHICLES: { - switch(getVehicleType()) - { - case VEHICLE_MULTI: - return PAGE_MULTI; - case VEHICLE_FIXEDWING: - return PAGE_FIXEDWING; - case VEHICLE_HELI: - return PAGE_HELI; - case VEHICLE_SURFACE: - return PAGE_SURFACE; - default: - return PAGE_NOTYETIMPLEMENTED; - } - } - case PAGE_MULTI: - return PAGE_OUTPUT; - case PAGE_INPUT: - if(isRestartNeeded()) { - saveHardwareSettings(); - return PAGE_REBOOT; - } - else { - return PAGE_VEHICLES; - } - case PAGE_REBOOT: - return PAGE_VEHICLES; - case PAGE_OUTPUT: - return PAGE_SUMMARY; - case PAGE_BIAS_CALIBRATION: - return PAGE_OUTPUT_CALIBRATION; - //case PAGE_REVO_CALIBRATION: - // return PAGE_OUTPUT_CALIBRATION; - case PAGE_OUTPUT_CALIBRATION: - return PAGE_SAVE; - case PAGE_SUMMARY: - { - switch(getControllerType()) - { - case CONTROLLER_CC: - case CONTROLLER_CC3D: - case CONTROLLER_REVO: - return PAGE_BIAS_CALIBRATION; - default: - return PAGE_NOTYETIMPLEMENTED; - } - } - case PAGE_SAVE: - return PAGE_END; - case PAGE_NOTYETIMPLEMENTED: - return PAGE_END; + case PAGE_UPDATE: + return PAGE_CONTROLLER; + + case PAGE_CONTROLLER: + { + switch (getControllerType()) { + case CONTROLLER_CC: + case CONTROLLER_CC3D: + case CONTROLLER_REVO: + return PAGE_INPUT; + + case CONTROLLER_OPLINK: default: - return -1; + return PAGE_NOTYETIMPLEMENTED; + } + } + case PAGE_VEHICLES: + { + switch (getVehicleType()) { + case VEHICLE_MULTI: + return PAGE_MULTI; + + case VEHICLE_FIXEDWING: + return PAGE_FIXEDWING; + + case VEHICLE_HELI: + return PAGE_HELI; + + case VEHICLE_SURFACE: + return PAGE_SURFACE; + + default: + return PAGE_NOTYETIMPLEMENTED; + } + } + case PAGE_MULTI: + return PAGE_OUTPUT; + + case PAGE_INPUT: + if (isRestartNeeded()) { + saveHardwareSettings(); + return PAGE_REBOOT; + } else { + return PAGE_VEHICLES; + } + case PAGE_REBOOT: + return PAGE_VEHICLES; + + case PAGE_OUTPUT: + return PAGE_SUMMARY; + + case PAGE_BIAS_CALIBRATION: + return PAGE_OUTPUT_CALIBRATION; + + // case PAGE_REVO_CALIBRATION: + // return PAGE_OUTPUT_CALIBRATION; + case PAGE_OUTPUT_CALIBRATION: + return PAGE_SAVE; + + case PAGE_SUMMARY: + { + switch (getControllerType()) { + case CONTROLLER_CC: + case CONTROLLER_CC3D: + case CONTROLLER_REVO: + return PAGE_BIAS_CALIBRATION; + + default: + return PAGE_NOTYETIMPLEMENTED; + } + } + case PAGE_SAVE: + return PAGE_END; + + case PAGE_NOTYETIMPLEMENTED: + return PAGE_END; + + default: + return -1; } } QString SetupWizard::getSummaryText() { QString summary = ""; + summary.append("").append(tr("Controller type: ")).append(""); - switch(getControllerType()) - { - case CONTROLLER_CC: - summary.append(tr("OpenPilot CopterControl")); - break; - case CONTROLLER_CC3D: - summary.append(tr("OpenPilot CopterControl 3D")); - break; - case CONTROLLER_REVO: - summary.append(tr("OpenPilot Revolution")); - break; - case CONTROLLER_OPLINK: - summary.append(tr("OpenPilot OPLink Radio Modem")); - break; - default: - summary.append(tr("Unknown")); - break; + switch (getControllerType()) { + case CONTROLLER_CC: + summary.append(tr("OpenPilot CopterControl")); + break; + case CONTROLLER_CC3D: + summary.append(tr("OpenPilot CopterControl 3D")); + break; + case CONTROLLER_REVO: + summary.append(tr("OpenPilot Revolution")); + break; + case CONTROLLER_OPLINK: + summary.append(tr("OpenPilot OPLink Radio Modem")); + break; + default: + summary.append(tr("Unknown")); + break; } summary.append("
"); summary.append("").append(tr("Vehicle type: ")).append(""); - switch (getVehicleType()) - { - case VEHICLE_MULTI: - summary.append(tr("Multirotor")); - - summary.append("
"); - summary.append("").append(tr("Vehicle sub type: ")).append(""); - switch (getVehicleSubType()) - { - case SetupWizard::MULTI_ROTOR_TRI_Y: - summary.append(tr("Tricopter")); - break; - case SetupWizard::MULTI_ROTOR_QUAD_X: - summary.append(tr("Quadcopter X")); - break; - case SetupWizard::MULTI_ROTOR_QUAD_PLUS: - summary.append(tr("Quadcopter +")); - break; - case SetupWizard::MULTI_ROTOR_HEXA: - summary.append(tr("Hexacopter")); - break; - case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: - summary.append(tr("Hexacopter Coax (Y6)")); - break; - case SetupWizard::MULTI_ROTOR_HEXA_H: - summary.append(tr("Hexacopter X")); - break; - case SetupWizard::MULTI_ROTOR_OCTO: - summary.append(tr("Octocopter")); - break; - case SetupWizard::MULTI_ROTOR_OCTO_COAX_X: - summary.append(tr("Octocopter Coax X")); - break; - case SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS: - summary.append(tr("Octocopter Coax +")); - break; - case SetupWizard::MULTI_ROTOR_OCTO_V: - summary.append(tr("Octocopter V")); - break; - default: - summary.append(tr("Unknown")); - break; - } + switch (getVehicleType()) { + case VEHICLE_MULTI: + summary.append(tr("Multirotor")); + summary.append("
"); + summary.append("").append(tr("Vehicle sub type: ")).append(""); + switch (getVehicleSubType()) { + case SetupWizard::MULTI_ROTOR_TRI_Y: + summary.append(tr("Tricopter")); break; - case VEHICLE_FIXEDWING: - summary.append(tr("Fixed wing")); + case SetupWizard::MULTI_ROTOR_QUAD_X: + summary.append(tr("Quadcopter X")); break; - case VEHICLE_HELI: - summary.append(tr("Helicopter")); + case SetupWizard::MULTI_ROTOR_QUAD_PLUS: + summary.append(tr("Quadcopter +")); break; - case VEHICLE_SURFACE: - summary.append(tr("Surface vehicle")); + case SetupWizard::MULTI_ROTOR_HEXA: + summary.append(tr("Hexacopter")); + break; + case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: + summary.append(tr("Hexacopter Coax (Y6)")); + break; + case SetupWizard::MULTI_ROTOR_HEXA_H: + summary.append(tr("Hexacopter X")); + break; + case SetupWizard::MULTI_ROTOR_OCTO: + summary.append(tr("Octocopter")); + break; + case SetupWizard::MULTI_ROTOR_OCTO_COAX_X: + summary.append(tr("Octocopter Coax X")); + break; + case SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS: + summary.append(tr("Octocopter Coax +")); + break; + case SetupWizard::MULTI_ROTOR_OCTO_V: + summary.append(tr("Octocopter V")); break; default: summary.append(tr("Unknown")); + break; + } + + break; + case VEHICLE_FIXEDWING: + summary.append(tr("Fixed wing")); + break; + case VEHICLE_HELI: + summary.append(tr("Helicopter")); + break; + case VEHICLE_SURFACE: + summary.append(tr("Surface vehicle")); + break; + default: + summary.append(tr("Unknown")); } summary.append("
"); summary.append("").append(tr("Input type: ")).append(""); - switch (getInputType()) - { - case INPUT_PWM: - summary.append(tr("PWM (One cable per channel)")); - break; - case INPUT_PPM: - summary.append(tr("PPM (One cable for all channels)")); - break; - case INPUT_SBUS: - summary.append(tr("Futaba S.Bus")); - break; - case INPUT_DSM2: - summary.append(tr("Spektrum satellite (DSM2)")); - break; - case INPUT_DSMX10: - summary.append(tr("Spektrum satellite (DSMX10BIT)")); - break; - case INPUT_DSMX11: - summary.append(tr("Spektrum satellite (DSMX11BIT)")); - break; - default: - summary.append(tr("Unknown")); + switch (getInputType()) { + case INPUT_PWM: + summary.append(tr("PWM (One cable per channel)")); + break; + case INPUT_PPM: + summary.append(tr("PPM (One cable for all channels)")); + break; + case INPUT_SBUS: + summary.append(tr("Futaba S.Bus")); + break; + case INPUT_DSM2: + summary.append(tr("Spektrum satellite (DSM2)")); + break; + case INPUT_DSMX10: + summary.append(tr("Spektrum satellite (DSMX10BIT)")); + break; + case INPUT_DSMX11: + summary.append(tr("Spektrum satellite (DSMX11BIT)")); + break; + default: + summary.append(tr("Unknown")); } summary.append("
"); summary.append("").append(tr("ESC type: ")).append(""); - switch (getESCType()) - { - case ESC_LEGACY: - summary.append(tr("Legacy ESC (50 Hz)")); - break; - case ESC_RAPID: - summary.append(tr("Rapid ESC (400 Hz)")); - break; - default: - summary.append(tr("Unknown")); + switch (getESCType()) { + case ESC_LEGACY: + summary.append(tr("Legacy ESC (50 Hz)")); + break; + case ESC_RAPID: + summary.append(tr("Rapid ESC (400 Hz)")); + break; + default: + summary.append(tr("Unknown")); } /* - summary.append("
"); - summary.append("").append(tr("Reboot required: ")).append(""); - summary.append(isRestartNeeded() ? tr("Yes") : tr("No")); - */ + summary.append("
"); + summary.append("").append(tr("Reboot required: ")).append(""); + summary.append(isRestartNeeded() ? tr("Yes") : tr("No")); + */ return summary; } @@ -289,7 +296,7 @@ void SetupWizard::createPages() setPage(PAGE_INPUT, new InputPage(this)); setPage(PAGE_OUTPUT, new OutputPage(this)); setPage(PAGE_BIAS_CALIBRATION, new BiasCalibrationPage(this)); - //setPage(PAGE_REVO_CALIBRATION, new RevoCalibrationPage(this)); + // setPage(PAGE_REVO_CALIBRATION, new RevoCalibrationPage(this)); setPage(PAGE_OUTPUT_CALIBRATION, new OutputCalibrationPage(this)); setPage(PAGE_SUMMARY, new SummaryPage(this)); setPage(PAGE_SAVE, new SavePage(this)); @@ -309,10 +316,9 @@ void SetupWizard::createPages() void SetupWizard::customBackClicked() { - if(currentId() == PAGE_OUTPUT_CALIBRATION) { - static_cast(currentPage())->customBackClicked(); - } - else { + if (currentId() == PAGE_OUTPUT_CALIBRATION) { + static_cast(currentPage())->customBackClicked(); + } else { back(); } } @@ -326,14 +332,16 @@ void SetupWizard::pageChanged(int currId) bool SetupWizard::saveHardwareSettings() const { VehicleConfigurationHelper helper(const_cast(this)); + return helper.setupHardwareSettings(); } bool SetupWizard::canAutoUpdate() const { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm); - UploaderGadgetFactory *uploader = pm->getObject(); + UploaderGadgetFactory *uploader = pm->getObject(); Q_ASSERT(uploader); return uploader->isAutoUpdateCapable(); } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h index f0418660c..a908ac989 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h @@ -35,49 +35,115 @@ #include "vehicleconfigurationsource.h" #include "vehicleconfigurationhelper.h" -class SetupWizard : public QWizard, public VehicleConfigurationSource -{ +class SetupWizard : public QWizard, public VehicleConfigurationSource { Q_OBJECT public: SetupWizard(QWidget *parent = 0); int nextId() const; - void setControllerType(SetupWizard::CONTROLLER_TYPE type) { m_controllerType = type; } - SetupWizard::CONTROLLER_TYPE getControllerType() const { return m_controllerType; } + void setControllerType(SetupWizard::CONTROLLER_TYPE type) + { + m_controllerType = type; + } + SetupWizard::CONTROLLER_TYPE getControllerType() const + { + return m_controllerType; + } - void setVehicleType(SetupWizard::VEHICLE_TYPE type) { m_vehicleType = type; } - SetupWizard::VEHICLE_TYPE getVehicleType() const { return m_vehicleType; } + void setVehicleType(SetupWizard::VEHICLE_TYPE type) + { + m_vehicleType = type; + } + SetupWizard::VEHICLE_TYPE getVehicleType() const + { + return m_vehicleType; + } - void setVehicleSubType(SetupWizard::VEHICLE_SUB_TYPE type) { m_vehicleSubType = type; } - SetupWizard::VEHICLE_SUB_TYPE getVehicleSubType() const { return m_vehicleSubType; } + void setVehicleSubType(SetupWizard::VEHICLE_SUB_TYPE type) + { + m_vehicleSubType = type; + } + SetupWizard::VEHICLE_SUB_TYPE getVehicleSubType() const + { + return m_vehicleSubType; + } - void setInputType(SetupWizard::INPUT_TYPE type) { m_inputType = type; } - SetupWizard::INPUT_TYPE getInputType() const { return m_inputType; } + void setInputType(SetupWizard::INPUT_TYPE type) + { + m_inputType = type; + } + SetupWizard::INPUT_TYPE getInputType() const + { + return m_inputType; + } - void setESCType(SetupWizard::ESC_TYPE type) { m_escType = type; } - SetupWizard::ESC_TYPE getESCType() const { return m_escType; } + void setESCType(SetupWizard::ESC_TYPE type) + { + m_escType = type; + } + SetupWizard::ESC_TYPE getESCType() const + { + return m_escType; + } - void setGPSSetting(SetupWizard::GPS_SETTING setting) { m_gpsSetting = setting; } - SetupWizard::GPS_SETTING getGPSSetting() const {return m_gpsSetting;} + void setGPSSetting(SetupWizard::GPS_SETTING setting) + { + m_gpsSetting = setting; + } + SetupWizard::GPS_SETTING getGPSSetting() const + { + return m_gpsSetting; + } - void setRadioSetting(SetupWizard::RADIO_SETTING setting) { m_radioSetting = setting; } - SetupWizard::RADIO_SETTING getRadioSetting() const {return m_radioSetting;} + void setRadioSetting(SetupWizard::RADIO_SETTING setting) + { + m_radioSetting = setting; + } + SetupWizard::RADIO_SETTING getRadioSetting() const + { + return m_radioSetting; + } - void setLevellingBias(accelGyroBias bias) { m_calibrationBias = bias; m_calibrationPerformed = true; } - bool isCalibrationPerformed() const { return m_calibrationPerformed; } - accelGyroBias getCalibrationBias() const { return m_calibrationBias; } + void setLevellingBias(accelGyroBias bias) + { + m_calibrationBias = bias; m_calibrationPerformed = true; + } + bool isCalibrationPerformed() const + { + return m_calibrationPerformed; + } + accelGyroBias getCalibrationBias() const + { + return m_calibrationBias; + } - void setActuatorSettings(QList actuatorSettings) { m_actuatorSettings = actuatorSettings; } - bool isMotorCalibrationPerformed() const { return m_motorCalibrationPerformed; } - QList getActuatorSettings() const { return m_actuatorSettings; } + void setActuatorSettings(QList actuatorSettings) + { + m_actuatorSettings = actuatorSettings; + } + bool isMotorCalibrationPerformed() const + { + return m_motorCalibrationPerformed; + } + QList getActuatorSettings() const + { + return m_actuatorSettings; + } - void setRestartNeeded(bool needed) { m_restartNeeded = needed; } - bool isRestartNeeded() const {return m_restartNeeded; } + void setRestartNeeded(bool needed) + { + m_restartNeeded = needed; + } + bool isRestartNeeded() const + { + return m_restartNeeded; + } QString getSummaryText(); - Core::ConnectionManager* getConnectionManager() { + Core::ConnectionManager *getConnectionManager() + { if (!m_connectionManager) { m_connectionManager = Core::ICore::instance()->connectionManager(); Q_ASSERT(m_connectionManager); @@ -89,10 +155,10 @@ private slots: void customBackClicked(); void pageChanged(int currId); private: - enum {PAGE_START, PAGE_CONTROLLER, PAGE_VEHICLES, PAGE_MULTI, PAGE_FIXEDWING, - PAGE_HELI, PAGE_SURFACE, PAGE_INPUT, PAGE_OUTPUT, PAGE_BIAS_CALIBRATION, - PAGE_REVO_CALIBRATION, PAGE_OUTPUT_CALIBRATION, PAGE_SAVE, PAGE_SUMMARY, - PAGE_NOTYETIMPLEMENTED, PAGE_REBOOT, PAGE_END, PAGE_UPDATE}; + enum { PAGE_START, PAGE_CONTROLLER, PAGE_VEHICLES, PAGE_MULTI, PAGE_FIXEDWING, + PAGE_HELI, PAGE_SURFACE, PAGE_INPUT, PAGE_OUTPUT, PAGE_BIAS_CALIBRATION, + PAGE_REVO_CALIBRATION, PAGE_OUTPUT_CALIBRATION, PAGE_SAVE, PAGE_SUMMARY, + PAGE_NOTYETIMPLEMENTED, PAGE_REBOOT, PAGE_END, PAGE_UPDATE }; void createPages(); bool saveHardwareSettings() const; bool canAutoUpdate() const; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.cpp index defc1e927..2055c8e63 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.cpp @@ -38,23 +38,21 @@ #include SetupWizardPlugin::SetupWizardPlugin() : wizardRunning(false) -{ -} +{} SetupWizardPlugin::~SetupWizardPlugin() -{ -} +{} -bool SetupWizardPlugin::initialize(const QStringList& args, QString *errMsg) +bool SetupWizardPlugin::initialize(const QStringList & args, QString *errMsg) { Q_UNUSED(args); Q_UNUSED(errMsg); // Add Menu entry - Core::ActionManager* am = Core::ICore::instance()->actionManager(); - Core::ActionContainer* ac = am->actionContainer(Core::Constants::M_TOOLS); + Core::ActionManager *am = Core::ICore::instance()->actionManager(); + Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_TOOLS); - Core::Command* cmd = am->registerAction(new QAction(this), + Core::Command *cmd = am->registerAction(new QAction(this), "SetupWizardPlugin.ShowSetupWizard", QList() << Core::Constants::C_GLOBAL_ID); @@ -72,12 +70,10 @@ bool SetupWizardPlugin::initialize(const QStringList& args, QString *errMsg) } void SetupWizardPlugin::extensionsInitialized() -{ -} +{} void SetupWizardPlugin::shutdown() -{ -} +{} void SetupWizardPlugin::showSetupWizard() { @@ -85,7 +81,7 @@ void SetupWizardPlugin::showSetupWizard() wizardRunning = true; SetupWizard *m_wiz = new SetupWizard(); connect(m_wiz, SIGNAL(finished(int)), this, SLOT(wizardTerminated())); - m_wiz->setAttribute( Qt::WA_DeleteOnClose, true ); + m_wiz->setAttribute(Qt::WA_DeleteOnClose, true); m_wiz->setWindowFlags(m_wiz->windowFlags() | Qt::WindowStaysOnTopHint); m_wiz->show(); } @@ -94,7 +90,7 @@ void SetupWizardPlugin::showSetupWizard() void SetupWizardPlugin::wizardTerminated() { wizardRunning = false; - disconnect(this,SLOT(wizardTerminated())); + disconnect(this, SLOT(wizardTerminated())); } Q_EXPORT_PLUGIN(SetupWizardPlugin) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.h b/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.h index b79c0f7c5..f0765d993 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.h @@ -31,23 +31,22 @@ #include #include "setupwizard.h" -class SetupWizardPlugin : public ExtensionSystem::IPlugin -{ +class SetupWizardPlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: - SetupWizardPlugin(); - ~SetupWizardPlugin(); + SetupWizardPlugin(); + ~SetupWizardPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private slots: - void showSetupWizard(); - void wizardTerminated(); + void showSetupWizard(); + void wizardTerminated(); private: - bool wizardRunning; + bool wizardRunning; }; #endif // SETUPWIZARDPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 854a629cc..81d60f3c2 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -37,12 +37,12 @@ #include "revocalibration.h" const qint16 VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE = 50; -const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 400; +const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 400; VehicleConfigurationHelper::VehicleConfigurationHelper(VehicleConfigurationSource *configSource) : m_configSource(configSource), m_uavoManager(0), - m_transactionOK(false), m_transactionTimeout(false), m_currentTransactionObjectID(-1), - m_progress(0) + m_transactionOK(false), m_transactionTimeout(false), m_currentTransactionObjectID(-1), + m_progress(0) { Q_ASSERT(m_configSource); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); @@ -56,7 +56,7 @@ bool VehicleConfigurationHelper::setupVehicle(bool save) clearModifiedObjects(); resetVehicleConfig(); resetGUIData(); - if(!saveChangesToController(save)) { + if (!saveChangesToController(save)) { return false; } @@ -66,7 +66,7 @@ bool VehicleConfigurationHelper::setupVehicle(bool save) applyActuatorConfiguration(); applyFlighModeConfiguration(); - if(save) { + if (save) { applySensorBiasConfiguration(); } @@ -92,13 +92,13 @@ bool VehicleConfigurationHelper::setupHardwareSettings(bool save) void VehicleConfigurationHelper::addModifiedObject(UAVDataObject *object, QString description) { - m_modifiedObjects << new QPair(object, description); + m_modifiedObjects << new QPair(object, description); } void VehicleConfigurationHelper::clearModifiedObjects() { - for(int i = 0; i < m_modifiedObjects.count(); i++) { - QPair *pair = m_modifiedObjects.at(i); + for (int i = 0; i < m_modifiedObjects.count(); i++) { + QPair *pair = m_modifiedObjects.at(i); delete pair; } m_modifiedObjects.clear(); @@ -106,81 +106,79 @@ void VehicleConfigurationHelper::clearModifiedObjects() void VehicleConfigurationHelper::applyHardwareConfiguration() { - HwSettings* hwSettings = HwSettings::GetInstance(m_uavoManager); + HwSettings *hwSettings = HwSettings::GetInstance(m_uavoManager); HwSettings::DataFields data = hwSettings->getData(); - switch(m_configSource->getControllerType()) - { - case VehicleConfigurationSource::CONTROLLER_CC: - case VehicleConfigurationSource::CONTROLLER_CC3D: - // Reset all ports - data.CC_RcvrPort = HwSettings::CC_RCVRPORT_DISABLED; - //Default mainport to be active telemetry link - data.CC_MainPort = HwSettings::CC_MAINPORT_TELEMETRY; + switch (m_configSource->getControllerType()) { + case VehicleConfigurationSource::CONTROLLER_CC: + case VehicleConfigurationSource::CONTROLLER_CC3D: + // Reset all ports + data.CC_RcvrPort = HwSettings::CC_RCVRPORT_DISABLED; - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DISABLED; - switch(m_configSource->getInputType()) - { - case VehicleConfigurationSource::INPUT_PWM: - data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PWM; - break; - case VehicleConfigurationSource::INPUT_PPM: - data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PPM; - break; - case VehicleConfigurationSource::INPUT_SBUS: - // We have to set teletry on flexport since s.bus needs the mainport. - data.CC_MainPort = HwSettings::CC_MAINPORT_SBUS; - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_TELEMETRY; - break; - case VehicleConfigurationSource::INPUT_DSMX10: - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSMX10BIT; - break; - case VehicleConfigurationSource::INPUT_DSMX11: - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSMX11BIT; - break; - case VehicleConfigurationSource::INPUT_DSM2: - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM2; - break; - default: - break; - } + // Default mainport to be active telemetry link + data.CC_MainPort = HwSettings::CC_MAINPORT_TELEMETRY; + + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DISABLED; + switch (m_configSource->getInputType()) { + case VehicleConfigurationSource::INPUT_PWM: + data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PWM; break; - case VehicleConfigurationSource::CONTROLLER_REVO: - // Reset all ports - data.RM_RcvrPort = HwSettings::RM_RCVRPORT_DISABLED; - - //Default mainport to be active telemetry link - data.RM_MainPort = HwSettings::RM_MAINPORT_TELEMETRY; - - data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DISABLED; - switch(m_configSource->getInputType()) - { - case VehicleConfigurationSource::INPUT_PWM: - data.RM_RcvrPort = HwSettings::RM_RCVRPORT_PWM; - break; - case VehicleConfigurationSource::INPUT_PPM: - data.RM_RcvrPort = HwSettings::RM_RCVRPORT_PPM; - break; - case VehicleConfigurationSource::INPUT_SBUS: - // We have to set teletry on flexport since s.bus needs the mainport. - data.RM_MainPort = HwSettings::RM_MAINPORT_SBUS; - data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_TELEMETRY; - break; - case VehicleConfigurationSource::INPUT_DSMX10: - data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSMX10BIT; - break; - case VehicleConfigurationSource::INPUT_DSMX11: - data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSMX11BIT; - break; - case VehicleConfigurationSource::INPUT_DSM2: - data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM2; - break; - default: - break; - } + case VehicleConfigurationSource::INPUT_PPM: + data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PPM; + break; + case VehicleConfigurationSource::INPUT_SBUS: + // We have to set teletry on flexport since s.bus needs the mainport. + data.CC_MainPort = HwSettings::CC_MAINPORT_SBUS; + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_TELEMETRY; + break; + case VehicleConfigurationSource::INPUT_DSMX10: + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSMX10BIT; + break; + case VehicleConfigurationSource::INPUT_DSMX11: + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSMX11BIT; + break; + case VehicleConfigurationSource::INPUT_DSM2: + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM2; break; default: break; + } + break; + case VehicleConfigurationSource::CONTROLLER_REVO: + // Reset all ports + data.RM_RcvrPort = HwSettings::RM_RCVRPORT_DISABLED; + + // Default mainport to be active telemetry link + data.RM_MainPort = HwSettings::RM_MAINPORT_TELEMETRY; + + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DISABLED; + switch (m_configSource->getInputType()) { + case VehicleConfigurationSource::INPUT_PWM: + data.RM_RcvrPort = HwSettings::RM_RCVRPORT_PWM; + break; + case VehicleConfigurationSource::INPUT_PPM: + data.RM_RcvrPort = HwSettings::RM_RCVRPORT_PPM; + break; + case VehicleConfigurationSource::INPUT_SBUS: + // We have to set teletry on flexport since s.bus needs the mainport. + data.RM_MainPort = HwSettings::RM_MAINPORT_SBUS; + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_TELEMETRY; + break; + case VehicleConfigurationSource::INPUT_DSMX10: + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSMX10BIT; + break; + case VehicleConfigurationSource::INPUT_DSMX11: + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSMX11BIT; + break; + case VehicleConfigurationSource::INPUT_DSM2: + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM2; + break; + default: + break; + } + break; + default: + break; } hwSettings->setData(data); addModifiedObject(hwSettings, tr("Writing hardware settings")); @@ -188,127 +186,127 @@ void VehicleConfigurationHelper::applyHardwareConfiguration() void VehicleConfigurationHelper::applyVehicleConfiguration() { - - switch(m_configSource->getVehicleType()) + switch (m_configSource->getVehicleType()) { + case VehicleConfigurationSource::VEHICLE_MULTI: { - case VehicleConfigurationSource::VEHICLE_MULTI: - { - switch(m_configSource->getVehicleSubType()) - { - case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: - setupTriCopter(); - break; - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: - setupQuadCopter(); - break; - case VehicleConfigurationSource::MULTI_ROTOR_HEXA: - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: - setupHexaCopter(); - break; - case VehicleConfigurationSource::MULTI_ROTOR_OCTO: - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: - setupOctoCopter(); - break; - default: - break; - } + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: + setupTriCopter(); break; - } - case VehicleConfigurationSource::VEHICLE_FIXEDWING: - case VehicleConfigurationSource::VEHICLE_HELI: - case VehicleConfigurationSource::VEHICLE_SURFACE: - // TODO: Implement settings for other vehicle types? + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: + setupQuadCopter(); + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA: + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: + setupHexaCopter(); + break; + case VehicleConfigurationSource::MULTI_ROTOR_OCTO: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: + setupOctoCopter(); break; default: break; + } + break; + } + case VehicleConfigurationSource::VEHICLE_FIXEDWING: + case VehicleConfigurationSource::VEHICLE_HELI: + case VehicleConfigurationSource::VEHICLE_SURFACE: + // TODO: Implement settings for other vehicle types? + break; + default: + break; } } void VehicleConfigurationHelper::applyActuatorConfiguration() { - ActuatorSettings* actSettings = ActuatorSettings::GetInstance(m_uavoManager); - switch(m_configSource->getVehicleType()) { - case VehicleConfigurationSource::VEHICLE_MULTI: { - ActuatorSettings::DataFields data = actSettings->getData(); + ActuatorSettings *actSettings = ActuatorSettings::GetInstance(m_uavoManager); - QList actuatorSettings = m_configSource->getActuatorSettings(); - for(quint16 i = 0; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) { - data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM; - data.ChannelAddr[i] = i; - data.ChannelMin[i] = actuatorSettings[i].channelMin; - data.ChannelNeutral[i] = actuatorSettings[i].channelNeutral; - data.ChannelMax[i] = actuatorSettings[i].channelMax; - } + switch (m_configSource->getVehicleType()) { + case VehicleConfigurationSource::VEHICLE_MULTI: + { + ActuatorSettings::DataFields data = actSettings->getData(); - data.MotorsSpinWhileArmed = ActuatorSettings::MOTORSSPINWHILEARMED_FALSE; - - for(quint16 i = 0; i < ActuatorSettings::CHANNELUPDATEFREQ_NUMELEM; i++) { - data.ChannelUpdateFreq[i] = LEGACY_ESC_FREQUENCE; - } - - qint16 updateFrequence = LEGACY_ESC_FREQUENCE; - switch(m_configSource->getESCType()) { - case VehicleConfigurationSource::ESC_LEGACY: - updateFrequence = LEGACY_ESC_FREQUENCE; - break; - case VehicleConfigurationSource::ESC_RAPID: - updateFrequence = RAPID_ESC_FREQUENCE; - break; - default: - break; - } - - switch(m_configSource->getVehicleSubType()) { - case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: - data.ChannelUpdateFreq[0] = updateFrequence; - if(m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) { - data.ChannelUpdateFreq[1] = updateFrequence; - } - break; - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: - data.ChannelUpdateFreq[0] = updateFrequence; - data.ChannelUpdateFreq[1] = updateFrequence; - if(m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) { - data.ChannelUpdateFreq[2] = updateFrequence; - } - break; - case VehicleConfigurationSource::MULTI_ROTOR_HEXA: - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: - case VehicleConfigurationSource::MULTI_ROTOR_OCTO: - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: - data.ChannelUpdateFreq[0] = updateFrequence; - data.ChannelUpdateFreq[1] = updateFrequence; - data.ChannelUpdateFreq[2] = updateFrequence; - data.ChannelUpdateFreq[3] = updateFrequence; - break; - default: - break; - } - actSettings->setData(data); - addModifiedObject(actSettings, tr("Writing actuator settings")); - break; + QList actuatorSettings = m_configSource->getActuatorSettings(); + for (quint16 i = 0; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) { + data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM; + data.ChannelAddr[i] = i; + data.ChannelMin[i] = actuatorSettings[i].channelMin; + data.ChannelNeutral[i] = actuatorSettings[i].channelNeutral; + data.ChannelMax[i] = actuatorSettings[i].channelMax; } - case VehicleConfigurationSource::VEHICLE_FIXEDWING: - case VehicleConfigurationSource::VEHICLE_HELI: - case VehicleConfigurationSource::VEHICLE_SURFACE: - // TODO: Implement settings for other vehicle types? + + data.MotorsSpinWhileArmed = ActuatorSettings::MOTORSSPINWHILEARMED_FALSE; + + for (quint16 i = 0; i < ActuatorSettings::CHANNELUPDATEFREQ_NUMELEM; i++) { + data.ChannelUpdateFreq[i] = LEGACY_ESC_FREQUENCE; + } + + qint16 updateFrequence = LEGACY_ESC_FREQUENCE; + switch (m_configSource->getESCType()) { + case VehicleConfigurationSource::ESC_LEGACY: + updateFrequence = LEGACY_ESC_FREQUENCE; + break; + case VehicleConfigurationSource::ESC_RAPID: + updateFrequence = RAPID_ESC_FREQUENCE; break; default: break; + } + + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: + data.ChannelUpdateFreq[0] = updateFrequence; + if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) { + data.ChannelUpdateFreq[1] = updateFrequence; + } + break; + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: + data.ChannelUpdateFreq[0] = updateFrequence; + data.ChannelUpdateFreq[1] = updateFrequence; + if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) { + data.ChannelUpdateFreq[2] = updateFrequence; + } + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA: + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: + data.ChannelUpdateFreq[0] = updateFrequence; + data.ChannelUpdateFreq[1] = updateFrequence; + data.ChannelUpdateFreq[2] = updateFrequence; + data.ChannelUpdateFreq[3] = updateFrequence; + break; + default: + break; + } + actSettings->setData(data); + addModifiedObject(actSettings, tr("Writing actuator settings")); + break; + } + case VehicleConfigurationSource::VEHICLE_FIXEDWING: + case VehicleConfigurationSource::VEHICLE_HELI: + case VehicleConfigurationSource::VEHICLE_SURFACE: + // TODO: Implement settings for other vehicle types? + break; + default: + break; } } void VehicleConfigurationHelper::applyFlighModeConfiguration() { - ManualControlSettings* controlSettings = ManualControlSettings::GetInstance(m_uavoManager); + ManualControlSettings *controlSettings = ManualControlSettings::GetInstance(m_uavoManager); + Q_ASSERT(controlSettings); ManualControlSettings::DataFields data = controlSettings->getData(); @@ -322,31 +320,30 @@ void VehicleConfigurationHelper::applyFlighModeConfiguration() data.Stabilization3Settings[1] = ManualControlSettings::STABILIZATION3SETTINGS_RATE; data.Stabilization3Settings[2] = ManualControlSettings::STABILIZATION3SETTINGS_RATE; data.FlightModeNumber = 3; - data.FlightModePosition[0] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; - data.FlightModePosition[1] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; - data.FlightModePosition[2] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; - data.FlightModePosition[3] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; - data.FlightModePosition[4] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; - data.FlightModePosition[5] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[0] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[1] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[2] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[3] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[4] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[5] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; controlSettings->setData(data); addModifiedObject(controlSettings, tr("Writing flight mode settings")); } void VehicleConfigurationHelper::applySensorBiasConfiguration() { - if(m_configSource->isCalibrationPerformed()) - { + if (m_configSource->isCalibrationPerformed()) { accelGyroBias bias = m_configSource->getCalibrationBias(); float G = 9.81f; - switch(m_configSource->getControllerType()) { + switch (m_configSource->getControllerType()) { case VehicleConfigurationSource::CONTROLLER_CC: case VehicleConfigurationSource::CONTROLLER_CC3D: { const float ACCELERATION_SCALE = 0.004f * G; const float GYRO_SCALE = 100.0f; - AttitudeSettings* copterControlCalibration = AttitudeSettings::GetInstance(m_uavoManager); + AttitudeSettings *copterControlCalibration = AttitudeSettings::GetInstance(m_uavoManager); Q_ASSERT(copterControlCalibration); AttitudeSettings::DataFields data = copterControlCalibration->getData(); @@ -356,9 +353,9 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration() data.AccelBias[AttitudeSettings::ACCELBIAS_Y] += bias.m_accelerometerYBias / ACCELERATION_SCALE; data.AccelBias[AttitudeSettings::ACCELBIAS_Z] += (bias.m_accelerometerZBias + G) / ACCELERATION_SCALE; - data.GyroBias[AttitudeSettings::GYROBIAS_X] = -(bias.m_gyroXBias * GYRO_SCALE); - data.GyroBias[AttitudeSettings::GYROBIAS_Y] = -(bias.m_gyroYBias * GYRO_SCALE); - data.GyroBias[AttitudeSettings::GYROBIAS_Z] = -(bias.m_gyroZBias * GYRO_SCALE); + data.GyroBias[AttitudeSettings::GYROBIAS_X] = -(bias.m_gyroXBias * GYRO_SCALE); + data.GyroBias[AttitudeSettings::GYROBIAS_Y] = -(bias.m_gyroYBias * GYRO_SCALE); + data.GyroBias[AttitudeSettings::GYROBIAS_Z] = -(bias.m_gyroZBias * GYRO_SCALE); copterControlCalibration->setData(data); addModifiedObject(copterControlCalibration, tr("Writing gyro and accelerometer bias settings")); @@ -366,7 +363,7 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration() } case VehicleConfigurationSource::CONTROLLER_REVO: { - RevoCalibration* revolutionCalibration = RevoCalibration::GetInstance(m_uavoManager); + RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(m_uavoManager); Q_ASSERT(revolutionCalibration); RevoCalibration::DataFields data = revolutionCalibration->getData(); @@ -376,16 +373,16 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration() data.accel_bias[RevoCalibration::ACCEL_BIAS_Y] += bias.m_accelerometerYBias; data.accel_bias[RevoCalibration::ACCEL_BIAS_Z] += bias.m_accelerometerZBias + G; - data.gyro_bias[RevoCalibration::GYRO_BIAS_X] = bias.m_gyroXBias; - data.gyro_bias[RevoCalibration::GYRO_BIAS_Y] = bias.m_gyroYBias; - data.gyro_bias[RevoCalibration::GYRO_BIAS_Z] = bias.m_gyroZBias; + data.gyro_bias[RevoCalibration::GYRO_BIAS_X] = bias.m_gyroXBias; + data.gyro_bias[RevoCalibration::GYRO_BIAS_Y] = bias.m_gyroYBias; + data.gyro_bias[RevoCalibration::GYRO_BIAS_Z] = bias.m_gyroZBias; revolutionCalibration->setData(data); addModifiedObject(revolutionCalibration, tr("Writing gyro and accelerometer bias settings")); break; } default: - //Something went terribly wrong. + // Something went terribly wrong. break; } } @@ -393,7 +390,8 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration() void VehicleConfigurationHelper::applyStabilizationConfiguration() { - StabilizationSettings *stabSettings = StabilizationSettings::GetInstance(m_uavoManager); + StabilizationSettings *stabSettings = StabilizationSettings::GetInstance(m_uavoManager); + Q_ASSERT(stabSettings); StabilizationSettings::DataFields data = stabSettings->getData(); @@ -405,13 +403,14 @@ void VehicleConfigurationHelper::applyStabilizationConfiguration() void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings channels[]) { // Set all mixer data - MixerSettings* mSettings = MixerSettings::GetInstance(m_uavoManager); + MixerSettings *mSettings = MixerSettings::GetInstance(m_uavoManager); + Q_ASSERT(mSettings); // Set Mixer types and values - QString mixerTypePattern = "Mixer%1Type"; + QString mixerTypePattern = "Mixer%1Type"; QString mixerVectorPattern = "Mixer%1Vector"; - for(int i = 0; i < 10; i++) { + for (int i = 0; i < 10; i++) { UAVObjectField *field = mSettings->getField(mixerTypePattern.arg(i + 1)); Q_ASSERT(field); field->setValue(field->getOptions().at(channels[i].type)); @@ -422,18 +421,18 @@ void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings ch field->setValue((channels[i].throttle2 * 127) / 100, 1); field->setValue((channels[i].roll * 127) / 100, 2); field->setValue((channels[i].pitch * 127) / 100, 3); - field->setValue((channels[i].yaw *127) / 100, 4); + field->setValue((channels[i].yaw * 127) / 100, 4); } // Apply updates mSettings->setData(mSettings->getData()); addModifiedObject(mSettings, tr("Writing mixer settings")); - } void VehicleConfigurationHelper::applyMultiGUISettings(SystemSettings::AirframeTypeOptions airframe, GUIConfigDataUnion guiConfig) { - SystemSettings * sSettings = SystemSettings::GetInstance(m_uavoManager); + SystemSettings *sSettings = SystemSettings::GetInstance(m_uavoManager); + Q_ASSERT(sSettings); SystemSettings::DataFields data = sSettings->getData(); data.AirframeType = airframe; @@ -449,40 +448,40 @@ void VehicleConfigurationHelper::applyMultiGUISettings(SystemSettings::AirframeT void VehicleConfigurationHelper::applyManualControlDefaults() { ManualControlSettings *mcSettings = ManualControlSettings::GetInstance(m_uavoManager); + Q_ASSERT(mcSettings); ManualControlSettings::DataFields cData = mcSettings->getData(); ManualControlSettings::ChannelGroupsOptions channelType = ManualControlSettings::CHANNELGROUPS_PWM; - switch(m_configSource->getInputType()) - { - case VehicleConfigurationSource::INPUT_PWM: - channelType = ManualControlSettings::CHANNELGROUPS_PWM; - break; - case VehicleConfigurationSource::INPUT_PPM: - channelType = ManualControlSettings::CHANNELGROUPS_PPM; - break; - case VehicleConfigurationSource::INPUT_SBUS: - channelType = ManualControlSettings::CHANNELGROUPS_SBUS; - break; - case VehicleConfigurationSource::INPUT_DSMX10: - case VehicleConfigurationSource::INPUT_DSMX11: - case VehicleConfigurationSource::INPUT_DSM2: - channelType = ManualControlSettings::CHANNELGROUPS_DSMMAINPORT; - break; - default: - break; + switch (m_configSource->getInputType()) { + case VehicleConfigurationSource::INPUT_PWM: + channelType = ManualControlSettings::CHANNELGROUPS_PWM; + break; + case VehicleConfigurationSource::INPUT_PPM: + channelType = ManualControlSettings::CHANNELGROUPS_PPM; + break; + case VehicleConfigurationSource::INPUT_SBUS: + channelType = ManualControlSettings::CHANNELGROUPS_SBUS; + break; + case VehicleConfigurationSource::INPUT_DSMX10: + case VehicleConfigurationSource::INPUT_DSMX11: + case VehicleConfigurationSource::INPUT_DSM2: + channelType = ManualControlSettings::CHANNELGROUPS_DSMMAINPORT; + break; + default: + break; } - cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_THROTTLE] = channelType; - cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_ROLL] = channelType; + cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_THROTTLE] = channelType; + cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_ROLL] = channelType; cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_YAW] = channelType; - cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_PITCH] = channelType; + cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_PITCH] = channelType; cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_FLIGHTMODE] = channelType; - cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_THROTTLE] = 1; - cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_ROLL] = 2; + cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_THROTTLE] = 1; + cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_ROLL] = 2; cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_YAW] = 3; - cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_PITCH] = 4; + cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_PITCH] = 4; cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_FLIGHTMODE] = 5; mcSettings->setData(cData); @@ -497,7 +496,7 @@ bool VehicleConfigurationHelper::saveChangesToController(bool save) ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Q_ASSERT(pm); - UAVObjectUtilManager* utilMngr = pm->getObject(); + UAVObjectUtilManager *utilMngr = pm->getObject(); Q_ASSERT(utilMngr); QTimer outerTimeoutTimer; @@ -506,48 +505,47 @@ bool VehicleConfigurationHelper::saveChangesToController(bool save) QTimer innerTimeoutTimer; innerTimeoutTimer.setSingleShot(true); - connect(utilMngr, SIGNAL(saveCompleted(int ,bool)), this, SLOT(uAVOTransactionCompleted(int, bool))); + connect(utilMngr, SIGNAL(saveCompleted(int, bool)), this, SLOT(uAVOTransactionCompleted(int, bool))); connect(&innerTimeoutTimer, SIGNAL(timeout()), &m_eventLoop, SLOT(quit())); connect(&outerTimeoutTimer, SIGNAL(timeout()), this, SLOT(saveChangesTimeout())); outerTimeoutTimer.start(OUTER_TIMEOUT); - for(int i = 0; i < m_modifiedObjects.count(); i++) { - QPair *objPair = m_modifiedObjects.at(i); + for (int i = 0; i < m_modifiedObjects.count(); i++) { + QPair *objPair = m_modifiedObjects.at(i); m_transactionOK = false; - UAVDataObject* obj = objPair->first; + UAVDataObject *obj = objPair->first; QString objDescription = objPair->second; - if(UAVObject::GetGcsAccess(obj->getMetadata()) != UAVObject::ACCESS_READONLY && obj->isSettings()) { - + if (UAVObject::GetGcsAccess(obj->getMetadata()) != UAVObject::ACCESS_READONLY && obj->isSettings()) { emit saveProgress(m_modifiedObjects.count() + 1, ++m_progress, objDescription); m_currentTransactionObjectID = obj->getObjID(); - connect(obj, SIGNAL(transactionCompleted(UAVObject* ,bool)), this, SLOT(uAVOTransactionCompleted(UAVObject*, bool))); - while(!m_transactionOK && !m_transactionTimeout) { + connect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(uAVOTransactionCompleted(UAVObject *, bool))); + while (!m_transactionOK && !m_transactionTimeout) { // Allow the transaction to take some time innerTimeoutTimer.start(INNER_TIMEOUT); // Set object updated obj->updated(); - if(!m_transactionOK) { + if (!m_transactionOK) { m_eventLoop.exec(); } innerTimeoutTimer.stop(); } - disconnect(obj, SIGNAL(transactionCompleted(UAVObject* ,bool)), this, SLOT(uAVOTransactionCompleted(UAVObject*, bool))); - if(m_transactionOK) { + disconnect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(uAVOTransactionCompleted(UAVObject *, bool))); + if (m_transactionOK) { qDebug() << "Object " << obj->getName() << " was successfully updated."; - if(save) { + if (save) { m_transactionOK = false; m_currentTransactionObjectID = obj->getObjID(); // Try to save until success or timeout - while(!m_transactionOK && !m_transactionTimeout) { + while (!m_transactionOK && !m_transactionTimeout) { // Allow the transaction to take some time innerTimeoutTimer.start(INNER_TIMEOUT); // Persist object in controller utilMngr->saveObjectToSD(obj); - if(!m_transactionOK) { + if (!m_transactionOK) { m_eventLoop.exec(); } innerTimeoutTimer.stop(); @@ -556,17 +554,15 @@ bool VehicleConfigurationHelper::saveChangesToController(bool save) } } - if(!m_transactionOK) { + if (!m_transactionOK) { qDebug() << "Transaction timed out when trying to save: " << obj->getName(); - } - else { + } else { qDebug() << "Object " << obj->getName() << " was successfully saved."; } - } - else { + } else { qDebug() << "Trying to save a UAVDataObject that is read only or is not a settings object."; } - if(m_transactionTimeout) { + if (m_transactionTimeout) { qDebug() << "Transaction timed out when trying to save " << m_modifiedObjects.count() << " objects."; break; } @@ -584,8 +580,7 @@ bool VehicleConfigurationHelper::saveChangesToController(bool save) void VehicleConfigurationHelper::uAVOTransactionCompleted(int oid, bool success) { - if(oid == m_currentTransactionObjectID) - { + if (oid == m_currentTransactionObjectID) { m_transactionOK = success; m_eventLoop.quit(); } @@ -593,7 +588,7 @@ void VehicleConfigurationHelper::uAVOTransactionCompleted(int oid, bool success) void VehicleConfigurationHelper::uAVOTransactionCompleted(UAVObject *object, bool success) { - if(object) { + if (object) { uAVOTransactionCompleted(object->getObjID(), success); } } @@ -608,7 +603,7 @@ void VehicleConfigurationHelper::saveChangesTimeout() void VehicleConfigurationHelper::resetVehicleConfig() { // Reset all vehicle data - MixerSettings* mSettings = MixerSettings::GetInstance(m_uavoManager); + MixerSettings *mSettings = MixerSettings::GetInstance(m_uavoManager); // Reset feed forward, accel times etc mSettings->setFeedForward(0.0f); @@ -618,41 +613,42 @@ void VehicleConfigurationHelper::resetVehicleConfig() // Reset throttle curves QString throttlePattern = "ThrottleCurve%1"; - for(int i = 1; i <= 2; i++) { + for (int i = 1; i <= 2; i++) { UAVObjectField *field = mSettings->getField(throttlePattern.arg(i)); Q_ASSERT(field); - for(quint32 i = 0; i < field->getNumElements(); i++){ - field->setValue(i * ( 1.0f / (field->getNumElements() - 1)), i); + for (quint32 i = 0; i < field->getNumElements(); i++) { + field->setValue(i * (1.0f / (field->getNumElements() - 1)), i); } } // Reset Mixer types and values - QString mixerTypePattern = "Mixer%1Type"; + QString mixerTypePattern = "Mixer%1Type"; QString mixerVectorPattern = "Mixer%1Vector"; - for(int i = 1; i <= 10; i++) { + for (int i = 1; i <= 10; i++) { UAVObjectField *field = mSettings->getField(mixerTypePattern.arg(i)); Q_ASSERT(field); field->setValue(field->getOptions().at(0)); field = mSettings->getField(mixerVectorPattern.arg(i)); Q_ASSERT(field); - for(quint32 i = 0; i < field->getNumElements(); i++){ + for (quint32 i = 0; i < field->getNumElements(); i++) { field->setValue(0, i); } } // Apply updates - //mSettings->setData(mSettings->getData()); + // mSettings->setData(mSettings->getData()); addModifiedObject(mSettings, tr("Preparing mixer settings")); } void VehicleConfigurationHelper::resetGUIData() { - SystemSettings * sSettings = SystemSettings::GetInstance(m_uavoManager); + SystemSettings *sSettings = SystemSettings::GetInstance(m_uavoManager); + Q_ASSERT(sSettings); SystemSettings::DataFields data = sSettings->getData(); data.AirframeType = SystemSettings::AIRFRAMETYPE_CUSTOM; - for(quint32 i = 0; i < SystemSettings::GUICONFIGDATA_NUMELEM; i++) { + for (quint32 i = 0; i < SystemSettings::GUICONFIGDATA_NUMELEM; i++) { data.GUIConfigData[i] = 0; } sSettings->setData(data); @@ -670,37 +666,37 @@ void VehicleConfigurationHelper::setupTriCopter() mixerChannelSettings channels[10]; GUIConfigDataUnion guiSettings = getGUIConfigData(); - channels[0].type = MIXER_TYPE_MOTOR; + channels[0].type = MIXER_TYPE_MOTOR; channels[0].throttle1 = 100; channels[0].throttle2 = 0; - channels[0].roll = 100; - channels[0].pitch = 50; + channels[0].roll = 100; + channels[0].pitch = 50; channels[0].yaw = 0; - channels[1].type = MIXER_TYPE_MOTOR; + channels[1].type = MIXER_TYPE_MOTOR; channels[1].throttle1 = 100; channels[1].throttle2 = 0; - channels[1].roll = -100; - channels[1].pitch = 50; + channels[1].roll = -100; + channels[1].pitch = 50; channels[1].yaw = 0; - channels[2].type = MIXER_TYPE_MOTOR; + channels[2].type = MIXER_TYPE_MOTOR; channels[2].throttle1 = 100; channels[2].throttle2 = 0; - channels[2].roll = 0; - channels[2].pitch = -100; + channels[2].roll = 0; + channels[2].pitch = -100; channels[2].yaw = 0; - channels[3].type = MIXER_TYPE_SERVO; + channels[3].type = MIXER_TYPE_SERVO; channels[3].throttle1 = 0; channels[3].throttle2 = 0; - channels[3].roll = 0; - channels[3].pitch = 0; + channels[3].roll = 0; + channels[3].pitch = 0; channels[3].yaw = 100; guiSettings.multi.VTOLMotorNW = 1; guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorS = 3; + guiSettings.multi.VTOLMotorS = 3; guiSettings.multi.TRIYaw = 4; applyMixerConfiguration(channels); @@ -711,12 +707,13 @@ GUIConfigDataUnion VehicleConfigurationHelper::getGUIConfigData() { GUIConfigDataUnion configData; - SystemSettings * systemSettings = SystemSettings::GetInstance(m_uavoManager); + SystemSettings *systemSettings = SystemSettings::GetInstance(m_uavoManager); + Q_ASSERT(systemSettings); SystemSettings::DataFields systemSettingsData = systemSettings->getData(); - for(int i = 0; i < (int)(SystemSettings::GUICONFIGDATA_NUMELEM); i++) { - configData.UAVObject[i] = 0; //systemSettingsData.GUIConfigData[i]; + for (int i = 0; i < (int)(SystemSettings::GUICONFIGDATA_NUMELEM); i++) { + configData.UAVObject[i] = 0; // systemSettingsData.GUIConfigData[i]; } return configData; @@ -728,84 +725,85 @@ void VehicleConfigurationHelper::setupQuadCopter() GUIConfigDataUnion guiSettings = getGUIConfigData(); SystemSettings::AirframeTypeOptions frame; - switch(m_configSource->getVehicleSubType()) + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: { - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: { - frame = SystemSettings::AIRFRAMETYPE_QUADP; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 0; - channels[0].pitch = 100; - channels[0].yaw = -50; + frame = SystemSettings::AIRFRAMETYPE_QUADP; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 0; + channels[0].pitch = 100; + channels[0].yaw = -50; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = -100; - channels[1].pitch = 0; - channels[1].yaw = 50; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -100; + channels[1].pitch = 0; + channels[1].yaw = 50; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = 0; - channels[2].pitch = -100; - channels[2].yaw = -50; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = 0; + channels[2].pitch = -100; + channels[2].yaw = -50; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = 100; - channels[3].pitch = 0; - channels[3].yaw = 50; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = 100; + channels[3].pitch = 0; + channels[3].yaw = 50; - guiSettings.multi.VTOLMotorN = 1; - guiSettings.multi.VTOLMotorE = 2; - guiSettings.multi.VTOLMotorS = 3; - guiSettings.multi.VTOLMotorW = 4; + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorE = 2; + guiSettings.multi.VTOLMotorS = 3; + guiSettings.multi.VTOLMotorW = 4; - break; - } - case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: { - frame = SystemSettings::AIRFRAMETYPE_QUADX; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 50; - channels[0].pitch = 50; - channels[0].yaw = -50; + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: + { + frame = SystemSettings::AIRFRAMETYPE_QUADX; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 50; + channels[0].pitch = 50; + channels[0].yaw = -50; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = -50; - channels[1].pitch = 50; - channels[1].yaw = 50; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -50; + channels[1].pitch = 50; + channels[1].yaw = 50; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -50; - channels[2].pitch = -50; - channels[2].yaw = -50; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -50; + channels[2].pitch = -50; + channels[2].yaw = -50; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = 50; - channels[3].pitch = -50; - channels[3].yaw = 50; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = 50; + channels[3].pitch = -50; + channels[3].yaw = 50; - guiSettings.multi.VTOLMotorNW = 1; - guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorSE = 3; - guiSettings.multi.VTOLMotorSW = 4; + guiSettings.multi.VTOLMotorNW = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorSE = 3; + guiSettings.multi.VTOLMotorSW = 4; - break; - } - default: - break; + break; + } + default: + break; } applyMixerConfiguration(channels); applyMultiGUISettings(frame, guiSettings); @@ -817,172 +815,174 @@ void VehicleConfigurationHelper::setupHexaCopter() GUIConfigDataUnion guiSettings = getGUIConfigData(); SystemSettings::AirframeTypeOptions frame; - switch(m_configSource->getVehicleSubType()) + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::MULTI_ROTOR_HEXA: { - case VehicleConfigurationSource::MULTI_ROTOR_HEXA: { - frame = SystemSettings::AIRFRAMETYPE_HEXA; + frame = SystemSettings::AIRFRAMETYPE_HEXA; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 0; - channels[0].pitch = 33; - channels[0].yaw = -33; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 0; + channels[0].pitch = 33; + channels[0].yaw = -33; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = -50; - channels[1].pitch = 33; - channels[1].yaw = 33; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -50; + channels[1].pitch = 33; + channels[1].yaw = 33; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -50; - channels[2].pitch = -33; - channels[2].yaw = -33; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -50; + channels[2].pitch = -33; + channels[2].yaw = -33; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = 0; - channels[3].pitch = -33; - channels[3].yaw = 33; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = 0; + channels[3].pitch = -33; + channels[3].yaw = 33; - channels[4].type = MIXER_TYPE_MOTOR; - channels[4].throttle1 = 100; - channels[4].throttle2 = 0; - channels[4].roll = 50; - channels[4].pitch = -33; - channels[4].yaw = -33; + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 50; + channels[4].pitch = -33; + channels[4].yaw = -33; - channels[5].type = MIXER_TYPE_MOTOR; - channels[5].throttle1 = 100; - channels[5].throttle2 = 0; - channels[5].roll = 50; - channels[5].pitch = 33; - channels[5].yaw = 33; + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 50; + channels[5].pitch = 33; + channels[5].yaw = 33; - guiSettings.multi.VTOLMotorN = 1; - guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorSE = 3; - guiSettings.multi.VTOLMotorS = 4; - guiSettings.multi.VTOLMotorSW = 5; - guiSettings.multi.VTOLMotorNW = 6; + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorSE = 3; + guiSettings.multi.VTOLMotorS = 4; + guiSettings.multi.VTOLMotorSW = 5; + guiSettings.multi.VTOLMotorNW = 6; - break; - } - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: { - frame = SystemSettings::AIRFRAMETYPE_HEXACOAX; + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: + { + frame = SystemSettings::AIRFRAMETYPE_HEXACOAX; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 100; - channels[0].pitch = 25; - channels[0].yaw = -66; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 100; + channels[0].pitch = 25; + channels[0].yaw = -66; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = 100; - channels[1].pitch = 25; - channels[1].yaw = 66; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = 100; + channels[1].pitch = 25; + channels[1].yaw = 66; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -100; - channels[2].pitch = 25; - channels[2].yaw = -66; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -100; + channels[2].pitch = 25; + channels[2].yaw = -66; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = -100; - channels[3].pitch = 25; - channels[3].yaw = 66; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -100; + channels[3].pitch = 25; + channels[3].yaw = 66; - channels[4].type = MIXER_TYPE_MOTOR; - channels[4].throttle1 = 100; - channels[4].throttle2 = 0; - channels[4].roll = 0; - channels[4].pitch = -50; - channels[4].yaw = -66; + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 0; + channels[4].pitch = -50; + channels[4].yaw = -66; - channels[5].type = MIXER_TYPE_MOTOR; - channels[5].throttle1 = 100; - channels[5].throttle2 = 0; - channels[5].roll = 0; - channels[5].pitch = -50; - channels[5].yaw = 66; + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 0; + channels[5].pitch = -50; + channels[5].yaw = 66; - guiSettings.multi.VTOLMotorNW = 1; - guiSettings.multi.VTOLMotorW = 2; - guiSettings.multi.VTOLMotorNE = 3; - guiSettings.multi.VTOLMotorE = 4; - guiSettings.multi.VTOLMotorS = 5; - guiSettings.multi.VTOLMotorSE = 6; + guiSettings.multi.VTOLMotorNW = 1; + guiSettings.multi.VTOLMotorW = 2; + guiSettings.multi.VTOLMotorNE = 3; + guiSettings.multi.VTOLMotorE = 4; + guiSettings.multi.VTOLMotorS = 5; + guiSettings.multi.VTOLMotorSE = 6; - break; - } - case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: { - frame = SystemSettings::AIRFRAMETYPE_HEXAX; + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: + { + frame = SystemSettings::AIRFRAMETYPE_HEXAX; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = -33; - channels[0].pitch = 50; - channels[0].yaw = -33; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = -33; + channels[0].pitch = 50; + channels[0].yaw = -33; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = -33; - channels[1].pitch = 0; - channels[1].yaw = 33; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -33; + channels[1].pitch = 0; + channels[1].yaw = 33; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -33; - channels[2].pitch = -50; - channels[2].yaw = -33; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -33; + channels[2].pitch = -50; + channels[2].yaw = -33; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = -33; - channels[3].pitch = -50; - channels[3].yaw = 33; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -33; + channels[3].pitch = -50; + channels[3].yaw = 33; - channels[4].type = MIXER_TYPE_MOTOR; - channels[4].throttle1 = 100; - channels[4].throttle2 = 0; - channels[4].roll = 33; - channels[4].pitch = 0; - channels[4].yaw = -33; + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 33; + channels[4].pitch = 0; + channels[4].yaw = -33; - channels[5].type = MIXER_TYPE_MOTOR; - channels[5].throttle1 = 100; - channels[5].throttle2 = 0; - channels[5].roll = 33; - channels[5].pitch = 50; - channels[5].yaw = -33; + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 33; + channels[5].pitch = 50; + channels[5].yaw = -33; - guiSettings.multi.VTOLMotorNE = 1; - guiSettings.multi.VTOLMotorE = 2; - guiSettings.multi.VTOLMotorSE = 3; - guiSettings.multi.VTOLMotorSW = 4; - guiSettings.multi.VTOLMotorW = 5; - guiSettings.multi.VTOLMotorNW = 6; + guiSettings.multi.VTOLMotorNE = 1; + guiSettings.multi.VTOLMotorE = 2; + guiSettings.multi.VTOLMotorSE = 3; + guiSettings.multi.VTOLMotorSW = 4; + guiSettings.multi.VTOLMotorW = 5; + guiSettings.multi.VTOLMotorNW = 6; - break; - } - default: - break; + break; + } + default: + break; } applyMixerConfiguration(channels); applyMultiGUISettings(frame, guiSettings); @@ -994,289 +994,292 @@ void VehicleConfigurationHelper::setupOctoCopter() GUIConfigDataUnion guiSettings = getGUIConfigData(); SystemSettings::AirframeTypeOptions frame; - switch(m_configSource->getVehicleSubType()) + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::MULTI_ROTOR_OCTO: { - case VehicleConfigurationSource::MULTI_ROTOR_OCTO: { - frame = SystemSettings::AIRFRAMETYPE_OCTO; + frame = SystemSettings::AIRFRAMETYPE_OCTO; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 0; - channels[0].pitch = 33; - channels[0].yaw = -25; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 0; + channels[0].pitch = 33; + channels[0].yaw = -25; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = -33; - channels[1].pitch = 33; - channels[1].yaw = 25; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -33; + channels[1].pitch = 33; + channels[1].yaw = 25; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -33; - channels[2].pitch = 0; - channels[2].yaw = -25; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -33; + channels[2].pitch = 0; + channels[2].yaw = -25; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = -33; - channels[3].pitch = -33; - channels[3].yaw = 25; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -33; + channels[3].pitch = -33; + channels[3].yaw = 25; - channels[4].type = MIXER_TYPE_MOTOR; - channels[4].throttle1 = 100; - channels[4].throttle2 = 0; - channels[4].roll = 0; - channels[4].pitch = -33; - channels[4].yaw = -25; + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 0; + channels[4].pitch = -33; + channels[4].yaw = -25; - channels[5].type = MIXER_TYPE_MOTOR; - channels[5].throttle1 = 100; - channels[5].throttle2 = 0; - channels[5].roll = 33; - channels[5].pitch = -33; - channels[5].yaw = 25; + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 33; + channels[5].pitch = -33; + channels[5].yaw = 25; - channels[6].type = MIXER_TYPE_MOTOR; - channels[6].throttle1 = 100; - channels[6].throttle2 = 0; - channels[6].roll = 33; - channels[6].pitch = 0; - channels[6].yaw = -25; + channels[6].type = MIXER_TYPE_MOTOR; + channels[6].throttle1 = 100; + channels[6].throttle2 = 0; + channels[6].roll = 33; + channels[6].pitch = 0; + channels[6].yaw = -25; - channels[7].type = MIXER_TYPE_MOTOR; - channels[7].throttle1 = 100; - channels[7].throttle2 = 0; - channels[7].roll = 33; - channels[7].pitch = 33; - channels[7].yaw = 25; + channels[7].type = MIXER_TYPE_MOTOR; + channels[7].throttle1 = 100; + channels[7].throttle2 = 0; + channels[7].roll = 33; + channels[7].pitch = 33; + channels[7].yaw = 25; - guiSettings.multi.VTOLMotorN = 1; - guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorE = 3; - guiSettings.multi.VTOLMotorSE = 4; - guiSettings.multi.VTOLMotorS = 5; - guiSettings.multi.VTOLMotorSW = 6; - guiSettings.multi.VTOLMotorW = 7; - guiSettings.multi.VTOLMotorNW = 8; + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorE = 3; + guiSettings.multi.VTOLMotorSE = 4; + guiSettings.multi.VTOLMotorS = 5; + guiSettings.multi.VTOLMotorSW = 6; + guiSettings.multi.VTOLMotorW = 7; + guiSettings.multi.VTOLMotorNW = 8; - break; - } - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: { - frame = SystemSettings::AIRFRAMETYPE_OCTOCOAXX; + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: + { + frame = SystemSettings::AIRFRAMETYPE_OCTOCOAXX; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 50; - channels[0].pitch = 50; - channels[0].yaw = -50; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 50; + channels[0].pitch = 50; + channels[0].yaw = -50; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = 50; - channels[1].pitch = 50; - channels[1].yaw = 50; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = 50; + channels[1].pitch = 50; + channels[1].yaw = 50; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -50; - channels[2].pitch = 50; - channels[2].yaw = -50; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -50; + channels[2].pitch = 50; + channels[2].yaw = -50; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = -50; - channels[3].pitch = 50; - channels[3].yaw = 50; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -50; + channels[3].pitch = 50; + channels[3].yaw = 50; - channels[4].type = MIXER_TYPE_MOTOR; - channels[4].throttle1 = 100; - channels[4].throttle2 = 0; - channels[4].roll = -50; - channels[4].pitch = -50; - channels[4].yaw = -50; + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = -50; + channels[4].pitch = -50; + channels[4].yaw = -50; - channels[5].type = MIXER_TYPE_MOTOR; - channels[5].throttle1 = 100; - channels[5].throttle2 = 0; - channels[5].roll = -50; - channels[5].pitch = -50; - channels[5].yaw = 50; + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = -50; + channels[5].pitch = -50; + channels[5].yaw = 50; - channels[6].type = MIXER_TYPE_MOTOR; - channels[6].throttle1 = 100; - channels[6].throttle2 = 0; - channels[6].roll = 50; - channels[6].pitch = -50; - channels[6].yaw = -50; + channels[6].type = MIXER_TYPE_MOTOR; + channels[6].throttle1 = 100; + channels[6].throttle2 = 0; + channels[6].roll = 50; + channels[6].pitch = -50; + channels[6].yaw = -50; - channels[7].type = MIXER_TYPE_MOTOR; - channels[7].throttle1 = 100; - channels[7].throttle2 = 0; - channels[7].roll = 50; - channels[7].pitch = -50; - channels[7].yaw = 50; + channels[7].type = MIXER_TYPE_MOTOR; + channels[7].throttle1 = 100; + channels[7].throttle2 = 0; + channels[7].roll = 50; + channels[7].pitch = -50; + channels[7].yaw = 50; - guiSettings.multi.VTOLMotorNW = 1; - guiSettings.multi.VTOLMotorN = 2; - guiSettings.multi.VTOLMotorNE = 3; - guiSettings.multi.VTOLMotorE = 4; - guiSettings.multi.VTOLMotorSE = 5; - guiSettings.multi.VTOLMotorS = 6; - guiSettings.multi.VTOLMotorSW = 7; - guiSettings.multi.VTOLMotorW = 8; + guiSettings.multi.VTOLMotorNW = 1; + guiSettings.multi.VTOLMotorN = 2; + guiSettings.multi.VTOLMotorNE = 3; + guiSettings.multi.VTOLMotorE = 4; + guiSettings.multi.VTOLMotorSE = 5; + guiSettings.multi.VTOLMotorS = 6; + guiSettings.multi.VTOLMotorSW = 7; + guiSettings.multi.VTOLMotorW = 8; - break; - } - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: { - frame = SystemSettings::AIRFRAMETYPE_OCTOCOAXP; + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: + { + frame = SystemSettings::AIRFRAMETYPE_OCTOCOAXP; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 0; - channels[0].pitch = 100; - channels[0].yaw = -50; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 0; + channels[0].pitch = 100; + channels[0].yaw = -50; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = 0; - channels[1].pitch = 100; - channels[1].yaw = 50; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = 0; + channels[1].pitch = 100; + channels[1].yaw = 50; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -100; - channels[2].pitch = 0; - channels[2].yaw = -50; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -100; + channels[2].pitch = 0; + channels[2].yaw = -50; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = -100; - channels[3].pitch = 0; - channels[3].yaw = 50; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -100; + channels[3].pitch = 0; + channels[3].yaw = 50; - channels[4].type = MIXER_TYPE_MOTOR; - channels[4].throttle1 = 100; - channels[4].throttle2 = 0; - channels[4].roll = 0; - channels[4].pitch = -100; - channels[4].yaw = -50; + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 0; + channels[4].pitch = -100; + channels[4].yaw = -50; - channels[5].type = MIXER_TYPE_MOTOR; - channels[5].throttle1 = 100; - channels[5].throttle2 = 0; - channels[5].roll = 0; - channels[5].pitch = -100; - channels[5].yaw = 50; + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 0; + channels[5].pitch = -100; + channels[5].yaw = 50; - channels[6].type = MIXER_TYPE_MOTOR; - channels[6].throttle1 = 100; - channels[6].throttle2 = 0; - channels[6].roll = 100; - channels[6].pitch = 0; - channels[6].yaw = -50; + channels[6].type = MIXER_TYPE_MOTOR; + channels[6].throttle1 = 100; + channels[6].throttle2 = 0; + channels[6].roll = 100; + channels[6].pitch = 0; + channels[6].yaw = -50; - channels[7].type = MIXER_TYPE_MOTOR; - channels[7].throttle1 = 100; - channels[7].throttle2 = 0; - channels[7].roll = 100; - channels[7].pitch = 0; - channels[7].yaw = 50; + channels[7].type = MIXER_TYPE_MOTOR; + channels[7].throttle1 = 100; + channels[7].throttle2 = 0; + channels[7].roll = 100; + channels[7].pitch = 0; + channels[7].yaw = 50; - guiSettings.multi.VTOLMotorN = 1; - guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorE = 3; - guiSettings.multi.VTOLMotorSE = 4; - guiSettings.multi.VTOLMotorS = 5; - guiSettings.multi.VTOLMotorSW = 6; - guiSettings.multi.VTOLMotorW = 7; - guiSettings.multi.VTOLMotorNW = 8; + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorE = 3; + guiSettings.multi.VTOLMotorSE = 4; + guiSettings.multi.VTOLMotorS = 5; + guiSettings.multi.VTOLMotorSW = 6; + guiSettings.multi.VTOLMotorW = 7; + guiSettings.multi.VTOLMotorNW = 8; - break; - } - case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: { - frame = SystemSettings::AIRFRAMETYPE_OCTOV; - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = -25; - channels[0].pitch = 8; - channels[0].yaw = -25; + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: + { + frame = SystemSettings::AIRFRAMETYPE_OCTOV; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = -25; + channels[0].pitch = 8; + channels[0].yaw = -25; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = -25; - channels[1].pitch = 25; - channels[1].yaw = 25; + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -25; + channels[1].pitch = 25; + channels[1].yaw = 25; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = -25; - channels[2].pitch = -25; - channels[2].yaw = -25; + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -25; + channels[2].pitch = -25; + channels[2].yaw = -25; - channels[3].type = MIXER_TYPE_MOTOR; - channels[3].throttle1 = 100; - channels[3].throttle2 = 0; - channels[3].roll = -25; - channels[3].pitch = -8; - channels[3].yaw = 25; + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -25; + channels[3].pitch = -8; + channels[3].yaw = 25; - channels[4].type = MIXER_TYPE_MOTOR; - channels[4].throttle1 = 100; - channels[4].throttle2 = 0; - channels[4].roll = 25; - channels[4].pitch = -8; - channels[4].yaw = -25; + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 25; + channels[4].pitch = -8; + channels[4].yaw = -25; - channels[5].type = MIXER_TYPE_MOTOR; - channels[5].throttle1 = 100; - channels[5].throttle2 = 0; - channels[5].roll = 25; - channels[5].pitch = -25; - channels[5].yaw = 25; + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 25; + channels[5].pitch = -25; + channels[5].yaw = 25; - channels[6].type = MIXER_TYPE_MOTOR; - channels[6].throttle1 = 100; - channels[6].throttle2 = 0; - channels[6].roll = 25; - channels[6].pitch = 25; - channels[6].yaw = -25; + channels[6].type = MIXER_TYPE_MOTOR; + channels[6].throttle1 = 100; + channels[6].throttle2 = 0; + channels[6].roll = 25; + channels[6].pitch = 25; + channels[6].yaw = -25; - channels[7].type = MIXER_TYPE_MOTOR; - channels[7].throttle1 = 100; - channels[7].throttle2 = 0; - channels[7].roll = 25; - channels[7].pitch = 8; - channels[7].yaw = 25; + channels[7].type = MIXER_TYPE_MOTOR; + channels[7].throttle1 = 100; + channels[7].throttle2 = 0; + channels[7].roll = 25; + channels[7].pitch = 8; + channels[7].yaw = 25; - guiSettings.multi.VTOLMotorN = 1; - guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorE = 3; - guiSettings.multi.VTOLMotorSE = 4; - guiSettings.multi.VTOLMotorS = 5; - guiSettings.multi.VTOLMotorSW = 6; - guiSettings.multi.VTOLMotorW = 7; - guiSettings.multi.VTOLMotorNW = 8; + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorE = 3; + guiSettings.multi.VTOLMotorSE = 4; + guiSettings.multi.VTOLMotorS = 5; + guiSettings.multi.VTOLMotorSW = 6; + guiSettings.multi.VTOLMotorW = 7; + guiSettings.multi.VTOLMotorNW = 8; - break; - } - default: - break; + break; + } + default: + break; } applyMixerConfiguration(channels); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h index d890db66a..1ed1aa576 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h @@ -50,12 +50,11 @@ struct mixerChannelSettings { : type(t), throttle1(th1), throttle2(th2), roll(r), pitch(p), yaw(y) {} }; -class VehicleConfigurationHelper : public QObject -{ +class VehicleConfigurationHelper : public QObject { Q_OBJECT public: - VehicleConfigurationHelper(VehicleConfigurationSource* configSource); + VehicleConfigurationHelper(VehicleConfigurationSource *configSource); bool setupVehicle(bool save = true); bool setupHardwareSettings(bool save = true); static const qint16 LEGACY_ESC_FREQUENCE; @@ -66,15 +65,15 @@ signals: private: static const int MIXER_TYPE_DISABLED = 0; - static const int MIXER_TYPE_MOTOR = 1; - static const int MIXER_TYPE_SERVO = 2; + static const int MIXER_TYPE_MOTOR = 1; + static const int MIXER_TYPE_SERVO = 2; static const float DEFAULT_ENABLED_ACCEL_TAU = 0.1; VehicleConfigurationSource *m_configSource; UAVObjectManager *m_uavoManager; - QList* > m_modifiedObjects; - void addModifiedObject(UAVDataObject* object, QString description); + QList * > m_modifiedObjects; + void addModifiedObject(UAVDataObject *object, QString description); void clearModifiedObjects(); void applyHardwareConfiguration(); @@ -106,11 +105,9 @@ private: void setupOctoCopter(); private slots: - void uAVOTransactionCompleted(UAVObject* object, bool success); + void uAVOTransactionCompleted(UAVObject *object, bool success); void uAVOTransactionCompleted(int oid, bool success); void saveChangesTimeout(); - - }; #endif // VEHICLECONFIGURATIONHELPER_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.cpp index 3acb2400d..75b92a023 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.cpp @@ -28,5 +28,4 @@ #include "vehicleconfigurationsource.h" VehicleConfigurationSource::VehicleConfigurationSource() -{ -} +{} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h index a51dea708..98f06a991 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -46,35 +46,34 @@ struct actuatorChannelSettings { quint16 channelNeutral; quint16 channelMax; - //Default values - actuatorChannelSettings(): channelMin(1000), channelNeutral(1000), channelMax(1900) {} + // Default values + actuatorChannelSettings() : channelMin(1000), channelNeutral(1000), channelMax(1900) {} }; -class VehicleConfigurationSource -{ +class VehicleConfigurationSource { public: VehicleConfigurationSource(); - enum CONTROLLER_TYPE {CONTROLLER_UNKNOWN, CONTROLLER_CC, CONTROLLER_CC3D, CONTROLLER_REVO, CONTROLLER_OPLINK}; - enum VEHICLE_TYPE {VEHICLE_UNKNOWN, VEHICLE_MULTI, VEHICLE_FIXEDWING, VEHICLE_HELI, VEHICLE_SURFACE}; - enum VEHICLE_SUB_TYPE {MULTI_ROTOR_UNKNOWN, MULTI_ROTOR_TRI_Y, MULTI_ROTOR_QUAD_X, MULTI_ROTOR_QUAD_PLUS, - MULTI_ROTOR_HEXA, MULTI_ROTOR_HEXA_H, MULTI_ROTOR_HEXA_COAX_Y, MULTI_ROTOR_OCTO, - MULTI_ROTOR_OCTO_V, MULTI_ROTOR_OCTO_COAX_X, MULTI_ROTOR_OCTO_COAX_PLUS, FIXED_WING_AILERON, - FIXED_WING_VTAIL, HELI_CCPM}; - enum ESC_TYPE {ESC_RAPID, ESC_LEGACY, ESC_UNKNOWN}; - enum INPUT_TYPE {INPUT_PWM, INPUT_PPM, INPUT_SBUS, INPUT_DSMX10, INPUT_DSMX11, INPUT_DSM2, INPUT_UNKNOWN}; + enum CONTROLLER_TYPE { CONTROLLER_UNKNOWN, CONTROLLER_CC, CONTROLLER_CC3D, CONTROLLER_REVO, CONTROLLER_OPLINK }; + enum VEHICLE_TYPE { VEHICLE_UNKNOWN, VEHICLE_MULTI, VEHICLE_FIXEDWING, VEHICLE_HELI, VEHICLE_SURFACE }; + enum VEHICLE_SUB_TYPE { MULTI_ROTOR_UNKNOWN, MULTI_ROTOR_TRI_Y, MULTI_ROTOR_QUAD_X, MULTI_ROTOR_QUAD_PLUS, + MULTI_ROTOR_HEXA, MULTI_ROTOR_HEXA_H, MULTI_ROTOR_HEXA_COAX_Y, MULTI_ROTOR_OCTO, + MULTI_ROTOR_OCTO_V, MULTI_ROTOR_OCTO_COAX_X, MULTI_ROTOR_OCTO_COAX_PLUS, FIXED_WING_AILERON, + FIXED_WING_VTAIL, HELI_CCPM }; + enum ESC_TYPE { ESC_RAPID, ESC_LEGACY, ESC_UNKNOWN }; + enum INPUT_TYPE { INPUT_PWM, INPUT_PPM, INPUT_SBUS, INPUT_DSMX10, INPUT_DSMX11, INPUT_DSM2, INPUT_UNKNOWN }; - enum GPS_SETTING {GPS_UBX, GPS_NMEA, GPS_DISABLED}; - enum RADIO_SETTING {RADIO_TELEMETRY, RADIO_DISABLED}; + enum GPS_SETTING { GPS_UBX, GPS_NMEA, GPS_DISABLED }; + enum RADIO_SETTING { RADIO_TELEMETRY, RADIO_DISABLED }; - virtual VehicleConfigurationSource::CONTROLLER_TYPE getControllerType() const = 0; + virtual VehicleConfigurationSource::CONTROLLER_TYPE getControllerType() const = 0; virtual VehicleConfigurationSource::VEHICLE_TYPE getVehicleType() const = 0; virtual VehicleConfigurationSource::VEHICLE_SUB_TYPE getVehicleSubType() const = 0; - virtual VehicleConfigurationSource::INPUT_TYPE getInputType() const = 0; - virtual VehicleConfigurationSource::ESC_TYPE getESCType() const = 0; + virtual VehicleConfigurationSource::INPUT_TYPE getInputType() const = 0; + virtual VehicleConfigurationSource::ESC_TYPE getESCType() const = 0; - virtual VehicleConfigurationSource::GPS_SETTING getGPSSetting() const = 0; + virtual VehicleConfigurationSource::GPS_SETTING getGPSSetting() const = 0; virtual VehicleConfigurationSource::RADIO_SETTING getRadioSetting() const = 0; virtual bool isCalibrationPerformed() const = 0; @@ -85,7 +84,7 @@ public: virtual bool isRestartNeeded() const = 0; - virtual QString getSummaryText() = 0; + virtual QString getSummaryText() = 0; }; #endif // VEHICLECONFIGURATIONSOURCE_H diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.cpp b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.cpp index fe7406f6c..c58de58b0 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.cpp +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.cpp @@ -30,10 +30,9 @@ #include "systemhealthgadgetconfiguration.h" SystemHealthGadget::SystemHealthGadget(QString classId, SystemHealthGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} + IUAVGadget(classId, parent), + m_widget(widget) +{} SystemHealthGadget::~SystemHealthGadget() { @@ -41,13 +40,14 @@ SystemHealthGadget::~SystemHealthGadget() } /* - This is called when a configuration is loaded, and updates the plugin's settings. - Careful: the plugin is already drawn before the loadConfiguration method is called the - first time, so you have to be careful not to assume all the plugin values are initialized - the first time you use them + This is called when a configuration is loaded, and updates the plugin's settings. + Careful: the plugin is already drawn before the loadConfiguration method is called the + first time, so you have to be careful not to assume all the plugin values are initialized + the first time you use them */ -void SystemHealthGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void SystemHealthGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - SystemHealthGadgetConfiguration *m = qobject_cast(config); + SystemHealthGadgetConfiguration *m = qobject_cast(config); + m_widget->setSystemFile(m->getSystemFile()); // Triggers widget repaint } diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.h b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.h index 2884fe14e..bcd4870f5 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.h +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadget.h @@ -38,15 +38,17 @@ class SystemHealthGadgetWidget; using namespace Core; -class SystemHealthGadget : public Core::IUAVGadget -{ +class SystemHealthGadget : public Core::IUAVGadget { Q_OBJECT public: SystemHealthGadget(QString classId, SystemHealthGadgetWidget *widget, QWidget *parent = 0); ~SystemHealthGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: SystemHealthGadgetWidget *m_widget; diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.cpp index 6ddd9aaa8..de16464da 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.cpp @@ -32,13 +32,13 @@ * Loads a saved configuration or defaults if non exist. * */ -SystemHealthGadgetConfiguration::SystemHealthGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +SystemHealthGadgetConfiguration::SystemHealthGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), systemFile("Unknown") { - //if a saved configuration exists load it - if(qSettings != 0) { - QString diagram= qSettings->value("diagram").toString(); + // if a saved configuration exists load it + if (qSettings != 0) { + QString diagram = qSettings->value("diagram").toString(); systemFile = Utils::PathUtils().InsertDataPath(diagram); } } @@ -50,7 +50,8 @@ SystemHealthGadgetConfiguration::SystemHealthGadgetConfiguration(QString classId IUAVGadgetConfiguration *SystemHealthGadgetConfiguration::clone() { SystemHealthGadgetConfiguration *m = new SystemHealthGadgetConfiguration(this->classId()); - m->systemFile=systemFile; + + m->systemFile = systemFile; return m; } @@ -58,7 +59,9 @@ IUAVGadgetConfiguration *SystemHealthGadgetConfiguration::clone() * Saves a configuration. * */ -void SystemHealthGadgetConfiguration::saveConfig(QSettings* qSettings) const { +void SystemHealthGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ QString diagram = Utils::PathUtils().RemoveDataPath(systemFile); + qSettings->setValue("diagram", diagram); } diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.h index 4cafd7cdb..7eb42c80b 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetconfiguration.h @@ -34,26 +34,30 @@ using namespace Core; /* This is a generic system health gadget displaying system alarms for one or more components. - */ -class SystemHealthGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT + */ +class SystemHealthGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit SystemHealthGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit SystemHealthGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - //set system health configuration functions - void setSystemFile(QString filename){systemFile=filename;} + // set system health configuration functions + void setSystemFile(QString filename) + { + systemFile = filename; + } - //get dial configuration functions - QString getSystemFile() {return systemFile;} + // get dial configuration functions + QString getSystemFile() + { + return systemFile; + } - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: // systemFile contains the source SVG: QString systemFile; - }; #endif // SYSTEMHEALTHGADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.cpp index 5e9100a63..34a52364d 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.cpp @@ -32,29 +32,27 @@ #include SystemHealthGadgetFactory::SystemHealthGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("SystemHealthGadget"), - tr("System Health"), - parent) -{ -} + IUAVGadgetFactory(QString("SystemHealthGadget"), + tr("System Health"), + parent) +{} SystemHealthGadgetFactory::~SystemHealthGadgetFactory() -{ -} +{} -Core::IUAVGadget* SystemHealthGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *SystemHealthGadgetFactory::createGadget(QWidget *parent) { - SystemHealthGadgetWidget* gadgetWidget = new SystemHealthGadgetWidget(parent); + SystemHealthGadgetWidget *gadgetWidget = new SystemHealthGadgetWidget(parent); + return new SystemHealthGadget(QString("SystemHealthGadget"), gadgetWidget, parent); } -IUAVGadgetConfiguration *SystemHealthGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *SystemHealthGadgetFactory::createConfiguration(QSettings *qSettings) { return new SystemHealthGadgetConfiguration(QString("SystemHealthGadget"), qSettings); } IOptionsPage *SystemHealthGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new SystemHealthGadgetOptionsPage(qobject_cast(config)); + return new SystemHealthGadgetOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.h b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.h index f17ed33f7..cdc38590b 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetfactory.h @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class SystemHealthGadgetFactory : public IUAVGadgetFactory -{ +class SystemHealthGadgetFactory : public IUAVGadgetFactory { Q_OBJECT public: SystemHealthGadgetFactory(QObject *parent = 0); ~SystemHealthGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.cpp index 229a8caff..4bb606833 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.cpp @@ -34,20 +34,18 @@ #include SystemHealthGadgetOptionsPage::SystemHealthGadgetOptionsPage(SystemHealthGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget (uses the UI file) +// creates options page widget (uses the UI file) QWidget *SystemHealthGadgetOptionsPage::createPage(QWidget *parent) { - Q_UNUSED(parent); options_page = new Ui::SystemHealthGadgetOptionsPage(); - //main widget + // main widget QWidget *optionsPageWidget = new QWidget; - //main layout + // main layout options_page->setupUi(optionsPageWidget); // Restore the contents from the settings: diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.h b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.h index f5fbe7d8f..3b729d69a 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetoptionspage.h @@ -40,14 +40,13 @@ class IUAVGadgetConfiguration; class SystemHealthGadgetConfiguration; namespace Ui { - class SystemHealthGadgetOptionsPage; +class SystemHealthGadgetOptionsPage; } using namespace Core; -class SystemHealthGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class SystemHealthGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit SystemHealthGadgetOptionsPage(SystemHealthGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp index 68d19c532..38f62569d 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp @@ -39,15 +39,15 @@ */ SystemHealthGadgetWidget::SystemHealthGadgetWidget(QWidget *parent) : QGraphicsView(parent) { - setMinimumSize(128,128); + setMinimumSize(128, 128); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setScene(new QGraphicsScene(this)); - + m_renderer = new QSvgRenderer(); background = new QGraphicsSvgItem(); foreground = new QGraphicsSvgItem(); - nolink = new QGraphicsSvgItem(); + nolink = new QGraphicsSvgItem(); paint(); @@ -55,11 +55,11 @@ SystemHealthGadgetWidget::SystemHealthGadgetWidget(QWidget *parent) : QGraphicsV ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - SystemAlarms* obj = dynamic_cast(objManager->getObject(QString("SystemAlarms"))); - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateAlarms(UAVObject*))); + SystemAlarms *obj = dynamic_cast(objManager->getObject(QString("SystemAlarms"))); + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateAlarms(UAVObject *))); // Listen to autopilot connection events - TelemetryManager* telMngr = pm->getObject(); + TelemetryManager *telMngr = pm->getObject(); connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); @@ -67,53 +67,56 @@ SystemHealthGadgetWidget::SystemHealthGadgetWidget(QWidget *parent) : QGraphicsV } /** - * Hide the "No Link" overlay - */ + * Hide the "No Link" overlay + */ void SystemHealthGadgetWidget::onAutopilotConnect() { nolink->setVisible(false); } /** - * Show the "No Link" overlay - */ + * Show the "No Link" overlay + */ void SystemHealthGadgetWidget::onAutopilotDisconnect() { nolink->setVisible(true); } -void SystemHealthGadgetWidget::updateAlarms(UAVObject* systemAlarm) +void SystemHealthGadgetWidget::updateAlarms(UAVObject *systemAlarm) { // This code does not know anything about alarms beforehand, and // I found no efficient way to locate items inside the scene by // name, so it's just as simple to reset the scene: // And add the one with the right name. QGraphicsScene *m_scene = scene(); - foreach ( QGraphicsItem* item ,background->childItems()){ + + foreach(QGraphicsItem * item, background->childItems()) { m_scene->removeItem(item); delete item; // removeItem does _not_ delete the item. } QString alarm = systemAlarm->getName(); - foreach (UAVObjectField *field, systemAlarm->getFields()) { + foreach(UAVObjectField * field, systemAlarm->getFields()) { for (uint i = 0; i < field->getNumElements(); ++i) { QString element = field->getElementNames()[i]; - QString value = field->getValue(i).toString(); + QString value = field->getValue(i).toString(); if (m_renderer->elementExists(element)) { QMatrix blockMatrix = m_renderer->matrixForElement(element); qreal startX = blockMatrix.mapRect(m_renderer->boundsOnElement(element)).x(); qreal startY = blockMatrix.mapRect(m_renderer->boundsOnElement(element)).y(); - QString element2 = element + "-" + value; + QString element2 = element + "-" + value; if (m_renderer->elementExists(element2)) { QGraphicsSvgItem *ind = new QGraphicsSvgItem(); ind->setSharedRenderer(m_renderer); ind->setElementId(element2); ind->setParentItem(background); QTransform matrix; - matrix.translate(startX,startY); - ind->setTransform(matrix,false); + matrix.translate(startX, startY); + ind->setTransform(matrix, false); } else { - if (value.compare("Uninitialised")!=0)qDebug() << "Warning: element " << element2 << " not found in SVG."; + if (value.compare("Uninitialised") != 0) { + qDebug() << "Warning: element " << element2 << " not found in SVG."; + } } } else { qDebug() << "Warning: Element " << element << " not found in SVG."; @@ -124,54 +127,53 @@ void SystemHealthGadgetWidget::updateAlarms(UAVObject* systemAlarm) SystemHealthGadgetWidget::~SystemHealthGadgetWidget() { - // Do nothing + // Do nothing } void SystemHealthGadgetWidget::setSystemFile(QString dfn) { setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); - if (QFile::exists(dfn)) { - m_renderer->load(dfn); - if(m_renderer->isValid()) { - fgenabled = false; - background->setSharedRenderer(m_renderer); - background->setElementId("background"); + if (QFile::exists(dfn)) { + m_renderer->load(dfn); + if (m_renderer->isValid()) { + fgenabled = false; + background->setSharedRenderer(m_renderer); + background->setElementId("background"); - if (m_renderer->elementExists("foreground")) { - foreground->setSharedRenderer(m_renderer); - foreground->setElementId("foreground"); - foreground->setZValue(99); - fgenabled = true; - } - if (m_renderer->elementExists("nolink")) { - nolink->setSharedRenderer(m_renderer); - nolink->setElementId("nolink"); - nolink->setZValue(100); - } + if (m_renderer->elementExists("foreground")) { + foreground->setSharedRenderer(m_renderer); + foreground->setElementId("foreground"); + foreground->setZValue(99); + fgenabled = true; + } + if (m_renderer->elementExists("nolink")) { + nolink->setSharedRenderer(m_renderer); + nolink->setElementId("nolink"); + nolink->setZValue(100); + } - QGraphicsScene *l_scene = scene(); - l_scene->setSceneRect(background->boundingRect()); - fitInView(background, Qt::KeepAspectRatio ); + QGraphicsScene *l_scene = scene(); + l_scene->setSceneRect(background->boundingRect()); + fitInView(background, Qt::KeepAspectRatio); - // Check whether the autopilot is connected already, by the way: - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - TelemetryManager* telMngr = pm->getObject(); - if (telMngr->isConnected()) { - onAutopilotConnect(); - SystemAlarms* obj = dynamic_cast(objManager->getObject(QString("SystemAlarms"))); - updateAlarms(obj); - } - } - } - else - { qDebug() <<"SystemHealthGadget: no file"; } + // Check whether the autopilot is connected already, by the way: + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + TelemetryManager *telMngr = pm->getObject(); + if (telMngr->isConnected()) { + onAutopilotConnect(); + SystemAlarms *obj = dynamic_cast(objManager->getObject(QString("SystemAlarms"))); + updateAlarms(obj); + } + } + } else { qDebug() << "SystemHealthGadget: no file"; } } void SystemHealthGadgetWidget::paint() { QGraphicsScene *l_scene = scene(); + l_scene->clear(); l_scene->addItem(background); l_scene->addItem(foreground); @@ -182,11 +184,11 @@ void SystemHealthGadgetWidget::paint() void SystemHealthGadgetWidget::paintEvent(QPaintEvent *event) { // Skip painting until the dial file is loaded - if (! m_renderer->isValid()) { - qDebug() <<"SystemHealthGadget: System file not loaded, not rendering"; + if (!m_renderer->isValid()) { + qDebug() << "SystemHealthGadget: System file not loaded, not rendering"; return; } - QGraphicsView::paintEvent(event); + QGraphicsView::paintEvent(event); } // This event enables the dial to be dynamically resized @@ -195,34 +197,35 @@ void SystemHealthGadgetWidget::paintEvent(QPaintEvent *event) void SystemHealthGadgetWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - fitInView(background, Qt::KeepAspectRatio ); + fitInView(background, Qt::KeepAspectRatio); } -void SystemHealthGadgetWidget::mousePressEvent ( QMouseEvent * event ) +void SystemHealthGadgetWidget::mousePressEvent(QMouseEvent *event) { QGraphicsScene *graphicsScene = scene(); - if(graphicsScene){ + + if (graphicsScene) { QPoint point = event->pos(); bool haveAlarmItem = false; - foreach(QGraphicsItem* sceneItem, items(point)){ - QGraphicsSvgItem *clickedItem = dynamic_cast(sceneItem); + foreach(QGraphicsItem * sceneItem, items(point)) { + QGraphicsSvgItem *clickedItem = dynamic_cast(sceneItem); - if(clickedItem){ - if((clickedItem != foreground) && (clickedItem != background)){ + if (clickedItem) { + if ((clickedItem != foreground) && (clickedItem != background)) { // Clicked an actual alarm. We need to set haveAlarmItem to true // as two of the items in this loop will always be foreground and // background. Without this flag, at some point in the loop we // would always call showAllAlarmDescriptions... haveAlarmItem = true; QString itemId = clickedItem->elementId(); - if(itemId.contains("OK")){ + if (itemId.contains("OK")) { // No alarm set for this item showAlarmDescriptionForItemId("AlarmOK", event->globalPos()); - }else{ + } else { // Warning, error or critical alarm showAlarmDescriptionForItemId(itemId, event->globalPos()); } - }else if(!haveAlarmItem){ + } else if (!haveAlarmItem) { // Clicked foreground or background showAllAlarmDescriptions(event->globalPos()); } @@ -231,29 +234,34 @@ void SystemHealthGadgetWidget::mousePressEvent ( QMouseEvent * event ) } } -void SystemHealthGadgetWidget::showAlarmDescriptionForItemId(const QString itemId, const QPoint& location){ +void SystemHealthGadgetWidget::showAlarmDescriptionForItemId(const QString itemId, const QPoint & location) +{ QFile alarmDescription(":/systemhealth/html/" + itemId + ".html"); - if(alarmDescription.open(QIODevice::ReadOnly | QIODevice::Text)){ + + if (alarmDescription.open(QIODevice::ReadOnly | QIODevice::Text)) { QTextStream textStream(&alarmDescription); QWhatsThis::showText(location, textStream.readAll()); } } -void SystemHealthGadgetWidget::showAllAlarmDescriptions(const QPoint& location){ +void SystemHealthGadgetWidget::showAllAlarmDescriptions(const QPoint & location) +{ QGraphicsScene *graphicsScene = scene(); - if(graphicsScene){ + + if (graphicsScene) { QString alarmsText; // Loop through all items in the scene looking for svg items that represent alarms - foreach(QGraphicsItem* curItem, graphicsScene->items()){ - QGraphicsSvgItem* curSvgItem = dynamic_cast(curItem); - if(curSvgItem && (curSvgItem != foreground) && (curSvgItem != background)){ + foreach(QGraphicsItem * curItem, graphicsScene->items()) { + QGraphicsSvgItem *curSvgItem = dynamic_cast(curItem); + + if (curSvgItem && (curSvgItem != foreground) && (curSvgItem != background)) { QString elementId = curSvgItem->elementId(); - if(!elementId.contains("OK")){ + if (!elementId.contains("OK")) { // Found an alarm, get its corresponding alarm html file contents // and append to the cumulative string for all alarms. QFile alarmDescription(":/systemhealth/html/" + elementId + ".html"); - if(alarmDescription.open(QIODevice::ReadOnly | QIODevice::Text)){ + if (alarmDescription.open(QIODevice::ReadOnly | QIODevice::Text)) { QTextStream textStream(&alarmDescription); alarmsText.append(textStream.readAll()); } @@ -262,7 +270,7 @@ void SystemHealthGadgetWidget::showAllAlarmDescriptions(const QPoint& location){ } // Show alarms text if we have any - if(alarmsText.length() > 0){ + if (alarmsText.length() > 0) { QWhatsThis::showText(location, alarmsText); } } diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h index 15ab31952..a76c58e1a 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h @@ -39,38 +39,36 @@ #include #include -class SystemHealthGadgetWidget : public QGraphicsView -{ +class SystemHealthGadgetWidget : public QGraphicsView { Q_OBJECT public: SystemHealthGadgetWidget(QWidget *parent = 0); - ~SystemHealthGadgetWidget(); - void setSystemFile(QString dfn); - void setIndicator(QString indicator); - void paint(); + ~SystemHealthGadgetWidget(); + void setSystemFile(QString dfn); + void setIndicator(QString indicator); + void paint(); protected: - void paintEvent(QPaintEvent *event); - void resizeEvent(QResizeEvent *event); - void mousePressEvent ( QMouseEvent * event ); + void paintEvent(QPaintEvent *event); + void resizeEvent(QResizeEvent *event); + void mousePressEvent(QMouseEvent *event); private slots: - void updateAlarms(UAVObject *systemAlarm); // Called by the systemalarms UAVObject - void onAutopilotConnect(); - void onAutopilotDisconnect(); + void updateAlarms(UAVObject *systemAlarm); // Called by the systemalarms UAVObject + void onAutopilotConnect(); + void onAutopilotDisconnect(); private: - QSvgRenderer *m_renderer; - QGraphicsSvgItem *background; - QGraphicsSvgItem *foreground; - QGraphicsSvgItem *nolink; + QSvgRenderer *m_renderer; + QGraphicsSvgItem *background; + QGraphicsSvgItem *foreground; + QGraphicsSvgItem *nolink; - // Simple flag to skip rendering if the - bool fgenabled; // layer does not exist. - - void showAlarmDescriptionForItemId(const QString itemId, const QPoint& location); - void showAllAlarmDescriptions(const QPoint &location); + // Simple flag to skip rendering if the + bool fgenabled; // layer does not exist. + void showAlarmDescriptionForItemId(const QString itemId, const QPoint & location); + void showAllAlarmDescriptions(const QPoint &location); }; #endif /* SYSTEMHEALTHGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.cpp b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.cpp index 3975c8508..db5bc6aa9 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.cpp +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.cpp @@ -35,31 +35,31 @@ SystemHealthPlugin::SystemHealthPlugin() { - // Do nothing + // Do nothing } SystemHealthPlugin::~SystemHealthPlugin() { - // Do nothing + // Do nothing } -bool SystemHealthPlugin::initialize(const QStringList& args, QString *errMsg) +bool SystemHealthPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new SystemHealthGadgetFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new SystemHealthGadgetFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void SystemHealthPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void SystemHealthPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(SystemHealthPlugin) diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.h b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.h index 6f68288ea..332c7eee6 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.h +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthplugin.h @@ -32,16 +32,15 @@ class SystemHealthGadgetFactory; -class SystemHealthPlugin : public ExtensionSystem::IPlugin -{ +class SystemHealthPlugin : public ExtensionSystem::IPlugin { public: - SystemHealthPlugin(); - ~SystemHealthPlugin(); + SystemHealthPlugin(); + ~SystemHealthPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - SystemHealthGadgetFactory *mf; + SystemHealthGadgetFactory *mf; }; #endif /* SYSTEMHEALTHPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.cpp index c0ac4fa01..028dc82c3 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.cpp @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -29,16 +29,15 @@ #include "fieldtreeitem.h" BrowserItemDelegate::BrowserItemDelegate(QObject *parent) : - QStyledItemDelegate(parent) -{ -} + QStyledItemDelegate(parent) +{} QWidget *BrowserItemDelegate::createEditor(QWidget *parent, - const QStyleOptionViewItem & option , - const QModelIndex & index ) const + const QStyleOptionViewItem & option, + const QModelIndex & index) const { Q_UNUSED(option) - FieldTreeItem *item = static_cast(index.internalPointer()); + FieldTreeItem * item = static_cast(index.internalPointer()); QWidget *editor = item->createEditor(parent); Q_ASSERT(editor); return editor; @@ -48,21 +47,23 @@ QWidget *BrowserItemDelegate::createEditor(QWidget *parent, void BrowserItemDelegate::setEditorData(QWidget *editor, const QModelIndex &index) const { - FieldTreeItem *item = static_cast(index.internalPointer()); + FieldTreeItem *item = static_cast(index.internalPointer()); QVariant value = index.model()->data(index, Qt::EditRole); + item->setEditorValue(editor, value); } void BrowserItemDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, const QModelIndex &index) const { - FieldTreeItem *item = static_cast(index.internalPointer()); + FieldTreeItem *item = static_cast(index.internalPointer()); QVariant value = item->getEditorValue(editor); + model->setData(index, value, Qt::EditRole); } void BrowserItemDelegate::updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &/* index */) const + const QStyleOptionViewItem &option, const QModelIndex & /* index */) const { editor->setGeometry(option.rect); } diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.h index 9c6a0e983..17952b5db 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browseritemdelegate.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,9 +30,8 @@ #include -class BrowserItemDelegate : public QStyledItemDelegate -{ -Q_OBJECT +class BrowserItemDelegate : public QStyledItemDelegate { + Q_OBJECT public: explicit BrowserItemDelegate(QObject *parent = 0); @@ -44,15 +43,14 @@ public: const QModelIndex &index) const; void updateEditorGeometry(QWidget *editor, - const QStyleOptionViewItem &option, const QModelIndex &index) const; + const QStyleOptionViewItem &option, const QModelIndex &index) const; QSize sizeHint(const QStyleOptionViewItem & option, - const QModelIndex &index) const; + const QModelIndex &index) const; signals: public slots: - }; #endif // BROWSERITEMDELEGATE_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.cpp index f30a08e0e..703ea1290 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.cpp @@ -32,32 +32,31 @@ BrowserPlugin::BrowserPlugin() { - // Do nothing + // Do nothing } BrowserPlugin::~BrowserPlugin() { - // Do nothing + // Do nothing } -bool BrowserPlugin::initialize(const QStringList& args, QString *errMsg) +bool BrowserPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new UAVObjectBrowserFactory(this); - addAutoReleasedObject(mf); + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new UAVObjectBrowserFactory(this); + addAutoReleasedObject(mf); - return true; + return true; } void BrowserPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void BrowserPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(BrowserPlugin) - diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.h index 4644e7475..aac119a59 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/browserplugin.h @@ -32,16 +32,15 @@ class UAVObjectBrowserFactory; -class BrowserPlugin : public ExtensionSystem::IPlugin -{ +class BrowserPlugin : public ExtensionSystem::IPlugin { public: - BrowserPlugin(); - ~BrowserPlugin(); + BrowserPlugin(); + ~BrowserPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - UAVObjectBrowserFactory *mf; + UAVObjectBrowserFactory *mf; }; #endif /* UAVOBJECTBROWSERPLUGIN_H_ */ diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.cpp index fa3c9e203..2c1407d06 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.cpp @@ -10,20 +10,19 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "fieldtreeitem.h" - diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.h index bd980749f..256775172 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/fieldtreeitem.h @@ -37,86 +37,100 @@ #include #include -#define QINT8MIN std::numeric_limits::min() -#define QINT8MAX std::numeric_limits::max() -#define QUINTMIN std::numeric_limits::min() -#define QUINT8MAX std::numeric_limits::max() -#define QINT16MIN std::numeric_limits::min() -#define QINT16MAX std::numeric_limits::max() +#define QINT8MIN std::numeric_limits::min() +#define QINT8MAX std::numeric_limits::max() +#define QUINTMIN std::numeric_limits::min() +#define QUINT8MAX std::numeric_limits::max() +#define QINT16MIN std::numeric_limits::min() +#define QINT16MAX std::numeric_limits::max() #define QUINT16MAX std::numeric_limits::max() -#define QINT32MIN std::numeric_limits::min() -#define QINT32MAX std::numeric_limits::max() +#define QINT32MIN std::numeric_limits::min() +#define QINT32MAX std::numeric_limits::max() #define QUINT32MAX std::numeric_limits::max() -class FieldTreeItem : public TreeItem -{ -Q_OBJECT +class FieldTreeItem : public TreeItem { + Q_OBJECT public: FieldTreeItem(int index, const QList &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_index(index) { } + TreeItem(data, parent), m_index(index) {} FieldTreeItem(int index, const QVariant &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_index(index) { } - bool isEditable() { return true; } - virtual QWidget *createEditor(QWidget *parent) = 0; + TreeItem(data, parent), m_index(index) {} + bool isEditable() + { + return true; + } + virtual QWidget *createEditor(QWidget *parent) = 0; virtual QVariant getEditorValue(QWidget *editor) = 0; virtual void setEditorValue(QWidget *editor, QVariant value) = 0; - virtual void apply() { } + virtual void apply() {} protected: int m_index; }; -class EnumFieldTreeItem : public FieldTreeItem -{ -Q_OBJECT +class EnumFieldTreeItem : public FieldTreeItem { + Q_OBJECT public: EnumFieldTreeItem(UAVObjectField *field, int index, const QList &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) { } + FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) {} EnumFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) { } - void setData(QVariant value, int column) { + FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) {} + void setData(QVariant value, int column) + { QStringList options = m_field->getOptions(); - QVariant tmpValue = m_field->getValue(m_index); - int tmpValIndex = options.indexOf(tmpValue.toString()); + QVariant tmpValue = m_field->getValue(m_index); + int tmpValIndex = options.indexOf(tmpValue.toString()); + setChanged(tmpValIndex != value); TreeItem::setData(value, column); } - QString enumOptions(int index) { - if((index < 0) || (index >= m_enumOptions.length())) { + QString enumOptions(int index) + { + if ((index < 0) || (index >= m_enumOptions.length())) { return QString("Invalid Value (") + QString().setNum(index) + QString(")"); } return m_enumOptions.at(index); } - void apply() { + void apply() + { int value = data(dataColumn).toInt(); QStringList options = m_field->getOptions(); + m_field->setValue(options[value], m_index); setChanged(false); } - void update() { + void update() + { QStringList options = m_field->getOptions(); QVariant value = m_field->getValue(m_index); - int valIndex = options.indexOf(value.toString()); + int valIndex = options.indexOf(value.toString()); + if (data() != valIndex || changed()) { TreeItem::setData(valIndex); setHighlight(true); } } - QWidget *createEditor(QWidget *parent) { + QWidget *createEditor(QWidget *parent) + { QComboBox *editor = new QComboBox(parent); - foreach (QString option, m_enumOptions) - editor->addItem(option); + + foreach(QString option, m_enumOptions) + editor->addItem(option); return editor; } - QVariant getEditorValue(QWidget *editor) { - QComboBox *comboBox = static_cast(editor); + QVariant getEditorValue(QWidget *editor) + { + QComboBox *comboBox = static_cast(editor); + return comboBox->currentIndex(); } - void setEditorValue(QWidget *editor, QVariant value) { - QComboBox *comboBox = static_cast(editor); + void setEditorValue(QWidget *editor, QVariant value) + { + QComboBox *comboBox = static_cast(editor); + comboBox->setCurrentIndex(value.toInt()); } private: @@ -124,20 +138,22 @@ private: UAVObjectField *m_field; }; -class IntFieldTreeItem : public FieldTreeItem -{ -Q_OBJECT +class IntFieldTreeItem : public FieldTreeItem { + Q_OBJECT public: IntFieldTreeItem(UAVObjectField *field, int index, const QList &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { + FieldTreeItem(index, data, parent), m_field(field) + { setMinMaxValues(); } IntFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field) { + FieldTreeItem(index, data, parent), m_field(field) + { setMinMaxValues(); } - void setMinMaxValues() { + void setMinMaxValues() + { switch (m_field->getType()) { case UAVObjectField::INT8: m_minValue = QINT8MIN; @@ -169,33 +185,43 @@ public: } } - QWidget *createEditor(QWidget *parent) { + QWidget *createEditor(QWidget *parent) + { QSpinBox *editor = new QSpinBox(parent); + editor->setMinimum(m_minValue); editor->setMaximum(m_maxValue); return editor; } - QVariant getEditorValue(QWidget *editor) { - QSpinBox *spinBox = static_cast(editor); + QVariant getEditorValue(QWidget *editor) + { + QSpinBox *spinBox = static_cast(editor); + spinBox->interpretText(); return spinBox->value(); } - void setEditorValue(QWidget *editor, QVariant value) { - QSpinBox *spinBox = static_cast(editor); + void setEditorValue(QWidget *editor, QVariant value) + { + QSpinBox *spinBox = static_cast(editor); + spinBox->setValue(value.toInt()); } - void setData(QVariant value, int column) { + void setData(QVariant value, int column) + { setChanged(m_field->getValue(m_index) != value); TreeItem::setData(value, column); } - void apply() { + void apply() + { m_field->setValue(data(dataColumn).toInt(), m_index); setChanged(false); } - void update() { + void update() + { int value = m_field->getValue(m_index).toInt(); + if (data() != value || changed()) { TreeItem::setData(value); setHighlight(true); @@ -208,72 +234,76 @@ private: int m_maxValue; }; -class FloatFieldTreeItem : public FieldTreeItem -{ -Q_OBJECT +class FloatFieldTreeItem : public FieldTreeItem { + Q_OBJECT public: FloatFieldTreeItem(UAVObjectField *field, int index, const QList &data, bool scientific = false, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field), m_useScientificNotation(scientific){} + FieldTreeItem(index, data, parent), m_field(field), m_useScientificNotation(scientific) {} FloatFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, bool scientific = false, TreeItem *parent = 0) : - FieldTreeItem(index, data, parent), m_field(field), m_useScientificNotation(scientific) { } - void setData(QVariant value, int column) { + FieldTreeItem(index, data, parent), m_field(field), m_useScientificNotation(scientific) {} + void setData(QVariant value, int column) + { setChanged(m_field->getValue(m_index) != value); TreeItem::setData(value, column); } - void apply() { + void apply() + { m_field->setValue(data(dataColumn).toDouble(), m_index); setChanged(false); } - void update() { + void update() + { double value = m_field->getValue(m_index).toDouble(); + if (data() != value || changed()) { TreeItem::setData(value); setHighlight(true); } } - QWidget *createEditor(QWidget *parent) { - if(m_useScientificNotation) { + QWidget *createEditor(QWidget *parent) + { + if (m_useScientificNotation) { QScienceSpinBox *editor = new QScienceSpinBox(parent); editor->setDecimals(6); editor->setMinimum(-std::numeric_limits::max()); editor->setMaximum(std::numeric_limits::max()); return editor; } else { - QDoubleSpinBox *editor = new QDoubleSpinBox(parent); - editor->setDecimals(8); + QDoubleSpinBox *editor = new QDoubleSpinBox(parent); + editor->setDecimals(8); editor->setMinimum(-std::numeric_limits::max()); editor->setMaximum(std::numeric_limits::max()); return editor; } } - QVariant getEditorValue(QWidget *editor) { - if(m_useScientificNotation) { - QScienceSpinBox *spinBox = static_cast(editor); + QVariant getEditorValue(QWidget *editor) + { + if (m_useScientificNotation) { + QScienceSpinBox *spinBox = static_cast(editor); spinBox->interpretText(); return spinBox->value(); } else { - QDoubleSpinBox *spinBox = static_cast(editor); + QDoubleSpinBox *spinBox = static_cast(editor); spinBox->interpretText(); return spinBox->value(); } } - void setEditorValue(QWidget *editor, QVariant value) { - - if(m_useScientificNotation) { - QScienceSpinBox *spinBox = static_cast(editor); + void setEditorValue(QWidget *editor, QVariant value) + { + if (m_useScientificNotation) { + QScienceSpinBox *spinBox = static_cast(editor); spinBox->setValue(value.toDouble()); } else { - QDoubleSpinBox *spinBox = static_cast(editor); + QDoubleSpinBox *spinBox = static_cast(editor); spinBox->setValue(value.toDouble()); } } private: UAVObjectField *m_field; bool m_useScientificNotation; - }; #endif // FIELDTREEITEM_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp index fac18561b..85c5aa144 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -45,8 +45,7 @@ bool HighLightManager::add(TreeItem *itemToAdd) QMutexLocker locker(&m_listMutex); // Check so that the item isn't already in the list - if(!m_itemsList.contains(itemToAdd)) - { + if (!m_itemsList.contains(itemToAdd)) { m_itemsList.append(itemToAdd); return true; } @@ -78,17 +77,15 @@ void HighLightManager::checkItemsExpired() QMutexLocker locker(&m_listMutex); // Get a mutable iterator for the list - QMutableLinkedListIterator iter(m_itemsList); + QMutableLinkedListIterator iter(m_itemsList); // This is the timestamp to compare with QTime now = QTime::currentTime(); // Loop over all items, check if they expired. - while(iter.hasNext()) - { - TreeItem* item = iter.next(); - if(item->getHiglightExpires() < now) - { + while (iter.hasNext()) { + TreeItem *item = iter.next(); + if (item->getHiglightExpires() < now) { // If expired, call removeHighlight item->removeHighlight(); @@ -101,19 +98,18 @@ void HighLightManager::checkItemsExpired() int TreeItem::m_highlightTimeMs = 500; TreeItem::TreeItem(const QList &data, TreeItem *parent) : - QObject(0), - m_data(data), - m_parent(parent), - m_highlight(false), - m_changed(false) -{ -} + QObject(0), + m_data(data), + m_parent(parent), + m_highlight(false), + m_changed(false) +{} TreeItem::TreeItem(const QVariant &data, TreeItem *parent) : - QObject(0), - m_parent(parent), - m_highlight(false), - m_changed(false) + QObject(0), + m_parent(parent), + m_highlight(false), + m_changed(false) { m_data << data << "" << ""; } @@ -132,6 +128,7 @@ void TreeItem::appendChild(TreeItem *child) void TreeItem::insertChild(TreeItem *child) { int index = nameIndex(child->data(0).toString()); + m_children.insert(index, child); child->setParentTree(this); } @@ -148,8 +145,9 @@ int TreeItem::childCount() const int TreeItem::row() const { - if (m_parent) - return m_parent->m_children.indexOf(const_cast(this)); + if (m_parent) { + return m_parent->m_children.indexOf(const_cast(this)); + } return 0; } @@ -169,35 +167,35 @@ void TreeItem::setData(QVariant value, int column) m_data.replace(column, value); } -void TreeItem::update() { - foreach(TreeItem *child, treeChildren()) - child->update(); +void TreeItem::update() +{ + foreach(TreeItem * child, treeChildren()) + child->update(); } -void TreeItem::apply() { - foreach(TreeItem *child, treeChildren()) - child->apply(); +void TreeItem::apply() +{ + foreach(TreeItem * child, treeChildren()) + child->apply(); } /* * Called after a value has changed to trigger highlightning of tree item. */ -void TreeItem::setHighlight(bool highlight) { +void TreeItem::setHighlight(bool highlight) +{ m_highlight = highlight; - m_changed = false; + m_changed = false; if (highlight) { // Update the expires timestamp m_highlightExpires = QTime::currentTime().addMSecs(m_highlightTimeMs); // Add to highlightmanager - if(m_highlightManager->add(this)) - { + if (m_highlightManager->add(this)) { // Only emit signal if it was added emit updateHighlight(this); } - } - else if(m_highlightManager->remove(this)) - { + } else if (m_highlightManager->remove(this)) { // Only emit signal if it was removed emit updateHighlight(this); } @@ -205,15 +203,15 @@ void TreeItem::setHighlight(bool highlight) { // If we have a parent, call recursively to update highlight status of parents. // This will ensure that the root of a leaf that is changed also is highlighted. // Only updates that really changes values will trigger highlight of parents. - if(m_parent) - { + if (m_parent) { m_parent->setHighlight(highlight); } } -void TreeItem::removeHighlight() { +void TreeItem::removeHighlight() +{ m_highlight = false; - //update(); + // update(); emit updateHighlight(this); } diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h index f0209c598..9f693684b 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -43,33 +43,32 @@ class TreeItem; /* -* Small utility class that handles the higlighting of -* tree grid items. -* Basicly it maintains all items due to be restored to -* non highlighted state in a linked list. -* A timer traverses this list periodically to find out -* if any of the items should be restored. All items are -* updated withan expiration timestamp when they expires. -* An item that is beeing restored is removed from the -* list and its removeHighlight() method is called. Items -* that are not expired are left in the list til next time. -* Items that are updated during the expiration time are -* left untouched in the list. This reduces unwanted emits -* of signals to the repaint/update function. -*/ -class HighLightManager : public QObject -{ -Q_OBJECT + * Small utility class that handles the higlighting of + * tree grid items. + * Basicly it maintains all items due to be restored to + * non highlighted state in a linked list. + * A timer traverses this list periodically to find out + * if any of the items should be restored. All items are + * updated withan expiration timestamp when they expires. + * An item that is beeing restored is removed from the + * list and its removeHighlight() method is called. Items + * that are not expired are left in the list til next time. + * Items that are updated during the expiration time are + * left untouched in the list. This reduces unwanted emits + * of signals to the repaint/update function. + */ +class HighLightManager : public QObject { + Q_OBJECT public: // Constructor taking the checking interval in ms. HighLightManager(long checkingInterval); // This is called when an item has been set to // highlighted = true. - bool add(TreeItem* itemToAdd); + bool add(TreeItem *itemToAdd); - //This is called when an item is set to highlighted = false; - bool remove(TreeItem* itemToRemove); + // This is called when an item is set to highlighted = false; + bool remove(TreeItem *itemToRemove); private slots: // Timer callback method. @@ -80,15 +79,14 @@ private: QTimer m_expirationTimer; // The list holding all items due to be updated. - QLinkedList m_itemsList; + QLinkedList m_itemsList; - //Mutex to lock when accessing list. + // Mutex to lock when accessing list. QMutex m_listMutex; }; -class TreeItem : public QObject -{ -Q_OBJECT +class TreeItem : public QObject { + Q_OBJECT public: TreeItem(const QList &data, TreeItem *parent = 0); TreeItem(const QVariant &data, TreeItem *parent = 0); @@ -98,14 +96,22 @@ public: void insertChild(TreeItem *child); TreeItem *getChild(int index); - inline QList treeChildren() const { return m_children; } + inline QList treeChildren() const + { + return m_children; + } int childCount() const; int columnCount() const; QVariant data(int column = 1) const; - QString description() { return m_description; } - void setDescription(QString d) { // Split around 40 characters - int idx = d.indexOf(" ",40); - d.insert(idx,QString("
")); + QString description() + { + return m_description; + } + void setDescription(QString d) // Split around 40 characters + { + int idx = d.indexOf(" ", 40); + + d.insert(idx, QString("
")); d.remove("@Ref", Qt::CaseInsensitive); m_description = d; } @@ -113,36 +119,59 @@ public: // other columns are initialized in constructor virtual void setData(QVariant value, int column = 1); int row() const; - TreeItem *parent() { return m_parent; } - void setParentTree(TreeItem *parent) { m_parent = parent; } - inline virtual bool isEditable() { return false; } + TreeItem *parent() + { + return m_parent; + } + void setParentTree(TreeItem *parent) + { + m_parent = parent; + } + inline virtual bool isEditable() + { + return false; + } virtual void update(); virtual void apply(); - inline bool highlighted() { return m_highlight; } + inline bool highlighted() + { + return m_highlight; + } void setHighlight(bool highlight); - static void setHighlightTime(int time) { m_highlightTimeMs = time; } + static void setHighlightTime(int time) + { + m_highlightTimeMs = time; + } - inline bool changed() { return m_changed; } - inline void setChanged(bool changed) { m_changed = changed; } + inline bool changed() + { + return m_changed; + } + inline void setChanged(bool changed) + { + m_changed = changed; + } - virtual void setHighlightManager(HighLightManager* mgr); + virtual void setHighlightManager(HighLightManager *mgr); QTime getHiglightExpires(); virtual void removeHighlight(); - int nameIndex(QString name) { + int nameIndex(QString name) + { for (int i = 0; i < childCount(); ++i) { - if (name < getChild(i)->data(0).toString()) + if (name < getChild(i)->data(0).toString()) { return i; + } } return childCount(); } - TreeItem* findChildByName(QString name) + TreeItem *findChildByName(QString name) { - foreach (TreeItem* child, m_children) { + foreach(TreeItem * child, m_children) { if (name == child->data(0).toString()) { return child; } @@ -151,12 +180,12 @@ public: } signals: - void updateHighlight(TreeItem*); + void updateHighlight(TreeItem *); private slots: private: - QList m_children; + QList m_children; // m_data contains: [0] property name, [1] value, [2] unit QList m_data; QString m_description; @@ -164,7 +193,7 @@ private: bool m_highlight; bool m_changed; QTime m_highlightExpires; - HighLightManager* m_highlightManager; + HighLightManager *m_highlightManager; static int m_highlightTimeMs; public: static const int dataColumn = 1; @@ -173,102 +202,130 @@ public: class DataObjectTreeItem; class MetaObjectTreeItem; -class TopTreeItem : public TreeItem -{ -Q_OBJECT +class TopTreeItem : public TreeItem { + Q_OBJECT public: - TopTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) { } - TopTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { } + TopTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) {} + TopTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) {} - void addObjectTreeItem(quint32 objectId, DataObjectTreeItem* oti) { + void addObjectTreeItem(quint32 objectId, DataObjectTreeItem *oti) + { m_objectTreeItemsPerObjectIds[objectId] = oti; } - DataObjectTreeItem* findDataObjectTreeItemByObjectId(quint32 objectId) { + DataObjectTreeItem *findDataObjectTreeItemByObjectId(quint32 objectId) + { return m_objectTreeItemsPerObjectIds.contains(objectId) ? m_objectTreeItemsPerObjectIds[objectId] : 0; } - void addMetaObjectTreeItem(quint32 objectId, MetaObjectTreeItem* oti) { + void addMetaObjectTreeItem(quint32 objectId, MetaObjectTreeItem *oti) + { m_metaObjectTreeItemsPerObjectIds[objectId] = oti; } - MetaObjectTreeItem* findMetaObjectTreeItemByObjectId(quint32 objectId) { + MetaObjectTreeItem *findMetaObjectTreeItemByObjectId(quint32 objectId) + { return m_metaObjectTreeItemsPerObjectIds.contains(objectId) ? m_metaObjectTreeItemsPerObjectIds[objectId] : 0; } - QList getMetaObjectItems(); + QList getMetaObjectItems(); private: - QMap m_objectTreeItemsPerObjectIds; - QMap m_metaObjectTreeItemsPerObjectIds; + QMap m_objectTreeItemsPerObjectIds; + QMap m_metaObjectTreeItemsPerObjectIds; }; -class ObjectTreeItem : public TreeItem -{ -Q_OBJECT +class ObjectTreeItem : public TreeItem { + Q_OBJECT public: ObjectTreeItem(const QList &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_obj(0) { } + TreeItem(data, parent), m_obj(0) {} ObjectTreeItem(const QVariant &data, TreeItem *parent = 0) : - TreeItem(data, parent), m_obj(0) { } - void setObject(UAVObject *obj) { m_obj = obj; setDescription(obj->getDescription()); } - inline UAVObject *object() { return m_obj; } + TreeItem(data, parent), m_obj(0) {} + void setObject(UAVObject *obj) + { + m_obj = obj; setDescription(obj->getDescription()); + } + inline UAVObject *object() + { + return m_obj; + } private: UAVObject *m_obj; }; -class MetaObjectTreeItem : public ObjectTreeItem -{ -Q_OBJECT +class MetaObjectTreeItem : public ObjectTreeItem { + Q_OBJECT public: MetaObjectTreeItem(UAVObject *obj, const QList &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { setObject(obj); } + ObjectTreeItem(data, parent) + { + setObject(obj); + } MetaObjectTreeItem(UAVObject *obj, const QVariant &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { setObject(obj); } + ObjectTreeItem(data, parent) + { + setObject(obj); + } }; -class DataObjectTreeItem : public ObjectTreeItem -{ -Q_OBJECT +class DataObjectTreeItem : public ObjectTreeItem { + Q_OBJECT public: DataObjectTreeItem(const QList &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { } + ObjectTreeItem(data, parent) {} DataObjectTreeItem(const QVariant &data, TreeItem *parent = 0) : - ObjectTreeItem(data, parent) { } - virtual void apply() { - foreach(TreeItem *child, treeChildren()) { - MetaObjectTreeItem *metaChild = dynamic_cast(child); - if (!metaChild) + ObjectTreeItem(data, parent) {} + virtual void apply() + { + foreach(TreeItem * child, treeChildren()) { + MetaObjectTreeItem *metaChild = dynamic_cast(child); + + if (!metaChild) { child->apply(); + } } } - virtual void update() { - foreach(TreeItem *child, treeChildren()) { - MetaObjectTreeItem *metaChild = dynamic_cast(child); - if (!metaChild) + virtual void update() + { + foreach(TreeItem * child, treeChildren()) { + MetaObjectTreeItem *metaChild = dynamic_cast(child); + + if (!metaChild) { child->update(); + } } } }; -class InstanceTreeItem : public DataObjectTreeItem -{ -Q_OBJECT +class InstanceTreeItem : public DataObjectTreeItem { + Q_OBJECT public: InstanceTreeItem(UAVObject *obj, const QList &data, TreeItem *parent = 0) : - DataObjectTreeItem(data, parent) { setObject(obj); } + DataObjectTreeItem(data, parent) + { + setObject(obj); + } InstanceTreeItem(UAVObject *obj, const QVariant &data, TreeItem *parent = 0) : - DataObjectTreeItem(data, parent) { setObject(obj); } - virtual void apply() { TreeItem::apply(); } - virtual void update() { TreeItem::update(); } + DataObjectTreeItem(data, parent) + { + setObject(obj); + } + virtual void apply() + { + TreeItem::apply(); + } + virtual void update() + { + TreeItem::update(); + } }; -class ArrayFieldTreeItem : public TreeItem -{ -Q_OBJECT +class ArrayFieldTreeItem : public TreeItem { + Q_OBJECT public: - ArrayFieldTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) { } - ArrayFieldTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { } + ArrayFieldTreeItem(const QList &data, TreeItem *parent = 0) : TreeItem(data, parent) {} + ArrayFieldTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) {} }; #endif // TREEITEM_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp index 95288986e..6c5342e8d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp @@ -28,11 +28,11 @@ #include "uavobjectbrowserwidget.h" UAVObjectBrowser::UAVObjectBrowser(QString classId, UAVObjectBrowserWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget), - m_config(NULL) + IUAVGadget(classId, parent), + m_widget(widget), + m_config(NULL) { - connect(m_widget,SIGNAL(viewOptionsChanged(bool,bool,bool)),this,SLOT(viewOptionsChangedSlot(bool,bool,bool))); + connect(m_widget, SIGNAL(viewOptionsChanged(bool, bool, bool)), this, SLOT(viewOptionsChangedSlot(bool, bool, bool))); } UAVObjectBrowser::~UAVObjectBrowser() @@ -40,24 +40,23 @@ UAVObjectBrowser::~UAVObjectBrowser() delete m_widget; } -void UAVObjectBrowser::loadConfiguration(IUAVGadgetConfiguration* config) +void UAVObjectBrowser::loadConfiguration(IUAVGadgetConfiguration *config) { - UAVObjectBrowserConfiguration *m = qobject_cast(config); - m_config=m; + UAVObjectBrowserConfiguration *m = qobject_cast(config); + + m_config = m; m_widget->setRecentlyUpdatedColor(m->recentlyUpdatedColor()); m_widget->setManuallyChangedColor(m->manuallyChangedColor()); m_widget->setRecentlyUpdatedTimeout(m->recentlyUpdatedTimeout()); m_widget->setOnlyHilightChangedValues(m->onlyHighlightChangedValues()); - m_widget->setViewOptions(m->categorizedView(),m->scientificView(),m->showMetaData()); + m_widget->setViewOptions(m->categorizedView(), m->scientificView(), m->showMetaData()); } void UAVObjectBrowser::viewOptionsChangedSlot(bool categorized, bool scientific, bool metadata) { - if(m_config) - { + if (m_config) { m_config->setCategorizedView(categorized); m_config->setScientificView(scientific); m_config->setShowMetaData(metadata); } } - diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.h index 224be35a7..261583d06 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.h @@ -39,17 +39,19 @@ class UAVObjectBrowserWidget; using namespace Core; -class UAVObjectBrowser : public Core::IUAVGadget -{ +class UAVObjectBrowser : public Core::IUAVGadget { Q_OBJECT public: UAVObjectBrowser(QString classId, UAVObjectBrowserWidget *widget, QWidget *parent = 0); ~UAVObjectBrowser(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private slots: - void viewOptionsChangedSlot(bool categorized,bool scientific,bool metadata); + void viewOptionsChangedSlot(bool categorized, bool scientific, bool metadata); private: UAVObjectBrowserWidget *m_widget; UAVObjectBrowserConfiguration *m_config; diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp index 53b51003b..f249d496a 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp @@ -10,24 +10,24 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavobjectbrowserconfiguration.h" -UAVObjectBrowserConfiguration::UAVObjectBrowserConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +UAVObjectBrowserConfiguration::UAVObjectBrowserConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_recentlyUpdatedColor(QColor(255, 230, 230)), m_manuallyChangedColor(QColor(230, 230, 255)), @@ -37,19 +37,19 @@ UAVObjectBrowserConfiguration::UAVObjectBrowserConfiguration(QString classId, QS m_useScientificView(false), m_showMetaData(false) { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { QColor recent = qSettings->value("recentlyUpdatedColor").value(); QColor manual = qSettings->value("manuallyChangedColor").value(); - int timeout = qSettings->value("recentlyUpdatedTimeout").toInt(); - bool hilight = qSettings->value("onlyHilightChangedValues").toBool(); + int timeout = qSettings->value("recentlyUpdatedTimeout").toInt(); + bool hilight = qSettings->value("onlyHilightChangedValues").toBool(); - m_useCategorizedView = qSettings->value("CategorizedView").toBool(); - m_useScientificView = qSettings->value("ScientificView").toBool(); + m_useCategorizedView = qSettings->value("CategorizedView").toBool(); + m_useScientificView = qSettings->value("ScientificView").toBool(); m_showMetaData = qSettings->value("showMetaData").toBool(); - m_recentlyUpdatedColor = recent; - m_manuallyChangedColor = manual; - m_recentlyUpdatedTimeout = timeout; + m_recentlyUpdatedColor = recent; + m_manuallyChangedColor = manual; + m_recentlyUpdatedTimeout = timeout; m_onlyHilightChangedValues = hilight; } } @@ -57,12 +57,13 @@ UAVObjectBrowserConfiguration::UAVObjectBrowserConfiguration(QString classId, QS IUAVGadgetConfiguration *UAVObjectBrowserConfiguration::clone() { UAVObjectBrowserConfiguration *m = new UAVObjectBrowserConfiguration(this->classId()); - m->m_recentlyUpdatedColor = m_recentlyUpdatedColor; - m->m_manuallyChangedColor = m_manuallyChangedColor; - m->m_recentlyUpdatedTimeout = m_recentlyUpdatedTimeout; + + m->m_recentlyUpdatedColor = m_recentlyUpdatedColor; + m->m_manuallyChangedColor = m_manuallyChangedColor; + m->m_recentlyUpdatedTimeout = m_recentlyUpdatedTimeout; m->m_onlyHilightChangedValues = m_onlyHilightChangedValues; m->m_useCategorizedView = m_useCategorizedView; - m->m_useScientificView = m_useScientificView; + m->m_useScientificView = m_useScientificView; m->m_showMetaData = m_showMetaData; return m; } @@ -71,7 +72,8 @@ IUAVGadgetConfiguration *UAVObjectBrowserConfiguration::clone() * Saves a configuration. * */ -void UAVObjectBrowserConfiguration::saveConfig(QSettings* qSettings) const { +void UAVObjectBrowserConfiguration::saveConfig(QSettings *qSettings) const +{ qSettings->setValue("recentlyUpdatedColor", m_recentlyUpdatedColor); qSettings->setValue("manuallyChangedColor", m_manuallyChangedColor); qSettings->setValue("recentlyUpdatedTimeout", m_recentlyUpdatedTimeout); diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h index 82e442021..43152d9d6 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,40 +33,80 @@ using namespace Core; -class UAVObjectBrowserConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT -Q_PROPERTY(QColor m_recentlyUpdatedColor READ recentlyUpdatedColor WRITE setRecentlyUpdatedColor) -Q_PROPERTY(QColor m_manuallyChangedColor READ manuallyChangedColor WRITE setManuallyChangedColor) -Q_PROPERTY(int m_recentlyUpdatedTimeout READ recentlyUpdatedTimeout WRITE setRecentlyUpdatedTimeout) -Q_PROPERTY(bool m_onlyHilightChangedValues READ onlyHighlightChangedValues WRITE setOnlyHighlightChangedValues) -Q_PROPERTY(bool m_useCategorizedView READ categorizedView WRITE setCategorizedView) -Q_PROPERTY(bool m_useScientificView READ scientificView WRITE setScientificView) -Q_PROPERTY(bool m_showMetaData READ showMetaData WRITE setShowMetaData) +class UAVObjectBrowserConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT Q_PROPERTY(QColor m_recentlyUpdatedColor READ recentlyUpdatedColor WRITE setRecentlyUpdatedColor) + Q_PROPERTY(QColor m_manuallyChangedColor READ manuallyChangedColor WRITE setManuallyChangedColor) + Q_PROPERTY(int m_recentlyUpdatedTimeout READ recentlyUpdatedTimeout WRITE setRecentlyUpdatedTimeout) + Q_PROPERTY(bool m_onlyHilightChangedValues READ onlyHighlightChangedValues WRITE setOnlyHighlightChangedValues) + Q_PROPERTY(bool m_useCategorizedView READ categorizedView WRITE setCategorizedView) + Q_PROPERTY(bool m_useScientificView READ scientificView WRITE setScientificView) + Q_PROPERTY(bool m_showMetaData READ showMetaData WRITE setShowMetaData) public: - explicit UAVObjectBrowserConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit UAVObjectBrowserConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); - QColor recentlyUpdatedColor() const { return m_recentlyUpdatedColor; } - QColor manuallyChangedColor() const { return m_manuallyChangedColor; } - int recentlyUpdatedTimeout() const { return m_recentlyUpdatedTimeout; } - bool onlyHighlightChangedValues() const {return m_onlyHilightChangedValues;} - bool categorizedView() const { return m_useCategorizedView; } - bool scientificView() const { return m_useScientificView; } - bool showMetaData() const { return m_showMetaData; } + QColor recentlyUpdatedColor() const + { + return m_recentlyUpdatedColor; + } + QColor manuallyChangedColor() const + { + return m_manuallyChangedColor; + } + int recentlyUpdatedTimeout() const + { + return m_recentlyUpdatedTimeout; + } + bool onlyHighlightChangedValues() const + { + return m_onlyHilightChangedValues; + } + bool categorizedView() const + { + return m_useCategorizedView; + } + bool scientificView() const + { + return m_useScientificView; + } + bool showMetaData() const + { + return m_showMetaData; + } signals: public slots: - void setRecentlyUpdatedColor(QColor color) { m_recentlyUpdatedColor = color; } - void setManuallyChangedColor(QColor color) { m_manuallyChangedColor = color; } - void setRecentlyUpdatedTimeout(int timeout) { m_recentlyUpdatedTimeout = timeout; } - void setOnlyHighlightChangedValues(bool hilight) { m_onlyHilightChangedValues = hilight; } - void setCategorizedView(bool value) { m_useCategorizedView = value; } - void setScientificView(bool value) { m_useScientificView = value; } - void setShowMetaData(bool value) { m_showMetaData = value; } + void setRecentlyUpdatedColor(QColor color) + { + m_recentlyUpdatedColor = color; + } + void setManuallyChangedColor(QColor color) + { + m_manuallyChangedColor = color; + } + void setRecentlyUpdatedTimeout(int timeout) + { + m_recentlyUpdatedTimeout = timeout; + } + void setOnlyHighlightChangedValues(bool hilight) + { + m_onlyHilightChangedValues = hilight; + } + void setCategorizedView(bool value) + { + m_useCategorizedView = value; + } + void setScientificView(bool value) + { + m_useScientificView = value; + } + void setShowMetaData(bool value) + { + m_showMetaData = value; + } private: QColor m_recentlyUpdatedColor; diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.cpp index 3782ea87d..2fb7ee2ab 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.cpp @@ -32,21 +32,20 @@ #include UAVObjectBrowserFactory::UAVObjectBrowserFactory(QObject *parent) : - IUAVGadgetFactory(QString("UAVObjectBrowser"), tr("UAVObject Browser"), parent) -{ -} + IUAVGadgetFactory(QString("UAVObjectBrowser"), tr("UAVObject Browser"), parent) +{} UAVObjectBrowserFactory::~UAVObjectBrowserFactory() -{ -} +{} -Core::IUAVGadget* UAVObjectBrowserFactory::createGadget(QWidget *parent) +Core::IUAVGadget *UAVObjectBrowserFactory::createGadget(QWidget *parent) { - UAVObjectBrowserWidget* gadgetWidget = new UAVObjectBrowserWidget(parent); + UAVObjectBrowserWidget *gadgetWidget = new UAVObjectBrowserWidget(parent); + return new UAVObjectBrowser(QString("UAVObjectBrowser"), gadgetWidget, parent); } -IUAVGadgetConfiguration *UAVObjectBrowserFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *UAVObjectBrowserFactory::createConfiguration(QSettings *qSettings) { return new UAVObjectBrowserConfiguration(QString("UAVObjectBrowser"), qSettings); } @@ -54,6 +53,5 @@ IUAVGadgetConfiguration *UAVObjectBrowserFactory::createConfiguration(QSettings* IOptionsPage *UAVObjectBrowserFactory::createOptionsPage(IUAVGadgetConfiguration *config) { - return new UAVObjectBrowserOptionsPage(qobject_cast(config)); + return new UAVObjectBrowserOptionsPage(qobject_cast(config)); } - diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.h index cf8e988a6..a85fbeb1d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserfactory.h @@ -37,15 +37,14 @@ class IUAVGadgetFactory; using namespace Core; -class UAVObjectBrowserFactory : public Core::IUAVGadgetFactory -{ +class UAVObjectBrowserFactory : public Core::IUAVGadgetFactory { Q_OBJECT public: UAVObjectBrowserFactory(QObject *parent = 0); ~UAVObjectBrowserFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); }; diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.cpp index b36a33de7..a6cce1027 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.cpp @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,8 +40,7 @@ UAVObjectBrowserOptionsPage::UAVObjectBrowserOptionsPage(UAVObjectBrowserConfiguration *config, QObject *parent) : IOptionsPage(parent), m_config(config) -{ -} +{} QWidget *UAVObjectBrowserOptionsPage::createPage(QWidget *parent) { @@ -55,7 +54,6 @@ QWidget *UAVObjectBrowserOptionsPage::createPage(QWidget *parent) m_page->hilightBox->setChecked(m_config->onlyHighlightChangedValues()); return w; - } void UAVObjectBrowserOptionsPage::apply() diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.h index c49b9d7b2..ebe3e21eb 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -44,12 +44,11 @@ class QSpinBox; using namespace Core; namespace Ui { - class UAVObjectBrowserOptionsPage; +class UAVObjectBrowserOptionsPage; } -class UAVObjectBrowserOptionsPage : public IOptionsPage -{ -Q_OBJECT +class UAVObjectBrowserOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit UAVObjectBrowserOptionsPage(UAVObjectBrowserConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp index 06e2168dc..9ada2e9fa 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp @@ -42,7 +42,7 @@ UAVObjectBrowserWidget::UAVObjectBrowserWidget(QWidget *parent) : QWidget(parent) { - m_browser = new Ui_UAVObjectBrowser(); + m_browser = new Ui_UAVObjectBrowser(); m_viewoptions = new Ui_viewoptions(); m_viewoptionsDialog = new QDialog(this); m_viewoptions->setupUi(m_viewoptionsDialog); @@ -50,13 +50,13 @@ UAVObjectBrowserWidget::UAVObjectBrowserWidget(QWidget *parent) : QWidget(parent m_model = new UAVObjectTreeModel(); m_browser->treeView->setModel(m_model); m_browser->treeView->setColumnWidth(0, 300); - //m_browser->treeView->expandAll(); + // m_browser->treeView->expandAll(); BrowserItemDelegate *m_delegate = new BrowserItemDelegate(); m_browser->treeView->setItemDelegate(m_delegate); m_browser->treeView->setEditTriggers(QAbstractItemView::AllEditTriggers); m_browser->treeView->setSelectionBehavior(QAbstractItemView::SelectItems); showMetaData(m_viewoptions->cbMetaData->isChecked()); - connect(m_browser->treeView->selectionModel(), SIGNAL(currentChanged(QModelIndex,QModelIndex)), this, SLOT(currentChanged(QModelIndex,QModelIndex))); + connect(m_browser->treeView->selectionModel(), SIGNAL(currentChanged(QModelIndex, QModelIndex)), this, SLOT(currentChanged(QModelIndex, QModelIndex))); connect(m_viewoptions->cbMetaData, SIGNAL(toggled(bool)), this, SLOT(showMetaData(bool))); connect(m_viewoptions->cbCategorized, SIGNAL(toggled(bool)), this, SLOT(categorize(bool))); connect(m_browser->saveSDButton, SIGNAL(clicked()), this, SLOT(saveObject())); @@ -64,7 +64,7 @@ UAVObjectBrowserWidget::UAVObjectBrowserWidget(QWidget *parent) : QWidget(parent connect(m_browser->eraseSDButton, SIGNAL(clicked()), this, SLOT(eraseObject())); connect(m_browser->sendButton, SIGNAL(clicked()), this, SLOT(sendUpdate())); connect(m_browser->requestButton, SIGNAL(clicked()), this, SLOT(requestUpdate())); - connect(m_browser->tbView,SIGNAL(clicked()),this,SLOT(viewSlot())); + connect(m_browser->tbView, SIGNAL(clicked()), this, SLOT(viewSlot())); connect(m_viewoptions->cbScientific, SIGNAL(toggled(bool)), this, SLOT(useScientificNotation(bool))); connect(m_viewoptions->cbScientific, SIGNAL(toggled(bool)), this, SLOT(viewOptionsChangedSlot())); connect(m_viewoptions->cbMetaData, SIGNAL(toggled(bool)), this, SLOT(viewOptionsChangedSlot())); @@ -87,8 +87,7 @@ void UAVObjectBrowserWidget::setViewOptions(bool categorized, bool scientific, b void UAVObjectBrowserWidget::showMetaData(bool show) { QList metaIndexes = m_model->getMetaDataIndexes(); - foreach(QModelIndex index , metaIndexes) - { + foreach(QModelIndex index, metaIndexes) { m_browser->treeView->setRowHidden(index.row(), index.parent(), !show); } } @@ -96,12 +95,13 @@ void UAVObjectBrowserWidget::showMetaData(bool show) void UAVObjectBrowserWidget::categorize(bool categorize) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm); UAVObjectManager *objManager = pm->getObject(); Q_ASSERT(objManager); - UAVObjectTreeModel* tmpModel = m_model; - m_model = new UAVObjectTreeModel(0, categorize,m_viewoptions->cbScientific->isChecked()); + UAVObjectTreeModel *tmpModel = m_model; + m_model = new UAVObjectTreeModel(0, categorize, m_viewoptions->cbScientific->isChecked()); m_model->setRecentlyUpdatedColor(m_recentlyUpdatedColor); m_model->setManuallyChangedColor(m_manuallyChangedColor); m_model->setRecentlyUpdatedTimeout(m_recentlyUpdatedTimeout); @@ -115,12 +115,13 @@ void UAVObjectBrowserWidget::categorize(bool categorize) void UAVObjectBrowserWidget::useScientificNotation(bool scientific) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm); UAVObjectManager *objManager = pm->getObject(); Q_ASSERT(objManager); - UAVObjectTreeModel* tmpModel = m_model; - m_model = new UAVObjectTreeModel(0, m_viewoptions->cbCategorized,scientific); + UAVObjectTreeModel *tmpModel = m_model; + m_model = new UAVObjectTreeModel(0, m_viewoptions->cbCategorized, scientific); m_model->setRecentlyUpdatedColor(m_recentlyUpdatedColor); m_model->setManuallyChangedColor(m_manuallyChangedColor); m_model->setRecentlyUpdatedTimeout(m_recentlyUpdatedTimeout); @@ -144,6 +145,7 @@ void UAVObjectBrowserWidget::sendUpdate() void UAVObjectBrowserWidget::requestUpdate() { ObjectTreeItem *objItem = findCurrentObjectTreeItem(); + Q_ASSERT(objItem); UAVObject *obj = objItem->object(); Q_ASSERT(obj); @@ -152,13 +154,15 @@ void UAVObjectBrowserWidget::requestUpdate() ObjectTreeItem *UAVObjectBrowserWidget::findCurrentObjectTreeItem() { - QModelIndex current = m_browser->treeView->currentIndex(); - TreeItem *item = static_cast(current.internalPointer()); + QModelIndex current = m_browser->treeView->currentIndex(); + TreeItem *item = static_cast(current.internalPointer()); ObjectTreeItem *objItem = 0; + while (item) { - objItem = dynamic_cast(item); - if (objItem) + objItem = dynamic_cast(item); + if (objItem) { break; + } item = item->parent(); } return objItem; @@ -181,6 +185,7 @@ void UAVObjectBrowserWidget::loadObject() { // Load object ObjectTreeItem *objItem = findCurrentObjectTreeItem(); + Q_ASSERT(objItem); UAVObject *obj = objItem->object(); Q_ASSERT(obj); @@ -192,6 +197,7 @@ void UAVObjectBrowserWidget::loadObject() void UAVObjectBrowserWidget::eraseObject() { ObjectTreeItem *objItem = findCurrentObjectTreeItem(); + Q_ASSERT(objItem); UAVObject *obj = objItem->object(); Q_ASSERT(obj); @@ -202,13 +208,13 @@ void UAVObjectBrowserWidget::updateObjectPersistance(ObjectPersistence::Operatio { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - ObjectPersistence* objper = dynamic_cast( objManager->getObject(ObjectPersistence::NAME) ); - if (obj != NULL) - { + ObjectPersistence *objper = dynamic_cast(objManager->getObject(ObjectPersistence::NAME)); + + if (obj != NULL) { ObjectPersistence::DataFields data; - data.Operation = op; - data.Selection = ObjectPersistence::SELECTION_SINGLEOBJECT; - data.ObjectID = obj->getObjID(); + data.Operation = op; + data.Selection = ObjectPersistence::SELECTION_SINGLEOBJECT; + data.ObjectID = obj->getObjID(); data.InstanceID = obj->getInstID(); objper->setData(data); objper->updated(); @@ -219,25 +225,26 @@ void UAVObjectBrowserWidget::currentChanged(const QModelIndex ¤t, const QM { Q_UNUSED(previous); - TreeItem *item = static_cast(current.internalPointer()); - bool enable = true; - if (current == QModelIndex()) + TreeItem *item = static_cast(current.internalPointer()); + bool enable = true; + if (current == QModelIndex()) { enable = false; - TopTreeItem *top = dynamic_cast(item); - ObjectTreeItem *data = dynamic_cast(item); - if (top || (data && !data->object())) + } + TopTreeItem *top = dynamic_cast(item); + ObjectTreeItem *data = dynamic_cast(item); + if (top || (data && !data->object())) { enable = false; + } enableSendRequest(enable); } void UAVObjectBrowserWidget::viewSlot() { - if(m_viewoptionsDialog->isVisible()) + if (m_viewoptionsDialog->isVisible()) { m_viewoptionsDialog->setVisible(false); - else - { - QPoint pos=QCursor::pos(); - pos.setX(pos.x()-m_viewoptionsDialog->width()); + } else { + QPoint pos = QCursor::pos(); + pos.setX(pos.x() - m_viewoptionsDialog->width()); m_viewoptionsDialog->move(pos); m_viewoptionsDialog->show(); } @@ -245,7 +252,7 @@ void UAVObjectBrowserWidget::viewSlot() void UAVObjectBrowserWidget::viewOptionsChangedSlot() { - emit viewOptionsChanged(m_viewoptions->cbCategorized->isChecked(),m_viewoptions->cbScientific->isChecked(),m_viewoptions->cbMetaData->isChecked()); + emit viewOptionsChanged(m_viewoptions->cbCategorized->isChecked(), m_viewoptions->cbScientific->isChecked(), m_viewoptions->cbMetaData->isChecked()); } void UAVObjectBrowserWidget::enableSendRequest(bool enable) @@ -256,5 +263,3 @@ void UAVObjectBrowserWidget::enableSendRequest(bool enable) m_browser->readSDButton->setEnabled(enable); m_browser->eraseSDButton->setEnabled(enable); } - - diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h index 805bbbaf8..707473e3d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h @@ -38,18 +38,29 @@ class ObjectTreeItem; class Ui_UAVObjectBrowser; class Ui_viewoptions; -class UAVObjectBrowserWidget : public QWidget -{ +class UAVObjectBrowserWidget : public QWidget { Q_OBJECT public: UAVObjectBrowserWidget(QWidget *parent = 0); ~UAVObjectBrowserWidget(); - void setRecentlyUpdatedColor(QColor color) { m_recentlyUpdatedColor = color; m_model->setRecentlyUpdatedColor(color); } - void setManuallyChangedColor(QColor color) { m_manuallyChangedColor = color; m_model->setManuallyChangedColor(color); } - void setRecentlyUpdatedTimeout(int timeout) { m_recentlyUpdatedTimeout = timeout; m_model->setRecentlyUpdatedTimeout(timeout); } - void setOnlyHilightChangedValues(bool hilight) { m_onlyHilightChangedValues = hilight; m_model->setOnlyHilightChangedValues(hilight); } - void setViewOptions(bool categorized,bool scientific,bool metadata); + void setRecentlyUpdatedColor(QColor color) + { + m_recentlyUpdatedColor = color; m_model->setRecentlyUpdatedColor(color); + } + void setManuallyChangedColor(QColor color) + { + m_manuallyChangedColor = color; m_model->setManuallyChangedColor(color); + } + void setRecentlyUpdatedTimeout(int timeout) + { + m_recentlyUpdatedTimeout = timeout; m_model->setRecentlyUpdatedTimeout(timeout); + } + void setOnlyHilightChangedValues(bool hilight) + { + m_onlyHilightChangedValues = hilight; m_model->setOnlyHilightChangedValues(hilight); + } + void setViewOptions(bool categorized, bool scientific, bool metadata); public slots: void showMetaData(bool show); void categorize(bool categorize); @@ -65,7 +76,7 @@ private slots: void viewSlot(); void viewOptionsChangedSlot(); signals: - void viewOptionsChanged(bool categorized,bool scientific,bool metadata); + void viewOptionsChanged(bool categorized, bool scientific, bool metadata); private: QPushButton *m_requestUpdate; QPushButton *m_sendUpdate; diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp index 06c9f0016..42a9289b3 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,25 +33,25 @@ #include "uavobjectfield.h" #include "extensionsystem/pluginmanager.h" #include -//#include +// #include #include #include #include UAVObjectTreeModel::UAVObjectTreeModel(QObject *parent, bool categorize, bool useScientificNotation) : - QAbstractItemModel(parent), - m_useScientificFloatNotation(useScientificNotation), - m_recentlyUpdatedTimeout(500), // ms - m_recentlyUpdatedColor(QColor(255, 230, 230)), - m_manuallyChangedColor(QColor(230, 230, 255)) + QAbstractItemModel(parent), + m_useScientificFloatNotation(useScientificNotation), + m_recentlyUpdatedTimeout(500), // ms + m_recentlyUpdatedColor(QColor(255, 230, 230)), + m_manuallyChangedColor(QColor(230, 230, 255)) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); // Create highlight manager, let it run every 300 ms. m_highlightManager = new HighLightManager(300); - connect(objManager, SIGNAL(newObject(UAVObject*)), this, SLOT(newObject(UAVObject*))); - connect(objManager, SIGNAL(newInstance(UAVObject*)), this, SLOT(newObject(UAVObject*))); + connect(objManager, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *))); + connect(objManager, SIGNAL(newInstance(UAVObject *)), this, SLOT(newObject(UAVObject *))); TreeItem::setHighlightTime(m_recentlyUpdatedTimeout); setupModelData(objManager, categorize); @@ -68,21 +68,21 @@ void UAVObjectTreeModel::setupModelData(UAVObjectManager *objManager, bool categ // root QList rootData; rootData << tr("Property") << tr("Value") << tr("Unit"); - m_rootItem = new TreeItem(rootData); + m_rootItem = new TreeItem(rootData); - m_settingsTree = new TopTreeItem(tr("Settings"), m_rootItem); + m_settingsTree = new TopTreeItem(tr("Settings"), m_rootItem); m_settingsTree->setHighlightManager(m_highlightManager); m_rootItem->appendChild(m_settingsTree); m_nonSettingsTree = new TopTreeItem(tr("Data Objects"), m_rootItem); m_nonSettingsTree->setHighlightManager(m_highlightManager); m_rootItem->appendChild(m_nonSettingsTree); m_rootItem->setHighlightManager(m_highlightManager); - connect(m_settingsTree, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); - connect(m_nonSettingsTree, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(m_settingsTree, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); + connect(m_nonSettingsTree, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); - QList< QList > objList = objManager->getDataObjects(); - foreach (QList list, objList) { - foreach (UAVDataObject* obj, list) { + QList< QList > objList = objManager->getDataObjects(); + foreach(QList list, objList) { + foreach(UAVDataObject * obj, list) { addDataObject(obj, categorize); } } @@ -90,7 +90,8 @@ void UAVObjectTreeModel::setupModelData(UAVObjectManager *objManager, bool categ void UAVObjectTreeModel::newObject(UAVObject *obj) { - UAVDataObject *dobj = qobject_cast(obj); + UAVDataObject *dobj = qobject_cast(obj); + if (dobj) { addDataObject(dobj); } @@ -100,56 +101,57 @@ void UAVObjectTreeModel::addDataObject(UAVDataObject *obj, bool categorize) { TopTreeItem *root = obj->isSettings() ? m_settingsTree : m_nonSettingsTree; - TreeItem* parent = root; + TreeItem *parent = root; - if(categorize && obj->getCategory() != 0 && !obj->getCategory().isEmpty()) { + if (categorize && obj->getCategory() != 0 && !obj->getCategory().isEmpty()) { QStringList categoryPath = obj->getCategory().split('/'); parent = createCategoryItems(categoryPath, root); } - ObjectTreeItem* existing = root->findDataObjectTreeItemByObjectId(obj->getObjID()); + ObjectTreeItem *existing = root->findDataObjectTreeItemByObjectId(obj->getObjID()); if (existing) { addInstance(obj, existing); } else { DataObjectTreeItem *dataTreeItem = new DataObjectTreeItem(obj->getName() + " (" + QString::number(obj->getNumBytes()) + " bytes)"); dataTreeItem->setHighlightManager(m_highlightManager); - connect(dataTreeItem, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(dataTreeItem, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); parent->insertChild(dataTreeItem); root->addObjectTreeItem(obj->getObjID(), dataTreeItem); UAVMetaObject *meta = obj->getMetaObject(); - MetaObjectTreeItem* metaTreeItem = addMetaObject(meta, dataTreeItem); + MetaObjectTreeItem *metaTreeItem = addMetaObject(meta, dataTreeItem); root->addMetaObjectTreeItem(meta->getObjID(), metaTreeItem); addInstance(obj, dataTreeItem); } } -TreeItem* UAVObjectTreeModel::createCategoryItems(QStringList categoryPath, TreeItem* root) +TreeItem *UAVObjectTreeModel::createCategoryItems(QStringList categoryPath, TreeItem *root) { - TreeItem* parent = root; + TreeItem *parent = root; + foreach(QString category, categoryPath) { - TreeItem* existing = parent->findChildByName(category); - if(!existing) { - TreeItem* categoryItem = new TreeItem(category); - connect(categoryItem, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + TreeItem *existing = parent->findChildByName(category); + + if (!existing) { + TreeItem *categoryItem = new TreeItem(category); + connect(categoryItem, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); categoryItem->setHighlightManager(m_highlightManager); parent->insertChild(categoryItem); parent = categoryItem; - } - else { + } else { parent = existing; } } return parent; } -MetaObjectTreeItem* UAVObjectTreeModel::addMetaObject(UAVMetaObject *obj, TreeItem *parent) +MetaObjectTreeItem *UAVObjectTreeModel::addMetaObject(UAVMetaObject *obj, TreeItem *parent) { - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(highlightUpdatedObject(UAVObject*))); + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(highlightUpdatedObject(UAVObject *))); MetaObjectTreeItem *meta = new MetaObjectTreeItem(obj, tr("Meta Data")); meta->setHighlightManager(m_highlightManager); - connect(meta, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); - foreach (UAVObjectField *field, obj->getFields()) { + connect(meta, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); + foreach(UAVObjectField * field, obj->getFields()) { if (field->getNumElements() > 1) { addArrayField(field, meta); } else { @@ -162,20 +164,20 @@ MetaObjectTreeItem* UAVObjectTreeModel::addMetaObject(UAVMetaObject *obj, TreeIt void UAVObjectTreeModel::addInstance(UAVObject *obj, TreeItem *parent) { - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(highlightUpdatedObject(UAVObject*))); + connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(highlightUpdatedObject(UAVObject *))); TreeItem *item; if (obj->isSingleInstance()) { item = parent; - DataObjectTreeItem *p = static_cast(parent); + DataObjectTreeItem *p = static_cast(parent); p->setObject(obj); } else { - QString name = tr("Instance") + " " + QString::number(obj->getInstID()); + QString name = tr("Instance") + " " + QString::number(obj->getInstID()); item = new InstanceTreeItem(obj, name); item->setHighlightManager(m_highlightManager); - connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(item, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); parent->appendChild(item); } - foreach (UAVObjectField *field, obj->getFields()) { + foreach(UAVObjectField * field, obj->getFields()) { if (field->getNumElements() > 1) { addArrayField(field, item); } else { @@ -187,8 +189,9 @@ void UAVObjectTreeModel::addInstance(UAVObject *obj, TreeItem *parent) void UAVObjectTreeModel::addArrayField(UAVObjectField *field, TreeItem *parent) { TreeItem *item = new ArrayFieldTreeItem(field->getName()); + item->setHighlightManager(m_highlightManager); - connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(item, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); for (uint i = 0; i < field->getNumElements(); ++i) { addSingleField(i, field, item); } @@ -198,19 +201,21 @@ void UAVObjectTreeModel::addArrayField(UAVObjectField *field, TreeItem *parent) void UAVObjectTreeModel::addSingleField(int index, UAVObjectField *field, TreeItem *parent) { QList data; - if (field->getNumElements() == 1) + if (field->getNumElements() == 1) { data.append(field->getName()); - else - data.append( QString("[%1]").arg((field->getElementNames())[index]) ); + } else { + data.append(QString("[%1]").arg((field->getElementNames())[index])); + } FieldTreeItem *item; UAVObjectField::FieldType type = field->getType(); switch (type) { case UAVObjectField::BITFIELD: - case UAVObjectField::ENUM: { + case UAVObjectField::ENUM: + { QStringList options = field->getOptions(); QVariant value = field->getValue(); - data.append( options.indexOf(value.toString()) ); + data.append(options.indexOf(value.toString())); data.append(field->getUnits()); item = new EnumFieldTreeItem(field, index, data); break; @@ -234,42 +239,47 @@ void UAVObjectTreeModel::addSingleField(int index, UAVObjectField *field, TreeIt Q_ASSERT(false); } item->setHighlightManager(m_highlightManager); - connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + connect(item, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *))); parent->appendChild(item); } QModelIndex UAVObjectTreeModel::index(int row, int column, const QModelIndex &parent) - const +const { - if (!hasIndex(row, column, parent)) + if (!hasIndex(row, column, parent)) { return QModelIndex(); + } TreeItem *parentItem; - if (!parent.isValid()) + if (!parent.isValid()) { parentItem = m_rootItem; - else - parentItem = static_cast(parent.internalPointer()); + } else { + parentItem = static_cast(parent.internalPointer()); + } TreeItem *childItem = parentItem->getChild(row); - if (childItem) + if (childItem) { return createIndex(row, column, childItem); - else + } else { return QModelIndex(); + } } QModelIndex UAVObjectTreeModel::index(TreeItem *item) { - if (item->parent() == 0) + if (item->parent() == 0) { return QModelIndex(); + } QModelIndex root = index(item->parent()); for (int i = 0; i < rowCount(root); ++i) { QModelIndex childIndex = index(i, 0, root); - TreeItem *child = static_cast(childIndex.internalPointer()); - if (child == item) + TreeItem *child = static_cast(childIndex.internalPointer()); + if (child == item) { return childIndex; + } } Q_ASSERT(false); return QModelIndex(); @@ -277,14 +287,16 @@ QModelIndex UAVObjectTreeModel::index(TreeItem *item) QModelIndex UAVObjectTreeModel::parent(const QModelIndex &index) const { - if (!index.isValid()) + if (!index.isValid()) { return QModelIndex(); + } - TreeItem *childItem = static_cast(index.internalPointer()); + TreeItem *childItem = static_cast(index.internalPointer()); TreeItem *parentItem = childItem->parent(); - if (parentItem == m_rootItem) + if (parentItem == m_rootItem) { return QModelIndex(); + } return createIndex(parentItem->row(), 0, parentItem); } @@ -292,35 +304,37 @@ QModelIndex UAVObjectTreeModel::parent(const QModelIndex &index) const int UAVObjectTreeModel::rowCount(const QModelIndex &parent) const { TreeItem *parentItem; - if (parent.column() > 0) - return 0; - if (!parent.isValid()) + if (parent.column() > 0) { + return 0; + } + + if (!parent.isValid()) { parentItem = m_rootItem; - else - parentItem = static_cast(parent.internalPointer()); + } else { + parentItem = static_cast(parent.internalPointer()); + } return parentItem->childCount(); } int UAVObjectTreeModel::columnCount(const QModelIndex &parent) const { - if (parent.isValid()) - return static_cast(parent.internalPointer())->columnCount(); - else + if (parent.isValid()) { + return static_cast(parent.internalPointer())->columnCount(); + } else { return m_rootItem->columnCount(); + } } QList UAVObjectTreeModel::getMetaDataIndexes() { QList metaIndexes; - foreach(MetaObjectTreeItem *metaItem , m_settingsTree->getMetaObjectItems()) - { + foreach(MetaObjectTreeItem * metaItem, m_settingsTree->getMetaObjectItems()) { metaIndexes.append(index(metaItem)); } - foreach(MetaObjectTreeItem *metaItem , m_nonSettingsTree->getMetaObjectItems()) - { + foreach(MetaObjectTreeItem * metaItem, m_nonSettingsTree->getMetaObjectItems()) { metaIndexes.append(index(metaItem)); } return metaIndexes; @@ -328,43 +342,48 @@ QList UAVObjectTreeModel::getMetaDataIndexes() QVariant UAVObjectTreeModel::data(const QModelIndex &index, int role) const { - if (!index.isValid()) - return QVariant(); + if (!index.isValid()) { + return QVariant(); + } if (index.column() == TreeItem::dataColumn && role == Qt::EditRole) { - TreeItem *item = static_cast(index.internalPointer()); + TreeItem *item = static_cast(index.internalPointer()); return item->data(index.column()); } -// if (role == Qt::DecorationRole) -// return QIcon(":/core/images/openpilot_logo_128.png"); +// if (role == Qt::DecorationRole) +// return QIcon(":/core/images/openpilot_logo_128.png"); if (role == Qt::ToolTipRole) { - TreeItem *item = static_cast(index.internalPointer()); + TreeItem *item = static_cast(index.internalPointer()); return item->description(); } - TreeItem *item = static_cast(index.internalPointer()); + TreeItem *item = static_cast(index.internalPointer()); if (index.column() == 0 && role == Qt::BackgroundRole) { - if (!dynamic_cast(item) && item->highlighted()) + if (!dynamic_cast(item) && item->highlighted()) { return QVariant(m_recentlyUpdatedColor); + } } if (index.column() == TreeItem::dataColumn && role == Qt::BackgroundRole) { - FieldTreeItem *fieldItem = dynamic_cast(item); - if (fieldItem && fieldItem->highlighted()) + FieldTreeItem *fieldItem = dynamic_cast(item); + if (fieldItem && fieldItem->highlighted()) { return QVariant(m_recentlyUpdatedColor); + } - if (fieldItem && fieldItem->changed()) - return QVariant(m_manuallyChangedColor); + if (fieldItem && fieldItem->changed()) { + return QVariant(m_manuallyChangedColor); + } } - if (role != Qt::DisplayRole) + if (role != Qt::DisplayRole) { return QVariant(); + } if (index.column() == TreeItem::dataColumn) { - EnumFieldTreeItem *fieldItem = dynamic_cast(item); + EnumFieldTreeItem *fieldItem = dynamic_cast(item); if (fieldItem) { int enumIndex = fieldItem->data(index.column()).toInt(); return fieldItem->enumOptions(enumIndex); @@ -377,7 +396,7 @@ QVariant UAVObjectTreeModel::data(const QModelIndex &index, int role) const bool UAVObjectTreeModel::setData(const QModelIndex &index, const QVariant & value, int role) { Q_UNUSED(role) - TreeItem *item = static_cast(index.internalPointer()); + TreeItem * item = static_cast(index.internalPointer()); item->setData(value, index.column()); return true; } @@ -385,13 +404,15 @@ bool UAVObjectTreeModel::setData(const QModelIndex &index, const QVariant & valu Qt::ItemFlags UAVObjectTreeModel::flags(const QModelIndex &index) const { - if (!index.isValid()) + if (!index.isValid()) { return 0; + } if (index.column() == TreeItem::dataColumn) { - TreeItem *item = static_cast(index.internalPointer()); - if (item->isEditable()) + TreeItem *item = static_cast(index.internalPointer()); + if (item->isEditable()) { return Qt::ItemIsEnabled | Qt::ItemIsSelectable | Qt::ItemIsEditable; + } } return Qt::ItemIsEnabled | Qt::ItemIsSelectable; @@ -400,8 +421,9 @@ Qt::ItemFlags UAVObjectTreeModel::flags(const QModelIndex &index) const QVariant UAVObjectTreeModel::headerData(int section, Qt::Orientation orientation, int role) const { - if (orientation == Qt::Horizontal && role == Qt::DisplayRole) + if (orientation == Qt::Horizontal && role == Qt::DisplayRole) { return m_rootItem->data(section); + } return QVariant(); } @@ -411,11 +433,11 @@ void UAVObjectTreeModel::highlightUpdatedObject(UAVObject *obj) Q_ASSERT(obj); ObjectTreeItem *item = findObjectTreeItem(obj); Q_ASSERT(item); - if(!m_onlyHilightChangedValues){ + if (!m_onlyHilightChangedValues) { item->setHighlight(true); } item->update(); - if(!m_onlyHilightChangedValues){ + if (!m_onlyHilightChangedValues) { QModelIndex itemIndex = index(item); Q_ASSERT(itemIndex != QModelIndex()); emit dataChanged(itemIndex, itemIndex); @@ -424,8 +446,9 @@ void UAVObjectTreeModel::highlightUpdatedObject(UAVObject *obj) ObjectTreeItem *UAVObjectTreeModel::findObjectTreeItem(UAVObject *object) { - UAVDataObject *dataObject = qobject_cast(object); - UAVMetaObject *metaObject = qobject_cast(object); + UAVDataObject *dataObject = qobject_cast(object); + UAVMetaObject *metaObject = qobject_cast(object); + Q_ASSERT(dataObject || metaObject); if (dataObject) { return findDataObjectTreeItem(dataObject); @@ -435,15 +458,17 @@ ObjectTreeItem *UAVObjectTreeModel::findObjectTreeItem(UAVObject *object) return 0; } -DataObjectTreeItem* UAVObjectTreeModel::findDataObjectTreeItem(UAVDataObject *obj) +DataObjectTreeItem *UAVObjectTreeModel::findDataObjectTreeItem(UAVDataObject *obj) { TopTreeItem *root = obj->isSettings() ? m_settingsTree : m_nonSettingsTree; + return root->findDataObjectTreeItemByObjectId(obj->getObjID()); } -MetaObjectTreeItem* UAVObjectTreeModel::findMetaObjectTreeItem(UAVMetaObject *obj) +MetaObjectTreeItem *UAVObjectTreeModel::findMetaObjectTreeItem(UAVMetaObject *obj) { - UAVDataObject *dataObject = qobject_cast(obj->getParentObject()); + UAVDataObject *dataObject = qobject_cast(obj->getParentObject()); + Q_ASSERT(dataObject); TopTreeItem *root = dataObject->isSettings() ? m_settingsTree : m_nonSettingsTree; return root->findMetaObjectTreeItemByObjectId(obj->getObjID()); @@ -452,8 +477,7 @@ MetaObjectTreeItem* UAVObjectTreeModel::findMetaObjectTreeItem(UAVMetaObject *ob void UAVObjectTreeModel::updateHighlight(TreeItem *item) { QModelIndex itemIndex = index(item); + Q_ASSERT(itemIndex != QModelIndex()); emit dataChanged(itemIndex, itemIndex.sibling(itemIndex.row(), TreeItem::dataColumn)); } - - diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.h index 174210bd2..85e48159a 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.h @@ -10,18 +10,18 @@ * @brief The UAVObject Browser gadget plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -45,11 +45,10 @@ class UAVObjectManager; class QSignalMapper; class QTimer; -class UAVObjectTreeModel : public QAbstractItemModel -{ -Q_OBJECT +class UAVObjectTreeModel : public QAbstractItemModel { + Q_OBJECT public: - explicit UAVObjectTreeModel(QObject *parent = 0, bool categorize=true, bool useScientificNotation=false); + explicit UAVObjectTreeModel(QObject *parent = 0, bool categorize = true, bool useScientificNotation = false); ~UAVObjectTreeModel(); QVariant data(const QModelIndex &index, int role) const; @@ -63,13 +62,23 @@ public: int rowCount(const QModelIndex &parent = QModelIndex()) const; int columnCount(const QModelIndex &parent = QModelIndex()) const; - void setRecentlyUpdatedColor(QColor color) { m_recentlyUpdatedColor = color; } - void setManuallyChangedColor(QColor color) { m_manuallyChangedColor = color; } - void setRecentlyUpdatedTimeout(int timeout) { + void setRecentlyUpdatedColor(QColor color) + { + m_recentlyUpdatedColor = color; + } + void setManuallyChangedColor(QColor color) + { + m_manuallyChangedColor = color; + } + void setRecentlyUpdatedTimeout(int timeout) + { m_recentlyUpdatedTimeout = timeout; TreeItem::setHighlightTime(timeout); } - void setOnlyHilightChangedValues(bool hilight) {m_onlyHilightChangedValues = hilight; } + void setOnlyHilightChangedValues(bool hilight) + { + m_onlyHilightChangedValues = hilight; + } QList getMetaDataIndexes(); @@ -80,7 +89,7 @@ public slots: private slots: void highlightUpdatedObject(UAVObject *obj); - void updateHighlight(TreeItem*); + void updateHighlight(TreeItem *); private: void setupModelData(UAVObjectManager *objManager, bool categorize = true); diff --git a/ground/openpilotgcs/src/plugins/uavobjects/tests/main.cpp b/ground/openpilotgcs/src/plugins/uavobjects/tests/main.cpp index 2ffbb23b0..2f4b9bcda 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/tests/main.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/tests/main.cpp @@ -6,7 +6,7 @@ int main(int argc, char *argv[]) { QCoreApplication a(argc, argv); - UAVObjectsTest* test = new UAVObjectsTest(); + UAVObjectsTest *test = new UAVObjectsTest(); return a.exec(); } diff --git a/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.cpp b/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.cpp index 6b86ea6f9..8259dde64 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.cpp @@ -1,19 +1,19 @@ #include "uavobjectstest.h" -UAVObjectsTest::UAVObjectsTest(): sout(stdout), done(false) +UAVObjectsTest::UAVObjectsTest() : sout(stdout), done(false) { // Create object manager objMngr = new UAVObjectManager(); - connect(objMngr, SIGNAL(newObject(UAVObject*)), this, SLOT(newObject(UAVObject*))); - connect(objMngr, SIGNAL(newInstance(UAVObject*)), this, SLOT(newInstance(UAVObject*))); + connect(objMngr, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *))); + connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *))); // Create test objects obj1 = new ExampleObject1(); - connect(obj1, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objectUpdated(UAVObject*))); - connect(obj1, SIGNAL(objectUpdatedAuto(UAVObject*)), this, SLOT(objectUpdatedAuto(UAVObject*))); - connect(obj1, SIGNAL(objectUpdatedManual(UAVObject*)), this, SLOT(objectUpdatedManual(UAVObject*))); - connect(obj1, SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(objectUnpacked(UAVObject*))); - connect(obj1, SIGNAL(updateRequested(UAVObject*)), this, SLOT(updateRequested(UAVObject*))); + connect(obj1, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(objectUpdated(UAVObject *))); + connect(obj1, SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *))); + connect(obj1, SIGNAL(objectUpdatedManual(UAVObject *)), this, SLOT(objectUpdatedManual(UAVObject *))); + connect(obj1, SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *))); + connect(obj1, SIGNAL(updateRequested(UAVObject *)), this, SLOT(updateRequested(UAVObject *))); objMngr->registerObject(obj1); // Setup timer @@ -22,54 +22,53 @@ UAVObjectsTest::UAVObjectsTest(): sout(stdout), done(false) timer->start(1000); } -void UAVObjectsTest::objectUpdated(UAVObject* obj) +void UAVObjectsTest::objectUpdated(UAVObject *obj) { sout << QString("[Object Updated]\n%1").arg(obj->toString()); } -void UAVObjectsTest::objectUpdatedAuto(UAVObject* obj) +void UAVObjectsTest::objectUpdatedAuto(UAVObject *obj) { sout << QString("[Object Updated AUTO]\n%1").arg(obj->toString()); } -void UAVObjectsTest::objectUpdatedManual(UAVObject* obj) +void UAVObjectsTest::objectUpdatedManual(UAVObject *obj) { sout << QString("[Object Updated MANUAL]\n%1").arg(obj->toString()); } -void UAVObjectsTest::objectUnpacked(UAVObject* obj) +void UAVObjectsTest::objectUnpacked(UAVObject *obj) { sout << QString("[Object Updated UNPACKED]\n%1").arg(obj->toString()); } -void UAVObjectsTest::updateRequested(UAVObject* obj) +void UAVObjectsTest::updateRequested(UAVObject *obj) { sout << QString("[Object Update Requested]\n%1").arg(obj->toString()); } -void UAVObjectsTest::newObject(UAVObject* obj) +void UAVObjectsTest::newObject(UAVObject *obj) { - sout << QString("[New object]\n%1").arg(obj->toString()); + sout << QString("[New object]\n%1").arg(obj->toString()); } -void UAVObjectsTest::newInstance(UAVObject* obj) +void UAVObjectsTest::newInstance(UAVObject *obj) { sout << QString("[New instance]\n%1").arg(obj->toString()); } void UAVObjectsTest::runTest() { - if (!done) - { + if (!done) { // Create a new instance - ExampleObject1* obj2 = new ExampleObject1(); - objMngr->registerObject(obj2); + ExampleObject1 *obj2 = new ExampleObject1(); + objMngr->registerObject(obj2); // Set data ExampleObject1::DataFields data = obj1->getData(); - data.Field1 = 1; - data.Field2 = 2; - data.Field3 = 3; + data.Field1 = 1; + data.Field2 = 2; + data.Field3 = 3; data.Field4[0] = 4.1; data.Field4[1] = 4.2; data.Field4[2] = 4.3; @@ -86,15 +85,15 @@ void UAVObjectsTest::runTest() sout << obj2->getMetaObject()->toString(); // Create a new instance using clone() and an out of order instance ID - UAVDataObject* obj3 = obj2->clone(5); + UAVDataObject *obj3 = obj2->clone(5); objMngr->registerObject(obj3); // Pack, unpack testing - quint8* buf = new quint8[obj1->getNumBytes()]; + quint8 *buf = new quint8[obj1->getNumBytes()]; obj1->pack(buf); - data.Field1 = 10; - data.Field2 = 20; - data.Field3 = 30; + data.Field1 = 10; + data.Field2 = 20; + data.Field3 = 30; data.Field4[0] = 40.1; data.Field4[1] = 40.2; data.Field4[2] = 40.3; @@ -103,42 +102,39 @@ void UAVObjectsTest::runTest() // Save, load testing obj1->save(); - data.Field1 = 10; - data.Field2 = 20; - data.Field3 = 30; + data.Field1 = 10; + data.Field2 = 20; + data.Field3 = 30; data.Field4[0] = 40.1; data.Field4[1] = 40.2; data.Field4[2] = 40.3; obj1->setData(data); - obj1->load(); + obj1->load(); // Get all instances - QList objs = objMngr->getObjectInstances(ExampleObject1::OBJID); - for (int n = 0; n < objs.length(); ++n) - { + QList objs = objMngr->getObjectInstances(ExampleObject1::OBJID); + for (int n = 0; n < objs.length(); ++n) { sout << "[Printing object instances]\n"; sout << objs[n]->toString(); } // Get object fields QString objname("ExampleObject1"); - UAVObject* obj = objMngr->getObject(objname); - QList fields = obj->getFields(); + UAVObject *obj = objMngr->getObject(objname); + QList fields = obj->getFields(); // qint8 - UAVObjectFieldInt8* fieldint8 = dynamic_cast< UAVObjectFieldInt8* >(fields[0]); - if (fieldint8 != NULL) - { + UAVObjectFieldInt8 *fieldint8 = dynamic_cast< UAVObjectFieldInt8 * >(fields[0]); + if (fieldint8 != NULL) { fieldint8->setValue(10); qint8 value = fieldint8->getValue(); sout << value; } // enum - UAVObjectFieldEnum* fieldenum = dynamic_cast< UAVObjectFieldEnum* >(fields[7]); - if (fieldenum != NULL) - { + UAVObjectFieldEnum *fieldenum = dynamic_cast< UAVObjectFieldEnum * >(fields[7]); + if (fieldenum != NULL) { QStringList options = fieldenum->getOptions(); fieldenum->setSelected(options[1]); - QString selected = fieldenum->getSelected(); + QString selected = fieldenum->getSelected(); sout << selected; } diff --git a/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.h b/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.h index 46c390374..63ae2e7d1 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/tests/uavobjectstest.h @@ -6,8 +6,7 @@ #include #include -class UAVObjectsTest: QObject -{ +class UAVObjectsTest : QObject { Q_OBJECT @@ -15,20 +14,20 @@ public: UAVObjectsTest(); private slots: - void objectUpdated(UAVObject* obj); - void objectUpdatedAuto(UAVObject* obj); - void objectUpdatedManual(UAVObject* obj); - void objectUpdatedPeriodic(UAVObject* obj); - void objectUnpacked(UAVObject* obj); - void updateRequested(UAVObject* obj); + void objectUpdated(UAVObject *obj); + void objectUpdatedAuto(UAVObject *obj); + void objectUpdatedManual(UAVObject *obj); + void objectUpdatedPeriodic(UAVObject *obj); + void objectUnpacked(UAVObject *obj); + void updateRequested(UAVObject *obj); void runTest(); - void newObject(UAVObject* obj); - void newInstance(UAVObject* obj); + void newObject(UAVObject *obj); + void newInstance(UAVObject *obj); private: - UAVObjectManager* objMngr; - ExampleObject1* obj1; - QTimer* timer; + UAVObjectManager *objMngr; + ExampleObject1 *obj1; + QTimer *timer; QTextStream sout; bool done; }; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.cpp index 046aff6e4..8a74eb97f 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.cpp @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavdataobject.h" @@ -30,8 +30,8 @@ /** * Constructor */ -UAVDataObject::UAVDataObject(quint32 objID, bool isSingleInst, bool isSet,const QString& name): - UAVObject(objID, isSingleInst, name) +UAVDataObject::UAVDataObject(quint32 objID, bool isSingleInst, bool isSet, const QString & name) : + UAVObject(objID, isSingleInst, name) { mobj = NULL; this->isSet = isSet; @@ -40,9 +40,10 @@ UAVDataObject::UAVDataObject(quint32 objID, bool isSingleInst, bool isSet,const /** * Initialize instance ID and assign a metaobject */ -void UAVDataObject::initialize(quint32 instID, UAVMetaObject* mobj) +void UAVDataObject::initialize(quint32 instID, UAVMetaObject *mobj) { QMutexLocker locker(mutex); + this->mobj = mobj; UAVObject::initialize(instID); } @@ -50,9 +51,10 @@ void UAVDataObject::initialize(quint32 instID, UAVMetaObject* mobj) /** * Assign a metaobject */ -void UAVDataObject::initialize(UAVMetaObject* mobj) +void UAVDataObject::initialize(UAVMetaObject *mobj) { QMutexLocker locker(mutex); + this->mobj = mobj; } @@ -67,10 +69,9 @@ bool UAVDataObject::isSettings() /** * Set the object's metadata */ -void UAVDataObject::setMetadata(const Metadata& mdata) +void UAVDataObject::setMetadata(const Metadata & mdata) { - if ( mobj!=NULL ) - { + if (mobj != NULL) { mobj->setData(mdata); } } @@ -80,12 +81,9 @@ void UAVDataObject::setMetadata(const Metadata& mdata) */ UAVObject::Metadata UAVDataObject::getMetadata(void) { - if ( mobj!=NULL) - { + if (mobj != NULL) { return mobj->getData(); - } - else - { + } else { return getDefaultMetadata(); } } @@ -93,8 +91,7 @@ UAVObject::Metadata UAVDataObject::getMetadata(void) /** * Get the metaobject */ -UAVMetaObject* UAVDataObject::getMetaObject() +UAVMetaObject *UAVDataObject::getMetaObject() { return mobj; } - diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.h b/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.h index e711d2162..fc679b7e1 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavdataobject.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVDATAOBJECT_H @@ -34,25 +34,23 @@ #include "uavmetaobject.h" #include -class UAVOBJECTS_EXPORT UAVDataObject: public UAVObject -{ +class UAVOBJECTS_EXPORT UAVDataObject : public UAVObject { Q_OBJECT public: - UAVDataObject(quint32 objID, bool isSingleInst, bool isSet, const QString& name); - void initialize(quint32 instID, UAVMetaObject* mobj); - void initialize(UAVMetaObject* mobj); + UAVDataObject(quint32 objID, bool isSingleInst, bool isSet, const QString & name); + void initialize(quint32 instID, UAVMetaObject *mobj); + void initialize(UAVMetaObject *mobj); bool isSettings(); - void setMetadata(const Metadata& mdata); + void setMetadata(const Metadata & mdata); Metadata getMetadata(); - UAVMetaObject* getMetaObject(); - virtual UAVDataObject* clone(quint32 instID = 0) = 0; - virtual UAVDataObject* dirtyClone() = 0; + UAVMetaObject *getMetaObject(); + virtual UAVDataObject *clone(quint32 instID = 0) = 0; + virtual UAVDataObject *dirtyClone() = 0; private: - UAVMetaObject* mobj; + UAVMetaObject *mobj; bool isSet; - }; #endif // UAVDATAOBJECT_H diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp index d839d211f..92c6d8c3b 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavmetaobject.h" @@ -31,8 +31,8 @@ /** * Constructor */ -UAVMetaObject::UAVMetaObject(quint32 objID, const QString& name, UAVObject *parent): - UAVObject(objID, true, name) +UAVMetaObject::UAVMetaObject(quint32 objID, const QString & name, UAVObject *parent) : + UAVObject(objID, true, name) { this->parent = parent; // Setup default metadata of metaobject (can not be changed) @@ -40,14 +40,14 @@ UAVMetaObject::UAVMetaObject(quint32 objID, const QString& name, UAVObject *pare // Setup fields QStringList modesBitField; modesBitField << tr("FlightReadOnly") << tr("GCSReadOnly") << tr("FlightTelemetryAcked") << tr("GCSTelemetryAcked") << tr("FlightUpdatePeriodic") << tr("FlightUpdateOnChange") << tr("GCSUpdatePeriodic") << tr("GCSUpdateOnChange"); - QList fields; - fields.append( new UAVObjectField(tr("Modes"), tr("boolean"), UAVObjectField::BITFIELD, modesBitField, QStringList()) ); - fields.append( new UAVObjectField(tr("Flight Telemetry Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList()) ); - fields.append( new UAVObjectField(tr("GCS Telemetry Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList()) ); - fields.append( new UAVObjectField(tr("Logging Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList()) ); + QList fields; + fields.append(new UAVObjectField(tr("Modes"), tr("boolean"), UAVObjectField::BITFIELD, modesBitField, QStringList())); + fields.append(new UAVObjectField(tr("Flight Telemetry Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList())); + fields.append(new UAVObjectField(tr("GCS Telemetry Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList())); + fields.append(new UAVObjectField(tr("Logging Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList())); // Initialize parent UAVObject::initialize(0); - UAVObject::initializeFields(fields, (quint8*)&parentMetadata, sizeof(Metadata)); + UAVObject::initializeFields(fields, (quint8 *)&parentMetadata, sizeof(Metadata)); // Setup metadata of parent parentMetadata = parent->getDefaultMetadata(); } @@ -55,7 +55,7 @@ UAVMetaObject::UAVMetaObject(quint32 objID, const QString& name, UAVObject *pare /** * Get the parent object */ -UAVObject* UAVMetaObject::getParentObject() +UAVObject *UAVMetaObject::getParentObject() { return parent; } @@ -64,10 +64,10 @@ UAVObject* UAVMetaObject::getParentObject() * Set the metadata of the metaobject, this function will * do nothing since metaobjects have read-only metadata. */ -void UAVMetaObject::setMetadata(const Metadata& mdata) +void UAVMetaObject::setMetadata(const Metadata & mdata) { Q_UNUSED(mdata); - return; // can not update metaobject's metadata + // can not update metaobject's metadata } /** @@ -89,9 +89,10 @@ UAVObject::Metadata UAVMetaObject::getDefaultMetadata() /** * Set the metadata held by the metaobject */ -void UAVMetaObject::setData(const Metadata& mdata) +void UAVMetaObject::setData(const Metadata & mdata) { QMutexLocker locker(mutex); + parentMetadata = mdata; emit objectUpdatedAuto(this); // trigger object updated event emit objectUpdated(this); @@ -103,7 +104,6 @@ void UAVMetaObject::setData(const Metadata& mdata) UAVObject::Metadata UAVMetaObject::getData() { QMutexLocker locker(mutex); + return parentMetadata; } - - diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.h b/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.h index e3f270590..ef3be6a00 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVMETAOBJECT_H @@ -31,24 +31,22 @@ #include "uavobjects_global.h" #include "uavobject.h" -class UAVOBJECTS_EXPORT UAVMetaObject: public UAVObject -{ +class UAVOBJECTS_EXPORT UAVMetaObject : public UAVObject { Q_OBJECT public: - UAVMetaObject(quint32 objID, const QString& name, UAVObject* parent); - UAVObject* getParentObject(); - void setMetadata(const Metadata& mdata); + UAVMetaObject(quint32 objID, const QString & name, UAVObject *parent); + UAVObject *getParentObject(); + void setMetadata(const Metadata & mdata); Metadata getMetadata(); Metadata getDefaultMetadata(); - void setData(const Metadata& mdata); + void setData(const Metadata & mdata); Metadata getData(); private: - UAVObject* parent; + UAVObject *parent; Metadata ownMetadata; Metadata parentMetadata; - }; #endif // UAVMETAOBJECT_H diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp index 935a15d6b..d795e8cb3 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavobject.h" @@ -30,16 +30,16 @@ #include // Constants -#define UAVOBJ_ACCESS_SHIFT 0 -#define UAVOBJ_GCS_ACCESS_SHIFT 1 -#define UAVOBJ_TELEMETRY_ACKED_SHIFT 2 -#define UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT 3 -#define UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT 4 +#define UAVOBJ_ACCESS_SHIFT 0 +#define UAVOBJ_GCS_ACCESS_SHIFT 1 +#define UAVOBJ_TELEMETRY_ACKED_SHIFT 2 +#define UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT 3 +#define UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT 4 #define UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT 6 -#define UAVOBJ_UPDATE_MODE_MASK 0x3 +#define UAVOBJ_UPDATE_MODE_MASK 0x3 // Macros -#define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift); +#define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift); /** * Constructor @@ -47,13 +47,13 @@ * @param isSingleInst True if this object can only have a single instance * @param name Object name */ -UAVObject::UAVObject(quint32 objID, bool isSingleInst, const QString& name) +UAVObject::UAVObject(quint32 objID, bool isSingleInst, const QString & name) { - this->objID = objID; + this->objID = objID; this->instID = 0; this->isSingleInst = isSingleInst; - this->name = name; - this->mutex = new QMutex(QMutex::Recursive); + this->name = name; + this->mutex = new QMutex(QMutex::Recursive); } /** @@ -62,6 +62,7 @@ UAVObject::UAVObject(quint32 objID, bool isSingleInst, const QString& name) void UAVObject::initialize(quint32 instID) { QMutexLocker locker(mutex); + this->instID = instID; } @@ -71,30 +72,30 @@ void UAVObject::initialize(quint32 instID) * @param data Pointer to that actual object data, this is needed by the fields to access the data * @param numBytes Number of bytes in the object (total, including all fields) */ -void UAVObject::initializeFields(QList& fields, quint8* data, quint32 numBytes) +void UAVObject::initializeFields(QList & fields, quint8 *data, quint32 numBytes) { QMutexLocker locker(mutex); + this->numBytes = numBytes; - this->data = data; - this->fields = fields; + this->data = data; + this->fields = fields; // Initialize fields quint32 offset = 0; - for (int n = 0; n < fields.length(); ++n) - { + for (int n = 0; n < fields.length(); ++n) { fields[n]->initialize(data, offset, this); offset += fields[n]->getNumBytes(); - connect(fields[n], SIGNAL(fieldUpdated(UAVObjectField*)), this, SLOT(fieldUpdated(UAVObjectField*))); + connect(fields[n], SIGNAL(fieldUpdated(UAVObjectField *)), this, SLOT(fieldUpdated(UAVObjectField *))); } } /** * Called from the fields each time they are updated */ -void UAVObject::fieldUpdated(UAVObjectField* field) +void UAVObject::fieldUpdated(UAVObjectField *field) { Q_UNUSED(field); -// emit objectUpdatedAuto(this); // trigger object updated event -// emit objectUpdated(this); +// emit objectUpdatedAuto(this); // trigger object updated event +// emit objectUpdated(this); } /** @@ -140,7 +141,7 @@ QString UAVObject::getDescription() /** * Set the description of the object */ -void UAVObject::setDescription(const QString& description) +void UAVObject::setDescription(const QString & description) { this->description = description; } @@ -156,7 +157,7 @@ QString UAVObject::getCategory() /** * Set the category of the object */ -void UAVObject::setCategory(const QString& category) +void UAVObject::setCategory(const QString & category) { this->category = category; } @@ -214,7 +215,7 @@ void UAVObject::unlock() /** * Get object's mutex */ -QMutex* UAVObject::getMutex() +QMutex *UAVObject::getMutex() { return mutex; } @@ -225,15 +226,17 @@ QMutex* UAVObject::getMutex() qint32 UAVObject::getNumFields() { QMutexLocker locker(mutex); + return fields.count(); } /** * Get the object's fields */ -QList UAVObject::getFields() +QList UAVObject::getFields() { QMutexLocker locker(mutex); + return fields; } @@ -241,19 +244,18 @@ QList UAVObject::getFields() * Get a specific field * @returns The field or NULL if not found */ -UAVObjectField* UAVObject::getField(const QString& name) +UAVObjectField *UAVObject::getField(const QString & name) { QMutexLocker locker(mutex); + // Look for field - for (int n = 0; n < fields.length(); ++n) - { - if (name.compare(fields[n]->getName()) == 0) - { + for (int n = 0; n < fields.length(); ++n) { + if (name.compare(fields[n]->getName()) == 0) { return fields[n]; } } // If this point is reached then the field was not found - qWarning()<<"UAVObject::getField Non existant field "<pack(&dataOut[offset]); offset += fields[n]->getNumBytes(); } @@ -277,12 +279,12 @@ qint32 UAVObject::pack(quint8* dataOut) * Unpack the object data from a byte array * @returns The number of bytes copied */ -qint32 UAVObject::unpack(const quint8* dataIn) +qint32 UAVObject::unpack(const quint8 *dataIn) { QMutexLocker locker(mutex); qint32 offset = 0; - for (int n = 0; n < fields.length(); ++n) - { + + for (int n = 0; n < fields.length(); ++n) { fields[n]->unpack(&dataIn[offset]); offset += fields[n]->getNumBytes(); } @@ -305,14 +307,13 @@ bool UAVObject::save() // Open file QFile file(name + ".uavobj"); - if (!file.open(QFile::WriteOnly)) - { + + if (!file.open(QFile::WriteOnly)) { return false; } // Write object - if ( !save(file) ) - { + if (!save(file)) { return false; } @@ -327,7 +328,7 @@ bool UAVObject::save() * The data will be appended and the file will not be closed. * @returns True on success, false on failure */ -bool UAVObject::save(QFile& file) +bool UAVObject::save(QFile & file) { QMutexLocker locker(mutex); quint8 buffer[numBytes]; @@ -335,25 +336,21 @@ bool UAVObject::save(QFile& file) // Write the object ID qToLittleEndian(objID, tmpId); - if ( file.write((const char*)tmpId, 4) == -1 ) - { + if (file.write((const char *)tmpId, 4) == -1) { return false; } // Write the instance ID - if (!isSingleInst) - { + if (!isSingleInst) { qToLittleEndian(instID, tmpId); - if ( file.write((const char*)tmpId, 2) == -1 ) - { + if (file.write((const char *)tmpId, 2) == -1) { return false; } } // Write the data pack(buffer); - if ( file.write((const char*)buffer, numBytes) == -1 ) - { + if (file.write((const char *)buffer, numBytes) == -1) { return false; } @@ -374,14 +371,13 @@ bool UAVObject::load() // Open file QFile file(name + ".uavobj"); - if (!file.open(QFile::ReadOnly)) - { + + if (!file.open(QFile::ReadOnly)) { return false; } // Load object - if ( !load(file) ) - { + if (!load(file)) { return false; } @@ -396,39 +392,34 @@ bool UAVObject::load() * The data will be read and the file will not be closed. * @returns True on success, false on failure */ -bool UAVObject::load(QFile& file) +bool UAVObject::load(QFile & file) { QMutexLocker locker(mutex); quint8 buffer[numBytes]; quint8 tmpId[4]; // Read the object ID - if ( file.read((char*)tmpId, 4) != 4 ) - { + if (file.read((char *)tmpId, 4) != 4) { return false; } // Check that the IDs match - if (qFromLittleEndian(tmpId) != objID) - { + if (qFromLittleEndian(tmpId) != objID) { return false; } // Read the instance ID - if ( file.read((char*)tmpId, 2) != 2 ) - { + if (file.read((char *)tmpId, 2) != 2) { return false; } // Check that the IDs match - if (qFromLittleEndian(tmpId) != instID) - { + if (qFromLittleEndian(tmpId) != instID) { return false; } // Read and unpack the data - if ( file.read((char*)buffer, numBytes) != numBytes ) - { + if (file.read((char *)buffer, numBytes) != numBytes) { return false; } unpack(buffer); @@ -443,8 +434,9 @@ bool UAVObject::load(QFile& file) QString UAVObject::toString() { QString sout; - sout.append( toStringBrief() ); - sout.append( toStringData() ); + + sout.append(toStringBrief()); + sout.append(toStringData()); return sout; } @@ -454,12 +446,13 @@ QString UAVObject::toString() QString UAVObject::toStringBrief() { QString sout; - sout.append( QString("%1 (ID: %2, InstID: %3, NumBytes: %4, SInst: %5)\n") - .arg(getName()) - .arg(getObjID()) - .arg(getInstID()) - .arg(getNumBytes()) - .arg(isSingleInstance()) ); + + sout.append(QString("%1 (ID: %2, InstID: %3, NumBytes: %4, SInst: %5)\n") + .arg(getName()) + .arg(getObjID()) + .arg(getInstID()) + .arg(getNumBytes()) + .arg(isSingleInstance())); return sout; } @@ -469,10 +462,10 @@ QString UAVObject::toStringBrief() QString UAVObject::toStringData() { QString sout; + sout.append("Data:\n"); - for (int n = 0; n < fields.length(); ++n) - { - sout.append( QString("\t%1").arg(fields[n]->toString()) ); + for (int n = 0; n < fields.length(); ++n) { + sout.append(QString("\t%1").arg(fields[n]->toString())); } return sout; } @@ -488,7 +481,7 @@ void UAVObject::emitTransactionCompleted(bool success) /** * Emit the newInstance event */ -void UAVObject::emitNewInstance(UAVObject * obj) +void UAVObject::emitNewInstance(UAVObject *obj) { emit newInstance(obj); } @@ -497,18 +490,18 @@ void UAVObject::emitNewInstance(UAVObject * obj) * Initialize a UAVObjMetadata object. * \param[in] metadata The metadata object */ -void UAVObject::MetadataInitialize(UAVObject::Metadata& metadata) +void UAVObject::MetadataInitialize(UAVObject::Metadata & metadata) { - metadata.flags = - ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT | - ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT | - 1 << UAVOBJ_TELEMETRY_ACKED_SHIFT | - 1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT | - UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT | - UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT; - metadata.flightTelemetryUpdatePeriod = 0; - metadata.gcsTelemetryUpdatePeriod = 0; - metadata.loggingUpdatePeriod = 0; + metadata.flags = + ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT | + ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT | + 1 << UAVOBJ_TELEMETRY_ACKED_SHIFT | + 1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT | + UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT | + UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT; + metadata.flightTelemetryUpdatePeriod = 0; + metadata.gcsTelemetryUpdatePeriod = 0; + metadata.loggingUpdatePeriod = 0; } /** @@ -516,9 +509,9 @@ void UAVObject::MetadataInitialize(UAVObject::Metadata& metadata) * \param[in] metadata The metadata object * \return the access type */ -UAVObject::AccessMode UAVObject::GetFlightAccess(const UAVObject::Metadata& metadata) +UAVObject::AccessMode UAVObject::GetFlightAccess(const UAVObject::Metadata & metadata) { - return UAVObject::AccessMode((metadata.flags >> UAVOBJ_ACCESS_SHIFT) & 1); + return UAVObject::AccessMode((metadata.flags >> UAVOBJ_ACCESS_SHIFT) & 1); } /** @@ -526,9 +519,9 @@ UAVObject::AccessMode UAVObject::GetFlightAccess(const UAVObject::Metadata& meta * \param[in] metadata The metadata object * \param[in] mode The access mode */ -void UAVObject::SetFlightAccess(UAVObject::Metadata& metadata, UAVObject::AccessMode mode) +void UAVObject::SetFlightAccess(UAVObject::Metadata & metadata, UAVObject::AccessMode mode) { - SET_BITS(metadata.flags, UAVOBJ_ACCESS_SHIFT, mode, 1); + SET_BITS(metadata.flags, UAVOBJ_ACCESS_SHIFT, mode, 1); } /** @@ -536,9 +529,9 @@ void UAVObject::SetFlightAccess(UAVObject::Metadata& metadata, UAVObject::Access * \param[in] metadata The metadata object * \return the GCS access type */ -UAVObject::AccessMode UAVObject::GetGcsAccess(const UAVObject::Metadata& metadata) +UAVObject::AccessMode UAVObject::GetGcsAccess(const UAVObject::Metadata & metadata) { - return UAVObject::AccessMode((metadata.flags >> UAVOBJ_GCS_ACCESS_SHIFT) & 1); + return UAVObject::AccessMode((metadata.flags >> UAVOBJ_GCS_ACCESS_SHIFT) & 1); } /** @@ -546,8 +539,9 @@ UAVObject::AccessMode UAVObject::GetGcsAccess(const UAVObject::Metadata& metadat * \param[in] metadata The metadata object * \param[in] mode The access mode */ -void UAVObject::SetGcsAccess(UAVObject::Metadata& metadata, UAVObject::AccessMode mode) { - SET_BITS(metadata.flags, UAVOBJ_GCS_ACCESS_SHIFT, mode, 1); +void UAVObject::SetGcsAccess(UAVObject::Metadata & metadata, UAVObject::AccessMode mode) +{ + SET_BITS(metadata.flags, UAVOBJ_GCS_ACCESS_SHIFT, mode, 1); } /** @@ -555,8 +549,9 @@ void UAVObject::SetGcsAccess(UAVObject::Metadata& metadata, UAVObject::AccessMod * \param[in] metadata The metadata object * \return the telemetry acked boolean */ -quint8 UAVObject::GetFlightTelemetryAcked(const UAVObject::Metadata& metadata) { - return (metadata.flags >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 1; +quint8 UAVObject::GetFlightTelemetryAcked(const UAVObject::Metadata & metadata) +{ + return (metadata.flags >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 1; } /** @@ -564,8 +559,9 @@ quint8 UAVObject::GetFlightTelemetryAcked(const UAVObject::Metadata& metadata) { * \param[in] metadata The metadata object * \param[in] val The telemetry acked boolean */ -void UAVObject::SetFlightTelemetryAcked(UAVObject::Metadata& metadata, quint8 val) { - SET_BITS(metadata.flags, UAVOBJ_TELEMETRY_ACKED_SHIFT, val, 1); +void UAVObject::SetFlightTelemetryAcked(UAVObject::Metadata & metadata, quint8 val) +{ + SET_BITS(metadata.flags, UAVOBJ_TELEMETRY_ACKED_SHIFT, val, 1); } /** @@ -573,8 +569,9 @@ void UAVObject::SetFlightTelemetryAcked(UAVObject::Metadata& metadata, quint8 va * \param[in] metadata The metadata object * \return the telemetry acked boolean */ -quint8 UAVObject::GetGcsTelemetryAcked(const UAVObject::Metadata& metadata) { - return (metadata.flags >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 1; +quint8 UAVObject::GetGcsTelemetryAcked(const UAVObject::Metadata & metadata) +{ + return (metadata.flags >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 1; } /** @@ -582,8 +579,9 @@ quint8 UAVObject::GetGcsTelemetryAcked(const UAVObject::Metadata& metadata) { * \param[in] metadata The metadata object * \param[in] val The GCS telemetry acked boolean */ -void UAVObject::SetGcsTelemetryAcked(UAVObject::Metadata& metadata, quint8 val) { - SET_BITS(metadata.flags, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, val, 1); +void UAVObject::SetGcsTelemetryAcked(UAVObject::Metadata & metadata, quint8 val) +{ + SET_BITS(metadata.flags, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, val, 1); } /** @@ -591,8 +589,9 @@ void UAVObject::SetGcsTelemetryAcked(UAVObject::Metadata& metadata, quint8 val) * \param[in] metadata The metadata object * \return the telemetry update mode */ -UAVObject::UpdateMode UAVObject::GetFlightTelemetryUpdateMode(const UAVObject::Metadata& metadata) { - return UAVObject::UpdateMode((metadata.flags >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK); +UAVObject::UpdateMode UAVObject::GetFlightTelemetryUpdateMode(const UAVObject::Metadata & metadata) +{ + return UAVObject::UpdateMode((metadata.flags >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK); } /** @@ -600,8 +599,9 @@ UAVObject::UpdateMode UAVObject::GetFlightTelemetryUpdateMode(const UAVObject::M * \param[in] metadata The metadata object * \param[in] val The telemetry update mode */ -void UAVObject::SetFlightTelemetryUpdateMode(UAVObject::Metadata& metadata, UAVObject::UpdateMode val) { - SET_BITS(metadata.flags, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); +void UAVObject::SetFlightTelemetryUpdateMode(UAVObject::Metadata & metadata, UAVObject::UpdateMode val) +{ + SET_BITS(metadata.flags, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); } /** @@ -609,8 +609,9 @@ void UAVObject::SetFlightTelemetryUpdateMode(UAVObject::Metadata& metadata, UAVO * \param[in] metadata The metadata object * \return the GCS telemetry update mode */ -UAVObject::UpdateMode UAVObject::GetGcsTelemetryUpdateMode(const UAVObject::Metadata& metadata) { - return UAVObject::UpdateMode((metadata.flags >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK); +UAVObject::UpdateMode UAVObject::GetGcsTelemetryUpdateMode(const UAVObject::Metadata & metadata) +{ + return UAVObject::UpdateMode((metadata.flags >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK); } /** @@ -618,6 +619,7 @@ UAVObject::UpdateMode UAVObject::GetGcsTelemetryUpdateMode(const UAVObject::Meta * \param[in] metadata The metadata object * \param[in] val The GCS telemetry update mode */ -void UAVObject::SetGcsTelemetryUpdateMode(UAVObject::Metadata& metadata, UAVObject::UpdateMode val) { - SET_BITS(metadata.flags, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); +void UAVObject::SetGcsTelemetryUpdateMode(UAVObject::Metadata & metadata, UAVObject::UpdateMode val) +{ + SET_BITS(metadata.flags, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK); } diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h index d29151579..882378e7a 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVOBJECT_H @@ -39,18 +39,17 @@ #include #include "uavobjectfield.h" -#define UAVOBJ_ACCESS_SHIFT 0 -#define UAVOBJ_GCS_ACCESS_SHIFT 1 -#define UAVOBJ_TELEMETRY_ACKED_SHIFT 2 -#define UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT 3 -#define UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT 4 +#define UAVOBJ_ACCESS_SHIFT 0 +#define UAVOBJ_GCS_ACCESS_SHIFT 1 +#define UAVOBJ_TELEMETRY_ACKED_SHIFT 2 +#define UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT 3 +#define UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT 4 #define UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT 6 -#define UAVOBJ_UPDATE_MODE_MASK 0x3 +#define UAVOBJ_UPDATE_MODE_MASK 0x3 class UAVObjectField; -class UAVOBJECTS_EXPORT UAVObject: public QObject -{ +class UAVOBJECTS_EXPORT UAVObject : public QObject { Q_OBJECT public: @@ -59,18 +58,18 @@ public: * Object update mode */ typedef enum { - UPDATEMODE_MANUAL = 0, /** Manually update object, by calling the updated() function */ - UPDATEMODE_PERIODIC = 1, /** Automatically update object at periodic intervals */ - UPDATEMODE_ONCHANGE = 2, /** Only update object when its data changes */ - UPDATEMODE_THROTTLED = 3 /** Object is updated on change, but not more often than the interval time */ + UPDATEMODE_MANUAL = 0, /** Manually update object, by calling the updated() function */ + UPDATEMODE_PERIODIC = 1, /** Automatically update object at periodic intervals */ + UPDATEMODE_ONCHANGE = 2, /** Only update object when its data changes */ + UPDATEMODE_THROTTLED = 3 /** Object is updated on change, but not more often than the interval time */ } UpdateMode; /** * Access mode */ typedef enum { - ACCESS_READWRITE = 0, - ACCESS_READONLY = 1 + ACCESS_READWRITE = 0, + ACCESS_READONLY = 1 } AccessMode; /** @@ -89,15 +88,15 @@ public: * 4-5 telemetryUpdateMode Update mode used by the telemetry module (UAVObjUpdateMode) * 6-7 gcsTelemetryUpdateMode Update mode used by the GCS (UAVObjUpdateMode) */ - typedef struct { - quint8 flags; /** Defines flags for update and logging modes and whether an update should be ACK'd (bits defined above) */ + typedef struct { + quint8 flags; /** Defines flags for update and logging modes and whether an update should be ACK'd (bits defined above) */ quint16 flightTelemetryUpdatePeriod; /** Update period used by the telemetry module (only if telemetry mode is PERIODIC) */ quint16 gcsTelemetryUpdatePeriod; /** Update period used by the GCS (only if telemetry mode is PERIODIC) */ quint16 loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */ - } __attribute__((packed)) Metadata; + } __attribute__((packed)) Metadata; - UAVObject(quint32 objID, bool isSingleInst, const QString& name); + UAVObject(quint32 objID, bool isSingleInst, const QString & name); void initialize(quint32 instID); quint32 getObjID(); quint32 getInstID(); @@ -105,23 +104,23 @@ public: QString getName(); QString getCategory(); QString getDescription(); - quint32 getNumBytes(); - qint32 pack(quint8* dataOut); - qint32 unpack(const quint8* dataIn); + quint32 getNumBytes(); + qint32 pack(quint8 *dataOut); + qint32 unpack(const quint8 *dataIn); bool save(); - bool save(QFile& file); + bool save(QFile & file); bool load(); - bool load(QFile& file); - virtual void setMetadata(const Metadata& mdata) = 0; + bool load(QFile & file); + virtual void setMetadata(const Metadata & mdata) = 0; virtual Metadata getMetadata() = 0; virtual Metadata getDefaultMetadata() = 0; void lock(); void lock(int timeoutMs); void unlock(); - QMutex* getMutex(); + QMutex *getMutex(); qint32 getNumFields(); - QList getFields(); - UAVObjectField* getField(const QString& name); + QList getFields(); + UAVObjectField *getField(const QString & name); QString toString(); QString toStringBrief(); QString toStringData(); @@ -129,36 +128,36 @@ public: void emitNewInstance(UAVObject *); // Metadata accessors - static void MetadataInitialize(Metadata& meta); - static AccessMode GetFlightAccess(const Metadata& meta); - static void SetFlightAccess(Metadata& meta, AccessMode mode); - static AccessMode GetGcsAccess(const Metadata& meta); - static void SetGcsAccess(Metadata& meta, AccessMode mode); - static quint8 GetFlightTelemetryAcked(const Metadata& meta); - static void SetFlightTelemetryAcked(Metadata& meta, quint8 val); - static quint8 GetGcsTelemetryAcked(const Metadata& meta); - static void SetGcsTelemetryAcked(Metadata& meta, quint8 val); - static UpdateMode GetFlightTelemetryUpdateMode(const Metadata& meta); - static void SetFlightTelemetryUpdateMode(Metadata& meta, UpdateMode val); - static UpdateMode GetGcsTelemetryUpdateMode(const Metadata& meta); - static void SetGcsTelemetryUpdateMode(Metadata& meta, UpdateMode val); - + static void MetadataInitialize(Metadata & meta); + static AccessMode GetFlightAccess(const Metadata & meta); + static void SetFlightAccess(Metadata & meta, AccessMode mode); + static AccessMode GetGcsAccess(const Metadata & meta); + static void SetGcsAccess(Metadata & meta, AccessMode mode); + static quint8 GetFlightTelemetryAcked(const Metadata & meta); + static void SetFlightTelemetryAcked(Metadata & meta, quint8 val); + static quint8 GetGcsTelemetryAcked(const Metadata & meta); + static void SetGcsTelemetryAcked(Metadata & meta, quint8 val); + static UpdateMode GetFlightTelemetryUpdateMode(const Metadata & meta); + static void SetFlightTelemetryUpdateMode(Metadata & meta, UpdateMode val); + static UpdateMode GetGcsTelemetryUpdateMode(const Metadata & meta); + static void SetGcsTelemetryUpdateMode(Metadata & meta, UpdateMode val); + public slots: void requestUpdate(); void updated(); signals: - void objectUpdated(UAVObject* obj); - void objectUpdatedAuto(UAVObject* obj); - void objectUpdatedManual(UAVObject* obj); - void objectUpdatedPeriodic(UAVObject* obj); - void objectUnpacked(UAVObject* obj); - void updateRequested(UAVObject* obj); - void transactionCompleted(UAVObject* obj, bool success); - void newInstance(UAVObject* obj); + void objectUpdated(UAVObject *obj); + void objectUpdatedAuto(UAVObject *obj); + void objectUpdatedManual(UAVObject *obj); + void objectUpdatedPeriodic(UAVObject *obj); + void objectUnpacked(UAVObject *obj); + void updateRequested(UAVObject *obj); + void transactionCompleted(UAVObject *obj, bool success); + void newInstance(UAVObject *obj); private slots: - void fieldUpdated(UAVObjectField* field); + void fieldUpdated(UAVObjectField *field); protected: quint32 objID; @@ -168,13 +167,13 @@ protected: QString description; QString category; quint32 numBytes; - QMutex* mutex; - quint8* data; - QList fields; + QMutex *mutex; + quint8 *data; + QList fields; - void initializeFields(QList& fields, quint8* data, quint32 numBytes); - void setDescription(const QString& description); - void setCategory(const QString& category); + void initializeFields(QList & fields, quint8 *data, quint32 numBytes); + void setDescription(const QString & description); + void setCategory(const QString & category); }; #endif // UAVOBJECT_H diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp index b3af75613..4e0c5c613 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp @@ -29,39 +29,37 @@ #include #include -UAVObjectField::UAVObjectField(const QString& name, const QString& units, FieldType type, quint32 numElements, const QStringList& options, const QString &limits) +UAVObjectField::UAVObjectField(const QString & name, const QString & units, FieldType type, quint32 numElements, const QStringList & options, const QString &limits) { QStringList elementNames; + // Set element names - for (quint32 n = 0; n < numElements; ++n) - { + for (quint32 n = 0; n < numElements; ++n) { elementNames.append(QString("%1").arg(n)); } // Initialize - constructorInitialize(name, units, type, elementNames, options,limits); - + constructorInitialize(name, units, type, elementNames, options, limits); } -UAVObjectField::UAVObjectField(const QString& name, const QString& units, FieldType type, const QStringList& elementNames, const QStringList& options, const QString &limits) +UAVObjectField::UAVObjectField(const QString & name, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString &limits) { - constructorInitialize(name, units, type, elementNames, options,limits); + constructorInitialize(name, units, type, elementNames, options, limits); } -void UAVObjectField::constructorInitialize(const QString& name, const QString& units, FieldType type, const QStringList& elementNames, const QStringList& options,const QString &limits) +void UAVObjectField::constructorInitialize(const QString & name, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString &limits) { // Copy params - this->name = name; - this->units = units; - this->type = type; - this->options = options; - this->numElements = elementNames.length(); - this->offset = 0; - this->data = NULL; + this->name = name; + this->units = units; + this->type = type; + this->options = options; + this->numElements = elementNames.length(); + this->offset = 0; + this->data = NULL; this->obj = NULL; this->elementNames = elementNames; // Set field size - switch (type) - { + switch (type) { case INT8: numBytesPerElement = sizeof(qint8); break; @@ -88,7 +86,7 @@ void UAVObjectField::constructorInitialize(const QString& name, const QString& u break; case BITFIELD: numBytesPerElement = sizeof(quint8); - this->options = QStringList()<options = QStringList() << tr("0") << tr("1"); break; case STRING: numBytesPerElement = sizeof(quint8); @@ -102,64 +100,67 @@ void UAVObjectField::constructorInitialize(const QString& name, const QString& u void UAVObjectField::limitsInitialize(const QString &limits) { // Limit string format: - // % - start char - // XXXX - optional BOARD_TYPE and BOARD_REVISION (4 hex digits) - // TY - rule type (EQ-equal, NE-not equal, BE-between, BI-bigger, SM-smaller) - // VAL - values for TY separated by colon - // , - rule separator (may have leading or trailing spaces) - // ; - element separator (may have leading or trailing spaces) + // % - start char + // XXXX - optional BOARD_TYPE and BOARD_REVISION (4 hex digits) + // TY - rule type (EQ-equal, NE-not equal, BE-between, BI-bigger, SM-smaller) + // VAL - values for TY separated by colon + // , - rule separator (may have leading or trailing spaces) + // ; - element separator (may have leading or trailing spaces) // // Examples: - // Disable few flight modes for Revo (00903): - // "%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner" - // Original CC board (rev 1), first element bigger than 3 and second element inside [2.3-5.0]: - // "%0401BI:3; %BE:2.3:5" - // Set applicable range [0-500] for 3 elements of array for all boards: - // "%BE:0:500; %BE:0:500; %BE:0:500" - if(limits.isEmpty()) + // Disable few flight modes for Revo (00903): + // "%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner" + // Original CC board (rev 1), first element bigger than 3 and second element inside [2.3-5.0]: + // "%0401BI:3; %BE:2.3:5" + // Set applicable range [0-500] for 3 elements of array for all boards: + // "%BE:0:500; %BE:0:500; %BE:0:500" + if (limits.isEmpty()) { return; + } QStringList stringPerElement = limits.split(";"); - quint32 index=0; - foreach (QString str, stringPerElement) { + quint32 index = 0; + foreach(QString str, stringPerElement) { QStringList ruleList = str.split(","); + QList limitList; - foreach(QString rule,ruleList) - { - QString _str=rule.trimmed(); - if(_str.isEmpty()) + foreach(QString rule, ruleList) { + QString _str = rule.trimmed(); + + if (_str.isEmpty()) { continue; - QStringList valuesPerElement=_str.split(":"); + } + QStringList valuesPerElement = _str.split(":"); LimitStruct lstruc; - bool startFlag=valuesPerElement.at(0).startsWith("%"); - bool maxIndexFlag=(int)(index)<(int)numElements; - bool elemNumberSizeFlag=valuesPerElement.at(0).size()==3; + bool startFlag = valuesPerElement.at(0).startsWith("%"); + bool maxIndexFlag = (int)(index) < (int)numElements; + bool elemNumberSizeFlag = valuesPerElement.at(0).size() == 3; bool aux; - valuesPerElement.at(0).mid(1,4).toInt(&aux,16); - bool b4=((valuesPerElement.at(0).size())==7 && aux); - if(startFlag && maxIndexFlag && (elemNumberSizeFlag || b4)) - { - if(b4) - lstruc.board=valuesPerElement.at(0).mid(1,4).toInt(&aux,16); - else - lstruc.board=0; - if(valuesPerElement.at(0).right(2)=="EQ") - lstruc.type=EQUAL; - else if(valuesPerElement.at(0).right(2)=="NE") - lstruc.type=NOT_EQUAL; - else if(valuesPerElement.at(0).right(2)=="BE") - lstruc.type=BETWEEN; - else if(valuesPerElement.at(0).right(2)=="BI") - lstruc.type=BIGGER; - else if(valuesPerElement.at(0).right(2)=="SM") - lstruc.type=SMALLER; - else - qDebug()<<"limits parsing failed (invalid property) on UAVObjectField"<numelements) on UAVObjectField"<numelements) on UAVObjectField" << name << "index" << index << "numElements" << numElements; + } else if (!elemNumberSizeFlag || !b4) { + qDebug() << "limits parsing failed limit not starting with %XX or %YYYYXX where XX is the limit type and YYYY is the board type on UAVObjectField" << name; + } } } - elementLimits.insert(index,limitList); + elementLimits.insert(index, limitList); ++index; - } - foreach(QList limitList,elementLimits) - { - foreach(LimitStruct limit,limitList) - { - qDebug()<<"Limit type"< limitList, elementLimits) { + foreach(LimitStruct limit, limitList) { + qDebug() << "Limit type" << limit.type << "for board" << limit.board << "for field" << getName(); + foreach(QVariant var, limit.values) { + qDebug() << "value" << var; } } } } -bool UAVObjectField::isWithinLimits(QVariant var,quint32 index, int board) +bool UAVObjectField::isWithinLimits(QVariant var, quint32 index, int board) { - if(!elementLimits.keys().contains(index)) + if (!elementLimits.keys().contains(index)) { return true; + } - foreach(LimitStruct struc,elementLimits.value(index)) - { - if((struc.board!=board) && board!=0 && struc.board!=0) + foreach(LimitStruct struc, elementLimits.value(index)) { + if ((struc.board != board) && board != 0 && struc.board != 0) { continue; - switch(struc.type) - { + } + switch (struc.type) { case EQUAL: - switch (type) - { + switch (type) { case INT8: case INT16: case INT32: - foreach (QVariant vars, struc.values) { - if(var.toInt()==vars.toInt()) + foreach(QVariant vars, struc.values) { + if (var.toInt() == vars.toInt()) { return true; + } } return false; + break; case UINT8: case UINT16: case UINT32: case BITFIELD: - foreach (QVariant vars, struc.values) { - if(var.toUInt()==vars.toUInt()) + foreach(QVariant vars, struc.values) { + if (var.toUInt() == vars.toUInt()) { return true; + } } return false; + break; case ENUM: case STRING: - foreach (QVariant vars, struc.values) { - if(var.toString()==vars.toString()) + foreach(QVariant vars, struc.values) { + if (var.toString() == vars.toString()) { return true; + } } return false; + break; case FLOAT32: - foreach (QVariant vars, struc.values) { - if(var.toFloat()==vars.toFloat()) + foreach(QVariant vars, struc.values) { + if (var.toFloat() == vars.toFloat()) { return true; + } } return false; + break; default: return true; } break; case NOT_EQUAL: - switch (type) - { + switch (type) { case INT8: case INT16: case INT32: - foreach (QVariant vars, struc.values) { - if(var.toInt()==vars.toInt()) + foreach(QVariant vars, struc.values) { + if (var.toInt() == vars.toInt()) { return false; + } } return true; + break; case UINT8: case UINT16: case UINT32: case BITFIELD: - foreach (QVariant vars, struc.values) { - if(var.toUInt()==vars.toUInt()) + foreach(QVariant vars, struc.values) { + if (var.toUInt() == vars.toUInt()) { return false; + } } return true; + break; case ENUM: case STRING: - foreach (QVariant vars, struc.values) { - if(var.toString()==vars.toString()) + foreach(QVariant vars, struc.values) { + if (var.toString() == vars.toString()) { return false; + } } return true; + break; case FLOAT32: - foreach (QVariant vars, struc.values) { - if(var.toFloat()==vars.toFloat()) + foreach(QVariant vars, struc.values) { + if (var.toFloat() == vars.toFloat()) { return false; + } } return true; + break; default: return true; } break; case BETWEEN: - if(struc.values.length()<2) - { - qDebug()<<__FUNCTION__<<"between limit with less than 1 pair, aborting; field:"<2) - qDebug()<<__FUNCTION__<<"between limit with more than 1 pair, using first; field"< 2) { + qDebug() << __FUNCTION__ << "between limit with more than 1 pair, using first; field" << name; + } + switch (type) { case INT8: case INT16: case INT32: - if(!(var.toInt()>=struc.values.at(0).toInt() && var.toInt()<=struc.values.at(1).toInt())) - return false; + if (!(var.toInt() >= struc.values.at(0).toInt() && var.toInt() <= struc.values.at(1).toInt())) { + return false; + } return true; + break; case UINT8: case UINT16: case UINT32: case BITFIELD: - if(!(var.toUInt()>=struc.values.at(0).toUInt() && var.toUInt()<=struc.values.at(1).toUInt())) - return false; + if (!(var.toUInt() >= struc.values.at(0).toUInt() && var.toUInt() <= struc.values.at(1).toUInt())) { + return false; + } return true; + break; case ENUM: - if(!(options.indexOf(var.toString())>=options.indexOf(struc.values.at(0).toString()) && options.indexOf(var.toString())<=options.indexOf(struc.values.at(1).toString()))) - return false; + if (!(options.indexOf(var.toString()) >= options.indexOf(struc.values.at(0).toString()) && options.indexOf(var.toString()) <= options.indexOf(struc.values.at(1).toString()))) { + return false; + } return true; + break; case STRING: return true; + break; case FLOAT32: - if(!(var.toFloat()>=struc.values.at(0).toFloat() && var.toFloat()<=struc.values.at(1).toFloat())) - return false; + if (!(var.toFloat() >= struc.values.at(0).toFloat() && var.toFloat() <= struc.values.at(1).toFloat())) { + return false; + } return true; + break; default: return true; } break; case BIGGER: - if(struc.values.length()<1) - { - qDebug()<<__FUNCTION__<<"BIGGER limit with less than 1 value, aborting; field:"<1) - qDebug()<<__FUNCTION__<<"BIGGER limit with more than 1 value, using first; field"< 1) { + qDebug() << __FUNCTION__ << "BIGGER limit with more than 1 value, using first; field" << name; + } + switch (type) { case INT8: case INT16: case INT32: - if(!(var.toInt()>=struc.values.at(0).toInt())) - return false; + if (!(var.toInt() >= struc.values.at(0).toInt())) { + return false; + } return true; + break; case UINT8: case UINT16: case UINT32: case BITFIELD: - if(!(var.toUInt()>=struc.values.at(0).toUInt())) - return false; + if (!(var.toUInt() >= struc.values.at(0).toUInt())) { + return false; + } return true; + break; case ENUM: - if(!(options.indexOf(var.toString())>=options.indexOf(struc.values.at(0).toString()))) - return false; + if (!(options.indexOf(var.toString()) >= options.indexOf(struc.values.at(0).toString()))) { + return false; + } return true; + break; case STRING: return true; + break; case FLOAT32: - if(!(var.toFloat()>=struc.values.at(0).toFloat())) - return false; + if (!(var.toFloat() >= struc.values.at(0).toFloat())) { + return false; + } return true; + break; default: return true; } break; case SMALLER: - switch (type) - { + switch (type) { case INT8: case INT16: case INT32: - if(!(var.toInt()<=struc.values.at(0).toInt())) - return false; + if (!(var.toInt() <= struc.values.at(0).toInt())) { + return false; + } return true; + break; case UINT8: case UINT16: case UINT32: case BITFIELD: - if(!(var.toUInt()<=struc.values.at(0).toUInt())) - return false; + if (!(var.toUInt() <= struc.values.at(0).toUInt())) { + return false; + } return true; + break; case ENUM: - if(!(options.indexOf(var.toString())<=options.indexOf(struc.values.at(0).toString()))) - return false; + if (!(options.indexOf(var.toString()) <= options.indexOf(struc.values.at(0).toString()))) { + return false; + } return true; + break; case STRING: return true; + break; case FLOAT32: - if(!(var.toFloat()<=struc.values.at(0).toFloat())) - return false; + if (!(var.toFloat() <= struc.values.at(0).toFloat())) { + return false; + } return true; + break; default: return true; @@ -428,30 +462,34 @@ bool UAVObjectField::isWithinLimits(QVariant var,quint32 index, int board) return true; } -QVariant UAVObjectField::getMaxLimit(quint32 index,int board) +QVariant UAVObjectField::getMaxLimit(quint32 index, int board) { - if(!elementLimits.keys().contains(index)) + if (!elementLimits.keys().contains(index)) { return QVariant(); - foreach(LimitStruct struc,elementLimits.value(index)) - { - if((struc.board!=board) && board!=0 && struc.board!=0) + } + foreach(LimitStruct struc, elementLimits.value(index)) { + if ((struc.board != board) && board != 0 && struc.board != 0) { continue; - switch(struc.type) - { + } + switch (struc.type) { case EQUAL: case NOT_EQUAL: case BIGGER: return QVariant(); + break; break; case BETWEEN: return struc.values.at(1); + break; case SMALLER: return struc.values.at(0); + break; default: return QVariant(); + break; } } @@ -459,38 +497,42 @@ QVariant UAVObjectField::getMaxLimit(quint32 index,int board) } QVariant UAVObjectField::getMinLimit(quint32 index, int board) { - if(!elementLimits.keys().contains(index)) + if (!elementLimits.keys().contains(index)) { return QVariant(); - foreach(LimitStruct struc,elementLimits.value(index)) - { - if((struc.board!=board) && board!=0 && struc.board!=0) + } + foreach(LimitStruct struc, elementLimits.value(index)) { + if ((struc.board != board) && board != 0 && struc.board != 0) { return QVariant(); - switch(struc.type) - { + } + switch (struc.type) { case EQUAL: case NOT_EQUAL: case SMALLER: return QVariant(); + break; break; case BETWEEN: return struc.values.at(0); + break; case BIGGER: return struc.values.at(0); + break; default: return QVariant(); + break; } } return QVariant(); } -void UAVObjectField::initialize(quint8* data, quint32 dataOffset, UAVObject* obj) +void UAVObjectField::initialize(quint8 *data, quint32 dataOffset, UAVObject *obj) { - this->data = data; + this->data = data; this->offset = dataOffset; - this->obj = obj; + this->obj = obj; clear(); } @@ -501,28 +543,37 @@ UAVObjectField::FieldType UAVObjectField::getType() QString UAVObjectField::getTypeAsString() { - switch (type) - { + switch (type) { case UAVObjectField::INT8: return "int8"; + case UAVObjectField::INT16: return "int16"; + case UAVObjectField::INT32: return "int32"; + case UAVObjectField::UINT8: return "uint8"; + case UAVObjectField::UINT16: return "uint16"; + case UAVObjectField::UINT32: return "uint32"; + case UAVObjectField::FLOAT32: return "float32"; + case UAVObjectField::ENUM: return "enum"; + case UAVObjectField::BITFIELD: return "bitfield"; + case UAVObjectField::STRING: return "string"; + default: return ""; } @@ -533,7 +584,7 @@ QStringList UAVObjectField::getElementNames() return elementNames; } -UAVObject* UAVObjectField::getObject() +UAVObject *UAVObjectField::getObject() { return obj; } @@ -541,13 +592,13 @@ UAVObject* UAVObjectField::getObject() void UAVObjectField::clear() { QMutexLocker locker(obj->getMutex()); - switch (type) - { + + switch (type) { case BITFIELD: - memset(&data[offset], 0, numBytesPerElement*((quint32)(1+(numElements-1)/8))); + memset(&data[offset], 0, numBytesPerElement * ((quint32)(1 + (numElements - 1) / 8))); break; default: - memset(&data[offset], 0, numBytesPerElement*numElements); + memset(&data[offset], 0, numBytesPerElement * numElements); break; } } @@ -579,13 +630,14 @@ quint32 UAVObjectField::getDataOffset() quint32 UAVObjectField::getNumBytes() { - switch (type) - { + switch (type) { case BITFIELD: - return numBytesPerElement * ((quint32) (1+(numElements-1)/8)); + return numBytesPerElement * ((quint32)(1 + (numElements - 1) / 8)); + break; default: return numBytesPerElement * numElements; + break; } } @@ -593,81 +645,73 @@ quint32 UAVObjectField::getNumBytes() QString UAVObjectField::toString() { QString sout; - sout.append ( QString("%1: [ ").arg(name) ); - for (unsigned int n = 0; n < numElements; ++n) - { - sout.append( QString("%1 ").arg(getDouble(n)) ); + + sout.append(QString("%1: [ ").arg(name)); + for (unsigned int n = 0; n < numElements; ++n) { + sout.append(QString("%1 ").arg(getDouble(n))); } - sout.append( QString("] %1\n").arg(units) ); + sout.append(QString("] %1\n").arg(units)); return sout; } -qint32 UAVObjectField::pack(quint8* dataOut) +qint32 UAVObjectField::pack(quint8 *dataOut) { QMutexLocker locker(obj->getMutex()); + // Pack each element in output buffer - switch (type) - { + switch (type) { case INT8: memcpy(dataOut, &data[offset], numElements); break; case INT16: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { qint16 value; - memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement); - qToLittleEndian(value, &dataOut[numBytesPerElement*index]); + memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement); + qToLittleEndian(value, &dataOut[numBytesPerElement * index]); } break; case INT32: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { qint32 value; - memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement); - qToLittleEndian(value, &dataOut[numBytesPerElement*index]); + memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement); + qToLittleEndian(value, &dataOut[numBytesPerElement * index]); } break; case UINT8: - for (quint32 index = 0; index < numElements; ++index) - { - dataOut[numBytesPerElement*index] = data[offset + numBytesPerElement*index]; + for (quint32 index = 0; index < numElements; ++index) { + dataOut[numBytesPerElement * index] = data[offset + numBytesPerElement * index]; } break; case UINT16: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { quint16 value; - memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement); - qToLittleEndian(value, &dataOut[numBytesPerElement*index]); + memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement); + qToLittleEndian(value, &dataOut[numBytesPerElement * index]); } break; case UINT32: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { quint32 value; - memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement); - qToLittleEndian(value, &dataOut[numBytesPerElement*index]); + memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement); + qToLittleEndian(value, &dataOut[numBytesPerElement * index]); } break; case FLOAT32: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { quint32 value; - memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement); - qToLittleEndian(value, &dataOut[numBytesPerElement*index]); + memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement); + qToLittleEndian(value, &dataOut[numBytesPerElement * index]); } break; case ENUM: - for (quint32 index = 0; index < numElements; ++index) - { - dataOut[numBytesPerElement*index] = data[offset + numBytesPerElement*index]; + for (quint32 index = 0; index < numElements; ++index) { + dataOut[numBytesPerElement * index] = data[offset + numBytesPerElement * index]; } break; case BITFIELD: - for (quint32 index = 0; index < (quint32)(1+(numElements-1)/8); ++index) - { - dataOut[numBytesPerElement*index] = data[offset + numBytesPerElement*index]; + for (quint32 index = 0; index < (quint32)(1 + (numElements - 1) / 8); ++index) { + dataOut[numBytesPerElement * index] = data[offset + numBytesPerElement * index]; } break; case STRING: @@ -678,71 +722,63 @@ qint32 UAVObjectField::pack(quint8* dataOut) return getNumBytes(); } -qint32 UAVObjectField::unpack(const quint8* dataIn) +qint32 UAVObjectField::unpack(const quint8 *dataIn) { QMutexLocker locker(obj->getMutex()); + // Unpack each element from input buffer - switch (type) - { + switch (type) { case INT8: memcpy(&data[offset], dataIn, numElements); break; case INT16: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { qint16 value; - value = qFromLittleEndian(&dataIn[numBytesPerElement*index]); - memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement); + value = qFromLittleEndian(&dataIn[numBytesPerElement * index]); + memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement); } break; case INT32: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { qint32 value; - value = qFromLittleEndian(&dataIn[numBytesPerElement*index]); - memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement); + value = qFromLittleEndian(&dataIn[numBytesPerElement * index]); + memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement); } break; case UINT8: - for (quint32 index = 0; index < numElements; ++index) - { - data[offset + numBytesPerElement*index] = dataIn[numBytesPerElement*index]; + for (quint32 index = 0; index < numElements; ++index) { + data[offset + numBytesPerElement * index] = dataIn[numBytesPerElement * index]; } break; case UINT16: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { quint16 value; - value = qFromLittleEndian(&dataIn[numBytesPerElement*index]); - memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement); + value = qFromLittleEndian(&dataIn[numBytesPerElement * index]); + memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement); } break; case UINT32: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { quint32 value; - value = qFromLittleEndian(&dataIn[numBytesPerElement*index]); - memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement); + value = qFromLittleEndian(&dataIn[numBytesPerElement * index]); + memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement); } break; case FLOAT32: - for (quint32 index = 0; index < numElements; ++index) - { + for (quint32 index = 0; index < numElements; ++index) { quint32 value; - value = qFromLittleEndian(&dataIn[numBytesPerElement*index]); - memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement); + value = qFromLittleEndian(&dataIn[numBytesPerElement * index]); + memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement); } break; case ENUM: - for (quint32 index = 0; index < numElements; ++index) - { - data[offset + numBytesPerElement*index] = dataIn[numBytesPerElement*index]; + for (quint32 index = 0; index < numElements; ++index) { + data[offset + numBytesPerElement * index] = dataIn[numBytesPerElement * index]; } break; case BITFIELD: - for (quint32 index = 0; index < (quint32)(1+(numElements-1)/8); ++index) - { - data[offset + numBytesPerElement*index] = dataIn[numBytesPerElement*index]; + for (quint32 index = 0; index < (quint32)(1 + (numElements - 1) / 8); ++index) { + data[offset + numBytesPerElement * index] = dataIn[numBytesPerElement * index]; } break; case STRING: @@ -755,37 +791,46 @@ qint32 UAVObjectField::unpack(const quint8* dataIn) bool UAVObjectField::isNumeric() { - switch (type) - { + switch (type) { case INT8: return true; + break; case INT16: return true; + break; case INT32: return true; + break; case UINT8: return true; + break; case UINT16: return true; + break; case UINT32: return true; + break; case FLOAT32: return true; + break; case ENUM: return false; + break; case BITFIELD: return true; + break; case STRING: return false; + break; default: return false; @@ -794,37 +839,46 @@ bool UAVObjectField::isNumeric() bool UAVObjectField::isText() { - switch (type) - { + switch (type) { case INT8: return false; + break; case INT16: return false; + break; case INT32: return false; + break; case UINT8: return false; + break; case UINT16: return false; + break; case UINT32: return false; + break; case FLOAT32: return false; + break; case ENUM: return true; + break; case BITFIELD: return false; + break; case STRING: return true; + break; default: return false; @@ -834,87 +888,96 @@ bool UAVObjectField::isText() QVariant UAVObjectField::getValue(quint32 index) { QMutexLocker locker(obj->getMutex()); + // Check that index is not out of bounds - if ( index >= numElements ) - { + if (index >= numElements) { return QVariant(); } // Get value - switch (type) - { + switch (type) { case INT8: { qint8 tmpint8; - memcpy(&tmpint8, &data[offset + numBytesPerElement*index], numBytesPerElement); + memcpy(&tmpint8, &data[offset + numBytesPerElement * index], numBytesPerElement); return QVariant(tmpint8); + break; } case INT16: { qint16 tmpint16; - memcpy(&tmpint16, &data[offset + numBytesPerElement*index], numBytesPerElement); + memcpy(&tmpint16, &data[offset + numBytesPerElement * index], numBytesPerElement); return QVariant(tmpint16); + break; } case INT32: { qint32 tmpint32; - memcpy(&tmpint32, &data[offset + numBytesPerElement*index], numBytesPerElement); + memcpy(&tmpint32, &data[offset + numBytesPerElement * index], numBytesPerElement); return QVariant(tmpint32); + break; } case UINT8: { quint8 tmpuint8; - memcpy(&tmpuint8, &data[offset + numBytesPerElement*index], numBytesPerElement); + memcpy(&tmpuint8, &data[offset + numBytesPerElement * index], numBytesPerElement); return QVariant(tmpuint8); + break; } case UINT16: { quint16 tmpuint16; - memcpy(&tmpuint16, &data[offset + numBytesPerElement*index], numBytesPerElement); + memcpy(&tmpuint16, &data[offset + numBytesPerElement * index], numBytesPerElement); return QVariant(tmpuint16); + break; } case UINT32: { quint32 tmpuint32; - memcpy(&tmpuint32, &data[offset + numBytesPerElement*index], numBytesPerElement); + memcpy(&tmpuint32, &data[offset + numBytesPerElement * index], numBytesPerElement); return QVariant(tmpuint32); + break; } case FLOAT32: { float tmpfloat; - memcpy(&tmpfloat, &data[offset + numBytesPerElement*index], numBytesPerElement); + memcpy(&tmpfloat, &data[offset + numBytesPerElement * index], numBytesPerElement); return QVariant(tmpfloat); + break; } case ENUM: { quint8 tmpenum; - memcpy(&tmpenum, &data[offset + numBytesPerElement*index], numBytesPerElement); - if(tmpenum >= options.length()) { + memcpy(&tmpenum, &data[offset + numBytesPerElement * index], numBytesPerElement); + if (tmpenum >= options.length()) { qDebug() << "Invalid value for" << name; tmpenum = 0; } - return QVariant( options[tmpenum] ); + return QVariant(options[tmpenum]); + break; } case BITFIELD: { quint8 tmpbitfield; - memcpy(&tmpbitfield, &data[offset + numBytesPerElement*((quint32)(index/8))], numBytesPerElement); + memcpy(&tmpbitfield, &data[offset + numBytesPerElement * ((quint32)(index / 8))], numBytesPerElement); tmpbitfield = (tmpbitfield >> (index % 8)) & 1; - return QVariant( tmpbitfield ); + return QVariant(tmpbitfield); + break; } case STRING: { data[offset + numElements - 1] = '\0'; - QString str((char*)&data[offset]); - return QVariant( str ); + QString str((char *)&data[offset]); + return QVariant(str); + break; } } @@ -922,21 +985,19 @@ QVariant UAVObjectField::getValue(quint32 index) return QVariant(); } -bool UAVObjectField::checkValue(const QVariant& value, quint32 index) +bool UAVObjectField::checkValue(const QVariant & value, quint32 index) { QMutexLocker locker(obj->getMutex()); + // Check that index is not out of bounds - if ( index >= numElements ) - { + if (index >= numElements) { return false; } // Get metadata UAVObject::Metadata mdata = obj->getMetadata(); // Update value if the access mode permits - if ( UAVObject::GetFlightAccess(mdata) == UAVObject::ACCESS_READWRITE ) - { - switch (type) - { + if (UAVObject::GetFlightAccess(mdata) == UAVObject::ACCESS_READWRITE) { + switch (type) { case INT8: case INT16: case INT32: @@ -947,11 +1008,13 @@ bool UAVObjectField::checkValue(const QVariant& value, quint32 index) case STRING: case BITFIELD: return true; + break; case ENUM: { - qint8 tmpenum = options.indexOf( value.toString() ); - return ((tmpenum < 0) ? false : true); + qint8 tmpenum = options.indexOf(value.toString()); + return (tmpenum < 0) ? false : true; + break; } default: @@ -963,79 +1026,77 @@ bool UAVObjectField::checkValue(const QVariant& value, quint32 index) return true; } -void UAVObjectField::setValue(const QVariant& value, quint32 index) +void UAVObjectField::setValue(const QVariant & value, quint32 index) { QMutexLocker locker(obj->getMutex()); + // Check that index is not out of bounds - if ( index >= numElements ) - { + if (index >= numElements) { return; } // Get metadata UAVObject::Metadata mdata = obj->getMetadata(); // Update value if the access mode permits - if ( UAVObject::GetGcsAccess(mdata) == UAVObject::ACCESS_READWRITE ) - { - switch (type) - { + if (UAVObject::GetGcsAccess(mdata) == UAVObject::ACCESS_READWRITE) { + switch (type) { case INT8: { qint8 tmpint8 = value.toInt(); - memcpy(&data[offset + numBytesPerElement*index], &tmpint8, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpint8, numBytesPerElement); break; } case INT16: { qint16 tmpint16 = value.toInt(); - memcpy(&data[offset + numBytesPerElement*index], &tmpint16, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpint16, numBytesPerElement); break; } case INT32: { qint32 tmpint32 = value.toInt(); - memcpy(&data[offset + numBytesPerElement*index], &tmpint32, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpint32, numBytesPerElement); break; } case UINT8: { quint8 tmpuint8 = value.toUInt(); - memcpy(&data[offset + numBytesPerElement*index], &tmpuint8, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpuint8, numBytesPerElement); break; } case UINT16: { quint16 tmpuint16 = value.toUInt(); - memcpy(&data[offset + numBytesPerElement*index], &tmpuint16, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpuint16, numBytesPerElement); break; } case UINT32: { quint32 tmpuint32 = value.toUInt(); - memcpy(&data[offset + numBytesPerElement*index], &tmpuint32, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpuint32, numBytesPerElement); break; } case FLOAT32: { float tmpfloat = value.toFloat(); - memcpy(&data[offset + numBytesPerElement*index], &tmpfloat, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpfloat, numBytesPerElement); break; } case ENUM: { - qint8 tmpenum = options.indexOf( value.toString() ); + qint8 tmpenum = options.indexOf(value.toString()); // Default to 0 on invalid values. - if(tmpenum < 0) { + if (tmpenum < 0) { tmpenum = 0; } - memcpy(&data[offset + numBytesPerElement*index], &tmpenum, numBytesPerElement); + memcpy(&data[offset + numBytesPerElement * index], &tmpenum, numBytesPerElement); break; } case BITFIELD: { quint8 tmpbitfield; - memcpy(&tmpbitfield, &data[offset + numBytesPerElement*((quint32)(index/8))], numBytesPerElement); - tmpbitfield = (tmpbitfield & ~(1 << (index % 8))) | ( (value.toUInt()!=0?1:0) << (index % 8) ); - memcpy(&data[offset + numBytesPerElement*((quint32)(index/8))], &tmpbitfield, numBytesPerElement); + memcpy(&tmpbitfield, &data[offset + numBytesPerElement * ((quint32)(index / 8))], numBytesPerElement); + tmpbitfield = (tmpbitfield & ~(1 << (index % 8))) | ((value.toUInt() != 0 ? 1 : 0) << (index % 8)); + memcpy(&data[offset + numBytesPerElement * ((quint32)(index / 8))], &tmpbitfield, numBytesPerElement); break; } case STRING: @@ -1043,9 +1104,8 @@ void UAVObjectField::setValue(const QVariant& value, quint32 index) QString str = value.toString(); QByteArray barray = str.toAscii(); quint32 index; - for (index = 0; index < (quint32)barray.length() && index < (numElements-1); ++index) - { - data[offset+index] = barray[index]; + for (index = 0; index < (quint32)barray.length() && index < (numElements - 1); ++index) { + data[offset + index] = barray[index]; } barray[index] = '\0'; break; @@ -1063,4 +1123,3 @@ void UAVObjectField::setDouble(double value, quint32 index) { setValue(QVariant(value), index); } - diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.h index 653fcaa19..c782a86f3 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVOBJECTFIELD_H @@ -37,24 +37,22 @@ class UAVObject; -class UAVOBJECTS_EXPORT UAVObjectField: public QObject -{ +class UAVOBJECTS_EXPORT UAVObjectField : public QObject { Q_OBJECT public: typedef enum { INT8 = 0, INT16, INT32, UINT8, UINT16, UINT32, FLOAT32, ENUM, BITFIELD, STRING } FieldType; - typedef enum { EQUAL,NOT_EQUAL,BETWEEN,BIGGER,SMALLER } LimitType; - typedef struct - { + typedef enum { EQUAL, NOT_EQUAL, BETWEEN, BIGGER, SMALLER } LimitType; + typedef struct { LimitType type; QList values; int board; } LimitStruct; - UAVObjectField(const QString& name, const QString& units, FieldType type, quint32 numElements, const QStringList& options,const QString& limits=QString()); - UAVObjectField(const QString& name, const QString& units, FieldType type, const QStringList& elementNames, const QStringList& options,const QString& limits=QString()); - void initialize(quint8* data, quint32 dataOffset, UAVObject* obj); - UAVObject* getObject(); + UAVObjectField(const QString & name, const QString & units, FieldType type, quint32 numElements, const QStringList & options, const QString & limits = QString()); + UAVObjectField(const QString & name, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString & limits = QString()); + void initialize(quint8 *data, quint32 dataOffset, UAVObject *obj); + UAVObject *getObject(); FieldType getType(); QString getTypeAsString(); QString getName(); @@ -62,11 +60,11 @@ public: quint32 getNumElements(); QStringList getElementNames(); QStringList getOptions(); - qint32 pack(quint8* dataOut); - qint32 unpack(const quint8* dataIn); + qint32 pack(quint8 *dataOut); + qint32 unpack(const quint8 *dataIn); QVariant getValue(quint32 index = 0); - bool checkValue(const QVariant& data, quint32 index = 0); - void setValue(const QVariant& data, quint32 index = 0); + bool checkValue(const QVariant & data, quint32 index = 0); + void setValue(const QVariant & data, quint32 index = 0); double getDouble(quint32 index = 0); void setDouble(double value, quint32 index = 0); quint32 getDataOffset(); @@ -75,11 +73,11 @@ public: bool isText(); QString toString(); - bool isWithinLimits(QVariant var, quint32 index, int board=0); - QVariant getMaxLimit(quint32 index, int board=0); - QVariant getMinLimit(quint32 index, int board=0); + bool isWithinLimits(QVariant var, quint32 index, int board = 0); + QVariant getMaxLimit(quint32 index, int board = 0); + QVariant getMinLimit(quint32 index, int board = 0); signals: - void fieldUpdated(UAVObjectField* field); + void fieldUpdated(UAVObjectField *field); protected: QString name; @@ -90,14 +88,12 @@ protected: quint32 numElements; quint32 numBytesPerElement; quint32 offset; - quint8* data; - UAVObject* obj; + quint8 *data; + UAVObject *obj; QMap > elementLimits; void clear(); - void constructorInitialize(const QString& name, const QString& units, FieldType type, const QStringList& elementNames, const QStringList& options, const QString &limits); + void constructorInitialize(const QString & name, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString &limits); void limitsInitialize(const QString &limits); - - }; #endif // UAVOBJECTFIELD_H diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.cpp index c5d01f656..4b556b2cd 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.cpp @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavobjectmanager.h" @@ -46,47 +46,40 @@ UAVObjectManager::~UAVObjectManager() * an existing object. The object will be registered and will be properly initialized so that it can accept * updates. */ -bool UAVObjectManager::registerObject(UAVDataObject* obj) +bool UAVObjectManager::registerObject(UAVDataObject *obj) { QMutexLocker locker(mutex); + // Check if this object type is already in the list - for (int objidx = 0; objidx < objects.length(); ++objidx) - { + for (int objidx = 0; objidx < objects.length(); ++objidx) { // Check if the object ID is in the list - if (objects[objidx].length() > 0 && objects[objidx][0]->getObjID() == obj->getObjID()) - { + if (objects[objidx].length() > 0 && objects[objidx][0]->getObjID() == obj->getObjID()) { // Check if this is a single instance object, if yes we can not add a new instance - if (obj->isSingleInstance()) - { + if (obj->isSingleInstance()) { return false; } // The object type has alredy been added, so now we need to initialize the new instance with the appropriate id // There is a single metaobject for all object instances of this type, so no need to create a new one // Get object type metaobject from existing instance - UAVDataObject* refObj = dynamic_cast(objects[objidx][0]); - if (refObj == NULL) - { + UAVDataObject *refObj = dynamic_cast(objects[objidx][0]); + if (refObj == NULL) { return false; } - UAVMetaObject* mobj = refObj->getMetaObject(); + UAVMetaObject *mobj = refObj->getMetaObject(); // If the instance ID is specified and not at the default value (0) then we need to make sure // that there are no gaps in the instance list. If gaps are found then then additional instances // will be created. - if ( (obj->getInstID() > 0) && (obj->getInstID() < MAX_INSTANCES) ) - { - for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) - { - if ( objects[objidx][instidx]->getInstID() == obj->getInstID() ) - { + if ((obj->getInstID() > 0) && (obj->getInstID() < MAX_INSTANCES)) { + for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) { + if (objects[objidx][instidx]->getInstID() == obj->getInstID()) { // Instance conflict, do not add return false; } } // Check if there are any gaps between the requested instance ID and the ones in the list, // if any then create the missing instances. - for (quint32 instidx = objects[objidx].length(); instidx < obj->getInstID(); ++instidx) - { - UAVDataObject* cobj = obj->clone(instidx); + for (quint32 instidx = objects[objidx].length(); instidx < obj->getInstID(); ++instidx) { + UAVDataObject *cobj = obj->clone(instidx); cobj->initialize(mobj); objects[objidx].append(cobj); getObject(cobj->getObjID())->emitNewInstance(cobj); @@ -94,14 +87,10 @@ bool UAVObjectManager::registerObject(UAVDataObject* obj) } // Finally, initialize the actual object instance obj->initialize(mobj); - } - else if (obj->getInstID() == 0) - { + } else if (obj->getInstID() == 0) { // Assign the next available ID and initialize the object instance obj->initialize(objects[objidx].length(), mobj); - } - else - { + } else { return false; } // Add the actual object instance in the list @@ -116,7 +105,7 @@ bool UAVObjectManager::registerObject(UAVDataObject* obj) // Create metaobject QString mname = obj->getName(); mname.append("Meta"); - UAVMetaObject* mobj = new UAVMetaObject(obj->getObjID()+1, mname, obj); + UAVMetaObject *mobj = new UAVMetaObject(obj->getObjID() + 1, mname, obj); // Initialize object obj->initialize(0, mobj); // Add to list @@ -125,10 +114,10 @@ bool UAVObjectManager::registerObject(UAVDataObject* obj) return true; } -void UAVObjectManager::addObject(UAVObject* obj) +void UAVObjectManager::addObject(UAVObject *obj) { // Add to list - QList list; + QList list; list.append(obj); objects.append(list); emit newObject(obj); @@ -138,39 +127,36 @@ void UAVObjectManager::addObject(UAVObject* obj) * Get all objects. A two dimentional QList is returned. Objects are grouped by * instances of the same object type. */ -QList< QList > UAVObjectManager::getObjects() +QList< QList > UAVObjectManager::getObjects() { QMutexLocker locker(mutex); + return objects; } /** * Same as getObjects() but will only return DataObjects. */ -QList< QList > UAVObjectManager::getDataObjects() +QList< QList > UAVObjectManager::getDataObjects() { QMutexLocker locker(mutex); - QList< QList > dObjects; + + QList< QList > dObjects; // Go through objects and copy to new list when types match - for (int objidx = 0; objidx < objects.length(); ++objidx) - { - if (objects[objidx].length() > 0) - { + for (int objidx = 0; objidx < objects.length(); ++objidx) { + if (objects[objidx].length() > 0) { // Check type - UAVDataObject* obj = dynamic_cast(objects[objidx][0]); - if (obj != NULL) - { + UAVDataObject *obj = dynamic_cast(objects[objidx][0]); + if (obj != NULL) { // Create instance list - QList list; + QList list; // Go through instances and cast them to UAVDataObject, then add to list - for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) - { - obj = dynamic_cast(objects[objidx][instidx]); - if (obj != NULL) - { - list.append(obj); - } + for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) { + obj = dynamic_cast(objects[objidx][instidx]); + if (obj != NULL) { + list.append(obj); + } } // Append to object list dObjects.append(list); @@ -184,30 +170,26 @@ QList< QList > UAVObjectManager::getDataObjects() /** * Same as getObjects() but will only return MetaObjects. */ -QList > UAVObjectManager::getMetaObjects() +QList > UAVObjectManager::getMetaObjects() { QMutexLocker locker(mutex); - QList< QList > mObjects; + + QList< QList > mObjects; // Go through objects and copy to new list when types match - for (int objidx = 0; objidx < objects.length(); ++objidx) - { - if (objects[objidx].length() > 0) - { + for (int objidx = 0; objidx < objects.length(); ++objidx) { + if (objects[objidx].length() > 0) { // Check type - UAVMetaObject* obj = dynamic_cast(objects[objidx][0]); - if (obj != NULL) - { + UAVMetaObject *obj = dynamic_cast(objects[objidx][0]); + if (obj != NULL) { // Create instance list - QList list; + QList list; // Go through instances and cast them to UAVMetaObject, then add to list - for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) - { - obj = dynamic_cast(objects[objidx][instidx]); - if (obj != NULL) - { - list.append(obj); - } + for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) { + obj = dynamic_cast(objects[objidx][instidx]); + if (obj != NULL) { + list.append(obj); + } } // Append to object list mObjects.append(list); @@ -222,7 +204,7 @@ QList > UAVObjectManager::getMetaObjects() * Get a specific object given its name and instance ID * @returns The object is found or NULL if not */ -UAVObject* UAVObjectManager::getObject(const QString& name, quint32 instId) +UAVObject *UAVObjectManager::getObject(const QString & name, quint32 instId) { return getObject(&name, 0, instId); } @@ -231,7 +213,7 @@ UAVObject* UAVObjectManager::getObject(const QString& name, quint32 instId) * Get a specific object given its object and instance ID * @returns The object is found or NULL if not */ -UAVObject* UAVObjectManager::getObject(quint32 objId, quint32 instId) +UAVObject *UAVObjectManager::getObject(quint32 objId, quint32 instId) { return getObject(NULL, objId, instId); } @@ -239,29 +221,25 @@ UAVObject* UAVObjectManager::getObject(quint32 objId, quint32 instId) /** * Helper function for the public getObject() functions. */ -UAVObject* UAVObjectManager::getObject(const QString* name, quint32 objId, quint32 instId) +UAVObject *UAVObjectManager::getObject(const QString *name, quint32 objId, quint32 instId) { QMutexLocker locker(mutex); + // Check if this object type is already in the list - for (int objidx = 0; objidx < objects.length(); ++objidx) - { + for (int objidx = 0; objidx < objects.length(); ++objidx) { // Check if the object ID is in the list - if (objects[objidx].length() > 0) - { - if ( (name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId) ) - { + if (objects[objidx].length() > 0) { + if ((name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId)) { // Look for the requested instance ID - for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) - { - if (objects[objidx][instidx]->getInstID() == instId) - { + for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) { + if (objects[objidx][instidx]->getInstID() == instId) { return objects[objidx][instidx]; } } } } } - //qWarning("UAVObjectManager::getObject: Object not found. Probably a bug or mismatched GCS/flight versions."); + // qWarning("UAVObjectManager::getObject: Object not found. Probably a bug or mismatched GCS/flight versions."); // If this point is reached then the requested object could not be found return NULL; } @@ -269,7 +247,7 @@ UAVObject* UAVObjectManager::getObject(const QString* name, quint32 objId, quint /** * Get all the instances of the object specified by name */ -QList UAVObjectManager::getObjectInstances(const QString& name) +QList UAVObjectManager::getObjectInstances(const QString & name) { return getObjectInstances(&name, 0); } @@ -277,7 +255,7 @@ QList UAVObjectManager::getObjectInstances(const QString& name) /** * Get all the instances of the object specified by its ID */ -QList UAVObjectManager::getObjectInstances(quint32 objId) +QList UAVObjectManager::getObjectInstances(quint32 objId) { return getObjectInstances(NULL, objId); } @@ -285,29 +263,27 @@ QList UAVObjectManager::getObjectInstances(quint32 objId) /** * Helper function for the public getObjectInstances() */ -QList UAVObjectManager::getObjectInstances(const QString* name, quint32 objId) +QList UAVObjectManager::getObjectInstances(const QString *name, quint32 objId) { QMutexLocker locker(mutex); + // Check if this object type is already in the list - for (int objidx = 0; objidx < objects.length(); ++objidx) - { + for (int objidx = 0; objidx < objects.length(); ++objidx) { // Check if the object ID is in the list - if (objects[objidx].length() > 0) - { - if ( (name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId) ) - { + if (objects[objidx].length() > 0) { + if ((name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId)) { return objects[objidx]; } } } // If this point is reached then the requested object could not be found - return QList(); + return QList(); } /** * Get the number of instances for an object given its name */ -qint32 UAVObjectManager::getNumInstances(const QString& name) +qint32 UAVObjectManager::getNumInstances(const QString & name) { return getNumInstances(&name, 0); } @@ -323,17 +299,15 @@ qint32 UAVObjectManager::getNumInstances(quint32 objId) /** * Helper function for public getNumInstances */ -qint32 UAVObjectManager::getNumInstances(const QString* name, quint32 objId) +qint32 UAVObjectManager::getNumInstances(const QString *name, quint32 objId) { QMutexLocker locker(mutex); + // Check if this object type is already in the list - for (int objidx = 0; objidx < objects.length(); ++objidx) - { + for (int objidx = 0; objidx < objects.length(); ++objidx) { // Check if the object ID is in the list - if (objects[objidx].length() > 0) - { - if ( (name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId) ) - { + if (objects[objidx].length() > 0) { + if ((name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId)) { return objects[objidx].length(); } } diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.h index 5f17daa1f..a7fbfdb31 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectmanager.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVOBJECTMANAGER_H @@ -36,39 +36,38 @@ #include #include -class UAVOBJECTS_EXPORT UAVObjectManager: public QObject -{ +class UAVOBJECTS_EXPORT UAVObjectManager : public QObject { Q_OBJECT public: UAVObjectManager(); ~UAVObjectManager(); - bool registerObject(UAVDataObject* obj); - QList< QList > getObjects(); - QList< QList > getDataObjects(); - QList< QList > getMetaObjects(); - UAVObject* getObject(const QString& name, quint32 instId = 0); - UAVObject* getObject(quint32 objId, quint32 instId = 0); - QList getObjectInstances(const QString& name); - QList getObjectInstances(quint32 objId); - qint32 getNumInstances(const QString& name); + bool registerObject(UAVDataObject *obj); + QList< QList > getObjects(); + QList< QList > getDataObjects(); + QList< QList > getMetaObjects(); + UAVObject *getObject(const QString & name, quint32 instId = 0); + UAVObject *getObject(quint32 objId, quint32 instId = 0); + QList getObjectInstances(const QString & name); + QList getObjectInstances(quint32 objId); + qint32 getNumInstances(const QString & name); qint32 getNumInstances(quint32 objId); signals: - void newObject(UAVObject* obj); - void newInstance(UAVObject* obj); + void newObject(UAVObject *obj); + void newInstance(UAVObject *obj); private: static const quint32 MAX_INSTANCES = 1000; - QList< QList > objects; - QMutex* mutex; + QList< QList > objects; + QMutex *mutex; - void addObject(UAVObject* obj); - UAVObject* getObject(const QString* name, quint32 objId, quint32 instId); - QList getObjectInstances(const QString* name, quint32 objId); - qint32 getNumInstances(const QString* name, quint32 objId); + void addObject(UAVObject *obj); + UAVObject *getObject(const QString *name, quint32 objId, quint32 instId); + QList getObjectInstances(const QString *name, quint32 objId); + qint32 getNumInstances(const QString *name, quint32 objId); }; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects_global.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects_global.h index 9ef17f24b..b1e27f904 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects_global.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects_global.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsinit.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsinit.h index 03ae0b1d3..77e905ee1 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsinit.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsinit.h @@ -10,19 +10,19 @@ * @{ * @brief The UAVUObjects GCS plugin *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVOBJECTSINIT_H @@ -30,6 +30,6 @@ #include "uavobjectmanager.h" -void UAVObjectsInitialize(UAVObjectManager* objMngr); +void UAVObjectsInitialize(UAVObjectManager *objMngr); #endif // UAVOBJECTSINIT_H diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.cpp index c3f307927..892f2e11e 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.cpp @@ -11,42 +11,37 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavobjectsplugin.h" #include "uavobjectsinit.h" UAVObjectsPlugin::UAVObjectsPlugin() -{ - -} +{} UAVObjectsPlugin::~UAVObjectsPlugin() -{ - -} +{} void UAVObjectsPlugin::extensionsInitialized() -{ +{} -} - -bool UAVObjectsPlugin::initialize(const QStringList & arguments, QString * errorString) +bool UAVObjectsPlugin::initialize(const QStringList & arguments, QString *errorString) { // Create object manager and expose object - UAVObjectManager* objMngr = new UAVObjectManager(); + UAVObjectManager *objMngr = new UAVObjectManager(); + addAutoReleasedObject(objMngr); // Initialize UAVObjects UAVObjectsInitialize(objMngr); @@ -57,8 +52,6 @@ bool UAVObjectsPlugin::initialize(const QStringList & arguments, QString * error } void UAVObjectsPlugin::shutdown() -{ - -} +{} Q_EXPORT_PLUGIN(UAVObjectsPlugin) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.h index 137af2ed8..4a9238752 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectsplugin.h @@ -11,18 +11,18 @@ * @brief The UAVUObjects GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVOBJECTSPLUGIN_H @@ -33,9 +33,8 @@ #include #include "uavobjectmanager.h" -class UAVOBJECTS_EXPORT UAVObjectsPlugin: - public ExtensionSystem::IPlugin -{ +class UAVOBJECTS_EXPORT UAVObjectsPlugin : + public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -43,7 +42,7 @@ public: ~UAVObjectsPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); }; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/wireshark/op-uavtalk/packet-op-uavtalk.c b/ground/openpilotgcs/src/plugins/uavobjects/wireshark/op-uavtalk/packet-op-uavtalk.c index df8134df8..f45c9e868 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/wireshark/op-uavtalk/packet-op-uavtalk.c +++ b/ground/openpilotgcs/src/plugins/uavobjects/wireshark/op-uavtalk/packet-op-uavtalk.c @@ -1,26 +1,26 @@ /* packet-op-uavtalk.c - * Routines for OpenPilot UAVTalk packet dissection - * Copyright 2012 Stacey Sheldon - * - * $Id$ - * - * Wireshark - Network traffic analyzer - * By Gerald Combs - * Copyright 1998 Gerald Combs - * - * This program is free software; you can redistribute it and/or - * modify it under the terms of the GNU General Public License - * as published by the Free Software Foundation; either version 2 - * of the License, or (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * Routines for OpenPilot UAVTalk packet dissection + * Copyright 2012 Stacey Sheldon + * + * $Id$ + * + * Wireshark - Network traffic analyzer + * By Gerald Combs + * Copyright 1998 Gerald Combs + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. */ @@ -30,7 +30,7 @@ #include #include -#include /* ptvcursor_* */ +#include /* ptvcursor_* */ #include #include @@ -39,167 +39,167 @@ static guint global_op_uavtalk_port = 9000; static int proto_op_uavtalk = -1; -static gint ett_op_uavtalk = -1; +static gint ett_op_uavtalk = -1; static dissector_handle_t data_handle; static dissector_table_t uavtalk_subdissector_table; -static int hf_op_uavtalk_sync = -1; +static int hf_op_uavtalk_sync = -1; static int hf_op_uavtalk_version = -1; -static int hf_op_uavtalk_type = -1; -static int hf_op_uavtalk_len = -1; -static int hf_op_uavtalk_objid = -1; -static int hf_op_uavtalk_crc8 = -1; +static int hf_op_uavtalk_type = -1; +static int hf_op_uavtalk_len = -1; +static int hf_op_uavtalk_objid = -1; +static int hf_op_uavtalk_crc8 = -1; #define UAVTALK_SYNC_VAL 0x3C -static const value_string uavtalk_packet_types[]={ - { 0, "TxObj" }, - { 1, "GetObj" }, - { 2, "SetObjAckd" }, - { 3, "Ack" }, - { 4, "Nack" }, - { 0, NULL } +static const value_string uavtalk_packet_types[] = { + { 0, "TxObj" }, + { 1, "GetObj" }, + { 2, "SetObjAckd" }, + { 3, "Ack" }, + { 4, "Nack" }, + { 0, NULL } }; void proto_reg_handoff_op_uavtalk(void); -#define UAVTALK_HEADER_SIZE 8 +#define UAVTALK_HEADER_SIZE 8 #define UAVTALK_TRAILER_SIZE 1 static int dissect_op_uavtalk(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree) { - gint offset = 0; + gint offset = 0; - guint8 packet_type = tvb_get_guint8(tvb, 1) & 0x7; - guint32 objid = tvb_get_letohl(tvb, 4); - guint32 payload_length = tvb_get_letohs(tvb, 2) - UAVTALK_HEADER_SIZE - UAVTALK_TRAILER_SIZE; - guint32 reported_length = tvb_reported_length(tvb); + guint8 packet_type = tvb_get_guint8(tvb, 1) & 0x7; + guint32 objid = tvb_get_letohl(tvb, 4); + guint32 payload_length = tvb_get_letohs(tvb, 2) - UAVTALK_HEADER_SIZE - UAVTALK_TRAILER_SIZE; + guint32 reported_length = tvb_reported_length(tvb); - col_set_str(pinfo->cinfo, COL_PROTOCOL, "UAVTALK"); - /* Clear out stuff in the info column */ - col_clear(pinfo->cinfo, COL_INFO); + col_set_str(pinfo->cinfo, COL_PROTOCOL, "UAVTALK"); + /* Clear out stuff in the info column */ + col_clear(pinfo->cinfo, COL_INFO); - col_append_fstr(pinfo->cinfo, COL_INFO, "%s: 0x%08x", val_to_str_const(packet_type, uavtalk_packet_types, ""), objid); - if (objid & 0x1) { - col_append_str(pinfo->cinfo, COL_INFO, "(META)"); - } - - - if (tree) { /* we are being asked for details */ - proto_tree *op_uavtalk_tree = NULL; - ptvcursor_t * cursor; - proto_item *ti = NULL; - - /* Add a top-level entry to the dissector tree for this protocol */ - ti = proto_tree_add_item(tree, proto_op_uavtalk, tvb, 0, -1, ENC_NA); - - /* Create a subtree to contain the dissection of this protocol */ - op_uavtalk_tree = proto_item_add_subtree(ti, ett_op_uavtalk); - - /* Dissect the packet and populate the subtree */ - cursor = ptvcursor_new(op_uavtalk_tree, tvb, 0); - - /* Populate the fields in this protocol */ - ptvcursor_add(cursor, hf_op_uavtalk_sync, 1, ENC_LITTLE_ENDIAN); - ptvcursor_add_no_advance(cursor, hf_op_uavtalk_version, 1, ENC_LITTLE_ENDIAN); - ptvcursor_add(cursor, hf_op_uavtalk_type, 1, ENC_LITTLE_ENDIAN); - ptvcursor_add(cursor, hf_op_uavtalk_len, 2, ENC_LITTLE_ENDIAN); - ptvcursor_add(cursor, hf_op_uavtalk_objid, 4, ENC_LITTLE_ENDIAN); - - offset = ptvcursor_current_offset(cursor); - - ptvcursor_free(cursor); - - proto_tree_add_item(op_uavtalk_tree, hf_op_uavtalk_crc8, tvb, reported_length - UAVTALK_TRAILER_SIZE, UAVTALK_TRAILER_SIZE, ENC_LITTLE_ENDIAN); - } else { - offset = UAVTALK_HEADER_SIZE; - } - - { - tvbuff_t * next_tvb = tvb_new_subset(tvb, offset, - reported_length - UAVTALK_HEADER_SIZE - UAVTALK_TRAILER_SIZE, - payload_length); - - /* Check if we have an embedded objid to decode */ - if ((packet_type == 0) || (packet_type == 2)) { - /* Call any registered subdissector for this objid */ - if (!dissector_try_uint(uavtalk_subdissector_table, objid, next_tvb, pinfo, tree)) { - /* No subdissector registered, use the default data dissector */ - call_dissector(data_handle, next_tvb, pinfo, tree); - } - } else { - /* Render any remaining data as raw bytes */ - call_dissector(data_handle, next_tvb, pinfo, tree); + col_append_fstr(pinfo->cinfo, COL_INFO, "%s: 0x%08x", val_to_str_const(packet_type, uavtalk_packet_types, ""), objid); + if (objid & 0x1) { + col_append_str(pinfo->cinfo, COL_INFO, "(META)"); } - } - return UAVTALK_HEADER_SIZE + UAVTALK_TRAILER_SIZE; + + if (tree) { /* we are being asked for details */ + proto_tree *op_uavtalk_tree = NULL; + ptvcursor_t *cursor; + proto_item *ti = NULL; + + /* Add a top-level entry to the dissector tree for this protocol */ + ti = proto_tree_add_item(tree, proto_op_uavtalk, tvb, 0, -1, ENC_NA); + + /* Create a subtree to contain the dissection of this protocol */ + op_uavtalk_tree = proto_item_add_subtree(ti, ett_op_uavtalk); + + /* Dissect the packet and populate the subtree */ + cursor = ptvcursor_new(op_uavtalk_tree, tvb, 0); + + /* Populate the fields in this protocol */ + ptvcursor_add(cursor, hf_op_uavtalk_sync, 1, ENC_LITTLE_ENDIAN); + ptvcursor_add_no_advance(cursor, hf_op_uavtalk_version, 1, ENC_LITTLE_ENDIAN); + ptvcursor_add(cursor, hf_op_uavtalk_type, 1, ENC_LITTLE_ENDIAN); + ptvcursor_add(cursor, hf_op_uavtalk_len, 2, ENC_LITTLE_ENDIAN); + ptvcursor_add(cursor, hf_op_uavtalk_objid, 4, ENC_LITTLE_ENDIAN); + + offset = ptvcursor_current_offset(cursor); + + ptvcursor_free(cursor); + + proto_tree_add_item(op_uavtalk_tree, hf_op_uavtalk_crc8, tvb, reported_length - UAVTALK_TRAILER_SIZE, UAVTALK_TRAILER_SIZE, ENC_LITTLE_ENDIAN); + } else { + offset = UAVTALK_HEADER_SIZE; + } + + { + tvbuff_t *next_tvb = tvb_new_subset(tvb, offset, + reported_length - UAVTALK_HEADER_SIZE - UAVTALK_TRAILER_SIZE, + payload_length); + + /* Check if we have an embedded objid to decode */ + if ((packet_type == 0) || (packet_type == 2)) { + /* Call any registered subdissector for this objid */ + if (!dissector_try_uint(uavtalk_subdissector_table, objid, next_tvb, pinfo, tree)) { + /* No subdissector registered, use the default data dissector */ + call_dissector(data_handle, next_tvb, pinfo, tree); + } + } else { + /* Render any remaining data as raw bytes */ + call_dissector(data_handle, next_tvb, pinfo, tree); + } + } + + return UAVTALK_HEADER_SIZE + UAVTALK_TRAILER_SIZE; } void proto_register_op_uavtalk(void) { - module_t * op_uavtalk_module; + module_t *op_uavtalk_module; - static hf_register_info hf[] = { - { &hf_op_uavtalk_sync, - { "Sync Byte", "uavtalk.sync", FT_UINT8, - BASE_HEX, NULL, 0x0, NULL, HFILL } - }, - { &hf_op_uavtalk_version, - { "Version", "uavtalk.ver", FT_UINT8, - BASE_DEC, NULL, 0xf8, NULL, HFILL } - }, - { &hf_op_uavtalk_type, - { "Type", "uavtalk.type", FT_UINT8, - BASE_HEX, VALS(uavtalk_packet_types), 0x07, NULL, HFILL } - }, - { &hf_op_uavtalk_len, - { "Length", "uavtalk.len", FT_UINT16, - BASE_DEC, NULL, 0x0, NULL, HFILL } - }, - { &hf_op_uavtalk_objid, - { "ObjID", "uavtalk.objid", FT_UINT32, - BASE_HEX, NULL, 0x0, NULL, HFILL } - }, - { &hf_op_uavtalk_crc8, - { "Crc8", "uavtalk.crc8", FT_UINT8, - BASE_HEX, NULL, 0x0, NULL, HFILL } - }, - }; + static hf_register_info hf[] = { + { &hf_op_uavtalk_sync, + { "Sync Byte", "uavtalk.sync", FT_UINT8, + BASE_HEX, NULL, 0x0, NULL, HFILL } + }, + { &hf_op_uavtalk_version, + { "Version", "uavtalk.ver", FT_UINT8, + BASE_DEC, NULL, 0xf8, NULL, HFILL } + }, + { &hf_op_uavtalk_type, + { "Type", "uavtalk.type", FT_UINT8, + BASE_HEX, VALS(uavtalk_packet_types), 0x07, NULL, HFILL } + }, + { &hf_op_uavtalk_len, + { "Length", "uavtalk.len", FT_UINT16, + BASE_DEC, NULL, 0x0, NULL, HFILL } + }, + { &hf_op_uavtalk_objid, + { "ObjID", "uavtalk.objid", FT_UINT32, + BASE_HEX, NULL, 0x0, NULL, HFILL } + }, + { &hf_op_uavtalk_crc8, + { "Crc8", "uavtalk.crc8", FT_UINT8, + BASE_HEX, NULL, 0x0, NULL, HFILL } + }, + }; /* Setup protocol subtree array */ - static gint *ett[] = { - &ett_op_uavtalk - }; + static gint *ett[] = { + &ett_op_uavtalk + }; - proto_op_uavtalk = proto_register_protocol("OpenPilot UAVTalk Protocol", - "UAVTALK", - "uavtalk"); + proto_op_uavtalk = proto_register_protocol("OpenPilot UAVTalk Protocol", + "UAVTALK", + "uavtalk"); - /* Allow subdissectors for each objid to bind for decoding */ - uavtalk_subdissector_table = register_dissector_table("uavtalk.objid", "UAVObject ID", FT_UINT32, BASE_HEX); + /* Allow subdissectors for each objid to bind for decoding */ + uavtalk_subdissector_table = register_dissector_table("uavtalk.objid", "UAVObject ID", FT_UINT32, BASE_HEX); - proto_register_subtree_array(ett, array_length(ett)); - proto_register_field_array(proto_op_uavtalk, hf, array_length(hf)); + proto_register_subtree_array(ett, array_length(ett)); + proto_register_field_array(proto_op_uavtalk, hf, array_length(hf)); - op_uavtalk_module = prefs_register_protocol(proto_op_uavtalk, proto_reg_handoff_op_uavtalk); + op_uavtalk_module = prefs_register_protocol(proto_op_uavtalk, proto_reg_handoff_op_uavtalk); - prefs_register_uint_preference(op_uavtalk_module, "udp.port", "UAVTALK UDP port", - "UAVTALK port (default 9000)", 10, &global_op_uavtalk_port); + prefs_register_uint_preference(op_uavtalk_module, "udp.port", "UAVTALK UDP port", + "UAVTALK port (default 9000)", 10, &global_op_uavtalk_port); } void proto_reg_handoff_op_uavtalk(void) { - static dissector_handle_t op_uavtalk_handle; + static dissector_handle_t op_uavtalk_handle; - op_uavtalk_handle = new_create_dissector_handle(dissect_op_uavtalk, proto_op_uavtalk); - dissector_add_handle("udp.port", op_uavtalk_handle); /* for "decode as" */ + op_uavtalk_handle = new_create_dissector_handle(dissect_op_uavtalk, proto_op_uavtalk); + dissector_add_handle("udp.port", op_uavtalk_handle); /* for "decode as" */ - if (global_op_uavtalk_port != 0) { - dissector_add_uint("udp.port", global_op_uavtalk_port, op_uavtalk_handle); - } + if (global_op_uavtalk_port != 0) { + dissector_add_uint("udp.port", global_op_uavtalk_port, op_uavtalk_handle); + } - /* Lookup the default dissector for raw data */ - data_handle = find_dissector("data"); + /* Lookup the default dissector for raw data */ + data_handle = find_dissector("data"); } diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h b/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h index fb7cf2288..aedba4c33 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h @@ -2,55 +2,62 @@ #define DEVICEDESCRIPTORSTRUCT_H #include -struct deviceDescriptorStruct -{ +struct deviceDescriptorStruct { public: - QString gitHash; - QString gitDate; - QString gitTag; - QByteArray fwHash; - QByteArray uavoHash; - int boardType; - int boardRevision; - static QString idToBoardName(int id) - { - switch (id) { - case 0x0101: - // MB - return QString("OpenPilot MainBoard"); - break; - case 0x0201: - // INS - return QString("OpenPilot INS"); - break; - case 0x0301: - // OPLink Mini - return QString("OPLink"); - break; - case 0x0401: - // Coptercontrol - return QString("CopterControl"); - break; - case 0x0402: - // Coptercontrol 3D - // It would be nice to say CC3D here but since currently we use string comparisons - // for firmware compatibility and the filename path that would break - return QString("CopterControl"); - break; - case 0x0901: - // Revolution - return QString("Revolution"); - break; - case 0x0903: - // Revo Mini - return QString("Revolution"); - break; - default: - return QString(""); - break; - } - } - deviceDescriptorStruct(){} + QString gitHash; + QString gitDate; + QString gitTag; + QByteArray fwHash; + QByteArray uavoHash; + int boardType; + int boardRevision; + static QString idToBoardName(int id) + { + switch (id) { + case 0x0101: + // MB + return QString("OpenPilot MainBoard"); + + break; + case 0x0201: + // INS + return QString("OpenPilot INS"); + + break; + case 0x0301: + // OPLink Mini + return QString("OPLink"); + + break; + case 0x0401: + // Coptercontrol + return QString("CopterControl"); + + break; + case 0x0402: + // Coptercontrol 3D + // It would be nice to say CC3D here but since currently we use string comparisons + // for firmware compatibility and the filename path that would break + return QString("CopterControl"); + + break; + case 0x0901: + // Revolution + return QString("Revolution"); + + break; + case 0x0903: + // Revo Mini + return QString("Revolution"); + + break; + default: + return QString(""); + + break; + } + } + deviceDescriptorStruct() {} }; #endif // DEVICEDESCRIPTORSTRUCT_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index 3f942e3e0..c89683412 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -45,43 +45,40 @@ UAVObjectUtilManager::UAVObjectUtilManager() { - mutex = new QMutex(QMutex::Recursive); + mutex = new QMutex(QMutex::Recursive); saveState = IDLE; failureTimer.stop(); failureTimer.setSingleShot(true); failureTimer.setInterval(1000); - connect(&failureTimer, SIGNAL(timeout()),this,SLOT(objectPersistenceOperationFailed())); + connect(&failureTimer, SIGNAL(timeout()), this, SLOT(objectPersistenceOperationFailed())); - pm = NULL; - obm = NULL; + pm = NULL; + obm = NULL; obum = NULL; - pm = ExtensionSystem::PluginManager::instance(); - if (pm) - { - obm = pm->getObject(); + pm = ExtensionSystem::PluginManager::instance(); + if (pm) { + obm = pm->getObject(); obum = pm->getObject(); } - } UAVObjectUtilManager::~UAVObjectUtilManager() { -// while (!queue.isEmpty()) - { - } +// while (!queue.isEmpty()) + {} - disconnect(); + disconnect(); - if (mutex) - { - delete mutex; - mutex = NULL; - } + if (mutex) { + delete mutex; + mutex = NULL; + } } -UAVObjectManager* UAVObjectUtilManager::getObjectManager() { +UAVObjectManager *UAVObjectUtilManager::getObjectManager() +{ Q_ASSERT(obm); return obm; } @@ -92,8 +89,8 @@ UAVObjectManager* UAVObjectUtilManager::getObjectManager() { // /* - Add a new object to save in the queue - */ + Add a new object to save in the queue + */ void UAVObjectUtilManager::saveObjectToSD(UAVObject *obj) { // Add to queue @@ -103,35 +100,32 @@ void UAVObjectUtilManager::saveObjectToSD(UAVObject *obj) // If queue length is one, then start sending (call sendNextObject) // Otherwise, do nothing, it's sending anyway - if (queue.length()==1) + if (queue.length() == 1) { saveNextObject(); - - + } } void UAVObjectUtilManager::saveNextObject() { - if ( queue.isEmpty() ) - { + if (queue.isEmpty()) { return; } Q_ASSERT(saveState == IDLE); // Get next object from the queue - UAVObject* obj = queue.head(); + UAVObject *obj = queue.head(); qDebug() << "Send save object request to board " << obj->getName(); - ObjectPersistence* objper = dynamic_cast( getObjectManager()->getObject(ObjectPersistence::NAME) ); - connect(objper, SIGNAL(transactionCompleted(UAVObject*,bool)), this, SLOT(objectPersistenceTransactionCompleted(UAVObject*,bool))); - connect(objper, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objectPersistenceUpdated(UAVObject *))); + ObjectPersistence *objper = dynamic_cast(getObjectManager()->getObject(ObjectPersistence::NAME)); + connect(objper, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(objectPersistenceTransactionCompleted(UAVObject *, bool))); + connect(objper, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(objectPersistenceUpdated(UAVObject *))); saveState = AWAITING_ACK; - if (obj != NULL) - { + if (obj != NULL) { ObjectPersistence::DataFields data; - data.Operation = ObjectPersistence::OPERATION_SAVE; - data.Selection = ObjectPersistence::SELECTION_SINGLEOBJECT; - data.ObjectID = obj->getObjID(); + data.Operation = ObjectPersistence::OPERATION_SAVE; + data.Selection = ObjectPersistence::SELECTION_SINGLEOBJECT; + data.ObjectID = obj->getObjID(); data.InstanceID = obj->getInstID(); objper->setData(data); objper->updated(); @@ -144,16 +138,16 @@ void UAVObjectUtilManager::saveNextObject() } /** - * @brief Process the transactionCompleted message from Telemetry indicating request sent successfully - * @param[in] The object just transsacted. Must be ObjectPersistance - * @param[in] success Indicates that the transaction did not time out - * - * After a failed transaction (usually timeout) resends the save request. After a succesful - * transaction will then wait for a save completed update from the autopilot. - */ -void UAVObjectUtilManager::objectPersistenceTransactionCompleted(UAVObject* obj, bool success) + * @brief Process the transactionCompleted message from Telemetry indicating request sent successfully + * @param[in] The object just transsacted. Must be ObjectPersistance + * @param[in] success Indicates that the transaction did not time out + * + * After a failed transaction (usually timeout) resends the save request. After a succesful + * transaction will then wait for a save completed update from the autopilot. + */ +void UAVObjectUtilManager::objectPersistenceTransactionCompleted(UAVObject *obj, bool success) { - if(success) { + if (success) { Q_ASSERT(obj->getName().compare("ObjectPersistence") == 0); Q_ASSERT(saveState == AWAITING_ACK); // Two things can happen: @@ -163,7 +157,7 @@ void UAVObjectUtilManager::objectPersistenceTransactionCompleted(UAVObject* obj, // For this reason, we will arm a 1 second timer to make provision for this and not block // the queue: saveState = AWAITING_COMPLETED; - disconnect(obj, SIGNAL(transactionCompleted(UAVObject*,bool)), this, SLOT(objectPersistenceTransactionCompleted(UAVObject*,bool))); + disconnect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(objectPersistenceTransactionCompleted(UAVObject *, bool))); failureTimer.start(2000); // Create a timeout } else { // Can be caused by timeout errors on sending. Forget it and send next. @@ -178,20 +172,20 @@ void UAVObjectUtilManager::objectPersistenceTransactionCompleted(UAVObject* obj, } /** - * @brief Object persistence operation failed, i.e. we never got an update - * from the board saying "completed". - */ + * @brief Object persistence operation failed, i.e. we never got an update + * from the board saying "completed". + */ void UAVObjectUtilManager::objectPersistenceOperationFailed() { - if(saveState == AWAITING_COMPLETED) { - //TODO: some warning that this operation failed somehow + if (saveState == AWAITING_COMPLETED) { + // TODO: some warning that this operation failed somehow // We have to disconnect the object persistence 'updated' signal // and ask to save the next object: - ObjectPersistence * objectPersistence = ObjectPersistence::GetInstance(getObjectManager()); + ObjectPersistence *objectPersistence = ObjectPersistence::GetInstance(getObjectManager()); Q_ASSERT(objectPersistence); - UAVObject* obj = queue.dequeue(); // We can now remove the object, it failed anyway. + UAVObject *obj = queue.dequeue(); // We can now remove the object, it failed anyway. Q_ASSERT(obj); objectPersistence->disconnect(this); @@ -201,31 +195,29 @@ void UAVObjectUtilManager::objectPersistenceOperationFailed() saveNextObject(); } - } - /** - * @brief Process the ObjectPersistence updated message to confirm the right object saved - * then requests next object be saved. - * @param[in] The object just received. Must be ObjectPersistance - */ -void UAVObjectUtilManager::objectPersistenceUpdated(UAVObject * obj) + * @brief Process the ObjectPersistence updated message to confirm the right object saved + * then requests next object be saved. + * @param[in] The object just received. Must be ObjectPersistance + */ +void UAVObjectUtilManager::objectPersistenceUpdated(UAVObject *obj) { Q_ASSERT(obj); Q_ASSERT(obj->getObjID() == ObjectPersistence::OBJID); ObjectPersistence::DataFields objectPersistence = ((ObjectPersistence *)obj)->getData(); - if(saveState == AWAITING_COMPLETED && objectPersistence.Operation == ObjectPersistence::OPERATION_ERROR) { + if (saveState == AWAITING_COMPLETED && objectPersistence.Operation == ObjectPersistence::OPERATION_ERROR) { failureTimer.stop(); objectPersistenceOperationFailed(); } else if (saveState == AWAITING_COMPLETED && objectPersistence.Operation == ObjectPersistence::OPERATION_COMPLETED) { failureTimer.stop(); // Check right object saved - UAVObject* savingObj = queue.head(); - if(objectPersistence.ObjectID != savingObj->getObjID() ) { + UAVObject *savingObj = queue.head(); + if (objectPersistence.ObjectID != savingObj->getObjID()) { objectPersistenceOperationFailed(); return; } @@ -240,45 +232,49 @@ void UAVObjectUtilManager::objectPersistenceUpdated(UAVObject * obj) } /** - * Helper function that makes sure FirmwareIAP is updated and then returns the data - */ + * Helper function that makes sure FirmwareIAP is updated and then returns the data + */ FirmwareIAPObj::DataFields UAVObjectUtilManager::getFirmwareIap() { FirmwareIAPObj::DataFields dummy; FirmwareIAPObj *firmwareIap = FirmwareIAPObj::GetInstance(obm); + Q_ASSERT(firmwareIap); - if (!firmwareIap) + if (!firmwareIap) { return dummy; + } return firmwareIap->getData(); } /** - * Get the UAV Board model, for anyone interested. Return format is: - * (Board Type << 8) + BoardRevision. - */ + * Get the UAV Board model, for anyone interested. Return format is: + * (Board Type << 8) + BoardRevision. + */ int UAVObjectUtilManager::getBoardModel() { FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); - qDebug()<<"Board type="< #include -class UAVOBJECTUTIL_EXPORT UAVObjectUtilManager: public QObject -{ +class UAVOBJECTUTIL_EXPORT UAVObjectUtilManager : public QObject { Q_OBJECT public: - UAVObjectUtilManager(); - ~UAVObjectUtilManager(); + UAVObjectUtilManager(); + ~UAVObjectUtilManager(); - int setHomeLocation(double LLA[3], bool save_to_sdcard); - int getHomeLocation(bool &set, double LLA[3]); + int setHomeLocation(double LLA[3], bool save_to_sdcard); + int getHomeLocation(bool &set, double LLA[3]); - int getGPSPosition(double LLA[3]); + int getGPSPosition(double LLA[3]); - int getBoardModel(); - QByteArray getBoardCPUSerial(); - quint32 getFirmwareCRC(); - QByteArray getBoardDescription(); - deviceDescriptorStruct getBoardDescriptionStruct(); - static bool descriptionToStructure(QByteArray desc,deviceDescriptorStruct & struc); - UAVObjectManager* getObjectManager(); - void saveObjectToSD(UAVObject *obj); + int getBoardModel(); + QByteArray getBoardCPUSerial(); + quint32 getFirmwareCRC(); + QByteArray getBoardDescription(); + deviceDescriptorStruct getBoardDescriptionStruct(); + static bool descriptionToStructure(QByteArray desc, deviceDescriptorStruct & struc); + UAVObjectManager *getObjectManager(); + void saveObjectToSD(UAVObject *obj); protected: - FirmwareIAPObj::DataFields getFirmwareIap(); + FirmwareIAPObj::DataFields getFirmwareIap(); signals: - void saveCompleted(int objectID, bool status); + void saveCompleted(int objectID, bool status); private: QMutex *mutex; QQueue queue; - enum {IDLE, AWAITING_ACK, AWAITING_COMPLETED} saveState; + enum { IDLE, AWAITING_ACK, AWAITING_COMPLETED } saveState; void saveNextObject(); QTimer failureTimer; @@ -84,13 +83,11 @@ private: UAVObjectUtilManager *obum; private slots: - //void transactionCompleted(UAVObject *obj, bool success); - void objectPersistenceTransactionCompleted(UAVObject* obj, bool success); - void objectPersistenceUpdated(UAVObject * obj); - void objectPersistenceOperationFailed(); - - + // void transactionCompleted(UAVObject *obj, bool success); + void objectPersistenceTransactionCompleted(UAVObject *obj, bool success); + void objectPersistenceUpdated(UAVObject *obj); + void objectPersistenceOperationFailed(); }; -#endif +#endif // ifndef UAVOBJECTUTILMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.cpp index 0900f9e86..6ec8cda12 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.cpp @@ -11,49 +11,45 @@ * @brief The UAVUObjectUtil GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavobjectutilplugin.h" UAVObjectUtilPlugin::UAVObjectUtilPlugin() -{ -} +{} UAVObjectUtilPlugin::~UAVObjectUtilPlugin() -{ -} +{} void UAVObjectUtilPlugin::extensionsInitialized() -{ -} +{} -bool UAVObjectUtilPlugin::initialize(const QStringList & arguments, QString * errorString) +bool UAVObjectUtilPlugin::initialize(const QStringList & arguments, QString *errorString) { - Q_UNUSED(arguments) - Q_UNUSED(errorString) + Q_UNUSED(arguments) + Q_UNUSED(errorString) - // Create manager and expose object - UAVObjectUtilManager *objUtilMngr = new UAVObjectUtilManager(); - addAutoReleasedObject(objUtilMngr); + // Create manager and expose object + UAVObjectUtilManager * objUtilMngr = new UAVObjectUtilManager(); + addAutoReleasedObject(objUtilMngr); return true; } void UAVObjectUtilPlugin::shutdown() -{ -} +{} Q_EXPORT_PLUGIN(UAVObjectUtilPlugin) diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.h b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.h index 7eef7f9ee..87cbd838a 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.h @@ -11,18 +11,18 @@ * @brief The UAVUObjectUtil GCS plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVOBJECTUTILPLUGIN_H @@ -33,16 +33,15 @@ #include #include "uavobjectutilmanager.h" -class UAVOBJECTUTIL_EXPORT UAVObjectUtilPlugin: public ExtensionSystem::IPlugin -{ +class UAVOBJECTUTIL_EXPORT UAVObjectUtilPlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: - UAVObjectUtilPlugin(); - ~UAVObjectUtilPlugin(); + UAVObjectUtilPlugin(); + ~UAVObjectUtilPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); }; diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp index 0c4036091..2f007eec0 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp @@ -883,9 +883,9 @@ void ConfigTaskWidget::reloadButtonClicked() if (!list) { return; } - ObjectPersistence *objper = dynamic_cast(getObjectManager()->getObject(ObjectPersistence::NAME)); + ObjectPersistence *objper = dynamic_cast(getObjectManager()->getObject(ObjectPersistence::NAME)); timeOut = new QTimer(this); - QEventLoop *eventLoop = new QEventLoop(this); + QEventLoop *eventLoop = new QEventLoop(this); connect(timeOut, SIGNAL(timeout()), eventLoop, SLOT(quit())); connect(objper, SIGNAL(objectUpdated(UAVObject *)), eventLoop, SLOT(quit())); diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h index 461366251..c4c26f0fb 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h @@ -85,14 +85,14 @@ public: enum buttonTypeEnum { none, save_button, apply_button, reload_button, default_button, help_button }; struct uiRelationAutomation { - QString objname; - QString fieldname; - QString element; - QString url; + QString objname; + QString fieldname; + QString element; + QString url; buttonTypeEnum buttonType; QList buttonGroup; - double scale; - bool haslimits; + double scale; + bool haslimits; }; ConfigTaskWidget(QWidget *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.cpp index 7add08628..13c83bdca 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.cpp @@ -38,15 +38,14 @@ Edge::Edge(MixerNode *sourceNode, MixerNode *destNode) { setAcceptedMouseButtons(0); source = sourceNode; - dest = destNode; + dest = destNode; source->addEdge(this); dest->addEdge(this); adjust(); } Edge::~Edge() -{ -} +{} MixerNode *Edge::sourceNode() const { @@ -72,8 +71,9 @@ void Edge::setDestNode(MixerNode *node) void Edge::adjust() { - if (!source || !dest) + if (!source || !dest) { return; + } QLineF line(mapFromItem(source, 0, 0), mapFromItem(dest, 0, 0)); qreal length = line.length(); @@ -83,7 +83,7 @@ void Edge::adjust() if (length > qreal(20.)) { QPointF edgeOffset((line.dx() * 13) / length, (line.dy() * 13) / length); sourcePoint = line.p1() + edgeOffset; - destPoint = line.p2() - edgeOffset; + destPoint = line.p2() - edgeOffset; } else { sourcePoint = destPoint = line.p1(); } @@ -91,29 +91,31 @@ void Edge::adjust() QRectF Edge::boundingRect() const { - if (!source || !dest) + if (!source || !dest) { return QRectF(); + } qreal penWidth = 1; - qreal extra = (penWidth + arrowSize) / 2.0; + qreal extra = (penWidth + arrowSize) / 2.0; return QRectF(sourcePoint, QSizeF(destPoint.x() - sourcePoint.x(), destPoint.y() - sourcePoint.y())) - .normalized() - .adjusted(-extra, -extra, extra, extra); + .normalized() + .adjusted(-extra, -extra, extra, extra); } void Edge::paint(QPainter *painter, const QStyleOptionGraphicsItem *, QWidget *) { - if (!source || !dest) + if (!source || !dest) { return; + } QLineF line(sourcePoint, destPoint); - if (qFuzzyCompare(line.length(), qreal(0.))) + if (qFuzzyCompare(line.length(), qreal(0.))) { return; + } // Draw the line itself painter->setPen(QPen(Qt::black, 1, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); painter->drawLine(line); - } diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.h b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.h index 1c4281c4a..7392ceff9 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.h +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurveline.h @@ -37,8 +37,7 @@ class MixerNode; -class Edge : public QGraphicsItem -{ +class Edge : public QGraphicsItem { public: Edge(MixerNode *sourceNode, MixerNode *destNode); ~Edge(); @@ -52,12 +51,15 @@ public: void adjust(); enum { Type = UserType + 12 }; - int type() const { return Type; } - + int type() const + { + return Type; + } + protected: QRectF boundingRect() const; void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget); - + private: MixerNode *source, *dest; @@ -67,4 +69,3 @@ private: }; #endif // MIXERCURVELINE_H - diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp index 2ad7d4b95..2a2dd2e1d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp @@ -43,14 +43,14 @@ MixerNode::MixerNode(MixerCurveWidget *graphWidget) setFlag(ItemSendsGeometryChanges); setCacheMode(DeviceCoordinateCache); setZValue(-1); - vertical = false; - drawNode = true; - drawText = true; + vertical = false; + drawNode = true; + drawText = true; - positiveColor = "#609FF2"; //blueish? - neutralColor = "#14CE24"; //greenish? - negativeColor = "#EF5F5F"; //redish? - disabledColor = "#dddddd"; + positiveColor = "#609FF2"; // blueish? + neutralColor = "#14CE24"; // greenish? + negativeColor = "#EF5F5F"; // redish? + disabledColor = "#dddddd"; disabledTextColor = "#aaaaaa"; } @@ -74,6 +74,7 @@ QRectF MixerNode::boundingRect() const QPainterPath MixerNode::shape() const { QPainterPath path; + path.addEllipse(boundingRect()); return path; } @@ -81,6 +82,7 @@ QPainterPath MixerNode::shape() const void MixerNode::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *) { QString text = QString().sprintf("%.2f", value()); + painter->setFont(graph->font()); if (drawNode) { QRadialGradient gradient(-3, -3, 10); @@ -88,11 +90,9 @@ void MixerNode::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QColor color; if (value() < 0) { color = negativeColor; - } - else if (value() == 0) { + } else if (value() == 0) { color = neutralColor; - } - else { + } else { color = positiveColor; } @@ -117,24 +117,27 @@ void MixerNode::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, } if (drawText) { - if(graph->isEnabled()) { + if (graph->isEnabled()) { painter->setPen(QPen(drawNode ? Qt::white : Qt::black, 0)); } else { painter->setPen(QPen(disabledTextColor)); } - painter->drawText( (value() < 0) ? -10 : -8, 3, text); + painter->drawText((value() < 0) ? -10 : -8, 3, text); } } -void MixerNode::verticalMove(bool flag){ +void MixerNode::verticalMove(bool flag) +{ vertical = flag; } -double MixerNode::value() { - double h = graph->sceneRect().height(); +double MixerNode::value() +{ + double h = graph->sceneRect().height(); double ratio = (h - pos().y()) / h; - return ((graph->getMax() - graph->getMin()) * ratio ) + graph->getMin(); + + return ((graph->getMax() - graph->getMin()) * ratio) + graph->getMin(); } @@ -144,26 +147,30 @@ QVariant MixerNode::itemChange(GraphicsItemChange change, const QVariant &val) double h = graph->sceneRect().height(); switch (change) { - case ItemPositionChange: { - - if (!vertical) - break; + case ItemPositionChange: + { + if (!vertical) { + break; + } // Force node to move vertically newPos.setX(pos().x()); // Stay inside graph - if (newPos.y() < 0) + if (newPos.y() < 0) { newPos.setY(0); - //qDebug() << h << " - " << newPos.y(); - if (newPos.y() > h) + } + // qDebug() << h << " - " << newPos.y(); + if (newPos.y() > h) { newPos.setY(h); + } - return newPos; + return newPos; } - case ItemPositionHasChanged: { - foreach (Edge *edge, edgeList) - edge->adjust(); + case ItemPositionHasChanged: + { + foreach(Edge * edge, edgeList) + edge->adjust(); update(); @@ -172,7 +179,8 @@ QVariant MixerNode::itemChange(GraphicsItemChange change, const QVariant &val) } default: break; - }; + } + ; return QGraphicsItem::itemChange(change, val); } diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp index 03e2be777..17815aca4 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp @@ -38,7 +38,6 @@ */ MixerCurveWidget::MixerCurveWidget(QWidget *parent) : QGraphicsView(parent) { - // Create a layout, add a QGraphicsView and put the SVG inside. // The Mixer Curve widget looks like this: // |--------------------| @@ -55,13 +54,13 @@ MixerCurveWidget::MixerCurveWidget(QWidget *parent) : QGraphicsView(parent) setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); setRenderHint(QPainter::Antialiasing); - curveMin=0.0; - curveMax=1.0; + curveMin = 0.0; + curveMax = 1.0; setFrameStyle(QFrame::NoFrame); setStyleSheet("background:transparent"); - QGraphicsScene *scene = new QGraphicsScene(this); + QGraphicsScene *scene = new QGraphicsScene(this); QSvgRenderer *renderer = new QSvgRenderer(); plot = new QGraphicsSvgItem(); renderer->load(QString(":/uavobjectwidgetutils/images/curve-bg.svg")); @@ -89,31 +88,31 @@ MixerCurveWidget::~MixerCurveWidget() void MixerCurveWidget::setPositiveColor(QString color) { - for (int i=0; i < nodeList.count(); i++) { - MixerNode* node = nodeList.at(i); + for (int i = 0; i < nodeList.count(); i++) { + MixerNode *node = nodeList.at(i); node->setPositiveColor(color); } } void MixerCurveWidget::setNegativeColor(QString color) { - for (int i=0; i < nodeList.count(); i++) { - MixerNode* node = nodeList.at(i); + for (int i = 0; i < nodeList.count(); i++) { + MixerNode *node = nodeList.at(i); node->setNegativeColor(color); } } /** - Init curve: create a (flat) curve with a specified number of points. + Init curve: create a (flat) curve with a specified number of points. - If a curve exists already, resets it. - Points should be between 0 and 1. - */ -void MixerCurveWidget::initCurve(const QList* points) + If a curve exists already, resets it. + Points should be between 0 and 1. + */ +void MixerCurveWidget::initCurve(const QList *points) { - if (points->length() < 2) + if (points->length() < 2) { return; // We need at least 2 points on a curve! - + } // finally, set node positions setCurve(points); } @@ -122,8 +121,8 @@ void MixerCurveWidget::initNodes(int numPoints) { // First of all, clear any existing list if (nodeList.count()) { - foreach (MixerNode *node, nodeList ) { - foreach(Edge *edge, node->edges()) { + foreach(MixerNode * node, nodeList) { + foreach(Edge * edge, node->edges()) { if (edge->sourceNode() == node) { scene()->removeItem(edge); delete edge; @@ -137,15 +136,14 @@ void MixerCurveWidget::initNodes(int numPoints) } // Create the nodes and edges - MixerNode* prevNode = 0; - for (int i=0; i < numPoints; i++) { - + MixerNode *prevNode = 0; + for (int i = 0; i < numPoints; i++) { MixerNode *node = new MixerNode(this); nodeList.append(node); scene()->addItem(node); - node->setPos(0,0); + node->setPos(0, 0); if (prevNode) { scene()->addItem(new Edge(prevNode, node)); @@ -156,57 +154,57 @@ void MixerCurveWidget::initNodes(int numPoints) } /** - Returns the current curve settings - */ -QList MixerCurveWidget::getCurve() { - + Returns the current curve settings + */ +QList MixerCurveWidget::getCurve() +{ QList list; - foreach(MixerNode *node, nodeList) { + foreach(MixerNode * node, nodeList) { list.append(node->value()); } return list; } /** - Sets a linear graph - */ + Sets a linear graph + */ void MixerCurveWidget::initLinearCurve(int numPoints, double maxValue, double minValue) { double range = maxValue - minValue; // setRange(minValue, maxValue); QList points; - for (double i=0; i < (double)numPoints; i++) { - double val = (range * ( i / (double)(numPoints-1) ) ) + minValue; + for (double i = 0; i < (double)numPoints; i++) { + double val = (range * (i / (double)(numPoints - 1))) + minValue; points.append(val); } initCurve(&points); } /** - Setd the current curve settings - */ -void MixerCurveWidget::setCurve(const QList* points) + Setd the current curve settings + */ +void MixerCurveWidget::setCurve(const QList *points) { curveUpdating = true; int ptCnt = points->count(); - if (nodeList.count() != ptCnt) + if (nodeList.count() != ptCnt) { initNodes(ptCnt); + } double range = curveMax - curveMin; - qreal w = plot->boundingRect().width()/(ptCnt-1); + qreal w = plot->boundingRect().width() / (ptCnt - 1); qreal h = plot->boundingRect().height(); - for (int i=0; iat(i) < curveMin) ? curveMin : (points->at(i) > curveMax) ? curveMax : points->at(i); val += range; val -= (curveMin + range); val /= range; - MixerNode* node = nodeList.at(i); - node->setPos(w*i, h - (val*h)); + MixerNode *node = nodeList.at(i); + node->setPos(w * i, h - (val * h)); node->verticalMove(true); node->update(); @@ -227,22 +225,23 @@ void MixerCurveWidget::showEvent(QShowEvent *event) // the result is usually a ahrsbargraph that is way too small. QRectF rect = plot->boundingRect(); - fitInView(rect.adjusted(-15,-15,15,15), Qt::KeepAspectRatio); + fitInView(rect.adjusted(-15, -15, 15, 15), Qt::KeepAspectRatio); } -void MixerCurveWidget::resizeEvent(QResizeEvent* event) +void MixerCurveWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); QRectF rect = plot->boundingRect(); - fitInView(rect.adjusted(-15,-15,15,15), Qt::KeepAspectRatio); + fitInView(rect.adjusted(-15, -15, 15, 15), Qt::KeepAspectRatio); } void MixerCurveWidget::changeEvent(QEvent *event) { QGraphicsView::changeEvent(event); - if(event->type() == QEvent::EnabledChange) { - foreach (MixerNode *node, nodeList) { + + if (event->type() == QEvent::EnabledChange) { + foreach(MixerNode * node, nodeList) { node->update(); } } @@ -259,16 +258,18 @@ void MixerCurveWidget::itemMoved(double itemValue) void MixerCurveWidget::setMin(double value) { - if (curveMin != value) + if (curveMin != value) { emit curveMinChanged(value); + } curveMin = value; } void MixerCurveWidget::setMax(double value) { - if (curveMax != value) + if (curveMax != value) { emit curveMaxChanged(value); + } curveMax = value; } @@ -287,4 +288,3 @@ double MixerCurveWidget::setRange(double min, double max) curveMax = max; return curveMax - curveMin; } - diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/popupwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/popupwidget.cpp index 94190cd09..727e1b0e3 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/popupwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/popupwidget.cpp @@ -4,15 +4,15 @@ PopupWidget::PopupWidget(QWidget *parent) : QDialog(parent) -{ +{ m_widget = 0; - QVBoxLayout* mainLayout = new QVBoxLayout(); + QVBoxLayout *mainLayout = new QVBoxLayout(); m_layout = new QHBoxLayout(); mainLayout->addLayout(m_layout); - QHBoxLayout* buttonLayout = new QHBoxLayout(); + QHBoxLayout *buttonLayout = new QHBoxLayout(); m_closeButton = new QPushButton(tr("Close")); buttonLayout->addWidget(m_closeButton); @@ -21,24 +21,24 @@ PopupWidget::PopupWidget(QWidget *parent) : setLayout(mainLayout); - connect(m_closeButton,SIGNAL(clicked()), this, SLOT(close())); - connect(this, SIGNAL(accepted()),this,SLOT(close())); - connect(this,SIGNAL(rejected()), this, SLOT(close())); + connect(m_closeButton, SIGNAL(clicked()), this, SLOT(close())); + connect(this, SIGNAL(accepted()), this, SLOT(close())); + connect(this, SIGNAL(rejected()), this, SLOT(close())); } -void PopupWidget::popUp(QWidget* widget) +void PopupWidget::popUp(QWidget *widget) { setWidget(widget); exec(); } -void PopupWidget::setWidget(QWidget* widget) +void PopupWidget::setWidget(QWidget *widget) { m_widget = widget; m_widgetParent = widget->parentWidget(); // save the current width,height so we can restore when closed - m_widgetWidth = m_widget->width(); + m_widgetWidth = m_widget->width(); m_widgetHeight = m_widget->height(); // double the size of the widget for the dialog @@ -63,8 +63,7 @@ void PopupWidget::done(int result) void PopupWidget::closePopup() { if (m_widget && m_widgetParent) { - if(QGroupBox * w =qobject_cast(m_widgetParent)) - { + if (QGroupBox * w = qobject_cast(m_widgetParent)) { m_widget->resize(m_widgetWidth, m_widgetHeight); w->layout()->addWidget(m_widget); } diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.cpp index 5bc29994b..20f26e7ed 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.cpp @@ -27,149 +27,147 @@ #include "smartsavebutton.h" smartSaveButton::smartSaveButton() -{ - -} +{} void smartSaveButton::addButtons(QPushButton *save, QPushButton *apply) { - buttonList.insert(save,save_button); - buttonList.insert(apply,apply_button); - connect(save,SIGNAL(clicked()),this,SLOT(processClick())); - connect(apply,SIGNAL(clicked()),this,SLOT(processClick())); + buttonList.insert(save, save_button); + buttonList.insert(apply, apply_button); + connect(save, SIGNAL(clicked()), this, SLOT(processClick())); + connect(apply, SIGNAL(clicked()), this, SLOT(processClick())); } void smartSaveButton::addApplyButton(QPushButton *apply) { - buttonList.insert(apply,apply_button); - connect(apply,SIGNAL(clicked()),this,SLOT(processClick())); + buttonList.insert(apply, apply_button); + connect(apply, SIGNAL(clicked()), this, SLOT(processClick())); } void smartSaveButton::addSaveButton(QPushButton *save) { - buttonList.insert(save,save_button); - connect(save,SIGNAL(clicked()),this,SLOT(processClick())); + buttonList.insert(save, save_button); + connect(save, SIGNAL(clicked()), this, SLOT(processClick())); } void smartSaveButton::processClick() { emit beginOp(); - bool save=false; - QPushButton *button=qobject_cast(sender()); - if(!button) - return; - if(buttonList.value(button)==save_button) - save=true; - processOperation(button,save); + bool save = false; + QPushButton *button = qobject_cast(sender()); + if (!button) { + return; + } + if (buttonList.value(button) == save_button) { + save = true; + } + processOperation(button, save); } -void smartSaveButton::processOperation(QPushButton * button,bool save) +void smartSaveButton::processOperation(QPushButton *button, bool save) { emit preProcessOperations(); - if(button) - { + + if (button) { button->setEnabled(false); button->setIcon(QIcon(":/uploader/images/system-run.svg")); } QTimer timer; timer.setSingleShot(true); - bool error=false; + bool error = false; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectUtilManager* utilMngr = pm->getObject(); - foreach(UAVDataObject * obj,objects) - { - UAVObject::Metadata mdata= obj->getMetadata(); - if(UAVObject::GetGcsAccess(mdata)==UAVObject::ACCESS_READONLY) + UAVObjectUtilManager *utilMngr = pm->getObject(); + foreach(UAVDataObject * obj, objects) { + UAVObject::Metadata mdata = obj->getMetadata(); + + if (UAVObject::GetGcsAccess(mdata) == UAVObject::ACCESS_READONLY) { continue; - up_result=false; - current_object=obj; - for(int i=0;i<3;++i) - { - qDebug()<<"SMARTSAVEBUTTON"<<"Upload try number"<getName(); - connect(obj,SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(transaction_finished(UAVObject*, bool))); - connect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); + } + up_result = false; + current_object = obj; + for (int i = 0; i < 3; ++i) { + qDebug() << "SMARTSAVEBUTTON" << "Upload try number" << i << "Object" << obj->getName(); + connect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transaction_finished(UAVObject *, bool))); + connect(&timer, SIGNAL(timeout()), &loop, SLOT(quit())); obj->updated(); timer.start(3000); - //qDebug()<<"begin loop"; + // qDebug()<<"begin loop"; loop.exec(); - if(timer.isActive()) - { - qDebug()<<"SMARTSAVEBUTTON"<<"Upload did not timeout"<getName(); + if (timer.isActive()) { + qDebug() << "SMARTSAVEBUTTON" << "Upload did not timeout" << i << "Object" << obj->getName(); + } else { + qDebug() << "SMARTSAVEBUTTON" << "Upload TIMEOUT" << i << "Object" << obj->getName(); } - else - qDebug()<<"SMARTSAVEBUTTON"<<"Upload TIMEOUT"<getName(); timer.stop(); - disconnect(obj,SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(transaction_finished(UAVObject*, bool))); - disconnect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); - if(up_result) + disconnect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transaction_finished(UAVObject *, bool))); + disconnect(&timer, SIGNAL(timeout()), &loop, SLOT(quit())); + if (up_result) { break; + } } - if(up_result==false) - { - qDebug()<<"SMARTSAVEBUTTON"<<"Object upload error:"<getName(); - error=true; + if (up_result == false) { + qDebug() << "SMARTSAVEBUTTON" << "Object upload error:" << obj->getName(); + error = true; continue; } - sv_result=false; - current_objectID=obj->getObjID(); - if(save && (obj->isSettings())) - { - for(int i=0;i<3;++i) - { - qDebug()<<"SMARTSAVEBUTTON"<<"Save try number"<getName(); - connect(utilMngr,SIGNAL(saveCompleted(int,bool)),this,SLOT(saving_finished(int,bool))); - connect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); + sv_result = false; + current_objectID = obj->getObjID(); + if (save && (obj->isSettings())) { + for (int i = 0; i < 3; ++i) { + qDebug() << "SMARTSAVEBUTTON" << "Save try number" << i << "Object" << obj->getName(); + connect(utilMngr, SIGNAL(saveCompleted(int, bool)), this, SLOT(saving_finished(int, bool))); + connect(&timer, SIGNAL(timeout()), &loop, SLOT(quit())); utilMngr->saveObjectToSD(obj); timer.start(3000); loop.exec(); - if(timer.isActive()) - { - qDebug()<<"SMARTSAVEBUTTON"<<"Saving did not timeout"<getName(); + if (timer.isActive()) { + qDebug() << "SMARTSAVEBUTTON" << "Saving did not timeout" << i << "Object" << obj->getName(); + } else { + qDebug() << "SMARTSAVEBUTTON" << "Saving TIMEOUT" << i << "Object" << obj->getName(); } - else - qDebug()<<"SMARTSAVEBUTTON"<<"Saving TIMEOUT"<getName(); timer.stop(); - disconnect(utilMngr,SIGNAL(saveCompleted(int,bool)),this,SLOT(saving_finished(int,bool))); - disconnect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); - if(sv_result) + disconnect(utilMngr, SIGNAL(saveCompleted(int, bool)), this, SLOT(saving_finished(int, bool))); + disconnect(&timer, SIGNAL(timeout()), &loop, SLOT(quit())); + if (sv_result) { break; + } } - if(sv_result==false) - { - qDebug()<<"SMARTSAVEBUTTON"<<"failed to save:"<getName(); - error=true; + if (sv_result == false) { + qDebug() << "SMARTSAVEBUTTON" << "failed to save:" << obj->getName(); + error = true; } } } - if(button) + if (button) { button->setEnabled(true); - if(!error) - { - if(button) - button->setIcon(QIcon(":/uploader/images/dialog-apply.svg")); - emit saveSuccessfull(); } - else - { - if(button) + if (!error) { + if (button) { + button->setIcon(QIcon(":/uploader/images/dialog-apply.svg")); + } + emit saveSuccessfull(); + } else { + if (button) { button->setIcon(QIcon(":/uploader/images/process-stop.svg")); + } } emit endOp(); } void smartSaveButton::setObjects(QList list) { - objects=list; + objects = list; } -void smartSaveButton::addObject(UAVDataObject * obj) +void smartSaveButton::addObject(UAVDataObject *obj) { Q_ASSERT(obj); - if(!objects.contains(obj)) + if (!objects.contains(obj)) { objects.append(obj); + } } -void smartSaveButton::removeObject(UAVDataObject * obj) +void smartSaveButton::removeObject(UAVDataObject *obj) { - if(objects.contains(obj)) + if (objects.contains(obj)) { objects.removeAll(obj); + } } void smartSaveButton::removeAllObjects() { @@ -179,45 +177,41 @@ void smartSaveButton::clearObjects() { objects.clear(); } -void smartSaveButton::transaction_finished(UAVObject* obj, bool result) +void smartSaveButton::transaction_finished(UAVObject *obj, bool result) { - if(current_object==obj) - { - up_result=result; + if (current_object == obj) { + up_result = result; loop.quit(); } } void smartSaveButton::saving_finished(int id, bool result) { - if(id==current_objectID) - { - sv_result=result; - //qDebug()<<"saving_finished result="< #include -class UAVOBJECTWIDGETUTILS_EXPORT UAVObjectWidgetUtilsPlugin: public ExtensionSystem::IPlugin -{ +class UAVOBJECTWIDGETUTILS_EXPORT UAVObjectWidgetUtilsPlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -40,7 +39,7 @@ public: ~UAVObjectWidgetUtilsPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); }; diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.cpp b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.cpp index eb45634ff..6a35d30cb 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.cpp +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.cpp @@ -26,28 +26,27 @@ */ #include "importsummary.h" -ImportSummaryDialog::ImportSummaryDialog( QWidget *parent) : +ImportSummaryDialog::ImportSummaryDialog(QWidget *parent) : QDialog(parent), ui(new Ui::ImportSummaryDialog) { - ui->setupUi(this); - setWindowTitle(tr("Import Summary")); + ui->setupUi(this); + setWindowTitle(tr("Import Summary")); - ui->importSummaryList->setColumnCount(3); - ui->importSummaryList->setRowCount(0); - QStringList header; - header.append("Save"); - header.append("Name"); - header.append("Status"); - ui->importSummaryList->setHorizontalHeaderLabels(header); - ui->progressBar->setValue(0); + ui->importSummaryList->setColumnCount(3); + ui->importSummaryList->setRowCount(0); + QStringList header; + header.append("Save"); + header.append("Name"); + header.append("Status"); + ui->importSummaryList->setHorizontalHeaderLabels(header); + ui->progressBar->setValue(0); - connect( ui->closeButton, SIGNAL(clicked()), this, SLOT(close())); - connect(ui->saveToFlash, SIGNAL(clicked()), this, SLOT(doTheSaving())); - - // Connect the help button - connect(ui->helpButton, SIGNAL(clicked()), this, SLOT(openHelp())); + connect(ui->closeButton, SIGNAL(clicked()), this, SLOT(close())); + connect(ui->saveToFlash, SIGNAL(clicked()), this, SLOT(doTheSaving())); + // Connect the help button + connect(ui->helpButton, SIGNAL(clicked()), this, SLOT(openHelp())); } ImportSummaryDialog::~ImportSummaryDialog() @@ -56,30 +55,30 @@ ImportSummaryDialog::~ImportSummaryDialog() } /* - Open the right page on the wiki - */ + Open the right page on the wiki + */ void ImportSummaryDialog::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/UAV+Settings+import-export", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/display/Doc/UAV+Settings+import-export", QUrl::StrictMode)); } /* - Adds a new line about a UAVObject along with its status - (whether it got saved OK or not) - */ + Adds a new line about a UAVObject along with its status + (whether it got saved OK or not) + */ void ImportSummaryDialog::addLine(QString uavObjectName, QString text, bool status) { - ui->importSummaryList->setRowCount(ui->importSummaryList->rowCount()+1); - int row = ui->importSummaryList->rowCount()-1; - ui->importSummaryList->setCellWidget(row,0,new QCheckBox(ui->importSummaryList)); + ui->importSummaryList->setRowCount(ui->importSummaryList->rowCount() + 1); + int row = ui->importSummaryList->rowCount() - 1; + ui->importSummaryList->setCellWidget(row, 0, new QCheckBox(ui->importSummaryList)); QTableWidgetItem *objName = new QTableWidgetItem(uavObjectName); ui->importSummaryList->setItem(row, 1, objName); - QCheckBox *box = dynamic_cast(ui->importSummaryList->cellWidget(row,0)); - ui->importSummaryList->setItem(row,2,new QTableWidgetItem(text)); + QCheckBox *box = dynamic_cast(ui->importSummaryList->cellWidget(row, 0)); + ui->importSummaryList->setItem(row, 2, new QTableWidgetItem(text)); - //Disable editability and selectability in table elements - ui->importSummaryList->item(row,1)->setFlags(!Qt::ItemIsEditable); - ui->importSummaryList->item(row,2)->setFlags(!Qt::ItemIsEditable); + // Disable editability and selectability in table elements + ui->importSummaryList->item(row, 1)->setFlags(!Qt::ItemIsEditable); + ui->importSummaryList->item(row, 2)->setFlags(!Qt::ItemIsEditable); if (status) { box->setChecked(true); @@ -88,36 +87,38 @@ void ImportSummaryDialog::addLine(QString uavObjectName, QString text, bool stat box->setEnabled(false); } - this->repaint(); - this->showEvent(NULL); + this->repaint(); + this->showEvent(NULL); } /* - Saves every checked UAVObjet in the list to Flash - */ + Saves every checked UAVObjet in the list to Flash + */ void ImportSummaryDialog::doTheSaving() { - int itemCount=0; + int itemCount = 0; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVObjectUtilManager *utilManager = pm->getObject(); - connect(utilManager, SIGNAL(saveCompleted(int,bool)), this, SLOT(updateSaveCompletion())); + UAVObjectUtilManager *utilManager = pm->getObject(); - for(int i=0; i < ui->importSummaryList->rowCount(); i++) { - QCheckBox *box = dynamic_cast(ui->importSummaryList->cellWidget(i,0)); + connect(utilManager, SIGNAL(saveCompleted(int, bool)), this, SLOT(updateSaveCompletion())); + + for (int i = 0; i < ui->importSummaryList->rowCount(); i++) { + QCheckBox *box = dynamic_cast(ui->importSummaryList->cellWidget(i, 0)); if (box->isChecked()) { - ++itemCount; + ++itemCount; } } - if(itemCount==0) + if (itemCount == 0) { return; - ui->progressBar->setMaximum(itemCount+1); + } + ui->progressBar->setMaximum(itemCount + 1); ui->progressBar->setValue(1); - for(int i=0; i < ui->importSummaryList->rowCount(); i++) { - QString uavObjectName = ui->importSummaryList->item(i,1)->text(); - QCheckBox *box = dynamic_cast(ui->importSummaryList->cellWidget(i,0)); + for (int i = 0; i < ui->importSummaryList->rowCount(); i++) { + QString uavObjectName = ui->importSummaryList->item(i, 1)->text(); + QCheckBox *box = dynamic_cast(ui->importSummaryList->cellWidget(i, 0)); if (box->isChecked()) { - UAVObject* obj = objManager->getObject(uavObjectName); + UAVObject *obj = objManager->getObject(uavObjectName); utilManager->saveObjectToSD(obj); this->repaint(); } @@ -125,15 +126,13 @@ void ImportSummaryDialog::doTheSaving() ui->saveToFlash->setEnabled(false); ui->closeButton->setEnabled(false); - } void ImportSummaryDialog::updateSaveCompletion() { - ui->progressBar->setValue(ui->progressBar->value()+1); - if(ui->progressBar->value()==ui->progressBar->maximum()) - { + ui->progressBar->setValue(ui->progressBar->value() + 1); + if (ui->progressBar->value() == ui->progressBar->maximum()) { ui->saveToFlash->setEnabled(true); ui->closeButton->setEnabled(true); } @@ -142,6 +141,7 @@ void ImportSummaryDialog::updateSaveCompletion() void ImportSummaryDialog::changeEvent(QEvent *e) { QDialog::changeEvent(e); + switch (e->type()) { case QEvent::LanguageChange: ui->retranslateUi(this); @@ -155,8 +155,7 @@ void ImportSummaryDialog::showEvent(QShowEvent *event) { Q_UNUSED(event) ui->importSummaryList->resizeColumnsToContents(); - int width = ui->importSummaryList->width()-(ui->importSummaryList->columnWidth(0)+ - ui->importSummaryList->columnWidth(2)); - ui->importSummaryList->setColumnWidth(1,width-15); + int width = ui->importSummaryList->width() - (ui->importSummaryList->columnWidth(0) + + ui->importSummaryList->columnWidth(2)); + ui->importSummaryList->setColumnWidth(1, width - 15); } - diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.h b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.h index ab88eb835..524897c5b 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.h +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/importsummary.h @@ -38,17 +38,15 @@ #include "uavobjectutil/uavobjectutilmanager.h" - namespace Ui { - class ImportSummaryDialog; +class ImportSummaryDialog; } -class ImportSummaryDialog : public QDialog -{ +class ImportSummaryDialog : public QDialog { Q_OBJECT public: - ImportSummaryDialog(QWidget *parent=0); + ImportSummaryDialog(QWidget *parent = 0); ~ImportSummaryDialog(); void addLine(QString objectName, QString text, bool status); @@ -65,7 +63,6 @@ public slots: private slots: void doTheSaving(); void openHelp(); - }; #endif // IMPORTSUMMARY_H diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.cpp b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.cpp index acfcc7076..6f20de57c 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.cpp +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.cpp @@ -10,25 +10,25 @@ * @brief UAVSettings Import/Export Plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavsettingsimportexport.h" -#include -#include +#include +#include #include #include #include "importsummary.h" @@ -50,18 +50,18 @@ #include #include -UAVSettingsImportExportPlugin::UAVSettingsImportExportPlugin() -{ - // Do nothing -} +UAVSettingsImportExportPlugin::UAVSettingsImportExportPlugin() +{ + // Do nothing +} -UAVSettingsImportExportPlugin::~UAVSettingsImportExportPlugin() -{ - // Do nothing -} +UAVSettingsImportExportPlugin::~UAVSettingsImportExportPlugin() +{ + // Do nothing +} -bool UAVSettingsImportExportPlugin::initialize(const QStringList& args, QString *errMsg) -{ +bool UAVSettingsImportExportPlugin::initialize(const QStringList & args, QString *errMsg) +{ Q_UNUSED(args); Q_UNUSED(errMsg); mf = new UAVSettingsImportExportFactory(this); @@ -70,12 +70,9 @@ bool UAVSettingsImportExportPlugin::initialize(const QStringList& args, QString } void UAVSettingsImportExportPlugin::shutdown() -{ - // Do nothing +{ + // Do nothing } void UAVSettingsImportExportPlugin::extensionsInitialized() -{ -} +{} Q_EXPORT_PLUGIN(UAVSettingsImportExportPlugin) - - diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.h b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.h index 3cc77a9fc..685638c30 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.h +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.h @@ -10,44 +10,40 @@ * @brief UAVSettings Import/Export Plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef UAVSETTINGSIMPORTEXPORT_H -#define UAVSETTINGSIMPORTEXPORT_H +#ifndef UAVSETTINGSIMPORTEXPORT_H +#define UAVSETTINGSIMPORTEXPORT_H -#include +#include #include "uavobjectutil/uavobjectutilmanager.h" #include "uavsettingsimportexport_global.h" #include "../../gcs_version_info.h" #include "uavsettingsimportexportfactory.h" -class UAVSETTINGSIMPORTEXPORT_EXPORT UAVSettingsImportExportPlugin : public ExtensionSystem::IPlugin -{ +class UAVSETTINGSIMPORTEXPORT_EXPORT UAVSettingsImportExportPlugin : public ExtensionSystem::IPlugin { Q_OBJECT -public: - UAVSettingsImportExportPlugin(); - ~UAVSettingsImportExportPlugin(); +public: + UAVSettingsImportExportPlugin(); + ~UAVSettingsImportExportPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - UAVSettingsImportExportFactory *mf; - - - -}; + UAVSettingsImportExportFactory *mf; +}; #endif // UAVSETTINGSIMPORTEXPORT_H diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.cpp b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.cpp index 01557a4a1..d783d6c2c 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.cpp +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.cpp @@ -52,19 +52,19 @@ UAVSettingsImportExportFactory::~UAVSettingsImportExportFactory() { - // Do nothing + // Do nothing } -UAVSettingsImportExportFactory::UAVSettingsImportExportFactory(QObject *parent):QObject(parent) +UAVSettingsImportExportFactory::UAVSettingsImportExportFactory(QObject *parent) : QObject(parent) { + // Add Menu entry + Core::ActionManager *am = Core::ICore::instance()->actionManager(); + Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_FILE); + Core::Command *cmd = am->registerAction(new QAction(this), + "UAVSettingsImportExportPlugin.UAVSettingsExport", + QList() << + Core::Constants::C_GLOBAL_ID); - // Add Menu entry - Core::ActionManager *am = Core::ICore::instance()->actionManager(); - Core::ActionContainer *ac = am->actionContainer(Core::Constants::M_FILE); - Core::Command *cmd = am->registerAction(new QAction(this), - "UAVSettingsImportExportPlugin.UAVSettingsExport", - QList() << - Core::Constants::C_GLOBAL_ID); cmd->setDefaultKeySequence(QKeySequence("Ctrl+E")); cmd->action()->setText(tr("Export UAV Settings...")); ac->addAction(cmd, Core::Constants::G_FILE_SAVE); @@ -79,7 +79,7 @@ UAVSettingsImportExportFactory::UAVSettingsImportExportFactory(QObject *parent): ac->addAction(cmd, Core::Constants::G_FILE_SAVE); connect(cmd->action(), SIGNAL(triggered(bool)), this, SLOT(importUAVSettings())); - ac = am->actionContainer(Core::Constants::M_HELP); + ac = am->actionContainer(Core::Constants::M_HELP); cmd = am->registerAction(new QAction(this), "UAVSettingsImportExportPlugin.UAVDataExport", QList() << @@ -95,6 +95,7 @@ void UAVSettingsImportExportFactory::importUAVSettings() // ask for file name QString fileName; QString filters = tr("UAVObjects XML files (*.uav);; XML files (*.xml)"); + fileName = QFileDialog::getOpenFileName(0, tr("Import UAV Settings"), "", filters); if (fileName.isEmpty()) { return; @@ -103,7 +104,7 @@ void UAVSettingsImportExportFactory::importUAVSettings() // Now open the file QFile file(fileName); QDomDocument doc("UAVObjects"); - file.open(QFile::ReadOnly|QFile::Text); + file.open(QFile::ReadOnly | QFile::Text); if (!doc.setContent(file.readAll())) { QMessageBox msgBox; msgBox.setText(tr("File Parsing Failed.")); @@ -116,7 +117,7 @@ void UAVSettingsImportExportFactory::importUAVSettings() // find the root of settings subtree emit importAboutToBegin(); - qDebug()<<"Import about to begin"; + qDebug() << "Import about to begin"; QDomElement root = doc.documentElement(); if (root.tagName() == "uavobjects") { @@ -133,7 +134,7 @@ void UAVSettingsImportExportFactory::importUAVSettings() // We are now ok: setup the import summary dialog & update it as we // go along. - ImportSummaryDialog swui((QWidget*)Core::ICore::instance()->mainWindow()); + ImportSummaryDialog swui((QWidget *)Core::ICore::instance()->mainWindow()); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); @@ -143,24 +144,23 @@ void UAVSettingsImportExportFactory::importUAVSettings() while (!node.isNull()) { QDomElement e = node.toElement(); if (e.tagName() == "object") { - - // - Read each object - QString uavObjectName = e.attribute("name"); - uint uavObjectID = e.attribute("id").toUInt(NULL,16); + // - Read each object + QString uavObjectName = e.attribute("name"); + uint uavObjectID = e.attribute("id").toUInt(NULL, 16); // Sanity Check: - UAVObject *obj = objManager->getObject(uavObjectName); + UAVObject *obj = objManager->getObject(uavObjectName); if (obj == NULL) { // This object is unknown! qDebug() << "Object unknown:" << uavObjectName << uavObjectID; swui.addLine(uavObjectName, "Error (Object unknown)", false); } else { - // - Update each field - // - Issue and "updated" command - bool error = false; - bool setError = false; + // - Update each field + // - Issue and "updated" command + bool error = false; + bool setError = false; QDomNode field = node.firstChild(); - while(!field.isNull()) { + while (!field.isNull()) { QDomElement f = field.toElement(); if (f.tagName() == "field") { UAVObjectField *uavfield = obj->getField(f.attribute("name")); @@ -177,12 +177,12 @@ void UAVSettingsImportExportFactory::importUAVSettings() // This is an enum: int i = 0; QStringList list = f.attribute("values").split(","); - foreach (QString element, list) { + foreach(QString element, list) { if (false == uavfield->checkValue(element, i)) { qDebug() << "checkValue(list) returned false on: " << uavObjectName << list; setError = true; } else { - uavfield->setValue(element,i); + uavfield->setValue(element, i); } i++; } @@ -223,6 +223,7 @@ QString UAVSettingsImportExportFactory::createXMLDocument(const enum storedData // create an XML root QDomDocument doc("UAVObjects"); QDomElement root = doc.createElement("uavobjects"); + doc.appendChild(root); // add hardware, firmware and GCS version info @@ -230,7 +231,7 @@ QString UAVSettingsImportExportFactory::createXMLDocument(const enum storedData root.appendChild(versionInfo); UAVObjectUtilManager *utilMngr = pm->getObject(); - deviceDescriptorStruct board = utilMngr->getBoardDescriptionStruct(); + deviceDescriptorStruct board = utilMngr->getBoardDescriptionStruct(); QDomElement hw = doc.createElement("hardware"); hw.setAttribute("type", QString().setNum(board.boardType, 16)); @@ -245,11 +246,11 @@ QString UAVSettingsImportExportFactory::createXMLDocument(const enum storedData versionInfo.appendChild(fw); QString gcsRevision = QString::fromLatin1(Core::Constants::GCS_REVISION_STR); - QString gcsGitDate = gcsRevision.mid(gcsRevision.indexOf(" ") + 1, 14); - QString gcsGitHash = gcsRevision.mid(gcsRevision.indexOf(":") + 1, 8); - QString gcsGitTag = gcsRevision.left(gcsRevision.indexOf(":")); + QString gcsGitDate = gcsRevision.mid(gcsRevision.indexOf(" ") + 1, 14); + QString gcsGitHash = gcsRevision.mid(gcsRevision.indexOf(":") + 1, 8); + QString gcsGitTag = gcsRevision.left(gcsRevision.indexOf(":")); - QDomElement gcs = doc.createElement("gcs"); + QDomElement gcs = doc.createElement("gcs"); gcs.setAttribute("date", gcsGitDate); gcs.setAttribute("hash", gcsGitHash); gcs.setAttribute("tag", gcsGitTag); @@ -257,10 +258,9 @@ QString UAVSettingsImportExportFactory::createXMLDocument(const enum storedData // create settings and/or data elements QDomElement settings = doc.createElement("settings"); - QDomElement data = doc.createElement("data"); + QDomElement data = doc.createElement("data"); - switch (what) - { + switch (what) { case Settings: root.appendChild(settings); break; @@ -274,33 +274,33 @@ QString UAVSettingsImportExportFactory::createXMLDocument(const enum storedData } // iterate over settings objects - QList< QList > objList = objManager->getDataObjects(); - foreach (QList list, objList) { - foreach (UAVDataObject *obj, list) { + QList< QList > objList = objManager->getDataObjects(); + foreach(QList list, objList) { + foreach(UAVDataObject * obj, list) { if (((what == Settings) && obj->isSettings()) || - ((what == Data) && !obj->isSettings()) || - (what == Both)) { - + ((what == Data) && !obj->isSettings()) || + (what == Both)) { // add each object to the XML QDomElement o = doc.createElement("object"); o.setAttribute("name", obj->getName()); - o.setAttribute("id", QString("0x")+ QString().setNum(obj->getObjID(),16).toUpper()); + o.setAttribute("id", QString("0x") + QString().setNum(obj->getObjID(), 16).toUpper()); if (fullExport) { QDomElement d = doc.createElement("description"); - QDomText t = doc.createTextNode(obj->getDescription().remove("@Ref ", Qt::CaseInsensitive)); + QDomText t = doc.createTextNode(obj->getDescription().remove("@Ref ", Qt::CaseInsensitive)); d.appendChild(t); o.appendChild(d); } // iterate over fields - QList fieldList = obj->getFields(); + QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { + foreach(UAVObjectField * field, fieldList) { QDomElement f = doc.createElement("field"); // iterate over values QString vals; quint32 nelem = field->getNumElements(); + for (unsigned int n = 0; n < nelem; ++n) { vals.append(QString("%1,").arg(field->getValue(n).toString())); } @@ -320,10 +320,11 @@ QString UAVSettingsImportExportFactory::createXMLDocument(const enum storedData } // append to the settings or data element - if (obj->isSettings()) + if (obj->isSettings()) { settings.appendChild(o); - else + } else { data.appendChild(o); + } } } } @@ -357,7 +358,7 @@ void UAVSettingsImportExportFactory::exportUAVSettings() // save file QFile file(fileName); if (file.open(QIODevice::WriteOnly) && - (file.write(xml.toAscii()) != -1)) { + (file.write(xml.toAscii()) != -1)) { file.close(); } else { QMessageBox::critical(0, @@ -377,11 +378,11 @@ void UAVSettingsImportExportFactory::exportUAVSettings() void UAVSettingsImportExportFactory::exportUAVData() { if (QMessageBox::question(0, tr("Are you sure?"), - tr("This option is only useful for passing your current " - "system data to the technical support staff. " - "Do you really want to export?"), - QMessageBox::Ok | QMessageBox::Cancel, - QMessageBox::Ok) != QMessageBox::Ok) { + tr("This option is only useful for passing your current " + "system data to the technical support staff. " + "Do you really want to export?"), + QMessageBox::Ok | QMessageBox::Cancel, + QMessageBox::Ok) != QMessageBox::Ok) { return; } @@ -408,7 +409,7 @@ void UAVSettingsImportExportFactory::exportUAVData() // save file QFile file(fileName); if (file.open(QIODevice::WriteOnly) && - (file.write(xml.toAscii()) != -1)) { + (file.write(xml.toAscii()) != -1)) { file.close(); } else { QMessageBox::critical(0, diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.h b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.h index aa07d62e1..f49aaf6f0 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.h +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexportfactory.h @@ -29,8 +29,7 @@ #include "uavsettingsimportexport_global.h" #include "uavobjectutil/uavobjectutilmanager.h" #include "../../gcs_version_info.h" -class UAVSETTINGSIMPORTEXPORT_EXPORT UAVSettingsImportExportFactory : public QObject -{ +class UAVSETTINGSIMPORTEXPORT_EXPORT UAVSettingsImportExportFactory : public QObject { Q_OBJECT public: @@ -48,7 +47,6 @@ private slots: signals: void importAboutToBegin(); void importEnded(); - }; #endif // UAVSETTINGSIMPORTEXPORTFACTORY_H diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp index 80214ae13..bc7cfc6a3 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp @@ -10,18 +10,18 @@ * @brief The UAVTalk protocol plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,22 +37,21 @@ /** * Constructor */ -Telemetry::Telemetry(UAVTalk* utalk, UAVObjectManager* objMngr) +Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr) { - this->utalk = utalk; + this->utalk = utalk; this->objMngr = objMngr; mutex = new QMutex(QMutex::Recursive); // Process all objects in the list - QList< QList > objs = objMngr->getObjects(); - for (int objidx = 0; objidx < objs.length(); ++objidx) - { + QList< QList > objs = objMngr->getObjects(); + for (int objidx = 0; objidx < objs.length(); ++objidx) { registerObject(objs[objidx][0]); // we only need to register one instance per object type } // Listen to new object creations - connect(objMngr, SIGNAL(newObject(UAVObject*)), this, SLOT(newObject(UAVObject*))); - connect(objMngr, SIGNAL(newInstance(UAVObject*)), this, SLOT(newInstance(UAVObject*))); + connect(objMngr, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *))); + connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *))); // Listen to transaction completions - connect(utalk, SIGNAL(transactionCompleted(UAVObject*,bool)), this, SLOT(transactionCompleted(UAVObject*,bool))); + connect(utalk, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool))); // Get GCS stats object gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr); // Setup and start the periodic timer @@ -61,20 +60,21 @@ Telemetry::Telemetry(UAVTalk* utalk, UAVObjectManager* objMngr) connect(updateTimer, SIGNAL(timeout()), this, SLOT(processPeriodicUpdates())); updateTimer->start(1000); // Setup and start the stats timer - txErrors = 0; + txErrors = 0; txRetries = 0; } Telemetry::~Telemetry() { - for (QMap::iterator itr = transMap.begin(); itr != transMap.end(); ++itr) + for (QMap::iterator itr = transMap.begin(); itr != transMap.end(); ++itr) { delete itr.value(); + } } /** * Register a new object for periodic updates (if enabled) */ -void Telemetry::registerObject(UAVObject* obj) +void Telemetry::registerObject(UAVObject *obj) { // Setup object for periodic updates addObject(obj); @@ -86,13 +86,11 @@ void Telemetry::registerObject(UAVObject* obj) /** * Add an object in the list used for periodic updates */ -void Telemetry::addObject(UAVObject* obj) +void Telemetry::addObject(UAVObject *obj) { // Check if object type is already in the list - for (int n = 0; n < objList.length(); ++n) - { - if ( objList[n].obj->getObjID() == obj->getObjID() ) - { + for (int n = 0; n < objList.length(); ++n) { + if (objList[n].obj->getObjID() == obj->getObjID()) { // Object type (not instance!) is already in the list, do nothing return; } @@ -102,21 +100,19 @@ void Telemetry::addObject(UAVObject* obj) ObjectTimeInfo timeInfo; timeInfo.obj = obj; timeInfo.timeToNextUpdateMs = 0; - timeInfo.updatePeriodMs = 0; + timeInfo.updatePeriodMs = 0; objList.append(timeInfo); } /** * Update the object's timers */ -void Telemetry::setUpdatePeriod(UAVObject* obj, qint32 periodMs) +void Telemetry::setUpdatePeriod(UAVObject *obj, qint32 periodMs) { // Find object type (not instance!) and update its period - for (int n = 0; n < objList.length(); ++n) - { - if ( objList[n].obj->getObjID() == obj->getObjID() ) - { - objList[n].updatePeriodMs = periodMs; + for (int n = 0; n < objList.length(); ++n) { + if (objList[n].obj->getObjID() == obj->getObjID()) { + objList[n].updatePeriodMs = periodMs; objList[n].timeToNextUpdateMs = quint32((float)periodMs * (float)qrand() / (float)RAND_MAX); // avoid bunching of updates } } @@ -125,33 +121,27 @@ void Telemetry::setUpdatePeriod(UAVObject* obj, qint32 periodMs) /** * Connect to all instances of an object depending on the event mask specified */ -void Telemetry::connectToObjectInstances(UAVObject* obj, quint32 eventMask) +void Telemetry::connectToObjectInstances(UAVObject *obj, quint32 eventMask) { - QList objs = objMngr->getObjectInstances(obj->getObjID()); - for (int n = 0; n < objs.length(); ++n) - { + QList objs = objMngr->getObjectInstances(obj->getObjID()); + for (int n = 0; n < objs.length(); ++n) { // Disconnect all objs[n]->disconnect(this); // Connect only the selected events - if ( (eventMask&EV_UNPACKED) != 0) - { - connect(objs[n], SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(objectUnpacked(UAVObject*))); + if ((eventMask & EV_UNPACKED) != 0) { + connect(objs[n], SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *))); } - if ( (eventMask&EV_UPDATED) != 0) - { - connect(objs[n], SIGNAL(objectUpdatedAuto(UAVObject*)), this, SLOT(objectUpdatedAuto(UAVObject*))); + if ((eventMask & EV_UPDATED) != 0) { + connect(objs[n], SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *))); } - if ( (eventMask&EV_UPDATED_MANUAL) != 0) - { - connect(objs[n], SIGNAL(objectUpdatedManual(UAVObject*)), this, SLOT(objectUpdatedManual(UAVObject*))); + if ((eventMask & EV_UPDATED_MANUAL) != 0) { + connect(objs[n], SIGNAL(objectUpdatedManual(UAVObject *)), this, SLOT(objectUpdatedManual(UAVObject *))); } - if ( (eventMask&EV_UPDATED_PERIODIC) != 0) - { - connect(objs[n], SIGNAL(objectUpdatedPeriodic(UAVObject*)), this, SLOT(objectUpdatedPeriodic(UAVObject*))); + if ((eventMask & EV_UPDATED_PERIODIC) != 0) { + connect(objs[n], SIGNAL(objectUpdatedPeriodic(UAVObject *)), this, SLOT(objectUpdatedPeriodic(UAVObject *))); } - if ( (eventMask&EV_UPDATE_REQ) != 0) - { - connect(objs[n], SIGNAL(updateRequested(UAVObject*)), this, SLOT(updateRequested(UAVObject*))); + if ((eventMask & EV_UPDATE_REQ) != 0) { + connect(objs[n], SIGNAL(updateRequested(UAVObject *)), this, SLOT(updateRequested(UAVObject *))); } } } @@ -159,68 +149,57 @@ void Telemetry::connectToObjectInstances(UAVObject* obj, quint32 eventMask) /** * Update an object based on its metadata properties */ -void Telemetry::updateObject(UAVObject* obj, quint32 eventType) +void Telemetry::updateObject(UAVObject *obj, quint32 eventType) { // Get metadata - UAVObject::Metadata metadata = obj->getMetadata(); + UAVObject::Metadata metadata = obj->getMetadata(); UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata); // Setup object depending on update mode qint32 eventMask; - if ( updateMode == UAVObject::UPDATEMODE_PERIODIC ) - { + + if (updateMode == UAVObject::UPDATEMODE_PERIODIC) { // Set update period setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod); // Connect signals for all instances eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC; - if( dynamic_cast(obj) != NULL ) - { + if (dynamic_cast(obj) != NULL) { eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } connectToObjectInstances(obj, eventMask); - } - else if ( updateMode == UAVObject::UPDATEMODE_ONCHANGE ) - { + } else if (updateMode == UAVObject::UPDATEMODE_ONCHANGE) { // Set update period setUpdatePeriod(obj, 0); // Connect signals for all instances eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - if( dynamic_cast(obj) != NULL ) - { + if (dynamic_cast(obj) != NULL) { eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } connectToObjectInstances(obj, eventMask); - } - else if ( updateMode == UAVObject::UPDATEMODE_THROTTLED ) - { + } else if (updateMode == UAVObject::UPDATEMODE_THROTTLED) { // If we received a periodic update, we can change back to update on change - if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) { + if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) { // Set update period - if (eventType == EV_NONE) - setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod); + if (eventType == EV_NONE) { + setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod); + } // Connect signals for all instances - eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC; - } - else - { + eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC; + } else { // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates // Connect signals for all instances eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - } - if( dynamic_cast(obj) != NULL ) - { - eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) + } + if (dynamic_cast(obj) != NULL) { + eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } connectToObjectInstances(obj, eventMask); - } - else if ( updateMode == UAVObject::UPDATEMODE_MANUAL ) - { + } else if (updateMode == UAVObject::UPDATEMODE_MANUAL) { // Set update period setUpdatePeriod(obj, 0); // Connect signals for all instances eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ; - if( dynamic_cast(obj) != NULL ) - { + if (dynamic_cast(obj) != NULL) { eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } connectToObjectInstances(obj, eventMask); @@ -230,25 +209,24 @@ void Telemetry::updateObject(UAVObject* obj, quint32 eventType) /** * Called when a transaction is successfully completed (uavtalk event) */ -void Telemetry::transactionCompleted(UAVObject* obj, bool success) +void Telemetry::transactionCompleted(UAVObject *obj, bool success) { // Lookup the transaction in the transaction map. quint32 objId = obj->getObjID(); - QMap::iterator itr = transMap.find(objId); - if ( itr != transMap.end() ) - { - ObjectTransactionInfo *transInfo = itr.value(); + + QMap::iterator itr = transMap.find(objId); + if (itr != transMap.end()) { + ObjectTransactionInfo *transInfo = itr.value(); // Remove this transaction as it's complete. - transInfo->timer->stop(); - transMap.remove(objId); - delete transInfo; + transInfo->timer->stop(); + transMap.remove(objId); + delete transInfo; // Send signal obj->emitTransactionCompleted(success); // Process new object updates from queue processObjectQueue(); - } else - { - qDebug() << "Error: received a transaction completed when did not expect it."; + } else { + qDebug() << "Error: received a transaction completed when did not expect it."; } } @@ -259,24 +237,21 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo) { transInfo->timer->stop(); // Check if more retries are pending - if (transInfo->retriesRemaining > 0) - { + if (transInfo->retriesRemaining > 0) { --transInfo->retriesRemaining; processObjectTransaction(transInfo); ++txRetries; - } - else - { - // Stop the timer. - transInfo->timer->stop(); + } else { + // Stop the timer. + transInfo->timer->stop(); // Terminate transaction utalk->cancelTransaction(transInfo->obj); // Send signal transInfo->obj->emitTransactionCompleted(false); // Remove this transaction as it's complete. - transMap.remove(transInfo->obj->getObjID()); - delete transInfo; - // Process new object updates from queue + transMap.remove(transInfo->obj->getObjID()); + delete transInfo; + // Process new object updates from queue processObjectQueue(); ++txErrors; } @@ -287,60 +262,45 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo) */ void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo) { - // Initiate transaction - if (transInfo->objRequest) - { + if (transInfo->objRequest) { utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances); - } - else - { + } else { utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances); } // Start timer if a response is expected - if ( transInfo->objRequest || transInfo->acked ) - { + if (transInfo->objRequest || transInfo->acked) { transInfo->timer->start(REQ_TIMEOUT_MS); - } - else - { + } else { // Otherwise, remove this transaction as it's complete. - transMap.remove(transInfo->obj->getObjID()); - delete transInfo; + transMap.remove(transInfo->obj->getObjID()); + delete transInfo; } } /** * Process the event received from an object */ -void Telemetry::processObjectUpdates(UAVObject* obj, EventMask event, bool allInstances, bool priority) +void Telemetry::processObjectUpdates(UAVObject *obj, EventMask event, bool allInstances, bool priority) { // Push event into queue ObjectQueueInfo objInfo; - objInfo.obj = obj; + + objInfo.obj = obj; objInfo.event = event; objInfo.allInstances = allInstances; - if (priority) - { - if ( objPriorityQueue.length() < MAX_QUEUE_SIZE ) - { + if (priority) { + if (objPriorityQueue.length() < MAX_QUEUE_SIZE) { objPriorityQueue.enqueue(objInfo); - } - else - { + } else { ++txErrors; obj->emitTransactionCompleted(false); qxtLog->warning(tr("Telemetry: priority event queue is full, event lost (%1)").arg(obj->getName())); } - } - else - { - if ( objQueue.length() < MAX_QUEUE_SIZE ) - { + } else { + if (objQueue.length() < MAX_QUEUE_SIZE) { objQueue.enqueue(objInfo); - } - else - { + } else { ++txErrors; obj->emitTransactionCompleted(false); } @@ -357,53 +317,43 @@ void Telemetry::processObjectQueue() { // Get object information from queue (first the priority and then the regular queue) ObjectQueueInfo objInfo; - if ( !objPriorityQueue.isEmpty() ) - { + + if (!objPriorityQueue.isEmpty()) { objInfo = objPriorityQueue.dequeue(); - } - else if ( !objQueue.isEmpty() ) - { + } else if (!objQueue.isEmpty()) { objInfo = objQueue.dequeue(); - } - else - { + } else { return; } // Check if a connection has been established, only process GCSTelemetryStats updates // (used to establish the connection) GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); - if ( gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED ) - { + if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED) { objQueue.clear(); - if ( objInfo.obj->getObjID() != GCSTelemetryStats::OBJID && objInfo.obj->getObjID() != OPLinkSettings::OBJID && objInfo.obj->getObjID() != ObjectPersistence::OBJID ) - { + if (objInfo.obj->getObjID() != GCSTelemetryStats::OBJID && objInfo.obj->getObjID() != OPLinkSettings::OBJID && objInfo.obj->getObjID() != ObjectPersistence::OBJID) { objInfo.obj->emitTransactionCompleted(false); return; } } // Setup transaction (skip if unpack event) - UAVObject::Metadata metadata = objInfo.obj->getMetadata(); + UAVObject::Metadata metadata = objInfo.obj->getMetadata(); UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata); - if ( ( objInfo.event != EV_UNPACKED ) && ( ( objInfo.event != EV_UPDATED_PERIODIC ) || ( updateMode != UAVObject::UPDATEMODE_THROTTLED ) ) ) - { - QMap::iterator itr = transMap.find(objInfo.obj->getObjID()); - if ( itr != transMap.end() ) { + if ((objInfo.event != EV_UNPACKED) && ((objInfo.event != EV_UPDATED_PERIODIC) || (updateMode != UAVObject::UPDATEMODE_THROTTLED))) { + QMap::iterator itr = transMap.find(objInfo.obj->getObjID()); + if (itr != transMap.end()) { qDebug() << "!!!!!! Making request for an object: " << objInfo.obj->getName() << " for which a request is already in progress!!!!!!"; } - UAVObject::Metadata metadata = objInfo.obj->getMetadata(); + UAVObject::Metadata metadata = objInfo.obj->getMetadata(); ObjectTransactionInfo *transInfo = new ObjectTransactionInfo(this); - transInfo->obj = objInfo.obj; + transInfo->obj = objInfo.obj; transInfo->allInstances = objInfo.allInstances; transInfo->retriesRemaining = MAX_RETRIES; transInfo->acked = UAVObject::GetGcsTelemetryAcked(metadata); - if ( objInfo.event == EV_UPDATED || objInfo.event == EV_UPDATED_MANUAL || objInfo.event == EV_UPDATED_PERIODIC ) - { + if (objInfo.event == EV_UPDATED || objInfo.event == EV_UPDATED_MANUAL || objInfo.event == EV_UPDATED_PERIODIC) { transInfo->objRequest = false; - } - else if ( objInfo.event == EV_UPDATE_REQ ) - { + } else if (objInfo.event == EV_UPDATE_REQ) { transInfo->objRequest = true; } transInfo->telem = this; @@ -413,22 +363,20 @@ void Telemetry::processObjectQueue() } // If this is a metaobject then make necessary telemetry updates - UAVMetaObject* metaobj = dynamic_cast(objInfo.obj); - if ( metaobj != NULL ) - { - updateObject( metaobj->getParentObject(), EV_NONE ); - } - else if ( updateMode != UAVObject::UPDATEMODE_THROTTLED ) - { - updateObject( objInfo.obj, objInfo.event ); + UAVMetaObject *metaobj = dynamic_cast(objInfo.obj); + if (metaobj != NULL) { + updateObject(metaobj->getParentObject(), EV_NONE); + } else if (updateMode != UAVObject::UPDATEMODE_THROTTLED) { + updateObject(objInfo.obj, objInfo.event); } // The fact we received an unpacked event does not mean that // we do not have additional objects still in the queue, // so we have to reschedule queue processing to make sure they are not // stuck: - if ( objInfo.event == EV_UNPACKED ) + if (objInfo.event == EV_UNPACKED) { processObjectQueue(); + } } /** @@ -444,21 +392,18 @@ void Telemetry::processPeriodicUpdates() // Iterate through each object and update its timer, if zero then transmit object. // Also calculate smallest delay to next update (will be used for setting timeToNextUpdateMs) - qint32 minDelay = MAX_UPDATE_PERIOD_MS; + qint32 minDelay = MAX_UPDATE_PERIOD_MS; ObjectTimeInfo *objinfo; qint32 elapsedMs = 0; QTime time; qint32 offset; - for (int n = 0; n < objList.length(); ++n) - { + for (int n = 0; n < objList.length(); ++n) { objinfo = &objList[n]; // If object is configured for periodic updates - if (objinfo->updatePeriodMs > 0) - { + if (objinfo->updatePeriodMs > 0) { objinfo->timeToNextUpdateMs -= timeToNextUpdateMs; // Check if time for the next update - if (objinfo->timeToNextUpdateMs <= 0) - { + if (objinfo->timeToNextUpdateMs <= 0) { // Reset timer offset = (-objinfo->timeToNextUpdateMs) % objinfo->updatePeriodMs; objinfo->timeToNextUpdateMs = objinfo->updatePeriodMs - offset; @@ -470,16 +415,14 @@ void Telemetry::processPeriodicUpdates() timeToNextUpdateMs += elapsedMs; } // Update minimum delay - if (objinfo->timeToNextUpdateMs < minDelay) - { + if (objinfo->timeToNextUpdateMs < minDelay) { minDelay = objinfo->timeToNextUpdateMs; } } } // Check if delay for the next update is too short - if (minDelay < MIN_UPDATE_PERIOD_MS) - { + if (minDelay < MIN_UPDATE_PERIOD_MS) { minDelay = MIN_UPDATE_PERIOD_MS; } @@ -499,15 +442,16 @@ Telemetry::TelemetryStats Telemetry::getStats() // Update stats TelemetryStats stats; - stats.txBytes = utalkStats.txBytes; - stats.rxBytes = utalkStats.rxBytes; + + stats.txBytes = utalkStats.txBytes; + stats.rxBytes = utalkStats.rxBytes; stats.txObjectBytes = utalkStats.txObjectBytes; stats.rxObjectBytes = utalkStats.rxObjectBytes; - stats.rxObjects = utalkStats.rxObjects; - stats.txObjects = utalkStats.txObjects; - stats.txErrors = utalkStats.txErrors + txErrors; - stats.rxErrors = utalkStats.rxErrors; - stats.txRetries = txRetries; + stats.rxObjects = utalkStats.rxObjects; + stats.txObjects = utalkStats.txObjects; + stats.txErrors = utalkStats.txErrors + txErrors; + stats.rxErrors = utalkStats.rxErrors; + stats.txRetries = txRetries; // Done return stats; @@ -516,58 +460,66 @@ Telemetry::TelemetryStats Telemetry::getStats() void Telemetry::resetStats() { QMutexLocker locker(mutex); + utalk->resetStats(); - txErrors = 0; + txErrors = 0; txRetries = 0; } -void Telemetry::objectUpdatedAuto(UAVObject* obj) +void Telemetry::objectUpdatedAuto(UAVObject *obj) { QMutexLocker locker(mutex); + processObjectUpdates(obj, EV_UPDATED, false, true); } -void Telemetry::objectUpdatedManual(UAVObject* obj) +void Telemetry::objectUpdatedManual(UAVObject *obj) { QMutexLocker locker(mutex); + processObjectUpdates(obj, EV_UPDATED_MANUAL, false, true); } -void Telemetry::objectUpdatedPeriodic(UAVObject* obj) +void Telemetry::objectUpdatedPeriodic(UAVObject *obj) { QMutexLocker locker(mutex); + processObjectUpdates(obj, EV_UPDATED_PERIODIC, false, true); } -void Telemetry::objectUnpacked(UAVObject* obj) +void Telemetry::objectUnpacked(UAVObject *obj) { QMutexLocker locker(mutex); + processObjectUpdates(obj, EV_UNPACKED, false, true); } -void Telemetry::updateRequested(UAVObject* obj) +void Telemetry::updateRequested(UAVObject *obj) { QMutexLocker locker(mutex); + processObjectUpdates(obj, EV_UPDATE_REQ, false, true); } -void Telemetry::newObject(UAVObject* obj) +void Telemetry::newObject(UAVObject *obj) { QMutexLocker locker(mutex); + registerObject(obj); } -void Telemetry::newInstance(UAVObject* obj) +void Telemetry::newInstance(UAVObject *obj) { QMutexLocker locker(mutex); + registerObject(obj); } -ObjectTransactionInfo::ObjectTransactionInfo(QObject* parent):QObject(parent) +ObjectTransactionInfo::ObjectTransactionInfo(QObject *parent) : QObject(parent) { obj = 0; - allInstances = false; - objRequest = false; + allInstances = false; + objRequest = false; retriesRemaining = 0; acked = false; telem = 0; @@ -586,6 +538,7 @@ ObjectTransactionInfo::~ObjectTransactionInfo() void ObjectTransactionInfo::timeout() { - if (!telem.isNull()) + if (!telem.isNull()) { telem->transactionTimeout(this); + } } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h index 97d564557..751aeaeae 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h @@ -10,18 +10,18 @@ * @brief The UAVTalk protocol plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -37,25 +37,24 @@ #include #include -class ObjectTransactionInfo: public QObject { +class ObjectTransactionInfo : public QObject { Q_OBJECT public: - ObjectTransactionInfo(QObject * parent); + ObjectTransactionInfo(QObject *parent); ~ObjectTransactionInfo(); - UAVObject* obj; + UAVObject *obj; bool allInstances; bool objRequest; qint32 retriesRemaining; bool acked; QPointertelem; - QTimer* timer; + QTimer *timer; private slots: void timeout(); }; -class Telemetry: public QObject -{ +class Telemetry : public QObject { Q_OBJECT public: @@ -71,7 +70,7 @@ public: quint32 txRetries; } TelemetryStats; - Telemetry(UAVTalk* utalk, UAVObjectManager* objMngr); + Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr); ~Telemetry(); TelemetryStats getStats(); void resetStats(); @@ -82,7 +81,7 @@ signals: private: // Constants static const int REQ_TIMEOUT_MS = 250; - static const int MAX_RETRIES = 2; + static const int MAX_RETRIES = 2; static const int MAX_UPDATE_PERIOD_MS = 1000; static const int MIN_UPDATE_PERIOD_MS = 1; static const int MAX_QUEUE_SIZE = 20; @@ -92,62 +91,61 @@ private: * Events generated by objects */ typedef enum { - EV_NONE = 0x00, /** No event */ - EV_UNPACKED = 0x01, /** Object data updated by unpacking */ - EV_UPDATED = 0x02, /** Object data updated by changing the data structure */ - EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */ + EV_NONE = 0x00, /** No event */ + EV_UNPACKED = 0x01, /** Object data updated by unpacking */ + EV_UPDATED = 0x02, /** Object data updated by changing the data structure */ + EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */ EV_UPDATED_PERIODIC = 0x8, /** Object update event generated by timer */ - EV_UPDATE_REQ = 0x010 /** Request to update object data */ + EV_UPDATE_REQ = 0x010 /** Request to update object data */ } EventMask; typedef struct { - UAVObject* obj; - qint32 updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */ - qint32 timeToNextUpdateMs; /** Time delay to the next update */ + UAVObject *obj; + qint32 updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */ + qint32 timeToNextUpdateMs; /** Time delay to the next update */ } ObjectTimeInfo; typedef struct { - UAVObject* obj; + UAVObject *obj; EventMask event; bool allInstances; } ObjectQueueInfo; // Variables - UAVObjectManager* objMngr; - UAVTalk* utalk; - GCSTelemetryStats* gcsStatsObj; + UAVObjectManager *objMngr; + UAVTalk *utalk; + GCSTelemetryStats *gcsStatsObj; QList objList; QQueue objQueue; QQueue objPriorityQueue; - QMaptransMap; - QMutex* mutex; - QTimer* updateTimer; - QTimer* statsTimer; + QMaptransMap; + QMutex *mutex; + QTimer *updateTimer; + QTimer *statsTimer; qint32 timeToNextUpdateMs; quint32 txErrors; quint32 txRetries; // Methods - void registerObject(UAVObject* obj); - void addObject(UAVObject* obj); - void setUpdatePeriod(UAVObject* obj, qint32 periodMs); - void connectToObjectInstances(UAVObject* obj, quint32 eventMask); - void updateObject(UAVObject* obj, quint32 eventMask); - void processObjectUpdates(UAVObject* obj, EventMask event, bool allInstances, bool priority); + void registerObject(UAVObject *obj); + void addObject(UAVObject *obj); + void setUpdatePeriod(UAVObject *obj, qint32 periodMs); + void connectToObjectInstances(UAVObject *obj, quint32 eventMask); + void updateObject(UAVObject *obj, quint32 eventMask); + void processObjectUpdates(UAVObject *obj, EventMask event, bool allInstances, bool priority); void processObjectTransaction(ObjectTransactionInfo *transInfo); void processObjectQueue(); private slots: - void objectUpdatedAuto(UAVObject* obj); - void objectUpdatedManual(UAVObject* obj); - void objectUpdatedPeriodic(UAVObject* obj); - void objectUnpacked(UAVObject* obj); - void updateRequested(UAVObject* obj); - void newObject(UAVObject* obj); - void newInstance(UAVObject* obj); + void objectUpdatedAuto(UAVObject *obj); + void objectUpdatedManual(UAVObject *obj); + void objectUpdatedPeriodic(UAVObject *obj); + void objectUnpacked(UAVObject *obj); + void updateRequested(UAVObject *obj); + void newObject(UAVObject *obj); + void newInstance(UAVObject *obj); void processPeriodicUpdates(); - void transactionCompleted(UAVObject* obj, bool success); - + void transactionCompleted(UAVObject *obj, bool success); }; #endif // TELEMETRY_H diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp index 61705fb81..008e3af73 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp @@ -35,17 +35,16 @@ TelemetryManager::TelemetryManager() : { moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread()); // Get UAVObjectManager instance - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); objMngr = pm->getObject(); // connect to start stop signals - connect(this, SIGNAL(myStart()), this, SLOT(onStart()),Qt::QueuedConnection); - connect(this, SIGNAL(myStop()), this, SLOT(onStop()),Qt::QueuedConnection); + connect(this, SIGNAL(myStart()), this, SLOT(onStart()), Qt::QueuedConnection); + connect(this, SIGNAL(myStop()), this, SLOT(onStop()), Qt::QueuedConnection); } TelemetryManager::~TelemetryManager() -{ -} +{} bool TelemetryManager::isConnected() { @@ -54,14 +53,14 @@ bool TelemetryManager::isConnected() void TelemetryManager::start(QIODevice *dev) { - device=dev; + device = dev; emit myStart(); } void TelemetryManager::onStart() { - utalk = new UAVTalk(device, objMngr); - telemetry = new Telemetry(utalk, objMngr); + utalk = new UAVTalk(device, objMngr); + telemetry = new Telemetry(utalk, objMngr); telemetryMon = new TelemetryMonitor(objMngr, telemetry); connect(telemetryMon, SIGNAL(connected()), this, SLOT(onConnect())); connect(telemetryMon, SIGNAL(disconnected()), this, SLOT(onDisconnect())); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h index 7006f1d7e..c4e7301ec 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h @@ -36,8 +36,7 @@ #include #include -class UAVTALK_EXPORT TelemetryManager: public QObject -{ +class UAVTALK_EXPORT TelemetryManager : public QObject { Q_OBJECT public: @@ -61,10 +60,10 @@ private slots: void onStop(); private: - UAVObjectManager* objMngr; - UAVTalk* utalk; - Telemetry* telemetry; - TelemetryMonitor* telemetryMon; + UAVObjectManager *objMngr; + UAVTalk *utalk; + Telemetry *telemetry; + TelemetryMonitor *telemetryMon; QIODevice *device; bool autopilotConnected; }; diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp index 32437f374..6d1a8268e 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp @@ -33,10 +33,10 @@ /** * Constructor */ -TelemetryMonitor::TelemetryMonitor(UAVObjectManager* objMngr, Telemetry* tel) +TelemetryMonitor::TelemetryMonitor(UAVObjectManager *objMngr, Telemetry *tel) { - this->objMngr = objMngr; - this->tel = tel; + this->objMngr = objMngr; + this->tel = tel; this->objPending = NULL; this->connectionTimer = new QTime(); @@ -44,11 +44,11 @@ TelemetryMonitor::TelemetryMonitor(UAVObjectManager* objMngr, Telemetry* tel) mutex = new QMutex(QMutex::Recursive); // Get stats objects - gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr); + gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr); flightStatsObj = FlightTelemetryStats::GetInstance(objMngr); // Listen for flight stats updates - connect(flightStatsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(flightStatsUpdated(UAVObject*))); + connect(flightStatsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(flightStatsUpdated(UAVObject *))); // Start update timer statsTimer = new QTimer(this); @@ -56,14 +56,16 @@ TelemetryMonitor::TelemetryMonitor(UAVObjectManager* objMngr, Telemetry* tel) statsTimer->start(STATS_CONNECT_PERIOD_MS); Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager(); - connect(this,SIGNAL(connected()),cm,SLOT(telemetryConnected())); - connect(this,SIGNAL(disconnected()),cm,SLOT(telemetryDisconnected())); - connect(this,SIGNAL(telemetryUpdated(double,double)),cm,SLOT(telemetryUpdated(double,double))); + connect(this, SIGNAL(connected()), cm, SLOT(telemetryConnected())); + connect(this, SIGNAL(disconnected()), cm, SLOT(telemetryDisconnected())); + connect(this, SIGNAL(telemetryUpdated(double, double)), cm, SLOT(telemetryUpdated(double, double))); } -TelemetryMonitor::~TelemetryMonitor() { +TelemetryMonitor::~TelemetryMonitor() +{ // Before saying goodbye, set the GCS connection status to disconnected too: GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); + gcsStats.Status = GCSTelemetryStats::STATUS_DISCONNECTED; // Set data gcsStatsObj->setData(gcsStats); @@ -77,27 +79,19 @@ void TelemetryMonitor::startRetrievingObjects() // Clear object queue queue.clear(); // Get all objects, add metaobjects, settings and data objects with OnChange update mode to the queue - QList< QList > objs = objMngr->getObjects(); - for (int n = 0; n < objs.length(); ++n) - { - UAVObject* obj = objs[n][0]; - UAVMetaObject* mobj = dynamic_cast(obj); - UAVDataObject* dobj = dynamic_cast(obj); + QList< QList > objs = objMngr->getObjects(); + for (int n = 0; n < objs.length(); ++n) { + UAVObject *obj = objs[n][0]; + UAVMetaObject *mobj = dynamic_cast(obj); + UAVDataObject *dobj = dynamic_cast(obj); UAVObject::Metadata mdata = obj->getMetadata(); - if ( mobj != NULL ) - { + if (mobj != NULL) { queue.enqueue(obj); - } - else if ( dobj != NULL ) - { - if ( dobj->isSettings() ) - { + } else if (dobj != NULL) { + if (dobj->isSettings()) { queue.enqueue(obj); - } - else - { - if ( UAVObject::GetFlightTelemetryUpdateMode(mdata) == UAVObject::UPDATEMODE_ONCHANGE ) - { + } else { + if (UAVObject::GetFlightTelemetryUpdateMode(mdata) == UAVObject::UPDATEMODE_ONCHANGE) { queue.enqueue(obj); } } @@ -105,7 +99,7 @@ void TelemetryMonitor::startRetrievingObjects() } // Start retrieving qxtLog->debug(tr("Starting to retrieve meta and settings objects from the autopilot (%1 objects)") - .arg( queue.length()) ); + .arg(queue.length())); retrieveNextObject(); } @@ -124,17 +118,16 @@ void TelemetryMonitor::stopRetrievingObjects() void TelemetryMonitor::retrieveNextObject() { // If queue is empty return - if ( queue.isEmpty() ) - { + if (queue.isEmpty()) { qxtLog->debug("Object retrieval completed"); emit connected(); return; } // Get next object from the queue - UAVObject* obj = queue.dequeue(); - //qxtLog->trace( tr("Retrieving object: %1").arg(obj->getName()) ); + UAVObject *obj = queue.dequeue(); + // qxtLog->trace( tr("Retrieving object: %1").arg(obj->getName()) ); // Connect to object - connect(obj, SIGNAL(transactionCompleted(UAVObject*,bool)), this, SLOT(transactionCompleted(UAVObject*,bool))); + connect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool))); // Request update obj->requestUpdate(); objPending = obj; @@ -143,7 +136,7 @@ void TelemetryMonitor::retrieveNextObject() /** * Called by the retrieved object when a transaction is completed. */ -void TelemetryMonitor::transactionCompleted(UAVObject* obj, bool success) +void TelemetryMonitor::transactionCompleted(UAVObject *obj, bool success) { Q_UNUSED(success); QMutexLocker locker(mutex); @@ -152,12 +145,9 @@ void TelemetryMonitor::transactionCompleted(UAVObject* obj, bool success) objPending = NULL; // Process next object if telemetry is still available GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); - if ( gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED ) - { + if (gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED) { retrieveNextObject(); - } - else - { + } else { stopRetrievingObjects(); } } @@ -165,7 +155,7 @@ void TelemetryMonitor::transactionCompleted(UAVObject* obj, bool success) /** * Called each time the flight stats object is updated by the autopilot */ -void TelemetryMonitor::flightStatsUpdated(UAVObject* obj) +void TelemetryMonitor::flightStatsUpdated(UAVObject *obj) { Q_UNUSED(obj); QMutexLocker locker(mutex); @@ -173,9 +163,8 @@ void TelemetryMonitor::flightStatsUpdated(UAVObject* obj) // Force update if not yet connected GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); FlightTelemetryStats::DataFields flightStats = flightStatsObj->getData(); - if ( gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED || - flightStats.Status != FlightTelemetryStats::STATUS_CONNECTED ) - { + if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED || + flightStats.Status != FlightTelemetryStats::STATUS_CONNECTED) { processStatsUpdates(); } } @@ -190,51 +179,41 @@ void TelemetryMonitor::processStatsUpdates() // Get telemetry stats GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData(); FlightTelemetryStats::DataFields flightStats = flightStatsObj->getData(); - Telemetry::TelemetryStats telStats = tel->getStats(); + Telemetry::TelemetryStats telStats = tel->getStats(); + tel->resetStats(); - // Update stats object - gcsStats.RxDataRate = (float)telStats.rxBytes / ((float)statsTimer->interval()/1000.0); - gcsStats.TxDataRate = (float)telStats.txBytes / ((float)statsTimer->interval()/1000.0); + // Update stats object + gcsStats.RxDataRate = (float)telStats.rxBytes / ((float)statsTimer->interval() / 1000.0); + gcsStats.TxDataRate = (float)telStats.txBytes / ((float)statsTimer->interval() / 1000.0); gcsStats.RxFailures += telStats.rxErrors; gcsStats.TxFailures += telStats.txErrors; - gcsStats.TxRetries += telStats.txRetries; + gcsStats.TxRetries += telStats.txRetries; // Check for a connection timeout bool connectionTimeout; - if ( telStats.rxObjects > 0 ) - { + if (telStats.rxObjects > 0) { connectionTimer->start(); } - if ( connectionTimer->elapsed() > CONNECTION_TIMEOUT_MS ) - { + if (connectionTimer->elapsed() > CONNECTION_TIMEOUT_MS) { connectionTimeout = true; - } - else - { + } else { connectionTimeout = false; } // Update connection state int oldStatus = gcsStats.Status; - if ( gcsStats.Status == GCSTelemetryStats::STATUS_DISCONNECTED ) - { + if (gcsStats.Status == GCSTelemetryStats::STATUS_DISCONNECTED) { // Request connection gcsStats.Status = GCSTelemetryStats::STATUS_HANDSHAKEREQ; - } - else if ( gcsStats.Status == GCSTelemetryStats::STATUS_HANDSHAKEREQ ) - { + } else if (gcsStats.Status == GCSTelemetryStats::STATUS_HANDSHAKEREQ) { // Check for connection acknowledge - if ( flightStats.Status == FlightTelemetryStats::STATUS_HANDSHAKEACK ) - { + if (flightStats.Status == FlightTelemetryStats::STATUS_HANDSHAKEACK) { gcsStats.Status = GCSTelemetryStats::STATUS_CONNECTED; } - } - else if ( gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED ) - { + } else if (gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED) { // Check if the connection is still active and the the autopilot is still connected - if (flightStats.Status == FlightTelemetryStats::STATUS_DISCONNECTED || connectionTimeout) - { + if (flightStats.Status == FlightTelemetryStats::STATUS_DISCONNECTED || connectionTimeout) { gcsStats.Status = GCSTelemetryStats::STATUS_DISCONNECTED; } } @@ -245,25 +224,21 @@ void TelemetryMonitor::processStatsUpdates() gcsStatsObj->setData(gcsStats); // Force telemetry update if not yet connected - if ( gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED || - flightStats.Status != FlightTelemetryStats::STATUS_CONNECTED ) - { + if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED || + flightStats.Status != FlightTelemetryStats::STATUS_CONNECTED) { gcsStatsObj->updated(); } // Act on new connections or disconnections - if (gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED && gcsStats.Status != oldStatus) - { + if (gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED && gcsStats.Status != oldStatus) { statsTimer->setInterval(STATS_UPDATE_PERIOD_MS); qxtLog->info("Connection with the autopilot established"); startRetrievingObjects(); } - if (gcsStats.Status == GCSTelemetryStats::STATUS_DISCONNECTED && gcsStats.Status != oldStatus) - { + if (gcsStats.Status == GCSTelemetryStats::STATUS_DISCONNECTED && gcsStats.Status != oldStatus) { statsTimer->setInterval(STATS_CONNECT_PERIOD_MS); qxtLog->info("Connection with the autopilot lost"); qxtLog->info("Trying to connect to the autopilot"); emit disconnected(); } } - diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.h b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.h index 6a1ad7d9f..58d70dff6 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.h @@ -40,12 +40,11 @@ #include "systemstats.h" #include "telemetry.h" -class TelemetryMonitor : public QObject -{ +class TelemetryMonitor : public QObject { Q_OBJECT public: - TelemetryMonitor(UAVObjectManager* objMngr, Telemetry* tel); + TelemetryMonitor(UAVObjectManager *objMngr, Telemetry *tel); ~TelemetryMonitor(); signals: @@ -54,24 +53,24 @@ signals: void telemetryUpdated(double txRate, double rxRate); public slots: - void transactionCompleted(UAVObject* obj, bool success); + void transactionCompleted(UAVObject *obj, bool success); void processStatsUpdates(); - void flightStatsUpdated(UAVObject* obj); + void flightStatsUpdated(UAVObject *obj); private: - static const int STATS_UPDATE_PERIOD_MS = 4000; + static const int STATS_UPDATE_PERIOD_MS = 4000; static const int STATS_CONNECT_PERIOD_MS = 2000; - static const int CONNECTION_TIMEOUT_MS = 8000; + static const int CONNECTION_TIMEOUT_MS = 8000; - UAVObjectManager* objMngr; - Telemetry* tel; - QQueue queue; - GCSTelemetryStats* gcsStatsObj; - FlightTelemetryStats* flightStatsObj; - QTimer* statsTimer; - UAVObject* objPending; - QMutex* mutex; - QTime* connectionTimer; + UAVObjectManager *objMngr; + Telemetry *tel; + QQueue queue; + GCSTelemetryStats *gcsStatsObj; + FlightTelemetryStats *flightStatsObj; + QTimer *statsTimer; + UAVObject *objPending; + QMutex *mutex; + QTime *connectionTimer; void startRetrievingObjects(); void retrieveNextObject(); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index da98bee98..13f621880 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -10,18 +10,18 @@ * @brief The UAVTalk protocol plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavtalk.h" @@ -29,13 +29,13 @@ #include #include #include -//#define UAVTALK_DEBUG +// #define UAVTALK_DEBUG #ifdef UAVTALK_DEBUG #include "qxtlogger.h" - #define UAVTALK_QXTLOG_DEBUG(args...) qxtLog->debug(args...) -#else // UAVTALK_DEBUG - #define UAVTALK_QXTLOG_DEBUG(args...) -#endif // UAVTALK_DEBUG + #define UAVTALK_QXTLOG_DEBUG(args ...) qxtLog->debug(args ...) +#else // UAVTALK_DEBUG + #define UAVTALK_QXTLOG_DEBUG(args ...) +#endif // UAVTALK_DEBUG #define SYNC_VAL 0x3C @@ -62,11 +62,11 @@ const quint8 UAVTalk::crc_table[256] = { /** * Constructor */ -UAVTalk::UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr) +UAVTalk::UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr) { io = iodev; - this->objMngr = objMngr; + this->objMngr = objMngr; rxState = STATE_SYNC; rxPacketLength = 0; @@ -77,17 +77,16 @@ UAVTalk::UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr) connect(io, SIGNAL(readyRead()), this, SLOT(processInputStream())); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); - useUDPMirror=settings->useUDPMirror(); - qDebug()<<"USE UDP:::::::::::."<getObject(); + useUDPMirror = settings->useUDPMirror(); + qDebug() << "USE UDP:::::::::::." << useUDPMirror; + if (useUDPMirror) { + udpSocketTx = new QUdpSocket(this); + udpSocketRx = new QUdpSocket(this); udpSocketTx->bind(9000); - udpSocketRx->connectToHost(QHostAddress::LocalHost,9000); - connect(udpSocketTx,SIGNAL(readyRead()),this,SLOT(dummyUDPRead())); - connect(udpSocketRx,SIGNAL(readyRead()),this,SLOT(dummyUDPRead())); + udpSocketRx->connectToHost(QHostAddress::LocalHost, 9000); + connect(udpSocketTx, SIGNAL(readyRead()), this, SLOT(dummyUDPRead())); + connect(udpSocketRx, SIGNAL(readyRead()), this, SLOT(dummyUDPRead())); } } @@ -95,7 +94,7 @@ UAVTalk::~UAVTalk() { // According to Qt, it is not necessary to disconnect upon // object deletion. - //disconnect(io, SIGNAL(readyRead()), this, SLOT(processInputStream())); + // disconnect(io, SIGNAL(readyRead()), this, SLOT(processInputStream())); } @@ -105,6 +104,7 @@ UAVTalk::~UAVTalk() void UAVTalk::resetStats() { QMutexLocker locker(mutex); + memset(&stats, 0, sizeof(ComStats)); } @@ -114,6 +114,7 @@ void UAVTalk::resetStats() UAVTalk::ComStats UAVTalk::getStats() { QMutexLocker locker(mutex); + return stats; } @@ -125,9 +126,8 @@ void UAVTalk::processInputStream() quint8 tmp; if (io && io->isReadable()) { - while (io->bytesAvailable() > 0) - { - io->read((char*)&tmp, 1); + while (io->bytesAvailable() > 0) { + io->read((char *)&tmp, 1); processInputByte(tmp); } } @@ -135,12 +135,12 @@ void UAVTalk::processInputStream() void UAVTalk::dummyUDPRead() { - QUdpSocket *socket=qobject_cast(sender()); + QUdpSocket *socket = qobject_cast(sender()); QByteArray junk; - while(socket->hasPendingDatagrams()) - { + + while (socket->hasPendingDatagrams()) { junk.resize(socket->pendingDatagramSize()); - socket->readDatagram(junk.data(),junk.size()); + socket->readDatagram(junk.data(), junk.size()); } } @@ -151,9 +151,10 @@ void UAVTalk::dummyUDPRead() * \param[in] allInstances If set true then all instances will be updated * \return Success (true), Failure (false) */ -bool UAVTalk::sendObjectRequest(UAVObject* obj, bool allInstances) +bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances) { QMutexLocker locker(mutex); + return objectTransaction(obj, TYPE_OBJ_REQ, allInstances); } @@ -164,15 +165,13 @@ bool UAVTalk::sendObjectRequest(UAVObject* obj, bool allInstances) * \param[in] allInstances If set true then all instances will be updated * \return Success (true), Failure (false) */ -bool UAVTalk::sendObject(UAVObject* obj, bool acked, bool allInstances) +bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances) { QMutexLocker locker(mutex); - if (acked) - { + + if (acked) { return objectTransaction(obj, TYPE_OBJ_ACK, allInstances); - } - else - { + } else { return objectTransaction(obj, TYPE_OBJ, allInstances); } } @@ -180,17 +179,18 @@ bool UAVTalk::sendObject(UAVObject* obj, bool acked, bool allInstances) /** * Cancel a pending transaction */ -void UAVTalk::cancelTransaction(UAVObject* obj) +void UAVTalk::cancelTransaction(UAVObject *obj) { QMutexLocker locker(mutex); quint32 objId = obj->getObjID(); - if(io.isNull()) + + if (io.isNull()) { return; - QMap::iterator itr = transMap.find(objId); - if ( itr != transMap.end() ) - { + } + QMap::iterator itr = transMap.find(objId); + if (itr != transMap.end()) { transMap.remove(objId); - delete itr.value(); + delete itr.value(); } } @@ -198,36 +198,28 @@ void UAVTalk::cancelTransaction(UAVObject* obj) * Execute the requested transaction on an object. * \param[in] obj Object * \param[in] type Transaction type - * TYPE_OBJ: send object, - * TYPE_OBJ_REQ: request object update - * TYPE_OBJ_ACK: send object with an ack + * TYPE_OBJ: send object, + * TYPE_OBJ_REQ: request object update + * TYPE_OBJ_ACK: send object with an ack * \param[in] allInstances If set true then all instances will be updated * \return Success (true), Failure (false) */ -bool UAVTalk::objectTransaction(UAVObject* obj, quint8 type, bool allInstances) +bool UAVTalk::objectTransaction(UAVObject *obj, quint8 type, bool allInstances) { // Send object depending on if a response is needed - if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) - { - if ( transmitObject(obj, type, allInstances) ) - { - Transaction *trans = new Transaction(); - trans->obj = obj; - trans->allInstances = allInstances; + if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) { + if (transmitObject(obj, type, allInstances)) { + Transaction *trans = new Transaction(); + trans->obj = obj; + trans->allInstances = allInstances; transMap.insert(obj->getObjID(), trans); return true; - } - else - { + } else { return false; } - } - else if (type == TYPE_OBJ) - { + } else if (type == TYPE_OBJ) { return transmitObject(obj, TYPE_OBJ, allInstances); - } - else - { + } else { return false; } } @@ -242,260 +234,232 @@ bool UAVTalk::processInputByte(quint8 rxbyte) // Update stats stats.rxBytes++; - rxPacketLength++; // update packet byte count + rxPacketLength++; // update packet byte count - if(useUDPMirror) + if (useUDPMirror) { rxDataArray.append(rxbyte); + } // Receive state machine - switch (rxState) - { - case STATE_SYNC: + switch (rxState) { + case STATE_SYNC: - if (rxbyte != SYNC_VAL) - { - UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Sync (" + QString::number(rxbyte) + " " + QString("0x%1").arg(rxbyte,2,16) + ")"); - break; - } - - // Initialize and update CRC - rxCS = updateCRC(0, rxbyte); - - rxPacketLength = 1; - - if(useUDPMirror) - { - rxDataArray.clear(); - rxDataArray.append(rxbyte); - } - - rxState = STATE_TYPE; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Type"); + if (rxbyte != SYNC_VAL) { + UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Sync (" + QString::number(rxbyte) + " " + QString("0x%1").arg(rxbyte, 2, 16) + ")"); break; + } - case STATE_TYPE: + // Initialize and update CRC + rxCS = updateCRC(0, rxbyte); - // Update CRC - rxCS = updateCRC(rxCS, rxbyte); + rxPacketLength = 1; - if ((rxbyte & TYPE_MASK) != TYPE_VER) - { + if (useUDPMirror) { + rxDataArray.clear(); + rxDataArray.append(rxbyte); + } + + rxState = STATE_TYPE; + UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Type"); + break; + + case STATE_TYPE: + + // Update CRC + rxCS = updateCRC(rxCS, rxbyte); + + if ((rxbyte & TYPE_MASK) != TYPE_VER) { + rxState = STATE_SYNC; + UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Sync"); + break; + } + + rxType = rxbyte; + + packetSize = 0; + + rxState = STATE_SIZE; + UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Size"); + rxCount = 0; + break; + + case STATE_SIZE: + + // Update CRC + rxCS = updateCRC(rxCS, rxbyte); + + if (rxCount == 0) { + packetSize += rxbyte; + rxCount++; + UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Size"); + break; + } + + packetSize += (quint32)rxbyte << 8; + + if (packetSize < MIN_HEADER_LENGTH || packetSize > MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH) { // incorrect packet size + rxState = STATE_SYNC; + UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Sync"); + break; + } + + rxCount = 0; + rxState = STATE_OBJID; + UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->ObjID"); + break; + + case STATE_OBJID: + + // Update CRC + rxCS = updateCRC(rxCS, rxbyte); + + rxTmpBuffer[rxCount++] = rxbyte; + if (rxCount < 4) { + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->ObjID"); + break; + } + + // Search for object, if not found reset state machine + rxObjId = (qint32)qFromLittleEndian(rxTmpBuffer); + { + UAVObject *rxObj = objMngr->getObject(rxObjId); + if (rxObj == NULL && rxType != TYPE_OBJ_REQ) { + stats.rxErrors++; rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Sync"); + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (badtype)"); break; } - rxType = rxbyte; - - packetSize = 0; - - rxState = STATE_SIZE; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Size"); - rxCount = 0; - break; - - case STATE_SIZE: - - // Update CRC - rxCS = updateCRC(rxCS, rxbyte); - - if (rxCount == 0) - { - packetSize += rxbyte; - rxCount++; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Size"); - break; + // Determine data length + if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK) { + rxLength = 0; + rxInstanceLength = 0; + } else { + rxLength = rxObj->getNumBytes(); + rxInstanceLength = (rxObj->isSingleInstance() ? 0 : 2); } - packetSize += (quint32)rxbyte << 8; - - if (packetSize < MIN_HEADER_LENGTH || packetSize > MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH) - { // incorrect packet size + // Check length and determine next state + if (rxLength >= MAX_PAYLOAD_LENGTH) { + stats.rxErrors++; rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Sync"); + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (oversize)"); break; } - rxCount = 0; - rxState = STATE_OBJID; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->ObjID"); + // Check the lengths match + if ((rxPacketLength + rxInstanceLength + rxLength) != packetSize) { // packet error - mismatched packet size + stats.rxErrors++; + rxState = STATE_SYNC; + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (length mismatch)"); + break; + } + + // Check if this is a single instance object (i.e. if the instance ID field is coming next) + if (rxObj == NULL) { + // This is a non-existing object, just skip to checksum + // and we'll send a NACK next. + rxState = STATE_CS; + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->CSum (no obj)"); + rxInstId = 0; + rxCount = 0; + } else if (rxObj->isSingleInstance()) { + // If there is a payload get it, otherwise receive checksum + if (rxLength > 0) { + rxState = STATE_DATA; + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Data (needs data)"); + } else { + rxState = STATE_CS; + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Checksum"); + } + rxInstId = 0; + rxCount = 0; + } else { + rxState = STATE_INSTID; + UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->InstID"); + rxCount = 0; + } + } + + break; + + case STATE_INSTID: + + // Update CRC + rxCS = updateCRC(rxCS, rxbyte); + + rxTmpBuffer[rxCount++] = rxbyte; + if (rxCount < 2) { + UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->InstID"); break; + } - case STATE_OBJID: + rxInstId = (qint16)qFromLittleEndian(rxTmpBuffer); - // Update CRC - rxCS = updateCRC(rxCS, rxbyte); - - rxTmpBuffer[rxCount++] = rxbyte; - if (rxCount < 4) - { - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->ObjID"); - break; - } - - // Search for object, if not found reset state machine - rxObjId = (qint32)qFromLittleEndian(rxTmpBuffer); - { - UAVObject *rxObj = objMngr->getObject(rxObjId); - if (rxObj == NULL && rxType != TYPE_OBJ_REQ) - { - stats.rxErrors++; - rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (badtype)"); - break; - } - - // Determine data length - if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK) - { - rxLength = 0; - rxInstanceLength = 0; - } - else - { - rxLength = rxObj->getNumBytes(); - rxInstanceLength = (rxObj->isSingleInstance() ? 0 : 2); - } - - // Check length and determine next state - if (rxLength >= MAX_PAYLOAD_LENGTH) - { - stats.rxErrors++; - rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (oversize)"); - break; - } - - // Check the lengths match - if ((rxPacketLength + rxInstanceLength + rxLength) != packetSize) - { // packet error - mismatched packet size - stats.rxErrors++; - rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (length mismatch)"); - break; - } - - // Check if this is a single instance object (i.e. if the instance ID field is coming next) - if (rxObj == NULL) - { - // This is a non-existing object, just skip to checksum - // and we'll send a NACK next. - rxState = STATE_CS; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->CSum (no obj)"); - rxInstId = 0; - rxCount = 0; - } - else if (rxObj->isSingleInstance()) - { - // If there is a payload get it, otherwise receive checksum - if (rxLength > 0) - { - rxState = STATE_DATA; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Data (needs data)"); - } - else - { - rxState = STATE_CS; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Checksum"); - } - rxInstId = 0; - rxCount = 0; - } - else - { - rxState = STATE_INSTID; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->InstID"); - rxCount = 0; - } - } - - break; - - case STATE_INSTID: - - // Update CRC - rxCS = updateCRC(rxCS, rxbyte); - - rxTmpBuffer[rxCount++] = rxbyte; - if (rxCount < 2) - { - UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->InstID"); - break; - } - - rxInstId = (qint16)qFromLittleEndian(rxTmpBuffer); - - rxCount = 0; - - // If there is a payload get it, otherwise receive checksum - if (rxLength > 0) - { - rxState = STATE_DATA; - UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->Data"); - } - else - { - rxState = STATE_CS; - UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->CSum"); - } - break; - - case STATE_DATA: - - // Update CRC - rxCS = updateCRC(rxCS, rxbyte); - - rxBuffer[rxCount++] = rxbyte; - if (rxCount < rxLength) - { - UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->Data"); - break; - } + rxCount = 0; + // If there is a payload get it, otherwise receive checksum + if (rxLength > 0) { + rxState = STATE_DATA; + UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->Data"); + } else { rxState = STATE_CS; - UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->CSum"); - rxCount = 0; + UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->CSum"); + } + break; + + case STATE_DATA: + + // Update CRC + rxCS = updateCRC(rxCS, rxbyte); + + rxBuffer[rxCount++] = rxbyte; + if (rxCount < rxLength) { + UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->Data"); break; + } - case STATE_CS: + rxState = STATE_CS; + UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->CSum"); + rxCount = 0; + break; - // The CRC byte - rxCSPacket = rxbyte; + case STATE_CS: - if (rxCS != rxCSPacket) - { // packet error - faulty CRC - stats.rxErrors++; - rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (badcrc)"); - break; - } + // The CRC byte + rxCSPacket = rxbyte; - if (rxPacketLength != packetSize + 1) - { // packet error - mismatched packet size - stats.rxErrors++; - rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (length mismatch)"); - break; - } - - mutex->lock(); - receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength); - if(useUDPMirror) - { - udpSocketTx->writeDatagram(rxDataArray,QHostAddress::LocalHost,udpSocketRx->localPort()); - } - stats.rxObjectBytes += rxLength; - stats.rxObjects++; - mutex->unlock(); - - rxState = STATE_SYNC; - UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (OK)"); - break; - - default: - rxState = STATE_SYNC; + if (rxCS != rxCSPacket) { // packet error - faulty CRC stats.rxErrors++; - UAVTALK_QXTLOG_DEBUG("UAVTalk: \?\?\?->Sync"); //Use the escape character for '?' so that the tripgraph isn't triggered. + rxState = STATE_SYNC; + UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (badcrc)"); + break; + } + + if (rxPacketLength != packetSize + 1) { // packet error - mismatched packet size + stats.rxErrors++; + rxState = STATE_SYNC; + UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (length mismatch)"); + break; + } + + mutex->lock(); + receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength); + if (useUDPMirror) { + udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort()); + } + stats.rxObjectBytes += rxLength; + stats.rxObjects++; + mutex->unlock(); + + rxState = STATE_SYNC; + UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (OK)"); + break; + + default: + rxState = STATE_SYNC; + stats.rxErrors++; + UAVTALK_QXTLOG_DEBUG("UAVTalk: \?\?\?->Sync"); // Use the escape character for '?' so that the tripgraph isn't triggered. } // Done @@ -511,74 +475,56 @@ bool UAVTalk::processInputByte(quint8 rxbyte) * \param[in] length Buffer length * \return Success (true), Failure (false) */ -bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* data, qint32 length) +bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length) { Q_UNUSED(length); - UAVObject* obj = NULL; - bool error = false; - bool allInstances = (instId == ALL_INSTANCES); + UAVObject *obj = NULL; + bool error = false; + bool allInstances = (instId == ALL_INSTANCES); // Process message type switch (type) { case TYPE_OBJ: // All instances, not allowed for OBJ messages - if (!allInstances) - { + if (!allInstances) { // Get object and update its data obj = updateObject(objId, instId, data); // Check if an ack is pending - if ( obj != NULL ) - { + if (obj != NULL) { updateAck(obj); - } - else - { + } else { error = true; } - } - else - { + } else { error = true; } break; case TYPE_OBJ_ACK: // All instances, not allowed for OBJ_ACK messages - if (!allInstances) - { + if (!allInstances) { // Get object and update its data obj = updateObject(objId, instId, data); // Transmit ACK - if ( obj != NULL ) - { - transmitObject(obj, TYPE_ACK, false); - } - else - { + if (obj != NULL) { + transmitObject(obj, TYPE_ACK, false); + } else { error = true; } - } - else - { + } else { error = true; } break; case TYPE_OBJ_REQ: // Get object, if all instances are requested get instance 0 of the object - if (allInstances) - { + if (allInstances) { obj = objMngr->getObject(objId); - } - else - { + } else { obj = objMngr->getObject(objId, instId); } // If object was found transmit it - if (obj != NULL) - { + if (obj != NULL) { transmitObject(obj, TYPE_OBJ, allInstances); - } - else - { + } else { // Object was not found, transmit a NACK with the // objId which was not found. transmitNack(objId); @@ -587,34 +533,26 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* break; case TYPE_NACK: // All instances, not allowed for NACK messages - if (!allInstances) - { + if (!allInstances) { // Get object obj = objMngr->getObject(objId, instId); // Check if object exists: - if (obj != NULL) - { + if (obj != NULL) { updateNack(obj); - } - else - { - error = true; + } else { + error = true; } } break; case TYPE_ACK: // All instances, not allowed for ACK messages - if (!allInstances) - { + if (!allInstances) { // Get object obj = objMngr->getObject(objId, instId); // Check if an ack is pending - if (obj != NULL) - { + if (obj != NULL) { updateAck(obj); - } - else - { + } else { error = true; } } @@ -631,36 +569,31 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* * If the object instance could not be found in the list, then a * new one is created. */ -UAVObject* UAVTalk::updateObject(quint32 objId, quint16 instId, quint8* data) +UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data) { // Get object - UAVObject* obj = objMngr->getObject(objId, instId); + UAVObject *obj = objMngr->getObject(objId, instId); + // If the instance does not exist create it - if (obj == NULL) - { + if (obj == NULL) { // Get the object type - UAVObject* tobj = objMngr->getObject(objId); - if (tobj == NULL) - { + UAVObject *tobj = objMngr->getObject(objId); + if (tobj == NULL) { return NULL; } // Make sure this is a data object - UAVDataObject* dobj = dynamic_cast(tobj); - if (dobj == NULL) - { + UAVDataObject *dobj = dynamic_cast(tobj); + if (dobj == NULL) { return NULL; } // Create a new instance, unpack and register - UAVDataObject* instobj = dobj->clone(instId); - if ( !objMngr->registerObject(instobj) ) - { + UAVDataObject *instobj = dobj->clone(instId); + if (!objMngr->registerObject(instobj)) { return NULL; } instobj->unpack(data); return instobj; - } - else - { + } else { // Unpack data into object instance obj->unpack(data); return obj; @@ -670,17 +603,17 @@ UAVObject* UAVTalk::updateObject(quint32 objId, quint16 instId, quint8* data) /** * Check if a transaction is pending and if yes complete it. */ -void UAVTalk::updateNack(UAVObject* obj) +void UAVTalk::updateNack(UAVObject *obj) { Q_ASSERT(obj); - if ( ! obj ) + if (!obj) { return; + } quint32 objId = obj->getObjID(); - QMap::iterator itr = transMap.find(objId); - if ( itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) - { + QMap::iterator itr = transMap.find(objId); + if (itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) { transMap.remove(objId); - delete itr.value(); + delete itr.value(); emit transactionCompleted(obj, false); } } @@ -689,14 +622,14 @@ void UAVTalk::updateNack(UAVObject* obj) /** * Check if a transaction is pending and if yes complete it. */ -void UAVTalk::updateAck(UAVObject* obj) +void UAVTalk::updateAck(UAVObject *obj) { quint32 objId = obj->getObjID(); - QMap::iterator itr = transMap.find(objId); - if ( itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) - { + + QMap::iterator itr = transMap.find(objId); + if (itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) { transMap.remove(objId); - delete itr.value(); + delete itr.value(); emit transactionCompleted(obj, true); } } @@ -709,51 +642,36 @@ void UAVTalk::updateAck(UAVObject* obj) * \param[in] allInstances True is all instances of the object are to be sent * \return Success (true), Failure (false) */ -bool UAVTalk::transmitObject(UAVObject* obj, quint8 type, bool allInstances) -{ +bool UAVTalk::transmitObject(UAVObject *obj, quint8 type, bool allInstances) +{ // If all instances are requested on a single instance object it is an error - if (allInstances && obj->isSingleInstance()) - { + if (allInstances && obj->isSingleInstance()) { allInstances = false; } // Process message type - if ( type == TYPE_OBJ || type == TYPE_OBJ_ACK ) - { - if (allInstances) - { + if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) { + if (allInstances) { // Get number of instances quint32 numInst = objMngr->getNumInstances(obj->getObjID()); // Send all instances - for (quint32 instId = 0; instId < numInst; ++instId) - { - UAVObject* inst = objMngr->getObject(obj->getObjID(), instId); + for (quint32 instId = 0; instId < numInst; ++instId) { + UAVObject *inst = objMngr->getObject(obj->getObjID(), instId); transmitSingleObject(inst, type, false); } return true; - } - else - { + } else { return transmitSingleObject(obj, type, false); } - } - else if (type == TYPE_OBJ_REQ) - { + } else if (type == TYPE_OBJ_REQ) { return transmitSingleObject(obj, TYPE_OBJ_REQ, allInstances); - } - else if (type == TYPE_ACK) - { - if (!allInstances) - { + } else if (type == TYPE_ACK) { + if (!allInstances) { return transmitSingleObject(obj, TYPE_ACK, false); - } - else - { + } else { return false; } - } - else - { + } else { return false; } } @@ -776,26 +694,21 @@ bool UAVTalk::transmitNack(quint32 objId) qToLittleEndian(dataOffset, &txBuffer[2]); // Send buffer, check that the transmit backlog does not grow above limit - if (io && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE ) - { - io->write((const char*)txBuffer, dataOffset+CHECKSUM_LENGTH); - if(useUDPMirror) - { - udpSocketRx->writeDatagram((const char*)txBuffer,dataOffset+CHECKSUM_LENGTH,QHostAddress::LocalHost,udpSocketTx->localPort()); + if (io && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE) { + io->write((const char *)txBuffer, dataOffset + CHECKSUM_LENGTH); + if (useUDPMirror) { + udpSocketRx->writeDatagram((const char *)txBuffer, dataOffset + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort()); } - } - else - { + } else { ++stats.txErrors; return false; } // Update stats - stats.txBytes += 8+CHECKSUM_LENGTH; + stats.txBytes += 8 + CHECKSUM_LENGTH; // Done return true; - } @@ -805,7 +718,7 @@ bool UAVTalk::transmitNack(quint32 objId) * \param[in] type Transaction type * \return Success (true), Failure (false) */ -bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances) +bool UAVTalk::transmitSingleObject(UAVObject *obj, quint8 type, bool allInstances) { qint32 length; qint32 dataOffset; @@ -820,19 +733,13 @@ bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstance qToLittleEndian(objId, &txBuffer[4]); // Setup instance ID if one is required - if ( obj->isSingleInstance() ) - { + if (obj->isSingleInstance()) { dataOffset = 8; - } - else - { + } else { // Check if all instances are requested - if (allInstances) - { + if (allInstances) { qToLittleEndian(allInstId, &txBuffer[8]); - } - else - { + } else { instId = obj->getInstID(); qToLittleEndian(instId, &txBuffer[8]); } @@ -840,26 +747,20 @@ bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstance } // Determine data length - if (type == TYPE_OBJ_REQ || type == TYPE_ACK) - { + if (type == TYPE_OBJ_REQ || type == TYPE_ACK) { length = 0; - } - else - { + } else { length = obj->getNumBytes(); } // Check length - if (length >= MAX_PAYLOAD_LENGTH) - { + if (length >= MAX_PAYLOAD_LENGTH) { return false; } // Copy data (if any) - if (length > 0) - { - if ( !obj->pack(&txBuffer[dataOffset]) ) - { + if (length > 0) { + if (!obj->pack(&txBuffer[dataOffset])) { return false; } } @@ -867,26 +768,22 @@ bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstance qToLittleEndian(dataOffset + length, &txBuffer[2]); // Calculate checksum - txBuffer[dataOffset+length] = updateCRC(0, txBuffer, dataOffset + length); + txBuffer[dataOffset + length] = updateCRC(0, txBuffer, dataOffset + length); // Send buffer, check that the transmit backlog does not grow above limit - if (!io.isNull() && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE ) - { - io->write((const char*)txBuffer, dataOffset+length+CHECKSUM_LENGTH); - if(useUDPMirror) - { - udpSocketRx->writeDatagram((const char*)txBuffer,dataOffset+length+CHECKSUM_LENGTH,QHostAddress::LocalHost,udpSocketTx->localPort()); + if (!io.isNull() && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE) { + io->write((const char *)txBuffer, dataOffset + length + CHECKSUM_LENGTH); + if (useUDPMirror) { + udpSocketRx->writeDatagram((const char *)txBuffer, dataOffset + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort()); } - } - else - { + } else { ++stats.txErrors; return false; } // Update stats ++stats.txObjects; - stats.txBytes += dataOffset+length+CHECKSUM_LENGTH; + stats.txBytes += dataOffset + length + CHECKSUM_LENGTH; stats.txObjectBytes += length; // Done @@ -915,9 +812,10 @@ quint8 UAVTalk::updateCRC(quint8 crc, const quint8 data) { return crc_table[crc ^ data]; } -quint8 UAVTalk::updateCRC(quint8 crc, const quint8* data, qint32 length) +quint8 UAVTalk::updateCRC(quint8 crc, const quint8 *data, qint32 length) { - while (length--) + while (length--) { crc = crc_table[crc ^ *data++]; + } return crc; } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index 3d2c69d7b..fd22b66ef 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h @@ -10,18 +10,18 @@ * @brief The UAVTalk protocol plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVTALK_H @@ -37,8 +37,7 @@ #include "uavtalk_global.h" #include -class UAVTALK_EXPORT UAVTalk: public QObject -{ +class UAVTALK_EXPORT UAVTalk : public QObject { Q_OBJECT public: @@ -53,16 +52,16 @@ public: quint32 rxErrors; } ComStats; - UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr); + UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr); ~UAVTalk(); - bool sendObject(UAVObject* obj, bool acked, bool allInstances); - bool sendObjectRequest(UAVObject* obj, bool allInstances); - void cancelTransaction(UAVObject* obj); + bool sendObject(UAVObject *obj, bool acked, bool allInstances); + bool sendObjectRequest(UAVObject *obj, bool allInstances); + void cancelTransaction(UAVObject *obj); ComStats getStats(); void resetStats(); signals: - void transactionCompleted(UAVObject* obj, bool success); + void transactionCompleted(UAVObject *obj, bool success); private slots: void processInputStream(void); @@ -71,42 +70,42 @@ private slots: private: typedef struct { - UAVObject* obj; + UAVObject *obj; bool allInstances; } Transaction; // Constants - static const int TYPE_MASK = 0xF8; - static const int TYPE_VER = 0x20; - static const int TYPE_OBJ = (TYPE_VER | 0x00); + static const int TYPE_MASK = 0xF8; + static const int TYPE_VER = 0x20; + static const int TYPE_OBJ = (TYPE_VER | 0x00); static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01); static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02); - static const int TYPE_ACK = (TYPE_VER | 0x03); - static const int TYPE_NACK = (TYPE_VER | 0x04); + static const int TYPE_ACK = (TYPE_VER | 0x03); + static const int TYPE_NACK = (TYPE_VER | 0x04); - static const int MIN_HEADER_LENGTH = 8; // sync(1), type (1), size(2), object ID(4) - static const int MAX_HEADER_LENGTH = 10; // sync(1), type (1), size(2), object ID (4), instance ID(2, not used in single objects) + static const int MIN_HEADER_LENGTH = 8; // sync(1), type (1), size(2), object ID(4) + static const int MAX_HEADER_LENGTH = 10; // sync(1), type (1), size(2), object ID (4), instance ID(2, not used in single objects) - static const int CHECKSUM_LENGTH = 1; + static const int CHECKSUM_LENGTH = 1; static const int MAX_PAYLOAD_LENGTH = 256; - static const int MAX_PACKET_LENGTH = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH); + static const int MAX_PACKET_LENGTH = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH); - static const quint16 ALL_INSTANCES = 0xFFFF; + static const quint16 ALL_INSTANCES = 0xFFFF; static const quint16 OBJID_NOTFOUND = 0x0000; - static const int TX_BUFFER_SIZE = 2*1024; + static const int TX_BUFFER_SIZE = 2 * 1024; static const quint8 crc_table[256]; // Types - typedef enum {STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxStateType; + typedef enum { STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS } RxStateType; // Variables QPointer io; - UAVObjectManager* objMngr; - QMutex* mutex; - QMap transMap; + UAVObjectManager *objMngr; + QMutex *mutex; + QMap transMap; quint8 rxBuffer[MAX_PACKET_LENGTH]; quint8 txBuffer[MAX_PACKET_LENGTH]; // Variables used by the receive state machine @@ -125,22 +124,22 @@ private: ComStats stats; bool useUDPMirror; - QUdpSocket * udpSocketTx; - QUdpSocket * udpSocketRx; + QUdpSocket *udpSocketTx; + QUdpSocket *udpSocketRx; QByteArray rxDataArray; // Methods - bool objectTransaction(UAVObject* obj, quint8 type, bool allInstances); + bool objectTransaction(UAVObject *obj, quint8 type, bool allInstances); bool processInputByte(quint8 rxbyte); - bool receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* data, qint32 length); - UAVObject* updateObject(quint32 objId, quint16 instId, quint8* data); - void updateAck(UAVObject* obj); - void updateNack(UAVObject* obj); + bool receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length); + UAVObject *updateObject(quint32 objId, quint16 instId, quint8 *data); + void updateAck(UAVObject *obj); + void updateNack(UAVObject *obj); bool transmitNack(quint32 objId); - bool transmitObject(UAVObject* obj, quint8 type, bool allInstances); - bool transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances); + bool transmitObject(UAVObject *obj, quint8 type, bool allInstances); + bool transmitSingleObject(UAVObject *obj, quint8 type, bool allInstances); quint8 updateCRC(quint8 crc, const quint8 data); - quint8 updateCRC(quint8 crc, const quint8* data, qint32 length); + quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length); }; #endif // UAVTALK_H diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk_global.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk_global.h index d442e99ad..819070dff 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk_global.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk_global.h @@ -10,18 +10,18 @@ * @brief The UAVTalk protocol plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.cpp index 15d024f98..398e0b016 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.cpp @@ -10,18 +10,18 @@ * @brief The UAVTalk protocol plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavtalkplugin.h" @@ -30,31 +30,26 @@ #include UAVTalkPlugin::UAVTalkPlugin() -{ - -} +{} UAVTalkPlugin::~UAVTalkPlugin() -{ - -} +{} /** - * Called once all the plugins which depend on us have been loaded - */ + * Called once all the plugins which depend on us have been loaded + */ void UAVTalkPlugin::extensionsInitialized() -{ -} +{} /** - * Called at startup, before any plugin which depends on us is initialized - */ -bool UAVTalkPlugin::initialize(const QStringList & arguments, QString * errorString) + * Called at startup, before any plugin which depends on us is initialized + */ +bool UAVTalkPlugin::initialize(const QStringList & arguments, QString *errorString) { // Done Q_UNUSED(arguments); Q_UNUSED(errorString); // Get UAVObjectManager instance - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); objMngr = pm->getObject(); // Create TelemetryManager @@ -71,9 +66,7 @@ bool UAVTalkPlugin::initialize(const QStringList & arguments, QString * errorStr } void UAVTalkPlugin::shutdown() -{ - -} +{} void UAVTalkPlugin::onDeviceConnect(QIODevice *dev) { diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.h index 7a5508078..e28acfc1f 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalkplugin.h @@ -10,18 +10,18 @@ * @brief The UAVTalk protocol plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef UAVTALKPLUGIN_H @@ -36,8 +36,7 @@ #include "telemetrymanager.h" #include "uavobjectmanager.h" -class UAVTALK_EXPORT UAVTalkPlugin: public ExtensionSystem::IPlugin -{ +class UAVTALK_EXPORT UAVTalkPlugin : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -45,7 +44,7 @@ public: ~UAVTalkPlugin(); void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); + bool initialize(const QStringList & arguments, QString *errorString); void shutdown(); protected slots: @@ -53,8 +52,8 @@ protected slots: void onDeviceDisconnect(); private: - UAVObjectManager* objMngr; - TelemetryManager* telMngr; + UAVObjectManager *objMngr; + TelemetryManager *telMngr; }; #endif // UAVTALKPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/uploader/SSP/port.cpp b/ground/openpilotgcs/src/plugins/uploader/SSP/port.cpp index 499c94323..54ff148c8 100644 --- a/ground/openpilotgcs/src/plugins/uploader/SSP/port.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/SSP/port.cpp @@ -26,20 +26,20 @@ */ #include "port.h" #include "delay.h" -port::port(PortSettings settings,QString name):mstatus(port::closed) +port::port(PortSettings settings, QString name) : mstatus(port::closed) { timer.start(); - sport = new QextSerialPort(name,settings, QextSerialPort::Polling); - if(sport->open(QIODevice::ReadWrite|QIODevice::Unbuffered)) - { - mstatus=port::open; - // sport->setDtr(); + sport = new QextSerialPort(name, settings, QextSerialPort::Polling); + if (sport->open(QIODevice::ReadWrite | QIODevice::Unbuffered)) { + mstatus = port::open; + // sport->setDtr(); + } else { + mstatus = port::error; } - else - mstatus=port::error; } -port::~port() { +port::~port() +{ sport->close(); } @@ -49,21 +49,20 @@ port::portstatus port::status() } int16_t port::pfSerialRead(void) { - char c[1]; - if( sport->bytesAvailable() ) - { - sport->read(c,1); - } - else return -1; + + if (sport->bytesAvailable()) { + sport->read(c, 1); + } else { return -1; } return (uint8_t)c[0]; } void port::pfSerialWrite(uint8_t c) { char cc[1]; - cc[0]=c; - sport->write(cc,1); + + cc[0] = c; + sport->write(cc, 1); } uint32_t port::pfGetTime(void) diff --git a/ground/openpilotgcs/src/plugins/uploader/SSP/port.h b/ground/openpilotgcs/src/plugins/uploader/SSP/port.h index 1521ab412..8cc87f48f 100644 --- a/ground/openpilotgcs/src/plugins/uploader/SSP/port.h +++ b/ground/openpilotgcs/src/plugins/uploader/SSP/port.h @@ -32,38 +32,37 @@ #include #include "common.h" -class port -{ +class port { public: - enum portstatus{open,closed,error}; - virtual int16_t pfSerialRead(void); // function to read a character from the serial input stream - virtual void pfSerialWrite( uint8_t ); // function to write a byte to be sent out the serial port + enum portstatus { open, closed, error }; + virtual int16_t pfSerialRead(void); // function to read a character from the serial input stream + virtual void pfSerialWrite(uint8_t); // function to write a byte to be sent out the serial port virtual uint32_t pfGetTime(void); - uint8_t retryCount; // how many times have we tried to transmit the 'send' packet - uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet - uint16_t max_retry; // Maximum number of retrys for a single transmit. - int32_t timeoutLen; // how long to wait for each retry to succeed - uint32_t timeout; // current timeout. when 'time' reaches this point we have timed out - uint8_t txSeqNo; // current 'send' packet sequence number - uint16_t rxBufPos; // current buffer position in the receive packet - uint16_t rxBufLen; // number of 'data' bytes in the buffer - uint8_t rxSeqNo; // current 'receive' packet number - uint16_t rxBufSize; // size of the receive buffer. - uint16_t txBufSize; // size of the transmit buffer. - uint8_t *txBuf; // transmit buffer. REquired to store a copy of packet data in case a retry is needed. - uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received. - uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host + uint8_t retryCount; // how many times have we tried to transmit the 'send' packet + uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet + uint16_t max_retry; // Maximum number of retrys for a single transmit. + int32_t timeoutLen; // how long to wait for each retry to succeed + uint32_t timeout; // current timeout. when 'time' reaches this point we have timed out + uint8_t txSeqNo; // current 'send' packet sequence number + uint16_t rxBufPos; // current buffer position in the receive packet + uint16_t rxBufLen; // number of 'data' bytes in the buffer + uint8_t rxSeqNo; // current 'receive' packet number + uint16_t rxBufSize; // size of the receive buffer. + uint16_t txBufSize; // size of the transmit buffer. + uint8_t *txBuf; // transmit buffer. REquired to store a copy of packet data in case a retry is needed. + uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received. + uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host // this is required when switching from the application to the bootloader // and vice-versa. This fixes the firwmare download timeout. // when this flag is set to true, the next time we send a packet we will first // send a synchronize packet. - ReceiveState InputState; - decodeState_ DecodeState; - uint16_t SendState; - uint16_t crc; - uint32_t RxError; - uint32_t TxError; - uint16_t flags; - port(PortSettings settings,QString name); + ReceiveState InputState; + decodeState_ DecodeState; + uint16_t SendState; + uint16_t crc; + uint32_t RxError; + uint32_t TxError; + uint16_t flags; + port(PortSettings settings, QString name); ~port(); portstatus status(); private: diff --git a/ground/openpilotgcs/src/plugins/uploader/SSP/qssp.cpp b/ground/openpilotgcs/src/plugins/uploader/SSP/qssp.cpp index b5eb5b1b7..517772ad9 100644 --- a/ground/openpilotgcs/src/plugins/uploader/SSP/qssp.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/SSP/qssp.cpp @@ -33,48 +33,47 @@ /** PRIVATE DEFINITIONS **/ -#define SYNC 225 // Sync character used in Serial Protocol -#define ESC 224 // ESC character used in Serial Protocol -#define ESC_SYNC 1 // ESC_SYNC character used in Serial Protocol -#define ACK_BIT 0x80 // Ack bit, bit 7 of sequence number, 1 = Acknowledge, 0 = -// new packet +#define SYNC 225 // Sync character used in Serial Protocol +#define ESC 224 // ESC character used in Serial Protocol +#define ESC_SYNC 1 // ESC_SYNC character used in Serial Protocol +#define ACK_BIT 0x80 // Ack bit, bit 7 of sequence number, 1 = Acknowledge, 0 = +// new packet // packet location definitions. -#define LENGTH 0 -#define SEQNUM 1 -#define DATA 2 +#define LENGTH 0 +#define SEQNUM 1 +#define DATA 2 // Make larger sized integers from smaller sized integers -#define MAKEWORD16( ub, lb ) ((uint16_t)0x0000 | ((uint16_t)(ub) << 8) | (uint16_t)(lb) ) -#define MAKEWORD32( uw, lw ) ((uint32_t)(0x0UL | ((uint32_t)(uw) << 16) | (uint32_t)(lw)) ) -#define MAKEWORD32B( b3, b2, b1, b0 ) ((uint32_t)((uint32_t)(b3)<< 24) | ((uint32_t)(b2)<<16) | ((uint32_t)(b1)<<8) | ((uint32_t)(b0) ) +#define MAKEWORD16(ub, lb) ((uint16_t)0x0000 | ((uint16_t)(ub) << 8) | (uint16_t)(lb)) +#define MAKEWORD32(uw, lw) ((uint32_t)(0x0UL | ((uint32_t)(uw) << 16) | (uint32_t)(lw))) +#define MAKEWORD32B(b3, b2, b1, b0) ((uint32_t)((uint32_t)(b3) << 24) | ((uint32_t)(b2) << 16) | ((uint32_t)(b1) << 8) | ((uint32_t)(b0)) // Used to extract smaller integers from larger sized intergers -#define LOWERBYTE( w ) (uint8_t)((w) & 0x00ff ) -#define UPPERBYTE( w ) (uint8_t)(((w) & 0xff00) >> 8 ) -#define UPPERWORD(lw) (uint16_t)(((lw) & 0xffff0000) >> 16 ) -#define LOWERWORD(lw) (uint16_t)((lw) & 0x0000ffff) +#define LOWERBYTE(w) (uint8_t)((w) & 0x00ff) +#define UPPERBYTE(w) (uint8_t)(((w) & 0xff00) >> 8) +#define UPPERWORD(lw) (uint16_t)(((lw) & 0xffff0000) >> 16) +#define LOWERWORD(lw) (uint16_t)((lw) & 0x0000ffff) // Macros to operate on a target and bitmask. -#define CLEARBIT( a, b ) ((a) = (a) & ~(b)) -#define SETBIT( a, b ) ((a) = (a) | (b) ) -#define TOGGLEBIT(a,b) ((a) = (a) ^ (b) ) +#define CLEARBIT(a, b) ((a) = (a) & ~(b)) +#define SETBIT(a, b) ((a) = (a) | (b)) +#define TOGGLEBIT(a, b) ((a) = (a) ^ (b)) // test bit macros operate using a bit mask. -#define ISBITSET( a, b ) ( ((a) & (b)) == (b) ? TRUE : FALSE ) -#define ISBITCLEAR( a, b) ( (~(a) & (b)) == (b) ? TRUE : FALSE ) - +#define ISBITSET(a, b) (((a) & (b)) == (b) ? TRUE : FALSE) +#define ISBITCLEAR(a, b) ((~(a) & (b)) == (b) ? TRUE : FALSE) /* Flag bit masks...*/ -#define SENT_SYNCH (0x01) -#define ACK_RECEIVED (0x02) -#define ACK_EXPECTED (0x04) +#define SENT_SYNCH (0x01) +#define ACK_RECEIVED (0x02) +#define ACK_EXPECTED (0x04) -#define SSP_AWAITING_ACK 0 -#define SSP_ACKED 1 -#define SSP_IDLE 2 +#define SSP_AWAITING_ACK 0 +#define SSP_ACKED 1 +#define SSP_IDLE 2 /** PRIVATE DATA **/ static const uint16_t CRC_TABLE[] = { @@ -131,27 +130,25 @@ static const uint16_t CRC_TABLE[] = { * Must be called before calling the Send or REceive process functions. */ -void qssp::ssp_Init(const PortConfig_t* const info) +void qssp::ssp_Init(const PortConfig_t *const info) { - - - thisport->maxRetryCount = info->max_retry; - thisport->timeoutLen = info->timeoutLen; + thisport->maxRetryCount = info->max_retry; + thisport->timeoutLen = info->timeoutLen; thisport->txBufSize = info->txBufSize; - thisport->rxBufSize = info->rxBufSize; - thisport->txBuf = info->txBuf; - thisport->rxBuf = info->rxBuf; + thisport->rxBufSize = info->rxBufSize; + thisport->txBuf = info->txBuf; + thisport->rxBuf = info->rxBuf; thisport->retryCount = 0; - thisport->sendSynch = FALSE; //TRUE; - thisport->rxSeqNo = 255; - thisport->txSeqNo = 255; - thisport->SendState = SSP_IDLE; - thisport->InputState =(ReceiveState)0; - thisport->DecodeState =(decodeState_)0; - thisport->TxError =0; - thisport->RxError =0; - thisport->txSeqNo =0; - thisport->rxSeqNo =0; + thisport->sendSynch = FALSE; // TRUE; + thisport->rxSeqNo = 255; + thisport->txSeqNo = 255; + thisport->SendState = SSP_IDLE; + thisport->InputState = (ReceiveState)0; + thisport->DecodeState = (decodeState_)0; + thisport->TxError = 0; + thisport->RxError = 0; + thisport->txSeqNo = 0; + thisport->rxSeqNo = 0; } /*! @@ -165,11 +162,11 @@ void qssp::ssp_Init(const PortConfig_t* const info) * \note * */ -int16_t qssp::ssp_SendProcess( ) +int16_t qssp::ssp_SendProcess() { int16_t value = SSP_TX_WAITING; - if (thisport->SendState == SSP_AWAITING_ACK ) { + if (thisport->SendState == SSP_AWAITING_ACK) { if (sf_CheckTimeout() == TRUE) { if (thisport->retryCount < thisport->maxRetryCount) { // Try again @@ -179,16 +176,17 @@ int16_t qssp::ssp_SendProcess( ) } else { // Give up, # of trys has exceded the limit value = SSP_TX_TIMEOUT; - CLEARBIT( thisport->flags, ACK_RECEIVED); + CLEARBIT(thisport->flags, ACK_RECEIVED); thisport->SendState = SSP_IDLE; - if (debug) - qDebug()<<"Send TimeOut!"; + if (debug) { + qDebug() << "Send TimeOut!"; + } } } else { value = SSP_TX_WAITING; } - } else if( thisport->SendState == SSP_ACKED ) { - SETBIT( thisport->flags, ACK_RECEIVED); + } else if (thisport->SendState == SSP_ACKED) { + SETBIT(thisport->flags, ACK_RECEIVED); value = SSP_TX_ACKED; thisport->SendState = SSP_IDLE; } else { @@ -231,13 +229,13 @@ int16_t qssp::ssp_ReceiveProcess() * */ -int16_t qssp::ssp_ReceiveByte() +int16_t qssp::ssp_ReceiveByte() { int16_t b; int16_t packet_status = SSP_RX_IDLE; b = thisport->pfSerialRead(); - if( b != -1 ) { + if (b != -1) { packet_status = sf_ReceiveState(b); } return packet_status; @@ -254,17 +252,17 @@ int16_t qssp::ssp_ReceiveByte() * \note * */ -uint16_t qssp::ssp_SendDataBlock(uint8_t *data, uint16_t length ) +uint16_t qssp::ssp_SendDataBlock(uint8_t *data, uint16_t length) { - int16_t packet_status = SSP_TX_WAITING; - uint16_t retval = FALSE; + int16_t packet_status = SSP_TX_WAITING; + uint16_t retval = FALSE; - packet_status = ssp_SendData(data, length ); // send the data - while( packet_status == SSP_TX_WAITING ) { // check the status - (void)ssp_ReceiveProcess(); // process any bytes received. - packet_status = ssp_SendProcess(); // check the send status + packet_status = ssp_SendData(data, length); // send the data + while (packet_status == SSP_TX_WAITING) { // check the status + (void)ssp_ReceiveProcess(); // process any bytes received. + packet_status = ssp_SendProcess(); // check the send status } - if( packet_status == SSP_TX_ACKED ) { // figure out what happened to the packet + if (packet_status == SSP_TX_ACKED) { // figure out what happened to the packet retval = TRUE; } else { retval = FALSE; @@ -284,61 +282,62 @@ uint16_t qssp::ssp_SendDataBlock(uint8_t *data, uint16_t length ) * \note * */ -int16_t qssp::ssp_SendData(const uint8_t *data, const uint16_t length ) +int16_t qssp::ssp_SendData(const uint8_t *data, const uint16_t length) { - int16_t value = SSP_TX_WAITING; - if( (length + 2) > thisport->txBufSize ) { + if ((length + 2) > thisport->txBufSize) { // TRYING to send too much data. value = SSP_TX_BUFOVERRUN; - } else if( thisport->SendState == SSP_IDLE ) { + } else if (thisport->SendState == SSP_IDLE) { #ifdef ACTIVE_SYNCH - if( thisport->sendSynch == TRUE ) { + if (thisport->sendSynch == TRUE) { sf_SendSynchPacket(); } #endif #ifdef SYNCH_SEND - if( length == 0 ) { + if (length == 0) { // TODO this method could allow a task/user to start a synchronisation step if a zero is mistakenly passed to this function. - // could add a check for a NULL data pointer, or use some sort of static flag that can only be accessed by a static function - // that must be called before calling this function. + // could add a check for a NULL data pointer, or use some sort of static flag that can only be accessed by a static function + // that must be called before calling this function. // we are attempting to send a synch packet - thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us + thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us SETBIT(thisport->flags, SENT_SYNCH); } else { // we are sending a data packet - CLEARBIT( thisport->txSeqNo, ACK_BIT ); // make sure we are not sending a ACK packet - thisport->txSeqNo++; // update the sequence number. - if( thisport->txSeqNo > 0x7F) { // check for sequence number rollover - thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, + CLEARBIT(thisport->txSeqNo, ACK_BIT); // make sure we are not sending a ACK packet + thisport->txSeqNo++; // update the sequence number. + if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover + thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, // zero is reserviced for synchronization requests } } #else - CLEARBIT( thisport->txSeqNo, ACK_BIT ); // make sure we are not sending a ACK packet - thisport->txSeqNo++; // update the sequence number. - if( thisport->txSeqNo > 0x7F) { // check for sequence number rollover - thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, + CLEARBIT(thisport->txSeqNo, ACK_BIT); // make sure we are not sending a ACK packet + thisport->txSeqNo++; // update the sequence number. + if (thisport->txSeqNo > 0x7F) { // check for sequence number rollover + thisport->txSeqNo = 1; // if we do have rollover then reset to 1 not zero, // zero is reserved for synchronization requests } -#endif - CLEARBIT( thisport->flags, ACK_RECEIVED); - thisport->SendState = SSP_AWAITING_ACK; +#endif // ifdef SYNCH_SEND + CLEARBIT(thisport->flags, ACK_RECEIVED); + thisport->SendState = SSP_AWAITING_ACK; value = SSP_TX_WAITING; - thisport->retryCount = 0; // zero out the retry counter for this transmission - sf_MakePacket( thisport->txBuf, data, length, thisport->txSeqNo ); - sf_SendPacket( ); // punch out the packet to the serial port - sf_SetSendTimeout( ); // do the timeout values - if (debug) - qDebug()<<"Sent DATA PACKET:"<txSeqNo; + thisport->retryCount = 0; // zero out the retry counter for this transmission + sf_MakePacket(thisport->txBuf, data, length, thisport->txSeqNo); + sf_SendPacket(); // punch out the packet to the serial port + sf_SetSendTimeout(); // do the timeout values + if (debug) { + qDebug() << "Sent DATA PACKET:" << thisport->txSeqNo; + } } else { // error we are already sending a packet. Need to wait for the current packet to be acked or timeout. value = SSP_TX_BUSY; - if (debug) - qDebug()<<"Error sending TX was busy"; + if (debug) { + qDebug() << "Error sending TX was busy"; + } } return value; } @@ -352,46 +351,47 @@ int16_t qssp::ssp_SendData(const uint8_t *data, const uint16_t length ) * \note * A. send a packet with a sequence number equal to zero * B. if timed out then: - * send synch packet again - * increment try counter - * if number of tries exceed maximum try limit then exit + * send synch packet again + * increment try counter + * if number of tries exceed maximum try limit then exit * C. goto A */ -uint16_t qssp::ssp_Synchronise( ) +uint16_t qssp::ssp_Synchronise() { - int16_t packet_status; - uint16_t retval = FALSE; + int16_t packet_status; + uint16_t retval = FALSE; #ifndef USE_SENDPACKET_DATA - thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us + thisport->txSeqNo = 0; // make this zero to cause the other end to re-synch with us SETBIT(thisport->flags, SENT_SYNCH); // TODO - should this be using ssp_SendPacketData()?? - sf_MakePacket( thisport->txBuf, NULL, 0, thisport->txSeqNo ); // construct the packet - sf_SendPacket( ); - sf_SetSendTimeout( ); + sf_MakePacket(thisport->txBuf, NULL, 0, thisport->txSeqNo); // construct the packet + sf_SendPacket(); + sf_SetSendTimeout(); thisport->SendState = SSP_AWAITING_ACK; packet_status = SSP_TX_WAITING; #else - packet_status = ssp_SendData( NULL, 0 ); + packet_status = ssp_SendData(NULL, 0); #endif - while( packet_status == SSP_TX_WAITING ) { // we loop until we time out. - (void)ssp_ReceiveProcess( ); // do the receive process - packet_status = ssp_SendProcess( ); // do the send process + while (packet_status == SSP_TX_WAITING) { // we loop until we time out. + (void)ssp_ReceiveProcess(); // do the receive process + packet_status = ssp_SendProcess(); // do the send process } thisport->sendSynch = FALSE; - switch( packet_status ) { + switch (packet_status) { case SSP_TX_ACKED: retval = TRUE; break; - case SSP_TX_BUSY: // intentional fall through. - case SSP_TX_TIMEOUT: // intentional fall through. + case SSP_TX_BUSY: // intentional fall through. + case SSP_TX_TIMEOUT: // intentional fall through. case SSP_TX_BUFOVERRUN: retval = FALSE; break; default: retval = FALSE; break; - }; + } + ; return retval; } @@ -411,8 +411,8 @@ void qssp::sf_SendPacket() // use the raw serial write function so the SYNC byte does not get 'escaped' thisport->pfSerialWrite(SYNC); - for( uint8_t x = 0; x < packetLen; x++ ) { - sf_write_byte(thisport->txBuf[x] ); + for (uint8_t x = 0; x < packetLen; x++) { + sf_write_byte(thisport->txBuf[x]); } thisport->retryCount++; } @@ -434,26 +434,25 @@ void qssp::sf_SendPacket() * 3. TODO: Should this function return an error if data length to be sent is greater th tx buffer size? * */ -void qssp::sf_MakePacket( uint8_t *txBuf, const uint8_t * pdata, uint16_t length, uint8_t seqNo ) +void qssp::sf_MakePacket(uint8_t *txBuf, const uint8_t *pdata, uint16_t length, uint8_t seqNo) { - uint16_t crc = 0xffff; - uint16_t bufPos = 0; - uint8_t b; + uint16_t crc = 0xffff; + uint16_t bufPos = 0; + uint8_t b; // add 1 for the seq. number txBuf[LENGTH] = length + 1; txBuf[SEQNUM] = seqNo; - crc = sf_crc16( crc, seqNo ); + crc = sf_crc16(crc, seqNo); - length = length + 2; // add two for the length and seqno bytes which are added before the loop. - for( bufPos = 2; bufPos < length; bufPos++ ) { - b = *pdata++; + length = length + 2; // add two for the length and seqno bytes which are added before the loop. + for (bufPos = 2; bufPos < length; bufPos++) { + b = *pdata++; txBuf[bufPos] = b; - crc = sf_crc16( crc, b ); // update CRC value + crc = sf_crc16(crc, b); // update CRC value } txBuf[bufPos++] = LOWERBYTE(crc); txBuf[bufPos] = UPPERBYTE(crc); - } /*! @@ -468,13 +467,14 @@ void qssp::sf_MakePacket( uint8_t *txBuf, const uint8_t * pdata, uint16_t length void qssp::sf_SendAckPacket(uint8_t seqNumber) { - uint8_t AckSeqNumber = SETBIT( seqNumber, ACK_BIT ); + uint8_t AckSeqNumber = SETBIT(seqNumber, ACK_BIT); // create the packet, note we pass AckSequenceNumber directly - sf_MakePacket( thisport->txBuf, NULL, 0, AckSeqNumber ); - sf_SendPacket( ); - if (debug) - qDebug()<<"Sent ACK PACKET:"<txBuf, NULL, 0, AckSeqNumber); + sf_SendPacket(); + if (debug) { + qDebug() << "Sent ACK PACKET:" << seqNumber; + } // we don't set the timeout for an ACK because we don't ACK our ACKs in this protocol } @@ -487,30 +487,30 @@ void qssp::sf_SendAckPacket(uint8_t seqNumber) * \note * */ -void qssp::sf_write_byte(uint8_t c ) +void qssp::sf_write_byte(uint8_t c) { - if( c == SYNC ) { // check for SYNC byte - thisport->pfSerialWrite( ESC ); // since we are not starting a packet we must ESCAPE the SYNCH byte - thisport->pfSerialWrite( ESC_SYNC ); // now send the escaped synch char - } else if( c == ESC ) { // Check for ESC character - thisport->pfSerialWrite( ESC ); // if it is, we need to send it twice - thisport->pfSerialWrite( ESC ); + if (c == SYNC) { // check for SYNC byte + thisport->pfSerialWrite(ESC); // since we are not starting a packet we must ESCAPE the SYNCH byte + thisport->pfSerialWrite(ESC_SYNC); // now send the escaped synch char + } else if (c == ESC) { // Check for ESC character + thisport->pfSerialWrite(ESC); // if it is, we need to send it twice + thisport->pfSerialWrite(ESC); } else { - thisport->pfSerialWrite( c ); // otherwise write the byte to serial port + thisport->pfSerialWrite(c); // otherwise write the byte to serial port } } /************************************************************************************************************ -* -* NAME: uint16_t ssp_crc16( uint16_t crc, uint16_t data ) -* DESCRIPTION: Uses crc_table to calculate new crc -* ARGUMENTS: -* arg1: crc -* arg2: data - byte to calculate into CRC -* RETURN: New crc -* CREATED: 5/8/02 -* -*************************************************************************************************************/ + * + * NAME: uint16_t ssp_crc16( uint16_t crc, uint16_t data ) + * DESCRIPTION: Uses crc_table to calculate new crc + * ARGUMENTS: + * arg1: crc + * arg2: data - byte to calculate into CRC + * RETURN: New crc + * CREATED: 5/8/02 + * + *************************************************************************************************************/ /*! * \brief calculates the new CRC value for 'data' * \param crc = current CRC value @@ -521,9 +521,9 @@ void qssp::sf_write_byte(uint8_t c ) * */ -uint16_t qssp::sf_crc16( uint16_t crc, uint8_t data ) +uint16_t qssp::sf_crc16(uint16_t crc, uint8_t data) { - return (crc >> 8) ^ CRC_TABLE[( crc ^ data ) & 0x00FF ]; + return (crc >> 8) ^ CRC_TABLE[(crc ^ data) & 0x00FF]; } @@ -538,7 +538,8 @@ uint16_t qssp::sf_crc16( uint16_t crc, uint8_t data ) void qssp::sf_SetSendTimeout() { - uint32_t timeout; + uint32_t timeout; + timeout = thisport->pfGetTime() + thisport->timeoutLen; thisport->timeout = timeout; } @@ -552,32 +553,34 @@ void qssp::sf_SetSendTimeout() * \note * */ -uint16_t qssp::sf_CheckTimeout() +uint16_t qssp::sf_CheckTimeout() { - uint16_t retval = FALSE; - uint32_t current_time; + uint16_t retval = FALSE; + uint32_t current_time; current_time = thisport->pfGetTime(); - if( current_time > thisport->timeout ) { + if (current_time > thisport->timeout) { retval = TRUE; } - if(retval) - if (debug) - qDebug()<<"timeout "<timeout; + if (retval) { + if (debug) { + qDebug() << "timeout " << current_time << thisport->timeout; + } + } return retval; } /**************************************************************************** - * NAME: sf_ReceiveState - * DESC: Implements the receive state handling code for escaped and unescaped data - * ARGS: thisport - which port to operate on - * c - incoming byte - * RETURN: - * CREATED: - * NOTES: - * 1. change from using pointer to functions. - ****************************************************************************/ +* NAME: sf_ReceiveState +* DESC: Implements the receive state handling code for escaped and unescaped data +* ARGS: thisport - which port to operate on +* c - incoming byte +* RETURN: +* CREATED: +* NOTES: +* 1. change from using pointer to functions. +****************************************************************************/ /*! * \brief implements the receive state handling code for escaped and unescaped data * \param thisport = which port to use @@ -587,46 +590,46 @@ uint16_t qssp::sf_CheckTimeout() * \note * */ -int16_t qssp::sf_ReceiveState(uint8_t c ) +int16_t qssp::sf_ReceiveState(uint8_t c) { - int16_t retval = SSP_RX_RECEIVING; + int16_t retval = SSP_RX_RECEIVING; - switch( thisport->InputState ) { + switch (thisport->InputState) { case state_unescaped_e: - if( c == SYNC ) { + if (c == SYNC) { thisport->DecodeState = decode_len1_e; - } else if ( c == ESC ) { + } else if (c == ESC) { thisport->InputState = state_escaped_e; } else { retval = sf_DecodeState(c); } - break; // end of unescaped state. - case state_escaped_e: + break; // end of unescaped state. + case state_escaped_e: thisport->InputState = state_unescaped_e; - if( c == SYNC ) { + if (c == SYNC) { thisport->DecodeState = decode_len1_e; - } else if (c == ESC_SYNC ) { + } else if (c == ESC_SYNC) { retval = sf_DecodeState(SYNC); } else { retval = sf_DecodeState(c); } - break; // end of the escaped state. - default: + break; // end of the escaped state. + default: break; } return retval; } /**************************************************************************** - * NAME: sf_DecodeState - * DESC: Implements the receive state finite state machine - * ARGS: thisport - which port to operate on - * c - incoming byte - * RETURN: - * CREATED: - * NOTES: - * 1. change from using pointer to functions. - ****************************************************************************/ +* NAME: sf_DecodeState +* DESC: Implements the receive state finite state machine +* ARGS: thisport - which port to operate on +* c - incoming byte +* RETURN: +* CREATED: +* NOTES: +* 1. change from using pointer to functions. +****************************************************************************/ /*! * \brief implements the receiving decoding state machine @@ -637,19 +640,20 @@ int16_t qssp::sf_ReceiveState(uint8_t c ) * \note * */ -int16_t qssp::sf_DecodeState( uint8_t c ) +int16_t qssp::sf_DecodeState(uint8_t c) { - int16_t retval; - switch( thisport->DecodeState ) { + int16_t retval; + + switch (thisport->DecodeState) { case decode_idle_e: // 'c' is ignored in this state as the only way to leave the idle state is // recognition of the SYNC byte in the sf_ReceiveState function. retval = SSP_RX_IDLE; break; case decode_len1_e: - thisport->rxBuf[LENGTH]= c; + thisport->rxBuf[LENGTH] = c; thisport->rxBufLen = c; - if( thisport->rxBufLen <= thisport->rxBufSize ) { + if (thisport->rxBufLen <= thisport->rxBufSize) { thisport->DecodeState = decode_seqNo_e; retval = SSP_RX_RECEIVING; } else { @@ -657,37 +661,37 @@ int16_t qssp::sf_DecodeState( uint8_t c ) retval = SSP_RX_IDLE; } break; - case decode_seqNo_e: + case decode_seqNo_e: thisport->rxBuf[SEQNUM] = c; thisport->crc = 0xffff; - thisport->rxBufLen--; // subtract 1 for the seq. no. + thisport->rxBufLen--; // subtract 1 for the seq. no. thisport->rxBufPos = 2; - thisport->crc = sf_crc16( thisport->crc, c ); - if( thisport->rxBufLen > 0 ) { + thisport->crc = sf_crc16(thisport->crc, c); + if (thisport->rxBufLen > 0) { thisport->DecodeState = decode_data_e; } else { thisport->DecodeState = decode_crc1_e; } retval = SSP_RX_RECEIVING; break; - case decode_data_e: - thisport->rxBuf[ (thisport->rxBufPos)++] = c; - thisport->crc = sf_crc16( thisport->crc, c ); - if( thisport->rxBufPos == (thisport->rxBufLen+2) ) { + case decode_data_e: + thisport->rxBuf[(thisport->rxBufPos)++] = c; + thisport->crc = sf_crc16(thisport->crc, c); + if (thisport->rxBufPos == (thisport->rxBufLen + 2)) { thisport->DecodeState = decode_crc1_e; } retval = SSP_RX_RECEIVING; break; - case decode_crc1_e: - thisport->crc = sf_crc16( thisport->crc, c ); + case decode_crc1_e: + thisport->crc = sf_crc16(thisport->crc, c); thisport->DecodeState = decode_crc2_e; retval = SSP_RX_RECEIVING; break; - case decode_crc2_e: + case decode_crc2_e: thisport->DecodeState = decode_idle_e; // verify the CRC value for the packet - if( sf_crc16( thisport->crc, c) == 0) { + if (sf_crc16(thisport->crc, c) == 0) { // TODO shouldn't the return value of sf_ReceivePacket() be checked? sf_ReceivePacket(); retval = SSP_RX_COMPLETE; @@ -696,8 +700,8 @@ int16_t qssp::sf_DecodeState( uint8_t c ) retval = SSP_RX_IDLE; } break; - default: - thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet. + default: + thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet. retval = SSP_RX_IDLE; break; } @@ -705,18 +709,18 @@ int16_t qssp::sf_DecodeState( uint8_t c ) } /************************************************************************************************************ -* -* NAME: int16_t sf_ReceivePacket( ) -* DESCRIPTION: Receive one packet, assumed that data is in rec.buff[] -* ARGUMENTS: -* RETURN: 0 . no new packet was received, could be ack or same packet -* 1 . new packet received -* SSP_PACKET_? -* SSP_PACKET_COMPLETE -* SSP_PACKET_ACK -* CREATED: 5/8/02 -* -*************************************************************************************************************/ + * + * NAME: int16_t sf_ReceivePacket( ) + * DESCRIPTION: Receive one packet, assumed that data is in rec.buff[] + * ARGUMENTS: + * RETURN: 0 . no new packet was received, could be ack or same packet + * 1 . new packet received + * SSP_PACKET_? + * SSP_PACKET_COMPLETE + * SSP_PACKET_ACK + * CREATED: 5/8/02 + * + *************************************************************************************************************/ /*! * \brief receive one packet. calls the callback function if needed. * \param thisport = which port to use @@ -732,78 +736,81 @@ int16_t qssp::sf_ReceivePacket() { int16_t value = FALSE; - if( ISBITSET(thisport->rxBuf[SEQNUM], ACK_BIT ) ) { - // Received an ACK packet, need to check if it matches the previous sent packet - if( ( thisport->rxBuf[SEQNUM] & 0x7F) == (thisport->txSeqNo & 0x7f)) { - // It matches the last packet sent by us - SETBIT( thisport->txSeqNo, ACK_BIT ); + if (ISBITSET(thisport->rxBuf[SEQNUM], ACK_BIT)) { + // Received an ACK packet, need to check if it matches the previous sent packet + if ((thisport->rxBuf[SEQNUM] & 0x7F) == (thisport->txSeqNo & 0x7f)) { + // It matches the last packet sent by us + SETBIT(thisport->txSeqNo, ACK_BIT); thisport->SendState = SSP_ACKED; value = FALSE; - if (debug) - qDebug()<<"Received ACK:"<<(thisport->txSeqNo & 0x7F); + if (debug) { + qDebug() << "Received ACK:" << (thisport->txSeqNo & 0x7F); + } } // else ignore the ACK packet } else { - // Received a 'data' packet, figure out what type of packet we received... - if( thisport->rxBuf[SEQNUM] == 0 ) { - if (debug) - qDebug()<<"Received SYNC Request"; + // Received a 'data' packet, figure out what type of packet we received... + if (thisport->rxBuf[SEQNUM] == 0) { + if (debug) { + qDebug() << "Received SYNC Request"; + } // Synchronize sequence number with host #ifdef ACTIVE_SYNCH thisport->sendSynch = TRUE; #endif - sf_SendAckPacket(thisport->rxBuf[SEQNUM] ); - thisport->rxSeqNo = 0; + sf_SendAckPacket(thisport->rxBuf[SEQNUM]); + thisport->rxSeqNo = 0; value = FALSE; - } else if( thisport->rxBuf[SEQNUM] == thisport->rxSeqNo ) { + } else if (thisport->rxBuf[SEQNUM] == thisport->rxSeqNo) { // Already seen this packet, just ack it, don't act on the packet. - sf_SendAckPacket(thisport->rxBuf[SEQNUM] ); + sf_SendAckPacket(thisport->rxBuf[SEQNUM]); value = FALSE; } else { - //New Packet + // New Packet thisport->rxSeqNo = thisport->rxBuf[SEQNUM]; // Let the application do something with the data/packet. // skip the first two bytes (length and seq. no.) in the buffer. - if (debug) - qDebug()<<"Received DATA PACKET seq="<rxSeqNo<<"Data="<<(uint8_t)thisport->rxBuf[2]<<(uint8_t)thisport->rxBuf[3]<<(uint8_t)thisport->rxBuf[4]; - pfCallBack( &(thisport->rxBuf[2]), thisport->rxBufLen); + if (debug) { + qDebug() << "Received DATA PACKET seq=" << thisport->rxSeqNo << "Data=" << (uint8_t)thisport->rxBuf[2] << (uint8_t)thisport->rxBuf[3] << (uint8_t)thisport->rxBuf[4]; + } + pfCallBack(&(thisport->rxBuf[2]), thisport->rxBufLen); // after we send the ACK, it is possible for the host to send a new packet. // Thus the application needs to copy the data and reset the receive buffer // inside of thisport->pfCallBack() - sf_SendAckPacket(thisport->rxBuf[SEQNUM] ); + sf_SendAckPacket(thisport->rxBuf[SEQNUM]); value = TRUE; } } return value; } -qssp::qssp(port * info,bool debug):debug(debug) +qssp::qssp(port *info, bool debug) : debug(debug) { - - thisport=info; - thisport->maxRetryCount = info->max_retry; - thisport->timeoutLen = info->timeoutLen; + thisport = info; + thisport->maxRetryCount = info->max_retry; + thisport->timeoutLen = info->timeoutLen; thisport->txBufSize = info->txBufSize; - thisport->rxBufSize = info->rxBufSize; - thisport->txBuf = info->txBuf; - thisport->rxBuf = info->rxBuf; + thisport->rxBufSize = info->rxBufSize; + thisport->txBuf = info->txBuf; + thisport->rxBuf = info->rxBuf; thisport->retryCount = 0; - thisport->sendSynch = FALSE; //TRUE; - thisport->rxSeqNo = 255; - thisport->txSeqNo = 255; - thisport->SendState = SSP_IDLE; - thisport->InputState =(ReceiveState)0; - thisport->DecodeState =(decodeState_)0; - thisport->TxError =0; - thisport->RxError =0; - thisport->txSeqNo =0; - thisport->rxSeqNo =0; + thisport->sendSynch = FALSE; // TRUE; + thisport->rxSeqNo = 255; + thisport->txSeqNo = 255; + thisport->SendState = SSP_IDLE; + thisport->InputState = (ReceiveState)0; + thisport->DecodeState = (decodeState_)0; + thisport->TxError = 0; + thisport->RxError = 0; + thisport->txSeqNo = 0; + thisport->rxSeqNo = 0; } -void qssp::pfCallBack( uint8_t * buf, uint16_t size) +void qssp::pfCallBack(uint8_t *buf, uint16_t size) { Q_UNUSED(size); - if (debug) - qDebug()<<"receive callback"<ssp_ReceiveProcess(); - sendstatus=this->ssp_SendProcess(); + while (!endthread) { + receivestatus = this->ssp_ReceiveProcess(); + sendstatus = this->ssp_SendProcess(); msleep(1); sendbufmutex.lock(); - if(datapending && receivestatus==SSP_TX_IDLE) - { - this->ssp_SendData(mbuf,msize); - datapending=false; + if (datapending && receivestatus == SSP_TX_IDLE) { + this->ssp_SendData(mbuf, msize); + datapending = false; } sendbufmutex.unlock(); - if(sendstatus==SSP_TX_ACKED) + if (sendstatus == SSP_TX_ACKED) { sendwait.wakeAll(); + } } - } -bool qsspt::sendData(uint8_t * buf,uint16_t size) +bool qsspt::sendData(uint8_t *buf, uint16_t size) { - if(datapending) + if (datapending) { return false; + } sendbufmutex.lock(); - datapending=true; - mbuf=buf; - msize=size; + datapending = true; + mbuf = buf; + msize = size; sendbufmutex.unlock(); msendwait.lock(); - sendwait.wait(&msendwait,10000); + sendwait.wait(&msendwait, 10000); msendwait.unlock(); return true; } -void qsspt::pfCallBack( uint8_t * buf, uint16_t size) +void qsspt::pfCallBack(uint8_t *buf, uint16_t size) { - if (debug) - qDebug()<<"receive callback"< -class delay : public QThread -{ +class delay : public QThread { public: static void msleep(unsigned long msecs) { diff --git a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp index f2e7b9c73..0cb91125a 100644 --- a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp @@ -53,10 +53,10 @@ void DeviceWidget::showEvent(QShowEvent *event) // Thit fitInView method should only be called now, once the // widget is shown, otherwise it cannot compute its values and // the result is usually a ahrsbargraph that is way too small - myDevice->gVDevice->fitInView(devicePic.rect(),Qt::KeepAspectRatio); + myDevice->gVDevice->fitInView(devicePic.rect(), Qt::KeepAspectRatio); } -void DeviceWidget::resizeEvent(QResizeEvent* event) +void DeviceWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); myDevice->gVDevice->fitInView(devicePic.rect(), Qt::KeepAspectRatio); @@ -73,17 +73,18 @@ void DeviceWidget::setDfu(DFUObject *dfu) } /** - Fills the various fields for the device - */ + Fills the various fields for the device + */ void DeviceWidget::populate() { int id = m_dfu->devices[deviceID].ID; + myDevice->lbldevID->setText(QString("Device ID: ") + QString::number(id, 16)); // DeviceID tells us what sort of HW we have detected: // display a nice icon: myDevice->gVDevice->scene()->clear(); myDevice->lblDevName->setText(deviceDescriptorStruct::idToBoardName(id)); - myDevice->lblHWRev->setText(QString(tr("HW Revision: "))+QString::number(id & 0x00FF, 16)); + myDevice->lblHWRev->setText(QString(tr("HW Revision: ")) + QString::number(id & 0x00FF, 16)); switch (id) { case 0x0101: @@ -109,24 +110,24 @@ void DeviceWidget::populate() } myDevice->gVDevice->scene()->addPixmap(devicePic); myDevice->gVDevice->setSceneRect(devicePic.rect()); - myDevice->gVDevice->fitInView(devicePic.rect(),Qt::KeepAspectRatio); + myDevice->gVDevice->fitInView(devicePic.rect(), Qt::KeepAspectRatio); bool r = m_dfu->devices[deviceID].Readable; bool w = m_dfu->devices[deviceID].Writable; myDevice->lblAccess->setText(QString("Flash access: ") + QString(r ? "R" : "-") + QString(w ? "W" : "-")); - myDevice->lblMaxCode->setText(QString("Max code size: ") +QString::number(m_dfu->devices[deviceID].SizeOfCode)); + myDevice->lblMaxCode->setText(QString("Max code size: ") + QString::number(m_dfu->devices[deviceID].SizeOfCode)); myDevice->lblCRC->setText(QString::number(m_dfu->devices[deviceID].FW_CRC)); myDevice->lblBLVer->setText(QString("BL version: ") + QString::number(m_dfu->devices[deviceID].BL_Version)); - int size=((OP_DFU::device)m_dfu->devices[deviceID]).SizeOfDesc; + int size = ((OP_DFU::device)m_dfu->devices[deviceID]).SizeOfDesc; m_dfu->enterDFU(deviceID); QByteArray desc = m_dfu->DownloadDescriptionAsBA(size); - if (! populateBoardStructuredDescription(desc)) { - //TODO + if (!populateBoardStructuredDescription(desc)) { + // TODO // desc was not a structured description QString str = m_dfu->DownloadDescription(size); - myDevice->lblDescription->setText(QString("Firmware custom description: ")+str.left(str.indexOf(QChar(255)))); + myDevice->lblDescription->setText(QString("Firmware custom description: ") + str.left(str.indexOf(QChar(255)))); QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); myDevice->lblCertified->setPixmap(pix); myDevice->lblCertified->setToolTip(tr("Custom Firmware Build")); @@ -139,9 +140,9 @@ void DeviceWidget::populate() } /** - Freezes the contents of the widget so that a user cannot - try to modify the contents - */ + Freezes the contents of the widget so that a user cannot + try to modify the contents + */ void DeviceWidget::freeze() { updateButtons(false); @@ -155,8 +156,7 @@ void DeviceWidget::updateButtons(bool enabled) myDevice->confirmCheckBox->setEnabled(false); myDevice->updateButton->setEnabled(false); myDevice->retrieveButton->setEnabled(false); - } - else { + } else { myDevice->description->setEnabled(true); // Load button (i.e. choose file) is always enabled myDevice->pbLoad->setEnabled(true); @@ -169,22 +169,20 @@ void DeviceWidget::updateButtons(bool enabled) } /** - Populates the widget field with the description in case - it is structured properly - */ + Populates the widget field with the description in case + it is structured properly + */ bool DeviceWidget::populateBoardStructuredDescription(QByteArray desc) { - if(UAVObjectUtilManager::descriptionToStructure(desc,onBoardDescription)) { + if (UAVObjectUtilManager::descriptionToStructure(desc, onBoardDescription)) { myDevice->lblGitTag->setText(onBoardDescription.gitHash); - myDevice->lblBuildDate->setText(onBoardDescription.gitDate.insert(4,"-").insert(7,"-")); - if(onBoardDescription.gitTag.startsWith("RELEASE",Qt::CaseSensitive)) { + myDevice->lblBuildDate->setText(onBoardDescription.gitDate.insert(4, "-").insert(7, "-")); + if (onBoardDescription.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { myDevice->lblDescription->setText(onBoardDescription.gitTag); QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); myDevice->lblCertified->setPixmap(pix); myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build")); - - } - else { + } else { myDevice->lblDescription->setText(onBoardDescription.gitTag); QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); myDevice->lblCertified->setPixmap(pix); @@ -201,10 +199,10 @@ bool DeviceWidget::populateBoardStructuredDescription(QByteArray desc) bool DeviceWidget::populateLoadedStructuredDescription(QByteArray desc) { - if(UAVObjectUtilManager::descriptionToStructure(desc,LoadedDescription)) { + if (UAVObjectUtilManager::descriptionToStructure(desc, LoadedDescription)) { myDevice->lblGitTagL->setText(LoadedDescription.gitHash); - myDevice->lblBuildDateL->setText( LoadedDescription.gitDate.insert(4, "-").insert(7, "-")); - if(LoadedDescription.gitTag.startsWith("RELEASE",Qt::CaseSensitive)) { + myDevice->lblBuildDateL->setText(LoadedDescription.gitDate.insert(4, "-").insert(7, "-")); + if (LoadedDescription.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { myDevice->lblDescritpionL->setText(LoadedDescription.gitTag); myDevice->description->setText(LoadedDescription.gitTag); QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); @@ -225,8 +223,8 @@ bool DeviceWidget::populateLoadedStructuredDescription(QByteArray desc) } /** - Updates status message for messages coming from DFU - */ + Updates status message for messages coming from DFU + */ void DeviceWidget::dfuStatus(QString str) { status(str, STATUSICON_RUNNING); @@ -238,11 +236,12 @@ void DeviceWidget::confirmCB(int value) } /** - Updates status message - */ + Updates status message + */ void DeviceWidget::status(QString str, StatusIcon ic) { QPixmap px; + myDevice->statusLabel->setText(str); switch (ic) { case STATUSICON_RUNNING: @@ -285,13 +284,13 @@ void DeviceWidget::loadFirmware() QByteArray desc = loadedFW.right(100); QPixmap px; - if (loadedFW.length()>m_dfu->devices[deviceID].SizeOfCode) { + if (loadedFW.length() > m_dfu->devices[deviceID].SizeOfCode) { myDevice->lblCRCL->setText(tr("Can't calculate, file too big for device")); } else { - myDevice->lblCRCL->setText( QString::number(DFUObject::CRCFromQBArray(loadedFW,m_dfu->devices[deviceID].SizeOfCode))); + myDevice->lblCRCL->setText(QString::number(DFUObject::CRCFromQBArray(loadedFW, m_dfu->devices[deviceID].SizeOfCode))); } - //myDevice->lblFirmwareSizeL->setText(QString("Firmware size: ")+QVariant(loadedFW.length()).toString()+ QString(" bytes")); + // myDevice->lblFirmwareSizeL->setText(QString("Firmware size: ")+QVariant(loadedFW.length()).toString()+ QString(" bytes")); if (populateLoadedStructuredDescription(desc)) { myDevice->confirmCheckBox->setChecked(true); myDevice->verticalGroupBox_loaded->setVisible(true); @@ -324,8 +323,8 @@ void DeviceWidget::loadFirmware() } /** - Sends a firmware to the device - */ + Sends a firmware to the device + */ void DeviceWidget::uploadFirmware() { // clear progress bar now @@ -339,9 +338,9 @@ void DeviceWidget::uploadFirmware() return; } - bool verify = false; + bool verify = false; /* TODO: does not work properly on current Bootloader! - if (m_dfu->devices[deviceID].Readable) + if (m_dfu->devices[deviceID].Readable) verify = true; */ @@ -351,11 +350,10 @@ void DeviceWidget::uploadFirmware() // Now do sanity checking: // - Check whether board type matches firmware: int board = m_dfu->devices[deviceID].ID; - int firmwareBoard = ((desc.at(12)&0xff)<<8) + (desc.at(13)&0xff); - if((board == 0x401 && firmwareBoard == 0x402) || - (board == 0x901 && firmwareBoard == 0x902) || // L3GD20 revo supports Revolution firmware - (board == 0x902 && firmwareBoard == 0x903)) // RevoMini1 supporetd by RevoMini2 firmware - { + int firmwareBoard = ((desc.at(12) & 0xff) << 8) + (desc.at(13) & 0xff); + if ((board == 0x401 && firmwareBoard == 0x402) || + (board == 0x901 && firmwareBoard == 0x902) || // L3GD20 revo supports Revolution firmware + (board == 0x902 && firmwareBoard == 0x903)) { // RevoMini1 supporetd by RevoMini2 firmware // These firmwares are designed to be backwards compatible } else if (firmwareBoard != board) { status("Error: firmware does not match board", STATUSICON_FAIL); @@ -363,8 +361,8 @@ void DeviceWidget::uploadFirmware() return; } // Check the firmware embedded in the file: - QByteArray firmwareHash = desc.mid(40,20); - QByteArray fileHash = QCryptographicHash::hash(loadedFW.left(loadedFW.length()-100), QCryptographicHash::Sha1); + QByteArray firmwareHash = desc.mid(40, 20); + QByteArray fileHash = QCryptographicHash::hash(loadedFW.left(loadedFW.length() - 100), QCryptographicHash::Sha1); if (firmwareHash != fileHash) { status("Error: firmware file corrupt", STATUSICON_FAIL); updateButtons(true); @@ -381,14 +379,13 @@ void DeviceWidget::uploadFirmware() // We don't know which device was used previously, so we // are cautious and reenter DFU for this deviceID: - if(!m_dfu->enterDFU(deviceID)) - { + if (!m_dfu->enterDFU(deviceID)) { emit uploadEnded(false); status("Error:Could not enter DFU mode", STATUSICON_FAIL); updateButtons(true); return; } - OP_DFU::Status ret=m_dfu->StatusRequest(); + OP_DFU::Status ret = m_dfu->StatusRequest(); qDebug() << m_dfu->StatusToString(ret); m_dfu->AbortOperation(); // Necessary, otherwise I get random failures. @@ -407,8 +404,8 @@ void DeviceWidget::uploadFirmware() } /** - Retrieves the firmware from the device - */ + Retrieves the firmware from the device + */ void DeviceWidget::downloadFirmware() { // clear progress bar now @@ -437,7 +434,7 @@ void DeviceWidget::downloadFirmware() connect(m_dfu, SIGNAL(downloadFinished()), this, SLOT(downloadFinished())); downloadedFirmware.clear(); // Empty the byte array - bool ret = m_dfu->DownloadFirmware(&downloadedFirmware,deviceID); + bool ret = m_dfu->DownloadFirmware(&downloadedFirmware, deviceID); if (!ret) { emit downloadEnded(false); @@ -447,12 +444,11 @@ void DeviceWidget::downloadFirmware() } status("Downloading, please wait...", STATUSICON_RUNNING); - return; } /** - Callback for the firmware download result - */ + Callback for the firmware download result + */ void DeviceWidget::downloadFinished() { disconnect(m_dfu, SIGNAL(downloadFinished()), this, SLOT(downloadFinished())); @@ -467,8 +463,8 @@ void DeviceWidget::downloadFinished() } /** - Callback for the firmware upload result - */ + Callback for the firmware upload result + */ void DeviceWidget::uploadFinished(OP_DFU::Status retstatus) { disconnect(m_dfu, SIGNAL(uploadFinished(OP_DFU::Status)), this, SLOT(uploadFinished(OP_DFU::Status))); @@ -480,31 +476,29 @@ void DeviceWidget::uploadFinished(OP_DFU::Status retstatus) status(QString("Upload failed with code: ") + m_dfu->StatusToString(retstatus).toLatin1().data(), STATUSICON_FAIL); updateButtons(true); return; - } else - if (!descriptionArray.isEmpty()) { - // We have a structured array to save - status(QString("Updating description"), STATUSICON_RUNNING); - repaint(); // Make sure the text above shows right away - retstatus = m_dfu->UploadDescription(descriptionArray); - if (retstatus != OP_DFU::Last_operation_Success) { - emit uploadEnded(false); - status(QString("Upload failed with code: ") + m_dfu->StatusToString(retstatus).toLatin1().data(), STATUSICON_FAIL); - updateButtons(true); - return; - } - - } else if (!myDevice->description->text().isEmpty()) { - // Fallback: we save the description field: - status(QString("Updating description"), STATUSICON_RUNNING); - repaint(); // Make sure the text above shows right away - retstatus = m_dfu->UploadDescription(myDevice->description->text()); - if (retstatus != OP_DFU::Last_operation_Success) { - emit uploadEnded(false); - status(QString("Upload failed with code: ") + m_dfu->StatusToString(retstatus).toLatin1().data(), STATUSICON_FAIL); - updateButtons(true); - return; - } + } else if (!descriptionArray.isEmpty()) { + // We have a structured array to save + status(QString("Updating description"), STATUSICON_RUNNING); + repaint(); // Make sure the text above shows right away + retstatus = m_dfu->UploadDescription(descriptionArray); + if (retstatus != OP_DFU::Last_operation_Success) { + emit uploadEnded(false); + status(QString("Upload failed with code: ") + m_dfu->StatusToString(retstatus).toLatin1().data(), STATUSICON_FAIL); + updateButtons(true); + return; } + } else if (!myDevice->description->text().isEmpty()) { + // Fallback: we save the description field: + status(QString("Updating description"), STATUSICON_RUNNING); + repaint(); // Make sure the text above shows right away + retstatus = m_dfu->UploadDescription(myDevice->description->text()); + if (retstatus != OP_DFU::Last_operation_Success) { + emit uploadEnded(false); + status(QString("Upload failed with code: ") + m_dfu->StatusToString(retstatus).toLatin1().data(), STATUSICON_FAIL); + updateButtons(true); + return; + } + } populate(); @@ -514,15 +508,15 @@ void DeviceWidget::uploadFinished(OP_DFU::Status retstatus) } /** - Slot to update the progress bar - */ + Slot to update the progress bar + */ void DeviceWidget::setProgress(int percent) { myDevice->progressBar->setValue(percent); } /** - *Opens an open file dialog. + * Opens an open file dialog. */ QString DeviceWidget::setOpenFileName() { @@ -531,25 +525,25 @@ QString DeviceWidget::setOpenFileName() QString fwDirectoryStr; QDir fwDirectory; - //Format filename for file chooser + // Format filename for file chooser #ifdef Q_OS_WIN - fwDirectoryStr=QCoreApplication::applicationDirPath(); - fwDirectory=QDir(fwDirectoryStr); - fwDirectory.cdUp(); - fwDirectory.cd("firmware"); - fwDirectoryStr=fwDirectory.absolutePath(); + fwDirectoryStr = QCoreApplication::applicationDirPath(); + fwDirectory = QDir(fwDirectoryStr); + fwDirectory.cdUp(); + fwDirectory.cd("firmware"); + fwDirectoryStr = fwDirectory.absolutePath(); #elif defined Q_OS_LINUX - fwDirectoryStr=QCoreApplication::applicationDirPath(); - fwDirectory=QDir(fwDirectoryStr); + fwDirectoryStr = QCoreApplication::applicationDirPath(); + fwDirectory = QDir(fwDirectoryStr); fwDirectory.cd("../../.."); - fwDirectoryStr=fwDirectory.absolutePath(); - fwDirectoryStr=fwDirectoryStr+"/fw_"+myDevice->lblBrdName->text().toLower()+"/fw_"+myDevice->lblBrdName->text().toLower()+".opfw"; + fwDirectoryStr = fwDirectory.absolutePath(); + fwDirectoryStr = fwDirectoryStr + "/fw_" + myDevice->lblBrdName->text().toLower() + "/fw_" + myDevice->lblBrdName->text().toLower() + ".opfw"; #elif defined Q_OS_MAC - fwDirectoryStr=QCoreApplication::applicationDirPath(); - fwDirectory=QDir(fwDirectoryStr); + fwDirectoryStr = QCoreApplication::applicationDirPath(); + fwDirectory = QDir(fwDirectoryStr); fwDirectory.cd("../../../../../.."); - fwDirectoryStr=fwDirectory.absolutePath(); - fwDirectoryStr=fwDirectoryStr+"/fw_"+myDevice->lblBrdName->text().toLower()+"/fw_"+myDevice->lblBrdName->text().toLower()+".opfw"; + fwDirectoryStr = fwDirectory.absolutePath(); + fwDirectoryStr = fwDirectoryStr + "/fw_" + myDevice->lblBrdName->text().toLower() + "/fw_" + myDevice->lblBrdName->text().toLower() + ".opfw"; #endif QString fileName = QFileDialog::getOpenFileName(this, tr("Select firmware file"), @@ -561,7 +555,7 @@ QString DeviceWidget::setOpenFileName() } /** - *Set the save file name + * Set the save file name */ QString DeviceWidget::setSaveFileName() { @@ -573,5 +567,6 @@ QString DeviceWidget::setSaveFileName() tr("Firmware Files (*.bin)"), &selectedFilter, options); + return fileName; } diff --git a/ground/openpilotgcs/src/plugins/uploader/devicewidget.h b/ground/openpilotgcs/src/plugins/uploader/devicewidget.h index b87988c16..489b81168 100644 --- a/ground/openpilotgcs/src/plugins/uploader/devicewidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/devicewidget.h @@ -45,16 +45,15 @@ #include "uploader_global.h" using namespace OP_DFU; -class UPLOADER_EXPORT DeviceWidget : public QWidget -{ +class UPLOADER_EXPORT DeviceWidget : public QWidget { Q_OBJECT public: DeviceWidget(QWidget *parent = 0); void setDeviceID(int devID); - void setDfu(DFUObject* dfu); + void setDfu(DFUObject *dfu); void populate(); void freeze(); - typedef enum { STATUSICON_OK, STATUSICON_RUNNING, STATUSICON_FAIL, STATUSICON_INFO} StatusIcon; + typedef enum { STATUSICON_OK, STATUSICON_RUNNING, STATUSICON_FAIL, STATUSICON_INFO } StatusIcon; QString setOpenFileName(); QString setSaveFileName(); @@ -93,7 +92,6 @@ public slots: protected: void showEvent(QShowEvent *event); void resizeEvent(QResizeEvent *event); - }; #endif // DEVICEWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/uploader/enums.h b/ground/openpilotgcs/src/plugins/uploader/enums.h index 2c89bdf1d..2b3f540e7 100644 --- a/ground/openpilotgcs/src/plugins/uploader/enums.h +++ b/ground/openpilotgcs/src/plugins/uploader/enums.h @@ -27,9 +27,8 @@ #ifndef ENUMS_H #define ENUMS_H -namespace uploader -{ - typedef enum { IAP_STATE_READY, IAP_STATE_STEP_1, IAP_STATE_STEP_2, IAP_STEP_RESET, IAP_STATE_BOOTLOADER} IAPStep; - typedef enum { WAITING_DISCONNECT, WAITING_CONNECT, JUMP_TO_BL, LOADING_FW, UPLOADING_FW, UPLOADING_DESC, BOOTING, SUCCESS, FAILURE} AutoUpdateStep; +namespace uploader { +typedef enum { IAP_STATE_READY, IAP_STATE_STEP_1, IAP_STATE_STEP_2, IAP_STEP_RESET, IAP_STATE_BOOTLOADER } IAPStep; +typedef enum { WAITING_DISCONNECT, WAITING_CONNECT, JUMP_TO_BL, LOADING_FW, UPLOADING_FW, UPLOADING_DESC, BOOTING, SUCCESS, FAILURE } AutoUpdateStep; } #endif // ENUMS_H diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp index 921473b7e..67a0a080f 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp @@ -33,44 +33,42 @@ using namespace OP_DFU; -DFUObject::DFUObject(bool _debug,bool _use_serial,QString portname): - debug(_debug),use_serial(_use_serial),mready(true) +DFUObject::DFUObject(bool _debug, bool _use_serial, QString portname) : + debug(_debug), use_serial(_use_serial), mready(true) { info = NULL; numberOfDevices = 0; qRegisterMetaType("Status"); - if(use_serial) - { + if (use_serial) { PortSettings settings; - settings.BaudRate=BAUD57600; - settings.DataBits=DATA_8; - settings.FlowControl=FLOW_OFF; - settings.Parity=PAR_NONE; - settings.StopBits=STOP_1; - settings.Timeout_Millisec=1000; - info = new port(settings,portname); - info->rxBuf = sspRxBuf; - info->rxBufSize = MAX_PACKET_DATA_LEN; - info->txBuf = sspTxBuf; - info->txBufSize = MAX_PACKET_DATA_LEN; - info->max_retry = 10; - info->timeoutLen = 1000; - if(info->status()!=port::open) - { + settings.BaudRate = BAUD57600; + settings.DataBits = DATA_8; + settings.FlowControl = FLOW_OFF; + settings.Parity = PAR_NONE; + settings.StopBits = STOP_1; + settings.Timeout_Millisec = 1000; + info = new port(settings, portname); + info->rxBuf = sspRxBuf; + info->rxBufSize = MAX_PACKET_DATA_LEN; + info->txBuf = sspTxBuf; + info->txBufSize = MAX_PACKET_DATA_LEN; + info->max_retry = 10; + info->timeoutLen = 1000; + if (info->status() != port::open) { cout << "Could not open serial port\n"; mready = false; return; } - serialhandle = new qsspt(info,debug); + serialhandle = new qsspt(info, debug); int count = 0; - while((serialhandle->ssp_Synchronise()==false) && (count < 10)) - { - if (debug) - qDebug()<<"SYNC failed, resending"; - count++; + while ((serialhandle->ssp_Synchronise() == false) && (count < 10)) { + if (debug) { + qDebug() << "SYNC failed, resending"; + } + count++; } if (count == 10) { mready = false; @@ -78,56 +76,54 @@ DFUObject::DFUObject(bool _debug,bool _use_serial,QString portname): } qDebug() << "SYNC Succeded"; serialhandle->start(); - } - else - { + } else { mready = false; QEventLoop m_eventloop; - QTimer::singleShot(200,&m_eventloop, SLOT(quit())); + QTimer::singleShot(200, &m_eventloop, SLOT(quit())); m_eventloop.exec(); QList devices; - devices = USBMonitor::instance()->availableDevices(0x20a0,-1,-1,USBMonitor::Bootloader); - if (devices.length()==1) { - if (hidHandle.open(1,devices.first().vendorID,devices.first().productID,0,0)==1) { - mready=true; - QTimer::singleShot(200,&m_eventloop, SLOT(quit())); + devices = USBMonitor::instance()->availableDevices(0x20a0, -1, -1, USBMonitor::Bootloader); + if (devices.length() == 1) { + if (hidHandle.open(1, devices.first().vendorID, devices.first().productID, 0, 0) == 1) { + mready = true; + QTimer::singleShot(200, &m_eventloop, SLOT(quit())); m_eventloop.exec(); - } else + } else { hidHandle.close(0); + } } else { // Wait for the board to appear on the USB bus: - USBSignalFilter filter(0x20a0,-1,-1,USBMonitor::Bootloader); - connect(&filter, SIGNAL(deviceDiscovered()),&m_eventloop, SLOT(quit())); - for(int x=0;x<4;++x) - { - qDebug()<<"OP_DFU trying to detect bootloader:"<availableDevices(0x20a0,-1,-1,USBMonitor::Bootloader); + devices = USBMonitor::instance()->availableDevices(0x20a0, -1, -1, USBMonitor::Bootloader); qDebug() << "Devices length: " << devices.length(); - if (devices.length()==1) { + if (devices.length() == 1) { qDebug() << "Opening device"; - if(hidHandle.open(1,devices.first().vendorID,devices.first().productID,0,0)==1) - { - QTimer::singleShot(200,&m_eventloop, SLOT(quit())); + if (hidHandle.open(1, devices.first().vendorID, devices.first().productID, 0, 0) == 1) { + QTimer::singleShot(200, &m_eventloop, SLOT(quit())); m_eventloop.exec(); - qDebug()<<"OP_DFU detected after delay"; - mready=true; + qDebug() << "OP_DFU detected after delay"; + mready = true; qDebug() << "Detected"; break; - } else + } else { hidHandle.close(0); + } } else { - qDebug() << devices.length() << " device(s) detected, don't know what to do!"; + qDebug() << devices.length() << " device(s) detected, don't know what to do!"; mready = false; } } } - } } @@ -141,16 +137,16 @@ DFUObject::~DFUObject() } else { hidHandle.close(0); } - } bool DFUObject::SaveByteArrayToFile(QString const & sfile, const QByteArray &array) { QFile file(sfile); - if (!file.open(QIODevice::WriteOnly)) - { - if(debug) - qDebug()<<"Can't open file"; + + if (!file.open(QIODevice::WriteOnly)) { + if (debug) { + qDebug() << "Can't open file"; + } return false; } file.write(array); @@ -159,72 +155,74 @@ bool DFUObject::SaveByteArrayToFile(QString const & sfile, const QByteArray &arr } /** - Tells the mainboard to enter DFU Mode. - */ + Tells the mainboard to enter DFU Mode. + */ bool DFUObject::enterDFU(int const &devNumber) { char buf[BUF_LEN]; - buf[0] =0x02; //reportID - buf[1] = OP_DFU::EnterDFU; //DFU Command - buf[2] = 0; //DFU Count - buf[3] = 0; //DFU Count - buf[4] = 0; //DFU Count - buf[5] = 0; //DFU Count - buf[6] = devNumber; //DFU Data0 - buf[7] = 1; //DFU Data1 - buf[8] = 1; //DFU Data2 - buf[9] = 1; //DFU Data3 + + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::EnterDFU; // DFU Command + buf[2] = 0; // DFU Count + buf[3] = 0; // DFU Count + buf[4] = 0; // DFU Count + buf[5] = 0; // DFU Count + buf[6] = devNumber; // DFU Data0 + buf[7] = 1; // DFU Data1 + buf[8] = 1; // DFU Data2 + buf[9] = 1; // DFU Data3 int result = sendData(buf, BUF_LEN); - if(result<1) + if (result < 1) { return false; - if(debug) + } + if (debug) { qDebug() << "EnterDFU: " << result << " bytes sent"; + } return true; } /** - Tells the board to get ready for an upload. It will in particular - erase the memory to make room for the data. You will have to query - its status to wait until erase is done before doing the actual upload. - */ -bool DFUObject::StartUpload(qint32 const & numberOfBytes, TransferTypes const & type,quint32 crc) + Tells the board to get ready for an upload. It will in particular + erase the memory to make room for the data. You will have to query + its status to wait until erase is done before doing the actual upload. + */ +bool DFUObject::StartUpload(qint32 const & numberOfBytes, TransferTypes const & type, quint32 crc) { int lastPacketCount; - qint32 numberOfPackets=numberOfBytes/4/14; - int pad=(numberOfBytes-numberOfPackets*4*14)/4; - if(pad==0) - { - lastPacketCount=14; - } - else - { + qint32 numberOfPackets = numberOfBytes / 4 / 14; + int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4; + + if (pad == 0) { + lastPacketCount = 14; + } else { ++numberOfPackets; - lastPacketCount=pad; + lastPacketCount = pad; } char buf[BUF_LEN]; - buf[0] =0x02;//reportID - buf[1] = setStartBit(OP_DFU::Upload);//DFU Command - buf[2] = numberOfPackets>>24;//DFU Count - buf[3] = numberOfPackets>>16;//DFU Count - buf[4] = numberOfPackets>>8;//DFU Count - buf[5] = numberOfPackets;//DFU Count - buf[6] = (int)type;//DFU Data0 - buf[7] = lastPacketCount;//DFU Data1 - buf[8] = crc>>24; - buf[9] = crc>>16; - buf[10] = crc>>8; + buf[0] = 0x02; // reportID + buf[1] = setStartBit(OP_DFU::Upload); // DFU Command + buf[2] = numberOfPackets >> 24; // DFU Count + buf[3] = numberOfPackets >> 16; // DFU Count + buf[4] = numberOfPackets >> 8; // DFU Count + buf[5] = numberOfPackets; // DFU Count + buf[6] = (int)type; // DFU Data0 + buf[7] = lastPacketCount; // DFU Data1 + buf[8] = crc >> 24; + buf[9] = crc >> 16; + buf[10] = crc >> 8; buf[11] = crc; - if(debug) - qDebug()<<"Number of packets:"<0) - { + } + if (result > 0) { return true; } return false; @@ -232,154 +230,148 @@ bool DFUObject::StartUpload(qint32 const & numberOfBytes, TransferTypes const & /** - Does the actual data upload to the board. Needs to be called once the - board is ready to accept data following a StartUpload command, and it is erased. - */ -bool DFUObject::UploadData(qint32 const & numberOfBytes, QByteArray & data) + Does the actual data upload to the board. Needs to be called once the + board is ready to accept data following a StartUpload command, and it is erased. + */ +bool DFUObject::UploadData(qint32 const & numberOfBytes, QByteArray & data) { int lastPacketCount; - qint32 numberOfPackets=numberOfBytes/4/14; - int pad=(numberOfBytes-numberOfPackets*4*14)/4; - if(pad==0) - { - lastPacketCount=14; - } - else - { + qint32 numberOfPackets = numberOfBytes / 4 / 14; + int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4; + + if (pad == 0) { + lastPacketCount = 14; + } else { ++numberOfPackets; - lastPacketCount=pad; + lastPacketCount = pad; + } + if (debug) { + qDebug() << "Start Uploading:" << numberOfPackets << "4Bytes"; } - if(debug) - qDebug()<<"Start Uploading:"<>24;//DFU Count - buf[3] = packetcount>>16;//DFU Count - buf[4] = packetcount>>8;//DFU Count - buf[5] = packetcount;//DFU Count - char *pointer=data.data(); - pointer=pointer+4*14*packetcount; - // qDebug()<<"Packet Number="<> 24; // DFU Count + buf[3] = packetcount >> 16; // DFU Count + buf[4] = packetcount >> 8; // DFU Count + buf[5] = packetcount; // DFU Count + char *pointer = data.data(); + pointer = pointer + 4 * 14 * packetcount; + // qDebug()<<"Packet Number="<append(buf+6,size); + result = receiveData(buf, BUF_LEN); + if (debug) { + qDebug() << result << " bytes received" << " Count=" << x << "-" << (int)buf[2] << ";" << (int)buf[3] << ";" << (int)buf[4] << ";" << (int)buf[5] << " Data=" << (int)buf[6] << ";" << (int)buf[7] << ";" << (int)buf[8] << ";" << (int)buf[9]; + } + if (x == numberOfPackets - 1) { + size = lastPacketCount * 4; + } else { + size = 14 * 4; + } + fw->append(buf + 6, size); } StatusRequest(); @@ -474,13 +467,14 @@ bool DFUObject::StartDownloadT(QByteArray *fw, qint32 const & numberOfBytes, Tra /** - Resets the device - */ + Resets the device + */ int DFUObject::ResetDevice(void) { char buf[BUF_LEN]; - buf[0] =0x02; //reportID - buf[1] = OP_DFU::Reset; //DFU Command + + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::Reset; // DFU Command buf[2] = 0; buf[3] = 0; buf[4] = 0; @@ -491,14 +485,15 @@ int DFUObject::ResetDevice(void) buf[9] = 0; return sendData(buf, BUF_LEN); - //return hidHandle.send(0,buf, BUF_LEN, 500); + // return hidHandle.send(0,buf, BUF_LEN, 500); } int DFUObject::AbortOperation(void) { char buf[BUF_LEN]; - buf[0] =0x02;//reportID - buf[1] = OP_DFU::Abort_Operation;//DFU Command + + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::Abort_Operation; // DFU Command buf[2] = 0; buf[3] = 0; buf[4] = 0; @@ -511,13 +506,14 @@ int DFUObject::AbortOperation(void) return sendData(buf, BUF_LEN); } /** - Starts the firmware (leaves bootloader and boots the main software) - */ + Starts the firmware (leaves bootloader and boots the main software) + */ int DFUObject::JumpToApp(bool safeboot, bool erase) { char buf[BUF_LEN]; - buf[0] =0x02;//reportID - buf[1] = OP_DFU::JumpFW;//DFU Command + + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::JumpFW; // DFU Command buf[2] = 0; buf[3] = 0; buf[4] = 0; @@ -533,37 +529,37 @@ int DFUObject::JumpToApp(bool safeboot, bool erase) buf[9] = 0; } if (erase) { - // force data flash clear - buf[10] = 0x00; - buf[11] = 0x00; - buf[12] = 0xFA; - buf[13] = 0x5F; + // force data flash clear + buf[10] = 0x00; + buf[11] = 0x00; + buf[12] = 0xFA; + buf[13] = 0x5F; - buf[14] = 0x00; - buf[15] = 0x00; - buf[16] = 0x00; - buf[17] = 0x01; + buf[14] = 0x00; + buf[15] = 0x00; + buf[16] = 0x00; + buf[17] = 0x01; - buf[18] = 0x00; - buf[19] = 0x00; - buf[20] = 0x00; - buf[21] = 0x00; - } else { - buf[10] = 0x00; - buf[11] = 0x00; - buf[12] = 0x00; - buf[13] = 0x00; + buf[18] = 0x00; + buf[19] = 0x00; + buf[20] = 0x00; + buf[21] = 0x00; + } else { + buf[10] = 0x00; + buf[11] = 0x00; + buf[12] = 0x00; + buf[13] = 0x00; - buf[14] = 0x00; - buf[15] = 0x00; - buf[16] = 0x00; - buf[17] = 0x00; + buf[14] = 0x00; + buf[15] = 0x00; + buf[16] = 0x00; + buf[17] = 0x00; - buf[18] = 0x00; - buf[19] = 0x00; - buf[20] = 0x00; - buf[21] = 0x00; - } + buf[18] = 0x00; + buf[19] = 0x00; + buf[20] = 0x00; + buf[21] = 0x00; + } return sendData(buf, BUF_LEN); } @@ -571,8 +567,9 @@ int DFUObject::JumpToApp(bool safeboot, bool erase) OP_DFU::Status DFUObject::StatusRequest() { char buf[BUF_LEN]; - buf[0] =0x02; //reportID - buf[1] = OP_DFU::Status_Request; //DFU Command + + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::Status_Request; // DFU Command buf[2] = 0; buf[3] = 0; buf[4] = 0; @@ -583,28 +580,29 @@ OP_DFU::Status DFUObject::StatusRequest() buf[9] = 0; int result = sendData(buf, BUF_LEN); - if(debug) + if (debug) { qDebug() << "StatusRequest: " << result << " bytes sent"; - result = receiveData(buf,BUF_LEN); - if(debug) - qDebug() << "StatusRequest: " << result << " bytes received"; - if(buf[1]==OP_DFU::Status_Rep) - { - return (OP_DFU::Status)buf[6]; } - else + result = receiveData(buf, BUF_LEN); + if (debug) { + qDebug() << "StatusRequest: " << result << " bytes received"; + } + if (buf[1] == OP_DFU::Status_Rep) { + return (OP_DFU::Status)buf[6]; + } else { return OP_DFU::abort; + } } /** - Ask the bootloader for the list of devices available - */ + Ask the bootloader for the list of devices available + */ bool DFUObject::findDevices() { devices.clear(); char buf[BUF_LEN]; - buf[0] =0x02; //reportID - buf[1] = OP_DFU::Req_Capabilities; //DFU Command + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::Req_Capabilities; // DFU Command buf[2] = 0; buf[3] = 0; buf[4] = 0; @@ -615,70 +613,68 @@ bool DFUObject::findDevices() buf[9] = 0; int result = sendData(buf, BUF_LEN); - if (result < 1) + if (result < 1) { return false; + } - result = receiveData(buf,BUF_LEN); - if (result < 1) + result = receiveData(buf, BUF_LEN); + if (result < 1) { return false; + } - numberOfDevices=buf[7]; - RWFlags=buf[8]; - RWFlags=RWFlags<<8 | buf[9]; + numberOfDevices = buf[7]; + RWFlags = buf[8]; + RWFlags = RWFlags << 8 | buf[9]; - if(buf[1]==OP_DFU::Rep_Capabilities) - { - for(int x=0;x>(x*2) & 1); - dev.Writable=(bool)(RWFlags>>(x*2+1) & 1); + dev.Readable = (bool)(RWFlags >> (x * 2) & 1); + dev.Writable = (bool)(RWFlags >> (x * 2 + 1) & 1); devices.append(dev); - buf[0] =0x02; //reportID - buf[1] = OP_DFU::Req_Capabilities; //DFU Command + buf[0] = 0x02; // reportID + buf[1] = OP_DFU::Req_Capabilities; // DFU Command buf[2] = 0; buf[3] = 0; buf[4] = 0; buf[5] = 0; - buf[6] = x+1; + buf[6] = x + 1; buf[7] = 0; buf[8] = 0; buf[9] = 0; int result = sendData(buf, BUF_LEN); - result = receiveData(buf,BUF_LEN); - devices[x].ID=buf[14]; - devices[x].ID=devices[x].ID<<8 | (quint8)buf[15]; - devices[x].BL_Version=buf[7]; - devices[x].SizeOfDesc=buf[8]; + result = receiveData(buf, BUF_LEN); + devices[x].ID = buf[14]; + devices[x].ID = devices[x].ID << 8 | (quint8)buf[15]; + devices[x].BL_Version = buf[7]; + devices[x].SizeOfDesc = buf[8]; quint32 aux; - aux=(quint8)buf[10]; - aux=aux<<8 |(quint8)buf[11]; - aux=aux<<8 |(quint8)buf[12]; - aux=aux<<8 |(quint8)buf[13]; + aux = (quint8)buf[10]; + aux = aux << 8 | (quint8)buf[11]; + aux = aux << 8 | (quint8)buf[12]; + aux = aux << 8 | (quint8)buf[13]; - devices[x].FW_CRC=aux; + devices[x].FW_CRC = aux; - aux=(quint8)buf[2]; - aux=aux<<8 |(quint8)buf[3]; - aux=aux<<8 |(quint8)buf[4]; - aux=aux<<8 |(quint8)buf[5]; - devices[x].SizeOfCode=aux; + aux = (quint8)buf[2]; + aux = aux << 8 | (quint8)buf[3]; + aux = aux << 8 | (quint8)buf[4]; + aux = aux << 8 | (quint8)buf[5]; + devices[x].SizeOfCode = aux; } - if(debug) - { + if (debug) { qDebug() << "Found " << numberOfDevices << " devices"; - for(int x=0;x0) + } + if (result > 0) { return true; + } return false; } // /** - Starts a firmware upload (asynchronous) - */ -bool DFUObject::UploadFirmware(const QString &sfile, const bool &verify,int device) + Starts a firmware upload (asynchronous) + */ +bool DFUObject::UploadFirmware(const QString &sfile, const bool &verify, int device) { - - if (isRunning()) + if (isRunning()) { return false; + } requestedOperation = OP_DFU::Upload; - requestFilename = sfile; + requestFilename = sfile; requestDevice = device; requestVerify = verify; start(); return true; } -OP_DFU::Status DFUObject::UploadFirmwareT(const QString &sfile, const bool &verify,int device) +OP_DFU::Status DFUObject::UploadFirmwareT(const QString &sfile, const bool &verify, int device) { OP_DFU::Status ret; - if (debug) - qDebug() <<"Starting Firmware Uploading..."; + if (debug) { + qDebug() << "Starting Firmware Uploading..."; + } QFile file(sfile); - if (!file.open(QIODevice::ReadOnly)) - { - if(debug) - qDebug()<<"Cant open file"; + if (!file.open(QIODevice::ReadOnly)) { + if (debug) { + qDebug() << "Cant open file"; + } return OP_DFU::abort; } QByteArray arr = file.readAll(); - if(debug) - qDebug()<<"Bytes Loaded="<"); - }else{ - bar.replace(i,1," "); + for (int i = 0; i < 50; i++) { + if (i < (percent / 2)) { + bar.replace(i, 1, "="); + } else if (i == (percent / 2)) { + bar.replace(i, 1, ">"); + } else { + bar.replace(i, 1, " "); } } - std::cout<< "\r"<> 2; // /4 Size passed in as a byte count, assumed to be a multiple of 4 + // Size = Size >> 2; // /4 Size passed in as a byte count, assumed to be a multiple of 4 - while(Size--) - { + while (Size--) { static const quint32 CrcTable[16] = { // Nibble lookup table for 0x04C11DB7 polynomial - 0x00000000,0x04C11DB7,0x09823B6E,0x0D4326D9,0x130476DC,0x17C56B6B,0x1A864DB2,0x1E475005, - 0x2608EDB8,0x22C9F00F,0x2F8AD6D6,0x2B4BCB61,0x350C9B64,0x31CD86D3,0x3C8EA00A,0x384FBDBD }; + 0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005, + 0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61, 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD + }; - Crc = Crc ^ *((quint32 *)Buffer); // Apply all 32-bits + Crc = Crc ^ *((quint32 *)Buffer); // Apply all 32-bits Buffer += 1; // Process 32-bits, 4 at a time, or 8 rounds Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // Assumes 32-bit reg, masking index to 4-bits - Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // 0x04C11DB7 Polynomial used in STM32 + Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // 0x04C11DB7 Polynomial used in STM32 Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; @@ -991,113 +997,114 @@ quint32 DFUObject::CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer) Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; } - return(Crc); + return Crc; } /** - Utility function - */ + Utility function + */ quint32 DFUObject::CRCFromQBArray(QByteArray array, quint32 Size) { - quint32 pad=Size-array.length(); - array.append(QByteArray(pad,255)); - quint32 t[Size/4]; - for(int x=0;xsendData((uint8_t*)data+1,size-1)) - { - if (debug) - qDebug() << "packet sent" << "data0" << ((uint8_t*)data+1)[0]; + if (serialhandle->sendData((uint8_t *)data + 1, size - 1)) { + if (debug) { + qDebug() << "packet sent" << "data0" << ((uint8_t *)data + 1)[0]; + } return size; } - if(debug) + if (debug) { qDebug() << "Serial send OVERRUN"; + } return -1; } /** - Receive data from the bootloader, either through the serial port - of through the HID handle, depending on the mode we're using - */ -int DFUObject::receiveData(void * data,int size) + Receive data from the bootloader, either through the serial port + of through the HID handle, depending on the mode we're using + */ +int DFUObject::receiveData(void *data, int size) { - if(!use_serial) - return hidHandle.receive(0,data, size, 10000); + if (!use_serial) { + return hidHandle.receive(0, data, size, 10000); + } // Serial Mode: int x; QTime time; time.start(); - while(true) - { - if((x=serialhandle->read_Packet(((char *) data)+1)!=-1) || time.elapsed()>10000) - { - if(time.elapsed()>10000) - qDebug()<<"____timeout"; + while (true) { + if ((x = serialhandle->read_Packet(((char *)data) + 1) != -1) || time.elapsed() > 10000) { + if (time.elapsed() > 10000) { + qDebug() << "____timeout"; + } return x; } } } -#define BOARD_ID_MB 1 -#define BOARD_ID_INS 2 -#define BOARD_ID_PIP 3 -#define BOARD_ID_CC 4 -#define BOARD_ID_REVO 9 +#define BOARD_ID_MB 1 +#define BOARD_ID_INS 2 +#define BOARD_ID_PIP 3 +#define BOARD_ID_CC 4 +#define BOARD_ID_REVO 9 /** - Gets the type of board connected - */ + Gets the type of board connected + */ OP_DFU::eBoardType DFUObject::GetBoardType(int boardNum) { OP_DFU::eBoardType brdType = eBoardUnkwn; // First of all, check what Board type we are talking to int board = devices[boardNum].ID; + qDebug() << "Board model: " << board; switch (board >> 8) { - case BOARD_ID_MB: // Mainboard family - brdType = eBoardMainbrd; - break; - case BOARD_ID_INS: // Inertial Nav - brdType = eBoardINS; - break; - case BOARD_ID_PIP: // PIP RF Modem - brdType = eBoardPip; - break; - case BOARD_ID_CC: // CopterControl family - brdType = eBoardCC; - break; - case BOARD_ID_REVO: // Revo board - brdType = eBoardRevo; - break; - } + case BOARD_ID_MB: // Mainboard family + brdType = eBoardMainbrd; + break; + case BOARD_ID_INS: // Inertial Nav + brdType = eBoardINS; + break; + case BOARD_ID_PIP: // PIP RF Modem + brdType = eBoardPip; + break; + case BOARD_ID_CC: // CopterControl family + brdType = eBoardCC; + break; + case BOARD_ID_REVO: // Revo board + brdType = eBoardRevo; + break; + } return brdType; } - - diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.h b/ground/openpilotgcs/src/plugins/uploader/op_dfu.h index 14615943d..8805fe477 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.h +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.h @@ -24,198 +24,190 @@ #include "SSP/qsspt.h" using namespace std; -#define BUF_LEN 64 +#define BUF_LEN 64 -#define MAX_PACKET_DATA_LEN 255 -#define MAX_PACKET_BUF_SIZE (1+1+MAX_PACKET_DATA_LEN+2) +#define MAX_PACKET_DATA_LEN 255 +#define MAX_PACKET_BUF_SIZE (1 + 1 + MAX_PACKET_DATA_LEN + 2) namespace OP_DFU { +enum TransferTypes { + FW, + Descript +}; - enum TransferTypes +enum CompareType { + crccompare, + bytetobytecompare +}; + +Q_ENUMS(Status) +enum Status { + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + idle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, // 12 + abort // 13 +}; + +enum Actions { + actionProgram, + actionProgramAndVerify, + actionDownload, + actionCompareAll, + actionCompareCrc, + actionListDevs, + actionStatusReq, + actionReset, + actionJump +}; + +enum Commands { + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep, // 12 +}; + +enum eBoardType { + eBoardUnkwn = 0, + eBoardMainbrd = 1, + eBoardINS, + eBoardPip = 3, + eBoardCC = 4, + eBoardRevo = 9, +}; + +struct device { + int ID; + quint32 FW_CRC; + int BL_Version; + int SizeOfDesc; + quint32 SizeOfCode; + bool Readable; + bool Writable; +}; + + +class DFUObject : public QThread { + Q_OBJECT; + +public: + static quint32 CRCFromQBArray(QByteArray array, quint32 Size); + // DFUObject(bool debug); + DFUObject(bool debug, bool use_serial, QString port); + + ~DFUObject(); + + // Service commands: + bool enterDFU(int const &devNumber); + bool findDevices(); + int JumpToApp(bool safeboot, bool erase); + int ResetDevice(void); + OP_DFU::Status StatusRequest(); + bool EndOperation(); + int AbortOperation(void); + bool ready() { - FW, - Descript - }; + return mready; + } - enum CompareType + // Upload (send to device) commands + OP_DFU::Status UploadDescription(QVariant description); + bool UploadFirmware(const QString &sfile, const bool &verify, int device); + + // Download (get from device) commands: + // DownloadDescription is synchronous + QString DownloadDescription(int const & numberOfChars); + QByteArray DownloadDescriptionAsBA(int const & numberOfChars); + // Asynchronous firmware download: initiates fw download, + // and a downloadFinished signal is emitted when download + // if finished: + bool DownloadFirmware(QByteArray *byteArray, int device); + + // Comparison functions (is this needed?) + OP_DFU::Status CompareFirmware(const QString &sfile, const CompareType &type, int device); + + bool SaveByteArrayToFile(QString const & file, QByteArray const &array); + + + // Variables: + QList devices; + int numberOfDevices; + int send_delay; + bool use_delay; + + // Helper functions: + QString StatusToString(OP_DFU::Status const & status); + static quint32 CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer); + OP_DFU::eBoardType GetBoardType(int boardNum); + + +signals: + void progressUpdated(int); + void downloadFinished(); + void uploadFinished(OP_DFU::Status); + void operationProgress(QString status); + +private: + // Generic variables: + bool debug; + bool use_serial; + bool mready; + int RWFlags; + qsspt *serialhandle; + int sendData(void *, int); + int receiveData(void *data, int size); + uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; + uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; + port *info; + + + // USB Bootloader: + pjrc_rawhid hidHandle; + int setStartBit(int command) { - crccompare, - bytetobytecompare - }; + return command | 0x20; + } - Q_ENUMS(Status) - enum Status - { - DFUidle,//0 - uploading,//1 - wrong_packet_received,//2 - too_many_packets,//3 - too_few_packets,//4 - Last_operation_Success,//5 - downloading,//6 - idle,//7 - Last_operation_failed,//8 - uploadingStarting,//9 - outsideDevCapabilities,//10 - CRC_Fail,//11 - failed_jump,//12 - abort//13 - }; + void CopyWords(char *source, char *destination, int count); + void printProgBar(int const & percent, QString const & label); + bool StartUpload(qint32 const &numberOfBytes, TransferTypes const & type, quint32 crc); + bool UploadData(qint32 const & numberOfPackets, QByteArray & data); - enum Actions - { - actionProgram, - actionProgramAndVerify, - actionDownload, - actionCompareAll, - actionCompareCrc, - actionListDevs, - actionStatusReq, - actionReset, - actionJump - }; - - enum Commands - { - Reserved,//0 - Req_Capabilities,//1 - Rep_Capabilities,//2 - EnterDFU,//3 - JumpFW,//4 - Reset,//5 - Abort_Operation,//6 - Upload,//7 - Op_END,//8 - Download_Req,//9 - Download,//10 - Status_Request,//11 - Status_Rep,//12 - - }; - - enum eBoardType - { - eBoardUnkwn = 0, - eBoardMainbrd = 1, - eBoardINS, - eBoardPip = 3, - eBoardCC = 4, - eBoardRevo = 9, - }; - - struct device - { - int ID; - quint32 FW_CRC; - int BL_Version; - int SizeOfDesc; - quint32 SizeOfCode; - bool Readable; - bool Writable; - }; - - - class DFUObject : public QThread - { - Q_OBJECT; - - public: - static quint32 CRCFromQBArray(QByteArray array, quint32 Size); - //DFUObject(bool debug); - DFUObject(bool debug,bool use_serial,QString port); - - ~DFUObject(); - - // Service commands: - bool enterDFU(int const &devNumber); - bool findDevices(); - int JumpToApp(bool safeboot, bool erase); - int ResetDevice(void); - OP_DFU::Status StatusRequest(); - bool EndOperation(); - int AbortOperation(void); - bool ready() { return mready; } - - // Upload (send to device) commands - OP_DFU::Status UploadDescription(QVariant description); - bool UploadFirmware(const QString &sfile, const bool &verify,int device); - - // Download (get from device) commands: - // DownloadDescription is synchronous - QString DownloadDescription(int const & numberOfChars); - QByteArray DownloadDescriptionAsBA(int const & numberOfChars); - // Asynchronous firmware download: initiates fw download, - // and a downloadFinished signal is emitted when download - // if finished: - bool DownloadFirmware(QByteArray *byteArray, int device); - - // Comparison functions (is this needed?) - OP_DFU::Status CompareFirmware(const QString &sfile, const CompareType &type,int device); - - bool SaveByteArrayToFile(QString const & file,QByteArray const &array); - - - - // Variables: - QList devices; - int numberOfDevices; - int send_delay; - bool use_delay; - - // Helper functions: - QString StatusToString(OP_DFU::Status const & status); - static quint32 CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer); - OP_DFU::eBoardType GetBoardType(int boardNum); - - - - signals: - void progressUpdated(int); - void downloadFinished(); - void uploadFinished(OP_DFU::Status); - void operationProgress(QString status); - - private: - // Generic variables: - bool debug; - bool use_serial; - bool mready; - int RWFlags; - qsspt * serialhandle; - int sendData(void*,int); - int receiveData(void * data,int size); - uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; - uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; - port * info; - - - // USB Bootloader: - pjrc_rawhid hidHandle; - int setStartBit(int command){ return command|0x20; } - - void CopyWords(char * source, char* destination, int count); - void printProgBar( int const & percent,QString const& label); - bool StartUpload(qint32 const &numberOfBytes, TransferTypes const & type,quint32 crc); - bool UploadData(qint32 const & numberOfPackets,QByteArray & data); - - // Thread management: - // Same as startDownload except that we store in an external array: - bool StartDownloadT(QByteArray *fw, qint32 const & numberOfBytes, TransferTypes const & type); - OP_DFU::Status UploadFirmwareT(const QString &sfile, const bool &verify,int device); - QMutex mutex; - OP_DFU::Commands requestedOperation; - qint32 requestSize; - OP_DFU::TransferTypes requestTransferType; - QByteArray *requestStorage; - QString requestFilename; - bool requestVerify; - int requestDevice; - - protected: - void run();// Executes the upload or download operations - - }; + // Thread management: + // Same as startDownload except that we store in an external array: + bool StartDownloadT(QByteArray *fw, qint32 const & numberOfBytes, TransferTypes const & type); + OP_DFU::Status UploadFirmwareT(const QString &sfile, const bool &verify, int device); + QMutex mutex; + OP_DFU::Commands requestedOperation; + qint32 requestSize; + OP_DFU::TransferTypes requestTransferType; + QByteArray *requestStorage; + QString requestFilename; + bool requestVerify; + int requestDevice; +protected: + void run(); // Executes the upload or download operations +}; } Q_DECLARE_METATYPE(OP_DFU::Status) diff --git a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp index 539a3042f..904080ef5 100644 --- a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp @@ -44,22 +44,22 @@ void RunningDeviceWidget::showEvent(QShowEvent *event) // Thit fitInView method should only be called now, once the // widget is shown, otherwise it cannot compute its values and // the result is usually a ahrsbargraph that is way too small. - myDevice->devicePicture->fitInView(devicePic.rect(),Qt::KeepAspectRatio); + myDevice->devicePicture->fitInView(devicePic.rect(), Qt::KeepAspectRatio); } -void RunningDeviceWidget::resizeEvent(QResizeEvent* event) +void RunningDeviceWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); myDevice->devicePicture->fitInView(devicePic.rect(), Qt::KeepAspectRatio); } /** - Fills the various fields for the device - */ + Fills the various fields for the device + */ void RunningDeviceWidget::populate() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectUtilManager* utilMngr = pm->getObject(); + UAVObjectUtilManager *utilMngr = pm->getObject(); int id = utilMngr->getBoardModel(); myDevice->lblDeviceID->setText(QString("Device ID: ") + QString::number(id, 16)); @@ -95,20 +95,19 @@ void RunningDeviceWidget::populate() } myDevice->devicePicture->scene()->addPixmap(devicePic); myDevice->devicePicture->setSceneRect(devicePic.rect()); - myDevice->devicePicture->fitInView(devicePic.rect(),Qt::KeepAspectRatio); + myDevice->devicePicture->fitInView(devicePic.rect(), Qt::KeepAspectRatio); QString serial = utilMngr->getBoardCPUSerial().toHex(); myDevice->CPUSerial->setText(serial); QByteArray description = utilMngr->getBoardDescription(); deviceDescriptorStruct devDesc; - if(UAVObjectUtilManager::descriptionToStructure(description, devDesc)) { - if(devDesc.gitTag.startsWith("RELEASE",Qt::CaseSensitive)) { + if (UAVObjectUtilManager::descriptionToStructure(description, devDesc)) { + if (devDesc.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { myDevice->lblFWTag->setText(QString("Firmware tag: ") + devDesc.gitTag); QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); myDevice->lblCertified->setPixmap(pix); myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build")); - } else { myDevice->lblFWTag->setText(QString("Firmware tag: ") + devDesc.gitTag); QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); @@ -116,7 +115,7 @@ void RunningDeviceWidget::populate() myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build")); } myDevice->lblGitCommitTag->setText("Git commit hash: " + devDesc.gitHash); - myDevice->lblFWDate->setText(QString("Firmware date: ") + devDesc.gitDate.insert(4,"-").insert(7,"-")); + myDevice->lblFWDate->setText(QString("Firmware date: ") + devDesc.gitDate.insert(4, "-").insert(7, "-")); } else { myDevice->lblFWTag->setText(QString("Firmware tag: ") + QString(description).left(QString(description).indexOf(QChar(255)))); myDevice->lblGitCommitTag->setText("Git commit tag: Unknown"); diff --git a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.h b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.h index 15a3d5b71..44fcdc944 100644 --- a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.h @@ -41,29 +41,27 @@ #include "uavobjectutilmanager.h" #include "uploader_global.h" -class UPLOADER_EXPORT RunningDeviceWidget : public QWidget -{ +class UPLOADER_EXPORT RunningDeviceWidget : public QWidget { Q_OBJECT public: - RunningDeviceWidget( QWidget *parent = 0); + RunningDeviceWidget(QWidget *parent = 0); void populate(); void freeze(); QString setOpenFileName(); QString setSaveFileName(); - typedef enum { STATUSICON_OK, STATUSICON_RUNNING, STATUSICON_FAIL, STATUSICON_INFO} StatusIcon; + typedef enum { STATUSICON_OK, STATUSICON_RUNNING, STATUSICON_FAIL, STATUSICON_INFO } StatusIcon; private: Ui_runningDeviceWidget *myDevice; int deviceID; QPixmap devicePic; - //void status(QString str, StatusIcon ic); + // void status(QString str, StatusIcon ic); signals: protected: void showEvent(QShowEvent *event); void resizeEvent(QResizeEvent *event); - }; #endif // RUNNINGDEVICEWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadget.cpp index 5481753e9..bc52ab2de 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadget.cpp @@ -28,11 +28,10 @@ #include "uploadergadgetwidget.h" #include "uploadergadgetconfiguration.h" -UploaderGadget::UploaderGadget(QString classId, UploaderGadgetWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ -} +UploaderGadget::UploaderGadget(QString classId, UploaderGadgetWidget *widget, QWidget *parent) : + IUAVGadget(classId, parent), + m_widget(widget) +{} UploaderGadget::~UploaderGadget() { @@ -42,10 +41,9 @@ UploaderGadget::~UploaderGadget() * Loads a configuration. * */ -void UploaderGadget::loadConfiguration(IUAVGadgetConfiguration* config) +void UploaderGadget::loadConfiguration(IUAVGadgetConfiguration *config) { Q_UNUSED(config); /* UploaderGadgetConfiguration *m = qobject_cast< UploaderGadgetConfiguration*>(config); - */ + */ } - diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadget.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadget.h index 440c9d8e3..ee195bc32 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadget.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadget.h @@ -39,19 +39,20 @@ class UploaderGadgetWidget; using namespace Core; -class UPLOADER_EXPORT UploaderGadget : public Core::IUAVGadget -{ +class UPLOADER_EXPORT UploaderGadget : public Core::IUAVGadget { Q_OBJECT public: UploaderGadget(QString classId, UploaderGadgetWidget *widget, QWidget *parent = 0); ~UploaderGadget(); - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); + QWidget *widget() + { + return m_widget; + } + void loadConfiguration(IUAVGadgetConfiguration *config); private: UploaderGadgetWidget *m_widget; }; #endif // UPLOADERGADGET_H - diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.cpp index 3c5b6d7c7..91e08c15f 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.cpp @@ -32,7 +32,7 @@ * Loads a saved configuration or defaults if non exist. * */ -UploaderGadgetConfiguration::UploaderGadgetConfiguration(QString classId, QSettings* qSettings, QObject *parent) : +UploaderGadgetConfiguration::UploaderGadgetConfiguration(QString classId, QSettings *qSettings, QObject *parent) : IUAVGadgetConfiguration(classId, parent), m_defaultPort("Unknown"), m_defaultSpeed(BAUD19200), @@ -41,37 +41,34 @@ UploaderGadgetConfiguration::UploaderGadgetConfiguration(QString classId, QSetti m_defaultParity(PAR_NONE), m_defaultStopBits(STOP_1), m_defaultTimeOut(5000) - { - //if a saved configuration exists load it - if(qSettings != 0) { + // if a saved configuration exists load it + if (qSettings != 0) { BaudRateType speed; DataBitsType databits; FlowType flow; ParityType parity; StopBitsType stopbits; - int ispeed = qSettings->value("defaultSpeed").toInt(); + int ispeed = qSettings->value("defaultSpeed").toInt(); int idatabits = qSettings->value("defaultDataBits").toInt(); - int iflow = qSettings->value("defaultFlow").toInt(); - int iparity = qSettings->value("defaultParity").toInt(); + int iflow = qSettings->value("defaultFlow").toInt(); + int iparity = qSettings->value("defaultParity").toInt(); int istopbits = qSettings->value("defaultStopBits").toInt(); - QString port = qSettings->value("defaultPort").toString(); - - databits=(DataBitsType) idatabits; - flow=(FlowType)iflow; - parity=(ParityType)iparity; - stopbits=(StopBitsType)istopbits; - speed=(BaudRateType)ispeed; - m_defaultPort=port; - m_defaultSpeed=speed; - m_defaultDataBits=databits; - m_defaultFlow=flow; - m_defaultParity=parity; - m_defaultStopBits=stopbits; + QString port = qSettings->value("defaultPort").toString(); + databits = (DataBitsType)idatabits; + flow = (FlowType)iflow; + parity = (ParityType)iparity; + stopbits = (StopBitsType)istopbits; + speed = (BaudRateType)ispeed; + m_defaultPort = port; + m_defaultSpeed = speed; + m_defaultDataBits = databits; + m_defaultFlow = flow; + m_defaultParity = parity; + m_defaultStopBits = stopbits; } - } /** @@ -82,12 +79,12 @@ IUAVGadgetConfiguration *UploaderGadgetConfiguration::clone() { UploaderGadgetConfiguration *m = new UploaderGadgetConfiguration(this->classId()); - m->m_defaultSpeed=m_defaultSpeed; - m->m_defaultDataBits=m_defaultDataBits; - m->m_defaultFlow=m_defaultFlow; - m->m_defaultParity=m_defaultParity; - m->m_defaultStopBits=m_defaultStopBits; - m->m_defaultPort=m_defaultPort; + m->m_defaultSpeed = m_defaultSpeed; + m->m_defaultDataBits = m_defaultDataBits; + m->m_defaultFlow = m_defaultFlow; + m->m_defaultParity = m_defaultParity; + m->m_defaultStopBits = m_defaultStopBits; + m->m_defaultPort = m_defaultPort; return m; } @@ -95,7 +92,8 @@ IUAVGadgetConfiguration *UploaderGadgetConfiguration::clone() * Saves a configuration. * */ -void UploaderGadgetConfiguration::saveConfig(QSettings* qSettings) const { +void UploaderGadgetConfiguration::saveConfig(QSettings *qSettings) const +{ qSettings->setValue("defaultSpeed", m_defaultSpeed); qSettings->setValue("defaultDataBits", m_defaultDataBits); qSettings->setValue("defaultFlow", m_defaultFlow); diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.h index 78bc5fb5b..558cb96ae 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetconfiguration.h @@ -34,31 +34,72 @@ using namespace Core; -class UPLOADER_EXPORT UploaderGadgetConfiguration : public IUAVGadgetConfiguration -{ -Q_OBJECT +class UPLOADER_EXPORT UploaderGadgetConfiguration : public IUAVGadgetConfiguration { + Q_OBJECT public: - explicit UploaderGadgetConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); + explicit UploaderGadgetConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0); - //set port configuration functions - void setSpeed(BaudRateType speed) {m_defaultSpeed=speed;} - void setDataBits(DataBitsType databits) {m_defaultDataBits=databits;} - void setFlow(FlowType flow) {m_defaultFlow=flow;} - void setParity(ParityType parity) {m_defaultParity=parity;} - void setStopBits(StopBitsType stopbits) {m_defaultStopBits=stopbits;} - void setPort(QString port){m_defaultPort=port;} - void setTimeOut(long timeout){m_defaultTimeOut=timeout;} + // set port configuration functions + void setSpeed(BaudRateType speed) + { + m_defaultSpeed = speed; + } + void setDataBits(DataBitsType databits) + { + m_defaultDataBits = databits; + } + void setFlow(FlowType flow) + { + m_defaultFlow = flow; + } + void setParity(ParityType parity) + { + m_defaultParity = parity; + } + void setStopBits(StopBitsType stopbits) + { + m_defaultStopBits = stopbits; + } + void setPort(QString port) + { + m_defaultPort = port; + } + void setTimeOut(long timeout) + { + m_defaultTimeOut = timeout; + } - //get port configuration functions - BaudRateType Speed() {return m_defaultSpeed;} - DataBitsType DataBits() {return m_defaultDataBits;} - FlowType Flow() {return m_defaultFlow;} - ParityType Parity() {return m_defaultParity;} - StopBitsType StopBits() {return m_defaultStopBits;} - QString Port(){return m_defaultPort;} - long TimeOut(){return m_defaultTimeOut;} + // get port configuration functions + BaudRateType Speed() + { + return m_defaultSpeed; + } + DataBitsType DataBits() + { + return m_defaultDataBits; + } + FlowType Flow() + { + return m_defaultFlow; + } + ParityType Parity() + { + return m_defaultParity; + } + StopBitsType StopBits() + { + return m_defaultStopBits; + } + QString Port() + { + return m_defaultPort; + } + long TimeOut() + { + return m_defaultTimeOut; + } - void saveConfig(QSettings* settings) const; + void saveConfig(QSettings *settings) const; IUAVGadgetConfiguration *clone(); private: diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.cpp index f69735b72..35733ce6c 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.cpp @@ -33,24 +33,23 @@ #include "uploadergadgetwidget.h" UploaderGadgetFactory::UploaderGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("Uploader"), tr("Uploader"), parent),isautocapable(false) -{ -} + IUAVGadgetFactory(QString("Uploader"), tr("Uploader"), parent), isautocapable(false) +{} UploaderGadgetFactory::~UploaderGadgetFactory() -{ -} +{} -Core::IUAVGadget* UploaderGadgetFactory::createGadget(QWidget *parent) +Core::IUAVGadget *UploaderGadgetFactory::createGadget(QWidget *parent) { - UploaderGadgetWidget* gadgetWidget = new UploaderGadgetWidget(parent); - isautocapable=gadgetWidget->autoUpdateCapable(); - connect(this,SIGNAL(autoUpdate()),gadgetWidget,SLOT(autoUpdate())); - connect(gadgetWidget,SIGNAL(autoUpdateSignal(uploader::AutoUpdateStep,QVariant)),this,SIGNAL(autoUpdateSignal(uploader::AutoUpdateStep,QVariant))); + UploaderGadgetWidget *gadgetWidget = new UploaderGadgetWidget(parent); + + isautocapable = gadgetWidget->autoUpdateCapable(); + connect(this, SIGNAL(autoUpdate()), gadgetWidget, SLOT(autoUpdate())); + connect(gadgetWidget, SIGNAL(autoUpdateSignal(uploader::AutoUpdateStep, QVariant)), this, SIGNAL(autoUpdateSignal(uploader::AutoUpdateStep, QVariant))); return new UploaderGadget(QString("Uploader"), gadgetWidget, parent); } -IUAVGadgetConfiguration *UploaderGadgetFactory::createConfiguration(QSettings* qSettings) +IUAVGadgetConfiguration *UploaderGadgetFactory::createConfiguration(QSettings *qSettings) { return new UploaderGadgetConfiguration(QString("Uploader"), qSettings); } diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.h index 9953201b8..3303148f4 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetfactory.h @@ -38,20 +38,19 @@ class IUAVGadgetFactory; using namespace Core; -class UPLOADER_EXPORT UploaderGadgetFactory : public Core::IUAVGadgetFactory -{ +class UPLOADER_EXPORT UploaderGadgetFactory : public Core::IUAVGadgetFactory { Q_OBJECT public: UploaderGadgetFactory(QObject *parent = 0); ~UploaderGadgetFactory(); Core::IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); + IUAVGadgetConfiguration *createConfiguration(QSettings *qSettings); bool isAutoUpdateCapable(); private: bool isautocapable; signals: - void autoUpdateSignal(uploader::AutoUpdateStep,QVariant); + void autoUpdateSignal(uploader::AutoUpdateStep, QVariant); void autoUpdate(); }; diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.cpp index 9bc3f6e22..ccaa091a1 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.cpp @@ -39,17 +39,16 @@ UploaderGadgetOptionsPage::UploaderGadgetOptionsPage(UploaderGadgetConfiguration *config, QObject *parent) : - IOptionsPage(parent), - m_config(config) -{ -} + IOptionsPage(parent), + m_config(config) +{} -//creates options page widget +// creates options page widget QWidget *UploaderGadgetOptionsPage::createPage(QWidget *parent) { Q_UNUSED(parent); - //main widget + // main widget QWidget *widget = new QWidget; return widget; @@ -61,13 +60,7 @@ QWidget *UploaderGadgetOptionsPage::createPage(QWidget *parent) * */ void UploaderGadgetOptionsPage::apply() -{ - -} +{} void UploaderGadgetOptionsPage::finish() -{ - -} - - +{} diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.h index 4546ea423..63a72b936 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetoptionspage.h @@ -44,9 +44,8 @@ class QSpinBox; using namespace Core; -class UPLOADER_EXPORT UploaderGadgetOptionsPage : public IOptionsPage -{ -Q_OBJECT +class UPLOADER_EXPORT UploaderGadgetOptionsPage : public IOptionsPage { + Q_OBJECT public: explicit UploaderGadgetOptionsPage(UploaderGadgetConfiguration *config, QObject *parent = 0); diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index aeab03d8f..62793298a 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -34,17 +34,17 @@ UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) { - m_config = new Ui_UploaderWidget(); + m_config = new Ui_UploaderWidget(); m_config->setupUi(this); currentStep = IAP_STATE_READY; - resetOnly=false; + resetOnly = false; dfu = NULL; - m_timer = 0; - m_progress = 0; - msg=new QErrorMessage(this); + m_timer = 0; + m_progress = 0; + msg = new QErrorMessage(this); // Listen to autopilot connection events ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - TelemetryManager* telMngr = pm->getObject(); + TelemetryManager *telMngr = pm->getObject(); connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); connect(telMngr, SIGNAL(connected()), this, SLOT(versionMatchCheck())); @@ -57,7 +57,7 @@ UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) connect(m_config->eraseBootButton, SIGNAL(clicked()), this, SLOT(systemEraseBoot())); connect(m_config->rescueButton, SIGNAL(clicked()), this, SLOT(systemRescue())); Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager(); - connect(cm,SIGNAL(deviceConnected(QIODevice*)),this,SLOT(onPhisicalHWConnect())); + connect(cm, SIGNAL(deviceConnected(QIODevice *)), this, SLOT(onPhisicalHWConnect())); getSerialPorts(); QIcon rbi; @@ -68,37 +68,37 @@ UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) connect(m_config->refreshPorts, SIGNAL(clicked()), this, SLOT(getSerialPorts())); - connect(m_config->pbHelp,SIGNAL(clicked()),this,SLOT(openHelp())); + connect(m_config->pbHelp, SIGNAL(clicked()), this, SLOT(openHelp())); // And check whether by any chance we are not already connected - if (telMngr->isConnected()) - { + if (telMngr->isConnected()) { onAutopilotConnect(); versionMatchCheck(); } } -bool sortPorts(const QextPortInfo &s1,const QextPortInfo &s2) +bool sortPorts(const QextPortInfo &s1, const QextPortInfo &s2) { - return s1.portNametelemetryLink->clear(); list.append(QString("USB")); QList ports = QextSerialEnumerator::getPorts(); - //sort the list by port number (nice idea from PT_Dreamer :)) - qSort(ports.begin(), ports.end(),sortPorts); - foreach( QextPortInfo port, ports ) { - list.append(port.friendName); + // sort the list by port number (nice idea from PT_Dreamer :)) + qSort(ports.begin(), ports.end(), sortPorts); + foreach(QextPortInfo port, ports) { + list.append(port.friendName); } m_config->telemetryLink->addItems(list); @@ -108,32 +108,33 @@ void UploaderGadgetWidget::getSerialPorts() QString UploaderGadgetWidget::getPortDevice(const QString &friendName) { QList ports = QextSerialEnumerator::getPorts(); - foreach( QextPortInfo port, ports ) { - if(port.friendName == friendName) + foreach(QextPortInfo port, ports) { + if (port.friendName == friendName) #ifdef Q_OS_WIN - return port.portName; + { return port.portName; } #else - return port.physName; + { return port.physName; } #endif - } + } return ""; } void UploaderGadgetWidget::connectSignalSlot(QWidget *widget) { - connect(qobject_cast(widget),SIGNAL(uploadStarted()),this,SLOT(uploadStarted())); - connect(qobject_cast(widget),SIGNAL(uploadEnded(bool)),this,SLOT(uploadEnded(bool))); - connect(qobject_cast(widget),SIGNAL(downloadStarted()),this,SLOT(downloadStarted())); - connect(qobject_cast(widget),SIGNAL(downloadEnded(bool)),this,SLOT(downloadEnded(bool))); + connect(qobject_cast(widget), SIGNAL(uploadStarted()), this, SLOT(uploadStarted())); + connect(qobject_cast(widget), SIGNAL(uploadEnded(bool)), this, SLOT(uploadEnded(bool))); + connect(qobject_cast(widget), SIGNAL(downloadStarted()), this, SLOT(downloadStarted())); + connect(qobject_cast(widget), SIGNAL(downloadEnded(bool)), this, SLOT(downloadEnded(bool))); } FlightStatus *UploaderGadgetWidget::getFlightStatus() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm); UAVObjectManager *objManager = pm->getObject(); Q_ASSERT(objManager); - FlightStatus *status = dynamic_cast(objManager->getObject(QString("FlightStatus"))); + FlightStatus *status = dynamic_cast(objManager->getObject(QString("FlightStatus"))); Q_ASSERT(status); return status; } @@ -144,11 +145,11 @@ void UploaderGadgetWidget::bootButtonsSetEnable(bool enabled) m_config->safeBootButton->setEnabled(enabled); - m_config->eraseBootButton->setEnabled( - enabled && - // this feature is supported only on BL revision >= 4 - ((dfu != NULL) && (dfu->devices[0].BL_Version >= 4))); - } + m_config->eraseBootButton->setEnabled( + enabled && + // this feature is supported only on BL revision >= 4 + ((dfu != NULL) && (dfu->devices[0].BL_Version >= 4))); +} void UploaderGadgetWidget::onPhisicalHWConnect() { @@ -158,9 +159,10 @@ void UploaderGadgetWidget::onPhisicalHWConnect() } /** - Enables widget buttons if autopilot connected - */ -void UploaderGadgetWidget::onAutopilotConnect(){ + Enables widget buttons if autopilot connected + */ +void UploaderGadgetWidget::onAutopilotConnect() +{ QTimer::singleShot(1000, this, SLOT(populate())); } @@ -175,20 +177,20 @@ void UploaderGadgetWidget::populate() // Add a very simple widget with Board model & serial number // Delete all previous tabs: while (m_config->systemElements->count()) { - QWidget *qw = m_config->systemElements->widget(0); - m_config->systemElements->removeTab(0); - delete qw; + QWidget *qw = m_config->systemElements->widget(0); + m_config->systemElements->removeTab(0); + delete qw; } - RunningDeviceWidget* dw = new RunningDeviceWidget(this); + RunningDeviceWidget *dw = new RunningDeviceWidget(this); dw->populate(); m_config->systemElements->addTab(dw, QString("Connected Device")); } /** - Enables widget buttons if autopilot disconnected - */ -void UploaderGadgetWidget::onAutopilotDisconnect(){ - + Enables widget buttons if autopilot disconnected + */ +void UploaderGadgetWidget::onAutopilotDisconnect() +{ m_config->haltButton->setEnabled(false); m_config->resetButton->setEnabled(false); bootButtonsSetEnable(true); @@ -202,17 +204,17 @@ void UploaderGadgetWidget::onAutopilotDisconnect(){ } /** - Tell the mainboard to go to bootloader: + Tell the mainboard to go to bootloader: - Send the relevant IAP commands - setup callback for MoBo acknowledge - */ -void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) + */ +void UploaderGadgetWidget::goToBootloader(UAVObject *callerObj, bool success) { Q_UNUSED(callerObj); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVObject *fwIAP = dynamic_cast(objManager->getObject(QString("FirmwareIAPObj"))); + UAVObject *fwIAP = dynamic_cast(objManager->getObject(QString("FirmwareIAPObj"))); switch (currentStep) { case IAP_STATE_READY: @@ -223,18 +225,18 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) fwIAP->getField("Command")->setValue("1122"); fwIAP->getField("BoardRevision")->setDouble(0); fwIAP->getField("BoardType")->setDouble(0); - connect(fwIAP,SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(goToBootloader(UAVObject*, bool))); + connect(fwIAP, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(goToBootloader(UAVObject *, bool))); currentStep = IAP_STATE_STEP_1; clearLog(); log(QString("IAP Step 1")); fwIAP->updated(); break; case IAP_STATE_STEP_1: - if (!success) { + if (!success) { log(QString("Oops, failure step 1")); log("Reset did NOT happen"); currentStep = IAP_STATE_READY; - disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(goToBootloader(UAVObject*, bool))); + disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(goToBootloader(UAVObject *, bool))); m_config->haltButton->setEnabled(true); break; } @@ -250,7 +252,7 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) log(QString("Oops, failure step 2")); log("Reset did NOT happen"); currentStep = IAP_STATE_READY; - disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(goToBootloader(UAVObject*, bool))); + disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(goToBootloader(UAVObject *, bool))); m_config->haltButton->setEnabled(true); break; } @@ -267,7 +269,7 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) if (!success) { log("Oops, failure step 3"); log("Reset did NOT happen"); - disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(goToBootloader(UAVObject*, bool))); + disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(goToBootloader(UAVObject *, bool))); m_config->haltButton->setEnabled(true); break; } @@ -285,25 +287,26 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) m_eventloop.exec(); log("Board Halt"); m_config->boardStatus->setText("Bootloader"); - if (dlj.startsWith("USB")) + if (dlj.startsWith("USB")) { m_config->telemetryLink->setCurrentIndex(m_config->telemetryLink->findText("USB")); - else + } else { m_config->telemetryLink->setCurrentIndex(m_config->telemetryLink->findText(dli)); + } - disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(goToBootloader(UAVObject*, bool))); + disconnect(fwIAP, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(goToBootloader(UAVObject *, bool))); currentStep = IAP_STATE_BOOTLOADER; // Tell the mainboard to get into bootloader state: log("Detecting devices, please wait a few seconds..."); if (!dfu) { - if (dlj.startsWith("USB")) + if (dlj.startsWith("USB")) { dfu = new DFUObject(DFU_DEBUG, false, QString()); - else - dfu = new DFUObject(DFU_DEBUG,true, getPortDevice(dli)); + } else { + dfu = new DFUObject(DFU_DEBUG, true, getPortDevice(dli)); + } } - if (!dfu->ready()) - { + if (!dfu->ready()) { log("Could not enter DFU mode."); delete dfu; dfu = NULL; @@ -314,8 +317,7 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) return; } dfu->AbortOperation(); - if(!dfu->enterDFU(0)) - { + if (!dfu->enterDFU(0)) { log("Could not enter DFU mode."); delete dfu; dfu = NULL; @@ -324,10 +326,10 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) m_config->boardStatus->setText("Bootloader?"); return; } - //dfu.StatusRequest(); + // dfu.StatusRequest(); - QTimer::singleShot(500, &m_eventloop, SLOT(quit())); - m_eventloop.exec(); + QTimer::singleShot(500, &m_eventloop, SLOT(quit())); + m_eventloop.exec(); dfu->findDevices(); log(QString("Found ") + QString::number(dfu->numberOfDevices) + QString(" device(s).")); if (dfu->numberOfDevices < 0 || dfu->numberOfDevices > 3) { @@ -339,12 +341,12 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) } // Delete all previous tabs: while (m_config->systemElements->count()) { - QWidget *qw = m_config->systemElements->widget(0); - m_config->systemElements->removeTab(0); - delete qw; + QWidget *qw = m_config->systemElements->widget(0); + m_config->systemElements->removeTab(0); + delete qw; } - for(int i=0;inumberOfDevices;i++) { - DeviceWidget* dw = new DeviceWidget(this); + for (int i = 0; i < dfu->numberOfDevices; i++) { + DeviceWidget *dw = new DeviceWidget(this); connectSignalSlot(dw); dw->setDeviceID(i); dw->setDfu(dfu); @@ -352,41 +354,38 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) m_config->systemElements->addTab(dw, QString("Device") + QString::number(i)); } /* - m_config->haltButton->setEnabled(false); - m_config->resetButton->setEnabled(false); - */ + m_config->haltButton->setEnabled(false); + m_config->resetButton->setEnabled(false); + */ // Need to re-enable in case we were not connected bootButtonsSetEnable(true); /* - m_config->telemetryLink->setEnabled(false); - m_config->rescueButton->setEnabled(false); - */ + m_config->telemetryLink->setEnabled(false); + m_config->rescueButton->setEnabled(false); + */ if (resetOnly) { - resetOnly=false; + resetOnly = false; delay::msleep(3500); systemBoot(); break; } - } - break; + break; case IAP_STATE_BOOTLOADER: // We should never end up here anyway. break; } - } void UploaderGadgetWidget::systemHalt() { - FlightStatus* status = getFlightStatus(); + FlightStatus *status = getFlightStatus(); // The board can not be halted when in armed state. // If board is armed, or arming. Show message with notice. if (status->getArmed() == FlightStatus::ARMED_DISARMED) { goToBootloader(); - } - else { + } else { QMessageBox mbox(this); mbox.setText(QString(tr("The controller board is armed and can not be halted.\n\n" "Please make sure the board is not armed and then press halt again to proceed\n" @@ -398,13 +397,13 @@ void UploaderGadgetWidget::systemHalt() } /** - Tell the mainboard to reset: + Tell the mainboard to reset: - Send the relevant IAP commands - setup callback for MoBo acknowledge - */ + */ void UploaderGadgetWidget::systemReset() { - FlightStatus* status = getFlightStatus(); + FlightStatus *status = getFlightStatus(); // The board can not be reset when in armed state. // If board is armed, or arming. Show message with notice. @@ -417,8 +416,7 @@ void UploaderGadgetWidget::systemReset() clearLog(); log("Board Reset initiated."); goToBootloader(); - } - else { + } else { QMessageBox mbox(this); mbox.setText(QString(tr("The controller board is armed and can not be reset.\n\n" "Please make sure the board is not armed and then press reset again to proceed\n" @@ -431,34 +429,34 @@ void UploaderGadgetWidget::systemReset() void UploaderGadgetWidget::systemBoot() { - commonSystemBoot(false, false); + commonSystemBoot(false, false); } void UploaderGadgetWidget::systemSafeBoot() { - commonSystemBoot(true, false); + commonSystemBoot(true, false); } void UploaderGadgetWidget::systemEraseBoot() { - QMessageBox msgBox; - int result; - msgBox.setWindowTitle(tr("Erase Settings")); - msgBox.setInformativeText(tr("Do you want to erase all settings from the board?\nSettings cannot be recovered after this operation.\nThe board will be restarted and all the setting erased")); - msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel | QMessageBox::Help); - result = msgBox.exec(); - if(result == QMessageBox::Ok) - { - commonSystemBoot(true, true); - } else if(result == QMessageBox::Help) { - QDesktopServices::openUrl( QUrl(tr("http://wiki.openpilot.org/display/Doc/Erase+board+settings"), QUrl::StrictMode) ); - } + QMessageBox msgBox; + int result; + + msgBox.setWindowTitle(tr("Erase Settings")); + msgBox.setInformativeText(tr("Do you want to erase all settings from the board?\nSettings cannot be recovered after this operation.\nThe board will be restarted and all the setting erased")); + msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel | QMessageBox::Help); + result = msgBox.exec(); + if (result == QMessageBox::Ok) { + commonSystemBoot(true, true); + } else if (result == QMessageBox::Help) { + QDesktopServices::openUrl(QUrl(tr("http://wiki.openpilot.org/display/Doc/Erase+board+settings"), QUrl::StrictMode)); + } } /** - * Tells the system to boot (from Bootloader state) - * @param[in] safeboot Indicates whether the firmware should use the stock HWSettings - */ + * Tells the system to boot (from Bootloader state) + * @param[in] safeboot Indicates whether the firmware should use the stock HWSettings + */ void UploaderGadgetWidget::commonSystemBoot(bool safeboot, bool erase) { clearLog(); @@ -474,14 +472,14 @@ void UploaderGadgetWidget::commonSystemBoot(bool safeboot, bool erase) repaint(); if (!dfu) { - if (devName == "USB") + if (devName == "USB") { dfu = new DFUObject(DFU_DEBUG, false, QString()); - else - dfu = new DFUObject(DFU_DEBUG,true,getPortDevice(devName)); + } else { + dfu = new DFUObject(DFU_DEBUG, true, getPortDevice(devName)); + } } dfu->AbortOperation(); - if(!dfu->enterDFU(0)) - { + if (!dfu->enterDFU(0)) { log("Could not enter DFU mode."); delete dfu; dfu = NULL; @@ -496,13 +494,13 @@ void UploaderGadgetWidget::commonSystemBoot(bool safeboot, bool erase) m_config->rescueButton->setEnabled(true); m_config->telemetryLink->setEnabled(true); m_config->boardStatus->setText("Running"); - if (currentStep == IAP_STATE_BOOTLOADER ) { + if (currentStep == IAP_STATE_BOOTLOADER) { // Freeze the tabs, they are not useful anymore and their buttons // will cause segfaults or weird stuff if we use them. - for (int i=0; i< m_config->systemElements->count(); i++) { + for (int i = 0; i < m_config->systemElements->count(); i++) { // OP-682 arriving here too "early" (before the devices are refreshed) was leading to a crash // OP-682 the crash was due to an unchecked cast in the line below that would cast a RunningDeviceGadget to a DeviceGadget - DeviceWidget *qw = dynamic_cast(m_config->systemElements->widget(i)); + DeviceWidget *qw = dynamic_cast(m_config->systemElements->widget(i)); if (qw) { // OP-682 fixed a second crash by disabling *all* buttons in the device widget // disabling the buttons is only half of the solution as even if the buttons are enabled @@ -525,6 +523,7 @@ bool UploaderGadgetWidget::autoUpdateCapable() bool UploaderGadgetWidget::autoUpdate() { Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager(); + cm->disconnectDevice(); cm->suspendPolling(); if (dfu) { @@ -534,60 +533,55 @@ bool UploaderGadgetWidget::autoUpdate() QEventLoop loop; QTimer timer; timer.setSingleShot(true); - connect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); - while(USBMonitor::instance()->availableDevices(0x20a0,-1,-1,-1).length()>0) - { - emit autoUpdateSignal(WAITING_DISCONNECT,QVariant()); - if(QMessageBox::warning(this,tr("OpenPilot Uploader"),tr("Please disconnect all openpilot boards"),QMessageBox::Ok,QMessageBox::Cancel)==QMessageBox::Cancel) - { - emit autoUpdateSignal(FAILURE,QVariant()); - return false; - } - timer.start(500); - loop.exec(); + connect(&timer, SIGNAL(timeout()), &loop, SLOT(quit())); + while (USBMonitor::instance()->availableDevices(0x20a0, -1, -1, -1).length() > 0) { + emit autoUpdateSignal(WAITING_DISCONNECT, QVariant()); + if (QMessageBox::warning(this, tr("OpenPilot Uploader"), tr("Please disconnect all openpilot boards"), QMessageBox::Ok, QMessageBox::Cancel) == QMessageBox::Cancel) { + emit autoUpdateSignal(FAILURE, QVariant()); + return false; + } + timer.start(500); + loop.exec(); } - emit autoUpdateSignal(WAITING_CONNECT,0); - autoUpdateConnectTimeout=0; + emit autoUpdateSignal(WAITING_CONNECT, 0); + autoUpdateConnectTimeout = 0; m_timer = new QTimer(this); connect(m_timer, SIGNAL(timeout()), this, SLOT(performAuto())); m_timer->start(1000); - connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)),&m_eventloop, SLOT(quit())); + connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), &m_eventloop, SLOT(quit())); m_eventloop.exec(); - if(!m_timer->isActive()) - { + if (!m_timer->isActive()) { m_timer->stop(); - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(FAILURE, QVariant()); return false; } m_timer->stop(); dfu = new DFUObject(DFU_DEBUG, false, QString()); dfu->AbortOperation(); - emit autoUpdateSignal(JUMP_TO_BL,QVariant()); - if(!dfu->enterDFU(0)) - { + emit autoUpdateSignal(JUMP_TO_BL, QVariant()); + if (!dfu->enterDFU(0)) { delete dfu; dfu = NULL; cm->resumePolling(); - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(FAILURE, QVariant()); return false; } - if(!dfu->findDevices() || (dfu->numberOfDevices != 1)) - { + if (!dfu->findDevices() || (dfu->numberOfDevices != 1)) { delete dfu; dfu = NULL; cm->resumePolling(); - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(FAILURE, QVariant()); return false; } if (dfu->numberOfDevices > 5) { delete dfu; dfu = NULL; cm->resumePolling(); - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(FAILURE, QVariant()); return false; } QString filename; - emit autoUpdateSignal(LOADING_FW,QVariant()); + emit autoUpdateSignal(LOADING_FW, QVariant()); switch (dfu->devices[0].ID) { case 0x301: filename = "fw_oplinkmini"; @@ -606,62 +600,60 @@ bool UploaderGadgetWidget::autoUpdate() filename = "fw_revolution"; break; default: - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(FAILURE, QVariant()); return false; + break; } filename = ":/firmware/" + filename + ".opfw"; QByteArray firmware; - if(!QFile::exists(filename)) - { - emit autoUpdateSignal(FAILURE,QVariant()); + if (!QFile::exists(filename)) { + emit autoUpdateSignal(FAILURE, QVariant()); return false; } QFile file(filename); if (!file.open(QIODevice::ReadOnly)) { - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(FAILURE, QVariant()); return false; } firmware = file.readAll(); connect(dfu, SIGNAL(progressUpdated(int)), this, SLOT(autoUpdateProgress(int))); connect(dfu, SIGNAL(uploadFinished(OP_DFU::Status)), &m_eventloop, SLOT(quit())); - emit autoUpdateSignal(UPLOADING_FW,QVariant()); - if(!dfu->enterDFU(0)) - { - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(UPLOADING_FW, QVariant()); + if (!dfu->enterDFU(0)) { + emit autoUpdateSignal(FAILURE, QVariant()); return false; } dfu->AbortOperation(); - if(!dfu->UploadFirmware(filename,false,0)) - { - emit autoUpdateSignal(FAILURE,QVariant()); + if (!dfu->UploadFirmware(filename, false, 0)) { + emit autoUpdateSignal(FAILURE, QVariant()); return false; } m_eventloop.exec(); QByteArray desc = firmware.right(100); - emit autoUpdateSignal(UPLOADING_DESC,QVariant()); - if(dfu->UploadDescription(desc)!= OP_DFU::Last_operation_Success) - { - emit autoUpdateSignal(FAILURE,QVariant()); + emit autoUpdateSignal(UPLOADING_DESC, QVariant()); + if (dfu->UploadDescription(desc) != OP_DFU::Last_operation_Success) { + emit autoUpdateSignal(FAILURE, QVariant()); return false; } systemBoot(); - emit autoUpdateSignal(SUCCESS,QVariant()); + emit autoUpdateSignal(SUCCESS, QVariant()); return true; } void UploaderGadgetWidget::autoUpdateProgress(int value) { - emit autoUpdateSignal(UPLOADING_FW,value); + emit autoUpdateSignal(UPLOADING_FW, value); } /** - Attempt a guided procedure to put both boards in BL mode when - the system is not bootable - */ + Attempt a guided procedure to put both boards in BL mode when + the system is not bootable + */ void UploaderGadgetWidget::systemRescue() { Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager(); + cm->disconnectDevice(); // stop the polling thread: otherwise it will mess up DFU cm->suspendPolling(); @@ -686,10 +678,8 @@ void UploaderGadgetWidget::systemRescue() log("** Follow those instructions to attempt a system rescue **"); log("**********************************************************"); log("You will be prompted to first connect USB, then system power"); - if(USBMonitor::instance()->availableDevices(0x20a0,-1,-1,-1).length()>0) - { - if(QMessageBox::warning(this,tr("OpenPilot Uploader"),tr("Please disconnect all openpilot boards"),QMessageBox::Ok,QMessageBox::Cancel)==QMessageBox::Cancel) - { + if (USBMonitor::instance()->availableDevices(0x20a0, -1, -1, -1).length() > 0) { + if (QMessageBox::warning(this, tr("OpenPilot Uploader"), tr("Please disconnect all openpilot boards"), QMessageBox::Ok, QMessageBox::Cancel) == QMessageBox::Cancel) { m_config->rescueButton->setEnabled(true); return; } @@ -701,22 +691,21 @@ void UploaderGadgetWidget::systemRescue() log("**********************************************************"); log("You will be prompted to first connect USB, then system power"); m_progress = new QProgressDialog(tr("Please connect the board (USB only!)"), tr("Cancel"), 0, 20); - QProgressBar * bar=new QProgressBar(m_progress); + QProgressBar *bar = new QProgressBar(m_progress); bar->setFormat("Timeout"); m_progress->setBar(bar); m_progress->setMinimumDuration(0); - m_progress->setRange(0,20); + m_progress->setRange(0, 20); connect(m_progress, SIGNAL(canceled()), this, SLOT(cancel())); m_timer = new QTimer(this); connect(m_timer, SIGNAL(timeout()), this, SLOT(perform())); m_timer->start(1000); - connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)),&m_eventloop, SLOT(quit())); + connect(USBMonitor::instance(), SIGNAL(deviceDiscovered(USBPortInfo)), &m_eventloop, SLOT(quit())); m_eventloop.exec(); - if(!m_timer->isActive()) - { + if (!m_timer->isActive()) { m_progress->close(); m_timer->stop(); - QMessageBox::warning(this,tr("Openpilot Uploader"),tr("No board connection was detected!")); + QMessageBox::warning(this, tr("Openpilot Uploader"), tr("No board connection was detected!")); m_config->rescueButton->setEnabled(true); return; } @@ -726,8 +715,7 @@ void UploaderGadgetWidget::systemRescue() repaint(); dfu = new DFUObject(DFU_DEBUG, false, QString()); dfu->AbortOperation(); - if(!dfu->enterDFU(0)) - { + if (!dfu->enterDFU(0)) { log("Could not enter DFU mode."); delete dfu; dfu = NULL; @@ -735,8 +723,7 @@ void UploaderGadgetWidget::systemRescue() m_config->rescueButton->setEnabled(true); return; } - if(!dfu->findDevices() || (dfu->numberOfDevices != 1)) - { + if (!dfu->findDevices() || (dfu->numberOfDevices != 1)) { log("Could not detect a board, aborting!"); delete dfu; dfu = NULL; @@ -753,8 +740,8 @@ void UploaderGadgetWidget::systemRescue() m_config->rescueButton->setEnabled(true); return; } - for(int i=0;inumberOfDevices;i++) { - DeviceWidget* dw = new DeviceWidget(this); + for (int i = 0; i < dfu->numberOfDevices; i++) { + DeviceWidget *dw = new DeviceWidget(this); connectSignalSlot(dw); dw->setDeviceID(i); dw->setDfu(dfu); @@ -770,23 +757,20 @@ void UploaderGadgetWidget::systemRescue() void UploaderGadgetWidget::perform() { - if(m_progress->value()==19) - { + if (m_progress->value() == 19) { m_timer->stop(); m_eventloop.exit(); } - m_progress->setValue(m_progress->value()+1); + m_progress->setValue(m_progress->value() + 1); } void UploaderGadgetWidget::performAuto() { ++autoUpdateConnectTimeout; - emit autoUpdateSignal(WAITING_CONNECT,autoUpdateConnectTimeout*5); - if(autoUpdateConnectTimeout==20) - { + emit autoUpdateSignal(WAITING_CONNECT, autoUpdateConnectTimeout * 5); + if (autoUpdateConnectTimeout == 20) { m_timer->stop(); m_eventloop.exit(); } - } void UploaderGadgetWidget::cancel() { @@ -833,13 +817,13 @@ void UploaderGadgetWidget::downloadEnded(bool succeed) } /** - Update log entry - */ + Update log entry + */ void UploaderGadgetWidget::log(QString str) { - qDebug() << str; - m_config->textBrowser->append(str); - m_config->textBrowser->repaint(); + qDebug() << str; + m_config->textBrowser->append(str); + m_config->textBrowser->repaint(); } void UploaderGadgetWidget::clearLog() @@ -848,15 +832,15 @@ void UploaderGadgetWidget::clearLog() } /** - * Remove all the device widgets... - */ + * Remove all the device widgets... + */ UploaderGadgetWidget::~UploaderGadgetWidget() { while (m_config->systemElements->count()) { - QWidget *qw = m_config->systemElements->widget(0); - m_config->systemElements->removeTab(0); - delete qw; - qw = 0; + QWidget *qw = m_config->systemElements->widget(0); + m_config->systemElements->removeTab(0); + delete qw; + qw = 0; } if (m_progress) { delete m_progress; @@ -870,13 +854,13 @@ UploaderGadgetWidget::~UploaderGadgetWidget() /** -Shows a message box with an error string. + Shows a message box with an error string. -@param errorString The error string to display. + @param errorString The error string to display. -@param errorNumber Not used + @param errorNumber Not used -*/ + */ void UploaderGadgetWidget::error(QString errorString, int errorNumber) { Q_UNUSED(errorNumber); @@ -888,13 +872,13 @@ void UploaderGadgetWidget::error(QString errorString, int errorNumber) } /** -Shows a message box with an information string. + Shows a message box with an information string. -@param infoString The information string to display. + @param infoString The information string to display. -@param infoNumber Not used + @param infoNumber Not used -*/ + */ void UploaderGadgetWidget::info(QString infoString, int infoNumber) { Q_UNUSED(infoNumber); @@ -904,48 +888,45 @@ void UploaderGadgetWidget::info(QString infoString, int infoNumber) void UploaderGadgetWidget::versionMatchCheck() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectUtilManager *utilMngr = pm->getObject(); + UAVObjectUtilManager *utilMngr = pm->getObject(); deviceDescriptorStruct boardDescription = utilMngr->getBoardDescriptionStruct(); QByteArray uavoHashArray; QString uavoHash = QString::fromLatin1(Core::Constants::UAVOSHA1_STR); + uavoHash.chop(2); - uavoHash.remove(0,2); - uavoHash=uavoHash.trimmed(); + uavoHash.remove(0, 2); + uavoHash = uavoHash.trimmed(); bool ok; - foreach(QString str,uavoHash.split(",")) - { - uavoHashArray.append(str.toInt(&ok,16)); + foreach(QString str, uavoHash.split(",")) { + uavoHashArray.append(str.toInt(&ok, 16)); } - QByteArray fwVersion=boardDescription.uavoHash; + QByteArray fwVersion = boardDescription.uavoHash; if (fwVersion != uavoHashArray) { - QString gcsDescription = QString::fromLatin1(Core::Constants::GCS_REVISION_STR); - QString gcsGitHash = gcsDescription.mid(gcsDescription.indexOf(":")+1, 8); - gcsGitHash.remove( QRegExp("^[0]*") ); - QString gcsGitDate = gcsDescription.mid(gcsDescription.indexOf(" ")+1, 14); + QString gcsGitHash = gcsDescription.mid(gcsDescription.indexOf(":") + 1, 8); + gcsGitHash.remove(QRegExp("^[0]*")); + QString gcsGitDate = gcsDescription.mid(gcsDescription.indexOf(" ") + 1, 14); QString gcsUavoHashStr; QString fwUavoHashStr; - foreach(char i, fwVersion) - { - fwUavoHashStr.append(QString::number(i,16).right(2)); + foreach(char i, fwVersion) { + fwUavoHashStr.append(QString::number(i, 16).right(2)); } - foreach(char i, uavoHashArray) - { - gcsUavoHashStr.append(QString::number(i,16).right(2)); + foreach(char i, uavoHashArray) { + gcsUavoHashStr.append(QString::number(i, 16).right(2)); } - QString gcsVersion = gcsGitDate + " (" + gcsGitHash + "-"+ gcsUavoHashStr.left(8) + ")"; - QString fwVersion = boardDescription.gitDate + " (" + boardDescription.gitHash + "-" + fwUavoHashStr.left(8) + ")"; + QString gcsVersion = gcsGitDate + " (" + gcsGitHash + "-" + gcsUavoHashStr.left(8) + ")"; + QString fwVersion = boardDescription.gitDate + " (" + boardDescription.gitHash + "-" + fwUavoHashStr.left(8) + ")"; - QString warning = QString(tr( - "GCS and firmware versions of the UAV objects set do not match which can cause configuration problems. " - "GCS version: %1 Firmware version: %2.")).arg(gcsVersion).arg(fwVersion); + QString warning = QString(tr( + "GCS and firmware versions of the UAV objects set do not match which can cause configuration problems. " + "GCS version: %1 Firmware version: %2.")).arg(gcsVersion).arg(fwVersion); msg->showMessage(warning); } } void UploaderGadgetWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/AoBZ", QUrl::StrictMode) ); + QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/AoBZ", QUrl::StrictMode)); } diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h index 0654aa1cc..4f088c9aa 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h @@ -63,14 +63,13 @@ using namespace uploader; class FlightStatus; -class UPLOADER_EXPORT UploaderGadgetWidget : public QWidget -{ +class UPLOADER_EXPORT UploaderGadgetWidget : public QWidget { Q_OBJECT public: UploaderGadgetWidget(QWidget *parent = 0); - ~UploaderGadgetWidget(); + ~UploaderGadgetWidget(); void log(QString str); bool autoUpdateCapable(); public slots: @@ -81,35 +80,35 @@ public slots: bool autoUpdate(); void autoUpdateProgress(int); signals: - void autoUpdateSignal(uploader::AutoUpdateStep,QVariant); + void autoUpdateSignal(uploader::AutoUpdateStep, QVariant); private: - Ui_UploaderWidget *m_config; - DFUObject *dfu; - IAPStep currentStep; - bool resetOnly; - void clearLog(); - QString getPortDevice(const QString &friendName); - QProgressDialog* m_progress; - QTimer* m_timer; - QLineEdit* openFileNameLE; - QEventLoop m_eventloop; - QErrorMessage * msg; - void connectSignalSlot(QWidget * widget); - int autoUpdateConnectTimeout; - FlightStatus * getFlightStatus(); - void bootButtonsSetEnable(bool enabled); + Ui_UploaderWidget *m_config; + DFUObject *dfu; + IAPStep currentStep; + bool resetOnly; + void clearLog(); + QString getPortDevice(const QString &friendName); + QProgressDialog *m_progress; + QTimer *m_timer; + QLineEdit *openFileNameLE; + QEventLoop m_eventloop; + QErrorMessage *msg; + void connectSignalSlot(QWidget *widget); + int autoUpdateConnectTimeout; + FlightStatus *getFlightStatus(); + void bootButtonsSetEnable(bool enabled); private slots: void onPhisicalHWConnect(); void versionMatchCheck(); - void error(QString errorString,int errorNumber); - void info(QString infoString,int infoNumber); - void goToBootloader(UAVObject* = NULL, bool = false); + void error(QString errorString, int errorNumber); + void info(QString infoString, int infoNumber); + void goToBootloader(UAVObject * = NULL, bool = false); void systemHalt(); void systemReset(); void systemBoot(); void systemSafeBoot(); - void systemEraseBoot(); - void commonSystemBoot(bool safeboot = false, bool erase = false); + void systemEraseBoot(); + void commonSystemBoot(bool safeboot = false, bool erase = false); void systemRescue(); void getSerialPorts(); void perform(); diff --git a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp index 7fd5dd79d..629eb82d6 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.cpp @@ -1,4 +1,3 @@ - /** ****************************************************************************** * @@ -33,31 +32,30 @@ UploaderPlugin::UploaderPlugin() { - // Do nothing + // Do nothing } UploaderPlugin::~UploaderPlugin() { - // Do nothing + // Do nothing } -bool UploaderPlugin::initialize(const QStringList& args, QString *errMsg) +bool UploaderPlugin::initialize(const QStringList & args, QString *errMsg) { - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new UploaderGadgetFactory(this); - addAutoReleasedObject(mf); - return true; + Q_UNUSED(args); + Q_UNUSED(errMsg); + mf = new UploaderGadgetFactory(this); + addAutoReleasedObject(mf); + return true; } void UploaderPlugin::extensionsInitialized() { - // Do nothing + // Do nothing } void UploaderPlugin::shutdown() { - // Do nothing + // Do nothing } Q_EXPORT_PLUGIN(UploaderPlugin) - diff --git a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h index e74c342ca..082519f43 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploaderplugin.h @@ -33,16 +33,15 @@ class UploaderGadgetFactory; -class UPLOADER_EXPORT UploaderPlugin : public ExtensionSystem::IPlugin -{ +class UPLOADER_EXPORT UploaderPlugin : public ExtensionSystem::IPlugin { public: - UploaderPlugin(); - ~UploaderPlugin(); + UploaderPlugin(); + ~UploaderPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString *errorString); + void shutdown(); private: - UploaderGadgetFactory *mf; + UploaderGadgetFactory *mf; }; #endif // UPLOADERPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/welcome/welcome_global.h b/ground/openpilotgcs/src/plugins/welcome/welcome_global.h index 91386815e..15fd8de88 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcome_global.h +++ b/ground/openpilotgcs/src/plugins/welcome/welcome_global.h @@ -11,18 +11,18 @@ * @brief The GCS Welcome plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp b/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp index a109f328a..895597672 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp +++ b/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp @@ -11,18 +11,18 @@ * @brief The GCS Welcome plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -56,17 +56,14 @@ using namespace ExtensionSystem; using namespace Utils; namespace Welcome { - -struct WelcomeModePrivate -{ +struct WelcomeModePrivate { WelcomeModePrivate(); QDeclarativeView *declarativeView; }; WelcomeModePrivate::WelcomeModePrivate() -{ -} +{} // --- WelcomeMode WelcomeMode::WelcomeMode() : @@ -100,12 +97,12 @@ int WelcomeMode::priority() const return m_priority; } -QWidget* WelcomeMode::widget() +QWidget *WelcomeMode::widget() { return m_d->declarativeView; } -const char* WelcomeMode::uniqueModeName() const +const char *WelcomeMode::uniqueModeName() const { return Core::Constants::MODE_WELCOME; } @@ -114,6 +111,7 @@ QList WelcomeMode::context() const { static QList contexts = QList() << Core::UniqueIDManager::instance()->uniqueIdentifier(Core::Constants::C_WELCOME_MODE); + return contexts; } @@ -131,5 +129,4 @@ void WelcomeMode::triggerAction(const QString &actionId) { Core::ModeManager::instance()->triggerAction(actionId); } - } // namespace Welcome diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomemode.h b/ground/openpilotgcs/src/plugins/welcome/welcomemode.h index 14a00312e..fa3f22856 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomemode.h +++ b/ground/openpilotgcs/src/plugins/welcome/welcomemode.h @@ -11,18 +11,18 @@ * @brief The GCS Welcome plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -40,11 +40,9 @@ class QUrl; QT_END_NAMESPACE namespace Welcome { - struct WelcomeModePrivate; -class WELCOME_EXPORT WelcomeMode : public Core::IMode -{ +class WELCOME_EXPORT WelcomeMode : public Core::IMode { Q_OBJECT public: @@ -59,8 +57,14 @@ public: const char *uniqueModeName() const; QList context() const; void activated(); - QString contextHelpId() const { return QLatin1String("OpenPilot GCS"); } - void setPriority(int priority) { m_priority = priority; } + QString contextHelpId() const + { + return QLatin1String("OpenPilot GCS"); + } + void setPriority(int priority) + { + m_priority = priority; + } public slots: void openUrl(const QString &url); @@ -71,7 +75,6 @@ private: WelcomeModePrivate *m_d; int m_priority; }; - } // namespace Welcome #endif // WELCOMEMODE_H diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.cpp b/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.cpp index 3e79878b5..85e427739 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.cpp +++ b/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.cpp @@ -11,18 +11,18 @@ * @brief The GCS Welcome plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -47,9 +47,8 @@ using namespace Welcome::Internal; WelcomePlugin::WelcomePlugin() - : m_welcomeMode(0) -{ -} + : m_welcomeMode(0) +{} WelcomePlugin::~WelcomePlugin() { @@ -64,7 +63,7 @@ WelcomePlugin::~WelcomePlugin() \a error_message can be used to pass an error message to the plugin system, if there was any. -*/ + */ bool WelcomePlugin::initialize(const QStringList &arguments, QString *error_message) { Q_UNUSED(arguments) @@ -86,7 +85,7 @@ bool WelcomePlugin::initialize(const QStringList &arguments, QString *error_mess The WelcomePlugin doesn't need things from other plugins, so it does nothing here. -*/ + */ void WelcomePlugin::extensionsInitialized() { Core::ModeManager::instance()->activateMode(m_welcomeMode->uniqueModeName()); diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.h b/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.h index 1f8e31988..0a9d65604 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.h +++ b/ground/openpilotgcs/src/plugins/welcome/welcomeplugin.h @@ -11,18 +11,18 @@ * @brief The GCS Welcome plugin *****************************************************************************/ /* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,14 +32,11 @@ #include namespace Welcome { - class WelcomeMode; namespace Internal { - class WelcomePlugin - : public ExtensionSystem::IPlugin -{ + : public ExtensionSystem::IPlugin { Q_OBJECT public: @@ -53,7 +50,6 @@ public: private: WelcomeMode *m_welcomeMode; }; - } // namespace Welcome } // namespace Internal diff --git a/ground/openpilotgcs/src/shared/namespace_global.h b/ground/openpilotgcs/src/shared/namespace_global.h index 80ac9aaa4..c72a21a8a 100644 --- a/ground/openpilotgcs/src/shared/namespace_global.h +++ b/ground/openpilotgcs/src/shared/namespace_global.h @@ -4,25 +4,25 @@ * @file namespace_global.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -41,7 +41,7 @@ # define QT_BEGIN_MOC_NAMESPACE # define QT_END_MOC_NAMESPACE # define QT_FORWARD_DECLARE_CLASS(name) class name; -# define QT_MANGLE_NAMESPACE(name) name +# define QT_MANGLE_NAMESPACE(name) name #endif #endif // NAMESPACE_GLOBAL_H diff --git a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.cpp b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.cpp index baed7993b..e4ee86cb9 100644 --- a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.cpp +++ b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.cpp @@ -4,32 +4,31 @@ * @file qtlockedfile.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "qtlockedfile.h" namespace SharedTools { - /*! \class QtLockedFile @@ -52,7 +51,7 @@ namespace SharedTools { The lock provided by an instance of \e QtLockedFile is released whenever the program terminates. This is true even when the program crashes and no destructors are called. -*/ + */ /*! \enum QtLockedFile::LockMode @@ -61,22 +60,22 @@ namespace SharedTools { \value ReadLock A read lock. \value WriteLock A write lock. \value NoLock Neither a read lock nor a write lock. -*/ + */ /*! Constructs an unlocked \e QtLockedFile object. This constructor behaves in the same way as \e QFile::QFile(). \sa QFile::QFile() -*/ + */ QtLockedFile::QtLockedFile() : QFile() { #ifdef Q_OS_WIN m_semaphore_hnd = 0; - m_mutex_hnd = 0; + m_mutex_hnd = 0; #endif - m_lock_mode = NoLock; + m_lock_mode = NoLock; } /*! @@ -84,15 +83,15 @@ QtLockedFile::QtLockedFile() the same way as \e QFile::QFile(const QString&). \sa QFile::QFile() -*/ + */ QtLockedFile::QtLockedFile(const QString &name) : QFile(name) { #ifdef Q_OS_WIN m_semaphore_hnd = 0; - m_mutex_hnd = 0; + m_mutex_hnd = 0; #endif - m_lock_mode = NoLock; + m_lock_mode = NoLock; } /*! @@ -100,7 +99,7 @@ QtLockedFile::QtLockedFile(const QString &name) otherwise returns \e false. \sa lockMode() -*/ + */ bool QtLockedFile::isLocked() const { return m_lock_mode != NoLock; @@ -110,7 +109,7 @@ bool QtLockedFile::isLocked() const Returns the type of lock currently held by this object, or \e QtLockedFile::NoLock. \sa isLocked() -*/ + */ QtLockedFile::LockMode QtLockedFile::lockMode() const { return m_lock_mode; @@ -133,7 +132,7 @@ QtLockedFile::LockMode QtLockedFile::lockMode() const and \e false otherwise. \sa unlock(), isLocked(), lockMode() -*/ + */ /*! \fn bool QtLockedFile::unlock() @@ -146,12 +145,11 @@ QtLockedFile::LockMode QtLockedFile::lockMode() const this object, and \e false otherwise. \sa lock(), isLocked(), lockMode() -*/ + */ /*! \fn QtLockedFile::~QtLockedFile() Destroys the \e QtLockedFile object. If any locks were held, they are released. -*/ - + */ } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.h b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.h index cbeb80fed..3f8622523 100644 --- a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.h +++ b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile.h @@ -4,25 +4,25 @@ * @file qtlockedfile.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -48,21 +48,19 @@ #endif namespace SharedTools { - -class QT_QTLOCKEDFILE_EXPORT QtLockedFile : public QFile -{ -public: +class QT_QTLOCKEDFILE_EXPORT QtLockedFile : public QFile { +public: enum LockMode { NoLock = 0, ReadLock, WriteLock }; QtLockedFile(); QtLockedFile(const QString &name); ~QtLockedFile(); - + bool lock(LockMode mode, bool block = true); bool unlock(); bool isLocked() const; LockMode lockMode() const; - + private: #ifdef Q_OS_WIN Qt::HANDLE m_semaphore_hnd; @@ -70,7 +68,6 @@ private: #endif LockMode m_lock_mode; }; - } // namespace SharedTools #endif // QTLOCKEDFILE_H diff --git a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_unix.cpp b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_unix.cpp index f4a0decc0..e0a460daf 100644 --- a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_unix.cpp +++ b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_unix.cpp @@ -4,25 +4,25 @@ * @file qtlockedfile_unix.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,38 +34,41 @@ #include namespace SharedTools { - bool QtLockedFile::lock(LockMode mode, bool block) { if (!isOpen()) { qWarning("QtLockedFile::lock(): file is not opened"); return false; } - - if (mode == NoLock) - return unlock(); - - if (mode == m_lock_mode) - return true; - if (m_lock_mode != NoLock) + if (mode == NoLock) { + return unlock(); + } + + if (mode == m_lock_mode) { + return true; + } + + if (m_lock_mode != NoLock) { unlock(); + } struct flock fl; fl.l_whence = SEEK_SET; - fl.l_start = 0; - fl.l_len = 0; - fl.l_type = (mode == ReadLock) ? F_RDLCK : F_WRLCK; + fl.l_start = 0; + fl.l_len = 0; + fl.l_type = (mode == ReadLock) ? F_RDLCK : F_WRLCK; int cmd = block ? F_SETLKW : F_SETLK; int ret = fcntl(handle(), cmd, &fl); - + if (ret == -1) { - if (errno != EINTR && errno != EAGAIN) + if (errno != EINTR && errno != EAGAIN) { qWarning("QtLockedFile::lock(): fcntl: %s", strerror(errno)); + } return false; } - + m_lock_mode = mode; return true; } @@ -78,29 +81,30 @@ bool QtLockedFile::unlock() return false; } - if (!isLocked()) + if (!isLocked()) { return true; + } struct flock fl; fl.l_whence = SEEK_SET; - fl.l_start = 0; - fl.l_len = 0; - fl.l_type = F_UNLCK; + fl.l_start = 0; + fl.l_len = 0; + fl.l_type = F_UNLCK; int ret = fcntl(handle(), F_SETLKW, &fl); - + if (ret == -1) { qWarning("QtLockedFile::lock(): fcntl: %s", strerror(errno)); return false; } - + m_lock_mode = NoLock; return true; } QtLockedFile::~QtLockedFile() { - if (isOpen()) + if (isOpen()) { unlock(); + } } - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_win.cpp b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_win.cpp index 061787896..fa63c2e22 100644 --- a/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_win.cpp +++ b/ground/openpilotgcs/src/shared/qtlockedfile/qtlockedfile_win.cpp @@ -4,25 +4,25 @@ * @file qtlockedfile_win.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,24 +32,26 @@ #include namespace SharedTools { - #define SEMAPHORE_PREFIX "QtLockedFile semaphore " -#define MUTEX_PREFIX "QtLockedFile mutex " -#define SEMAPHORE_MAX 100 +#define MUTEX_PREFIX "QtLockedFile mutex " +#define SEMAPHORE_MAX 100 static QString errorCodeToString(DWORD errorCode) { QString result; char *data = 0; - FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, - 0, errorCode, 0, - (char*)&data, 0, 0); - result = QString::fromLocal8Bit(data); - if (data != 0) - LocalFree(data); - if (result.endsWith("\n")) + FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, + 0, errorCode, 0, + (char *)&data, 0, 0); + result = QString::fromLocal8Bit(data); + if (data != 0) { + LocalFree(data); + } + + if (result.endsWith("\n")) { result.truncate(result.length() - 1); + } return result; } @@ -61,24 +63,27 @@ bool QtLockedFile::lock(LockMode mode, bool block) return false; } - if (mode == m_lock_mode) + if (mode == m_lock_mode) { return true; + } - if (m_lock_mode != 0) + if (m_lock_mode != 0) { unlock(); + } if (m_semaphore_hnd == 0) { QFileInfo fi(*this); QString sem_name = QString::fromLatin1(SEMAPHORE_PREFIX) + fi.absoluteFilePath().toLower(); - QT_WA( { - m_semaphore_hnd = CreateSemaphoreW(0, SEMAPHORE_MAX, SEMAPHORE_MAX, - (TCHAR*)sem_name.utf16()); - } , { - m_semaphore_hnd = CreateSemaphoreA(0, SEMAPHORE_MAX, SEMAPHORE_MAX, - sem_name.toLocal8Bit().constData()); - } ); + QT_WA({ + m_semaphore_hnd = CreateSemaphoreW(0, SEMAPHORE_MAX, SEMAPHORE_MAX, + (TCHAR *)sem_name.utf16()); + }, { + m_semaphore_hnd = CreateSemaphoreA(0, SEMAPHORE_MAX, SEMAPHORE_MAX, + sem_name.toLocal8Bit().constData()); + } + ); if (m_semaphore_hnd == 0) { qWarning("QtLockedFile::lock(): CreateSemaphore: %s", @@ -97,11 +102,12 @@ bool QtLockedFile::lock(LockMode mode, bool block) QFileInfo fi(*this); QString mut_name = QString::fromLatin1(MUTEX_PREFIX) + fi.absoluteFilePath().toLower(); - QT_WA( { - m_mutex_hnd = CreateMutexW(NULL, FALSE, (TCHAR*)mut_name.utf16()); - } , { - m_mutex_hnd = CreateMutexA(NULL, FALSE, mut_name.toLocal8Bit().constData()); - } ); + QT_WA({ + m_mutex_hnd = CreateMutexW(NULL, FALSE, (TCHAR *)mut_name.utf16()); + }, { + m_mutex_hnd = CreateMutexA(NULL, FALSE, mut_name.toLocal8Bit().constData()); + } + ); if (m_mutex_hnd == 0) { qWarning("QtLockedFile::lock(): CreateMutex: %s", @@ -110,8 +116,9 @@ bool QtLockedFile::lock(LockMode mode, bool block) } } DWORD res = WaitForSingleObject(m_mutex_hnd, block ? INFINITE : 0); - if (res == WAIT_TIMEOUT) + if (res == WAIT_TIMEOUT) { return false; + } if (res == WAIT_FAILED) { qWarning("QtLockedFile::lock(): WaitForSingleObject (mutex): %s", errorCodeToString(GetLastError()).toLatin1().constData()); @@ -131,22 +138,25 @@ bool QtLockedFile::lock(LockMode mode, bool block) // Fall through } } - if (gotMutex) + if (gotMutex) { ReleaseMutex(m_mutex_hnd); + } return false; - } + } if (res != WAIT_OBJECT_0) { - if (gotMutex) + if (gotMutex) { ReleaseMutex(m_mutex_hnd); + } qWarning("QtLockedFile::lock(): WaitForSingleObject (semaphore): %s", - errorCodeToString(GetLastError()).toLatin1().constData()); + errorCodeToString(GetLastError()).toLatin1().constData()); return false; } } m_lock_mode = mode; - if (gotMutex) + if (gotMutex) { ReleaseMutex(m_mutex_hnd); + } return true; } @@ -157,19 +167,21 @@ bool QtLockedFile::unlock() return false; } - if (!isLocked()) + if (!isLocked()) { return true; + } int increment; - if (m_lock_mode == ReadLock) + if (m_lock_mode == ReadLock) { increment = 1; - else + } else { increment = SEMAPHORE_MAX; + } DWORD ret = ReleaseSemaphore(m_semaphore_hnd, increment, 0); if (ret == 0) { qWarning("QtLockedFile::unlock(): ReleaseSemaphore: %s", - errorCodeToString(GetLastError()).toLatin1().constData()); + errorCodeToString(GetLastError()).toLatin1().constData()); return false; } @@ -179,13 +191,14 @@ bool QtLockedFile::unlock() QtLockedFile::~QtLockedFile() { - if (isOpen()) + if (isOpen()) { unlock(); + } if (m_mutex_hnd != 0) { DWORD ret = CloseHandle(m_mutex_hnd); if (ret == 0) { qWarning("QtLockedFile::~QtLockedFile(): CloseHandle (mutex): %s", - errorCodeToString(GetLastError()).toLatin1().constData()); + errorCodeToString(GetLastError()).toLatin1().constData()); } m_mutex_hnd = 0; } @@ -193,10 +206,9 @@ QtLockedFile::~QtLockedFile() DWORD ret = CloseHandle(m_semaphore_hnd); if (ret == 0) { qWarning("QtLockedFile::~QtLockedFile(): CloseHandle (semaphore): %s", - errorCodeToString(GetLastError()).toLatin1().constData()); + errorCodeToString(GetLastError()).toLatin1().constData()); } m_semaphore_hnd = 0; } } - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.cpp b/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.cpp index 1d9a8dd8d..448c26b49 100644 --- a/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.cpp +++ b/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.cpp @@ -4,25 +4,25 @@ * @file qtlocalpeer.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,7 +34,7 @@ #if defined(Q_OS_WIN) #include #include -typedef BOOL(WINAPI*PProcessIdToSessionId)(DWORD,DWORD*); +typedef BOOL (WINAPI * PProcessIdToSessionId)(DWORD, DWORD *); static PProcessIdToSessionId pProcessIdToSessionId = 0; #endif @@ -44,18 +44,17 @@ static PProcessIdToSessionId pProcessIdToSessionId = 0; #endif namespace SharedTools { - const char *QtLocalPeer::ack = "ack"; QtLocalPeer::QtLocalPeer(QObject *parent, const QString &appId) : QObject(parent), id(appId) { - if (id.isEmpty()) - id = QCoreApplication::applicationFilePath(); //### On win, check if this returns .../argv[0] without casefolding; .\MYAPP == .\myapp on Win - + if (id.isEmpty()) { + id = QCoreApplication::applicationFilePath(); // ### On win, check if this returns .../argv[0] without casefolding; .\MYAPP == .\myapp on Win + } QByteArray idc = id.toUtf8(); - quint16 idNum = qChecksum(idc.constData(), idc.size()); - //### could do: two 16bit checksums over separate halves of id, for a 32bit result - improved uniqeness probability. Every-other-char split would be best. + quint16 idNum = qChecksum(idc.constData(), idc.size()); + // ### could do: two 16bit checksums over separate halves of id, for a 32bit result - improved uniqeness probability. Every-other-char split would be best. socketName = QLatin1String("qtsingleapplication-") + QString::number(idNum, 16); @@ -73,7 +72,7 @@ QtLocalPeer::QtLocalPeer(QObject *parent, const QString &appId) socketName += QLatin1Char('-') + QString::number(::getuid(), 16); #endif - server = new QLocalServer(this); + server = new QLocalServer(this); QString lockName = QDir(QDir::tempPath()).absolutePath() + QLatin1Char('/') + socketName + QLatin1String("-lockfile"); @@ -83,34 +82,40 @@ QtLocalPeer::QtLocalPeer(QObject *parent, const QString &appId) bool QtLocalPeer::isClient() { - if (lockFile.isLocked()) + if (lockFile.isLocked()) { return false; + } - if (!lockFile.lock(QtLockedFile::WriteLock, false)) + if (!lockFile.lock(QtLockedFile::WriteLock, false)) { return true; + } - if (!QLocalServer::removeServer(socketName)) + if (!QLocalServer::removeServer(socketName)) { qWarning("QtSingleCoreApplication: could not cleanup socket"); + } bool res = server->listen(socketName); - if (!res) + if (!res) { qWarning("QtSingleCoreApplication: listen on local socket failed, %s", qPrintable(server->errorString())); + } QObject::connect(server, SIGNAL(newConnection()), SLOT(receiveConnection())); return false; } bool QtLocalPeer::sendMessage(const QString &message, int timeout) { - if (!isClient()) + if (!isClient()) { return false; + } QLocalSocket socket; bool connOk = false; for (int i = 0; i < 2; i++) { // Try twice, in case the other instance is just starting up socket.connectToServer(socketName); - connOk = socket.waitForConnected(timeout/2); - if (connOk || i) + connOk = socket.waitForConnected(timeout / 2); + if (connOk || i) { break; + } int ms = 250; #if defined(Q_OS_WIN) Sleep(DWORD(ms)); @@ -119,8 +124,9 @@ bool QtLocalPeer::sendMessage(const QString &message, int timeout) nanosleep(&ts, NULL); #endif } - if (!connOk) + if (!connOk) { return false; + } QByteArray uMsg(message.toUtf8()); QDataStream ds(&socket); @@ -133,28 +139,31 @@ bool QtLocalPeer::sendMessage(const QString &message, int timeout) void QtLocalPeer::receiveConnection() { - QLocalSocket* socket = server->nextPendingConnection(); - if (!socket) + QLocalSocket *socket = server->nextPendingConnection(); + + if (!socket) { return; + } // Why doesn't Qt have a blocking stream that takes care of this shait??? - while (socket->bytesAvailable() < static_cast(sizeof(quint32))) + while (socket->bytesAvailable() < static_cast(sizeof(quint32))) { socket->waitForReadyRead(); + } QDataStream ds(socket); QByteArray uMsg; quint32 remaining; ds >> remaining; uMsg.resize(remaining); int got = 0; - char* uMsgBuf = uMsg.data(); - //qDebug() << "RCV: remaining" << remaining; + char *uMsgBuf = uMsg.data(); + // qDebug() << "RCV: remaining" << remaining; do { - got = ds.readRawData(uMsgBuf, remaining); + got = ds.readRawData(uMsgBuf, remaining); remaining -= got; - uMsgBuf += got; - //qDebug() << "RCV: got" << got << "remaining" << remaining; + uMsgBuf += got; + // qDebug() << "RCV: got" << got << "remaining" << remaining; } while (remaining && got >= 0 && socket->waitForReadyRead(2000)); - //### error check: got<0 + // ### error check: got<0 if (got < 0) { qWarning() << "QtLocalPeer: Message reception failed" << socket->errorString(); delete socket; @@ -167,5 +176,4 @@ void QtLocalPeer::receiveConnection() delete socket; emit messageReceived(message); // ##(might take a long time to return) } - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.h b/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.h index 7d2a39152..82c215d04 100644 --- a/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.h +++ b/ground/openpilotgcs/src/shared/qtsingleapplication/qtlocalpeer.h @@ -4,25 +4,25 @@ * @file qtlocalpeer.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,9 +33,7 @@ #include namespace SharedTools { - -class QtLocalPeer : public QObject -{ +class QtLocalPeer : public QObject { Q_OBJECT public: @@ -43,7 +41,9 @@ public: bool isClient(); bool sendMessage(const QString &message, int timeout); QString applicationId() const - { return id; } + { + return id; + } Q_SIGNALS: void messageReceived(const QString &message); @@ -54,11 +54,10 @@ protected Q_SLOTS: protected: QString id; QString socketName; - QLocalServer* server; + QLocalServer *server; QtLockedFile lockFile; private: - static const char* ack; + static const char *ack; }; - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.cpp b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.cpp index 724fc20dd..5d1394cac 100644 --- a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.cpp +++ b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.cpp @@ -4,25 +4,25 @@ * @file qtsingleapplication.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -33,30 +33,29 @@ #include namespace SharedTools { - void QtSingleApplication::sysInit(const QString &appId) { actWin = 0; - peer = new QtLocalPeer(this, appId); - connect(peer, SIGNAL(messageReceived(const QString&)), SIGNAL(messageReceived(const QString&))); + peer = new QtLocalPeer(this, appId); + connect(peer, SIGNAL(messageReceived(const QString &)), SIGNAL(messageReceived(const QString &))); } -QtSingleApplication::QtSingleApplication(int &argc, char **argv, bool GUIenabled) +QtSingleApplication::QtSingleApplication(int &argc, char * *argv, bool GUIenabled) : QApplication(argc, argv, GUIenabled) { sysInit(); } -QtSingleApplication::QtSingleApplication(const QString &appId, int &argc, char **argv) +QtSingleApplication::QtSingleApplication(const QString &appId, int &argc, char * *argv) : QApplication(argc, argv) { sysInit(appId); } -QtSingleApplication::QtSingleApplication(int &argc, char **argv, Type type) +QtSingleApplication::QtSingleApplication(int &argc, char * *argv, Type type) : QApplication(argc, argv, type) { sysInit(); @@ -64,20 +63,20 @@ QtSingleApplication::QtSingleApplication(int &argc, char **argv, Type type) #if defined(Q_WS_X11) -QtSingleApplication::QtSingleApplication(Display* dpy, Qt::HANDLE visual, Qt::HANDLE colormap) +QtSingleApplication::QtSingleApplication(Display *dpy, Qt::HANDLE visual, Qt::HANDLE colormap) : QApplication(dpy, visual, colormap) { sysInit(); } -QtSingleApplication::QtSingleApplication(Display *dpy, int &argc, char **argv, Qt::HANDLE visual, Qt::HANDLE cmap) +QtSingleApplication::QtSingleApplication(Display *dpy, int &argc, char * *argv, Qt::HANDLE visual, Qt::HANDLE cmap) : QApplication(dpy, argc, argv, visual, cmap) { sysInit(); } -QtSingleApplication::QtSingleApplication(Display* dpy, const QString &appId, - int argc, char **argv, Qt::HANDLE visual, Qt::HANDLE colormap) +QtSingleApplication::QtSingleApplication(Display *dpy, const QString &appId, + int argc, char * *argv, Qt::HANDLE visual, Qt::HANDLE colormap) : QApplication(dpy, argc, argv, visual, colormap) { sysInit(appId); @@ -87,7 +86,7 @@ QtSingleApplication::QtSingleApplication(Display* dpy, const QString &appId, bool QtSingleApplication::event(QEvent *event) { if (event->type() == QEvent::FileOpen) { - QFileOpenEvent *foe = static_cast(event); + QFileOpenEvent *foe = static_cast(event); emit fileOpenRequest(foe->file()); return true; } @@ -115,14 +114,15 @@ QString QtSingleApplication::id() const void QtSingleApplication::setActivationWindow(QWidget *aw, bool activateOnMessage) { actWin = aw; - if (activateOnMessage) + if (activateOnMessage) { connect(peer, SIGNAL(messageReceived(QString)), this, SLOT(activateWindow())); - else + } else { disconnect(peer, SIGNAL(messageReceived(QString)), this, SLOT(activateWindow())); + } } -QWidget* QtSingleApplication::activationWindow() const +QWidget *QtSingleApplication::activationWindow() const { return actWin; } @@ -136,5 +136,4 @@ void QtSingleApplication::activateWindow() actWin->activateWindow(); } } - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.h b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.h index 9e0e7cd2b..2d1a6d05e 100644 --- a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.h +++ b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsingleapplication.h @@ -4,52 +4,50 @@ * @file qtsingleapplication.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include namespace SharedTools { - class QtLocalPeer; -class QtSingleApplication : public QApplication -{ +class QtSingleApplication : public QApplication { Q_OBJECT public: - QtSingleApplication(int &argc, char **argv, bool GUIenabled = true); - QtSingleApplication(const QString &id, int &argc, char **argv); - QtSingleApplication(int &argc, char **argv, Type type); + QtSingleApplication(int &argc, char * *argv, bool GUIenabled = true); + QtSingleApplication(const QString &id, int &argc, char * *argv); + QtSingleApplication(int &argc, char * *argv, Type type); #if defined(Q_WS_X11) QtSingleApplication(Display *dpy, Qt::HANDLE visual = 0, Qt::HANDLE colormap = 0); - QtSingleApplication(Display *dpy, int &argc, char **argv, Qt::HANDLE visual = 0, Qt::HANDLE cmap = 0); + QtSingleApplication(Display *dpy, int &argc, char * *argv, Qt::HANDLE visual = 0, Qt::HANDLE cmap = 0); #endif bool isRunning(); QString id() const; - void setActivationWindow(QWidget* aw, bool activateOnMessage = true); - QWidget* activationWindow() const; + void setActivationWindow(QWidget *aw, bool activateOnMessage = true); + QWidget *activationWindow() const; bool event(QEvent *event); @@ -57,13 +55,15 @@ public Q_SLOTS: bool sendMessage(const QString &message, int timeout = 5000); void activateWindow(); -//Obsolete methods: +// Obsolete methods: public: void initialize(bool = true) - { isRunning(); } + { + isRunning(); + } #if defined(Q_WS_X11) - QtSingleApplication(Display* dpy, const QString &id, int argc, char **argv, Qt::HANDLE visual = 0, Qt::HANDLE colormap = 0); + QtSingleApplication(Display *dpy, const QString &id, int argc, char * *argv, Qt::HANDLE visual = 0, Qt::HANDLE colormap = 0); #endif // end obsolete methods @@ -76,5 +76,4 @@ private: QtLocalPeer *peer; QWidget *actWin; }; - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.cpp b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.cpp index ac556eae7..47eb0c359 100644 --- a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.cpp +++ b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.cpp @@ -4,25 +4,25 @@ * @file qtsinglecoreapplication.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -30,8 +30,7 @@ #include "qtlocalpeer.h" namespace SharedTools { - -QtSingleCoreApplication::QtSingleCoreApplication(int &argc, char **argv) +QtSingleCoreApplication::QtSingleCoreApplication(int &argc, char * *argv) : QCoreApplication(argc, argv) { peer = new QtLocalPeer(this); @@ -39,7 +38,7 @@ QtSingleCoreApplication::QtSingleCoreApplication(int &argc, char **argv) } -QtSingleCoreApplication::QtSingleCoreApplication(const QString &appId, int &argc, char **argv) +QtSingleCoreApplication::QtSingleCoreApplication(const QString &appId, int &argc, char * *argv) : QCoreApplication(argc, argv) { peer = new QtLocalPeer(this, appId); @@ -63,5 +62,4 @@ QString QtSingleCoreApplication::id() const { return peer->applicationId(); } - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.h b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.h index 9e8ca5344..0a63e243a 100644 --- a/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.h +++ b/ground/openpilotgcs/src/shared/qtsingleapplication/qtsinglecoreapplication.h @@ -4,41 +4,39 @@ * @file qtsinglecoreapplication.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include namespace SharedTools { - class QtLocalPeer; -class QtSingleCoreApplication : public QCoreApplication -{ +class QtSingleCoreApplication : public QCoreApplication { Q_OBJECT public: - QtSingleCoreApplication(int &argc, char **argv); - QtSingleCoreApplication(const QString &id, int &argc, char **argv); + QtSingleCoreApplication(int &argc, char * *argv); + QtSingleCoreApplication(const QString &id, int &argc, char * *argv); bool isRunning(); QString id() const; @@ -52,7 +50,6 @@ Q_SIGNALS: private: - QtLocalPeer* peer; + QtLocalPeer *peer; }; - } // namespace SharedTools diff --git a/ground/openpilotgcs/src/shared/scriptwrapper/interface_wrap_helpers.h b/ground/openpilotgcs/src/shared/scriptwrapper/interface_wrap_helpers.h index aacc00dd2..a754a63b0 100644 --- a/ground/openpilotgcs/src/shared/scriptwrapper/interface_wrap_helpers.h +++ b/ground/openpilotgcs/src/shared/scriptwrapper/interface_wrap_helpers.h @@ -4,25 +4,25 @@ * @file interface_wrap_helpers.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -32,21 +32,21 @@ #include namespace SharedTools { - // Convert a QObjectInterface to Scriptvalue // To be registered as a magic creation function with qScriptRegisterMetaType(). // (see registerQObjectInterface) template -static QScriptValue qObjectInterfaceToScriptValue(QScriptEngine *engine, QObjectInterface* const &qoif) +static QScriptValue qObjectInterfaceToScriptValue(QScriptEngine *engine, QObjectInterface *const &qoif) { - if (!qoif) - return QScriptValue(engine, QScriptValue::NullValue); + if (!qoif) { + return QScriptValue(engine, QScriptValue::NullValue); + } QObject *qObject = const_cast(qoif); const QScriptEngine::QObjectWrapOptions wrapOptions = - QScriptEngine::ExcludeChildObjects|QScriptEngine::ExcludeSuperClassMethods|QScriptEngine::ExcludeSuperClassProperties; + QScriptEngine::ExcludeChildObjects | QScriptEngine::ExcludeSuperClassMethods | QScriptEngine::ExcludeSuperClassProperties; return engine->newQObject(qObject, QScriptEngine::QtOwnership, wrapOptions); } @@ -55,10 +55,11 @@ static QScriptValue qObjectInterfaceToScriptValue(QScriptEngine *engine, QObject // (see registerQObjectInterface) template -static void scriptValueToQObjectInterface(const QScriptValue &sv, QObjectInterface *&p) +static void scriptValueToQObjectInterface(const QScriptValue &sv, QObjectInterface * &p) { - QObject *qObject = sv.toQObject(); - p = qobject_cast(qObject); + QObject *qObject = sv.toQObject(); + + p = qobject_cast(qObject); } // Magically register a Workbench interface derived from @@ -74,14 +75,14 @@ static void registerQObjectInterface(QScriptEngine *engine) Prototype *protoType = new Prototype(engine); const QScriptValue scriptProtoType = engine->newQObject(protoType); - const int metaTypeId = qScriptRegisterMetaType( + const int metaTypeId = qScriptRegisterMetaType( engine, qObjectInterfaceToScriptValue, scriptValueToQObjectInterface, scriptProtoType); + Q_UNUSED(metaTypeId) } - } // namespace SharedTools #endif // INTERFACE_WRAP_HELPERS_H diff --git a/ground/openpilotgcs/src/shared/scriptwrapper/wrap_helpers.h b/ground/openpilotgcs/src/shared/scriptwrapper/wrap_helpers.h index 6a8b98d0f..8c045227f 100644 --- a/ground/openpilotgcs/src/shared/scriptwrapper/wrap_helpers.h +++ b/ground/openpilotgcs/src/shared/scriptwrapper/wrap_helpers.h @@ -4,25 +4,25 @@ * @file wrap_helpers.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @brief + * @brief * @see The GNU Public License (GPL) Version 3 - * @defgroup + * @defgroup * @{ - * + * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ @@ -34,27 +34,27 @@ #include namespace SharedTools { - // Strip a const ref from a type via template specialization trick. // Use for determining function call args template - struct RemoveConstRef { - typedef T Result; - }; +struct RemoveConstRef { + typedef T Result; +}; template - struct RemoveConstRef { - typedef T Result; - }; +struct RemoveConstRef { + typedef T Result; +}; // Template that retrieves a QObject-derived class from a QScriptValue. template - QObjectDerived *qObjectFromScriptValue(const QScriptValue &v) +QObjectDerived *qObjectFromScriptValue(const QScriptValue &v) { - if (!v.isQObject()) + if (!v.isQObject()) { return 0; + } QObject *o = v.toQObject(); return qobject_cast(o); } @@ -63,13 +63,15 @@ template // The wrapped object is accessed through an accessor of // the QObject-derived wrapper. -template - Wrapped *wrappedFromScriptValue(const QScriptValue &v, - Wrapped * (Wrapper::*wrappedAccessor) () const) +template +Wrapped *wrappedFromScriptValue(const QScriptValue &v, + Wrapped * (Wrapper::*wrappedAccessor)() const) { Wrapper *wrapper = qObjectFromScriptValue(v); - if (!wrapper) + + if (!wrapper) { return 0; + } return (wrapper->*wrappedAccessor)(); } @@ -77,11 +79,12 @@ template // a QObject-derived script wrapper object that is set as 'this' in // a script context via accessor. -template - static inline Wrapped *wrappedThisFromContext(QScriptContext *context, - Wrapped * (Wrapper::*wrappedAccessor) () const) +template +static inline Wrapped *wrappedThisFromContext(QScriptContext *context, + Wrapped * (Wrapper::*wrappedAccessor)() const) { Wrapped *wrapped = wrappedFromScriptValue(context->thisObject(), wrappedAccessor); + Q_ASSERT(wrapped); return wrapped; } @@ -91,12 +94,13 @@ template // the core interface accessors). Mangles out the wrapper object from // thisObject(), accesses the wrapped object and returns the contained object. -template - static inline Contained *containedFromContext(QScriptContext *context, - Wrapped * (Wrapper::*wrappedAccessor) () const, - Contained * (Wrapped::*containedAccessor)() const) +template +static inline Contained *containedFromContext(QScriptContext *context, + Wrapped * (Wrapper::*wrappedAccessor)() const, + Contained * (Wrapped::*containedAccessor)() const) { Wrapped *wrapped = wrappedThisFromContext(context, wrappedAccessor); + return (wrapped->*containedAccessor)(); } @@ -104,10 +108,10 @@ template // in a script getter call and creates a new script-object via engine->newQObject(). // To be called from a script getter callback. -template - static inline QScriptValue containedQObjectFromContextToScriptValue(QScriptContext *context, QScriptEngine *engine, - Wrapped * (Wrapper::*wrappedAccessor) () const, - Contained * (Wrapped::*containedAccessor)() const) +template +static inline QScriptValue containedQObjectFromContextToScriptValue(QScriptContext *context, QScriptEngine *engine, + Wrapped * (Wrapper::*wrappedAccessor)() const, + Contained * (Wrapped::*containedAccessor)() const) { return engine->newQObject(containedFromContext(context, wrappedAccessor, containedAccessor)); } @@ -117,14 +121,16 @@ template // a new instance of ContainedWrapper (which casts to QScriptValue). // To be called from a script getter callback. -template - static inline QScriptValue wrapContainedFromContextAsScriptValue(QScriptContext *context, QScriptEngine *engine, - Wrapped * (Wrapper::*wrappedAccessor) () const, - Contained * (Wrapped::*containedAccessor)() const) +template +static inline QScriptValue wrapContainedFromContextAsScriptValue(QScriptContext *context, QScriptEngine *engine, + Wrapped * (Wrapper::*wrappedAccessor)() const, + Contained * (Wrapped::*containedAccessor)() const) { Contained *c = containedFromContext(context, wrappedAccessor, containedAccessor); - if (!c) + + if (!c) { return QScriptValue(engine, QScriptValue::NullValue); + } ContainedWrapper *cw = new ContainedWrapper(*engine, c); return *cw; // cast to QScriptValue @@ -134,64 +140,70 @@ template - static inline QScriptValue scriptCallConstMember_0(QScriptContext *context, QScriptEngine *engine, - Wrapped * (Wrapper::*wrappedAccessor) () const, - Ret (Wrapped::*member)() const) +template +static inline QScriptValue scriptCallConstMember_0(QScriptContext *context, QScriptEngine *engine, + Wrapped * (Wrapper::*wrappedAccessor)() const, + Ret (Wrapped::*member)() const) { Wrapped *wrapped = wrappedThisFromContext(context, wrappedAccessor); - return engine->toScriptValue( (wrapped->*member)() ); + + return engine->toScriptValue((wrapped->*member)()); } // Ditto for non-const -template - static inline QScriptValue scriptCallMember_0(QScriptContext *context, QScriptEngine *engine, - Wrapped * (Wrapper::*wrappedAccessor) () const, - Ret (Wrapped::*member)()) +template +static inline QScriptValue scriptCallMember_0(QScriptContext *context, QScriptEngine *engine, + Wrapped * (Wrapper::*wrappedAccessor)() const, + Ret (Wrapped::*member)()) { Wrapped *wrapped = wrappedThisFromContext(context, wrappedAccessor); - return engine->toScriptValue( (wrapped->*member)() ); + + return engine->toScriptValue((wrapped->*member)()); } // Template that retrieves a wrapped object from context (this) // and calls a const-member function with 1 parameter on it. // To be called from a script getter callback. -template - static inline QScriptValue scriptCallConstMember_1(QScriptContext *context, QScriptEngine *engine, - Wrapped * (Wrapper::*wrappedAccessor) () const, - Ret (Wrapped::*member)(Argument a1) const) +template +static inline QScriptValue scriptCallConstMember_1(QScriptContext *context, QScriptEngine *engine, + Wrapped * (Wrapper::*wrappedAccessor)() const, + Ret (Wrapped::*member)(Argument a1) const) { const int argumentCount = context->argumentCount(); - if ( argumentCount != 1) - return QScriptValue (engine, QScriptValue::NullValue); + + if (argumentCount != 1) { + return QScriptValue(engine, QScriptValue::NullValue); + } Wrapped *wrapped = wrappedThisFromContext(context, wrappedAccessor); // call member. If the argument is a const ref, strip it. typedef typename RemoveConstRef::Result ArgumentBase; - ArgumentBase a = qscriptvalue_cast(context->argument(0)); - return engine->toScriptValue( (wrapped->*member)(a) ); + ArgumentBase a = qscriptvalue_cast(context->argument(0)); + return engine->toScriptValue((wrapped->*member)(a)); } // Template that retrieves a wrapped object // and calls a member function with 1 parameter on it. // To be called from a script getter callback. -template - static inline QScriptValue scriptCallMember_1(QScriptContext *context, QScriptEngine *engine, - Wrapped * (Wrapper::*wrappedAccessor) () const, - Ret (Wrapped::*member)(Argument a1)) +template +static inline QScriptValue scriptCallMember_1(QScriptContext *context, QScriptEngine *engine, + Wrapped * (Wrapper::*wrappedAccessor)() const, + Ret (Wrapped::*member)(Argument a1)) { const int argumentCount = context->argumentCount(); - if ( argumentCount != 1) - return QScriptValue (engine, QScriptValue::NullValue); + + if (argumentCount != 1) { + return QScriptValue(engine, QScriptValue::NullValue); + } Wrapped *wrapped = wrappedThisFromContext(context, wrappedAccessor); // call member. If the argument is a const ref, strip it. typedef typename RemoveConstRef::Result ArgumentBase; - ArgumentBase a = qscriptvalue_cast(context->argument(0)); - return engine->toScriptValue( (wrapped->*member)(a) ); + ArgumentBase a = qscriptvalue_cast(context->argument(0)); + return engine->toScriptValue((wrapped->*member)(a)); } // Template that retrieves a wrapped object @@ -200,52 +212,56 @@ template // Typically used for something like 'setCurrentEditor(Editor*)' // To be called from a script callback. -template +template static QScriptValue scriptCallVoidMember_Wrapped1(QScriptContext *context, QScriptEngine *engine, - ThisWrapped * (ThisWrapper::*thisWrappedAccessor) () const, + ThisWrapped * (ThisWrapper::*thisWrappedAccessor)() const, ArgumentWrapped *(ArgumentWrapper::*argumentWrappedAccessor)() const, - void (ThisWrapped::*member)(ArgumentWrapped *a1), + void (ThisWrapped::*member)(ArgumentWrapped *a1), bool acceptNullArgument = false) { const QScriptValue voidRC = QScriptValue(engine, QScriptValue::UndefinedValue); - if (context->argumentCount() < 1) + + if (context->argumentCount() < 1) { return voidRC; + } ThisWrapped *thisWrapped = wrappedThisFromContext(context, thisWrappedAccessor); ArgumentWrapped *aw = wrappedFromScriptValue(context->argument(0), argumentWrappedAccessor); - if (acceptNullArgument || aw) + if (acceptNullArgument || aw) { (thisWrapped->*member)(aw); + } return voidRC; } // Macros that define the static functions to call members #define SCRIPT_CALL_CONST_MEMBER_0(funcName, accessor, member) \ -static QScriptValue funcName(QScriptContext *context, QScriptEngine *engine) \ -{ return SharedTools::scriptCallConstMember_0(context, engine, accessor, member); } + static QScriptValue funcName(QScriptContext * context, QScriptEngine * engine) \ + { return SharedTools::scriptCallConstMember_0(context, engine, accessor, member); } #define SCRIPT_CALL_MEMBER_0(funcName, accessor, member) \ -static QScriptValue funcName(QScriptContext *context, QScriptEngine *engine) \ -{ return SharedTools::scriptCallMember_0(context, engine, accessor, member); } + static QScriptValue funcName(QScriptContext * context, QScriptEngine * engine) \ + { return SharedTools::scriptCallMember_0(context, engine, accessor, member); } #define SCRIPT_CALL_CONST_MEMBER_1(funcName, accessor, member) \ -static QScriptValue funcName(QScriptContext *context, QScriptEngine *engine) \ -{ return SharedTools::scriptCallConstMember_1(context, engine, accessor, member); } + static QScriptValue funcName(QScriptContext * context, QScriptEngine * engine) \ + { return SharedTools::scriptCallConstMember_1(context, engine, accessor, member); } #define SCRIPT_CALL_MEMBER_1(funcName, accessor, member) \ -static QScriptValue funcName(QScriptContext *context, QScriptEngine *engine) \ -{ return SharedTools::scriptCallMember_1(context, engine, accessor, member); } + static QScriptValue funcName(QScriptContext * context, QScriptEngine * engine) \ + { return SharedTools::scriptCallMember_1(context, engine, accessor, member); } // Create a script list of wrapped non-qobjects by wrapping them. // Wrapper must cast to QScriptValue. template - static inline QScriptValue wrapObjectList( QScriptEngine *engine, Iterator i1, Iterator i2) +static inline QScriptValue wrapObjectList(QScriptEngine *engine, Iterator i1, Iterator i2) { QScriptValue rc = engine->newArray(i2 - i1); // Grrr! quint32 i = 0; - for ( ; i1 != i2 ; ++i1, i++) { - Wrapper * wrapper = new Wrapper(*engine, *i1); + + for (; i1 != i2; ++i1, i++) { + Wrapper *wrapper = new Wrapper(*engine, *i1); rc.setProperty(i, *wrapper); } return rc; @@ -254,24 +270,27 @@ template // Unwrap a list of wrapped objects from a script list. template - static inline QList unwrapObjectList(const QScriptValue &v, - Wrapped *(Wrapper::*wrappedAccessor)() const) +static inline QList unwrapObjectList(const QScriptValue &v, + Wrapped *(Wrapper::*wrappedAccessor)() const) { - QList rc; + QList rc; - if (!v.isArray()) + if (!v.isArray()) { return rc; + } - const quint32 len = v.property(QLatin1String("length")).toUInt32(); - if (!len) + const quint32 len = v.property(QLatin1String("length")).toUInt32(); + if (!len) { return rc; + } for (quint32 i = 0; i < len; i++) { const QScriptValue e = v.property(i); if (e.isQObject()) { QObject *o = e.toQObject(); - if (Wrapper * wrapper = qobject_cast(o)) + if (Wrapper * wrapper = qobject_cast(o)) { rc.push_back((wrapper->*wrappedAccessor)()); + } } } return rc; @@ -281,12 +300,12 @@ template // that can be converted via script value casts via Q_DECLARE_METATYPE. template - static void registerInterfaceWithDefaultPrototype(QScriptEngine &engine) +static void registerInterfaceWithDefaultPrototype(QScriptEngine &engine) { Prototype *protoType = new Prototype(&engine); const QScriptValue scriptProtoType = engine.newQObject(protoType); - engine.setDefaultPrototype(qMetaTypeId(), scriptProtoType); + engine.setDefaultPrototype(qMetaTypeId(), scriptProtoType); } // Convert a class derived from QObject to Scriptvalue via engine->newQObject() to make @@ -295,7 +314,7 @@ template // (see registerQObject() template -static QScriptValue qObjectToScriptValue(QScriptEngine *engine, SomeQObject * const &qo) +static QScriptValue qObjectToScriptValue(QScriptEngine *engine, SomeQObject *const &qo) { return engine->newQObject(qo, QScriptEngine::QtOwnership, QScriptEngine::ExcludeChildObjects); } @@ -307,8 +326,9 @@ static QScriptValue qObjectToScriptValue(QScriptEngine *engine, SomeQObject * co template static void scriptValueToQObject(const QScriptValue &sv, SomeQObject * &p) { - QObject *qObject = sv.toQObject(); - p = qobject_cast(qObject); + QObject *qObject = sv.toQObject(); + + p = qobject_cast(qObject); Q_ASSERT(p); } @@ -320,11 +340,10 @@ static void scriptValueToQObject(const QScriptValue &sv, SomeQObject * &p) template static void registerQObject(QScriptEngine *engine) { - qScriptRegisterMetaType(engine, - qObjectToScriptValue, - scriptValueToQObject); + qScriptRegisterMetaType(engine, + qObjectToScriptValue, + scriptValueToQObject); } - } // namespace SharedTools #endif // WRAP_HELPERS_H diff --git a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp index 18b97a225..2bc7623e7 100644 --- a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp +++ b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp @@ -28,67 +28,67 @@ using namespace std; -bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString templatepath,QString outputpath) { +bool UAVObjectGeneratorFlight::generate(UAVObjectParser *parser, QString templatepath, QString outputpath) +{ + fieldTypeStrC << "int8_t" << "int16_t" << "int32_t" << "uint8_t" + << "uint16_t" << "uint32_t" << "float" << "uint8_t"; - fieldTypeStrC << "int8_t" << "int16_t" << "int32_t" <<"uint8_t" - <<"uint16_t" << "uint32_t" << "float" << "uint8_t"; - - QString flightObjInit,objInc,objFileNames,objNames; + QString flightObjInit, objInc, objFileNames, objNames; qint32 sizeCalc; - flightCodePath = QDir( templatepath + QString(FLIGHT_CODE_DIR)); - flightOutputPath = QDir( outputpath + QString("flight") ); + flightCodePath = QDir(templatepath + QString(FLIGHT_CODE_DIR)); + flightOutputPath = QDir(outputpath + QString("flight")); flightOutputPath.mkpath(flightOutputPath.absolutePath()); - flightCodeTemplate = readFile( flightCodePath.absoluteFilePath("uavobject.c.template") ); - flightIncludeTemplate = readFile( flightCodePath.absoluteFilePath("inc/uavobject.h.template") ); - flightInitTemplate = readFile( flightCodePath.absoluteFilePath("uavobjectsinit.c.template") ); - flightInitIncludeTemplate = readFile( flightCodePath.absoluteFilePath("inc/uavobjectsinit.h.template") ); - flightMakeTemplate = readFile( flightCodePath.absoluteFilePath("Makefile.inc.template") ); + flightCodeTemplate = readFile(flightCodePath.absoluteFilePath("uavobject.c.template")); + flightIncludeTemplate = readFile(flightCodePath.absoluteFilePath("inc/uavobject.h.template")); + flightInitTemplate = readFile(flightCodePath.absoluteFilePath("uavobjectsinit.c.template")); + flightInitIncludeTemplate = readFile(flightCodePath.absoluteFilePath("inc/uavobjectsinit.h.template")); + flightMakeTemplate = readFile(flightCodePath.absoluteFilePath("Makefile.inc.template")); - if ( flightCodeTemplate.isNull() || flightIncludeTemplate.isNull() || flightInitTemplate.isNull()) { - cerr << "Error: Could not open flight template files." << endl; - return false; - } + if (flightCodeTemplate.isNull() || flightIncludeTemplate.isNull() || flightInitTemplate.isNull()) { + cerr << "Error: Could not open flight template files." << endl; + return false; + } sizeCalc = 0; for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { - ObjectInfo* info=parser->getObjectByIndex(objidx); + ObjectInfo *info = parser->getObjectByIndex(objidx); process_object(info); - flightObjInit.append("#ifdef UAVOBJ_INIT_" + info->namelc +"\n"); + flightObjInit.append("#ifdef UAVOBJ_INIT_" + info->namelc + "\n"); flightObjInit.append(" " + info->name + "Initialize();\n"); flightObjInit.append("#endif\n"); objInc.append("#include \"" + info->namelc + ".h\"\n"); - objFileNames.append(" " + info->namelc); - objNames.append(" " + info->name); - if (parser->getNumBytes(objidx)>sizeCalc) { - sizeCalc = parser->getNumBytes(objidx); - } + objFileNames.append(" " + info->namelc); + objNames.append(" " + info->name); + if (parser->getNumBytes(objidx) > sizeCalc) { + sizeCalc = parser->getNumBytes(objidx); + } } // Write the flight object inialization files - flightInitTemplate.replace( QString("$(OBJINC)"), objInc); - flightInitTemplate.replace( QString("$(OBJINIT)"), flightObjInit); - bool res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/uavobjectsinit.c", - flightInitTemplate ); + flightInitTemplate.replace(QString("$(OBJINC)"), objInc); + flightInitTemplate.replace(QString("$(OBJINIT)"), flightObjInit); + bool res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/uavobjectsinit.c", + flightInitTemplate); if (!res) { cout << "Error: Could not write flight object init file" << endl; return false; } // Write the flight object initialization header - flightInitIncludeTemplate.replace( QString("$(SIZECALCULATION)"), QString().setNum(sizeCalc)); - res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/uavobjectsinit.h", - flightInitIncludeTemplate ); + flightInitIncludeTemplate.replace(QString("$(SIZECALCULATION)"), QString().setNum(sizeCalc)); + res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/uavobjectsinit.h", + flightInitIncludeTemplate); if (!res) { cout << "Error: Could not write flight object init header file" << endl; return false; } // Write the flight object Makefile - flightMakeTemplate.replace( QString("$(UAVOBJFILENAMES)"), objFileNames); - flightMakeTemplate.replace( QString("$(UAVOBJNAMES)"), objNames); - res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/Makefile.inc", - flightMakeTemplate ); + flightMakeTemplate.replace(QString("$(UAVOBJFILENAMES)"), objFileNames); + flightMakeTemplate.replace(QString("$(UAVOBJNAMES)"), objNames); + res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/Makefile.inc", + flightMakeTemplate); if (!res) { cout << "Error: Could not write flight Makefile" << endl; return false; @@ -100,15 +100,16 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template /** * Generate the Flight object files -**/ -bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info) + **/ +bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info) { - if (info == NULL) + if (info == NULL) { return false; + } // Prepare output strings QString outInclude = flightIncludeTemplate; - QString outCode = flightCodeTemplate; + QString outCode = flightCodeTemplate; // Replace common tags replaceCommonTags(outInclude, info); @@ -117,77 +118,66 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info) // Replace the $(DATAFIELDS) tag QString type; QString fields; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { // Determine type type = fieldTypeStrC[info->fields[n]->type]; // Append field - if ( info->fields[n]->numElements > 1 ) - { - fields.append( QString(" %1 %2[%3];\n").arg(type) - .arg(info->fields[n]->name).arg(info->fields[n]->numElements) ); - } - else - { - fields.append( QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name) ); + if (info->fields[n]->numElements > 1) { + fields.append(QString(" %1 %2[%3];\n").arg(type) + .arg(info->fields[n]->name).arg(info->fields[n]->numElements)); + } else { + fields.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name)); } } outInclude.replace(QString("$(DATAFIELDS)"), fields); // Replace the $(DATAFIELDINFO) tag QString enums; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { enums.append(QString("/* Field %1 information */\n").arg(info->fields[n]->name)); // Only for enum types - if (info->fields[n]->type == FIELDTYPE_ENUM) - { + if (info->fields[n]->type == FIELDTYPE_ENUM) { enums.append(QString("\n// Enumeration options for field %1\n").arg(info->fields[n]->name)); enums.append("typedef enum {\n"); // Go through each option QStringList options = info->fields[n]->options; for (int m = 0; m < options.length(); ++m) { - QString s = (m == (options.length()-1)) ? " %1_%2_%3=%4\n" : " %1_%2_%3=%4,\n"; - enums.append( s - .arg( info->name.toUpper() ) - .arg( info->fields[n]->name.toUpper() ) - .arg( options[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), "") ) - .arg(m) ); - + QString s = (m == (options.length() - 1)) ? " %1_%2_%3=%4\n" : " %1_%2_%3=%4,\n"; + enums.append(s + .arg(info->name.toUpper()) + .arg(info->fields[n]->name.toUpper()) + .arg(options[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), "")) + .arg(m)); } - enums.append( QString("} %1%2Options;\n") - .arg( info->name ) - .arg( info->fields[n]->name ) ); + enums.append(QString("} %1%2Options;\n") + .arg(info->name) + .arg(info->fields[n]->name)); } // Generate element names (only if field has more than one element) - if (info->fields[n]->numElements > 1 && !info->fields[n]->defaultElementNames) - { + if (info->fields[n]->numElements > 1 && !info->fields[n]->defaultElementNames) { enums.append(QString("\n// Array element names for field %1\n").arg(info->fields[n]->name)); enums.append("typedef enum {\n"); // Go through the element names QStringList elemNames = info->fields[n]->elementNames; - for (int m = 0; m < elemNames.length(); ++m) - { - QString s = (m != (elemNames.length()-1)) ? " %1_%2_%3=%4,\n" : " %1_%2_%3=%4\n"; - enums.append( s - .arg( info->name.toUpper() ) - .arg( info->fields[n]->name.toUpper() ) - .arg( elemNames[m].toUpper() ) - .arg(m) ); - + for (int m = 0; m < elemNames.length(); ++m) { + QString s = (m != (elemNames.length() - 1)) ? " %1_%2_%3=%4,\n" : " %1_%2_%3=%4\n"; + enums.append(s + .arg(info->name.toUpper()) + .arg(info->fields[n]->name.toUpper()) + .arg(elemNames[m].toUpper()) + .arg(m)); } - enums.append( QString("} %1%2Elem;\n") - .arg( info->name ) - .arg( info->fields[n]->name ) ); + enums.append(QString("} %1%2Elem;\n") + .arg(info->name) + .arg(info->fields[n]->name)); } // Generate array information - if (info->fields[n]->numElements > 1) - { + if (info->fields[n]->numElements > 1) { enums.append(QString("\n// Number of elements for field %1\n").arg(info->fields[n]->name)); - enums.append( QString("#define %1_%2_NUMELEM %3\n") - .arg( info->name.toUpper() ) - .arg( info->fields[n]->name.toUpper() ) - .arg( info->fields[n]->numElements ) ); + enums.append(QString("#define %1_%2_NUMELEM %3\n") + .arg(info->name.toUpper()) + .arg(info->fields[n]->name.toUpper()) + .arg(info->fields[n]->numElements)); } enums.append(QString("\n")); @@ -196,57 +186,41 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info) // Replace the $(INITFIELDS) tag QString initfields; - for (int n = 0; n < info->fields.length(); ++n) - { - if (!info->fields[n]->defaultValues.isEmpty() ) - { + for (int n = 0; n < info->fields.length(); ++n) { + if (!info->fields[n]->defaultValues.isEmpty()) { // For non-array fields - if ( info->fields[n]->numElements == 1) - { - if ( info->fields[n]->type == FIELDTYPE_ENUM ) - { - initfields.append( QString(" data.%1 = %2;\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->options.indexOf( info->fields[n]->defaultValues[0] ) ) ); + if (info->fields[n]->numElements == 1) { + if (info->fields[n]->type == FIELDTYPE_ENUM) { + initfields.append(QString(" data.%1 = %2;\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->options.indexOf(info->fields[n]->defaultValues[0]))); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + initfields.append(QString(" data.%1 = %2f;\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0].toFloat(), 0, 'e', 6)); + } else { + initfields.append(QString(" data.%1 = %2;\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0].toInt())); } - else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) - { - initfields.append( QString(" data.%1 = %2f;\n") - .arg(info->fields[n]->name) - .arg(info->fields[n]->defaultValues[0].toFloat(), 0, 'e', 6)); - } - else - { - initfields.append( QString(" data.%1 = %2;\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->defaultValues[0].toInt() ) ); - } - } - else - { + } else { // Initialize all fields in the array - for (int idx = 0; idx < info->fields[n]->numElements; ++idx) - { - if ( info->fields[n]->type == FIELDTYPE_ENUM ) - { - initfields.append( QString(" data.%1[%2] = %3;\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->options.indexOf( info->fields[n]->defaultValues[idx] ) ) ); - } - else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) - { - initfields.append( QString(" data.%1[%2] = %3f;\n") - .arg(info->fields[n]->name) - .arg(idx) - .arg(info->fields[n]->defaultValues[idx].toFloat(), 0, 'e', 6)); - } - else - { - initfields.append( QString(" data.%1[%2] = %3;\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->defaultValues[idx].toInt() ) ); + for (int idx = 0; idx < info->fields[n]->numElements; ++idx) { + if (info->fields[n]->type == FIELDTYPE_ENUM) { + initfields.append(QString(" data.%1[%2] = %3;\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->options.indexOf(info->fields[n]->defaultValues[idx]))); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + initfields.append(QString(" data.%1[%2] = %3f;\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx].toFloat(), 0, 'e', 6)); + } else { + initfields.append(QString(" data.%1[%2] = %3;\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx].toInt())); } } } @@ -256,102 +230,93 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info) // Replace the $(SETGETFIELDS) tag QString setgetfields; - for (int n = 0; n < info->fields.length(); ++n) - { - //if (!info->fields[n]->defaultValues.isEmpty() ) + for (int n = 0; n < info->fields.length(); ++n) { + // if (!info->fields[n]->defaultValues.isEmpty() ) { // For non-array fields - if ( info->fields[n]->numElements == 1) - { + if (info->fields[n]->numElements == 1) { + /* Set */ + setgetfields.append(QString("void %2%3Set(%1 *New%3)\n") + .arg(fieldTypeStrC[info->fields[n]->type]) + .arg(info->name) + .arg(info->fields[n]->name)); + setgetfields.append(QString("{\n")); + setgetfields.append(QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), sizeof(%3));\n") + .arg(info->name) + .arg(info->fields[n]->name) + .arg(fieldTypeStrC[info->fields[n]->type])); + setgetfields.append(QString("}\n")); - /* Set */ - setgetfields.append( QString("void %2%3Set(%1 *New%3)\n") - .arg( fieldTypeStrC[info->fields[n]->type] ) - .arg( info->name ) - .arg( info->fields[n]->name ) ); - setgetfields.append( QString("{\n") ); - setgetfields.append( QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), sizeof(%3));\n") - .arg( info->name ) - .arg( info->fields[n]->name ) - .arg( fieldTypeStrC[info->fields[n]->type] ) ); - setgetfields.append( QString("}\n") ); + /* GET */ + setgetfields.append(QString("void %2%3Get(%1 *New%3)\n") + .arg(fieldTypeStrC[info->fields[n]->type]) + .arg(info->name) + .arg(info->fields[n]->name)); + setgetfields.append(QString("{\n")); + setgetfields.append(QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), sizeof(%3));\n") + .arg(info->name) + .arg(info->fields[n]->name) + .arg(fieldTypeStrC[info->fields[n]->type])); + setgetfields.append(QString("}\n")); + } else { + /* SET */ + setgetfields.append(QString("void %2%3Set( %1 *New%3 )\n") + .arg(fieldTypeStrC[info->fields[n]->type]) + .arg(info->name) + .arg(info->fields[n]->name)); + setgetfields.append(QString("{\n")); + setgetfields.append(QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n") + .arg(info->name) + .arg(info->fields[n]->name) + .arg(info->fields[n]->numElements) + .arg(fieldTypeStrC[info->fields[n]->type])); + setgetfields.append(QString("}\n")); - /* GET */ - setgetfields.append( QString("void %2%3Get(%1 *New%3)\n") - .arg( fieldTypeStrC[info->fields[n]->type] ) - .arg( info->name ) - .arg( info->fields[n]->name )); - setgetfields.append( QString("{\n") ); - setgetfields.append( QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), sizeof(%3));\n") - .arg( info->name ) - .arg( info->fields[n]->name ) - .arg( fieldTypeStrC[info->fields[n]->type] ) ); - setgetfields.append( QString("}\n") ); - - } - else - { - - /* SET */ - setgetfields.append( QString("void %2%3Set( %1 *New%3 )\n") - .arg( fieldTypeStrC[info->fields[n]->type] ) - .arg( info->name ) - .arg( info->fields[n]->name ) ); - setgetfields.append( QString("{\n") ); - setgetfields.append( QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n") - .arg( info->name ) - .arg( info->fields[n]->name ) - .arg( info->fields[n]->numElements ) - .arg( fieldTypeStrC[info->fields[n]->type] ) ); - setgetfields.append( QString("}\n") ); - - /* GET */ - setgetfields.append( QString("void %2%3Get( %1 *New%3 )\n") - .arg( fieldTypeStrC[info->fields[n]->type] ) - .arg( info->name ) - .arg( info->fields[n]->name ) ); - setgetfields.append( QString("{\n") ); - setgetfields.append( QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n") - .arg( info->name ) - .arg( info->fields[n]->name ) - .arg( info->fields[n]->numElements ) - .arg( fieldTypeStrC[info->fields[n]->type] ) ); - setgetfields.append( QString("}\n") ); + /* GET */ + setgetfields.append(QString("void %2%3Get( %1 *New%3 )\n") + .arg(fieldTypeStrC[info->fields[n]->type]) + .arg(info->name) + .arg(info->fields[n]->name)); + setgetfields.append(QString("{\n")); + setgetfields.append(QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n") + .arg(info->name) + .arg(info->fields[n]->name) + .arg(info->fields[n]->numElements) + .arg(fieldTypeStrC[info->fields[n]->type])); + setgetfields.append(QString("}\n")); } } } outCode.replace(QString("$(SETGETFIELDS)"), setgetfields); // Replace the $(SETGETFIELDSEXTERN) tag - QString setgetfieldsextern; - for (int n = 0; n < info->fields.length(); ++n) - { - //if (!info->fields[n]->defaultValues.isEmpty() ) - { + QString setgetfieldsextern; + for (int n = 0; n < info->fields.length(); ++n) { + // if (!info->fields[n]->defaultValues.isEmpty() ) + { + /* SET */ + setgetfieldsextern.append(QString("extern void %2%3Set(%1 *New%3);\n") + .arg(fieldTypeStrC[info->fields[n]->type]) + .arg(info->name) + .arg(info->fields[n]->name)); - /* SET */ - setgetfieldsextern.append( QString("extern void %2%3Set(%1 *New%3);\n") - .arg( fieldTypeStrC[info->fields[n]->type] ) - .arg( info->name ) - .arg( info->fields[n]->name ) ); - - /* GET */ - setgetfieldsextern.append( QString("extern void %2%3Get(%1 *New%3);\n") - .arg( fieldTypeStrC[info->fields[n]->type] ) - .arg( info->name ) - .arg( info->fields[n]->name ) ); - } - } - outInclude.replace(QString("$(SETGETFIELDSEXTERN)"), setgetfieldsextern); + /* GET */ + setgetfieldsextern.append(QString("extern void %2%3Get(%1 *New%3);\n") + .arg(fieldTypeStrC[info->fields[n]->type]) + .arg(info->name) + .arg(info->fields[n]->name)); + } + } + outInclude.replace(QString("$(SETGETFIELDSEXTERN)"), setgetfieldsextern); // Write the flight code - bool res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/" + info->namelc + ".c", outCode ); + bool res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/" + info->namelc + ".c", outCode); if (!res) { cout << "Error: Could not write flight code files" << endl; return false; } - res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude ); + res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude); if (!res) { cout << "Error: Could not write flight include files" << endl; return false; diff --git a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h index e475c21ba..118d11e54 100644 --- a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h +++ b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h @@ -31,19 +31,16 @@ #include "../generator_common.h" -class UAVObjectGeneratorFlight -{ +class UAVObjectGeneratorFlight { public: - bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); + bool generate(UAVObjectParser *gen, QString templatepath, QString outputpath); QStringList fieldTypeStrC; QString flightCodeTemplate, flightIncludeTemplate, flightInitTemplate, flightInitIncludeTemplate, flightMakeTemplate; QDir flightCodePath; QDir flightOutputPath; private: - bool process_object(ObjectInfo* info); - + bool process_object(ObjectInfo *info); }; #endif - diff --git a/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp b/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp index 93fb980e2..2b1eb5d14 100644 --- a/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp +++ b/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp @@ -27,21 +27,21 @@ #include "uavobjectgeneratorgcs.h" using namespace std; -bool UAVObjectGeneratorGCS::generate(UAVObjectParser* parser,QString templatepath,QString outputpath) { - +bool UAVObjectGeneratorGCS::generate(UAVObjectParser *parser, QString templatepath, QString outputpath) +{ fieldTypeStrCPP << "qint8" << "qint16" << "qint32" << "quint8" << "quint16" << "quint32" << "float" << "quint8"; fieldTypeStrCPPClass << "INT8" << "INT16" << "INT32" - << "UINT8" << "UINT16" << "UINT32" << "FLOAT32" << "ENUM"; + << "UINT8" << "UINT16" << "UINT32" << "FLOAT32" << "ENUM"; - gcsCodePath = QDir( templatepath + QString(GCS_CODE_DIR)); - gcsOutputPath = QDir( outputpath + QString("gcs") ); + gcsCodePath = QDir(templatepath + QString(GCS_CODE_DIR)); + gcsOutputPath = QDir(outputpath + QString("gcs")); gcsOutputPath.mkpath(gcsOutputPath.absolutePath()); - gcsCodeTemplate = readFile( gcsCodePath.absoluteFilePath("uavobject.cpp.template") ); - gcsIncludeTemplate = readFile( gcsCodePath.absoluteFilePath("uavobject.h.template") ); - QString gcsInitTemplate = readFile( gcsCodePath.absoluteFilePath("uavobjectsinit.cpp.template") ); + gcsCodeTemplate = readFile(gcsCodePath.absoluteFilePath("uavobject.cpp.template")); + gcsIncludeTemplate = readFile(gcsCodePath.absoluteFilePath("uavobject.h.template")); + QString gcsInitTemplate = readFile(gcsCodePath.absoluteFilePath("uavobjectsinit.cpp.template")); if (gcsCodeTemplate.isEmpty() || gcsIncludeTemplate.isEmpty() || gcsInitTemplate.isEmpty()) { std::cerr << "Problem reading gcs code templates" << endl; @@ -52,7 +52,7 @@ bool UAVObjectGeneratorGCS::generate(UAVObjectParser* parser,QString templatepat QString gcsObjInit; for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { - ObjectInfo* info=parser->getObjectByIndex(objidx); + ObjectInfo *info = parser->getObjectByIndex(objidx); process_object(info); gcsObjInit.append(" objMngr->registerObject( new " + info->name + "() );\n"); @@ -60,9 +60,9 @@ bool UAVObjectGeneratorGCS::generate(UAVObjectParser* parser,QString templatepat } // Write the gcs object inialization files - gcsInitTemplate.replace( QString("$(OBJINC)"), objInc); - gcsInitTemplate.replace( QString("$(OBJINIT)"), gcsObjInit); - bool res = writeFileIfDiffrent( gcsOutputPath.absolutePath() + "/uavobjectsinit.cpp", gcsInitTemplate ); + gcsInitTemplate.replace(QString("$(OBJINC)"), objInc); + gcsInitTemplate.replace(QString("$(OBJINIT)"), gcsObjInit); + bool res = writeFileIfDiffrent(gcsOutputPath.absolutePath() + "/uavobjectsinit.cpp", gcsInitTemplate); if (!res) { cout << "Error: Could not write output files" << endl; return false; @@ -74,14 +74,15 @@ bool UAVObjectGeneratorGCS::generate(UAVObjectParser* parser,QString templatepat /** * Generate the GCS object files */ -bool UAVObjectGeneratorGCS::process_object(ObjectInfo* info) +bool UAVObjectGeneratorGCS::process_object(ObjectInfo *info) { - if (info == NULL) + if (info == NULL) { return false; + } // Prepare output strings QString outInclude = gcsIncludeTemplate; - QString outCode = gcsCodeTemplate; + QString outCode = gcsCodeTemplate; // Replace common tags replaceCommonTags(outInclude, info); @@ -90,19 +91,15 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo* info) // Replace the $(DATAFIELDS) tag QString type; QString fields; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { // Determine type type = fieldTypeStrCPP[info->fields[n]->type]; // Append field - if ( info->fields[n]->numElements > 1 ) - { - fields.append( QString(" %1 %2[%3];\n").arg(type).arg(info->fields[n]->name) - .arg(info->fields[n]->numElements) ); - } - else - { - fields.append( QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name) ); + if (info->fields[n]->numElements > 1) { + fields.append(QString(" %1 %2[%3];\n").arg(type).arg(info->fields[n]->name) + .arg(info->fields[n]->numElements)); + } else { + fields.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name)); } } outInclude.replace(QString("$(DATAFIELDS)"), fields); @@ -115,80 +112,80 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo* info) QString propertyNotifications; QString propertyNotificationsImpl; - //to avoid name conflicts + // to avoid name conflicts QStringList reservedProperties; reservedProperties << "Description" << "Metadata"; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { FieldInfo *field = info->fields[n]; - if (reservedProperties.contains(field->name)) + if (reservedProperties.contains(field->name)) { continue; + } // Determine type type = fieldTypeStrCPP[field->type]; // Append field - if ( field->numElements > 1 ) { + if (field->numElements > 1) { propertyGetters += - QString(" Q_INVOKABLE %1 get%2(quint32 index) const;\n") - .arg(type).arg(field->name); - propertiesImpl += - QString("%1 %2::get%3(quint32 index) const\n" - "{\n" - " QMutexLocker locker(mutex);\n" - " return data.%3[index];\n" - "}\n") - .arg(type).arg(info->name).arg(field->name); + QString(" Q_INVOKABLE %1 get%2(quint32 index) const;\n") + .arg(type).arg(field->name); + propertiesImpl += + QString("%1 %2::get%3(quint32 index) const\n" + "{\n" + " QMutexLocker locker(mutex);\n" + " return data.%3[index];\n" + "}\n") + .arg(type).arg(info->name).arg(field->name); propertySetters += - QString(" void set%1(quint32 index, %2 value);\n") - .arg(field->name).arg(type); - propertiesImpl += - QString("void %1::set%2(quint32 index, %3 value)\n" - "{\n" - " mutex->lock();\n" - " bool changed = data.%2[index] != value;\n" - " data.%2[index] = value;\n" - " mutex->unlock();\n" - " if (changed) emit %2Changed(index,value);\n" - "}\n\n") - .arg(info->name).arg(field->name).arg(type); + QString(" void set%1(quint32 index, %2 value);\n") + .arg(field->name).arg(type); + propertiesImpl += + QString("void %1::set%2(quint32 index, %3 value)\n" + "{\n" + " mutex->lock();\n" + " bool changed = data.%2[index] != value;\n" + " data.%2[index] = value;\n" + " mutex->unlock();\n" + " if (changed) emit %2Changed(index,value);\n" + "}\n\n") + .arg(info->name).arg(field->name).arg(type); propertyNotifications += - QString(" void %1Changed(quint32 index, %2 value);\n") - .arg(field->name).arg(type); + QString(" void %1Changed(quint32 index, %2 value);\n") + .arg(field->name).arg(type); } else { properties += QString(" Q_PROPERTY(%1 %2 READ get%2 WRITE set%2 NOTIFY %2Changed);\n") - .arg(type).arg(field->name); + .arg(type).arg(field->name); propertyGetters += - QString(" Q_INVOKABLE %1 get%2() const;\n") - .arg(type).arg(field->name); - propertiesImpl += - QString("%1 %2::get%3() const\n" - "{\n" - " QMutexLocker locker(mutex);\n" - " return data.%3;\n" - "}\n") - .arg(type).arg(info->name).arg(field->name); + QString(" Q_INVOKABLE %1 get%2() const;\n") + .arg(type).arg(field->name); + propertiesImpl += + QString("%1 %2::get%3() const\n" + "{\n" + " QMutexLocker locker(mutex);\n" + " return data.%3;\n" + "}\n") + .arg(type).arg(info->name).arg(field->name); propertySetters += - QString(" void set%1(%2 value);\n") - .arg(field->name).arg(type); - propertiesImpl += - QString("void %1::set%2(%3 value)\n" - "{\n" - " mutex->lock();\n" - " bool changed = data.%2 != value;\n" - " data.%2 = value;\n" - " mutex->unlock();\n" - " if (changed) emit %2Changed(value);\n" - "}\n\n") - .arg(info->name).arg(field->name).arg(type); - propertyNotifications += - QString(" void %1Changed(%2 value);\n") - .arg(field->name).arg(type); + QString(" void set%1(%2 value);\n") + .arg(field->name).arg(type); + propertiesImpl += + QString("void %1::set%2(%3 value)\n" + "{\n" + " mutex->lock();\n" + " bool changed = data.%2 != value;\n" + " data.%2 = value;\n" + " mutex->unlock();\n" + " if (changed) emit %2Changed(value);\n" + "}\n\n") + .arg(info->name).arg(field->name).arg(type); + propertyNotifications += + QString(" void %1Changed(%2 value);\n") + .arg(field->name).arg(type); propertyNotificationsImpl += - QString(" //if (data.%1 != oldData.%1)\n" - " emit %1Changed(data.%1);\n") - .arg(field->name); + QString(" //if (data.%1 != oldData.%1)\n" + " emit %1Changed(data.%1);\n") + .arg(field->name); } } @@ -202,43 +199,42 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo* info) // Replace the $(FIELDSINIT) tag QString finit; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { // Setup element names - QString varElemName = info->fields[n]->name + "ElemNames"; - finit.append( QString(" QStringList %1;\n").arg(varElemName) ); + QString varElemName = info->fields[n]->name + "ElemNames"; + finit.append(QString(" QStringList %1;\n").arg(varElemName)); QStringList elemNames = info->fields[n]->elementNames; - for (int m = 0; m < elemNames.length(); ++m) - finit.append( QString(" %1.append(\"%2\");\n") - .arg(varElemName) - .arg(elemNames[m]) ); + for (int m = 0; m < elemNames.length(); ++m) { + finit.append(QString(" %1.append(\"%2\");\n") + .arg(varElemName) + .arg(elemNames[m])); + } // Only for enum types if (info->fields[n]->type == FIELDTYPE_ENUM) { QString varOptionName = info->fields[n]->name + "EnumOptions"; - finit.append( QString(" QStringList %1;\n").arg(varOptionName) ); - QStringList options = info->fields[n]->options; - for (int m = 0; m < options.length(); ++m) - { - finit.append( QString(" %1.append(\"%2\");\n") - .arg(varOptionName) - .arg(options[m]) ); + finit.append(QString(" QStringList %1;\n").arg(varOptionName)); + QStringList options = info->fields[n]->options; + for (int m = 0; m < options.length(); ++m) { + finit.append(QString(" %1.append(\"%2\");\n") + .arg(varOptionName) + .arg(options[m])); } - finit.append( QString(" fields.append( new UAVObjectField(QString(\"%1\"), QString(\"%2\"), UAVObjectField::ENUM, %3, %4, QString(\"%5\")));\n") - .arg(info->fields[n]->name) - .arg(info->fields[n]->units) - .arg(varElemName) - .arg(varOptionName) - .arg(info->fields[n]->limitValues)); + finit.append(QString(" fields.append( new UAVObjectField(QString(\"%1\"), QString(\"%2\"), UAVObjectField::ENUM, %3, %4, QString(\"%5\")));\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->units) + .arg(varElemName) + .arg(varOptionName) + .arg(info->fields[n]->limitValues)); } // For all other types else { - finit.append( QString(" fields.append( new UAVObjectField(QString(\"%1\"), QString(\"%2\"), UAVObjectField::%3, %4, QStringList(), QString(\"%5\")));\n") - .arg(info->fields[n]->name) - .arg(info->fields[n]->units) - .arg(fieldTypeStrCPPClass[info->fields[n]->type]) - .arg(varElemName) - .arg(info->fields[n]->limitValues)); + finit.append(QString(" fields.append( new UAVObjectField(QString(\"%1\"), QString(\"%2\"), UAVObjectField::%3, %4, QStringList(), QString(\"%5\")));\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->units) + .arg(fieldTypeStrCPPClass[info->fields[n]->type]) + .arg(varElemName) + .arg(info->fields[n]->limitValues)); } } outCode.replace(QString("$(FIELDSINIT)"), finit); @@ -246,25 +242,22 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo* info) // Replace the $(DATAFIELDINFO) tag QString name; QString enums; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { enums.append(QString(" // Field %1 information\n").arg(info->fields[n]->name)); // Only for enum types - if (info->fields[n]->type == FIELDTYPE_ENUM) - { + if (info->fields[n]->type == FIELDTYPE_ENUM) { enums.append(QString(" /* Enumeration options for field %1 */\n").arg(info->fields[n]->name)); enums.append(" typedef enum { "); // Go through each option QStringList options = info->fields[n]->options; for (int m = 0; m < options.length(); ++m) { - QString s = (m != (options.length()-1)) ? "%1_%2=%3, " : "%1_%2=%3"; - enums.append( s.arg( info->fields[n]->name.toUpper() ) - .arg( options[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), "") ) - .arg(m) ); - + QString s = (m != (options.length() - 1)) ? "%1_%2=%3, " : "%1_%2=%3"; + enums.append(s.arg(info->fields[n]->name.toUpper()) + .arg(options[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), "")) + .arg(m)); } - enums.append( QString(" } %1Options;\n") - .arg( info->fields[n]->name ) ); + enums.append(QString(" } %1Options;\n") + .arg(info->fields[n]->name)); } // Generate element names (only if field has more than one element) if (info->fields[n]->numElements > 1 && !info->fields[n]->defaultElementNames) { @@ -273,75 +266,61 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo* info) // Go through the element names QStringList elemNames = info->fields[n]->elementNames; for (int m = 0; m < elemNames.length(); ++m) { - QString s = (m != (elemNames.length()-1)) ? "%1_%2=%3, " : "%1_%2=%3"; - enums.append( s.arg( info->fields[n]->name.toUpper() ) - .arg( elemNames[m].toUpper() ) - .arg(m) ); - + QString s = (m != (elemNames.length() - 1)) ? "%1_%2=%3, " : "%1_%2=%3"; + enums.append(s.arg(info->fields[n]->name.toUpper()) + .arg(elemNames[m].toUpper()) + .arg(m)); } - enums.append( QString(" } %1Elem;\n") - .arg( info->fields[n]->name ) ); + enums.append(QString(" } %1Elem;\n") + .arg(info->fields[n]->name)); } // Generate array information if (info->fields[n]->numElements > 1) { enums.append(QString(" /* Number of elements for field %1 */\n").arg(info->fields[n]->name)); - enums.append( QString(" static const quint32 %1_NUMELEM = %2;\n") - .arg( info->fields[n]->name.toUpper() ) - .arg( info->fields[n]->numElements ) ); + enums.append(QString(" static const quint32 %1_NUMELEM = %2;\n") + .arg(info->fields[n]->name.toUpper()) + .arg(info->fields[n]->numElements)); } } outInclude.replace(QString("$(DATAFIELDINFO)"), enums); // Replace the $(INITFIELDS) tag QString initfields; - for (int n = 0; n < info->fields.length(); ++n) - { - if (!info->fields[n]->defaultValues.isEmpty() ) - { + for (int n = 0; n < info->fields.length(); ++n) { + if (!info->fields[n]->defaultValues.isEmpty()) { // For non-array fields - if ( info->fields[n]->numElements == 1) - { - if ( info->fields[n]->type == FIELDTYPE_ENUM ) - { - initfields.append( QString(" data.%1 = %2;\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->options.indexOf( info->fields[n]->defaultValues[0] ) ) ); + if (info->fields[n]->numElements == 1) { + if (info->fields[n]->type == FIELDTYPE_ENUM) { + initfields.append(QString(" data.%1 = %2;\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->options.indexOf(info->fields[n]->defaultValues[0]))); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + initfields.append(QString(" data.%1 = %2;\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0].toFloat())); + } else { + initfields.append(QString(" data.%1 = %2;\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0].toInt())); } - else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) - { - initfields.append( QString(" data.%1 = %2;\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->defaultValues[0].toFloat() ) ); - } - else - { - initfields.append( QString(" data.%1 = %2;\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->defaultValues[0].toInt() ) ); - } - } - else - { + } else { // Initialize all fields in the array - for (int idx = 0; idx < info->fields[n]->numElements; ++idx) - { - if ( info->fields[n]->type == FIELDTYPE_ENUM ) { - initfields.append( QString(" data.%1[%2] = %3;\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->options.indexOf( info->fields[n]->defaultValues[idx] ) ) ); - } - else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) { - initfields.append( QString(" data.%1[%2] = %3;\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->defaultValues[idx].toFloat() ) ); - } - else { - initfields.append( QString(" data.%1[%2] = %3;\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->defaultValues[idx].toInt() ) ); + for (int idx = 0; idx < info->fields[n]->numElements; ++idx) { + if (info->fields[n]->type == FIELDTYPE_ENUM) { + initfields.append(QString(" data.%1[%2] = %3;\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->options.indexOf(info->fields[n]->defaultValues[idx]))); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + initfields.append(QString(" data.%1[%2] = %3;\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx].toFloat())); + } else { + initfields.append(QString(" data.%1[%2] = %3;\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx].toInt())); } } } @@ -351,12 +330,12 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo* info) outCode.replace(QString("$(INITFIELDS)"), initfields); // Write the GCS code - bool res = writeFileIfDiffrent( gcsOutputPath.absolutePath() + "/" + info->namelc + ".cpp", outCode ); + bool res = writeFileIfDiffrent(gcsOutputPath.absolutePath() + "/" + info->namelc + ".cpp", outCode); if (!res) { cout << "Error: Could not write gcs output files" << endl; return false; } - res = writeFileIfDiffrent( gcsOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude ); + res = writeFileIfDiffrent(gcsOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude); if (!res) { cout << "Error: Could not write gcs output files" << endl; return false; diff --git a/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.h b/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.h index a0106d6bc..a41e47011 100644 --- a/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.h +++ b/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.h @@ -31,16 +31,15 @@ #include "../generator_common.h" -class UAVObjectGeneratorGCS -{ +class UAVObjectGeneratorGCS { public: - bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); + bool generate(UAVObjectParser *gen, QString templatepath, QString outputpath); private: - bool process_object(ObjectInfo* info); + bool process_object(ObjectInfo *info); - QString gcsCodeTemplate,gcsIncludeTemplate; - QStringList fieldTypeStrCPP,fieldTypeStrCPPClass; + QString gcsCodeTemplate, gcsIncludeTemplate; + QStringList fieldTypeStrCPP, fieldTypeStrCPPClass; QDir gcsCodePath; QDir gcsOutputPath; }; diff --git a/ground/uavobjgenerator/generators/generator_common.cpp b/ground/uavobjgenerator/generators/generator_common.cpp index 12fae596d..3a96ca982 100644 --- a/ground/uavobjgenerator/generators/generator_common.cpp +++ b/ground/uavobjgenerator/generators/generator_common.cpp @@ -25,7 +25,7 @@ */ #include "generator_common.h" -void replaceCommonTags(QString& out) +void replaceCommonTags(QString & out) { // Replace $(GENERATEDWARNING) tag out.replace(QString("$(GENERATEDWARNING)"), "This is a autogenerated file!! Do not modify and expect a result."); @@ -33,10 +33,10 @@ void replaceCommonTags(QString& out) /** * Replace all the common tags from the template file with actual object information. */ -void replaceCommonTags(QString& out, ObjectInfo* info) +void replaceCommonTags(QString & out, ObjectInfo *info) { + QStringList updateModeStr, accessModeStr; - QStringList updateModeStr,accessModeStr; updateModeStr << "UPDATEMODE_MANUAL" << "UPDATEMODE_PERIODIC" << "UPDATEMODE_ONCHANGE" << "UPDATEMODE_THROTTLED"; @@ -45,7 +45,7 @@ void replaceCommonTags(QString& out, ObjectInfo* info) QString value; - // replace the tags which don't need info + // replace the tags which don't need info replaceCommonTags(out); // Replace $(XMLFILE) tag @@ -65,13 +65,13 @@ void replaceCommonTags(QString& out, ObjectInfo* info) // Replace $(UOBJID) tag out.replace(QString("$(UOBJID)"), QString().setNum((qint32)info->id)); // Replace $(OBJIDHEX) tag - out.replace(QString("$(OBJIDHEX)"),QString("0x")+ QString().setNum(info->id,16).toUpper()); + out.replace(QString("$(OBJIDHEX)"), QString("0x") + QString().setNum(info->id, 16).toUpper()); // Replace $(ISSINGLEINST) tag - out.replace(QString("$(ISSINGLEINST)"), boolTo01String( info->isSingleInst )); - out.replace(QString("$(ISSINGLEINSTTF)"), boolToTRUEFALSEString( info->isSingleInst )); + out.replace(QString("$(ISSINGLEINST)"), boolTo01String(info->isSingleInst)); + out.replace(QString("$(ISSINGLEINSTTF)"), boolToTRUEFALSEString(info->isSingleInst)); // Replace $(ISSETTINGS) tag - out.replace(QString("$(ISSETTINGS)"), boolTo01String( info->isSettings )); - out.replace(QString("$(ISSETTINGSTF)"), boolToTRUEFALSEString( info->isSettings )); + out.replace(QString("$(ISSETTINGS)"), boolTo01String(info->isSettings)); + out.replace(QString("$(ISSETTINGSTF)"), boolToTRUEFALSEString(info->isSettings)); // Replace $(GCSACCESS) tag value = accessModeStr[info->gcsAccess]; out.replace(QString("$(GCSACCESS)"), value); @@ -79,16 +79,16 @@ void replaceCommonTags(QString& out, ObjectInfo* info) value = accessModeStr[info->flightAccess]; out.replace(QString("$(FLIGHTACCESS)"), value); // Replace $(FLIGHTTELEM_ACKED) tag - out.replace(QString("$(FLIGHTTELEM_ACKED)"), boolTo01String( info->flightTelemetryAcked )); - out.replace(QString("$(FLIGHTTELEM_ACKEDTF)"), boolToTRUEFALSEString( info->flightTelemetryAcked )); + out.replace(QString("$(FLIGHTTELEM_ACKED)"), boolTo01String(info->flightTelemetryAcked)); + out.replace(QString("$(FLIGHTTELEM_ACKEDTF)"), boolToTRUEFALSEString(info->flightTelemetryAcked)); // Replace $(FLIGHTTELEM_UPDATEMODE) tag - value =updateModeStr[info->flightTelemetryUpdateMode]; + value = updateModeStr[info->flightTelemetryUpdateMode]; out.replace(QString("$(FLIGHTTELEM_UPDATEMODE)"), value); // Replace $(FLIGHTTELEM_UPDATEPERIOD) tag out.replace(QString("$(FLIGHTTELEM_UPDATEPERIOD)"), QString().setNum(info->flightTelemetryUpdatePeriod)); // Replace $(GCSTELEM_ACKED) tag - out.replace(QString("$(GCSTELEM_ACKED)"), boolTo01String( info->gcsTelemetryAcked )); - out.replace(QString("$(GCSTELEM_ACKEDTF)"), boolToTRUEFALSEString( info->gcsTelemetryAcked )); + out.replace(QString("$(GCSTELEM_ACKED)"), boolTo01String(info->gcsTelemetryAcked)); + out.replace(QString("$(GCSTELEM_ACKEDTF)"), boolToTRUEFALSEString(info->gcsTelemetryAcked)); // Replace $(GCSTELEM_UPDATEMODE) tag value = updateModeStr[info->gcsTelemetryUpdateMode]; out.replace(QString("$(GCSTELEM_UPDATEMODE)"), value); @@ -99,8 +99,6 @@ void replaceCommonTags(QString& out, ObjectInfo* info) out.replace(QString("$(LOGGING_UPDATEMODE)"), value); // Replace $(LOGGING_UPDATEPERIOD) tag out.replace(QString("$(LOGGING_UPDATEPERIOD)"), QString().setNum(info->loggingUpdatePeriod)); - - } /** @@ -108,8 +106,9 @@ void replaceCommonTags(QString& out, ObjectInfo* info) */ QString boolTo01String(bool value) { - if ( value ) + if (value) { return QString("1"); + } return QString("0"); } @@ -119,9 +118,9 @@ QString boolTo01String(bool value) */ QString boolToTRUEFALSEString(bool value) { - if ( value ) + if (value) { return QString("TRUE"); + } return QString("FALSE"); } - diff --git a/ground/uavobjgenerator/generators/generator_common.h b/ground/uavobjgenerator/generators/generator_common.h index 95dab21e5..ab648b5c4 100644 --- a/ground/uavobjgenerator/generators/generator_common.h +++ b/ground/uavobjgenerator/generators/generator_common.h @@ -33,8 +33,8 @@ // These special chars (regexp) will be removed from C/java identifiers #define ENUM_SPECIAL_CHARS "[\\.\\-\\s\\+/\\(\\)]" -void replaceCommonTags(QString& out, ObjectInfo* info); -void replaceCommonTags(QString& out); +void replaceCommonTags(QString & out, ObjectInfo *info); +void replaceCommonTags(QString & out); QString boolTo01String(bool value); QString boolToTRUEFALSEString(bool value); diff --git a/ground/uavobjgenerator/generators/generator_io.cpp b/ground/uavobjgenerator/generators/generator_io.cpp index ac81f20b3..67afdd05a 100644 --- a/ground/uavobjgenerator/generators/generator_io.cpp +++ b/ground/uavobjgenerator/generators/generator_io.cpp @@ -31,12 +31,14 @@ using namespace std; /** * Read a file and return its contents as a string */ -QString readFile(QString name,bool do_warn) +QString readFile(QString name, bool do_warn) { QFile file(name); - if (!file.open(QFile::ReadOnly)) { - if (do_warn) + + if (!file.open(QFile::ReadOnly)) { + if (do_warn) { std::cout << "Warning: Could not open " << name.toStdString() << endl; + } return QString(); } @@ -51,17 +53,19 @@ QString readFile(QString name,bool do_warn) */ QString readFile(QString name) { - return readFile(name,true); + return readFile(name, true); } /** * Write contents of string to file */ -bool writeFile(QString name, QString& str) +bool writeFile(QString name, QString & str) { QFile file(name); - if (!file.open(QFile::WriteOnly)) + + if (!file.open(QFile::WriteOnly)) { return false; + } QTextStream fileStr(&file); fileStr << str; file.close(); @@ -71,9 +75,10 @@ bool writeFile(QString name, QString& str) /** * Write contents of string to file if the content changes */ -bool writeFileIfDiffrent(QString name, QString& str) +bool writeFileIfDiffrent(QString name, QString & str) { - if (str==readFile(name,false)) + if (str == readFile(name, false)) { return true; - return writeFile(name,str); + } + return writeFile(name, str); } diff --git a/ground/uavobjgenerator/generators/generator_io.h b/ground/uavobjgenerator/generators/generator_io.h index 47c554c7e..8d4500bf4 100644 --- a/ground/uavobjgenerator/generators/generator_io.h +++ b/ground/uavobjgenerator/generators/generator_io.h @@ -36,7 +36,7 @@ QString readFile(QString name); QString readFile(QString name); -bool writeFile(QString name, QString& str); -bool writeFileIfDiffrent(QString name, QString& str); +bool writeFile(QString name, QString & str); +bool writeFileIfDiffrent(QString name, QString & str); #endif diff --git a/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.cpp b/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.cpp index 3698ebba1..a4e97457d 100644 --- a/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.cpp +++ b/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.cpp @@ -28,19 +28,20 @@ #include "uavobjectgeneratorjava.h" using namespace std; -bool UAVObjectGeneratorJava::generate(UAVObjectParser* parser,QString templatepath,QString outputpath) { +bool UAVObjectGeneratorJava::generate(UAVObjectParser *parser, QString templatepath, QString outputpath) +{ fieldTypeStrCPP << "Byte" << "Short" << "Int" << "Short" << "Int" << "Long" << "Float" << "Byte"; fieldTypeStrCPPClass << "INT8" << "INT16" << "INT32" - << "UINT8" << "UINT16" << "UINT32" << "FLOAT32" << "ENUM"; + << "UINT8" << "UINT16" << "UINT32" << "FLOAT32" << "ENUM"; - javaCodePath = QDir( templatepath + QString(JAVA_TEMPLATE_DIR)); - javaOutputPath = QDir( outputpath + QString("java") ); + javaCodePath = QDir(templatepath + QString(JAVA_TEMPLATE_DIR)); + javaOutputPath = QDir(outputpath + QString("java")); javaOutputPath.mkpath(javaOutputPath.absolutePath()); - javaCodeTemplate = readFile( javaCodePath.absoluteFilePath("uavobject.java.template") ); - QString javaInitTemplate = readFile( javaCodePath.absoluteFilePath("uavobjectsinit.java.template") ); + javaCodeTemplate = readFile(javaCodePath.absoluteFilePath("uavobject.java.template")); + QString javaInitTemplate = readFile(javaCodePath.absoluteFilePath("uavobjectsinit.java.template")); if (javaCodeTemplate.isEmpty() || javaInitTemplate.isEmpty()) { std::cerr << "Problem reading java code templates" << endl; @@ -51,7 +52,7 @@ bool UAVObjectGeneratorJava::generate(UAVObjectParser* parser,QString templatepa QString javaObjInit; for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { - ObjectInfo* info=parser->getObjectByIndex(objidx); + ObjectInfo *info = parser->getObjectByIndex(objidx); process_object(info); javaObjInit.append("\t\t\tobjMngr.registerObject( new " + info->name + "() );\n"); @@ -59,9 +60,9 @@ bool UAVObjectGeneratorJava::generate(UAVObjectParser* parser,QString templatepa } // Write the gcs object inialization files - javaInitTemplate.replace( QString("$(OBJINC)"), objInc); - javaInitTemplate.replace( QString("$(OBJINIT)"), javaObjInit); - bool res = writeFileIfDiffrent( javaOutputPath.absolutePath() + "/UAVObjectsInitialize.java", javaInitTemplate ); + javaInitTemplate.replace(QString("$(OBJINC)"), objInc); + javaInitTemplate.replace(QString("$(OBJINIT)"), javaObjInit); + bool res = writeFileIfDiffrent(javaOutputPath.absolutePath() + "/UAVObjectsInitialize.java", javaInitTemplate); if (!res) { cout << "Error: Could not write output files" << endl; return false; @@ -74,14 +75,15 @@ bool UAVObjectGeneratorJava::generate(UAVObjectParser* parser,QString templatepa /** * Generate the java object files */ -bool UAVObjectGeneratorJava::process_object(ObjectInfo* info) +bool UAVObjectGeneratorJava::process_object(ObjectInfo *info) { - if (info == NULL) + if (info == NULL) { return false; + } // Prepare output strings QString outInclude = javaIncludeTemplate; - QString outCode = javaCodeTemplate; + QString outCode = javaCodeTemplate; // Replace common tags replaceCommonTags(outInclude, info); @@ -90,62 +92,57 @@ bool UAVObjectGeneratorJava::process_object(ObjectInfo* info) // Replace the $(DATAFIELDS) tag QString type; QString fields; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { // Determine type type = fieldTypeStrCPP[info->fields[n]->type]; // Append field - if ( info->fields[n]->numElements > 1 ) - { - fields.append( QString(" %1 %2[%3];\n").arg(type).arg(info->fields[n]->name) - .arg(info->fields[n]->numElements) ); - } - else - { - fields.append( QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name) ); + if (info->fields[n]->numElements > 1) { + fields.append(QString(" %1 %2[%3];\n").arg(type).arg(info->fields[n]->name) + .arg(info->fields[n]->numElements)); + } else { + fields.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name)); } } outInclude.replace(QString("$(DATAFIELDS)"), fields); // Replace the $(FIELDSINIT) tag QString finit; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { finit.append("\n"); // Setup element names - QString varElemName = info->fields[n]->name + "ElemNames"; - finit.append( QString("\t\tList %1 = new ArrayList();\n").arg(varElemName) ); + QString varElemName = info->fields[n]->name + "ElemNames"; + finit.append(QString("\t\tList %1 = new ArrayList();\n").arg(varElemName)); QStringList elemNames = info->fields[n]->elementNames; - for (int m = 0; m < elemNames.length(); ++m) - finit.append( QString("\t\t%1.add(\"%2\");\n") - .arg(varElemName) - .arg(elemNames[m]) ); + for (int m = 0; m < elemNames.length(); ++m) { + finit.append(QString("\t\t%1.add(\"%2\");\n") + .arg(varElemName) + .arg(elemNames[m])); + } // Only for enum types if (info->fields[n]->type == FIELDTYPE_ENUM) { QString varOptionName = info->fields[n]->name + "EnumOptions"; - finit.append( QString("\t\tList %1 = new ArrayList();\n").arg(varOptionName) ); - QStringList options = info->fields[n]->options; - for (int m = 0; m < options.length(); ++m) - { - finit.append( QString("\t\t%1.add(\"%2\");\n") - .arg(varOptionName) - .arg(options[m]) ); + finit.append(QString("\t\tList %1 = new ArrayList();\n").arg(varOptionName)); + QStringList options = info->fields[n]->options; + for (int m = 0; m < options.length(); ++m) { + finit.append(QString("\t\t%1.add(\"%2\");\n") + .arg(varOptionName) + .arg(options[m])); } - finit.append( QString("\t\tfields.add( new UAVObjectField(\"%1\", \"%2\", UAVObjectField.FieldType.ENUM, %3, %4) );\n") - .arg(info->fields[n]->name) - .arg(info->fields[n]->units) - .arg(varElemName) - .arg(varOptionName) ); + finit.append(QString("\t\tfields.add( new UAVObjectField(\"%1\", \"%2\", UAVObjectField.FieldType.ENUM, %3, %4) );\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->units) + .arg(varElemName) + .arg(varOptionName)); } // For all other types else { - finit.append( QString("\t\tfields.add( new UAVObjectField(\"%1\", \"%2\", UAVObjectField.FieldType.%3, %4, null) );\n") - .arg(info->fields[n]->name) - .arg(info->fields[n]->units) - .arg(fieldTypeStrCPPClass[info->fields[n]->type]) - .arg(varElemName) ); + finit.append(QString("\t\tfields.add( new UAVObjectField(\"%1\", \"%2\", UAVObjectField.FieldType.%3, %4, null) );\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->units) + .arg(fieldTypeStrCPPClass[info->fields[n]->type]) + .arg(varElemName)); } } outCode.replace(QString("$(FIELDSINIT)"), finit); @@ -153,25 +150,22 @@ bool UAVObjectGeneratorJava::process_object(ObjectInfo* info) // Replace the $(DATAFIELDINFO) tag QString name; QString enums; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { enums.append(QString(" // Field %1 information\n").arg(info->fields[n]->name)); // Only for enum types - if (info->fields[n]->type == FIELDTYPE_ENUM) - { + if (info->fields[n]->type == FIELDTYPE_ENUM) { enums.append(QString(" /* Enumeration options for field %1 */\n").arg(info->fields[n]->name)); enums.append(" typedef enum { "); // Go through each option QStringList options = info->fields[n]->options; for (int m = 0; m < options.length(); ++m) { - QString s = (m != (options.length()-1)) ? "%1_%2=%3, " : "%1_%2=%3"; - enums.append( s.arg( info->fields[n]->name.toUpper() ) - .arg( options[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), "") ) - .arg(m) ); - + QString s = (m != (options.length() - 1)) ? "%1_%2=%3, " : "%1_%2=%3"; + enums.append(s.arg(info->fields[n]->name.toUpper()) + .arg(options[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), "")) + .arg(m)); } - enums.append( QString(" } %1Options;\n") - .arg( info->fields[n]->name ) ); + enums.append(QString(" } %1Options;\n") + .arg(info->fields[n]->name)); } // Generate element names (only if field has more than one element) if (info->fields[n]->numElements > 1 && !info->fields[n]->defaultElementNames) { @@ -180,75 +174,61 @@ bool UAVObjectGeneratorJava::process_object(ObjectInfo* info) // Go through the element names QStringList elemNames = info->fields[n]->elementNames; for (int m = 0; m < elemNames.length(); ++m) { - QString s = (m != (elemNames.length()-1)) ? "%1_%2=%3, " : "%1_%2=%3"; - enums.append( s.arg( info->fields[n]->name.toUpper() ) - .arg( elemNames[m].toUpper() ) - .arg(m) ); - + QString s = (m != (elemNames.length() - 1)) ? "%1_%2=%3, " : "%1_%2=%3"; + enums.append(s.arg(info->fields[n]->name.toUpper()) + .arg(elemNames[m].toUpper()) + .arg(m)); } - enums.append( QString(" } %1Elem;\n") - .arg( info->fields[n]->name ) ); + enums.append(QString(" } %1Elem;\n") + .arg(info->fields[n]->name)); } // Generate array information if (info->fields[n]->numElements > 1) { enums.append(QString(" /* Number of elements for field %1 */\n").arg(info->fields[n]->name)); - enums.append( QString(" static const quint32 %1_NUMELEM = %2;\n") - .arg( info->fields[n]->name.toUpper() ) - .arg( info->fields[n]->numElements ) ); + enums.append(QString(" static const quint32 %1_NUMELEM = %2;\n") + .arg(info->fields[n]->name.toUpper()) + .arg(info->fields[n]->numElements)); } } outInclude.replace(QString("$(DATAFIELDINFO)"), enums); // Replace the $(INITFIELDS) tag QString initfields; - for (int n = 0; n < info->fields.length(); ++n) - { - if (!info->fields[n]->defaultValues.isEmpty() ) - { + for (int n = 0; n < info->fields.length(); ++n) { + if (!info->fields[n]->defaultValues.isEmpty()) { // For non-array fields - if ( info->fields[n]->numElements == 1) - { - if ( info->fields[n]->type == FIELDTYPE_ENUM ) - { - initfields.append( QString("\t\tgetField(\"%1\").setValue(\"%2\");\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->defaultValues[0] ) ); + if (info->fields[n]->numElements == 1) { + if (info->fields[n]->type == FIELDTYPE_ENUM) { + initfields.append(QString("\t\tgetField(\"%1\").setValue(\"%2\");\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0])); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + initfields.append(QString("\t\tgetField(\"%1\").setValue(%2);\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0].toFloat())); + } else { + initfields.append(QString("\t\tgetField(\"%1\").setValue(%2);\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0].toInt())); } - else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) - { - initfields.append( QString("\t\tgetField(\"%1\").setValue(%2);\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->defaultValues[0].toFloat() ) ); - } - else - { - initfields.append( QString("\t\tgetField(\"%1\").setValue(%2);\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->defaultValues[0].toInt() ) ); - } - } - else - { + } else { // Initialize all fields in the array - for (int idx = 0; idx < info->fields[n]->numElements; ++idx) - { - if ( info->fields[n]->type == FIELDTYPE_ENUM ) { - initfields.append( QString("\t\tgetField(\"%1\").setValue(\"%3\",%2);\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->defaultValues[idx] ) ); - } - else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) { - initfields.append( QString("\t\tgetField(\"%1\").setValue(%3,%2);\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->defaultValues[idx].toFloat() ) ); - } - else { - initfields.append( QString("\t\tgetField(\"%1\").setValue(%3,%2);\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->defaultValues[idx].toInt() ) ); + for (int idx = 0; idx < info->fields[n]->numElements; ++idx) { + if (info->fields[n]->type == FIELDTYPE_ENUM) { + initfields.append(QString("\t\tgetField(\"%1\").setValue(\"%3\",%2);\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx])); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + initfields.append(QString("\t\tgetField(\"%1\").setValue(%3,%2);\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx].toFloat())); + } else { + initfields.append(QString("\t\tgetField(\"%1\").setValue(%3,%2);\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx].toInt())); } } } @@ -258,7 +238,7 @@ bool UAVObjectGeneratorJava::process_object(ObjectInfo* info) outCode.replace(QString("$(INITFIELDS)"), initfields); // Write the java code - bool res = writeFileIfDiffrent( javaOutputPath.absolutePath() + "/" + info->name + ".java", outCode ); + bool res = writeFileIfDiffrent(javaOutputPath.absolutePath() + "/" + info->name + ".java", outCode); if (!res) { cout << "Error: Could not write gcs output files" << endl; return false; @@ -266,4 +246,3 @@ bool UAVObjectGeneratorJava::process_object(ObjectInfo* info) return true; } - diff --git a/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.h b/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.h index 8d07484f6..8488d57ed 100644 --- a/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.h +++ b/ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.h @@ -28,22 +28,21 @@ #define UAVOBJECTGENERATORJAVA_H #define JAVA_TEMPLATE_DIR "ground/openpilotgcs/src/libs/juavobjects/templates/" -#define JAVA_CODE_DIR "java/src/org/openpilot/uavtalk" +#define JAVA_CODE_DIR "java/src/org/openpilot/uavtalk" #include "../generator_common.h" -class UAVObjectGeneratorJava -{ +class UAVObjectGeneratorJava { public: - bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); + bool generate(UAVObjectParser *gen, QString templatepath, QString outputpath); private: - bool process_object(ObjectInfo* info); + bool process_object(ObjectInfo *info); QString javaCodeTemplate, javaIncludeTemplate; - QStringList fieldTypeStrCPP,fieldTypeStrCPPClass; + QStringList fieldTypeStrCPP, fieldTypeStrCPPClass; QDir javaCodePath; QDir javaOutputPath; - }; +}; -#endif +#endif // ifndef UAVOBJECTGENERATORJAVA_H diff --git a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp index d0894b892..b6611164b 100644 --- a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp +++ b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp @@ -28,38 +28,38 @@ using namespace std; -bool UAVObjectGeneratorMatlab::generate(UAVObjectParser* parser,QString templatepath,QString outputpath) { - +bool UAVObjectGeneratorMatlab::generate(UAVObjectParser *parser, QString templatepath, QString outputpath) +{ fieldTypeStrMatlab << "int8" << "int16" << "int32" - << "uint8" << "uint16" << "uint32" << "single" << "uint8"; + << "uint8" << "uint16" << "uint32" << "single" << "uint8"; fieldSizeStrMatlab << "1" << "2" << "4" - << "1" << "2" << "4" << "4" << "1"; + << "1" << "2" << "4" << "4" << "1"; - QDir matlabTemplatePath = QDir( templatepath + QString(MATLAB_CODE_DIR)); - QDir matlabOutputPath = QDir( outputpath + QString("matlab") ); + QDir matlabTemplatePath = QDir(templatepath + QString(MATLAB_CODE_DIR)); + QDir matlabOutputPath = QDir(outputpath + QString("matlab")); matlabOutputPath.mkpath(matlabOutputPath.absolutePath()); - QString matlabCodeTemplate = readFile( matlabTemplatePath.absoluteFilePath( "uavobject.m.template") ); + QString matlabCodeTemplate = readFile(matlabTemplatePath.absoluteFilePath("uavobject.m.template")); - if (matlabCodeTemplate.isEmpty() ) { + if (matlabCodeTemplate.isEmpty()) { std::cerr << "Problem reading matlab templates" << endl; return false; } for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { - ObjectInfo* info=parser->getObjectByIndex(objidx); - int numBytes=parser->getNumBytes(objidx); + ObjectInfo *info = parser->getObjectByIndex(objidx); + int numBytes = parser->getNumBytes(objidx); process_object(info, numBytes); } - matlabCodeTemplate.replace( QString("$(INSTANTIATIONCODE)"), matlabInstantiationCode); - matlabCodeTemplate.replace( QString("$(SWITCHCODE)"), matlabSwitchCode); - matlabCodeTemplate.replace( QString("$(CLEANUPCODE)"), matlabCleanupCode); - matlabCodeTemplate.replace( QString("$(SAVEOBJECTSCODE)"), matlabSaveObjectsCode); - matlabCodeTemplate.replace( QString("$(ALLOCATIONCODE)"), matlabAllocationCode); - matlabCodeTemplate.replace( QString("$(EXPORTCSVCODE)"), matlabExportCsvCode); + matlabCodeTemplate.replace(QString("$(INSTANTIATIONCODE)"), matlabInstantiationCode); + matlabCodeTemplate.replace(QString("$(SWITCHCODE)"), matlabSwitchCode); + matlabCodeTemplate.replace(QString("$(CLEANUPCODE)"), matlabCleanupCode); + matlabCodeTemplate.replace(QString("$(SAVEOBJECTSCODE)"), matlabSaveObjectsCode); + matlabCodeTemplate.replace(QString("$(ALLOCATIONCODE)"), matlabAllocationCode); + matlabCodeTemplate.replace(QString("$(EXPORTCSVCODE)"), matlabExportCsvCode); - bool res = writeFile( matlabOutputPath.absolutePath() + "/OPLogConvert.m", matlabCodeTemplate ); + bool res = writeFile(matlabOutputPath.absolutePath() + "/OPLogConvert.m", matlabCodeTemplate); if (!res) { cout << "Error: Could not write output files" << endl; return false; @@ -71,23 +71,24 @@ bool UAVObjectGeneratorMatlab::generate(UAVObjectParser* parser,QString template /** * Generate the matlab object files */ -bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info, int numBytes) +bool UAVObjectGeneratorMatlab::process_object(ObjectInfo *info, int numBytes) { - if (info == NULL) + if (info == NULL) { return false; + } - //Declare variables + // Declare variables QString objectName(info->name); // QString objectTableName(objectName + "Objects"); QString objectTableName(objectName); QString tableIdxName(objectName.toLower() + "Idx"); QString objectID(QString().setNum(info->id)); - QString numBytesString=QString("%1").arg(numBytes); + QString numBytesString = QString("%1").arg(numBytes); - //=========================================================================// + // =========================================================================// // Generate instantiation code (will replace the $(INSTANTIATIONCODE) tag) // - //=========================================================================// + // =========================================================================// QString type; QString instantiationFields; @@ -100,10 +101,11 @@ bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info, int numBytes) // Determine type type = fieldTypeStrMatlab[info->fields[n]->type]; // Append field - if ( info->fields[n]->numElements > 1 ) + if (info->fields[n]->numElements > 1) { instantiationFields.append(",...\n\t\t '" + info->fields[n]->name + "', zeros(" + QString::number(info->fields[n]->numElements, 10) + ",1)"); - else + } else { instantiationFields.append(",...\n\t\t '" + info->fields[n]->name + "', 0"); + } } instantiationFields.append(");\n"); @@ -113,83 +115,81 @@ bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info, int numBytes) matlabInstantiationCode.append("\t" + objectName + "FidIdx = [];\n"); - //==============================================================// + // ==============================================================// // Generate 'Switch:' code (will replace the $(SWITCHCODE) tag) // - //==============================================================// + // ==============================================================// matlabSwitchCode.append("\t\tcase " + objectTableName.toUpper() + "_OBJID\n"); - matlabSwitchCode.append("\t\t\t" + tableIdxName + " = " + tableIdxName +" + 1;\n"); + matlabSwitchCode.append("\t\t\t" + tableIdxName + " = " + tableIdxName + " + 1;\n"); matlabSwitchCode.append("\t\t\t" + objectTableName + "FidIdx(" + tableIdxName + ") = bufferIdx; %#ok<*AGROW>\n"); - matlabSwitchCode.append("\t\t\tbufferIdx=bufferIdx + " + objectTableName.toUpper() + "_NUMBYTES+1; %+1 is for CRC\n"); - if(!info->isSingleInst){ + matlabSwitchCode.append("\t\t\tbufferIdx=bufferIdx + " + objectTableName.toUpper() + "_NUMBYTES+1; %+1 is for CRC\n"); + if (!info->isSingleInst) { matlabSwitchCode.append("\t\t\tbufferIdx = bufferIdx + 2; %An extra two bytes for the instance ID\n"); } - matlabSwitchCode.append("\t\t\tif " + tableIdxName + " >= length(" + objectTableName +"FidIdx) %Check to see if pre-allocated memory is exhausted\n"); + matlabSwitchCode.append("\t\t\tif " + tableIdxName + " >= length(" + objectTableName + "FidIdx) %Check to see if pre-allocated memory is exhausted\n"); matlabSwitchCode.append("\t\t\t\t" + objectTableName + "FidIdx(" + tableIdxName + "*2) = 0;\n"); matlabSwitchCode.append("\t\t\tend\n"); - - //============================================================// - // Generate 'Cleanup:' code (will replace the $(CLEANUP) tag) // - //============================================================// - matlabCleanupCode.append(objectTableName + "FidIdx =" + objectTableName + "FidIdx(1:" + tableIdxName +");\n" ); - //=================================================================// + // ============================================================// + // Generate 'Cleanup:' code (will replace the $(CLEANUP) tag) // + // ============================================================// + matlabCleanupCode.append(objectTableName + "FidIdx =" + objectTableName + "FidIdx(1:" + tableIdxName + ");\n"); + + // =================================================================// // Generate functions code (will replace the $(ALLOCATIONCODE) tag) // - //=================================================================// - //Generate function description comment + // =================================================================// + // Generate function description comment matlabAllocationCode.append("% " + objectName + " typecasting\n"); QString allocationFields; - //Add timestamp + // Add timestamp allocationFields.append("\t" + objectName + ".timestamp = " + - "double(typecast(buffer(mcolon(" + objectName + "FidIdx " - "- 20, " + objectName + "FidIdx + 4-1 -20)), 'uint32'))';\n"); + "double(typecast(buffer(mcolon(" + objectName + "FidIdx " + "- 20, " + objectName + "FidIdx + 4-1 -20)), 'uint32'))';\n"); - int currentIdx=0; + int currentIdx = 0; - //Add Instance ID, if necessary - if(!info->isSingleInst){ + // Add Instance ID, if necessary + if (!info->isSingleInst) { allocationFields.append("\t" + objectName + ".instanceID = " + - "double(typecast(buffer(mcolon(" + objectName + "FidIdx " - ", " + objectName + "FidIdx + 2-1)), 'uint16'))';\n"); - currentIdx+=2; + "double(typecast(buffer(mcolon(" + objectName + "FidIdx " + ", " + objectName + "FidIdx + 2-1)), 'uint16'))';\n"); + currentIdx += 2; } for (int n = 0; n < info->fields.length(); ++n) { // Determine variabel type type = fieldTypeStrMatlab[info->fields[n]->type]; - //Determine variable type length + // Determine variable type length QString size = fieldSizeStrMatlab[info->fields[n]->type]; // Append field - if ( info->fields[n]->numElements > 1 ){ + if (info->fields[n]->numElements > 1) { allocationFields.append("\t" + objectName + "." + info->fields[n]->name + " = " + - "reshape(double(typecast(buffer(mcolon(" + objectName + "FidIdx + " + QString("%1").arg(currentIdx) + - ", " + objectName + "FidIdx + " + QString("%1").arg(currentIdx + size.toInt()*info->fields[n]->numElements - 1) + ")), '" + type + "')), "+ QString::number(info->fields[n]->numElements, 10) + ", [] );\n"); - } - else{ + "reshape(double(typecast(buffer(mcolon(" + objectName + "FidIdx + " + QString("%1").arg(currentIdx) + + ", " + objectName + "FidIdx + " + QString("%1").arg(currentIdx + size.toInt() * info->fields[n]->numElements - 1) + ")), '" + type + "')), " + QString::number(info->fields[n]->numElements, 10) + ", [] );\n"); + } else { allocationFields.append("\t" + objectName + "." + info->fields[n]->name + " = " + - "double(typecast(buffer(mcolon(" + objectName + "FidIdx + " + QString("%1").arg(currentIdx) + - ", " + objectName + "FidIdx + " + QString("%1").arg(currentIdx + size.toInt() - 1) + ")), '" + type + "'))';\n"); + "double(typecast(buffer(mcolon(" + objectName + "FidIdx + " + QString("%1").arg(currentIdx) + + ", " + objectName + "FidIdx + " + QString("%1").arg(currentIdx + size.toInt() - 1) + ")), '" + type + "'))';\n"); } - currentIdx+=size.toInt()*info->fields[n]->numElements; + currentIdx += size.toInt() * info->fields[n]->numElements; } matlabAllocationCode.append(allocationFields); matlabAllocationCode.append("\n"); - //========================================================================// + // ========================================================================// // Generate objects saving code (will replace the $(SAVEOBJECTSCODE) tag) // - //========================================================================// - matlabSaveObjectsCode.append(",'"+objectTableName+"'"); + // ========================================================================// + matlabSaveObjectsCode.append(",'" + objectTableName + "'"); - //==========================================================================// + // ==========================================================================// // Generate objects csv export code (will replace the $(EXPORTCSVCODE) tag) // - //==========================================================================// - matlabExportCsvCode.append("\tOPLog2csv(" + objectTableName + ", '"+objectTableName+"', logfile);\n"); -// OPLog2csv(ActuatorCommand, 'ActuatorCommand', logfile) - + // ==========================================================================// + matlabExportCsvCode.append("\tOPLog2csv(" + objectTableName + ", '" + objectTableName + "', logfile);\n"); +// OPLog2csv(ActuatorCommand, 'ActuatorCommand', logfile) return true; diff --git a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h index 1d3761724..68593f358 100644 --- a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h +++ b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h @@ -31,13 +31,12 @@ #include "../generator_common.h" -class UAVObjectGeneratorMatlab -{ +class UAVObjectGeneratorMatlab { public: - bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); + bool generate(UAVObjectParser *gen, QString templatepath, QString outputpath); private: - bool process_object(ObjectInfo* info, int numBytes); + bool process_object(ObjectInfo *info, int numBytes); QString matlabInstantiationCode; QString matlabSwitchCode; QString matlabCleanupCode; @@ -46,7 +45,6 @@ private: QString matlabExportCsvCode; QStringList fieldTypeStrMatlab; QStringList fieldSizeStrMatlab; - }; -#endif +#endif // ifndef UAVOBJECTGENERATORMATLAB_H diff --git a/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.cpp b/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.cpp index 35e8e017c..f1ee41143 100644 --- a/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.cpp +++ b/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.cpp @@ -27,12 +27,13 @@ #include "uavobjectgeneratorpython.h" using namespace std; -bool UAVObjectGeneratorPython::generate(UAVObjectParser* parser,QString templatepath,QString outputpath) { +bool UAVObjectGeneratorPython::generate(UAVObjectParser *parser, QString templatepath, QString outputpath) +{ // Load template and setup output directory - pythonCodePath = QDir( templatepath + QString("flight/modules/FlightPlan/lib")); - pythonOutputPath = QDir( outputpath + QString("python") ); + pythonCodePath = QDir(templatepath + QString("flight/modules/FlightPlan/lib")); + pythonOutputPath = QDir(outputpath + QString("python")); pythonOutputPath.mkpath(pythonOutputPath.absolutePath()); - pythonCodeTemplate = readFile( pythonCodePath.absoluteFilePath("uavobject.pyt.template") ); + pythonCodeTemplate = readFile(pythonCodePath.absoluteFilePath("uavobject.pyt.template")); if (pythonCodeTemplate.isEmpty()) { std::cerr << "Problem reading python templates" << endl; return false; @@ -40,7 +41,7 @@ bool UAVObjectGeneratorPython::generate(UAVObjectParser* parser,QString template // Process each object for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { - ObjectInfo* info=parser->getObjectByIndex(objidx); + ObjectInfo *info = parser->getObjectByIndex(objidx); process_object(info); } @@ -50,10 +51,11 @@ bool UAVObjectGeneratorPython::generate(UAVObjectParser* parser,QString template /** * Generate the python object files */ -bool UAVObjectGeneratorPython::process_object(ObjectInfo* info) +bool UAVObjectGeneratorPython::process_object(ObjectInfo *info) { - if (info == NULL) + if (info == NULL) { return false; + } // Prepare output strings QString outCode = pythonCodeTemplate; @@ -63,35 +65,33 @@ bool UAVObjectGeneratorPython::process_object(ObjectInfo* info) // Replace the ($DATAFIELDS) tag QString datafields; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { // Class header datafields.append(QString("# Field %1 definition\n").arg(info->fields[n]->name)); datafields.append(QString("class %1Field(UAVObjectField):\n").arg(info->fields[n]->name)); // Only for enum types - if (info->fields[n]->type == FIELDTYPE_ENUM) - { + if (info->fields[n]->type == FIELDTYPE_ENUM) { datafields.append(QString(" # Enumeration options\n")); // Go through each option QStringList options = info->fields[n]->options; for (int m = 0; m < options.length(); ++m) { QString name = options[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), ""); - if (name[0].isDigit()) + if (name[0].isDigit()) { name = QString("N%1").arg(name); + } datafields.append(QString(" %1 = %2\n").arg(name).arg(m)); } } // Generate element names (only if field has more than one element) - if (info->fields[n]->numElements > 1 && !info->fields[n]->defaultElementNames) - { + if (info->fields[n]->numElements > 1 && !info->fields[n]->defaultElementNames) { datafields.append(QString(" # Array element names\n")); // Go through the element names QStringList elemNames = info->fields[n]->elementNames; - for (int m = 0; m < elemNames.length(); ++m) - { + for (int m = 0; m < elemNames.length(); ++m) { QString name = elemNames[m].toUpper().replace(QRegExp(ENUM_SPECIAL_CHARS), ""); - if (name[0].isDigit()) + if (name[0].isDigit()) { name = QString("N%1").arg(name); + } datafields.append(QString(" %1 = %2\n").arg(name).arg(m)); } } @@ -103,15 +103,14 @@ bool UAVObjectGeneratorPython::process_object(ObjectInfo* info) // Replace the $(DATAFIELDINIT) tag QString fields; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { fields.append(QString(" self.%1 = %1Field()\n").arg(info->fields[n]->name)); fields.append(QString(" self.addField(self.%1)\n").arg(info->fields[n]->name)); } outCode.replace(QString("$(DATAFIELDINIT)"), fields); // Write the Python code - bool res = writeFileIfDiffrent( pythonOutputPath.absolutePath() + "/" + info->namelc + ".py", outCode ); + bool res = writeFileIfDiffrent(pythonOutputPath.absolutePath() + "/" + info->namelc + ".py", outCode); if (!res) { cout << "Error: Could not write Python output files" << endl; return false; diff --git a/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.h b/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.h index 3d21b2923..6cb31d616 100644 --- a/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.h +++ b/ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.h @@ -29,13 +29,12 @@ #include "../generator_common.h" -class UAVObjectGeneratorPython -{ +class UAVObjectGeneratorPython { public: - bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); + bool generate(UAVObjectParser *gen, QString templatepath, QString outputpath); private: - bool process_object(ObjectInfo* info); + bool process_object(ObjectInfo *info); QString pythonCodeTemplate; QDir pythonCodePath; diff --git a/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.cpp b/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.cpp index 653e466e3..8a095f342 100644 --- a/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.cpp +++ b/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.cpp @@ -28,36 +28,36 @@ using namespace std; -bool UAVObjectGeneratorWireshark::generate(UAVObjectParser* parser,QString templatepath,QString outputpath) { +bool UAVObjectGeneratorWireshark::generate(UAVObjectParser *parser, QString templatepath, QString outputpath) +{ + fieldTypeStrHf << "FT_INT8" << "FT_INT16" << "FT_INT32" << "FT_UINT8" + << "FT_UINT16" << "FT_UINT32" << "FT_FLOAT" << "FT_UINT8"; + fieldTypeStrGlib << "gint8" << "gint16" << "gint32" << "guint8" + << "guint16" << "guint32" << "gfloat" << "guint8"; - fieldTypeStrHf << "FT_INT8" << "FT_INT16" << "FT_INT32" <<"FT_UINT8" - <<"FT_UINT16" << "FT_UINT32" << "FT_FLOAT" << "FT_UINT8"; - fieldTypeStrGlib << "gint8" << "gint16" << "gint32" <<"guint8" - <<"guint16" << "guint32" << "gfloat" << "guint8"; + wiresharkCodePath = QDir(templatepath + QString("ground/openpilotgcs/src/plugins/uavobjects/wireshark")); - wiresharkCodePath = QDir( templatepath + QString("ground/openpilotgcs/src/plugins/uavobjects/wireshark")); - - wiresharkOutputPath = QDir( outputpath + QString("wireshark") ); + wiresharkOutputPath = QDir(outputpath + QString("wireshark")); wiresharkOutputPath.mkpath(wiresharkOutputPath.absolutePath()); - wiresharkCodeTemplate = readFile( wiresharkCodePath.absoluteFilePath("op-uavobjects/packet-op-uavobjects.c.template") ); - wiresharkMakeTemplate = readFile( wiresharkCodePath.absoluteFilePath("op-uavobjects/Makefile.common.template") ); + wiresharkCodeTemplate = readFile(wiresharkCodePath.absoluteFilePath("op-uavobjects/packet-op-uavobjects.c.template")); + wiresharkMakeTemplate = readFile(wiresharkCodePath.absoluteFilePath("op-uavobjects/Makefile.common.template")); - if ( wiresharkCodeTemplate.isNull() || wiresharkMakeTemplate.isNull()) { - cerr << "Error: Could not open wireshark template files." << endl; - return false; + if (wiresharkCodeTemplate.isNull() || wiresharkMakeTemplate.isNull()) { + cerr << "Error: Could not open wireshark template files." << endl; + return false; } /* Copy static files for wireshark plugins root directory into output directory */ QStringList topstaticfiles; topstaticfiles << "Custom.m4" << "Custom.make" << "Custom.nmake"; for (int i = 0; i < topstaticfiles.length(); ++i) { - QFile::copy(wiresharkCodePath.absoluteFilePath(topstaticfiles[i]), - wiresharkOutputPath.absoluteFilePath(topstaticfiles[i])); + QFile::copy(wiresharkCodePath.absoluteFilePath(topstaticfiles[i]), + wiresharkOutputPath.absoluteFilePath(topstaticfiles[i])); } /* Copy static files for op-uavtalk dissector into output directory */ - QDir uavtalkOutputPath = QDir( outputpath + QString("wireshark/op-uavtalk") ); + QDir uavtalkOutputPath = QDir(outputpath + QString("wireshark/op-uavtalk")); uavtalkOutputPath.mkpath(uavtalkOutputPath.absolutePath()); QStringList uavtalkstaticfiles; uavtalkstaticfiles << "AUTHORS" << "COPYING" << "ChangeLog"; @@ -66,12 +66,12 @@ bool UAVObjectGeneratorWireshark::generate(UAVObjectParser* parser,QString templ uavtalkstaticfiles << "plugin.rc.in"; uavtalkstaticfiles << "Makefile.common" << "packet-op-uavtalk.c"; for (int i = 0; i < uavtalkstaticfiles.length(); ++i) { - QFile::copy(wiresharkCodePath.absoluteFilePath("op-uavtalk/" + uavtalkstaticfiles[i]), - uavtalkOutputPath.absoluteFilePath(uavtalkstaticfiles[i])); + QFile::copy(wiresharkCodePath.absoluteFilePath("op-uavtalk/" + uavtalkstaticfiles[i]), + uavtalkOutputPath.absoluteFilePath(uavtalkstaticfiles[i])); } /* Copy static files for op-uavobjects dissector into output directory */ - QDir uavobjectsOutputPath = QDir( outputpath + QString("wireshark/op-uavobjects") ); + QDir uavobjectsOutputPath = QDir(outputpath + QString("wireshark/op-uavobjects")); uavobjectsOutputPath.mkpath(uavobjectsOutputPath.absolutePath()); QStringList uavostaticfiles; uavostaticfiles << "AUTHORS" << "COPYING" << "ChangeLog"; @@ -79,25 +79,25 @@ bool UAVObjectGeneratorWireshark::generate(UAVObjectParser* parser,QString templ uavostaticfiles << "Makefile.am" << "moduleinfo.h" << "moduleinfo.nmake"; uavostaticfiles << "plugin.rc.in"; for (int i = 0; i < uavostaticfiles.length(); ++i) { - QFile::copy(wiresharkCodePath.absoluteFilePath("op-uavobjects/" + uavostaticfiles[i]), - uavobjectsOutputPath.absoluteFilePath(uavostaticfiles[i])); + QFile::copy(wiresharkCodePath.absoluteFilePath("op-uavobjects/" + uavostaticfiles[i]), + uavobjectsOutputPath.absoluteFilePath(uavostaticfiles[i])); } /* Generate the per-object files from the templates, and keep track of the list of generated filenames */ QString objFileNames; for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { - ObjectInfo* info = parser->getObjectByIndex(objidx); - process_object(info, uavobjectsOutputPath); - objFileNames.append(" packet-op-uavobjects-" + info->namelc + ".c"); + ObjectInfo *info = parser->getObjectByIndex(objidx); + process_object(info, uavobjectsOutputPath); + objFileNames.append(" packet-op-uavobjects-" + info->namelc + ".c"); } /* Write the uavobject dissector's Makefile.common */ - wiresharkMakeTemplate.replace( QString("$(UAVOBJFILENAMES)"), objFileNames); - bool res = writeFileIfDiffrent( uavobjectsOutputPath.absolutePath() + "/Makefile.common", - wiresharkMakeTemplate ); + wiresharkMakeTemplate.replace(QString("$(UAVOBJFILENAMES)"), objFileNames); + bool res = writeFileIfDiffrent(uavobjectsOutputPath.absolutePath() + "/Makefile.common", + wiresharkMakeTemplate); if (!res) { - cout << "Error: Could not write wireshark Makefile" << endl; - return false; + cout << "Error: Could not write wireshark Makefile" << endl; + return false; } return true; @@ -106,11 +106,12 @@ bool UAVObjectGeneratorWireshark::generate(UAVObjectParser* parser,QString templ /** * Generate the Flight object files -**/ -bool UAVObjectGeneratorWireshark::process_object(ObjectInfo* info, QDir outputpath) + **/ +bool UAVObjectGeneratorWireshark::process_object(ObjectInfo *info, QDir outputpath) { - if (info == NULL) + if (info == NULL) { return false; + } // Prepare output strings QString outCode = wiresharkCodeTemplate; @@ -121,18 +122,18 @@ bool UAVObjectGeneratorWireshark::process_object(ObjectInfo* info, QDir outputpa // Replace the $(SUBTREES) and $(SUBTREESTATICS) tags QString subtrees; QString subtreestatics; - subtreestatics.append( QString("static gint ett_uavo = -1;\r\n") ); - subtrees.append( QString("&ett_uavo,\r\n") ); + subtreestatics.append(QString("static gint ett_uavo = -1;\r\n")); + subtrees.append(QString("&ett_uavo,\r\n")); for (int n = 0; n < info->fields.length(); ++n) { - if (info->fields[n]->numElements > 1) { - /* Reserve a subtree for each array */ - subtreestatics.append( QString("static gint ett_%1_%2 = -1;\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) ); - subtrees.append( QString("&ett_%1_%2,\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) ); - } + if (info->fields[n]->numElements > 1) { + /* Reserve a subtree for each array */ + subtreestatics.append(QString("static gint ett_%1_%2 = -1;\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + subtrees.append(QString("&ett_%1_%2,\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + } } outCode.replace(QString("$(SUBTREES)"), subtrees); outCode.replace(QString("$(SUBTREESTATICS)"), subtreestatics); @@ -141,144 +142,144 @@ bool UAVObjectGeneratorWireshark::process_object(ObjectInfo* info, QDir outputpa QString type; QString fields; for (int n = 0; n < info->fields.length(); ++n) { - fields.append( QString("static int hf_op_uavobjects_%1_%2 = -1;\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name)); - if (info->fields[n]->numElements > 1) { - QStringList elemNames = info->fields[n]->elementNames; - for (int m = 0; m < elemNames.length(); ++m) { - fields.append( QString("static int hf_op_uavobjects_%1_%2_%3 = -1;\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) - .arg(elemNames[m]) ); - } - } + fields.append(QString("static int hf_op_uavobjects_%1_%2 = -1;\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + if (info->fields[n]->numElements > 1) { + QStringList elemNames = info->fields[n]->elementNames; + for (int m = 0; m < elemNames.length(); ++m) { + fields.append(QString("static int hf_op_uavobjects_%1_%2_%3 = -1;\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name) + .arg(elemNames[m])); + } + } } outCode.replace(QString("$(FIELDHANDLES)"), fields); // Replace the $(ENUMFIELDNAMES) tag QString enums; for (int n = 0; n < info->fields.length(); ++n) { - // Only for enum types - if (info->fields[n]->type == FIELDTYPE_ENUM) { - enums.append(QString("/* Field %1 information */\r\n").arg(info->fields[n]->name) ); - enums.append(QString("/* Enumeration options for field %1 */\r\n").arg(info->fields[n]->name)); - enums.append( QString("static const value_string uavobjects_%1_%2[]= {\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) ); - // Go through each option - QStringList options = info->fields[n]->options; - for (int m = 0; m < options.length(); ++m) { - enums.append ( QString("\t{ %1, \"%2\" },\r\n") - .arg(m) - .arg(options[m].replace(QRegExp(ENUM_SPECIAL_CHARS), "") ) ); - } - enums.append( QString("\t{ 0, NULL }\r\n") ); - enums.append( QString("};\r\n") ); - } + // Only for enum types + if (info->fields[n]->type == FIELDTYPE_ENUM) { + enums.append(QString("/* Field %1 information */\r\n").arg(info->fields[n]->name)); + enums.append(QString("/* Enumeration options for field %1 */\r\n").arg(info->fields[n]->name)); + enums.append(QString("static const value_string uavobjects_%1_%2[]= {\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + // Go through each option + QStringList options = info->fields[n]->options; + for (int m = 0; m < options.length(); ++m) { + enums.append(QString("\t{ %1, \"%2\" },\r\n") + .arg(m) + .arg(options[m].replace(QRegExp(ENUM_SPECIAL_CHARS), ""))); + } + enums.append(QString("\t{ 0, NULL }\r\n")); + enums.append(QString("};\r\n")); + } } outCode.replace(QString("$(ENUMFIELDNAMES)"), enums); // Replace the $(POPULATETREE) tag QString treefields; for (int n = 0; n < info->fields.length(); ++n) { - if ( info->fields[n]->numElements == 1 ) { - treefields.append( QString(" ptvcursor_add(cursor, hf_op_uavobjects_%1_%2, sizeof(%3), ENC_LITTLE_ENDIAN);\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) - .arg(fieldTypeStrGlib[info->fields[n]->type]) ); - } else { - treefields.append( QString(" {\r\n") ); - treefields.append( QString(" proto_item * it = NULL;\r\n") ); - treefields.append( QString(" it = ptvcursor_add_no_advance(cursor, hf_op_uavobjects_%1_%2, 1, ENC_NA);\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) ); - treefields.append( QString(" ptvcursor_push_subtree(cursor, it, ett_%1_%2);\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) ); - /* Populate each array element into the table */ - QStringList elemNames = info->fields[n]->elementNames; - for (int m = 0; m < elemNames.length(); ++m) { - treefields.append( QString(" ptvcursor_add(cursor, hf_op_uavobjects_%1_%2_%3, sizeof(%4), ENC_LITTLE_ENDIAN);\r\n") - .arg(info->namelc) - .arg(info->fields[n]->name) - .arg(elemNames[m]) - .arg(fieldTypeStrGlib[info->fields[n]->type]) ); - } - treefields.append( QString(" ptvcursor_pop_subtree(cursor);\r\n") ); - treefields.append( QString(" }\r\n") ); - } + if (info->fields[n]->numElements == 1) { + treefields.append(QString(" ptvcursor_add(cursor, hf_op_uavobjects_%1_%2, sizeof(%3), ENC_LITTLE_ENDIAN);\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name) + .arg(fieldTypeStrGlib[info->fields[n]->type])); + } else { + treefields.append(QString(" {\r\n")); + treefields.append(QString(" proto_item * it = NULL;\r\n")); + treefields.append(QString(" it = ptvcursor_add_no_advance(cursor, hf_op_uavobjects_%1_%2, 1, ENC_NA);\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + treefields.append(QString(" ptvcursor_push_subtree(cursor, it, ett_%1_%2);\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + /* Populate each array element into the table */ + QStringList elemNames = info->fields[n]->elementNames; + for (int m = 0; m < elemNames.length(); ++m) { + treefields.append(QString(" ptvcursor_add(cursor, hf_op_uavobjects_%1_%2_%3, sizeof(%4), ENC_LITTLE_ENDIAN);\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name) + .arg(elemNames[m]) + .arg(fieldTypeStrGlib[info->fields[n]->type])); + } + treefields.append(QString(" ptvcursor_pop_subtree(cursor);\r\n")); + treefields.append(QString(" }\r\n")); + } } outCode.replace(QString("$(POPULATETREE)"), treefields); // Replace the $(HEADERFIELDS) tag QString headerfields; - headerfields.append( QString(" static hf_register_info hf[] = {\r\n") ); + headerfields.append(QString(" static hf_register_info hf[] = {\r\n")); for (int n = 0; n < info->fields.length(); ++n) { - // For non-array fields - if ( info->fields[n]->numElements == 1) { - headerfields.append( QString("\t { &hf_op_uavobjects_%1_%2,\r\n") - .arg( info->namelc ) - .arg( info->fields[n]->name ) ); - headerfields.append( QString("\t { \"%1\", \"%2.%3\", %4,\r\n") - .arg( info->fields[n]->name ) - .arg( info->namelc ) - .arg( info->fields[n]->name ) - .arg( fieldTypeStrHf[info->fields[n]->type] ) ); - if ( info->fields[n]->type == FIELDTYPE_ENUM ) { - headerfields.append( QString("\t BASE_DEC, VALS(uavobjects_%1_%2), 0x0, NULL, HFILL \r\n") - .arg( info->namelc ) - .arg( info->fields[n]->name ) ); - } else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) { - headerfields.append( QString("\t BASE_NONE, NULL, 0x0, NULL, HFILL \r\n") ); - } else { - headerfields.append( QString("\t BASE_DEC_HEX, NULL, 0x0, NULL, HFILL\r\n") ); - } - headerfields.append( QString("\t },\r\n") ); - headerfields.append( QString("\t },\r\n") ); - } else { - headerfields.append( QString("\t { &hf_op_uavobjects_%1_%2,\r\n") - .arg( info->namelc ) - .arg( info->fields[n]->name ) ); - headerfields.append( QString("\t { \"%1\", \"%2.%3\", FT_NONE,\r\n") - .arg( info->fields[n]->name ) - .arg( info->namelc ) - .arg( info->fields[n]->name ) ); - headerfields.append( QString("\t BASE_NONE, NULL, 0x0, NULL, HFILL\r\n") ); - headerfields.append( QString("\t },\r\n") ); - headerfields.append( QString("\t },\r\n") ); + // For non-array fields + if (info->fields[n]->numElements == 1) { + headerfields.append(QString("\t { &hf_op_uavobjects_%1_%2,\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + headerfields.append(QString("\t { \"%1\", \"%2.%3\", %4,\r\n") + .arg(info->fields[n]->name) + .arg(info->namelc) + .arg(info->fields[n]->name) + .arg(fieldTypeStrHf[info->fields[n]->type])); + if (info->fields[n]->type == FIELDTYPE_ENUM) { + headerfields.append(QString("\t BASE_DEC, VALS(uavobjects_%1_%2), 0x0, NULL, HFILL \r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + headerfields.append(QString("\t BASE_NONE, NULL, 0x0, NULL, HFILL \r\n")); + } else { + headerfields.append(QString("\t BASE_DEC_HEX, NULL, 0x0, NULL, HFILL\r\n")); + } + headerfields.append(QString("\t },\r\n")); + headerfields.append(QString("\t },\r\n")); + } else { + headerfields.append(QString("\t { &hf_op_uavobjects_%1_%2,\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + headerfields.append(QString("\t { \"%1\", \"%2.%3\", FT_NONE,\r\n") + .arg(info->fields[n]->name) + .arg(info->namelc) + .arg(info->fields[n]->name)); + headerfields.append(QString("\t BASE_NONE, NULL, 0x0, NULL, HFILL\r\n")); + headerfields.append(QString("\t },\r\n")); + headerfields.append(QString("\t },\r\n")); - QStringList elemNames = info->fields[n]->elementNames; - for (int m = 0; m < elemNames.length(); ++m) { - headerfields.append( QString("\t { &hf_op_uavobjects_%1_%2_%3,\r\n") - .arg( info->namelc ) - .arg( info->fields[n]->name ) - .arg( elemNames[m]) ); - headerfields.append( QString("\t { \"%1\", \"%2.%3.%4\", %5,\r\n") - .arg( elemNames[m] ) - .arg( info->namelc ) - .arg( info->fields[n]->name ) - .arg( elemNames[m] ) - .arg( fieldTypeStrHf[info->fields[n]->type] ) ); - if ( info->fields[n]->type == FIELDTYPE_ENUM ) { - headerfields.append( QString("\t BASE_DEC, VALS(uavobjects_%1_%2), 0x0, NULL, HFILL \r\n") - .arg( info->namelc ) - .arg( info->fields[n]->name ) ); - } else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) { - headerfields.append( QString("\t BASE_NONE, NULL, 0x0, NULL, HFILL \r\n") ); - } else { - headerfields.append( QString("\t BASE_DEC_HEX, NULL, 0x0, NULL, HFILL\r\n") ); - } - headerfields.append( QString("\t },\r\n") ); - headerfields.append( QString("\t },\r\n") ); - } - } + QStringList elemNames = info->fields[n]->elementNames; + for (int m = 0; m < elemNames.length(); ++m) { + headerfields.append(QString("\t { &hf_op_uavobjects_%1_%2_%3,\r\n") + .arg(info->namelc) + .arg(info->fields[n]->name) + .arg(elemNames[m])); + headerfields.append(QString("\t { \"%1\", \"%2.%3.%4\", %5,\r\n") + .arg(elemNames[m]) + .arg(info->namelc) + .arg(info->fields[n]->name) + .arg(elemNames[m]) + .arg(fieldTypeStrHf[info->fields[n]->type])); + if (info->fields[n]->type == FIELDTYPE_ENUM) { + headerfields.append(QString("\t BASE_DEC, VALS(uavobjects_%1_%2), 0x0, NULL, HFILL \r\n") + .arg(info->namelc) + .arg(info->fields[n]->name)); + } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { + headerfields.append(QString("\t BASE_NONE, NULL, 0x0, NULL, HFILL \r\n")); + } else { + headerfields.append(QString("\t BASE_DEC_HEX, NULL, 0x0, NULL, HFILL\r\n")); + } + headerfields.append(QString("\t },\r\n")); + headerfields.append(QString("\t },\r\n")); + } + } } - headerfields.append( QString(" };\r\n") ); + headerfields.append(QString(" };\r\n")); outCode.replace(QString("$(HEADERFIELDS)"), headerfields); // Write the flight code - bool res = writeFileIfDiffrent( outputpath.absolutePath() + "/packet-op-uavobjects-" + info->namelc + ".c", outCode ); + bool res = writeFileIfDiffrent(outputpath.absolutePath() + "/packet-op-uavobjects-" + info->namelc + ".c", outCode); if (!res) { cout << "Error: Could not write wireshark code files" << endl; return false; @@ -286,5 +287,3 @@ bool UAVObjectGeneratorWireshark::process_object(ObjectInfo* info, QDir outputpa return true; } - - diff --git a/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.h b/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.h index 402aac17e..cd87dca80 100644 --- a/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.h +++ b/ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.h @@ -29,10 +29,9 @@ #include "../generator_common.h" -class UAVObjectGeneratorWireshark -{ +class UAVObjectGeneratorWireshark { public: - bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); + bool generate(UAVObjectParser *gen, QString templatepath, QString outputpath); QStringList fieldTypeStrHf; QStringList fieldTypeStrGlib; QString wiresharkCodeTemplate, wiresharkMakeTemplate; @@ -40,9 +39,7 @@ public: QDir wiresharkOutputPath; private: - bool process_object(ObjectInfo* info, QDir outputpath); - + bool process_object(ObjectInfo *info, QDir outputpath); }; #endif - diff --git a/ground/uavobjgenerator/main.cpp b/ground/uavobjgenerator/main.cpp index 8bce4fd09..55de04f3d 100644 --- a/ground/uavobjgenerator/main.cpp +++ b/ground/uavobjgenerator/main.cpp @@ -37,17 +37,18 @@ #include "generators/wireshark/uavobjectgeneratorwireshark.h" #define RETURN_ERR_USAGE 1 -#define RETURN_ERR_XML 2 -#define RETURN_OK 0 +#define RETURN_ERR_XML 2 +#define RETURN_OK 0 using namespace std; /** * print usage info */ -void usage() { +void usage() +{ cout << "Usage: uavobjectgenerator [-gcs] [-flight] [-java] [-python] [-matlab] [-wireshark] [-none] [-v] xml_path template_base [UAVObj1] ... [UAVObjN]" << endl; - cout << "Languages: "<< endl; + cout << "Languages: " << endl; cout << "\t-gcs build groundstation code" << endl; cout << "\t-flight build flight code" << endl; cout << "\t-java build java code" << endl; @@ -55,7 +56,7 @@ void usage() { cout << "\t-matlab build matlab code" << endl; cout << "\t-wireshark build wireshark plugin" << endl; cout << "\tIf no language is specified ( and not -none ) -> all are built." << endl; - cout << "Misc: "<< endl; + cout << "Misc: " << endl; cout << "\t-none build no language - just parse xml's" << endl; cout << "\t-h this help" << endl; cout << "\t-v verbose" << endl; @@ -69,7 +70,8 @@ void usage() { /** * inform user of invalid usage */ -int usage_err() { +int usage_err() +{ cout << "Invalid usage!" << endl; usage(); return RETURN_ERR_USAGE; @@ -91,78 +93,81 @@ int main(int argc, char *argv[]) QStringList objects_stringlist; // process arguments - for (int argi=1;argi0)||(arguments_stringlist.removeAll("-h")>0)) { - usage(); - return RETURN_OK; } - bool verbose=(arguments_stringlist.removeAll("-v")>0); - bool do_gcs=(arguments_stringlist.removeAll("-gcs")>0); - bool do_flight=(arguments_stringlist.removeAll("-flight")>0); - bool do_java=(arguments_stringlist.removeAll("-java")>0); - bool do_python=(arguments_stringlist.removeAll("-python")>0); - bool do_matlab=(arguments_stringlist.removeAll("-matlab")>0); - bool do_wireshark=(arguments_stringlist.removeAll("-wireshark")>0); - bool do_none=(arguments_stringlist.removeAll("-none")>0); // + if ((arguments_stringlist.removeAll("-h") > 0) || (arguments_stringlist.removeAll("-h") > 0)) { + usage(); + return RETURN_OK; + } - bool do_all=((do_gcs||do_flight||do_java||do_python||do_matlab)==false); - bool do_allObjects=true; + bool verbose = (arguments_stringlist.removeAll("-v") > 0); + bool do_gcs = (arguments_stringlist.removeAll("-gcs") > 0); + bool do_flight = (arguments_stringlist.removeAll("-flight") > 0); + bool do_java = (arguments_stringlist.removeAll("-java") > 0); + bool do_python = (arguments_stringlist.removeAll("-python") > 0); + bool do_matlab = (arguments_stringlist.removeAll("-matlab") > 0); + bool do_wireshark = (arguments_stringlist.removeAll("-wireshark") > 0); + bool do_none = (arguments_stringlist.removeAll("-none") > 0); // + + bool do_all = ((do_gcs || do_flight || do_java || do_python || do_matlab) == false); + bool do_allObjects = true; if (arguments_stringlist.length() >= 2) { - inputpath = arguments_stringlist.at(0); + inputpath = arguments_stringlist.at(0); templatepath = arguments_stringlist.at(1); } else { // wrong number of arguments return usage_err(); } - if (arguments_stringlist.length() >2) { - do_allObjects=false; - for (int argi=2;argi 2) { + do_allObjects = false; + for (int argi = 2; argi < arguments_stringlist.length(); argi++) { + objects_stringlist << (arguments_stringlist.at(argi).toLower() + ".xml"); } } - if (!inputpath.endsWith("/")) + if (!inputpath.endsWith("/")) { inputpath.append("/"); // append a slash if it is not there - - if (!templatepath.endsWith("/")) + } + if (!templatepath.endsWith("/")) { templatepath.append("/"); // append a slash if it is not there - + } // put all output files in the current directory outputpath = QString("./"); QDir xmlPath = QDir(inputpath); - UAVObjectParser* parser = new UAVObjectParser(); + UAVObjectParser *parser = new UAVObjectParser(); - QStringList filters=QStringList("*.xml"); + QStringList filters = QStringList("*.xml"); xmlPath.setNameFilters(filters); - QFileInfoList xmlList = xmlPath.entryInfoList(); + QFileInfoList xmlList = xmlPath.entryInfoList(); // Read in each XML file and parse object(s) in them - + for (int n = 0; n < xmlList.length(); ++n) { QFileInfo fileinfo = xmlList[n]; if (!do_allObjects) { if (!objects_stringlist.removeAll(fileinfo.fileName().toLower())) { - if (verbose) - cout << "Skipping XML file: " << fileinfo.fileName().toStdString() << endl; - continue; + if (verbose) { + cout << "Skipping XML file: " << fileinfo.fileName().toStdString() << endl; + } + continue; } } - if (verbose) - cout << "Parsing XML file: " << fileinfo.fileName().toStdString() << endl; + if (verbose) { + cout << "Parsing XML file: " << fileinfo.fileName().toStdString() << endl; + } QString filename = fileinfo.fileName(); - QString xmlstr = readFile(fileinfo.absoluteFilePath()); + QString xmlstr = readFile(fileinfo.absoluteFilePath()); QString res = parser->parseXML(xmlstr, filename); if (!res.isNull()) { - if (!verbose) { - cout << "Error in XML file: " << fileinfo.fileName().toStdString() << endl; + if (!verbose) { + cout << "Error in XML file: " << fileinfo.fileName().toStdString() << endl; } cout << "Error parsing " << res.toStdString() << endl; return RETURN_ERR_XML; @@ -176,13 +181,14 @@ int main(int argc, char *argv[]) // check for duplicate object ID's QList objIDList; - int numBytesTotal=0; + int numBytesTotal = 0; for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { quint32 id = parser->getObjectID(objidx); - numBytesTotal+=parser->getNumBytes(objidx); - if (verbose) - cout << "Checking object " << parser->getObjectName(objidx).toStdString() << " (" << parser->getNumBytes(objidx) << " bytes)" << endl; - if ( objIDList.contains(id) || id == 0 ) { + numBytesTotal += parser->getNumBytes(objidx); + if (verbose) { + cout << "Checking object " << parser->getObjectName(objidx).toStdString() << " (" << parser->getNumBytes(objidx) << " bytes)" << endl; + } + if (objIDList.contains(id) || id == 0) { cout << "Error: Object ID collision found in object " << parser->getObjectName(objidx).toStdString() << ", modify object name" << endl; return RETURN_ERR_XML; } @@ -193,56 +199,57 @@ int main(int argc, char *argv[]) // done parsing and checking cout << "Done: processed " << xmlList.length() << " XML files and generated " << objIDList.length() << " objects with no ID collisions. Total size of the data fields is " << numBytesTotal << " bytes." << endl; - - if (verbose) + + if (verbose) { cout << "used units: " << parser->all_units.join(",").toStdString() << endl; + } - if (do_none) - return RETURN_OK; + if (do_none) { + return RETURN_OK; + } // generate flight code if wanted - if (do_flight|do_all) { - cout << "generating flight code" << endl ; + if (do_flight | do_all) { + cout << "generating flight code" << endl; UAVObjectGeneratorFlight flightgen; - flightgen.generate(parser,templatepath,outputpath); + flightgen.generate(parser, templatepath, outputpath); } // generate gcs code if wanted - if (do_gcs|do_all) { - cout << "generating gcs code" << endl ; + if (do_gcs | do_all) { + cout << "generating gcs code" << endl; UAVObjectGeneratorGCS gcsgen; - gcsgen.generate(parser,templatepath,outputpath); + gcsgen.generate(parser, templatepath, outputpath); } // generate java code if wanted - if (do_java|do_all) { - cout << "generating java code" << endl ; + if (do_java | do_all) { + cout << "generating java code" << endl; UAVObjectGeneratorJava javagen; - javagen.generate(parser,templatepath,outputpath); + javagen.generate(parser, templatepath, outputpath); } // generate python code if wanted - if (do_python|do_all) { - cout << "generating python code" << endl ; + if (do_python | do_all) { + cout << "generating python code" << endl; UAVObjectGeneratorPython pygen; - pygen.generate(parser,templatepath,outputpath); + pygen.generate(parser, templatepath, outputpath); } // generate matlab code if wanted - if (do_matlab|do_all) { - cout << "generating matlab code" << endl ; + if (do_matlab | do_all) { + cout << "generating matlab code" << endl; UAVObjectGeneratorMatlab matlabgen; - matlabgen.generate(parser,templatepath,outputpath); + matlabgen.generate(parser, templatepath, outputpath); } // generate wireshark plugin if wanted - if (do_wireshark|do_all) { - cout << "generating wireshark code" << endl ; + if (do_wireshark | do_all) { + cout << "generating wireshark code" << endl; UAVObjectGeneratorWireshark wiresharkgen; - wiresharkgen.generate(parser,templatepath,outputpath); + wiresharkgen.generate(parser, templatepath, outputpath); } return RETURN_OK; } - diff --git a/ground/uavobjgenerator/uavobjectparser.cpp b/ground/uavobjgenerator/uavobjectparser.cpp index 9bf26f014..118e35029 100644 --- a/ground/uavobjgenerator/uavobjectparser.cpp +++ b/ground/uavobjgenerator/uavobjectparser.cpp @@ -32,18 +32,17 @@ UAVObjectParser::UAVObjectParser() { fieldTypeStrXML << "int8" << "int16" << "int32" << "uint8" - << "uint16" << "uint32" <<"float" << "enum"; + << "uint16" << "uint32" << "float" << "enum"; updateModeStrXML << "manual" << "periodic" << "onchange" << "throttled"; accessModeStr << "ACCESS_READWRITE" << "ACCESS_READONLY"; fieldTypeNumBytes << int(1) << int(2) << int(4) << - int(1) << int(2) << int(4) << - int(4) << int(1); + int(1) << int(2) << int(4) << + int(4) << int(1); accessModeStrXML << "readwrite" << "readonly"; - } /** @@ -57,12 +56,12 @@ int UAVObjectParser::getNumObjects() /** * Get the detailed object information */ -QList UAVObjectParser::getObjectInfo() +QList UAVObjectParser::getObjectInfo() { return objInfo; } -ObjectInfo* UAVObjectParser::getObjectByIndex(int objIndex) +ObjectInfo *UAVObjectParser::getObjectByIndex(int objIndex) { return objInfo[objIndex]; } @@ -72,9 +71,11 @@ ObjectInfo* UAVObjectParser::getObjectByIndex(int objIndex) */ QString UAVObjectParser::getObjectName(int objIndex) { - ObjectInfo* info = objInfo[objIndex]; - if (info == NULL) + ObjectInfo *info = objInfo[objIndex]; + + if (info == NULL) { return QString(); + } return info->name; } @@ -84,9 +85,11 @@ QString UAVObjectParser::getObjectName(int objIndex) */ quint32 UAVObjectParser::getObjectID(int objIndex) { - ObjectInfo* info = objInfo[objIndex]; - if (info == NULL) + ObjectInfo *info = objInfo[objIndex]; + + if (info == NULL) { return 0; + } return info->id; } @@ -95,23 +98,20 @@ quint32 UAVObjectParser::getObjectID(int objIndex) */ int UAVObjectParser::getNumBytes(int objIndex) { - ObjectInfo* info = objInfo[objIndex]; - if (info == NULL) - { + ObjectInfo *info = objInfo[objIndex]; + + if (info == NULL) { return 0; - } - else - { + } else { int numBytes = 0; - for (int n = 0; n < info->fields.length(); ++n) - { + for (int n = 0; n < info->fields.length(); ++n) { numBytes += info->fields[n]->numBytes * info->fields[n]->numElements; } return numBytes; } } -bool fieldTypeLessThan(const FieldInfo* f1, const FieldInfo* f2) +bool fieldTypeLessThan(const FieldInfo *f1, const FieldInfo *f2) { return f1->numBytes > f2->numBytes; } @@ -122,115 +122,125 @@ bool fieldTypeLessThan(const FieldInfo* f1, const FieldInfo* f2) * @param filename The xml filename * @returns Null QString() on success, error message on failure */ -QString UAVObjectParser::parseXML(QString& xml, QString& filename) +QString UAVObjectParser::parseXML(QString & xml, QString & filename) { // Create DOM document and parse it QDomDocument doc("UAVObjects"); bool parsed = doc.setContent(xml); - if (!parsed) return QString("Improperly formated XML file"); + + if (!parsed) { + return QString("Improperly formated XML file"); + } // Read all objects contained in the XML file, creating an new ObjectInfo for each QDomElement docElement = doc.documentElement(); QDomNode node = docElement.firstChild(); - while ( !node.isNull() ) { + while (!node.isNull()) { // Create new object entry - ObjectInfo* info = new ObjectInfo; + ObjectInfo *info = new ObjectInfo; - info->filename=filename; + info->filename = filename; // Process object attributes QString status = processObjectAttributes(node, info); - if (!status.isNull()) + if (!status.isNull()) { return status; + } // Process child elements (fields and metadata) - QDomNode childNode = node.firstChild(); - bool fieldFound = false; - bool accessFound = false; - bool telGCSFound = false; - bool telFlightFound = false; + QDomNode childNode = node.firstChild(); + bool fieldFound = false; + bool accessFound = false; + bool telGCSFound = false; + bool telFlightFound = false; bool logFound = false; bool descriptionFound = false; - while ( !childNode.isNull() ) { + while (!childNode.isNull()) { // Process element depending on its type - if ( childNode.nodeName().compare(QString("field")) == 0 ) { + if (childNode.nodeName().compare(QString("field")) == 0) { QString status = processObjectFields(childNode, info); - if (!status.isNull()) + if (!status.isNull()) { return status; + } fieldFound = true; - } - else if ( childNode.nodeName().compare(QString("access")) == 0 ) { + } else if (childNode.nodeName().compare(QString("access")) == 0) { QString status = processObjectAccess(childNode, info); - if (!status.isNull()) + if (!status.isNull()) { return status; + } accessFound = true; - } - else if ( childNode.nodeName().compare(QString("telemetrygcs")) == 0 ) { + } else if (childNode.nodeName().compare(QString("telemetrygcs")) == 0) { QString status = processObjectMetadata(childNode, &info->gcsTelemetryUpdateMode, &info->gcsTelemetryUpdatePeriod, &info->gcsTelemetryAcked); - if (!status.isNull()) + if (!status.isNull()) { return status; + } telGCSFound = true; - } - else if ( childNode.nodeName().compare(QString("telemetryflight")) == 0 ) { + } else if (childNode.nodeName().compare(QString("telemetryflight")) == 0) { QString status = processObjectMetadata(childNode, &info->flightTelemetryUpdateMode, &info->flightTelemetryUpdatePeriod, &info->flightTelemetryAcked); - if (!status.isNull()) + if (!status.isNull()) { return status; + } telFlightFound = true; - } - else if ( childNode.nodeName().compare(QString("logging")) == 0 ) { + } else if (childNode.nodeName().compare(QString("logging")) == 0) { QString status = processObjectMetadata(childNode, &info->loggingUpdateMode, &info->loggingUpdatePeriod, NULL); - if (!status.isNull()) + if (!status.isNull()) { return status; + } logFound = true; - } - else if ( childNode.nodeName().compare(QString("description")) == 0 ) { + } else if (childNode.nodeName().compare(QString("description")) == 0) { QString status = processObjectDescription(childNode, &info->description); - if (!status.isNull()) + if (!status.isNull()) { return status; + } descriptionFound = true; - } - else if (!childNode.isComment()) { + } else if (!childNode.isComment()) { return QString("Unknown object element"); } // Get next element childNode = childNode.nextSibling(); } - - // Sort all fields according to size + + // Sort all fields according to size qStableSort(info->fields.begin(), info->fields.end(), fieldTypeLessThan); // Sort all fields according to size qStableSort(info->fields.begin(), info->fields.end(), fieldTypeLessThan); // Make sure that required elements were found - if ( !fieldFound ) + if (!fieldFound) { return QString("Object::field element is missing"); + } - if ( !accessFound ) + if (!accessFound) { return QString("Object::access element is missing"); + } - if ( !telGCSFound ) + if (!telGCSFound) { return QString("Object::telemetrygcs element is missing"); + } - if ( !telFlightFound ) + if (!telFlightFound) { return QString("Object::telemetryflight element is missing"); + } - if ( !logFound ) + if (!logFound) { return QString("Object::logging element is missing"); + } // TODO: Make into error once all objects updated - if ( !descriptionFound ) + if (!descriptionFound) { return QString("Object::description element is missing"); + } // Calculate ID calculateID(info); @@ -253,10 +263,11 @@ QString UAVObjectParser::parseXML(QString& xml, QString& filename) * and is used to avoid connecting objects with incompatible configurations. * The LSB is set to zero and is reserved for metadata */ -void UAVObjectParser::calculateID(ObjectInfo* info) +void UAVObjectParser::calculateID(ObjectInfo *info) { // Hash object name quint32 hash = updateHash(info->name, 0); + // Hash object attributes hash = updateHash(info->isSettings, hash); hash = updateHash(info->isSingleInst, hash); @@ -265,10 +276,11 @@ void UAVObjectParser::calculateID(ObjectInfo* info) hash = updateHash(info->fields[n]->name, hash); hash = updateHash(info->fields[n]->numElements, hash); hash = updateHash(info->fields[n]->type, hash); - if(info->fields[n]->type == FIELDTYPE_ENUM) { + if (info->fields[n]->type == FIELDTYPE_ENUM) { QStringList options = info->fields[n]->options; - for (int m = 0; m < options.length(); m++) + for (int m = 0; m < options.length(); m++) { hash = updateHash(options[m], hash); + } } } // Done @@ -283,18 +295,20 @@ void UAVObjectParser::calculateID(ObjectInfo* info) */ quint32 UAVObjectParser::updateHash(quint32 value, quint32 hash) { - return (hash ^ ((hash<<5) + (hash>>2) + value)); + return hash ^ ((hash << 5) + (hash >> 2) + value); } /** * Update the hash given a string */ -quint32 UAVObjectParser::updateHash(QString& value, quint32 hash) +quint32 UAVObjectParser::updateHash(QString & value, quint32 hash) { QByteArray bytes = value.toAscii(); - quint32 hashout = hash; - for (int n = 0; n < bytes.length(); ++n) + quint32 hashout = hash; + + for (int n = 0; n < bytes.length(); ++n) { hashout = updateHash(bytes[n], hashout); + } return hashout; } @@ -302,41 +316,47 @@ quint32 UAVObjectParser::updateHash(QString& value, quint32 hash) /** * Process the metadata part of the XML */ -QString UAVObjectParser::processObjectMetadata(QDomNode& childNode, UpdateMode* mode, int* period, bool* acked) +QString UAVObjectParser::processObjectMetadata(QDomNode & childNode, UpdateMode *mode, int *period, bool *acked) { // Get updatemode attribute QDomNamedNodeMap elemAttributes = childNode.attributes(); QDomNode elemAttr = elemAttributes.namedItem("updatemode"); - if ( elemAttr.isNull() ) + + if (elemAttr.isNull()) { return QString("Object:telemetrygcs:updatemode attribute is missing"); + } - int index = updateModeStrXML.indexOf( elemAttr.nodeValue() ); + int index = updateModeStrXML.indexOf(elemAttr.nodeValue()); - if (index<0) + if (index < 0) { return QString("Object:telemetrygcs:updatemode attribute value is invalid"); + } - *mode = (UpdateMode)index; + *mode = (UpdateMode)index; // Get period attribute elemAttr = elemAttributes.namedItem("period"); - if ( elemAttr.isNull() ) + if (elemAttr.isNull()) { return QString("Object:telemetrygcs:period attribute is missing"); + } *period = elemAttr.nodeValue().toInt(); // Get acked attribute (only if acked parameter is not null, not applicable for logging metadata) - if ( acked != NULL) { + if (acked != NULL) { elemAttr = elemAttributes.namedItem("acked"); - if ( elemAttr.isNull()) + if (elemAttr.isNull()) { return QString("Object:telemetrygcs:acked attribute is missing"); + } - if ( elemAttr.nodeValue().compare(QString("true")) == 0 ) + if (elemAttr.nodeValue().compare(QString("true")) == 0) { *acked = true; - else if ( elemAttr.nodeValue().compare(QString("false")) == 0 ) + } else if (elemAttr.nodeValue().compare(QString("false")) == 0) { *acked = false; - else + } else { return QString("Object:telemetrygcs:acked attribute value is invalid"); + } } // Done return QString(); @@ -345,30 +365,35 @@ QString UAVObjectParser::processObjectMetadata(QDomNode& childNode, UpdateMode* /** * Process the object access tag of the XML */ -QString UAVObjectParser::processObjectAccess(QDomNode& childNode, ObjectInfo* info) +QString UAVObjectParser::processObjectAccess(QDomNode & childNode, ObjectInfo *info) { // Get gcs attribute QDomNamedNodeMap elemAttributes = childNode.attributes(); QDomNode elemAttr = elemAttributes.namedItem("gcs"); - if ( elemAttr.isNull() ) - return QString("Object:access:gcs attribute is missing"); - int index = accessModeStrXML.indexOf( elemAttr.nodeValue() ); - if (index >= 0) + if (elemAttr.isNull()) { + return QString("Object:access:gcs attribute is missing"); + } + + int index = accessModeStrXML.indexOf(elemAttr.nodeValue()); + if (index >= 0) { info->gcsAccess = (AccessMode)index; - else + } else { return QString("Object:access:gcs attribute value is invalid"); + } // Get flight attribute elemAttr = elemAttributes.namedItem("flight"); - if ( elemAttr.isNull() ) + if (elemAttr.isNull()) { return QString("Object:access:flight attribute is missing"); + } - index = accessModeStrXML.indexOf( elemAttr.nodeValue() ); - if (index >= 0) + index = accessModeStrXML.indexOf(elemAttr.nodeValue()); + if (index >= 0) { info->flightAccess = (AccessMode)index; - else + } else { return QString("Object:access:flight attribute value is invalid"); + } // Done return QString(); @@ -377,13 +402,14 @@ QString UAVObjectParser::processObjectAccess(QDomNode& childNode, ObjectInfo* in /** * Process the object fields of the XML */ -QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* info) +QString UAVObjectParser::processObjectFields(QDomNode & childNode, ObjectInfo *info) { // Create field - FieldInfo* field = new FieldInfo; + FieldInfo *field = new FieldInfo; // Get name attribute QDomNamedNodeMap elemAttributes = childNode.attributes(); QDomNode elemAttr = elemAttributes.namedItem("name"); + if (elemAttr.isNull()) { return QString("Object:field:name attribute is missing"); } @@ -393,12 +419,12 @@ QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* in // field that has already been declared elemAttr = elemAttributes.namedItem("cloneof"); if (!elemAttr.isNull()) { - QString parentName = elemAttr.nodeValue(); + QString parentName = elemAttr.nodeValue(); if (!parentName.isEmpty()) { - foreach(FieldInfo * parent, info->fields) { + foreach(FieldInfo * parent, info->fields) { if (parent->name == parentName) { // clone from this parent - *field = *parent; // safe shallow copy, no ptrs in struct + *field = *parent; // safe shallow copy, no ptrs in struct field->name = name; // set our name // Add field to object info->fields.append(field); @@ -407,35 +433,34 @@ QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* in } } return QString("Object:field::cloneof parent unknown"); - } - else { + } else { return QString("Object:field:cloneof attribute is empty"); } - } - else { + } else { // this field is not a clone, so remember its name field->name = name; } // Get units attribute elemAttr = elemAttributes.namedItem("units"); - if ( elemAttr.isNull() ) + if (elemAttr.isNull()) { return QString("Object:field:units attribute is missing"); + } field->units = elemAttr.nodeValue(); all_units << field->units; // Get type attribute elemAttr = elemAttributes.namedItem("type"); - if ( elemAttr.isNull() ) + if (elemAttr.isNull()) { return QString("Object:field:type attribute is missing"); + } int index = fieldTypeStrXML.indexOf(elemAttr.nodeValue()); if (index >= 0) { - field->type = (FieldType)index; + field->type = (FieldType)index; field->numBytes = fieldTypeNumBytes[index]; - } - else { + } else { return QString("Object:field:type attribute value is invalid"); } @@ -443,28 +468,28 @@ QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* in field->numElements = 0; // Look for element names as an attribute first elemAttr = elemAttributes.namedItem("elementnames"); - if ( !elemAttr.isNull() ) { + if (!elemAttr.isNull()) { // Get element names QStringList names = elemAttr.nodeValue().split(",", QString::SkipEmptyParts); - for (int n = 0; n < names.length(); ++n) + for (int n = 0; n < names.length(); ++n) { names[n] = names[n].trimmed(); + } field->elementNames = names; - field->numElements = names.length(); + field->numElements = names.length(); field->defaultElementNames = false; - } - else { + } else { // Look for a list of child elementname nodes QDomNode listNode = childNode.firstChildElement("elementnames"); if (!listNode.isNull()) { for (QDomElement node = listNode.firstChildElement("elementname"); !node.isNull(); node = node.nextSiblingElement("elementname")) { - QDomNode name = node.firstChild(); + QDomNode name = node.firstChild(); if (!name.isNull() && name.isText() && !name.nodeValue().isEmpty()) { field->elementNames.append(name.nodeValue()); } } - field->numElements = field->elementNames.length(); + field->numElements = field->elementNames.length(); field->defaultElementNames = false; } } @@ -472,20 +497,19 @@ QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* in // for the number of elements in the 'elements' attribute if (field->numElements == 0) { elemAttr = elemAttributes.namedItem("elements"); - if ( elemAttr.isNull() ) { + if (elemAttr.isNull()) { return QString("Object:field:elements and Object:field:elementnames attribute/element is missing"); - } - else { + } else { field->numElements = elemAttr.nodeValue().toInt(); - for (int n = 0; n < field->numElements; ++n) + for (int n = 0; n < field->numElements; ++n) { field->elementNames.append(QString("%1").arg(n)); + } field->defaultElementNames = true; } } // Get options attribute or child elements (only if an enum type) if (field->type == FIELDTYPE_ENUM) { - // Look for options attribute elemAttr = elemAttributes.namedItem("options"); if (!elemAttr.isNull()) { @@ -494,19 +518,18 @@ QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* in options[n] = options[n].trimmed(); } field->options = options; - } - else { - // Look for a list of child 'option' nodes - QDomNode listNode = childNode.firstChildElement("options"); - if (!listNode.isNull()) { - for (QDomElement node = listNode.firstChildElement("option"); - !node.isNull(); node = node.nextSiblingElement("option")) { - QDomNode name = node.firstChild(); + } else { + // Look for a list of child 'option' nodes + QDomNode listNode = childNode.firstChildElement("options"); + if (!listNode.isNull()) { + for (QDomElement node = listNode.firstChildElement("option"); + !node.isNull(); node = node.nextSiblingElement("option")) { + QDomNode name = node.firstChild(); if (!name.isNull() && name.isText() && !name.nodeValue().isEmpty()) { - field->options.append(name.nodeValue()); - } - } - } + field->options.append(name.nodeValue()); + } + } + } } if (field->options.length() == 0) { return QString("Object:field:options attribute/element is missing"); @@ -515,35 +538,37 @@ QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* in // Get the default value attribute (required for settings objects, optional for the rest) elemAttr = elemAttributes.namedItem("defaultvalue"); - if ( elemAttr.isNull() ) { - if ( info->isSettings ) + if (elemAttr.isNull()) { + if (info->isSettings) { return QString("Object:field:defaultvalue attribute is missing (required for settings objects)"); + } field->defaultValues = QStringList(); - } - else { - QStringList defaults = elemAttr.nodeValue().split(",", QString::SkipEmptyParts); - for (int n = 0; n < defaults.length(); ++n) - defaults[n] = defaults[n].trimmed(); + } else { + QStringList defaults = elemAttr.nodeValue().split(",", QString::SkipEmptyParts); + for (int n = 0; n < defaults.length(); ++n) { + defaults[n] = defaults[n].trimmed(); + } - if(defaults.length() != field->numElements) { - if(defaults.length() != 1) - return QString("Object:field:incorrect number of default values"); + if (defaults.length() != field->numElements) { + if (defaults.length() != 1) { + return QString("Object:field:incorrect number of default values"); + } - /*support legacy single default for multiple elements - We should really issue a warning*/ - for(int ct=1; ct< field->numElements; ct++) - defaults.append(defaults[0]); - } - field->defaultValues = defaults; + /*support legacy single default for multiple elements + We should really issue a warning*/ + for (int ct = 1; ct < field->numElements; ct++) { + defaults.append(defaults[0]); + } + } + field->defaultValues = defaults; } // Limits attribute elemAttr = elemAttributes.namedItem("limits"); - if ( elemAttr.isNull() ) { - field->limitValues=QString(); - } - else{ - field->limitValues=elemAttr.nodeValue(); + if (elemAttr.isNull()) { + field->limitValues = QString(); + } else { + field->limitValues = elemAttr.nodeValue(); } // Add field to object info->fields.append(field); @@ -554,52 +579,58 @@ QString UAVObjectParser::processObjectFields(QDomNode& childNode, ObjectInfo* in /** * Process the object attributes from the XML */ -QString UAVObjectParser::processObjectAttributes(QDomNode& node, ObjectInfo* info) +QString UAVObjectParser::processObjectAttributes(QDomNode & node, ObjectInfo *info) { // Get name attribute QDomNamedNodeMap attributes = node.attributes(); QDomNode attr = attributes.namedItem("name"); - if ( attr.isNull() ) - return QString("Object:name attribute is missing"); - info->name = attr.nodeValue(); + if (attr.isNull()) { + return QString("Object:name attribute is missing"); + } + + info->name = attr.nodeValue(); info->namelc = attr.nodeValue().toLower(); // Get category attribute if present attr = attributes.namedItem("category"); - if ( !attr.isNull() ) - { + if (!attr.isNull()) { info->category = attr.nodeValue(); } // Get singleinstance attribute attr = attributes.namedItem("singleinstance"); - if ( attr.isNull() ) + if (attr.isNull()) { return QString("Object:singleinstance attribute is missing"); + } - if ( attr.nodeValue().compare(QString("true")) == 0 ) + if (attr.nodeValue().compare(QString("true")) == 0) { info->isSingleInst = true; - else if ( attr.nodeValue().compare(QString("false")) == 0 ) + } else if (attr.nodeValue().compare(QString("false")) == 0) { info->isSingleInst = false; - else + } else { return QString("Object:singleinstance attribute value is invalid"); + } // Get settings attribute attr = attributes.namedItem("settings"); - if ( attr.isNull() ) + if (attr.isNull()) { return QString("Object:settings attribute is missing"); + } - if ( attr.nodeValue().compare(QString("true")) == 0 ) - info->isSettings = true; - else if ( attr.nodeValue().compare(QString("false")) == 0 ) + if (attr.nodeValue().compare(QString("true")) == 0) { + info->isSettings = true; + } else if (attr.nodeValue().compare(QString("false")) == 0) { info->isSettings = false; - else + } else { return QString("Object:settings attribute value is invalid"); + } // Settings objects can only have a single instance - if ( info->isSettings && !info->isSingleInst ) + if (info->isSettings && !info->isSingleInst) { return QString("Object: Settings objects can not have multiple instances"); + } // Done return QString(); @@ -608,7 +639,7 @@ QString UAVObjectParser::processObjectAttributes(QDomNode& node, ObjectInfo* inf /** * Process the description field from the XML file */ -QString UAVObjectParser::processObjectDescription(QDomNode& childNode, QString * description) +QString UAVObjectParser::processObjectDescription(QDomNode & childNode, QString *description) { description->append(childNode.firstChild().nodeValue()); return QString(); diff --git a/ground/uavobjgenerator/uavobjectparser.h b/ground/uavobjgenerator/uavobjectparser.h index bab24142e..d6ae4b60c 100644 --- a/ground/uavobjgenerator/uavobjectparser.h +++ b/ground/uavobjgenerator/uavobjectparser.h @@ -48,75 +48,74 @@ typedef enum { } FieldType; typedef struct { - QString name; - QString units; - FieldType type; + QString name; + QString units; + FieldType type; int numElements; int numBytes; QStringList elementNames; QStringList options; // for enums only bool defaultElementNames; QStringList defaultValues; - QString limitValues; + QString limitValues; } FieldInfo; /** * Object update mode */ typedef enum { - UPDATEMODE_MANUAL = 0, /** Manually update object, by calling the updated() function */ - UPDATEMODE_PERIODIC = 1, /** Automatically update object at periodic intervals */ + UPDATEMODE_MANUAL = 0, /** Manually update object, by calling the updated() function */ + UPDATEMODE_PERIODIC = 1, /** Automatically update object at periodic intervals */ UPDATEMODE_ONCHANGE = 2, /** Only update object when its data changes */ - UPDATEMODE_THROTTLED = 3 /** Object is updated on change, but not more often than the interval time */ + UPDATEMODE_THROTTLED = 3 /** Object is updated on change, but not more often than the interval time */ } UpdateMode; typedef enum { ACCESS_READWRITE = 0, - ACCESS_READONLY = 1 + ACCESS_READONLY = 1 } AccessMode; typedef struct { - QString name; - QString namelc; /** name in lowercase */ - QString filename; - quint32 id; - bool isSingleInst; - bool isSettings; + QString name; + QString namelc; /** name in lowercase */ + QString filename; + quint32 id; + bool isSingleInst; + bool isSettings; AccessMode gcsAccess; AccessMode flightAccess; - bool flightTelemetryAcked; + bool flightTelemetryAcked; UpdateMode flightTelemetryUpdateMode; /** Update mode used by the autopilot (UpdateMode) */ int flightTelemetryUpdatePeriod; /** Update period used by the autopilot (only if telemetry mode is PERIODIC) */ - bool gcsTelemetryAcked; + bool gcsTelemetryAcked; UpdateMode gcsTelemetryUpdateMode; /** Update mode used by the GCS (UpdateMode) */ int gcsTelemetryUpdatePeriod; /** Update period used by the GCS (only if telemetry mode is PERIODIC) */ UpdateMode loggingUpdateMode; /** Update mode used by the logging module (UpdateMode) */ int loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */ - QList fields; /** The data fields for the object **/ - QString description; /** Description used for Doxygen **/ - QString category; /** Description used for Doxygen **/ + QList fields; /** The data fields for the object **/ + QString description; /** Description used for Doxygen **/ + QString category; /** Description used for Doxygen **/ } ObjectInfo; -class UAVObjectParser -{ +class UAVObjectParser { public: // Functions UAVObjectParser(); - QString parseXML(QString& xml, QString& filename); + QString parseXML(QString & xml, QString & filename); int getNumObjects(); - QList getObjectInfo(); + QList getObjectInfo(); QString getObjectName(int objIndex); quint32 getObjectID(int objIndex); - ObjectInfo* getObjectByIndex(int objIndex); + ObjectInfo *getObjectByIndex(int objIndex); int getNumBytes(int objIndex); QStringList all_units; private: - QList objInfo; + QList objInfo; QStringList fieldTypeStrXML; QList fieldTypeNumBytes; QStringList updateModeStr; @@ -124,15 +123,15 @@ private: QStringList accessModeStr; QStringList accessModeStrXML; - QString processObjectAttributes(QDomNode& node, ObjectInfo* info); - QString processObjectFields(QDomNode& childNode, ObjectInfo* info); - QString processObjectAccess(QDomNode& childNode, ObjectInfo* info); - QString processObjectDescription(QDomNode& childNode, QString * description); - QString processObjectCategory(QDomNode& childNode, QString * category); - QString processObjectMetadata(QDomNode& childNode, UpdateMode* mode, int* period, bool* acked); - void calculateID(ObjectInfo* info); + QString processObjectAttributes(QDomNode & node, ObjectInfo *info); + QString processObjectFields(QDomNode & childNode, ObjectInfo *info); + QString processObjectAccess(QDomNode & childNode, ObjectInfo *info); + QString processObjectDescription(QDomNode & childNode, QString *description); + QString processObjectCategory(QDomNode & childNode, QString *category); + QString processObjectMetadata(QDomNode & childNode, UpdateMode *mode, int *period, bool *acked); + void calculateID(ObjectInfo *info); quint32 updateHash(quint32 value, quint32 hash); - quint32 updateHash(QString& value, quint32 hash); + quint32 updateHash(QString & value, quint32 hash); }; #endif // UAVOBJECTPARSER_H From 2ab9f938c91466bd734394105d1b62ea5a45b7b5 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 19 May 2013 17:11:09 +0200 Subject: [PATCH 06/61] ongoing dev work, incomplete --- .../modules/StateEstimation/stateestimation.c | 319 ++++++++++++++++++ shared/uavobjectdefinition/barostate.ekf | 10 + shared/uavobjectdefinition/revosettings.xml | 2 +- 3 files changed, 330 insertions(+), 1 deletion(-) create mode 100644 flight/modules/StateEstimation/stateestimation.c create mode 100644 shared/uavobjectdefinition/barostate.ekf diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c new file mode 100644 index 000000000..be0466652 --- /dev/null +++ b/flight/modules/StateEstimation/stateestimation.c @@ -0,0 +1,319 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup State Estimation + * @brief Acquires sensor data and computes state estimate + * @{ + * + * @file stateestimation.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief Module to handle all comms to the AHRS on a periodic basis. + * + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +// TODO goes in header + + +typedef enum { + gyr_UPDATED = 1 << 0, + acc_UPDATED = 1 << 1, + mag_UPDATED = 1 << 2, + pos_UPDATED = 1 << 3, + vel_UPDATED = 1 << 4, + bar_UPDATED = 1 << 5, + ias_UPDATED = 1 << 6 +} sensorUpdates; + +typedef struct { + float gyr[3]; + float acc[3]; + float mag[3]; + float pos[3]; + float vel[3]; + float bar[1]; + float ias[1]; + sensorUpdates updated; +} stateEstimation; + +#define ISSET(bitfield, bit) ((bitfield) & (bit) ? 1 : 0) +#define UNSET(bitfield, bit) (bitfield) &= ~(bit) + + +typedef struct stateFilterStruct { + int32_t (*init)(void); + int32_t (*update)(stateEstimation *state); +} stateFilter; + +struct filterQueueStruct; +typedef struct filterQueueStruct { + stateFilter filter; + struct filterQueueStruct *next; +} filtereQueue; + +// + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "revosettings.h" +#include "homelocation.h" + +#include "CoordinateConversions.h" + +// Private constants +#define STACK_SIZE_BYTES 2048 +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_HIGH +#define TASK_PRIORITY CALLBACK_TASKPRIORITY_FLIGHTCONTROL +#define TIMEOUT_MS 100 + +// Private types + +// Private variables +static xTaskHandle attitudeTaskHandle; +static DelayedCallbackInfo *stateEstimationCallback; + +static RevoSettingsData revoSettings; +static HomeLocationData homeLocation; +static float LLA2NEDM[3]; +static volatile sensorUpdates updatedSensors; + +// Private functions + +static void settingsUpdatedCb(UAVObjEvent *objEv); +static void sensorUpdatedCb(UAVObjEvent *objEv); +static void StateEstimationCb(void); +static int32_t getNED(GPSPositionData *gpsPosition, float *NED); + + +/** + * Initialise the module. Called before the start function + * \returns 0 on success or -1 if initialisation failed + */ +int32_t StateEstimationInitialize(void) +{ + RevoSettingsInitialize(); + + RevoSettingsConnectCallback(&settingsUpdatedCb); + HomeLocationConnectCallback(&settingsUpdatedCb); + + GyroSensorConnectCallback(&sensorUpdatedCb); + AccelSensorConnectCallback(&sensorUpdatedCb); + MagnetoSensorConnectCallback(&sensorUpdatedCb); + BaroSensorConnectCallback(&sensorUpdatedCb); + AirspeedSensorConnectCallback(&sensorUpdatedCb); + GPSPositionConnectCallback(&sensorUpdatedCb); + GPSVelocityConnectCallback(&sensorUpdatedCb); + + stateEstimationCallback = DelayedCallbackCreate(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, STACK_SIZE_BYTES); + + return 0; +} + +/** + * Start the task. Expects all objects to be initialized by this point. + * \returns 0 on success or -1 if initialisation failed + */ +int32_t StateEstimationStart(void) +{ + RevoSettingsConnectCallback(&settingsUpdatedCb); + + // Force settings update to make sure rotation loaded + settingsUpdatedCb(NULL); + + // Initialize Filters + stateFilter magFilter = filterMagInitialize(); + stateFilter baroFilter = filterBaroInitialize(); + stateFilter stationaryFilter = filterStationaryInitialize(); + stateFilter cfFilter = filterCFInitialize(); + stateFilter cfmFilter = filterCFMInitialize(); + stateFilter ekf13Filter = filterEKF13Initialize(); + stateFilter ekf16Filter = filterEKF16Initialize(); + + + return 0; +} + +MODULE_INITCALL(AttitudeInitialize, AttitudeStart) + +/** + * Module callback + */ +static void StateEstimationCb(void) +{ + // alarms flag + uint8_t alarm = 0; + + // set alarm to warning if called through timeout + if (updatedSensor == 0) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); + alarm = 1; + } + + // read updated sensor UAVObjects and set initial state + stateEstimation sensors; + sensors.updated = updatedSensors; + updatedSensors ^= sensors.updated; + + // most sensors get only rudimentary sanity checks + #define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \ + if (ISSET(sensors.updated, shortname##_UPDATED)) { \ + sensorname##Data s; \ + sensorname##GET(&s); \ + if (sane(s.a1) && sane(s.a2) && sane(s.a3)) { \ + sensors.shortname[0] = s.a1; \ + sensors.shortname[1] = s.a2; \ + sensors.shortname[2] = s.a3; \ + } \ + else { \ + UNSET(sensors.updated, shortname##_UPDATED); \ + } \ + } + SANITYCHECK3(GyroSensor, gyr, x, y, z); + SANITYCHECK3(AccelSensor, acc, x, y, z); + SANITYCHECK3(MagnetoSensor, mag, x, y, z); + SANITYCHECK3(GPSVelocity, vel, North, East, Down); + #define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \ + if (ISSET(sensors.updated, shortname##_UPDATED)) { \ + sensorname##Data s; \ + sensorname##GET(&s); \ + if (sane(s.a1) && EXTRACHECK) { \ + sensors.shortname[0] = s.a1; \ + } \ + else { \ + UNSET(sensors.updated, shortname##_UPDATED); \ + } \ + } + SANITYCHECK1(BaroSensor, bar, Altitude, 1); + SANITYCHECK1(AirspeedSensor, ias, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); + + if (ISSET(sensors.updated, pos_UPDATED)) { + GPSPositionData s; + GPSPositionGet(&s); + if (homeLocation.Set == HOMELOCATION_SET_TRUE && sane(s.Latitude) && sane(s.Longitude) && sane(s.Altitude) && (fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f)) { + getNED(&s, sensors.pos); + } else { + UNSET(sensors.updated, pos_UPDATED); + } + } + + // at this point sensor state is stored in "sensors" with some rudimentary filtering applied + + // traverse filtering chain + + + // clear alarms if everything is alright, then schedule callback execution after timeout + if (!alarm) { + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + } + DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, UPDATEMODE_SOONER); +} + +static void settingsUpdatedCb(UAVObjEvent *ev) +{ + HomeLocationGet(&homeLocation); + + if (sane(homeLocation.Latitude) && sane(homeLocation.Longitude) && sane(homeLocation.Altitude) && sane(homeLocation.Be[0]) && sane(homeLocation.Be[1]) && sane(homeLocation.Be[2])) { + // Compute matrix to convert deltaLLA to NED + float lat, alt; + lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); + alt = homeLocation.Altitude; + + LLA2NEDM[0] = alt + 6.378137E6f; + LLA2NEDM[1] = cosf(lat) * (alt + 6.378137E6f); + LLA2NEDM[2] = -1.0f; + + // TODO: convert positionState to new reference frame and gracefully update EKF state! + // needed for long range flights where the reference coordinate is adjusted in flight + } + + RevoSettingsGet(&revoSettings); + + if +} + +static void sensorUpdatedCb(UAVObjEvent *ev) +{ + if (!ev) { + return; + } + + if (ev->obj == GyroSensorHandle()) { + updatedSensors |= gyr_UPDATED; + } + + if (ev->obj == AccelSensorHandle()) { + updatedSensors |= acc_UPDATED; + } + + if (ev->obj == MagnetoSensorHandle()) { + updatedSensors |= mag_UPDATED; + } + + if (ev->obj == GPSPositionHandle()) { + updatedSensors |= pos_UPDATED; + } + + if (ev->obj == GPSVelocityHandle()) { + updatedSensors |= vel_UPDATED; + } + + if (ev->obj == BaroSensorHandle()) { + updatedSensors |= bar_UPDATED; + } + + if (ev->obj == AirspeedSensorHandle()) { + updatedSensors |= ias_UPDATED; + } + + DelayedCallbackDispatch(stateEstimationCallback); +} + +/** + * @brief Convert the GPS LLA position into NED coordinates + * @note this method uses a taylor expansion around the home coordinates + * to convert to NED which allows it to be done with all floating + * calculations + * @param[in] Current GPS coordinates + * @param[out] NED frame coordinates + * @returns 0 for success, -1 for failure + */ +static void getNED(GPSPositionData *gpsPosition, float *NED) +{ + float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f), + DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f), + (gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) }; + + NED[0] = LLA2NEDM[0] * dL[0]; + NED[1] = LLA2NEDM[1] * dL[1]; + NED[2] = LLA2NEDM[2] * dL[2]; +} + + +/** + * @} + * @} + */ diff --git a/shared/uavobjectdefinition/barostate.ekf b/shared/uavobjectdefinition/barostate.ekf new file mode 100644 index 000000000..56a331776 --- /dev/null +++ b/shared/uavobjectdefinition/barostate.ekf @@ -0,0 +1,10 @@ + + + The filtered altitude estimate. + + + + + + + diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 56d11927e..b2654e1d8 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -1,7 +1,7 @@ Settings for the revo to control the algorithm and what is updated - + From b62c7588315839901587f388aed0ce4c359fa6f8 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 19 May 2013 20:38:32 +0200 Subject: [PATCH 07/61] statefilter main file almost complete, time to start work on filter classes --- .../modules/StateEstimation/stateestimation.c | 283 +++++++++++++++--- 1 file changed, 245 insertions(+), 38 deletions(-) diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index be0466652..395116833 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -38,8 +38,8 @@ typedef enum { mag_UPDATED = 1 << 2, pos_UPDATED = 1 << 3, vel_UPDATED = 1 << 4, - bar_UPDATED = 1 << 5, - ias_UPDATED = 1 << 6 + air_UPDATED = 1 << 5, + bar_UPDATED = 1 << 6, } sensorUpdates; typedef struct { @@ -48,8 +48,8 @@ typedef struct { float mag[3]; float pos[3]; float vel[3]; + float air[2]; float bar[1]; - float ias[1]; sensorUpdates updated; } stateEstimation; @@ -59,18 +59,19 @@ typedef struct { typedef struct stateFilterStruct { int32_t (*init)(void); - int32_t (*update)(stateEstimation *state); + int32_t (*filter)(stateEstimation *state); } stateFilter; struct filterQueueStruct; typedef struct filterQueueStruct { - stateFilter filter; + stateFilter *filter; struct filterQueueStruct *next; } filtereQueue; // #include + #include #include #include @@ -79,8 +80,17 @@ typedef struct filterQueueStruct { #include #include +#include +#include +#include +#include +#include +#include +#include + #include "revosettings.h" #include "homelocation.h" +#include "flightstatus.h" #include "CoordinateConversions.h" @@ -96,17 +106,117 @@ typedef struct filterQueueStruct { static xTaskHandle attitudeTaskHandle; static DelayedCallbackInfo *stateEstimationCallback; -static RevoSettingsData revoSettings; -static HomeLocationData homeLocation; +static volatile RevoSettingsData revoSettings; +static volatile HomeLocationData homeLocation; static float LLA2NEDM[3]; static volatile sensorUpdates updatedSensors; +static int32_t fusionAlgorithm = -1; +static filterQueue *filterChain = NULL; + +// different filters available to state estimation +static stateFilter *magFilter; +static stateFilter *baroFilter; +static stateFilter *airFilter; +static stateFilter *stationaryFilter; +static stateFilter *cfFilter; +static stateFilter *cfmFilter; +static stateFilter *ekf13iFilter; +static stateFilter *ekf13Filter; +static stateFilter *ekf16Filter; +static stateFilter *ekf16iFilter; + +// preconfigured filter chains selectable via revoSettings.FusionAlgorithm +static const filterQueue *cfQueue = &(filterQueue) { + .filter = magFilter, + .next = &(filterQueue) { + .filter = baroFilter, + .next = &(filterQueue) { + .filter = airFilter, + .next = &(filterQueue) { + .filter = cfFilter, + .next = NULL, + } + } + } +}; +static const filterQueue *cfmQueue = &(filterQueue) { + .filter = magFilter, + .next = &(filterQueue) { + .filter = baroFilter, + .next = &(filterQueue) { + .filter = airFilter, + .next = &(filterQueue) { + .filter = cfmFilter, + .next = NULL, + } + } + } +}; +static const filterQueue *ekf13iQueue = &(filterQueue) { + .filter = magFilter, + .next = &(filterQueue) { + .filter = baroFilter, + .next = &(filterQueue) { + .filter = stationaryFilter, + .next = &(filterQueue) { + .filter = airFilter, + .next = &(filterQueue) { + .filter = ekf13iFilter, + .next = NULL, + } + } + } + } +}; +static const filterQueue *ekf13Queue = &(filterQueue) { + .filter = magFilter, + .next = &(filterQueue) { + .filter = baroFilter, + .next = &(filterQueue) { + .filter = airFilter, + .next = &(filterQueue) { + .filter = ekf13Filter, + .next = NULL, + } + } + } +}; +static const filterQueue *ekf16iQueue = &(filterQueue) { + .filter = magFilter, + .next = &(filterQueue) { + .filter = baroFilter, + .next = &(filterQueue) { + .filter = stationaryFilter, + .next = &(filterQueue) { + .filter = airFilter, + .next = &(filterQueue) { + .filter = ekf16iFilter, + .next = NULL, + } + } + } + } +}; +static const filterQueue *ekf16Queue = &(filterQueue) { + .filter = magFilter, + .next = &(filterQueue) { + .filter = baroFilter, + .next = &(filterQueue) { + .filter = airFilter, + .next = &(filterQueue) { + .filter = ekf16Filter, + .next = NULL, + } + } + } +}; // Private functions static void settingsUpdatedCb(UAVObjEvent *objEv); static void sensorUpdatedCb(UAVObjEvent *objEv); static void StateEstimationCb(void); -static int32_t getNED(GPSPositionData *gpsPosition, float *NED); +static void getNED(GPSPositionData *gpsPosition, float *NED); /** @@ -145,14 +255,16 @@ int32_t StateEstimationStart(void) settingsUpdatedCb(NULL); // Initialize Filters - stateFilter magFilter = filterMagInitialize(); - stateFilter baroFilter = filterBaroInitialize(); - stateFilter stationaryFilter = filterStationaryInitialize(); - stateFilter cfFilter = filterCFInitialize(); - stateFilter cfmFilter = filterCFMInitialize(); - stateFilter ekf13Filter = filterEKF13Initialize(); - stateFilter ekf16Filter = filterEKF16Initialize(); - + magFilter = filterMagInitialize(); + baroFilter = filterBaroInitialize(); + airFilter = filterAirInitialize(); + stationaryFilter = filterStationaryInitialize(); + cfFilter = filterCFInitialize(); + cfmFilter = filterCFMInitialize(); + ekf13iFilter = filterEKF13iInitialize(); + ekf13Filter = filterEKF13Initialize(); + ekf16Filter = filterEKF16Initialize(); + ekf16iFilter = filterEKF16iInitialize(); return 0; } @@ -173,56 +285,153 @@ static void StateEstimationCb(void) alarm = 1; } + // check if a new filter chain should be initialized + if (fusionAlgorithm != revoSettings.fusionAlgorithm) { + FlightStatusData fs; + FlightStatusGet(&fs); + if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == -1) { + filterQueue *newFilterChain; + switch (fusionAlgorithm) { + case REVOSETTINGS.FUSIONALGORITHM_COMPLEMENTARY: + newFilterChain = cfQueue; + break; + case REVOSETTINGS.FUSIONALGORITHM_COMPLEMENTARYMAG: + newFilterChain = cfmQueue; + break; + case REVOSETTINGS.FUSIONALGORITHM_INS13INDOOR: + newFilterChain = ekf13iQueue; + break; + case REVOSETTINGS.FUSIONALGORITHM_INS13OUTDOOR: + newFilterChain = ekf13Queue; + break; + case REVOSETTINGS.FUSIONALGORITHM_INS16INDOOR: + newFilterChain = ekf16iQueue; + break; + case REVOSETTINGS.FUSIONALGORITHM_INS16OUTDOOR: + newFilterChain = ekf16Queue; + break; + default: + newFilterChain = NULL; + } + // initialize filters in chain + filterQueue *current = newFilterChain; + bool error = 0; + while (current != NULL) { + int32_t result = current.init(); + if (result != 0) { + error = 1; + break; + } + current = current.next; + } + if (error) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + return; + } else { + // set new fusion algortithm + filterChain = newFilterChain; + fustionAlgorithm = revoSettings.fusionAlgorithm; + } + } + } + // read updated sensor UAVObjects and set initial state - stateEstimation sensors; - sensors.updated = updatedSensors; - updatedSensors ^= sensors.updated; + stateEstimation states; + states.updated = updatedSensors; + updatedSensors ^= states.updated; // most sensors get only rudimentary sanity checks - #define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \ - if (ISSET(sensors.updated, shortname##_UPDATED)) { \ +#define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \ + if (ISSET(states.updated, shortname##_UPDATED)) { \ sensorname##Data s; \ sensorname##GET(&s); \ if (sane(s.a1) && sane(s.a2) && sane(s.a3)) { \ - sensors.shortname[0] = s.a1; \ - sensors.shortname[1] = s.a2; \ - sensors.shortname[2] = s.a3; \ + states.shortname[0] = s.a1; \ + states.shortname[1] = s.a2; \ + states.shortname[2] = s.a3; \ } \ else { \ - UNSET(sensors.updated, shortname##_UPDATED); \ + UNSET(states.updated, shortname##_UPDATED); \ } \ } SANITYCHECK3(GyroSensor, gyr, x, y, z); SANITYCHECK3(AccelSensor, acc, x, y, z); SANITYCHECK3(MagnetoSensor, mag, x, y, z); SANITYCHECK3(GPSVelocity, vel, North, East, Down); - #define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \ - if (ISSET(sensors.updated, shortname##_UPDATED)) { \ +#define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \ + if (ISSET(states.updated, shortname##_UPDATED)) { \ sensorname##Data s; \ sensorname##GET(&s); \ if (sane(s.a1) && EXTRACHECK) { \ - sensors.shortname[0] = s.a1; \ + states.shortname[0] = s.a1; \ } \ else { \ - UNSET(sensors.updated, shortname##_UPDATED); \ + UNSET(states.updated, shortname##_UPDATED); \ } \ } SANITYCHECK1(BaroSensor, bar, Altitude, 1); - SANITYCHECK1(AirspeedSensor, ias, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); + SANITYCHECK1(AirspeedSensor, air, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); - if (ISSET(sensors.updated, pos_UPDATED)) { + // GPS is a tiny bit more tricky as GPSPosition is not float (otherwise the conversion to NED could sit in a filter) but integers, for precision reasons + if (ISSET(states.updated, pos_UPDATED)) { GPSPositionData s; GPSPositionGet(&s); if (homeLocation.Set == HOMELOCATION_SET_TRUE && sane(s.Latitude) && sane(s.Longitude) && sane(s.Altitude) && (fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f)) { - getNED(&s, sensors.pos); + getNED(&s, states.pos); } else { - UNSET(sensors.updated, pos_UPDATED); + UNSET(states.updated, pos_UPDATED); } } - // at this point sensor state is stored in "sensors" with some rudimentary filtering applied + // at this point sensor state is stored in "states" with some rudimentary filtering applied - // traverse filtering chain + // apply all filters in the current filter chain + filterQueue *current = filterChain; + bool error = 0; + while (current != NULL) { + int32_t result = current.filter(&states); + if (result != 0) { + error = 1; + } + current = current.next; + } + if (error) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + } + + // the final output of filters is saved in state variables +#define STORE3(statename, shortname, a1, a2, a3) \ + if (ISSET(states.updated, shortname##_UPDATED)) { \ + statename##Data s; \ + statename##GET(&s); \ + s.a1 = states.shortname[0]; \ + s.a2 = states.shortname[1]; \ + s.a3 = states.shortname[2]; \ + statename##SET(&s); \ + } + STORE3(GyroState, gyr, x, y, z); + STORE3(AccelState, acc, x, y, z); + STORE3(MagnetoState, mag, x, y, z); + STORE3(PositionState, pos, North, East, Down); + STORE3(VelocityState, vel, North, East, Down); +#define STORE2(statename, shortname, a1, a2) \ + if (ISSET(states.updated, shortname##_UPDATED)) { \ + statename##Data s; \ + statename##GET(&s); \ + s.a1 = states.shortname[0]; \ + s.a2 = states.shortname[1]; \ + s.a3 = states.shortname[2]; \ + statename##SET(&s); \ + } + STORE2(AirspeedState, air, CalibratedAirspeed, TrueAirspeed); +#define STORE1(statename, shortname, a1) \ + if (ISSET(states.updated, shortname##_UPDATED)) { \ + statename##Data s; \ + statename##GET(&s); \ + s.a1 = states.shortname[0]; \ + statename##SET(&s); \ + } + STORE1(BaroState, bar, Altitude); // clear alarms if everything is alright, then schedule callback execution after timeout @@ -251,8 +460,6 @@ static void settingsUpdatedCb(UAVObjEvent *ev) } RevoSettingsGet(&revoSettings); - - if } static void sensorUpdatedCb(UAVObjEvent *ev) @@ -286,7 +493,7 @@ static void sensorUpdatedCb(UAVObjEvent *ev) } if (ev->obj == AirspeedSensorHandle()) { - updatedSensors |= ias_UPDATED; + updatedSensors |= air_UPDATED; } DelayedCallbackDispatch(stateEstimationCallback); From bc6f06e9439818803785b5ed5722eb84866ba6ee Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 19 May 2013 21:44:33 +0200 Subject: [PATCH 08/61] first filter implemented and some bugs fixed --- .../modules/StateEstimation/stateestimation.c | 232 ++++++++---------- .../targets/boards/simposix/firmware/Makefile | 1 + .../boards/simposix/firmware/UAVObjects.inc | 1 + .../{barostate.ekf => barostate.xml} | 0 4 files changed, 111 insertions(+), 123 deletions(-) rename shared/uavobjectdefinition/{barostate.ekf => barostate.xml} (100%) diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 395116833..5b2171c55 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -29,48 +29,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -// TODO goes in header - - -typedef enum { - gyr_UPDATED = 1 << 0, - acc_UPDATED = 1 << 1, - mag_UPDATED = 1 << 2, - pos_UPDATED = 1 << 3, - vel_UPDATED = 1 << 4, - air_UPDATED = 1 << 5, - bar_UPDATED = 1 << 6, -} sensorUpdates; - -typedef struct { - float gyr[3]; - float acc[3]; - float mag[3]; - float pos[3]; - float vel[3]; - float air[2]; - float bar[1]; - sensorUpdates updated; -} stateEstimation; - -#define ISSET(bitfield, bit) ((bitfield) & (bit) ? 1 : 0) -#define UNSET(bitfield, bit) (bitfield) &= ~(bit) - - -typedef struct stateFilterStruct { - int32_t (*init)(void); - int32_t (*filter)(stateEstimation *state); -} stateFilter; - -struct filterQueueStruct; -typedef struct filterQueueStruct { - stateFilter *filter; - struct filterQueueStruct *next; -} filtereQueue; - -// - -#include +#include "inc/stateestimation.h" #include #include @@ -92,18 +51,24 @@ typedef struct filterQueueStruct { #include "homelocation.h" #include "flightstatus.h" -#include "CoordinateConversions.h" +// #include "CoordinateConversions.h" // Private constants #define STACK_SIZE_BYTES 2048 -#define CALLBACK_PRIORITY CALLBACK_PRIORITY_HIGH -#define TASK_PRIORITY CALLBACK_TASKPRIORITY_FLIGHTCONTROL +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR +#define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL #define TIMEOUT_MS 100 // Private types +struct filterQueueStruct; + +typedef struct filterQueueStruct { + stateFilter *filter; + struct filterQueueStruct *next; +} filterQueue; + // Private variables -static xTaskHandle attitudeTaskHandle; static DelayedCallbackInfo *stateEstimationCallback; static volatile RevoSettingsData revoSettings; @@ -114,54 +79,54 @@ static int32_t fusionAlgorithm = -1; static filterQueue *filterChain = NULL; // different filters available to state estimation -static stateFilter *magFilter; -static stateFilter *baroFilter; -static stateFilter *airFilter; -static stateFilter *stationaryFilter; -static stateFilter *cfFilter; -static stateFilter *cfmFilter; -static stateFilter *ekf13iFilter; -static stateFilter *ekf13Filter; -static stateFilter *ekf16Filter; -static stateFilter *ekf16iFilter; +static stateFilter magFilter; +static stateFilter baroFilter; +static stateFilter airFilter; +static stateFilter stationaryFilter; +static stateFilter cfFilter; +static stateFilter cfmFilter; +static stateFilter ekf13iFilter; +static stateFilter ekf13Filter; +static stateFilter ekf16Filter; +static stateFilter ekf16iFilter; // preconfigured filter chains selectable via revoSettings.FusionAlgorithm static const filterQueue *cfQueue = &(filterQueue) { - .filter = magFilter, + .filter = &magFilter, .next = &(filterQueue) { - .filter = baroFilter, + .filter = &baroFilter, .next = &(filterQueue) { - .filter = airFilter, + .filter = &airFilter, .next = &(filterQueue) { - .filter = cfFilter, + .filter = &cfFilter, .next = NULL, - } + }, } } }; static const filterQueue *cfmQueue = &(filterQueue) { - .filter = magFilter, + .filter = &magFilter, .next = &(filterQueue) { - .filter = baroFilter, + .filter = &baroFilter, .next = &(filterQueue) { - .filter = airFilter, + .filter = &airFilter, .next = &(filterQueue) { - .filter = cfmFilter, + .filter = &cfmFilter, .next = NULL, } } } }; static const filterQueue *ekf13iQueue = &(filterQueue) { - .filter = magFilter, + .filter = &magFilter, .next = &(filterQueue) { - .filter = baroFilter, + .filter = &baroFilter, .next = &(filterQueue) { - .filter = stationaryFilter, + .filter = &stationaryFilter, .next = &(filterQueue) { - .filter = airFilter, + .filter = &airFilter, .next = &(filterQueue) { - .filter = ekf13iFilter, + .filter = &ekf13iFilter, .next = NULL, } } @@ -169,28 +134,28 @@ static const filterQueue *ekf13iQueue = &(filterQueue) { } }; static const filterQueue *ekf13Queue = &(filterQueue) { - .filter = magFilter, + .filter = &magFilter, .next = &(filterQueue) { - .filter = baroFilter, + .filter = &baroFilter, .next = &(filterQueue) { - .filter = airFilter, + .filter = &airFilter, .next = &(filterQueue) { - .filter = ekf13Filter, + .filter = &ekf13Filter, .next = NULL, } } } }; static const filterQueue *ekf16iQueue = &(filterQueue) { - .filter = magFilter, + .filter = &magFilter, .next = &(filterQueue) { - .filter = baroFilter, + .filter = &baroFilter, .next = &(filterQueue) { - .filter = stationaryFilter, + .filter = &stationaryFilter, .next = &(filterQueue) { - .filter = airFilter, + .filter = &airFilter, .next = &(filterQueue) { - .filter = ekf16iFilter, + .filter = &ekf16iFilter, .next = NULL, } } @@ -198,13 +163,13 @@ static const filterQueue *ekf16iQueue = &(filterQueue) { } }; static const filterQueue *ekf16Queue = &(filterQueue) { - .filter = magFilter, + .filter = &magFilter, .next = &(filterQueue) { - .filter = baroFilter, + .filter = &baroFilter, .next = &(filterQueue) { - .filter = airFilter, + .filter = &airFilter, .next = &(filterQueue) { - .filter = ekf16Filter, + .filter = &ekf16Filter, .next = NULL, } } @@ -217,6 +182,7 @@ static void settingsUpdatedCb(UAVObjEvent *objEv); static void sensorUpdatedCb(UAVObjEvent *objEv); static void StateEstimationCb(void); static void getNED(GPSPositionData *gpsPosition, float *NED); +static float sane(float value); /** @@ -255,16 +221,16 @@ int32_t StateEstimationStart(void) settingsUpdatedCb(NULL); // Initialize Filters - magFilter = filterMagInitialize(); - baroFilter = filterBaroInitialize(); - airFilter = filterAirInitialize(); - stationaryFilter = filterStationaryInitialize(); - cfFilter = filterCFInitialize(); - cfmFilter = filterCFMInitialize(); - ekf13iFilter = filterEKF13iInitialize(); - ekf13Filter = filterEKF13Initialize(); - ekf16Filter = filterEKF16Initialize(); - ekf16iFilter = filterEKF16iInitialize(); + filterMagInitialize(&magFilter); + filterBaroInitialize(&baroFilter); + filterAirInitialize(&airFilter); + filterStationaryInitialize(&stationaryFilter); + filterCFInitialize(&cfFilter); + filterCFMInitialize(&cfmFilter); + filterEKF13iInitialize(&ekf13iFilter); + filterEKF13Initialize(&ekf13Filter); + filterEKF16Initialize(&ekf16Filter); + filterEKF16iInitialize(&ekf16iFilter); return 0; } @@ -280,57 +246,57 @@ static void StateEstimationCb(void) uint8_t alarm = 0; // set alarm to warning if called through timeout - if (updatedSensor == 0) { + if (updatedSensors == 0) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); alarm = 1; } // check if a new filter chain should be initialized - if (fusionAlgorithm != revoSettings.fusionAlgorithm) { + if (fusionAlgorithm != revoSettings.FusionAlgorithm) { FlightStatusData fs; FlightStatusGet(&fs); if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == -1) { - filterQueue *newFilterChain; - switch (fusionAlgorithm) { - case REVOSETTINGS.FUSIONALGORITHM_COMPLEMENTARY: + const filterQueue *newFilterChain; + switch (revoSettings.FusionAlgorithm) { + case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: newFilterChain = cfQueue; break; - case REVOSETTINGS.FUSIONALGORITHM_COMPLEMENTARYMAG: + case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG: newFilterChain = cfmQueue; break; - case REVOSETTINGS.FUSIONALGORITHM_INS13INDOOR: + case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR: newFilterChain = ekf13iQueue; break; - case REVOSETTINGS.FUSIONALGORITHM_INS13OUTDOOR: + case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR: newFilterChain = ekf13Queue; break; - case REVOSETTINGS.FUSIONALGORITHM_INS16INDOOR: + case REVOSETTINGS_FUSIONALGORITHM_INS16INDOOR: newFilterChain = ekf16iQueue; break; - case REVOSETTINGS.FUSIONALGORITHM_INS16OUTDOOR: + case REVOSETTINGS_FUSIONALGORITHM_INS16OUTDOOR: newFilterChain = ekf16Queue; break; default: newFilterChain = NULL; } // initialize filters in chain - filterQueue *current = newFilterChain; + filterQueue *current = (filterQueue *)newFilterChain; bool error = 0; while (current != NULL) { - int32_t result = current.init(); + int32_t result = current->filter->init(); if (result != 0) { error = 1; break; } - current = current.next; + current = current->next; } if (error) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); return; } else { // set new fusion algortithm - filterChain = newFilterChain; - fustionAlgorithm = revoSettings.fusionAlgorithm; + filterChain = (filterQueue *)newFilterChain; + fusionAlgorithm = revoSettings.FusionAlgorithm; } } } @@ -344,7 +310,7 @@ static void StateEstimationCb(void) #define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \ if (ISSET(states.updated, shortname##_UPDATED)) { \ sensorname##Data s; \ - sensorname##GET(&s); \ + sensorname##Get(&s); \ if (sane(s.a1) && sane(s.a2) && sane(s.a3)) { \ states.shortname[0] = s.a1; \ states.shortname[1] = s.a2; \ @@ -361,7 +327,7 @@ static void StateEstimationCb(void) #define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \ if (ISSET(states.updated, shortname##_UPDATED)) { \ sensorname##Data s; \ - sensorname##GET(&s); \ + sensorname##Get(&s); \ if (sane(s.a1) && EXTRACHECK) { \ states.shortname[0] = s.a1; \ } \ @@ -386,14 +352,14 @@ static void StateEstimationCb(void) // at this point sensor state is stored in "states" with some rudimentary filtering applied // apply all filters in the current filter chain - filterQueue *current = filterChain; + filterQueue *current = (filterQueue *)filterChain; bool error = 0; while (current != NULL) { - int32_t result = current.filter(&states); + int32_t result = current->filter->filter(&states); if (result != 0) { error = 1; } - current = current.next; + current = current->next; } if (error) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); @@ -403,11 +369,11 @@ static void StateEstimationCb(void) #define STORE3(statename, shortname, a1, a2, a3) \ if (ISSET(states.updated, shortname##_UPDATED)) { \ statename##Data s; \ - statename##GET(&s); \ + statename##Get(&s); \ s.a1 = states.shortname[0]; \ s.a2 = states.shortname[1]; \ s.a3 = states.shortname[2]; \ - statename##SET(&s); \ + statename##Set(&s); \ } STORE3(GyroState, gyr, x, y, z); STORE3(AccelState, acc, x, y, z); @@ -417,19 +383,18 @@ static void StateEstimationCb(void) #define STORE2(statename, shortname, a1, a2) \ if (ISSET(states.updated, shortname##_UPDATED)) { \ statename##Data s; \ - statename##GET(&s); \ + statename##Get(&s); \ s.a1 = states.shortname[0]; \ s.a2 = states.shortname[1]; \ - s.a3 = states.shortname[2]; \ - statename##SET(&s); \ + statename##Set(&s); \ } STORE2(AirspeedState, air, CalibratedAirspeed, TrueAirspeed); #define STORE1(statename, shortname, a1) \ if (ISSET(states.updated, shortname##_UPDATED)) { \ statename##Data s; \ - statename##GET(&s); \ + statename##Get(&s); \ s.a1 = states.shortname[0]; \ - statename##SET(&s); \ + statename##Set(&s); \ } STORE1(BaroState, bar, Altitude); @@ -438,12 +403,16 @@ static void StateEstimationCb(void) if (!alarm) { AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); } - DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, UPDATEMODE_SOONER); + DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER); } + +/** + * Callback for eventdispatcher when HomeLocation or RevoSettings has been updated + */ static void settingsUpdatedCb(UAVObjEvent *ev) { - HomeLocationGet(&homeLocation); + HomeLocationGet((HomeLocationData *)&homeLocation); if (sane(homeLocation.Latitude) && sane(homeLocation.Longitude) && sane(homeLocation.Altitude) && sane(homeLocation.Be[0]) && sane(homeLocation.Be[1]) && sane(homeLocation.Be[2])) { // Compute matrix to convert deltaLLA to NED @@ -459,9 +428,13 @@ static void settingsUpdatedCb(UAVObjEvent *ev) // needed for long range flights where the reference coordinate is adjusted in flight } - RevoSettingsGet(&revoSettings); + RevoSettingsGet((RevoSettingsData *)&revoSettings); } +/** + * Callback for eventdispatcher when any sensor UAVObject has been updated + * updates the list of "recently updated UAVObjects" and dispatches the state estimator callback + */ static void sensorUpdatedCb(UAVObjEvent *ev) { if (!ev) { @@ -519,6 +492,19 @@ static void getNED(GPSPositionData *gpsPosition, float *NED) NED[2] = LLA2NEDM[2] * dL[2]; } +/** + * @brief sanity check for float values + * @note makes sure a float value is safe for further processing, ie not NAN and not INF + * @param[in] float value + * @returns true for safe and false for unsafe + */ +static float sane(float value) +{ + if (isnan(value) || isinf(value)) { + return false; + } + return true; +} /** * @} diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index f78bbce23..ae7a6a03c 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -38,6 +38,7 @@ MODULES += VtolPathFollower MODULES += CameraStab MODULES += Telemetry MODULES += FirmwareIAP +MODULES += StateEstimation #MODULES += OveroSync # Paths diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index c5a401d52..324e8f20e 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -38,6 +38,7 @@ UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magnetosensor UAVOBJSRCFILENAMES += magnetostate UAVOBJSRCFILENAMES += barosensor +UAVOBJSRCFILENAMES += barostate UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedstate diff --git a/shared/uavobjectdefinition/barostate.ekf b/shared/uavobjectdefinition/barostate.xml similarity index 100% rename from shared/uavobjectdefinition/barostate.ekf rename to shared/uavobjectdefinition/barostate.xml From bf80b3f103f7c6c1320cba12f18cc0e3d9000c32 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 19 May 2013 22:40:50 +0200 Subject: [PATCH 09/61] readded missing header --- .../StateEstimation/inc/stateestimation.h | 77 +++++++++++++++++++ .../boards/simposix/firmware/UAVObjects.inc | 3 +- 2 files changed, 79 insertions(+), 1 deletion(-) create mode 100644 flight/modules/StateEstimation/inc/stateestimation.h diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h new file mode 100644 index 000000000..dd6a3a157 --- /dev/null +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -0,0 +1,77 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StateSetimation Module + * @{ + * + * @file stateestimation.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * @brief Acquires sensor data and fuses it into attitude estimate for CC + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef STATEESTIMATION_H +#define STATEESTIMATION_H + +#include + +typedef enum { + gyr_UPDATED = 1 << 0, + acc_UPDATED = 1 << 1, + mag_UPDATED = 1 << 2, + pos_UPDATED = 1 << 3, + vel_UPDATED = 1 << 4, + air_UPDATED = 1 << 5, + bar_UPDATED = 1 << 6, +} sensorUpdates; + +typedef struct { + float gyr[3]; + float acc[3]; + float mag[3]; + float pos[3]; + float vel[3]; + float air[2]; + float bar[1]; + sensorUpdates updated; +} stateEstimation; + +#define ISSET(bitfield, bit) ((bitfield) & (bit) ? 1 : 0) +#define UNSET(bitfield, bit) (bitfield) &= ~(bit) + + +typedef struct stateFilterStruct { + int32_t (*init)(void); + int32_t (*filter)(stateEstimation *state); +} stateFilter; + + +void filterMagInitialize(stateFilter *handle); +void filterBaroInitialize(stateFilter *handle); +void filterAirInitialize(stateFilter *handle); +void filterStationaryInitialize(stateFilter *handle); +void filterCFInitialize(stateFilter *handle); +void filterCFMInitialize(stateFilter *handle); +void filterEKF13iInitialize(stateFilter *handle); +void filterEKF13Initialize(stateFilter *handle); +void filterEKF16iInitialize(stateFilter *handle); +void filterEKF16Initialize(stateFilter *handle); + +#endif // STATEESTIMATION_H diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 324e8f20e..86fc385b2 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -93,7 +93,8 @@ UAVOBJSRCFILENAMES += hwsettings UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings -UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += revosettings UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) From 46e7d2121faa46f83b6648b0337ba399210e6ffa Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 19 May 2013 22:49:11 +0200 Subject: [PATCH 10/61] fake filter for stationary position data added --- .../StateEstimation/filterstationary.c | 75 +++++++++++++++++++ 1 file changed, 75 insertions(+) create mode 100644 flight/modules/StateEstimation/filterstationary.c diff --git a/flight/modules/StateEstimation/filterstationary.c b/flight/modules/StateEstimation/filterstationary.c new file mode 100644 index 000000000..b33e3b9f7 --- /dev/null +++ b/flight/modules/StateEstimation/filterstationary.c @@ -0,0 +1,75 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup State Estimation + * @brief Acquires sensor data and computes state estimate + * @{ + * + * @file filterstationary.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief Provides "fake" stationary state data for indoor mode + * + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "inc/stateestimation.h" + +// Private constants + +// Private types + +// Private variables + +// Private functions + +static int32_t init(void); +static int32_t filter(stateEstimation *state); + + +void filterStationaryInitialize(stateFilter *handle) +{ + handle->init = &init; + handle->filter = &filter; +} + +static int32_t init(void) +{ + return 0; +} + +static int32_t filter(stateEstimation *state) +{ + state->pos[0] = 0.0f; + state->pos[1] = 0.0f; + state->pos[2] = 0.0f; + state->updated |= pos_UPDATED; + + state->vel[0] = 0.0f; + state->vel[1] = 0.0f; + state->vel[2] = 0.0f; + state->updated |= vel_UPDATED; + + return 0; +} + +/** + * @} + * @} + */ From 1236bf3ed9470f945e20d3b14783f33ed6285f17 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 10:33:02 +0200 Subject: [PATCH 11/61] renamed Magneto to Mag --- flight/modules/Attitude/revolution/attitude.c | 42 +++++++++---------- flight/modules/Extensions/MagBaro/magbaro.c | 8 ++-- flight/modules/Sensors/sensors.c | 14 +++---- flight/modules/Sensors/simulated/sensors.c | 22 +++++----- flight/targets/boards/osd/firmware/Makefile | 2 +- .../boards/revolution/firmware/UAVObjects.inc | 4 +- .../revolution/firmware/pios_board_sim.c | 4 +- .../boards/revoproto/firmware/UAVObjects.inc | 4 +- .../revoproto/firmware/pios_board_sim.c | 4 +- .../boards/simposix/firmware/UAVObjects.inc | 4 +- .../default_configurations/Developer.xml | 6 +-- .../default_configurations/OpenPilotGCS.xml | 6 +-- .../OpenPilotGCS_wide.xml | 6 +-- .../src/plugins/config/configrevowidget.cpp | 34 +++++++-------- .../src/plugins/config/configrevowidget.h | 2 +- .../src/plugins/uavobjects/uavobjects.pro | 8 ++-- .../{magnetosensor.xml => magsensor.xml} | 2 +- .../{magnetostate.xml => magstate.xml} | 2 +- 18 files changed, 87 insertions(+), 87 deletions(-) rename shared/uavobjectdefinition/{magnetosensor.xml => magsensor.xml} (87%) rename shared/uavobjectdefinition/{magnetostate.xml => magstate.xml} (87%) diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index aa72bab88..2a3b38d3d 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -64,8 +64,8 @@ #include "gyrostate.h" #include "gyrosensor.h" #include "homelocation.h" -#include "magnetosensor.h" -#include "magnetostate.h" +#include "magsensor.h" +#include "magstate.h" #include "positionstate.h" #include "ekfconfiguration.h" #include "ekfstatevariance.h" @@ -125,7 +125,7 @@ static void settingsUpdatedCb(UAVObjEvent *objEv); static int32_t getNED(GPSPositionData *gpsPosition, float *NED); -static void magOffsetEstimation(MagnetoSensorData *mag); +static void magOffsetEstimation(MagSensorData *mag); // check for invalid values static inline bool invalid(float data) @@ -167,8 +167,8 @@ int32_t AttitudeInitialize(void) GyroStateInitialize(); AccelSensorInitialize(); AccelStateInitialize(); - MagnetoSensorInitialize(); - MagnetoStateInitialize(); + MagSensorInitialize(); + MagStateInitialize(); AirspeedSensorInitialize(); AirspeedStateInitialize(); BaroSensorInitialize(); @@ -230,7 +230,7 @@ int32_t AttitudeStart(void) GyroSensorConnectQueue(gyroQueue); AccelSensorConnectQueue(accelQueue); - MagnetoSensorConnectQueue(magQueue); + MagSensorConnectQueue(magQueue); AirspeedSensorConnectQueue(airspeedQueue); BaroSensorConnectQueue(baroQueue); GPSPositionConnectQueue(gpsQueue); @@ -335,10 +335,10 @@ static int32_t updateAttitudeComplementary(bool first_run) if (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE) { return -1; } - MagnetoSensorData magData; - MagnetoSensorGet(&magData); + MagSensorData magData; + MagSensorGet(&magData); #else - MagnetoSensorData magData; + MagSensorData magData; magData.x = 100.0f; magData.y = 0.0f; magData.z = 0.0f; @@ -434,20 +434,20 @@ static int32_t updateAttitudeComplementary(bool first_run) // Rotate gravity to body frame and cross with accels float brot[3]; float Rbe[3][3]; - MagnetoSensorData mag; + MagSensorData mag; Quaternion2R(q, Rbe); - MagnetoSensorGet(&mag); + MagSensorGet(&mag); // TODO: separate filter! if (revoCalibration.MagBiasNullingRate > 0) { magOffsetEstimation(&mag); } - MagnetoStateData mags; + MagStateData mags; mags.x = mag.x; mags.y = mag.y; mags.z = mag.z; - MagnetoStateSet(&mags); + MagStateSet(&mags); // If the mag is producing bad data don't use it (normally bad calibration) if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) { @@ -612,7 +612,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) UAVObjEvent ev; GyroSensorData gyroSensorData; AccelSensorData accelSensorData; - MagnetoStateData magData; + MagStateData magData; AirspeedSensorData airspeedData; BaroSensorData baroData; GPSPositionData gpsData; @@ -694,17 +694,17 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) AccelSensorGet(&accelSensorData); // TODO: separate filter! if (mag_updated) { - MagnetoSensorData mags; - MagnetoSensorGet(&mags); + MagSensorData mags; + MagSensorGet(&mags); if (revoCalibration.MagBiasNullingRate > 0) { magOffsetEstimation(&mags); } magData.x = mags.x; magData.y = mags.y; magData.z = mags.z; - MagnetoStateSet(&magData); + MagStateSet(&magData); } - MagnetoStateGet(&magData); + MagStateGet(&magData); BaroSensorGet(&baroData); AirspeedSensorGet(&airspeedData); @@ -860,7 +860,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) } // xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); - // MagnetoSensorGet(&magData); + // MagSensorGet(&magData); AttitudeStateData attitudeState; AttitudeStateGet(&attitudeState); @@ -1162,10 +1162,10 @@ static void settingsUpdatedCb(UAVObjEvent *ev) /** * Perform an update of the @ref MagBias based on - * Magnetometer Offset Cancellation: Theory and Implementation, + * Magmeter Offset Cancellation: Theory and Implementation, * revisited William Premerlani, October 14, 2011 */ -static void magOffsetEstimation(MagnetoSensorData *mag) +static void magOffsetEstimation(MagSensorData *mag) { #if 0 // Constants, to possibly go into a UAVO diff --git a/flight/modules/Extensions/MagBaro/magbaro.c b/flight/modules/Extensions/MagBaro/magbaro.c index 3bff3cbca..927482f3f 100644 --- a/flight/modules/Extensions/MagBaro/magbaro.c +++ b/flight/modules/Extensions/MagBaro/magbaro.c @@ -41,7 +41,7 @@ #include "hwsettings.h" #include "magbaro.h" #include "barosensor.h" // object that will be updated by the module -#include "magnetosensor.h" +#include "magsensor.h" #include "taskinfo.h" // Private constants @@ -109,7 +109,7 @@ int32_t MagBaroInitialize() if (magbaroEnabled) { #if defined(PIOS_INCLUDE_HMC5883) - MagnetoSensorInitialize(); + MagSensorInitialize(); #endif #if defined(PIOS_INCLUDE_BMP085) @@ -149,7 +149,7 @@ static void magbaroTask(__attribute__((unused)) void *parameters) #endif #if defined(PIOS_INCLUDE_HMC5883) - MagnetoSensorData mag; + MagSensorData mag; PIOS_HMC5883_Init(&pios_hmc5883_cfg); uint32_t mag_update_time = PIOS_DELAY_GetRaw(); #endif @@ -207,7 +207,7 @@ static void magbaroTask(__attribute__((unused)) void *parameters) mag.x = mags[0]; mag.y = mags[1]; mag.z = mags[2]; - MagnetoSensorSet(&mag); + MagSensorSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); } #endif diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index 5a1041c93..b95d77bdf 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -4,7 +4,7 @@ * @{ * @addtogroup Sensors * @brief Acquires sensor data - * Specifically updates the the @ref GyroSensor, @ref AccelSensor, and @ref MagnetoSensor objects + * Specifically updates the the @ref GyroSensor, @ref AccelSensor, and @ref MagSensor objects * @{ * * @file sensors.c @@ -32,7 +32,7 @@ /** * Input objects: None, takes sensor data via pios - * Output objects: @ref GyroSensor @ref AccelSensor @ref MagnetoSensor + * Output objects: @ref GyroSensor @ref AccelSensor @ref MagSensor * * The module executes in its own thread. * @@ -49,7 +49,7 @@ #include #include -#include +#include #include #include #include @@ -73,7 +73,7 @@ // Private functions static void SensorsTask(void *parameters); static void settingsUpdatedCb(UAVObjEvent *objEv); -// static void magOffsetEstimation(MagnetoSensorData *mag); +// static void magOffsetEstimation(MagSensorData *mag); // Private variables static xTaskHandle sensorsTaskHandle; @@ -110,7 +110,7 @@ int32_t SensorsInitialize(void) { GyroSensorInitialize(); AccelSensorInitialize(); - MagnetoSensorInitialize(); + MagSensorInitialize(); RevoCalibrationInitialize(); AttitudeSettingsInitialize(); @@ -383,7 +383,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters) // and make it average zero (weakly) #if defined(PIOS_INCLUDE_HMC5883) - MagnetoSensorData mag; + MagSensorData mag; if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { int16_t values[3]; PIOS_HMC5883_ReadMag(values); @@ -402,7 +402,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters) mag.z = mags[2]; } - MagnetoSensorSet(&mag); + MagSensorSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); } #endif /* if defined(PIOS_INCLUDE_HMC5883) */ diff --git a/flight/modules/Sensors/simulated/sensors.c b/flight/modules/Sensors/simulated/sensors.c index fdd58abf0..b236f6cd0 100644 --- a/flight/modules/Sensors/simulated/sensors.c +++ b/flight/modules/Sensors/simulated/sensors.c @@ -4,7 +4,7 @@ * @{ * @addtogroup Sensors * @brief Acquires sensor data - * Specifically updates the the @ref GyroSensor, @ref AccelSensor, and @ref MagnetoSensor objects + * Specifically updates the the @ref GyroSensor, @ref AccelSensor, and @ref MagSensor objects * @{ * * @file sensors.c @@ -32,7 +32,7 @@ /** * Input objects: None, takes sensor data via pios - * Output objects: @ref GyroSensor @ref AccelSensor @ref MagnetoSensor + * Output objects: @ref GyroSensor @ref AccelSensor @ref MagSensor * * The module executes in its own thread. * @@ -113,7 +113,7 @@ int32_t SensorsInitialize(void) // GyrosBiasInitialize(); GPSPositionInitialize(); GPSVelocityInitialize(); - MagnetoSensorInitialize(); + MagSensorInitialize(); MagBiasInitialize(); RevoCalibrationInitialize(); @@ -252,11 +252,11 @@ static void simulateConstant() // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) - MagnetoSensorData mag; + MagSensorData mag; mag.x = 400; mag.y = 0; mag.z = 800; - MagnetoSensorSet(&mag); + MagSensorSet(&mag); } static void simulateModelAgnostic() @@ -314,11 +314,11 @@ static void simulateModelAgnostic() // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) - MagnetoSensorData mag; + MagSensorData mag; mag.x = 400; mag.y = 0; mag.z = 800; - MagnetoSensorSet(&mag); + MagSensorSet(&mag); } float thrustToDegs = 50; @@ -528,12 +528,12 @@ static void simulateModelQuadcopter() // Update mag periodically static uint32_t last_mag_time = 0; if (PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { - MagnetoSensorData mag; + MagSensorData mag; mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; - MagnetoSensorSet(&mag); + MagSensorSet(&mag); last_mag_time = PIOS_DELAY_GetRaw(); } @@ -811,11 +811,11 @@ static void simulateModelAirplane() // Update mag periodically static uint32_t last_mag_time = 0; if (PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { - MagnetoSensorData mag; + MagSensorData mag; mag.x = 100 + homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; mag.y = 100 + homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; mag.z = 100 + homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; - MagnetoSensorSet(&mag); + MagSensorSet(&mag); last_mag_time = PIOS_DELAY_GetRaw(); } diff --git a/flight/targets/boards/osd/firmware/Makefile b/flight/targets/boards/osd/firmware/Makefile index 524a4baad..7c88af224 100644 --- a/flight/targets/boards/osd/firmware/Makefile +++ b/flight/targets/boards/osd/firmware/Makefile @@ -93,7 +93,7 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/gpstime.c SRC += $(OPUAVSYNTHDIR)/osdsettings.c SRC += $(OPUAVSYNTHDIR)/barosensor.c - SRC += $(OPUAVSYNTHDIR)/magnetosensor.c + SRC += $(OPUAVSYNTHDIR)/magsensor.c else ## Test Code SRC += $(OPTESTS)/test_common.c diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 34f387250..1ee34b95d 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -30,8 +30,8 @@ UAVOBJSRCFILENAMES += gyrostate UAVOBJSRCFILENAMES += gyrosensor UAVOBJSRCFILENAMES += accelstate UAVOBJSRCFILENAMES += accelsensor -UAVOBJSRCFILENAMES += magnetosensor -UAVOBJSRCFILENAMES += magnetostate +UAVOBJSRCFILENAMES += magsensor +UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings diff --git a/flight/targets/boards/revolution/firmware/pios_board_sim.c b/flight/targets/boards/revolution/firmware/pios_board_sim.c index ca2c3de98..9cf758199 100644 --- a/flight/targets/boards/revolution/firmware/pios_board_sim.c +++ b/flight/targets/boards/revolution/firmware/pios_board_sim.c @@ -35,7 +35,7 @@ #include #include #include -#include +#include #include void Stack_Change() {} @@ -138,7 +138,7 @@ void PIOS_Board_Init(void) AccelSensorInitialize(); BaroSensorInitialize(); - MagnetoSensorInitialize(); + MagSensorInitialize(); GPSPositionInitialize(); GyroSensorInitialize(); diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 02169d2b8..5742838ba 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -35,8 +35,8 @@ UAVOBJSRCFILENAMES += gyrostate UAVOBJSRCFILENAMES += gyrosensor UAVOBJSRCFILENAMES += accelstate UAVOBJSRCFILENAMES += accelsensor -UAVOBJSRCFILENAMES += magnetostate -UAVOBJSRCFILENAMES += magnetosensor +UAVOBJSRCFILENAMES += magstate +UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings diff --git a/flight/targets/boards/revoproto/firmware/pios_board_sim.c b/flight/targets/boards/revoproto/firmware/pios_board_sim.c index 0b026b530..27d998798 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board_sim.c +++ b/flight/targets/boards/revoproto/firmware/pios_board_sim.c @@ -35,7 +35,7 @@ #include #include #include -#include +#include #include void Stack_Change() {} @@ -138,7 +138,7 @@ void PIOS_Board_Init(void) AccelSensorInitialize(); BaroSensorInitialize(); - MagnetoSensorInitialize(); + MagSensorInitialize(); GPSPositionInitialize(); GyroStatInitialize(); GyroSensorInitialize(); diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index c5a401d52..9e9661152 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -35,8 +35,8 @@ UAVOBJSRCFILENAMES += gyrostate UAVOBJSRCFILENAMES += gyrosensor UAVOBJSRCFILENAMES += accelstate UAVOBJSRCFILENAMES += accelsensor -UAVOBJSRCFILENAMES += magnetosensor -UAVOBJSRCFILENAMES += magnetostate +UAVOBJSRCFILENAMES += magsensor +UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml index 5b1282f5e..233061c29 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml @@ -2233,7 +2233,7 @@ 4294901760 None x - MagnetoState + MagState 6.92916505601691e-310 1 3.86203233966055e-312 @@ -2243,7 +2243,7 @@ 4283782655 None y - MagnetoState + MagState 1.72723371101889e-77 1 -1 @@ -2253,7 +2253,7 @@ 4283804160 None z - MagnetoState + MagState 1.72723371102028e-77 1 7.21478569792807e-313 diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml index f00c303fe..4ad20f5da 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml @@ -2264,7 +2264,7 @@ 4294901760 None x - MagnetoState + MagState 0 1 0 @@ -2274,7 +2274,7 @@ 4283782655 None y - MagnetoState + MagState 0 1 0 @@ -2284,7 +2284,7 @@ 4283804160 None z - MagnetoState + MagState 0 1 0 diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml index 916b88152..4273cf797 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml @@ -2242,7 +2242,7 @@ 4294901760 None x - MagnetoState + MagState 0 1 0 @@ -2252,7 +2252,7 @@ 4283782655 None y - MagnetoState + MagState 0 1 0 @@ -2262,7 +2262,7 @@ 4283804160 None z - MagnetoState + MagState 0 1 0 diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index c3dcbba6a..9d22339cc 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -46,7 +46,7 @@ #include #include #include -#include +#include #define GRAVITY 9.81f #include "assertions.h" @@ -585,10 +585,10 @@ void ConfigRevoWidget::doStartSixPointCalibration() #endif /* Need to get as many mag updates as possible */ - MagnetoState *mag = MagnetoState::GetInstance(getObjectManager()); + MagState *mag = MagState::GetInstance(getObjectManager()); Q_ASSERT(mag); - initialMagnetoStateMdata = mag->getMetadata(); - mdata = initialMagnetoStateMdata; + initialMagStateMdata = mag->getMetadata(); + mdata = initialMagStateMdata; UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; mag->setMetadata(mdata); @@ -624,7 +624,7 @@ void ConfigRevoWidget::savePositionData() AccelState *accelState = AccelState::GetInstance(getObjectManager()); Q_ASSERT(accelState); - MagnetoState *mag = MagnetoState::GetInstance(getObjectManager()); + MagState *mag = MagState::GetInstance(getObjectManager()); Q_ASSERT(mag); connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *))); @@ -653,10 +653,10 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj) accel_accum_y.append(accelStateData.y); accel_accum_z.append(accelStateData.z); #endif - } else if (obj->getObjID() == MagnetoState::OBJID) { - MagnetoState *mag = MagnetoState::GetInstance(getObjectManager()); + } else if (obj->getObjID() == MagState::OBJID) { + MagState *mag = MagState::GetInstance(getObjectManager()); Q_ASSERT(mag); - MagnetoState::DataFields magData = mag->getData(); + MagState::DataFields magData = mag->getData(); mag_accum_x.append(magData.x); mag_accum_y.append(magData.y); mag_accum_z.append(magData.z); @@ -685,7 +685,7 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj) #endif // Store the mean for this position for the mag - MagnetoState *mag = MagnetoState::GetInstance(getObjectManager()); + MagState *mag = MagState::GetInstance(getObjectManager()); Q_ASSERT(mag); disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *))); mag_data_x[position] = listMean(mag_accum_x); @@ -722,7 +722,7 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj) #ifdef SIX_POINT_CAL_ACCEL accelState->setMetadata(initialAccelStateMdata); #endif - mag->setMetadata(initialMagnetoStateMdata); + mag->setMetadata(initialMagStateMdata); // Recall saved board rotation recallBoardRotation(); @@ -928,7 +928,7 @@ void ConfigRevoWidget::doStartNoiseMeasurement() Q_ASSERT(accelState); GyroState *gyroState = GyroState::GetInstance(getObjectManager()); Q_ASSERT(gyroState); - MagnetoState *mag = MagnetoState::GetInstance(getObjectManager()); + MagState *mag = MagState::GetInstance(getObjectManager()); Q_ASSERT(mag); UAVObject::Metadata mdata; @@ -945,8 +945,8 @@ void ConfigRevoWidget::doStartNoiseMeasurement() mdata.flightTelemetryUpdatePeriod = 100; gyroState->setMetadata(mdata); - initialMagnetoStateMdata = mag->getMetadata(); - mdata = initialMagnetoStateMdata; + initialMagStateMdata = mag->getMetadata(); + mdata = initialMagStateMdata; UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; mag->setMetadata(mdata); @@ -990,11 +990,11 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject *obj) accel_accum_z.append(accelStateData.z); break; } - case MagnetoState::OBJID: + case MagState::OBJID: { - MagnetoState *mags = MagnetoState::GetInstance(getObjectManager()); + MagState *mags = MagState::GetInstance(getObjectManager()); Q_ASSERT(mags); - MagnetoState::DataFields magData = mags->getData(); + MagState::DataFields magData = mags->getData(); mag_accum_x.append(magData.x); mag_accum_y.append(magData.y); mag_accum_z.append(magData.z); @@ -1017,7 +1017,7 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject *obj) gyro_accum_x.length() >= NOISE_SAMPLES && accel_accum_x.length() >= NOISE_SAMPLES) { // No need to for more updates - MagnetoState *mags = MagnetoState::GetInstance(getObjectManager()); + MagState *mags = MagState::GetInstance(getObjectManager()); AccelState *accelState = AccelState::GetInstance(getObjectManager()); GyroState *gyroState = GyroState::GetInstance(getObjectManager()); disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *))); diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.h b/ground/openpilotgcs/src/plugins/config/configrevowidget.h index 2e3789025..6e26abf91 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.h +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.h @@ -92,7 +92,7 @@ private: UAVObject::Metadata initialAccelStateMdata; UAVObject::Metadata initialGyroStateMdata; - UAVObject::Metadata initialMagnetoStateMdata; + UAVObject::Metadata initialMagStateMdata; UAVObject::Metadata initialBaroSensorMdata; float initialMagCorrectionRate; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 745b0b935..4ff9d4050 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -42,8 +42,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/gyrosensor.h \ $$UAVOBJECT_SYNTHETICS/accelsensor.h \ $$UAVOBJECT_SYNTHETICS/accelstate.h \ - $$UAVOBJECT_SYNTHETICS/magnetosensor.h \ - $$UAVOBJECT_SYNTHETICS/magnetostate.h \ + $$UAVOBJECT_SYNTHETICS/magsensor.h \ + $$UAVOBJECT_SYNTHETICS/magstate.h \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.h \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.h \ $$UAVOBJECT_SYNTHETICS/systemstats.h \ @@ -126,8 +126,8 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/accelstate.cpp \ $$UAVOBJECT_SYNTHETICS/gyrostate.cpp \ $$UAVOBJECT_SYNTHETICS/gyrosensor.cpp \ - $$UAVOBJECT_SYNTHETICS/magnetosensor.cpp \ - $$UAVOBJECT_SYNTHETICS/magnetostate.cpp \ + $$UAVOBJECT_SYNTHETICS/magsensor.cpp \ + $$UAVOBJECT_SYNTHETICS/magstate.cpp \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.cpp \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.cpp \ $$UAVOBJECT_SYNTHETICS/systemstats.cpp \ diff --git a/shared/uavobjectdefinition/magnetosensor.xml b/shared/uavobjectdefinition/magsensor.xml similarity index 87% rename from shared/uavobjectdefinition/magnetosensor.xml rename to shared/uavobjectdefinition/magsensor.xml index 5974636f6..1681713b6 100644 --- a/shared/uavobjectdefinition/magnetosensor.xml +++ b/shared/uavobjectdefinition/magsensor.xml @@ -1,5 +1,5 @@ - + The mag data. diff --git a/shared/uavobjectdefinition/magnetostate.xml b/shared/uavobjectdefinition/magstate.xml similarity index 87% rename from shared/uavobjectdefinition/magnetostate.xml rename to shared/uavobjectdefinition/magstate.xml index 546b52d69..85b2bb458 100644 --- a/shared/uavobjectdefinition/magnetostate.xml +++ b/shared/uavobjectdefinition/magstate.xml @@ -1,5 +1,5 @@ - + The mag data. From 34294d26357091ecb42f8860e2c929647a74d11d Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 10:35:54 +0200 Subject: [PATCH 12/61] Magneto to Mag fix --- flight/modules/StateEstimation/stateestimation.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 5b2171c55..1ecdf7e1b 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -33,7 +33,7 @@ #include #include -#include +#include #include #include #include @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include #include @@ -198,7 +198,7 @@ int32_t StateEstimationInitialize(void) GyroSensorConnectCallback(&sensorUpdatedCb); AccelSensorConnectCallback(&sensorUpdatedCb); - MagnetoSensorConnectCallback(&sensorUpdatedCb); + MagSensorConnectCallback(&sensorUpdatedCb); BaroSensorConnectCallback(&sensorUpdatedCb); AirspeedSensorConnectCallback(&sensorUpdatedCb); GPSPositionConnectCallback(&sensorUpdatedCb); @@ -322,7 +322,7 @@ static void StateEstimationCb(void) } SANITYCHECK3(GyroSensor, gyr, x, y, z); SANITYCHECK3(AccelSensor, acc, x, y, z); - SANITYCHECK3(MagnetoSensor, mag, x, y, z); + SANITYCHECK3(MagSensor, mag, x, y, z); SANITYCHECK3(GPSVelocity, vel, North, East, Down); #define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \ if (ISSET(states.updated, shortname##_UPDATED)) { \ @@ -377,7 +377,7 @@ static void StateEstimationCb(void) } STORE3(GyroState, gyr, x, y, z); STORE3(AccelState, acc, x, y, z); - STORE3(MagnetoState, mag, x, y, z); + STORE3(MagState, mag, x, y, z); STORE3(PositionState, pos, North, East, Down); STORE3(VelocityState, vel, North, East, Down); #define STORE2(statename, shortname, a1, a2) \ @@ -449,7 +449,7 @@ static void sensorUpdatedCb(UAVObjEvent *ev) updatedSensors |= acc_UPDATED; } - if (ev->obj == MagnetoSensorHandle()) { + if (ev->obj == MagSensorHandle()) { updatedSensors |= mag_UPDATED; } From 1bd9cdafab63be88d17230ab3bacb4ac1eec0f70 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 10:42:23 +0200 Subject: [PATCH 13/61] added missing attitude state variable ;) --- .../modules/StateEstimation/inc/stateestimation.h | 10 ++++++---- flight/modules/StateEstimation/stateestimation.c | 14 +++++++++++++- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index dd6a3a157..84672d939 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -36,16 +36,18 @@ typedef enum { gyr_UPDATED = 1 << 0, acc_UPDATED = 1 << 1, mag_UPDATED = 1 << 2, - pos_UPDATED = 1 << 3, - vel_UPDATED = 1 << 4, - air_UPDATED = 1 << 5, - bar_UPDATED = 1 << 6, + att_UPDATED = 1 << 3, + pos_UPDATED = 1 << 4, + vel_UPDATED = 1 << 5, + air_UPDATED = 1 << 6, + bar_UPDATED = 1 << 7, } sensorUpdates; typedef struct { float gyr[3]; float acc[3]; float mag[3]; + float att[4]; float pos[3]; float vel[3]; float air[2]; diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 1ecdf7e1b..4968f2aba 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -51,7 +52,7 @@ #include "homelocation.h" #include "flightstatus.h" -// #include "CoordinateConversions.h" +#include "CoordinateConversions.h" // Private constants #define STACK_SIZE_BYTES 2048 @@ -397,6 +398,17 @@ static void StateEstimationCb(void) statename##Set(&s); \ } STORE1(BaroState, bar, Altitude); + // attitude nees manual conversion from quaternion to euler + if (ISSET(states.updated, att_UPDATED)) { \ + AttitudeStateData s; + AttitudeStateGet(&s); + s.q1 = states.att[0]; + s.q2 = states.att[1]; + s.q3 = states.att[2]; + s.q4 = states.att[3]; + Quaternion2RPY(&s.q1, &s.Roll); + AttitudeStateSet(&s); + } // clear alarms if everything is alright, then schedule callback execution after timeout From 2f082dbf87a4d4178953386d056b9aff3a282787 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 11:24:57 +0200 Subject: [PATCH 14/61] added filter for magnetometer bias drift compensation --- flight/modules/StateEstimation/filtermag.c | 182 +++++++++++++++++++++ 1 file changed, 182 insertions(+) create mode 100644 flight/modules/StateEstimation/filtermag.c diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c new file mode 100644 index 000000000..a54e99257 --- /dev/null +++ b/flight/modules/StateEstimation/filtermag.c @@ -0,0 +1,182 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup State Estimation + * @brief Acquires sensor data and computes state estimate + * @{ + * + * @file filtermag.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief Magnetometer drift compensation, uses previous cycles + * AttitudeState for estimation + * + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "inc/stateestimation.h" +#include +#include +#include + +#include + +// Private constants + +// Private types + +// Private variables + +static HomeLocationData homeLocation; +static RevoCalibrationData revoCalibration; +static float magBias[3] = { 0 }; + +// Private functions + +static int32_t init(void); +static int32_t filter(stateEstimation *state); +static void magOffsetEstimation(float mag[3]); + + +void filterMagInitialize(stateFilter *handle) +{ + handle->init = &init; + handle->filter = &filter; +} + +static int32_t init(void) +{ + magBias[0] = magBias[1] = magBias[2] = 0.0f; + HomeLocationGet(&homeLocation); + RevoCalibrationGet(&revoCalibration); + return 0; +} + +static int32_t filter(stateEstimation *state) +{ + if (ISSET(state->updated, mag_UPDATED)) { + if (revoCalibration.MagBiasNullingRate > 0) { + magOffsetEstimation(state->mag); + } + } + + return 0; +} + +/** + * Perform an update of the @ref MagBias based on + * Magmeter Offset Cancellation: Theory and Implementation, + * revisited William Premerlani, October 14, 2011 + */ +static void magOffsetEstimation(float mag[3]) +{ +#if 0 + // Constants, to possibly go into a UAVO + static const float MIN_NORM_DIFFERENCE = 50; + + static float B2[3] = { 0, 0, 0 }; + + MagBiasData magBias; + MagBiasGet(&magBias); + + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; + + // First call + if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { + B2[0] = mag->x; + B2[1] = mag->y; + B2[2] = mag->z; + return; + } + + float B1[3] = { mag->x, mag->y, mag->z }; + float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2)); + if (norm_diff > MIN_NORM_DIFFERENCE) { + float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]); + float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]); + float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; + float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale }; + + magBias.x += b_error[0]; + magBias.y += b_error[1]; + magBias.z += b_error[2]; + + MagBiasSet(&magBias); + + // Store this value to compare against next update + B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; + } +#else // if 0 + + const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]); + const float Rz = homeLocation.Be[2]; + + const float rate = revoCalibration.MagBiasNullingRate; + float Rot[3][3]; + float B_e[3]; + float xy[2]; + float delta[3]; + + AttitudeStateData attitude; + AttitudeStateGet(&attitude); + + // Get the rotation matrix + Quaternion2R(&attitude.q1, Rot); + + // Rotate the mag into the NED frame + B_e[0] = Rot[0][0] * mag[0] + Rot[1][0] * mag[1] + Rot[2][0] * mag[2]; + B_e[1] = Rot[0][1] * mag[0] + Rot[1][1] * mag[1] + Rot[2][1] * mag[2]; + B_e[2] = Rot[0][2] * mag[0] + Rot[1][2] * mag[1] + Rot[2][2] * mag[2]; + + float cy = cosf(DEG2RAD(attitude.Yaw)); + float sy = sinf(DEG2RAD(attitude.Yaw)); + + xy[0] = cy * B_e[0] + sy * B_e[1]; + xy[1] = -sy * B_e[0] + cy * B_e[1]; + + float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]); + + delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); + delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); + delta[2] = -rate * (Rz - B_e[2]); + + if (!isnan(delta[0]) && !isinf(delta[0]) && + !isnan(delta[1]) && !isinf(delta[1]) && + !isnan(delta[2]) && !isinf(delta[2])) { + magBias[0] += delta[0]; + magBias[1] += delta[1]; + magBias[2] += delta[2]; + } + + // Add bias to state estimation + mag[0] += magBias[0]; + mag[1] += magBias[1]; + mag[2] += magBias[2]; + +#endif // if 0 +} + + +/** + * @} + * @} + */ From 7ea258a895a7cc759c11f966a97b25f51fcaefef Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 12:36:20 +0200 Subject: [PATCH 15/61] airspeed filter added --- flight/modules/StateEstimation/filterair.c | 84 +++++++++++++++++++ .../modules/StateEstimation/stateestimation.c | 29 +++---- 2 files changed, 99 insertions(+), 14 deletions(-) create mode 100644 flight/modules/StateEstimation/filterair.c diff --git a/flight/modules/StateEstimation/filterair.c b/flight/modules/StateEstimation/filterair.c new file mode 100644 index 000000000..c63185d2a --- /dev/null +++ b/flight/modules/StateEstimation/filterair.c @@ -0,0 +1,84 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup State Estimation + * @brief Acquires sensor data and computes state estimate + * @{ + * + * @file filterair.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief Airspeed filter, calculates true airspeed based on indicated + * airspeed and uncorrected barometric altitude + * NOTE: This Sensor uses UNCORRECTED barometric altitude for + * correction -- run before barometric bias correction! + * + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "inc/stateestimation.h" + +// Private constants + +// simple IAS to TAS aproximation - 2% increase per 1000ft +// since we do not have flowing air temperature information +#define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f)) + +// Private types + +// Private variables +static float altitude = 0.0f; + +// Private functions + +static int32_t init(void); +static int32_t filter(stateEstimation *state); + + +void filterAirInitialize(stateFilter *handle) +{ + handle->init = &init; + handle->filter = &filter; +} + +static int32_t init(void) +{ + altitude = 0.0f; + return 0; +} + +static int32_t filter(stateEstimation *state) +{ + // take static pressure altitude estimation for + if (ISSET(state->updated, bar_UPDATED)) { + altitude = state->bar[0]; + } + // calculate true airspeed estimation + if (ISSET(state->updated, air_UPDATED)) { + state->air[1] = state->air[0] * IAS2TAS(altitude); + } + + return 0; +} + + +/** + * @} + * @} + */ diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 4968f2aba..6914c0773 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -95,9 +95,9 @@ static stateFilter ekf16iFilter; static const filterQueue *cfQueue = &(filterQueue) { .filter = &magFilter, .next = &(filterQueue) { - .filter = &baroFilter, + .filter = &airFilter, .next = &(filterQueue) { - .filter = &airFilter, + .filter = &baroFilter, .next = &(filterQueue) { .filter = &cfFilter, .next = NULL, @@ -108,9 +108,9 @@ static const filterQueue *cfQueue = &(filterQueue) { static const filterQueue *cfmQueue = &(filterQueue) { .filter = &magFilter, .next = &(filterQueue) { - .filter = &baroFilter, + .filter = &airFilter, .next = &(filterQueue) { - .filter = &airFilter, + .filter = &baroFilter, .next = &(filterQueue) { .filter = &cfmFilter, .next = NULL, @@ -121,11 +121,11 @@ static const filterQueue *cfmQueue = &(filterQueue) { static const filterQueue *ekf13iQueue = &(filterQueue) { .filter = &magFilter, .next = &(filterQueue) { - .filter = &baroFilter, + .filter = &airFilter, .next = &(filterQueue) { - .filter = &stationaryFilter, + .filter = &baroFilter, .next = &(filterQueue) { - .filter = &airFilter, + .filter = &stationaryFilter, .next = &(filterQueue) { .filter = &ekf13iFilter, .next = NULL, @@ -137,9 +137,9 @@ static const filterQueue *ekf13iQueue = &(filterQueue) { static const filterQueue *ekf13Queue = &(filterQueue) { .filter = &magFilter, .next = &(filterQueue) { - .filter = &baroFilter, + .filter = &airFilter, .next = &(filterQueue) { - .filter = &airFilter, + .filter = &baroFilter, .next = &(filterQueue) { .filter = &ekf13Filter, .next = NULL, @@ -150,11 +150,11 @@ static const filterQueue *ekf13Queue = &(filterQueue) { static const filterQueue *ekf16iQueue = &(filterQueue) { .filter = &magFilter, .next = &(filterQueue) { - .filter = &baroFilter, + .filter = &airFilter, .next = &(filterQueue) { - .filter = &stationaryFilter, + .filter = &baroFilter, .next = &(filterQueue) { - .filter = &airFilter, + .filter = &stationaryFilter, .next = &(filterQueue) { .filter = &ekf16iFilter, .next = NULL, @@ -166,9 +166,9 @@ static const filterQueue *ekf16iQueue = &(filterQueue) { static const filterQueue *ekf16Queue = &(filterQueue) { .filter = &magFilter, .next = &(filterQueue) { - .filter = &baroFilter, + .filter = &airFilter, .next = &(filterQueue) { - .filter = &airFilter, + .filter = &baroFilter, .next = &(filterQueue) { .filter = &ekf16Filter, .next = NULL, @@ -338,6 +338,7 @@ static void StateEstimationCb(void) } SANITYCHECK1(BaroSensor, bar, Altitude, 1); SANITYCHECK1(AirspeedSensor, air, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); + states.air[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter // GPS is a tiny bit more tricky as GPSPosition is not float (otherwise the conversion to NED could sit in a filter) but integers, for precision reasons if (ISSET(states.updated, pos_UPDATED)) { From 741c70cda4fe1674f0529501702ad52225fca00d Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 12:56:40 +0200 Subject: [PATCH 16/61] barometric bias filter added --- flight/modules/StateEstimation/filterbaro.c | 97 +++++++++++++++++++++ 1 file changed, 97 insertions(+) create mode 100644 flight/modules/StateEstimation/filterbaro.c diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c new file mode 100644 index 000000000..6cda6b175 --- /dev/null +++ b/flight/modules/StateEstimation/filterbaro.c @@ -0,0 +1,97 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup State Estimation + * @brief Acquires sensor data and computes state estimate + * @{ + * + * @file filterbaro.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief Barometric altitude filter, calculates altitude offset based on + * GPS altitude offset if available + * + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "inc/stateestimation.h" + +// Private constants + +// low pass filter configuration to calculate offset +// of barometric altitude sensor +// reasoning: updates at: 10 Hz, tau= 300 s settle time +// exp(-(1/f) / tau ) ~=~ 0.9997 +#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f + +// Private types + +// Private variables +static float baroOffset = 0.0f; +static bool first_run = 1; + +// Private functions + +static int32_t init(void); +static int32_t filter(stateEstimation *state); + + +void filterBaroInitialize(stateFilter *handle) +{ + handle->init = &init; + handle->filter = &filter; +} + +static int32_t init(void) +{ + baroOffset = 0.0f; + first_run = 1; + return 0; +} + +static int32_t filter(stateEstimation *state) +{ + if (first_run) { + // Initialize to current altitude reading at initial location + if (ISSET(state->updated, bar_UPDATED)) { + first_run = 0; + baroOffset = state->bar[0]; + } + } else { + // Track barometric altitude offset with a low pass filter + // based on GPS altitude if available + if (ISSET(state->updated, pos_UPDATED)) { + baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + + (1.0f - BARO_OFFSET_LOWPASS_ALPHA) + * (-state->pos[2] - state->bar[0]); + } + // calculate bias corrected altitude + if (ISSET(state->updated, bar_UPDATED)) { + state->bar[0] -= baroOffset; + } + } + + return 0; +} + + +/** + * @} + * @} + */ From a03f87efb552498e6d3f46371d65247235b3d8a6 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 15:04:36 +0200 Subject: [PATCH 17/61] Complementary filter added --- flight/modules/StateEstimation/filtercf.c | 410 ++++++++++++++++++ .../modules/StateEstimation/stateestimation.c | 13 +- 2 files changed, 419 insertions(+), 4 deletions(-) create mode 100644 flight/modules/StateEstimation/filtercf.c diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c new file mode 100644 index 000000000..5d6b8405c --- /dev/null +++ b/flight/modules/StateEstimation/filtercf.c @@ -0,0 +1,410 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup State Estimation + * @brief Acquires sensor data and computes state estimate + * @{ + * + * @file filtercf.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief Complementary filter to calculate Attitude from Accels and Gyros + * and optionally magnetometers: + * WARNING: Will drift if the mean acceleration force doesn't point + * to ground, unsafe on fixed wing, since position hold is + * implemented as continuous circular flying! + * + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "inc/stateestimation.h" + +#include +#include +#include +#include + +#include + +// Private constants + + +// Private types + +// Private variables +static AttitudeSettingsData attitudeSettings; +static FlightStatusData flightStatus; +static HomeLocationData homeLocation; + +static bool first_run = 1; +static bool useMag = 0; +static float currentAccel[3]; +static float currentMag[3]; +static float gyroBias[3]; +static bool accelUpdated = 0; +static bool magUpdated = 0; + + +static float accel_alpha = 0; +static bool accel_filter_enabled = false; + +// Private functions + +static int32_t initwithmag(void); +static int32_t initwithoutmag(void); +static int32_t maininit(void); +static int32_t filter(stateEstimation *state); +static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], float q[4]); + +static void flightStatusUpdatedCb(UAVObjEvent *ev); + + +void filterCFInitialize(stateFilter *handle) +{ + handle->init = &initwithoutmag; + handle->filter = &filter; + FlightStatusConnectCallback(&flightStatusUpdatedCb); + HomeLocationConnectCallback(&flightStatusUpdatedCb); + flightStatusUpdatedCb(NULL); +} + +void filterCFMInitialize(stateFilter *handle) +{ + handle->init = &initwithmag; + handle->filter = &filter; +} + +static int32_t initwithmag(void) +{ + useMag = 1; + return maininit(); +} + +static int32_t initwithoutmag(void) +{ + useMag = 0; + return maininit(); +} + +static int32_t maininit(void) +{ + first_run = 1; + accelUpdated = 0; + AttitudeSettingsGet(&attitudeSettings); + + const float fakeDt = 0.0025f; + if (attitudeSettings.AccelTau < 0.0001f) { + accel_alpha = 0; // not trusting this to resolve to 0 + accel_filter_enabled = false; + } else { + accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau); + accel_filter_enabled = true; + } + + // reset gyro Bias + gyroBias[0] = 0.0f; + gyroBias[1] = 0.0f; + gyroBias[2] = 0.0f; + + return 0; +} + +/** + * Collect all required state variables, then run complementary filter + */ +static int32_t filter(stateEstimation *state) +{ + int32_t result = 0; + + if (ISSET(state->updated, mag_UPDATED)) { + magUpdated = 1; + currentMag[0] = state->mag[0]; + currentMag[1] = state->mag[1]; + currentMag[2] = state->mag[2]; + } + if (ISSET(state->updated, acc_UPDATED)) { + accelUpdated = 1; + currentAccel[0] = state->acc[0]; + currentAccel[1] = state->acc[1]; + currentAccel[2] = state->acc[2]; + } + if (ISSET(state->updated, gyr_UPDATED)) { + if (accelUpdated) { + float att[4]; + result = complementaryFilter(state->gyr, currentAccel, currentMag, att); + if (!result) { + state->att[0] = att[0]; + state->att[1] = att[1]; + state->att[2] = att[2]; + state->att[3] = att[3]; + state->updated |= att_UPDATED; + } + accelUpdated = 0; + magUpdated = 0; + } + state->gyr[0] -= gyroBias[0]; + state->gyr[1] -= gyroBias[1]; + state->gyr[2] -= gyroBias[2]; + } + return result; +} + + +static inline void apply_accel_filter(const float *raw, float *filtered) +{ + if (accel_filter_enabled) { + filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha); + filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha); + filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha); + } else { + filtered[0] = raw[0]; + filtered[1] = raw[1]; + filtered[2] = raw[2]; + } +} + +static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], float q[4]) +{ + static int32_t timeval; + float dT; + static uint8_t init = 0; + float magKp = 0.0f; // TODO: make this non hardcoded at some point + float magKi = 0.000001f; + + // During initialization and + if (first_run) { +#if defined(PIOS_INCLUDE_HMC5883) + // wait until mags have been updated + if (!mag_updated) { + return 1; + } +#else + mag[0] = 100.0f; + mag[1] = 0.0f; + mag[2] = 0.0f; +#endif + AttitudeStateData attitudeState; // base on previous state + AttitudeStateGet(&attitudeState); + init = 0; + + // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, + // so pseudo "north" vector can be estimated even if the board is not level + attitudeState.Roll = atan2f(-accel[2], -accel[3]); + float zn = cosf(attitudeState.Roll) * mag[2] + sinf(attitudeState.Roll) * mag[1]; + float yn = cosf(attitudeState.Roll) * mag[1] - sinf(attitudeState.Roll) * mag[2]; + + // rotate accels z vector according to roll + float azn = cosf(attitudeState.Roll) * accel[2] + sinf(attitudeState.Roll) * accel[1]; + attitudeState.Pitch = atan2f(accel[0], -azn); + + float xn = cosf(attitudeState.Pitch) * mag[0] + sinf(attitudeState.Pitch) * zn; + + attitudeState.Yaw = atan2f(-yn, xn); + // TODO: This is still a hack + // Put this in a proper generic function in CoordinateConversion.c + // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) + // should calculate the rotation in 3d space using proper cross product math + // SUBTODO: formulate the math required + + attitudeState.Roll = RAD2DEG(attitudeState.Roll); + attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); + attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); + + RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1); + q[0] = attitudeState.q1; + q[1] = attitudeState.q1; + q[2] = attitudeState.q1; + q[3] = attitudeState.q1; + + timeval = PIOS_DELAY_GetRaw(); + + return 0; + } + + if ((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { + // For first 7 seconds use accels to get gyro bias + attitudeSettings.AccelKp = 1.0f; + attitudeSettings.AccelKi = 0.9f; + attitudeSettings.YawBiasRate = 0.23f; + magKp = 1.0f; + } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { + attitudeSettings.AccelKp = 1.0f; + attitudeSettings.AccelKi = 0.9f; + attitudeSettings.YawBiasRate = 0.23f; + magKp = 1.0f; + init = 0; + } else if (init == 0) { + // Reload settings (all the rates) + AttitudeSettingsGet(&attitudeSettings); + magKp = 0.01f; + init = 1; + } + + // Compute the dT using the cpu clock + dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; + timeval = PIOS_DELAY_GetRaw(); + if (dT < 0.001f) { // safe bounds + dT = 0.001f; + } + + AttitudeStateData attitudeState; // base on previous state + AttitudeStateGet(&attitudeState); + + // Get the current attitude estimate + quat_copy(&attitudeState.q1, q); + + float accels_filtered[3]; + // Apply smoothing to accel values, to reduce vibration noise before main calculations. + apply_accel_filter(accel, accels_filtered); + + // Rotate gravity to body frame and cross with accels + float grot[3]; + grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2])); + grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1])); + grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); + + float grot_filtered[3]; + float accel_err[3]; + apply_accel_filter(grot, grot_filtered); + CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); + + // Account for accel magnitude + float accel_mag = sqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]); + if (accel_mag < 1.0e-3f) { + return 2; // safety feature copied from CC + } + + // Account for filtered gravity vector magnitude + float grot_mag; + if (accel_filter_enabled) { + grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); + } else { + grot_mag = 1.0f; + } + if (grot_mag < 1.0e-3f) { + return 2; + } + + accel_err[0] /= (accel_mag * grot_mag); + accel_err[1] /= (accel_mag * grot_mag); + accel_err[2] /= (accel_mag * grot_mag); + + float mag_err[3] = { 0.0f }; + if (magUpdated && useMag) { + // Rotate gravity to body frame and cross with accels + float brot[3]; + float Rbe[3][3]; + + Quaternion2R(q, Rbe); + + rot_mult(Rbe, homeLocation.Be, brot); + + float mag_len = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]); + mag[0] /= mag_len; + mag[1] /= mag_len; + mag[2] /= mag_len; + + float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]); + brot[0] /= bmag; + brot[1] /= bmag; + brot[2] /= bmag; + + // Only compute if neither vector is null + if (bmag < 1.0f || mag_len < 1.0f) { + mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; + } else { + CrossProduct((const float *)mag, (const float *)brot, mag_err); + } + } else { + mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; + } + + // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s + gyroBias[0] -= accel_err[0] * attitudeSettings.AccelKi; + gyroBias[1] -= accel_err[1] * attitudeSettings.AccelKi; + if (useMag) { + gyroBias[2] -= mag_err[2] * magKi; + } + + // Correct rates based on integral coefficient + gyro[0] -= gyroBias[0]; + gyro[1] -= gyroBias[1]; + gyro[2] -= gyroBias[2]; + + float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] }; + // Correct rates based on proportional coefficient + gyrotmp[0] += accel_err[0] * attitudeSettings.AccelKp / dT; + gyrotmp[1] += accel_err[1] * attitudeSettings.AccelKp / dT; + if (useMag) { + gyrotmp[2] += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; + } else { + gyrotmp[2] += accel_err[2] * attitudeSettings.AccelKp / dT; + } + + // Work out time derivative from INSAlgo writeup + // Also accounts for the fact that gyros are in deg/s + float qdot[4]; + qdot[0] = DEG2RAD(-q[1] * gyrotmp[0] - q[2] * gyrotmp[1] - q[3] * gyrotmp[2]) * dT / 2; + qdot[1] = DEG2RAD(q[0] * gyrotmp[0] - q[3] * gyrotmp[1] + q[2] * gyrotmp[2]) * dT / 2; + qdot[2] = DEG2RAD(q[3] * gyrotmp[0] + q[0] * gyrotmp[1] - q[1] * gyrotmp[2]) * dT / 2; + qdot[3] = DEG2RAD(-q[2] * gyrotmp[0] + q[1] * gyrotmp[1] + q[0] * gyrotmp[2]) * dT / 2; + + // Take a time step + q[0] = q[0] + qdot[0]; + q[1] = q[1] + qdot[1]; + q[2] = q[2] + qdot[2]; + q[3] = q[3] + qdot[3]; + + if (q[0] < 0.0f) { + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } + + // Renomalize + float qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + q[0] = q[0] / qmag; + q[1] = q[1] / qmag; + q[2] = q[2] / qmag; + q[3] = q[3] / qmag; + + // If quaternion has become inappropriately short or is nan reinit. + // THIS SHOULD NEVER ACTUALLY HAPPEN + if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { + q[0] = 1.0f; + q[1] = 0.0f; + q[2] = 0.0f; + q[3] = 0.0f; + return 2; + } + + return 0; +} + +static void flightStatusUpdatedCb(UAVObjEvent *ev) +{ + FlightStatusGet(&flightStatus); + HomeLocationGet(&homeLocation); +} + +/** + * @} + * @} + */ diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 6914c0773..01d1da481 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -355,16 +355,21 @@ static void StateEstimationCb(void) // apply all filters in the current filter chain filterQueue *current = (filterQueue *)filterChain; - bool error = 0; + uint8_t error = 0; while (current != NULL) { int32_t result = current->filter->filter(&states); - if (result != 0) { - error = 1; + if (result > error) { + error = result; + alarm = 1; } current = current->next; } - if (error) { + if (error == 1) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); + } else if (error == 2) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + } else if (error == 3) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); } // the final output of filters is saved in state variables From a286cc6b24b939c3b6ddb140fe97981bae68acbc Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 15:28:18 +0200 Subject: [PATCH 18/61] update settings during initialisation only --- flight/modules/StateEstimation/filtercf.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 5d6b8405c..8bf3fbdfa 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -80,7 +80,6 @@ void filterCFInitialize(stateFilter *handle) handle->init = &initwithoutmag; handle->filter = &filter; FlightStatusConnectCallback(&flightStatusUpdatedCb); - HomeLocationConnectCallback(&flightStatusUpdatedCb); flightStatusUpdatedCb(NULL); } @@ -107,6 +106,7 @@ static int32_t maininit(void) first_run = 1; accelUpdated = 0; AttitudeSettingsGet(&attitudeSettings); + HomeLocationGet(&homeLocation); const float fakeDt = 0.0025f; if (attitudeSettings.AccelTau < 0.0001f) { @@ -401,7 +401,6 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], static void flightStatusUpdatedCb(UAVObjEvent *ev) { FlightStatusGet(&flightStatus); - HomeLocationGet(&homeLocation); } /** From e7c35a94f67c2a958e7968eede385862cb5a06ae Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 16:31:10 +0200 Subject: [PATCH 19/61] added EKF filter --- flight/modules/StateEstimation/filtercf.c | 2 +- flight/modules/StateEstimation/filterekf.c | 427 ++++++++++++++++++ .../boards/simposix/firmware/UAVObjects.inc | 2 + 3 files changed, 430 insertions(+), 1 deletion(-) create mode 100644 flight/modules/StateEstimation/filterekf.c diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 8bf3fbdfa..60f37aba0 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -205,7 +205,7 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, // so pseudo "north" vector can be estimated even if the board is not level - attitudeState.Roll = atan2f(-accel[2], -accel[3]); + attitudeState.Roll = atan2f(-accel[1], -accel[2]); float zn = cosf(attitudeState.Roll) * mag[2] + sinf(attitudeState.Roll) * mag[1]; float yn = cosf(attitudeState.Roll) * mag[1] - sinf(attitudeState.Roll) * mag[2]; diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c new file mode 100644 index 000000000..1f264ec52 --- /dev/null +++ b/flight/modules/StateEstimation/filterekf.c @@ -0,0 +1,427 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup State Estimation + * @brief Acquires sensor data and computes state estimate + * @{ + * + * @file filterekf.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief Extended Kalman Filter. Calculates complete system state except + * accelerometer drift. + * + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "inc/stateestimation.h" + +#include +#include +#include +#include +#include + + +#include +#include + +// Private constants + + +// Private types + +// Private variables +static EKFConfigurationData ekfConfiguration; +static HomeLocationData homeLocation; + +static bool first_run = 1; +static bool usePos = 0; + + +// Private functions + +static int32_t init13i(void); +static int32_t init13(void); +static int32_t maininit(void); +static int32_t filter(stateEstimation *state); +static inline bool invalid(float data); +static inline bool invalid_var(float data); + + +void filterEKF13iInitialize(stateFilter *handle) +{ + handle->init = &init13i; + handle->filter = &filter; +} +void filterEKF13Initialize(stateFilter *handle) +{ + handle->init = &init13; + handle->filter = &filter; +} +// XXX +// TODO: Until the 16 state EKF is implemented, run 13 state, so compilation runs through +// XXX +void filterEKF16iInitialize(stateFilter *handle) +{ + handle->init = &init13i; + handle->filter = &filter; +} +void filterEKF16Initialize(stateFilter *handle) +{ + handle->init = &init13; + handle->filter = &filter; +} + + +static int32_t init13i(void) +{ + usePos = 0; + return maininit(); +} + +static int32_t init13(void) +{ + usePos = 1; + return maininit(); +} + +static int32_t maininit(void) +{ + first_run = 1; + + EKFConfigurationGet(&ekfConfiguration); + int t; + // plausibility check + for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { + if (invalid_var(ekfConfiguration.P[t])) { + return 2; + } + } + for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { + if (invalid_var(ekfConfiguration.Q[t])) { + return 2; + } + } + for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) { + if (invalid_var(ekfConfiguration.R[t])) { + return 2; + } + } + HomeLocationGet(&homeLocation); + // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily + // switching between indoor and outdoor mode with Set = false) + if ((homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f)) { + return 2; + } + + + return 0; +} + +/** + * Collect all required state variables, then run complementary filter + */ +static int32_t filter(stateEstimation *state) +{ + static int32_t init_stage; + + static stateEstimation work; + + static uint32_t ins_last_time = 0; + static bool inited; + + const float zeros[3] = { 0.0f, 0.0f, 0.0f }; + + // Perform the update + float dT; + uint16_t sensors = 0; + + if (inited) { + work.updated = 0; + } + + if (first_run) { + first_run = false; + inited = false; + init_stage = 0; + + work.updated = 0; + + ins_last_time = PIOS_DELAY_GetRaw(); + + return 1; + } + + work.updated |= state->updated; + + // Get most recent data +#define UPDATE(shortname, num) \ + if (ISSET(state->updated, shortname##_UPDATED)) { \ + uint8_t t; \ + for (t = 0; t < num; t++) { \ + work.shortname[t] = state->shortname[t]; \ + } \ + } + UPDATE(gyr, 3); + UPDATE(acc, 3); + UPDATE(mag, 3); + UPDATE(bar, 1); + UPDATE(pos, 3); + UPDATE(vel, 3); + UPDATE(air, 2); + + + if (usePos) { + GPSPositionData gpsData; + GPSPositionGet(&gpsData); + // Have a minimum requirement for gps usage + if ((gpsData.Satellites < 7) || + (gpsData.PDOP > 4.0f) || + (gpsData.Latitude == 0 && gpsData.Longitude == 0) || + (homeLocation.Set != HOMELOCATION_SET_TRUE)) { + UNSET(state->updated, pos_UPDATED); + UNSET(state->updated, vel_UPDATED); + UNSET(work.updated, pos_UPDATED); + UNSET(work.updated, vel_UPDATED); + } + } + + dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; + ins_last_time = PIOS_DELAY_GetRaw(); + + // This should only happen at start up or at mode switches + if (dT > 0.01f) { + dT = 0.01f; + } else if (dT <= 0.001f) { + dT = 0.001f; + } + + if (!inited && ISSET(work.updated, mag_UPDATED) && ISSET(work.updated, bar_UPDATED) && ISSET(work.updated, pos_UPDATED)) { + // Don't initialize until all sensors are read + if (init_stage == 0) { + // Reset the INS algorithm + INSGPSInit(); + INSSetMagVar((float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_MAGX], + ekfConfiguration.R[EKFCONFIGURATION_R_MAGY], + ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] } + ); + INSSetAccelVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX], + ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY], + ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } + ); + INSSetGyroVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX], + ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY], + ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } + ); + INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX], + ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY], + ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } + ); + INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]); + + // Initialize the gyro bias + float gyro_bias[3] = { 0.0f, 0.0f, 0.0f }; + INSSetGyroBias(gyro_bias); + + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + + // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, + // so pseudo "north" vector can be estimated even if the board is not level + attitudeState.Roll = atan2f(-work.acc[1], -work.acc[2]); + float zn = cosf(attitudeState.Roll) * work.mag[2] + sinf(attitudeState.Roll) * work.mag[1]; + float yn = cosf(attitudeState.Roll) * work.mag[1] - sinf(attitudeState.Roll) * work.mag[2]; + + // rotate accels z vector according to roll + float azn = cosf(attitudeState.Roll) * work.acc[2] + sinf(attitudeState.Roll) * work.acc[1]; + attitudeState.Pitch = atan2f(work.acc[0], -azn); + + float xn = cosf(attitudeState.Pitch) * work.mag[0] + sinf(attitudeState.Pitch) * zn; + + attitudeState.Yaw = atan2f(-yn, xn); + // TODO: This is still a hack + // Put this in a proper generic function in CoordinateConversion.c + // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) + // should calculate the rotation in 3d space using proper cross product math + // SUBTODO: formulate the math required + + attitudeState.Roll = RAD2DEG(attitudeState.Roll); + attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); + attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); + + RPY2Quaternion(&attitudeState.Roll, work.att); + + INSSetState(work.pos, (float *)zeros, work.att, (float *)zeros, (float *)zeros); + + INSResetP(ekfConfiguration.P); + } else { + // Run prediction a bit before any corrections + + float gyros[3] = { DEG2RAD(work.gyr[0]), DEG2RAD(work.gyr[1]), DEG2RAD(work.gyr[2]) }; + INSStatePrediction(gyros, work.acc, dT); + + state->att[0] = Nav.q[0]; + state->att[1] = Nav.q[1]; + state->att[2] = Nav.q[2]; + state->att[3] = Nav.q[3]; + state->gyr[0] += Nav.gyro_bias[0]; + state->gyr[1] += Nav.gyro_bias[1]; + state->gyr[2] += Nav.gyro_bias[2]; + state->pos[0] = Nav.Pos[0]; + state->pos[1] = Nav.Pos[1]; + state->pos[2] = Nav.Pos[2]; + state->vel[0] = Nav.Vel[0]; + state->vel[1] = Nav.Vel[1]; + state->vel[2] = Nav.Vel[2]; + state->updated |= att_UPDATED | pos_UPDATED | vel_UPDATED | gyr_UPDATED; + } + + init_stage++; + if (init_stage > 10) { + inited = true; + } + + return 0; + } + + if (!inited) { + return 1; + } + + float gyros[3] = { DEG2RAD(work.gyr[0]), DEG2RAD(work.gyr[1]), DEG2RAD(work.gyr[2]) }; + + // Advance the state estimate + INSStatePrediction(gyros, work.acc, dT); + + // Copy the attitude into the UAVO + state->att[0] = Nav.q[0]; + state->att[1] = Nav.q[1]; + state->att[2] = Nav.q[2]; + state->att[3] = Nav.q[3]; + state->gyr[0] += Nav.gyro_bias[0]; + state->gyr[1] += Nav.gyro_bias[1]; + state->gyr[2] += Nav.gyro_bias[2]; + state->pos[0] = Nav.Pos[0]; + state->pos[1] = Nav.Pos[1]; + state->pos[2] = Nav.Pos[2]; + state->vel[0] = Nav.Vel[0]; + state->vel[1] = Nav.Vel[1]; + state->vel[2] = Nav.Vel[2]; + state->updated |= att_UPDATED | pos_UPDATED | vel_UPDATED | gyr_UPDATED; + + // Advance the covariance estimate + INSCovariancePrediction(dT); + + if (ISSET(work.updated, mag_UPDATED)) { + sensors |= MAG_SENSORS; + } + + if (ISSET(work.updated, bar_UPDATED)) { + sensors |= BARO_SENSOR; + } + + INSSetMagNorth(homeLocation.Be); + + if (!usePos) { + // position and velocity variance used in indoor mode + INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, + (float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] } + ); + } else { + // position and velocity variance used in outdoor mode + INSSetPosVelVar((float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH], + ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST], + ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] }, + (float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH], + ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST], + ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] } + ); + } + + if (ISSET(work.updated, pos_UPDATED)) { + sensors |= POS_SENSORS; + } + + if (ISSET(work.updated, vel_UPDATED)) { + sensors |= HORIZ_SENSORS | VERT_SENSORS; + } + + if (ISSET(work.updated, air_UPDATED) && ((!ISSET(work.updated, vel_UPDATED) && !ISSET(work.updated, pos_UPDATED)) | !usePos)) { + // HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance + sensors |= HORIZ_SENSORS | VERT_SENSORS; + INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, + (float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], + ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] } + ); + // rotate airspeed vector into NED frame - airspeed is measured in X axis only + float R[3][3]; + Quaternion2R(Nav.q, R); + float vtas[3] = { work.air[1], 0.0f, 0.0f }; + rot_mult(R, vtas, work.vel); + } + + /* + * TODO: Need to add a general sanity check for all the inputs to make sure their kosher + * although probably should occur within INS itself + */ + if (sensors) { + INSCorrection(work.mag, work.pos, work.vel, work.bar[0], sensors); + } + + EKFStateVarianceData vardata; + EKFStateVarianceGet(&vardata); + INSGetP(vardata.P); + EKFStateVarianceSet(&vardata); + + return 0; +} + +// check for invalid values +static inline bool invalid(float data) +{ + if (isnan(data) || isinf(data)) { + return true; + } + return false; +} + +// check for invalid variance values +static inline bool invalid_var(float data) +{ + if (invalid(data)) { + return true; + } + if (data < 1e-15f) { // var should not be close to zero. And not negative either. + return true; + } + return false; +} + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index c78edc050..dbaf20e87 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -96,6 +96,8 @@ UAVOBJSRCFILENAMES += camerastabsettings UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += revosettings UAVOBJSRCFILENAMES += altitudeholddesired +UAVOBJSRCFILENAMES += ekfconfiguration +UAVOBJSRCFILENAMES += ekfstatevariance UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) From 8a36959d39104c1e06699cc412278ea77df625b9 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 17:04:52 +0200 Subject: [PATCH 20/61] OP-945: Filter refaktoring - last fixes to make it compile --- flight/modules/StateEstimation/filtercf.c | 4 ++-- flight/modules/StateEstimation/stateestimation.c | 8 ++++---- flight/targets/boards/revolution/firmware/Makefile | 2 +- flight/targets/boards/revolution/firmware/UAVObjects.inc | 1 + flight/targets/boards/revoproto/firmware/Makefile | 2 +- flight/targets/boards/revoproto/firmware/UAVObjects.inc | 1 + 6 files changed, 10 insertions(+), 8 deletions(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 60f37aba0..f610cd57b 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -191,7 +191,7 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], if (first_run) { #if defined(PIOS_INCLUDE_HMC5883) // wait until mags have been updated - if (!mag_updated) { + if (!magUpdated) { return 1; } #else @@ -398,7 +398,7 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], return 0; } -static void flightStatusUpdatedCb(UAVObjEvent *ev) +static void flightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { FlightStatusGet(&flightStatus); } diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 01d1da481..0dfade465 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -183,7 +183,7 @@ static void settingsUpdatedCb(UAVObjEvent *objEv); static void sensorUpdatedCb(UAVObjEvent *objEv); static void StateEstimationCb(void); static void getNED(GPSPositionData *gpsPosition, float *NED); -static float sane(float value); +static bool sane(float value); /** @@ -236,7 +236,7 @@ int32_t StateEstimationStart(void) return 0; } -MODULE_INITCALL(AttitudeInitialize, AttitudeStart) +MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart) /** * Module callback @@ -428,7 +428,7 @@ static void StateEstimationCb(void) /** * Callback for eventdispatcher when HomeLocation or RevoSettings has been updated */ -static void settingsUpdatedCb(UAVObjEvent *ev) +static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { HomeLocationGet((HomeLocationData *)&homeLocation); @@ -516,7 +516,7 @@ static void getNED(GPSPositionData *gpsPosition, float *NED) * @param[in] float value * @returns true for safe and false for unsafe */ -static float sane(float value) +static bool sane(float value) { if (isnan(value) || isinf(value)) { return false; diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 888f73fcb..9733af952 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -29,7 +29,7 @@ USE_DSP_LIB ?= NO # List of mandatory modules to include MODULES += Sensors -MODULES += Attitude/revolution +MODULES += StateEstimation MODULES += Altitude/revolution MODULES += Airspeed/revolution MODULES += AltitudeHold diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 1ee34b95d..820187757 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -33,6 +33,7 @@ UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor +UAVOBJSRCFILENAMES += barostate UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedstate diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index ecf9e24d1..6ca4dbe9e 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -29,7 +29,7 @@ USE_DSP_LIB ?= NO # List of mandatory modules to include MODULES += Sensors -MODULES += Attitude/revolution +MODULES += StateEstimation MODULES += Altitude/revolution MODULES += Airspeed/revolution MODULES += AltitudeHold diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 5742838ba..d516cada5 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -38,6 +38,7 @@ UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += barosensor +UAVOBJSRCFILENAMES += barostate UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedstate From 018399670200916d7725dac644f00839943e7457 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 18:51:52 +0200 Subject: [PATCH 21/61] added missing uavobject in GCS --- ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 4ff9d4050..d588782cf 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -25,6 +25,7 @@ OTHER_FILES += UAVObjects.pluginspec # Add in all of the synthetic/generated uavobject files HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/barosensor.h \ + $$UAVOBJECT_SYNTHETICS/barostate.h \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.h \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.h \ $$UAVOBJECT_SYNTHETICS/airspeedstate.h \ @@ -109,6 +110,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/barosensor.cpp \ + $$UAVOBJECT_SYNTHETICS/barostate.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \ From a04b76f836a17ad7ded704ae0bbeb6b4170843ab Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 20:18:38 +0200 Subject: [PATCH 22/61] some bugfixes to make it boot on revo --- .../modules/StateEstimation/stateestimation.c | 38 ++++++++++++------- 1 file changed, 25 insertions(+), 13 deletions(-) diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 0dfade465..5662287be 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -193,6 +193,22 @@ static bool sane(float value); int32_t StateEstimationInitialize(void) { RevoSettingsInitialize(); + HomeLocationInitialize(); + + GyroSensorInitialize(); + MagSensorInitialize(); + BaroSensorInitialize(); + AirspeedSensorInitialize(); + GPSPositionInitialize(); + GPSVelocityInitialize(); + + GyroStateInitialize(); + AccelStateInitialize(); + MagStateInitialize(); + BaroStateInitialize(); + AirspeedStateInitialize(); + PositionStateInitialize(); + VelocityStateInitialize(); RevoSettingsConnectCallback(&settingsUpdatedCb); HomeLocationConnectCallback(&settingsUpdatedCb); @@ -248,7 +264,6 @@ static void StateEstimationCb(void) // set alarm to warning if called through timeout if (updatedSensors == 0) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); alarm = 1; } @@ -355,22 +370,13 @@ static void StateEstimationCb(void) // apply all filters in the current filter chain filterQueue *current = (filterQueue *)filterChain; - uint8_t error = 0; while (current != NULL) { int32_t result = current->filter->filter(&states); - if (result > error) { - error = result; - alarm = 1; + if (result > alarm) { + alarm = result; } current = current->next; } - if (error == 1) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); - } else if (error == 2) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - } else if (error == 3) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - } // the final output of filters is saved in state variables #define STORE3(statename, shortname, a1, a2, a3) \ @@ -418,7 +424,13 @@ static void StateEstimationCb(void) // clear alarms if everything is alright, then schedule callback execution after timeout - if (!alarm) { + if (alarm == 1) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); + } else if (alarm == 2) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + } else if (alarm >= 3) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + } else { AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); } DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER); From 3cb4f59c9428705a3f96aecad54206a5b754d602 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 20:49:34 +0200 Subject: [PATCH 23/61] failsafe, fallback to attitude for now until StateEstimation works reliably --- flight/modules/Attitude/revolution/attitude.c | 4 ++-- flight/targets/boards/revolution/firmware/Makefile | 3 ++- flight/targets/boards/revoproto/firmware/Makefile | 3 ++- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index 2a3b38d3d..25145d052 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -269,10 +269,10 @@ static void AttitudeTask(__attribute__((unused)) void *parameters) case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: ret_val = updateAttitudeComplementary(first_run); break; - case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: + case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR: ret_val = updateAttitudeINSGPS(first_run, true); break; - case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: + case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR: ret_val = updateAttitudeINSGPS(first_run, false); break; default: diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 9733af952..887b43318 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -29,7 +29,8 @@ USE_DSP_LIB ?= NO # List of mandatory modules to include MODULES += Sensors -MODULES += StateEstimation +MODULES += Attitude/revolution +#MODULES += StateEstimation # use instead of Attitude MODULES += Altitude/revolution MODULES += Airspeed/revolution MODULES += AltitudeHold diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 6ca4dbe9e..c2fe5e312 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -29,7 +29,8 @@ USE_DSP_LIB ?= NO # List of mandatory modules to include MODULES += Sensors -MODULES += StateEstimation +MODULES += Attitude/revolution +#MODULES += StateEstimation # use instead of Attitude MODULES += Altitude/revolution MODULES += Airspeed/revolution MODULES += AltitudeHold From fb433a97d76f86bd072f6ad81fbfac6fafe272d3 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 20 May 2013 20:52:45 +0200 Subject: [PATCH 24/61] Bugfix: add gyro bias to correct axis --- flight/modules/Attitude/revolution/attitude.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index 2a3b38d3d..ad5d1dc90 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -476,8 +476,8 @@ static int32_t updateAttitudeComplementary(bool first_run) // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s gyro_bias[0] -= accel_err[0] * attitudeSettings.AccelKi; - gyro_bias[0] -= accel_err[1] * attitudeSettings.AccelKi; - gyro_bias[0] -= mag_err[2] * magKi; + gyro_bias[1] -= accel_err[1] * attitudeSettings.AccelKi; + gyro_bias[2] -= mag_err[2] * magKi; // Correct rates based on integral coefficient gyroStateData.x -= gyro_bias[0]; From ecc4a529a3c8de4f1bbb214e995ede8c8599db32 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 21 May 2013 07:27:31 +0200 Subject: [PATCH 25/61] some fixes to filtercf and stateestimation and filterekf - Thanks Werner for help :-) --- flight/modules/StateEstimation/filtercf.c | 48 ++++++++++--------- flight/modules/StateEstimation/filterekf.c | 20 +++++++- .../modules/StateEstimation/stateestimation.c | 24 ++++++++-- .../boards/revolution/firmware/Makefile | 4 +- 4 files changed, 65 insertions(+), 31 deletions(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index f610cd57b..0a3c599cd 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -52,8 +52,9 @@ static AttitudeSettingsData attitudeSettings; static FlightStatusData flightStatus; static HomeLocationData homeLocation; -static bool first_run = 1; -static bool useMag = 0; +static bool initialized = 0; +static bool first_run = 1; +static bool useMag = 0; static float currentAccel[3]; static float currentMag[3]; static float gyroBias[3]; @@ -74,17 +75,28 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], static void flightStatusUpdatedCb(UAVObjEvent *ev); +static void globalInit(void); + + +static void globalInit(void) +{ + if (!initialized) { + initialized = 1; + FlightStatusConnectCallback(&flightStatusUpdatedCb); + flightStatusUpdatedCb(NULL); + } +} void filterCFInitialize(stateFilter *handle) { + globalInit(); handle->init = &initwithoutmag; handle->filter = &filter; - FlightStatusConnectCallback(&flightStatusUpdatedCb); - flightStatusUpdatedCb(NULL); } void filterCFMInitialize(stateFilter *handle) { + globalInit(); handle->init = &initwithmag; handle->filter = &filter; } @@ -158,9 +170,6 @@ static int32_t filter(stateEstimation *state) accelUpdated = 0; magUpdated = 0; } - state->gyr[0] -= gyroBias[0]; - state->gyr[1] -= gyroBias[1]; - state->gyr[2] -= gyroBias[2]; } return result; } @@ -226,13 +235,11 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); - RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1); - q[0] = attitudeState.q1; - q[1] = attitudeState.q1; - q[2] = attitudeState.q1; - q[3] = attitudeState.q1; + RPY2Quaternion(&attitudeState.Roll, q); - timeval = PIOS_DELAY_GetRaw(); + first_run = 0; + + timeval = PIOS_DELAY_GetRaw(); return 0; } @@ -338,14 +345,12 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s gyroBias[0] -= accel_err[0] * attitudeSettings.AccelKi; gyroBias[1] -= accel_err[1] * attitudeSettings.AccelKi; - if (useMag) { - gyroBias[2] -= mag_err[2] * magKi; - } + gyroBias[2] -= mag_err[2] * magKi; // Correct rates based on integral coefficient - gyro[0] -= gyroBias[0]; - gyro[1] -= gyroBias[1]; - gyro[2] -= gyroBias[2]; + gyro[0] -= gyroBias[0]; + gyro[1] -= gyroBias[1]; + gyro[2] -= gyroBias[2]; float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] }; // Correct rates based on proportional coefficient @@ -388,10 +393,7 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { - q[0] = 1.0f; - q[1] = 0.0f; - q[2] = 0.0f; - q[3] = 0.0f; + first_run = 1; return 2; } diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 1f264ec52..3d6271fda 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -51,8 +51,9 @@ static EKFConfigurationData ekfConfiguration; static HomeLocationData homeLocation; -static bool first_run = 1; -static bool usePos = 0; +static bool initialized = 0; +static bool first_run = 1; +static bool usePos = 0; // Private functions @@ -64,14 +65,27 @@ static int32_t filter(stateEstimation *state); static inline bool invalid(float data); static inline bool invalid_var(float data); +static void globalInit(void); + + +static void globalInit(void) +{ + if (!initialized) { + initialized = 1; + EKFConfigurationInitialize(); + EKFStateVarianceInitialize(); + } +} void filterEKF13iInitialize(stateFilter *handle) { + globalInit(); handle->init = &init13i; handle->filter = &filter; } void filterEKF13Initialize(stateFilter *handle) { + globalInit(); handle->init = &init13; handle->filter = &filter; } @@ -80,11 +94,13 @@ void filterEKF13Initialize(stateFilter *handle) // XXX void filterEKF16iInitialize(stateFilter *handle) { + globalInit(); handle->init = &init13i; handle->filter = &filter; } void filterEKF16Initialize(stateFilter *handle) { + globalInit(); handle->init = &init13; handle->filter = &filter; } diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 5662287be..ae8c64abd 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -260,7 +260,9 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart) static void StateEstimationCb(void) { // alarms flag - uint8_t alarm = 0; + int8_t alarm = 0; + static int8_t lastAlarm = -1; + static uint16_t alarmcounter = 0; // set alarm to warning if called through timeout if (updatedSensors == 0) { @@ -422,13 +424,27 @@ static void StateEstimationCb(void) AttitudeStateSet(&s); } + // throttle alarms, raise alarm flags immediately + // but require system to run for a while before decreasing + // to prevent alarm flapping + if (alarm >= lastAlarm) { + lastAlarm = alarm; + alarmcounter = 0; + } else { + if (alarmcounter < 100) { + alarmcounter++; + } else { + lastAlarm = alarm; + alarmcounter = 0; + } + } // clear alarms if everything is alright, then schedule callback execution after timeout - if (alarm == 1) { + if (lastAlarm == 1) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); - } else if (alarm == 2) { + } else if (lastAlarm == 2) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - } else if (alarm >= 3) { + } else if (lastAlarm >= 3) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); } else { AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 887b43318..95eb42bca 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -29,8 +29,8 @@ USE_DSP_LIB ?= NO # List of mandatory modules to include MODULES += Sensors -MODULES += Attitude/revolution -#MODULES += StateEstimation # use instead of Attitude +#MODULES += Attitude/revolution +MODULES += StateEstimation # use instead of Attitude MODULES += Altitude/revolution MODULES += Airspeed/revolution MODULES += AltitudeHold From 8fe159c457b846c2cf20a6e5dfa6033bcc13bae0 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 22 May 2013 19:05:28 +0200 Subject: [PATCH 26/61] some changes... --- flight/modules/StateEstimation/filterair.c | 13 +- .../StateEstimation/inc/stateestimation.h | 51 +-- .../modules/StateEstimation/stateestimation.c | 374 ++++++++++-------- 3 files changed, 241 insertions(+), 197 deletions(-) diff --git a/flight/modules/StateEstimation/filterair.c b/flight/modules/StateEstimation/filterair.c index c63185d2a..1cf875493 100644 --- a/flight/modules/StateEstimation/filterair.c +++ b/flight/modules/StateEstimation/filterair.c @@ -36,6 +36,8 @@ // Private constants +#define STACK_REQUIRED 64 + // simple IAS to TAS aproximation - 2% increase per 1000ft // since we do not have flowing air temperature information #define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f)) @@ -51,10 +53,11 @@ static int32_t init(void); static int32_t filter(stateEstimation *state); -void filterAirInitialize(stateFilter *handle) +int32_t filterAirInitialize(stateFilter *handle) { handle->init = &init; handle->filter = &filter; + return STACK_REQUIRED; } static int32_t init(void) @@ -66,12 +69,12 @@ static int32_t init(void) static int32_t filter(stateEstimation *state) { // take static pressure altitude estimation for - if (ISSET(state->updated, bar_UPDATED)) { - altitude = state->bar[0]; + if (ISSET(state->updated, SENSORUPDATES_baro)) { + altitude = state->baro[0]; } // calculate true airspeed estimation - if (ISSET(state->updated, air_UPDATED)) { - state->air[1] = state->air[0] * IAS2TAS(altitude); + if (ISSET(state->updated, SENSORUPDATES_airspeed)) { + state->air[1] = state->airspeed[0] * IAS2TAS(altitude); } return 0; diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index 84672d939..a334d64ac 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -33,25 +33,25 @@ #include typedef enum { - gyr_UPDATED = 1 << 0, - acc_UPDATED = 1 << 1, - mag_UPDATED = 1 << 2, - att_UPDATED = 1 << 3, - pos_UPDATED = 1 << 4, - vel_UPDATED = 1 << 5, - air_UPDATED = 1 << 6, - bar_UPDATED = 1 << 7, + SENSORUPDATES_gyro = 1 << 0, + SENSORUPDATES_accel = 1 << 1, + SENSORUPDATES_mag = 1 << 2, + SENSORUPDATES_attitude = 1 << 3, + SENSORUPDATES_pos = 1 << 4, + SENSORUPDATES_vel = 1 << 5, + SENSORUPDATES_airspeed = 1 << 6, + SENSORUPDATES_baro = 1 << 7, } sensorUpdates; typedef struct { - float gyr[3]; - float acc[3]; + float gyro[3]; + float accel[3]; float mag[3]; - float att[4]; + float attitude[4]; float pos[3]; float vel[3]; - float air[2]; - float bar[1]; + float airspeed[2]; + float baro[1]; sensorUpdates updated; } stateEstimation; @@ -60,20 +60,21 @@ typedef struct { typedef struct stateFilterStruct { - int32_t (*init)(void); - int32_t (*filter)(stateEstimation *state); + int32_t (*init)(struct stateFilterStruct *self); + int32_t (*filter)(struct stateFilterStruct *self, stateEstimation *state); + void *localdata; } stateFilter; -void filterMagInitialize(stateFilter *handle); -void filterBaroInitialize(stateFilter *handle); -void filterAirInitialize(stateFilter *handle); -void filterStationaryInitialize(stateFilter *handle); -void filterCFInitialize(stateFilter *handle); -void filterCFMInitialize(stateFilter *handle); -void filterEKF13iInitialize(stateFilter *handle); -void filterEKF13Initialize(stateFilter *handle); -void filterEKF16iInitialize(stateFilter *handle); -void filterEKF16Initialize(stateFilter *handle); +int32_t filterMagInitialize(stateFilter *handle); +int32_t filterBaroInitialize(stateFilter *handle); +int32_t filterAirInitialize(stateFilter *handle); +int32_t filterStationaryInitialize(stateFilter *handle); +int32_t filterCFInitialize(stateFilter *handle); +int32_t filterCFMInitialize(stateFilter *handle); +int32_t filterEKF13iInitialize(stateFilter *handle); +int32_t filterEKF13Initialize(stateFilter *handle); +int32_t filterEKF16iInitialize(stateFilter *handle); +int32_t filterEKF16Initialize(stateFilter *handle); #endif // STATEESTIMATION_H diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index ae8c64abd..2c039dd8d 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -62,12 +62,12 @@ // Private types -struct filterQueueStruct; +struct filterPipelineStruct; -typedef struct filterQueueStruct { - stateFilter *filter; - struct filterQueueStruct *next; -} filterQueue; +typedef const struct filterPipelineStruct { + const stateFilter *filter; + const struct filterPipelineStruct *next; +} filterPipeline; // Private variables static DelayedCallbackInfo *stateEstimationCallback; @@ -76,8 +76,8 @@ static volatile RevoSettingsData revoSettings; static volatile HomeLocationData homeLocation; static float LLA2NEDM[3]; static volatile sensorUpdates updatedSensors; -static int32_t fusionAlgorithm = -1; -static filterQueue *filterChain = NULL; +static int32_t fusionAlgorithm = -1; +static filterPipeline *filterChain = NULL; // different filters available to state estimation static stateFilter magFilter; @@ -92,41 +92,41 @@ static stateFilter ekf16Filter; static stateFilter ekf16iFilter; // preconfigured filter chains selectable via revoSettings.FusionAlgorithm -static const filterQueue *cfQueue = &(filterQueue) { +static filterPipeline *cfQueue = &(filterPipeline) { .filter = &magFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &airFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &baroFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &cfFilter, .next = NULL, }, } } }; -static const filterQueue *cfmQueue = &(filterQueue) { +static const filterPipeline *cfmQueue = &(filterPipeline) { .filter = &magFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &airFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &baroFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &cfmFilter, .next = NULL, } } } }; -static const filterQueue *ekf13iQueue = &(filterQueue) { +static const filterPipeline *ekf13iQueue = &(filterPipeline) { .filter = &magFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &airFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &baroFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &stationaryFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &ekf13iFilter, .next = NULL, } @@ -134,28 +134,28 @@ static const filterQueue *ekf13iQueue = &(filterQueue) { } } }; -static const filterQueue *ekf13Queue = &(filterQueue) { +static const filterPipeline *ekf13Queue = &(filterPipeline) { .filter = &magFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &airFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &baroFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &ekf13Filter, .next = NULL, } } } }; -static const filterQueue *ekf16iQueue = &(filterQueue) { +static const filterPipeline *ekf16iQueue = &(filterPipeline) { .filter = &magFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &airFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &baroFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &stationaryFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &ekf16iFilter, .next = NULL, } @@ -163,13 +163,13 @@ static const filterQueue *ekf16iQueue = &(filterQueue) { } } }; -static const filterQueue *ekf16Queue = &(filterQueue) { +static const filterPipeline *ekf16Queue = &(filterPipeline) { .filter = &magFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &airFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &baroFilter, - .next = &(filterQueue) { + .next = &(filterPipeline) { .filter = &ekf16Filter, .next = NULL, } @@ -185,6 +185,13 @@ static void StateEstimationCb(void); static void getNED(GPSPositionData *gpsPosition, float *NED); static bool sane(float value); +static inline int32_t maxint32_t(int32_t a, int2_t b) +{ + if (a > b) { + return a; + } + return b; +} /** * Initialise the module. Called before the start function @@ -221,7 +228,20 @@ int32_t StateEstimationInitialize(void) GPSPositionConnectCallback(&sensorUpdatedCb); GPSVelocityConnectCallback(&sensorUpdatedCb); - stateEstimationCallback = DelayedCallbackCreate(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, STACK_SIZE_BYTES); + uint32_t stack_required = STACK_SIZE_BYTES; + // Initialize Filters + stack_required = maxint32_t(stack_required, filterMagInitialize(&magFilter)); + stack_required = maxint32_t(stack_required, filterBaroInitialize(&baroFilter)); + stack_required = maxint32_t(stack_required, filterAirInitialize(&airFilter)); + stack_required = maxint32_t(stack_required, filterStationaryInitialize(&stationaryFilter)); + stack_required = maxint32_t(stack_required, filterCFInitialize(&cfFilter)); + stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter)); + stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter)); + stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter)); + stack_required = maxint32_t(stack_required, filterEKF16Initialize(&ekf16Filter)); + stack_required = maxint32_t(stack_required, filterEKF16iInitialize(&ekf16iFilter)); + + stateEstimationCallback = DelayedCallbackCreate(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, stack_required); return 0; } @@ -237,18 +257,6 @@ int32_t StateEstimationStart(void) // Force settings update to make sure rotation loaded settingsUpdatedCb(NULL); - // Initialize Filters - filterMagInitialize(&magFilter); - filterBaroInitialize(&baroFilter); - filterAirInitialize(&airFilter); - filterStationaryInitialize(&stationaryFilter); - filterCFInitialize(&cfFilter); - filterCFMInitialize(&cfmFilter); - filterEKF13iInitialize(&ekf13iFilter); - filterEKF13Initialize(&ekf13Filter); - filterEKF16Initialize(&ekf16Filter); - filterEKF16iInitialize(&ekf16iFilter); - return 0; } @@ -259,72 +267,78 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart) */ static void StateEstimationCb(void) { - // alarms flag - int8_t alarm = 0; + static enum { RUNSTATE_LOAD = 0, RUNSTATE_FILTER = 1, RUNSTATE_SAVE = 2 } runState = RUNSTATE_LOAD; + static int8_t alarm = 0; static int8_t lastAlarm = -1; static uint16_t alarmcounter = 0; + static filterPipeline *current; + static stateEstimation states; - // set alarm to warning if called through timeout - if (updatedSensors == 0) { - alarm = 1; - } + switch (runState) { + case RUNSTATE_LOAD: - // check if a new filter chain should be initialized - if (fusionAlgorithm != revoSettings.FusionAlgorithm) { - FlightStatusData fs; - FlightStatusGet(&fs); - if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == -1) { - const filterQueue *newFilterChain; - switch (revoSettings.FusionAlgorithm) { - case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: - newFilterChain = cfQueue; - break; - case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG: - newFilterChain = cfmQueue; - break; - case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR: - newFilterChain = ekf13iQueue; - break; - case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR: - newFilterChain = ekf13Queue; - break; - case REVOSETTINGS_FUSIONALGORITHM_INS16INDOOR: - newFilterChain = ekf16iQueue; - break; - case REVOSETTINGS_FUSIONALGORITHM_INS16OUTDOOR: - newFilterChain = ekf16Queue; - break; - default: - newFilterChain = NULL; - } - // initialize filters in chain - filterQueue *current = (filterQueue *)newFilterChain; - bool error = 0; - while (current != NULL) { - int32_t result = current->filter->init(); - if (result != 0) { - error = 1; + alarm = 0; + + // set alarm to warning if called through timeout + if (updatedSensors == 0) { + alarm = 1; + } + + // check if a new filter chain should be initialized + if (fusionAlgorithm != revoSettings.FusionAlgorithm) { + FlightStatusData fs; + FlightStatusGet(&fs); + if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == -1) { + const filterPipeline *newFilterChain; + switch (revoSettings.FusionAlgorithm) { + case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: + newFilterChain = cfQueue; break; + case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG: + newFilterChain = cfmQueue; + break; + case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR: + newFilterChain = ekf13iQueue; + break; + case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR: + newFilterChain = ekf13Queue; + break; + case REVOSETTINGS_FUSIONALGORITHM_INS16INDOOR: + newFilterChain = ekf16iQueue; + break; + case REVOSETTINGS_FUSIONALGORITHM_INS16OUTDOOR: + newFilterChain = ekf16Queue; + break; + default: + newFilterChain = NULL; + } + // initialize filters in chain + current = (filterPipeline *)newFilterChain; + bool error = 0; + while (current != NULL) { + int32_t result = current->filter->init(); + if (result != 0) { + error = 1; + break; + } + current = current->next; + } + if (error) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + return; + } else { + // set new fusion algortithm + filterChain = (filterPipeline *)newFilterChain; + fusionAlgorithm = revoSettings.FusionAlgorithm; } - current = current->next; - } - if (error) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - return; - } else { - // set new fusion algortithm - filterChain = (filterQueue *)newFilterChain; - fusionAlgorithm = revoSettings.FusionAlgorithm; } } - } - // read updated sensor UAVObjects and set initial state - stateEstimation states; - states.updated = updatedSensors; - updatedSensors ^= states.updated; + // read updated sensor UAVObjects and set initial state + states.updated = updatedSensors; + updatedSensors ^= states.updated; - // most sensors get only rudimentary sanity checks + // most sensors get only rudimentary sanity checks #define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \ if (ISSET(states.updated, shortname##_UPDATED)) { \ sensorname##Data s; \ @@ -338,10 +352,10 @@ static void StateEstimationCb(void) UNSET(states.updated, shortname##_UPDATED); \ } \ } - SANITYCHECK3(GyroSensor, gyr, x, y, z); - SANITYCHECK3(AccelSensor, acc, x, y, z); - SANITYCHECK3(MagSensor, mag, x, y, z); - SANITYCHECK3(GPSVelocity, vel, North, East, Down); + SANITYCHECK3(GyroSensor, gyr, x, y, z); + SANITYCHECK3(AccelSensor, acc, x, y, z); + SANITYCHECK3(MagSensor, mag, x, y, z); + SANITYCHECK3(GPSVelocity, vel, North, East, Down); #define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \ if (ISSET(states.updated, shortname##_UPDATED)) { \ sensorname##Data s; \ @@ -353,34 +367,51 @@ static void StateEstimationCb(void) UNSET(states.updated, shortname##_UPDATED); \ } \ } - SANITYCHECK1(BaroSensor, bar, Altitude, 1); - SANITYCHECK1(AirspeedSensor, air, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); - states.air[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter + SANITYCHECK1(BaroSensor, bar, Altitude, 1); + SANITYCHECK1(AirspeedSensor, air, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); + states.air[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter - // GPS is a tiny bit more tricky as GPSPosition is not float (otherwise the conversion to NED could sit in a filter) but integers, for precision reasons - if (ISSET(states.updated, pos_UPDATED)) { - GPSPositionData s; - GPSPositionGet(&s); - if (homeLocation.Set == HOMELOCATION_SET_TRUE && sane(s.Latitude) && sane(s.Longitude) && sane(s.Altitude) && (fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f)) { - getNED(&s, states.pos); - } else { - UNSET(states.updated, pos_UPDATED); + // GPS is a tiny bit more tricky as GPSPosition is not float (otherwise the conversion to NED could sit in a filter) but integers, for precision reasons + if (ISSET(states.updated, pos_UPDATED)) { + GPSPositionData s; + GPSPositionGet(&s); + if (homeLocation.Set == HOMELOCATION_SET_TRUE && sane(s.Latitude) && sane(s.Longitude) && sane(s.Altitude) && (fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f)) { + getNED(&s, states.pos); + } else { + UNSET(states.updated, pos_UPDATED); + } } - } - // at this point sensor state is stored in "states" with some rudimentary filtering applied + // at this point sensor state is stored in "states" with some rudimentary filtering applied - // apply all filters in the current filter chain - filterQueue *current = (filterQueue *)filterChain; - while (current != NULL) { - int32_t result = current->filter->filter(&states); - if (result > alarm) { - alarm = result; + // apply all filters in the current filter chain + current = (filterPipeline *)filterChain; + + // we are not done, re-dispatch self execution + runState = RUNSTATE_FILTER; + DelayedCallbackDispatch(stateEstimationCallback); + break; + + case RUNSTATE_FILTER: + + if (current != NULL) { + int32_t result = current->filter->filter(&states); + if (result > alarm) { + alarm = result; + } + current = current->next; } - current = current->next; - } - // the final output of filters is saved in state variables + // we are not done, re-dispatch self execution + if (!current) { + runState = RUNSTATE_SAVE; + } + DelayedCallbackDispatch(stateEstimationCallback); + break; + + case RUNSTATE_SAVE: + + // the final output of filters is saved in state variables #define STORE3(statename, shortname, a1, a2, a3) \ if (ISSET(states.updated, shortname##_UPDATED)) { \ statename##Data s; \ @@ -390,11 +421,11 @@ static void StateEstimationCb(void) s.a3 = states.shortname[2]; \ statename##Set(&s); \ } - STORE3(GyroState, gyr, x, y, z); - STORE3(AccelState, acc, x, y, z); - STORE3(MagState, mag, x, y, z); - STORE3(PositionState, pos, North, East, Down); - STORE3(VelocityState, vel, North, East, Down); + STORE3(GyroState, gyr, x, y, z); + STORE3(AccelState, acc, x, y, z); + STORE3(MagState, mag, x, y, z); + STORE3(PositionState, pos, North, East, Down); + STORE3(VelocityState, vel, North, East, Down); #define STORE2(statename, shortname, a1, a2) \ if (ISSET(states.updated, shortname##_UPDATED)) { \ statename##Data s; \ @@ -403,7 +434,7 @@ static void StateEstimationCb(void) s.a2 = states.shortname[1]; \ statename##Set(&s); \ } - STORE2(AirspeedState, air, CalibratedAirspeed, TrueAirspeed); + STORE2(AirspeedState, air, CalibratedAirspeed, TrueAirspeed); #define STORE1(statename, shortname, a1) \ if (ISSET(states.updated, shortname##_UPDATED)) { \ statename##Data s; \ @@ -411,45 +442,54 @@ static void StateEstimationCb(void) s.a1 = states.shortname[0]; \ statename##Set(&s); \ } - STORE1(BaroState, bar, Altitude); - // attitude nees manual conversion from quaternion to euler - if (ISSET(states.updated, att_UPDATED)) { \ - AttitudeStateData s; - AttitudeStateGet(&s); - s.q1 = states.att[0]; - s.q2 = states.att[1]; - s.q3 = states.att[2]; - s.q4 = states.att[3]; - Quaternion2RPY(&s.q1, &s.Roll); - AttitudeStateSet(&s); - } + STORE1(BaroState, bar, Altitude); + // attitude nees manual conversion from quaternion to euler + if (ISSET(states.updated, att_UPDATED)) { \ + AttitudeStateData s; + AttitudeStateGet(&s); + s.q1 = states.att[0]; + s.q2 = states.att[1]; + s.q3 = states.att[2]; + s.q4 = states.att[3]; + Quaternion2RPY(&s.q1, &s.Roll); + AttitudeStateSet(&s); + } - // throttle alarms, raise alarm flags immediately - // but require system to run for a while before decreasing - // to prevent alarm flapping - if (alarm >= lastAlarm) { - lastAlarm = alarm; - alarmcounter = 0; - } else { - if (alarmcounter < 100) { - alarmcounter++; - } else { + // throttle alarms, raise alarm flags immediately + // but require system to run for a while before decreasing + // to prevent alarm flapping + if (alarm >= lastAlarm) { lastAlarm = alarm; alarmcounter = 0; + } else { + if (alarmcounter < 100) { + alarmcounter++; + } else { + lastAlarm = alarm; + alarmcounter = 0; + } } - } - // clear alarms if everything is alright, then schedule callback execution after timeout - if (lastAlarm == 1) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); - } else if (lastAlarm == 2) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - } else if (lastAlarm >= 3) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + // clear alarms if everything is alright, then schedule callback execution after timeout + if (lastAlarm == 1) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); + } else if (lastAlarm == 2) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + } else if (lastAlarm >= 3) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + } + + // we are done, re-schedule next self execution + runState = RUNSTATE_LOAD; + if (updatedSensors) { + DelayedCallbackDispatch(stateEstimationCallback); + } else { + DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER); + } + break; } - DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER); } From b56de3b66b76f0d0817e9b3173669ce18bf1cdf3 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 22 May 2013 21:11:48 +0200 Subject: [PATCH 27/61] filterchain rework --- flight/modules/StateEstimation/filterair.c | 29 +- flight/modules/StateEstimation/filterbaro.c | 52 +-- flight/modules/StateEstimation/filtercf.c | 284 ++++++++------- flight/modules/StateEstimation/filterekf.c | 333 +++++++++--------- flight/modules/StateEstimation/filtermag.c | 65 ++-- .../StateEstimation/filterstationary.c | 22 +- .../modules/StateEstimation/stateestimation.c | 74 ++-- .../boards/revolution/firmware/UAVObjects.inc | 1 - .../boards/revoproto/firmware/UAVObjects.inc | 1 - .../boards/simposix/firmware/UAVObjects.inc | 1 - .../src/plugins/uavobjects/uavobjects.pro | 2 - shared/uavobjectdefinition/barostate.xml | 10 - 12 files changed, 450 insertions(+), 424 deletions(-) delete mode 100644 shared/uavobjectdefinition/barostate.xml diff --git a/flight/modules/StateEstimation/filterair.c b/flight/modules/StateEstimation/filterair.c index 1cf875493..f5a4ee60d 100644 --- a/flight/modules/StateEstimation/filterair.c +++ b/flight/modules/StateEstimation/filterair.c @@ -43,38 +43,43 @@ #define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f)) // Private types - -// Private variables -static float altitude = 0.0f; +struct data { + float altitude; +}; // Private functions -static int32_t init(void); -static int32_t filter(stateEstimation *state); +static int32_t init(stateFilter *self); +static int32_t filter(stateFilter *self, stateEstimation *state); int32_t filterAirInitialize(stateFilter *handle) { - handle->init = &init; - handle->filter = &filter; + handle->init = &init; + handle->filter = &filter; + handle->localdata = pvPortMalloc(sizeof(struct data)); return STACK_REQUIRED; } -static int32_t init(void) +static int32_t init(stateFilter *self) { - altitude = 0.0f; + struct data *this = (struct data *)self->localdata; + + this->altitude = 0.0f; return 0; } -static int32_t filter(stateEstimation *state) +static int32_t filter(stateFilter *self, stateEstimation *state) { + struct data *this = (struct data *)self->localdata; + // take static pressure altitude estimation for if (ISSET(state->updated, SENSORUPDATES_baro)) { - altitude = state->baro[0]; + this->altitude = state->baro[0]; } // calculate true airspeed estimation if (ISSET(state->updated, SENSORUPDATES_airspeed)) { - state->air[1] = state->airspeed[0] * IAS2TAS(altitude); + state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude); } return 0; diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index 6cda6b175..b8b8f14be 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -34,6 +34,8 @@ // Private constants +#define STACK_REQUIRED 64 + // low pass filter configuration to calculate offset // of barometric altitude sensor // reasoning: updates at: 10 Hz, tau= 300 s settle time @@ -41,49 +43,57 @@ #define BARO_OFFSET_LOWPASS_ALPHA 0.9997f // Private types +struct data { + float baroOffset; + bool first_run; +}; // Private variables -static float baroOffset = 0.0f; -static bool first_run = 1; // Private functions -static int32_t init(void); -static int32_t filter(stateEstimation *state); +static int32_t init(stateFilter *self); +static int32_t filter(stateFilter *self, stateEstimation *state); -void filterBaroInitialize(stateFilter *handle) +int32_t filterBaroInitialize(stateFilter *handle) { - handle->init = &init; - handle->filter = &filter; + handle->init = &init; + handle->filter = &filter; + handle->localdata = pvPortMalloc(sizeof(struct data)); + return STACK_REQUIRED; } -static int32_t init(void) +static int32_t init(stateFilter *self) { - baroOffset = 0.0f; - first_run = 1; + struct data *this = (struct data *)self->localdata; + + this->baroOffset = 0.0f; + this->first_run = 1; return 0; } -static int32_t filter(stateEstimation *state) +static int32_t filter(stateFilter *self, stateEstimation *state) { - if (first_run) { + struct data *this = (struct data *)self->localdata; + + if (this->first_run) { // Initialize to current altitude reading at initial location - if (ISSET(state->updated, bar_UPDATED)) { - first_run = 0; - baroOffset = state->bar[0]; + if (ISSET(state->updated, SENSORUPDATES_baro)) { + this->first_run = 0; + this->baroOffset = state->baro[0]; } } else { // Track barometric altitude offset with a low pass filter // based on GPS altitude if available - if (ISSET(state->updated, pos_UPDATED)) { - baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + - (1.0f - BARO_OFFSET_LOWPASS_ALPHA) - * (-state->pos[2] - state->bar[0]); + if (ISSET(state->updated, SENSORUPDATES_pos)) { + this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset + + (1.0f - BARO_OFFSET_LOWPASS_ALPHA) + * (-state->pos[2] - state->baro[0]); } // calculate bias corrected altitude - if (ISSET(state->updated, bar_UPDATED)) { - state->bar[0] -= baroOffset; + if (ISSET(state->updated, SENSORUPDATES_baro)) { + state->baro[0] -= this->baroOffset; } } diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 0a3c599cd..27ebbcce9 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -44,34 +44,36 @@ // Private constants +#define STACK_REQUIRED 512 // Private types +struct data { + AttitudeSettingsData attitudeSettings; + HomeLocationData homeLocation; + bool first_run; + bool useMag; + float currentAccel[3]; + float currentMag[3]; + float gyroBias[3]; + bool accelUpdated; + bool magUpdated; + float accel_alpha; + bool accel_filter_enabled; + int32_t timeval; + uint8_t init; +}; // Private variables -static AttitudeSettingsData attitudeSettings; +bool initialized = 0; static FlightStatusData flightStatus; -static HomeLocationData homeLocation; - -static bool initialized = 0; -static bool first_run = 1; -static bool useMag = 0; -static float currentAccel[3]; -static float currentMag[3]; -static float gyroBias[3]; -static bool accelUpdated = 0; -static bool magUpdated = 0; - - -static float accel_alpha = 0; -static bool accel_filter_enabled = false; // Private functions -static int32_t initwithmag(void); -static int32_t initwithoutmag(void); -static int32_t maininit(void); -static int32_t filter(stateEstimation *state); -static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], float q[4]); +static int32_t initwithmag(stateFilter *self); +static int32_t initwithoutmag(stateFilter *self); +static int32_t maininit(stateFilter *self); +static int32_t filter(stateFilter *self, stateEstimation *state); +static int32_t complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]); static void flightStatusUpdatedCb(UAVObjEvent *ev); @@ -87,52 +89,62 @@ static void globalInit(void) } } -void filterCFInitialize(stateFilter *handle) +int32_t filterCFInitialize(stateFilter *handle) { globalInit(); - handle->init = &initwithoutmag; - handle->filter = &filter; + handle->init = &initwithoutmag; + handle->filter = &filter; + handle->localdata = pvPortMalloc(sizeof(struct data)); + return STACK_REQUIRED; } -void filterCFMInitialize(stateFilter *handle) +int32_t filterCFMInitialize(stateFilter *handle) { globalInit(); - handle->init = &initwithmag; - handle->filter = &filter; + handle->init = &initwithmag; + handle->filter = &filter; + handle->localdata = pvPortMalloc(sizeof(struct data)); + return STACK_REQUIRED; } -static int32_t initwithmag(void) +static int32_t initwithmag(stateFilter *self) { - useMag = 1; - return maininit(); + struct data *this = (struct data *)self->localdata; + + this->useMag = 1; + return maininit(self); } -static int32_t initwithoutmag(void) +static int32_t initwithoutmag(stateFilter *self) { - useMag = 0; - return maininit(); + struct data *this = (struct data *)self->localdata; + + this->useMag = 0; + return maininit(self); } -static int32_t maininit(void) +static int32_t maininit(stateFilter *self) { - first_run = 1; - accelUpdated = 0; - AttitudeSettingsGet(&attitudeSettings); - HomeLocationGet(&homeLocation); + struct data *this = (struct data *)self->localdata; + + this->first_run = 1; + this->accelUpdated = 0; + AttitudeSettingsGet(&this->attitudeSettings); + HomeLocationGet(&this->homeLocation); const float fakeDt = 0.0025f; - if (attitudeSettings.AccelTau < 0.0001f) { - accel_alpha = 0; // not trusting this to resolve to 0 - accel_filter_enabled = false; + if (this->attitudeSettings.AccelTau < 0.0001f) { + this->accel_alpha = 0; // not trusting this to resolve to 0 + this->accel_filter_enabled = false; } else { - accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau); - accel_filter_enabled = true; + this->accel_alpha = expf(-fakeDt / this->attitudeSettings.AccelTau); + this->accel_filter_enabled = true; } // reset gyro Bias - gyroBias[0] = 0.0f; - gyroBias[1] = 0.0f; - gyroBias[2] = 0.0f; + this->gyroBias[0] = 0.0f; + this->gyroBias[1] = 0.0f; + this->gyroBias[2] = 0.0f; return 0; } @@ -140,47 +152,49 @@ static int32_t maininit(void) /** * Collect all required state variables, then run complementary filter */ -static int32_t filter(stateEstimation *state) +static int32_t filter(stateFilter *self, stateEstimation *state) { - int32_t result = 0; + struct data *this = (struct data *)self->localdata; - if (ISSET(state->updated, mag_UPDATED)) { - magUpdated = 1; - currentMag[0] = state->mag[0]; - currentMag[1] = state->mag[1]; - currentMag[2] = state->mag[2]; + int32_t result = 0; + + if (ISSET(state->updated, SENSORUPDATES_mag)) { + this->magUpdated = 1; + this->currentMag[0] = state->mag[0]; + this->currentMag[1] = state->mag[1]; + this->currentMag[2] = state->mag[2]; } - if (ISSET(state->updated, acc_UPDATED)) { - accelUpdated = 1; - currentAccel[0] = state->acc[0]; - currentAccel[1] = state->acc[1]; - currentAccel[2] = state->acc[2]; + if (ISSET(state->updated, SENSORUPDATES_accel)) { + this->accelUpdated = 1; + this->currentAccel[0] = state->accel[0]; + this->currentAccel[1] = state->accel[1]; + this->currentAccel[2] = state->accel[2]; } - if (ISSET(state->updated, gyr_UPDATED)) { - if (accelUpdated) { - float att[4]; - result = complementaryFilter(state->gyr, currentAccel, currentMag, att); + if (ISSET(state->updated, SENSORUPDATES_gyro)) { + if (this->accelUpdated) { + float attitude[4]; + result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, attitude); if (!result) { - state->att[0] = att[0]; - state->att[1] = att[1]; - state->att[2] = att[2]; - state->att[3] = att[3]; - state->updated |= att_UPDATED; + state->attitude[0] = attitude[0]; + state->attitude[1] = attitude[1]; + state->attitude[2] = attitude[2]; + state->attitude[3] = attitude[3]; + state->updated |= SENSORUPDATES_attitude; } - accelUpdated = 0; - magUpdated = 0; + this->accelUpdated = 0; + this->magUpdated = 0; } } return result; } -static inline void apply_accel_filter(const float *raw, float *filtered) +static inline void apply_accel_filter(const struct data *this, const float *raw, float *filtered) { - if (accel_filter_enabled) { - filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha); - filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha); - filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha); + if (this->accel_filter_enabled) { + filtered[0] = filtered[0] * this->accel_alpha + raw[0] * (1 - this->accel_alpha); + filtered[1] = filtered[1] * this->accel_alpha + raw[1] * (1 - this->accel_alpha); + filtered[2] = filtered[2] * this->accel_alpha + raw[2] * (1 - this->accel_alpha); } else { filtered[0] = raw[0]; filtered[1] = raw[1]; @@ -188,19 +202,17 @@ static inline void apply_accel_filter(const float *raw, float *filtered) } } -static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], float q[4]) +static int32_t complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]) { - static int32_t timeval; float dT; - static uint8_t init = 0; float magKp = 0.0f; // TODO: make this non hardcoded at some point float magKi = 0.000001f; // During initialization and - if (first_run) { + if (this->first_run) { #if defined(PIOS_INCLUDE_HMC5883) // wait until mags have been updated - if (!magUpdated) { + if (!this->magUpdated) { return 1; } #else @@ -210,7 +222,7 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], #endif AttitudeStateData attitudeState; // base on previous state AttitudeStateGet(&attitudeState); - init = 0; + this->init = 0; // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, // so pseudo "north" vector can be estimated even if the board is not level @@ -235,37 +247,37 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); - RPY2Quaternion(&attitudeState.Roll, q); + RPY2Quaternion(&attitudeState.Roll, attitude); - first_run = 0; + this->first_run = 0; - timeval = PIOS_DELAY_GetRaw(); + this->timeval = PIOS_DELAY_GetRaw(); return 0; } - if ((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { + if ((this->init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { // For first 7 seconds use accels to get gyro bias - attitudeSettings.AccelKp = 1.0f; - attitudeSettings.AccelKi = 0.9f; - attitudeSettings.YawBiasRate = 0.23f; + this->attitudeSettings.AccelKp = 1.0f; + this->attitudeSettings.AccelKi = 0.9f; + this->attitudeSettings.YawBiasRate = 0.23f; magKp = 1.0f; - } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { - attitudeSettings.AccelKp = 1.0f; - attitudeSettings.AccelKi = 0.9f; - attitudeSettings.YawBiasRate = 0.23f; + } else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { + this->attitudeSettings.AccelKp = 1.0f; + this->attitudeSettings.AccelKi = 0.9f; + this->attitudeSettings.YawBiasRate = 0.23f; magKp = 1.0f; - init = 0; - } else if (init == 0) { + this->init = 0; + } else if (this->init == 0) { // Reload settings (all the rates) - AttitudeSettingsGet(&attitudeSettings); + AttitudeSettingsGet(&this->attitudeSettings); magKp = 0.01f; - init = 1; + this->init = 1; } // Compute the dT using the cpu clock - dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; - timeval = PIOS_DELAY_GetRaw(); + dT = PIOS_DELAY_DiffuS(this->timeval) / 1000000.0f; + this->timeval = PIOS_DELAY_GetRaw(); if (dT < 0.001f) { // safe bounds dT = 0.001f; } @@ -274,21 +286,21 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], AttitudeStateGet(&attitudeState); // Get the current attitude estimate - quat_copy(&attitudeState.q1, q); + quat_copy(&attitudeState.q1, attitude); float accels_filtered[3]; // Apply smoothing to accel values, to reduce vibration noise before main calculations. - apply_accel_filter(accel, accels_filtered); + apply_accel_filter(this, accel, accels_filtered); // Rotate gravity to body frame and cross with accels float grot[3]; - grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2])); - grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1])); - grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); + grot[0] = -(2.0f * (attitude[1] * attitude[3] - attitude[0] * attitude[2])); + grot[1] = -(2.0f * (attitude[2] * attitude[3] + attitude[0] * attitude[1])); + grot[2] = -(attitude[0] * attitude[0] - attitude[1] * attitude[1] - attitude[2] * attitude[2] + attitude[3] * attitude[3]); float grot_filtered[3]; float accel_err[3]; - apply_accel_filter(grot, grot_filtered); + apply_accel_filter(this, grot, grot_filtered); CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); // Account for accel magnitude @@ -299,7 +311,7 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], // Account for filtered gravity vector magnitude float grot_mag; - if (accel_filter_enabled) { + if (this->accel_filter_enabled) { grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); } else { grot_mag = 1.0f; @@ -313,14 +325,14 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], accel_err[2] /= (accel_mag * grot_mag); float mag_err[3] = { 0.0f }; - if (magUpdated && useMag) { + if (this->magUpdated && this->useMag) { // Rotate gravity to body frame and cross with accels float brot[3]; float Rbe[3][3]; - Quaternion2R(q, Rbe); + Quaternion2R(attitude, Rbe); - rot_mult(Rbe, homeLocation.Be, brot); + rot_mult(Rbe, this->homeLocation.Be, brot); float mag_len = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]); mag[0] /= mag_len; @@ -343,57 +355,57 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], } // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s - gyroBias[0] -= accel_err[0] * attitudeSettings.AccelKi; - gyroBias[1] -= accel_err[1] * attitudeSettings.AccelKi; - gyroBias[2] -= mag_err[2] * magKi; + this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi; + this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi; + this->gyroBias[2] -= mag_err[2] * magKi; // Correct rates based on integral coefficient - gyro[0] -= gyroBias[0]; - gyro[1] -= gyroBias[1]; - gyro[2] -= gyroBias[2]; + gyro[0] -= this->gyroBias[0]; + gyro[1] -= this->gyroBias[1]; + gyro[2] -= this->gyroBias[2]; float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] }; // Correct rates based on proportional coefficient - gyrotmp[0] += accel_err[0] * attitudeSettings.AccelKp / dT; - gyrotmp[1] += accel_err[1] * attitudeSettings.AccelKp / dT; - if (useMag) { - gyrotmp[2] += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; + gyrotmp[0] += accel_err[0] * this->attitudeSettings.AccelKp / dT; + gyrotmp[1] += accel_err[1] * this->attitudeSettings.AccelKp / dT; + if (this->useMag) { + gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; } else { - gyrotmp[2] += accel_err[2] * attitudeSettings.AccelKp / dT; + gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT; } // Work out time derivative from INSAlgo writeup // Also accounts for the fact that gyros are in deg/s float qdot[4]; - qdot[0] = DEG2RAD(-q[1] * gyrotmp[0] - q[2] * gyrotmp[1] - q[3] * gyrotmp[2]) * dT / 2; - qdot[1] = DEG2RAD(q[0] * gyrotmp[0] - q[3] * gyrotmp[1] + q[2] * gyrotmp[2]) * dT / 2; - qdot[2] = DEG2RAD(q[3] * gyrotmp[0] + q[0] * gyrotmp[1] - q[1] * gyrotmp[2]) * dT / 2; - qdot[3] = DEG2RAD(-q[2] * gyrotmp[0] + q[1] * gyrotmp[1] + q[0] * gyrotmp[2]) * dT / 2; + qdot[0] = DEG2RAD(-attitude[1] * gyrotmp[0] - attitude[2] * gyrotmp[1] - attitude[3] * gyrotmp[2]) * dT / 2; + qdot[1] = DEG2RAD(attitude[0] * gyrotmp[0] - attitude[3] * gyrotmp[1] + attitude[2] * gyrotmp[2]) * dT / 2; + qdot[2] = DEG2RAD(attitude[3] * gyrotmp[0] + attitude[0] * gyrotmp[1] - attitude[1] * gyrotmp[2]) * dT / 2; + qdot[3] = DEG2RAD(-attitude[2] * gyrotmp[0] + attitude[1] * gyrotmp[1] + attitude[0] * gyrotmp[2]) * dT / 2; // Take a time step - q[0] = q[0] + qdot[0]; - q[1] = q[1] + qdot[1]; - q[2] = q[2] + qdot[2]; - q[3] = q[3] + qdot[3]; + attitude[0] = attitude[0] + qdot[0]; + attitude[1] = attitude[1] + qdot[1]; + attitude[2] = attitude[2] + qdot[2]; + attitude[3] = attitude[3] + qdot[3]; - if (q[0] < 0.0f) { - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; + if (attitude[0] < 0.0f) { + attitude[0] = -attitude[0]; + attitude[1] = -attitude[1]; + attitude[2] = -attitude[2]; + attitude[3] = -attitude[3]; } // Renomalize - float qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); - q[0] = q[0] / qmag; - q[1] = q[1] / qmag; - q[2] = q[2] / qmag; - q[3] = q[3] / qmag; + float qmag = sqrtf(attitude[0] * attitude[0] + attitude[1] * attitude[1] + attitude[2] * attitude[2] + attitude[3] * attitude[3]); + attitude[0] = attitude[0] / qmag; + attitude[1] = attitude[1] / qmag; + attitude[2] = attitude[2] / qmag; + attitude[3] = attitude[3] / qmag; // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { - first_run = 1; + this->first_run = 1; return 2; } diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 3d6271fda..e6e20e648 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -44,25 +44,33 @@ // Private constants +#define STACK_REQUIRED 2048 // Private types +struct data { + EKFConfigurationData ekfConfiguration; + HomeLocationData homeLocation; + + bool usePos; + + int32_t init_stage; + + stateEstimation work; + + uint32_t ins_last_time; + bool inited; +}; // Private variables -static EKFConfigurationData ekfConfiguration; -static HomeLocationData homeLocation; - static bool initialized = 0; -static bool first_run = 1; -static bool usePos = 0; // Private functions -static int32_t init13i(void); -static int32_t init13(void); -static int32_t maininit(void); -static int32_t filter(stateEstimation *state); -static inline bool invalid(float data); +static int32_t init13i(stateFilter *self); +static int32_t init13(stateFilter *self); +static int32_t maininit(stateFilter *self); +static int32_t filter(stateFilter *self, stateEstimation *state); static inline bool invalid_var(float data); static void globalInit(void); @@ -77,73 +85,90 @@ static void globalInit(void) } } -void filterEKF13iInitialize(stateFilter *handle) +int32_t filterEKF13iInitialize(stateFilter *handle) { globalInit(); - handle->init = &init13i; - handle->filter = &filter; + handle->init = &init13i; + handle->filter = &filter; + handle->localdata = pvPortMalloc(sizeof(struct data)); + return STACK_REQUIRED; } -void filterEKF13Initialize(stateFilter *handle) +int32_t filterEKF13Initialize(stateFilter *handle) { globalInit(); - handle->init = &init13; - handle->filter = &filter; + handle->init = &init13; + handle->filter = &filter; + handle->localdata = pvPortMalloc(sizeof(struct data)); + return STACK_REQUIRED; } // XXX // TODO: Until the 16 state EKF is implemented, run 13 state, so compilation runs through // XXX -void filterEKF16iInitialize(stateFilter *handle) +int32_t filterEKF16iInitialize(stateFilter *handle) { globalInit(); - handle->init = &init13i; - handle->filter = &filter; + handle->init = &init13i; + handle->filter = &filter; + handle->localdata = pvPortMalloc(sizeof(struct data)); + return STACK_REQUIRED; } -void filterEKF16Initialize(stateFilter *handle) +int32_t filterEKF16Initialize(stateFilter *handle) { globalInit(); - handle->init = &init13; - handle->filter = &filter; + handle->init = &init13; + handle->filter = &filter; + handle->localdata = pvPortMalloc(sizeof(struct data)); + return STACK_REQUIRED; } -static int32_t init13i(void) +static int32_t init13i(stateFilter *self) { - usePos = 0; - return maininit(); + struct data *this = (struct data *)self->localdata; + + this->usePos = 0; + return maininit(self); } -static int32_t init13(void) +static int32_t init13(stateFilter *self) { - usePos = 1; - return maininit(); + struct data *this = (struct data *)self->localdata; + + this->usePos = 1; + return maininit(self); } -static int32_t maininit(void) +static int32_t maininit(stateFilter *self) { - first_run = 1; + struct data *this = (struct data *)self->localdata; - EKFConfigurationGet(&ekfConfiguration); + this->inited = false; + this->init_stage = 0; + this->work.updated = 0; + this->ins_last_time = PIOS_DELAY_GetRaw(); + + EKFConfigurationGet(&this->ekfConfiguration); int t; // plausibility check for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { - if (invalid_var(ekfConfiguration.P[t])) { + if (invalid_var(this->ekfConfiguration.P[t])) { return 2; } } for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { - if (invalid_var(ekfConfiguration.Q[t])) { + if (invalid_var(this->ekfConfiguration.Q[t])) { return 2; } } for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) { - if (invalid_var(ekfConfiguration.R[t])) { + if (invalid_var(this->ekfConfiguration.R[t])) { return 2; } } - HomeLocationGet(&homeLocation); + HomeLocationGet(&this->homeLocation); // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily // switching between indoor and outdoor mode with Set = false) - if ((homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f)) { + if ((this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2] < 1e-5f)) { return 2; } @@ -154,14 +179,9 @@ static int32_t maininit(void) /** * Collect all required state variables, then run complementary filter */ -static int32_t filter(stateEstimation *state) +static int32_t filter(stateFilter *self, stateEstimation *state) { - static int32_t init_stage; - - static stateEstimation work; - - static uint32_t ins_last_time = 0; - static bool inited; + struct data *this = (struct data *)self->localdata; const float zeros[3] = { 0.0f, 0.0f, 0.0f }; @@ -169,58 +189,52 @@ static int32_t filter(stateEstimation *state) float dT; uint16_t sensors = 0; - if (inited) { - work.updated = 0; - } - - if (first_run) { - first_run = false; - inited = false; - init_stage = 0; - - work.updated = 0; - - ins_last_time = PIOS_DELAY_GetRaw(); - - return 1; - } - - work.updated |= state->updated; + this->work.updated |= state->updated; // Get most recent data #define UPDATE(shortname, num) \ - if (ISSET(state->updated, shortname##_UPDATED)) { \ + if (ISSET(state->updated, SENSORUPDATES_##shortname)) { \ uint8_t t; \ for (t = 0; t < num; t++) { \ - work.shortname[t] = state->shortname[t]; \ + this->work.shortname[t] = state->shortname[t]; \ } \ } - UPDATE(gyr, 3); - UPDATE(acc, 3); + UPDATE(gyro, 3); + UPDATE(accel, 3); UPDATE(mag, 3); - UPDATE(bar, 1); + UPDATE(baro, 1); UPDATE(pos, 3); UPDATE(vel, 3); - UPDATE(air, 2); + UPDATE(airspeed, 2); + // check whether mandatory updates are present accels must have been supplied already, + // and gyros must be supplied just now for a prediction step to take place + // ("gyros last" rule for multi object synchronization) + if (!(ISSET(this->work.updated, SENSORUPDATES_accel) && ISSET(state->updated, SENSORUPDATES_gyro))) { + UNSET(state->updated, SENSORUPDATES_pos); + UNSET(state->updated, SENSORUPDATES_vel); + UNSET(state->updated, SENSORUPDATES_attitude); + UNSET(state->updated, SENSORUPDATES_gyro); + return 0; + } - if (usePos) { + if (this->usePos) { GPSPositionData gpsData; GPSPositionGet(&gpsData); // Have a minimum requirement for gps usage if ((gpsData.Satellites < 7) || (gpsData.PDOP > 4.0f) || (gpsData.Latitude == 0 && gpsData.Longitude == 0) || - (homeLocation.Set != HOMELOCATION_SET_TRUE)) { - UNSET(state->updated, pos_UPDATED); - UNSET(state->updated, vel_UPDATED); - UNSET(work.updated, pos_UPDATED); - UNSET(work.updated, vel_UPDATED); + (this->homeLocation.Set != HOMELOCATION_SET_TRUE)) { + UNSET(state->updated, SENSORUPDATES_pos); + UNSET(state->updated, SENSORUPDATES_vel); + UNSET(this->work.updated, SENSORUPDATES_pos); + UNSET(this->work.updated, SENSORUPDATES_vel); } } - dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; - ins_last_time = PIOS_DELAY_GetRaw(); + dT = PIOS_DELAY_DiffuS(this->ins_last_time) / 1.0e6f; + this->ins_last_time = PIOS_DELAY_GetRaw(); // This should only happen at start up or at mode switches if (dT > 0.01f) { @@ -229,28 +243,28 @@ static int32_t filter(stateEstimation *state) dT = 0.001f; } - if (!inited && ISSET(work.updated, mag_UPDATED) && ISSET(work.updated, bar_UPDATED) && ISSET(work.updated, pos_UPDATED)) { + if (!this->inited && ISSET(this->work.updated, SENSORUPDATES_mag) && ISSET(this->work.updated, SENSORUPDATES_baro) && ISSET(this->work.updated, SENSORUPDATES_pos)) { // Don't initialize until all sensors are read - if (init_stage == 0) { + if (this->init_stage == 0) { // Reset the INS algorithm INSGPSInit(); - INSSetMagVar((float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_MAGX], - ekfConfiguration.R[EKFCONFIGURATION_R_MAGY], - ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] } + INSSetMagVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGX], + this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY], + this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] } ); - INSSetAccelVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX], - ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY], - ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } + INSSetAccelVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX], + this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY], + this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } ); - INSSetGyroVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } + INSSetGyroVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX], + this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY], + this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } ); - INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } + INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX], + this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY], + this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } ); - INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]); + INSSetBaroVar(this->ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]); // Initialize the gyro bias float gyro_bias[3] = { 0.0f, 0.0f, 0.0f }; @@ -261,15 +275,15 @@ static int32_t filter(stateEstimation *state) // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, // so pseudo "north" vector can be estimated even if the board is not level - attitudeState.Roll = atan2f(-work.acc[1], -work.acc[2]); - float zn = cosf(attitudeState.Roll) * work.mag[2] + sinf(attitudeState.Roll) * work.mag[1]; - float yn = cosf(attitudeState.Roll) * work.mag[1] - sinf(attitudeState.Roll) * work.mag[2]; + attitudeState.Roll = atan2f(-this->work.accel[1], -this->work.accel[2]); + float zn = cosf(attitudeState.Roll) * this->work.mag[2] + sinf(attitudeState.Roll) * this->work.mag[1]; + float yn = cosf(attitudeState.Roll) * this->work.mag[1] - sinf(attitudeState.Roll) * this->work.mag[2]; // rotate accels z vector according to roll - float azn = cosf(attitudeState.Roll) * work.acc[2] + sinf(attitudeState.Roll) * work.acc[1]; - attitudeState.Pitch = atan2f(work.acc[0], -azn); + float azn = cosf(attitudeState.Roll) * this->work.accel[2] + sinf(attitudeState.Roll) * this->work.accel[1]; + attitudeState.Pitch = atan2f(this->work.accel[0], -azn); - float xn = cosf(attitudeState.Pitch) * work.mag[0] + sinf(attitudeState.Pitch) * zn; + float xn = cosf(attitudeState.Pitch) * this->work.mag[0] + sinf(attitudeState.Pitch) * zn; attitudeState.Yaw = atan2f(-yn, xn); // TODO: This is still a hack @@ -282,122 +296,125 @@ static int32_t filter(stateEstimation *state) attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); - RPY2Quaternion(&attitudeState.Roll, work.att); + RPY2Quaternion(&attitudeState.Roll, this->work.attitude); - INSSetState(work.pos, (float *)zeros, work.att, (float *)zeros, (float *)zeros); + INSSetState(this->work.pos, (float *)zeros, this->work.attitude, (float *)zeros, (float *)zeros); - INSResetP(ekfConfiguration.P); + INSResetP(this->ekfConfiguration.P); } else { // Run prediction a bit before any corrections - float gyros[3] = { DEG2RAD(work.gyr[0]), DEG2RAD(work.gyr[1]), DEG2RAD(work.gyr[2]) }; - INSStatePrediction(gyros, work.acc, dT); + float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) }; + INSStatePrediction(gyros, this->work.accel, dT); - state->att[0] = Nav.q[0]; - state->att[1] = Nav.q[1]; - state->att[2] = Nav.q[2]; - state->att[3] = Nav.q[3]; - state->gyr[0] += Nav.gyro_bias[0]; - state->gyr[1] += Nav.gyro_bias[1]; - state->gyr[2] += Nav.gyro_bias[2]; + // Copy the attitude into the state + // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true + state->attitude[0] = Nav.q[0]; + state->attitude[1] = Nav.q[1]; + state->attitude[2] = Nav.q[2]; + state->attitude[3] = Nav.q[3]; + state->gyro[0] += Nav.gyro_bias[0]; + state->gyro[1] += Nav.gyro_bias[1]; + state->gyro[2] += Nav.gyro_bias[2]; state->pos[0] = Nav.Pos[0]; state->pos[1] = Nav.Pos[1]; state->pos[2] = Nav.Pos[2]; state->vel[0] = Nav.Vel[0]; state->vel[1] = Nav.Vel[1]; state->vel[2] = Nav.Vel[2]; - state->updated |= att_UPDATED | pos_UPDATED | vel_UPDATED | gyr_UPDATED; + state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel; } - init_stage++; - if (init_stage > 10) { - inited = true; + this->init_stage++; + if (this->init_stage > 10) { + this->inited = true; } return 0; } - if (!inited) { + if (!this->inited) { return 1; } - float gyros[3] = { DEG2RAD(work.gyr[0]), DEG2RAD(work.gyr[1]), DEG2RAD(work.gyr[2]) }; + float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) }; // Advance the state estimate - INSStatePrediction(gyros, work.acc, dT); + INSStatePrediction(gyros, this->work.accel, dT); - // Copy the attitude into the UAVO - state->att[0] = Nav.q[0]; - state->att[1] = Nav.q[1]; - state->att[2] = Nav.q[2]; - state->att[3] = Nav.q[3]; - state->gyr[0] += Nav.gyro_bias[0]; - state->gyr[1] += Nav.gyro_bias[1]; - state->gyr[2] += Nav.gyro_bias[2]; + // Copy the attitude into the state + // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true + state->attitude[0] = Nav.q[0]; + state->attitude[1] = Nav.q[1]; + state->attitude[2] = Nav.q[2]; + state->attitude[3] = Nav.q[3]; + state->gyro[0] += Nav.gyro_bias[0]; + state->gyro[1] += Nav.gyro_bias[1]; + state->gyro[2] += Nav.gyro_bias[2]; state->pos[0] = Nav.Pos[0]; state->pos[1] = Nav.Pos[1]; state->pos[2] = Nav.Pos[2]; state->vel[0] = Nav.Vel[0]; state->vel[1] = Nav.Vel[1]; state->vel[2] = Nav.Vel[2]; - state->updated |= att_UPDATED | pos_UPDATED | vel_UPDATED | gyr_UPDATED; + state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel; // Advance the covariance estimate INSCovariancePrediction(dT); - if (ISSET(work.updated, mag_UPDATED)) { + if (ISSET(this->work.updated, SENSORUPDATES_mag)) { sensors |= MAG_SENSORS; } - if (ISSET(work.updated, bar_UPDATED)) { + if (ISSET(this->work.updated, SENSORUPDATES_baro)) { sensors |= BARO_SENSOR; } - INSSetMagNorth(homeLocation.Be); + INSSetMagNorth(this->homeLocation.Be); - if (!usePos) { + if (!this->usePos) { // position and velocity variance used in indoor mode - INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, - (float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] } + INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, + (float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], + this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], + this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] } ); } else { // position and velocity variance used in outdoor mode - INSSetPosVelVar((float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] }, - (float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] } + INSSetPosVelVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH], + this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST], + this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] }, + (float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH], + this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST], + this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] } ); } - if (ISSET(work.updated, pos_UPDATED)) { + if (ISSET(this->work.updated, SENSORUPDATES_pos)) { sensors |= POS_SENSORS; } - if (ISSET(work.updated, vel_UPDATED)) { + if (ISSET(this->work.updated, SENSORUPDATES_vel)) { sensors |= HORIZ_SENSORS | VERT_SENSORS; } - if (ISSET(work.updated, air_UPDATED) && ((!ISSET(work.updated, vel_UPDATED) && !ISSET(work.updated, pos_UPDATED)) | !usePos)) { + if (ISSET(this->work.updated, SENSORUPDATES_airspeed) && ((!ISSET(this->work.updated, SENSORUPDATES_vel) && !ISSET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) { // HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance sensors |= HORIZ_SENSORS | VERT_SENSORS; - INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, - (float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] } + INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], + this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, + (float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], + this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], + this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] } ); // rotate airspeed vector into NED frame - airspeed is measured in X axis only float R[3][3]; Quaternion2R(Nav.q, R); - float vtas[3] = { work.air[1], 0.0f, 0.0f }; - rot_mult(R, vtas, work.vel); + float vtas[3] = { this->work.airspeed[1], 0.0f, 0.0f }; + rot_mult(R, vtas, this->work.vel); } /* @@ -405,7 +422,7 @@ static int32_t filter(stateEstimation *state) * although probably should occur within INS itself */ if (sensors) { - INSCorrection(work.mag, work.pos, work.vel, work.bar[0], sensors); + INSCorrection(this->work.mag, this->work.pos, this->work.vel, this->work.baro[0], sensors); } EKFStateVarianceData vardata; @@ -413,22 +430,16 @@ static int32_t filter(stateEstimation *state) INSGetP(vardata.P); EKFStateVarianceSet(&vardata); - return 0; -} + // all sensor data has been used, reset! + this->work.updated = 0; -// check for invalid values -static inline bool invalid(float data) -{ - if (isnan(data) || isinf(data)) { - return true; - } - return false; + return 0; } // check for invalid variance values static inline bool invalid_var(float data) { - if (invalid(data)) { + if (isnan(data) || isinf(data)) { return true; } if (data < 1e-15f) { // var should not be close to zero. And not negative either. diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index a54e99257..ceec76747 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -38,41 +38,50 @@ #include // Private constants +// +#define STACK_REQUIRED 256 // Private types +struct data { + HomeLocationData homeLocation; + RevoCalibrationData revoCalibration; + float magBias[3]; +}; // Private variables -static HomeLocationData homeLocation; -static RevoCalibrationData revoCalibration; -static float magBias[3] = { 0 }; - // Private functions -static int32_t init(void); -static int32_t filter(stateEstimation *state); -static void magOffsetEstimation(float mag[3]); +static int32_t init(stateFilter *self); +static int32_t filter(stateFilter *self, stateEstimation *state); +static void magOffsetEstimation(struct data *this, float mag[3]); -void filterMagInitialize(stateFilter *handle) +int32_t filterMagInitialize(stateFilter *handle) { - handle->init = &init; - handle->filter = &filter; + handle->init = &init; + handle->filter = &filter; + handle->localdata = pvPortMalloc(sizeof(struct data)); + return STACK_REQUIRED; } -static int32_t init(void) +static int32_t init(stateFilter *self) { - magBias[0] = magBias[1] = magBias[2] = 0.0f; - HomeLocationGet(&homeLocation); - RevoCalibrationGet(&revoCalibration); + struct data *this = (struct data *)self->localdata; + + this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f; + HomeLocationGet(&this->homeLocation); + RevoCalibrationGet(&this->revoCalibration); return 0; } -static int32_t filter(stateEstimation *state) +static int32_t filter(stateFilter *self, stateEstimation *state) { - if (ISSET(state->updated, mag_UPDATED)) { - if (revoCalibration.MagBiasNullingRate > 0) { - magOffsetEstimation(state->mag); + struct data *this = (struct data *)self->localdata; + + if (ISSET(state->updated, SENSORUPDATES_mag)) { + if (this->revoCalibration.MagBiasNullingRate > 0) { + magOffsetEstimation(this, state->mag); } } @@ -84,7 +93,7 @@ static int32_t filter(stateEstimation *state) * Magmeter Offset Cancellation: Theory and Implementation, * revisited William Premerlani, October 14, 2011 */ -static void magOffsetEstimation(float mag[3]) +static void magOffsetEstimation(struct data *this, float mag[3]) { #if 0 // Constants, to possibly go into a UAVO @@ -127,10 +136,10 @@ static void magOffsetEstimation(float mag[3]) } #else // if 0 - const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]); - const float Rz = homeLocation.Be[2]; + const float Rxy = sqrtf(this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1]); + const float Rz = this->homeLocation.Be[2]; - const float rate = revoCalibration.MagBiasNullingRate; + const float rate = this->revoCalibration.MagBiasNullingRate; float Rot[3][3]; float B_e[3]; float xy[2]; @@ -162,15 +171,15 @@ static void magOffsetEstimation(float mag[3]) if (!isnan(delta[0]) && !isinf(delta[0]) && !isnan(delta[1]) && !isinf(delta[1]) && !isnan(delta[2]) && !isinf(delta[2])) { - magBias[0] += delta[0]; - magBias[1] += delta[1]; - magBias[2] += delta[2]; + this->magBias[0] += delta[0]; + this->magBias[1] += delta[1]; + this->magBias[2] += delta[2]; } // Add bias to state estimation - mag[0] += magBias[0]; - mag[1] += magBias[1]; - mag[2] += magBias[2]; + mag[0] += this->magBias[0]; + mag[1] += this->magBias[1]; + mag[2] += this->magBias[2]; #endif // if 0 } diff --git a/flight/modules/StateEstimation/filterstationary.c b/flight/modules/StateEstimation/filterstationary.c index b33e3b9f7..14075fd1c 100644 --- a/flight/modules/StateEstimation/filterstationary.c +++ b/flight/modules/StateEstimation/filterstationary.c @@ -33,38 +33,42 @@ // Private constants +#define STACK_REQUIRED 64 + // Private types // Private variables // Private functions -static int32_t init(void); -static int32_t filter(stateEstimation *state); +static int32_t init(stateFilter *self); +static int32_t filter(stateFilter *self, stateEstimation *state); -void filterStationaryInitialize(stateFilter *handle) +int32_t filterStationaryInitialize(stateFilter *handle) { - handle->init = &init; - handle->filter = &filter; + handle->init = &init; + handle->filter = &filter; + handle->localdata = NULL; + return STACK_REQUIRED; } -static int32_t init(void) +static int32_t init(__attribute__((unused)) stateFilter *self) { return 0; } -static int32_t filter(stateEstimation *state) +static int32_t filter(__attribute__((unused)) stateFilter *self, stateEstimation *state) { state->pos[0] = 0.0f; state->pos[1] = 0.0f; state->pos[2] = 0.0f; - state->updated |= pos_UPDATED; + state->updated |= SENSORUPDATES_pos; state->vel[0] = 0.0f; state->vel[1] = 0.0f; state->vel[2] = 0.0f; - state->updated |= vel_UPDATED; + state->updated |= SENSORUPDATES_vel; return 0; } diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 2c039dd8d..dd8247d71 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -42,7 +42,6 @@ #include #include #include -#include #include #include #include @@ -55,7 +54,7 @@ #include "CoordinateConversions.h" // Private constants -#define STACK_SIZE_BYTES 2048 +#define STACK_SIZE_BYTES 256 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR #define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL #define TIMEOUT_MS 100 @@ -185,7 +184,7 @@ static void StateEstimationCb(void); static void getNED(GPSPositionData *gpsPosition, float *NED); static bool sane(float value); -static inline int32_t maxint32_t(int32_t a, int2_t b) +static inline int32_t maxint32_t(int32_t a, int32_t b) { if (a > b) { return a; @@ -212,7 +211,6 @@ int32_t StateEstimationInitialize(void) GyroStateInitialize(); AccelStateInitialize(); MagStateInitialize(); - BaroStateInitialize(); AirspeedStateInitialize(); PositionStateInitialize(); VelocityStateInitialize(); @@ -316,7 +314,7 @@ static void StateEstimationCb(void) current = (filterPipeline *)newFilterChain; bool error = 0; while (current != NULL) { - int32_t result = current->filter->init(); + int32_t result = current->filter->init((stateFilter *)current->filter); if (result != 0) { error = 1; break; @@ -340,7 +338,7 @@ static void StateEstimationCb(void) // most sensors get only rudimentary sanity checks #define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \ - if (ISSET(states.updated, shortname##_UPDATED)) { \ + if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ sensorname##Get(&s); \ if (sane(s.a1) && sane(s.a2) && sane(s.a3)) { \ @@ -349,36 +347,36 @@ static void StateEstimationCb(void) states.shortname[2] = s.a3; \ } \ else { \ - UNSET(states.updated, shortname##_UPDATED); \ + UNSET(states.updated, SENSORUPDATES_##shortname); \ } \ } - SANITYCHECK3(GyroSensor, gyr, x, y, z); - SANITYCHECK3(AccelSensor, acc, x, y, z); + SANITYCHECK3(GyroSensor, gyro, x, y, z); + SANITYCHECK3(AccelSensor, accel, x, y, z); SANITYCHECK3(MagSensor, mag, x, y, z); SANITYCHECK3(GPSVelocity, vel, North, East, Down); #define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \ - if (ISSET(states.updated, shortname##_UPDATED)) { \ + if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ sensorname##Get(&s); \ if (sane(s.a1) && EXTRACHECK) { \ states.shortname[0] = s.a1; \ } \ else { \ - UNSET(states.updated, shortname##_UPDATED); \ + UNSET(states.updated, SENSORUPDATES_##shortname); \ } \ } - SANITYCHECK1(BaroSensor, bar, Altitude, 1); - SANITYCHECK1(AirspeedSensor, air, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); - states.air[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter + SANITYCHECK1(BaroSensor, baro, Altitude, 1); + SANITYCHECK1(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); + states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter // GPS is a tiny bit more tricky as GPSPosition is not float (otherwise the conversion to NED could sit in a filter) but integers, for precision reasons - if (ISSET(states.updated, pos_UPDATED)) { + if (ISSET(states.updated, SENSORUPDATES_pos)) { GPSPositionData s; GPSPositionGet(&s); if (homeLocation.Set == HOMELOCATION_SET_TRUE && sane(s.Latitude) && sane(s.Longitude) && sane(s.Altitude) && (fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f)) { getNED(&s, states.pos); } else { - UNSET(states.updated, pos_UPDATED); + UNSET(states.updated, SENSORUPDATES_pos); } } @@ -395,7 +393,7 @@ static void StateEstimationCb(void) case RUNSTATE_FILTER: if (current != NULL) { - int32_t result = current->filter->filter(&states); + int32_t result = current->filter->filter((stateFilter *)current->filter, &states); if (result > alarm) { alarm = result; } @@ -413,7 +411,7 @@ static void StateEstimationCb(void) // the final output of filters is saved in state variables #define STORE3(statename, shortname, a1, a2, a3) \ - if (ISSET(states.updated, shortname##_UPDATED)) { \ + if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \ statename##Data s; \ statename##Get(&s); \ s.a1 = states.shortname[0]; \ @@ -421,36 +419,28 @@ static void StateEstimationCb(void) s.a3 = states.shortname[2]; \ statename##Set(&s); \ } - STORE3(GyroState, gyr, x, y, z); - STORE3(AccelState, acc, x, y, z); + STORE3(GyroState, gyro, x, y, z); + STORE3(AccelState, accel, x, y, z); STORE3(MagState, mag, x, y, z); STORE3(PositionState, pos, North, East, Down); STORE3(VelocityState, vel, North, East, Down); #define STORE2(statename, shortname, a1, a2) \ - if (ISSET(states.updated, shortname##_UPDATED)) { \ + if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \ statename##Data s; \ statename##Get(&s); \ s.a1 = states.shortname[0]; \ s.a2 = states.shortname[1]; \ statename##Set(&s); \ } - STORE2(AirspeedState, air, CalibratedAirspeed, TrueAirspeed); -#define STORE1(statename, shortname, a1) \ - if (ISSET(states.updated, shortname##_UPDATED)) { \ - statename##Data s; \ - statename##Get(&s); \ - s.a1 = states.shortname[0]; \ - statename##Set(&s); \ - } - STORE1(BaroState, bar, Altitude); + STORE2(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed); // attitude nees manual conversion from quaternion to euler - if (ISSET(states.updated, att_UPDATED)) { \ + if (ISSET(states.updated, SENSORUPDATES_attitude)) { \ AttitudeStateData s; AttitudeStateGet(&s); - s.q1 = states.att[0]; - s.q2 = states.att[1]; - s.q3 = states.att[2]; - s.q4 = states.att[3]; + s.q1 = states.attitude[0]; + s.q2 = states.attitude[1]; + s.q3 = states.attitude[2]; + s.q4 = states.attitude[3]; Quaternion2RPY(&s.q1, &s.Roll); AttitudeStateSet(&s); } @@ -528,31 +518,31 @@ static void sensorUpdatedCb(UAVObjEvent *ev) } if (ev->obj == GyroSensorHandle()) { - updatedSensors |= gyr_UPDATED; + updatedSensors |= SENSORUPDATES_gyro; } if (ev->obj == AccelSensorHandle()) { - updatedSensors |= acc_UPDATED; + updatedSensors |= SENSORUPDATES_accel; } if (ev->obj == MagSensorHandle()) { - updatedSensors |= mag_UPDATED; + updatedSensors |= SENSORUPDATES_mag; } if (ev->obj == GPSPositionHandle()) { - updatedSensors |= pos_UPDATED; + updatedSensors |= SENSORUPDATES_pos; } if (ev->obj == GPSVelocityHandle()) { - updatedSensors |= vel_UPDATED; + updatedSensors |= SENSORUPDATES_vel; } if (ev->obj == BaroSensorHandle()) { - updatedSensors |= bar_UPDATED; + updatedSensors |= SENSORUPDATES_baro; } if (ev->obj == AirspeedSensorHandle()) { - updatedSensors |= air_UPDATED; + updatedSensors |= SENSORUPDATES_airspeed; } DelayedCallbackDispatch(stateEstimationCallback); diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 820187757..1ee34b95d 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -33,7 +33,6 @@ UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor -UAVOBJSRCFILENAMES += barostate UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedstate diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index d516cada5..5742838ba 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -38,7 +38,6 @@ UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += barosensor -UAVOBJSRCFILENAMES += barostate UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedstate diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index dbaf20e87..5134a536d 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -38,7 +38,6 @@ UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor -UAVOBJSRCFILENAMES += barostate UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedstate diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index d588782cf..4ff9d4050 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -25,7 +25,6 @@ OTHER_FILES += UAVObjects.pluginspec # Add in all of the synthetic/generated uavobject files HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/barosensor.h \ - $$UAVOBJECT_SYNTHETICS/barostate.h \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.h \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.h \ $$UAVOBJECT_SYNTHETICS/airspeedstate.h \ @@ -110,7 +109,6 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/barosensor.cpp \ - $$UAVOBJECT_SYNTHETICS/barostate.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \ diff --git a/shared/uavobjectdefinition/barostate.xml b/shared/uavobjectdefinition/barostate.xml deleted file mode 100644 index 56a331776..000000000 --- a/shared/uavobjectdefinition/barostate.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - The filtered altitude estimate. - - - - - - - From 9b95af200645273d15420c9cf76e6f154e036860 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 22 May 2013 21:45:06 +0200 Subject: [PATCH 28/61] refaktored GPS Sensor UAVObjects --- .../Airspeed/revolution/gps_airspeed.c | 10 +-- flight/modules/Attitude/revolution/attitude.c | 40 +++++----- .../fixedwingpathfollower.c | 2 - flight/modules/GPS/GPS.c | 75 ++++++++++++++----- flight/modules/GPS/NMEA.c | 60 +++++++-------- flight/modules/GPS/UBX.c | 32 ++++---- flight/modules/GPS/inc/GPS.h | 4 +- flight/modules/GPS/inc/NMEA.h | 4 +- flight/modules/GPS/inc/UBX.h | 6 +- flight/modules/Osd/OsdEtStd/OsdEtStd.c | 12 +-- flight/modules/Osd/osdgen/osdgen.c | 12 +-- flight/modules/Sensors/simulated/sensors.c | 44 +++++------ flight/modules/StateEstimation/filterekf.c | 6 +- .../modules/StateEstimation/stateestimation.c | 28 +++---- .../VtolPathFollower/vtolpathfollower.c | 16 ++-- .../boards/coptercontrol/firmware/Makefile | 4 +- flight/targets/boards/osd/firmware/Makefile | 4 +- .../boards/revolution/firmware/UAVObjects.inc | 5 +- .../revolution/firmware/inc/pios_config.h | 1 + .../revolution/firmware/pios_board_sim.c | 4 +- .../boards/revoproto/firmware/UAVObjects.inc | 5 +- .../revoproto/firmware/inc/pios_config.h | 1 + .../revoproto/firmware/pios_board_sim.c | 4 +- .../boards/simposix/firmware/UAVObjects.inc | 5 +- .../default_configurations/Developer.xml | 10 +-- .../default_configurations/OpenPilotGCS.xml | 10 +-- .../OpenPilotGCS_wide.xml | 10 +-- .../pfd/default/PfdIndicators.qml | 4 +- .../pfd/default/PfdTerrainView.qml | 6 +- .../translations/openpilotgcs_de.ts | 2 +- .../translations/openpilotgcs_es.ts | 2 +- .../translations/openpilotgcs_fr.ts | 2 +- .../translations/openpilotgcs_ru.ts | 2 +- .../translations/openpilotgcs_zh_CN.ts | 2 +- .../plugins/antennatrack/telemetryparser.cpp | 4 +- .../plugins/gpsdisplay/telemetryparser.cpp | 4 +- .../src/plugins/hitl/hitlnoisegeneration.h | 20 ++--- .../src/plugins/hitl/simulator.cpp | 14 ++-- .../openpilotgcs/src/plugins/hitl/simulator.h | 8 +- .../src/plugins/opmap/opmapgadgetwidget.cpp | 10 +-- .../src/plugins/opmap/opmapgadgetwidget.h | 2 +- .../plugins/osgearthview/osgviewerwidget.cpp | 6 +- .../src/plugins/pfd/pfdgadgetwidget.cpp | 4 +- .../src/plugins/pfdqml/pfdqmlgadgetwidget.cpp | 2 +- .../plugins/qmlview/qmlviewgadgetwidget.cpp | 2 +- .../src/plugins/uavobjects/OPPlots.m | 14 ++-- .../src/plugins/uavobjects/uavobjects.pro | 10 ++- .../uavobjectutil/uavobjectutilmanager.cpp | 8 +- .../uavobjectutil/uavobjectutilmanager.h | 2 +- ...{gpsposition.xml => gpspositionsensor.xml} | 2 +- ...{gpsvelocity.xml => gpsvelocitysensor.xml} | 2 +- 51 files changed, 295 insertions(+), 253 deletions(-) rename shared/uavobjectdefinition/{gpsposition.xml => gpspositionsensor.xml} (94%) rename shared/uavobjectdefinition/{gpsvelocity.xml => gpsvelocitysensor.xml} (87%) diff --git a/flight/modules/Airspeed/revolution/gps_airspeed.c b/flight/modules/Airspeed/revolution/gps_airspeed.c index 6a0d665ec..7f63f622f 100644 --- a/flight/modules/Airspeed/revolution/gps_airspeed.c +++ b/flight/modules/Airspeed/revolution/gps_airspeed.c @@ -32,7 +32,7 @@ #include "openpilot.h" #include "gps_airspeed.h" -#include "gpsvelocity.h" +#include "gpsvelocitysensor.h" #include "attitudestate.h" #include "CoordinateConversions.h" #include @@ -66,8 +66,8 @@ void gps_airspeedInitialize() gps = (struct GPSGlobals *)pvPortMalloc(sizeof(struct GPSGlobals)); // GPS airspeed calculation variables - GPSVelocityData gpsVelData; - GPSVelocityGet(&gpsVelData); + GPSVelocitySensorData gpsVelData; + GPSVelocitySensorGet(&gpsVelData); gps->gpsVelOld_N = gpsVelData.North; gps->gpsVelOld_E = gpsVelData.East; @@ -115,8 +115,8 @@ void gps_airspeedGet(float *v_air_GPS) // If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue. if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) { - GPSVelocityData gpsVelData; - GPSVelocityGet(&gpsVelData); + GPSVelocitySensorData gpsVelData; + GPSVelocitySensorGet(&gpsVelData); // Calculate the norm^2 of the difference between the two GPS vectors float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f); diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index c8058d3cf..da80f5ca4 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -59,8 +59,8 @@ #include "attitudesettings.h" #include "barosensor.h" #include "flightstatus.h" -#include "gpsposition.h" -#include "gpsvelocity.h" +#include "gpspositionsensor.h" +#include "gpsvelocitysensor.h" #include "gyrostate.h" #include "gyrosensor.h" #include "homelocation.h" @@ -123,7 +123,7 @@ static int32_t updateAttitudeComplementary(bool first_run); static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode); static void settingsUpdatedCb(UAVObjEvent *objEv); -static int32_t getNED(GPSPositionData *gpsPosition, float *NED); +static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED); static void magOffsetEstimation(MagSensorData *mag); @@ -172,8 +172,8 @@ int32_t AttitudeInitialize(void) AirspeedSensorInitialize(); AirspeedStateInitialize(); BaroSensorInitialize(); - GPSPositionInitialize(); - GPSVelocityInitialize(); + GPSPositionSensorInitialize(); + GPSVelocitySensorInitialize(); AttitudeSettingsInitialize(); AttitudeStateInitialize(); PositionStateInitialize(); @@ -233,8 +233,8 @@ int32_t AttitudeStart(void) MagSensorConnectQueue(magQueue); AirspeedSensorConnectQueue(airspeedQueue); BaroSensorConnectQueue(baroQueue); - GPSPositionConnectQueue(gpsQueue); - GPSVelocityConnectQueue(gpsVelQueue); + GPSPositionSensorConnectQueue(gpsQueue); + GPSVelocitySensorConnectQueue(gpsVelQueue); return 0; } @@ -541,8 +541,8 @@ static int32_t updateAttitudeComplementary(bool first_run) if (xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE) { float NED[3]; // Transform the GPS position into NED coordinates - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); + GPSPositionSensorData gpsPosition; + GPSPositionSensorGet(&gpsPosition); getNED(&gpsPosition, NED); PositionStateData positionState; @@ -555,8 +555,8 @@ static int32_t updateAttitudeComplementary(bool first_run) if (xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE) { // Transform the GPS position into NED coordinates - GPSVelocityData gpsVelocity; - GPSVelocityGet(&gpsVelocity); + GPSVelocitySensorData gpsVelocity; + GPSVelocitySensorGet(&gpsVelocity); VelocityStateData velocityState; VelocityStateGet(&velocityState); @@ -615,8 +615,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) MagStateData magData; AirspeedSensorData airspeedData; BaroSensorData baroData; - GPSPositionData gpsData; - GPSVelocityData gpsVelData; + GPSPositionSensorData gpsData; + GPSVelocitySensorData gpsVelData; static bool mag_updated = false; static bool baro_updated; @@ -677,13 +677,13 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; // Check if we are running simulation - if (!GPSPositionReadOnly()) { + if (!GPSPositionSensorReadOnly()) { gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; } else { gps_updated |= pdTRUE && outdoor_mode; } - if (!GPSVelocityReadOnly()) { + if (!GPSVelocitySensorReadOnly()) { gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; } else { gps_vel_updated |= pdTRUE && outdoor_mode; @@ -708,8 +708,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) BaroSensorGet(&baroData); AirspeedSensorGet(&airspeedData); - GPSPositionGet(&gpsData); - GPSVelocityGet(&gpsVelData); + GPSPositionSensorGet(&gpsData); + GPSVelocitySensorGet(&gpsVelData); // TODO: put in separate filter AccelStateData accelState; @@ -845,8 +845,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) float pos[3] = { 0.0f, 0.0f, 0.0f }; if (outdoor_mode) { - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); + GPSPositionSensorData gpsPosition; + GPSPositionSensorGet(&gpsPosition); // Transform the GPS position into NED coordinates getNED(&gpsPosition, pos); @@ -1074,7 +1074,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) * @returns 0 for success, -1 for failure */ float T[3]; -static int32_t getNED(GPSPositionData *gpsPosition, float *NED) +static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED) { float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f), DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f), diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 665496a44..438143d4f 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -53,8 +53,6 @@ #include "flightstatus.h" #include "pathstatus.h" #include "airspeedstate.h" -#include "gpsvelocity.h" -#include "gpsposition.h" #include "fixedwingpathfollowersettings.h" #include "fixedwingpathfollowerstatus.h" #include "homelocation.h" diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 0fdb9142e..6c36cb5f3 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -32,11 +32,12 @@ #include -#include "gpsposition.h" +#include "gpspositionsensor.h" #include "homelocation.h" +#include "positionsensor.h" #include "gpstime.h" #include "gpssatellites.h" -#include "gpsvelocity.h" +#include "gpsvelocitysensor.h" #include "systemsettings.h" #include "taskinfo.h" #include "hwsettings.h" @@ -56,9 +57,12 @@ static void gpsTask(void *parameters); static void updateSettings(); #ifdef PIOS_GPS_SETS_HOMELOCATION -static void setHomeLocation(GPSPositionData *gpsData); +static void setHomeLocation(GPSPositionSensorData *gpsData); static float GravityAccel(float latitude, float longitude, float altitude); #endif +#ifdef PIOS_GPS_SETS_POSITIONSENSOR +static void setPositionSensor(GPSPositionSensorData *gpsData); +#endif // **************** // Private constants @@ -147,23 +151,27 @@ int32_t GPSInitialize(void) // These objects MUST be initialized for Revolution // because the rest of the system expects to just // attach to their queues - GPSPositionInitialize(); - GPSVelocityInitialize(); + GPSPositionSensorInitialize(); + GPSVelocitySensorInitialize(); GPSTimeInitialize(); GPSSatellitesInitialize(); HomeLocationInitialize(); + PositionSensorInitialize(); updateSettings(); #else if (gpsPort && gpsEnabled) { - GPSPositionInitialize(); - GPSVelocityInitialize(); + GPSPositionSensorInitialize(); + GPSVelocitySensorInitialize(); #if !defined(PIOS_GPS_MINIMAL) GPSTimeInitialize(); GPSSatellitesInitialize(); #endif -#ifdef PIOS_GPS_SETS_HOMELOCATION +#if (defined(PIOS_GPS_SETS_HOMELOCATION) || defined(PIOS_GPS_SETS_POSITIONSENSOR)) HomeLocationInitialize(); +#endif +#ifdef PIOS_GPS_SETS_POSITIONSENSOR + PositionSensorInitialize(); #endif updateSettings(); } @@ -202,7 +210,7 @@ static void gpsTask(__attribute__((unused)) void *parameters) portTickType xDelay = 100 / portTICK_RATE_MS; uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - GPSPositionData gpsposition; + GPSPositionSensorData gpspositionsensor; uint8_t gpsProtocol; SystemSettingsGPSDataProtocolGet(&gpsProtocol); @@ -210,7 +218,7 @@ static void gpsTask(__attribute__((unused)) void *parameters) timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; - GPSPositionGet(&gpsposition); + GPSPositionSensorGet(&gpspositionsensor); // Loop forever while (1) { uint8_t c; @@ -221,12 +229,12 @@ static void gpsTask(__attribute__((unused)) void *parameters) switch (gpsProtocol) { #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA: - res = parse_nmea_stream(c, gps_rx_buffer, &gpsposition, &gpsRxStats); + res = parse_nmea_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); break; #endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX: - res = parse_ubx_stream(c, gps_rx_buffer, &gpsposition, &gpsRxStats); + res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); break; #endif default: @@ -246,25 +254,28 @@ static void gpsTask(__attribute__((unused)) void *parameters) if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) { // we have not received any valid GPS sentences for a while. // either the GPS is not plugged in or a hardware problem or the GPS has locked up. - uint8_t status = GPSPOSITION_STATUS_NOGPS; - GPSPositionStatusSet(&status); + uint8_t status = GPSPOSITIONSENSOR_STATUS_NOGPS; + GPSPositionSensorStatusSet(&status); AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); } else { // we appear to be receiving GPS sentences OK, we've had an update // criteria for GPS-OK taken from this post... // http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 - if ((gpsposition.PDOP < 3.5f) && (gpsposition.Satellites >= 7) && - (gpsposition.Status == GPSPOSITION_STATUS_FIX3D)) { + if ((gpspositionsensor.PDOP < 3.5f) && (gpspositionsensor.Satellites >= 7) && + (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D)) { AlarmsClear(SYSTEMALARMS_ALARM_GPS); #ifdef PIOS_GPS_SETS_HOMELOCATION HomeLocationData home; HomeLocationGet(&home); if (home.Set == HOMELOCATION_SET_FALSE) { - setHomeLocation(&gpsposition); + setHomeLocation(&gpspositionsensor); } #endif - } else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D) { +#ifdef PIOS_GPS_SETS_POSITIONSENSOR + setPositionSensor(&gpspositionsensor); +#endif + } else if (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) { AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); @@ -292,7 +303,7 @@ static float GravityAccel(float latitude, __attribute__((unused)) float longitud // **************** -static void setHomeLocation(GPSPositionData *gpsData) +static void setHomeLocation(GPSPositionSensorData *gpsData) { HomeLocationData home; @@ -325,6 +336,32 @@ static void setHomeLocation(GPSPositionData *gpsData) } #endif /* ifdef PIOS_GPS_SETS_HOMELOCATION */ +#ifdef PIOS_GPS_SETS_POSITIONSENSOR +static void setPositionSensor(GPSPositionSensorData *gpsData) +{ + HomeLocationData home; + + HomeLocationGet(&home); + PositionSensorData pos; + PositionSensorGet(&pos); + + float ECEF[3]; + float Rne[3][3]; + { + float LLA[3] = { home.Latitude, home.Longitude, home.Altitude }; + LLA2ECEF(LLA, ECEF); + RneFromLLA(LLA, Rne); + } + { float LLA[3] = { gpsData->Latitude, gpsData->Longitude, gpsData->Altitude + gpsData->GeoidSeparation }; + float NED[3]; + LLA2Base(LLA, ECEF, Rne, NED); + pos.North = NED[0]; + pos.East = NED[1]; + pos.Down = NED[2]; } + PositionSensorSet(&pos); +} +#endif /* ifdef PIOS_GPS_SETS_POSITIONSENSOR */ + /** * Update the GPS settings, called on startup. * FIXME: This should be in the GPSSettings object. But objects have diff --git a/flight/modules/GPS/NMEA.c b/flight/modules/GPS/NMEA.c index d5be3131d..cc16b64c0 100644 --- a/flight/modules/GPS/NMEA.c +++ b/flight/modules/GPS/NMEA.c @@ -33,7 +33,7 @@ #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) -#include "gpsposition.h" +#include "gpspositionsensor.h" #include "NMEA.h" #include "gpstime.h" #include "gpssatellites.h" @@ -65,16 +65,16 @@ struct nmea_parser { const char *prefix; - bool (*handler)(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); + bool (*handler)(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); }; -static bool nmeaProcessGPGGA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); -static bool nmeaProcessGPRMC(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); -static bool nmeaProcessGPVTG(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); -static bool nmeaProcessGPGSA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +static bool nmeaProcessGPRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +static bool nmeaProcessGPVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +static bool nmeaProcessGPGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); #if !defined(PIOS_GPS_MINIMAL) -static bool nmeaProcessGPZDA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); -static bool nmeaProcessGPGSV(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +static bool nmeaProcessGPZDA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); +static bool nmeaProcessGPGSV(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam); #endif // PIOS_GPS_MINIMAL static const struct nmea_parser nmea_parsers[] = { @@ -106,7 +106,7 @@ static const struct nmea_parser nmea_parsers[] = { #endif // PIOS_GPS_MINIMAL }; -int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats) +int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats) { static uint8_t rx_count = 0; static bool start_flag = false; @@ -351,12 +351,12 @@ static bool NMEA_latlon_to_fixed_point(int32_t *latlon, char *nmea_latlon, bool /** - * Parses a complete NMEA sentence and updates the GPSPosition UAVObject + * Parses a complete NMEA sentence and updates the GPSPositionSensor UAVObject * \param[in] An NMEA sentence with a valid checksum * \return true if the sentence was successfully parsed * \return false if any errors were encountered with the parsing */ -bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) +bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData) { char *p = nmea_sentence; char *params[MAX_NB_PARAMS]; @@ -412,7 +412,7 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) #endif // Send the message to the parser and get it update the GpsData // Information from various different NMEA messages are temporarily - // cumulated in the GpsData structure. An actual GPSPosition update + // cumulated in the GpsData structure. An actual GPSPositionSensor update // is triggered by GGA messages only. This message type sets the // gpsDataUpdated flag to request this. bool gpsDataUpdated = false; @@ -420,8 +420,8 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) { // Parse failed DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]); - if (gpsDataUpdated && (GpsData->Status == GPSPOSITION_STATUS_NOFIX)) { - GPSPositionSet(GpsData); + if (gpsDataUpdated && (GpsData->Status == GPSPOSITIONSENSOR_STATUS_NOFIX)) { + GPSPositionSensorSet(GpsData); } return false; } @@ -432,7 +432,7 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) #ifdef DEBUG_MGSID_IN DEBUG_MSG("U"); #endif - GPSPositionSet(GpsData); + GPSPositionSensorSet(GpsData); } #ifdef DEBUG_MGSID_IN @@ -444,10 +444,10 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) /** * Parse an NMEA GPGGA sentence and update the given UAVObject - * \param[in] A pointer to a GPSPosition UAVObject to be updated. + * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated. * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPGGA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) +static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { if (nbParam != 15) { return false; @@ -470,7 +470,7 @@ static bool nmeaProcessGPGGA(GPSPositionData *GpsData, bool *gpsDataUpdated, cha // do this first to make sure we get this information, even if later checks exit // this function early if (param[6][0] == '0') { - GpsData->Status = GPSPOSITION_STATUS_NOFIX; // treat invalid fix as NOFIX + GpsData->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; // treat invalid fix as NOFIX } // get latitude [DDMM.mmmmm] [N|S] @@ -497,10 +497,10 @@ static bool nmeaProcessGPGGA(GPSPositionData *GpsData, bool *gpsDataUpdated, cha /** * Parse an NMEA GPRMC sentence and update the given UAVObject - * \param[in] A pointer to a GPSPosition UAVObject to be updated. + * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated. * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPRMC(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) +static bool nmeaProcessGPRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { if (nbParam != 13) { return false; @@ -565,10 +565,10 @@ static bool nmeaProcessGPRMC(GPSPositionData *GpsData, bool *gpsDataUpdated, cha /** * Parse an NMEA GPVTG sentence and update the given UAVObject - * \param[in] A pointer to a GPSPosition UAVObject to be updated. + * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated. * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPVTG(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) +static bool nmeaProcessGPVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { if (nbParam != 9 && nbParam != 10 /*GTOP GPS seems to gemnerate an extra parameter...*/) { return false; @@ -590,10 +590,10 @@ static bool nmeaProcessGPVTG(GPSPositionData *GpsData, bool *gpsDataUpdated, cha #if !defined(PIOS_GPS_MINIMAL) /** * Parse an NMEA GPZDA sentence and update the @ref GPSTime object - * \param[in] A pointer to a GPSPosition UAVObject to be updated (unused). + * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated (unused). * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPZDA(__attribute__((unused)) GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) +static bool nmeaProcessGPZDA(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { if (nbParam != 7) { return false; @@ -633,7 +633,7 @@ static uint8_t gsv_processed_mask; static uint16_t gsv_incomplete_error; static uint16_t gsv_duplicate_error; -static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) +static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { if (nbParam < 4) { return false; @@ -717,10 +717,10 @@ static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionData *GpsData, b /** * Parse an NMEA GPGSA sentence and update the given UAVObject - * \param[in] A pointer to a GPSPosition UAVObject to be updated. + * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated. * \param[in] An NMEA sentence with a valid checksum */ -static bool nmeaProcessGPGSA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) +static bool nmeaProcessGPGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam) { if (nbParam != 18) { return false; @@ -737,13 +737,13 @@ static bool nmeaProcessGPGSA(GPSPositionData *GpsData, bool *gpsDataUpdated, cha switch (atoi(param[2])) { case 1: - GpsData->Status = GPSPOSITION_STATUS_NOFIX; + GpsData->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; break; case 2: - GpsData->Status = GPSPOSITION_STATUS_FIX2D; + GpsData->Status = GPSPOSITIONSENSOR_STATUS_FIX2D; break; case 3: - GpsData->Status = GPSPOSITION_STATUS_FIX3D; + GpsData->Status = GPSPOSITIONSENSOR_STATUS_FIX3D; break; default: /* Unhandled */ diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index bf8fe8039..a58e926e4 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -38,7 +38,7 @@ // parse incoming character stream for messages in UBX binary format -int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats) +int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats) { enum proto_states { START, @@ -193,10 +193,10 @@ bool checksum_ubx_message(struct UBXPacket *ubx) } } -void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPosition) +void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionSensorData *GpsPosition) { if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) { - if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { + if (GpsPosition->Status != GPSPOSITIONSENSOR_STATUS_NOFIX) { GpsPosition->Altitude = (float)posllh->hMSL * 0.001f; GpsPosition->GeoidSeparation = (float)(posllh->height - posllh->hMSL) * 0.001f; GpsPosition->Latitude = posllh->lat; @@ -205,7 +205,7 @@ void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPos } } -void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionData *GpsPosition) +void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionSensorData *GpsPosition) { if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) { GpsPosition->Satellites = sol->numSV; @@ -213,20 +213,20 @@ void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionData *GpsPosition) if (sol->flags & STATUS_FLAGS_GPSFIX_OK) { switch (sol->gpsFix) { case STATUS_GPSFIX_2DFIX: - GpsPosition->Status = GPSPOSITION_STATUS_FIX2D; + GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_FIX2D; break; case STATUS_GPSFIX_3DFIX: - GpsPosition->Status = GPSPOSITION_STATUS_FIX3D; + GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_FIX3D; break; - default: GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; + default: GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; } } else { // fix is not valid so we make sure to treat is as NOFIX - GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; + GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; } } } -void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionData *GpsPosition) +void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionSensorData *GpsPosition) { if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) { GpsPosition->HDOP = (float)dop->hDOP * 0.01f; @@ -235,16 +235,16 @@ void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionData *GpsPosition) } } -void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionData *GpsPosition) +void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData *GpsPosition) { - GPSVelocityData GpsVelocity; + GPSVelocitySensorData GpsVelocity; if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) { - if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { + if (GpsPosition->Status != GPSPOSITIONSENSOR_STATUS_NOFIX) { GpsVelocity.North = (float)velned->velN / 100.0f; GpsVelocity.East = (float)velned->velE / 100.0f; GpsVelocity.Down = (float)velned->velD / 100.0f; - GPSVelocitySet(&GpsVelocity); + GPSVelocitySensorSet(&GpsVelocity); GpsPosition->Groundspeed = (float)velned->gSpeed * 0.01f; GpsPosition->Heading = (float)velned->heading * 1.0e-5f; } @@ -302,7 +302,7 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo) // UBX message parser // returns UAVObjectID if a UAVObject structure is ready for further processing -uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionData *GpsPosition) +uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition) { uint32_t id = 0; @@ -333,9 +333,9 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionData *GpsPosition) break; } if (msgtracker.msg_received == ALL_RECEIVED) { - GPSPositionSet(GpsPosition); + GPSPositionSensorSet(GpsPosition); msgtracker.msg_received = NONE_RECEIVED; - id = GPSPOSITION_OBJID; + id = GPSPOSITIONSENSOR_OBJID; } return id; } diff --git a/flight/modules/GPS/inc/GPS.h b/flight/modules/GPS/inc/GPS.h index ecc7470d8..6cecaa845 100644 --- a/flight/modules/GPS/inc/GPS.h +++ b/flight/modules/GPS/inc/GPS.h @@ -34,9 +34,9 @@ #ifndef GPS_H #define GPS_H -#include "gpsvelocity.h" +#include "gpsvelocitysensor.h" #include "gpssatellites.h" -#include "gpsposition.h" +#include "gpspositionsensor.h" #include "gpstime.h" #define NO_PARSER -3 // no parser available diff --git a/flight/modules/GPS/inc/NMEA.h b/flight/modules/GPS/inc/NMEA.h index a1e94a24e..04a7dc4ce 100644 --- a/flight/modules/GPS/inc/NMEA.h +++ b/flight/modules/GPS/inc/NMEA.h @@ -37,8 +37,8 @@ #define NMEA_MAX_PACKET_LENGTH 96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed -extern bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData); +extern bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData); extern bool NMEA_checksum(char *nmea_sentence); -extern int parse_nmea_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *); +extern int parse_nmea_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *); #endif /* NMEA_H */ diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 9a1c86eeb..fd1ee0fb1 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -31,7 +31,7 @@ #ifndef UBX_H #define UBX_H #include "openpilot.h" -#include "gpsposition.h" +#include "gpspositionsensor.h" #include "GPS.h" @@ -218,7 +218,7 @@ struct UBXPacket { }; bool checksum_ubx_message(struct UBXPacket *); -uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionData *); -int parse_ubx_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *); +uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *); +int parse_ubx_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *); #endif /* UBX_H */ diff --git a/flight/modules/Osd/OsdEtStd/OsdEtStd.c b/flight/modules/Osd/OsdEtStd/OsdEtStd.c index 29b04c4cc..fdf74f072 100644 --- a/flight/modules/Osd/OsdEtStd/OsdEtStd.c +++ b/flight/modules/Osd/OsdEtStd/OsdEtStd.c @@ -32,7 +32,7 @@ #include "openpilot.h" #include "flightbatterystate.h" -#include "gpsposition.h" +#include "gpspositionsensor.h" #include "attitudestate.h" #include "barosensor.h" @@ -209,7 +209,7 @@ static void FlightBatteryStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) newBattData = TRUE; } -static void GPSPositionUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +static void GPSPositionSensorUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { newPosData = TRUE; } @@ -440,17 +440,17 @@ static void Run(void) } if (newPosData) { - GPSPositionData positionData; + GPSPositionSensorData positionData; AttitudeStateData attitudeStateData; - GPSPositionGet(&positionData); + GPSPositionSensorGet(&positionData); AttitudeStateGet(&attitudeStateData); // DEBUG_MSG("%5d Pos: #stat=%d #sats=%d alt=%d\n\r", cnt, // positionData.Status, positionData.Satellites, (uint32_t)positionData.Altitude); // GPS Status - if (positionData.Status == GPSPOSITION_STATUS_FIX3D) { + if (positionData.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) { msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_FIX; } else { msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_NOFIX; @@ -543,7 +543,7 @@ static void onTimer(UAVObjEvent *ev) */ int32_t OsdEtStdInitialize(void) { - GPSPositionConnectCallback(GPSPositionUpdatedCb); + GPSPositionSensorConnectCallback(GPSPositionSensorUpdatedCb); FlightBatteryStateConnectCallback(FlightBatteryStateUpdatedCb); BaroSensorConnectCallback(BaroSensorUpdatedCb); diff --git a/flight/modules/Osd/osdgen/osdgen.c b/flight/modules/Osd/osdgen/osdgen.c index 23539bcc7..c2f2a0b18 100644 --- a/flight/modules/Osd/osdgen/osdgen.c +++ b/flight/modules/Osd/osdgen/osdgen.c @@ -35,7 +35,7 @@ #include "osdgen.h" #include "attitudestate.h" -#include "gpsposition.h" +#include "gpspositionsensor.h" #include "homelocation.h" #include "gpstime.h" #include "gpssatellites.h" @@ -2036,8 +2036,8 @@ void calcHomeArrow(int16_t m_yaw) HomeLocationData home; HomeLocationGet(&home); - GPSPositionData gpsData; - GPSPositionGet(&gpsData); + GPSPositionSensorData gpsData; + GPSPositionSensorGet(&gpsData); /** http://www.movable-type.co.uk/scripts/latlong.html **/ float lat1, lat2, lon1, lon2, a, c, d, x, y, brng, u2g; @@ -2139,8 +2139,8 @@ void updateGraphics() OsdSettingsGet(&OsdSettings); AttitudeStateData attitude; AttitudeStateGet(&attitude); - GPSPositionData gpsData; - GPSPositionGet(&gpsData); + GPSPositionSensorData gpsData; + GPSPositionSensorGet(&gpsData); HomeLocationData home; HomeLocationGet(&home); BaroSensorData baro; @@ -2421,7 +2421,7 @@ int32_t osdgenInitialize(void) { AttitudeStateInitialize(); #ifdef PIOS_INCLUDE_GPS - GPSPositionInitialize(); + GPSPositionSensorInitialize(); #if !defined(PIOS_GPS_MINIMAL) GPSTimeInitialize(); GPSSatellitesInitialize(); diff --git a/flight/modules/Sensors/simulated/sensors.c b/flight/modules/Sensors/simulated/sensors.c index b236f6cd0..dd19a6d2d 100644 --- a/flight/modules/Sensors/simulated/sensors.c +++ b/flight/modules/Sensors/simulated/sensors.c @@ -58,8 +58,8 @@ #include "barosensor.h" #include "gyrosensor.h" #include "flightstatus.h" -#include "gpsposition.h" -#include "gpsvelocity.h" +#include "gpspositionsensor.h" +#include "gpsvelocitysensor.h" #include "homelocation.h" #include "sensor.h" #include "ratedesired.h" @@ -111,8 +111,8 @@ int32_t SensorsInitialize(void) AirspeedSensorInitialize(); GyroSensorInitialize(); // GyrosBiasInitialize(); - GPSPositionInitialize(); - GPSVelocityInitialize(); + GPSPositionSensorInitialize(); + GPSVelocitySensorInitialize(); MagSensorInitialize(); MagBiasInitialize(); RevoCalibrationInitialize(); @@ -243,12 +243,12 @@ static void simulateConstant() baroSensor.Altitude = 1; BaroSensorSet(&baroSensor); - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); + GPSPositionSensorData gpsPosition; + GPSPositionSensorGet(&gpsPosition); gpsPosition.Latitude = 0; gpsPosition.Longitude = 0; gpsPosition.Altitude = 0; - GPSPositionSet(&gpsPosition); + GPSPositionSensorSet(&gpsPosition); // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) @@ -305,12 +305,12 @@ static void simulateModelAgnostic() baroSensor.Altitude = 1; BaroSensorSet(&baroSensor); - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); + GPSPositionSensorData gpsPosition; + GPSPositionSensorGet(&gpsPosition); gpsPosition.Latitude = 0; gpsPosition.Longitude = 0; gpsPosition.Altitude = 0; - GPSPositionSet(&gpsPosition); + GPSPositionSensorSet(&gpsPosition); // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) @@ -500,8 +500,8 @@ static void simulateModelQuadcopter() gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0; gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0; - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); + GPSPositionSensorData gpsPosition; + GPSPositionSensorGet(&gpsPosition); gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6); gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1]) / T[1] * 10.0e6); gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]); @@ -509,19 +509,19 @@ static void simulateModelQuadcopter() gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1], vel[0] + gps_vel_drift[0]); gpsPosition.Satellites = 7; gpsPosition.PDOP = 1; - GPSPositionSet(&gpsPosition); + GPSPositionSensorSet(&gpsPosition); last_gps_time = PIOS_DELAY_GetRaw(); } // Update GPS Velocity measurements static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond if (PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) { - GPSVelocityData gpsVelocity; - GPSVelocityGet(&gpsVelocity); + GPSVelocitySensorData gpsVelocity; + GPSVelocitySensorGet(&gpsVelocity); gpsVelocity.North = vel[0] + gps_vel_drift[0]; gpsVelocity.East = vel[1] + gps_vel_drift[1]; gpsVelocity.Down = vel[2] + gps_vel_drift[2]; - GPSVelocitySet(&gpsVelocity); + GPSVelocitySensorSet(&gpsVelocity); last_gps_vel_time = PIOS_DELAY_GetRaw(); } @@ -783,8 +783,8 @@ static void simulateModelAirplane() gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0; gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0; - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); + GPSPositionSensorData gpsPosition; + GPSPositionSensorGet(&gpsPosition); gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6); gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1]) / T[1] * 10.0e6); gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]); @@ -792,19 +792,19 @@ static void simulateModelAirplane() gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1], vel[0] + gps_vel_drift[0]); gpsPosition.Satellites = 7; gpsPosition.PDOP = 1; - GPSPositionSet(&gpsPosition); + GPSPositionSensorSet(&gpsPosition); last_gps_time = PIOS_DELAY_GetRaw(); } // Update GPS Velocity measurements static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond if (PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) { - GPSVelocityData gpsVelocity; - GPSVelocityGet(&gpsVelocity); + GPSVelocitySensorData gpsVelocity; + GPSVelocitySensorGet(&gpsVelocity); gpsVelocity.North = vel[0] + gps_vel_drift[0]; gpsVelocity.East = vel[1] + gps_vel_drift[1]; gpsVelocity.Down = vel[2] + gps_vel_drift[2]; - GPSVelocitySet(&gpsVelocity); + GPSVelocitySensorSet(&gpsVelocity); last_gps_vel_time = PIOS_DELAY_GetRaw(); } diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index e6e20e648..34fd3204d 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include @@ -219,8 +219,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state) } if (this->usePos) { - GPSPositionData gpsData; - GPSPositionGet(&gpsData); + GPSPositionSensorData gpsData; + GPSPositionSensorGet(&gpsData); // Have a minimum requirement for gps usage if ((gpsData.Satellites < 7) || (gpsData.PDOP > 4.0f) || diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index dd8247d71..82a4dad6c 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -36,8 +36,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -181,7 +181,7 @@ static const filterPipeline *ekf16Queue = &(filterPipeline) { static void settingsUpdatedCb(UAVObjEvent *objEv); static void sensorUpdatedCb(UAVObjEvent *objEv); static void StateEstimationCb(void); -static void getNED(GPSPositionData *gpsPosition, float *NED); +static void getNED(GPSPositionSensorData *gpsPosition, float *NED); static bool sane(float value); static inline int32_t maxint32_t(int32_t a, int32_t b) @@ -205,8 +205,8 @@ int32_t StateEstimationInitialize(void) MagSensorInitialize(); BaroSensorInitialize(); AirspeedSensorInitialize(); - GPSPositionInitialize(); - GPSVelocityInitialize(); + GPSPositionSensorInitialize(); + GPSVelocitySensorInitialize(); GyroStateInitialize(); AccelStateInitialize(); @@ -223,8 +223,8 @@ int32_t StateEstimationInitialize(void) MagSensorConnectCallback(&sensorUpdatedCb); BaroSensorConnectCallback(&sensorUpdatedCb); AirspeedSensorConnectCallback(&sensorUpdatedCb); - GPSPositionConnectCallback(&sensorUpdatedCb); - GPSVelocityConnectCallback(&sensorUpdatedCb); + GPSPositionSensorConnectCallback(&sensorUpdatedCb); + GPSVelocitySensorConnectCallback(&sensorUpdatedCb); uint32_t stack_required = STACK_SIZE_BYTES; // Initialize Filters @@ -353,7 +353,7 @@ static void StateEstimationCb(void) SANITYCHECK3(GyroSensor, gyro, x, y, z); SANITYCHECK3(AccelSensor, accel, x, y, z); SANITYCHECK3(MagSensor, mag, x, y, z); - SANITYCHECK3(GPSVelocity, vel, North, East, Down); + SANITYCHECK3(GPSVelocitySensor, vel, North, East, Down); #define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \ if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ @@ -369,10 +369,10 @@ static void StateEstimationCb(void) SANITYCHECK1(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter - // GPS is a tiny bit more tricky as GPSPosition is not float (otherwise the conversion to NED could sit in a filter) but integers, for precision reasons + // GPS is a tiny bit more tricky as GPSPositionSensor is not float (otherwise the conversion to NED could sit in a filter) but integers, for precision reasons if (ISSET(states.updated, SENSORUPDATES_pos)) { - GPSPositionData s; - GPSPositionGet(&s); + GPSPositionSensorData s; + GPSPositionSensorGet(&s); if (homeLocation.Set == HOMELOCATION_SET_TRUE && sane(s.Latitude) && sane(s.Longitude) && sane(s.Altitude) && (fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f)) { getNED(&s, states.pos); } else { @@ -529,11 +529,11 @@ static void sensorUpdatedCb(UAVObjEvent *ev) updatedSensors |= SENSORUPDATES_mag; } - if (ev->obj == GPSPositionHandle()) { + if (ev->obj == GPSPositionSensorHandle()) { updatedSensors |= SENSORUPDATES_pos; } - if (ev->obj == GPSVelocityHandle()) { + if (ev->obj == GPSVelocitySensorHandle()) { updatedSensors |= SENSORUPDATES_vel; } @@ -557,7 +557,7 @@ static void sensorUpdatedCb(UAVObjEvent *ev) * @param[out] NED frame coordinates * @returns 0 for success, -1 for failure */ -static void getNED(GPSPositionData *gpsPosition, float *NED) +static void getNED(GPSPositionSensorData *gpsPosition, float *NED) { float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f), DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f), diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 764c094d0..e9a12979c 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -58,8 +58,8 @@ #include "manualcontrol.h" #include "flightstatus.h" #include "pathstatus.h" -#include "gpsvelocity.h" -#include "gpsposition.h" +#include "gpsvelocitysensor.h" +#include "gpspositionsensor.h" #include "homelocation.h" #include "vtolpathfollowersettings.h" #include "nedaccel.h" @@ -481,8 +481,8 @@ void updateEndpointVelocity() { // this used to work with the NEDposition UAVObject // however this UAVObject has been removed - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); + GPSPositionSensorData gpsPosition; + GPSPositionSensorGet(&gpsPosition); HomeLocationData homeLocation; HomeLocationGet(&homeLocation); float lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); @@ -602,8 +602,8 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) break; case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: { - GPSVelocityData gpsVelocity; - GPSVelocityGet(&gpsVelocity); + GPSVelocitySensorData gpsVelocity; + GPSVelocitySensorGet(&gpsVelocity); northVel = gpsVelocity.North; eastVel = gpsVelocity.East; downVel = gpsVelocity.Down; @@ -611,8 +611,8 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) break; case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS: { - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); + GPSPositionSensorData gpsPosition; + GPSPositionSensorGet(&gpsPosition); northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading)); eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading)); downVel = velocityState.Down; diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index e3c0a8c23..6ca39951e 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -99,8 +99,8 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/attitudesettings.c SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c SRC += $(OPUAVSYNTHDIR)/cameradesired.c - SRC += $(OPUAVSYNTHDIR)/gpsposition.c - SRC += $(OPUAVSYNTHDIR)/gpsvelocity.c + SRC += $(OPUAVSYNTHDIR)/gpspositionsensor.c + SRC += $(OPUAVSYNTHDIR)/gpsvelocitysensor.c SRC += $(OPUAVSYNTHDIR)/hwsettings.c SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c SRC += $(OPUAVSYNTHDIR)/receiveractivity.c diff --git a/flight/targets/boards/osd/firmware/Makefile b/flight/targets/boards/osd/firmware/Makefile index 7c88af224..9ffd1385f 100644 --- a/flight/targets/boards/osd/firmware/Makefile +++ b/flight/targets/boards/osd/firmware/Makefile @@ -87,9 +87,9 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/taskinfo.c SRC += $(OPUAVSYNTHDIR)/mixerstatus.c SRC += $(OPUAVSYNTHDIR)/homelocation.c - SRC += $(OPUAVSYNTHDIR)/gpsposition.c + SRC += $(OPUAVSYNTHDIR)/gpspositionsensor.c SRC += $(OPUAVSYNTHDIR)/gpssatellites.c - SRC += $(OPUAVSYNTHDIR)/gpsvelocity.c + SRC += $(OPUAVSYNTHDIR)/gpsvelocitysensor.c SRC += $(OPUAVSYNTHDIR)/gpstime.c SRC += $(OPUAVSYNTHDIR)/osdsettings.c SRC += $(OPUAVSYNTHDIR)/barosensor.c diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 1ee34b95d..188b15bab 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -45,10 +45,10 @@ UAVOBJSRCFILENAMES += flightplanstatus UAVOBJSRCFILENAMES += flighttelemetrystats UAVOBJSRCFILENAMES += gcstelemetrystats UAVOBJSRCFILENAMES += gcsreceiver -UAVOBJSRCFILENAMES += gpsposition +UAVOBJSRCFILENAMES += gpspositionsensor UAVOBJSRCFILENAMES += gpssatellites UAVOBJSRCFILENAMES += gpstime -UAVOBJSRCFILENAMES += gpsvelocity +UAVOBJSRCFILENAMES += gpsvelocitysensor UAVOBJSRCFILENAMES += fixedwingpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus UAVOBJSRCFILENAMES += vtolpathfollowersettings @@ -65,6 +65,7 @@ UAVOBJSRCFILENAMES += overosyncsettings UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathstatus +UAVOBJSRCFILENAMES += positionsensor UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += relaytuning diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index f170eebfe..0ce0ff3ab 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -137,6 +137,7 @@ #define PIOS_INCLUDE_GPS_NMEA_PARSER #define PIOS_INCLUDE_GPS_UBX_PARSER #define PIOS_GPS_SETS_HOMELOCATION +#define PIOS_GPS_SETS_POSITIONSENSOR /* Stabilization options */ /* #define PIOS_QUATERNION_STABILIZATION */ diff --git a/flight/targets/boards/revolution/firmware/pios_board_sim.c b/flight/targets/boards/revolution/firmware/pios_board_sim.c index 9cf758199..9def851ae 100644 --- a/flight/targets/boards/revolution/firmware/pios_board_sim.c +++ b/flight/targets/boards/revolution/firmware/pios_board_sim.c @@ -33,7 +33,7 @@ #include #include -#include +#include #include #include #include @@ -139,7 +139,7 @@ void PIOS_Board_Init(void) AccelSensorInitialize(); BaroSensorInitialize(); MagSensorInitialize(); - GPSPositionInitialize(); + GPSPositionSensorInitialize(); GyroSensorInitialize(); /* Initialize the alarms library */ diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 5742838ba..996708520 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -50,10 +50,10 @@ UAVOBJSRCFILENAMES += flightplanstatus UAVOBJSRCFILENAMES += flighttelemetrystats UAVOBJSRCFILENAMES += gcstelemetrystats UAVOBJSRCFILENAMES += gcsreceiver -UAVOBJSRCFILENAMES += gpsposition +UAVOBJSRCFILENAMES += gpspositionsensor UAVOBJSRCFILENAMES += gpssatellites UAVOBJSRCFILENAMES += gpstime -UAVOBJSRCFILENAMES += gpsvelocity +UAVOBJSRCFILENAMES += gpsvelocitysensor UAVOBJSRCFILENAMES += fixedwingpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus UAVOBJSRCFILENAMES += vtolpathfollowersettings @@ -70,6 +70,7 @@ UAVOBJSRCFILENAMES += overosyncsettings UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathstatus +UAVOBJSRCFILENAMES += positionsensor UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += relaytuning diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index 30be6bb7a..8b1bba11c 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -137,6 +137,7 @@ #define PIOS_INCLUDE_GPS_NMEA_PARSER #define PIOS_INCLUDE_GPS_UBX_PARSER #define PIOS_GPS_SETS_HOMELOCATION +#define PIOS_GPS_SETS_POSITIONSENSOR /* Stabilization options */ /* #define PIOS_QUATERNION_STABILIZATION */ diff --git a/flight/targets/boards/revoproto/firmware/pios_board_sim.c b/flight/targets/boards/revoproto/firmware/pios_board_sim.c index 27d998798..9b4c60bc0 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board_sim.c +++ b/flight/targets/boards/revoproto/firmware/pios_board_sim.c @@ -33,7 +33,7 @@ #include #include -#include +#include #include #include #include @@ -139,7 +139,7 @@ void PIOS_Board_Init(void) AccelSensorInitialize(); BaroSensorInitialize(); MagSensorInitialize(); - GPSPositionInitialize(); + GPSPositionSensorInitialize(); GyroStatInitialize(); GyroSensorInitialize(); diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 5134a536d..4901ca9eb 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -49,10 +49,10 @@ UAVOBJSRCFILENAMES += flightplansettings UAVOBJSRCFILENAMES += flightplanstatus UAVOBJSRCFILENAMES += flighttelemetrystats UAVOBJSRCFILENAMES += gcstelemetrystats -UAVOBJSRCFILENAMES += gpsposition +UAVOBJSRCFILENAMES += gpspositionsensor UAVOBJSRCFILENAMES += gpssatellites UAVOBJSRCFILENAMES += gpstime -UAVOBJSRCFILENAMES += gpsvelocity +UAVOBJSRCFILENAMES += gpsvelocitysensor UAVOBJSRCFILENAMES += vtolpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus @@ -68,6 +68,7 @@ UAVOBJSRCFILENAMES += overosyncstats UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathstatus +UAVOBJSRCFILENAMES += positionsensor UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += revocalibration diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml index 233061c29..99d6599b3 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml @@ -449,7 +449,7 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 @@ -554,7 +554,7 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 @@ -764,7 +764,7 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 @@ -1202,7 +1202,7 @@ 0 0 0 - GPSPosition + GPSPositionSensor Satellites false 0 @@ -1225,7 +1225,7 @@ 0 33 0 - GPSPosition + GPSPositionSensor Status false 66 diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml index 4ad20f5da..0c2236054 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml @@ -449,7 +449,7 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 @@ -551,7 +551,7 @@ %%DATAPATH%%dials/default/speed.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 @@ -750,7 +750,7 @@ %%DATAPATH%%dials/hi-contrast/speed.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 @@ -1219,7 +1219,7 @@ 0 0 0 - GPSPosition + GPSPositionSensor Satellites false 0 @@ -1242,7 +1242,7 @@ 0 33 0 - GPSPosition + GPSPositionSensor Status false 66 diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml index 4273cf797..89a57f21e 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml @@ -449,7 +449,7 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 @@ -551,7 +551,7 @@ %%DATAPATH%%dials/default/speed.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 @@ -750,7 +750,7 @@ %%DATAPATH%%dials/hi-contrast/speed.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 @@ -1219,7 +1219,7 @@ 0 0 0 - GPSPosition + GPSPositionSensor Satellites false 0 @@ -1242,7 +1242,7 @@ 0 33 0 - GPSPosition + GPSPositionSensor Status false 66 diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdIndicators.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdIndicators.qml index 8d821e7c3..51e68dde3 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdIndicators.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdIndicators.qml @@ -31,12 +31,12 @@ Item { Text { id: gps_text - text: "GPS: " + GPSPosition.Satellites + "\nPDP: " + GPSPosition.PDOP + text: "GPS: " + GPSPositionSensor.Satellites + "\nPDP: " + GPSPositionSensor.PDOP color: "white" font.family: "Arial" font.pixelSize: telemetry_status.height * 0.75 - visible: GPSPosition.Satellites > 0 + visible: GPSPositionSensor.Satellites > 0 property variant scaledBounds: svgRenderer.scaledElementBounds("pfd.svg", "gps-txt") x: Math.floor(scaledBounds.x * sceneItem.width) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdTerrainView.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdTerrainView.qml index 6a9a8d24b..2beebf05f 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdTerrainView.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdTerrainView.qml @@ -12,9 +12,9 @@ OsgEarth { roll: AttitudeState.Roll latitude: qmlWidget.actualPositionUsed ? - GPSPosition.Latitude/10000000.0 : qmlWidget.latitude + GPSPositionSensor.Latitude/10000000.0 : qmlWidget.latitude longitude: qmlWidget.actualPositionUsed ? - GPSPosition.Longitude/10000000.0 : qmlWidget.longitude + GPSPositionSensor.Longitude/10000000.0 : qmlWidget.longitude altitude: qmlWidget.actualPositionUsed ? - GPSPosition.Altitude : qmlWidget.altitude + GPSPositionSensor.Altitude : qmlWidget.altitude } diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_de.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_de.ts index cfd19556d..b173f1ff1 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_de.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_de.ts @@ -1807,7 +1807,7 @@ p, li { white-space: pre-wrap; } - GPSPosition + GPSPositionSensor diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_es.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_es.ts index 5182fddf2..d200b257f 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_es.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_es.ts @@ -1808,7 +1808,7 @@ p, li { white-space: pre-wrap; } - GPSPosition + GPSPositionSensor diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts index e74746db8..2fed8d566 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts @@ -1829,7 +1829,7 @@ p, li { white-space: pre-wrap; } - GPSPosition + GPSPositionSensor diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_ru.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_ru.ts index e368aca9d..5acabb9f8 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_ru.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_ru.ts @@ -1761,7 +1761,7 @@ Sat SNR is displayed above (in dBHz) - GPSPosition + GPSPositionSensor diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_zh_CN.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_zh_CN.ts index b80073462..d01683dc6 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_zh_CN.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_zh_CN.ts @@ -2647,7 +2647,7 @@ Sat SNR is displayed above (in dBHz) - GPSPosition + GPSPositionSensor diff --git a/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp b/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp index 6bbdef701..3b9669521 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp @@ -39,12 +39,12 @@ TelemetryParser::TelemetryParser(QObject *parent) : GPSParser(parent) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject *gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + UAVDataObject *gpsObj = dynamic_cast(objManager->getObject("GPSPositionSensor")); if (gpsObj != NULL) { connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGPS(UAVObject *))); } else { - qDebug() << "Error: Object is unknown (GPSPosition)."; + qDebug() << "Error: Object is unknown (GPSPositionSensor)."; } gpsObj = dynamic_cast(objManager->getObject("HomeLocation")); diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp index 70fc4bc1a..780fc7854 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp @@ -39,12 +39,12 @@ TelemetryParser::TelemetryParser(QObject *parent) : GPSParser(parent) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject *gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + UAVDataObject *gpsObj = dynamic_cast(objManager->getObject("GPSPositionSensor")); if (gpsObj != NULL) { connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGPS(UAVObject *))); } else { - qDebug() << "Error: Object is unknown (GPSPosition)."; + qDebug() << "Error: Object is unknown (GPSPositionSensor)."; } gpsObj = dynamic_cast(objManager->getObject("GPSTime")); diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h index 9ecbbd205..22df94b72 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h @@ -37,16 +37,16 @@ #include struct Noise { - AccelState::DataFields accelStateData; - AttitudeState::DataFields attStateData; - BaroSensor::DataFields baroAltData; - AirspeedState::DataFields airspeedState; - GPSPosition::DataFields gpsPosData; - GPSVelocity::DataFields gpsVelData; - GyroState::DataFields gyroStateData; - HomeLocation::DataFields homeData; - PositionState::DataFields positionStateData; - VelocityState::DataFields velocityStateData; + AccelState::DataFields accelStateData; + AttitudeState::DataFields attStateData; + BaroSensor::DataFields baroAltData; + AirspeedState::DataFields airspeedState; + GPSPositionSensor::DataFields gpsPosData; + GPSVelocitySensor::DataFields gpsVelData; + GyroState::DataFields gyroStateData; + HomeLocation::DataFields homeData; + PositionState::DataFields positionStateData; + VelocityState::DataFields velocityStateData; }; class HitlNoiseGeneration { diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp index 13bd35bbf..67e0de65b 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp @@ -156,8 +156,8 @@ void Simulator::onStart() attSettings = AttitudeSettings::GetInstance(objManager); accelState = AccelState::GetInstance(objManager); gyroState = GyroState::GetInstance(objManager); - gpsPos = GPSPosition::GetInstance(objManager); - gpsVel = GPSVelocity::GetInstance(objManager); + gpsPos = GPSPositionSensor::GetInstance(objManager); + gpsVel = GPSVelocitySensor::GetInstance(objManager); telStats = GCSTelemetryStats::GetInstance(objManager); groundTruth = GroundTruth::GetInstance(objManager); @@ -633,8 +633,8 @@ void Simulator::updateUAVOs(Output2Hardware out) if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) { qDebug() << " GPS time:" << gpsPosTime << ", currentTime: " << currentTime << ", difference: " << gpsPosTime.msecsTo(currentTime); // Update GPS Position objects - GPSPosition::DataFields gpsPosData; - memset(&gpsPosData, 0, sizeof(GPSPosition::DataFields)); + GPSPositionSensor::DataFields gpsPosData; + memset(&gpsPosData, 0, sizeof(GPSPositionSensor::DataFields)); gpsPosData.Altitude = out.altitude + noise.gpsPosData.Altitude; gpsPosData.Heading = out.heading + noise.gpsPosData.Heading; gpsPosData.Groundspeed = out.groundspeed + noise.gpsPosData.Groundspeed; @@ -644,13 +644,13 @@ void Simulator::updateUAVOs(Output2Hardware out) gpsPosData.PDOP = 3.0; gpsPosData.VDOP = gpsPosData.PDOP * 1.5; gpsPosData.Satellites = 10; - gpsPosData.Status = GPSPosition::STATUS_FIX3D; + gpsPosData.Status = GPSPositionSensor::STATUS_FIX3D; gpsPos->setData(gpsPosData); // Update GPS Velocity.{North,East,Down} - GPSVelocity::DataFields gpsVelData; - memset(&gpsVelData, 0, sizeof(GPSVelocity::DataFields)); + GPSVelocitySensor::DataFields gpsVelData; + memset(&gpsVelData, 0, sizeof(GPSVelocitySensor::DataFields)); gpsVelData.North = out.velNorth + noise.gpsVelData.North; gpsVelData.East = out.velEast + noise.gpsVelData.East; gpsVelData.Down = out.velDown + noise.gpsVelData.Down; diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.h b/ground/openpilotgcs/src/plugins/hitl/simulator.h index ecf4ca872..7d9ac4ad9 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.h @@ -49,8 +49,8 @@ #include "flightstatus.h" #include "gcsreceiver.h" #include "gcstelemetrystats.h" -#include "gpsposition.h" -#include "gpsvelocity.h" +#include "gpspositionsensor.h" +#include "gpsvelocitysensor.h" #include "groundtruth.h" #include "gyrostate.h" #include "homelocation.h" @@ -326,8 +326,8 @@ protected: AttitudeState *attState; AttitudeSettings *attSettings; VelocityState *velState; - GPSPosition *gpsPos; - GPSVelocity *gpsVel; + GPSPositionSensor *gpsPos; + GPSVelocitySensor *gpsVel; PositionState *posState; HomeLocation *posHome; AccelState *accelState; diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index a5a60b187..114435773 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -50,7 +50,7 @@ #include "positionstate.h" #include "homelocation.h" -#include "gpsposition.h" +#include "gpspositionsensor.h" #include "gyrostate.h" #include "attitudestate.h" #include "positionstate.h" @@ -579,10 +579,10 @@ void OPMapGadgetWidget::updatePosition() // ************* // get the current GPS position and heading - GPSPosition *gpsPositionObj = GPSPosition::GetInstance(obm); + GPSPositionSensor *gpsPositionObj = GPSPositionSensor::GetInstance(obm); Q_ASSERT(gpsPositionObj); - GPSPosition::DataFields gpsPositionData = gpsPositionObj->getData(); + GPSPositionSensor::DataFields gpsPositionData = gpsPositionObj->getData(); gps_heading = gpsPositionData.Heading; gps_latitude = gpsPositionData.Latitude; @@ -2236,7 +2236,7 @@ double OPMapGadgetWidget::getUAV_Yaw() return yaw; } -bool OPMapGadgetWidget::getGPSPosition(double &latitude, double &longitude, double &altitude) +bool OPMapGadgetWidget::getGPSPositionSensor(double &latitude, double &longitude, double &altitude) { double LLA[3]; @@ -2244,7 +2244,7 @@ bool OPMapGadgetWidget::getGPSPosition(double &latitude, double &longitude, doub return false; } - if (obum->getGPSPosition(LLA) < 0) { + if (obum->getGPSPositionSensor(LLA) < 0) { return false; // error } latitude = LLA[0]; diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h index 7de84fce2..f5db4d5a0 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h @@ -113,7 +113,7 @@ public: void setMaxUpdateRate(int update_rate); void setHomePosition(QPointF pos); void setOverlayOpacity(qreal value); - bool getGPSPosition(double &latitude, double &longitude, double &altitude); + bool getGPSPositionSensor(double &latitude, double &longitude, double &altitude); signals: void defaultLocationAndZoomChanged(double lng, double lat, double zoom); void overlayOpacityChanged(qreal); diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp index cd08ce5dd..fce21f931 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp @@ -96,7 +96,7 @@ using namespace osgEarth::Annotation; #include "utils/worldmagmodel.h" #include "utils/coordinateconversions.h" #include "attitudestate.h" -#include "gpsposition.h" +#include "gpspositionsensor.h" #include "homelocation.h" #include "positionstate.h" #include "systemsettings.h" @@ -270,8 +270,8 @@ void OsgViewerWidget::paintEvent(QPaintEvent *event) CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA); uavPos->getLocator()->setPosition(osg::Vec3d(LLA[1], LLA[0], LLA[2])); // Note this takes longtitude first } else { - GPSPosition *gpsPosObj = GPSPosition::GetInstance(objMngr); - GPSPosition::DataFields gpsPos = gpsPosObj->getData(); + GPSPositionSensor *gpsPosObj = GPSPositionSensor::GetInstance(objMngr); + GPSPositionSensor::DataFields gpsPos = gpsPosObj->getData(); uavPos->getLocator()->setPosition(osg::Vec3d(gpsPos.Longitude / 10.0e6, gpsPos.Latitude / 10.0e6, gpsPos.Altitude)); } diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp index bbb1f4c48..1ccf3a7e9 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp @@ -170,11 +170,11 @@ void PFDGadgetWidget::connectNeedles() } if (gcsGPSStats) { - gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + gpsObj = dynamic_cast(objManager->getObject("GPSPositionSensor")); if (gpsObj != NULL) { connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGPS(UAVObject *))); } else { - qDebug() << "Error: Object is unknown (GPSPosition)."; + qDebug() << "Error: Object is unknown (GPSPositionSensor)."; } } diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp index aca816bea..307347067 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp @@ -55,7 +55,7 @@ PfdQmlGadgetWidget::PfdQmlGadgetWidget(QWidget *parent) : "VelocityDesired" << "PositionDesired" << "AttitudeHoldDesired" << - "GPSPosition" << + "GPSPositionSensor" << "GCSTelemetryStats" << "FlightBatteryState"; diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp index 337faf7c0..9abbb4b23 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp @@ -52,7 +52,7 @@ QmlViewGadgetWidget::QmlViewGadgetWidget(QWidget *parent) : objectsToExport << "VelocityState" << "PositionState" << "AttitudeState" << - "GPSPosition" << + "GPSPositionSensor" << "GCSTelemetryStats" << "FlightBatteryState"; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/OPPlots.m b/ground/openpilotgcs/src/plugins/uavobjects/OPPlots.m index 3dabe5a39..9b0303a39 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/OPPlots.m +++ b/ground/openpilotgcs/src/plugins/uavobjects/OPPlots.m @@ -10,9 +10,9 @@ function OPPlots() [VelocityState.East] [VelocityState.Down]]/100; - TimeGPSPos = [GPSPosition.timestamp]/1000; - Vgps=[[GPSPosition.Groundspeed].*cos([GPSPosition.Heading]*pi/180) - [GPSPosition.Groundspeed].*sin([GPSPosition.Heading]*pi/180)]; + TimeGPSPos = [GPSPositionSensor.timestamp]/1000; + Vgps=[[GPSPositionSensor.Groundspeed].*cos([GPSPositionSensor.Heading]*pi/180) + [GPSPositionSensor.Groundspeed].*sin([GPSPositionSensor.Heading]*pi/180)]; figure(1); plot(TimeVA,VA(1,:),TimeVA,VA(2,:),TimeGPSPos,Vgps(1,:),TimeGPSPos,Vgps(2,:)); @@ -30,10 +30,10 @@ function OPPlots() [PositionState.East] [PositionState.Down]]/100; - TimeGPSPos = [GPSPosition.timestamp]/1000; - LLA=[[GPSPosition.Latitude]*1e-7; - [GPSPosition.Longitude]*1e-7; - [GPSPosition.Altitude]+[GPSPosition.GeoidSeparation]]; + TimeGPSPos = [GPSPositionSensor.timestamp]/1000; + LLA=[[GPSPositionSensor.Latitude]*1e-7; + [GPSPositionSensor.Longitude]*1e-7; + [GPSPositionSensor.Altitude]+[GPSPositionSensor.GeoidSeparation]]; BaseECEF = LLA2ECEF([HomeLocation.Latitude*1e-7, HomeLocation.Longitude*1e-7, HomeLocation.Altitude]'); Rne = RneFromLLA([HomeLocation.Latitude*1e-7, HomeLocation.Longitude*1e-7, HomeLocation.Altitude]'); GPSPos=LLA2Base(LLA,BaseECEF,Rne); diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 4ff9d4050..a4c5c51d7 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -59,13 +59,14 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/actuatorsettings.h \ $$UAVOBJECT_SYNTHETICS/actuatordesired.h \ $$UAVOBJECT_SYNTHETICS/actuatorcommand.h \ - $$UAVOBJECT_SYNTHETICS/gpsposition.h \ + $$UAVOBJECT_SYNTHETICS/gpspositionsensor.h \ $$UAVOBJECT_SYNTHETICS/gpstime.h \ $$UAVOBJECT_SYNTHETICS/gpssatellites.h \ $$UAVOBJECT_SYNTHETICS/pathaction.h \ $$UAVOBJECT_SYNTHETICS/pathdesired.h \ $$UAVOBJECT_SYNTHETICS/pathstatus.h \ - $$UAVOBJECT_SYNTHETICS/gpsvelocity.h \ + $$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.h \ + $$UAVOBJECT_SYNTHETICS/positionsensor.h \ $$UAVOBJECT_SYNTHETICS/positionstate.h \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.h \ $$UAVOBJECT_SYNTHETICS/homelocation.h \ @@ -143,13 +144,14 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/actuatorsettings.cpp \ $$UAVOBJECT_SYNTHETICS/actuatordesired.cpp \ $$UAVOBJECT_SYNTHETICS/actuatorcommand.cpp \ - $$UAVOBJECT_SYNTHETICS/gpsposition.cpp \ + $$UAVOBJECT_SYNTHETICS/gpspositionsensor.cpp \ $$UAVOBJECT_SYNTHETICS/gpstime.cpp \ $$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \ $$UAVOBJECT_SYNTHETICS/pathaction.cpp \ $$UAVOBJECT_SYNTHETICS/pathdesired.cpp \ $$UAVOBJECT_SYNTHETICS/pathstatus.cpp \ - $$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \ + $$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.cpp \ + $$UAVOBJECT_SYNTHETICS/positionsensor.cpp \ $$UAVOBJECT_SYNTHETICS/positionstate.cpp \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \ $$UAVOBJECT_SYNTHETICS/homelocation.cpp \ diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index c89683412..dd92585cf 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -38,7 +38,7 @@ #include "firmwareiapobj.h" #include "homelocation.h" -#include "gpsposition.h" +#include "gpspositionsensor.h" // ****************************** // constructor/destructor @@ -377,13 +377,13 @@ int UAVObjectUtilManager::getHomeLocation(bool &set, double LLA[3]) // ****************************** // GPS -int UAVObjectUtilManager::getGPSPosition(double LLA[3]) +int UAVObjectUtilManager::getGPSPositionSensor(double LLA[3]) { - GPSPosition *gpsPosition = GPSPosition::GetInstance(obm); + GPSPositionSensor *gpsPosition = GPSPositionSensor::GetInstance(obm); Q_ASSERT(gpsPosition != NULL); - GPSPosition::DataFields gpsPositionData = gpsPosition->getData(); + GPSPositionSensor::DataFields gpsPositionData = gpsPosition->getData(); LLA[0] = gpsPositionData.Latitude; LLA[1] = gpsPositionData.Longitude; diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h index 68260693c..7ad27b8d1 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h @@ -55,7 +55,7 @@ public: int setHomeLocation(double LLA[3], bool save_to_sdcard); int getHomeLocation(bool &set, double LLA[3]); - int getGPSPosition(double LLA[3]); + int getGPSPositionSensor(double LLA[3]); int getBoardModel(); QByteArray getBoardCPUSerial(); diff --git a/shared/uavobjectdefinition/gpsposition.xml b/shared/uavobjectdefinition/gpspositionsensor.xml similarity index 94% rename from shared/uavobjectdefinition/gpsposition.xml rename to shared/uavobjectdefinition/gpspositionsensor.xml index 7a8ae82db..e7a497ab4 100644 --- a/shared/uavobjectdefinition/gpsposition.xml +++ b/shared/uavobjectdefinition/gpspositionsensor.xml @@ -1,5 +1,5 @@ - + Raw GPS data from @ref GPSModule. Should only be used by @ref AHRSCommsModule. diff --git a/shared/uavobjectdefinition/gpsvelocity.xml b/shared/uavobjectdefinition/gpsvelocitysensor.xml similarity index 87% rename from shared/uavobjectdefinition/gpsvelocity.xml rename to shared/uavobjectdefinition/gpsvelocitysensor.xml index 8673463a8..21e32338e 100644 --- a/shared/uavobjectdefinition/gpsvelocity.xml +++ b/shared/uavobjectdefinition/gpsvelocitysensor.xml @@ -1,5 +1,5 @@ - + Raw GPS data from @ref GPSModule. From 843db63cf68d11627ca7d40a0b65963a7293bb09 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 22 May 2013 22:58:14 +0200 Subject: [PATCH 29/61] some last issues from review --- flight/modules/StateEstimation/filterair.c | 4 +- flight/modules/StateEstimation/filterbaro.c | 6 +- flight/modules/StateEstimation/filtercf.c | 6 +- flight/modules/StateEstimation/filterekf.c | 32 +++---- flight/modules/StateEstimation/filtermag.c | 2 +- .../StateEstimation/inc/stateestimation.h | 4 - .../modules/StateEstimation/stateestimation.c | 84 +++++-------------- flight/pios/inc/pios_math.h | 16 +++- shared/uavobjectdefinition/positionsensor.xml | 12 +++ 9 files changed, 71 insertions(+), 95 deletions(-) create mode 100644 shared/uavobjectdefinition/positionsensor.xml diff --git a/flight/modules/StateEstimation/filterair.c b/flight/modules/StateEstimation/filterair.c index f5a4ee60d..3ed89b99d 100644 --- a/flight/modules/StateEstimation/filterair.c +++ b/flight/modules/StateEstimation/filterair.c @@ -74,11 +74,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state) struct data *this = (struct data *)self->localdata; // take static pressure altitude estimation for - if (ISSET(state->updated, SENSORUPDATES_baro)) { + if (IS_SET(state->updated, SENSORUPDATES_baro)) { this->altitude = state->baro[0]; } // calculate true airspeed estimation - if (ISSET(state->updated, SENSORUPDATES_airspeed)) { + if (IS_SET(state->updated, SENSORUPDATES_airspeed)) { state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude); } diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index b8b8f14be..0d8e83f67 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -79,20 +79,20 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (this->first_run) { // Initialize to current altitude reading at initial location - if (ISSET(state->updated, SENSORUPDATES_baro)) { + if (IS_SET(state->updated, SENSORUPDATES_baro)) { this->first_run = 0; this->baroOffset = state->baro[0]; } } else { // Track barometric altitude offset with a low pass filter // based on GPS altitude if available - if (ISSET(state->updated, SENSORUPDATES_pos)) { + if (IS_SET(state->updated, SENSORUPDATES_pos)) { this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset + (1.0f - BARO_OFFSET_LOWPASS_ALPHA) * (-state->pos[2] - state->baro[0]); } // calculate bias corrected altitude - if (ISSET(state->updated, SENSORUPDATES_baro)) { + if (IS_SET(state->updated, SENSORUPDATES_baro)) { state->baro[0] -= this->baroOffset; } } diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 27ebbcce9..32ccc1a72 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -158,19 +158,19 @@ static int32_t filter(stateFilter *self, stateEstimation *state) int32_t result = 0; - if (ISSET(state->updated, SENSORUPDATES_mag)) { + if (IS_SET(state->updated, SENSORUPDATES_mag)) { this->magUpdated = 1; this->currentMag[0] = state->mag[0]; this->currentMag[1] = state->mag[1]; this->currentMag[2] = state->mag[2]; } - if (ISSET(state->updated, SENSORUPDATES_accel)) { + if (IS_SET(state->updated, SENSORUPDATES_accel)) { this->accelUpdated = 1; this->currentAccel[0] = state->accel[0]; this->currentAccel[1] = state->accel[1]; this->currentAccel[2] = state->accel[2]; } - if (ISSET(state->updated, SENSORUPDATES_gyro)) { + if (IS_SET(state->updated, SENSORUPDATES_gyro)) { if (this->accelUpdated) { float attitude[4]; result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, attitude); diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 34fd3204d..bbbb870af 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -193,7 +193,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // Get most recent data #define UPDATE(shortname, num) \ - if (ISSET(state->updated, SENSORUPDATES_##shortname)) { \ + if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \ uint8_t t; \ for (t = 0; t < num; t++) { \ this->work.shortname[t] = state->shortname[t]; \ @@ -210,11 +210,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // check whether mandatory updates are present accels must have been supplied already, // and gyros must be supplied just now for a prediction step to take place // ("gyros last" rule for multi object synchronization) - if (!(ISSET(this->work.updated, SENSORUPDATES_accel) && ISSET(state->updated, SENSORUPDATES_gyro))) { - UNSET(state->updated, SENSORUPDATES_pos); - UNSET(state->updated, SENSORUPDATES_vel); - UNSET(state->updated, SENSORUPDATES_attitude); - UNSET(state->updated, SENSORUPDATES_gyro); + if (!(IS_SET(this->work.updated, SENSORUPDATES_accel) && IS_SET(state->updated, SENSORUPDATES_gyro))) { + UNSET_MASK(state->updated, SENSORUPDATES_pos); + UNSET_MASK(state->updated, SENSORUPDATES_vel); + UNSET_MASK(state->updated, SENSORUPDATES_attitude); + UNSET_MASK(state->updated, SENSORUPDATES_gyro); return 0; } @@ -226,10 +226,10 @@ static int32_t filter(stateFilter *self, stateEstimation *state) (gpsData.PDOP > 4.0f) || (gpsData.Latitude == 0 && gpsData.Longitude == 0) || (this->homeLocation.Set != HOMELOCATION_SET_TRUE)) { - UNSET(state->updated, SENSORUPDATES_pos); - UNSET(state->updated, SENSORUPDATES_vel); - UNSET(this->work.updated, SENSORUPDATES_pos); - UNSET(this->work.updated, SENSORUPDATES_vel); + UNSET_MASK(state->updated, SENSORUPDATES_pos); + UNSET_MASK(state->updated, SENSORUPDATES_vel); + UNSET_MASK(this->work.updated, SENSORUPDATES_pos); + UNSET_MASK(this->work.updated, SENSORUPDATES_vel); } } @@ -243,7 +243,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) dT = 0.001f; } - if (!this->inited && ISSET(this->work.updated, SENSORUPDATES_mag) && ISSET(this->work.updated, SENSORUPDATES_baro) && ISSET(this->work.updated, SENSORUPDATES_pos)) { + if (!this->inited && IS_SET(this->work.updated, SENSORUPDATES_mag) && IS_SET(this->work.updated, SENSORUPDATES_baro) && IS_SET(this->work.updated, SENSORUPDATES_pos)) { // Don't initialize until all sensors are read if (this->init_stage == 0) { // Reset the INS algorithm @@ -362,11 +362,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // Advance the covariance estimate INSCovariancePrediction(dT); - if (ISSET(this->work.updated, SENSORUPDATES_mag)) { + if (IS_SET(this->work.updated, SENSORUPDATES_mag)) { sensors |= MAG_SENSORS; } - if (ISSET(this->work.updated, SENSORUPDATES_baro)) { + if (IS_SET(this->work.updated, SENSORUPDATES_baro)) { sensors |= BARO_SENSOR; } @@ -392,15 +392,15 @@ static int32_t filter(stateFilter *self, stateEstimation *state) ); } - if (ISSET(this->work.updated, SENSORUPDATES_pos)) { + if (IS_SET(this->work.updated, SENSORUPDATES_pos)) { sensors |= POS_SENSORS; } - if (ISSET(this->work.updated, SENSORUPDATES_vel)) { + if (IS_SET(this->work.updated, SENSORUPDATES_vel)) { sensors |= HORIZ_SENSORS | VERT_SENSORS; } - if (ISSET(this->work.updated, SENSORUPDATES_airspeed) && ((!ISSET(this->work.updated, SENSORUPDATES_vel) && !ISSET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) { + if (IS_SET(this->work.updated, SENSORUPDATES_airspeed) && ((!IS_SET(this->work.updated, SENSORUPDATES_vel) && !IS_SET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) { // HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance sensors |= HORIZ_SENSORS | VERT_SENSORS; INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index ceec76747..8c62cf418 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -79,7 +79,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) { struct data *this = (struct data *)self->localdata; - if (ISSET(state->updated, SENSORUPDATES_mag)) { + if (IS_SET(state->updated, SENSORUPDATES_mag)) { if (this->revoCalibration.MagBiasNullingRate > 0) { magOffsetEstimation(this, state->mag); } diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index a334d64ac..9b954874a 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -55,10 +55,6 @@ typedef struct { sensorUpdates updated; } stateEstimation; -#define ISSET(bitfield, bit) ((bitfield) & (bit) ? 1 : 0) -#define UNSET(bitfield, bit) (bitfield) &= ~(bit) - - typedef struct stateFilterStruct { int32_t (*init)(struct stateFilterStruct *self); int32_t (*filter)(struct stateFilterStruct *self, stateEstimation *state); diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 82a4dad6c..3b2a413e6 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include @@ -57,7 +57,7 @@ #define STACK_SIZE_BYTES 256 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR #define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL -#define TIMEOUT_MS 100 +#define TIMEOUT_MS 10 // Private types @@ -181,8 +181,6 @@ static const filterPipeline *ekf16Queue = &(filterPipeline) { static void settingsUpdatedCb(UAVObjEvent *objEv); static void sensorUpdatedCb(UAVObjEvent *objEv); static void StateEstimationCb(void); -static void getNED(GPSPositionSensorData *gpsPosition, float *NED); -static bool sane(float value); static inline int32_t maxint32_t(int32_t a, int32_t b) { @@ -205,7 +203,7 @@ int32_t StateEstimationInitialize(void) MagSensorInitialize(); BaroSensorInitialize(); AirspeedSensorInitialize(); - GPSPositionSensorInitialize(); + PositionSensorInitialize(); GPSVelocitySensorInitialize(); GyroStateInitialize(); @@ -223,7 +221,7 @@ int32_t StateEstimationInitialize(void) MagSensorConnectCallback(&sensorUpdatedCb); BaroSensorConnectCallback(&sensorUpdatedCb); AirspeedSensorConnectCallback(&sensorUpdatedCb); - GPSPositionSensorConnectCallback(&sensorUpdatedCb); + PositionSensorConnectCallback(&sensorUpdatedCb); GPSVelocitySensorConnectCallback(&sensorUpdatedCb); uint32_t stack_required = STACK_SIZE_BYTES; @@ -271,6 +269,7 @@ static void StateEstimationCb(void) static uint16_t alarmcounter = 0; static filterPipeline *current; static stateEstimation states; + static uint32_t last_time; switch (runState) { case RUNSTATE_LOAD: @@ -279,7 +278,11 @@ static void StateEstimationCb(void) // set alarm to warning if called through timeout if (updatedSensors == 0) { - alarm = 1; + if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) { + alarm = 1; + } + } else { + last_time = PIOS_DELAY_GetRaw(); } // check if a new filter chain should be initialized @@ -338,48 +341,38 @@ static void StateEstimationCb(void) // most sensors get only rudimentary sanity checks #define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \ - if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \ + if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ sensorname##Get(&s); \ - if (sane(s.a1) && sane(s.a2) && sane(s.a3)) { \ + if (ISREAL(s.a1) && ISREAL(s.a2) && ISREAL(s.a3)) { \ states.shortname[0] = s.a1; \ states.shortname[1] = s.a2; \ states.shortname[2] = s.a3; \ } \ else { \ - UNSET(states.updated, SENSORUPDATES_##shortname); \ + UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ } \ } SANITYCHECK3(GyroSensor, gyro, x, y, z); SANITYCHECK3(AccelSensor, accel, x, y, z); SANITYCHECK3(MagSensor, mag, x, y, z); + SANITYCHECK3(PositionSensor, pos, North, East, Down); SANITYCHECK3(GPSVelocitySensor, vel, North, East, Down); #define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \ - if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \ + if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ sensorname##Get(&s); \ - if (sane(s.a1) && EXTRACHECK) { \ + if (ISREAL(s.a1) && EXTRACHECK) { \ states.shortname[0] = s.a1; \ } \ else { \ - UNSET(states.updated, SENSORUPDATES_##shortname); \ + UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ } \ } SANITYCHECK1(BaroSensor, baro, Altitude, 1); SANITYCHECK1(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter - // GPS is a tiny bit more tricky as GPSPositionSensor is not float (otherwise the conversion to NED could sit in a filter) but integers, for precision reasons - if (ISSET(states.updated, SENSORUPDATES_pos)) { - GPSPositionSensorData s; - GPSPositionSensorGet(&s); - if (homeLocation.Set == HOMELOCATION_SET_TRUE && sane(s.Latitude) && sane(s.Longitude) && sane(s.Altitude) && (fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f)) { - getNED(&s, states.pos); - } else { - UNSET(states.updated, SENSORUPDATES_pos); - } - } - // at this point sensor state is stored in "states" with some rudimentary filtering applied // apply all filters in the current filter chain @@ -411,7 +404,7 @@ static void StateEstimationCb(void) // the final output of filters is saved in state variables #define STORE3(statename, shortname, a1, a2, a3) \ - if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \ + if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ statename##Data s; \ statename##Get(&s); \ s.a1 = states.shortname[0]; \ @@ -425,7 +418,7 @@ static void StateEstimationCb(void) STORE3(PositionState, pos, North, East, Down); STORE3(VelocityState, vel, North, East, Down); #define STORE2(statename, shortname, a1, a2) \ - if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \ + if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ statename##Data s; \ statename##Get(&s); \ s.a1 = states.shortname[0]; \ @@ -434,7 +427,7 @@ static void StateEstimationCb(void) } STORE2(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed); // attitude nees manual conversion from quaternion to euler - if (ISSET(states.updated, SENSORUPDATES_attitude)) { \ + if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \ AttitudeStateData s; AttitudeStateGet(&s); s.q1 = states.attitude[0]; @@ -490,7 +483,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { HomeLocationGet((HomeLocationData *)&homeLocation); - if (sane(homeLocation.Latitude) && sane(homeLocation.Longitude) && sane(homeLocation.Altitude) && sane(homeLocation.Be[0]) && sane(homeLocation.Be[1]) && sane(homeLocation.Be[2])) { + if (ISREAL(homeLocation.Latitude) && ISREAL(homeLocation.Longitude) && ISREAL(homeLocation.Altitude) && ISREAL(homeLocation.Be[0]) && ISREAL(homeLocation.Be[1]) && ISREAL(homeLocation.Be[2])) { // Compute matrix to convert deltaLLA to NED float lat, alt; lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); @@ -529,7 +522,7 @@ static void sensorUpdatedCb(UAVObjEvent *ev) updatedSensors |= SENSORUPDATES_mag; } - if (ev->obj == GPSPositionSensorHandle()) { + if (ev->obj == PositionSensorHandle()) { updatedSensors |= SENSORUPDATES_pos; } @@ -548,39 +541,6 @@ static void sensorUpdatedCb(UAVObjEvent *ev) DelayedCallbackDispatch(stateEstimationCallback); } -/** - * @brief Convert the GPS LLA position into NED coordinates - * @note this method uses a taylor expansion around the home coordinates - * to convert to NED which allows it to be done with all floating - * calculations - * @param[in] Current GPS coordinates - * @param[out] NED frame coordinates - * @returns 0 for success, -1 for failure - */ -static void getNED(GPSPositionSensorData *gpsPosition, float *NED) -{ - float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f), - DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f), - (gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) }; - - NED[0] = LLA2NEDM[0] * dL[0]; - NED[1] = LLA2NEDM[1] * dL[1]; - NED[2] = LLA2NEDM[2] * dL[2]; -} - -/** - * @brief sanity check for float values - * @note makes sure a float value is safe for further processing, ie not NAN and not INF - * @param[in] float value - * @returns true for safe and false for unsafe - */ -static bool sane(float value) -{ - if (isnan(value) || isinf(value)) { - return false; - } - return true; -} /** * @} diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h index 47c8d798b..8eb823d4b 100644 --- a/flight/pios/inc/pios_math.h +++ b/flight/pios/inc/pios_math.h @@ -46,11 +46,19 @@ #define M_EULER_F 0.57721566490153286060651209008f /* Euler constant */ // Conversion macro -#define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F)) -#define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f)) +#define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F)) +#define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f)) // Useful math macros -#define MAX(a, b) ((a) > (b) ? (a) : (b)) -#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#define MIN(a, b) ((a) < (b) ? (a) : (b)) + +#define ISREAL(f) (!isnan(f) && !isinf(f)) + +// Bitfield access + +#define IS_SET(field, mask) (((field) & (mask)) == (mask)) +#define SET_MASK(field, mask) (field) |= (mask) +#define UNSET_MASK(field, mask) (field) &= ~(mask) #endif // PIOS_MATH_H diff --git a/shared/uavobjectdefinition/positionsensor.xml b/shared/uavobjectdefinition/positionsensor.xml new file mode 100644 index 000000000..34ec55a24 --- /dev/null +++ b/shared/uavobjectdefinition/positionsensor.xml @@ -0,0 +1,12 @@ + + + Contains the position measured relative to @ref HomeLocation + + + + + + + + + From 53ba3f4b74cd96f895c8828741a11a0dc4208a7c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 22 May 2013 23:20:31 +0200 Subject: [PATCH 30/61] removed GPS check from ekf and put into GPS --- flight/modules/GPS/GPS.c | 3 ++- flight/modules/StateEstimation/filterekf.c | 16 ---------------- 2 files changed, 2 insertions(+), 17 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 6c36cb5f3..c6d6d04ff 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -262,7 +262,8 @@ static void gpsTask(__attribute__((unused)) void *parameters) // criteria for GPS-OK taken from this post... // http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 if ((gpspositionsensor.PDOP < 3.5f) && (gpspositionsensor.Satellites >= 7) && - (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D)) { + (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && + (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) { AlarmsClear(SYSTEMALARMS_ALARM_GPS); #ifdef PIOS_GPS_SETS_HOMELOCATION HomeLocationData home; diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index bbbb870af..b672bffae 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -34,7 +34,6 @@ #include #include -#include #include #include @@ -218,21 +217,6 @@ static int32_t filter(stateFilter *self, stateEstimation *state) return 0; } - if (this->usePos) { - GPSPositionSensorData gpsData; - GPSPositionSensorGet(&gpsData); - // Have a minimum requirement for gps usage - if ((gpsData.Satellites < 7) || - (gpsData.PDOP > 4.0f) || - (gpsData.Latitude == 0 && gpsData.Longitude == 0) || - (this->homeLocation.Set != HOMELOCATION_SET_TRUE)) { - UNSET_MASK(state->updated, SENSORUPDATES_pos); - UNSET_MASK(state->updated, SENSORUPDATES_vel); - UNSET_MASK(this->work.updated, SENSORUPDATES_pos); - UNSET_MASK(this->work.updated, SENSORUPDATES_vel); - } - } - dT = PIOS_DELAY_DiffuS(this->ins_last_time) / 1.0e6f; this->ins_last_time = PIOS_DELAY_GetRaw(); From 40864b2d3b6607df0892aaef9cbefd2dd6c67843 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 23 May 2013 21:21:14 +0200 Subject: [PATCH 31/61] some cleanups --- flight/modules/StateEstimation/filtercf.c | 2 ++ flight/modules/StateEstimation/filterekf.c | 2 +- flight/modules/StateEstimation/filtermag.c | 1 + .../modules/StateEstimation/stateestimation.c | 27 +++---------------- flight/pios/inc/pios_math.h | 3 +-- 5 files changed, 8 insertions(+), 27 deletions(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 32ccc1a72..eec23cf11 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -84,6 +84,8 @@ static void globalInit(void) { if (!initialized) { initialized = 1; + FlightStatusInitialize(); + HomeLocationInitialize(); FlightStatusConnectCallback(&flightStatusUpdatedCb); flightStatusUpdatedCb(NULL); } diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index b672bffae..d23a72b53 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -37,7 +37,6 @@ #include #include - #include #include @@ -81,6 +80,7 @@ static void globalInit(void) initialized = 1; EKFConfigurationInitialize(); EKFStateVarianceInitialize(); + HomeLocationInitialize(); } } diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index 8c62cf418..7997016e0 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -62,6 +62,7 @@ int32_t filterMagInitialize(stateFilter *handle) handle->init = &init; handle->filter = &filter; handle->localdata = pvPortMalloc(sizeof(struct data)); + HomeLocationInitialize(); return STACK_REQUIRED; } diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 3b2a413e6..4a0d3c0dc 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -48,7 +48,6 @@ #include #include "revosettings.h" -#include "homelocation.h" #include "flightstatus.h" #include "CoordinateConversions.h" @@ -72,8 +71,6 @@ typedef const struct filterPipelineStruct { static DelayedCallbackInfo *stateEstimationCallback; static volatile RevoSettingsData revoSettings; -static volatile HomeLocationData homeLocation; -static float LLA2NEDM[3]; static volatile sensorUpdates updatedSensors; static int32_t fusionAlgorithm = -1; static filterPipeline *filterChain = NULL; @@ -197,7 +194,6 @@ static inline int32_t maxint32_t(int32_t a, int32_t b) int32_t StateEstimationInitialize(void) { RevoSettingsInitialize(); - HomeLocationInitialize(); GyroSensorInitialize(); MagSensorInitialize(); @@ -214,7 +210,6 @@ int32_t StateEstimationInitialize(void) VelocityStateInitialize(); RevoSettingsConnectCallback(&settingsUpdatedCb); - HomeLocationConnectCallback(&settingsUpdatedCb); GyroSensorConnectCallback(&sensorUpdatedCb); AccelSensorConnectCallback(&sensorUpdatedCb); @@ -344,7 +339,7 @@ static void StateEstimationCb(void) if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ sensorname##Get(&s); \ - if (ISREAL(s.a1) && ISREAL(s.a2) && ISREAL(s.a3)) { \ + if (IS_REAL(s.a1) && IS_REAL(s.a2) && IS_REAL(s.a3)) { \ states.shortname[0] = s.a1; \ states.shortname[1] = s.a2; \ states.shortname[2] = s.a3; \ @@ -362,7 +357,7 @@ static void StateEstimationCb(void) if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ sensorname##Get(&s); \ - if (ISREAL(s.a1) && EXTRACHECK) { \ + if (IS_REAL(s.a1) && EXTRACHECK) { \ states.shortname[0] = s.a1; \ } \ else { \ @@ -477,26 +472,10 @@ static void StateEstimationCb(void) /** - * Callback for eventdispatcher when HomeLocation or RevoSettings has been updated + * Callback for eventdispatcher when RevoSettings has been updated */ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - HomeLocationGet((HomeLocationData *)&homeLocation); - - if (ISREAL(homeLocation.Latitude) && ISREAL(homeLocation.Longitude) && ISREAL(homeLocation.Altitude) && ISREAL(homeLocation.Be[0]) && ISREAL(homeLocation.Be[1]) && ISREAL(homeLocation.Be[2])) { - // Compute matrix to convert deltaLLA to NED - float lat, alt; - lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); - alt = homeLocation.Altitude; - - LLA2NEDM[0] = alt + 6.378137E6f; - LLA2NEDM[1] = cosf(lat) * (alt + 6.378137E6f); - LLA2NEDM[2] = -1.0f; - - // TODO: convert positionState to new reference frame and gracefully update EKF state! - // needed for long range flights where the reference coordinate is adjusted in flight - } - RevoSettingsGet((RevoSettingsData *)&revoSettings); } diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h index 8eb823d4b..da6876d8a 100644 --- a/flight/pios/inc/pios_math.h +++ b/flight/pios/inc/pios_math.h @@ -53,10 +53,9 @@ #define MAX(a, b) ((a) > (b) ? (a) : (b)) #define MIN(a, b) ((a) < (b) ? (a) : (b)) -#define ISREAL(f) (!isnan(f) && !isinf(f)) +#define IS_REAL(f) (!isnan(f) && !isinf(f)) // Bitfield access - #define IS_SET(field, mask) (((field) & (mask)) == (mask)) #define SET_MASK(field, mask) (field) |= (mask) #define UNSET_MASK(field, mask) (field) &= ~(mask) From 259eeecbbc36c9c02a9f94d0cf39ca44a8bb959b Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 23 May 2013 21:56:47 +0200 Subject: [PATCH 32/61] Put Macros in StateEstimation into the file header after request in review. Author expresses concernes about the code readability in this layout (Commit message edited, was: "HATE THIS") --- .../modules/StateEstimation/stateestimation.c | 117 +++++++++--------- 1 file changed, 61 insertions(+), 56 deletions(-) diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 4a0d3c0dc..983751b6e 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -58,6 +58,51 @@ #define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL #define TIMEOUT_MS 10 +// local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_LOAD after the update of states updated! +#define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(sensorname, shortname, a1, a2, a3) \ + if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ + sensorname##Data s; \ + sensorname##Get(&s); \ + if (IS_REAL(s.a1) && IS_REAL(s.a2) && IS_REAL(s.a3)) { \ + states.shortname[0] = s.a1; \ + states.shortname[1] = s.a2; \ + states.shortname[2] = s.a3; \ + } \ + else { \ + UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ + } \ + } +#define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, EXTRACHECK) \ + if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ + sensorname##Data s; \ + sensorname##Get(&s); \ + if (IS_REAL(s.a1) && EXTRACHECK) { \ + states.shortname[0] = s.a1; \ + } \ + else { \ + UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ + } \ + } + +// local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_SAVE before the check of alarms! +#define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(statename, shortname, a1, a2, a3) \ + if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ + statename##Data s; \ + statename##Get(&s); \ + s.a1 = states.shortname[0]; \ + s.a2 = states.shortname[1]; \ + s.a3 = states.shortname[2]; \ + statename##Set(&s); \ + } +#define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(statename, shortname, a1, a2) \ + if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ + statename##Data s; \ + statename##Get(&s); \ + s.a1 = states.shortname[0]; \ + s.a2 = states.shortname[1]; \ + statename##Set(&s); \ + } + // Private types struct filterPipelineStruct; @@ -253,6 +298,7 @@ int32_t StateEstimationStart(void) MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart) + /** * Module callback */ @@ -334,39 +380,15 @@ static void StateEstimationCb(void) states.updated = updatedSensors; updatedSensors ^= states.updated; - // most sensors get only rudimentary sanity checks -#define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \ - if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ - sensorname##Data s; \ - sensorname##Get(&s); \ - if (IS_REAL(s.a1) && IS_REAL(s.a2) && IS_REAL(s.a3)) { \ - states.shortname[0] = s.a1; \ - states.shortname[1] = s.a2; \ - states.shortname[2] = s.a3; \ - } \ - else { \ - UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ - } \ - } - SANITYCHECK3(GyroSensor, gyro, x, y, z); - SANITYCHECK3(AccelSensor, accel, x, y, z); - SANITYCHECK3(MagSensor, mag, x, y, z); - SANITYCHECK3(PositionSensor, pos, North, East, Down); - SANITYCHECK3(GPSVelocitySensor, vel, North, East, Down); -#define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \ - if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ - sensorname##Data s; \ - sensorname##Get(&s); \ - if (IS_REAL(s.a1) && EXTRACHECK) { \ - states.shortname[0] = s.a1; \ - } \ - else { \ - UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ - } \ - } - SANITYCHECK1(BaroSensor, baro, Altitude, 1); - SANITYCHECK1(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); - states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter + // fetch sensors, check values, and load into state struct + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z); + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z); + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z); + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(PositionSensor, pos, North, East, Down); + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down); + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true); + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); + states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter, set to zero for now // at this point sensor state is stored in "states" with some rudimentary filtering applied @@ -398,29 +420,12 @@ static void StateEstimationCb(void) case RUNSTATE_SAVE: // the final output of filters is saved in state variables -#define STORE3(statename, shortname, a1, a2, a3) \ - if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ - statename##Data s; \ - statename##Get(&s); \ - s.a1 = states.shortname[0]; \ - s.a2 = states.shortname[1]; \ - s.a3 = states.shortname[2]; \ - statename##Set(&s); \ - } - STORE3(GyroState, gyro, x, y, z); - STORE3(AccelState, accel, x, y, z); - STORE3(MagState, mag, x, y, z); - STORE3(PositionState, pos, North, East, Down); - STORE3(VelocityState, vel, North, East, Down); -#define STORE2(statename, shortname, a1, a2) \ - if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ - statename##Data s; \ - statename##Get(&s); \ - s.a1 = states.shortname[0]; \ - s.a2 = states.shortname[1]; \ - statename##Set(&s); \ - } - STORE2(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed); + EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z); + EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z); + EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(MagState, mag, x, y, z); + EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down); + EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down); + EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed); // attitude nees manual conversion from quaternion to euler if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \ AttitudeStateData s; From b916df5448808e852c522ea7b9db5c2ee67d72ea Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 24 May 2013 23:59:12 +0200 Subject: [PATCH 33/61] ported cyr's patch to new CF --- flight/modules/StateEstimation/filtercf.c | 36 ++++++++++++------- .../uavobjectdefinition/attitudesettings.xml | 2 ++ 2 files changed, 25 insertions(+), 13 deletions(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index eec23cf11..a9ef506ab 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -59,6 +59,7 @@ struct data { bool magUpdated; float accel_alpha; bool accel_filter_enabled; + float rollPitchBiasRate; int32_t timeval; uint8_t init; }; @@ -207,8 +208,6 @@ static inline void apply_accel_filter(const struct data *this, const float *raw, static int32_t complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]) { float dT; - float magKp = 0.0f; // TODO: make this non hardcoded at some point - float magKi = 0.000001f; // During initialization and if (this->first_run) { @@ -261,19 +260,26 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel if ((this->init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { // For first 7 seconds use accels to get gyro bias this->attitudeSettings.AccelKp = 1.0f; - this->attitudeSettings.AccelKi = 0.9f; + this->attitudeSettings.AccelKi = 0.0f; this->attitudeSettings.YawBiasRate = 0.23f; - magKp = 1.0f; + this->accel_filter_enabled = false; + this->rollPitchBiasRate = 0.01f; + this->attitudeSettings.MagKp = 1.0f; } else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { this->attitudeSettings.AccelKp = 1.0f; - this->attitudeSettings.AccelKi = 0.9f; + this->attitudeSettings.AccelKi = 0.0f; this->attitudeSettings.YawBiasRate = 0.23f; - magKp = 1.0f; + this->accel_filter_enabled = false; + this->rollPitchBiasRate = 0.01f; + this->attitudeSettings.MagKp = 1.0f; this->init = 0; } else if (this->init == 0) { // Reload settings (all the rates) AttitudeSettingsGet(&this->attitudeSettings); - magKp = 0.01f; + this->rollPitchBiasRate = 0.0f; + if (this->accel_alpha > 0.0f) { + this->accel_filter_enabled = true; + } this->init = 1; } @@ -356,22 +362,26 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; } - // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s - this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi; - this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi; - this->gyroBias[2] -= mag_err[2] * magKi; - // Correct rates based on integral coefficient gyro[0] -= this->gyroBias[0]; gyro[1] -= this->gyroBias[1]; gyro[2] -= this->gyroBias[2]; + // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s + this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi - gyro[0] * this->rollPitchBiasRate; + this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi - gyro[1] * this->rollPitchBiasRate;; + if (this->useMag) { + this->gyroBias[2] -= mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate; + } else { + this->gyroBias[2] -= -gyro[2] * this->rollPitchBiasRate; + } + float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] }; // Correct rates based on proportional coefficient gyrotmp[0] += accel_err[0] * this->attitudeSettings.AccelKp / dT; gyrotmp[1] += accel_err[1] * this->attitudeSettings.AccelKp / dT; if (this->useMag) { - gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; + gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT + mag_err[2] * this->attitudeSettings.MagKp / dT; } else { gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT; } diff --git a/shared/uavobjectdefinition/attitudesettings.xml b/shared/uavobjectdefinition/attitudesettings.xml index 274a2ef30..56dac2f46 100644 --- a/shared/uavobjectdefinition/attitudesettings.xml +++ b/shared/uavobjectdefinition/attitudesettings.xml @@ -7,6 +7,8 @@ + + From 902bf29c920cbe1000a68a06e545d33bae3ae50c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 25 May 2013 00:05:06 +0200 Subject: [PATCH 34/61] some small bugfixes to filterekf --- flight/modules/StateEstimation/filterekf.c | 35 +++++++++++----------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index d23a72b53..a0a3c9d1e 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -44,6 +44,14 @@ #define STACK_REQUIRED 2048 +#define IMPORT_SENSOR_IF_UPDATED(shortname, num) \ + if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \ + uint8_t t; \ + for (t = 0; t < num; t++) { \ + this->work.shortname[t] = state->shortname[t]; \ + } \ + } + // Private types struct data { EKFConfigurationData ekfConfiguration; @@ -191,20 +199,13 @@ static int32_t filter(stateFilter *self, stateEstimation *state) this->work.updated |= state->updated; // Get most recent data -#define UPDATE(shortname, num) \ - if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \ - uint8_t t; \ - for (t = 0; t < num; t++) { \ - this->work.shortname[t] = state->shortname[t]; \ - } \ - } - UPDATE(gyro, 3); - UPDATE(accel, 3); - UPDATE(mag, 3); - UPDATE(baro, 1); - UPDATE(pos, 3); - UPDATE(vel, 3); - UPDATE(airspeed, 2); + IMPORT_SENSOR_IF_UPDATED(gyro, 3); + IMPORT_SENSOR_IF_UPDATED(accel, 3); + IMPORT_SENSOR_IF_UPDATED(mag, 3); + IMPORT_SENSOR_IF_UPDATED(baro, 1); + IMPORT_SENSOR_IF_UPDATED(pos, 3); + IMPORT_SENSOR_IF_UPDATED(vel, 3); + IMPORT_SENSOR_IF_UPDATED(airspeed, 2); // check whether mandatory updates are present accels must have been supplied already, // and gyros must be supplied just now for a prediction step to take place @@ -332,9 +333,9 @@ static int32_t filter(stateFilter *self, stateEstimation *state) state->attitude[1] = Nav.q[1]; state->attitude[2] = Nav.q[2]; state->attitude[3] = Nav.q[3]; - state->gyro[0] += Nav.gyro_bias[0]; - state->gyro[1] += Nav.gyro_bias[1]; - state->gyro[2] += Nav.gyro_bias[2]; + state->gyro[0] -= Nav.gyro_bias[0]; + state->gyro[1] -= Nav.gyro_bias[1]; + state->gyro[2] -= Nav.gyro_bias[2]; state->pos[0] = Nav.Pos[0]; state->pos[1] = Nav.Pos[1]; state->pos[2] = Nav.Pos[2]; From 87b1b4873c8f50c31b11419dc809c66cbf49ffa8 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 25 May 2013 02:02:53 +0200 Subject: [PATCH 35/61] bugfix in baro alt filter plugin --- flight/modules/StateEstimation/filterbaro.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index 0d8e83f67..7c5793717 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -45,6 +45,7 @@ // Private types struct data { float baroOffset; + float baroAlt; bool first_run; }; @@ -82,6 +83,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (IS_SET(state->updated, SENSORUPDATES_baro)) { this->first_run = 0; this->baroOffset = state->baro[0]; + this->baroAlt = state->baro[0]; } } else { // Track barometric altitude offset with a low pass filter @@ -89,10 +91,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (IS_SET(state->updated, SENSORUPDATES_pos)) { this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset + (1.0f - BARO_OFFSET_LOWPASS_ALPHA) - * (-state->pos[2] - state->baro[0]); + * (this->baroAlt + state->pos[2]); } // calculate bias corrected altitude if (IS_SET(state->updated, SENSORUPDATES_baro)) { + this->baroAlt = state->baro[0]; state->baro[0] -= this->baroOffset; } } From 6c25e826339316ad0e24ccd1a7747f699014e707 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 25 May 2013 12:28:32 +0200 Subject: [PATCH 36/61] fixed bugs in GPS, corrected zeroing of updated flags in stateestimation --- flight/modules/GPS/GPS.c | 6 +++--- flight/modules/StateEstimation/stateestimation.c | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index c6d6d04ff..42b85f137 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -72,7 +72,7 @@ static void setPositionSensor(GPSPositionSensorData *gpsData); #ifdef PIOS_GPS_SETS_HOMELOCATION // Unfortunately need a good size stack for the WMM calculation - #define STACK_SIZE_BYTES 850 + #define STACK_SIZE_BYTES 1024 #else #if defined(PIOS_GPS_MINIMAL) #define STACK_SIZE_BYTES 500 @@ -349,11 +349,11 @@ static void setPositionSensor(GPSPositionSensorData *gpsData) float ECEF[3]; float Rne[3][3]; { - float LLA[3] = { home.Latitude, home.Longitude, home.Altitude }; + float LLA[3] = { (home.Latitude) / 10e6f, (home.Longitude) / 10e6f, (home.Altitude) }; LLA2ECEF(LLA, ECEF); RneFromLLA(LLA, Rne); } - { float LLA[3] = { gpsData->Latitude, gpsData->Longitude, gpsData->Altitude + gpsData->GeoidSeparation }; + { float LLA[3] = { (gpsData->Latitude) / 10e6f, (gpsData->Longitude) / 10e6f, gpsData->Altitude + gpsData->GeoidSeparation }; float NED[3]; LLA2Base(LLA, ECEF, Rne, NED); pos.North = NED[0]; diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 983751b6e..6efe1adb8 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -377,8 +377,8 @@ static void StateEstimationCb(void) } // read updated sensor UAVObjects and set initial state - states.updated = updatedSensors; - updatedSensors ^= states.updated; + states.updated = updatedSensors; + updatedSensors = 0; // fetch sensors, check values, and load into state struct FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z); From 7bf58f09c5984fedff0433e581acf8167478f7b8 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 25 May 2013 16:18:18 +0200 Subject: [PATCH 37/61] filterekf gyro bias adjustment bugfix --- flight/modules/StateEstimation/filterekf.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index a0a3c9d1e..762b5e8ba 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -298,9 +298,9 @@ static int32_t filter(stateFilter *self, stateEstimation *state) state->attitude[1] = Nav.q[1]; state->attitude[2] = Nav.q[2]; state->attitude[3] = Nav.q[3]; - state->gyro[0] += Nav.gyro_bias[0]; - state->gyro[1] += Nav.gyro_bias[1]; - state->gyro[2] += Nav.gyro_bias[2]; + state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]); + state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]); + state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]); state->pos[0] = Nav.Pos[0]; state->pos[1] = Nav.Pos[1]; state->pos[2] = Nav.Pos[2]; @@ -333,9 +333,9 @@ static int32_t filter(stateFilter *self, stateEstimation *state) state->attitude[1] = Nav.q[1]; state->attitude[2] = Nav.q[2]; state->attitude[3] = Nav.q[3]; - state->gyro[0] -= Nav.gyro_bias[0]; - state->gyro[1] -= Nav.gyro_bias[1]; - state->gyro[2] -= Nav.gyro_bias[2]; + state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]); + state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]); + state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]); state->pos[0] = Nav.Pos[0]; state->pos[1] = Nav.Pos[1]; state->pos[2] = Nav.Pos[2]; From e82a52e8ff4998d3b1e45de635af8b216753091d Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 25 May 2013 23:06:30 +0200 Subject: [PATCH 38/61] filterekf: changed mag variance to 1/1000000 to compensate for normalization, as it was done on old AHRS --- flight/modules/StateEstimation/filterekf.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 762b5e8ba..cdf8a7b99 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -233,9 +233,10 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (this->init_stage == 0) { // Reset the INS algorithm INSGPSInit(); - INSSetMagVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGX], - this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY], - this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] } + // variance is measured in mGaus, but internally the EKF works with a normalized vector. Scaling factor from old AHRS code (1000*1000) + INSSetMagVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGX] / 1e6f, + this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY] / 1e6f, + this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] / 1e6f } ); INSSetAccelVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX], this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY], From 6435604182ad4bae7e9a86999815b8f168035f25 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 29 May 2013 22:18:08 +0200 Subject: [PATCH 39/61] some fixes and corrections as suggested in review --- flight/modules/Attitude/revolution/attitude.c | 9 +++++---- flight/modules/StateEstimation/filtercf.c | 2 +- shared/uavobjectdefinition/accelsensor.xml | 4 ++-- shared/uavobjectdefinition/accelstate.xml | 2 +- shared/uavobjectdefinition/airspeedsensor.xml | 2 +- shared/uavobjectdefinition/airspeedstate.xml | 4 ++-- shared/uavobjectdefinition/attitudestate.xml | 4 ++-- shared/uavobjectdefinition/barosensor.xml | 2 +- shared/uavobjectdefinition/gpspositionsensor.xml | 4 ++-- shared/uavobjectdefinition/gpsvelocitysensor.xml | 4 ++-- shared/uavobjectdefinition/gyrosensor.xml | 4 ++-- shared/uavobjectdefinition/gyrostate.xml | 2 +- shared/uavobjectdefinition/magsensor.xml | 4 ++-- shared/uavobjectdefinition/magstate.xml | 4 ++-- shared/uavobjectdefinition/positionsensor.xml | 4 ++-- shared/uavobjectdefinition/positionstate.xml | 4 ++-- shared/uavobjectdefinition/velocitystate.xml | 4 ++-- 17 files changed, 32 insertions(+), 31 deletions(-) diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index da80f5ca4..8d0c5f38e 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -703,8 +703,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) magData.y = mags.y; magData.z = mags.z; MagStateSet(&magData); + } else { + MagStateGet(&magData); } - MagStateGet(&magData); BaroSensorGet(&baroData); AirspeedSensorGet(&airspeedData); @@ -1051,9 +1052,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) VelocityStateSet(&velocityState); GyroStateData gyroState; - gyroState.x = RAD2DEG(gyros[0] - Nav.gyro_bias[0]); - gyroState.y = RAD2DEG(gyros[1] - Nav.gyro_bias[1]); - gyroState.z = RAD2DEG(gyros[2] - Nav.gyro_bias[2]); + gyroState.x = RAD2DEG(gyros[0] - RAD2DEG(Nav.gyro_bias[0])); + gyroState.y = RAD2DEG(gyros[1] - RAD2DEG(Nav.gyro_bias[1])); + gyroState.z = RAD2DEG(gyros[2] - RAD2DEG(Nav.gyro_bias[2])); GyroStateSet(&gyroState); EKFStateVarianceData vardata; diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index a9ef506ab..4bbf0da10 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -369,7 +369,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi - gyro[0] * this->rollPitchBiasRate; - this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi - gyro[1] * this->rollPitchBiasRate;; + this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi - gyro[1] * this->rollPitchBiasRate; if (this->useMag) { this->gyroBias[2] -= mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate; } else { diff --git a/shared/uavobjectdefinition/accelsensor.xml b/shared/uavobjectdefinition/accelsensor.xml index 664533eb2..66b97a3f4 100644 --- a/shared/uavobjectdefinition/accelsensor.xml +++ b/shared/uavobjectdefinition/accelsensor.xml @@ -1,6 +1,6 @@ - - The accel data. + + Calibrated sensor data from 3 axis accelerometer in m/s². diff --git a/shared/uavobjectdefinition/accelstate.xml b/shared/uavobjectdefinition/accelstate.xml index c2236e875..19060ebe4 100644 --- a/shared/uavobjectdefinition/accelstate.xml +++ b/shared/uavobjectdefinition/accelstate.xml @@ -1,5 +1,5 @@ - + The filtered acceleration data. diff --git a/shared/uavobjectdefinition/airspeedsensor.xml b/shared/uavobjectdefinition/airspeedsensor.xml index 0ac1f3ecd..8f55cbeae 100644 --- a/shared/uavobjectdefinition/airspeedsensor.xml +++ b/shared/uavobjectdefinition/airspeedsensor.xml @@ -1,5 +1,5 @@ - + The raw data from the dynamic pressure sensor with pressure, temperature and airspeed. diff --git a/shared/uavobjectdefinition/airspeedstate.xml b/shared/uavobjectdefinition/airspeedstate.xml index f1b1a78f7..3a7db51d9 100644 --- a/shared/uavobjectdefinition/airspeedstate.xml +++ b/shared/uavobjectdefinition/airspeedstate.xml @@ -1,6 +1,6 @@ - - UAVO for true airspeed, calibrated airspeed, angle of attack, and angle of slip + + UAVO for true airspeed and calibrated airspeed state estimation. diff --git a/shared/uavobjectdefinition/attitudestate.xml b/shared/uavobjectdefinition/attitudestate.xml index ae89f073a..98ceaa3c7 100644 --- a/shared/uavobjectdefinition/attitudestate.xml +++ b/shared/uavobjectdefinition/attitudestate.xml @@ -1,6 +1,6 @@ - - The updated Attitude estimation from @ref AHRSCommsModule. + + The updated Attitude estimation from @ref StateEstimationModule. diff --git a/shared/uavobjectdefinition/barosensor.xml b/shared/uavobjectdefinition/barosensor.xml index 32f49c17e..0730f3078 100644 --- a/shared/uavobjectdefinition/barosensor.xml +++ b/shared/uavobjectdefinition/barosensor.xml @@ -1,5 +1,5 @@ - + The raw data from the barometric sensor with pressure, temperature and altitude estimate. diff --git a/shared/uavobjectdefinition/gpspositionsensor.xml b/shared/uavobjectdefinition/gpspositionsensor.xml index e7a497ab4..8686a76ef 100644 --- a/shared/uavobjectdefinition/gpspositionsensor.xml +++ b/shared/uavobjectdefinition/gpspositionsensor.xml @@ -1,6 +1,6 @@ - - Raw GPS data from @ref GPSModule. Should only be used by @ref AHRSCommsModule. + + Raw GPS data from @ref GPSModule. diff --git a/shared/uavobjectdefinition/gpsvelocitysensor.xml b/shared/uavobjectdefinition/gpsvelocitysensor.xml index 21e32338e..c94474ca2 100644 --- a/shared/uavobjectdefinition/gpsvelocitysensor.xml +++ b/shared/uavobjectdefinition/gpsvelocitysensor.xml @@ -1,6 +1,6 @@ - - Raw GPS data from @ref GPSModule. + + Raw GPS velocity in NED frame and m/s from @ref GPSModule. diff --git a/shared/uavobjectdefinition/gyrosensor.xml b/shared/uavobjectdefinition/gyrosensor.xml index 36f2d3f0b..d92080fb8 100644 --- a/shared/uavobjectdefinition/gyrosensor.xml +++ b/shared/uavobjectdefinition/gyrosensor.xml @@ -1,6 +1,6 @@ - - The gyro data. + + Calibrated sensor data from 3 axis gyroscope in deg/s. diff --git a/shared/uavobjectdefinition/gyrostate.xml b/shared/uavobjectdefinition/gyrostate.xml index b8111b33e..a4d497578 100644 --- a/shared/uavobjectdefinition/gyrostate.xml +++ b/shared/uavobjectdefinition/gyrostate.xml @@ -1,5 +1,5 @@ - + The filtered rotation sensor data. diff --git a/shared/uavobjectdefinition/magsensor.xml b/shared/uavobjectdefinition/magsensor.xml index 1681713b6..8622bddd9 100644 --- a/shared/uavobjectdefinition/magsensor.xml +++ b/shared/uavobjectdefinition/magsensor.xml @@ -1,6 +1,6 @@ - - The mag data. + + Calibrated sensor data from 3 axis magnetometer in MilliGauss. diff --git a/shared/uavobjectdefinition/magstate.xml b/shared/uavobjectdefinition/magstate.xml index 85b2bb458..1cd0fefaa 100644 --- a/shared/uavobjectdefinition/magstate.xml +++ b/shared/uavobjectdefinition/magstate.xml @@ -1,6 +1,6 @@ - - The mag data. + + The filtered magnet vector. diff --git a/shared/uavobjectdefinition/positionsensor.xml b/shared/uavobjectdefinition/positionsensor.xml index 34ec55a24..dfa16b803 100644 --- a/shared/uavobjectdefinition/positionsensor.xml +++ b/shared/uavobjectdefinition/positionsensor.xml @@ -1,6 +1,6 @@ - - Contains the position measured relative to @ref HomeLocation + + Contains the position in NED frame measured relative to @ref HomeLocation. diff --git a/shared/uavobjectdefinition/positionstate.xml b/shared/uavobjectdefinition/positionstate.xml index afa3c201b..207ded992 100644 --- a/shared/uavobjectdefinition/positionstate.xml +++ b/shared/uavobjectdefinition/positionstate.xml @@ -1,6 +1,6 @@ - - Contains the current position relative to @ref HomeLocation + + Contains the estimate of the current position relative to @ref HomeLocation, in NED coordinates diff --git a/shared/uavobjectdefinition/velocitystate.xml b/shared/uavobjectdefinition/velocitystate.xml index ad50b4725..90a89a4dd 100644 --- a/shared/uavobjectdefinition/velocitystate.xml +++ b/shared/uavobjectdefinition/velocitystate.xml @@ -1,6 +1,6 @@ - - Updated by filters, velocity relative to @ref HomeLocation + + Updated by @ref StateEstimationModule, velocity relative to @ref HomeLocation. From cdf9eaba64f8893ab0decf0c60deda420a002ba4 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 29 May 2013 23:39:18 +0200 Subject: [PATCH 40/61] new default variances to go with new mag var calculation --- shared/uavobjectdefinition/ekfconfiguration.xml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/shared/uavobjectdefinition/ekfconfiguration.xml b/shared/uavobjectdefinition/ekfconfiguration.xml index 73fafdfdc..fa2b8dc8f 100644 --- a/shared/uavobjectdefinition/ekfconfiguration.xml +++ b/shared/uavobjectdefinition/ekfconfiguration.xml @@ -23,9 +23,9 @@ + 0.000000002, 0.000000002, 0.000000002"> GyroX GyroY @@ -41,7 +41,7 @@ GPSPosNorth @@ -57,8 +57,8 @@ FakeGPSPosIndoor From dc68d7d94e695777c3ad8c23890cba2aa229bb6e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 5 Jun 2013 17:50:00 +0200 Subject: [PATCH 41/61] changed insgps, removed unnecessary gain representation --- flight/libraries/insgps13state.c | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index 414cbfb33..d014babb1 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -44,7 +44,7 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], float Q[NUMW], float dT, float P[NUMX][NUMX]); void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], - float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], float K[NUMX][NUMV], + float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], uint16_t SensorsUsed); void RungeKutta(float X[NUMX], float U[NUMU], float dT); void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]); @@ -97,7 +97,6 @@ static struct EKFData { // input noise and measurement noise variances float Q[NUMW]; float R[NUMV]; - float K[NUMX][NUMV]; // feedback gain matrix } ekf; // Global variables @@ -129,7 +128,6 @@ void INSGPSInit() // pretty much just a place holder for now for (int j = 0; j < NUMV; j++) { ekf.H[j][i] = 0.0f; - ekf.K[i][j] = 0.0f; } ekf.X[i] = 0.0f; @@ -402,7 +400,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], // EKF correction step LinearizeH(ekf.X, ekf.Be, ekf.H); MeasurementEq(ekf.X, ekf.Be, Y); - SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, ekf.K, SensorsUsed); + SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed); qmag = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); ekf.X[6] /= qmag; ekf.X[7] /= qmag; @@ -515,11 +513,12 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], // ************************************************ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], - float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], float K[NUMX][NUMV], + float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], uint16_t SensorsUsed) { float HP[NUMX], HPHR, Error; uint8_t i, j, k, m; + float Km[NUMX]; for (m = 0; m < NUMV; m++) { if (SensorsUsed & (0x01 << m)) { // use this sensor for update @@ -535,18 +534,18 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], } for (k = 0; k < NUMX; k++) { - K[k][m] = HP[k] / HPHR; // find K = HP/HPHR + Km[k] = HP[k] / HPHR; // find K = HP/HPHR } for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP for (j = i; j < NUMX; j++) { P[i][j] = P[j][i] = - P[i][j] - K[i][m] * HP[j]; + P[i][j] - Km[i] * HP[j]; } } Error = Z[m] - Y[m]; for (i = 0; i < NUMX; i++) { // Find X(m)= X(m-1) + K*Error - X[i] = X[i] + K[i][m] * Error; + X[i] = X[i] + Km[i] * Error; } } } From 5e306250a55143cb575d35e197ec7b9d237be8cb Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 5 Jun 2013 19:11:42 +0200 Subject: [PATCH 42/61] Enabled simulated sensors in SimPosix for debugging of EKF behaviour including: - SimPosix again stores and reads UAVObjects to disk (was disabled by someone) - simulated sensors moved to allow automated module initialisation in simposix - fixed a categorization bug in a uavobject --- flight/modules/Sensors/simulated/{ => Sensors}/sensors.c | 8 ++++---- flight/targets/boards/simposix/firmware/Makefile | 1 + flight/targets/boards/simposix/firmware/UAVObjects.inc | 1 + flight/targets/boards/simposix/firmware/inc/pios_config.h | 1 + shared/uavobjectdefinition/gyrosensor.xml | 2 +- 5 files changed, 8 insertions(+), 5 deletions(-) rename flight/modules/Sensors/simulated/{ => Sensors}/sensors.c (99%) diff --git a/flight/modules/Sensors/simulated/sensors.c b/flight/modules/Sensors/simulated/Sensors/sensors.c similarity index 99% rename from flight/modules/Sensors/simulated/sensors.c rename to flight/modules/Sensors/simulated/Sensors/sensors.c index dd19a6d2d..86aa89d5d 100644 --- a/flight/modules/Sensors/simulated/sensors.c +++ b/flight/modules/Sensors/simulated/Sensors/sensors.c @@ -48,20 +48,21 @@ #include -#include "attitude.h" +#include "attitudestate.h" #include "accelsensor.h" #include "actuatordesired.h" #include "attitudestate.h" #include "attitudesimulated.h" #include "attitudesettings.h" -#include "rawairspeed.h" +#include "airspeedsensor.h" #include "barosensor.h" +#include "magsensor.h" #include "gyrosensor.h" #include "flightstatus.h" #include "gpspositionsensor.h" #include "gpsvelocitysensor.h" #include "homelocation.h" -#include "sensor.h" +// #include "sensor.h" #include "ratedesired.h" #include "revocalibration.h" #include "systemsettings.h" @@ -114,7 +115,6 @@ int32_t SensorsInitialize(void) GPSPositionSensorInitialize(); GPSVelocitySensorInitialize(); MagSensorInitialize(); - MagBiasInitialize(); RevoCalibrationInitialize(); return 0; diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index ae7a6a03c..badf65db1 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -39,6 +39,7 @@ MODULES += CameraStab MODULES += Telemetry MODULES += FirmwareIAP MODULES += StateEstimation +MODULES += Sensors/simulated/Sensors #MODULES += OveroSync # Paths diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 4901ca9eb..58ad986ca 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -31,6 +31,7 @@ UAVOBJSRCFILENAMES += actuatorsettings UAVOBJSRCFILENAMES += altholdsmoothed UAVOBJSRCFILENAMES += attitudesettings UAVOBJSRCFILENAMES += attitudestate +UAVOBJSRCFILENAMES += attitudesimulated UAVOBJSRCFILENAMES += gyrostate UAVOBJSRCFILENAMES += gyrosensor UAVOBJSRCFILENAMES += accelstate diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h index 70e3ed2b6..afa9557fa 100644 --- a/flight/targets/boards/simposix/firmware/inc/pios_config.h +++ b/flight/targets/boards/simposix/firmware/inc/pios_config.h @@ -45,6 +45,7 @@ #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_LED #define PIOS_INCLUDE_SDCARD +#define PIOS_USE_SETTINGS_ON_SDCARD // #define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_SERVO #define PIOS_INCLUDE_SPI diff --git a/shared/uavobjectdefinition/gyrosensor.xml b/shared/uavobjectdefinition/gyrosensor.xml index d92080fb8..3107f952e 100644 --- a/shared/uavobjectdefinition/gyrosensor.xml +++ b/shared/uavobjectdefinition/gyrosensor.xml @@ -1,5 +1,5 @@ - + Calibrated sensor data from 3 axis gyroscope in deg/s. From 6f6ca2481ef2e4d3d23e5298f371a5abf23f91f4 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 5 Jun 2013 20:40:49 +0200 Subject: [PATCH 43/61] Changed CoordinateConversion to use doubles for LLA2NED conversion, so coordinates can be represented in ECEF with sufficient resolution --- flight/libraries/CoordinateConversions.c | 92 ++++++++++---------- flight/libraries/inc/CoordinateConversions.h | 10 +-- flight/modules/GPS/GPS.c | 6 +- flight/pios/inc/pios_math.h | 21 +++++ 4 files changed, 75 insertions(+), 54 deletions(-) diff --git a/flight/libraries/CoordinateConversions.c b/flight/libraries/CoordinateConversions.c index 5d286aac1..1b0a51277 100644 --- a/flight/libraries/CoordinateConversions.c +++ b/flight/libraries/CoordinateConversions.c @@ -35,27 +35,27 @@ #define MIN_ALLOWABLE_MAGNITUDE 1e-30f // ****** convert Lat,Lon,Alt to ECEF ************ -void LLA2ECEF(float LLA[3], float ECEF[3]) +void LLA2ECEF(float LLA[3], double ECEF[3]) { - const float a = 6378137.0f; // Equatorial Radius - const float e = 8.1819190842622e-2f; // Eccentricity - float sinLat, sinLon, cosLat, cosLon; - float N; + const double a = 6378137.0d; // Equatorial Radius + const double e = 8.1819190842622e-2d; // Eccentricity + double sinLat, sinLon, cosLat, cosLon; + double N; - sinLat = sinf(DEG2RAD(LLA[0])); - sinLon = sinf(DEG2RAD(LLA[1])); - cosLat = cosf(DEG2RAD(LLA[0])); - cosLon = cosf(DEG2RAD(LLA[1])); + sinLat = sin(DEG2RAD(LLA[0])); + sinLon = sin(DEG2RAD(LLA[1])); + cosLat = cos(DEG2RAD(LLA[0])); + cosLon = cos(DEG2RAD(LLA[1])); - N = a / sqrtf(1.0f - e * e * sinLat * sinLat); // prime vertical radius of curvature + N = a / sqrt(1.0d - e * e * sinLat * sinLat); // prime vertical radius of curvature - ECEF[0] = (N + LLA[2]) * cosLat * cosLon; - ECEF[1] = (N + LLA[2]) * cosLat * sinLon; - ECEF[2] = ((1 - e * e) * N + LLA[2]) * sinLat; + ECEF[0] = (N + (double)LLA[2]) * cosLat * cosLon; + ECEF[1] = (N + (double)LLA[2]) * cosLat * sinLon; + ECEF[2] = ((1 - e * e) * N + (double)LLA[2]) * sinLat; } // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* -uint16_t ECEF2LLA(float ECEF[3], float LLA[3]) +uint16_t ECEF2LLA(double ECEF[3], float LLA[3]) { /** * LLA parameter is used to prime the iteration. @@ -66,48 +66,48 @@ uint16_t ECEF2LLA(float ECEF[3], float LLA[3]) * Suggestion: [0,0,0] **/ - const float a = 6378137.0f; // Equatorial Radius - const float e = 8.1819190842622e-2f; // Eccentricity - float x = ECEF[0], y = ECEF[1], z = ECEF[2]; - float Lat, N, NplusH, delta, esLat; + const double a = 6378137.0f; // Equatorial Radius + const double e = 8.1819190842622e-2f; // Eccentricity + double x = ECEF[0], y = ECEF[1], z = ECEF[2]; + double Lat, N, NplusH, delta, esLat; uint16_t iter; #define MAX_ITER 10 // should not take more than 5 for valid coordinates -#define ACCURACY 1.0e-11f // used to be e-14, but we don't need sub micrometer exact calculations +#define ACCURACY 1.0e-11d // used to be e-14, but we don't need sub micrometer exact calculations - LLA[1] = RAD2DEG(atan2f(y, x)); - Lat = DEG2RAD(LLA[0]); - esLat = e * sinf(Lat); - N = a / sqrtf(1 - esLat * esLat); - NplusH = N + LLA[2]; + LLA[1] = (float)RAD2DEG_D(atan2(y, x)); + Lat = DEG2RAD_D((double)LLA[0]); + esLat = e * sin(Lat); + N = a / sqrt(1 - esLat * esLat); + NplusH = N + (double)LLA[2]; delta = 1; iter = 0; while (((delta > ACCURACY) || (delta < -ACCURACY)) && (iter < MAX_ITER)) { - delta = Lat - atanf(z / (sqrtf(x * x + y * y) * (1 - (N * e * e / NplusH)))); + delta = Lat - atan(z / (sqrt(x * x + y * y) * (1 - (N * e * e / NplusH)))); Lat = Lat - delta; - esLat = e * sinf(Lat); - N = a / sqrtf(1 - esLat * esLat); - NplusH = sqrtf(x * x + y * y) / cosf(Lat); + esLat = e * sin(Lat); + N = a / sqrt(1 - esLat * esLat); + NplusH = sqrt(x * x + y * y) / cos(Lat); iter += 1; } - LLA[0] = RAD2DEG(Lat); + LLA[0] = RAD2DEG_D(Lat); LLA[2] = NplusH - N; return iter < MAX_ITER; } // ****** find ECEF to NED rotation matrix ******** -void RneFromLLA(float LLA[3], float Rne[3][3]) +void RneFromLLA(float LLA[3], double Rne[3][3]) { - float sinLat, sinLon, cosLat, cosLon; + double sinLat, sinLon, cosLat, cosLon; - sinLat = (float)sinf(DEG2RAD(LLA[0])); - sinLon = (float)sinf(DEG2RAD(LLA[1])); - cosLat = (float)cosf(DEG2RAD(LLA[0])); - cosLon = (float)cosf(DEG2RAD(LLA[1])); + sinLat = sin(DEG2RAD(LLA[0])); + sinLon = sin(DEG2RAD(LLA[1])); + cosLat = cos(DEG2RAD(LLA[0])); + cosLon = cos(DEG2RAD(LLA[1])); Rne[0][0] = -sinLat * cosLon; Rne[0][1] = -sinLat * sinLon; @@ -188,16 +188,16 @@ void Quaternion2R(float q[4], float Rbe[3][3]) } // ****** Express LLA in a local NED Base Frame ******** -void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3]) +void LLA2Base(float LLA[3], double BaseECEF[3], double Rne[3][3], float NED[3]) { - float ECEF[3]; - float diff[3]; + double ECEF[3]; + double diff[3]; LLA2ECEF(LLA, ECEF); - diff[0] = (float)(ECEF[0] - BaseECEF[0]); - diff[1] = (float)(ECEF[1] - BaseECEF[1]); - diff[2] = (float)(ECEF[2] - BaseECEF[2]); + diff[0] = (ECEF[0] - BaseECEF[0]); + diff[1] = (ECEF[1] - BaseECEF[1]); + diff[2] = (ECEF[2] - BaseECEF[2]); NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; @@ -205,13 +205,13 @@ void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3]) } // ****** Express ECEF in a local NED Base Frame ******** -void ECEF2Base(float ECEF[3], float BaseECEF[3], float Rne[3][3], float NED[3]) +void ECEF2Base(double ECEF[3], double BaseECEF[3], double Rne[3][3], float NED[3]) { - float diff[3]; + double diff[3]; - diff[0] = (float)(ECEF[0] - BaseECEF[0]); - diff[1] = (float)(ECEF[1] - BaseECEF[1]); - diff[2] = (float)(ECEF[2] - BaseECEF[2]); + diff[0] = (ECEF[0] - BaseECEF[0]); + diff[1] = (ECEF[1] - BaseECEF[1]); + diff[2] = (ECEF[2] - BaseECEF[2]); NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h index 46b100e0f..5e25f2053 100644 --- a/flight/libraries/inc/CoordinateConversions.h +++ b/flight/libraries/inc/CoordinateConversions.h @@ -31,12 +31,12 @@ #define COORDINATECONVERSIONS_H_ // ****** convert Lat,Lon,Alt to ECEF ************ -void LLA2ECEF(float LLA[3], float ECEF[3]); +void LLA2ECEF(float LLA[3], double ECEF[3]); // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* -uint16_t ECEF2LLA(float ECEF[3], float LLA[3]); +uint16_t ECEF2LLA(double ECEF[3], float LLA[3]); -void RneFromLLA(float LLA[3], float Rne[3][3]); +void RneFromLLA(float LLA[3], double Rne[3][3]); // ****** find rotation matrix from rotation vector void Rv2Rot(float Rv[3], float R[3][3]); @@ -51,10 +51,10 @@ void RPY2Quaternion(const float rpy[3], float q[4]); void Quaternion2R(float q[4], float Rbe[3][3]); // ****** Express LLA in a local NED Base Frame ******** -void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3]); +void LLA2Base(float LLA[3], double BaseECEF[3], double Rne[3][3], float NED[3]); // ****** Express ECEF in a local NED Base Frame ******** -void ECEF2Base(float ECEF[3], float BaseECEF[3], float Rne[3][3], float NED[3]); +void ECEF2Base(double ECEF[3], double BaseECEF[3], double Rne[3][3], float NED[3]); // ****** convert Rotation Matrix to Quaternion ******** // ****** if R converts from e to b, q is rotation from e to b **** diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 42b85f137..5f844aa82 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -346,14 +346,14 @@ static void setPositionSensor(GPSPositionSensorData *gpsData) PositionSensorData pos; PositionSensorGet(&pos); - float ECEF[3]; - float Rne[3][3]; + double ECEF[3]; + double Rne[3][3]; { float LLA[3] = { (home.Latitude) / 10e6f, (home.Longitude) / 10e6f, (home.Altitude) }; LLA2ECEF(LLA, ECEF); RneFromLLA(LLA, Rne); } - { float LLA[3] = { (gpsData->Latitude) / 10e6f, (gpsData->Longitude) / 10e6f, gpsData->Altitude + gpsData->GeoidSeparation }; + { float LLA[3] = { (gpsData->Latitude) / 10e6d, (gpsData->Longitude) / 10e6d, gpsData->Altitude + gpsData->GeoidSeparation }; float NED[3]; LLA2Base(LLA, ECEF, Rne, NED); pos.North = NED[0]; diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h index da6876d8a..dc843999d 100644 --- a/flight/pios/inc/pios_math.h +++ b/flight/pios/inc/pios_math.h @@ -45,10 +45,31 @@ #define M_LNPI_F 1.14472988584940017414342735135f /* ln(pi) */ #define M_EULER_F 0.57721566490153286060651209008f /* Euler constant */ +#define M_E_D 2.71828182845904523536028747135d /* e */ +#define M_LOG2E_D 1.44269504088896340735992468100d /* log_2 (e) */ +#define M_LOG10E_D 0.43429448190325182765112891892d /* log_10 (e) */ +#define M_SQRT2_D 1.41421356237309504880168872421d /* sqrt(2) */ +#define M_SQRT1_2_D 0.70710678118654752440084436210d /* sqrt(1/2) */ +#define M_SQRT3_D 1.73205080756887729352744634151d /* sqrt(3) */ +#define M_PI_D 3.14159265358979323846264338328d /* pi */ +#define M_PI_2_D 1.57079632679489661923132169164d /* pi/2 */ +#define M_PI_4_D 0.78539816339744830961566084582d /* pi/4 */ +#define M_SQRTPI_D 1.77245385090551602729816748334d /* sqrt(pi) */ +#define M_2_SQRTPI_D 1.12837916709551257389615890312d /* 2/sqrt(pi) */ +#define M_1_PI_D 0.31830988618379067153776752675d /* 1/pi */ +#define M_2_PI_D 0.63661977236758134307553505349d /* 2/pi */ +#define M_LN10_D 2.30258509299404568401799145468d /* ln(10) */ +#define M_LN2_D 0.69314718055994530941723212146d /* ln(2) */ +#define M_LNPI_D 1.14472988584940017414342735135d /* ln(pi) */ +#define M_EULER_D 0.57721566490153286060651209008d /* Euler constant */ + // Conversion macro #define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F)) #define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f)) +#define RAD2DEG_D(rad) ((rad) * (180.0d / M_PI_D)) +#define DEG2RAD_D(deg) ((deg) * (M_PI_D / 180.0d)) + // Useful math macros #define MAX(a, b) ((a) > (b) ? (a) : (b)) #define MIN(a, b) ((a) < (b) ? (a) : (b)) From f22a0d299e87712455a7963f1bf4bceb451748dc Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 5 Jun 2013 23:51:22 +0200 Subject: [PATCH 44/61] EKF: Averaging term for dT - prevent scheduling jitter from screwing with the filter while keeping dT accurate --- flight/modules/StateEstimation/filterekf.c | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index cdf8a7b99..af5ffcd77 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -43,6 +43,8 @@ // Private constants #define STACK_REQUIRED 2048 +#define DT_ALPHA 1e-3f +#define DT_INIT (1.0f / 666.0f) // initialize with 666 Hz (default sensor update rate on revo) #define IMPORT_SENSOR_IF_UPDATED(shortname, num) \ if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \ @@ -65,6 +67,8 @@ struct data { uint32_t ins_last_time; bool inited; + + float dTa; }; // Private variables @@ -153,6 +157,7 @@ static int32_t maininit(stateFilter *self) this->init_stage = 0; this->work.updated = 0; this->ins_last_time = PIOS_DELAY_GetRaw(); + this->dTa = DT_INIT; EKFConfigurationGet(&this->ekfConfiguration); int t; @@ -228,6 +233,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state) dT = 0.001f; } + this->dTa = this->dTa * (1.0f - DT_ALPHA) + dT * DT_ALPHA; // low pass for average dT, compensate timing jitter from scheduler + if (!this->inited && IS_SET(this->work.updated, SENSORUPDATES_mag) && IS_SET(this->work.updated, SENSORUPDATES_baro) && IS_SET(this->work.updated, SENSORUPDATES_pos)) { // Don't initialize until all sensors are read if (this->init_stage == 0) { @@ -291,7 +298,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // Run prediction a bit before any corrections float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) }; - INSStatePrediction(gyros, this->work.accel, dT); + INSStatePrediction(gyros, this->work.accel, this->dTa); // Copy the attitude into the state // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true @@ -326,7 +333,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) }; // Advance the state estimate - INSStatePrediction(gyros, this->work.accel, dT); + INSStatePrediction(gyros, this->work.accel, this->dTa); // Copy the attitude into the state // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true @@ -346,7 +353,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel; // Advance the covariance estimate - INSCovariancePrediction(dT); + INSCovariancePrediction(this->dTa); if (IS_SET(this->work.updated, SENSORUPDATES_mag)) { sensors |= MAG_SENSORS; From bab95e7ca83008c1617d7c968b6e1bdaee8f7863 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 6 Jun 2013 00:07:11 +0200 Subject: [PATCH 45/61] check variances for validity and reinitialize if invalid --- flight/modules/StateEstimation/filterekf.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index af5ffcd77..dc4fa38d3 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -422,6 +422,13 @@ static int32_t filter(stateFilter *self, stateEstimation *state) EKFStateVarianceGet(&vardata); INSGetP(vardata.P); EKFStateVarianceSet(&vardata); + int t; + for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) { + if (invalid_var(vardata.P[t])) { + INSResetP(this->ekfConfiguration.P); + break; + } + } // all sensor data has been used, reset! this->work.updated = 0; From b0db19170373c88b6e6e0c39ffee6669a47a43dd Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 6 Jun 2013 17:54:59 +0200 Subject: [PATCH 46/61] Divide mag variance by Be squared to cope with normalization correctly --- flight/modules/StateEstimation/filterekf.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index dc4fa38d3..e0c9ed36a 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -240,10 +240,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (this->init_stage == 0) { // Reset the INS algorithm INSGPSInit(); - // variance is measured in mGaus, but internally the EKF works with a normalized vector. Scaling factor from old AHRS code (1000*1000) - INSSetMagVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGX] / 1e6f, - this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY] / 1e6f, - this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] / 1e6f } + // variance is measured in mGaus, but internally the EKF works with a normalized vector. Scale down by Be^2 + float Be2 = this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2]; + INSSetMagVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGX] / Be2, + this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY] / Be2, + this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] / Be2 } ); INSSetAccelVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX], this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY], From fcc3f519d87207638c33aff4aa97f7fa32c224b5 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 6 Jun 2013 17:59:27 +0200 Subject: [PATCH 47/61] filterekf: Remember critical divergence and set error flag to prevent (re)arming so the user will notice --- flight/modules/StateEstimation/filterekf.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index e0c9ed36a..8003c1f13 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -427,6 +427,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) { if (invalid_var(vardata.P[t])) { INSResetP(this->ekfConfiguration.P); + this->init_stage = -1; break; } } @@ -434,7 +435,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // all sensor data has been used, reset! this->work.updated = 0; - return 0; + if (this->init_stage < 0) { + return 2; + } else { + return 0; + } } // check for invalid variance values From a99255a36a1f63af96ab5859fdee450107d42f89 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 9 Jun 2013 13:57:36 +0200 Subject: [PATCH 48/61] allow smaller nonzero variances during runtime --- flight/modules/StateEstimation/filterekf.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 8003c1f13..46b59723b 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -425,7 +425,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) EKFStateVarianceSet(&vardata); int t; for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) { - if (invalid_var(vardata.P[t])) { + if (!IS_REAL(vardata.P[t]) || vardata.P[t] <= 0.0f) { INSResetP(this->ekfConfiguration.P); this->init_stage = -1; break; From d8f9f839f2653864cf48fc86c700c0214473c549 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 9 Jun 2013 21:49:04 +0200 Subject: [PATCH 49/61] startup grace time for filter initialisation --- flight/modules/StateEstimation/stateestimation.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 148b7d207..6345782e3 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -311,6 +311,14 @@ static void StateEstimationCb(void) static filterPipeline *current; static stateEstimation states; static uint32_t last_time; + static uint16_t bootDelay = 64; + + // after system startup, first few sensor readings might be messed up, delay until everything has settled + if (bootDelay) { + bootDelay--; + DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER); + return; + } switch (runState) { case RUNSTATE_LOAD: From f051b7c31911f239c837ea7e4a640cba8e9f6fc5 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 9 Jun 2013 22:19:54 +0200 Subject: [PATCH 50/61] fixed uavobject update mode for GCS --- shared/uavobjectdefinition/positionsensor.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/positionsensor.xml b/shared/uavobjectdefinition/positionsensor.xml index dfa16b803..286163871 100644 --- a/shared/uavobjectdefinition/positionsensor.xml +++ b/shared/uavobjectdefinition/positionsensor.xml @@ -5,7 +5,7 @@ - + From 47523bfe1598a2a7e916c6ab9f12782740cac0cf Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 9 Jun 2013 22:42:14 +0200 Subject: [PATCH 51/61] update PositionSensor whenever there is a 3d fix position update, even if its not good quality... --- flight/modules/GPS/GPS.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 05d05e5f0..973340e1a 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -276,8 +276,12 @@ static void gpsTask(__attribute__((unused)) void *parameters) #ifdef PIOS_GPS_SETS_POSITIONSENSOR setPositionSensor(&gpspositionsensor); #endif - } else if (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) { + } else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && + (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) { AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); +#ifdef PIOS_GPS_SETS_POSITIONSENSOR + setPositionSensor(&gpspositionsensor); +#endif } else { AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); } From 6f38e26a539e06364e3cfdcac17e64f5b2af2531 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 22 Jun 2013 19:58:54 +0200 Subject: [PATCH 52/61] Categorizing all UAVObjects into 5 main categories: State, Sensors, System, Navigation, Control --- .../uavobjectdefinition/accessorydesired.xml | 2 +- .../uavobjectdefinition/actuatorcommand.xml | 2 +- .../uavobjectdefinition/actuatordesired.xml | 2 +- .../uavobjectdefinition/actuatorsettings.xml | 2 +- .../uavobjectdefinition/airspeedsettings.xml | 2 +- .../uavobjectdefinition/altholdsmoothed.xml | 2 +- .../altitudeholddesired.xml | 2 +- .../altitudeholdsettings.xml | 2 +- .../uavobjectdefinition/attitudesettings.xml | 2 +- .../uavobjectdefinition/attitudesimulated.xml | 2 +- shared/uavobjectdefinition/cameradesired.xml | 2 +- .../camerastabsettings.xml | 2 +- .../uavobjectdefinition/ekfconfiguration.xml | 24 +++++++++---------- .../uavobjectdefinition/ekfstatevariance.xml | 2 +- shared/uavobjectdefinition/faultsettings.xml | 2 +- shared/uavobjectdefinition/firmwareiapobj.xml | 2 +- .../fixedwingpathfollowersettings.xml | 2 +- .../fixedwingpathfollowerstatus.xml | 2 +- .../flightbatterysettings.xml | 2 +- .../flightbatterystate.xml | 2 +- .../uavobjectdefinition/flightplancontrol.xml | 2 +- .../flightplansettings.xml | 2 +- .../uavobjectdefinition/flightplanstatus.xml | 2 +- shared/uavobjectdefinition/flightstatus.xml | 2 +- .../flighttelemetrystats.xml | 2 +- shared/uavobjectdefinition/gcsreceiver.xml | 2 +- .../uavobjectdefinition/gcstelemetrystats.xml | 2 +- shared/uavobjectdefinition/gpssatellites.xml | 2 +- shared/uavobjectdefinition/gpstime.xml | 2 +- shared/uavobjectdefinition/groundtruth.xml | 2 +- shared/uavobjectdefinition/homelocation.xml | 2 +- shared/uavobjectdefinition/hwsettings.xml | 2 +- shared/uavobjectdefinition/i2cstats.xml | 2 +- .../manualcontrolcommand.xml | 2 +- .../manualcontrolsettings.xml | 2 +- shared/uavobjectdefinition/mixersettings.xml | 2 +- shared/uavobjectdefinition/mixerstatus.xml | 2 +- shared/uavobjectdefinition/nedaccel.xml | 2 +- .../uavobjectdefinition/objectpersistence.xml | 2 +- shared/uavobjectdefinition/oplinksettings.xml | 2 +- shared/uavobjectdefinition/oplinkstatus.xml | 2 +- shared/uavobjectdefinition/osdsettings.xml | 2 +- .../uavobjectdefinition/overosyncsettings.xml | 2 +- shared/uavobjectdefinition/overosyncstats.xml | 2 +- shared/uavobjectdefinition/pathaction.xml | 2 +- shared/uavobjectdefinition/pathdesired.xml | 2 +- shared/uavobjectdefinition/pathstatus.xml | 2 +- .../uavobjectdefinition/poilearnsettings.xml | 2 +- shared/uavobjectdefinition/poilocation.xml | 2 +- shared/uavobjectdefinition/ratedesired.xml | 2 +- .../uavobjectdefinition/receiveractivity.xml | 2 +- shared/uavobjectdefinition/relaytuning.xml | 2 +- .../relaytuningsettings.xml | 2 +- .../uavobjectdefinition/revocalibration.xml | 2 +- shared/uavobjectdefinition/revosettings.xml | 2 +- shared/uavobjectdefinition/sonaraltitude.xml | 2 +- .../stabilizationdesired.xml | 2 +- .../stabilizationsettings.xml | 2 +- shared/uavobjectdefinition/systemalarms.xml | 2 +- shared/uavobjectdefinition/systemsettings.xml | 2 +- shared/uavobjectdefinition/systemstats.xml | 4 ++-- shared/uavobjectdefinition/taskinfo.xml | 2 +- shared/uavobjectdefinition/txpidsettings.xml | 2 +- .../uavobjectdefinition/velocitydesired.xml | 2 +- .../vtolpathfollowersettings.xml | 2 +- shared/uavobjectdefinition/watchdogstatus.xml | 4 ++-- shared/uavobjectdefinition/waypoint.xml | 2 +- shared/uavobjectdefinition/waypointactive.xml | 2 +- 68 files changed, 81 insertions(+), 81 deletions(-) diff --git a/shared/uavobjectdefinition/accessorydesired.xml b/shared/uavobjectdefinition/accessorydesired.xml index dbeb583d9..bce35a7d7 100644 --- a/shared/uavobjectdefinition/accessorydesired.xml +++ b/shared/uavobjectdefinition/accessorydesired.xml @@ -1,5 +1,5 @@ - + Desired Auxillary actuator settings. Comes from @ref ManualControlModule. diff --git a/shared/uavobjectdefinition/actuatorcommand.xml b/shared/uavobjectdefinition/actuatorcommand.xml index 2ee3a29b2..4bd41a009 100644 --- a/shared/uavobjectdefinition/actuatorcommand.xml +++ b/shared/uavobjectdefinition/actuatorcommand.xml @@ -1,5 +1,5 @@ - + Contains the pulse duration sent to each of the channels. Set by @ref ActuatorModule diff --git a/shared/uavobjectdefinition/actuatordesired.xml b/shared/uavobjectdefinition/actuatordesired.xml index 4c4bbba8b..371d025b1 100644 --- a/shared/uavobjectdefinition/actuatordesired.xml +++ b/shared/uavobjectdefinition/actuatordesired.xml @@ -1,5 +1,5 @@ - + Desired raw, pitch and yaw actuator settings. Comes from either @ref StabilizationModule or @ref ManualControlModule depending on FlightMode. diff --git a/shared/uavobjectdefinition/actuatorsettings.xml b/shared/uavobjectdefinition/actuatorsettings.xml index 98681555e..c32be9c6c 100644 --- a/shared/uavobjectdefinition/actuatorsettings.xml +++ b/shared/uavobjectdefinition/actuatorsettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref ActuatorModule that controls the channel assignments for the mixer based on AircraftType diff --git a/shared/uavobjectdefinition/airspeedsettings.xml b/shared/uavobjectdefinition/airspeedsettings.xml index 697c9a3ec..a208ff692 100644 --- a/shared/uavobjectdefinition/airspeedsettings.xml +++ b/shared/uavobjectdefinition/airspeedsettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref BaroAirspeed module used on CopterControl or Revolution diff --git a/shared/uavobjectdefinition/altholdsmoothed.xml b/shared/uavobjectdefinition/altholdsmoothed.xml index ef1b5f144..b32e977c2 100644 --- a/shared/uavobjectdefinition/altholdsmoothed.xml +++ b/shared/uavobjectdefinition/altholdsmoothed.xml @@ -1,5 +1,5 @@ - + The output on the kalman filter on altitude. diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index 8cb004011..cf056cfa0 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -1,5 +1,5 @@ - + Holds the desired altitude (from manual control) as well as the desired attitude to pass through diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 61f36d37f..3a2cf6149 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref AltitudeHold module diff --git a/shared/uavobjectdefinition/attitudesettings.xml b/shared/uavobjectdefinition/attitudesettings.xml index 8c86e2af3..34f281636 100644 --- a/shared/uavobjectdefinition/attitudesettings.xml +++ b/shared/uavobjectdefinition/attitudesettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref Attitude module used on CopterControl diff --git a/shared/uavobjectdefinition/attitudesimulated.xml b/shared/uavobjectdefinition/attitudesimulated.xml index be688b877..47c4257cc 100644 --- a/shared/uavobjectdefinition/attitudesimulated.xml +++ b/shared/uavobjectdefinition/attitudesimulated.xml @@ -1,5 +1,5 @@ - + The simulated Attitude estimation from @ref Sensors. diff --git a/shared/uavobjectdefinition/cameradesired.xml b/shared/uavobjectdefinition/cameradesired.xml index c842edf2a..fe98c940d 100644 --- a/shared/uavobjectdefinition/cameradesired.xml +++ b/shared/uavobjectdefinition/cameradesired.xml @@ -1,5 +1,5 @@ - + Desired camera outputs. Comes from @ref CameraStabilization module. diff --git a/shared/uavobjectdefinition/camerastabsettings.xml b/shared/uavobjectdefinition/camerastabsettings.xml index 0a3626183..0a017dff8 100644 --- a/shared/uavobjectdefinition/camerastabsettings.xml +++ b/shared/uavobjectdefinition/camerastabsettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref CameraStab mmodule diff --git a/shared/uavobjectdefinition/ekfconfiguration.xml b/shared/uavobjectdefinition/ekfconfiguration.xml index fa2b8dc8f..4f0afeac5 100644 --- a/shared/uavobjectdefinition/ekfconfiguration.xml +++ b/shared/uavobjectdefinition/ekfconfiguration.xml @@ -1,9 +1,9 @@ - + Extended Kalman Filter initialisation @@ -23,9 +23,9 @@ + 0.0000001, 0.0000001, 0.0000001"> GyroX GyroY @@ -39,10 +39,10 @@ + 10, 10, 1000, + 1, 1, 1, + 1000, 1000, 1000, + 1"> GPSPosNorth GPSPosEast @@ -57,9 +57,9 @@ + 10, + 1, + 1000"> FakeGPSPosIndoor FakeGPSVelIndoor diff --git a/shared/uavobjectdefinition/ekfstatevariance.xml b/shared/uavobjectdefinition/ekfstatevariance.xml index 05b684dbd..cd9cc412a 100644 --- a/shared/uavobjectdefinition/ekfstatevariance.xml +++ b/shared/uavobjectdefinition/ekfstatevariance.xml @@ -1,5 +1,5 @@ - + Extended Kalman Filter state covariance diff --git a/shared/uavobjectdefinition/faultsettings.xml b/shared/uavobjectdefinition/faultsettings.xml index 6750c4f99..43c2a854f 100644 --- a/shared/uavobjectdefinition/faultsettings.xml +++ b/shared/uavobjectdefinition/faultsettings.xml @@ -1,5 +1,5 @@ - + Allows testers to simulate various fault scenarios. diff --git a/shared/uavobjectdefinition/firmwareiapobj.xml b/shared/uavobjectdefinition/firmwareiapobj.xml index 4703ae448..f83c8aa67 100644 --- a/shared/uavobjectdefinition/firmwareiapobj.xml +++ b/shared/uavobjectdefinition/firmwareiapobj.xml @@ -1,5 +1,5 @@ - + Queries board for SN, model, revision, and sends reset command diff --git a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml index ac987e866..f792fd7dc 100644 --- a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml +++ b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref FixedWingPathFollowerModule diff --git a/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml b/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml index a8425dbd0..7bdca082d 100644 --- a/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml +++ b/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml @@ -1,5 +1,5 @@ - + Object Storing Debugging Information on PID internals diff --git a/shared/uavobjectdefinition/flightbatterysettings.xml b/shared/uavobjectdefinition/flightbatterysettings.xml index 4fe5ba025..38d8ee646 100644 --- a/shared/uavobjectdefinition/flightbatterysettings.xml +++ b/shared/uavobjectdefinition/flightbatterysettings.xml @@ -1,5 +1,5 @@ - + Flight Battery configuration. diff --git a/shared/uavobjectdefinition/flightbatterystate.xml b/shared/uavobjectdefinition/flightbatterystate.xml index be20caec9..1f7dc19ea 100644 --- a/shared/uavobjectdefinition/flightbatterystate.xml +++ b/shared/uavobjectdefinition/flightbatterystate.xml @@ -1,5 +1,5 @@ - + Battery status information. diff --git a/shared/uavobjectdefinition/flightplancontrol.xml b/shared/uavobjectdefinition/flightplancontrol.xml index 62144edc8..586794af8 100644 --- a/shared/uavobjectdefinition/flightplancontrol.xml +++ b/shared/uavobjectdefinition/flightplancontrol.xml @@ -1,5 +1,5 @@ - + Control the flight plan script diff --git a/shared/uavobjectdefinition/flightplansettings.xml b/shared/uavobjectdefinition/flightplansettings.xml index 98fad1f2e..6357aad75 100644 --- a/shared/uavobjectdefinition/flightplansettings.xml +++ b/shared/uavobjectdefinition/flightplansettings.xml @@ -1,5 +1,5 @@ - + Settings for the flight plan module, control the execution of the script diff --git a/shared/uavobjectdefinition/flightplanstatus.xml b/shared/uavobjectdefinition/flightplanstatus.xml index c5d14ca6a..e5b31a6e6 100644 --- a/shared/uavobjectdefinition/flightplanstatus.xml +++ b/shared/uavobjectdefinition/flightplanstatus.xml @@ -1,5 +1,5 @@ - + Status of the flight plan script diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml index 24befac63..9c9336c1a 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -1,5 +1,5 @@ - + Contains major flight status information for other modules. diff --git a/shared/uavobjectdefinition/flighttelemetrystats.xml b/shared/uavobjectdefinition/flighttelemetrystats.xml index 57a19850c..352bb7de0 100644 --- a/shared/uavobjectdefinition/flighttelemetrystats.xml +++ b/shared/uavobjectdefinition/flighttelemetrystats.xml @@ -1,5 +1,5 @@ - + Maintains the telemetry statistics from the OpenPilot flight computer. diff --git a/shared/uavobjectdefinition/gcsreceiver.xml b/shared/uavobjectdefinition/gcsreceiver.xml index ee4fa696f..5e76d1928 100644 --- a/shared/uavobjectdefinition/gcsreceiver.xml +++ b/shared/uavobjectdefinition/gcsreceiver.xml @@ -1,5 +1,5 @@ - + A receiver channel group carried over the telemetry link. diff --git a/shared/uavobjectdefinition/gcstelemetrystats.xml b/shared/uavobjectdefinition/gcstelemetrystats.xml index 83ce10cc1..9cdf9985d 100644 --- a/shared/uavobjectdefinition/gcstelemetrystats.xml +++ b/shared/uavobjectdefinition/gcstelemetrystats.xml @@ -1,5 +1,5 @@ - + The telemetry statistics from the ground computer diff --git a/shared/uavobjectdefinition/gpssatellites.xml b/shared/uavobjectdefinition/gpssatellites.xml index d23cddb4b..47c4c0f56 100644 --- a/shared/uavobjectdefinition/gpssatellites.xml +++ b/shared/uavobjectdefinition/gpssatellites.xml @@ -1,5 +1,5 @@ - + Contains information about the GPS satellites in view from @ref GPSModule. diff --git a/shared/uavobjectdefinition/gpstime.xml b/shared/uavobjectdefinition/gpstime.xml index c397a5108..ab93d4e9c 100644 --- a/shared/uavobjectdefinition/gpstime.xml +++ b/shared/uavobjectdefinition/gpstime.xml @@ -1,5 +1,5 @@ - + Contains the GPS time from @ref GPSModule. Required to compute the world magnetic model correctly when setting the home location. diff --git a/shared/uavobjectdefinition/groundtruth.xml b/shared/uavobjectdefinition/groundtruth.xml index c852754ee..923078847 100644 --- a/shared/uavobjectdefinition/groundtruth.xml +++ b/shared/uavobjectdefinition/groundtruth.xml @@ -1,5 +1,5 @@ - + Ground truth data output by a simulator. diff --git a/shared/uavobjectdefinition/homelocation.xml b/shared/uavobjectdefinition/homelocation.xml index a0503517f..3f99eb24b 100644 --- a/shared/uavobjectdefinition/homelocation.xml +++ b/shared/uavobjectdefinition/homelocation.xml @@ -1,5 +1,5 @@ - + HomeLocation setting which contains the constants to tranlate from longitutde and latitude to NED reference frame. Automatically set by @ref GPSModule after acquiring a 3D lock. Used by @ref AHRSCommsModule. diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 5d0a29fec..8149a7d62 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -1,5 +1,5 @@ - + Selection of optional hardware configurations. diff --git a/shared/uavobjectdefinition/i2cstats.xml b/shared/uavobjectdefinition/i2cstats.xml index 40663eba6..3e79d79ea 100644 --- a/shared/uavobjectdefinition/i2cstats.xml +++ b/shared/uavobjectdefinition/i2cstats.xml @@ -1,5 +1,5 @@ - + Tracks statistics on the I2C bus. diff --git a/shared/uavobjectdefinition/manualcontrolcommand.xml b/shared/uavobjectdefinition/manualcontrolcommand.xml index 2a7eb9c78..d8cc27017 100644 --- a/shared/uavobjectdefinition/manualcontrolcommand.xml +++ b/shared/uavobjectdefinition/manualcontrolcommand.xml @@ -1,5 +1,5 @@ - + The output from the @ref ManualControlModule which descodes the receiver inputs. Overriden by GCS for fly-by-wire control. diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 99c89e5bb..0f183424f 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -1,5 +1,5 @@ - + Settings to indicate how to decode receiver input by @ref ManualControlModule. - + Settings for the @ref ActuatorModule that controls the channel assignments for the mixer based on AircraftType diff --git a/shared/uavobjectdefinition/mixerstatus.xml b/shared/uavobjectdefinition/mixerstatus.xml index 314af78b1..c7491a92e 100644 --- a/shared/uavobjectdefinition/mixerstatus.xml +++ b/shared/uavobjectdefinition/mixerstatus.xml @@ -1,5 +1,5 @@ - + Status for the matrix mixer showing the output of each mixer after all scaling diff --git a/shared/uavobjectdefinition/nedaccel.xml b/shared/uavobjectdefinition/nedaccel.xml index 274fde9e2..ce1636530 100644 --- a/shared/uavobjectdefinition/nedaccel.xml +++ b/shared/uavobjectdefinition/nedaccel.xml @@ -1,5 +1,5 @@ - + The projection of acceleration in the NED reference frame used by @ref Guidance. diff --git a/shared/uavobjectdefinition/objectpersistence.xml b/shared/uavobjectdefinition/objectpersistence.xml index ba21631e3..9b29721df 100644 --- a/shared/uavobjectdefinition/objectpersistence.xml +++ b/shared/uavobjectdefinition/objectpersistence.xml @@ -1,5 +1,5 @@ - + Someone who knows please enter this diff --git a/shared/uavobjectdefinition/oplinksettings.xml b/shared/uavobjectdefinition/oplinksettings.xml index b78b8d4c9..4822b961e 100644 --- a/shared/uavobjectdefinition/oplinksettings.xml +++ b/shared/uavobjectdefinition/oplinksettings.xml @@ -1,5 +1,5 @@ - + OPLink configurations options. diff --git a/shared/uavobjectdefinition/oplinkstatus.xml b/shared/uavobjectdefinition/oplinkstatus.xml index 1e79630fe..9280ef818 100644 --- a/shared/uavobjectdefinition/oplinkstatus.xml +++ b/shared/uavobjectdefinition/oplinkstatus.xml @@ -1,5 +1,5 @@ - + OPLink device status. diff --git a/shared/uavobjectdefinition/osdsettings.xml b/shared/uavobjectdefinition/osdsettings.xml index c3ceef706..aa82c7e85 100644 --- a/shared/uavobjectdefinition/osdsettings.xml +++ b/shared/uavobjectdefinition/osdsettings.xml @@ -1,5 +1,5 @@ - + OSD settings used by the OSDgen module diff --git a/shared/uavobjectdefinition/overosyncsettings.xml b/shared/uavobjectdefinition/overosyncsettings.xml index 2c16b5ad3..2d884bc1a 100644 --- a/shared/uavobjectdefinition/overosyncsettings.xml +++ b/shared/uavobjectdefinition/overosyncsettings.xml @@ -1,5 +1,5 @@ - + Settings to control the behavior of the overo sync module diff --git a/shared/uavobjectdefinition/overosyncstats.xml b/shared/uavobjectdefinition/overosyncstats.xml index 7c120f18d..de069e38c 100644 --- a/shared/uavobjectdefinition/overosyncstats.xml +++ b/shared/uavobjectdefinition/overosyncstats.xml @@ -1,5 +1,5 @@ - + Maintains statistics on transfer rate to and from over diff --git a/shared/uavobjectdefinition/pathaction.xml b/shared/uavobjectdefinition/pathaction.xml index 70936f58d..88f6af2da 100644 --- a/shared/uavobjectdefinition/pathaction.xml +++ b/shared/uavobjectdefinition/pathaction.xml @@ -1,5 +1,5 @@ - + A waypoint command the pathplanner is to use at a certain waypoint + The endpoint or path the craft is trying to achieve. Can come from @ref ManualControl or @ref PathPlanner diff --git a/shared/uavobjectdefinition/pathstatus.xml b/shared/uavobjectdefinition/pathstatus.xml index 814e62baf..746d15c47 100644 --- a/shared/uavobjectdefinition/pathstatus.xml +++ b/shared/uavobjectdefinition/pathstatus.xml @@ -1,5 +1,5 @@ - + Status of the current path mode Can come from any @ref PathFollower module diff --git a/shared/uavobjectdefinition/poilearnsettings.xml b/shared/uavobjectdefinition/poilearnsettings.xml index 281d064e3..21ff01bbf 100644 --- a/shared/uavobjectdefinition/poilearnsettings.xml +++ b/shared/uavobjectdefinition/poilearnsettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref Point of Interest feature diff --git a/shared/uavobjectdefinition/poilocation.xml b/shared/uavobjectdefinition/poilocation.xml index 70022a86a..6ab6f0273 100644 --- a/shared/uavobjectdefinition/poilocation.xml +++ b/shared/uavobjectdefinition/poilocation.xml @@ -1,5 +1,5 @@ - + Contains the current Point of interest relative to @ref HomeLocation diff --git a/shared/uavobjectdefinition/ratedesired.xml b/shared/uavobjectdefinition/ratedesired.xml index d9266517c..5d68597ee 100644 --- a/shared/uavobjectdefinition/ratedesired.xml +++ b/shared/uavobjectdefinition/ratedesired.xml @@ -1,5 +1,5 @@ - + Status for the matrix mixer showing the output of each mixer after all scaling diff --git a/shared/uavobjectdefinition/receiveractivity.xml b/shared/uavobjectdefinition/receiveractivity.xml index 5f9ed8278..9360f877c 100644 --- a/shared/uavobjectdefinition/receiveractivity.xml +++ b/shared/uavobjectdefinition/receiveractivity.xml @@ -1,5 +1,5 @@ - + Monitors which receiver channels have been active within the last second. - + The input to the relay tuning. diff --git a/shared/uavobjectdefinition/relaytuningsettings.xml b/shared/uavobjectdefinition/relaytuningsettings.xml index b55e165a6..8daa2698f 100644 --- a/shared/uavobjectdefinition/relaytuningsettings.xml +++ b/shared/uavobjectdefinition/relaytuningsettings.xml @@ -1,5 +1,5 @@ - + Setting to run a relay tuning algorithm diff --git a/shared/uavobjectdefinition/revocalibration.xml b/shared/uavobjectdefinition/revocalibration.xml index ed620769e..5f4778791 100644 --- a/shared/uavobjectdefinition/revocalibration.xml +++ b/shared/uavobjectdefinition/revocalibration.xml @@ -1,5 +1,5 @@ - + Settings for the INS to control the algorithm and what is updated diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index b2654e1d8..e3b19f05f 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -1,5 +1,5 @@ - + Settings for the revo to control the algorithm and what is updated diff --git a/shared/uavobjectdefinition/sonaraltitude.xml b/shared/uavobjectdefinition/sonaraltitude.xml index 98b678413..11ee33933 100644 --- a/shared/uavobjectdefinition/sonaraltitude.xml +++ b/shared/uavobjectdefinition/sonaraltitude.xml @@ -1,5 +1,5 @@ - + The raw data from the ultrasound sonar sensor with altitude estimate. diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index 7e147a48f..bd70011bb 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -1,5 +1,5 @@ - + The desired attitude that @ref StabilizationModule will try and achieve if FlightMode is Stabilized. Comes from @ref ManaulControlModule. diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 3c8928b50..9434313e0 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -1,5 +1,5 @@ - + PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired diff --git a/shared/uavobjectdefinition/systemalarms.xml b/shared/uavobjectdefinition/systemalarms.xml index fedeef14a..faa1cc9b1 100644 --- a/shared/uavobjectdefinition/systemalarms.xml +++ b/shared/uavobjectdefinition/systemalarms.xml @@ -1,5 +1,5 @@ - + Alarms from OpenPilot to indicate failure conditions or warnings. Set by various modules. Some modules may have a module defined Status and Substatus fields that details its condition. diff --git a/shared/uavobjectdefinition/systemsettings.xml b/shared/uavobjectdefinition/systemsettings.xml index bc7a98019..196292638 100644 --- a/shared/uavobjectdefinition/systemsettings.xml +++ b/shared/uavobjectdefinition/systemsettings.xml @@ -1,5 +1,5 @@ - + Select airframe type. Currently used by @ref ActuatorModule to choose mixing from @ref ActuatorDesired to @ref ActuatorCommand diff --git a/shared/uavobjectdefinition/systemstats.xml b/shared/uavobjectdefinition/systemstats.xml index d271c6517..e09c817a4 100644 --- a/shared/uavobjectdefinition/systemstats.xml +++ b/shared/uavobjectdefinition/systemstats.xml @@ -1,5 +1,5 @@ - + CPU and memory usage from OpenPilot computer. @@ -18,4 +18,4 @@ - \ No newline at end of file + diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index 869e3b84b..40b27265a 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -1,5 +1,5 @@ - + Task information diff --git a/shared/uavobjectdefinition/txpidsettings.xml b/shared/uavobjectdefinition/txpidsettings.xml index 6d8f1498f..41c7d06ca 100644 --- a/shared/uavobjectdefinition/txpidsettings.xml +++ b/shared/uavobjectdefinition/txpidsettings.xml @@ -1,5 +1,5 @@ - + Settings used by @ref TxPID optional module to tune PID settings using R/C transmitter - + Used within @ref GuidanceModule to communicate between the task computing the desired velocity and the PID loop to achieve it (running at different rates). diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index 20adeea52..35089b9e0 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref VtolPathFollowerModule diff --git a/shared/uavobjectdefinition/watchdogstatus.xml b/shared/uavobjectdefinition/watchdogstatus.xml index 2c09abac8..2adef2c87 100644 --- a/shared/uavobjectdefinition/watchdogstatus.xml +++ b/shared/uavobjectdefinition/watchdogstatus.xml @@ -1,5 +1,5 @@ - + For monitoring the flags in the watchdog and especially the bootup flags @@ -8,4 +8,4 @@ - \ No newline at end of file + diff --git a/shared/uavobjectdefinition/waypoint.xml b/shared/uavobjectdefinition/waypoint.xml index 45107e7d4..bcadf49ac 100644 --- a/shared/uavobjectdefinition/waypoint.xml +++ b/shared/uavobjectdefinition/waypoint.xml @@ -1,5 +1,5 @@ - + A waypoint the aircraft can try and hit. Used by the @ref PathPlanner module diff --git a/shared/uavobjectdefinition/waypointactive.xml b/shared/uavobjectdefinition/waypointactive.xml index c8dff749e..3343715b5 100644 --- a/shared/uavobjectdefinition/waypointactive.xml +++ b/shared/uavobjectdefinition/waypointactive.xml @@ -1,5 +1,5 @@ - + Indicates the currently active waypoint From 3392622d2bf3f70ca15e80142f600a25f52100c3 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 22 Jun 2013 23:07:53 +0200 Subject: [PATCH 53/61] backporting change on next to modified Attitude module --- flight/modules/Attitude/revolution/attitude.c | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index 5d97f1748..fba41c75f 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -415,12 +415,9 @@ static int32_t updateAttitudeComplementary(bool first_run) // Zero gyro bias // This is really needed after updating calibration settings. - GyrosBiasData zeroGyrosBias; - GyrosBiasGet(&zeroGyrosBias); - zeroGyrosBias.x = 0; - zeroGyrosBias.y = 0; - zeroGyrosBias.z = 0; - GyrosBiasSet(&zeroGyrosBias); + gyro_bias[0] = 0.0f; + gyro_bias[1] = 0.0f; + gyro_bias[2] = 0.0f; return 0; } From 4bf934ae696ee38573617029d9409fc53c2e1f27 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 22 Jun 2013 23:15:10 +0200 Subject: [PATCH 54/61] prevent positionSensor updates if GPS lock is low quality. Sometimes bad updates are worse than no updates. --- flight/modules/GPS/GPS.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index cecbd1b7f..b2b96a15d 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -277,9 +277,6 @@ static void gpsTask(__attribute__((unused)) void *parameters) } else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) { AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); -#ifdef PIOS_GPS_SETS_POSITIONSENSOR - setPositionSensor(&gpspositionsensor); -#endif } else { AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); } From 73b22f5cb1bc9740adc9dfa2222a3f421061c05c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 22 Jun 2013 23:49:03 +0200 Subject: [PATCH 55/61] typo bugfix --- .../openpilotgcs/share/openpilotgcs/pfd/default/SpeedScale.qml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/SpeedScale.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/SpeedScale.qml index b6715ba38..bbb7d50e2 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/SpeedScale.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/SpeedScale.qml @@ -4,7 +4,7 @@ Item { id: sceneItem property variant sceneSize property real groundSpeed : qmlWidget.speedFactor * Math.sqrt(Math.pow(VelocityState.North,2)+ - Math.pow(VelocityActual.State,2)) + Math.pow(VelocityState.East,2)) SvgElementImage { id: speed_bg From 00a603c88fb474364f9b6a52fd68d22b6fea133b Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 23 Jun 2013 00:10:00 +0200 Subject: [PATCH 56/61] more bugfixes related to UAVObject renaming --- ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml | 2 +- ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml index 29ac73e9a..e03a20e02 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml @@ -54,7 +54,7 @@ Rectangle { sceneSize: background.sceneSize smooth: true - property real sideSlip: Accels.y + property real sideSlip: AccelState.y //smooth side slip changes, a low pass filter replacement //accels are updated once per second Behavior on sideSlip { diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp index 5e7683c7f..aa6879b7e 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp @@ -57,8 +57,8 @@ PfdQmlGadgetWidget::PfdQmlGadgetWidget(QWidget *parent) : "AttitudeState" << "AccelState" << "VelocityDesired" << - "PositionDesired" << - "AttitudeHoldDesired" << + "PathDesired" << + "AltitudeHoldDesired" << "GPSPositionSensor" << "GCSTelemetryStats" << "FlightBatteryState"; From 407817f95979c3651af0edd2c924fa03d5d892c6 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 23 Jun 2013 18:37:43 +0200 Subject: [PATCH 57/61] backport of complementary filter bugfix --- flight/modules/StateEstimation/filtercf.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 4bc239251..704bac46a 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -62,6 +62,7 @@ struct data { bool accel_filter_enabled; float rollPitchBiasRate; int32_t timeval; + int32_t starttime; uint8_t init; bool magCalibrated; }; @@ -267,11 +268,16 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel this->first_run = 0; this->timeval = PIOS_DELAY_GetRaw(); + this->starttime = this->timeval; - return 0; + return 0; // must return zero on initial initialization, so attitude will init with a valid quaternion } - if ((this->init == 0 && xTaskGetTickCount() < 10000) && (xTaskGetTickCount() > 4000)) { + if (this->init == 0 && PIOS_DELAY_DiffuS(this->starttime) < 4000000) { + // wait 4 seconds for the user to get his hands off in case the board was just powered + this->timeval = PIOS_DELAY_GetRaw(); + return 1; + } else if (this->init == 0 && PIOS_DELAY_DiffuS(this->starttime) < 10000000 && PIOS_DELAY_DiffuS(this->starttime) >= 4000000) { // For first 7 seconds use accels to get gyro bias this->attitudeSettings.AccelKp = 1.0f; this->attitudeSettings.AccelKi = 0.0f; From e916902c9ee9d2e2f41d51ea8d359dd93b72e5a0 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 23 Jun 2013 19:26:17 +0200 Subject: [PATCH 58/61] changed alarm return states, so cf initialisation will cause visible LED blinks... --- flight/modules/StateEstimation/filtercf.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 704bac46a..39579e78c 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -441,7 +441,11 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel return 2; } - return 0; + if (this->init) { + return 0; + } else { + return 2; // return "critical" for now, so users can see the zeroing period, switch to more graceful notification later + } } static void flightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) From 5bdfad4bdb3966f166b76030bc2b3da2ff227454 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 23 Jun 2013 19:39:00 +0200 Subject: [PATCH 59/61] removed redundant check as suggested in review --- flight/modules/StateEstimation/filtercf.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 39579e78c..ae896905a 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -277,7 +277,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel // wait 4 seconds for the user to get his hands off in case the board was just powered this->timeval = PIOS_DELAY_GetRaw(); return 1; - } else if (this->init == 0 && PIOS_DELAY_DiffuS(this->starttime) < 10000000 && PIOS_DELAY_DiffuS(this->starttime) >= 4000000) { + } else if (this->init == 0 && PIOS_DELAY_DiffuS(this->starttime) < 10000000) { // For first 7 seconds use accels to get gyro bias this->attitudeSettings.AccelKp = 1.0f; this->attitudeSettings.AccelKi = 0.0f; From f2abe1f5c76ffbad1dab2551a1598b3d7d571cf2 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 25 Jun 2013 19:30:21 +0200 Subject: [PATCH 60/61] StateEstimation: removed (not yet implemented) 16 state EKF support for now --- .../modules/StateEstimation/stateestimation.c | 39 ------------------- shared/uavobjectdefinition/revosettings.xml | 2 +- 2 files changed, 1 insertion(+), 40 deletions(-) diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 6345782e3..5ed371844 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -129,8 +129,6 @@ static stateFilter cfFilter; static stateFilter cfmFilter; static stateFilter ekf13iFilter; static stateFilter ekf13Filter; -static stateFilter ekf16Filter; -static stateFilter ekf16iFilter; // preconfigured filter chains selectable via revoSettings.FusionAlgorithm static filterPipeline *cfQueue = &(filterPipeline) { @@ -188,35 +186,6 @@ static const filterPipeline *ekf13Queue = &(filterPipeline) { } } }; -static const filterPipeline *ekf16iQueue = &(filterPipeline) { - .filter = &magFilter, - .next = &(filterPipeline) { - .filter = &airFilter, - .next = &(filterPipeline) { - .filter = &baroFilter, - .next = &(filterPipeline) { - .filter = &stationaryFilter, - .next = &(filterPipeline) { - .filter = &ekf16iFilter, - .next = NULL, - } - } - } - } -}; -static const filterPipeline *ekf16Queue = &(filterPipeline) { - .filter = &magFilter, - .next = &(filterPipeline) { - .filter = &airFilter, - .next = &(filterPipeline) { - .filter = &baroFilter, - .next = &(filterPipeline) { - .filter = &ekf16Filter, - .next = NULL, - } - } - } -}; // Private functions @@ -274,8 +243,6 @@ int32_t StateEstimationInitialize(void) stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter)); stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter)); stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter)); - stack_required = maxint32_t(stack_required, filterEKF16Initialize(&ekf16Filter)); - stack_required = maxint32_t(stack_required, filterEKF16iInitialize(&ekf16iFilter)); stateEstimationCallback = DelayedCallbackCreate(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, stack_required); @@ -353,12 +320,6 @@ static void StateEstimationCb(void) case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR: newFilterChain = ekf13Queue; break; - case REVOSETTINGS_FUSIONALGORITHM_INS16INDOOR: - newFilterChain = ekf16iQueue; - break; - case REVOSETTINGS_FUSIONALGORITHM_INS16OUTDOOR: - newFilterChain = ekf16Queue; - break; default: newFilterChain = NULL; } diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index e3b19f05f..26e129bdd 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -1,7 +1,7 @@ Settings for the revo to control the algorithm and what is updated - + From e2fca0fcdd03da27aad0e044e1b8100962f825cb Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 25 Jun 2013 19:32:50 +0200 Subject: [PATCH 61/61] bugfix in uavobject definition --- shared/uavobjectdefinition/positionstate.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/positionstate.xml b/shared/uavobjectdefinition/positionstate.xml index 207ded992..f6d689741 100644 --- a/shared/uavobjectdefinition/positionstate.xml +++ b/shared/uavobjectdefinition/positionstate.xml @@ -5,7 +5,7 @@ - +